/******************************************************************************
文 件 名;UDS_ISO14229_Services.c
功能描述;ISO 14229 规范规定的诊断服务服务器端服务实现函数库文件
作    者;张暄
版    本;V1.0
日    期;2016.11.1
******************************************************************************/
#include "UDS_ISO14229_Services.h"
//#include "mc9s12xhy128.h"
#include "comdef.h"
#include "Non_volatile_Memorys.h"
#include "CAN_Communication_Matrix.h"
#include "System_Status_Monitor.h"
#include "GUI.h"
#include "Data_Processor.h"

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


static uint8_t Send_Command[] =
{
    0x1C, 0x01, 0x06, 0x80, 0x1F, 0x01, 0x06, 0x80, 0xFB, 0x3D
};

typedef struct
{
    uint8_t code[15];             /* Structure required to copy code to ram memory */

} FnCmdInRamStruct;

typedef void ( * near pFnCmdInRam ) ( void );



#pragma CODE_SEG __NEAR_SEG NON_BANKED

static void FnCmdInRam_ ( void )
{
    FSTAT = 0x80U;                       /* Clear flag command buffer empty */

    while ( FSTAT_CCIF == 0U ) {}        /* Wait to command complete */

    return;
}

#pragma MESSAGE DISABLE C1805          /* Disable warning C1805 "Non-standard conversion used" */
/*lint -save  -e740 -e931 Disable MISRA rule (1.2) checking. */
static void CallFnCmdInRam()
{
    FnCmdInRamStruct FnCmdInRam = * ( FnCmdInRamStruct * ) ( FnCmdInRam_ ); /* Create a copy of Wait in RAM routine on stack */
    ( ( pFnCmdInRam ) &FnCmdInRam ) ();  /* Call code in RAM */
    return;
}

#pragma CODE_SEG DEFAULT

extern NVMConfigF110Union    NVMConfigF110;
extern NVMConfigF101Union    NVMConfigF101;
extern NVMUserSettingsUnion  NVMUserSettings;
INT8U EraserFlag;
INT8U EraseMemoryFF[128];
INT8U  APPFLAG;
DiagSendDataRes PosRes;
DiagSendDataNeg NegRes;
uint8_t S3_ServerEN;        // 扩展会话5s定时器使能开关
uint8_t S3_ServerCnt;       //扩展会话定时器计数器
uint8_t S3_Server_refresh;  //刷新扩展会话S3时间标志
uint16_t S19DataCntOfReceive; //S19数据的接收总长度
SRecord_t srcd;
//uint16_t CheckSum;
uint8_t BlockSCT;   //BlockSequenceCounter   (1,2,...255,0)
uint8_t REC_DLlen;  //已经接收到的数据个数(多少字节)
uint8_t  DTCControlType;   //DTC记录控制开关  0
uint8_t BlocKCnt;           //S19 传输的block数目
uint8_t RequestTransfer;   //请求下载的标志
//uint32_t SectorAddress;     //扇区地址
uint32_t EraseAddress;      //擦除地址
uint8_t ControlType;       // 通信控制的类型
uint8_t CommunicationType; // 通信控制的类型
uint8_t Wait10s;           //27服务尝试失败需要等待10秒
uint8_t wait10cnt = 0;
uint8_t Wait10sFlag;        //等待10秒标志
uint8_t DiagLockFlag;       //27服务解锁标志
//uint8_t AttemptCnt;         //27服务尝试次数
//uint8_t RequestSeedCnt;      //请求种子的次数
uint8_t SeedkeyValidFlag;    //置1 表示秘钥生效
uint32_t ValidSeedKey;       //表示有效的秘钥
uint16_t SeedHigh;
uint16_t SeedLow;
uint8_t Services27_01_Requested;
uint8_t Services27_11_Requested;
uint8_t Time10sLock;
uint16_t EngineSpeed;       //发动机转速
uint8_t MotorPosition;      //电机位置
uint8_t DisplayColor;       //需要显示的颜色
uint8_t ControlMotor;       //需要控制的是哪个电机

uint16_t ESpeedValNUM;         //转速
uint16_t VspeedValNUM;         //车速
uint16_t ESpeedVal;         //转速
uint16_t VspeedVal;         //车速
uint8_t TempSeg;            //水温
uint16_t FuelSeg;            //燃油

uint8_t BuzzerStatus;       //蜂鸣器状态
uint8_t LampWarningStatus;  //灯光状态
uint8_t SessionType;        //会话类型
uint8_t IO_Group_1;
uint8_t IO_Group_2;
uint8_t IO_Group_3;
uint8_t IO_Group_4;
uint8_t IO_MASK_1;
uint8_t IO_MASK_2;
uint8_t IO_MASK_3;
uint8_t IO_MASK_4;
uint8_t  Error;
uint32_t DFLASHeep_adr;
INT16U MYeturnCode;
uint8_t seed[4];//用于27服务存放种子可以为局部变量
uint32_t RANDOM;//用于随机数计算  必须为全局变量
const uint8_t XorArray[4] = {0x31, 0x23, 0x56, 0x71};
uint8_t UDS_ISO14229_Transfer[250];

uint8_t  ContrlorResumeofBuzzer;  //蜂鸣器控制
uint8_t  ContrlorResumeofGauge;   //所有表头控制
uint8_t  ContrlorResumeofLCD;     //LCD控制
uint8_t  ContrlorResumeofLamp;    //所有报警灯测试
uint8_t  ContrlorResumeofEspeed;  //转速控制
uint8_t  ContrlorResumeofVspeed;  //车速控制
uint8_t  ContrlorResumeofIO;      //IO控制
uint8_t  ContrlorResumeofIOLast;      //IO控制
uint8_t  ContrlorResumeofFUEL;    //燃油表控制
uint8_t  ContrlorResumeofTEMP;    //水温表控制
uint8_t  CAN_NORMAL_TT;
uint8_t  CAN_NORMAL_2SFF;
uint8_t  CAN_DTC_OFF;
uint8_t   ACC_OK_FF;
DiagReceiveData diag;
Diag27Data  diag27;
DiagReceiveData1 diag1;
DiagReceiveData2 config;
uint8_t UPDOWNFLAG;
uint8_t DTCstatusCHGFF;
uint8_t CJINF[18];		//存储厂家信息(18位ASSIC字符串)(保存在EEPROM但不备份)
uint8_t DTC_STATUS[cnDTCslen];
RecDTC_WaitBusoff_STRUCT	RecDTC_Wait_NoBusoff[cnDTCslen];
INT8U HIGH_VOLTAGE;
INT8U LOW_VOLTAGE;
INT8U BUS_OFF_FLAG;
INT16U VoltageAbnormalTimer;
INT8U VoltageACKFF;

extern INT8U  GUIIconSeatbeltRStatus;
extern INT8U  GUIIconDRLGStatus;
extern INT8U  GUIIconSVSYStatus;
extern INT8U  GUIIconHiTempRStatus;
extern INT8U  GUIIconLowFuelYStatus;
extern INT8U  GUIIconTirePressureYStatus;
extern INT8U  GUIIconEPBYStatus ;
extern INT8U  GUIIconCruiseGStatus  ;
extern INT8U  GUIIconEBDYStatus  ;
extern INT8U  GUIIconEPSYStatus  ;
extern INT8U  GUIIconCruiseStateGStatus  ;
extern INT8U  GUIIconTCUYStatus  ;
extern INT8U  GUIIconLowBatteryRStatus ;
extern INT8U  GUIIconESCOffYStatus;
extern INT8U  GUIIconESCYStatus;
extern INT8U  GUIIconHDCYStatus;
extern INT8U  GUIIconHDCGStatus;
extern INT8U  GUIIconAVHGStatus;
extern INT8U  GUIIconAVHRStatus;
extern INT8U  GUIIconDRLGStatus;
extern INT8U  GUIIconABSYStatus;
extern INT8U  GUIIconBrakeRStatus;
/*****************************************************************
FlashDriver.c
********************************************************************/
uint32_t ChangeLocalAddressToGlobalAddress ( uint32_t address )
{
    if ( ( 0xF08000 <= address ) && ( address <= 0xF0BFFF ) )
    {
        address = address - 0xF08000 + 0x7C0000;
        return address;
    }

    if ( ( 0xF18000 <= address ) && ( address <= 0xF1BFFF ) )
    {
        address = address - 0xF18000 + 0x7C4000;
        return address;
    }

    if ( ( 0xF28000 <= address ) && ( address <= 0xF2BFFF ) )
    {
        address = address - 0xF28000 + 0x7C8000;
        return address;
    }

    if ( ( 0xF38000 <= address ) && ( address <= 0xF3BFFF ) )
    {
        address = address - 0xF38000 + 0x7CC000;
        return address;
    }

    if ( ( 0xF48000 <= address ) && ( address <= 0xF4BFFF ) )
    {
        address = address - 0xF48000 + 0x7D0000;
        return address;
    }

    if ( ( 0xF58000 <= address ) && ( address <= 0xF5BFFF ) )
    {
        address = address - 0xF58000 + 0x7D4000;
        return address;
    }

    if ( ( 0xF68000 <= address ) && ( address <= 0xF6BFFF ) )
    {
        address = address - 0xF68000 + 0x7D8000;
        return address;
    }

    if ( ( 0xF78000 <= address ) && ( address <= 0xF7BFFF ) )
    {
        address = address - 0xF78000 + 0x7DC000;
        return address;
    }

    if ( ( 0xF88000 <= address ) && ( address <= 0xF8BFFF ) )
    {
        address = address - 0xF88000 + 0x7E0000;
        return address;
    }

    if ( ( 0xF98000 <= address ) && ( address <= 0xF9BFFF ) )
    {
        address = address - 0xF98000 + 0x7E4000;
        return address;
    }

    if ( ( 0xFA8000 <= address ) && ( address <= 0xFABFFF ) )
    {
        address = address - 0xFA8000 + 0x7E8000;
        return address;
    }

    if ( ( 0xFB8000 <= address ) && ( address <= 0xFBBFFF ) )
    {
        address = address - 0xFB8000 + 0x7EC000;
        return address;
    }

    if ( ( 0xFC8000 <= address ) && ( address <= 0xFCBFFF ) )
    {
        address = address - 0xFC8000 + 0x7F0000;
        return address;
    }

    if ( ( ( address >= 0x4000 ) && ( address <= 0x7FFF ) ) || ( ( address >= 0xC000 ) && ( address <= 0xFFFF ) ) )
    {
        address = address | 0x7F0000; //7F E7FF
        return address;                                  //FFF8
    }

    if ( ( 0xFE8000 <= address ) && ( address <= 0xFEBFFF ) )
    {
        address = address - 0xFE8000 + 0x7F8000;
        return address;
    }

    return 0;
}
/******************************************************************************
Get Current Sector
******************************************************************************/
uint8_t GET_Global_sector_PRG ( uint32_t address )
{
    uint8_t m8;
    //PAGE_F8       0xF88000 TO 0xF89FFF; //8K     7E0000 - 7E1FFF  (0--7)
    //PAGE_F9       0xF98000 TO 0xF9BFFF; //16K    7E4000 - 7E7FFF  (16--31)
    //PAGE_FA       0xFA8000 TO 0xFABFFF; //16K    7E8000 - 7EBFFF  (32--47)
    //PAGE_FB       0xFB8000 TO 0xFBBFFF; //16K    7EC000 - 7EFFFF  (48--63)
    //PAGE_FC       0xFC8000 TO 0xFCBFFF; //16K    7F0000 - 7F3FFF  (64--79)
    //PAGE_FD       0xFD8000 TO 0xFDBFFF; //16K    7F4000 - 7F7FFF  (80--95)    equivalent to ROM_4000
    //PAGE_FE       0xFE8000 TO 0xFEBFFF; //16K    7F8000 - 7FBFFF  (96--111)
    //PAGE_FF       0xFF8000 TO 0xFFBFFF; //15K    7FC000 - 7FFBFF  (112--126)  equivalent to ROM_C000
    m8 = ( uint8_t ) ( ( address - 0x7C0000 ) >> 10 );
    return m8;
}

INT8U  D_P_FLASH_Erase_Sector ( INT8U mTYPE, INT32U  address )
{
    INT16U code;
    INT32U m32;

    if ( mTYPE == 0 )
    {
        code = 0x1200;
        m32 = 0xFFFFFFFE;
    }
    else
    {
        code = 0x0A00;
        //m32=0x00000007;
        m32 = 0xFFFFFFF8;
    }

    address = address & m32;

    while ( FSTAT_CCIF == 0 ); //wait if command in progress

    FSTAT = 0x30;             //clear ACCERR and PVIOL
    FCCOBIX = 0x00;
    FCCOB = code | ( ( address & 0x007F0000 ) >> 16 );   //0x00030000
    FCCOBIX = 0x01;
    FCCOB = ( INT16U ) address;
    cli();
    CallFnCmdInRam();//asm JSR Send_Command;   //equal to    FSTAT_CCIF=1;   while(FSTAT_CCIF==0)
    sei();

    if ( ( FSTAT & ( FSTAT_ACCERR_MASK | FSTAT_FPVIOL_MASK ) ) != 0 )
        return ACCESS_ERROR;

    if ( FSTAT_MGSTAT != 0 )
        return VERIFICATION_FAILED;

    return OK;
}
//==============================================================================
//DFLASH/PFLASH Program
//@param D_P_FlashSelect    1  D_Flash    0 P_Flash
//@param address            Global_Address
//@param ptr                Data  which will be write to the Global Address
//@param number_of_words    Number of words will be write to P_D_Flash
//==============================================================================
INT8U  D_P_FLASH_Program ( INT8U mTYPE, INT32U  address, INT16U *ptr, INT8U number_of_words )
{
    INT8U i;
    INT16U code;
    INT32U m32;

    if ( mTYPE == 0 )
    {
        code = 0x1100;
        m32 = 0x00000001;
    }
    else
    {
        code = 0x0600;
        m32 = 0x00000007;
    }

    if ( ( address & m32 ) != 0 )
        return MISALIGNED_ADDRESS;

    while ( FSTAT_CCIF == 0 ); //wait if command in progress

    FSTAT = 0x30;             //clear ACCERR and PVIOL
    FCCOBIX = 0x00;
    FCCOB = code | ( ( address & 0x007F0000 ) >> 16 ); //0x00030000
    FCCOBIX = 0x01;
    FCCOB = ( INT16U ) address;

    for ( i = 1; i <= number_of_words; i++ ) //fill appropriate number of words to FCCOB
    {
        FCCOBIX = i + 1;
        FCCOB = *ptr;
        ptr++;
    }

    cli();								//我加的
    CallFnCmdInRam();//asm JSR Send_Command;   //equal to    FSTAT_CCIF=1;   while(FSTAT_CCIF==0)asm JSR Send_Command;
    sei();								//我加的

    if ( ( FSTAT & ( FSTAT_ACCERR_MASK | FSTAT_FPVIOL_MASK ) ) != 0 )
        return ACCESS_ERROR;

    return OK;
}
//==============================================================================
//DFLASH_Erase_Verify_Section
//
//入口:address 为全局地址, number_of_words 为要校验的字单元个数
//==============================================================================
INT8U DFLASH_Erase_Verify_Section ( INT32U address, INT16U number_of_words )
{
    if ( ( address & 0x00000007 ) != 0 )
        return MISALIGNED_ADDRESS;

    while ( FSTAT_CCIF == 0 ); //wait if command in progress

    FSTAT = 0x30;             //clear ACCERR and PVIOL
    FCCOBIX = 0x00;
    FCCOB = 0x1000 | ( ( address & 0x00FF0000 ) >> 16 );
    FCCOBIX = 0x01;
    FCCOB = ( address & 0x0000FFFF );
    FCCOBIX = 0x02;
    FCCOB = number_of_words;
    cli();
    CallFnCmdInRam();//asm JSR Send_Command;   //equal to    FSTAT_CCIF=1;   while(FSTAT_CCIF==0)asm JSR Send_Command;   //equal to    FSTAT_CCIF=1;   while(FSTAT_CCIF==0)
    sei();

    if ( ( FSTAT & ( FSTAT_ACCERR_MASK | FSTAT_FPVIOL_MASK ) ) != 0 )
        return ACCESS_ERROR;

    if ( FSTAT_MGSTAT != 0 )
        return NON_ERASED;
    else
        return ERASED;
}

//==============================================================================
//DFLASH/PFLASH_N_Program              2015-5-13
//@param address            Global_Address
//@param ptr                Data  which will be write to the Global Address
//@param number_of_words    Number of words will be write to P_D_Flash
//@param return             Result of Program P Flash
//==============================================================================
uint8_t  PP_FLASH_N_Program ( uint32_t  address, uint8_t *ptr, uint8_t number_of_bytes )
{
    INT8U mRUL;
    INT8U  i;
    INT8U  buff[8];

    if ( number_of_bytes == 0 )
        return  OK;

    for ( ;; )
    {
        __RESET_WATCHDOG();

        if ( number_of_bytes >= 8 )
        {
            mRUL = D_P_FLASH_Program ( 1, address, ( INT16U * ) ptr, 4 );

            if ( mRUL != OK )
                return mRUL;

            ptr = ptr + 8;
            address = address + 8;
            number_of_bytes = number_of_bytes - 8;
        }
        else
        {
            if ( number_of_bytes == 0 )
                return  OK;

            for ( i = 0; i < 8; i++ )
                buff[i] = 0xFF;

            for ( i = 0; i < number_of_bytes; i++ )
                buff[i] = ptr[i];

            mRUL = D_P_FLASH_Program ( 1, address, ( INT16U * ) buff, 4 );
            return mRUL;
        }
    }
}

uint32_t DFLASH_Read_Word ( INT32U  address )
{
    INT16U data16;
    EPAGE = 0;
    data16 = ( * ( volatile unsigned short * ) ( address - ( DFLASH_FirstAddress - 0x800 ) ) );
    return data16;
}

