/**
 * \file    drv_i2c.c
 * \brief   I2C interface drivers
 *
 * \par     Copyright (c) 2018 UWE Technologies Corp., Ltd. All Rights Reserved.
 * \author  James
 * \date    2018-04-23
 * \version 
 * 
 * \history 
 *  Date        Note
 *  2018-04-23  Original Creation
 * 
 */

#include "arch.h"
#include "libc.h"
#include "drv_dma.h"
#include "drv_i2c.h"
#include "drv_interrupt.h"
#include "sync.h"
#include "drv_gpio.h"
#include "drv_uart.h"
#if SAL_MSQ_ENABLED
#include "msq.h"
#endif

#if I2C_MODULE_ENABLED

#if SYS_FREERTOS_ENABLED
extern QueueHandle_t xInterfaceQueue;
#endif /* SYS_FREERTOS_ENABLED */


u32_t g_i2c_addr_offset_map[DRV_I2C_ID_MAX] = 
{
    0,
    0x100
};

i2c_info_t g_i2c_info[DRV_I2C_ID_MAX];



void drv_i2c_init(void)
{
    memset((void *)&g_i2c_info[0], 0, DRV_I2C_ID_MAX*sizeof(i2c_info_t));
}

/**
 * I2C Interface Initialization
 */
void drv_i2c_intf_init(DRV_I2C_ID_t i2c_id)
{
    // enable I2C interfaces
    DRV_REG_GET(I2C_CTRL_REG+g_i2c_addr_offset_map[i2c_id]) = I2C_EN;

    // enable I2C ACK
    drv_i2c_set_ack(i2c_id); 

    if (g_i2c_info[i2c_id].intr_en_flag)
    {
        //enable I2C interrupt
        drv_i2c_interrupt_enable(i2c_id);
    }
}

/**
 * I2C interrupt handler
 *
 * \see
 */
