/*
 * Copyright (c) 2022, Shenzhen CVA Innovation CO.,LTD
 * All rights reserved.
 *
 * Shenzhen CVA Innovation CO.,LTD (CVA chip) is supplying this file for use
 * exclusively with CVA's microcontroller products. This file can be freely
 * distributed within development tools that are supporting such microcontroller
 * products.
 *
 * THIS SOFTWARE IS PROVIDED "AS IS".  NO WARRANTIES, WHETHER EXPRESS, IMPLIED
 * OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE.
 * CVA SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL,
 * OR CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
 */

/*******************************************************************************
 * the includes
 ******************************************************************************/

#include <stdint.h>
#include <stddef.h>
#include "isotp.h"

/*******************************************************************************
 * the defines
 ******************************************************************************/

/*! \brief Protocol Control Information (PCI) types, for identifying each frame of an ISO-TP message.
 */
#define ISOTP_PCI_TYPE_SINGLE_FRAME        (0x0)
#define ISOTP_PCI_TYPE_FIRST_FRAME         (0x1)
#define ISOTP_PCI_TYPE_CONSECUTIVE_FRAME   (0x2)
#define ISOTP_PCI_TYPE_FLOW_CONTROL_FRAME  (0x3)

/*! \brief Protocol Control Information (PCI) flow control identifiers.
 */
#define ISOTP_PCI_FLOW_STATUS_CONTINUE     (0x0)
#define ISOTP_PCI_FLOW_STATUS_WAIT         (0x1)
#define ISOTP_PCI_FLOW_STATUS_OVERFLOW     (0x2)

/* invalid bs */
#define ISOTP_INVALID_BS                   0xFFFF

/*! \brief Network layer result code
 */
#define ISOTP_PROTOCOL_RESULT_OK           (0)
#define ISOTP_PROTOCOL_RESULT_TIMEOUT_A    (-1)
#define ISOTP_PROTOCOL_RESULT_TIMEOUT_BS   (-2)
#define ISOTP_PROTOCOL_RESULT_TIMEOUT_CR   (-3)
#define ISOTP_PROTOCOL_RESULT_WRONG_SN     (-4)
#define ISOTP_PROTOCOL_RESULT_INVALID_FS   (-5)
#define ISOTP_PROTOCOL_RESULT_UNEXP_PDU    (-6)
#define ISOTP_PROTOCOL_RESULT_WFT_OVRN     (-7)
#define ISOTP_PROTOCOL_RESULT_BUFFER_OVFLW (-8)
#define ISOTP_PROTOCOL_RESULT_ERROR        (-9)

/*! \brief Return logic true if 'a' is after 'b'
 */
#define IsoTp_TimeAfter(a, b)              ((int64_t)((int64_t)(b) - (int64_t)(a)) < 0)

#define IsoTp_SetPciType(byte, type)       (byte[0] = (0xFF & ((((uint8_t)type) << 4) | 0x0f)))
#define IsoTp_GetPciType(byte)             ((uint8_t)(byte[0] >> 4))

/*! \brief single frame
 *         +-------------------------+-----+
 *         | byte #0                 | ... |
 *         +-------------------------+-----+
 *         | nibble #0   | nibble #1 | ... |
 *         +-------------+-----------+ ... +
 *         | PCIType = 0 | SF_DL     | ... |
 *         +-------------+-----------+-----+
 */
#define IsoTp_SetSFDataLen(byte, len)      (byte[0] &= (((uint8_t)len) | 0xf0))
#define IsoTp_GetSFDataLen(byte)           (byte[0] & 0x0f)

/*! \brief first frame
 *         +-------------------------+-----------------------+-----+
 *         | byte #0                 | byte #1               | ... |
 *         +-------------------------+-----------+-----------+-----+
 *         | nibble #0   | nibble #1 | nibble #2 | nibble #3 | ... |
 *         +-------------+-----------+-----------+-----------+-----+
 *         | PCIType = 1 | FF_DL                             | ... |
 *         +-------------+-----------+-----------------------+-----+
 */
#define IsoTp_SetFFDataLen(byte, len)   \
    do                                  \
    {                                   \
        byte[0] &= ((len >> 8) | 0xf0); \
        byte[1] = (len & 0xff);         \
    } while(0)
#define IsoTp_GetFFDataLen(byte)       (((((uint16_t)byte[0]) & 0x0f) << 8) + byte[1])

