From 7248d3d7d28adb5daee8ac6ee69a4b2d49596641 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Bj=C3=B6rn=20Quentin?= <bjoernQ@users.noreply.github.com>
Date: Fri, 5 Sep 2025 19:04:39 +0200
Subject: [PATCH] Make BLE work on ESP32-C2 with 26 MHz Xtal (#4062)

* Make BLE work on ESP32-C2 with 26 MHz Xtal

* CHANGELOG.md
---
 esp-hal/src/clock/clocks_ll/esp32c2.rs |  8 ++++++--
 esp-radio/CHANGELOG.md                 |  1 +
 esp-radio/src/ble/npl.rs               | 19 +++++++++++++++++++
 3 files changed, 26 insertions(+), 2 deletions(-)

diff --git a/esp-hal/src/clock/clocks_ll/esp32c2.rs b/esp-hal/src/clock/clocks_ll/esp32c2.rs
index 0bab1ebac..c6c3ba945 100644
--- a/esp-hal/src/clock/clocks_ll/esp32c2.rs
+++ b/esp-hal/src/clock/clocks_ll/esp32c2.rs
@@ -188,10 +188,14 @@ pub(super) fn ble_rtc_clk_init() {
         w.lp_timer_sel_rtc_slow().clear_bit()
     });
 
-    // assume 40MHz xtal
+    let divider = match crate::rtc_cntl::RtcClock::xtal_freq() {
+        XtalClock::_26M => 129,
+        XtalClock::_40M => 249,
+    };
+
     MODEM_CLKRST::regs()
         .modem_lp_timer_conf()
-        .modify(|_, w| unsafe { w.lp_timer_clk_div_num().bits(249) });
+        .modify(|_, w| unsafe { w.lp_timer_clk_div_num().bits(divider) });
 
     MODEM_CLKRST::regs().etm_clk_conf().modify(|_, w| {
         w.etm_clk_active().set_bit();
diff --git a/esp-radio/CHANGELOG.md b/esp-radio/CHANGELOG.md
index 78da123b3..9dbc3a08c 100644
--- a/esp-radio/CHANGELOG.md
+++ b/esp-radio/CHANGELOG.md
@@ -47,6 +47,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 - Fixed a BLE panic caused by unimplemented functions (#3762)
 - Fixed the BLE stack crashing in certain cases (#3854)
 - `ADC2` now cannot be used simultaneously with `radio` on ESP32 (#3876)
+- BLE on ESP32-C2 with 26MHz xtal (#4062)
 
 ### Removed
 
diff --git a/esp-radio/src/ble/npl.rs b/esp-radio/src/ble/npl.rs
index 8a182e963..277a9dcc0 100644
--- a/esp-radio/src/ble/npl.rs
+++ b/esp-radio/src/ble/npl.rs
@@ -1055,8 +1055,27 @@ pub(crate) fn ble_init() {
 
         self::ble_os_adapter_chip_specific::ble_rtc_clk_init();
 
+        #[cfg(esp32c2)]
+        let mut cfg = ble_os_adapter_chip_specific::BLE_CONFIG;
+
+        #[cfg(not(esp32c2))]
         let cfg = ble_os_adapter_chip_specific::BLE_CONFIG;
 
+        #[cfg(esp32c2)]
+        {
+            use esp_hal::clock::Clock;
+
+            let xtal = crate::hal::rtc_cntl::RtcClock::xtal_freq();
+            let mhz = xtal.mhz() as u8;
+
+            cfg.main_xtal_freq = mhz;
+
+            if mhz == 26 {
+                cfg.rtc_freq = 40000;
+                cfg.main_xtal_freq = 26;
+            }
+        }
+
         let res = esp_register_ext_funcs(&G_OSI_FUNCS as *const ExtFuncsT);
         if res != 0 {
             panic!("esp_register_ext_funcs returned {}", res);