void drv_i2c_intr_handler(u32_t arg)
{
    int i2c_id = 0; 

    if ((IPIC_CFG_INTR_IDX_I2C_INT <= arg) && (arg <= IPIC_CFG_INTR_IDX_SEC_I2C_INT))
    {
        i2c_id = arg - IPIC_CFG_INTR_IDX_I2C_INT;
    }

    //disable I2C interrupt
    drv_i2c_interrupt_disable(i2c_id);

#if I2C_ISR_DBG_ENABLED
	g_i2c_info[i2c_id].intr_occurence_cnt++;
	g_i2c_info[i2c_id].sta_buf[g_i2c_info[i2c_id].sta_buf_idx++] = (u8_t)DRV_REG_GET(I2C_STA_REG+g_i2c_addr_offset_map[i2c_id]);
	if(g_i2c_info[i2c_id].sta_buf_idx >= 32)  g_i2c_info[i2c_id].sta_buf_idx = 0;
#endif

    switch (DRV_REG_GET(I2C_STA_REG+g_i2c_addr_offset_map[i2c_id]))
    {

    /* * * Master General * * */

    case I2C_STA_START:             // 0x08: Sent start condition (slave address and R/W)
    case I2C_STA_REP_START:         // 0x10: Sent repeated start condition
        drv_i2c_clear_start(i2c_id);
        if (g_i2c_info[i2c_id].tx_data_index < g_i2c_info[i2c_id].tx_data_len)
        {
            i2c_master_write_mode_set(i2c_id, g_i2c_info[i2c_id].sla_addr);
        }
        else if (g_i2c_info[i2c_id].rx_data_index < g_i2c_info[i2c_id].rx_data_len)
        {
            i2c_master_read_mode_set(i2c_id, g_i2c_info[i2c_id].sla_addr);
        }
        DRV_REG_GET(I2C_DATA_REG+g_i2c_addr_offset_map[i2c_id]) = g_i2c_info[i2c_id].sla_addr;
        break;

    /* * * Master Transmitter & Receiver status codes * * */

    case I2C_STA_MT_SLA_ACK:        // 0x18: SLA+W  transmitted, ACK received
    case I2C_STA_MT_DATA_ACK:       // 0x28: Data transmitted, ACK received
        if (g_i2c_info[i2c_id].tx_data_index < g_i2c_info[i2c_id].tx_data_len)
        {
            // transmit data
            DRV_REG_GET(I2C_DATA_REG+g_i2c_addr_offset_map[i2c_id]) = g_i2c_info[i2c_id].tx_data[g_i2c_info[i2c_id].tx_data_index++];
        }
        else if (g_i2c_info[i2c_id].rx_data_index < g_i2c_info[i2c_id].rx_data_len)
        {
            // generate repeat start
            delay(50);
            drv_i2c_set_start(i2c_id);
        }
        else {
            // transmit stop
            drv_i2c_set_stop(i2c_id);
            g_i2c_info[i2c_id].last_status = I2C_OK;
        }
        break;

    case I2C_STA_MR_SLA_NAK:        // 0x48: SLA+W/R transmitted, NOT ACK received
    case I2C_STA_MT_SLA_NAK:        // 0x20: SLA+W/R transmitted, NOT ACK received
        drv_i2c_set_stop(i2c_id);
        g_i2c_info[i2c_id].last_status = I2C_NO_ACK_ON_ADDRESS;
        break;

    case I2C_STA_MT_DATA_NAK:       // 0x30: DATA transmitted, NOT ACK received
        drv_i2c_set_stop(i2c_id);
        g_i2c_info[i2c_id].last_status = I2C_NO_ACK_ON_DATA;
        break;

    case I2C_STA_MT_ARB_LOST:       // 0x38: Bus arbitration lost in SLA+W or DATA
        drv_i2c_intf_init(i2c_id);
        g_i2c_info[i2c_id].last_status = I2C_ARBITRATION_LOST;
        break;

    case I2C_STA_MR_DATA_ACK:       // 0x50: DATA received, ACK has been returned
        g_i2c_info[i2c_id].rx_data[g_i2c_info[i2c_id].rx_data_index++] = DRV_REG_GET(I2C_DATA_REG+g_i2c_addr_offset_map[i2c_id]);
        // no break, continue to see if more bytes will be received
    case I2C_STA_MR_SLA_ACK:        // 0x40: SLA+R transmitted, ACK received
        if (g_i2c_info[i2c_id].rx_data_index < (g_i2c_info[i2c_id].rx_data_len-1))
            drv_i2c_set_ack(i2c_id); // ACK on next byte
        else
            drv_i2c_clear_ack(i2c_id); // No ack on next byte
        break;

    case I2C_STA_MR_DATA_NAK:       // 0x58: DATA received, NOT ACK has been returned
        g_i2c_info[i2c_id].rx_data[g_i2c_info[i2c_id].rx_data_index++] = DRV_REG_GET(I2C_DATA_REG+g_i2c_addr_offset_map[i2c_id]);
        if (g_i2c_info[i2c_id].rx_data_index < g_i2c_info[i2c_id].rx_data_len)
            g_i2c_info[i2c_id].last_status = I2C_INCOMPLETE_TRANSFER;
        else
            g_i2c_info[i2c_id].last_status = I2C_OK;
        drv_i2c_set_stop(i2c_id);
        break;

#if I2C_MODULE_SLA_ENABLED
    /* * * Slave Receiver status codes * * */

    case I2C_STA_SR_SLA_ACK:            // 0x60: own SLA+W has been received, ACK has been returned
    case I2C_STA_SR_ARB_LOST_SLA_ACK:   // 0x68: own SLA+W has been received, ACK has been returned
    case I2C_STA_SR_GCALL_ACK:          // 0x70:     GCA+W has been received, ACK has been returned
    case I2C_STA_SR_ARB_LOST_GCALL_ACK: // 0x78:     GCA+W has been received, ACK has been returned
        drv_i2c_set_ack(i2c_id);
        break;

    case I2C_STA_SR_DATA_ACK:           // 0x80: data byte has been received, ACK has been returned
    case I2C_STA_SR_GCALL_DATA_ACK:     // 0x90: data byte has been received, ACK has been returned
        g_i2c_info[i2c_id].rx_data[g_i2c_info[i2c_id].rx_data_index++] = DRV_REG_GET(I2C_DATA_REG+g_i2c_addr_offset_map[i2c_id]);
        if ( g_i2c_info[i2c_id].rx_data_index < I2C_BUFFER_SIZE )
        {
            drv_i2c_set_ack(i2c_id); // ACK on next byte
        }
        else {
            drv_i2c_clear_ack(i2c_id); // No ack on next byte
            g_i2c_info[i2c_id].last_status = I2C_BUFFER_OVERFLOV_ERR;
        }
        break;


    case I2C_STA_SR_DATA_NAK:           // 0x88: data byte has been received, NAK has been returned
    case I2C_STA_SR_GCALL_DATA_NAK:     // 0x98: data byte has been received, NAK has been returned
        g_i2c_info[i2c_id].rx_data[g_i2c_info[i2c_id].rx_data_index++] = DRV_REG_GET(I2C_DATA_REG+g_i2c_addr_offset_map[i2c_id]);
        /* pass along to restart slave mode */
    case I2C_STA_SR_STOP:               // 0xA0: STOP or REPEATED START has been received while addressed as slave
        // Buffer overflow routine
        if (g_i2c_info[i2c_id].rx_data_index >= I2C_BUFFER_SIZE){
            g_i2c_info[i2c_id].last_status = I2C_BUFFER_OVERFLOV_ERR;
        }
        else {
            g_i2c_info[i2c_id].last_status = I2C_OK;
        }
        /* i2c receive is complete, call sla_rx_func */
        if (g_i2c_info[i2c_id].sla_rx_func) {
            g_i2c_info[i2c_id].sla_rx_func(i2c_id, g_i2c_info[i2c_id].rx_data, g_i2c_info[i2c_id].rx_data_index); 
            g_i2c_info[i2c_id].rx_data_index = 0;
        }
        else {
            // set g_i2c_info[i2c_id].intr_en_flag to false to don't enable I2C interrupt
            g_i2c_info[i2c_id].intr_en_flag = 0;
#if SAL_MSQ_ENABLED
            msq_push_msg(SAL_MSQ_I2C_SLA+i2c_id, 0, 0, 0, 0);
#else
            xQueueSendFromISR(xInterfaceQueue, (void *)&arg, NULL);
#endif /* SAL_MSQ_ENABLED */
        }
        // enable I2C ACK
        drv_i2c_set_ack(i2c_id); 
        break;

    /* * * Slave Transmitter * * */

    case I2C_STA_ST_SLA_ACK:            // 0xA8: own SLA+R has been received, ACK has been returned
    case I2C_STA_ST_ARB_LOST_SLA_ACK:   // 0xB0:     GCA+R has been received, ACK has been returned
        // request data from application
        if(g_i2c_info[i2c_id].sla_tx_func) 
        {
            g_i2c_info[i2c_id].tx_data_len = g_i2c_info[i2c_id].sla_tx_func(i2c_id, I2C_BUFFER_SIZE, g_i2c_info[i2c_id].tx_data);
        }
        g_i2c_info[i2c_id].tx_data_index = 0;
        /* fall-through to transmit first data byte */
    case I2C_STA_ST_DATA_ACK:   // 0xB8: data byte has been transmitted, ACK has been received
        // transmit data
        DRV_REG_GET(I2C_DATA_REG+g_i2c_addr_offset_map[i2c_id]) = g_i2c_info[i2c_id].tx_data[g_i2c_info[i2c_id].tx_data_index++];
        break;

    case I2C_STA_ST_DATA_NAK:   // 0xC0: data byte has been transmitted, NACK has been received
    case I2C_STA_ST_LAST_DATA:  // 0xC8: last data byte transmitted, ACK received
        g_i2c_info[i2c_id].last_status = I2C_OK;
        drv_i2c_intf_init(i2c_id);
        break;
#endif /* I2C_MODULE_SLA_ENABLED */

    /* * * Misc * * */
    case I2C_STA_BUS_ERROR:     // 0x00: Bus error
        drv_i2c_set_stop(i2c_id);
        g_i2c_info[i2c_id].last_status = I2C_BUS_ERROR;
        break;

    case I2C_STA_NO_INFO:
        break;

#if 0  /* deleted by James Gan - 2018-01-04 */
    default:
        drv_i2c_intf_init(i2c_id);
        break;
#endif /* #if 0 */
    }

    // enable I2C interrupt
    if ( g_i2c_info[i2c_id].intr_en_flag )
    {
        drv_i2c_clear_si(i2c_id);
        drv_i2c_interrupt_enable(i2c_id);
    }
    else {
        g_i2c_info[i2c_id].intr_en_flag = 1;
    }
}

