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

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_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 0xF189:    //长安定义的ECU软件版本编号
            memcpy(SW_Version, &UDS_ISO14229_Transfer [ 2 ], Service22DiagDataLength - 2);
            break;
        case 0xF089:    //长安定义的ECU硬件版本编号

            memcpy(HW_Version, &UDS_ISO14229_Transfer [ 2 ], Service22DiagDataLength - 2);
            break;
        case 0xF188:    //零件号
            memcpy(Part_Version, &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 = 1;
            }
            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();
}
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 1:
            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 2:
            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( );
}
const uint8_t XorArray [ 4 ] = {0x74, 0xF7, 0x15, 0x17};

static void Ser27_CalculateKeyLV1(uint8_t Seed [], uint8_t Key [])
{
    uint8_t  i, j;
    uint8_t  seed2 [ 4 ] = {0};
    uint8_t  key [ 4 ]   = {0};
    uint8_t  key1 [ 4 ]  = {0};
    uint8_t  key2 [ 4 ]  = {0};
    uint32_t keyL        = 0;
    uint32_t key1L       = 0;
    uint32_t key2L       = 0;
    uint8_t *p;

    for ( i = 0; i < 4; ++i )
    {
        for ( j = 0; j < 8; ++j )
        {
            if ( j < 4 )
            {
                seed2 [ 3 - i ] |= (Seed [ i ] & (0x01 << j)) << ( uint8_t )(2 * (3 - j) + 1);
            }
            else
            {
                seed2 [ 3 - i ] |= (Seed [ i ] & (0x01 << j)) >> ( uint8_t )(2 * (j - 4) + 1);
            }
        }
    }

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

    key1L = ( uint32_t )key1 [ 0 ] << 24 | ( uint32_t )key1 [ 1 ] << 16 | ( uint32_t )key1 [ 2 ] << 8 | ( uint32_t )key1 [ 3 ];
    key2L = ( uint32_t )key2 [ 0 ] << 24 | ( uint32_t )key2 [ 1 ] << 16 | ( uint32_t )key2 [ 2 ] << 8 | ( uint32_t )key2 [ 3 ];

    keyL = key1L + key2L;

    for ( i = 0; i < 4; ++i )
    {
        key [ i ] = ( uint8_t )(keyL >> ( uint8_t )((3 - i) * 8));
    }

    // SeedKey为计算得到的最终Key
    Key [ 0 ] = key [ 0 ];
    Key [ 1 ] = key [ 1 ];
    Key [ 2 ] = key [ 2 ];
    Key [ 3 ] = key [ 3 ];
}

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;
    }
}
