#include <string.h>
#include "common_include.h"
#include "s6j3360_TCFCFG.h"
#include "UDS_Common.h"
#include "UDS_Service.h"
#include "UDS_Parameter.h"
#include <stdlib.h>

#include "abstract.h"
#include "mem.h"
#include "tcflash.h"

#include "s6j3360_CANFD.h"
#include "canfd_320.h"
#include "canfd_320_driver.h"
#include "dtc.h"

//fixed, don't touch
static UDS_ConfigType UDS_Config =
{
    UDS_ServiceTable, sizeof(UDS_ServiceTable) / sizeof(UDS_ServiceType),
    UDS_ReadDIDTable, sizeof(UDS_ReadDIDTable) / sizeof(UDS_DIDType),
    UDS_WriteDIDTable, sizeof(UDS_WriteDIDTable) / sizeof(UDS_DIDType),
    UDS_RoutineContrlTable, sizeof(UDS_RoutineContrlTable) / sizeof(UDS_RCType),
    UDS_InputOutputControlByIdentifierDIDTable, sizeof(UDS_InputOutputControlByIdentifierDIDTable) / sizeof(UDS_IOCTLType),
};
//fixed, don't touch end

//General
extern volatile uint16_t UDSServiceTimerCnts;
UDS_PARAMETER UdsParameter;
void InitDiagnostic(void) 
{
    UDS_INIT_PARA init;
    init.RespID = 0x7ce;
    init.FillByte = 0xaa;
    init.Tp_N_Ar = 1000;   //typical val
    init.Tp_N_Br = 1000;    //typical val
    init.Tp_N_STmin = 8;  //typical val
    init.Tp_N_BS = 8;    //typical val
    init.Uds_cfgP2Server = 5000;  //typical val
    //fixed, don't touch
    memcpy(&init.Uds_Config, &UDS_Config, sizeof (UDS_Config));
    //fixed, don't touch end
    UDS_InitDiagnostic(&init);  
}

uint8_t CheckVoltage(void)
{
    if (CurIgnVoltage >= 1600) {
        /*电压异常错误反馈*/
        return 1;
    } else if (CurIgnVoltage <= 900) {
        /*电压异常错误反馈*/
        return 2; 
    }
    return 0;
}

_BootJumpToAppReg JumpToAppReg;
uint8_t Api_CreateCRCCheckSum(uint8_t *p ,uint8_t Nub)
{
    uint8_t bCheckSun;
    uint8_t i;
    bCheckSun = 0xEB;
    while (Nub--) {
        bCheckSun ^= *p++;
        for (i = 0;i < 8; i++) {
            if (bCheckSun & 0x01) {
                bCheckSun = (bCheckSun >> 1) ^ 0x8C;
            } else {
                bCheckSun >>= 1;
            }
        }
    }
    return(bCheckSun);
}

void McuReset(uint8_t GotoBoot) 
{
    if (GotoBoot) {
        JumpToAppReg.Checkflag = 0x55;
        JumpToAppReg.AppExist = 0;
        JumpToAppReg.SessionType = 2;
        JumpToAppReg.bCheckSun = Api_CreateCRCCheckSum((uint8_t*) &JumpToAppReg,15);
        bsp_eeprom_write(0xAE,0,(uint8_t*)&JumpToAppReg,16);  
    }
    for (int i = 0; i < 10000; i++) {
        NOP();
        bsp_HDOG_Feed();
    }  
    IRQ_DISABLE_LOCAL();
    HWDG_TRG0 = 123456;
    HWDG_TRG1 = 123456;
    IRQ_RESTORE();
}

void SendMessageToCan(uint8_t CanCh, uint16_t ID_H, uint16_t ID_L, uint8_t *data, uint8_t len) 
{
    if (CanCh == 0) {
        CanFD_Send_StandMsg(CANFD0_Type, (ID_H << 16) | (ID_L), BUF_DIAG, TX_ISR_EN, data, len);
    } else if (CanCh == 1) {
        if (AbnormalPowerCondition == ABNORMAL_POWER_CONDITION_NORMAL) {
            CanFD_Send_StandMsg(CANFD1_Type, (ID_H << 16) | (ID_L), BUF_DIAG, TX_ISR_EN, data, len);
        }
    }
}

uint8_t UDS_CheckRequestLength(uint8_t SID, uint16_t len)
{
    if ((SID != SID_WRITE_DATA_BY_IDENTIFIER) && (UdsEolStart == 1)) {
        UDSServiceTimerCnts = 0xffff;
        UdsEolStart = 0;
    }
    switch (SID) {
        case SID_DIAGNOSTIC_SESSION_CONTROL:
            if (len == 2) {
                return 1;
            } else {
                return 0;
            }
        break;
        case SID_STOP_DIAGNOSITC_SESSION:
            if (len == 1) {
                return 1;
            } else {
                return 0;
            }
        break;
        case SID_ECU_RESET:
            if (len == 2) {
                return 1;
            } else {
                return 0;
            }
        break;
        case SID_SECURITY_ACCESS:
            if (len >= 2) {
                return 1;
            } else {
                return 0;
            }
        break;
        case SID_COMMUNICATION_CONTROL:
            if (len == 2) {
                return 1;
            } else {
                return 0;
            }
        break;
        case SID_ENABLE_NORMAL_MESSAGE_TRANSMISSION:
            if (len == 2) {
                return 1;
            } else {
                return 0;
            }
        break;
        case SID_TESTER_PRESENT:
            if (len == 2) {
                return 1;
            } else {
                return 0;
            }
        break;
        case SID_CONTROL_DTC_SETTING:
            if (len == 2) {
                return 1;
            } else {
                return 0;
            }
        break;
        case SID_CLEAR_DIAGNOSTIC_INFORMATION:
            if (len == 4) {
                return 1;
            } else {
                return 0;
            }
        break;
        case SID_READ_DTC_INFORMATION:
            if ((len == 3) || (len == 6)) {
                return 1;
            } else {
                return 0;
            }
        break;
        case SID_READ_DATA_BY_IDENTIFIER:
            if ((len >= 3) && (len % 2)) {
                return 1;
            } else {
                return 0;
            }
        break;
        case SID_WRITE_DATA_BY_IDENTIFIER:
            if (len > 3) {
                return 1;
            } else {
                return 0;
            }
        break;
        case SID_INPUT_OUTPUT_CONTROL_BY_IDENTIFIER:
            if (len == 4) {
                return 1;
            } else {
                return 0;
            }
        break;
        case SID_ROUTINE_CONTROL:
            if (len >= 4) {
                return 1;
            } else {
                return 0;
            }
        break;
        case SID_REQUEST_DOWNLOAD:
            if ((len == 9) || (len == 11)) {
                return 1;
            } else {
                return 0;
            }
        break;
        case SID_TRANSFER_DATA:
            if ((len > 2) && (len <= 258)) {
                return 1;
            } else {
                return 0;
            }
        break;
        case SID_REQUEST_TRANSFER_EXIT:
            if (len == 1) {
                return 1;
            } else {
                return 0;
            }
        break;
    }
    return 0;
}
//General end

//0x10
uint8_t UDS_CheckSessionLevel(UDS_SessionMaskType sessionMask)
{
    uint8_t isOk = FALSE;
    if ((sessionMask & (GetCurSession())) || (sessionMask == UdsAllSession)) {
        isOk = TRUE;
    }
    return isOk;
}

uint8_t UDS_IsDefaultSession(UDS_SessionMaskType sessionMask)
{
    if (sessionMask == UdsDefaultSession) {
        return 1;
    }
    return 0;
}

