[WIP] Handle APB frequency change

CMakeLists.txt

@@ -3,6 +3,7 @@ set(CORE_SRCS
   cores/esp32/cbuf.cpp
   cores/esp32/esp32-hal-adc.c
   cores/esp32/esp32-hal-bt.c
+  cores/esp32/esp32-hal-cpu.c
   cores/esp32/esp32-hal-dac.c
   cores/esp32/esp32-hal-gpio.c
   cores/esp32/esp32-hal-i2c.c

boards.txt

@@ -50,34 +50,22 @@ esp32.menu.PartitionScheme.min_spiffs.upload.maximum_size=1966080
 esp32.menu.PartitionScheme.fatflash=16M Fat
 esp32.menu.PartitionScheme.fatflash.build.partitions=ffat
 
-esp32.menu.CPUFreq.240=240MHz
+esp32.menu.CPUFreq.240=240MHz (WiFi/BT)
 esp32.menu.CPUFreq.240.build.f_cpu=240000000L
-esp32.menu.CPUFreq.160=160MHz
+esp32.menu.CPUFreq.160=160MHz (WiFi/BT)
 esp32.menu.CPUFreq.160.build.f_cpu=160000000L
-esp32.menu.CPUFreq.80=80MHz
+esp32.menu.CPUFreq.80=80MHz (WiFi/BT)
 esp32.menu.CPUFreq.80.build.f_cpu=80000000L
 esp32.menu.CPUFreq.40=40MHz (40MHz XTAL)
 esp32.menu.CPUFreq.40.build.f_cpu=40000000L
 esp32.menu.CPUFreq.26=26MHz (26MHz XTAL)
 esp32.menu.CPUFreq.26.build.f_cpu=26000000L
 esp32.menu.CPUFreq.20=20MHz (40MHz XTAL)
 esp32.menu.CPUFreq.20.build.f_cpu=20000000L
-esp32.menu.CPUFreq.13=13MHz
+esp32.menu.CPUFreq.13=13MHz (26MHz XTAL)
 esp32.menu.CPUFreq.13.build.f_cpu=13000000L
 esp32.menu.CPUFreq.10=10MHz (40MHz XTAL)
 esp32.menu.CPUFreq.10.build.f_cpu=10000000L
-esp32.menu.CPUFreq.8=8MHz (40MHz XTAL)
-esp32.menu.CPUFreq.8.build.f_cpu=8000000L
-esp32.menu.CPUFreq.5=5MHz
-esp32.menu.CPUFreq.5.build.f_cpu=5000000L
-esp32.menu.CPUFreq.4=4MHz
-esp32.menu.CPUFreq.4.build.f_cpu=4000000L
-esp32.menu.CPUFreq.3=3MHz
-esp32.menu.CPUFreq.3.build.f_cpu=3000000L
-esp32.menu.CPUFreq.2=2MHz
-esp32.menu.CPUFreq.2.build.f_cpu=2000000L
-esp32.menu.CPUFreq.1=1MHz
-esp32.menu.CPUFreq.1.build.f_cpu=1000000L
 
 esp32.menu.FlashMode.qio=QIO
 esp32.menu.FlashMode.qio.build.flash_mode=dio

cores/esp32/esp32-hal-cpu.c

@@ -0,0 +1,211 @@
+// Copyright 2015-2016 Espressif Systems (Shanghai) PTE LTD
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "sdkconfig.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/semphr.h"
+#include "freertos/task.h"
+#include "freertos/xtensa_timer.h"
+#include "esp_attr.h"
+#include "esp_log.h"
+#include "soc/rtc.h"
+#include "soc/rtc_cntl_reg.h"
+#include "rom/rtc.h"
+#include "soc/apb_ctrl_reg.h"
+#include "esp32-hal.h"
+#include "esp32-hal-cpu.h"
+
+typedef struct apb_change_cb_s {
+        struct apb_change_cb_s * next;
+        void * arg;
+        apb_change_cb_t cb;
+} apb_change_t;
+
+const uint32_t MHZ = 1000000;
+
+static apb_change_t * apb_change_callbacks = NULL;
+static xSemaphoreHandle apb_change_lock = NULL;
+
+static void initApbChangeCallback(){
+    static volatile bool initialized = false;
+    if(!initialized){
+        initialized = true;
+        apb_change_lock = xSemaphoreCreateMutex();
+        if(!apb_change_lock){
+            initialized = false;
+        }
+    }
+}
+
+static void triggerApbChangeCallback(apb_change_ev_t ev_type, uint32_t old_apb, uint32_t new_apb){
+    initApbChangeCallback();
+    xSemaphoreTake(apb_change_lock, portMAX_DELAY);
+    apb_change_t * r = apb_change_callbacks;
+    while(r != NULL){
+        r->cb(r->arg, ev_type, old_apb, new_apb);
+        r=r->next;
+    }
+    xSemaphoreGive(apb_change_lock);
+}
+
+bool addApbChangeCallback(void * arg, apb_change_cb_t cb){
+    initApbChangeCallback();
+    apb_change_t * c = (apb_change_t*)malloc(sizeof(apb_change_t));
+    if(!c){
+        log_e("Callback Object Malloc Failed");
+        return false;
+    }
+    c->next = NULL;
+    c->arg = arg;
+    c->cb = cb;
+    xSemaphoreTake(apb_change_lock, portMAX_DELAY);
+    if(apb_change_callbacks == NULL){
+        apb_change_callbacks = c;
+    } else {
+        apb_change_t * r = apb_change_callbacks;
+        if(r->cb != cb || r->arg != arg){
+            while(r->next){
+                r = r->next;
+                if(r->cb == cb && r->arg == arg){
+                    free(c);
+                    goto unlock_and_exit;
+                }
+            }
+            r->next = c;
+        }
+    }
+unlock_and_exit:
+    xSemaphoreGive(apb_change_lock);
+    return true;
+}
+
+bool removeApbChangeCallback(void * arg, apb_change_cb_t cb){
+    initApbChangeCallback();
+    xSemaphoreTake(apb_change_lock, portMAX_DELAY);
+    apb_change_t * r = apb_change_callbacks;
+    if(r == NULL){
+        xSemaphoreGive(apb_change_lock);
+        return false;
+    }
+    if(r->cb == cb && r->arg == arg){
+        apb_change_callbacks = r->next;
+        free(r);
+    } else {
+        while(r->next && (r->next->cb != cb || r->next->arg != arg)){
+            r = r->next;
+        }
+        if(r->next == NULL || r->next->cb != cb || r->next->arg != arg){
+            xSemaphoreGive(apb_change_lock);
+            return false;
+        }
+        apb_change_t * c = r->next;
+        r->next = c->next;
+        free(c);
+    }
+    xSemaphoreGive(apb_change_lock);
+    return true;
+}
+
+static uint32_t calculateApb(rtc_cpu_freq_config_t * conf){
+    if(conf->freq_mhz >= 80){
+        return 80 * MHZ;
+    }
+    return (conf->source_freq_mhz * MHZ) / conf->div;
+}
+
+void esp_timer_impl_update_apb_freq(uint32_t apb_ticks_per_us); //private in IDF
+
+bool setCpuFrequencyMhz(uint32_t cpu_freq_mhz){
+    rtc_cpu_freq_config_t conf, cconf;
+    uint32_t capb, apb;
+    //Get XTAL Frequency and calculate min CPU MHz
+    rtc_xtal_freq_t xtal = rtc_clk_xtal_freq_get();
+    uint32_t min_cpu_mhz = 10;
+    if(xtal > RTC_XTAL_FREQ_AUTO){
+        if(xtal < RTC_XTAL_FREQ_40M) {
+            min_cpu_mhz = xtal / 2; //13Mhz for 26Mhz XTAL
+            if(cpu_freq_mhz <= xtal && cpu_freq_mhz != xtal && cpu_freq_mhz != (xtal/2)){
+                log_e("Bad frequency: %u MHz! Options are: 240, 160, 80, %u and %u MHz", cpu_freq_mhz, xtal, xtal/2);
+                return false;
+            }
+        } else if(cpu_freq_mhz <= xtal && cpu_freq_mhz != xtal && cpu_freq_mhz != (xtal/2) && cpu_freq_mhz != (xtal/4)){
+            log_e("Bad frequency: %u MHz! Options are: 240, 160, 80, %u, %u and %u MHz", cpu_freq_mhz, xtal, xtal/2, xtal/4);
+            return false;
+        }
+    }
+    if(cpu_freq_mhz > xtal && cpu_freq_mhz != 240 && cpu_freq_mhz != 160 && cpu_freq_mhz != 80){
+        if(xtal >= RTC_XTAL_FREQ_40M){
+            log_e("Bad frequency: %u MHz! Options are: 240, 160, 80, %u, %u and %u MHz", cpu_freq_mhz, xtal, xtal/2, xtal/4);
+        } else {
+            log_e("Bad frequency: %u MHz! Options are: 240, 160, 80, %u and %u MHz", cpu_freq_mhz, xtal, xtal/2);
+        }
+        return false;
+    }
+    //Get current CPU clock configuration
+    rtc_clk_cpu_freq_get_config(&cconf);
+    //return if frequency has not changed
+    if(cconf.freq_mhz == cpu_freq_mhz){
+        return true;
+    }
+    //Get configuration for the new CPU frequency
+    if(!rtc_clk_cpu_freq_mhz_to_config(cpu_freq_mhz, &conf)){
+        log_e("CPU clock could not be set to %u MHz", cpu_freq_mhz);
+        return false;
+    }
+    //Current APB
+    capb = calculateApb(&cconf);
+    //New APB
+    apb = calculateApb(&conf);
+    log_d("%s: %u / %u = %u Mhz, APB: %u Hz", (conf.source == RTC_CPU_FREQ_SRC_PLL)?"PLL":((conf.source == RTC_CPU_FREQ_SRC_APLL)?"APLL":((conf.source == RTC_CPU_FREQ_SRC_XTAL)?"XTAL":"8M")), conf.source_freq_mhz, conf.div, conf.freq_mhz, apb);
+    //Call peripheral functions before the APB change
+    if(apb_change_callbacks){
+        triggerApbChangeCallback(APB_BEFORE_CHANGE, capb, apb);
+    }
+    //Make the frequency change
+    rtc_clk_cpu_freq_set_config_fast(&conf);
+    if(capb != apb){
+        //Update REF_TICK (uncomment if REF_TICK is different than 1MHz)
+        //if(conf.freq_mhz < 80){
+        //    ESP_REG(APB_CTRL_XTAL_TICK_CONF_REG) = conf.freq_mhz / (REF_CLK_FREQ / MHZ) - 1;
+        //}
+        //Update APB Freq REG
+        rtc_clk_apb_freq_update(apb);
+        //Update esp_timer divisor
+        esp_timer_impl_update_apb_freq(apb / MHZ);
+    }
+    //Update FreeRTOS Tick Divisor
+    uint32_t fcpu = (conf.freq_mhz >= 80)?(conf.freq_mhz * MHZ):(apb);
+    _xt_tick_divisor = fcpu / XT_TICK_PER_SEC;
+    //Call peripheral functions after the APB change
+    if(apb_change_callbacks){
+        triggerApbChangeCallback(APB_AFTER_CHANGE, capb, apb);
+    }
+    return true;
+}
+
+uint32_t getCpuFrequencyMhz(){
+    rtc_cpu_freq_config_t conf;
+    rtc_clk_cpu_freq_get_config(&conf);
+    return conf.freq_mhz;
+}
+
+uint32_t getXtalFrequencyMhz(){
+    return rtc_clk_xtal_freq_get();
+}
+
+uint32_t getApbFrequency(){
+    rtc_cpu_freq_config_t conf;
+    rtc_clk_cpu_freq_get_config(&conf);
+    return calculateApb(&conf);
+}

cores/esp32/esp32-hal-cpu.h

@@ -0,0 +1,48 @@
+// Copyright 2015-2016 Espressif Systems (Shanghai) PTE LTD
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef _ESP32_HAL_CPU_H_
+#define _ESP32_HAL_CPU_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdlib.h>
+
+typedef enum { APB_BEFORE_CHANGE, APB_AFTER_CHANGE } apb_change_ev_t;
+
+typedef void (* apb_change_cb_t)(void * arg, apb_change_ev_t ev_type, uint32_t old_apb, uint32_t new_apb);
+
+bool addApbChangeCallback(void * arg, apb_change_cb_t cb);
+bool removeApbChangeCallback(void * arg, apb_change_cb_t cb);
+
+//function takes the following frequencies as valid values:
+//  240, 160, 80    <<< For all XTAL types
+//  40, 20, 10      <<< For 40MHz XTAL
+//  26, 13          <<< For 26MHz XTAL
+//  24, 12          <<< For 24MHz XTAL
+bool setCpuFrequencyMhz(uint32_t cpu_freq_mhz);
+
+uint32_t getCpuFrequencyMhz();  // In MHz
+uint32_t getXtalFrequencyMhz(); // In MHz
+uint32_t getApbFrequency();     // In Hz
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _ESP32_HAL_CPU_H_ */