/*! \brief consecutive frame
 *         +-------------------------+-----+
 *         | byte #0                 | ... |
 *         +-------------------------+-----+
 *         | nibble #0   | nibble #1 | ... |
 *         +-------------+-----------+ ... +
 *         | PCIType = 2 | SN        | ... |
 *         +-------------+-----------+-----+
 */
#define IsoTp_SetCFSn(byte, sn)        (byte[0] &= (((uint8_t)sn) | 0xf0))
#define IsoTp_GetCFSn(byte)            (byte[0] & 0x0f)

/*! \brief flow control frame
 *         +-------------------------+-----------------------+-----------------------+-----+
 *         | byte #0                 | byte #1               | byte #2               | ... |
 *         +-------------------------+-----------+-----------+-----------+-----------+-----+
 *         | nibble #0   | nibble #1 | nibble #2 | nibble #3 | nibble #4 | nibble #5 | ... |
 *         +-------------+-----------+-----------+-----------+-----------+-----------+-----+
 *         | PCIType = 1 | FS        | BS                    | STmin                 | ... |
 *         +-------------+-----------+-----------------------+-----------------------+-----+
 */
#define IsoTp_SetFCFlowState(byte, fs) (byte[0] &= (((uint8_t)fs) | 0xf0))
#define IsoTp_GetFCFlowState(byte)     (byte[0] & 0x0f)
#define IsoTp_SetFCBlockSize(byte, bs) (byte[1] = bs)
#define IsoTp_GetFCBlockSize(byte)     (byte[1])
#define IsoTp_SetFCStmin(byte, STmin)  (byte[2] = STmin)
#define IsoTp_GetFCStmin(byte)         (byte[2])

/*******************************************************************************
 * the typedefs
 ******************************************************************************/

/*! \brief ISOTP sender status
 */
typedef enum
{
    ISOTP_SEND_STATUS_IDLE,
    ISOTP_SEND_STATUS_INPROGRESS,
    ISOTP_SEND_STATUS_ERROR,
} IsoTp_SendStatusType;

/*! \brief ISOTP receiver status
 */
typedef enum
{
    ISOTP_RECEIVE_STATUS_IDLE,
    ISOTP_RECEIVE_STATUS_INPROGRESS,
    ISOTP_RECEIVE_STATUS_FULL,
} IsoTp_ReceiveStatusType;

/*! \brief ISOTP message
 */
typedef struct _IsoTp_MsgType_
{
    uint8_t byte[8];
} IsoTp_MsgType;

/*******************************************************************************
 * the globals
 ******************************************************************************/

/*******************************************************************************
 * the functions
 ******************************************************************************/

/* st_min to microsecond */
static uint8_t IsoTp_MsToStMin(uint8_t ms)
{
    uint8_t stMin;

    stMin = ms;
    if(stMin > 0x7F)
    {
        stMin = 0x7F;
    }

    return stMin;
}

/* st_min to msec  */
static uint8_t IsoTp_StMinToMs(uint8_t stMin)
{
    uint8_t ms;

    if(stMin >= 0xF1 && stMin <= 0xF9)
    {
        ms = 1; /* Actually it should be 0.1 - 0.9 ms */
    }
    else if(stMin <= 0x7F)
    {
        ms = stMin;
    }
    else
    {
        ms = 0;
    }

    return ms;
}

static int8_t IsoTp_SendFlowControl(IsoTpType *obj, uint8_t flow_status, uint8_t block_size, uint8_t st_min_ms)
{
    int8_t        ret;
    uint8_t       index = 0;
    IsoTp_MsgType msg;

    /* Setup message */
    IsoTp_SetPciType(msg.byte, ISOTP_PCI_TYPE_FLOW_CONTROL_FRAME);
    IsoTp_SetFCFlowState(msg.byte, flow_status);
    IsoTp_SetFCBlockSize(msg.byte, block_size);
    IsoTp_SetFCStmin(msg.byte, IsoTp_MsToStMin(st_min_ms));

    /* Send message */
    if(obj->framePadding)
    {
        /* All pad with 0 */
        for(index = 3; index < 8; ++index)
        {
            msg.byte[index] = 0;
        }
        ret = obj->sendCanMsg(obj->sendArbitrationId, msg.byte, 8);
    }
    else
    {
        ret = obj->sendCanMsg(obj->sendArbitrationId, msg.byte, 3);
    }

    return ret;
}

