/**
 * @file uds_ip.c
 * @brief 
 * @version 0.1
 * @date 2024-08-31
 * 
 * (c) Copyright 2024 Yuntu Microelectronics co.,ltd.
 * 
 * All Rights Reserved
 * 
 */

/*==================================================================================================
 *                                 GENERAL INCLUDE FILES
==================================================================================================*/
#include "uds_ip.h"
#include "stddef.h"
#include "string.h"
#include "interrupt_manager.h"
#include "Uds_Service.h"
#include "stdlib.h"
#include <stdint.h>
#include "boot_jump.h"
#include "KeyGenAlgoInterfaceEx.h"
/*==================================================================================================
 *                           SECURITY ACCESS INCLUDE FILES AND MACROS
==================================================================================================*/
#if (defined CPU_YTM32B1ME0) || (defined CPU_YTM32B1MD1) || (defined CPU_YTM32B1MC0) || (defined CPU_YTM32B1HA0)
#include "trng_driver.h"
#include "hcu_driver.h"
#define AES_SEED_LEN 		16
#endif
#define ATTEMPT_CNT_MAX     5
/*==================================================================================================
 *                           FLASH INCLUDE FILES MACROS AND VARIABLES
==================================================================================================*/
#ifndef UDS_PROGRAM_CHUNK_SIZE 
#define UDS_PROGRAM_CHUNK_SIZE 256
#endif

#ifndef UDS_ERASE_CHUNK_SECTOR_SIZE
#define UDS_ERASE_CHUNK_SECTOR_SIZE 16
#endif

#ifdef UDS_BUILD_IN_FLASH_ENABLE
#include "flash_driver.h"
extern uint32_t FLASH_GetSectorSize(uint32_t dest);
#else
#include "fls_drv_bin.h"
#define FLS_DRV_INVALID_ADDR    (0xFFFFFFFF)
#define FLS_DRV_VALID_ADDR      (0x20003000)
/* Base address of the flash driver bin which download to SRAM, change the value to valid address when received flash driver download notification */
static fls_drv_tbl_t * pfls_drv_tbl = (fls_drv_tbl_t *)FLS_DRV_VALID_ADDR;
#endif

static uds_ud_t udState[UDS_CHANNEL_NUM];

/*==================================================================================================
 *         RequestDownload RequestUpload TransferData MACROS
==================================================================================================*/
#ifndef UDS_MAX_BLOCK_SIZE
#define UDS_MAX_BLOCK_SIZE 0x82
#endif

/*==================================================================================================
 *                          CRC INCLUDE FILES AND MACROS
==================================================================================================*/

#if defined CPU_YTM32B1HA0
#include "pcrc_driver.h"
#define INST_PCRC        (0)
#else
#include "crc_driver.h"
#endif
#define INST_CRC        (0)


/*==================================================================================================
 *                                   LOCAL FUNCTION
==================================================================================================*/
#ifdef UDS_IP_RoutineControlCrcCheck_ENABLE
static uds_u16_t crcCalculate(const uds_u16_t *data, uds_u32_t lenth){
    uds_u16_t crcResult = 0;
#if defined CPU_YTM32B1HA0
    pcrc_user_config_t pcrc_config;
    PCRC_DRV_GetConfig(INST_PCRC, &pcrc_config);
    PCRC_DRV_WriteData16(INST_PCRC, data, lenth);
    crcResult = (uds_u16_t)PCRC_DRV_GetCrcResult(INST_CRC);
    PCRC_DRV_Init(INST_PCRC, &pcrc_config);
#else
    crc_user_config_t crc_config;
    CRC_DRV_GetConfig(INST_CRC, &crc_config);
    CRC_DRV_WriteData16(INST_CRC, data, lenth);
    crcResult = (uds_u16_t)CRC_DRV_GetCrcResult(INST_CRC);
    CRC_DRV_Init(INST_CRC, &crc_config);
#endif
    return crcResult;
}
#endif

#if (defined UDS_IP_SecurityAccess_ENABLE)&&(defined CPU_YTM32B1LE0)
void xor_encrypt_decrypt(uds_u8_t *data, uds_u8_t data_len, const uds_u8_t *key, uds_u8_t key_len, uds_u8_t *result) {
    for (uds_u8_t i = 0; i < data_len; i++) {
        result[i] = data[i] ^ key[i % key_len];
    }
}
#endif

