#include <stdint.h>
#include <stdbool.h>
#include "board_util.h"
#include "system.h"
#include "user_io.h"
#include "uartstdio.h"  // User local version with larger RX buffer
#include "inc/hw_i2c.h"
#include "inc/hw_memmap.h"
#include "inc/hw_types.h"
#include "inc/hw_gpio.h"
#include "driverlib/i2c.h"
#include "driverlib/sysctl.h"
#include "driverlib/gpio.h"
#include "driverlib/pin_map.h"

#include "arm_acle.h"

#include "i2c0.h"
#include "i2c_pcal64xx.h"

static int i2c0_initFlg = 0;

void i2c0_isr_handler()
{
    // We currently poll the isr status bits, rather than actually using the isr.
    // If this is reached the I2C interrupt was accidentally turned on
    // Just lock up here to trigger a fault - if we switch to actually use interrupts this will be implemented
    for(;;)
    {
        crash_phfetsos();
    }
}

static void i2c0_bitbang_start_one()
{
    // Assert SDA
    MAP_GPIOPinWrite(GPIO_PORTB_BASE, GPIO_PIN_3, 0);
    delay_usec(15);

    // Assert SCL
    MAP_GPIOPinWrite(GPIO_PORTB_BASE, GPIO_PIN_2, 0);
    delay_usec(15);

    // Release SDA (encode 1)
    MAP_GPIOPinWrite(GPIO_PORTB_BASE, GPIO_PIN_3, GPIO_PIN_3);
    delay_usec(15);

    // Release SCL (latch 1)
    MAP_GPIOPinWrite(GPIO_PORTB_BASE, GPIO_PIN_2, GPIO_PIN_2);
    delay_usec(15);
}

static void i2c0_bitbang_start_stop()
{
    // toggle SDA with high clock to send a start / stop pulse

    MAP_GPIOPinWrite(GPIO_PORTB_BASE, GPIO_PIN_2, GPIO_PIN_2);
    delay_usec(15);

    MAP_GPIOPinWrite(GPIO_PORTB_BASE, GPIO_PIN_3, 0);
    delay_usec(15);

    MAP_GPIOPinWrite(GPIO_PORTB_BASE, GPIO_PIN_3, GPIO_PIN_3);
    delay_usec(15);

}

int i2c0_stuck_bus_release()
{
    GPIOPinTypeGPIOInput(GPIO_PORTB_BASE, GPIO_PIN_2 | GPIO_PIN_3);

    int32_t pins = GPIOPinRead(GPIO_PORTB_BASE, GPIO_PIN_2 | GPIO_PIN_3) & 0xF;
    int32_t scl = pins & GPIO_PIN_2;
    // int32_t sda = pins & GPIO_PIN_3;

    if( ! scl )
    {
        // Fatal, power cycle
        return 0;
    }

    // send 9 clock pulses
    GPIOPinWrite(GPIO_PORTB_BASE, GPIO_PIN_2 | GPIO_PIN_3, GPIO_PIN_2 | GPIO_PIN_3);
    GPIOPinTypeGPIOOutputOD(GPIO_PORTB_BASE, GPIO_PIN_2 | GPIO_PIN_3);
    delay_usec(15);

    // 15us -> 66kHz, go slow since we don't monitor the rise time
    for(size_t i = 0; i < 10; i++)
    {
        i2c0_bitbang_start_one();
    }

    i2c0_bitbang_start_stop();

    // restore i2c mode
    GPIOPinConfigure(GPIO_PB2_I2C0SCL);
    GPIOPinConfigure(GPIO_PB3_I2C0SDA);

    GPIOPinTypeI2CSCL(GPIO_PORTB_BASE, GPIO_PIN_2);
    GPIOPinTypeI2C(GPIO_PORTB_BASE, GPIO_PIN_3);

    _dsb();
    _isb();

    return 1;
}

bool i2c_mcs_is_error_write_op(const uint32_t mcs)
{
    return mcs & (I2C_MCS_ERROR | I2C_MCS_CLKTO | I2C_MCS_ARBLST);
}

