diff --git a/example/gatt_battery_query.c b/example/gatt_battery_query.c index d38d93844..e063c57b8 100644 --- a/example/gatt_battery_query.c +++ b/example/gatt_battery_query.c @@ -68,6 +68,10 @@ typedef enum { TC_W4_BATTERY_DATA } gc_state_t; +static int blacklist_index = 0; +static bd_addr_t blacklist[20]; +static advertising_report_t report; + static void handle_gatt_client_event(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size); static bd_addr_t cmdline_addr = { }; @@ -90,6 +94,24 @@ static void printUUID(uint8_t * uuid128, uint16_t uuid16){ } } +static int blacklist_size(void){ + return sizeof(blacklist) / sizeof(bd_addr_t); +} + +static int blacklist_contains(bd_addr_t addr){ + int i; + for (i=0; ievent_type, e->address_type, bd_addr_to_str(e->address), e->rssi, e->length); @@ -116,36 +138,27 @@ static void dump_service(gatt_client_service_t * service){ printf("\n"); } -static void error_code(int status){ - switch(status){ - case ATT_ERROR_INSUFFICIENT_AUTHENTICATION: - printf("ATT_ERROR_INSUFFICIENT_AUTHENTICATION. TODO: security manager is not complete yet.\n"); - break; - default: - printf(" status 0x%02x\n", status); - break; - } -} static void handle_gatt_client_event(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ + int status; + uint8_t battery_level; switch(state){ case TC_W4_SERVICE_RESULT: switch(hci_event_packet_get_type(packet)){ case GATT_EVENT_SERVICE_QUERY_RESULT: gatt_event_service_query_result_get_service(packet, &battery_service); - printf("Battery service found:\n"); dump_service(&battery_service); break; case GATT_EVENT_QUERY_COMPLETE: - if (!packet[4]){ - printf("Battery service not found. Restart scan.\n"); - state = TC_W4_SCAN_RESULT; - gap_start_scan(); + if (packet[4] != 0){ + printf("SERVICE_QUERY_RESULT - Error status %x.\n", packet[4]); + add_to_blacklist(report.address); + gap_disconnect(connection_handle); break; } state = TC_W4_CHARACTERISTIC_RESULT; - printf("\nSearch for battery level characteristic in battery service. "); + printf("\nSearch for battery level characteristic.\n"); gatt_client_discover_characteristics_for_service_by_uuid16(handle_gatt_client_event, connection_handle, &battery_service, battery_level_characteristic_uuid); break; default: @@ -156,30 +169,54 @@ static void handle_gatt_client_event(uint8_t packet_type, uint16_t channel, uint case TC_W4_CHARACTERISTIC_RESULT: switch(hci_event_packet_get_type(packet)){ case GATT_EVENT_CHARACTERISTIC_QUERY_RESULT: - printf("Battery level characteristic found:\n"); gatt_event_characteristic_query_result_get_characteristic(packet, &config_characteristic); dump_characteristic(&config_characteristic); break; case GATT_EVENT_QUERY_COMPLETE: + if (packet[4] != 0){ + printf("CHARACTERISTIC_QUERY_RESULT - Error status %x.\n", packet[4]); + add_to_blacklist(report.address); + gap_disconnect(connection_handle); + break; + } state = TC_W4_BATTERY_DATA; - printf("\nConfigure battery level characteristic for notify."); - gatt_client_write_client_characteristic_configuration(handle_gatt_client_event, connection_handle, &config_characteristic, GATT_CLIENT_CHARACTERISTICS_CONFIGURATION_NOTIFICATION); + printf("\nConfigure battery level characteristic for notify.\n"); + status = gatt_client_write_client_characteristic_configuration(handle_gatt_client_event, connection_handle, &config_characteristic, GATT_CLIENT_CHARACTERISTICS_CONFIGURATION_NOTIFICATION); + if (status != 0){ + printf("\nNotification not supported. Query value of characteristic.\n"); + gatt_client_read_value_of_characteristic(handle_gatt_client_event, connection_handle, &config_characteristic); + } + break; default: break; } break; case TC_W4_BATTERY_DATA: - if (hci_event_packet_get_type(packet) == GATT_EVENT_QUERY_COMPLETE){ - if (packet[4] != 0){ - printf("\nNotification is not possible: "); - error_code(packet[4]); - } - break; + switch(hci_event_packet_get_type(packet)){ + case GATT_EVENT_NOTIFICATION: + printf("\nBattery Data:\n"); + dump_characteristic_value(&packet[8], little_endian_read_16(packet, 6)); + break; + case GATT_EVENT_CHARACTERISTIC_VALUE_QUERY_RESULT: + + if (gatt_event_characteristic_value_query_result_get_value_length(packet) < 1) break; + battery_level = gatt_event_characteristic_value_query_result_get_value(packet)[0]; + printf("Battry level %d \n", battery_level); + break; + + case GATT_EVENT_QUERY_COMPLETE: + if (packet[4] != 0){ + printf("CHARACTERISTIC_VALUE_QUERY_RESULT - Error status %x.\n", packet[4]); + break; + } + // Use timer if repeated request is needed and notification is not supperted + gap_disconnect(connection_handle); + break; + default: + printf("Unknown packet type %x\n", hci_event_packet_get_type(packet)); + break; } - if (hci_event_packet_get_type(packet) != GATT_EVENT_NOTIFICATION) break; - printf("\nBattery Data:\n"); - dump_characteristic_value(&packet[8], little_endian_read_16(packet, 6)); break; default: @@ -189,30 +226,30 @@ static void handle_gatt_client_event(uint8_t packet_type, uint16_t channel, uint } -static void fill_advertising_report_from_packet(advertising_report_t * report, uint8_t *packet){ - gap_event_advertising_report_get_address(packet, report->address); - report->event_type = gap_event_advertising_report_get_advertising_event_type(packet); - report->address_type = gap_event_advertising_report_get_address_type(packet); - report->rssi = gap_event_advertising_report_get_rssi(packet); - report->length = gap_event_advertising_report_get_data_length(packet); - report->data = gap_event_advertising_report_get_data(packet); +static void fill_advertising_report_from_packet(advertising_report_t * adv_report, uint8_t *packet){ + gap_event_advertising_report_get_address(packet, adv_report->address); + adv_report->event_type = gap_event_advertising_report_get_advertising_event_type(packet); + adv_report->address_type = gap_event_advertising_report_get_address_type(packet); + adv_report->rssi = gap_event_advertising_report_get_rssi(packet); + adv_report->length = gap_event_advertising_report_get_data_length(packet); + adv_report->data = gap_event_advertising_report_get_data(packet); } 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; - advertising_report_t report; + 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("Start connect to %s\n", bd_addr_to_str(cmdline_addr)); + printf("Connect to %s\n", bd_addr_to_str(cmdline_addr)); state = TC_W4_CONNECT; gap_connect(cmdline_addr, 0); break; } - printf("BTstack activated, start scanning!\n"); + printf("Start scanning!\n"); state = TC_W4_SCAN_RESULT; gap_set_scan_parameters(0,0x0030, 0x0030); gap_start_scan(); @@ -220,13 +257,17 @@ static void hci_event_handler(uint8_t packet_type, uint16_t channel, uint8_t *pa 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. Start connect to device with addr %s.\n", bd_addr_to_str(report.address)); + 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 @@ -240,8 +281,16 @@ static void hci_event_handler(uint8_t packet_type, uint16_t channel, uint8_t *pa gatt_client_discover_primary_services_by_uuid16(handle_gatt_client_event, connection_handle, battery_service_uuid); break; case HCI_EVENT_DISCONNECTION_COMPLETE: - printf("\nDISCONNECTED\n"); - exit(0); + + 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; @@ -249,8 +298,8 @@ static void hci_event_handler(uint8_t packet_type, uint16_t channel, uint8_t *pa } static void usage(const char *name){ - fprintf(stderr, "\nUsage: %s [-a|--address aa:bb:cc:dd:ee:ff]\n", name); - fprintf(stderr, "If no argument is provided, GATT browser will start scanning and connect to the first found device.\nTo connect to a specific device use argument [-a].\n\n"); + fprintf(stderr, "\nUsage: %s [-a|--address aa:bb:cc:dd:ee:ff]\n", name); + fprintf(stderr, "If no argument is provided, GATT browser will start scanning and connect to the first found device.\nTo connect to a specific device use argument [-a].\n\n"); } int btstack_main(int argc, const char * argv[]); @@ -260,15 +309,16 @@ int btstack_main(int argc, const char * argv[]){ cmdline_addr_found = 0; while (arg < argc) { - if(!strcmp(argv[arg], "-a") || !strcmp(argv[arg], "--address")){ - arg++; - cmdline_addr_found = sscanf_bd_addr(argv[arg], cmdline_addr); + if(!strcmp(argv[arg], "-a") || !strcmp(argv[arg], "--address")){ arg++; + cmdline_addr_found = sscanf_bd_addr(argv[arg], cmdline_addr); + arg++; + if (!cmdline_addr_found) exit(1); continue; } usage(argv[0]); return 0; - } + } hci_event_callback_registration.callback = &hci_event_handler; hci_add_event_handler(&hci_event_callback_registration); diff --git a/src/btstack_util.c b/src/btstack_util.c index 7529c638e..db675dacf 100644 --- a/src/btstack_util.c +++ b/src/btstack_util.c @@ -54,7 +54,7 @@ * @brief Compare two Bluetooth addresses * @param a * @param b - * @return true if equal + * @return 0 if equal */ int bd_addr_cmp(bd_addr_t a, bd_addr_t b){ return memcmp(a,b, BD_ADDR_LEN); @@ -261,7 +261,7 @@ int sscanf_bd_addr(const char * addr_string, bd_addr_t addr){ int single_byte = scan_hex_byte(addr_string); if (single_byte < 0) break; addr_string += 2; - addr[i] = single_byte; + buffer[i] = single_byte; // don't check seperator after last byte if (i == BD_ADDR_LEN - 1) { result = 1;