/*
MIT License

Copyright (c) 2016 Andrew Carter

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.

Modifications copyright 2019 Mark Qvist

*/

#pragma GCC diagnostic push 
#pragma GCC diagnostic ignored "-Wparentheses"

#include <stdint.h>
#include <string.h>
#include <avr/pgmspace.h>

#include "AES.h"

// Preprocessor Definitions
#define MAP(i,j) (((j) << 2) + (i))
#define MUL(x,y) aes_GaloisFieldMultiply((x),(y))
#define MUL2(x)  (((x) << 1) ^ (0x1B & (((x) >> 7) * 0xFF))) & 0xFF
#define MUL3(x)  ((x) ^ MUL2(x))
#define SUB4(x)  (((uint32_t)pgm_read_byte(&(s_box[((x) & 0xFF000000) >> 24])) << 24) | ((uint32_t)pgm_read_byte(&(s_box[((x) & 0xFF0000) >> 16])) << 16) | (pgm_read_byte(&(s_box[((x) & 0xFF00) >> 8])) << 8) | pgm_read_byte(&(s_box[((x) & 0xFF)])))

// AES Tables
const uint8_t s_box[256] PROGMEM = {
  0x63, 0x7C, 0x77, 0x7B, 0xF2, 0x6B, 0x6F, 0xC5, 0x30, 0x01, 0x67, 0x2B, 0xFE, 0xD7, 0xAB, 0x76,
  0xCA, 0x82, 0xC9, 0x7D, 0xFA, 0x59, 0x47, 0xF0, 0xAD, 0xD4, 0xA2, 0xAF, 0x9C, 0xA4, 0x72, 0xC0,
  0xB7, 0xFD, 0x93, 0x26, 0x36, 0x3F, 0xF7, 0xCC, 0x34, 0xA5, 0xE5, 0xF1, 0x71, 0xD8, 0x31, 0x15,
  0x04, 0xC7, 0x23, 0xC3, 0x18, 0x96, 0x05, 0x9A, 0x07, 0x12, 0x80, 0xE2, 0xEB, 0x27, 0xB2, 0x75,
  0x09, 0x83, 0x2C, 0x1A, 0x1B, 0x6E, 0x5A, 0xA0, 0x52, 0x3B, 0xD6, 0xB3, 0x29, 0xE3, 0x2F, 0x84,
  0x53, 0xD1, 0x00, 0xED, 0x20, 0xFC, 0xB1, 0x5B, 0x6A, 0xCB, 0xBE, 0x39, 0x4A, 0x4C, 0x58, 0xCF,
  0xD0, 0xEF, 0xAA, 0xFB, 0x43, 0x4D, 0x33, 0x85, 0x45, 0xF9, 0x02, 0x7F, 0x50, 0x3C, 0x9F, 0xA8,
  0x51, 0xA3, 0x40, 0x8F, 0x92, 0x9D, 0x38, 0xF5, 0xBC, 0xB6, 0xDA, 0x21, 0x10, 0xFF, 0xF3, 0xD2,
  0xCD, 0x0C, 0x13, 0xEC, 0x5F, 0x97, 0x44, 0x17, 0xC4, 0xA7, 0x7E, 0x3D, 0x64, 0x5D, 0x19, 0x73,
  0x60, 0x81, 0x4F, 0xDC, 0x22, 0x2A, 0x90, 0x88, 0x46, 0xEE, 0xB8, 0x14, 0xDE, 0x5E, 0x0B, 0xDB,
  0xE0, 0x32, 0x3A, 0x0A, 0x49, 0x06, 0x24, 0x5C, 0xC2, 0xD3, 0xAC, 0x62, 0x91, 0x95, 0xE4, 0x79,
  0xE7, 0xC8, 0x37, 0x6D, 0x8D, 0xD5, 0x4E, 0xA9, 0x6C, 0x56, 0xF4, 0xEA, 0x65, 0x7A, 0xAE, 0x08,
  0xBA, 0x78, 0x25, 0x2E, 0x1C, 0xA6, 0xB4, 0xC6, 0xE8, 0xDD, 0x74, 0x1F, 0x4B, 0xBD, 0x8B, 0x8A,
  0x70, 0x3E, 0xB5, 0x66, 0x48, 0x03, 0xF6, 0x0E, 0x61, 0x35, 0x57, 0xB9, 0x86, 0xC1, 0x1D, 0x9E,
  0xE1, 0xF8, 0x98, 0x11, 0x69, 0xD9, 0x8E, 0x94, 0x9B, 0x1E, 0x87, 0xE9, 0xCE, 0x55, 0x28, 0xDF,
  0x8C, 0xA1, 0x89, 0x0D, 0xBF, 0xE6, 0x42, 0x68, 0x41, 0x99, 0x2D, 0x0F, 0xB0, 0x54, 0xBB, 0x16
};