bool i2c_mcs_is_error_read_op(const uint32_t mcs)
{
#if defined(TARGET_IS_TM4C129_RA0) || defined(TARGET_IS_TM4C129_RA1) || defined(TARGET_IS_TM4C129_RA2)
    return mcs & (I2C_MCS_ERROR | I2C_MCS_ARBLST | I2C_MCS_CLKTO);
#elif defined(TARGET_IS_TM4C123_RA1) || defined(TARGET_IS_TM4C123_RA3) || defined(TARGET_IS_TM4C123_RB1) || defined(TARGET_IS_TM4C123_RB2)
    // For TM4C123 spmz849f errata I2C#07, the DATACK bit may get stuck set if a valid read happens after a write to a non-ack address
    // I2C_MCS_ERROR is supposed to be ADRACK | DATACK | ARBLST
    if(mcs & I2C_MCS_ERROR)
    {
        return mcs & (I2C_MCS_CLKTO | I2C_MCS_ARBLST | I2C_MCS_ADRACK);
    }
    else
    {
        return false;
    }
#else
    #error "Unknown target"
#endif
}

static void clear_i2c_mris(uint32_t base)
{
#if defined(TARGET_IS_TM4C129_RA0) || defined(TARGET_IS_TM4C129_RA1) || defined(TARGET_IS_TM4C129_RA2)
    HWREG(base + I2C_O_MICR) = I2C_MICR_RXFFIC | I2C_MICR_TXFEIC | I2C_MICR_RXIC | I2C_MICR_TXIC | I2C_MICR_ARBLOSTIC | I2C_MICR_STOPIC | I2C_MICR_STARTIC | I2C_MICR_NACKIC | I2C_MICR_DMATXIC | I2C_MICR_DMARXIC | I2C_MICR_CLKIC | I2C_MICR_IC;
#elif defined(TARGET_IS_TM4C123_RA1) || defined(TARGET_IS_TM4C123_RA3) || defined(TARGET_IS_TM4C123_RB1) || defined(TARGET_IS_TM4C123_RB2)
    HWREG(base + I2C_O_MICR) = I2C_MICR_CLKIC | I2C_MICR_IC;
#else
    #error "Unknown target"
#endif
}

//
// Perform a i2c operation, wait for completion, and return I2C MCS. Clears MRIS before waiting. Clears I2C_MRIS_RIS after waiting.
//
static uint32_t do_wait_i2c_operation(uint32_t base, uint32_t op)
{
    uint32_t mcs_reg;

    clear_i2c_mris(base);

    MAP_I2CMasterControl(base, op);

#if defined(TARGET_IS_TM4C129_RA0) || defined(TARGET_IS_TM4C129_RA1) || defined(TARGET_IS_TM4C129_RA2)

    uint32_t mris_reg;
    do
    {
        mris_reg = HWREG(base + I2C_O_MRIS);
    } while( ((mris_reg & I2C_MRIS_RIS) == 0) && ((mris_reg & I2C_MRIS_CLKRIS) == 0) );

    // Check for timeout
    // it seems like I2C_MCS_CLKTO is not getting set when I2C_MRIS_CLKRIS is, so set I2C_MCS_CLKTO if I2C_MRIS_CLKRIS is set
    uint32_t mcs_had_to = 0;
    if(mris_reg & I2C_MRIS_CLKRIS)
    {
        mcs_had_to = I2C_MCS_CLKTO;
    }

    HWREG(base + I2C_O_MICR) = I2C_MICR_IC | I2C_MICR_CLKIC;

    mcs_reg = HWREG(base + I2C_O_MCS) | mcs_had_to;

#elif defined(TARGET_IS_TM4C123_RA1) || defined(TARGET_IS_TM4C123_RA3) || defined(TARGET_IS_TM4C123_RB1) || defined(TARGET_IS_TM4C123_RB2)
    do
    {
        mcs_reg = HWREG(base + I2C_O_MCS);
    } while(mcs_reg & I2C_MCS_BUSY);
    HWREG(base + I2C_O_MICR) = I2C_MICR_IC;
#else
    #error "Unknown target"
#endif

    return mcs_reg;
}