INT8U  DFLASH_Program ( INT32U  address, INT16U *ptr, INT8U number_of_words )
{
    unsigned int i;

    if ( ( number_of_words < 1 ) || ( number_of_words > 4 ) )
        return LENGTH_OUT_OF_RANGE;

    //check if address is aligned (global address [0] != 0)
    if ( ( address & 0x00000001 ) != 0 )
        return MISALIGNED_ADDRESS;

    //check if the word(s) is/are erased
    if ( ( DFLASH_Erase_Verify_Section ( address, number_of_words ) ) == NON_ERASED )
        return NON_ERASED;

    while ( FSTAT_CCIF == 0 ); //wait if command in progress

    FSTAT = 0x30;             //clear ACCERR and PVIOL
    FCCOBIX = 0x00;
    FCCOB = 0x1100 | ( ( address & 0x007F0000 ) >> 16 ); //0x00030000
    FCCOBIX = 0x01;
    FCCOB = ( address & 0x0000FFFF );

    for ( i = 1; i <= number_of_words; i++ ) //fill appropriate number of words to FCCOB
    {
        FCCOBIX = i + 1;
        FCCOB = *ptr;
        ptr++;
    }

    cli();
    CallFnCmdInRam();//asm JSR Send_Command;   //equal to    FSTAT_CCIF=1;   while(FSTAT_CCIF==0)asm JSR Send_Command;   //equal to    FSTAT_CCIF=1;   while(FSTAT_CCIF==0)
    sei();

    if ( ( FSTAT & ( FSTAT_ACCERR_MASK | FSTAT_FPVIOL_MASK ) ) != 0 )
        return ACCESS_ERROR;

    if ( FSTAT_MGSTAT != 0 )
        return VERIFICATION_FAILED;

    return OK;
}


INT8U  DFLASH_Erase_Sector ( INT32U  address )
{
    //size of sector is 256 BYTE
    //check if address is aligned (global address [0] != 0)
    if ( ( address & 0x00000001 ) != 0 )
        return MISALIGNED_ADDRESS;

    while ( FSTAT_CCIF == 0 ); //wait if command in progress

    FSTAT = 0x30;             //clear ACCERR and PVIOL
    FCCOBIX = 0x00;
    FCCOB = 0x1200 | ( ( address & 0x007F0000 ) >> 16 );   //0x00030000
    //FCCOB =0x1210;
    FCCOBIX = 0x01;
    FCCOB = ( unsigned int ) address;
    cli();
    CallFnCmdInRam();//asm JSR Send_Command;   //equal to    FSTAT_CCIF=1;   while(FSTAT_CCIF==0)asm JSR Send_Command;   //equal to    FSTAT_CCIF=1;   while(FSTAT_CCIF==0)
    sei();

    if ( ( FSTAT & ( FSTAT_ACCERR_MASK | FSTAT_FPVIOL_MASK ) ) != 0 )
        return ACCESS_ERROR;

    if ( FSTAT_MGSTAT != 0 )
        return VERIFICATION_FAILED;

    return OK;
}

//==============================================================================
//DFLASH_N_Program
//
//入口:address 为全局地址
//
//     1<=number_of_words<=128
//==============================================================================
INT8U  DFLASH_N_Program ( INT32U  address, INT16U *ptr, INT8U number_of_words )
{
    INT8U mRUL;

    for ( ;; )
    {
        __RESET_WATCHDOG();

        if ( number_of_words >= 4 )
        {
            mRUL = DFLASH_Program ( address, ptr, 4 );

            if ( mRUL != OK )
                return mRUL;

            ptr = ptr + 4;
            address = address + 8;
            number_of_words = number_of_words - 4;
        }
        else
        {
            if ( number_of_words == 0 )
                return  OK;

            mRUL = DFLASH_Program ( address, ptr, number_of_words );
            return mRUL;
        }
    }
}

//====================================================
//从FLASH中读N个字节数据到RAM   (mLEN必须为偶数)
//====================================================
void ReadNbyteFrDFalsh ( INT16U *mP, INT16U mLEN )
{
    INT8U i;
    INT8U k;
    __RESET_WATCHDOG();    //复位看门狗定时器
    k = mLEN / 2;

    for ( i = 0; i < k; i++ )
    {
        mP[i] = DFLASH_Read_Word ( DFLASHeep_adr );
        DFLASHeep_adr = DFLASHeep_adr + 2;
    }
}
//====================================================
//写N个字节数据到FLASH   (mLEN必须为偶数)
//====================================================
void WriteNbyteToDFalsh ( INT16U *mP, INT16U mLEN )
{
    __RESET_WATCHDOG();    //复位看门狗定时器
    MYeturnCode = DFLASH_N_Program ( DFLASHeep_adr, mP, mLEN / 2 );
    DFLASHeep_adr = DFLASHeep_adr + mLEN;
}

//读取改写APP部分程序标志位
void WRITE_BZ_inDFLAH2Sector ( INT16U bz )
{
    if ( DFLASH_Erase_Sector ( DFLASH_APPvAddress ) != OK )
        return;

    DFLASH_Program ( DFLASH_APPvAddress, &bz, 1 );
}

//========================================================
//从FLASH中读诊断协议2E内容参数到RAM
//========================================================
void Read_2E_DATA_FROM_FLASH ( void )
{
    INT8U i;

    if ( DFLASH_Read_Word ( DFLASH_DiagAddress ) != 0x7AA7 )
    {
       for ( i = 0; i < 8; i++ )
            diag.F112[i] = 0x00;

        //-----------------------
        for ( i = 0; i < 17; i++ )
            diag.F190[i] = 0x00;    

        //-----------------------
        for ( i = 0; i < 8; i++ )
            diag.F197[i] = 0x00;

        //-----------------
        for ( i = 0; i < 16; i++ )
            diag.F198[i] = 0x00;

        //-------------------
        diag.F199[0] = 0x20;
        diag.F199[1] = 0x00;
        diag.F199[2] = 0x01;
        diag.F199[3] = 0x01;
        //-----------------
        diag.F19D[0] = 0x20;
        diag.F19D[1] = 0x00;
        diag.F19D[2] = 0x01;
        diag.F19D[3] = 0x01;
        diag.attemptcnt = 0;
    }
    else
    {
        DFLASHeep_adr = DFLASH_DiagAddress + 2;
        ReadNbyteFrDFalsh ( ( INT16U * ) &diag, sizeof ( diag ) );
    }
}
//========================================================
//写 诊断协议2E内容 参数到FALSH
//========================================================
void Write_2E_DATA_TO_FLASH ( void )
{
    INT16U m16;
    MYeturnCode = DFLASH_Erase_Sector ( DFLASH_DiagAddress );							//擦除第3个扇区(256字节)

    if ( MYeturnCode != OK )
        return;

    DFLASHeep_adr = DFLASH_DiagAddress + 2;
    WriteNbyteToDFalsh ( ( INT16U * ) &diag, sizeof ( diag ) );
    m16 = 0x7AA7;
    MYeturnCode = DFLASH_N_Program ( DFLASH_DiagAddress, &m16, 1 );
}

//========================================================
//从FLASH中读诊断协议2E内容参数到RAM
//========================================================
void Read_BACKUP_DATA_FROM_FLASH ( void )
{
    INT8U i;

    if ( DFLASH_Read_Word ( DFLASH_BackupAddress ) != 0x7AA7 )
    {
        for ( i = 1; i < 64; i++ )
            diag1.DFEC[i] = 0x00;

        diag1.DFEC[0] = 0x20;

        //-----------------------
        for ( i = 1; i < 64; i++ )
            diag1.DFED[i] = 0x00;

        diag1.DFED[0] = 0x20;

        //-----------------------
        for ( i = 1; i < 64; i++ )
            diag1.DFEE[i] = 0x00;

        diag1.DFEE[0] = 0x20;

        //-----------------
        for ( i = 1; i < 64; i++ )
            diag1.DFEF[i] = 0x00;

        diag1.DFEF[0] = 0x20;
        //-------------------
    }
    else
    {
        DFLASHeep_adr = DFLASH_BackupAddress + 2;
        ReadNbyteFrDFalsh ( ( INT16U * ) &diag1, sizeof ( diag1 ) );
    }
}
//========================================================
//写 诊断协议2E内容 参数到FALSH
//========================================================
void Write_BACKUP_DATA_TO_FLASH ( void )
{
    INT16U m16;
    INT16U MYeturnCode;
    MYeturnCode = DFLASH_Erase_Sector ( DFLASH_BackupAddress );							//擦除第3个扇区(256字节)

    if ( MYeturnCode != OK )
        return;

    DFLASHeep_adr = DFLASH_BackupAddress + 2;
    WriteNbyteToDFalsh ( ( INT16U * ) &diag1, sizeof ( diag1 ) );
    m16 = 0x7AA7;
    MYeturnCode = DFLASH_N_Program ( DFLASH_BackupAddress, &m16, 1 );
}
//========================================================
//从FLASH中读诊断协议2E内容参数到RAM
//========================================================
void Read_CONFIG_DATA_FROM_FLASH ( void )
{
    INT8U i;

    if ( DFLASH_Read_Word ( DFLASH_USERAddress ) != 0x7AA7 )
    {
        config.F101[0] = 0xF8;
        config.F101[1] = 0xAC;
        config.F101[2] = 0x00;
        config.F101[3] = 0x00;
        config.F101[4] = 0x00;
        config.F101[5] = 0x00;
        config.F101[6] = 0x00;
        config.F101[7] = 0x00;
        //-----------------------
        config.F110[0] = 0x03;
        config.F110[1] = 0xA8;
        config.F110[2] = 0x00;
        config.F110[3] = 0x00;
        config.F110[4] = 0x58;
        config.F110[5] = 0x5C;
        config.F110[6] = 0x0F;
        config.F110[7] = 0x00;
    }
    else
    {
        DFLASHeep_adr = DFLASH_USERAddress + 2;
        ReadNbyteFrDFalsh ( ( INT16U * ) &config, sizeof ( config ) );
    }
}
//========================================================
//写 诊断协议2E内容 参数到FALSH
//========================================================
void Write_CONFIG_DATA_TO_FLASH ( void )
{
    INT16U m16;
    INT16U MYeturnCode;
    MYeturnCode = DFLASH_Erase_Sector ( DFLASH_USERAddress );							//擦除第3个扇区(256字节)

    if ( MYeturnCode != OK )
        return;

    DFLASHeep_adr = DFLASH_USERAddress + 2;
    WriteNbyteToDFalsh ( ( INT16U * ) &config, sizeof ( config ) );
    m16 = 0x7AA7;
    MYeturnCode = DFLASH_N_Program ( DFLASH_USERAddress, &m16, 1 );
}


//========================================================
//从FLASH中读诊断协议27内容参数到RAM
//========================================================
void Read_27_DATA_FROM_FLASH ( void )
{
    INT8U i;

    if ( DFLASH_Read_Word ( DFLASH_LockAddress ) != 0x7AA7 )
    {
        diag27.attemptcnt = 0;
        diag27.RequestSeedCnt = 0;
        diag27.attemptcnt11 = 0;
        diag27.RequestSeedCnt11 = 0;
    }
    else
    {
        DFLASHeep_adr = DFLASH_LockAddress + 2;
        ReadNbyteFrDFalsh ( ( INT16U * ) &diag27, sizeof ( diag27 ) );
    }
}
//========================================================
//写 诊断协议2E内容 参数到FALSH
//========================================================
void Write_27_DATA_TO_FLASH ( void )
{
    INT16U m16;
    MYeturnCode = DFLASH_Erase_Sector ( DFLASH_LockAddress );							//擦除第3个扇区(256字节)

    if ( MYeturnCode != OK )
        return;

    DFLASHeep_adr = DFLASH_LockAddress + 2;
    WriteNbyteToDFalsh ( ( INT16U * ) &diag27, sizeof ( diag27 ) );
    m16 = 0x7AA7;
    MYeturnCode = DFLASH_N_Program ( DFLASH_LockAddress, &m16, 1 );
}
/******************************************************************************
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;
******************************************************************************/

/******************************************************************************
每一个服务由三个函数实现;

 1. 函数名; <service_name>_Indication
             <service_name> 统一命名为 UDS_Service_<SI>
    功  能;应用层通过该函数向诊断服务实现函数请求服务并将相关数据传送给诊断服
            务实现函数
            The indication primitive is used by the application layer, to
            indicate an internal event which is significant to the ECU
            diagnostic application and pass data about the requested diagnostic
            service to the server function of the ECU diagnostic application.
    参  数;A_TA_type ;源地址类型,物理寻址或功能寻址
            A_Length  ;传输的数据的长度
            A_Data    ;传输的数据
    返回值;无

 2. 函数名; <service_name>_Response
             <service_name> 统一命名为 UDS_Service_<SI>
    功  能;该函数由诊断服务实现函数发起,用于向应用层发送诊断服务函数生成的反
            馈数据
            The response primitive is used by the server function in the ECU
            diagnostic application, to initiate the service and pass response
            data provided by the requested diagnostic service to the
            application layer.
    参  数;RspType   ;反馈类型  POSITIVE_RSP 正反馈
                                  NEGATIVE_RSP 负反馈
            A_TA_type ;目标地址类型,发送数据请使用 DIAG_ID_Tx
            A_Length  ;传输的数据的长度
            A_Data    ;传输的数据
    返回值;无

 3. 函数名; <service_name>_Rsp_Confirm
             <service_name> 统一命名为 UDS_Service_<SI>
    功  能;反馈确认函数由应用层发起,用于向诊断服务实现方告知前一次向应用层发送
            反馈数据的传输结果
            The response-confirm primitive is used by the application layer
            to indicate an internal event, which is significant to the server
            application, and pass communication results of an associated
            previous service response to the server function in the ECU
            application.
    参  数;A_TA_type ;目标地址类型
            A_Result  ;传输结果   A_OK  正确传输
                                   A_ERR 传输失败
    返回值;无
******************************************************************************/
void Randomcnt ( void )
{
    RANDOM++;

    if ( RANDOM >= 0xffff00f0 )
        RANDOM = 0;
}

static void AppExecute ( void )
{
    COPCTL = 0x01;        //enable watchdog
    ARMCOP = 0x00;
    //value written to ARMCOP register different from 0xAA or 0x55 will reset
    //the MCU immediately.
}
void S3_ServerCNT ( void )
{
    EngineSpeed = DATA_ENGINE_SPEED_ACTUAL;

    if ( Wait10sFlag == 1 )
    {
        wait10cnt++;

        if ( wait10cnt >= 100 )
        {
            wait10cnt = 0;
            Wait10s = 1;
        }
    }

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

        S3_ServerCnt++;

        if ( S3_ServerCnt > S3_SERVER ) //S3超时
        {
            S3_ServerCnt = 0;
            S3_ServerEN = 0;
            SessionType = DefaultSession;
            SeedkeyValidFlag = 0;
            DiagLockFlag = 0;
            ContrlorResumeofBuzzer = 0; //蜂鸣器控制
            ContrlorResumeofGauge = 0; //所有表头控制
            ContrlorResumeofLCD = 0;   //LCD控制
            ContrlorResumeofLamp = 0;  //所有报警灯测试
            ContrlorResumeofEspeed = 0; //转速控制
            ContrlorResumeofVspeed = 0; //车速控制
            ContrlorResumeofIO = 0;    //IO控制
            ContrlorResumeofFUEL = 0;  //燃油表控制
            ContrlorResumeofTEMP = 0; //水温表控制
        
            IO_Group_1 = 0;
            IO_Group_2 = 0;
            IO_Group_3 = 0;
            IO_Group_4 = 0;
            IO_MASK_1 = 0;
            IO_MASK_2 = 0;
            IO_MASK_3 = 0;
            IO_MASK_4 = 0;
            Com_RxStart();
            Com_TxStart();
            DiagnosticFlag = 0;
        }
    }
}