const uint8_t s_box_inverse[256] PROGMEM = {
  0x52, 0x09, 0x6A, 0xD5, 0x30, 0x36, 0xA5, 0x38, 0xBF, 0x40, 0xA3, 0x9E, 0x81, 0xF3, 0xD7, 0xFB,
  0x7C, 0xE3, 0x39, 0x82, 0x9B, 0x2F, 0xFF, 0x87, 0x34, 0x8E, 0x43, 0x44, 0xC4, 0xDE, 0xE9, 0xCB,
  0x54, 0x7B, 0x94, 0x32, 0xA6, 0xC2, 0x23, 0x3D, 0xEE, 0x4C, 0x95, 0x0B, 0x42, 0xFA, 0xC3, 0x4E,
  0x08, 0x2E, 0xA1, 0x66, 0x28, 0xD9, 0x24, 0xB2, 0x76, 0x5B, 0xA2, 0x49, 0x6D, 0x8B, 0xD1, 0x25,
  0x72, 0xF8, 0xF6, 0x64, 0x86, 0x68, 0x98, 0x16, 0xD4, 0xA4, 0x5C, 0xCC, 0x5D, 0x65, 0xB6, 0x92,
  0x6C, 0x70, 0x48, 0x50, 0xFD, 0xED, 0xB9, 0xDA, 0x5E, 0x15, 0x46, 0x57, 0xA7, 0x8D, 0x9D, 0x84,
  0x90, 0xD8, 0xAB, 0x00, 0x8C, 0xBC, 0xD3, 0x0A, 0xF7, 0xE4, 0x58, 0x05, 0xB8, 0xB3, 0x45, 0x06,
  0xD0, 0x2C, 0x1E, 0x8F, 0xCA, 0x3F, 0x0F, 0x02, 0xC1, 0xAF, 0xBD, 0x03, 0x01, 0x13, 0x8A, 0x6B,
  0x3A, 0x91, 0x11, 0x41, 0x4F, 0x67, 0xDC, 0xEA, 0x97, 0xF2, 0xCF, 0xCE, 0xF0, 0xB4, 0xE6, 0x73,
  0x96, 0xAC, 0x74, 0x22, 0xE7, 0xAD, 0x35, 0x85, 0xE2, 0xF9, 0x37, 0xE8, 0x1C, 0x75, 0xDF, 0x6E,
  0x47, 0xF1, 0x1A, 0x71, 0x1D, 0x29, 0xC5, 0x89, 0x6F, 0xB7, 0x62, 0x0E, 0xAA, 0x18, 0xBE, 0x1B,
  0xFC, 0x56, 0x3E, 0x4B, 0xC6, 0xD2, 0x79, 0x20, 0x9A, 0xDB, 0xC0, 0xFE, 0x78, 0xCD, 0x5A, 0xF4,
  0x1F, 0xDD, 0xA8, 0x33, 0x88, 0x07, 0xC7, 0x31, 0xB1, 0x12, 0x10, 0x59, 0x27, 0x80, 0xEC, 0x5F,
  0x60, 0x51, 0x7F, 0xA9, 0x19, 0xB5, 0x4A, 0x0D, 0x2D, 0xE5, 0x7A, 0x9F, 0x93, 0xC9, 0x9C, 0xEF,
  0xA0, 0xE0, 0x3B, 0x4D, 0xAE, 0x2A, 0xF5, 0xB0, 0xC8, 0xEB, 0xBB, 0x3C, 0x83, 0x53, 0x99, 0x61,
  0x17, 0x2B, 0x04, 0x7E, 0xBA, 0x77, 0xD6, 0x26, 0xE1, 0x69, 0x14, 0x63, 0x55, 0x21, 0x0C, 0x7D
};

const uint8_t rcon[10] = { 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80, 0x1B, 0x36 };

// AES Operation Definitions

void aes_Rotate        (uint8_t block[4]);
void aes_SubBytes      (uint8_t state[16]);
void aes_InvSubBytes   (uint8_t state[16]);
void aes_ShiftRows     (uint8_t state[16]);
void aes_InvShiftRows  (uint8_t state[16]);
void aes_MixColumns    (uint8_t state[16]);
void aes_InvMixColumns (uint8_t state[16]);
void aes_AddRoundKey   (uint8_t state[16], uint8_t sub_key[16]);

// Maths Operation Definitions

