smp: Start CPU cores on the second t6002 die

Signed-off-by: Janne Grunau <[email protected]>

Author: jannau

src/smp.c

@@ -4,12 +4,17 @@
 #include "adt.h"
 #include "cpu_regs.h"
 #include "malloc.h"
+#include "pmgr.h"
 #include "string.h"
 #include "types.h"
 #include "utils.h"
 
 #define CPU_START_OFF 0x54000
 
+#define CPU_REG_CORE    GENMASK(7, 0)
+#define CPU_REG_CLUSTER GENMASK(10, 8)
+#define CPU_REG_DIE     GENMASK(14, 11)
+
 struct spin_table {
     u64 mpidr;
     u64 flag;
@@ -69,7 +74,7 @@ void smp_secondary_entry(void)
     }
 }
 
-static void smp_start_cpu(int index, int cluster, int core, u64 rvbar, u64 cpu_start_base)
+static void smp_start_cpu(int index, int die, int cluster, int core, u64 rvbar, u64 cpu_start_base)
 {
     int i;
 
@@ -79,7 +84,7 @@ static void smp_start_cpu(int index, int cluster, int core, u64 rvbar, u64 cpu_s
     if (spin_table[index].flag)
         return;
 
-    printf("Starting CPU %d (%d:%d)... ", index, cluster, core);
+    printf("Starting CPU %d (%d:%d:%d)... ", index, die, cluster, core);
 
     memset(&spin_table[index], 0, sizeof(struct spin_table));
 
@@ -91,6 +96,8 @@ static void smp_start_cpu(int index, int cluster, int core, u64 rvbar, u64 cpu_s
 
     write64(rvbar, (u64)_vectors_start);
 
+    cpu_start_base += die * PMGR_DIE_OFFSET;
+
     // Some kind of system level startup/status bit
     // Without this, IRQs don't work
     write32(cpu_start_base + 0x4, 1 << (4 * cluster + core));
@@ -166,7 +173,11 @@ void smp_start_secondaries(void)
         if (ADT_GETPROP_ARRAY(adt, node, "cpu-impl-reg", cpu_impl_reg) < 0)
             continue;
 
-        smp_start_cpu(i, reg >> 8, reg & 0xff, cpu_impl_reg[0], pmgr_reg + CPU_START_OFF);
+        u8 core = FIELD_GET(CPU_REG_CORE, reg);
+        u8 cluster = FIELD_GET(CPU_REG_CLUSTER, reg);
+        u8 die = FIELD_GET(CPU_REG_DIE, reg);
+
+        smp_start_cpu(i, die, cluster, core, cpu_impl_reg[0], pmgr_reg + CPU_START_OFF);
     }
 
     spin_table[0].mpidr = mrs(MPIDR_EL1) & 0xFFFFFF;