UDS_ISO14229_Services.c

/******************************************************************************
 �� �� ��;UDS_ISO14229_Services.c
 ��������;ISO 14229 �淶�涨����Ϸ���������˷���ʵ�ֺ������ļ�
 ��    ��;����
 ��    ��;V1.0
 ��    ��;2016.11.1
 ******************************************************************************/
#include "UDS_ISO14229_Services.h"
#include "flash.h"
#include "CAN_Signal\CAN_Lib.h"
#include "CAN_APP\CAN_CH0_CAN_Communication_Matrix.h"
typedef struct
{
    uint8_t ProjecName[16]; // 项目名
    uint8_t PartNumber[16]; // 零件号
    uint8_t PlatForm[16];   // 平台
    struct 
    {
      uint8_t type[16];     //标准*
      uint8_t ch[16];       //通道*
      uint8_t baudrate[16]; //速率*
      uint32_t diagID[4];   //诊断ID*
    }DiagCanCfg[1];         //(诊断CAN)通道配置*
} ProjectInfoStruct;

extern void UDS_Service_Response(uint8_t si, uint8_t RspType, uint16_t A_TA_type, uint16_t A_Length, uint8_t *A_Data);

/*诊断使用, 判断App一致性, 禁止修改（内部版本号除外）*/
#if(IC_Current == TY200_080000b)
extern const ProjectInfoStruct ProjectInfo __attribute__((section(".ARM.__at_0x00007800"))) =
{
    "TianYing100",    // 天鹰100
    "TY200.080000b",      //零件号
    "BAT32G139GK64FB", // 中微芯片
    {
        "CAN_STD",
        "CAN_CH_0",
        "CAN_500Kbps",
        {DIAG_ID_Tx, DIAG_ID_Rx_PHY, DIAG_ID_Rx_FUN, 0}
    }
};
#else
extern const ProjectInfoStruct ProjectInfo __attribute__((section(".ARM.__at_0x00007800"))) =
{
    "TianYing100",    // 天鹰100
    "TY200.080000b_ty",      //零件号
    "BAT32G139GK64FB", // 中微芯片
    {
        "CAN_STD",
        "CAN_CH_0",
        "CAN_500Kbps",
        {DIAG_ID_Tx, DIAG_ID_Rx_PHY, DIAG_ID_Rx_FUN, 0}
    }
};
#endif
DiagSendDataNeg NegRes;
uint8_t UDS_ISO14229_Transfer[250];

DiagDFlashData DiagDataForDFlash;
Ser2EDFlashData Ser2EDataForDFlash;
DiagFlag DiagDataForFlag;
Ser27_FlowCtrlCntUnion Ser27_FlowCtrlCnt;
uint8_t                FlashDriverCheckSum = 0;
uint8_t                BlocKCnt            = 0;
/****************S3Timer********************/
uint8_t S3_ServerEN       = 0;
uint8_t S3_ServerCnt      = 0;
uint8_t S3_Server_refresh = 0;
/****************DiagnosticSession*****************/
uint8_t SessionType = 0x01;
/****************Ser28*********************/
uint8_t ControlType;
uint8_t CommunicationType;
/****************Ser27*********************/
uint8_t  wait10cnt    = 0;
uint8_t  Wait10sFlag  = 0;
uint8_t  DiagLockFlag = 0;
uint8_t  Seed [ 4 ];
uint32_t ValidSeedKey;
uint8_t  Services27_01_Requested;
/******************************************************************************
 The service access point of the diagnostics application layer provides a number
 of services that all have the same general structure. For each service, three
 service primitives are specified:

 - a service indication primitive, used by the diagnostics application layer, to
 pass data to the server function of the ECU diagnostic application;

 - a service response primitive, used by the server function in the ECU
 diagnostic application, to pass response data provided by the requested
 diagnostic service to the diagnostics application layer;

 - a service response-confirmation primitive, used by the server function in the
 ECU diagnostic application, to indicate that the data passed in the service
 response primitive is successfully sent on the vehicle communication bus the
 ECU received the diagnostic request on;
 ******************************************************************************/

#if(IC_Current == TY200_080000b)
//软件版本号 F195
uint8_t MCU_SWversion[6] = {'S', 'V', (uint8_t)((SWV >> 8u) & 0x0Fu) + 0x30u, '.', (uint8_t)((SWV >> 4u) & 0x0Fu) + 0x30u, (uint8_t)(SWV & 0x0Fu) + 0x30u};

//硬件版本号 F193
uint8_t MCU_HWversion[6] = {'H', 'V', (uint8_t)((HWV >> 8u) & 0x0Fu) + 0x30u, '.', (uint8_t)((HWV >> 4u) & 0x0Fu) + 0x30u, (uint8_t)(HWV & 0x0Fu) + 0x30u};

//引导程序版本信息 F180
uint8_t MCU_FBLversion[6] = {'B', 'V', (uint8_t)((BTV >> 8u) & 0x0Fu) + 0x30u, '.', (uint8_t)((BTV >> 4u) & 0x0Fu) + 0x30u, (uint8_t)(BTV & 0x0Fu) + 0x30u};

//零件号信息 F187
uint8_t MCU_PartNumber[13] = {'T', 'Y', '2', '0', '0', '.', '0', '8', '0', '0', '0', '0', 'b'};
#else
//软件版本号 F195
uint8_t MCU_SWversion[6] = {'S', 'V', (uint8_t)((SWV >> 8u) & 0x0Fu) + 0x30u, '.', (uint8_t)((SWV >> 4u) & 0x0Fu) + 0x30u, (uint8_t)(SWV & 0x0Fu) + 0x30u};

//硬件版本号 F193
uint8_t MCU_HWversion[6] = {'H', 'V', (uint8_t)((HWV >> 8u) & 0x0Fu) + 0x30u, '.', (uint8_t)((HWV >> 4u) & 0x0Fu) + 0x30u, (uint8_t)(HWV & 0x0Fu) + 0x30u};

//引导程序版本信息 F180
uint8_t MCU_FBLversion[6] = {'B', 'V', (uint8_t)((BTV >> 8u) & 0x0Fu) + 0x30u, '.', (uint8_t)((BTV >> 4u) & 0x0Fu) + 0x30u, (uint8_t)(BTV & 0x0Fu) + 0x30u};

//零件号信息 F187
uint8_t MCU_PartNumber[18] = {'T', 'Y', '2', '0', '0', '.', '0', '8', '0', '0', '0', '0', '0', 'b', '(', 't', 'y', ')'};
#endif

void Data_Set_DiagPara(void)
{
    uint8_t i;
    uint8_t length = 0;
    uint8_t Filldata_length = 0;

    //获取DID的值，等待写入DFlash
    for(i = 0; i < 6; i++)
    {
        DiagDataForDFlash.DID_F180[i] = MCU_FBLversion[i];
    }
    length = (IC_Current == TY200_080000b) ? 13 : 18;
    for( i = 0; i < length; i++)
    {
        DiagDataForDFlash.DID_F187[i] = MCU_PartNumber[i];
    }
    for(i = 0; i < 6; i++)
    {
        DiagDataForDFlash.DID_F193[i] = MCU_HWversion[i];
    }
    for(i = 0; i < 6; i++)
    {
        DiagDataForDFlash.DID_F195[i] = MCU_SWversion[i];
    }
    Filldata_length = (IC_Current == TY200_080000b) ? 1 : 4;
    for(i = 0; i < Filldata_length; i++)
    {
        DiagDataForDFlash.Filldata[i] = 0u;
    }
}