void drv_i2c_master_start(DRV_I2C_ID_t i2c_id)
{
    g_i2c_info[i2c_id].rx_data_index = 0;
    g_i2c_info[i2c_id].tx_data_index = 0;

    if (g_i2c_info[i2c_id].rx_data_len)
        memset((char *)&(g_i2c_info[i2c_id].rx_data[0]),0,g_i2c_info[i2c_id].rx_data_len);

    g_i2c_info[i2c_id].last_status = I2C_DATA_TRANSFER;

    //enable I2C interrupt
    drv_i2c_interrupt_enable(i2c_id);

    // generate START condition
    drv_i2c_set_start(i2c_id);
}

void drv_i2c_open(DRV_I2C_ID_t i2c_id, u8_t sla_addr)
{
    g_i2c_info[i2c_id].sla_rx_func = 0;
    g_i2c_info[i2c_id].sla_tx_func = 0;
    g_i2c_info[i2c_id].intr_en_flag = 1;
    g_i2c_info[i2c_id].rx_data_index = 0;
    g_i2c_info[i2c_id].rx_data_len = 0;
    g_i2c_info[i2c_id].tx_data_index = 0;
    g_i2c_info[i2c_id].tx_data_len = 0;
    if (0 == sla_addr)
    {
        if (DRV_I2C_ID_1 == i2c_id)
        {
            g_i2c_info[i2c_id].sla_addr = SLAVE_ADDRESS_1;
        }
        else {
            g_i2c_info[i2c_id].sla_addr = SLAVE_ADDRESS_0;
        }
    }
    else {
        g_i2c_info[i2c_id].sla_addr = sla_addr;
    }

    // set slave addresses
    DRV_REG_SET(I2C_ADR_REG+g_i2c_addr_offset_map[i2c_id], g_i2c_info[i2c_id].sla_addr);

    drv_intr_ext_set(IPIC_CFG_INTR_IDX_I2C_INT+i2c_id, TRUE, IPIC_INTR_PRI_2+i2c_id, drv_i2c_intr_handler);
    drv_i2c_intf_init(i2c_id);

    if (i2c_id)
    {
        drv_gpio_en_set(SEC_I2C_SCL,FALSE);
        drv_gpio_pd_set(SEC_I2C_SCL,FALSE);
        drv_gpio_pu_set(SEC_I2C_SCL,FALSE);
        drv_gpio_od_set(SEC_I2C_SCL,DRV_GPIO_OD_TYPE_NMOS_1KOHM_PU);
        drv_gpio_en_set(SEC_I2C_SDA,FALSE);
        drv_gpio_pd_set(SEC_I2C_SDA,FALSE);
        drv_gpio_pu_set(SEC_I2C_SDA,FALSE);
        drv_gpio_od_set(SEC_I2C_SDA,DRV_GPIO_OD_TYPE_NMOS_1KOHM_PU);
    }
    else {
        drv_gpio_en_set(I2C_SCL,FALSE);
        drv_gpio_pd_set(I2C_SCL,FALSE);
        drv_gpio_pu_set(I2C_SCL,FALSE);
        drv_gpio_od_set(I2C_SCL,DRV_GPIO_OD_TYPE_NMOS_1KOHM_PU);
        drv_gpio_en_set(I2C_SDA,FALSE);
        drv_gpio_pd_set(I2C_SDA,FALSE);
        drv_gpio_pu_set(I2C_SDA,FALSE);
        drv_gpio_od_set(I2C_SDA,DRV_GPIO_OD_TYPE_NMOS_1KOHM_PU);
    }

    if (g_i2c_info[i2c_id].bus_lock)
        sync_mutex_delete(g_i2c_info[i2c_id].bus_lock);
    g_i2c_info[i2c_id].bus_lock = (void *)sync_mutex_create();
}

