diff --git a/os/hal/include/i2c.h b/os/hal/include/i2c.h
index cf4d47713..5ee4887e9 100644
--- a/os/hal/include/i2c.h
+++ b/os/hal/include/i2c.h
@@ -87,67 +87,15 @@ typedef enum {
   I2C_READY = 2,                            /**< Ready.                     */
   I2C_ACTIVE_TRANSMIT = 3,                  /**< Transmitting.              */
   I2C_ACTIVE_RECEIVE = 4,                   /**< Receiving.                 */
-  I2C_ACTIVE_TRANSCEIVE = 5,                /**< Receiving after transmit.  */
 } i2cstate_t;
 
 #include "i2c_lld.h"
 
-/**
- * @brief   I2C notification callback type.
- * @details This callback invoked when byte transfer finish event occurs,
- *          no matter if sending or reading.
- *
- * @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 (*i2ccallback_t)(I2CDriver *i2cp, const I2CSlaveConfig *i2cscfg);
-
-/**
- * @brief   I2C error notification callback type.
- *
- * @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)(I2CDriver *i2cp,
-                                   const I2CSlaveConfig *i2cscfg);
-
-/**
- * @brief   I2C transmission data block size.
- */
-typedef uint8_t i2cblock_t;
-
-/**
- * @brief   Structure representing an I2C slave configuration.
- * @details Each slave device has its own config structure with input and
- *          output buffers for temporally storing data.
- */
-struct I2CSlaveConfig{
-  /**
-   * @brief Callback pointer.
-   * @note  Transfer finished callback. Invoke when all data transferred.
-   *        If set to @p NULL then the callback is disabled.
-   */
-  i2ccallback_t         id_callback;
-  /**
-   * @brief Callback pointer.
-   * @note  This callback will be invoked when error condition occur.
-   *        If set to @p NULL then the callback is disabled.
-   */
-  i2cerrorcallback_t    id_err_callback;
-#if defined(I2C_SLAVECONFIG_EXT_FIELDS)
-  I2C_SLAVECONFIG_EXT_FIELDS
-#endif
-};
 
 /*===========================================================================*/
 /* Driver macros.                                                            */
 /*===========================================================================*/
 
-#if I2C_USE_WAIT || defined(__DOXYGEN__)
 /**
  * @brief   Waits for operation completion.
  * @details This function waits for the driver to complete the current
@@ -185,58 +133,40 @@ struct I2CSlaveConfig{
     chSysUnlockFromIsr();                                                   \
   }                                                                         \
 }
-#else /* !I2C_USE_WAIT */
-#define _i2c_wait_s(i2cp)
-#define _i2c_wakeup_isr(i2cp)
-#endif /* !I2C_USE_WAIT */
 
 /**
  * @brief   Common ISR code.
  * @details This code handles the portable part of the ISR code:
- *          - Callback invocation.
- *          - Waiting thread wakeup, if any.
+ *          - Waiting thread wakeup.
  *          - Driver state transitions.
  *
  * @note    This macro is meant to be used in the low level drivers
  *          implementation only.
  *
  * @param[in] i2cp      pointer to the @p I2CDriver object
- * @param[in] i2cscfg   pointer to the @p I2CSlaveConfig object
  *
  * @notapi
  */
 #define _i2c_isr_code(i2cp, i2cscfg) {                                      \
-  if(((i2cp)->id_slave_config)->id_callback) {                              \
-    ((i2cp)->id_slave_config)->id_callback(i2cp, i2cscfg);                  \
-    (i2cp)->id_state = I2C_READY;                                           \
-  }                                                                         \
-  else                                                                      \
-    (i2cp)->id_state = I2C_READY;                                           \
+  (i2cp)->id_state = I2C_READY;                                             \
   _i2c_wakeup_isr(i2cp);                                                    \
 }
 
 /**
  * @brief   Error ISR code.
  * @details This code handles the portable part of the ISR code:
- *          - Error callback invocation.
- *          - Waiting thread wakeup, if any.
+ *          - Waiting thread wakeup.
  *          - Driver state transitions.
  *
  * @note    This macro is meant to be used in the low level drivers
  *          implementation only.
  *
  * @param[in] i2cp      pointer to the @p I2CDriver object
- * @param[in] i2cscfg   pointer to the @p I2CSlaveConfig object
  *
  * @notapi
  */
 #define _i2c_isr_err_code(i2cp, i2cscfg) {                                  \
-  if(((i2cp)->id_slave_config)->id_err_callback) {                          \
-    ((i2cp)->id_slave_config)->id_err_callback(i2cp, i2cscfg);              \
-    (i2cp)->id_state = I2C_READY;                                           \
-  }                                                                         \
-  else                                                                      \
-    (i2cp)->id_state = I2C_READY;                                           \
+  (i2cp)->id_state = I2C_READY;                                             \
   _i2c_wakeup_isr(i2cp);                                                    \
 }
 
