/*!
 * \file      soft-se.c
 *
 * \brief     Secure Element software implementation
 *
 * \copyright Revised BSD License, see section \ref LICENSE.
 *
 * \code
 *                ______                              _
 *               / _____)             _              | |
 *              ( (____  _____ ____ _| |_ _____  ____| |__
 *               \____ \| ___ |    (_   _) ___ |/ ___)  _ \
 *               _____) ) ____| | | || |_| ____( (___| | | |
 *              (______/|_____)_|_|_| \__)_____)\____)_| |_|
 *              (C)2020 Semtech
 *
 *               ___ _____ _   ___ _  _____ ___  ___  ___ ___
 *              / __|_   _/_\ / __| |/ / __/ _ \| _ \/ __| __|
 *              \__ \ | |/ _ \ (__| ' <| _| (_) |   / (__| _|
 *              |___/ |_/_/ \_\___|_|\_\_| \___/|_|_\\___|___|
 *              embedded.connectivity.solutions===============
 *
 * \endcode
 *
 */
#include <stdlib.h>
#include <stdint.h>

#include "system/utilities.h"
#include "aes.h"
#include "cmac.h"

#include "mac/LoRaMacHeaderTypes.h"

#include "mac/secure-element.h"
#include "mac/secure-element-nvm.h"
#include "se-identity.h"
#include "soft-se-hal.h"
#include <Arduino.h>

RTC_DATA_ATTR static SecureElementNvmData_t* SeNvm;

/*
 * Local functions
 */

/*
 * Gets key item from key list.
 *
 * \param[IN]  keyID          - Key identifier
 * \param[OUT] keyItem        - Key item reference
 * \retval                    - Status of the operation
 */
static SecureElementStatus_t GetKeyByID( KeyIdentifier_t keyID, Key_t** keyItem )
{
    for( uint8_t i = 0; i < NUM_OF_KEYS; i++ )
    {
        if( SeNvm->KeyList[i].KeyID == keyID )
        {
            *keyItem = &( SeNvm->KeyList[i] );
            return SECURE_ELEMENT_SUCCESS;
        }
    }
    return SECURE_ELEMENT_ERROR_INVALID_KEY_ID;
}

/*
 * Computes a CMAC of a message using provided initial Bx block
 *
 *  cmac = aes128_cmac(keyID, blocks[i].Buffer)
 *
 * \param[IN]  micBxBuffer    - Buffer containing the initial Bx block
 * \param[IN]  buffer         - Data buffer
 * \param[IN]  size           - Data buffer size
 * \param[IN]  keyID          - Key identifier to determine the AES key to be used
 * \param[OUT] cmac           - Computed cmac
 * \retval                    - Status of the operation
 */
static SecureElementStatus_t ComputeCmac( uint8_t* micBxBuffer, uint8_t* buffer, uint16_t size, KeyIdentifier_t keyID,
                                          uint32_t* cmac )
{
    if( ( buffer == NULL ) || ( cmac == NULL ) )
    {
        return SECURE_ELEMENT_ERROR_NPE;
    }

    uint8_t Cmac[16];
    AES_CMAC_CTX aesCmacCtx[1];

    AES_CMAC_Init( aesCmacCtx );

    Key_t*                keyItem;
    SecureElementStatus_t retval = GetKeyByID( keyID, &keyItem );

    // uint8_t * temp_Key = keyItem->KeyValue;
    // printf("keyItem->KeyValue = ");
    // for(uint8_t i = 0; i < 16; i++)
    // {   
    //     printf("%02X", temp_Key[i]);
    // }
    // printf("\n");

    if( retval == SECURE_ELEMENT_SUCCESS )
    {
        AES_CMAC_SetKey( aesCmacCtx, keyItem->KeyValue );

        if( micBxBuffer != NULL )
        {
            AES_CMAC_Update( aesCmacCtx, micBxBuffer, 16 );
        }

        AES_CMAC_Update( aesCmacCtx, buffer, size );

        AES_CMAC_Final( Cmac, aesCmacCtx );

        // Bring into the required format
        *cmac = ( uint32_t )( ( uint32_t ) Cmac[3] << 24 | ( uint32_t ) Cmac[2] << 16 | ( uint32_t ) Cmac[1] << 8 |
                              ( uint32_t ) Cmac[0] );
    }

    return retval;
}

/*
 * API functions
 */

