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;
}
}