/******************************************************************************
10# - DiagnosticSessionControl
******************************************************************************/
void UDS_Service_10_Indication ( uint16_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    uint16_t i;
    uint8_t  Service10DiagDataLength;
    uint16_t  FunorPhy;
    FunorPhy = A_TA_type;
    UDS_ISO14229_Transfer[1] = 0;
    UDS_ISO14229_Transfer[2] = 50;
    UDS_ISO14229_Transfer[3] = 0;
    UDS_ISO14229_Transfer[4] = 200;
    NegRes.code  = 0;
    Service10DiagDataLength = A_Length;
    S3_ServerCnt = 0;

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

    if ( Service10DiagDataLength != 1 )
    {
        NegRes.code = incorrectMessageLength; //返回 ConditionsNotCorrect
        UDS_Service_Response ( 0x10, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    if ( Service10DiagDataLength == 1 )
    {
        if ( ( UDS_ISO14229_Transfer[0] == 0x01 ) || ( UDS_ISO14229_Transfer[0] == 0x81 ) )
        {
            DiagLockFlag = 0;
            S3_ServerEN = 0;
            Services27_01_Requested = 0;
            Services27_11_Requested = 0;
            SessionType = DefaultSession;

            if ( UDS_ISO14229_Transfer[0] == 0x01 )
                UDS_Service_Response ( 0x10, POSITIVE_RSP, DIAG_ID_Tx, 5, UDS_ISO14229_Transfer );

            return;
        }
        else if ( ( UDS_ISO14229_Transfer[0] == 0x02 ) || ( UDS_ISO14229_Transfer[0] == 0x82 ) )
        {
            if ( ( SessionType == ExtendedDiagnosticSession ) || ( SessionType == ProgrammingSession ) ) //当前在扩展会话模式下  可以执行
            {
                S3_ServerCnt = 0;
                S3_ServerEN = 1;
                Services27_01_Requested = 0;
                Services27_11_Requested = 0;
                SeedkeyValidFlag = 0;
                DiagLockFlag = 0;
                SessionType = ProgrammingSession;

                if ( UDS_ISO14229_Transfer[0] == 0x02 )
                    UDS_Service_Response ( 0x10, POSITIVE_RSP, DIAG_ID_Tx, 5, UDS_ISO14229_Transfer );
            }
            else if ( ( SessionType == DefaultSession ) || ( SessionType == 0 ) ) //如果当前默认会话 不可以执行
            {
                S3_Server_refresh = 1;
                S3_ServerEN = 1;
                NegRes.code = subfunctionNotSupportinActiveSession; //返回 ConditionsNotCorrect
                UDS_Service_Response ( 0x10, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }
        }
        else if ( ( UDS_ISO14229_Transfer[0] == 0x03 ) || ( UDS_ISO14229_Transfer[0] == 0x83 ) )
        {
            if ( SessionType == ProgrammingSession )
            {
                S3_Server_refresh = 1;
                S3_ServerEN = 1;
                NegRes.code = subfunctionNotSupportinActiveSession; //返回 ConditionsNotCorrect
                UDS_Service_Response ( 0x10, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            S3_Server_refresh = 1;
            S3_ServerEN = 1;
            Services27_01_Requested = 0;
            Services27_11_Requested = 0;
            SeedkeyValidFlag = 0;
            DiagLockFlag = 0;
            SessionType = ExtendedDiagnosticSession;

            if ( UDS_ISO14229_Transfer[0] == 0x03 )
                UDS_Service_Response ( 0x10, POSITIVE_RSP, DIAG_ID_Tx, 5, UDS_ISO14229_Transfer );

            return;
            //APPFLAG=DFLASH_Read_Word(DFLASH_APPvAddress);
        }
        else
        {
            if ( FunorPhy == 0x7DF )
                return;

            NegRes.code = subFunctionNotSupported;    //返回sub Function 不支持
            UDS_Service_Response ( 0x10, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
            return;
        }
    }
    else
    {
        NegRes.code = incorrectMessageLength;   //返回长度错误
        UDS_Service_Response ( 0x10, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
}

extern uint8_t    BLPWMDutyCycle;
extern uint8_t GUIIconDRLGStatus;
/*************************************************************************************
诊断服务执行步骤;
  1、获取数据长度，判断数据长度是否有误
  2、获取目标地址类型，判断功能寻址和物理寻址
  3、根据长度获取数据。判断数据是否符合要求
  4、根据数据判断子服务是否符合要求
***************************************************************************************/


//DID读取服务
void UDS_Service_22_Indication ( uint16_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    uint16_t i;
    uint16_t DIDRequest;
    uint8_t  Service22DiagDataLength;
    uint16_t Diag_Id = 0;
    uint16_t  m16;
    Diag_Id = A_TA_type;;
    S3_ServerCnt = 0;

    if ( Diag_Id == DIAG_ID_Rx_FUN ) //不支持功能寻址
    {
        return;
    }

    NegRes.code = 0;
    Service22DiagDataLength = A_Length;

    if ( Service22DiagDataLength != 2 ) //只是数据的长度
    {
        NegRes.code = incorrectMessageLength;
        UDS_Service_Response ( 0x22, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

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

    DIDRequest = ( ( uint16_t ) UDS_ISO14229_Transfer[0] << 8 ) | ( ( uint16_t ) UDS_ISO14229_Transfer[1] );

    switch ( DIDRequest )
    {
        case 0xF112:  //Vehicle Name   ASCII
            UDS_ISO14229_Transfer[2] = diag.F112[0];
            UDS_ISO14229_Transfer[3] = diag.F112[1];
            UDS_ISO14229_Transfer[4] = diag.F112[2];
            UDS_ISO14229_Transfer[5] = diag.F112[3];
            UDS_ISO14229_Transfer[6] = diag.F112[4];
            UDS_ISO14229_Transfer[7] = diag.F112[5];
            UDS_ISO14229_Transfer[8] = diag.F112[6];
            UDS_ISO14229_Transfer[9] = diag.F112[7];
            UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 10, UDS_ISO14229_Transfer );
            break;

        case 0xF180:  //BootSoftwareIdentification   ASCII
            for ( i = 0; i < 16; i++ )
            {
                UDS_ISO14229_Transfer[i + 2] = 0x20;
            }

            UDS_ISO14229_Transfer[2] = 0x46;
            UDS_ISO14229_Transfer[3] = 0x42;
            UDS_ISO14229_Transfer[4] = 0X4C;
            UDS_ISO14229_Transfer[5] = 0X3A;
            UDS_ISO14229_Transfer[6] = 0X30;
            UDS_ISO14229_Transfer[7] = 0X2E;
            UDS_ISO14229_Transfer[8] = 0X30;
            UDS_ISO14229_Transfer[9] = 0X2E;
            UDS_ISO14229_Transfer[10] = 0X30;
            UDS_ISO14229_Transfer[11] = 0X2E;
            UDS_ISO14229_Transfer[12] = 0X30;
            UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 18, UDS_ISO14229_Transfer );
            break;

        case 0xF187:  //vehicleManufacturerSparePartNumber    ASCII
            UDS_ISO14229_Transfer[2] = 0x01;
            UDS_ISO14229_Transfer[3] = 0x73;
            UDS_ISO14229_Transfer[4] = 0X22;
            UDS_ISO14229_Transfer[5] = 0X17;
            UDS_ISO14229_Transfer[6] = 0x20;
            UDS_ISO14229_Transfer[7] = 0X20;
            UDS_ISO14229_Transfer[8] = 0X20;
            UDS_ISO14229_Transfer[9] = 0X20;
            UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 10, UDS_ISO14229_Transfer );
            break;

        case 0xF189:  //vehicleManufacturerECUSoftwareVersionNumber    ASCII
            for ( i = 0; i < 16; i++ )
                UDS_ISO14229_Transfer[i + 2] = 0x20;

            UDS_ISO14229_Transfer[2] = 0x41;
            UDS_ISO14229_Transfer[3] = 0x31;
            UDS_ISO14229_Transfer[4] = 0X30;
            UDS_ISO14229_Transfer[5] = 0X30;
            UDS_ISO14229_Transfer[6] = 0X30;
            UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 18, UDS_ISO14229_Transfer );
            break;

        case 0xF18A:  //systemSupplier     ASCII
            UDS_ISO14229_Transfer[2] = 0x34;
            UDS_ISO14229_Transfer[3] = 0x35;
            UDS_ISO14229_Transfer[4] = 0X31;
            UDS_ISO14229_Transfer[5] = 0X30;
            UDS_ISO14229_Transfer[6] = 0X30;
            UDS_ISO14229_Transfer[7] = 0X33;
            UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 8, UDS_ISO14229_Transfer );
            break;

        case 0xF18C:  //ECUSerialNumber     ASCII
            for ( i = 0; i < 16; i++ )
                UDS_ISO14229_Transfer[i + 2] = 0x00;

            UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 18, UDS_ISO14229_Transfer );
            break;

        case 0xF18B:  //ECUManufacturingDate   BCD
            UDS_ISO14229_Transfer[2] = 0x20;
            UDS_ISO14229_Transfer[3] = 0x17;
            UDS_ISO14229_Transfer[4] = 0X05;
            UDS_ISO14229_Transfer[5] = 0X01;
            UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 6, UDS_ISO14229_Transfer );
            break;

        case 0xF190:  //VIN(Vehicle Information No.)      ASCII
            for ( i = 0; i < 17; i++ )
                UDS_ISO14229_Transfer[i + 2] = diag.F190[i];

            UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 19, UDS_ISO14229_Transfer );
            break;

        case 0xF193:  //SystemSupplierECUHardwareVersionNumber     ASCII
            for ( i = 0; i < 16; i++ )
                UDS_ISO14229_Transfer[i + 2] = 0x20;

            UDS_ISO14229_Transfer[2] = 0X48;
            UDS_ISO14229_Transfer[3] = 0X57;
            UDS_ISO14229_Transfer[4] = 0X3A;
            UDS_ISO14229_Transfer[5] = 0X30;
            UDS_ISO14229_Transfer[6] = 0x2E;
            UDS_ISO14229_Transfer[7] = 0x30;
            UDS_ISO14229_Transfer[8] = 0X2E;
            UDS_ISO14229_Transfer[9] = 0X32;
            UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 18, UDS_ISO14229_Transfer );
            break;

        case 0xF195:  //systemSupplierECUSoftwareInformation      ASCII
            for ( i = 0; i < 16; i++ )
                UDS_ISO14229_Transfer[i + 2] = 0x20;

            UDS_ISO14229_Transfer[2] = 'S';
            UDS_ISO14229_Transfer[3] = 'W';
            UDS_ISO14229_Transfer[4] = ':';
            UDS_ISO14229_Transfer[5] = '0';
            UDS_ISO14229_Transfer[6] = '.';
            UDS_ISO14229_Transfer[7] = '3';
            UDS_ISO14229_Transfer[8] = '.';
            UDS_ISO14229_Transfer[9] = '5';
            UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 18, UDS_ISO14229_Transfer );
            break;

        case 0xF197:  //systemSupplierECUSoftwareInformation      ASCII
            for ( i = 0; i < 8; i++ )
                UDS_ISO14229_Transfer[i + 2] = diag.F197[i];

            UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 10, UDS_ISO14229_Transfer );
            break;

        case 0xF198:  //repairShopCodeOrTesterSerialNumber        ASCII
            for ( i = 0; i < 16; i++ )
                UDS_ISO14229_Transfer[i + 2] = diag.F198[i];

            UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 18, UDS_ISO14229_Transfer );
            break;

        case 0xF199:  //Programming/configuration Date      BCD
            for ( i = 0; i < 4; i++ )
                UDS_ISO14229_Transfer[i + 2] = diag.F199[i];

            UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 6, UDS_ISO14229_Transfer );
            break;

        case 0xF19D:  //ECUInstallationDate      BCD
            for ( i = 0; i < 4; i++ )
                UDS_ISO14229_Transfer[i + 2] = diag.F19D[i];

            UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 6, UDS_ISO14229_Transfer );
            break;

        case 0xF110:  //ConfigurationInformation1       hex
            for ( i = 0; i < 8; i++ )
                UDS_ISO14229_Transfer[i + 2] = NVMConfigF110.Data[i];

            UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 10, UDS_ISO14229_Transfer );
            break;

        case 0xF101:  //ConfigurationInformation2      hex
            for ( i = 0; i < 8; i++ )
                UDS_ISO14229_Transfer[i + 2] = NVMConfigF101.Data[i];

            UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 10, UDS_ISO14229_Transfer );
            break;

        case 0xDF02:  //engine speed      hex
            // m16=EMS_EngineSpeedRPM;
            UDS_ISO14229_Transfer[2] = ( m16 >> 8 ) & 0xFF;
            UDS_ISO14229_Transfer[3] = ( m16 & 0xFF );
            UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 4, UDS_ISO14229_Transfer );
            break;

        case 0xDF01:  //Vehicle Speed    hex
            //    CANMsg068.Sig.S05H  CANMsg068.Sig.S05L
            //                      ABS_ESC_VehicleSpeed
            UDS_ISO14229_Transfer[2] = ( INT8U ) ( ( ABS_ESC_VehicleSpeed >> 8 ) & 0X1F );
            UDS_ISO14229_Transfer[3] = ( INT8U ) ( ABS_ESC_VehicleSpeed & 0xff );
            UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 4, UDS_ISO14229_Transfer );
            break;

        case 0xDF00:  //ECU Supplier voltage     hex
            UDS_ISO14229_Transfer[2] = ANALOG_KL30_VOLTAGE / 100;
            UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 3, UDS_ISO14229_Transfer );
            break;

        case 0x6200:  //Coolant Temperature     hex
            UDS_ISO14229_Transfer[2] = 0; //EMS_EngineCoolantTemperature;
            UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 3, UDS_ISO14229_Transfer );
            break;

        case 0x6203:  //Read Backlight Dimming Level     hex
            UDS_ISO14229_Transfer[2] = PWMDTY6;
            UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 3, UDS_ISO14229_Transfer );
            break;

        case 0x6204:  //odometer     hex       NVM_ODO_VALUE
            UDS_ISO14229_Transfer[2] = ( uint8_t ) ( ( NVM_ODO_VALUE / 10 ) >> 24 );
            UDS_ISO14229_Transfer[3] = ( uint8_t ) ( ( NVM_ODO_VALUE / 10 ) >> 16 );
            UDS_ISO14229_Transfer[4] = ( uint8_t ) ( ( NVM_ODO_VALUE / 10 ) >> 8 );
            UDS_ISO14229_Transfer[5] = ( uint8_t ) ( NVM_ODO_VALUE / 10 );
            UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 6, UDS_ISO14229_Transfer );
            break;

        case 0x6205:  //Service Trip    hex
            UDS_ISO14229_Transfer[2] = ( uint8_t ) ( ( DATA_SERVICE_INTERVAL ) >> 8 );
            UDS_ISO14229_Transfer[3] = ( uint8_t ) ( DATA_SERVICE_INTERVAL );
            UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 4, UDS_ISO14229_Transfer );
            break;

        case 0xFE03: //io状态读取

            ///*                    7            6             5             4                 3           2                 1       0
            /*    IO_Group_1       左转向	        右转向	     位置灯	      安全带	          安全气囊	    白昼灯	       远光灯	   机油压力低
                  IO_Group_2       发动机排放故障	发动机故障	 冷却液温度高	燃油低	          前雾灯	      后雾灯	       胎压故障	 EPB故障
                  IO_Group_3       定速巡航	      EBD电子助力	 EPS电动助力	定速巡航状态显示	TCU 系统故障	充电系统	     ESC_OFF	 车身稳定
                  IO_Group_4       陡坡缓降黄 	  陡坡缓降绿	 自动驻车绿	  自动驻车黄	      驻车制动指示	制动系统故障	 ABS
               */
            for ( i = 0; i < 4; i++ )
                UDS_ISO14229_Transfer[i + 2] = 0;

            //------------------------------------------------------
            //左转
            if ( bit_is_set ( PTP, 6 ) )
                SETBIT ( UDS_ISO14229_Transfer[2], 7 );

            //右转
            if ( bit_is_set ( PORTA, 2 ) )
                SETBIT ( UDS_ISO14229_Transfer[2], 6 );

            //位置灯
            if ( bit_is_set ( PORTA, 5 ) )
                SETBIT ( UDS_ISO14229_Transfer[2], 5 );

            //安全带
            if ( ( GUIIconSeatbeltRStatus == ICON_ON ) || ( GUIIconSeatbeltRStatus == ICON_FORCE_ON ) )
                SETBIT ( UDS_ISO14229_Transfer[2], 4 );

            //安全气囊
            if ( bit_is_set ( PTR, 2 ) )
                SETBIT ( UDS_ISO14229_Transfer[2], 3 );

            //白昼灯
            if ( ( GUIIconDRLGStatus == ICON_ON ) || ( GUIIconDRLGStatus == ICON_FORCE_ON ) )
                SETBIT ( UDS_ISO14229_Transfer[2], 2 );

            //远光灯
            if ( bit_is_set ( PORTA, 6 ) )
                SETBIT ( UDS_ISO14229_Transfer[2], 1 );

            //机油压力
            if ( bit_is_set ( PT01AD, 3 ) )
                SETBIT ( UDS_ISO14229_Transfer[2], 0 );

            //------------------------------------------------------
            //发动机排放故障
            if ( bit_is_set ( PTV, 0 ) )
                SETBIT ( UDS_ISO14229_Transfer[3], 7 );

            //发动机故障 GUIIconSVSYStatus
            if ( ( GUIIconSVSYStatus == ICON_ON ) || ( GUIIconSVSYStatus == ICON_FORCE_ON ) )
                SETBIT ( UDS_ISO14229_Transfer[3], 6 );

            //冷却液温度高
            if ( ( GUIIconHiTempRStatus == ICON_ON ) || ( GUIIconHiTempRStatus == ICON_FORCE_ON ) )
                SETBIT ( UDS_ISO14229_Transfer[3], 5 );

            //燃油低
            if ( ( GUIIconLowFuelYStatus == ICON_ON ) || ( GUIIconLowFuelYStatus == ICON_FORCE_ON ) )
                SETBIT ( UDS_ISO14229_Transfer[3], 4 );

            //前雾灯
            if ( bit_is_set ( PORTA, 3 ) )
                SETBIT ( UDS_ISO14229_Transfer[3], 3 );

            //后雾灯
            if ( bit_is_set ( PORTA, 4 ) )
                SETBIT ( UDS_ISO14229_Transfer[3], 2 );

            //胎压故障        GUIIconTirePressureYStatus
            if ( ( GUIIconTirePressureYStatus == ICON_ON ) || ( GUIIconTirePressureYStatus == ICON_FORCE_ON ) )
                SETBIT ( UDS_ISO14229_Transfer[3], 1 );

            //EPB故障     GUIIconEPBYStatus
            if ( ( GUIIconEPBYStatus == ICON_ON ) || ( GUIIconEPBYStatus == ICON_FORCE_ON ) )
                SETBIT ( UDS_ISO14229_Transfer[3], 0 );

            //------------------------------------------------------
            //定速巡航    GUIIconCruiseGStatus
            if ( ( GUIIconCruiseGStatus == ICON_ON ) || ( GUIIconCruiseGStatus == ICON_FORCE_ON ) )
                SETBIT ( UDS_ISO14229_Transfer[4], 7 );

            //EBD电子助力      GUIIconEBDYStatus
            if ( ( GUIIconEBDYStatus == ICON_ON ) || ( GUIIconEBDYStatus == ICON_FORCE_ON ) )
                SETBIT ( UDS_ISO14229_Transfer[4], 6 );

            //EPS电动助力       GUIIconEPSYStatus
            if ( ( GUIIconEPSYStatus == ICON_ON ) || ( GUIIconEPSYStatus == ICON_FORCE_ON ) )
                SETBIT ( UDS_ISO14229_Transfer[4], 5 );

            //定速巡航状态显示 GUIIconCruiseStateGStatus
            if ( ( GUIIconCruiseStateGStatus == ICON_ON ) || ( GUIIconCruiseStateGStatus == ICON_FORCE_ON ) )
                SETBIT ( UDS_ISO14229_Transfer[4], 4 );

            //TCU 系统故障  GUIIconTCUYStatus
            if ( ( GUIIconTCUYStatus == ICON_ON ) || ( GUIIconTCUYStatus == ICON_FORCE_ON ) )
                SETBIT ( UDS_ISO14229_Transfer[4], 3 );

            //充电系统  GUIIconLowBatteryRStatus
            if ( ( GUIIconLowBatteryRStatus == ICON_ON ) || ( GUIIconLowBatteryRStatus == ICON_FORCE_ON ) )
                SETBIT ( UDS_ISO14229_Transfer[4], 2 );

            //ESC_OFF       GUIIconESCYStatus     GUIIconESCOffYStatus
            if ( ( GUIIconESCOffYStatus == ICON_ON ) || ( GUIIconESCOffYStatus == ICON_FORCE_ON ) )
                SETBIT ( UDS_ISO14229_Transfer[4], 1 );

            //车身稳定
            if ( ( GUIIconESCYStatus == ICON_ON ) || ( GUIIconESCYStatus == ICON_FORCE_ON ) )
                SETBIT ( UDS_ISO14229_Transfer[4], 0 );

            //------------------------------------------------------
            //陡坡缓降黄  GUIIconHDCYStatus
            if ( ( GUIIconHDCYStatus == ICON_ON ) || ( GUIIconHDCYStatus == ICON_FORCE_ON ) )
                SETBIT ( UDS_ISO14229_Transfer[5], 7 );

            //陡坡缓降绿   GUIIconHDCGStatus
            if ( ( GUIIconHDCGStatus == ICON_ON ) || ( GUIIconHDCGStatus == ICON_FORCE_ON ) )
                SETBIT ( UDS_ISO14229_Transfer[5], 6 );

            //自动驻车绿        GUIIconAVHGStatus
            if ( ( GUIIconAVHGStatus == ICON_ON ) || ( GUIIconAVHGStatus == ICON_FORCE_ON ) )
                SETBIT ( UDS_ISO14229_Transfer[5], 5 );

            //自动驻车黄
            if ( ( GUIIconAVHRStatus == ICON_ON ) || ( GUIIconAVHRStatus == ICON_FORCE_ON ) )
                SETBIT ( UDS_ISO14229_Transfer[5], 4 );

            //驻车制动指示
            if ( bit_is_set ( PTP, 5 ) )
                SETBIT ( UDS_ISO14229_Transfer[5], 3 );

            //制动系统故障      GUIIconBrakeRStatus
            if ( ( GUIIconBrakeRStatus == ICON_ON ) || ( GUIIconBrakeRStatus == ICON_FORCE_ON ) )
                SETBIT ( UDS_ISO14229_Transfer[5], 2 );

            //ABS       GUIIconABSYStatus
            if ( ( GUIIconABSYStatus == ICON_ON ) || ( GUIIconABSYStatus == ICON_FORCE_ON ) )
                SETBIT ( UDS_ISO14229_Transfer[5], 1 );

            UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 6, UDS_ISO14229_Transfer );
            break;

        case 0xDFEC:
            for ( i = 0; i < 64; i++ )
                UDS_ISO14229_Transfer[i + 2] = diag1.DFEC[i];

            UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 66, UDS_ISO14229_Transfer );
            break;

        case 0xDFED:
            for ( i = 0; i < 64; i++ )
                UDS_ISO14229_Transfer[i + 2] = diag1.DFED[i];

            UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 66, UDS_ISO14229_Transfer );
            break;

        case 0xDFEE:
            for ( i = 0; i < 64; i++ )
                UDS_ISO14229_Transfer[i + 2] = diag1.DFEE[i];

            UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 66, UDS_ISO14229_Transfer );
            break;

        case 0xDFEF:
            for ( i = 0; i < 64; i++ )
                UDS_ISO14229_Transfer[i + 2] = diag1.DFEF[i];

            UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 66, UDS_ISO14229_Transfer );
            break;

        default:
            NegRes.code = requestOutOfRange;
            UDS_Service_Response ( 0x22, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
            break;
    }
}

