diff --git a/cpu/cc2538/include/periph_cpu.h b/cpu/cc2538/include/periph_cpu.h
index b04a46d669bda5a2c5be74b36937ec42386f906b..0508516e9629c7faf4d3cb9454c8ce69e1a78bab 100644
--- a/cpu/cc2538/include/periph_cpu.h
+++ b/cpu/cc2538/include/periph_cpu.h
@@ -87,10 +87,35 @@ void gpio_init_af(gpio_t pin, uint8_t sel, uint8_t over);
  * @param[in] func  Set pin for peripheral function (input)
  */
 void gpio_init_mux(gpio_t pin, uint8_t over, uint8_t sel, uint8_t func);
+
+/**
+ * @name   Use shared I2C functions
+ * @{
+ */
+#define PERIPH_I2C_NEED_READ_REG
+#define PERIPH_I2C_NEED_READ_REGS
+#define PERIPH_I2C_NEED_WRITE_REG
+#define PERIPH_I2C_NEED_WRITE_REGS
+/** @} */
+
+/**
+ * @name   Override I2C clock speed values
+ * @{
+ */
+#define HAVE_I2C_SPEED_T
+typedef enum {
+    I2C_SPEED_LOW       = 0x01,     /**< not supported */
+    I2C_SPEED_NORMAL    = 100000U,  /**< normal mode:   ~100kbit/s */
+    I2C_SPEED_FAST      = 400000U,  /**< fast mode:     ~400kbit/s */
+    I2C_SPEED_FAST_PLUS = 0x02,     /**< not supported */
+    I2C_SPEED_HIGH      = 0x03,     /**< not supported */
+} i2c_speed_t;
+/** @} */
 /**
  * @brief   I2C configuration options
  */
 typedef struct {
+    i2c_speed_t speed;      /**< baudrate used for the bus */
     gpio_t scl_pin;         /**< pin used for SCL */
     gpio_t sda_pin;         /**< pin used for SDA */
 } i2c_conf_t;
diff --git a/cpu/cc2538/periph/i2c.c b/cpu/cc2538/periph/i2c.c
index 98e7ceb0f6563df13575e94b65182f24d32e292b..3305c9d71aff1b56997b2a4a6022d25cf2ad479b 100644
--- a/cpu/cc2538/periph/i2c.c
+++ b/cpu/cc2538/periph/i2c.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (C) 2015 Loci Controls Inc.
+ *               2017 HAW Hamburg
  *
  * This file is subject to the terms and conditions of the GNU Lesser
  * General Public License v2.1. See the file LICENSE in the top level
@@ -8,14 +9,14 @@
 
 /**
  * @ingroup     cpu_cc2538
- * @ingroup     drivers_periph
+ * @ingroup     drivers_periph_i2c
  * @{
  *
  * @file
  * @brief       Low-level I2C driver implementation
  *
  * @author      Ian Martin <ian@locicontrols.com>
- *
+ * @author      Sebastian Meiling <s@mlng.net>
  * @}
  */
 
@@ -23,643 +24,337 @@
 #include <stdbool.h>
 #include <stdio.h>
 #include <stdint.h>
+#include <errno.h>
 
 #include "mutex.h"
 #include "cpu.h"
+#include "periph/gpio.h"
 #include "periph/i2c.h"
-#include "thread.h"
-#ifdef MODULE_XTIMER
-#include "xtimer.h"
-#endif
-#include "timex.h" /* for US_PER_SEC */
 
 #define ENABLE_DEBUG (0)
 #include "debug.h"
 
-#ifndef I2C_0_SCL_PIN
-#define I2C_0_SCL_PIN i2c_config[I2C_0].scl_pin
-#endif
-
-#ifndef I2C_0_SDA_PIN
-#define I2C_0_SDA_PIN i2c_config[I2C_0].sda_pin
-#endif
-
-#undef BIT
-#define BIT(n)     ( 1 << (n) )
-
-/* Standard I2C Parameters */
-#define DATA_BITS  8
-#define ACK_BITS   1
-
-/* I2CM_DR Bits */
-#define RS         BIT(0)
+/* short cuts macros */
+#define SCL_PIN(x)  (i2c_config[x].scl_pin)
+#define SDA_PIN(x)  (i2c_config[x].sda_pin)
+#define SPEED(x)    (i2c_config[x].speed)
 
 /* I2CM_CTRL Bits */