uint8_t aes_GaloisFieldMultiply (uint8_t fixed, uint8_t variable);

// AES Core Procedures

void aes_init(uint32_t *round_key, counter b, uint32_t *key, counter n)
{
  uint32_t t;
  counter i, position, cycle;

  memcpy(round_key, key, n * sizeof(uint32_t));

  for (position = n, cycle = n, t = round_key[n - 1], i = 0; position < b; position++, cycle++) {
    if (cycle == n) {
      cycle = 0;

      aes_Rotate((uint8_t *) &t);
      t = SUB4(t);
      ((uint8_t *) &t)[0] ^= rcon[i++];
    } else if (cycle == 4 && n == 8) {
      t = SUB4(t);
    }

    t ^= round_key[position - n];
    round_key[position] = t;
  }
}

void aes_encrypt(uint8_t *round_key, uint8_t block[16], counter rounds)
{
  counter i;

  aes_AddRoundKey(block, round_key);

  for (i = 1; i < rounds; i++) {
    aes_SubBytes(block);
    aes_ShiftRows(block);
    aes_MixColumns(block);
    aes_AddRoundKey(block, round_key + (i << 4));
  }

  aes_SubBytes(block);
  aes_ShiftRows(block);
  aes_AddRoundKey(block, round_key + (i << 4));
}

void aes_decrypt(uint8_t *round_key, uint8_t block[16], counter rounds)
{
  counter i;

  aes_AddRoundKey(block, round_key + (rounds << 4));

  for (i = 1; i < rounds; i++) {
    aes_InvShiftRows(block);
    aes_InvSubBytes(block);
    aes_AddRoundKey(block, round_key + ((rounds - i) << 4));
    aes_InvMixColumns(block);
  }

  aes_InvShiftRows(block);
  aes_InvSubBytes(block);
  aes_AddRoundKey(block, round_key);
}

// AES Key Specific API

void aes_256_init(aes_256_context_t *context, uint8_t key[32])
{
  aes_init((uint32_t *) context->round_key, sizeof(context->round_key) / sizeof(uint32_t), (uint32_t *) key, 8);
}

void aes_256_encrypt(aes_256_context_t *context, uint8_t block[16])
{
  aes_encrypt(context->round_key, block, AES_256_ROUNDS);
}

void aes_256_decrypt(aes_256_context_t *context, uint8_t block[16])
{
  aes_decrypt(context->round_key, block, AES_256_ROUNDS);
}

void aes_192_init(aes_192_context_t *context, uint8_t key[24])
{
  aes_init((uint32_t *) context->round_key, sizeof(context->round_key) / sizeof(uint32_t), (uint32_t *) key, 6);
}

void aes_192_encrypt(aes_192_context_t *context, uint8_t block[16])
{
  aes_encrypt(context->round_key, block, AES_192_ROUNDS);
}

void aes_192_decrypt(aes_192_context_t *context, uint8_t block[16])
{
  aes_decrypt(context->round_key, block, AES_192_ROUNDS);
}

void aes_128_init(aes_128_context_t *context, uint8_t key[16])
{
  aes_init((uint32_t *) context->round_key, sizeof(context->round_key) / sizeof(uint32_t), (uint32_t *) key, 4);
}

void aes_128_encrypt(aes_128_context_t *context, uint8_t block[16])
{
  aes_encrypt(context->round_key, block, AES_128_ROUNDS);
}

void aes_128_decrypt(aes_128_context_t *context, uint8_t block[16])
{
  aes_decrypt(context->round_key, block, AES_128_ROUNDS);
}

// AES Operation Implementations

void aes_Rotate(uint8_t block[4])
{
  uint8_t tmp;

  tmp = block[0];
  block[0] = block[1];
  block[1] = block[2];
  block[2] = block[3];
  block[3] = tmp;
}

void aes_SubBytes(uint8_t state[16])
{
  counter i = 16;

  while (i--) {
    state[i] = pgm_read_byte(&(s_box[state[i]]));
  }
}

void aes_InvSubBytes(uint8_t state[16])
{
  counter i = 16;

  while (i--) {
    state[i] = pgm_read_byte(&(s_box_inverse[state[i]]));
  }
}

