diff --git a/radio/src/boards/generic_stm32/CMakeLists.txt b/radio/src/boards/generic_stm32/CMakeLists.txt
index b64fb2f1b..d1492a863 100644
--- a/radio/src/boards/generic_stm32/CMakeLists.txt
+++ b/radio/src/boards/generic_stm32/CMakeLists.txt
@@ -12,6 +12,7 @@ set(MINIMAL_BOARD_LIB_SRC
   ${CMAKE_CURRENT_BINARY_DIR}/stm32_keys.inc
 
   boards/generic_stm32/inputs.cpp
+  boards/generic_stm32/i2c_bus.cpp
 )
 
 # Dependencies only used in firmware
diff --git a/radio/src/boards/generic_stm32/i2c_bus.cpp b/radio/src/boards/generic_stm32/i2c_bus.cpp
new file mode 100644
index 000000000..3f78f12ab
--- /dev/null
+++ b/radio/src/boards/generic_stm32/i2c_bus.cpp
@@ -0,0 +1,96 @@
+/*
+ * 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 "stm32_i2c_driver.h"
+#include "stm32_hal_ll.h"
+
+#include "hal/i2c_driver.h"
+#include "hal.h"
+
+#define I2C_DEFAULT_TIMEOUT 10
+#define I2C_DEFAULT_RETRIES 2
+
+#if defined(I2C_B1)
+static const stm32_i2c_hw_def_t _i2c1 = {
+  .I2Cx = I2C_B1,
+  .SCL_GPIO = (gpio_t)I2C_B1_SCL_GPIO,
+  .SDA_GPIO = (gpio_t)I2C_B1_SDA_GPIO,
+  .GPIO_AF = I2C_B1_GPIO_AF,
+  .set_pwr = nullptr,
+};
+#endif
+
+#if defined(I2C_B2_PWR_GPIO)
+void _i2c_b2_pwr(bool enable)
+{
+  gpio_init(I2C_B2_PWR_GPIO, GPIO_OUT, GPIO_PIN_SPEED_LOW);
+  gpio_write(I2C_B2_PWR_GPIO, enable);
+}
+#else
+# define _i2c_b2_pwr nullptr
+#endif
+
+#if defined(I2C_B2)
+static const stm32_i2c_hw_def_t _i2c2 = {
+  .I2Cx = I2C_B2,
+  .SCL_GPIO = (gpio_t)I2C_B2_SCL_GPIO,
+  .SDA_GPIO = (gpio_t)I2C_B2_SDA_GPIO,
+  .GPIO_AF = I2C_B2_GPIO_AF,
+  .set_pwr = _i2c_b2_pwr,
+};
+#endif
+
+int i2c_init(etx_i2c_bus_t bus)
+{
+#if defined(I2C_B1)
+  if (bus == I2C_Bus_1) {
+    return stm32_i2c_init(I2C_Bus_1, I2C_B1_CLK_RATE, &_i2c1);
+  }
+#endif
+
+#if defined(I2C_B2)
+  if (bus == I2C_Bus_2) {
+    return stm32_i2c_init(I2C_Bus_2, I2C_B2_CLK_RATE, &_i2c2);
+  }
+#endif
+
+  return -1;
+}
+
+int i2c_dev_ready(etx_i2c_bus_t bus, uint16_t addr)
+{
+  return stm32_i2c_is_dev_ready(bus, addr, I2C_DEFAULT_RETRIES,
+                                I2C_DEFAULT_TIMEOUT);
+}
+
+int i2c_read(uint8_t bus, uint16_t addr, uint16_t reg, uint16_t reg_size,
+             uint8_t* data, uint16_t len)
+{
+  return stm32_i2c_read(bus, addr, reg, reg_size, data, len,
+                        I2C_DEFAULT_TIMEOUT);
+}
+
+int i2c_write(uint8_t bus, uint16_t addr, uint16_t reg, uint16_t reg_size,
+              uint8_t* data, uint16_t len)
+{
+  return stm32_i2c_write(bus, addr, reg, reg_size, data, len,
+                         I2C_DEFAULT_TIMEOUT);
+}
diff --git a/radio/src/drivers/pca95xx.cpp b/radio/src/drivers/pca95xx.cpp
new file mode 100644
index 000000000..71ecaa11c
--- /dev/null
+++ b/radio/src/drivers/pca95xx.cpp
@@ -0,0 +1,114 @@
+/*
+ * 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 "pca95xx.h"
+
+#define REG_INPUT     0x00
+#define REG_OUTPUT    0x02
+#define REG_POLARITY  0x04
+#define REG_DIRECTION 0x06
+
+static int pca95xx_i2c_write16(pca95xx_t* dev, uint16_t reg, uint16_t value)
+{
+  if (!dev->addr) return -1;
+
+  if (i2c_write(dev->bus, dev->addr, reg, 1, (uint8_t*)&value, sizeof(value)) < 0) {
+    return -1;
+  }
+
+  return 0;  
+}
+
+int pca95xx_init(pca95xx_t* dev, etx_i2c_bus_t bus, uint16_t addr)
+{
+  dev->bus = 0;
+  dev->addr = 0;
+  dev->polarity = 0;
+  dev->direction = 0xFFFF;
+  dev->output = 0;
+
+  if (i2c_init(bus) < 0) {
+    return -1;
+  }
+
+  if (i2c_dev_ready(bus, addr) < 0) {
+    return -1;
+  }
+
+  dev->bus = bus;
+  dev->addr = addr;
+  return 0;
+}
+
+int pca95xx_set_direction(pca95xx_t* dev, uint16_t mask, uint16_t dir)
+{
+  uint16_t tmp = dev->direction;
+  tmp &= ~mask;
+  tmp |= dir & mask;
+
+  if (pca95xx_i2c_write16(dev, REG_DIRECTION, tmp) < 0) {
+    return -1;
+  }
+
+  dev->direction = tmp;
+  return 0;
+}
+
+int pca95xx_set_polarity(pca95xx_t* dev, uint16_t mask, uint16_t dir)
+{
+  uint16_t tmp = dev->polarity;
+  tmp &= ~mask;
+  tmp |= dir & mask;
+
+  if (pca95xx_i2c_write16(dev, REG_POLARITY, tmp) < 0) {
+    return -1;
+  }
+
+  dev->polarity = tmp;
+  return 0;
+}
+
+int pca95xx_write(pca95xx_t* dev, uint16_t mask, uint16_t value)
+{
+  uint16_t tmp = dev->output;
+  tmp &= ~mask;
+  tmp |= value & mask;
+
+  if (pca95xx_i2c_write16(dev, REG_OUTPUT, tmp) < 0) {
+    return -1;
+  }
+
+  dev->output = tmp;
+  return 0;
+}
+
+int pca95xx_read(pca95xx_t* dev, uint16_t mask, uint16_t* value)
+{
+  if (!dev->addr) return -1;
+  
+  uint16_t tmp;
+  if (i2c_read(dev->bus, dev->addr, REG_INPUT, 1, (uint8_t*)&tmp, 2) < 0) {
+    return -1;
+  }
+
+  *value = tmp & mask;
+  return 0;
+}
diff --git a/radio/src/drivers/pca95xx.h b/radio/src/drivers/pca95xx.h
new file mode 100644
index 000000000..c4f598127
--- /dev/null
+++ b/radio/src/drivers/pca95xx.h
@@ -0,0 +1,55 @@
+/*
+ * 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.
+ */
+
+#pragma once
+
+#include "hal/i2c_driver.h"
+
+#define PCA95XX_PIN_0 (1 << 0)
+#define PCA95XX_PIN_1 (1 << 1)
+#define PCA95XX_PIN_2 (1 << 2)
+#define PCA95XX_PIN_3 (1 << 3)
+#define PCA95XX_PIN_4 (1 << 4)
+#define PCA95XX_PIN_5 (1 << 5)
+#define PCA95XX_PIN_6 (1 << 6)
+#define PCA95XX_PIN_7 (1 << 7)
+#define PCA95XX_PIN_8 (1 << 8)
+#define PCA95XX_PIN_9 (1 << 9)
+#define PCA95XX_PIN_10 (1 << 10)
+#define PCA95XX_PIN_11 (1 << 11)
+#define PCA95XX_PIN_12 (1 << 12)
+#define PCA95XX_PIN_13 (1 << 13)
+#define PCA95XX_PIN_14 (1 << 14)
+#define PCA95XX_PIN_15 (1 << 15)
+
+typedef struct {
+  etx_i2c_bus_t bus;
+  uint16_t      addr;
+  uint16_t      polarity;
+  uint16_t      direction;
+  uint16_t      output;
+} pca95xx_t;
+
+int pca95xx_init(pca95xx_t* dev, etx_i2c_bus_t bus, uint16_t addr);
+int pca95xx_set_direction(pca95xx_t* dev, uint16_t mask, uint16_t dir);
+int pca95xx_set_polarity(pca95xx_t* dev, uint16_t mask, uint16_t dir);
+int pca95xx_write(pca95xx_t* dev, uint16_t mask, uint16_t value);
+int pca95xx_read(pca95xx_t* dev, uint16_t mask, uint16_t* value);
diff --git a/radio/src/hal/i2c_driver.h b/radio/src/hal/i2c_driver.h
new file mode 100644
index 000000000..83d0fdf01
--- /dev/null
+++ b/radio/src/hal/i2c_driver.h
@@ -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.
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+typedef uint8_t etx_i2c_bus_t;
+
+int i2c_init(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,
+              uint8_t* data, uint16_t len);
+
+int i2c_write(uint8_t bus, uint16_t addr, uint16_t reg, uint16_t reg_size,
+              uint8_t* data, uint16_t len);
+
diff --git a/radio/src/targets/common/arm/stm32/lsm6ds_driver.cpp b/radio/src/targets/common/arm/stm32/lsm6ds_driver.cpp
index 026e252c9..b1b619927 100644
--- a/radio/src/targets/common/arm/stm32/lsm6ds_driver.cpp
+++ b/radio/src/targets/common/arm/stm32/lsm6ds_driver.cpp
@@ -19,16 +19,17 @@
  * GNU General Public License for more details.
  */
 