static uds_ip_mem_t getMemoryTypeA(uds_u32_t address,uds_u32_t size){
#ifdef CPU_YTM32B1ME0
    if((address+size)<=0xFFFFF){
        return UDS_IP_MEM_FLASH;
    }
    if(address>=0x100000&&(address+size)<=0x13ffff){
        return UDS_IP_MEM_FLASH;
    }
    if(address>=0x1fff0000&&(address+size)<=0x2000ffff){
        return UDS_IP_MEM_RAM;
    }
    return UDS_IP_MEM_INVALID;
#elif defined CPU_YTM32B1HA0
    if(address>=0x2000000&&(address+size)<=0x21FFFFF){
        return UDS_IP_MEM_FLASH;
    }
    if(address>=0x20020000&&(address+size)<=0x2005ffff){
        return UDS_IP_MEM_RAM;
    }
    return UDS_IP_MEM_INVALID;
#elif defined CPU_YTM32B1MD1
    if(address<=0x7FFFF){
        return UDS_IP_MEM_FLASH;
    }
    if(address>=0X1FFF8000&&(address+size)<=0x20007FFF){
        return UDS_IP_MEM_RAM;
    }
    return UDS_IP_MEM_INVALID;
#elif defined CPU_YTM32B1MC0
    if(address<=0x3FFFF){
        return UDS_IP_MEM_FLASH;
    }
    if(address>=0X20000000&&(address+size)<=0x20007FFF){
        return UDS_IP_MEM_RAM;
    }
    return UDS_IP_MEM_INVALID;
#elif defined CPU_YTM32B1LE0
    if(address<=0x100007FF){
        return UDS_IP_MEM_FLASH;
    }
    if(address>=0x20000000&&(address+size)<=0x20003fff){
        return UDS_IP_MEM_RAM;
    }
    return UDS_IP_MEM_INVALID;
#else
    return UDS_IP_MEM_INVALID;
#endif
}

static uds_s32_t readFlashA(uds_u32_t address, uds_u8_t* data, uds_u32_t dataLength){
    memcpy((void*)data, (void*)address, dataLength);
    return dataLength;
}
#ifdef UDS_BUILD_IN_FLASH_ENABLE
static uds_s32_t writeFlashA(uds_u32_t address, uds_u8_t* data, uds_u32_t dataLength){
    status_t status;
    uds_s32_t ret=-1;
    uds_u32_t writeLen=0;
    do{
        if(dataLength%FEATURE_EFM_WRITE_UNIT_SIZE!=0){
            break;
        }
        if(address%FEATURE_EFM_WRITE_UNIT_SIZE!=0){
            break;
        }
        status=FLASH_DRV_SetAsyncMode(0, false);
        if(status!=STATUS_SUCCESS){
            break;
        }
        if(dataLength>UDS_PROGRAM_CHUNK_SIZE){
            writeLen=UDS_PROGRAM_CHUNK_SIZE;
        }else{
            writeLen=dataLength;
        }
        INT_SYS_DisableIRQGlobal();
#ifdef CPU_YTM32B1HA0
        for (uds_u8_t i = 0; i < writeLen / 32; i++) {
            uint32_t tmp[8];
            for (int j = 0; j < 8; j++) {
                tmp[j] = (data[32 * i + 4 * j + 3] << 24) | 
                        (data[32 * i + 4 * j + 2] << 16) | 
                        (data[32 * i + 4 * j + 1] << 8)  | 
                        (data[32 * i + 4 * j]);
            }
            INT_SYS_DisableIRQGlobal();
            status=FLASH_DRV_Program(0, address, 32, tmp);
            INT_SYS_EnableIRQGlobal();
            address += 32;
        }
#elif (defined CPU_YTM32B1ME0 || defined CPU_YTM32B1MD1 || defined CPU_YTM32B1MC0 || defined CPU_YTM32B1LE0)
        for(uds_u8_t i=0; i<writeLen/8; i++){
            uint32_t tmp[2];
            tmp[0]=((data[8*i+3]<<24)|(data[8*i+2]<<16)|(data[8*i+1]<<8)|(data[8*i]));
            tmp[1]=((data[8*i+7]<<24)|(data[8*i+6]<<16)|(data[8*i+5]<<8)|(data[8*i+4]));
            INT_SYS_DisableIRQGlobal();
            status=FLASH_DRV_Program(0, address, 8, tmp);
            INT_SYS_EnableIRQGlobal();
            address+=8;
        }
#else
        status=STATUS_ERROR;
#endif
        INT_SYS_EnableIRQGlobal();
        if(status!=STATUS_SUCCESS){
            break;
        }
        ret=writeLen;
    }while(0);

    return ret;
}

