/******************************************************************************
?? ?? ??;UDS_ISO14229_Services.c
????????;ISO 14229 ?œZ?ÕÇ?????????????????????????????
??    ??;????
??    ??;V1.0
??    ??;2016.11.1
******************************************************************************/
//#include "Include.h"
//#include "Const.h"

#include <hidef.h>           /* common defines and macros */
#include "derivative.h"      /* derivative-specific definitions */
#include "mc9s12xhy256.h"
#include "stdint.h"
#include <string.h> 
#include "comdef.h"
#include "Unit_Conv.h"

//#include "AutoGUI.h"
#include "main.h" 

/* Driver */
#include "CRG.h"
#include "ADC.h"
#include "GPIO.h"
#include "MSCAN0.h"
#include "PWM.h"
#include "SCI0_UART.h"

#include "Sound_List.h"

/* System */ 
#include "PowerManagement.h"
#include "API.h" 
#include "Line_In.h"
#include "Key.h"
#include "CRG.h"

/* Protocol */
#include "CAN_Communication_Matrix.h"
#include "Analog_Signal_Conversion.h"
#include "Communication_Over_CAN.h"
#include "Simulated_IIC_Master.h"
#include "Simulated_SPI_Master.h"
#include "YGV642_SPI.h"

/* Device */
#include "EEPROM_24Cxx.h"
#include "EEPROM_24Cxx_Remap.h"
#include "ISD2360.h"
#include "LED.h"
#include "GUI.h"
#include "YGV642.h"

/* Module */
#include "Flash_synchronizer.h"

#include "Non_volatile_Memorys.h"
#include "Sound_Player.h"
#include "System_Status_Monitor.h"
#include "TFT_LCD.h"

/* Implementation */
#include "Popup_List.h"
#include "IO_Manager.h"
#include "Popup_Scheduler.h"
#include "Sound_Scheduler.h"
#include "Warning_Processor.h"
#include "Data_Processor.h"
#include "IPC.h"

/* Application */

#include "Telltales.h"
//#include "Back_Light.h"
#include "Drive_Info.h"
#include "Gauges.h"
#include "Menus.h"
#include "Popups.h"

#include "Startup_Animation.h"

#include "FuelConfig.h" 
//#include "FuelConfig1.h" 
 
/*Diagnostic*/
#include "Diag_ID_Def.h"
#include "DoCAN_ISO15765_Config.h"
#include "DoCAN_ISO15765.h"
#include "UDS_ISO14229_Services.h"
#include "Diag_ID_Def.h"
#include "UDS_ISO14229_Server.h"
#include "UDS_ISO14229_Server_Config.h"

#include "API_BD8379.h"
//#define TESTDIAG

//DTC
uint32_t msgList[3][4] = {

    {0x10FF7521, 8, 4000,    0, },
	{0x18FEBF0B, 8, 1000,    0, },
	{0x0CF00400, 8, 400,    0,  },

};
uint16_t DTCTimerList [ cnDTCslen ] = {0};
uint8_t isNeedSaveDTC  = 0;

uint8_t DTCUpdataStatusList[ cnDTCslen ] = {0};
uint8_t  isEnable       = 0;
uint16_t enableTimer    = 0;
uint16_t enableMaxTimer = KL15DTCEnableTimer;
uint8_t VoltageStatus = NormalVottage;
uint8_t UDSKL15Status = 0;

const uint32_t MASK0_Safe= 0x8EACBD9F;
const uint32_t MASK1_Safe= 0x1CBE5D3A;
const uint32_t MASK2_Safe= 0x2CBDE543 ;
DIGKeyValue      DigKeyValue; 

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

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???????????
uint16_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 ValidSeedKey1;       //?????§¹?????
//uint32_t ValidSeedKey4;       //?????§¹?????
uint32_t ValidSeedKey2;       //?????§¹?????
uint16_t SeedHigh;
uint16_t SeedLow;
uint8_t Services27_01_Requested;
uint8_t noDataLenth;
uint8_t Time10sLock;
uint16_t EngineSpeed;       //?????????
uint8_t MotorPosition;      //???¦Ë??
uint8_t DisplayColor;       //???????????
uint8_t ControlMotor;       //????????????????

uint16_t ESpeedVal;         //???
uint16_t VspeedVal;         //????
uint8_t TempSeg;            //???

uint8_t BuzzerStatus;       //????????

uint8_t SessionType = 1;    //??????
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  ContrlorResumeofSOC;     //soc????
uint8_t  SocSeg;                  //Soc
uint8_t  ContrlorResumeofEspeed;  //??????
uint16_t ESpeedValNUM;            //???
uint8_t  ContrlorResumeofVspeed;  //???????
uint16_t VspeedValNUM;            //????
//----hyq--20190827 ???????
uint8_t  ContrlorResumeofGAS1  ;  //???1

uint8_t  ContrlorResumeofLamp;    //???§Ò????????
uint8_t LampWarningStatus;        //?????

uint8_t  ContrlorResumeofBuzzer;  //??????????
uint8_t  ContrlorResumeofGauge;   //???§Ò??????
uint8_t  ContrlorResumeofLCD;     //LCD????
uint8_t  ContrlorResumeofIO;      //IO????
uint8_t  ContrlorResumeofIOLast;  //IO????

uint8_t  ContrlorResumeofBuzzer;  //??????????
uint8_t  ContrlorResumeofGauge;   //???§Ò??????
uint8_t  ContrlorResumeofLCD;     //LCD????
uint8_t  ContrlorResumeofIO;      //IO????
uint8_t  ContrlorResumeofIOLast;  //IO????

uint8_t  ContrlorResumeofTEMP;    //??¡À?????
uint8_t  CAN_NORMAL_TT;
uint8_t  CAN_NORMAL_2SFF;
uint8_t  CAN_DTC_OFF;
uint8_t  ACC_OK_FF;

uint8_t  CANNetTxOFF = 0;
uint8_t  CANNetRxOFF = 0;
uint8_t  CoCanTxOff = 0;
Diag27Data  diag27;
uint8_t UPDOWNFLAG;
uint8_t CJINF[18];		//?›¥???????(18¦ËASSIC?????)(??????EEPROM????????)
_DTC DiagDtc[cnDTCslen];
RecDTC_WaitBusoff_STRUCT	RecDTC_Wait_NoBusoff[cnDTCslen];
INT8U HIGH_VOLTAGE;
INT8U LOW_VOLTAGE;
INT8U BUS_OFF_FLAG;
INT8U VoltageACKFF;
//DTC?????
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;
DIAG_2E_DATA   Diag2eData;
DIAG_Boot_DATA DiagBootData;
/******************************************************************************
Get Current Sector
******************************************************************************/
/*uint8_t GET_Global_sector_PRG ( uint32_t address )
{
#if 0
    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;
#endif
}
*/

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;
    //?????????
    uint32_t LogicAddress = address - DFLASH_FirstAddress;
    //DFLASH??PAGE?????????
    EPAGE = (uint8_t)(LogicAddress / 0x400);
    //??PAGE?§Ö??????
    LogicAddress %= 0x400;
    //?????????????????
    Data16 = ( *(volatile unsigned short * )(0x800 + LogicAddress));
    return Data16;
}

uint8_t DFLASH_Read_Word1(INT32U Adrr, uint32_t *Data,uint8_t len)
{
  uint8_t i;
  uint8_t eStatus;
  uint16_t *DataTmp;
  uint8_t PageTmp;  
  uint32_t LogicAddr;
  
  eStatus =0;

  if(len > 256)
  {
     eStatus = LENGTH_OUT_OF_RANGE;
  }

  len = len / 2;

  if(eStatus == 0)
  {
      PageTmp = (INT8U)( Adrr / 0x400); 
      EPAGE =(INT8U )(PageTmp & 0xff);
      DataTmp = (INT16U*)Data;
      LogicAddr = (Adrr - DFLASH_FirstAddress) % 0x400 + 0x800;
      for(i =0;i<len;i++)
      {
          *DataTmp = (*(volatile unsigned short * )(LogicAddr + (i * 2)));
          DataTmp++;
      }
  }
  
  return eStatus;	/**/
}

/*-------------------------------------------------------------------------
* Function Name  : write2EData
* Description    :
* Input          :
* Output         : None
* Return         : write result
* onther         :
--------------------------------------------------------------------------*/
uint8_t write2EData(void)
{
    uint8_t enResult = 0;
    
    enResult = WriteDFlashData(DFLASH_Ser2EAddress, (INT32U *)&Diag2eData.Flag, sizeof(Diag2eData));
    
    return enResult;
}

/*-------------------------------------------------------------------------
* Function Name  : WriteDFlashData
* Description    :
* Input          :
* Output         : None
* Return         : write result
* onther         :
--------------------------------------------------------------------------*/
uint8_t WriteDFlashData(uint32_t u8BlockNum, uint32_t u32Data[], uint16_t u16Len)
{
    uint8_t enResult = 0;
    enResult = Write_Base_DATA_FROM_FLASH(u8BlockNum,(uint8_t *)u32Data, u16Len);
    return enResult;
}

/*-------------------------------------------------------------------------
* Function Name  : WriteDFlashData
* Description    :
* Input          :
* Output         : None
* Return         : write result
* onther         :
--------------------------------------------------------------------------*/
uint8_t Write_Base_DATA_FROM_FLASH(uint32_t Adrr, uint8_t *Data,uint8_t len)
{
    uint8_t status  = 0;
    status = D_P_FLASH_Erase_Sector(0,Adrr);
    if(status != 0)
    {
        status = ACCESS_ERROR;
    }
    else
    {
        status = DFLASH_N_Program(Adrr, Data, len);
        if(status != 0) 
        {
          status = ACCESS_ERROR;
        }
    }
  return status;/**/
}