-#define ACK        BIT(3)
-#define STOP       BIT(2)
-#define START      BIT(1)
-#define RUN        BIT(0)
+#define ACK         (0x08)
+#define STOP        (0x04)
+#define START       (0x02)
+#define RUN         (0x01)
 
 /* I2CM_STAT Bits */
-#define BUSBSY     BIT(6)
-#define IDLE       BIT(5)
-#define ARBLST     BIT(4)
-#define DATACK     BIT(3)
-#define ADRACK     BIT(2)
-#define ERROR      BIT(1)
-#define BUSY       BIT(0)
+#define BUSBSY      (0x40)
+#define IDLE        (0x20)
+#define ARBLST      (0x10)
+#define DATACK      (0x08)
+#define ADRACK      (0x04)
+#define ERROR       (0x02)
+#define BUSY        (0x01)
 
 /* I2CM_CR Bits */
-#define SFE        BIT(5)
-#define MFE        BIT(4)
-#define LPBK       BIT(0)
-
-#define ANY_ERROR  (ARBLST | DATACK | ADRACK | ERROR)
-
-#define SCL_LP     6 /**< SCL Low Period (fixed at 6). */
-#define SCL_HP     4 /**< SCL High Period (fixed at 4). */
-
-static mutex_t mutex = MUTEX_INIT;
-static mutex_t i2c_wait_mutex = MUTEX_INIT;
-static uint32_t speed_hz;
-static uint32_t scl_delay;
-
-#define bus_quiet()            ( cc2538_gpio_read(I2C_0_SCL_PIN) && cc2538_gpio_read(I2C_0_SDA_PIN) )
-#define WARN_IF(cond) \
-        if (cond) { \
-            DEBUG("%s at %s:%u\n", #cond, RIOT_FILE_NOPATH, __LINE__); \
-        }
-
-void cc2538_i2c_init_master(uint32_t speed_hz);
-
-static void i2cm_ctrl_write(uint_fast8_t value) {
-    WARN_IF(I2CM_STAT & BUSY);
-    I2CM_CTRL = value;
-}
-
-static void assert_scl(void) {
-    cc2538_gpio_clear(I2C_0_SCL_PIN);
-    IOC_PXX_OVER[I2C_0_SCL_PIN] |= IOC_OVERRIDE_OE;
-    gpio_dir_output(I2C_0_SCL_PIN);
-    gpio_software_control(I2C_0_SCL_PIN);
-}
-
-static void release_scl(void) {
-    IOC_PXX_OVER[I2C_0_SCL_PIN] &= ~(IOC_OVERRIDE_OE | IOC_OVERRIDE_PDE);
-    gpio_dir_input(I2C_0_SCL_PIN);
-    gpio_software_control(I2C_0_SCL_PIN);
-}
-
-static void release_sda(void) {
-    IOC_PXX_OVER[I2C_0_SDA_PIN] &= ~(IOC_OVERRIDE_OE | IOC_OVERRIDE_PDE);
-    gpio_dir_input(I2C_0_SDA_PIN);
-    gpio_software_control(I2C_0_SDA_PIN);
-}
-
-static void recover_i2c_bus(void) {
-    /* Switch to software GPIO mode for bus recovery */
-    release_sda();
-    release_scl();
-
-    if (!bus_quiet()) {
-        const uint_fast8_t try_limit = 200;
-        uint_fast8_t n;
-        for (n = 0; n < try_limit; n++) {
-            if (bus_quiet()) {
-                DEBUG("%s(): SDA released after%4u SCL pulses.\n", __FUNCTION__, n);
-                break;
-            }
-
-            assert_scl();
-
-#ifdef MODULE_XTIMER
-            xtimer_usleep(scl_delay);
-#else
-            thread_yield();
-#endif
-
-            release_scl();
-
-#ifdef MODULE_XTIMER
-            xtimer_usleep(scl_delay);
-#else
-            thread_yield();
-#endif
-        }
-
-        if (n >= try_limit) {
-            DEBUG("%s(): Failed to release SDA after%4u SCL pulses.\n", __FUNCTION__, n);
-        }
-    }
+#define SFE         (0x20)  /**< I2C slave function enable */
+#define MFE         (0x10)  /**< I2C master function enable */
+#define LPBK        (0x01)  /**< I2C loopback */
 
-    /* Return to hardware mode for the I2C pins */
-    gpio_hardware_control(I2C_0_SCL_PIN);
-    gpio_hardware_control(I2C_0_SDA_PIN);
-}
-
-#ifdef MODULE_XTIMER
-static void _timer_cb(void *arg)
-{
-    (void)arg;
-    mutex_unlock(&i2c_wait_mutex);
-}
-#endif
-
-static uint_fast8_t i2c_ctrl_blocking(uint_fast8_t flags)
-{
-#ifdef MODULE_XTIMER
-    const unsigned int xtimer_timeout = 3 * (DATA_BITS + ACK_BITS) * US_PER_SEC / speed_hz;
-#endif
+#define SCL_LP      (6U)    /**< SCL Low Period (fixed at 6). */
+#define SCL_HP      (4U)    /**< SCL High Period (fixed at 4). */
 
-    mutex_trylock(&i2c_wait_mutex);
-
-    assert(I2CM_IMR & 1);
-    i2cm_ctrl_write(flags);
-
-#ifdef MODULE_XTIMER
-    /* Set a timeout at double the expected time to transmit a byte: */
-    xtimer_t xtimer = { .callback = _timer_cb, .arg = NULL };
-    xtimer_set(&xtimer, xtimer_timeout);
-#endif
-
-    mutex_lock(&i2c_wait_mutex);
-
-#ifdef MODULE_XTIMER
-    xtimer_remove(&xtimer);
-#endif
-
-    if (I2CM_STAT & BUSY) {
-        /* If the controller is still busy, it probably will be forever */
-#ifdef MODULE_XTIMER
-        DEBUG("Master is still BUSY after %u usec. Resetting.\n", xtimer_timeout);
-#endif
-        cc2538_i2c_init_master(speed_hz);
-    }
+#define CMD_WAIT    (16U)   /**< small delay to wait for I2C command */
+#define RST_WAIT    (32U)   /**< reset delay */
 
-    WARN_IF(I2CM_STAT & BUSY);
+#define INVALID_SPEED_MASK  (0x0f)
 
-    return I2CM_STAT;
-}
+static mutex_t lock = MUTEX_INIT;
 
 void isr_i2c(void)
 {
     /* Clear the interrupt flag */
-    I2CM_ICR = 1;
-
-    /* Unlock the wait mutex */
-    mutex_unlock(&i2c_wait_mutex);
+    I2CM_ICR = 0x1;
+    I2CM_MIS = 0x1;
 
     cortexm_isr_end();
 }
 
-void cc2538_i2c_init_master(uint32_t speed_hz)
+static inline void _i2c_reset(void)
 {
-    SYS_CTRL_RCGCI2C |= 1; /**< Enable the I2C0 clock. */
-    SYS_CTRL_SCGCI2C |= 1; /**< Enable the I2C0 clock. */
-    SYS_CTRL_DCGCI2C |= 1; /**< Enable the I2C0 clock. */
-
+    DEBUG("%s\n", __FUNCTION__);
     /* Reset I2C peripheral */
     SYS_CTRL_SRI2C |= 1;
-
-#ifdef MODULE_XTIMER
-    xtimer_usleep(50);
-#else
-    thread_yield();
-#endif
-
+    /* wait shortly for reset */
+    for (unsigned delay = 0; delay < RST_WAIT; ++delay) {}
+    /* clear periph reset trigger */
     SYS_CTRL_SRI2C &= ~1;
-
-    /* Clear all pin override flags except PUE (Pull-Up Enable) */
-    IOC_PXX_OVER[I2C_0_SCL_PIN] &= IOC_OVERRIDE_PUE;
-    IOC_PXX_OVER[I2C_0_SDA_PIN] &= IOC_OVERRIDE_PUE;
-
-    IOC_PXX_SEL[I2C_0_SCL_PIN] = I2C_SCL_OUT;
-    IOC_PXX_SEL[I2C_0_SDA_PIN] = I2C_SDA_OUT;
-
-    IOC_I2CMSSCL = I2C_0_SCL_PIN;
-    IOC_I2CMSSDA = I2C_0_SDA_PIN;
-
-    gpio_hardware_control(I2C_0_SCL_PIN);
-    gpio_hardware_control(I2C_0_SDA_PIN);
-
-    /* Initialize the I2C master by setting the Master Function Enable bit */
-    I2CM_CR |= MFE;
-
-    /* Set the SCL clock speed */
-    uint32_t ps = sys_clock_freq();
-    uint32_t denom = 2 * (SCL_LP + SCL_HP) * speed_hz;
-    ps += denom / 2;
-    ps /= denom;
-    I2CM_TPR = ps - 1;
-
-    /* Enable I2C master interrupts */
-    NVIC_SetPriority(I2C_IRQn, I2C_IRQ_PRIO);
-    NVIC_EnableIRQ(I2C_IRQn);
-
-    i2cm_ctrl_write(STOP);
-
-    /* Enable I2C master interrupts */
-    I2CM_IMR = 1;
 }
 
-int i2c_init_master(i2c_t dev, i2c_speed_t speed)
+static inline void _i2c_clock_enable(bool enable)
 {
-    switch (dev) {
-#if I2C_0_EN
-        case I2C_0:
-            break;
-#endif
-        default:
-            return -1;
+    DEBUG("%s\n", __FUNCTION__);
+    if (enable) {
+        SYS_CTRL_RCGCI2C |= 1;
+        SYS_CTRL_SCGCI2C |= 1;
+        SYS_CTRL_DCGCI2C |= 1;
     }
-
-    switch (speed) {
-        case I2C_SPEED_LOW:
-            speed_hz = 10000;
-            break;
-
-        case I2C_SPEED_NORMAL:
-            speed_hz = 100000;
-            break;
-
-        case I2C_SPEED_FAST:
-            speed_hz = 400000;
-            break;
-
-        case I2C_SPEED_FAST_PLUS:
-            speed_hz = 1000000;
-            break;
-
-        case I2C_SPEED_HIGH:
-            speed_hz = 3400000;
-            break;
-
-        default:
-            return -2;
+    else {
+        SYS_CTRL_RCGCI2C &= ~1;
+        SYS_CTRL_SCGCI2C &= ~1;
+        SYS_CTRL_DCGCI2C &= ~1;
     }
-
-    cc2538_i2c_init_master(speed_hz);
-
-    /* Pre-compute an SCL delay in microseconds */
-    scl_delay = US_PER_SEC;
-    scl_delay += speed_hz;
-    scl_delay /= 2 * speed_hz;
-
-    return 0;
-}
-
-int i2c_init_slave(i2c_t dev, uint8_t address)
-{
-    (void) dev;
-    (void) address;
-    /* Slave mode is not (yet) supported. */
-    return -1;
 }
 
-int i2c_acquire(i2c_t dev)
+static inline void _i2c_master_enable(bool enable)
 {
-    if (dev == I2C_0) {
-        mutex_lock(&mutex);
-        return 0;
+    DEBUG("%s: %s\n", __FUNCTION__, (enable ? "yes" : "no"));
+    if (enable) {
+        /* enable I2C master function */
+        I2CM_CR |= MFE;
+        /* Enable I2C master interrupts */
+        NVIC_SetPriority(I2C_IRQn, I2C_IRQ_PRIO);
+        NVIC_EnableIRQ(I2C_IRQn);
+        /* Enable I2C master interrupts */
+        I2CM_IMR = 1;
     }
     else {
-        return -1;
+        /* Disable I2C master interrupts */
+        I2CM_IMR = 0;
+        NVIC_DisableIRQ(I2C_IRQn);
+        /* disable master function */
+        I2CM_CR &= ~(MFE);
     }
 }
 
-int i2c_release(i2c_t dev)
+static inline void _i2c_master_frequency(i2c_speed_t speed)
 {
-    if (dev == I2C_0) {
-        mutex_unlock(&mutex);
-        return 0;
-    }
-    else {
-        return -1;
+    assert ((speed == I2C_SPEED_NORMAL) || (speed == I2C_SPEED_FAST));
+    DEBUG("%s (%" PRIu32 ")\n", __FUNCTION__, (uint32_t)speed);
+    /* if selected speed is not applicable fall back to normal */
+    if (speed & INVALID_SPEED_MASK) {
+        DEBUG("! invalid speed setting, fall back to normal !\n");
+        speed = I2C_SPEED_NORMAL;
     }
+    /* Set the SCL clock speed */
+    uint32_t denom = 2 * (SCL_LP + SCL_HP) * (uint32_t)speed;
+    uint32_t ps = (sys_clock_freq() + denom - 1) / denom;
+    I2CM_TPR = (ps - 1);
 }
 
-static bool i2c_busy(void) {
-    if (I2CM_STAT & BUSY) {
-        cc2538_i2c_init_master(speed_hz);
-        return (I2CM_STAT & BUSY) != 0;
-    }
-
-    return false;
+static uint_fast8_t _i2c_master_stat_get(void)
+{
+    return I2CM_STAT;
 }
 
-int i2c_read_byte(i2c_t dev, uint8_t address, void *data)
+static bool _i2c_master_busy(void)
 {
-    return i2c_read_bytes(dev, address, data, 1);
+    return ((I2CM_STAT & BUSY) ? true : false);
 }
 
-static int i2c_read_bytes_dumb(uint8_t address, uint8_t *data, int length)
+static bool _i2c_master_busbusy(void)
 {
-    int n = 0;
-    uint_fast8_t stat;
-
-    switch (length) {
-        case 0:
-            break;
-
-        case 1:
-            if (i2c_busy()) {
-                break;
-            }
-
-            I2CM_SA = (address << 1) | RS;
-            stat = i2c_ctrl_blocking(STOP | START | RUN);
-            if (stat & ANY_ERROR) {
-                break;
-            }
-            data[n] = I2CM_DR;
-            n++;
-            break;
-
-        default:
-            if (i2c_busy()) {
-                break;
-            }
-
-            I2CM_SA = (address << 1) | RS;
-            stat = i2c_ctrl_blocking(ACK | START | RUN);
-
-            if (stat & ARBLST) {
-                 break;
-            }
-            else if (stat & ANY_ERROR) {
-                i2cm_ctrl_write(STOP);
-                break;
-            }
-
-            data[n] = I2CM_DR;
-            n++;
-
-            while (n < length) {
-                stat = i2c_ctrl_blocking((n < length - 1) ? (ACK | RUN) : (STOP | RUN));
-
-                if (stat & ARBLST) {
-                     break;
-                }
-                else if (stat & ANY_ERROR) {
-                    i2cm_ctrl_write(STOP);
-                    break;
-                }
-
-                data[n] = I2CM_DR;
-                n++;
-            }
-
-            break;
-    }
-
-    return n;
+    return ((I2CM_STAT & BUSBSY) ? true : false);
 }
 
-int i2c_read_bytes(i2c_t dev, uint8_t address, void *data, int length)
+static void _i2c_master_slave_addr(uint16_t addr, bool receive)
 {
-    switch (dev) {
-#if I2C_0_EN
-        case I2C_0:
-            break;
-#endif
-
-        default:
-            return -1;
-    }
-
-    WARN_IF(I2CM_STAT & BUSY);
-
-    if ( (length <= 0) || i2c_busy() ) {
-        return 0;
-    }
-
-    WARN_IF(I2CM_STAT & BUSBSY);
-
-    if (I2CM_STAT & BUSBSY) {
-        recover_i2c_bus();
-
-        if (I2CM_STAT & BUSBSY) {
-            return 0;
-        }
-    }
-
-    return i2c_read_bytes_dumb(address, data, length);
+    DEBUG("%s (%" PRIx16 ", %d)\n", __FUNCTION__, addr, (int)receive);
+    assert(!(addr & 0x80));
+    I2CM_SA = (addr << 1) | receive;
 }
 
-int i2c_read_reg(i2c_t dev, uint8_t address, uint8_t reg, void *data)
+static void _i2c_master_data_put(uint8_t data)
 {
-    return i2c_read_regs(dev, address, reg, data, 1);
+    DEBUG("%s (0x%x)\n", __FUNCTION__, data);
+    I2CM_DR = data;
 }
 
-int i2c_read_regs(i2c_t dev, uint8_t address, uint8_t reg, void *data, int length)
+static uint_fast8_t _i2c_master_data_get(void)
 {
-    uint_fast8_t stat;
+    uint_fast8_t data = I2CM_DR;
+    DEBUG("%s (0x%x)\n", __FUNCTION__, data);
+    return data;
+}
 
-    if (dev != I2C_0) {
-        return -1;
-    }
+static inline void _i2c_master_ctrl(uint_fast8_t cmd)
+{
+    DEBUG("%s (%" PRIx32 ")\n", __FUNCTION__, (uint32_t)cmd);
+    I2CM_CTRL = cmd;
+}
 
-    /* Transmit reg byte to slave */
-    if (i2c_busy()) {
+static inline int _i2c_master_status(void)
+{
+    DEBUG("%s\n", __FUNCTION__);
+    uint_fast8_t stat = _i2c_master_stat_get();
+    DEBUG(" - I2C master status (0x%x): ", stat);
+    if (stat & BUSY) {
+        DEBUG("busy!\n");
         return 0;
     }
-
-    WARN_IF(I2CM_STAT & BUSBSY);
-
-    if (I2CM_STAT & BUSBSY) {
-        recover_i2c_bus();
-
-        if (I2CM_STAT & BUSBSY) {
-            return 0;
+    else if (stat & (ERROR | ARBLST)) {
+        _i2c_master_ctrl(STOP | RUN);
+        unsigned cw = CMD_WAIT;
+        while (_i2c_master_busy() || (cw--)) {}
+        if (stat & ADRACK) {
+            DEBUG("addr ack lost!\n");
+            return -ENXIO;
         }
+        if (stat & DATACK) {
+            DEBUG("data ack lost!\n");
+            return -EIO;
+        }
+        if (stat & ARBLST) {
+            DEBUG("lost bus arbitration!\n");
+            return -EAGAIN;
+        }
+        DEBUG("unknown!\n");
+        return -EAGAIN;
     }
+    DEBUG("okay.\n");
+    return 0;
+}
 
-    I2CM_SA = address << 1;
-    I2CM_DR = reg;
-    stat = i2c_ctrl_blocking(START | RUN);
+void i2c_init(i2c_t dev)
+{
+    DEBUG("%s (%i)\n", __FUNCTION__, (int)dev);
+    assert(dev < I2C_NUMOF);
+    /* enable i2c clock */
+    _i2c_clock_enable(true);
+    /* reset i2c periph */
+    _i2c_reset();
+    /* init pins */
+    gpio_init_mux(SCL_PIN(dev), OVERRIDE_PULLUP, I2C_SCL_OUT, I2C_SCL_IN);
+    gpio_init_mux(SDA_PIN(dev), OVERRIDE_PULLUP, I2C_SDA_OUT, I2C_SDA_IN);
+    /* enable master mode */
+    _i2c_master_enable(true);
+    /* set bus frequency */
+    _i2c_master_frequency(SPEED(dev));
+    DEBUG(" - I2C master status (0x%x).\n", _i2c_master_stat_get());
+}
 
-    if (stat & ARBLST) {
-        return 0;
-    }
-    else if (stat & ANY_ERROR) {
-        i2cm_ctrl_write(STOP);
+int i2c_acquire(i2c_t dev)
+{
+    DEBUG("%s\n", __FUNCTION__);
+    if (dev < I2C_NUMOF) {
+        mutex_lock(&lock);
         return 0;
     }
-    else {
-        /* Receive data from slave */
-        return i2c_read_bytes_dumb(address, data, length);
-    }
+    return -1;
 }
 
-int i2c_write_byte(i2c_t dev, uint8_t address, uint8_t data)
+int i2c_release(i2c_t dev)
 {
-    return i2c_write_bytes(dev, address, &data, 1);
+    DEBUG("%s\n", __FUNCTION__);
+    if (dev < I2C_NUMOF) {
+        mutex_unlock(&lock);
+        return 0;
+    }
+    return -1;
 }
 
-int i2c_write_bytes(i2c_t dev, uint8_t address, const void *data, int length)
+int i2c_read_bytes(i2c_t dev, uint16_t addr,
+                   void *data, size_t len, uint8_t flags)
 {
-    int n = 0;
-    const uint8_t *my_data = data;
+    DEBUG("%s\n", __FUNCTION__);
+    DEBUG(" - I2C master status (0x%x).\n", _i2c_master_stat_get());
 
-    if (dev != I2C_0) {
-        return -1;
+    if ((dev >= I2C_NUMOF) || (data == NULL) || (len == 0)) {
+        return -EINVAL;
     }
-
-    WARN_IF(I2CM_STAT & BUSBSY);
-
-    if (I2CM_STAT & BUSBSY) {
-        recover_i2c_bus();
-
-        if (I2CM_STAT & BUSBSY) {
-            return 0;
-        }
+    if (flags & (I2C_REG16 | I2C_ADDR10)) {
+        return -EOPNOTSUPP;
+    }
+    if (_i2c_master_busy()) {
+        DEBUG("i2c_read_bytes: device busy!\n");
+        return -EAGAIN;
+    }
+    if (!(_i2c_master_busbusy()) && (flags & I2C_NOSTART)) {
+        DEBUG("i2c_read_bytes: bus not busy!\n");
+        return -EAGAIN;
     }
 
-    I2CM_SA = address << 1;
-    uint_fast8_t flags = START | RUN;
-
-    for (n = 0; n < length; n++) {
-        if (n >= length - 1) flags |= STOP;
-        WARN_IF(I2CM_STAT & BUSY);
-        I2CM_DR = my_data[n];
-        i2c_ctrl_blocking(flags);
-
-        WARN_IF(I2CM_STAT & ARBLST);
-        WARN_IF(I2CM_STAT & DATACK);
-        WARN_IF(I2CM_STAT & ADRACK);
-        WARN_IF(I2CM_STAT & ERROR);
+    /* set slave address for receive */
+    _i2c_master_slave_addr(addr, true);
+    int rc = 0;
+    uint8_t *buf = data;
 
-        if (I2CM_STAT & ARBLST) {
-             break;
+    for (size_t n = 0; n < len; n++) {
+        uint_fast8_t cmd = RUN;
+        if ((n == 0) && !(flags & I2C_NOSTART)) {
+            cmd |= START;
+        }
+        if (((len - n) == 1) && !(flags & I2C_NOSTOP)) {
+            cmd |= STOP;
+        }
+        else {
+            cmd |= ACK;
         }
-        else if (I2CM_STAT & ANY_ERROR) {
-            i2cm_ctrl_write(STOP);
+        /* run command */
+        _i2c_master_ctrl(cmd);
+        /* wait until master is done transferring */
+        DEBUG ("%s: wait for master...\n", __FUNCTION__);
+        unsigned cw = CMD_WAIT;
+        while (_i2c_master_busy() || (cw--)) {}
+
+        /* check master status */
+        if ((rc = _i2c_master_status()) != 0) {
             break;
         }
-
-        flags = RUN;
+        /* read data into buffer */
+        buf[n] = _i2c_master_data_get();
     }
 
-        if (n < length) {
-            DEBUG("%s(%u, %p, %u): %u/%u bytes delivered.\n",
-                  __FUNCTION__, address, (void *)my_data, length, n, length);
-        }
-
-    return n;
+    return rc;
 }
 
-int i2c_write_reg(i2c_t dev, uint8_t address, uint8_t reg, uint8_t data)
+int i2c_write_bytes(i2c_t dev, uint16_t addr, const void *data,
+                    size_t len, uint8_t flags)
 {
-    return i2c_write_regs(dev, address, reg, &data, 1);
-}
-
-int i2c_write_regs(i2c_t dev, uint8_t address, uint8_t reg, const void *data, int length)
-{
-    uint_fast8_t stat;
-    const uint8_t *my_data = data;
+    DEBUG("%s\n", __FUNCTION__);
+    DEBUG(" - I2C master status (0x%x).\n", _i2c_master_stat_get());
 
-    if (dev != I2C_0) {
-        return -1;
+    if ((dev >= I2C_NUMOF) || (data == NULL) || (len == 0)) {
+        return -EINVAL;
     }
-
-    /* Transmit reg byte to slave */
-    if (i2c_busy()) {
-        return 0;
+    if (flags & (I2C_REG16 | I2C_ADDR10)) {
+        return -EOPNOTSUPP;
     }
-
-    WARN_IF(I2CM_STAT & BUSBSY);
-
-    if (I2CM_STAT & BUSBSY) {
-        recover_i2c_bus();
-
-        if (I2CM_STAT & BUSBSY) {
-            return 0;
-        }
+    if (_i2c_master_busy()) {
+        DEBUG("i2c_write_bytes: device busy!\n");
+        return -EAGAIN;
+    }
+    if (!(_i2c_master_busbusy()) && (flags & I2C_NOSTART)) {
+        DEBUG("i2c_read_bytes: bus not busy!\n");
+        return -EAGAIN;
     }
 
-    I2CM_SA = address << 1;
-    I2CM_DR = reg;
+    /* set slave address for write */
+    _i2c_master_slave_addr(addr, false);
 
-    uint_fast8_t flags = (length > 0)? (START | RUN) : (STOP | START | RUN);
-    stat = i2c_ctrl_blocking(flags);
+    int rc = 0;
+    const uint8_t *buf = data;
 
-    if (stat & ARBLST) {
-        return 0;
-    }
-    else if (stat & ANY_ERROR) {
-        i2cm_ctrl_write(STOP);
-        return 0;
-    }
-    else {
-        /* Transmit data to slave */
-        int n = 0;
-
-        flags &= ~START;
-
-        for (n = 0; n < length; n++) {
-            if (n >= length - 1) flags |= STOP;
-            WARN_IF(I2CM_STAT & BUSY);
-            I2CM_DR = my_data[n];
-
-            i2c_ctrl_blocking(flags);
-
-            WARN_IF(I2CM_STAT & ARBLST);
-            WARN_IF(I2CM_STAT & DATACK);
-            WARN_IF(I2CM_STAT & ADRACK);
-            WARN_IF(I2CM_STAT & ERROR);
-
-            if (I2CM_STAT & ARBLST) {
-                 break;
-            }
-            else if (I2CM_STAT & ANY_ERROR) {
-                i2cm_ctrl_write(STOP);
-                break;
-            }
+    for (size_t n = 0; n < len; n++) {
+        uint_fast8_t cmd = RUN;
+        if ((n == 0) && !(flags & I2C_NOSTART)) {
+            cmd |= START;
         }
-
-        if (n < length) {
-            DEBUG(
-                "%s(%u, %u, %u, %p, %u): %u/%u bytes delivered.\n",
-                __FUNCTION__,
-                dev,
-                address,
-                reg,
-                data,
-                length,
-                n,
-                length
-            );
+        if (((len - n) == 1) && !(flags & I2C_NOSTOP)) {
+            cmd |= STOP;
+        }
+        /* write byte to mem */
+        _i2c_master_data_put(buf[n]);
+        /* run command */
+        _i2c_master_ctrl(cmd);
+        /* wait until master is done transferring */
+        DEBUG ("%s: wait for master...\n", __FUNCTION__);
+        unsigned cw = CMD_WAIT;
+        while (_i2c_master_busy() || (cw--)) {}
+
+        /* check master status */
+        if ((rc = _i2c_master_status()) != 0) {
+            break;
         }
-
-        return n;
-    }
-}
-
-void i2c_poweron(i2c_t dev)
-{
-    if (dev == I2C_0) {
-        SYS_CTRL_RCGCI2C |= 1; /**< Enable the I2C0 clock. */
-        SYS_CTRL_SCGCI2C |= 1; /**< Enable the I2C0 clock. */
-        SYS_CTRL_DCGCI2C |= 1; /**< Enable the I2C0 clock. */
-
-        I2CM_CR |= MFE;        /**< I2C master function enable. */
-
-        /* Enable I2C master interrupts */
-        I2CM_IMR = 1;
     }
-}
 
-void i2c_poweroff(i2c_t dev)
-{
-    if (dev == I2C_0) {
-        /* Disable I2C master interrupts */
-        I2CM_IMR = 0;
-        NVIC_DisableIRQ(I2C_IRQn);
-
-        I2CM_CR &= ~MFE;        /**< I2C master function enable. */
-
-        SYS_CTRL_RCGCI2C &= ~1; /**< Disable the I2C0 clock. */
-        SYS_CTRL_SCGCI2C &= ~1; /**< Disable the I2C0 clock. */
-        SYS_CTRL_DCGCI2C &= ~1; /**< Disable the I2C0 clock. */
-    }
+    return rc;
 }