#include "UDS_ISO14229_Client.h"
#include "TYW_stdint.h"
#include "Check_Ctrl.h"
#include "string.h"
#include "COM_CAN.h"

const uint8_t XorArray[4]= {0x56,0x45,0x73,0x54};//密钥使用


uint8_t     UDS_ISO14229_Transfer [ 200 ];
uint8_t     loc_buffer [ 100 ];
static void Ser27_CalculateKeyLV1(uint8_t Seed [], uint8_t Key []);

void UDS_Service_50_Indication(uint32_t A_TA_type, uint16_t A_Length, uint8_t A_Data [])
{
    uint32_t i;
    uint16_t DIDRequest;
    uint8_t  Service22DiagDataLength;
    Service22DiagDataLength = A_Length;
    for ( i = 0; i < Service22DiagDataLength; i++ )
    {
        UDS_ISO14229_Transfer [ i ] = *(A_Data + i);
    }
    if ( UDS_ISO14229_Transfer [ 0 ] == 0x3 )
    {
        Global_Session = 3;
        if ( loc_Step == 1 )
        {
            loc_Step = 2;
        }
    }
}
void UDS_Service_51_Indication(uint32_t A_TA_type, uint16_t A_Length, uint8_t A_Data [])
{
    uint32_t i;
    uint16_t DIDRequest;
    uint8_t  Service22DiagDataLength;
    Service22DiagDataLength = A_Length;
    for ( i = 0; i < Service22DiagDataLength; i++ )
    {
        UDS_ISO14229_Transfer [ i ] = *(A_Data + i);
    }
}

void UDS_Service_59_Indication(uint32_t A_TA_type, uint16_t A_Length, uint8_t A_Data [])
{
    uint32_t i;
    uint16_t DIDRequest;
    uint8_t  Service22DiagDataLength;
    Service22DiagDataLength = A_Length;
    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 0x0A09:    //天有为版本
            memcpy(Voltage_UDS, &UDS_ISO14229_Transfer [ 2 ], Service22DiagDataLength - 2);
            break;
    }
    Clear_IPK_NONE( );
}
void UDS_Service_62_Indication(uint32_t A_TA_type, uint16_t A_Length, uint8_t A_Data [])
{
    uint32_t i;
    uint16_t DIDRequest;
    uint8_t  Service22DiagDataLength;
    Service22DiagDataLength = A_Length;
    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 0xF086:    //天有为版本
            memcpy(InterNal_Version, &UDS_ISO14229_Transfer [ 2 ], Service22DiagDataLength - 2);
            break;


        case 0x2014:    //天有为版本
            memcpy(BCM_2014, &UDS_ISO14229_Transfer [ 2 ], Service22DiagDataLength - 2);
            break;

        case SV_DID:    //软件版本号
            memcpy(SW_Version, &UDS_ISO14229_Transfer [ 2 ], Service22DiagDataLength - 2);
            break;
        case HV_DID:    //硬件版本号          
            memcpy(HW_Version, &UDS_ISO14229_Transfer [ 2 ], Service22DiagDataLength - 2);
            break;
        case PV_DID:    //零件号
            memcpy(Part_Version, &UDS_ISO14229_Transfer [ 2 ], Service22DiagDataLength - 2);
            break;
        case FUEL_DID:    //燃油阻值
            RES_CHECK_BEGIN = 2u;
            memcpy(Res_St, &UDS_ISO14229_Transfer [ 2 ], Service22DiagDataLength - 2);
            break;  
        case DATE_DID:    //制造日期
            memcpy(Made_Date, &UDS_ISO14229_Transfer [ 2 ], Service22DiagDataLength - 2);
            break;
        case CODE_DID:    //供应商代码
            memcpy(SUPPLIER, &UDS_ISO14229_Transfer [ 2 ], Service22DiagDataLength - 2);
            break;  






        case 0xF202:
            memcpy(Evade_ODO, &UDS_ISO14229_Transfer [ 2 ], Service22DiagDataLength - 2);
            break;
        case 0xF203:
            memcpy(Risk_Evade, &UDS_ISO14229_Transfer [ 2 ], Service22DiagDataLength - 2);
            break;




        case 0x6200:    
            //Global_Session = UDS_ISO14229_Transfer [ 2 ];
            if ( loc_Step == 0 )
            {
                loc_Step = 5;
            }
            break;




        case 0x6201:    
            LINE_IN_CHECK_BEGIN = 2;
            memcpy(Line_IN_St, &UDS_ISO14229_Transfer [ 2 ], Service22DiagDataLength - 2);
            break;
        case 0x6202:    
            RES_CHECK_BEGIN = 2u;
            memcpy(Res_St, &UDS_ISO14229_Transfer [ 2 ], Service22DiagDataLength - 2);
            break;
        case 0x6203:    
            memcpy(Part_Version, &UDS_ISO14229_Transfer [ 2 ], Service22DiagDataLength - 2);
            break;
    }
    Clear_IPK_NONE( );
    //Execute_Specify_Step(6);
}   
uint8_t seed_buff [ 4 ];
void    UDS_Service_67_Indication(uint32_t A_TA_type, uint16_t A_Length, uint8_t A_Data [])//解锁
{
    uint32_t i;
    uint16_t DIDRequest;
    uint8_t  Service22DiagDataLength;
    Service22DiagDataLength = A_Length;
    for ( i = 0; i < Service22DiagDataLength; i++ )
    {
        UDS_ISO14229_Transfer [ i ] = *(A_Data + i);
    }
    DIDRequest = UDS_ISO14229_Transfer [ 0 ];
    switch ( DIDRequest )
    {
        case 3:
            UnLock_Flag = 2;
            if ( loc_Step == 2 )
            {
                loc_Step = 3;
            }
            memcpy(seed_buff, &UDS_ISO14229_Transfer [ 1 ], 4);
            Ser27_CalculateKeyLV1(seed_buff, KEYBUFFER);//密钥计算
            break;
        case 4:
            UnLock_Flag = 0x55;
            if ( loc_Step == 3 )
            {
                loc_Step = 4;
            }
            memset(seed_buff, 0, 4);
            memset(KEYBUFFER, 0, 4);
            break;
    }
}
void UDS_Service_6F_Indication(uint32_t A_TA_type, uint16_t A_Length, uint8_t A_Data [])
{
    uint32_t i;
    uint16_t DIDRequest;
    uint8_t  Service22DiagDataLength;
    Service22DiagDataLength = A_Length;
    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 0x6202:    // voice
            VOICE_CHECN_BEGIN = 2;
            break;
        case 0x6201:    // SEG LCD
            LCD_CHECN_BEGIN = 2u;
            break;
        case 0x6204:    // tft
            TFT_CHECN_BEGIN = 2u;
            break;
    }
}
void UDS_Service_6E_Indication(uint32_t A_TA_type, uint16_t A_Length, uint8_t A_Data [])
{
    uint32_t i;
    uint16_t DIDRequest;
    uint8_t  Service22DiagDataLength;
    Service22DiagDataLength = A_Length;
    for ( i = 0; i < Service22DiagDataLength; i++ )
    {
        UDS_ISO14229_Transfer [ i ] = *(A_Data + i);
    }
}
void UDS_Service_7E_Indication(uint32_t A_TA_type, uint16_t A_Length, uint8_t A_Data [])
{
    uint32_t i;
    uint16_t DIDRequest;
    uint8_t  Service22DiagDataLength;
    Service22DiagDataLength = A_Length;
    for ( i = 0; i < Service22DiagDataLength; i++ )
    {
        UDS_ISO14229_Transfer [ i ] = *(A_Data + i);
    }
    if ( loc_Step == 1 )
    {
        loc_Step = 2;
    }
    Clear_IPK_NONE( );
}


