1 #include <stdio.h> 2 #include <stdint.h> 3 #include "btstack_util.h" 4 #include "aes_cmac.h" 5 #include <errno.h> 6 7 typedef uint8_t key_t[16]; 8 9 #define LOG_KEY(NAME) { printf("%16s: ", #NAME); printf_hexdump(NAME, 16); } 10 #define PARSE_KEY(NAME) { parse_hex(NAME, NAME##_string); LOG_KEY(NAME); } 11 #define DEFINE_KEY(NAME, VALUE) key_t NAME; parse_hex(NAME, VALUE); LOG_KEY(NAME); 12 13 static int parse_hex(uint8_t * buffer, const char * hex_string){ 14 int len = 0; 15 while (*hex_string){ 16 if (*hex_string == ' '){ 17 hex_string++; 18 continue; 19 } 20 int high_nibble = nibble_for_char(*hex_string++); 21 int low_nibble = nibble_for_char(*hex_string++); 22 *buffer++ = (high_nibble << 4) | low_nibble; 23 len++; 24 } 25 return len; 26 } 27 28 // CCM Encrypt & Decrypt from Zephyr Project 29 30 typedef uint8_t u8_t; 31 typedef uint16_t u16_t; 32 typedef uint64_t u64_t; 33 34 static void sys_put_be16(uint16_t value, uint8_t * buffer) { 35 big_endian_store_16(buffer, 0, value); 36 } 37 static int bt_encrypt_be(const uint8_t * key, const uint8_t * plain, uint8_t * cipher) { 38 aes128_calc_cyphertext(key, plain, cipher); 39 return 0; 40 } 41 42 static int bt_mesh_ccm_decrypt(const u8_t key[16], u8_t nonce[13], 43 const u8_t *enc_msg, size_t msg_len, 44 const u8_t *aad, size_t aad_len, 45 u8_t *out_msg, size_t mic_size) 46 { 47 u8_t msg[16], pmsg[16], cmic[16], cmsg[16], Xn[16], mic[16]; 48 u16_t last_blk, blk_cnt; 49 size_t i, j; 50 int err; 51 52 if (msg_len < 1 || aad_len >= 0xff00) { 53 return -EINVAL; 54 } 55 56 /* C_mic = e(AppKey, 0x01 || nonce || 0x0000) */ 57 pmsg[0] = 0x01; 58 memcpy(pmsg + 1, nonce, 13); 59 sys_put_be16(0x0000, pmsg + 14); 60 61 err = bt_encrypt_be(key, pmsg, cmic); 62 if (err) { 63 return err; 64 } 65 66 /* X_0 = e(AppKey, 0x09 || nonce || length) */ 67 if (mic_size == sizeof(u64_t)) { 68 pmsg[0] = 0x19 | (aad_len ? 0x40 : 0x00); 69 } else { 70 pmsg[0] = 0x09 | (aad_len ? 0x40 : 0x00); 71 } 72 73 memcpy(pmsg + 1, nonce, 13); 74 sys_put_be16(msg_len, pmsg + 14); 75 76 err = bt_encrypt_be(key, pmsg, Xn); 77 if (err) { 78 return err; 79 } 80 81 /* If AAD is being used to authenticate, include it here */ 82 if (aad_len) { 83 sys_put_be16(aad_len, pmsg); 84 85 for (i = 0; i < sizeof(u16_t); i++) { 86 pmsg[i] = Xn[i] ^ pmsg[i]; 87 } 88 89 j = 0; 90 aad_len += sizeof(u16_t); 91 while (aad_len > 16) { 92 do { 93 pmsg[i] = Xn[i] ^ aad[j]; 94 i++, j++; 95 } while (i < 16); 96 97 aad_len -= 16; 98 i = 0; 99 100 err = bt_encrypt_be(key, pmsg, Xn); 101 if (err) { 102 return err; 103 } 104 } 105 106 for (i = 0; i < aad_len; i++, j++) { 107 pmsg[i] = Xn[i] ^ aad[j]; 108 } 109 110 for (i = aad_len; i < 16; i++) { 111 pmsg[i] = Xn[i]; 112 } 113 114 err = bt_encrypt_be(key, pmsg, Xn); 115 if (err) { 116 return err; 117 } 118 } 119 120 last_blk = msg_len % 16; 121 blk_cnt = (msg_len + 15) / 16; 122 if (!last_blk) { 123 last_blk = 16; 124 } 125 126 for (j = 0; j < blk_cnt; j++) { 127 if (j + 1 == blk_cnt) { 128 /* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */ 129 pmsg[0] = 0x01; 130 memcpy(pmsg + 1, nonce, 13); 131 sys_put_be16(j + 1, pmsg + 14); 132 133 err = bt_encrypt_be(key, pmsg, cmsg); 134 if (err) { 135 return err; 136 } 137 138 /* Encrypted = Payload[0-15] ^ C_1 */ 139 for (i = 0; i < last_blk; i++) { 140 msg[i] = enc_msg[(j * 16) + i] ^ cmsg[i]; 141 } 142 143 memcpy(out_msg + (j * 16), msg, last_blk); 144 145 /* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */ 146 for (i = 0; i < last_blk; i++) { 147 pmsg[i] = Xn[i] ^ msg[i]; 148 } 149 150 for (i = last_blk; i < 16; i++) { 151 pmsg[i] = Xn[i] ^ 0x00; 152 } 153 154 err = bt_encrypt_be(key, pmsg, Xn); 155 if (err) { 156 return err; 157 } 158 159 /* MIC = C_mic ^ X_1 */ 160 for (i = 0; i < sizeof(mic); i++) { 161 mic[i] = cmic[i] ^ Xn[i]; 162 } 163 } else { 164 /* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */ 165 pmsg[0] = 0x01; 166 memcpy(pmsg + 1, nonce, 13); 167 sys_put_be16(j + 1, pmsg + 14); 168 169 err = bt_encrypt_be(key, pmsg, cmsg); 170 if (err) { 171 return err; 172 } 173 174 /* Encrypted = Payload[0-15] ^ C_1 */ 175 for (i = 0; i < 16; i++) { 176 msg[i] = enc_msg[(j * 16) + i] ^ cmsg[i]; 177 } 178 179 memcpy(out_msg + (j * 16), msg, 16); 180 181 /* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */ 182 for (i = 0; i < 16; i++) { 183 pmsg[i] = Xn[i] ^ msg[i]; 184 } 185 186 err = bt_encrypt_be(key, pmsg, Xn); 187 if (err) { 188 return err; 189 } 190 } 191 } 192 193 if (memcmp(mic, enc_msg + msg_len, mic_size)) { 194 return -EBADMSG; 195 } 196 197 return 0; 198 } 199 static int bt_mesh_ccm_encrypt(const u8_t key[16], u8_t nonce[13], 200 const u8_t *msg, size_t msg_len, 201 const u8_t *aad, size_t aad_len, 202 u8_t *out_msg, size_t mic_size) 203 { 204 u8_t pmsg[16], cmic[16], cmsg[16], mic[16], Xn[16]; 205 u16_t blk_cnt, last_blk; 206 size_t i, j; 207 int err; 208 209 // BT_DBG("key %s", bt_hex(key, 16)); 210 // BT_DBG("nonce %s", bt_hex(nonce, 13)); 211 // BT_DBG("msg (len %zu) %s", msg_len, bt_hex(msg, msg_len)); 212 // BT_DBG("aad_len %zu mic_size %zu", aad_len, mic_size); 213 214 /* Unsupported AAD size */ 215 if (aad_len >= 0xff00) { 216 return -EINVAL; 217 } 218 219 /* C_mic = e(AppKey, 0x01 || nonce || 0x0000) */ 220 pmsg[0] = 0x01; 221 memcpy(pmsg + 1, nonce, 13); 222 sys_put_be16(0x0000, pmsg + 14); 223 224 err = bt_encrypt_be(key, pmsg, cmic); 225 if (err) { 226 return err; 227 } 228 229 /* X_0 = e(AppKey, 0x09 || nonce || length) */ 230 if (mic_size == sizeof(u64_t)) { 231 pmsg[0] = 0x19 | (aad_len ? 0x40 : 0x00); 232 } else { 233 pmsg[0] = 0x09 | (aad_len ? 0x40 : 0x00); 234 } 235 236 memcpy(pmsg + 1, nonce, 13); 237 sys_put_be16(msg_len, pmsg + 14); 238 239 err = bt_encrypt_be(key, pmsg, Xn); 240 if (err) { 241 return err; 242 } 243 244 /* If AAD is being used to authenticate, include it here */ 245 if (aad_len) { 246 sys_put_be16(aad_len, pmsg); 247 248 for (i = 0; i < sizeof(u16_t); i++) { 249 pmsg[i] = Xn[i] ^ pmsg[i]; 250 } 251 252 j = 0; 253 aad_len += sizeof(u16_t); 254 while (aad_len > 16) { 255 do { 256 pmsg[i] = Xn[i] ^ aad[j]; 257 i++, j++; 258 } while (i < 16); 259 260 aad_len -= 16; 261 i = 0; 262 263 err = bt_encrypt_be(key, pmsg, Xn); 264 if (err) { 265 return err; 266 } 267 } 268 269 for (i = 0; i < aad_len; i++, j++) { 270 pmsg[i] = Xn[i] ^ aad[j]; 271 } 272 273 for (i = aad_len; i < 16; i++) { 274 pmsg[i] = Xn[i]; 275 } 276 277 err = bt_encrypt_be(key, pmsg, Xn); 278 if (err) { 279 return err; 280 } 281 } 282 283 last_blk = msg_len % 16; 284 blk_cnt = (msg_len + 15) / 16; 285 if (!last_blk) { 286 last_blk = 16; 287 } 288 289 for (j = 0; j < blk_cnt; j++) { 290 if (j + 1 == blk_cnt) { 291 /* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */ 292 for (i = 0; i < last_blk; i++) { 293 pmsg[i] = Xn[i] ^ msg[(j * 16) + i]; 294 } 295 for (i = last_blk; i < 16; i++) { 296 pmsg[i] = Xn[i] ^ 0x00; 297 } 298 299 err = bt_encrypt_be(key, pmsg, Xn); 300 if (err) { 301 return err; 302 } 303 304 /* MIC = C_mic ^ X_1 */ 305 for (i = 0; i < sizeof(mic); i++) { 306 mic[i] = cmic[i] ^ Xn[i]; 307 } 308 309 /* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */ 310 pmsg[0] = 0x01; 311 memcpy(pmsg + 1, nonce, 13); 312 sys_put_be16(j + 1, pmsg + 14); 313 314 err = bt_encrypt_be(key, pmsg, cmsg); 315 if (err) { 316 return err; 317 } 318 319 /* Encrypted = Payload[0-15] ^ C_1 */ 320 for (i = 0; i < last_blk; i++) { 321 out_msg[(j * 16) + i] = 322 msg[(j * 16) + i] ^ cmsg[i]; 323 } 324 } else { 325 /* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */ 326 for (i = 0; i < 16; i++) { 327 pmsg[i] = Xn[i] ^ msg[(j * 16) + i]; 328 } 329 330 err = bt_encrypt_be(key, pmsg, Xn); 331 if (err) { 332 return err; 333 } 334 335 /* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */ 336 pmsg[0] = 0x01; 337 memcpy(pmsg + 1, nonce, 13); 338 sys_put_be16(j + 1, pmsg + 14); 339 340 err = bt_encrypt_be(key, pmsg, cmsg); 341 if (err) { 342 return err; 343 } 344 345 /* Encrypted = Payload[0-15] ^ C_N */ 346 for (i = 0; i < 16; i++) { 347 out_msg[(j * 16) + i] = 348 msg[(j * 16) + i] ^ cmsg[i]; 349 } 350 351 } 352 } 353 354 memcpy(out_msg + msg_len, mic, mic_size); 355 356 return 0; 357 } 358 359 static void message_24(void){ 360 DEFINE_KEY(encryption_key, "0953fa93e7caac9638f58820220a398e"); 361 362 uint8_t network_nonce[13]; 363 parse_hex(network_nonce, "000307080d1234000012345677"); 364 printf("%16s: ", "network_nonce"); printf_hexdump(network_nonce, 13); 365 366 uint8_t plaintext[18]; 367 parse_hex(plaintext, "9736e6a03401de1547118463123e5f6a17b9"); 368 printf("%16s: ", "plaintext"); printf_hexdump(plaintext, sizeof(plaintext)); 369 370 uint8_t ciphertext[18+4]; 371 bt_mesh_ccm_encrypt(encryption_key, network_nonce, plaintext, sizeof(plaintext), NULL, 0, ciphertext, 4); 372 printf("%16s: ", "ciphertext"); printf_hexdump(ciphertext, 18); 373 printf("%16s: ", "NetMIC"); printf_hexdump(&ciphertext[18], 4); 374 } 375 376 int main(void){ 377 message_24(); 378 return 0; 379 } 380