From 5ed166e97f02df3166941fbd0a887110918fa1a3 Mon Sep 17 00:00:00 2001 From: wuzhenghui Date: Fri, 30 Jan 2026 20:03:52 +0800 Subject: [PATCH] feat(esp_hw_support): support esp32p4 RV FPU context retention in pd_cpu sleep --- .../esp_hw_support/lowpower/CMakeLists.txt | 7 ++ .../lowpower/port/esp32p4/rvsleep-frames.h | 36 +++++- .../lowpower/port/esp32p4/sleep_cpu.c | 28 ++++- .../lowpower/port/esp32p4/sleep_fpu_asm.S | 118 ++++++++++++++++++ .../riscv/include/freertos/portmacro.h | 17 ++- .../FreeRTOS-Kernel/portable/riscv/port.c | 8 +- components/freertos/linker.lf | 2 + .../esp32p4/include/soc/Kconfig.soc_caps.in | 4 + components/soc/esp32p4/include/soc/soc_caps.h | 1 + 9 files changed, 213 insertions(+), 8 deletions(-) create mode 100644 components/esp_hw_support/lowpower/port/esp32p4/sleep_fpu_asm.S diff --git a/components/esp_hw_support/lowpower/CMakeLists.txt b/components/esp_hw_support/lowpower/CMakeLists.txt index 22558a817b..532b28bc21 100644 --- a/components/esp_hw_support/lowpower/CMakeLists.txt +++ b/components/esp_hw_support/lowpower/CMakeLists.txt @@ -15,6 +15,13 @@ if(CONFIG_PM_POWER_DOWN_CPU_IN_LIGHT_SLEEP OR APPEND PROPERTY INTERFACE_LINK_LIBRARIES "-u rv_core_critical_regs_save") set_property(TARGET ${COMPONENT_LIB} APPEND PROPERTY INTERFACE_LINK_LIBRARIES "-u rv_core_critical_regs_restore") + if(CONFIG_SOC_CPU_HAS_FPU AND CONFIG_SOC_PM_FPU_RETENTION_BY_SW) + list(APPEND srcs "port/${target}/sleep_fpu_asm.S") + set_property(TARGET ${COMPONENT_LIB} + APPEND PROPERTY INTERFACE_LINK_LIBRARIES "-u rv_core_fpu_save") + set_property(TARGET ${COMPONENT_LIB} + APPEND PROPERTY INTERFACE_LINK_LIBRARIES "-u rv_core_fpu_restore") + endif() endif() endif() diff --git a/components/esp_hw_support/lowpower/port/esp32p4/rvsleep-frames.h b/components/esp_hw_support/lowpower/port/esp32p4/rvsleep-frames.h index bcff56ea41..235c4c1567 100644 --- a/components/esp_hw_support/lowpower/port/esp32p4/rvsleep-frames.h +++ b/components/esp_hw_support/lowpower/port/esp32p4/rvsleep-frames.h @@ -1,5 +1,5 @@ /* - * SPDX-FileCopyrightText: 2023-2024 Espressif Systems (Shanghai) CO LTD + * SPDX-FileCopyrightText: 2023-2026 Espressif Systems (Shanghai) CO LTD * * SPDX-License-Identifier: Apache-2.0 */ @@ -88,6 +88,40 @@ STRUCT_BEGIN STRUCT_FIELD (long, 4, RV_SLP_CTX_MIE, mie) /* Machine intr enable */ STRUCT_FIELD (long, 4, RV_SLP_CTX_MIP, mip) /* Machine intr pending */ + /* FPU context */ + STRUCT_FIELD (long, 4, RV_SLP_CTX_FPU_FT0, fpu_ft0) + STRUCT_FIELD (long, 4, RV_SLP_CTX_FPU_FT1, fpu_ft1) + STRUCT_FIELD (long, 4, RV_SLP_CTX_FPU_FT2, fpu_ft2) + STRUCT_FIELD (long, 4, RV_SLP_CTX_FPU_FT3, fpu_ft3) + STRUCT_FIELD (long, 4, RV_SLP_CTX_FPU_FT4, fpu_ft4) + STRUCT_FIELD (long, 4, RV_SLP_CTX_FPU_FT5, fpu_ft5) + STRUCT_FIELD (long, 4, RV_SLP_CTX_FPU_FT6, fpu_ft6) + STRUCT_FIELD (long, 4, RV_SLP_CTX_FPU_FT7, fpu_ft7) + STRUCT_FIELD (long, 4, RV_SLP_CTX_FPU_FS0, fpu_fs0) + STRUCT_FIELD (long, 4, RV_SLP_CTX_FPU_FS1, fpu_fs1) + STRUCT_FIELD (long, 4, RV_SLP_CTX_FPU_FA0, fpu_fa0) + STRUCT_FIELD (long, 4, RV_SLP_CTX_FPU_FA1, fpu_fa1) + STRUCT_FIELD (long, 4, RV_SLP_CTX_FPU_FA2, fpu_fa2) + STRUCT_FIELD (long, 4, RV_SLP_CTX_FPU_FA3, fpu_fa3) + STRUCT_FIELD (long, 4, RV_SLP_CTX_FPU_FA4, fpu_fa4) + STRUCT_FIELD (long, 4, RV_SLP_CTX_FPU_FA5, fpu_fa5) + STRUCT_FIELD (long, 4, RV_SLP_CTX_FPU_FA6, fpu_fa6) + STRUCT_FIELD (long, 4, RV_SLP_CTX_FPU_FA7, fpu_fa7) + STRUCT_FIELD (long, 4, RV_SLP_CTX_FPU_FS2, fpu_fs2) + STRUCT_FIELD (long, 4, RV_SLP_CTX_FPU_FS3, fpu_fs3) + STRUCT_FIELD (long, 4, RV_SLP_CTX_FPU_FS4, fpu_fs4) + STRUCT_FIELD (long, 4, RV_SLP_CTX_FPU_FS5, fpu_fs5) + STRUCT_FIELD (long, 4, RV_SLP_CTX_FPU_FS6, fpu_fs6) + STRUCT_FIELD (long, 4, RV_SLP_CTX_FPU_FS7, fpu_fs7) + STRUCT_FIELD (long, 4, RV_SLP_CTX_FPU_FS8, fpu_fs8) + STRUCT_FIELD (long, 4, RV_SLP_CTX_FPU_FS9, fpu_fs9) + STRUCT_FIELD (long, 4, RV_SLP_CTX_FPU_FS10, fpu_fs10) + STRUCT_FIELD (long, 4, RV_SLP_CTX_FPU_FS11, fpu_fs11) + STRUCT_FIELD (long, 4, RV_SLP_CTX_FPU_FT8, fpu_ft8) + STRUCT_FIELD (long, 4, RV_SLP_CTX_FPU_FT9, fpu_ft9) + STRUCT_FIELD (long, 4, RV_SLP_CTX_FPU_FT10, fpu_ft10) + STRUCT_FIELD (long, 4, RV_SLP_CTX_FPU_FT11, fpu_ft11) + STRUCT_FIELD (long, 4, RV_SLP_CTX_FPU_FCSR, fpu_fcsr) STRUCT_FIELD (long, 4, RV_SLP_CTX_PMUFUNC, pmufunc) /* A field is used to identify whether it is going * to sleep or has just been awakened. We use the * lowest 2 bits as indication information, 3 means diff --git a/components/esp_hw_support/lowpower/port/esp32p4/sleep_cpu.c b/components/esp_hw_support/lowpower/port/esp32p4/sleep_cpu.c index 99ff4f0329..9db058a2e4 100644 --- a/components/esp_hw_support/lowpower/port/esp32p4/sleep_cpu.c +++ b/components/esp_hw_support/lowpower/port/esp32p4/sleep_cpu.c @@ -1,5 +1,5 @@ /* - * SPDX-FileCopyrightText: 2023-2024 Espressif Systems (Shanghai) CO LTD + * SPDX-FileCopyrightText: 2023-2026 Espressif Systems (Shanghai) CO LTD * * SPDX-License-Identifier: Apache-2.0 */ @@ -19,6 +19,7 @@ #include "freertos/FreeRTOS.h" #include "freertos/task.h" #include "esp_heap_caps.h" +#include "freertos/portmacro.h" #include "riscv/csr.h" #include "soc/clic_reg.h" #include "soc/rtc_periph.h" @@ -54,6 +55,8 @@ typedef enum { static TCM_DRAM_ATTR smp_retention_state_t s_smp_retention_state[portNUM_PROCESSORS]; #endif +static bool s_fpu_saved[portNUM_PROCESSORS]; + static __attribute__((unused)) const char *TAG = "sleep"; typedef struct { @@ -376,17 +379,22 @@ static TCM_IRAM_ATTR void validate_retention_frame_crc(uint32_t *frame_ptr, uint extern RvCoreCriticalSleepFrame * rv_core_critical_regs_save(void); extern RvCoreCriticalSleepFrame * rv_core_critical_regs_restore(void); +extern void rv_core_fpu_save(RvCoreCriticalSleepFrame *frame); +extern void rv_core_fpu_restore(RvCoreCriticalSleepFrame *frame); typedef uint32_t (* sleep_cpu_entry_cb_t)(uint32_t, uint32_t, uint32_t, bool); static TCM_IRAM_ATTR esp_err_t do_cpu_retention(sleep_cpu_entry_cb_t goto_sleep, uint32_t wakeup_opt, uint32_t reject_opt, uint32_t lslp_mem_inf_fpu, bool dslp) { uint8_t core_id = esp_cpu_get_core_id(); + RvCoreCriticalSleepFrame *frame = s_cpu_retention.retent.critical_frame[core_id]; /* mstatus is core privated CSR, do it near the core critical regs restore */ uint32_t mstatus = save_mstatus_and_disable_global_int(); + s_fpu_saved[core_id] = xPortFPUContextIsDirty(core_id); + if (s_fpu_saved[core_id]) { + rv_core_fpu_save(frame); + } rv_core_critical_regs_save(); - - RvCoreCriticalSleepFrame * frame = s_cpu_retention.retent.critical_frame[core_id]; if ((frame->pmufunc & 0x3) == 0x1) { esp_sleep_execute_event_callbacks(SLEEP_EVENT_SW_CPU_TO_MEM_END, (void *)0); #if CONFIG_PM_CHECK_SLEEP_RETENTION_FRAME @@ -413,6 +421,9 @@ static TCM_IRAM_ATTR esp_err_t do_cpu_retention(sleep_cpu_entry_cb_t goto_sleep, validate_retention_frame_crc((uint32_t*)frame, RV_SLEEP_CTX_SZ1 - 2 * sizeof(long), (uint32_t *)(&frame->frame_crc)); } #endif + if (s_fpu_saved[core_id]) { + rv_core_fpu_restore(frame); + } restore_mstatus(mstatus); return pmu_sleep_finish(dslp); } @@ -526,9 +537,13 @@ static TCM_IRAM_ATTR void smp_core_do_retention(void) atomic_store(&s_smp_retention_state[core_id], SMP_BACKUP_START); rv_core_noncritical_regs_save(); cpu_domain_dev_regs_save(s_cpu_retention.retent.clic_frame[core_id]); - uint32_t mstatus = save_mstatus_and_disable_global_int(); - rv_core_critical_regs_save(); RvCoreCriticalSleepFrame *frame_critical = s_cpu_retention.retent.critical_frame[core_id]; + uint32_t mstatus = save_mstatus_and_disable_global_int(); + s_fpu_saved[core_id] = xPortFPUContextIsDirty(core_id); + if (s_fpu_saved[core_id]) { + rv_core_fpu_save(frame_critical); + } + rv_core_critical_regs_save(); if ((frame_critical->pmufunc & 0x3) == 0x1) { atomic_store(&s_smp_retention_state[core_id], SMP_BACKUP_DONE); // wait another core trigger sleep and wakeup @@ -546,6 +561,9 @@ static TCM_IRAM_ATTR void smp_core_do_retention(void) REG_CLR_BIT(HP_SYS_CLKRST_HP_RST_EN0_REG, HP_SYS_CLKRST_REG_RST_EN_CORE1_GLOBAL); } atomic_store(&s_smp_retention_state[core_id], SMP_RESTORE_START); + if (s_fpu_saved[core_id]) { + rv_core_fpu_restore(frame_critical); + } restore_mstatus(mstatus); cpu_domain_dev_regs_restore(s_cpu_retention.retent.clic_frame[core_id]); rv_core_noncritical_regs_restore(); diff --git a/components/esp_hw_support/lowpower/port/esp32p4/sleep_fpu_asm.S b/components/esp_hw_support/lowpower/port/esp32p4/sleep_fpu_asm.S new file mode 100644 index 0000000000..ba6ee1be35 --- /dev/null +++ b/components/esp_hw_support/lowpower/port/esp32p4/sleep_fpu_asm.S @@ -0,0 +1,118 @@ +/* + * SPDX-FileCopyrightText: 2026 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include "rvsleep-frames.h" +#include "freertos/FreeRTOSConfig.h" +#include "sdkconfig.h" + + .section .tcm.text,"ax" + .global rv_core_fpu_save + .type rv_core_fpu_save,@function + .align 4 + +/* Bit to set in mstatus to enable the FPU */ +#define CSR_MSTATUS_FPU_ENABLE (1 << 13) +/* Bit to clear in mstatus to disable the FPU */ +#define CSR_MSTATUS_FPU_DISABLE (3 << 13) + +.macro fpu_enable reg + li \reg, CSR_MSTATUS_FPU_ENABLE + csrs mstatus, \reg +.endm + +rv_core_fpu_save: + fpu_enable t0 + mv t0, a0 /* t0 = frame (arg0) */ + fsw ft0, RV_SLP_CTX_FPU_FT0(t0) + fsw ft1, RV_SLP_CTX_FPU_FT1(t0) + fsw ft2, RV_SLP_CTX_FPU_FT2(t0) + fsw ft3, RV_SLP_CTX_FPU_FT3(t0) + fsw ft4, RV_SLP_CTX_FPU_FT4(t0) + fsw ft5, RV_SLP_CTX_FPU_FT5(t0) + fsw ft6, RV_SLP_CTX_FPU_FT6(t0) + fsw ft7, RV_SLP_CTX_FPU_FT7(t0) + fsw fs0, RV_SLP_CTX_FPU_FS0(t0) + fsw fs1, RV_SLP_CTX_FPU_FS1(t0) + fsw fa0, RV_SLP_CTX_FPU_FA0(t0) + fsw fa1, RV_SLP_CTX_FPU_FA1(t0) + fsw fa2, RV_SLP_CTX_FPU_FA2(t0) + fsw fa3, RV_SLP_CTX_FPU_FA3(t0) + fsw fa4, RV_SLP_CTX_FPU_FA4(t0) + fsw fa5, RV_SLP_CTX_FPU_FA5(t0) + fsw fa6, RV_SLP_CTX_FPU_FA6(t0) + fsw fa7, RV_SLP_CTX_FPU_FA7(t0) + fsw fs2, RV_SLP_CTX_FPU_FS2(t0) + fsw fs3, RV_SLP_CTX_FPU_FS3(t0) + fsw fs4, RV_SLP_CTX_FPU_FS4(t0) + fsw fs5, RV_SLP_CTX_FPU_FS5(t0) + fsw fs6, RV_SLP_CTX_FPU_FS6(t0) + fsw fs7, RV_SLP_CTX_FPU_FS7(t0) + fsw fs8, RV_SLP_CTX_FPU_FS8(t0) + fsw fs9, RV_SLP_CTX_FPU_FS9(t0) + fsw fs10, RV_SLP_CTX_FPU_FS10(t0) + fsw fs11, RV_SLP_CTX_FPU_FS11(t0) + fsw ft8, RV_SLP_CTX_FPU_FT8(t0) + fsw ft9, RV_SLP_CTX_FPU_FT9(t0) + fsw ft10, RV_SLP_CTX_FPU_FT10(t0) + fsw ft11, RV_SLP_CTX_FPU_FT11(t0) + csrr t1, fcsr + sw t1, RV_SLP_CTX_FPU_FCSR(t0) + /* Chip will go to sleep and FPU will be powered down, not necessary to disable FPU here. */ + ret + + .size rv_core_fpu_save, . - rv_core_fpu_save + +/* +-------------------------------------------------------------------------------- + FPU restore: a0 = RvCoreCriticalSleepFrame *. Restore all FP registers from the frame. +-------------------------------------------------------------------------------- +*/ + .section .iram1,"ax" + .global rv_core_fpu_restore + .type rv_core_fpu_restore,@function + .align 4 + +rv_core_fpu_restore: + fpu_enable t0 + mv t0, a0 /* t0 = frame (arg0) */ + flw ft0, RV_SLP_CTX_FPU_FT0(t0) + flw ft1, RV_SLP_CTX_FPU_FT1(t0) + flw ft2, RV_SLP_CTX_FPU_FT2(t0) + flw ft3, RV_SLP_CTX_FPU_FT3(t0) + flw ft4, RV_SLP_CTX_FPU_FT4(t0) + flw ft5, RV_SLP_CTX_FPU_FT5(t0) + flw ft6, RV_SLP_CTX_FPU_FT6(t0) + flw ft7, RV_SLP_CTX_FPU_FT7(t0) + flw fs0, RV_SLP_CTX_FPU_FS0(t0) + flw fs1, RV_SLP_CTX_FPU_FS1(t0) + flw fa0, RV_SLP_CTX_FPU_FA0(t0) + flw fa1, RV_SLP_CTX_FPU_FA1(t0) + flw fa2, RV_SLP_CTX_FPU_FA2(t0) + flw fa3, RV_SLP_CTX_FPU_FA3(t0) + flw fa4, RV_SLP_CTX_FPU_FA4(t0) + flw fa5, RV_SLP_CTX_FPU_FA5(t0) + flw fa6, RV_SLP_CTX_FPU_FA6(t0) + flw fa7, RV_SLP_CTX_FPU_FA7(t0) + flw fs2, RV_SLP_CTX_FPU_FS2(t0) + flw fs3, RV_SLP_CTX_FPU_FS3(t0) + flw fs4, RV_SLP_CTX_FPU_FS4(t0) + flw fs5, RV_SLP_CTX_FPU_FS5(t0) + flw fs6, RV_SLP_CTX_FPU_FS6(t0) + flw fs7, RV_SLP_CTX_FPU_FS7(t0) + flw fs8, RV_SLP_CTX_FPU_FS8(t0) + flw fs9, RV_SLP_CTX_FPU_FS9(t0) + flw fs10, RV_SLP_CTX_FPU_FS10(t0) + flw fs11, RV_SLP_CTX_FPU_FS11(t0) + flw ft8, RV_SLP_CTX_FPU_FT8(t0) + flw ft9, RV_SLP_CTX_FPU_FT9(t0) + flw ft10, RV_SLP_CTX_FPU_FT10(t0) + flw ft11, RV_SLP_CTX_FPU_FT11(t0) + lw t1, RV_SLP_CTX_FPU_FCSR(t0) + csrw fcsr, t1 + /* Caller restores mstatus (and thus FPU state); not necessary to disable FPU here. */ + ret + + .size rv_core_fpu_restore, . - rv_core_fpu_restore diff --git a/components/freertos/FreeRTOS-Kernel/portable/riscv/include/freertos/portmacro.h b/components/freertos/FreeRTOS-Kernel/portable/riscv/include/freertos/portmacro.h index f1b0257d54..c5f2dd0b72 100644 --- a/components/freertos/FreeRTOS-Kernel/portable/riscv/include/freertos/portmacro.h +++ b/components/freertos/FreeRTOS-Kernel/portable/riscv/include/freertos/portmacro.h @@ -6,7 +6,7 @@ * * SPDX-License-Identifier: MIT * - * SPDX-FileContributor: 2023-2024 Espressif Systems (Shanghai) CO LTD + * SPDX-FileContributor: 2023-2026 Espressif Systems (Shanghai) CO LTD * * Permission is hereby granted, free of charge, to any person obtaining a copy of * this software and associated documentation files (the "Software"), to deal in @@ -758,6 +758,21 @@ extern volatile UBaseType_t xPortSwitchFlag[portNUM_PROCESSORS]; #define os_task_switch_is_pended(_cpu_) (false) #endif +// -------------- FPU softerware retention ------------------ +#if (SOC_CPU_COPROC_NUM > 0) && SOC_CPU_HAS_FPU && SOC_PM_FPU_RETENTION_BY_SW +/** + * @brief Whether the FPU context is dirty on the given core. + * + * Returns non-zero if any task has used the FPU, such context should be + * saved during sleep retention. + * + * @param core_id Core id + * @return pdTRUE FPU context is dirty + * @return pdFALSE FPU was not used + */ +BaseType_t xPortFPUContextIsDirty(BaseType_t core_id); +#endif + #ifdef __cplusplus } #endif diff --git a/components/freertos/FreeRTOS-Kernel/portable/riscv/port.c b/components/freertos/FreeRTOS-Kernel/portable/riscv/port.c index f5302a205f..30e1ca0911 100644 --- a/components/freertos/FreeRTOS-Kernel/portable/riscv/port.c +++ b/components/freertos/FreeRTOS-Kernel/portable/riscv/port.c @@ -6,7 +6,7 @@ * * SPDX-License-Identifier: MIT * - * SPDX-FileContributor: 2023-2025 Espressif Systems (Shanghai) CO LTD + * SPDX-FileContributor: 2023-2026 Espressif Systems (Shanghai) CO LTD * * Permission is hereby granted, free of charge, to any person obtaining a copy of * this software and associated documentation files (the "Software"), to deal in @@ -886,3 +886,9 @@ void vPortCoprocUsedInISR(void* frame) /* ---------------------------------------------- Misc Implementations ------------------------------------------------- * * ------------------------------------------------------------------------------------------------------------------ */ +#if (SOC_CPU_COPROC_NUM > 0) && SOC_CPU_HAS_FPU && SOC_PM_FPU_RETENTION_BY_SW +BaseType_t xPortFPUContextIsDirty(BaseType_t core_id) +{ + return (BaseType_t)(port_uxCoprocOwner[core_id][FPU_COPROC_IDX] != NULL); +} +#endif /* (SOC_CPU_COPROC_NUM > 0) && SOC_CPU_HAS_FPU && SOC_PM_FPU_RETENTION_BY_SW */ diff --git a/components/freertos/linker.lf b/components/freertos/linker.lf index 22c547d8e6..16d83a6066 100644 --- a/components/freertos/linker.lf +++ b/components/freertos/linker.lf @@ -374,3 +374,5 @@ entries: port:xPortInterruptedFromISRContext (default) port:vPortYieldFromISR (default) port:vPortYieldOtherCore (default) + if (SOC_CPU_COPROC_NUM > 0) && SOC_CPU_HAS_FPU && SOC_PM_FPU_RETENTION_BY_SW: + port:xPortFPUContextIsDirty (noflash_text) diff --git a/components/soc/esp32p4/include/soc/Kconfig.soc_caps.in b/components/soc/esp32p4/include/soc/Kconfig.soc_caps.in index 03f4ea92e4..397bcede3d 100644 --- a/components/soc/esp32p4/include/soc/Kconfig.soc_caps.in +++ b/components/soc/esp32p4/include/soc/Kconfig.soc_caps.in @@ -2127,6 +2127,10 @@ config SOC_PM_CPU_RETENTION_BY_SW bool default y +config SOC_PM_FPU_RETENTION_BY_SW + bool + default y + config SOC_PM_CACHE_RETENTION_BY_PAU bool default y diff --git a/components/soc/esp32p4/include/soc/soc_caps.h b/components/soc/esp32p4/include/soc/soc_caps.h index 24316d58ed..23c893fe04 100644 --- a/components/soc/esp32p4/include/soc/soc_caps.h +++ b/components/soc/esp32p4/include/soc/soc_caps.h @@ -777,6 +777,7 @@ #define SOC_PM_SUPPORT_DEEPSLEEP_CHECK_STUB_ONLY (1) /*!