From 63f0ac45edbd37e2b11bbee452b87caddafe744b Mon Sep 17 00:00:00 2001 From: Matthias Ringwald Date: Thu, 6 Oct 2016 16:09:54 +0200 Subject: [PATCH] l2cap: extend test, fix bugs, add missing checks to pass PTS --- src/l2cap.c | 105 +++++++++++++++++++++++++++++++------ test/pts/.gitignore | 3 +- test/pts/Makefile | 1 + test/pts/le_data_channel.c | 57 +++++++++++++++----- 4 files changed, 134 insertions(+), 32 deletions(-) diff --git a/src/l2cap.c b/src/l2cap.c index 557a037fd..7a4bd03cd 100644 --- a/src/l2cap.c +++ b/src/l2cap.c @@ -611,7 +611,11 @@ static void l2cap_run(void){ break; case COMMAND_REJECT: l2cap_send_signaling_packet(handle, COMMAND_REJECT, sig_id, result, 0, NULL); + break; #ifdef ENABLE_BLE + case LE_CREDIT_BASED_CONNECTION_REQUEST: + l2cap_send_le_signaling_packet(handle, LE_CREDIT_BASED_CONNECTION_RESPONSE, sig_id, 0, 0, 0, 0, result); + break; case COMMAND_REJECT_LE: l2cap_send_le_signaling_packet(handle, COMMAND_REJECT, sig_id, result, 0, NULL); break; @@ -747,20 +751,24 @@ static void l2cap_run(void){ channel->state = L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE; // le psm, source cid, mtu, mps, initial credits channel->local_sig_id = l2cap_next_sig_id(); - l2cap_send_le_signaling_packet( channel->con_handle, LE_CREDIT_BASED_CONNECTION_REQUEST, channel->local_sig_id, channel->psm, channel->local_cid, channel->local_mtu, 23, 1); + channel->credits_incoming = channel->new_credits_incoming; + channel->new_credits_incoming = 0; + l2cap_send_le_signaling_packet( channel->con_handle, LE_CREDIT_BASED_CONNECTION_REQUEST, channel->local_sig_id, channel->psm, channel->local_cid, channel->local_mtu, 23, channel->credits_incoming); break; case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT: if (!hci_can_send_acl_packet_now(channel->con_handle)) break; // TODO: support larger MPS channel->state = L2CAP_STATE_OPEN; - l2cap_send_le_signaling_packet(channel->con_handle, LE_CREDIT_BASED_CONNECTION_RESPONSE, channel->remote_sig_id, channel->remote_cid, channel->local_mtu, 23, channel->credits_incoming, 0); + channel->credits_incoming = channel->new_credits_incoming; + channel->new_credits_incoming = 0; + l2cap_send_le_signaling_packet(channel->con_handle, LE_CREDIT_BASED_CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->local_mtu, 23, channel->credits_incoming, 0); // notify client l2cap_emit_channel_opened(channel, 0); break; case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE: if (!hci_can_send_acl_packet_now(channel->con_handle)) break; channel->state = L2CAP_STATE_INVALID; - l2cap_send_le_signaling_packet(channel->con_handle, LE_CREDIT_BASED_CONNECTION_RESPONSE, channel->remote_sig_id, channel->remote_cid, 0, 0, 0, channel->reason); + l2cap_send_le_signaling_packet(channel->con_handle, LE_CREDIT_BASED_CONNECTION_RESPONSE, channel->remote_sig_id, 0, 0, 0, 0, channel->reason); // discard channel - l2cap_finialize_channel_close without sending l2cap close event l2cap_stop_rtx(channel); btstack_linked_list_iterator_remove(&it); @@ -800,7 +808,7 @@ static void l2cap_run(void){ memcpy(&l2cap_payload[pos], &channel->send_sdu_buffer[channel->send_sdu_pos-2], payload_size); // -2 for virtual SDU len pos += payload_size; channel->send_sdu_pos += payload_size; - l2cap_setup_header(acl_buffer, channel->con_handle, channel->remote_cid, payload_size); + l2cap_setup_header(acl_buffer, channel->con_handle, channel->remote_cid, pos); // done channel->credits_outgoing--; @@ -1558,7 +1566,8 @@ static int l2cap_le_signaling_handler_dispatch(hci_con_handle_t handle, uint8_t uint8_t event[10]; uint16_t le_psm; uint16_t local_cid; - uint16_t credits; + uint16_t new_credits; + uint16_t credits_before; l2cap_service_t * service; l2cap_channel_t * channel; btstack_linked_list_iterator_t it; @@ -1618,6 +1627,32 @@ static int l2cap_le_signaling_handler_dispatch(hci_con_handle_t handle, uint8_t (*l2cap_event_packet_handler)( HCI_EVENT_PACKET, 0, event, sizeof(event)); break; + case COMMAND_REJECT: + // Find channel for this sig_id and connection handle + channel = NULL; + btstack_linked_list_iterator_init(&it, &l2cap_le_channels); + while (btstack_linked_list_iterator_has_next(&it)){ + l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); + if (a_channel->con_handle != handle) continue; + if (a_channel->local_sig_id != sig_id) continue; + channel = a_channel; + break; + } + if (!channel) break; + + // if received while waiting for le connection response, assume legacy device + if (channel->state == L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE){ + channel->state = L2CAP_STATE_CLOSED; + // no official value for this, use: Connection refused – LE_PSM not supported - 0x0002 + l2cap_emit_channel_opened(channel, 0x0002); + + // discard channel + btstack_linked_list_remove(&l2cap_le_channels, (btstack_linked_item_t *) channel); + btstack_memory_l2cap_channel_free(channel); + break; + } + break; + case LE_CREDIT_BASED_CONNECTION_REQUEST: // get hci connection, bail if not found (must not happen) @@ -1702,7 +1737,7 @@ static int l2cap_le_signaling_handler_dispatch(hci_con_handle_t handle, uint8_t l2cap_emit_channel_opened(channel, result); // discard channel - btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); + btstack_linked_list_remove(&l2cap_le_channels, (btstack_linked_item_t *) channel); btstack_memory_l2cap_channel_free(channel); break; } @@ -1720,10 +1755,20 @@ static int l2cap_le_signaling_handler_dispatch(hci_con_handle_t handle, uint8_t // find channel local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); channel = l2cap_le_get_channel_for_local_cid(local_cid); - if (!channel) break; - credits = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); - channel->credits_outgoing += credits; - log_info("l2cap: %u credits for 0x%02x", credits, local_cid); + if (!channel) { + log_error("l2cap: no channel for cid 0x%02x", local_cid); + break; + } + new_credits = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); + credits_before = channel->credits_outgoing; + channel->credits_outgoing += new_credits; + // check for credit overrun + if (credits_before > channel->credits_outgoing){ + log_error("l2cap: new credits caused overrrun for cid 0x%02x, disconnecting", local_cid); + channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; + break; + } + log_info("l2cap: %u credits for 0x%02x, now %u", new_credits, local_cid, channel->credits_outgoing); break; case DISCONNECTION_REQUEST: @@ -1735,6 +1780,9 @@ static int l2cap_le_signaling_handler_dispatch(hci_con_handle_t handle, uint8_t channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE; break; + case DISCONNECTION_RESPONSE: + break; + default: // command unknown -> reject command return 0; @@ -1826,11 +1874,13 @@ static void l2cap_acl_handler(uint8_t packet_type, uint16_t channel, uint8_t *pa memcpy(&l2cap_channel->receive_sdu_buffer[l2cap_channel->receive_sdu_pos], &packet[COMPLETE_L2CAP_HEADER+pos], size-COMPLETE_L2CAP_HEADER); l2cap_channel->receive_sdu_pos += size - COMPLETE_L2CAP_HEADER; // done? - log_debug("le packet pos %u, len %u", l2cap_channel->receive_sdu_pos, l2cap_channel->receive_sdu_len); + log_info("le packet pos %u, len %u", l2cap_channel->receive_sdu_pos, l2cap_channel->receive_sdu_len); if (l2cap_channel->receive_sdu_pos >= l2cap_channel->receive_sdu_len){ l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, l2cap_channel->receive_sdu_buffer, l2cap_channel->receive_sdu_len); l2cap_channel->receive_sdu_len = 0; } + } else { + log_error("LE Data Channel packet received but no channel found for cid 0x%02x", channel_id); } #endif break; @@ -1999,7 +2049,7 @@ uint8_t l2cap_le_accept_connection(uint16_t local_cid, uint8_t * receive_sdu_buf channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; // test - channel->new_credits_incoming = 1; + // channel->new_credits_incoming = 1; // go l2cap_run(); @@ -2052,7 +2102,7 @@ uint8_t l2cap_le_create_channel(btstack_packet_handler_t packet_handler, bd_addr channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; // test - channel->new_credits_incoming = 1; + // channel->new_credits_incoming = 1; // add to connections list btstack_linked_list_add(&l2cap_le_channels, (btstack_linked_item_t *) channel); @@ -2063,6 +2113,7 @@ uint8_t l2cap_le_create_channel(btstack_packet_handler_t packet_handler, bd_addr if (conn){ log_info("l2cap_le_create_channel, hci connection already exists"); l2cap_handle_le_connection_complete(channel, 0, conn->con_handle); + l2cap_run(); } else { // @@ -2088,12 +2139,32 @@ uint8_t l2cap_le_create_channel(btstack_packet_handler_t packet_handler, bd_addr * @param credits Number additional credits for peer */ uint8_t l2cap_le_provide_credits(uint16_t local_cid, uint16_t credits){ - // get channel - // bail if missing + + l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); + if (!channel) { + log_error("l2cap_le_provide_credits no channel for cid 0x%02x", local_cid); + return L2CAP_LOCAL_CID_DOES_NOT_EXIST; + } + // check state - // check incoming credits + credits <= 0xffff + if (channel->state != L2CAP_STATE_OPEN){ + log_error("l2cap_le_provide_credits but channel 0x%02x not open yet", local_cid); + } + + // assert incoming credits + credits <= 0xffff + uint32_t total_credits = channel->credits_incoming; + total_credits += channel->new_credits_incoming; + total_credits += credits; + if (total_credits > 0xffff){ + log_error("l2cap_le_provide_credits overrun: current %u, scheduled %u, additional %u", channel->credits_incoming, + channel->new_credits_incoming, credits); + } + // set credits_granted - // l2cap_le_run() + channel->new_credits_incoming += credits; + + // go + l2cap_run(); return 0; } diff --git a/test/pts/.gitignore b/test/pts/.gitignore index b266d15d3..da7bc00d7 100644 --- a/test/pts/.gitignore +++ b/test/pts/.gitignore @@ -13,4 +13,5 @@ l2cap_test profile.h sco_loopbackiopt sco_loopback -ioptle_data_channel +iopt +le_data_channel diff --git a/test/pts/Makefile b/test/pts/Makefile index fac7e3162..149f7c933 100644 --- a/test/pts/Makefile +++ b/test/pts/Makefile @@ -30,6 +30,7 @@ LDFLAGS += $(shell pkg-config libusb-1.0 --libs) # LDFLAGS += -L/sw/lib -lportaudio -Wl,-framework,CoreAudio -Wl,-framework,AudioToolbox -Wl,-framework,AudioUnit -Wl,-framework,Carbon EXAMPLES = iopt ble_peripheral_test ble_central_test l2cap_test classic_test bnep_test hsp_ag_test hsp_hs_test sco_loopback le_data_channel +EXAMPLES = le_data_channel all: ${EXAMPLES} diff --git a/test/pts/le_data_channel.c b/test/pts/le_data_channel.c index 81afd4917..e705b12d4 100644 --- a/test/pts/le_data_channel.c +++ b/test/pts/le_data_channel.c @@ -66,9 +66,8 @@ static btstack_packet_callback_registration_t hci_event_callback_registration; static btstack_packet_callback_registration_t sm_event_callback_registration; -static bd_addr_t pts_address = { 0x00, 0x02, 0x72, 0xDC, 0x31, 0xC1}; +static bd_addr_t pts_address = { 0x00, 0x1a, 0x7d, 0xda, 0x71, 0x0a}; static int pts_address_type = 0; -static bd_addr_t master_address = { 0x00, 0x02, 0x72, 0xDC, 0x31, 0xC1}; static int master_addr_type = 0; static hci_con_handle_t handle; static uint32_t ui_passkey; @@ -78,7 +77,11 @@ static uint16_t local_cid; // general discoverable flags static uint8_t adv_general_discoverable[] = { 2, 01, 02 }; -const uint16_t psm_x = 0xf0; +const uint16_t TSPX_le_psm = 0x25; +const uint16_t TSPX_psm_unsupported = 0xf1; +static uint16_t initial_credits = L2CAP_LE_AUTOMATIC_CREDITS; +const char * data_short = "a"; +const char * data_long = "0123456789abcdefghijklmnopqrstuvwxyz"; uint8_t receive_buffer_X[100]; @@ -125,7 +128,7 @@ static void app_packet_handler (uint8_t packet_type, uint16_t channel, uint8_t * case L2CAP_EVENT_INCOMING_CONNECTION: cid = little_endian_read_16(packet, 12); printf("L2CAP: Accepting incoming connection request for 0x%02x\n", cid); - l2cap_le_accept_connection(cid, receive_buffer_X, sizeof(receive_buffer_X), L2CAP_LE_AUTOMATIC_CREDITS); + l2cap_le_accept_connection(cid, receive_buffer_X, sizeof(receive_buffer_X), initial_credits); break; case L2CAP_EVENT_CHANNEL_OPENED: @@ -155,7 +158,7 @@ static void app_packet_handler (uint8_t packet_type, uint16_t channel, uint8_t * // display number master_addr_type = packet[4]; reverse_bd_addr(&packet[5], event_address); - printf("\nGAP Bonding %s (%u): Enter 6 digit passkey: '", bd_addr_to_str(master_address), master_addr_type); + printf("\nGAP Bonding %s (%u): Enter 6 digit passkey: '", bd_addr_to_str(pts_address), master_addr_type); fflush(stdout); ui_passkey = 0; ui_digits_for_passkey = 6; @@ -163,11 +166,11 @@ static void app_packet_handler (uint8_t packet_type, uint16_t channel, uint8_t * case SM_EVENT_PASSKEY_DISPLAY_NUMBER: // display number - printf("\nGAP Bonding %s (%u): Display Passkey '%06u\n", bd_addr_to_str(master_address), master_addr_type, little_endian_read_32(packet, 11)); + printf("\nGAP Bonding %s (%u): Display Passkey '%06u\n", bd_addr_to_str(pts_address), master_addr_type, little_endian_read_32(packet, 11)); break; case SM_EVENT_PASSKEY_DISPLAY_CANCEL: - printf("\nGAP Bonding %s (%u): Display cancel\n", bd_addr_to_str(master_address), master_addr_type); + printf("\nGAP Bonding %s (%u): Display cancel\n", bd_addr_to_str(pts_address), master_addr_type); break; case SM_EVENT_AUTHORIZATION_REQUEST: @@ -190,8 +193,12 @@ void show_usage(void){ gap_advertisements_get_address(&uit_addr_type, iut_address); printf("\n--- CLI for LE Data Channel %s ---\n", bd_addr_to_str(iut_address)); - printf("a - connect to type %u address %s PSM 0x%02x\n", pts_address_type, bd_addr_to_str(pts_address), psm_x); - printf("s - send data\n"); + printf("a - connect to type %u address %s PSM 0x%02x (TSPX_le_psm)\n", pts_address_type, bd_addr_to_str(pts_address), TSPX_le_psm); + printf("A - connect to type %u address %s PSM 0x%02x (TSPX_psm_unsupported)\n", pts_address_type, bd_addr_to_str(pts_address), TSPX_psm_unsupported); + printf("c - send 10 credits\n"); + printf("m - enable manual credit managment (incoming connections only)\n"); + printf("s - send short data %s\n", data_short); + printf("S - send long data %s\n", data_long); printf("t - disconnect channel\n"); printf("---\n"); printf("Ctrl-c - exit\n"); @@ -220,15 +227,37 @@ static void stdin_process(btstack_data_source_t *ds, btstack_data_source_callbac switch (buffer){ case 'a': - printf("Creating connection to %s\n", bd_addr_to_str(pts_address)); + printf("Creating connection to %s 0x%02x\n", bd_addr_to_str(pts_address), TSPX_le_psm); gap_advertisements_enable(0); - l2cap_le_create_channel(&app_packet_handler,pts_address, pts_address_type, psm_x, buffer_x, + l2cap_le_create_channel(&app_packet_handler,pts_address, pts_address_type, TSPX_le_psm, buffer_x, sizeof(buffer_x), L2CAP_LE_AUTOMATIC_CREDITS, LEVEL_0, &cid_x); break; + case 'A': + printf("Creating connection to %s 0x%02x\n", bd_addr_to_str(pts_address), TSPX_psm_unsupported); + gap_advertisements_enable(0); + l2cap_le_create_channel(&app_packet_handler,pts_address, pts_address_type, TSPX_psm_unsupported, buffer_x, + sizeof(buffer_x), L2CAP_LE_AUTOMATIC_CREDITS, LEVEL_0, &cid_x); + break; + + case 'c': + printf("Provide 10 credits\n"); + l2cap_le_provide_credits(local_cid, 10); + break; + + case 'm': + printf("Enable manual credit management for incoming connections\n"); + initial_credits = 5; + break; + case 's': - printf("Send L2CAP Data\n"); - l2cap_le_send_data(local_cid, (uint8_t *) "0123456789abcdefghijklmnopqrstuvwxyz", 36); + printf("Send L2CAP Data Short %s\n", data_short); + l2cap_le_send_data(local_cid, (uint8_t *) data_short, strlen(data_short)); + break; + + case 'S': + printf("Send L2CAP Data Long %s\n", data_long); + l2cap_le_send_data(local_cid, (uint8_t *) data_long, strlen(data_long)); break; case 't': @@ -276,7 +305,7 @@ int btstack_main(int argc, const char * argv[]){ sm_set_authentication_requirements(0); // le data channel setup - l2cap_le_register_service(&app_packet_handler, psm_x, LEVEL_0); + l2cap_le_register_service(&app_packet_handler, TSPX_le_psm, LEVEL_0); // turn on! hci_power_control(HCI_POWER_ON);