1 #include "aes_cmac.h"
2 #include "rijndael.h"
3
4 #include <string.h>
5
sm_shift_left_by_one_bit_inplace(int len,uint8_t * data)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
aes128_calc_cyphertext(const uint8_t key[16],const uint8_t plaintext[16],uint8_t cyphertext[16])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
aes_cmac_calc_subkeys(sm_key_t k0,sm_key_t k1,sm_key_t k2)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
aes_cmac(sm_key_t aes_cmac,const sm_key_t key,const uint8_t * data,int sm_cmac_message_len)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 }