/*-------------------------------------------------------------------------
* Function Name  : ReadDFlashData
* Description    :
* Input          :
* Output         : None
* Return         : write result
* onther         :
--------------------------------------------------------------------------*/
uint8_t ReadDFlashData(uint32_t u8BlockNum, uint32_t u32Data[], uint16_t u16Len, InitFunction initFunction)
{
    uint8_t enResult = 0;
    enResult = DFLASH_Read_Word1(u8BlockNum, (uint8_t *)u32Data, u16Len);
    if(DFLASH_Ser2EAddress == u8BlockNum) 
    {
      if(enResult == 0) 
      {
        if (*u32Data != 0x7AA7A55Au)
        {
          if(initFunction != NULL) 
          {
            initFunction();
            enResult = WriteDFlashData(u8BlockNum, u32Data, u16Len);
          }
        }
      }
    } 
    else 
    {
    } 
    return enResult;
}
/*-------------------------------------------------------------------------
* Function Name  : Ser2EToDFlashInfoInit
* Description    :
* Input          :
* Output         : None
* Return         : None
* onther         :
--------------------------------------------------------------------------*/
void Ser2EToDFlashInfoInit(void) 
{
   Diag2eData.Flag = 0x7AA7A55Au;
   memset(Diag2eData.D_F190, 0x20, 17u);
   
   memset(Diag2eData.D_F1A8, 0x20, 20u);
   Diag2eData.D_F1A8[0] = 0x00;
   Diag2eData.D_F1A8[1] = 0x00;
   Diag2eData.D_F1A8[2] = 0x01;
   Diag2eData.D_F1A8[3] = 0x01;
   
   memset(Diag2eData.D_F1FA, 0x00, 6);
   
   memset(Diag2eData.D_3200, 0x00, 16);
   
   //ConfigItem.EngineConfig = Diag2eData.D_3200[0];
   //ConfigItem.ACUConfig    = Diag2eData.D_3200[1];
   //ConfigItem.ADASConfig   = Diag2eData.D_3200[2];
   

}


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 = 0;

    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;
    INT16U 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 )
{
    INT8U mRUL = 0;
    if ( DFLASH_Erase_Sector ( DFLASH_APPvAddress ) != OK )
        return;

    mRUL=DFLASH_Program ( DFLASH_APPvAddress + 4, &bz, 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;
        Write_27_DATA_TO_FLASH();
    }
    else
    {
        DFLASHeep_adr = DFLASH_LockAddress + 2;
        ReadNbyteFrDFalsh ( ( INT16U * ) &diag27, sizeof ( diag27 ) );
        
        if (diag27.RequestSeedCnt != 0 || diag27.attemptcnt != 0){
            diag27.RequestSeedCnt = 3;
            diag27.attemptcnt = 3;
            Wait10sFlag = 1;
            wait10cnt = 0;
        }
    }
}
//========================================================
//§Õ ???§¿??2E???? ??????FALSH
//========================================================
void Write_27_DATA_TO_FLASH ( void )
{
    INT16U m16;
    //????27????flash?›¥????
    MYeturnCode = DFLASH_Erase_Sector ( DFLASH_LockAddress );							//??????3??????(256???)
    //?????????????
    if ( MYeturnCode != OK )
        return;

    DFLASHeep_adr = DFLASH_LockAddress + 2;
    //??????§Õ??falsh??
    WriteNbyteToDFalsh ( ( INT16U * ) &diag27, sizeof ( diag27 ) );
    m16 = 0x7AA7;
    MYeturnCode = DFLASH_N_Program ( DFLASH_LockAddress, &m16, 1 );
}

void ReadAllDflashData(void) 
{
   ReadDFlashData(DFLASH_Ser2EAddress,(uint32_t *)&Diag2eData.Flag, sizeof(Diag2eData), Ser2EToDFlashInfoInit);
   ReadDFlashData(DFLASH_SerBOOTAddress,(uint32_t *)&DiagBootData.Flag, sizeof(DiagBootData), NULL);

}
/******************************************************************************
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 )
        {
            Wait10sFlag = 0;
            wait10cnt = 0;;
            if (diag27.RequestSeedCnt >= 2){
                diag27.RequestSeedCnt = 1;
            }
            if (diag27.attemptcnt >= 2){
                diag27.attemptcnt = 1;
            }
            Write_27_DATA_TO_FLASH();
        }
    }

    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;

            ContrlorResumeofIO = 0;
            ContrlorResumeofSOC = 0;	 //soc????
            SocSeg = 0;			//Soc
            ContrlorResumeofEspeed = 0;  //??????
            ESpeedValNUM = 0;		   //??? 		  
            ContrlorResumeofVspeed = 0;  //???????	  
            VspeedValNUM = 0;		   //???? 
            ContrlorResumeofLamp = 0;
            // ISD_Stop_Loop();

            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();
            CANNetRxOFF = 0;
			      CANNetTxOFF = 0;
            Services27_01_Requested = 0;
            CAN_DTC_OFF = 0;
        }
    }
}

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

    //???????????????13??????
    if ( Service10DiagDataLength != 1 )
    {
        NegRes.code = incorrectMessageLength; 
        UDS_Service_Response ( 0x10, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    //?????????????????0???????22??????
    //???????
    for ( i = 0; i < Service10DiagDataLength; i++ )
    {
        UDS_ISO14229_Transfer[i] = * ( A_Data + i );
    }
    //?§Ø????????????
    switch(UDS_ISO14229_Transfer[0]){
      case 0x81 :
      case 0x01 :
        if( DefaultSession !=SessionType )
        {
          S3_ServerEN      = 1;
          S3_ServerCnt     = 50;
          SeedkeyValidFlag = 0;
          DiagLockFlag     = NormalKeyLock;
          //?§Ý???????
          SessionType = DefaultSession;
        }
        //10 01????????? 10 81???????????
        if ( UDS_ISO14229_Transfer[0] == 0x01 )
        {
          UDS_Service_Response ( 0x10, POSITIVE_RSP, DIAG_ID_Tx, 5, UDS_ISO14229_Transfer );;
        }
        break;
      case 0x02 :

        //????????????7E??????
        if( DefaultSession ==SessionType ){
          if(FunorPhy == DIAG_ID_Rx_FUN )
          return ;
          NegRes.code = serviceNotSupportedInActiveSession;
          UDS_Service_Response ( 0x10, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
          break;
        }
        
        if ( UDS_ISO14229_Transfer[0] == 0x02 ){
            NegRes.code = requestCorrectlyReceivedResponsePending;
            UDS_Service_Response ( 0x10, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
            for(i = 0; i < 30000; ++i){
                nop();
            }
            WRITE_BZ_inDFLAH2Sector ( 0x3AA3 ); //????????
        }else {
            WRITE_BZ_inDFLAH2Sector ( 0x8AA8 ); //????????
        }
        COPCTL = 0x01;        //enable watchdog
        ARMCOP = 0x00;
        //??????????§Ó???
        break;  
        
      case 0x83 :
      case 0x03 :
        //??????????§Ó???
        if( ExtendedDiagnosticSession !=SessionType )
        {
          //???????????????
          S3_Server_refresh = 1;
          S3_ServerEN      = 1;
          S3_ServerCnt     = 0;
          SeedkeyValidFlag = 0;
        //  DiagLockFlag     = NormalKeyLock;
          //?§Ý????????
          SessionType = ExtendedDiagnosticSession;
        }
        
        DiagLockFlag     = NormalKeyLock; //???????
        
        //10 03????????? 10 83???????????
        if ( UDS_ISO14229_Transfer[0] == 0x03 ) 
        {      
            UDS_Service_Response ( 0x10, POSITIVE_RSP, DIAG_ID_Tx, 5, UDS_ISO14229_Transfer );
        }
        break;
        
      default  ://??????????????12??????
        if(FunorPhy == DIAG_ID_Rx_FUN )
          return ;
        NegRes.code = subFunctionNotSupported;    //????sub Function ?????
        UDS_Service_Response ( 0x10, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        break;
    }
}


/*void CalculateKey ( uint32_t Seed )    //????seed????¨®?key? ???????Level1 ?? Level4 ???
{
    uint8_t i;
    uint8_t xorArray[4] = {0x74, 0xF7, 0x15, 0x17 };
    uint8_t calData[4];
    uint8_t key[4];
    uint8_t seedArray[4];
    
    DigKeyValue.Value=Seed;
    
    memcpy(seedArray,DigKeyValue.Msg,4);

    for(i = 0; i < 4; ++i)
    {
      calData[i] = seedArray[i] ^ xorArray[i];
    }
    
    key[0] = (((calData[3]&0x0F)<<4)|(calData[3]&0xF0));
    key[1] = (((calData[1]&0x0F)<<4)|((calData[0]&0xF0)>>4));
    key[2] = ((calData[1]&0xF0)|((calData[2]&0xF0)>>4));
    key[3] = (((calData[0]&0x0f)<<4)|(calData[2]&0x0F));
    
    memcpy(DigKeyValue.Msg, key,4); 
    
    ValidSeedKey1 = DigKeyValue.Value;
    
    key[0] = (((calData[3]&0x07)<<5)|(calData[0]&0xF8)>>3);
    key[1] = (((calData[0]&0x07)<<5)|((calData[2]&0x1F)>>4));
    key[2] = ((calData[1]&0xF8)|((calData[3]&0xE0)>>5));
    key[3] = (((calData[2]&0xF8))|(calData[1]&0x07));
    
    memcpy(DigKeyValue.Msg, key,4); 
    
    ValidSeedKey2 = DigKeyValue.Value;
    
    key[0] = (((calData[2]&0x03)<<6)|(calData[3]&0xFC)>>2);
    key[1] = (((calData[3]&0x03)<<6)|((calData[0]&0x3F)));
    key[2] = ((calData[0]&0xFC)|((calData[1]&0xC0)>>6));
    key[3] = (((calData[1]&0xFC))|(calData[2]&0x03));
    
    memcpy(DigKeyValue.Msg, key,4); 
    
    ValidSeedKey4 = DigKeyValue.Value;
    
    SeedkeyValidFlag = 1;     
}  */