void CalculateKey ( uint8_t *Seed )     //根据seed计算得出key值
{
    uint8_t i;
    uint8_t Cal[4];
    uint8_t *p;

    for ( i = 0; i < 4; i++ )
    {
        Cal[i] = Seed[i] ^ XorArray[i];
    }

    p = ( uint8_t * ) &ValidSeedKey;
    p[0] = ( ( Cal[3] & 0x0F ) << 4 ) | ( Cal[3] & 0xF0 );
    p[1] = ( ( Cal[1] & 0x0F ) << 4 ) | ( ( Cal[0] & 0xF0 ) >> 4 );
    p[2] = ( Cal[1] & 0xF0 ) | ( ( Cal[2] & 0xF0 ) >> 4 );
    p[3] = ( ( Cal[0] & 0x0F ) << 4 ) | ( Cal[2] & 0x0F );
    SeedkeyValidFlag = 1;
}

void CalculateKeyLv11 ( uint8_t *Seed )     //根据seed计算得出key值
{
    uint8_t i;
    uint8_t Cal[4];
    uint8_t *p;

    for ( i = 0; i < 4; i++ )
    {
        Cal[i] = Seed[i] ^ XorArray[i];
    }

    p = ( uint8_t * ) &ValidSeedKey;
    p[0] = ( ( ( Cal[2] & 0x03 ) << 6 ) | ( Cal[3] & 0xFC ) >> 2 );
    p[1] = ( ( Cal[3] & 0x03 ) << 6 ) | ( ( Cal[0] & 0x3F ) );
    p[2] = ( Cal[0] & 0xFC ) | ( ( Cal[1] & 0xC0 ) >> 6 );
    p[3] = ( ( Cal[1] & 0xFC ) ) | ( Cal[2] & 0x03 );
    SeedkeyValidFlag = 1;
}