//
// Only for error recovery
// Perform a i2c operation, wait for completion, and return I2C MCS. Clears MRIS before waiting. Clears I2C_MRIS_RIS after waiting.
// If the i2c controller is idle, nothing is done and MCS is returned.
//
static uint32_t do_i2c_error_operation(uint32_t base, uint32_t op)
{
    clear_i2c_mris(base);

    uint32_t temp = HWREG(base + I2C_O_MCS);
    if(temp & I2C_MCS_IDLE)
    {
        // TI says we are supposed to send a stop
        // but on address NAK it seems a stop is already generated and the controller moves to idle
        // skip sending the stop bit if controller is in idle state
        // we still want to send the stop bit if the controller is not idle
        // See Table 18-5. Write Field Decoding for I2CMCS[6:0] from ref manual
        return temp;
    }

    // If we are here, a target ACK'd an address, but NAK'd a data byte while we still wanted to read or write more
    // We need to send a stop to reset the state machine
    // JS: TODO: This code is not reachable right now, as to test we need a i2c target to intentionally NAK us mid-transaction
    // We might need to spin on a bit other than I2C_MRIS_RIS, it is unclear if I2C_MRIS_RIS is also set for a single stop operation

    do_wait_i2c_operation(base, op);

    return HWREG(base + I2C_O_MCS);
}

static int is_i2c_bus_busy(const uint32_t base)
{
    const uint32_t i2c_mcs = HWREG(base + I2C_O_MCS);
    return i2c_mcs & (I2C_MCS_BUSY | I2C_MCS_BUSBSY);
}

static int wait_for_idle_i2c(const uint32_t base, const bool reset_bus)
{
    const unsigned long t_0 = Timer_1ms();

    int ret = 1;

    while(is_i2c_bus_busy(base))
    {
        if( is_timer_1ms_expired(t_0, 1000) )
        {
            if( ! reset_bus )
            {
                //Fatal, no recovery requested
                ret = -2;
                break;
            }

            if( i2c0_stuck_bus_release() <= 0)
            {
                //Fatal, power cycle
                ret = -2;
                break;
            }

            bool is_busy = true;
            for(int i = 0; i < 10; i++)
            {
                is_busy = is_i2c_bus_busy(base);
                if( ! is_busy )
                {
                    break;
                }
                delay_usec(50);
            }

            if(is_busy)
            {
                //Timeout, we tried to recover the bus
                ret = -1;
            }
            else
            {
                //we tried to recovered the bus, and probably did
                ret = 1;
            }

            break;
        }
    }

    return ret;
}

void i2c0_init(bool bFast)
{
    if(i2c0_initFlg > 0)        // if init'd, then return
        return;

    //enable onchip I2C peripheral
    MAP_SysCtlPeripheralEnable(SYSCTL_PERIPH_I2C0);

    // wait for enable
    while( ! MAP_SysCtlPeripheralReady(SYSCTL_PERIPH_I2C0) )
    {

    }

    //reset I2C module
    MAP_SysCtlPeripheralReset(SYSCTL_PERIPH_I2C0);

    // wait for reset
    while( ! MAP_SysCtlPeripheralReady(SYSCTL_PERIPH_I2C0) )
    {

    }

    // Mask ISR off
    MAP_I2CMasterIntDisable(I2C0_BASE);
    // Sync to ensure isr is off
    _dsb();
    _isb();

    // Set the isr handler
    I2CIntRegister(I2C0_BASE, &i2c0_isr_handler);
    // Sync to ensure handler is set
    _dsb();
    _isb();

    // Also disable at the NVIC level
    // On TM4C123G I am getting interrupts even when the mask register is 0
    MAP_IntDisable(INT_I2C0);
    _dsb();
    _isb();

    // Configure the pin muxing for I2C0 functions on port PB2(SCL) and PB3(SDA).
    MAP_GPIOPinConfigure(GPIO_PB2_I2C0SCL);     // This is the same on 144 pin part and 64pin part
    MAP_GPIOPinConfigure(GPIO_PB3_I2C0SDA);
    // Sync to ensure pinmux is set
    _dsb();
    _isb();

    // Configure the appropriate pins to be I2C instead of GPIO.
    MAP_GPIOPinTypeI2CSCL(GPIO_PORTB_BASE, GPIO_PIN_2);
    MAP_GPIOPinTypeI2C(GPIO_PORTB_BASE, GPIO_PIN_3);

    // Initialize the I2C master - Transfer speed is defined depend on variable bFast
    MAP_I2CMasterInitExpClk(I2C0_BASE, GetSysClkHz(), bFast);

    // set timeout in units of I2C clocks
    // Not all devices have it, but 123 and 129 seem to
    // This sets a 10ms timeout, value is number of sck cycles downshifted by 4
    if(bFast)
    {
        MAP_I2CMasterTimeoutSet(I2C0_BASE, 4000U >> 4);
    }
    else
    {
        MAP_I2CMasterTimeoutSet(I2C0_BASE, 1000U >> 4);
    }

    // Clear I2C FIFOs only on TM4C129
#if defined(TARGET_IS_TM4C129_RA0) || defined(TARGET_IS_TM4C129_RA1) || defined(TARGET_IS_TM4C129_RA2)
    MAP_I2CRxFIFOFlush(I2C0_BASE);
    MAP_I2CTxFIFOFlush(I2C0_BASE);
    MAP_I2CTxFIFOConfigSet(I2C0_BASE, I2C_FIFO_CFG_TX_MASTER | I2C_FIFO_CFG_TX_TRIG_1);
    MAP_I2CRxFIFOConfigSet(I2C0_BASE, I2C_FIFO_CFG_RX_MASTER | I2C_FIFO_CFG_RX_TRIG_1);
#endif

    MAP_I2CMasterEnable(I2C0_BASE);      //I2C is ready to use

    i2c0_initFlg = 1;

    // Sync
    _dsb();
    _isb();
}

