/*
 * MagmaEncryptor.cpp
 * 
 * Библиотека для шифрования (с аутентификацией), построенная на основе
 * шифра "Магма" (в версии ГОСТ Р 34.12-2015).
 * 
 * Состав:
 * реализация шифра (криптопримитив);
 * реализация шифрования в режиме счётчика (гаммирования);
 * реализация схемы аутентифиукации сообщений CMAC 
 * (на базе шифра);
 * 
 * Версия: 0.1beta от 11/12/2016
 * Автор: Александр Венедюхин
 * Сайт: https://dxdt.ru/
 * Условия использования: свободное распространение, использование
 * без ограничений.
 * Страница библиотеки: https://dxdt.ru/arduino-crypto/
 * 
 * Пример:
 * 
 * ---
 * 
 * MagmaEncryptor M;
 * uint32_t nonce;
 * uint16_t len;
 * byte MAC[8];
 * byte plain_text[19];
 * byte cipher_text[19];
 * 
 * nonce = 127;
 * len = 19;
 * 
 * M.setKeys(c_key, MAC_key);
 * 
 * M.lock(&nonce, plain_text, cipher_text, len, MAC);
 * 
 * ---
 *  
 * Логика работы достаточно подробно описана ниже, в комментариях внутри 
 * исходного кода.
 *  
 * "Магма" (известен также как ГОСТ 28147-89) - блочный симметричный шифр,
 * с разрядностью блока 64 бита и разрядностью ключа - 256 бит.
 * 
 * С целью повышения быстродействия и получения более компактного кода
 * криптопримитив шифра для микроконтроллера "Ардуино" целиком реализован на 
 * inline-ассемблере - функция DoCipher().
 * 
 * Библиотека обеспечивает шифрование/расшифрование сообщений с аутентификацией -
 * соответственно, функции (методы) lock() и unlock().
 * 
 * Для работы необходимо загрузить действующие ключи. _Обязательно_ 
 * использование двух различных ключей: для шифрования и аутентификации.
 * 
 * Для аутентификации применяется алгоритм CMAC (NIST SP 800-38B). В состав 
 * сообщения, для  вычисления кода аутентификации (имитовставки) включается 
 * значение nonce. Это же значение nonce должно использоваться для генерации 
 * ключевого потока. Nonce с данным ключом шифрования может быть использовано 
 * только один раз. 
 * 
 * Предполагается, что узлы, обменивающиеся защищёнными данными, используют 
 * синхронное значение nonce. Это значение может (но не должно) передаваться 
 * вместе с сообщением, в открытом виде.
 * 
 */

#include "MagmaEncryptor.h"


MagmaEncryptor::MagmaEncryptor()
{

  KeysReady = 0;
  
}

