Line data    Source code

       1             : #include "Cryptography.h"
       2             : 
       3             : #include <string.h>
       4             : 
       5           1 : RadioLibAES128::RadioLibAES128() {
       6             : 
       7           1 : }
       8             : 
       9           4 : void RadioLibAES128::init(uint8_t* key) {
      10           4 :   this->keyPtr = key;
      11           4 :   this->keyExpansion(this->roundKey, key);
      12           4 : }
      13             : 
      14          13 : size_t RadioLibAES128::encryptECB(const uint8_t* in, size_t len, uint8_t* out) {
      15          13 :   size_t num_blocks = len / RADIOLIB_AES128_BLOCK_SIZE;
      16          13 :   if(len % RADIOLIB_AES128_BLOCK_SIZE) {
      17           0 :     num_blocks++;
      18             :   }
      19             : 
      20          13 :   memset(out, 0x00, RADIOLIB_AES128_BLOCK_SIZE * num_blocks);
      21          13 :   memcpy(out, in, len);
      22             : 
      23          26 :   for(size_t i = 0; i < num_blocks; i++) {
      24          13 :     this->cipher((state_t*)(out + (RADIOLIB_AES128_BLOCK_SIZE * i)), this->roundKey);
      25             :   }
      26             : 
      27          13 :   return(num_blocks*RADIOLIB_AES128_BLOCK_SIZE);
      28             : }
      29             : 
      30           0 : size_t RadioLibAES128::decryptECB(const uint8_t* in, size_t len, uint8_t* out) {
      31           0 :   size_t num_blocks = len / RADIOLIB_AES128_BLOCK_SIZE;
      32           0 :   if(len % RADIOLIB_AES128_BLOCK_SIZE) {
      33           0 :     num_blocks++;
      34             :   }
      35             : 
      36           0 :   memset(out, 0x00, RADIOLIB_AES128_BLOCK_SIZE * num_blocks);
      37           0 :   memcpy(out, in, len);
      38             : 
      39           0 :   for(size_t i = 0; i < num_blocks; i++) {
      40           0 :     this->decipher((state_t*)(out + (RADIOLIB_AES128_BLOCK_SIZE * i)), this->roundKey);
      41             :   }
      42             : 
      43           0 :   return(num_blocks*RADIOLIB_AES128_BLOCK_SIZE);
      44             : }
      45             : 
      46             : /*
      47             :  * CMAC streaming API
      48             :  *
      49             :  * Usage:
      50             :  *   RadioLibCMAC_State st;
      51             :  *   RadioLibAES128_initCMACState(&RadioLibAES128Instance, &st);
      52             :  *   RadioLibAES128_updateCMACState(&RadioLibAES128Instance, &st, chunk1, len1);
      53             :  *   RadioLibAES128_updateCMACState(&RadioLibAES128Instance, &st, chunk2, len2);
      54             :  *   uint8_t mac[16];
      55             :  *   RadioLibAES128_finishCMACState(&RadioLibAES128Instance, &st, mac);
      56             :  */
      57             : 
      58           4 : void RadioLibAES128::initCMAC(RadioLibCmacState* st) {
      59           4 :   if(!st) {
      60           0 :     return;
      61             :   }
      62           4 :   memset(st->X, 0x00, RADIOLIB_AES128_BLOCK_SIZE);
      63           4 :   memset(st->buffer, 0x00, RADIOLIB_AES128_BLOCK_SIZE);
      64           4 :   st->buffer_len = 0;
      65           4 :   st->subkeys_generated = false;
      66             : }
      67             : 
      68           4 : void RadioLibAES128::updateCMAC(RadioLibCmacState* st, const uint8_t* data, size_t len) {
      69           4 :   if(!st || (!data && len != 0)) {
      70           0 :     return;
      71             :   }
      72             : 
      73             :   // ensure subkeys are present
      74           4 :   if(!st->subkeys_generated) {
      75           4 :     this->generateSubkeys(st->k1, st->k2);
      76           4 :     st->subkeys_generated = true;
      77             :   }
      78             : 
      79             :   uint8_t tmp[RADIOLIB_AES128_BLOCK_SIZE];
      80           4 :   size_t offset = 0;
      81             : 
      82          12 :   while(len > 0) {
      83             : 
      84             :     // fill buffer up to one full block
      85           8 :     size_t to_copy = RADIOLIB_AES128_BLOCK_SIZE - st->buffer_len;
      86           8 :     if(to_copy > len) {
      87           1 :       to_copy = len;
      88             :     }
      89             : 
      90             :     // copy the data into the buffer
      91           8 :     memcpy(&st->buffer[st->buffer_len], &data[offset], to_copy);
      92           8 :     st->buffer_len += to_copy;
      93           8 :     offset += to_copy;
      94           8 :     len -= to_copy;
      95             : 
      96             :     // if we now have a full block AND there is still more input remaining,
      97             :     // this block is NOT the final one, so process it.
      98           8 :     if(st->buffer_len == RADIOLIB_AES128_BLOCK_SIZE && len > 0) {
      99           5 :       this->blockXor(tmp, st->buffer, st->X);
     100           5 :       this->encryptECB(tmp, RADIOLIB_AES128_BLOCK_SIZE, st->X);
     101           5 :       st->buffer_len = 0;
     102             :     }
     103             :   }
     104             : }
     105             : 
     106           4 : void RadioLibAES128::finishCMAC(RadioLibCmacState* st, uint8_t* out) {
     107           4 :   if(!st || !out) {
     108           0 :     return;
     109             :   }
     110             : 
     111             :   // ensure subkeys are present
     112           4 :   if(!st->subkeys_generated) {
     113           0 :     this->generateSubkeys(st->k1, st->k2);
     114           0 :     st->subkeys_generated = true;
     115             :   }
     116             : 
     117             :   uint8_t last[RADIOLIB_AES128_BLOCK_SIZE];
     118             :   uint8_t Y[RADIOLIB_AES128_BLOCK_SIZE];
     119             : 
     120           4 :   if(st->buffer_len == RADIOLIB_AES128_BLOCK_SIZE) {
     121           2 :     this->blockXor(last, st->buffer, st->k1);
     122             :   } else {
     123           2 :     memset(last, 0x00, RADIOLIB_AES128_BLOCK_SIZE);
     124           2 :     if(st->buffer_len > 0) {
     125           1 :       memcpy(last, st->buffer, st->buffer_len);
     126             :     }
     127           2 :     last[st->buffer_len] = 0x80;
     128           2 :     this->blockXor(last, last, st->k2);
     129             :   }
     130             : 
     131           4 :   this->blockXor(Y, last, st->X);
     132           4 :   this->encryptECB(Y, RADIOLIB_AES128_BLOCK_SIZE, out);
     133             : }
     134             : 
     135           4 : void RadioLibAES128::generateCMAC(const uint8_t* in, size_t len, uint8_t* cmac) {
     136             :   RadioLibCmacState st;
     137           4 :   this->initCMAC(&st);
     138           4 :   this->updateCMAC(&st, in, len);
     139           4 :   this->finishCMAC(&st, cmac);
     140           4 : }
     141             : 
     142           0 : bool RadioLibAES128::verifyCMAC(const uint8_t* in, size_t len, const uint8_t* cmac) {
     143             :   uint8_t cmacReal[RADIOLIB_AES128_BLOCK_SIZE];
     144           0 :   this->generateCMAC(in, len, cmacReal);
     145           0 :   for(size_t i = 0; i < RADIOLIB_AES128_BLOCK_SIZE; i++) {
     146           0 :     if((cmacReal[i] != cmac[i])) {
     147           0 :       return(false);
     148             :     }
     149             :   }
     150           0 :   return(true);
     151             : }
     152             : 
     153           4 : void RadioLibAES128::keyExpansion(uint8_t* roundKey, const uint8_t* key) {
     154             :   uint8_t tmp[4];
     155             : 
     156             :   // the first round key is the key itself
     157          20 :   for(uint8_t i = 0; i < RADIOLIB_AES128_N_K; i++) {
     158          80 :     for(uint8_t j = 0; j < 4; j++) {
     159          64 :       roundKey[(i * 4) + j] = key[(i * 4) + j];
     160             :     }
     161             :   }
     162             : 
     163             :   // All other round keys are found from the previous round keys.
     164         164 :   for(uint8_t i = RADIOLIB_AES128_N_K; i < RADIOLIB_AES128_N_B * (RADIOLIB_AES128_N_R + 1); ++i) {
     165         160 :     uint8_t j = (i - 1) * 4;
     166         800 :     for(uint8_t k = 0; k < 4; k++) {
     167         640 :       tmp[k] = roundKey[j + k];
     168             :     }
     169             : 
     170         160 :     if(i % RADIOLIB_AES128_N_K == 0) {
     171          40 :       this->rotWord(tmp);
     172          40 :       this->subWord(tmp);
     173          40 :       tmp[0] = tmp[0] ^ aesRcon[i/RADIOLIB_AES128_N_K];
     174             :     }
     175             : 
     176         160 :     j = i * 4;
     177         160 :     uint8_t k = (i - RADIOLIB_AES128_N_K) * 4;
     178         800 :     for(uint8_t l = 0; l < 4; l++) {
     179         640 :       roundKey[j + l] = roundKey[k + l] ^ tmp[l];
     180             :     }
     181             :   }
     182           4 : }
     183             : 
     184          13 : void RadioLibAES128::cipher(state_t* state, uint8_t* roundKey) {
     185          13 :   this->addRoundKey(0, state, roundKey);
     186         130 :   for(uint8_t round = 1; round < RADIOLIB_AES128_N_R; round++) {
     187         117 :     this->subBytes(state, aesSbox);
     188         117 :     this->shiftRows(state, false);
     189         117 :     this->mixColumns(state, false);
     190         117 :     this->addRoundKey(round, state, roundKey);
     191             :   }
     192             : 
     193          13 :   this->subBytes(state, aesSbox);
     194          13 :   this->shiftRows(state, false);
     195          13 :   this->addRoundKey(RADIOLIB_AES128_N_R, state, roundKey);
     196          13 : }
     197             : 
     198             : 
     199           0 : void RadioLibAES128::decipher(state_t* state, uint8_t* roundKey) {
     200           0 :   this->addRoundKey(RADIOLIB_AES128_N_R, state, roundKey);
     201           0 :   for(uint8_t round = RADIOLIB_AES128_N_R - 1; round > 0; --round) {
     202           0 :     this->shiftRows(state, true);
     203           0 :     this->subBytes(state, aesSboxInv);
     204           0 :     this->addRoundKey(round, state, roundKey);
     205           0 :     this->mixColumns(state, true);
     206             :   }
     207             : 
     208           0 :   this->shiftRows(state, true);
     209           0 :   this->subBytes(state, aesSboxInv);
     210           0 :   this->addRoundKey(0, state, roundKey);
     211           0 : }
     212             : 
     213          40 : void RadioLibAES128::subWord(uint8_t* word) {
     214         200 :   for(size_t i = 0; i < 4; i++) {
     215         160 :     uint8_t* ptr = const_cast<uint8_t*>(&aesSbox[word[i]]);
     216         160 :     word[i] = RADIOLIB_NONVOLATILE_READ_BYTE(ptr);
     217             :   }
     218          40 : }
     219             : 
     220          40 : void RadioLibAES128::rotWord(uint8_t* word) {
     221             :   uint8_t tmp[4];
     222          40 :   memcpy(tmp, word, 4);
     223         200 :   for(size_t i = 0; i < 4; i++) {
     224         160 :     word[i] = tmp[(i + 1) % 4];
     225             :   }
     226          40 : }
     227             : 
     228         143 : void RadioLibAES128::addRoundKey(uint8_t round, state_t* state, const uint8_t* roundKey) {
     229         715 :   for(size_t row = 0; row < 4; row++) {
     230        2860 :     for(size_t col = 0; col < 4; col++) {
     231        2288 :       (*state)[row][col] ^= roundKey[(round * RADIOLIB_AES128_N_B * 4) + (row * RADIOLIB_AES128_N_B) + col];
     232             :     }
     233             :   }
     234         143 : }
     235             : 
     236          17 : void RadioLibAES128::blockXor(uint8_t* dst, const uint8_t* a, const uint8_t* b) {
     237         289 :   for(uint8_t j = 0; j < RADIOLIB_AES128_BLOCK_SIZE; j++) {
     238         272 :     dst[j] = a[j] ^ b[j];
     239             :   }
     240          17 : }
     241             : 
     242           8 : void RadioLibAES128::blockLeftshift(uint8_t* dst, const uint8_t* src) {
     243           8 :   uint8_t ovf = 0x00;
     244         136 :   for(int8_t i = RADIOLIB_AES128_BLOCK_SIZE - 1; i >= 0; i--) {
     245         128 :     dst[i] = src[i] << 1;
     246         128 :     dst[i] |= ovf;
     247         128 :     ovf = (src[i] & 0x80) ? 1 : 0;
     248             :   }
     249           8 : }
     250             : 
     251           4 : void RadioLibAES128::generateSubkeys(uint8_t* key1, uint8_t* key2) {
     252           4 :   const uint8_t const_Zero[] = {
     253             :     0x00, 0x00, 0x00, 0x00,
     254             :     0x00, 0x00, 0x00, 0x00,
     255             :     0x00, 0x00, 0x00, 0x00,
     256             :     0x00, 0x00, 0x00, 0x00
     257             :   };
     258             : 
     259           4 :   const uint8_t const_Rb[] = {
     260             :     0x00, 0x00, 0x00, 0x00,
     261             :     0x00, 0x00, 0x00, 0x00,
     262             :     0x00, 0x00, 0x00, 0x00,
     263             :     0x00, 0x00, 0x00, 0x87
     264             :   };
     265             : 
     266             :   uint8_t L[RADIOLIB_AES128_BLOCK_SIZE];
     267           4 :   this->encryptECB(const_Zero, RADIOLIB_AES128_BLOCK_SIZE, L);
     268           4 :   this->blockLeftshift(key1, L);
     269           4 :   if(L[0] & 0x80) {
     270           0 :     this->blockXor(key1, key1, const_Rb);
     271             :   }
     272             : 
     273           4 :   this->blockLeftshift(key2, key1);
     274           4 :   if(key1[0] & 0x80) {
     275           4 :     this->blockXor(key2, key2, const_Rb);
     276             :   }
     277           4 : }
     278             : 
     279         130 : void RadioLibAES128::subBytes(state_t* state, const uint8_t* box) {
     280         650 :   for(size_t row = 0; row < 4; row++) {
     281        2600 :     for(size_t col = 0; col < 4; col++) {
     282        2080 :       uint8_t* ptr = const_cast<uint8_t*>(&box[(*state)[col][row]]);
     283        2080 :       (*state)[col][row] = RADIOLIB_NONVOLATILE_READ_BYTE(ptr);
     284             :     }
     285             :   }
     286         130 : }
     287             : 
     288         130 : void RadioLibAES128::shiftRows(state_t* state, bool inv) {
     289             :   uint8_t tmp[4];
     290         520 :   for(size_t row = 1; row < 4; row++) {
     291        1950 :     for(size_t col = 0; col < 4; col++) {
     292        1560 :       if(!inv) {
     293        1560 :         tmp[col] = (*state)[(row + col) % 4][row];
     294             :       } else {
     295           0 :         tmp[(row + col) % 4] = (*state)[col][row];
     296             :       }
     297             :     }
     298        1950 :     for(size_t col = 0; col < 4; col++) {
     299        1560 :       (*state)[col][row] = tmp[col];
     300             :     }
     301             :   }
     302         130 : }
     303             : 
     304         117 : void RadioLibAES128::mixColumns(state_t* state, bool inv) {
     305             :   uint8_t tmp[4];
     306         117 :   uint8_t matmul[][4] = {
     307             :     0x02, 0x03, 0x01, 0x01,
     308             :     0x01, 0x02, 0x03, 0x01,
     309             :     0x01, 0x01, 0x02, 0x03,
     310             :     0x03, 0x01, 0x01, 0x02
     311             :   };
     312         117 :   if(inv) {
     313           0 :     uint8_t matmul_inv[][4] = {
     314             :       0x0e, 0x0b, 0x0d, 0x09,
     315             :       0x09, 0x0e, 0x0b, 0x0d,
     316             :       0x0d, 0x09, 0x0e, 0x0b,
     317             :       0x0b, 0x0d, 0x09, 0x0e
     318             :     };
     319           0 :     memcpy(matmul, matmul_inv, sizeof(matmul_inv));
     320             :   }
     321             : 
     322         585 :   for(size_t col = 0; col < 4; col++) {
     323        2340 :     for(size_t row = 0; row < 4; row++) {
     324        1872 :       tmp[row] = (*state)[col][row];
     325             :     }
     326        2340 :     for(size_t i = 0; i < 4; i++) {
     327        1872 :       (*state)[col][i] = 0x00;
     328        9360 :       for(size_t j = 0; j < 4; j++) {
     329        7488 :         (*state)[col][i] ^= mul(matmul[i][j], tmp[j]);
     330             :       }
     331             :     }
     332             :   }
     333         117 : }
     334             : 
     335        7488 : uint8_t RadioLibAES128::mul(uint8_t a, uint8_t b) {
     336             :   uint8_t sb[4];
     337        7488 :   uint8_t out = 0;
     338        7488 :   sb[0] = b;
     339       29952 :   for(size_t i = 1; i < 4; i++) {
     340       22464 :     sb[i] = sb[i - 1] << 1;
     341       22464 :     if (sb[i - 1] & 0x80) {
     342       11032 :       sb[i] ^= 0x1b;
     343             :     }
     344             :   }
     345       37440 :   for(size_t i = 0; i < 4; i++) {
     346       29952 :     if(a >> i & 0x01) {
     347        9360 :       out ^= sb[i];
     348             :     }
     349             :   }
     350        7488 :   return(out);
     351             : }
     352             : 
     353             : RadioLibAES128 RadioLibAES128Instance;