/**
 * @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 "stdlib.h"
/*==================================================================================================
 *                           SECURITY ACCESS INCLUDE FILES AND MACROS
==================================================================================================*/
#ifdef UDS_IP_SecurityAccess_ENABLE
#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
#endif
/*==================================================================================================
 *                           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 uds_u32_t FLASH_GetSectorSize(uds_u32_t dest);
#else
#include "fls_drv_bin.h"
#define FLS_DRV_INVALID_ADDR (0xFFFFFFFF)
/* 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_INVALID_ADDR;
#endif

/*==================================================================================================
 *         RequestDownload RequestUpload TransferData MACROS
==================================================================================================*/
#if defined(UDS_IP_RequestDownload_ENABLE) || defined(UDS_IP_RequestUpload_ENABLE) || defined(UDS_IP_TransferData_ENABLE)
#ifndef UDS_MAX_BLOCK_SIZE
#define UDS_MAX_BLOCK_SIZE 0x81
#endif
#endif

/*==================================================================================================
 *                          CRC INCLUDE FILES AND MACROS
==================================================================================================*/
#ifdef UDS_IP_RoutineControlCrcCheck_ENABLE
#if defined CPU_YTM32B1HA0
#include "pcrc_driver.h"
#define INST_PCRC        (0)
#else
#include "crc_driver.h"
#endif
#define INST_CRC        (0)
#endif

/*==================================================================================================
 *                                   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 getMemoryType(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 readFlash(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 writeFlash(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++) {
            uds_u32_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++){
            uds_u32_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 eraseFlash(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)
{
    uds_u32_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 writeFlash(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++) {
            uds_u32_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)((uds_u32_t)(pfls_drv_tbl->program) + (uds_u32_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++){
            uds_u32_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)((uds_u32_t)(pfls_drv_tbl->program) + (uds_u32_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 eraseFlash(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)((uds_u32_t)(pfls_drv_tbl->eraseSector) + (uds_u32_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 readRam(uds_u32_t address, uds_u8_t* data, uds_u32_t dataLength){
    memcpy((void*)data, (void*)address, dataLength);
    return dataLength;
}

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

static uds_s32_t eraseRam(uds_u32_t address, uds_u32_t dataLength){
    memset((void*)address, 0, dataLength);
    return dataLength;
}

/*==================================================================================================
 *                                       VARIABLES
==================================================================================================*/
uds_ip_api_t uds_global_ip_api = {
    .getMemoryType = getMemoryType,
    .readFlash = readFlash,
    .writeFlash = writeFlash,
    .eraseFlash = eraseFlash,
    .readRam = readRam,
    .writeRam = writeRam,
    .eraseRam = eraseRam
};

//uds_bool_t resetCommand = uds_false;

#ifdef UDS_IP_SecurityAccess_ENABLE
#ifdef CPU_YTM32B1MC0
/* user can change it*/
const uds_u32_t key[4] = {0x2b7e1516, 0x28aed2a6, 0xabf71588, 0x09cf4f3c};
#elif defined CPU_YTM32B1LE0
const uds_u8_t key[4] = {0x81, 0x68, 0x22, 0x25};
#endif
#endif

/*==================================================================================================
 *                                       GLOBAL FUNCTIONS
==================================================================================================*/

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

#ifdef UDS_IP_Session_ENABLE
/**
 * @brief DiagnosticSessionControl (0x10) service
 * 
 * @param channel Uds channel
 * @param data Received data
 * @param dataLength Received data length
 * @param param function param
 */
void UDS_IP_Session(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= data[1];
        Uds_SetSession(channel, sf);
        QUICK_REPL(channel, data[0], sf);
    }
}
#endif


#ifdef UDS_IP_TesterPresent_ENABLE
/**
 * @brief TesterPresent (0x3E) service
 * 
 * @param channel Uds channel
 * @param data Received data
 * @param dataLength Received data length
 * @param param function param
 */