static uds_s32_t eraseFlashA(uds_u32_t address, uds_u32_t dataLength){
    status_t status;
    uds_s32_t ret=-1;
    uds_u32_t eraseLen=0;
    do{
        status=FLASH_DRV_SetAsyncMode(0, false);
        if(status!=STATUS_SUCCESS){
            break;
        }
        eraseLen=(uds_u32_t)FLASH_GetSectorSize(address);
        if(dataLength<eraseLen){
            break;
        }
        if(dataLength%eraseLen!=0){
            break;
        }
        if(dataLength>eraseLen*UDS_ERASE_CHUNK_SECTOR_SIZE){
            eraseLen=eraseLen*UDS_ERASE_CHUNK_SECTOR_SIZE;
        }else{
            eraseLen=dataLength;
        }
        INT_SYS_DisableIRQGlobal();
        status=FLASH_DRV_EraseSector(0, address, eraseLen);
        INT_SYS_EnableIRQGlobal();
        if(status!=STATUS_SUCCESS){
            break;
        }
        ret=eraseLen;
    }while(0);

    return ret;
}
#else
static uds_u32_t getSectorSize(uds_u32_t dest)
{
    uint32_t sectorSize = 0;
    if ((dest < FEATURE_EFM_MAIN_ARRAY_END_ADDRESS)
#if defined(FEATURE_EFM_MAIN_ARRAY_START_ADDRESS) && (FEATURE_EFM_MAIN_ARRAY_START_ADDRESS != 0)
            && (dest >= FEATURE_EFM_MAIN_ARRAY_START_ADDRESS)
#endif /* FEATURE_EFM_MAIN_ARRAY_START_ADDRESS */
        )
    {
        /* Flash main array */
        sectorSize = FEATURE_EFM_MAIN_ARRAY_SECTOR_SIZE;
    }
#if FEATURE_EFM_HAS_DATA_FLASH
    else if ((dest < FEATURE_EFM_DATA_ARRAY_END_ADDRESS)
            && (dest >= FEATURE_EFM_DATA_ARRAY_START_ADDRESS))
    {
        /* Flash data array */
        sectorSize = FEATURE_EFM_DATA_ARRAY_SECTOR_SIZE;
    }
#endif
#if FEATURE_EFM_HAS_NVR_FLASH
    else if ((dest < FEATURE_EFM_NVR_ARRAY_END_ADDRESS)
             && (dest >= FEATURE_EFM_NVR_ARRAY_START_ADDRESS))
    {
        /* Flash NVR array */
        sectorSize = FEATURE_EFM_NVR_ARRAY_SECTOR_SIZE;
    }
#endif
    else{
        sectorSize = 0;
    }
    return sectorSize;
}
static uds_s32_t writeFlashA(uds_u32_t address, uds_u8_t* data, uds_u32_t dataLength){
    /* User overwrite it */
    status_t status=STATUS_SUCCESS;
    uds_s32_t ret=-1;
    uds_u32_t writeLen=0;
    do{
        if(dataLength%FEATURE_EFM_WRITE_UNIT_SIZE!=0){
            break;
        }
        if((address%FEATURE_EFM_WRITE_UNIT_SIZE)!=0){
            break;
        }
        if(pfls_drv_tbl == (fls_drv_tbl_t *)FLS_DRV_INVALID_ADDR){
            break;
        }
        if(dataLength>UDS_PROGRAM_CHUNK_SIZE){
            writeLen=UDS_PROGRAM_CHUNK_SIZE;
        }else{
            writeLen=dataLength;
        }
#ifdef CPU_YTM32B1HA0
        for (uds_u8_t i = 0; i < writeLen / 32; i++) {
            uint32_t tmp[8];
            for (int j = 0; j < 8; j++) {
                tmp[j] = (data[32 * i + 4 * j + 3] << 24) | 
                        (data[32 * i + 4 * j + 2] << 16) | 
                        (data[32 * i + 4 * j + 1] << 8)  | 
                        (data[32 * i + 4 * j]);
            }
            INT_SYS_DisableIRQGlobal();
            status = ((FLASH_Program_t)((uint32_t)(pfls_drv_tbl->program) + (uint32_t)pfls_drv_tbl))(address, 32, tmp);
            INT_SYS_EnableIRQGlobal();
            address += 32;
        }
#elif (defined CPU_YTM32B1ME0 || defined CPU_YTM32B1MD1 || defined CPU_YTM32B1MC0 || defined CPU_YTM32B1LE0)
        for(uds_u8_t i=0; i<writeLen/8; i++){
            uint32_t tmp[2];
            tmp[0]=((data[8*i+3]<<24)|(data[8*i+2]<<16)|(data[8*i+1]<<8)|(data[8*i]));
            tmp[1]=((data[8*i+7]<<24)|(data[8*i+6]<<16)|(data[8*i+5]<<8)|(data[8*i+4]));
            INT_SYS_DisableIRQGlobal();
            status=((FLASH_Program_t)((uint32_t)(pfls_drv_tbl->program) + (uint32_t)pfls_drv_tbl))(address, 8, tmp);
            INT_SYS_EnableIRQGlobal();
            address+=8;
        }
#else
        status=STATUS_ERROR;
#endif
        if(status!=STATUS_SUCCESS){
            break;
        }
        ret=writeLen;
    }while(0);

    return ret;
}
static uds_s32_t eraseFlashA(uds_u32_t address, uds_u32_t dataLength){
    /* User overwrite it */
    status_t status;
    uds_s32_t ret=-1;
    uds_u32_t eraseLen=0;
    do{
        eraseLen=getSectorSize(address);
        if(dataLength<eraseLen){
            break;
        }
        if(dataLength%eraseLen!=0){
            break;
        }
        if(pfls_drv_tbl == (fls_drv_tbl_t *)FLS_DRV_INVALID_ADDR){
            break;
        }
        if(dataLength>eraseLen*UDS_ERASE_CHUNK_SECTOR_SIZE){
            eraseLen=eraseLen*UDS_ERASE_CHUNK_SECTOR_SIZE;
        }else{
            eraseLen=dataLength;
        }
        INT_SYS_DisableIRQGlobal();
        status=((FLASH_EraseSector_t)((uint32_t)(pfls_drv_tbl->eraseSector) + (uint32_t)pfls_drv_tbl))(address, eraseLen);
        INT_SYS_EnableIRQGlobal();
        if(status!=STATUS_SUCCESS){
            break;
        }
        ret=eraseLen;
    }while(0);


    return ret;
}
#endif