int MagmaEncryptor::setKeys(byte *cipher_key, byte *MAC_key){
/*
 * Генерирует раундовые ключи из входных ключей, дополнительные ключи для CMAC.
 * Помимо раундовых клюей, необходимых для шифра, CMAC использует два дополнительных ключа
 * K1 и K2. Эти ключи генерируются при помощи шифрования нулевого блока и выполнения над
 * шифротекстом дополнительных преобразований: циклического сдвига и, в зависимости от 
 * значения, XOR с константой.
 * 
 */
byte cblock[8];
byte pblock[8];
byte t;
int i;

  ExpandKey(cipher_key,magma_round_keys);
/*
 * Работа шифра в режиме счётчика требует только операции зашифрования, поэтому 
 * внутри библиотеки шифром и CMAC используются только раундовые ключи в прямом порядке.
 */

/*
 * Далее: вычисление набора ключей для CMAC.
 * 
 */

  ExpandKey(MAC_key,magma_MAC_round_keys);  // Раундовые ключи для шифра в режиме CMAC.
  
  for(i = 0; i < 8; i++){
    pblock[i] = 0;                          // Заполняем блок нулевыми значениями.
  }

/*
 * Для получения дополнительных ключей CMAC необходимо зашифровать нулевой блок
 * (блок, соответствующий разрядности шифра 64 бита, заполненный нулевыми битами)
 * с использованием ключей MAC. Полученный шифротекст служит исходными данными для
 * ключей K1 и K2.
 * 
 */

  DoCipher(magma_MAC_round_keys, pblock, cblock, pi_ptr);

/*
 * Результат зашифрования находится в cblock.
 */

  t = cblock[0] & 0B10000000;

/*
 * В зависимости от значения старшего бита выполняется XOR с дополнительной константой,
 * позволяющий получить различные ключи.
 */

asm volatile(
/*
 * В целях оптимизации 64-битной операции, битовый сдвиг реализован на ассемблере.
 */

    "push r26\n"         // Сохраняем значение указателя.
    "push r27\n"
    "ld   r10,X+\n"      // Загружаем байты блока в регистры.
    "ld   r11,X+\n"      // Регистры r10-17 - 8х8=64 бита.
    "ld   r12,X+\n"
    "ld   r13,X+\n"
    "ld   r14,X+\n"
    "ld   r15,X+\n"      
    "ld   r16,X+\n"      
    "ld   r17,X+\n"
    
    "lsl  r17\n"         // Логический сдвиг влево на один бит.
    "rol  r16\n"         // Циклический сдвиг, использующий флаг переноса.
    "rol  r15\n"
    "rol  r14\n"
    "rol  r13\n"
    "rol  r12\n"
    "rol  r11\n"
    "bst  r10,7\n"       // Старший бит регистра r10 копируется.
    "rol  r10\n"
    "bld  r17,0\n"       // Переносим старший бит из r10.

    "pop  r27\n"         // Восстанавливаем значение указателя.
    "pop  r26\n"
    "st   X+,r10\n"      // Выгружаем регистры в блок памяти.
    "st   X+,r11\n"
    "st   X+,r12\n"
    "st   X+,r13\n"
    "st   X+,r14\n"
    "st   X+,r15\n"
    "st   X+,r16\n"
    "st   X+,r17\n"
    
      : : "x" (cblock) : "r10", "r11", "r12", "r13", "r14", "r15", "r16", "r17", "memory"
);


  if(t){ // Если старший бит блока (до сдвига) был равен единице, выполняется дополнительная операция XOR.
    
         // XOR с константой 0(59)11011 (64-битное значение, первые 59 битов - равны нулю).
         
    cblock[7] = cblock[7] ^ 0B00011011;  
    
  }

/*
 * Мы получили в cblock значение первого дополнительного ключа CMAC - K1.
 */
  for( i = 0; i < 8; i++){
    magma_MAC_K1[i] = cblock[i];   // Запись значения ключа.
  }

  t = cblock[0] & 0B10000000;      // Сохранение значения старшего бита.
   
asm volatile(                      // Циклический сдвиг.

    "push r26\n"
    "push r27\n"
    "ld   r10,X+\n"      
    "ld   r11,X+\n"      
    "ld   r12,X+\n"
    "ld   r13,X+\n"
    "ld   r14,X+\n"
    "ld   r15,X+\n"      
    "ld   r16,X+\n"      
    "ld   r17,X+\n"
    
    "lsl  r17\n"         
    "rol  r16\n"         
    "rol  r15\n"
    "rol  r14\n"
    "rol  r13\n"
    "rol  r12\n"
    "rol  r11\n"
    "bst  r10,7\n"       
    "rol  r10\n"
    "bld  r17,0\n"       

    "pop  r27\n"
    "pop  r26\n"
    "st   X+,r10\n"      
    "st   X+,r11\n"
    "st   X+,r12\n"
    "st   X+,r13\n"
    "st   X+,r14\n"
    "st   X+,r15\n"
    "st   X+,r16\n"
    "st   X+,r17\n"
    
      : : "x" (cblock) : "r10", "r11", "r12", "r13", "r14", "r15", "r16", "r17", "memory"
);

  if(t){

    cblock[7] = cblock[7] ^ 0B00011011; 
    }

/*
 * Полученное значение сохраняется в качестве второго дополнительного ключа K2. 
 */

  for( i = 0; i < 8; i++){
    magma_MAC_K2[i] = cblock[i];
  }

  KeysReady = 1;      // Признак наличия ключей.
  
  return 1;
}