uint8_t UDS_IsProgramSession(UDS_SessionMaskType sessionMask)
{
    if (sessionMask == UdsProgramSession) {
        return 1;
    }
    return 0;
}

uint8_t UDS_IsExtendSession(UDS_SessionMaskType sessionMask)
{
    if (sessionMask == UdsExtendedSession) {
        return 1;
    }
    return 0;
}

uint8_t UDS_IsNewSessionValid(UDS_SessionType Session)
{
    uint8_t isValid = FALSE;
    if ((Session == 1) || (Session == 2) || (Session == 3) || (Session == 5) || (Session == 0x10) ||
        (Session == 0x81) || (Session == 0x85) || (Session == 0x90) || (Session == 0xd0) || (Session == 0xd1))
    {
        isValid = TRUE;
    }
    return isValid;
}

UDS_SessionType UDS_SessionMap(UDS_SessionType Session)
{
    UDS_SessionType newS;
    if ((Session == 1) || (Session == 0x81) || (Session == 0xd0) || (Session == 0xd1)) {
        newS = UdsDefaultSession;
    }
    if ((Session == 2) || (Session == 5) || (Session == 0x85)) {
        newS = UdsProgramSession;
    }
    if ((Session == 3) || (Session == 0x10) || (Session == 0x90)) {
        newS = UdsExtendedSession;
    }
    return newS;
}

void UDS_ChangeDefaultSession(void)
{
    UdsParameter.CommNormalMessageDisableTx = 0;
    UdsParameter.CommNormalMessageDisableRx = 0;
    UdsParameter.CommNMMessageDisableTx = 0;
    UdsParameter.CommNMMessageDisableRx = 0;
    UdsParameter.CommType = COMM_TYPE_EnRxEnTx_NormalAndNetworkMsg;
    UdsParameter.DtcSettingType = DTC_SETTING_TYPE_ON;
    UdsParameter.IOControlByIdentAllOn = 0;
    UdsParameter.IOControlByIdentAllOff = 0;
}

uint16_t UDS_SetResp_0x10(uint8_t *ReqData, uint16_t ReqLen, uint8_t *RespData, uint16_t RespLen)
{
    if (RespLen < 1) {
        return 0x8014;
    }
    if (RespData[0] == 2) {
        if ((CurIgnSt != EVENT_IGN_ON) || (InputOutputSingal.RPM)) {
            return 0x8022;
        }
    }
    RespData[0] = ReqData[0];
    //RespData[1] = 0x00;
    //RespData[2] = 0x32;
    //RespData[3] = 0x13;
    //RespData[4] = 0x88;
    return 1;
}
//0x10 end

//0x20
uint16_t UDS_SetResp_0x20(uint8_t *ReqData, uint16_t ReqLen, uint8_t *RespData, uint16_t RespLen)
{
    return 0;
}
//0x20 end

//0x11
uint8_t UDS_isResetValid(uint8_t data)
{
    if ((data == 1) || (data == 2) || (data == 3)) {
        return 1;
    }
    return 0;
}

uint16_t UDS_SetResp_0x11(uint8_t *ReqData, uint16_t ReqLen, uint8_t *RespData, uint16_t RespLen)
{
    if (RespLen < 2) {
        return 0x8014;
    }
    if ((CurIgnSt != EVENT_IGN_ON) || (InputOutputSingal.RPM)) {
        return 0x8022;
    }
    RespData[0] = ReqData[0];
    RespData[1] = 5;
    return 2;
}
//0x11 end

//0x28
void UDS_SetNormalEnTxEnRx(void)
{
    UdsParameter.CommType = COMM_TYPE_EnRxEnTx_NormalMsg;
    UdsParameter.CommNormalMessageDisableTx = 0;
    UdsParameter.CommNormalMessageDisableRx = 0;
}

void UDS_SetNMEnTxEnRx(void)
{
    UdsParameter.CommType = COMM_TYPE_EnRxEnTx_NetworkMsg;
    UdsParameter.CommNMMessageDisableTx = 0;
    UdsParameter.CommNMMessageDisableRx = 0;
}

void UDS_SetNMAndNormalEnTxEnRx(void)
{
    UdsParameter.CommType = COMM_TYPE_EnRxEnTx_NormalAndNetworkMsg;
    UdsParameter.CommNormalMessageDisableTx = 0;
    UdsParameter.CommNormalMessageDisableRx = 0;
    UdsParameter.CommNMMessageDisableTx = 0;
    UdsParameter.CommNMMessageDisableRx = 0;
}

void UDS_SetNormalDisTxDisRx(void)
{
    UdsParameter.CommType = COMM_TYPE_DisRxDisTx_NormalMsg;
    UdsParameter.CommNormalMessageDisableTx = 1;
    UdsParameter.CommNormalMessageDisableRx = 1;
}

void UDS_SetNMDisTxDisRx(void)
{
    UdsParameter.CommType = COMM_TYPE_DisRxDisTx_NetworkMsg;
    UdsParameter.CommNMMessageDisableTx = 1;
    UdsParameter.CommNMMessageDisableRx = 1;
}

void UDS_SetNMAndNormalDisTxDisRx(void)
{
    UdsParameter.CommType = COMM_TYPE_DisRxDisTx_NormalAndNetworkMsg;
    UdsParameter.CommNormalMessageDisableTx = 1;
    UdsParameter.CommNormalMessageDisableRx = 1;
    UdsParameter.CommNMMessageDisableTx = 1;
    UdsParameter.CommNMMessageDisableRx = 1;
}

void UDS_ResetCommParameter(void)
{
    UdsParameter.CommType = COMM_TYPE_EnRxEnTx_NormalAndNetworkMsg;
    UdsParameter.CommNormalMessageDisableTx = 0;
    UdsParameter.CommNormalMessageDisableRx = 0;
    UdsParameter.CommNMMessageDisableTx = 0;
    UdsParameter.CommNMMessageDisableRx = 0;
}

uint16_t UDS_SetResp_0x28(uint8_t *ReqData, uint16_t ReqLen, uint8_t *RespData, uint16_t RespLen)
{
    switch (ReqData[0]) {
        case 0x00: //enableRxAndTx
            UDS_SetNMAndNormalEnTxEnRx();
            //if ((ReqData[1] & 0xf) == 1) {     //NormalCommnuicationMessages
            //    UDS_SetNormalEnTxEnRx();
            //} else if ((ReqData[1] & 0xf) == 2) {        //NetworkManagementCommunicationMessages
            //    UDS_SetNMEnTxEnRx();
            //} else if ((ReqData[1] & 0xf) == 3) {        //NetworkManagementCommnunicationMessage and NormalCommunicationMessages
            //    UDS_SetNMAndNormalEnTxEnRx();
            //} else {
            //    return 0x8022;
            //}
        break;
        case 0x01: //diableNormalMessageTransmission (responseRequired)
        case 0x02: //diableNormalMessageTransmission (noResponseRequired)
        case 0x03: //disableRxAndTx
            UDS_SetNMAndNormalDisTxDisRx();
            //if ((ReqData[1] & 0xf) == 1) {     //NormalCommnuicationMessages
            //    UDS_SetNormalDisTxDisRx();
            //} else if ((ReqData[1] & 0xf) == 2) {        //NetworkManagementCommunicationMessages
            //    UDS_SetNMDisTxDisRx();
            //} else if ((ReqData[1] & 0xf) == 3) {        //NetworkManagementCommnunicationMessage and NormalCommunicationMessages
            //    UDS_SetNMAndNormalDisTxDisRx();
            //} else {
            //    return 0x8022;
            //}
        break;
        default:
            return 0x8012;
        break;
    }
    
    RespData[0] = ReqData[0];
    return 1;
}
//0x28 end

