/***************************************************************
*
* Broadcom Corp. Confidential
* Copyright 2011 Broadcom Corp. All Rights Reserved.
*
* THIS SOFTWARE MAY ONLY BE USED SUBJECT TO AN EXECUTED
* SOFTWARE LICENSE AGREEMENT BETWEEN THE USER AND BROADCOM.
* YOU HAVE NO RIGHT TO USE OR EXPLOIT THIS MATERIAL EXCEPT
* SUBJECT TO THE TERMS OF SUCH AN AGREEMENT.
*
* File:		aes-cmac.c
* Description: aes-cmac reference implementation. this implementation
* is lifted from RFC-4493 and modified to work with particular aes 
* implementation that I had. This used only for testing of signatures
* and not intended for production since it is very slow.
*
* Created: Mon November 15 2010.
*
*
*
****************************************************************/

#include "ministd.h"
#include "aes.h"

/* constants */

unsigned char const_Zero[16] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
unsigned char const_Rb[16] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0x87};

void xor_128(unsigned char *a, unsigned char *b, unsigned char *out)
{
	int i;
	for (i=0;i<16; i++)
	{
		out[i] = a[i] ^ b[i];
	}
}

void leftshift_onebit(unsigned char *input,unsigned char *output)
{
	int         i;
	unsigned char overflow = 0;

	for ( i=15; i>=0; i-- ) {
		output[i] = input[i] << 1;
		output[i] |= overflow;
		overflow = (input[i] & 0x80)?1:0;
	}
	return;
}

void generate_subkey(aes_ctx_t *ctx, unsigned char *K1, unsigned char *K2)
{
	unsigned char L[16];
	unsigned char tmp[16];

	aes_encrypt(ctx,const_Zero,L);

	if ( (L[0] & 0x80) == 0 ) { /* If MSB(L) = 0, then K1 = L << 1 */
		leftshift_onebit(L,K1);
	} else {    /* Else K1 = ( L << 1 ) (+) Rb */

		leftshift_onebit(L,tmp);
		xor_128(tmp,const_Rb,K1);
	}

	if ( (K1[0] & 0x80) == 0 ) {
		leftshift_onebit(K1,K2);
	} else {
		leftshift_onebit(K1,tmp);
		xor_128(tmp,const_Rb,K2);
	}
	return;
}

void padding ( unsigned char *lastb, unsigned char *pad, int length )
{
	int         j;

	/* original last block */
	for ( j=0; j<16; j++ ) {
		if ( j < length ) {
			pad[j] = lastb[j];
		} else if ( j == length ) {
			pad[j] = 0x80;
		} else {
			pad[j] = 0x00;
		}
	}
}

void aes_cmac ( aes_ctx_t *ctx, unsigned char *input, int length, unsigned char *mac )
{
	unsigned char       X[16],Y[16], M_last[16], padded[16];
	unsigned char       K1[16], K2[16];
	int         n, i, flag;
	generate_subkey(ctx, K1,K2);

	n = (length+15) / 16;       /* n is number of rounds */

	if ( n == 0 ) {
		n = 1;
		flag = 0;
	} else {
		if ( (length%16) == 0 ) { /* last block is a complete block */
			flag = 1;
		} else { /* last block is not complete block */
			flag = 0;
		}


	}

	if ( flag ) { /* last block is complete block */
		xor_128(&input[16*(n-1)],K1,M_last);
	} else {
		padding(&input[16*(n-1)],padded,length%16);
		xor_128(padded,K2,M_last);
	}

	for ( i=0; i<16; i++ ) X[i] = 0;
	for ( i=0; i<n-1; i++ ) {
		xor_128(X,&input[16*i],Y); /* Y := Mi (+) X  */
		aes_encrypt(ctx,Y,X);      /* X := AES-128(KEY, Y); */
	}

	xor_128(X,M_last,Y);
	aes_encrypt(ctx,Y,X);

	for ( i=0; i<16; i++ ) {
		mac[i] = X[i];
	}
}

int test_aes_cmac()
{
	unsigned char L[16], K1[16], K2[16], T[16];
	unsigned char M[64] = {
		0x6b, 0xc1, 0xbe, 0xe2, 0x2e, 0x40, 0x9f, 0x96,
		0xe9, 0x3d, 0x7e, 0x11, 0x73, 0x93, 0x17, 0x2a,
		0xae, 0x2d, 0x8a, 0x57, 0x1e, 0x03, 0xac, 0x9c,
		0x9e, 0xb7, 0x6f, 0xac, 0x45, 0xaf, 0x8e, 0x51,
		0x30, 0xc8, 0x1c, 0x46, 0xa3, 0x5c, 0xe4, 0x11,
		0xe5, 0xfb, 0xc1, 0x19, 0x1a, 0x0a, 0x52, 0xef,
		0xf6, 0x9f, 0x24, 0x45, 0xdf, 0x4f, 0x9b, 0x17,
		0xad, 0x2b, 0x41, 0x7b, 0xe6, 0x6c, 0x37, 0x10
	};
	unsigned char key[16] = {
		0x2b, 0x7e, 0x15, 0x16, 0x28, 0xae, 0xd2, 0xa6,
		0xab, 0xf7, 0x15, 0x88, 0x09, 0xcf, 0x4f, 0x3c
	};

	aes_ctx_t *ctx;

	ctx = aes_alloc_ctx(key, sizeof(key));

	printf("\nSubkey Generation\n");
	aes_encrypt(ctx,const_Zero,L);

	generate_subkey(ctx,K1,K2);


	aes_cmac(ctx,M,0,T);
	aes_cmac(ctx,M,16,T);
	aes_cmac(ctx,M,40,T);
	aes_cmac(ctx,M,64,T);
	printf("--------------------------------------------------\n");
	aes_free_ctx(ctx);

	return 0;
}