static uds_s32_t readRamA(uds_u32_t address, uds_u8_t* data, uds_u32_t dataLength){
    memcpy((void*)data, (void*)address, dataLength);
    return dataLength;
}

static uds_s32_t writeRamA(uds_u32_t address, uds_u8_t* data, uds_u32_t dataLength){
    memcpy((void*)address, (void*)data, dataLength);
    return dataLength;
}

static uds_u16_t crcCheck(uds_u8_t *ptr, uds_u16_t len)
{
    uds_u16_t i;
    uds_u16_t crc = 0xFFFF;
    while(len--)
    {
        crc ^= *ptr++;
        for (i = 0; i < 8; ++i)
        {
            if (crc & 1)
                crc = (crc >> 1) ^ 0xA001;
            else
                crc = (crc >> 1);
        }
    }
    return crc;
}
/*==================================================================================================
 *                                       VARIABLES
==================================================================================================*/
uds_ip_api_t uds_global_ip_api = {
    .getMemoryType = getMemoryTypeA,
    .readFlash = readFlashA,
    .writeFlash = writeFlashA,
    .eraseFlash = eraseFlashA,
    .readRam = readRamA,
    .writeRam = writeRamA
};

uds_bool_t resetCommand = uds_false;
/*==================================================================================================
 *                                       GLOBAL FUNCTIONS
==================================================================================================*/

#define QUICK_REPL(channel,sid,sf) do{\
    if(sf&0x80){\
        Uds_NoResponse(channel);\
    }else{\
        Uds_SendPositiveResponse(channel, sid, &sf, 1);\      
    }\
}while(0)



void UDS_IP_TesterPresentA(uds_channel_t channel, uds_u8_t* data, uds_u16_t dataLength, void* param){
    if(dataLength<2){
        Uds_SendNegativeResponse(channel, data[0], UDS_NRC_IMLOIF, NULL, 0);
    }else{
        QUICK_REPL(channel, data[0], data[1]);       
    }
}




void UDS_IP_SessionA(uds_channel_t channel, uds_u8_t* data, uds_u16_t dataLength, void* param){
    if(dataLength<2){
        Uds_SendNegativeResponse(channel, data[0], UDS_NRC_IMLOIF, NULL, 0);
    }else{
        uds_subfunc_t sf[8]={0x55};
        sf[0] = data[1];
        sf[1] = 0x00;
        sf[2] = 0x00;//P2_Server    0x32
        sf[3] = 0x00;//             0x13
        sf[4] = 0x00;//P2*_Server   0x88
        Uds_SetSession(channel, sf[0]);
        Uds_SendPositiveResponse(channel, data[0], &sf[0], 5);//DLC behind SID
    }
}

void UDS_IP_SessionA_1001(uds_channel_t channel, uds_u8_t* data, uds_u16_t dataLength, void* param) {
    if(dataLength<2){
        Uds_SendNegativeResponse(channel, data[0], UDS_NRC_IMLOIF, NULL, 0);
    }else{
        uds_subfunc_t sf[8]={0x55};
        sf[0] = data[1];
        sf[1] = 0x00;
        sf[2] = 0x00;//P2_Server    0x32
        sf[3] = 0x00;//             0x13
        sf[4] = 0x00;//P2*_Server   0x88
        Uds_SetSession(channel, sf[0]);
        Uds_SendPositiveResponse(channel, data[0], &sf[0], 5);//DLC behind SID
    }
}

void UDS_IP_SessionA_1002(uds_channel_t channel, uds_u8_t* data, uds_u16_t dataLength, void* param) {
    if(dataLength<2){
        Uds_SendNegativeResponse(channel, data[0], UDS_NRC_IMLOIF, NULL, 0);
    }else{
        uds_subfunc_t sf[8]={0x55};
        sf[0] = data[1];
        sf[1] = 0x00;
        sf[2] = 0x00;//P2_Server    0x32
        sf[3] = 0x00;//             0x13
        sf[4] = 0x00;//P2*_Server   0x88
        Uds_SetSession(channel, sf[0]);
        Uds_SendPositiveResponse(channel, data[0], &sf[0], 5);//DLC behind SID
    }
}

void UDS_IP_SessionA_1003(uds_channel_t channel, uds_u8_t* data, uds_u16_t dataLength, void* param) {
    if(dataLength<2){
        Uds_SendNegativeResponse(channel, data[0], UDS_NRC_IMLOIF, NULL, 0);
    }else{
        uds_subfunc_t sf[8]={0x55};
        sf[0] = data[1];
        sf[1] = 0x00;
        sf[2] = 0x00;//P2_Server    0x32
        sf[3] = 0x00;//             0x13
        sf[4] = 0x00;//P2*_Server   0x88
        Uds_SetSession(channel, sf[0]);
        Uds_SendPositiveResponse(channel, data[0], &sf[0], 5);//DLC behind SID
    }
}