/*写App有效性标志*/
void Write_App_InValid(uint32_t m32)
{
	//设置App状态值
    DiagDataForFlag.Flag = 0xA77A5AA5u;
    DiagDataForFlag.APP_STATUS = m32;
    //擦除扇区
	EraseSector (APP_STATUS_ADDR);
    //App程序状态和22服务数据全写进去
    ProgramPage (APP_STATUS_ADDR, 8U, (uint8_t *)(&DiagDataForFlag));
}

/*写22服务数据*/
void DFlash_init(void)
{
    DiagDFlashData* ReadDiagDataForDFlash =(DiagDFlashData*)APP_DATA_INFO; 
    
    if( (ReadDiagDataForDFlash->Flag != 0x5AA5A77Au)||
        (0!=memcmp(ReadDiagDataForDFlash->DID_F180 , MCU_FBLversion,sizeof(MCU_FBLversion)))||
        (0!=memcmp(ReadDiagDataForDFlash->DID_F187 , MCU_PartNumber,sizeof(MCU_PartNumber)))||
        (0!=memcmp(ReadDiagDataForDFlash->DID_F193 , MCU_HWversion ,sizeof(MCU_HWversion )))||
        (0!=memcmp(ReadDiagDataForDFlash->DID_F195 , MCU_SWversion ,sizeof(MCU_SWversion )))
      )
    {
        Data_Set_DiagPara();
        DiagDataForDFlash.Flag = 0x5AA5A77Au;
        //擦除扇区
        EraseSector (APP_DATA_INFO);
        //App程序状态和22服务数据全写进去
        ProgramPage (APP_DATA_INFO, sizeof(DiagDataForDFlash), (uint8_t *)(&DiagDataForDFlash));
    }
    else
    {
        memcpy(&DiagDataForDFlash,ReadDiagDataForDFlash,sizeof(DiagDFlashData));
    }

}
void Data_Read_DiagPara(void)
{
    uint8_t i;
    Ser2EDFlashData* ReadSer2EDataForDFlash = (Ser2EDFlashData*)0x500600ul; 
    for ( i = 0; i < 32; i++)
    {
        Ser2EDataForDFlash.DID_F184[i] = ReadSer2EDataForDFlash->DID_F184[i];
    }
}
void S3_ServerCNTT(void)
{
    if ( Wait10sFlag == 1 )
    {
        if ( wait10cnt >= 100 )
        {
            Ser27_FlowCtrlCnt.RequestSeedCnt = 2;
            Ser27_FlowCtrlCnt.Attemptcnt     = 2;
            // WriteDFlashData(0x02u, ( uint32_t * )&Ser27_FlowCtrlCnt.Flag, sizeof(Ser27_FlowCtrlCnt) / 4u, NoNeedWait);
            wait10cnt   = 0;
            Wait10sFlag = 0;
        }
        else
        {
            ++wait10cnt;
        }
    }

    if ( S3_ServerEN == 1 )
    {
        if ( S3_Server_refresh == 1 )
        {
            S3_Server_refresh = 0;
            S3_ServerCnt      = 0;
        }

        if ( S3_ServerCnt >= S3_SERVER )
        {
            DIAG_InitParameter( );
            CAN_RX_SetEnable(&CAN_CH0_CanMsgOp, CAN_N_RX_Enable);
            CAN_TX_SetEnable(&CAN_CH0_CanMsgTxOp, CAN_N_TX_Enable);//S3超时解除
        }
        else
        {
            ++S3_ServerCnt;
        }
    }
}

uint32_t RANDOM = 0xA77A;
void Randomcnt(void)
{
    RANDOM++;
    if (RANDOM >= 0xFF0F00F0)
    {
        RANDOM = 0x363;
    }
}

uint8_t SerXXNRC12Filter(uint8_t insub, const uint8_t *supportsub, uint8_t size, uint8_t *index)
{
    uint8_t sub     = insub;
    uint8_t i       = 0;
    uint8_t nrccode = 0;

    if ( size > 0 )
    {
        nrccode = subFunctionNotSupported;
        while ( i < size )
        {
            if ( supportsub [ i ] == sub )
            {
                nrccode = 0;
                *index  = i;
                break;
            }
            i++;
        }
    }
    else
    {
        *index = 0;
    }

    return nrccode;
}

uint8_t SerXXNRC13Filter(uint8_t type, uint16_t serlength, const uint8_t *length, uint8_t index)
{
    uint8_t nrccode;
    nrccode = incorrectMessageLength;

    if ( 0 == type )
    {
        if ( length != 0 )
        {
            if ( length [ 0 ] <= serlength )
            {
                nrccode = 0;
            }
        }
        else
        {
            nrccode = incorrectMessageLength;
        }
    }
    else
    {
        if ( length != 0 )
        {
            if ( length [ index ] == serlength )
            {
                nrccode = 0;
            }
        }
        else
        {
            nrccode = incorrectMessageLength;
        }
    }
    return nrccode;
}

uint8_t SerXXNRC22Filter(void)
{
    uint8_t nrccode = 0;
    nrccode         = 0;

    return nrccode;
}

uint8_t SerXXNRC31DIDFilter(uint16_t inDID, const uint16_t *DIDList, uint8_t size, uint8_t *index)
{
    uint8_t nrccode = 0;
    uint8_t i       = 0;

    if ( size > 0 )
    {
        nrccode = requestOutOfRange;
        while ( i < size )
        {
            if ( inDID == DIDList [ i ] )
            {
                nrccode = 0;
                *index  = i;
                break;
            }
            else
            {
                ++i;
            }
        }
    }
    return nrccode;
}

uint8_t SerXXNRC33Filter(const uint8_t *levelList, uint8_t index)
{
    uint8_t nrccode;
    nrccode = 0;

    if ( NoNeed != levelList [ index ] )
    {
        if ( DiagLockFlag != levelList [ index ] )
        {
            nrccode = securityAccessDenied;
        }
    }
    return nrccode;
}

uint8_t SerXXNRC7EFilter(const uint8_t *SubSupportSession, uint8_t index)
{
    uint8_t nrccode;
    nrccode = 0;

    if ( 0 == (GetCurrentSession( ) & SubSupportSession [ index ]) )
    {
        nrccode = subfunctionNotSupportinActiveSession;
    }
    return nrccode;
}

uint8_t SerXXNRC7FFilter(uint8_t SupportSession)
{
    uint8_t nrccode;
    nrccode = 0;

    if ( 0 == (GetCurrentSession( ) & SupportSession) )
    {
        nrccode = serviceNotSupportedInActiveSession;
    }
    return nrccode;
}

uint8_t GetCurrentSession(void)
{
    uint8_t SessionMode;
    if ( DefaultSession == SessionType )
    {
        SessionMode = DefaultMode;
    }
    else if ( ProgrammingSession == SessionType )
    {
        SessionMode = ProgrammingMode;
    }
    else if ( ExtendedDiagnosticSession == SessionType )
    {
        SessionMode = ExtendedDiagnosticMode;
    }
    else
    {
        SessionType = DefaultSession;
        SessionMode = DefaultMode;
    }
    return SessionMode;
}

/******************************************************************************
 10# - DiagnosticSessionControl
 ******************************************************************************/
