From 14eaa3e097e33fa11c6739470249045fbfd0ff2a Mon Sep 17 00:00:00 2001
From: oscgonfer <oscgonfer@gmail.com>
Date: Wed, 23 Jul 2025 13:42:34 +0200
Subject: [PATCH] Fix I2C scan speed

---
 src/main.cpp | 53 ++++++++++++++++++++++++++++++++--------------------
 1 file changed, 33 insertions(+), 20 deletions(-)

diff --git a/src/main.cpp b/src/main.cpp
index 132dab9e2..deda7f107 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -524,6 +524,39 @@ void setup()
     LOG_INFO("Scan for i2c devices");
 #endif
 
+// Scan I2C port at desired speed
+#ifdef SCAN_I2C_CLOCK_SPEED
+    uint32_t currentClock;
+    currentClock = i2cScanner->getClockSpeed(ScanI2C::I2CPort::WIRE);
+    LOG_INFO("Clock speed: %uHz on WIRE", currentClock);
+    LOG_DEBUG("Setting Wire with defined clock speed, %uHz...", SCAN_I2C_CLOCK_SPEED);
+
+    if(!i2cScanner->setClockSpeed(ScanI2C::I2CPort::WIRE, SCAN_I2C_CLOCK_SPEED)) {
+        LOG_ERROR("Unable to set clock speed on WIRE");
+    } else {
+        currentClock = i2cScanner->getClockSpeed(ScanI2C::I2CPort::WIRE);
+        LOG_INFO("Set clock speed: %uHz on WIRE", currentClock);
+    }
+
+    // TODO Check if necessary
+    // LOG_DEBUG("Starting Wire with defined clock speed, %d...", SCAN_I2C_CLOCK_SPEED);
+    // if(!i2cScanner->setClockSpeed(ScanI2C::I2CPort::WIRE1, SCAN_I2C_CLOCK_SPEED)) {
+    //     LOG_ERROR("Unable to set clock speed on WIRE1");
+    // } else {
+    //     LOG_INFO("Set clock speed: %d on WIRE1", SCAN_I2C_CLOCK_SPEED);
+    // }
+
+    // Restore clock speed
+    if (currentClock != SCAN_I2C_CLOCK_SPEED) {
+        if(!i2cScanner->setClockSpeed(ScanI2C::I2CPort::WIRE, currentClock)) {
+            LOG_ERROR("Unable to restore clock speed on WIRE");
+        } else {
+            currentClock = i2cScanner->getClockSpeed(ScanI2C::I2CPort::WIRE);
+            LOG_INFO("Set clock speed restored to: %uHz on WIRE", currentClock);
+        }
+    }
+#endif
+
 #if defined(I2C_SDA1) || (defined(NRF52840_XXAA) && (WIRE_INTERFACES_COUNT == 2))
     i2cScanner->scanPort(ScanI2C::I2CPort::WIRE1);
 #endif
@@ -539,26 +572,6 @@ void setup()
     i2cScanner->scanPort(ScanI2C::I2CPort::WIRE);
 #endif
 
-#ifdef I2C_CLOCK_SPEED
-    uint32_t currentClock;
-    currentClock = i2cScanner->getClockSpeed(ScanI2C::I2CPort::WIRE);
-    LOG_INFO("Clock speed: %uHz on WIRE", currentClock);
-    LOG_DEBUG("Setting Wire with defined clock speed, %uHz...", I2C_CLOCK_SPEED);
-    if(!i2cScanner->setClockSpeed(ScanI2C::I2CPort::WIRE, I2C_CLOCK_SPEED)) {
-        LOG_ERROR("Unable to set clock speed on WIRE");
-    } else {
-
-        currentClock = i2cScanner->getClockSpeed(ScanI2C::I2CPort::WIRE);
-        LOG_INFO("Set clock speed: %uHz on WIRE", currentClock);
-    }
-    // LOG_DEBUG("Starting Wire with defined clock speed, %d...", I2C_CLOCK_SPEED);
-    // if(!i2cScanner->setClockSpeed(ScanI2C::I2CPort::WIRE1, I2C_CLOCK_SPEED)) {
-    //     LOG_ERROR("Unable to set clock speed on WIRE1");
-    // } else {
-    //     LOG_INFO("Set clock speed: %d on WIRE1", I2C_CLOCK_SPEED);
-    // }
-#endif
-
     auto i2cCount = i2cScanner->countDevices();
     if (i2cCount == 0) {
         LOG_INFO("No I2C devices found");