-#include <stdint.h>
 #include "gyro.h"
+#include "hal.h"
 
 #if defined(IMU_I2C_BUS) && defined(IMU_I2C_ADDRESS)
 
+#include "hal/i2c_driver.h"
 #include "stm32_i2c_driver.h"
 #include "delays_driver.h"
-#include "hal.h"
 
 #include <string.h>
+#include <stdint.h>
 
 /* COMMON VALUES FOR ACCEL-GYRO SENSORS */
 #define LSM6DS_WHO_AM_I                         0x0f
@@ -246,7 +247,7 @@ static int I2C_LSM6DS_ReadRegister(uint8_t reg)
 
 int gyroInit()
 {
-  if (stm32_i2c_init(IMU_I2C_BUS, IMU_I2C_CLK_RATE) < 0)
+  if (i2c_init(IMU_I2C_BUS) < 0)
     return -1;
 
   // LSM6DS33TR works with LSM6DSLTR code for our use
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 ae62d9cb7..764d74c01 100644
--- a/radio/src/targets/common/arm/stm32/stm32_i2c_driver.cpp
+++ b/radio/src/targets/common/arm/stm32/stm32_i2c_driver.cpp
@@ -19,94 +19,33 @@
  * GNU General Public License for more details.
  */
 
+#include "stm32_i2c_driver.h"
+#include "stm32_gpio.h"
 #include "stm32_hal_ll.h"
 #include "stm32_hal.h"
-#include "stm32_i2c_driver.h"
-#include "hal.h"
 
 #include "timers_driver.h"
 #include "debug.h"
 
-struct _i2c_defs {
-  GPIO_TypeDef*      SCL_GPIOx;
-  uint32_t           SCL_Pin;
-  GPIO_TypeDef*      SDA_GPIOx;
-  uint32_t           SDA_Pin;
-  uint32_t           Alternate;
-  void (*set_pwr)(bool enable);
+#define MAX_I2C_DEVICES 2
+
+struct stm32_i2c_device {
+  I2C_HandleTypeDef handle;
+  const stm32_i2c_hw_def_t* hw_def;
 };
 
-#if defined(I2C_B1)
-static I2C_HandleTypeDef hi2c1 = {
-  .Instance = I2C_B1,
-  .Init = { 0, 0, 0, 0, 0, 0, 0, 0 },
-};
-static const _i2c_defs pins_hi2c1 = {
-  .SCL_GPIOx = I2C_B1_GPIO,
-  .SCL_Pin = I2C_B1_SCL_GPIO_PIN,
-  .SDA_GPIOx = I2C_B1_GPIO,
-  .SDA_Pin = I2C_B1_SDA_GPIO_PIN,
-  .Alternate = I2C_B1_GPIO_AF,
-  .set_pwr = nullptr,
-};
-#endif
+static stm32_i2c_device _i2c_devs[MAX_I2C_DEVICES] = {};
 
-#if defined(I2C_B2)
-static I2C_HandleTypeDef hi2c2 = {
-  .Instance = I2C_B2,
-  .Init = { 0, 0, 0, 0, 0, 0, 0, 0 },
-};
-
-#if defined(I2C_B2_PWR_GPIO)
-void _i2c_b2_pwr(bool enable)
+static stm32_i2c_device* i2c_get_device(uint8_t bus)
 {
-  LL_GPIO_InitTypeDef pinInit;
-  LL_GPIO_StructInit(&pinInit);
-  
-  pinInit.Pin = I2C_B2_PWR_GPIO_PIN;
-  pinInit.Mode = LL_GPIO_MODE_OUTPUT;
-  pinInit.Pull = LL_GPIO_PULL_UP;
-  LL_GPIO_Init(I2C_B2_PWR_GPIO, &pinInit);
-
-  if (enable) {
-    LL_GPIO_SetOutputPin(I2C_B2_PWR_GPIO, I2C_B2_PWR_GPIO_PIN);
-  } else {
-    LL_GPIO_ResetOutputPin(I2C_B2_PWR_GPIO, I2C_B2_PWR_GPIO_PIN);
-  }
+  if (bus >= MAX_I2C_DEVICES) return nullptr;
+  return &_i2c_devs[bus];
 }
-#endif
 
-static const _i2c_defs pins_hi2c2 = {
-#if defined(I2C_B2_GPIO)
-  .SCL_GPIOx = I2C_B2_GPIO,
-#else
-  .SCL_GPIOx = I2C_B2_SCL_GPIO,
-#endif
-  .SCL_Pin = I2C_B2_SCL_GPIO_PIN,
-#if defined(I2C_B2_GPIO)
-  .SDA_GPIOx = I2C_B2_GPIO,
-#else
-  .SDA_GPIOx = I2C_B2_SDA_GPIO,
-#endif
-  .SDA_Pin = I2C_B2_SDA_GPIO_PIN,
-  .Alternate = I2C_B2_GPIO_AF,
-#if defined(I2C_B2_PWR_GPIO)
-  .set_pwr = _i2c_b2_pwr,
-#else
-  .set_pwr = nullptr,
-#endif
-};
-#endif
-
-static I2C_HandleTypeDef* get_i2c_handle(uint8_t bus)
+static I2C_HandleTypeDef* i2c_get_handle(uint8_t bus)
 {
-#if defined(I2C_B1)
-  if (bus == I2C_Bus_1) { return &hi2c1; }
-#endif
-#if defined(I2C_B2)
-  if (bus == I2C_Bus_2) { return &hi2c2; }
-#endif
-  return nullptr;
+  if (bus >= MAX_I2C_DEVICES) return nullptr;
+  return &_i2c_devs[bus].handle;
 }
 
 #if defined(STM32H7) || defined(STM32H7RS)
@@ -428,154 +367,6 @@ static int i2c_init_clock_source(I2C_TypeDef* instance)
 
 #endif
 
-int stm32_i2c_init(uint8_t bus, uint32_t clock_rate)
-{
-  I2C_HandleTypeDef* h = get_i2c_handle(bus);
-  if (!h) return -1;
-
-  I2C_InitTypeDef& init = h->Init;
-
-#if defined(STM32H7) || defined(STM32H7RS)
-  i2c_init_clock_source(h->Instance);
-# if defined(LL_RCC_I2C123_CLKSOURCE_PCLK1)
-  uint32_t pclk_freq = LL_RCC_GetI2CClockFreq(LL_RCC_I2C123_CLKSOURCE_PCLK1);
-# elif defined(LL_RCC_I2C1_CLKSOURCE_PCLK1)
-  uint32_t pclk_freq = LL_RCC_GetI2CClockFreq(LL_RCC_I2C1_CLKSOURCE_PCLK1);
-# endif
-  init.Timing = I2C_GetTiming(pclk_freq, clock_rate);
-#else
-  if (init.ClockSpeed > 0) {
-    if (init.ClockSpeed != clock_rate) return -1;
-    return 0;
-  }
-  init.ClockSpeed = clock_rate;
-  init.DutyCycle = I2C_DUTYCYCLE_16_9;
-#endif
-
-  init.OwnAddress1 = 0;
-  init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
-  init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
-  init.OwnAddress2 = 0;
-  init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
-  init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
-
-  if (HAL_I2C_Init(h) != HAL_OK) {
-    TRACE("I2C ERROR: HAL_I2C_Init() failed");
-    return -1;
-  }
-
-#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
-
-  return 1;
-}
-
-int stm32_i2c_deinit(uint8_t bus)
-{
-  I2C_HandleTypeDef* h = get_i2c_handle(bus);
-  if (!h) return -1;  
-
-  if (HAL_I2C_DeInit(h) != HAL_OK) return -1;
-  //h->Init.ClockSpeed = 0;
-  
-  return 0;
-}
-
-int stm32_i2c_master_tx(uint8_t bus, uint16_t addr, uint8_t *data, uint16_t len,
-                        uint32_t timeout)
-{
-  I2C_HandleTypeDef* h = get_i2c_handle(bus);
-  if (!h) return -1;  
-  
-  if (HAL_I2C_Master_Transmit(h, addr << 1, data, len, timeout) != HAL_OK) {
-    return -1;
-  }
-
-  return 0;
-}
-
-int stm32_i2c_master_rx(uint8_t bus, uint16_t addr, uint8_t *data, uint16_t len,
-                        uint32_t timeout)
-{
-  I2C_HandleTypeDef* h = get_i2c_handle(bus);
-  if (!h) return -1;  
-  
-  if (HAL_I2C_Master_Receive(h, addr << 1, data, len, timeout) != HAL_OK) {
-    return -1;
-  }
-
-  return 0;
-}
-
-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 = get_i2c_handle(bus);
-  if (!h) return -1;  
-
-  if (HAL_I2C_Mem_Read(h, addr << 1, reg, reg_size, data, len, timeout) != HAL_OK) {
-    return -1;
-  }
-
-  return 0;
-}
-
-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 = get_i2c_handle(bus);
-  if (!h) return -1;  
-  
-  if (HAL_I2C_Mem_Write(h, addr << 1, reg, reg_size, data, len, timeout) != HAL_OK) {
-    return -1;
-  }
-
-  return 0;
-}
-
-int stm32_i2c_is_dev_ready(uint8_t bus, uint16_t addr, uint32_t retries, uint32_t timeout)
-{
-  I2C_HandleTypeDef* h = get_i2c_handle(bus);
-  if (!h) return -1;
-
-  HAL_StatusTypeDef err = HAL_I2C_IsDeviceReady(h, addr << 1, retries, timeout);
-  if (err != HAL_OK) return -1;
-
-  return 0;
-}
-
-int stm32_i2c_is_dev_ready(uint8_t bus, uint16_t addr, uint32_t timeout)
-{
-  return stm32_i2c_is_dev_ready(bus, addr, 1, timeout);
-}
-
-static int i2c_enable_gpio_clock(GPIO_TypeDef *GPIOx)
-{
-  if (GPIOx == GPIOA)
-    __HAL_RCC_GPIOA_CLK_ENABLE();
-  else if (GPIOx == GPIOB)
-    __HAL_RCC_GPIOB_CLK_ENABLE();
-  else if (GPIOx == GPIOC)
-    __HAL_RCC_GPIOC_CLK_ENABLE();
-  else if (GPIOx == GPIOH)
-    __HAL_RCC_GPIOH_CLK_ENABLE();
-  else
-    return -1;
-  return 0;
-}
-
 static int i2c_enable_clock(I2C_TypeDef* instance)
 {
   /* Peripheral clock enable */
@@ -607,28 +398,16 @@ static int i2c_disable_clock(I2C_TypeDef* instance)
   return 0;
 }
 