static const uint8_t Ser10BanResponse          = AbleResponseType;
static const uint8_t Ser10Sub []               = {DefaultSession, ProgrammingSession, ExtendedDiagnosticSession};
static const uint8_t Ser10MinLen []            = {1};
static const uint8_t Ser10SubLen []            = {1, 1, 1};
static const uint8_t Ser10UnlockLevel []       = {NoNeed, NoNeed, NoNeed};
static const uint8_t Ser10SupportSession       = DefaultMode | ProgrammingMode | ExtendedDiagnosticMode;
static const uint8_t Ser10SubSupportSession [] = {DefaultMode | ProgrammingMode | ExtendedDiagnosticMode, ProgrammingMode | ExtendedDiagnosticMode,
                                                  DefaultMode | ExtendedDiagnosticMode};
void UDS_Service_10_Indication(uint16_t A_TA_type, uint16_t A_Length, uint8_t *A_Data)
{
    uint8_t i;
    uint8_t si = DiagnosticSessionControl;
    uint8_t NrcCode = 0;
    uint8_t Index = 0;
    uint8_t SubFunction = 0;
    uint16_t Service10DiagDataLength = A_Length;
    uint16_t FunorPhy = A_TA_type;

    for (i = 0; i < Service10DiagDataLength; ++i)
    {
        UDS_ISO14229_Transfer[i] = *(A_Data + i);
    }

    if (0 == NrcCode)
    {
        NrcCode = SerXXNRC7FFilter(Ser10SupportSession);
    }

    if (0 == NrcCode)
    {
        NrcCode = SerXXNRC13Filter(MinLengthType, Service10DiagDataLength, Ser10MinLen, Index);
    }

    if (0 == NrcCode)
    {
        if (Ser10BanResponse == AbleResponseType)
        {
            SubFunction = UDS_ISO14229_Transfer[0] & 0x7F;
        }
        else
        {
            SubFunction = UDS_ISO14229_Transfer[0];
        }
        NrcCode = SerXXNRC12Filter(SubFunction, Ser10Sub, sizeof(Ser10Sub), &Index);
    }

    if ((0 == NrcCode) && (DIAG_ID_Rx_FUN == FunorPhy) && (ProgrammingSession == Ser10Sub[Index]))
    {
        NrcCode = subFunctionNotSupported;
    }

    if (0 == NrcCode && Index < sizeof(Ser10UnlockLevel))
    {
        NrcCode = SerXXNRC33Filter(Ser10UnlockLevel, Index);
    }

    if (0 == NrcCode && (Index < sizeof(Ser10SubSupportSession)))
    {
        NrcCode = SerXXNRC7EFilter(Ser10SubSupportSession, Index);
    }

    if (0 == NrcCode && (Index < sizeof(Ser10SubLen)))
    {
        NrcCode = SerXXNRC13Filter(SubLengthType, Service10DiagDataLength, Ser10SubLen, Index);
    }

    if (0 == NrcCode)
    {
        NrcCode = SerXXNRC22Filter();
    }

    if (0 == NrcCode)
    {
        UDS_ISO14229_Transfer[1] = 0;
        UDS_ISO14229_Transfer[2] = 50;
        UDS_ISO14229_Transfer[3] = 0;
        UDS_ISO14229_Transfer[4] = 200;
        if (DefaultSession == SubFunction)
        {
            S3_ServerEN = 0;
            SessionType = DefaultSession;
            DIAG_InitParameter();
            CAN_RX_SetEnable(&CAN_CH0_CanMsgOp, CAN_N_RX_Enable);
            CAN_TX_SetEnable(&CAN_CH0_CanMsgTxOp, CAN_N_TX_Enable);//切会话解除


            if (AbleResponseType == Ser10BanResponse)
            {
                if (bit_is_clear(UDS_ISO14229_Transfer[0], 7))
                {
                    UDS_Service_Response(si, POSITIVE_RSP, DIAG_ID_Tx, 5, UDS_ISO14229_Transfer);
                }
            }
            else
            {
                UDS_Service_Response(si, POSITIVE_RSP, DIAG_ID_Tx, 5, UDS_ISO14229_Transfer);
            }
        }
        else if (ProgrammingSession == SubFunction)
        {
            S3_ServerEN = 1;
            S3_Server_refresh = 1;
            SessionType = ProgrammingSession;
            Services27_01_Requested = 0;
            DiagLockFlag = 0;

            if (AbleResponseType == Ser10BanResponse)
            {
                if (bit_is_clear(UDS_ISO14229_Transfer[0], 7))
                {
                    NegRes.code = requestCorrectlyReceivedResponsePending;
                    UDS_Service_Response(si, NEGATIVE_RSP, DIAG_ID_Tx, 1, (uint8_t *)(&NegRes.code));

                    Write_App_InValid(Jump_To_Boot_Need_Answer);

                    for (i = 0; i < 250; ++i)
                    {
                        __NOP();
                    }
                    __NVIC_SystemReset();
                }
                else
                {
                    Write_App_InValid(Jump_To_Boot_No_Answer);
                    for (i = 0; i < 250; ++i)
                    {
                        __NOP();
                    }
                    __NVIC_SystemReset();
                }
            }
            else
            {
                NegRes.code = requestCorrectlyReceivedResponsePending;
                UDS_Service_Response(si, NEGATIVE_RSP, DIAG_ID_Tx, 1, (uint8_t *)(&NegRes.code));
                Write_App_InValid(Jump_To_Boot_Need_Answer);
                for (i = 0; i < 250; ++i)
                {
                    __NOP();
                }
                __NVIC_SystemReset();
            }
        }
        else if (ExtendedDiagnosticSession == SubFunction)
        {
            S3_ServerEN = 1;
            S3_Server_refresh = 1;
            SessionType = ExtendedDiagnosticSession;
            Services27_01_Requested = 0;
            DiagLockFlag = 0;

            if (AbleResponseType == Ser10BanResponse)
            {
                if (bit_is_clear(UDS_ISO14229_Transfer[0], 7))
                {
                    UDS_Service_Response(si, POSITIVE_RSP, DIAG_ID_Tx, 5, UDS_ISO14229_Transfer);
                }
            }
            else
            {
                UDS_Service_Response(si, POSITIVE_RSP, DIAG_ID_Tx, 5, UDS_ISO14229_Transfer);
            }
        }
    }
    else
    {
        if (DIAG_ID_Rx_FUN == FunorPhy)
        {
            if ((NrcCode != 0x11) && (NrcCode != 0x12) && (NrcCode != 0x13) && (NrcCode != 0x7E) && (NrcCode != 0x7F) && (NrcCode != 0x31))
            {
                NegRes.code = NrcCode;
                UDS_Service_Response(si, NEGATIVE_RSP, DIAG_ID_Tx, 1, (uint8_t *)(&NegRes.code));
                return;
            }
        }
        else
        {
            NegRes.code = NrcCode;
            UDS_Service_Response(si, NEGATIVE_RSP, DIAG_ID_Tx, 1, (uint8_t *)(&NegRes.code));
            return;
        }
    }
}

/******************************************************************************
 11# - ECUReset
 ******************************************************************************/
