static int check_and_remove(bd_addr_t addr, int remove, link_key_t key) {
int mode = remove ? O_RDWR : O_RDONLY;
int fd = open(HIDDEVS_FILE, mode);
if (fd < 0) {
// printf("WARNING - could not open " HIDDEVS_FILE "\n");
return 0;
}
char buf[4096];
int n = read(fd, buf, sizeof(buf));
if (n == sizeof(buf)) {
printf("WARNING - hiddevs file is ridiculously big, will be truncated\n");
n--;
}
buf[n] = '\0';
char *addr_str = bd_addr_to_str(addr);
char *found = strcasestr(buf, addr_str);
if (found && key)
sscanf_link_key(found + 3*BD_ADDR_LEN, key);
if (!remove || !found) {
close(fd);
return !!found;
}
int ret = 0;
lseek(fd, 0, SEEK_SET);
if (write(fd, buf, found - buf) < 0)
printf("ERROR - couldn't write to " HIDDEVS_FILE "\n");
while (*found++ != '\n');
if (write(fd, found, strlen(found)) < 0)
printf("ERROR - couldn't write to " HIDDEVS_FILE "\n");
close(fd);
return 1;
}
int le_device_db_add(int addr_type, bd_addr_t addr, sm_key_t irk){
int i;
int index = -1;
for (i=0;i<LE_DEVICE_MEMORY_SIZE;i++){
if (le_devices[i].addr_type == INVALID_ENTRY_ADDR_TYPE){
index = i;
break;
}
}
if (index < 0) return -1;
log_info("Central Device DB adding type %u - %s", addr_type, bd_addr_to_str(addr));
log_key("irk", irk);
le_devices[index].addr_type = addr_type;
memcpy(le_devices[index].addr, addr, 6);
memcpy(le_devices[index].irk, irk, 16);
le_devices[index].remote_counter = 0;
return index;
}
int le_device_db_add(int addr_type, bd_addr_t addr, sm_key_t irk){
int i;
int index = -1;
for (i=0;i<MAX_NR_LE_DEVICE_DB_ENTRIES;i++){
if (le_devices[i].addr_type == INVALID_ENTRY_ADDR_TYPE){
index = i;
break;
}
}
if (index < 0) return -1;
log_info("LE Device DB adding type %u - %s", addr_type, bd_addr_to_str(addr));
log_info_key("irk", irk);
le_devices[index].addr_type = addr_type;
memcpy(le_devices[index].addr, addr, 6);
memcpy(le_devices[index].irk, irk, 16);
#ifdef ENABLE_LE_SIGNED_WRITE
le_devices[index].remote_counter = 0;
#endif
return index;
}
static int stdin_process(struct data_source *ds){
char buffer;
read(ds->fd, &buffer, 1);
switch (buffer){
case 'p':
printf("Establishing HFP service level connection to PTS module %s...\n", bd_addr_to_str(pts_addr));
hfp_hf_establish_service_level_connection(pts_addr);
break;
case 'e':
printf("Establishing HFP service level connection to %s...\n", bd_addr_to_str(phone));
hfp_hf_establish_service_level_connection(phone);
break;
case 'd':
printf("Releasing HFP service level connection.\n");
hfp_hf_release_service_level_connection(phone);
break;
default:
show_usage();
break;
}
return 0;
}
static void att_server_persistent_ccc_clear(att_server_t * att_server){
if (!att_server) return;
int le_device_index = att_server->ir_le_device_db_index;
log_info("Clear CCC values of remote %s, le device id %d", bd_addr_to_str(att_server->peer_address), le_device_index);
// check if bonded
if (le_device_index < 0) return;
// get btstack_tlv
const btstack_tlv_t * tlv_impl = NULL;
void * tlv_context;
btstack_tlv_get_instance(&tlv_impl, &tlv_context);
if (!tlv_impl) return;
// get all ccc tag
int index;
persistent_ccc_entry_t entry;
for (index=0;index<NVN_NUM_GATT_SERVER_CCC;index++){
uint32_t tag = att_server_persistent_ccc_tag_for_index(index);
int len = tlv_impl->get_tag(tlv_context, tag, (uint8_t *) &entry, sizeof(persistent_ccc_entry_t));
if (len != sizeof(persistent_ccc_entry_t)) continue;
if (entry.device_index != le_device_index) continue;
// delete entry
log_info("CCC Index %u: Delete", index);
tlv_impl->delete_tag(tlv_context, tag);
}
}
static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
bd_addr_t addr;
if (packet_type != HCI_EVENT_PACKET) return;
switch (hci_event_packet_get_type(packet)){
case BTSTACK_EVENT_STATE:
if (btstack_event_state_get_state(packet) != HCI_STATE_WORKING) break;
gap_local_bd_addr(addr);
printf("BTstack up and running at %s\n", bd_addr_to_str(addr));
break;
case HCI_EVENT_COMMAND_COMPLETE:
if (HCI_EVENT_IS_COMMAND_COMPLETE(packet, hci_read_local_name)){
if (hci_event_command_complete_get_return_parameters(packet)[0]) break;
// terminate, name 248 chars
packet[6+248] = 0;
printf("Local name: %s\n", &packet[6]);
}
if (HCI_EVENT_IS_COMMAND_COMPLETE(packet, hci_read_local_version_information)){
local_version_information_handler(packet);
}
break;
default:
break;
}
}
// Bluetooth logic
static void packet_handler (void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
bd_addr_t event_addr;
uint8_t rfcomm_channel_nr;
uint16_t mtu;
switch (packet_type) {
case HCI_EVENT_PACKET:
switch (packet[0]) {
case BTSTACK_EVENT_STATE:
// bt stack activated, get started - set local name
if (packet[2] == HCI_STATE_WORKING) {
hci_send_cmd(&hci_write_local_name, "BlueMSP-Demo");
}
break;
case HCI_EVENT_COMMAND_COMPLETE:
if (COMMAND_COMPLETE_EVENT(packet, hci_read_bd_addr)){
bt_flip_addr(event_addr, &packet[6]);
printf("BD-ADDR: %s\n\r", bd_addr_to_str(event_addr));
break;
}
if (COMMAND_COMPLETE_EVENT(packet, hci_write_local_name)){
hci_discoverable_control(1);
break;
}
break;
case HCI_EVENT_LINK_KEY_REQUEST:
// deny link key request
printf("Link key request\n\r");
bt_flip_addr(event_addr, &packet[2]);
hci_send_cmd(&hci_link_key_request_negative_reply, &event_addr);
break;
case HCI_EVENT_PIN_CODE_REQUEST:
// inform about pin code request
printf("Pin code request - using '0000'\n\r");
bt_flip_addr(event_addr, &packet[2]);
hci_send_cmd(&hci_pin_code_request_reply, &event_addr, 4, "0000");
break;
case RFCOMM_EVENT_INCOMING_CONNECTION:
// data: event (8), len(8), address(48), channel (8), rfcomm_cid (16)
bt_flip_addr(event_addr, &packet[2]);
rfcomm_channel_nr = packet[8];
rfcomm_channel_id = READ_BT_16(packet, 9);
printf("RFCOMM channel %u requested for %s\n\r", rfcomm_channel_nr, bd_addr_to_str(event_addr));
rfcomm_accept_connection_internal(rfcomm_channel_id);
break;
case RFCOMM_EVENT_OPEN_CHANNEL_COMPLETE:
// data: event(8), len(8), status (8), address (48), server channel(8), rfcomm_cid(16), max frame size(16)
if (packet[2]) {
printf("RFCOMM channel open failed, status %u\n\r", packet[2]);
} else {
rfcomm_channel_id = READ_BT_16(packet, 12);
mtu = READ_BT_16(packet, 14);
printf("\n\rRFCOMM channel open succeeded. New RFCOMM Channel ID %u, max frame size %u\n\r", rfcomm_channel_id, mtu);
}
break;
case RFCOMM_EVENT_CHANNEL_CLOSED:
rfcomm_channel_id = 0;
break;
default:
break;
}
break;
case RFCOMM_DATA_PACKET:
// hack: truncate data (we know that the packet is at least on byte bigger
packet[size] = 0;
puts( (const char *) packet);
rfcomm_send_credit = 1;
default:
break;
}
}
static void app_packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
UNUSED(channel);
UNUSED(size);
bd_addr_t local_addr;
switch (packet_type) {
case HCI_EVENT_PACKET:
switch (packet[0]) {
case BTSTACK_EVENT_STATE:
// bt stack activated, get started
if (btstack_event_state_get_state(packet) == HCI_STATE_WORKING){
gap_local_bd_addr(local_addr);
printf("BD_ADDR: %s\n", bd_addr_to_str(local_addr));
// generate OOB data
sm_generate_sc_oob_data(sc_local_oob_generated_callback);
}
break;
case HCI_EVENT_LE_META:
switch (hci_event_le_meta_get_subevent_code(packet)) {
case HCI_SUBEVENT_LE_CONNECTION_COMPLETE:
connection_handle = little_endian_read_16(packet, 4);
printf("CONNECTED: Connection handle 0x%04x\n", connection_handle);
break;
default:
break;
}
break;
case HCI_EVENT_DISCONNECTION_COMPLETE:
break;
case SM_EVENT_JUST_WORKS_REQUEST:
printf("JUST_WORKS_REQUEST\n");
break;
case SM_EVENT_NUMERIC_COMPARISON_REQUEST:
printf("NUMERIC_COMPARISON_REQUEST\n");
break;
case SM_EVENT_PASSKEY_INPUT_NUMBER:
// display number
printf("PASSKEY_INPUT_NUMBER\n");
ui_passkey = 0;
ui_digits_for_passkey = 6;
sm_keypress_notification(connection_handle, SM_KEYPRESS_PASSKEY_ENTRY_STARTED);
break;
case SM_EVENT_PASSKEY_DISPLAY_NUMBER:
// display number
printf("PASSKEY_DISPLAY_NUMBER: %06u\n", little_endian_read_32(packet, 11));
break;
case SM_EVENT_PASSKEY_DISPLAY_CANCEL:
break;
case SM_EVENT_AUTHORIZATION_REQUEST:
break;
case SM_EVENT_PAIRING_COMPLETE:
printf("\nPAIRING_COMPLETE: %u,%u\n", sm_event_pairing_complete_get_status(packet), sm_event_pairing_complete_get_reason(packet));
if (sm_event_pairing_complete_get_status(packet)) break;
if (we_are_central){
printf("Search for LE Counter service.\n");
state = TC_W4_SERVICE_RESULT;
gatt_client_discover_primary_services_by_uuid128(handle_gatt_client_event, connection_handle, le_counter_service_uuid);
}
break;
case ATT_EVENT_HANDLE_VALUE_INDICATION_COMPLETE:
break;
case ATT_EVENT_CAN_SEND_NOW:
att_server_notify(connection_handle, ATT_CHARACTERISTIC_0000FF11_0000_1000_8000_00805F9B34FB_01_VALUE_HANDLE, (uint8_t *) "Pairing Success!", 16);
break;
default:
break;
}
}
fflush(stdout);
}
static int stdin_process(struct data_source *ds){
char buffer;
read(ds->fd, &buffer, 1);
switch (buffer){
case 'p':
printf("Connecting to PTS at %s...\n", bd_addr_to_str(pts_addr));
bnep_connect(NULL, pts_addr, bnep_l2cap_psm, bnep_src_uuid, bnep_dest_uuid);
break;
case 'e':
printf("Sending general ethernet packet\n");
send_ethernet_packet(0,0);
break;
case 'c':
printf("Sending compressed ethernet packet\n");
send_ethernet_packet(1,1);
break;
case 's':
printf("Sending src only compressed ethernet packet\n");
send_ethernet_packet(0,1);
break;
case 'd':
printf("Sending dst only ethernet packet\n");
send_ethernet_packet(1,0);
break;
case 'f':
printf("Setting network protocol filter\n");
set_network_protocol_filter();
break;
case 'm':
printf("Setting multicast filter\n");
set_multicast_filter();
break;
case '1':
printf("Sending ICMP Ping via IPv4\n");
send_ping_request_ipv4();
break;
case '2':
printf("Sending ICMP Ping via IPv6\n");
send_ping_request_ipv6();
break;
case '4':
printf("Sending IPv4 ARP Probe\n");
send_arp_probe_ipv4();
break;
case '6':
printf("Sending IPv6 ARP Probe\n");
send_ndp_probe_ipv6();
break;
case '7':
printf("Sending IPv4 LLMNR Request\n");
send_llmnr_request_ipv4();
break;
case '8':
printf("Sending IPv6 LLMNR Request\n");
send_llmnr_request_ipv6();
break;
default:
show_usage();
break;
}
return 0;
}
/* LISTING_START(GAPLEAdvDataParsing): Parsing advertising data */
static void dump_advertisement_data(const uint8_t * adv_data, uint8_t adv_size){
ad_context_t context;
bd_addr_t address;
uint8_t uuid_128[16];
for (ad_iterator_init(&context, adv_size, (uint8_t *)adv_data) ; ad_iterator_has_more(&context) ; ad_iterator_next(&context)){
uint8_t data_type = ad_iterator_get_data_type(&context);
uint8_t size = ad_iterator_get_data_len(&context);
const uint8_t * data = ad_iterator_get_data(&context);
if (data_type > 0 && data_type < 0x1B){
printf(" %s: ", ad_types[data_type]);
}
int i;
// Assigned Numbers GAP
switch (data_type){
case 0x01: // Flags
// show only first octet, ignore rest
for (i=0; i<8;i++){
if (data[0] & (1<<i)){
printf("%s; ", flags[i]);
}
}
break;
case 0x02: // Incomplete List of 16-bit Service Class UUIDs
case 0x03: // Complete List of 16-bit Service Class UUIDs
case 0x14: // List of 16-bit Service Solicitation UUIDs
for (i=0; i<size;i+=2){
printf("%02X ", little_endian_read_16(data, i));
}
break;
case 0x04: // Incomplete List of 32-bit Service Class UUIDs
case 0x05: // Complete List of 32-bit Service Class UUIDs
for (i=0; i<size;i+=4){
printf("%04"PRIX32, little_endian_read_32(data, i));
}
break;
case 0x06: // Incomplete List of 128-bit Service Class UUIDs
case 0x07: // Complete List of 128-bit Service Class UUIDs
case 0x15: // List of 128-bit Service Solicitation UUIDs
reverse_128(data, uuid_128);
printf("%s", uuid128_to_str(uuid_128));
break;
case 0x08: // Shortened Local Name
case 0x09: // Complete Local Name
for (i=0; i<size;i++){
printf("%c", (char)(data[i]));
}
break;
case 0x0A: // Tx Power Level
printf("%d dBm", *(int8_t*)data);
break;
case 0x12: // Slave Connection Interval Range
printf("Connection Interval Min = %u ms, Max = %u ms", little_endian_read_16(data, 0) * 5/4, little_endian_read_16(data, 2) * 5/4);
break;
case 0x16: // Service Data
printf_hexdump(data, size);
break;
case 0x17: // Public Target Address
case 0x18: // Random Target Address
reverse_bd_addr(data, address);
printf("%s", bd_addr_to_str(address));
break;
case 0x19: // Appearance
// https://developer.bluetooth.org/gatt/characteristics/Pages/CharacteristicViewer.aspx?u=org.bluetooth.characteristic.gap.appearance.xml
printf("%02X", little_endian_read_16(data, 0) );
break;
case 0x1A: // Advertising Interval
printf("%u ms", little_endian_read_16(data, 0) * 5/8 );
break;
case 0x3D: // 3D Information Data
printf_hexdump(data, size);
break;
case 0xFF: // Manufacturer Specific Data
break;
case 0x0D: // Class of Device (3B)
case 0x0E: // Simple Pairing Hash C (16B)
case 0x0F: // Simple Pairing Randomizer R (16B)
case 0x10: // Device ID
case 0x11: // Security Manager TK Value (16B)
default:
printf("Unknown Advertising Data Type");
break;
}
printf("\n");
}
printf("\n");
}
static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
//static void packet_handler (uint8_t packet_type, uint8_t *packet, uint16_t size){
bd_addr_t addr;
int i;
int numResponses;
// printf("packet_handler: pt: 0x%02x, packet[0]: 0x%02x\n", packet_type, packet[0]);
if (packet_type != HCI_EVENT_PACKET) return;
uint8_t event = packet[0];
switch(state){
case INIT:
if (packet[2] == HCI_STATE_WORKING) {
bt_send_cmd(&hci_write_inquiry_mode, 0x01); // with RSSI
state = W4_INQUIRY_MODE_COMPLETE;
}
break;
case W4_INQUIRY_MODE_COMPLETE:
switch(event){
case HCI_EVENT_COMMAND_COMPLETE:
if ( COMMAND_COMPLETE_EVENT(packet, hci_write_inquiry_mode) ) {
start_scan();
state = ACTIVE;
}
break;
case HCI_EVENT_COMMAND_STATUS:
if ( COMMAND_STATUS_EVENT(packet, hci_write_inquiry_mode) ) {
printf("Ignoring error (0x%x) from hci_write_inquiry_mode.\n", packet[2]);
start_scan();
state = ACTIVE;
}
break;
default:
break;
}
break;
case ACTIVE:
switch(event){
case HCI_EVENT_INQUIRY_RESULT:
case HCI_EVENT_INQUIRY_RESULT_WITH_RSSI:
numResponses = packet[2];
for (i=0; i<numResponses && deviceCount < MAX_DEVICES;i++){
bt_flip_addr(addr, &packet[3+i*6]);
int index = getDeviceIndexForAddress(addr);
if (index >= 0) continue;
memcpy(devices[deviceCount].address, addr, 6);
devices[deviceCount].pageScanRepetitionMode = packet [3 + numResponses*(6) + i*1];
if (event == HCI_EVENT_INQUIRY_RESULT){
devices[deviceCount].classOfDevice = READ_BT_24(packet, 3 + numResponses*(6+1+1+1) + i*3);
devices[deviceCount].clockOffset = READ_BT_16(packet, 3 + numResponses*(6+1+1+1+3) + i*2) & 0x7fff;
devices[deviceCount].rssi = 0;
} else {
devices[deviceCount].classOfDevice = READ_BT_24(packet, 3 + numResponses*(6+1+1) + i*3);
devices[deviceCount].clockOffset = READ_BT_16(packet, 3 + numResponses*(6+1+1+3) + i*2) & 0x7fff;
devices[deviceCount].rssi = packet [3 + numResponses*(6+1+1+3+2) + i*1];
}
devices[deviceCount].state = REMOTE_NAME_REQUEST;
printf("Device found: %s with COD: 0x%06x, pageScan %u, clock offset 0x%04x, rssi 0x%02x\n", bd_addr_to_str(addr),
devices[deviceCount].classOfDevice, devices[deviceCount].pageScanRepetitionMode,
devices[deviceCount].clockOffset, devices[deviceCount].rssi);
deviceCount++;
}
break;
case HCI_EVENT_INQUIRY_COMPLETE:
for (i=0;i<deviceCount;i++) {
// retry remote name request
if (devices[i].state == REMOTE_NAME_INQUIRED)
devices[i].state = REMOTE_NAME_REQUEST;
}
if (has_more_remote_name_requests()){
do_next_remote_name_request();
break;
}
start_scan();
break;
case BTSTACK_EVENT_REMOTE_NAME_CACHED:
bt_flip_addr(addr, &packet[3]);
printf("Cached remote name for %s: '%s'\n", bd_addr_to_str(addr), &packet[9]);
break;
case HCI_EVENT_REMOTE_NAME_REQUEST_COMPLETE:
bt_flip_addr(addr, &packet[3]);
int index = getDeviceIndexForAddress(addr);
if (index >= 0) {
if (packet[2] == 0) {
printf("Name: '%s'\n", &packet[9]);
devices[index].state = REMOTE_NAME_FETCHED;
} else {
printf("Failed to get name: page timeout\n");
}
}
if (has_more_remote_name_requests()){
do_next_remote_name_request();
break;
}
start_scan();
break;
default:
break;
}
break;
default:
break;
}
}
static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
UNUSED(channel);
bd_addr_t event_addr;
uint8_t rfcomm_channel_nr;
uint16_t mtu;
int i;
switch (packet_type) {
case HCI_EVENT_PACKET:
switch (hci_event_packet_get_type(packet)) {
case HCI_EVENT_PIN_CODE_REQUEST:
// inform about pin code request
printf("Pin code request - using '0000'\n");
hci_event_pin_code_request_get_bd_addr(packet, event_addr);
gap_pin_code_response(event_addr, "0000");
break;
case HCI_EVENT_USER_CONFIRMATION_REQUEST:
// inform about user confirmation request
printf("SSP User Confirmation Request with numeric value '%06"PRIu32"'\n", little_endian_read_32(packet, 8));
printf("SSP User Confirmation Auto accept\n");
break;
case HCI_EVENT_DISCONNECTION_COMPLETE:
le_notification_enabled = 0;
break;
case ATT_EVENT_CAN_SEND_NOW:
att_server_notify(att_con_handle, ATT_CHARACTERISTIC_0000FF11_0000_1000_8000_00805F9B34FB_01_VALUE_HANDLE, (uint8_t*) counter_string, counter_string_len);
break;
case RFCOMM_EVENT_INCOMING_CONNECTION:
// data: event (8), len(8), address(48), channel (8), rfcomm_cid (16)
rfcomm_event_incoming_connection_get_bd_addr(packet, event_addr);
rfcomm_channel_nr = rfcomm_event_incoming_connection_get_server_channel(packet);
rfcomm_channel_id = rfcomm_event_incoming_connection_get_rfcomm_cid(packet);
printf("RFCOMM channel %u requested for %s\n", rfcomm_channel_nr, bd_addr_to_str(event_addr));
rfcomm_accept_connection(rfcomm_channel_id);
break;
case RFCOMM_EVENT_CHANNEL_OPENED:
// data: event(8), len(8), status (8), address (48), server channel(8), rfcomm_cid(16), max frame size(16)
if (rfcomm_event_channel_opened_get_status(packet)) {
printf("RFCOMM channel open failed, status %u\n", rfcomm_event_channel_opened_get_status(packet));
} else {
rfcomm_channel_id = rfcomm_event_channel_opened_get_rfcomm_cid(packet);
mtu = rfcomm_event_channel_opened_get_max_frame_size(packet);
printf("RFCOMM channel open succeeded. New RFCOMM Channel ID %u, max frame size %u\n", rfcomm_channel_id, mtu);
}
break;
case RFCOMM_EVENT_CAN_SEND_NOW:
rfcomm_send(rfcomm_channel_id, (uint8_t*) counter_string, counter_string_len);
break;
case RFCOMM_EVENT_CHANNEL_CLOSED:
printf("RFCOMM channel closed\n");
rfcomm_channel_id = 0;
break;
default:
break;
}
break;
case RFCOMM_DATA_PACKET:
printf("RCV: '");
for (i=0;i<size;i++){
putchar(packet[i]);
}
printf("'\n");
break;
default:
break;
}
}
void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
bd_addr_t event_addr;
switch (packet_type) {
case RFCOMM_DATA_PACKET:
printf("Received RFCOMM data on channel id %u, size %u\n", channel, size);
hexdump(packet, size);
bt_send_rfcomm(channel, packet, size);
break;
case HCI_EVENT_PACKET:
switch (packet[0]) {
case BTSTACK_EVENT_POWERON_FAILED:
// handle HCI init failure
printf("HCI Init failed - make sure you have turned off Bluetooth in the System Settings\n");
exit(1);
break;
case BTSTACK_EVENT_STATE:
// bt stack activated, get started
if (packet[2] == HCI_STATE_WORKING) {
// get persistent RFCOMM channel
printf("HCI_STATE_WORKING\n");
bt_send_cmd(&rfcomm_persistent_channel_for_service, "ch.ringwald.btstack.rfcomm-echo2");
}
break;
case RFCOMM_EVENT_PERSISTENT_CHANNEL:
rfcomm_channel_nr = packet[3];
printf("RFCOMM channel %u was assigned by BTdaemon\n", rfcomm_channel_nr);
bt_send_cmd(&rfcomm_register_service, rfcomm_channel_nr, 100); // reserved channel, mtu=100
break;
case RFCOMM_EVENT_SERVICE_REGISTERED:
printf("RFCOMM_EVENT_SERVICE_REGISTERED channel: %u, status: 0x%02x\n", packet[3], packet[2]);
// register SDP for our SPP
create_spp_service(service_buffer, rfcomm_channel_nr);
bt_send_cmd(&sdp_register_service_record, service_buffer);
bt_send_cmd(&btstack_set_discoverable, 1);
break;
case HCI_EVENT_PIN_CODE_REQUEST:
// inform about pin code request
printf("Using PIN 0000\n");
bt_flip_addr(event_addr, &packet[2]);
bt_send_cmd(&hci_pin_code_request_reply, &event_addr, 4, "0000");
break;
case RFCOMM_EVENT_INCOMING_CONNECTION:
// data: event (8), len(8), address(48), channel (8), rfcomm_cid (16)
bt_flip_addr(event_addr, &packet[2]);
rfcomm_channel_nr = packet[8];
rfcomm_channel_id = READ_BT_16(packet, 9);
printf("RFCOMM channel %u requested for %s\n", rfcomm_channel_nr, bd_addr_to_str(event_addr));
bt_send_cmd(&rfcomm_accept_connection, rfcomm_channel_id);
break;
case RFCOMM_EVENT_OPEN_CHANNEL_COMPLETE:
// data: event(8), len(8), status (8), address (48), handle(16), server channel(8), rfcomm_cid(16), max frame size(16)
if (packet[2]) {
printf("RFCOMM channel open failed, status %u\n", packet[2]);
} else {
rfcomm_channel_id = READ_BT_16(packet, 12);
mtu = READ_BT_16(packet, 14);
printf("RFCOMM channel open succeeded. New RFCOMM Channel ID %u, max frame size %u\n", rfcomm_channel_id, mtu);
}
break;
case HCI_EVENT_DISCONNECTION_COMPLETE:
// connection closed -> quit test app
printf("Basebank connection closed\n");
break;
default:
break;
}
break;
default:
break;
}
}
static void user_command(char cmd){
switch (cmd){
case 'a':
memcpy(device_addr, pts_addr, 6);
printf("Establish HFP service level connection to PTS module %s...\n", bd_addr_to_str(device_addr));
hfp_ag_establish_service_level_connection(device_addr);
break;
case 'A':
printf("Release HFP service level connection.\n");
hfp_ag_release_service_level_connection(device_addr);
break;
case 'Z':
printf("Release HFP service level connection to %s...\n", bd_addr_to_str(device_addr));
hfp_ag_release_service_level_connection(device_addr);
break;
case 'b':
printf("Establish Audio connection %s...\n", bd_addr_to_str(device_addr));
hfp_ag_establish_audio_connection(device_addr);
break;
case 'B':
printf("Release Audio connection.\n");
hfp_ag_release_audio_connection(device_addr);
break;
case 'c':
printf("Simulate incoming call from 1234567\n");
current_call_exists_a = 1;
current_call_status_a = HFP_ENHANCED_CALL_STATUS_INCOMING;
current_call_dir = HFP_ENHANCED_CALL_DIR_INCOMING;
hfp_ag_set_clip(129, "1234567");
hfp_ag_incoming_call();
break;
case 'm':
printf("Simulate incoming call from 7654321\n");
current_call_exists_b = 1;
current_call_status_b = HFP_ENHANCED_CALL_STATUS_INCOMING;
current_call_dir = HFP_ENHANCED_CALL_DIR_INCOMING;
hfp_ag_set_clip(129, "7654321");
hfp_ag_incoming_call();
break;
case 'C':
printf("Simulate terminate call\n");
hfp_ag_call_dropped();
break;
case 'd':
printf("Report AG failure\n");
hfp_ag_report_extended_audio_gateway_error_result_code(device_addr, HFP_CME_ERROR_AG_FAILURE);
break;
case 'e':
printf("Answer call on AG\n");
if (current_call_status_a == HFP_ENHANCED_CALL_STATUS_INCOMING){
current_call_status_a = HFP_ENHANCED_CALL_STATUS_ACTIVE;
}
if (current_call_status_b == HFP_ENHANCED_CALL_STATUS_INCOMING){
current_call_status_b = HFP_ENHANCED_CALL_STATUS_ACTIVE;
current_call_status_a = HFP_ENHANCED_CALL_STATUS_HELD;
}
hfp_ag_answer_incoming_call();
break;
case 'E':
printf("Reject call on AG\n");
hfp_ag_terminate_call();
break;
case 'f':
printf("Disable cellular network\n");
hfp_ag_set_registration_status(0);
break;
case 'F':
printf("Enable cellular network\n");
hfp_ag_set_registration_status(1);
break;
case 'g':
printf("Set signal strength to 0\n");
hfp_ag_set_signal_strength(0);
break;
case 'G':
printf("Set signal strength to 5\n");
hfp_ag_set_signal_strength(5);
break;
case 'h':
printf("Disable roaming\n");
hfp_ag_set_roaming_status(0);
break;
case 'H':
printf("Enable roaming\n");
hfp_ag_set_roaming_status(1);
break;
case 'i':
printf("Set battery level to 3\n");
hfp_ag_set_battery_level(3);
break;
case 'I':
printf("Set battery level to 5\n");
hfp_ag_set_battery_level(5);
break;
case 'j':
printf("Answering call on remote side\n");
hfp_ag_outgoing_call_established();
break;
case 'r':
printf("Disable in-band ring tone\n");
hfp_ag_set_use_in_band_ring_tone(0);
break;
case 'k':
printf("Memory 1 cleared\n");
memory_1_enabled = 0;
break;
case 'K':
printf("Memory 1 set\n");
memory_1_enabled = 1;
break;
case 'l':
printf("Last dialed number cleared\n");
last_number_exists = 0;
break;
case 'L':
printf("Last dialed number set\n");
last_number_exists = 1;
break;
case 'n':
printf("Disable Voice Recognition\n");
hfp_ag_activate_voice_recognition(device_addr, 0);
break;
case 'N':
printf("Enable Voice Recognition\n");
hfp_ag_activate_voice_recognition(device_addr, 1);
break;
case 'o':
printf("Set speaker gain to 0 (minimum)\n");
hfp_ag_set_speaker_gain(device_addr, 0);
break;
case 'O':
printf("Set speaker gain to 9 (default)\n");
hfp_ag_set_speaker_gain(device_addr, 9);
break;
case 'p':
printf("Set speaker gain to 12 (higher)\n");
hfp_ag_set_speaker_gain(device_addr, 12);
break;
case 'P':
printf("Set speaker gain to 15 (maximum)\n");
hfp_ag_set_speaker_gain(device_addr, 15);
break;
case 'q':
printf("Set microphone gain to 0\n");
hfp_ag_set_microphone_gain(device_addr, 0);
break;
case 'Q':
printf("Set microphone gain to 9\n");
hfp_ag_set_microphone_gain(device_addr, 9);
break;
case 's':
printf("Set microphone gain to 12\n");
hfp_ag_set_microphone_gain(device_addr, 12);
break;
case 'S':
printf("Set microphone gain to 15\n");
hfp_ag_set_microphone_gain(device_addr, 15);
break;
case 'R':
printf("Enable in-band ring tone\n");
hfp_ag_set_use_in_band_ring_tone(1);
break;
case 't':
printf("Terminate HCI connection.\n");
gap_disconnect(handle);
break;
case 'u':
printf("Join held call\n");
current_call_mpty = HFP_ENHANCED_CALL_MPTY_CONFERENCE_CALL;
hfp_ag_join_held_call();
break;
case 'v':
printf("Starting inquiry scan..\n");
// hci_send_cmd(&hci_inquiry, HCI_INQUIRY_LAP, INQUIRY_INTERVAL, 0);
break;
case 'w':
printf("AG: Put incoming call on hold (Response and Hold)\n");
hfp_ag_hold_incoming_call();
break;
case 'x':
printf("AG: Accept held incoming call (Response and Hold)\n");
hfp_ag_accept_held_incoming_call();
break;
case 'X':
printf("AG: Reject held incoming call (Response and Hold)\n");
hfp_ag_reject_held_incoming_call();
break;
default:
break;
}
}
static void user_command(char cmd){
switch (cmd){
case '#':
case '-':
case '+':
case '*':
printf("DTMF Code: %c\n", cmd);
hfp_hf_send_dtmf_code(device_addr, cmd);
break;
case 'a':
printf("Establish Service level connection to device with Bluetooth address %s...\n", bd_addr_to_str(device_addr));
hfp_hf_establish_service_level_connection(device_addr);
break;
case 'A':
printf("Release Service level connection.\n");
hfp_hf_release_service_level_connection(device_addr);
break;
case 'b':
printf("Establish Audio connection to device with Bluetooth address %s...\n", bd_addr_to_str(device_addr));
hfp_hf_establish_audio_connection(device_addr);
break;
case 'B':
printf("Release Audio service level connection.\n");
hfp_hf_release_audio_connection(device_addr);
break;
case 'C':
printf("Enable registration status update for all AG indicators.\n");
hfp_hf_enable_status_update_for_all_ag_indicators(device_addr);
case 'c':
printf("Disable registration status update for all AG indicators.\n");
hfp_hf_disable_status_update_for_all_ag_indicators(device_addr);
break;
case 'D':
printf("Set HFP AG registration status update for individual indicators (0111111).\n");
hfp_hf_set_status_update_for_individual_ag_indicators(device_addr, 63);
break;
case 'd':
printf("Query network operator.\n");
hfp_hf_query_operator_selection(device_addr);
break;
case 'E':
printf("Enable reporting of the extended AG error result code.\n");
hfp_hf_enable_report_extended_audio_gateway_error_result_code(device_addr);
break;
case 'e':
printf("Disable reporting of the extended AG error result code.\n");
hfp_hf_disable_report_extended_audio_gateway_error_result_code(device_addr);
break;
case 'f':
printf("Answer incoming call.\n");
hfp_hf_answer_incoming_call(device_addr);
break;
case 'F':
printf("Hangup call.\n");
hfp_hf_terminate_call(device_addr);
break;
case 'G':
printf("Reject incoming call.\n");
hfp_hf_reject_incoming_call(device_addr);
break;
case 'g':
printf("Query operator.\n");
hfp_hf_query_operator_selection(device_addr);
break;
case 't':
printf("Terminate HCI connection.\n");
gap_disconnect(handle);
break;
case 'i':
printf("Dial 1234567\n");
hfp_hf_dial_number(device_addr, (char *)"1234567");
break;
case 'I':
printf("Dial 7654321\n");
hfp_hf_dial_number(device_addr, (char *)"7654321");
break;
case 'j':
printf("Dial #1\n");
hfp_hf_dial_memory(device_addr, 1);
break;
case 'J':
printf("Dial #99\n");
hfp_hf_dial_memory(device_addr, 99);
break;
case 'k':
printf("Deactivate call waiting notification\n");
hfp_hf_deactivate_call_waiting_notification(device_addr);
break;
case 'K':
printf("Activate call waiting notification\n");
hfp_hf_activate_call_waiting_notification(device_addr);
break;
case 'l':
printf("Deactivate calling line notification\n");
hfp_hf_deactivate_calling_line_notification(device_addr);
break;
case 'L':
printf("Activate calling line notification\n");
hfp_hf_activate_calling_line_notification(device_addr);
break;
case 'm':
printf("Deactivate echo canceling and noise reduction\n");
hfp_hf_deactivate_echo_canceling_and_noise_reduction(device_addr);
break;
case 'M':
printf("Activate echo canceling and noise reduction\n");
hfp_hf_activate_echo_canceling_and_noise_reduction(device_addr);
break;
case 'n':
printf("Deactivate voice recognition\n");
hfp_hf_deactivate_voice_recognition_notification(device_addr);
break;
case 'N':
printf("Activate voice recognition\n");
hfp_hf_activate_voice_recognition_notification(device_addr);
break;
case 'o':
printf("Set speaker gain to 0 (minimum)\n");
hfp_hf_set_speaker_gain(device_addr, 0);
break;
case 'O':
printf("Set speaker gain to 9 (default)\n");
hfp_hf_set_speaker_gain(device_addr, 9);
break;
case 'p':
printf("Set speaker gain to 12 (higher)\n");
hfp_hf_set_speaker_gain(device_addr, 12);
break;
case 'P':
printf("Set speaker gain to 15 (maximum)\n");
hfp_hf_set_speaker_gain(device_addr, 15);
break;
case 'q':
printf("Set microphone gain to 0\n");
hfp_hf_set_microphone_gain(device_addr, 0);
break;
case 'Q':
printf("Set microphone gain to 9\n");
hfp_hf_set_microphone_gain(device_addr, 9);
break;
case 's':
printf("Set microphone gain to 12\n");
hfp_hf_set_microphone_gain(device_addr, 12);
break;
case 'S':
printf("Set microphone gain to 15\n");
hfp_hf_set_microphone_gain(device_addr, 15);
break;
case 'u':
printf("Send 'user busy' (Three-Way Call 0)\n");
hfp_hf_user_busy(device_addr);
break;
case 'U':
printf("End active call and accept waiting/held call (Three-Way Call 1)\n");
hfp_hf_end_active_and_accept_other(device_addr);
break;
case 'v':
printf("Swap active call and hold/waiting call (Three-Way Call 2)\n");
hfp_hf_swap_calls(device_addr);
break;
case 'V':
printf("Join hold call (Three-Way Call 3)\n");
hfp_hf_join_held_call(device_addr);
break;
case 'w':
printf("Connect calls (Three-Way Call 4)\n");
hfp_hf_connect_calls(device_addr);
break;
case 'W':
printf("Redial\n");
hfp_hf_redial_last_number(device_addr);
break;
case 'x':
printf("Request phone number for voice tag\n");
hfp_hf_request_phone_number_for_voice_tag(device_addr);
break;
case 'X':
printf("Query current call status\n");
hfp_hf_query_current_call_status(device_addr);
break;
case 'y':
printf("Release call with index 2\n");
hfp_hf_release_call_with_index(device_addr, 2);
break;
case 'Y':
printf("Private consulation with call 2\n");
hfp_hf_private_consultation_with_call(device_addr, 2);
break;
case '[':
printf("Query Response and Hold status (RHH ?)\n");
hfp_hf_rrh_query_status(device_addr);
break;
case ']':
printf("Place call in a response and held state (RHH 0)\n");
hfp_hf_rrh_hold_call(device_addr);
break;
case '{':
printf("Accept held call (RHH 1)\n");
hfp_hf_rrh_accept_held_call(device_addr);
break;
case '}':
printf("Reject held call (RHH 2)\n");
hfp_hf_rrh_reject_held_call(device_addr);
break;
case '?':
printf("Query Subscriber Number\n");
hfp_hf_query_subscriber_number(device_addr);
break;
case '!':
printf("Update HF indicator with assigned number 1 (HFI)\n");
hfp_hf_set_hf_indicator(device_addr, 1, 1);
break;
default:
printf("HF: undefined user command\n");
break;
}
}
static void hci_event_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
if (packet_type != HCI_EVENT_PACKET) return;
uint8_t event = hci_event_packet_get_type(packet);
switch (event) {
case BTSTACK_EVENT_STATE:
// BTstack activated, get started
if (btstack_event_state_get_state(packet) != HCI_STATE_WORKING) break;
if (cmdline_addr_found){
printf("Connect to %s\n", bd_addr_to_str(cmdline_addr));
state = TC_W4_CONNECT;
gap_connect(cmdline_addr, 0);
break;
}
printf("Start scanning!\n");
state = TC_W4_SCAN_RESULT;
gap_set_scan_parameters(0,0x0030, 0x0030);
gap_start_scan();
break;
case GAP_EVENT_ADVERTISING_REPORT:
if (state != TC_W4_SCAN_RESULT) return;
fill_advertising_report_from_packet(&report, packet);
if (blacklist_contains(report.address)) {
break;
}
dump_advertising_report(&report);
// stop scanning, and connect to the device
state = TC_W4_CONNECT;
gap_stop_scan();
printf("Stop scan. Connect to device with addr %s.\n", bd_addr_to_str(report.address));
gap_connect(report.address,report.address_type);
break;
case HCI_EVENT_LE_META:
// wait for connection complete
if (hci_event_le_meta_get_subevent_code(packet) != HCI_SUBEVENT_LE_CONNECTION_COMPLETE) break;
if (state != TC_W4_CONNECT) return;
connection_handle = hci_subevent_le_connection_complete_get_connection_handle(packet);
// initialize gatt client context with handle, and add it to the list of active clients
// query primary services
printf("\nSearch for battery service.\n");
state = TC_W4_SERVICE_RESULT;
gatt_client_discover_primary_services_by_uuid16(handle_gatt_client_event, connection_handle, battery_service_uuid);
break;
case HCI_EVENT_DISCONNECTION_COMPLETE:
if (cmdline_addr_found){
printf("\nDisconnected %s\n", bd_addr_to_str(cmdline_addr));
exit(0);
}
printf("\nDisconnected %s\n", bd_addr_to_str(report.address));
printf("Restart scan.\n");
state = TC_W4_SCAN_RESULT;
gap_start_scan();
break;
default:
break;
}
}
static void add_to_blacklist(bd_addr_t addr){
printf("%s added to blacklist (no battery service found\n", bd_addr_to_str(addr));
bd_addr_copy(blacklist[blacklist_index], addr);
blacklist_index = (blacklist_index + 1) % blacklist_size();
}
static void handle_query_rfcomm_event(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
switch (hci_event_packet_get_type(packet)){
case SDP_EVENT_QUERY_RFCOMM_SERVICE:
channel_nr = sdp_event_query_rfcomm_service_get_rfcomm_channel(packet);
log_info("** Service name: '%s', RFCOMM port %u", sdp_event_query_rfcomm_service_get_name(packet), channel_nr);
break;
case SDP_EVENT_QUERY_COMPLETE:
if (channel_nr > 0){
hsp_state = HSP_W4_RFCOMM_CONNECTED;
log_info("HSP: SDP_QUERY_COMPLETE. RFCOMM create channel, addr %s, rfcomm channel nr %d", bd_addr_to_str(remote), channel_nr);
rfcomm_create_channel(packet_handler, remote, channel_nr, NULL);
break;
}
hsp_hs_reset_state();
log_info("Service not found, status %u.", sdp_event_query_complete_get_status(packet));
break;
}
}
static void handle_query_rfcomm_event(sdp_query_event_t * event, void * context){
sdp_query_rfcomm_service_event_t * ve;
sdp_query_complete_event_t * ce;
switch (event->type){
case SDP_QUERY_RFCOMM_SERVICE:
ve = (sdp_query_rfcomm_service_event_t*) event;
channel_nr = ve->channel_nr;
log_info("** Service name: '%s', RFCOMM port %u", ve->service_name, channel_nr);
break;
case SDP_QUERY_COMPLETE:
ce = (sdp_query_complete_event_t*) event;
if (channel_nr > 0){
hsp_state = HSP_W4_RFCOMM_CONNECTED;
log_info("HSP: SDP_QUERY_COMPLETE. RFCOMM create channel, addr %s, rfcomm channel nr %d", bd_addr_to_str(remote), channel_nr);
rfcomm_create_channel_internal(NULL, remote, channel_nr);
break;
}
hsp_hs_reset_state();
log_info("Service not fou nd, status %u.", ce->status);
break;
}
}
static void PacketHandler(void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) {
bd_addr_t event_addr;
uint8_t rfcomm_channel_nr;
uint16_t mtu;
switch (packet_type) {
case HCI_EVENT_PACKET:
switch (packet[0]) {
case BTSTACK_EVENT_STATE:
// bt stack activated, get started - set local name
if (packet[2] == HCI_STATE_WORKING) {
hci_send_cmd(&hci_write_local_name, local_name);
}
break;
case HCI_EVENT_COMMAND_COMPLETE:
if (COMMAND_COMPLETE_EVENT(packet, hci_read_bd_addr)) {
bt_flip_addr(event_addr, &packet[6]);
log_printf("BD-ADDR: %s\n\r", bd_addr_to_str(event_addr));
sprintf(local_name, "IOIO (%02X:%02X)", event_addr[4], event_addr[5]);
break;
}
if (COMMAND_COMPLETE_EVENT(packet, hci_write_local_name)) {
hci_discoverable_control(1);
break;
}
break;
case HCI_EVENT_LINK_KEY_REQUEST:
// deny link key request
log_printf("Link key request\n\r");
bt_flip_addr(event_addr, &packet[2]);
hci_send_cmd(&hci_link_key_request_negative_reply, &event_addr);
break;
case HCI_EVENT_PIN_CODE_REQUEST:
// inform about pin code request
log_printf("Pin code request - using '4545'\n\r");
bt_flip_addr(event_addr, &packet[2]);
hci_send_cmd(&hci_pin_code_request_reply, &event_addr, 4, "4545");
break;
case RFCOMM_EVENT_INCOMING_CONNECTION:
// data: event (8), len(8), address(48), channel (8), rfcomm_cid (16)
bt_flip_addr(event_addr, &packet[2]);
rfcomm_channel_nr = packet[8];
rfcomm_channel_id = READ_BT_16(packet, 9);
log_printf("RFCOMM channel %u requested for %s\n\r", rfcomm_channel_nr, bd_addr_to_str(event_addr));
rfcomm_accept_connection_internal(rfcomm_channel_id);
break;
case RFCOMM_EVENT_OPEN_CHANNEL_COMPLETE:
// data: event(8), len(8), status (8), address (48), server channel(8), rfcomm_cid(16), max frame size(16)
if (packet[2]) {
log_printf("RFCOMM channel open failed, status %u\n\r", packet[2]);
} else {
rfcomm_channel_id = READ_BT_16(packet, 12);
rfcomm_send_credit = 1;
mtu = READ_BT_16(packet, 14);
log_printf("\n\rRFCOMM channel open succeeded. New RFCOMM Channel ID %u, max frame size %u\n\r", rfcomm_channel_id, mtu);
}
break;
case RFCOMM_EVENT_CHANNEL_CLOSED:
log_printf("RFCOMM channel closed.");
client_callback(NULL, 0, client_callback_arg);
client_callback = DummyCallback;
rfcomm_channel_id = 0;
break;
default:
break;
}
break;
case RFCOMM_DATA_PACKET:
client_callback(packet, size, client_callback_arg);
rfcomm_send_credit = 1;
default:
break;
}
}
static void packet_handler (void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
//printf("packet_handler type %u, packet[0] %x\n", packet_type, packet[0]);
if (packet_type == RFCOMM_DATA_PACKET){
// skip over leading newline
while (size > 0 && (packet[0] == '\n' || packet[0] == '\r')){
size--;
packet++;
}
if (strncmp((char *)packet, HSP_AG_RING, strlen(HSP_AG_RING)) == 0){
emit_ring_event();
} else if (strncmp((char *)packet, HSP_AG_OK, strlen(HSP_AG_OK)) == 0){
wait_ok = 0;
} else if (strncmp((char *)packet, HSP_MICROPHONE_GAIN, strlen(HSP_MICROPHONE_GAIN)) == 0){
uint8_t gain = (uint8_t)atoi((char*)&packet[strlen(HSP_MICROPHONE_GAIN)]);
emit_event(HSP_SUBEVENT_MICROPHONE_GAIN_CHANGED, gain);
} else if (strncmp((char *)packet, HSP_SPEAKER_GAIN, strlen(HSP_SPEAKER_GAIN)) == 0){
uint8_t gain = (uint8_t)atoi((char*)&packet[strlen(HSP_SPEAKER_GAIN)]);
emit_event(HSP_SUBEVENT_SPEAKER_GAIN_CHANGED, gain);
} else {
if (!hsp_hs_callback) return;
// strip trailing newline
while (size > 0 && (packet[size-1] == '\n' || packet[size-1] == '\r')){
size--;
}
// add trailing \0
packet[size] = 0;
// re-use incoming buffer to avoid reserving large buffers - ugly but efficient
uint8_t * event = packet - 4;
event[0] = HCI_EVENT_HSP_META;
event[1] = size + 2;
event[2] = HSP_SUBEVENT_AG_INDICATION;
event[3] = size;
(*hsp_hs_callback)(event, size+4);
}
hsp_run();
return;
}
if (packet_type != HCI_EVENT_PACKET) return;
uint8_t event = packet[0];
bd_addr_t event_addr;
uint16_t handle;
switch (event) {
case HCI_EVENT_SYNCHRONOUS_CONNECTION_COMPLETE:{
if (hsp_state < HSP_RFCOMM_CONNECTION_ESTABLISHED) return;
int index = 2;
uint8_t status = packet[index++];
sco_handle = READ_BT_16(packet, index);
index+=2;
bd_addr_t address;
memcpy(address, &packet[index], 6);
index+=6;
uint8_t link_type = packet[index++];
uint8_t transmission_interval = packet[index++]; // measured in slots
uint8_t retransmission_interval = packet[index++];// measured in slots
uint16_t rx_packet_length = READ_BT_16(packet, index); // measured in bytes
index+=2;
uint16_t tx_packet_length = READ_BT_16(packet, index); // measured in bytes
index+=2;
uint8_t air_mode = packet[index];
if (status != 0){
log_error("(e)SCO Connection failed, status %u", status);
emit_event_audio_connected(status, sco_handle);
hsp_state = HSP_RFCOMM_CONNECTION_ESTABLISHED ;
break;
}
switch (link_type){
case 0x00:
log_info("SCO Connection established.");
if (transmission_interval != 0) log_error("SCO Connection: transmission_interval not zero: %d.", transmission_interval);
if (retransmission_interval != 0) log_error("SCO Connection: retransmission_interval not zero: %d.", retransmission_interval);
if (rx_packet_length != 0) log_error("SCO Connection: rx_packet_length not zero: %d.", rx_packet_length);
if (tx_packet_length != 0) log_error("SCO Connection: tx_packet_length not zero: %d.", tx_packet_length);
break;
case 0x02:
log_info("eSCO Connection established.");
break;
default:
log_error("(e)SCO reserved link_type 0x%2x", link_type);
break;
}
log_info("sco_handle 0x%2x, address %s, transmission_interval %u slots, retransmission_interval %u slots, "
" rx_packet_length %u bytes, tx_packet_length %u bytes, air_mode 0x%2x (0x02 == CVSD)", sco_handle,
bd_addr_to_str(address), transmission_interval, retransmission_interval, rx_packet_length, tx_packet_length, air_mode);
// forward event to app
hsp_hs_callback(packet, size);
hsp_state = HSP_AUDIO_CONNECTION_ESTABLISHED;
emit_event_audio_connected(status, sco_handle);
break;
}
case RFCOMM_EVENT_INCOMING_CONNECTION:
// data: event (8), len(8), address(48), channel (8), rfcomm_cid (16)
if (hsp_state != HSP_IDLE) return;
bt_flip_addr(event_addr, &packet[2]);
rfcomm_cid = READ_BT_16(packet, 9);
log_info("RFCOMM channel %u requested for %s", packet[8], bd_addr_to_str(event_addr));
hsp_state = HSP_W4_RFCOMM_CONNECTED;
rfcomm_accept_connection_internal(rfcomm_cid);
break;
case RFCOMM_EVENT_OPEN_CHANNEL_COMPLETE:
// printf("RFCOMM_EVENT_OPEN_CHANNEL_COMPLETE packet_handler type %u, packet[0] %x\n", packet_type, packet[0]);
// data: event(8), len(8), status (8), address (48), handle(16), server channel(8), rfcomm_cid(16), max frame size(16)
if (hsp_state != HSP_W4_RFCOMM_CONNECTED) return;
if (packet[2]) {
log_info("RFCOMM channel open failed, status %u", packet[2]);
hsp_state = HSP_IDLE;
hsp_hs_reset_state();
} else {
// data: event(8) , len(8), status (8), address (48), handle (16), server channel(8), rfcomm_cid(16), max frame size(16)
rfcomm_handle = READ_BT_16(packet, 9);
rfcomm_cid = READ_BT_16(packet, 12);
mtu = READ_BT_16(packet, 14);
log_info("RFCOMM channel open succeeded. New RFCOMM Channel ID %u, max frame size %u, handle %02x", rfcomm_cid, mtu, rfcomm_handle);
hsp_state = HSP_RFCOMM_CONNECTION_ESTABLISHED;
}
emit_event(HSP_SUBEVENT_RFCOMM_CONNECTION_COMPLETE, packet[2]);
break;
case BTSTACK_EVENT_STATE:
case DAEMON_EVENT_HCI_PACKET_SENT:
case HCI_EVENT_NUMBER_OF_COMPLETED_PACKETS:
case RFCOMM_EVENT_CREDITS:
hsp_hs_callback(packet, size);
break;
case HCI_EVENT_DISCONNECTION_COMPLETE:
handle = READ_BT_16(packet,3);
if (handle == sco_handle){
sco_handle = 0;
hsp_state = HSP_RFCOMM_CONNECTION_ESTABLISHED;
emit_event(HSP_SUBEVENT_AUDIO_DISCONNECTION_COMPLETE,0);
break;
}
if (handle == rfcomm_handle) {
rfcomm_handle = 0;
hsp_state = HSP_IDLE;
emit_event(HSP_SUBEVENT_RFCOMM_DISCONNECTION_COMPLETE,0);
hsp_hs_reset_state();
}
break;
case RFCOMM_EVENT_CHANNEL_CLOSED:
hsp_hs_reset_state();
hsp_hs_callback(packet, size);
break;
default:
break;
}
hsp_run();
}
static void handle_query_rfcomm_event(sdp_query_event_t * event, void * context){
sdp_query_rfcomm_service_event_t * ve;
sdp_query_complete_event_t * ce;
hfp_connection_t * connection = connection_doing_sdp_query;
if ( connection->state != HFP_W4_SDP_QUERY_COMPLETE) return;
switch (event->type){
case SDP_QUERY_RFCOMM_SERVICE:
ve = (sdp_query_rfcomm_service_event_t*) event;
if (!connection) {
log_error("handle_query_rfcomm_event alloc connection for RFCOMM port %u failed", ve->channel_nr);
return;
}
connection->rfcomm_channel_nr = ve->channel_nr;
break;
case SDP_QUERY_COMPLETE:
connection_doing_sdp_query = NULL;
ce = (sdp_query_complete_event_t*) event;
if (connection->rfcomm_channel_nr > 0){
connection->state = HFP_W4_RFCOMM_CONNECTED;
log_info("HFP: SDP_QUERY_COMPLETE context %p, addr %s, state %d", connection, bd_addr_to_str( connection->remote_addr), connection->state);
rfcomm_create_channel_internal(NULL, connection->remote_addr, connection->rfcomm_channel_nr);
break;
}
log_info("rfcomm service not found, status %u.", ce->status);
break;
default:
break;
}
}
static void dump_ad_event(ad_event_t * e){
printf(" *** adv. event *** evt-type %u, addr-type %u, addr %s, rssi %u, length adv %u, data: ", e->event_type,
e->address_type, bd_addr_to_str(e->address), e->rssi, e->length);
printf_hexdump(e->data, e->length);
}
void hfp_handle_hci_event(hfp_callback_t callback, uint8_t packet_type, uint8_t *packet, uint16_t size){
bd_addr_t event_addr;
uint16_t rfcomm_cid, handle;
hfp_connection_t * context = NULL;
switch (packet[0]) {
case BTSTACK_EVENT_STATE:
// bt stack activated, get started
if (packet[2] == HCI_STATE_WORKING){
printf("BTstack activated, get started .\n");
}
break;
case HCI_EVENT_PIN_CODE_REQUEST:
// inform about pin code request
printf("Pin code request - using '0000'\n\r");
bt_flip_addr(event_addr, &packet[2]);
hci_send_cmd(&hci_pin_code_request_reply, &event_addr, 4, "0000");
break;
case RFCOMM_EVENT_INCOMING_CONNECTION:
// data: event (8), len(8), address(48), channel (8), rfcomm_cid (16)
bt_flip_addr(event_addr, &packet[2]);
context = get_hfp_connection_context_for_bd_addr(event_addr);
if (!context || context->state != HFP_IDLE) return;
context->rfcomm_cid = READ_BT_16(packet, 9);
context->state = HFP_W4_RFCOMM_CONNECTED;
printf("RFCOMM channel %u requested for %s\n", context->rfcomm_cid, bd_addr_to_str(context->remote_addr));
rfcomm_accept_connection_internal(context->rfcomm_cid);
break;
case RFCOMM_EVENT_OPEN_CHANNEL_COMPLETE:
// data: event(8), len(8), status (8), address (48), handle(16), server channel(8), rfcomm_cid(16), max frame size(16)
printf("RFCOMM_EVENT_OPEN_CHANNEL_COMPLETE packet_handler type %u, packet[0] %x\n", packet_type, packet[0]);
bt_flip_addr(event_addr, &packet[3]);
context = get_hfp_connection_context_for_bd_addr(event_addr);
if (!context || context->state != HFP_W4_RFCOMM_CONNECTED) return;
if (packet[2]) {
hfp_emit_event(callback, HFP_SUBEVENT_SERVICE_LEVEL_CONNECTION_ESTABLISHED, packet[2]);
remove_hfp_connection_context(context);
} else {
context->con_handle = READ_BT_16(packet, 9);
context->rfcomm_cid = READ_BT_16(packet, 12);
uint16_t mtu = READ_BT_16(packet, 14);
printf("RFCOMM channel open succeeded. Context %p, RFCOMM Channel ID 0x%02x, max frame size %u\n", context, context->rfcomm_cid, mtu);
switch (context->state){
case HFP_W4_RFCOMM_CONNECTED:
context->state = HFP_EXCHANGE_SUPPORTED_FEATURES;
break;
case HFP_W4_CONNECTION_ESTABLISHED_TO_SHUTDOWN:
context->state = HFP_W2_DISCONNECT_RFCOMM;
printf("Shutting down RFCOMM.\n");
break;
default:
break;
}
}
break;
case HCI_EVENT_SYNCHRONOUS_CONNECTION_COMPLETE:{
int index = 2;
uint8_t status = packet[index++];
uint16_t sco_handle = READ_BT_16(packet, index);
index+=2;
bd_addr_t address;
memcpy(address, &packet[index], 6);
index+=6;
uint8_t link_type = packet[index++];
uint8_t transmission_interval = packet[index++]; // measured in slots
uint8_t retransmission_interval = packet[index++];// measured in slots
uint16_t rx_packet_length = READ_BT_16(packet, index); // measured in bytes
index+=2;
uint16_t tx_packet_length = READ_BT_16(packet, index); // measured in bytes
index+=2;
uint8_t air_mode = packet[index];
if (status != 0){
log_error("(e)SCO Connection is not established, status %u", status);
break;
}
switch (link_type){
case 0x00:
printf("SCO Connection established. \n");
if (transmission_interval != 0) log_error("SCO Connection: transmission_interval not zero: %d.", transmission_interval);
if (retransmission_interval != 0) log_error("SCO Connection: retransmission_interval not zero: %d.", retransmission_interval);
if (rx_packet_length != 0) log_error("SCO Connection: rx_packet_length not zero: %d.", rx_packet_length);
if (tx_packet_length != 0) log_error("SCO Connection: tx_packet_length not zero: %d.", tx_packet_length);
break;
case 0x02:
printf("eSCO Connection established. \n");
break;
default:
log_error("(e)SCO reserved link_type 0x%2x", link_type);
break;
}
log_info("sco_handle 0x%2x, address %s, transmission_interval %u slots, retransmission_interval %u slots, "
" rx_packet_length %u bytes, tx_packet_length %u bytes, air_mode 0x%2x (0x02 == CVSD)", sco_handle,
bd_addr_to_str(address), transmission_interval, retransmission_interval, rx_packet_length, tx_packet_length, air_mode);
context = get_hfp_connection_context_for_bd_addr(address);
if (context->state == HFP_W4_CONNECTION_ESTABLISHED_TO_SHUTDOWN){
context->state = HFP_W2_DISCONNECT_SCO;
break;
}
context->sco_handle = sco_handle;
context->state = HFP_AUDIO_CONNECTION_ESTABLISHED;
hfp_emit_event(callback, HFP_SUBEVENT_AUDIO_CONNECTION_COMPLETE, packet[2]);
break;
}
case RFCOMM_EVENT_CHANNEL_CLOSED:
rfcomm_cid = READ_BT_16(packet,2);
context = get_hfp_connection_context_for_rfcomm_cid(rfcomm_cid);
if (!context) break;
if (context->state == HFP_W4_RFCOMM_DISCONNECTED_AND_RESTART){
context->state = HFP_IDLE;
hfp_establish_service_level_connection(context->remote_addr, context->service_uuid);
break;
}
remove_hfp_connection_context(context);
hfp_emit_event(callback, HFP_SUBEVENT_SERVICE_LEVEL_CONNECTION_RELEASED, 0);
break;
case HCI_EVENT_DISCONNECTION_COMPLETE:
handle = READ_BT_16(packet,3);
context = get_hfp_connection_context_for_handle(handle);
if (!context) break;
if (context->state == HFP_W4_RFCOMM_DISCONNECTED_AND_RESTART){
context->state = HFP_IDLE;
hfp_establish_service_level_connection(context->remote_addr, context->service_uuid);
break;
}
hfp_emit_event(callback, HFP_SUBEVENT_SERVICE_LEVEL_CONNECTION_RELEASED, packet[2]);
remove_hfp_connection_context(context);
break;
default:
break;
}
}
static void packet_handler (void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
bd_addr_t event_addr;
uint8_t rfcomm_channel_nr;
uint16_t mtu;
int i;
switch (packet_type) {
case HCI_EVENT_PACKET:
switch (packet[0]) {
case BTSTACK_EVENT_STATE:
// bt stack activated, get started - set local name
if (packet[2] == HCI_STATE_WORKING) {
hci_send_cmd(&hci_write_local_name, "BTstack SPP Counter");
}
break;
case HCI_EVENT_COMMAND_COMPLETE:
if (COMMAND_COMPLETE_EVENT(packet, hci_read_bd_addr)){
bt_flip_addr(event_addr, &packet[6]);
printf("BD-ADDR: %s\n", bd_addr_to_str(event_addr));
break;
}
break;
case HCI_EVENT_PIN_CODE_REQUEST:
// inform about pin code request
printf("Pin code request - using '0000'\n");
bt_flip_addr(event_addr, &packet[2]);
hci_send_cmd(&hci_pin_code_request_reply, &event_addr, 4, "0000");
break;
case HCI_EVENT_USER_CONFIRMATION_REQUEST:
// inform about user confirmation request
printf("SSP User Confirmation Request with numeric value '%06u'\n", READ_BT_32(packet, 8));
printf("SSP User Confirmation Auto accept\n");
break;
case RFCOMM_EVENT_INCOMING_CONNECTION:
// data: event (8), len(8), address(48), channel (8), rfcomm_cid (16)
bt_flip_addr(event_addr, &packet[2]);
rfcomm_channel_nr = packet[8];
rfcomm_channel_id = READ_BT_16(packet, 9);
printf("RFCOMM channel %u requested for %s\n", rfcomm_channel_nr, bd_addr_to_str(event_addr));
rfcomm_accept_connection_internal(rfcomm_channel_id);
break;
case RFCOMM_EVENT_OPEN_CHANNEL_COMPLETE:
// data: event(8), len(8), status (8), address (48), server channel(8), rfcomm_cid(16), max frame size(16)
if (packet[2]) {
printf("RFCOMM channel open failed, status %u\n", packet[2]);
} else {
rfcomm_channel_id = READ_BT_16(packet, 12);
mtu = READ_BT_16(packet, 14);
printf("RFCOMM channel open succeeded. New RFCOMM Channel ID %u, max frame size %u\n", rfcomm_channel_id, mtu);
}
break;
case RFCOMM_EVENT_CHANNEL_CLOSED:
printf("RFCOMM channel closed\n");
rfcomm_channel_id = 0;
break;
default:
break;
}
break;
case RFCOMM_DATA_PACKET:
printf("RCV: '");
for (i=0;i<size;i++){
putchar(packet[i]);
}
printf("'\n");
break;
default:
break;
}
}
// write requests
static int att_write_callback(uint16_t con_handle, uint16_t attribute_handle, uint16_t transaction_mode, uint16_t offset, uint8_t *buffer, uint16_t buffer_size){
printf("WRITE Callback, handle %04x, mode %u, offset %u, data: ", handle, transaction_mode, offset);
printf_hexdump(buffer, buffer_size);
uint16_t uuid16 = att_uuid_for_handle(handle);
switch (uuid16){
case 0x2902:
client_configuration = buffer[0];
client_configuration_handle = handle;
printf("Client Configuration set to %u for handle %04x\n", client_configuration, handle);
return 0; // ok
case 0x2a00:
memcpy(gap_device_name, buffer, buffer_size);
gap_device_name[buffer_size]=0;
printf("Setting device name to '%s'\n", gap_device_name);
return 0;
case 0x2a01:
gap_appearance = READ_BT_16(buffer, 0);
printf("Setting appearance to 0x%04x'\n", gap_appearance);
return 0;
case 0x2a02:
gap_privacy = buffer[0];
printf("Setting privacy to 0x%04x'\n", gap_privacy);
update_advertisements();
return 0;
case 0x2A03:
bt_flip_addr(gap_reconnection_address, buffer);
printf("Setting Reconnection Address to %s\n", bd_addr_to_str(gap_reconnection_address));
return 0;
// handle device name
// handle appearance
}
// check transaction mode
int attributes_index;
int writes_index;
switch (transaction_mode){
case ATT_TRANSACTION_MODE_NONE:
attributes_index = att_attribute_for_handle(handle);
if (attributes_index < 0){
attributes_index = att_attribute_for_handle(0);
if (attributes_index < 0) return 0; // ok, but we couldn't store it (our fault)
att_attributes[attributes_index].handle = handle;
// not written before
uint8_t * att_value;
uint16_t att_value_len;
if (att_default_value_long){
att_value = (uint8_t*) default_value_long;
att_value_len = strlen(default_value_long);
} else {
att_value = (uint8_t*) default_value_short;
att_value_len = strlen(default_value_short);
}
att_attributes[attributes_index].len = att_value_len;
}
if (buffer_size > att_attributes[attributes_index].len) return ATT_ERROR_INVALID_ATTRIBUTE_VALUE_LENGTH;
att_attributes[attributes_index].len = buffer_size;
memcpy(att_attributes[attributes_index].value, buffer, buffer_size);
break;
case ATT_TRANSACTION_MODE_ACTIVE:
writes_index = att_write_queue_for_handle(handle);
if (writes_index < 0) return ATT_ERROR_PREPARE_QUEUE_FULL;
if (offset > att_write_queues[writes_index].len) return ATT_ERROR_INVALID_OFFSET;
if (buffer_size + offset > ATT_VALUE_MAX_LEN) return ATT_ERROR_INVALID_ATTRIBUTE_VALUE_LENGTH;
att_write_queues[writes_index].len = buffer_size + offset;
memcpy(&(att_write_queues[writes_index].value[offset]), buffer, buffer_size);
break;
case ATT_TRANSACTION_MODE_EXECUTE:
for (writes_index=0 ; writes_index<ATT_NUM_WRITE_QUEUES ; writes_index++){
handle = att_write_queues[writes_index].handle;
if (handle == 0) continue;
attributes_index = att_attribute_for_handle(handle);
if (attributes_index < 0){
attributes_index = att_attribute_for_handle(0);
if (attributes_index < 0) continue;
att_attributes[attributes_index].handle = handle;
}
att_attributes[attributes_index].len = att_write_queues[writes_index].len;
memcpy(att_attributes[attributes_index].value, att_write_queues[writes_index].value, att_write_queues[writes_index].len);
}
att_write_queue_init();
break;
case ATT_TRANSACTION_MODE_CANCEL:
att_write_queue_init();
break;
}
return 0;
}
/*************** PANU client routines *********************/
static void packet_handler (void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size)
{
uint8_t event;
bd_addr_t event_addr;
bd_addr_t src_addr;
bd_addr_t dst_addr;
uint16_t uuid_source;
uint16_t uuid_dest;
uint16_t mtu;
uint16_t network_type;
uint8_t protocol_type;
uint8_t icmp_type;
int ihl;
int payload_offset;
switch (packet_type) {
case HCI_EVENT_PACKET:
event = packet[0];
switch (event) {
case BTSTACK_EVENT_STATE:
/* BT Stack activated, get started */
if (packet[2] == HCI_STATE_WORKING) {
printf("BNEP Test ready\n");
show_usage();
}
break;
case HCI_EVENT_COMMAND_COMPLETE:
if (COMMAND_COMPLETE_EVENT(packet, hci_read_bd_addr)){
bt_flip_addr(local_addr, &packet[6]);
printf("BD-ADDR: %s\n", bd_addr_to_str(local_addr));
break;
}
break;
case HCI_EVENT_USER_CONFIRMATION_REQUEST:
// inform about user confirmation request
printf("SSP User Confirmation Request with numeric value '%06u'\n", READ_BT_32(packet, 8));
printf("SSP User Confirmation Auto accept\n");
break;
case BNEP_EVENT_OPEN_CHANNEL_COMPLETE:
if (packet[2]) {
printf("BNEP channel open failed, status %02x\n", packet[2]);
} else {
// data: event(8), len(8), status (8), bnep source uuid (16), bnep destination uuid (16), remote_address (48)
uuid_source = READ_BT_16(packet, 3);
uuid_dest = READ_BT_16(packet, 5);
mtu = READ_BT_16(packet, 7);
bnep_cid = channel;
//bt_flip_addr(event_addr, &packet[9]);
memcpy(&event_addr, &packet[9], sizeof(bd_addr_t));
printf("BNEP connection open succeeded to %s source UUID 0x%04x dest UUID: 0x%04x, max frame size %u\n", bd_addr_to_str(event_addr), uuid_source, uuid_dest, mtu);
}
break;
case BNEP_EVENT_CHANNEL_TIMEOUT:
printf("BNEP channel timeout! Channel will be closed\n");
break;
case BNEP_EVENT_CHANNEL_CLOSED:
printf("BNEP channel closed\n");
break;
case BNEP_EVENT_READY_TO_SEND:
/* Check for parked network packets and send it out now */
if (network_buffer_len > 0) {
bnep_send(bnep_cid, network_buffer, network_buffer_len);
network_buffer_len = 0;
}
break;
default:
break;
}
break;
case BNEP_DATA_PACKET:
// show received packet on console
// TODO: fix BNEP to return BD ADDR in little endian, to use these lines
// bt_flip_addr(dst_addr, &packet[0]);
// bt_flip_addr(src_addr, &packet[6]);
// instead of these
memcpy(dst_addr, &packet[0], 6);
memcpy(src_addr, &packet[6], 6);
// END TOOD
network_type = READ_NET_16(packet, 12);
printf("BNEP packet received\n");
printf("Dst Addr: %s\n", bd_addr_to_str(dst_addr));
printf("Src Addr: %s\n", bd_addr_to_str(src_addr));
printf("Net Type: %04x\n", network_type);
// ignore the next 60 bytes
// hexdumpf(&packet[74], size - 74);
switch (network_type){
case NETWORK_TYPE_IPv4:
ihl = packet[14] & 0x0f;
payload_offset = 14 + (ihl << 2);
// protocol
protocol_type = packet[14 + 9]; // offset 9 into IPv4
switch (protocol_type){
case 0x01: // ICMP
icmp_type = packet[payload_offset];
hexdumpf(&packet[payload_offset], size - payload_offset);
printf("ICMP packet of type %x\n", icmp_type);
switch (icmp_type){
case ICMP_V4_TYPE_PING_REQUEST:
printf("IPv4 Ping Request received, sending pong\n");
send_ping_response_ipv4();
break;
break;
}
case 0x11: // UDP
printf("UDP IPv4 packet\n");
hexdumpf(&packet[payload_offset], size - payload_offset);
break;
default:
printf("Unknown IPv4 protocol type %x", protocol_type);
break;
}
break;
case NETWORK_TYPE_IPv6:
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);
break;
}
break;
default:
break;
}
}
static void app_packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
switch (packet_type) {
case HCI_EVENT_PACKET:
switch (packet[0]) {
case BTSTACK_EVENT_STATE:
// bt stack activated, get started
if (packet[2] == HCI_STATE_WORKING) {
printf("SM Init completed\n");
todos = SET_ADVERTISEMENT_PARAMS | SET_ADVERTISEMENT_DATA | SET_SCAN_RESPONSE_DATA | ENABLE_ADVERTISEMENTS;
update_advertisements();
gap_run();
}
break;
case HCI_EVENT_LE_META:
switch (packet[2]) {
case HCI_SUBEVENT_LE_CONNECTION_COMPLETE:
advertisements_enabled = 0;
handle = READ_BT_16(packet, 4);
printf("Connection handle 0x%04x\n", handle);
// request connection parameter update - test parameters
// l2cap_le_request_connection_parameter_update(READ_BT_16(packet, 4), 20, 1000, 100, 100);
break;
default:
break;
}
break;
case HCI_EVENT_DISCONNECTION_COMPLETE:
if (!advertisements_enabled == 0 && gap_discoverable){
todos = ENABLE_ADVERTISEMENTS;
}
att_attributes_init();
att_write_queue_init();
break;
case SM_PASSKEY_INPUT_NUMBER: {
// display number
sm_event_t * event = (sm_event_t *) packet;
memcpy(master_address, event->address, 6);
master_addr_type = event->addr_type;
printf("\nGAP Bonding %s (%u): Enter 6 digit passkey: '", bd_addr_to_str(master_address), master_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(master_address), master_addr_type, event->passkey);
break;
}
case SM_PASSKEY_DISPLAY_CANCEL:
printf("\nGAP Bonding %s (%u): Display cancel\n", bd_addr_to_str(master_address), master_addr_type);
break;
case SM_AUTHORIZATION_REQUEST: {
// auto-authorize connection if requested
sm_event_t * event = (sm_event_t *) packet;
sm_authorization_grant(event->addr_type, event->address);
break;
}
case ATT_HANDLE_VALUE_INDICATION_COMPLETE:
printf("ATT_HANDLE_VALUE_INDICATION_COMPLETE status %u\n", packet[2]);
break;
default:
break;
}
}
gap_run();
}
static void event_handler(uint8_t *packet, int size){
bd_addr_t addr;
uint8_t link_type;
hci_con_handle_t handle;
hci_connection_t * conn;
int i;
switch (packet[0]) {
case HCI_EVENT_COMMAND_COMPLETE:
// get num cmd packets
// log_info("HCI_EVENT_COMMAND_COMPLETE cmds old %u - new %u\n", hci_stack.num_cmd_packets, packet[2]);
hci_stack.num_cmd_packets = packet[2];
if (COMMAND_COMPLETE_EVENT(packet, hci_read_buffer_size)){
// from offset 5
// status
// "The HC_ACL_Data_Packet_Length return parameter will be used to determine the size of the L2CAP segments contained in ACL Data Packets"
hci_stack.acl_data_packet_length = READ_BT_16(packet, 6);
// ignore: SCO data packet len (8)
hci_stack.total_num_acl_packets = packet[9];
// ignore: total num SCO packets
if (hci_stack.state == HCI_STATE_INITIALIZING){
// determine usable ACL payload size
if (HCI_ACL_PAYLOAD_SIZE < hci_stack.acl_data_packet_length){
hci_stack.acl_data_packet_length = HCI_ACL_PAYLOAD_SIZE;
}
// determine usable ACL packet types
hci_stack.packet_types = hci_acl_packet_types_for_buffer_size(hci_stack.acl_data_packet_length);
log_error("hci_read_buffer_size: used size %u, count %u, packet types %04x\n",
hci_stack.acl_data_packet_length, hci_stack.total_num_acl_packets, hci_stack.packet_types);
}
}
if (COMMAND_COMPLETE_EVENT(packet, hci_write_scan_enable)){
hci_emit_discoverable_enabled(hci_stack.discoverable);
}
break;
case HCI_EVENT_COMMAND_STATUS:
// get num cmd packets
// log_info("HCI_EVENT_COMMAND_STATUS cmds - old %u - new %u\n", hci_stack.num_cmd_packets, packet[3]);
hci_stack.num_cmd_packets = packet[3];
break;
case HCI_EVENT_NUMBER_OF_COMPLETED_PACKETS:
for (i=0; i<packet[2];i++){
handle = READ_BT_16(packet, 3 + 2*i);
uint16_t num_packets = READ_BT_16(packet, 3 + packet[2]*2 + 2*i);
conn = connection_for_handle(handle);
if (!conn){
log_error("hci_number_completed_packet lists unused con handle %u\n", handle);
continue;
}
conn->num_acl_packets_sent -= num_packets;
// log_info("hci_number_completed_packet %u processed for handle %u, outstanding %u\n", num_packets, handle, conn->num_acl_packets_sent);
}
break;
case HCI_EVENT_CONNECTION_REQUEST:
bt_flip_addr(addr, &packet[2]);
// TODO: eval COD 8-10
link_type = packet[11];
log_info("Connection_incoming: %s, type %u\n", bd_addr_to_str(addr), link_type);
if (link_type == 1) { // ACL
conn = connection_for_address(addr);
if (!conn) {
conn = create_connection_for_addr(addr);
}
if (!conn) {
// CONNECTION REJECTED DUE TO LIMITED RESOURCES (0X0D)
hci_stack.decline_reason = 0x0d;
BD_ADDR_COPY(hci_stack.decline_addr, addr);
break;
}
conn->state = RECEIVED_CONNECTION_REQUEST;
hci_run();
} else {
// SYNCHRONOUS CONNECTION LIMIT TO A DEVICE EXCEEDED (0X0A)
hci_stack.decline_reason = 0x0a;
BD_ADDR_COPY(hci_stack.decline_addr, addr);
}
break;
case HCI_EVENT_CONNECTION_COMPLETE:
// Connection management
bt_flip_addr(addr, &packet[5]);
log_info("Connection_complete (status=%u) %s\n", packet[2], bd_addr_to_str(addr));
conn = connection_for_address(addr);
if (conn) {
if (!packet[2]){
conn->state = OPEN;
conn->con_handle = READ_BT_16(packet, 3);
#ifdef HAVE_TICK
// restart timer
run_loop_set_timer(&conn->timeout, HCI_CONNECTION_TIMEOUT_MS);
run_loop_add_timer(&conn->timeout);
#endif
log_info("New connection: handle %u, %s\n", conn->con_handle, bd_addr_to_str(conn->address));
hci_emit_nr_connections_changed();
} else {
// connection failed, remove entry
linked_list_remove(&hci_stack.connections, (linked_item_t *) conn);
btstack_memory_hci_connection_free( conn );
// if authentication error, also delete link key
if (packet[2] == 0x05) {
hci_drop_link_key_for_bd_addr(&addr);
}
}
}
break;
case HCI_EVENT_LINK_KEY_REQUEST:
log_info("HCI_EVENT_LINK_KEY_REQUEST\n");
hci_add_connection_flags_for_flipped_bd_addr(&packet[2], RECV_LINK_KEY_REQUEST);
if (!hci_stack.remote_device_db) break;
hci_add_connection_flags_for_flipped_bd_addr(&packet[2], HANDLE_LINK_KEY_REQUEST);
hci_run();
// request already answered
return;
case HCI_EVENT_LINK_KEY_NOTIFICATION:
hci_add_connection_flags_for_flipped_bd_addr(&packet[2], RECV_LINK_KEY_NOTIFICATION);
if (!hci_stack.remote_device_db) break;
bt_flip_addr(addr, &packet[2]);
hci_stack.remote_device_db->put_link_key(&addr, (link_key_t *) &packet[8]);
// still forward event to allow dismiss of pairing dialog
break;
case HCI_EVENT_PIN_CODE_REQUEST:
hci_add_connection_flags_for_flipped_bd_addr(&packet[2], RECV_PIN_CODE_REQUEST);
break;
#ifndef EMBEDDED
case HCI_EVENT_REMOTE_NAME_REQUEST_COMPLETE:
if (!hci_stack.remote_device_db) break;
if (packet[2]) break; // status not ok
bt_flip_addr(addr, &packet[3]);
// fix for invalid remote names - terminate on 0xff
for (i=0; i<248;i++){
if (packet[9+i] == 0xff){
packet[9+i] = 0;
break;
}
}
memset(&device_name, 0, sizeof(device_name_t));
strncpy((char*) device_name, (char*) &packet[9], 248);
hci_stack.remote_device_db->put_name(&addr, &device_name);
break;
case HCI_EVENT_INQUIRY_RESULT:
case HCI_EVENT_INQUIRY_RESULT_WITH_RSSI:
if (!hci_stack.remote_device_db) break;
// first send inq result packet
hci_stack.packet_handler(HCI_EVENT_PACKET, packet, size);
// then send cached remote names
for (i=0; i<packet[2];i++){
bt_flip_addr(addr, &packet[3+i*6]);
if (hci_stack.remote_device_db->get_name(&addr, &device_name)){
hci_emit_remote_name_cached(&addr, &device_name);
}
}
return;
#endif
case HCI_EVENT_DISCONNECTION_COMPLETE:
if (!packet[2]){
handle = READ_BT_16(packet, 3);
hci_connection_t * conn = connection_for_handle(handle);
if (conn) {
hci_shutdown_connection(conn);
}
}
break;
case HCI_EVENT_HARDWARE_ERROR:
if(hci_stack.control->hw_error){
(*hci_stack.control->hw_error)();
}
break;
#ifdef HAVE_BLE
case HCI_EVENT_LE_META:
switch (packet[2]) {
case HCI_SUBEVENT_LE_CONNECTION_COMPLETE:
// Connection management
bt_flip_addr(addr, &packet[8]);
log_info("LE Connection_complete (status=%u) %s\n", packet[3], bd_addr_to_str(addr));
// LE connections are auto-accepted, so just create a connection if there isn't one already
conn = connection_for_address(addr);
if (packet[3]){
if (conn){
// outgoing connection failed, remove entry
linked_list_remove(&hci_stack.connections, (linked_item_t *) conn);
btstack_memory_hci_connection_free( conn );
}
// if authentication error, also delete link key
if (packet[3] == 0x05) {
hci_drop_link_key_for_bd_addr(&addr);
}
break;
}
if (!conn){
conn = create_connection_for_addr(addr);
}
if (!conn){
// no memory
break;
}
conn->state = OPEN;
conn->con_handle = READ_BT_16(packet, 4);
// TODO: store - role, peer address type, conn_interval, conn_latency, supervision timeout, master clock
// restart timer
// run_loop_set_timer(&conn->timeout, HCI_CONNECTION_TIMEOUT_MS);
// run_loop_add_timer(&conn->timeout);
log_info("New connection: handle %u, %s\n", conn->con_handle, bd_addr_to_str(conn->address));
hci_emit_nr_connections_changed();
break;
default:
break;
}
break;
#endif
default:
break;
}
// handle BT initialization
if (hci_stack.state == HCI_STATE_INITIALIZING){
// handle H4 synchronization loss on restart
// if (hci_stack.substate == 1 && packet[0] == HCI_EVENT_HARDWARE_ERROR){
// hci_stack.substate = 0;
// }
// handle normal init sequence
if (hci_stack.substate % 2){
// odd: waiting for event
if (packet[0] == HCI_EVENT_COMMAND_COMPLETE){
hci_stack.substate++;
}
}
}
// help with BT sleep
if (hci_stack.state == HCI_STATE_FALLING_ASLEEP
&& hci_stack.substate == 1
&& COMMAND_COMPLETE_EVENT(packet, hci_write_scan_enable)){
hci_stack.substate++;
}
hci_stack.packet_handler(HCI_EVENT_PACKET, packet, size);
// execute main loop
hci_run();
}
int stdin_process(struct data_source *ds){
char buffer;
read(ds->fd, &buffer, 1);
// passkey input
if (ui_digits_for_passkey){
if (buffer < '0' || buffer > '9') return 0;
printf("%c", buffer);
fflush(stdout);
ui_passkey = ui_passkey * 10 + buffer - '0';
ui_digits_for_passkey--;
if (ui_digits_for_passkey == 0){
printf("\nSending Passkey '%06x'\n", ui_passkey);
sm_passkey_input(master_addr_type, master_address, ui_passkey);
}
return 0;
}
switch (buffer){
case 'a':
gap_advertisements = 0;
update_advertisements();
show_usage();
break;
case 'A':
gap_advertisements = 1;
update_advertisements();
show_usage();
break;
case 'b':
gap_bondable = 0;
sm_set_authentication_requirements(SM_AUTHREQ_NO_BONDING);
show_usage();
break;
case 'B':
gap_bondable = 1;
sm_set_authentication_requirements(SM_AUTHREQ_BONDING);
show_usage();
break;
case 'c':
gap_connectable = 0;
update_advertisements();
break;
case 'C':
gap_connectable = 1;
update_advertisements();
break;
case 'd':
gap_discoverable = 0;
update_advertisements();
break;
case 'D':
gap_discoverable = 1;
update_advertisements();
break;
case 'r':
gap_random = 0;
update_advertisements();
break;
case 'R':
gap_random = 1;
update_advertisements();
break;
case 'x':
gap_directed_connectable = 0;
update_advertisements();
break;
case 'X':
gap_directed_connectable = 1;
update_advertisements();
break;
case 'y':
gap_scannable = 0;
update_advertisements();
break;
case 'Y':
gap_scannable = 1;
update_advertisements();
break;
case '1':
case '2':
case '3':
case '4':
case '5':
case '6':
case '7':
case '8':
case '9':
case '0':
advertisement_index = buffer - '0';
update_advertisements();
break;
case '+':
advertisement_index = 10;
update_advertisements();
break;
case '-':
advertisement_index = 11;
update_advertisements();
break;
case '&':
advertisement_index = 12;
update_advertisements();
break;
case '=':
advertisement_index = 13;
update_advertisements();
break;
case '/':
advertisement_index = 14;
update_advertisements();
break;
case '#':
advertisement_index = 15;
update_advertisements();
break;
case 's':
printf("SM: sending security request\n");
sm_send_security_request();
break;
case 'e':
sm_io_capabilities = "IO_CAPABILITY_DISPLAY_ONLY";
sm_set_io_capabilities(IO_CAPABILITY_DISPLAY_ONLY);
show_usage();
break;
case 'f':
sm_io_capabilities = "IO_CAPABILITY_DISPLAY_YES_NO";
sm_set_io_capabilities(IO_CAPABILITY_DISPLAY_YES_NO);
show_usage();
break;
case 'g':
sm_io_capabilities = "IO_CAPABILITY_NO_INPUT_NO_OUTPUT";
sm_set_io_capabilities(IO_CAPABILITY_NO_INPUT_NO_OUTPUT);
show_usage();
break;
case 'h':
sm_io_capabilities = "IO_CAPABILITY_KEYBOARD_ONLY";
sm_set_io_capabilities(IO_CAPABILITY_KEYBOARD_ONLY);
show_usage();
break;
case 'i':
sm_io_capabilities = "IO_CAPABILITY_KEYBOARD_DISPLAY";
sm_set_io_capabilities(IO_CAPABILITY_KEYBOARD_DISPLAY);
show_usage();
break;
case 't':
printf("Terminating connection\n");
hci_send_cmd(&hci_disconnect, handle, 0x13);
break;
case 'z':
printf("Sending l2cap connection update parameter request\n");
l2cap_le_request_connection_parameter_update(handle, 50, 120, 0, 550);
break;
case 'l':
att_default_value_long = 0;
show_usage();
break;
case 'L':
att_default_value_long = 1;
show_usage();
break;
case 'p':
gap_privacy = 0;
show_usage();
break;
case 'P':
gap_privacy = 1;
show_usage();
break;
case 'o':
sm_have_oob_data = 0;
show_usage();
break;
case 'O':
sm_have_oob_data = 1;
show_usage();
break;
case 'k':
sm_min_key_size = 7;
sm_set_encryption_key_size_range(7, 16);
show_usage();
break;
case 'K':
sm_min_key_size = 16;
sm_set_encryption_key_size_range(16, 16);
show_usage();
break;
case 'm':
sm_mitm_protection = 0;
update_auth_req();
show_usage();
break;
case 'M':
sm_mitm_protection = 1;
update_auth_req();
show_usage();
break;
case 'j':
printf("Create L2CAP Connection to %s\n", bd_addr_to_str(tester_address));
hci_send_cmd(&hci_le_create_connection,
1000, // scan interval: 625 ms
1000, // scan interval: 625 ms
0, // don't use whitelist
0, // peer address type: public
tester_address, // remote bd addr
tester_address_type, // random or public
80, // conn interval min
80, // conn interval max (3200 * 0.625)
0, // conn latency
2000, // supervision timeout
0, // min ce length
1000 // max ce length
);
break;
default:
show_usage();
break;
}
return 0;
}
