/*
 * 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 "uds_service31.h"
#include "bootloader.h"

/*******************************************************************************
 * the defines
 ******************************************************************************/
#define RID_ERASE_MEMORY          0xFD01
#define RID_CHECK_FLSDRV_VALID    0xFD02
#define RID_CHECK_DATA_VALIDITY   0xFD03
#define RID_CHECK_DATA_DEPENDENCE 0xFD04

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

/*! \brief The Uds routine control type
 */
typedef enum _UdsRoutineCtrlType_
{
    UDS_ROUTINE_CTRL_NONE           = 0,
    UDS_ROUTINE_CTRL_START          = 0x01,
    UDS_ROUTINE_CTRL_STOP           = 0x02,
    UDS_ROUTINE_CTRL_REQUEST_RESULT = 0x03
} UdsRoutineCtrlType;

/*! \brief The Uds routine control status
 */
typedef enum _UdsRtnCtrlStatus_
{
    UDS_RTN_ST_IDLE    = 0x00,
    UDS_RTN_ST_RUNNING = 0x01,
} UdsRtnCtrlStatus;

/*! \brief The Uds routine control struct
 */
typedef struct _Uds_RtnCtrl_
{
    uint16_t         rtnCtrlId;
    uint8_t          sessionLevel;
    uint8_t          securityLevel;
    UdsRtnCtrlStatus rtnStatus;
    void (*Routine_Start)(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen);
    void (*Routine_Stop)(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen);
    void (*Routine_Result)(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen);
} Uds_RtnCtrl;

/*******************************************************************************
 * the constants
 ******************************************************************************/

static void Routine_StartEreaseMemory(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen);
static void Routine_StopEreaseMemory(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen);
static void Routine_ResultEreaseMemory(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen);

static void Routine_StartValidityData(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen);
static void Routine_StopValidityData(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen);
static void Routine_ResultValidityData(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen);

static void Routine_StartCheckFlsDriverValid(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen);
static void Routine_StopCheckFlsDriverValid(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen);
static void Routine_ResultCheckFlsDriverValid(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen);

static void Routine_StartVerifyDataDependency(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen);
static void Routine_StopVerifyDataDependency(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen);
static void Routine_ResultVerifyDataDependency(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen);

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

Uds_RtnCtrl udsRtnCtrlTable[] = {
    {RID_ERASE_MEMORY,          UDS_SESSION_PROGRAMMING, UDS_SA_LEVEL_09, UDS_RTN_ST_IDLE, &Routine_StartEreaseMemory,         &Routine_StopEreaseMemory,         &Routine_ResultEreaseMemory        },
    {RID_CHECK_FLSDRV_VALID,    UDS_SESSION_PROGRAMMING, UDS_SA_LEVEL_09, UDS_RTN_ST_IDLE, &Routine_StartCheckFlsDriverValid,  &Routine_StopCheckFlsDriverValid,  &Routine_ResultCheckFlsDriverValid },
    {RID_CHECK_DATA_VALIDITY,   UDS_SESSION_PROGRAMMING, UDS_SA_LEVEL_09, UDS_RTN_ST_IDLE, &Routine_StartValidityData,         &Routine_StopValidityData,         &Routine_ResultValidityData        },
    {RID_CHECK_DATA_DEPENDENCE, UDS_SESSION_PROGRAMMING, UDS_SA_LEVEL_09, UDS_RTN_ST_IDLE, &Routine_StartVerifyDataDependency, &Routine_StopVerifyDataDependency, &Routine_ResultVerifyDataDependency},
};

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

void Routine_StatusInit(uint16_t routineId)
{
    uint8_t rtnIndex = 0;
    uint8_t rntNum   = sizeof(udsRtnCtrlTable) / sizeof(Uds_RtnCtrl);

    for(rtnIndex = 0; rtnIndex < rntNum; rtnIndex++)
    {
        if(udsRtnCtrlTable[rtnIndex].rtnCtrlId == routineId)
        {
            udsRtnCtrlTable[rtnIndex].rtnStatus = UDS_RTN_ST_IDLE;
        }
    }
}

static void Routine_StartEreaseMemory(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen)
{
    /* user add start erease memory code */
    uint32_t memAddress = 0, memSize = 0;
    uint8_t  addressLen = 0, memSizeLen = 0, i = 0;

    if(pdata == NULL)
    {
        *pRspLen = 0;
        return;
    }

    addressLen = ((pdata[0] >> 4) & 0x0F);
    memSizeLen = ((pdata[0] >> 0) & 0x0F);

    for(i = 0; i < addressLen; i++)
    {
        memAddress |= (((uint32_t)pdata[i + 1]) << ((3 - i) * 8));
    }

    for(i = 0; i < memSizeLen; i++)
    {
        memSize |= (((uint32_t)pdata[i + 5]) << ((3 - i) * 8));
    }

    Bootloader_EventEraseRequest(memAddress, memSize);
}