@@ -251,15 +181,16 @@ extern "C" {
   void i2cObjectInit(I2CDriver *i2cp);
   void i2cStart(I2CDriver *i2cp, const I2CConfig *config);
   void i2cStop(I2CDriver *i2cp);
-  void i2cMasterTransmit(I2CDriver *i2cp, const I2CSlaveConfig *i2cscfg,
-                         uint16_t slave_addr,
+  i2cflags_t i2cMasterTransmit(I2CDriver *i2cp,
+                         uint8_t slave_addr,
                          uint8_t *txbuf, size_t txbytes,
                          uint8_t *rxbuf, size_t rxbytes);
-  void i2cMasterReceive(I2CDriver *i2cp, const I2CSlaveConfig *i2cscfg,
-                        uint16_t slave_addr, uint8_t *rxbuf, size_t rxbytes);
+  i2cflags_t i2cMasterReceive(I2CDriver *i2cp,
+                        uint8_t slave_addr, uint8_t *rxbuf, size_t rxbytes);
   void i2cMasterStart(I2CDriver *i2cp);
   void i2cMasterStop(I2CDriver *i2cp);
   void i2cAddFlagsI(I2CDriver *i2cp, i2cflags_t mask);
+  i2cflags_t i2cGetAndClearFlags(I2CDriver *i2cp);
 
 #if I2C_USE_MUTUAL_EXCLUSION
   void i2cAcquireBus(I2CDriver *i2cp);
diff --git a/os/hal/platforms/STM32/I2Cv1/i2c_lld.c b/os/hal/platforms/STM32/I2Cv1/i2c_lld.c
new file mode 100644
index 000000000..93b4c4061
--- /dev/null
+++ b/os/hal/platforms/STM32/I2Cv1/i2c_lld.c
@@ -0,0 +1,1100 @@
+/*
+    ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010,
+                 2011 Giovanni Di Sirio.
+
+    This file is part of ChibiOS/RT.
+
+    ChibiOS/RT is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 3 of the License, or
+    (at your option) any later version.
+
+    ChibiOS/RT is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file STM32/i2c_lld.c
+ * @brief STM32 I2C subsystem low level driver source. Slave mode not implemented.
+ * @addtogroup I2C
+ * @{
+ */
+
+#include "ch.h"
+#include "hal.h"
+#include "i2c_lld.h"
+
+#if HAL_USE_I2C || defined(__DOXYGEN__)
+
+/*===========================================================================*/
+/* Datasheet notes.                                                          */
+/*===========================================================================*/
+/**
+ * From RM0008.pdf
+ *
+ * Note:
+ * When the STOP, START or PEC bit is set, the software must NOT perform
+ * any write access to I2C_CR1 before this bit is cleared by hardware.
+ * Otherwise there is a  risk of setting a second STOP, START or PEC request.
+ */
+
+/*===========================================================================*/
+/* Driver constants.                                                         */
+/*===========================================================================*/
+#define I2C_STOP_GPT_TIMEOUT  50     /* waiting timer value */
+#define I2C_START_GPT_TIMEOUT 50     /* waiting timer value */
+#define I2C_POLLING_TIMEOUT   0xFFFF
+
+/*===========================================================================*/
+/* Driver exported variables.                                                */
+/*===========================================================================*/
+
+/** @brief I2C1 driver identifier.*/
+#if STM32_I2C_USE_I2C1 || defined(__DOXYGEN__)
+I2CDriver I2CD1;
+#endif
+
+/** @brief I2C2 driver identifier.*/
+#if STM32_I2C_USE_I2C2 || defined(__DOXYGEN__)
+I2CDriver I2CD2;
+#endif
+
+/*===========================================================================*/
+/* Driver local variables.                                                   */
+/*===========================================================================*/
+
+/* Debugging variables */
+#if CH_DBG_ENABLE_ASSERTS
+static volatile uint16_t dbgSR1 = 0;
+static volatile uint16_t dbgSR2 = 0;
+static volatile uint16_t dbgCR1 = 0;
+static volatile uint16_t dbgCR2 = 0;
+#endif /* CH_DBG_ENABLE_ASSERTS */
+
+/* defines for convenience purpose */
+#if I2C_SUPPORTS_CALLBACKS
+#define txBuffp (i2cp->txbuff_p)
+#define rxBuffp (i2cp->rxbuff_p)
+#endif /* I2C_SUPPORTS_CALLBACKS */
+
+/*===========================================================================*/
+/* Driver local functions.                                                   */
+/*===========================================================================*/
+#if I2C_SUPPORTS_CALLBACKS
+#if (!(STM32_I2C_I2C1_USE_POLLING_WAIT)) && STM32_I2C_USE_I2C1
+/* I2C1 GPT callback. */
+static void i2c1gptcb(GPTDriver *gptp) {
+  (void)gptp;
+  I2CDriver *i2cp = &I2CD1;
+
+  chSysLockFromIsr();
+  i2cp->flags &= ~I2C_FLG_TIMER_ARMED;
+
+  switch(i2cp->id_state){
+  case I2C_ACTIVE_TRANSMIT:
+    i2c_lld_master_transmit(i2cp, i2cp->slave_addr, i2cp->txbuf, i2cp->txbytes, i2cp->rxbuf, i2cp->rxbytes);
+  break;
+
+  case I2C_ACTIVE_RECEIVE:
+    i2c_lld_master_receive(i2cp, i2cp->slave_addr, i2cp->rxbuf, i2cp->rxbytes);
+  break;
+
+  case I2C_ACTIVE_TRANSCEIVE:
+    i2c_lld_master_transceive(i2cp);
+  break;
+
+  default:
+  break;
+  }
+  chSysUnlockFromIsr();
+}
+/* I2C1 GPT configuration. */
+static const GPTConfig i2c1gptcfg = {
+  1000000,  /* 1MHz timer clock.*/
+  i2c1gptcb    /* Timer callback.*/
+};
+#endif /* STM32_I2C_I2C1_USE_POLLING_WAIT */
+
+#if (!(STM32_I2C_I2C2_USE_POLLING_WAIT)) && STM32_I2C_USE_I2C2
+/* I2C2 GPT callback. */
+static void i2c2gptcb(GPTDriver *gptp) {
+  (void)gptp;
+  I2CDriver *i2cp = &I2CD2;
+
+  chSysLockFromIsr();
+  i2cp->flags &= ~I2C_FLG_TIMER_ARMED;
+
+  switch(i2cp->id_state){
+  case I2C_ACTIVE_TRANSMIT:
+    i2c_lld_master_transmit(i2cp, i2cp->slave_addr, i2cp->txbuf, i2cp->txbytes, i2cp->rxbuf, i2cp->rxbytes);
+  break;
+
+  case I2C_ACTIVE_RECEIVE:
+    i2c_lld_master_receive(i2cp, i2cp->slave_addr, i2cp->rxbuf, i2cp->rxbytes);
+  break;
+
+  case I2C_ACTIVE_TRANSCEIVE:
+    i2c_lld_master_transceive(i2cp);
+  break;
+
+  default:
+  break;
+  }
+  chSysUnlockFromIsr();
+}
+/* I2C2 GPT configuration. */
+static const GPTConfig i2c2gptcfg = {
+  1000000,  /* 1MHz timer clock.*/
+  i2c2gptcb    /* Timer callback.*/
+};
+#endif /* STM32_I2C_I2C2_USE_POLLING_WAIT */
+#endif /* I2C_SUPPORTS_CALLBACKS */
+
+/**
+ * @brief   Function for I2C debugging purpose.
+ * @note    Internal use only.
+ *
+ * @param[in] i2cp      pointer to the @p I2CDriver object
+ *
+ * @notapi
+ */
+#if CH_DBG_ENABLE_ASSERTS
+void _i2c_unhandled_case(I2CDriver *i2cp){
+  dbgCR1 = i2cp->id_i2c->CR1;
+  dbgCR2 = i2cp->id_i2c->CR2;
+  chDbgAssert((dbgSR1 + dbgSR2) == 0,
+        "i2c_serve_event_interrupt(), #1",
+        "unhandled case");
+}
+#else
+#define _i2c_unhandled_case(i2cp)
+#endif /* CH_DBG_ENABLE_ASSERTS */
+
+#if I2C_SUPPORTS_CALLBACKS
+/**
+ * @brief   Return the last event value from I2C status registers.
+ * @note    Internal use only.
+ *
+ * @param[in] i2cp      pointer to the @p I2CDriver object
+ *
+ * @notapi
+ */
+static uint32_t i2c_get_event(I2CDriver *i2cp){
+  uint16_t regSR1 = i2cp->id_i2c->SR1;
+  uint16_t regSR2 = i2cp->id_i2c->SR2;
+  #if CH_DBG_ENABLE_ASSERTS
+    dbgSR1 = regSR1;
+    dbgSR2 = regSR2;
+  #endif /* CH_DBG_ENABLE_ASSERTS */
+
+  return (I2C_EV_MASK & (regSR1 | (regSR2 << 16)));
+}
+
+/**
+ * @brief   Handle the flags/interrupts.
+ * @note    Internal use only.
+ *
+ * @param[in] i2cp      pointer to the @p I2CDriver object
+ *
+ * @notapi
+ */
+void _i2c_ev6_master_rec_mode_selected(I2CDriver *i2cp){
+  I2C_TypeDef *dp = i2cp->id_i2c;
+
+  switch(i2cp->flags & EV6_SUBEV_MASK) {
+
+  case I2C_EV6_3_MASTER_REC_1BTR_MODE_SELECTED: /* only an single byte to receive */
+    dp->CR1 &= (uint16_t)~I2C_CR1_ACK;          /* Clear ACK */
+    dp->CR1 |= I2C_CR1_STOP;                    /* Program the STOP */
+    break;
+
+  case I2C_EV6_1_MASTER_REC_2BTR_MODE_SELECTED: /* only two bytes to receive */
+    dp->CR1 &= (uint16_t)~I2C_CR1_ACK;          /* Clear ACK */
+    dp->CR2 &= (uint16_t)~I2C_CR2_ITBUFEN;      /* Disable the ITBUF in order to have only the BTF interrupt */
+    break;
+
+  default: /* more than 2 bytes to receive */
+    break;
+  }
+}
+
+/**
+ * @brief   Handle cases of 2 or 3 bytes receiving.
+ * @note    Internal use only.
+ *
+ * @param[in] i2cp      pointer to the @p I2CDriver object
+ *
+ * @notapi
+ */
+void _i2c_ev7_master_rec_byte_qued(I2CDriver *i2cp){
+  I2C_TypeDef *dp = i2cp->id_i2c;
+
+  switch(i2cp->flags & EV7_SUBEV_MASK) {
+
+  case I2C_EV7_2_MASTER_REC_3BYTES_TO_PROCESS:
+    /* Only for case of three bytes to be received.
+     * DataN-2 and DataN-1 already received. */
+    dp->CR1 &= (uint16_t)~I2C_CR1_ACK;  /* Clear ACK */
+    *rxBuffp = dp->DR;                  /* Read the DataN-2. This clear the RXE & BFT flags and launch the DataN exception in the shift register (ending the SCL stretch) */
+    rxBuffp++;
+    chSysLockFromIsr();
+    dp->CR1 |= I2C_CR1_STOP;            /* Program the STOP */
+    *rxBuffp = dp->DR;                  /* Read the DataN-1 */
+    chSysUnlockFromIsr();
+    rxBuffp++;
+    i2cp->rxbytes -= 2;                 /* Decrement the number of readed bytes */
+    i2cp->flags = 0;
+    dp->CR2 |= I2C_CR2_ITBUFEN;         /* ready for read DataN. Enable interrupt for next (and last) RxNE event*/
+    break;
+
+  case I2C_EV7_3_MASTER_REC_2BYTES_TO_PROCESS:
+    /* only for case of two bytes to be received
+     * DataN-1 and DataN are received */
+    dp->CR2 &= (uint16_t)~I2C_CR2_ITEVTEN;
+    dp->CR2 &= (uint16_t)~I2C_CR2_ITBUFEN;
+    chSysLockFromIsr();
+    dp->CR1 |= I2C_CR1_STOP;                    /* Program the STOP */
+    *rxBuffp = dp->DR;                          /* Read the DataN-1*/
+    rxBuffp++;
+    *rxBuffp = dp->DR;                          /* Read the DataN*/
+    chSysUnlockFromIsr();
+    i2cp->rxbytes = 0;
+    i2cp->flags = 0;
+    _i2c_isr_code(i2cp, i2cp->id_slave_config); /* Portable I2C ISR code defined in the high level driver. */
+    break;
+
+  case I2C_FLG_MASTER_RECEIVER:
+    /* Some times in hi load scenarions it is possible to "miss" interrupt
+     * because STM32 I2C has OR'ed interrupt sources. This case handle that
+     * scenario. */
+    if (i2cp->rxbytes > 3){
+      *rxBuffp = dp->DR;
+      rxBuffp++;
+      (i2cp->rxbytes)--;
+    }
+    else{
+      _i2c_unhandled_case(i2cp);
+    }
+    break;
+
+  default:
+    _i2c_unhandled_case(i2cp);
+    break;
+  }
+}
+
+/**
+ * @brief   Main I2C interrupt handler.
+ * @note    Internal use only.
+ *
+ * @param[in] i2cp      pointer to the @p I2CDriver object
+ *
+ * @notapi
+ */
+static void i2c_serve_event_interrupt(I2CDriver *i2cp) {
+  I2C_TypeDef *dp = i2cp->id_i2c;
+
+  switch(i2c_get_event(i2cp)) {
+
+  case I2C_EV5_MASTER_MODE_SELECT:
+    i2cp->flags &= ~I2C_FLG_HEADER_SENT;
+    dp->DR = i2cp->slave_addr1;
+    break;
+
+  case I2C_EV9_MASTER_ADDR_10BIT:
+    if(i2cp->flags & I2C_FLG_MASTER_RECEIVER) {
+      i2cp->slave_addr1 |= 0x01;
+      i2cp->flags |= I2C_FLG_HEADER_SENT;
+    }
+    dp->DR = i2cp->slave_addr2;
+    break;
+
+  /**************************************************************************
+   * Master Transmitter part
+   */
+  case I2C_EV6_MASTER_TRA_MODE_SELECTED:
+    if(i2cp->flags & I2C_FLG_HEADER_SENT){
+      dp->CR1 |= I2C_CR1_START;             /* re-send the start in 10-Bit address mode */
+      break;
+    }
+    txBuffp = (uint8_t*)i2cp->txbuf;        /* Initialize the transmit buffer pointer */
+    i2cp->txbytes--;
+    if(i2cp->txbytes == 0) {                /* If no further data to be sent, disable the I2C ITBUF in order to not have a TxE interrupt */
+      dp->CR2 &= (uint16_t)~I2C_CR2_ITBUFEN;
+    }
+    dp->DR = *txBuffp;                      /* EV8_1 write the first data */
+    txBuffp++;
+    break;
+
+  case I2C_EV8_MASTER_BYTE_TRANSMITTING:
+    if(i2cp->txbytes > 0) {
+      i2cp->txbytes--;
+      if(i2cp->txbytes == 0) {              /* If no further data to be sent, disable the ITBUF in order to not have a TxE interrupt */
+        dp->CR2 &= (uint16_t)~I2C_CR2_ITBUFEN;
+      }
+      dp->DR = *txBuffp;
+      txBuffp++;
+    }
+    break;
+
+  case I2C_EV8_2_MASTER_BYTE_TRANSMITTED:
+    dp->CR2 &= (uint16_t)~I2C_CR2_ITEVTEN;        /* Disable ITEVT In order to not have again a BTF IT */
+    if (i2cp->rxbytes == 0){                      /* if nothing to read then generate stop */
+      dp->CR1 |= I2C_CR1_STOP;
+      _i2c_isr_code(i2cp, i2cp->id_slave_config); /* Portable I2C ISR code defined in the high level driver. */
+    }
+    else{                                         /* start reading operation */
+      i2cp->id_i2c->CR1 |= I2C_CR1_START;         /* send start bit */
+      i2c_lld_master_transceive(i2cp);
+    }
+    break;
+
+  /**************************************************************************
+   * Master Receiver part
+   */
+  case I2C_EV6_MASTER_REC_MODE_SELECTED:
+    _i2c_ev6_master_rec_mode_selected(i2cp);
+    rxBuffp = i2cp->rxbuf;                        /* Initialize receive buffer pointer */
+    break;
+
+  case I2C_EV7_MASTER_REC_BYTE_RECEIVED:
+    if(i2cp->rxbytes > 3) {
+      *rxBuffp = dp->DR;                          /* Read the data register */
+      rxBuffp++;
+      i2cp->rxbytes--;
+      if(i2cp->rxbytes == 3){                     /* Disable the ITBUF in order to have only the BTF interrupt */
+        dp->CR2 &= (uint16_t)~I2C_CR2_ITBUFEN;
+        i2cp->flags |= I2C_FLG_3BTR;
+      }
+    }
+    else if (i2cp->rxbytes == 3){                 /* Disable the ITBUF in order to have only the BTF interrupt */
+      dp->CR2 &= (uint16_t)~I2C_CR2_ITBUFEN;
+      i2cp->flags |= I2C_FLG_3BTR;
+    }
+    break;
+
+  case I2C_EV7_MASTER_REC_BYTE_QUEUED:
+    _i2c_ev7_master_rec_byte_qued(i2cp);
+    break;
+
+  default:                                        /* only 1 byte must to be read to complete trasfer. Stop already sent to bus. */
+    chDbgAssert((i2cp->rxbytes) == 1,
+          "i2c_serve_event_interrupt(), #1",
+          "more than 1 byte to be received");
+    *rxBuffp = dp->DR;                            /* Read the data register */
+    i2cp->rxbytes = 0;
+    dp->CR2 &= (uint16_t)~I2C_CR2_ITEVTEN;        /* disable interrupts */
+    dp->CR2 &= (uint16_t)~I2C_CR2_ITBUFEN;
+    _i2c_isr_code(i2cp, i2cp->id_slave_config);   /* Portable I2C ISR code defined in the high level driver.*/
+    break;
+  }
+}
+#endif /* I2C_SUPPORTS_CALLBACKS */
+
+static void i2c_serve_error_interrupt(I2CDriver *i2cp) {
+  i2cflags_t flags;
+  I2C_TypeDef *reg;
+
+  reg = i2cp->id_i2c;
+  flags = I2CD_NO_ERROR;
+
+  if(reg->SR1 & I2C_SR1_BERR) {                /* Bus error */
+    reg->SR1 &= ~I2C_SR1_BERR;
+    flags |= I2CD_BUS_ERROR;
+  }
+  if(reg->SR1 & I2C_SR1_ARLO) {                /* Arbitration lost */
+    reg->SR1 &= ~I2C_SR1_ARLO;
+    flags |= I2CD_ARBITRATION_LOST;
+  }
+  if(reg->SR1 & I2C_SR1_AF) {                  /* Acknowledge fail */
+    reg->SR1 &= ~I2C_SR1_AF;
+    reg->CR1 |= I2C_CR1_STOP;                  /* setting stop bit */
+    while(i2cp->id_i2c->CR1 & I2C_CR1_STOP)
+      ;
+    flags |= I2CD_ACK_FAILURE;
+  }
+  if(reg->SR1 & I2C_SR1_OVR) {                 /* Overrun */
+    reg->SR1 &= ~I2C_SR1_OVR;
+    flags |= I2CD_OVERRUN;
+  }
+  if(reg->SR1 & I2C_SR1_PECERR) {              /* PEC error */
+    reg->SR1 &= ~I2C_SR1_PECERR;
+    flags |= I2CD_PEC_ERROR;
+  }
+  if(reg->SR1 & I2C_SR1_TIMEOUT) {             /* SMBus Timeout */
+    reg->SR1 &= ~I2C_SR1_TIMEOUT;
+    flags |= I2CD_TIMEOUT;
+  }
+  if(reg->SR1 & I2C_SR1_SMBALERT) {            /* SMBus alert */
+    reg->SR1 &= ~I2C_SR1_SMBALERT;
+    flags |= I2CD_SMB_ALERT;
+  }
+
+  if(flags != I2CD_NO_ERROR) {                /* send communication end signal */
+    chSysLockFromIsr();
+    i2cAddFlagsI(i2cp, flags);
+    chSysUnlockFromIsr();
+  #if I2C_SUPPORTS_CALLBACKS
+    _i2c_isr_err_code(i2cp, i2cp->id_slave_config);
+  #endif /* I2C_SUPPORTS_CALLBACKS */
+  }
+}
+
+
+#if STM32_I2C_USE_I2C1 || defined(__DOXYGEN__)
+/**
+ * @brief I2C1 event interrupt handler.
+ */
+#if I2C_SUPPORTS_CALLBACKS
+CH_IRQ_HANDLER(VectorBC) {
+
+  CH_IRQ_PROLOGUE();
+  i2c_serve_event_interrupt(&I2CD1);
+  CH_IRQ_EPILOGUE();
+}
+#endif /* I2C_SUPPORTS_CALLBACKS */
+/**
+ * @brief I2C1 error interrupt handler.
+ */
+CH_IRQ_HANDLER(VectorC0) {
+
+  CH_IRQ_PROLOGUE();
+  i2c_serve_error_interrupt(&I2CD1);
+  CH_IRQ_EPILOGUE();
+}
+#endif /* STM32_I2C_USE_I2C1 */
+
+#if STM32_I2C_USE_I2C2 || defined(__DOXYGEN__)
+/**
+ * @brief I2C2 event interrupt handler.
+ */
+#if I2C_SUPPORTS_CALLBACKS
+CH_IRQ_HANDLER(VectorC4) {
+
+  CH_IRQ_PROLOGUE();
+  i2c_serve_event_interrupt(&I2CD2);
+  CH_IRQ_EPILOGUE();
+}
+#endif /* I2C_SUPPORTS_CALLBACKS */
+/**
+ * @brief I2C2 error interrupt handler.
+ */
+CH_IRQ_HANDLER(VectorC8) {
+
+  CH_IRQ_PROLOGUE();
+  i2c_serve_error_interrupt(&I2CD2);
+  CH_IRQ_EPILOGUE();
+}
+#endif /* STM32_I2C_USE_I2C2 */
+
+/**
+ * @brief Low level I2C driver initialization.
+ */
+void i2c_lld_init(void) {
+
+#if STM32_I2C_USE_I2C1
+  i2cObjectInit(&I2CD1);
+  I2CD1.id_i2c      = I2C1;
+
+#if I2C_SUPPORTS_CALLBACKS
+#if !(STM32_I2C_I2C1_USE_POLLING_WAIT)
+  I2CD1.timer       = &(STM32_I2C_I2C1_USE_GPT_TIM);
+  I2CD1.timer_cfg   = &i2c1gptcfg;
+#endif /* !(STM32_I2C_I2C1_USE_POLLING_WAIT) */
+#endif /* I2C_SUPPORTS_CALLBACKS */
+
+#endif /* STM32_I2C_USE_I2C */
+
+#if STM32_I2C_USE_I2C2
+  i2cObjectInit(&I2CD2);
+  I2CD2.id_i2c      = I2C2;
+
+#if I2C_SUPPORTS_CALLBACKS
+#if !(STM32_I2C_I2C2_USE_POLLING_WAIT)
+  I2CD2.timer       = &(STM32_I2C_I2C2_USE_GPT_TIM);
+  I2CD2.timer_cfg   = &i2c2gptcfg;
+#endif /* !(STM32_I2C_I2C2_USE_POLLING_WAIT) */
+#endif /* I2C_SUPPORTS_CALLBACKS */
+
+#endif /* STM32_I2C_USE_I2C2 */
+}
+
+/**
+ * @brief Configures and activates the I2C peripheral.
+ *
+ * @param[in] i2cp      pointer to the @p I2CDriver object
+ */
+void i2c_lld_start(I2CDriver *i2cp) {
+  if (i2cp->id_state == I2C_STOP) {         /* If in stopped state then enables the I2C clock.*/
+#if STM32_I2C_USE_I2C1
+    if (&I2CD1 == i2cp) {
+#if I2C_SUPPORTS_CALLBACKS
+      NVICEnableVector(I2C1_EV_IRQn,
+          CORTEX_PRIORITY_MASK(STM32_I2C_I2C1_IRQ_PRIORITY));
+#endif /* I2C_SUPPORTS_CALLBACKS */
+      NVICEnableVector(I2C1_ER_IRQn,
+          CORTEX_PRIORITY_MASK(STM32_I2C_I2C1_IRQ_PRIORITY));
+      rccEnableI2C1(FALSE);
+    }
+#endif
+#if STM32_I2C_USE_I2C2
+    if (&I2CD2 == i2cp) {
+#if I2C_SUPPORTS_CALLBACKS
+      NVICEnableVector(I2C2_EV_IRQn,
+          CORTEX_PRIORITY_MASK(STM32_I2C_I2C1_IRQ_PRIORITY));
+#endif /* I2C_SUPPORTS_CALLBACKS */
+      NVICEnableVector(I2C2_ER_IRQn,
+          CORTEX_PRIORITY_MASK(STM32_I2C_I2C1_IRQ_PRIORITY));
+      rccEnableI2C2(FALSE);
+    }
+#endif
+  }
+
+  i2cp->id_i2c->CR1 = I2C_CR1_SWRST;        /* reset i2c peripherial */
+  i2cp->id_i2c->CR1 = 0;
+  i2c_lld_set_clock(i2cp);
+  i2c_lld_set_opmode(i2cp);
+  i2cp->id_i2c->CR1 |= 1;                   /* enable interface */
+}
+
+void i2c_lld_reset(I2CDriver *i2cp){
+  chDbgCheck((i2cp->id_state == I2C_STOP)||(i2cp->id_state == I2C_READY),
+             "i2c_lld_reset: invalid state");
+
+  rccResetI2C1();
+}
+
+
+/**
+ * @brief Set clock speed.
+ *
+ * @param[in] i2cp      pointer to the @p I2CDriver object
+ */
+void i2c_lld_set_clock(I2CDriver *i2cp) {
+  volatile uint16_t regCCR, regCR2, freq, clock_div;
+  volatile uint16_t pe_bit_saved;
+  int32_t clock_speed = i2cp->id_config->clock_speed;
+  i2cdutycycle_t duty = i2cp->id_config->duty_cycle;
+
+  chDbgCheck((i2cp != NULL) && (clock_speed > 0) && (clock_speed <= 4000000),
+             "i2c_lld_set_clock");
+
+  /**************************************************************************
+   * CR2 Configuration
+   */
+  regCR2 = i2cp->id_i2c->CR2;                   /* Get the I2Cx CR2 value */
+  regCR2 &= (uint16_t)~I2C_CR2_FREQ;            /* Clear frequency FREQ[5:0] bits */
+  freq = (uint16_t)(STM32_PCLK1 / 1000000);     /* Set frequency bits depending on pclk1 value */
+#ifdef STM32F4XX
+  chDbgCheck((freq >= 2) && (freq <= 42),
+                "i2c_lld_set_clock() : Peripheral clock freq. out of range");
+#else
+  chDbgCheck((freq >= 2) && (freq <= 36),
+              "i2c_lld_set_clock() : Peripheral clock freq. out of range");
+#endif
+  regCR2 |= freq;
+  i2cp->id_i2c->CR2 = regCR2;
+
+  /**************************************************************************
+   * CCR Configuration
+   */
+  pe_bit_saved = (i2cp->id_i2c->CR1 & I2C_CR1_PE);
+  i2cp->id_i2c->CR1 &= (uint16_t)~I2C_CR1_PE;                   /* Disable the selected I2C peripheral to configure TRISE */
+  regCCR = 0;                                                   /* Clear F/S, DUTY and CCR[11:0] bits */
+  clock_div = I2C_CCR_CCR;
+
+  if (clock_speed <= 100000) {                                  /* Configure clock_div in standard mode */
+    chDbgAssert(duty == STD_DUTY_CYCLE,
+                "i2c_lld_set_clock(), #1",
+                "Invalid standard mode duty cycle");
+    clock_div = (uint16_t)(STM32_PCLK1 / (clock_speed * 2));    /* Standard mode clock_div calculate: Tlow/Thigh = 1/1 */
+    if (clock_div < 0x04) clock_div = 0x04;                     /* Test if CCR value is under 0x4, and set the minimum allowed value */
+    regCCR |= (clock_div & I2C_CCR_CCR);                        /* Set clock_div value for standard mode */
+    i2cp->id_i2c->TRISE = freq + 1;                             /* Set Maximum Rise Time for standard mode */
+  }
+  else if(clock_speed <= 400000) {                              /* Configure clock_div in fast mode */
+    chDbgAssert((duty == FAST_DUTY_CYCLE_2) ||
+                (duty == FAST_DUTY_CYCLE_16_9),
+                "i2c_lld_set_clock(), #2",
+                "Invalid fast mode duty cycle");
+    if(duty == FAST_DUTY_CYCLE_2) {
+      clock_div = (uint16_t)(STM32_PCLK1 / (clock_speed * 3));  /* Fast mode clock_div calculate: Tlow/Thigh = 2/1 */
+    }
+    else if(duty == FAST_DUTY_CYCLE_16_9) {
+      clock_div = (uint16_t)(STM32_PCLK1 / (clock_speed * 25)); /* Fast mode clock_div calculate: Tlow/Thigh = 16/9 */
+      regCCR |= I2C_CCR_DUTY;                                   /* Set DUTY bit */
+    }
+    if(clock_div < 0x01) clock_div = 0x01;                      /* Test if CCR value is under 0x1, and set the minimum allowed value */
+    regCCR |= (I2C_CCR_FS | (clock_div & I2C_CCR_CCR));         /* Set clock_div value and F/S bit for fast mode*/
+    i2cp->id_i2c->TRISE = (freq * 300 / 1000) + 1;              /* Set Maximum Rise Time for fast mode */
+  }
+  chDbgAssert((clock_div <= I2C_CCR_CCR),
+      "i2c_lld_set_clock(), #3", "Too low clock clock speed selected");
+
+  i2cp->id_i2c->CCR = regCCR;                                   /* Write to I2Cx CCR */
+  i2cp->id_i2c->CR1 |= pe_bit_saved;                            /* restore the I2C peripheral enabled state */
+}
+
+/**
+ * @brief Set operation mode of I2C hardware.
+ *
+ * @param[in] i2cp      pointer to the @p I2CDriver object
+ */
+void i2c_lld_set_opmode(I2CDriver *i2cp) {
+  i2copmode_t opmode = i2cp->id_config->op_mode;
+  uint16_t regCR1;
+
+  regCR1 = i2cp->id_i2c->CR1;             /* Get the I2Cx CR1 value */
+  switch(opmode){
+  case OPMODE_I2C:
+    regCR1 &= (uint16_t)~(I2C_CR1_SMBUS|I2C_CR1_SMBTYPE);
+    break;
+  case OPMODE_SMBUS_DEVICE:
+    regCR1 |= I2C_CR1_SMBUS;
+    regCR1 &= (uint16_t)~(I2C_CR1_SMBTYPE);
+    break;
+  case OPMODE_SMBUS_HOST:
+    regCR1 |= (I2C_CR1_SMBUS|I2C_CR1_SMBTYPE);
+    break;
+  }
+
+  i2cp->id_i2c->CR1 = regCR1;             /* Write to I2Cx CR1 */
+}
+
+/**
+ * @brief Set own address.
+ *
+ * @param[in] i2cp      pointer to the @p I2CDriver object
+ */
+void i2c_lld_set_own_address(I2CDriver *i2cp) {
+  /* TODO: dual address mode */
+
+  i2cp->id_i2c->OAR1 |= 1 << 14;
+
+  if (&(i2cp->id_config->own_addr_10) == NULL){       /* only 7-bit address */
+    i2cp->id_i2c->OAR1 &= (~I2C_OAR1_ADDMODE);
+    i2cp->id_i2c->OAR1 |= i2cp->id_config->own_addr_7 << 1;
+  }
+  else {
+    chDbgAssert((i2cp->id_config->own_addr_10 < 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->own_addr_10;
+  }
+}
+
+
+/**
+ * @brief Deactivates the I2C peripheral.
+ *
+ * @param[in] i2cp      pointer to the @p I2CDriver object
+ */
+void i2c_lld_stop(I2CDriver *i2cp) {
+  if (i2cp->id_state == I2C_READY) {  /* If in ready state then disables the I2C clock.*/
+#if STM32_I2C_USE_I2C1
+    if (&I2CD1 == i2cp) {
+      NVICDisableVector(I2C1_EV_IRQn);
+      NVICDisableVector(I2C1_ER_IRQn);
+      rccDisableI2C1(FALSE);
+    }
+#endif
+#if STM32_I2C_USE_I2C2
+    if (&I2CD2 == i2cp) {
+      NVICDisableVector(I2C2_EV_IRQn);
+      NVICDisableVector(I2C2_ER_IRQn);
+      rccDisableI2C2(FALSE);
+    }
+#endif
+  }
+
+  i2cp->id_state = I2C_STOP;
+}
+
+
+#if I2C_SUPPORTS_CALLBACKS
+/**
+ * @brief Transmits data via the I2C bus as master.
+ *
+ * @param[in] i2cp        pointer to the @p I2CDriver object
+ * @param[in] slave_addr  Slave device address. Bits 0-9 contain slave
+ *                        device address. Bit 15 must be set to 1 if 10-bit
+ *                        addressing modes used. Otherwise	keep it cleared.
+ *                        Bits 10-14 unused.
+ * @param[in] txbuf       pointer to the transmit buffer
+ * @param[in] txbytes     number of bytes to be transmitted
+ * @param[in] rxbuf       pointer to the receive buffer
+ * @param[in] rxbytes     number of bytes to be received
+ */
+void i2c_lld_master_transmit(I2CDriver *i2cp, uint16_t slave_addr,
+    uint8_t *txbuf, size_t txbytes, uint8_t *rxbuf, size_t rxbytes) {
+
+  /* "waiting" for STOP bit routine*/
+  #if STM32_I2C_I2C1_USE_POLLING_WAIT
+    uint32_t timeout = I2C_POLLING_TIMEOUT;
+    while((i2cp->id_i2c->CR1 & I2C_CR1_STOP) && timeout)
+      timeout--;
+    chDbgAssert((timeout > 0), "i2c_lld_master_transmit(), #1", "time to STOP is out");
+  #else
+    chDbgAssert(!(i2cp->flags & I2C_FLG_TIMER_ARMED), "i2c_lld_master_transmit(), #1", "time to STOP is out");
+    if ((i2cp->id_i2c->CR1 & I2C_CR1_STOP) && i2cp->timer != NULL && i2cp->timer_cfg != NULL){
+      chSysLockFromIsr();
+      gptStartOneShotI(i2cp->timer, I2C_STOP_GPT_TIMEOUT);
+      i2cp->flags |= I2C_FLG_TIMER_ARMED;
+      chSysUnlockFromIsr();
+      return;
+    }
+  #endif /* STM32_I2C_I2C1_USE_POLLING_WAIT */
+
+  /* init driver fields */
+  i2cp->slave_addr = slave_addr;
+  i2cp->txbytes = txbytes;
+  i2cp->rxbytes = rxbytes;
+  i2cp->txbuf = txbuf;
+  i2cp->rxbuf = rxbuf;
+
+  /* init address fields */
+  if(slave_addr & 0x8000){                                    /* 10-bit mode used */
+    i2cp->slave_addr1 = ((slave_addr >>7) & 0x0006);          /* add the two msb of 10-bit address to the header */
+    i2cp->slave_addr1 |= 0xF0;                                /* add the header bits with LSB = 0 -> write */
+    i2cp->slave_addr2 = slave_addr & 0x00FF;                  /* the remaining 8 bit of 10-bit address */
+  }
+  else{
+    i2cp->slave_addr1 = ((slave_addr <<1) & 0x00FE);          /* LSB = 0 -> write */
+  }
+
+  /* setting flags and register bits */
+  i2cp->flags = 0;
+  i2cp->errors = 0;
+  i2cp->id_i2c->CR1 &= ~I2C_CR1_POS;
+  i2cp->id_i2c->CR1 |= I2C_CR1_START;
+  i2cp->id_i2c->CR2 |= (I2C_CR2_ITERREN|I2C_CR2_ITEVTEN|I2C_CR2_ITBUFEN); /* enable ERR, EVT & BUF ITs */
+}
+
+/**
+ * @brief Receives data from the I2C bus.
+ *
+ * @param[in] i2cp        pointer to the @p I2CDriver object
+ * @param[in] slave_addr  Slave device address. Bits 0-9 contain slave
+ *                        device address. Bit 15 must be set to 1 if 10-bit
+ *                        addressing modes used. Otherwise	keep it cleared.
+ *                        Bits 10-14 unused.
+ * @param[in] rxbuf       pointer to the receive buffer
+ * @param[in] rxbytes     number of bytes to be received
+ */
+void i2c_lld_master_receive(I2CDriver *i2cp, uint16_t slave_addr,
+    uint8_t *rxbuf, size_t rxbytes){
+
+  chDbgAssert((i2cp->id_i2c->SR1 + i2cp->id_i2c->SR2) == 0,
+        "i2c_lld_master_receive(), #1",
+        "some interrupt sources not clear");
+
+  /* "waiting" for STOP bit routine*/
+  #if STM32_I2C_I2C1_USE_POLLING_WAIT
+    uint32_t timeout = I2C_POLLING_TIMEOUT;
+    while((i2cp->id_i2c->CR1 & I2C_CR1_STOP) && timeout)
+      timeout--;
+    chDbgAssert((timeout > 0), "i2c_lld_master_receive(), #1", "time to STOP is out");
+  #else
+    chDbgAssert(!(i2cp->flags & I2C_FLG_TIMER_ARMED), "i2c_lld_master_receive(), #1", "time to STOP is out");
+    if ((i2cp->id_i2c->CR1 & I2C_CR1_STOP) && i2cp->timer != NULL && i2cp->timer_cfg != NULL){
+      chSysLockFromIsr();
+      gptStartOneShotI(i2cp->timer, I2C_STOP_GPT_TIMEOUT);
+      i2cp->flags |= I2C_FLG_TIMER_ARMED;
+      chSysUnlockFromIsr();
+      return;
+    }
+  #endif /* STM32_I2C_I2C1_USE_POLLING_WAIT */
+
+  /* init driver fields */
+	i2cp->slave_addr = slave_addr;
+	i2cp->rxbytes = rxbytes;
+	i2cp->rxbuf = rxbuf;
+
+	/* init address fields */
+  if(slave_addr & 0x8000){                            /* 10-bit mode used */
+    i2cp->slave_addr1 = ((slave_addr >>7) & 0x0006);  /* add the two msb of 10-bit address to the header */
+    i2cp->slave_addr1 |= 0xF0;                        /* add the header bits (the LSB -> 1 will be add to second */
+    i2cp->slave_addr2 = slave_addr & 0x00FF;          /* the remaining 8 bit of 10-bit address */
+  }
+  else{
+    i2cp->slave_addr1 = ((slave_addr <<1) | 0x01);    /* LSB = 1 -> receive */
+  }
+
+  /* setting flags and register bits */
+  i2cp->flags |= I2C_FLG_MASTER_RECEIVER;
+  i2cp->errors = 0;
+
+  i2cp->id_i2c->CR1 |= I2C_CR1_ACK;                   /* acknowledge returned */
+  i2cp->id_i2c->CR1 &= ~I2C_CR1_POS;
+
+  if(i2cp->rxbytes == 1) {                            /* Only one byte to be received */
+    i2cp->flags |= I2C_FLG_1BTR;
+  }
+  else if(i2cp->rxbytes == 2) {                       /* Only two bytes to be received */
+    i2cp->flags |= I2C_FLG_2BTR;
+    i2cp->id_i2c->CR1 |= I2C_CR1_POS;                 /* Acknowledge Position */
+  }
+
+  i2cp->id_i2c->CR1 |= I2C_CR1_START;                 /* send start bit */
+  i2cp->id_i2c->CR2 |= (I2C_CR2_ITERREN|I2C_CR2_ITEVTEN|I2C_CR2_ITBUFEN); /* enable ERR, EVT & BUF ITs */
+}
+
+
+/**
+ * @brief Realize read-though-write behavior.
+ *
+ * @param[in] i2cp        pointer to the @p I2CDriver object
+ *
+ * @notapi
+ */
+void i2c_lld_master_transceive(I2CDriver *i2cp){
+
+  chDbgAssert((i2cp != NULL) && (i2cp->slave_addr1 != 0) &&\
+      (i2cp->rxbytes > 0) && (i2cp->rxbuf != NULL),
+      "i2c_lld_master_transceive(), #1",
+      "");
+
+  i2cp->id_state = I2C_ACTIVE_TRANSCEIVE;
+
+  /* "waiting" for START bit routine*/
+  #if STM32_I2C_I2C1_USE_POLLING_WAIT
+    uint32_t timeout = I2C_POLLING_TIMEOUT;
+    while((i2cp->id_i2c->CR1 & I2C_CR1_START) && timeout);
+      timeout--;
+    chDbgAssert((timeout > 0), "i2c_lld_master_transceive(), #1", "time to START is out");
+  #else
+    chDbgAssert(!(i2cp->flags & I2C_FLG_TIMER_ARMED), "i2c_lld_master_transceive(), #1", "time to START is out");
+    if ((i2cp->id_i2c->CR1 & I2C_CR1_START) && i2cp->timer != NULL && i2cp->timer_cfg != NULL){
+      chSysLockFromIsr();
+      gptStartOneShotI(i2cp->timer, I2C_START_GPT_TIMEOUT);
+      i2cp->flags |= I2C_FLG_TIMER_ARMED;
+      chSysUnlockFromIsr();
+      return;
+    }
+  #endif /* STM32_I2C_I2C1_USE_POLLING_WAIT */
+
+  /* init address fields */
+  if(i2cp->slave_addr & 0x8000){                          /* 10-bit mode used */
+    i2cp->slave_addr1 = ((i2cp->slave_addr >>7) & 0x0006);/* add the two msb of 10-bit address to the header */
+    i2cp->slave_addr1 |= 0xF0;                            /* add the header bits (the LSB -> 1 will be add to second */
+    i2cp->slave_addr2 = i2cp->slave_addr & 0x00FF;        /* the remaining 8 bit of 10-bit address */
+  }
+  else{
+    i2cp->slave_addr1 |= 0x01;
+  }
+
+  /* setting flags and register bits */
+  i2cp->flags |= I2C_FLG_MASTER_RECEIVER;
+  i2cp->errors = 0;
+  i2cp->id_i2c->CR1 |= I2C_CR1_ACK;                       /* acknowledge returned */
+  i2cp->id_i2c->CR1 &= ~I2C_CR1_POS;
+
+  if(i2cp->rxbytes == 1) {                                /* Only one byte to be received */
+    i2cp->flags |= I2C_FLG_1BTR;
+  }
+  else if(i2cp->rxbytes == 2) {                           /* Only two bytes to be received */
+    i2cp->flags |= I2C_FLG_2BTR;
+    i2cp->id_i2c->CR1 |= I2C_CR1_POS;                     /* Acknowledge Position */
+  }
+
+  i2cp->id_i2c->CR2 |= (I2C_CR2_ITERREN|I2C_CR2_ITEVTEN|I2C_CR2_ITBUFEN);  /* enable ERR, EVT & BUF ITs */
+}
+
+#else /*I2C_SUPPORTS_CALLBACKS*/
+
+/**
+ * @brief Synchronously transmits data via the I2C bus as master.
+ *
+ * @param[in] i2cp        pointer to the @p I2CDriver object
+ * @param[in] slave_addr  Slave device address. Bits 0-9 contain slave
+ *                        device address. Bit 15 must be set to 1 if 10-bit
+ *                        addressing modes used. Otherwise  keep it cleared.
+ *                        Bits 10-14 unused.
+ * @param[in] txbuf       pointer to the transmit buffer
+ * @param[in] txbytes     number of bytes to be transmitted
+ * @param[in] rxbuf       pointer to the receive buffer
+ * @param[in] rxbytes     number of bytes to be received
+ */
+void i2c_lld_master_transmit(I2CDriver *i2cp, uint16_t slave_addr,
+    uint8_t *txbuf, size_t txbytes, uint8_t *rxbuf, size_t rxbytes) {
+
+  /* init driver fields */
+  i2cp->slave_addr = slave_addr;
+  i2cp->txbytes = txbytes;
+  i2cp->rxbytes = rxbytes;
+  i2cp->txbuf = txbuf;
+  i2cp->rxbuf = rxbuf;
+
+  /* init address fields */
+  if(slave_addr & 0x8000){                                    /* 10-bit mode used */
+    i2cp->slave_addr1 = ((slave_addr >>7) & 0x0006);          /* add the two msb of 10-bit address to the header */
+    i2cp->slave_addr1 |= 0xF0;                                /* add the header bits with LSB = 0 -> write */
+    i2cp->slave_addr2 = slave_addr & 0x00FF;                  /* the remaining 8 bit of 10-bit address */
+  }
+  else{
+    i2cp->slave_addr1 = ((slave_addr <<1) & 0x00FE);          /* LSB = 0 -> write */
+  }
+
+  i2cp->flags = 0;
+  i2cp->errors = 0;
+  i2cp->id_i2c->CR1 &= ~I2C_CR1_POS;
+  i2cp->id_i2c->CR2 &= ~I2C_CR2_ITEVTEN;                      /* disable event interrupts */
+  i2cp->id_i2c->CR2 |= I2C_CR2_ITERREN;                       /* enable error interrupts */
+
+  i2cp->id_i2c->CR1 |= I2C_CR1_START;
+  while (!(i2cp->id_i2c->SR1 & I2C_SR1_SB))
+    ;
+  i2cp->id_i2c->DR = i2cp->slave_addr1;
+  while (!(i2cp->id_i2c->SR1 & I2C_SR1_ADDR))
+    ;
+  while (!(i2cp->id_i2c->SR2 & I2C_SR2_BUSY))
+    ;
+  i2cp->id_i2c->DR = *txbuf;
+  txbuf++;
+  i2cp->txbytes--;
+  while(i2cp->txbytes > 0){
+    while(!(i2cp->id_i2c->SR1 & I2C_SR1_BTF))
+      ;
+    i2cp->id_i2c->DR = *txbuf;
+    txbuf++;
+    i2cp->txbytes--;
+  }
+  while(!(i2cp->id_i2c->SR1 & I2C_SR1_BTF))
+    ;
+  if(rxbytes == 0){
+    i2cp->id_i2c->CR1 |= I2C_CR1_STOP;
+    while (i2cp->id_i2c->CR1 & I2C_CR1_STOP)
+      ;
+    }
+  else{
+    i2c_lld_master_receive(i2cp, slave_addr, rxbuf, rxbytes);
+  }
+}
+
+
+/**
+ * @brief Synchronously receives data from the I2C bus.
+ *
+ * @param[in] i2cp        pointer to the @p I2CDriver object
+ * @param[in] slave_addr  Slave device address. Bits 0-9 contain slave
+ *                        device address. Bit 15 must be set to 1 if 10-bit
+ *                        addressing modes used. Otherwise  keep it cleared.
+ *                        Bits 10-14 unused.
+ * @param[in] rxbuf       pointer to the receive buffer
+ * @param[in] rxbytes     number of bytes to be received
+ */
+void i2c_lld_master_receive(I2CDriver *i2cp, uint16_t slave_addr,
+    uint8_t *rxbuf, size_t rxbytes){
+
+  /* init driver fields */
+  i2cp->slave_addr = slave_addr;
+  i2cp->rxbytes = rxbytes;
+  i2cp->rxbuf = rxbuf;
+
+  /* init address fields */
+  if(slave_addr & 0x8000){                            /* 10-bit mode used */
+    i2cp->slave_addr1 = ((slave_addr >>7) & 0x0006);  /* add the two msb of 10-bit address to the header */
+    i2cp->slave_addr1 |= 0xF0;                        /* add the header bits (the LSB -> 1 will be add to second */
+    i2cp->slave_addr2 = slave_addr & 0x00FF;          /* the remaining 8 bit of 10-bit address */
+  }
+  else{
+    i2cp->slave_addr1 = ((slave_addr <<1) | 0x01);    /* LSB = 1 -> receive */
+  }
+
+
+  /* setting flags and register bits */
+  i2cp->flags = 0;
+  i2cp->errors = 0;
+  i2cp->id_i2c->CR1 &= ~I2C_CR1_POS;
+  i2cp->id_i2c->CR2 &= ~I2C_CR2_ITEVTEN;              /* disable event interrupts */
+  i2cp->id_i2c->CR2 |= I2C_CR2_ITERREN;               /* enable error interrupts */
+
+  i2cp->id_i2c->CR1 |= I2C_CR1_START;
+  while (!(i2cp->id_i2c->SR1 & I2C_SR1_SB))
+    ;
+
+  i2cp->id_i2c->DR = i2cp->slave_addr1;
+  while (!(i2cp->id_i2c->SR1 & I2C_SR1_ADDR))
+    ;
+
+  if(i2cp->rxbytes >= 3){                             /* more than 2 bytes receiving procedure */
+    while(!(i2cp->id_i2c->SR2 & I2C_SR2_BUSY))        /* to clear ADDR bit */
+      ;
+    while(i2cp->rxbytes > 3){
+      while(!(i2cp->id_i2c->SR1 & I2C_SR1_BTF))
+        ;
+      *rxbuf = i2cp->id_i2c->DR;
+      rxbuf++;
+      i2cp->rxbytes--;
+    }
+    while(!(i2cp->id_i2c->SR1 & I2C_SR1_BTF))          /* stopping procedure */
+      ;
+    i2cp->id_i2c->CR1 &= ~I2C_CR1_ACK;
+    chSysLock();
+    i2cp->id_i2c->CR1 |= I2C_CR1_STOP;
+    *rxbuf = i2cp->id_i2c->DR;
+    rxbuf++;
+    i2cp->rxbytes--;
+    chSysUnlock();
+    while(!(i2cp->id_i2c->SR1 & I2C_SR1_RXNE))
+      ;
+    *rxbuf = i2cp->id_i2c->DR;
+    rxbuf++;
+    i2cp->rxbytes--;
+    while (i2cp->id_i2c->CR1 & I2C_CR1_STOP)
+      ;
+    i2cp->id_i2c->CR1 |= I2C_CR1_ACK;
+  }
+  else{                                                 /* 1 or 2 bytes receiving procedure */
+    if(i2cp->rxbytes == 2){
+      i2cp->id_i2c->CR1 |= I2C_CR1_POS;
+      chSysLock();
+      while(!(i2cp->id_i2c->SR2 & I2C_SR2_BUSY))        /* to clear ADDR bit */
+        ;
+      i2cp->id_i2c->CR1 &= ~I2C_CR1_ACK;
+      chSysUnlock();
+      while(!(i2cp->id_i2c->SR1 & I2C_SR1_BTF))
+        ;
+      chSysLock();
+      i2cp->id_i2c->CR1 |= I2C_CR1_STOP;
+      *rxbuf = i2cp->id_i2c->DR;
+      rxbuf++;
+      i2cp->rxbytes--;
+      chSysUnlock();
+      *rxbuf = i2cp->id_i2c->DR;
+      rxbuf++;
+      i2cp->rxbytes--;
+      while (i2cp->id_i2c->CR1 & I2C_CR1_STOP)
+        ;
+      i2cp->id_i2c->CR1 &= ~I2C_CR1_POS;
+      i2cp->id_i2c->CR1 |= I2C_CR1_ACK;
+    }
+    else{ /* 1 byte */
+      i2cp->id_i2c->CR1 &= ~I2C_CR1_ACK;
+      chSysLock();
+      while(!(i2cp->id_i2c->SR2 & I2C_SR2_BUSY))       /* to clear ADDR bit */
+        ;
+      i2cp->id_i2c->CR1 |= I2C_CR1_STOP;
+      chSysUnlock();
+      while(!(i2cp->id_i2c->SR1 & I2C_SR1_RXNE))
+        ;
+      *rxbuf = i2cp->id_i2c->DR;
+      rxbuf++;
+      i2cp->rxbytes--;
+      while (i2cp->id_i2c->CR1 & I2C_CR1_STOP)
+        ;
+      i2cp->id_i2c->CR1 |= I2C_CR1_ACK;
+    }
+  }
+}
+#endif /* I2C_SUPPORTS_CALLBACKS */
+
+#undef rxBuffp
+#undef txBuffp
+
+#endif /* HAL_USE_I2C */
diff --git a/os/hal/platforms/STM32/I2Cv1/i2c_lld.h b/os/hal/platforms/STM32/I2Cv1/i2c_lld.h
new file mode 100644
index 000000000..81a9f62dc
--- /dev/null
+++ b/os/hal/platforms/STM32/I2Cv1/i2c_lld.h
@@ -0,0 +1,318 @@
+/*
+    ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010,
+                 2011 Giovanni Di Sirio.
+
+    This file is part of ChibiOS/RT.
+
+    ChibiOS/RT is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 3 of the License, or
+    (at your option) any later version.
+
+    ChibiOS/RT is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file STM32/i2c_lld.h
+ * @brief STM32 I2C subsystem low level driver header.
+ * @addtogroup I2C
+ * @{
+ */
+
+#ifndef _I2C_LLD_H_
+#define _I2C_LLD_H_
+
+#if HAL_USE_I2C || defined(__DOXYGEN__)
+
+/*===========================================================================*/
+/* Driver constants.                                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver pre-compile time settings.                                         */
+/*===========================================================================*/
+
+/**
+ * @name    Configuration options
+ * @{
+ */
+/**
+ * @brief Switch between callback based and synchronouse driver.
+ * @note  The default is synchronouse.
+ */
+#if !defined(I2C_SUPPORTS_CALLBACKS) || defined(__DOXYGEN__)
+#define I2C_SUPPORTS_CALLBACKS              TRUE
+#endif
+
+/**
+ * @brief I2C1 driver synchronization choice between GPT and polling.
+ * @note The default is polling wait.
+ */
+#if !defined(STM32_I2C_I2C1_USE_GPT_TIM)       || \
+    !defined(STM32_I2C_I2C1_USE_POLLING_WAIT)  || \
+    defined(__DOXYGEN__)
+#define STM32_I2C_I2C1_USE_POLLING_WAIT     TRUE
+#endif
+
+/**
+ * @brief I2C2 driver synchronization choice between GPT and polling.
+ * @note The default is polling wait.
+ */
+#if !defined(STM32_I2C_I2C2_USE_GPT_TIM)       || \
+    !defined(STM32_I2C_I2C2_USE_POLLING_WAIT)  || \
+    defined(__DOXYGEN__)
+#define STM32_I2C_I2C2_USE_POLLING_WAIT     TRUE
+#endif
+
+/**
+ * @brief I2C1 driver enable switch.
+ * @details If set to @p TRUE the support for I2C1 is included.
+ * @note The default is @p TRUE.
+ */
+#if !defined(STM32_I2C_USE_I2C1) || defined(__DOXYGEN__)
+#define STM32_I2C_USE_I2C1              TRUE
+#endif
+
+/**
+ * @brief I2C2 driver enable switch.
+ * @details If set to @p TRUE the support for I2C2 is included.
+ * @note The default is @p TRUE.
+ */
+#if !defined(STM32_I2C_USE_I2C2) || defined(__DOXYGEN__)
+#define STM32_I2C_USE_I2C2              TRUE
+#endif
+
+/**
+ * @brief I2C1 interrupt priority level setting.
+ * @note @p BASEPRI_KERNEL >= @p STM32_I2C_I2C1_IRQ_PRIORITY > @p PRIORITY_PENDSV.
+ */
+#if !defined(STM32_I2C_I2C1_IRQ_PRIORITY) || defined(__DOXYGEN__)
+#define STM32_I2C_I2C1_IRQ_PRIORITY     0xA0
+#endif
+
+/**
+ * @brief I2C2 interrupt priority level setting.
+ * @note @p BASEPRI_KERNEL >= @p STM32_I2C_I2C2_IRQ_PRIORITY > @p PRIORITY_PENDSV.
+ */
+#if !defined(STM32_I2C_I2C2_IRQ_PRIORITY) || defined(__DOXYGEN__)
+#define STM32_I2C_I2C2_IRQ_PRIORITY     0xA0
+#endif
+/** @} */
+
+/*===========================================================================*/
+/* Derived constants and error checks.                                       */
+/*===========================================================================*/
+
+/** @brief  EV5 */
+#define I2C_EV5_MASTER_MODE_SELECT          ((uint32_t)(((I2C_SR2_MSL|I2C_SR2_BUSY)<< 16)|I2C_SR1_SB))  /* BUSY, MSL and SB flag */
+/** @brief  EV6 */
+#define I2C_EV6_MASTER_TRA_MODE_SELECTED    ((uint32_t)(((I2C_SR2_MSL|I2C_SR2_BUSY|I2C_SR2_TRA)<< 16)|I2C_SR1_ADDR|I2C_SR1_TXE))  /* BUSY, MSL, ADDR, TXE and TRA flags */
+#define I2C_EV6_MASTER_REC_MODE_SELECTED    ((uint32_t)(((I2C_SR2_MSL|I2C_SR2_BUSY)<< 16)|I2C_SR1_ADDR))  /* BUSY, MSL and ADDR flags */
+/** @brief  EV7 */
+#define I2C_EV7_MASTER_REC_BYTE_RECEIVED    ((uint32_t)(((I2C_SR2_MSL|I2C_SR2_BUSY)<< 16)|I2C_SR1_RXNE))             /* BUSY, MSL and RXNE flags */
+#define I2C_EV7_MASTER_REC_BYTE_QUEUED      ((uint32_t)(((I2C_SR2_MSL|I2C_SR2_BUSY)<< 16)|I2C_SR1_BTF|I2C_SR1_RXNE)) /* BUSY, MSL, RXNE and BTF flags*/
+/** @brief  EV8 */
+#define I2C_EV8_MASTER_BYTE_TRANSMITTING    ((uint32_t)(((I2C_SR2_MSL|I2C_SR2_BUSY|I2C_SR2_TRA)<< 16)|I2C_SR1_TXE))   /* TRA, BUSY, MSL, TXE flags */
+/** @brief  EV8_2 */
+#define I2C_EV8_2_MASTER_BYTE_TRANSMITTED   ((uint32_t)(((I2C_SR2_MSL|I2C_SR2_BUSY|I2C_SR2_TRA)<< 16)|I2C_SR1_BTF|I2C_SR1_TXE))  /* TRA, BUSY, MSL, TXE and BTF flags */
+/** @brief  EV9 */
+#define I2C_EV9_MASTER_ADDR_10BIT           ((uint32_t)(((I2C_SR2_MSL|I2C_SR2_BUSY)<< 16)|I2C_SR1_ADD10))  /* BUSY, MSL and ADD10 flags */
+#define I2C_EV_MASK                         0x00FFFFFF  /* First byte zeroed because there is no need of PEC register part from SR2 */
+
+#define I2C_FLG_1BTR            0x01 /* Single byte to be received and processed */
+#define I2C_FLG_2BTR            0x02 /* Two bytes to be received and processed */
+#define I2C_FLG_3BTR            0x04 /* Last three received bytes to be processed */
+#define I2C_FLG_MASTER_RECEIVER 0x10
+#define I2C_FLG_HEADER_SENT     0x80
+#define I2C_FLG_TIMER_ARMED     0x40 /* Used to check locks on the bus */
+
+#define EV6_SUBEV_MASK  (I2C_FLG_1BTR|I2C_FLG_2BTR|I2C_FLG_MASTER_RECEIVER)
+#define EV7_SUBEV_MASK  (I2C_FLG_2BTR|I2C_FLG_3BTR|I2C_FLG_MASTER_RECEIVER)
+
+#define I2C_EV6_1_MASTER_REC_2BTR_MODE_SELECTED (I2C_FLG_2BTR|I2C_FLG_MASTER_RECEIVER)
+#define I2C_EV6_3_MASTER_REC_1BTR_MODE_SELECTED (I2C_FLG_1BTR|I2C_FLG_MASTER_RECEIVER)
+#define I2C_EV7_2_MASTER_REC_3BYTES_TO_PROCESS  (I2C_FLG_3BTR|I2C_FLG_MASTER_RECEIVER)
+#define I2C_EV7_3_MASTER_REC_2BYTES_TO_PROCESS  (I2C_FLG_2BTR|I2C_FLG_MASTER_RECEIVER)
+
+/*===========================================================================*/
+/* Driver data structures and types.                                         */
+/*===========================================================================*/
+
+/**
+ * @brief   I2C Driver condition flags type.
+ */
+typedef uint32_t i2cflags_t;
+
+typedef enum {
+  OPMODE_I2C = 1,
+  OPMODE_SMBUS_DEVICE = 2,
+  OPMODE_SMBUS_HOST = 3,
+} i2copmode_t;
+
+typedef enum {
+  STD_DUTY_CYCLE = 1,
+  FAST_DUTY_CYCLE_2 = 2,
+  FAST_DUTY_CYCLE_16_9 = 3,
+} i2cdutycycle_t;
+
+/**
+ * @brief Driver configuration structure.
+ */
+typedef struct {
+  i2copmode_t     op_mode;       /**< @brief Specifies the I2C mode.*/
+  uint32_t        clock_speed;   /**< @brief Specifies the clock frequency. Must be set to a value lower than 400kHz */
+  i2cdutycycle_t  duty_cycle;    /**< @brief Specifies the I2C fast mode duty cycle */
+  uint8_t         own_addr_7;    /**< @brief Specifies the first device 7-bit own address. */
+  uint16_t        own_addr_10;   /**< @brief Specifies the second part of device own address in 10-bit mode. Set to NULL if not used. */
+  uint8_t         nbit_own_addr; /**< @brief Specifies if 7-bit or 10-bit address is acknowledged */
+} I2CConfig;
+
+
+/**
+ * @brief   Type of a structure representing an I2C driver.
+ */
+typedef struct I2CDriver I2CDriver;
+
+/**
+ * @brief   Type of a structure representing an I2C slave config.
+ */
+typedef struct I2CSlaveConfig I2CSlaveConfig;
+
+/**
+ * @brief Structure representing an I2C driver.
+ */
+struct I2CDriver{
+  /**
+   * @brief Driver state.
+   */
+  i2cstate_t            id_state;
+
+#if I2C_USE_WAIT
+  /**
+   * @brief Thread waiting for I/O completion.
+   */
+  Thread                *id_thread;
+#endif /* I2C_USE_WAIT */
+#if I2C_USE_MUTUAL_EXCLUSION || defined(__DOXYGEN__)
+#if CH_USE_MUTEXES || defined(__DOXYGEN__)
+  /**
+   * @brief Mutex protecting the bus.
+   */
+  Mutex                 id_mutex;
+#elif CH_USE_SEMAPHORES
+  Semaphore             id_semaphore;
+#endif
+#endif /* I2C_USE_MUTUAL_EXCLUSION */
+
+  /**
+   * @brief Current configuration data.
+   */
+  const I2CConfig       *id_config;
+  /**
+   * @brief Current slave configuration data.
+   */
+  const I2CSlaveConfig  *id_slave_config;
+
+  __IO size_t           txbytes;    /*!< @brief Number of bytes to be transmitted. */
+  __IO size_t           rxbytes;    /*!< @brief Number of bytes to be received. */
+  uint8_t               *rxbuf;     /*!< @brief Pointer to receive buffer. */
+  uint8_t               *txbuf;     /*!< @brief Pointer to transmit buffer.*/
+  uint8_t               *rxbuff_p;  /*!< @brief Pointer to the current byte in slave rx buffer. */
+  uint8_t               *txbuff_p;  /*!< @brief Pointer to the current byte in slave tx buffer. */
+
+  __IO i2cflags_t       errors;     /*!< @brief Error flags.*/
+  __IO i2cflags_t       flags;      /*!< @brief State flags.*/
+
+  uint16_t              slave_addr; /*!< @brief Current slave address. */
+  uint8_t               slave_addr1;/*!< @brief 7-bit address of the slave with r\w bit.*/
+  uint8_t               slave_addr2;/*!< @brief Uses in 10-bit address mode. */
+
+#if CH_USE_EVENTS
+  EventSource           sevent;     /*!< @brief Status Change @p EventSource.*/
+#endif
+
+  /*********** End of the mandatory fields. **********************************/
+
+  /**
+   * @brief Pointer to the I2Cx registers block.
+   */
+  I2C_TypeDef           *id_i2c;
+
+#if !(STM32_I2C_I2C1_USE_POLLING_WAIT)
+  /* TODO: capability to switch this GPT fields off */
+  /**
+   * @brief Timer for waiting STOP condition on the bus.
+   * @details This is workaround for STM32 buggy I2C cell.
+   */
+  GPTDriver             *timer;
+
+  /**
+   * @brief Config for workaround timer.
+   */
+  const GPTConfig       *timer_cfg;
+#endif /* !(STM32_I2C_I2C1_USE_POLLING_WAIT) */
+};
+
+
+/*===========================================================================*/
+/* Driver macros.                                                            */
+/*===========================================================================*/
+
+#define i2c_lld_bus_is_busy(i2cp)                                           \
+  (i2cp->id_i2c->SR2 & I2C_SR2_BUSY)
+
+
+/* Wait until BUSY flag is reset: a STOP has been generated on the bus
+ * signaling the end of transmission. Normally this wait function
+ * does not block thread, only if slave not response it does.
+ */
+#define i2c_lld_wait_bus_free(i2cp) {                                       \
+  uint32_t tmo = 0xfffff;                                                   \
+  while((i2cp->id_i2c->SR2 & I2C_SR2_BUSY) && tmo--)                        \
+    ;                                                                       \
+}
+
+/*===========================================================================*/
+/* External declarations.                                                    */
+/*===========================================================================*/
+
+/** @cond never*/
+#if STM32_I2C_USE_I2C1
+extern I2CDriver I2CD1;
+#endif
+
+#if STM32_I2C_USE_I2C2
+extern I2CDriver I2CD2;
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void i2c_lld_init(void);
+void i2c_lld_reset(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_start(I2CDriver *i2cp);
+void i2c_lld_stop(I2CDriver *i2cp);
+void i2c_lld_master_transmit(I2CDriver *i2cp, uint16_t slave_addr,
+    uint8_t *txbuf, size_t txbytes, uint8_t *rxbuf, size_t rxbytes);
+void i2c_lld_master_receive(I2CDriver *i2cp, uint16_t slave_addr,
+    uint8_t *rxbuf, size_t rxbytes);
+void i2c_lld_master_transceive(I2CDriver *i2cp);
+
+#ifdef __cplusplus
+}
+#endif
+/** @endcond*/
+
+#endif /* CH_HAL_USE_I2C  */
+
+#endif /* _I2C_LLD_H_ */
diff --git a/os/hal/platforms/STM32/i2c_lld.c b/os/hal/platforms/STM32/i2c_lld.c
index c5cd2308f..1345c847b 100644
--- a/os/hal/platforms/STM32/i2c_lld.c
+++ b/os/hal/platforms/STM32/i2c_lld.c
@@ -35,7 +35,7 @@
 /* Datasheet notes.                                                          */
 /*===========================================================================*/
 /**
- * From RM0008.pdf
+ * From reference manuals from ST:
  *
  * Note:
  * When the STOP, START or PEC bit is set, the software must NOT perform
@@ -43,12 +43,57 @@
  * Otherwise there is a  risk of setting a second STOP, START or PEC request.
  */
 
+/*===========================================================================*/
+/* Knowledge base.                                                            */
+/*===========================================================================*/
+/*
+Not all system functions are usable in a given context.
+
+The most restrictive type are the i-class, an I-class function is a function that must:
+- Not access the "current" thread in any way (from an ISR the current thread
+is random so it is meaningless).
+- Not reschedule internally (from an ISR the reschedule is done at the end of
+the ISR chain, rescheduling from within an ISR is forbidden because would
+leave the IRQ stack not empty with all kind of funny consequences.
+- Not try to change state for the current thread.
+- Must be invoked between a lock() and an unlock() but never lock/unlock internally.
+
+A bit less restrictive are the S-class that must simply:
+- Be invoked between a lock() and an unlock() but never lock/unlock internally.
+S-class can reschedule internally, access the current thread implicitly and
+also change state so are not eligible for ISR context.
+
+Normal functions can be invoked from thread context only but have no internal
+restrictions.
+*/
+
+/*===========================================================================*/
+/* Driver local definitions.                                                 */
+/*===========================================================================*/
+#define I2C1_RX_DMA_CHANNEL                                                 \
+  STM32_DMA_GETCHANNEL(STM32_I2C_I2C1_RX_DMA_STREAM,                        \
+                       STM32_I2C1_RX_DMA_CHN)
+#define I2C1_TX_DMA_CHANNEL                                                 \
+  STM32_DMA_GETCHANNEL(STM32_I2C_I2C1_TX_DMA_STREAM,                        \
+                       STM32_I2C1_TX_DMA_CHN)
+
+#define I2C2_RX_DMA_CHANNEL                                                 \
+  STM32_DMA_GETCHANNEL(STM32_I2C_I2C2_RX_DMA_STREAM,                        \
+                       STM32_I2C2_RX_DMA_CHN)
+#define I2C2_TX_DMA_CHANNEL                                                 \
+  STM32_DMA_GETCHANNEL(STM32_I2C_I2C2_TX_DMA_STREAM,                        \
+                       STM32_I2C2_TX_DMA_CHN)
+
+#define I2C3_RX_DMA_CHANNEL                                                 \
+  STM32_DMA_GETCHANNEL(STM32_I2C_I2C3_RX_DMA_STREAM,                        \
+                       STM32_I2C3_RX_DMA_CHN)
+#define I2C3_TX_DMA_CHANNEL                                                 \
+  STM32_DMA_GETCHANNEL(STM32_I2C_I2C3_TX_DMA_STREAM,                        \
+                       STM32_I2C3_TX_DMA_CHN)
+
 /*===========================================================================*/
 /* Driver constants.                                                         */
 /*===========================================================================*/
-#define I2C_STOP_GPT_TIMEOUT  50     /* waiting timer value */
-#define I2C_START_GPT_TIMEOUT 50     /* waiting timer value */
-#define I2C_POLLING_TIMEOUT   0xFFFF
 
 /*===========================================================================*/
 /* Driver exported variables.                                                */
@@ -64,10 +109,14 @@ I2CDriver I2CD1;
 I2CDriver I2CD2;
 #endif
 
+/** @brief I2C2 driver identifier.*/
+#if STM32_I2C_USE_I2C3 || defined(__DOXYGEN__)
+I2CDriver I2CD3;
+#endif
+
 /*===========================================================================*/
 /* Driver local variables.                                                   */
 /*===========================================================================*/
-
 /* Debugging variables */
 #if CH_DBG_ENABLE_ASSERTS
 static volatile uint16_t dbgSR1 = 0;
@@ -76,108 +125,13 @@ static volatile uint16_t dbgCR1 = 0;
 static volatile uint16_t dbgCR2 = 0;
 #endif /* CH_DBG_ENABLE_ASSERTS */
 
-/* defines for convenience purpose */
-#if I2C_SUPPORTS_CALLBACKS
-#define txBuffp (i2cp->txbuff_p)
-#define rxBuffp (i2cp->rxbuff_p)
-#endif /* I2C_SUPPORTS_CALLBACKS */
-
 /*===========================================================================*/
 /* Driver local functions.                                                   */
 /*===========================================================================*/
-#if I2C_SUPPORTS_CALLBACKS
-#if (!(STM32_I2C_I2C1_USE_POLLING_WAIT)) && STM32_I2C_USE_I2C1
-/* I2C1 GPT callback. */
-static void i2c1gptcb(GPTDriver *gptp) {
-  (void)gptp;
-  I2CDriver *i2cp = &I2CD1;
 
-  chSysLockFromIsr();
-  i2cp->flags &= ~I2C_FLG_TIMER_ARMED;
-
-  switch(i2cp->id_state){
-  case I2C_ACTIVE_TRANSMIT:
-    i2c_lld_master_transmit(i2cp, i2cp->slave_addr, i2cp->txbuf, i2cp->txbytes, i2cp->rxbuf, i2cp->rxbytes);
-  break;
-
-  case I2C_ACTIVE_RECEIVE:
-    i2c_lld_master_receive(i2cp, i2cp->slave_addr, i2cp->rxbuf, i2cp->rxbytes);
-  break;
-
-  case I2C_ACTIVE_TRANSCEIVE:
-    i2c_lld_master_transceive(i2cp);
-  break;
-
-  default:
-  break;
-  }
-  chSysUnlockFromIsr();
-}
-/* I2C1 GPT configuration. */
-static const GPTConfig i2c1gptcfg = {
-  1000000,  /* 1MHz timer clock.*/
-  i2c1gptcb    /* Timer callback.*/
-};
-#endif /* STM32_I2C_I2C1_USE_POLLING_WAIT */
-
-#if (!(STM32_I2C_I2C2_USE_POLLING_WAIT)) && STM32_I2C_USE_I2C2
-/* I2C2 GPT callback. */
-static void i2c2gptcb(GPTDriver *gptp) {
-  (void)gptp;
-  I2CDriver *i2cp = &I2CD2;
-
-  chSysLockFromIsr();
-  i2cp->flags &= ~I2C_FLG_TIMER_ARMED;
-
-  switch(i2cp->id_state){
-  case I2C_ACTIVE_TRANSMIT:
-    i2c_lld_master_transmit(i2cp, i2cp->slave_addr, i2cp->txbuf, i2cp->txbytes, i2cp->rxbuf, i2cp->rxbytes);
-  break;
-
-  case I2C_ACTIVE_RECEIVE:
-    i2c_lld_master_receive(i2cp, i2cp->slave_addr, i2cp->rxbuf, i2cp->rxbytes);
-  break;
-
-  case I2C_ACTIVE_TRANSCEIVE:
-    i2c_lld_master_transceive(i2cp);
-  break;
-
-  default:
-  break;
-  }
-  chSysUnlockFromIsr();
-}
-/* I2C2 GPT configuration. */
-static const GPTConfig i2c2gptcfg = {
-  1000000,  /* 1MHz timer clock.*/
-  i2c2gptcb    /* Timer callback.*/
-};
-#endif /* STM32_I2C_I2C2_USE_POLLING_WAIT */
-#endif /* I2C_SUPPORTS_CALLBACKS */
-
-/**
- * @brief   Function for I2C debugging purpose.
- * @note    Internal use only.
- *
- * @param[in] i2cp      pointer to the @p I2CDriver object
- *
- * @notapi
- */
-#if CH_DBG_ENABLE_ASSERTS
-void _i2c_unhandled_case(I2CDriver *i2cp){
-  dbgCR1 = i2cp->id_i2c->CR1;
-  dbgCR2 = i2cp->id_i2c->CR2;
-  chDbgAssert((dbgSR1 + dbgSR2) == 0,
-        "i2c_serve_event_interrupt(), #1",
-        "unhandled case");
-}
-#else
-#define _i2c_unhandled_case(i2cp)
-#endif /* CH_DBG_ENABLE_ASSERTS */
-
-#if I2C_SUPPORTS_CALLBACKS
 /**
  * @brief   Return the last event value from I2C status registers.
+ * @details Important but implicit function is clearing interrpts flags.
  * @note    Internal use only.
  *
  * @param[in] i2cp      pointer to the @p I2CDriver object
@@ -195,253 +149,129 @@ static uint32_t i2c_get_event(I2CDriver *i2cp){
   return (I2C_EV_MASK & (regSR1 | (regSR2 << 16)));
 }
 
-/**
- * @brief   Handle the flags/interrupts.
- * @note    Internal use only.
- *
- * @param[in] i2cp      pointer to the @p I2CDriver object
- *
- * @notapi
- */
-void _i2c_ev6_master_rec_mode_selected(I2CDriver *i2cp){
-  I2C_TypeDef *dp = i2cp->id_i2c;
-
-  switch(i2cp->flags & EV6_SUBEV_MASK) {
-
-  case I2C_EV6_3_MASTER_REC_1BTR_MODE_SELECTED: /* only an single byte to receive */
-    dp->CR1 &= (uint16_t)~I2C_CR1_ACK;          /* Clear ACK */
-    dp->CR1 |= I2C_CR1_STOP;                    /* Program the STOP */
-    break;
-
-  case I2C_EV6_1_MASTER_REC_2BTR_MODE_SELECTED: /* only two bytes to receive */
-    dp->CR1 &= (uint16_t)~I2C_CR1_ACK;          /* Clear ACK */
-    dp->CR2 &= (uint16_t)~I2C_CR2_ITBUFEN;      /* Disable the ITBUF in order to have only the BTF interrupt */
-    break;
-
-  default: /* more than 2 bytes to receive */
-    break;
-  }
-}
 
 /**
- * @brief   Handle cases of 2 or 3 bytes receiving.
- * @note    Internal use only.
+ * @brief I2C interrupts handler.
  *
  * @param[in] i2cp      pointer to the @p I2CDriver object
- *
- * @notapi
- */
-void _i2c_ev7_master_rec_byte_qued(I2CDriver *i2cp){
-  I2C_TypeDef *dp = i2cp->id_i2c;
-
-  switch(i2cp->flags & EV7_SUBEV_MASK) {
-
-  case I2C_EV7_2_MASTER_REC_3BYTES_TO_PROCESS:
-    /* Only for case of three bytes to be received.
-     * DataN-2 and DataN-1 already received. */
-    dp->CR1 &= (uint16_t)~I2C_CR1_ACK;  /* Clear ACK */
-    *rxBuffp = dp->DR;                  /* Read the DataN-2. This clear the RXE & BFT flags and launch the DataN exception in the shift register (ending the SCL stretch) */
-    rxBuffp++;
-    chSysLockFromIsr();
-    dp->CR1 |= I2C_CR1_STOP;            /* Program the STOP */
-    *rxBuffp = dp->DR;                  /* Read the DataN-1 */
-    chSysUnlockFromIsr();
-    rxBuffp++;
-    i2cp->rxbytes -= 2;                 /* Decrement the number of readed bytes */
-    i2cp->flags = 0;
-    dp->CR2 |= I2C_CR2_ITBUFEN;         /* ready for read DataN. Enable interrupt for next (and last) RxNE event*/
-    break;
-
-  case I2C_EV7_3_MASTER_REC_2BYTES_TO_PROCESS:
-    /* only for case of two bytes to be received
-     * DataN-1 and DataN are received */
-    dp->CR2 &= (uint16_t)~I2C_CR2_ITEVTEN;
-    dp->CR2 &= (uint16_t)~I2C_CR2_ITBUFEN;
-    chSysLockFromIsr();
-    dp->CR1 |= I2C_CR1_STOP;                    /* Program the STOP */
-    *rxBuffp = dp->DR;                          /* Read the DataN-1*/
-    rxBuffp++;
-    *rxBuffp = dp->DR;                          /* Read the DataN*/
-    chSysUnlockFromIsr();
-    i2cp->rxbytes = 0;
-    i2cp->flags = 0;
-    _i2c_isr_code(i2cp, i2cp->id_slave_config); /* Portable I2C ISR code defined in the high level driver. */
-    break;
-
-  case I2C_FLG_MASTER_RECEIVER:
-    /* Some times in hi load scenarions it is possible to "miss" interrupt
-     * because STM32 I2C has OR'ed interrupt sources. This case handle that
-     * scenario. */
-    if (i2cp->rxbytes > 3){
-      *rxBuffp = dp->DR;
-      rxBuffp++;
-      (i2cp->rxbytes)--;
-    }
-    else{
-      _i2c_unhandled_case(i2cp);
-    }
-    break;
-
-  default:
-    _i2c_unhandled_case(i2cp);
-    break;
-  }
-}
-
-/**
- * @brief   Main I2C interrupt handler.
- * @note    Internal use only.
- *
- * @param[in] i2cp      pointer to the @p I2CDriver object
- *
- * @notapi
  */
 static void i2c_serve_event_interrupt(I2CDriver *i2cp) {
   I2C_TypeDef *dp = i2cp->id_i2c;
 
-  switch(i2c_get_event(i2cp)) {
-
+  switch(i2c_get_event(i2cp)){
   case I2C_EV5_MASTER_MODE_SELECT:
+    /* catch start generated event */
     i2cp->flags &= ~I2C_FLG_HEADER_SENT;
-    dp->DR = i2cp->slave_addr1;
+    dp->DR = i2cp->slave_addr;
     break;
 
-  case I2C_EV9_MASTER_ADDR_10BIT:
-    if(i2cp->flags & I2C_FLG_MASTER_RECEIVER) {
-      i2cp->slave_addr1 |= 0x01;
-      i2cp->flags |= I2C_FLG_HEADER_SENT;
-    }
-    dp->DR = i2cp->slave_addr2;
+  case I2C_EV6_MASTER_REC_MODE_SELECTED:
+    dmaStreamEnable(i2cp->dmarx);
+    i2cp->id_i2c->CR2 |= I2C_CR2_DMAEN | I2C_CR2_LAST;
     break;
 
-  /**************************************************************************
-   * Master Transmitter part
-   */
   case I2C_EV6_MASTER_TRA_MODE_SELECTED:
-    if(i2cp->flags & I2C_FLG_HEADER_SENT){
-      dp->CR1 |= I2C_CR1_START;             /* re-send the start in 10-Bit address mode */
-      break;
-    }
-    txBuffp = (uint8_t*)i2cp->txbuf;        /* Initialize the transmit buffer pointer */
-    i2cp->txbytes--;
-    if(i2cp->txbytes == 0) {                /* If no further data to be sent, disable the I2C ITBUF in order to not have a TxE interrupt */
-      dp->CR2 &= (uint16_t)~I2C_CR2_ITBUFEN;
-    }
-    dp->DR = *txBuffp;                      /* EV8_1 write the first data */
-    txBuffp++;
-    break;
-
-  case I2C_EV8_MASTER_BYTE_TRANSMITTING:
-    if(i2cp->txbytes > 0) {
-      i2cp->txbytes--;
-      if(i2cp->txbytes == 0) {              /* If no further data to be sent, disable the ITBUF in order to not have a TxE interrupt */
-        dp->CR2 &= (uint16_t)~I2C_CR2_ITBUFEN;
-      }
-      dp->DR = *txBuffp;
-      txBuffp++;
-    }
+    dmaStreamEnable(i2cp->dmatx);
+    i2cp->id_i2c->CR2 |= I2C_CR2_DMAEN | I2C_CR2_LAST;
     break;
 
   case I2C_EV8_2_MASTER_BYTE_TRANSMITTED:
-    dp->CR2 &= (uint16_t)~I2C_CR2_ITEVTEN;        /* Disable ITEVT In order to not have again a BTF IT */
-    if (i2cp->rxbytes == 0){                      /* if nothing to read then generate stop */
-      dp->CR1 |= I2C_CR1_STOP;
-      _i2c_isr_code(i2cp, i2cp->id_slave_config); /* Portable I2C ISR code defined in the high level driver. */
-    }
-    else{                                         /* start reading operation */
-      i2cp->id_i2c->CR1 |= I2C_CR1_START;         /* send start bit */
-      i2c_lld_master_transceive(i2cp);
+    /* catch BTF event after the end of trasmission */
+    if (i2cp->rxbytes > 1){
+      /* start "read after write" operation */
+      i2c_lld_master_receive(i2cp, (i2cp->slave_addr >> 1),
+                            i2cp->rxbuf, i2cp->rxbytes);
+      return;
     }
+    else
+      i2cp->id_i2c->CR1 |= I2C_CR1_STOP;
+      _i2c_isr_code(i2cp, i2cp->id_slave_config);
     break;
 
-  /**************************************************************************
-   * Master Receiver part
-   */
-  case I2C_EV6_MASTER_REC_MODE_SELECTED:
-    _i2c_ev6_master_rec_mode_selected(i2cp);
-    rxBuffp = i2cp->rxbuf;                        /* Initialize receive buffer pointer */
-    break;
-
-  case I2C_EV7_MASTER_REC_BYTE_RECEIVED:
-    if(i2cp->rxbytes > 3) {
-      *rxBuffp = dp->DR;                          /* Read the data register */
-      rxBuffp++;
-      i2cp->rxbytes--;
-      if(i2cp->rxbytes == 3){                     /* Disable the ITBUF in order to have only the BTF interrupt */
-        dp->CR2 &= (uint16_t)~I2C_CR2_ITBUFEN;
-        i2cp->flags |= I2C_FLG_3BTR;
-      }
-    }
-    else if (i2cp->rxbytes == 3){                 /* Disable the ITBUF in order to have only the BTF interrupt */
-      dp->CR2 &= (uint16_t)~I2C_CR2_ITBUFEN;
-      i2cp->flags |= I2C_FLG_3BTR;
-    }
-    break;
-
-  case I2C_EV7_MASTER_REC_BYTE_QUEUED:
-    _i2c_ev7_master_rec_byte_qued(i2cp);
-    break;
-
-  default:                                        /* only 1 byte must to be read to complete trasfer. Stop already sent to bus. */
-    chDbgAssert((i2cp->rxbytes) == 1,
-          "i2c_serve_event_interrupt(), #1",
-          "more than 1 byte to be received");
-    *rxBuffp = dp->DR;                            /* Read the data register */
-    i2cp->rxbytes = 0;
-    dp->CR2 &= (uint16_t)~I2C_CR2_ITEVTEN;        /* disable interrupts */
-    dp->CR2 &= (uint16_t)~I2C_CR2_ITBUFEN;
-    _i2c_isr_code(i2cp, i2cp->id_slave_config);   /* Portable I2C ISR code defined in the high level driver.*/
+  default:
     break;
   }
 }
-#endif /* I2C_SUPPORTS_CALLBACKS */
 
+
+/**
+ * @brief DMA rx end IRQ handler.
+ *
+ * @param[in] i2cp      pointer to the @p I2CDriver object
+ */
+static void i2c_lld_serve_rx_end_irq(I2CDriver *i2cp){
+  dmaStreamDisable(i2cp->dmarx);
+
+  i2cp->id_i2c->CR1 |= I2C_CR1_STOP;
+  _i2c_isr_code(i2cp, i2cp->id_slave_config);
+}
+
+
+/**
+ * @brief DMA tx enr IRQ handler.
+ *
+ * @param[in] i2cp      pointer to the @p I2CDriver object
+ */
+static void i2c_lld_serve_tx_end_irq(I2CDriver *i2cp){
+  dmaStreamDisable(i2cp->dmatx);
+}
+
+
+/**
+ * @brief I2C error handler.
+ *
+ * @param[in] i2cp      pointer to the @p I2CDriver object
+ */
 static void i2c_serve_error_interrupt(I2CDriver *i2cp) {
-  i2cflags_t flags;
+  i2cflags_t errors;
   I2C_TypeDef *reg;
 
+  chSysLockFromIsr();
+  /* clear interrupt falgs just to be safe */
+  dmaStreamDisable(i2cp->dmatx);
+  dmaStreamDisable(i2cp->dmarx);
+  dmaStreamClearInterrupt(i2cp->dmatx);
+  dmaStreamClearInterrupt(i2cp->dmarx);
+  chSysUnlockFromIsr();
+
   reg = i2cp->id_i2c;
-  flags = I2CD_NO_ERROR;
+  errors = I2CD_NO_ERROR;
 
   if(reg->SR1 & I2C_SR1_BERR) {                /* Bus error */
     reg->SR1 &= ~I2C_SR1_BERR;
-    flags |= I2CD_BUS_ERROR;
+    errors |= I2CD_BUS_ERROR;
   }
   if(reg->SR1 & I2C_SR1_ARLO) {                /* Arbitration lost */
     reg->SR1 &= ~I2C_SR1_ARLO;
-    flags |= I2CD_ARBITRATION_LOST;
+    errors |= I2CD_ARBITRATION_LOST;
   }
   if(reg->SR1 & I2C_SR1_AF) {                  /* Acknowledge fail */
     reg->SR1 &= ~I2C_SR1_AF;
     reg->CR1 |= I2C_CR1_STOP;                  /* setting stop bit */
-    while(i2cp->id_i2c->CR1 & I2C_CR1_STOP)
-      ;
-    flags |= I2CD_ACK_FAILURE;
+    errors |= I2CD_ACK_FAILURE;
   }
   if(reg->SR1 & I2C_SR1_OVR) {                 /* Overrun */
     reg->SR1 &= ~I2C_SR1_OVR;
-    flags |= I2CD_OVERRUN;
+    errors |= I2CD_OVERRUN;
   }
   if(reg->SR1 & I2C_SR1_PECERR) {              /* PEC error */
     reg->SR1 &= ~I2C_SR1_PECERR;
-    flags |= I2CD_PEC_ERROR;
+    errors |= I2CD_PEC_ERROR;
   }
   if(reg->SR1 & I2C_SR1_TIMEOUT) {             /* SMBus Timeout */
     reg->SR1 &= ~I2C_SR1_TIMEOUT;
-    flags |= I2CD_TIMEOUT;
+    errors |= I2CD_TIMEOUT;
   }
   if(reg->SR1 & I2C_SR1_SMBALERT) {            /* SMBus alert */
     reg->SR1 &= ~I2C_SR1_SMBALERT;
-    flags |= I2CD_SMB_ALERT;
+    errors |= I2CD_SMB_ALERT;
   }
 
-  if(flags != I2CD_NO_ERROR) {                /* send communication end signal */
+  if(errors != I2CD_NO_ERROR) {                /* send communication end signal */
     chSysLockFromIsr();
-    i2cAddFlagsI(i2cp, flags);
+    i2cAddFlagsI(i2cp, errors);
     chSysUnlockFromIsr();
-  #if I2C_SUPPORTS_CALLBACKS
     _i2c_isr_err_code(i2cp, i2cp->id_slave_config);
-  #endif /* I2C_SUPPORTS_CALLBACKS */
   }
 }
 
@@ -450,19 +280,15 @@ static void i2c_serve_error_interrupt(I2CDriver *i2cp) {
 /**
  * @brief I2C1 event interrupt handler.
  */
-#if I2C_SUPPORTS_CALLBACKS
-CH_IRQ_HANDLER(VectorBC) {
-
+CH_IRQ_HANDLER(I2C1_EV_IRQHandler) {
   CH_IRQ_PROLOGUE();
   i2c_serve_event_interrupt(&I2CD1);
   CH_IRQ_EPILOGUE();
 }
-#endif /* I2C_SUPPORTS_CALLBACKS */
 /**
  * @brief I2C1 error interrupt handler.
  */
-CH_IRQ_HANDLER(VectorC0) {
-
+CH_IRQ_HANDLER(I2C1_ER_IRQHandler) {
   CH_IRQ_PROLOGUE();
   i2c_serve_error_interrupt(&I2CD1);
   CH_IRQ_EPILOGUE();
@@ -473,25 +299,41 @@ CH_IRQ_HANDLER(VectorC0) {
 /**
  * @brief I2C2 event interrupt handler.
  */
-#if I2C_SUPPORTS_CALLBACKS
-CH_IRQ_HANDLER(VectorC4) {
-
+CH_IRQ_HANDLER(I2C2_EV_IRQHandler) {
   CH_IRQ_PROLOGUE();
   i2c_serve_event_interrupt(&I2CD2);
   CH_IRQ_EPILOGUE();
 }
-#endif /* I2C_SUPPORTS_CALLBACKS */
 /**
  * @brief I2C2 error interrupt handler.
  */
-CH_IRQ_HANDLER(VectorC8) {
-
+CH_IRQ_HANDLER(I2C2_ER_IRQHandler) {
   CH_IRQ_PROLOGUE();
   i2c_serve_error_interrupt(&I2CD2);
   CH_IRQ_EPILOGUE();
 }
 #endif /* STM32_I2C_USE_I2C2 */
 
+#if STM32_I2C_USE_I2C3 || defined(__DOXYGEN__)
+/**
+ * @brief I2C3 event interrupt handler.
+ */
+CH_IRQ_HANDLER(I2C3_EV_IRQHandler) {
+  CH_IRQ_PROLOGUE();
+  i2c_serve_event_interrupt(&I2CD3);
+  CH_IRQ_EPILOGUE();
+}
+/**
+ * @brief I2C3 error interrupt handler.
+ */
+CH_IRQ_HANDLER(I2C3_ER_IRQHandler) {
+  CH_IRQ_PROLOGUE();
+  i2c_serve_error_interrupt(&I2CD3);
+  CH_IRQ_EPILOGUE();
+}
+#endif /* STM32_I2C_USE_I2C3 */
+
+
 /**
  * @brief Low level I2C driver initialization.
  */
@@ -499,29 +341,24 @@ void i2c_lld_init(void) {
 
 #if STM32_I2C_USE_I2C1
   i2cObjectInit(&I2CD1);
-  I2CD1.id_i2c      = I2C1;
-
-#if I2C_SUPPORTS_CALLBACKS
-#if !(STM32_I2C_I2C1_USE_POLLING_WAIT)
-  I2CD1.timer       = &(STM32_I2C_I2C1_USE_GPT_TIM);
-  I2CD1.timer_cfg   = &i2c1gptcfg;
-#endif /* !(STM32_I2C_I2C1_USE_POLLING_WAIT) */
-#endif /* I2C_SUPPORTS_CALLBACKS */
-
-#endif /* STM32_I2C_USE_I2C */
+  I2CD1.id_i2c = I2C1;
+  I2CD1.dmarx  = STM32_DMA_STREAM(STM32_I2C_I2C1_RX_DMA_STREAM);
+  I2CD1.dmatx  = STM32_DMA_STREAM(STM32_I2C_I2C1_TX_DMA_STREAM);
+#endif /* STM32_I2C_USE_I2C1 */
 
 #if STM32_I2C_USE_I2C2
   i2cObjectInit(&I2CD2);
-  I2CD2.id_i2c      = I2C2;
-
-#if I2C_SUPPORTS_CALLBACKS
-#if !(STM32_I2C_I2C2_USE_POLLING_WAIT)
-  I2CD2.timer       = &(STM32_I2C_I2C2_USE_GPT_TIM);
-  I2CD2.timer_cfg   = &i2c2gptcfg;
-#endif /* !(STM32_I2C_I2C2_USE_POLLING_WAIT) */
-#endif /* I2C_SUPPORTS_CALLBACKS */
-
+  I2CD2.id_i2c = I2C2;
+  I2CD2.dmarx  = STM32_DMA_STREAM(STM32_I2C_I2C2_RX_DMA_STREAM);
+  I2CD2.dmatx  = STM32_DMA_STREAM(STM32_I2C_I2C2_TX_DMA_STREAM);
 #endif /* STM32_I2C_USE_I2C2 */
+
+#if STM32_I2C_USE_I2C3
+  i2cObjectInit(&I2CD3);
+  I2CD3.id_i2c = I2C3;
+  I2CD3.dmarx  = STM32_DMA_STREAM(STM32_I2C_I2C3_RX_DMA_STREAM);
+  I2CD3.dmatx  = STM32_DMA_STREAM(STM32_I2C_I2C3_TX_DMA_STREAM);
+#endif /* STM32_I2C_USE_I2C3 */
 }
 
 /**
@@ -530,43 +367,204 @@ void i2c_lld_init(void) {
  * @param[in] i2cp      pointer to the @p I2CDriver object
  */
 void i2c_lld_start(I2CDriver *i2cp) {
+
+  i2cp->dmamode = STM32_DMA_CR_DMEIE | STM32_DMA_CR_TEIE;
+
   if (i2cp->id_state == I2C_STOP) {         /* If in stopped state then enables the I2C clock.*/
 #if STM32_I2C_USE_I2C1
     if (&I2CD1 == i2cp) {
-#if I2C_SUPPORTS_CALLBACKS
+
+      bool_t b;
+      b = dmaStreamAllocate(i2cp->dmarx,
+                            STM32_I2C_I2C1_IRQ_PRIORITY,
+                            (stm32_dmaisr_t)i2c_lld_serve_rx_end_irq,
+                            (void *)i2cp);
+      chDbgAssert(!b, "uart_lld_start(), #3", "stream already allocated");
+      b = dmaStreamAllocate(i2cp->dmatx,
+                            STM32_I2C_I2C1_IRQ_PRIORITY,
+                            (stm32_dmaisr_t)i2c_lld_serve_tx_end_irq,
+                            (void *)i2cp);
+      chDbgAssert(!b, "uart_lld_start(), #4", "stream already allocated");
+      rccEnableI2C1(FALSE);
       NVICEnableVector(I2C1_EV_IRQn,
           CORTEX_PRIORITY_MASK(STM32_I2C_I2C1_IRQ_PRIORITY));
-#endif /* I2C_SUPPORTS_CALLBACKS */
       NVICEnableVector(I2C1_ER_IRQn,
           CORTEX_PRIORITY_MASK(STM32_I2C_I2C1_IRQ_PRIORITY));
-      rccEnableI2C1(FALSE);
+
+      i2cp->dmamode |= STM32_DMA_CR_CHSEL(I2C1_RX_DMA_CHANNEL) | \
+      	  	  	  	  	  STM32_DMA_CR_PL(STM32_I2C_I2C1_DMA_PRIORITY);
+      __NOP();
     }
-#endif
+#endif /* STM32_I2C_USE_I2C1 */
+
 #if STM32_I2C_USE_I2C2
     if (&I2CD2 == i2cp) {
-#if I2C_SUPPORTS_CALLBACKS
-      NVICEnableVector(I2C2_EV_IRQn,
-          CORTEX_PRIORITY_MASK(STM32_I2C_I2C1_IRQ_PRIORITY));
-#endif /* I2C_SUPPORTS_CALLBACKS */
-      NVICEnableVector(I2C2_ER_IRQn,
-          CORTEX_PRIORITY_MASK(STM32_I2C_I2C1_IRQ_PRIORITY));
+
+      bool_t b;
+      b = dmaStreamAllocate(i2cp->dmarx,
+                            STM32_I2C_I2C2_IRQ_PRIORITY,
+                            (stm32_dmaisr_t)i2c_lld_serve_rx_end_irq,
+                            (void *)i2cp);
+      chDbgAssert(!b, "uart_lld_start(), #3", "stream already allocated");
+      b = dmaStreamAllocate(i2cp->dmatx,
+                            STM32_I2C_I2C2_IRQ_PRIORITY,
+                            (stm32_dmaisr_t)i2c_lld_serve_tx_end_irq,
+                            (void *)i2cp);
+      chDbgAssert(!b, "uart_lld_start(), #4", "stream already allocated");
       rccEnableI2C2(FALSE);
+      NVICEnableVector(I2C2_EV_IRQn,
+          CORTEX_PRIORITY_MASK(STM32_I2C_I2C2_IRQ_PRIORITY));
+      NVICEnableVector(I2C2_ER_IRQn,
+          CORTEX_PRIORITY_MASK(STM32_I2C_I2C2_IRQ_PRIORITY));
+
+      i2cp->dmamode |= STM32_DMA_CR_CHSEL(I2C2_RX_DMA_CHANNEL) |
+                            STM32_DMA_CR_PL(STM32_I2C_I2C2_DMA_PRIORITY);
     }
-#endif
+#endif /* STM32_I2C_USE_I2C2 */
+
+#if STM32_I2C_USE_I2C3
+    if (&I2CD3 == i2cp) {
+
+      bool_t b;
+      b = dmaStreamAllocate(i2cp->dmarx,
+                            STM32_I2C_I2C3_IRQ_PRIORITY,
+                            (stm32_dmaisr_t)i2c_lld_serve_rx_end_irq,
+                            (void *)i2cp);
+      chDbgAssert(!b, "uart_lld_start(), #3", "stream already allocated");
+      b = dmaStreamAllocate(i2cp->dmatx,
+                            STM32_I2C_I2C3_IRQ_PRIORITY,
+                            (stm32_dmaisr_t)i2c_lld_serve_tx_end_irq,
+                            (void *)i2cp);
+      chDbgAssert(!b, "uart_lld_start(), #4", "stream already allocated");
+      rccEnableI2C3(FALSE);
+      NVICEnableVector(I2C3_EV_IRQn,
+          CORTEX_PRIORITY_MASK(STM32_I2C_I2C3_IRQ_PRIORITY));
+      NVICEnableVector(I2C3_ER_IRQn,
+          CORTEX_PRIORITY_MASK(STM32_I2C_I2C3_IRQ_PRIORITY));
+
+      i2cp->dmamode |= STM32_DMA_CR_CHSEL(I2C3_RX_DMA_CHANNEL) |
+                            STM32_DMA_CR_PL(STM32_I2C_I2C3_DMA_PRIORITY);
+    }
+#endif /* STM32_I2C_USE_I2C2 */
+
   }
+  i2cp->dmamode |= STM32_DMA_CR_PSIZE_BYTE |
+                   STM32_DMA_CR_MSIZE_BYTE |
+                   STM32_DMA_CR_MINC |
+                   STM32_DMA_CR_TCIE;
+  dmaStreamSetPeripheral(i2cp->dmarx, &i2cp->id_i2c->DR);
+  dmaStreamSetPeripheral(i2cp->dmatx, &i2cp->id_i2c->DR);
 
   i2cp->id_i2c->CR1 = I2C_CR1_SWRST;        /* reset i2c peripherial */
   i2cp->id_i2c->CR1 = 0;
   i2c_lld_set_clock(i2cp);
   i2c_lld_set_opmode(i2cp);
+
   i2cp->id_i2c->CR1 |= 1;                   /* enable interface */
 }
 
+
+/**
+ * @brief Reset interface via RCC.
+ */
 void i2c_lld_reset(I2CDriver *i2cp){
   chDbgCheck((i2cp->id_state == I2C_STOP)||(i2cp->id_state == I2C_READY),
              "i2c_lld_reset: invalid state");
 
-  rccResetI2C1();
+  #if STM32_I2C_USE_I2C1
+  if (&I2CD1 == i2cp)
+    rccResetI2C1();
+  #endif /* STM32_I2C_USE_I2C1 */
+
+  #if STM32_I2C_USE_I2C2
+  if (&I2CD2 == i2cp)
+    rccResetI2C2();
+  #endif /* STM32_I2C_USE_I2C2 */
+
+  #if STM32_I2C_USE_I2C3
+  if (&I2CD3 == i2cp)
+      rccResetI2C3();
+  #endif /* STM32_I2C_USE_I2C3 */
+}
+
+
+/**
+ * @brief Receive data via the I2C bus as master.
+ *
+ * @param[in] i2cp        pointer to the @p I2CDriver object
+ * @param[in] slave_addr  slave device address
+ * @param[in] rxbuf       pointer to the receive buffer
+ * @param[in] rxbytes     number of bytes to be received
+ */
+void i2c_lld_master_receive(I2CDriver *i2cp, uint8_t slave_addr,
+                            uint8_t *rxbuf, size_t rxbytes){
+
+  uint32_t mode = 0;
+
+  /* init driver fields */
+  i2cp->slave_addr = (slave_addr << 1) | 0x01;    /* LSB = 1 -> receive */
+  i2cp->rxbytes = rxbytes;
+  i2cp->rxbuf = rxbuf;
+  i2cp->flags = 0;
+
+  /* setting flags and register bits */
+  i2cp->flags |= I2C_FLG_MASTER_RECEIVER;
+  i2cp->errors = 0;
+
+  mode = STM32_DMA_CR_DIR_P2M;
+  // TODO: DMA error handling
+  dmaStreamSetMemory0(i2cp->dmarx, rxbuf);
+  dmaStreamSetTransactionSize(i2cp->dmarx, rxbytes);
+  dmaStreamSetMode(i2cp->dmarx, ((i2cp->dmamode) | mode));
+
+  /* wait stop bit from previouse transaction*/
+  while(i2cp->id_i2c->CR1 & I2C_CR1_STOP)
+    ;
+
+  i2cp->id_i2c->CR2 |= I2C_CR2_ITERREN | I2C_CR2_ITEVTEN;
+  i2cp->id_i2c->CR1 |= I2C_CR1_START | I2C_CR1_ACK;
+}
+
+
+/**
+ * @brief Transmits data via the I2C bus as master.
+ *
+ * @param[in] i2cp        pointer to the @p I2CDriver object
+ * @param[in] slave_addr  slave device address
+ * @param[in] txbuf       pointer to the transmit buffer
+ * @param[in] txbytes     number of bytes to be transmitted
+ * @param[in] rxbuf       pointer to the receive buffer
+ * @param[in] rxbytes     number of bytes to be received
+ */
+void i2c_lld_master_transmit(I2CDriver *i2cp, uint8_t slave_addr,
+                             uint8_t *txbuf, size_t txbytes,
+                             uint8_t *rxbuf, size_t rxbytes){
+
+  uint32_t mode = 0;
+
+  /* init driver fields */
+  i2cp->slave_addr = (slave_addr << 1) & 0x00FE;         /* LSB = 0 -> write */
+  i2cp->txbytes = txbytes;
+  i2cp->rxbytes = rxbytes;
+  i2cp->txbuf = txbuf;
+  i2cp->rxbuf = rxbuf;
+
+  /* setting flags and register bits */
+  i2cp->flags = 0;
+  i2cp->errors = 0;
+
+  mode = STM32_DMA_CR_DIR_M2P;
+  // TODO: DMA error handling
+  dmaStreamSetMemory0(i2cp->dmatx, txbuf);
+  dmaStreamSetTransactionSize(i2cp->dmatx, txbytes);
+  dmaStreamSetMode(i2cp->dmatx, ((i2cp->dmamode) | mode));
+
+  /* wait stop bit from previouse transaction*/
+  while(i2cp->id_i2c->CR1 & I2C_CR1_STOP)
+    ;
+
+  i2cp->id_i2c->CR2 |= I2C_CR2_ITERREN | I2C_CR2_ITEVTEN;
+  i2cp->id_i2c->CR1 |= I2C_CR1_START;
 }
 
 
@@ -590,8 +588,13 @@ void i2c_lld_set_clock(I2CDriver *i2cp) {
   regCR2 = i2cp->id_i2c->CR2;                   /* Get the I2Cx CR2 value */
   regCR2 &= (uint16_t)~I2C_CR2_FREQ;            /* Clear frequency FREQ[5:0] bits */
   freq = (uint16_t)(STM32_PCLK1 / 1000000);     /* Set frequency bits depending on pclk1 value */
+#ifdef STM32F4XX
+  chDbgCheck((freq >= 2) && (freq <= 42),
+                "i2c_lld_set_clock() : Peripheral clock freq. out of range");
+#else
   chDbgCheck((freq >= 2) && (freq <= 36),
               "i2c_lld_set_clock() : Peripheral clock freq. out of range");
+#endif /* define STM32F4XX */
   regCR2 |= freq;
   i2cp->id_i2c->CR2 = regCR2;
 
@@ -635,6 +638,7 @@ void i2c_lld_set_clock(I2CDriver *i2cp) {
   i2cp->id_i2c->CR1 |= pe_bit_saved;                            /* restore the I2C peripheral enabled state */
 }
 
+
 /**
  * @brief Set operation mode of I2C hardware.
  *
@@ -661,28 +665,6 @@ void i2c_lld_set_opmode(I2CDriver *i2cp) {
   i2cp->id_i2c->CR1 = regCR1;             /* Write to I2Cx CR1 */
 }
 
-/**
- * @brief Set own address.
- *
- * @param[in] i2cp      pointer to the @p I2CDriver object
- */
-void i2c_lld_set_own_address(I2CDriver *i2cp) {
-  /* TODO: dual address mode */
-
-  i2cp->id_i2c->OAR1 |= 1 << 14;
-
-  if (&(i2cp->id_config->own_addr_10) == NULL){       /* only 7-bit address */
-    i2cp->id_i2c->OAR1 &= (~I2C_OAR1_ADDMODE);
-    i2cp->id_i2c->OAR1 |= i2cp->id_config->own_addr_7 << 1;
-  }
-  else {
-    chDbgAssert((i2cp->id_config->own_addr_10 < 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->own_addr_10;
-  }
-}
-
 
 /**
  * @brief Deactivates the I2C peripheral.
@@ -690,7 +672,16 @@ void i2c_lld_set_own_address(I2CDriver *i2cp) {
  * @param[in] i2cp      pointer to the @p I2CDriver object
  */
 void i2c_lld_stop(I2CDriver *i2cp) {
-  if (i2cp->id_state == I2C_READY) {  /* If in ready state then disables the I2C clock.*/
+
+  if (i2cp->id_state != I2C_STOP) {  /* If in ready state then disables the I2C clock.*/
+
+  dmaStreamDisable(i2cp->dmatx);
+  dmaStreamDisable(i2cp->dmarx);
+  dmaStreamClearInterrupt(i2cp->dmatx);
+  dmaStreamClearInterrupt(i2cp->dmarx);
+  dmaStreamRelease(i2cp->dmatx);
+  dmaStreamRelease(i2cp->dmarx);
+
 #if STM32_I2C_USE_I2C1
     if (&I2CD1 == i2cp) {
       NVICDisableVector(I2C1_EV_IRQn);
@@ -698,6 +689,7 @@ void i2c_lld_stop(I2CDriver *i2cp) {
       rccDisableI2C1(FALSE);
     }
 #endif
+
 #if STM32_I2C_USE_I2C2
     if (&I2CD2 == i2cp) {
       NVICDisableVector(I2C2_EV_IRQn);
@@ -705,391 +697,18 @@ void i2c_lld_stop(I2CDriver *i2cp) {
       rccDisableI2C2(FALSE);
     }
 #endif
+
+#if STM32_I2C_USE_I2C3
+    if (&I2CD3 == i2cp) {
+      NVICDisableVector(I2C3_EV_IRQn);
+      NVICDisableVector(I2C3_ER_IRQn);
+      rccDisableI2C3(FALSE);
+    }
+#endif
   }
 
   i2cp->id_state = I2C_STOP;
 }
 
 
-#if I2C_SUPPORTS_CALLBACKS
-/**
- * @brief Transmits data via the I2C bus as master.
- *
- * @param[in] i2cp        pointer to the @p I2CDriver object
- * @param[in] slave_addr  Slave device address. Bits 0-9 contain slave
- *                        device address. Bit 15 must be set to 1 if 10-bit
- *                        addressing modes used. Otherwise	keep it cleared.
- *                        Bits 10-14 unused.
- * @param[in] txbuf       pointer to the transmit buffer
- * @param[in] txbytes     number of bytes to be transmitted
- * @param[in] rxbuf       pointer to the receive buffer
- * @param[in] rxbytes     number of bytes to be received
- */
-void i2c_lld_master_transmit(I2CDriver *i2cp, uint16_t slave_addr,
-    uint8_t *txbuf, size_t txbytes, uint8_t *rxbuf, size_t rxbytes) {
-
-  /* "waiting" for STOP bit routine*/
-  #if STM32_I2C_I2C1_USE_POLLING_WAIT
-    uint32_t timeout = I2C_POLLING_TIMEOUT;
-    while((i2cp->id_i2c->CR1 & I2C_CR1_STOP) && timeout)
-      timeout--;
-    chDbgAssert((timeout > 0), "i2c_lld_master_transmit(), #1", "time to STOP is out");
-  #else
-    chDbgAssert(!(i2cp->flags & I2C_FLG_TIMER_ARMED), "i2c_lld_master_transmit(), #1", "time to STOP is out");
-    if ((i2cp->id_i2c->CR1 & I2C_CR1_STOP) && i2cp->timer != NULL && i2cp->timer_cfg != NULL){
-      chSysLockFromIsr();
-      gptStartOneShotI(i2cp->timer, I2C_STOP_GPT_TIMEOUT);
-      i2cp->flags |= I2C_FLG_TIMER_ARMED;
-      chSysUnlockFromIsr();
-      return;
-    }
-  #endif /* STM32_I2C_I2C1_USE_POLLING_WAIT */
-
-  /* init driver fields */
-  i2cp->slave_addr = slave_addr;
-  i2cp->txbytes = txbytes;
-  i2cp->rxbytes = rxbytes;
-  i2cp->txbuf = txbuf;
-  i2cp->rxbuf = rxbuf;
-
-  /* init address fields */
-  if(slave_addr & 0x8000){                                    /* 10-bit mode used */
-    i2cp->slave_addr1 = ((slave_addr >>7) & 0x0006);          /* add the two msb of 10-bit address to the header */
-    i2cp->slave_addr1 |= 0xF0;                                /* add the header bits with LSB = 0 -> write */
-    i2cp->slave_addr2 = slave_addr & 0x00FF;                  /* the remaining 8 bit of 10-bit address */
-  }
-  else{
-    i2cp->slave_addr1 = ((slave_addr <<1) & 0x00FE);          /* LSB = 0 -> write */
-  }
-
-  /* setting flags and register bits */
-  i2cp->flags = 0;
-  i2cp->errors = 0;
-  i2cp->id_i2c->CR1 &= ~I2C_CR1_POS;
-  i2cp->id_i2c->CR1 |= I2C_CR1_START;
-  i2cp->id_i2c->CR2 |= (I2C_CR2_ITERREN|I2C_CR2_ITEVTEN|I2C_CR2_ITBUFEN); /* enable ERR, EVT & BUF ITs */
-}
-
-/**
- * @brief Receives data from the I2C bus.
- *
- * @param[in] i2cp        pointer to the @p I2CDriver object
- * @param[in] slave_addr  Slave device address. Bits 0-9 contain slave
- *                        device address. Bit 15 must be set to 1 if 10-bit
- *                        addressing modes used. Otherwise	keep it cleared.
- *                        Bits 10-14 unused.
- * @param[in] rxbuf       pointer to the receive buffer
- * @param[in] rxbytes     number of bytes to be received
- */
-void i2c_lld_master_receive(I2CDriver *i2cp, uint16_t slave_addr,
-    uint8_t *rxbuf, size_t rxbytes){
-
-  chDbgAssert((i2cp->id_i2c->SR1 + i2cp->id_i2c->SR2) == 0,
-        "i2c_lld_master_receive(), #1",
-        "some interrupt sources not clear");
-
-  /* "waiting" for STOP bit routine*/
-  #if STM32_I2C_I2C1_USE_POLLING_WAIT
-    uint32_t timeout = I2C_POLLING_TIMEOUT;
-    while((i2cp->id_i2c->CR1 & I2C_CR1_STOP) && timeout)
-      timeout--;
-    chDbgAssert((timeout > 0), "i2c_lld_master_receive(), #1", "time to STOP is out");
-  #else
-    chDbgAssert(!(i2cp->flags & I2C_FLG_TIMER_ARMED), "i2c_lld_master_receive(), #1", "time to STOP is out");
-    if ((i2cp->id_i2c->CR1 & I2C_CR1_STOP) && i2cp->timer != NULL && i2cp->timer_cfg != NULL){
-      chSysLockFromIsr();
-      gptStartOneShotI(i2cp->timer, I2C_STOP_GPT_TIMEOUT);
-      i2cp->flags |= I2C_FLG_TIMER_ARMED;
-      chSysUnlockFromIsr();
-      return;
-    }
-  #endif /* STM32_I2C_I2C1_USE_POLLING_WAIT */
-
-  /* init driver fields */
-	i2cp->slave_addr = slave_addr;
-	i2cp->rxbytes = rxbytes;
-	i2cp->rxbuf = rxbuf;
-
-	/* init address fields */
-  if(slave_addr & 0x8000){                            /* 10-bit mode used */
-    i2cp->slave_addr1 = ((slave_addr >>7) & 0x0006);  /* add the two msb of 10-bit address to the header */
-    i2cp->slave_addr1 |= 0xF0;                        /* add the header bits (the LSB -> 1 will be add to second */
-    i2cp->slave_addr2 = slave_addr & 0x00FF;          /* the remaining 8 bit of 10-bit address */
-  }
-  else{
-    i2cp->slave_addr1 = ((slave_addr <<1) | 0x01);    /* LSB = 1 -> receive */
-  }
-
-  /* setting flags and register bits */
-  i2cp->flags |= I2C_FLG_MASTER_RECEIVER;
-  i2cp->errors = 0;
-
-  i2cp->id_i2c->CR1 |= I2C_CR1_ACK;                   /* acknowledge returned */
-  i2cp->id_i2c->CR1 &= ~I2C_CR1_POS;
-
-  if(i2cp->rxbytes == 1) {                            /* Only one byte to be received */
-    i2cp->flags |= I2C_FLG_1BTR;
-  }
-  else if(i2cp->rxbytes == 2) {                       /* Only two bytes to be received */
-    i2cp->flags |= I2C_FLG_2BTR;
-    i2cp->id_i2c->CR1 |= I2C_CR1_POS;                 /* Acknowledge Position */
-  }
-
-  i2cp->id_i2c->CR1 |= I2C_CR1_START;                 /* send start bit */
-  i2cp->id_i2c->CR2 |= (I2C_CR2_ITERREN|I2C_CR2_ITEVTEN|I2C_CR2_ITBUFEN); /* enable ERR, EVT & BUF ITs */
-}
-
-
-/**
- * @brief Realize read-though-write behavior.
- *
- * @param[in] i2cp        pointer to the @p I2CDriver object
- *
- * @notapi
- */
-void i2c_lld_master_transceive(I2CDriver *i2cp){
-
-  chDbgAssert((i2cp != NULL) && (i2cp->slave_addr1 != 0) &&\
-      (i2cp->rxbytes > 0) && (i2cp->rxbuf != NULL),
-      "i2c_lld_master_transceive(), #1",
-      "");
-
-  i2cp->id_state = I2C_ACTIVE_TRANSCEIVE;
-
-  /* "waiting" for START bit routine*/
-  #if STM32_I2C_I2C1_USE_POLLING_WAIT
-    uint32_t timeout = I2C_POLLING_TIMEOUT;
-    while((i2cp->id_i2c->CR1 & I2C_CR1_START) && timeout);
-      timeout--;
-    chDbgAssert((timeout > 0), "i2c_lld_master_transceive(), #1", "time to START is out");
-  #else
-    chDbgAssert(!(i2cp->flags & I2C_FLG_TIMER_ARMED), "i2c_lld_master_transceive(), #1", "time to START is out");
-    if ((i2cp->id_i2c->CR1 & I2C_CR1_START) && i2cp->timer != NULL && i2cp->timer_cfg != NULL){
-      chSysLockFromIsr();
-      gptStartOneShotI(i2cp->timer, I2C_START_GPT_TIMEOUT);
-      i2cp->flags |= I2C_FLG_TIMER_ARMED;
-      chSysUnlockFromIsr();
-      return;
-    }
-  #endif /* STM32_I2C_I2C1_USE_POLLING_WAIT */
-
-  /* init address fields */
-  if(i2cp->slave_addr & 0x8000){                          /* 10-bit mode used */
-    i2cp->slave_addr1 = ((i2cp->slave_addr >>7) & 0x0006);/* add the two msb of 10-bit address to the header */
-    i2cp->slave_addr1 |= 0xF0;                            /* add the header bits (the LSB -> 1 will be add to second */
-    i2cp->slave_addr2 = i2cp->slave_addr & 0x00FF;        /* the remaining 8 bit of 10-bit address */
-  }
-  else{
-    i2cp->slave_addr1 |= 0x01;
-  }
-
-  /* setting flags and register bits */
-  i2cp->flags |= I2C_FLG_MASTER_RECEIVER;
-  i2cp->errors = 0;
-  i2cp->id_i2c->CR1 |= I2C_CR1_ACK;                       /* acknowledge returned */
-  i2cp->id_i2c->CR1 &= ~I2C_CR1_POS;
-
-  if(i2cp->rxbytes == 1) {                                /* Only one byte to be received */
-    i2cp->flags |= I2C_FLG_1BTR;
-  }
-  else if(i2cp->rxbytes == 2) {                           /* Only two bytes to be received */
-    i2cp->flags |= I2C_FLG_2BTR;
-    i2cp->id_i2c->CR1 |= I2C_CR1_POS;                     /* Acknowledge Position */
-  }
-
-  i2cp->id_i2c->CR2 |= (I2C_CR2_ITERREN|I2C_CR2_ITEVTEN|I2C_CR2_ITBUFEN);  /* enable ERR, EVT & BUF ITs */
-}
-
-#else /*I2C_SUPPORTS_CALLBACKS*/
-
-/**
- * @brief Synchronously transmits data via the I2C bus as master.
- *
- * @param[in] i2cp        pointer to the @p I2CDriver object
- * @param[in] slave_addr  Slave device address. Bits 0-9 contain slave
- *                        device address. Bit 15 must be set to 1 if 10-bit
- *                        addressing modes used. Otherwise  keep it cleared.
- *                        Bits 10-14 unused.
- * @param[in] txbuf       pointer to the transmit buffer
- * @param[in] txbytes     number of bytes to be transmitted
- * @param[in] rxbuf       pointer to the receive buffer
- * @param[in] rxbytes     number of bytes to be received
- */
-void i2c_lld_master_transmit(I2CDriver *i2cp, uint16_t slave_addr,
-    uint8_t *txbuf, size_t txbytes, uint8_t *rxbuf, size_t rxbytes) {
-
-  /* init driver fields */
-  i2cp->slave_addr = slave_addr;
-  i2cp->txbytes = txbytes;
-  i2cp->rxbytes = rxbytes;
-  i2cp->txbuf = txbuf;
-  i2cp->rxbuf = rxbuf;
-
-  /* init address fields */
-  if(slave_addr & 0x8000){                                    /* 10-bit mode used */
-    i2cp->slave_addr1 = ((slave_addr >>7) & 0x0006);          /* add the two msb of 10-bit address to the header */
-    i2cp->slave_addr1 |= 0xF0;                                /* add the header bits with LSB = 0 -> write */
-    i2cp->slave_addr2 = slave_addr & 0x00FF;                  /* the remaining 8 bit of 10-bit address */
-  }
-  else{
-    i2cp->slave_addr1 = ((slave_addr <<1) & 0x00FE);          /* LSB = 0 -> write */
-  }
-
-  i2cp->flags = 0;
-  i2cp->errors = 0;
-  i2cp->id_i2c->CR1 &= ~I2C_CR1_POS;
-  i2cp->id_i2c->CR2 &= ~I2C_CR2_ITEVTEN;                      /* disable event interrupts */
-  i2cp->id_i2c->CR2 |= I2C_CR2_ITERREN;                       /* enable error interrupts */
-
-  i2cp->id_i2c->CR1 |= I2C_CR1_START;
-  while (!(i2cp->id_i2c->SR1 & I2C_SR1_SB))
-    ;
-  i2cp->id_i2c->DR = i2cp->slave_addr1;
-  while (!(i2cp->id_i2c->SR1 & I2C_SR1_ADDR))
-    ;
-  while (!(i2cp->id_i2c->SR2 & I2C_SR2_BUSY))
-    ;
-  i2cp->id_i2c->DR = *txbuf;
-  txbuf++;
-  i2cp->txbytes--;
-  while(i2cp->txbytes > 0){
-    while(!(i2cp->id_i2c->SR1 & I2C_SR1_BTF))
-      ;
-    i2cp->id_i2c->DR = *txbuf;
-    txbuf++;
-    i2cp->txbytes--;
-  }
-  while(!(i2cp->id_i2c->SR1 & I2C_SR1_BTF))
-    ;
-  if(rxbytes == 0){
-    i2cp->id_i2c->CR1 |= I2C_CR1_STOP;
-    while (i2cp->id_i2c->CR1 & I2C_CR1_STOP)
-      ;
-    }
-  else{
-    i2c_lld_master_receive(i2cp, slave_addr, rxbuf, rxbytes);
-  }
-}
-
-
-/**
- * @brief Synchronously receives data from the I2C bus.
- *
- * @param[in] i2cp        pointer to the @p I2CDriver object
- * @param[in] slave_addr  Slave device address. Bits 0-9 contain slave
- *                        device address. Bit 15 must be set to 1 if 10-bit
- *                        addressing modes used. Otherwise  keep it cleared.
- *                        Bits 10-14 unused.
- * @param[in] rxbuf       pointer to the receive buffer
- * @param[in] rxbytes     number of bytes to be received
- */
-void i2c_lld_master_receive(I2CDriver *i2cp, uint16_t slave_addr,
-    uint8_t *rxbuf, size_t rxbytes){
-
-  /* init driver fields */
-  i2cp->slave_addr = slave_addr;
-  i2cp->rxbytes = rxbytes;
-  i2cp->rxbuf = rxbuf;
-
-  /* init address fields */
-  if(slave_addr & 0x8000){                            /* 10-bit mode used */
-    i2cp->slave_addr1 = ((slave_addr >>7) & 0x0006);  /* add the two msb of 10-bit address to the header */
-    i2cp->slave_addr1 |= 0xF0;                        /* add the header bits (the LSB -> 1 will be add to second */
-    i2cp->slave_addr2 = slave_addr & 0x00FF;          /* the remaining 8 bit of 10-bit address */
-  }
-  else{
-    i2cp->slave_addr1 = ((slave_addr <<1) | 0x01);    /* LSB = 1 -> receive */
-  }
-
-
-  /* setting flags and register bits */
-  i2cp->flags = 0;
-  i2cp->errors = 0;
-  i2cp->id_i2c->CR1 &= ~I2C_CR1_POS;
-  i2cp->id_i2c->CR2 &= ~I2C_CR2_ITEVTEN;              /* disable event interrupts */
-  i2cp->id_i2c->CR2 |= I2C_CR2_ITERREN;               /* enable error interrupts */
-
-  i2cp->id_i2c->CR1 |= I2C_CR1_START;
-  while (!(i2cp->id_i2c->SR1 & I2C_SR1_SB))
-    ;
-
-  i2cp->id_i2c->DR = i2cp->slave_addr1;
-  while (!(i2cp->id_i2c->SR1 & I2C_SR1_ADDR))
-    ;
-
-  if(i2cp->rxbytes >= 3){                             /* more than 2 bytes receiving procedure */
-    while(!(i2cp->id_i2c->SR2 & I2C_SR2_BUSY))        /* to clear ADDR bit */
-      ;
-    while(i2cp->rxbytes > 3){
-      while(!(i2cp->id_i2c->SR1 & I2C_SR1_BTF))
-        ;
-      *rxbuf = i2cp->id_i2c->DR;
-      rxbuf++;
-      i2cp->rxbytes--;
-    }
-    while(!(i2cp->id_i2c->SR1 & I2C_SR1_BTF))          /* stopping procedure */
-      ;
-    i2cp->id_i2c->CR1 &= ~I2C_CR1_ACK;
-    chSysLock();
-    i2cp->id_i2c->CR1 |= I2C_CR1_STOP;
-    *rxbuf = i2cp->id_i2c->DR;
-    rxbuf++;
-    i2cp->rxbytes--;
-    chSysUnlock();
-    while(!(i2cp->id_i2c->SR1 & I2C_SR1_RXNE))
-      ;
-    *rxbuf = i2cp->id_i2c->DR;
-    rxbuf++;
-    i2cp->rxbytes--;
-    while (i2cp->id_i2c->CR1 & I2C_CR1_STOP)
-      ;
-    i2cp->id_i2c->CR1 |= I2C_CR1_ACK;
-  }
-  else{                                                 /* 1 or 2 bytes receiving procedure */
-    if(i2cp->rxbytes == 2){
-      i2cp->id_i2c->CR1 |= I2C_CR1_POS;
-      chSysLock();
-      while(!(i2cp->id_i2c->SR2 & I2C_SR2_BUSY))        /* to clear ADDR bit */
-        ;
-      i2cp->id_i2c->CR1 &= ~I2C_CR1_ACK;
-      chSysUnlock();
-      while(!(i2cp->id_i2c->SR1 & I2C_SR1_BTF))
-        ;
-      chSysLock();
-      i2cp->id_i2c->CR1 |= I2C_CR1_STOP;
-      *rxbuf = i2cp->id_i2c->DR;
-      rxbuf++;
-      i2cp->rxbytes--;
-      chSysUnlock();
-      *rxbuf = i2cp->id_i2c->DR;
-      rxbuf++;
-      i2cp->rxbytes--;
-      while (i2cp->id_i2c->CR1 & I2C_CR1_STOP)
-        ;
-      i2cp->id_i2c->CR1 &= ~I2C_CR1_POS;
-      i2cp->id_i2c->CR1 |= I2C_CR1_ACK;
-    }
-    else{ /* 1 byte */
-      i2cp->id_i2c->CR1 &= ~I2C_CR1_ACK;
-      chSysLock();
-      while(!(i2cp->id_i2c->SR2 & I2C_SR2_BUSY))       /* to clear ADDR bit */
-        ;
-      i2cp->id_i2c->CR1 |= I2C_CR1_STOP;
-      chSysUnlock();
-      while(!(i2cp->id_i2c->SR1 & I2C_SR1_RXNE))
-        ;
-      *rxbuf = i2cp->id_i2c->DR;
-      rxbuf++;
-      i2cp->rxbytes--;
-      while (i2cp->id_i2c->CR1 & I2C_CR1_STOP)
-        ;
-      i2cp->id_i2c->CR1 |= I2C_CR1_ACK;
-    }
-  }
-}
-#endif /* I2C_SUPPORTS_CALLBACKS */
-
-#undef rxBuffp
-#undef txBuffp
-
 #endif /* HAL_USE_I2C */
diff --git a/os/hal/platforms/STM32/i2c_lld.h b/os/hal/platforms/STM32/i2c_lld.h
index 81a9f62dc..4e9051e39 100644
--- a/os/hal/platforms/STM32/i2c_lld.h
+++ b/os/hal/platforms/STM32/i2c_lld.h
@@ -42,50 +42,32 @@
  * @name    Configuration options
  * @{
  */
-/**
- * @brief Switch between callback based and synchronouse driver.
- * @note  The default is synchronouse.
- */
-#if !defined(I2C_SUPPORTS_CALLBACKS) || defined(__DOXYGEN__)
-#define I2C_SUPPORTS_CALLBACKS              TRUE
-#endif
-
-/**
- * @brief I2C1 driver synchronization choice between GPT and polling.
- * @note The default is polling wait.
- */
-#if !defined(STM32_I2C_I2C1_USE_GPT_TIM)       || \
-    !defined(STM32_I2C_I2C1_USE_POLLING_WAIT)  || \
-    defined(__DOXYGEN__)
-#define STM32_I2C_I2C1_USE_POLLING_WAIT     TRUE
-#endif
-
-/**
- * @brief I2C2 driver synchronization choice between GPT and polling.
- * @note The default is polling wait.
- */
-#if !defined(STM32_I2C_I2C2_USE_GPT_TIM)       || \
-    !defined(STM32_I2C_I2C2_USE_POLLING_WAIT)  || \
-    defined(__DOXYGEN__)
-#define STM32_I2C_I2C2_USE_POLLING_WAIT     TRUE
-#endif
 
 /**
  * @brief I2C1 driver enable switch.
  * @details If set to @p TRUE the support for I2C1 is included.
- * @note The default is @p TRUE.
+ * @note The default is @p FALSE.
  */
 #if !defined(STM32_I2C_USE_I2C1) || defined(__DOXYGEN__)
-#define STM32_I2C_USE_I2C1              TRUE
+#define STM32_I2C_USE_I2C1              FALSE
 #endif
 
 /**
  * @brief I2C2 driver enable switch.
  * @details If set to @p TRUE the support for I2C2 is included.
- * @note The default is @p TRUE.
+ * @note The default is @p FALSE.
  */
 #if !defined(STM32_I2C_USE_I2C2) || defined(__DOXYGEN__)
-#define STM32_I2C_USE_I2C2              TRUE
+#define STM32_I2C_USE_I2C2              FALSE
+#endif
+
+/**
+ * @brief I2C3 driver enable switch.
+ * @details If set to @p TRUE the support for I2C3 is included.
+ * @note The default is @p FALSE.
+ */
+#if !defined(STM32_I2C_USE_I2C3) || defined(__DOXYGEN__)
+#define STM32_I2C_USE_I2C3              FALSE
 #endif
 
 /**
@@ -103,42 +85,157 @@
 #if !defined(STM32_I2C_I2C2_IRQ_PRIORITY) || defined(__DOXYGEN__)
 #define STM32_I2C_I2C2_IRQ_PRIORITY     0xA0
 #endif
+
+/**
+ * @brief I2C2 interrupt priority level setting.
+ * @note @p BASEPRI_KERNEL >= @p STM32_I2C_I2C2_IRQ_PRIORITY > @p PRIORITY_PENDSV.
+ */
+#if !defined(STM32_I2C_I2C3_IRQ_PRIORITY) || defined(__DOXYGEN__)
+#define STM32_I2C_I2C3_IRQ_PRIORITY     0xA0
+#endif
+
+/**
+ * @brief   I2C1 DMA error hook.
+ * @note    The default action for DMA errors is a system halt because DMA
+ *          error can only happen because programming errors.
+ */
+#if !defined(STM32_I2C_DMA_ERROR_HOOK) || defined(__DOXYGEN__)
+#define STM32_I2C_DMA_ERROR_HOOK(uartp)    chSysHalt()
+#endif
+
+#if STM32_ADVANCED_DMA || defined(__DOXYGEN__)
+
+/**
+ * @brief   DMA stream used for I2C1 RX operations.
+ * @note    This option is only available on platforms with enhanced DMA.
+ */
+#if !defined(STM32_I2C_I2C1_RX_DMA_STREAM) || defined(__DOXYGEN__)
+#define STM32_I2C_I2C1_RX_DMA_STREAM     STM32_DMA_STREAM_ID(1, 0)
+#endif
+
+/**
+ * @brief   DMA stream used for I2C1 TX operations.
+ * @note    This option is only available on platforms with enhanced DMA.
+ */
+#if !defined(STM32_I2C_I2C1_TX_DMA_STREAM) || defined(__DOXYGEN__)
+#define STM32_I2C_I2C1_TX_DMA_STREAM     STM32_DMA_STREAM_ID(1, 6)
+#endif
+
+/**
+ * @brief   DMA stream used for I2C2 RX operations.
+ * @note    This option is only available on platforms with enhanced DMA.
+ */
+#if !defined(STM32_I2C_I2C2_RX_DMA_STREAM) || defined(__DOXYGEN__)
+#define STM32_I2C_I2C2_RX_DMA_STREAM     STM32_DMA_STREAM_ID(1, 2)
+#endif
+
+/**
+ * @brief   DMA stream used for I2C2 TX operations.
+ * @note    This option is only available on platforms with enhanced DMA.
+ */
+#if !defined(STM32_I2C_I2C2_TX_DMA_STREAM) || defined(__DOXYGEN__)
+#define STM32_I2C_I2C2_TX_DMA_STREAM     STM32_DMA_STREAM_ID(1, 7)
+#endif
+
+/**
+ * @brief   DMA stream used for I2C3 RX operations.
+ * @note    This option is only available on platforms with enhanced DMA.
+ */
+#if !defined(STM32_I2C_I2C3_RX_DMA_STREAM) || defined(__DOXYGEN__)
+#define STM32_I2C_I2C3_RX_DMA_STREAM     STM32_DMA_STREAM_ID(1, 2)
+#endif
+
+/**
+ * @brief   DMA stream used for I2C3 TX operations.
+ * @note    This option is only available on platforms with enhanced DMA.
+ */
+#if !defined(STM32_I2C_I2C3_TX_DMA_STREAM) || defined(__DOXYGEN__)
+#define STM32_I2C_I2C3_TX_DMA_STREAM     STM32_DMA_STREAM_ID(1, 4)
+#endif
+
+#else /* !STM32_ADVANCED_DMA */
+
+/* Fixed streams for platforms using the old DMA peripheral, the values are
+   valid for both STM32F1xx and STM32L1xx.*/
+#define STM32_I2C_I2C1_RX_DMA_STREAM     STM32_DMA_STREAM_ID(1, 7)
+#define STM32_I2C_I2C1_TX_DMA_STREAM     STM32_DMA_STREAM_ID(1, 6)
+#define STM32_I2C_I2C2_RX_DMA_STREAM     STM32_DMA_STREAM_ID(1, 5)
+#define STM32_I2C_I2C2_TX_DMA_STREAM     STM32_DMA_STREAM_ID(1, 4)
+
+#endif /* !STM32_ADVANCED_DMA*/
 /** @} */
 
 /*===========================================================================*/
 /* Derived constants and error checks.                                       */
 /*===========================================================================*/
 
-/** @brief  EV5 */
-#define I2C_EV5_MASTER_MODE_SELECT          ((uint32_t)(((I2C_SR2_MSL|I2C_SR2_BUSY)<< 16)|I2C_SR1_SB))  /* BUSY, MSL and SB flag */
-/** @brief  EV6 */
+/** @brief  flags for interrupt handling */
+#define I2C_EV5_MASTER_MODE_SELECT          ((uint32_t)(((I2C_SR2_MSL | I2C_SR2_BUSY) << 16) | I2C_SR1_SB))  /* BUSY, MSL and SB flag */
 #define I2C_EV6_MASTER_TRA_MODE_SELECTED    ((uint32_t)(((I2C_SR2_MSL|I2C_SR2_BUSY|I2C_SR2_TRA)<< 16)|I2C_SR1_ADDR|I2C_SR1_TXE))  /* BUSY, MSL, ADDR, TXE and TRA flags */
 #define I2C_EV6_MASTER_REC_MODE_SELECTED    ((uint32_t)(((I2C_SR2_MSL|I2C_SR2_BUSY)<< 16)|I2C_SR1_ADDR))  /* BUSY, MSL and ADDR flags */
-/** @brief  EV7 */
-#define I2C_EV7_MASTER_REC_BYTE_RECEIVED    ((uint32_t)(((I2C_SR2_MSL|I2C_SR2_BUSY)<< 16)|I2C_SR1_RXNE))             /* BUSY, MSL and RXNE flags */
-#define I2C_EV7_MASTER_REC_BYTE_QUEUED      ((uint32_t)(((I2C_SR2_MSL|I2C_SR2_BUSY)<< 16)|I2C_SR1_BTF|I2C_SR1_RXNE)) /* BUSY, MSL, RXNE and BTF flags*/
-/** @brief  EV8 */
-#define I2C_EV8_MASTER_BYTE_TRANSMITTING    ((uint32_t)(((I2C_SR2_MSL|I2C_SR2_BUSY|I2C_SR2_TRA)<< 16)|I2C_SR1_TXE))   /* TRA, BUSY, MSL, TXE flags */
-/** @brief  EV8_2 */
-#define I2C_EV8_2_MASTER_BYTE_TRANSMITTED   ((uint32_t)(((I2C_SR2_MSL|I2C_SR2_BUSY|I2C_SR2_TRA)<< 16)|I2C_SR1_BTF|I2C_SR1_TXE))  /* TRA, BUSY, MSL, TXE and BTF flags */
-/** @brief  EV9 */
-#define I2C_EV9_MASTER_ADDR_10BIT           ((uint32_t)(((I2C_SR2_MSL|I2C_SR2_BUSY)<< 16)|I2C_SR1_ADD10))  /* BUSY, MSL and ADD10 flags */
+#define I2C_EV8_2_MASTER_BYTE_TRANSMITTED   ((uint32_t)(((I2C_SR2_MSL | I2C_SR2_BUSY | I2C_SR2_TRA) << 16) | I2C_SR1_BTF | I2C_SR1_TXE))  /* TRA, BUSY, MSL, TXE and BTF flags */
 #define I2C_EV_MASK                         0x00FFFFFF  /* First byte zeroed because there is no need of PEC register part from SR2 */
 
-#define I2C_FLG_1BTR            0x01 /* Single byte to be received and processed */
-#define I2C_FLG_2BTR            0x02 /* Two bytes to be received and processed */
-#define I2C_FLG_3BTR            0x04 /* Last three received bytes to be processed */
-#define I2C_FLG_MASTER_RECEIVER 0x10
-#define I2C_FLG_HEADER_SENT     0x80
-#define I2C_FLG_TIMER_ARMED     0x40 /* Used to check locks on the bus */
+#define I2C_FLG_MASTER_RECEIVER 			0x10
+#define I2C_FLG_HEADER_SENT     			0x80
 
-#define EV6_SUBEV_MASK  (I2C_FLG_1BTR|I2C_FLG_2BTR|I2C_FLG_MASTER_RECEIVER)
-#define EV7_SUBEV_MASK  (I2C_FLG_2BTR|I2C_FLG_3BTR|I2C_FLG_MASTER_RECEIVER)
+/** @brief  error checks */
+#if STM32_I2C_USE_I2C1 && !STM32_HAS_I2C1
+#error "I2C1 not present in the selected device"
+#endif
 
-#define I2C_EV6_1_MASTER_REC_2BTR_MODE_SELECTED (I2C_FLG_2BTR|I2C_FLG_MASTER_RECEIVER)
-#define I2C_EV6_3_MASTER_REC_1BTR_MODE_SELECTED (I2C_FLG_1BTR|I2C_FLG_MASTER_RECEIVER)
-#define I2C_EV7_2_MASTER_REC_3BYTES_TO_PROCESS  (I2C_FLG_3BTR|I2C_FLG_MASTER_RECEIVER)
-#define I2C_EV7_3_MASTER_REC_2BYTES_TO_PROCESS  (I2C_FLG_2BTR|I2C_FLG_MASTER_RECEIVER)
+#if STM32_I2C_USE_I2C2 && !STM32_HAS_I2C2
+#error "I2C2 not present in the selected device"
+#endif
+
+#if STM32_I2C_USE_I2C3 && !STM32_HAS_I2C3
+#error "I2C3 not present in the selected device"
+#endif
+
+#if !STM32_I2C_USE_I2C1 && !STM32_I2C_USE_I2C2 &&                     		\
+    !STM32_I2C_USE_I2C3
+#error "I2C driver activated but no I2C peripheral assigned"
+#endif
+
+#if STM32_I2C_USE_I2C1 &&                                                \
+    !STM32_DMA_IS_VALID_ID(STM32_I2C_I2C1_RX_DMA_STREAM,                 \
+                           STM32_I2C1_RX_DMA_MSK)
+#error "invalid DMA stream associated to I2C1 RX"
+#endif
+
+#if STM32_I2C_USE_I2C1 &&                                                \
+    !STM32_DMA_IS_VALID_ID(STM32_I2C_I2C1_TX_DMA_STREAM,                 \
+                           STM32_I2C1_TX_DMA_MSK)
+#error "invalid DMA stream associated to I2C1 TX"
+#endif
+
+#if STM32_I2C_USE_I2C2 &&                                                \
+    !STM32_DMA_IS_VALID_ID(STM32_I2C_I2C2_RX_DMA_STREAM,                 \
+                           STM32_I2C2_RX_DMA_MSK)
+#error "invalid DMA stream associated to I2C2 RX"
+#endif
+
+#if STM32_I2C_USE_I2C2 &&                                                \
+    !STM32_DMA_IS_VALID_ID(STM32_I2C_I2C2_TX_DMA_STREAM,                 \
+                           STM32_I2C2_TX_DMA_MSK)
+#error "invalid DMA stream associated to I2C2 TX"
+#endif
+
+#if STM32_I2C_USE_I2C3 &&                                                \
+    !STM32_DMA_IS_VALID_ID(STM32_I2C_I2C3_RX_DMA_STREAM,                 \
+                           STM32_I2C3_RX_DMA_MSK)
+#error "invalid DMA stream associated to I2C3 RX"
+#endif
+
+#if STM32_I2C_USE_I2C3 &&                                                \
+    !STM32_DMA_IS_VALID_ID(STM32_I2C_I2C3_TX_DMA_STREAM,                 \
+                           STM32_I2C3_TX_DMA_MSK)
+#error "invalid DMA stream associated to I2C3 TX"
+#endif
+
+#if !defined(STM32_DMA_REQUIRED)
+#define STM32_DMA_REQUIRED
+#endif
 
 /*===========================================================================*/
 /* Driver data structures and types.                                         */
@@ -168,9 +265,6 @@ typedef struct {
   i2copmode_t     op_mode;       /**< @brief Specifies the I2C mode.*/
   uint32_t        clock_speed;   /**< @brief Specifies the clock frequency. Must be set to a value lower than 400kHz */
   i2cdutycycle_t  duty_cycle;    /**< @brief Specifies the I2C fast mode duty cycle */
-  uint8_t         own_addr_7;    /**< @brief Specifies the first device 7-bit own address. */
-  uint16_t        own_addr_10;   /**< @brief Specifies the second part of device own address in 10-bit mode. Set to NULL if not used. */
-  uint8_t         nbit_own_addr; /**< @brief Specifies if 7-bit or 10-bit address is acknowledged */
 } I2CConfig;
 
 
@@ -179,11 +273,6 @@ typedef struct {
  */
 typedef struct I2CDriver I2CDriver;
 
-/**
- * @brief   Type of a structure representing an I2C slave config.
- */
-typedef struct I2CSlaveConfig I2CSlaveConfig;
-
 /**
  * @brief Structure representing an I2C driver.
  */
@@ -191,72 +280,46 @@ struct I2CDriver{
   /**
    * @brief Driver state.
    */
-  i2cstate_t            id_state;
+  i2cstate_t                id_state;
 
-#if I2C_USE_WAIT
   /**
    * @brief Thread waiting for I/O completion.
    */
-  Thread                *id_thread;
-#endif /* I2C_USE_WAIT */
+  Thread                    *id_thread;
+
 #if I2C_USE_MUTUAL_EXCLUSION || defined(__DOXYGEN__)
 #if CH_USE_MUTEXES || defined(__DOXYGEN__)
   /**
    * @brief Mutex protecting the bus.
    */
-  Mutex                 id_mutex;
+  Mutex                     id_mutex;
 #elif CH_USE_SEMAPHORES
-  Semaphore             id_semaphore;
+  Semaphore                 id_semaphore;
 #endif
 #endif /* I2C_USE_MUTUAL_EXCLUSION */
 
   /**
    * @brief Current configuration data.
    */
-  const I2CConfig       *id_config;
-  /**
-   * @brief Current slave configuration data.
-   */
-  const I2CSlaveConfig  *id_slave_config;
+  const I2CConfig           *id_config;
 
-  __IO size_t           txbytes;    /*!< @brief Number of bytes to be transmitted. */
-  __IO size_t           rxbytes;    /*!< @brief Number of bytes to be received. */
-  uint8_t               *rxbuf;     /*!< @brief Pointer to receive buffer. */
-  uint8_t               *txbuf;     /*!< @brief Pointer to transmit buffer.*/
-  uint8_t               *rxbuff_p;  /*!< @brief Pointer to the current byte in slave rx buffer. */
-  uint8_t               *txbuff_p;  /*!< @brief Pointer to the current byte in slave tx buffer. */
+  __IO size_t               txbytes;    /*!< @brief Number of bytes to be transmitted. */
+  __IO size_t               rxbytes;    /*!< @brief Number of bytes to be received. */
+  uint8_t                   *rxbuf;     /*!< @brief Pointer to receive buffer. */
+  uint8_t                   *txbuf;     /*!< @brief Pointer to transmit buffer.*/
 
-  __IO i2cflags_t       errors;     /*!< @brief Error flags.*/
-  __IO i2cflags_t       flags;      /*!< @brief State flags.*/
+  __IO i2cflags_t           errors;     /*!< @brief Error flags.*/
+  __IO i2cflags_t           flags;      /*!< @brief State flags.*/
 
-  uint16_t              slave_addr; /*!< @brief Current slave address. */
-  uint8_t               slave_addr1;/*!< @brief 7-bit address of the slave with r\w bit.*/
-  uint8_t               slave_addr2;/*!< @brief Uses in 10-bit address mode. */
-
-#if CH_USE_EVENTS
-  EventSource           sevent;     /*!< @brief Status Change @p EventSource.*/
-#endif
+  uint8_t                   slave_addr; /*!< @brief Current slave address without R/W bit. */
 
   /*********** End of the mandatory fields. **********************************/
 
-  /**
-   * @brief Pointer to the I2Cx registers block.
-   */
-  I2C_TypeDef           *id_i2c;
+  uint32_t                  dmamode;    /*!< @brief DMA mode bit mask.*/
+  const stm32_dma_stream_t  *dmarx;     /*!< @brief Receive DMA channel.*/
+  const stm32_dma_stream_t  *dmatx;     /*!< @brief Transmit DMA channel.*/
 
-#if !(STM32_I2C_I2C1_USE_POLLING_WAIT)
-  /* TODO: capability to switch this GPT fields off */
-  /**
-   * @brief Timer for waiting STOP condition on the bus.
-   * @details This is workaround for STM32 buggy I2C cell.
-   */
-  GPTDriver             *timer;
-
-  /**
-   * @brief Config for workaround timer.
-   */
-  const GPTConfig       *timer_cfg;
-#endif /* !(STM32_I2C_I2C1_USE_POLLING_WAIT) */
+  I2C_TypeDef               *id_i2c;    /*!< @brief Pointer to the I2Cx registers block. */
 };
 
 
@@ -268,8 +331,7 @@ struct I2CDriver{
   (i2cp->id_i2c->SR2 & I2C_SR2_BUSY)
 
 
-/* Wait until BUSY flag is reset: a STOP has been generated on the bus
- * signaling the end of transmission. Normally this wait function
+/* Wait until BUSY flag is reset. Normally this wait function
  * does not block thread, only if slave not response it does.
  */
 #define i2c_lld_wait_bus_free(i2cp) {                                       \
@@ -291,6 +353,10 @@ extern I2CDriver I2CD1;
 extern I2CDriver I2CD2;
 #endif
 
+#if STM32_I2C_USE_I2C3
+extern I2CDriver I2CD3;
+#endif
+
 #ifdef __cplusplus
 extern "C" {
 #endif
@@ -299,14 +365,12 @@ void i2c_lld_init(void);
 void i2c_lld_reset(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_start(I2CDriver *i2cp);
 void i2c_lld_stop(I2CDriver *i2cp);
-void i2c_lld_master_transmit(I2CDriver *i2cp, uint16_t slave_addr,
+void i2c_lld_master_transmit(I2CDriver *i2cp, uint8_t slave_addr,
     uint8_t *txbuf, size_t txbytes, uint8_t *rxbuf, size_t rxbytes);
-void i2c_lld_master_receive(I2CDriver *i2cp, uint16_t slave_addr,
+void i2c_lld_master_receive(I2CDriver *i2cp, uint8_t slave_addr,
     uint8_t *rxbuf, size_t rxbytes);
-void i2c_lld_master_transceive(I2CDriver *i2cp);
 
 #ifdef __cplusplus
 }
diff --git a/os/hal/platforms/STM32F1xx/hal_lld_f100.h b/os/hal/platforms/STM32F1xx/hal_lld_f100.h
index 3f5d9502b..96f5a7f33 100644
--- a/os/hal/platforms/STM32F1xx/hal_lld_f100.h
+++ b/os/hal/platforms/STM32F1xx/hal_lld_f100.h
@@ -295,8 +295,22 @@
 
 /* I2C attributes.*/
 #define STM32_HAS_I2C1          TRUE
+#define STM32_I2C1_RX_DMA_MSK   (STM32_DMA_STREAM_ID_MSK(1, 7))
+#define STM32_I2C1_RX_DMA_CHN   0x00000000
+#define STM32_I2C1_TX_DMA_MSK   (STM32_DMA_STREAM_ID_MSK(1, 6))
+#define STM32_I2C1_TX_DMA_CHN   0x00000000
+
 #define STM32_HAS_I2C2          TRUE
+#define STM32_I2C2_RX_DMA_MSK   (STM32_DMA_STREAM_ID_MSK(1, 5))
+#define STM32_I2C2_RX_DMA_CHN   0x00000000
+#define STM32_I2C2_TX_DMA_MSK   (STM32_DMA_STREAM_ID_MSK(1, 4))
+#define STM32_I2C2_TX_DMA_CHN   0x00000000
+
 #define STM32_HAS_I2C3          FALSE
+#define STM32_I2C3_RX_DMA_MSK   0
+#define STM32_I2C3_RX_DMA_CHN   0x00000000
+#define STM32_I2C3_TX_DMA_MSK   0
+#define STM32_I2C3_TX_DMA_CHN   0x00000000
 
 /* RTC attributes.*/
 #define STM32_HAS_RTC           TRUE
diff --git a/os/hal/platforms/STM32F1xx/hal_lld_f103.h b/os/hal/platforms/STM32F1xx/hal_lld_f103.h
index 22d955c65..d251e6a5d 100644
--- a/os/hal/platforms/STM32F1xx/hal_lld_f103.h
+++ b/os/hal/platforms/STM32F1xx/hal_lld_f103.h
@@ -306,8 +306,24 @@
 
 /* I2C attributes.*/
 #define STM32_HAS_I2C1          TRUE
+#define STM32_I2C1_RX_DMA_MSK   (STM32_DMA_STREAM_ID_MSK(1, 7))
+#define STM32_I2C1_RX_DMA_CHN   0x00000000
+#define STM32_I2C1_TX_DMA_MSK   (STM32_DMA_STREAM_ID_MSK(1, 6))
+#define STM32_I2C1_TX_DMA_CHN   0x00000000
+
 #define STM32_HAS_I2C2          TRUE
+#define STM32_I2C2_RX_DMA_MSK   (STM32_DMA_STREAM_ID_MSK(1, 5))
+#define STM32_I2C2_RX_DMA_CHN   0x00000000
+#define STM32_I2C2_TX_DMA_MSK   (STM32_DMA_STREAM_ID_MSK(1, 4))
+#define STM32_I2C2_TX_DMA_CHN   0x00000000
+
 #define STM32_HAS_I2C3          FALSE
+#define STM32_I2C3_RX_DMA_MSK   0
+#define STM32_I2C3_RX_DMA_CHN   0x00000000
+#define STM32_I2C3_TX_DMA_MSK   0
+#define STM32_I2C3_TX_DMA_CHN   0x00000000
+
+
 
 /* RTC attributes.*/
 #define STM32_HAS_RTC           TRUE
diff --git a/os/hal/platforms/STM32F1xx/hal_lld_f105_f107.h b/os/hal/platforms/STM32F1xx/hal_lld_f105_f107.h
index 882ec6094..9a612b0ee 100644
--- a/os/hal/platforms/STM32F1xx/hal_lld_f105_f107.h
+++ b/os/hal/platforms/STM32F1xx/hal_lld_f105_f107.h
@@ -166,8 +166,22 @@
 
 /* I2C attributes.*/
 #define STM32_HAS_I2C1          TRUE
+#define STM32_I2C1_RX_DMA_MSK   (STM32_DMA_STREAM_ID_MSK(1, 7))
+#define STM32_I2C1_RX_DMA_CHN   0x00000000
+#define STM32_I2C1_TX_DMA_MSK   (STM32_DMA_STREAM_ID_MSK(1, 6))
+#define STM32_I2C1_TX_DMA_CHN   0x00000000
+
 #define STM32_HAS_I2C2          TRUE
+#define STM32_I2C2_RX_DMA_MSK   (STM32_DMA_STREAM_ID_MSK(1, 5))
+#define STM32_I2C2_RX_DMA_CHN   0x00000000
+#define STM32_I2C2_TX_DMA_MSK   (STM32_DMA_STREAM_ID_MSK(1, 4))
+#define STM32_I2C2_TX_DMA_CHN   0x00000000
+
 #define STM32_HAS_I2C3          FALSE
+#define STM32_I2C3_RX_DMA_MSK   0
+#define STM32_I2C3_RX_DMA_CHN   0x00000000
+#define STM32_I2C3_TX_DMA_MSK   0
+#define STM32_I2C3_TX_DMA_CHN   0x00000000
 
 /* RTC attributes.*/
 #define STM32_HAS_RTC           TRUE
diff --git a/os/hal/platforms/STM32F2xx/hal_lld.h b/os/hal/platforms/STM32F2xx/hal_lld.h
index 91135950f..cad926330 100644
--- a/os/hal/platforms/STM32F2xx/hal_lld.h
+++ b/os/hal/platforms/STM32F2xx/hal_lld.h
@@ -157,8 +157,27 @@
 #define STM32_HAS_GPIOH         TRUE
 #define STM32_HAS_GPIOI         TRUE
 
+/* I2C attributes.*/
 #define STM32_HAS_I2C1          TRUE
+#define STM32_I2C1_RX_DMA_MSK ((STM32_DMA_STREAM_ID_MSK(1, 0) |             \
+                               STM32_DMA_STREAM_ID_MSK(1, 5)))
+#define STM32_I2C1_RX_DMA_CHN 0x00100001
+#define STM32_I2C1_TX_DMA_MSK ((STM32_DMA_STREAM_ID_MSK(1, 7)) |            \
+                              (STM32_DMA_STREAM_ID_MSK(1, 6)))
+#define STM32_I2C1_TX_DMA_CHN 0x10000000
+
 #define STM32_HAS_I2C2          TRUE
+#define STM32_I2C2_RX_DMA_MSK ((STM32_DMA_STREAM_ID_MSK(1, 2) |             \
+                               STM32_DMA_STREAM_ID_MSK(1, 3)))
+#define STM32_I2C2_RX_DMA_CHN 0x00007700
+#define STM32_I2C2_TX_DMA_MSK (STM32_DMA_STREAM_ID_MSK(1, 7))
+#define STM32_I2C2_TX_DMA_CHN 0x70000000
+
+#define STM32_HAS_I2C3          TRUE
+#define STM32_I2C3_RX_DMA_MSK (STM32_DMA_STREAM_ID_MSK(1, 2))
+#define STM32_I2C3_RX_DMA_CHN 0x00000300
+#define STM32_I2C3_TX_DMA_MSK (STM32_DMA_STREAM_ID_MSK(1, 4))
+#define STM32_I2C3_TX_DMA_CHN 0x00030000
 
 #define STM32_HAS_RTC           TRUE
 
diff --git a/os/hal/platforms/STM32F4xx/platform.mk b/os/hal/platforms/STM32F4xx/platform.mk
index 7acce0172..449b40731 100644
--- a/os/hal/platforms/STM32F4xx/platform.mk
+++ b/os/hal/platforms/STM32F4xx/platform.mk
@@ -9,6 +9,7 @@ PLATFORMSRC = ${CHIBIOS}/os/hal/platforms/STM32F4xx/stm32_dma.c \
               ${CHIBIOS}/os/hal/platforms/STM32/serial_lld.c \
               ${CHIBIOS}/os/hal/platforms/STM32/spi_lld.c \
               ${CHIBIOS}/os/hal/platforms/STM32/uart_lld.c \
+              ${CHIBIOS}/os/hal/platforms/STM32/i2c_lld.c \
               ${CHIBIOS}/os/hal/platforms/STM32/GPIOv2/pal_lld.c
 
 # Required include directories
diff --git a/os/hal/platforms/STM32L1xx/hal_lld.h b/os/hal/platforms/STM32L1xx/hal_lld.h
index b25bad51d..8d6cfd6f0 100644
--- a/os/hal/platforms/STM32L1xx/hal_lld.h
+++ b/os/hal/platforms/STM32L1xx/hal_lld.h
@@ -198,8 +198,22 @@
 
 /* I2C attributes.*/
 #define STM32_HAS_I2C1          TRUE
+#define STM32_I2C1_RX_DMA_MSK   (STM32_DMA_STREAM_ID_MSK(1, 7))
+#define STM32_I2C1_RX_DMA_CHN   0x00000000
+#define STM32_I2C1_TX_DMA_MSK   (STM32_DMA_STREAM_ID_MSK(1, 6))
+#define STM32_I2C1_TX_DMA_CHN   0x00000000
+
 #define STM32_HAS_I2C2          TRUE
+#define STM32_I2C2_RX_DMA_MSK   (STM32_DMA_STREAM_ID_MSK(1, 5))
+#define STM32_I2C2_RX_DMA_CHN   0x00000000
+#define STM32_I2C2_TX_DMA_MSK   (STM32_DMA_STREAM_ID_MSK(1, 4))
+#define STM32_I2C2_TX_DMA_CHN   0x00000000
+
 #define STM32_HAS_I2C3          FALSE
+#define STM32_I2C3_RX_DMA_MSK   0
+#define STM32_I2C3_RX_DMA_CHN   0x00000000
+#define STM32_I2C3_TX_DMA_MSK   0
+#define STM32_I2C3_TX_DMA_CHN   0x00000000
 
 /* RTC attributes.*/
 #define STM32_HAS_RTC           TRUE
diff --git a/os/hal/src/i2c.c b/os/hal/src/i2c.c
index 9ce2cc76f..f20932ef3 100644
--- a/os/hal/src/i2c.c
+++ b/os/hal/src/i2c.c
@@ -73,15 +73,9 @@ void i2cObjectInit(I2CDriver *i2cp) {
 
   i2cp->id_state  = I2C_STOP;
   i2cp->id_config = NULL;
-  i2cp->rxbuff_p = NULL;
-  i2cp->txbuff_p = NULL;
   i2cp->rxbuf = NULL;
   i2cp->txbuf = NULL;
-  i2cp->id_slave_config = NULL;
-
-#if I2C_USE_WAIT
   i2cp->id_thread   = NULL;
-#endif /* I2C_USE_WAIT */
 
 #if I2C_USE_MUTUAL_EXCLUSION
 #if CH_USE_MUTEXES
@@ -111,10 +105,6 @@ void i2cStart(I2CDriver *i2cp, const I2CConfig *config) {
               "i2cStart(), #1",
               "invalid state");
 
-#if (!(STM32_I2C_I2C2_USE_POLLING_WAIT) && I2C_SUPPORTS_CALLBACKS)
-    gptStart(i2cp->timer, i2cp->timer_cfg);
-#endif /* !(STM32_I2C_I2C2_USE_POLLING_WAIT) */
-
   chSysLock();
   i2cp->id_config = config;
   i2c_lld_start(i2cp);
@@ -136,10 +126,6 @@ void i2cStop(I2CDriver *i2cp) {
               "i2cStop(), #1",
               "invalid state");
 
-#if (!(STM32_I2C_I2C2_USE_POLLING_WAIT) && I2C_SUPPORTS_CALLBACKS)
-    gptStop(i2cp->timer);
-#endif /* !(STM32_I2C_I2C2_USE_POLLING_WAIT) */
-
   chSysLock();
   i2c_lld_stop(i2cp);
   i2cp->id_state = I2C_STOP;
@@ -153,35 +139,33 @@ void i2cStop(I2CDriver *i2cp) {
  *          paradigm. If you want transmit data without any further read,
  *          than set @b rxbytes field to 0.
  *
+ * @details Number of receiving byts must be 0 or more than 1 because of stm32
+ *          hardware restrictions.
+ *
  * @param[in] i2cp        pointer to the @p I2CDriver object
- * @param[in] i2cscfg     pointer to the @p I2C slave config
- * @param[in] slave_addr  Slave device address. Bits 0-9 contain slave
- *                        device address. Bit 15 must be set to 1 if 10-bit
- *                        addressing mode used. Otherwise	keep it cleared.
- *                        Bits 10-14 unused.
+ * @param[in] slave_addr  Slave device address (7 bits) without R/W bit
  * @param[in] txbuf       pointer to transmit buffer
  * @param[in] txbytes     number of bytes to be transmitted
  * @param[in] rxbuf       pointer to receive buffer
  * @param[in] rxbytes     number of bytes to be received, set it to 0 if
  *                        you want transmit only
+ *
+ * @return                Zero if no errors, otherwise return error code.
  */
-void i2cMasterTransmit(I2CDriver *i2cp,
-                      const I2CSlaveConfig *i2cscfg,
-                      uint16_t slave_addr,
+i2cflags_t i2cMasterTransmit(I2CDriver *i2cp,
+                      uint8_t slave_addr,
                       uint8_t *txbuf,
                       size_t txbytes,
                       uint8_t *rxbuf,
                       size_t rxbytes) {
 
-  chDbgCheck((i2cp != NULL) && (i2cscfg != NULL) &&\
+  chDbgCheck((i2cp != NULL) &&\
   		(slave_addr != 0) &&\
   		(txbytes > 0) &&\
-  		(txbuf != NULL),
+  		(txbuf != NULL) &&\
+  		((rxbytes == 0) || ((rxbytes > 1) && (rxbuf != NULL))),
   		"i2cMasterTransmit");
 
-  /* init slave config field in driver */
-  i2cp->id_slave_config = i2cscfg;
-
   i2c_lld_wait_bus_free(i2cp);
   chDbgAssert(!(i2c_lld_bus_is_busy(i2cp)), "i2cMasterReceive(), #1", "time is out");
 
@@ -190,40 +174,34 @@ void i2cMasterTransmit(I2CDriver *i2cp,
 
   i2cp->id_state = I2C_ACTIVE_TRANSMIT;
   i2c_lld_master_transmit(i2cp, slave_addr, txbuf, txbytes, rxbuf, rxbytes);
-#if I2C_SUPPORTS_CALLBACKS
   _i2c_wait_s(i2cp);
-#else
-  i2cp->id_state = I2C_READY;
-#endif /* I2C_SUPPORTS_CALLBACKS */
+
+  return i2cGetAndClearFlags(i2cp);
 }
 
 /**
  * @brief Receives data from the I2C bus.
+ * @details Number of receiving byts must be more than 1 because of stm32
+ *          hardware restrictions.
  *
  * @param[in] i2cp        pointer to the @p I2CDriver object
- * @param[in] i2cscfg     pointer to the @p I2C slave config
- * @param[in] slave_addr  Slave device address. Bits 0-9 contain slave
- *                        device address. Bit 15 must be set to 1 if 10-bit
- *                        addressing mode used. Otherwise	keep it cleared.
- *                        Bits 10-14 unused.
+ * @param[in] slave_addr  slave device address (7 bits) without R/W bit
  * @param[in] rxbytes     number of bytes to be received
  * @param[in] rxbuf       pointer to receive buffer
+ *
+ * @return                Zero if no errors, otherwise return error code.
  */
-void i2cMasterReceive(I2CDriver *i2cp,
-                      const I2CSlaveConfig *i2cscfg,
-                      uint16_t slave_addr,
+i2cflags_t i2cMasterReceive(I2CDriver *i2cp,
+                      uint8_t slave_addr,
                       uint8_t *rxbuf,
                       size_t rxbytes){
 
-  chDbgCheck((i2cp != NULL) && (i2cscfg != NULL) &&\
+  chDbgCheck((i2cp != NULL) &&\
   		(slave_addr != 0) &&\
-  		(rxbytes > 0) && \
+  		(rxbytes > 1) && \
   		(rxbuf != NULL),
       "i2cMasterReceive");
 
-  /* init slave config field in driver */
-  i2cp->id_slave_config = i2cscfg;
-
   i2c_lld_wait_bus_free(i2cp);
   chDbgAssert(!(i2c_lld_bus_is_busy(i2cp)), "i2cMasterReceive(), #1", "time is out");
 
@@ -232,20 +210,10 @@ void i2cMasterReceive(I2CDriver *i2cp,
 
   i2cp->id_state = I2C_ACTIVE_RECEIVE;
   i2c_lld_master_receive(i2cp, slave_addr, rxbuf, rxbytes);
-#if I2C_SUPPORTS_CALLBACKS
   _i2c_wait_s(i2cp);
-#else
-  i2cp->id_state = I2C_READY;
-#endif /* I2C_SUPPORTS_CALLBACKS */
-}
 
-
-/* FIXME: I do not know what this function must do. And can not test it
-uint16_t i2cSMBusAlertResponse(I2CDriver *i2cp, I2CSlaveConfig *i2cscfg) {
-  i2cMasterReceive(i2cp, i2cscfg);
-  return i2cp->id_slave_config->slave_addr;
+  return i2cGetAndClearFlags(i2cp);
 }
-*/
 
 /**
  * @brief   Handles communication events/errors.
@@ -262,7 +230,6 @@ void i2cAddFlagsI(I2CDriver *i2cp, i2cflags_t mask) {
   chDbgCheck(i2cp != NULL, "i2cAddFlagsI");
 
   i2cp->errors |= mask;
-  chEvtBroadcastI(&i2cp->sevent);
 }
 
 /**
@@ -323,7 +290,6 @@ void i2cReleaseBus(I2CDriver *i2cp) {
   chDbgCheck(i2cp != NULL, "i2cReleaseBus");
 
 #if CH_USE_MUTEXES
-  (void)i2cp;
   chMtxUnlock();
 #elif CH_USE_SEMAPHORES
   chSemSignal(&i2cp->id_semaphore);
diff --git a/testhal/STM32F1xx/I2C/Makefile b/testhal/STM32F1xx/I2C/Makefile
index dc2441200..08494928a 100644
--- a/testhal/STM32F1xx/I2C/Makefile
+++ b/testhal/STM32F1xx/I2C/Makefile
@@ -31,7 +31,7 @@ endif
 
 # Enable this if you want to see the full log while compiling.
 ifeq ($(USE_VERBOSE_COMPILE),)
-  USE_VERBOSE_COMPILE = no
+  USE_VERBOSE_COMPILE = yes
 endif
 
 #
@@ -63,7 +63,7 @@ LDSCRIPT= $(PORTLD)/STM32F103xB.ld
 
 # Imported source files
 CHIBIOS = ../../..
-include $(CHIBIOS)/boards/OLIMEX_STM32_P103/board.mk
+include $(CHIBIOS)/boards/OLIMEX_STM32_103STK/board.mk
 include $(CHIBIOS)/os/hal/platforms/STM32F1xx/platform.mk
 include $(CHIBIOS)/os/hal/hal.mk
 include $(CHIBIOS)/os/ports/GCC/ARMCMx/STM32F1xx/port.mk
@@ -82,9 +82,9 @@ CSRC = $(PORTSRC) \
        $(CHIBIOS)/os/various/syscalls.c \
        main.c \
        i2c_pns.c \
-       tmp75.c\
-  	   max1236.c\
-  	   lis3.c\
+  	   lis3.c \
+  	   tmp75.c \
+  	   fake.c
 
 	   
        
@@ -127,7 +127,6 @@ INCDIR = $(PORTINC) $(KERNINC) $(TESTINC) \
 # Compiler settings
 #
 
-# -lm �������� ������ �����, ������ ��� ������ ������
 MCU  = cortex-m3
 
 #TRGT = arm-elf-
diff --git a/testhal/STM32F1xx/I2C/fake.c b/testhal/STM32F1xx/I2C/fake.c
new file mode 100644
index 000000000..53779893e
--- /dev/null
+++ b/testhal/STM32F1xx/I2C/fake.c
@@ -0,0 +1,58 @@
+/*
+    ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010,
+                 2011 Giovanni Di Sirio.
+
+    This file is part of ChibiOS/RT.
+
+    ChibiOS/RT is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 3 of the License, or
+    (at your option) any later version.
+
+    ChibiOS/RT is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * Not responding slave test
+ */
+
+#include <stdlib.h>
+
+#include "ch.h"
+#include "hal.h"
+
+#include "fake.h"
+
+
+/* input buffer */
+static uint8_t rx_data[2];
+
+/* temperature value */
+static int16_t temperature = 0;
+
+
+#define addr 0b1001100
+
+/* This is main function. */
+void request_fake(void){
+  i2cflags_t errors = 0;
+
+  i2cAcquireBus(&I2CD1);
+  errors = i2cMasterReceive(&I2CD1, addr, rx_data, 2);
+  i2cReleaseBus(&I2CD1);
+
+  if (errors == I2CD_ACK_FAILURE){
+    __NOP();
+  }
+  else{
+    temperature = (rx_data[0] << 8) + rx_data[1];
+  }
+}
+
+
diff --git a/testhal/STM32F1xx/I2C/fake.h b/testhal/STM32F1xx/I2C/fake.h
new file mode 100644
index 000000000..04f4206bf
--- /dev/null
+++ b/testhal/STM32F1xx/I2C/fake.h
@@ -0,0 +1,26 @@
+/*
+    ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010,
+                 2011 Giovanni Di Sirio.
+
+    This file is part of ChibiOS/RT.
+
+    ChibiOS/RT is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 3 of the License, or
+    (at your option) any later version.
+
+    ChibiOS/RT is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef FAKE_H_
+#define FAKE_H_
+
+void request_fake(void);
+
+#endif /* FAKE_H_ */
diff --git a/testhal/STM32F1xx/I2C/halconf.h b/testhal/STM32F1xx/I2C/halconf.h
index 31d581932..54a69c2f7 100644
--- a/testhal/STM32F1xx/I2C/halconf.h
+++ b/testhal/STM32F1xx/I2C/halconf.h
@@ -44,7 +44,7 @@
  * @brief   Enables the ADC subsystem.
  */
 #if !defined(HAL_USE_ADC) || defined(__DOXYGEN__)
-#define HAL_USE_ADC                 TRUE
+#define HAL_USE_ADC                 FALSE
 #endif
 
 /**
@@ -100,7 +100,7 @@
  * @brief   Enables the PWM subsystem.
  */
 #if !defined(HAL_USE_PWM) || defined(__DOXYGEN__)
-#define HAL_USE_PWM                 TRUE
+#define HAL_USE_PWM                 FALSE
 #endif
 
 /**
@@ -121,7 +121,7 @@
  * @brief   Enables the SERIAL subsystem.
  */
 #if !defined(HAL_USE_SERIAL) || defined(__DOXYGEN__)
-#define HAL_USE_SERIAL              TRUE
+#define HAL_USE_SERIAL              FALSE
 #endif
 
 /**
@@ -192,7 +192,7 @@
  * @note    Disabling this option saves both code and data space.
  */
 #if !defined(I2C_USE_WAIT) || defined(__DOXYGEN__)
-#define I2C_USE_WAIT                FALSE
+#define I2C_USE_WAIT                TRUE
 #endif
 
 /**
@@ -202,13 +202,6 @@
 #define I2C_USE_MUTUAL_EXCLUSION    TRUE
 #endif
 
-/**
- * @brief   Switch to asynchronouse driver with callbacks.
- */
-#if !defined(I2C_SUPPORTS_CALLBACKS) || defined(__DOXYGEN__)
-#define I2C_SUPPORTS_CALLBACKS      TRUE
-#endif
-
 /*===========================================================================*/
 /* MAC driver related settings.                                              */
 /*===========================================================================*/
diff --git a/testhal/STM32F1xx/I2C/i2c_pns.c b/testhal/STM32F1xx/I2C/i2c_pns.c
index 44f4a8a33..1c73482e3 100644
--- a/testhal/STM32F1xx/I2C/i2c_pns.c
+++ b/testhal/STM32F1xx/I2C/i2c_pns.c
@@ -1,32 +1,35 @@
+/*
+    ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010,
+                 2011 Giovanni Di Sirio.
+
+    This file is part of ChibiOS/RT.
+
+    ChibiOS/RT is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 3 of the License, or
+    (at your option) any later version.
+
+    ChibiOS/RT is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
 #include "ch.h"
 #include "hal.h"
 
 #include "i2c_pns.h"
-
 #include "lis3.h"
-#include "tmp75.h"
-#include "max1236.h"
+
 
 /* I2C1 */
 static const I2CConfig i2cfg1 = {
     OPMODE_I2C,
-    100000,
-    STD_DUTY_CYCLE,
-    0,
-    0,
-    0,
-    0,
-};
-
-/* I2C2 */
-static const I2CConfig i2cfg2 = {
-    OPMODE_I2C,
-    100000,
-    STD_DUTY_CYCLE,
-    0,
-    0,
-    0,
-    0,
+    400000,
+    FAST_DUTY_CYCLE_16_9,
 };
 
 
@@ -35,22 +38,14 @@ void I2CInit_pns(void){
   i2cInit();
 
   i2cStart(&I2CD1, &i2cfg1);
-  i2cStart(&I2CD2, &i2cfg2);
 
   /* tune ports for I2C1*/
   palSetPadMode(IOPORT2, 6, PAL_MODE_STM32_ALTERNATE_OPENDRAIN);
   palSetPadMode(IOPORT2, 7, PAL_MODE_STM32_ALTERNATE_OPENDRAIN);
 
-  /* tune ports for I2C2*/
-  palSetPadMode(IOPORT2, 10, PAL_MODE_STM32_ALTERNATE_OPENDRAIN);
-  palSetPadMode(IOPORT2, 11, PAL_MODE_STM32_ALTERNATE_OPENDRAIN);
-
   /* startups. Pauses added just to be safe */
-  chThdSleepMilliseconds(1000);
-  init_max1236();
-  chThdSleepMilliseconds(1000);
+  chThdSleepMilliseconds(100);
   init_lis3();
-  chThdSleepMilliseconds(1000);
 }
 
 
diff --git a/testhal/STM32F1xx/I2C/i2c_pns.h b/testhal/STM32F1xx/I2C/i2c_pns.h
index 4dfdf320e..dae359ecf 100644
--- a/testhal/STM32F1xx/I2C/i2c_pns.h
+++ b/testhal/STM32F1xx/I2C/i2c_pns.h
@@ -1,3 +1,23 @@
+/*
+    ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010,
+                 2011 Giovanni Di Sirio.
+
+    This file is part of ChibiOS/RT.
+
+    ChibiOS/RT is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 3 of the License, or
+    (at your option) any later version.
+
+    ChibiOS/RT is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
 #ifndef I2C_PNS_H_
 #define I2C_PNS_H_
 
diff --git a/testhal/STM32F1xx/I2C/lis3.c b/testhal/STM32F1xx/I2C/lis3.c
index 401f56199..d3369ac82 100644
--- a/testhal/STM32F1xx/I2C/lis3.c
+++ b/testhal/STM32F1xx/I2C/lis3.c
@@ -1,7 +1,26 @@
+/*
+    ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010,
+                 2011 Giovanni Di Sirio.
+
+    This file is part of ChibiOS/RT.
+
+    ChibiOS/RT is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 3 of the License, or
+    (at your option) any later version.
+
+    ChibiOS/RT is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
 /**
- * This is most complex and difficult device.
- * It realize "read through write" paradigm. This is not standard, but
- * most of I2C devices use this paradigm.
+ * This is device realize "read through write" paradigm. This is not
+ * standard, but most of I2C devices use this paradigm.
  * You must write to device reading address, send restart to bus,
  * and then begin reading process.
  */
@@ -18,34 +37,13 @@
 
 
 /* buffers */
-static i2cblock_t accel_rx_data[ACCEL_RX_DEPTH];
-static i2cblock_t accel_tx_data[ACCEL_TX_DEPTH];
+static uint8_t accel_rx_data[ACCEL_RX_DEPTH];
+static uint8_t accel_tx_data[ACCEL_TX_DEPTH];
 
 static int16_t acceleration_x = 0;
 static int16_t acceleration_y = 0;
 static int16_t acceleration_z = 0;
 
-/* Error trap */
-static void i2c_lis3_error_cb(I2CDriver *i2cp, const I2CSlaveConfig *i2cscfg){
-  (void)i2cscfg;
-  int status = 0;
-  status = i2cp->id_i2c->SR1;
-  while(TRUE);
-}
-
-/* This callback raise up when transfer finished */
-static void i2c_lis3_cb(I2CDriver *i2cp, const I2CSlaveConfig *i2cscfg){
-  (void)i2cp;
-  (void)i2cscfg;
-}
-
-
-/* Accelerometer lis3lv02dq config */
-static const I2CSlaveConfig lis3 = {
-	i2c_lis3_cb,
-  i2c_lis3_error_cb,
-};
-
 
 /**
  * Init function. Here we will also start personal serving thread.
@@ -58,7 +56,9 @@ int init_lis3(void){
   accel_tx_data[3] = 0b00000000;
 
   /* sending */
-  i2cMasterTransmit(&I2CD1, &lis3, lis3_addr, accel_tx_data, 4, accel_rx_data, 0);
+  i2cAcquireBus(&I2CD1);
+  i2cMasterTransmit(&I2CD1, lis3_addr, accel_tx_data, 4, accel_rx_data, 0);
+  i2cReleaseBus(&I2CD1);
   return 0;
 }
 
@@ -67,9 +67,9 @@ int init_lis3(void){
  */
 void request_acceleration_data(void){
   accel_tx_data[0] = ACCEL_OUT_DATA | AUTO_INCREMENT_BIT; // register address
-  //i2cAcquireBus(&I2CD1);
-  i2cMasterTransmit(&I2CD1, &lis3, lis3_addr, accel_tx_data, 1, accel_rx_data, 6);
-  //i2cReleaseBus(&I2CD1);
+  i2cAcquireBus(&I2CD1);
+  i2cMasterTransmit(&I2CD1, lis3_addr, accel_tx_data, 1, accel_rx_data, 6);
+  i2cReleaseBus(&I2CD1);
 
   acceleration_x = accel_rx_data[0] + (accel_rx_data[1] << 8);
   acceleration_y = accel_rx_data[2] + (accel_rx_data[3] << 8);
diff --git a/testhal/STM32F1xx/I2C/lis3.h b/testhal/STM32F1xx/I2C/lis3.h
index e50359bde..0c2eefa5d 100644
--- a/testhal/STM32F1xx/I2C/lis3.h
+++ b/testhal/STM32F1xx/I2C/lis3.h
@@ -1,3 +1,23 @@
+/*
+    ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010,
+                 2011 Giovanni Di Sirio.
+
+    This file is part of ChibiOS/RT.
+
+    ChibiOS/RT is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 3 of the License, or
+    (at your option) any later version.
+
+    ChibiOS/RT is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
 #include <stdlib.h>
 #include "ch.h"
 
diff --git a/testhal/STM32F1xx/I2C/main.c b/testhal/STM32F1xx/I2C/main.c
index b828953c5..60cb1f3ae 100644
--- a/testhal/STM32F1xx/I2C/main.c
+++ b/testhal/STM32F1xx/I2C/main.c
@@ -1,21 +1,22 @@
-/**
- * Lets imagine that we have board with LIS3LV02DL accelerometer on channel #1
- * and MAX1236 ADC, TMP75 thermometer on channel #2.
- *
- * NOTE: I assume, that you have datasheets on all this stuff.
- *
- * NOTE: Also, I assume, that you know how to I2C works.
- *
- * In order from simplicity to complexity:
- *   TMP75
- *   MAX1236
- *   LIS3LV02DL
- *
- * Project splitted to separate source files for each device.
- *
- * Data from sensors we will be read from different thread sleeping different
- * amount of time.
- */
+/*
+    ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010,
+                 2011 Giovanni Di Sirio.
+
+    This file is part of ChibiOS/RT.
+
+    ChibiOS/RT is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 3 of the License, or
+    (at your option) any later version.
+
+    ChibiOS/RT is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
 
 #include <stdlib.h>
 
@@ -23,10 +24,9 @@
 #include "hal.h"
 
 #include "i2c_pns.h"
-#include "tmp75.h"
-#include "max1236.h"
 #include "lis3.h"
-
+#include "tmp75.h"
+#include "fake.h"
 
 
 /*
@@ -44,55 +44,51 @@ static msg_t Blink(void *arg) {
   return 0;
 }
 
+/*
+ * Accelerometer thread
+ */
+static WORKING_AREA(PollAccelThreadWA, 128);
+static msg_t PollAccelThread(void *arg) {
+  chRegSetThreadName("PollAccel");
+  (void)arg;
+  while (TRUE) {
+//    chThdSleepMilliseconds(rand() & 31);
+    chThdSleepMilliseconds(32);
+    request_acceleration_data();
+  }
+  return 0;
+}
 
 
 /* Temperature polling thread */
 static WORKING_AREA(PollTmp75ThreadWA, 128);
 static msg_t PollTmp75Thread(void *arg) {
+  chRegSetThreadName("PollTmp75");
   (void)arg;
-  systime_t time = chTimeNow();
-
   while (TRUE) {
-    time += MS2ST(1001);
+//    chThdSleepMilliseconds(rand() & 31);
+    chThdSleepMilliseconds(15);
     /* Call reading function */
     request_temperature();
-    chThdSleepUntil(time);
   }
   return 0;
 }
 
-/* MAX1236 polling thread */
-static WORKING_AREA(PollMax1236ThreadWA, 128);
-static msg_t PollMax1236Thread(void *arg) {
-  (void)arg;
-  systime_t time = chTimeNow();
 
+/* Temperature polling thread */
+static WORKING_AREA(PollFakeThreadWA, 128);
+static msg_t PollFakeThread(void *arg) {
+  chRegSetThreadName("PollFake");
+  (void)arg;
   while (TRUE) {
-    time += MS2ST(200);
+    chThdSleepMilliseconds(16);
     /* Call reading function */
-    read_max1236();
-    chThdSleepUntil(time);
+    request_fake();
   }
   return 0;
 }
 
 
-static WORKING_AREA(PollAccelThreadWA, 128);
-static msg_t PollAccelThread(void *arg) {
-  (void)arg;
-  systime_t time = chTimeNow();
-
-  while (TRUE) {
-    time += MS2ST(20);
-    request_acceleration_data();
-    chThdSleepUntil(time);
-  }
-  return 0;
-}
-
-
-
-
 /*
  * Entry point, note, the main() function is already a thread in the system
  * on entry.
@@ -102,8 +98,16 @@ int main(void) {
   halInit();
   chSysInit();
 
+  chThdSleepMilliseconds(200);
   I2CInit_pns();
 
+  /* Create accelerometer thread */
+  chThdCreateStatic(PollAccelThreadWA,
+          sizeof(PollAccelThreadWA),
+          NORMALPRIO,
+          PollAccelThread,
+          NULL);
+
   /* Create temperature thread */
   chThdCreateStatic(PollTmp75ThreadWA,
           sizeof(PollTmp75ThreadWA),
@@ -111,24 +115,15 @@ int main(void) {
           PollTmp75Thread,
           NULL);
 
-
-  /* Create max1236 thread */
-  chThdCreateStatic(PollMax1236ThreadWA,
-          sizeof(PollMax1236ThreadWA),
+  /* Create not responding thread */
+  chThdCreateStatic(PollFakeThreadWA,
+          sizeof(PollFakeThreadWA),
           NORMALPRIO,
-          PollMax1236Thread,
-          NULL);
-
-
-  /* Create accelerometer thread */
-  chThdCreateStatic(PollAccelThreadWA,
-          sizeof(PollAccelThreadWA),
-          HIGHPRIO,
-          PollAccelThread,
+          PollFakeThread,
           NULL);
 
   /* Creates the blinker thread. */
-  chThdCreateStatic(BlinkWA, sizeof(BlinkWA), LOWPRIO, Blink, NULL);
+  chThdCreateStatic(BlinkWA, sizeof(BlinkWA), HIGHPRIO, Blink, NULL);
 
   /* main loop that do nothing */
   while (TRUE) {
diff --git a/testhal/STM32F1xx/I2C/main.h b/testhal/STM32F1xx/I2C/main.h
deleted file mode 100644
index 1435a05e5..000000000
--- a/testhal/STM32F1xx/I2C/main.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*
- * main.h
- *
- *  Created on: 25.03.2011
- *      Author: barthess
- */
-
-#ifndef MAIN_H_
-#define MAIN_H_
-
-
-// ãëîáàëüíûå ôëàãè
-#define GET_FILTERED_RAW_GYRO TRUE
-#define GET_FILTERED_RAW_ACCEL TRUE
-
-
-
-
-#endif /* MAIN_H_ */
diff --git a/testhal/STM32F1xx/I2C/max1236.c b/testhal/STM32F1xx/I2C/max1236.c
deleted file mode 100644
index 09e2c8b35..000000000
--- a/testhal/STM32F1xx/I2C/max1236.c
+++ /dev/null
@@ -1,75 +0,0 @@
-/**
- * Maxim ADC has not so suitable default settings after startup.
- * So we will create init function to tune this ADC.
- */
-
-#include <stdlib.h>
-
-#include "ch.h"
-#include "hal.h"
-
-#include "max1236.h"
-
-
-#define max1236_addr 0b0110100
-
-
-/* Data buffers */
-static i2cblock_t max1236_rx_data[MAX1236_RX_DEPTH];
-static i2cblock_t max1236_tx_data[MAX1236_TX_DEPTH];
-/* ADC results */
-static uint16_t ch1 = 0, ch2 = 0, ch3 = 0, ch4 = 0;
-
-
-/* Error trap */
-static void i2c_max1236_error_cb(I2CDriver *i2cp, const I2CSlaveConfig *i2cscfg){
-  (void)i2cscfg;
-  int status = 0;
-  status = i2cp->id_i2c->SR1;
-  while(TRUE);
-}
-
-
-/* This callback raise up when transfer finished */
-static void i2c_max1236_cb(I2CDriver *i2cp, const I2CSlaveConfig *i2cscfg){
-  (void)*i2cp;
-  (void)*i2cscfg;
-  /* get ADC data */
-}
-
-
-/* ADC maxim MAX1236 config */
-
-static const I2CSlaveConfig max1236 = {
-		i2c_max1236_cb,
-    i2c_max1236_error_cb,
-};
-
-
-/**
- * Initilization routine. See datasheet on page 13 to understand
- * how to initialize ADC.
- */
-void init_max1236(void){
-  /* this data we must send via IC to setup ADC */
-  max1236_tx_data[0] = 0b10000011; /* config register content. Consult datasheet */
-  max1236_tx_data[1] = 0b00000111; /* config register content. Consult datasheet */
-
-  /* transmit out 2 bytes */
-  i2cAcquireBus(&I2CD2);
-  i2cMasterTransmit(&I2CD2, &max1236, max1236_addr, max1236_tx_data, 2, max1236_rx_data, 0);
-  i2cReleaseBus(&I2CD2);
-}
-
-
-/* Now simply read 8 bytes to get all 4 ADC channels */
-void read_max1236(void){
-  i2cAcquireBus(&I2CD2);
-  i2cMasterReceive(&I2CD2, &max1236, max1236_addr, max1236_rx_data, 8);
-  i2cReleaseBus(&I2CD2);
-
-  ch1 = ((max1236_rx_data[0] & 0xF) << 8) + max1236_rx_data[1];
-  ch2 = ((max1236_rx_data[2] & 0xF) << 8) + max1236_rx_data[3];
-  ch3 = ((max1236_rx_data[4] & 0xF) << 8) + max1236_rx_data[5];
-  ch4 = ((max1236_rx_data[6] & 0xF) << 8) + max1236_rx_data[7];
-}
diff --git a/testhal/STM32F1xx/I2C/max1236.h b/testhal/STM32F1xx/I2C/max1236.h
deleted file mode 100644
index aff466cf4..000000000
--- a/testhal/STM32F1xx/I2C/max1236.h
+++ /dev/null
@@ -1,14 +0,0 @@
-#include "ch.h"
-
-#ifndef MAX1236_H_
-#define MAX1236_H_
-
-
-#define MAX1236_RX_DEPTH 8
-#define MAX1236_TX_DEPTH 2
-
-
-void init_max1236(void);
-void read_max1236(void);
-
-#endif /* MAX1236_H_ */
diff --git a/testhal/STM32F1xx/I2C/mcuconf.h b/testhal/STM32F1xx/I2C/mcuconf.h
index 6afc97f28..dc4d3960c 100644
--- a/testhal/STM32F1xx/I2C/mcuconf.h
+++ b/testhal/STM32F1xx/I2C/mcuconf.h
@@ -47,7 +47,7 @@
 /*
  * ADC driver system settings.
  */
-#define STM32_ADC_USE_ADC1                  TRUE
+#define STM32_ADC_USE_ADC1                  FALSE
 #define STM32_ADC_ADC1_DMA_PRIORITY         3
 #define STM32_ADC_ADC1_IRQ_PRIORITY         5
 #define STM32_ADC_ADC1_DMA_ERROR_HOOK()     chSysHalt()
@@ -122,8 +122,8 @@
 /*
  * SERIAL driver system settings.
  */
-#define STM32_SERIAL_USE_USART1             TRUE
-#define STM32_SERIAL_USE_USART2             TRUE
+#define STM32_SERIAL_USE_USART1             FALSE
+#define STM32_SERIAL_USE_USART2             FALSE
 #define STM32_SERIAL_USE_USART3             FALSE
 #define STM32_SERIAL_USE_UART4              FALSE
 #define STM32_SERIAL_USE_UART5              FALSE
@@ -138,8 +138,8 @@
 /*
  * SPI driver system settings.
  */
-#define STM32_SPI_USE_SPI1                  TRUE
-#define STM32_SPI_USE_SPI2                  TRUE
+#define STM32_SPI_USE_SPI1                  FALSE
+#define STM32_SPI_USE_SPI2                  FALSE
 #define STM32_SPI_USE_SPI3                  FALSE
 #define STM32_SPI_SPI1_DMA_PRIORITY         1
 #define STM32_SPI_SPI2_DMA_PRIORITY         1
@@ -167,25 +167,18 @@
  * I2C driver system settings.
  */
 #define STM32_I2C_USE_I2C1                  TRUE
-#define STM32_I2C_USE_I2C2                  TRUE
+#define STM32_I2C_USE_I2C2                  FALSE
 #define STM32_I2C_I2C1_IRQ_PRIORITY         10
 #define STM32_I2C_I2C2_IRQ_PRIORITY         10
-#define STM32_I2C_I2C1_DMA_PRIORITY         4
-#define STM32_I2C_I2C2_DMA_PRIORITY         4
+#define STM32_I2C_I2C1_DMA_PRIORITY         1
+#define STM32_I2C_I2C2_DMA_PRIORITY         1
 #define STM32_I2C_I2C1_DMA_ERROR_HOOK()     chSysHalt()
 #define STM32_I2C_I2C2_DMA_ERROR_HOOK()     chSysHalt()
-/* I2C1 */
-#define STM32_I2C_I2C1_USE_GPT_TIM          GPTD1
-#define STM32_I2C_I2C1_USE_POLLING_WAIT     TRUE
-/* I2C2 */
-#define STM32_I2C_I2C2_USE_GPT_TIM          GPTD2
-#define STM32_I2C_I2C2_USE_POLLING_WAIT     TRUE
-
 
 /*
  * USB driver system settings.
  */
-#define STM32_USB_USE_USB1                  TRUE
+#define STM32_USB_USE_USB1                  FALSE
 #define STM32_USB_LOW_POWER_ON_SUSPEND      FALSE
 #define STM32_USB_USB1_HP_IRQ_PRIORITY      6
 #define STM32_USB_USB1_LP_IRQ_PRIORITY      14
diff --git a/testhal/STM32F1xx/I2C/tmp75.c b/testhal/STM32F1xx/I2C/tmp75.c
index 72e634527..630a76dac 100644
--- a/testhal/STM32F1xx/I2C/tmp75.c
+++ b/testhal/STM32F1xx/I2C/tmp75.c
@@ -1,3 +1,23 @@
+/*
+    ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010,
+                 2011 Giovanni Di Sirio.
+
+    This file is part of ChibiOS/RT.
+
+    ChibiOS/RT is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 3 of the License, or
+    (at your option) any later version.
+
+    ChibiOS/RT is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
 /**
  * TMP75 is most simple I2C device in our case. It is already useful with
  * default settings after powerup.
@@ -13,40 +33,25 @@
 
 
 /* input buffer */
-static i2cblock_t tmp75_rx_data[TMP75_RX_DEPTH];
+static uint8_t tmp75_rx_data[TMP75_RX_DEPTH];
 
 /* temperature value */
 static int16_t temperature = 0;
 
-/* Simple error trap */
-static void i2c_tmp75_error_cb(I2CDriver *i2cp, const I2CSlaveConfig *i2cscfg){
-  (void)i2cscfg;
-  int status = 0;
-  status = i2cp->id_i2c->SR1;
-  while(TRUE);
-}
-
-/* This callback raise up when transfer finished */
-static void i2c_tmp75_cb(I2CDriver *i2cp, const I2CSlaveConfig *i2cscfg){
-  (void)*i2cp;
-  (void)*i2cscfg;
-  /* store temperature value */
-}
-
-/* Fill TMP75 config. */
-static const I2CSlaveConfig tmp75 = {
-    i2c_tmp75_cb,
-    i2c_tmp75_error_cb,
-};
 
 #define tmp75_addr 0b1001000
 
 /* This is main function. */
 void request_temperature(void){
-  i2cAcquireBus(&I2CD2);
-  i2cMasterReceive(&I2CD2, &tmp75, tmp75_addr, tmp75_rx_data, 2);
-  i2cReleaseBus(&I2CD2);
-  temperature = (tmp75_rx_data[0] << 8) + tmp75_rx_data[1];
+  int16_t t_int = 0, t_frac = 0;
+
+  i2cAcquireBus(&I2CD1);
+  i2cMasterReceive(&I2CD1, tmp75_addr, tmp75_rx_data, 2);
+  i2cReleaseBus(&I2CD1);
+
+  t_int = tmp75_rx_data[0] * 100;
+  t_frac = (tmp75_rx_data[1] * 100) >> 8;
+  temperature = t_int + t_frac;
 }
 
 
diff --git a/testhal/STM32F1xx/I2C/tmp75.h b/testhal/STM32F1xx/I2C/tmp75.h
index ab4b5fa9b..13648b154 100644
--- a/testhal/STM32F1xx/I2C/tmp75.h
+++ b/testhal/STM32F1xx/I2C/tmp75.h
@@ -1,3 +1,23 @@
+/*
+    ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010,
+                 2011 Giovanni Di Sirio.
+
+    This file is part of ChibiOS/RT.
+
+    ChibiOS/RT is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 3 of the License, or
+    (at your option) any later version.
+
+    ChibiOS/RT is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
 #ifndef TMP75_H_
 #define TMP75_H_