static int8_t IsoTp_SendSingleFrame(IsoTpType *obj)
{
    int8_t        ret;
    uint8_t       i     = 0;
    uint8_t       index = 0;
    IsoTp_MsgType msg;

    /* Setup message */
    IsoTp_SetPciType(msg.byte, ISOTP_PCI_TYPE_SINGLE_FRAME);
    IsoTp_SetSFDataLen(msg.byte, (uint8_t)obj->sendSize);
    for(index = 0; index < obj->sendSize; ++index)
    {
        msg.byte[index + 1] = obj->sendBuffer[index];
    }

    /* Send message */
    if(obj->framePadding)
    {
        /* All pad with 0 */
        for(i = 0; i < 7 - obj->sendSize; ++i)
        {
            msg.byte[i + obj->sendSize + 1] = 0;
        }
        ret = obj->sendCanMsg(obj->sendArbitrationId, msg.byte, 8);
    }
    else
    {
        ret = obj->sendCanMsg(obj->sendArbitrationId, msg.byte, obj->sendSize + 1);
    }

    return ret;
}

static int8_t IsoTp_SendFirstFrame(IsoTpType *obj)
{
    int8_t        ret;
    IsoTp_MsgType msg;

    /* Setup message */
    IsoTp_SetPciType(msg.byte, ISOTP_PCI_TYPE_FIRST_FRAME);
    IsoTp_SetFFDataLen(msg.byte, obj->sendSize);
    uint8_t index = 0;
    for(index = 0; index < 6; ++index) /* 6 data bytes for the first frame */
    {
        msg.byte[index + 2] = obj->sendBuffer[index];
    }

    /* Send message */
    ret = obj->sendCanMsg(obj->sendArbitrationId, msg.byte, 8);
    if(ISOTP_RET_OK == ret)
    {
        obj->sendOffset += 6; /* 6 data bytes for the first frame */
        obj->sendSN = 1;
    }

    return ret;
}

static int8_t IsoTp_SendConsecutiveFrame(IsoTpType *obj)
{
    int8_t        ret;
    IsoTp_MsgType msg;
    uint16_t      dataLength;
    uint8_t       index = 0;

    /* setup message  */
    IsoTp_SetPciType(msg.byte, ISOTP_PCI_TYPE_CONSECUTIVE_FRAME);
    IsoTp_SetCFSn(msg.byte, obj->sendSN);
    dataLength = obj->sendSize - obj->sendOffset;
    if(dataLength > 7) /* Max 7 data bytes for the consecutive frame */
    {
        dataLength = 7;
    }
    for(index = 0; index < dataLength; ++index)
    {
        msg.byte[index + 1] = obj->sendBuffer[obj->sendOffset + index];
    }

    /* Send message */
    if(obj->framePadding)
    {
        /* All pad with 0 */
        uint8_t i = 0;
        for(i = 0; i < 7 - dataLength; ++i)
        {
            msg.byte[i + 1 + dataLength] = 0;
        }
        ret = obj->sendCanMsg(obj->sendArbitrationId, msg.byte, 8);
    }
    else
    {
        ret = obj->sendCanMsg(obj->sendArbitrationId, msg.byte, dataLength + 1);
    }

    if(ISOTP_RET_OK == ret)
    {
        obj->sendOffset += dataLength;
        if(++(obj->sendSN) > 0x0F)
        {
            obj->sendSN = 0;
        }
    }

    return ret;
}

static int8_t IsoTp_ReceiveSingleFrame(IsoTpType *obj, const IsoTp_MsgType *pMsg, uint8_t len)
{
    uint8_t index = 0;

    /* Check data length */
    if((0 == IsoTp_GetSFDataLen(pMsg->byte))
       || (IsoTp_GetSFDataLen(pMsg->byte) > (len - 1)))
    {
        if(obj->debug != NULL)
        {
            obj->debug("Single-frame length too small or too large.");
        }
        return ISOTP_RET_LENGTH;
    }

    /* Copying data */
    /* polyspace-begin DEFECT:OUT_BOUND_ARRAY [No action planned:High] "Still keep default because one frame max length is 8" */
    obj->receiveSize = IsoTp_GetSFDataLen(pMsg->byte);
    for(index = 0; index < obj->receiveSize; ++index)
    {
        obj->receiveBuffer[index] = pMsg->byte[index + 1];
    }
    /* polyspace-end DEFECT:OUT_BOUND_ARRAY [No action planned:High] "Still keep default because one frame max length is 8" */

    return ISOTP_RET_OK;
}