SecureElementStatus_t SecureElementInit( SecureElementNvmData_t* nvm )
{
    SecureElementNvmData_t seNvmInit =
    {
        /*!
        * end-device IEEE EUI (big endian)
        *
        * \remark In this application the value is automatically generated by
        *         calling BoardGetUniqueId function
        */
        .DevEui = LORAWAN_DEVICE_EUI,
        /*!
        * App/Join server IEEE EUI (big endian)
        */
        .JoinEui = LORAWAN_JOIN_EUI,
        /*!
        * Secure-element pin (big endian)
        */
        .Pin = SECURE_ELEMENT_PIN,
        /*!
        * LoRaWAN key list
        */
        .KeyList = SOFT_SE_KEY_LIST
    };


    if( nvm == NULL )
    {
        return SECURE_ELEMENT_ERROR_NPE;
    }

    // Initialize nvm pointer
    SeNvm = nvm;

    // Initialize data
    memcpy1( ( uint8_t* )SeNvm, ( uint8_t* )&seNvmInit, sizeof( seNvmInit ) );

#if !defined( SECURE_ELEMENT_PRE_PROVISIONED )
#if( STATIC_DEVICE_EUI == 0 )
    // Get a DevEUI from MCU unique ID
    SoftSeHalGetUniqueId( SeNvm->DevEui );
#endif
#endif
    return SECURE_ELEMENT_SUCCESS;
}

SecureElementStatus_t SecureElementSetKey( KeyIdentifier_t keyID, uint8_t* key )
{
    if( key == NULL )
    {
        return SECURE_ELEMENT_ERROR_NPE;
    }

    for( uint8_t i = 0; i < NUM_OF_KEYS; i++ )
    {
        if( SeNvm->KeyList[i].KeyID == keyID )
        {
            if( ( keyID == MC_KEY_0 ) || ( keyID == MC_KEY_1 ) || ( keyID == MC_KEY_2 ) || ( keyID == MC_KEY_3 ) )
            {  // Decrypt the key if its a Mckey
                SecureElementStatus_t retval           = SECURE_ELEMENT_ERROR;
                uint8_t               decryptedKey[16] = { 0 };

                retval = SecureElementAesEncrypt( key, 16, MC_KE_KEY, decryptedKey );

                memcpy1( SeNvm->KeyList[i].KeyValue, decryptedKey, SE_KEY_SIZE );
                return retval;
            }
            else
            {
                memcpy1( SeNvm->KeyList[i].KeyValue, key, SE_KEY_SIZE );
                return SECURE_ELEMENT_SUCCESS;
            }
        }
    }

    return SECURE_ELEMENT_ERROR_INVALID_KEY_ID;
}

SecureElementStatus_t SecureElementComputeAesCmac( uint8_t* micBxBuffer, uint8_t* buffer, uint16_t size,
                                                   KeyIdentifier_t keyID, uint32_t* cmac )
{
    if( keyID >= LORAMAC_CRYPTO_MULTICAST_KEYS )
    {
        // Never accept multicast key identifier for cmac computation
        return SECURE_ELEMENT_ERROR_INVALID_KEY_ID;
    }

    return ComputeCmac( micBxBuffer, buffer, size, keyID, cmac );
}

SecureElementStatus_t SecureElementVerifyAesCmac( uint8_t* buffer, uint16_t size, uint32_t expectedCmac,
                                                  KeyIdentifier_t keyID )
{
    if( buffer == NULL )
    {
        return SECURE_ELEMENT_ERROR_NPE;
    }

    SecureElementStatus_t retval   = SECURE_ELEMENT_ERROR;
    uint32_t              compCmac = 0;
    retval                         = ComputeCmac( NULL, buffer, size, keyID, &compCmac );

    // printf("\n------SecureElementVerifyAesCmac expectedCmac(GW) = 0x%8X------\n", expectedCmac);
    // printf("\n------SecureElementVerifyAesCmac compCmac         = 0x%8X------\n", compCmac);

    if( retval != SECURE_ELEMENT_SUCCESS )
    {
        return retval;
    }

    if( expectedCmac != compCmac )
    {
        retval = SECURE_ELEMENT_FAIL_CMAC;          // CMAC不匹配
    }

    return retval;
}

