From 741d82c42b1ae96fe3634be4469fb2f33def2df7 Mon Sep 17 00:00:00 2001 From: GuEe-GUI <2991707448@qq.com> Date: Mon, 15 Jun 2026 15:35:26 +0800 Subject: [PATCH] [libcpu][aarch64] support DVFS idle Signed-off-by: GuEe-GUI <2991707448@qq.com> --- libcpu/aarch64/Kconfig | 11 ++ libcpu/aarch64/common/SConscript | 3 + libcpu/aarch64/common/cpuidle.c | 203 +++++++++++++++++++++++++++++++ libcpu/aarch64/common/setup.c | 61 ++++++++++ 4 files changed, 278 insertions(+) create mode 100644 libcpu/aarch64/common/cpuidle.c diff --git a/libcpu/aarch64/Kconfig b/libcpu/aarch64/Kconfig index 5429751d3a..d2f531da24 100644 --- a/libcpu/aarch64/Kconfig +++ b/libcpu/aarch64/Kconfig @@ -22,4 +22,15 @@ menu "AArch64 Architecture Configuration" config ARCH_INIT_PAGE_SIZE hex "Size of init page region" default 0x200000 + + config ARCH_USING_CPUIDLE + bool "Using ARM PSCI CPU idle (cpuidle)" + depends on RT_USING_DVFS + depends on RT_USING_OFW + default n + help + Enable PSCI-based CPU idle states from /cpus/idle-states. + Standby states use CPU_SUSPEND with entry_point=0. + Power-down states are skipped until full suspend context save + is implemented. endmenu diff --git a/libcpu/aarch64/common/SConscript b/libcpu/aarch64/common/SConscript index b0e2698898..a32e8c52d6 100644 --- a/libcpu/aarch64/common/SConscript +++ b/libcpu/aarch64/common/SConscript @@ -34,6 +34,9 @@ if GetDepend('RT_USING_PIC') == True: if GetDepend('RT_CLOCK_TIME_ARM_ARCH') == True: SrcRemove(src, ['gtimer.c']) +if GetDepend('ARCH_USING_CPUIDLE') == False: + SrcRemove(src, ['cpuidle.c']) + group = DefineGroup('CPU', src, depend = [''], CPPPATH = CPPPATH) # build for sub-directory diff --git a/libcpu/aarch64/common/cpuidle.c b/libcpu/aarch64/common/cpuidle.c new file mode 100644 index 0000000000..6dca545384 --- /dev/null +++ b/libcpu/aarch64/common/cpuidle.c @@ -0,0 +1,203 @@ +/* + * Copyright (c) 2006-2022, RT-Thread Development Team + * + * SPDX-License-Identifier: Apache-2.0 + * + * Change Logs: + * Date Author Notes + * 2022-11-21 GuEe-GUI first version + */ + +#include +#include + +#include + +#define DBG_TAG "cpuidle" +#define DBG_LVL DBG_INFO +#include + +struct arm_cpuidle +{ + struct rt_dvfs_idle parent; + struct rt_device device; + + rt_bool_t arch_timer_wakeup_source; +}; + +#if defined(RT_CLOCK_TIME_ARM_ARCH) || defined(RT_HWTIMER_ARM_ARCH) +#define ARM_CPUIDLE_ARCH_TIMER 1 +#endif + +#ifdef __GNUC__ +__asm__ ( +"asm_arm_cpuidle_entry: \n\t" +" stp x29, x30, [sp, #-0x20]! \n\t" +" stp x19, x20, [sp, #0x10] \n\t" +" mov x19, x0 \n\t" +#ifdef ARM_CPUIDLE_ARCH_TIMER +" bl arm_arch_timer_local_disable\n\t" +#endif +" mov x0, x19 \n\t" +" mov x1, xzr \n\t" +" bl rt_psci_cpu_suspend \n\t" +" mov x20, x0 \n\t" +#ifdef ARM_CPUIDLE_ARCH_TIMER +" bl arm_arch_timer_local_enable \n\t" +#endif +" mov x0, x20 \n\t" +" ldp x19, x20, [sp, #0x10] \n\t" +" ldp x29, x30, [sp], #0x20 \n\t" +" ret \n\t" + ); +#else +#error "No suspend context save" +#endif + +extern rt_uint32_t asm_arm_cpuidle_entry(rt_uint32_t psci_param); + +static rt_bool_t arm_cpuidle_state_loses_context(rt_uint32_t state) +{ + return PSCI_POWER_STATE_TYPE_VAL(state) == PSCI_POWER_STATE_TYPE_POWER_DOWN; +} + +static rt_err_t arm_cpuidle_entry(struct rt_dvfs_idle *idle, struct rt_dvfs_idle_status *status) +{ + return asm_arm_cpuidle_entry((rt_ubase_t)status->priv) >= 0 ? RT_EOK : -RT_ENOSYS; +} + +static rt_err_t arm_cpuidle_exit(struct rt_dvfs_idle *idle, struct rt_dvfs_idle_status *status) +{ + return RT_EOK; +} + +static rt_bool_t arm_cpuidle_timer_can_stop(struct rt_dvfs_idle *idle) +{ + struct arm_cpuidle *cpuidle = rt_container_of(idle, struct arm_cpuidle, parent); + + return !cpuidle->arch_timer_wakeup_source; +} + +static struct rt_dvfs_idle_ops arm_cpuidle_ops = +{ + .entry = arm_cpuidle_entry, + .exit = arm_cpuidle_exit, + .timer_can_stop = arm_cpuidle_timer_can_stop, +}; + +#ifdef ARM_CPUIDLE_ARCH_TIMER +static const struct rt_ofw_node_id arm_arch_timer_ofw_ids[] = +{ + { .compatible = "arm,armv7-timer", }, + { .compatible = "arm,armv8-timer", }, + { /* sentinel */ } +}; +#endif + +static void arm_cpuidle_status_free(struct rt_dvfs_idle *idle, struct rt_dvfs_idle_status *status) +{ + rt_free(status); +} + +static int arm_cpuidle_probe(void) +{ + const char *entry_method; + struct arm_cpuidle *cpuidle; + struct rt_ofw_node *np, *child_np, *cpu_np; + struct rt_dvfs_idle_status *status; + rt_bool_t timer_wakeup = RT_FALSE; + + if (!(np = rt_ofw_find_node_by_path("/cpus/idle-states"))) + { + return (int)-RT_ENOSYS; + } + + if (rt_ofw_prop_read_string(np, "entry-method", &entry_method)) + { + return (int)-RT_EINVAL; + } + + if (rt_strcmp(entry_method, "psci") && + rt_strcmp(entry_method, "arm,psci")) + { + return (int)-RT_ENOSYS; + } + +#ifdef ARM_CPUIDLE_ARCH_TIMER + /* Check arch timer wakeup source */ + struct rt_ofw_node *timer_np; + if ((timer_np = rt_ofw_find_node_by_ids(RT_NULL, arm_arch_timer_ofw_ids))) + { + timer_wakeup = rt_ofw_prop_read_bool(timer_np, "wakeup-source"); + rt_ofw_node_put(timer_np); + } +#endif + + /* Iterate over all CPUs and register idle devices */ + rt_ofw_foreach_cpu_node(cpu_np) + { + int cpu_id = rt_ofw_get_cpu_id(cpu_np); + if (cpu_id < 0 || cpu_id >= RT_CPUS_NR) + { + continue; + } + + if (!(cpuidle = rt_calloc(1, sizeof(*cpuidle)))) + { + return (int)-RT_ENOMEM; + } + + /* Parse idle states */ + rt_ofw_foreach_child_node(np, child_np) + { + rt_uint32_t psci_param; + + if (rt_ofw_prop_read_u32(child_np, "arm,psci-suspend-param", &psci_param)) + { + psci_param = PSCI_POWER_STATE( + PSCI_POWER_STATE_LEVEL_CORES, PSCI_POWER_STATE_TYPE_STANDBY, 0); + } + + if (arm_cpuidle_state_loses_context(psci_param)) + { + LOG_W("skip power-down idle state on %s (context save not supported)", + rt_ofw_node_full_name(child_np)); + continue; + } + + status = rt_calloc(1, sizeof(*status)); + + if (!status) + { + rt_dvfs_idle_remove_status_all(&cpuidle->parent, arm_cpuidle_status_free); + rt_free(cpuidle); + return (int)-RT_ENOMEM; + } + + rt_ofw_prop_read_u32(child_np, "entry-latency-us", &status->entry_latency_us); + rt_ofw_prop_read_u32(child_np, "exit-latency-us", &status->exit_latency_us); + rt_ofw_prop_read_u32(child_np, "min-residency-us", &status->min_residency_us); + status->timer_stop = rt_ofw_prop_read_bool(child_np, "local-timer-stop"); + status->priv = (void *)(rt_ubase_t)psci_param; + + rt_dvfs_idle_add_status(&cpuidle->parent, status); + } + + /* Check if any idle states were added */ + if (!cpuidle->parent.status_table || rt_list_isempty(&cpuidle->parent.status_table->status_nodes)) + { + rt_free(cpuidle); + continue; + } + + cpuidle->device.ofw_node = cpu_np; + cpuidle->parent.dev = &cpuidle->device; + cpuidle->parent.ops = &arm_cpuidle_ops; + cpuidle->arch_timer_wakeup_source = timer_wakeup; + + rt_dvfs_idle_register(&cpuidle->parent); + } + + return RT_EOK; +} +INIT_PLATFORM_EXPORT(arm_cpuidle_probe); diff --git a/libcpu/aarch64/common/setup.c b/libcpu/aarch64/common/setup.c index 79df36ec5b..89acc15009 100644 --- a/libcpu/aarch64/common/setup.c +++ b/libcpu/aarch64/common/setup.c @@ -57,6 +57,9 @@ static struct cpu_ops_t *cpu_ops[] = #endif }; +#ifdef ARCH_USING_CPUIDLE +struct rt_dvfs_idle *cpu_idle[RT_CPUS_NR] = {}; +#endif static struct rt_ofw_node *cpu_np[RT_CPUS_NR] = { }; void rt_hw_fdt_install_early(void *fdt) @@ -136,6 +139,21 @@ static void cpu_us_delay(rt_uint32_t us) rt_weak void rt_hw_idle_wfi(void) { +#ifdef ARCH_USING_CPUIDLE + struct rt_dvfs_idle *cpuidle = cpu_idle[rt_hw_cpu_id()]; + + if (cpuidle) + { + rt_dvfs_idle_entry(cpuidle); + + __asm__ volatile ("wfi"); + + rt_dvfs_idle_exit(cpuidle); + + return; + } +#endif /* ARCH_USING_CPUIDLE */ + __asm__ volatile ("wfi"); } @@ -409,6 +427,34 @@ void rt_hw_common_setup(void) #endif } +#ifdef ARCH_USING_CPUIDLE +static int cpuidle_init(void) +{ + static struct rt_device cpuidle_dev = {}; + + for (int i = 0; i < RT_ARRAY_SIZE(cpu_idle); ++i) + { + struct rt_dvfs_idle *cpuidle; + + if (!cpu_np[i]) + { + continue; + } + + cpuidle_dev.ofw_node = cpu_np[i]; + cpuidle = rt_dvfs_idle_get(&cpuidle_dev); + + if (!rt_is_err(cpuidle)) + { + cpu_idle[i] = cpuidle; + } + } + + return 0; +} +INIT_PREV_EXPORT(cpuidle_init); +#endif /* ARCH_USING_CPUIDLE */ + #ifdef RT_USING_SMP rt_weak void rt_hw_secondary_cpu_up(void) { @@ -503,6 +549,21 @@ rt_weak void rt_hw_secondary_cpu_bsp_start(void) rt_weak void rt_hw_secondary_cpu_idle_exec(void) { +#ifdef ARCH_USING_CPUIDLE + struct rt_dvfs_idle *cpuidle = cpu_idle[rt_hw_cpu_id()]; + + if (cpuidle) + { + rt_dvfs_idle_entry(cpuidle); + + rt_hw_wfe(); + + rt_dvfs_idle_exit(cpuidle); + + return; + } +#endif /* ARCH_USING_CPUIDLE */ + rt_hw_wfe(); } #endif