/*
 * 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)


/*! \brief Network layer result code
 */
#define ISOTP_PROTOCOL_RESULT_OK           (0)
#define ISOTP_PROTOCOL_RESULT_TIMEOUT_AS   (-1)
#define ISOTP_PROTOCOL_RESULT_TIMEOUT_CS   (-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_SetNad(byte, nad)            (byte[0] = nad)
#define IsoTp_GetNad(byte)                 (byte[0])

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

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

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

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

/*******************************************************************************
 * 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
 ******************************************************************************/

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

    /* Setup message */
    IsoTp_SetNad(msg.byte, obj->physNad);
    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 + 2] = obj->sendBuffer[index];
    }

    /* All pad with 0 */
    for(i = 0; i < 6 - obj->sendSize; ++i)
    {
        msg.byte[i + obj->sendSize + 2] = 0;
    }

    /* Send message */
    ret = obj->sendLinMsg(msg.byte, 8);

    return ret;
}

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

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

    /* Send message */
    ret = obj->sendLinMsg(msg.byte, 8);
    if(ISOTP_RET_OK == ret)
    {
        obj->sendOffset += 5; /* 5 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_SetNad(msg.byte, obj->physNad);
    IsoTp_SetPciType(msg.byte, ISOTP_PCI_TYPE_CONSECUTIVE_FRAME);
    IsoTp_SetCFSn(msg.byte, obj->sendSN);
    dataLength = obj->sendSize - obj->sendOffset;
    if(dataLength > 6) /* Max 6 data bytes for the consecutive frame */
    {
        dataLength = 6;
    }
    for(index = 0; index < dataLength; ++index)
    {
        msg.byte[index + 2] = obj->sendBuffer[obj->sendOffset + index];
    }

    /* All pad with 0 */
    uint8_t i = 0;
    for(i = 0; i < 6 - dataLength; ++i)
    {
        msg.byte[i + 2 + dataLength] = 0;
    }

    /* Send message */
    ret = obj->sendLinMsg(msg.byte, 8);

    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;
    uint8_t nad = IsoTp_GetNad(pMsg->byte);

    /* Check NAD */
    if((nad != obj->physNad) && (nad != obj->funcNad) && (nad != obj->broadcastNad))
    {
        if(obj->debug != NULL)
        {
            obj->debug("Single-frame NAD is not match.");
        }
        return ISOTP_RET_WRONG_NAD;
    }

    /* Check data length */
    if((0 == IsoTp_GetSFDataLen(pMsg->byte))
       || (IsoTp_GetSFDataLen(pMsg->byte) > (len - 2)))
    {
        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 + 2];
    }
    /* 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;
    uint8_t nad = IsoTp_GetNad(pMsg->byte);

    /* Check NAD */
    if((nad != obj->physNad) && (nad != obj->funcNad) && (nad != obj->broadcastNad))
    {
        if(obj->debug != NULL)
        {
            obj->debug("Single-frame NAD is not match.");
        }
        return ISOTP_RET_WRONG_NAD;
    }

    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 <= 6)
    {
        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 < 5; ++index) /* 5 data bytes for the first frame */
    {
        obj->receiveBuffer[index] = pMsg->byte[index + 3];
    }
    obj->receiveOffset = 5; /* 5 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;
    uint8_t nad = IsoTp_GetNad(pMsg->byte);

    /* Check NAD */
    if(nad != obj->receiveArbitrationNad)
    {
        if(obj->debug != NULL)
        {
            obj->debug("Single-frame NAD is not match.");
        }
        return ISOTP_RET_WRONG_NAD;
    }

    /* 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 > 6) /* Max 6 data bytes for the consecutive frame */
    {
        remaining_bytes = 6;
    }
    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 + 2];
    }

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

    return ISOTP_RET_OK;
}

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

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

    if((obj->receiveArbitrationNad == obj->funcNad) || (obj->receiveArbitrationNad == obj->broadcastNad))
    {
        return ISOTP_RET_OK;
    }

    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 < 7)
    {
        /* 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)
        {
            /* Refresh AS timer */
            obj->sendTimerAs   = obj->getTimeMs() + ISOTP_DEFAULT_CONFIRM_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_HandleIncomingLinMsg(IsoTpType *obj, const uint8_t *data, uint8_t len)
{
    int8_t          ret;
    uint8_t         i = 0;
    IsoTp_MsgType   msg;

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

    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->receiveArbitrationNad = IsoTp_GetNad(msg.byte);
                /* 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;
                break;
            }

            /* If receive successful */
            if(ISOTP_RET_OK == ret)
            {
                obj->receiveArbitrationNad = IsoTp_GetNad(msg.byte);
                /* Change status */
                obj->receiveStatus = ISOTP_RECEIVE_STATUS_INPROGRESS;
                /* 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(IsoTp_GetNad(msg.byte) != obj->receiveArbitrationNad)
            {
                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;
                }
            }

            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->receiveArbitrationNad == link->funcNad) ? 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->physNad               = pParams->physNad;
    obj->funcNad               = pParams->funcNad;
    obj->broadcastNad          = pParams->broadcastNad;
    obj->sendBuffer            = pParams->sendBuf;
    obj->sendBufferSize        = pParams->sendBufSize;
    obj->sendSize              = 0;
    obj->sendOffset            = 0;
    obj->sendSN                = 0;
    obj->sendTimerAs           = 0;
    obj->sendTimerSeptime      = 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->receiveTimerCr        = 0;
    obj->receiveProtocolResult = ISOTP_PROTOCOL_RESULT_OK;
    obj->receiveStatus         = ISOTP_RECEIVE_STATUS_IDLE;

    obj->debug                 = pParams->debug;
    obj->sendLinMsg            = pParams->sendLinMsg;
    obj->isLinSendComplete  = pParams->isLinSendComplete;
    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(obj->isLinSendComplete())
        {
            ret = IsoTp_SendConsecutiveFrame(obj);
            if(ISOTP_RET_OK == ret)
            {
                /* 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->sendTimerAs))
        {
            obj->sendProtocolResult = ISOTP_PROTOCOL_RESULT_TIMEOUT_AS;
            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;
}