diff --git a/boards/arduino-atmega-common/include/periph_conf.h b/boards/arduino-atmega-common/include/periph_conf.h
index 12043bfa661e75aae01ee51918301864a90a78dd..71b69036938ab9f6afef31c9dfe256217f6b2d3c 100644
--- a/boards/arduino-atmega-common/include/periph_conf.h
+++ b/boards/arduino-atmega-common/include/periph_conf.h
@@ -144,12 +144,9 @@ extern "C" {
 /** @} */
 
 /**
- * @name    I2C configuration
-  * @{
+ * @brief    I2C configuration
  */
 #define I2C_NUMOF           1
-#define I2C_0_EN            1
-#define I2C_IRQ_PRIO        1
 
 #ifdef __cplusplus
 }
diff --git a/boards/waspmote-pro/include/periph_conf.h b/boards/waspmote-pro/include/periph_conf.h
index 25f06d7ffad27d5500b80a1ae4968f45f1267e4b..0caa7548cf78e43fd185f483c2554eac71016b80 100644
--- a/boards/waspmote-pro/include/periph_conf.h
+++ b/boards/waspmote-pro/include/periph_conf.h
@@ -105,12 +105,9 @@ extern "C" {
 /** @} */
 
 /**
- * @name    I2C configuration
-  * @{
+ * @brief    I2C configuration
  */
 #define I2C_NUMOF           1
-#define I2C_0_EN            1
-#define I2C_IRQ_PRIO        1
 
 #ifdef __cplusplus
 }
diff --git a/cpu/atmega1281/include/periph_cpu.h b/cpu/atmega1281/include/periph_cpu.h
index 3d7c73861e10606bc4e415994a729137addd117a..3d74de0ec46ee7a664b67e75996ecaef230a53b3 100644
--- a/cpu/atmega1281/include/periph_cpu.h
+++ b/cpu/atmega1281/include/periph_cpu.h
@@ -42,6 +42,15 @@ enum {
     PORT_G = 6,       /**< port G */
 };
 
+/**
+ * @name   Defines for the I2C interface
+ * @{
+ */
+#define I2C_PORT_REG            PORTD
+#define I2C_PIN_MASK            (1 << PORTD0) | (1 << PORTD1)
+#define I2C_POWER_REG           PRR0
+/** @} */
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/cpu/atmega2560/include/periph_cpu.h b/cpu/atmega2560/include/periph_cpu.h
index 719eb9a7c06e3ee30b03e16d01ba760e778003e9..83257818d68ee3299e68edc29f37fbb69ffc798c 100644
--- a/cpu/atmega2560/include/periph_cpu.h
+++ b/cpu/atmega2560/include/periph_cpu.h
@@ -44,6 +44,15 @@ enum {
     PORT_L = 10       /**< port L */
 };
 
+/**
+ * @name   Defines for the I2C interface
+ * @{
+ */
+#define I2C_PORT_REG            PORTD
+#define I2C_PIN_MASK            (1 << PORTD0) | (1 << PORTD1)
+#define I2C_POWER_REG           PRR0
+/** @} */
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/cpu/atmega328p/include/periph_cpu.h b/cpu/atmega328p/include/periph_cpu.h
index 2a7ff88f9f5952723442da084a953f51fefba87e..3555d5b0ef7ff7a5a087f58e31643ab9dde287f8 100644
--- a/cpu/atmega328p/include/periph_cpu.h
+++ b/cpu/atmega328p/include/periph_cpu.h
@@ -41,6 +41,15 @@ enum {
     PORT_D = 3       /**< port D */
 };
 