//0x29
uint8_t UDS_isEnableNormalMsgValid(uint8_t data)
{
    if ((data == 1) || (data == 2)) {
        return 1;
    }
    return 0;
}
	
uint16_t UDS_SetResp_0x29(uint8_t *ReqData, uint16_t ReqLen, uint8_t *RespData, uint16_t RespLen)
{
    UDS_SetNMAndNormalEnTxEnRx();
    return 0;
}
//0x29 end

//0x85
void UDS_SetDtcOn(void)
{
    UdsParameter.DtcSettingType = DTC_SETTING_TYPE_ON;
    ResetAllCanFrameTime();
}

void UDS_SetDtcOff(void)
{
    UdsParameter.DtcSettingType = DTC_SETTING_TYPE_OFF;
}

uint16_t UDS_SetResp_0x85(uint8_t *ReqData, uint16_t ReqLen, uint8_t *RespData, uint16_t RespLen)
{
    if ((ReqData[0] == 1) || (ReqData[0] == 2)) {
        if (ReqData[0] == 1) {
            UDS_SetDtcOn();
        } else {
            UDS_SetDtcOff();
        }
        RespData[0] = ReqData[0];
        return 1;
    } else {
        return 0x8012;
    }
}
//0x85 end

//0x27
//0x27
void UDS_Service27LimitTimerDecrease(uint8_t n)
{
    if (UdsParameter.Service27LimitTimer > n) {
        UdsParameter.Service27LimitTimer -= n;
    } else if (UdsParameter.Service27LimitTimer) {
        UdsParameter.Service27ErrorCnts = 0;
        UdsParameter.Service27LimitTimer = 0;
    }
}

uint8_t UDS_CheckSecurityLevel(UDS_SecurityLevelMaskType securityLevelMask)
{
    uint8_t isOk = FALSE;
    if (UdsUnSecurityLevel == securityLevelMask) {
        isOk = TRUE;
    } else if (securityLevelMask == GetCurSecyritySt()) {
        isOk = TRUE;
    }
    return isOk;
}

uint32_t UDS_CalcKey(uint8_t *seed)
{
    uint32_t tmp;
    tmp = (seed[0] << 24) | (seed[1] << 16) | (seed[2] << 8) | seed[3];
    tmp = ~tmp;
    tmp += 0xd;
    return tmp;
}

uint8_t UDS_PrepareSeed(uint8_t *seed, uint8_t GetSeedType)
{
    int32_t tmp;
    srand(Timer1msCnts);
    tmp = rand();
    seed[0] = (tmp >> 24) & 0xff;
    seed[1] = (tmp >> 16) & 0xff;
    seed[2] = (tmp >> 8) & 0xff;
    seed[3] = (tmp >> 0) & 0xff;
    
    UdsParameter.Seed = *((uint32_t*)seed);
    UdsParameter.Key = UDS_CalcKey((uint8_t*)&UdsParameter.Seed);
    return 4;
}

uint8_t UDS_CompareKey(uint8_t *key, uint8_t length)
{
    if ((4 == length) && (key[0] == ((uint8_t*)&UdsParameter.Key)[3]) && (key[1] == ((uint8_t*)&UdsParameter.Key)[2]) && (key[2] == ((uint8_t*)&UdsParameter.Key)[1]) && (key[3] == ((uint8_t*)&UdsParameter.Key)[0])) {
        return TRUE;
    }
    return FALSE;
}

uint8_t UDS_CheckRequestSeedValid(uint8_t SubFun)
{
    if ((0x01u == SubFun) || (0x11u == SubFun)) {  // Request Seed
        return 1;
    }
    return 0;
}

uint8_t UDS_CheckRequestSeedLength(uint8_t len)
{
	if (0x01u == len) {
        return 1;
    }
    return 0;
}

uint8_t UDS_CheckSendKeyValid(uint8_t SubFun)
{
    if ((0x02u == SubFun) || (0x12u == SubFun) || (0x42u == SubFun)) {
        return 1;
    }
    return 0;
}
	
UDS_SecurityLevelType udsSecurityLevelRequested = 0u;
uint8_t udsIsSeedRequested = FALSE;
uint16_t UDS_SetResp_0x27(uint8_t *ReqData, uint16_t ReqLen, uint8_t *RespData, uint16_t RespLen)
{
    uint8_t retRequestSeed = 0;
    uint8_t retSendKey = 0;
    uint8_t subFnc = ReqData[0];
    //check error cnt
    //check error cnt end
    retRequestSeed = UDS_CheckRequestSeedValid(subFnc);
    retSendKey = UDS_CheckSendKeyValid(subFnc);
    if (retRequestSeed) {  // Request Seed
        if (UDS_CheckRequestSeedLength(ReqLen)) {
            if (UdsParameter.Service27LimitTimer == 0) {
                udsSecurityLevelRequested = subFnc;
                if (UDS_CheckSecurityLevel(udsSecurityLevelRequested)) {
                    RespData[0] = subFnc;
                    RespData[1] = 0;
                    RespData[2] = 0;
                    RespData[3] = 0;
                    RespData[4] = 0;
                    return 1 + 4 /* eg(1:subfnc(1),4:seed */;
                } else {
                    udsIsSeedRequested = TRUE;
                    RespData[0] = subFnc;
                    return 1 + UDS_PrepareSeed(&RespData[1], subFnc);
                }
            } else {
                return 0x8037;
            }
        } else {
            return 0x8013;
        }
    } else if (retSendKey) {     // Send key
        if (TRUE == udsIsSeedRequested) {
            if (TRUE == UDS_CompareKey(&ReqData[1], ReqLen - 1)) {
                UDS_SetUdsSecurityLevel(udsSecurityLevelRequested);
                udsIsSeedRequested = FALSE;
                RespData[0] = subFnc;
                UdsParameter.Service27ErrorCnts = 0;
                return 1;
            } else {
                if (UdsParameter.Service27ErrorCnts == 0) {
                    UdsParameter.Service27ErrorCnts++;
                    udsIsSeedRequested = FALSE;
                    return 0x8035;
                } else if (UdsParameter.Service27ErrorCnts == 1) {
                    udsIsSeedRequested = FALSE;
                    UdsParameter.Service27LimitTimer = 10000;
                    return 0x8036;
                }
                return 0x8035;
            }
        } else {
            return 0x8024;
        }
    } else {
        return 0x8012;
    }
}
//0x27 end

//0x22
//BootSoftwareIdentificationDataIdentifier 
uint16_t UdsRDID_F180(uint8_t *Data, uint16_t length)
{
    return 0;
}

//ApplicationSoftwareIdentificationDataIdentifier
uint16_t UdsRDID_F181(uint8_t *Data, uint16_t length)
{
    return 0;
}

//ApplicationDataIdentificationDataIdentifier 
uint16_t UdsRDID_F182(uint8_t *Data, uint16_t length)
{
    return 0;
}

//BootSoftwareFingerprintDataIdentifier 
uint16_t UdsRDID_F183(uint8_t *Data, uint16_t length)
{
    return 0;
}

//ApplicationSoftwareFingerprintDataIdentifier
uint16_t UdsRDID_F184(uint8_t *Data, uint16_t length)
{
    return 0;
}

