diff --git a/src/rfcomm.c b/src/rfcomm.c index da85472d1..af928315c 100644 --- a/src/rfcomm.c +++ b/src/rfcomm.c @@ -248,6 +248,7 @@ static void rfcomm_multiplexer_initialize(rfcomm_multiplexer_t *multiplexer){ multiplexer->l2cap_credits = 0; multiplexer->send_dm_for_dlci = 0; multiplexer->max_frame_size = rfcomm_max_frame_size_for_l2cap_mtu(l2cap_max_mtu()); + multiplexer->test_data_len = 0; } static rfcomm_multiplexer_t * rfcomm_multiplexer_create_for_addr(bd_addr_t *addr){ @@ -505,6 +506,32 @@ static int rfcomm_send_dm_pf(rfcomm_multiplexer_t *multiplexer, uint8_t dlci){ return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_DM_PF, 0, NULL, 0); } +// static int rfcomm_send_uih_test_cmd(rfcomm_multiplexer_t *multiplexer, uint8_t * data, uint16_t len) { +// int dlci = 0; +// uint8_t address = (1 << 0) | (multiplexer->outgoing << 1); +// uint8_t payload[3+len]; +// uint8_t pos = 0; +// payload[pos++] = BT_RFCOMM_TEST_CMD; +// payload[pos++] = len << 1 | 1; // len +// payload[pos++] = (1 << 0) | (1 << 1) | (dlci << 2); // CMD => C/R = 1 +// memcpy(&payload[pos], data, len); +// pos += len; +// return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_UIH, 0, (uint8_t *) payload, pos); +// } + +static int rfcomm_send_uih_test_rsp(rfcomm_multiplexer_t *multiplexer, uint8_t * data, uint16_t len) { + int dlci = 0; + uint8_t address = (1 << 0) | (multiplexer->outgoing << 1); + uint8_t payload[3+len]; + uint8_t pos = 0; + payload[pos++] = BT_RFCOMM_TEST_RSP; + payload[pos++] = len << 1 | 1; // len + payload[pos++] = (1 << 0) | (1 << 1) | (dlci << 2); // CMD => C/R = 1 + memcpy(&payload[pos], data, len); + pos += len; + return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_UIH, 0, (uint8_t *) payload, pos); +} + static int rfcomm_send_uih_msc_cmd(rfcomm_multiplexer_t *multiplexer, uint8_t dlci, uint8_t signals) { uint8_t address = (1 << 0) | (multiplexer->outgoing << 1); uint8_t payload[4]; @@ -886,7 +913,18 @@ static int rfcomm_multiplexer_l2cap_packet_handler(uint16_t channel, uint8_t *pa return 1; } break; - + + case BT_RFCOMM_TEST_CMD: { + log_info("Received test command"); + int len = packet[length_offset]; // length < 125 + if (len > RFCOMM_TEST_DATA_MAX_LEN){ + len = RFCOMM_TEST_DATA_MAX_LEN; + } + multiplexer->test_data_len = len; + memcpy(multiplexer->test_data, &packet[payload_offset], len); + return 1; + } + default: break; @@ -900,6 +938,7 @@ static void rfcomm_multiplexer_state_machine(rfcomm_multiplexer_t * multiplexer, if (multiplexer->send_dm_for_dlci){ rfcomm_send_dm_pf(multiplexer, multiplexer->send_dm_for_dlci); multiplexer->send_dm_for_dlci = 0; + return; } switch (multiplexer->state) { @@ -943,6 +982,22 @@ static void rfcomm_multiplexer_state_machine(rfcomm_multiplexer_t * multiplexer, break; } break; + case RFCOMM_MULTIPLEXER_OPEN: + switch (event) { + case MULT_EV_READY_TO_SEND: + // respond to test command + if (multiplexer->test_data_len){ + int len = multiplexer->test_data_len; + log_info("Sending TEST Response with %u bytes", len); + multiplexer->test_data_len = 0; + rfcomm_send_uih_test_rsp(multiplexer, multiplexer->test_data, len); + return; + } + break; + default: + break; + } + break; default: break; } diff --git a/src/rfcomm.h b/src/rfcomm.h index 87ae2238c..5709c3566 100644 --- a/src/rfcomm.h +++ b/src/rfcomm.h @@ -51,6 +51,8 @@ extern "C" { #define UNLIMITED_INCOMING_CREDITS 0xff +#define RFCOMM_TEST_DATA_MAX_LEN 4 + // private structs typedef enum { RFCOMM_MULTIPLEXER_CLOSED = 1, @@ -194,13 +196,17 @@ typedef struct { uint8_t outgoing; // hack to deal with authentication failure only observed by remote side - uint8_t at_least_one_connection; + uint8_t at_least_one_connection; uint16_t max_frame_size; // send DM for DLCI != 0 uint8_t send_dm_for_dlci; + // test data - limited to RFCOMM_TEST_DATA_MAX_LEN + uint8_t test_data_len; + uint8_t test_data[RFCOMM_TEST_DATA_MAX_LEN]; + } rfcomm_multiplexer_t; // info regarding an actual coneection