static int8_t IsoTp_ReceiveFirstFrame(IsoTpType *obj, const IsoTp_MsgType *pMsg, uint8_t len)
{
    uint16_t payloadLength;
    uint8_t  index = 0;

    if(8 != len)
    {
        if(obj->debug != NULL)
        {
            obj->debug("First frame should be 8 bytes in length.");
        }
        return ISOTP_RET_LENGTH;
    }

    /* Check data length */
    payloadLength = IsoTp_GetFFDataLen(pMsg->byte);

    /* Should not use multiple frame transmission */
    if(payloadLength <= 7)
    {
        if(obj->debug != NULL)
        {
            obj->debug("First frame should not use multiple frame transmission.");
        }
        return ISOTP_RET_LENGTH;
    }

    if(payloadLength > obj->receiveBufferSize)
    {
        if(obj->debug != NULL)
        {
            obj->debug("Multi-frame response too large for receiving buffer.");
        }
        return ISOTP_RET_OVERFLOW;
    }

    /* Copying data */
    obj->receiveSize = payloadLength;
    for(index = 0; index < 6; ++index) /* 6 data bytes for the first frame */
    {
        obj->receiveBuffer[index] = pMsg->byte[index + 2];
    }
    obj->receiveOffset = 6; /* 6 data bytes for the first frame */
    obj->receiveSN     = 1;

    return ISOTP_RET_OK;
}

static int8_t IsoTp_ReceiveConsecutiveFrame(IsoTpType *obj, const IsoTp_MsgType *pMsg, uint8_t len)
{
    uint16_t remaining_bytes;
    uint8_t  index = 0;

    /* Check SN */
    if(obj->receiveSN != IsoTp_GetCFSn(pMsg->byte))
    {
        return ISOTP_RET_WRONG_SN;
    }

    /* Check data length */
    remaining_bytes = obj->receiveSize - obj->receiveOffset;
    if(remaining_bytes > 7) /* Max 7 data bytes for the consecutive frame */
    {
        remaining_bytes = 7;
    }
    if(remaining_bytes > len - 1)
    {
        if(obj->debug != NULL)
        {
            obj->debug("Consecutive frame too short.");
        }
        return ISOTP_RET_LENGTH;
    }

    /* Copying data */
    for(index = 0; index < remaining_bytes; ++index)
    {
        obj->receiveBuffer[index + obj->receiveOffset] = pMsg->byte[index + 1];
    }

    obj->receiveOffset += remaining_bytes;
    if(++(obj->receiveSN) > 0x0F)
    {
        obj->receiveSN = 0;
    }

    return ISOTP_RET_OK;
}

static int8_t IsoTp_ReceiveFlowControlFrame(IsoTpType *obj, const IsoTp_MsgType *pMsg, uint8_t len)
{
    /* Check message length */
    if(len < 3)
    {
        if(obj->debug != NULL)
        {
            obj->debug("Flow control frame too short.");
        }
        return ISOTP_RET_LENGTH;
    }

    return ISOTP_RET_OK;
}

int8_t IsoTp_Send(IsoTpType *obj, const uint8_t payload[], uint16_t size)
{
    return IsoTp_SendWithId(obj, obj->sendArbitrationId, payload, size);
}