int MagmaEncryptor::KDF(byte *source_key, byte *source_MAC_key, byte *base, int epoch){
/*
 * Функция получения набора ключей на основе действующих ключей и "базового" блока.
 * 
 * Параметры:
 * source_key - указатель на исходный ключ (шифрования);
 * source_MAC_key - указатель на исходный ключ MAC;
 * base - указатель на базовый блок, длиной 256 бит;
 * epoch - поколение ключей: значение, задающее число повторов процедуры генерации.
 * 
 * KDF() требуется для получения нового набора ключей после того, как имеющиеся ключи устарели -
 * то есть, были использованы для шифрования максимально допустимого числа блоков. Значение epoch
 * позволяет получать очередной набор ключей после окончания действия предыдущего.
 * 
 * Новые ключи генерируются путём расшифрования базового блока (base) в режиме простой замены
 * действующими ключами.
 * 
 */

byte temp_round_keys[128];
byte temp_key[32];
int i,k;

  if(epoch < 1){

    return 0;
  
  }

  ExpandKeyDecrypt(source_key,temp_round_keys);

    
  for(i = 0; i < epoch; i++){

    for(k = 0; k < 4; k++){
      DoCipher(temp_round_keys,&base[k*8],&temp_key[k*8],pi_ptr);
    }

    ExpandKeyDecrypt(temp_key,temp_round_keys);
  }

  ExpandKey(temp_key,magma_round_keys);

  ExpandKeyDecrypt(source_MAC_key,temp_round_keys);
  
  for(i = 0; i < epoch; i++){

    for(k = 0; k < 4; k++){
      DoCipher(temp_round_keys,&base[k*8],&temp_key[k*8],pi_ptr);
      }

    ExpandKeyDecrypt(temp_key,temp_round_keys);
  }

  ExpandKey(temp_key,magma_MAC_round_keys);

  KeysReady = 1;

  return 1;
}


int MagmaEncryptor::lock(uint32_t *nonce, byte *plain_text, byte *cipher_text, uint16_t len, byte *MAC){
/*
 * Функция шифрования, также вычисляющая код аутентификации.
 * Данная функция служит для получения защищённого блока данных
 * (длины len).
 */
 uint32_t i_nonce;

if(!KeysReady){
    return 0;          // Для работы требуется загрузка ключей.
  }

 i_nonce = *nonce;
   
/*
 * Функция состоит из вызова функции шифрования в режиме счётчика, с последующим 
 * вычислением MAC (кода аутентификации) для полученного шифротекста и исходного 
 * значения nonce.
 */
 
  MagmaEncrypt(nonce,plain_text,cipher_text,len);
  MagmaMAC(i_nonce,cipher_text,len,MAC);
  
  return 1;
}


int MagmaEncryptor::unlock(uint32_t *nonce, byte *plain_text, byte *cipher_text, uint16_t len, byte *MAC){
/*
 * Обратная к lock() функция, предназначенная для восстановления открытого текста
 * из защищённого блока данных.
 * Данная функция проверяет значение MAC.
 */

byte MAC_result[8];
byte cmp_flag;
int i;

if(!KeysReady){
    return 0;          // Для работы требуется загрузка ключей.
  }

  MagmaMAC(*nonce,cipher_text,len,MAC_result);
  MagmaDecrypt(nonce,plain_text,cipher_text,len);
/*
 * Расшифрование производится в любом случае, это минимальное решение, позволяющее
 * обеспечить одинаковое время работы при верном и неверном значении MAC.
 */

  cmp_flag = 0;

/*
 * Сравнение переданного MAC и вычисленного.
 */

  for( i = 0; i < 8; i++){
    cmp_flag |= MAC[i] ^ MAC_result[i];
  }

/*
 * В случае, если значения MAC совпали, возвращается 1. Во всех остальных случаях - 0.
 */
  return (int)!cmp_flag;
}

MagmaEncryptor::~MagmaEncryptor()
{
  // Пусто.
}

