diff --git a/radio/src/boards/generic_stm32/i2c_bus.cpp b/radio/src/boards/generic_stm32/i2c_bus.cpp
index 4160ea1cc3d..60f2c41846e 100644
--- a/radio/src/boards/generic_stm32/i2c_bus.cpp
+++ b/radio/src/boards/generic_stm32/i2c_bus.cpp
@@ -25,9 +25,27 @@
 #include "hal/i2c_driver.h"
 #include "hal.h"
 
+#if defined(FREE_RTOS)
+#include "os/task.h"
+#endif
+
 #define I2C_DEFAULT_TIMEOUT 10
 #define I2C_DEFAULT_RETRIES 2
 
+#if defined(FREE_RTOS)
+static mutex_handle_t _i2c_mutex[MAX_I2C_BUSES];
+static bool _i2c_mutex_initialized[MAX_I2C_BUSES] = {};
+
+static void i2c_ensure_mutex(uint8_t bus)
+{
+  if (bus < MAX_I2C_BUSES && !_i2c_mutex_initialized[bus] &&
+      scheduler_is_running()) {
+    mutex_create(&_i2c_mutex[bus]);
+    _i2c_mutex_initialized[bus] = true;
+  }
+}
+#endif
+
 #if defined(I2C_B1)
 static const stm32_i2c_hw_def_t _i2c1 = {
   .I2Cx = I2C_B1,
@@ -92,6 +110,35 @@ int i2c_deinit(etx_i2c_bus_t bus)
   return -1;
 }
 
+void i2c_lock(etx_i2c_bus_t bus)
+{
+#if defined(FREE_RTOS)
+  if (bus >= MAX_I2C_BUSES) return;
+  i2c_ensure_mutex(bus);
+  if (scheduler_is_running())
+    mutex_lock(&_i2c_mutex[bus]);
+#endif
+}
+
+bool i2c_trylock(etx_i2c_bus_t bus)
+{
+#if defined(FREE_RTOS)
+  if (bus >= MAX_I2C_BUSES) return false;
+  i2c_ensure_mutex(bus);
+  if (scheduler_is_running())
+    return mutex_trylock(&_i2c_mutex[bus]);
+#endif
+  return true;
+}
+
+void i2c_unlock(etx_i2c_bus_t bus)
+{
+#if defined(FREE_RTOS)
+  if (bus < MAX_I2C_BUSES && scheduler_is_running())
+    mutex_unlock(&_i2c_mutex[bus]);
+#endif
+}
+
 int i2c_dev_ready(etx_i2c_bus_t bus, uint16_t addr)
 {
   return stm32_i2c_is_dev_ready(bus, addr, I2C_DEFAULT_RETRIES,
diff --git a/radio/src/boards/jumper-h750/touch_driver.cpp b/radio/src/boards/jumper-h750/touch_driver.cpp
index 96ed51480e5..5a003635949 100644
--- a/radio/src/boards/jumper-h750/touch_driver.cpp
+++ b/radio/src/boards/jumper-h750/touch_driver.cpp
@@ -248,6 +248,8 @@ TouchState touchPanelRead()
 {
   if (!touchEventOccured) return internalTouchState;
 
+  i2c_lock(TOUCH_I2C_BUS);
+
   touchEventOccured = false;
 
   tmr10ms_t now = timersGetMsTick();
@@ -285,6 +287,7 @@ TouchState touchPanelRead()
   if (internalTouchState.event == TE_UP ||
       internalTouchState.event == TE_SLIDE_END)
     internalTouchState.event = TE_NONE;
+  i2c_unlock(TOUCH_I2C_BUS);
   return ret;
 }
 
diff --git a/radio/src/boards/rm-h750/bsp_io.cpp b/radio/src/boards/rm-h750/bsp_io.cpp
index 110a1ecfc44..a548f4f1715 100644
--- a/radio/src/boards/rm-h750/bsp_io.cpp
+++ b/radio/src/boards/rm-h750/bsp_io.cpp
@@ -24,6 +24,7 @@
 #include "stm32_rgbleds.h"
 #include "stm32_switch_driver.h"
 #include "stm32_i2c_driver.h"
+#include "hal/i2c_driver.h"
 #include "hal/switch_driver.h"
 #include "drivers/pca95xx.h"
 #include "timers_driver.h"
@@ -112,8 +113,12 @@ static void _poll_switches(void *param1, uint32_t trigger_source)
   // Suspend hardware reads when required
   if (suspendI2CTasks) return;
 
+  if (!i2c_trylock(IO_EXPANDER_I2C_BUS)) return;
+
   _read_io_expander(&_io_switches);
   _read_io_expander(&_io_fs_switches);
+
+  i2c_unlock(IO_EXPANDER_I2C_BUS);
 }
 
 static void _io_int_handler()
diff --git a/radio/src/boards/rm-h750/tp_gt911.cpp b/radio/src/boards/rm-h750/tp_gt911.cpp
index 833fc83f3ee..f9ca5c48870 100644
--- a/radio/src/boards/rm-h750/tp_gt911.cpp
+++ b/radio/src/boards/rm-h750/tp_gt911.cpp
@@ -232,6 +232,8 @@ struct TouchState touchPanelRead()
 
   if (!touchEventOccured) return internalTouchState;
 
+  i2c_lock(TOUCH_I2C_BUS);
+
   touchEventOccured = false;
 
   uint32_t startReadStatus = timersGetMsTick();
@@ -241,6 +243,7 @@ struct TouchState touchPanelRead()
       touchGT911hiccups++;
       TRACE("GT911 I2C read XY error");
       if (!I2C_ReInit()) TRACE("I2C B1 ReInit failed");
+      i2c_unlock(TOUCH_I2C_BUS);
       return internalTouchState;
     }
 
@@ -266,6 +269,7 @@ struct TouchState touchPanelRead()
         touchGT911hiccups++;
         TRACE("GT911 I2C data read error");
         if (!I2C_ReInit()) TRACE("I2C B1 ReInit failed");
+        i2c_unlock(TOUCH_I2C_BUS);
         return internalTouchState;
       }
         
@@ -316,6 +320,7 @@ struct TouchState touchPanelRead()
   }
 
   TRACE("touch event = %s", event2str(internalTouchState.event));
+  i2c_unlock(TOUCH_I2C_BUS);
   return internalTouchState;
 }
 
diff --git a/radio/src/cli.cpp b/radio/src/cli.cpp
index af6f62bd36c..823a17109f2 100644
--- a/radio/src/cli.cpp
+++ b/radio/src/cli.cpp
@@ -1845,6 +1845,45 @@ int cliResetGT911(const char** argv)
 }
 #endif
 
