From 5f4ad52dea483bba412bd1d20e0dca1d6c3ab2d8 Mon Sep 17 00:00:00 2001 From: kettenis Date: Sun, 19 Feb 2023 17:16:13 +0000 Subject: [PATCH] Add support for deep(er) idle states that can be entered using PSCI. For now this only supports states advertised in device trees, but ACPI support could be added as well. The parsing of the idle states as well as the heuristic to pick the deepest one is probably a bit to simple, but more complex cases can be added later. Worst case cores will use WFI and use more power in suspend. ok phessler@ --- sys/arch/arm64/arm64/cpu.c | 231 +++++++++++++++++++++++++++-------- sys/arch/arm64/include/cpu.h | 4 +- sys/dev/fdt/psci.c | 26 +++- sys/dev/fdt/pscivar.h | 1 + 4 files changed, 209 insertions(+), 53 deletions(-) diff --git a/sys/arch/arm64/arm64/cpu.c b/sys/arch/arm64/arm64/cpu.c index 1fffc89cf39..e43b09edb56 100644 --- a/sys/arch/arm64/arm64/cpu.c +++ b/sys/arch/arm64/arm64/cpu.c @@ -1,4 +1,4 @@ -/* $OpenBSD: cpu.c,v 1.84 2023/02/09 23:35:06 jsg Exp $ */ +/* $OpenBSD: cpu.c,v 1.85 2023/02/19 17:16:13 kettenis Exp $ */ /* * Copyright (c) 2016 Dale Rahn @@ -240,6 +240,7 @@ struct cfdriver cpu_cd = { }; void cpu_opp_init(struct cpu_info *, uint32_t); +void cpu_psci_init(struct cpu_info *); void cpu_flush_bp_noop(void); void cpu_flush_bp_psci(void); @@ -879,6 +880,8 @@ cpu_attach(struct device *parent, struct device *dev, void *aux) if (opp) cpu_opp_init(ci, opp); + cpu_psci_init(ci); + printf("\n"); } @@ -1112,12 +1115,19 @@ void cpu_halt(void) { struct cpu_info *ci = curcpu(); + vaddr_t start_va; + paddr_t ci_pa, start_pa; int count = 0; u_long psw; + int32_t status; KERNEL_ASSERT_UNLOCKED(); SCHED_ASSERT_UNLOCKED(); + start_va = (vaddr_t)cpu_hatch_secondary; + pmap_extract(pmap_kernel(), start_va, &start_pa); + pmap_extract(pmap_kernel(), (vaddr_t)ci, &ci_pa); + psw = intr_disable(); atomic_clearbits_int(&ci->ci_flags, @@ -1141,7 +1151,15 @@ cpu_halt(void) READ_SPECIALREG(cntv_ctl_el0) | CNTV_CTL_IMASK); while ((ci->ci_flags & CPUF_GO) == 0) { - __asm volatile("wfi"); +#if NPSCI > 0 + if (ci->ci_psci_suspend_param) { + status = psci_cpu_suspend(ci->ci_psci_suspend_param, + start_pa, ci_pa); + if (status != PSCI_SUCCESS) + ci->ci_psci_suspend_param = 0; + } else +#endif + __asm volatile("wfi"); count++; } @@ -1193,7 +1211,6 @@ cpu_init_primary(void) cpu_startclock(); - cpu_suspended = 1; longjmp(&cpu_suspend_jmpbuf); } @@ -1204,68 +1221,76 @@ cpu_suspend_primary(void) vaddr_t start_va; paddr_t ci_pa, start_pa; uint64_t ttbr1; + int32_t status; + int count = 0; - if (!psci_can_suspend()) { - int count = 0; + __asm("mrs %x0, ttbr1_el1": "=r"(ttbr1)); + ci->ci_ttbr1 = ttbr1; + cpu_dcache_wb_range((vaddr_t)ci, sizeof(*ci)); - /* - * If PSCI doesn't support SYSTEM_SUSPEND, spin in a - * low power state waiting for an interrupt that wakes - * us up again. - */ + start_va = (vaddr_t)cpu_hatch_primary; + pmap_extract(pmap_kernel(), start_va, &start_pa); + pmap_extract(pmap_kernel(), (vaddr_t)ci, &ci_pa); - /* Mask clock interrupts. */ - WRITE_SPECIALREG(cntv_ctl_el0, - READ_SPECIALREG(cntv_ctl_el0) | CNTV_CTL_IMASK); +#if NPSCI > 0 + if (psci_can_suspend()) { + if (setjmp(&cpu_suspend_jmpbuf)) { + /* XXX wait for debug output on Allwinner A64 */ + delay(200000); + return 0; + } - /* - * All non-wakeup interrupts should be masked at this - * point; re-enable interrupts such that wakeup - * interrupts actually wake us up. Set a flag such - * that drivers can tell we're suspended and change - * their behaviour accordingly. They can wake us up - * by clearing the flag. - */ - cpu_suspended = 1; - intr_enable_wakeup(); + psci_system_suspend(start_pa, ci_pa); - while (cpu_suspended) { - __asm volatile("wfi"); - count++; - } + return EOPNOTSUPP; + } +#endif - intr_disable_wakeup(); + if (setjmp(&cpu_suspend_jmpbuf)) + goto resume; - /* Unmask clock interrupts. */ - WRITE_SPECIALREG(cntv_ctl_el0, - READ_SPECIALREG(cntv_ctl_el0) & ~CNTV_CTL_IMASK); + /* + * If PSCI doesn't support SYSTEM_SUSPEND, spin in a low power + * state waiting for an interrupt that wakes us up again. + */ - printf("%s: %d wakeup events\n", ci->ci_dev->dv_xname, count); + /* Mask clock interrupts. */ + WRITE_SPECIALREG(cntv_ctl_el0, + READ_SPECIALREG(cntv_ctl_el0) | CNTV_CTL_IMASK); - return 0; - } + /* + * All non-wakeup interrupts should be masked at this point; + * re-enable interrupts such that wakeup interrupts actually + * wake us up. Set a flag such that drivers can tell we're + * suspended and change their behaviour accordingly. They can + * wake us up by clearing the flag. + */ + cpu_suspended = 1; + intr_enable_wakeup(); - cpu_suspended = 0; - setjmp(&cpu_suspend_jmpbuf); - if (cpu_suspended) { - /* XXX wait for debug output from SCP on Allwinner A64 */ - delay(200000); - return 0; + while (cpu_suspended) { +#if NPSCI > 0 + if (ci->ci_psci_suspend_param) { + status = psci_cpu_suspend(ci->ci_psci_suspend_param, + start_pa, ci_pa); + if (status != PSCI_SUCCESS) + ci->ci_psci_suspend_param = 0; + } else +#endif + __asm volatile("wfi"); + count++; } - __asm("mrs %x0, ttbr1_el1": "=r"(ttbr1)); - ci->ci_ttbr1 = ttbr1; - cpu_dcache_wb_range((vaddr_t)ci, sizeof(*ci)); +resume: + intr_disable_wakeup(); - start_va = (vaddr_t)cpu_hatch_primary; - pmap_extract(pmap_kernel(), start_va, &start_pa); - pmap_extract(pmap_kernel(), (vaddr_t)ci, &ci_pa); + /* Unmask clock interrupts. */ + WRITE_SPECIALREG(cntv_ctl_el0, + READ_SPECIALREG(cntv_ctl_el0) & ~CNTV_CTL_IMASK); -#if NPSCI > 0 - psci_system_suspend(start_pa, ci_pa); -#endif + printf("%s: %d wakeup events\n", ci->ci_dev->dv_xname, count); - return EOPNOTSUPP; + return 0; } #ifdef MULTIPROCESSOR @@ -1608,3 +1633,109 @@ cpu_opp_set_cooling_level(void *cookie, uint32_t *cells, uint32_t level) task_add(systq, &cpu_opp_task); } } + + +void +cpu_psci_init(struct cpu_info *ci) +{ + uint32_t *domains; + uint32_t *domain; + uint32_t *states; + uint32_t ncells; + uint32_t cluster; + int idx, len, node; + + /* + * Hunt for the deppest idle state for this CPU. This is + * fairly complicated as it requires traversing quite a few + * nodes in the device tree. The first step is to look up the + * "psci" power domain for this CPU. + */ + + idx = OF_getindex(ci->ci_node, "psci", "power-domain-names"); + if (idx < 0) + return; + + len = OF_getproplen(ci->ci_node, "power-domains"); + if (len <= 0) + return; + + domains = malloc(len, M_TEMP, M_WAITOK); + OF_getpropintarray(ci->ci_node, "power-domains", domains, len); + + domain = domains; + while (domain && domain < domains + (len / sizeof(uint32_t))) { + if (idx == 0) + break; + + node = OF_getnodebyphandle(domain[0]); + if (node == 0) + break; + + ncells = OF_getpropint(node, "#power-domain-cells", 0); + domain = domain + ncells + 1; + idx--; + } + + node = idx == 0 ? OF_getnodebyphandle(domain[0]) : 0; + free(domains, M_TEMP, len); + if (node == 0) + return; + + /* + * We found the "psci" power domain. If this power domain has + * a parent power domain, stash its phandle away for later. + */ + + cluster = OF_getpropint(node, "power-domains", 0); + + /* + * Get the deepest idle state for the CPU; this should be the + * last one that is listed. + */ + + len = OF_getproplen(node, "domain-idle-states"); + if (len < sizeof(uint32_t)) + return; + + states = malloc(len, M_TEMP, M_WAITOK); + OF_getpropintarray(node, "domain-idle-states", states, len); + + node = OF_getnodebyphandle(states[len / sizeof(uint32_t) - 1]); + free(states, M_TEMP, len); + if (node == 0) + return; + + ci->ci_psci_suspend_param = + OF_getpropint(node, "arm,psci-suspend-param", 0); + + /* + * Qualcomm Snapdragon always seem to operate in OS Initiated + * mode. This means that the last CPU to suspend can pick the + * idle state that powers off the entire cluster. In our case + * that will always be the primary CPU. + */ + + if (ci->ci_flags & CPUF_AP) + return; + + node = OF_getnodebyphandle(cluster); + if (node == 0) + return; + + /* + * Get the deepest idle state for the cluster; this should be + * the last one that is listed. + */ + + states = malloc(len, M_TEMP, M_WAITOK); + OF_getpropintarray(node, "domain-idle-states", states, len); + + node = OF_getnodebyphandle(states[len / sizeof(uint32_t) - 1]); + free(states, M_TEMP, len); + if (node == 0) + return; + + ci->ci_psci_suspend_param = + OF_getpropint(node, "arm,psci-suspend-param", 0); +} diff --git a/sys/arch/arm64/include/cpu.h b/sys/arch/arm64/include/cpu.h index 3ab8a14e166..2ddaec58f66 100644 --- a/sys/arch/arm64/include/cpu.h +++ b/sys/arch/arm64/include/cpu.h @@ -1,4 +1,4 @@ -/* $OpenBSD: cpu.h,v 1.34 2022/12/31 00:30:21 patrick Exp $ */ +/* $OpenBSD: cpu.h,v 1.35 2023/02/19 17:16:13 kettenis Exp $ */ /* * Copyright (c) 2016 Dale Rahn * @@ -142,6 +142,8 @@ struct cpu_info { uint64_t ci_ttbr1; vaddr_t ci_el1_stkend; + uint32_t ci_psci_suspend_param; + struct opp_table *ci_opp_table; volatile int ci_opp_idx; volatile int ci_opp_max; diff --git a/sys/dev/fdt/psci.c b/sys/dev/fdt/psci.c index 469cf1f67d0..675a97d5cd8 100644 --- a/sys/dev/fdt/psci.c +++ b/sys/dev/fdt/psci.c @@ -1,4 +1,4 @@ -/* $OpenBSD: psci.c,v 1.13 2023/02/13 19:26:15 kettenis Exp $ */ +/* $OpenBSD: psci.c,v 1.14 2023/02/19 17:16:13 kettenis Exp $ */ /* * Copyright (c) 2016 Jonathan Gray @@ -37,6 +37,11 @@ extern void (*powerdownfn)(void); #define SMCCC_ARCH_WORKAROUND_3 0x80003fff #define PSCI_VERSION 0x84000000 +#ifdef __LP64__ +#define CPU_SUSPEND 0xc4000001 +#else +#define CPU_SUSPEND 0x84000001 +#endif #define CPU_OFF 0x84000002 #ifdef __LP64__ #define CPU_ON 0xc4000003 @@ -56,12 +61,13 @@ struct psci_softc { struct device sc_dev; register_t (*sc_callfn)(register_t, register_t, register_t, register_t); - uint32_t sc_psci_version; + uint32_t sc_psci_version; uint32_t sc_system_off; uint32_t sc_system_reset; uint32_t sc_system_suspend; uint32_t sc_cpu_on; uint32_t sc_cpu_off; + uint32_t sc_cpu_suspend; uint32_t sc_smccc_version; uint32_t sc_method; @@ -131,6 +137,7 @@ psci_attach(struct device *parent, struct device *self, void *aux) sc->sc_system_reset = SYSTEM_RESET; sc->sc_cpu_on = CPU_ON; sc->sc_cpu_off = CPU_OFF; + sc->sc_cpu_suspend = CPU_SUSPEND; } else if (OF_is_compatible(faa->fa_node, "arm,psci")) { sc->sc_system_off = OF_getpropint(faa->fa_node, "system_off", 0); @@ -138,6 +145,8 @@ psci_attach(struct device *parent, struct device *self, void *aux) "system_reset", 0); sc->sc_cpu_on = OF_getpropint(faa->fa_node, "cpu_on", 0); sc->sc_cpu_off = OF_getpropint(faa->fa_node, "cpu_off", 0); + sc->sc_cpu_suspend = OF_getpropint(faa->fa_node, + "cpu_suspend", 0); } psci_sc = sc; @@ -332,6 +341,19 @@ psci_cpu_on(register_t target_cpu, register_t entry_point_address, return PSCI_NOT_SUPPORTED; } +int32_t +psci_cpu_suspend(register_t power_state, register_t entry_point_address, + register_t context_id) +{ + struct psci_softc *sc = psci_sc; + + if (sc && sc->sc_callfn && sc->sc_cpu_suspend != 0) + return (*sc->sc_callfn)(sc->sc_cpu_suspend, power_state, + entry_point_address, context_id); + + return PSCI_NOT_SUPPORTED; +} + int32_t psci_features(uint32_t psci_func_id) { diff --git a/sys/dev/fdt/pscivar.h b/sys/dev/fdt/pscivar.h index 9a400290f93..04e418f3190 100644 --- a/sys/dev/fdt/pscivar.h +++ b/sys/dev/fdt/pscivar.h @@ -15,6 +15,7 @@ int psci_can_suspend(void); int32_t psci_system_suspend(register_t, register_t); int32_t psci_cpu_on(register_t, register_t, register_t); int32_t psci_cpu_off(void); +int32_t psci_cpu_suspend(register_t, register_t, register_t); void psci_flush_bp(void); int psci_flush_bp_has_bhb(void); int psci_method(void); -- 2.20.1