diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp
index b4e95e2..670789f 100644
--- a/src/Braccio++.cpp
+++ b/src/Braccio++.cpp
@@ -101,7 +101,7 @@ bool BraccioClass::begin(voidFuncPtr custom_menu)
   _pd_thd.start(mbed::callback(this, &BraccioClass::pd_thread_func));
   attachInterrupt(PIN_FUSB302_INT, braccio_unlock_pd_semaphore_irq, FALLING);
   _pd_timer.attach(braccio_unlock_pd_semaphore, 10ms);
-  _PD_UFP.init_PPS(PPS_V(7.2), PPS_A(2.0));
+  _PD_UFP.init_PPS(_i2c_mtx, PPS_V(7.2), PPS_A(2.0));
 
   button_init();
 
@@ -117,11 +117,9 @@ bool BraccioClass::begin(voidFuncPtr custom_menu)
   {
     if (!_PD_UFP.is_PPS_ready())
     {
-      _i2c_mtx.lock();
       _PD_UFP.print_status(Serial);
       _PD_UFP.set_PPS(PPS_V(7.2), PPS_A(2.0));
       delay(10);
-      _i2c_mtx.unlock();
     }
   };
 
@@ -473,13 +471,11 @@ void BraccioClass::pd_thread_func()
       _pd_timer.detach();
       _pd_timer.attach(braccio_unlock_pd_semaphore, 50ms);
     }
-    _i2c_mtx.lock();
     if (millis() - last_time_ask_pps > 5000) {
       _PD_UFP.set_PPS(PPS_V(7.2), PPS_A(2.0));
       last_time_ask_pps = millis();
     }
     _PD_UFP.run();
-    _i2c_mtx.unlock();
     if (_PD_UFP.is_power_ready() && _PD_UFP.is_PPS_ready()) {
 
     }
diff --git a/src/lib/powerdelivery/FUSB302_UFP.c b/src/lib/powerdelivery/FUSB302_UFP.cpp
similarity index 99%
rename from src/lib/powerdelivery/FUSB302_UFP.c
rename to src/lib/powerdelivery/FUSB302_UFP.cpp
index 3dbcb30..18a33ea 100644
--- a/src/lib/powerdelivery/FUSB302_UFP.c
+++ b/src/lib/powerdelivery/FUSB302_UFP.cpp
@@ -15,7 +15,6 @@
  * 
  */
  
-#include <string.h>
 #include "FUSB302_UFP.h"
 
 /* Switches0 : 02h */