static void Routine_StopEreaseMemory(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen)
{
    
}

static void Routine_ResultEreaseMemory(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen)
{
    uint16_t txLen = *pRspLen;

    if(Bootloader_GetBootState() == BOOTLOADER_STATE_ERASE)
    {
        Bootloader_SubStateType subState;

        subState = Bootloader_GetBootSubState();
        if(subState == BOOTLOADER_SUB_STATE_END)
        {
            pRspData[txLen++]            = 0x01;
            udsRtnCtrlTable[0].rtnStatus = UDS_RTN_ST_IDLE;
        }
        else
        {
            pRspData[txLen++] = 0x02;
        }
    }
    else
    {
        pRspData[txLen++]            = 0x03;
        udsRtnCtrlTable[0].rtnStatus = UDS_RTN_ST_IDLE;
    }

    *pRspLen = txLen;
}

static void Routine_StartValidityData(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen)
{
    Bootloader_EventDataCheckRequest();
}

static void Routine_StopValidityData(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen)
{
    
}

static void Routine_ResultValidityData(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen)
{
    uint16_t txLen = *pRspLen;

    if(Bootloader_GetBootState() == BOOTLOADER_STATE_CHECK)
    {
        if(Bootloader_GetBootSubState() == BOOTLOADER_SUB_STATE_DOING)
        {
            pRspData[txLen++] = 0x02;
        }
        else if(Bootloader_GetBootSubState() == BOOTLOADER_SUB_STATE_END)
        {
            if(Bootloader_GetCrcResult() == true)
            {
                pRspData[txLen++] = 0x01;

                udsRtnCtrlTable[2].rtnStatus = UDS_RTN_ST_IDLE;
            }
            else
            {
                pRspData[txLen++] = 0x03;

                udsRtnCtrlTable[2].rtnStatus = UDS_RTN_ST_IDLE;
            }
        }
    }

    *pRspLen = txLen; 
}

static void Routine_StartCheckFlsDriverValid(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen)
{
    Bootloader_EventDataCheckRequest();
}

static void Routine_StopCheckFlsDriverValid(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen)
{
    
}

static void Routine_ResultCheckFlsDriverValid(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen)
{
    uint16_t txLen = *pRspLen;

    if(Bootloader_GetBootState() == BOOTLOADER_STATE_CHECK)
    {
        if(Bootloader_GetBootSubState() == BOOTLOADER_SUB_STATE_DOING)
        {
            pRspData[txLen++] = 0x02;
        }
        else if(Bootloader_GetBootSubState() == BOOTLOADER_SUB_STATE_END)
        {
            if(Bootloader_GetCrcResult() == true)
            {
                pRspData[txLen++] = 0x01;

                udsRtnCtrlTable[1].rtnStatus = UDS_RTN_ST_IDLE;
            }
            else
            {
                pRspData[txLen++] = 0x03;

                udsRtnCtrlTable[1].rtnStatus = UDS_RTN_ST_IDLE;
            }
        }
    }

    *pRspLen = txLen;
}

static void Routine_StartVerifyDataDependency(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen)
{
    
}

static void Routine_StopVerifyDataDependency(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen)
{
    
}

static void Routine_ResultVerifyDataDependency(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen)
{
    
}

