/******************************************************************************/
/* Williams Advanced Engineering Limited                     Copyright(C) 2021*/
/* Grove                                                  All rights reserved */
/*                                                                            */
/******************************************************************************/

#include "cmac.h"
#include "aes.h"
#include "aes_utils.h"
#include "Ios_Std.h"


// Calculate the CAMC
uint8* aes_cmac(uint8* in, uint32 length, uint8* out, uint8* key)
{
    uint8 K1[16] = {0};
    uint8 K2[16] = {0};
    uint8 i;

    uint8 X[] = {
                 0x00, 0x00, 0x00, 0x00,
                 0x00, 0x00, 0x00, 0x00,
                 0x00, 0x00, 0x00, 0x00,
                 0x00, 0x00, 0x00, 0x00 };
    uint8 Y[const_Bsize];
    uint8 M[1][const_Bsize];
    uint32 n = (length / const_Bsize);
    boolean flag = FALSE;

    if(length == const_Bsize)
    {
        GenerateSubkey(key, K1, K2);

        if (length % const_Bsize != 0)
        {
            n++;
        }

        if (n == 0)
        {
            n = 1;
        }
        else if (length % const_Bsize == 0)
        {
            flag = TRUE;
        }


        memset(M[0], 0, n * const_Bsize);
        memcpy(M[0], in, length);

        if (!flag)
        {
            memset(M[0] + length, 0x80, 1);
        }

        if (flag)
        {
            block_xor(M[n - 1], M[n - 1], K1);
        }
        else
        {
            block_xor(M[n - 1], M[n - 1], K2);
        }

        for (i = 0; i < n - 1; i++)
        {
            block_xor(Y, M[i], X);
            aes_128_encrypt(Y, X, key);
        }

        block_xor(Y, M[n - 1], X);

        aes_128_encrypt(Y, out, key);
    }
    else
    {
        wcu_printf("Aes_cmac: length = %d argument has wrong value\n", length);
    }

    return out;
}

// Verify the CMAC
boolean verify_mac(uint8* in, unsigned int length, uint8* out, uint8* key)
{
    boolean flag = TRUE;
    uint8 result[16];
    uint8 i;
    aes_cmac(in, length, (uint8*)result, key);

    for (i = 0; i < const_Bsize; i++)
    {
        if (!(result[i] ^ out[i]))
        {
            flag = FALSE;
            break;
        }
    }
    return flag;
}

// Generate the Sub keys
void GenerateSubkey(uint8* key, uint8* K1, uint8* K2)
{
    uint8 const_Zero[] = {
        0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00
    };

    uint8 const_Rb[] = {
        0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x87
    };

    uint8 L[16];
    aes_128_encrypt(const_Zero, L, key);

    block_leftshift(K1, L);

    if (L[0] & 0x80)
    {
        block_xor(K1, K1, const_Rb);
    }

    block_leftshift(K2, K1);

    if (K1[0] & 0x80)
    {
        block_xor(K2, K2, const_Rb);
    }
}