void UDS_IP_SecurityAccessA(uds_channel_t channel, uds_u8_t* data, uds_u16_t dataLength, void* param){
    uds_sid_t sid=data[0];
    uds_subfunc_t sf= data[1];
    uds_nrc_t nrc=UDS_NRC_GR;
    uds_bool_t ok=uds_false;
    static uds_u8_t seedBuf[UDS_CHANNEL_NUM][4] = { 0u };
    static uds_u8_t keyBuf[UDS_CHANNEL_NUM][4] = { 0u };
    static uds_bool_t seedState[UDS_CHANNEL_NUM] = { uds_false };
    static uds_u8_t attemptsCnt[UDS_CHANNEL_NUM] = { 0 };
    do{
        if((sf < 0x01)||(sf > 0x7E))
        {
            nrc=UDS_NRC_SFNS;
            break;
        }
        /* requestSeed */
        if(sf % 2 != 0)
        {
            uds_u32_t randseed;
            randseed = SysTick->VAL;

            srand(randseed);
            for (uds_u8_t i = 0; i < 4; i++) {
                seedBuf[channel][i] = (uds_u8_t)rand();
            }

            uds_u8_t tmpData[5];
            /* subfunction */
            tmpData[0]=data[1];
            /* seed */
            for (uds_u8_t i = 0; i < 4; i++) {
                tmpData[i+1] = seedBuf[channel][i];
            }
            attemptsCnt[channel] = 0;
            seedState[channel] = uds_true;
            ok=uds_true;
            Uds_SendPositiveResponse(channel, sid, tmpData, 5);
        }
        /* sendKey */
        else{
            if(seedState[channel] == uds_false)
            {
                /*requestSequenceError*/
                nrc=UDS_NRC_RSE;
                break;
            }
            if(attemptsCnt[channel] > ATTEMPT_CNT_MAX)
            {
                nrc=UDS_NRC_ENOA;
                break;
            }
            /* sid,subfunc,4Byte data */
            if(dataLength != 6)
            {
                nrc=UDS_NRC_IK;
                attemptsCnt[channel]++;
                break;
            }

            uds_u8_t i = 0;
            uint32_t keysize;
            uds_u8_t RxKey[4];
            for (i = 0; i < 4; i++)
            {
                RxKey[i] = data[i+2];
            }

            VKeyGenResultEx status = KGRE_Ok;
            status = GenerateKeyEx(&seedBuf[channel][0],4,1,1,&keyBuf[channel][0],1024,&keysize);
            (void)status;

            for (i = 0; i < keysize; i++)
            {
                if (RxKey[i] != keyBuf[channel][i])
                {
                    attemptsCnt[channel]++;
                    nrc=UDS_NRC_IK;
                    break;
                }
            }
            if(i == 4)
            {
                seedState[channel] = uds_false;
                attemptsCnt[channel] = 0;
                ok=uds_true;
                Uds_SetSecureLevel(channel, sf/2);
                Uds_SendPositiveResponse(channel, sid, &data[1], 1);
            }
        }
    }
    while(0);
    if(ok==uds_false){
        Uds_SendNegativeResponse(channel, sid, nrc, NULL, 0);
    }
}

uint16_t userCNT = 0;
void UDS_IP_RoutineControlEraseFlashMemoryA(uds_channel_t channel, uds_u8_t* data, uds_u16_t dataLength, void* param) {
    static uds_u32_t eraseOffset[UDS_CHANNEL_NUM]={0};
    uds_nrc_t nrc=UDS_NRC_GR;
    uds_bool_t ok=uds_false;
    uds_u32_t eraseSizeOnce = MCU_SECTOR_SIZE;
    // uds_u32_t eraseSizeOnce = APP_IMAGE_LEN_MAX;

    uint8_t mark_data[8]={0};
    uds_global_ip_api.readFlash(APP_IMAGE_START-MCU_SECTOR_SIZE,mark_data,0x08);
    if (mark_data[0] != 1)
    {
        mark_data[0] = 1;
        uds_global_ip_api.eraseFlash(APP_IMAGE_START-MCU_SECTOR_SIZE,MCU_SECTOR_SIZE);
        uds_global_ip_api.writeFlash(APP_IMAGE_START-MCU_SECTOR_SIZE,mark_data,0x08);
    }  

    do{
        uds_s32_t ret=uds_global_ip_api.eraseFlash(APP_IMAGE_START+eraseOffset[channel],eraseSizeOnce);
        if(ret<0){
            eraseOffset[channel]=0;
            /* GeneralProgrammingFailure. This NRC shall be returned if the server detects an error when performing a routine, which accesses server internal memory.*/
            nrc=UDS_NRC_GPF;
            break;
        }

        eraseOffset[channel]+=eraseSizeOnce;
        if(eraseOffset[channel]>=APP_IMAGE_LEN_MAX){
            ok=uds_true;

            uds_sid_t sid=data[0];
            uds_u8_t tmpData[3];
            /* routineControlType */
            tmpData[0]=data[1];
            /* routineIdentifier */
            tmpData[1]=data[2];
            tmpData[2]=data[3];

            ok=uds_true;
            Uds_SendPositiveResponse(channel, sid, tmpData, 3);

            eraseOffset[channel]=0;
        }else{
            /* not finished */
            nrc = UDS_NRC_RCRRP;
            userCNT++;
        }
    }while(0);

    if(ok==uds_false){
        Uds_SendNegativeResponse(channel, data[0], nrc, NULL, 0);
    }

}

