From 66d168a2a3932c5e22d18081130f011067db3a1e Mon Sep 17 00:00:00 2001 From: "matthias.ringwald@gmail.com" Date: Fri, 1 Nov 2013 14:07:59 +0000 Subject: [PATCH] working test code for BLE bonding --- example/libusb/ble_server.c | 279 +++++++++++++++++++++++++++++++++++- 1 file changed, 275 insertions(+), 4 deletions(-) diff --git a/example/libusb/ble_server.c b/example/libusb/ble_server.c index afcf9e42e..b12f2a714 100644 --- a/example/libusb/ble_server.c +++ b/example/libusb/ble_server.c @@ -60,15 +60,75 @@ #include "att.h" +#include "rijndael.h" -#define FONT_HEIGHT 12 // Each character has 13 lines -#define FONT_WIDTH 8 +typedef enum { + SM_CODE_PAIRING_REQUEST = 0X01, + SM_CODE_PAIRING_RESPONSE, + SM_CODE_PAIRING_CONFIRM, + SM_CODE_PAIRING_RANDOM, + SM_CODE_PAIRING_FAILED, + SM_CODE_ENCRYPTION_INFORMATION, + SM_CODE_MASTER_IDENTIFICATION, + SM_CODE_IDENTITY_INFORMATION, + SM_CODE_IDENTITY_ADDRESS_INFORMATION, + SM_CODE_SIGNING_INFORMATION, + SM_CODE_SECURITY_REQUEST +} SECURITY_MANAGER_COMMANDS; + +// Authentication requirement flags +#define SM_AUTHREQ_NO_BONDING 0x00 +#define SM_AUTHREQ_BONDING 0x01 +#define SM_AUTHREQ_MITM_PROTECTION 0x02 + +typedef uint8_t key_t[16]; static att_connection_t att_connection; static uint16_t att_response_handle = 0; static uint16_t att_response_size = 0; static uint8_t att_response_buffer[28]; +static uint16_t sm_response_handle = 0; +static uint16_t sm_response_size = 0; +static uint8_t sm_response_buffer[28]; + +static int sm_send_security_request = 0; +static int sm_send_encryption_information = 0; +static int sm_send_master_identification = 0; +static int sm_send_identity_information = 0; +static int sm_send_identity_address_information = 0; +static int sm_send_signing_identification = 0; + +static int sm_received_encryption_information = 0; +static int sm_received_master_identification = 0; +static int sm_received_identity_information = 0; +static int sm_received_identity_address_information = 0; +static int sm_received_signing_identification = 0; + +static key_t sm_tk; +static key_t sm_m_random; +static key_t sm_m_confirm; +static uint8_t sm_preq[7]; +static uint8_t sm_pres[7]; + +// key distribution, slave sends +static key_t sm_s_ltk; +static uint16_t sm_s_ediv; +static uint8_t sm_s_rand[8]; +static uint8_t sm_s_addr_type; +static bd_addr_t sm_s_address; +static key_t sm_s_csrk; +static key_t sm_s_irk; + +// key distribution, recevied from master +static key_t sm_m_ltk; +static uint16_t sm_m_ediv; +static uint8_t sm_m_rand[8]; +static uint8_t sm_m_addr_type; +static bd_addr_t sm_m_address; +static key_t sm_m_csrk; +static key_t sm_m_irk; + static void att_try_respond(void){ if (!att_response_size) return; if (!att_response_handle) return; @@ -80,7 +140,6 @@ static void att_try_respond(void){ l2cap_send_connectionless(att_response_handle, L2CAP_CID_ATTRIBUTE_PROTOCOL, att_response_buffer, size); } - static void att_packet_handler(uint8_t packet_type, uint16_t handle, uint8_t *packet, uint16_t size){ if (packet_type != ATT_DATA_PACKET) return; @@ -89,10 +148,195 @@ static void att_packet_handler(uint8_t packet_type, uint16_t handle, uint8_t *pa att_try_respond(); } +// SECURITY MANAGER (SM) MATERIALIZES HERE + +static void sm_validate(void){ +} + +static inline void swap128(uint8_t src[16], uint8_t dst[16]){ + int i; + for (i = 0; i < 16; i++) + dst[15 - i] = src[i]; +} + +static inline void swap56(uint8_t src[7], uint8_t dst[7]){ + int i; + for (i = 0; i < 7; i++) + dst[6 - i] = src[i]; +} + +static void sm_s1(key_t k, key_t r1, key_t r2, key_t s1){ + key_t r_prime; + memcpy(&r_prime[0], r2, 8); + memcpy(&r_prime[8], r1, 8); + key_t r_flipped; + swap128(r_prime, r_flipped); + printf("r': "); hexdump(r_flipped, 16); + + key_t tk_flipped; + swap128(sm_tk, tk_flipped); + printf("tk' "); hexdump(tk_flipped, 16); + + // setup aes decryption + unsigned long rk[RKLENGTH(KEYBITS)]; + int nrounds = rijndaelSetupEncrypt(rk, &tk_flipped[0], KEYBITS); + + key_t s1_flipped; + rijndaelEncrypt(rk, nrounds, r_flipped, s1_flipped); + + printf("s1' "); hexdump(s1_flipped, 16); + + swap128(s1_flipped, s1); + printf("s1: "); hexdump(s1, 16); +} + +static void sm_run(void){ + if (!hci_can_send_packet_now(HCI_ACL_DATA_PACKET)) return; + + // send security manager packet + if (sm_response_size){ + uint16_t size = sm_response_size; + sm_response_size = 0; + l2cap_send_connectionless(sm_response_handle, L2CAP_CID_SECURITY_MANAGER_PROTOCOL, (uint8_t*) sm_response_buffer, size); + } + // send security request + if (sm_send_security_request){ + sm_send_security_request = 0; + uint8_t buffer[2]; + buffer[0] = SM_CODE_SECURITY_REQUEST; + buffer[1] = SM_AUTHREQ_BONDING; + l2cap_send_connectionless(sm_response_handle, L2CAP_CID_SECURITY_MANAGER_PROTOCOL, (uint8_t*) buffer, sizeof(buffer)); + return; + } + if (sm_send_encryption_information){ + sm_send_encryption_information = 0; + uint8_t buffer[17]; + buffer[0] = SM_CODE_ENCRYPTION_INFORMATION; + memcpy(&buffer[1], sm_s_ltk, 16); + l2cap_send_connectionless(sm_response_handle, L2CAP_CID_SECURITY_MANAGER_PROTOCOL, (uint8_t*) buffer, sizeof(buffer)); + return; + } + if (sm_send_master_identification){ + sm_send_master_identification = 0; + uint8_t buffer[11]; + buffer[0] = SM_CODE_MASTER_IDENTIFICATION; + bt_store_16(buffer, 1, sm_s_ediv); + memcpy(&buffer[3],sm_s_rand,8); + l2cap_send_connectionless(sm_response_handle, L2CAP_CID_SECURITY_MANAGER_PROTOCOL, (uint8_t*) buffer, sizeof(buffer)); + return; + } + if (sm_send_identity_information){ + sm_send_identity_information = 0; + uint8_t buffer[17]; + buffer[0] = SM_CODE_IDENTITY_INFORMATION; + memcpy(&buffer[1], sm_s_irk, 16); + l2cap_send_connectionless(sm_response_handle, L2CAP_CID_SECURITY_MANAGER_PROTOCOL, (uint8_t*) buffer, sizeof(buffer)); + return; + } + if (sm_send_identity_address_information ){ + sm_send_identity_address_information = 0; + uint8_t buffer[8]; + buffer[0] = SM_CODE_IDENTITY_ADDRESS_INFORMATION; + buffer[1] = sm_s_addr_type; + BD_ADDR_COPY(&buffer[2], sm_s_address); + l2cap_send_connectionless(sm_response_handle, L2CAP_CID_SECURITY_MANAGER_PROTOCOL, (uint8_t*) buffer, sizeof(buffer)); + return; + } + if (sm_send_signing_identification){ + sm_send_signing_identification = 0; + uint8_t buffer[17]; + buffer[0] = SM_CODE_SIGNING_INFORMATION; + memcpy(&buffer[1], sm_s_csrk, 16); + l2cap_send_connectionless(sm_response_handle, L2CAP_CID_SECURITY_MANAGER_PROTOCOL, (uint8_t*) buffer, sizeof(buffer)); + return; + } +} + +static void sm_packet_handler(uint8_t packet_type, uint16_t handle, uint8_t *packet, uint16_t size){ + + if (packet_type != SM_DATA_PACKET) return; + + printf("sm_packet_handler, request %0x\n", packet[0]); + + switch (packet[0]){ + case SM_CODE_PAIRING_REQUEST: + // for validate + memcpy(sm_preq, packet, 7); + + memcpy(sm_response_buffer, packet, size); + sm_response_buffer[0] = SM_CODE_PAIRING_RESPONSE; + // sm_response_buffer[1] = 0x00; // io capability: DisplayOnly + // sm_response_buffer[1] = 0x02; // io capability: KeyboardOnly + // sm_response_buffer[1] = 0x03; // io capability: NoInputNoOutput + sm_response_buffer[1] = 0x04; // io capability: KeyboardDisplay + sm_response_buffer[2] = 0x00; // no oob data available + sm_response_buffer[3] = sm_response_buffer[3] & 3; // remove MITM flag + sm_response_buffer[4] = 0x10; // maxium encryption key size + sm_response_size = 7; + + // for validate + memcpy(sm_pres, sm_response_buffer, 7); + break; + case SM_CODE_PAIRING_CONFIRM: + // received confirm value + memcpy(sm_m_confirm, &packet[1], 16); + sm_response_size = 17; + + // dummy + memcpy(sm_response_buffer, packet, size); + break; + case SM_CODE_PAIRING_RANDOM: + // received confirm value + memcpy(sm_m_random, &packet[1], 16); + sm_response_size = 17; + + // validate + sm_validate(); + + // + memcpy(sm_response_buffer, packet, size); + break; + + case SM_CODE_ENCRYPTION_INFORMATION: + sm_received_encryption_information = 1; + memcpy(sm_m_ltk, &packet[1], 16); + break; + + case SM_CODE_MASTER_IDENTIFICATION: + sm_received_master_identification = 1; + sm_m_ediv = READ_BT_16(packet, 1); + memcpy(sm_m_rand, &packet[3],8); + break; + + case SM_CODE_IDENTITY_INFORMATION: + sm_received_identity_information = 1; + memcpy(sm_m_irk, &packet[1], 16); + break; + + case SM_CODE_IDENTITY_ADDRESS_INFORMATION: + sm_received_identity_address_information = 1; + sm_m_addr_type = packet[1]; + BD_ADDR_COPY(sm_m_address, &packet[2]); + break; + + case SM_CODE_SIGNING_INFORMATION: + sm_received_signing_identification = 1; + memcpy(sm_m_csrk, &packet[1], 16); + break; + } + + // try to send preparared packet + sm_run(); +} + +// END OF SM // enable LE, setup ADV data static void packet_handler (void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ uint8_t adv_data[] = { 02, 01, 05, 03, 02, 0xf0, 0xff }; + + sm_run(); + switch (packet_type) { case HCI_EVENT_PACKET: @@ -113,14 +357,38 @@ static void packet_handler (void * connection, uint8_t packet_type, uint16_t cha case HCI_EVENT_LE_META: switch (packet[2]) { case HCI_SUBEVENT_LE_CONNECTION_COMPLETE: + sm_response_handle = READ_BT_16(packet, 4); + + // request security + // sm_send_security_request = 1; + // reset connection MTU att_connection.mtu = 23; break; + + case HCI_SUBEVENT_LE_LONG_TERM_KEY_REQUEST: + sm_s1(sm_tk, sm_m_random, sm_m_random, sm_s_ltk); + hci_send_cmd(&hci_le_long_term_key_request_reply, READ_BT_16(packet, 3), sm_s_ltk); + break; + default: break; } break; - + case HCI_EVENT_ENCRYPTION_CHANGE: + + // TODO: check state: + + // TODO: only send requested keys + + // distribute keys + sm_send_encryption_information = 1; + sm_send_master_identification = 1; + sm_send_identity_information = 1; + sm_send_identity_address_information = 1; + sm_send_signing_identification = 1; + break; + case HCI_EVENT_DISCONNECTION_COMPLETE: att_response_handle =0; att_response_size = 0; @@ -147,6 +415,8 @@ static void packet_handler (void * connection, uint8_t packet_type, uint16_t cha } } } + + sm_run(); } // test profile @@ -185,6 +455,7 @@ void setup(void){ // set up l2cap_le l2cap_init(); l2cap_register_fixed_channel(att_packet_handler, L2CAP_CID_ATTRIBUTE_PROTOCOL); + l2cap_register_fixed_channel(sm_packet_handler, L2CAP_CID_SECURITY_MANAGER_PROTOCOL); l2cap_register_packet_handler(packet_handler); // set up ATT