1 #include "aes_cmac.h" 2 #include "rijndael.h" 3 4 #include <string.h> 5 6 static void sm_shift_left_by_one_bit_inplace(int len, uint8_t * data){ 7 int i; 8 int carry = 0; 9 for (i=len-1; i >= 0 ; i--){ 10 int new_carry = data[i] >> 7; 11 data[i] = data[i] << 1 | carry; 12 carry = new_carry; 13 } 14 } 15 16 void aes128_calc_cyphertext(const uint8_t key[16], const uint8_t plaintext[16], uint8_t cyphertext[16]){ 17 uint32_t rk[RKLENGTH(KEYBITS)]; 18 int nrounds = rijndaelSetupEncrypt(rk, &key[0], KEYBITS); 19 rijndaelEncrypt(rk, nrounds, plaintext, cyphertext); 20 } 21 22 void aes_cmac_calc_subkeys(sm_key_t k0, sm_key_t k1, sm_key_t k2){ 23 memcpy(k1, k0, 16); 24 sm_shift_left_by_one_bit_inplace(16, k1); 25 if (k0[0] & 0x80){ 26 k1[15] ^= 0x87; 27 } 28 memcpy(k2, k1, 16); 29 sm_shift_left_by_one_bit_inplace(16, k2); 30 if (k1[0] & 0x80){ 31 k2[15] ^= 0x87; 32 } 33 } 34 35 void aes_cmac(sm_key_t aes_cmac, const sm_key_t key, const uint8_t * data, int sm_cmac_message_len){ 36 sm_key_t k0, k1, k2, zero; 37 memset(zero, 0, 16); 38 39 aes128_calc_cyphertext(key, zero, k0); 40 aes_cmac_calc_subkeys(k0, k1, k2); 41 42 int sm_cmac_block_count = (sm_cmac_message_len + 15) / 16; 43 44 // step 3: .. 45 if (sm_cmac_block_count==0){ 46 sm_cmac_block_count = 1; 47 } 48 49 // step 4: set m_last 50 sm_key_t sm_cmac_m_last; 51 int sm_cmac_last_block_complete = sm_cmac_message_len != 0 && (sm_cmac_message_len & 0x0f) == 0; 52 int i; 53 if (sm_cmac_last_block_complete){ 54 for (i=0;i<16;i++){ 55 sm_cmac_m_last[i] = data[sm_cmac_message_len - 16 + i] ^ k1[i]; 56 } 57 } else { 58 int valid_octets_in_last_block = sm_cmac_message_len & 0x0f; 59 for (i=0;i<16;i++){ 60 if (i < valid_octets_in_last_block){ 61 sm_cmac_m_last[i] = data[(sm_cmac_message_len & 0xfff0) + i] ^ k2[i]; 62 continue; 63 } 64 if (i == valid_octets_in_last_block){ 65 sm_cmac_m_last[i] = 0x80 ^ k2[i]; 66 continue; 67 } 68 sm_cmac_m_last[i] = k2[i]; 69 } 70 } 71 72 // printf("sm_cmac_start: len %u, block count %u\n", sm_cmac_message_len, sm_cmac_block_count); 73 // LOG_KEY(sm_cmac_m_last); 74 75 // Step 5 76 sm_key_t sm_cmac_x; 77 memset(sm_cmac_x, 0, 16); 78 79 // Step 6 80 sm_key_t sm_cmac_y; 81 for (int block = 0 ; block < sm_cmac_block_count-1 ; block++){ 82 for (i=0;i<16;i++){ 83 sm_cmac_y[i] = sm_cmac_x[i] ^ data[block * 16 + i]; 84 } 85 aes128_calc_cyphertext(key, sm_cmac_y, sm_cmac_x); 86 } 87 for (i=0;i<16;i++){ 88 sm_cmac_y[i] = sm_cmac_x[i] ^ sm_cmac_m_last[i]; 89 } 90 91 // Step 7 92 aes128_calc_cyphertext(key, sm_cmac_y, aes_cmac); 93 }