int i2c0_read_buf(uint8_t i2c_addr, uint8_t* out_data, int length)
{
    uint32_t i2c_mcs = 0;
    int i;

    if( wait_for_idle_i2c(I2C0_BASE, true) <= 0)
    {
        return 0;
    }

    if(i2c_addr > 127)
        i2c_addr /= 2;

    if(length <= 0)
    {
        return 0;
    }
    else if(length == 1)
    {
        MAP_I2CMasterSlaveAddrSet(I2C0_BASE, i2c_addr, true);
        i2c_mcs = do_wait_i2c_operation(I2C0_BASE, I2C_MASTER_CMD_SINGLE_RECEIVE);
        if( i2c_mcs_is_error_read_op(i2c_mcs) )
        {
            return 0;
        }
        out_data[0] = I2CMasterDataGet(I2C0_BASE);
    }
    else
    {
        MAP_I2CMasterSlaveAddrSet(I2C0_BASE, i2c_addr, true);
        i2c_mcs = do_wait_i2c_operation(I2C0_BASE, I2C_MASTER_CMD_BURST_RECEIVE_START);
        if( i2c_mcs_is_error_read_op(i2c_mcs) )
        {
            if((i2c_mcs & I2C_MCS_ARBLST) == 0)
            {
                do_i2c_error_operation(I2C0_BASE, I2C_MASTER_CMD_BURST_RECEIVE_ERROR_STOP);
            }
            return 0;
        }
        out_data[0] = I2CMasterDataGet(I2C0_BASE);

        for(i = 1; i < (length-1); i++)
        {
             i2c_mcs = do_wait_i2c_operation(I2C0_BASE, I2C_MASTER_CMD_BURST_RECEIVE_CONT);
             if( i2c_mcs_is_error_read_op(i2c_mcs) )
             {
                 if((i2c_mcs & I2C_MCS_ARBLST) == 0)
                 {
                     do_i2c_error_operation(I2C0_BASE, I2C_MASTER_CMD_BURST_RECEIVE_ERROR_STOP);
                 }
                 return 0;
             }
             out_data[i] = I2CMasterDataGet(I2C0_BASE);
        }

         i2c_mcs = do_wait_i2c_operation(I2C0_BASE, I2C_MASTER_CMD_BURST_RECEIVE_FINISH);
         if( i2c_mcs_is_error_read_op(i2c_mcs) )
         {
             if((i2c_mcs & I2C_MCS_ARBLST) == 0)
             {
                 do_i2c_error_operation(I2C0_BASE, I2C_MASTER_CMD_BURST_RECEIVE_ERROR_STOP);
             }
             return 0;
         }
         out_data[length-1] = I2CMasterDataGet(I2C0_BASE);
    }

    return 1;
}


int i2c0_write_buf(uint8_t i2c_addr, uint8_t* data, int length)
{
    return i2c0_write_then_write(i2c_addr, data, length, NULL, 0);
}

