diff --git a/src/main/drivers/bus_i2c_config.c b/src/main/drivers/bus_i2c_config.c
index 8eed1c1c2..f1469cacc 100644
--- a/src/main/drivers/bus_i2c_config.c
+++ b/src/main/drivers/bus_i2c_config.c
@@ -68,10 +68,18 @@ void i2cHardwareConfigure(const i2cConfig_t *i2cConfig)
         memset(pDev, 0, sizeof(*pDev));
 
         for (int pindex = 0 ; pindex < I2C_PIN_SEL_MAX ; pindex++) {
-            if (i2cConfig->ioTagScl[device] == hardware->sclPins[pindex])
-              pDev->scl = IOGetByTag(i2cConfig->ioTagScl[device]);
-            if (i2cConfig->ioTagSda[device] == hardware->sdaPins[pindex])
-              pDev->sda = IOGetByTag(i2cConfig->ioTagSda[device]);
+            if (i2cConfig->ioTagScl[device] == hardware->sclPins[pindex].ioTag) {
+                pDev->scl = IOGetByTag(i2cConfig->ioTagScl[device]);
+#if defined(STM32F4)
+                pDev->sclAF = hardware->sclPins[pindex].af;
+#endif
+            }
+            if (i2cConfig->ioTagSda[device] == hardware->sdaPins[pindex].ioTag) {
+                pDev->sda = IOGetByTag(i2cConfig->ioTagSda[device]);
+#if defined(STM32F4)
+                pDev->sdaAF = hardware->sdaPins[pindex].af;
+#endif
+            }
         }
 
         if (pDev->scl && pDev->sda) {
@@ -82,4 +90,5 @@ void i2cHardwareConfigure(const i2cConfig_t *i2cConfig)
         }
     }
 }
+
 #endif // defined(USE_I2C) && !defined(USE_SOFT_I2C)
