deep sleep: Calculate RTC CRC immediately before deep sleep, without using RAM

Fix for issues where RTC FAST memory is updated as part of going into deep
sleep. Very high risk if heaps are in RTC memory - in particular task stacks
may be in RTC memory, but also other variables.

Also fixes potential concurrency problems as RTC FAST memory is not accessible
by CPU during the CRC calculation itself.

Method:
- Disable interrupts (currently for single core only, will need update for S3)
- Load all registers before calculating CRC or going to sleep

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. */
-static _lock_t lock_rtc_memory_crc;
+   is not thread-safe, so we need to disable interrupts before going to deep sleep. */
+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/esp32s2/sleep_modes.c

@@ -87,8 +87,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. */
-static _lock_t lock_rtc_memory_crc;
+   is not thread-safe, so we need to disable interrupts before going to deep sleep. */
+static portMUX_TYPE spinlock_rtc_deep_sleep = portMUX_INITIALIZER_UNLOCKED;
 
 static const char* TAG = "sleep";
 
@@ -103,16 +103,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;
@@ -122,10 +112,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();
@@ -216,7 +207,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);
@@ -227,6 +238,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/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/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,