From b5c321d9817cb8cbc9055391425d5217ba2ffbcf Mon Sep 17 00:00:00 2001
From: Mahavir Jain <mahavir@espressif.com>
Date: Tue, 25 Sep 2018 22:40:32 +0530
Subject: [PATCH] esp32: fix regression introduced in automatic light sleep pm

Closes: https://github.com/espressif/esp-idf/issues/2459

Signed-off-by: Mahavir Jain <mahavir@espressif.com>
---
 components/esp32/freertos_hooks.c |  4 +---
 components/esp32/pm_esp32.c       | 30 +++++++++++++++++++++++-------
 components/esp32/pm_impl.h        |  4 ++++
 3 files changed, 28 insertions(+), 10 deletions(-)

diff --git a/components/esp32/freertos_hooks.c b/components/esp32/freertos_hooks.c
index e5198542b8..4e2d713386 100644
--- a/components/esp32/freertos_hooks.c
+++ b/components/esp32/freertos_hooks.c
@@ -60,9 +60,7 @@ void esp_vApplicationIdleHook()
     esp_pm_impl_idle_hook();
 #endif
 
-#ifndef CONFIG_FREERTOS_USE_TICKLESS_IDLE
-    asm("waiti 0");
-#endif
+    esp_pm_impl_waiti();
 }
 
 esp_err_t esp_register_freertos_idle_hook_for_cpu(esp_freertos_idle_cb_t new_idle_cb, UBaseType_t cpuid)
diff --git a/components/esp32/pm_esp32.c b/components/esp32/pm_esp32.c
index bf01418337..af25f47709 100644
--- a/components/esp32/pm_esp32.c
+++ b/components/esp32/pm_esp32.c
@@ -80,6 +80,14 @@ static uint32_t s_mode_mask;
 static uint32_t s_ccount_div;
 static uint32_t s_ccount_mul;
 
+#if CONFIG_FREERTOS_USE_TICKLESS_IDLE
+/* Indicates if light sleep entry was successful for given CPU.
+ * This in turn gets used in IDLE hook to decide if `waiti` needs
+ * to be invoked or not.
+ */
+static bool s_entered_light_sleep[portNUM_PROCESSORS];
+#endif // CONFIG_FREERTOS_USE_TICKLESS_IDLE
+
 /* Indicates to the ISR hook that CCOMPARE needs to be updated on the given CPU.
  * Used in conjunction with cross-core interrupt to update CCOMPARE on the other CPU.
  */
@@ -453,11 +461,24 @@ void IRAM_ATTR esp_pm_impl_isr_hook()
     ESP_PM_TRACE_EXIT(ISR_HOOK, core_id);
 }
 
+void esp_pm_impl_waiti()
+{
+#if CONFIG_FREERTOS_USE_TICKLESS_IDLE
+    int core_id = xPortGetCoreID();
+    if (!s_entered_light_sleep[core_id]) {
+        asm("waiti 0");
+    } else {
+        s_entered_light_sleep[core_id] = false;
+    }
+#else
+    asm("waiti 0");
+#endif // CONFIG_FREERTOS_USE_TICKLESS_IDLE
+}
+
 #if CONFIG_FREERTOS_USE_TICKLESS_IDLE
 
 void IRAM_ATTR vApplicationSleep( TickType_t xExpectedIdleTime )
 {
-    bool result = false;
     portENTER_CRITICAL(&s_switch_lock);
     if (s_mode == PM_MODE_LIGHT_SLEEP && !s_is_switching) {
         /* Calculate how much we can sleep */
@@ -495,15 +516,10 @@ void IRAM_ATTR vApplicationSleep( TickType_t xExpectedIdleTime )
                     ;
                 }
             }
-            result = true;
+            s_entered_light_sleep[core_id] = true;
         }
     }
     portEXIT_CRITICAL(&s_switch_lock);
-
-    /* Tick less idle was not successful, can block till next interrupt here */
-    if (!result) {
-        asm("waiti 0");
-    }
 }
 #endif //CONFIG_FREERTOS_USE_TICKLESS_IDLE
 
diff --git a/components/esp32/pm_impl.h b/components/esp32/pm_impl.h
index 7f1bf09cbe..ddb90c02a8 100644
--- a/components/esp32/pm_impl.h
+++ b/components/esp32/pm_impl.h
@@ -104,6 +104,10 @@ void esp_pm_impl_isr_hook();
  */
 void esp_pm_impl_dump_stats(FILE* out);
 
+/**
+ * @brief Hook function implementing `waiti` instruction, should be invoked from idle task context
+ */
+void esp_pm_impl_waiti();
 
 #ifdef CONFIG_PM_PROFILING
 #define WITH_PROFILING
-- 
2.40.0