void TwoWire::end()
{
    NVIC_DisableIRQ(irqNumber);
    I2C_HAL_Disable(instance);
    I2C_HAL_SetAddress7bit(instance, 0);
    SIM_HAL_DisableClock(SIM, gate_name);
}
/*FUNCTION**********************************************************************
 *
 * Function Name : I2C_DRV_SlaveInit
 * Description   : initializes the I2C module.
 * This function will save the application callback info, turn on the clock of
 * I2C instance, setup according to user configuration.
 *
 *END**************************************************************************/
void I2C_DRV_SlaveInit(uint32_t instance,
                       const i2c_slave_user_config_t * userConfigPtr,
                       i2c_slave_state_t * slave)
{
    assert(slave);
    assert(instance < HW_I2C_INSTANCE_COUNT);

    uint32_t baseAddr = g_i2cBaseAddr[instance];

    /* Exit if current instance is already initialized. */
    if (g_i2cStatePtr[instance])
    {
        return;
    }

    /* Init driver instance structure */
    memset(slave, 0, sizeof(i2c_slave_state_t));
    slave->slaveListening = userConfigPtr->slaveListening;
    slave->slaveCallback = userConfigPtr->slaveCallback;
    slave->callbackParam = userConfigPtr->callbackParam;

    /* Enable clock for I2C.*/
    CLOCK_SYS_EnableI2cClock(instance);

    /* Init instance to known state. */
    I2C_HAL_Init(baseAddr);

    /* Set slave address.*/
    I2C_HAL_SetAddress7bit(baseAddr, userConfigPtr->address);

    /* Save runtime structure pointer.*/
    g_i2cStatePtr[instance] = slave;

    /* Create Event for irqSync */
    OSA_EventCreate(&slave->irqEvent, kEventAutoClear);

#if FSL_FEATURE_I2C_HAS_START_STOP_DETECT
    /* Enable I2C START&STOP signal detect interrupt in the peripheral.*/
    if(userConfigPtr->startStopDetect)
    {
        I2C_HAL_SetStartStopIntCmd(baseAddr,true);
    }
#endif
#if FSL_FEATURE_I2C_HAS_STOP_DETECT
    /* Enable STOP signal detect interrupt in the peripheral.*/
    if(userConfigPtr->stopDetect)
    {
        I2C_HAL_SetStopIntCmd(baseAddr,true);
    }
#endif

    /* Enable I2C interrupt as default if setup slave listening mode */
    I2C_HAL_SetIntCmd(baseAddr, slave->slaveListening);

    /* Enable I2C interrupt from NVIC */
    INT_SYS_EnableIRQ(g_i2cIrqId[instance]);

    /* Enable the peripheral operation.*/
    I2C_HAL_Enable(baseAddr);
}
// Public Methods //////////////////////////////////////////////////////////////
void TwoWire::begin(uint8_t address)
{
    rxBufferIndex = 0;
    rxBufferLength = 0;

    txBufferIndex = 0;
    txBufferLength = 0;

    slaveBufferIndex = 0;
    slaveBufferLength = 0;

    transmitting_master = false;
    transmitting_slave = false;
    receiving_slave = false;

    SIM_HAL_EnableClock(SIM, gate_name);

    PORT_CLOCK_ENABLE(sda);
    PORT_CLOCK_ENABLE(scl);
    PORT_BWR_PCR_ODE(PERIPH_PORT(sda), PINS_PIN(sda), true); //set as open drain
    PORT_BWR_PCR_ODE(PERIPH_PORT(scl), PINS_PIN(scl), true); //set as open drain
    PORT_SET_MUX_I2C(sda);
    PORT_SET_MUX_I2C(scl);
    I2C_HAL_Init(instance);

    I2C_HAL_SetAddress7bit(instance, address);
    I2C_HAL_SetStartStopIntCmd(instance, true);
    I2C_HAL_SetIntCmd(instance, true);
    NVIC_EnableIRQ(irqNumber);
    I2C_HAL_Enable(instance);
    setClock(100000);
}
/*!
* @brief The i2c slave
* The function runs i2c slave with polling mode (HAL layer). Slave receive data
* from master and echo back to master
*/
void main(void)
{
    // Number byte data will be transfer
    uint32_t count = 0;
    // Buffer store data to transfer
    uint8_t dataBuff[50] = {0};
    // slave address
    uint16_t address = 0x7FU;
    // i2c slave base address
    I2C_Type * baseAddr = (I2C_Type*)I2C0_BASE;

    // Initialize hardware
    hardware_init();

    // Configure pin for i2c slave
    configure_i2c_pins(BOARD_I2C_COMM_INSTANCE);

    // Initialize uart to debug
    dbg_uart_init();

    printf("Slave is running ...");

    /* Enable clock for I2C.*/
    CLOCK_SYS_EnableI2cClock(BOARD_I2C_COMM_INSTANCE);

    /* Init instance to known state. */
    I2C_HAL_Init(baseAddr);

    /* Set slave address.*/
    I2C_HAL_SetAddress7bit(baseAddr, address);

    /* Enable the peripheral operation.*/
    I2C_HAL_Enable(baseAddr);

    // Loop transfer
    while(1)
    {
        // count is length of string
        I2C_HAL_SlaveReceiveDataPolling(baseAddr, (uint8_t*)&count, 1);

        // Slave receive buffer from master
        I2C_HAL_SlaveReceiveDataPolling(baseAddr, dataBuff, count);

        // Slave send buffer received from master
        I2C_HAL_SlaveSendDataPolling(baseAddr, dataBuff, count);
    }
}