/*! *********************************************************************************
* Copyright 2022 NXP
* All rights reserved.
*
* \file
*
* SPDX-License-Identifier: BSD-3-Clause
***********************************************************************************/

#include "EmbeddedTypes.h"
#include "PhyPacket.h"
#include "Phy.h"
#include <string.h>
#include <assert.h>
#include <fsl_ltc.h>

static bool_t sPhySecurity = FALSE;
static uint8_t sPrevKey[16];
static uint8_t sCurrKey[16];
static uint8_t sNextKey[16];

uint8_t  sKeyId = 0;
uint32_t sFrameCounter = 0;
uint32_t sUpdateFrameCounter = 0;

void PhySec_Enable()
{
    sPhySecurity = TRUE;
}

bool_t PhySec_IsEnabled()
{
    return sPhySecurity;
}

void PhySec_SetKeys(uint8_t keyId, uint8_t *prevKey, uint8_t *currKey, uint8_t *nextKey)
{
    sKeyId = keyId;
    memcpy(sPrevKey, prevKey, 16);
    memcpy(sCurrKey, currKey, 16);
    memcpy(sNextKey, nextKey, 16);
}

static void PhySec_GenerateNonce(uint8_t *nonce, uint8_t secLevel)
{
    (void)PhyPpGetLongAddr(nonce, 0);
    nonce += 8;

    /* Big-Endian Format */
    nonce[0] = (sFrameCounter >> 24) & 0xff;
    nonce[1] = (sFrameCounter >> 16) & 0xff;
    nonce[2] = (sFrameCounter >>  8) & 0xff;
    nonce[3] = (sFrameCounter >>  0) & 0xff;
    nonce += 4;

    nonce[0] = secLevel;
}

void PhySec_Encrypt(uint8_t *packet, uint8_t packetLength)
{
    phyFcf_t *fcf = (phyFcf_t *)(packet);

    if (fcf->securityEnabled)
    {
        uint8_t *secHeader = PhyPacket_GetSecurityHeader(packet);
        uint8_t keyId      = PhyPacket_GetKeyIndex(packet);
        uint8_t keyIdMode  = PhyPacket_GetKeyIdMode(secHeader);
        uint8_t secLevel   = PhyPacket_GetSecurityLevel(packet);
        uint8_t micLength  = PhyPacket_ComputeMicLength(secLevel);
        uint8_t mhrLength  = PhyPacket_GetMacHdrLength(packet, packetLength);
        uint8_t dataLength = packetLength - mhrLength - micLength - 2;
        uint8_t *encData   = packet + mhrLength;
        uint8_t nonce[13];
        uint8_t mic[16];
        uint8_t *key = NULL;

        assert(keyIdMode == 1);
        assert(keyId != 0);

        if (keyId == sKeyId)
        {
            key = sCurrKey;
        }
        else if (keyId == sKeyId - 1)
        {
            key = sPrevKey;
        }
        else if (keyId == sKeyId + 1)
        {
            key = sNextKey;
        } else {
            assert(0);
        }

        PhySec_GenerateNonce(nonce, secLevel);

        LTC_AES_EncryptTagCcm(LTC0, encData, encData, dataLength, nonce, 13, packet, mhrLength, key, 16, mic, micLength);

        memcpy(packet + packetLength - micLength - 2, mic, micLength);
    }
}
