From 548babf8a04510d99a49b2d129a9579afc33e182 Mon Sep 17 00:00:00 2001
From: Andrew Leech <andrew.leech@planetinnovation.com.au>
Date: Sun, 27 Oct 2024 14:51:37 +1100
Subject: [PATCH] esp32/machine_pwm: Use IDF functions to calculate resolution
 correctly.

This commit fixes PWM configuration across C3, C6, S2 and S3 chips, which
was broken by 6d799378bad4474e77ddb2fa2187ecd6e290e0ba.  Without this fix
the PWM frequency is limited to a maximum of 2446Hz (on S2 at least).

Signed-off-by: Andrew Leech <andrew@alelec.net>
---
 ports/esp32/machine_pwm.c | 50 +++++++++++++--------------------------
 1 file changed, 16 insertions(+), 34 deletions(-)

diff --git a/ports/esp32/machine_pwm.c b/ports/esp32/machine_pwm.c
index 71402e1c4..61efdfa18 100644
--- a/ports/esp32/machine_pwm.c
+++ b/ports/esp32/machine_pwm.c
@@ -34,6 +34,7 @@
 #include "py/mphal.h"
 #include "driver/ledc.h"
 #include "esp_err.h"
+#include "esp_clk_tree.h"
 #include "soc/gpio_sig_map.h"
 
 #define PWM_DBG(...)
@@ -209,51 +210,32 @@ static void configure_channel(machine_pwm_obj_t *self) {
 
 static void set_freq(machine_pwm_obj_t *self, unsigned int freq, ledc_timer_config_t *timer) {
     if (freq != timer->freq_hz) {
-        // Find the highest bit resolution for the requested frequency
-        unsigned int i = APB_CLK_FREQ; // 80 MHz
-        #if SOC_LEDC_SUPPORT_REF_TICK
-        if (freq < EMPIRIC_FREQ) {
-            i = REF_CLK_FREQ; // 1 MHz
-        }
-        #endif
-
-        int divider = (i + freq / 2) / freq; // rounded
-        if (divider == 0) {
-            divider = 1;
-        }
-        float f = (float)i / divider; // actual frequency
-        if (f <= 1.0) {
-            f = 1.0;
-        }
-        i = (unsigned int)roundf((float)i / f);
-
-        unsigned int res = 0;
-        for (; i > 1; i >>= 1) {
-            ++res;
-        }
-        if (res == 0) {
-            res = 1;
-        } else if (res > HIGHEST_PWM_RES) {
-            // Limit resolution to HIGHEST_PWM_RES to match units of our duty
-            res = HIGHEST_PWM_RES;
-        }
-
-        // Configure the new resolution and frequency
-        timer->duty_resolution = res;
+        // Configure the new frequency and resolution
         timer->freq_hz = freq;
-        #if SOC_LEDC_SUPPORT_XTAL_CLOCK
+
+        #if SOC_LEDC_SUPPORT_PLL_DIV_CLOCK
+        timer->clk_cfg = LEDC_USE_PLL_DIV_CLK;
+        #elif SOC_LEDC_SUPPORT_APB_CLOCK
+        timer->clk_cfg = LEDC_USE_APB_CLK;
+        #elif SOC_LEDC_SUPPORT_XTAL_CLOCK
         timer->clk_cfg = LEDC_USE_XTAL_CLK;
         #else
-        timer->clk_cfg = LEDC_USE_APB_CLK;
+        #error No supported PWM / LEDC clocks.
         #endif
         #if SOC_LEDC_SUPPORT_REF_TICK
         if (freq < EMPIRIC_FREQ) {
             timer->clk_cfg = LEDC_USE_REF_TICK;
         }
         #endif
+        uint32_t src_clk_freq = 0;
+        esp_err_t err = esp_clk_tree_src_get_freq_hz(timer->clk_cfg, ESP_CLK_TREE_SRC_FREQ_PRECISION_CACHED, &src_clk_freq);
+        if (err != ESP_OK) {
+            mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("unable to query source clock frequency %d"), (int)timer->clk_cfg);
+        }
+        timer->duty_resolution = ledc_find_suitable_duty_resolution(src_clk_freq, timer->freq_hz);
 
         // Set frequency
-        esp_err_t err = ledc_timer_config(timer);
+        err = ledc_timer_config(timer);
         if (err != ESP_OK) {
             if (err == ESP_FAIL) {
                 mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("unreachable frequency %d"), freq);