/* The array for SEED starts with [1], the array for KEY starts with [0] */
/* seed contains the SEED from the ECU */
/* length contains the number of bytes of seed */
/* key contains the KEY to send to ECU */
/* retLen contains the number of bytes to send to ECU as key */

//void CalculateKey(uint32_t Seed , DWORD length, uchar *key, DWORD *retLen)
void CalculateKey(uint32_t Seed,const uint32_t mask)
{
    uint32_t i;
    uint8_t  key[4];
    union
    {
        uchar byte[4];
        uint32_t wort;
    } seedlokal;
    //const uint32_t mask = 0x1CBE5D3A;

    //const uint32_t mask = 0x8EACBD9F;
    if ((Seed>>16) == 0)
        return;
    else
    {
        seedlokal.wort = Seed;
        for (i=0; i<35; i++)
        {
            if (seedlokal.wort & 0x80000000)
            {
                seedlokal.wort = seedlokal.wort << 1;
                seedlokal.wort = seedlokal.wort ^ mask;
            }
            else
            {
                seedlokal.wort = seedlokal.wort << 1;
            }
        }
        for (i=0; i<4; i++)
        {
           // key[3-i] = seedlokal.byte[i];
            key[i] = seedlokal.byte[i];         //20190726
        }
        (void) memcpy(DigKeyValue.Msg, key, 4); 
        if ( mask == MASK1_Safe)
            ValidSeedKey1 = DigKeyValue.Value;
        else if( mask == MASK2_Safe)
            ValidSeedKey2 = DigKeyValue.Value;
        // *retLen = length - 1;
    }
    SeedkeyValidFlag = 1;  
}  