int MagmaEncryptor::DoCipher(byte *r_keys, byte *plain_text, byte *cipher_text, const byte *s_boxes)
/*
 * Функция, реализующая шифр. В данной версии - полностью на ассемблере AVR.
 * 
 * Оперции зашифрования и расшифрования отличаются только порядком применения 
 * ключей раундов. В данной библиотеке в режиме расшифрования шифр используется
 * только для получения новых ключей.
 * 
 * Параметры:
 * r_keys - раундовые ключи;
 * plain_text, cipher_text - открытый текст, шифротекст;
 * s_boxes - таблица подстановок.
 * 
 *  Логика реализации шифра: блок открытого текста загружается в регистры, где над ним 
 *  проводятся преобразования шифра. Получившийся блок шифротекста выгружается
 *  в область памяти, предназначенную для шифротекста.
 *  
 *  Cхема представления блока (разбивается на две равные части, согласно алгоритму):
 *  
 *  r10, r11, r12, r13 = a1 \
 *                           } PT - блок открытого текста.
 *  r16, r17, r18, r19 = a0 / 
 */
{

asm volatile(

    "eor  r23,r23\n"       // r23 - значение 0, для использования в качестве операнда
                           // при суммировании с флагом переноса.

/*
 * Регистры-указатели X,Y,Z - содержат, соответственно, адреса начала блока открытого 
 * текста, начала массива раундовых ключей, начала массива подстановок. Их значения 
 * задаются аргументами конструкции asm (см. ниже).
 */
    
    "ld   r10,X+\n"      // Загружаем байты открытого текста в регистры.
    "ld   r11,X+\n"      // Регистры r10-13 - "старшая" часть, a1 в стандарте.
    "ld   r12,X+\n"
    "ld   r13,X+\n"
    
    "ld   r16,X+\n"      // Регистры r16-19 - "младшая" часть, a0.
    "ld   r17,X+\n"      
    "ld   r18,X+\n"      
    "ld   r19,X+\n"
/*
 * Мы разделили исходный блок открытого текста на две части. Каждая по 32 бита, то есть 4 байта
 * или четыре регистра.
 */

/*
 * Шифр состоит из 32 раундов, но в последнем раунде не производится перестановка 
 * частей блока.
 * 
 * Каждый раунд получает на вход соответствующий 32-битный ключ раунда.
 * Раунд состоит из сложения с ключом, подстановок, циклического сдвига и суммирования
 * по модулую 2 (XOR) частей a1,a0. Завершается раунд (кроме последнего) перестановкой 
 * частей блока a1<->a0.
 */

    "ldi  r21,32\n"      // 32 раунда.
    "r:\n"               // Метка начала цикла.
    "mov  r2,r16\n"      // Сохранение текущего значения a0.
    "mov  r3,r17\n"
    "mov  r4,r18\n"
    "mov  r5,r19\n"

    "ld   r6,Y+\n"       // Загружаем ключ действующего раунда в регистры r6-9.
    "ld   r7,Y+\n"       // Раундовые ключи имеют разрядность 32 бита,
    "ld   r8,Y+\n"       // поэтому для хранения одного ключа нужно 4 регистра.
    "ld   r9,Y+\n"
    
    "add  r19,r9\n"      // Вычисляем сумму (mod 2^32) раундового ключа и a0.
    "adc  r18,r8\n"
    "adc  r17,r7\n"
    "adc  r16,r6\n"

/*
 * Нелинейное преобразование. Подстановки. "Магма" использует подстановки полубайтов (4-бита). 
 * То есть, каждое 4-байтовое значение преобразуемого блока заменяется на соответствующее 
 * значение из таблицы подстановок. В данной реализации, подстановки объединены в байты, поэтому
 * для загрузки нужного значения требуются дополнительные операции.
 * 
 * Регистр r20 используется для хранения промежуточных результатов.
 */
 
    "mov  r20,r19\n"    // Загружаем байт преобразуемого блока в r20.
    "andi r20,0xF0\n"   // Маскируем старший полубайт.
    "swap r20\n"        // Переставляем в младший полубайт, для правильной индексации ниже.
    "push r31\n"        // Значения указателя Z должны быть сохранены, так как очередной
    "push r30\n"        // индекс всегда вычисляется от начала массива.
    "add  r30,r20\n"    // Смещение для указателя - вычисляем индекс.
    "adc  r31,r23\n"    // Добавление флага переноса в старший байт 16-битного адреса (r23 = 0).

// Указатель Z теперь показывает на байт с подстановками для старшего полубайта из r20.

    "ld   r20,Z\n"      // Загружаем подстановки (загружается байт).
    "pop  r30\n"        // Восстанавливаем значение указателя Z.
    "pop  r31\n"
    "andi r20,0xF0\n"   // Выбираем и применяем только старший полубайт подстановки.
    "andi r19,0x0F\n"
    "or   r19,r20\n"    // Старшая часть r19 заменена на значение подстановки.

    "mov  r20,r19\n"    // Дальнейшие операции - аналогичны.
    "andi r20,0x0F\n"   // Младший полубайт.
    "push r31\n"
    "push r30\n"
    "add  r30,r20\n"    // Смещение для указателя.
    "adc  r31,r23\n"    // Добавление флага переноса (r23 = 0).

    "ld   r20,Z\n"      // Загружаем подстановки.
    "pop  r30\n"
    "pop  r31\n"
    "andi r20,0x0F\n"   // Выбираем и загружаем только младший полубайт подстановки.
    "andi r19,0xF0\n"
    "or   r19,r20\n"
/*
 * Аналогичные операции с всеми оставшимися регистрами a0.
 */
// r18
    "mov  r20,r18\n"
    "andi r20,0xF0\n"
    "swap r20\n"
    "push r31\n"
    "push r30\n"
    "add  r30,r20\n"    
    "adc  r31,r23\n"
    "adiw r30,16\n"     // Сдвиг на очередной блок подстановок в таблице подстановок.
                            
    "ld   r20,Z\n"      
    "pop  r30\n"
    "pop  r31\n"
    "andi r20,0xF0\n"   
    "andi r18,0x0F\n"
    "or   r18,r20\n"

    "mov  r20,r18\n"    
    "andi r20,0x0F\n"
    "push r31\n"
    "push r30\n"
    "add  r30,r20\n" 
    "adc  r31,r23\n" 
    "adiw r30,16\n"

    "ld   r20,Z\n"   
    "pop r30\n"
    "pop r31\n"
    "andi r20,0x0f\n"  
    "andi r18,0xf0\n"
    "or   r18,r20\n"
// r17
    "mov  r20,r17\n"
    "andi r20,0xF0\n"
    "swap r20\n"
    "push r31\n"
    "push r30\n"
    "add  r30,r20\n"    
    "adc  r31,r23\n"
    "adiw r30,32\n"    
                      
    "ld   r20,Z\n"      
    "pop  r30\n"
    "pop  r31\n"
    "andi r20,0xf0\n"   
    "andi r17,0x0f\n"
    "or   r17,r20\n"

    "mov  r20,r17\n"    
    "andi r20,0x0F\n"
    "push r31\n"
    "push r30\n"
    "add  r30,r20\n" 
    "adc  r31,r23\n" 
    "adiw r30,32\n"

    "ld   r20,Z\n"   
    "pop  r30\n"
    "pop  r31\n"
    "andi r20,0x0f\n"  
    "andi r17,0xf0\n"
    "or   r17,r20\n"
// r16
    "mov  r20,r16\n"
    "andi r20,0xF0\n"
    "swap r20\n"
    "push r31\n"
    "push r30\n"
    "add  r30,r20\n"    
    "adc  r31,r23\n"    
    "adiw r30,48\n"
                    
    "ld   r20,Z\n"      
    "pop  r30\n"
    "pop  r31\n"
    "andi r20,0xf0\n"   
    "andi r16,0x0f\n"
    "or   r16,r20\n"

    "mov  r20,r16\n"    
    "andi r20,0x0F\n"
    "push r31\n"
    "push r30\n"
    "add  r30,r20\n" 
    "adc  r31,r23\n" 
    "adiw r30,48\n"

    "ld   r20,Z\n"   
    "pop  r30\n"
    "pop  r31\n"
    "andi r20,0x0f\n"  
    "andi r16,0xf0\n"
    "or   r16,r20\n"
/*
 * Подстановки завершены.
 */
/*
 * Циклический сдвиг влево, на 11 разрядов.
 */
    "ldi  r23,11\n"      // Используем цикл, с регистром r23.
    "l:\n"               // Метка цикла.
    "lsl  r19\n"         // Логический сдвиг влево на один бит.
    "rol  r18\n"         // Циклический сдвиг, использующий флаг переноса.
    "rol  r17\n"
    "bst  r16,7\n"       // Старший бит регистра r16 копируется.
    "rol  r16\n"
    "bld  r19,0\n"       // Переносим старший бит из r16.
    "dec  r23\n"         // Подсчёт итераций.
    "brne l\n"           // Повтор.

    "eor  r16,r10\n"     // XOR двух частей блока - a1,a0.
    "eor  r17,r11\n"
    "eor  r18,r12\n"
    "eor  r19,r13\n"
    
/*    
 * Мы получили в r16-19 промежуточный результат раунда для a1. 
 * Остаётся произвести обмен двух частей блока. Этой операции эквивалентно
 * восстановление ранее сохранённой исходной части a0 в регистры, 
 * выделенные для a1.
 * 
 */
    "dec  r21\n"         // Подсчёт раундов.
    "breq e\n"           // Завершение цикла (выход происходит до 
                         // перестановки a0<->a1).

    "mov  r10,r2\n"      // Загрузка a1.
    "mov  r11,r3\n"
    "mov  r12,r4\n"
    "mov  r13,r5\n"

    "jmp  r\n"           // Cледующий раунд.
    
    "e:\n"               // Метка выхода из основного цикла.
/*
 * Окончание преобразований.
 * Далее происходит выгрузка полученного шифротекста (r10-19) в блок памяти.
 */
    "mov r30,%A0\n"      // Указатель на блок памяти для шифротекста.
    "mov r31,%B0\n"
    
    "st Z+,r16\n"        // Выгружаем регистры в память.
    "st Z+,r17\n"
    "st Z+,r18\n"
    "st Z+,r19\n"

    "st Z+,r2\n"
    "st Z+,r3\n"
    "st Z+,r4\n"
    "st Z+,r5\n"
       
    : : "r" (cipher_text), "x" (plain_text), "y" (r_keys), "z" (s_boxes): "r2","r3","r4","r5","r6","r7","r8","r9","r10","r11","r12","r13","r14","r15","r16","r17","r18","r19","r20","r21","r22","r23","memory"
    
  );

return 1;

}