static void Ser27_CalculateKeyLV1(uint8_t Seed [], uint8_t Key []) //密钥计算： 小康FS04 / 小康FS02R
{
    uint16_t i;
    uint16_t seed = 0;
    uint32_t seed1 = 0;
    uint16_t mask=0x5528;
    uint32_t mask1 = 0x63938231;
    uint8_t Cal[4];
    
    //if(level == 0x03)
    if (1)
    {
        seed=((uint16_t)Seed[0])<<8;
        seed|=(((uint16_t)Seed[1])&0x00FF);
        if (seed !=0 ) 
        { 
            for (i=0; i<17; i++ ) 
            { 
                if (seed&0x8000) 
                { 
                    seed = (uint16_t)(seed <<1); 
                    seed = seed^mask; 
                } 
                else 
                { 
                    seed = seed << 1; 
                } 
            } 

        }   

        Key[0]=(uint8_t)(seed>>8);
        Key[1]=(uint8_t)(seed&0x00FF);
    }
    else
    { 
        seed1 |= Seed[0];    
        seed1 = (seed1 << 8) | Seed[1];
        seed1 = (seed1 << 8) | Seed[2];
        seed1 = (seed1 << 8) | Seed[3];
        for(i=0;i<35;i++)
        {    
            if(seed1&0x80000000)
            {
                seed1 = seed1 << 1;
                seed1^=mask1; 
            } 
            else
            {
                seed1 = seed1 << 1;
            }
        }
        Key[0] = (uint8_t)(seed1 >> 24);
        Key[1] = (uint8_t)(seed1 >> 16);
        Key[2] = (uint8_t)(seed1 >> 8);
        Key[3] = (uint8_t)seed1; 
    } 
}

void UDS_Service_7F_Indication(uint32_t A_TA_type, uint16_t A_Length, uint8_t A_Data [])
{
    uint32_t i;
    uint16_t DIDRequest;
    uint8_t  Service22DiagDataLength;
    Service22DiagDataLength = A_Length;
    for ( i = 0; i < Service22DiagDataLength; i++ )
    {
        UDS_ISO14229_Transfer [ i ] = *(A_Data + i);
    }
    if ( (UDS_ISO14229_Transfer [ 1 ] == 0x33) || (UDS_ISO14229_Transfer [ 1 ] == 0x7F) )
    {
        UnLock_Flag = 0;
        loc_Step    = 0;
    }
}
