Process9 CMAC implementation

P9_CMAC.c

#include "aes.h"

static void aes128_processBlock(u8 *dst, const u8 *src, u32 keyslot)
{
	u8 zeroes[16]  __attribute__((aligned(32))) = {0};
	use_aeskey(keyslot);
	set_ctr(zeroes);
	aes_decrypt(src, dst, 1, (AES_CBC_ENCRYPT_MODE | AES_CNT_INPUT_ORDER | AES_CNT_OUTPUT_ORDER | AES_CNT_INPUT_ENDIAN | AES_CNT_OUTPUT_ENDIAN));
}

static void multiplyBy2(u8 buf[16])
{
	bool finalXOR = (buf[0] & 0x80) != 0;
	for(u32 i = 0; i < 15; i++)
		buf[i] = (buf[i] << 1) | (buf[i + 1] >> 7);

	buf[15] <<= 1;
	if(finalXOR)
		buf[15] ^= 0x87;
}

void CMAC(u8 result[16], u8 *data, u32 size, u32 keyslot)
{
	// Looks like it's CMAC (you can compare it to http://ftp.usa.openbsd.org/pub/OpenBSD/src/sys/crypto/cmac.c)
	u8 vec[16] = {0};
	u8* end = data + size;
	for(u32 i = 0; i < size / 16; i++)
	{
		for(u32 j = 0; j < 16; j++)
			vec[j] ^= *data++;
		if(data != end)
			aes128_processBlock(vec, vec, keyslot);
	}

	// Last block
	u32 remaining = size % 16;
	u8 vec2[16] = {0};
	
	for(u32 i = 0; i < remaining; i++)
		vec[i] ^= *data++;
	
	aes128_processBlock(vec2, vec2, keyslot);
	multiplyBy2(vec2);

	if(remaining != 0)
	{
		vec[remaining] ^= 0x80;
		multiplyBy2(vec2);
	}

	for(u32 i = 0; i < 16; i++)
		vec2[i] ^= vec[i];

	aes128_processBlock(result, vec2, keyslot);
}