TactilityKernel improvements (#464)

* **New Features**
  * Thread API with lifecycle, priority, affinity and state monitoring
  * Timer subsystem: one-shot/periodic timers, reset, pending callbacks, expiry queries
  * Expanded I²C: register-level ops, bulk writes, presence checks, ESP32 integration and new config properties
  * GPIO controller: pin level and options APIs
  * Error utilities: new error code, error-to-string, timeout helper and common macros
  * Device construction helpers (construct+start)

* **Tests**
  * New unit tests for thread and timer behavior

* **Documentation**
  * Expanded coding style guide (files/folders, C conventions)

Tactility/CMakeLists.txt

@@ -5,6 +5,7 @@ if (DEFINED ENV{ESP_IDF_VERSION})
 
     list(APPEND REQUIRES_LIST
         TactilityKernel
+        PlatformEsp32
         TactilityCore
         TactilityFreeRtos
         lvgl
@@ -54,31 +55,27 @@ else()
 
     add_library(Tactility OBJECT)
 
-    target_sources(Tactility
-        PRIVATE ${SOURCES}
-    )
-
-    include_directories(
-        PRIVATE Private/
-    )
+    target_sources(Tactility PRIVATE ${SOURCES})
 
     target_include_directories(Tactility
+        PRIVATE Private/
         PUBLIC Include/
     )
 
     add_definitions(-D_Nullable=)
     add_definitions(-D_Nonnull=)
 
-    target_link_libraries(Tactility
-        PUBLIC cJSON
-        PUBLIC TactilityFreeRtos
-        PUBLIC TactilityCore
-        PUBLIC TactilityKernel
-        PUBLIC freertos_kernel
-        PUBLIC lvgl
-        PUBLIC lv_screenshot
-        PUBLIC minmea
-        PUBLIC minitar
+    target_link_libraries(Tactility PUBLIC
+        cJSON
+        TactilityFreeRtos
+        TactilityCore
+        TactilityKernel
+        PlatformPosix
+        freertos_kernel
+        lvgl
+        lv_screenshot
+        minmea
+        minitar
     )
 endif()
 

Tactility/Include/Tactility/hal/i2c/I2c.h

@@ -14,7 +14,6 @@ constexpr TickType_t defaultTimeout = 10 / portTICK_PERIOD_MS;
 
 enum class InitMode {
     ByTactility, // Tactility will initialize it in the correct bootup phase
-    ByExternal, // The device is already initialized and Tactility should assume it works
     Disabled // Not initialized by default
 };
 
@@ -39,15 +38,6 @@ enum class Status {
     Unknown
 };
 
-/**
- * Reconfigure a port with the provided settings.
- * @warning This fails when the HAL Configuration is not mutable.
- * @param[in] port the port to reconfigure
- * @param[in] configuration the new configuration
- * @return true on success
- */
-bool configure(i2c_port_t port, const i2c_config_t& configuration);
-
 /**
  * Start the bus for the specified port.
  * Devices might be started automatically at boot if their HAL configuration requires it.
@@ -60,6 +50,9 @@ bool stop(i2c_port_t port);
 /** @return true if the bus is started */
 bool isStarted(i2c_port_t port);
 
+/** @return name or nullptr */
+const char* getName(i2c_port_t port);
+
 /** Read bytes in master mode. */
 bool masterRead(i2c_port_t port, uint8_t address, uint8_t* data, size_t dataSize, TickType_t timeout = defaultTimeout);
 

Tactility/Private/Tactility/app/i2cscanner/I2cHelpers.h

@@ -9,4 +9,6 @@ std::string getAddressText(uint8_t address);
 
 std::string getPortNamesForDropdown();
 
+bool getActivePortAtIndex(int32_t index, int32_t& out);
+
 }

Tactility/Source/Tactility.cpp

@@ -74,16 +74,13 @@ namespace service {
 
 namespace app {
     namespace addgps { extern const AppManifest manifest; }
+    namespace alertdialog { extern const AppManifest manifest; }
     namespace apphub { extern const AppManifest manifest; }
     namespace apphubdetails { extern const AppManifest manifest; }
-    namespace alertdialog { extern const AppManifest manifest; }
     namespace appdetails { extern const AppManifest manifest; }
     namespace applist { extern const AppManifest manifest; }
     namespace appsettings { extern const AppManifest manifest; }
     namespace boot { extern const AppManifest manifest; }
-#if defined(CONFIG_SOC_WIFI_SUPPORTED) && !defined(CONFIG_SLAVE_SOC_WIFI_SUPPORTED)
-    namespace chat { extern const AppManifest manifest; }
-#endif
     namespace development { extern const AppManifest manifest; }
     namespace display { extern const AppManifest manifest; }
     namespace files { extern const AppManifest manifest; }
@@ -94,9 +91,6 @@ namespace app {
     namespace imageviewer { extern const AppManifest manifest; }
     namespace inputdialog { extern const AppManifest manifest; }
     namespace launcher { extern const AppManifest manifest; }
-#if defined(ESP_PLATFORM) && defined(CONFIG_TT_DEVICE_LILYGO_TDECK)
-    namespace keyboardsettings { extern const AppManifest manifest; }
-#endif
     namespace localesettings { extern const AppManifest manifest; }
     namespace notes { extern const AppManifest manifest; }
     namespace power { extern const AppManifest manifest; }
@@ -105,21 +99,27 @@ namespace app {
     namespace systeminfo { extern const AppManifest manifest; }
     namespace timedatesettings { extern const AppManifest manifest; }
     namespace timezone { extern const AppManifest manifest; }
-#if defined(ESP_PLATFORM) && defined(CONFIG_TT_DEVICE_LILYGO_TDECK)
-    namespace trackballsettings { extern const AppManifest manifest; }
-#endif
     namespace usbsettings { extern const AppManifest manifest; }
     namespace wifiapsettings { extern const AppManifest manifest; }
     namespace wificonnect { extern const AppManifest manifest; }
     namespace wifimanage { extern const AppManifest manifest; }
+
 #ifdef ESP_PLATFORM
+    namespace crashdiagnostics { extern const AppManifest manifest; }
     namespace webserversettings { extern const AppManifest manifest; }
 #endif
+
+#if defined(ESP_PLATFORM) && defined(CONFIG_TT_DEVICE_LILYGO_TDECK)
+    namespace keyboardsettings { extern const AppManifest manifest; }
+    namespace trackballsettings { extern const AppManifest manifest; }
+#endif
+
 #if TT_FEATURE_SCREENSHOT_ENABLED
     namespace screenshot { extern const AppManifest manifest; }
 #endif
-#ifdef ESP_PLATFORM
-    namespace crashdiagnostics { extern const AppManifest manifest; }
+
+#if defined(CONFIG_SOC_WIFI_SUPPORTED) && !defined(CONFIG_SLAVE_SOC_WIFI_SUPPORTED)
+    namespace chat { extern const AppManifest manifest; }
 #endif
 }
 
@@ -138,12 +138,11 @@ static void registerInternalApps() {
     addAppManifest(app::display::manifest);
     addAppManifest(app::files::manifest);
     addAppManifest(app::fileselection::manifest);
+    addAppManifest(app::i2cscanner::manifest);
+    addAppManifest(app::i2csettings::manifest);
     addAppManifest(app::imageviewer::manifest);
     addAppManifest(app::inputdialog::manifest);
     addAppManifest(app::launcher::manifest);
-#if defined(ESP_PLATFORM) && defined(CONFIG_TT_DEVICE_LILYGO_TDECK)
-    addAppManifest(app::keyboardsettings::manifest);
-#endif
     addAppManifest(app::localesettings::manifest);
     addAppManifest(app::notes::manifest);
     addAppManifest(app::settings::manifest);
@@ -151,14 +150,19 @@ static void registerInternalApps() {
     addAppManifest(app::systeminfo::manifest);
     addAppManifest(app::timedatesettings::manifest);
     addAppManifest(app::timezone::manifest);
-#if defined(ESP_PLATFORM) && defined(CONFIG_TT_DEVICE_LILYGO_TDECK)
-    addAppManifest(app::trackballsettings::manifest);
-#endif
     addAppManifest(app::wifiapsettings::manifest);
     addAppManifest(app::wificonnect::manifest);
     addAppManifest(app::wifimanage::manifest);
+
 #ifdef ESP_PLATFORM
     addAppManifest(app::webserversettings::manifest);
+    addAppManifest(app::crashdiagnostics::manifest);
+    addAppManifest(app::development::manifest);
+#endif
+
+#if defined(ESP_PLATFORM) && defined(CONFIG_TT_DEVICE_LILYGO_TDECK)
+    addAppManifest(app::keyboardsettings::manifest);
+    addAppManifest(app::trackballsettings::manifest);
 #endif
 
 #if defined(CONFIG_TINYUSB_MSC_ENABLED) && CONFIG_TINYUSB_MSC_ENABLED
@@ -173,16 +177,6 @@ static void registerInternalApps() {
     addAppManifest(app::chat::manifest);
 #endif
 
-#ifdef ESP_PLATFORM
-    addAppManifest(app::crashdiagnostics::manifest);
-    addAppManifest(app::development::manifest);
-#endif
-
-    if (!hal::getConfiguration()->i2c.empty()) {
-        addAppManifest(app::i2cscanner::manifest);
-        addAppManifest(app::i2csettings::manifest);
-    }
-
     if (!hal::getConfiguration()->uart.empty()) {
         addAppManifest(app::addgps::manifest);
         addAppManifest(app::gpssettings::manifest);

Tactility/Source/app/i2cscanner/I2cHelpers.cpp

@@ -19,18 +19,31 @@ std::string getAddressText(uint8_t address) {
 
 std::string getPortNamesForDropdown() {
     std::vector<std::string> config_names;
-    size_t port_index = 0;
-    for (const auto& i2c_config: tt::getConfiguration()->hardware->i2c) {
-        if (!i2c_config.name.empty()) {
-            config_names.push_back(i2c_config.name);
-        } else {
-            std::stringstream stream;
-            stream << "Port " << std::to_string(port_index);
-            config_names.push_back(stream.str());
+    for (int port = 0; port < I2C_NUM_MAX; ++port) {
+        auto native_port = static_cast<i2c_port_t>(port);
+        if (hal::i2c::isStarted(native_port)) {
+            auto* name = hal::i2c::getName(native_port);
+            if (name != nullptr) {
+                config_names.push_back(name);
+            }
         }
-        port_index++;
     }
     return string::join(config_names, "\n");
 }
 
+bool getActivePortAtIndex(int32_t index, int32_t& out) {
+    int current_index = -1;
+    for (int port = 0; port < I2C_NUM_MAX; ++port) {
+        auto native_port = static_cast<i2c_port_t>(port);
+        if (hal::i2c::isStarted(native_port)) {
+            current_index++;
+            if (current_index == index) {
+                out = port;
+                return true;
+            }
+        }
+    }
+    return false;
+}
+
 }

Tactility/Source/app/i2cscanner/I2cScanner.cpp

@@ -141,11 +141,10 @@ void I2cScannerApp::onShow(AppContext& app, lv_obj_t* parent) {
     lv_obj_add_flag(scan_list, LV_OBJ_FLAG_HIDDEN);
     scanListWidget = scan_list;
 
-    auto i2c_devices = getConfiguration()->hardware->i2c;
-    if (!i2c_devices.empty()) {
-        assert(selected_bus < i2c_devices.size());
-        port = i2c_devices[selected_bus].port;
-        selectBus(selected_bus);
+    int32_t first_port;
+    if (getActivePortAtIndex(0, first_port)) {
+        lv_dropdown_set_selected(port_dropdown, 0);
+        selectBus(0);
     }
 }
 
@@ -307,12 +306,14 @@ void I2cScannerApp::onSelectBus(lv_event_t* event) {
 }
 
 void I2cScannerApp::selectBus(int32_t selected) {
-    auto i2c_devices = getConfiguration()->hardware->i2c;
-    assert(selected < i2c_devices.size());
+    int32_t found_port;
+    if (!getActivePortAtIndex(selected, found_port)) {
+        return;
+    }
 
     if (mutex.lock(100 / portTICK_PERIOD_MS)) {
         scannedAddresses.clear();
-        port = i2c_devices[selected].port;
+        port = static_cast<i2c_port_t>(found_port);
         scanState = ScanStateInitial;
         mutex.unlock();
     }

Tactility/Source/hal/i2c/I2c.cpp

@@ -2,7 +2,14 @@
 
 #include <Tactility/Logger.h>
 #include <Tactility/Mutex.h>
+
 #include <tactility/check.h>
+#include <tactility/drivers/i2c_controller.h>
+#include <tactility/time.h>
+
+#ifdef ESP_PLATFORM
+#include <tactility/drivers/esp32_i2c.h>
+#endif
 
 namespace tt::hal::i2c {
 
@@ -11,270 +18,250 @@ static const auto LOGGER = Logger("I2C");
 struct Data {
     Mutex mutex;
     bool isConfigured = false;
-    bool isStarted = false;
-    Configuration configuration;
+    Device* device = nullptr;
+#ifdef ESP_PLATFORM
+    Esp32I2cConfig config = {
+        .port = I2C_NUM_0,
+        .clockFrequency = 0,
+        .pinSda = 0,
+        .pinScl = 0,
+        .pinSdaPullUp = false,
+        .pinSclPullUp = false
+    };
+#endif
 };
 
-static const uint8_t ACK_CHECK_EN = 1;
 static Data dataArray[I2C_NUM_MAX];
 
+#ifdef ESP_PLATFORM
+void registerDriver(Data& data, const Configuration& configuration) {
+    // Should only be called on init
+    check(data.device == nullptr);
+
+    data.config.port = configuration.port;
+    data.config.clockFrequency = configuration.config.master.clk_speed;
+    data.config.pinSda = configuration.config.sda_io_num;
+    data.config.pinScl = configuration.config.scl_io_num;
+    data.config.pinSdaPullUp = configuration.config.sda_pullup_en;
+    data.config.pinSclPullUp = configuration.config.scl_pullup_en;
+
+    data.device = new Device();
+    data.device->name = configuration.name.c_str();
+    data.device->config = &data.config;
+    data.device->parent = nullptr;
+
+    if (device_construct_add(data.device, "espressif,esp32-i2c") == ERROR_NONE) {
+        data.isConfigured = true;
+    }
+}
+
+Device* findExistingKernelDevice(i2c_port_t port) {
+    struct Params {
+        i2c_port_t port;
+        Device* device;
+    };
+
+    Params params = {
+        .port = port,
+        .device = nullptr
+    };
+
+    for_each_device_of_type(&I2C_CONTROLLER_TYPE, &params, [](auto* device, auto* context) {
+        auto* params_ptr = (Params*)context;
+        auto* driver = device_get_driver(device);
+        if (driver == nullptr) return true;
+        if (!driver_is_compatible(driver, "espressif,esp32-i2c")) return true;
+        i2c_port_t port;
+        if (esp32_i2c_get_port(device, &port) != ERROR_NONE) return true;
+        if (port != params_ptr->port) return true;
+        // Found it, stop iterating
+        params_ptr->device = device;
+        return false;
+    });
+
+    return params.device;
+}
+#endif
+
 bool init(const std::vector<Configuration>& configurations) {
    LOGGER.info("Init");
-   for (const auto& configuration: configurations) {
 #ifdef ESP_PLATFORM
-       if (configuration.config.mode != I2C_MODE_MASTER) {
-           LOGGER.error("Currently only master mode is supported");
-           return false;
-       }
-#endif // ESP_PLATFORM
-       Data& data = dataArray[configuration.port];
-       data.configuration = configuration;
-       data.isConfigured = true;
-   }
-
-   for (const auto& config: configurations) {
-       if (config.initMode == InitMode::ByTactility) {
-           if (!start(config.port)) {
-               return false;
-           }
-       } else if (config.initMode == InitMode::ByExternal) {
-           dataArray[config.port].isStarted = true;
-       }
-   }
-
-   return true;
-}
-
-bool configure(i2c_port_t port, const i2c_config_t& configuration) {
-    auto lock = getLock(port).asScopedLock();
-    lock.lock();
-
-    Data& data = dataArray[port];
-    if (data.isStarted) {
-        LOGGER.error("({}) Cannot reconfigure while interface is started", static_cast<int>(port));
-        return false;
-    } else if (!data.configuration.isMutable) {
-        LOGGER.error("({}) Mutation not allowed because configuration is immutable", static_cast<int>(port));
-        return false;
-    } else {
-        data.configuration.config = configuration;
-        return true;
+    bool found_existing = false;
+    for (int port = 0; port < I2C_NUM_MAX; ++port) {
+        auto native_port = static_cast<i2c_port_t>(port);
+        auto existing_device = findExistingKernelDevice(native_port);
+        if (existing_device != nullptr) {
+            LOGGER.info("Initialized port {} with existing kernel device", port);
+            auto& data = dataArray[port];
+            data.device = existing_device;
+            data.isConfigured = true;
+            memcpy(&data.config, existing_device->config, sizeof(Esp32I2cConfig));
+            // Ensure we don't initialize
+            found_existing = true;
+        }
     }
+
+    // Nothing found in HAL, so try configuration
+    for (const auto& configuration: configurations) {
+        check(!found_existing, "hal::Configuration specifies I2C, but I2C was already initialized by devicetree. Remove the hal::Configuration I2C entries!");
+        if (configuration.config.mode != I2C_MODE_MASTER) {
+            LOGGER.error("Currently only master mode is supported");
+            return false;
+        }
+        Data& data = dataArray[configuration.port];
+        registerDriver(data, configuration);
+    }
+
+    if (!found_existing) {
+        for (const auto& config: configurations) {
+            if (config.initMode == InitMode::ByTactility) {
+                if (!start(config.port)) {
+                    return false;
+                }
+            }
+        }
+    }
+
+#endif
+    return true;
 }
 
 bool start(i2c_port_t port) {
+#ifdef ESP_PLATFORM
     auto lock = getLock(port).asScopedLock();
     lock.lock();
 
     Data& data = dataArray[port];
-    Configuration& config = data.configuration;
-
-    if (data.isStarted) {
-        LOGGER.error("({}) Starting: Already started", static_cast<int>(port));
-        return false;
-    }
 
     if (!data.isConfigured) {
         LOGGER.error("({}) Starting: Not configured", static_cast<int>(port));
         return false;
     }
 
-#ifdef ESP_PLATFORM
-    esp_err_t result = i2c_param_config(port, &config.config);
-    if (result != ESP_OK) {
-        LOGGER.error("({}) Starting: Failed to configure: {}", static_cast<int>(port), esp_err_to_name(result));
+    check(data.device);
+
+    error_t error = device_start(data.device);
+    if (error != ERROR_NONE) {
+        LOGGER.error("Failed to start device {}: {}", data.device->name, error_to_string(error));
         return false;
     }
 
-    result = i2c_driver_install(port, config.config.mode, 0, 0, 0);
-    if (result != ESP_OK) {
-        LOGGER.error("({}) Starting: Failed to install driver: {}", static_cast<int>(port), esp_err_to_name(result));
-        return false;
-    }
-#endif // ESP_PLATFORM
-
-    data.isStarted = true;
-
-    LOGGER.info("({}) Started", static_cast<int>(port));
     return true;
+#else
+    return false;
+#endif
 }
 
 bool stop(i2c_port_t port) {
+#ifdef ESP_PLATFORM
     auto lock = getLock(port).asScopedLock();
     lock.lock();
 
     Data& data = dataArray[port];
-    Configuration& config = data.configuration;
-
-    if (!config.isMutable) {
-        LOGGER.error("({}) Stopping: Not allowed for immutable configuration", static_cast<int>(port));
-        return false;
-    }
-
-    if (!data.isStarted) {
-        LOGGER.error("({}) Stopping: Not started", static_cast<int>(port));
-        return false;
-    }
-
-#ifdef ESP_PLATFORM
-    esp_err_t result = i2c_driver_delete(port);
-    if (result != ESP_OK) {
-        LOGGER.error("({}) Stopping: Failed to delete driver: {}", static_cast<int>(port), esp_err_to_name(result));
-        return false;
-    }
-#endif // ESP_PLATFORM
-
-    data.isStarted = false;
-
-    LOGGER.info("({}) Stopped", static_cast<int>(port));
-    return true;
+    if (!dataArray[port].isConfigured) return false;
+    return device_stop(data.device) == ERROR_NONE;
+#else
+    return false;
+#endif
 }
 
 bool isStarted(i2c_port_t port) {
+#ifdef ESP_PLATFORM
     auto lock = getLock(port).asScopedLock();
     lock.lock();
-    return dataArray[port].isStarted;
+    if (!dataArray[port].isConfigured) return false;
+    return device_is_ready(dataArray[port].device);
+#else
+    return false;
+#endif
+}
+
+const char* getName(i2c_port_t port) {
+#ifdef ESP_PLATFORM
+    auto lock = getLock(port).asScopedLock();
+    lock.lock();
+    if (!dataArray[port].isConfigured) return nullptr;
+    return dataArray[port].device->name;
+#else
+    return nullptr;
+#endif
 }
 
 bool masterRead(i2c_port_t port, uint8_t address, uint8_t* data, size_t dataSize, TickType_t timeout) {
-    auto lock = getLock(port).asScopedLock();
-    if (!lock.lock(timeout)) {
-        LOGGER.error("({}) Mutex timeout", static_cast<int>(port));
-        return false;
-    }
-
 #ifdef ESP_PLATFORM
-    auto result = i2c_master_read_from_device(port, address, data, dataSize, timeout);
-    ESP_ERROR_CHECK_WITHOUT_ABORT(result);
-    return result == ESP_OK;
+    auto lock = getLock(port).asScopedLock();
+    lock.lock();
+    if (!dataArray[port].isConfigured) return false;
+    return i2c_controller_read(dataArray[port].device, address, data, dataSize, timeout) == ERROR_NONE;
 #else
     return false;
-#endif // ESP_PLATFORM
+#endif
 }
 
 bool masterReadRegister(i2c_port_t port, uint8_t address, uint8_t reg, uint8_t* data, size_t dataSize, TickType_t timeout) {
-    auto lock = getLock(port).asScopedLock();
-    if (!lock.lock(timeout)) {
-        LOGGER.error("({}) Mutex timeout", static_cast<int>(port));
-        return false;
-    }
-
 #ifdef ESP_PLATFORM
-    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
-    // Set address pointer
-    i2c_master_start(cmd);
-    i2c_master_write_byte(cmd, (address << 1) | I2C_MASTER_WRITE, ACK_CHECK_EN);
-    i2c_master_write(cmd, &reg, 1, ACK_CHECK_EN);
-    // Read length of response from current pointer
-    i2c_master_start(cmd);
-    i2c_master_write_byte(cmd, (address << 1) | I2C_MASTER_READ, ACK_CHECK_EN);
-    if (dataSize > 1) {
-        i2c_master_read(cmd, data, dataSize - 1, I2C_MASTER_ACK);
-    }
-    i2c_master_read_byte(cmd, data + dataSize - 1, I2C_MASTER_NACK);
-    i2c_master_stop(cmd);
-    // TODO: We're passing an inaccurate timeout value as we already lost time with locking
-    esp_err_t result = i2c_master_cmd_begin(port, cmd, timeout);
-    i2c_cmd_link_delete(cmd);
-
-    ESP_ERROR_CHECK_WITHOUT_ABORT(result);
-
-    return result == ESP_OK;
+    auto lock = getLock(port).asScopedLock();
+    lock.lock();
+    if (!dataArray[port].isConfigured) return false;
+    return i2c_controller_read_register(dataArray[port].device, address, reg, data, dataSize, timeout) == ERROR_NONE;
 #else
     return false;
-#endif // ESP_PLATFORM
+#endif
 }
 
 bool masterWrite(i2c_port_t port, uint8_t address, const uint8_t* data, uint16_t dataSize, TickType_t timeout) {
-    auto lock = getLock(port).asScopedLock();
-    if (!lock.lock(timeout)) {
-        LOGGER.error("({}) Mutex timeout", static_cast<int>(port));
-        return false;
-    }
-
 #ifdef ESP_PLATFORM
-    auto result = i2c_master_write_to_device(port, address, data, dataSize, timeout);
-    ESP_ERROR_CHECK_WITHOUT_ABORT(result);
-    return result == ESP_OK;
+    auto lock = getLock(port).asScopedLock();
+    lock.lock();
+    if (!dataArray[port].isConfigured) return false;
+    return i2c_controller_write(dataArray[port].device, address, data, dataSize, timeout) == ERROR_NONE;
 #else
     return false;
-#endif // ESP_PLATFORM
+#endif
 }
 
 bool masterWriteRegister(i2c_port_t port, uint8_t address, uint8_t reg, const uint8_t* data, uint16_t dataSize, TickType_t timeout) {
-    check(reg != 0);
-
-    auto lock = getLock(port).asScopedLock();
-    if (!lock.lock(timeout)) {
-        LOGGER.error("({}) Mutex timeout", static_cast<int>(port));
-        return false;
-    }
-
 #ifdef ESP_PLATFORM
-
-    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
-    i2c_master_start(cmd);
-    i2c_master_write_byte(cmd, (address << 1) | I2C_MASTER_WRITE, ACK_CHECK_EN);
-    i2c_master_write_byte(cmd, reg, ACK_CHECK_EN);
-    i2c_master_write(cmd, (uint8_t*) data, dataSize, ACK_CHECK_EN);
-    i2c_master_stop(cmd);
-    // TODO: We're passing an inaccurate timeout value as we already lost time with locking
-    esp_err_t result = i2c_master_cmd_begin(port, cmd, timeout);
-    i2c_cmd_link_delete(cmd);
-
-    ESP_ERROR_CHECK_WITHOUT_ABORT(result);
-    return result == ESP_OK;
+    auto lock = getLock(port).asScopedLock();
+    lock.lock();
+    if (!dataArray[port].isConfigured) return false;
+    return i2c_controller_write_register(dataArray[port].device, address, reg, data, dataSize, timeout) == ERROR_NONE;
 #else
     return false;
-#endif // ESP_PLATFORM
+#endif
 }
 
 bool masterWriteRegisterArray(i2c_port_t port, uint8_t address, const uint8_t* data, uint16_t dataSize, TickType_t timeout) {
 #ifdef ESP_PLATFORM
-    assert(dataSize % 2 == 0);
-    bool result = true;
-    for (int i = 0; i < dataSize; i += 2) {
-        // TODO: We're passing an inaccurate timeout value as we already lost time with locking and previous writes in this loop
-        if (!masterWriteRegister(port, address, data[i], &data[i + 1], 1, timeout)) {
-            result = false;
-        }
-    }
-    return result;
+    auto lock = getLock(port).asScopedLock();
+    lock.lock();
+    if (!dataArray[port].isConfigured) return false;
+    return i2c_controller_write_register_array(dataArray[port].device, address, data, dataSize, timeout) == ERROR_NONE;
 #else
     return false;
-#endif // ESP_PLATFORM
+#endif
 }
 
 bool masterWriteRead(i2c_port_t port, uint8_t address, const uint8_t* writeData, size_t writeDataSize, uint8_t* readData, size_t readDataSize, TickType_t timeout) {
-    auto lock = getLock(port).asScopedLock();
-    if (!lock.lock(timeout)) {
-        LOGGER.error("({}) Mutex timeout", static_cast<int>(port));
-        return false;
-    }
-
 #ifdef ESP_PLATFORM
-    esp_err_t result = i2c_master_write_read_device(port, address, writeData, writeDataSize, readData, readDataSize, timeout);
-    ESP_ERROR_CHECK_WITHOUT_ABORT(result);
-    return result == ESP_OK;
+    auto lock = getLock(port).asScopedLock();
+    lock.lock();
+    if (!dataArray[port].isConfigured) return false;
+    return i2c_controller_write_read(dataArray[port].device, address, writeData, writeDataSize, readData, readDataSize, timeout) == ERROR_NONE;
 #else
     return false;
-#endif // ESP_PLATFORM
+#endif
 }
 
 bool masterHasDeviceAtAddress(i2c_port_t port, uint8_t address, TickType_t timeout) {
-    auto lock = getLock(port).asScopedLock();
-    if (!lock.lock(timeout)) {
-        LOGGER.error("({}) Mutex timeout", static_cast<int>(port));
-        return false;
-    }
-
 #ifdef ESP_PLATFORM
-    uint8_t message[2] = { 0, 0 };
-    // TODO: We're passing an inaccurate timeout value as we already lost time with locking
-    return i2c_master_write_to_device(port, address, message, 2, timeout) == ESP_OK;
+    auto lock = getLock(port).asScopedLock();
+    lock.lock();
+    if (!dataArray[port].isConfigured) return false;
+    return i2c_controller_has_device_at_address(dataArray[port].device, address, timeout) == ERROR_NONE;
 #else
     return false;
-#endif // ESP_PLATFORM
+#endif
 }
 
 Lock& getLock(i2c_port_t port) {