static inline int i2c_do_write(i2c_t *obj, int value) {
    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 -1;
    }

    return 0;
}

int i2c_slave_read(i2c_t *obj, char *data, int length) {
    int count = 0;
    int status;
    
    do {
        i2c_clear_SI(obj);
        i2c_wait_SI(obj);
        status = i2c_status(obj);
        if((status == 0x80) || (status == 0x90)) {
            data[count] = I2C_DAT(obj) & 0xFF;
        }
        count++;
    } while (((status == 0x80) || (status == 0x90) ||
            (status == 0x060) || (status == 0x70)) && (count < length));
 
    // Clear old status and wait for Serial Interrupt. 
    i2c_clear_SI(obj);
    i2c_wait_SI(obj);
 
    // Obtain new status.
    status = i2c_status(obj);
 
    if(status != 0xA0) {
        i2c_stop(obj);
    }
 
    i2c_clear_SI(obj);
    
    return count;
}

int i2c_byte_write(i2c_t *obj, int data) {
    int ack = 0;
    int status;
    int timeout = 0;
    
    status = i2c_do_write(obj, (data & 0xFF));
    if (status != 0) {
        i2c_set_SR2_NACKF_STOP(obj);
    } else {
        while (((i2c_status(obj) & SR2_RDRF) == 0) && ((i2c_status(obj) & SR2_TEND) == 0)) {
            timeout++;
            if (timeout >= WAIT_TIMEOUT) {
                return ack;
            }
        }
        /* check ACK/NACK */
        if ((REG(SR2.UINT32) & SR2_NACKF) != 0) {
            /* NACK */
            i2c_set_SR2_NACKF_STOP(obj);
        } else {
            ack = 1;
        }
    }

    return ack;
}

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);
        REG(DRT.UINT32) = value;
    }  else {
        return 0xff;
    }
    return i2c_status(obj);
}

uint8_t TwoWire::requestFrom(uint8_t address, uint8_t length, uint8_t sendStop)
{
	uint8_t tmp __attribute__((unused));
	uint8_t status, count=0;

	rxBufferIndex = 0;
	rxBufferLength = 0;
	//serial_print("requestFrom\n");
	// clear the status flags
	I2C0_S = I2C_S_IICIF | I2C_S_ARBL;
	// now take control of the bus...
	if (I2C0_C1 & I2C_C1_MST) {
		// we are already the bus master, so send a repeated start
		I2C0_C1 = I2C_C1_IICEN | I2C_C1_MST | I2C_C1_RSTA | I2C_C1_TX;
	} else {
		// we are not currently the bus master, so wait for bus ready
		while (i2c_status() & I2C_S_BUSY) ;
		// become the bus master in transmit mode (send start)
		slave_mode = 0;
		I2C0_C1 = I2C_C1_IICEN | I2C_C1_MST | I2C_C1_TX;
	}
	// send the address
	I2C0_D = (address << 1) | 1;
	i2c_wait();
	status = i2c_status();
	if ((status & I2C_S_RXAK) || (status & I2C_S_ARBL)) {
		// the slave device did not acknowledge
		// or we lost bus arbitration to another master
		I2C0_C1 = I2C_C1_IICEN;
		return 0;
	}
	if (length == 0) {
		// TODO: does anybody really do zero length reads?
		// if so, does this code really work?
		I2C0_C1 = I2C_C1_IICEN | (sendStop ? 0 : I2C_C1_MST);
		return 0;
	} else if (length == 1) {
		I2C0_C1 = I2C_C1_IICEN | I2C_C1_MST | I2C_C1_TXAK;
	} else {
		I2C0_C1 = I2C_C1_IICEN | I2C_C1_MST;
	}
	tmp = I2C0_D; // initiate the first receive
	while (length > 1) {
		i2c_wait();
		length--;
		if (length == 1) I2C0_C1 = I2C_C1_IICEN | I2C_C1_MST | I2C_C1_TXAK;
		rxBuffer[count++] = I2C0_D;
	}
	i2c_wait();
	I2C0_C1 = I2C_C1_IICEN | I2C_C1_MST | I2C_C1_TX;
	rxBuffer[count++] = I2C0_D;
	if (sendStop) I2C0_C1 = I2C_C1_IICEN;
	rxBufferLength = count;
	return count;
}