void drv_i2c_close(DRV_I2C_ID_t i2c_id)
{
    // disable I2C interrupt
    drv_i2c_interrupt_disable(i2c_id);

    DRV_REG_SET(I2C_CTRL_REG+g_i2c_addr_offset_map[i2c_id], 0x0);
    g_i2c_info[i2c_id].sla_addr = 0;
    sync_mutex_delete(g_i2c_info[i2c_id].bus_lock);
    g_i2c_info[i2c_id].bus_lock = (void *)0;
}

void drv_i2c_master_freq_set(DRV_I2C_ID_t i2c_id, DRV_I2C_FREQ_t freq)
{
    if ((I2C_FREQ_INVALID == freq) || (I2C_FREQ_VALID_END < freq))
        return;

    DRV_REG_SET(I2C_FREQ_REG+g_i2c_addr_offset_map[i2c_id], (u8_t)freq);
}

u8_t drv_i2c_master_transmit(DRV_I2C_ID_t i2c_id, u8_t addr, u8_t *ptx_data, u8_t tx_len, u8_t *prx_data, u8_t rx_len)
{
    sync_mutex_take(g_i2c_info[i2c_id].bus_lock, SYNC_MAX_DELAY);
    if (tx_len >= I2C_BUFFER_SIZE)
        tx_len = I2C_BUFFER_SIZE;
    if (rx_len >= I2C_BUFFER_SIZE)
        rx_len = I2C_BUFFER_SIZE;
    i2c_master_write_mode_set(i2c_id,addr);
    if (ptx_data != &g_i2c_info[i2c_id].tx_data[0])
    {
        memmove((char *)&(g_i2c_info[i2c_id].tx_data[0]), (char *)ptx_data, tx_len);
    }
    i2c_master_write_len_set(i2c_id,tx_len);
    i2c_master_read_len_set(i2c_id,rx_len);
    timer_start(T_WAKE_I2C_FAIL, &(g_i2c_info[i2c_id].timer));
    drv_i2c_master_start(i2c_id);

    while ((g_i2c_info[i2c_id].last_status == I2C_DATA_TRANSFER) && (!timer_expired(&(g_i2c_info[i2c_id].timer))));

    if (timer_expired(&(g_i2c_info[i2c_id].timer)))
    {
        g_i2c_info[i2c_id].last_status = I2C_TIME_OUT;
    }
    timer_stop(&(g_i2c_info[i2c_id].timer));

    if ((rx_len) && (prx_data != &g_i2c_info[i2c_id].rx_data[0]))
    {
        memmove((char *)prx_data, (char *)&(g_i2c_info[i2c_id].rx_data[0]), rx_len);
    }

    drv_i2c_intf_init(i2c_id);
    sync_mutex_give(g_i2c_info[i2c_id].bus_lock);
    return g_i2c_info[i2c_id].last_status;
}

