/* SensorBoard Secure Library
 *  Subject: AES-CMAC implementation from  IETF RFC 4493
 *    (http://tools.ietf.org/html/rfc4493)
 *  Copyright (C) 2015, T.Kurosawa
 */
#include <string.h>

#include "aes_cmac.h"
#include "aes128.h"

static uint mac[4]; //aes_datalen_i];
static uint  K1[4]; //aes_datalen_i];
static uint  K2[4]; //aes_datalen_i];

static const uint const_Zero[] =
  {0x00000000, 0x00000000, 0x00000000, 0x00000000 };

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

/// operators
static void ShiftCopy(uint *to, uint *from)
{
  int i, carry = 0;
  for (i = aes_datalen_i-1; i>= 0; i--) {
    to[i] = from[i] << 1 | carry;
    carry = (from[i] & (1<<31))? 1: 0;
  }
}

static void Xor(uint *a, const uint *b)
{
  int i;
  for (i = 0; i< aes_datalen_i; i++) {
    a[i] ^= b[i];
  }
}

static int Ceil(int a, int b)
{
  return (a/b)+(((a % b)!=0)?1:0);
}

static void padding(uint *P, int len)
{
  int i;
  uint8 *p = (uint8 *)P;

  len %= aes_datalen;
  p[len] = 0x80;
  for (i = len+1; i< aes_datalen; i++) {
    p[i] = 0x00;
  }
}


/// sub function
void Generate_Subkey(uint8 *key)
{
  uint L[aes_datalen_i];

  memcpy(L, const_Zero, sizeof(L));
  AES128_crypt(L, key);

  ShiftCopy(K1, L);
  if ((L[0] & (1<<31)) != 0) Xor(K1, (uint *)const_Rb);

  ShiftCopy(K2, K1);
  if ((K1[0] & (1<<31)) != 0) Xor(K2, (uint *)const_Rb);
}

/// exported function
uint *AES_cmac(uint8 *K, uint *M, int len)
{
  uint M_last[aes_datalen_i];
  int n = Ceil(len, aes_datalen);
  int i, flag;

  Generate_Subkey(K);

  if (len == 0)
    flag = 0;
  else 
    flag = ((len % aes_datalen) == 0)? 1: 0;

  if (flag) {
    memcpy(M_last, M+(n-1)*aes_datalen_i, aes_datalen);
    Xor(M_last, K1);
  } else {
    memcpy(M_last, M+(n-1)*aes_datalen_i, (len % aes_datalen));
    padding(M_last, len);
    Xor(M_last, K2);
  }
  memcpy(mac, const_Zero, sizeof(mac));
  for (i = 0; i< n-1; i++) {
    Xor(mac, M+i*4);
    AES128_crypt(mac, K);
  }
  Xor(mac, M_last);
  AES128_crypt(mac, K);
  return mac;
}

// O.M.A.J.I.N.A.I.
// vi:expandtab:foldmethod=syntax sw=2 ts=2