void MagmaEncryptor::ExpandKey(byte *source_key, byte *round_keys)
/*
 * Функция развертывания ключа (для шифрования).
 * Шифр использует набор раундовых ключей, который строится из основного ключа 
 * шифрования.
 * 
 * Алгоритм развертывания ключа сводится к копированию 32-битных блоков 
 * исходного ключа в прямом порядке раундов, за исключением последних 8 ключей, 
 * которые копируются в обратном порядке.
 * 
 * Схема:
 * 
 * | k_1, k_2, k_3 ... k_8 | k_1 ... k_8 | k_1 ... k_8 | k_8, k_7 ... k_1 |
 * 
 * 
 * Параметры: 
 * source_key - указатель на основной ключ (256 бит - массив из 32 байтов);
 * round_keys - указатель на массив, куда будут помещены ключи раундов.
 * 
 */
{
int i;
byte *p, *d;

p = source_key;
d = round_keys;

for(i = 0; i<8; i++){

     *d = *(p+i*4);
     *(d+1) = *(p+1+i*4);
     *(d+2) = *(p+2+i*4);
     *(d+3) = *(p+3+i*4);

     *(d+8*4) = *(p+i*4);
     *(d+1+8*4) = *(p+1+i*4);
     *(d+2+8*4) = *(p+2+i*4);
     *(d+3+8*4) = *(p+3+i*4);

     *(d+16*4) = *(p+i*4);
     *(d+1+16*4) = *(p+1+i*4);
     *(d+2+16*4) = *(p+2+i*4);
     *(d+3+16*4) = *(p+3+i*4);

     *(d+24*4) = *(p+(7-i)*4);
     *(d+1+24*4) = *(p+1+(7-i)*4);
     *(d+2+24*4) = *(p+2+(7-i)*4);
     *(d+3+24*4) = *(p+3+(7-i)*4);
     
     d+=4;
     
  }
  
}

