From bedcc905a3b17b6d9a63e638d5636a01256d753a Mon Sep 17 00:00:00 2001
From: Hector Martin <marcan@marcan.st>
Date: Fri, 17 Dec 2021 21:31:02 +0900
Subject: [PATCH] gxf, smp: Allocate stacks dynamically

This significantly shrinks our .bss section

Signed-off-by: Hector Martin <marcan@marcan.st>
---
 src/gxf.c     | 19 +++++++++++++++++--
 src/gxf_asm.S | 34 +++++++---------------------------
 src/main.c    |  2 ++
 src/memory.c  | 10 +---------
 src/smp.c     |  9 +++++++--
 src/smp.h     |  2 +-
 src/startup.c |  2 --
 7 files changed, 35 insertions(+), 43 deletions(-)

diff --git a/src/gxf.c b/src/gxf.c
index 998fb3fb..7b751c5f 100644
--- a/src/gxf.c
+++ b/src/gxf.c
@@ -3,6 +3,7 @@
 #include "cpu_regs.h"
 #include "exception.h"
 #include "gxf.h"
+#include "malloc.h"
 #include "memory.h"
 #include "smp.h"
 #include "uart.h"
@@ -10,8 +11,22 @@
 
 uint64_t gxf_enter(void *func, uint64_t a, uint64_t b, uint64_t c, uint64_t d);
 