SecureElementStatus_t SecureElementAesEncrypt( uint8_t* buffer, uint16_t size, KeyIdentifier_t keyID,
                                               uint8_t* encBuffer )
{
    if( buffer == NULL || encBuffer == NULL )
    {
        return SECURE_ELEMENT_ERROR_NPE;
    }

    // Check if the size is divisible by 16,
    if( ( size % 16 ) != 0 )
    {
        return SECURE_ELEMENT_ERROR_BUF_SIZE;
    }
// printf("-----------SecureElementAesEncrypt 1  step------------\n");
    aes_context aesContext;
    memset1( aesContext.ksch, '\0', 240 );

    Key_t*                pItem;
    SecureElementStatus_t retval = GetKeyByID( keyID, &pItem );

    if( retval == SECURE_ELEMENT_SUCCESS )
    {
        aes_set_key( pItem->KeyValue, 16, &aesContext );

        uint8_t block = 0;

        while( size != 0 )
        {
            lora_aes_encrypt( &buffer[block], &encBuffer[block], &aesContext );
            block = block + 16;
            size  = size - 16;
        }
    }
    return retval;
}

SecureElementStatus_t SecureElementDeriveAndStoreKey( uint8_t* input, KeyIdentifier_t rootKeyID,
                                                      KeyIdentifier_t targetKeyID )
{
    if( input == NULL )
    {
        return SECURE_ELEMENT_ERROR_NPE;
    }

    SecureElementStatus_t retval  = SECURE_ELEMENT_ERROR;
    uint8_t               key[16] = { 0 };

    // In case of MC_KE_KEY, only McRootKey can be used as root key
    if( targetKeyID == MC_KE_KEY )
    {
        if( rootKeyID != MC_ROOT_KEY )
        {
            return SECURE_ELEMENT_ERROR_INVALID_KEY_ID;
        }
    }

    // Derive key   得出密钥
    retval = SecureElementAesEncrypt( input, 16, rootKeyID, key );
    if( retval != SECURE_ELEMENT_SUCCESS )
    {
        return retval;
    }

    // Store key    储存密钥
    retval = SecureElementSetKey( targetKeyID, key );
    if( retval != SECURE_ELEMENT_SUCCESS )
    {
        return retval;
    }

    return SECURE_ELEMENT_SUCCESS;
}

