int hci_send_cmd_packet(uint8_t *packet, int size){ bd_addr_t addr; hci_connection_t * conn; // house-keeping // create_connection? if (IS_COMMAND(packet, hci_create_connection)){ bt_flip_addr(addr, &packet[3]); log_info("Create_connection to %s\n", bd_addr_to_str(addr)); conn = connection_for_address(addr); if (conn) { // if connection exists if (conn->state == OPEN) { // and OPEN, emit connection complete command hci_emit_connection_complete(conn, 0); } // otherwise, just ignore as it is already in the open process return 0; // don't sent packet to controller } // create connection struct and register, state = SENT_CREATE_CONNECTION conn = create_connection_for_addr(addr); if (!conn){ // notify client that alloc failed hci_emit_connection_complete(conn, BTSTACK_MEMORY_ALLOC_FAILED); return 0; // don't sent packet to controller } conn->state = SENT_CREATE_CONNECTION; } if (IS_COMMAND(packet, hci_link_key_request_reply)){ hci_add_connection_flags_for_flipped_bd_addr(&packet[3], SENT_LINK_KEY_REPLY); } if (IS_COMMAND(packet, hci_link_key_request_negative_reply)){ hci_add_connection_flags_for_flipped_bd_addr(&packet[3], SENT_LINK_KEY_NEGATIVE_REQUEST); } if (IS_COMMAND(packet, hci_pin_code_request_reply)){ hci_add_connection_flags_for_flipped_bd_addr(&packet[3], SENT_PIN_CODE_REPLY); } if (IS_COMMAND(packet, hci_pin_code_request_negative_reply)){ hci_add_connection_flags_for_flipped_bd_addr(&packet[3], SENT_PIN_CODE_NEGATIVE_REPLY); } if (IS_COMMAND(packet, hci_delete_stored_link_key)){ if (hci_stack.remote_device_db){ bt_flip_addr(addr, &packet[3]); hci_stack.remote_device_db->delete_link_key(&addr); } } hci_stack.num_cmd_packets--; return hci_stack.hci_transport->send_packet(HCI_COMMAND_DATA_PACKET, packet, size); }
// @note: bd addr has to be set after sending init script (it might just get re-set) static int bt_control_bcm_set_bd_addr_cmd(void * config, bd_addr_t addr, uint8_t *hci_cmd_buffer){ hci_cmd_buffer[0] = 0x01; hci_cmd_buffer[1] = 0xfc; hci_cmd_buffer[2] = 0x06; bt_flip_addr(&hci_cmd_buffer[3], addr); return 0; }
int hci_connected(uint8_t *packet, uint16_t size) { bd_addr_t event_addr; // inform about new l2cap connection bt_flip_addr(event_addr, &packet[3]); uint16_t psm = READ_BT_16(packet, 11); bt_accessor(0)->connflags.source_cid = READ_BT_16(packet, 13); bt_accessor(0)->connflags.con_handle = READ_BT_16(packet, 9); if (packet[2] == 0) { printf("Channel successfully opened: "); print_bd_addr(event_addr); printf(", handle 0x%02x, psm 0x%02x, source cid 0x%02x, dest cid 0x%02x\n", bt_accessor(0)->connflags.con_handle, psm, bt_accessor(0)->connflags.source_cid, READ_BT_16(packet, 15)); // send SABM command on dlci 0 printf("Sending SABM #0\n"); _bt_rfcomm_send_sabm(bt_accessor(0)->connflags.source_cid, 1, 0); } else { printf("L2CAP connection to device "); print_bd_addr(event_addr); printf(" failed. status code %u\n", packet[2]); return (1); } return (0); }
int hci_inquiry_result(uint8_t *packet, uint16_t size) { int numResponses; bd_addr_t addr; t_device res; int i; numResponses = packet[2]; for (i=0; i< numResponses && btscan_count_node(bt_accessor(0)->root) < MAX_DEVICES;i++) { bt_flip_addr(addr, &packet[3+i*6]); if (btscan_find_node(bt_accessor(0)->root, addr)) continue; memset(&res, 0, sizeof(res)); memcpy(res.address, addr, 6); res.pageScanRepetitionMode = packet [3 + numResponses*(6) + i*1]; res.classOfDevice = READ_BT_24(packet, 3 + numResponses*(packet[0] == HCI_EVENT_INQUIRY_RESULT_WITH_RSSI ? (6+1+1) : (6+1+1+1))+ i*3); res.clockOffset = READ_BT_16(packet, 3 + numResponses*(packet[0] == HCI_EVENT_INQUIRY_RESULT_WITH_RSSI ? (6+1+1+3) : (6+1+1+1+3)) + i*2) & 0x7fff; res.rssi = (packet[0] == HCI_EVENT_INQUIRY_RESULT_WITH_RSSI ? packet [3 + numResponses*(6+1+1+3+2) + i*1] : 0); res.state = 1; printf("Device found: "); print_bd_addr(addr); printf(" with COD: 0x%06x, pageScan %u, clock offset 0x%04x\n", res.classOfDevice, res.pageScanRepetitionMode, res.clockOffset); bt_accessor(0)->root = btscan_add_node(bt_accessor(0)->root, &res); } return (0); }
static void handle_hci_event(void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ if (packet_type != HCI_EVENT_PACKET) return; bd_addr_t address; uint8_t event = packet[0]; switch (event) { case BTSTACK_EVENT_STATE: if (packet[2] != HCI_STATE_WORKING) break; le_central_set_scan_parameters(0,0x0030, 0x0030); le_central_start_scan(); break; case GAP_LE_ADVERTISING_REPORT:{ advertisement_received = 1; memcpy(advertisement_packet, packet, size); bt_flip_addr(address, &packet[4]); le_central_connect(address, (bd_addr_type_t)packet[3]); break; } case HCI_EVENT_LE_META: // wait for connection complete if (packet[2] != HCI_SUBEVENT_LE_CONNECTION_COMPLETE) break; connected = 1; break; case HCI_EVENT_DISCONNECTION_COMPLETE: exit(0); break; default: break; } }
static void handle_hci_event(uint8_t packet_type, uint8_t *packet, uint16_t size){ if (packet_type != HCI_EVENT_PACKET) return; switch (packet[0]) { case BTSTACK_EVENT_STATE: // BTstack activated, get started if (packet[2] == HCI_STATE_WORKING) { printf("BTstack activated, start scaning!\n"); le_central_set_scan_parameters(0,0x0030, 0x0030); le_central_start_scan(); } break; case GAP_LE_ADVERTISING_REPORT:{ int pos = 2; uint8_t event_type = packet[pos++]; uint8_t address_type = packet[pos++]; bd_addr_t address; bt_flip_addr(address, &packet[pos]); pos += 6; uint8_t rssi = packet[pos++]; uint8_t length = packet[pos++]; uint8_t * data = &packet[pos]; printf("Advertisement event: evt-type %u, addr-type %u, addr %s, rssi %u, data[%u] ", event_type, address_type, bd_addr_to_str(address), rssi, length); printf_hexdump(data, length); dump_advertisement_data(data, length); break; } default: break; } }
/** * add authentication flags and reset timer */ static void hci_add_connection_flags_for_flipped_bd_addr(uint8_t *bd_addr, hci_authentication_flags_t flags){ bd_addr_t addr; bt_flip_addr(addr, *(bd_addr_t *) bd_addr); hci_connection_t * conn = connection_for_address(addr); if (conn) { conn->authentication_flags |= flags; hci_connection_timestamp(conn); } }
int hci_link_key_request(uint8_t *packet, uint16_t size) { bd_addr_t event_addr; // link key request bt_flip_addr(event_addr, &packet[2]); bt_send_cmd(&hci_link_key_request_negative_reply, &event_addr); return (0); }
void hci_emit_remote_name_cached(bd_addr_t *addr, device_name_t *name){ uint8_t event[2+1+6+248]; event[0] = BTSTACK_EVENT_REMOTE_NAME_CACHED; event[1] = sizeof(event) - 2; event[2] = 0; // just to be compatible with HCI_EVENT_REMOTE_NAME_REQUEST_COMPLETE bt_flip_addr(&event[3], *addr); memcpy(&event[9], name, 248); hci_dump_packet(HCI_EVENT_PACKET, 0, event, sizeof(event)); hci_stack.packet_handler(HCI_EVENT_PACKET, event, sizeof(event)); }
int hci_pin_code_request(uint8_t *packet, uint16_t size) { bd_addr_t event_addr; // inform about pin code request bt_flip_addr(event_addr, &packet[2]); bt_send_cmd(&hci_pin_code_request_reply, &event_addr, 4, bt_accessor(0)->pin); printf("Please enter PIN %s on remote device\n", bt_accessor(0)->pin); return (0); }
void l2cap_emit_connection_request(l2cap_channel_t *channel) { uint8_t event[16]; event[0] = L2CAP_EVENT_INCOMING_CONNECTION; event[1] = sizeof(event) - 2; bt_flip_addr(&event[2], channel->address); bt_store_16(event, 8, channel->handle); bt_store_16(event, 10, channel->psm); bt_store_16(event, 12, channel->local_cid); bt_store_16(event, 14, channel->remote_cid); hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); l2cap_dispatch(channel, HCI_EVENT_PACKET, event, sizeof(event)); }
void hci_emit_connection_complete(hci_connection_t *conn, uint8_t status){ uint8_t event[13]; event[0] = HCI_EVENT_CONNECTION_COMPLETE; event[1] = sizeof(event) - 2; event[2] = status; bt_store_16(event, 3, conn->con_handle); bt_flip_addr(&event[5], conn->address); event[11] = 1; // ACL connection event[12] = 0; // encryption disabled hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); hci_stack.packet_handler(HCI_EVENT_PACKET, event, sizeof(event)); }
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){ hfp_service_level_connection_state++; if (rfcomm_can_send_packet_now(rfcomm_cid)) send_packet(); return; } if (packet_type != HCI_EVENT_PACKET) return; uint8_t event = packet[0]; bd_addr_t event_addr; switch (event) { case BTSTACK_EVENT_STATE: // bt stack activated, get started if (packet[2] == HCI_STATE_WORKING){ printf("Start SDP RFCOMM Query for UUID 0x%02x\n", SDP_Handsfree); // sdp_query_rfcomm_channel_and_name_for_uuid(remote, SDP_Handsfree); } 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_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 (packet[2]) { printf("RFCOMM channel open failed, status %u\n", packet[2]); } else { // data: event(8), len(8), status (8), address (48), handle (16), server channel(8), rfcomm_cid(16), max frame size(16) rfcomm_cid = 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_cid, mtu); break; } break; case DAEMON_EVENT_HCI_PACKET_SENT: case RFCOMM_EVENT_CREDITS: if (!rfcomm_cid) break; if (rfcomm_can_send_packet_now(rfcomm_cid)) send_packet(); break; default: break; } }
void l2cap_emit_channel_opened(l2cap_channel_t *channel, uint8_t status) { uint8_t event[21]; event[0] = L2CAP_EVENT_CHANNEL_OPENED; event[1] = sizeof(event) - 2; event[2] = status; bt_flip_addr(&event[3], channel->address); bt_store_16(event, 9, channel->handle); bt_store_16(event, 11, channel->psm); bt_store_16(event, 13, channel->local_cid); bt_store_16(event, 15, channel->remote_cid); bt_store_16(event, 17, channel->local_mtu); bt_store_16(event, 19, channel->remote_mtu); hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); l2cap_dispatch(channel, HCI_EVENT_PACKET, event, sizeof(event)); }
int hci_name_request(uint8_t *packet, uint16_t size) { bd_addr_t addr; t_device *t; bt_flip_addr(addr, &packet[3]); if ((t = btscan_find_node(bt_accessor(0)->root, addr))) if (packet[2] == 0 && t->name[0] == 0) { printf("Name: '%s'\n", &packet[9]); memcpy(t->name, &packet[9], (strlen((char *)(&packet[9])) < 254 ? strlen((char *)(&packet[9])) : 253)); t->state = 3; } return (0); }
// enable LE, setup ADV data static void app_packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ if (packet_type != HCI_EVENT_PACKET) return; bd_addr_t addr; uint8_t adv_data[] = { 02, 01, 05, 03, 02, 0xf0, 0xff }; switch (packet[0]) { case BTSTACK_EVENT_STATE: // bt stack activated, get started - set local name if (packet[2] == HCI_STATE_WORKING) { printf("Working!\n"); hci_send_cmd(&hci_le_set_advertising_data, sizeof(adv_data), adv_data); } break; case BTSTACK_EVENT_NR_CONNECTIONS_CHANGED: if (packet[2]) { printf("CONNECTED"); } else { printf("DISCONNECTED"); } break; case HCI_EVENT_DISCONNECTION_COMPLETE: // restart advertising hci_send_cmd(&hci_le_set_advertise_enable, 1); break; case HCI_EVENT_COMMAND_COMPLETE: if (COMMAND_COMPLETE_EVENT(packet, hci_read_bd_addr)){ bt_flip_addr(addr, &packet[6]); printf("BD ADDR: %s\n", bd_addr_to_str(addr)); break; } if (COMMAND_COMPLETE_EVENT(packet, hci_le_set_advertising_data)){ hci_send_cmd(&hci_le_set_scan_response_data, 10, adv_data); break; } if (COMMAND_COMPLETE_EVENT(packet, hci_le_set_scan_response_data)){ hci_send_cmd(&hci_le_set_advertise_enable, 1); break; } default: break; } }
static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ bd_addr_t event_addr; uint16_t psm; if (packet_type != HCI_EVENT_PACKET) return; switch (packet[0]) { case BTSTACK_EVENT_STATE: // bt stack activated, get started if (packet[2] == HCI_STATE_WORKING){ printf("BTstack L2CAP Test Ready\n"); show_usage(); } break; case L2CAP_EVENT_CHANNEL_OPENED: // inform about new l2cap connection bt_flip_addr(event_addr, &packet[3]); psm = READ_BT_16(packet, 11); local_cid = READ_BT_16(packet, 13); handle = READ_BT_16(packet, 9); if (packet[2] == 0) { printf("Channel successfully opened: %s, handle 0x%02x, psm 0x%02x, local cid 0x%02x, remote cid 0x%02x\n", bd_addr_to_str(event_addr), handle, psm, local_cid, READ_BT_16(packet, 15)); } else { printf("L2CAP connection to device %s failed. status code %u\n", bd_addr_to_str(event_addr), packet[2]); } break; case L2CAP_EVENT_INCOMING_CONNECTION: { uint16_t l2cap_cid = READ_BT_16(packet, 12); printf("L2CAP Accepting incoming connection request\n"); l2cap_accept_connection_internal(l2cap_cid); 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; } }
// ATT Client Read Callback for Dynamic Data // - if buffer == NULL, don't copy data, just return size of value // - if buffer != NULL, copy data and return number bytes copied // @param offset defines start of attribute value static uint16_t att_read_callback(uint16_t con_handle, uint16_t attribute_handle, uint16_t offset, uint8_t * buffer, uint16_t buffer_size){ printf("READ Callback, handle %04x, offset %u, buffer size %u\n", handle, offset, buffer_size); uint16_t att_value_len; uint16_t uuid16 = att_uuid_for_handle(handle); switch (uuid16){ case 0x2902: if (buffer) { buffer[0] = client_configuration; } return 1; case 0x2a00: att_value_len = strlen(gap_device_name); if (buffer) { memcpy(buffer, gap_device_name, att_value_len); } return att_value_len; case 0x2a01: if (buffer){ bt_store_16(buffer, 0, gap_appearance); } return 2; case 0x2a02: if (buffer){ buffer[0] = gap_privacy; } return 1; case 0x2A03: if (buffer) { bt_flip_addr(buffer, gap_reconnection_address); } return 6; // handle device name // handle appearance default: break; } // find attribute int index = att_attribute_for_handle(handle); uint8_t * att_value; if (index < 0){ // not written before 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); } } else { att_value = att_attributes[index].value; att_value_len = att_attributes[index].len; } printf("Attribute len %u, data: ", att_value_len); printf_hexdump(att_value, att_value_len); // assert offset <= att_value_len if (offset > att_value_len) { return 0; } uint16_t bytes_to_copy = att_value_len - offset; if (!buffer) return bytes_to_copy; if (bytes_to_copy > buffer_size){ bytes_to_copy = buffer_size; } memcpy(buffer, &att_value[offset], bytes_to_copy); return bytes_to_copy; }
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(); }
// 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; }
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; } }
// 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 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(); }
void l2cap_event_handler( uint8_t *packet, uint16_t size ){ bd_addr_t address; hci_con_handle_t handle; l2cap_channel_t * channel; linked_item_t *it; int hci_con_used; switch(packet[0]){ // handle connection complete events case HCI_EVENT_CONNECTION_COMPLETE: bt_flip_addr(address, &packet[5]); if (packet[2] == 0){ handle = READ_BT_16(packet, 3); l2cap_handle_connection_success_for_addr(address, handle); } else { l2cap_handle_connection_failed_for_addr(address, packet[2]); } break; // handle successful create connection cancel command case HCI_EVENT_COMMAND_COMPLETE: if ( COMMAND_COMPLETE_EVENT(packet, hci_create_connection_cancel) ) { if (packet[5] == 0){ bt_flip_addr(address, &packet[6]); // CONNECTION TERMINATED BY LOCAL HOST (0X16) l2cap_handle_connection_failed_for_addr(address, 0x16); } } l2cap_run(); // try sending signaling packets first break; case HCI_EVENT_COMMAND_STATUS: l2cap_run(); // try sending signaling packets first break; // handle disconnection complete events case HCI_EVENT_DISCONNECTION_COMPLETE: // send l2cap disconnect events for all channels on this handle handle = READ_BT_16(packet, 3); it = (linked_item_t *) &l2cap_channels; while (it->next){ l2cap_channel_t * channel = (l2cap_channel_t *) it->next; if ( channel->handle == handle ){ // update prev item before free'ing next element - don't call l2cap_finalize_channel_close it->next = it->next->next; l2cap_emit_channel_closed(channel); btstack_memory_l2cap_channel_free(channel); } else { it = it->next; } } break; case HCI_EVENT_NUMBER_OF_COMPLETED_PACKETS: l2cap_run(); // try sending signaling packets first l2cap_hand_out_credits(); break; // HCI Connection Timeouts case L2CAP_EVENT_TIMEOUT_CHECK: handle = READ_BT_16(packet, 2); if (hci_authentication_active_for_handle(handle)) break; hci_con_used = 0; for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ channel = (l2cap_channel_t *) it; if (channel->handle == handle) { hci_con_used = 1; } } if (hci_con_used) break; if (!hci_can_send_packet_now(HCI_COMMAND_DATA_PACKET)) break; hci_send_cmd(&hci_disconnect, handle, 0x13); // remote closed connection break; case DAEMON_EVENT_HCI_PACKET_SENT: for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ channel = (l2cap_channel_t *) it; if (channel->packet_handler) { (* (channel->packet_handler))(HCI_EVENT_PACKET, channel->local_cid, packet, size); } } if (attribute_protocol_packet_handler) { (*attribute_protocol_packet_handler)(HCI_EVENT_PACKET, 0, packet, size); } if (security_protocol_packet_handler) { (*security_protocol_packet_handler)(HCI_EVENT_PACKET, 0, packet, size); } break; default: break; } // pass on (*packet_handler)(NULL, HCI_EVENT_PACKET, 0, packet, size); }
static void att_event_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 DAEMON_EVENT_HCI_PACKET_SENT: att_run(); break; case HCI_EVENT_LE_META: switch (packet[2]) { case HCI_SUBEVENT_LE_CONNECTION_COMPLETE: // store connection info att_client_addr_type = packet[7]; bt_flip_addr(att_client_address, &packet[8]); // reset connection properties att_connection.con_handle = READ_BT_16(packet, 4); att_connection.mtu = ATT_DEFAULT_MTU; att_connection.max_mtu = l2cap_max_le_mtu(); att_connection.encryption_key_size = 0; att_connection.authenticated = 0; att_connection.authorized = 0; break; default: break; } break; case HCI_EVENT_ENCRYPTION_CHANGE: // check handle if (att_connection.con_handle != READ_BT_16(packet, 3)) break; att_connection.encryption_key_size = sm_encryption_key_size(att_client_addr_type, att_client_address); att_connection.authenticated = sm_authenticated(att_client_addr_type, att_client_address); break; case HCI_EVENT_DISCONNECTION_COMPLETE: att_clear_transaction_queue(&att_connection); att_connection.con_handle = 0; att_handle_value_indication_handle = 0; // reset error state // restart advertising if we have been connected before // -> avoid sending advertise enable a second time before command complete was received att_server_state = ATT_SERVER_IDLE; break; case SM_IDENTITY_RESOLVING_STARTED: log_info("SM_IDENTITY_RESOLVING_STARTED"); att_ir_lookup_active = 1; break; case SM_IDENTITY_RESOLVING_SUCCEEDED: att_ir_lookup_active = 0; att_ir_le_device_db_index = ((sm_event_t*) packet)->le_device_db_index; log_info("SM_IDENTITY_RESOLVING_SUCCEEDED id %u", att_ir_le_device_db_index); att_run(); break; case SM_IDENTITY_RESOLVING_FAILED: log_info("SM_IDENTITY_RESOLVING_FAILED"); att_ir_lookup_active = 0; att_ir_le_device_db_index = -1; att_run(); break; case SM_AUTHORIZATION_RESULT: { sm_event_t * event = (sm_event_t *) packet; if (event->addr_type != att_client_addr_type) break; if (memcmp(event->address, att_client_address, 6) != 0) break; att_connection.authorized = event->authorization_result; att_run(); break; } default: break; } } if (att_client_packet_handler){ att_client_packet_handler(packet_type, channel, packet, size); } }
void process_hci_event(uint8_t* packet) { uint16_t local_cid; //char pin[20]; //int i; //CFDataRef cfData; switch (packet[0]) { case BTSTACK_EVENT_POWERON_FAILED: printf("HCI Init failed - make sure you have turned off Bluetooth in the System Settings\n"); changeState(iSAP_state_error); break; case BTSTACK_EVENT_STATE: if (packet[2] == HCI_STATE_WORKING) { bt_send_cmd(&hci_write_local_name, device_name); changeState(iSAP_state_ready); } break; case L2CAP_EVENT_INCOMING_CONNECTION: local_cid = READ_BT_16(packet, 12); // accept bt_send_cmd(&l2cap_accept_connection, local_cid); changeState(iSAP_state_starting_l2cap); break; case HCI_EVENT_LINK_KEY_NOTIFICATION: //implement link key saving break; case HCI_EVENT_LINK_KEY_REQUEST: // link key request bt_flip_addr(event_addr, &packet[2]); //unsigned char lk[16] = {0x88, 0xf2, 0x5a, 0x92, 0x5a, 0x9e, 0x00, 0x4b, 0x05, 0xf9, 0xf7, 0x02, 0xd9, 0x1a, 0x43, 0xbb}; //bt_send_cmd(&hci_link_key_request_reply, &event_addr, lk); bt_send_cmd(&hci_link_key_request_negative_reply, &event_addr); break; case HCI_EVENT_PIN_CODE_REQUEST: // inform about pin code request sendMessageToGUI(CFMSG_setPIN, 0, NULL, NULL, CFSTR(_messagePortName_client)); //while (!allowPin) { // sleep(1); //} //printf("Please enter PIN here: "); //fgets(pin, 20, stdin); //i = strlen(pin)-1; //if( pin[i] == '\n') { // pin[i] = '\0'; //} //printf("PIN = '%s'\n", pin); bt_flip_addr(event_addr, &packet[2]); //bt_send_cmd(&hci_pin_code_request_reply, &event_addr, strlen(pin), pin); break; case L2CAP_EVENT_CHANNEL_OPENED: // inform about new l2cap connection source_cid = READ_BT_16(packet, 13); con_handle = READ_BT_16(packet, 9); break; case HCI_EVENT_CONNECTION_REQUEST: { // accept incoming connections bt_flip_addr(event_addr, &packet[2]); bt_send_cmd(&hci_accept_connection_request, &event_addr, 1); changeState(iSAP_state_starting_hci); break; } case HCI_EVENT_CONNECTION_COMPLETE: // handle connections break; case HCI_EVENT_DISCONNECTION_COMPLETE: printf("Basebank connection closed\n"); changeState(iSAP_state_ready); break; case HCI_EVENT_COMMAND_COMPLETE: // use pairing yes/no if ( COMMAND_COMPLETE_EVENT(packet, hci_write_local_name) ) { bt_send_cmd(&hci_write_authentication_enable, 0); } else if ( COMMAND_COMPLETE_EVENT(packet, hci_write_authentication_enable) ) { bt_send_cmd(&hci_write_class_of_device, 0x5a020C); } break; default: // other event if (DEBUG) { printf("Unknown packet %02x\n", packet[0]); } break; } }
void comm_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ bd_addr_t event_addr; static uint8_t msc_resp_send = 0; static uint8_t msc_resp_received = 0; static uint8_t credits_used = 0; static uint8_t credits_free = 0; uint8_t packet_processed = 0; switch (packet_type) { case L2CAP_DATA_PACKET: // rfcomm: data[8] = addr // rfcomm: data[9] = command // received 1. message BT_RF_COMM_UA if (size == 4 && packet[1] == BT_RFCOMM_UA && packet[0] == 0x03){ packet_processed++; printf("Received RFCOMM unnumbered acknowledgement for channel 0 - multiplexer working\n"); printf("Sending UIH Parameter Negotiation Command\n"); _bt_rfcomm_send_uih_pn_command(source_cid, 1, RFCOMM_CHANNEL_ID, 100); } // received UIH Parameter Negotiation Response if (size == 14 && packet[1] == BT_RFCOMM_UIH && packet[3] == BT_RFCOMM_PN_RSP){ packet_processed++; printf("UIH Parameter Negotiation Response\n"); printf("Sending SABM #%u\n", RFCOMM_CHANNEL_ID); _bt_rfcomm_send_sabm(source_cid, 1, RFCOMM_CHANNEL_ID); } // received 2. message BT_RF_COMM_UA if (size == 4 && packet[1] == BT_RFCOMM_UA && packet[0] == ((RFCOMM_CHANNEL_ID << 3) | 3) ){ packet_processed++; printf("Received RFCOMM unnumbered acknowledgement for channel %u - channel opened\n", RFCOMM_CHANNEL_ID); printf("Sending MSC 'I'm ready'\n"); _bt_rfcomm_send_uih_msc_cmd(source_cid, 1, RFCOMM_CHANNEL_ID, 0x8d); // ea=1,fc=0,rtc=1,rtr=1,ic=0,dv=1 } // received BT_RFCOMM_MSC_CMD if (size == 8 && packet[1] == BT_RFCOMM_UIH && packet[3] == BT_RFCOMM_MSC_CMD){ packet_processed++; printf("Received BT_RFCOMM_MSC_CMD\n"); printf("Responding to 'I'm ready'\n"); // fine with this uint8_t address = packet[0] | 2; // set response packet[3] = BT_RFCOMM_MSC_RSP; // " " rfcomm_send_packet(source_cid, address, BT_RFCOMM_UIH, 0x30, (uint8_t*)&packet[3], 4); msc_resp_send = 1; } // received BT_RFCOMM_MSC_RSP if (size == 8 && packet[1] == BT_RFCOMM_UIH && packet[3] == BT_RFCOMM_MSC_RSP){ packet_processed++; msc_resp_received = 1; } if (packet[1] == BT_RFCOMM_UIH && packet[0] == ((RFCOMM_CHANNEL_ID<<3)|1)){ packet_processed++; credits_used++; if(DEBUG){ printf("RX: address %02x, control %02x: ", packet[0], packet[1]); hexdump( (uint8_t*) &packet[3], size-4); } int written = 0; int length = size-4; int start_of_data = 3; //write data to fifo while (length) { if ((written = write(fifo_fd, &packet[start_of_data], length)) == -1) { printf("Error writing to FIFO\n"); } else { length -= written; } } } if (packet[1] == BT_RFCOMM_UIH_PF && packet[0] == ((RFCOMM_CHANNEL_ID<<3)|1)){ packet_processed++; credits_used++; if (!credits_free) { printf("Got %u credits, can send!\n", packet[2]); } credits_free = packet[2]; if(DEBUG){ printf("RX: address %02x, control %02x: ", packet[0], packet[1]); hexdump( (uint8_t *) &packet[4], size-5); } int written = 0; int length = size-5; int start_of_data = 4; //write data to fifo while (length) { if ((written = write(fifo_fd, &packet[start_of_data], length)) == -1) { printf("Error writing to FIFO\n"); } else { length -= written; } } } uint8_t send_credits_packet = 0; if (credits_used >= NR_CREDITS ) { send_credits_packet = 1; credits_used -= NR_CREDITS; } if (msc_resp_send && msc_resp_received) { send_credits_packet = 1; msc_resp_send = msc_resp_received = 0; printf("RFCOMM up and running!\n"); } if (send_credits_packet) { // send 0x30 credits uint8_t initiator = 1; uint8_t address = (1 << 0) | (initiator << 1) | (initiator << 1) | (RFCOMM_CHANNEL_ID << 3); rfcomm_send_packet(source_cid, address, BT_RFCOMM_UIH_PF, NR_CREDITS, NULL, 0); } if (!packet_processed){ // just dump data for now printf("??: address %02x, control %02x: ", packet[0], packet[1]); hexdump( 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 - use authentication yes/no if (packet[2] == HCI_STATE_WORKING) { bt_send_cmd(&hci_write_authentication_enable, 1); } break; case HCI_EVENT_LINK_KEY_REQUEST: // link key request bt_flip_addr(event_addr, &packet[2]); bt_send_cmd(&hci_link_key_request_negative_reply, &event_addr); break; case HCI_EVENT_PIN_CODE_REQUEST: // inform about pin code request bt_flip_addr(event_addr, &packet[2]); bt_send_cmd(&hci_pin_code_request_reply, &event_addr, 4, PIN); printf("Please enter PIN %s on remote device\n", PIN); break; case L2CAP_EVENT_CHANNEL_OPENED: // inform about new l2cap connection bt_flip_addr(event_addr, &packet[3]); uint16_t psm = READ_BT_16(packet, 11); source_cid = READ_BT_16(packet, 13); con_handle = READ_BT_16(packet, 9); if (packet[2] == 0) { printf("Channel successfully opened: "); print_bd_addr(event_addr); printf(", handle 0x%02x, psm 0x%02x, source cid 0x%02x, dest cid 0x%02x\n", con_handle, psm, source_cid, READ_BT_16(packet, 15)); // send SABM command on dlci 0 printf("Sending SABM #0\n"); _bt_rfcomm_send_sabm(source_cid, 1, 0); } else { printf("L2CAP connection to device "); print_bd_addr(event_addr); printf(" failed. status code %u\n", packet[2]); exit(1); } break; case HCI_EVENT_DISCONNECTION_COMPLETE: // connection closed -> quit test app printf("Basebank connection closed, exit.\n"); exit(0); break; case HCI_EVENT_COMMAND_COMPLETE: // connect to RFCOMM device (PSM 0x03) at addr if ( COMMAND_COMPLETE_EVENT(packet, hci_write_authentication_enable) ) { bt_send_cmd(&l2cap_create_channel, addr, 0x03); } break; default: // unhandled event if(DEBUG) printf("unhandled event : %02x\n", packet[0]); break; } break; default: // unhandled packet type if(DEBUG) printf("unhandled packet type : %02x\n", packet_type); break; } }
static int btstack_command_handler(connection_t *connection, uint8_t *packet, uint16_t size){ bd_addr_t addr; uint16_t cid; uint16_t psm; uint16_t service_channel; uint16_t mtu; uint8_t reason; uint8_t rfcomm_channel; uint8_t rfcomm_credits; uint32_t service_record_handle; client_state_t *client; uint16_t serviceSearchPatternLen; uint16_t attributeIDListLen; // BTstack internal commands - 16 Bit OpCode, 8 Bit ParamLen, Params... switch (READ_CMD_OCF(packet)){ case BTSTACK_GET_STATE: log_info("BTSTACK_GET_STATE"); hci_emit_state(); break; case BTSTACK_SET_POWER_MODE: log_info("BTSTACK_SET_POWER_MODE %u", packet[3]); // track client power requests client = client_for_connection(connection); if (!client) break; client->power_mode = packet[3]; // handle merged state if (!clients_require_power_on()){ start_power_off_timer(); } else if (!power_management_sleep) { stop_power_off_timer(); hci_power_control(HCI_POWER_ON); } break; case BTSTACK_GET_VERSION: log_info("BTSTACK_GET_VERSION"); hci_emit_btstack_version(); break; #ifdef USE_BLUETOOL case BTSTACK_SET_SYSTEM_BLUETOOTH_ENABLED: log_info("BTSTACK_SET_SYSTEM_BLUETOOTH_ENABLED %u", packet[3]); iphone_system_bt_set_enabled(packet[3]); hci_emit_system_bluetooth_enabled(iphone_system_bt_enabled()); break; case BTSTACK_GET_SYSTEM_BLUETOOTH_ENABLED: log_info("BTSTACK_GET_SYSTEM_BLUETOOTH_ENABLED"); hci_emit_system_bluetooth_enabled(iphone_system_bt_enabled()); break; #else case BTSTACK_SET_SYSTEM_BLUETOOTH_ENABLED: case BTSTACK_GET_SYSTEM_BLUETOOTH_ENABLED: hci_emit_system_bluetooth_enabled(0); break; #endif case BTSTACK_SET_DISCOVERABLE: log_info("BTSTACK_SET_DISCOVERABLE discoverable %u)", packet[3]); // track client discoverable requests client = client_for_connection(connection); if (!client) break; client->discoverable = packet[3]; // merge state hci_discoverable_control(clients_require_discoverable()); break; case BTSTACK_SET_BLUETOOTH_ENABLED: log_info("BTSTACK_SET_BLUETOOTH_ENABLED: %u\n", packet[3]); if (packet[3]) { // global enable global_enable = 1; hci_power_control(HCI_POWER_ON); } else { global_enable = 0; clients_clear_power_request(); hci_power_control(HCI_POWER_OFF); } break; case L2CAP_CREATE_CHANNEL_MTU: bt_flip_addr(addr, &packet[3]); psm = READ_BT_16(packet, 9); mtu = READ_BT_16(packet, 11); l2cap_create_channel_internal( connection, NULL, addr, psm, mtu); break; case L2CAP_CREATE_CHANNEL: bt_flip_addr(addr, &packet[3]); psm = READ_BT_16(packet, 9); l2cap_create_channel_internal( connection, NULL, addr, psm, 150); // until r865 break; case L2CAP_DISCONNECT: cid = READ_BT_16(packet, 3); reason = packet[5]; l2cap_disconnect_internal(cid, reason); break; case L2CAP_REGISTER_SERVICE: psm = READ_BT_16(packet, 3); mtu = READ_BT_16(packet, 5); l2cap_register_service_internal(connection, NULL, psm, mtu); break; case L2CAP_UNREGISTER_SERVICE: psm = READ_BT_16(packet, 3); l2cap_unregister_service_internal(connection, psm); break; case L2CAP_ACCEPT_CONNECTION: cid = READ_BT_16(packet, 3); l2cap_accept_connection_internal(cid); break; case L2CAP_DECLINE_CONNECTION: cid = READ_BT_16(packet, 3); reason = packet[7]; l2cap_decline_connection_internal(cid, reason); break; case RFCOMM_CREATE_CHANNEL: bt_flip_addr(addr, &packet[3]); rfcomm_channel = packet[9]; rfcomm_create_channel_internal( connection, &addr, rfcomm_channel ); break; case RFCOMM_CREATE_CHANNEL_WITH_CREDITS: bt_flip_addr(addr, &packet[3]); rfcomm_channel = packet[9]; rfcomm_credits = packet[10]; rfcomm_create_channel_with_initial_credits_internal( connection, &addr, rfcomm_channel, rfcomm_credits ); break; case RFCOMM_DISCONNECT: cid = READ_BT_16(packet, 3); reason = packet[5]; rfcomm_disconnect_internal(cid); break; case RFCOMM_REGISTER_SERVICE: rfcomm_channel = packet[3]; mtu = READ_BT_16(packet, 4); rfcomm_register_service_internal(connection, rfcomm_channel, mtu); break; case RFCOMM_REGISTER_SERVICE_WITH_CREDITS: rfcomm_channel = packet[3]; mtu = READ_BT_16(packet, 4); rfcomm_credits = packet[6]; rfcomm_register_service_with_initial_credits_internal(connection, rfcomm_channel, mtu, rfcomm_credits); break; case RFCOMM_UNREGISTER_SERVICE: service_channel = READ_BT_16(packet, 3); rfcomm_unregister_service_internal(service_channel); break; case RFCOMM_ACCEPT_CONNECTION: cid = READ_BT_16(packet, 3); rfcomm_accept_connection_internal(cid); break; case RFCOMM_DECLINE_CONNECTION: cid = READ_BT_16(packet, 3); reason = packet[7]; rfcomm_decline_connection_internal(cid); break; case RFCOMM_GRANT_CREDITS: cid = READ_BT_16(packet, 3); rfcomm_credits = packet[5]; rfcomm_grant_credits(cid, rfcomm_credits); break; case RFCOMM_PERSISTENT_CHANNEL: { if (remote_device_db) { // enforce \0 packet[3+248] = 0; rfcomm_channel = remote_device_db->persistent_rfcomm_channel((char*)&packet[3]); } else { // NOTE: hack for non-iOS platforms rfcomm_channel = rfcomm_channel_generator++; } log_info("RFCOMM_EVENT_PERSISTENT_CHANNEL %u", rfcomm_channel); uint8_t event[4]; event[0] = RFCOMM_EVENT_PERSISTENT_CHANNEL; event[1] = sizeof(event) - 2; event[2] = 0; event[3] = rfcomm_channel; hci_dump_packet(HCI_EVENT_PACKET, 0, event, sizeof(event)); socket_connection_send_packet(connection, HCI_EVENT_PACKET, 0, (uint8_t *) event, sizeof(event)); break; } case SDP_REGISTER_SERVICE_RECORD: log_info("SDP_REGISTER_SERVICE_RECORD size %u\n", size); sdp_register_service_internal(connection, &packet[3]); break; case SDP_UNREGISTER_SERVICE_RECORD: service_record_handle = READ_BT_32(packet, 3); log_info("SDP_UNREGISTER_SERVICE_RECORD handle 0x%x ", service_record_handle); sdp_unregister_service_internal(connection, service_record_handle); break; case SDP_CLIENT_QUERY_RFCOMM_SERVICES: bt_flip_addr(addr, &packet[3]); serviceSearchPatternLen = de_get_len(&packet[9]); memcpy(serviceSearchPattern, &packet[9], serviceSearchPatternLen); sdp_query_rfcomm_register_callback(handle_sdp_rfcomm_service_result, connection); sdp_query_rfcomm_channel_and_name_for_search_pattern(addr, serviceSearchPattern); break; case SDP_CLIENT_QUERY_SERVICES: bt_flip_addr(addr, &packet[3]); sdp_parser_init(); sdp_parser_register_callback(handle_sdp_client_query_result); serviceSearchPatternLen = de_get_len(&packet[9]); memcpy(serviceSearchPattern, &packet[9], serviceSearchPatternLen); attributeIDListLen = de_get_len(&packet[9+serviceSearchPatternLen]); memcpy(attributeIDList, &packet[9+serviceSearchPatternLen], attributeIDListLen); sdp_client_query(addr, (uint8_t*)&serviceSearchPattern[0], (uint8_t*)&attributeIDList[0]); // sdp_general_query_for_uuid(addr, 0x1002); break; default: log_error("Error: command %u not implemented\n:", READ_CMD_OCF(packet)); break; } // verbose log info on command before dumped command unknown to PacketLogger or Wireshark hci_dump_packet( HCI_COMMAND_DATA_PACKET, 1, packet, size); 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; } }