@@ -248,7 +247,7 @@ enum FUSB302_state_t {
 
 static inline FUSB302_ret_t reg_read(FUSB302_dev_t *dev, uint8_t address, uint8_t *data, uint8_t count)
 {
-    FUSB302_ret_t ret = dev->i2c_read(dev->i2c_address, address, data, count);
+    FUSB302_ret_t ret = dev->i2c_read(*(dev->wire_mtx), dev->i2c_address, address, data, count);
     if (ret != FUSB302_SUCCESS) {
         dev->err_msg = FUSB302_ERR_MSG("Fail to read register");
     }
@@ -257,7 +256,7 @@ static inline FUSB302_ret_t reg_read(FUSB302_dev_t *dev, uint8_t address, uint8_
 
 static inline FUSB302_ret_t reg_write(FUSB302_dev_t *dev, uint8_t address, uint8_t *data, uint8_t count)
 {
-    FUSB302_ret_t ret = dev->i2c_write(dev->i2c_address, address, data, count);
+    FUSB302_ret_t ret = dev->i2c_write(*(dev->wire_mtx), dev->i2c_address, address, data, count);
     if (ret != FUSB302_SUCCESS) {
         dev->err_msg = FUSB302_ERR_MSG("Fail to write register");
     }
diff --git a/src/lib/powerdelivery/FUSB302_UFP.h b/src/lib/powerdelivery/FUSB302_UFP.h
index 77beafb..f210bdb 100644
--- a/src/lib/powerdelivery/FUSB302_UFP.h
+++ b/src/lib/powerdelivery/FUSB302_UFP.h
@@ -18,7 +18,8 @@
 #ifndef FUSB302_UFP_H
 #define FUSB302_UFP_H
 
-#include <stdint.h>
+#include <Arduino.h>
+#include <mbed.h>
 
 enum {
     FUSB302_SUCCESS             = 0,
@@ -39,8 +40,9 @@ typedef uint8_t FUSB302_event_t;
 typedef struct {
     /* setup by user */
     uint8_t i2c_address;
-    FUSB302_ret_t (*i2c_read)(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint8_t count);
-    FUSB302_ret_t (*i2c_write)(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint8_t count);
+    rtos::Mutex * wire_mtx;
+    FUSB302_ret_t (*i2c_read)(rtos::Mutex & wire_mtx, uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint8_t count);
+    FUSB302_ret_t (*i2c_write)(rtos::Mutex & wire_mtx, uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint8_t count);
     FUSB302_ret_t (*delay_ms)(uint32_t t);
 
     /* used by this library */
diff --git a/src/lib/powerdelivery/PD_UFP.cpp b/src/lib/powerdelivery/PD_UFP.cpp
index 375a193..0b19f25 100644
--- a/src/lib/powerdelivery/PD_UFP.cpp
+++ b/src/lib/powerdelivery/PD_UFP.cpp
@@ -13,9 +13,6 @@
  * 
  */
  
-#include <stdint.h>
-#include <string.h>
-
 #include "PD_UFP.h"
 
 #define t_PD_POLLING            100
@@ -61,16 +58,12 @@ PD_UFP_core_c::PD_UFP_core_c():
     memset(&protocol, 0, sizeof(PD_protocol_t));
 }
 
-void PD_UFP_core_c::init(enum PD_power_option_t power_option)
-{
-    init_PPS(0, 0, power_option);
-}
-
-void PD_UFP_core_c::init_PPS(uint16_t PPS_voltage, uint8_t PPS_current, enum PD_power_option_t power_option)
+void PD_UFP_core_c::init_PPS(rtos::Mutex & wire_mtx, uint16_t PPS_voltage, uint8_t PPS_current, enum PD_power_option_t power_option)
 {
     // Initialize FUSB302
     pinMode(PIN_FUSB302_INT, INPUT_PULLUP); // Set FUSB302 int pin input ant pull up
     FUSB302.i2c_address = 0x22;
+    FUSB302.wire_mtx = &wire_mtx;
     FUSB302.i2c_read = FUSB302_i2c_read;
     FUSB302.i2c_write = FUSB302_i2c_write;
     FUSB302.delay_ms = FUSB302_delay_ms;
@@ -127,8 +120,10 @@ void PD_UFP_core_c::clock_prescale_set(uint8_t prescaler)
     }
 }
 
-FUSB302_ret_t PD_UFP_core_c::FUSB302_i2c_read(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint8_t count)
+FUSB302_ret_t PD_UFP_core_c::FUSB302_i2c_read(rtos::Mutex & wire_mtx, uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint8_t count)
 {
+    mbed::ScopedLock<rtos::Mutex> lock(wire_mtx);
+
     Wire.beginTransmission(dev_addr);
     Wire.write(reg_addr);
     Wire.endTransmission();
@@ -140,8 +135,10 @@ FUSB302_ret_t PD_UFP_core_c::FUSB302_i2c_read(uint8_t dev_addr, uint8_t reg_addr
     return count == 0 ? FUSB302_SUCCESS : FUSB302_ERR_READ_DEVICE;
 }
 
-FUSB302_ret_t PD_UFP_core_c::FUSB302_i2c_write(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint8_t count)
+FUSB302_ret_t PD_UFP_core_c::FUSB302_i2c_write(rtos::Mutex & wire_mtx, uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint8_t count)
 {
+    mbed::ScopedLock<rtos::Mutex> lock(wire_mtx);
+
     Wire.beginTransmission(dev_addr);
     Wire.write(reg_addr);
     while (count > 0) {
diff --git a/src/lib/powerdelivery/PD_UFP.h b/src/lib/powerdelivery/PD_UFP.h
index 8163f47..5197d6c 100644
--- a/src/lib/powerdelivery/PD_UFP.h
+++ b/src/lib/powerdelivery/PD_UFP.h
@@ -16,16 +16,15 @@
 #ifndef PD_UFP_H
 #define PD_UFP_H
 
-#include <stdint.h>
-
 #include <Arduino.h>
 #include <Wire.h>
 #include <HardwareSerial.h>
 
 #define PIN_FUSB302_INT         A2
 
-extern "C" {
-    #include "FUSB302_UFP.h"
+#include "FUSB302_UFP.h"
+extern "C"
+{
     #include "PD_UFP_Protocol.h"
 }
 
@@ -64,8 +63,7 @@ class PD_UFP_core_c
     public:
         PD_UFP_core_c();
         // Init
-        void init(enum PD_power_option_t power_option = PD_POWER_OPTION_MAX_5V);
-        void init_PPS(uint16_t PPS_voltage, uint8_t PPS_current, enum PD_power_option_t power_option = PD_POWER_OPTION_MAX_5V);
+        void init_PPS(rtos::Mutex & wire_mtx, uint16_t PPS_voltage, uint8_t PPS_current, enum PD_power_option_t power_option = PD_POWER_OPTION_MAX_5V);
         // Task
         void run(void);
         // Status
@@ -82,8 +80,8 @@ class PD_UFP_core_c
         static void clock_prescale_set(uint8_t prescaler);
 
     protected:
-        static FUSB302_ret_t FUSB302_i2c_read(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint8_t count);
-        static FUSB302_ret_t FUSB302_i2c_write(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint8_t count);
+        static FUSB302_ret_t FUSB302_i2c_read(rtos::Mutex & wire_mtx, uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint8_t count);
+        static FUSB302_ret_t FUSB302_i2c_write(rtos::Mutex & wire_mtx, uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint8_t count);
         static FUSB302_ret_t FUSB302_delay_ms(uint32_t t);
         void handle_protocol_event(PD_protocol_event_t events);
         void handle_FUSB302_event(FUSB302_event_t events);