void aes_ShiftRows(uint8_t state[16])
{
  uint8_t tmp;

  tmp = state[MAP(1,0)];
  state[MAP(1,0)] = state[MAP(1,1)];
  state[MAP(1,1)] = state[MAP(1,2)];
  state[MAP(1,2)] = state[MAP(1,3)];
  state[MAP(1,3)] = tmp;

  tmp = state[MAP(2,0)];
  state[MAP(2,0)] = state[MAP(2,2)];
  state[MAP(2,2)] = tmp;

  tmp = state[MAP(2,1)];
  state[MAP(2,1)] = state[MAP(2,3)];
  state[MAP(2,3)] = tmp;

  tmp = state[MAP(3,0)];
  state[MAP(3,0)] = state[MAP(3,3)];
  state[MAP(3,3)] = state[MAP(3,2)];
  state[MAP(3,2)] = state[MAP(3,1)];
  state[MAP(3,1)] = tmp;
}

void aes_InvShiftRows(uint8_t state[16])
{
  uint8_t tmp;

  tmp = state[MAP(3,0)];
  state[MAP(3,0)] = state[MAP(3,1)];
  state[MAP(3,1)] = state[MAP(3,2)];
  state[MAP(3,2)] = state[MAP(3,3)];
  state[MAP(3,3)] = tmp;

  tmp = state[MAP(2,0)];
  state[MAP(2,0)] = state[MAP(2,2)];
  state[MAP(2,2)] = tmp;

  tmp = state[MAP(2,1)];
  state[MAP(2,1)] = state[MAP(2,3)];
  state[MAP(2,3)] = tmp;

  tmp = state[MAP(1,0)];
  state[MAP(1,0)] = state[MAP(1,3)];
  state[MAP(1,3)] = state[MAP(1,2)];
  state[MAP(1,2)] = state[MAP(1,1)];
  state[MAP(1,1)] = tmp;
}

void aes_MixColumns(uint8_t state[16])
{
  counter i = 4;
  uint8_t new_state[4];

  while (i--) {
    new_state[0] = MUL2(state[MAP(0,i)]) ^ MUL3(state[MAP(1,i)]) ^      state[MAP(2,i)]  ^      state[MAP(3,i)] ;
    new_state[1] =      state[MAP(0,i)]  ^ MUL2(state[MAP(1,i)]) ^ MUL3(state[MAP(2,i)]) ^      state[MAP(3,i)] ;
    new_state[2] =      state[MAP(0,i)]  ^      state[MAP(1,i)]  ^ MUL2(state[MAP(2,i)]) ^ MUL3(state[MAP(3,i)]);
    new_state[3] = MUL3(state[MAP(0,i)]) ^      state[MAP(1,i)]  ^      state[MAP(2,i)]  ^ MUL2(state[MAP(3,i)]);

    state[MAP(0,i)] = new_state[0];
    state[MAP(1,i)] = new_state[1];
    state[MAP(2,i)] = new_state[2];
    state[MAP(3,i)] = new_state[3];
  }
}

void aes_InvMixColumns(uint8_t state[16])
{
  counter i = 4;
  uint8_t new_state[4];

  while (i--) {
    new_state[0] = MUL(14, state[MAP(0,i)]) ^ MUL(11, state[MAP(1,i)]) ^ MUL(13, state[MAP(2,i)]) ^ MUL( 9, state[MAP(3,i)]);
    new_state[1] = MUL( 9, state[MAP(0,i)]) ^ MUL(14, state[MAP(1,i)]) ^ MUL(11, state[MAP(2,i)]) ^ MUL(13, state[MAP(3,i)]);
    new_state[2] = MUL(13, state[MAP(0,i)]) ^ MUL( 9, state[MAP(1,i)]) ^ MUL(14, state[MAP(2,i)]) ^ MUL(11, state[MAP(3,i)]);
    new_state[3] = MUL(11, state[MAP(0,i)]) ^ MUL(13, state[MAP(1,i)]) ^ MUL( 9, state[MAP(2,i)]) ^ MUL(14, state[MAP(3,i)]);

    state[MAP(0,i)] = new_state[0];
    state[MAP(1,i)] = new_state[1];
    state[MAP(2,i)] = new_state[2];
    state[MAP(3,i)] = new_state[3];
  }
}

void aes_AddRoundKey(uint8_t state[16], uint8_t sub_key[16])
{
#ifdef OPTIMISE_8_BIT

  counter i = 16;

  while (i--) {
    state[i] ^= sub_key[i];
  }

#else

  counter i = 4;

  while (i--) {
    ((uint32_t *) state)[i] ^= ((uint32_t *) sub_key)[i];
  }

#endif
}

// Maths Operation Implementations

uint8_t aes_GaloisFieldMultiply(uint8_t fixed, uint8_t variable)
{
  uint8_t result = 0;

  while (fixed) {
    result ^= variable & ((fixed & 0x01) * 0xFF);
    variable = MUL2(variable);
    fixed >>= 1;
  }

  return result;
}

#pragma GCC diagnostic pop