int8_t IsoTp_SendWithId(IsoTpType *obj, uint32_t id, const uint8_t payload[], uint16_t size)
{
    int8_t  ret;
    uint8_t i = 0;

    if(obj == NULL)
    {
        return ISOTP_RET_ERROR;
    }

    if(size > obj->sendBufferSize)
    {
        if(obj->debug != NULL)
        {
            obj->debug("Message size too large. Set a larger send buffer\n");
        }
        return ISOTP_RET_OVERFLOW;
    }

    if(ISOTP_SEND_STATUS_INPROGRESS == obj->sendStatus)
    {
        if(obj->debug != NULL)
        {
            obj->debug("Abort previous message, transmission in progress.\n");
        }
        return ISOTP_RET_INPROGRESS;
    }

    /* Copy into local buffer */
    obj->sendSize   = size;
    obj->sendOffset = 0;
    for(i = 0; i < size; ++i)
    {
        obj->sendBuffer[i] = payload[i];
    }

    if(obj->sendSize < 8)
    {
        /* Send single frame */
        ret = IsoTp_SendSingleFrame(obj);
    }
    else
    {
        /* Send multiple frames */
        ret = IsoTp_SendFirstFrame(obj);

        /* Initialize multi-frame control flags */
        if(ISOTP_RET_OK == ret)
        {
            obj->sendBsRemain       = 0;
            obj->sendSTMin          = 0;
            obj->sendWaitFrameCount = 0;
            obj->sendTimerSeptime   = obj->getTimeMs();
            /* Refresh BS timer */
            obj->sendTimerBlockSize = obj->getTimeMs() + ISOTP_DEFAULT_RESPONSE_TIMEOUT;
            obj->sendProtocolResult = ISOTP_PROTOCOL_RESULT_OK;
            obj->sendStatus         = ISOTP_SEND_STATUS_INPROGRESS;
        }
    }
    /* polyspace-end DEFECT:NULL_PTR [No action planned:High] "Still keep default because the null pointer is handled" */

    return ret;
}