SecureElementStatus_t SecureElementProcessJoinAccept( JoinReqIdentifier_t joinReqType, uint8_t* joinEui,
                                                      uint16_t devNonce, uint8_t* encJoinAccept,
                                                      uint8_t encJoinAcceptSize, uint8_t* decJoinAccept,
                                                      uint8_t* versionMinor )
{
    if( ( encJoinAccept == NULL ) || ( decJoinAccept == NULL ) || ( versionMinor == NULL ) )
    {
        return SECURE_ELEMENT_ERROR_NPE;
    }

    // Check that frame size isn't bigger than a JoinAccept with CFList size
    if( encJoinAcceptSize > LORAMAC_JOIN_ACCEPT_FRAME_MAX_SIZE )
    {
        return SECURE_ELEMENT_ERROR_BUF_SIZE;
    }

    // Determine decryption key
    KeyIdentifier_t encKeyID = NWK_KEY;

    if( joinReqType != JOIN_REQ )
    {
        encKeyID = J_S_ENC_KEY;
    }

    memcpy1( decJoinAccept, encJoinAccept, encJoinAcceptSize );

    // Decrypt JoinAccept, skip MHDR
    if( SecureElementAesEncrypt( encJoinAccept + LORAMAC_MHDR_FIELD_SIZE, encJoinAcceptSize - LORAMAC_MHDR_FIELD_SIZE,
                                 encKeyID, decJoinAccept + LORAMAC_MHDR_FIELD_SIZE ) != SECURE_ELEMENT_SUCCESS )
    {
        return SECURE_ELEMENT_FAIL_ENCRYPT;
    }

    *versionMinor = ( ( decJoinAccept[11] & 0x80 ) == 0x80 ) ? 1 : 0;

    uint32_t mic = 0;

    mic = ( ( uint32_t ) decJoinAccept[encJoinAcceptSize - LORAMAC_MIC_FIELD_SIZE] << 0 );
    mic |= ( ( uint32_t ) decJoinAccept[encJoinAcceptSize - LORAMAC_MIC_FIELD_SIZE + 1] << 8 );
    mic |= ( ( uint32_t ) decJoinAccept[encJoinAcceptSize - LORAMAC_MIC_FIELD_SIZE + 2] << 16 );
    mic |= ( ( uint32_t ) decJoinAccept[encJoinAcceptSize - LORAMAC_MIC_FIELD_SIZE + 3] << 24 );

    //  - Header buffer to be used for MIC computation
    //        - LoRaWAN 1.0.x : micHeader = [MHDR(1)]
    //        - LoRaWAN 1.1.x : micHeader = [JoinReqType(1), JoinEUI(8), DevNonce(2), MHDR(1)]

    // Verify mic
    if( *versionMinor == 0 )
    {
        // For LoRaWAN 1.0.x
        //   cmac = aes128_cmac(NwkKey, MHDR |  JoinNonce | NetID | DevAddr | DLSettings | RxDelay | CFList |
        //   CFListType)
        if( SecureElementVerifyAesCmac( decJoinAccept, ( encJoinAcceptSize - LORAMAC_MIC_FIELD_SIZE ), mic, NWK_KEY ) !=
            SECURE_ELEMENT_SUCCESS )
        {
            return SECURE_ELEMENT_FAIL_CMAC;
        }
    }
#if( USE_LRWAN_1_1_X_CRYPTO == 1 )
    else if( *versionMinor == 1 )
    {
        uint8_t  micHeader11[JOIN_ACCEPT_MIC_COMPUTATION_OFFSET] = { 0 };
        uint16_t bufItr                                          = 0;

        micHeader11[bufItr++] = ( uint8_t ) joinReqType;

        memcpyr( micHeader11 + bufItr, joinEui, LORAMAC_JOIN_EUI_FIELD_SIZE );
        bufItr += LORAMAC_JOIN_EUI_FIELD_SIZE;

        micHeader11[bufItr++] = devNonce & 0xFF;
        micHeader11[bufItr++] = ( devNonce >> 8 ) & 0xFF;

        // For LoRaWAN 1.1.x and later:
        //   cmac = aes128_cmac(JSIntKey, JoinReqType | JoinEUI | DevNonce | MHDR | JoinNonce | NetID | DevAddr |
        //   DLSettings | RxDelay | CFList | CFListType)
        // Prepare the msg for integrity check (adding JoinReqType, JoinEUI and DevNonce)
        uint8_t localBuffer[LORAMAC_JOIN_ACCEPT_FRAME_MAX_SIZE + JOIN_ACCEPT_MIC_COMPUTATION_OFFSET] = { 0 };

        memcpy1( localBuffer, micHeader11, JOIN_ACCEPT_MIC_COMPUTATION_OFFSET );
        memcpy1( localBuffer + JOIN_ACCEPT_MIC_COMPUTATION_OFFSET - 1, decJoinAccept, encJoinAcceptSize );

        if( SecureElementVerifyAesCmac( localBuffer,
                                        encJoinAcceptSize + JOIN_ACCEPT_MIC_COMPUTATION_OFFSET -
                                            LORAMAC_MHDR_FIELD_SIZE - LORAMAC_MIC_FIELD_SIZE,
                                        mic, J_S_INT_KEY ) != SECURE_ELEMENT_SUCCESS )
        {
            return SECURE_ELEMENT_FAIL_CMAC;
        }
    }
#endif
    else
    {
        return SECURE_ELEMENT_ERROR_INVALID_LORAWAM_SPEC_VERSION;
    }

    return SECURE_ELEMENT_SUCCESS;
}

SecureElementStatus_t SecureElementRandomNumber( uint32_t* randomNum )
{
    if( randomNum == NULL )
    {
        return SECURE_ELEMENT_ERROR_NPE;
    }
    *randomNum = SoftSeHalGetRandomNumber( );
    return SECURE_ELEMENT_SUCCESS;
}

SecureElementStatus_t SecureElementSetDevEui( uint8_t* devEui )
{
    if( devEui == NULL )
    {        
        return SECURE_ELEMENT_ERROR_NPE;
    }

    if (SeNvm == NULL) 
    {
    printf("Error: SeNvm is NULL\n");
    return SECURE_ELEMENT_ERROR;
    }

    if (SeNvm->DevEui == NULL) 
    {
    printf("SeNvm->DevEui is NULL\n");
    return SECURE_ELEMENT_ERROR;
    }


    memcpy1( SeNvm->DevEui, devEui, SE_EUI_SIZE );
    // printf("---------------- ------------------\n");
    return SECURE_ELEMENT_SUCCESS;
}