//???????
void UDS_Service_27_Indication ( uint32_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];
    //uint32_t Seedlong;
    uint16_t tempbuffer;
    uint32_t Diag_Id = 0;
    
    Diag_Id = A_TA_type;;
    //??????????
    if ( Diag_Id == DIAG_ID_Rx_FUN ) 
        return;
    //?????????????????0??????22??????
  
    //?????????,????7F??????
    if ( SessionType == DefaultSession ) 
    {
        NegRes.code = serviceNotSupportedInActiveSession; //?????????
        UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
   
    S3_ServerCnt = 0;
    //??????????
    Service27DiagDataLength = A_Length;
    //???????
    
    (void) memcpy(UDS_ISO14229_Transfer,A_Data,Service27DiagDataLength);
    //27??????§³?????1
    if(Service27DiagDataLength < 1) 
    {
        NegRes.code = incorrectMessageLength;
        UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
      //??????27 01??27 02?????????????12??????
    if ( ( UDS_ISO14229_Transfer[0] != 0x01 ) && ( UDS_ISO14229_Transfer[0] != 0x02 ) && ( UDS_ISO14229_Transfer[0] != 0x05 ) && ( UDS_ISO14229_Transfer[0] != 0x06 ))
    {
        NegRes.code = subFunctionNotSupported;
        UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
     //??????27 01??27 02?????????????12??????
    if ( (( UDS_ISO14229_Transfer[0] == 0x01 ) || ( UDS_ISO14229_Transfer[0] == 0x02 )) && (SessionType != ExtendedDiagnosticSession))
    {
        NegRes.code = subfunctionNotSupportinActiveSession;
        UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    
    if (( ( UDS_ISO14229_Transfer[0] == 0x05 ) || ( UDS_ISO14229_Transfer[0] == 0x06 )) && (SessionType != ProgrammingSession))
    {
        NegRes.code = subfunctionNotSupportinActiveSession;
        UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    //?§Ø??????????
    switch ( UDS_ISO14229_Transfer[0] )
    {
        case 0x01:
            //???????????????13??????
            if ( Service27DiagDataLength != 1 )
            {
                NegRes.code = incorrectMessageLength;
                UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }     
            //?§Ø???10s??????
            if(Wait10sFlag==1)
            { //????????????3?¦²?????????4?¦Ç??????????????§Ù??????????? ??????10?? ??????????¦Ã???
                NegRes.code=requiredTimeDelayNotExpired;//???10?????¦Ä??
                UDS_Service_Response(0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, (uint8_t*)(&NegRes.code));
                return;
            }
            //??????¦Ë??1
            Services27_01_Requested = 1;
           
           //????????????????????????
            if (   DiagLockFlag == Level1UnLock|| DiagLockFlag == Level2UnLock  )
            {
                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
            }
            else
            {
              //???????+1
              //??27??????§Õ??flash
              //?????????????????????36??????
              if ( diag27.RequestSeedCnt >= 2 ) //????????¦²???????10??
              {
                  //????????¦Ë???
                  Services27_01_Requested = 0;
                  Wait10sFlag = 1;   //10????????
                  wait10cnt = 0;     //10??????????
                  NegRes.code = requiredTimeDelayNotExpired; //??????
                  UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                  return;
              }  
              //????????????§¹???????????????
              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
            (void)   memcpy(DigKeyValue.Msg, UDS_ISO14229_Transfer+1,4);
              
              //??????????????????????????
              CalculateKey ( DigKeyValue.Value,MASK1_Safe );
            }
            UDS_Service_Response ( 0x27, POSITIVE_RSP, DIAG_ID_Tx, 5, UDS_ISO14229_Transfer );
            break;

        case 0x02:
            //???????????????13??????
            if ( Service27DiagDataLength != 5 )
            {
                NegRes.code = incorrectMessageLength; 
                UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }
            //???????§Õ??????24??????
            if ( Services27_01_Requested != 1 )
            {
                NegRes.code = requestSequenceError; //???§Ó???
                UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }
            
            (void)memcpy(DigKeyValue.Msg, UDS_ISO14229_Transfer + 1, 4);

            //????????¦Ë???
            Services27_01_Requested = 0;

            if(DiagLockFlag == Level1UnLock){
               if(DigKeyValue.Value == 0x00000000){
                  //???level 1???????
                  diag27.attemptcnt = 0;
                  //???level 1???????????
                  diag27.RequestSeedCnt = 0;
                  UDS_Service_Response ( 0x27, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer );
               }else{
                    //?????§¹
                    SeedkeyValidFlag = 0;
                    //????????
                    DiagLockFlag = 0;
                    //level 1???????+1
                    diag27.attemptcnt = diag27.RequestSeedCnt;
					
					diag27.RequestSeedCnt++;
                    //??27??????§Õ??flash
                    Write_27_DATA_TO_FLASH();
                    //????????????Ú…???????10s??????????????1??????36??????
                    if ( diag27.attemptcnt >= 1 )
                    {
                        Wait10sFlag = 1;   //10????????
                        wait10cnt = 0;
                        NegRes.code = exceedNumberOfAttempts; //??????
                        UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                        return;
                    }
                    //??§¹?????????35??????
                    NegRes.code = invalidKey; 
                    UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
               }
           }else{
               //?????????§¹??????????????????????????????
               // if ( ( DigKeyValue.Value == ValidSeedKey1 ) && ( SeedkeyValidFlag == 1 ) )
                if ( ( DigKeyValue.Value == ValidSeedKey1 ) && ( SeedkeyValidFlag == 1 ))
                {
                    //??????????           0
                    DiagLockFlag = Level1UnLock;
                    //???level 1???????
                    diag27.attemptcnt = 0;
                    //???level 1???????????
                    diag27.RequestSeedCnt = 0;
                    //SeedkeyValidFlag = 1;
                    UDS_ISO14229_Transfer[0] = 0x02;
                    UDS_Service_Response ( 0x27, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer );
                    //??27??????§Õ??flash
                    Write_27_DATA_TO_FLASH();                
                } 
                else
                {
                  #ifdef TESTDIAG
                    NegRes.code = invalidKey; 
                    UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
                  #endif
                     
                    //?????§¹
                    SeedkeyValidFlag = 0;
                    //level 1???????+1
                    diag27.attemptcnt = diag27.RequestSeedCnt;
					
					diag27.RequestSeedCnt++;
                    //??27??????§Õ??flash
                    Write_27_DATA_TO_FLASH();
                    //????????????Ú…???????10s??????????????1??????36??????
                    if ( diag27.attemptcnt >= 1 )
                    {
                        Wait10sFlag = 1;   //10????????
                        wait10cnt = 0;
                        NegRes.code = exceedNumberOfAttempts; //??????
                        UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                        return;
                    }
                    //??§¹?????????35??????
                    NegRes.code = invalidKey; 
                    UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
                }
            }
            break;  
        //?????????,????12??????
        default:
            NegRes.code = subFunctionNotSupported; 
            UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
            break;
    }
}

extern uint8_t BLPWMDutyCycle;
extern uint8_t GUIIconDRLGStatus;
/*************************************************************************************
????????§Ó???;
  1??????????????§Ø???????????????
  2????????????????§Ø??????????????
  3?????????????????§Ø??????????????
  4???????????§Ø???????????????
***************************************************************************************/

extern INT32U  BD83790A;
//DID???????
void UDS_Service_22_Indication ( uint32_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
  uint16_t i;
  uint16_t DIDRequest;
  uint8_t  Service22DiagDataLength;
  uint32_t Diag_Id = 0;
  uint32_t Value = 0;
 // uint16_t  m16;
 // uint16_t TempValue;
 // uint8_t  TempHi;
//  uint8_t  TempLo;
 // uint8_t  d[4];
 // uint32_t year;
  Diag_Id = A_TA_type;
  S3_ServerCnt = 0; 
  
    Service22DiagDataLength = A_Length;
    //???????????????13??????
    if ( Service22DiagDataLength != 2 )
    {
        NegRes.code = incorrectMessageLength;
        UDS_Service_Response ( 0x22, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    //?????????????????0??????22??????
    /*if (( HIGH_VOLTAGE )||( LOW_VOLTAGE )||(DATA_VEHICLE_SPEED_DISPLAYING>0))
    {
        NegRes.code = conditionsNotCorrect; 
        UDS_Service_Response ( 0x22, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }*/
    NegRes.code = 0;
    
    
    //???????
    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 0xF18A:
          UDS_ISO14229_Transfer[2] = '2';
          UDS_ISO14229_Transfer[3] = '0';
          UDS_ISO14229_Transfer[4] = '2';
          UDS_ISO14229_Transfer[5] = '6';
          UDS_ISO14229_Transfer[6] = '5';
          UDS_ISO14229_Transfer[7] = '6';
          UDS_ISO14229_Transfer[8]  = ' ';
          UDS_ISO14229_Transfer[9]  = ' ';
          UDS_ISO14229_Transfer[10]  = ' ';
          UDS_ISO14229_Transfer[11]  = ' ';
          UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 12, UDS_ISO14229_Transfer );
      break;
	    case 0xF187:
              UDS_ISO14229_Transfer[2]  = '3';//'9';
              UDS_ISO14229_Transfer[3]  = '8';//'0';
              UDS_ISO14229_Transfer[4]  = '2';
              UDS_ISO14229_Transfer[5]  = '0';//'5';
              UDS_ISO14229_Transfer[6]  = '0';//'7';
              UDS_ISO14229_Transfer[7]  = '1';//'4';
              UDS_ISO14229_Transfer[8]  = '0';//'4';
              UDS_ISO14229_Transfer[9]  = '-';//'3';
              UDS_ISO14229_Transfer[10] = 'E';
              UDS_ISO14229_Transfer[11] = 'F';
              UDS_ISO14229_Transfer[12] = '0';
              UDS_ISO14229_Transfer[13] = '1';
          //}
          UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 14, UDS_ISO14229_Transfer );
      break;
     case 0xF1A0:
         // memset(UDS_ISO14229_Transfer+2,0x20,16);  //???????
          UDS_ISO14229_Transfer[ 2] = 'S';
          UDS_ISO14229_Transfer[ 3] = 'W';
          UDS_ISO14229_Transfer[ 4] = ':';
          UDS_ISO14229_Transfer[ 5] = 'c';
          UDS_ISO14229_Transfer[ 6] = '.';
          UDS_ISO14229_Transfer[ 7] = '0';
          UDS_ISO14229_Transfer[ 8] = '0';
          UDS_ISO14229_Transfer[ 9] = '.';
          UDS_ISO14229_Transfer[10] = '1';
          UDS_ISO14229_Transfer[11] = '0';
          UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 12, UDS_ISO14229_Transfer );    
      break;
	  case 0xF191:
          //memcpy(UDS_ISO14229_Transfer+2,diag.F191,sizeof(diag.F191));  //???????
          UDS_ISO14229_Transfer[ 2] = 'H';
          UDS_ISO14229_Transfer[ 3] = 'W';
          UDS_ISO14229_Transfer[ 4] = ':';
          UDS_ISO14229_Transfer[ 5] = 'C';
          UDS_ISO14229_Transfer[ 6] = '.';
          UDS_ISO14229_Transfer[ 7] = '0';
          UDS_ISO14229_Transfer[ 8] = '.';
          UDS_ISO14229_Transfer[ 9] = '5';
          UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 10, UDS_ISO14229_Transfer );   
      break;
	  case 0xF190:
          memcpy(UDS_ISO14229_Transfer+2,Diag2eData.D_F190,sizeof(Diag2eData.D_F190));
          
          UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 19, UDS_ISO14229_Transfer );    
      break; 

      case 0xF1FA:
          memcpy(UDS_ISO14229_Transfer+2,Diag2eData.D_F1FA,sizeof(Diag2eData.D_F1FA));
          
          UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 8, UDS_ISO14229_Transfer );    
      break;
	  case 0xF1A8:
          memcpy(UDS_ISO14229_Transfer+2,Diag2eData.D_F1A8,sizeof(Diag2eData.D_F1A8));
          UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 22, UDS_ISO14229_Transfer );   
      break;
	  case 0xF18C:
          UDS_ISO14229_Transfer[ 2] = 0;
          UDS_ISO14229_Transfer[ 3] = 0;
          UDS_ISO14229_Transfer[ 4] = 0;
          UDS_ISO14229_Transfer[ 5] = 0x20;
          UDS_ISO14229_Transfer[ 6] = 0x20;
          UDS_ISO14229_Transfer[ 7] = 0x20;
          UDS_ISO14229_Transfer[ 8] = 0x20;
          UDS_ISO14229_Transfer[ 9] = 19;
          UDS_ISO14229_Transfer[10] = 7;
          UDS_ISO14229_Transfer[11] = 28;
          UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 12, UDS_ISO14229_Transfer );    
      break;
      //????????????????????
      case 0xF184:
       //memcpy(UDS_ISO14229_Transfer+2,NVMConfigF184.Data,sizeof(NVMConfigF184.Data));
       memcpy(UDS_ISO14229_Transfer+2,DiagBootData.D_F184,sizeof(DiagBootData.D_F184));
       UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 9, UDS_ISO14229_Transfer );
      break;  
      
      //??????????????
      case 0xF183:
          memcpy(UDS_ISO14229_Transfer+2,DiagBootData.D_F183,sizeof(DiagBootData.D_F183));  //???????
          UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 12, UDS_ISO14229_Transfer );    
      break;
      //???????????
      case 0x3200:
          memcpy(UDS_ISO14229_Transfer+2,Diag2eData.D_3200,sizeof(Diag2eData.D_3200));
          UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 18, UDS_ISO14229_Transfer );     
      break;
      //?????
      case 0x3201:
          Value = NVM_ODO_VALUE / 10;
          UDS_ISO14229_Transfer[ 2] = Value >> 16;
          UDS_ISO14229_Transfer[ 3] = Value >> 8;
          UDS_ISO14229_Transfer[ 4] = Value;
          UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 5, UDS_ISO14229_Transfer );     
      break;    
      //???????
      case 0x3202:
          Value = DATA_TRIPA / 10; 
          UDS_ISO14229_Transfer[ 2] = Value >> 16;
          UDS_ISO14229_Transfer[ 3] = Value >> 8;
          UDS_ISO14229_Transfer[ 4] = Value;
          UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 5, UDS_ISO14229_Transfer );     
      break;
      //????????
      case 0x3203:
        if(TelltaleLCDFlag.Sig.LCD23 == 0) 
        {
          Value = 0; 
        } 
        else {
          Value = 1;
        
        }
          UDS_ISO14229_Transfer[ 2] = Value;
          UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 3, UDS_ISO14229_Transfer );    
      break;
      //??????????
      case 0x3204:
          if(LINE_IN_SEATBELT_SIGNAL == 1) {
            Value = 0;
          } else {
            Value = 1;
          }
          UDS_ISO14229_Transfer[ 2] = Value;
          UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 3, UDS_ISO14229_Transfer );    
      break;
      //??????????
      case 0x3205:
          UDS_ISO14229_Transfer[ 2] = 1;
          UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 3, UDS_ISO14229_Transfer );    
      break;
      //??????????
      case 0x3206:
          if(TelltaleLCDFlag.Sig.LCD06 == 1) {
            Value = 0;
          } else {
            Value = 1;
          }
          UDS_ISO14229_Transfer[ 2] = Value;
          UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 3, UDS_ISO14229_Transfer );    
      break;
      //?????????
      case 0x3207:
          if(BD83790A & BD8379_D9) 
          {
            Value = 1;
          } else {
            Value = 0;
          }
          UDS_ISO14229_Transfer[ 2 ] = Value;
          UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 3, UDS_ISO14229_Transfer );    
      break;
	  default:
           if(Diag_Id == DIAG_ID_Rx_FUN )
            return ;
          NegRes.code = requestOutOfRange;             //????31??????
          UDS_Service_Response ( 0x22, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
          break;
          
    } 
}