#if I2C_MODULE_SLA_ENABLED

void drv_i2c_sla_addr_set(DRV_I2C_ID_t i2c_id, u8_t addr)
{
    g_i2c_info[i2c_id].sla_addr = addr;
    DRV_REG_SET(I2C_ADR_REG+g_i2c_addr_offset_map[i2c_id], addr);
}

void drv_i2c_sla_cleanup(DRV_I2C_ID_t i2c_id)
{
    g_i2c_info[i2c_id].rx_data_index = 0;
    g_i2c_info[i2c_id].rx_data_len = 0;
    g_i2c_info[i2c_id].tx_data_index = 0;
    g_i2c_info[i2c_id].tx_data_len = 0;
}


void drv_i2c_sla_rx_handler_set(DRV_I2C_ID_t i2c_id, void (*sla_rx_func)(DRV_I2C_ID_t id, u8_t* received_data, u8_t received_len))
{
    g_i2c_info[i2c_id].sla_rx_func = sla_rx_func;
}

void drv_i2c_sla_tx_handler_set(DRV_I2C_ID_t i2c_id, u8_t (*sla_tx_func)(DRV_I2C_ID_t id, u8_t tx_data_len_max, u8_t* transmit_data))
{
    g_i2c_info[i2c_id].sla_tx_func = sla_tx_func;
}
#endif /* I2C_MODULE_SLA_ENABLED */


#if I2C_MODULE_REG_ENABLED
/** Get register via I2C interface */
u8_t drv_i2c_reg_get(DRV_I2C_ID_t i2c_id, u8_t addr, u8_t reg)
{
    u8_t val = 0;
    drv_i2c_master_transmit(i2c_id, addr, &reg, 1, &val, 1);
    return val;
}

/** Set register via I2C interface */
void drv_i2c_reg_set(DRV_I2C_ID_t i2c_id, u8_t addr, u8_t reg, u8_t val)
{
    u8_t tx_bytes[2];
    tx_bytes[0] = reg;
    tx_bytes[1] = val;
    drv_i2c_master_transmit(i2c_id, addr, &tx_bytes[0], 2, NULL, 0);
}

/** Get multiple registers */
void drv_i2c_reg_mget(DRV_I2C_ID_t i2c_id, u8_t addr, u8_t reg, u8_t cnt, u8_t *p_val)
{
    drv_i2c_master_transmit(i2c_id, addr, &reg, 1, p_val, cnt);
}

/** Set multiple registers */
void drv_i2c_reg_mset(DRV_I2C_ID_t i2c_id, u8_t addr, u8_t reg, u8_t cnt, u8_t *p_val)
{
    g_i2c_info[i2c_id].tx_data[0] = reg;
    memmove((char *)&(g_i2c_info[i2c_id].tx_data[1]), (char *)p_val, cnt);
    drv_i2c_master_transmit(i2c_id, addr, &(g_i2c_info[i2c_id].tx_data[0]), cnt+1, NULL, 0);
}

