diff --git a/extras/ads111x/ads111x.c b/extras/ads111x/ads111x.c
index 9241e13..c9d3cf3 100644
--- a/extras/ads111x/ads111x.c
+++ b/extras/ads111x/ads111x.c
@@ -55,7 +55,7 @@ const float ads111x_gain_values[] = {
 static uint16_t read_reg(uint8_t addr, uint8_t reg)
 {
     uint16_t res = 0;
-    if (!i2c_slave_read(addr, reg, (uint8_t *)&res, 2))
+    if (i2c_slave_read(addr, &reg, (uint8_t *)&res, 2))
         debug("Could not read register %d", reg);
     //debug("Read %d: 0x%04x", reg, res);
     return res;
@@ -64,8 +64,8 @@ static uint16_t read_reg(uint8_t addr, uint8_t reg)
 static void write_reg(uint8_t addr, uint8_t reg, uint16_t val)
 {
     //debug("Write %d: 0x%04x", reg, val);
-    uint8_t buf[3] = {reg, val >> 8, val};
-    if (!i2c_slave_write(addr, buf, 3))
+    uint8_t buf[2] = { val >> 8, val};
+    if (i2c_slave_write(addr, &reg, buf, 2))
         debug("Could not write 0x%04x to register %d", val, reg);
 }
 
diff --git a/extras/bmp180/bmp180.c b/extras/bmp180/bmp180.c
index c3c8afd..df7a503 100644
--- a/extras/bmp180/bmp180.c
+++ b/extras/bmp180/bmp180.c
@@ -39,35 +39,36 @@
 //
 #define BMP180_RESET_VALUE        0xB6
 
-static bool bmp180_readRegister16(uint8_t reg, int16_t *r)
+static int bmp180_readRegister16(uint8_t reg, int16_t *r)
 {
     uint8_t d[] = { 0, 0 };
+    int error ;
 
-    if (!i2c_slave_read(BMP180_DEVICE_ADDRESS, reg, d, 2))
-        return false;
+    if ((error = i2c_slave_read(BMP180_DEVICE_ADDRESS, &reg, d, 2)))
+        return error;
 
     *r = ((int16_t)d[0] << 8) | (d[1]);
-    return true;
+    return 0;
 }
 
-static bool bmp180_start_Messurement(uint8_t cmd)
+static int bmp180_start_Messurement(uint8_t cmd)
 {
-    uint8_t d[] = { BMP180_CONTROL_REG, cmd };
+    uint8_t reg = BMP180_CONTROL_REG ;
 
-    return i2c_slave_write(BMP180_DEVICE_ADDRESS, d, 2);
+    return i2c_slave_write(BMP180_DEVICE_ADDRESS, &reg, &cmd, 1);
 }
 
 static bool bmp180_get_uncompensated_temperature(int32_t *ut)
 {
     // Write Start Code into reg 0xF4.
-    if (!bmp180_start_Messurement(BMP180_MEASURE_TEMP))
+    if (bmp180_start_Messurement(BMP180_MEASURE_TEMP))
         return false;
 
     // Wait 5ms, datasheet states 4.5ms
     sdk_os_delay_us(5000);
 
     int16_t v;
-    if (!bmp180_readRegister16(BMP180_OUT_MSB_REG, &v))
+    if (bmp180_readRegister16(BMP180_OUT_MSB_REG, &v))
         return false;
 
     *ut = v;
@@ -88,13 +89,14 @@ static bool bmp180_get_uncompensated_pressure(uint8_t oss, uint32_t *up)
     }
 
     // Write Start Code into reg 0xF4
-    if (!bmp180_start_Messurement(BMP180_MEASURE_PRESS | (oss << 6)))
+    if (bmp180_start_Messurement(BMP180_MEASURE_PRESS | (oss << 6)))
         return false;
 
     sdk_os_delay_us(us);
 
     uint8_t d[] = { 0, 0, 0 };
-    if (!i2c_slave_read(BMP180_DEVICE_ADDRESS, BMP180_OUT_MSB_REG, d, 3))
+    uint8_t reg = BMP180_OUT_MSB_REG;
+    if (i2c_slave_read(BMP180_DEVICE_ADDRESS, &reg, d, 3))
         return false;
 
     uint32_t r = ((uint32_t)d[0] << 16) | ((uint32_t)d[1] << 8) | d[2];
@@ -106,17 +108,17 @@ static bool bmp180_get_uncompensated_pressure(uint8_t oss, uint32_t *up)
 // Returns true of success else false.
 bool bmp180_fillInternalConstants(bmp180_constants_t *c)
 {
-    if (!bmp180_readRegister16(BMP180_CALIBRATION_REG+0, &c->AC1) ||
-        !bmp180_readRegister16(BMP180_CALIBRATION_REG+2, &c->AC2) ||
-        !bmp180_readRegister16(BMP180_CALIBRATION_REG+4, &c->AC3) ||
-        !bmp180_readRegister16(BMP180_CALIBRATION_REG+6, (int16_t *)&c->AC4) ||
-        !bmp180_readRegister16(BMP180_CALIBRATION_REG+8, (int16_t *)&c->AC5) ||
-        !bmp180_readRegister16(BMP180_CALIBRATION_REG+10, (int16_t *)&c->AC6) ||
-        !bmp180_readRegister16(BMP180_CALIBRATION_REG+12, &c->B1) ||
-        !bmp180_readRegister16(BMP180_CALIBRATION_REG+14, &c->B2) ||
-        !bmp180_readRegister16(BMP180_CALIBRATION_REG+16, &c->MB) ||
-        !bmp180_readRegister16(BMP180_CALIBRATION_REG+18, &c->MC) ||
-        !bmp180_readRegister16(BMP180_CALIBRATION_REG+20, &c->MD)) {
+    if (bmp180_readRegister16(BMP180_CALIBRATION_REG+0, &c->AC1) ||
+        bmp180_readRegister16(BMP180_CALIBRATION_REG+2, &c->AC2) ||
+        bmp180_readRegister16(BMP180_CALIBRATION_REG+4, &c->AC3) ||
+        bmp180_readRegister16(BMP180_CALIBRATION_REG+6, (int16_t *)&c->AC4) ||
+        bmp180_readRegister16(BMP180_CALIBRATION_REG+8, (int16_t *)&c->AC5) ||
+        bmp180_readRegister16(BMP180_CALIBRATION_REG+10, (int16_t *)&c->AC6) ||
+        bmp180_readRegister16(BMP180_CALIBRATION_REG+12, &c->B1) ||
+        bmp180_readRegister16(BMP180_CALIBRATION_REG+14, &c->B2) ||
+        bmp180_readRegister16(BMP180_CALIBRATION_REG+16, &c->MB) ||
+        bmp180_readRegister16(BMP180_CALIBRATION_REG+18, &c->MC) ||
+        bmp180_readRegister16(BMP180_CALIBRATION_REG+20, &c->MD)) {
         return false;
     }
 
@@ -140,8 +142,10 @@ bool bmp180_fillInternalConstants(bmp180_constants_t *c)
 bool bmp180_is_available()
 {
     uint8_t id;
-    return i2c_slave_read(BMP180_DEVICE_ADDRESS, BMP180_VERSION_REG, &id, 1) &&
-        id == BMP180_CHIP_ID;
+    uint8_t reg = BMP180_VERSION_REG;
+    if (i2c_slave_read(BMP180_DEVICE_ADDRESS, &reg, &id, 1))
+        return false;
+    return id == BMP180_CHIP_ID;
 }
 
 bool bmp180_measure(bmp180_constants_t *c, int32_t *temperature,
@@ -208,8 +212,6 @@ bool bmp180_measure(bmp180_constants_t *c, int32_t *temperature,
     return true;
 }
 
-
-
 // BMP180_Event_Command
 typedef struct
 {
diff --git a/extras/bmp280/bmp280.c b/extras/bmp280/bmp280.c
index 83fdfe0..a106b07 100644
--- a/extras/bmp280/bmp280.c
+++ b/extras/bmp280/bmp280.c
@@ -69,13 +69,18 @@ void bmp280_init_default_params(bmp280_params_t *params)
 static bool read_register16(uint8_t i2c_addr, uint8_t addr, uint16_t *value)
 {
     uint8_t d[] = {0, 0};
-    if (i2c_slave_read(i2c_addr, addr, d, sizeof(d))) {
+    if (!i2c_slave_read(i2c_addr, &addr, d, sizeof(d))) {
         *value = d[0] | (d[1] << 8);
         return true;
     }
     return false;
 }
 
+static inline int read_data(uint8_t i2c_addr, uint8_t addr, uint8_t *value, uint8_t len)
+{
+    return i2c_slave_read(i2c_addr, &addr, value, len);
+}
+
 static bool read_calibration_data(bmp280_t *dev)
 {
     uint8_t i2c_addr = dev->i2c_addr;
@@ -118,12 +123,12 @@ static bool read_hum_calibration_data(bmp280_t *dev)
     uint8_t i2c_addr = dev->i2c_addr;
     uint16_t h4, h5;
 
-    if (i2c_slave_read(i2c_addr, 0xa1, &dev->dig_H1, 1) &&
+    if (!read_data(i2c_addr, 0xa1, &dev->dig_H1, 1) &&
         read_register16(i2c_addr, 0xe1, (uint16_t *)&dev->dig_H2) &&
-        i2c_slave_read(i2c_addr, 0xe3, &dev->dig_H3, 1) &&
+        !read_data(i2c_addr, 0xe3, &dev->dig_H3, 1) &&
         read_register16(i2c_addr, 0xe4, &h4) &&
         read_register16(i2c_addr, 0xe5, &h5) &&
-        i2c_slave_read(i2c_addr, 0xe7, (uint8_t *)&dev->dig_H6, 1)) {
+        !read_data(i2c_addr, 0xe7, (uint8_t *)&dev->dig_H6, 1)) {
         dev->dig_H4 = (h4 & 0x00ff) << 4 | (h4 & 0x0f00) >> 8;
         dev->dig_H5 = h5 >> 4;
         debug("Calibration data received:");
@@ -139,11 +144,9 @@ static bool read_hum_calibration_data(bmp280_t *dev)
     return false;
 }
 
-static bool write_register8(uint8_t i2c_addr, uint8_t addr, uint8_t value)
+static int write_register8(uint8_t i2c_addr, uint8_t addr, uint8_t value)
 {
-    uint8_t d[] = {addr, value};
-
-    return i2c_slave_write(i2c_addr, d, 2);
+    return i2c_slave_write(i2c_addr, &addr, &value, 1);
 }
 
 bool bmp280_init(bmp280_t *dev, bmp280_params_t *params)
@@ -155,7 +158,7 @@ bool bmp280_init(bmp280_t *dev, bmp280_params_t *params)
         return false;
     }
 
-    if (!i2c_slave_read(i2c_addr, BMP280_REG_ID, &dev->id, 1)) {
+    if (read_data(i2c_addr, BMP280_REG_ID, &dev->id, 1)) {
         debug("Sensor not found");
         return false;
     }
@@ -166,7 +169,7 @@ bool bmp280_init(bmp280_t *dev, bmp280_params_t *params)
     }
 
     // Soft reset.
-    if (!write_register8(i2c_addr, BMP280_REG_RESET, BMP280_RESET_VALUE)) {
+    if (write_register8(i2c_addr, BMP280_REG_RESET, BMP280_RESET_VALUE)) {
         debug("Failed resetting sensor");
         return false;
     }
@@ -174,7 +177,7 @@ bool bmp280_init(bmp280_t *dev, bmp280_params_t *params)
     // Wait until finished copying over the NVP data.
     while (1) {
         uint8_t status;
-        if (i2c_slave_read(i2c_addr, BMP280_REG_STATUS, &status, 1) && (status & 1) == 0)
+        if (!read_data(i2c_addr, BMP280_REG_STATUS, &status, 1) && (status & 1) == 0)
             break;
     }
 
@@ -190,7 +193,7 @@ bool bmp280_init(bmp280_t *dev, bmp280_params_t *params)
 
     uint8_t config = (params->standby << 5) | (params->filter << 2);
     debug("Writing config reg=%x", config);
-    if (!write_register8(i2c_addr, BMP280_REG_CONFIG, config)) {
+    if (write_register8(i2c_addr, BMP280_REG_CONFIG, config)) {
         debug("Failed configuring sensor");
         return false;
     }
@@ -207,14 +210,14 @@ bool bmp280_init(bmp280_t *dev, bmp280_params_t *params)
         // Write crtl hum reg first, only active after write to BMP280_REG_CTRL.
         uint8_t ctrl_hum = params->oversampling_humidity;
         debug("Writing ctrl hum reg=%x", ctrl_hum);
-        if (!write_register8(i2c_addr, BMP280_REG_CTRL_HUM, ctrl_hum)) {
+        if (write_register8(i2c_addr, BMP280_REG_CTRL_HUM, ctrl_hum)) {
             debug("Failed controlling sensor");
             return false;
         }
     }
 
     debug("Writing ctrl reg=%x", ctrl);
-    if (!write_register8(i2c_addr, BMP280_REG_CTRL, ctrl)) {
+    if (write_register8(i2c_addr, BMP280_REG_CTRL, ctrl)) {
         debug("Failed controlling sensor");
         return false;
     }
@@ -225,12 +228,12 @@ bool bmp280_init(bmp280_t *dev, bmp280_params_t *params)
 bool bmp280_force_measurement(bmp280_t *dev)
 {
     uint8_t ctrl;
-    if (!i2c_slave_read(dev->i2c_addr, BMP280_REG_CTRL, &ctrl, 1))
+    if (read_data(dev->i2c_addr, BMP280_REG_CTRL, &ctrl, 1))
         return false;
     ctrl &= ~0b11;  // clear two lower bits
     ctrl |= BMP280_MODE_FORCED;
     debug("Writing ctrl reg=%x", ctrl);
-    if (!write_register8(dev->i2c_addr, BMP280_REG_CTRL, ctrl)) {
+    if (write_register8(dev->i2c_addr, BMP280_REG_CTRL, ctrl)) {
         debug("Failed starting forced mode");
         return false;
     }
@@ -240,7 +243,7 @@ bool bmp280_force_measurement(bmp280_t *dev)
 bool bmp280_is_measuring(bmp280_t *dev)
 {
     uint8_t status;
-    if (!i2c_slave_read(dev->i2c_addr, BMP280_REG_STATUS, &status, 1))
+    if (read_data(dev->i2c_addr, BMP280_REG_STATUS, &status, 1))
         return false;
     if (status & (1 << 3)) {
         debug("Status: measuring");
@@ -342,7 +345,7 @@ bool bmp280_read_fixed(bmp280_t *dev, int32_t *temperature,
 
     // Need to read in one sequence to ensure they match.
     size_t size = humidity ? 8 : 6;
-    if (!i2c_slave_read(dev->i2c_addr, 0xf7, data, size)) {
+    if (read_data(dev->i2c_addr, 0xf7, data, size)) {
         debug("Failed reading");
         return false;
     }
diff --git a/extras/ds1307/ds1307.c b/extras/ds1307/ds1307.c
index e5306a8..f0f52c2 100644
--- a/extras/ds1307/ds1307.c
+++ b/extras/ds1307/ds1307.c
@@ -43,18 +43,15 @@ static uint8_t dec2bcd(uint8_t val)
 static uint8_t read_register(uint8_t reg)
 {
     uint8_t val;
-    i2c_slave_read(ADDR, reg, &val, 1);
+    i2c_slave_read(ADDR, &reg, &val, 1);
     return val;
 }
 
 static void update_register(uint8_t reg, uint8_t mask, uint8_t val)
 {
-    uint8_t buf[2];
+    uint8_t buf = (read_register(reg) & mask) | val;
 
-    buf[0] = reg;
-    buf[1] = (read_register(reg) & mask) | val;
-
-    i2c_slave_write(ADDR, buf, 2);
+    i2c_slave_write(ADDR, &reg, &buf, 1);
 }
 
 void ds1307_start(bool start)
@@ -70,8 +67,9 @@ bool ds1307_is_running()
 void ds1307_get_time(struct tm *time)
 {
     uint8_t buf[7];
+    uint8_t reg = TIME_REG ;
 
-    i2c_slave_read(ADDR, TIME_REG, buf, 7);
+    i2c_slave_read(ADDR, &reg , buf, 7);
 
     time->tm_sec = bcd2dec(buf[0] & SECONDS_MASK);
     time->tm_min = bcd2dec(buf[1]);
@@ -101,7 +99,7 @@ void ds1307_set_time(const struct tm *time)
     buf[6] = dec2bcd(time->tm_mon + 1);
     buf[7] = dec2bcd(time->tm_year - 2000);
 
-    i2c_slave_write(ADDR, buf, 8);
+    i2c_slave_write(ADDR, &buf[0], &buf[1] , 7);
 }
 
 void ds1307_enable_squarewave(bool enable)
@@ -134,31 +132,18 @@ void ds1307_set_output(bool value)
     update_register(CONTROL_REG, OUT_MASK, value ? OUT_BIT : 0);
 }
 
-bool ds1307_read_ram(uint8_t offset, uint8_t *buf, uint8_t len)
+int ds1307_read_ram(uint8_t offset, uint8_t *buf, uint8_t len)
 {
     if (offset + len > RAM_SIZE) return false;
+    uint8_t reg = RAM_REG + offset ;
 
-    return i2c_slave_read(ADDR, RAM_REG + offset, buf, len);
+    return i2c_slave_read(ADDR, &reg, buf, len);
 }
 
-bool ds1307_write_ram(uint8_t offset, uint8_t *buf, uint8_t len)
+int ds1307_write_ram(uint8_t offset, uint8_t *buf, uint8_t len)
 {
     if (offset + len > RAM_SIZE) return false;
+    uint8_t reg = RAM_REG + offset ;
 
-    // temporary buffer on the stack is not good so copy-paste :(
-    bool success = false;
-    do {
-        i2c_start();
-        if (!i2c_write(ADDR << 1))
-            break;
-        if (!i2c_write(RAM_REG + offset))
-            break;
-        while (len--) {
-            if (!i2c_write(*buf++))
-                break;
-        }
-        i2c_stop();
-        success = true;
-    } while(0);
-    return success;
+    return i2c_slave_write(ADDR, &reg, buf, len);
 }
diff --git a/extras/ds1307/ds1307.h b/extras/ds1307/ds1307.h
index d2daeee..5af38ae 100644
--- a/extras/ds1307/ds1307.h
+++ b/extras/ds1307/ds1307.h
@@ -93,18 +93,18 @@ void ds1307_set_output(bool value);
  * \param offset Start byte, 0..55
  * \param buf Buffer
  * \param len Bytes to read, 1..56
- * \return false if error occured
+ * \return Non-zero if error occured
  */
-bool ds1307_read_ram(uint8_t offset, uint8_t *buf, uint8_t len);
+int ds1307_read_ram(uint8_t offset, uint8_t *buf, uint8_t len);
 
 /**
  * \brief Write buffer to RTC RAM
  * \param offset Start byte, 0..55
  * \param buf Buffer
  * \param len Bytes to write, 1..56
- * \return false if error occured
+ * \return Non-zero if error occured
  */
-bool ds1307_write_ram(uint8_t offset, uint8_t *buf, uint8_t len);
+int ds1307_write_ram(uint8_t offset, uint8_t *buf, uint8_t len);
 
 
 #ifdef __cplusplus
diff --git a/extras/ds3231/ds3231.c b/extras/ds3231/ds3231.c
index 543474e..4de534c 100644
--- a/extras/ds3231/ds3231.c
+++ b/extras/ds3231/ds3231.c
@@ -28,47 +28,43 @@ static inline uint8_t  bcdToDec(uint8_t bcd)
 /* Send a number of bytes to the rtc over i2c
  * returns true to indicate success
  */
-static inline bool ds3231_send(uint8_t *data, uint8_t len)
+static inline int ds3231_send(uint8_t reg, uint8_t *data, uint8_t len)
 {
-    return i2c_slave_write(DS3231_ADDR, data, len);
+    return i2c_slave_write(DS3231_ADDR, &reg, data, len);
 }
 
 /* Read a number of bytes from the rtc over i2c
  * returns true to indicate success
  */
-static inline bool ds3231_recv(uint8_t *data, uint8_t len)
+static inline int ds3231_recv(uint8_t reg, uint8_t *data, uint8_t len)
 {
-    return i2c_slave_read(DS3231_ADDR, data[0], data, len);
+    return i2c_slave_read(DS3231_ADDR, &reg, data, len);
 }
 
-bool ds3231_setTime(struct tm *time)
+int ds3231_setTime(struct tm *time)
 {
-    uint8_t data[8];
+    uint8_t data[7];
 
-    /* start register */
-    data[0] = DS3231_ADDR_TIME;
     /* time/date data */
-    data[1] = decToBcd(time->tm_sec);
-    data[2] = decToBcd(time->tm_min);
-    data[3] = decToBcd(time->tm_hour);
-    data[4] = decToBcd(time->tm_wday + 1);
-    data[5] = decToBcd(time->tm_mday);
-    data[6] = decToBcd(time->tm_mon + 1);
-    data[7] = decToBcd(time->tm_year - 100);
+    data[0] = decToBcd(time->tm_sec);
+    data[1] = decToBcd(time->tm_min);
+    data[2] = decToBcd(time->tm_hour);
+    data[3] = decToBcd(time->tm_wday + 1);
+    data[4] = decToBcd(time->tm_mday);
+    data[5] = decToBcd(time->tm_mon + 1);
+    data[6] = decToBcd(time->tm_year - 100);
 
-    return ds3231_send(data, 8);
+    return ds3231_send(DS3231_ADDR_TIME, data, 7);
 }
 
-bool ds3231_setAlarm(uint8_t alarms, struct tm *time1, uint8_t option1, struct tm *time2, uint8_t option2)
+int ds3231_setAlarm(uint8_t alarms, struct tm *time1, uint8_t option1, struct tm *time2, uint8_t option2)
 {
     int i = 0;
-    uint8_t data[8];
-
-    /* start register */
-    data[i++] = (alarms == DS3231_ALARM_2 ? DS3231_ADDR_ALARM2 : DS3231_ADDR_ALARM1);
+    uint8_t data[7];
 
     /* alarm 1 data */
-    if (alarms != DS3231_ALARM_2) {
+    if (alarms != DS3231_ALARM_2)
+    {
         data[i++] = (option1 >= DS3231_ALARM1_MATCH_SEC ? decToBcd(time1->tm_sec) : DS3231_ALARM_NOTSET);
         data[i++] = (option1 >= DS3231_ALARM1_MATCH_SECMIN ? decToBcd(time1->tm_min) : DS3231_ALARM_NOTSET);
         data[i++] = (option1 >= DS3231_ALARM1_MATCH_SECMINHOUR ? decToBcd(time1->tm_hour) : DS3231_ALARM_NOTSET);
@@ -77,14 +73,15 @@ bool ds3231_setAlarm(uint8_t alarms, struct tm *time1, uint8_t option1, struct t
     }
 
     /* alarm 2 data */
-    if (alarms != DS3231_ALARM_1) {
+    if (alarms != DS3231_ALARM_1)
+    {
         data[i++] = (option2 >= DS3231_ALARM2_MATCH_MIN ? decToBcd(time2->tm_min) : DS3231_ALARM_NOTSET);
         data[i++] = (option2 >= DS3231_ALARM2_MATCH_MINHOUR ? decToBcd(time2->tm_hour) : DS3231_ALARM_NOTSET);
         data[i++] = (option2 == DS3231_ALARM2_MATCH_MINHOURDAY ? (decToBcd(time2->tm_wday + 1) & DS3231_ALARM_WDAY) :
             (option2 == DS3231_ALARM2_MATCH_MINHOURDATE ? decToBcd(time2->tm_mday) : DS3231_ALARM_NOTSET));
     }
 
-    return ds3231_send(data, i);
+    return ds3231_send((alarms == DS3231_ALARM_2 ? DS3231_ADDR_ALARM2 : DS3231_ADDR_ALARM1), data, i);
 }
 
 /* Get a byte containing just the requested bits
@@ -96,13 +93,13 @@ bool ds3231_setAlarm(uint8_t alarms, struct tm *time1, uint8_t option1, struct t
  */
 bool ds3231_getFlag(uint8_t addr, uint8_t mask, uint8_t *flag)
 {
-    uint8_t data[1];
+    uint8_t data;
 
     /* get register */
-    data[0] = addr;
-    if (ds3231_send(data, 1) && ds3231_recv(data, 1)) {
+    if (!ds3231_recv(addr, &data, 1))
+    {
         /* return only requested flag */
-        *flag = (data[0] & mask);
+        *flag = (data & mask);
         return true;
     }
 
@@ -117,22 +114,21 @@ bool ds3231_getFlag(uint8_t addr, uint8_t mask, uint8_t *flag)
  */
 bool ds3231_setFlag(uint8_t addr, uint8_t bits, uint8_t mode)
 {
-    uint8_t data[2];
+    uint8_t data;
 
-    data[0] = addr;
     /* get status register */
-    if (ds3231_send(data, 1) && ds3231_recv(data+1, 1)) {
+    if (!ds3231_recv(addr, &data, 1))
+    {
         /* clear the flag */
         if (mode == DS3231_REPLACE)
-            data[1] = bits;
+            data = bits;
         else if (mode == DS3231_SET)
-            data[1] |= bits;
+            data |= bits;
         else
-            data[1] &= ~bits;
+            data &= ~bits;
 
-        if (ds3231_send(data, 2)) {
+        if (!ds3231_send(addr, &data, 1))
             return true;
-        }
     }
 
     return false;
@@ -142,7 +138,8 @@ bool ds3231_getOscillatorStopFlag(bool *flag)
 {
     uint8_t f;
 
-    if (ds3231_getFlag(DS3231_ADDR_STATUS, DS3231_STAT_OSCILLATOR, &f)) {
+    if (ds3231_getFlag(DS3231_ADDR_STATUS, DS3231_STAT_OSCILLATOR, &f))
+    {
         *flag = (f ? true : false);
         return true;
     }
@@ -202,7 +199,8 @@ bool ds3231_setSquarewaveFreq(uint8_t freq)
 {
     uint8_t flag = 0;
 
-    if (ds3231_getFlag(DS3231_ADDR_CONTROL, 0xff, &flag)) {
+    if (ds3231_getFlag(DS3231_ADDR_CONTROL, 0xff, &flag))
+    {
         /* clear current rate */
         flag &= ~DS3231_CTRL_SQWAVE_8192HZ;
         /* set new rate */
@@ -218,7 +216,8 @@ bool ds3231_getRawTemp(int16_t *temp)
     uint8_t data[2];
 
     data[0] = DS3231_ADDR_TEMP;
-    if (ds3231_send(data, 1) && ds3231_recv(data, 2)) {
+    if (!ds3231_recv(DS3231_ADDR_TEMP,data, 2))
+    {
         *temp = (int16_t)(int8_t)data[0] << 2 | data[1] >> 6;
         return true;
     }
@@ -254,14 +253,9 @@ bool ds3231_getTime(struct tm *time)
 {
     uint8_t data[7];
 
-    /* start register address */
-    data[0] = DS3231_ADDR_TIME;
-    if (!ds3231_send(data, 1)) {
-        return false;
-    }
-
     /* read time */
-    if (!ds3231_recv(data, 7)) {
+    if (ds3231_recv(DS3231_ADDR_TIME, data, 7))
+    {
         return false;
     }
 
diff --git a/extras/ds3231/ds3231.h b/extras/ds3231/ds3231.h
index ccc7335..40ff596 100644
--- a/extras/ds3231/ds3231.h
+++ b/extras/ds3231/ds3231.h
@@ -87,7 +87,7 @@ enum {
  * I suggest using GMT and applying timezone and DST when read back
  * returns true to indicate success
  */
-bool ds3231_setTime(struct tm *time);
+int ds3231_setTime(struct tm *time);
 
 /* Set alarms
  * alarm1 works with seconds, minutes, hours and day of week/month, or fires every second
@@ -100,7 +100,7 @@ bool ds3231_setTime(struct tm *time);
  * if you want to enable interrupts for the alarms you need to do that separately
  * returns true to indicate success
  */
-bool ds3231_setAlarm(uint8_t alarms, struct tm *time1, uint8_t option1, struct tm *time2, uint8_t option2);
+int ds3231_setAlarm(uint8_t alarms, struct tm *time1, uint8_t option1, struct tm *time2, uint8_t option2);
 
 /* Check if oscillator has previously stopped, e.g. no power/battery or disabled
  * sets flag to true if there has been a stop
diff --git a/extras/hmc5883l/hmc5883l.c b/extras/hmc5883l/hmc5883l.c
index 28d9cf7..ac1bcf2 100644
--- a/extras/hmc5883l/hmc5883l.c
+++ b/extras/hmc5883l/hmc5883l.c
@@ -54,14 +54,13 @@ static hmc5883l_operating_mode_t current_mode;
 
 static inline void write_register(uint8_t reg, uint8_t val)
 {
-    uint8_t buf[2] = { reg, val };
-    i2c_slave_write(ADDR, buf, 2);
+    i2c_slave_write(ADDR, &reg, &val, 1);
 }
 
 static inline uint8_t read_register(uint8_t reg)
 {
     uint8_t res;
-    i2c_slave_read(ADDR, reg, &res, 1);
+    i2c_slave_read(ADDR, &reg, &res, 1);
     return res;
 }
 
@@ -82,7 +81,8 @@ bool hmc5883l_init()
 uint32_t hmc5883l_get_id()
 {
     uint32_t res = 0;
-    i2c_slave_read(ADDR, REG_ID_A, (uint8_t *)&res, 3);
+    uint8_t reg = REG_ID_A;
+    i2c_slave_read(ADDR, &reg, (uint8_t *)&res, 3);
     return res;
 }
 
@@ -164,7 +164,8 @@ bool hmc5883l_get_raw_data(hmc5883l_raw_data_t *data)
         }
     }
     uint8_t buf[6];
-    i2c_slave_read(ADDR, REG_DX_H, buf, 6);
+    uint8_t reg = REG_DX_H;
+    i2c_slave_read(ADDR, &reg, buf, 6);
     data->x = ((int16_t)buf[REG_DX_H - REG_DX_H] << 8) | buf[REG_DX_L - REG_DX_H];
     data->y = ((int16_t)buf[REG_DY_H - REG_DX_H] << 8) | buf[REG_DY_L - REG_DX_H];
     data->z = ((int16_t)buf[REG_DZ_H - REG_DX_H] << 8) | buf[REG_DZ_L - REG_DX_H];
diff --git a/extras/i2c/i2c.c b/extras/i2c/i2c.c
index 2d31e8e..fb875f4 100644
--- a/extras/i2c/i2c.c
+++ b/extras/i2c/i2c.c
@@ -24,24 +24,35 @@
 
 #include <esp8266.h>
 #include <espressif/esp_misc.h> // sdk_os_delay_us
+#include <espressif/esp_system.h>
 #include "i2c.h"
 
+//#define I2C_DEBUG true
 
-// I2C driver for ESP8266 written for use with esp-open-rtos
-// Based on https://en.wikipedia.org/wiki/I²C#Example_of_bit-banging_the_I.C2.B2C_Master_protocol
-
-// With calling overhead, we end up at ~100kbit/s
-#define CLK_HALF_PERIOD_US (1)
+#ifdef I2C_DEBUG
+#define debug(fmt, ...) printf("%s: " fmt "\n", "I2C", ## __VA_ARGS__)
+#else
+#define debug(fmt, ...)
+#endif
 
 #define CLK_STRETCH  (10)
 
 static bool started;
+static bool flag;
+static bool force;
+static uint8_t freq ;
 static uint8_t g_scl_pin;
 static uint8_t g_sda_pin;
 
+inline bool i2c_status(void)
+{
+    return started;
+}
+
 void i2c_init(uint8_t scl_pin, uint8_t sda_pin)
 {
     started = false;
+    flag = false ;
     g_scl_pin = scl_pin;
     g_sda_pin = sda_pin;
 
@@ -55,11 +66,33 @@ void i2c_init(uint8_t scl_pin, uint8_t sda_pin)
     // I2C bus idle state.
     gpio_write(g_scl_pin, 1);
     gpio_write(g_sda_pin, 1);
+
+    // Prevent user, if frequency is high
+    if (sdk_system_get_cpu_freq() == SYS_CPU_80MHZ)
+        if (I2C_CUSTOM_DELAY_80MHZ == 1)
+            debug("Max frequency is 320Khz at 80MHz");
+
 }
 
 static inline void i2c_delay(void)
 {
-    sdk_os_delay_us(CLK_HALF_PERIOD_US);
+    uint32_t delay;
+    if (freq == SYS_CPU_160MHZ)
+    {
+        __asm volatile (
+            "movi %0, %1" "\n"
+            "1: addi %0, %0, -1" "\n"
+            "bnez %0, 1b" "\n"
+        : "=a" (delay) : "i" (I2C_CUSTOM_DELAY_160MHZ));
+    }
+    else
+    {
+        __asm volatile (
+            "movi %0, %1" "\n"
+            "1: addi %0, %0, -1" "\n"
+            "bnez %0, 1b" "\n"
+        : "=a" (delay) : "i" (I2C_CUSTOM_DELAY_80MHZ));
+    }
 }
 
 // Set SCL as input, allowing it to float high, and return current
@@ -96,6 +129,7 @@ static inline void clear_sda(void)
 void i2c_start(void)
 {
     uint32_t clk_stretch = CLK_STRETCH;
+    freq = sdk_system_get_cpu_freq();
     if (started) { // if started, do a restart cond
         // Set SDA to 1
         (void) read_sda();
@@ -104,18 +138,18 @@ void i2c_start(void)
         // Repeated start setup time, minimum 4.7us
         i2c_delay();
     }
+    started = true;
     if (read_sda() == 0) {
-        printf("I2C: arbitration lost in i2c_start\n");
+        debug("arbitration lost in i2c_start");
     }
     // SCL is high, set SDA from 1 to 0.
     clear_sda();
     i2c_delay();
     clear_scl();
-    started = true;
 }
 
 // Output stop condition
-void i2c_stop(void)
+bool i2c_stop(void)
 {
     uint32_t clk_stretch = CLK_STRETCH;
     // Set SDA to 0
@@ -127,10 +161,15 @@ void i2c_stop(void)
     i2c_delay();
     // SCL is high, set SDA from 0 to 1
     if (read_sda() == 0) {
-        printf("I2C: arbitration lost in i2c_stop\n");
+        debug("arbitration lost in i2c_stop");
     }
     i2c_delay();
+    if (!started) {
+        debug("link was break!");
+        return false ; //If bus was stop in other way, the current transmission Failed
+    }
     started = false;
+    return true;
 }
 
 // Write a bit to I2C bus
@@ -148,7 +187,7 @@ static void i2c_write_bit(bool bit)
     // SCL is high, now data is valid
     // If SDA is high, check that nobody else is driving SDA
     if (bit && read_sda() == 0) {
-        printf("I2C: arbitration lost in i2c_write_bit\n");
+        debug("arbitration lost in i2c_write_bit");
     }
     i2c_delay();
     clear_scl();
@@ -194,47 +233,95 @@ uint8_t i2c_read(bool ack)
     return byte;
 }
 
-bool i2c_slave_write(uint8_t slave_addr, uint8_t *data, uint8_t len)
+void i2c_force_bus(bool state)
 {
-    bool success = false;
-    do {
-        i2c_start();
-        if (!i2c_write(slave_addr << 1))
-            break;
-        while (len--) {
-            if (!i2c_write(*data++))
-                break;
-        }
-        i2c_stop();
-        success = true;
-    } while(0);
-    return success;
+    force = state ;
 }
 
-bool i2c_slave_read(uint8_t slave_addr, uint8_t data, uint8_t *buf, uint32_t len)
+static int i2c_bus_test()
 {
-    bool success = false;
-    do {
-        i2c_start();
-        if (!i2c_write(slave_addr << 1)) {
-            break;
-        }
-        i2c_write(data);
-        i2c_stop();
-        i2c_start();
-        if (!i2c_write(slave_addr << 1 | 1)) { // Slave address + read
-            break;
-        }
-        while(len) {
-            *buf = i2c_read(len == 1);
-            buf++;
-            len--;
-        }
-        success = true;
-    } while(0);
-    i2c_stop();
-    if (!success) {
-        printf("I2C: write error\n");
+    taskENTER_CRITICAL(); // To prevent task swaping after checking flag and before set it!
+    bool status = flag ; // get current status
+    if(force)
+    {
+        flag = true ; // force bus on
+        taskEXIT_CRITICAL();
+        if(status)
+           i2c_stop(); //Bus was busy, stop it.
     }
-    return success;
+    else
+    {
+        if (status)
+        {
+            taskEXIT_CRITICAL();
+            debug("busy");
+            taskYIELD(); // If bus busy, change task to try finish last com.
+            return -EBUSY ;  // If bus busy, inform user
+        }
+        else
+        {
+            flag = true ; // Set Bus busy
+            taskEXIT_CRITICAL();
+        }
+    }
+    return 0 ;
+}
+
+int i2c_slave_write(uint8_t slave_addr, const uint8_t *data, const uint8_t *buf, uint32_t len)
+{
+    if(i2c_bus_test())
+        return -EBUSY ;
+    i2c_start();
+    if (!i2c_write(slave_addr << 1))
+        goto error;
+    if(data != NULL)
+        if (!i2c_write(*data))
+            goto error;
+    while (len--) {
+        if (!i2c_write(*buf++))
+            goto error;
+    }
+    if (!i2c_stop())
+        goto error;
+    flag = false ; // Bus free
+    return 0;
+
+    error:
+    debug("Write Error");
+    i2c_stop();
+    flag = false ; // Bus free
+    return -EIO;
+}
+
+int i2c_slave_read(uint8_t slave_addr, const uint8_t *data, uint8_t *buf, uint32_t len)
+{
+    if(i2c_bus_test())
+        return -EBUSY ;
+    if(data != NULL) {
+        i2c_start();
+        if (!i2c_write(slave_addr << 1))
+            goto error;
+        if (!i2c_write(*data))
+            goto error;
+        if (!i2c_stop())
+            goto error;
+    }
+    i2c_start();
+    if (!i2c_write(slave_addr << 1 | 1)) // Slave address + read
+        goto error;
+    while(len) {
+        *buf = i2c_read(len == 1);
+        buf++;
+        len--;
+    }
+    if (!i2c_stop())
+        goto error;
+    flag = false ; // Bus free
+    return 0;
+
+    error:
+    debug("Read Error");
+    i2c_stop();
+    flag = false ; // Bus free
+    return -EIO;
 }
diff --git a/extras/i2c/i2c.h b/extras/i2c/i2c.h
index bfb3811..9fa65ca 100644
--- a/extras/i2c/i2c.h
+++ b/extras/i2c/i2c.h
@@ -27,31 +27,108 @@
 
 #include <stdint.h>
 #include <stdbool.h>
+#include <errno.h>
+#include <FreeRTOS.h>
+#include <task.h>
 
 #ifdef	__cplusplus
 extern "C" {
 #endif
 
-// Init bitbanging I2C driver on given pins
+
+#define I2C_FREQUENCY_400K true  // for test WIP
+
+/*
+ *  Some bit can be transmit slower.
+ *  Selected frequency fix the speed of a bit transmission
+ *  I2C lib take the maximum frequency defined
+ *  Don't change frequency when I2C transaction had begin
+ */
+
+#ifdef I2C_FREQUENCY_500K
+#define I2C_CUSTOM_DELAY_160MHZ   6
+#define I2C_CUSTOM_DELAY_80MHZ    1   //Sry, maximum is 320kHz at 80MHz
+#elif defined(I2C_FREQUENCY_400K)
+#define I2C_CUSTOM_DELAY_160MHZ   10
+#define I2C_CUSTOM_DELAY_80MHZ    1   //Sry, maximum is 320kHz at 80MHz
+#else
+#define I2C_CUSTOM_DELAY_160MHZ   100
+#define I2C_CUSTOM_DELAY_80MHZ    20
+#endif
+
+// I2C driver for ESP8266 written for use with esp-open-rtos
+// Based on https://en.wikipedia.org/wiki/I²C#Example_of_bit-banging_the_I.C2.B2C_Master_protocol
+// With calling overhead, we end up at ~320kbit/s
+
+//Level 0 API
+
+/**
+ * Init bitbanging I2C driver on given pins
+ * @param scl_pin SCL pin for I2C
+ * @param sda_pin SDA pin for I2C
+ */
 void i2c_init(uint8_t scl_pin, uint8_t sda_pin);
 
-// Write a byte to I2C bus. Return true if slave acked.
+/**
+ * Write a byte to I2C bus.
+ * @param byte Pointer to device descriptor
+ * @return true if slave acked
+ */
 bool i2c_write(uint8_t byte);
 
-// Read a byte from I2C bus. Return true if slave acked.
+/**
+ * Read a byte from I2C bus.
+ * @param ack Set Ack for slave (false: Ack // true: NoAck)
+ * @return byte read from slave.
+ */
 uint8_t i2c_read(bool ack);
 
-// Write 'len' bytes from 'buf' to slave. Return true if slave acked.
-bool i2c_slave_write(uint8_t slave_addr, uint8_t *buf, uint8_t len);
-
-// Issue a read operation and send 'data', followed by reading 'len' bytes
-// from slave into 'buf'. Return true if slave acked.
-bool i2c_slave_read(uint8_t slave_addr, uint8_t data, uint8_t *buf, uint32_t len);
-
-// Send start and stop conditions. Only needed when implementing protocols for
-// devices where the i2c_slave_[read|write] functions above are of no use.
+/**
+ * Send start or restart condition
+ */
 void i2c_start(void);
-void i2c_stop(void);
+
+/**
+ * Send stop condition
+ * @return false if link was broken
+ */
+bool i2c_stop(void);
+
+/**
+ * get status from I2C bus.
+ * @return true if busy.
+ */
+bool i2c_status(void);
+
+//Level 1 API (Don't need functions above)
+
+/**
+ * This function will allow you to force a transmission I2C, cancel current transmission.
+ * Warning: Use with precaution. Don't use it if you can avoid it. Usefull for priority transmission.
+ * @param state Force the next I2C transmission if true (Use with precaution)
+ */
+void i2c_force_bus(bool state);
+
+/**
+ * Write 'len' bytes from 'buf' to slave at 'data' register adress .
+ * @param slave_addr slave device address
+ * @param data Pointer to register address to send if non-null
+ * @param buf Pointer to data buffer
+ * @param len Number of byte to send
+ * @return Non-Zero if error occured
+ */
+int i2c_slave_write(uint8_t slave_addr, const uint8_t *data, const uint8_t *buf, uint32_t len);
+
+/**
+ * Issue a send operation of 'data' register adress, followed by reading 'len' bytes
+ * from slave into 'buf'.
+ * @param slave_addr slave device address
+ * @param data Pointer to register address to send if non-null
+ * @param buf Pointer to data buffer
+ * @param len Number of byte to read
+ * @return Non-Zero if error occured
+ */
+int i2c_slave_read(uint8_t slave_addr, const uint8_t *data, uint8_t *buf, uint32_t len);
 
 #ifdef	__cplusplus
 }
diff --git a/extras/ina3221/ina3221.c b/extras/ina3221/ina3221.c
index f507b2c..805b09d 100644
--- a/extras/ina3221/ina3221.c
+++ b/extras/ina3221/ina3221.c
@@ -10,6 +10,8 @@
 
 #include "ina3221.h"
 
+//#define INA3221_DEBUG true
+
 #ifdef INA3221_DEBUG
 #define debug(fmt, ...) printf("%s: " fmt "\n", "INA3221", ## __VA_ARGS__)
 #else
@@ -18,51 +20,20 @@
 
 static int _wireWriteRegister (uint8_t addr, uint8_t reg, uint16_t value)
 {
-    i2c_start();
-    if (!i2c_write(addr<<1)) // adress + W
-        goto error;
-    if (!i2c_write(reg))
-        goto error;
-    if (!i2c_write((value >> 8) & 0xFF))
-        goto error;
-    if (!i2c_write(value & 0xFF))
-        goto error;
-    i2c_stop();
+    uint8_t d[2] = { 0 , 0 };
+    d[1] = value  & 0x00FF;
+    d[0] = (value >> 8) & 0x00FF;
     debug("Data write to %02X : %02X+%04X\n",addr,reg,value);
-
-    return 0 ;
-
-    error:
-    debug("Error while xmitting I2C slave\n");
-    i2c_stop();
-    return -EIO;
+    return i2c_slave_write(addr, &reg, d, sizeof(d));
 }
 
 static int _wireReadRegister(uint8_t addr, uint8_t reg, uint16_t *value)
 {
-    uint8_t tampon[2] = { 0 } ;
-
-    i2c_start();
-    if (!i2c_write(addr<<1)) // adress + W
-        goto error;
-    if (!i2c_write(reg))
-        goto error;
-    i2c_stop();
-    i2c_start(); // restart condition
-    if (!i2c_write((addr<<1) | 1)) // adress + R
-        goto error;
-    tampon[1] = i2c_read(0);
-    tampon[0] = i2c_read(1);
-    i2c_stop();
-    *value = tampon[1]<<8 | tampon[0] ;
+    uint8_t d[] = {0, 0};
+    int error = i2c_slave_read(addr, &reg, d, sizeof(d))
     debug("Data read from %02X: %02X+%04X\n",addr,reg,*value);
-
-    return 0;
-
-    error:
-    debug("Error while xmitting I2C slave\n");
-    i2c_stop();
-    return -EIO;
+    *value = d[1] | (d[0] << 8);
+    return error;
 }
 
 int ina3221_trigger(ina3221_t *dev)
diff --git a/extras/ina3221/ina3221.h b/extras/ina3221/ina3221.h
index 000919c..74cec76 100644
--- a/extras/ina3221/ina3221.h
+++ b/extras/ina3221/ina3221.h
@@ -12,6 +12,8 @@
 
 #include <errno.h>
 #include <stdio.h>
+#include <FreeRTOS.h>
+#include <task.h>
 
 #ifdef __cplusplus
 extern "C" {
diff --git a/extras/mcp4725/mcp4725.c b/extras/mcp4725/mcp4725.c
index 889ddef..f53d6bd 100644
--- a/extras/mcp4725/mcp4725.c
+++ b/extras/mcp4725/mcp4725.c
@@ -14,11 +14,7 @@
 
 static void read_data(uint8_t addr, uint8_t *buf, uint8_t size)
 {
-    i2c_start();
-    i2c_write(addr << 1 | 1);
-    while (size--)
-        *(buf++) = i2c_read(!size);
-    i2c_stop();
+    i2c_slave_read(addr, NULL, buf, size);
 }
 
 bool mcp4725_eeprom_busy(uint8_t addr)
@@ -45,7 +41,7 @@ void mcp4725_set_power_mode(uint8_t addr, mcp4725_power_mode_t mode, bool eeprom
         value >> 4,
         value << 4
     };
-    i2c_slave_write(addr, data, 3);
+    i2c_slave_write(addr, &data[0], &data[1], 2);
 }
 
 uint16_t mcp4725_get_raw_output(uint8_t addr, bool eeprom)
@@ -65,7 +61,7 @@ void mcp4725_set_raw_output(uint8_t addr, uint16_t value, bool eeprom)
         value >> 4,
         value << 4
     };
-    i2c_slave_write(addr, data, 3);
+    i2c_slave_write(addr, &data[0], &data[1], 2);
 }
 
 
diff --git a/extras/ms561101ba03/ms561101ba03.c b/extras/ms561101ba03/ms561101ba03.c
index 282b21b..965e9df 100644
--- a/extras/ms561101ba03/ms561101ba03.c
+++ b/extras/ms561101ba03/ms561101ba03.c
@@ -26,60 +26,67 @@
  */
 #define CONVERSION_TIME     20 / portTICK_PERIOD_MS // milliseconds
 
-static inline bool reset(uint8_t addr)
+static inline int reset(uint8_t addr)
 {
     uint8_t buf[1] = { RESET };
-    return i2c_slave_write(addr, buf, 1);
+    return i2c_slave_write(addr, NULL, buf, 1);
 }
 
 static inline bool read_prom(ms561101ba03_t *dev)
 {
     uint8_t tmp[2] = { 0, 0 };
+    uint8_t reg = 0xA2 ;
 
-    if (!i2c_slave_read(dev->addr, 0xA2, tmp, 2))
+    if (i2c_slave_read(dev->addr, &reg, tmp, 2))
         return false;
     dev->config_data.sens = tmp[0] << 8 | tmp[1];
 
-    if (!i2c_slave_read(dev->addr, 0xA4, tmp, 2))
+    reg = 0xA4 ;
+    if (i2c_slave_read(dev->addr, &reg, tmp, 2))
         return false;
     dev->config_data.off = tmp[0] << 8 | tmp[1];
 
-    if (!i2c_slave_read(dev->addr, 0xA6, tmp, 2))
+    reg = 0xA6 ;
+    if (i2c_slave_read(dev->addr, &reg, tmp, 2))
         return false;
     dev->config_data.tcs = tmp[0] << 8 | tmp[1];
 
-    if (!i2c_slave_read(dev->addr, 0xA8, tmp, 2))
+    reg = 0xA8 ;
+    if (i2c_slave_read(dev->addr, &reg, tmp, 2))
         return false;
     dev->config_data.tco = tmp[0] << 8 | tmp[1];
 
-    if (!i2c_slave_read(dev->addr, 0xAA, tmp, 2))
+    reg = 0xAA ;
+    if (i2c_slave_read(dev->addr, &reg, tmp, 2))
         return false;
     dev->config_data.t_ref = tmp[0] << 8 | tmp[1];
 
-    if (!i2c_slave_read(dev->addr, 0xAC, tmp, 2))
+    reg = 0xAC ;
+    if (i2c_slave_read(dev->addr, &reg, tmp, 2))
         return false;
     dev->config_data.tempsens = tmp[0] << 8 | tmp[1];
 
     return true;
 }
 
-static inline bool start_pressure_conversion(ms561101ba03_t *dev) //D1
+static inline int start_pressure_conversion(ms561101ba03_t *dev) //D1
 {
     uint8_t buf = CONVERT_D1 + dev->osr;
-    return i2c_slave_write(dev->addr, &buf, 1);
+    return i2c_slave_write(dev->addr, NULL, &buf, 1);
 }
 
-static inline bool start_temperature_conversion(ms561101ba03_t *dev) //D2
+static inline int start_temperature_conversion(ms561101ba03_t *dev) //D2
 {
     uint8_t buf = CONVERT_D2 + dev->osr;
-    return i2c_slave_write(dev->addr, &buf, 1);
+    return i2c_slave_write(dev->addr, NULL, &buf, 1);
 }
 
 static inline bool read_adc(uint8_t addr, uint32_t *result)
 {
     *result = 0;
     uint8_t tmp[3];
-    if (!i2c_slave_read(addr, 0x00, tmp, 3))
+    uint8_t reg = 0x00 ;
+    if (i2c_slave_read(addr, &reg, tmp, 3))
         return false;
 
     *result = (tmp[0] << 16) | (tmp[1] << 8) | tmp[2];
@@ -132,7 +139,7 @@ static inline int32_t calc_p(uint32_t digital_pressure, int64_t sens, int64_t of
 
 static inline bool get_raw_temperature(ms561101ba03_t *dev, uint32_t *result)
 {
-    if (!start_temperature_conversion(dev))
+    if (start_temperature_conversion(dev))
         return false;
 
     vTaskDelay(CONVERSION_TIME);
@@ -145,7 +152,7 @@ static inline bool get_raw_temperature(ms561101ba03_t *dev, uint32_t *result)
 
 static inline bool get_raw_pressure(ms561101ba03_t *dev, uint32_t *result)
 {
-    if (!start_pressure_conversion(dev))
+    if (start_pressure_conversion(dev))
     	return false;
 
     vTaskDelay(CONVERSION_TIME);
@@ -209,7 +216,7 @@ bool ms561101ba03_get_sensor_data(ms561101ba03_t *dev)
 bool ms561101ba03_init(ms561101ba03_t *dev)
 {
     // First of all we need to reset the chip
-    if (!reset(dev->addr))
+    if (reset(dev->addr))
     	return false;
     // Wait a bit for the device to reset
     vTaskDelay(CONVERSION_TIME);
diff --git a/extras/pca9685/pca9685.c b/extras/pca9685/pca9685.c
index 0e116e2..daea4fc 100644
--- a/extras/pca9685/pca9685.c
+++ b/extras/pca9685/pca9685.c
@@ -61,15 +61,14 @@ inline static uint32_t round_div(uint32_t x, uint32_t y)
 
 inline static void write_reg(uint8_t addr, uint8_t reg, uint8_t val)
 {
-    uint8_t data[2] = { reg, val };
-    if (!i2c_slave_write(addr, data, 2))
+    if (i2c_slave_write(addr, &reg, &val, 1))
         debug("Could not write 0x%02x to 0x%02x, addr = 0x%02x", reg, val, addr);
 }
 
 inline static uint8_t read_reg(uint8_t addr, uint8_t reg)
 {
     uint8_t res = 0;
-    if (!i2c_slave_read(addr, reg, &res, 1))
+    if (i2c_slave_read(addr, &reg, &res, 1))
         debug("Could not read from 0x%02x, addr = 0x%02x", reg, addr);
     return res;
 }
@@ -192,8 +191,8 @@ void pca9685_set_pwm_value(uint8_t addr, uint8_t channel, uint16_t val)
     else if (val < 4096)
     {
         // Normal
-        uint8_t buf[5] = { reg, 0, 0, val, val >> 8 };
-        i2c_slave_write(addr, buf, 5);
+        uint8_t buf[4] = { 0, 0, val, val >> 8 };
+        i2c_slave_write(addr, &reg, buf, 4);
     }
     else
     {
diff --git a/extras/pcf8574/pcf8574.c b/extras/pcf8574/pcf8574.c
index e92ebc8..1d08625 100644
--- a/extras/pcf8574/pcf8574.c
+++ b/extras/pcf8574/pcf8574.c
@@ -3,9 +3,9 @@
 
 uint8_t pcf8574_port_read(uint8_t addr)
 {
-    i2c_start();
-    uint8_t res = i2c_write((addr << 1) | 1) ? i2c_read(1) : 0;
-    i2c_stop();
+    uint8_t res;
+    if (i2c_slave_read(addr, NULL, &res, 1))
+            return 0;
     return res;
 }
 
@@ -14,11 +14,8 @@ size_t pcf8574_port_read_buf(uint8_t addr, void *buf, size_t len)
     if (!len || !buf) return 0;
     uint8_t *_buf = (uint8_t *)buf;
 
-    i2c_start();
-    if (!i2c_write((addr << 1) | 1)) return 0;
-    for (size_t i = 0; i < len; i++)
-        *_buf++ = i2c_read(i == len - 1);
-    i2c_stop();
+    if (i2c_slave_read(addr, NULL, _buf, len))
+        return 0;
     return len;
 }
 
@@ -27,18 +24,14 @@ size_t pcf8574_port_write_buf(uint8_t addr, void *buf, size_t len)
     if (!len || !buf) return 0;
     uint8_t *_buf = (uint8_t *)buf;
 
-    i2c_start();
-    if (!i2c_write(addr << 1)) return 0;
-    for (size_t i = 0; i < len; i++)
-        i2c_write(*_buf++);
+    if (i2c_slave_write(addr, NULL, _buf, len))
+        return 0;
     return len;
 }
 
 void pcf8574_port_write(uint8_t addr, uint8_t value)
 {
-    i2c_start();
-    if (i2c_write(addr << 1)) i2c_write(value);
-    i2c_stop();
+    i2c_slave_write(addr, NULL, &value, 1);
 }
 
 bool pcf8574_gpio_read(uint8_t addr, uint8_t num)
diff --git a/extras/ssd1306/ssd1306.c b/extras/ssd1306/ssd1306.c
index c4f43a9..ca3b48a 100644
--- a/extras/ssd1306/ssd1306.c
+++ b/extras/ssd1306/ssd1306.c
@@ -88,6 +88,14 @@
 #define abs(x) ((x)<0 ? -(x) : (x))
 #define swap(x, y) do { typeof(x) temp##x##y = x; x = y; y = temp##x##y; } while (0)
 
+
+#if (SSD1306_I2C_SUPPORT)
+static int inline i2c_send(const ssd1306_t *dev, uint8_t reg, uint8_t* data, uint8_t len)
+{
+    return i2c_slave_write(dev->addr, &reg, data, len);
+}
+#endif
+
 /* Issue a command to SSD1306 device
  * I2C proto format:
  * |S|Slave Address|W|ACK|0x00|Command|Ack|P|
@@ -101,23 +109,7 @@ int ssd1306_command(const ssd1306_t *dev, uint8_t cmd)
     switch (dev->protocol) {
 #if (SSD1306_I2C_SUPPORT)
         case SSD1306_PROTO_I2C:
-            i2c_start();
-            if (!i2c_write(dev->addr << 1)) {
-                debug("Error while xmitting I2C slave address\n");
-                i2c_stop();
-                return -EIO;
-            }
-            if (!i2c_write(0x00)) {
-                debug("Error while xmitting transmission type\n");
-                i2c_stop();
-                return -EIO;
-            }
-            if (!i2c_write(cmd)) {
-                debug("Error while xmitting command: 0x%02X\n", cmd);
-                i2c_stop();
-                return -EIO;
-            }
-            i2c_stop();
+            return i2c_send(dev, 0x00, &cmd, 1);
             break;
 #endif
 #if (SSD1306_SPI4_SUPPORT)
@@ -247,7 +239,9 @@ int ssd1306_load_frame_buffer(const ssd1306_t *dev, uint8_t buf[])
 {
     uint16_t i;
     uint8_t j;
-
+#if (SSD1306_I2C_SUPPORT)
+    uint8_t tab[16] = { 0 } ;
+#endif
     size_t len = dev->width * dev->height / 8;
     if(dev->screen == SSD1306_SCREEN)
     {
@@ -258,31 +252,11 @@ int ssd1306_load_frame_buffer(const ssd1306_t *dev, uint8_t buf[])
     switch (dev->protocol) {
 #if (SSD1306_I2C_SUPPORT)
         case SSD1306_PROTO_I2C:
-            for (i = 0; i < len; i++) {
+            for (i = 0; i < len; i++)
+            {
                 if(dev->screen == SH1106_SCREEN && i%dev->width == 0) sh1106_go_coordinate(dev,0,i/dev->width);
-                i2c_start();
-                if (!i2c_write(dev->addr << 1)) {
-                    debug("Error while xmitting I2C slave address\n");
-                    i2c_stop();
-                    return -EIO;
-                }
-                if (!i2c_write(0x40)) {
-                    debug("Error while xmitting transmission type\n");
-                    i2c_stop();
-                    return -EIO;
-                }
-
-                for (j = 0; j < 16; j++) {
-                    if (!i2c_write(buf ? buf[i] : 0)) {
-                        debug("Error while writing to GDDRAM\n");
-                        i2c_stop();
-                        return -EIO;
-                    }
-                    i++;
-                }
-                i--;
-                i2c_stop();
-                taskYIELD();
+                i2c_send(dev, 0x40, buf ? &buf[i] : tab, 16);
+                i+=15 ;
             }
             break;
 #endif
diff --git a/extras/ssd1306/ssd1306.h b/extras/ssd1306/ssd1306.h
index 9f620d1..cdfae61 100644
--- a/extras/ssd1306/ssd1306.h
+++ b/extras/ssd1306/ssd1306.h
@@ -192,14 +192,6 @@ int ssd1306_set_display_offset(const ssd1306_t *dev, uint8_t offset);
  */
 int sh1106_set_charge_pump_voltage(const ssd1306_t *dev, sh1106_voltage_t select);
 
-/**
- * Select charge pump voltage. See value in datasheet.
- * @param dev Pointer to device descriptor
- * @param select Select charge pump voltage value
- * @return Non-zero if error occured
- */
-int sh1106_set_charge_pump_voltage(const ssd1306_t *dev, sh1106_voltage_t select);
-
 /**
  * Enable or disable the charge pump. See application note in datasheet.
  * @param dev Pointer to device descriptor
diff --git a/extras/tsl2561/tsl2561.c b/extras/tsl2561/tsl2561.c
index 36c52e9..991d803 100644
--- a/extras/tsl2561/tsl2561.c
+++ b/extras/tsl2561/tsl2561.c
@@ -96,19 +96,18 @@
 #define B8C 0x0000 // 0.000 * 2^LUX_SCALE
 #define M8C 0x0000 // 0.000 * 2^LUX_SCALE
 
-static bool write_register(uint8_t i2c_addr, uint8_t reg, uint8_t value)
+static int write_register(uint8_t i2c_addr, uint8_t reg, uint8_t value)
 {
-    uint8_t data[2];
-    data[0] = TSL2561_REG_COMMAND | reg;
-    data[1] = value;
-    return i2c_slave_write(i2c_addr, data, 2);
+    reg = TSL2561_REG_COMMAND | reg;
+    return i2c_slave_write(i2c_addr, &reg, &value, 1);
 }
 
 static uint8_t read_register(uint8_t i2c_addr, uint8_t reg)
 {
     uint8_t data[1];
+    reg = TSL2561_REG_COMMAND | reg;
 
-    if (!i2c_slave_read(i2c_addr, TSL2561_REG_COMMAND | reg, data, 1))
+    if (i2c_slave_read(i2c_addr, &reg , data, 1))
     {
         printf("Error in tsl261 read_register\n");
     }
@@ -120,8 +119,9 @@ static uint16_t read_register_16(uint8_t i2c_addr, uint8_t low_register_addr)
 {
     uint16_t value = 0;
     uint8_t data[2];
+    low_register_addr = TSL2561_REG_COMMAND | TSL2561_READ_WORD | low_register_addr;
 
-    if (!i2c_slave_read(i2c_addr, TSL2561_REG_COMMAND | TSL2561_READ_WORD | low_register_addr, data, 2))
+    if (i2c_slave_read(i2c_addr, &low_register_addr, data, 2))
     {
         printf("Error with i2c_slave_read in read_register_16\n");
     }
@@ -131,19 +131,19 @@ static uint16_t read_register_16(uint8_t i2c_addr, uint8_t low_register_addr)
     return value;
 }
 
-static bool enable(uint8_t i2c_addr)
+static int enable(uint8_t i2c_addr)
 {
     return write_register(i2c_addr, TSL2561_REG_CONTROL, TSL2561_ON);
 }
 
-static bool disable(uint8_t i2c_addr)
+static int disable(uint8_t i2c_addr)
 {
     return write_register(i2c_addr, TSL2561_REG_CONTROL, TSL2561_OFF);
 }
 
 void tsl2561_init(tsl2561_t *device)
 {
-    if (!enable(device->i2c_addr))
+    if (enable(device->i2c_addr))
     {
         printf("Error initializing tsl2561\n");
     }