+/**
+ * @name   Defines for the I2C interface
+ * @{
+ */
+#define I2C_PORT_REG            PORTC
+#define I2C_PIN_MASK            (1 << PORTC4) | (1 << PORTC5)
+#define I2C_POWER_REG           PRR
+/** @} */
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/cpu/atmega_common/periph/i2c.c b/cpu/atmega_common/periph/i2c.c
index fc7509f63f563e30a4007dd2b2c4e03e067b9ef6..92c1dad11630855032c26246a9fd12cb2398594c 100644
--- a/cpu/atmega_common/periph/i2c.c
+++ b/cpu/atmega_common/periph/i2c.c
@@ -7,13 +7,14 @@
  */
 
 /**
- * @addtogroup  driver_periph
+ * @ingroup cpu_atmega_common
+ * @ingroup drivers_periph_i2c
  * @{
  *
  * @file
  * @brief       Low-level I2C driver implementation fot atmega common
  *
- * @note This implementation only implements the 7-bit addressing mode.
+ * @note        This implementation only implements the 7-bit addressing mode.
  *
  * @author      Dimitri Nahm <dimitri.nahm@haw-hamburg.de>
  *
@@ -24,38 +25,35 @@
 
 #include "cpu.h"
 #include "mutex.h"
+#include "assert.h"
 #include "periph/i2c.h"
 #include "periph_conf.h"
 
-#define ENABLE_DEBUG    (0)
 #include "debug.h"
-
-#define MT_START 0x08
-#define MT_ADDRESS_ACK 0x18
-#define MT_DATA_ACK 0x28
-#define MR_ADDRESS_ACK 0x40
+#define ENABLE_DEBUG      (0)
 
 /* guard file in case no I2C device is defined */
 #if I2C_NUMOF
 
+#define MT_START            0x08
+#define MT_START_REPEATED   0x10
+#define MT_ADDRESS_ACK      0x18
+#define MT_DATA_ACK         0x28
+#define MR_ADDRESS_ACK      0x40
+
 /* static function definitions */
 static int _start(uint8_t address, uint8_t rw_flag);
 static int _write(const uint8_t *data, int length);
 static void _stop(void);
 