void UDS_IP_TesterPresent(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]);
    }
}
#endif

#ifdef UDS_IP_ReadMemoryByAddress_ENABLE
/**
 * @brief ReadMemoryByAddress (0x23) service
 * 
 * @param channel Uds channel
 * @param data Received data
 * @param dataLength Received data length
 * @param param function param
 */
void UDS_IP_ReadMemoryByAddress(uds_channel_t channel, uds_u8_t* data, uds_u16_t dataLength, void* param){
    uds_ip_mem_t memType;
    uds_bool_t ok=uds_false;
    uds_nrc_t nrc=UDS_NRC_GR;
    uds_sid_t sid=data[0];
    do{
        if(dataLength<4){
        /* the length of the message is wrong */
            nrc=UDS_NRC_IMLOIF;
            break;
        }
        /* bit 3 - 0: Length (number of bytes) of the memoryAddress parameter*/
        uds_u8_t addrLen= data[1]&0xf;
        /* bit 7 - 4: Length (number of bytes) of the memorySize parameter. */
        uds_u8_t sizeLen= (data[1]>>4)&0xf;
        if(dataLength<(2+addrLen+sizeLen)){
            /* incorrectMessageLengthOrInvalidFormat. The length of the message is wrong */
            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[2+i];
        }
        uds_u32_t size=0;
        for(uds_u8_t i=0;i<sizeLen;i++){
            size<<=8;
            size+=data[2+addrLen+i];
        }
        /* get memory type of the address */
        memType= uds_global_ip_api.getMemoryType(addr,size);
        if(memType==UDS_IP_MEM_INVALID){
            nrc=UDS_NRC_ROOR;
            break;
        }
        if((size+1)>uds_config[channel].bufferSize){
            /* responseTooLong. The response to be generated exceeds the maximum number.*/
            nrc=UDS_NRC_RTL;
            break;
        }
        /* reused the rx buffer*/
        if(memType==UDS_IP_MEM_RAM){
            if(uds_global_ip_api.readRam(addr, data, size)!=size){
                nrc=UDS_NRC_GR;
                break;
            }
        }else{
            if(uds_global_ip_api.readFlash(addr, data, size)!=size){
                nrc=UDS_NRC_GR;
                break;
            }
        }
        ok=uds_true;
        Uds_SendPositiveResponse(channel, sid, data, size);
    }while(0);
    if(ok==uds_false){
        Uds_SendNegativeResponse(channel, sid, nrc, NULL, 0);
    }
}
#endif


#ifdef UDS_IP_RoutineControlEraseFlashMemory_ENABLE
/**
 * @brief RoutineControl (0x31) service Erase Flash Memory
 * 
 * @param channel Uds channel
 * @param data Received data, routineControlOptionRecord1-4:address, routineControlOptionRecord5-8:size,
 * @param dataLength Received data length
 * @param param function param
 */
void UDS_IP_RoutineControlEraseFlashMemory(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;
    do{
        if(dataLength<12){
            /* the length of the message is wrong */
            eraseOffset[channel]=0;
            nrc=UDS_NRC_IMLOIF;
            break;
        }
        if(((data[2]<<8)|(data[3]))!=UDS_RC_ERASE_FLASH){
            eraseOffset[channel]=0;
            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_s32_t ret=uds_global_ip_api.eraseFlash(addr+eraseOffset[channel],size-eraseOffset[channel]);
        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]+=ret;
        if(eraseOffset[channel]==size){
            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;
        }
    }while(0);

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

}
#endif

#ifdef UDS_IP_RoutineControlEraseRamMemory_ENABLE
/**
 * @brief RoutineControl (0x31) service Erase Ram Memory
 * 
 * @param channel Uds channel
 * @param data Received data, routineControlOptionRecord1-4:address, routineControlOptionRecord5-8:size,
 * @param dataLength Received data length
 * @param param function param
 */
