Diff: targets/hal/TARGET_RENESAS/TARGET_RZ_A1H/i2c_api.c

Revision:

441:d2c15dda23c1

Parent:

430:d406b7919023

Child:

443:ed48b4122bfb

--- a/targets/hal/TARGET_RENESAS/TARGET_RZ_A1H/i2c_api.c	Tue Dec 16 08:15:08 2014 +0000
+++ b/targets/hal/TARGET_RENESAS/TARGET_RZ_A1H/i2c_api.c	Tue Jan 06 16:15:36 2015 +0000
@@ -17,7 +17,7 @@
 #include "i2c_api.h"
 #include "cmsis.h"
 #include "pinmap.h"
-
+#include "r_typedefs.h"
 
 #include "riic_iodefine.h"
 #include "RZ_A1_Init.h"
@@ -28,7 +28,29 @@
 #define REG(N) \
     RIIC[obj->i2c]->RIICn##N
 
-#define NACKF (1 << 4)
+/* RIICnCR1 */
+#define CR1_RST   (1 << 6)
+#define CR1_ICE   (1 << 7)
+
+/* RIICnCR2 */
+#define CR2_ST    (1 << 1)
+#define CR2_SP    (1 << 3)
+#define CR2_NACKF (1 << 4)
+#define CR2_BBSY  (1 << 7)
+
+/* RIICnMR3 */
+#define MR3_ACKBT (1 << 3)
+#define MR3_ACKWP (1 << 4)
+#define MR3_WAIT  (1 << 6)
+
+/* RIICnSR2 */
+#define SR2_STOP  (1 << 3)
+#define SR2_NACKF (1 << 4)
+#define SR2_RDRF  (1 << 5)
+#define SR2_TEND  (1 << 6)
+#define SR2_TDRE  (1 << 7)
+
+#define TIMEOUT_1S    (3600000)  /* Loop counter : Time-out is about 1s. By 3600000 loops, measured value is 969ms. */
 
 static const PinMap PinMap_I2C_SDA[] = {
     {P1_1 , I2C_0, 1},
@@ -44,7 +66,8 @@
     {NC   , NC,    0}
 };
 
-// Clear the Transmit data Empty TDRE
+
+/* Clear the Transmit data Empty TDRE */
 static inline int i2c_addressed(i2c_t *obj) {
     volatile int sar0 = (REG(SR1.UINT8[0])&1),
                  trs  = (REG(CR2.UINT8[0])&0x20) >> 5;
@@ -56,54 +79,70 @@
 }
 
 static inline void i2c_clear_TDRE(i2c_t *obj) {
-    REG(SR2.UINT32) &= ~(1 << 7);
-}
-
-static inline void i2c_wait_RDRF(i2c_t *obj) {
-    while (!(i2c_status(obj) & (1 << 5))) ;
+    REG(SR2.UINT32) &= ~SR2_TDRE;
 }
 