void MagmaEncryptor::ExpandKeyDecrypt(byte *source_key, byte *round_keys)
/*
 * Функция развертывания ключа - версия для расшифрования.
 * Операция расшифрования отличается только обратным порядком раундовых ключей.
 * 
 * Шифр в режиме расшифрования используется для генерации новых 
 * наборов ключей.
 * 
 * Схема:
 * 
 * | k_1 ... k_7, k_8 | k_8 ... k_1 | K_8 ... k_1 | k_8 ... k_1 |
 * 
 * 
 * Параметры: 
 * source_key - указатель на основной ключ (256 бит - массив из 32 байтов);
 * round_keys - указатель на массив, куда будут помещены ключи раундов.
 * 
 */
{
int i;
byte *p, *d;

p = source_key;
d = round_keys;

for(i = 0; i<8; i++){

     *d = *(p+i*4);
     *(d+1) = *(p+1+i*4);
     *(d+2) = *(p+2+i*4);
     *(d+3) = *(p+3+i*4);

     *(d+8*4) = *(p+(7-i)*4);
     *(d+1+8*4) = *(p+1+(7-i)*4);
     *(d+2+8*4) = *(p+2+(7-i)*4);
     *(d+3+8*4) = *(p+3+(7-i)*4);

     *(d+16*4) = *(p+(7-i)*4);
     *(d+1+16*4) = *(p+1+(7-i)*4);
     *(d+2+16*4) = *(p+2+(7-i)*4);
     *(d+3+16*4) = *(p+3+(7-i)*4);

     *(d+24*4) = *(p+(7-i)*4);
     *(d+1+24*4) = *(p+1+(7-i)*4);
     *(d+2+24*4) = *(p+2+(7-i)*4);
     *(d+3+24*4) = *(p+3+(7-i)*4);
     
     d+=4;
     
  }

  return;
  
}