void UDS_IP_RoutineControlEraseRamMemory(uds_channel_t channel, uds_u8_t* data, uds_u16_t dataLength, void* param){
    uds_nrc_t nrc=UDS_NRC_GR;
    uds_bool_t ok=uds_false;
    do{
        if(dataLength<12){
            /* the length of the message is wrong */
            nrc=UDS_NRC_IMLOIF;
            break;
        }
        if(((data[2]<<8)|(data[3]))!=UDS_RC_ERASE_RAM){
            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_s32_t ret=uds_global_ip_api.eraseRam(addr,size);
        if(ret<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;
        }
        
        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);

    }while(0);

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

}
#endif

#ifdef UDS_IP_RoutineControlCrcCheck_ENABLE
/**
 * @brief RoutineControl (0x31) service Crc Check
 * 
 * @param channel Uds channel
 * @param data Received data, routineControlOptionRecord1-4:address, routineControlOptionRecord5-8:size, routineControlOptionRecord9-10:crcResult
 * @param dataLength Received data length
 * @param param function param
 */
void UDS_IP_RoutineControlCrcCheck(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(addr,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 = crcCalculate((const uds_u16_t *)addr, size);
        if(((crcResult&0xFF) == data[13])&&(((crcResult>>8)&0xFF) == data[12])){

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

            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);
    }
}
#endif

#ifndef UDS_BUILD_IN_FLASH_ENABLE
#ifdef UDS_IP_RoutineControlFlashDriverDownloaded_ENABLE
/**
 * @brief RoutineControl (0x31) service flash driver downloaded notification
 * 
 * @param channel Uds channel
 * @param data Received data, routineControlOptionRecord1-4:address, routineControlOptionRecord5-8:size
 * @param dataLength Received data length
 * @param param function param
 */
void UDS_IP_RoutineControlFlashDriverDownloaded(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{
        /* 1Byte sid, 1Byte subfuc, 2Byte routineIdentifier, 4Byte flash driver address */
        if(dataLength<8){
            /* the length of the message is wrong */
            nrc=UDS_NRC_IMLOIF;
            break;
        }
        if(((data[2]<<8)|(data[3]))!=UDS_RC_FLASHDRIVER_DOWNLOADED){
            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];
        }

        pfls_drv_tbl = (fls_drv_tbl_t *)addr;

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

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

    if(ok==uds_false){
        Uds_SendNegativeResponse(channel, data[0], nrc, NULL, 0);
    }
}
#endif /* UDS_IP_RoutineControlFlashDriverDownloaded_ENABLE */
#ifdef UDS_IP_BootSwap_ENABLE
/**
 * @brief BootSwap (0x31) service flash boot swap
 * 
 * @param channel Uds channel
 * @param data Received data
 * @param dataLength Received data length
 * @param param function param
 */
