1 files changed, 18 insertions, 35 deletions

diff --git a/fw/fe310/eos/i2c.c b/fw/fe310/eos/i2c.c
index 72e53ae..122a1b2 100644
--- a/fw/fe310/eos/i2c.c
+++ b/fw/fe310/eos/i2c.c
@@ -10,20 +10,11 @@
 
 void eos_i2c_start(uint32_t baud_rate) {
     eos_i2c_set_baud_rate(baud_rate);
-
-    GPIO_REG(GPIO_PULLUP_EN)    &= ~(1 << IOF_I2C0_SCL);
-    GPIO_REG(GPIO_OUTPUT_XOR)   &= ~(1 << IOF_I2C0_SCL);
-
-    GPIO_REG(GPIO_PULLUP_EN)    &= ~(1 << IOF_I2C0_SDA);
-    GPIO_REG(GPIO_OUTPUT_XOR)   &= ~(1 << IOF_I2C0_SDA);
-
     GPIO_REG(GPIO_IOF_SEL)      &= ~IOF0_I2C0_MASK;
     GPIO_REG(GPIO_IOF_EN)       |=  IOF0_I2C0_MASK;
 }
 
 void eos_i2c_stop(void) {
-    GPIO_REG(GPIO_PULLUP_EN)    |=  (1 << IOF_I2C0_SCL);
-    GPIO_REG(GPIO_PULLUP_EN)    |=  (1 << IOF_I2C0_SDA);
     GPIO_REG(GPIO_IOF_EN)       &=  ~IOF0_I2C0_MASK;
 }
 
@@ -38,70 +29,62 @@ void eos_i2c_set_baud_rate(uint32_t baud_rate) {
 }
 
 
-static int i2c_addr(uint8_t addr, uint8_t rw_flag) {
-    I2C0_REGB(I2C_TRANSMIT) = ((addr & 0x7F) << 1) | (rw_flag & 0x1);
-    I2C0_REGB(I2C_COMMAND) = I2C_CMD_START | I2C_CMD_WRITE;
+static int i2c_read(uint8_t cmd) {
+    I2C0_REGB(I2C_COMMAND) = I2C_CMD_READ | cmd;
     while (I2C0_REGB(I2C_STATUS) & I2C_STATUS_TIP);
 
-    if (I2C0_REGB(I2C_STATUS) & I2C_STATUS_RXACK) return EOS_ERR;
-    return EOS_OK;
+    return I2C0_REGB(I2C_RECEIVE);
 }
 
-static int i2c_reg(uint8_t reg) {
-    I2C0_REGB(I2C_TRANSMIT) = reg;
-    I2C0_REGB(I2C_COMMAND) = I2C_CMD_WRITE;
+static int i2c_write(uint8_t cmd, uint8_t b) {
+    I2C0_REGB(I2C_TRANSMIT) = b;
+    I2C0_REGB(I2C_COMMAND) = I2C_CMD_WRITE | cmd;
     while (I2C0_REGB(I2C_STATUS) & I2C_STATUS_TIP);
 
     if (I2C0_REGB(I2C_STATUS) & I2C_STATUS_RXACK) return EOS_ERR;
     return EOS_OK;
 }
 
+static int i2c_addr(uint8_t addr, uint8_t rw_flag) {
+    return i2c_write(I2C_CMD_START, ((addr & 0x7F) << 1) | (rw_flag & 0x1));
+}
+
 int eos_i2c_read(uint8_t addr, uint8_t reg, uint8_t *buffer, uint16_t len) {
-    uint8_t cmd;
     int rv;
     int i;
 
     rv = i2c_addr(addr, I2C_WRITE);
     if (rv) return rv;
 
-    rv = i2c_reg(reg);
+    rv = i2c_write(0, reg);
     if (rv) return rv;
 
     rv = i2c_addr(addr, I2C_READ);
     if (rv) return rv;
 
-    cmd = I2C_CMD_READ;
-
     for (i=0; i<len; i++) {
-        /* Set NACK to end read, and generate STOP condition */
-        if (i == (len - 1)) cmd |= I2C_CMD_ACK | I2C_CMD_STOP;
-        I2C0_REGB(I2C_COMMAND) = cmd;
-        while (I2C0_REGB(I2C_STATUS) & I2C_STATUS_TIP);
-        buffer[i] = I2C0_REGB(I2C_RECEIVE);
+        rv = i2c_read(i == (len - 1) ? (I2C_CMD_ACK | I2C_CMD_STOP) : 0);  /* Set NACK to end read, and generate STOP condition */
+        if (rv < 0) return rv;
+
+        buffer[i] = (uint8_t)rv;
     }
 
     return EOS_OK;
 }
 
 int eos_i2c_write(uint8_t addr, uint8_t reg, uint8_t *buffer, uint16_t len) {
-    uint8_t cmd;
     int rv;
     int i;
 
     rv = i2c_addr(addr, I2C_WRITE);
     if (rv) return rv;
 
-    rv = i2c_reg(reg);
+    rv = i2c_write(0, reg);
     if (rv) return rv;
 
-    cmd = I2C_CMD_WRITE;
-
     for (i=0; i<len; i++) {
-        if (i == (len - 1)) cmd |= I2C_CMD_STOP;
-        I2C0_REGB(I2C_TRANSMIT) = buffer[i];
-        I2C0_REGB(I2C_COMMAND) = cmd;
-        while (I2C0_REGB(I2C_STATUS) & I2C_STATUS_TIP);
-        if (I2C0_REGB(I2C_STATUS) & I2C_STATUS_RXACK) return EOS_ERR;
+        rv = i2c_write(i == (len - 1) ? I2C_CMD_STOP : 0, buffer[i]);
+        if (rv) return rv;
     }
 
     return EOS_OK;