mirror/btstack
1
0
Fork 0
mirror of https://github.com/bluekitchen/btstack.git synced 2025-03-14 01:27:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of https://github.com/bluekitchen/btstack

Browse Source
This commit is contained in:
Milanka Ringwald 2015-08-05 17:05:22 +02:00
commit 8240f3cf00
18 changed files with 454 additions and 208 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ble/le_device_db_memory.c
View File

@ -130,6 +130,7 @@ void le_device_db_info(int index, int * addr_type, bd_addr_t addr, sm_key_t irk)
* @brief ltk
*/
void le_device_db_encryption_set(int index, uint16_t ediv, uint8_t rand[8], sm_key_t ltk){
log_info("Central Device DB set encryption for %u, ediv x%04x", index, ediv);
le_devices[index].ediv = ediv;
if (rand) memcpy(le_devices[index].rand, rand, 8);
if (ltk) memcpy(le_devices[index].ltk, ltk, 16);
@ -143,6 +144,7 @@ void le_device_db_encryption_set(int index, uint16_t ediv, uint8_t rand[8], sm_k
* @brief ltk
*/
void le_device_db_encryption_get(int index, uint16_t * ediv, uint8_t rand[8], sm_key_t ltk){
log_info("Central Device DB encryption for %u, ediv x%04x", le_devices[index].ediv);
if (ediv) *ediv = le_devices[index].ediv;
if (rand) memcpy(rand, le_devices[index].rand, 8);
if (ltk) memcpy(ltk, le_devices[index].ltk, 16);

228
ble/sm.c
View File

@ -39,6 +39,8 @@
#include <string.h>
#include <btstack/linked_list.h>
#include "btstack_memory.h"
#include "debug.h"
#include "hci.h"
#include "l2cap.h"
@ -100,6 +102,16 @@ typedef enum {
SM_AES128_ACTIVE
} sm_aes128_state_t;
typedef enum {
ADDRESS_RESOLUTION_IDLE,
ADDRESS_RESOLUTION_GENERAL,
ADDRESS_RESOLUTION_FOR_CONNECTION,
} address_resolution_mode_t;
typedef enum {
ADDRESS_RESOLUTION_SUCEEDED,
ADDRESS_RESOLUTION_FAILED,
} address_resolution_event_t;
//
// GLOBAL DATA
//
@ -111,7 +123,6 @@ static uint8_t sm_min_encryption_key_size;
static uint8_t sm_auth_req = 0;
static uint8_t sm_io_capabilities = IO_CAPABILITY_NO_INPUT_NO_OUTPUT;
static uint8_t sm_slave_request_security;
static uint8_t sm_authenticate_outgoing_connections = 0; // might go away
// Security Manager Master Keys, please use sm_set_er(er) and sm_set_ir(ir) with your own 128 bit random values
static sm_key_t sm_persistent_er;
@ -144,11 +155,12 @@ static void (*sm_cmac_done_handler)(uint8_t hash[8]);
// resolvable private address lookup / CSRK calculation
static int sm_address_resolution_test;
static int sm_address_resolution_matched;
static int sm_address_resolution_ah_calculation_active;
static uint8_t sm_address_resolution_addr_type;
static bd_addr_t sm_address_resolution_address;
static void * sm_address_resolution_context;
static address_resolution_mode_t sm_address_resolution_mode;
static linked_list_t sm_address_resolution_general_queue;
// aes128 crypto engine. store current sm_connection_t in sm_aes128_context
static sm_aes128_state_t sm_aes128_state;
@ -539,21 +551,38 @@ static void sm_setup_key_distribution(uint8_t key_set){
// CSRK Key Lookup
static void sm_address_resolution_start_lookup(uint8_t addr_type, bd_addr_t addr, void * context){
static int sm_address_resolution_idle(void){
return sm_address_resolution_mode == ADDRESS_RESOLUTION_IDLE;
}
static void sm_address_resolution_start_lookup(uint8_t addr_type, bd_addr_t addr, address_resolution_mode_t mode, void * context){
memcpy(sm_address_resolution_address, addr, 6);
sm_address_resolution_addr_type = addr_type;
sm_address_resolution_test = 0;
sm_address_resolution_matched = -1;
sm_address_resolution_mode = mode;
sm_address_resolution_context = context;
sm_notify_client(SM_IDENTITY_RESOLVING_STARTED, addr_type, addr, 0, 0);
}
void sm_address_resolution_lookup(uint8_t addr_type, bd_addr_t addr){
sm_address_resolution_start_lookup(addr_type, addr, NULL);
}
int sm_address_resolution_idle(void){
return sm_address_resolution_test < 0;
int sm_address_resolution_lookup(uint8_t address_type, bd_addr_t address){
// check if already in list
linked_list_iterator_t it;
sm_lookup_entry_t * entry;
linked_list_iterator_init(&it, &sm_address_resolution_general_queue);
while(linked_list_iterator_has_next(&it)){
entry = (sm_lookup_entry_t *) linked_list_iterator_next(&it);
if (entry->address_type != address_type) continue;
if (memcmp(entry->address, address, 6)) continue;
// already in list
return BTSTACK_BUSY;
}
entry = btstack_memory_sm_lookup_entry_get();
if (!entry) return BTSTACK_MEMORY_ALLOC_FAILED;
entry->address_type = (bd_addr_type_t) address_type;
memcpy(entry->address, address, 6);
linked_list_add(&sm_address_resolution_general_queue, (linked_item_t *) entry);
sm_run();
return 0;
}
// CMAC Implementation using AES128 engine
@ -731,17 +760,17 @@ static void sm_trigger_user_response(sm_connection_t * sm_conn){
case PK_RESP_INPUT:
if (sm_conn->sm_role){
setup->sm_user_response = SM_USER_RESPONSE_PENDING;
sm_notify_client(SM_PASSKEY_INPUT_NUMBER, setup->sm_m_addr_type, setup->sm_m_address, 0, 0);
sm_notify_client(SM_PASSKEY_INPUT_NUMBER, sm_conn->sm_peer_addr_type, sm_conn->sm_peer_address, 0, 0);
} else {
sm_notify_client(SM_PASSKEY_DISPLAY_NUMBER, setup->sm_m_addr_type, setup->sm_m_address, READ_NET_32(setup->sm_tk, 12), 0);
sm_notify_client(SM_PASSKEY_DISPLAY_NUMBER, sm_conn->sm_peer_addr_type, sm_conn->sm_peer_address, READ_NET_32(setup->sm_tk, 12), 0);
}
break;
case PK_INIT_INPUT:
if (sm_conn->sm_role){
sm_notify_client(SM_PASSKEY_DISPLAY_NUMBER, setup->sm_m_addr_type, setup->sm_m_address, READ_NET_32(setup->sm_tk, 12), 0);
sm_notify_client(SM_PASSKEY_DISPLAY_NUMBER, sm_conn->sm_peer_addr_type, sm_conn->sm_peer_address, READ_NET_32(setup->sm_tk, 12), 0);
} else {
setup->sm_user_response = SM_USER_RESPONSE_PENDING;
sm_notify_client(SM_PASSKEY_INPUT_NUMBER, setup->sm_m_addr_type, setup->sm_m_address, 0, 0);
sm_notify_client(SM_PASSKEY_INPUT_NUMBER, sm_conn->sm_peer_addr_type, sm_conn->sm_peer_address, 0, 0);
}
break;
case JUST_WORKS:
@ -749,7 +778,7 @@ static void sm_trigger_user_response(sm_connection_t * sm_conn){
case IO_CAPABILITY_KEYBOARD_DISPLAY:
case IO_CAPABILITY_DISPLAY_YES_NO:
setup->sm_user_response = SM_USER_RESPONSE_PENDING;
sm_notify_client(SM_JUST_WORKS_REQUEST, setup->sm_m_addr_type, setup->sm_m_address, READ_NET_32(setup->sm_tk, 12), 0);
sm_notify_client(SM_JUST_WORKS_REQUEST, sm_conn->sm_peer_addr_type, sm_conn->sm_peer_address, READ_NET_32(setup->sm_tk, 12), 0);
break;
default:
// cannot ask user
@ -852,6 +881,64 @@ static int sm_stk_generation_init(sm_connection_t * sm_conn){
return 0;
}
static void sm_address_resolution_handle_event(address_resolution_event_t event){
// cache and reset context
int matched_device_id = sm_address_resolution_test;
address_resolution_mode_t mode = sm_address_resolution_mode;
void * context = sm_address_resolution_context;
// reset context
sm_address_resolution_mode = ADDRESS_RESOLUTION_IDLE;
sm_address_resolution_context = NULL;
sm_address_resolution_test = -1;
sm_connection_t * sm_connection;
switch (mode){
case ADDRESS_RESOLUTION_GENERAL:
switch (event){
case ADDRESS_RESOLUTION_SUCEEDED:
// sm_address_resolution_succeeded_for_advertisement();
break;
case ADDRESS_RESOLUTION_FAILED:
// sm_address_resolution_succeeded_for_advertisement();
break;
}
break;
case ADDRESS_RESOLUTION_FOR_CONNECTION:
sm_connection = (sm_connection_t *) context;
switch (event){
case ADDRESS_RESOLUTION_SUCEEDED:
// use stored LTK/EDIV/RAND if we're master and have a valid ediv/random/ltk combination
sm_connection->sm_le_db_index = matched_device_id;
if (sm_connection->sm_role == 0){
uint16_t ediv;
le_device_db_encryption_get(sm_connection->sm_le_db_index, &ediv, NULL, NULL);
if (ediv){
log_info("sm: Setting up previous ltk/ediv/rand for device index %u", sm_connection->sm_le_db_index);
sm_connection->sm_engine_state = SM_INITIATOR_PH0_HAS_LTK;
}
}
break;
case ADDRESS_RESOLUTION_FAILED:
break;
}
sm_connection->sm_csrk_lookup_state = CSRK_LOOKUP_IDLE;
break;
default:
break;
}
switch (event){
case ADDRESS_RESOLUTION_SUCEEDED:
sm_notify_client(SM_IDENTITY_RESOLVING_SUCCEEDED, sm_address_resolution_addr_type, sm_address_resolution_address, 0, matched_device_id);
break;
case ADDRESS_RESOLUTION_FAILED:
sm_notify_client(SM_IDENTITY_RESOLVING_FAILED, sm_address_resolution_addr_type, sm_address_resolution_address, 0, 0);
break;
}
}
static void sm_run(void){
linked_list_iterator_t it;
@ -938,15 +1025,25 @@ static void sm_run(void){
sm_connection_t * sm_connection = &hci_connection->sm_connection;
if (sm_connection->sm_csrk_lookup_state == CSRK_LOOKUP_W4_READY){
// and start lookup
sm_address_resolution_start_lookup(sm_connection->sm_peer_addr_type, sm_connection->sm_peer_address, sm_connection);
sm_address_resolution_start_lookup(sm_connection->sm_peer_addr_type, sm_connection->sm_peer_address, ADDRESS_RESOLUTION_FOR_CONNECTION, sm_connection);
sm_connection->sm_csrk_lookup_state = CSRK_LOOKUP_STARTED;
break;
}
}
}
// -- if csrk lookup ready, resolved addresses for received addresses
if (sm_address_resolution_idle()) {
if (!linked_list_empty(&sm_address_resolution_general_queue)){
sm_lookup_entry_t * entry = (sm_lookup_entry_t *) sm_address_resolution_general_queue;
linked_list_remove(&sm_address_resolution_general_queue, (linked_item_t *) entry);
sm_address_resolution_start_lookup(entry->address_type, entry->address, ADDRESS_RESOLUTION_GENERAL, NULL);
btstack_memory_sm_lookup_entry_free(entry);
}
}
// -- Continue with CSRK device lookup by public or resolvable private address
if (sm_address_resolution_test >= 0){
if (!sm_address_resolution_idle()){
log_info("LE Device Lookup: device %u/%u", sm_address_resolution_test, le_device_db_count());
while (sm_address_resolution_test < le_device_db_count()){
int addr_type;
@ -957,24 +1054,7 @@ static void sm_run(void){
if (sm_address_resolution_addr_type == addr_type && memcmp(addr, sm_address_resolution_address, 6) == 0){
log_info("LE Device Lookup: found CSRK by { addr_type, address} ");
sm_connection_t * sm_csrk_connection = (sm_connection_t *) sm_address_resolution_context;
sm_address_resolution_context = NULL;
sm_address_resolution_matched = sm_address_resolution_test;
sm_address_resolution_test = -1;
sm_notify_client(SM_IDENTITY_RESOLVING_SUCCEEDED, sm_address_resolution_addr_type, sm_address_resolution_address, 0, sm_address_resolution_matched);
// if context given, we're resolving an active connection
if (!sm_csrk_connection) break;
// re-use stored LTK/EDIV/RAND if requested & we're master
// TODO: replace global with flag in sm_connection_t
if (sm_authenticate_outgoing_connections && sm_csrk_connection->sm_role == 0){
sm_csrk_connection->sm_engine_state = SM_INITIATOR_PH0_HAS_LTK;
log_info("sm: Setting up previous ltk/ediv/rand");
}
// ready for other requests
sm_csrk_connection->sm_csrk_lookup_state = CSRK_LOOKUP_IDLE;
sm_address_resolution_handle_event(ADDRESS_RESOLUTION_SUCEEDED);
break;
}
@ -997,14 +1077,7 @@ static void sm_run(void){
if (sm_address_resolution_test >= le_device_db_count()){
log_info("LE Device Lookup: not found");
sm_connection_t * sm_csrk_connection = (sm_connection_t *) sm_address_resolution_context;
sm_address_resolution_context = NULL;
sm_address_resolution_test = -1;
sm_notify_client(SM_IDENTITY_RESOLVING_FAILED, sm_address_resolution_addr_type, sm_address_resolution_address, 0, 0);
if (sm_address_resolution_context) {
sm_csrk_connection->sm_csrk_lookup_state = CSRK_LOOKUP_IDLE;
}
sm_address_resolution_handle_event(ADDRESS_RESOLUTION_FAILED);
}
}
@ -1101,10 +1174,11 @@ static void sm_run(void){
sm_key_t plaintext;
log_info("sm_run: state %u", connection->sm_engine_state);
// responding state
switch (connection->sm_engine_state){
// general
case SM_GENERAL_SEND_PAIRING_FAILED: {
uint8_t buffer[2];
@ -1121,7 +1195,10 @@ static void sm_run(void){
sm_key_t peer_ltk_flipped;
swap128(setup->sm_peer_ltk, peer_ltk_flipped);
connection->sm_engine_state = SM_INITIATOR_PH0_W4_CONNECTION_ENCRYPTED;
hci_send_cmd(&hci_le_start_encryption, connection->sm_handle, setup->sm_peer_rand, setup->sm_peer_ediv, peer_ltk_flipped);
log_info("sm: hci_le_start_encryption ediv 0x%04x", setup->sm_peer_ediv);
uint32_t rand_high = READ_NET_32(setup->sm_peer_rand, 0);
uint32_t rand_low = READ_NET_32(setup->sm_peer_rand, 4);
hci_send_cmd(&hci_le_start_encryption, connection->sm_handle,rand_low, rand_high, setup->sm_peer_ediv, peer_ltk_flipped);
return;
}
@ -1365,20 +1442,15 @@ static void sm_handle_encryption_result(uint8_t * data){
uint8_t hash[3];
swap24(data, hash);
if (memcmp(&sm_address_resolution_address[3], hash, 3) == 0){
// found
sm_address_resolution_matched = sm_address_resolution_test;
sm_address_resolution_test = -1;
sm_connection_t * sm_csrk_connection = (sm_connection_t *) sm_address_resolution_context;
sm_address_resolution_context = NULL;
sm_csrk_connection->sm_csrk_lookup_state = CSRK_LOOKUP_IDLE;
sm_notify_client(SM_IDENTITY_RESOLVING_SUCCEEDED, sm_address_resolution_addr_type, sm_address_resolution_address, 0, sm_address_resolution_matched);
log_info("LE Device Lookup: matched resolvable private address");
sm_address_resolution_handle_event(ADDRESS_RESOLUTION_SUCEEDED);
return;
}
// no match
// no match, try next
sm_address_resolution_test++;
return;
}
switch (dkg_state){
case DKG_W4_IRK:
swap128(data, sm_persistent_irk);
@ -1701,6 +1773,7 @@ static void sm_event_packet_handler (uint8_t packet_type, uint16_t channel, uint
sm_conn->sm_connection_encrypted = packet[5];
log_info("Encryption state change: %u, key size %u", sm_conn->sm_connection_encrypted,
sm_conn->sm_actual_encryption_key_size);
log_info("event handler, state %u", sm_conn->sm_engine_state);
if (!sm_conn->sm_connection_encrypted) break;
// continue if part of initial pairing
switch (sm_conn->sm_engine_state){
@ -1710,8 +1783,10 @@ static void sm_event_packet_handler (uint8_t packet_type, uint16_t channel, uint
break;
case SM_PH2_W4_CONNECTION_ENCRYPTED:
if (sm_conn->sm_role){
// slave
sm_conn->sm_engine_state = SM_PH3_GET_RANDOM;
} else {
// master
sm_conn->sm_engine_state = SM_PH3_RECEIVE_KEYS;
}
break;
@ -1886,7 +1961,7 @@ static void sm_packet_handler(uint8_t packet_type, uint16_t handle, uint8_t *pac
// notify client to hide shown passkey
if (setup->sm_stk_generation_method == PK_INIT_INPUT){
sm_notify_client(SM_PASSKEY_DISPLAY_CANCEL, setup->sm_m_addr_type, setup->sm_m_address, 0, 0);
sm_notify_client(SM_PASSKEY_DISPLAY_CANCEL, sm_conn->sm_peer_addr_type, sm_conn->sm_peer_address, 0, 0);
}
// handle user cancel pairing?
@ -1953,24 +2028,41 @@ static void sm_packet_handler(uint8_t packet_type, uint16_t handle, uint8_t *pac
// done with key distribution?
if (sm_key_distribution_all_received(sm_conn)){
// store, if: it's a public address, or, we got an IRK
if (setup->sm_peer_addr_type == 0 || (setup->sm_key_distribution_received_set & SM_KEYDIST_FLAG_IDENTITY_INFORMATION)) {
sm_conn->sm_le_db_index = le_device_db_add(setup->sm_peer_addr_type, setup->sm_peer_address, setup->sm_peer_irk);
}
sm_conn->sm_le_db_index = -1;
if (setup->sm_key_distribution_received_set & SM_KEYDIST_FLAG_IDENTITY_INFORMATION){
// lookup device based on IRK
int i;
for (i=0; i < le_device_db_count(); i++){
sm_key_t irk;
bd_addr_t address;
int address_type;
le_device_db_info(i, &address_type, address, irk);
if (memcmp(irk, setup->sm_peer_irk, 16) == 0){
log_info("sm: device found for IRK, updating");
sm_conn->sm_le_db_index = i;
break;
}
}
// if not found, add to db
if (sm_conn->sm_le_db_index < 0) {
sm_conn->sm_le_db_index = le_device_db_add(setup->sm_peer_addr_type, setup->sm_peer_address, setup->sm_peer_irk);
}
}
if (sm_conn->sm_le_db_index >= 0){
le_device_db_local_counter_set(sm_conn->sm_le_db_index, 0);
// store CSRK
if (setup->sm_key_distribution_received_set & SM_KEYDIST_FLAG_SIGNING_IDENTIFICATION){
log_info("sm: set csrk");
le_device_db_csrk_set(sm_conn->sm_le_db_index, setup->sm_peer_csrk);
le_device_db_remote_counter_set(sm_conn->sm_le_db_index, 0);
}
// store encryption information as Central
if (sm_conn->sm_role == 0
&& setup->sm_key_distribution_received_set & SM_KEYDIST_FLAG_ENCRYPTION_INFORMATION
// store encryption information
if (setup->sm_key_distribution_received_set & SM_KEYDIST_FLAG_ENCRYPTION_INFORMATION
&& setup->sm_key_distribution_received_set & SM_KEYDIST_FLAG_MASTER_IDENTIFICATION){
log_info("sm: set encryption information");
le_device_db_encryption_set(sm_conn->sm_le_db_index, setup->sm_peer_ediv, setup->sm_peer_rand, setup->sm_peer_ltk);
}
}
@ -2078,7 +2170,9 @@ void sm_init(void){
sm_aes128_state = SM_AES128_IDLE;
sm_address_resolution_test = -1; // no private address to resolve yet
sm_address_resolution_ah_calculation_active = 0;
sm_address_resolution_mode = ADDRESS_RESOLUTION_IDLE;
sm_address_resolution_general_queue = NULL;
gap_random_adress_update_period = 15 * 60 * 1000L;
sm_active_connection = 0;
@ -2131,12 +2225,8 @@ void sm_request_authorization(uint8_t addr_type, bd_addr_t address){
if (sm_conn->sm_role){
// code has no effect so far
sm_conn->sm_connection_authorization_state = AUTHORIZATION_PENDING;
sm_notify_client(SM_AUTHORIZATION_REQUEST, setup->sm_m_addr_type, setup->sm_m_address, 0, 0);
sm_notify_client(SM_AUTHORIZATION_REQUEST, sm_conn->sm_peer_addr_type, sm_conn->sm_peer_address, 0, 0);
} else {
// HACK
sm_authenticate_outgoing_connections = 1;
// used as a trigger to start central/master/initiator security procedures
if (sm_conn->sm_engine_state == SM_INITIATOR_CONNECTED){
sm_conn->sm_engine_state = SM_INITIATOR_PH1_W2_SEND_PAIRING_REQUEST;
@ -2150,14 +2240,14 @@ void sm_authorization_decline(uint8_t addr_type, bd_addr_t address){
sm_connection_t * sm_conn = sm_get_connection(addr_type, address);
if (!sm_conn) return; // wrong connection
sm_conn->sm_connection_authorization_state = AUTHORIZATION_DECLINED;
sm_notify_client_authorization(SM_AUTHORIZATION_RESULT, setup->sm_m_addr_type, setup->sm_m_address, 0);
sm_notify_client_authorization(SM_AUTHORIZATION_RESULT, sm_conn->sm_peer_addr_type, sm_conn->sm_peer_address, 0);
}
void sm_authorization_grant(uint8_t addr_type, bd_addr_t address){
sm_connection_t * sm_conn = sm_get_connection(addr_type, address);
if (!sm_conn) return; // wrong connection
sm_conn->sm_connection_authorization_state = AUTHORIZATION_GRANTED;
sm_notify_client_authorization(SM_AUTHORIZATION_RESULT, setup->sm_m_addr_type, setup->sm_m_address, 1);
sm_notify_client_authorization(SM_AUTHORIZATION_RESULT, sm_conn->sm_peer_addr_type, sm_conn->sm_peer_address, 1);
}
// GAP Bonding API

16
ble/sm.h
View File

@ -110,6 +110,13 @@ typedef enum {
// Only for PTS testing
void sm_test_set_irk(sm_key_t irk);
typedef struct {
linked_item_t item;
bd_addr_t address;
bd_addr_type_t address_type;
} sm_lookup_entry_t;
/* API_START */
/**
@ -256,17 +263,12 @@ void sm_authorization_grant(uint8_t addr_type, bd_addr_t address);
int sm_cmac_ready(void);
void sm_cmac_start(sm_key_t k, uint16_t message_len, uint8_t * message, uint32_t sign_counter, void (*done_handler)(uint8_t hash[8]));
/**
* @brief Address Resolution Engine idle
* @return 1 if idle, 0 otherwise
*/
int sm_address_resolution_idle(void);
/*
* @brief Match address against bonded devices
* @return 0 if successfully added to lookup queue
* @note Triggers SM_IDENTITY_RESOLVING_* events
*/
void sm_address_resolution_lookup(uint8_t addr_type, bd_addr_t addr);
int sm_address_resolution_lookup(uint8_t addr_type, bd_addr_t addr);
/**
* @brief Identify device in LE Device DB.

2
example/embedded/gap_le_advertisements.c
View File

@ -241,7 +241,7 @@ static void handle_hci_event(uint8_t packet_type, uint8_t *packet, uint16_t size
uint8_t event_type = packet[pos++];
uint8_t address_type = packet[pos++];
bd_addr_t address;
memcpy(address, &packet[pos], 6);
bt_flip_addr(address, &packet[pos]);
pos += 6;
uint8_t rssi = packet[pos++];
uint8_t length = packet[pos++];

9
include/btstack/hci_cmds.h
View File

@ -559,17 +559,14 @@ extern "C" {
// data: event(8), len(8), status (8), bnep source uuid (16), bnep destination uuid (16), mtu (16), remote_address (48)
#define BNEP_EVENT_OPEN_CHANNEL_COMPLETE 0xC1
// data: event(8), len(8), status (8), bnep source uuid (16), bnep destination uuid (16), mtu (16), remote_address (48)
#define BNEP_EVENT_INCOMING_CONNECTION 0xC2
// data: event(8), len(8), bnep source uuid (16), bnep destination uuid (16), remote_address (48)
#define BNEP_EVENT_CHANNEL_CLOSED 0xC3
#define BNEP_EVENT_CHANNEL_CLOSED 0xC2
// data: event(8), len(8), bnep source uuid (16), bnep destination uuid (16), remote_address (48), channel state (8)
#define BNEP_EVENT_CHANNEL_TIMEOUT 0xC4
#define BNEP_EVENT_CHANNEL_TIMEOUT 0xC3
// data: event(8), len(8)
#define BNEP_EVENT_READY_TO_SEND 0xC5
#define BNEP_EVENT_READY_TO_SEND 0xC4
// data: event(8), address_type(8), address (48), [number(32)]
#define SM_JUST_WORKS_REQUEST 0xD0

2
platforms/arduino/btstack-config.h
View File

@ -33,6 +33,6 @@
#define MAX_ATT_DB_SIZE 200
#define MAX_NO_HFP_CONNECTIONS 0
#define MAX_NO_WHITELIST_ENTRIES 1
#define MAX_NO_SM_LOOKUP_ENTRIES 3
#endif

1
platforms/ez430-rf2560/btstack-config.h
View File

@ -36,6 +36,7 @@
#define MAX_NO_DB_MEM_SERVICES 1
#define MAX_NO_HFP_CONNECTIONS 0
#define MAX_NO_WHITELIST_ENTRIES 1
#define MAX_NO_SM_LOOKUP_ENTRIES 3
#endif

1
platforms/msp-exp430f5438-cc2564b/btstack-config.h
View File

@ -36,6 +36,7 @@
#define MAX_NO_DB_MEM_SERVICES 1
#define MAX_NO_HFP_CONNECTIONS 0
#define MAX_NO_WHITELIST_ENTRIES 1
#define MAX_NO_SM_LOOKUP_ENTRIES 3
#endif

1
platforms/msp430f5229lp-cc2564b/btstack-config.h
View File

@ -36,6 +36,7 @@
#define MAX_NO_DB_MEM_SERVICES 1
#define MAX_NO_HFP_CONNECTIONS 0
#define MAX_NO_WHITELIST_ENTRIES 1
#define MAX_NO_SM_LOOKUP_ENTRIES 3
#endif

1
platforms/pic32-harmony/src/btstack-config.h
View File

@ -33,5 +33,6 @@
#define MAX_NO_BNEP_CHANNELS 0
#define MAX_NO_HFP_CONNECTIONS 0
#define MAX_NO_WHITELIST_ENTRIES 1
#define MAX_NO_SM_LOOKUP_ENTRIES 3
#endif

1
platforms/stm32-f103rb-nucleo/btstack-config.h
View File

@ -33,5 +33,6 @@
#define MAX_NO_BNEP_CHANNELS 0
#define MAX_NO_HFP_CONNECTIONS 0
#define MAX_NO_WHITELIST_ENTRIES 1
#define MAX_NO_SM_LOOKUP_ENTRIES 3
#endif

76
src/bnep.c
View File

@ -105,20 +105,6 @@ static void bnep_emit_open_channel_complete(bnep_channel_t *channel, uint8_t sta
(*app_packet_handler)(channel->connection, HCI_EVENT_PACKET, channel->l2cap_cid, (uint8_t *) event, sizeof(event));
}
static void bnep_emit_incoming_connection(bnep_channel_t *channel)
{
log_info("BNEP_EVENT_INCOMING_CONNECTION bd_addr: %s", bd_addr_to_str(channel->remote_addr));
uint8_t event[2 + sizeof(bd_addr_t) + 3 * sizeof(uint16_t)];
event[0] = BNEP_EVENT_INCOMING_CONNECTION;
event[1] = sizeof(event) - 2;
bt_store_16(event, 2, channel->uuid_source);
bt_store_16(event, 4, channel->uuid_dest);
bt_store_16(event, 6, channel->max_frame_size);
BD_ADDR_COPY(&event[8], channel->remote_addr);
hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
(*app_packet_handler)(channel->connection, HCI_EVENT_PACKET, channel->l2cap_cid, (uint8_t *) event, sizeof(event));
}
static void bnep_emit_channel_timeout(bnep_channel_t *channel)
{
log_info("BNEP_EVENT_CHANNEL_TIMEOUT bd_addr: %s", bd_addr_to_str(channel->remote_addr));
@ -888,6 +874,17 @@ static int bnep_handle_connection_response(bnep_channel_t *channel, uint8_t *pac
return 1 + 2;
}
static int bnep_can_handle_extensions(bnep_channel_t * channel){
/* Extension are primarily handled in CONNECTED state */
if (channel->state == BNEP_CHANNEL_STATE_CONNECTED) return 1;
/* and if we've received connection request, but haven't sent the reponse yet. */
if ((channel->state == BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST) &&
(channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_RESPONSE)) {
return 1;
}
return 0;
}
static int bnep_handle_filter_net_type_set(bnep_channel_t *channel, uint8_t *packet, uint16_t size)
{
uint16_t list_length;
@ -904,8 +901,7 @@ static int bnep_handle_filter_net_type_set(bnep_channel_t *channel, uint8_t *pac
return 0;
}
if (channel->state != BNEP_CHANNEL_STATE_CONNECTED) {
/* Ignore filter net type set in any state but CONNECTED */
if (!bnep_can_handle_extensions(channel)){
log_error("BNEP_FILTER_NET_TYPE_SET: Ignored in channel state %d", channel->state);
return 3 + list_length;
}
@ -955,8 +951,7 @@ static int bnep_handle_filter_net_type_response(bnep_channel_t *channel, uint8_t
return 0;
}
if (channel->state != BNEP_CHANNEL_STATE_CONNECTED) {
/* Ignore a filter net type response in any state but CONNECTED */
if (!bnep_can_handle_extensions(channel)){
log_error("BNEP_FILTER_NET_TYPE_RESPONSE: Ignored in channel state %d", channel->state);
return 1 + 2;
}
@ -988,8 +983,7 @@ static int bnep_handle_multi_addr_set(bnep_channel_t *channel, uint8_t *packet,
return 0;
}
if (channel->state != BNEP_CHANNEL_STATE_CONNECTED) {
/* Ignore multicast filter address set in any state but CONNECTED */
if (!bnep_can_handle_extensions(channel)){
log_error("BNEP_MULTI_ADDR_SET: Ignored in channel state %d", channel->state);
return 3 + list_length;
}
@ -1042,8 +1036,7 @@ static int bnep_handle_multi_addr_response(bnep_channel_t *channel, uint8_t *pac
return 0;
}
if (channel->state != BNEP_CHANNEL_STATE_CONNECTED) {
/* Ignore multicast filter set response in any state but CONNECTED */
if (!bnep_can_handle_extensions(channel)){
log_error("BNEP_MULTI_ADDR_RESPONSE: Ignored in channel state %d", channel->state);
return 1 + 2;
}
@ -1251,22 +1244,29 @@ static int bnep_hci_event_handler(uint8_t *packet, uint16_t size)
return 1;
}
if (channel->state == BNEP_CHANNEL_STATE_CLOSED) {
log_info("L2CAP_EVENT_CHANNEL_OPENED: outgoing connection");
switch (channel->state){
case BNEP_CHANNEL_STATE_CLOSED:
log_info("L2CAP_EVENT_CHANNEL_OPENED: outgoing connection");
bnep_channel_start_timer(channel, BNEP_CONNECTION_TIMEOUT_MS);
bnep_channel_start_timer(channel, BNEP_CONNECTION_TIMEOUT_MS);
/* Assign connection handle and l2cap cid */
channel->l2cap_cid = l2cap_cid;
channel->con_handle = con_handle;
/* Assign connection handle and l2cap cid */
channel->l2cap_cid = l2cap_cid;
channel->con_handle = con_handle;
/* Initiate the connection request */
channel->state = BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_RESPONSE;
bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_REQUEST);
channel->max_frame_size = bnep_max_frame_size_for_l2cap_mtu(READ_BT_16(packet, 17));
bnep_run();
} else {
log_error("L2CAP_EVENT_CHANNEL_OPENED: Invalid state: %d", channel->state);
/* Initiate the connection request */
channel->state = BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_RESPONSE;
bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_REQUEST);
channel->max_frame_size = bnep_max_frame_size_for_l2cap_mtu(READ_BT_16(packet, 17));
bnep_run();
break;
case BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST:
/* New information: channel mtu */
channel->max_frame_size = bnep_max_frame_size_for_l2cap_mtu(READ_BT_16(packet, 17));
break;
default:
log_error("L2CAP_EVENT_CHANNEL_OPENED: Invalid state: %d", channel->state);
break;
}
return 1;
@ -1465,17 +1465,21 @@ static void bnep_channel_state_machine(bnep_channel_t* channel, bnep_channel_eve
bnep_send_connection_request(channel, channel->uuid_source, channel->uuid_dest);
}
if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_RESPONSE) {
int emit_connected = 0;
if ((channel->state == BNEP_CHANNEL_STATE_CLOSED) ||
(channel->state == BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST)) {
/* Set channel state to STATE_CONNECTED */
channel->state = BNEP_CHANNEL_STATE_CONNECTED;
/* Stop timeout timer! */
bnep_channel_stop_timer(channel);
emit_connected = 1;
}
bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_RESPONSE);
bnep_send_connection_response(channel, channel->response_code);
bnep_emit_incoming_connection(channel);
if (emit_connected){
bnep_emit_open_channel_complete(channel, 0);
}
return;
}
if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_SET) {

35
src/btstack_memory.c
View File

@ -539,6 +539,38 @@ void btstack_memory_whitelist_entry_free(whitelist_entry_t *whitelist_entry){
#endif
// MARK: sm_lookup_entry_t
#ifdef MAX_NO_SM_LOOKUP_ENTRIES
#if MAX_NO_SM_LOOKUP_ENTRIES > 0
static sm_lookup_entry_t sm_lookup_entry_storage[MAX_NO_SM_LOOKUP_ENTRIES];
static memory_pool_t sm_lookup_entry_pool;
sm_lookup_entry_t * btstack_memory_sm_lookup_entry_get(void){
return (sm_lookup_entry_t *) memory_pool_get(&sm_lookup_entry_pool);
}
void btstack_memory_sm_lookup_entry_free(sm_lookup_entry_t *sm_lookup_entry){
memory_pool_free(&sm_lookup_entry_pool, sm_lookup_entry);
}
#else
sm_lookup_entry_t * btstack_memory_sm_lookup_entry_get(void){
return NULL;
}
void btstack_memory_sm_lookup_entry_free(sm_lookup_entry_t *sm_lookup_entry){
// silence compiler warning about unused parameter in a portable way
(void) sm_lookup_entry;
};
#endif
#elif defined(HAVE_MALLOC)
sm_lookup_entry_t * btstack_memory_sm_lookup_entry_get(void){
return (sm_lookup_entry_t*) malloc(sizeof(sm_lookup_entry_t));
}
void btstack_memory_sm_lookup_entry_free(sm_lookup_entry_t *sm_lookup_entry){
free(sm_lookup_entry);
}
#else
#error "Neither HAVE_MALLOC nor MAX_NO_SM_LOOKUP_ENTRIES for struct sm_lookup_entry is defined. Please, edit the config file."
#endif
#endif
// init
void btstack_memory_init(void){
@ -588,5 +620,8 @@ void btstack_memory_init(void){
#if MAX_NO_WHITELIST_ENTRIES > 0
memory_pool_create(&whitelist_entry_pool, whitelist_entry_storage, MAX_NO_WHITELIST_ENTRIES, sizeof(whitelist_entry_t));
#endif
#if MAX_NO_SM_LOOKUP_ENTRIES > 0
memory_pool_create(&sm_lookup_entry_pool, sm_lookup_entry_storage, MAX_NO_SM_LOOKUP_ENTRIES, sizeof(sm_lookup_entry_t));
#endif
#endif
}

7
src/btstack_memory.h
View File

@ -38,7 +38,7 @@
/*
* btstsack_memory.h
* btstack_memory.h
*
* @brief BTstack memory management via configurable memory pools
*
@ -62,6 +62,7 @@ extern "C" {
#ifdef HAVE_BLE
#include "gatt_client.h"
#include "sm.h"
#endif
/* API_START */
@ -110,13 +111,15 @@ hfp_connection_t * btstack_memory_hfp_connection_get(void);
void btstack_memory_hfp_connection_free(hfp_connection_t *hfp_connection);
#ifdef HAVE_BLE
// gatt_client, gatt_subclient, whitelist_entry
// gatt_client, gatt_subclient, whitelist_entry, sm_lookup_entry
gatt_client_t * btstack_memory_gatt_client_get(void);
void btstack_memory_gatt_client_free(gatt_client_t *gatt_client);
gatt_subclient_t * btstack_memory_gatt_subclient_get(void);
void btstack_memory_gatt_subclient_free(gatt_subclient_t *gatt_subclient);
whitelist_entry_t * btstack_memory_whitelist_entry_get(void);
void btstack_memory_whitelist_entry_free(whitelist_entry_t *whitelist_entry);
sm_lookup_entry_t * btstack_memory_sm_lookup_entry_get(void);
void btstack_memory_sm_lookup_entry_free(sm_lookup_entry_t *sm_lookup_entry);
#endif
#if defined __cplusplus

7
test/pts/Makefile
View File

@ -18,8 +18,11 @@ VPATH += ${BTSTACK_ROOT}/platforms/posix/src
VPATH += ${BTSTACK_ROOT}/platforms/libusb
# use pkg-config
CFLAGS += $(shell pkg-config libusb-1.0 --cflags)
LDFLAGS += $(shell pkg-config libusb-1.0 --libs)
# CFLAGS += $(shell pkg-config libusb-1.0 --cflags)
# LDFLAGS += $(shell pkg-config libusb-1.0 --libs)
# hard coded flags for libusb in /usr/local/lib
CFLAGS += -I/usr/local/include
LDFLAGS += -L/usr/local/lib -lusb-1.0
EXAMPLES = hfp_hf_test hfp_ag_test ble_peripheral_test ble_central_test l2cap_test classic_test bnep_test hsp_ag_test hsp_hs_test

74
test/pts/ble_central_test.c
View File

@ -158,8 +158,14 @@ static void handle_advertising_event(uint8_t * packet, int size){
// filter PTS
bd_addr_t addr;
bt_flip_addr(addr, &packet[4]);
if (memcmp(addr, public_pts_address, 6)) return;
printf("Advertisement: %s, ", ad_event_types[packet[2]]);
// always request address resolution
sm_address_resolution_lookup(packet[3], addr);
// ignore advertisement from devices other than pts
if (memcmp(addr, current_pts_address, 6)) return;
printf("Advertisement: %s - %s, ", bd_addr_to_str(addr), ad_event_types[packet[2]]);
int adv_size = packet[11];
uint8_t * adv_data = &packet[12];
@ -216,6 +222,9 @@ static void gap_run(void){
}
void app_packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
uint16_t aHandle;
sm_event_t * sm_event;
switch (packet_type) {
case HCI_EVENT_PACKET:
@ -243,51 +252,56 @@ void app_packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *packet,
break;
case HCI_EVENT_DISCONNECTION_COMPLETE:
aHandle = READ_BT_16(packet, 3);
printf("Disconnected from handle 0x%04x\n", aHandle);
break;
case SM_PASSKEY_INPUT_NUMBER: {
// display number
sm_event_t * event = (sm_event_t *) packet;
memcpy(peer_address, event->address, 6);
peer_addr_type = event->addr_type;
printf("\nGAP Bonding %s (%u): Enter 6 digit passkey: '", bd_addr_to_str(peer_address), peer_addr_type);
case SM_PASSKEY_INPUT_NUMBER:
// store peer address for input
sm_event = (sm_event_t *) packet;
memcpy(peer_address, sm_event->address, 6);
peer_addr_type = sm_event->addr_type;
printf("\nGAP Bonding %s (%u): Enter 6 digit passkey: '", bd_addr_to_str(sm_event->address), sm_event->addr_type);
fflush(stdout);
ui_passkey = 0;
ui_digits_for_passkey = 6;
break;
}
case SM_PASSKEY_DISPLAY_NUMBER: {
// display number
sm_event_t * event = (sm_event_t *) packet;
printf("\nGAP Bonding %s (%u): Display Passkey '%06u\n", bd_addr_to_str(peer_address), peer_addr_type, event->passkey);
case SM_PASSKEY_DISPLAY_NUMBER:
sm_event = (sm_event_t *) packet;
printf("\nGAP Bonding %s (%u): Display Passkey '%06u\n", bd_addr_to_str(sm_event->address), sm_event->addr_type, sm_event->passkey);
break;
}
case SM_PASSKEY_DISPLAY_CANCEL:
printf("\nGAP Bonding %s (%u): Display cancel\n", bd_addr_to_str(peer_address), peer_addr_type);
sm_event = (sm_event_t *) packet;
printf("\nGAP Bonding %s (%u): Display cancel\n", bd_addr_to_str(sm_event->address), sm_event->addr_type);
break;
case SM_JUST_WORKS_REQUEST:
{
// auto-authorize connection if requested
sm_event_t * event = (sm_event_t *) packet;
sm_just_works_confirm(current_pts_address_type, current_pts_address);
sm_event = (sm_event_t *) packet;
sm_just_works_confirm(sm_event->addr_type, sm_event->address);
printf("Just Works request confirmed\n");
break;
}
break;
case SM_AUTHORIZATION_REQUEST:
{
// auto-authorize connection if requested
sm_event_t * event = (sm_event_t *) packet;
sm_authorization_grant(current_pts_address_type, current_pts_address);
sm_event = (sm_event_t *) packet;
sm_authorization_grant(sm_event->addr_type, sm_event->address);
break;
}
case GAP_LE_ADVERTISING_REPORT:
handle_advertising_event(packet, size);
break;
case SM_IDENTITY_RESOLVING_SUCCEEDED:
// skip already detected pts
if (memcmp( ((sm_event_t*) packet)->address, current_pts_address, 6) == 0) break;
memcpy(current_pts_address, ((sm_event_t*) packet)->address, 6);
current_pts_address_type = ((sm_event_t*) packet)->addr_type;
printf("Address resolving succeeded: resolvable address %s, addr type %u\n",
bd_addr_to_str(current_pts_address), current_pts_address_type);
break;
default:
break;
}
@ -379,12 +393,18 @@ uint16_t attribute_size = 1;
int scanning_active = 0;
void show_usage(void){
uint8_t iut_address_type;
bd_addr_t iut_address;
hci_le_advertisement_address(&iut_address_type, iut_address);
printf("\e[1;1H\e[2J");
printf("--- CLI for LE Central ---\n");
printf("PTS: addr type %u, addr %s\n", current_pts_address_type, bd_addr_to_str(current_pts_address));
printf("IUT: addr type %u, addr %s\n", iut_address_type, bd_addr_to_str(iut_address));
printf("--------------------------\n");
printf("GAP: connectable %u\n", gap_connectable);
printf("SM: %s, MITM protection %u, OOB data %u, key range [%u..16]\n",
sm_io_capabilities, sm_mitm_protection, sm_have_oob_data, sm_min_key_size);
printf("PTS: addr type %u, addr %s\n", current_pts_address_type, current_pts_address);
printf("Privacy %u\n", gap_privacy);
printf("Device name: %s\n", gap_device_name);
printf("Value Handle: %x\n", value_handle);
@ -577,6 +597,10 @@ int btstack_main(int argc, const char * argv[]){
// Setup LE Device DB
le_device_db_init();
// add bonded device with IRK 0x00112233..FF for gap-conn-prda-bv-2
uint8_t pts_irk[] = { 0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88, 0x99, 0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff };
le_device_db_add(public_pts_address_type, public_pts_address, pts_irk);
// set adv params
update_advertisment_params();

194
test/pts/bnep_test.c
View File

@ -73,8 +73,10 @@
#define NETWORK_TYPE_ARP 0x0806
#define NETWORK_TYPE_IPv6 0x86DD
#define IPv4_PROTOCOL_ICMP 0x0001
#define IPv4_PROTOCOL_UDP 0x0011
#define IP_PROTOCOL_ICMP_IPv4 0x0001
#define IP_PROTOCOL_ICMP_IPv6 0x003a
#define IP_PROTOCOL_UDP 0x0011
#define IPv4_
#define ICMP_V4_TYPE_PING_REQUEST 0x08
#define ICMP_V4_TYPE_PING_RESPONSE 0x00
@ -96,8 +98,10 @@ static bd_addr_t pts_addr = {0x00,0x1b,0xDC,0x07,0x32,0xEF};
//static bd_addr_t pts_addr = {0xE0,0x06,0xE6,0xBB,0x95,0x79}; // Ole Thinkpad
// static bd_addr_t other_addr = { 0x33, 0x33, 0x00, 0x00, 0x00, 0x16};
static bd_addr_t other_addr = { 0,0,0,0,0,0};
// broadcast
static bd_addr_t broadcast_addr = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
// static bd_addr_t broadcast_addr = { 0x33, 0x33, 0x00, 0x01, 0x00, 0x03 };
// Outgoing: Must match PTS TSPX_UUID_src_addresss
static uint16_t bnep_src_uuid = 0x1115;
@ -242,7 +246,7 @@ static uint16_t calc_internet_checksum(uint8_t * data, int size){
static void send_ping_request_ipv4(void){
uint8_t ipv4_packet[] = {
uint8_t ipv4_header[] = {
// ip
0x45, 0x00, 0x00, 0x00, // version + ihl, dscp } ecn, total len
0x00, 0x00, 0x00, 0x00, // identification (16), flags + fragment offset
@ -261,13 +265,13 @@ static void send_ping_request_ipv4(void){
int pos = setup_ethernet_header(1, 0, 0, NETWORK_TYPE_IPv4); // IPv4
// ipv4
int total_length = sizeof(ipv4_packet) + sizeof(icmp_packet);
net_store_16(ipv4_packet, 2, total_length);
uint16_t ipv4_checksum = calc_internet_checksum(ipv4_packet, sizeof(ipv4_packet));
net_store_16(ipv4_packet, 10, ipv4_checksum);
int total_length = sizeof(ipv4_header) + sizeof(icmp_packet);
net_store_16(ipv4_header, 2, total_length);
uint16_t ipv4_checksum = calc_internet_checksum(ipv4_header, sizeof(ipv4_header));
net_store_16(ipv4_header, 10, ipv4_checksum);
// TODO: also set src/dest ip address
memcpy(&network_buffer[pos], ipv4_packet, sizeof(ipv4_packet));
pos += sizeof(ipv4_packet);
memcpy(&network_buffer[pos], ipv4_header, sizeof(ipv4_header));
pos += sizeof(ipv4_header);
// icmp
uint16_t icmp_checksum = calc_internet_checksum(icmp_packet, sizeof(icmp_packet));
@ -281,7 +285,7 @@ static void send_ping_request_ipv4(void){
static void send_ping_response_ipv4(void){
uint8_t ipv4_packet[] = {
uint8_t ipv4_header[] = {
// ip
0x45, 0x00, 0x00, 0x00, // version + ihl, dscp } ecn, total len
0x00, 0x00, 0x00, 0x00, // identification (16), flags + fragment offset
@ -300,13 +304,13 @@ static void send_ping_response_ipv4(void){
int pos = setup_ethernet_header(1, 0, 0, NETWORK_TYPE_IPv4); // IPv4
// ipv4
int total_length = sizeof(ipv4_packet) + sizeof(icmp_packet);
net_store_16(ipv4_packet, 2, total_length);
uint16_t ipv4_checksum = calc_internet_checksum(ipv4_packet, sizeof(ipv4_packet));
net_store_16(ipv4_packet, 10, ipv4_checksum);
int total_length = sizeof(ipv4_header) + sizeof(icmp_packet);
net_store_16(ipv4_header, 2, total_length);
uint16_t ipv4_checksum = calc_internet_checksum(ipv4_header, sizeof(ipv4_header));
net_store_16(ipv4_header, 10, ipv4_checksum);
// TODO: also set src/dest ip address
memcpy(&network_buffer[pos], ipv4_packet, sizeof(ipv4_packet));
pos += sizeof(ipv4_packet);
memcpy(&network_buffer[pos], ipv4_header, sizeof(ipv4_header));
pos += sizeof(ipv4_header);
// icmp
uint16_t icmp_checksum = calc_internet_checksum(icmp_packet, sizeof(icmp_packet));
@ -318,9 +322,10 @@ static void send_ping_response_ipv4(void){
send_buffer(pos);
}
/* Untested */
static void send_ping_request_ipv6(void){
uint8_t ipv6_packet[] = {
uint8_t ipv6_header[] = {
// ip
0x60, 0x00, 0x00, 0x00, // version (4) + traffic class (8) + flow label (24)
0x00, 0x00, 58, 0x01, // payload length(16), next header = IPv6-ICMP, hop limit
@ -345,14 +350,14 @@ static void send_ping_request_ipv6(void){
// ipv6
int payload_length = sizeof(icmp_packet);
net_store_16(ipv6_packet, 4, payload_length);
net_store_16(ipv6_header, 4, payload_length);
// TODO: also set src/dest ip address
int checksum = calc_internet_checksum(&ipv6_packet[8], 32);
checksum = sum_ones_complement(checksum, sizeof(ipv6_packet) + sizeof(icmp_packet));
int checksum = calc_internet_checksum(&ipv6_header[8], 32);
checksum = sum_ones_complement(checksum, payload_length);
checksum = sum_ones_complement(checksum, 58 << 8);
net_store_16(icmp_packet, 2, checksum);
memcpy(&network_buffer[pos], ipv6_packet, sizeof(ipv6_packet));
pos += sizeof(ipv6_packet);
memcpy(&network_buffer[pos], ipv6_header, sizeof(ipv6_header));
pos += sizeof(ipv6_header);
// icmp
uint16_t icmp_checksum = calc_internet_checksum(icmp_packet, sizeof(icmp_packet));
@ -366,9 +371,9 @@ static void send_ping_request_ipv6(void){
static void send_ndp_probe_ipv6(void){
uint8_t ipv6_packet[] = {
uint8_t ipv6_header[] = {
// ip
0x60, 0x00, 0x00, 0x00, // version (4) + traffic class (8) + flow label (24)
0x60, 0x00, 0x00, 0x00, // version (6) + traffic class (8) + flow label (24)
0x00, 0x00, 58, 0x01, // payload length(16), next header = IPv6-ICMP, hop limit
0x00, 0x00, 0x00, 0x00, // source IP address
0x00, 0x00, 0x00, 0x00, // source IP address
@ -391,24 +396,23 @@ static void send_ndp_probe_ipv6(void){
// ipv6
int payload_length = sizeof(icmp_packet);
net_store_16(ipv6_packet, 4, payload_length);
net_store_16(ipv6_header, 4, payload_length);
// source address ::
// dest addresss - Modified EUI-64
// ipv6_packet[24..31] = FE80::
ipv6_packet[32] = local_addr[0] ^ 0x2;
ipv6_packet[33] = local_addr[1];
ipv6_packet[34] = local_addr[2];
ipv6_packet[35] = 0xff;
ipv6_packet[36] = 0xfe;
ipv6_packet[37] = local_addr[3];
ipv6_packet[38] = local_addr[4];
ipv6_packet[39] = local_addr[5];
int checksum = calc_internet_checksum(&ipv6_packet[8], 32);
checksum = sum_ones_complement(checksum, sizeof(ipv6_packet) + sizeof(icmp_packet));
checksum = sum_ones_complement(checksum, 58 << 8);
net_store_16(icmp_packet, 2, checksum);
memcpy(&network_buffer[pos], ipv6_packet, sizeof(ipv6_packet));
pos += sizeof(ipv6_packet);
// ipv6_header[24..31] = FE80::
ipv6_header[32] = local_addr[0] ^ 0x2;
ipv6_header[33] = local_addr[1];
ipv6_header[34] = local_addr[2];
ipv6_header[35] = 0xff;
ipv6_header[36] = 0xfe;
ipv6_header[37] = local_addr[3];
ipv6_header[38] = local_addr[4];
ipv6_header[39] = local_addr[5];
int checksum = calc_internet_checksum(&ipv6_header[8], 32);
checksum = sum_ones_complement(checksum, payload_length);
checksum = sum_ones_complement(checksum, ipv6_header[6] << 8);
memcpy(&network_buffer[pos], ipv6_header, sizeof(ipv6_header));
pos += sizeof(ipv6_header);
// icmp
uint16_t icmp_checksum = calc_internet_checksum(icmp_packet, sizeof(icmp_packet));
@ -423,24 +427,28 @@ static void send_ndp_probe_ipv6(void){
static void send_llmnr_request_ipv4(void){
uint8_t ipv4_header[] = {
0x45, 0x00, 0x00, 0x00, // version + ihl, dscp } ecn, total len
0x45, 0x00, 0x00, 0x00, // version + ihl, dscp } ecn, total len
0x00, 0x00, 0x00, 0x00, // identification (16), flags + fragment offset
0x01, 0x11, 0x00, 0x00, // time to live, procotol: UDP, checksum (16),
192, 168, 167, 152, // source IP address
224, 0, 0, 252, // destination IP address
224, 0, 0, 252, // destination IP address
};
uint8_t udp_header[8];
uint8_t llmnr_packet[12];
uint8_t dns_data[] = { 0x08, 0x61, 0x61, 0x70, 0x70, 0x6c, 0x65, 0x74, 0x76,
0x05, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x00, 0x00,
0x01, 0x00, 0x01 };
// ethernet header
int pos = setup_ethernet_header(1, 0, 0, NETWORK_TYPE_IPv4); // IPv4
// ipv4
int total_length = sizeof(ipv4_header) + sizeof(udp_header) + sizeof (llmnr_packet);
int total_length = sizeof(ipv4_header) + sizeof(udp_header) + sizeof (llmnr_packet) + sizeof(dns_data);
net_store_16(ipv4_header, 2, total_length);
uint16_t ipv4_checksum = calc_internet_checksum(ipv4_header, sizeof(ipv4_header));
net_store_16(ipv4_header, 10, ipv4_checksum);
net_store_16(ipv4_header, 10, ~ipv4_checksum);
// TODO: also set src/dest ip address
memcpy(&network_buffer[pos], ipv4_header, sizeof(ipv4_header));
pos += sizeof(ipv4_header);
@ -448,23 +456,89 @@ static void send_llmnr_request_ipv4(void){
// udp packet
net_store_16(udp_header, 0, 5355); // source port
net_store_16(udp_header, 2, 5355); // destination port
net_store_16(udp_header, 4, sizeof(udp_header) + sizeof(llmnr_packet));
net_store_16(udp_header, 4, sizeof(udp_header) + sizeof(llmnr_packet) + sizeof(dns_data));
net_store_16(udp_header, 6, 0); // no checksum
memcpy(&network_buffer[pos], udp_header, sizeof(udp_header));
pos += sizeof(udp_header);
// llmnr packet
bzero(llmnr_packet, sizeof(llmnr_packet));
net_store_16(llmnr_packet, 0, 0x1234);
net_store_16(llmnr_packet, 0, 0x1234); // transaction id
net_store_16(llmnr_packet, 4, 1); // one query
memcpy(&network_buffer[pos], llmnr_packet, sizeof(llmnr_packet));
pos += sizeof(llmnr_packet);
memcpy(&network_buffer[pos], dns_data, sizeof(dns_data));
pos += sizeof(dns_data);
// send
send_buffer(pos);
}
static void send_llmnr_request_ipv6(void){
// https://msdn.microsoft.com/en-us/library/dd240361.aspx
uint8_t ipv6_header[] = {
0x60, 0x00, 0x00, 0x00, // version (6) + traffic class (8) + flow label (24)
0x00, 0x00, 17, 0x01, // payload length(16), next header = UDP, hop limit
0xfe, 0x80, 0x00, 0x00, // source IP address
0x00, 0x00, 0x00, 0x00, // source IP address
0xd9, 0xf6, 0xce, 0x2e, // source IP address
0x48, 0x75, 0xab, 0x03, // source IP address
0xff, 0x02, 0x00, 0x00, // destination IP address
0x00, 0x00, 0x00, 0x00, // destination IP address
0x00, 0x00, 0x00, 0x00, // destination IP address
0x00, 0x01, 0x00, 0x03, // destination IP address
};
uint8_t udp_header[8];
uint8_t llmnr_packet[12];
uint8_t dns_data[] = { 0x08, 0x61, 0x61, 0x70, 0x70, 0x6c, 0x65, 0x74, 0x76,
0x05, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x00, 0x00,
0x01, 0x00, 0x01 };
int payload_length = sizeof(udp_header) + sizeof(llmnr_packet) + sizeof(dns_data);
// llmnr header
bzero(llmnr_packet, sizeof(llmnr_packet));
net_store_16(llmnr_packet, 0, 0x1235); // transaction id
net_store_16(llmnr_packet, 4, 1); // one query
// ipv6 header
net_store_16(ipv6_header, 4, payload_length);
// udp header
bzero(udp_header, sizeof(udp_header));
net_store_16(udp_header, 0, 5355); // source port
net_store_16(udp_header, 2, 5355); // destination port
net_store_16(udp_header, 4, payload_length);
int checksum = calc_internet_checksum(&ipv6_header[8], 32);
checksum = sum_ones_complement(checksum, payload_length); // payload len
checksum = sum_ones_complement(checksum, ipv6_header[6] << 8); // next header
checksum = sum_ones_complement(checksum, calc_internet_checksum(udp_header, sizeof(udp_header)));
checksum = sum_ones_complement(checksum, calc_internet_checksum(llmnr_packet, sizeof(llmnr_packet)));
checksum = sum_ones_complement(checksum, calc_internet_checksum(dns_data, sizeof(dns_data)));
net_store_16(udp_header, 6, ~checksum);
// ethernet header
int pos = setup_ethernet_header(1, 0, 1, NETWORK_TYPE_IPv6); // IPv6
memcpy(&network_buffer[pos], ipv6_header, sizeof(ipv6_header));
pos += sizeof(ipv6_header);
memcpy(&network_buffer[pos], udp_header, sizeof(udp_header));
pos += sizeof(udp_header);
memcpy(&network_buffer[pos], llmnr_packet, sizeof(llmnr_packet));
pos += sizeof(llmnr_packet);
memcpy(&network_buffer[pos], dns_data, sizeof(dns_data));
pos += sizeof(dns_data);
// send
send_buffer(pos);
}
static void show_usage(void){
@ -487,12 +561,6 @@ static void show_usage(void){
printf("6 - send IPv6 NDP request\n");
printf("7 - send IPv4 LLMNR request\n");
printf("8 - send IPv6 LLMNR request\n");
#if 0
printf("1 - get IP address via DHCP\n");
printf("2 - send DNS request\n");
printf("9 - send some IPv6 packet\n");
printf("0 - send some IPv6 packet 2\n");
#endif
printf("---\n");
printf("Ctrl-c - exit\n");
printf("---\n");
@ -553,8 +621,7 @@ static int stdin_process(struct data_source *ds){
break;
case '8':
printf("Sending IPv6 LLMNR Request\n");
printf("(Not implemented yet)\n");
// send_llmnr_request_ipv6();
send_llmnr_request_ipv6();
break;
default:
show_usage();
@ -678,7 +745,7 @@ static void packet_handler (void * connection, uint8_t packet_type, uint16_t cha
break;
}
case 0x11: // UDP
printf("UDP packet\n");
printf("UDP IPv4 packet\n");
hexdumpf(&packet[payload_offset], size - payload_offset);
break;
default:
@ -687,8 +754,21 @@ static void packet_handler (void * connection, uint8_t packet_type, uint16_t cha
}
break;
case NETWORK_TYPE_IPv6:
printf("IPV6 packet");
hexdumpf(&packet[14], size - 14);
protocol_type = packet[6];
switch(protocol_type){
case 0x11: // UDP
printf("UDP IPv6 packet\n");
payload_offset = 40; // fixed
hexdumpf(&packet[payload_offset], size - payload_offset);
// send response
break;
default:
printf("IPv6 packet of protocol 0x%02x\n", protocol_type);
hexdumpf(&packet[14], size - 14);
break;
}
break;
default:
printf("Unknown network type %x", network_type);

5
tools/btstack_memory_generator.py
View File

@ -41,7 +41,7 @@ copyright = """/*
hfile_header_begin = """
/*
* btstsack_memory.h
* btstack_memory.h
*
* @brief BTstack memory management via configurable memory pools
*
@ -65,6 +65,7 @@ extern "C" {
#ifdef HAVE_BLE
#include "gatt_client.h"
#include "sm.h"
#endif
/* API_START */
@ -156,7 +157,7 @@ def replacePlaceholder(template, struct_name):
return snippet
list_of_structs = [ ["hci_connection"], ["l2cap_service", "l2cap_channel"], ["rfcomm_multiplexer", "rfcomm_service", "rfcomm_channel"], ["db_mem_device_name", "db_mem_device_link_key", "db_mem_service"], ["bnep_service", "bnep_channel"], ["hfp_connection"]]
list_of_le_structs = [["gatt_client", "gatt_subclient","whitelist_entry"]]
list_of_le_structs = [["gatt_client", "gatt_subclient", "whitelist_entry", "sm_lookup_entry"]]
file_name = "../src/btstack_memory"