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 unsigned int i; 37 sm_key_t k0, k1, k2, zero; 38 memset(zero, 0, 16); 39 40 aes128_calc_cyphertext(key, zero, k0); 41 aes_cmac_calc_subkeys(k0, k1, k2); 42 43 int sm_cmac_block_count = (sm_cmac_message_len + 15) / 16; 44 45 // step 3: .. 46 if (sm_cmac_block_count==0){ 47 sm_cmac_block_count = 1; 48 } 49 50 // printf("sm_cmac_start: len %u, block count %u\n", sm_cmac_message_len, sm_cmac_block_count); 51 // LOG_KEY(sm_cmac_m_last); 52 53 // Step 5 54 sm_key_t sm_cmac_x; 55 memset(sm_cmac_x, 0, 16); 56 57 // Step 6 58 sm_key_t sm_cmac_y; 59 int block; 60 for (block = 0 ; block < sm_cmac_block_count-1 ; block++){ 61 for (i=0;i<16;i++){ 62 sm_cmac_y[i] = sm_cmac_x[i] ^ data[block * 16 + i]; 63 } 64 aes128_calc_cyphertext(key, sm_cmac_y, sm_cmac_x); 65 } 66 67 // step 4: set m_last 68 sm_key_t sm_cmac_m_last; 69 int sm_cmac_last_block_complete = sm_cmac_message_len != 0 && (sm_cmac_message_len & 0x0f) == 0; 70 if (sm_cmac_last_block_complete){ 71 for (i=0;i<16;i++){ 72 sm_cmac_m_last[i] = data[sm_cmac_message_len - 16 + i] ^ k1[i]; 73 } 74 } else { 75 int valid_octets_in_last_block = sm_cmac_message_len & 0x0f; 76 for (i=0;i<16;i++){ 77 if (i < valid_octets_in_last_block){ 78 sm_cmac_m_last[i] = data[(sm_cmac_message_len & 0xfff0) + i] ^ k2[i]; 79 continue; 80 } 81 if (i == valid_octets_in_last_block){ 82 sm_cmac_m_last[i] = 0x80 ^ k2[i]; 83 continue; 84 } 85 sm_cmac_m_last[i] = k2[i]; 86 } 87 } 88 89 for (i=0;i<16;i++){ 90 sm_cmac_y[i] = sm_cmac_x[i] ^ sm_cmac_m_last[i]; 91 } 92 93 // Step 7 94 aes128_calc_cyphertext(key, sm_cmac_y, aes_cmac); 95 }