static const uint8_t Ser11BanResponse          = AbleResponseType;
static const uint8_t Ser11Sub []               = {HardReset};
static const uint8_t Ser11MinLen []            = {1};
static const uint8_t Ser11SubLen []            = {1};
static const uint8_t Ser11UnlockLevel []       = {NoNeed};
static const uint8_t Ser11SupportSession       = ProgrammingMode | ExtendedDiagnosticMode;
static const uint8_t Ser11SubSupportSession [] = {ProgrammingMode | ExtendedDiagnosticMode};
void UDS_Service_11_Indication(uint16_t A_TA_type, uint16_t A_Length, uint8_t *A_Data)
{
    uint8_t si = ECUReset;
    uint8_t NrcCode = 0;
    uint8_t Index = 0;
    uint8_t SubFunction = 0;
    uint16_t i;
    uint16_t Service11DiagDataLength = A_Length;
    uint16_t FunorPhy = A_TA_type;

    for (i = 0; i < Service11DiagDataLength; ++i)
    {
        UDS_ISO14229_Transfer[i] = *(A_Data + i);
    }

    if ((0 == NrcCode) && (DIAG_ID_Rx_FUN == FunorPhy))
    {
        NrcCode = serviceNotSupported;
    }

    if (0 == NrcCode)
    {
        NrcCode = SerXXNRC7FFilter(Ser11SupportSession);
    }

    if (0 == NrcCode)
    {
        NrcCode = SerXXNRC13Filter(MinLengthType, Service11DiagDataLength, Ser11MinLen, Index);
    }

    if (0 == NrcCode)
    {
        if (AbleResponseType == Ser11BanResponse)
        {
            SubFunction = UDS_ISO14229_Transfer[0] & 0x7F;
        }
        else
        {
            SubFunction = UDS_ISO14229_Transfer[0];
        }
        NrcCode = SerXXNRC12Filter(SubFunction, Ser11Sub, sizeof(Ser11Sub), &Index);
    }

    if (0 == NrcCode && Index < sizeof(Ser11UnlockLevel))
    {
        NrcCode = SerXXNRC33Filter(Ser11UnlockLevel, Index);
    }

    if (0 == NrcCode && (Index < sizeof(Ser11SubSupportSession)))
    {
        NrcCode = SerXXNRC7EFilter(Ser11SubSupportSession, Index);
    }

    if (0 == NrcCode && (Index < sizeof(Ser11SubLen)))
    {
        NrcCode = SerXXNRC13Filter(SubLengthType, Service11DiagDataLength, Ser11SubLen, Index);
    }

    if (0 == NrcCode)
    {
        NrcCode = SerXXNRC22Filter();
    }

    if (0 == NrcCode)
    {
        if (HardReset == SubFunction)
        {
            if (AbleResponseType == Ser11BanResponse)
            {
                if (bit_is_clear(UDS_ISO14229_Transfer[0], 7))
                {
                    UDS_Service_Response(si, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer);
                }
            }
            else
            {
                UDS_Service_Response(si, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer);
            }
            i = 900;
            while (--i)
            {
                __NOP();
            }
            __NVIC_SystemReset();
        }
    }
    else
    {
        if (DIAG_ID_Rx_FUN == FunorPhy)
        {
            if ((NrcCode != 0x11) && (NrcCode != 0x12) && (NrcCode != 0x13) && (NrcCode != 0x7E) && (NrcCode != 0x7F) && (NrcCode != 0x31))
            {
                NegRes.code = NrcCode;
                UDS_Service_Response(si, NEGATIVE_RSP, DIAG_ID_Tx, 1, (uint8_t *)(&NegRes.code));
                return;
            }
        }
        else
        {
            NegRes.code = NrcCode;
            UDS_Service_Response(si, NEGATIVE_RSP, DIAG_ID_Tx, 1, (uint8_t *)(&NegRes.code));
            return;
        }
    }
}

/******************************************************************************
 22# - ReadDataByIdentifier
 ******************************************************************************/