int MagmaEncryptor::MagmaEncrypt(uint32_t *nonce, byte *pt, byte *dst,  uint16_t len)
/*
 * Функция шифрования в режиме счётчика (гаммирования - в русскоязычной терминологии).
 * 
 * Режим устроен следующим образом. Входное сообщение (открытый текст) разбивается на
 * блоки, длина которых соответствует разрядности шифра (64 бита). Последний блок может 
 * быть неполным. На основе nonce (значения, используемого однократно) строится 
 * последовательность блоков счётчика (в данной реализации, увеличивается 
 * последний 32-битный полублок). Блоки счётчика зашифровываются, полученные блоки
 * шифротекста суммируются (XOR) c соответствующими блоками открытого текста. Если 
 * последний блок - неполный, то используется только эквивалентная часть блока 
 * шифротекста. Полученная последовательность байтов - представляет результат 
 * зашифрования входного сообщения.
 * 
 * Другими словами: поток открытого текста суммируется с ключевым потоком равной длины,
 * ключевой поток генерируется при помощи (последовательного) шифрования значений 
 * счётчика.
 * 
 * Функция использует текущие раундовые ключи. В nonce возвращается новое значение,
 * полученное после окончания шифрования. Это нужно для того, чтобы код приложения 
 * получил простую возможность дальнейшего использования nonce.
 * 
 * Параметры:
 * 
 * nonce - 32-битное значение, задающее начальное состояние счётчика;
 * pt - указатель на блок открытого текста;
 * dst - указатель на буфер, куда будет записан шифротекст;
 * len - длина открытого текста, в байтах. Совпадает с длиной шифротекста.
 * 
 * Значение nonce и каждое значение счётчика с заданным ключом для зашифрования 
 * следует использовать только один раз.
 * 
 */
{
uint16_t bytes, blocks_num; // Число записанных байтов. Число блоков.
uint32_t counter;           // Значение счётчика.
int i,n;                    // Переменные (индексы) циклов.
byte *c,*p;                 // Указатели: буфер шифротекста, буфер открытого текста.
byte block[8];              // Блок счётчика.
byte g[8];                  // Результат зашифрования блока счётчика - блок гаммы.

  c = dst;
  p = pt;
  bytes = 0;
  
  // Определение необходимого числа блоков.
  if((len) % 8){ 
    blocks_num = (len) / 8 + 1;
  }else{
    blocks_num = (len) / 8;
  }

/*
 * Начальное значение счётчика - nonce.
 * Для формирования блока счётчика требуется 64 бита. В данной 
 * реализации первая половина блока образуется nonce, вторая (32 бита)
 * представляет собой увеличивающееся значение счётчика nonce. Таким образом,
 * первый блок имеет следующий вид: nonce,nonce+1 (запятая обозначает
 * конкатенацию битовых строк).
 * 
 */
  counter = (*nonce)+1;

  for(n = 0; n < 4; n++){

          block[n] = *((byte *)nonce+(3-n));
          block[n+4] = *((byte *)&counter+(3-n));

  }

// Цикл по числу блоков.
  for(i = 0; i < blocks_num; i++){

// Выполнение преобразования шифра.
    DoCipher(magma_round_keys, block, g, pi_ptr);

// Вычисление шифротекста (XOR открытого текста и гаммы).
    for(n = 0; n < 8; n++){
      *c++ = *p++^(g[n]);
      bytes++;
      if(bytes>=len){
      // При достижении конца открытого текста - возврат.
        *nonce = counter;
        return 1;
      }
    }

    counter++;
    for(n = 0; n < 4; n++){
          block[n+4] = *((byte *)&counter+(3-n));
    }
    
  }
  
return 1;
}