void UDS_IP_BootSwap(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;
    status_t status=STATUS_SUCCESS;
    uds_bool_t ok=uds_false;
    do{
        /* 1Byte sid, 1Byte subfuc, 2Byte routineIdentifier */
        if(dataLength<4){
            /* the length of the message is wrong */
            /* incorrectMessageLengthOrInvalidFormat */
            nrc=UDS_NRC_IMLOIF;
            break;
        }
        if(((data[2]<<8)|(data[3]))!=UDS_RC_BOOT_SWAP){
            nrc=UDS_NRC_IMLOIF;
            break;
        }
        if(pfls_drv_tbl == (fls_drv_tbl_t *)FLS_DRV_INVALID_ADDR){
            nrc=UDS_NRC_GPF;
            break;
        }
        /* boot swap */
        status=((FLASH_BootSwap_t)((uds_u32_t)(pfls_drv_tbl->bootSwap) + (uds_u32_t)pfls_drv_tbl))();
        if(status!=STATUS_SUCCESS){
            nrc=UDS_NRC_GPF;
            break;
        }

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

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

    if(ok==uds_false){
        Uds_SendNegativeResponse(channel, data[0], nrc, NULL, 0);
    }
}
#endif   /* UDS_IP_BootSwap_ENABLE */
#else
#ifdef UDS_IP_BootSwap_ENABLE
/**
 * @brief BootSwap (0x31) service flash boot swap
 * 
 * @param channel Uds channel
 * @param data Received data
 * @param dataLength Received data length
 * @param param function param
 */
void UDS_IP_BootSwap(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;
    status_t status=STATUS_SUCCESS;
    uds_bool_t ok=uds_false;
    do{
        /* 1Byte sid, 1Byte subfuc, 2Byte routineIdentifier */
        if(dataLength<4){
            /* the length of the message is wrong */
            nrc=UDS_NRC_IMLOIF;
            break;
        }
        if(((data[2]<<8)|(data[3]))!=UDS_RC_BOOT_SWAP){
            nrc=UDS_NRC_IMLOIF;
            break;
        }

        /* boot swap */
        status=FLASH_DRV_BootSwap(0);
        if(status!=STATUS_SUCCESS){
            nrc=UDS_NRC_GPF;
            break;
        }

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

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

    if(ok==uds_false){
        Uds_SendNegativeResponse(channel, data[0], nrc, NULL, 0);
    }
}
#endif   /* UDS_IP_BootSwap_ENABLE */
#endif   /* #ifndef UDS_BUILD_IN_FLASH_ENABLE */
#ifdef UDS_IP_WriteMemoryByAddress_ENABLE
/**
 * @brief WriteMemoryByAddress (0x3D) service
 * 
 * @param channel Uds channel
 * @param data Received data
 * @param dataLength Received data length
 * @param param function param
 */
void UDS_IP_WriteMemoryByAddress(uds_channel_t channel, uds_u8_t* data, uds_u16_t dataLength, void* param){
    static uds_u32_t writeOffset[UDS_CHANNEL_NUM]={0};
    uds_ip_mem_t memType;
    uds_bool_t ok=uds_false;
    uds_nrc_t nrc=UDS_NRC_GR;
    /* data[0] sid */
    uds_sid_t sid=data[0];
    /* data[1] addressAndLengthFormatIdentifier */
    /* bit 3 - 0: Length (number of bytes) of the memoryAddress parameter */
    uds_u8_t addrLen= data[1]&0xf;
    /* bit 7 - 4: Length (number of bytes) of the memorySize parameter */
    uds_u8_t sizeLen= (data[1]>>4)&0xf;
    do{
        if(dataLength<4){
            /* incorrectMessageLengthOrInvalidFormat */
            nrc=UDS_NRC_IMLOIF;
            break;
        }
        if(dataLength<(2+addrLen+sizeLen)){
            /* incorrectMessageLengthOrInvalidFormat */
            nrc=UDS_NRC_IMLOIF;
            break;
        }
        if(addrLen>4||sizeLen>4){
            /* outof range */
            nrc = UDS_NRC_ROOR;
            break;
        }

        uds_u32_t addr=0;
        for(uds_u8_t i=0;i<addrLen;i++){
            addr<<=8;
            addr+=data[2+i];
        }
        uds_u32_t size=0;
        for(uds_u8_t i=0;i<sizeLen;i++){
            size<<=8;
            size+=data[2+addrLen+i];
        }
        memType= uds_global_ip_api.getMemoryType(addr,size);
        if(memType==UDS_IP_MEM_INVALID){
            /* requestOutOfRange */
            nrc=UDS_NRC_ROOR;
            break;
        }
        if(memType==UDS_IP_MEM_RAM){
            if(uds_global_ip_api.writeRam(addr, data+2+addrLen+sizeLen, size)!=size){
                /* generalProgrammingFailure. This NRC shall be returned if the server detects an error when writing to a memory location. */
                nrc=UDS_NRC_GPF;
            }else{
                ok=uds_true;
            }
        }else{
            uds_s32_t ret=uds_global_ip_api.writeFlash(addr+writeOffset[channel], data+2+addrLen+sizeLen+writeOffset[channel], size-writeOffset[channel]);
            if(ret<0){
                /* generalProgrammingFailure. This NRC shall be returned if the server detects an error when writing to a memory location. */
                nrc=UDS_NRC_GPF;
            }else{
                writeOffset[channel]+=ret;
                if(writeOffset[channel]==size){
                    ok=uds_true;
                   
                }else{
                    /* not finished */
                    nrc=UDS_NRC_RCRRP;
                }
            }
        }
    }while(0);

    if(ok==uds_false){
        if(nrc!=UDS_NRC_RCRRP){
            writeOffset[channel]=0;
        }
        Uds_SendNegativeResponse(channel, sid, nrc, NULL, 0);
    }else{
        writeOffset[channel]=0;
        Uds_SendPositiveResponse(channel, sid, &data[1], 1+addrLen+sizeLen);
    }
}
#endif


#ifdef UDS_IP_RequestDownload_ENABLE
/**
 * @brief RequestDownload (0x34) service
 * 
 * @param channel Uds channel
 * @param data Received data
 * @param dataLength Received data length
 * @param param function param
 */
void UDS_IP_RequestDownload(uds_channel_t channel, uds_u8_t* data, uds_u16_t dataLength, void* param){
    uds_sid_t sid=data[0];
    static uds_ud_t udState[UDS_CHANNEL_NUM];
    uds_nrc_t nrc=UDS_NRC_GR;
    uds_bool_t ok=uds_false;
    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);
    }
}
#endif

