diff --git a/src/kboot.c b/src/kboot.c index e8c8788b..417cab3a 100644 --- a/src/kboot.c +++ b/src/kboot.c @@ -8,6 +8,7 @@ #include "libfdt/libfdt.h" #include "malloc.h" #include "memory.h" +#include "smp.h" #include "types.h" #include "utils.h" #include "xnuboot.h" @@ -150,6 +151,41 @@ static int dt_set_memory(void) return 0; } +static int dt_set_cpus(void) +{ + int cpus = fdt_path_offset(dt, "/cpus"); + if (cpus < 0) + bail("FDT: /cpus node not found in devtree\n"); + + int node, cpu = 0; + fdt_for_each_subnode(node, dt, cpus) + { + const fdt64_t *prop = fdt_getprop(dt, node, "reg", NULL); + if (!prop) + bail("FDT: failed to get reg property of CPU\n"); + + u64 dt_mpidr = fdt64_ld(prop); + u64 mpidr = smp_get_mpidr(cpu); + + if (dt_mpidr != mpidr) + bail("FDT: DT CPU %d MPIDR mismatch: 0x%x != 0x%x\n", cpu, dt_mpidr, mpidr); + + u64 release_addr = smp_get_release_addr(cpu); + if (fdt_setprop_u64(dt, node, "cpu-release-addr", release_addr)) + bail("FDT: couldn't set cpu-release-addr property\n"); + + printf("FDT: CPU %d MPIDR=0x%x release-addr=0x%lx\n", cpu, mpidr, release_addr); + + cpu++; + } + + if ((node < 0) && (node != -FDT_ERR_NOTFOUND)) { + bail("FDT: error iterating through CPUs\n"); + } + + return 0; +} + void kboot_set_initrd(void *start, size_t size) { initrd_start = start; @@ -193,6 +229,8 @@ int kboot_prepare_dt(void *fdt) return -1; if (dt_set_memory()) return -1; + if (dt_set_cpus()) + return -1; if (fdt_pack(dt)) bail("FDT: fdt_pack() failed\n"); diff --git a/src/smp.c b/src/smp.c index 164d6159..2040fdc4 100644 --- a/src/smp.c +++ b/src/smp.c @@ -11,6 +11,7 @@ #define SECONDARY_STACK_SIZE 0x4000 struct spin_table { + u64 mpidr; u64 flag; u64 target; u64 args[4]; @@ -31,6 +32,8 @@ void smp_secondary_entry(void) struct spin_table *me = &spin_table[target_cpu]; printf(" Index: %d (table: %p)\n\n", target_cpu, me); + me->mpidr = mrs(MPIDR_EL1) & 0xFFFFFF; + sysop("dmb sy"); me->flag = 1; sysop("dmb sy"); @@ -145,6 +148,8 @@ void smp_start_secondaries(void) smp_start_cpu(i, reg >> 8, reg & 0xff, cpu_impl_reg[0], pmgr_reg + CPU_START_OFF); } + + spin_table[0].mpidr = mrs(MPIDR_EL1) & 0xFFFFFF; } void smp_call4(int cpu, void *func, u64 arg0, u64 arg1, u64 arg2, u64 arg3) @@ -176,3 +181,19 @@ u64 smp_wait(int cpu) return target->retval; } + +int smp_get_mpidr(int cpu) +{ + return spin_table[cpu].mpidr; +} + +u64 smp_get_release_addr(int cpu) +{ + struct spin_table *target = &spin_table[cpu]; + + target->args[0] = 0; + target->args[1] = 0; + target->args[2] = 0; + target->args[3] = 0; + return (u64)&target->target; +} diff --git a/src/smp.h b/src/smp.h index ed310d4e..882a12dd 100644 --- a/src/smp.h +++ b/src/smp.h @@ -20,4 +20,7 @@ void smp_call4(int cpu, void *func, u64 arg0, u64 arg1, u64 arg2, u64 arg3); u64 smp_wait(int cpu); +int smp_get_mpidr(int cpu); +u64 smp_get_release_addr(int cpu); + #endif