uint8_t* SecureElementGetDevEui( void )
{
    return SeNvm->DevEui;
}

SecureElementStatus_t SecureElementSetJoinEui( uint8_t* joinEui )
{
    if( joinEui == NULL )
    {
        return SECURE_ELEMENT_ERROR_NPE;
    }
    memcpy1( SeNvm->JoinEui, joinEui, SE_EUI_SIZE );
    return SECURE_ELEMENT_SUCCESS;
}

uint8_t* SecureElementGetJoinEui( void )
{
    return SeNvm->JoinEui;
}

SecureElementStatus_t SecureElementSetPin( uint8_t* pin )
{
    if( pin == NULL )
    {
        return SECURE_ELEMENT_ERROR_NPE;
    }

    memcpy1( SeNvm->Pin, pin, SE_PIN_SIZE );
    return SECURE_ELEMENT_SUCCESS;
}

uint8_t* SecureElementGetPin( void )
{
    return SeNvm->Pin;
}

// 获取APPSKEY
uint8_t* SecureElementGetAppskey( void )
{
    Key_t* pAppSKey;
    SecureElementStatus_t retval = GetKeyByID( APP_S_KEY, &pAppSKey );
    if( retval == SECURE_ELEMENT_SUCCESS )  // 获取成功
    {
        return pAppSKey->KeyValue;
    }
    else
    {
        return NULL;
    }
}

// 获取NWKSKEY
uint8_t* SecureElementGetNwkskey( void )
{
    Key_t* pNwkSKey;
    SecureElementStatus_t retval = GetKeyByID( NWK_S_ENC_KEY, &pNwkSKey );
    if( retval == SECURE_ELEMENT_SUCCESS )  // 获取成功
    {
        return pNwkSKey->KeyValue;
    }
    else
    {
        return NULL;
    }
}

// 打印NWKSKEY的三个组成密钥 Fkey Skey EncKey
void ShowSecureElementAllNwkskey( void )
{
    Key_t* pAppSKey;
    SecureElementStatus_t retval0 = GetKeyByID( APP_S_KEY, &pAppSKey );
    if( retval0 == SECURE_ELEMENT_SUCCESS )  // 获取成功
    {        
        uint8_t * temp_Key = pAppSKey->KeyValue;
        printf("APP_S_KEY = ");
        for(uint8_t i = 0; i < 16; i++)
        {   
            printf("%02X", temp_Key[i]);
        }
        printf("\n");
    }
    else
    {
        printf("Get APP_S_KEY ERROR!\n");
    }

    Key_t* FNwkSKey;
    SecureElementStatus_t retval1 = GetKeyByID( F_NWK_S_INT_KEY, &FNwkSKey );
    if( retval1 == SECURE_ELEMENT_SUCCESS )  // 获取成功
    {
        uint8_t * temp_Key = FNwkSKey->KeyValue;
        printf("F_NWK_S_INT_KEY = ");
        for(uint8_t i = 0; i < 16; i++)
        {   
            printf("%02X", temp_Key[i]);
        }
        printf("\n");
    }
    else
    {
        printf("Get F_NWK_S_INT_KEY ERROR!\n");
    }

    Key_t* SNwkSKey;
    SecureElementStatus_t retval2 = GetKeyByID( S_NWK_S_INT_KEY, &SNwkSKey );
    if( retval2 == SECURE_ELEMENT_SUCCESS )  // 获取成功
    {
        uint8_t * temp_Key = SNwkSKey->KeyValue;
        printf("S_NWK_S_INT_KEY = ");
        for(uint8_t i = 0; i < 16; i++)
        {   
            printf("%02X", temp_Key[i]);
        }
        printf("\n");
    }
    else
    {
        printf("Get S_NWK_S_INT_KEY ERROR!\n");
    }

    Key_t* ENwkSKey;
    SecureElementStatus_t retval3 = GetKeyByID( NWK_S_ENC_KEY, &ENwkSKey );
    if( retval3 == SECURE_ELEMENT_SUCCESS )  // 获取成功
    {
        uint8_t * temp_Key = ENwkSKey->KeyValue;
        printf("NWK_S_ENC_KEY = ");
        for(uint8_t i = 0; i < 16; i++)
        {   
            printf("%02X", temp_Key[i]);
        }
        printf("\n");
    }
    else
    {
        printf("Get NWK_S_ENC_KEY ERROR!\n");
    }

    return;
}
