static void att_run_for_context(att_server_t * att_server){ switch (att_server->state){ case ATT_SERVER_REQUEST_RECEIVED: // wait until pairing is complete if (att_server->pairing_active) break; #ifdef ENABLE_LE_SIGNED_WRITE if (att_server->request_buffer[0] == ATT_SIGNED_WRITE_COMMAND){ log_info("ATT Signed Write!"); if (!sm_cmac_ready()) { log_info("ATT Signed Write, sm_cmac engine not ready. Abort"); att_server->state = ATT_SERVER_IDLE; return; } if (att_server->request_size < (3 + 12)) { log_info("ATT Signed Write, request to short. Abort."); att_server->state = ATT_SERVER_IDLE; return; } if (att_server->ir_lookup_active){ return; } if (att_server->ir_le_device_db_index < 0){ log_info("ATT Signed Write, CSRK not available"); att_server->state = ATT_SERVER_IDLE; return; } // check counter uint32_t counter_packet = little_endian_read_32(att_server->request_buffer, att_server->request_size-12); uint32_t counter_db = le_device_db_remote_counter_get(att_server->ir_le_device_db_index); log_info("ATT Signed Write, DB counter %"PRIu32", packet counter %"PRIu32, counter_db, counter_packet); if (counter_packet < counter_db){ log_info("ATT Signed Write, db reports higher counter, abort"); att_server->state = ATT_SERVER_IDLE; return; } // signature is { sequence counter, secure hash } sm_key_t csrk; le_device_db_remote_csrk_get(att_server->ir_le_device_db_index, csrk); att_server->state = ATT_SERVER_W4_SIGNED_WRITE_VALIDATION; log_info("Orig Signature: "); log_info_hexdump( &att_server->request_buffer[att_server->request_size-8], 8); uint16_t attribute_handle = little_endian_read_16(att_server->request_buffer, 1); sm_cmac_signed_write_start(csrk, att_server->request_buffer[0], attribute_handle, att_server->request_size - 15, &att_server->request_buffer[3], counter_packet, att_signed_write_handle_cmac_result); return; } #endif // move on att_server->state = ATT_SERVER_REQUEST_RECEIVED_AND_VALIDATED; att_dispatch_server_request_can_send_now_event(att_server->connection.con_handle); break; default: break; } }
static void att_signed_write_handle_cmac_result(uint8_t hash[8]){ att_server_t * att_server = att_server_for_state(ATT_SERVER_W4_SIGNED_WRITE_VALIDATION); if (!att_server) return; uint8_t hash_flipped[8]; reverse_64(hash, hash_flipped); if (memcmp(hash_flipped, &att_server->request_buffer[att_server->request_size-8], 8)){ log_info("ATT Signed Write, invalid signature"); att_server->state = ATT_SERVER_IDLE; return; } log_info("ATT Signed Write, valid signature"); // update sequence number uint32_t counter_packet = little_endian_read_32(att_server->request_buffer, att_server->request_size-12); le_device_db_remote_counter_set(att_server->ir_le_device_db_index, counter_packet+1); att_server->state = ATT_SERVER_REQUEST_RECEIVED_AND_VALIDATED; att_dispatch_server_request_can_send_now_event(att_server->connection.con_handle); }
void app_packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ uint16_t aHandle; bd_addr_t event_address; switch (packet_type) { case HCI_EVENT_PACKET: switch (packet[0]) { case SM_EVENT_PASSKEY_INPUT_NUMBER: // store peer address for input printf("\nGAP Bonding: Enter 6 digit passkey: '"); fflush(stdout); break; case SM_EVENT_PASSKEY_DISPLAY_NUMBER: printf("\nGAP Bonding: Display Passkey '%06u\n", little_endian_read_32(packet, 11)); break; case SM_EVENT_PASSKEY_DISPLAY_CANCEL: printf("\nGAP Bonding: Display cancel\n"); break; case SM_EVENT_JUST_WORKS_REQUEST: // auto-authorize connection if requested sm_just_works_confirm(little_endian_read_16(packet, 2)); printf("Just Works request confirmed\n"); break; case SM_EVENT_AUTHORIZATION_REQUEST: // auto-authorize connection if requested sm_authorization_grant(little_endian_read_16(packet, 2)); break; 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); }
/* 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){ 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; } }
/* LISTING_START(packetHandler): Packet Handler */ static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) { /* LISTING_PAUSE */ uint8_t event; bd_addr_t event_addr; uint8_t status; uint16_t l2cap_cid; /* LISTING_RESUME */ switch (packet_type) { case HCI_EVENT_PACKET: event = hci_event_packet_get_type(packet); switch (event) { /* @text When BTSTACK_EVENT_STATE with state HCI_STATE_WORKING * is received and the example is started in client mode, the remote SDP HID query is started. */ case BTSTACK_EVENT_STATE: if (btstack_event_state_get_state(packet) == HCI_STATE_WORKING){ printf("Start SDP HID query for remote HID Device.\n"); sdp_client_query_uuid16(&handle_sdp_client_query_result, remote_addr, BLUETOOTH_SERVICE_CLASS_HUMAN_INTERFACE_DEVICE_SERVICE); } break; /* LISTING_PAUSE */ 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 '%"PRIu32"'\n", little_endian_read_32(packet, 8)); printf("SSP User Confirmation Auto accept\n"); break; /* LISTING_RESUME */ case L2CAP_EVENT_CHANNEL_OPENED: status = packet[2]; if (status){ printf("L2CAP Connection failed: 0x%02x\n", status); break; } l2cap_cid = little_endian_read_16(packet, 13); if (!l2cap_cid) break; if (l2cap_cid == l2cap_hid_control_cid){ status = l2cap_create_channel(packet_handler, remote_addr, hid_interrupt_psm, 48, &l2cap_hid_interrupt_cid); if (status){ printf("Connecting to HID Control failed: 0x%02x\n", status); break; } } if (l2cap_cid == l2cap_hid_interrupt_cid){ printf("HID Connection established\n"); } break; default: break; } break; case L2CAP_DATA_PACKET: // for now, just dump incoming data if (channel == l2cap_hid_interrupt_cid){ hid_host_handle_interrupt_report(packet, size); } else if (channel == l2cap_hid_control_cid){ printf("HID Control: "); printf_hexdump(packet, size); } else { break; } default: break; } }
static void handle_hci_event(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ UNUSED(packet_type); UNUSED(channel); UNUSED(size); int connection_encrypted; // handle connect / disconncet events first switch (hci_event_packet_get_type(packet)) { case HCI_EVENT_LE_META: switch (packet[2]) { case HCI_SUBEVENT_LE_CONNECTION_COMPLETE: gc_handle = little_endian_read_16(packet, 4); log_info("Connection handle 0x%04x, request encryption", gc_handle); // we need to be paired to enable notifications tc_state = TC_W4_ENCRYPTED_CONNECTION; sm_send_security_request(gc_handle); break; default: break; } return; case HCI_EVENT_ENCRYPTION_CHANGE: if (gc_handle != little_endian_read_16(packet, 3)) return; connection_encrypted = packet[5]; log_info("Encryption state change: %u", connection_encrypted); if (!connection_encrypted) return; if (tc_state != TC_W4_ENCRYPTED_CONNECTION) return; // let's start log_info("\nANCS Client - CONNECTED, discover ANCS service"); tc_state = TC_W4_SERVICE_RESULT; gatt_client_discover_primary_services_by_uuid128(handle_hci_event, gc_handle, ancs_service_uuid); return; case HCI_EVENT_DISCONNECTION_COMPLETE: if (hci_event_disconnection_complete_get_connection_handle(packet) != gc_handle) break; if (tc_state == TC_SUBSCRIBED){ notify_client_simple(ANCS_SUBEVENT_CLIENT_DISCONNECTED); } tc_state = TC_IDLE; gc_handle = 0; return; default: break; } gatt_client_characteristic_t characteristic; uint8_t * value; uint16_t value_handle; uint16_t value_length; switch(tc_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, &ancs_service); ancs_service_found = 1; break; case GATT_EVENT_QUERY_COMPLETE: if (!ancs_service_found){ log_info("ANCS Service not found"); tc_state = TC_IDLE; break; } tc_state = TC_W4_CHARACTERISTIC_RESULT; log_info("ANCS Client - Discover characteristics for ANCS SERVICE "); gatt_client_discover_characteristics_for_service(handle_hci_event, gc_handle, &ancs_service); break; default: break; } break; case TC_W4_CHARACTERISTIC_RESULT: switch(hci_event_packet_get_type(packet)){ case GATT_EVENT_CHARACTERISTIC_QUERY_RESULT: gatt_event_characteristic_query_result_get_characteristic(packet, &characteristic); if (memcmp(characteristic.uuid128, ancs_notification_source_uuid, 16) == 0){ log_info("ANCS Notification Source found, attribute handle %u", characteristic.value_handle); ancs_notification_source_characteristic = characteristic; ancs_characteristcs++; break; } if (memcmp(characteristic.uuid128, ancs_control_point_uuid, 16) == 0){ log_info("ANCS Control Point found, attribute handle %u", characteristic.value_handle); ancs_control_point_characteristic = characteristic; ancs_characteristcs++; break; } if (memcmp(characteristic.uuid128, ancs_data_source_uuid, 16) == 0){ log_info("ANCS Data Source found, attribute handle %u", characteristic.value_handle); ancs_data_source_characteristic = characteristic; ancs_characteristcs++; break; } break; case GATT_EVENT_QUERY_COMPLETE: log_info("ANCS Characteristcs count %u", ancs_characteristcs); tc_state = TC_W4_NOTIFICATION_SOURCE_SUBSCRIBED; gatt_client_listen_for_characteristic_value_updates(&ancs_notification_source_notification, &handle_hci_event, gc_handle, &ancs_notification_source_characteristic); gatt_client_write_client_characteristic_configuration(handle_hci_event, gc_handle, &ancs_notification_source_characteristic, GATT_CLIENT_CHARACTERISTICS_CONFIGURATION_NOTIFICATION); break; default: break; } break; case TC_W4_NOTIFICATION_SOURCE_SUBSCRIBED: switch(hci_event_packet_get_type(packet)){ case GATT_EVENT_QUERY_COMPLETE: log_info("ANCS Notification Source subscribed"); tc_state = TC_W4_DATA_SOURCE_SUBSCRIBED; gatt_client_listen_for_characteristic_value_updates(&ancs_data_source_notification, &handle_hci_event, gc_handle, &ancs_data_source_characteristic); gatt_client_write_client_characteristic_configuration(handle_hci_event, gc_handle, &ancs_data_source_characteristic, GATT_CLIENT_CHARACTERISTICS_CONFIGURATION_NOTIFICATION); break; default: break; } break; case TC_W4_DATA_SOURCE_SUBSCRIBED: switch(hci_event_packet_get_type(packet)){ case GATT_EVENT_QUERY_COMPLETE: log_info("ANCS Data Source subscribed"); tc_state = TC_SUBSCRIBED; notify_client_simple(ANCS_SUBEVENT_CLIENT_CONNECTED); break; default: break; } break; case TC_SUBSCRIBED: if (hci_event_packet_get_type(packet) != GATT_EVENT_NOTIFICATION && hci_event_packet_get_type(packet) != GATT_EVENT_INDICATION ) break; value_handle = little_endian_read_16(packet, 4); value_length = little_endian_read_16(packet, 6); value = &packet[8]; log_info("ANCS Notification, value handle %u", value_handle); if (value_handle == ancs_data_source_characteristic.value_handle){ int i; for (i=0;i<value_length;i++) { ancs_chunk_parser_handle_byte(value[i]); } } else if (value_handle == ancs_notification_source_characteristic.value_handle){ ancs_notification_uid = little_endian_read_32(value, 4); log_info("Notification received: EventID %02x, EventFlags %02x, CategoryID %02x, CategoryCount %u, UID %04x", value[0], value[1], value[2], value[3], (int) ancs_notification_uid); static uint8_t get_notification_attributes[] = {0, 0,0,0,0, 0, 1,32,0, 2,32,0, 3,32,0, 4, 5}; little_endian_store_32(get_notification_attributes, 1, ancs_notification_uid); ancs_notification_uid = 0; ancs_chunk_parser_init(); gatt_client_write_value_of_characteristic(handle_hci_event, gc_handle, ancs_control_point_characteristic.value_handle, sizeof(get_notification_attributes), get_notification_attributes); } else { log_info("Unknown Source: "); log_info_hexdump(value , value_length); } break; default: break; } // app_run(); }
/*************** PANU client routines *********************/ static void packet_handler(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 (btstack_event_state_get_state(packet) == HCI_STATE_WORKING){ printf("BNEP Test ready\n"); show_usage(); } break; case HCI_EVENT_COMMAND_COMPLETE: if (HCI_EVENT_IS_COMMAND_COMPLETE(packet, hci_read_bd_addr)){ reverse_bd_addr(&packet[6], local_addr); 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", little_endian_read_32(packet, 8)); printf("SSP User Confirmation Auto accept\n"); break; case BNEP_EVENT_CHANNEL_OPENED: if (bnep_event_channel_opened_get_status(packet)) { printf("BNEP channel open failed, status %02x\n", bnep_event_channel_opened_get_status(packet)); } else { // data: event(8), len(8), status (8), bnep source uuid (16), bnep destination uuid (16), remote_address (48) bnep_cid = bnep_event_channel_opened_get_bnep_cid(packet); uuid_source = bnep_event_channel_opened_get_source_uuid(packet); uuid_dest = bnep_event_channel_opened_get_destination_uuid(packet); mtu = bnep_event_channel_opened_get_mtu(packet); //bt_flip_addr(event_addr, &packet[9]); memcpy(&event_addr, &packet[11], 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_CAN_SEND_NOW: /* 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 = big_endian_read_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; } }