diff --git a/proxyclient/m1n1/proxy.py b/proxyclient/m1n1/proxy.py index c0bbb90e..cbfb2ff8 100644 --- a/proxyclient/m1n1/proxy.py +++ b/proxyclient/m1n1/proxy.py @@ -550,6 +550,7 @@ class M1N1Proxy(Reloadable): P_SMP_WAIT = 0x503 P_SMP_SET_WFE_MODE = 0x504 P_SMP_IS_ALIVE = 0x505 + P_SMP_STOP_SECONDARIES = 0x506 P_HEAPBLOCK_ALLOC = 0x600 P_MALLOC = 0x601 @@ -949,6 +950,8 @@ class M1N1Proxy(Reloadable): return self.request(self.P_SMP_SET_WFE_MODE, mode) def smp_is_alive(self, cpu): return self.request(self.P_SMP_IS_ALIVE, cpu) + def smp_stop_secondaries(self, deep_sleep=False): + self.request(self.P_SMP_STOP_SECONDARIES, deep_sleep) def heapblock_alloc(self, size): return self.request(self.P_HEAPBLOCK_ALLOC, size) diff --git a/src/proxy.c b/src/proxy.c index 2d45977d..c0584dd7 100644 --- a/src/proxy.c +++ b/src/proxy.c @@ -327,6 +327,9 @@ int proxy_process(ProxyRequest *request, ProxyReply *reply) case P_SMP_START_SECONDARIES: smp_start_secondaries(); break; + case P_SMP_STOP_SECONDARIES: + smp_stop_secondaries(request->args[0]); + break; case P_SMP_CALL: smp_call4(request->args[0], (void *)request->args[1], request->args[2], request->args[3], request->args[4], request->args[5]); diff --git a/src/proxy.h b/src/proxy.h index 56399e8e..778a00d7 100644 --- a/src/proxy.h +++ b/src/proxy.h @@ -85,6 +85,7 @@ typedef enum { P_SMP_WAIT, P_SMP_SET_WFE_MODE, P_SMP_IS_ALIVE, + P_SMP_STOP_SECONDARIES, P_HEAPBLOCK_ALLOC = 0x600, // Heap and memory management ops P_MALLOC, diff --git a/src/smp.c b/src/smp.c index 4cbaf001..4b993e4c 100644 --- a/src/smp.c +++ b/src/smp.c @@ -36,7 +36,10 @@ u8 *secondary_stacks[MAX_CPUS] = {dummy_stack}; static bool wfe_mode = false; static int target_cpu; +static int cpu_nodes[MAX_CPUS]; static struct spin_table spin_table[MAX_CPUS]; +static u64 pmgr_reg; +static u64 cpu_start_off; extern u8 _vectors_start[0]; @@ -79,7 +82,7 @@ void smp_secondary_entry(void) } } -static void smp_start_cpu(int index, int die, int cluster, int core, u64 rvbar, u64 cpu_start_base) +static void smp_start_cpu(int index, int die, int cluster, int core, u64 impl, u64 cpu_start_base) { int i; @@ -99,7 +102,7 @@ static void smp_start_cpu(int index, int die, int cluster, int core, u64 rvbar, sysop("dmb sy"); - write64(rvbar, (u64)_vectors_start); + write64(impl, (u64)_vectors_start); cpu_start_base += die * PMGR_DIE_OFFSET; @@ -110,14 +113,14 @@ static void smp_start_cpu(int index, int die, int cluster, int core, u64 rvbar, // Actually start the core write32(cpu_start_base + 0x8 + 4 * cluster, 1 << core); - for (i = 0; i < 500; i++) { + for (i = 0; i < 100; i++) { sysop("dmb ld"); if (spin_table[index].flag) break; udelay(1000); } - if (i >= 500) + if (i >= 100) printf("Failed!\n"); else printf(" Started.\n"); @@ -125,12 +128,58 @@ static void smp_start_cpu(int index, int die, int cluster, int core, u64 rvbar, _reset_stack = dummy_stack + DUMMY_STACK_SIZE; } +static void smp_stop_cpu(int index, int die, int cluster, int core, u64 impl, u64 cpu_start_base, + bool deep_sleep) +{ + int i; + + if (index >= MAX_CPUS) + return; + + if (!spin_table[index].flag) + return; + + printf("Stopping CPU %d (%d:%d:%d)... ", index, die, cluster, core); + + cpu_start_base += die * PMGR_DIE_OFFSET; + + // Request CPU stop + write32(cpu_start_base + 0x0, 1 << (4 * cluster + core)); + + // Put the CPU to sleep + smp_call1(index, cpu_sleep, deep_sleep); + + // If going into deep sleep, powering off the last core in a cluster kills our register + // access, so just wait a bit. + if (deep_sleep) { + udelay(10000); + printf(" Presumed stopped.\n"); + memset(&spin_table[index], 0, sizeof(struct spin_table)); + return; + } + + // Check that it actually shut down + for (i = 0; i < 50; i++) { + sysop("dmb ld"); + if (!(read64(impl + 0x100) & 0xff)) + break; + udelay(1000); + } + + if (i >= 50) { + printf("Failed!\n"); + } else { + printf(" Stopped.\n"); + + memset(&spin_table[index], 0, sizeof(struct spin_table)); + } +} + void smp_start_secondaries(void) { printf("Starting secondary CPUs...\n"); int pmgr_path[8]; - u64 pmgr_reg; if (adt_path_offset_trace(adt, "/arm-io/pmgr", pmgr_path) < 0) { printf("Error getting /arm-io/pmgr node\n"); @@ -147,9 +196,6 @@ void smp_start_secondaries(void) return; } - int cpu_nodes[MAX_CPUS]; - u64 cpu_start_off; - memset(cpu_nodes, 0, sizeof(cpu_nodes)); switch (chip_id) { @@ -208,6 +254,32 @@ void smp_start_secondaries(void) spin_table[0].mpidr = mrs(MPIDR_EL1) & 0xFFFFFF; } +void smp_stop_secondaries(bool deep_sleep) +{ + printf("Stopping secondary CPUs...\n"); + smp_set_wfe_mode(true); + + for (int i = 1; i < MAX_CPUS; i++) { + int node = cpu_nodes[i]; + + if (!node) + continue; + + u32 reg; + u64 cpu_impl_reg[2]; + if (ADT_GETPROP(adt, node, "reg", ®) < 0) + continue; + if (ADT_GETPROP_ARRAY(adt, node, "cpu-impl-reg", cpu_impl_reg) < 0) + continue; + + 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_stop_cpu(i, die, cluster, core, cpu_impl_reg[0], pmgr_reg + cpu_start_off, deep_sleep); + } +} + void smp_send_ipi(int cpu) { if (cpu >= MAX_CPUS) diff --git a/src/smp.h b/src/smp.h index c135f942..eaed0a92 100644 --- a/src/smp.h +++ b/src/smp.h @@ -14,6 +14,7 @@ extern u8 *secondary_stacks[MAX_CPUS]; void smp_secondary_entry(void); void smp_start_secondaries(void); +void smp_stop_secondaries(bool deep_sleep); #define smp_call0(i, f) smp_call4(i, f, 0, 0, 0, 0) #define smp_call1(i, f, a) smp_call4(i, f, a, 0, 0, 0)