//ApplicationDataFingerprintDataIdentifier 
uint16_t UdsRDID_F185(uint8_t *Data, uint16_t length)
{
    return 0;
}

//ActiveDiagnosticSessionDataIdentifier 
uint16_t UdsRDID_F186(uint8_t *Data, uint16_t length)
{
    return 0;
    /*
    if (length < 1) { 
        return 0;
    }
    
    if (GetCurSession() == UdsDefaultSession) {
        Data[0] = 1;
    } else if (GetCurSession() == UdsProgramSession) {
        Data[0] = 2;
    } else if (GetCurSession() == UdsExtendedSession) {
        Data[0] = 3;
    } else {
        return 0x8012;
    }
    
    return 1;
    */
}

//VehicleManufacturerSparePartNumberDataIdentifier 
uint16_t UdsRDID_F187(uint8_t *Data, uint16_t length)
{
    if (length < 11) {
        return 0;
    }
    
    Data[0] = '9';
    Data[1] = '4';
    Data[2] = '0';
    Data[3] = '1';
    Data[4] = '3';    
    Data[5] = 'L';     
    Data[6] = '4';    
    Data[7] = '0';    
    Data[8] = '1';    
    Data[9] = '0';
    
    return 10;
}

//VehicleManufacturerECUSoftwareNumberDataIdentifier 
uint16_t UdsRDID_F188(uint8_t *Data, uint16_t length)
{
    return 0;
}

//VehicleManufacturerECUSoftwareVersionNumberDataIdentifier
uint16_t UdsRDID_F189(uint8_t *Data, uint16_t length)
{
    return 0;
}

//SystemSupplierIdentifierDataIdentifier
uint16_t UdsRDID_F18A(uint8_t *Data, uint16_t length)
{
    return 0;
}

//ECUManufacturingDateDataIdentifier 
uint16_t UdsRDID_F18B(uint8_t *Data, uint16_t length)
{
    if (length < 4) {
        return 0;
    }
    Data[0] = 0x20;
    Data[1] = 0x19;
    Data[2] = 0x05;
    Data[3] = 0x29;
    return 4;
}

//ECUSerialNumberDataIdentifier 
uint16_t UdsRDID_F18C(uint8_t *Data, uint16_t length)
{
    return 0;
}

//SupportedFunctionalUnitsDataIdentifier 
uint16_t UdsRDID_F18D(uint8_t *Data, uint16_t length)
{
    return 0;
}

//VehicleManufacturerKitAssemblyPartNumberDataIdentifier
uint16_t UdsRDID_F18E(uint8_t *Data, uint16_t length)
{
    return 0;
}