void IsoTp_HandleIncomingCanMsg(IsoTpType *obj, uint32_t id, const uint8_t *data, uint8_t len)
{
    int8_t        ret;
    IsoTp_MsgType msg;

    if(len < 2 || len > 8)
    {
        return;
    }

    uint8_t i = 0;
    for(i = 0; i < len; ++i)
    {
        msg.byte[i] = data[i];
    }
    for(i = 0; i < 8 - len; ++i) /* len will NOT >  8 */
    {
        msg.byte[i + len] = 0; /* Padding */
    }

    switch(IsoTp_GetPciType(msg.byte))
    {
    case ISOTP_PCI_TYPE_SINGLE_FRAME:
        {
            /* Update protocol result */
            if(ISOTP_RECEIVE_STATUS_INPROGRESS == obj->receiveStatus)
            {
                obj->receiveProtocolResult = ISOTP_PROTOCOL_RESULT_UNEXP_PDU;
            }
            else
            {
                obj->receiveProtocolResult = ISOTP_PROTOCOL_RESULT_OK;
            }

            /* Handle message */
            ret = IsoTp_ReceiveSingleFrame(obj, &msg, len);

            if(ISOTP_RET_OK == ret)
            {
                obj->receiveArbitrationId = id;
                /* Change status */
                obj->receiveStatus = ISOTP_RECEIVE_STATUS_FULL;
            }
            break;
        }
    case ISOTP_PCI_TYPE_FIRST_FRAME:
        {
            /* Update protocol result */
            if(ISOTP_RECEIVE_STATUS_INPROGRESS == obj->receiveStatus)
            {
                obj->receiveProtocolResult = ISOTP_PROTOCOL_RESULT_UNEXP_PDU;
            }
            else
            {
                obj->receiveProtocolResult = ISOTP_PROTOCOL_RESULT_OK;
            }

            /* Handle message */
            ret = IsoTp_ReceiveFirstFrame(obj, &msg, len);

            /* If overflow happened */
            if(ISOTP_RET_OVERFLOW == ret)
            {
                /* Update protocol result */
                obj->receiveProtocolResult = ISOTP_PROTOCOL_RESULT_BUFFER_OVFLW;
                /* Change status */
                obj->receiveStatus = ISOTP_RECEIVE_STATUS_IDLE;
                /* Send error message */
                IsoTp_SendFlowControl(obj, ISOTP_PCI_FLOW_STATUS_OVERFLOW, 0, 0);
                break;
            }

            /* If receive successful */
            if(ISOTP_RET_OK == ret)
            {
                obj->receiveArbitrationId = id;
                /* Change status */
                obj->receiveStatus = ISOTP_RECEIVE_STATUS_INPROGRESS;
                /* Send FC frame */
                obj->receiveBlockSizeCount = obj->blockSize;
                IsoTp_SendFlowControl(obj, ISOTP_PCI_FLOW_STATUS_CONTINUE, obj->receiveBlockSizeCount, ISOTP_DEFAULT_ST_MIN);
                /* Refresh timer CR */
                obj->receiveTimerCr = obj->getTimeMs() + ISOTP_DEFAULT_RESPONSE_TIMEOUT;
            }

            break;
        }
    case ISOTP_PCI_TYPE_CONSECUTIVE_FRAME:
        {
            /* Check if in receiving status */
            if(ISOTP_RECEIVE_STATUS_INPROGRESS != obj->receiveStatus)
            {
                obj->receiveProtocolResult = ISOTP_PROTOCOL_RESULT_UNEXP_PDU;
                break;
            }

            if(id != obj->receiveArbitrationId)
            {
                break;
            }

            /* Handle message */
            ret = IsoTp_ReceiveConsecutiveFrame(obj, &msg, len);

            /* If wrong SN */
            if(ISOTP_RET_WRONG_SN == ret)
            {
                obj->receiveProtocolResult = ISOTP_PROTOCOL_RESULT_WRONG_SN;
                obj->receiveStatus         = ISOTP_RECEIVE_STATUS_IDLE;
                break;
            }

            /* If success */
            if(ISOTP_RET_OK == ret)
            {
                /* Refresh timer CR */
                obj->receiveTimerCr = obj->getTimeMs() + ISOTP_DEFAULT_RESPONSE_TIMEOUT;

                /* Receive finished */
                if(obj->receiveOffset >= obj->receiveSize)
                {
                    obj->receiveStatus = ISOTP_RECEIVE_STATUS_FULL;
                }
                else
                {
                    /* Send FC when BS reaches limit */
                    if(0 == --obj->receiveBlockSizeCount)
                    {
                        obj->receiveBlockSizeCount = obj->blockSize;
                        IsoTp_SendFlowControl(obj, ISOTP_PCI_FLOW_STATUS_CONTINUE, obj->receiveBlockSizeCount, ISOTP_DEFAULT_ST_MIN);
                    }
                }
            }

            break;
        }
    case ISOTP_PCI_TYPE_FLOW_CONTROL_FRAME:
        {
            /* Handle fc frame only when sending in progress */
            if(ISOTP_SEND_STATUS_INPROGRESS != obj->sendStatus)
            {
                break;
            }

            /* Handle message */
            ret = IsoTp_ReceiveFlowControlFrame(obj, &msg, len);

            if(ISOTP_RET_OK == ret)
            {
                /* Refresh BS timer */
                obj->sendTimerBlockSize = obj->getTimeMs() + ISOTP_DEFAULT_RESPONSE_TIMEOUT;

                /* Overflow */
                if(ISOTP_PCI_FLOW_STATUS_OVERFLOW == IsoTp_GetFCFlowState(msg.byte))
                {
                    obj->sendProtocolResult = ISOTP_PROTOCOL_RESULT_BUFFER_OVFLW;
                    obj->sendStatus         = ISOTP_SEND_STATUS_ERROR;
                }

                /* Wait */
                else if(ISOTP_PCI_FLOW_STATUS_WAIT == IsoTp_GetFCFlowState(msg.byte))
                {
                    obj->sendWaitFrameCount += 1;
                    /* Wait exceed allowed count */
                    if(obj->sendWaitFrameCount > ISOTP_MAX_WFT_NUMBER)
                    {
                        obj->sendProtocolResult = ISOTP_PROTOCOL_RESULT_WFT_OVRN;
                        obj->sendStatus         = ISOTP_SEND_STATUS_ERROR;
                    }
                }

                /* Permit send */
                else if(ISOTP_PCI_FLOW_STATUS_CONTINUE == IsoTp_GetFCFlowState(msg.byte))
                {
                    if(0 == IsoTp_GetFCBlockSize(msg.byte))
                    {
                        obj->sendBsRemain = ISOTP_INVALID_BS;
                    }
                    else
                    {
                        obj->sendBsRemain = IsoTp_GetFCBlockSize(msg.byte);
                    }
                    obj->sendSTMin          = IsoTp_StMinToMs(IsoTp_GetFCStmin(msg.byte));
                    obj->sendWaitFrameCount = 0;
                }
            }
            break;
        }
    default:
        break;
    };

    return;
}

int8_t IsoTp_Receive(IsoTpType *link, bool *IsFuncAddr, uint8_t *payload, uint16_t payload_size, uint16_t *out_size)
{
    uint16_t copylen;
    uint16_t i = 0;

    if(ISOTP_RECEIVE_STATUS_FULL != link->receiveStatus)
    {
        return ISOTP_RET_NO_DATA;
    }

    *IsFuncAddr = (link->receiveArbitrationId == link->funcId) ? true : false;

    copylen = link->receiveSize;
    if(copylen > payload_size)
    {
        copylen = payload_size;
    }

    for(i = 0; i < copylen; ++i)
    {
        payload[i] = link->receiveBuffer[i];
    }
    *out_size = copylen;

    link->receiveStatus = ISOTP_RECEIVE_STATUS_IDLE;

    return ISOTP_RET_OK;
}