//New version WH, Tested OK for Start and Repeated Start
//Old version was Wrong: Calls i2c_start without setting address, i2c_do_read continues before checking status, status check for wrong value
int i2c_read(i2c_t *obj, int address, char *data, int length, int stop) {
    int count, status;
    
    //Store the address+RD and then generate STA
    I2C_DAT(obj) = address | 0x01;
    i2c_start(obj);    

    // Wait for completion of STA and Sending of SlaveAddress+RD and first Read byte
    i2c_wait_SI(obj);
    status = i2c_status(obj);    
    if (status == 0x03) { // NAK on SlaveAddress
        i2c_stop(obj);
        return I2C_ERROR_NO_SLAVE;
    }

    // Read in all except last byte
    for (count = 0; count < (length-1); count++) {
        
      // Wait for it to arrive, note that first byte read after address+RD is already waiting
      i2c_wait_SI(obj);
      status = i2c_status(obj);
      if (status != 0x01) { // RX RDY
        i2c_stop(obj);
        return count;
      }
      data[count] = I2C_DAT(obj) & 0xFF; // Store read byte

      obj->i2c->MSTCTL = (1 << 0); // Send ACK and Continue to read
    }
    
    // Read final byte
    // Wait for it to arrive
    i2c_wait_SI(obj);

    status = i2c_status(obj);
    if (status != 0x01) { // RX RDY
      i2c_stop(obj);
      return count;
    }
    data[count] = I2C_DAT(obj) & 0xFF; // Store final read byte

    // If not repeated start, send stop.
    if (stop) {
        i2c_stop(obj); // Also sends NAK for last read byte
    } else {
        repeated_start = 1;
    }
   
    return length;
}

int i2c_slave_read(i2c_t *obj, char *data, int length) {
    int timeout = 0;
    int count;
    int break_flg = 0;

    if(length <= 0) {
        return 0;
    }
    for (count = 0; ((count < (length + 1)) && (break_flg == 0)); count++) {
        /* There is no timeout, but the upper limit value is set to avoid an infinite loop. */
        while ((i2c_status(obj) & SR2_STOP) || (!(i2c_status(obj) & SR2_RDRF))) {
            /* RIICnSR2.STOP = 1 or RIICnSR2.RDRF = 0 */
            if (i2c_status(obj) & SR2_STOP) {
                /* RIICnSR2.STOP = 1 */
                break_flg = 1;
                break;
            }
            timeout ++;
            if (timeout >= TIMEOUT_1S) {
                return -1;
            }
        }
        if (break_flg == 0) {
            if (count == 0) {
                /* dummy read */
                (void)REG(DRR.UINT32);
            } else {
                data[count - 1] = (char)(REG(DRR.UINT32) & 0xFF);
            }
        }
    }
    if (break_flg == 0) {
        (void)i2c_wait_STOP(obj);
    } else {
        if (i2c_status(obj) & SR2_RDRF) {
            if (count <= 1) {
                /* fail safe */
                /* dummy read */
                (void)REG(DRR.UINT32);
            } else {
                data[count - 2] = (char)(REG(DRR.UINT32) & 0xFF);
            }
        }
    }
    /* SR2.STOP = 0 */
    REG(SR2.UINT32) &= ~SR2_STOP;

    return (count - 1);
}

inline int i2c_start(i2c_t *obj) {
    int status = 0;
    int isInterrupted = I2C_CONSET(obj) & (1 << 3);

    // 8.1 Before master mode can be entered, I2CON must be initialised to:
    //  - I2EN STA STO SI AA - -
    //  -  1    0   0   x  x - -
    // if AA = 0, it can't enter slave mode
    i2c_conclr(obj, 1, 1, 0, 1);

    // The master mode may now be entered by setting the STA bit
    // this will generate a start condition when the bus becomes free
    i2c_conset(obj, 1, 0, 0, 1);
    // Clearing SI bit when it wasn't set on entry can jump past state
    // 0x10 or 0x08 and erroneously send uninitialized slave address.
    if (isInterrupted)
        i2c_clear_SI(obj);

    i2c_wait_SI(obj);
    status = i2c_status(obj);

    // Clear start bit now that it's transmitted
    i2c_conclr(obj, 1, 0, 0, 0);
    return status;
}

//New version WH, Tested OK for Start and Repeated Start
//Old version was Wrong: Calls i2c_start without setting address first
int i2c_write(i2c_t *obj, int address, const char *data, int length, int stop) {
    int i, status;

    //Store the address+/WR and then generate STA
    I2C_DAT(obj) = address & 0xFE;   
    i2c_start(obj);
    
    // Wait for completion of STA and Sending of SlaveAddress+/WR
    i2c_wait_SI(obj);
    status = i2c_status(obj);    
    if (status == 0x03) { // NAK SlaveAddress
        i2c_stop(obj);
        return I2C_ERROR_NO_SLAVE;
    }
    
    //Write all bytes
    for (i=0; i<length; i++) {
        status = i2c_do_write(obj, data[i], 0);
        if (status != 0x02) { // TX RDY. Handles a Slave NAK on datawrite
            i2c_stop(obj);
            return i;
        }
    }
    
    // If not repeated start, send stop.
    if (stop) {
        i2c_stop(obj);
    } else {
        repeated_start = 1;
    }
    
    return length;
}

int i2c_read(i2c_t *obj, int address, char *data, int length, int stop) {
    int count, status;

    status = i2c_start(obj);

    if ((status != 0x10) && (status != 0x08)) {
        i2c_stop(obj);
        return status;
    }

    status = i2c_do_write(obj, (address | 0x01));
    if (status != 0x40) {
        i2c_stop(obj);
        return status;
    }

    // Read in all except last byte
    for (count = 0; count < (length - 1); count++) {
        int value = i2c_do_read(obj, 0);
        status = i2c_status(obj);
        if (status != 0x50) {
            i2c_stop(obj);
            return status;
        }
        data[count] = (char) value;
    }

    // read in last byte
    int value = i2c_do_read(obj, 1);
    status = i2c_status(obj);
    if (status != 0x58) {
        i2c_stop(obj);
        return status;
    }

    data[count] = (char) value;

    // If not repeated start, send stop.
    if (stop) {
        if(i2c_stop(obj) == 1)
            return 1;
    }

    return 0;
}

