void l2cap_acl_handler( uint8_t *packet, uint16_t size ){ // Get Channel ID uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet); switch (channel_id) { case L2CAP_CID_SIGNALING: { uint16_t command_offset = 8; while (command_offset < size) { // handle signaling commands l2cap_signaling_handler_dispatch(handle, &packet[command_offset]); // increment command_offset command_offset += L2CAP_SIGNALING_COMMAND_DATA_OFFSET + READ_BT_16(packet, command_offset + L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); } break; } case L2CAP_CID_ATTRIBUTE_PROTOCOL: if (attribute_protocol_packet_handler) { (*attribute_protocol_packet_handler)(ATT_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); } break; case L2CAP_CID_SECURITY_MANAGER_PROTOCOL: if (security_protocol_packet_handler) { (*security_protocol_packet_handler)(SM_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); } break; default: { // Find channel for this channel_id and connection handle l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(channel_id); if (channel) { l2cap_dispatch(channel, L2CAP_DATA_PACKET, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); } break; } } l2cap_run(); }
int hci_send_acl_packet(uint8_t *packet, int size){ // check for free places on BT module if (!hci_number_free_acl_slots()) return BTSTACK_ACL_BUFFERS_FULL; hci_con_handle_t con_handle = READ_ACL_CONNECTION_HANDLE(packet); hci_connection_t *connection = connection_for_handle( con_handle); if (!connection) return 0; hci_connection_timestamp(connection); // count packet connection->num_acl_packets_sent++; // log_info("hci_send_acl_packet - handle %u, sent %u\n", connection->con_handle, connection->num_acl_packets_sent); // send packet int err = hci_stack.hci_transport->send_packet(HCI_ACL_DATA_PACKET, packet, size); return err; }
static void acl_handler(uint8_t *packet, int size){ // get info hci_con_handle_t con_handle = READ_ACL_CONNECTION_HANDLE(packet); hci_connection_t *conn = connection_for_handle(con_handle); uint8_t acl_flags = READ_ACL_FLAGS(packet); uint16_t acl_length = READ_ACL_LENGTH(packet); // ignore non-registered handle if (!conn){ log_error( "hci.c: acl_handler called with non-registered handle %u!\n" , con_handle); return; } // update idle timestamp hci_connection_timestamp(conn); // handle different packet types switch (acl_flags & 0x03) { case 0x01: // continuation fragment // sanity check if (conn->acl_recombination_pos == 0) { log_error( "ACL Cont Fragment but no first fragment for handle 0x%02x\n", con_handle); return; } // append fragment payload (header already stored) memcpy(&conn->acl_recombination_buffer[conn->acl_recombination_pos], &packet[4], acl_length ); conn->acl_recombination_pos += acl_length; // log_error( "ACL Cont Fragment: acl_len %u, combined_len %u, l2cap_len %u\n", acl_length, // conn->acl_recombination_pos, conn->acl_recombination_length); // forward complete L2CAP packet if complete. if (conn->acl_recombination_pos >= conn->acl_recombination_length + 4 + 4){ // pos already incl. ACL header hci_stack.packet_handler(HCI_ACL_DATA_PACKET, conn->acl_recombination_buffer, conn->acl_recombination_pos); // reset recombination buffer conn->acl_recombination_length = 0; conn->acl_recombination_pos = 0; } break; case 0x02: { // first fragment // sanity check if (conn->acl_recombination_pos) { log_error( "ACL First Fragment but data in buffer for handle 0x%02x\n", con_handle); return; } // peek into L2CAP packet! uint16_t l2cap_length = READ_L2CAP_LENGTH( packet ); // log_error( "ACL First Fragment: acl_len %u, l2cap_len %u\n", acl_length, l2cap_length); // compare fragment size to L2CAP packet size if (acl_length >= l2cap_length + 4){ // forward fragment as L2CAP packet hci_stack.packet_handler(HCI_ACL_DATA_PACKET, packet, acl_length + 4); } else { // store first fragment and tweak acl length for complete package memcpy(conn->acl_recombination_buffer, packet, acl_length + 4); conn->acl_recombination_pos = acl_length + 4; conn->acl_recombination_length = l2cap_length; bt_store_16(conn->acl_recombination_buffer, 2, l2cap_length +4); } break; } default: log_error( "hci.c: acl_handler called with invalid packet boundary flags %u\n", acl_flags & 0x03); return; } // execute main loop hci_run(); }
void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ hci_con_handle_t acl_in; hci_con_handle_t acl_out; switch (packet_type){ case HCI_ACL_DATA_PACKET: acl_in = READ_ACL_CONNECTION_HANDLE(packet); acl_out = 0; if (acl_in == alice_handle) { printf("Alice: "); hexdump( packet, size ); printf("\n\n"); acl_out = bob_handle; } if (acl_in == bob_handle) { printf("Bob: "); hexdump( packet, size ); printf("\n\n"); acl_out = alice_handle; } if (acl_out){ bt_store_16( packet, 0, (READ_BT_16(packet, 0) & 0xf000) | acl_out); bt_send_acl(packet, size); } break; case HCI_EVENT_PACKET: switch(packet[0]){ case BTSTACK_EVENT_STATE: // bt stack activated, get started - set COD if (packet[2] == HCI_STATE_WORKING) { bt_send_cmd(&hci_write_class_of_device, 0x7A020C); // used on iPhone } break; case HCI_EVENT_EXTENDED_INQUIRY_RESPONSE: // process EIR responses if (packet[17]) { bt_flip_addr(temp_addr, &packet[3]); if (BD_ADDR_CMP(temp_addr, bob_addr)) { printf("2. Got BOB's EIR. "); int i, k; bzero(bob_EIR, EIR_LEN); for (i=17, k=0;i<EIR_LEN && packet[i]; i += packet[i] + 1, k += bob_EIR[k] + 1){ if (packet[i+1] == 0x09) { // complete name id -- use own bob_EIR[k+0] = 1 + strlen(NAME); bob_EIR[k+1] = 0x09; memcpy(&bob_EIR[k+2], NAME, strlen(NAME)); } else { // vendor specific if (packet[i+1] == 0x0ff ) { bob_got_EIR = 1; } memcpy(&bob_EIR[k], &packet[i], packet[i]+1); } } hexdump(&bob_EIR, k); printf("\n\n"); bob_clock_offset = READ_BT_16(packet, 14); bob_page_scan_repetition_mode = packet[9]; } // stop inquiry // bt_send_cmd(&hci_inquiry_cancel); } break; case HCI_EVENT_CONNECTION_REQUEST: // accept incoming connections bt_flip_addr(temp_addr, &packet[2]); if (BD_ADDR_CMP(temp_addr, bob_addr) ){ printf("-> Connection request from BOB. Denying\n"); // bt_send_cmd(&hci_accept_connection_request, &temp_addr, 1); } else { printf("-> Connection request from Alice. Sending Accept\n"); bt_send_cmd(&hci_accept_connection_request, &temp_addr, 1); } break; case HCI_EVENT_CONNECTION_COMPLETE: // handle connections bt_flip_addr(temp_addr, &packet[5]); if (packet[2] == 0){ hci_con_handle_t incoming_handle = READ_BT_16(packet, 3); if (BD_ADDR_CMP(temp_addr, bob_addr)){ bob_handle = incoming_handle; printf("7. Connected to BOB (handle %u). Relaying data!\n", bob_handle); } else { alice_handle = incoming_handle; printf("6. Alice connected (handle %u). Connecting to BOB.\n", alice_handle); bt_send_cmd(&hci_create_connection, &bob_addr, 0x18, bob_page_scan_repetition_mode, 0, 0x8000 || bob_clock_offset, 0); } } else { printf("Connection complete status %u for connection", packet[2]); print_bd_addr(temp_addr); printf("\n"); } break; case HCI_EVENT_PIN_CODE_REQUEST: // inform about pin code request printf("Please enter PIN 1234 on remote device\n"); 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: // use pairing yes/no if ( COMMAND_COMPLETE_EVENT(packet, hci_write_class_of_device) ) { bt_send_cmd(&hci_write_authentication_enable, 0); } // allow Extended Inquiry responses if ( COMMAND_COMPLETE_EVENT(packet, hci_write_authentication_enable) ) { bt_send_cmd(&hci_write_inquiry_mode, 2); } // get all events, including EIRs if ( COMMAND_COMPLETE_EVENT(packet, hci_write_inquiry_mode) ) { bt_send_cmd(&hci_set_event_mask, 0xffffffff, 0x1fffffff); } // fine with us, too if ( COMMAND_COMPLETE_EVENT(packet, hci_set_event_mask) ) { bt_send_cmd(&hci_write_simple_pairing_mode, 1); } // start inquiry if ( COMMAND_COMPLETE_EVENT(packet, hci_write_simple_pairing_mode) ) { // enable capure bt_send_cmd(&btstack_set_acl_capture_mode, 1); printf("1. Started inquiry.\n"); bt_send_cmd(&hci_inquiry, HCI_INQUIRY_LAP, 15, 0); } // Connect to BOB if ( COMMAND_COMPLETE_EVENT(packet, hci_write_extended_inquiry_response) ) { printf("5. Waiting for Alice!...\n"); // bt_send_cmd(&hci_write_scan_enable, 3); // 3 inq scan + page scan // bt_send_cmd(&hci_create_connection, &addr, 0x18, page_scan_repetition_mode, 0, 0x8000 || clock_offset, 0); } break; default: // Inquiry done, set EIR if (packet[0] == HCI_EVENT_INQUIRY_COMPLETE || COMMAND_COMPLETE_EVENT(packet, hci_inquiry_cancel)){ if (!inquiry_done){ inquiry_done = 1; printf("3. Inquiry Complete\n"); if (bob_got_EIR){ printf("4. Set EIR to Bob's.\n"); bt_send_cmd(&hci_write_extended_inquiry_response, 0, bob_EIR); } else { // failed to get BOB's EIR } } } break; } default: break; } }