-static void i2c_reg_reset(i2c_t *obj) {
-    // full reset
-    REG(CR1.UINT8[0]) &= ~(1 << 7); // CR1.ICE off
-    REG(CR1.UINT8[0]) |=  (1 << 6); // CR1.IICRST on
-    REG(CR1.UINT8[0]) |=  (1 << 7); // CR1.ICE on
-
-    REG(MR1.UINT8[0])  =  0x08;  // P_phi /8  9bit (including Ack)
-    REG(SER.UINT8[0])  =  0x00;  // no slave addr enabled
-
-    // set frequency
-    REG(MR1.UINT8[0]) |=  obj->pclk_bit;
-    REG(BRL.UINT32)    =  obj->width;
-    REG(BRH.UINT32)    =  obj->width;
-
-    REG(MR2.UINT8[0])  =  0x07;
-    REG(MR3.UINT8[0])  =  0x00;
-
-    REG(FER.UINT8[0])  =  0x72;  // SCLE, NFE enabled, TMOT
-    REG(IER.UINT8[0])  =  0x00;  // no interrupt
-
-    REG(CR1.UINT32) &= ~(1 << 6); // CR1.IICRST negate reset
-}
-
-// Wait until the Trans Data Empty (TDRE) is set
-static int i2c_wait_TDRE(i2c_t *obj) {
+static inline int i2c_wait_RDRF(i2c_t *obj) {
     int timeout = 0;
-
-    while (!(i2c_status(obj) & (1 << 7))) {
+    
+    /* There is no timeout, but the upper limit value is set to avoid an infinite loop. */
+    while (!(i2c_status(obj) & SR2_RDRF)) {
         timeout ++;
-        if (timeout > 100000) return -1;
+        if (timeout >= TIMEOUT_1S) {
+            return -1;
+        }
     }
 
     return 0;
 }
 
-static inline int i2c_wait_TEND(i2c_t *obj) {
+static void i2c_reg_reset(i2c_t *obj) {
+    /* full reset */
+    REG(CR1.UINT8[0]) &= ~CR1_ICE; // CR1.ICE off
+    REG(CR1.UINT8[0]) |=  CR1_RST; // CR1.IICRST on
+    REG(CR1.UINT8[0]) |=  CR1_ICE; // CR1.ICE on
+
+    REG(MR1.UINT8[0])  =  0x08;    // P_phi /x  9bit (including Ack)
+    REG(SER.UINT8[0])  =  0x00;    // no slave addr enabled
+
+    /* set frequency */
+    REG(MR1.UINT8[0]) |=  obj->pclk_bit;
+    REG(BRL.UINT8[0])  =  obj->width_low;
+    REG(BRH.UINT8[0])  =  obj->width_hi;
+
+    REG(MR2.UINT8[0])  =  0x07;
+    REG(MR3.UINT8[0])  =  0x00;
+
+    REG(FER.UINT8[0])  =  0x72;    // SCLE, NFE enabled, TMOT
+    REG(IER.UINT8[0])  =  0x00;    // no interrupt
+
+    REG(CR1.UINT32) &= ~CR1_RST;   // CR1.IICRST negate reset
+}
+
+/* Wait until the Trans Data Empty (TDRE) is set */
+static int i2c_wait_TDRE(i2c_t *obj) {
     int timeout = 0;
 
-    while (!(i2c_status(obj) & (1 << 6))) {
+    /* There is no timeout, but the upper limit value is set to avoid an infinite loop. */
+    while (!(i2c_status(obj) & SR2_TDRE)) {
         timeout ++;
-        if (timeout > 100000) return -1;
+        if (timeout >= TIMEOUT_1S) {
+            return -1;
+        }
+    }
+
+    return 0;
+}
+
+static int i2c_wait_TEND(i2c_t *obj) {
+    int timeout = 0;
+    
+    /* There is no timeout, but the upper limit value is set to avoid an infinite loop. */
+    while (!(i2c_status(obj) & SR2_TEND)) {
+        timeout ++;
+        if (timeout >= TIMEOUT_1S) {
+            return -1;
+        }
     }
 
     return 0;
@@ -111,21 +150,31 @@
 
 
 static int i2c_wait_STOP(i2c_t *obj) {
-    volatile uint32_t work_reg;
+    int timeout = 0;
+    
+    /* There is no timeout, but the upper limit value is set to avoid an infinite loop. */
+    while (!(i2c_status(obj) & SR2_STOP)) {
+        timeout ++;
+        if (timeout >= TIMEOUT_1S) {
+            return -1;
+        }
+    }
 
-    /* wait SR2.STOP = 1 */
-    work_reg = REG(SR2.UINT32);
-    while ((work_reg & (1 << 3)) == 0) {
-        work_reg = REG(SR2.UINT32);
-    }
-    /* SR2.NACKF = 0 */
-    REG(SR2.UINT32) &= ~(1 << 4);
-    /* SR2.STOP  = 0 */
-    REG(SR2.UINT32) &= ~(1 << 3);
-    
     return 0;
 }
 
+static void i2c_set_NACKF_STOP(i2c_t *obj) {
+    /* SR2.NACKF = 0 */
+    REG(SR2.UINT32) &= ~SR2_NACKF;
+    /* SR2.STOP  = 0 */
+    REG(SR2.UINT32) &= ~SR2_STOP;
+}
+
+static void i2c_set_err_noslave(i2c_t *obj) {
+    i2c_stop(obj);
+    (void)i2c_wait_STOP(obj);
+    i2c_set_NACKF_STOP(obj);
+}
 
 static inline void i2c_power_enable(i2c_t *obj) {
     volatile uint8_t dummy;
@@ -139,140 +188,193 @@
 }
 
 void i2c_init(i2c_t *obj, PinName sda, PinName scl) {
-    // determine the SPI to use
+    /* determine the I2C to use */
     I2CName i2c_sda = (I2CName)pinmap_peripheral(sda, PinMap_I2C_SDA);
     I2CName i2c_scl = (I2CName)pinmap_peripheral(scl, PinMap_I2C_SCL);
     obj->i2c = pinmap_merge(i2c_sda, i2c_scl);
     MBED_ASSERT((int)obj->i2c != NC);
 
-    // enable power
+    /* enable power */
     i2c_power_enable(obj);
 
-    // set default frequency at 100k
+    /* set default frequency at 100k */
     i2c_frequency(obj, 100000);
 
-    // full reset
-    i2c_reg_reset(obj);
-
     pinmap_pinout(sda, PinMap_I2C_SDA);
     pinmap_pinout(scl, PinMap_I2C_SCL);
 }
 
 inline int i2c_start(i2c_t *obj) {
-    if (REG(CR2.UINT32) & (1 << 7)) { // BBSY check
-        return 0xff;
+    int timeout = 0;
+
+    while (REG(CR2.UINT32) & CR2_BBSY) {
+        timeout ++;
+        if (timeout >= obj->bbsy_wait_cnt) {
+            i2c_reg_reset(obj);
+            /* Start Condition */
+            REG(CR2.UINT8[0]) |= CR2_ST;
+            return 0;
+        }
     }
-    REG(CR2.UINT8[0]) |= 0x02; // start
+    /* Start Condition */
+    REG(CR2.UINT8[0]) |= CR2_ST;
 
-    return 0x10;
+    return 0;
 }
 
 inline int i2c_stop(i2c_t *obj) {
     /* SR2.STOP  = 0 */
-    REG(SR2.UINT32) &= ~(1 << 3);
-    // write the stop bit
-    REG(CR2.UINT32) |= (1 << 3);
+    REG(SR2.UINT32) &= ~SR2_STOP;
+    /* Stop condition */
+    REG(CR2.UINT32) |= CR2_SP;
 
     return 0;
 }
 
 static inline int i2c_do_write(i2c_t *obj, int value) {
-    // write the data
-    if (!(i2c_status(obj) & NACKF)) { // NACF=0
-        i2c_wait_TDRE(obj);
+    int timeout = 0;
+
+    if (!(i2c_status(obj) & SR2_NACKF)) {
+        /* RIICnSR2.NACKF=0 */
+        /* There is no timeout, but the upper limit value is set to avoid an infinite loop. */
+        while (!(i2c_status(obj) & SR2_TDRE)) {
+            /* RIICnSR2.TDRE=0 */
+            timeout ++;
+            if (timeout >= TIMEOUT_1S) {
+                return -1;
+            }
+            if (i2c_status(obj) & SR2_NACKF) {
+                /* RIICnSR2.NACKF=1 */
+                return -1;
+            }
+        }
+        /* write the data */
         REG(DRT.UINT32) = value;
-    }  else {
-        return 0xff;
+    } else {
+        return -1;
     }
-    return i2c_status(obj);
+
+    return 0;
+}
+
+static inline int i2c_read_address_write(i2c_t *obj, int value) {
+    int status;
+    status = i2c_wait_TDRE(obj);
+    if (status == 0) {
+        /* write the data */
+        REG(DRT.UINT32) = value;
+        return 0;
+    } else {
+        return status;
+    }
+
 }
 
 static inline int i2c_do_read(i2c_t *obj, int last) {
-    if (obj->dummy) {
-        volatile int dummy = REG(DRR.UINT32);
-        obj->dummy = 0;
-    }
-
-    // wait for it to arrive
-    i2c_wait_RDRF(obj);
-
     if (last == 2) {
         /* this time is befor last byte read */
-        /* Set MR3 WATI bit is 1 */;
-        REG(MR3.UINT32) |= (1 << 6);
+        /* Set MR3 WAIT bit is 1 */;
+        REG(MR3.UINT32) |= MR3_WAIT;
     } else if (last == 1) {
-        // send a NOT ACK
-        REG(MR3.UINT32) |= (1 <<4);
-        REG(MR3.UINT32) |=  (1 <<3);
-        REG(MR3.UINT32) &= ~(1 <<4);
+        /* send a NOT ACK */
+        REG(MR3.UINT32) |=  MR3_ACKWP;
+        REG(MR3.UINT32) |=  MR3_ACKBT;
+        REG(MR3.UINT32) &= ~MR3_ACKWP;
     } else {
-        // send a ACK
-        REG(MR3.UINT32) |= (1 <<4);
-        REG(MR3.UINT32) &= ~(1 <<3);
-        REG(MR3.UINT32) &= ~(1 <<4);
+        /* send a ACK */
+        REG(MR3.UINT32) |=  MR3_ACKWP;
+        REG(MR3.UINT32) &= ~MR3_ACKBT;
+        REG(MR3.UINT32) &= ~MR3_ACKWP;
     }
 
-    // return the data
+    /* return the data */
     return (REG(DRR.UINT32) & 0xFF);
 }
 
 void i2c_frequency(i2c_t *obj, int hz) {
-    int freq;
-    int oldfreq = 0;
-    int newfreq = 0;
-    uint32_t pclk;
-    uint32_t pclk_base;
-    uint32_t tmp_width;
-    uint32_t width = 0;
-    uint8_t count;
-    uint8_t pclk_bit = 0;
+    float64_t pclk_val;
+    float64_t wait_utime;
+    volatile float64_t bps;
+    volatile float64_t L_time;         /* H Width period */
+    volatile float64_t H_time;         /* L Width period */
+    uint32_t tmp_L_width;
+    uint32_t tmp_H_width;
+    uint32_t remainder;
+    uint32_t wk_cks = 0;
 
     /* set PCLK */
     if (false == RZ_A1_IsClockMode0()) {
-        pclk_base = (uint32_t)CM1_RENESAS_RZ_A1_P0_CLK;
+        pclk_val = (float64_t)CM1_RENESAS_RZ_A1_P0_CLK;
     } else {
-        pclk_base = (uint32_t)CM0_RENESAS_RZ_A1_P0_CLK;
+        pclk_val = (float64_t)CM0_RENESAS_RZ_A1_P0_CLK;
     }
 
     /* Min 10kHz, Max 400kHz */
     if (hz < 10000) {
-        freq = 10000;
+        bps = 10000;
     } else if (hz > 400000) {
-        freq = 400000;
+        bps = 400000;
     } else {
-        freq = hz;
+        bps = (float64_t)hz;
     }
 
-    for (count = 0; count < 7; count++) {
-        // IIC phi = P0 phi / rate
-        pclk = pclk_base / (2 << count);
-        // In case of "CLE = 1, NFE = 1, CKS != 000( IIC phi < P0 phi ), nf = 1"
-        // freq = 1 / {[( BRH + 2 + 1 ) + ( BRL + 2 + 1 )] / pclk }
-        // BRH is regarded as same value with BRL
-        // 2( BRH + 3 ) / pclk  = 1 / freq
-        tmp_width = ((pclk / freq) / 2) - 3;
-        // Carry in a decimal point
-        tmp_width += 1;
-        if ((tmp_width >= 0x00000001) && (tmp_width <= 0x0000001F)) {
-            // Calculate theoretical value, and Choose max value of them
-            newfreq = pclk / (tmp_width + 3) / 2;
-            if (newfreq >= oldfreq) {
-                oldfreq  = newfreq;
-                width    = tmp_width;
-                pclk_bit = (uint8_t)(0x10 * (count + 1));
-            }
+    /* Calculation L width time */
+    L_time = (1 / (2 * bps));   /* Harf period of frequency */
+    H_time = L_time;
+
+    /* Check I2C mode of Speed */
+    if (bps > 100000) {
+        /* Fast-mode */
+        L_time -= 102E-9;    /* Falling time of SCL clock. */
+        H_time -= 138E-9;    /* Rising time of SCL clock. */
+        /* Check L wideth */
+        if (L_time < 1.3E-6) {
+            /* Wnen L width less than 1.3us */
+            /* Subtract Rise up and down time for SCL from H/L width */
+            L_time = 1.3E-6;
+            H_time = (1 / bps) - L_time - 138E-9 - 102E-9;
         }
     }
 
-    if (width != 0) {
-        // I2C Rate
-        obj->pclk_bit = pclk_bit;  // P_phi / xx
-        obj->width    = (width | 0x000000E0);
+    tmp_L_width   = (uint32_t)(L_time * pclk_val * 10);
+    tmp_L_width >>= 1;
+    wk_cks++;
+    while (tmp_L_width >= 341) {
+        tmp_L_width >>= 1;
+        wk_cks++;
+    }
+    remainder   = tmp_L_width % 10;
+    tmp_L_width = ((tmp_L_width + 9) / 10) - 3;       /* carry */
+
+    tmp_H_width   = (uint32_t)(H_time * pclk_val * 10);
+    tmp_H_width >>= wk_cks;
+    if (remainder == 0) {
+        tmp_H_width   = ((tmp_H_width + 9) / 10) - 3; /* carry */
     } else {
-        // Default 
-        obj->pclk_bit = 0x00;      // P_phi / 1
-        obj->width    = 0x000000FF;
+        remainder    += tmp_H_width % 10;
+        tmp_H_width   = (tmp_H_width / 10) - 3;
+        if (remainder > 10) {
+            tmp_H_width += 1;                         /* fine adjustment */
+        }
     }
+    /* timeout of BBSY bit is minimum low width by frequency */
+    /* so timeout calculates "(low width) * 2" by frequency */
+    wait_utime = (L_time * 2) * 1000000;
+    /* 1 wait of BBSY bit is about 0.3us. if it's below 0.3us, wait count is set as 1. */
+    if (wait_utime <= 0.3) {
+        obj->bbsy_wait_cnt = 1;
+    } else {
+        obj->bbsy_wait_cnt = (int)(wait_utime / 0.3);
+    }
+
+
+    /* I2C Rate */
+    obj->pclk_bit  = (uint8_t)(0x10 * wk_cks);        /* P_phi / xx */
+    obj->width_low = (uint8_t)(tmp_L_width | 0x000000E0);
+    obj->width_hi  = (uint8_t)(tmp_H_width | 0x000000E0);
+
+    /* full reset */
+    i2c_reg_reset(obj);
 }
 
 int i2c_read(i2c_t *obj, int address, char *data, int length, int stop) {
@@ -281,40 +383,41 @@
     int value;
     volatile uint32_t work_reg = 0;
 
-    // full reset
-    i2c_reg_reset(obj);
-    obj->dummy = 1;
-    
     status = i2c_start(obj);
-
-    if (status == 0xff) {
-        i2c_stop(obj);
-        i2c_wait_STOP(obj);
+    if (status != 0) {
+        i2c_set_err_noslave(obj);
         return I2C_ERROR_BUS_BUSY;
     }
-
-    status = i2c_do_write(obj, (address | 0x01));
-    if (status & 0x01) {
-        i2c_stop(obj);
-        i2c_wait_STOP(obj);
+    /*  Send Slave address */
+    status = i2c_read_address_write(obj, (address | 0x01));
+    if (status != 0) {
+        i2c_set_err_noslave(obj);
         return I2C_ERROR_NO_SLAVE;
     }
-    
     /* wati RDRF */
-    i2c_wait_RDRF(obj);
+    status = i2c_wait_RDRF(obj);
     /* check ACK/NACK */
-    if ((REG(SR2.UINT32) & (1 << 4) == 1)) {
+    if ((status != 0) || (REG(SR2.UINT32) & CR2_NACKF == 1)) {
         /* Slave sends NACK */
         i2c_stop(obj);
-        // dummy read
+        /* dummy read */
         value = REG(DRR.UINT32);
-        i2c_wait_STOP(obj);
+        (void)i2c_wait_STOP(obj);
+        i2c_set_NACKF_STOP(obj);
         return I2C_ERROR_NO_SLAVE;
     }
-    
-    // Read in all except last byte
+    /* Read in all except last byte */
     if (length > 2) {
+        /* dummy read */
+        value = REG(DRR.UINT32);
         for (count = 0; count < (length - 1); count++) {
+            /* wait for it to arrive */
+            status = i2c_wait_RDRF(obj);
+            if (status != 0) {
+                i2c_set_err_noslave(obj);
+                return I2C_ERROR_NO_SLAVE;
+            }
+            /* Recieve the data */
             if (count == (length - 2)) {
                 value = i2c_do_read(obj, 1);
             } else if ((length >= 3) && (count == (length - 3))) {
@@ -322,124 +425,113 @@
             } else {
                 value = i2c_do_read(obj, 0);
             }
-            status = i2c_status(obj);
-            if (status & 0x10) {
-                i2c_stop(obj);
-                i2c_wait_STOP(obj);
-                return count;
-            }
-            data[count] = (char) value;
+            data[count] = (char)value;
         }
     } else if (length == 2) {
         /* Set MR3 WATI bit is 1 */;
-        REG(MR3.UINT32) |= (1 << 6);
-        // dummy read
+        REG(MR3.UINT32) |= MR3_WAIT;
+        /* dummy read */
         value = REG(DRR.UINT32);
-        // wait for it to arrive
-        i2c_wait_RDRF(obj);
-        // send a NOT ACK
-        REG(MR3.UINT32) |= (1 <<4);
-        REG(MR3.UINT32) |=  (1 <<3);
-        REG(MR3.UINT32) &= ~(1 <<4);
+        /* wait for it to arrive */
+        status = i2c_wait_RDRF(obj);
+        if (status != 0) {
+            i2c_set_err_noslave(obj);
+            return I2C_ERROR_NO_SLAVE;
+        }
+        /* send a NOT ACK */
+        REG(MR3.UINT32) |=  MR3_ACKWP;
+        REG(MR3.UINT32) |=  MR3_ACKBT;
+        REG(MR3.UINT32) &= ~MR3_ACKWP;
         data[count] = (char)REG(DRR.UINT32);
         count++;
     } else if (length == 1) {
         /* Set MR3 WATI bit is 1 */;
-        REG(MR3.UINT32) |= (1 << 6);
-        // send a NOT ACK
-        REG(MR3.UINT32) |= (1 <<4);
-        REG(MR3.UINT32) |=  (1 <<3);
-        REG(MR3.UINT32) &= ~(1 <<4);
-        // dummy read
+        REG(MR3.UINT32) |=  MR3_WAIT;
+        /* send a NOT ACK */
+        REG(MR3.UINT32) |=  MR3_ACKWP;
+        REG(MR3.UINT32) |=  MR3_ACKBT;
+        REG(MR3.UINT32) &= ~MR3_ACKWP;
+        /* dummy read */
         value = REG(DRR.UINT32);
     } else {
-        // Do Nothing
+        return I2C_ERROR_NO_SLAVE;
+    }
+    /* wait for it to arrive */
+    status = i2c_wait_RDRF(obj);
+    if (status != 0) {
+        i2c_set_err_noslave(obj);
+        return I2C_ERROR_NO_SLAVE;
     }
-
-    // read in last byte
-    i2c_wait_RDRF(obj);
-    // If not repeated start, send stop.
-    if (stop) {
-        /* RIICnSR2.STOP = 0 */
-        REG(SR2.UINT32) &= ~(1 << 3);
-        /* RIICnCR2.SP   = 1 */
-        REG(CR2.UINT32) |= (1 << 3);
-        /* RIICnDRR read */
-        value = REG(DRR.UINT32) & 0xFF;
-        data[count] = (char) value;
-        /* RIICnMR3.WAIT = 0 */
-        REG(MR3.UINT32) &= ~(1 << 6);
-        i2c_wait_STOP(obj);
-    } else {
-        /* RIICnDRR read */
-        value = REG(DRR.UINT32) & 0xFF;
-        data[count] = (char) value;
-    }
+    /* RIICnSR2.STOP = 0 */
+    REG(SR2.UINT32) &= ~SR2_STOP;
+    /* RIICnCR2.SP   = 1 */
+    REG(CR2.UINT32) |= CR2_SP;
+    /* RIICnDRR read */
+    value = REG(DRR.UINT32) & 0xFF;
+    data[count] = (char)value;
+    /* RIICnMR3.WAIT = 0 */
+    REG(MR3.UINT32) &= ~MR3_WAIT;
+    (void)i2c_wait_STOP(obj);
+    i2c_set_NACKF_STOP(obj);
 
     return length;
 }
 
 int i2c_write(i2c_t *obj, int address, const char *data, int length, int stop) {
-    int i, status;
-
-    // full reset
-    i2c_reg_reset(obj);
+    int cnt;
+    int status;
 
     status = i2c_start(obj);
-
-    if ((status == 0xff)) {
-        i2c_stop(obj);
-        i2c_wait_STOP(obj);
+    if (status != 0) {
+        i2c_set_err_noslave(obj);
         return I2C_ERROR_BUS_BUSY;
     }
-    
-    /**/
-    status = REG(CR2.UINT32);
-    status = REG(SR2.UINT32);
-    /**/
-
+    /*  Send Slave address */
     status = i2c_do_write(obj, address);
-    if (status & 0x10) {
-        i2c_stop(obj);
-        i2c_wait_STOP(obj);
+    if (status != 0) {
+        i2c_set_err_noslave(obj);
         return I2C_ERROR_NO_SLAVE;
     }
-
-    /**/
-    status = REG(CR2.UINT32);
-    status = REG(SR2.UINT32);
-    /**/
-    for (i=0; i<length; i++) {
-    /**/
-    status = REG(CR2.UINT32);
-    status = REG(SR2.UINT32);
-    /**/
-        status = i2c_do_write(obj, data[i]);
-        if(status & 0x10) {
-            i2c_stop(obj);
-            i2c_wait_STOP(obj);
-            return i;
+    /* Send Write data */
+    for (cnt=0; cnt<length; cnt++) {
+        status = i2c_do_write(obj, data[cnt]);
+        if(status != 0) {
+            i2c_set_err_noslave(obj);
+            return cnt;
         }
     }
-
-    i2c_wait_TEND(obj);
-
-    // If not repeated start, send stop.
-    if (stop) {
-        i2c_stop(obj);
-        i2c_wait_STOP(obj);
+    /* Wait send end */
+    status = i2c_wait_TEND(obj);
+    if (status != 0) {
+        i2c_set_err_noslave(obj);
+        return I2C_ERROR_NO_SLAVE;
     }
-
+    i2c_stop(obj);
+    (void)i2c_wait_STOP(obj);
+    i2c_set_NACKF_STOP(obj);
+    
     return length;
 }
 
 void i2c_reset(i2c_t *obj) {
     i2c_stop(obj);
-    i2c_wait_STOP(obj);
+    (void)i2c_wait_STOP(obj);
+    i2c_set_NACKF_STOP(obj);
 }
 
 int i2c_byte_read(i2c_t *obj, int last) {
-    obj->dummy = 1;
+    int status;
+
+    /* dummy read */
+    (void)REG(DRR.UINT32);
+    /* wait for it to arrive */
+    status = i2c_wait_RDRF(obj);
+    if (status != 0) {
+        i2c_stop(obj);
+        (void)i2c_wait_STOP(obj);
+        i2c_set_NACKF_STOP(obj);
+        return I2C_ERROR_NO_SLAVE;
+    }
     
     return (i2c_do_read(obj, last) & 0xFF);
 }
@@ -447,7 +539,7 @@
 int i2c_byte_write(i2c_t *obj, int data) {
     int ack;
     int status = i2c_do_write(obj, (data & 0xFF));
-    if (status & NACKF) {
+    if (status != 0) {
         ack = 0;
     } else {
         ack = 1;
@@ -496,7 +588,8 @@
 
     if(status & 0x10) {
         i2c_stop(obj);
-        i2c_wait_STOP(obj);
+        (void)i2c_wait_STOP(obj);
+        i2c_set_NACKF_STOP(obj);
     }
 
     //i2c_clear_TDRE(obj);
@@ -519,7 +612,8 @@
 
     if (!(status & 0x10)) {
         i2c_stop(obj);
-        i2c_wait_STOP(obj);
+        (void)i2c_wait_STOP(obj);
+        i2c_set_NACKF_STOP(obj);
     }
 
     i2c_clear_TDRE(obj);