-u8 gl1_stack[MAX_CPUS][GL_STACK_SIZE] ALIGNED(0x4000);
-u8 gl2_stack[MAX_CPUS][GL_STACK_SIZE] ALIGNED(0x4000);
+void _gxf_init(void *gl2_stack, void *gl1_stack);
+
+u8 *gl1_stack[MAX_CPUS];
+u8 *gl2_stack[MAX_CPUS];
+
+void gxf_init(void)
+{
+    int cpu = smp_id();
+
+    if (!gl2_stack[cpu])
+        gl2_stack[cpu] = memalign(0x4000, GL_STACK_SIZE);
+    if (in_el2() && !gl1_stack[cpu])
+        gl1_stack[cpu] = memalign(0x4000, GL_STACK_SIZE);
+
+    _gxf_init(gl2_stack[cpu], gl1_stack[cpu]);
+}
 
 bool gxf_enabled(void)
 {
diff --git a/src/gxf_asm.S b/src/gxf_asm.S
index 9d6c7905..6b6405f5 100644
--- a/src/gxf_asm.S
+++ b/src/gxf_asm.S
@@ -7,13 +7,12 @@
 #define genter .long 0x00201420
 #define gexit .long 0x00201400
 
-.extern gl1_stack
-.extern gl2_stack
-
-.global gxf_init
-.type gxf_init, @function
-gxf_init:
+.global _gxf_init
+.type _gxf_init, @function
+_gxf_init:
     str x30, [sp, #-16]!
+    mov x5, x0
+    mov x6, x1
     mov x0, 1
     msr SYS_IMP_APL_SPRR_CONFIG_EL1, x0
     isb
@@ -37,20 +36,7 @@ gxf_enter:
     ret
 
 _gxf_setup:
-    mrs x2, TPIDR_EL1
-    mrs x4, CurrentEL
-    cmp x4, #8
-    bne 1f
-    mrs x2, TPIDR_EL2
-1:
-    add x2, x2, 1
-
-    ldr x0, =gl2_stack
-    ldr x1, =GL_STACK_SIZE
-    mul x1, x1, x2
-    add x0, x0, x1
-
-    mov sp, x0
+    mov sp, x5
     ldr x1, =_gxf_vectors
     ldr x2, =_gxf_exc_sync
     ldr x3, =_gxf_entry
@@ -62,13 +48,7 @@ _gxf_setup:
     cmp x4, #8
     bne 1f
 
-    mrs x2, TPIDR_EL2
-    ldr x0, =gl1_stack
-    ldr x1, =GL_STACK_SIZE
-    mul x1, x1, x2
-    add x0, x0, x1
-
-    msr SYS_IMP_APL_SP_GL12, x0
+    msr SYS_IMP_APL_SP_GL12, x6
     msr SYS_IMP_APL_VBAR_GL12, x1
     msr SYS_IMP_APL_GXF_ABORT_EL12, x2
     msr SYS_IMP_APL_GXF_ENTER_EL12, x3
diff --git a/src/main.c b/src/main.c
index f154b700..17859e48 100644
--- a/src/main.c
+++ b/src/main.c
@@ -7,6 +7,7 @@
 #include "cpufreq.h"
 #include "exception.h"
 #include "fb.h"
+#include "gxf.h"
 #include "heapblock.h"
 #include "mcc.h"
 #include "memory.h"
@@ -83,6 +84,7 @@ void m1n1_main(void)
     get_device_info();
 
     heapblock_init();
+    gxf_init();
     mcc_init();
     mmu_init();
 
diff --git a/src/memory.c b/src/memory.c
index 3cffc1ff..e6fa8071 100644
--- a/src/memory.c
+++ b/src/memory.c
@@ -35,8 +35,6 @@ CACHE_RANGE_OP(dc_cvau_range, "dc cvau")
 CACHE_RANGE_OP(dc_civac_range, "dc civac")
 
 extern u8 _stack_top[];
-extern u8 gl1_stack[GL_STACK_SIZE];
-extern u8 gl2_stack[MAX_CPUS][GL_STACK_SIZE];
 
 uint64_t ram_base = 0;
 
@@ -403,16 +401,10 @@ static void mmu_add_default_mappings(void)
                     PERM_RX_EL0);
 
     /*
-     * Make guard pages at the end of stacks
+     * Make guard page at the end of the main stack
      */
     mmu_rm_mapping((u64)_stack_top, PAGE_SIZE);
 
-    for (int i = 0; i < MAX_CPUS; i++) {
-        mmu_rm_mapping((u64)secondary_stacks[i], PAGE_SIZE);
-        mmu_rm_mapping((u64)gl1_stack[i], PAGE_SIZE);
-        mmu_rm_mapping((u64)gl2_stack[i], PAGE_SIZE);
-    }
-
     /*
      * Create mapping for RAM from 0x88_0000_0000,
      * read/writable/exec by EL0 (but not executable by EL1)
diff --git a/src/smp.c b/src/smp.c
index c8e4ea44..cbab3263 100644
--- a/src/smp.c
+++ b/src/smp.c
@@ -3,6 +3,7 @@
 #include "smp.h"
 #include "adt.h"
 #include "cpu_regs.h"
+#include "malloc.h"
 #include "string.h"
 #include "types.h"
 #include "utils.h"
@@ -19,7 +20,10 @@ struct spin_table {
 
 void *_reset_stack;
 
-u8 secondary_stacks[MAX_CPUS][SECONDARY_STACK_SIZE] ALIGNED(0x4000);
+#define DUMMY_STACK_SIZE 0x1000
+u8 dummy_stack[DUMMY_STACK_SIZE];
+
+u8 *secondary_stacks[MAX_CPUS] = {dummy_stack};
 
 static bool wfe_mode = false;
 
@@ -77,6 +81,7 @@ static void smp_start_cpu(int index, int cluster, int core, u64 rvbar, u64 cpu_s
     memset(&spin_table[index], 0, sizeof(struct spin_table));
 
     target_cpu = index;
+    secondary_stacks[index] = memalign(0x4000, SECONDARY_STACK_SIZE);
     _reset_stack = secondary_stacks[index] + SECONDARY_STACK_SIZE;
 
     sysop("dmb sy");
@@ -102,7 +107,7 @@ static void smp_start_cpu(int index, int cluster, int core, u64 rvbar, u64 cpu_s
     else
         printf("  Started.\n");
 
-    _reset_stack = secondary_stacks[0] + SECONDARY_STACK_SIZE;
+    _reset_stack = dummy_stack + DUMMY_STACK_SIZE;
 }
 
 void smp_start_secondaries(void)
diff --git a/src/smp.h b/src/smp.h
index 0fb0bbdc..c802f3ec 100644
--- a/src/smp.h
+++ b/src/smp.h
@@ -9,7 +9,7 @@
 #define MAX_CPUS 20
 
 #define SECONDARY_STACK_SIZE 0x10000
-extern u8 secondary_stacks[MAX_CPUS][SECONDARY_STACK_SIZE];
+extern u8 *secondary_stacks[MAX_CPUS];
 
 void smp_secondary_entry(void);
 
diff --git a/src/startup.c b/src/startup.c
index bee6610d..10527070 100644
--- a/src/startup.c
+++ b/src/startup.c
@@ -2,7 +2,6 @@
 
 #include "chickens.h"
 #include "exception.h"
-#include "gxf.h"
 #include "smp.h"
 #include "string.h"
 #include "types.h"
@@ -101,7 +100,6 @@ void _start_c(void *boot_args, void *base)
     printf("\n");
 
     exception_initialize();
-    gxf_init();
     m1n1_main();
 }