int i2c0_write_then_write(uint8_t i2c_addr, uint8_t* write1_data, int write1_length, uint8_t* write2_data, int write2_length)
{
    uint32_t i2c_mcs = 0;
    int i;

    if( wait_for_idle_i2c(I2C0_BASE, true) <= 0)
    {
        return 0;
    }

    if(i2c_addr > 127)
        i2c_addr /= 2;

    if((write1_length < 0) || (write2_length < 0) )
    {
        // write1 and write2 may not be negative
        return 0;
    }
    else if((write1_length == 0) && (write2_length != 0) )
    {
        // write1 must be nonzero if write2 is nonzero
        return 0;
    }
    else if((write1_length == 0) && (write2_length == 0) )
    {
        // no data
        return 0;
    }
    else if((write1_length == 1) && (write2_length == 0) )
    {
        // only one data section of single byte
        MAP_I2CMasterSlaveAddrSet(I2C0_BASE, i2c_addr, false);
        MAP_I2CMasterDataPut(I2C0_BASE, write1_data[0]);
        i2c_mcs = do_wait_i2c_operation(I2C0_BASE, I2C_MASTER_CMD_SINGLE_SEND);
        if( i2c_mcs_is_error_write_op(i2c_mcs) )
        {
            return 0;
        }
    }
    else
    {
        // Have first data section of several bytes OR may have second section
        MAP_I2CMasterSlaveAddrSet(I2C0_BASE, i2c_addr, false);
        MAP_I2CMasterDataPut(I2C0_BASE, write1_data[0]);
        i2c_mcs = do_wait_i2c_operation(I2C0_BASE, I2C_MASTER_CMD_BURST_SEND_START);
        if( i2c_mcs_is_error_write_op(i2c_mcs) )
        {
            if((i2c_mcs & I2C_MCS_ARBLST) == 0)
            {
                do_i2c_error_operation(I2C0_BASE, I2C_MASTER_CMD_BURST_SEND_ERROR_STOP);
            }
            return 0;
        }


        if(write1_length > 1)
        {

            for(i = 1; i < (write1_length-1); i++)
            {
                wdog_pet();

                MAP_I2CMasterDataPut(I2C0_BASE, write1_data[i]);
                i2c_mcs = do_wait_i2c_operation(I2C0_BASE, I2C_MASTER_CMD_BURST_SEND_CONT);
                if( i2c_mcs_is_error_write_op(i2c_mcs) )
                {
                    if((i2c_mcs & I2C_MCS_ARBLST) == 0)
                    {
                        do_i2c_error_operation(I2C0_BASE, I2C_MASTER_CMD_BURST_SEND_ERROR_STOP);
                    }
                    return 0;
                }
            }

            MAP_I2CMasterDataPut(I2C0_BASE, write1_data[write1_length-1]);
            if(write2_length > 0)
            {
                i2c_mcs = do_wait_i2c_operation(I2C0_BASE, I2C_MASTER_CMD_BURST_SEND_CONT);
            }
            else
            {
                i2c_mcs = do_wait_i2c_operation(I2C0_BASE, I2C_MASTER_CMD_BURST_SEND_FINISH);
            }
            if( i2c_mcs_is_error_write_op(i2c_mcs) )
            {
                if((i2c_mcs & I2C_MCS_ARBLST) == 0)
                {
                    do_i2c_error_operation(I2C0_BASE, I2C_MASTER_CMD_BURST_SEND_ERROR_STOP);
                }
                return 0;
            }
        }

        if(write2_length > 0)
        {
            for(i = 0; i < (write2_length-1); i++)
            {
                wdog_pet();

                MAP_I2CMasterDataPut(I2C0_BASE, write2_data[i]);
                i2c_mcs = do_wait_i2c_operation(I2C0_BASE, I2C_MASTER_CMD_BURST_SEND_CONT);
                if( i2c_mcs_is_error_write_op(i2c_mcs) )
                {
                    if((i2c_mcs & I2C_MCS_ARBLST) == 0)
                    {
                        do_i2c_error_operation(I2C0_BASE, I2C_MASTER_CMD_BURST_SEND_ERROR_STOP);
                    }
                    return 0;
                }
            }
            MAP_I2CMasterDataPut(I2C0_BASE, write2_data[write2_length-1]);
            i2c_mcs = do_wait_i2c_operation(I2C0_BASE, I2C_MASTER_CMD_BURST_SEND_FINISH);
            if( i2c_mcs_is_error_write_op(i2c_mcs) )
            {
                if((i2c_mcs & I2C_MCS_ARBLST) == 0)
                {
                    do_i2c_error_operation(I2C0_BASE, I2C_MASTER_CMD_BURST_SEND_ERROR_STOP);
                }
                return 0;
            }
        }
    }

     return 1;
}