/** Get register field with width via I2C interface */
u8_t drv_i2c_reg_field_get(DRV_I2C_ID_t i2c_id, u8_t addr, u8_t reg, u8_t field, u8_t width)
{
    u8_t val = drv_i2c_reg_get(i2c_id, addr, reg);
    return ((val&((~(0xFF<<(width)))<<(field)))>>(field));
}

/** Set register field with width */
void drv_i2c_reg_field_set(DRV_I2C_ID_t i2c_id, u8_t addr, u8_t reg, u8_t field, u8_t width, u8_t val)
{
    u8_t old_val = drv_i2c_reg_get(i2c_id, addr, reg);
    old_val = (old_val&(~((~(0xFF<<(width)))<<(field))))|((val<<(field))&((~(0xFF<<(width)))<<(field)));
    drv_i2c_reg_set(i2c_id, addr, reg, old_val);
}

/** Get register field with field set */
u8_t drv_i2c_reg_field_get2(DRV_I2C_ID_t i2c_id, u8_t addr, u8_t reg, u8_t field, u8_t field_set)
{
    u8_t val = drv_i2c_reg_get(i2c_id, addr, reg);
    return ((val & field_set) >> field);
}

/** Set register field with field set and mask */
void drv_i2c_reg_field_set2(DRV_I2C_ID_t i2c_id, u8_t addr, u8_t reg, u8_t field, u8_t field_set, u8_t field_mask, u8_t val)
{
    u8_t old_val = drv_i2c_reg_get(i2c_id, addr, reg);
    old_val = (old_val & field_mask) | ((val << field) & field_set);
    drv_i2c_reg_set(i2c_id, addr, reg, old_val);
}


#if I2C_MODULE_REG_16_ENABLED
/** Get 2 bytes register */
u16_t drv_i2c_reg_get_u16(DRV_I2C_ID_t i2c_id, u8_t addr, u8_t reg)
{
    u16_t val;
    drv_i2c_reg_mget(i2c_id, addr,reg,2,(u8_t *)&val);
    return val;
}

/** Set 2 bytes register */
void drv_i2c_reg_set_u16(DRV_I2C_ID_t i2c_id, u8_t addr, u8_t reg, u16_t val)
{
    drv_i2c_reg_mset(i2c_id, addr,reg,2,(u8_t *)&val);
}

/** Get register field with width */
u16_t drv_i2c_reg_field_get_u16(DRV_I2C_ID_t i2c_id, u8_t addr, u8_t reg, u8_t field, u8_t width)
{
    u16_t val = drv_i2c_reg_get_u16(i2c_id, addr, reg);
    return ((val&((~(0xFFFFU<<(width)))<<(field)))>>(field));
}

/** Set register field with width */
void drv_i2c_reg_field_set_u16(DRV_I2C_ID_t i2c_id, u8_t addr, u8_t reg, u8_t field, u8_t width, u16_t val)
{
    u16_t old_val = drv_i2c_reg_get_u16(i2c_id, addr, reg);
    old_val = (old_val&(~((~(0xFFFFU<<(width)))<<(field))))|((val<<(field))&((~(0xFFFFU<<(width)))<<(field)));
    drv_i2c_reg_set_u16(i2c_id, addr, reg, old_val);
}


/** Get register field with field set */
u16_t drv_i2c_reg_field_get2_u16(DRV_I2C_ID_t i2c_id, u8_t addr, u8_t reg, u8_t field, u16_t field_set)
{
    u16_t val = drv_i2c_reg_get_u16(i2c_id, addr, reg);
    return ((val & field_set) >> field);
}

/** Set register field with field set and mask */
void drv_i2c_reg_field_set2_u16(DRV_I2C_ID_t i2c_id, u8_t addr, u8_t reg, u8_t field, u16_t field_set, u16_t field_mask, u16_t val)
{
    u16_t old_val = drv_i2c_reg_get_u16(i2c_id, addr, reg);
    old_val = (old_val & field_mask) | ((val << field) & field_set);
    drv_i2c_reg_set_u16(i2c_id, addr, reg, old_val);
}
#endif /* I2C_MODULE_REG_16_ENABLED */

#endif /* I2C_MODULE_REG_ENABLED */

#endif