//VINDataIdentifier 
uint8_t F190TestBuf[17] = {'H', 'L', 'J', 'T', 'Y', 'W', '_', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9'};
uint16_t UdsRDID_F190(uint8_t *Data, uint16_t length)
{
    return 0;
    /*
    if (length < 17) {
        return 0; 
    }
    memcpy(Data, F190TestBuf, 17);
    return 17;
    */
}

//VehicleManufacturerECUHardwareNumberDataIdentifier 
uint16_t UdsRDID_F191(uint8_t *Data, uint16_t length)
{
    return 0;
}

//SystemSupplierECUHardwareNumberDataIdentifier 
uint16_t UdsRDID_F192(uint8_t *Data, uint16_t length)
{
    return 0;
}

//SystemSupplierECUHardwareVersionNumberDataIdentifier
uint16_t UdsRDID_F193(uint8_t *Data, uint16_t length)
{
    if (length < strlen(HARDWARE_VERSION)) {
        return 0; 
    }
    memcpy(Data, HARDWARE_VERSION, strlen(HARDWARE_VERSION));
    return strlen(HARDWARE_VERSION);
}


//SystemSupplierECUSoftwareNumberDataIdentifier 
uint16_t UdsRDID_F194(uint8_t *Data, uint16_t length)
{
    return 0;
}

//SystemSupplierECUSoftwareVersionNumberDataIdentifier
uint16_t UdsRDID_F195(uint8_t *Data, uint16_t length)
{
    if (length < strlen(SOFTWARE_VERSION)) { 
        return 0; 
    }
    memcpy(Data, SOFTWARE_VERSION, strlen(SOFTWARE_VERSION));
    return strlen(SOFTWARE_VERSION);
}

//ExhaustRegulationOrTypeApprovalNumberDataIdentifier 
uint16_t UdsRDID_F196(uint8_t *Data, uint16_t length)
{
    return 0;
}

//SystemNameOrEngineTypeDataIdentifier 
uint16_t UdsRDID_F197(uint8_t *Data, uint16_t length)
{
    return 0;
}

//RepairShopCodeOrTesterSerialNumberDataIdentifier 
uint16_t UdsRDID_F198(uint8_t *Data, uint16_t length)
{
    return 0;
}

//ProgrammingDateDataIdentifier
uint16_t UdsRDID_F199(uint8_t *Data, uint16_t length)
{
    return 0;
}

//CalibrationRepairShopCodeOrCalibrationEquipmentSerialNumberDataIdentifier 
uint16_t UdsRDID_F19A(uint8_t *Data, uint16_t length)
{
    return 0;
}

//CalibrationDateDataIdentifier 
uint16_t UdsRDID_F19B(uint8_t *Data, uint16_t length)
{
    return 0;
}

//CalibrationEquipmentSoftwareNumberDataIdentifier 
uint16_t UdsRDID_F19C(uint8_t *Data, uint16_t length)
{
    return 0;
}

//ECUInstallationDateDataIdentifier 
uint16_t UdsRDID_F19D(uint8_t *Data, uint16_t length)
{
    return 0;
}

//ODXFileDataIdentifier 
uint16_t UdsRDID_F19E(uint8_t *Data, uint16_t length)
{
    return 0;
}

//EntityDataIdentifier 
uint16_t UdsRDID_F19F(uint8_t *Data, uint16_t length)
{
    return 0;
}

//SoftwareVersionforHKMCVehicleManufactureDataIdentifier
uint16_t UdsRDID_F1A0(uint8_t *Data, uint16_t length)
{
    return 0;
}

//MM-CAN DB VERSION
uint16_t UdsRDID_F100(uint8_t *Data, uint16_t length)
{
    if (length < 3) { 
        return 0; 
    }
    Data[0] = '8';      //2018.08 first
    Data[1] = '7';
    Data[2] = '1';
    return 3;
}

uint16_t UdsRDID_B001(uint8_t *Data, uint16_t length)
{
    if (length < 5) { 
        return 0; 
    }
    Data[0] = 0;
    Data[1] = 0;
    Data[2] = 0;
    Data[3] = 0;
    Data[4] = 0;
    return 5;
}

uint16_t UdsRDID_B002(uint8_t *Data, uint16_t length)
{
    uint32_t tmp = 0;
    if (length < 12) {
        return 0;
    }
    Data[0] = 0xe0;     //Supported PID: 0x01 ~ 0x20
    Data[1] = 0;
    Data[2] = 0;
    Data[3] = 0;    
    
    tmp = GetFuelCapacityVal();
    Data[4] = tmp / 1000 * 2;
    if ((GetFuelSensorState() == FuelSensorShortCircuit) || (GetFuelSensorState() == FuelSensorOpenCircuit))  {
        Data[4] = 0xff;
    }
    if (CurIgnSt != EVENT_IGN_ON)  {
        Data[4] = 0xff;
    }
    
    tmp = CurBatteryVoltage;
    Data[5] = tmp / 8;
    
    tmp = TripCPara.TotalODO / 1000;
    Data[6] = (tmp >> 16) & 0xff;
    Data[7] = (tmp >> 8) & 0xff;
    Data[8] = tmp & 0xff;
    
    Data[9] = 0;
    Data[10] = 0;
    Data[11] = 0;
    return 12;
}

uint16_t UdsRDID_B003(uint8_t *Data, uint16_t length)
{
    if (length < 5) {
        return 0; 
    }
    Data[0] = 0x98;
    Data[1] = 0;
    Data[2] = 0;
    Data[3] = 0;
    
    if (CurIgnSt == EVENT_IGN_ON) {
        Data[4] = 1;
    } else {
        Data[4] = 0;
    }
    if (Can0DataStruct.ENG_STAT == 3) {
        Data[4] |= 0x8;
    }
    return 5;
}

uint16_t UdsRDID_0060(uint8_t *Data, uint16_t length)
{
    if (length < 2) { 
        return 0; 
    }
    memcpy(Data, (uint8_t*)&EolStruct, sizeof (EolStruct));
    return 2;
}

uint16_t UdsRDID_0070(uint8_t *Data, uint16_t length)
{
    if (length < 1) { 
        return 0; 
    }
    Data[0] = TripCPara.ServiceType;
    return 1;
}

uint16_t UdsRDID_0072(uint8_t *Data, uint16_t length)
{
    if (length < 4) { 
        return 0; 
    }
    Data[0] = (TripCPara.ServiceReminderKm >> 16) & 0xff;
    Data[1] = (TripCPara.ServiceReminderKm >> 8) & 0xff;
    Data[2] = TripCPara.ServiceReminderKm & 0xff;
    Data[3] = 0;
    return 4;
}

uint16_t UdsRDID_0073(uint8_t *Data, uint16_t length)
{
    if (length < 2) { 
        return 0; 
    }
    Data[0] = ((TripCPara.ServiceReminderSeconds / 3600 / 24 / 30) >> 8) & 0xff;
    Data[1] = (TripCPara.ServiceReminderSeconds / 3600 / 24 / 30) & 0xff;
    return 2;
}
//0x22 end

//0x2e
uint16_t UdsWDID_F190(uint8_t *Data, uint16_t length)
{
    return 0x8031;
    /*
    if (length == 17) {
        memcpy(F190TestBuf, Data, 17);
        return 1;
    } else {
        return 0x8013;
    }
    return 0;
    */
}

EOL_STRUCT TmpEolStruct;
uint8_t TestEolSCCResult = 0xff;
uint8_t TestEolEPBResult = 0xff;
uint8_t TestEolMDPSResult = 0xff;
uint8_t TestEolESCResult = 0xff;
uint8_t TestEolTPMSResult = 0xff;
uint8_t TestEolLKA_LDWResult = 0xff;
uint8_t TestEolAIR_BAGResult = 0xff;
uint8_t TestEolATResult = 0xff;
uint8_t UdsEolStart = 0;
uint16_t UdsWDID_0060(uint8_t *Data, uint16_t length)
{
    if ((CurIgnSt != EVENT_IGN_ON) || (InputOutputSingal.RPM)) {
        return 0x8022;
    }
    if (length == 2) {
        if (UdsEolStart == 0) {
            UdsEolStart = 1;
            UDSServiceTimerCnts = 0xffff;
        }
    }
    if (UDSServiceTimerCnts == 0xffff) {
        UDSServiceTimerCnts = 2000;
        ResetAllCanFrameTime();
        return 0x8078;
    } else if (UDSServiceTimerCnts) {
        //Self configuration ing...
        return 0x8078;
    } else if (UDSServiceTimerCnts == 0) {
        UDSServiceTimerCnts = 0xffff;
        UdsEolStart = 0;
        memcpy(&TmpEolStruct, Data, length);
        if (memcmp(&EolStruct, &TmpEolStruct, sizeof (TmpEolStruct)) != 0) {
            memcpy(&EolStruct, &TmpEolStruct, sizeof (TmpEolStruct));
            //SaveMiscValid = 1;
        }
        if ((EolStruct._SCC == 0) && (GetCanFrameState(0x420) != CAN_FRAME_STATE_TIME_OUT)) {
            TestEolSCCResult = 1;
        } else if ((EolStruct._SCC == 1) && (GetCanFrameState(0x420) == CAN_FRAME_STATE_TIME_OUT)) {
            TestEolSCCResult = 2;
        } else {
            TestEolSCCResult = 3;
        }
        
        if ((EolStruct.EPB == 0) && (GetCanFrameState(0x490) != CAN_FRAME_STATE_TIME_OUT)) {
            TestEolEPBResult = 1;
        } else if ((EolStruct.EPB == 1) && (GetCanFrameState(0x490) == CAN_FRAME_STATE_TIME_OUT)) {
            TestEolEPBResult = 2;
        } else {
            TestEolEPBResult = 3;
        }
        
        if ((EolStruct.MDPS == 0) && (GetCanFrameState(0x381) != CAN_FRAME_STATE_TIME_OUT)) {
            TestEolMDPSResult = 1;
        } else if ((EolStruct.MDPS == 1) && (GetCanFrameState(0x381) == CAN_FRAME_STATE_TIME_OUT)) {
            TestEolMDPSResult = 2;
        } else {
            TestEolMDPSResult = 3;
        }
        
        if ((EolStruct.ESC == 0) && (GetCanFrameState(0x507) != CAN_FRAME_STATE_TIME_OUT)) {
            TestEolESCResult = 1;
        } else if ((EolStruct.ESC == 1) && (GetCanFrameState(0x507) == CAN_FRAME_STATE_TIME_OUT)) {
            TestEolESCResult = 2;
        } else {
            TestEolESCResult = 3;
        }
        
        if ((EolStruct.TPMS == 0) && (GetCanFrameState(0x593) != CAN_FRAME_STATE_TIME_OUT)) {
            TestEolTPMSResult = 1;
        } else if ((EolStruct.TPMS == 1) && (GetCanFrameState(0x593) == CAN_FRAME_STATE_TIME_OUT)) {
            TestEolTPMSResult = 2;
        } else {
            TestEolTPMSResult = 3;
        }
        
        if ((EolStruct.LKA_LDW == 0) && (GetCanFrameState(0x340) != CAN_FRAME_STATE_TIME_OUT)) {
            TestEolLKA_LDWResult = 1;
        } else if ((EolStruct.LKA_LDW == 1) && (GetCanFrameState(0x340) == CAN_FRAME_STATE_TIME_OUT)) {
            TestEolLKA_LDWResult = 2;
        } else {
            TestEolLKA_LDWResult = 3;
        }
        
        if ((EolStruct.AIR_BAG == 0) && (GetCanFrameState(0x500) != CAN_FRAME_STATE_TIME_OUT)) {
            TestEolAIR_BAGResult = 1;
        } else if ((EolStruct.AIR_BAG == 1) && (GetCanFrameState(0x500) == CAN_FRAME_STATE_TIME_OUT)) {
            TestEolAIR_BAGResult = 2;
        } else {
            TestEolAIR_BAGResult = 3;
        }
        
        if ((EolStruct._AT == 0) && (GetCanFrameState(0x368) != CAN_FRAME_STATE_TIME_OUT)) {
            TestEolATResult = 1;
        } else if ((EolStruct._AT == 1) && (GetCanFrameState(0x368) == CAN_FRAME_STATE_TIME_OUT)) {
            TestEolATResult = 2;
        } else {
            TestEolATResult = 3;
        }

        if ((TestEolSCCResult == 3) && (TestEolEPBResult == 3) && (TestEolMDPSResult == 3) && (TestEolESCResult == 3) && (TestEolTPMSResult == 3) && (TestEolLKA_LDWResult == 3) && (TestEolAIR_BAGResult == 3) && (TestEolATResult == 3)) {
            EolResult = EOL_OK;
            if (EolCounts < 0xff) {
                EolCounts++;
            }
            SaveMiscValid = 1;
        } else {
            EolResult = EOL_ERROR;
            if ((EolCounts) && (EolCounts < 0xff)) {
                EolCounts++;
            }
            SaveMiscValid = 1;
        }
        return 1;
    }
    return 1;
}

uint16_t UdsWDID_0070(uint8_t *Data, uint16_t length)
{
    if (1 != length) {
        return 0;
    }
    if ((CurIgnSt != EVENT_IGN_ON) || (InputOutputSingal.RPM)) {
        return 0x8022;
    }
    TripCPara.ServiceType = Data[0];
    SaveServiceRemainderValid = 2;
    return 1;
}

uint16_t UdsWDID_0071(uint8_t *Data, uint16_t length)
{
    if ((CurIgnSt != EVENT_IGN_ON) || (InputOutputSingal.RPM)) {
        return 0x8022;
    }
    if (TripCPara.ServiceReminderValid & SERVICE_REMAINDER_VALID_KM) {
        TripCPara.ServiceReminderKm = TripCPara.SetServiceReminderKm;
        SaveServiceRemainderValid = 2;
    }
    if (TripCPara.ServiceReminderValid & SERVICE_REMAINDER_VALID_TIME) {
        TripCPara.ServiceReminderSeconds = TripCPara.SetServiceReminderSeconds;
        SaveServiceRemainderValid = 2;
    }
    return 1;
}

uint16_t UdsWDID_0072(uint8_t *Data, uint16_t length)
{
    uint32_t tmp = 0;
    if (3 != length) {
        return 0;
    }
    if ((CurIgnSt != EVENT_IGN_ON) || (InputOutputSingal.RPM)) {
        return 0x8022;
    }
    tmp = (Data[0] << 16) | (Data[1] << 8) | (Data[2]);
    if (tmp > 99999) {
        return 0;
    }
    if (tmp == 0) {
        TripCPara.ServiceReminderValid &= ~0x1;
        TripCPara.ServiceReminderKm = 0;
        TripCPara.SetServiceReminderKm = 0;
    } else {
        TripCPara.ServiceReminderValid |= 0x1;
        TripCPara.ServiceReminderKm = tmp;
        TripCPara.SetServiceReminderKm = tmp;
    }
    SaveServiceRemainderValid = 2;
    return 1;
}

uint16_t UdsWDID_0073(uint8_t *Data, uint16_t length)
{
    uint16_t tmp = 0;
    if (2 != length) {
        return 0;
    }
    if ((CurIgnSt != EVENT_IGN_ON) || (InputOutputSingal.RPM)) {
        return 0x8022;
    }
    if (tmp > 99) {
        return 0;
    }
    tmp = (Data[0] << 8) | (Data[1]);
    if (tmp == 0) {
        TripCPara.ServiceReminderValid &= ~0x2;
        TripCPara.ServiceReminderSeconds = 0;
        TripCPara.SetServiceReminderSeconds = 0;
    } else {
        TripCPara.ServiceReminderValid |= 0x2;
        TripCPara.ServiceReminderSeconds = tmp * 30 * 24 * 3600;
        TripCPara.ServiceReminderSeconds = TripCPara.ServiceReminderSeconds;
    }
    SaveServiceRemainderValid = 2;
    return 1;
}
//0x2e end

//0x2f
uint16_t UdsIOCtlDID_B000(uint8_t *RequestData, uint16_t length, uint8_t *RespData, uint16_t RespLen)
{
    if (1u != length) {
        return 0x8014;
    }
    if (RequestData[0] == 2) {
        if (RespLen < 5) {
            return 0;
        }
        RespData[0] = 02;
        RespData[1] = 0xc0;
        RespData[2] = 0;
        RespData[3] = 0;
        RespData[4] = 0;
        return 5;
    } else {
        return 0x8031;
    }
}

uint16_t UdsIOCtlDID_B001(uint8_t *RequestData, uint16_t length, uint8_t *RespData, uint16_t RespLen)
{
    if (1u != length) {
        return 0x8014;
    }
    if (RequestData[0] == 0x3) {
        UdsParameter.IOControlByIdentAllOn = 1;
        UdsParameter.IOControlByIdentAllOff = 0;
    } else if (RequestData[0] == 0x0) {
        UdsParameter.IOControlByIdentAllOn = 0;
        UdsParameter.IOControlByIdentAllOff = 0;
    } else {
        return 0x8031;
    }
    return 1;
}

uint16_t UdsIOCtlDID_B002(uint8_t *RequestData, uint16_t length, uint8_t *RespData, uint16_t RespLen)
{
    if (1u != length) {
        return 0;
    }
    if (RequestData[0] == 0x3) {
        UdsParameter.IOControlByIdentAllOn = 0;
        UdsParameter.IOControlByIdentAllOff = 1;
    } else if (RequestData[0] == 0x0) {
        UdsParameter.IOControlByIdentAllOn = 0;
        UdsParameter.IOControlByIdentAllOff = 0;
    } else {
        return 0x8031;
    }
    return 1;
}
//0x2f

//0x31
uint16_t UdsStartRC_FF00(uint8_t *param,uint16_t length,uint8_t *status)
{
    if ((CurIgnSt != EVENT_IGN_ON) || (InputOutputSingal.RPM)) {
        return 0x8022;
    }
    return 1;
}

uint16_t UdsStopRC_FF00(uint8_t *param,uint16_t length,uint8_t *status)
{
    if ((CurIgnSt != EVENT_IGN_ON) || (InputOutputSingal.RPM)) {
        return 0x8022;
    }
    return 1;
}
uint16_t UdsRequestResultRC_FF00(uint8_t *param,uint16_t length,uint8_t *result)
{
    if ((CurIgnSt != EVENT_IGN_ON) || (InputOutputSingal.RPM)) {
        return 0x8022;
    }
    return 1;
}

uint16_t UdsStartRC_FF01(uint8_t *param,uint16_t length,uint8_t *status)
{
    if ((CurIgnSt != EVENT_IGN_ON) || (InputOutputSingal.RPM)) {
        return 0x8022;
    }
    return 1;
}

uint16_t UdsStopRC_FF01(uint8_t *param,uint16_t length,uint8_t *status)
{
    if ((CurIgnSt != EVENT_IGN_ON) || (InputOutputSingal.RPM)) {
        return 0x8022;
    }
    return 1;
}
uint16_t UdsRequestResultRC_FF01(uint8_t *param,uint16_t length,uint8_t *result)
{
    if ((CurIgnSt != EVENT_IGN_ON) || (InputOutputSingal.RPM)) {
        return 0x8022;
    }
    return 1;
}
//0x31 end

//0x14
#define DTC_RET_OK 0
#define DTC_RET_OUT_OF_RANGE 1
#define DTC_RET_SUBFUNCTION_NOT_SUPPORTED 0xff
#define DTC_RET_OF_RANGE 0xfe
#define DTC_RET_TOO_LONG 0xfd
uint8_t ClearDtc(uint32_t DtcNum) 
{
    uint8_t ret = DTC_RET_OK;
    uint8_t i;
    _19_6ReqRecord D19_6ReqRecord;
    if (DtcNum != 0xffffff) {
        for (i = 0; i < DtcListTotal; i++) {
            if ((D19_6ReqRecord.DtcNum == DiagDtc[i].DtcNum.Num) && (dtc_map_info[i].Exist)) {
                App_DtcInfoInit((_DTC*)&DiagDtc[i], dtc_map_info[i].DtcNum);
                SaveDtcInfo.EE_WriteDtcSel = StoreDTC_EE;
                break;
            }
        }
        ResetAllCanFrameTime();
        TestEolSCCResult = 0xff;
        TestEolEPBResult = 0xff;
        TestEolMDPSResult = 0xff;
        TestEolESCResult = 0xff;
        TestEolTPMSResult = 0xff;
        TestEolLKA_LDWResult = 0xff;
        TestEolAIR_BAGResult = 0xff;
        TestEolATResult = 0xff;
        if (i == DtcListTotal) {
            ret = DTC_RET_OUT_OF_RANGE;
        }
    } else {
        for (i = 0; i < DtcListTotal; i++) {
            App_DtcInfoInit((_DTC*)&DiagDtc[i], dtc_map_info[i].DtcNum);
        }
        SaveDtcInfo.EE_WriteDtcSel = ClearDTC_EE;
        ResetAllCanFrameTime();
        
        TestEolSCCResult = 0xff;
        TestEolEPBResult = 0xff;
        TestEolMDPSResult = 0xff;
        TestEolESCResult = 0xff;
        TestEolTPMSResult = 0xff;
        TestEolLKA_LDWResult = 0xff;
        TestEolAIR_BAGResult = 0xff;
        TestEolATResult = 0xff;
    }  
    return ret; 
}

uint16_t UDS_SetResp_0x14(uint8_t *ReqData, uint16_t ReqLen, uint8_t *RespData, uint16_t RespLen)
{
    uint8_t ret;
    uint32_t DtcNum = 0;
    DtcNum = ReqData[0];
    DtcNum = DtcNum << 8;
    DtcNum |= ReqData[1];
    DtcNum = DtcNum << 8;
    DtcNum |= ReqData[2];
    
    ret = ClearDtc(DtcNum);
    if (ret == DTC_RET_OK) {
        return 0;
    } else if (ret == DTC_RET_OUT_OF_RANGE) {
        return 0x8031;
    } else {
        return 0x8012;
    }
}
//0x14 end

//0x19
uint16_t ReadDTC(uint8_t *ReqData, uint8_t ReqLen, uint8_t *RespData, uint16_t RespLen) 
{
    uint8_t RetLen = 0;
    uint8_t Sid = 0;
    uint8_t Mask = 0;
    uint16_t count = 0;
    uint8_t i;
    _19_6ReqRecord D19_6ReqRecord;
    _19_6RetRecord D19_6RetRecord;
    
    Sid = ReqData[0];
    Mask = ReqData[1]; 
    
    if (ReqData[0] == 0x1) {
        count = App_DTCCount_handle(Mask);
        RespData[RetLen] = Sid;
        RetLen++;
        RespData[RetLen] = Mask;
        RetLen++;
        RespData[RetLen] = 1;
        RetLen++;
        RespData[RetLen] = (count >> 8) & 0xff;
        RetLen++;
        RespData[RetLen] = count & 0xff;
        RetLen++;
        if (RespLen < (RetLen + 1)) {
            return 0;  
        }
        return RetLen;
    } else if (Sid == 0x2) {
        count = App_Load_DTCAndStatus_Mask(Mask, 1);
        RespData[RetLen] = Sid;
        RetLen++;
        RespData[RetLen] = Mask;
        RetLen++;
        if (count != 0) {
            if (RespLen < count * 4) {
                return DTC_RET_TOO_LONG;
            }
            if (RespData) {
                memcpy(RespData + RetLen, (uint8_t*)DtcAndStatus, count * 4);
                RetLen += count * 4;
                return RetLen;
            }
        } else {
            return RetLen;
        }
        return 0;
    } else if (Sid == 0x6) {
        uint32_t ReqDtcNum = 0;

        ReqDtcNum = ReqData[1];
        ReqDtcNum = ReqDtcNum << 8;
        ReqDtcNum |= ReqData[2];
        ReqDtcNum = ReqDtcNum << 8;
        ReqDtcNum |= ReqData[3];
        
        for (i = 0; i < DtcListTotal; i++) {
            if ((ReqDtcNum == DiagDtc[i].DtcNum.Num) && (dtc_map_info[i].Exist)) {
                App_19_6Load(&D19_6RetRecord, &DiagDtc[i], &D19_6ReqRecord);
                RespData[RetLen] = Sid;
                RetLen++;
                RespData[RetLen] = ((D19_6RetRecord.NumAndStatus >> 24) & 0xff);
                RetLen++;
                RespData[RetLen] = ((D19_6RetRecord.NumAndStatus >> 16) & 0xff);
                RetLen++;
                RespData[RetLen] = ((D19_6RetRecord.NumAndStatus >> 8) & 0xff);
                RetLen++; 
                RespData[RetLen] = D19_6RetRecord.NumAndStatus & 0xff;
                RetLen++;
                
                if (RespLen < D19_6RetRecord.dlc) {
                    return DTC_RET_TOO_LONG;   
                }

                if (RespData) {
                    memcpy(RespData + RetLen, (uint8_t*)D19_6RetRecord.Data, D19_6RetRecord.dlc);
                    RetLen += D19_6RetRecord.dlc;
                    return RetLen;
                }
                break;
            } 
        }
        if (i == DtcListTotal) {
            return DTC_RET_OF_RANGE;
        }
        
    } else if (Sid == 0xA) {
        count = App_Load_DTCAndStatus_Mask(0, 0);
        RespData[RetLen] = Sid;
        RetLen++;
        RespData[RetLen] = Mask;
        RetLen++;
        
        if (RespLen < count * 4) {
            return DTC_RET_TOO_LONG;   
        }

        if (RespData) {
            memcpy(RespData + RetLen, (uint8_t*)DtcAndStatus, count * 4);
            RetLen += count * 4;
            return RetLen;
        }
    } else {
        return DTC_RET_SUBFUNCTION_NOT_SUPPORTED;
    }
    return 0; 
}

uint16_t UDS_SetResp_0x19(uint8_t *ReqData, uint16_t ReqLen, uint8_t *RespData, uint16_t RespLen)
{
    uint16_t rlen;
    rlen = ReadDTC(&ReqData[0], ReqLen, &RespData[0], RespLen);
    if (rlen == DTC_RET_SUBFUNCTION_NOT_SUPPORTED) {
        return 0x8012;  
    } else if (rlen == DTC_RET_OF_RANGE) {
        return 0x8031;  
    } else if (rlen == DTC_RET_TOO_LONG) {
        return 0x8014;  
    } else {
        return rlen;
    }
}
//0x19 end

//0x34
DOWN_SEQUENCE DownloadSequence = SE_WAIT_31_ERASE;
DOWN_SEQUENCE DownloadCurrentSequence = SE_IDLE;
volatile _DownInfo DownInfo;
uint16_t UDS_SetResp_0x34(uint8_t *ReqData, uint16_t ReqLen, uint8_t *RespData, uint16_t RespLen)
{
    uint8_t DataFormat = ReqData[0];
    uint8_t AddrLenFormat = ReqData[1];
    if ((CurIgnSt != EVENT_IGN_ON) || (InputOutputSingal.RPM)) {
        return 0x8022;
    }
    if (DownloadSequence != SE_WAIT_34) {
        return 0x8024;
    }
    if ((DataFormat == 0x00) && ((AddrLenFormat == 0x33) || (AddrLenFormat == 0x44))) {
        uint8_t AddrLen = (AddrLenFormat >> 4) & 0xf;
        uint8_t LenLen = AddrLenFormat & 0xf;
        if (AddrLen == 3) {
            DownInfo.memoryAddress = ReqData[2];
            DownInfo.memoryAddress = DownInfo.memoryAddress << 8;
            DownInfo.memoryAddress |= ReqData[3];
            DownInfo.memoryAddress = DownInfo.memoryAddress << 8;
            DownInfo.memoryAddress |= ReqData[4];
        } else if (AddrLen == 4) {
            DownInfo.memoryAddress = ReqData[2];
            DownInfo.memoryAddress = DownInfo.memoryAddress << 8;
            DownInfo.memoryAddress |= ReqData[3];
            DownInfo.memoryAddress = DownInfo.memoryAddress << 8;
            DownInfo.memoryAddress |= ReqData[4];
            DownInfo.memoryAddress = DownInfo.memoryAddress << 8;
            DownInfo.memoryAddress |= ReqData[5];
        }
        if (LenLen == 3) {
            DownInfo.memorySize = ReqData[5];
            DownInfo.memorySize = DownInfo.memorySize << 8;
            DownInfo.memorySize |= ReqData[6];
            DownInfo.memorySize = DownInfo.memorySize << 8;
            DownInfo.memorySize |= ReqData[7];
        } else if (LenLen == 4) {
            DownInfo.memorySize = ReqData[6];
            DownInfo.memorySize = DownInfo.memorySize << 8;
            DownInfo.memorySize |= ReqData[7];
            DownInfo.memorySize = DownInfo.memorySize << 8;
            DownInfo.memorySize |= ReqData[8];
            DownInfo.memorySize = DownInfo.memorySize << 8;
            DownInfo.memorySize |= ReqData[9];
        }

        DownInfo.RecvDataLen = 0;
        DownInfo.RecvBlockNum = 1;
        memset((void *)DownInfo.memoryData, 0, 308);
        
        if ((DownInfo.memoryAddress < 0x1a20000) || (DownInfo.memoryAddress >= 0x1c00000) || ((DownInfo.memoryAddress + DownInfo.memorySize - 1) >= 0x1c00000)) {
            return 0x8031;
        } else {
            // Create Response
            RespData[0] = 0x20;
            RespData[1] = 0x00;
            RespData[2] = 0x82;
            DownloadSequence = SE_WAIT_36_37;
            DownloadCurrentSequence = SE_CURRENT_34;       
            return 3;
        }
    } else {
        return 0x8031;
    }
}
//0x34 end

//0x36
void TcflashWrite(uint32_t addr, uint8_t *Data,  uint16_t len)
{
    uint16_t i = 0;

    bsp_HDOG_Feed();
    for(i = 0; i < len; i += 4) {
        Tcflash_Write32(addr + i, *((uint32_t*)&Data[i]), TRUE);
    }
    
    if (len % 4) {
        Tcflash_Write32(addr + i, (uint32_t)Data[i], TRUE);
    }
}

uint32_t TcflashRead(uint32_t addr)
{
    return Tcflash_Read32(addr, FALSE);
}

uint8_t CRC32_CalcBuf[256] = {0};
uint32_t CRC32_readCalcVal = 0xffffffff;
uint32_t CRC32_writeCalcVal = 0xffffffff;
static void CRC32_cal(uint32_t *crcCheck,uint8_t *buf, int len)
{
    uint8_t i;
    int n = 0;
    uint32_t crc = *crcCheck;  // Initial value
    for(n = 0; n < len; n++) {
        crc ^= (uint32_t)(buf[n]) << 24;
        for (i = 0; i < 8; ++i) {
            if (crc & 0x80000000L) { 
                crc = (crc << 1) ^ 0x04C11DB7; 
            } else { 
                crc <<= 1; 
            }
        }
    }
    *crcCheck = crc;
}

volatile _DownInfo flashInfo;
volatile _DownInfo CRCInfo;
uint16_t UDS_SetResp_0x36(uint8_t *ReqData, uint16_t ReqLen, uint8_t *RespData, uint16_t RespLen)
{
    uint8_t VoltageCheckResult = 0;
    VoltageCheckResult = CheckVoltage();
    if ((CurIgnSt != EVENT_IGN_ON) || (InputOutputSingal.RPM)) {
        return 0x8022;
    }
    if (DownloadSequence != SE_WAIT_36_37) {
        return 0x8024;
    } else if (VoltageCheckResult == 1) {
        return 0x8092;
    } else if (VoltageCheckResult == 2) {
        return 0x8093;
    } else if (DownloadSequence == SE_WAIT_36_37) {
        uint8_t RecvBlockNum = ReqData[0];
        if (DownInfo.RecvBlockNum == RecvBlockNum) {
            uint8_t m8, *p8;
            int i;
            uint8_t *pData = &ReqData[1];
            DownInfo.RecvBlockNum = (DownInfo.RecvBlockNum + 1) % 256;
            memcpy((void*)&DownInfo.memoryData[8], pData, ReqLen - 1);
            memcpy((void*)&flashInfo, (void*)&DownInfo, sizeof (DownInfo));
            flashInfo.memorySize = ReqLen - 1;
            memcpy((void*)&CRCInfo, (void*)&flashInfo, sizeof (flashInfo));
            DownInfo.RecvDataLen += ReqLen - 1;
            DownInfo.memoryAddress += ReqLen - 1;
            
            m8 = flashInfo.memoryAddress & 0x7;
            if (m8) {
                p8 = (uint8_t *)&flashInfo.memoryData[8 - m8];
                flashInfo.memorySize = ReqLen - 1 + m8;
                flashInfo.memoryAddress = flashInfo.memoryAddress - m8;
                memset(p8, 0xff, m8);
            } else {
                p8 = (uint8_t *)&flashInfo.memoryData[8];
            }
            
            TcflashWrite(flashInfo.memoryAddress, p8, flashInfo.memorySize);
            CRC32_cal(&CRC32_writeCalcVal,(uint8_t*)p8, flashInfo.memorySize);
            
            for(i = 0; i < CRCInfo.memorySize; i += 4) {
                *((uint32_t *)&CRC32_CalcBuf[i]) = TcflashRead(CRCInfo.memoryAddress + i);
            }
            CRC32_cal(&CRC32_readCalcVal,CRC32_CalcBuf, CRCInfo.memorySize);

            // Create Response
            RespData[0] = RecvBlockNum;
            return 1;
        } else {
            return 0x8073;
        }
    }
    return 0;
}
//0x36 end

//0x37
uint16_t UDS_SetResp_0x37(uint8_t *ReqData, uint16_t ReqLen, uint8_t *RespData, uint16_t RespLen)
{
    if ((CurIgnSt != EVENT_IGN_ON) || (InputOutputSingal.RPM)) {
        return 0x8022;
    }
    if (DownloadSequence != SE_WAIT_36_37) {
        return 0x8024;
    } else if (DownloadSequence == SE_WAIT_36_37) {
        DownloadSequence = SE_WAIT_34;
        DownloadCurrentSequence = SE_CURRENT_37;
        return 0;
    }
    return 0;
}
//0x37 end
