/*
 * 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_PGM_CONDITION_CHECK     0x0203
#define RID_CRC_CHECK               0x0202
#define RID_ERASE_MEMORY            0xFF00
#define RID_COMPATIBILITY           0xFF01


/*******************************************************************************
 * 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_StartCheckPgmCondition(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen);
static void Routine_StopCheckPgmCondition(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen);
static void Routine_ResultCheckPgmCondition(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen);

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_StartCheckCrc(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen);
static void Routine_StopCheckCrc(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen);
static void Routine_ResultCheckCrc(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen);

static void Routine_StartVerifyCompatibility(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen);
static void Routine_StopVerifyCompatibility(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen);
static void Routine_ResultVerifyCompatibility(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_PGM_CONDITION_CHECK, UDS_SESSION_PROGRAMMING | UDS_SESSION_EXTENDED, UDS_SA_NONE,    UDS_RTN_ST_IDLE, &Routine_StartCheckPgmCondition,    &Routine_StopCheckPgmCondition,    &Routine_ResultCheckPgmCondition   },
    {RID_CRC_CHECK,           UDS_SESSION_PROGRAMMING, UDS_SA_LEVEL_09, UDS_RTN_ST_IDLE, &Routine_StartCheckCrc,             &Routine_StopCheckCrc,             &Routine_ResultCheckCrc            },
    {RID_COMPATIBILITY,       UDS_SESSION_PROGRAMMING, UDS_SA_LEVEL_09, UDS_RTN_ST_IDLE, &Routine_StartVerifyCompatibility,  &Routine_StopVerifyCompatibility,  &Routine_ResultVerifyCompatibility},
};

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

void Routine_Init(void)
{
    uint8_t rtnIndex = 0;
    uint8_t rntNum   = sizeof(udsRtnCtrlTable) / sizeof(Uds_RtnCtrl);

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

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;
            break;
        }
    }
}

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

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

    /* TODO: Check pgm condition, always set true */
    pRspData[txLen++] = 0x00;
    *pRspLen          = txLen;

    Routine_StatusInit(RID_PGM_CONDITION_CHECK);
}

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

}

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

}

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 = 4;
    memSizeLen = 4;

    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);

    pRspData[0]  = NEGATIVE_RSP;
    pRspData[1]  = 0x31;
    pRspData[2]  = NRC_SERVICE_BUSY;
    *pRspLen     = 3;
}

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_StartCheckCrc(const uint8_t *pdata, uint16_t len, uint8_t *pRspData, uint16_t rspSize, uint16_t *pRspLen)
{
    uint32_t memAddrStart = 0, memSize = 0, memCrc = 0;
    uint8_t i = 0;

    *pRspLen = 0;

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

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

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

    Bootloader_EventDataCheckRequest(memAddrStart, memSize, memCrc);
}

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

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

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

    if(Bootloader_GetCrcResult() == true)
    {
        uint32_t *pTailData,addr,TailData=0xfffffffful;
        addr = *((uint32_t *)BOOTLOADER_CFG_APP_TAIL_ADDR);
        if (addr > 0x10000ul && addr < 0x38000ul)
        {
            pTailData = (uint32_t *)(addr - 4);
            TailData = *pTailData;
        }
        
        if(((*((uint32_t *)BOOTLOADER_CFG_APP_HEAD_ADDR)) != BOOTLOADER_CFG_HEAD_MASK) &&
           (TailData != BOOTLOADER_CFG_TAIL_MASK))
        {
            /* compatibility is failed */
            pRspData[txLen++] = 0x01;
            *pRspLen          = txLen;
        }
        else
        {
            /* compatibility is pass */
            pRspData[txLen++] = 0x00;
            *pRspLen          = txLen;
        }
    }
    else
    {
        /* compatibility is invalid */
        pRspData[txLen++] = 0x02;
        *pRspLen          = txLen;
    }
}

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

}

static void Routine_ResultVerifyCompatibility(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) != 0)
            {
                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);
        }
        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]);
    }
}