#ifdef UDS_IP_RequestUpload_ENABLE
/**
 * @brief RequestUpload (0x35) service
 * 
 * @param channel Uds channel
 * @param data Received data
 * @param dataLength Received data length
 * @param param function param
 */
void UDS_IP_RequestUpload(uds_channel_t channel, uds_u8_t* data, uds_u16_t dataLength, void* param){
    uds_sid_t sid=data[0];
    static uds_ud_t udState[UDS_CHANNEL_NUM];
    uds_nrc_t nrc=UDS_NRC_GR;
    uds_bool_t ok=uds_false;
    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);
    }
}
#endif

#ifdef UDS_IP_TransferData_ENABLE
/**
 * @brief TransferData (0x36) service
 * 
 * @param channel Uds channel
 * @param data Received data
 * @param dataLength Received data length
 * @param param function param
 */
void UDS_IP_TransferData(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 */
        }
    }
}
#endif

#ifdef UDS_IP_RequestTransferExit_ENABLE
/**
 * @brief RequestTransferExit (0x37) service
 * 
 * @param channel Uds channel
 * @param data Received data
 * @param dataLength Received data length
 * @param param function param
 */
void UDS_IP_RequestTransferExit(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;
    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);
    }
}
#endif

#ifdef UDS_IP_SecurityAccess_ENABLE
/**
 * @brief SecurityAccess (0x27) service
 * 
 * @param channel Uds channel
 * @param data  Received data
 * @param dataLength Received data length
 * @param param function param
 */