//??????
void UDS_Service_28_Indication ( uint32_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    uint16_t i;
    uint8_t  Service28DiagDataLength;
    uint32_t FunorPhy;
    
    FunorPhy = A_TA_type;
    S3_ServerCnt = 0;
    //???????????????7F??????
    if ( SessionType != ExtendedDiagnosticSession )
    {
        if (FunorPhy == DIAG_ID_Rx_FUN )
            return ;
        NegRes.code = serviceNotSupportedInActiveSession; 
        UDS_Service_Response ( 0x28, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    Service28DiagDataLength = A_Length;
    //???????????????13??????
    if ( Service28DiagDataLength != 2 )
    {
        NegRes.code = incorrectMessageLength;
        UDS_Service_Response ( 0x28, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
        //?????????????????0???????22??????
    /*if (( HIGH_VOLTAGE )||( LOW_VOLTAGE )||(DATA_VEHICLE_SPEED_DISPLAYING>0)) 
    {
        NegRes.code = conditionsNotCorrect;
        UDS_Service_Response ( 0x10, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }*/                           
    //???????
    for ( i = 0; i < Service28DiagDataLength; i++ ) //???????? ???????
    {
        UDS_ISO14229_Transfer[i] = * ( A_Data + i );
    }
    //????0x03????
    switch(UDS_ISO14229_Transfer[0])
	  {
	    
	  //??????
	  case 0x80:
	  case enableRxAndTx:
		 // CommunicationType=UDS_ISO14229_Transfer[0];
		 // ControlType=enableRxAndTx;
		 //?????????§µ?????22??????
  	/*	if ( EngineSpeed >= 400 )
      {
        NegRes.code = conditionsNotCorrect;
        UDS_Service_Response ( 0x28, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
      } */
		 //??????????
		 #if 0
		 if( 1 == UDS_ISO14229_Transfer[1])
		 	{
				Com_RxStart();
				Com_TxStart();
		 	}
		 	//???????????????
		 	else if (2 == UDS_ISO14229_Transfer[1])
		 	{
        CANNetRxOFF = 0;
        CANNetTxOFF = 0;
		 	}
		 	//??????????????????????????
		 	else 
		 	#endif
		 	if( 3 == UDS_ISO14229_Transfer[1] )
		 	{
				Com_RxStart();
				Com_TxStart();
		 		CANNetRxOFF = 0;
				CANNetTxOFF = 0;
		 	}
		 	//???????¦¶??????31??????
		  else
		 	{
		 		if(FunorPhy == DIAG_ID_Rx_FUN )
          return ;
		 		NegRes.code = 	requestOutOfRange ;
				UDS_Service_Response(0x28, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ));
				break;
		 	}
	 
		  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;
      //??????????
      #if 0
			if( 1 == UDS_ISO14229_Transfer[1])
		 	{
				Com_TxStop();
				Com_RxStop();	
		 	}		 	
			//???????????????
		 	else if (2 == UDS_ISO14229_Transfer[1])
		 	{
        CANNetRxOFF = 1;
        CANNetTxOFF = 1;
		 	}
		 //??????????????????????????	
		 else 
		 #endif
		 if( 3 == UDS_ISO14229_Transfer[1] )
		 	{
				Com_TxStop();
				Com_RxStop();
				CANNetRxOFF = 1;
				CANNetTxOFF = 1;
		 	}
	 	 //???????¦¶??????31??????
		 else
		 	{
  		 	
  		 	if(FunorPhy == DIAG_ID_Rx_FUN )
          return ;
  		 	NegRes.code = 	requestOutOfRange ;
				UDS_Service_Response(0x28, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ));
				break;
		 	}
      if(bit_is_clear(UDS_ISO14229_Transfer[0],7))
        UDS_Service_Response(0x28, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer);
      
      break;
	  //??????????????12??????   
	  default:
  		if(FunorPhy == DIAG_ID_Rx_FUN )
          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 ( uint32_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    uint16_t i;
    uint8_t  Service85DiagDataLength;
    uint32_t FunorPhy;
    FunorPhy = A_TA_type;
    S3_ServerCnt = 0;
   
    //???????????????7F??????
    if ( SessionType == DefaultSession )
    { 
        if (FunorPhy == DIAG_ID_Rx_FUN )
            return;
        NegRes.code = serviceNotSupportedInActiveSession;
        UDS_Service_Response ( 0x85, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    } 
    //??????????
    Service85DiagDataLength = A_Length;
    //???????????????13??????
    if ( Service85DiagDataLength != 1 )
    {
        NegRes.code = incorrectMessageLength;
        UDS_Service_Response ( 0x85, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    
     //?????????????????0???????22??????
    /*if (( HIGH_VOLTAGE )||( LOW_VOLTAGE )||(DATA_VEHICLE_SPEED_DISPLAYING>0))
    {
        NegRes.code = conditionsNotCorrect;
        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] )
    {   //01 01 ????DTC????
        
        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;   
        //02 02 ???DTC????
        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 ://??????????????12??????
             if(FunorPhy == DIAG_ID_Rx_FUN )
                return ;
            NegRes.code = subFunctionNotSupported;
            UDS_Service_Response ( 0x85, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
            break;
    }
}

//???????
void UDS_Service_31_Indication ( uint32_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    uint8_t  Service31DiagDataLength;
    //uint8_t  EraserResult;
    uint16_t i;
    uint16_t RoutineID;
    uint32_t FunOrPhy;
    uint32_t checkSum;
    //uint32_t SectorAddress;
    FunOrPhy = A_TA_type;
    S3_ServerCnt = 0;
    //??????????
    if ( DIAG_ID_Rx_FUN == FunOrPhy )
        return;
    //?????????????????0??????22??????
    //if ( EngineSpeed >= 400 )
     /*if (( HIGH_VOLTAGE )||( LOW_VOLTAGE )||(DATA_VEHICLE_SPEED_DISPLAYING>0))
    {
        NegRes.code = conditionsNotCorrect;
        UDS_Service_Response ( 0x31, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }*/
    
    //????????????7F??????
    if ( SessionType != ExtendedDiagnosticSession )
    {
        NegRes.code = serviceNotSupportedInActiveSession; 
        UDS_Service_Response ( 0x31, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    //?????????????????33??????
    if ( DiagLockFlag == NormalKeyLock )
    {
        NegRes.code = securityAccessDenied; //?????¦Ä????
        UDS_Service_Response ( 0x31, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    //??????????
    Service31DiagDataLength = A_Length;
    //???????????????13??????
    if ( Service31DiagDataLength != 3 )
    {
        NegRes.code = incorrectMessageLength; //??????????
        UDS_Service_Response ( 0x31, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    //???????
    for ( i = 0; i < Service31DiagDataLength; i++ )
    {
        UDS_ISO14229_Transfer[i] = * ( A_Data + i );
    }
    if(UDS_ISO14229_Transfer[0] != 1) 
    {
       NegRes.code = subFunctionNotSupported; 
       UDS_Service_Response ( 0x31, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
       return;
    }
    RoutineID = ( ( uint16_t ) UDS_ISO14229_Transfer[1] << 8 ) | ( uint16_t ) UDS_ISO14229_Transfer[2];

    switch ( RoutineID )
    {
        case MaintenanceReset: //???????
            //wdt_reset();
            //NVM_USER_SETTINGS_SERVICE=5000;
            //NVM_Data_Update(NVM_USER_SETTINGS);
            UDS_ISO14229_Transfer[3] = RoutineCompletedSuccessfully;
            UDS_Service_Response ( 0x31, POSITIVE_RSP, DIAG_ID_Tx, 4, UDS_ISO14229_Transfer );
                
        break;
        default:
          NegRes.code = requestOutOfRange; 
          UDS_Service_Response ( 0x31, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
          return;
        break;
    }
}



//???????????
void UDS_Service_34_Indication ( uint32_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    uint16_t i;
    uint8_t  Service34DiagDataLength;
    //uint32_t memoryAddress;
    uint32_t FunOrPhy;
    //uint32_t SectorAddress;
    FunOrPhy = A_TA_type;
    S3_ServerCnt = 0;

    if ( FunOrPhy == DIAG_ID_Rx_FUN )
        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;
    }
     //?????????????????0???????22??????
    /*if (( HIGH_VOLTAGE )||( LOW_VOLTAGE )||(DATA_VEHICLE_SPEED_DISPLAYING>0)) 
    {
        NegRes.code = conditionsNotCorrect;
        UDS_Service_Response ( 0x10, 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] != 0 ) || ( UDS_ISO14229_Transfer[1] != 0x44 ) )
    {
        NegRes.code = incorrectMessageLength; //??????????
        UDS_Service_Response ( 0x34, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    BlocKCnt =1;
    UDS_ISO14229_Transfer[0] = LengthFormatIdentifier;
    UDS_ISO14229_Transfer[1] = 0x00;
    UDS_ISO14229_Transfer[2] = 226;
    UDS_Service_Response ( 0x34, POSITIVE_RSP, DIAG_ID_Tx, 3, UDS_ISO14229_Transfer );
}

//???????????
void UDS_Service_36_Indication ( uint32_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    uint16_t i;
    uint8_t  m8;
    //uint8_t  *p8;
    uint8_t  Service36DiagDataLength;
    uint32_t FunOrPhy;
    //uint32_t SectorAddress;
    FunOrPhy = A_TA_type;
    S3_ServerCnt = 0;

    if ( FunOrPhy == DIAG_ID_Rx_FUN )
        return;

    if ( SessionType != ProgrammingSession ) //?????????
    {
        NegRes.code = serviceNotSupportedInActiveSession; //?????????  7F
        UDS_Service_Response ( 0x36, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    if ( DiagLockFlag == BootKeyLock )
    {
        NegRes.code = securityAccessDenied; //?????¦Ä????   33
        UDS_Service_Response ( 0x36, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
        //?????????????????0???????22??????
    /*if (( HIGH_VOLTAGE )||( LOW_VOLTAGE )||(DATA_VEHICLE_SPEED_DISPLAYING>0)) 
    {
        NegRes.code = conditionsNotCorrect;
        UDS_Service_Response ( 0x10, 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; //???????§Õ???    73
        UDS_Service_Response ( 0x36, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    m8 = BlocKCnt;
    BlocKCnt++;
   
    UDS_ISO14229_Transfer[0] = m8;

    UDS_Service_Response ( 0x36, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer );
}

//???????????
void UDS_Service_37_Indication ( uint32_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    //uint16_t i;
    //uint8_t  Service37DiagDataLength;
    uint32_t FunOrPhy;
    //uint32_t SectorAddress;
    FunOrPhy = A_TA_type;

    if ( FunOrPhy == DIAG_ID_Rx_FUN )
        return;

    S3_ServerCnt = 0;

    if ( SessionType != ProgrammingSession ) //?????????
    {
        NegRes.code = serviceNotSupportedInActiveSession; //????????? 7F
        UDS_Service_Response ( 0x37, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
        //?????????????????0???????22??????
    /*if (( HIGH_VOLTAGE )||( LOW_VOLTAGE )||(DATA_VEHICLE_SPEED_DISPLAYING>0)) 
    {
        NegRes.code = conditionsNotCorrect;
        UDS_Service_Response ( 0x10, 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 ( uint32_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    uint16_t i;
    uint8_t  Service3EDiagDataLength;
    uint32_t FunorPhy;
    FunorPhy = A_TA_type;
    Service3EDiagDataLength = A_Length;
    S3_ServerCnt = 0;
    //???????????????13??????
    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 );
    }
    //???????00??80???????12???????????
    if ( ( UDS_ISO14229_Transfer[0] != 0 ) && ( UDS_ISO14229_Transfer[0] != 0x80 ) )
    //if ( UDS_ISO14229_Transfer[0] != 0 )
    {
        //??????????
         if(FunorPhy == DIAG_ID_Rx_FUN )
            return ;
        NegRes.code = subFunctionNotSupported;
        UDS_Service_Response ( 0x3E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    S3_Server_refresh = 1;
    //3E 00??????????3E 80?????????
    if ( UDS_ISO14229_Transfer[0] == 0x00 )
    {
        UDS_Service_Response ( 0x3E, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer );
    }
    return;
}

void UDS_Service_11_Indication ( uint32_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    uint16_t i;
    uint8_t  Service11DiagDataLength;
    uint32_t FunorPhy;
    FunorPhy = A_TA_type;
    S3_ServerCnt = 0;
    Service11DiagDataLength = A_Length;
    //?????????????????0??????22??????
    /*if (( HIGH_VOLTAGE )||( LOW_VOLTAGE )||(DATA_VEHICLE_SPEED_DISPLAYING>0))
    {
        NegRes.code = conditionsNotCorrect;
        UDS_Service_Response ( 0x11, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }*/
    //???????????????7F??????
    /*
    if ( ( SessionType == DefaultSession ) ) 
    {
        NegRes.code = serviceNotSupportedInActiveSession; //?????????
        UDS_Service_Response ( 0x11, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    */
    //???????????????13??????
    if ( Service11DiagDataLength != 1 )
    {
        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 );
    }
    //1.6?·Ú???????11 02??11 82????
   // 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] == 0x01 )|| ( UDS_ISO14229_Transfer[0] == 0x81 ))     //?????01?????
    {
        //????????????
        SessionType = 1;
        if( UDS_ISO14229_Transfer[0] == 0x01 ){
          
        UDS_Service_Response ( 0x11, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer );
          i = 30000;
          while(i--){
           	  	nop();
            }
        }
        COPCTL = 0x01;        //enable watchdog
        ARMCOP = 0x00;  
        //if ( ( UDS_ISO14229_Transfer[0] == 0x81 ) || ( UDS_ISO14229_Transfer[0] == 0x83 ) ||( UDS_ISO14229_Transfer[0] == 0x82 ))
        //{
        //    return;
        //}
        
        //return;
    }
    else//??????????????12??????
    {
        if ( FunorPhy == DIAG_ID_Rx_FUN )
            return;
        NegRes.code = subFunctionNotSupported; //?????????
        UDS_Service_Response ( 0x11, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
}

//??????›¥?§Ò?

const uint32_t DTC_CODE_TABLE[] =
{
    U007488,
    U300316,
    U300317,
    U111087,
    U014087,
    U012187,
    U010087, 
};
//=================================================
//??????????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;


      S3_ServerCnt = 0;
      S3_ServerEN = 0;
      SessionType = DefaultSession;
      SeedkeyValidFlag = 0;
      DiagLockFlag = 0;


      ContrlorResumeofIO = 0;    //IO????


      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();
      Services27_01_Requested = 0;
      CAN_DTC_OFF = 0;
      CANNetRxOFF = 0;
      CANNetTxOFF = 0;

    //for ( i = 0; i < cnDTCslen; i++ )
    //    DTC_STATUS[i] = DTC_STATUS[i] | 0x50; //??4,6 BIT??1
}
//===========================================================
//???DTC?????
//????:DTCstatusCHGFF=1???DTC????§Ú??(????›¥??EEPROM??)
//===========================================================

void ChangeDTCs_ST_PRG ( INT8U pos, INT8U status )
{
    if(CAN_DTC_OFF == 0)
	{
		if ( ( uint8_t )(DiagDtc[ pos ].Status.Flag & DTCNoTestLastClear) == DTCNoTestLastClear )
		{
			DiagDtc[ pos ].Status.Flag = ( uint8_t )(DiagDtc [ pos ].Status.Flag & (~DTCNoTestLastClear));
		}
		if ( ( uint8_t )(DiagDtc [ pos ].Status.Flag & DTCNoTestThisCycle) == DTCNoTestThisCycle )
		{
			DiagDtc [ pos ].Status.Flag = ( uint8_t )(DiagDtc [ pos ].Status.Flag & (~DTCNoTestThisCycle));
		}
		if ( status == DTCError )
		{
			if ( ( uint8_t )(DiagDtc [ pos ].Status.Flag & DTCError) != DTCError )
			{
				DiagDtc [ pos ].Status.Flag |= DTCError;
				if ( ( uint8_t )(DiagDtc [ pos ].Status.Flag & DTCErrorThisCycle) != DTCErrorThisCycle )
				{
					DiagDtc [ pos ].Status.Flag |= DTCErrorThisCycle;
					DiagDtc [ pos ].DTCAgingCount = 0u;
					DiagDtc [ pos ].FaultPendCount++;
					if (DiagDtc [ pos ].FaultPendCount >= 1 )
					{
						if ( ( uint8_t )(DiagDtc [ pos ].Status.Flag & DTCConfirmed) != DTCConfirmed )
						{
							DiagDtc [ pos ].Status.Flag |= DTCConfirmed;
						}
					}
				}
				if ( ( uint8_t )(DiagDtc [ pos ].Status.Flag & DTCPending) != DTCPending )
				{
					DiagDtc [ pos ].Status.Flag |= DTCPending;

				}
				if ( ( uint8_t )(DiagDtc [ pos ].Status.Flag & DTCErrorLastClear) != DTCErrorLastClear )
				{
					DiagDtc [ pos ].Status.Flag |= DTCErrorLastClear;
				}
				DTCUpdataStatusList [ pos ]                                 = 1u;
				isNeedSaveDTC                                               = 1u;
			}
		}
		else
		{
			if ( ( uint8_t )(DiagDtc [ pos ].Status.Flag & DTCError) == DTCError )
			{
				DiagDtc [ pos ].Status.Flag &= DTCNoError;
				DTCUpdataStatusList [ pos ] = 1u;
				isNeedSaveDTC                  = 1u;
			}
		}
	}
}

//========================================
//???DTC????§Ú???????EEPROM??     100ms
//========================================

void SaveDTCsToEEPROM ( void )
{
    uint8_t i;

    if ( isNeedSaveDTC )			//DTC?????????,YES
    {
        isNeedSaveDTC = 0;

        for ( i = 0; i < cnDTCslen; i++ )
            NVMDTC.Data[i] = DiagDtc[ i ].Status.Flag ;//& 0x08;
        NVM_Data_Update ( NVM_DTC_DATA );
    }
}

//=========================================
//+++++++++100MS+++++++++++
//
//CAN????NORMAL??2S???
//=========================================
void CAN_NORMAL_TIME_PRG ( void )
{
    if (IO_DISPLAY_MODE == IO_DISP_MODE_IGN_ON )
    {
        CAN_NORMAL_TT++;

        if ( CAN_NORMAL_TT == 50 ) // 2S?,YES	//??ACC 6S
        {
            CAN_NORMAL_TT = 0;
       
            CAN_NORMAL_2SFF = 1;   // =1???CAN????NORMAL????2S
        }
    }
}
/*
Get KL15 Status
*/
uint8_t getKL15Status(void)
{
uint8_t flag = 0;
  if(ANALOG_KL15_VOLTAGE > 500) {
    flag = 1;
  } else {
    flag = 0;
  }
    return flag;
}

/*
Get busoff Status
*/
uint8_t GetBusOffState_NotNM(void)
{
    uint8_t rscan_st = 0;
	if(CAN0MISC_BOHOLD)
	{
		rscan_st = 1;
	}
    return rscan_st;
}

uint8_t getBusoffStatus(void)
{
    if ( GetBusOffState_NotNM( ) )
    {
        setDTCEnableTimer(BusOffDTCEnableTimer);//busoff???????????????
    }
    return GetBusOffState_NotNM( );
}

/**
  *Diag enable time
  */
void enableDTCTimer(uint16_t Timer)
{
    uint8_t BUS_OFF_Status = getBusoffStatus( );
	uint8_t KL15Status = getKL15Status( );
    if ( (KL15Status == 1u) && (BUS_OFF_Status == 0u) && (VoltageStatus == 0u))
    {
        if ( isEnable == 0u )
        {
            if ( enableTimer >= enableMaxTimer )
            {
                isEnable    = 1u;
                enableTimer = 0u;
            }
            else
            {
                enableTimer += Timer;
            }
        }
    }
}

/*
Set Diag enable time
*/
void setDTCEnableTimer(uint16_t Timer)
{
    uint8_t i;
    if ( isEnable == 1 )
    {
        isEnable       = 0;
        enableTimer    = 0;
        enableMaxTimer = Timer;
        for ( i = 0; i < 8; ++i )
        {
            msgList [ i ][ 3 ] = 0;
        }
    }
    else
    {
        if ( enableMaxTimer == Timer )
        {
            enableTimer = 0;
        }
        else
        {
            if ( enableMaxTimer < enableTimer + Timer )
            {
                enableMaxTimer = enableTimer + Timer;
            }
        }
    }
}

/*???30????*/
uint8_t getPowerStatus(void)
{
    uint8_t  enResult    = NormalVottage;
    uint16_t KL30Voltage = ANALOG_KL30_VOLTAGE;

    if ( (KL30Voltage >= 9000) && (KL30Voltage <= 16000) )
    {
        if ( VoltageStatus == LowVottage )
        {
            if ( KL30Voltage < 9500 )
            //if ( KL30Voltage < 7500u )
            {
                enResult = LowVottage;
                setDTCEnableTimer(H_LVoltageDTCEnableTimer);
            }
            else
            {
                VoltageStatus = NormalVottage;
                enResult       = NormalVottage;
            }
        }
        else if ( VoltageStatus == HightVottage )
        {
            if ( KL30Voltage > 15500 )
            //if ( KL30Voltage > 17000u )
            {
                enResult = HightVottage;
                setDTCEnableTimer(H_LVoltageDTCEnableTimer);
            }
            else
            {
                VoltageStatus = NormalVottage;
                enResult       = NormalVottage;
            }
        }
        else
        {
            enResult = NormalVottage;
        }
    }
    else if ( KL30Voltage > 16000 )
    {
        VoltageStatus  = HightVottage;
        enResult       = HightVottage;
        setDTCEnableTimer(H_LVoltageDTCEnableTimer);
    }
    else
    {
        VoltageStatus = LowVottage;
        enResult       = LowVottage;
        setDTCEnableTimer(H_LVoltageDTCEnableTimer);
    }
    return enResult;
}


//2ms????
void CanLoseDetected ( void )
{  
    if( CAN_DTC_OFF == 0 )
    {
        //???????
        
        if(getKL15Status()) 
        {
            //Low
            if (ANALOG_KL30_VOLTAGE < 9000) 
            {
                if(DTCTimerList[cnU300316] < 1000)
                {
                    DTCTimerList[cnU300316] += 2;
                }
                else
                {
                    ChangeDTCs_ST_PRG ( cnU300316, 0x01 );
                }
            }
            if (ANALOG_KL30_VOLTAGE > 9500) 
            {
                if(DTCTimerList[cnU300316] > 2)
                {
                    DTCTimerList[cnU300316] -= 2;
                }
                else
                {
                    ChangeDTCs_ST_PRG ( cnU300316, 0x00 );
                }
            }
            //high
            if (ANALOG_KL30_VOLTAGE > 16000) 
            {
                if(DTCTimerList[cnU300317] < 1000)
                {
                    DTCTimerList[cnU300317] += 2;
                }
                else
                {
                    ChangeDTCs_ST_PRG ( cnU300317, 0x01 );
                }
            }
            if (ANALOG_KL30_VOLTAGE < 15500) 
            {
                if(DTCTimerList[cnU300317] > 2)
                {
                    DTCTimerList[cnU300317] -= 2;
                }
                else
                {
                    ChangeDTCs_ST_PRG ( cnU300317, 0x00 );
                }
            }
            if( ! getPowerStatus( ) )
            {
                //busoff
                if ( BUS_OFF_FLAG )
                {
                    ChangeDTCs_ST_PRG ( cnU007488, 0x01 );
                }
                else
                {
                    ChangeDTCs_ST_PRG ( cnU007488, 0x00 );
                }

                if( ! getBusoffStatus( ))
                { 
                    if ( isEnable == 1 )
                    {
                        if(BCM_Sleep_Indicator) 
                        {
                            if(DTCTimerList[cnU111087] < 2000) {
                              DTCTimerList[cnU111087] += 2;
                            } else {
                              ChangeDTCs_ST_PRG(cnU111087,0x01);
                            }
                        }
                        else
                        {
                            if(DTCTimerList[cnU111087] > 2) {
                              DTCTimerList[cnU111087] -= 2;
                            } else {
                              DTCTimerList[cnU111087] = 0;
                              ChangeDTCs_ST_PRG(cnU111087,0x00);
                            }
                        }
                        if(getMSGStatus(cnMSG521))
                        {
                            /*Record DTC*/
                            ChangeDTCs_ST_PRG(cnU014087,0x01);
                        }
                        else
                        {
                            ChangeDTCs_ST_PRG(cnU014087,0x00);
                            /*Recover DTC*/
                        }

                        if(getMSGStatus(cnMSGF0B))
                        {
                            /*Record DTC*/
                            ChangeDTCs_ST_PRG(cnU012187,0x01);
                        }
                        else
                        {
                            ChangeDTCs_ST_PRG(cnU012187,0x00);
                            /*Recover DTC*/
                        }

                        if(getMSGStatus(cnMSG400))
                        {
                            /*Record DTC*/
                            ChangeDTCs_ST_PRG(cnU010087,0x01);
                        }
                        else
                        {
                            ChangeDTCs_ST_PRG(cnU010087,0x00);
                            /*Recover DTC*/
                        }
                    }
                }
            }
        }
    }
}

void ReceivedMsg(uint32_t MsgID, uint8_t DLC)
{
    uint8_t i;
    if ( CoCANCtrl.RxEnable == CoCAN_ENABLE )
    {
        for ( i = 0u; i < 8; ++i )
        {
            if ( (MsgID == msgList [ i ][ 0 ]) )//&& (DLC == msgList [ i ][ 1u ]) )
            {
                msgList [ i ][ 3 ] = 0u;
                break;
            }
        }
    }
}
/*
Lost DTC Message Check
*/
void MsgLostTimer(uint16_t timer)
{
    uint8_t i;
    uint8_t BUS_OFF_Status = getBusoffStatus( );
	uint8_t KL15Status = getKL15Status( );
    if ( (isEnable == 1) && (CoCANCtrl.RxEnable == CoCAN_ENABLE) && (KL15Status == 1) && (BUS_OFF_Status == 0) && (VoltageStatus == 0) )
    {
        for ( i = 0; i < (sizeof(msgList) / sizeof(msgList [ 0 ])); ++i )
        {
      			if ( msgList [ i ][ 3u ] < msgList [ i ][ 2u ] )
      			{
      				  msgList [ i ][ 3u ] += timer;
      			}
        }
    }
}
/*
Get DTC Message Status
*/
uint8_t getMSGStatus(uint8_t cnMsgID)
{
    uint8_t enResult;
  	if ( msgList [ cnMsgID ][ 3 ] < msgList [ cnMsgID ][ 2 ]  )
  	{
  		enResult = 0u;
  	}
  	else
  	{
  		enResult = 1u;
  	}
    return enResult;

}


/**
  *DTC Aged
  */
void DTCAgeing(void)
{
    INT8U i;
    for ( i = 0u; i < cnDTCslen; ++i )
    {
        DiagDtc[ i ].Status.Flag |= (DTCNoTestThisCycle);
        if ( ( uint8_t )(DiagDtc[ i ].Status.Flag & DTCErrorThisCycle) != DTCErrorThisCycle )
        {
            DiagDtc[ i ].Status.Flag       = ( uint8_t )(DiagDtc[ i ].Status.Flag & (~DTCPending));
            DiagDtc[ i ].FaultPendCount = 0u;
            if ( ( uint8_t )(DiagDtc[ i ].Status.Flag & DTCConfirmed) == DTCConfirmed )
            {
                DiagDtc[ i ].DTCAgingCount++;
                if ( DiagDtc[ i ].DTCAgingCount >= AgeingCount )
                {
                    DiagDtc[ i ].Status.Flag = ( uint8_t )(DiagDtc[ i ].Status.Flag & (~DTCConfirmed));
					DTCUpdataStatusList [ i ] = 1u;
                }
            }
        }
        else
        {
            DiagDtc[ i ].Status.Flag = ( uint8_t )(DiagDtc[ i ].Status.Flag & (~DTCErrorThisCycle));
        }
        DiagDtc[ i ].Status.Flag = ( uint8_t )(DiagDtc[ i ].Status.Flag & DTCNoError);
    }
}

void UDS_KL15_init(void)
{
    INT8U KL15Status = getKL15Status( );
    if ( UDSKL15Status == 0u )
    {
        if ( KL15Status == 1u )
        {
            UDSKL15Status  = 1u;
            setDTCEnableTimer(KL15DTCEnableTimer);
            SessionType = 1;
            //S3_ServiceTimeOut( );
            DTCAgeing( );
        }
    }
    else
    {
        if ( KL15Status == 0u )
        {
            UDSKL15Status = 0u;
            setDTCEnableTimer(KL15DTCEnableTimer);
        }
    }
}

void UDS_Service_19_Indication ( uint32_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;
    uint32_t  FunorPhy;
    //uint32_t DTCMaskRecord;
    uint8_t  DTCSnapshotRecordNumber;
    //NVMDTC_SnapshotUnion m_SnapshotData;
    FunorPhy = A_TA_type;
    Service19DiagDataLength = A_Length;
    __RESET_WATCHDOG();
    S3_ServerCnt = 0;
    //??????????????7F??????
    if ( SessionType == ProgrammingSession )
    {
        NegRes.code = serviceNotSupportedInActiveSession;
        UDS_Service_Response ( 0x19, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    //????????????13??????
    if ( Service19DiagDataLength < 1 )
    {
       NegRes.code = incorrectMessageLength;
       UDS_Service_Response ( 0x19, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
       return;
    }
    //???????
    for ( i = 0; i < Service19DiagDataLength; i++ )
    {
        UDS_ISO14229_Transfer[i] = * ( A_Data + i );
    }
    //????????
    switch ( UDS_ISO14229_Transfer[0] )
    {
        //???????????DTC????
        
        case 0x01:
            //???????????????13??????
            if ( Service19DiagDataLength != 2 )
            {
                NegRes.code = incorrectMessageLength; //??????????
                UDS_Service_Response ( 0x19, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }
            DTCSnapshotRecordNumber = UDS_ISO14229_Transfer[1];
            
            DTCnum = 0;

            for ( i = 0; i < cnDTCslen; i++ )
            {
                if ( DiagDtc[ i ].Status.Flag & UDS_ISO14229_Transfer[1] )
                    DTCnum++;
            }

            UDS_ISO14229_Transfer[1] = 0x09;
            UDS_ISO14229_Transfer[2] = 0x00;
            UDS_ISO14229_Transfer[3] = ( INT8U ) ( DTCnum >> 8 );
            UDS_ISO14229_Transfer[4] = ( INT8U ) ( DTCnum & 0x00FF );
            UDS_Service_Response ( 0x19, POSITIVE_RSP, DIAG_ID_Tx, 5, UDS_ISO14229_Transfer );
            break;                             
        //???????????DTC
        case 0x02:
            //???????????????13??????
            if ( Service19DiagDataLength != 2 )
            {
                NegRes.code = incorrectMessageLength; //??????????
                UDS_Service_Response ( 0x19, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }
            DTCSnapshotRecordNumber = UDS_ISO14229_Transfer[1];

            DTCcnt = 2;
            //????DTC????¦Ë????¦Ë????¦Ë?????DTC??
            for ( i = 0; i < cnDTCslen; i++ )
            {
                if ( DiagDtc[ i ].Status.Flag & 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] = DiagDtc[ i ].Status.Flag & 0x09;
                    DTCcnt++;
                }
            }
            UDS_ISO14229_Transfer[1] = 0x09;
            UDS_Service_Response ( 0x19, POSITIVE_RSP, DIAG_ID_Tx, DTCcnt, UDS_ISO14229_Transfer );
            break;
      
        //????????DTC */
        case 0x0A:  //Only available if more than 1 DTC exist
            //???????????????13??????
            if ( Service19DiagDataLength != 1 )
            {
                NegRes.code = incorrectMessageLength; //??????????
                UDS_Service_Response ( 0x19, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }
            /*
            DTCSnapshotRecordNumber = UDS_ISO14229_Transfer[4];
            if (( DTCSnapshotRecordNumber == 1 ) || ( DTCSnapshotRecordNumber == 0xff ) )
            {
            }
            else
            {
                NegRes.code = requestOutOfRange; //??????????
                UDS_Service_Response ( 0x19, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }
            */
            DTCcnt0A = 2;
            //????DTC????¦Ë????¦Ë????¦Ë?????DTC??
            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] = DiagDtc[ i ].Status.Flag & 0x09;
                DTCcnt0A++;
            }
            UDS_ISO14229_Transfer[1] = 0x09;
            UDS_Service_Response ( 0x19, POSITIVE_RSP, DIAG_ID_Tx, DTCcnt0A, UDS_ISO14229_Transfer );
            break;
        //??????????????12??????       
        default:
            if(FunorPhy == DIAG_ID_Rx_FUN )
            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 ( uint32_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    //???§Õ????? ?§Õ??EEPROM
    uint16_t i;
    uint16_t DID;
    uint8_t  Service2EDiagDataLength;
    uint32_t DiagIDNum = 0;
    Service2EDiagDataLength = A_Length;
    DiagIDNum = A_TA_type;
     S3_ServerCnt = 0;
    if ( DiagIDNum == DIAG_ID_Rx_FUN )
        return;

    if ( SessionType == DefaultSession ) //?????????
    {
        NegRes.code = serviceNotSupportedInActiveSession; //?????????   7F
        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 <= 2 )
    {
        NegRes.code = incorrectMessageLength; //??????????  13
        UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

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



    switch ( DID ) //????????2??????????§Õ???????[2]???
    {  
      case  0xF1A8:
      
          if(DiagLockFlag!=Level1UnLock)
          {
            NegRes.code=securityAccessDenied;//¦Ä????   33
            UDS_Service_Response(0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, (uint8_t*)(&NegRes.code));
            return;
          }
            
          if ( Service2EDiagDataLength != 20 + 2 )
          {
              NegRes.code = incorrectMessageLength; //?????????? 13
              UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
              return;
          }
           
                       
          if((UDS_ISO14229_Transfer[3] <= 0xFF )&& \
          ((UDS_ISO14229_Transfer[4] <= 0x0C) && (UDS_ISO14229_Transfer[4] > 0x00)) && ( (UDS_ISO14229_Transfer[5] <= 0x1F) && (UDS_ISO14229_Transfer[5] > 0) ) )
           {  
                (void)memcpy(&Diag2eData.D_F1A8[0],UDS_ISO14229_Transfer+2,20);
                write2EData();

                UDS_Service_Response ( 0x2E, POSITIVE_RSP, DIAG_ID_Tx, 2, UDS_ISO14229_Transfer );
            }
            else
            {
               NegRes.code = requestOutOfRange; //????????  0x31 ?????????¦¶
               
               UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
              return;
            } 
            break;

        case  0xF190:
                
            if(DiagLockFlag!=Level1UnLock)
            {
              NegRes.code=securityAccessDenied;//¦Ä????
              UDS_Service_Response(0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, (uint8_t*)(&NegRes.code));
              return;
            }

            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++)
            {
                if((UDS_ISO14229_Transfer[i + 2] < 0x20) || (UDS_ISO14229_Transfer[i + 2] > 0x7F))
                {
                    NegRes.code = requestOutOfRange; //??????????
                    UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
                }
            }
           (void)  memcpy(&Diag2eData.D_F190[0],UDS_ISO14229_Transfer+2,17);
            write2EData();
            UDS_Service_Response ( 0x2E, POSITIVE_RSP, DIAG_ID_Tx, 2, UDS_ISO14229_Transfer );
            break;

       
         case 0x3200:
       
          if(DiagLockFlag!=Level1UnLock)
          {
            NegRes.code=securityAccessDenied;//¦Ä????   33
            UDS_Service_Response(0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, (uint8_t*)(&NegRes.code));
            return;
          }
          
          if ( Service2EDiagDataLength != 16 + 2 )
          {
              NegRes.code = incorrectMessageLength; //??????????   13
              UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
              return;
          }
            (void)  memcpy(&Diag2eData.D_3200[0],UDS_ISO14229_Transfer+2,16);
            write2EData();
            UDS_Service_Response ( 0x2E, POSITIVE_RSP, DIAG_ID_Tx, 2, UDS_ISO14229_Transfer );
        break;    
        case 0xF1FA:
            if(DiagLockFlag!=Level1UnLock)
            {
                NegRes.code=securityAccessDenied;//??????   33
                UDS_Service_Response(0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, (uint8_t*)(&NegRes.code));
                return;
            }

            if ( Service2EDiagDataLength != 6 + 2 )
            {
                NegRes.code = incorrectMessageLength; //??????????   13
                UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }     
            (void)  memcpy(&Diag2eData.D_F1FA[0],UDS_ISO14229_Transfer+2,1);
            write2EData();
            UDS_Service_Response ( 0x2E, POSITIVE_RSP, DIAG_ID_Tx, 2, UDS_ISO14229_Transfer );
        break;    

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

void UDS_Service_2F_Indication ( uint32_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{  
    uint16_t i;
    uint16_t Service2FDiagDataLength;
    uint16_t ControlDataIdentifier;
    uint32_t DiagIDNum;
    //uint32_t TempBuffer;
    DiagIDNum = A_TA_type;
     S3_ServerCnt = 0;
     if(DiagIDNum == DIAG_ID_Rx_FUN) 
     {
        return;
     }
    NegRes.code = serviceNotSupported;    //????????
    UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );  
}


void UDS_Service_14_Indication ( uint32_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;
    //?????????????????0??????22??????          
    /*if (( HIGH_VOLTAGE )||( LOW_VOLTAGE )||(DATA_VEHICLE_SPEED_DISPLAYING>0))
    {
        NegRes.code = conditionsNotCorrect;
        UDS_Service_Response ( 0x14, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }*/
    //???????????????13??????
    if ( Service14DiagDataLength != 3 )
    {
        NegRes.code = incorrectMessageLength; //??????????
        UDS_Service_Response ( 0x14, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    //??????????????7E??????
    /*if ( SessionType == ProgrammingSession ) 
    {
        NegRes.code = serviceNotSupportedInActiveSession; //?????????
        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] ) );
    //??????????14 FF FF FF???????????
    if ( ClearDTCGroup == 0xffffff )
    {
        //ClearAllDTC();
        for ( i = 0; i < cnDTCslen; i++ )
        {
			DiagDtc[ i ].Status.Flag = 0x00;
            DTCTimerList[i] = 0;
			DTCUpdataStatusList [ i ] = 1u;
	        msgList[i][3] = 0;
        }

        UDS_Service_Response ( 0x14, POSITIVE_RSP, DIAG_ID_Tx, 0, UDS_ISO14229_Transfer );
        isNeedSaveDTC = 1;
        SaveDTCsToEEPROM();
    }
    else//??????????????14 FF FF FF??????31??????
    {   if ( A_TA_type != DIAG_ID_Rx_FUN ) 
        {
          NegRes.code = requestOutOfRange; //?????????
          UDS_Service_Response ( 0x14, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
          return;
        }
    }
}

void HandleOf2FServices ( void )      //lgh
{
    if ( ContrlorResumeofIO != Contrl )
    {
        if ( ContrlorResumeofIOLast == Contrl )
        {
            ContrlorResumeofIOLast = Resume;

              ContrlorResumeofIO = 0;
              ContrlorResumeofSOC = 0;     //soc????
              SocSeg = 0;                  //Soc
              ContrlorResumeofEspeed = 0;  //??????
              ESpeedValNUM = 0;            //???           
              ContrlorResumeofVspeed = 0;  //???????      
              VspeedValNUM = 0;            //???? 
              ContrlorResumeofLamp = 0;
              //ISD_Stop_Loop();
              IO_Group_1 = 0;
              IO_Group_2 = 0;
              IO_Group_3 = 0;
              IO_Group_4 = 0;
              
            return;
        }

        return;
    }

    ContrlorResumeofIOLast = ContrlorResumeofIO;
}
