From 547767ca68fc435b84a61634907d5496829028c1 Mon Sep 17 00:00:00 2001
From: oscgonfer <oscgonfer@gmail.com>
Date: Fri, 11 Jul 2025 14:39:26 +0200
Subject: [PATCH] Add functions to check for I2C bus speed and set it

---
 src/detect/ScanI2CTwoWire.cpp | 69 +++++++++++++++++++++++++++++++++++
 src/detect/ScanI2CTwoWire.h   |  3 ++
 src/main.cpp                  | 21 +++++++++++
 3 files changed, 93 insertions(+)

diff --git a/src/detect/ScanI2CTwoWire.cpp b/src/detect/ScanI2CTwoWire.cpp
index 9e9441123..5734fc200 100644
--- a/src/detect/ScanI2CTwoWire.cpp
+++ b/src/detect/ScanI2CTwoWire.cpp
@@ -109,6 +109,75 @@ uint16_t ScanI2CTwoWire::getRegisterValue(const ScanI2CTwoWire::RegisterLocation
     return value;
 }
 
+bool ScanI2CTwoWire::setClockSpeed(I2CPort port, uint32_t speed) {
+
+    DeviceAddress addr(port, 0x00);
+    TwoWire *i2cBus;
+
+#if WIRE_INTERFACES_COUNT == 2
+    if (port == I2CPort::WIRE1) {
+        i2cBus = &Wire1;
+    } else {
+#endif
+        i2cBus = &Wire;
+#if WIRE_INTERFACES_COUNT == 2
+    }
+#endif
+
+    return i2cBus->setClock(speed);
+}
+
+uint32_t ScanI2CTwoWire::getClockSpeed(I2CPort port) {
+
+    DeviceAddress addr(port, 0x00);
+    TwoWire *i2cBus;
+
+#if WIRE_INTERFACES_COUNT == 2
+    if (port == I2CPort::WIRE1) {
+        i2cBus = &Wire1;
+    } else {
+#endif
+        i2cBus = &Wire;
+#if WIRE_INTERFACES_COUNT == 2
+    }
+#endif
+
+    return i2cBus->getClock();
+}
+
+/// for SEN5X detection
+String readSEN5xProductName(TwoWire* i2cBus, uint8_t address) {
+    uint8_t cmd[] = { 0xD0, 0x14 };
+    uint8_t response[48] = {0};
+
+    i2cBus->beginTransmission(address);
+    i2cBus->write(cmd, 2);
+    if (i2cBus->endTransmission() != 0) return "";
+
+    delay(20);
+    if (i2cBus->requestFrom(address, (uint8_t)48) != 48) return "";
+
+    for (int i = 0; i < 48 && i2cBus->available(); ++i) {
+        response[i] = i2cBus->read();
+    }
+
+    char productName[33] = {0};
+    int j = 0;
+    for (int i = 0; i < 48 && j < 32; i += 3) {
+        if (response[i] >= 32 && response[i] <= 126)
+            productName[j++] = response[i];
+        else
+            break;
+
+        if (response[i + 1] >= 32 && response[i + 1] <= 126)
+            productName[j++] = response[i + 1];
+        else
+            break;
+    }
+
+    return String(productName);
+}
+
 #define SCAN_SIMPLE_CASE(ADDR, T, ...)                                                                                           \
     case ADDR:                                                                                                                   \
         logFoundDevice(__VA_ARGS__);                                                                                             \
diff --git a/src/detect/ScanI2CTwoWire.h b/src/detect/ScanI2CTwoWire.h
index 6988091ad..28b073a17 100644
--- a/src/detect/ScanI2CTwoWire.h
+++ b/src/detect/ScanI2CTwoWire.h
@@ -29,6 +29,9 @@ class ScanI2CTwoWire : public ScanI2C
 
     size_t countDevices() const override;
 
+    bool setClockSpeed(ScanI2C::I2CPort, uint32_t);
+    uint32_t getClockSpeed(ScanI2C::I2CPort);
+
   protected:
     FoundDevice firstOfOrNONE(size_t, DeviceType[]) const override;
 
diff --git a/src/main.cpp b/src/main.cpp
index 2e2adfd46..9f3e4fe6a 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -472,6 +472,7 @@ void setup()
     Wire.setSCL(I2C_SCL);
     Wire.begin();
 #elif defined(I2C_SDA) && !defined(ARCH_RP2040)
+    LOG_INFO("Starting Bus with (SDA) %d and (SCL) %d: ", I2C_SDA, I2C_SCL);
     Wire.begin(I2C_SDA, I2C_SCL);
 #elif defined(ARCH_PORTDUINO)
     if (settingsStrings[i2cdev] != "") {
@@ -530,6 +531,26 @@ 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");