void UDS_IP_RequestDownloadA(uds_channel_t channel, uds_u8_t* data, uds_u16_t dataLength, void* param){
    uds_sid_t sid=data[0];
    uds_nrc_t nrc=UDS_NRC_GR;
    uds_bool_t ok=uds_false;

    volatile uint8_t testbuf[10];
    for(int i=0;i<10;i++)
    {
        testbuf[i] = data[i];
    }

    do{
        if(dataLength<5){
            nrc=UDS_NRC_IMLOIF;
            break;
        }
        /* data[1] dataFormatIdentifier. The value 0x00 specifies that neither compressionMethod nor encryptingMethod is used.
           Values other than 0x00 are vehicle manufacturer specific.*/
        // if(data[1]!=0x00){
        //     nrc=UDS_NRC_ROOR;
        //     break;
        // }
        /* data[2] addressAndLengthFormatIdentifier */
        /* bit 7 - 4: Length (number of bytes) of the memorySize parameter */
        /* bit 3 - 0: Length (number of bytes) of the memoryAddress parameter */
        uds_u8_t addrLen= data[2]&0xf;
        uds_u8_t sizeLen= (data[2]>>4)&0xf;
        if(dataLength<(3+addrLen+sizeLen)){
            /* incorrectMessageLengthOrInvalidFormat */
            nrc=UDS_NRC_IMLOIF;
            break;
        }
        if(addrLen>4||sizeLen>4){
            /* requestOutOfRange */
            nrc = UDS_NRC_ROOR;
            break;
        }

        uds_u32_t addr=0;
        for(uds_u8_t i=0;i<addrLen;i++){
            addr<<=8;
            addr+=data[3+i];
        }
        uds_u32_t size=0;
        for(uds_u8_t i=0;i<sizeLen;i++){
            size<<=8;
            size+=data[3+addrLen+i];
        }
        if(size==0){
            nrc=UDS_NRC_ROOR;
            break;
        }
        /*memory type check */
        if(uds_global_ip_api.getMemoryType(addr,size)==UDS_IP_MEM_INVALID){
            nrc=UDS_NRC_ROOR;
            break;
        }
        
        udState[channel].address=addr;
        udState[channel].size=size;
        udState[channel].leftSize=size;
        udState[channel].sid=sid;
        udState[channel].bsc=1;
        udState[channel].started=uds_true;
        ok=uds_true;
        uds_u8_t tmpData[5];
        /* lengthFormatIdentifier */
        /* bit 7 - 4: Length (number of bytes) of the maxNumberOfBlockLength parameter. */
        /* bit 3 - 0: reserved by document, to be set to '0'.*/
        tmpData[0]=0x40;
        size=UDS_MAX_BLOCK_SIZE;
        /* maxNumberOfBlockLength */
        for(uds_u8_t i=0;i<4;i++){
            tmpData[i+1]=(size>>(8*(3-i)))&0xff;
        }
        Uds_SetupTmpParam(channel, &udState[channel]);
        Uds_SendPositiveResponse(channel, sid, tmpData, 5);
    }while(0);
    if(ok==uds_false){
        Uds_SendNegativeResponse(channel, sid, nrc, NULL, 0);
    }
}