-static int i2c_pins_init(const _i2c_defs* def)
+static int i2c_gpio_init(const stm32_i2c_hw_def_t* hw_def)
 {
-  if (i2c_enable_gpio_clock(def->SCL_GPIOx) < 0) return -1;
-  if (i2c_enable_gpio_clock(def->SDA_GPIOx) < 0) return -1;
+  gpio_init(hw_def->SCL_GPIO, GPIO_OD_PU, GPIO_PIN_SPEED_MEDIUM);
+  gpio_init_af(hw_def->SCL_GPIO, hw_def->GPIO_AF, GPIO_PIN_SPEED_MEDIUM);
 
-  LL_GPIO_InitTypeDef pinInit;
-  LL_GPIO_StructInit(&pinInit);
-  
-  pinInit.Speed = LL_GPIO_SPEED_FREQ_LOW;
-  pinInit.Mode = LL_GPIO_MODE_ALTERNATE;
-  pinInit.OutputType = LL_GPIO_OUTPUT_OPENDRAIN;
-  pinInit.Pull = LL_GPIO_PULL_NO;
-  pinInit.Alternate = def->Alternate;
+  gpio_init(hw_def->SDA_GPIO, GPIO_OD_PU, GPIO_PIN_SPEED_MEDIUM);
+  gpio_init_af(hw_def->SDA_GPIO, hw_def->GPIO_AF, GPIO_PIN_SPEED_MEDIUM);
 
-  pinInit.Pin = def->SCL_Pin;
-  LL_GPIO_Init(def->SCL_GPIOx, &pinInit);
-
-  pinInit.Pin = def->SDA_Pin;
-  LL_GPIO_Init(def->SDA_GPIOx, &pinInit);
-
-  if (def->set_pwr) {
-    def->set_pwr(true);
+  if (hw_def->set_pwr) {
+    hw_def->set_pwr(true);
     // Add some delay to leave enought time
     // for devices to boot before querying them
     HAL_Delay(20);
@@ -637,80 +416,174 @@ static int i2c_pins_init(const _i2c_defs* def)
   return 0;
 }
 
-static int i2c_pins_deinit(const _i2c_defs* def)
+static int i2c_gpio_deinit(const stm32_i2c_hw_def_t* hw_def)
 {
   // reconfigure pins as open-drain input
-  LL_GPIO_InitTypeDef pinInit;
-  LL_GPIO_StructInit(&pinInit);
-  
-  pinInit.Speed = LL_GPIO_SPEED_FREQ_MEDIUM;
-  pinInit.Mode = LL_GPIO_MODE_INPUT;
-  pinInit.OutputType = LL_GPIO_OUTPUT_OPENDRAIN;
-  pinInit.Pull = LL_GPIO_PULL_NO;
-  pinInit.Alternate = 0;
+  gpio_init(hw_def->SCL_GPIO, GPIO_IN, GPIO_PIN_SPEED_LOW);
+  gpio_init(hw_def->SDA_GPIO, GPIO_IN, GPIO_PIN_SPEED_LOW);
 
-  pinInit.Pin = def->SCL_Pin;
-  LL_GPIO_Init(def->SCL_GPIOx, &pinInit);
-
-  pinInit.Pin = def->SDA_Pin;
-  LL_GPIO_Init(def->SDA_GPIOx, &pinInit);
-
-  if (def->set_pwr) {
-    def->set_pwr(false);
+  if (hw_def->set_pwr) {
+    hw_def->set_pwr(false);
   }
 
   return 0;
 }
 
-/**
-  * @brief  Initialize the I2C MSP.
-  * @param  h Pointer to a I2C_HandleTypeDef structure that contains
-  *         the configuration information for the specified I2C.
-  * @retval None
-  */
-void HAL_I2C_MspInit(I2C_HandleTypeDef *h)
+
+int stm32_i2c_init(uint8_t bus, uint32_t clock_rate, const stm32_i2c_hw_def_t* hw_def)
 {
-  const _i2c_defs* defs = nullptr;
-#if defined(I2C_B1)
-  if (h == &hi2c1) { defs = &pins_hi2c1; }
-#endif
-#if defined(I2C_B2)
-  if (h == &hi2c2) { defs = &pins_hi2c2; }
-#endif
-  if (!defs) return;
-  
-  if (i2c_pins_init(defs) < 0) {
-    TRACE("I2C ERROR: HAL_I2C_MspInit() I2C_GPIO misconfiguration");
-    return;
+  auto dev = i2c_get_device(bus);
+  if (!dev) return -1;
+
+  if (!dev->hw_def) {
+    dev->hw_def = hw_def;
+    dev->handle.Instance = hw_def->I2Cx;
   }
 
-  if (i2c_enable_clock(h->Instance) < 0) {
-    TRACE("I2C ERROR: HAL_I2C_MspInit() I2C misconfiguration");
-    return;
+  auto h = &dev->handle;
+  I2C_InitTypeDef& init = h->Init;
+
+#if defined(STM32H7) || defined(STM32H7RS)
+  i2c_init_clock_source(h->Instance);
+# if defined(LL_RCC_I2C123_CLKSOURCE_PCLK1)
+  uint32_t pclk_freq = LL_RCC_GetI2CClockFreq(LL_RCC_I2C123_CLKSOURCE_PCLK1);
+# elif defined(LL_RCC_I2C1_CLKSOURCE_PCLK1)
+  uint32_t pclk_freq = LL_RCC_GetI2CClockFreq(LL_RCC_I2C1_CLKSOURCE_PCLK1);
+# endif
+  init.Timing = I2C_GetTiming(pclk_freq, clock_rate);
+#else
+  if (init.ClockSpeed > 0) {
+    if (init.ClockSpeed != clock_rate) return -1;
+    return 0;
   }
+  init.ClockSpeed = clock_rate;
+  init.DutyCycle = I2C_DUTYCYCLE_16_9;
+#endif
+
+  init.OwnAddress1 = 0;
+  init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
+  init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
+  init.OwnAddress2 = 0;
+  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 -1;
+  }
+
+  if (i2c_enable_clock(hw_def->I2Cx) < 0) {
+    TRACE("I2C ERROR: HAL_I2C_MspInit() I2C misconfiguration");
+    return -1;
+  }
+
+  if (HAL_I2C_Init(h) != HAL_OK) {
+    TRACE("I2C ERROR: HAL_I2C_Init() failed");
+    return -1;
+  }
+
+#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
+
+  return 1;
 }
 
-/**
-  * @brief  DeInitialize the I2C MSP.
-  * @param  h Pointer to a I2C_HandleTypeDef structure that contains
-  *         the configuration information for the specified I2C.
-  * @retval None
-  */
-void HAL_I2C_MspDeInit(I2C_HandleTypeDef *h)
+int stm32_i2c_deinit(uint8_t bus)
 {
-  const _i2c_defs* defs = nullptr;
-#if defined(I2C_B1)
-  if (h == &hi2c1) { defs = &pins_hi2c1; }
-#endif
-#if defined(I2C_B2)
-  if (h == &hi2c2) { defs = &pins_hi2c2; }
-#endif
-  if (!defs) return;
+  I2C_HandleTypeDef* h = i2c_get_handle(bus);
+  if (!h) return -1;  
 
+  if (HAL_I2C_DeInit(h) != HAL_OK) return -1;
+  
   if (i2c_disable_clock(h->Instance) < 0) {
     TRACE("I2C ERROR: HAL_I2C_MspDeInit() I2C misconfiguration");
-    return;
+    return -1;
   }
 
-  i2c_pins_deinit(defs);
+  auto dev = i2c_get_device(bus);
+  if (!dev) return -1;
+
+  i2c_gpio_deinit(dev->hw_def);
+
+  return 0;
+}
+
+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;  
+  
+  if (HAL_I2C_Master_Transmit(h, addr << 1, data, len, timeout) != HAL_OK) {
+    return -1;
+  }
+
+  return 0;
+}
+
+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;  
+  
+  if (HAL_I2C_Master_Receive(h, addr << 1, data, len, timeout) != HAL_OK) {
+    return -1;
+  }
+
+  return 0;
+}
+
+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;  
+
+  if (HAL_I2C_Mem_Read(h, addr << 1, reg, reg_size, data, len, timeout) != HAL_OK) {
+    return -1;
+  }
+
+  return 0;
+}
+
+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;  
+  
+  if (HAL_I2C_Mem_Write(h, addr << 1, reg, reg_size, data, len, timeout) != HAL_OK) {
+    return -1;
+  }
+
+  return 0;
+}
+
+int stm32_i2c_is_dev_ready(uint8_t bus, uint16_t addr, uint32_t retries, uint32_t timeout)
+{
+  I2C_HandleTypeDef* h = i2c_get_handle(bus);
+  if (!h) return -1;
+
+  HAL_StatusTypeDef err = HAL_I2C_IsDeviceReady(h, addr << 1, retries, timeout);
+  if (err != HAL_OK) return -1;
+
+  return 0;
+}
+
+int stm32_i2c_is_dev_ready(uint8_t bus, uint16_t addr, uint32_t timeout)
+{
+  return stm32_i2c_is_dev_ready(bus, addr, 1, timeout);
 }
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 8458a1f37..015015334 100644
--- a/radio/src/targets/common/arm/stm32/stm32_i2c_driver.h
+++ b/radio/src/targets/common/arm/stm32/stm32_i2c_driver.h
@@ -21,13 +21,27 @@
 
 #pragma once
 
