1 2 #include <stdio.h> 3 #include <string.h> 4 #include "aes_cmac.h" 5 6 // #include "btstack_util.h" 7 8 typedef uint8_t sm_key24_t[3]; 9 typedef uint8_t sm_key56_t[7]; 10 typedef uint8_t sm_key256_t[32]; 11 12 static const char * key_string = "2b7e1516 28aed2a6 abf71588 09cf4f3c"; 13 static const char * k0_string = "7df76b0c 1ab899b3 3e42f047 b91b546f"; 14 static const char * k1_string = "fbeed618 35713366 7c85e08f 7236a8de"; 15 static const char * k2_string = "f7ddac30 6ae266cc f90bc11e e46d513b"; 16 17 static const char * m0_string = ""; 18 static const char * cmac_m0_string = "bb1d6929 e9593728 7fa37d12 9b756746"; 19 static const char * m16_string = "6bc1bee2 2e409f96 e93d7e11 7393172a"; 20 static const char * cmac_m16_string = "070a16b4 6b4d4144 f79bdd9d d04a287c"; 21 static const char * m40_string = "6bc1bee2 2e409f96 e93d7e11 7393172a ae2d8a57 1e03ac9c 9eb76fac 45af8e51 30c81c46 a35ce411"; 22 static const char * cmac_m40_string = "dfa66747 de9ae630 30ca3261 1497c827"; 23 static const char * m64_string = "6bc1bee2 2e409f96 e93d7e11 7393172a ae2d8a57 1e03ac9c 9eb76fac 45af8e51 30c81c46 a35ce411 e5fbc119 1a0a52ef f69f2445 df4f9b17 ad2b417b e66c3710"; 24 static const char * cmac_m64_string = "51f0bebf 7e3b9d92 fc497417 79363cfe"; 25 26 // f4 27 static const char * f4_u_string = "20b003d2 f297be2c 5e2c83a7 e9f9a5b9 eff49111 acf4fddb cc030148 0e359de6"; 28 static const char * f4_v_string = "55188b3d 32f6bb9a 900afcfb eed4e72a 59cb9ac2 f19d7cfb 6b4fdd49 f47fc5fd"; 29 static const char * f4_x_string = "d5cb8454 d177733e ffffb2ec 712baeab"; 30 static const char * f4_z_string = "00"; 31 static const char * f4_cmac_string = "f2c916f1 07a9bd1c f1eda1be a974872d"; 32 33 // f5 34 const char * f5_w_string = "ec0234a3 57c8ad05 341010a6 0a397d9b 99796b13 b4f866f1 868d34f3 73bfa698"; 35 const char * f5_t_string = "3c128f20 de883288 97624bdb 8dac6989"; 36 const char * f5_n1_string = "d5cb8454 d177733e ffffb2ec 712baeab"; 37 const char * f5_n2_string = "a6e8e7cc 25a75f6e 216583f7 ff3dc4cf"; 38 const char * f5_a1_string = "00561237 37bfce"; 39 const char * f5_a2_string = "00a71370 2dcfc1"; 40 const char * f5_cmac_string = "2965f176 a1084a02 fd3f6a20 ce636e20"; 41 42 // f6 43 const char * f6_n1_string = "d5cb8454 d177733e ffffb2ec 712baeab"; 44 const char * f6_n2_string = "a6e8e7cc 25a75f6e 216583f7 ff3dc4cf"; 45 const char * f6_mac_key_string = "2965f176 a1084a02 fd3f6a20 ce636e20"; 46 const char * f6_r_string = "12a3343b b453bb54 08da42d2 0c2d0fc8"; 47 const char * f6_io_cap_string = "010102"; 48 const char * f6_a1_string = "00561237 37bfce"; 49 const char * f6_a2_string = "00a71370 2dcfc1"; 50 const char * f6_cmac_string = "e3c47398 9cd0e8c5 d26c0b09 da958f61"; 51 52 // g2 53 const char * g2_u_string = "20b003d2 f297be2c 5e2c83a7 e9f9a5b9 eff49111 acf4fddb cc030148 0e359de6"; 54 const char * g2_v_string = "55188b3d 32f6bb9a 900afcfb eed4e72a 59cb9ac2 f19d7cfb 6b4fdd49 f47fc5fd"; 55 const char * g2_x_string = "d5cb8454 d177733e ffffb2ec 712baeab"; 56 const char * g2_y_string = "a6e8e7cc 25a75f6e 216583f7 ff3dc4cf"; 57 // const char * g2_cmac_string = "1536d18d e3d20df9 9b7044c1 2f9ed5ba"; 58 const char * g2_res_string = "2f9ed5ba"; 59 60 // h6 61 const char * h6_key_string = "ec0234a3 57c8ad05 341010a6 0a397d9b"; 62 const char * h6_key_id_string = "6c656272"; 63 const char * h6_cmac_string = "2d9ae102 e76dc91c e8d3a9e2 80b16399"; 64 65 // h7 66 const char * h7_key_string = "ec0234a3 57c8ad05 341010a6 0a397d9b"; 67 const char * h7_cmac_string = "fb173597 c6a3c0ec d2998c2a 75a57011"; 68 69 70 static uint32_t big_endian_read_32( const uint8_t * buffer, int pos) { 71 return ((uint32_t) buffer[(pos)+3]) | (((uint32_t)buffer[(pos)+2]) << 8) | (((uint32_t)buffer[(pos)+1]) << 16) | (((uint32_t) buffer[pos]) << 24); 72 } 73 74 static void big_endian_store_32(uint8_t *buffer, uint16_t pos, uint32_t value){ 75 buffer[pos++] = value >> 24; 76 buffer[pos++] = value >> 16; 77 buffer[pos++] = value >> 8; 78 buffer[pos++] = value; 79 } 80 81 static void hexdump2(void *data, int size){ 82 if (size <= 0) return; 83 int i; 84 for (i=0; i<size;i++){ 85 printf("%02X ", ((uint8_t *)data)[i]); 86 } 87 printf("\n"); 88 } 89 90 static int nibble_for_char(char c){ 91 if (c >= '0' && c <= '9') return c - '0'; 92 if (c >= 'a' && c <= 'f') return c - 'a' + 10; 93 if (c >= 'A' && c <= 'F') return c - 'A' + 10; 94 return -1; 95 } 96 97 static int parse_hex(uint8_t * buffer, const char * hex_string){ 98 int len = 0; 99 while (*hex_string){ 100 if (*hex_string == ' '){ 101 hex_string++; 102 continue; 103 } 104 int high_nibble = nibble_for_char(*hex_string++); 105 int low_nibble = nibble_for_char(*hex_string++); 106 *buffer++ = (high_nibble << 4) | low_nibble; 107 len++; 108 } 109 return len; 110 } 111 112 #define LOG_KEY(NAME) { printf("%16s: ", #NAME); hexdump2(NAME, 16); } 113 #define PARSE_KEY(NAME) { parse_hex(NAME, NAME##_string); LOG_KEY(NAME); } 114 #define VALIDATE_KEY(NAME) { LOG_KEY(NAME); sm_key_t test; parse_hex(test, NAME##_string); if (memcmp(NAME, test, 16)){ printf("Error calculating key\n"); } } 115 #define VALIDATE_MESSAGE(NAME) validate_message(#NAME, NAME##_string, cmac_##NAME##_string) 116 117 118 static void validate_message(const char * name, const char * message_string, const char * cmac_string){ 119 120 uint8_t m[128]; 121 int len = parse_hex(m, message_string); 122 123 sm_key_t cmac; 124 parse_hex(cmac, cmac_string); 125 126 printf("-- verify message %s, len %u:\nm: %s\ncmac: %s\n", name, len, message_string, cmac_string); 127 128 sm_key_t key; 129 parse_hex(key, key_string); 130 131 sm_key_t cmac_test; 132 aes_cmac(cmac_test, key, m, len); 133 134 LOG_KEY(cmac_test); 135 136 if (memcmp(cmac_test, cmac, 16)){ 137 printf("CMAC incorrect!\n"); 138 } else { 139 printf("CMAC correct!\n"); 140 } 141 } 142 143 static void f4(sm_key_t res, const sm_key256_t u, const sm_key256_t v, const sm_key_t x, uint8_t z){ 144 uint8_t buffer[65]; 145 memcpy(buffer, u, 32); 146 memcpy(buffer+32, v, 32); 147 buffer[64] = z; 148 // hexdump2(buffer, sizeof(buffer)); 149 aes_cmac(res, x, buffer, sizeof(buffer)); 150 } 151 152 const sm_key_t f5_salt = { 0x6C ,0x88, 0x83, 0x91, 0xAA, 0xF5, 0xA5, 0x38, 0x60, 0x37, 0x0B, 0xDB, 0x5A, 0x60, 0x83, 0xBE}; 153 const uint8_t f5_key_id[] = { 0x62, 0x74, 0x6c, 0x65 }; 154 const uint8_t f5_length[] = { 0x01, 0x00}; 155 static void f5(sm_key256_t res, const sm_key256_t w, const sm_key_t n1, const sm_key_t n2, const sm_key56_t a1, const sm_key56_t a2){ 156 // T = AES-CMACSAL_T(W) 157 sm_key_t t; 158 aes_cmac(t, f5_salt, w, 32); 159 // f5(W, N1, N2, A1, A2) = AES-CMACT (Counter = 0 || keyID || N1 || N2|| A1|| A2 || Length = 256) -- this is the MacKey 160 uint8_t buffer[53]; 161 buffer[0] = 0; 162 memcpy(buffer+01, f5_key_id, 4); 163 memcpy(buffer+05, n1, 16); 164 memcpy(buffer+21, n2, 16); 165 memcpy(buffer+37, a1, 7); 166 memcpy(buffer+44, a2, 7); 167 memcpy(buffer+51, f5_length, 2); 168 // hexdump2(buffer, sizeof(buffer)); 169 aes_cmac(res, t, buffer, sizeof(buffer)); 170 // hexdump2(res, 16); 171 // || AES-CMACT (Counter = 1 || keyID || N1 || N2|| A1|| A2 || Length = 256) -- this is the LTK 172 buffer[0] = 1; 173 // hexdump2(buffer, sizeof(buffer)); 174 aes_cmac(res+16, t, buffer, sizeof(buffer)); 175 // hexdump2(res+16, 16); 176 } 177 178 // f6(W, N1, N2, R, IOcap, A1, A2) = AES-CMACW (N1 || N2 || R || IOcap || A1 || A2 179 // - W is 128 bits 180 // - N1 is 128 bits 181 // - N2 is 128 bits 182 // - R is 128 bits 183 // - IOcap is 24 bits 184 // - A1 is 56 bits 185 // - A2 is 56 bits 186 static void f6(sm_key_t res, const sm_key_t w, const sm_key_t n1, const sm_key_t n2, const sm_key_t r, const sm_key24_t io_cap, const sm_key56_t a1, const sm_key56_t a2){ 187 uint8_t buffer[65]; 188 memcpy(buffer, n1, 16); 189 memcpy(buffer+16, n2, 16); 190 memcpy(buffer+32, r, 16); 191 memcpy(buffer+48, io_cap, 3); 192 memcpy(buffer+51, a1, 7); 193 memcpy(buffer+58, a2, 7); 194 aes_cmac(res, w, buffer,sizeof(buffer)); 195 } 196 197 // g2(U, V, X, Y) = AES-CMACX(U || V || Y) mod 2^32 198 // - U is 256 bits 199 // - V is 256 bits 200 // - X is 128 bits 201 // - Y is 128 bits 202 static uint32_t g2(const sm_key256_t u, const sm_key256_t v, const sm_key_t x, const sm_key_t y){ 203 uint8_t buffer[80]; 204 memcpy(buffer, u, 32); 205 memcpy(buffer+32, v, 32); 206 memcpy(buffer+64, y, 16); 207 sm_key_t cmac; 208 aes_cmac(cmac, x, buffer, sizeof(buffer)); 209 return big_endian_read_32(cmac, 12); 210 } 211 212 // h6(W, keyID) = AES-CMAC_W(keyID) 213 // - W is 128 bits 214 // - keyID is 32 bits 215 static void h6(sm_key_t res, const sm_key_t w, const uint32_t key_id){ 216 uint8_t key_id_buffer[4]; 217 big_endian_store_32(key_id_buffer, 0, key_id); 218 aes_cmac(res, w, key_id_buffer, 4); 219 } 220 221 // h7(SALT, W) = AES-CMAC_SALT(W) 222 // - SALT is 128 bit 223 // - W is 128 bits 224 static void h7(sm_key_t res, const sm_key_t salt, const sm_key_t w){ 225 uint8_t key_id_buffer[4]; 226 aes_cmac(res, salt, w, 16); 227 } 228 229 int main(void){ 230 sm_key_t key, k0, k1, k2, zero; 231 memset(zero, 0, 16); 232 PARSE_KEY(key); 233 234 // validate subkey k0,k1,k2 generation 235 aes128_calc_cyphertext(key, zero, k0); 236 VALIDATE_KEY(k0); 237 aes_cmac_calc_subkeys(k0, k1, k2); 238 VALIDATE_KEY(k1); 239 VALIDATE_KEY(k2); 240 241 // validate AES_CMAC for some messages 242 VALIDATE_MESSAGE(m0); 243 VALIDATE_MESSAGE(m16); 244 VALIDATE_MESSAGE(m40); 245 VALIDATE_MESSAGE(m64); 246 247 // validate f4 248 printf("-- verify f4\n"); 249 sm_key_t f4_x, f4_cmac, f4_cmac_test; 250 sm_key256_t f4_u, f4_v; 251 uint8_t f4_z; 252 parse_hex(f4_cmac, f4_cmac_string); 253 parse_hex(f4_u, f4_u_string); 254 parse_hex(f4_v, f4_v_string); 255 parse_hex(f4_x, f4_x_string); 256 parse_hex(&f4_z, f4_z_string); 257 f4(f4_cmac_test, f4_u, f4_v, f4_x, f4_z); 258 if (memcmp(f4_cmac_test, f4_cmac, 16)){ 259 printf("CMAC incorrect!\n"); 260 } else { 261 printf("CMAC correct!\n"); 262 } 263 264 // valdiate f5 265 printf("-- verify f5\n"); 266 sm_key_t f5_cmac, f5_mackey, f5_n1, f5_n2; 267 sm_key56_t f5_a1, f5_a2; 268 sm_key256_t f5_w, f5_res; 269 uint8_t f5_z; 270 parse_hex(f5_w, f5_w_string); 271 parse_hex(f5_n1, f5_n1_string); 272 parse_hex(f5_n2, f5_n2_string); 273 parse_hex(f5_a1, f5_a1_string); 274 parse_hex(f5_a2, f5_a2_string); 275 f5(f5_res, f5_w, f5_n1, f5_n2, f5_a1, f5_a2); 276 printf("MacKey:"); 277 hexdump2(f5_res, 16); 278 printf("LTK: "); 279 hexdump2(f5_res+16, 16); 280 parse_hex(f5_cmac, f5_cmac_string); 281 if (memcmp(f5_res, f5_cmac, 16)){ 282 printf("CMAC incorrect!\n"); 283 } else { 284 printf("CMAC correct!\n"); 285 } 286 287 // validate f6 288 printf("-- verify f6\n"); 289 sm_key_t f6_cmac, f6_mac_key, f6_n1, f6_n2, f6_r, f6_res; 290 sm_key24_t f6_io_cap; 291 sm_key56_t f6_a1, f6_a2; 292 uint8_t f6_z; 293 parse_hex(f6_n1, f6_n1_string); 294 parse_hex(f6_n2, f6_n2_string); 295 parse_hex(f6_a1, f6_a1_string); 296 parse_hex(f6_a2, f6_a2_string); 297 parse_hex(f6_mac_key, f6_mac_key_string); 298 parse_hex(f6_r, f6_r_string); 299 parse_hex(f6_io_cap, f6_io_cap_string); 300 f6(f6_res, f6_mac_key, f6_n1, f6_n2, f6_r, f6_io_cap, f6_a1, f6_a2); 301 hexdump2(f6_res, 16); 302 parse_hex(f6_cmac, f6_cmac_string); 303 if (memcmp(f6_res, f6_cmac, 16)){ 304 printf("CMAC incorrect!\n"); 305 } else { 306 printf("CMAC correct!\n"); 307 } 308 309 // validate g2 310 printf("-- verify g2\n"); 311 sm_key_t g2_cmac, g2_x, g2_y; 312 sm_key256_t g2_u, g2_v; 313 parse_hex(g2_x, g2_x_string); 314 parse_hex(g2_y, g2_y_string); 315 parse_hex(g2_u, g2_u_string); 316 parse_hex(g2_v, g2_v_string); 317 uint32_t g2_test = g2(g2_u, g2_v, g2_x, g2_y); 318 printf("%08x\n", g2_test); 319 uint8_t g2_res_buffer[4]; 320 parse_hex(g2_res_buffer, g2_res_string); 321 uint32_t g2_res = big_endian_read_32(g2_res_buffer, 0); 322 if (g2_test != g2_res){ 323 printf("G2 incorrect!\n"); 324 } else { 325 printf("G2 correct!\n"); 326 } 327 328 // validate h6 329 printf("-- verify h6\n"); 330 sm_key_t h6_key, h6_res, h6_cmac; 331 uint8_t h6_key_id_buffer[4]; 332 parse_hex(h6_key, h6_key_string); 333 parse_hex(h6_key_id_buffer, h6_key_id_string); 334 parse_hex(h6_cmac, h6_cmac_string); 335 uint32_t h6_key_id = big_endian_read_32(h6_key_id_buffer, 0); 336 h6(h6_res, h6_key, h6_key_id); 337 hexdump2(h6_res, 16); 338 if (memcmp(h6_res, h6_cmac, 16)){ 339 printf("CMAC incorrect!\n"); 340 } else { 341 printf("CMAC correct!\n"); 342 } 343 344 // validate h7 345 printf("-- verify h7\n"); 346 sm_key_t h7_key, h7_res, h7_cmac; 347 const uint8_t salt[16] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x74, 0x6D, 0x70, 0x31 }; // "tmp1" 348 parse_hex(h7_key, h7_key_string); 349 parse_hex(h7_cmac, h7_cmac_string); 350 h7(h7_res, salt, h7_key); 351 hexdump2(h7_res, 16); 352 if (memcmp(h7_res, h7_cmac, 16)){ 353 printf("CMAC incorrect!\n"); 354 } else { 355 printf("CMAC correct!\n"); 356 } 357 } 358