void IsoTp_Init(IsoTpType *obj, const IsoTp_Params *pParams)
{
    obj->framePadding          = pParams->framePadding;
    obj->blockSize             = pParams->blockSize;
    obj->physId                = pParams->recvPhysId;
    obj->funcId                = pParams->recvFuncId;
    obj->sendArbitrationId     = pParams->sendid;
    obj->sendBuffer            = pParams->sendBuf;
    obj->sendBufferSize        = pParams->sendBufSize;
    obj->sendSize              = 0;
    obj->sendOffset            = 0;
    obj->sendSN                = 0;
    obj->sendBsRemain          = 0;
    obj->sendSTMin             = 0;
    obj->sendWaitFrameCount    = 0;
    obj->sendTimerSeptime      = 0;
    obj->sendTimerBlockSize    = 0;
    obj->sendProtocolResult    = 0;
    obj->sendStatus            = ISOTP_SEND_STATUS_IDLE;
    obj->receiveBuffer         = pParams->recvBuf;
    obj->receiveBufferSize     = pParams->recvBufSize;
    obj->receiveSize           = 0;
    obj->receiveOffset         = 0;
    obj->receiveSN             = 0;
    obj->receiveBlockSizeCount = 0;
    obj->receiveTimerCr        = 0;
    obj->receiveProtocolResult = ISOTP_PROTOCOL_RESULT_OK;
    obj->receiveStatus         = ISOTP_RECEIVE_STATUS_IDLE;

    obj->debug      = pParams->debug;
    obj->sendCanMsg = pParams->sendCanMsg;
    obj->getTimeMs  = pParams->getTimeMs;
}

void IsoTp_Poll(IsoTpType *obj)
{
    int8_t ret;

    /* Only polling when operation in progress */
    if(ISOTP_SEND_STATUS_INPROGRESS == obj->sendStatus)
    {
        /* Continue send data */
        if((ISOTP_INVALID_BS == obj->sendBsRemain || obj->sendBsRemain > 0) /* send data if bs_remain is invalid or bs_remain large than zero */
           && (0 == obj->sendSTMin || (0 != obj->sendSTMin && IsoTp_TimeAfter(obj->getTimeMs(), obj->sendTimerSeptime))))
        { /* st_min is zero or go beyond interval time */
            ret = IsoTp_SendConsecutiveFrame(obj);
            if(ISOTP_RET_OK == ret)
            {
                if(ISOTP_INVALID_BS != obj->sendBsRemain)
                {
                    obj->sendBsRemain -= 1;
                }
                /* Refresh BS timer */
                obj->sendTimerBlockSize = obj->getTimeMs() + ISOTP_DEFAULT_RESPONSE_TIMEOUT;
                obj->sendTimerSeptime   = obj->getTimeMs() + obj->sendSTMin;

                /* Check if send finish */
                if(obj->sendOffset >= obj->sendSize)
                {
                    obj->sendStatus = ISOTP_SEND_STATUS_IDLE;
                }
            }
            else
            {
                obj->sendStatus = ISOTP_SEND_STATUS_ERROR;
            }
        }

        /* Check timeout */
        if(IsoTp_TimeAfter(obj->getTimeMs(), obj->sendTimerBlockSize))
        {
            obj->sendProtocolResult = ISOTP_PROTOCOL_RESULT_TIMEOUT_BS;
            obj->sendStatus         = ISOTP_SEND_STATUS_ERROR;
        }
    }

    /* Only polling when operation in progress */
    if(ISOTP_RECEIVE_STATUS_INPROGRESS == obj->receiveStatus)
    {
        /* check timeout */
        if(IsoTp_TimeAfter(obj->getTimeMs(), obj->receiveTimerCr))
        {
            obj->receiveProtocolResult = ISOTP_PROTOCOL_RESULT_TIMEOUT_CR;
            obj->receiveStatus         = ISOTP_RECEIVE_STATUS_IDLE;
        }
    }

    return;
}