+#include "stm32_cmsis.h"
+#include "stm32_gpio.h"
+#include "hal/gpio.h"
+
 #include <stdint.h>
 
+// Maxmimum 2 I2C bus instances
 enum { I2C_Bus_1 = 0, I2C_Bus_2 };
 
+typedef struct {
+  I2C_TypeDef* I2Cx;
+  gpio_t       SCL_GPIO;
+  gpio_t       SDA_GPIO;
+  uint32_t     GPIO_AF;
+  void (*set_pwr)(bool enable);
+} stm32_i2c_hw_def_t;
+
 // Init I2C bus
 // @return -1 if error, 0 if already running, 1 otherwise
-int stm32_i2c_init(uint8_t bus, uint32_t clock_rate);
+int stm32_i2c_init(uint8_t bus, uint32_t clock_rate,
+                   const stm32_i2c_hw_def_t* hw_def);
 
 // DeInit I2C bus
 // @return -1 if error, 0 otherwise
diff --git a/radio/src/targets/horus/cst8xx_driver.cpp b/radio/src/targets/horus/cst8xx_driver.cpp
index f0745d428..aea76cc32 100644
--- a/radio/src/targets/horus/cst8xx_driver.cpp
+++ b/radio/src/targets/horus/cst8xx_driver.cpp
@@ -25,6 +25,7 @@
 #include "delays_driver.h"
 #include "hal.h"
 #include "hal/gpio.h"