diff --git a/src/main/drivers/bus_i2c_hal.c b/src/main/drivers/bus_i2c_hal.c
index b5fb760c0..a31fefb8b 100644
--- a/src/main/drivers/bus_i2c_hal.c
+++ b/src/main/drivers/bus_i2c_hal.c
@@ -46,8 +46,8 @@ const i2cHardware_t i2cHardware[I2CDEV_COUNT] = {
     {
         .device = I2CDEV_1,
         .reg = I2C1,
-        .sclPins = { DEFIO_TAG_E(PB6), DEFIO_TAG_E(PB8) },
-        .sdaPins = { DEFIO_TAG_E(PB7), DEFIO_TAG_E(PB9) },
+        .sclPins = { I2CPINDEF(PB6), I2CPINDEF(PB8) },
+        .sdaPins = { I2CPINDEF(PB7), I2CPINDEF(PB9) },
         .rcc = RCC_APB1(I2C1),
         .ev_irq = I2C1_EV_IRQn,
         .er_irq = I2C1_ER_IRQn,
@@ -57,8 +57,8 @@ const i2cHardware_t i2cHardware[I2CDEV_COUNT] = {
     {
         .device = I2CDEV_2,
         .reg = I2C2,
-        .sclPins = { DEFIO_TAG_E(PB10), DEFIO_TAG_E(PF1) },
-        .sdaPins = { DEFIO_TAG_E(PB11), DEFIO_TAG_E(PF0) },
+        .sclPins = { I2CPINDEF(PB10), I2CPINDEF(PF1) },
+        .sdaPins = { I2CPINDEF(PB11), I2CPINDEF(PF0) },
         .rcc = RCC_APB1(I2C2),
         .ev_irq = I2C2_EV_IRQn,
         .er_irq = I2C2_ER_IRQn,
@@ -68,8 +68,8 @@ const i2cHardware_t i2cHardware[I2CDEV_COUNT] = {
     {
         .device = I2CDEV_3,
         .reg = I2C3,
-        .sclPins = { DEFIO_TAG_E(PA8) },
-        .sdaPins = { DEFIO_TAG_E(PC9) },
+        .sclPins = { I2CPINDEF(PA8) },
+        .sdaPins = { I2CPINDEF(PC9) },
         .rcc = RCC_APB1(I2C3),
         .ev_irq = I2C3_EV_IRQn,
         .er_irq = I2C3_ER_IRQn,
@@ -79,8 +79,8 @@ const i2cHardware_t i2cHardware[I2CDEV_COUNT] = {
     {
         .device = I2CDEV_4,
         .reg = I2C4,
-        .sclPins = { DEFIO_TAG_E(PD12), DEFIO_TAG_E(PF14) },
-        .sdaPins = { DEFIO_TAG_E(PD13), DEFIO_TAG_E(PF15) },
+        .sclPins = { I2CPINDEF(PD12), I2CPINDEF(PF14) },
+        .sdaPins = { I2CPINDEF(PD13), I2CPINDEF(PF15) },
         .rcc = RCC_APB1(I2C4),
         .ev_irq = I2C4_EV_IRQn,
         .er_irq = I2C4_ER_IRQn,
diff --git a/src/main/drivers/bus_i2c_impl.h b/src/main/drivers/bus_i2c_impl.h
index 839584593..42cb32751 100644
--- a/src/main/drivers/bus_i2c_impl.h
+++ b/src/main/drivers/bus_i2c_impl.h
@@ -26,13 +26,28 @@
 #define I2C_LONG_TIMEOUT             ((uint32_t)(10 * I2C_SHORT_TIMEOUT))
 #define I2C_DEFAULT_TIMEOUT          I2C_SHORT_TIMEOUT
 
-#define I2C_PIN_SEL_MAX 3
+#define I2C_PIN_SEL_MAX 4
+
+typedef struct i2cPinDef_s {
+    ioTag_t ioTag;
+#if defined(STM32F4)
+    uint8_t af;
+#endif
+} i2cPinDef_t;
+
+#if defined(STM32F4)
+#define I2CPINDEF(pin, af) { DEFIO_TAG_E(pin), af }
+#elif defined(STM32F1)
+#define I2CPINDEF(pin, af) { DEFIO_TAG_E(pin) }
+#else
+#define I2CPINDEF(pin) { DEFIO_TAG_E(pin) }
+#endif
 
 typedef struct i2cHardware_s {
     I2CDevice device;
     I2C_TypeDef *reg;
-    ioTag_t sclPins[I2C_PIN_SEL_MAX];
-    ioTag_t sdaPins[I2C_PIN_SEL_MAX];
+    i2cPinDef_t sclPins[I2C_PIN_SEL_MAX];
+    i2cPinDef_t sdaPins[I2C_PIN_SEL_MAX];
     rccPeriphTag_t rcc;
 #if !defined(STM32F303xC)
     uint8_t ev_irq;
@@ -61,6 +76,10 @@ typedef struct i2cDevice_s {
     I2C_TypeDef *reg;
     IO_t scl;
     IO_t sda;
+#ifdef STM32F4
+    uint8_t sclAF;
+    uint8_t sdaAF;
+#endif
     bool overClock;
     bool pullUp;
 
diff --git a/src/main/drivers/bus_i2c_stm32f10x.c b/src/main/drivers/bus_i2c_stm32f10x.c
index a38e39445..9734a57e3 100644
--- a/src/main/drivers/bus_i2c_stm32f10x.c
+++ b/src/main/drivers/bus_i2c_stm32f10x.c
@@ -38,8 +38,6 @@ static void i2c_er_handler(I2CDevice device);
 static void i2c_ev_handler(I2CDevice device);
 static void i2cUnstick(IO_t scl, IO_t sda);
 
-#define GPIO_AF_I2C GPIO_AF_I2C1
-
 #ifdef STM32F4
 #define IOCFG_I2C_PU IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_OD, GPIO_PuPd_UP)
 #define IOCFG_I2C    IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)
@@ -52,8 +50,14 @@ const i2cHardware_t i2cHardware[I2CDEV_COUNT] = {
     {
         .device = I2CDEV_1,
         .reg = I2C1,
-        .sclPins = { DEFIO_TAG_E(PB6), DEFIO_TAG_E(PB8) },
-        .sdaPins = { DEFIO_TAG_E(PB7), DEFIO_TAG_E(PB9) },
+        .sclPins = {
+            I2CPINDEF(PB6, GPIO_AF_I2C1),
+            I2CPINDEF(PB8, GPIO_AF_I2C1),
+        },
+        .sdaPins = {
+            I2CPINDEF(PB7, GPIO_AF_I2C1),
+            I2CPINDEF(PB9, GPIO_AF_I2C1),
+        },
         .rcc = RCC_APB1(I2C1),
         .ev_irq = I2C1_EV_IRQn,
         .er_irq = I2C1_ER_IRQn,
@@ -63,8 +67,20 @@ const i2cHardware_t i2cHardware[I2CDEV_COUNT] = {
     {
         .device = I2CDEV_2,
         .reg = I2C2,
-        .sclPins = { DEFIO_TAG_E(PB10), DEFIO_TAG_E(PF1) },
-        .sdaPins = { DEFIO_TAG_E(PB11), DEFIO_TAG_E(PF0) },
+        .sclPins = {
+            I2CPINDEF(PB10, GPIO_AF_I2C2),
+            I2CPINDEF(PF1,  GPIO_AF_I2C2),
+        },
+        .sdaPins = {
+            I2CPINDEF(PB11, GPIO_AF_I2C2),
+            I2CPINDEF(PF0,  GPIO_AF_I2C2),
+
+#if defined(STM32F40_41xxx) || defined (STM32F411xE)
+            // STM32F401xx/STM32F410xx/STM32F411xE/STM32F412xG
+            I2CPINDEF(PB3,  GPIO_AF9_I2C2),
+            I2CPINDEF(PB9,  GPIO_AF9_I2C2),
+#endif
+        },
         .rcc = RCC_APB1(I2C2),
         .ev_irq = I2C2_EV_IRQn,
         .er_irq = I2C2_ER_IRQn,
@@ -74,8 +90,18 @@ const i2cHardware_t i2cHardware[I2CDEV_COUNT] = {
     {
         .device = I2CDEV_3,
         .reg = I2C3,
-        .sclPins = { DEFIO_TAG_E(PA8) },
-        .sdaPins = { DEFIO_TAG_E(PC9) },
+        .sclPins = {
+            I2CPINDEF(PA8, GPIO_AF_I2C3),
+        },
+        .sdaPins = {
+            I2CPINDEF(PC9, GPIO_AF_I2C3),
+
+#if defined(STM32F40_41xxx) || defined (STM32F411xE)
+            // STM32F401xx/STM32F410xx/STM32F411xE/STM32F412xG
+            I2CPINDEF(PB4, GPIO_AF9_I2C3),
+            I2CPINDEF(PB8, GPIO_AF9_I2C3),
+#endif
+        },
         .rcc = RCC_APB1(I2C3),
         .ev_irq = I2C3_EV_IRQn,
         .er_irq = I2C3_ER_IRQn,
@@ -394,8 +420,8 @@ void i2cInit(I2CDevice device)
 
     // Init pins
 #ifdef STM32F4
-    IOConfigGPIOAF(scl, pDev->pullUp ? IOCFG_I2C_PU : IOCFG_I2C, GPIO_AF_I2C);
-    IOConfigGPIOAF(sda, pDev->pullUp ? IOCFG_I2C_PU : IOCFG_I2C, GPIO_AF_I2C);
+    IOConfigGPIOAF(scl, pDev->pullUp ? IOCFG_I2C_PU : IOCFG_I2C, pDev->sclAF);
+    IOConfigGPIOAF(sda, pDev->pullUp ? IOCFG_I2C_PU : IOCFG_I2C, pDev->sdaAF);
 #else
     IOConfigGPIO(scl, IOCFG_I2C);
     IOConfigGPIO(sda, IOCFG_I2C);
diff --git a/src/main/drivers/bus_i2c_stm32f30x.c b/src/main/drivers/bus_i2c_stm32f30x.c
index e6e22d2d3..3ea33575c 100644
--- a/src/main/drivers/bus_i2c_stm32f30x.c
+++ b/src/main/drivers/bus_i2c_stm32f30x.c
@@ -50,8 +50,8 @@ const i2cHardware_t i2cHardware[I2CDEV_COUNT] = {
     {
         .device = I2CDEV_1,
         .reg = I2C1,
-        .sclPins = { DEFIO_TAG_E(PA15), DEFIO_TAG_E(PB6), DEFIO_TAG_E(PB8) },
-        .sdaPins = { DEFIO_TAG_E(PA14), DEFIO_TAG_E(PB7), DEFIO_TAG_E(PB9) },
+        .sclPins = { I2CPINDEF(PA15), I2CPINDEF(PB6), I2CPINDEF(PB8) },
+        .sdaPins = { I2CPINDEF(PA14), I2CPINDEF(PB7), I2CPINDEF(PB9) },
         .rcc = RCC_APB1(I2C1),
     },
 #endif
@@ -59,8 +59,8 @@ const i2cHardware_t i2cHardware[I2CDEV_COUNT] = {
     {
         .device = I2CDEV_2,
         .reg = I2C2,
-        .sclPins = { DEFIO_TAG_E(PA9), DEFIO_TAG_E(PF6) },
-        .sdaPins = { DEFIO_TAG_E(PA10) },
+        .sclPins = { I2CPINDEF(PA9), I2CPINDEF(PF6) },
+        .sdaPins = { I2CPINDEF(PA10) },
         .rcc = RCC_APB1(I2C2),
     },
 #endif