int MagmaEncryptor::MagmaDecrypt(uint32_t *nonce, byte *dst, byte *ct,  uint16_t len)
/*
 * Функция расшифрования. В режиме счётчика - обратная к MagmaEncrypt().
 * Операция расшифрования сводится к генерации гаммы, с последующим сложением
 * блоков шифротекста с блоками гаммы. Для успешного расшифрования, 
 * значения ключа и nonce должны соответствовать шифротексту.
 * 
 * Параметры:
 * 
 * nonce - 32-битное значение, задающее начальное состояние счётчика;
 * dst - указатель на буфер, куда будет записан открытый текст;
 * ct - указатель на блок шифротекста;
 * len - длина открытого текста, в байтах. Совпадает с длиной шифротекста.
 * 
 */
{
uint16_t bytes,blocks_num; // Число записанных байтов. Число блоков.
uint32_t counter;          // Значение счётчика.
int i,n;                   // Переменные (индексы) циклов.
byte *c,*p;                // Указатели: буфер шифротекста, буфер открытого текста.
byte block[8];             // Блок счётчика.
byte g[8];                 // Результат зашифрования блока счётчика - блок гаммы.

  c = ct;
  p = dst;
  bytes = 0;
  
  // Определение необходимого числа блоков.
  if((len) % 8){ 
    blocks_num = (len) / 8 + 1;
  }else{
    blocks_num = (len) / 8;
  }
 
  counter = (*nonce)+1;

  for(n = 0; n < 4; n++){

          block[n] = *((byte *)nonce+(3-n));
          block[n+4] = *((byte *)&counter+(3-n));

  }

// Цикл по числу блоков.
  for(i = 0; i < blocks_num; i++){

// Выполнение преобразования шифра.
    DoCipher(magma_round_keys, block, g, pi_ptr);

// Вычисление шифротекста (XOR открытого текста и гаммы).
    for(n = 0; n < 8; n++){
      *p++ = *c++^(g[n]);
      bytes++;
      if(bytes>=len){
      // При достижении конца открытого текста - возврат.
        *nonce = counter;
        return 1;
      }
    }

    counter++;
    for(n = 0; n < 4; n++){
          block[n+4] = *((byte *)&counter+(3-n));

    }
    
  }
  
return 1;
}

int MagmaEncryptor::MagmaMAC(uint32_t nonce, byte *text,  uint16_t len, byte *MAC){
/*
 * Вычисление кода аутентификации.
 * 
 * Используется алгоритм CMAC (NIST SP 800-38B.
 * 
 * MAC вычисляется для сообщения целиком, включая nonce. Предполагается, что данная функция
 * вызывается для проверки целостности полученного шифротекста. Значение nonce может не 
 * передаваться вместе с шифротекстом, но должно быть синхронным на обоих узлах.
 * 
 */
uint16_t bytes,blocks_num; // Число записанных байтов. Число блоков.
int i,n,t;                 // Переменные (индексы) циклов.
byte *c,*p;                // Указатели: буфер шифротекста, буфер открытого текста.
byte pt[8];       // Открытый текст.
byte ct[8];       // Шифротекст.

  for(i = 0; i < 8; i++){
    ct[i] = 0;
  }

  p = text;
  
  // Определение необходимого числа блоков.
  if((len) % 8){ 
    blocks_num = (len) / 8 + 1;
    t = 1; // Признак наличия последнего неполного блока.
  }else{
    blocks_num = (len) / 8;
    t = 0; // Последний блок - полный.
  }

  
  for(i = 0; i < 4; i++){  // Значение nonce формирует первый блок CMAC.

          pt[i] = *((byte *)&nonce+(3-i));    // Блок имеет разрядность 64 бита,
          pt[i+4] = *((byte *)&nonce+(3-i));  // поэтому 32-битное nonce копируется.

  }

  DoCipher(magma_MAC_round_keys, pt, ct, pi_ptr);

  bytes = 0;

  for(n = 0; n < (blocks_num-1); n++){
    
   for(i = 0; i < 8; i++){
      pt[i] = *p++ ^ ct[i];
   }

   DoCipher(magma_MAC_round_keys, pt, ct, pi_ptr);
   bytes += 8;
   
  }

  if(t){ // Если последний блок - неполный.
    int q;
    q = (8*blocks_num) - len;  // q - число байтов дополнения.
    pt[8-q] = 0B10000000;      // Первый байт дополнения имеет значение 128.
    for(i = (8-q+1); i < 8; i++){
      pt[i] = 0;               // Остальные байты дополнения - нулевые.
    }
    for(i = 0; i < 8-q; i++){
      pt[i] = *p++;
    }
       
    for(i = 0; i < 8; i++){
      pt[i] = pt[i] ^ magma_MAC_K2[i];  // Неполный блок суммируется с ключом K2.
      pt[i] = pt[i] ^ ct[i];
    }
   
  }else{

    for(i = 0; i < 8; i++){
      pt[i] = *p++ ^ magma_MAC_K1[i];  // Полный блок суммируется с ключом K1.
      pt[i] = pt[i] ^ ct[i];
    }
   
  }

  DoCipher(magma_MAC_round_keys, pt, ct, pi_ptr); // Шифрование последнего блока.

  c = MAC;

  for(i = 0; i < 8; i++){
    c[i] = ct[i];    // Вывод результата.
  }
  
return 1;
}