From 2991418b2e1f95c5a45eef9e8c5622d85e4f2aa7 Mon Sep 17 00:00:00 2001
From: chenwen <chenwen@espressif.com>
Date: Fri, 6 Nov 2020 17:42:13 +0800
Subject: [PATCH] xtensa/esp32: keep cpu clock while configured cpu clock is
 consistent with the default

---
 arch/xtensa/src/esp32/esp32_clockconfig.c       | 17 ++++++++++++++---
 arch/xtensa/src/esp32/esp32_pm.c                |  9 ++++++++-
 .../esp32/esp32-core/scripts/esp32_rom.ld       |  1 +
 3 files changed, 23 insertions(+), 4 deletions(-)

diff --git a/arch/xtensa/src/esp32/esp32_clockconfig.c b/arch/xtensa/src/esp32/esp32_clockconfig.c
index d05c2312d18..a19f3160e27 100644
--- a/arch/xtensa/src/esp32/esp32_clockconfig.c
+++ b/arch/xtensa/src/esp32/esp32_clockconfig.c
@@ -168,12 +168,13 @@ void IRAM_ATTR esp32_set_cpu_freq(int cpu_freq_mhz)
 
   value = (((80 * MHZ) >> 12) & UINT16_MAX) |
           ((((80 * MHZ) >> 12) & UINT16_MAX) << 16);
-  putreg32(value, RTC_APB_FREQ_REG);
   putreg32(per_conf, DPORT_CPU_PER_CONF_REG);
   REG_SET_FIELD(RTC_CNTL_REG, RTC_CNTL_DIG_DBIAS_WAK, dbias);
   REG_SET_FIELD(RTC_CNTL_CLK_CONF_REG, RTC_CNTL_SOC_CLK_SEL,
                 RTC_CNTL_SOC_CLK_SEL_PLL);
-  g_ticks_per_us_pro = cpu_freq_mhz;
+  putreg32(value, RTC_APB_FREQ_REG);
+  esp32_update_cpu_freq(cpu_freq_mhz);
+  esp32_rtc_wait_for_slow_cycle();
 }
 
 /****************************************************************************
@@ -189,10 +190,16 @@ void IRAM_ATTR esp32_set_cpu_freq(int cpu_freq_mhz)
 void esp32_clockconfig(void)
 {
   uint32_t freq_mhz = CONFIG_ESP32_DEFAULT_CPU_FREQ_MHZ;
-
+  uint32_t old_freq_mhz;
   uint32_t source_freq_mhz;
   enum esp32_rtc_xtal_freq_e xtal_freq = RTC_XTAL_FREQ_40M;
 
+  old_freq_mhz = esp_rtc_clk_get_cpu_freq();
+  if (old_freq_mhz == freq_mhz)
+    {
+      return;
+    }
+
   switch (freq_mhz)
     {
       case 240:
@@ -204,6 +211,9 @@ void esp32_clockconfig(void)
         break;
 
       case 80:
+        source_freq_mhz = RTC_PLL_FREQ_320M;
+        break;
+
       default:
         return;
     }
@@ -252,3 +262,4 @@ int IRAM_ATTR esp_clk_apb_freq(void)
 {
   return MIN(g_ticks_per_us_pro, 80) * MHZ;
 }
+
diff --git a/arch/xtensa/src/esp32/esp32_pm.c b/arch/xtensa/src/esp32/esp32_pm.c
index 71e25d5e859..d3143d2a6a0 100644
--- a/arch/xtensa/src/esp32/esp32_pm.c
+++ b/arch/xtensa/src/esp32/esp32_pm.c
@@ -581,7 +581,14 @@ static int IRAM_ATTR esp32_sleep_start(uint32_t pd_flags)
 
   /* Restore CPU frequency */
 
-  esp32_configure_cpu_freq(cur_freq);
+  if (esp32_configure_cpu_freq(cur_freq) != OK)
+    {
+      pwrwarn("WARNING: Faile to restore CPU frequency"
+              "Configure cpu frequency %d.\n", cur_freq);
+    }
+
+  /* Re-enable UART output */
+
   esp32_resume_uarts();
 
   return result;
diff --git a/boards/xtensa/esp32/esp32-core/scripts/esp32_rom.ld b/boards/xtensa/esp32/esp32-core/scripts/esp32_rom.ld
index 306f4ef6a04..dd68edead42 100644
--- a/boards/xtensa/esp32/esp32-core/scripts/esp32_rom.ld
+++ b/boards/xtensa/esp32/esp32-core/scripts/esp32_rom.ld
@@ -188,6 +188,7 @@ PROVIDE ( ets_isr_unmask = 0x40006808 );
 PROVIDE ( ets_post = 0x4000673c );
 PROVIDE ( ets_printf = 0x40007d54 );
 PROVIDE ( g_ticks_per_us_pro = 0x3ffe01e0 );
+PROVIDE ( g_ticks_per_us_app = 0x3ffe40f0 );
 PROVIDE ( ets_readySet_ = 0x3ffe01f0 );
 PROVIDE ( ets_run = 0x400066bc );
 PROVIDE ( ets_secure_boot_check = 0x4005cb40 );