+#include "hal/i2c_driver.h"
+
+int cliI2C(const char** argv)
+{
+  if (!argv[1]) {
+    cliSerialPrint("Usage: i2c scan <bus>");
+    return 0;
+  }
+
+  if (!strcmp(argv[1], "scan")) {
+    int bus = 0;
+    if (toInt(argv, 2, &bus) < 0) {
+      cliSerialPrint("Usage: i2c scan <bus>");
+      return -1;
+    }
+    if (bus < 0 || bus >= MAX_I2C_BUSES) {
+      cliSerialPrint("Invalid I2C bus: %d", bus);
+      return -1;
+    }
+
+    auto i2cBus = (etx_i2c_bus_t)bus;
+    cliSerialPrint("Scanning I2C bus %d...", bus);
+    i2c_lock(i2cBus);
+    int found = 0;
+    for (int addr = 0x08; addr < 0x78; addr++) {
+      if (i2c_dev_ready(i2cBus, addr) >= 0) {
+        cliSerialPrint("  0x%02X: ACK", addr);
+        found++;
+      }
+    }
+    i2c_unlock(i2cBus);
+    cliSerialPrint("Found %d device(s)", found);
+    return 0;
+  }
+
+  cliSerialPrint("Unknown subcommand: %s", argv[1]);
+  return -1;
+}
+
 const CliCommand cliCommands[] = {
   { "beep", cliBeep, "[<frequency>] [<duration>]" },
   { "ls", cliLs, "<directory>" },
@@ -1871,6 +1910,7 @@ const CliCommand cliCommands[] = {
   { "repeat", cliRepeat, "<interval in ms> <command>" },
   { "testfatfs", cliTestFatFsSD, "" },
 #endif
+  { "i2c", cliI2C, "scan <bus>" },
   { "help", cliHelp, "[<command>]" },
 #if defined(JITTER_MEASURE)
   { "jitter", cliShowJitter, "" },
diff --git a/radio/src/drivers/csd203.cpp b/radio/src/drivers/csd203.cpp
new file mode 100644
index 00000000000..60080c7c5b4
--- /dev/null
+++ b/radio/src/drivers/csd203.cpp
@@ -0,0 +1,191 @@
+/*
+ * Copyright (C) EdgeTX
+ *
+ * Based on code named
+ *   opentx - https://github.com/opentx/opentx
+ *   th9x - http://code.google.com/p/th9x
+ *   er9x - http://code.google.com/p/er9x
+ *   gruvin9x - http://code.google.com/p/gruvin9x
+ *
+ * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include "csd203.h"
+
+#include "delays_driver.h"
+#include "hal/i2c_driver.h"
+#include "os/timer.h"
+
+// CSD203 register map
+#define CSD203_REG_CONFIG       0x00
+#define CSD203_REG_SHUNT_V     0x01
+#define CSD203_REG_BUS_V       0x02
+#define CSD203_REG_POWER       0x03
+#define CSD203_REG_CURRENT     0x04
+#define CSD203_REG_CALIBRATION 0x05
+#define CSD203_REG_MFR_ID      0xFE
+
+#define CSD203_MFR_ID_EXPECTED 0x4153
+
+// Calibration parameter: gain * 10000K
+#define CSD203_CAL_PARAM       51200
+
+// Config register fields (matching CSD203 datasheet)
+#define CFG_RST         (1 << 15)
+#define CFG_AVG_16      (2 << 9)
+#define CFG_VBUS_CT_1MS (4 << 5)
+#define CFG_VSHT_CT_1MS (4 << 2)
+#define CFG_MODE_CONT   7
+
+extern bool suspendI2CTasks;
+
+static int csd203_read_reg(csd203_t* dev, uint8_t reg, uint16_t* value)
+{
+  uint8_t buf[2];
+  if (i2c_read(dev->bus, dev->addr, reg, 1, buf, 2) < 0)
+    return -1;
+  *value = (buf[0] << 8) | buf[1];
+  return 0;
+}
+
+static int csd203_write_reg(csd203_t* dev, uint8_t reg, uint16_t value)
+{
+  uint8_t buf[2] = {(uint8_t)(value >> 8), (uint8_t)(value & 0xFF)};
+  return i2c_write(dev->bus, dev->addr, reg, 1, buf, 2);
+}
+
+int csd203_init(csd203_t* dev, etx_i2c_bus_t bus, uint16_t addr,
+                uint16_t rshunt, uint16_t current_lsb)
+{
+  dev->bus = bus;
+  dev->addr = addr;
+  dev->initialized = false;
+
+  uint16_t config = CFG_RST | CFG_AVG_16 | CFG_VBUS_CT_1MS |
+                    CFG_VSHT_CT_1MS | CFG_MODE_CONT;
+  if (csd203_write_reg(dev, CSD203_REG_CONFIG, config) < 0)
+    return -1;
+
+  uint16_t cal = CSD203_CAL_PARAM / (current_lsb * rshunt);
+  if (csd203_write_reg(dev, CSD203_REG_CALIBRATION, cal) < 0)
+    return -1;
+
+  delay_ms(1);
+
+  uint16_t mfr_id;
+  if (csd203_read_reg(dev, CSD203_REG_MFR_ID, &mfr_id) < 0)
+    return -1;
+
+  if (mfr_id != CSD203_MFR_ID_EXPECTED)
+    return -1;
+
+  // Dummy current read to clear conversion-ready flag
+  uint16_t dummy;
+  csd203_read_reg(dev, CSD203_REG_CURRENT, &dummy);
+
+  dev->initialized = true;
+  return 0;
+}
+
+uint16_t csd203_read_voltage(csd203_t* dev)
+{
+  uint16_t val = 0;
+  csd203_read_reg(dev, CSD203_REG_BUS_V, &val);
+  return val;
+}
+
+uint16_t csd203_read_current(csd203_t* dev)
+{
+  uint16_t val = 0;
+  csd203_read_reg(dev, CSD203_REG_CURRENT, &val);
+  return val;
+}
+
+// Board-level: 3 CSD203 sensors (main, internal, external)
+// Addresses: A1=VS/A0=GND (0x44), A1=VS/A0=VS (0x45), A1=GND/A0=VS (0x41)
+#define CSD203_ADDR_MAIN     0x44
+#define CSD203_ADDR_INTERNAL 0x45
+#define CSD203_ADDR_EXTERNAL 0x41
+
+// Shunt resistor 10 mOhm, current LSB 1 mA
+#define CSD203_RSHUNT      10
+#define CSD203_CURRENT_LSB 10
+
+static csd203_t csd203_main;
+static csd203_t csd203_internal;
+static csd203_t csd203_external;
+
+static etx_i2c_bus_t csd203_bus;
+static uint16_t csd203_ext_vbus;
+static timer_handle_t csd203_timer = TIMER_INITIALIZER;
+
+#define CSD203_POLL_PERIOD_MS 10
+
+static void csd203TimerCb(timer_handle_t* timer)
+{
+  (void)timer;
+  static uint16_t step = 0;
+
+  if (suspendI2CTasks) return;
+  if (!i2c_trylock(csd203_bus)) return;
+
+  if (step == 0 && csd203_main.initialized) {
+    csd203_read_current(&csd203_main);
+    csd203_read_voltage(&csd203_main);
+  } else if (step == 1 && csd203_internal.initialized) {
+    csd203_read_current(&csd203_internal);
+    csd203_read_voltage(&csd203_internal);
+  } else if (step == 2 && csd203_external.initialized) {
+    csd203_read_current(&csd203_external);
+    csd203_ext_vbus = (uint16_t)(csd203_read_voltage(&csd203_external) * 1.25f);
+  }
+
+  i2c_unlock(csd203_bus);
+  if (++step >= 3) step = 0;
+}
+
+void csd203_start(etx_i2c_bus_t bus)
+{
+  csd203_bus = bus;
+
+  if (i2c_init(bus) < 0)
+    return;
+
+  csd203_init(&csd203_main, bus, CSD203_ADDR_MAIN,
+              CSD203_RSHUNT, CSD203_CURRENT_LSB);
+
+  delay_ms(5);
+  csd203_init(&csd203_internal, bus, CSD203_ADDR_INTERNAL,
+              CSD203_RSHUNT, CSD203_CURRENT_LSB);
+
+  delay_ms(5);
+  csd203_init(&csd203_external, bus, CSD203_ADDR_EXTERNAL,
+              CSD203_RSHUNT, CSD203_CURRENT_LSB);
+
+  // Prime the external Vbus cache so getBatteryVoltage() doesn't report a
+  // false 0V before the timer's first external-sensor poll. Runs pre-scheduler,
+  // so no bus lock is needed (matches the init reads above).
+  if (csd203_external.initialized)
+    csd203_ext_vbus = (uint16_t)(csd203_read_voltage(&csd203_external) * 1.25f);
+
+  if (csd203_main.initialized || csd203_internal.initialized ||
+      csd203_external.initialized) {
+    timer_create(&csd203_timer, csd203TimerCb, "csd203",
+                 CSD203_POLL_PERIOD_MS, true);
+    timer_start(&csd203_timer);
+  }
+}
+
+uint16_t getBatteryVoltage()
+{
+  return csd203_ext_vbus / 10;
+}
diff --git a/radio/src/targets/common/arm/stm32/csd203_sensor.h b/radio/src/drivers/csd203.h
similarity index 67%
rename from radio/src/targets/common/arm/stm32/csd203_sensor.h
rename to radio/src/drivers/csd203.h
index 023ce01ee14..cc57cbaf325 100644
--- a/radio/src/targets/common/arm/stm32/csd203_sensor.h
+++ b/radio/src/drivers/csd203.h
@@ -21,6 +21,18 @@
 
 #pragma once
 
-void IICcsd203init(void);
-void initCSD203(void);
-void readCSD203(void);
+#include "hal/i2c_driver.h"
+
+typedef struct {
+  etx_i2c_bus_t bus;
+  uint16_t addr;
+  bool initialized;
+} csd203_t;
+
+int csd203_init(csd203_t* dev, etx_i2c_bus_t bus, uint16_t addr,
+                uint16_t rshunt, uint16_t current_lsb);
+
+uint16_t csd203_read_voltage(csd203_t* dev);
+uint16_t csd203_read_current(csd203_t* dev);
+
+void csd203_start(etx_i2c_bus_t bus);
diff --git a/radio/src/drivers/tas2505.cpp b/radio/src/drivers/tas2505.cpp
index 7cc93b19aba..c52a5c9a93a 100644
--- a/radio/src/drivers/tas2505.cpp
+++ b/radio/src/drivers/tas2505.cpp
@@ -23,6 +23,7 @@
 #include "stm32_i2s.h"
 #include "timers_driver.h"
 #include "hal/audio_driver.h"
+#include "hal/i2c_driver.h"
 
 #include "debug.h"
 
@@ -139,6 +140,8 @@ void tas2505_set_volume(tas2505_t* dev, uint8_t volume, bool headphone_mode = fa
   if (volume > VOLUME_LEVEL_MAX) {
     volume = VOLUME_LEVEL_MAX;
   }
+
+  i2c_lock(dev->bus);
   // maximum volume is 0x00 and total silence is 0xFE
   if (volume == 0) {
     tas2505_write_reg(dev, TAS2505_SPKVOL1, 0xFE);
@@ -154,4 +157,5 @@ void tas2505_set_volume(tas2505_t* dev, uint8_t volume, bool headphone_mode = fa
       tas2505_write_reg(dev, TAS2505_HP_VOL, 0xFE);
     }
   }
+  i2c_unlock(dev->bus);
 }
diff --git a/radio/src/edgetx.cpp b/radio/src/edgetx.cpp
index c269d438c77..bb5d9d59452 100644
--- a/radio/src/edgetx.cpp
+++ b/radio/src/edgetx.cpp
@@ -68,9 +68,6 @@
 #include "telemetry/crossfire.h"
 #endif
 
-#if defined(CSD203_SENSOR)
-  #include "csd203_sensor.h"
-#endif
 
 #if !defined(SIMU)
 #include <malloc.h>
@@ -223,10 +220,6 @@ void per10ms()
   }
 #endif
 
-#if defined(CSD203_SENSOR) && !defined(SIMU)
-  readCSD203();
-#endif
-
   telemetryInterrupt10ms();
 
   // These moved here from evalFlightModeMixes() to improve beep trigger reliability.
diff --git a/radio/src/gyro.cpp b/radio/src/gyro.cpp
index f623c221a76..f157d8b085f 100644
--- a/radio/src/gyro.cpp
+++ b/radio/src/gyro.cpp
@@ -21,23 +21,29 @@
 
 #include "edgetx.h"
 #include "hal.h"
+#include "hal/i2c_driver.h"
 #include "hal/usb_driver.h"
+#include "os/timer.h"
 
 #define _USE_MATH_DEFINES
 #include <math.h>
 
 #define COMPLEMENTARY_ALPHA  0.92f
 #define LP_FILTER_ALPHA      0.9f
-#define SAMPLE_TIME_S        0.005f
+#define SAMPLE_TIME_S        0.01f
+#define GYRO_POLL_PERIOD_MS  10
 
 int16_t gyroOutputs[2];
 
 static imu_read_fn readFn;
+static etx_i2c_bus_t i2cBus;
 static uint8_t errors;
 static int16_t offset_x, offset_y;
 static int16_t range_x = 8192, range_y = 8192;
 static int16_t raw_ax, raw_ay;
 
+static timer_handle_t _gyro_timer = TIMER_INITIALIZER;
+
 // filter state
 static bool filterInitialized;
 static float roll, pitch, yaw;
@@ -102,24 +108,21 @@ static void gyroFilter(etx_imu_data_t* raw)
   }
 }
 
-void gyroStart(imu_read_fn fn)
-{
-  readFn = fn;
-}
-
-void gyroWakeup()
+static void gyroTimerCb(timer_handle_t* timer)
 {
-  static tmr10ms_t gyroWakeupTime = 0;
+  (void)timer;
 
-  tmr10ms_t now = get_tmr10ms();
-  if (!readFn || errors >= 100 || now < gyroWakeupTime ||
-      usbPluggedInStorageMode())
+  if (!readFn || errors >= 100 || usbPluggedInStorageMode())
     return;
 
-  gyroWakeupTime = now + 1; /* 10ms default */
+  if (!i2c_trylock(i2cBus))
+    return;
 
   etx_imu_data_t raw;
-  if (readFn(&raw) < 0) {
+  int rc = readFn(&raw);
+  i2c_unlock(i2cBus);
+
+  if (rc < 0) {
     ++errors;
     return;
   }
@@ -138,6 +141,26 @@ void gyroWakeup()
   gyroOutputs[1] = (ay * float(RESX)) / range_y;
 }
 
+void gyroStart(const etx_imu_t* imu)
+{
+  if (imu) {
+    readFn = imu->driver->read;
+    i2cBus = imu->bus;
+
+    timer_create(&_gyro_timer, gyroTimerCb, "gyro",
+                 GYRO_POLL_PERIOD_MS, true);
+    timer_start(&_gyro_timer);
+  } else {
+    readFn = nullptr;
+  }
+}
+
+void gyroStop()
+{
+  timer_stop(&_gyro_timer);
+  readFn = nullptr;
+}
+
 int16_t gyroScaledX()
 {
   return limit<int16_t>(-RESX, gyroOutputs[0], RESX);
diff --git a/radio/src/gyro.h b/radio/src/gyro.h
index e852ed92542..b1104058288 100644
--- a/radio/src/gyro.h
+++ b/radio/src/gyro.h
@@ -29,8 +29,8 @@
 #define IMU_OFFSET_MIN       -30
 #define IMU_OFFSET_MAX        10
 
-void gyroStart(imu_read_fn fn);
-void gyroWakeup();
+void gyroStart(const etx_imu_t* imu);
+void gyroStop();
 void gyroSetIMU_X(int16_t offset, int16_t range);
 void gyroSetIMU_Y(int16_t offset, int16_t range);
 int16_t gyroScaledX();
diff --git a/radio/src/hal/eeprom/eeprom_driver.cpp b/radio/src/hal/eeprom/eeprom_driver.cpp
index 755bfd99c4b..b87ce79acb7 100644
--- a/radio/src/hal/eeprom/eeprom_driver.cpp
+++ b/radio/src/hal/eeprom/eeprom_driver.cpp
@@ -93,9 +93,13 @@ static bool I2C_EE_ReadBlock(uint8_t* pBuffer, uint16_t ReadAddr, uint16_t NumBy
 
 void eepromReadBlock(uint8_t * buffer, size_t address, size_t size)
 {
+  i2c_lock(EEPROM_I2C_BUS);
   while (!I2C_EE_ReadBlock(buffer, address, size)) {
+    i2c_unlock(EEPROM_I2C_BUS);
     eepromInit();
+    i2c_lock(EEPROM_I2C_BUS);
   }
+  i2c_unlock(EEPROM_I2C_BUS);
 }
 
 /**
@@ -134,6 +138,7 @@ static void eepromPageWrite(uint8_t* pBuffer, uint16_t WriteAddr, uint8_t NumByt
   */
 void eepromWriteBlock(uint8_t * buffer, size_t address, size_t size)
 {
+  i2c_lock(EEPROM_I2C_BUS);
   uint8_t offset = address % EEPROM_PAGESIZE;
   uint8_t count = EEPROM_PAGESIZE - offset;
   if (size < count) {
@@ -150,6 +155,7 @@ void eepromWriteBlock(uint8_t * buffer, size_t address, size_t size)
       count = size;
     }
   }
+  i2c_unlock(EEPROM_I2C_BUS);
 }
 
 uint8_t eepromIsTransferComplete()
diff --git a/radio/src/hal/i2c_driver.h b/radio/src/hal/i2c_driver.h
index ec74af34926..67d89dde5ae 100644
--- a/radio/src/hal/i2c_driver.h
+++ b/radio/src/hal/i2c_driver.h
@@ -25,9 +25,23 @@
 
 typedef uint8_t etx_i2c_bus_t;
 
+// Maximum number of I2C buses addressable via etx_i2c_bus_t; valid bus ids are
+// 0 .. MAX_I2C_BUSES - 1.
+#define MAX_I2C_BUSES 2
+
 int i2c_init(etx_i2c_bus_t bus);
 int i2c_deinit(etx_i2c_bus_t bus);
 
+// Explicit bus locking — caller must lock before transfer functions.
+//  - i2c_lock: blocking, for regular task context
+//  - i2c_trylock: non-blocking, for timer-callback/task context (returns false
+//    if busy); task-only, not ISR-safe (uses xSemaphoreTake, no FromISR path)
+//  - i2c_unlock: release
+// All are no-ops before the RTOS scheduler starts.
+void i2c_lock(etx_i2c_bus_t bus);
+bool i2c_trylock(etx_i2c_bus_t bus);
+void i2c_unlock(etx_i2c_bus_t bus);
+
 int i2c_dev_ready(etx_i2c_bus_t bus, uint16_t addr);
 
 int i2c_read(uint8_t bus, uint16_t addr, uint16_t reg, uint16_t reg_size,
diff --git a/radio/src/hal/imu.cpp b/radio/src/hal/imu.cpp
index 273fe3c9bd6..4ec48d75930 100644
--- a/radio/src/hal/imu.cpp
+++ b/radio/src/hal/imu.cpp
@@ -21,20 +21,24 @@
 
 #include "hal/imu.h"
 
-static const char* s_imu_name = nullptr;
+// Holds a copy of the matched candidate so imuDetect() can return a pointer
+// that outlives the (often stack-local) candidates array. A null driver means
+// nothing was detected.
+static etx_imu_t s_detected = {};
 
-imu_read_fn imuDetect(const etx_imu_t* candidates, uint8_t count)
+const etx_imu_t* imuDetect(const etx_imu_t* candidates, uint8_t count)
 {
   for (uint8_t i = 0; i < count; i++) {
     if (candidates[i].driver->init(candidates[i].bus, candidates[i].addr) == 0) {
-      s_imu_name = candidates[i].driver->name;
-      return candidates[i].driver->read;
+      s_detected = candidates[i];
+      return &s_detected;
     }
   }
+  s_detected = {};
   return nullptr;
 }
 
 const char* imuGetName()
 {
-  return s_imu_name;
+  return s_detected.driver ? s_detected.driver->name : nullptr;
 }
diff --git a/radio/src/hal/imu.h b/radio/src/hal/imu.h
index 9aa70ed06ec..39921d48cf7 100644
--- a/radio/src/hal/imu.h
+++ b/radio/src/hal/imu.h
@@ -44,8 +44,10 @@ struct etx_imu_t {
   uint16_t addr;
 };
 
-// Generic detection: iterates candidates, returns read fn on success
-imu_read_fn imuDetect(const etx_imu_t* candidates, uint8_t count);
+// Generic detection: iterates candidates, returns the matched IMU on success
+// (or nullptr if none responded). The returned pointer stays valid until the
+// next imuDetect() call.
+const etx_imu_t* imuDetect(const etx_imu_t* candidates, uint8_t count);
 
 // Returns the name of the detected IMU, or nullptr if none
 const char* imuGetName();
diff --git a/radio/src/mixer.cpp b/radio/src/mixer.cpp
index 9aa7f3782b3..c96bd8dee32 100644
--- a/radio/src/mixer.cpp
+++ b/radio/src/mixer.cpp
@@ -35,9 +35,6 @@
 #include "luminosity_sensor.h"
 #endif
 
-#if defined(RADIO_GX12)
-#include "targets/taranis/gx12/bsp_io.h"
-#endif
 #define DELAY_POS_MARGIN   3
 
 uint8_t s_mixer_first_run_done = false;
@@ -1147,11 +1144,6 @@ void evalMixes(uint8_t tick10ms)
   static uint16_t delta = 0;
   static uint16_t flightModesFade = 0;
 
-#if defined(RADIO_GX12)
-  // see #6159
-  _poll_switches();
-#endif
-
   uint8_t fm = getFlightMode();
 
   if (lastFlightMode != fm) {
diff --git a/radio/src/os/task.h b/radio/src/os/task.h
index e1a62ed931a..ae70e7372d2 100644
--- a/radio/src/os/task.h
+++ b/radio/src/os/task.h
@@ -44,77 +44,3 @@ void mutex_create(mutex_handle_t* h);
 bool mutex_lock(mutex_handle_t* h);
 void mutex_unlock(mutex_handle_t* h);
 bool mutex_trylock(mutex_handle_t* h);
-
-
-// this helper class is only to be used on the stack and never to be shared outside the scope where it was created
-/* example usage
- *
- * void doWork()
- * {
- *
- *   MutexLock lock = MutexLock::MakeInstance(mutex);
- *   while(!queue.isEmpty)
- *   {
- *     lock.unlock();
- *     auto item = queue.pop();
- *     <do something with item>
- *     lock.lock();
- *   }
- * } // auto mutex unlock, because lock is destructed here
- *
- * int sendMessage()
- * {
- *   MutexLock lock = MutexLock::MakeInstance(busMutex);
- *   if(busSendData(...) != OK)
- *     return ERROR; // auto mutex unlock, because lock is destructed here
- *
- *   lock.unlock();
- *   <do some other stuff>
- *   if(<something>)
- *     return ERROR; // no auto mutex unlock, because already unlocked
- *   lock.lock()
- *   if(busSendData(...) != OK)
- *     return ERROR; // auto mutex unlock, because lock is destructed here
- *
- *   return OK;
- * } // auto mutex unlock here, because lock is destructed here
- *
- * int doSomething()
- * {
- *
- *   <code here>
- *   if(<bus usage condition>)
- *   {
- *     MutexLock lock = MutexLock::MakeInstance(busMutex);
- *     if(busSendData(...) != OK)
- *       return ERROR; // auto mutex unlock, because lock is destructed here
- *   } // auto mutex unlock here, because lock is destructed here
- *
- *   <code here>
- *
- *   return OK;
- * }
- */
-
-class MutexLock
-{
-public:
-  static MutexLock MakeInstance(mutex_handle_t* mtx) {return MutexLock(mtx);}
-  ~MutexLock() {if(!scheduler_is_running()) return; if(locked) mutex_unlock(mutex);}
-  void lock() {if(!scheduler_is_running()) return; if(!locked) mutex_lock(mutex); locked=true;}
-  void unlock() {if(!scheduler_is_running()) return; if(locked) mutex_unlock(mutex); locked=false;}
-
-  MutexLock(const MutexLock&)               = delete;
-  MutexLock(MutexLock&&)                    = delete;
-  MutexLock& operator   =(const MutexLock&) = delete;
-  MutexLock& operator   =(MutexLock&&)      = delete;
-  static void* operator new     (size_t)    = delete;
-  static void* operator new[]   (size_t)    = delete;
-  static void  operator delete  (void*)     = delete;
-  static void  operator delete[](void*)     = delete;
-private:
-  MutexLock(mutex_handle_t* mtx):mutex(mtx),locked(false) {if(!scheduler_is_running()) return; mutex_lock(mutex); locked = true;}
-  mutex_handle_t* mutex;
-  bool locked;
-};
-
diff --git a/radio/src/pdm_wav_recorder.cpp b/radio/src/pdm_wav_recorder.cpp
index 8b0d646a2b9..a2163ce4492 100644
--- a/radio/src/pdm_wav_recorder.cpp
+++ b/radio/src/pdm_wav_recorder.cpp
@@ -27,12 +27,11 @@
 
 #include "os/task.h"
 
-// Guards against concurrent access from the audio task and the owner thread.
-static PdmWavRecorder* s_active = nullptr;
-static mutex_handle_t s_mutex;
-static bool s_mutexInited = false;
+PdmWavRecorder* PdmWavRecorder::s_active = nullptr;
+mutex_handle_t PdmWavRecorder::s_mutex;
+bool PdmWavRecorder::s_mutexInited = false;
 
-static void ensureMutex()
+void PdmWavRecorder::ensureMutex()
 {
   if (!s_mutexInited) {
     mutex_create(&s_mutex);
@@ -56,9 +55,16 @@ static void writeLE32(uint8_t* p, uint32_t v)
 
 FRESULT PdmWavRecorder::start(const char* path, uint32_t expectedSeconds)
 {
+  // Owner thread (CLI/GUI menu task): blocking lock is fine here.
   ensureMutex();
-  MutexLock lock = MutexLock::MakeInstance(&s_mutex);
+  mutex_lock(&s_mutex);
+  FRESULT res = startLocked(path, expectedSeconds);
+  mutex_unlock(&s_mutex);
+  return res;
+}
 
+FRESULT PdmWavRecorder::startLocked(const char* path, uint32_t expectedSeconds)
+{
   if (s_active != nullptr) return FR_LOCKED;
 
   samplesWritten = 0;
@@ -124,18 +130,25 @@ bool PdmWavRecorder::tickLocked()
 
 void PdmWavRecorder::audioTick()
 {
-  if (s_active == nullptr) return;
   ensureMutex();
-  MutexLock lock = MutexLock::MakeInstance(&s_mutex);
+  // Real-time audio task: never block on the owner thread's SD I/O. If start()/
+  // stop() holds the lock, skip this chunk rather than stall the audio task.
+  // s_active is read under the lock — start()/stop() write it under the lock too.
+  if (!mutex_trylock(&s_mutex)) return;
   if (s_active != nullptr) s_active->tickLocked();
+  mutex_unlock(&s_mutex);
 }
 
 FRESULT PdmWavRecorder::stop()
 {
+  // Owner thread (CLI/GUI menu task): blocking lock is fine here.
   ensureMutex();
-  MutexLock lock = MutexLock::MakeInstance(&s_mutex);
+  mutex_lock(&s_mutex);
 
-  if (s_active != this) return FR_OK;
+  if (s_active != this) {
+    mutex_unlock(&s_mutex);
+    return FR_OK;
+  }
 
   s_active = nullptr;
   recording = false;
@@ -152,7 +165,9 @@ FRESULT PdmWavRecorder::stop()
   f_lseek(&file, 40);
   f_write(&file, buf, 4, &written);
 
-  return f_close(&file);
+  FRESULT res = f_close(&file);
+  mutex_unlock(&s_mutex);
+  return res;
 }
 
 FRESULT PdmWavRecorder::trimSilence(const char* path)
diff --git a/radio/src/pdm_wav_recorder.h b/radio/src/pdm_wav_recorder.h
index 5eec2ece5eb..040f27882b5 100644
--- a/radio/src/pdm_wav_recorder.h
+++ b/radio/src/pdm_wav_recorder.h
@@ -27,6 +27,7 @@
 
 #include <stdint.h>
 #include "ff.h"
+#include "os/task.h"
 
 // 16 kHz: PDM_CLOCK_FREQ(1.6 MHz)/PDM_PCM_DECIMATION(100) = 16000 Hz directly.
 // Must also divide AUDIO_SAMPLE_RATE(32 kHz) evenly for the WAV player.
@@ -54,6 +55,16 @@ class PdmWavRecorder
  private:
   static constexpr uint32_t PCM_MAX = 256;
 
+  // Guards the global "which recorder is active" selection (s_active) and its
+  // file against concurrent access from the audio task and the owner thread.
+  // Shared (static), not per-instance: audioTick() must take the lock before it
+  // can even safely read s_active to find the active instance.
+  static PdmWavRecorder* s_active;
+  static mutex_handle_t s_mutex;
+  static bool s_mutexInited;
+  static void ensureMutex();
+
+  FRESULT startLocked(const char* path, uint32_t expectedSeconds);
   bool tickLocked();
 
   FIL file;
diff --git a/radio/src/switches.cpp b/radio/src/switches.cpp
index 8f7327ef73d..21af340ffea 100644
--- a/radio/src/switches.cpp
+++ b/radio/src/switches.cpp
@@ -32,9 +32,6 @@
 #include "inactivity_timer.h"
 #include "tasks/mixer_task.h"
 
-#if defined(RADIO_GX12)
-#include "targets/taranis/gx12/bsp_io.h"
-#endif
 
 #define CS_LAST_VALUE_INIT -32768
 
@@ -938,9 +935,6 @@ void checkSwitches()
 #endif
 
   while (true) {
-#if defined(RADIO_GX12)
-    _poll_switches();
-#endif
     if (!isSwitchWarningRequired(bad_pots))
       break;
 
diff --git a/radio/src/targets/common/arm/stm32/CMakeLists.txt b/radio/src/targets/common/arm/stm32/CMakeLists.txt
index c5beb411617..17bc41857d8 100644
--- a/radio/src/targets/common/arm/stm32/CMakeLists.txt
+++ b/radio/src/targets/common/arm/stm32/CMakeLists.txt
@@ -161,7 +161,7 @@ endif()
 
 if(CSD203_SENSOR)
   set(FIRMWARE_SRC ${FIRMWARE_SRC}
-  targets/common/arm/stm32/csd203_sensor.cpp
+  drivers/csd203.cpp
   )
   add_definitions(-DCSD203_SENSOR)
   message("-- Adding support for Current sensor")
diff --git a/radio/src/targets/common/arm/stm32/csd203_sensor.cpp b/radio/src/targets/common/arm/stm32/csd203_sensor.cpp
deleted file mode 100644
index bb4b0f7daff..00000000000
--- a/radio/src/targets/common/arm/stm32/csd203_sensor.cpp
+++ /dev/null
@@ -1,433 +0,0 @@
-/*
- * Copyright (C) EdgeTX
- *
- * Based on code named
- *   opentx - https://github.com/opentx/opentx
- *   th9x - http://code.google.com/p/th9x
- *   er9x - http://code.google.com/p/er9x
- *   gruvin9x - http://code.google.com/p/gruvin9x
- *
- * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- */
-
-#include "csd203_sensor.h"
-
-#include <stdlib.h>
-#include <string.h>
-
-#include "debug.h"
-#include "delays_driver.h"
-#include "edgetx_types.h"
-#include "hal.h"
-#include "rtos.h"
-#include "stm32_exti_driver.h"
-#include "stm32_gpio_driver.h"
-#include "stm32_hal.h"
-#include "stm32_hal_ll.h"
-#include "stm32_i2c_driver.h"
-#include "hal/i2c_driver.h"
-
-// clang-format off
-/* CSD203 Regsistor map */
-#define CONFIGURATION                   0X00
-/*Read Only*/
-#define SHUNT_VOLTAGE                   0X01
-#define BUS_VOLTAGE                     0X02
-#define POWER                           0X03
-#define CURRENT                         0x04
-/*Read Only*/
-#define CALIBRATION                     0x05
-#define MASKENABLE                      0x06
-#define ALERTLIMIT                      0x07
-
-#define ManufacturerID               	0xFE	//Manufacturer ID Register
-#define DieID 		                	0xFF	//Die ID Register
-
-#define ManIDCode 		                0x4153	//CHIP ID
-
-/*Calibration Calculation parameter*/
-/* This Parameter Gain*10000K*/
-#define CalParam                        51200
-/* ↓ add your shunt here ↓   */
-#define CSD_CONFIG_Rs_1mR			    1
-#define CSD_CONFIG_Rs_2mR		    	2
-#define CSD_CONFIG_Rs_5mR		    	5
-#define CSD_CONFIG_Rs_10mR		    	10
-#define CSD_CONFIG_Rs_20mR		    	20
-#define CSD_CONFIG_Rs_50mR		    	50
-#define CSD_CONFIG_CurrentLsb1mA    	10
-#define CSD_CONFIG_CurrentLsb2mA    	20
-#define CSD_CONFIG_CurrentLsb5mA    	50
-#define CSD_CONFIG_CurrentLsb10mA    	100
-#define CSD_CONFIG_CurrentLsb20mA  	    200
-/*Calibration Calculation parameter*/
-
-/* CSD203 Regsistor Config*/
-#define CSD_CONFIG_RST 		 	    	1
-#define CSD_CONFIG_UnRST 	 	    	0
-
-#define CSD_CONFIG_Avg1			    	0
-#define CSD_CONFIG_Avg4			    	1
-#define CSD_CONFIG_Avg16		     	2
-#define CSD_CONFIG_Avg64	    		3
-#define CSD_CONFIG_Avg128		    	4
-#define CSD_CONFIG_Avg256	    		5
-#define CSD_CONFIG_Avg512   			6
-#define CSD_CONFIG_Avg1024			    7
-
-#define CSD_CONFIG_VBUS_CT1_1mS 	    4
-#define CSD_CONFIG_VShunt_CT1_1mS   	4
-
-#define CSD_CONFIG_ShuntBus_CON     	7
-
-#define CSD_CONFIG_ADDR_A1_GND_A0_GND 	64
-#define CSD_CONFIG_ADDR_A1_GND_VS_GND 	65
-#define CSD_CONFIG_ADDR_A1_GND_SDA_GND 	66
-#define CSD_CONFIG_ADDR_A1_GND_SCL_GND 	67
-#define CSD_CONFIG_ADDR_A1_VS_A0_GND 	68
-#define CSD_CONFIG_ADDR_A1_VS_A0_VS 	69
-#define CSD_CONFIG_ADDR_A1_VS_A0_SDA 	70
-#define CSD_CONFIG_ADDR_A1_VS_A0_SCL 	71
-#define CSD_CONFIG_ADDR_A1_SDA_A0_GND 	72
-#define CSD_CONFIG_ADDR_A1_SDA_A0_VS 	73
-#define CSD_CONFIG_ADDR_A1_SDA_A0_SDA 	74
-#define CSD_CONFIG_ADDR_A1_SDA_A0_SCL 	75
-#define CSD_CONFIG_ADDR_A1_SCL_A0_GND 	76
-#define CSD_CONFIG_ADDR_A1_SCL_A0_VS 	77
-#define CSD_CONFIG_ADDR_A1_SCL_A0_SDA 	78
-#define CSD_CONFIG_ADDR_A1_SCL_A0_SCL 	79
-
-/******************/
-/*CSD Alert Option*/
-/******************/
-/*Vshunt Voltage Over Voltage*/
-#define CSD_ALERT_VShunt_OVA_ON 		1
-#define CSD_ALERT_VShunt_OVA_OFF 		0
-/*Vshunt Voltage Under Voltage*/
-#define CSD_ALERT_VShunt_UVA_ON 		1
-#define CSD_ALERT_VShunt_UVA_OFF 		0
-/*VBus Voltage Over Voltage*/
-#define CSD_ALERT_VBUS_OVA_ON 			1
-#define CSD_ALERT_VBUS_OVA_OFF 			0
-/*VBus Voltage Under Voltage*/
-#define CSD_ALERT_VBUS_UVA_ON 			1
-#define CSD_ALERT_VBUS_UVA_OFF 			0
-/* Power Over Limit */
-#define CSD_ALERT_Power_Over_ON 		1
-#define CSD_ALERT_Power_Over_OFF 		0
-/* ADC Coversion Ready */
-#define CSD_ALERT_CoversionReady_ON 	1
-#define CSD_ALERT_CoversionReady_OFF 	0
-#define CSD_ALERT_CNVR_FLAG_ON 			1
-#define CSD_ALERT_CNVR_FLAG_OFF 		0
-/*  ALERT Function Flag and Alert latch Work Together */
-#define CSD_ALERT_ALERT_FLAG_ON 		1
-#define CSD_ALERT_ALERT_FLAG_OFF 		0
-#define CSD_ALERT_ALERT_Latch_ON 		1
-#define CSD_ALERT_ALERT_Latch_OFF 		0
-/*Power over flow */
-#define CSD_ALERT_OVF_ON 				1
-#define CSD_ALERT_OVF_OFF 				0
-/*Alert Pin */
-#define CSD_ALERT_APOL_ActiveLow 		1
-#define CSD_ALERT_APOL_ActiveHigh 		0
-// clang-format on
-
-/*CSD Basic Configuration struct*/
-typedef struct {
-  uint8_t DeviceADDR;
-  uint8_t RST;
-  uint8_t Average;
-  uint8_t VBUS_Conv_Time;
-  uint8_t VShunt_Conv_Time;
-  uint8_t Mode;
-  uint16_t CurrentLSB;
-  uint16_t Rshunt;
-} CSD_CONFIG;
-/*CSD Basic Configuration struct*/
-
-/*CSD ALERT Configuration struct*/
-typedef struct {
-  uint8_t VShunt_OVA;
-  uint8_t VShuntUVA;
-  uint8_t VBUS_OVA;
-  uint8_t VBUS_UVA;
-  uint8_t Power_OVER_Limit;
-  uint8_t CNVR;
-  uint8_t ALERT_FLAG;
-  uint8_t CNVR_FLAG;
-  uint8_t OVF;
-  uint8_t APOL;
-  uint8_t ALERT_Latch;
-} CSD_ALERT;
-/*CSD ALERT Configuration struct*/
-
-extern bool suspendI2CTasks;
-
-void CSD203_Init(CSD_CONFIG *CSD203_CFG);            // Initialise CSD203 config
-uint16_t CSD203_ReadVbus(CSD_CONFIG *CSD203_CFG);    // Read Vbus Voltage
-uint16_t CSD203_ReadRshunt(CSD_CONFIG *CSD203_CFG);  // Read Rshunt
-uint16_t CSD203_ReadPower(CSD_CONFIG *CSD203_CFG);   // Read Power
-uint16_t CSD203_ReadCurrent(CSD_CONFIG *CSD203_CFG);  // Read Current
-
-void IIC_DUT_W(uint8_t addr, uint8_t reg, uint16_t data);
-uint16_t IIC_DUT_R(uint8_t addr, uint8_t reg);
-
-bool CSD203MainInitFlag = false;
-bool CSD203InInitFlag = false;
-bool CSD203ExtInitFlag = false;
-
-bool IICReadStatusFlag = false;
-
-static uint16_t csd203extvbus = 0;
-
-static CSD_CONFIG CSD203_MainSensorCFG;
-static CSD_CONFIG CSD203_InSensorCFG;
-static CSD_CONFIG CSD203_ExtSensorCFG;
-
-bool I2C_CSD203_WriteRegister(uint8_t I2C_ADDR, uint8_t reg, uint8_t *buf,
-                              uint8_t len)
-{
-  uint8_t uAddrAndBuf[6];
-  uAddrAndBuf[0] = (uint8_t)(reg & 0x00FF);
-
-  if (len > 0) {
-    for (int i = 0; i < len; i++) {
-      uAddrAndBuf[i + 1] = buf[i];
-    }
-  }
-
-  if (stm32_i2c_master_tx(I2C_Bus_1, I2C_ADDR, uAddrAndBuf, len + 1, 100) <
-      0) {
-    TRACE("I2C B1 ERROR: WriteRegister failed");
-    return false;
-  }
-  return true;
-}
-
-bool I2C_CSD203_ReadRegister(uint8_t I2C_ADDR, uint8_t reg, uint8_t *buf,
-                             uint8_t len)
-{
-  uint8_t uRegAddr[2];
-  // uRegAddr[0] = (uint8_t)((reg & 0xFF00) >> 8);
-  uRegAddr[0] = (uint8_t)(reg & 0x00FF);
-
-  if (stm32_i2c_master_tx(I2C_Bus_1, I2C_ADDR, uRegAddr, 1, 100) < 0) {
-    TRACE("I2C B1 ERROR: ReadRegister write reg address failed");
-    return false;
-  }
-
-  if (stm32_i2c_master_rx(I2C_Bus_1, I2C_ADDR, buf, len, 100) < 0) {
-    TRACE("I2C B1 ERROR: ReadRegister read reg address failed");
-    return false;
-  }
-  return true;
-}
-
-void IIC_DUT_W(uint8_t addr, uint8_t reg, uint16_t data)
-{
-  uint8_t buf[2];
-
-  buf[0] = data >> 8;
-  buf[1] = data & 0xff;
-
-  I2C_CSD203_WriteRegister(addr, reg, buf, 2);
-}
-
-uint16_t IIC_DUT_R(uint8_t addr, uint8_t reg)
-{
-  uint8_t buf[2];
-
-  I2C_CSD203_ReadRegister(addr, reg, buf, 2);
-
-  return buf[0] << 8 | buf[1];
-}
-
-void CSD203_Init(CSD_CONFIG *CSD203_CFG)
-{
-  uint16_t Data = 0, ADDR = 0;
-  Data |= (CSD203_CFG->RST) << 15;
-  Data |= (CSD203_CFG->Average) << 9;
-  Data |= (CSD203_CFG->VBUS_Conv_Time) << 5;
-  Data |= (CSD203_CFG->VShunt_Conv_Time) << 2;
-  Data |= CSD203_CFG->Mode;
-
-  ADDR = (CSD203_CFG->DeviceADDR);
-
-  IIC_DUT_W(ADDR, CONFIGURATION, Data);
-
-  Data = CalParam / ((CSD203_CFG->CurrentLSB) * (CSD203_CFG->Rshunt));
-  IIC_DUT_W(ADDR, CALIBRATION, Data);
-}
-
-/*Read Vbus*/
-uint16_t CSD203_ReadVbus(CSD_CONFIG *CSD203_CFG)
-{
-  uint16_t Data = 0, ADDR = 0;
-  ADDR = (CSD203_CFG->DeviceADDR);
-  Data = IIC_DUT_R(ADDR, BUS_VOLTAGE);
-  return Data;
-}
-
-/*Read Rshunt*/
-uint16_t CSD203_ReadRshunt(CSD_CONFIG *CSD203_CFG)
-{
-  uint16_t Data = 0, ADDR = 0;
-  ADDR = (CSD203_CFG->DeviceADDR);
-  Data = IIC_DUT_R(ADDR, SHUNT_VOLTAGE);
-  return Data;
-}
-
-/*Read Power*/
-uint16_t CSD203_ReadPower(CSD_CONFIG *CSD203_CFG)
-{
-  uint16_t Data = 0, ADDR = 0;
-  ADDR = (CSD203_CFG->DeviceADDR);
-  Data = IIC_DUT_R(ADDR, POWER);
-  return Data;
-}
-
-uint16_t CSD203_ReadCurrent(CSD_CONFIG *CSD203_CFG)
-{
-  uint16_t Data = 0, ADDR = 0;
-  ADDR = (CSD203_CFG->DeviceADDR);
-  Data = IIC_DUT_R(ADDR, CURRENT);
-  return Data;
-}
-
-uint16_t CSD203_ReadCONFIGURATION(CSD_CONFIG *CSD203_CFG)
-{
-  uint16_t Data = 0, ADDR = 0;
-  ADDR = (CSD203_CFG->DeviceADDR);
-  Data = IIC_DUT_R(ADDR, CONFIGURATION);
-  return Data;
-}
-
-uint16_t CSD203_ReadManufacturerID(CSD_CONFIG *CSD203_CFG)
-{
-  uint16_t Data = 0, ADDR = 0;
-  ADDR = (CSD203_CFG->DeviceADDR);
-  Data = IIC_DUT_R(ADDR, ManufacturerID);
-  return Data;
-}
-
-void IICcsd203init(void)
-{
-  if (i2c_init(I2C_Bus_1) < 0) {
-    return;  //err
-  }
-}
-void ReIICcsd203init(void)
-{
-	if (i2c_init(I2C_Bus_1) < 0) {
-    return;  //err
-  }
-}
-
-void initCSD203(void)
-{
-  CSD203MainInitFlag = false;
-  CSD203InInitFlag = false;
-  CSD203ExtInitFlag = false;
-
-  // TRACE("202 test ...");		//A0=CLK A1=SDA  RadioSky
-
-  CSD203_MainSensorCFG.RST = CSD_CONFIG_RST;
-  CSD203_MainSensorCFG.Average = CSD_CONFIG_Avg16;
-  CSD203_MainSensorCFG.VBUS_Conv_Time = CSD_CONFIG_VBUS_CT1_1mS;
-  CSD203_MainSensorCFG.VShunt_Conv_Time = CSD_CONFIG_VShunt_CT1_1mS;
-  CSD203_MainSensorCFG.Rshunt = CSD_CONFIG_Rs_10mR;
-  CSD203_MainSensorCFG.CurrentLSB = CSD_CONFIG_CurrentLsb1mA;
-  CSD203_MainSensorCFG.Mode = 7;
-  CSD203_MainSensorCFG.DeviceADDR = CSD_CONFIG_ADDR_A1_VS_A0_GND;  // 100 0100
-  // CSD203_MainSensorCFG.DeviceADDR=CSD_CONFIG_ADDR_A1_GND_A0_GND;
-
-  CSD203_InSensorCFG.RST = CSD_CONFIG_RST;
-  CSD203_InSensorCFG.Average = CSD_CONFIG_Avg16;
-  CSD203_InSensorCFG.VBUS_Conv_Time = CSD_CONFIG_VBUS_CT1_1mS;
-  CSD203_InSensorCFG.VShunt_Conv_Time = CSD_CONFIG_VShunt_CT1_1mS;
-  CSD203_InSensorCFG.Rshunt = CSD_CONFIG_Rs_10mR;
-  CSD203_InSensorCFG.CurrentLSB = CSD_CONFIG_CurrentLsb1mA;
-  CSD203_InSensorCFG.Mode = 7;
-  CSD203_InSensorCFG.DeviceADDR = CSD_CONFIG_ADDR_A1_VS_A0_VS;  // 100 0101
-  // CSD203_InSensorCFG.DeviceADDR=CSD_CONFIG_ADDR_A1_GND_A0_GND;
-
-  CSD203_ExtSensorCFG.RST = CSD_CONFIG_RST;
-  CSD203_ExtSensorCFG.Average = CSD_CONFIG_Avg16;
-  CSD203_ExtSensorCFG.VBUS_Conv_Time = CSD_CONFIG_VBUS_CT1_1mS;
-  CSD203_ExtSensorCFG.VShunt_Conv_Time = CSD_CONFIG_VShunt_CT1_1mS;
-  CSD203_ExtSensorCFG.Rshunt = CSD_CONFIG_Rs_10mR;
-  CSD203_ExtSensorCFG.CurrentLSB = CSD_CONFIG_CurrentLsb1mA;
-  CSD203_ExtSensorCFG.Mode = 7;
-  CSD203_ExtSensorCFG.DeviceADDR = CSD_CONFIG_ADDR_A1_GND_VS_GND;  // 100 0001
-  // CSD203_ExtSensorCFG.DeviceADDR=CSD_CONFIG_ADDR_A1_GND_A0_GND;
-
-  CSD203_Init(&CSD203_MainSensorCFG);
-  delay_ms(1);
-  uint16_t cfg = CSD203_ReadManufacturerID(&CSD203_MainSensorCFG);
-  if (cfg == ManIDCode) {
-    CSD203_ReadCurrent(&CSD203_MainSensorCFG);
-    CSD203MainInitFlag = true;
-  }
-
-  delay_ms(5);
-  CSD203_Init(&CSD203_InSensorCFG);
-  delay_ms(1);
-  cfg = CSD203_ReadManufacturerID(&CSD203_InSensorCFG);
-  if (cfg == ManIDCode) {
-    CSD203_ReadCurrent(&CSD203_InSensorCFG);
-    CSD203InInitFlag = true;
-  }
-
-  delay_ms(5);
-  CSD203_Init(&CSD203_ExtSensorCFG);
-  delay_ms(1);
-  cfg = CSD203_ReadManufacturerID(&CSD203_ExtSensorCFG);
-  if (cfg == ManIDCode) {
-    CSD203_ReadCurrent(&CSD203_ExtSensorCFG);
-    CSD203ExtInitFlag = true;
-  }
-}
-
-static uint16_t getCSD203BatteryVoltage(void)
-{  // 1000=1000mV
-  return csd203extvbus;
-}
-
-uint16_t getBatteryVoltage()
-{
-  return getCSD203BatteryVoltage() / 10;
-}
-
-void readCSD203(void)
-{  // 5ms
-  static uint16_t GetSenSorStep = 0;
-
-  if (suspendI2CTasks) return;
-
-  if (IICReadStatusFlag == true) return;
-
-  IICReadStatusFlag = true;
-  if (GetSenSorStep == 0 && CSD203MainInitFlag == true) {
-    CSD203_ReadCurrent(&CSD203_MainSensorCFG);
-    CSD203_ReadVbus(&CSD203_MainSensorCFG);
-  } else if (GetSenSorStep == 1 && CSD203InInitFlag == true) {
-    CSD203_ReadCurrent(&CSD203_InSensorCFG);
-    CSD203_ReadVbus(&CSD203_InSensorCFG);
-  } else if (GetSenSorStep == 2 && CSD203ExtInitFlag == true) {
-    CSD203_ReadCurrent(&CSD203_ExtSensorCFG);
-    csd203extvbus = (CSD203_ReadVbus(&CSD203_ExtSensorCFG) * 1.25);
-    // TRACE("Vbat=%d\r",csd203extvbus);
-  }
-  IICReadStatusFlag = false;
-  if (++GetSenSorStep >= 3) GetSenSorStep = 0;
-}
diff --git a/radio/src/targets/common/arm/stm32/stm32_i2c_driver.cpp b/radio/src/targets/common/arm/stm32/stm32_i2c_driver.cpp
index 156519eddb4..258391da515 100644
--- a/radio/src/targets/common/arm/stm32/stm32_i2c_driver.cpp
+++ b/radio/src/targets/common/arm/stm32/stm32_i2c_driver.cpp
@@ -27,9 +27,7 @@
 #include "stm32_hal.h"
 
 #include "timers_driver.h"
-#if !defined(BOOT)
-#include "os/task.h"
-#endif
+#include "delays_driver.h"
 #include "debug.h"
 
 #define MAX_I2C_DEVICES 2
@@ -37,10 +35,6 @@
 struct stm32_i2c_device {
   I2C_HandleTypeDef handle;
   const stm32_i2c_hw_def_t* hw_def;
-#if !defined(BOOT)
-  mutex_handle_t mutex;
-  bool mutex_initialized;
-#endif
 };
 
 static stm32_i2c_device _i2c_devs[MAX_I2C_DEVICES] = {};
@@ -57,20 +51,6 @@ static I2C_HandleTypeDef* i2c_get_handle(uint8_t bus)
   return &_i2c_devs[bus].handle;
 }
 
-#if !defined(BOOT)
-static void i2c_ensure_mutex(stm32_i2c_device* dev)
-{
-  if (!dev->mutex_initialized && scheduler_is_running()) {
-    mutex_create(&dev->mutex);
-    dev->mutex_initialized = true;
-  }
-}
-
-#define I2CMutex(bus) i2c_ensure_mutex(i2c_get_device(bus)); MutexLock mutexLock = MutexLock::MakeInstance(&i2c_get_device(bus)->mutex)
-#else
-#define I2CMutex(bus)
-#endif
-
 #if defined(STM32H7) || defined(STM32H7RS)
 
 /* ST's I2C timing computation */
@@ -468,6 +448,108 @@ static int i2c_gpio_deinit(const stm32_i2c_hw_def_t* hw_def)
   return 0;
 }
 
+// Re-initialize I2C peripheral GPIO, clock, and HAL config.
+// Assumes dev->hw_def and dev->handle.Init are already populated.
+static int i2c_periph_init(stm32_i2c_device* dev)
+{
+  auto hw_def = dev->hw_def;
+  auto h = &dev->handle;
+
+  if (i2c_gpio_init(hw_def) < 0) {
+    TRACE("I2C ERROR: i2c_periph_init() GPIO misconfiguration");
+    return -1;
+  }
+
+  if (i2c_enable_clock(hw_def->I2Cx) < 0) {
+    TRACE("I2C ERROR: i2c_periph_init() clock misconfiguration");
+    return -1;
+  }
+
+  if (HAL_I2C_Init(h) != HAL_OK) {
+    TRACE("I2C ERROR: i2c_periph_init() HAL_I2C_Init failed");
+    return -1;
+  }
+
+#if defined(I2C_FLTR_ANOFF) && defined(I2C_FLTR_DNF) || defined(STM32H7) || \
+    defined(STM32H7RS)
+  if (HAL_I2CEx_ConfigAnalogFilter(h, I2C_ANALOGFILTER_ENABLE) != HAL_OK) {
+    TRACE("I2C ERROR: i2c_periph_init() analog filter failed");
+    return -1;
+  }
+
+  if (HAL_I2CEx_ConfigDigitalFilter(h, 0) != HAL_OK) {
+    TRACE("I2C ERROR: i2c_periph_init() digital filter failed");
+    return -1;
+  }
+#endif
+
+  return 0;
+}
+
+// I2C bus recovery via SCL bit-banging (per ST AN2824).
+//
+// When a slave holds SDA low (e.g. after an interrupted transaction),
+// the master must clock SCL manually to let the slave finish its byte
+// and release SDA.  Then a STOP condition resets all slaves on the bus.
+//
+// Must be called with the bus mutex held (or before scheduler starts).
+// Returns 0 on success, -1 if SDA still stuck after recovery.
+static int i2c_bus_recover(stm32_i2c_device* dev)
+{
+  auto hw_def = dev->hw_def;
+  auto h = &dev->handle;
+
+  TRACE("I2C bus recovery: starting");
+
+  // Step 1: Deinit the I2C peripheral so we can bit-bang the pins
+  HAL_I2C_DeInit(h);
+
+  // Step 2: Configure SCL and SDA as open-drain GPIO outputs (high)
+  gpio_set(hw_def->SCL_GPIO);
+  gpio_set(hw_def->SDA_GPIO);
+  gpio_init(hw_def->SCL_GPIO, GPIO_OD_PU, GPIO_PIN_SPEED_MEDIUM);
+  gpio_init(hw_def->SDA_GPIO, GPIO_OD_PU, GPIO_PIN_SPEED_MEDIUM);
+
+  // Step 3: Check if SDA is already high — bus may not actually be stuck
+  delay_us(5);
+  if (gpio_read(hw_def->SDA_GPIO)) {
+    TRACE("I2C bus recovery: SDA is high, reinitializing peripheral");
+    return i2c_periph_init(dev);
+  }
+
+  // Step 4: Clock SCL up to 9 times to flush the slave's shift register.
+  // After each rising edge, check if SDA has been released.
+  for (int i = 0; i < 9; i++) {
+    gpio_clear(hw_def->SCL_GPIO);
+    delay_us(5);
+    gpio_set(hw_def->SCL_GPIO);
+    delay_us(5);
+
+    if (gpio_read(hw_def->SDA_GPIO)) {
+      TRACE("I2C bus recovery: SDA released after %d clocks", i + 1);
+      break;
+    }
+  }
+
+  if (!gpio_read(hw_def->SDA_GPIO)) {
+    TRACE("I2C bus recovery: SDA still stuck low after 9 clocks");
+    // Reinit peripheral anyway — best effort
+    i2c_periph_init(dev);
+    return -1;
+  }
+
+  // Step 5: Generate a STOP condition (SDA low→high while SCL is high)
+  gpio_clear(hw_def->SDA_GPIO);
+  delay_us(5);
+  gpio_set(hw_def->SCL_GPIO);
+  delay_us(5);
+  gpio_set(hw_def->SDA_GPIO);
+  delay_us(5);
+
+  // Step 6: Reinitialize the I2C peripheral
+  TRACE("I2C bus recovery: reinitializing peripheral");
+  return i2c_periph_init(dev);
+}
 
 int stm32_i2c_init(uint8_t bus, uint32_t clock_rate, const stm32_i2c_hw_def_t* hw_def)
 {
@@ -506,39 +588,8 @@ int stm32_i2c_init(uint8_t bus, uint32_t clock_rate, const stm32_i2c_hw_def_t* h
   init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
   init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
 
-  if (i2c_gpio_init(hw_def) < 0) {
-    TRACE("I2C ERROR: HAL_I2C_MspInit() I2C_GPIO misconfiguration");
-    return -2;
-  }
-
-  if (i2c_enable_clock(hw_def->I2Cx) < 0) {
-    TRACE("I2C ERROR: HAL_I2C_MspInit() I2C misconfiguration");
-    return -3;
-  }
-
-  if (HAL_I2C_Init(h) != HAL_OK) {
-    TRACE("I2C ERROR: HAL_I2C_Init() failed");
-    return -4;
-  }
-
-#if defined(I2C_FLTR_ANOFF) && defined(I2C_FLTR_DNF) || defined(STM32H7) || \
-    defined(STM32H7RS)
-  // Configure Analogue filter
-  if (HAL_I2CEx_ConfigAnalogFilter(h, I2C_ANALOGFILTER_ENABLE) != HAL_OK) {
-    TRACE("I2C ERROR: HAL_I2CEx_ConfigAnalogFilter() failed");
-    return -1;
-  }
-
-  // Configure Digital filter
-  if (HAL_I2CEx_ConfigDigitalFilter(h, 0) != HAL_OK) {
-    TRACE("I2C ERROR: HAL_I2CEx_ConfigDigitalFilter() failed");
-    return -1;
-  }
-#endif
+  if (i2c_periph_init(dev) < 0) return -1;
 
-#if !defined(BOOT)
-  dev->mutex_initialized = false;
-#endif
   return 1;
 }
 
@@ -564,16 +615,29 @@ int stm32_i2c_deinit(uint8_t bus)
   return 0;
 }
 
+int stm32_i2c_bus_recover(uint8_t bus)
+{
+  auto dev = i2c_get_device(bus);
+  if (!dev || !dev->hw_def) return -1;
+
+  return i2c_bus_recover(dev);
+}
+
+// Transfer functions — caller must hold the bus lock.
+
 int stm32_i2c_master_tx(uint8_t bus, uint16_t addr, uint8_t *data, uint16_t len,
                         uint32_t timeout)
 {
-  I2C_HandleTypeDef* h = i2c_get_handle(bus);
-  if (!h) return -1;  
-  
-  I2CMutex(bus);
-  if (HAL_I2C_Master_Transmit(h, addr << 1, data, len, timeout) != HAL_OK) {
+  auto dev = i2c_get_device(bus);
+  if (!dev) return -1;
+  auto h = &dev->handle;
+
+  if (HAL_I2C_Master_Transmit(h, addr << 1, data, len, timeout) == HAL_OK)
+    return 0;
+
+  if (i2c_bus_recover(dev) < 0) return -1;
+  if (HAL_I2C_Master_Transmit(h, addr << 1, data, len, timeout) != HAL_OK)
     return -1;
-  }
 
   return 0;
 }
@@ -581,13 +645,16 @@ int stm32_i2c_master_tx(uint8_t bus, uint16_t addr, uint8_t *data, uint16_t len,
 int stm32_i2c_master_rx(uint8_t bus, uint16_t addr, uint8_t *data, uint16_t len,
                         uint32_t timeout)
 {
-  I2C_HandleTypeDef* h = i2c_get_handle(bus);
-  if (!h) return -1;  
-  
-  I2CMutex(bus);
-  if (HAL_I2C_Master_Receive(h, addr << 1, data, len, timeout) != HAL_OK) {
+  auto dev = i2c_get_device(bus);
+  if (!dev) return -1;
+  auto h = &dev->handle;
+
+  if (HAL_I2C_Master_Receive(h, addr << 1, data, len, timeout) == HAL_OK)
+    return 0;
+
+  if (i2c_bus_recover(dev) < 0) return -1;
+  if (HAL_I2C_Master_Receive(h, addr << 1, data, len, timeout) != HAL_OK)
     return -1;
-  }
 
   return 0;
 }
@@ -595,13 +662,16 @@ int stm32_i2c_master_rx(uint8_t bus, uint16_t addr, uint8_t *data, uint16_t len,
 int stm32_i2c_read(uint8_t bus, uint16_t addr, uint16_t reg, uint16_t reg_size,
                    uint8_t* data, uint16_t len, uint32_t timeout)
 {
-  I2C_HandleTypeDef* h = i2c_get_handle(bus);
-  if (!h) return -1;  
+  auto dev = i2c_get_device(bus);
+  if (!dev) return -1;
+  auto h = &dev->handle;
+
+  if (HAL_I2C_Mem_Read(h, addr << 1, reg, reg_size, data, len, timeout) == HAL_OK)
+    return 0;
 
-  I2CMutex(bus);
-  if (HAL_I2C_Mem_Read(h, addr << 1, reg, reg_size, data, len, timeout) != HAL_OK) {
+  if (i2c_bus_recover(dev) < 0) return -1;
+  if (HAL_I2C_Mem_Read(h, addr << 1, reg, reg_size, data, len, timeout) != HAL_OK)
     return -1;
-  }
 
   return 0;
 }
@@ -609,13 +679,16 @@ int stm32_i2c_read(uint8_t bus, uint16_t addr, uint16_t reg, uint16_t reg_size,
 int stm32_i2c_write(uint8_t bus, uint16_t addr, uint16_t reg, uint16_t reg_size,
                     uint8_t* data, uint16_t len, uint32_t timeout)
 {
-  I2C_HandleTypeDef* h = i2c_get_handle(bus);
-  if (!h) return -1;  
+  auto dev = i2c_get_device(bus);
+  if (!dev) return -1;
+  auto h = &dev->handle;
+
+  if (HAL_I2C_Mem_Write(h, addr << 1, reg, reg_size, data, len, timeout) == HAL_OK)
+    return 0;
 
-  I2CMutex(bus);
-  if (HAL_I2C_Mem_Write(h, addr << 1, reg, reg_size, data, len, timeout) != HAL_OK) {
+  if (i2c_bus_recover(dev) < 0) return -1;
+  if (HAL_I2C_Mem_Write(h, addr << 1, reg, reg_size, data, len, timeout) != HAL_OK)
     return -1;
-  }
 
   return 0;
 }
@@ -625,7 +698,6 @@ int stm32_i2c_is_dev_ready(uint8_t bus, uint16_t addr, uint32_t retries, uint32_
   I2C_HandleTypeDef* h = i2c_get_handle(bus);
   if (!h) return -1;
 
-  I2CMutex(bus);
   HAL_StatusTypeDef err = HAL_I2C_IsDeviceReady(h, addr << 1, retries, timeout);
   if (err != HAL_OK) return -1;
 
@@ -634,6 +706,11 @@ int stm32_i2c_is_dev_ready(uint8_t bus, uint16_t addr, uint32_t retries, uint32_
 
 int stm32_i2c_is_dev_ready(uint8_t bus, uint16_t addr, uint32_t timeout)
 {
-  I2CMutex(bus);
-  return stm32_i2c_is_dev_ready(bus, addr, 1, timeout);
+  I2C_HandleTypeDef* h = i2c_get_handle(bus);
+  if (!h) return -1;
+
+  HAL_StatusTypeDef err = HAL_I2C_IsDeviceReady(h, addr << 1, 1, timeout);
+  if (err != HAL_OK) return -1;
+
+  return 0;
 }
diff --git a/radio/src/targets/common/arm/stm32/stm32_i2c_driver.h b/radio/src/targets/common/arm/stm32/stm32_i2c_driver.h
index 015015334de..003d7123d3c 100644
--- a/radio/src/targets/common/arm/stm32/stm32_i2c_driver.h
+++ b/radio/src/targets/common/arm/stm32/stm32_i2c_driver.h
@@ -47,7 +47,14 @@ int stm32_i2c_init(uint8_t bus, uint32_t clock_rate,
 // @return -1 if error, 0 otherwise
 int stm32_i2c_deinit(uint8_t bus);
 
+// Recover I2C bus by SCL bit-banging (per ST AN2824).
+// Clocks SCL up to 9 times to release a stuck slave, then reinits peripheral.
+// Caller must hold the bus lock.
+// @return 0 on success, -1 if SDA still stuck or reinit failed
+int stm32_i2c_bus_recover(uint8_t bus);
+
 // Transmit in master mode (blocking mode)
+// Caller must hold the bus lock.
 // @return -1 if error, 0 otherwise
 int stm32_i2c_master_tx(uint8_t bus, uint16_t addr, uint8_t *data, uint16_t len,
                         uint32_t timeout);
diff --git a/radio/src/targets/horus/board.cpp b/radio/src/targets/horus/board.cpp
index 04308fd289e..ae923057bdb 100644
--- a/radio/src/targets/horus/board.cpp
+++ b/radio/src/targets/horus/board.cpp
@@ -49,7 +49,8 @@
 #include <string.h>
 
 #if defined(CSD203_SENSOR)
-  #include "csd203_sensor.h"
+  #include "drivers/csd203.h"
+  #include "stm32_i2c_driver.h"
 #endif
 
 #if defined(IMU) && defined(IMU_I2C_BUS) && defined(IMU_I2C_ADDRESS)
@@ -211,7 +212,7 @@ void boardInit()
 #endif
 
 #if defined(CSD203_SENSOR)
-  initCSD203();
+  csd203_start(I2C_Bus_1);
 #endif
 
   hapticInit();
diff --git a/radio/src/targets/horus/cst8xx_driver.cpp b/radio/src/targets/horus/cst8xx_driver.cpp
index 71092978cd1..8dc36a16b34 100644
--- a/radio/src/targets/horus/cst8xx_driver.cpp
+++ b/radio/src/targets/horus/cst8xx_driver.cpp
@@ -249,6 +249,8 @@ TouchState touchPanelRead()
 {
   if (!touchEventOccured) return internalTouchState;
 
+  i2c_lock(TOUCH_I2C_BUS);
+
   touchEventOccured = false;
 
   uint32_t now = timersGetMsTick();
@@ -286,6 +288,7 @@ TouchState touchPanelRead()
   if (internalTouchState.event == TE_UP ||
       internalTouchState.event == TE_SLIDE_END)
     internalTouchState.event = TE_NONE;
+  i2c_unlock(TOUCH_I2C_BUS);
   return ret;
 }
 
diff --git a/radio/src/targets/horus/tp_gt911.cpp b/radio/src/targets/horus/tp_gt911.cpp
index 5d2936d3149..81720ce081e 100644
--- a/radio/src/targets/horus/tp_gt911.cpp
+++ b/radio/src/targets/horus/tp_gt911.cpp
@@ -644,28 +644,13 @@ static const char *event2str(uint8_t ev)
 }
 #endif
 
-#if defined(CSD203_SENSOR)
-  extern bool IICReadStatusFlag;
-#endif
-
 struct TouchState touchPanelRead()
 {
   uint8_t state = 0;
 
   if (!touchEventOccured) return internalTouchState;
-#if defined(CSD203_SENSOR)
-  for(int a=0;a<6;a++)
-  {//IIC bus preemption
-    if(IICReadStatusFlag==false){
-      IICReadStatusFlag=true;
-      break;
-    } 
-    else if(a>6){
-      return internalTouchState;
-    }
-    delay_us(10);
-  }
-#endif  
+
+  i2c_lock(TOUCH_I2C_BUS);
 
   touchEventOccured = false;
 
@@ -676,9 +661,7 @@ struct TouchState touchPanelRead()
       touchGT911hiccups++;
       TRACE("GT911 I2C read XY error");
       if (!I2C_ReInit()) TRACE("I2C B1 ReInit failed");
-    #if defined(CSD203_SENSOR)
-      IICReadStatusFlag=false;
-    #endif  
+      i2c_unlock(TOUCH_I2C_BUS);
       return internalTouchState;
     }
 
@@ -704,9 +687,7 @@ struct TouchState touchPanelRead()
         touchGT911hiccups++;
         TRACE("GT911 I2C data read error");
         if (!I2C_ReInit()) TRACE("I2C B1 ReInit failed");
-      #if defined(CSD203_SENSOR)
-        IICReadStatusFlag=false;
-      #endif  
+        i2c_unlock(TOUCH_I2C_BUS);
         return internalTouchState;
       }
         
@@ -757,9 +738,7 @@ struct TouchState touchPanelRead()
   }
 
   TRACE("touch event = %s", event2str(internalTouchState.event));
-#if defined(CSD203_SENSOR)
-  IICReadStatusFlag=false;
-#endif
+  i2c_unlock(TOUCH_I2C_BUS);
   return internalTouchState;
 }
 
diff --git a/radio/src/targets/pa01/bsp_io.cpp b/radio/src/targets/pa01/bsp_io.cpp
index 54778450fbc..07abc50404a 100644
--- a/radio/src/targets/pa01/bsp_io.cpp
+++ b/radio/src/targets/pa01/bsp_io.cpp
@@ -25,8 +25,6 @@
 #include "drivers/aw9523b.h"
 #include "stm32_i2c_driver.h"
 #include "stm32_switch_driver.h"
-#include "delays_driver.h"
-#include "debug.h"
 
 #define BSP_I2C_BUS I2C_Bus_1
 #define BSP_I2C_ADDR 0x5b
@@ -58,38 +56,11 @@ bool bsp_get_shouldReadKeys()
   return tmp;
 }
 
-#define I2C_ERROR_RECOVER_THRESHOLD 3
-
-static uint8_t i2c_consecutive_errors = 0;
-
-static void _recover_i2c()
-{
-  TRACE("I2C ERROR: resetting I2C bus");
-  i2c_deinit(BSP_I2C_BUS);
-  delay_ms(1);
-  i2c_init(BSP_I2C_BUS);
-  TRACE("I2C recovery complete");
-}
-
-static void _track_i2c_error(int result)
-{
-  if (result < 0) {
-    i2c_consecutive_errors++;
-    if (i2c_consecutive_errors >= I2C_ERROR_RECOVER_THRESHOLD) {
-      _recover_i2c();
-      i2c_consecutive_errors = 0;
-    }
-  } else {
-    i2c_consecutive_errors = 0;
-  }
-}
-
 static void bsp_input_read()
 {
   uint16_t value;
-  int ret = aw9523b_read(&i2c_exp, BSP_IN_MASK, &value);
-  _track_i2c_error(ret);
-  if (ret >= 0) inputState = value;
+  // I2C bus recovery is handled centrally by the HAL transfer layer
+  if (aw9523b_read(&i2c_exp, BSP_IN_MASK, &value) >= 0) inputState = value;
 }
 
 int bsp_io_init()
@@ -107,13 +78,13 @@ int bsp_io_init()
 }
 
 void bsp_output_set(uint16_t pin)
-  { _track_i2c_error(aw9523b_write(&i2c_exp, pin, pin)); }
+  { aw9523b_write(&i2c_exp, pin, pin); }
 
 void bsp_output_set(uint16_t mask, uint16_t pin)
-  { _track_i2c_error(aw9523b_write(&i2c_exp, mask, pin)); }
+  { aw9523b_write(&i2c_exp, mask, pin); }
 
 void bsp_output_clear(uint16_t pin)
-  { _track_i2c_error(aw9523b_write(&i2c_exp, pin, 0)); }
+  { aw9523b_write(&i2c_exp, pin, 0); }
 
 uint16_t bsp_input_get()
 {
diff --git a/radio/src/targets/pa01/key_driver.cpp b/radio/src/targets/pa01/key_driver.cpp
index ba177162568..6d165091bb3 100644
--- a/radio/src/targets/pa01/key_driver.cpp
+++ b/radio/src/targets/pa01/key_driver.cpp
@@ -25,6 +25,7 @@
 #include "stm32_hal_ll.h"
 #include "stm32_gpio_driver.h"
 #include "stm32_i2c_driver.h"
+#include "hal/i2c_driver.h"
 
 #include "hal.h"
 #include "delays_driver.h"
@@ -174,6 +175,13 @@ void pollKeys()
     return;
   }
 
+  // Timer-context: bail out (keeping cached columns) if the bus is busy
+  if (!i2c_trylock(I2C_Bus_1)) {
+    keyState = col_cache[0] | col_cache[1] | col_cache[2] | col_cache[3] | ent_mask;
+    syncelem.ui8ReadInProgress = 0;
+    return;
+  }
+
   if (bsp_get_shouldReadKeys()) {
     scan_pending = SCAN_COLS;
     idle_cycles = 0;
@@ -197,6 +205,8 @@ void pollKeys()
   bsp_output_set(BSP_KEY_OUT_MASK, col_drive[scan_col]);
   scan_col = (scan_col + 1) % SCAN_COLS;
 
+  i2c_unlock(I2C_Bus_1);
+
   uint32_t result = col_cache[0] | col_cache[1] | col_cache[2] | col_cache[3] | ent_mask;
 
   syncelem.oldResult = result;
diff --git a/radio/src/targets/pl18/touch_driver.cpp b/radio/src/targets/pl18/touch_driver.cpp
index 2e72f83dc3e..92a2671064a 100644
--- a/radio/src/targets/pl18/touch_driver.cpp
+++ b/radio/src/targets/pl18/touch_driver.cpp
@@ -492,6 +492,8 @@ struct TouchState touchPanelRead()
 
   if (!touchEventOccured) return internalTouchState;
 
+  i2c_lock(TOUCH_I2C_BUS);
+
   touchEventOccured = false;
 
   uint32_t now = timersGetMsTick();
@@ -580,6 +582,7 @@ struct TouchState touchPanelRead()
   TRACE("%s: event=%d,X=%d,Y=%d", TOUCH_CONTROLLER_STR[touchController], ret.event, ret.x, ret.y);
 #endif
 
+  i2c_unlock(TOUCH_I2C_BUS);
   return ret;
 }
 
diff --git a/radio/src/targets/simu/CMakeLists.txt b/radio/src/targets/simu/CMakeLists.txt
index 0f71244e720..062d0443062 100644
--- a/radio/src/targets/simu/CMakeLists.txt
+++ b/radio/src/targets/simu/CMakeLists.txt
@@ -38,6 +38,7 @@ set(SIMU_DRIVERS
   bt_driver.cpp
   timers_driver.cpp
   abnormal_reboot.cpp
+  i2c_driver.cpp
 )
 
 set(HW_DESC_JSON ${FLAVOUR}.json)
diff --git a/radio/src/targets/simu/i2c_driver.cpp b/radio/src/targets/simu/i2c_driver.cpp
new file mode 100644
index 00000000000..bcd013ef221
--- /dev/null
+++ b/radio/src/targets/simu/i2c_driver.cpp
@@ -0,0 +1,37 @@
+/*
+ * Copyright (C) EdgeTX
+ *
+ * Based on code named
+ *   opentx - https://github.com/opentx/opentx
+ *   th9x - http://code.google.com/p/th9x
+ *   er9x - http://code.google.com/p/er9x
+ *   gruvin9x - http://code.google.com/p/gruvin9x
+ *
+ * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include "hal/i2c_driver.h"
+
+int i2c_init(etx_i2c_bus_t bus) { return -1; }
+int i2c_deinit(etx_i2c_bus_t bus) { return -1; }
+
+void i2c_lock(etx_i2c_bus_t bus) {}
+bool i2c_trylock(etx_i2c_bus_t bus) { return true; }
+void i2c_unlock(etx_i2c_bus_t bus) {}
+
+int i2c_dev_ready(etx_i2c_bus_t bus, uint16_t addr) { return -1; }
+
+int i2c_read(uint8_t bus, uint16_t addr, uint16_t reg, uint16_t reg_size,
+             uint8_t* data, uint16_t len) { return -1; }
+
+int i2c_write(uint8_t bus, uint16_t addr, uint16_t reg, uint16_t reg_size,
+              uint8_t* data, uint16_t len) { return -1; }
diff --git a/radio/src/targets/simu/switch_driver.cpp b/radio/src/targets/simu/switch_driver.cpp
index 58095301698..4d9b1da493f 100644
--- a/radio/src/targets/simu/switch_driver.cpp
+++ b/radio/src/targets/simu/switch_driver.cpp
@@ -44,10 +44,6 @@ struct hw_switch_def {
 
 int8_t switchesStates[MAX_SWITCHES];
 
-#if defined(RADIO_GX12)
-void _poll_switches() {}
-#endif
-
 void simuSetSwitch(uint8_t swtch, int8_t state)
 {
   assert(swtch < switchGetMaxAllSwitches());
diff --git a/radio/src/targets/st16/touch_driver.cpp b/radio/src/targets/st16/touch_driver.cpp
index 5318dd322e1..1144f77c827 100644
--- a/radio/src/targets/st16/touch_driver.cpp
+++ b/radio/src/targets/st16/touch_driver.cpp
@@ -22,6 +22,7 @@
 #include "stm32_hal_ll.h"
 #include "stm32_hal.h"
 #include "stm32_i2c_driver.h"
+#include "hal/i2c_driver.h"
 #include "stm32_gpio_driver.h"
 #include "stm32_exti_driver.h"
 
@@ -434,6 +435,8 @@ struct TouchState touchPanelRead()
 {
   if (!touchEventOccured || touchController == TC_NONE) return internalTouchState;
 
+  i2c_lock(TOUCH_I2C_BUS);
+
   touchEventOccured = false;
 
   tmr10ms_t now = get_tmr10ms();
@@ -514,6 +517,7 @@ struct TouchState touchPanelRead()
   TRACE("%s: event=%d,X=%d,Y=%d", TOUCH_CONTROLLER_STR[touchController], ret.event, ret.x, ret.y);
 #endif
 
+  i2c_unlock(TOUCH_I2C_BUS);
   return ret;
 }
 
diff --git a/radio/src/targets/st16/tp_cst340.cpp b/radio/src/targets/st16/tp_cst340.cpp
index c8e633ff3cf..dbf1fb022cf 100644
--- a/radio/src/targets/st16/tp_cst340.cpp
+++ b/radio/src/targets/st16/tp_cst340.cpp
@@ -26,6 +26,7 @@
 #include "stm32_exti_driver.h"
 
 #include "hal.h"
+#include "hal/i2c_driver.h"
 #include "timers_driver.h"
 #include "delays_driver.h"
 #include "tp_cst340.h"
@@ -510,6 +511,8 @@ TouchState touchPanelRead()
 {
   if (!touchEventOccured) return internalTouchState;
 
+  i2c_lock(TOUCH_I2C_BUS);
+
   touchEventOccured = false;
 
   tmr10ms_t now = get_tmr10ms();
@@ -551,6 +554,7 @@ TouchState touchPanelRead()
   TRACE("%s: Event = %d", touchController == TC_CST340 ? "CST340" : "FT6236", ret.event);
 #endif
 
+  i2c_unlock(TOUCH_I2C_BUS);
   return ret;
 }
 
diff --git a/radio/src/targets/stm32h7s78-dk/seesaw.cpp b/radio/src/targets/stm32h7s78-dk/seesaw.cpp
index db880378dfd..04cd1dc77c6 100644
--- a/radio/src/targets/stm32h7s78-dk/seesaw.cpp
+++ b/radio/src/targets/stm32h7s78-dk/seesaw.cpp
@@ -14,6 +14,7 @@
  */
 
 #include "seesaw.h"
+#include "hal/i2c_driver.h"
 
 #define STATUS_HW_ID   0x0001
 #define STATUS_VERSION 0x0002
@@ -108,33 +109,44 @@ int seesaw_pin_mode(seesaw_t* dev, uint32_t mask, seesaw_input_mode mode)
 {
   if (!dev->addr) return -1;
 
+  i2c_lock(dev->bus);
+  int rc = -1;
+
   switch (mode) {
     case SEESAW_OUTPUT:
-      return seesaw_write_32bit_be(dev, GPIO_DIRSET_BULK, mask);
+      rc = seesaw_write_32bit_be(dev, GPIO_DIRSET_BULK, mask);
+      break;
 
     case SEESAW_INPUT:
       if (seesaw_write_32bit_be(dev, GPIO_DIRCLR_BULK, mask) == 0 &&
           seesaw_write_32bit_be(dev, GPIO_PULLENCLR, mask) == 0)
-        return 0;
+        rc = 0;
+      break;
 
     case SEESAW_INPUT_PULLUP:
       if (seesaw_write_32bit_be(dev, GPIO_DIRCLR_BULK, mask) == 0 &&
           seesaw_write_32bit_be(dev, GPIO_PULLENSET, mask) == 0 &&
           seesaw_write_32bit_be(dev, GPIO_BULK_SET, mask) == 0)
-        return 0;
+        rc = 0;
+      break;
 
     case SEESAW_INPUT_PULLDOWN:
       if (seesaw_write_32bit_be(dev, GPIO_DIRCLR_BULK, mask) == 0 &&
           seesaw_write_32bit_be(dev, GPIO_PULLENSET, mask) == 0 &&
           seesaw_write_32bit_be(dev, GPIO_BULK_CLR, mask) == 0)
-        return 0;
+        rc = 0;
+      break;
   }
 
-  return -1;
+  i2c_unlock(dev->bus);
+  return rc;
 }
 
 int seesaw_digital_read(seesaw_t* dev, uint32_t pins, uint32_t* value)
 {
   if (!dev->addr) return -1;
-  return seesaw_read_32bit_be(dev, GPIO_BULK, value);  
+  i2c_lock(dev->bus);
+  int rc = seesaw_read_32bit_be(dev, GPIO_BULK, value);
+  i2c_unlock(dev->bus);
+  return rc;
 }
diff --git a/radio/src/targets/stm32h7s78-dk/tp_gt911.cpp b/radio/src/targets/stm32h7s78-dk/tp_gt911.cpp
index d4936b7213f..2911dd779e1 100644
--- a/radio/src/targets/stm32h7s78-dk/tp_gt911.cpp
+++ b/radio/src/targets/stm32h7s78-dk/tp_gt911.cpp
@@ -215,6 +215,8 @@ struct TouchState touchPanelRead()
 
   if (!touchEventOccured) return internalTouchState;
 
+  i2c_lock(TOUCH_I2C_BUS);
+
   touchEventOccured = false;
 
   uint32_t startReadStatus = timersGetMsTick();
@@ -224,6 +226,7 @@ struct TouchState touchPanelRead()
       touchGT911hiccups++;
       TRACE("GT911 I2C read XY error");
       // if (!I2C_ReInit()) { TRACE("I2C B1 ReInit failed"); }
+      i2c_unlock(TOUCH_I2C_BUS);
       return internalTouchState;
     }
 
@@ -249,6 +252,7 @@ struct TouchState touchPanelRead()
         touchGT911hiccups++;
         TRACE("GT911 I2C data read error");
         // if (!I2C_ReInit()) { TRACE("I2C B1 ReInit failed"); }
+        i2c_unlock(TOUCH_I2C_BUS);
         return internalTouchState;
       }
         
@@ -299,6 +303,7 @@ struct TouchState touchPanelRead()
   }
 
   TRACE("touch event = %s", event2str(internalTouchState.event));
+  i2c_unlock(TOUCH_I2C_BUS);
   return internalTouchState;
 }
 
diff --git a/radio/src/targets/taranis/board.cpp b/radio/src/targets/taranis/board.cpp
index b66f6e384f9..86f4ecb04b1 100644
--- a/radio/src/targets/taranis/board.cpp
+++ b/radio/src/targets/taranis/board.cpp
@@ -83,7 +83,8 @@ void boardBLInit()
 #endif
 
 #if defined(CSD203_SENSOR)
-  #include "csd203_sensor.h"
+  #include "drivers/csd203.h"
+  #include "stm32_i2c_driver.h"
 #endif
 
 HardwareOptions hardwareOptions;
@@ -173,8 +174,7 @@ void boardInit()
 #endif
 
 #if defined(CSD203_SENSOR)
-  IICcsd203init();
-  initCSD203();
+  csd203_start(I2C_Bus_1);
 #endif
 
 // If the radio was powered on by dual use USB, halt the boot process, let battery charge
diff --git a/radio/src/targets/taranis/gx12/bsp_io.cpp b/radio/src/targets/taranis/gx12/bsp_io.cpp
index a3fb2554f44..b633f33e367 100644
--- a/radio/src/targets/taranis/gx12/bsp_io.cpp
+++ b/radio/src/targets/taranis/gx12/bsp_io.cpp
@@ -20,15 +20,24 @@
  */
 
 #include "drivers/pca95xx.h"
+#include "hal/i2c_driver.h"
 #include "hal/switch_driver.h"
-#include "myeeprom.h"
 #include "stm32_i2c_driver.h"
 #include "stm32_switch_driver.h"
+#include "delays_driver.h"
 
-constexpr uint8_t MAX_UNREAD_CYCLE = 100; // Force a read every 100 cycles even if no interrupt was triggered
-#define IO_INT_GPIO GPIO_PIN(GPIOE, 14)
+#if !defined(BOOT)
+#include "os/async.h"
+#include "os/timer.h"
+#endif
+
+#include "debug.h"
+
+#define IO_INT_GPIO   GPIO_PIN(GPIOE, 14)
 #define IO_RESET_GPIO GPIO_PIN(GPIOE, 15)
+
 extern const stm32_switch_t* boardGetSwitchDef(uint8_t idx);
+extern bool suspendI2CTasks;
 
 struct bsp_io_expander {
   pca95xx_t exp;
@@ -36,14 +45,13 @@ struct bsp_io_expander {
   uint32_t state;
 };
 
-static volatile uint8_t _poll_switches_cnt = 0;
 static bsp_io_expander _io_switches;
 static bsp_io_expander _io_fs_switches;
 
-static void _io_int_handler()
-{
-  _poll_switches_cnt = 0;
-}
+#if !defined(BOOT)
+static volatile bool _poll_switches_in_queue = false;
+static timer_handle_t _poll_timer = TIMER_INITIALIZER;
+#endif
 
 static void _init_io_expander(bsp_io_expander* io, uint32_t mask)
 {
@@ -51,40 +59,79 @@ static void _init_io_expander(bsp_io_expander* io, uint32_t mask)
   io->state = 0;
 }
 
+static void _expanders_reset()
+{
+  gpio_clear(IO_RESET_GPIO);
+  delay_us(1);  // Only 6ns needed according to PCA datasheet
+  gpio_set(IO_RESET_GPIO);
+  delay_us(1);  // Chip time to reset is 400ns
+}
+
 static uint32_t _read_io_expander(bsp_io_expander* io)
 {
   uint16_t value = 0;
   if (pca95xx_read(&io->exp, io->mask, &value) == 0) {
     io->state = value;
   } else {
-    gpio_clear(IO_RESET_GPIO);
-    TRACE("PCA95 was reset");
-    delay_us(1);  // Only 6ns are needed according to PCA datasheet, but lets be safe
-    gpio_set(IO_RESET_GPIO);
+    TRACE("ERROR: resetting PCA95XX");
+    _expanders_reset();
+    if (pca95xx_read(&io->exp, io->mask, &value) == 0) {
+      io->state = value;
+    } else {
+      TRACE("ERROR: PCA95XX I2C bus recovery");
+      stm32_i2c_bus_recover(I2C_Bus_2);
+      if (pca95xx_read(&io->exp, io->mask, &value) == 0) {
+        io->state = value;
+      } else {
+        TRACE("ERROR: Unrecoverable error on PCA95XX");
+      }
+    }
   }
   return io->state;
 }
 
-void _poll_switches()
+#if !defined(BOOT)
+typedef enum {
+  TRIGGERED_BY_TIMER = 0,
+  TRIGGERED_BY_IRQ,
+} trigger_source_t;
+
+static void _poll_switches(void* param1, uint32_t trigger_source)
 {
-  if ( _poll_switches_cnt == 0) {
-    _read_io_expander(&_io_switches);
-    _read_io_expander(&_io_fs_switches);
-    _poll_switches_cnt = MAX_UNREAD_CYCLE;
-  } else {
-    _poll_switches_cnt--;
+  if (trigger_source == TRIGGERED_BY_IRQ) {
+    _poll_switches_in_queue = false;
+    timer_reset(&_poll_timer);
   }
+
+  if (suspendI2CTasks) return;
+  if (!i2c_trylock(I2C_Bus_2)) return;
+
+  _read_io_expander(&_io_switches);
+  _read_io_expander(&_io_fs_switches);
+
+  i2c_unlock(I2C_Bus_2);
+}
+
+static void _io_int_handler()
+{
+  async_call_isr(_poll_switches, &_poll_switches_in_queue, nullptr,
+                 TRIGGERED_BY_IRQ);
 }
 
-uint32_t bsp_io_read_switches()
+static void _poll_cb(timer_handle_t* timer)
 {
-  return _read_io_expander(&_io_switches);
+  (void)timer;
+  _poll_switches(nullptr, TRIGGERED_BY_TIMER);
 }
 
-uint32_t bsp_io_read_fs_switches()
+static void start_poll_timer()
 {
-  return _read_io_expander(&_io_fs_switches);
+  timer_create(&_poll_timer, _poll_cb, "portex", 100, true);
+  timer_start(&_poll_timer);
 }
+#else
+static void _io_int_handler() {}
+#endif
 
 int bsp_io_init()
 {
@@ -92,13 +139,13 @@ int bsp_io_init()
     return -1;
   }
 
-  // configure expander 1
+  // configure expander 1 (standard switches)
   _init_io_expander(&_io_switches, 0xFC3F);
   if (pca95xx_init(&_io_switches.exp, I2C_Bus_2, 0x74) < 0) {
     return -1;
   }
 
-  // configure expander 2
+  // configure expander 2 (function switches)
   _init_io_expander(&_io_fs_switches, 0x3F);
   if (pca95xx_init(&_io_fs_switches.exp, I2C_Bus_2, 0x75) < 0) {
     return -1;
@@ -111,8 +158,13 @@ int bsp_io_init()
   gpio_init(IO_RESET_GPIO, GPIO_OUT, GPIO_PIN_SPEED_LOW);
   gpio_set(IO_RESET_GPIO);
 
-  bsp_io_read_switches();
-  bsp_io_read_fs_switches();
+  // initial read
+  _read_io_expander(&_io_switches);
+  _read_io_expander(&_io_fs_switches);
+
+#if !defined(BOOT)
+  start_poll_timer();
+#endif
 
   return 0;
 }
@@ -122,11 +174,6 @@ void boardInitSwitches()
   bsp_io_init();
 }
 
-struct bsp_io_sw_def {
-  uint32_t pin_high;
-  uint32_t pin_low;
-};
-
 static SwitchHwPos _get_switch_pos(uint8_t idx)
 {
   SwitchHwPos pos = SWITCH_HW_UP;
@@ -174,4 +221,4 @@ SwitchHwPos boardSwitchGetPosition(uint8_t idx)
 {
   if (idx < 8) return _get_switch_pos(idx);
   return _get_fs_switch_pos(idx);
-}
\ No newline at end of file
+}
diff --git a/radio/src/targets/taranis/gx12/bsp_io.h b/radio/src/targets/taranis/gx12/bsp_io.h
index ad5eb63a5fe..d5585dccba3 100644
--- a/radio/src/targets/taranis/gx12/bsp_io.h
+++ b/radio/src/targets/taranis/gx12/bsp_io.h
@@ -1,5 +1,5 @@
 /*
-* Copyright (C) EdgeTX
+ * Copyright (C) EdgeTX
  *
  * Based on code named
  *   opentx - https://github.com/opentx/opentx
@@ -21,4 +21,4 @@
 
 #pragma once
 
-void _poll_switches();
\ No newline at end of file
+int bsp_io_init();
diff --git a/radio/src/targets/taranis/hal.h b/radio/src/targets/taranis/hal.h
index 764ea89991b..bb1a75edc16 100644
--- a/radio/src/targets/taranis/hal.h
+++ b/radio/src/targets/taranis/hal.h
@@ -1089,8 +1089,11 @@
   #define I2C_B2_GPIO_AF                LL_GPIO_AF_4
   #define I2C_B2_CLK_RATE               400000
 
-  #define USE_EXTI1_IRQ
-  #define EXTI1_IRQ_Priority 5
+  // PCA9555 interrupt on PE14
+  #if !defined(USE_EXTI15_10_IRQ)
+    #define USE_EXTI15_10_IRQ
+    #define EXTI15_10_IRQ_Priority 5
+  #endif
 #endif
 
 // SD - SPI2
diff --git a/radio/src/targets/taranis/volume_i2c.cpp b/radio/src/targets/taranis/volume_i2c.cpp
index d1ed2365cf0..585796f2979 100644
--- a/radio/src/targets/taranis/volume_i2c.cpp
+++ b/radio/src/targets/taranis/volume_i2c.cpp
@@ -35,17 +35,23 @@ static const uint8_t volumeScale[VOLUME_LEVEL_MAX + 1] = {
 
 static int32_t read_i2c_volume()
 {
+  i2c_lock(VOLUME_I2C_BUS);
   uint8_t value = 0;
-  if (i2c_read(VOLUME_I2C_BUS, VOLUME_I2C_ADDRESS, 0, 1, &value, 1) < 0)
+  if (i2c_read(VOLUME_I2C_BUS, VOLUME_I2C_ADDRESS, 0, 1, &value, 1) < 0) {
+    i2c_unlock(VOLUME_I2C_BUS);
     return -1;
+  }
 
+  i2c_unlock(VOLUME_I2C_BUS);
   return value;
 }
 
 static void write_i2c_volume(uint8_t volume)
 {
+  i2c_lock(VOLUME_I2C_BUS);
   i2c_init(VOLUME_I2C_BUS);
   i2c_write(VOLUME_I2C_BUS, VOLUME_I2C_ADDRESS, 0, 1, &volume, 1);
+  i2c_unlock(VOLUME_I2C_BUS);
 }
 
 void audioSetVolume(uint8_t volume)
diff --git a/radio/src/tasks/mixer_task.cpp b/radio/src/tasks/mixer_task.cpp
index 7be2699564c..91d70634e9c 100644
--- a/radio/src/tasks/mixer_task.cpp
+++ b/radio/src/tasks/mixer_task.cpp
@@ -127,10 +127,6 @@ constexpr uint8_t MIXER_MAX_PERIOD = MAX_REFRESH_RATE / 1000 /*ms*/;
 
 void execMixerFrequentActions()
 {
-#if defined(IMU)
-  gyroWakeup();
-#endif
-
 #if defined(BLUETOOTH)
   bluetooth.wakeup();
 #endif