smp: Added A7-A11 start support

This includes:

- EL3 handling
- "Slow" IPI support
- New start offsets

Signed-off-by: Nick Chan <towinchenmi@gmail.com>

src/aic.c

@@ -201,6 +201,11 @@ void aic_set_sw(int irq, bool active)
                 MASK_BIT(irq));
 }
 
+void aic_write(u32 reg, u32 val)
+{
+    write32(aic->base + reg, val);
+}
+
 uint32_t aic_ack(void)
 {
     return read32(aic->base + aic->regs.event);

src/aic.h

@@ -39,6 +39,7 @@ extern struct aic *aic;
 
 void aic_init(void);
 void aic_set_sw(int irq, bool active);
+void aic_write(u32 reg, u32 val);
 uint32_t aic_ack(void);
 
 #endif

src/smp.c

@@ -2,6 +2,8 @@
 
 #include "smp.h"
 #include "adt.h"
+#include "aic.h"
+#include "aic_regs.h"
 #include "cpu_regs.h"
 #include "malloc.h"
 #include "pmgr.h"
@@ -10,10 +12,12 @@
 #include "types.h"
 #include "utils.h"
 
-#define CPU_START_OFF_T8103 0x54000
-#define CPU_START_OFF_T8112 0x34000
-#define CPU_START_OFF_T6020 0x28000
-#define CPU_START_OFF_T6031 0x88000
+#define CPU_START_OFF_S5L8960X 0x30000
+#define CPU_START_OFF_S8000    0xd4000
+#define CPU_START_OFF_T8103    0x54000
+#define CPU_START_OFF_T8112    0x34000
+#define CPU_START_OFF_T6020    0x28000
+#define CPU_START_OFF_T6031    0x88000
 
 #define CPU_REG_CORE    GENMASK(7, 0)
 #define CPU_REG_CLUSTER GENMASK(10, 8)
@@ -28,11 +32,14 @@ struct spin_table {
 };
 
 void *_reset_stack;
+void *_reset_stack_el1;
 
 #define DUMMY_STACK_SIZE 0x1000
-u8 dummy_stack[DUMMY_STACK_SIZE];
+u8 dummy_stack[DUMMY_STACK_SIZE];     // Highest EL
+u8 dummy_stack_el1[DUMMY_STACK_SIZE]; // EL1 stack if EL3 exists
 
 u8 *secondary_stacks[MAX_CPUS] = {dummy_stack};
+u8 *secondary_stacks_el3[MAX_EL3_CPUS];
 
 static bool wfe_mode = false;
 
@@ -63,14 +70,28 @@ void smp_secondary_entry(void)
     me->flag = 1;
     sysop("dmb sy");
     u64 target;
+    if (!cpufeat_fast_ipi)
+        aic_write(AIC_IPI_MASK_SET, AIC_IPI_SELF); // we only use the "other" IPI
 
     while (1) {
         while (!(target = me->target)) {
             if (wfe_mode) {
                 sysop("wfe");
             } else {
-                deep_wfi();
-                msr(SYS_IMP_APL_IPI_SR_EL1, 1);
+                if (!supports_arch_retention()) {
+                    // A7 - A11 does not support state retention across deep WFI
+                    // i.e. CPU always ends up at rvbar after deep WFI
+                    sysop("wfi");
+                } else {
+                    deep_wfi();
+                }
+                if (cpufeat_fast_ipi) {
+                    msr(SYS_IMP_APL_IPI_SR_EL1, 1);
+                } else {
+                    aic_ack(); // Actually read IPI reason
+                    aic_write(AIC_IPI_ACK, AIC_IPI_OTHER);
+                    aic_write(AIC_IPI_MASK_CLR, AIC_IPI_OTHER);
+                }
             }
             sysop("isb");
         }
@@ -92,6 +113,9 @@ static void smp_start_cpu(int index, int die, int cluster, int core, u64 impl, u
     if (index >= MAX_CPUS)
         return;
 
+    if (has_el3() && index >= MAX_EL3_CPUS)
+        return;
+
     if (spin_table[index].flag)
         return;
 
@@ -101,7 +125,12 @@ static void smp_start_cpu(int index, int die, int cluster, int core, u64 impl, u
 
     target_cpu = index;
     secondary_stacks[index] = memalign(0x4000, SECONDARY_STACK_SIZE);
-    _reset_stack = secondary_stacks[index] + SECONDARY_STACK_SIZE;
+    if (has_el3()) {
+        secondary_stacks_el3[index] = memalign(0x4000, SECONDARY_STACK_SIZE);
+        _reset_stack = secondary_stacks_el3[index] + SECONDARY_STACK_SIZE; // EL3
+        _reset_stack_el1 = secondary_stacks[index] + SECONDARY_STACK_SIZE; // EL1
+    } else
+        _reset_stack = secondary_stacks[index] + SECONDARY_STACK_SIZE;
 
     sysop("dmb sy");
 
@@ -129,6 +158,7 @@ static void smp_start_cpu(int index, int die, int cluster, int core, u64 impl, u
         printf("  Started.\n");
 
     _reset_stack = dummy_stack + DUMMY_STACK_SIZE;
+    _reset_stack_el1 = dummy_stack_el1 + DUMMY_STACK_SIZE;
 }
 
 static void smp_stop_cpu(int index, int die, int cluster, int core, u64 impl, u64 cpu_start_base,
@@ -202,6 +232,20 @@ void smp_start_secondaries(void)
     memset(cpu_nodes, 0, sizeof(cpu_nodes));
 
     switch (chip_id) {
+        case S5L8960X:
+        case T7000:
+        case T7001:
+            cpu_start_off = CPU_START_OFF_S5L8960X;
+            break;
+        case S8000:
+        case S8001:
+        case S8003:
+        case T8010:
+        case T8011:
+        case T8012:
+        case T8015:
+            cpu_start_off = CPU_START_OFF_S8000;
+            break;
         case T8103:
         case T6000:
         case T6001:
@@ -324,7 +368,11 @@ void smp_send_ipi(int cpu)
         return;
 
     u64 mpidr = spin_table[cpu].mpidr;
-    msr(SYS_IMP_APL_IPI_RR_GLOBAL_EL1, (mpidr & 0xff) | ((mpidr & 0xff00) << 8));
+    if (cpufeat_fast_ipi) {
+        msr(SYS_IMP_APL_IPI_RR_GLOBAL_EL1, (mpidr & 0xff) | ((mpidr & 0xff00) << 8));
+    } else {
+        aic_write(AIC_IPI_SEND, AIC_IPI_SEND_CPU(cpu));
+    }
 }
 
 void smp_call4(int cpu, void *func, u64 arg0, u64 arg1, u64 arg2, u64 arg3)

src/smp.h

@@ -6,10 +6,12 @@
 #include "types.h"
 #include "utils.h"
 
-#define MAX_CPUS 24
+#define MAX_CPUS     24
+#define MAX_EL3_CPUS 4
 
 #define SECONDARY_STACK_SIZE 0x10000
 extern u8 *secondary_stacks[MAX_CPUS];
+extern u8 *secondary_stacks_el3[MAX_EL3_CPUS];
 
 void smp_secondary_entry(void);
 

src/start.S

@@ -161,9 +161,22 @@ cpu_reset:
     bl debug_putc
 
     mov x0, sp
+
+    mrs x8, CurrentEL
+    cmp x8, #0xc
+    beq cpu_reset_el3
+
     bl _cpu_reset_c
     b .
 
+cpu_reset_el3:
+    bl _cpu_reset_c
+    adrp x6, _reset_stack_el1
+    add x6, x6, :lo12:_reset_stack_el1
+    ldr x6, [x6]
+    mov x0, x6
+    adr x7, _cpu_reset_c
+
 el3_eret_to_el1:
     adr x8, _vectors_start
     msr sctlr_el1, xzr