+#include "hal/i2c_driver.h"
 #include "rtos.h"
 #include "stm32_exti_driver.h"
 #include "stm32_gpio.h"
@@ -62,8 +63,8 @@ void initTouchI2C(void)
 {
   TRACE("CST836U I2C Init");
 
-  if (stm32_i2c_init(TOUCH_I2C_BUS, TOUCH_I2C_CLK_RATE) < 0) {
-    TRACE("CST836U ERROR: stm32_i2c_init failed");
+  if (i2c_init(TOUCH_I2C_BUS) < 0) {
+    TRACE("CST836U ERROR: i2c_init failed");
     return;
   }
 }
diff --git a/radio/src/targets/horus/hal.h b/radio/src/targets/horus/hal.h
index 94016249f..2027ff83c 100644
--- a/radio/src/targets/horus/hal.h
+++ b/radio/src/targets/horus/hal.h
@@ -828,7 +828,6 @@
 // Touch
 #if defined(HARDWARE_TOUCH)
   #define TOUCH_I2C_BUS                   I2C_Bus_1
-  #define TOUCH_I2C_CLK_RATE              400000
   #define TOUCH_INT_GPIO                  GPIO_PIN(GPIOH, 2)  // PH.02
 #if defined(PCBX12S)
   #define TOUCH_RST_GPIO                  GPIO_PIN(GPIOF, 7)  // PF.7
@@ -851,30 +850,29 @@
 
 // First I2C Bus
 #if defined(RADIO_TX16S) || defined(RADIO_F16) || defined(PCBX12S) || defined(RADIO_T15)
-  #define I2C_B1                          I2C1
-  #define I2C_B1_GPIO                     GPIOB
-  #define I2C_B1_SCL_GPIO_PIN             LL_GPIO_PIN_8  // PB.08
-  #define I2C_B1_SDA_GPIO_PIN             LL_GPIO_PIN_9  // PB.09
-  #define I2C_B1_GPIO_AF                  LL_GPIO_AF_4   // I2C1
+  #define I2C_B1                      I2C1
+  #define I2C_B1_SCL_GPIO             GPIO_PIN(GPIOB, 8)  // PB.08
+  #define I2C_B1_SDA_GPIO             GPIO_PIN(GPIOB, 9)  // PB.09
+  #define I2C_B1_GPIO_AF              LL_GPIO_AF_4   // I2C1
+  #define I2C_B1_CLK_RATE             400000
 #else
-  #define I2C_B1                          I2C3
-  #define I2C_B1_GPIO                     GPIOH
-  #define I2C_B1_SCL_GPIO_PIN             LL_GPIO_PIN_7  // PH.07
-  #define I2C_B1_SDA_GPIO_PIN             LL_GPIO_PIN_8  // PH.08
-  #define I2C_B1_GPIO_AF                  LL_GPIO_AF_4   // I2C3
+  #define I2C_B1                      I2C3
+  #define I2C_B1_SCL_GPIO             GPIO_PIN(GPIOH, 7)  // PH.07
+  #define I2C_B1_SDA_GPIO             GPIO_PIN(GPIOH, 8)  // PH.08
+  #define I2C_B1_GPIO_AF              LL_GPIO_AF_4   // I2C3
+  #define I2C_B1_CLK_RATE             400000
 #endif
 
 // Second I2C Bus
 #if !defined(AUX_SERIAL) && defined(IMU_LSM6DS33)
-  #define I2C_B2                          I2C2
-  #define I2C_B2_GPIO                     GPIOB
-  #define I2C_B2_SCL_GPIO_PIN             LL_GPIO_PIN_10  // PB.10
-  #define I2C_B2_SDA_GPIO_PIN             LL_GPIO_PIN_11  // PB.11
-  #define I2C_B2_GPIO_AF                  LL_GPIO_AF_4    // I2C2
- #if defined(RADIO_TX16S) || defined(RADIO_F16)
-   #define I2C_B2_PWR_GPIO                GPIOA
-   #define I2C_B2_PWR_GPIO_PIN            LL_GPIO_PIN_15  // PA.15
- #endif
+  #define I2C_B2                      I2C2
+  #define I2C_B2_SCL_GPIO_PIN         GPIO_PIN(GPIOB, 10)  // PB.10
+  #define I2C_B2_SDA_GPIO_PIN         GPIO_PIN(GPIOB, 11)  // PB.11
+  #define I2C_B2_GPIO_AF              LL_GPIO_AF_4    // I2C2
+  #define I2C_B2_CLK_RATE             400000
+  #if defined(RADIO_TX16S) || defined(RADIO_F16)
+    #define I2C_B2_PWR_GPIO           GPIO_PIN(GPIOA, 15)  // PA.15
+  #endif
 #endif
 
 #if defined(IMU)
@@ -885,7 +883,6 @@
    #define IMU_I2C_BUS                   I2C_Bus_2
    #define IMU_I2C_ADDRESS               0x6A
  #endif
- #define IMU_I2C_CLK_RATE                400000
 #endif
 
 // Haptic
diff --git a/radio/src/targets/horus/tp_gt911.cpp b/radio/src/targets/horus/tp_gt911.cpp
index 43af66d1e..99984b237 100644
--- a/radio/src/targets/horus/tp_gt911.cpp
+++ b/radio/src/targets/horus/tp_gt911.cpp
@@ -20,6 +20,7 @@
  */
 
 #include "hal/gpio.h"
+#include "hal/i2c_driver.h"
 #include "stm32_gpio.h"
 #include "stm32_hal_ll.h"
 #include "stm32_hal.h"
@@ -446,8 +447,8 @@ void I2C_Init_Radio(void)
 {
   TRACE("GT911 I2C Init");
 
-  if (stm32_i2c_init(TOUCH_I2C_BUS, TOUCH_I2C_CLK_RATE) < 0) {
-    TRACE("GT911 ERROR: stm32_i2c_init failed");
+  if (i2c_init(TOUCH_I2C_BUS) < 0) {
+    TRACE("GT911 ERROR: i2c_init failed");
     return;
   }
 }
diff --git a/radio/src/targets/nv14/hal.h b/radio/src/targets/nv14/hal.h
index df31a3596..b27f16aa3 100644
--- a/radio/src/targets/nv14/hal.h
+++ b/radio/src/targets/nv14/hal.h
@@ -328,15 +328,13 @@
 
 // I2C Bus
 #define I2C_B1                        I2C1
-#define I2C_B1_GPIO                   GPIOB
-#define I2C_B1_SCL_GPIO_PIN           LL_GPIO_PIN_8  // PB.08
-#define I2C_B1_SDA_GPIO_PIN           LL_GPIO_PIN_7  // PB.09
+#define I2C_B1_SCL_GPIO               GPIO_PIN(GPIOB, 8)  // PB.08
+#define I2C_B1_SDA_GPIO               GPIO_PIN(GPIOB, 7)  // PB.07
 #define I2C_B1_GPIO_AF                LL_GPIO_AF_4
+#define I2C_B1_CLK_RATE               400000
 
 // Touch
 #define TOUCH_I2C_BUS                 I2C_Bus_1
-#define TOUCH_I2C_CLK_RATE            400000
-
 #define TOUCH_INT_GPIO                GPIO_PIN(GPIOB, 9)  // PB.09
 #define TOUCH_RST_GPIO                GPIO_PIN(GPIOB, 12) // PB.12
 
diff --git a/radio/src/targets/nv14/touch_driver.cpp b/radio/src/targets/nv14/touch_driver.cpp
index 568f4806f..e72a4f8c9 100644
--- a/radio/src/targets/nv14/touch_driver.cpp
+++ b/radio/src/targets/nv14/touch_driver.cpp
@@ -20,6 +20,7 @@
  */
 
 #include "hal/gpio.h"
+#include "hal/i2c_driver.h"
 #include "stm32_gpio.h"
 #include "stm32_hal_ll.h"
 #include "stm32_hal.h"
@@ -72,8 +73,8 @@ void I2C_Init_Radio(void)
 {
   TRACE("CST836U I2C Init");
 
-  if (stm32_i2c_init(TOUCH_I2C_BUS, TOUCH_I2C_CLK_RATE) < 0) {
-    TRACE("CST836U ERROR: stm32_i2c_init failed");
+  if (i2c_init(TOUCH_I2C_BUS) < 0) {
+    TRACE("CST836U ERROR: i2c_init failed");
     return;
   }
 }
diff --git a/radio/src/targets/pl18/hal.h b/radio/src/targets/pl18/hal.h
index bbf2a40ef..29271167f 100644
--- a/radio/src/targets/pl18/hal.h
+++ b/radio/src/targets/pl18/hal.h
@@ -453,16 +453,16 @@
 
 // I2C Bus
 #define I2C_B1                          I2C1
-#define I2C_B1_GPIO                     GPIOB
-#define I2C_B1_SDA_GPIO_PIN             LL_GPIO_PIN_7  // PB.07
-#define I2C_B1_SCL_GPIO_PIN             LL_GPIO_PIN_8  // PB.08
+#define I2C_B1_SCL_GPIO                 GPIO_PIN(GPIOB, 8)  // PB.08
+#define I2C_B1_SDA_GPIO                 GPIO_PIN(GPIOB, 7)  // PB.07
 #define I2C_B1_GPIO_AF                  LL_GPIO_AF_4
+#define I2C_B1_CLK_RATE                 400000
 
 // Touch
 #define TOUCH_I2C_BUS                   I2C_Bus_1
-#define TOUCH_I2C_CLK_RATE              400000
-#define TOUCH_INT_GPIO                  GPIO_PIN(GPIOB, 9)  // PB.09
+#define TOUCH_INT_GPIO                  GPIO_PIN(GPIOB, 9)   // PB.09
 #define TOUCH_RST_GPIO                  GPIO_PIN(GPIOB, 12)  // PB.12
+
 #define TOUCH_INT_EXTI_Line             LL_EXTI_LINE_9
 #define TOUCH_INT_EXTI_Port             LL_SYSCFG_EXTI_PORTB
 #define TOUCH_INT_EXTI_SysCfgLine       LL_SYSCFG_EXTI_LINE9
diff --git a/radio/src/targets/pl18/touch_driver.cpp b/radio/src/targets/pl18/touch_driver.cpp
index 983f4c24c..1815b2908 100644
--- a/radio/src/targets/pl18/touch_driver.cpp
+++ b/radio/src/targets/pl18/touch_driver.cpp
@@ -20,6 +20,7 @@
  */
 
 #include "hal/gpio.h"
+#include "hal/i2c_driver.h"
 #include "stm32_gpio.h"
 #include "stm32_hal_ll.h"
 #include "stm32_hal.h"
@@ -122,8 +123,8 @@ static void _i2c_init(void)
 {
   TRACE("Touch I2C Init");
 
-  if (stm32_i2c_init(TOUCH_I2C_BUS, TOUCH_I2C_CLK_RATE) < 0) {
-    TRACE("Touch I2C Init ERROR: stm32_i2c_init failed");
+  if (i2c_init(TOUCH_I2C_BUS) < 0) {
+    TRACE("Touch I2C Init ERROR: i2c_init failed");
     return;
   }
 }
diff --git a/radio/src/targets/taranis/eeprom_driver.cpp b/radio/src/targets/taranis/eeprom_driver.cpp
index e9c40ea2f..755bfd99c 100644
--- a/radio/src/targets/taranis/eeprom_driver.cpp
+++ b/radio/src/targets/taranis/eeprom_driver.cpp
@@ -25,6 +25,7 @@
 #include "stm32_gpio_driver.h"
 
 #include "hal.h"
+#include "hal/i2c_driver.h"
 #include "hal/eeprom_driver.h"
 
 static void init_wp_pin()
@@ -47,8 +48,7 @@ static void init_wp_pin()
 
 void eepromInit()
 {
-  stm32_i2c_deinit(EEPROM_I2C_BUS);
-  stm32_i2c_init(EEPROM_I2C_BUS, I2C_B1_CLK_RATE);
+  i2c_init(EEPROM_I2C_BUS);
   init_wp_pin();
 }
 
@@ -59,7 +59,7 @@ void eepromInit()
   */
 static bool I2C_EE_WaitEepromStandbyState(void)
 {
-  if (stm32_i2c_is_dev_ready(EEPROM_I2C_BUS, EEPROM_I2C_ADDRESS, 20 /* 20ms */) < 0)
+  if (i2c_dev_ready(EEPROM_I2C_BUS, EEPROM_I2C_ADDRESS) < 0)
     return false;
 
   return true;
diff --git a/radio/src/targets/taranis/hal.h b/radio/src/targets/taranis/hal.h
index edd62e00d..93969b6cb 100644
--- a/radio/src/targets/taranis/hal.h
+++ b/radio/src/targets/taranis/hal.h
@@ -2456,13 +2456,11 @@
 
 #if defined(PCBXLITE) || defined(PCBX9LITE) || defined(PCBX7ACCESS) || \
     defined(RADIO_ZORRO) || defined(RADIO_POCKET) || defined(RADIO_X9DP2019)
-  #define I2C_B1_GPIO                   GPIOB
-  #define I2C_B1_SDA_GPIO_PIN           LL_GPIO_PIN_9  // PB.09
-  #define I2C_B1_SCL_GPIO_PIN           LL_GPIO_PIN_8  // PB.08
+  #define I2C_B1_SCL_GPIO               GPIO_PIN(GPIOB, 8)  // PB.08
+  #define I2C_B1_SDA_GPIO               GPIO_PIN(GPIOB, 9)  // PB.09
 #else
-  #define I2C_B1_GPIO                   GPIOB
-  #define I2C_B1_SCL_GPIO_PIN           LL_GPIO_PIN_6  // PB.06
-  #define I2C_B1_SDA_GPIO_PIN           LL_GPIO_PIN_7  // PB.07
+  #define I2C_B1_SCL_GPIO               GPIO_PIN(GPIOB, 6)  // PB.06
+  #define I2C_B1_SDA_GPIO               GPIO_PIN(GPIOB, 7)  // PB.07
 #endif
 
 // EEPROM
@@ -2500,16 +2498,13 @@
 // Second I2C Bus: IMU
 #if defined(PCBXLITES)
   #define I2C_B2                        I2C3
-  #define I2C_B2_SCL_GPIO               GPIOA
-  #define I2C_B2_SCL_GPIO_PIN           LL_GPIO_PIN_8  // PA.08
-  #define I2C_B2_SDA_GPIO               GPIOC
-  #define I2C_B2_SDA_GPIO_PIN           LL_GPIO_PIN_9  // PC.09
+  #define I2C_B2_SCL_GPIO               GPIO_PIN(GPIOA, 8) // PA.08
+  #define I2C_B2_SDA_GPIO               GPIO_PIN(GPIOC, 9) // PC.09
   #define I2C_B2_GPIO_AF                LL_GPIO_AF_4
-  #define I2C_B2_SPEED                  400000
+  #define I2C_B2_CLK_RATE               400000
 
   #define IMU_I2C_BUS                   I2C_Bus_2
   #define IMU_I2C_ADDRESS               0x6B
-  #define IMU_I2C_CLK_RATE              I2C_B2_SPEED
 #endif
 
 // SD - SPI2
diff --git a/radio/src/targets/taranis/volume_i2c.cpp b/radio/src/targets/taranis/volume_i2c.cpp
index e6c7e8c1e..b4ce4f316 100644
--- a/radio/src/targets/taranis/volume_i2c.cpp
+++ b/radio/src/targets/taranis/volume_i2c.cpp
@@ -21,7 +21,9 @@
 
 #if !defined(SOFTWARE_VOLUME)
 
+#include "hal/i2c_driver.h"
 #include "stm32_i2c_driver.h"
+
 #include "board.h"
 
 static const uint8_t volumeScale[VOLUME_LEVEL_MAX+1] = {
@@ -32,7 +34,7 @@ static const uint8_t volumeScale[VOLUME_LEVEL_MAX+1] = {
 int32_t getVolume()
 {
   uint8_t value = 0;
-  if (stm32_i2c_read(VOLUME_I2C_BUS, VOLUME_I2C_ADDRESS, 0, 1, &value, 1, 10) < 0)
+  if (i2c_read(VOLUME_I2C_BUS, VOLUME_I2C_ADDRESS, 0, 1, &value, 1) < 0)
     return -1;
 
   return value;
@@ -40,8 +42,8 @@ int32_t getVolume()
 
 void setVolume(uint8_t volume)
 {
-  stm32_i2c_init(VOLUME_I2C_BUS, I2C_B1_CLK_RATE);
-  stm32_i2c_write(VOLUME_I2C_BUS, VOLUME_I2C_ADDRESS, 0, 1, &volume, 1, 10);
+  i2c_init(VOLUME_I2C_BUS);
+  i2c_write(VOLUME_I2C_BUS, VOLUME_I2C_ADDRESS, 0, 1, &volume, 1);
 }
 
 void setScaledVolume(uint8_t volume)