diff --git a/radio/src/CMakeLists.txt b/radio/src/CMakeLists.txt
index df716552d..611de4512 100644
--- a/radio/src/CMakeLists.txt
+++ b/radio/src/CMakeLists.txt
@@ -595,6 +595,7 @@ endif()
 target_compile_definitions(board PUBLIC FREE_RTOS)
 target_compile_definitions(board_lib PUBLIC FREE_RTOS)
 target_compile_definitions(firmware PUBLIC FREE_RTOS)
+target_compile_definitions(stm32_drivers_w_dbg_fw PUBLIC FREE_RTOS)
 
 target_sources(firmware PUBLIC
   os/async_freertos.cpp
diff --git a/radio/src/bootloader/CMakeLists.txt b/radio/src/bootloader/CMakeLists.txt
index 0caa0866e..cd1c38d21 100644
--- a/radio/src/bootloader/CMakeLists.txt
+++ b/radio/src/bootloader/CMakeLists.txt
@@ -58,6 +58,7 @@ endif()
 add_executable(bootloader EXCLUDE_FROM_ALL ${BOOTLOADER_SRC})
 add_dependencies(bootloader ${BITMAPS_TARGET})
 
+target_compile_definitions(stm32_drivers_w_dbg_bl PUBLIC BOOT)
 target_compile_definitions(bootloader PUBLIC BOOT)
 target_compile_definitions(board_bl PUBLIC BOOT)
 
diff --git a/radio/src/os/task.h b/radio/src/os/task.h
index 2ff0e841e..f4cfb0839 100644
--- a/radio/src/os/task.h
+++ b/radio/src/os/task.h
@@ -43,3 +43,15 @@ bool mutex_lock(mutex_handle_t* h);
 void mutex_unlock(mutex_handle_t* h);
 bool mutex_trylock(mutex_handle_t* h);
 
+class MutexLock
+{
+public:
+  MutexLock(mutex_handle_t* mtx):mutex(mtx),locked(false) {mutex_lock(mutex); locked = true;}
+  ~MutexLock() {if(locked) mutex_unlock(mutex);}
+  void lock() {if(!locked) mutex_lock(mutex); locked=true;}
+  void unlock() {if(locked) mutex_unlock(mutex); locked=false;}
+private:
+  mutex_handle_t* mutex;
+  bool locked;
+};
+
diff --git a/radio/src/targets/common/arm/stm32/CMakeLists.txt b/radio/src/targets/common/arm/stm32/CMakeLists.txt
index 2697b4e77..2a3cdeb84 100644
--- a/radio/src/targets/common/arm/stm32/CMakeLists.txt
+++ b/radio/src/targets/common/arm/stm32/CMakeLists.txt
@@ -96,7 +96,6 @@ if(NOT NATIVE_BUILD)
     ${STM32_DRIVER_DIR}/stm32_serial_driver.cpp
     ${STM32_DRIVER_DIR}/stm32_usart_driver.cpp
     ${STM32_DRIVER_DIR}/stm32_gpio_driver.cpp
-    ${STM32_DRIVER_DIR}/stm32_i2c_driver.cpp
     ${STM32_DRIVER_DIR}/stm32_timer.cpp
     ${STM32_DRIVER_DIR}/stm32_dma.cpp
     ${STM32_DRIVER_DIR}/stm32_gpio.cpp
@@ -121,10 +120,12 @@ if(NOT NATIVE_BUILD)
   add_library(stm32_drivers_w_dbg_fw OBJECT EXCLUDE_FROM_ALL
       ${STM32_DRIVER_DIR}/sdcard_spi.cpp
       ${STM32_DRIVER_DIR}/diskio_sdio.cpp
+      ${STM32_DRIVER_DIR}/stm32_i2c_driver.cpp
   )
   add_library(stm32_drivers_w_dbg_bl OBJECT EXCLUDE_FROM_ALL
       ${STM32_DRIVER_DIR}/sdcard_spi.cpp
       ${STM32_DRIVER_DIR}/diskio_sdio.cpp
+      ${STM32_DRIVER_DIR}/stm32_i2c_driver.cpp
   )
 
   set(FIRMWARE_SRC ${FIRMWARE_SRC} $<TARGET_OBJECTS:stm32_drivers>)
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 e74ce57fb..6b58a3f05 100644
--- a/radio/src/targets/common/arm/stm32/stm32_i2c_driver.cpp
+++ b/radio/src/targets/common/arm/stm32/stm32_i2c_driver.cpp
@@ -25,6 +25,9 @@
 #include "stm32_hal.h"
 
 #include "timers_driver.h"
+#if !defined(BOOT)
+#include "os/task.h"
+#endif
 #include "debug.h"
 
 #define MAX_I2C_DEVICES 2
@@ -32,6 +35,9 @@
 struct stm32_i2c_device {
   I2C_HandleTypeDef handle;
   const stm32_i2c_hw_def_t* hw_def;
+#if !defined(BOOT)
+  mutex_handle_t mutex;
+#endif
 };
 
 static stm32_i2c_device _i2c_devs[MAX_I2C_DEVICES] = {};
@@ -48,6 +54,12 @@ static I2C_HandleTypeDef* i2c_get_handle(uint8_t bus)
   return &_i2c_devs[bus].handle;
 }
 
+#if !defined(BOOT)
+#define I2CMutex(bus) MutexLock mutexLock(&i2c_get_device(bus)->mutex)
+#else
+#define I2CMutex(bus)
+#endif
+
 #if defined(STM32H7) || defined(STM32H7RS)
 
 /* ST's I2C timing computation */
@@ -513,6 +525,9 @@ int stm32_i2c_init(uint8_t bus, uint32_t clock_rate, const stm32_i2c_hw_def_t* h
   }
 #endif
 
+#if !defined(BOOT)
+  mutex_create(&dev->mutex);
+#endif
   return 1;
 }
 
@@ -542,6 +557,7 @@ int stm32_i2c_master_tx(uint8_t bus, uint16_t addr, uint8_t *data, uint16_t len,
   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) {
     return -1;
   }
@@ -555,6 +571,7 @@ int stm32_i2c_master_rx(uint8_t bus, uint16_t addr, uint8_t *data, uint16_t len,
   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) {
     return -1;
   }
@@ -568,6 +585,7 @@ int stm32_i2c_read(uint8_t bus, uint16_t addr, uint16_t reg, uint16_t reg_size,
   I2C_HandleTypeDef* h = i2c_get_handle(bus);
   if (!h) return -1;  
 
+  I2CMutex(bus);
   if (HAL_I2C_Mem_Read(h, addr << 1, reg, reg_size, data, len, timeout) != HAL_OK) {
     return -1;
   }
@@ -580,7 +598,8 @@ int stm32_i2c_write(uint8_t bus, uint16_t addr, uint16_t reg, uint16_t reg_size,
 {
   I2C_HandleTypeDef* h = i2c_get_handle(bus);
   if (!h) return -1;  
-  
+
+  I2CMutex(bus);
   if (HAL_I2C_Mem_Write(h, addr << 1, reg, reg_size, data, len, timeout) != HAL_OK) {
     return -1;
   }
@@ -593,6 +612,7 @@ 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;
 
@@ -601,5 +621,6 @@ 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);
 }