int i2c0_write_then_read(uint8_t i2c_addr, uint8_t* write_data, int write_length, uint8_t* read_data, int read_length)
{
    uint32_t i2c_mcs = 0;
    int i;

    if( wait_for_idle_i2c(I2C0_BASE, true) <= 0)
    {
        return 0;
    }

    if(i2c_addr > 127)
        i2c_addr /= 2;

    if((write_length < 0) || (read_length < 0) )
    {
        // write_length and read_length may not be negative
        return 0;
    }
    else if((write_length == 0) && (read_length != 0) )
    {
        // write_length must be nonzero if read_length is nonzero
        return 0;
    }
    else if((write_length == 0) && (read_length == 0) )
    {
        // no data
        return 0;
    }
    else if((write_length == 1) && (read_length == 0) )
    {
        // only one data section of single byte
        MAP_I2CMasterSlaveAddrSet(I2C0_BASE, i2c_addr, false);
        MAP_I2CMasterDataPut(I2C0_BASE, write_data[0]);
        i2c_mcs = do_wait_i2c_operation(I2C0_BASE, I2C_MASTER_CMD_SINGLE_SEND);
        if( i2c_mcs_is_error_write_op(i2c_mcs) )
        {
            return 0;
        }
    }
    else
    {
        // have multi-byte write OR read
        MAP_I2CMasterSlaveAddrSet(I2C0_BASE, i2c_addr, false);
        MAP_I2CMasterDataPut(I2C0_BASE, write_data[0]);
        i2c_mcs = do_wait_i2c_operation(I2C0_BASE, I2C_MASTER_CMD_BURST_SEND_START);
        if( i2c_mcs_is_error_write_op(i2c_mcs) )
        {
            if((i2c_mcs & I2C_MCS_ARBLST) == 0)
            {
                do_i2c_error_operation(I2C0_BASE, I2C_MASTER_CMD_BURST_SEND_ERROR_STOP);
            }
            return 0;
        }

        if(write_length > 1)
        {
            for(i = 1; i < (write_length-1); i++)
            {
                MAP_I2CMasterDataPut(I2C0_BASE, write_data[i]);
                i2c_mcs = do_wait_i2c_operation(I2C0_BASE, I2C_MASTER_CMD_BURST_SEND_CONT);
                if( i2c_mcs_is_error_write_op(i2c_mcs) )
                {
                    if((i2c_mcs & I2C_MCS_ARBLST) == 0)
                    {
                        do_i2c_error_operation(I2C0_BASE, I2C_MASTER_CMD_BURST_SEND_ERROR_STOP);
                    }
                    return 0;
                }
            }

            MAP_I2CMasterDataPut(I2C0_BASE, write_data[write_length-1]);
            if(read_length > 0)
            {
                i2c_mcs = do_wait_i2c_operation(I2C0_BASE, I2C_MASTER_CMD_BURST_SEND_CONT);
            }
            else
            {
                i2c_mcs = do_wait_i2c_operation(I2C0_BASE, I2C_MASTER_CMD_BURST_SEND_FINISH);
            }
            if( i2c_mcs_is_error_write_op(i2c_mcs) )
            {
                if((i2c_mcs & I2C_MCS_ARBLST) == 0)
                {
                    do_i2c_error_operation(I2C0_BASE, I2C_MASTER_CMD_BURST_SEND_ERROR_STOP);
                }
                return 0;
            }
        }

        if(read_length > 0)
        {
            MAP_I2CMasterSlaveAddrSet(I2C0_BASE, i2c_addr, true);
            if(read_length == 1)
            {
                i2c_mcs = do_wait_i2c_operation(I2C0_BASE, I2C_MASTER_CMD_SINGLE_RECEIVE); // IE, RUN/START/STOP no ack
            }
            else
            {
                i2c_mcs = do_wait_i2c_operation(I2C0_BASE, I2C_MASTER_CMD_BURST_RECEIVE_START);
                if( i2c_mcs_is_error_read_op(i2c_mcs) )
                {
                    if((i2c_mcs & I2C_MCS_ARBLST) == 0)
                    {
                        do_i2c_error_operation(I2C0_BASE, I2C_MASTER_CMD_BURST_RECEIVE_ERROR_STOP);
                    }
                    return 0;
                }
            }
            read_data[0] = I2CMasterDataGet(I2C0_BASE);

            if(read_length > 1)
            {
                for(i = 1; i < (read_length-1); i++)
                {
                     i2c_mcs = do_wait_i2c_operation(I2C0_BASE, I2C_MASTER_CMD_BURST_RECEIVE_CONT);
                     if( i2c_mcs_is_error_read_op(i2c_mcs) )
                     {
                         if((i2c_mcs & I2C_MCS_ARBLST) == 0)
                         {
                             do_i2c_error_operation(I2C0_BASE, I2C_MASTER_CMD_BURST_RECEIVE_ERROR_STOP);
                         }
                         return 0;
                     }
                     read_data[i] = I2CMasterDataGet(I2C0_BASE);
                }

                 i2c_mcs = do_wait_i2c_operation(I2C0_BASE, I2C_MASTER_CMD_BURST_RECEIVE_FINISH);
                 if( i2c_mcs_is_error_read_op(i2c_mcs) )
                 {
                     if((i2c_mcs & I2C_MCS_ARBLST) == 0)
                     {
                         do_i2c_error_operation(I2C0_BASE, I2C_MASTER_CMD_BURST_RECEIVE_ERROR_STOP);
                     }
                     return 0;
                 }
                 read_data[read_length-1] = I2CMasterDataGet(I2C0_BASE);
            }
        }
    }

     return 1;
}

