Merge branch 'bugfix/deep_sleep_stub_heap_rtc_fast_mem_v4.2' into 'release/v4.2'

deep sleep: Calculate RTC CRC without using any stack or other RTC heap memory (v4.2)

See merge request espressif/esp-idf!10883

components/esp32/sleep_modes.c

@@ -84,8 +84,8 @@ static sleep_config_t s_config = {
 static bool s_light_sleep_wakeup = false;
 
 /* Updating RTC_MEMORY_CRC_REG register via set_rtc_memory_crc()
-   is not thread-safe. */
+   is not thread-safe, so we need to disable interrupts before going to deep sleep. */
-static _lock_t lock_rtc_memory_crc;
+static portMUX_TYPE spinlock_rtc_deep_sleep = portMUX_INITIALIZER_UNLOCKED;
 
 static const char* TAG = "sleep";
 
@@ -99,16 +99,6 @@ static void timer_wakeup_prepare(void);
 */
 esp_deep_sleep_wake_stub_fn_t esp_get_deep_sleep_wake_stub(void)
 {
-    _lock_acquire(&lock_rtc_memory_crc);
-    uint32_t stored_crc = REG_READ(RTC_MEMORY_CRC_REG);
-    set_rtc_memory_crc();
-    uint32_t calc_crc = REG_READ(RTC_MEMORY_CRC_REG);
-    REG_WRITE(RTC_MEMORY_CRC_REG, stored_crc);
-    _lock_release(&lock_rtc_memory_crc);
-
-    if(stored_crc != calc_crc) {
-        return NULL;
-    }
     esp_deep_sleep_wake_stub_fn_t stub_ptr = (esp_deep_sleep_wake_stub_fn_t) REG_READ(RTC_ENTRY_ADDR_REG);
     if (!esp_ptr_executable(stub_ptr)) {
         return NULL;
@@ -118,10 +108,7 @@ esp_deep_sleep_wake_stub_fn_t esp_get_deep_sleep_wake_stub(void)
 
 void esp_set_deep_sleep_wake_stub(esp_deep_sleep_wake_stub_fn_t new_stub)
 {
-    _lock_acquire(&lock_rtc_memory_crc);
     REG_WRITE(RTC_ENTRY_ADDR_REG, (uint32_t)new_stub);
-    set_rtc_memory_crc();
-    _lock_release(&lock_rtc_memory_crc);
 }
 
 void RTC_IRAM_ATTR esp_default_wake_deep_sleep(void) {
@@ -174,12 +161,16 @@ static void IRAM_ATTR resume_uarts(void)
     }
 }
 
+inline static uint32_t IRAM_ATTR call_rtc_sleep_start(uint32_t reject_triggers);
+
 static uint32_t IRAM_ATTR esp_sleep_start(uint32_t pd_flags)
 {
     // Stop UART output so that output is not lost due to APB frequency change.
     // For light sleep, suspend UART output — it will resume after wakeup.
     // For deep sleep, wait for the contents of UART FIFO to be sent.
-    if (pd_flags & RTC_SLEEP_PD_DIG) {
+    bool deep_sleep = pd_flags & RTC_SLEEP_PD_DIG;
+
+    if (deep_sleep) {
         flush_uarts();
     } else {
         suspend_uarts();
@@ -211,7 +202,27 @@ static uint32_t IRAM_ATTR esp_sleep_start(uint32_t pd_flags)
         s_config.sleep_duration > 0) {
         timer_wakeup_prepare();
     }
-    uint32_t result = rtc_sleep_start(s_config.wakeup_triggers, 0);
+
+    uint32_t result;
+    if (deep_sleep) {
+        /* Disable interrupts in case another task writes to RTC memory while we
+         * calculate RTC memory CRC
+         */
+        portENTER_CRITICAL(&spinlock_rtc_deep_sleep);
+
+#if !CONFIG_ESP32_ALLOW_RTC_FAST_MEM_AS_HEAP
+        /* If not possible stack is in RTC FAST memory, use the ROM function to calculate the CRC and save ~140 bytes IRAM */
+        set_rtc_memory_crc();
+        result = call_rtc_sleep_start(0);
+#else
+        /* Otherwise, need to call the dedicated soc function for this */
+        result = rtc_deep_sleep_start(s_config.wakeup_triggers, 0);
+#endif
+
+        portEXIT_CRITICAL(&spinlock_rtc_deep_sleep);
+    } else {
+        result = call_rtc_sleep_start(0);
+    }
 
     // Restore CPU frequency
     rtc_clk_cpu_freq_set_config(&cpu_freq_config);
@@ -222,6 +233,15 @@ static uint32_t IRAM_ATTR esp_sleep_start(uint32_t pd_flags)
     return result;
 }
 
+inline static uint32_t IRAM_ATTR call_rtc_sleep_start(uint32_t reject_triggers)
+{
+#ifdef CONFIG_IDF_TARGET_ESP32
+        return rtc_sleep_start(s_config.wakeup_triggers, reject_triggers);
+#else
+        return rtc_sleep_start(s_config.wakeup_triggers, reject_triggers, 1);
+#endif
+}
+
 void IRAM_ATTR esp_deep_sleep_start(void)
 {
     // record current RTC time

components/esp32/test/test_sleep.c

@@ -274,6 +274,67 @@ TEST_CASE_MULTIPLE_STAGES("can set sleep wake stub", "[deepsleep][reset=DEEPSLEE
         check_wake_stub);
 
 
+#if CONFIG_ESP32_ALLOW_RTC_FAST_MEM_AS_HEAP || CONFIG_ESP32S2_ALLOW_RTC_FAST_MEM_AS_HEAP \
+    || CONFIG_ESP32S3_ALLOW_RTC_FAST_MEM_AS_HEAP
+#if CONFIG_FREERTOS_SUPPORT_STATIC_ALLOCATION
+
+/* Version of prepare_wake_stub() that sets up the deep sleep call while running
+   from RTC memory as stack, with a high frequency timer also writing RTC FAST
+   memory.
+
+   This is important because the ROM code (ESP32 & ESP32-S2) requires software
+   trigger a CRC calculation (done in hardware) for the entire RTC FAST memory
+   before going to deep sleep and if it's invalid then the stub is not
+   run. Also, while the CRC is being calculated the RTC FAST memory is not
+   accesible by the CPU (reads all zeros).
+*/
+
+static void increment_rtc_memory_cb(void *arg)
+{
+    static volatile RTC_FAST_ATTR unsigned counter;
+    counter++;
+}
+
+static void prepare_wake_stub_from_rtc(void)
+{
+    /* RTC memory can be used as heap, however there is no API call that returns this as
+       a memory capability (as it's an implementation detail). So to test this we need to allocate
+       the stack statically.
+    */
+    static RTC_FAST_ATTR uint8_t sleep_stack[1024];
+    static RTC_FAST_ATTR StaticTask_t sleep_task;
+
+    /* normally BSS like sleep_stack will be cleared on reset, but RTC memory is not cleared on
+     * wake from deep sleep. So to ensure unused stack is different if test is re-run without a full reset,
+     * fill with some random bytes
+     */
+    esp_fill_random(sleep_stack, sizeof(sleep_stack));
+
+    /* to make things extra sure, start a periodic timer to write to RTC FAST RAM at high frequency */
+    const esp_timer_create_args_t timer_args = {
+                                          .callback = increment_rtc_memory_cb,
+                                          .arg = NULL,
+                                          .dispatch_method = ESP_TIMER_TASK,
+                                          .name = "Write RTC MEM"
+    };
+    esp_timer_handle_t timer;
+    ESP_ERROR_CHECK( esp_timer_create(&timer_args, &timer) );
+    ESP_ERROR_CHECK( esp_timer_start_periodic(timer, 200) );
+
+    printf("Creating test task with stack %p\n", sleep_stack);
+    TEST_ASSERT_NOT_NULL(xTaskCreateStatic( (void *)prepare_wake_stub, "sleep", sizeof(sleep_stack), NULL,
+                                            UNITY_FREERTOS_PRIORITY, sleep_stack, &sleep_task));
+    vTaskDelay(1000 / portTICK_PERIOD_MS);
+    TEST_FAIL_MESSAGE("Should be asleep by now");
+}
+
+TEST_CASE_MULTIPLE_STAGES("can set sleep wake stub from stack in RTC RAM", "[deepsleep][reset=DEEPSLEEP_RESET]",
+        prepare_wake_stub_from_rtc,
+        check_wake_stub);
+
+#endif // CONFIG_FREERTOS_SUPPORT_STATIC_ALLOCATION
+#endif // CONFIG_xyz_ALLOW_RTC_FAST_MEM_AS_HEAP
+
 TEST_CASE("wake up using ext0 (13 high)", "[deepsleep][ignore]")
 {
     ESP_ERROR_CHECK(rtc_gpio_init(GPIO_NUM_13));

components/esp32s2/sleep_modes.c

@@ -88,8 +88,8 @@ static sleep_config_t s_config = {
 static bool s_light_sleep_wakeup = false;
 
 /* Updating RTC_MEMORY_CRC_REG register via set_rtc_memory_crc()
-   is not thread-safe. */
+   is not thread-safe, so we need to disable interrupts before going to deep sleep. */
-static _lock_t lock_rtc_memory_crc;
+static portMUX_TYPE spinlock_rtc_deep_sleep = portMUX_INITIALIZER_UNLOCKED;
 
 static const char* TAG = "sleep";
 
@@ -104,16 +104,6 @@ static void touch_wakeup_prepare(void);
 */
 esp_deep_sleep_wake_stub_fn_t esp_get_deep_sleep_wake_stub(void)
 {
-    _lock_acquire(&lock_rtc_memory_crc);
-    uint32_t stored_crc = REG_READ(RTC_MEMORY_CRC_REG);
-    set_rtc_memory_crc();
-    uint32_t calc_crc = REG_READ(RTC_MEMORY_CRC_REG);
-    REG_WRITE(RTC_MEMORY_CRC_REG, stored_crc);
-    _lock_release(&lock_rtc_memory_crc);
-
-    if (stored_crc != calc_crc) {
-        return NULL;
-    }
     esp_deep_sleep_wake_stub_fn_t stub_ptr = (esp_deep_sleep_wake_stub_fn_t) REG_READ(RTC_ENTRY_ADDR_REG);
     if (!esp_ptr_executable(stub_ptr)) {
         return NULL;
@@ -123,10 +113,7 @@ esp_deep_sleep_wake_stub_fn_t esp_get_deep_sleep_wake_stub(void)
 
 void esp_set_deep_sleep_wake_stub(esp_deep_sleep_wake_stub_fn_t new_stub)
 {
-    _lock_acquire(&lock_rtc_memory_crc);
     REG_WRITE(RTC_ENTRY_ADDR_REG, (uint32_t)new_stub);
-    set_rtc_memory_crc();
-    _lock_release(&lock_rtc_memory_crc);
 }
 
 void RTC_IRAM_ATTR esp_default_wake_deep_sleep(void) {
@@ -175,12 +162,16 @@ static void IRAM_ATTR resume_uarts(void)
     }
 }
 
+inline static uint32_t IRAM_ATTR call_rtc_sleep_start(uint32_t reject_triggers);
+
 static uint32_t IRAM_ATTR esp_sleep_start(uint32_t pd_flags)
 {
     // Stop UART output so that output is not lost due to APB frequency change.
     // For light sleep, suspend UART output — it will resume after wakeup.
     // For deep sleep, wait for the contents of UART FIFO to be sent.
-    if (pd_flags & RTC_SLEEP_PD_DIG) {
+    bool deep_sleep = pd_flags & RTC_SLEEP_PD_DIG;
+
+    if (deep_sleep) {
         flush_uarts();
     } else {
         suspend_uarts();
@@ -217,7 +208,27 @@ static uint32_t IRAM_ATTR esp_sleep_start(uint32_t pd_flags)
         s_config.sleep_duration > 0) {
         timer_wakeup_prepare();
     }
-    uint32_t result = rtc_sleep_start(s_config.wakeup_triggers, 0, 1);
+
+    uint32_t result;
+    if (deep_sleep) {
+        /* Disable interrupts in case another task writes to RTC memory while we
+         * calculate RTC memory CRC
+         */
+        portENTER_CRITICAL(&spinlock_rtc_deep_sleep);
+
+#if !CONFIG_ESP32S2_ALLOW_RTC_FAST_MEM_AS_HEAP
+        /* If not possible stack is in RTC FAST memory, use the ROM function to calculate the CRC and save ~140 bytes IRAM */
+        set_rtc_memory_crc();
+        result = call_rtc_sleep_start(0);
+#else
+        /* Otherwise, need to call the dedicated soc function for this */
+        result = rtc_deep_sleep_start(s_config.wakeup_triggers, 0);
+#endif
+
+        portEXIT_CRITICAL(&spinlock_rtc_deep_sleep);
+    } else {
+        result = call_rtc_sleep_start(0);
+    }
 
     // Restore CPU frequency
     rtc_clk_cpu_freq_set_config(&cpu_freq_config);
@@ -228,6 +239,15 @@ static uint32_t IRAM_ATTR esp_sleep_start(uint32_t pd_flags)
     return result;
 }
 
+inline static uint32_t IRAM_ATTR call_rtc_sleep_start(uint32_t reject_triggers)
+{
+#ifdef CONFIG_IDF_TARGET_ESP32
+        return rtc_sleep_start(s_config.wakeup_triggers, reject_triggers);
+#else
+        return rtc_sleep_start(s_config.wakeup_triggers, reject_triggers, 1);
+#endif
+}
+
 void IRAM_ATTR esp_deep_sleep_start(void)
 {
     /* Due to hardware limitations, on S2 the brownout detector sometimes trigger during deep sleep

components/soc/soc/esp32/include/soc/rtc.h

@@ -579,6 +579,29 @@ void rtc_sleep_set_wakeup_time(uint64_t t);
  */
 uint32_t rtc_sleep_start(uint32_t wakeup_opt, uint32_t reject_opt);
 
+/**
+ * @brief Enter deep sleep mode
+ *
+ * Similar to rtc_sleep_start(), but additionally uses hardware to calculate the CRC value
+ * of RTC FAST memory. On wake, this CRC is used to determine if a deep sleep wake
+ * stub is valid to execute (if a wake address is set).
+ *
+ * No RAM is accessed while calculating the CRC and going into deep sleep, which makes
+ * this function safe to use even if the caller's stack is in RTC FAST memory.
+ *
+ * @note If no deep sleep wake stub address is set then calling rtc_sleep_start() will
+ * have the same effect and takes less time as CRC calculation is skipped.
+ *
+ * @note This function should only be called after rtc_sleep_init() has been called to
+ * configure the system for deep sleep.
+ *
+ * @param wakeup_opt - same as for rtc_sleep_start
+ * @param reject_opt - same as for rtc_sleep_start
+ *
+ * @return non-zero if sleep was rejected by hardware
+ */
+uint32_t rtc_deep_sleep_start(uint32_t wakeup_opt, uint32_t reject_opt);
+
 /**
  * RTC power and clock control initialization settings
  */

components/soc/soc/esp32/include/soc/rtc_cntl_reg.h

@@ -2044,23 +2044,23 @@
 #define RTC_MEM_CRC_FINISH      (BIT(31))
 #define RTC_MEM_CRC_FINISH_M    (BIT(31))
 #define RTC_MEM_CRC_FINISH_V    0x1
-#define RTC_MEM_CRC_FINISH_S    (31)
+#define RTC_MEM_CRC_FINISH_S    31
 #define RTC_MEM_CRC_LEN         (0x7ff)
 #define RTC_MEM_CRC_LEN_M       ((RTC_MEM_CRC_LEN_V)<<(RTC_MEM_CRC_LEN_S))
-#define RTC_MEM_CRC_LEN_V       (0x7ff)
+#define RTC_MEM_CRC_LEN_V       0x7ff
-#define RTC_MEM_CRC_LEN_S       (20)
+#define RTC_MEM_CRC_LEN_S       20
-#define RTC_MEM_CRC_ADDR        (0x7ff)
+#define RTC_MEM_CRC_ADDR        0x7ff
 #define RTC_MEM_CRC_ADDR_M      ((RTC_MEM_CRC_ADDR_V)<<(RTC_MEM_CRC_ADDR_S))
-#define RTC_MEM_CRC_ADDR_V      (0x7ff)
+#define RTC_MEM_CRC_ADDR_V      0x7ff
-#define RTC_MEM_CRC_ADDR_S      (9)
+#define RTC_MEM_CRC_ADDR_S      9
 #define RTC_MEM_CRC_START       (BIT(8))
 #define RTC_MEM_CRC_START_M     (BIT(8))
 #define RTC_MEM_CRC_START_V     0x1
-#define RTC_MEM_CRC_START_S     (8)
+#define RTC_MEM_CRC_START_S     8
-#define RTC_MEM_PID_CONF        (0xff)
+#define RTC_MEM_PID_CONF        0xff
-#define RTC_MEM_PID_CONF_M      (0xff)
+#define RTC_MEM_PID_CONF_M      0xff
-#define RTC_MEM_PID_CONF_V      (0xff)
+#define RTC_MEM_PID_CONF_V      0xff
-#define RTC_MEM_PID_CONF_S      (0)
+#define RTC_MEM_PID_CONF_S      0
 
 #define RTC_MEM_CRC_RES         (DR_REG_RTCCNTL_BASE + 0x41 * 4)
 

components/soc/soc/esp32s2/include/soc/rtc.h

@@ -734,10 +734,36 @@ void rtc_sleep_set_wakeup_time(uint64_t t);
  *                      - RTC_CNTL_SDIO_REJECT_EN
  *                    These flags are used to prevent entering sleep when e.g.
  *                    an external host is communicating via SDIO slave
+ * @param lslp_mem_inf_fpu If non-zero then the low power config is restored
+ *                         immediately on wake. Recommended for light sleep,
+ *                         has no effect if the system goes into deep sleep.
  * @return non-zero if sleep was rejected by hardware
  */
 uint32_t rtc_sleep_start(uint32_t wakeup_opt, uint32_t reject_opt, uint32_t lslp_mem_inf_fpu);
 
+/**
+ * @brief Enter deep sleep mode
+ *
+ * Similar to rtc_sleep_start(), but additionally uses hardware to calculate the CRC value
+ * of RTC FAST memory. On wake, this CRC is used to determine if a deep sleep wake
+ * stub is valid to execute (if a wake address is set).
+ *
+ * No RAM is accessed while calculating the CRC and going into deep sleep, which makes
+ * this function safe to use even if the caller's stack is in RTC FAST memory.
+ *
+ * @note If no deep sleep wake stub address is set then calling rtc_sleep_start() will
+ * have the same effect and takes less time as CRC calculation is skipped.
+ *
+ * @note This function should only be called after rtc_sleep_init() has been called to
+ * configure the system for deep sleep.
+ *
+ * @param wakeup_opt - same as for rtc_sleep_start
+ * @param reject_opt - same as for rtc_sleep_start
+ *
+ * @return non-zero if sleep was rejected by hardware
+ */
+uint32_t rtc_deep_sleep_start(uint32_t wakeup_opt, uint32_t reject_opt);
+
 /**
  * RTC power and clock control initialization settings
  */

components/soc/src/esp32/rtc_sleep.c

@@ -24,6 +24,7 @@
 #include "soc/fe_reg.h"
 #include "soc/rtc.h"
 #include "esp32/rom/ets_sys.h"
+#include "esp32/rom/rtc.h"
 
 #define MHZ (1000000)
 
@@ -218,6 +219,9 @@ void rtc_sleep_set_wakeup_time(uint64_t t)
     WRITE_PERI_REG(RTC_CNTL_SLP_TIMER1_REG, t >> 32);
 }
 
+/* Read back 'reject' status when waking from light or deep sleep */
+static uint32_t rtc_sleep_finish(void);
+
 uint32_t rtc_sleep_start(uint32_t wakeup_opt, uint32_t reject_opt)
 {
     REG_SET_FIELD(RTC_CNTL_WAKEUP_STATE_REG, RTC_CNTL_WAKEUP_ENA, wakeup_opt);
@@ -227,9 +231,92 @@ uint32_t rtc_sleep_start(uint32_t wakeup_opt, uint32_t reject_opt)
     SET_PERI_REG_MASK(RTC_CNTL_STATE0_REG, RTC_CNTL_SLEEP_EN);
 
     while (GET_PERI_REG_MASK(RTC_CNTL_INT_RAW_REG,
-            RTC_CNTL_SLP_REJECT_INT_RAW | RTC_CNTL_SLP_WAKEUP_INT_RAW) == 0) {
+                             RTC_CNTL_SLP_REJECT_INT_RAW | RTC_CNTL_SLP_WAKEUP_INT_RAW) == 0) {
         ;
     }
+
+    return rtc_sleep_finish();
+}
+
+#define STR2(X) #X
+#define STR(X) STR2(X)
+
+uint32_t rtc_deep_sleep_start(uint32_t wakeup_opt, uint32_t reject_opt)
+{
+    REG_SET_FIELD(RTC_CNTL_WAKEUP_STATE_REG, RTC_CNTL_WAKEUP_ENA, wakeup_opt);
+    WRITE_PERI_REG(RTC_CNTL_SLP_REJECT_CONF_REG, reject_opt);
+
+    /* Calculate RTC Fast Memory CRC (for wake stub) & go to deep sleep
+
+       Because we may be running from RTC memory as stack, we can't easily call any
+       functions to do this (as registers may spill to stack, corrupting the CRC).
+
+       Instead, load all the values we need into registers (triggering any stack spills)
+       then use register ops only to calculate the CRC value, write it to the RTC CRC value
+       register, and immediately go into deep sleep.
+     */
+
+    /* Values used to set the RTC_MEM_CONFG value */
+    const unsigned CRC_START_ADDR = 0;
+    const unsigned CRC_LEN = 0x7ff;
+    const unsigned RTC_MEM_PID = 1;
+
+    asm volatile(
+                 "movi a2, 0\n" // trigger a stack spill on working register if needed
+
+                 /* Start CRC calculation */
+                 "s32i %1, %0, 0\n" // set RTC_MEM_CRC_ADDR & RTC_MEM_CRC_LEN
+                 "or a2, %1, %2\n"
+                 "s32i a2, %0, 0\n" // set RTC_MEM_CRC_START
+
+                 /* Wait for the CRC calculation to finish */
+                 ".Lwaitcrc:\n"
+                 "memw\n"
+                 "l32i a2, %0, 0\n"
+                 "bbci a2, "STR(RTC_MEM_CRC_FINISH_S)", .Lwaitcrc\n"
+                 "and a2, a2, %3\n" // clear RTC_MEM_CRC_START
+                 "s32i a2, %0, 0\n"
+                 "memw\n"
+
+                 /* Store the calculated value in RTC_MEM_CRC_REG */
+                 "l32i a2, %4, 0\n"
+                 "s32i a2, %5, 0\n"
+                 "memw\n"
+
+                 /* Set register bit to go into deep sleep */
+                 "l32i a2, %6, 0\n"
+                 "or   a2, a2, %7\n"
+                 "s32i a2, %6, 0\n"
+                 "memw\n"
+
+                 /* Set wait cycle for touch or COCPU after deep sleep. */
+                 ".Lwaitsleep:"
+                 "memw\n"
+                 "l32i a2, %8, 0\n"
+                 "and a2, a2, %9\n"
+                 "beqz a2, .Lwaitsleep\n"
+
+                 :
+                 : "r" (RTC_MEM_CONF), // %0
+                   "r" ( (CRC_START_ADDR << RTC_MEM_CRC_ADDR_S)
+                         | (CRC_LEN << RTC_MEM_CRC_LEN_S)
+                         | (RTC_MEM_PID << RTC_MEM_PID_CONF_S) ), // %1
+                   "r" (RTC_MEM_CRC_START), // %2
+                   "r" (~RTC_MEM_CRC_START), // %3
+                   "r" (RTC_MEM_CRC_RES), // %4
+                   "r" (RTC_MEMORY_CRC_REG), // %5
+                   "r" (RTC_CNTL_STATE0_REG), // %6
+                   "r" (RTC_CNTL_SLEEP_EN), // %7
+                   "r" (RTC_CNTL_INT_RAW_REG), // %8
+                   "r" (RTC_CNTL_SLP_REJECT_INT_RAW | RTC_CNTL_SLP_WAKEUP_INT_RAW) // %9
+                 : "a2" // working register
+                 );
+
+    return rtc_sleep_finish();
+}
+
+static uint32_t rtc_sleep_finish(void)
+{
     /* In deep sleep mode, we never get here */
     uint32_t reject = REG_GET_FIELD(RTC_CNTL_INT_RAW_REG, RTC_CNTL_SLP_REJECT_INT_RAW);
     SET_PERI_REG_MASK(RTC_CNTL_INT_CLR_REG,

components/soc/src/esp32s2/rtc_sleep.c

@@ -26,6 +26,7 @@
 #include "soc/fe_reg.h"
 #include "soc/rtc.h"
 #include "esp32s2/rom/ets_sys.h"
+#include "esp32s2/rom/rtc.h"
 
 /**
  * Configure whether certain peripherals are powered down in deep sleep
@@ -131,11 +132,19 @@ void rtc_sleep_set_wakeup_time(uint64_t t)
     WRITE_PERI_REG(RTC_CNTL_SLP_TIMER1_REG, t >> 32);
 }
 
+/* Read back 'reject' status when waking from light or deep sleep */
+static uint32_t rtc_sleep_finish(uint32_t lslp_mem_inf_fpu);
+
+static const unsigned DEEP_SLEEP_TOUCH_WAIT_CYCLE = 0xFF;
+
 uint32_t rtc_sleep_start(uint32_t wakeup_opt, uint32_t reject_opt, uint32_t lslp_mem_inf_fpu)
 {
     REG_SET_FIELD(RTC_CNTL_WAKEUP_STATE_REG, RTC_CNTL_WAKEUP_ENA, wakeup_opt);
     REG_SET_FIELD(RTC_CNTL_SLP_REJECT_CONF_REG, RTC_CNTL_SLEEP_REJECT_ENA, reject_opt);
 
+    /* Set wait cycle for touch or COCPU after deep sleep. */
+    REG_SET_FIELD(RTC_CNTL_TIMER2_REG, RTC_CNTL_ULPCP_TOUCH_START_WAIT, DEEP_SLEEP_TOUCH_WAIT_CYCLE);
+
     /* Start entry into sleep mode */
     SET_PERI_REG_MASK(RTC_CNTL_STATE0_REG, RTC_CNTL_SLEEP_EN);
 
@@ -143,6 +152,98 @@ uint32_t rtc_sleep_start(uint32_t wakeup_opt, uint32_t reject_opt, uint32_t lslp
                              RTC_CNTL_SLP_REJECT_INT_RAW | RTC_CNTL_SLP_WAKEUP_INT_RAW) == 0) {
         ;
     }
+
+    return rtc_sleep_finish(lslp_mem_inf_fpu);
+}
+
+#define STR2(X) #X
+#define STR(X) STR2(X)
+
+uint32_t rtc_deep_sleep_start(uint32_t wakeup_opt, uint32_t reject_opt)
+{
+    REG_SET_FIELD(RTC_CNTL_WAKEUP_STATE_REG, RTC_CNTL_WAKEUP_ENA, wakeup_opt);
+    WRITE_PERI_REG(RTC_CNTL_SLP_REJECT_CONF_REG, reject_opt);
+
+    /* Calculate RTC Fast Memory CRC (for wake stub) & go to deep sleep
+
+       Because we may be running from RTC memory as stack, we can't easily call any
+       functions to do this (as registers may spill to stack, corrupting the CRC).
+
+       Instead, load all the values we need into registers (triggering any stack spills)
+       then use register ops only to calculate the CRC value, write it to the RTC CRC value
+       register, and immediately go into deep sleep.
+     */
+
+    /* Values used to set the DPORT_RTC_FASTMEM_CONFIG_REG value */
+    const unsigned CRC_START_ADDR = 0;
+    const unsigned CRC_LEN = 0x7ff;
+
+    asm volatile(
+                 "movi a2, 0\n" // trigger a stack spill on working register if needed
+
+                 /* Start CRC calculation */
+                 "s32i %1, %0, 0\n" // set RTC_MEM_CRC_ADDR & RTC_MEM_CRC_LEN
+                 "or a2, %1, %2\n"
+                 "s32i a2, %0, 0\n" // set RTC_MEM_CRC_START
+
+                 /* Wait for the CRC calculation to finish */
+                 ".Lwaitcrc:\n"
+                 "memw\n"
+                 "l32i a2, %0, 0\n"
+                 "bbci a2, "STR(DPORT_RTC_MEM_CRC_FINISH_S)", .Lwaitcrc\n"
+                 "xor %2, %2, %2\n" // %2 -> ~DPORT_RTC_MEM_CRC_START
+                 "and a2, a2, %2\n"
+                 "s32i a2, %0, 0\n"  // clear RTC_MEM_CRC_START
+                 "memw\n"
+                 "xor %2, %2, %2\n" // %2 -> DPORT_RTC_MEM_CRC_START, probably unnecessary but gcc assumes inputs unchanged
+
+                 /* Store the calculated value in RTC_MEM_CRC_REG */
+                 "l32i a2, %3, 0\n"
+                 "s32i a2, %4, 0\n"
+                 "memw\n"
+
+                 /* Set wait cycle for touch or COCPU after deep sleep (can be moved to C code part?) */
+                 "l32i a2, %5, 0\n"
+                 "and a2, a2, %6\n"
+                 "or a2, a2, %7\n"
+                 "s32i a2, %5, 0\n"
+
+                 /* Set register bit to go into deep sleep */
+                 "l32i a2, %8, 0\n"
+                 "or   a2, a2, %9\n"
+                 "s32i a2, %8, 0\n"
+                 "memw\n"
+
+                 /* Wait for sleep reject interrupt (never finishes if successful) */
+                 ".Lwaitsleep:"
+                 "memw\n"
+                 "l32i a2, %10, 0\n"
+                 "and a2, a2, %11\n"
+                 "beqz a2, .Lwaitsleep\n"
+
+                 :
+                 : /* Note, at -O0 this is the limit of available registers in this function */
+                   "r" (DPORT_RTC_FASTMEM_CONFIG_REG), // %0
+                   "r" ( (CRC_START_ADDR << DPORT_RTC_MEM_CRC_START_S)
+                         | (CRC_LEN << DPORT_RTC_MEM_CRC_LEN_S)), // %1
+                   "r" (DPORT_RTC_MEM_CRC_START), // %2
+                   "r" (DPORT_RTC_FASTMEM_CRC_REG), // %3
+                   "r" (RTC_MEMORY_CRC_REG), // %4
+                   "r" (RTC_CNTL_TIMER2_REG), // %5
+                   "r" (~RTC_CNTL_ULPCP_TOUCH_START_WAIT_M), // %6
+                   "r" (DEEP_SLEEP_TOUCH_WAIT_CYCLE << RTC_CNTL_ULPCP_TOUCH_START_WAIT_S), // %7
+                   "r" (RTC_CNTL_STATE0_REG), // %8
+                   "r" (RTC_CNTL_SLEEP_EN), // %9
+                   "r" (RTC_CNTL_INT_RAW_REG), // %10
+                   "r" (RTC_CNTL_SLP_REJECT_INT_RAW | RTC_CNTL_SLP_WAKEUP_INT_RAW) // %11
+                 : "a2" // working register
+                 );
+
+    return rtc_sleep_finish(0);
+}
+
+static uint32_t rtc_sleep_finish(uint32_t lslp_mem_inf_fpu)
+{
     /* In deep sleep mode, we never get here */
     uint32_t reject = REG_GET_FIELD(RTC_CNTL_INT_RAW_REG, RTC_CNTL_SLP_REJECT_INT_RAW);
     SET_PERI_REG_MASK(RTC_CNTL_INT_CLR_REG,