static const uint8_t Ser22BanResponse = DisableResponseType;
static const uint8_t Ser22Sub []      = {0xFF};
static const uint8_t Ser22MinLen []   = {2};
static const uint8_t Ser22SubLen []   = {2};
static const uint16_t Ser22DIDList [] = {0xF195, 0xF193, 0xF180, 0xF187, 0x1024, 0x1028, 0xF184,0x2024 };
static const uint8_t Ser22UnlockLevel []       = {NoNeed};
static const uint8_t Ser22SupportSession       = DefaultMode | ProgrammingMode | ExtendedDiagnosticMode;
static const uint8_t Ser22SubSupportSession [] = {DefaultMode | ProgrammingMode | ExtendedDiagnosticMode};
void UDS_Service_22_Indication(uint16_t A_TA_type, uint16_t A_Length, uint8_t *A_Data)
{
    uint8_t i;
    uint8_t si = ReadDataByIdentifier;
    uint8_t NrcCode = 0;
    uint8_t Index = 0;
    uint8_t SubFunction = 0;
    uint16_t DID;
    uint16_t Service22DiagDataLength = A_Length;
    uint16_t FunorPhy = A_TA_type;
    uint8_t length = 0;

    for (i = 0; i < Service22DiagDataLength; ++i)
    {
        UDS_ISO14229_Transfer[i] = *(A_Data + i);
    }

    if ((0 == NrcCode) && (DIAG_ID_Rx_FUN == FunorPhy))
    {
        NrcCode = serviceNotSupported;
    }

    if (0 == NrcCode)
    {
        NrcCode = SerXXNRC7FFilter(Ser22SupportSession);
    }

    if (0 == NrcCode)
    {
        NrcCode = SerXXNRC13Filter(MinLengthType, Service22DiagDataLength, Ser22MinLen, Index);
    }

    if (0 == NrcCode)
    {
        if (AbleResponseType == Ser22BanResponse)
        {
            SubFunction = UDS_ISO14229_Transfer[0] & 0x7F;
        }
        else
        {
            SubFunction = UDS_ISO14229_Transfer[0];
        }
        NrcCode = SerXXNRC12Filter(SubFunction, Ser22Sub, 0, &Index);
    }

    if (0 == NrcCode && Index < sizeof(Ser22UnlockLevel))
    {
        NrcCode = SerXXNRC33Filter(Ser22UnlockLevel, Index);
    }

    if (0 == NrcCode && (Index < sizeof(Ser22SubSupportSession)))
    {
        NrcCode = SerXXNRC7EFilter(Ser22SubSupportSession, Index);
    }

    if (0 == NrcCode && (Index < sizeof(Ser22SubLen)))
    {
        NrcCode = SerXXNRC13Filter(SubLengthType, Service22DiagDataLength, Ser22SubLen, Index);
    }

    if (0 == NrcCode)
    {
        NrcCode = SerXXNRC22Filter();
    }

    if (0 == NrcCode)
    {
        DID = ((uint16_t)UDS_ISO14229_Transfer[0] << 8) | UDS_ISO14229_Transfer[1];
        NrcCode = SerXXNRC31DIDFilter(DID, Ser22DIDList, (uint8_t)(sizeof(Ser22DIDList) / 2), &Index);
    }

    if (0 == NrcCode)
    {
        switch (DID)
        {
        case 0xF195: // 读软件版本号
            for (i = 0; i < 6; i++)
            {
                UDS_ISO14229_Transfer[i + 2] = MCU_SWversion[i];
            }
            UDS_Service_Response(si, POSITIVE_RSP, DIAG_ID_Tx, 2 + 6, UDS_ISO14229_Transfer);
            break;

        case 0xF193: // 读硬件版本号
            for (i = 0; i < 6; i++)
            {
                UDS_ISO14229_Transfer[i + 2] = MCU_HWversion[i];
            }
            UDS_Service_Response(si, POSITIVE_RSP, DIAG_ID_Tx, 2 + 6, UDS_ISO14229_Transfer);
            break;
        case 0xF180: // 引导版本信息
            for (i = 0; i < 6; i++)
            {
                UDS_ISO14229_Transfer[i + 2] = MCU_FBLversion[i];
            }
            UDS_Service_Response(si, POSITIVE_RSP, DIAG_ID_Tx, 2 + 6, UDS_ISO14229_Transfer);
            break;

        case 0xF187: // 读零件号
            length = (IC_Current == TY200_080000b) ? 13 : 18;
            for (uint8_t i = 0; i < length; i++) 
            {
                UDS_ISO14229_Transfer[i + 2] = MCU_PartNumber[i];
            }
            UDS_Service_Response(si, POSITIVE_RSP, DIAG_ID_Tx, 2 + length, UDS_ISO14229_Transfer);
            
            break;

        case 0x1024: // 读内部版本号
            UDS_ISO14229_Transfer[0] = 0x10u;
            UDS_ISO14229_Transfer[1] = 0x24u;
            UDS_ISO14229_Transfer[2] = 'A';
            UDS_ISO14229_Transfer[3] = '_';
            UDS_ISO14229_Transfer[4] = 'V';
            UDS_ISO14229_Transfer[5] = (uint8_t)((INTLV >> 8u) & 0x0Fu) + 0x30u;
            UDS_ISO14229_Transfer[6] = '.';
            UDS_ISO14229_Transfer[7] = (uint8_t)((INTLV >> 4u) & 0x0Fu) + 0x30u;
            UDS_ISO14229_Transfer[8] = (uint8_t)(INTLV & 0x0Fu) + 0x30u;
            UDS_Service_Response(si, POSITIVE_RSP, DIAG_ID_Tx, 2 + 7, UDS_ISO14229_Transfer);
            break;

        case 0x1028: // 读内部版本号
            UDS_ISO14229_Transfer[0] = 0x10u;
            UDS_ISO14229_Transfer[1] = 0x28u;
            UDS_ISO14229_Transfer[2] = '2';
            UDS_ISO14229_Transfer[3] = '0';
            UDS_ISO14229_Transfer[4] = (uint8_t)((PROG_Y >> 4u) & 0x0Fu) + 0x30u;
            UDS_ISO14229_Transfer[5] = (uint8_t)(PROG_Y & 0x0Fu) + 0x30u;
            UDS_ISO14229_Transfer[6] = (uint8_t)((PROG_M >> 4u) & 0x0Fu) + 0x30u;
            UDS_ISO14229_Transfer[7] = (uint8_t)(PROG_M & 0x0Fu) + 0x30u;
            UDS_ISO14229_Transfer[8] = (uint8_t)((PROG_D >> 4u) & 0x0Fu) + 0x30u;
            UDS_ISO14229_Transfer[9] = (uint8_t)(PROG_D & 0x0Fu) + 0x30u;
            UDS_Service_Response(si, POSITIVE_RSP, DIAG_ID_Tx, 2 + 8, UDS_ISO14229_Transfer);
            break;

        case 0xF184: // UUID
            for (i = 0; i < 32; i++)
            {
                UDS_ISO14229_Transfer[i + 2] = Ser2EDataForDFlash.DID_F184[i];
            }
            UDS_Service_Response(0x22, POSITIVE_RSP, DIAG_ID_Tx, 2 + 32, UDS_ISO14229_Transfer);
            break;

        case 0x2024: // ESP32内部版本号
            UDS_ISO14229_Transfer[2] = BlueTooth.ESP32_SWV;
            UDS_Service_Response(0x22, POSITIVE_RSP, DIAG_ID_Tx, 2 + 1, UDS_ISO14229_Transfer);
            break;

        default:
            if (FunorPhy == DIAG_ID_Rx_FUN)
            {
                return;
            }
            NegRes.code = requestOutOfRange;
            UDS_Service_Response ( 0x22, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
            break;
        }
    }
    else
    {
        if (DIAG_ID_Rx_FUN == FunorPhy)
        {
            if ((NrcCode != 0x11) && (NrcCode != 0x12) && (NrcCode != 0x13) && (NrcCode != 0x7E) && (NrcCode != 0x7F) && (NrcCode != 0x31))
            {
                NegRes.code = NrcCode;
                UDS_Service_Response(si, NEGATIVE_RSP, DIAG_ID_Tx, 1, (uint8_t *)(&NegRes.code));
                return;
            }
        }
        else
        {
            NegRes.code = NrcCode;
            UDS_Service_Response(si, NEGATIVE_RSP, DIAG_ID_Tx, 1, (uint8_t *)(&NegRes.code));
            return;
        }
    }
}

void UDS_Service_2E_Indication ( uint16_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    uint16_t i;
    uint16_t DID;
    uint16_t  Service2EDiagDataLength;
    uint16_t FunorPhy = 0;
    Service2EDiagDataLength = A_Length;
    S3_ServerCnt = 0;
    FunorPhy = A_TA_type;

    for (i = 0; i < Service2EDiagDataLength; ++i)
    {
        UDS_ISO14229_Transfer[i] = *(A_Data + i);
    }
    if ( FunorPhy == 0X7DF )
    {
        return;
    }
    
    if( (SessionType == DefaultSession) || (SessionType == ProgrammingSession))
    {
        NegRes.code = serviceNotSupportedInActiveSession; //NRC7F
        UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    if ( Service2EDiagDataLength < 3 )
    {
        NegRes.code = incorrectMessageLength; //NRC13
        UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    if ( DiagLockFlag == 0x00 )
    {
        NegRes.code = securityAccessDenied; //NRC33
        UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    DID = ((uint16_t)UDS_ISO14229_Transfer[0] << 8) | ((uint16_t)UDS_ISO14229_Transfer[1]);
    switch ( DID )
    {
        case 0xF184: // 写入UUID
            if ( Service2EDiagDataLength == 34 ) //限制最大长度
            {
                for (i = 0; i < (sizeof(Ser2EDataForDFlash.DID_F184)); i++)
                {
                    Ser2EDataForDFlash.DID_F184[i] = UDS_ISO14229_Transfer[i + 2];
                }
                Ser2EDataForDFlash.Flag = 0x5AA5A77Au;
                // 擦除扇区
                EraseSector(0x500600ul);
                // App程序状态和22服务数据全写进去
                ProgramPage(0x500600ul, sizeof(Ser2EDataForDFlash), (uint8_t *)(&Ser2EDataForDFlash));
                UDS_Service_Response(0x2E, POSITIVE_RSP, DIAG_ID_Tx, 2, UDS_ISO14229_Transfer);
            }
            else
            {
                NegRes.code = incorrectMessageLength; //NRC13
                UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
            }
            break;
        default:
            if ( FunorPhy == 0x7df )
            {
                return;
            }
            NegRes.code = requestOutOfRange;
            UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
            break;
    }
}


void GetSeed(void)
{
    uint8_t SeedHigh;
    uint8_t SeedLow;
    uint16_t tempbuffer;

    tempbuffer = (uint16_t)(RANDOM);
    SeedHigh = (uint8_t)(tempbuffer >> 8);
    SeedLow = (uint8_t)(tempbuffer & 0xff);
    Seed[0] = 0x31 + ~SeedHigh;
    Seed[1] = 0x23 + ~SeedLow;
    Seed[2] = 0x56 + SeedHigh;
    Seed[3] = 0x71 + SeedLow;
}

const uint32_t KeyK = 0x00000201;
void CalculateKey(void)
{
    uint32_t SaccSeed;
    uint32_t KeyResult;

    SaccSeed = ((uint32_t)Seed[0]) << 24 | ((uint32_t)Seed[1]) << 16 | ((uint32_t)Seed[2]) << 8 | ((uint32_t)Seed[3]);

    KeyResult = (((SaccSeed >> 1) ^ SaccSeed) << 3) ^ (SaccSeed >> 2);
    KeyResult = KeyResult ^ KeyK;

    ValidSeedKey = KeyResult;
}

void CalculateKeyLv11(void)
{
    uint32_t SaccSeed;
    uint32_t KeyResult;

    SaccSeed = (( uint32_t )Seed [ 0 ]) << 24 | (( uint32_t )Seed [ 1 ]) << 16 | (( uint32_t )Seed [ 2 ]) << 8 | (( uint32_t )Seed [ 3 ]);

    KeyResult = (((SaccSeed >> 1) ^ SaccSeed) << 3) ^ (SaccSeed >> 2);
    KeyResult = KeyResult ^ KeyK;

    ValidSeedKey = KeyResult;
}

/******************************************************************************
 27# - SecurityAccess
 ******************************************************************************/
static const uint8_t Ser27BanResponse          = DisableResponseType;
static const uint8_t Ser27Sub []               = {requestSeed_LV1, sendKey_LV1};
static const uint8_t Ser27MinLen []            = {1};
static const uint8_t Ser27SubLen []            = {1, 5};
static const uint8_t Ser27UnlockLevel []       = {NoNeed, NoNeed};
static const uint8_t Ser27SupportSession       = ProgrammingMode | ExtendedDiagnosticMode;
static const uint8_t Ser27SubSupportSession [] = {ProgrammingMode | ExtendedDiagnosticMode, ProgrammingMode | ExtendedDiagnosticMode};
void UDS_Service_27_Indication(uint16_t A_TA_type, uint16_t A_Length, uint8_t *A_Data)
{
    uint8_t i;
    uint8_t si = SecurityAccess;
    uint8_t NrcCode = 0;
    uint8_t Index = 0;
    uint8_t SubFunction = 0;
    uint16_t FunorPhy = A_TA_type;
    uint16_t Service27DiagDataLength = A_Length;
    uint32_t KeyReceive;

    for (i = 0; i < Service27DiagDataLength; ++i)
    {
        UDS_ISO14229_Transfer[i] = *(A_Data + i);
    }

    if ((0 == NrcCode) && (DIAG_ID_Rx_FUN == FunorPhy))
    {
        NrcCode = serviceNotSupported;
    }

    if (0 == NrcCode)
    {
        NrcCode = SerXXNRC7FFilter(Ser27SupportSession);
    }

    if (0 == NrcCode)
    {
        NrcCode = SerXXNRC13Filter(MinLengthType, Service27DiagDataLength, Ser27MinLen, Index);
    }

    if (0 == NrcCode)
    {
        if (AbleResponseType == Ser27BanResponse)
        {
            SubFunction = UDS_ISO14229_Transfer[0] & 0x7F;
        }
        else
        {
            SubFunction = UDS_ISO14229_Transfer[0];
        }
        NrcCode = SerXXNRC12Filter(SubFunction, Ser27Sub, sizeof(Ser27Sub), &Index);
    }

    if (0 == NrcCode && Index < sizeof(Ser27UnlockLevel))
    {
        NrcCode = SerXXNRC33Filter(Ser27UnlockLevel, Index);
    }

    if (0 == NrcCode && (Index < sizeof(Ser27SubSupportSession)))
    {
        NrcCode = SerXXNRC7EFilter(Ser27SubSupportSession, Index);
    }

    if (0 == NrcCode && (Index < sizeof(Ser27SubLen)))
    {
        NrcCode = SerXXNRC13Filter(SubLengthType, Service27DiagDataLength, Ser27SubLen, Index);
    }

    if (0 == NrcCode)
    {
        NrcCode = SerXXNRC22Filter();
    }

    if (0 == NrcCode)
    {
        if (1 == Wait10sFlag)
        {
            NrcCode = requiredTimeDelayNotExpired;
        }
    }

    if (0 == NrcCode)
    {
        if (0 == Services27_01_Requested && sendKey_LV1 == SubFunction)
        {
            NrcCode = requestSequenceError;
        }
    }

    if (0 == NrcCode)
    {
        KeyReceive = ((uint32_t)UDS_ISO14229_Transfer[1] << 24) | ((uint32_t)UDS_ISO14229_Transfer[2] << 16) | ((uint16_t)UDS_ISO14229_Transfer[3] << 8) | UDS_ISO14229_Transfer[4];
        if (sendKey_LV1 == SubFunction && (KeyReceive == 0x00000000 || KeyReceive == 0xFFFFFFFF))
        {
            NrcCode = requestOutOfRange;
        }
    }

    if (0 == NrcCode)
    {
        if (sendKey_LV1 == SubFunction && KeyReceive != ValidSeedKey)
        {
            Services27_01_Requested = 0;
            Ser27_FlowCtrlCnt.Attemptcnt++;
            // WriteDFlashData(0x02u, (uint32_t *)&Ser27_FlowCtrlCnt.Flag, sizeof(Ser27_FlowCtrlCnt) / 4u, NeedWait);
            NrcCode = invalidKey;
        }
    }

    if (0 == NrcCode || invalidKey == NrcCode)
    {
        if (sendKey_LV1 == SubFunction)
        {
            if (AttemptMaxCnt <= Ser27_FlowCtrlCnt.Attemptcnt)
            {
                Wait10sFlag = 1;
                NrcCode = exceedNumberOfAttempts;
            }
        }
        if (requestSeed_LV1 == SubFunction)
        {
            Ser27_FlowCtrlCnt.Attemptcnt = Ser27_FlowCtrlCnt.RequestSeedCnt;
            Ser27_FlowCtrlCnt.RequestSeedCnt++;
            // WriteDFlashData(0x02u, (uint32_t *)&Ser27_FlowCtrlCnt.Flag, sizeof(Ser27_FlowCtrlCnt) / 4u, NeedWait);
            if (RequestSeedMaxCnt <= Ser27_FlowCtrlCnt.RequestSeedCnt)
            {
                Wait10sFlag = 1;
                NrcCode = exceedNumberOfAttempts;
            }
        }
    }

    if (0 == NrcCode)
    {
        if (requestSeed_LV1 == SubFunction)
        {
            if (NormalKeyLock == DiagLockFlag)
            {
                Services27_01_Requested = 1;
                do
                {
                    GetSeed();
                } while (Seed[0] == 0 && Seed[1] == 0 && Seed[2] == 0 && Seed[3] == 0);
                UDS_ISO14229_Transfer[1] = Seed[0];
                UDS_ISO14229_Transfer[2] = Seed[1];
                UDS_ISO14229_Transfer[3] = Seed[2];
                UDS_ISO14229_Transfer[4] = Seed[3];
                if (AbleResponseType == Ser27BanResponse)
                {
                    if (bit_is_clear(UDS_ISO14229_Transfer[0], 7))
                    {
                        UDS_Service_Response(si, POSITIVE_RSP, DIAG_ID_Tx, 5, UDS_ISO14229_Transfer);
                    }
                }
                else
                {
                    UDS_Service_Response(si, POSITIVE_RSP, DIAG_ID_Tx, 5, UDS_ISO14229_Transfer);
                }
                CalculateKey();
            }
            else if (NormalKeyUnlock == DiagLockFlag)
            {
                UDS_ISO14229_Transfer[1] = 0;
                UDS_ISO14229_Transfer[2] = 0;
                UDS_ISO14229_Transfer[3] = 0;
                UDS_ISO14229_Transfer[4] = 0;
                if (AbleResponseType == Ser27BanResponse)
                {
                    if (bit_is_clear(UDS_ISO14229_Transfer[0], 7))
                    {
                        UDS_Service_Response(si, POSITIVE_RSP, DIAG_ID_Tx, 5, UDS_ISO14229_Transfer);
                    }
                }
                else
                {
                    UDS_Service_Response(si, POSITIVE_RSP, DIAG_ID_Tx, 5, UDS_ISO14229_Transfer);
                }
            }
        }
        else if (sendKey_LV1 == SubFunction)
        {
            Services27_01_Requested = 0;
            Ser27_FlowCtrlCnt.RequestSeedCnt = 0;
            Ser27_FlowCtrlCnt.Attemptcnt = 0;
            DiagLockFlag = NormalKeyUnlock;
            if (AbleResponseType == Ser27BanResponse)
            {
                if (bit_is_clear(UDS_ISO14229_Transfer[0], 7))
                {
                    UDS_Service_Response(si, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer);
                }
            }
            else
            {
                UDS_Service_Response(si, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer);
            }
            // WriteDFlashData(0x02u, (uint32_t *)&Ser27_FlowCtrlCnt.Flag, sizeof(Ser27_FlowCtrlCnt) / 4u, NeedWait);
        }
    }
    else
    {
        if (DIAG_ID_Rx_FUN == FunorPhy)
        {
            if ((NrcCode != 0x11) && (NrcCode != 0x12) && (NrcCode != 0x13) && (NrcCode != 0x7E) && (NrcCode != 0x7F) && (NrcCode != 0x31))
            {
                NegRes.code = NrcCode;
                UDS_Service_Response(si, NEGATIVE_RSP, DIAG_ID_Tx, 1, (uint8_t *)(&NegRes.code));
                return;
            }
        }
        else
        {
            NegRes.code = NrcCode;
            UDS_Service_Response(si, NEGATIVE_RSP, DIAG_ID_Tx, 1, (uint8_t *)(&NegRes.code));
            return;
        }
    }
}

/******************************************************************************
 28# - CommunicationControl
 ******************************************************************************/
static const uint8_t Ser28BanResponse          = AbleResponseType;
static const uint8_t Ser28Sub []               = {enableRxAndTx,/*enableRxAndDisableTx, disableRxAndEnableTx, */disableRxAndTx};
static const uint8_t Ser28MinLen []            = {2};
static const uint8_t Ser28SubLen []            = {2, /*2, 2,*/ 2};
static const uint8_t Ser28UnlockLevel []       = {NoNeed, NoNeed, NoNeed, NoNeed};
static const uint8_t Ser28SupportSession       = ProgrammingMode | ExtendedDiagnosticMode;
static const uint8_t Ser28SubSupportSession [] = {ProgrammingMode | ExtendedDiagnosticMode, ProgrammingMode | ExtendedDiagnosticMode, ProgrammingMode | ExtendedDiagnosticMode, ProgrammingMode | ExtendedDiagnosticMode};
void UDS_Service_28_Indication(uint16_t A_TA_type, uint16_t A_Length, uint8_t *A_Data)
{
    uint8_t i;
    uint8_t si = CommunicationControl;
    uint8_t NrcCode = 0;
    uint8_t Index = 0;
    uint8_t SubFunction = 0;
    uint16_t FunorPhy = A_TA_type;
    uint16_t Service28DiagDataLength = A_Length;

    for (i = 0; i < Service28DiagDataLength; ++i)
    {
        UDS_ISO14229_Transfer[i] = *(A_Data + i);
    }

    if (0 == NrcCode)
    {
        NrcCode = SerXXNRC7FFilter(Ser28SupportSession);
    }

    if (0 == NrcCode)
    {
        NrcCode = SerXXNRC13Filter(MinLengthType, Service28DiagDataLength, Ser28MinLen, Index);
    }

    if (0 == NrcCode)
    {
        if (AbleResponseType == Ser28BanResponse)
        {
            SubFunction = UDS_ISO14229_Transfer[0] & 0x7F;
        }
        else
        {
            SubFunction = UDS_ISO14229_Transfer[0];
        }
        NrcCode = SerXXNRC12Filter(SubFunction, Ser28Sub, sizeof(Ser28Sub), &Index);
    }

    if (0 == NrcCode && Index < sizeof(Ser28UnlockLevel))
    {
        NrcCode = SerXXNRC33Filter(Ser28UnlockLevel, Index);
    }

    if (0 == NrcCode && (Index < sizeof(Ser28SubSupportSession)))
    {
        NrcCode = SerXXNRC7EFilter(Ser28SubSupportSession, Index);
    }

    if (0 == NrcCode && (Index < sizeof(Ser28SubLen)))
    {
        NrcCode = SerXXNRC13Filter(SubLengthType, Service28DiagDataLength, Ser28SubLen, Index);
    }

    if (0 == NrcCode)
    {
        NrcCode = SerXXNRC22Filter();
    }

    // if (0 == NrcCode)
    // {
    //     if (normalCommunicationMessages != UDS_ISO14229_Transfer[1])
    //     {
    //         NrcCode = requestOutOfRange;
    //     }
    // }

    if (0 == NrcCode)
    {
        if (enableRxAndTx == SubFunction)
        {
            if (UDS_ISO14229_Transfer[1] & normalCommunicationMessages)
            {
                CAN_RX_SetEnable(&CAN_CH0_CanMsgOp, CAN_N_RX_Enable);
                CAN_TX_SetEnable(&CAN_CH0_CanMsgTxOp, CAN_N_TX_Enable);
            }
            if (UDS_ISO14229_Transfer[1] & networkManagementCommunicationMessages)
            {
            }
        }
        else if (disableRxAndTx == SubFunction)
        {
            if (UDS_ISO14229_Transfer[1] & normalCommunicationMessages)
            {
                CAN_RX_SetEnable(&CAN_CH0_CanMsgOp, CAN_N_RX_Disable);
                CAN_TX_SetEnable(&CAN_CH0_CanMsgTxOp, CAN_N_TX_Disable);
            }
            if (UDS_ISO14229_Transfer[1] & networkManagementCommunicationMessages)
            {
            }
            if (UDS_ISO14229_Transfer[1] & networkManagementCommunicationMessagesnormalCommunicationMessages)
            {
            }
        }

        if (AbleResponseType == Ser28BanResponse)
        {
            if (bit_is_clear(UDS_ISO14229_Transfer[0], 7))
            {
                UDS_Service_Response(si, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer);
            }
        }
        else
        {
            UDS_Service_Response(si, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer);
        }
    }
    else
    {
        if (DIAG_ID_Rx_FUN == FunorPhy)
        {
            if ((NrcCode != 0x11) && (NrcCode != 0x12) && (NrcCode != 0x13) && (NrcCode != 0x7E) && (NrcCode != 0x7F) && (NrcCode != 0x31))
            {
                NegRes.code = NrcCode;
                UDS_Service_Response(si, NEGATIVE_RSP, DIAG_ID_Tx, 1, (uint8_t *)(&NegRes.code));
                return;
            }
        }
        else
        {
            NegRes.code = NrcCode;
            UDS_Service_Response(si, NEGATIVE_RSP, DIAG_ID_Tx, 1, (uint8_t *)(&NegRes.code));
            return;
        }
    }
}

/******************************************************************************
 3E# - TesterPresent
 ******************************************************************************/
static const uint8_t Ser3EBanResponse          = AbleResponseType;
static const uint8_t Ser3ESub []               = {0x00};
static const uint8_t Ser3EMinLen []            = {1};
static const uint8_t Ser3ESubLen []            = {1};
static const uint8_t Ser3EUnlockLevel []       = {NoNeed};
static const uint8_t Ser3ESupportSession       = DefaultMode | ProgrammingMode | ExtendedDiagnosticMode;
static const uint8_t Ser3ESubSupportSession [] = {DefaultMode | ProgrammingMode | ExtendedDiagnosticMode};
void UDS_Service_3E_Indication(uint16_t A_TA_type, uint16_t A_Length, uint8_t *A_Data)
{
    uint8_t i;
    uint8_t si = TesterPresent;
    uint8_t NrcCode = 0;
    uint8_t Index = 0;
    uint8_t SubFunction = 0;
    uint16_t Service3EDiagDataLength = A_Length;
    uint16_t FunorPhy = A_TA_type;

    for (i = 0; i < Service3EDiagDataLength; ++i)
    {
        UDS_ISO14229_Transfer[i] = *(A_Data + i);
    }

    if (0 == NrcCode)
    {
        NrcCode = SerXXNRC7FFilter(Ser3ESupportSession);
    }

    if (0 == NrcCode)
    {
        NrcCode = SerXXNRC13Filter(MinLengthType, Service3EDiagDataLength, Ser3EMinLen, Index);
    }

    if (0 == NrcCode)
    {
        if (AbleResponseType == Ser3EBanResponse)
        {
            SubFunction = UDS_ISO14229_Transfer[0] & 0x7F;
        }
        else
        {
            SubFunction = UDS_ISO14229_Transfer[0];
        }
        NrcCode = SerXXNRC12Filter(SubFunction, Ser3ESub, sizeof(Ser3ESub), &Index);
    }

    if (0 == NrcCode && Index < sizeof(Ser3EUnlockLevel))
    {
        NrcCode = SerXXNRC33Filter(Ser3EUnlockLevel, Index);
    }

    if (0 == NrcCode && (Index < sizeof(Ser3ESubSupportSession)))
    {
        NrcCode = SerXXNRC7EFilter(Ser3ESubSupportSession, Index);
    }

    if (0 == NrcCode && (Index < sizeof(Ser3ESubLen)))
    {
        NrcCode = SerXXNRC13Filter(SubLengthType, Service3EDiagDataLength, Ser3ESubLen, Index);
    }

    if (0 == NrcCode)
    {
        NrcCode = SerXXNRC22Filter();
    }

    if (0 == NrcCode)
    {
        if (0x00 == SubFunction)
        {
            S3_Server_refresh = 1;
            if (AbleResponseType == Ser3EBanResponse)
            {
                if (bit_is_clear(UDS_ISO14229_Transfer[0], 7))
                {
                    UDS_Service_Response(si, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer);
                }
            }
            else
            {
                UDS_Service_Response(si, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer);
            }
        }
    }
    else
    {
        if (DIAG_ID_Rx_FUN == FunorPhy)
        {
            if ((NrcCode != 0x11) && (NrcCode != 0x12) && (NrcCode != 0x13) && (NrcCode != 0x7E) && (NrcCode != 0x7F) && (NrcCode != 0x31))
            {
                NegRes.code = NrcCode;
                UDS_Service_Response(si, NEGATIVE_RSP, DIAG_ID_Tx, 1, (uint8_t *)(&NegRes.code));
                return;
            }
        }
        else
        {
            NegRes.code = NrcCode;
            UDS_Service_Response(si, NEGATIVE_RSP, DIAG_ID_Tx, 1, (uint8_t *)(&NegRes.code));
            return;
        }
    }
}

/******************************************************************************
 85# - ControlDTCSetting
 ******************************************************************************/
static const uint8_t Ser85BanResponse          = AbleResponseType;
static const uint8_t Ser85Sub []               = {DTCSettingOn, DTCSettingOff};
static const uint8_t Ser85MinLen []            = {1};
static const uint8_t Ser85SubLen []            = {1, 1};
static const uint8_t Ser85UnlockLevel []       = {NoNeed, NoNeed};
static const uint8_t Ser85SupportSession       = ProgrammingMode | ExtendedDiagnosticMode;
static const uint8_t Ser85SubSupportSession [] = {ProgrammingMode | ExtendedDiagnosticMode};
void UDS_Service_85_Indication(uint16_t A_TA_type, uint16_t A_Length, uint8_t *A_Data)
{
    uint8_t i;
    uint8_t si = ControlDTCSetting;
    uint8_t NrcCode = 0;
    uint8_t Index = 0;
    uint8_t SubFunction = 0;
    uint16_t Service85DiagDataLength = A_Length;
    uint16_t FunorPhy = A_TA_type;

    for (i = 0; i < Service85DiagDataLength; ++i)
    {
        UDS_ISO14229_Transfer[i] = *(A_Data + i);
    }

    if (0 == NrcCode)
    {
        NrcCode = SerXXNRC7FFilter(Ser85SupportSession);
    }

    if (0 == NrcCode)
    {
        NrcCode = SerXXNRC13Filter(MinLengthType, Service85DiagDataLength, Ser85MinLen, Index);
    }

    if (0 == NrcCode)
    {
        if (AbleResponseType == Ser85BanResponse)
        {
            SubFunction = UDS_ISO14229_Transfer[0] & 0x7F;
        }
        else
        {
            SubFunction = UDS_ISO14229_Transfer[0];
        }
        NrcCode = SerXXNRC12Filter(SubFunction, Ser85Sub, sizeof(Ser85Sub), &Index);
    }

    if (0 == NrcCode && Index < sizeof(Ser85UnlockLevel))
    {
        NrcCode = SerXXNRC33Filter(Ser85UnlockLevel, Index);
    }

    if (0 == NrcCode && (Index < sizeof(Ser85SubSupportSession)))
    {
        NrcCode = SerXXNRC7EFilter(Ser85SubSupportSession, Index);
    }

    if (0 == NrcCode && (Index < sizeof(Ser85SubLen)))
    {
        NrcCode = SerXXNRC13Filter(SubLengthType, Service85DiagDataLength, Ser85SubLen, Index);
    }

    if (0 == NrcCode)
    {
        NrcCode = SerXXNRC22Filter();
    }

    if (0 == NrcCode)
    {
        if (DTCSettingOn == SubFunction)
        {
            // CAN_DTC_OFF = 0;
        }
        else if (DTCSettingOff == SubFunction)
        {
            // CAN_DTC_OFF = 1;
        }

        if (AbleResponseType == Ser85BanResponse)
        {
            if (bit_is_clear(UDS_ISO14229_Transfer[0], 7))
            {
                UDS_Service_Response(si, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer);
            }
        }
        else
        {
            UDS_Service_Response(si, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer);
        }
    }
    else
    {
        if (DIAG_ID_Rx_FUN == FunorPhy)
        {
            if ((NrcCode != 0x11) && (NrcCode != 0x12) && (NrcCode != 0x13) && (NrcCode != 0x7E) && (NrcCode != 0x7F) && (NrcCode != 0x31))
            {
                NegRes.code = NrcCode;
                UDS_Service_Response(si, NEGATIVE_RSP, DIAG_ID_Tx, 1, (uint8_t *)(&NegRes.code));
                return;
            }
        }
        else
        {
            NegRes.code = NrcCode;
            UDS_Service_Response(si, NEGATIVE_RSP, DIAG_ID_Tx, 1, (uint8_t *)(&NegRes.code));
            return;
        }
    }
}

/*************IGN on,S3 Timeout****************/
void DIAG_InitParameter(void)
{
    S3_ServerCnt = 0;
    S3_ServerEN  = 0;
    SessionType  = DefaultSession;

    DiagLockFlag            = 0;
    RANDOM                  = 1573;
    Services27_01_Requested = 0;
}