#if (defined CPU_YTM32B1ME0) || (defined CPU_YTM32B1MD1) || (defined CPU_YTM32B1MC0) || (defined CPU_YTM32B1HA0)
void UDS_IP_SecurityAccess(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;
#ifdef CPU_YTM32B1MC0
    uds_u32_t randomBuf[UDS_CHANNEL_NUM][4] = { 0u };
#else
    uds_u32_t randomBuf[UDS_CHANNEL_NUM][8] = { 0u };
#endif
    static uds_u8_t seedBuf[UDS_CHANNEL_NUM][AES_SEED_LEN] = { 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)
        {
            /* Generate 32-byte random numbers */
            status_t status = STATUS_ERROR;
            status = TRNG_DRV_GetStatus(0);
            while(STATUS_BUSY == TRNG_DRV_GetStatus(0));
            if(STATUS_SUCCESS != status){
                nrc=UDS_NRC_GR;
                break;
            }
            TRNG_DRV_Get_Ent(0, &(randomBuf[channel][0]));

            for (uds_u8_t i = 0; i < 4; i++) {
                seedBuf[channel][i * 4] = (randomBuf[channel][i] >> 24) & 0xFF;
                seedBuf[channel][i * 4 + 1] = (randomBuf[channel][i] >> 16) & 0xFF;
                seedBuf[channel][i * 4 + 2] = (randomBuf[channel][i] >> 8)  & 0xFF;
                seedBuf[channel][i * 4 + 3] = (randomBuf[channel][i])       & 0xFF;
            }

            uds_u8_t tmpData[AES_SEED_LEN + 1];
            /* subfunction */
            tmpData[0]=data[1];
            /* seed*/
            for (uds_u8_t i = 0; i < AES_SEED_LEN; i++) {
                tmpData[i+1] = seedBuf[channel][i];
            }
            attemptsCnt[channel] = 0;
            seedState[channel] = uds_true;
            ok=uds_true;
            Uds_SendPositiveResponse(channel, sid, tmpData, AES_SEED_LEN + 1);
        }
        /* sendKey */
        else{
            if(seedState[channel] == uds_false)
            {
                /*requestSequenceError*/
                nrc=UDS_NRC_RSE;
                break;
            }
            if(attemptsCnt[channel] > ATTEMPT_CNT_MAX)
            {
                nrc=UDS_NRC_ENOA;
                break;
            }
            /* AES 128bit*/
            /* sid,subfunc,16Byte data */
            if(dataLength != AES_SEED_LEN + 2)
            {
                nrc=UDS_NRC_IK;
                attemptsCnt[channel]++;
                break;
            }
            uds_u8_t* receivedCipher = (uds_u8_t*)(data+2);
            uds_u8_t plainText[AES_SEED_LEN];
#ifdef CPU_YTM32B1MC0            
            /* AES decrypt algorithm configuration */
            aes_algorithm_config_t decryptCfg = {
                .dataInputPtr = (uds_u32_t const *)receivedCipher,
                .dataOutputPtr = (uds_u32_t *)plainText,
                .msgLen = 64,
                .swKeyPtr = key,
                .hwKeySelected = false,
                .msgType = MSG_ALL,
                .keySize = KEY_SIZE_128_BITS,
            };
            status_t status = HCU_DRV_DecryptECB((&decryptCfg));
#else
            status_t status = HCU_DRV_DecryptECB(receivedCipher, AES_SEED_LEN, plainText);
#endif
            if(STATUS_SUCCESS != status)
            {
                nrc=UDS_NRC_GR;
                break;
            }
            uds_u8_t i = 0;
            for (i = 0; i < AES_SEED_LEN; i++)
            {
                if (plainText[i] != seedBuf[channel][i])
                {
                    attemptsCnt[channel]++;
                    nrc=UDS_NRC_IK;
                    break;
                }
            }
            if(i == AES_SEED_LEN)
            {
                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);
    }
}
#elif defined CPU_YTM32B1LE0
void UDS_IP_SecurityAccess(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_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* receivedCipher = (uds_u8_t*)(data+2);
            uds_u8_t plainText[4];

            xor_encrypt_decrypt(receivedCipher, 4, key, 4, plainText);

            uds_u8_t i = 0;
            for (i = 0; i < 4; i++)
            {
                if (plainText[i] != seedBuf[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);
    }
}
#else
#endif
#endif

#ifdef UDS_IP_ECUResetSoftReset_ENABLE
/**
 * @brief SecurityAccess (0x11) service subfuction03
 * 
 * @param channel Uds channel
 * @param data  Received data
 * @param dataLength Received data length
 * @param param function param
 */
void UDS_IP_ECUResetSoftReset(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);
    }
}
#endif