int i2c0_read_reg8(uint8_t i2c_addr, uint8_t reg_addr, uint8_t* out_data)
{
    return i2c0_write_then_read(i2c_addr, &reg_addr, 1, out_data, 1);
}
int i2c0_read_reg16(uint8_t i2c_addr, uint8_t reg_addr, uint16_t* out_data)
{
    return i2c0_write_then_read(i2c_addr, &reg_addr, 1, (uint8_t*)out_data, 2);
}

int i2c0_write_reg8(uint8_t i2c_addr, uint8_t reg_addr, uint8_t data)
{
    return i2c0_write_then_write(i2c_addr, &reg_addr, 1, &data, 1);
}

int i2c0_write_reg16(uint8_t i2c_addr, uint8_t reg_addr, uint16_t data)
{
    return i2c0_write_then_write(i2c_addr, &reg_addr, 1,  (uint8_t*)&data, 2);
}

int i2c0_busScan(void)
{
    uint32_t i2c_mcs = 0;
    uint8_t ucProbeAdress;

    for (ucProbeAdress=0; ucProbeAdress<127; ucProbeAdress++)
    {
        if( wait_for_idle_i2c(I2C0_BASE, true) <= 0)
        {
            return 0;
        }

        MAP_I2CMasterSlaveAddrSet(I2C0_BASE, ucProbeAdress, false);
        MAP_I2CMasterDataPut(I2C0_BASE, 0x00);

        i2c_mcs = do_wait_i2c_operation(I2C0_BASE, I2C_MASTER_CMD_SINGLE_SEND);
        if( i2c_mcs & (I2C_MCS_ADRACK | I2C_MCS_ARBLST | I2C_MCS_CLKTO) ) // We could retry on I2C_MCS_ARBLST, but since this is a single master system treat as error
        {
            continue;
        }

        uprintf("Address found:     0x%2x - %3d   (0x%02x)",ucProbeAdress,ucProbeAdress,ucProbeAdress/2);
#if BOARD_MSC == 3
        switch(ucProbeAdress)
        {
        //case 0x20:
        case PCAL6416_I2C_ADDR:
            uprintf("  PCAL6416 I/O expander on baseboard \r\n");
            break;
        //case 0x21:
        case PCAL6408A_I2C_ADDR:
            uprintf("  PCA6408A I/O expander on Tiva Module \r\n");
            break;
        case 0x40:
            uprintf("  INA260 on Tiva 1290 'rapido' Module \r\n");
            break;

        case 0x4c:
        case 0x4f:
            uprintf("  INA260 on baseboard \r\n");
            break;
        case 0x50:
        case 0x51:
            uprintf("  FRAM on Tiva Module \r\n");
            break;
        case 0x68:
            uprintf("  DS3231 RTC on baseboard \r\n");
            break;
        case 0x6B:
            uprintf("  DS1683 Event Timer on baseboard \r\n");
            break;
        case 0x77:
            uprintf("  BME280 on baseboard \r\n");
            break;
        default:
            prn_nl();
            break;
        }

#endif
        wait_consoleTx();
    }

    uprintf("\r\nI2C0 Bus-Scan done...\r\n");

    return 1;
}

void i2c0_probe(void)
{
    i2c0_busScan();
}