//安全访问
void UDS_Service_27_Indication ( uint16_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    uint16_t i;
    uint32_t KeyReceive;
    uint8_t Service27DiagDataLength;
    //uint16_t SeedHigh;
    //uint16_t SeedLow;
    uint8_t  Seed[4];
    uint16_t tempbuffer;
    uint16_t Diag_Id = 0;
    Diag_Id = A_TA_type;;

    if ( Diag_Id == DIAG_ID_Rx_FUN ) //不支持功能寻址
        return;

    S3_ServerCnt = 0;
    Service27DiagDataLength = A_Length;

    for ( i = 0; i < Service27DiagDataLength; i++ ) //长度不固定 要收数据
    {
        UDS_ISO14229_Transfer[i] = * ( A_Data + i );
    }

    if ( Service27DiagDataLength == 0 )
    {
        NegRes.code = incorrectMessageLength; //数据长度不对
        UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    if ( SessionType == DefaultSession ) //不支持默认会话
    {
        NegRes.code = subfunctionNotSupportinActiveSession; //会话模式不支持
        UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    if ( ( UDS_ISO14229_Transfer[0] != 0x01 ) && ( UDS_ISO14229_Transfer[0] != 0x11 ) && ( UDS_ISO14229_Transfer[0] != 0x02 ) && ( UDS_ISO14229_Transfer[0] != 0x82 ) && ( UDS_ISO14229_Transfer[0] != 0x92 ) && ( UDS_ISO14229_Transfer[0] != 0x12 ) )
    {
        NegRes.code = subFunctionNotSupported; //子功能不支持
        UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    if ( Wait10sFlag == 1 )
    {
        //超过最大尝试次数3次，或者连续4次发送请求种子而没有发送正确的秘钥 需要等待10秒 才可以进行一次尝试
        if ( Wait10s == 1 ) //需要做一个十秒的定时，时间到 Wait10s置1
        {
            Wait10s = 0;
            Wait10sFlag = 0;
        }
        else
        {
            NegRes.code = requiredTimeDelayNotExpired; //延时10秒时间未达到
            UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
            return;
        }
    }

    switch ( UDS_ISO14229_Transfer[0] )
    {
        case 0x01:
            Services27_01_Requested = 1;

            if ( SessionType != ExtendedDiagnosticSession ) //不支持默认会话
            {
                NegRes.code = subfunctionNotSupportinActiveSession; //会话模式不支持
                UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            if ( Service27DiagDataLength != 1 )
            {
                NegRes.code = incorrectMessageLength; //数据长度不对
                UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                break;
            }

            diag27.RequestSeedCnt++;

            if ( diag27.RequestSeedCnt >= 5 ) //错误超过四次，需要等待10秒
            {
                diag27.RequestSeedCnt = 3;
                Services27_01_Requested = 0;
                NegRes.code = exceedNumberOfAttempts; //最大次数
                Wait10sFlag = 1;   //10秒计时器打开
                wait10cnt = 0;
                UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            Write_27_DATA_TO_FLASH();

            if ( SeedkeyValidFlag == 0 ) //如果当前秘钥无效，则生成一个秘钥
            {
                tempbuffer = ( uint16_t ) ( RANDOM );
                SeedHigh = ( uint8_t ) ( tempbuffer >> 8 );
                SeedLow = ( uint8_t ) ( tempbuffer & 0x00ff );
                SeedkeyValidFlag = 1;
            }

            UDS_ISO14229_Transfer[0] = 0x01;
            UDS_ISO14229_Transfer[1] = 0x31 + ~SeedHigh; 		 //S1
            UDS_ISO14229_Transfer[2] = 0x23 + ~SeedLow;		 //S2
            UDS_ISO14229_Transfer[3] = 0x56 + SeedHigh;      	//S3
            UDS_ISO14229_Transfer[4] = 0x71 + SeedHigh;      	//S4

            if ( DiagLockFlag == NormalKeyUnlock )
            {
                UDS_ISO14229_Transfer[0] = 0x01;
                UDS_ISO14229_Transfer[1] = 0; 		 //S1
                UDS_ISO14229_Transfer[2] = 0;		  //S2
                UDS_ISO14229_Transfer[3] = 0;      	//S3
                UDS_ISO14229_Transfer[4] = 0;      	//S4
            }

            Seed[0] = UDS_ISO14229_Transfer[1];
            Seed[1] = UDS_ISO14229_Transfer[2];
            Seed[2] = UDS_ISO14229_Transfer[3];
            Seed[3] = UDS_ISO14229_Transfer[4];
            UDS_Service_Response ( 0x27, POSITIVE_RSP, DIAG_ID_Tx, 5, UDS_ISO14229_Transfer );
            CalculateKey ( Seed );
            break;

        case 0x11:
            Services27_11_Requested = 1;

            if ( SessionType != ProgrammingSession ) //不支持默认会话
            {
                NegRes.code = subfunctionNotSupportinActiveSession; //会话模式不支持
                UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            if ( Service27DiagDataLength != 1 )
            {
                NegRes.code = incorrectMessageLength; //数据长度不对
                UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                break;
            }

            diag27.RequestSeedCnt11++;

            if ( diag27.RequestSeedCnt11 >= 5 ) //错误超过四次，需要等待10秒
            {
                diag27.RequestSeedCnt11 = 3;
                Services27_11_Requested = 0;
                NegRes.code = exceedNumberOfAttempts; //最大次数
                Wait10sFlag = 1;   //10秒计时器打开
                wait10cnt = 0;
                UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            Write_27_DATA_TO_FLASH();

            if ( SeedkeyValidFlag == 0 ) //如果当前秘钥无效，则生成一个秘钥
            {
                tempbuffer = ( uint16_t ) ( RANDOM );
                SeedHigh = ( uint8_t ) ( tempbuffer >> 8 );
                SeedLow = ( uint8_t ) ( tempbuffer & 0x00ff );
                SeedkeyValidFlag = 1;
            }

            UDS_ISO14229_Transfer[0] = 0x11;
            UDS_ISO14229_Transfer[1] = 0x31 + ~SeedHigh; 		 //S1
            UDS_ISO14229_Transfer[2] = 0x23 + ~SeedLow;		 //S2
            UDS_ISO14229_Transfer[3] = 0x56 + SeedHigh;      	//S3
            UDS_ISO14229_Transfer[4] = 0x71 + SeedHigh;      	//S4

            if ( DiagLockFlag == BootKeyUnlock )
            {
                UDS_ISO14229_Transfer[0] = 0x11;
                UDS_ISO14229_Transfer[1] = 0; 		 //S1
                UDS_ISO14229_Transfer[2] = 0;		  //S2
                UDS_ISO14229_Transfer[3] = 0;      	//S3
                UDS_ISO14229_Transfer[4] = 0;      	//S4
            }

            Seed[0] = UDS_ISO14229_Transfer[1];
            Seed[1] = UDS_ISO14229_Transfer[2];
            Seed[2] = UDS_ISO14229_Transfer[3];
            Seed[3] = UDS_ISO14229_Transfer[4];
            UDS_Service_Response ( 0x27, POSITIVE_RSP, DIAG_ID_Tx, 5, UDS_ISO14229_Transfer );
            CalculateKeyLv11 ( Seed );
            break;

        case 0x82:
        case 0x02:
            if ( Service27DiagDataLength != 5 )
            {
                NegRes.code = incorrectMessageLength; //数据长度不对
                UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            if ( Services27_01_Requested != 1 )
            {
                NegRes.code = requestSequenceError; //序列不对
                UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            KeyReceive = ( ( uint32_t ) UDS_ISO14229_Transfer[1] << 24 ) | ( ( uint32_t ) UDS_ISO14229_Transfer[2] << 16 ) | ( ( uint32_t ) UDS_ISO14229_Transfer[3] << 8 ) | UDS_ISO14229_Transfer[4];

            if ( ( KeyReceive == ValidSeedKey ) && ( SeedkeyValidFlag == 1 ) )
            {
                DiagLockFlag = NormalKeyUnlock;
                diag27.attemptcnt = 0;
                diag27.RequestSeedCnt = 0;
                SeedkeyValidFlag = 1;
                UDS_ISO14229_Transfer[0] = 0x02;

                if ( UDS_ISO14229_Transfer[0] == 0x02 )
                    UDS_Service_Response ( 0x27, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer );

                Write_27_DATA_TO_FLASH();
            }
            else
            {
                SeedkeyValidFlag = 0;
                diag27.attemptcnt++;

                if ( diag27.attemptcnt >= 4 )
                {
                    diag27.attemptcnt = 2;
                    Wait10sFlag = 1;   //10秒计时器打开
                    wait10cnt = 0;
                }

                NegRes.code = invalidKey; //数据长度不对
                UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                Write_27_DATA_TO_FLASH();
                return;
            }

            break;

        case 0x92:
        case 0x12:
            if ( Service27DiagDataLength != 5 )
            {
                NegRes.code = incorrectMessageLength; //数据长度不对
                UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            if ( Services27_11_Requested != 1 )
            {
                NegRes.code = requestSequenceError; //序列不对
                UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            KeyReceive = ( ( uint32_t ) UDS_ISO14229_Transfer[1] << 24 ) | ( ( uint32_t ) UDS_ISO14229_Transfer[2] << 16 ) | ( ( uint32_t ) UDS_ISO14229_Transfer[3] << 8 ) | UDS_ISO14229_Transfer[4];

            if ( ( KeyReceive == ValidSeedKey ) && ( SeedkeyValidFlag == 1 ) )
            {
                DiagLockFlag = BootKeyUnlock;
                diag27.attemptcnt11 = 0;
                diag27.RequestSeedCnt11 = 0;
                SeedkeyValidFlag = 1;
                UDS_ISO14229_Transfer[0] = 0x12;

                if ( UDS_ISO14229_Transfer[0] == 0x12 )
                    UDS_Service_Response ( 0x27, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer );

                Write_27_DATA_TO_FLASH();
            }
            else
            {
                SeedkeyValidFlag = 0;
                diag27.attemptcnt11++;

                if ( diag27.attemptcnt11 >= 4 )
                {
                    diag27.attemptcnt11 = 2;
                    Wait10sFlag = 1;   //10秒计时器打开
                    wait10cnt = 0;
                }

                NegRes.code = invalidKey; //数据长度不对
                UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                Write_27_DATA_TO_FLASH();
                return;
            }

            break;

        default:
            NegRes.code = subFunctionNotSupported; //子功能不支持
            UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
            break;
    }
}


//通讯控制
void UDS_Service_28_Indication ( uint16_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    uint16_t i;
    uint8_t Service28DiagDataLength;
    uint16_t  FunorPhy;
    FunorPhy = A_TA_type;
    S3_ServerCnt = 0;

    if ( SessionType != ExtendedDiagnosticSession ) //不支持默认会话
    {
        NegRes.code = serviceNotSupportedInActiveSession; //会话模式不支持
        UDS_Service_Response ( 0x28, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    if ( EngineSpeed >= 400 ) //转速不对
    {
        NegRes.code = conditionsNotCorrect; //会话模式不支持
        UDS_Service_Response ( 0x28, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    Service28DiagDataLength = A_Length;

    if ( Service28DiagDataLength != 2 )
    {
        NegRes.code = incorrectMessageLength; //会话模式不支持
        UDS_Service_Response ( 0x28, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    for ( i = 0; i < Service28DiagDataLength; i++ ) //长度不固定 要收数据
    {
        UDS_ISO14229_Transfer[i] = * ( A_Data + i );
    }

    if ( UDS_ISO14229_Transfer[1] != 0x01 )
    {
        NegRes.code = requestOutOfRange; //会话模式不支持
        UDS_Service_Response ( 0x28, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    switch ( UDS_ISO14229_Transfer[0] )
    {
        case 0x80:
        case enableRxAndTx:
            CommunicationType = UDS_ISO14229_Transfer[0];
            ControlType = enableRxAndTx;
            Com_RxStart();
            Com_TxStart();

            if ( bit_is_clear ( UDS_ISO14229_Transfer[0], 7 ) )
                UDS_Service_Response ( 0x28, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer );

            break;

        case 0x81:
        case enableRxAndDisableTx:
            CommunicationType = UDS_ISO14229_Transfer[0];
            ControlType = enableRxAndDisableTx;
            Com_RxStart();
            Com_TxStop();

            if ( bit_is_clear ( UDS_ISO14229_Transfer[0], 7 ) )
                UDS_Service_Response ( 0x28, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer );

            break;

        case 0x82:
        case disableRxAndEnableTx:
            CommunicationType = UDS_ISO14229_Transfer[0];
            ControlType = disableRxAndEnableTx;
            Com_TxStop();
            Com_TxStart();

            if ( bit_is_clear ( UDS_ISO14229_Transfer[0], 7 ) )
                UDS_Service_Response ( 0x28, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer );

            break;

        case 0x83:
        case disableRxAndTx:
            CommunicationType = UDS_ISO14229_Transfer[0];
            ControlType = disableRxAndEnableTx;
            Com_TxStop();
            Com_RxStop();

            if ( bit_is_clear ( UDS_ISO14229_Transfer[0], 7 ) )
                UDS_Service_Response ( 0x28, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer );

            break;

        default:
            if ( FunorPhy == 0x7df )
                return;

            NegRes.code = subFunctionNotSupported; //超出申请范围
            UDS_Service_Response ( 0x28, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
            break;
    }
}

//DTC记录控制
void UDS_Service_85_Indication ( uint16_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    uint16_t i;
    uint8_t Service85DiagDataLength;
    uint16_t  FunorPhy;
    FunorPhy = A_TA_type;
    S3_ServerCnt = 0;

    if ( SessionType != ExtendedDiagnosticSession ) //不支持默认会话
    {
        NegRes.code = serviceNotSupportedInActiveSession; //会话模式不支持
        UDS_Service_Response ( 0x85, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    if ( EngineSpeed >= 400 ) //转速不对
    {
        NegRes.code = conditionsNotCorrect; //会话模式不支持
        UDS_Service_Response ( 0x85, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    Service85DiagDataLength = A_Length;

    if ( Service85DiagDataLength != 1 )
    {
        NegRes.code = incorrectMessageLength; //会话模式不支持
        UDS_Service_Response ( 0x85, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    for ( i = 0; i < Service85DiagDataLength; i++ ) //长度不固定 要收数据
    {
        UDS_ISO14229_Transfer[i] = * ( A_Data + i );
    }

    switch ( UDS_ISO14229_Transfer[0] )
    {
        case 0x81:
        case DTCSettingOn:
            DTCControlType = DTCSettingOn;
            CAN_DTC_OFF = 0;

            if ( bit_is_clear ( UDS_ISO14229_Transfer[0], 7 ) )
                UDS_Service_Response ( 0x85, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer );

            break;

        case 0x82:
        case DTCSettingOff:
            DTCControlType = DTCSettingOff;
            CAN_DTC_OFF = 1;

            if ( bit_is_clear ( UDS_ISO14229_Transfer[0], 7 ) )
                UDS_Service_Response ( 0x85, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer );

            break;

        default :
            if ( FunorPhy == 0x7df )
                return;

            NegRes.code = subFunctionNotSupported; //超出申请范围
            UDS_Service_Response ( 0x85, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
            break;
    }
}





//例程控制
void UDS_Service_31_Indication ( uint16_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    uint8_t  Service31DiagDataLength;
    uint8_t  EraserResult;
    uint16_t i;
    uint16_t RoutineID;
    uint16_t FunOrPhy;
    //uint32_t SectorAddress;
    FunOrPhy = A_TA_type;
    S3_ServerCnt = 0;

    if ( FunOrPhy == 0x7DF )
        return;

    if ( SessionType == DefaultSession ) //不支持默认会话
    {
        NegRes.code = serviceNotSupportedInActiveSession; //会话模式不支持
        UDS_Service_Response ( 0x31, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    if ( DiagLockFlag == BootKeyLock )
    {
        NegRes.code = securityAccessDenied; //安全模式未解锁
        UDS_Service_Response ( 0x31, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    if ( EngineSpeed >= 400 ) //转速不对
    {
        NegRes.code = conditionsNotCorrect; //会话模式不支持
        UDS_Service_Response ( 0x31, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    Service31DiagDataLength = A_Length;

    for ( i = 0; i < Service31DiagDataLength; i++ ) //长度不固定 要收数据
    {
        UDS_ISO14229_Transfer[i] = * ( A_Data + i );
    }

    RoutineID = ( ( uint16_t ) UDS_ISO14229_Transfer[1] << 8 ) | ( uint16_t ) UDS_ISO14229_Transfer[2];

    if ( ( UDS_ISO14229_Transfer[0] == 1 ) || ( UDS_ISO14229_Transfer[0] == 0x81 ) )
    {
        switch ( RoutineID )
        {
            case MaintenanceReset:
                if ( Service31DiagDataLength != 3 )
                {
                    NegRes.code = incorrectMessageLength; //数据长度不对
                    UDS_Service_Response ( 0x31, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
                }

                if ( DiagLockFlag != NormalKeyUnlock )
                {
                    NegRes.code = securityAccessDenied; //安全模式未解锁
                    UDS_Service_Response ( 0x31, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
                }

                if ( SessionType != ExtendedDiagnosticSession ) //必须在扩展回话进行
                {
                    NegRes.code = serviceNotSupportedInActiveSession; //会话模式不支持
                    UDS_Service_Response ( 0x31, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
                }

                if ( Service31DiagDataLength != 3 )
                {
                    NegRes.code = incorrectMessageLength; //数据长度不对
                    UDS_Service_Response ( 0x31, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
                }

                NVM_USER_SETTINGS_SERVICE = 5000;
                NVM_Data_Update ( NVM_USER_SETTINGS );
                UDS_ISO14229_Transfer[3] = RoutineCompletedSuccessfully;

                if ( bit_is_clear ( UDS_ISO14229_Transfer[0], 7 ) )
                    UDS_Service_Response ( 0x31, POSITIVE_RSP, DIAG_ID_Tx, 4, UDS_ISO14229_Transfer );

                break;

            case CheckMemory:
                if ( SessionType != ProgrammingSession ) //必须在编程回话进行
                {
                    NegRes.code = serviceNotSupportedInActiveSession; //会话模式不支持
                    UDS_Service_Response ( 0x31, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
                }

                if ( DiagLockFlag != BootKeyUnlock )
                {
                    NegRes.code = securityAccessDenied; //安全模式未解锁
                    UDS_Service_Response ( 0x31, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
                }

                UDS_ISO14229_Transfer[3] = RoutineCompletedSuccessfully;

                if ( UDS_ISO14229_Transfer[0] == 0x01 )
                    UDS_Service_Response ( 0x31, POSITIVE_RSP, DIAG_ID_Tx, 4, UDS_ISO14229_Transfer );

                break;

            case ECU_selftest:
                if ( Service31DiagDataLength != 3 )
                {
                    NegRes.code = incorrectMessageLength; //数据长度不对
                    UDS_Service_Response ( 0x31, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
                }

                if ( DiagLockFlag != NormalKeyUnlock )
                {
                    NegRes.code = securityAccessDenied; //安全模式未解锁
                    UDS_Service_Response ( 0x31, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
                }

                UDS_ISO14229_Transfer[3] = RoutineCompletedSuccessfully;

                //执行ECU自检函数 返回值  0x04成功  0x05失败  待添加
                if ( bit_is_clear ( UDS_ISO14229_Transfer[0], 7 ) )
                    UDS_Service_Response ( 0x31, POSITIVE_RSP, DIAG_ID_Tx, 4, UDS_ISO14229_Transfer );

                break;

            case EraseMemory:
                if ( DiagLockFlag != BootKeyUnlock )
                {
                    NegRes.code = securityAccessDenied; //安全模式未解锁
                    UDS_Service_Response ( 0x31, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
                }

                if ( SessionType != ProgrammingSession ) //必须在编程回话进行
                {
                    NegRes.code = serviceNotSupportedInActiveSession; //会话模式不支持
                    UDS_Service_Response ( 0x31, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
                }

                WRITE_BZ_inDFLAH2Sector ( 0x3AA3 ); //进入编程模式
                COPCTL = 0x01;        //enable watchdog
                ARMCOP = 0x00;
                break;

            case CheckProgrammingDependencies:
                if ( Service31DiagDataLength != 3 )
                {
                    NegRes.code = incorrectMessageLength; //数据长度不对
                    UDS_Service_Response ( 0x31, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
                }

                if ( DiagLockFlag != BootKeyUnlock )
                {
                    NegRes.code = securityAccessDenied; //安全模式未解锁
                    UDS_Service_Response ( 0x31, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
                }

                if ( SessionType != ProgrammingSession ) //不支持默认会话
                {
                    NegRes.code = serviceNotSupportedInActiveSession; //会话模式不支持
                    UDS_Service_Response ( 0x31, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
                }

                UDS_ISO14229_Transfer[3] = RoutineCompletedSuccessfully;

                if ( UDS_ISO14229_Transfer[0] == 0x01 )
                    UDS_Service_Response ( 0x31, POSITIVE_RSP, DIAG_ID_Tx, 4, UDS_ISO14229_Transfer );

                break;

            default:
                NegRes.code = requestOutOfRange; //请求的id不对
                UDS_Service_Response ( 0x31, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                break;
        }
    }
    else
    {
        NegRes.code = subFunctionNotSupported; //数据长度不对
        UDS_Service_Response ( 0x31, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
}



//请求下载服务
void UDS_Service_34_Indication ( uint16_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    uint16_t i;
    uint8_t Service34DiagDataLength;
    uint32_t memoryAddress;
    uint16_t FunOrPhy;
    //uint32_t SectorAddress;
    FunOrPhy = A_TA_type;
    S3_ServerCnt = 0;

    if ( FunOrPhy == 0x7DF )
        return;

    if ( SessionType != ProgrammingSession ) //只支持编程会话模式
    {
        NegRes.code = serviceNotSupportedInActiveSession; //会话模式不支持
        UDS_Service_Response ( 0x34, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    if ( DiagLockFlag == BootKeyLock )
    {
        NegRes.code = securityAccessDenied; //安全模式未解锁
        UDS_Service_Response ( 0x34, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    Service34DiagDataLength = A_Length;

    if ( Service34DiagDataLength != 10 )
    {
        NegRes.code = incorrectMessageLength; //数据长度不对
        UDS_Service_Response ( 0x34, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    for ( i = 0; i < Service34DiagDataLength; i++ ) //长度不固定 要收数据
    {
        UDS_ISO14229_Transfer[i] = * ( A_Data + i );
    }

    if ( ( UDS_ISO14229_Transfer[0] != 00 ) || ( UDS_ISO14229_Transfer[1] != 0x44 ) )
    {
        NegRes.code = incorrectMessageLength; //数据长度不对
        UDS_Service_Response ( 0x34, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    UDS_ISO14229_Transfer[0] = LengthFormatIdentifier;
    UDS_ISO14229_Transfer[1] = 0x00;
    UDS_ISO14229_Transfer[2] = 226;
    //  ErasrFor34();
    UDS_Service_Response ( 0x34, POSITIVE_RSP, DIAG_ID_Tx, 3, UDS_ISO14229_Transfer );
}

//数据下载服务
void UDS_Service_36_Indication ( uint16_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    uint16_t i;
    uint8_t m8;
    uint8_t *p8;
    uint8_t Service36DiagDataLength;
    uint16_t FunOrPhy;
    //uint32_t SectorAddress;
    FunOrPhy = A_TA_type;
    S3_ServerCnt = 0;

    if ( FunOrPhy == 0x7DF )
        return;

    if ( SessionType != ProgrammingSession ) //只支持编程会话模式
    {
        NegRes.code = serviceNotSupportedInActiveSession; //会话模式不支持
        UDS_Service_Response ( 0x36, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    if ( DiagLockFlag == BootKeyLock )
    {
        NegRes.code = securityAccessDenied; //安全模式未解锁
        UDS_Service_Response ( 0x36, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    Service36DiagDataLength = A_Length;

    for ( i = 0; i < Service36DiagDataLength; i++ ) //取数据
    {
        UDS_ISO14229_Transfer[i] = * ( A_Data + i );
    }

    BlocKCnt++;

    if ( UDS_ISO14229_Transfer[0] != BlocKCnt )
    {
        NegRes.code = wrongBlockSequenceCounter; //请求序列错误
        UDS_Service_Response ( 0x36, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    UDS_ISO14229_Transfer[0] = BlocKCnt;
    UDS_Service_Response ( 0x36, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer );
}

//请求下载服务
void UDS_Service_37_Indication ( uint16_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    uint16_t i;
    uint8_t Service37DiagDataLength;
    uint16_t FunOrPhy;
    //uint32_t SectorAddress;
    FunOrPhy = A_TA_type;

    if ( FunOrPhy == 0x7DF )
        return;

    S3_ServerCnt = 0;

    if ( SessionType != ProgrammingSession ) //只支持编程会话模式
    {
        NegRes.code = serviceNotSupportedInActiveSession; //会话模式不支持
        UDS_Service_Response ( 0x37, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    //if(RequestTransfer!=Download)
    //{
    //  NegRes.code=requestSequenceError;//会话模式不支持
    //  UDS_Service_Response(0x37, NEGATIVE_RSP, DIAG_ID_Tx, 1, (uint8_t*)(&NegRes.code));
    //  return;
    //}
    //Service37DiagDataLength=A_Length;
    //if(Service37DiagDataLength!=1)
    //{
    //  NegRes.code=incorrectMessageLength;//数据长度不对
    //  UDS_Service_Response(0x37, NEGATIVE_RSP, DIAG_ID_Tx, 1, (uint8_t*)(&NegRes.code));
    //  return;
    //}
    //for(i=0; i<Service37DiagDataLength; i++)  //长度不固定 要收数据
    //{
    //  UDS_ISO14229_Transfer[i]=*(A_Data+i);
    //}
    //if(UDS_ISO14229_Transfer[0]!=00)
    //{
    //  NegRes.code=subFunctionNotSupported;//数据长度不对
    //  UDS_Service_Response(0x37, NEGATIVE_RSP, DIAG_ID_Tx, 1, (uint8_t*)(&NegRes.code));
    //  return;
    //}
    //UDS_ISO14229_Transfer[0]=0x00;
    UDS_Service_Response ( 0x37, POSITIVE_RSP, DIAG_ID_Tx, 0, UDS_ISO14229_Transfer );
}



void UDS_Service_3E_Indication ( uint16_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    uint16_t i;
    uint8_t Service3EDiagDataLength;
    uint16_t  FunorPhy;
    FunorPhy = A_TA_type;
    Service3EDiagDataLength = A_Length;

    if ( Service3EDiagDataLength != 1 )
    {
        NegRes.code = incorrectMessageLength; //数据长度不对
        UDS_Service_Response ( 0x3E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    for ( i = 0; i < Service3EDiagDataLength; i++ ) //长度不固定 要收数据
    {
        UDS_ISO14229_Transfer[i] = * ( A_Data + i );
    }

    if ( ( UDS_ISO14229_Transfer[0] != 0 ) && ( UDS_ISO14229_Transfer[0] != 0x80 ) )
    {
        if ( FunorPhy == 0x7df )
            return;

        NegRes.code = subFunctionNotSupported; //子功能不支持
        UDS_Service_Response ( 0x3E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    if ( UDS_ISO14229_Transfer[0] == 0x80 )
    {
        S3_Server_refresh = 1;
        S3_ServerCnt = 0;
        return;
    }

    UDS_ISO14229_Transfer[0] = 0x00;
    S3_Server_refresh = 1;
    S3_ServerCnt = 0;
    UDS_Service_Response ( 0x3E, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer );
}



void UDS_Service_11_Indication ( uint16_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    uint16_t i;
    uint8_t Service11DiagDataLength;
    uint16_t  FunorPhy;
    FunorPhy = A_TA_type;
    S3_ServerCnt = 0;
    Service11DiagDataLength = A_Length;

    if ( ( SessionType == DefaultSession ) ) //不支持默认会话
    {
        if ( FunorPhy == 0x7df )
            return;

        NegRes.code = serviceNotSupportedInActiveSession; //会话模式不支持
        UDS_Service_Response ( 0x11, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    if ( Service11DiagDataLength != 1 )
    {
        if ( FunorPhy == 0x7df )
            return;

        NegRes.code = incorrectMessageLength; //数据长度不对
        UDS_Service_Response ( 0x11, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    for ( i = 0; i < Service11DiagDataLength; i++ ) //长度不固定 要收数据
    {
        UDS_ISO14229_Transfer[i] = * ( A_Data + i );
    }

    if ( ( UDS_ISO14229_Transfer[0] == 0x01 ) || ( UDS_ISO14229_Transfer[0] == 0x03 ) || ( UDS_ISO14229_Transfer[0] == 0x81 ) || ( UDS_ISO14229_Transfer[0] == 0x83 ) )
    {
        if ( ( UDS_ISO14229_Transfer[0] == 0x81 ) || ( UDS_ISO14229_Transfer[0] == 0x83 ) )
        {
            SessionType = 1;

            for ( ;; )
                nop();

            return;
        }

        UDS_Service_Response ( 0x11, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer );
        SessionType = 1;

        for ( ;; )
            nop();

        return;
    }
    else
    {
        if ( FunorPhy == 0x7df )
            return;

        NegRes.code = subFunctionNotSupported; //子功能不支持
        UDS_Service_Response ( 0x11, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
    }
}


//故障码存储列表

const uint32_t DTC_CODE_TABLE[] =
{
    U007300,
    U010087,
    U010187,
    U014087,
    U015187,
    U012287,
    U013187,
    U014387,
    U015387,
    U016487,
    U015687,
    U300617,
    U300616,
    B11001E,
    B110144,
    B110200,
    B110300,
    B110400,
    B110787,
};



//=================================================
//根据序号输出3个字节的DTC码
//=================================================
void GJXHOUTDTCCODE ( INT8U xh )
{
    INT32U m32;
    INT8U *p;
    m32 = DTC_CODE_TABLE[xh];
    p = ( INT8U * ) &m32;
    CJINF[0] = p[1];		 //H
    CJINF[1] = p[2];    //M
    CJINF[2] = p[3];    //L
}

//============================================
//每次开ACC后初始化DTC STATUS
//============================================
void INIT_DTC_STATUS_IGN_ON ( void )
{
    INT8U i;
    CAN_NORMAL_TT = 0;
    CAN_NORMAL_2SFF = 0;
    BUS_OFF_FLAG = 0;
    VoltageACKFF = 0;
    HIGH_VOLTAGE = 0;
    LOW_VOLTAGE = 0;
    VoltageAbnormalTimer = 0;

    for ( i = 0; i < cnDTCslen; i++ )
        DTC_STATUS[i] = DTC_STATUS[i] | 0x50; //第4,6 BIT置1
}


//============================================
//	20ms 任务
//============================================
void Record_DTCStatus_Wait_No_Busoff_xxms ( void )
{
    INT8U i;
    INT8U m8;
    INT8U status;

    for ( i = 0; i < cnDTCslen; i++ )
    {
        if ( RecDTC_Wait_NoBusoff[i].Flag )
        {
            RecDTC_Wait_NoBusoff[i].Timer++;

            if ( RecDTC_Wait_NoBusoff[i].Timer >= 25 )		//	500ms
            {
                RecDTC_Wait_NoBusoff[i].Flag = 0;
                RecDTC_Wait_NoBusoff[i].Timer = 0;

                if ( RecDTC_Wait_NoBusoff[i].Status != RecDTC_Wait_NoBusoff[i].LastStatus )
                {
                    status = 0x28;		//	0x29 0x00
                }
                else
                {
                    status = RecDTC_Wait_NoBusoff[i].Status;
                }

                m8 = ( DTC_STATUS[i] & 0x28 ) | status;

                if ( bit_is_set ( m8, 3 ) && bit_is_clear ( DTC_STATUS[i], 3 ) )
                {
                    DTCstatusCHGFF = 1;
                }

                DTC_STATUS[i] = m8;
            }
        }
    }
}
//===========================================================
//修改DTC状态记录
//出口:DTCstatusCHGFF=1表示DTC记录有改变(需要存储到EEPROM中)
//===========================================================

void ChangeDTCs_ST_PRG ( INT8U xh, INT8U status )
{
    INT8U m8;

    if ( ( CAN_NORMAL_2SFF == 0 ) || ( CAN_DTC_OFF ) )
        return;

    //	if( ( HIGH_VOLTAGE==1)||(LOW_VOLTAGE==1) ) //电压正常吗
    //		return;

    if ( ( VoltageACKFF == 1 ) && ( xh != cnU300616 ) )
        return;

    if ( ( status == 0x29 ) && ( xh !=  cnU007300 ) )
    {
        RecDTC_Wait_NoBusoff[xh].Flag = 1;
        RecDTC_Wait_NoBusoff[xh].Status = 0x29;
        RecDTC_Wait_NoBusoff[xh].LastStatus = 0x29;
    }
    else
    {
        RecDTC_Wait_NoBusoff[xh].LastStatus = status;
        m8 = ( DTC_STATUS[xh] & 0x28 ) | status;

        if ( bit_is_set ( m8, 3 ) && bit_is_clear ( DTC_STATUS[xh], 3 ) )
        {
            DTCstatusCHGFF = 1;
        }

        DTC_STATUS[xh] = m8;
    }
}



//========================================
//如果DTC记录有改变则存入EEPROM中     100ms
//========================================

void SaveDTCsToEEPROM ( void )
{
    INT8U i;
    INT8U d[cnDTCslen];

    //if( (MSY.CAN_NORMAL_2SFF==0)||(MSY.ACC_OK_FF==0) )
    //return;

    if ( DTCstatusCHGFF )			//DTC记录有改变?,YES
    {
        DTCstatusCHGFF = 0;

        for ( i = 0; i < cnDTCslen; i++ )
            NVMDTC.Data[i] = DTC_STATUS[i] & 0x28;

        NVM_Data_Update ( NVM_DTC_DATA );
        //Write_NbyteTo_EEPROM(DTC_STATUS,cnEEP_DTC_ADR,cnDTCslen);
        //		Write_NbyteTo_EEPROM(d,cnEEP_DTC_ADR,cnDTCslen);
    }
}



//=========================================
//+++++++++100MS+++++++++++
//
//CAN进入NORMAL状态2S计时
//=========================================
void CAN_NORMAL_TIME_PRG ( void )
{
    if ( SYS_OPR_STAT_IGN_ON )
    {
        CAN_NORMAL_TT++;

        if ( CAN_NORMAL_TT == 60 ) // 2S?,YES	//有ACC 6S
        {
            CAN_NORMAL_TT = 0;
            CAN_NORMAL_2SFF = 1;   // =1表示CAN进入NORMAL超过2S
        }
    }
}





void CanLoseDetected ( void )
{
    
}


void UDS_Service_19_Indication ( uint16_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    uint16_t i;
    uint16_t DTCnum;
    uint16_t DTCcnt;
    uint16_t DTCcnt0A;
    uint8_t Service19DiagDataLength;
    uint16_t  FunorPhy;
    FunorPhy = A_TA_type;
    Service19DiagDataLength = A_Length;
    __RESET_WATCHDOG();
    S3_ServerCnt = 0;

    for ( i = 0; i < Service19DiagDataLength; i++ ) //长度不固定 要收数据
    {
        UDS_ISO14229_Transfer[i] = * ( A_Data + i );
    }

    if ( ( UDS_ISO14229_Transfer[0] != 0x01 ) && ( UDS_ISO14229_Transfer[0] != 0x02 ) && ( UDS_ISO14229_Transfer[0] != 0x0A ) && ( UDS_ISO14229_Transfer[0] != 0x81 ) && ( UDS_ISO14229_Transfer[0] != 0x82 ) && ( UDS_ISO14229_Transfer[0] != 0x8A ) )
    {
        if ( FunorPhy == 0x7df )
            return;

        NegRes.code = subFunctionNotSupported; //子功能不支持
        UDS_Service_Response ( 0x19, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    if ( Service19DiagDataLength < 1 )
    {
        if ( FunorPhy == 0x7df )
            return;

        NegRes.code = incorrectMessageLength; //数据长度不对
        UDS_Service_Response ( 0x19, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    if ( SessionType == ProgrammingSession )
    {
        if ( FunorPhy == 0x7df )
            return;

        NegRes.code = serviceNotSupportedInActiveSession; //会话模式不支持
        UDS_Service_Response ( 0x19, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    switch ( UDS_ISO14229_Transfer[0] )
    {
        case 0x81:
        case 0x01://发送dtc
            if ( Service19DiagDataLength != 2 )
            {
                NegRes.code = incorrectMessageLength; //数据长度不对
                UDS_Service_Response ( 0x19, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            DTCnum = 0;

            for ( i = 0; i < cnDTCslen; i++ )
            {
                if ( DTC_STATUS[i]&UDS_ISO14229_Transfer[1] )
                    DTCnum++;
            }

            UDS_ISO14229_Transfer[1] = 0x79;
            UDS_ISO14229_Transfer[2] = 0x00;
            UDS_ISO14229_Transfer[3] = ( INT8U ) ( DTCnum >> 8 );
            UDS_ISO14229_Transfer[4] = ( INT8U ) ( DTCnum & 0x00FF );

            if ( UDS_ISO14229_Transfer[0] == 0x01 )
                UDS_Service_Response ( 0x19, POSITIVE_RSP, DIAG_ID_Tx, 5, UDS_ISO14229_Transfer );

            break;

        case 0x82:
        case 0x02://发送dtc
            if ( Service19DiagDataLength != 2 )
            {
                NegRes.code = incorrectMessageLength; //数据长度不对
                UDS_Service_Response ( 0x19, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            DTCcnt = 2;

            for ( i = 0; i < cnDTCslen; i++ )
            {
                if ( DTC_STATUS[i]&UDS_ISO14229_Transfer[1] )
                {
                    GJXHOUTDTCCODE ( i );	//根据序号输出3个字节的DTC码
                    UDS_ISO14229_Transfer[DTCcnt] = CJINF[0];
                    DTCcnt++;
                    UDS_ISO14229_Transfer[DTCcnt] = CJINF[1];
                    DTCcnt++;
                    UDS_ISO14229_Transfer[DTCcnt] = CJINF[2];
                    DTCcnt++;
                    UDS_ISO14229_Transfer[DTCcnt] = DTC_STATUS[i];
                    DTCcnt++;
                }
            }

            UDS_ISO14229_Transfer[1] = 0x79;

            if ( UDS_ISO14229_Transfer[0] == 0x02 )
                UDS_Service_Response ( 0x19, POSITIVE_RSP, DIAG_ID_Tx, DTCcnt, UDS_ISO14229_Transfer );

            break;

        case 0x8A:
        case 0x0A:  //Only available if more than 1 DTC exist
            if ( Service19DiagDataLength != 1 )
            {
                NegRes.code = incorrectMessageLength; //数据长度不对
                UDS_Service_Response ( 0x19, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            DTCcnt0A = 2;

            for ( i = 0; i < cnDTCslen; i++ )
            {
                __RESET_WATCHDOG();
                GJXHOUTDTCCODE ( i );	//根据序号输出3个字节的DTC码
                UDS_ISO14229_Transfer[DTCcnt0A] = CJINF[0];
                DTCcnt0A++;
                UDS_ISO14229_Transfer[DTCcnt0A] = CJINF[1];
                DTCcnt0A++;
                UDS_ISO14229_Transfer[DTCcnt0A] = CJINF[2];
                DTCcnt0A++;
                UDS_ISO14229_Transfer[DTCcnt0A] = DTC_STATUS[i];
                DTCcnt0A++;
            }

            UDS_ISO14229_Transfer[1] = 0x79;

            if ( UDS_ISO14229_Transfer[0] == 0x0A )
                UDS_Service_Response ( 0x19, POSITIVE_RSP, DIAG_ID_Tx, DTCcnt0A, UDS_ISO14229_Transfer );

            break;

        default:
            if ( FunorPhy == 0x7df )
                return;

            NegRes.code = subFunctionNotSupported; //子功能不支持
            UDS_Service_Response ( 0x19, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
            break;
    }

    /*

    UDS_ISO14229_Transfer[0]=0x00;
    S3_Server_refresh=1;
    UDS_Service_Response(0x19, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer);
    */
}


void UDS_Service_2E_Indication ( uint16_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    //每个写入之后 要写入到EEPROM
    uint16_t i;
    uint16_t DID;
    uint8_t  Service2EDiagDataLength;
    uint16_t DiagIDNum = 0;
    Service2EDiagDataLength = A_Length;
    S3_ServerCnt = 0;
    DiagIDNum = A_TA_type;

    if ( DiagIDNum == 0X7DF )
        return;

    if ( SessionType == DefaultSession ) //不支持默认会话
    {
        NegRes.code = serviceNotSupportedInActiveSession; //会话模式不支持
        UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    if ( DiagLockFlag == BootKeyLock )
    {
        NegRes.code = securityAccessDenied; //未解锁
        UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    if ( EngineSpeed >= 400 ) //转速不对
    {
        NegRes.code = conditionsNotCorrect; //会话模式不支持
        UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    for ( i = 0; i < Service2EDiagDataLength; i++ ) //长度不固定 要收数据
    {
        UDS_ISO14229_Transfer[i] = * ( A_Data + i );
    }

    if ( Service2EDiagDataLength <= 3 )
    {
        NegRes.code = incorrectMessageLength; //数据长度不对
        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] ) ) );

    if ( ( DID != 0xF199 ) && ( DID != 0xF198 ) )
    {
        if ( DiagLockFlag != NormalKeyUnlock )
        {
            NegRes.code = securityAccessDenied; //未解锁
            UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
            return;
        }
    }

    switch ( DID ) //已经使用了2个数据长度，写入内容从[2]开始
    {
        case  0xF112:
            if ( Service2EDiagDataLength != 8 + 2 )
            {
                NegRes.code = incorrectMessageLength; //数据长度不对
                UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            for ( i = 0; i < 8; i++ )
                diag.F112[i] = UDS_ISO14229_Transfer[i + 2];

            Write_2E_DATA_TO_FLASH();
            UDS_Service_Response ( 0x2E, POSITIVE_RSP, DIAG_ID_Tx, 2, UDS_ISO14229_Transfer );
            break;

        case  0xF190:
            if ( Service2EDiagDataLength != 17 + 2 )
            {
                NegRes.code = incorrectMessageLength; //数据长度不对
                UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            for ( i = 0; i < 17; i++ )
                diag.F190[i] = UDS_ISO14229_Transfer[i + 2];

            Write_2E_DATA_TO_FLASH();
            UDS_Service_Response ( 0x2E, POSITIVE_RSP, DIAG_ID_Tx, 2, UDS_ISO14229_Transfer );
            break;

        case 0xF197:
            if ( Service2EDiagDataLength != 8 + 2 )
            {
                NegRes.code = incorrectMessageLength; //数据长度不对
                UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            for ( i = 0; i < 8; i++ )
                diag.F197[i] = UDS_ISO14229_Transfer[i + 2];

            Write_2E_DATA_TO_FLASH();
            UDS_Service_Response ( 0x2E, POSITIVE_RSP, DIAG_ID_Tx, 2, UDS_ISO14229_Transfer );
            break;

        case 0xF198:
            if ( DiagLockFlag != BootKeyUnlock )
            {
                NegRes.code = securityAccessDenied; //未解锁
                UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            if ( Service2EDiagDataLength != 16 + 2 )
            {
                NegRes.code = incorrectMessageLength; //数据长度不对
                UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            for ( i = 0; i < 16; i++ )
                diag.F198[i] = UDS_ISO14229_Transfer[i + 2];

            Write_2E_DATA_TO_FLASH();
            UDS_Service_Response ( 0x2E, POSITIVE_RSP, DIAG_ID_Tx, 2, UDS_ISO14229_Transfer );
            break;

        case 0xF199:
            if ( DiagLockFlag != BootKeyUnlock )
            {
                NegRes.code = securityAccessDenied; //未解锁
                UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            if ( Service2EDiagDataLength != 4 + 2 )
            {
                NegRes.code = incorrectMessageLength; //数据长度不对
                UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            for ( i = 0; i < 4; i++ )
                diag.F199[i] = UDS_ISO14229_Transfer[i + 2];

            Write_2E_DATA_TO_FLASH();
            UDS_Service_Response ( 0x2E, POSITIVE_RSP, DIAG_ID_Tx, 2, UDS_ISO14229_Transfer );
            break;

        case 0xF19D:
            if ( Service2EDiagDataLength != 4 + 2 )
            {
                NegRes.code = incorrectMessageLength; //数据长度不对
                UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            for ( i = 0; i < 4; i++ )
                diag.F19D[i] = UDS_ISO14229_Transfer[i + 2];

            Write_2E_DATA_TO_FLASH();
            UDS_Service_Response ( 0x2E, POSITIVE_RSP, DIAG_ID_Tx, 2, UDS_ISO14229_Transfer );
            break;

        case 0xF110 :
            if ( Service2EDiagDataLength != 8 + 2 )
            {
                NegRes.code = incorrectMessageLength; //数据长度不对
                UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            for ( i = 0; i < 8; i++ )
                NVMConfigF110.Data[i] = UDS_ISO14229_Transfer[i + 2];

            /*
            NVM_USER_SETTINGS_AVG_FUEL_CONS_EN=1;
            NVM_USER_SETTINGS_INST_FUEL_CONS_EN=1;
            NVM_USER_SETTINGS_DTE_EN=1;
            NVM_USER_SETTINGS_DRV_TIME_EN=1;
            NVM_USER_SETTINGS_AVG_SPEED_EN=1;
            NVM_USER_SETTINGS_TIRE_STAT_EN=1;
            NVM_USER_SETTINGS_TRIP_EN=1;
            NVM_USER_SETTINGS_CURRENT_SPEED_EN=1;
            */
            NVMUserSettings.Sig.AvgFuelConsEn   = 1;
            NVMUserSettings.Sig.InstFuelConsEn  = 1;
            NVMUserSettings.Sig.DTEEn           = 1;
            NVMUserSettings.Sig.DrvTimeEn       = 1;
            NVMUserSettings.Sig.AvgSpeedEn      = 1;
            NVMUserSettings.Sig.TireStatEn      = 1;
            NVMUserSettings.Sig.TripEn          = 1;
            NVMUserSettings.Sig.CurrentSpeedEn  = 1;
            NVM_Data_Update ( NVM_USER_SETTINGS );
            NVM_Data_Update ( NVM_CONFIG_F110 );
            UDS_Service_Response ( 0x2E, POSITIVE_RSP, DIAG_ID_Tx, 2, UDS_ISO14229_Transfer );
            break;

        case 0xF101:
            if ( Service2EDiagDataLength != 8 + 2 )
            {
                NegRes.code = incorrectMessageLength; //数据长度不对
                UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            for ( i = 0; i < 8; i++ )
                NVMConfigF101.Data[i] = UDS_ISO14229_Transfer[i + 2];

            NVM_USER_SETTINGS_LANGUAGE = NVM_CONFIG_LANGUAGE;

            if ( NVM_CONFIG_VEHICLE_MODEL == NVM_MODEL_FE_3AC )
            {
                NVM_USER_SETTINGS_THEME = NVM_THEME_2;
            }
            else if ( NVM_CONFIG_VEHICLE_MODEL == NVM_MODEL_FE_4KA )
            {
                NVM_USER_SETTINGS_THEME = NVM_THEME_3;
            }
            else
            {
                NVM_USER_SETTINGS_THEME = NVM_THEME_1;
            }

            NVM_Data_Update ( NVM_USER_SETTINGS );
            NVM_Data_Update ( NVM_CONFIG_F101 );
            UDS_Service_Response ( 0x2E, POSITIVE_RSP, DIAG_ID_Tx, 2, UDS_ISO14229_Transfer );
            break;

        case 0xDFEC:
            if ( Service2EDiagDataLength != 64 + 2 )
            {
                NegRes.code = incorrectMessageLength; //数据长度不对
                UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            for ( i = 0; i < 64; i++ )
                diag1.DFEC[i] = UDS_ISO14229_Transfer[i + 2];

            Write_BACKUP_DATA_TO_FLASH();
            UDS_Service_Response ( 0x2E, POSITIVE_RSP, DIAG_ID_Tx, 2, UDS_ISO14229_Transfer );
            break;

        case 0xDFED:
            if ( Service2EDiagDataLength != 64 + 2 )
            {
                NegRes.code = incorrectMessageLength; //数据长度不对
                UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            for ( i = 0; i < 64; i++ )
                diag1.DFED[i] = UDS_ISO14229_Transfer[i + 2];

            Write_BACKUP_DATA_TO_FLASH();
            UDS_Service_Response ( 0x2E, POSITIVE_RSP, DIAG_ID_Tx, 2, UDS_ISO14229_Transfer );
            break;

        case 0xDFEE:
            if ( Service2EDiagDataLength != 64 + 2 )
            {
                NegRes.code = incorrectMessageLength; //数据长度不对
                UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            for ( i = 0; i < 64; i++ )
                diag1.DFEE[i] = UDS_ISO14229_Transfer[i + 2];

            Write_BACKUP_DATA_TO_FLASH();
            UDS_Service_Response ( 0x2E, POSITIVE_RSP, DIAG_ID_Tx, 2, UDS_ISO14229_Transfer );
            break;

        case 0xDFEF:
            if ( Service2EDiagDataLength != 64 + 2 )
            {
                NegRes.code = incorrectMessageLength; //数据长度不对
                UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            for ( i = 0; i < 64; i++ )
                diag1.DFEF[i] = UDS_ISO14229_Transfer[i + 2];

            Write_BACKUP_DATA_TO_FLASH();
            UDS_Service_Response ( 0x2E, POSITIVE_RSP, DIAG_ID_Tx, 2, UDS_ISO14229_Transfer );
            break;

        default:
            NegRes.code = requestOutOfRange; //会话模式不支持
            UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
            return;
            break;
    }
}

///*                    7            6             5             4                 3           2                 1       0
/*    IO_Group_1       左转向	        右转向	     位置灯	      安全带	          安全气囊	    白昼灯	       远光灯	   机油压力低
      IO_Group_2       发动机排放故障	发动机故障	 冷却液温度高	燃油低	          前雾灯	      后雾灯	       胎压故障	 EPB故障
      IO_Group_3       定速巡航	      EBD电子助力	 EPS电动助力	定速巡航状态显示	TCU 系统故障	充电系统	     ESC_OFF	 车身稳定
      IO_Group_4       陡坡缓降黄 	  陡坡缓降绿	 自动驻车绿	  自动驻车黄	      驻车制动指示	制动系统故障	 ABS
   */


void UDS_Service_2F_Indication ( uint16_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    uint16_t i;
    uint8_t Service2FDiagDataLength;
    uint16_t ControlDataIdentifier;
    uint16_t DiagIDNum;
    uint32_t TempBuffer;
    DiagIDNum = A_TA_type;

    if ( DiagIDNum == 0x7DF )
        return;

    S3_ServerCnt = 0;

    if ( SessionType != ExtendedDiagnosticSession ) //只支持扩展会话模式
    {
        NegRes.code = serviceNotSupportedInActiveSession; //会话模式不支持
        UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    if ( DiagLockFlag != NormalKeyUnlock )
    {
        NegRes.code = securityAccessDenied; //未解锁
        UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    if ( EngineSpeed >= 400 ) //转速不对
    {
        NegRes.code = conditionsNotCorrect; //会话模式不支持
        UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    Service2FDiagDataLength = A_Length;

    if ( Service2FDiagDataLength < 3 )
    {
        NegRes.code = incorrectMessageLength; //数据长度不对
        UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    for ( i = 0; i < Service2FDiagDataLength; i++ ) //长度不固定 要收数据
    {
        UDS_ISO14229_Transfer[i] = * ( A_Data + i );
    }

    ControlDataIdentifier = ( ( ( uint16_t ) UDS_ISO14229_Transfer[0] << 8 ) | ( ( uint16_t ) UDS_ISO14229_Transfer[1] ) );

    switch ( ControlDataIdentifier )
    {
        case  0x62E1:     //蜂鸣器
            if ( UDS_ISO14229_Transfer[2] == 0x00 )
            {
                if ( Service2FDiagDataLength != 3 )
                {
                    NegRes.code = incorrectMessageLength; //数据长度不对
                    UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
                }

                ContrlorResumeofBuzzer = Resume;
                ISD_Stop_Loop();
                UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 3, UDS_ISO14229_Transfer );
                return;
            }

            if ( UDS_ISO14229_Transfer[2] != 0x03 )
            {
                NegRes.code = subFunctionNotSupported; //申请超出限制
                UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            if ( Service2FDiagDataLength != 4 )
            {
                NegRes.code = incorrectMessageLength; //数据长度不对
                UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            ContrlorResumeofBuzzer = Contrl;

            if ( UDS_ISO14229_Transfer[3] == 0xff )
            {
                BuzzerStatus = 1; //蜂鸣器使用雷达音-----响
                ISD_Play_Vp_Loop ( 0X00, 0 );
                UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 4, UDS_ISO14229_Transfer );
            }
            else if ( UDS_ISO14229_Transfer[3] == 0X00 )
            {
                BuzzerStatus = 0; //蜂鸣器使用雷达音-----不响
                ISD_Stop_Loop();
                UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 4, UDS_ISO14229_Transfer );
            }
            else
            {
                NegRes.code = requestOutOfRange; //申请超出限制
                UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            break;

        case  0x62E2:     //指针测试
            if ( UDS_ISO14229_Transfer[2] == 0x00 )
            {
                if ( Service2FDiagDataLength != 3 )
                {
                    NegRes.code = incorrectMessageLength; //数据长度不对
                    UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
                }

                ContrlorResumeofFUEL = Resume;
                ContrlorResumeofTEMP = Resume;
                ContrlorResumeofVspeed = Resume;
                ContrlorResumeofEspeed = Resume;
                UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 3, UDS_ISO14229_Transfer );
                return;
            }

            if ( UDS_ISO14229_Transfer[2] != 0x03 )
            {
                NegRes.code = subFunctionNotSupported; //申请超出限制
                UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            if ( Service2FDiagDataLength != 5 )
            {
                NegRes.code = incorrectMessageLength; //数据长度不对
                UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            ContrlorResumeofFUEL = Contrl;
            ContrlorResumeofTEMP = Contrl;
            ContrlorResumeofVspeed = Contrl;
            ContrlorResumeofEspeed = Contrl;
            ESpeedVal = 0;
            VspeedVal = 0;
            VspeedValNUM = 0;
            ESpeedValNUM = 0;
            TempSeg = 0;
            FuelSeg = 0;

            if ( UDS_ISO14229_Transfer[3] == 0x01 )  //车速表指针
            {
                if ( UDS_ISO14229_Transfer[4] == 0x00 )
                {
                    MotorPosition = ZeroPosition;
                    VspeedValNUM = 0;
                    VspeedVal = 0;
                }
                else if ( UDS_ISO14229_Transfer[4] == 0x01 )
                {
                    MotorPosition = HalfPosition;
                    VspeedValNUM = 1200;
                    VspeedVal = 24;
                }
                else if ( UDS_ISO14229_Transfer[4] == 0x02 )
                {
                    VspeedVal = 37;
                    VspeedValNUM = 2400;
                    MotorPosition = FullPosition;
                }
                else
                {
                    NegRes.code = requestOutOfRange; //申请超出限制
                    UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
                }

                UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 4, UDS_ISO14229_Transfer );
            }
            else if ( UDS_ISO14229_Transfer[3] == 0X02 )   //转速表指针
            {
                if ( UDS_ISO14229_Transfer[4] == 0x00 )
                {
                    ESpeedVal = 0;
                    ESpeedValNUM = 0;       //转速
                    MotorPosition = ZeroPosition;
                }
                else if ( UDS_ISO14229_Transfer[4] == 0x01 )
                {
                    MotorPosition = HalfPosition;
                    ESpeedVal = 16;
                    ESpeedValNUM = 4000;       //转速
                }
                else if ( UDS_ISO14229_Transfer[4] == 0x02 )
                {
                    ESpeedVal = 33;
                    ESpeedValNUM = 8000;       //转速
                    MotorPosition = FullPosition;
                }
                else
                {
                    NegRes.code = requestOutOfRange; //申请超出限制
                    UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
                }

                UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 4, UDS_ISO14229_Transfer );
            }
            else if ( UDS_ISO14229_Transfer[3] == 0X03 )  //燃油表
            {
                if ( UDS_ISO14229_Transfer[4] == 0x00 )
                {
                    FuelSeg = 0;
                    MotorPosition = ZeroPosition;
                }
                else if ( UDS_ISO14229_Transfer[4] == 0x01 )
                {
                    FuelSeg = 40;
                    MotorPosition = HalfPosition;
                }
                else if ( UDS_ISO14229_Transfer[4] == 0x02 )
                {
                    FuelSeg = 80;
                    MotorPosition = FullPosition;
                }
                else
                {
                    NegRes.code = requestOutOfRange; //申请超出限制
                    UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
                }

                UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 4, UDS_ISO14229_Transfer );
            }
            else if ( UDS_ISO14229_Transfer[3] == 0X04 )      //水温表
            {
                if ( UDS_ISO14229_Transfer[4] == 0x00 )
                {
                    MotorPosition = ZeroPosition;
                    TempSeg = 0;
                }
                else if ( UDS_ISO14229_Transfer[4] == 0x01 )
                {
                    MotorPosition = HalfPosition;
                    TempSeg = 40;
                }
                else if ( UDS_ISO14229_Transfer[4] == 0x02 )
                {
                    MotorPosition = FullPosition;
                    TempSeg = 80;
                }
                else
                {
                    NegRes.code = requestOutOfRange; //申请超出限制
                    UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
                }

                UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 4, UDS_ISO14229_Transfer );
            }
            else
            {
                NegRes.code = requestOutOfRange; //申请超出限制
                UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            break;

        case  0x62E3:     //LCD测试
            if ( UDS_ISO14229_Transfer[2] == 0x00 )
            {
                if ( Service2FDiagDataLength != 3 )
                {
                    NegRes.code = incorrectMessageLength; //数据长度不对
                    UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
                }

                ContrlorResumeofLCD = Resume;
                TFT_LCD_Clear_Multi_Layer ( 112, 113 );
                UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 3, UDS_ISO14229_Transfer );
                return;
            }

            if ( UDS_ISO14229_Transfer[2] != 0x03 )
            {
                NegRes.code = subFunctionNotSupported; //申请超出限制
                UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            if ( Service2FDiagDataLength != 4 )
            {
                NegRes.code = incorrectMessageLength; //数据长度不对
                UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            ContrlorResumeofLCD = Contrl;

            if ( UDS_ISO14229_Transfer[3] == 0x01 )
            {
                DisplayColor = WhiteColor;
                //TFT_LCD_Display_Sprite(112, SN_Diag_White_bmp, 0, 0, 64);
                //TFT_LCD_Display_Sprite(113, SN_Diag_White_bmp, 400, 0, 64);
            }
            else if ( UDS_ISO14229_Transfer[3] == 0x02 )
            {
                DisplayColor = BlackColor;
                //TFT_LCD_Display_Sprite(112, SN_Diag_Black_bmp, 0, 0, 64);
                //TFT_LCD_Display_Sprite(113, SN_Diag_Black_bmp, 400, 0, 64);
            }
            else if ( UDS_ISO14229_Transfer[3] == 0x03 )
            {
                DisplayColor = RedColor;
                //TFT_LCD_Display_Sprite(112, SN_Diag_Red_bmp, 0, 0, 64);
                //TFT_LCD_Display_Sprite(113, SN_Diag_Red_bmp, 400, 0, 64);
            }
            else if ( UDS_ISO14229_Transfer[3] == 0x04 )
            {
                DisplayColor = GreenColor;
                //TFT_LCD_Display_Sprite(112, SN_Diag_Green_bmp , 0, 0, 64);
                //TFT_LCD_Display_Sprite(113, SN_Diag_Green_bmp, 400, 0, 64);
            }
            else if ( UDS_ISO14229_Transfer[3] == 0x05 )
            {
                DisplayColor = BlueColor;
                //TFT_LCD_Display_Sprite(112, SN_Diag_Blue_bmp, 0, 0, 64);
                //TFT_LCD_Display_Sprite(113, SN_Diag_Blue_bmp, 400, 0, 64);
            }
            else
            {
                NegRes.code = requestOutOfRange; //申请超出限制
                UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 4, UDS_ISO14229_Transfer );
            break;

        case  0x62E4:   //所有报警灯测试
            if ( UDS_ISO14229_Transfer[2] == 0x00 )
            {
                if ( Service2FDiagDataLength != 3 )
                {
                    NegRes.code = incorrectMessageLength; //数据长度不对
                    UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
                }

                ContrlorResumeofIO = Resume;
                UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 3, UDS_ISO14229_Transfer );
                return;
            }

            if ( UDS_ISO14229_Transfer[2] != 0x03 )
            {
                NegRes.code = subFunctionNotSupported; //申请超出限制
                UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            if ( Service2FDiagDataLength != 4 )
            {
                NegRes.code = incorrectMessageLength; //数据长度不对
                UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            ContrlorResumeofIO = Contrl;

            if ( UDS_ISO14229_Transfer[3] == 0xff )
            {
                LampWarningStatus = 1;
                IO_Group_1 = 0XFF;
                IO_Group_2 = 0XFF ;
                IO_Group_3 = 0XFF ;
                IO_Group_4 = 0XFF ;
                IO_MASK_1  = 0XFF ;
                IO_MASK_2  = 0XFF ;
                IO_MASK_3  = 0XFF ;
                IO_MASK_4  = 0XFF;
                UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 4, UDS_ISO14229_Transfer );
            }
            else if ( UDS_ISO14229_Transfer[3] == 0X00 )
            {
                LampWarningStatus = 0;
                IO_Group_1 = 0;
                IO_Group_2 = 0 ;
                IO_Group_3 = 0 ;
                IO_Group_4 = 0 ;
                IO_MASK_1  = 0 ;
                IO_MASK_2  = 0 ;
                IO_MASK_3  = 0 ;
                IO_MASK_4  = 0;
                UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 4, UDS_ISO14229_Transfer );
            }
            else
            {
                NegRes.code = requestOutOfRange; //申请超出限制
                UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }

            break;

        case 0xFE01:    //车速表控制
            switch ( UDS_ISO14229_Transfer[2] )
            {
                case 0x00://ReturnControlToECU
                    if ( Service2FDiagDataLength != 3 )
                    {
                        NegRes.code = incorrectMessageLength; //数据长度不对
                        UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                        return;
                    }

                    ContrlorResumeofVspeed = Resume;
                    UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 3, UDS_ISO14229_Transfer );
                    break;

                case 0x03:
                    if ( Service2FDiagDataLength != 5 )
                    {
                        NegRes.code = incorrectMessageLength; //数据长度不对
                        UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                        return;
                    }

                    ContrlorResumeofVspeed = Contrl;
                    TempBuffer = ( ( uint16_t ) UDS_ISO14229_Transfer[3] << 8 ) | UDS_ISO14229_Transfer[4];
                    TempBuffer = TempBuffer * 9;
                    TempBuffer = TempBuffer >> 4;

                    //10倍车速值
                    if ( TempBuffer > 2400 )
                        TempBuffer = 2400;

                    if ( TempBuffer == 0 )
                        VspeedVal = 0;
                    else if ( ( TempBuffer <= 50 ) && ( TempBuffer > 0 ) )
                        VspeedVal = 1;
                    else if ( ( TempBuffer <= 1200 ) && ( TempBuffer > 50 ) )
                        VspeedVal = TempBuffer / 50;
                    else if ( ( TempBuffer <= 2400 ) && ( TempBuffer > 1200 ) )
                        VspeedVal = ( TempBuffer - 1200 ) / 100 + 24;
                    else
                        VspeedVal = 37;

                    VspeedValNUM = TempBuffer;
                    UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 5, UDS_ISO14229_Transfer );
                    break;

                default:
                    NegRes.code = requestOutOfRange; //申请超出限制
                    UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
                    break;
            }

            break;

        case 0xFE02:      //转速表控制
            switch ( UDS_ISO14229_Transfer[2] )
            {
                case 0x00://ReturnControlToECU
                    if ( Service2FDiagDataLength != 3 )
                    {
                        NegRes.code = incorrectMessageLength; //数据长度不对
                        UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                        return;
                    }

                    ContrlorResumeofEspeed = Resume;
                    UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 3, UDS_ISO14229_Transfer );
                    break;

                case 0x03:
                    if ( Service2FDiagDataLength != 5 )
                    {
                        NegRes.code = incorrectMessageLength; //数据长度不对
                        UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                        return;
                    }

                    ContrlorResumeofEspeed = Contrl;
                    TempBuffer = ( ( uint16_t ) UDS_ISO14229_Transfer[3] << 8 ) | UDS_ISO14229_Transfer[4];
                    TempBuffer = TempBuffer >> 2;

                    if ( TempBuffer > 8000 )
                        TempBuffer = 8000;

                    if ( ( TempBuffer > 0 ) && ( TempBuffer <= 250 ) )
                        ESpeedVal = 1;
                    else if ( ( TempBuffer > 250 ) && ( TempBuffer <= 8000 ) )
                        ESpeedVal = TempBuffer / 250 + 1;
                    else
                        ESpeedVal = 0;

                    ESpeedValNUM = TempBuffer;
                    UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 5, UDS_ISO14229_Transfer );
                    break;

                default:
                    NegRes.code = requestOutOfRange; //申请超出限制
                    UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
                    break;
            }

            break;

        case 0xFE03:           //io控制
            switch ( UDS_ISO14229_Transfer[2] )
            {
                case 0x00://ReturnControlToECU
                    if ( Service2FDiagDataLength != 3 )
                    {
                        NegRes.code = incorrectMessageLength; //数据长度不对
                        UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                        return;
                    }

                    ContrlorResumeofIO = Resume;
                    UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 3, UDS_ISO14229_Transfer );
                    break;

                case 0x03:
                    if ( Service2FDiagDataLength != 11 )
                    {
                        NegRes.code = incorrectMessageLength; //数据长度不对
                        UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                        return;
                    }

                    ContrlorResumeofIO = Contrl;
                    IO_Group_1 = UDS_ISO14229_Transfer[3] ;
                    IO_Group_2 = UDS_ISO14229_Transfer[4] ;
                    IO_Group_3 = UDS_ISO14229_Transfer[5] ;
                    IO_Group_4 = UDS_ISO14229_Transfer[6] ;
                    IO_MASK_1  = UDS_ISO14229_Transfer[7] ;
                    IO_MASK_2  = UDS_ISO14229_Transfer[8] ;
                    IO_MASK_3  = UDS_ISO14229_Transfer[9] ;
                    IO_MASK_4  = UDS_ISO14229_Transfer[10];
                    UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 7, UDS_ISO14229_Transfer );
                    break;

                default:
                    NegRes.code = requestOutOfRange; //申请超出限制
                    UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
                    break;
            }

            break;

        case 0xFE04:        //水温控制
            switch ( UDS_ISO14229_Transfer[2] )
            {
                case 0x00://ReturnControlToECU
                    if ( Service2FDiagDataLength != 3 )
                    {
                        NegRes.code = incorrectMessageLength; //数据长度不对
                        UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                        return;
                    }

                    ContrlorResumeofTEMP = Resume;
                    UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 3, UDS_ISO14229_Transfer );
                    break;

                case 0x03:
                    if ( Service2FDiagDataLength != 4 )
                    {
                        NegRes.code = incorrectMessageLength; //数据长度不对
                        UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                        return;
                    }

                    ContrlorResumeofTEMP = Contrl;
                    TempBuffer = UDS_ISO14229_Transfer[3];

                    if ( TempBuffer > 8 )
                        TempBuffer = 8;

                    TempSeg = TempBuffer * 10;
                    UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 4, UDS_ISO14229_Transfer );
                    break;

                default:
                    NegRes.code = requestOutOfRange; //申请超出限制
                    UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
                    break;
            }

            break;

        case 0xFE05:      //燃油控制
            switch ( UDS_ISO14229_Transfer[2] )
            {
                case 0x00://ReturnControlToECU
                    if ( Service2FDiagDataLength != 3 )
                    {
                        NegRes.code = incorrectMessageLength; //数据长度不对
                        UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                        return;
                    }

                    ContrlorResumeofFUEL = Resume;
                    UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 3, UDS_ISO14229_Transfer );
                    break;

                case 0x03:
                    if ( Service2FDiagDataLength != 4 )
                    {
                        NegRes.code = incorrectMessageLength; //数据长度不对
                        UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                        return;
                    }

                    ContrlorResumeofFUEL = Contrl;
                    TempBuffer = UDS_ISO14229_Transfer[3];

                    if ( TempBuffer > 8 )
                        TempBuffer = 8;

                    FuelSeg = TempBuffer * 10;
                    UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 4, UDS_ISO14229_Transfer );
                    break;

                default:
                    NegRes.code = requestOutOfRange; //申请超出限制
                    UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
                    break;
            }

            break;

        default:
            NegRes.code = requestOutOfRange; //会话模式不支持
            UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
            return;
            break;
    }
}


void UDS_Service_14_Indication ( uint16_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    uint16_t i;
    uint8_t Service14DiagDataLength;
    uint32_t ClearDTCGroup;
    Service14DiagDataLength = A_Length;
    S3_ServerCnt = 0;

    if ( Service14DiagDataLength != 3 )
    {
        NegRes.code = incorrectMessageLength; //数据长度不对
        UDS_Service_Response ( 0x14, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    if ( SessionType == ProgrammingSession ) //不支持编程会话
    {
        NegRes.code = serviceNotSupportedInActiveSession; //会话模式不支持
        UDS_Service_Response ( 0x14, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    if ( EngineSpeed >= 400 ) //转速不对
    {
        NegRes.code = conditionsNotCorrect; //会话模式不支持
        UDS_Service_Response ( 0x14, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    for ( i = 0; i < Service14DiagDataLength; i++ ) //长度不固定 要收数据
    {
        UDS_ISO14229_Transfer[i] = * ( A_Data + i );
    }

    ClearDTCGroup = ( ( ( uint32_t ) UDS_ISO14229_Transfer[0] << 16 ) | ( ( uint32_t ) UDS_ISO14229_Transfer[1] << 8 ) | ( ( uint32_t ) UDS_ISO14229_Transfer[2] ) );

    if ( ClearDTCGroup == 0xffffff )
    {
        //ClearAllDTC();
        for ( i = 0; i < cnDTCslen; i++ )
        {
            DTC_STATUS[i] = ( DTC_STATUS[i] & 0x40 ) | 0x10;     //修改DTC状态记录
            RecDTC_Wait_NoBusoff[i].Flag = 0;
            RecDTC_Wait_NoBusoff[i].Timer = 0;
        }

        UDS_Service_Response ( 0x14, POSITIVE_RSP, DIAG_ID_Tx, 0, UDS_ISO14229_Transfer );
        DTCstatusCHGFF = 1;
        SaveDTCsToEEPROM();
    }
    else
    {
        NegRes.code = requestOutOfRange; //子功能不支持
        UDS_Service_Response ( 0x14, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
}



void HandleOf2FServices ( void )
{
    if ( ContrlorResumeofIO != Contrl )
    {
        if ( ContrlorResumeofIOLast == Contrl )
        {
            /*
            ContrlorResumeofIOLast=Resume;
            SEG_LCD_Icon_LEFT_G_Out(SEG_LCD_ICON_FREE);
            SEG_LCD_Icon_RIGHT_G_Out(SEG_LCD_ICON_FREE);
            SEG_LCD_Icon_POSITION_G_Out(SEG_LCD_ICON_FREE);
            GUI_Icon_Seatbelt_R_Display(SEG_LCD_ICON_FREE);
            SEG_LCD_Icon_AIRBAG_R_Out (SEG_LCD_ICON_FREE);
            GUI_Icon_DRL_G_Display(SEG_LCD_ICON_FREE);
            SEG_LCD_Icon_HI_BEAM_B_Out(SEG_LCD_ICON_FREE);
            SEG_LCD_Icon_LOW_OIL_PRESSURE_R_Out(SEG_LCD_ICON_FREE);
            SEG_LCD_Icon_MIL_Y_Out(SEG_LCD_ICON_FREE);
            GUI_Icon_SVS_Y_Display(SEG_LCD_ICON_FREE);
            GUI_Icon_Hi_Temp_R_Display(ICON_FREE);
            GUI_Icon_Low_Fuel_Y_Display(ICON_FREE);
            SEG_LCD_Icon_FOG_F_G_Out(ICON_FREE);
            SEG_LCD_Icon_FOG_R_Y_Out(ICON_FREE);
            GUI_Icon_Tire_Pressure_Y_Display(ICON_FREE);
            GUI_Icon_EPB_Y_Display(ICON_FREE);
            GUI_Icon_Cruise_G_Display(ICON_FREE);
            GUI_Icon_EBD_Y_Display(ICON_FREE);
            GUI_Icon_EPS_Y_Display(ICON_FREE);
            GUI_Icon_Cruise_State_G_Display(ICON_FREE);
            GUI_Icon_TCU_Y_Display(ICON_FREE);
            GUI_Icon_Low_Battery_R_Display(ICON_FREE);
            GUI_Icon_ESC_Off_Y_Display(ICON_FREE);
            GUI_Icon_ESC_Y_Display(ICON_FREE);
            GUI_Icon_HDC_Y_Display(ICON_FREE);
            GUI_Icon_HDC_G_Display(ICON_FREE);
            GUI_Icon_AVH_G_Display(ICON_FREE);
            GUI_Icon_AVH_R_Display(ICON_FREE);
            SEG_LCD_Icon_PARK_R_Out(ICON_FREE);
            GUI_Icon_Brake_R_Display(ICON_FREE);
            GUI_Icon_ABS_Y_Display(ICON_FREE);
            */
            return;
        }

        return;
    }

    //BYTE0
    /*
         if(bit_is_set(IO_Group_1,7)&&bit_is_set(IO_MASK_1,7))
               SEG_LCD_Icon_LEFT_G_Out(SEG_LCD_ICON_FORCE_ON);
         else
               SEG_LCD_Icon_LEFT_G_Out(SEG_LCD_ICON_FORCE_OFF);

         if(bit_is_set(IO_Group_1,6)&&bit_is_set(IO_MASK_1,6))
              SEG_LCD_Icon_RIGHT_G_Out(SEG_LCD_ICON_FORCE_ON);
      	 else
      	      SEG_LCD_Icon_RIGHT_G_Out(SEG_LCD_ICON_FORCE_OFF);

      	 if(bit_is_set(IO_Group_1,5)&&bit_is_set(IO_MASK_1,5))
      	       SEG_LCD_Icon_POSITION_G_Out(SEG_LCD_ICON_FORCE_ON);
      	 else
      	       SEG_LCD_Icon_POSITION_G_Out(SEG_LCD_ICON_FORCE_OFF);

      	 if(bit_is_set(IO_Group_1,4)&&bit_is_set(IO_MASK_1,4))
      	     GUI_Icon_Seatbelt_R_Display(SEG_LCD_ICON_FORCE_ON);
      	 else
      	     GUI_Icon_Seatbelt_R_Display(SEG_LCD_ICON_FORCE_OFF);

      	 if(bit_is_set(IO_Group_1,3)&&bit_is_set(IO_MASK_1,3))
      	    SEG_LCD_Icon_AIRBAG_R_Out(SEG_LCD_ICON_FORCE_ON);
      	 else
      	    SEG_LCD_Icon_AIRBAG_R_Out(SEG_LCD_ICON_FORCE_OFF);

      	 if(bit_is_set(IO_Group_1,2)&&bit_is_set(IO_MASK_1,2))
      	    GUI_Icon_DRL_G_Display(SEG_LCD_ICON_FORCE_ON);
      	 else
      	    GUI_Icon_DRL_G_Display(SEG_LCD_ICON_FORCE_OFF);

      	 if(bit_is_set(IO_Group_1,1)&&bit_is_set(IO_MASK_1,1))
      	   SEG_LCD_Icon_HI_BEAM_B_Out(SEG_LCD_ICON_FORCE_ON);
      	 else
      	   SEG_LCD_Icon_HI_BEAM_B_Out(SEG_LCD_ICON_FORCE_OFF);

      	 if(bit_is_set(IO_Group_1,0)&&bit_is_set(IO_MASK_1,0))
      	   SEG_LCD_Icon_LOW_OIL_PRESSURE_R_Out(SEG_LCD_ICON_FORCE_ON);
      	 else
      	   SEG_LCD_Icon_LOW_OIL_PRESSURE_R_Out(SEG_LCD_ICON_FORCE_OFF);
      */
    //BYTE1
    /*
         if(bit_is_set(IO_Group_2,7)&&bit_is_set(IO_MASK_2,7))
               SEG_LCD_Icon_MIL_Y_Out(SEG_LCD_ICON_FORCE_ON);
         else
               SEG_LCD_Icon_MIL_Y_Out(SEG_LCD_ICON_FORCE_OFF);

         if(bit_is_set(IO_Group_2,6)&&bit_is_set(IO_MASK_2,6))
              GUI_Icon_SVS_Y_Display(SEG_LCD_ICON_FORCE_ON);
      	 else
      	      GUI_Icon_SVS_Y_Display(SEG_LCD_ICON_FORCE_OFF);

      	 if(bit_is_set(IO_Group_2,5)&&bit_is_set(IO_MASK_2,5))
      	       GUI_Icon_Hi_Temp_R_Display(SEG_LCD_ICON_FORCE_ON);
      	 else
      	       GUI_Icon_Hi_Temp_R_Display(SEG_LCD_ICON_FORCE_OFF);

      	 if(bit_is_set(IO_Group_2,4)&&bit_is_set(IO_MASK_2,4))
      	     GUI_Icon_Low_Fuel_Y_Display(SEG_LCD_ICON_FORCE_ON);
      	 else
      	     GUI_Icon_Low_Fuel_Y_Display(SEG_LCD_ICON_FORCE_OFF);

      	 if(bit_is_set(IO_Group_2,3)&&bit_is_set(IO_MASK_2,3))
      	    SEG_LCD_Icon_FOG_F_G_Out(SEG_LCD_ICON_FORCE_ON);
      	 else
      	    SEG_LCD_Icon_FOG_F_G_Out(SEG_LCD_ICON_FORCE_OFF);

      	 if(bit_is_set(IO_Group_2,2)&&bit_is_set(IO_MASK_2,2))
      	    SEG_LCD_Icon_FOG_R_Y_Out(SEG_LCD_ICON_FORCE_ON);
      	 else
      	    SEG_LCD_Icon_FOG_R_Y_Out(SEG_LCD_ICON_FORCE_OFF);

      	 if(bit_is_set(IO_Group_2,1)&&bit_is_set(IO_MASK_2,1))
      	   GUI_Icon_Tire_Pressure_Y_Display(SEG_LCD_ICON_FORCE_ON);
      	 else
      	   GUI_Icon_Tire_Pressure_Y_Display(SEG_LCD_ICON_FORCE_OFF);

      	 if(bit_is_set(IO_Group_2,0)&&bit_is_set(IO_MASK_2,0))
      	   GUI_Icon_EPB_Y_Display(SEG_LCD_ICON_FORCE_ON);
      	 else
      	   GUI_Icon_EPB_Y_Display(SEG_LCD_ICON_FORCE_OFF);

      */
    //BYTE2
    /*
         if(bit_is_set(IO_Group_3,7)&&bit_is_set(IO_MASK_3,7))
               GUI_Icon_Cruise_G_Display(SEG_LCD_ICON_FORCE_ON);
         else
               GUI_Icon_Cruise_G_Display(SEG_LCD_ICON_FORCE_OFF);

         if(bit_is_set(IO_Group_3,6)&&bit_is_set(IO_MASK_3,6))
              GUI_Icon_EBD_Y_Display(SEG_LCD_ICON_FORCE_ON);
      	 else
      	      GUI_Icon_EBD_Y_Display(SEG_LCD_ICON_FORCE_OFF);

      	 if(bit_is_set(IO_Group_3,5)&&bit_is_set(IO_MASK_3,5))
      	       GUI_Icon_EPS_Y_Display(SEG_LCD_ICON_FORCE_ON);
      	 else
      	       GUI_Icon_EPS_Y_Display(SEG_LCD_ICON_FORCE_OFF);

      	 if(bit_is_set(IO_Group_3,4)&&bit_is_set(IO_MASK_3,4))
      	     GUI_Icon_Cruise_State_G_Display(SEG_LCD_ICON_FORCE_ON);
      	 else
      	     GUI_Icon_Cruise_State_G_Display(SEG_LCD_ICON_FORCE_OFF);

      	 if(bit_is_set(IO_Group_3,3)&&bit_is_set(IO_MASK_3,3))
      	    GUI_Icon_TCU_Y_Display(SEG_LCD_ICON_FORCE_ON);
      	 else
      	    GUI_Icon_TCU_Y_Display(SEG_LCD_ICON_FORCE_OFF);

      	 if(bit_is_set(IO_Group_3,2)&&bit_is_set(IO_MASK_3,2))
      	    GUI_Icon_Low_Battery_R_Display(SEG_LCD_ICON_FORCE_ON);
      	 else
      	    GUI_Icon_Low_Battery_R_Display(SEG_LCD_ICON_FORCE_OFF);

      	 if(bit_is_set(IO_Group_3,1)&&bit_is_set(IO_MASK_3,1))
      	   GUI_Icon_ESC_Off_Y_Display(SEG_LCD_ICON_FORCE_ON);
      	 else
      	   GUI_Icon_ESC_Off_Y_Display(SEG_LCD_ICON_FORCE_OFF);

      	 if(bit_is_set(IO_Group_3,0)&&bit_is_set(IO_MASK_3,0))
      	   GUI_Icon_ESC_Y_Display(SEG_LCD_ICON_FORCE_ON);
      	 else
      	   GUI_Icon_ESC_Y_Display(SEG_LCD_ICON_FORCE_OFF);
     */
    //BYTE3
    /*
    if(bit_is_set(IO_Group_4,7)&&bit_is_set(IO_MASK_4,7))
             GUI_Icon_HDC_Y_Display(SEG_LCD_ICON_FORCE_ON);
       else
             GUI_Icon_HDC_Y_Display(SEG_LCD_ICON_FORCE_OFF);

       if(bit_is_set(IO_Group_4,6)&&bit_is_set(IO_MASK_4,6))
            GUI_Icon_HDC_G_Display(SEG_LCD_ICON_FORCE_ON);
    	 else
    	      GUI_Icon_HDC_G_Display(SEG_LCD_ICON_FORCE_OFF);

    	 if(bit_is_set(IO_Group_4,5)&&bit_is_set(IO_MASK_4,5))
    	       GUI_Icon_AVH_G_Display(SEG_LCD_ICON_FORCE_ON);
    	 else
    	       GUI_Icon_AVH_G_Display(SEG_LCD_ICON_FORCE_OFF);

    	 if(bit_is_set(IO_Group_4,4)&&bit_is_set(IO_MASK_4,4))
    	     GUI_Icon_AVH_R_Display(SEG_LCD_ICON_FORCE_ON);
    	 else
    	     GUI_Icon_AVH_R_Display(SEG_LCD_ICON_FORCE_OFF);

    	 if(bit_is_set(IO_Group_4,3)&&bit_is_set(IO_MASK_4,3))
    	    SEG_LCD_Icon_PARK_R_Out(SEG_LCD_ICON_FORCE_ON);
    	 else
    	    SEG_LCD_Icon_PARK_R_Out(SEG_LCD_ICON_FORCE_OFF);

    	 if(bit_is_set(IO_Group_4,2)&&bit_is_set(IO_MASK_4,2))
    	    GUI_Icon_Brake_R_Display(SEG_LCD_ICON_FORCE_ON);
    	 else
    	    GUI_Icon_Brake_R_Display(SEG_LCD_ICON_FORCE_OFF);

    	 if(bit_is_set(IO_Group_4,1)&&bit_is_set(IO_MASK_4,1))
    	   GUI_Icon_ABS_Y_Display(SEG_LCD_ICON_FORCE_ON);
    	 else
    	   GUI_Icon_ABS_Y_Display(SEG_LCD_ICON_FORCE_OFF);
       */
    ContrlorResumeofIOLast = ContrlorResumeofIO;
}



