From c33f53806630fcfa471dfc2c8338f79b5d820708 Mon Sep 17 00:00:00 2001
From: Damien George <damien.p.george@gmail.com>
Date: Tue, 5 Feb 2019 23:26:01 +1100
Subject: [PATCH] esp32/modmachine: Add support for changing the CPU frequency.

---
 ports/esp32/Makefile                |  1 +
 ports/esp32/boards/sdkconfig        |  3 +++
 ports/esp32/boards/sdkconfig.spiram |  3 +++
 ports/esp32/modmachine.c            | 23 ++++++++++++++++-------
 4 files changed, 23 insertions(+), 7 deletions(-)

diff --git a/ports/esp32/Makefile b/ports/esp32/Makefile
index 15073453a..068757834 100644
--- a/ports/esp32/Makefile
+++ b/ports/esp32/Makefile
@@ -292,6 +292,7 @@ ESPIDF_ESP32_O = $(addprefix $(ESPCOMP)/esp32/,\
 	hw_random.o \
 	phy_init.o \
 	pm_esp32.o \
+	pm_locks.o \
 	intr_alloc.o \
 	dport_access.o \
 	wifi_init.o \
diff --git a/ports/esp32/boards/sdkconfig b/ports/esp32/boards/sdkconfig
index 0453cccbf..23fc8dead 100644
--- a/ports/esp32/boards/sdkconfig
+++ b/ports/esp32/boards/sdkconfig
@@ -15,6 +15,9 @@ CONFIG_ESP32_DEFAULT_CPU_FREQ_240=y
 CONFIG_TASK_WDT_CHECK_IDLE_TASK_CPU0=n
 CONFIG_ESP32_XTAL_FREQ_AUTO=y
 
+# Power Management
+CONFIG_PM_ENABLE=y
+
 # FreeRTOS
 CONFIG_FREERTOS_UNICORE=y
 CONFIG_FREERTOS_THREAD_LOCAL_STORAGE_POINTERS=2
diff --git a/ports/esp32/boards/sdkconfig.spiram b/ports/esp32/boards/sdkconfig.spiram
index f2ed44c5d..b34781a0d 100644
--- a/ports/esp32/boards/sdkconfig.spiram
+++ b/ports/esp32/boards/sdkconfig.spiram
@@ -18,6 +18,9 @@ CONFIG_SPIRAM_USE_MEMMAP=y
 CONFIG_TASK_WDT_CHECK_IDLE_TASK_CPU0=n
 CONFIG_ESP32_XTAL_FREQ_AUTO=y
 
+# Power Management
+CONFIG_PM_ENABLE=y
+
 # FreeRTOS
 CONFIG_FREERTOS_UNICORE=y
 CONFIG_FREERTOS_THREAD_LOCAL_STORAGE_POINTERS=2
diff --git a/ports/esp32/modmachine.c b/ports/esp32/modmachine.c
index be5f2eb0f..bc459ee5c 100644
--- a/ports/esp32/modmachine.c
+++ b/ports/esp32/modmachine.c
@@ -34,7 +34,8 @@
 #include "freertos/task.h"
 #include "rom/ets_sys.h"
 #include "rom/rtc.h"
-#include "esp_system.h"
+#include "esp_clk.h"
+#include "esp_pm.h"
 #include "driver/touch_pad.h"
 
 #include "py/obj.h"
@@ -60,16 +61,24 @@ typedef enum {
 STATIC mp_obj_t machine_freq(size_t n_args, const mp_obj_t *args) {
     if (n_args == 0) {
         // get
-        return mp_obj_new_int(ets_get_cpu_frequency() * 1000000);
+        return mp_obj_new_int(esp_clk_cpu_freq());
     } else {
         // set
         mp_int_t freq = mp_obj_get_int(args[0]) / 1000000;
-        if (freq != 80 && freq != 160 && freq != 240) {
-            mp_raise_ValueError("frequency can only be either 80Mhz, 160MHz or 240MHz");
+        if (freq != 20 && freq != 40 && freq != 80 && freq != 160 && freq != 240) {
+            mp_raise_ValueError("frequency must be 20MHz, 40MHz, 80Mhz, 160MHz or 240MHz");
+        }
+        esp_pm_config_esp32_t pm;
+        pm.max_freq_mhz = freq;
+        pm.min_freq_mhz = freq;
+        pm.light_sleep_enable = false;
+        esp_err_t ret = esp_pm_configure(&pm);
+        if (ret != ESP_OK) {
+            mp_raise_ValueError(NULL);
+        }
+        while (esp_clk_cpu_freq() != freq * 1000000) {
+            vTaskDelay(1);
         }
-        /*
-        system_update_cpu_freq(freq);
-        */
         return mp_const_none;
     }
 }