void UDS_IP_TransferDataA(uds_channel_t channel, uds_u8_t* data, uds_u16_t dataLength, void* param){
    uds_sid_t sid=data[0];
    uds_nrc_t nrc=UDS_NRC_GR;
    uds_bool_t ok=uds_false;
    uds_s32_t ret;
    static uds_u32_t writeOffset[UDS_CHANNEL_NUM]={0};
    uds_ud_t* udState=(uds_ud_t*)param;
    uds_u8_t readData[UDS_MAX_BLOCK_SIZE];

    do{

        if(udState==NULL){
            nrc=UDS_NRC_GR;
            break;
        }
        if(udState->started!=uds_true){
            /* requestSequenceError. The RequestDownload or RequestUpload service is not active when a request for this service is received*/
            nrc=UDS_NRC_RSE;
            break;
        }
        if(data[1]!=udState->bsc){
            /* wrongBlockSequenceCounter. This NRC shall be returned if the server detects an error in the sequence of the blockSequenceCounter.*/
            nrc=UDS_NRC_WBSC;
            break;
        }
        if(udState->sid==0x34&&dataLength>(UDS_MAX_BLOCK_SIZE+1)){
            /* incorrectMessageLengthOrInvalidFormat*/
            nrc=UDS_NRC_IMLOIF;
            break;
        }
        if(udState->sid==0x35&&dataLength>2){
            /* incorrectMessageLengthOrInvalidFormat*/
            nrc=UDS_NRC_IMLOIF;
            break;
        }
        uds_ip_mem_t memType=uds_global_ip_api.getMemoryType(udState->address,udState->size);
        if(memType==UDS_IP_MEM_INVALID){
            /* requestOutOfRange. The transferRequestParameterRecord is not consistent with the server��s memory alignment constraints.*/
            nrc=UDS_NRC_ROOR;
            break;
        }
        if(memType==UDS_IP_MEM_RAM){
            if(udState->sid==0x34){
                ret=uds_global_ip_api.writeRam(udState->address+(udState->size-udState->leftSize), data+2, dataLength-2);
                if(ret!=(dataLength-2)){
                    /* generalProgrammingFailure. This NRC shall be returned if the server detects an error when erasing or programming a 
                    memory location in the permanent memory device (e.g. Flash Memory) during the download of data.*/
                    nrc=UDS_NRC_GPF;
                    break;
                }else{
                    ok=uds_true;
                }
            }else if(udState->sid==0x35){
                ret=uds_global_ip_api.readRam(udState->address+(udState->size-udState->leftSize), readData, UDS_MAX_BLOCK_SIZE);
                if(ret!=(UDS_MAX_BLOCK_SIZE)){
                    nrc=UDS_NRC_GPF;
                    break;
                }else{
                    ok=uds_true;
                }
            }else{
                /* never be here */
            }
            
        }else{
            if(udState->sid==0x34)
            {
                ret=uds_global_ip_api.writeFlash(udState->address+writeOffset[channel]+(udState->size-udState->leftSize), data+2+writeOffset[channel], dataLength-2-writeOffset[channel]);
                if(ret<0){
                    /* generalProgrammingFailure. This NRC shall be returned if the server detects an error when erasing or programming a 
                    memory location in the permanent memory device (e.g. Flash Memory) during the download of data.*/
                    nrc=UDS_NRC_GPF;
                    break;
                }
                writeOffset[channel]+=ret;
                if(writeOffset[channel]==dataLength-2){
                    ok=uds_true;
                }else{
                    /* not finished */
                    nrc=UDS_NRC_RCRRP;
                }
            }else if(udState->sid==0x35){
                ret=uds_global_ip_api.readFlash(udState->address+(udState->size-udState->leftSize), readData, UDS_MAX_BLOCK_SIZE);
                if(ret!=(UDS_MAX_BLOCK_SIZE)){
                    nrc=UDS_NRC_GPF;
                    break;
                }else{
                    ok=uds_true;
                }
            }else{
                /* never be here */
            }
        }

    }while(0);
    if(ok==uds_false){
        if(nrc!=UDS_NRC_RCRRP){
            writeOffset[channel]=0;
        }
        Uds_SendNegativeResponse(channel, sid, nrc, NULL, 0);
    }else{
        if(udState->sid==0x34){
            writeOffset[channel]=0;
            Uds_SendPositiveResponse(channel, sid, &udState->bsc, 1);
            /* auto overflow uint8_t*/
            udState->bsc++;
            if(udState->leftSize>=(dataLength-2)){
                udState->leftSize-=(dataLength-2);
            }
        }else if(udState->sid==0x35){
            uds_u8_t tmpData[UDS_MAX_BLOCK_SIZE];
            tmpData[0]=udState->bsc;
            if(udState->leftSize>=UDS_MAX_BLOCK_SIZE-1){
                for(uds_u8_t i = 0; i < UDS_MAX_BLOCK_SIZE-1; i++){
                    tmpData[1+i] = readData[i];
                }
                udState->leftSize-=UDS_MAX_BLOCK_SIZE-1;
                Uds_SendPositiveResponse(channel, sid, tmpData, UDS_MAX_BLOCK_SIZE);
            }
            else
            {
                /* the last 36, the length < 128*/
                for(uds_u8_t i = 0; i < udState->leftSize; i++){
                    tmpData[1+i] = readData[i];
                }
                Uds_SendPositiveResponse(channel, sid, tmpData, 1+udState->leftSize);
                udState->leftSize=0;
            }
            /* auto overflow uint8_t*/
            udState->bsc++;
        }else{
            /* never be here */
        }
    }
}


