diff --git a/example/libusb/Makefile b/example/libusb/Makefile index 411024a03..b0c357453 100644 --- a/example/libusb/Makefile +++ b/example/libusb/Makefile @@ -64,14 +64,14 @@ spp_counter_ssp: ${CORE_OBJ} ${COMMON_OBJ} spp_counter_ssp.c profile.h: profile.gatt python ${BTSTACK_ROOT}/ble/compile-gatt.py $< $@ -ble_server: ${CORE_OBJ} ${COMMON_OBJ} rijndael.c ble_server.c profile.h - ${CC} ${CORE_OBJ} ${COMMON_OBJ} rijndael.c ble_server.c ${CFLAGS} ${LDFLAGS} -o $@ +ble_server: ${CORE_OBJ} ${COMMON_OBJ} ble_server.c profile.h + ${CC} ${CORE_OBJ} ${COMMON_OBJ} ble_server.c ${CFLAGS} ${LDFLAGS} -o $@ -ble_client: ${CORE_OBJ} ${COMMON_OBJ} rijndael.c ble_client.c ad_parser.c - ${CC} ${CORE_OBJ} ${COMMON_OBJ} ad_parser.c rijndael.c ble_client.c ${CFLAGS} ${LDFLAGS} -o $@ +ble_client: ${CORE_OBJ} ${COMMON_OBJ} ble_client.c ad_parser.c + ${CC} ${CORE_OBJ} ${COMMON_OBJ} ad_parser.c ble_client.c ${CFLAGS} ${LDFLAGS} -o $@ -ble_client_uart: ${CORE_OBJ} ${COMMON_OBJ} ${CC2564_OBJ} rijndael.c ble_client.c ad_parser.c - ${CC} -DHAVE_UART_CC2564 ${CORE_OBJ} ${COMMON_OBJ} ${CC2564_OBJ} ad_parser.c rijndael.c ble_client.c ${CFLAGS} ${LDFLAGS} -o $@ +ble_client_uart: ${CORE_OBJ} ${COMMON_OBJ} ${CC2564_OBJ} ble_client.c ad_parser.c + ${CC} -DHAVE_UART_CC2564 ${CORE_OBJ} ${COMMON_OBJ} ${CC2564_OBJ} ad_parser.c ble_client.c ${CFLAGS} ${LDFLAGS} -o $@ clean: rm -f spp_counter sdp_rfcomm_query sdp_general_query spp_counter_ssp ble_server ble_client diff --git a/example/libusb/ble_server.c b/example/libusb/ble_server.c index 4ba386304..412e45e3c 100644 --- a/example/libusb/ble_server.c +++ b/example/libusb/ble_server.c @@ -1697,307 +1697,6 @@ static void packet_handler (void * connection, uint8_t packet_type, uint16_t cha sm_run(); } -// aes128 c implementation only code - -static void sm_d1(key_t k, uint16_t d, uint16_t r, key_t d1){ - key_t d1_prime; - sm_d1_d_prime(d, r, d1_prime); - // d1(k,d,r) = e(k, d'), - unsigned long rk[RKLENGTH(KEYBITS)]; - int nrounds = rijndaelSetupEncrypt(rk, &k[0], KEYBITS); - rijndaelEncrypt(rk, nrounds, d1_prime, d1); -} - -static uint16_t sm_dm(key_t k, uint8_t r[8]){ - key_t r_prime; - sm_dm_r_prime(r, r_prime); - // dm(k, r) = e(k, r’) dm(k, r) = e(k, r’) - key_t dm128; - unsigned long rk[RKLENGTH(KEYBITS)]; - int nrounds = rijndaelSetupEncrypt(rk, &k[0], KEYBITS); - rijndaelEncrypt(rk, nrounds, r_prime, dm128); - - uint16_t dm = READ_NET_16(dm128, 14); - return dm; -} - -static uint16_t sm_y(key_t dhk, uint8_t rand[8]){ - // Y = dm(DHK, Rand) - return sm_dm(dhk, rand); -} - -static uint16_t sm_ediv(key_t dhk, uint8_t rand[8], uint16_t div){ - // EDIV = Y xor DIV - uint16_t y = sm_y(dhk, rand); - uint16_t ediv = y ^ div; - return ediv; -} - -static uint16_t sm_div(key_t dhk, uint8_t rand[8], uint16_t ediv){ - // DIV = Y xor EDIV - uint16_t y = sm_y(dhk, rand); - uint16_t div = y ^ ediv; - return div; -} - -static void sm_ltk(key_t er, uint16_t div, key_t ltk){ - // LTK = d1(ER, DIV, 0)) - sm_d1(er, div, 0, ltk); -} - -static void sm_csrk(key_t er, uint16_t div, key_t csrk){ - // LTK = d1(ER, DIV, 0)) - sm_d1(er, div, 1, csrk); -} - -static void sm_irk(key_t ir, key_t irk){ - // IRK = d1(IR, 1, 0) - sm_d1(ir, 1, 0, irk); -} - -static void sm_dhk(key_t ir, key_t dhk){ - // DHK = d1(IR, 3, 0) - sm_d1(ir, 3, 0, dhk); -} - -// -// Endianess: -// - preq, pres as found in SM PDUs (little endian), we flip it here -// - everything else in big endian incl. result -static void sm_c1(key_t k, key_t r, uint8_t preq[7], uint8_t pres[7], uint8_t iat, uint8_t rat, bd_addr_t ia, bd_addr_t ra, key_t c1){ - - printf("iat %u: ia ", iat); - print_bd_addr(ia); - printf("rat %u: ra ", rat); - print_bd_addr(ra); - - print_key("k", k); - - // first operation - key_t t1; - sm_c1_t1(r, preq, pres, iat, rat, t1); - - unsigned long rk[RKLENGTH(KEYBITS)]; - int nrounds = rijndaelSetupEncrypt(rk, &k[0], KEYBITS); - - // t2 = e(k, r_xor_p1) - key_t t2; - rijndaelEncrypt(rk, nrounds, t1, t2); - - print_key("t2", t2); - - // second operation - key_t t3; - sm_c1_t3(t2, ia, ra, t3); - - rijndaelEncrypt(rk, nrounds, t3, c1); - - print_key("c1", c1); -} - -static void sm_s1(key_t k, key_t r1, key_t r2, key_t s1){ - printf("sm_s1\n"); - print_key("k", k); - - key_t r_prime; - sm_s1_r_prime(r1, r2, r_prime); - - // setup aes encryption - unsigned long rk[RKLENGTH(KEYBITS)]; - int nrounds = rijndaelSetupEncrypt(rk, &k[0], KEYBITS); - rijndaelEncrypt(rk, nrounds, r_prime, s1); - print_key("s1", s1); - -} - -// test code using aes128 c implementation -static int sm_validate_m_confirm(void){ - printf("sm_validate_m_confirm\n"); - - key_t c1; - print_key("mc", sm_m_confirm); - - sm_c1(sm_tk, sm_m_random, sm_m_preq, sm_s_pres, sm_m_addr_type, sm_s_addr_type, sm_m_address, sm_s_address, c1); - - int m_confirm_valid = memcmp(c1, sm_m_confirm, 16) == 0; - printf("m_confirm_valid: %u\n", m_confirm_valid); - return m_confirm_valid; -} - -static void sm_test(){ - key_t k; - memset(k, 0, 16 ); - print_key("k", k); - - // c1 - key_t r = { 0x57, 0x83, 0xD5, 0x21, 0x56, 0xAD, 0x6F, 0x0E, 0x63, 0x88, 0x27, 0x4E, 0xC6, 0x70, 0x2E, 0xE0 }; - print_key("r", r); - - uint8_t preq[] = {0x01, 0x01, 0x00, 0x00, 0x10, 0x07, 0x07}; - uint8_t pres[] = {0x02, 0x03, 0x00, 0x00, 0x08, 0x00, 0x05}; - bd_addr_t ia = { 0xA1, 0xA2, 0xA3, 0xA4, 0xA5, 0xA6 }; - bd_addr_t ra = { 0xB1, 0xB2, 0xB3, 0xB4, 0xB5, 0xB6 }; - - key_t c1; - sm_c1(k, r, preq, pres, 1, 0, ia, ra, c1); - - // s1 - key_t s1; - key_t r1 = { 0x00, 0x0F, 0x0E, 0x0D, 0x0C, 0x0B, 0x0A, 0x09, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88}; - key_t r2 = { 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF, 0x00}; - sm_s1(k, r1, r2, s1); -} - -void sm_test2(){ - key_t k; - memset(k, 0, 16 ); - print_key("k", k); - - key_t r = { 0x55, 0x05, 0x1D, 0xF4, 0x7C, 0xC9, 0xBC, 0x97, 0x3C, 0x6A, 0x7D, 0x0D, 0x0F, 0x57, 0x0E, 0xC4 }; - print_key("r", r); - - // preq [ 01 04 00 01 10 07 07 ] - // pres [ 02 04 00 01 10 07 07 ] - - uint8_t preq[] = {0x01, 0x04, 0x00, 0x01, 0x10, 0x07, 0x07}; - uint8_t pres[] = {0x02, 0x04, 0x00, 0x01, 0x10, 0x07, 0x07}; - - // Initiator - // Peer_Address_Type: Random Device Address - // Peer_Address: 5C:49:F9:4F:1F:04 - - // Responder - // Peer_Address_Type: Public Device Address - // Peer_Address: 00:1B:DC:05:B5:DC - bd_addr_t ia = { 0x5c, 0x49, 0xf9, 0x4f, 0x1f, 0x04 }; - bd_addr_t ra = { 0x00, 0x1b, 0xdc, 0x05, 0xB5, 0xdc }; - - key_t c1; - key_t c1_true = { 0xFB, 0xAB, 0x63, 0x6F, 0xE4, 0xB4, 0xA5, 0x16, 0xAF, 0x8D, 0x88, 0xED, 0xBD, 0xB6, 0xA6, 0xFE }; - - bd_addr_t ia_le; - bd_addr_t ra_le; - bt_flip_addr(ia_le, ia); - bt_flip_addr(ra_le, ra); - - sm_c1(k, r, preq, pres, 1, 0, ia, ra, c1); - printf("Confirm value correct :%u\n", memcmp(c1, c1_true, 16) == 0); -} - - -// http://www.ietf.org/rfc/rfc4493.txt -void sm_cmac(key_t k, uint16_t message_len, uint8_t * message, key_t result){ - - // Step 1 : (K1,K2) := Generate_Subkey(K); - - // setup aes encryption - unsigned long rk[RKLENGTH(KEYBITS)]; - int nrounds = rijndaelSetupEncrypt(rk, &k[0], KEYBITS); - - key_t const_zero; - memset(const_zero, 0, 16); - - key_t l; - rijndaelEncrypt(rk, nrounds, const_zero, l); - - key_t k1; - memcpy(k1, l, 16); - sm_shift_left_by_one_bit_inplace(16, k1); - if (l[0] & 0x80){ - k1[15] ^= 0x87; - } - - key_t k2; - memcpy(k2, k1, 16); - sm_shift_left_by_one_bit_inplace(16, k2); - if (k1[0] & 0x80){ - k2[15] ^= 0x87; - } - - // print_key("k", k); - // print_key("k1", k1); - // print_key("k2", k2); - - // step 2: n := ceil(len/const_Bsize); - int n = (message_len + 15) / 16; - - // step 3: .. - int flag = (message_len > 0) && ((message_len & 0x0f) == 0); - if (n==0){ - n = 1; - } - - // printf("n %u, flag %u\n", n, flag); - - // step 4: set m_last - key_t m_last; - if (flag){ - int i; - for (i=0;i<16;i++){ - m_last[i] = message[message_len - 16 + i] ^ k1[i]; - } - } else { - int valid_octets_in_last_block = message_len & 0x0f; - int i; - for (i=0;i<16;i++){ - if (i < valid_octets_in_last_block){ - m_last[i] = message[(message_len & 0xfff0) + i] ^ k2[i]; - continue; - } - if (i == valid_octets_in_last_block){ - m_last[i] = 0x80 ^ k2[i]; - continue; - } - m_last[i] = k2[i]; - } - } - - // step 5. - key_t x; - memset(x, 0, 16); - - // print_key("x", x); - - // step 6. - int i; - for (i=0;i