void UdsService31_RoutineControl(UdsType *obj, const uint8_t msgBuf[], uint16_t msgLen)
{
    const uint8_t *pOptionParam               = NULL;
    uint8_t        subFunction                = 0;
    uint16_t       optionParamLen             = 0;
    uint16_t       routineId                  = 0;
    uint8_t        rtnIndex                   = 0;
    uint8_t        rntNum                     = sizeof(udsRtnCtrlTable) / sizeof(Uds_RtnCtrl);
    uint8_t        rspBuffer[UDS_RSP_LEN_MAX] = {0};
    uint16_t       rspLen                     = 0;

    if(msgLen < obj->seviceTable[obj->curServiceIdx].minLen)
    {
        Uds_NegativeResponse(obj, 0x31, NRC_INVALID_MESSAGE_LENGTH_OR_FORMAT);
        return;
    }

    subFunction    = UDS_GET_SUB_FUNCTION(msgBuf[1]);
    routineId      = ((uint16_t)msgBuf[2] << 8) + msgBuf[3];
    optionParamLen = msgLen - 4;
    if(optionParamLen > 0)
    {
        pOptionParam = &msgBuf[4];
    }

    for(rtnIndex = 0; rtnIndex < rntNum; rtnIndex++)
    {
        if(udsRtnCtrlTable[rtnIndex].rtnCtrlId == routineId)
        {
            if(obj->session == udsRtnCtrlTable[rtnIndex].sessionLevel)
            {
                if((udsRtnCtrlTable[rtnIndex].securityLevel != UDS_SA_NONE) && (udsRtnCtrlTable[rtnIndex].securityLevel != obj->securityLevel))
                {
                    Uds_NegativeResponse(obj, 0x31, NRC_SECURITY_ACCESS_DENIED);
                    return;
                }

                break;
            }
            else
            {
                Uds_NegativeResponse(obj, 0x31, NRC_CONDITIONS_NOT_CORRECT);
                return;
            }
        }
    }

    if(rtnIndex >= rntNum)
    {
        Uds_NegativeResponse(obj, 0x31, NRC_REQUEST_OUT_OF_RANGE);
        return;
    }

    memset(rspBuffer, 0, UDS_RSP_LEN_MAX);
    rspBuffer[0] = UDS_GET_POSITIVE_RSP(0x31);
    rspBuffer[1] = msgBuf[1];
    rspBuffer[2] = msgBuf[2];
    rspBuffer[3] = msgBuf[3];
    rspLen       = 4;

    switch(subFunction)
    {
    case UDS_ROUTINE_CTRL_START:
        if(udsRtnCtrlTable[rtnIndex].rtnStatus == UDS_RTN_ST_RUNNING)
        {
            rspBuffer[0] = NEGATIVE_RSP;
            rspBuffer[1] = 0x31;
            rspBuffer[2] = NRC_REQUEST_SEQUENCE_ERROR;
            rspLen       = 3;
        }
        else
        {
            udsRtnCtrlTable[rtnIndex].Routine_Start(pOptionParam, optionParamLen, rspBuffer, UDS_RSP_LEN_MAX, &rspLen);
            udsRtnCtrlTable[rtnIndex].rtnStatus = UDS_RTN_ST_RUNNING;
        }
        break;
    case UDS_ROUTINE_CTRL_STOP:
        if(udsRtnCtrlTable[rtnIndex].rtnStatus != UDS_RTN_ST_IDLE)
        {
            udsRtnCtrlTable[rtnIndex].Routine_Stop(pOptionParam, optionParamLen, rspBuffer, UDS_RSP_LEN_MAX, &rspLen);
            udsRtnCtrlTable[rtnIndex].rtnStatus = UDS_RTN_ST_IDLE;
        }
        else
        {
            rspBuffer[0] = NEGATIVE_RSP;
            rspBuffer[1] = 0x31;
            rspBuffer[2] = NRC_REQUEST_SEQUENCE_ERROR;
            rspLen       = 3;
        }
        break;
    case UDS_ROUTINE_CTRL_REQUEST_RESULT:
        if(udsRtnCtrlTable[rtnIndex].rtnStatus != UDS_RTN_ST_IDLE)
        {
            udsRtnCtrlTable[rtnIndex].Routine_Result(pOptionParam, optionParamLen, rspBuffer, UDS_RSP_LEN_MAX, &rspLen);
        }
        else
        {
            rspBuffer[0] = NEGATIVE_RSP;
            rspBuffer[1] = 0x31;
            rspBuffer[2] = NRC_REQUEST_SEQUENCE_ERROR;
            rspLen       = 3;
        }
        break;
    default:
        rspBuffer[0] = NEGATIVE_RSP;
        rspBuffer[1] = 0x31;
        rspBuffer[2] = NRC_SUBFUNCTION_NOT_SUPPORTED;
        rspLen       = 3;
        return;
    }

    /* polyspace-begin DEFECT:DEAD_CODE [No action planned:Low] "Still reserve though it's maybe unreachable" */
    if((rspLen >= 4) && (rspBuffer[0] == UDS_GET_POSITIVE_RSP(0x31)))
    {
        Uds_PositiveResponse(obj, rspBuffer, rspLen);
    }
    else if((rspLen == 3) && (rspBuffer[0] == NEGATIVE_RSP))
    {
        Uds_NegativeResponse(obj, 0x31, rspBuffer[2]);
    }
    else
    {
        Uds_NegativeResponse(obj, 0x31, NRC_REQUEST_SEQUENCE_ERROR);
    }
    /* polyspace-end DEFECT:DEAD_CODE [No action planned:Low] "Still reserve though it's maybe unreachable" */
}
