diff --git a/os/hal/include/i2c.h b/os/hal/include/i2c.h
index 64816186b..5a85ed5f7 100644
--- a/os/hal/include/i2c.h
+++ b/os/hal/include/i2c.h
@@ -33,8 +33,21 @@
 /*===========================================================================*/
 /* Driver constants.                                                         */
 /*===========================================================================*/
-
-
+#define  I2CD_NO_ERROR                  0
+/** @brief Bus Error.*/
+#define  I2CD_BUS_ERROR                 0x01
+/** @brief Arbitration Lost (master mode).*/
+#define  I2CD_ARBITRATION_LOST          0x02
+/** @brief Acknowledge Failure.*/
+#define  I2CD_ACK_FAILURE               0x04
+/** @brief Overrun/Underrun.*/
+#define  I2CD_OVERRUN                   0x08
+/** @brief PEC Error in reception.*/
+#define  I2CD_PEC_ERROR                 0x10
+/** @brief Timeout or Tlow Error.*/
+#define  I2CD_TIMEOUT                   0x20
+/** @brief SMBus Alert.*/
+#define  I2CD_SMB_ALERT                 0x40
 /*===========================================================================*/
 /* Driver pre-compile time settings.                                         */
 /*===========================================================================*/
@@ -125,7 +138,6 @@ extern "C" {
   void i2cMasterReceive(I2CDriver *i2cp, I2CSlaveConfig *i2cscfg);
 
 
-
   void i2cMasterStartI(I2CDriver *i2cp,uint16_t header);
   void i2cMasterStopI(I2CDriver *i2cp);
   void i2cMasterRestartI(I2CDriver *i2cp);
diff --git a/os/hal/platforms/STM32/i2c_lld.c b/os/hal/platforms/STM32/i2c_lld.c
index df2685387..65cdec1b3 100644
--- a/os/hal/platforms/STM32/i2c_lld.c
+++ b/os/hal/platforms/STM32/i2c_lld.c
@@ -33,40 +33,76 @@ I2CDriver I2CD2;
 /* Driver local functions.                                                   */
 /*===========================================================================*/
 
-/**
- * @brief   TODO: Status bits translation.
- *
- * @param[in] sr        USART SR register value
- *
- * @return  The error flags.
- */
-static i2cflags_t translate_i2c_errors(uint16_t sr) {
-  i2cflags_t sts = 0;
-
-  if (sr & USART_SR_ORE)
-    sts |= UART_OVERRUN_ERROR;
-  if (sr & USART_SR_PE)
-    sts |= UART_PARITY_ERROR;
-  if (sr & USART_SR_FE)
-    sts |= UART_FRAMING_ERROR;
-  if (sr & USART_SR_NE)
-    sts |= UART_NOISE_ERROR;
-  if (sr & USART_SR_LBD)
-    sts |= UART_BREAK_DETECTED;
-  return sts;
-}
-
-
 static void i2c_serve_error_interrupt(I2CDriver *i2cp) {
-  // TODO:remove this stub and write normal handler
-  // this is simply trap for errors
-  while TRUE{
-    translate_i2c_errors(i2cp->id_i2c->SR1);
-  }
+    chSysLockFromIsr();
+    i2cp->id_slave_config->id_err_callback(i2cp, i2cp->id_slave_config);
+    chSysUnlockFromIsr();
 }
 
-/* This function handle all regular interrupt conditions
- * TODO: 10 bit address handling
+/* helper function, not API
+ * write bytes in DR register
+ * return TRUE if last byte written
+ */
+inline bool_t i2c_lld_txbyte(I2CDriver *i2cp) {
+#define _txbufhead (i2cp->id_slave_config->txbufhead)
+#define _txbytes (i2cp->id_slave_config->txbytes)
+#define _txbuf (i2cp->id_slave_config->txbuf)
+
+  if (_txbufhead < _txbytes){
+    /* disable interrupt to avoid jumping to ISR */
+    if ( _txbytes - _txbufhead == 1)
+      i2cp->id_i2c->CR2 &= (~I2C_CR2_ITBUFEN);
+    i2cp->id_i2c->DR = _txbuf[_txbufhead];
+    (_txbufhead)++;
+    return(FALSE);
+  }
+  _txbufhead = 0;
+  return(TRUE); // last byte written
+#undef _txbufhead
+#undef _txbytes
+#undef _txbuf
+}
+
+
+/* helper function, not API
+ * read bytes from DR register
+ * return TRUE if last byte read
+ */
+inline bool_t i2c_lld_rxbyte(I2CDriver *i2cp) {
+  // temporal variables
+#define _rxbuf     (i2cp->id_slave_config->rxbuf)
+#define _rxbufhead (i2cp->id_slave_config->rxbufhead)
+#define _rxdepth   (i2cp->id_slave_config->rxdepth)
+#define _rxbytes   (i2cp->id_slave_config->rxbytes)
+
+  /* In order to generate the non-acknowledge pulse after the last received
+   * data byte, the ACK bit must be cleared just after reading the second
+   * last data byte (after second last RxNE event).
+   */
+  if (_rxbufhead < (_rxbytes - 1)){
+    _rxbuf[_rxbufhead] = i2cp->id_i2c->DR;
+    if ((_rxbytes - _rxbufhead) <= 2){
+      i2cp->id_i2c->CR1 &= (~I2C_CR1_ACK);// clear ACK bit for automatically send NACK
+    }
+    (_rxbufhead)++;
+    return(FALSE);
+  }
+  /* disable interrupt to avoid jumping to ISR */
+  i2cp->id_i2c->CR2 &= (~I2C_CR2_ITBUFEN);
+
+  _rxbuf[_rxbufhead] = i2cp->id_i2c->DR; // read last byte
+  _rxbufhead = 0;
+  return(TRUE); // last byte read
+
+#undef _rxbuf
+#undef _rxbufhead
+#undef _rxdepth
+#undef _rxbytes
+}
+
+
+/*
+ * This function handle all regular interrupt conditions
  */
 static void i2c_serve_event_interrupt(I2CDriver *i2cp) {
   // debug variables
@@ -76,12 +112,13 @@ static void i2c_serve_event_interrupt(I2CDriver *i2cp) {
 
   if ((i2cp->id_state == I2C_READY) && (i2cp->id_i2c->SR1 & I2C_SR1_SB)){// start bit sent
     i2cp->id_state = I2C_MACTIVE;
+    //TODO: 10 bit address handling
     i2cp->id_i2c->DR = (i2cp->id_slave_config->addr7 << 1) |
                         i2cp->id_slave_config->rw_bit; // write slave address in DR
     return;
   }
 
-  // now "wait" interrupt with ADDR flag
+  // "wait" interrupt with ADDR flag
   if ((i2cp->id_state == I2C_MACTIVE) && (i2cp->id_i2c->SR1 & I2C_SR1_ADDR)){// address successfully sent
     if(i2cp->id_i2c->SR2 & I2C_SR2_TRA){
       i2c_lld_txbyte(i2cp); // send first byte
@@ -130,7 +167,6 @@ static void i2c_serve_event_interrupt(I2CDriver *i2cp) {
   }
 }
 
-
 #if STM32_I2C_USE_I2C1 || defined(__DOXYGEN__)
 /**
  * @brief I2C1 event interrupt handler.
@@ -225,6 +261,7 @@ void i2c_lld_start(I2CDriver *i2cp) {
   i2cp->id_i2c->CR1 = 0;
 
   i2c_lld_set_clock(i2cp);
+  i2c_lld_set_opmode(i2cp);
   i2cp->id_i2c->CR2 |= I2C_CR2_ITERREN | I2C_CR2_ITEVTEN | I2C_CR2_ITBUFEN;
   i2cp->id_i2c->CR1 |= 1; // enable interface
 }
@@ -263,7 +300,7 @@ void i2c_lld_set_clock(I2CDriver *i2cp) {
   /* Configure clock_div in standard mode */
   if (clock_speed <= 100000) {
     chDbgAssert(duty == stdDutyCycle,
-                "i2c_lld_set_clock(), #3", "Invalid standard mode duty cycle");
+                "i2c_lld_set_clock(), #1", "Invalid standard mode duty cycle");
     /* Standard mode clock_div calculate: Tlow/Thigh = 1/1 */
     clock_div = (uint16_t)(STM32_PCLK1 / (clock_speed * 2));
     /* Test if CCR value is under 0x4, and set the minimum allowed value */
@@ -276,7 +313,7 @@ void i2c_lld_set_clock(I2CDriver *i2cp) {
   /* Configure clock_div in fast mode */
   else if(clock_speed <= 400000) {
     chDbgAssert((duty == fastDutyCycle_2) || (duty == fastDutyCycle_16_9),
-                "i2c_lld_set_clock(), #3", "Invalid fast mode duty cycle");
+                "i2c_lld_set_clock(), #2", "Invalid fast mode duty cycle");
     if(duty == fastDutyCycle_2) {
       /* Fast mode clock_div calculate: Tlow/Thigh = 2/1 */
       clock_div = (uint16_t)(STM32_PCLK1 / (clock_speed * 3));
@@ -295,7 +332,7 @@ void i2c_lld_set_clock(I2CDriver *i2cp) {
     i2cp->id_i2c->TRISE = (freq * 300 / 1000) + 1;
   }
   chDbgAssert((clock_div <= I2C_CCR_CCR),
-      "i2c_lld_set_clock(), #2", "Too low clock clock speed selected");
+      "i2c_lld_set_clock(), #3", "Too low clock clock speed selected");
 
   /* Write to I2Cx CCR */
   i2cp->id_i2c->CCR = regCCR;
@@ -304,6 +341,47 @@ void i2c_lld_set_clock(I2CDriver *i2cp) {
   i2cp->id_i2c->CR1 |= pe_bit_saved;
 }
 
+void i2c_lld_set_opmode(I2CDriver *i2cp) {
+  I2C_opMode_t opmode = i2cp->id_config->opMode;
+  uint16_t regCR1;
+
+  /*---------------------------- CR1 Configuration ------------------------*/
+  /* Get the I2Cx CR1 value */
+  regCR1 = i2cp->id_i2c->CR1;
+  switch(opmode){
+  case opmodeI2C:
+    regCR1 &= (uint16_t)~(I2C_CR1_SMBUS|I2C_CR1_SMBTYPE);
+    break;
+  case opmodeSMBusDevice:
+    regCR1 |= I2C_CR1_SMBUS;
+    regCR1 &= (uint16_t)~(I2C_CR1_SMBTYPE);
+    break;
+  case opmodeSMBusHost:
+    regCR1 |= (I2C_CR1_SMBUS|I2C_CR1_SMBTYPE);
+    break;
+  }
+  /* Write to I2Cx CR1 */
+  i2cp->id_i2c->CR1 = regCR1;
+}
+
+void i2c_lld_set_own_address(I2CDriver *i2cp) {
+  //TODO: dual address mode
+
+  /*---------------------------- OAR1 Configuration -----------------------*/
+  i2cp->id_i2c->OAR1 |= 1 << 14;
+
+  if (&(i2cp->id_config->OwnAddress10) == NULL){// only 7-bit address
+    i2cp->id_i2c->OAR1 &= (~I2C_OAR1_ADDMODE);
+    i2cp->id_i2c->OAR1 |= i2cp->id_config->OwnAddress7 << 1;
+  }
+  else {
+    chDbgAssert((i2cp->id_config->OwnAddress10 < 1024),
+        "i2c_lld_set_own_address(), #1", "10-bit address longer then 10 bit")
+    i2cp->id_i2c->OAR1 |= I2C_OAR1_ADDMODE;
+    i2cp->id_i2c->OAR1 |= i2cp->id_config->OwnAddress10;
+  }
+}
+
 
 /**
  * @brief Deactivates the I2C peripheral.
@@ -333,66 +411,6 @@ void i2c_lld_stop(I2CDriver *i2cp) {
 }
 
 
-/* helper function, not API
- * write bytes in DR register
- * return TRUE if last byte written
- */
-inline bool_t i2c_lld_txbyte(I2CDriver *i2cp) {
-#define _txbufhead (i2cp->id_slave_config->txbufhead)
-#define _txbytes (i2cp->id_slave_config->txbytes)
-#define _txbuf (i2cp->id_slave_config->txbuf)
-
-  if (_txbufhead < _txbytes){
-    /* disable interrupt to avoid jumping to ISR */
-    if ( _txbytes - _txbufhead == 1)
-      i2cp->id_i2c->CR2 &= (~I2C_CR2_ITBUFEN);
-    i2cp->id_i2c->DR = _txbuf[_txbufhead];
-    (_txbufhead)++;
-    return(FALSE);
-  }
-  _txbufhead = 0;
-  return(TRUE); // last byte written
-#undef _txbufhead
-#undef _txbytes
-#undef _txbuf
-}
-
-
-/* helper function, not API
- * read bytes from DR register
- * return TRUE if last byte read
- */
-inline bool_t i2c_lld_rxbyte(I2CDriver *i2cp) {
-  // temporal variables
-#define _rxbuf     (i2cp->id_slave_config->rxbuf)
-#define _rxbufhead (i2cp->id_slave_config->rxbufhead)
-#define _rxdepth   (i2cp->id_slave_config->rxdepth)
-#define _rxbytes   (i2cp->id_slave_config->rxbytes)
-
-  /* In order to generate the non-acknowledge pulse after the last received
-   * data byte, the ACK bit must be cleared just after reading the second
-   * last data byte (after second last RxNE event).
-   */
-  if (_rxbufhead < (_rxbytes - 1)){
-    _rxbuf[_rxbufhead] = i2cp->id_i2c->DR;
-    if ((_rxbytes - _rxbufhead) <= 2){
-      i2cp->id_i2c->CR1 &= (~I2C_CR1_ACK);// clear ACK bit for automatically send NACK
-    }
-    (_rxbufhead)++;
-    return(FALSE);
-  }
-  /* disable interrupt to avoid jumping to ISR */
-  i2cp->id_i2c->CR2 &= (~I2C_CR2_ITBUFEN);
-
-  _rxbuf[_rxbufhead] = i2cp->id_i2c->DR; // read last byte
-  _rxbufhead = 0;
-  return(TRUE); // last byte read
-
-#undef _rxbuf
-#undef _rxbufhead
-#undef _rxdepth
-#undef _rxbytes
-}
 
 
 void i2c_lld_master_start(I2CDriver *i2cp){
@@ -448,35 +466,25 @@ void i2c_lld_master_transmit(I2CDriver *i2cp, I2CSlaveConfig *i2cscfg, bool_t re
 
 
   i2cp->id_i2c->CR1 |= I2C_CR1_START; // generate start condition
-  while (!(i2cp->id_i2c->SR1 & I2C_SR1_SB)){
-    i++; // wait Address sent
-  }
+  while (!(i2cp->id_i2c->SR1 & I2C_SR1_SB)); // wait Address sent
 
   i2cp->id_i2c->DR = (i2cp->id_slave_config->addr7 << 1) | I2C_WRITE; // write slave addres in DR
-  while (!(i2cp->id_i2c->SR1 & I2C_SR1_ADDR)){
-    i++; // wait Address sent
-  }
-  i = i2cp->id_i2c->SR2; // TODO: check is it need to read this register for I2C to proper functionality
+  while (!(i2cp->id_i2c->SR1 & I2C_SR1_ADDR)); // wait Address sent
+  i = i2cp->id_i2c->SR2;
   i = i2cp->id_i2c->SR1; //i2cp->id_i2c->SR1 &= (~I2C_SR1_ADDR); // clear ADDR bit
 
   // now write data byte by byte in DR register
   uint32_t n = 0;
   for (n = 0; n < i2cp->id_slave_config->txbytes; n++){
   	i2cp->id_i2c->DR = i2cscfg->txbuf[n];
-  	while (!(i2cp->id_i2c->SR1 & I2C_SR1_TXE)){
-  	  i++;
-  	}
+  	while (!(i2cp->id_i2c->SR1 & I2C_SR1_TXE));
   }
 
-  while (!(i2cp->id_i2c->SR1 & I2C_SR1_BTF)){
-    i++;
-  }
+  while (!(i2cp->id_i2c->SR1 & I2C_SR1_BTF));
 
   if (restart){
     i2cp->id_i2c->CR1 |= I2C_CR1_START; // generate restart condition
-    while (!(i2cp->id_i2c->SR1 & I2C_SR1_SB)){
-      i++; // wait start bit
-    }
+    while (!(i2cp->id_i2c->SR1 & I2C_SR1_SB)); // wait start bit
   }
   else i2cp->id_i2c->CR1 |= I2C_CR1_STOP; // generate stop condition
 }
@@ -492,19 +500,15 @@ void i2c_lld_master_transmit(I2CDriver *i2cp, I2CSlaveConfig *i2cscfg, bool_t re
  */
 void i2c_lld_master_receive(I2CDriver *i2cp, I2CSlaveConfig *i2cscfg) {
 
-  chSysLock();
-
   i2cp->id_slave_config = i2cscfg;
 
   uint16_t i = 0;
-  uint16_t tmp = 0;
 
   // send slave addres with read-bit
   i2cp->id_i2c->DR = (i2cp->id_slave_config->addr7 << 1) | I2C_READ;
-  while (!(i2cp->id_i2c->SR1 & I2C_SR1_ADDR)){
-    i++; // wait Address sent
-  }
-  i = i2cp->id_i2c->SR2; // TODO: check is it need to read this register for I2C to proper functionality
+  while (!(i2cp->id_i2c->SR1 & I2C_SR1_ADDR)); // wait Address sent
+
+  i = i2cp->id_i2c->SR2;
   i = i2cp->id_i2c->SR1; //i2cp->id_i2c->SR1 &= (~I2C_SR1_ADDR); // clear ADDR bit
 
   // set ACK bit
@@ -512,18 +516,15 @@ void i2c_lld_master_receive(I2CDriver *i2cp, I2CSlaveConfig *i2cscfg) {
 
   // collect data from slave
   for (i = 0; i < i2cp->id_slave_config->rxbytes; i++){
-    if ((i2cp->id_slave_config->rxbytes - i) == 1){ // TODO: is it better <= in place of == ?
+    if ((i2cp->id_slave_config->rxbytes - i) == 1){
         // clear ACK bit for automatically send NACK
         i2cp->id_i2c->CR1 &= (~I2C_CR1_ACK);}
-    while (!(i2cp->id_i2c->SR1 & I2C_SR1_RXNE)){
-      tmp++;
-    }
+    while (!(i2cp->id_i2c->SR1 & I2C_SR1_RXNE));
+
     i2cp->id_slave_config->rxbuf[i] = i2cp->id_i2c->DR;
   }
   // generate STOP
   i2cp->id_i2c->CR1 |= I2C_CR1_STOP;
-
-  chSysUnlock();
 }
 
 
diff --git a/os/hal/platforms/STM32/i2c_lld.h b/os/hal/platforms/STM32/i2c_lld.h
index 82333b0f7..25e451962 100644
--- a/os/hal/platforms/STM32/i2c_lld.h
+++ b/os/hal/platforms/STM32/i2c_lld.h
@@ -56,23 +56,6 @@
 /* Derived constants and error checks.                                       */
 /*===========================================================================*/
 
-/** @brief No pending conditions.*/
-#define I2C_NO_ERROR          0
-/*@brief external Stop or Start condition during an address or a data transfer*/
-#define I2C_BUS_ERROR         1
-/** @brief */
-#define I2C_ARBITRATION_LOSS  2
-/** @brief */
-#define I2C_ACK_FAIL          4
-/** @brief */
-#define I2C_OVERRUN_UNDERRUN  8
-/** @brief */
-#define I2C_PEC_ERROR         16
-/** @brief */
-#define I2C_TIMEOUT           32
-/** @brief */
-#define I2C_SMBUS_ALERT       64
-
 /*===========================================================================*/
 /* Driver data structures and types.                                         */
 /*===========================================================================*/
@@ -104,10 +87,12 @@ typedef void (*i2ccallback_t)(I2CDriver *i2cp, I2CSlaveConfig *i2cscfg);
 /**
  * @brief   I2C error notification callback type.
  *
- * @param[in] i2cp      TODO: pointer to the @p I2CDriver object triggering the
+ * @param[in] i2cp      pointer to the @p I2CDriver object triggering the
+ *                      callback
+ * @param[in] i2cscfg   pointer to the @p I2CSlaveConfig object triggering the
  *                      callback
  */
-typedef void (*i2cerrorcallback_t)(void);
+typedef void (*i2cerrorcallback_t)(I2CDriver *i2cp, I2CSlaveConfig *i2cscfg);
 
 typedef enum {
   opmodeI2C,
@@ -129,7 +114,7 @@ typedef struct {
   uint32_t ClockSpeed;              /*!< Specifies the clock frequency. Must be set to a value lower than 400kHz */
   I2C_DutyCycle_t FastModeDutyCycle;/*!< Specifies the I2C fast mode duty cycle */
   uint8_t OwnAddress7;              /*!< Specifies the first device 7-bit own address. */
-  uint8_t OwnAddress10;             /*!< Specifies the second part of device own address in 10-bit mode. */
+  uint16_t OwnAddress10;            /*!< Specifies the second part of device own address in 10-bit mode. Set to NULL if not used. */
 } I2CConfig;
 
 
@@ -174,12 +159,11 @@ struct I2CSlaveConfig{
   size_t                txbufhead;
 
   uint8_t               addr7;      // 7-bit address of the slave
-  uint8_t               addr10;     // used in 10-bit address mode. Set to NULL if not used
+  uint16_t              addr10;     // used in 10-bit address mode. Set to NULL if not used
 
-  uint16_t              error_flags;
   uint8_t               rw_bit;     // this flag contain R/W bit
   bool_t                restart;    // send restart or stop event after complete data tx/rx
-  //TODO: join error_flags, rw_bit, restart in one word.
+  //TODO: join rw_bit, restart in one word.
 
 #if I2C_USE_WAIT
   /**
@@ -224,13 +208,9 @@ struct I2CDriver{
    * @brief Pointer to the I2Cx registers block.
    */
   I2C_TypeDef           *id_i2c;
-
 } ;
 
 
-
-
-
 /*===========================================================================*/
 /* Driver macros.                                                            */
 /*===========================================================================*/
@@ -255,20 +235,18 @@ extern "C" {
 void i2c_lld_init(void);
 void i2c_lld_start(I2CDriver *i2cp);
 void i2c_lld_stop(I2CDriver *i2cp);
-
 void i2c_lld_set_clock(I2CDriver *i2cp);
+void i2c_lld_set_opmode(I2CDriver *i2cp);
+void i2c_lld_set_own_address(I2CDriver *i2cp);
 
 void i2c_lld_master_start(I2CDriver *i2cp);
 void i2c_lld_master_stop(I2CDriver *i2cp);
 
+
 void i2c_lld_master_transmit(I2CDriver *i2cp, I2CSlaveConfig *i2cscfg, bool_t restart);
 void i2c_lld_master_transmitI(I2CDriver *i2cp, I2CSlaveConfig *i2cscfg);
-bool_t i2c_lld_txbyte(I2CDriver *i2cp); // helper function
-
 void i2c_lld_master_receive(I2CDriver *i2cp, I2CSlaveConfig *i2cscfg);
 void i2c_lld_master_receiveI(I2CDriver *i2cp, I2CSlaveConfig *i2cscfg);
-bool_t i2c_lld_rxbyte(I2CDriver *i2cp);
-
 #ifdef __cplusplus
 }
 #endif
diff --git a/os/hal/src/i2c.c b/os/hal/src/i2c.c
index 5a0471e0f..04af9a6c2 100644
--- a/os/hal/src/i2c.c
+++ b/os/hal/src/i2c.c
@@ -161,9 +161,6 @@ void i2cMasterReceive(I2CDriver *i2cp, I2CSlaveConfig *i2cscfg) {
 
 
 
-
-
-
 #if I2C_USE_MUTUAL_EXCLUSION || defined(__DOXYGEN__)
 /**
  * @brief   Gains exclusive access to the I2C bus.