// 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 << 7))) {
        timeout ++;
        if (timeout > 100000) return -1;
    }

    return 0;
}

uint8_t distco_transmit()
{
  if (i2c_start_transmission()==0)
    return 0;
  if (i2c_wait_transmission()==0)
    return 0;
  if (i2c_status()!=I2C_SUCCESS)
    return 0;
  return 1;
}

/*---------------------------------------------------------------------------*/
bool
board_i2c_write(uint8_t *data, uint8_t len)
{
  uint32_t i;
  bool success;

  /* Write slave address */
  ti_lib_i2c_master_slave_addr_set(I2C0_BASE, slave_addr, false);

  /* Write first byte */
  ti_lib_i2c_master_data_put(I2C0_BASE, data[0]);

  /* Check if another master has access */
  while(ti_lib_i2c_master_bus_busy(I2C0_BASE));

  /* Assert RUN + START */
  ti_lib_i2c_master_control(I2C0_BASE, I2C_MASTER_CMD_BURST_SEND_START);
  while(ti_lib_i2c_master_busy(I2C0_BASE));
  success = i2c_status();

  for(i = 1; i < len && success; i++) {
    /* Write next byte */
    ti_lib_i2c_master_data_put(I2C0_BASE, data[i]);
    if(i < len - 1) {
      /* Clear START */
      ti_lib_i2c_master_control(I2C0_BASE, I2C_MASTER_CMD_BURST_SEND_CONT);
      while(ti_lib_i2c_master_busy(I2C0_BASE));
      success = i2c_status();
    }
  }

  /* Assert stop */
  if(success) {
    /* Assert STOP */
    ti_lib_i2c_master_control(I2C0_BASE, I2C_MASTER_CMD_BURST_SEND_FINISH);
    while(ti_lib_i2c_master_busy(I2C0_BASE));
    success = i2c_status();
    while(ti_lib_i2c_master_bus_busy(I2C0_BASE));
  }

  return success;
}

//Spec says: first check Idle and status is Ok
static inline int i2c_do_write(i2c_t *obj, int value, uint8_t addr) {
    // write the data
    I2C_DAT(obj) = value;
    
    if (!addr)
        obj->i2c->MSTCTL = (1 << 0); //Set continue for data. Should not be set for addr since that uses STA
    
    // wait and return status
    i2c_wait_SI(obj);
    return i2c_status(obj);
}

static inline int i2c_do_write(i2c_t *obj, int value, uint8_t addr) {
    // write the data
    I2C_DAT(obj) = value;
    
    // clear SI to init a send
    i2c_clear_SI(obj);
    
    // wait and return status
    i2c_wait_SI(obj);
    return i2c_status(obj);
}

static inline int i2c_do_write(i2c_t *obj, int value, uint8_t addr) {
    // write the data
    I2C_DAT(obj) = value;
    
    if (!addr)
        obj->i2c->MSTCTL = (1 << 0);
    
    // wait and return status
    i2c_wait_SI(obj);
    return i2c_status(obj);
}

int i2c_read(i2c_t *obj, int address, char *data, int length, int stop) {
    int count, status;
    
    i2c_start(obj);
    
    status = i2c_do_write(obj, (address | 0x01), 1);
    if (status != 0x01) {
        i2c_stop(obj);
        return I2C_ERROR_NO_SLAVE;
    }
    
    // Read in all except last byte
    for (count = 0; count < (length - 1); count++) {
        int value = i2c_do_read(obj, 0);
        status = i2c_status(obj);
        if (status != 0x00) {
            i2c_stop(obj);
            return count;
        }
        data[count] = (char) value;
    }
    
    // read in last byte
    int value = i2c_do_read(obj, 1);
    status = i2c_status(obj);
    if (status != 0x01) {
        i2c_stop(obj);
        return length - 1;
    }
    
    data[count] = (char) value;
    
    // If not repeated start, send stop.
    if (stop) {
        i2c_stop(obj);
    } else {
        repeated_start = 1;
    }
    
    return length;
}

static void i2c_wait(void)
{
#if 0
	while (!(I2C0_S & I2C_S_IICIF)) ; // wait
	I2C0_S = I2C_S_IICIF;
#endif
	//Serial.write('^');
	while (1) {
		if ((i2c_status() & I2C_S_IICIF)) break;
	}
	I2C0_S = I2C_S_IICIF;
}

static int i2c_wait_START(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_START)) {
        timeout ++;
        if (timeout >= TIMEOUT_1S) {
            return -1;
        }
    }

    return 0;
}

static int i2c_wait_STOP(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_STOP) == 0) {
        timeout ++;
        if (timeout >= WAIT_TIMEOUT) {
            return -1;
        }
    }

    return 0;
}