// Simple user-created API to IIC controller
// http://forums.xilinx.com/t5/Embedded-Processor-System-Design/Why-the-IIC-can-t-be-stopped/td-p/295693
u32 iic_write(u32 baseaddr, u32 daddr, u32 waddr, u32 wdata) {
    int stat2;
    u32 timeout;
    int stat = (Xil_In32(baseaddr + 0x104));
    if(stat != 0xc0)
    {
    //    print("Bad stat.");
        Xil_Out32((baseaddr + 0x100), 0xc0|0x02); // reset tx fifo
//        Xil_Out32((baseaddr + 0x100), 0xc0|0x03); // enable iic
    }
    Xil_Out32((baseaddr + 0x100), 0xc0|0x00); // disable iic
    Xil_Out32((baseaddr + 0x100), 0xc0|0x01); // enable iic

    Xil_Out32((baseaddr + 0x100), 0xc0|0x0d); // Start; Transmit
    Xil_Out32((baseaddr + 0x108), (0x100 | (daddr & 0xFE))); // send slave address for write
    Xil_Out32((baseaddr + 0x108), waddr); // send register address
    Xil_Out32((baseaddr + 0x108), (0x200 | (wdata & 0xFF))); // send data byte with stop bit
    timeout = 100; // timeout
    while(1)    // ToDo: add timeout to avoid hang up when I2C slave is not responding (no ACK)
    {
        stat2 = Xil_In32(baseaddr + 0x104);
        // wait for TX FIFO empty
        if( ((stat2 & 0x80) ) && ((stat2 & 0x04)== 0) )    // TX FIFO empty && Bus not busy
            break;
        if (timeout == 0) {
            xil_printf("iic write timed out %x %x\n\r", waddr, wdata);
            return 0xFFFFFFFF; // data not valid
        }
        timeout--;
        usleep(200);
    }
    return 0;
}


u32 iic_read(u32 baseaddr, u32 daddr, u32 raddr) {
    int stat2;
    u32 rdata, timeout;
    int stat = (Xil_In32(baseaddr + 0x104));
    if(stat != 0xc0)
    {
    //    print("Bad stat.");
        Xil_Out32((baseaddr + 0x100), 0x002); // reset tx fifo
        Xil_Out32((baseaddr + 0x100), 0x003); // enable iic
    }
    Xil_Out32((baseaddr + 0x100), 0x000); // disable iic
    Xil_Out32((baseaddr + 0x100), 0x001); // enable iic

    Xil_Out32((baseaddr + 0x108), (0x100 | (daddr & 0xFE))); // send slave address for write
    Xil_Out32((baseaddr + 0x108), raddr); // send register address
    Xil_Out32((baseaddr + 0x108), (0x101 | (daddr & 0xFE))); // send slave address for read
    Xil_Out32((baseaddr + 0x108), 0x201); // set stop after 1 data byte
    timeout = 100; // timeout
    while(1)
    {
        stat2 = Xil_In32(baseaddr + 0x104);
        if ((((stat2 & 0x40) == 0x0)) || (timeout == 0)) // (RX FIFO not empty && Bus not busy) or timeout    //  && ((stat2 & 0x04)== 0)
            break;
            //(((Xil_In32(IIC_BASEADDR + 0x104) & 0x40) == 0x40) && (timeout != 0)) { // wait for read FIFO not empty
        timeout--;
        delay_ms(1);
    }
    rdata = Xil_In32(baseaddr + 0x10C) & 0xFF; // read data byte
    delay_ms(1);
    if (timeout == 0) {
        xil_printf("iic read timed out %x %x\n\r", raddr, rdata);
        return 0xFFFFFFFF; // data not valid
    }
    return (rdata);
}