void UDS_IP_RequestTransferExitA(uds_channel_t channel, uds_u8_t* data, uds_u16_t dataLength, void* param){
    uds_sid_t sid=data[0];
    uds_nrc_t nrc=UDS_NRC_GR;
    uds_bool_t ok=uds_false;
    uds_ud_t* udState=(uds_ud_t*)param;

#if 0
    uint8_t testbuf[80];
    for(int i=0;i<80;i++)
    {
        testbuf[i] = i;
    }
#if 0
    status_t status=STATUS_SUCCESS;
    INT_SYS_DisableIRQGlobal();
    status=((FLASH_EraseSector_t)((uds_u32_t)(pfls_drv_tbl->eraseSector) + (uds_u32_t)pfls_drv_tbl))(0xA000, MCU_SECTOR_SIZE);
    INT_SYS_EnableIRQGlobal();    

    INT_SYS_DisableIRQGlobal();
    status=((FLASH_Program_t)((uds_u32_t)(pfls_drv_tbl->program) + (uds_u32_t)pfls_drv_tbl))(0xA000, 80, testbuf);
    INT_SYS_EnableIRQGlobal();

    INT_SYS_DisableIRQGlobal();
    status=((FLASH_EraseSector_t)((uds_u32_t)(pfls_drv_tbl->eraseSector) + (uds_u32_t)pfls_drv_tbl))(0xA000, MCU_SECTOR_SIZE);
    INT_SYS_EnableIRQGlobal();      
#endif
    uds_global_ip_api.eraseFlash(0xA000,MCU_SECTOR_SIZE);
    uds_global_ip_api.writeFlash(0xA000,testbuf,80);
    uds_global_ip_api.eraseFlash(0xA000,MCU_SECTOR_SIZE);
#endif

    do{
        if(udState==NULL){
            nrc=UDS_NRC_GR;
            break;
        }
        if(udState->started!=uds_true||udState->leftSize!=0){
            /* requestSequenceError */
            nrc=UDS_NRC_RSE;
            break;
        }
        udState->started=uds_false;
        Uds_SetupTmpParam(channel, NULL);
        ok=uds_true;
        Uds_SendPositiveResponse(channel, sid, NULL, 0);
    }while(0);
    if(ok==uds_false){
        Uds_SendNegativeResponse(channel, sid, nrc, NULL, 0);
    }
}

void UDS_IP_RoutineControlCrcCheckA(uds_channel_t channel, uds_u8_t* data, uds_u16_t dataLength, void* param){
    uds_sid_t sid=data[0];
    uds_nrc_t nrc=UDS_NRC_GR;
    uds_bool_t ok=uds_false;
    uds_u16_t crcResult=0;
    do{
        /* 1Byte sid, 1Byte subfuc, 2Byte routineIdentifier, 4Byte address, 4Byte size, 2Byte crcResult */
        // if(dataLength<14){
        //     /* the length of the message is wrong */
        //     nrc=UDS_NRC_IMLOIF;
        //     break;
        // }
        // if(((data[2]<<8)|(data[3]))!=UDS_RC_CRC_CHECK){
        //     nrc=UDS_NRC_IMLOIF;
        //     break;
        // }
        // uds_u32_t addr=0;
        // for(uds_u8_t i=0;i<4;i++){
        //     addr<<=8;
        //     addr+=data[4+i];
        // }
        // uds_u32_t size=0;
        // for(uds_u8_t i=0;i<4;i++){
        //     size<<=8;
        //     size+=data[8+i];
        // }

        uds_ip_mem_t memType=uds_global_ip_api.getMemoryType(udState[channel].address,udState[channel].size);
        if(memType==UDS_IP_MEM_INVALID){
            /* requestOutOfRange. The transferRequestParameterRecord is not consistent with the server��s memory alignment constraints.*/
            nrc=UDS_NRC_ROOR;
            break;
        }
        crcResult = crcCheck((uds_u8_t *)udState[channel].address,udState[channel].size);

        uint8_t testbuf[8];
        uint16_t crcRx;
        for(int i=0;i<6;i++)
        {
            testbuf[i] = data[i];
        }

        crcRx = ((uint16_t)testbuf[4] << 8);
        crcRx = crcRx | (testbuf[5]);

        if(crcRx == crcResult){

            uds_u8_t tmpData[3];
            /* subfunction */
            tmpData[0]=data[1];
            /* routineIdentifier */
            tmpData[1]=data[2];
            tmpData[2]=data[3];

            if(memType==UDS_IP_MEM_FLASH){
                uint8_t mark_data[8]={0};
                uds_global_ip_api.readFlash(APP_IMAGE_START-MCU_SECTOR_SIZE,mark_data,0x08);
                if (mark_data[0] == 1)
                {
                    mark_data[0] = 0;
                    uds_global_ip_api.eraseFlash(APP_IMAGE_START-MCU_SECTOR_SIZE,MCU_SECTOR_SIZE);
                    uds_global_ip_api.writeFlash(APP_IMAGE_START-MCU_SECTOR_SIZE,mark_data,0x08);
                }
            }

            ok=uds_true;
            Uds_SendPositiveResponse(channel, sid, tmpData, 3);
        }
        else{
            nrc=UDS_NRC_GPF;
            break;
        }

    }while(0);

    if(ok==uds_false){
        Uds_SendNegativeResponse(channel, data[0], nrc, NULL, 0);
    }
}
void UDS_IP_ECUResetSoftResetA(uds_channel_t channel, uds_u8_t* data, uds_u16_t dataLength, void* param){
    uds_sid_t sid=data[0];
    uds_nrc_t nrc=UDS_NRC_GR;
    uds_bool_t ok=uds_false;
    do{
        if(dataLength != 2){
            /* incorrectMessageLengthOrInvalidFormat */
            nrc=UDS_NRC_IMLOIF;
            break;
        }
        ok=uds_true;
        // resetCommand = uds_true;
        QUICK_REPL(channel, data[0], data[1]);

    }
    while(0);
    if(ok==uds_false){
        Uds_SendNegativeResponse(channel, sid, nrc, NULL, 0);
    }
}