-/**
- * @brief Array holding one pre-initialized mutex for each I2C device
- */
-static mutex_t locks[] =  {
-#if I2C_0_EN
-    [I2C_0] = MUTEX_INIT,
-#endif
-};
+static mutex_t lock = MUTEX_INIT;
 
 int i2c_init_master(i2c_t dev, i2c_speed_t speed)
 {
-    /* TWI Bit Rate Register - division factor for the bit rate generator*/
-    int twibrr;
+    /* TWI Bit Rate Register - division factor for the bit rate generator */
+    uint8_t twibrr;
+    /* TWI Prescaler Bits - default 0 */
+    uint8_t twipb = 0;
 
     /* check if the line is valid */
     if (dev >= I2C_NUMOF) {
@@ -64,25 +62,49 @@ int i2c_init_master(i2c_t dev, i2c_speed_t speed)
 
     /* calculate speed configuration */
     switch (speed) {
+
+        case I2C_SPEED_LOW:
+            if (CLOCK_CORECLOCK > 20000000U || CLOCK_CORECLOCK < 1000000U) {
+                return -2;
+            }
+            twibrr = ((CLOCK_CORECLOCK/10000)-16)/(2*4);  // CLK Prescaler 4
+            twipb = 1;
+            break;
+
         case I2C_SPEED_NORMAL:
+            if (CLOCK_CORECLOCK > 50000000U || CLOCK_CORECLOCK < 2000000U) {
+                return -2;
+            }
             twibrr = ((CLOCK_CORECLOCK/100000)-16)/2;
             break;
 
         case I2C_SPEED_FAST:
+            if (CLOCK_CORECLOCK < 7500000U) {
+                return -2;
+            }
             twibrr = ((CLOCK_CORECLOCK/400000)-16)/2;
             break;
 
+        case I2C_SPEED_FAST_PLUS:
+            if (CLOCK_CORECLOCK < 18000000U) {
+                return -2;
+            }
+            twibrr = ((CLOCK_CORECLOCK/1000000)-16)/2;
+            break;
+
+        case I2C_SPEED_HIGH:
+            if (CLOCK_CORECLOCK < 62000000U) {
+                return -2;
+            }
+            twibrr = ((CLOCK_CORECLOCK/3400000)-16)/2;
+            break;
+
         default:
             return -2;
     }
 
     /* set pull-up on SCL and SDA */
-    #if defined (CPU_ATMEGA2560) || defined (CPU_ATMEGA1281)
-        PORTD |= (1 << PORTD0) | (1 << PORTD1);
-    #endif
-    #ifdef CPU_ATMEGA328P
-        PORTC |= (1 << PORTC4) | (1 << PORTC5);
-    #endif
+    I2C_PORT_REG |= (I2C_PIN_MASK);
 
     /* enable I2C clock */
     i2c_poweron(dev);
@@ -90,7 +112,9 @@ int i2c_init_master(i2c_t dev, i2c_speed_t speed)
     /* disable device */
     TWCR &= ~(1 << TWEN);
     /* configure I2C clock */
-    TWBR = twibrr;
+    TWBR = twibrr;                // Set TWI Bit Rate Register
+    TWSR &= ~(0x03);              // Reset TWI Prescaler Bits
+    TWSR |= twipb;                // Set TWI Prescaler Bits
     /* enable device */
     TWCR |= (1 << TWEN);
 
@@ -99,19 +123,15 @@ int i2c_init_master(i2c_t dev, i2c_speed_t speed)
 
 int i2c_acquire(i2c_t dev)
 {
-    if (dev >= I2C_NUMOF) {
-        return -1;
-    }
-    mutex_lock(&locks[dev]);
+    assert(dev < I2C_NUMOF);
+    mutex_lock(&lock);
     return 0;
 }
 
 int i2c_release(i2c_t dev)
 {
-    if (dev >= I2C_NUMOF) {
-        return -1;
-    }
-    mutex_unlock(&locks[dev]);
+    assert(dev < I2C_NUMOF);
+    mutex_unlock(&lock);
     return 0;
 }
 
@@ -124,24 +144,24 @@ int i2c_read_bytes(i2c_t dev, uint8_t address, void *data, int length)
 {
     uint8_t *my_data = data;
 
-    if ((unsigned int)dev >= I2C_NUMOF) {
-        return -1;
-    }
+    assert((dev < I2C_NUMOF) && (length > 0));
 
     /* send start condition and slave address */
-    if (_start(address, I2C_FLAG_READ) != 0)
+    if (_start(address, I2C_FLAG_READ) != 0) {
         return 0;
+    }
 
     for (int i = 0; i < length; i++) {
         /* Send NACK for last received byte */
-        if ((length-i) == 1)
+        if ((length - i) == 1) {
             TWCR = (1 << TWEN) | (1 << TWINT);
-        else
+        }
+        else {
             TWCR = (1 << TWEA) | (1 << TWEN) | (1 << TWINT);
+        }
         DEBUG("Wait for byte %i\n", i+1);
         /* Wait for TWINT Flag set. This indicates that DATA has been received.*/
-        while (!(TWCR & (1 << TWINT)))
-            {}
+        while (!(TWCR & (1 << TWINT))) {}
         /* receive data byte */
         my_data[i] = TWDR;
         DEBUG("Byte %i received\n", i+1);
@@ -160,13 +180,12 @@ int i2c_read_reg(i2c_t dev, uint8_t address, uint8_t reg, void *data)
 
 int i2c_read_regs(i2c_t dev, uint8_t address, uint8_t reg, void *data, int length)
 {
-    if ((unsigned int)dev >= I2C_NUMOF) {
-        return -1;
-    }
+    assert((dev < I2C_NUMOF) && (length > 0));
 
     /* start transmission and send slave address */
-    if (_start(address, I2C_FLAG_WRITE) != 0)
+    if (_start(address, I2C_FLAG_WRITE) != 0) {
         return 0;
+    }
 
     /* send register address and wait for complete transfer to be finished*/
     if (_write(&reg, 1) != 1) {
@@ -174,8 +193,6 @@ int i2c_read_regs(i2c_t dev, uint8_t address, uint8_t reg, void *data, int lengt
         return 0;
     }
 
-    _stop();
-
     /* now start a new start condition and receive data */
     return i2c_read_bytes(dev, address, data, length);
 }
@@ -189,13 +206,12 @@ int i2c_write_bytes(i2c_t dev, uint8_t address, const void *data, int length)
 {
     int bytes = 0;
 
-    if ((unsigned int)dev >= I2C_NUMOF) {
-        return -1;
-    }
+    assert((dev < I2C_NUMOF) && (length > 0));
 
     /* start transmission and send slave address */
-    if (_start(address, I2C_FLAG_WRITE) != 0)
+    if (_start(address, I2C_FLAG_WRITE) != 0) {
         return 0;
+    }
 
     /* send out data bytes */
     bytes = _write(data, length);
@@ -208,10 +224,6 @@ int i2c_write_bytes(i2c_t dev, uint8_t address, const void *data, int length)
 
 int i2c_write_reg(i2c_t dev, uint8_t address, uint8_t reg, uint8_t data)
 {
-    if ((unsigned int)dev >= I2C_NUMOF) {
-        return -1;
-    }
-
     return i2c_write_regs(dev, address, reg, &data, 1);
 }
 
@@ -219,17 +231,17 @@ int i2c_write_regs(i2c_t dev, uint8_t address, uint8_t reg, const void *data, in
 {
     int bytes = 0;
 
-    if ((unsigned int)dev >= I2C_NUMOF) {
-        return -1;
-    }
+    assert((dev < I2C_NUMOF) && (length > 0));
 
     /* start transmission and send slave address */
-    if (_start(address, I2C_FLAG_WRITE) != 0)
+    if (_start(address, I2C_FLAG_WRITE) != 0) {
         return 0;
+    }
     /* send register address and wait for complete transfer to be finished*/
-    if (_write(&reg, 1))
+    if (_write(&reg, 1)) {
         /* write data to register */
         bytes = _write(data, length);
+    }
     /* finish transfer */
     _stop();
     /* return number of bytes send */
@@ -238,26 +250,14 @@ int i2c_write_regs(i2c_t dev, uint8_t address, uint8_t reg, const void *data, in
 
 void i2c_poweron(i2c_t dev)
 {
-    if (dev < I2C_NUMOF) {
-        #if defined (CPU_ATMEGA2560) || defined (CPU_ATMEGA1281)
-            PRR0 &= ~(1 << PRTWI);
-        #endif
-        #ifdef CPU_ATMEGA328P
-            PRR &= ~(1 << PRTWI);
-        #endif
-    }
+    assert(dev < I2C_NUMOF);
+    I2C_POWER_REG &= ~(1 << PRTWI);
 }
 
 void i2c_poweroff(i2c_t dev)
 {
-    if (dev < I2C_NUMOF) {
-        #if defined (CPU_ATMEGA2560) || defined (CPU_ATMEGA1281)
-            PRR0 |= (1 << PRTWI);
-        #endif
-        #ifdef CPU_ATMEGA328P
-            PRR |= (1 << PRTWI);
-        #endif
-    }
+    assert(dev < I2C_NUMOF);
+    I2C_POWER_REG |= (1 << PRTWI);
 }
 
 static int _start(uint8_t address, uint8_t rw_flag)
@@ -267,36 +267,41 @@ static int _start(uint8_t address, uint8_t rw_flag)
     DEBUG("START condition transmitted\n");
 
     /* Wait for TWINT Flag set. This indicates that the START has been
-    transmitted, and ACK/NACK has been received.*/
-    while (!(TWCR & (1 << TWINT)))
-        {}
+     * transmitted, and ACK/NACK has been received.*/
+    while (!(TWCR & (1 << TWINT))) {}
 
     /* Check value of TWI Status Register. Mask prescaler bits.
-    If status different from START go to ERROR */
-    if ((TWSR & 0xF8) != MT_START) {
-        DEBUG("I2C Status Register is different from START\n");
+     * If status different from START go to ERROR */
+    if ((TWSR & 0xF8) == MT_START) {
+        DEBUG("I2C Status is: START\n");
+    }
+    else if ((TWSR & 0xF8) == MT_START_REPEATED) {
+        DEBUG("I2C Status is: START REPEATED\n");
+    }
+    else {
+        DEBUG("I2C Status Register is different from START/START_REPEATED\n");
         _stop();
         return -1;
     }
-    else
-        DEBUG("I2C Status Register is: START\n");
+
 
     /* Load ADDRESS and R/W Flag into TWDR Register.
-    Clear TWINT bit in TWCR to start transmission of ADDRESS */
+     * Clear TWINT bit in TWCR to start transmission of ADDRESS */
     TWDR = (address << 1) | rw_flag;
     TWCR = (1 << TWINT) | (1 << TWEN);
     DEBUG("ADDRESS and FLAG transmitted\n");
 
     /* Wait for TWINT Flag set. This indicates that ADDRESS has been transmitted.*/
-    while (!(TWCR & (1 << TWINT)))
-        {}
+    while (!(TWCR & (1 << TWINT))) {}
 
     /* Check value of TWI Status Register. Mask prescaler bits.
-    If status different from ADDRESS ACK go to ERROR */
-    if ((TWSR & 0xF8) == MT_ADDRESS_ACK)
+     * If status different from ADDRESS ACK go to ERROR */
+    if ((TWSR & 0xF8) == MT_ADDRESS_ACK) {
         DEBUG("ACK has been received for ADDRESS (write)\n");
-    else if ((TWSR & 0xF8) == MR_ADDRESS_ACK)
+    }
+    else if ((TWSR & 0xF8) == MR_ADDRESS_ACK) {
         DEBUG("ACK has been received for ADDRESS (read)\n");
+    }
     else {
         DEBUG("NOT ACK has been received for ADDRESS\n");
         _stop();
@@ -308,38 +313,36 @@ static int _start(uint8_t address, uint8_t rw_flag)
 
 static int _write(const uint8_t *data, int length)
 {
-    int i = 0;
-
-    for (i = 0; i < length; i++) {
+    for (int i = 0; i < length; i++) {
         /* Load DATA into TWDR Register.
-        Clear TWINT bit in TWCR to start transmission of data */
+         * Clear TWINT bit in TWCR to start transmission of data */
         TWDR = data[i];
         TWCR = (1 << TWINT) | (1 << TWEN);
         DEBUG("Byte %i transmitted\n", i+1);
 
         /* Wait for TWINT Flag set. This indicates that DATA has been transmitted.*/
-        while (!(TWCR & (1 << TWINT)))
-            {}
+        while (!(TWCR & (1 << TWINT))) {}
 
         /* Check value of TWI Status Register. Mask prescaler bits. If status
-        different from MT_DATA_ACK, return number of transmitted bytes */
+         * different from MT_DATA_ACK, return number of transmitted bytes */
         if ((TWSR & 0xF8) != MT_DATA_ACK) {
             DEBUG("NACK has been received for BYTE %i\n", i+1);
             return i;
         }
-        else
+        else {
             DEBUG("ACK has been received for BYTE %i\n", i+1);
+        }
     }
 
-    return i;
+    return length;
 }
 
 static void _stop(void)
 {
     /* Reset I2C Interrupt Flag and transmit STOP condition */
     TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN);
-    while (TWCR & (1 << TWSTO))
-        {}
+    /* Wait for STOP Flag reset. This indicates that STOP has been transmitted.*/
+    while (TWCR & (1 << TWSTO)) {}
     DEBUG("STOP condition transmitted\n");
     TWCR = 0;
 }