From 1e6aba4769cc9f1f9e6a695a7254eb66be5d4943 Mon Sep 17 00:00:00 2001 From: "matthias.ringwald" Date: Fri, 31 Jul 2009 21:41:15 +0000 Subject: [PATCH] moved l2cap state machine to l2cap.c. added 'channel' parameter to packet header for socket communications. removed unused functions. dispatch btstack commands in daemon.c. renamed non-HCI commands --- TODO.txt | 36 +++--- example/test.c | 14 ++- src/btstack.c | 243 +++------------------------------------- src/btstack.h | 13 +-- src/daemon.c | 79 ++++++++++--- src/hci.c | 21 +--- src/hci.h | 3 +- src/hci_cmds.c | 14 ++- src/hci_cmds.h | 12 +- src/hci_transport.h | 3 + src/hci_transport_usb.c | 4 +- src/l2cap.c | 213 +++++++++++++++++++++++++++++++++-- src/l2cap.h | 12 +- src/l2cap_signaling.c | 4 +- src/l2cap_signaling.h | 1 - src/socket_connection.c | 52 +++------ src/socket_connection.h | 9 +- 17 files changed, 374 insertions(+), 359 deletions(-) diff --git a/TODO.txt b/TODO.txt index e1e47a4cc..55d5c0ef5 100644 --- a/TODO.txt +++ b/TODO.txt @@ -1,29 +1,29 @@ /* new todo file for BTstack */ UNTIL 13. August 2009 : Google Open Source Jam -- naming conventions for various layers - - if all control is done by pseudo hci commands, only bt_send_cmd(&cmd_name, ... ) is needed -- move L2CAP code from btstack.c to l2cap.c - - connections are managed in BTdaemon - - source_cids and signaling_ids are assigned in BTdaemon - - create pseudo hci commands for create/destroy l2cap channel -- implement new L2CAP data interface, similar to unix sockets - - send data (channel, data, len) - - data callback(channel, data, len) - DEMOS: - - Python lightBlue: only L2CAP outgoing, or RFCOMM - - BTstack-cmd + - OpenGL-ES-WiiMote + - WiiMote - http://wiibrew.org/wiki/Wiimote#Bluetooth_Communication + - turn on accellerometer by sending packet 52 12 00 33 over PSM 0x13 OR 0x11 - 0x13 did not work -> send to 0x11 + - quick hack: run BTstack run loop in separate thread and use performOnMainThread to deliver data + - better: move run_loop.c to run_loop_posix.c and create run_loop_darwin.c + - BTstack-cmd - running on laptop and iPhone? - info - inquiry - remote name - open/close l2cap connection - evtl. SDP browser - - evtl. WiiMote - - evtl. RFCOMM to BT GPS NEXT: -- WiiMote - http://wiibrew.org/wiki/Wiimote#Bluetooth_Communication - - turn on accellerometer by sending packet 52 12 00 33 over PSM 0x13 OR 0x11 +- better connection management + - connections are managed in BTdaemon + - implement short cut for create connection, when connection already open + - already track connection request for better parallel access +- naming conventions for various layers + - if all control is done by pseudo hci commands, only bt_send_cmd(&cmd_name, ... ) is needed anyway +- auto-generate code for sending commands from structured text input file +- devise concept for access to event data + - auto-generate event struct getter? STRUCTURE_get_FIELD - implement rest of L2CAP state machine - incoming - error handling @@ -31,9 +31,5 @@ NEXT: - reassembly is best done in BTdaemon, assigning source_cids too. - client/server part using unix domain sockets (portable and fast) - implement RFCOMM -- implement -- use linked_list for: - - event handler - - data handler +- implement SDP - implement PAN -- implement some kind of noitfy mechanism between multiple units diff --git a/example/test.c b/example/test.c index b2483bddd..b6a005380 100644 --- a/example/test.c +++ b/example/test.c @@ -16,7 +16,7 @@ // bd_addr_t addr = {0x00, 0x03, 0xc9, 0x3d, 0x77, 0x43 }; // Think Outside Keyboard bd_addr_t addr = {0x00, 0x19, 0x1d, 0x90, 0x44, 0x68 }; // WiiMote -void acl_handler(uint8_t *packet, uint16_t size){ +void data_handler(uint8_t *packet, uint16_t size){ // just dump data for now hexdump( packet, size ); } @@ -35,7 +35,7 @@ void event_handler(uint8_t *packet, uint16_t size){ // connect to HID device (PSM 0x13) at addr if ( COMMAND_COMPLETE_EVENT(packet, hci_write_authentication_enable) ) { - l2cap_create_channel(addr, 0x13, event_handler, acl_handler); + bt_send_cmd(&l2cap_create_channel, addr, 0x13); } @@ -46,7 +46,12 @@ void event_handler(uint8_t *packet, uint16_t size){ // inform about new l2cap connection if (packet[0] == HCI_EVENT_L2CAP_CHANNEL_OPENED){ - printf("Channel successfully opened, handle 0x%02x, source cid 0x%02x, dest cid 0x%02x\n", READ_BT_16(packet, 2), READ_BT_16(packet, 4), READ_BT_16(packet, 6));; + uint16_t source_cid = READ_BT_16(packet, 4); + printf("Channel successfully opened, handle 0x%02x, source cid 0x%02x, dest cid 0x%02x\n", READ_BT_16(packet, 2), source_cid, READ_BT_16(packet, 6));; + + // request acceleration data.. probably has to be sent to control channel 0x11 instead of 0x13 + // uint8_t setMode33[] = { 0x52, 0x12, 0x00, 0x33 }; + // l2cap_send( source_cid, setMode33, sizeof(setMode33)); } } @@ -54,7 +59,8 @@ void event_handler(uint8_t *packet, uint16_t size){ int main (int argc, const char * argv[]){ bt_open(); bt_register_event_packet_handler(event_handler); - bt_send_cmd(&hci_set_power_mode, HCI_POWER_ON ); + bt_register_data_packet_handler(data_handler); + bt_send_cmd(&btstack_set_power_mode, HCI_POWER_ON ); run_loop_execute(); bt_close(); } \ No newline at end of file diff --git a/src/btstack.c b/src/btstack.c index d3b5f4390..eddf8c185 100644 --- a/src/btstack.c +++ b/src/btstack.c @@ -19,15 +19,11 @@ uint8_t hci_cmd_buffer[3+255]; -uint8_t l2cap_sig_buffer[48]; - static connection_t *btstack_connection = NULL; -static linked_list_t l2cap_channels = NULL; - /** prototypes */ static void dummy_handler(uint8_t *packet, uint16_t size); -int btstack_packet_handler(connection_t *connection, uint8_t packet_type, uint8_t *data, uint16_t size); +int btstack_packet_handler(connection_t *connection, uint16_t packet_type, uint16_t channel, uint8_t *data, uint16_t size); /* callback to L2CAP layer */ static void (*event_packet_handler)(uint8_t *packet, uint16_t size) = dummy_handler; @@ -49,182 +45,31 @@ int bt_close(){ return socket_connection_close_tcp(btstack_connection); } -void l2cap_event_handler( uint8_t *packet, uint16_t size ){ - // handle connection complete events - if (packet[0] == HCI_EVENT_CONNECTION_COMPLETE && packet[2] == 0){ - bd_addr_t address; - bt_flip_addr(address, &packet[5]); - - linked_item_t *it; - for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ - l2cap_channel_t * chan = (l2cap_channel_t *) it; - if ( ! BD_ADDR_CMP( chan->address, address) ){ - if (chan->state == L2CAP_STATE_CLOSED) { - chan->handle = READ_BT_16(packet, 3); - chan->sig_id = l2cap_next_sig_id(); - chan->source_cid = l2cap_next_source_cid(); - - bt_send_l2cap_signaling_packet( chan->handle, CONNECTION_REQUEST, chan->sig_id, chan->psm, chan->source_cid); - - chan->state = L2CAP_STATE_WAIT_CONNECT_RSP; - } - } - } - } - // handle disconnection complete events - // TODO ... -} - -void l2cap_signaling_handler(l2cap_channel_t *channel, uint8_t *packet, uint16_t size){ - - static uint8_t config_options[] = { 1, 2, 150, 0}; // mtu = 48 - - uint8_t code = READ_L2CAP_SIGNALING_CODE( packet ); - uint8_t identifier = READ_L2CAP_SIGNALING_IDENTIFIER( packet ); - - switch (channel->state) { - - case L2CAP_STATE_WAIT_CONNECT_RSP: - switch (code){ - case CONNECTION_RESPONSE: - if ( READ_BT_16 (packet, L2CAP_SIGNALING_DATA_OFFSET+3) == 0){ - // successfull connection - channel->dest_cid = READ_BT_16(packet, L2CAP_SIGNALING_DATA_OFFSET + 0); - channel->sig_id = l2cap_next_sig_id(); - bt_send_l2cap_signaling_packet(channel->handle, CONFIGURE_REQUEST, channel->sig_id, channel->dest_cid, 0, 4, &config_options); - channel->state = L2CAP_STATE_WAIT_CONFIG_REQ_RSP; - } else { - // TODO implement failed - } - break; - // TODO implement other signaling packets - } - break; - - case L2CAP_STATE_WAIT_CONFIG_REQ_RSP: - switch (code) { - case CONFIGURE_RESPONSE: - channel->state = L2CAP_STATE_WAIT_CONFIG_REQ; - break; - } - break; - - case L2CAP_STATE_WAIT_CONFIG_REQ: - switch (code) { - case CONFIGURE_REQUEST: - - // accept the other's configuration options - bt_send_l2cap_signaling_packet(channel->handle, CONFIGURE_RESPONSE, identifier, channel->dest_cid, 0, 0, size - 16, &packet[16]); - - channel->state = L2CAP_STATE_OPEN; - - // notify client - uint8_t event[8]; - event[0] = HCI_EVENT_L2CAP_CHANNEL_OPENED; - event[1] = 6; - bt_store_16(event, 2, channel->handle); - bt_store_16(event, 4, channel->source_cid); - bt_store_16(event, 6, channel->dest_cid); - (*channel->event_callback)(event, sizeof(event)); - break; - } - break; - } -} - -void l2cap_data_handler( uint8_t *packet, uint16_t size ){ - - // Get Channel ID and command code - uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); - uint8_t code = READ_L2CAP_SIGNALING_CODE( packet ); - - // Get Connection - hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet); - - // Signaling Packet? - if (channel_id == 1) { - - if (code < 1 || code == 2 || code >= 8){ - // not for a particular channel - return; - } - - // Get Signaling Identifier and potential destination CID - uint8_t sig_id = READ_L2CAP_SIGNALING_IDENTIFIER(packet); - uint16_t dest_cid = READ_BT_16(packet, L2CAP_SIGNALING_DATA_OFFSET); - - // Find channel for this sig_id and connection handle - linked_item_t *it; - for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ - l2cap_channel_t * chan = (l2cap_channel_t *) it; - if (chan->handle == handle) { - if (code & 1) { - // match odd commands by previous signaling identifier - if (chan->sig_id == sig_id) { - l2cap_signaling_handler( chan, packet, size); - } - } else { - // match even commands by source channel id - if (chan->source_cid == dest_cid) { - l2cap_signaling_handler( chan, packet, size); - } - } - } - } - return; - } - - // Find channel for this channel_id and connection handle - linked_item_t *it; - for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ - l2cap_channel_t * channel = (l2cap_channel_t *) it; - if ( channel->source_cid == channel_id && channel->handle == handle) { - (*channel->data_callback)(packet, size); - } - } -} - -int btstack_packet_handler(connection_t *connection, uint8_t packet_type, uint8_t *data, uint16_t size){ - switch (packet_type){ - case HCI_EVENT_PACKET: - l2cap_event_handler(data, size); - (*event_packet_handler)(data, size); - break; - case HCI_ACL_DATA_PACKET: - l2cap_data_handler(data, size); - (*acl_packet_handler)(data, size); - break; - default: - break; - } - return 0; -} - // send hci cmd packet int bt_send_cmd(hci_cmd_t *cmd, ...){ va_list argptr; va_start(argptr, cmd); uint16_t len = hci_create_cmd_internal(hci_cmd_buffer, cmd, argptr); va_end(argptr); - socket_connection_send_packet(btstack_connection, HCI_COMMAND_DATA_PACKET, hci_cmd_buffer, len); + socket_connection_send_packet(btstack_connection, HCI_COMMAND_DATA_PACKET, 0, hci_cmd_buffer, len); return 0; } -// send hci acl packet -int bt_send_acl_packet(uint8_t *packet, int size){ - // printf("Send ACL: "); hexdump(packet,size); printf("\n"); - socket_connection_send_packet(btstack_connection, HCI_ACL_DATA_PACKET, packet, size); +//@TODO: merge handler? +int btstack_packet_handler(connection_t *connection, uint16_t packet_type, uint16_t channel, uint8_t *data, uint16_t size){ + switch (packet_type){ + case HCI_EVENT_PACKET: + (*event_packet_handler)(data, size); + break; + case HCI_ACL_DATA_PACKET: + (*acl_packet_handler)(data, size); + break; + default: + break; + } return 0; } -int bt_send_l2cap_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, ...){ - va_list argptr; - va_start(argptr, identifier); - uint16_t len = l2cap_create_signaling_internal(l2cap_sig_buffer, handle, cmd, identifier, argptr); - va_end(argptr); - return bt_send_acl_packet(l2cap_sig_buffer, len); -} - static void dummy_handler(uint8_t *packet, uint16_t size){ } @@ -233,66 +78,12 @@ void bt_register_event_packet_handler(void (*handler)(uint8_t *packet, uint16_t event_packet_handler = handler; } -void bt_register_acl_packet_handler (void (*handler)(uint8_t *packet, uint16_t size)){ +void bt_register_data_packet_handler (void (*handler)(uint8_t *packet, uint16_t size)){ acl_packet_handler = handler; } -// open outgoing L2CAP channel -l2cap_channel_t * l2cap_create_channel(bd_addr_t address, uint16_t psm, void (*event_cb)(uint8_t *packet, uint16_t size), - void (*data_cb)(uint8_t *packet, uint16_t size)){ - // alloc structure - l2cap_channel_t * chan = malloc(sizeof(l2cap_channel_t)); - if (!chan) return NULL; - - // fill in - BD_ADDR_COPY(chan->address, address); - chan->psm = psm; - chan->handle = 0; - chan->event_callback = event_cb; - chan->data_callback = data_cb; - - // set initial state - chan->state = L2CAP_STATE_CLOSED; - chan->sig_id = L2CAP_SIG_ID_INVALID; - - // add to connections list - linked_list_add(&l2cap_channels, (linked_item_t *) chan); - - // send connection request - // BD_ADDR, Packet_Type, Page_Scan_Repetition_Mode, Reserved, Clock_Offset, Allow_Role_Switch - bt_send_cmd(&hci_create_connection, address, 0x18, 0, 0, 0, 0); - - return chan; -} - void l2cap_send(uint16_t source_cid, uint8_t *data, uint16_t len){ - // find channel for source_cid, construct l2cap packet and send - linked_item_t *it; - for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ - l2cap_channel_t * channel = (l2cap_channel_t *) it; - if ( channel->source_cid == source_cid) { - - // use hci_cmd_buffer for now - - // 0 - Connection handle : PB=10 : BC=00 - bt_store_16(hci_cmd_buffer, 0, channel->handle | (2 << 12) | (0 << 14)); - // 2 - ACL length - bt_store_16(hci_cmd_buffer, 2, len + 4); - // 4 - L2CAP packet length - bt_store_16(hci_cmd_buffer, 4, len + 0); - // 6 - L2CAP channel DEST - bt_store_16(hci_cmd_buffer, 6, channel->dest_cid); - // 8 - data - memcpy(&hci_cmd_buffer[8], data, len); - // send - bt_send_acl_packet(hci_cmd_buffer, len+8); - - return; - } - } -} - -void l2cap_disconnect(uint16_t source_cid, uint8_t reason){ - // TODO implement + // send + socket_connection_send_packet(btstack_connection, L2CAP_DATA_PACKET, source_cid, data, len); } diff --git a/src/btstack.h b/src/btstack.h index fb58c4c1a..423754c5c 100644 --- a/src/btstack.h +++ b/src/btstack.h @@ -23,19 +23,8 @@ int bt_close(); // send hci cmd packet int bt_send_cmd(hci_cmd_t *cmd, ...); -// send hci acl packet -int bt_send_acl_packet(uint8_t *packet, int size); - // register packet and event handler void bt_register_event_packet_handler(void (*handler)(uint8_t *packet, uint16_t size)); -void bt_register_acl_packet_handler (void (*handler)(uint8_t *packet, uint16_t size)); - -// TODO: temp -int bt_send_l2cap_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, ...); - -// outgoing connections -l2cap_channel_t * l2cap_create_channel(bd_addr_t bd_addr, uint16_t psm, void (*event_cb)(uint8_t *packet, uint16_t size), - void (*data_cb)(uint8_t *packet, uint16_t size)); -void l2cap_disconnect(uint16_t source_cid, uint8_t reason); +void bt_register_data_packet_handler (void (*handler)(uint8_t *packet, uint16_t size)); void l2cap_send(uint16_t source_cid, uint8_t *data, uint16_t len); diff --git a/src/daemon.c b/src/daemon.c index 3c06c7cc3..1e35028f1 100644 --- a/src/daemon.c +++ b/src/daemon.c @@ -37,22 +37,65 @@ static hci_transport_t * transport; static hci_uart_config_t config; -static int daemon_packet_handler(connection_t *connection, uint8_t packet_type, uint8_t *data, uint16_t length){ - switch (packet_type){ - case HCI_COMMAND_DATA_PACKET: - hci_send_cmd_packet(data, length); - // printf("CMD from client: "); +static int btstack_command_handler(connection_t *connection, uint8_t *packet, uint16_t size){ + // BTstack Commands + hci_dump_packet( HCI_COMMAND_DATA_PACKET, 1, packet, size); + bd_addr_t addr; + uint16_t cid; + uint16_t psm; + uint8_t reason; + // BTstack internal commands - 16 Bit OpCode, 8 Bit ParamLen, Params... + switch (READ_CMD_OCF(packet)){ + case HCI_BTSTACK_GET_STATE: + hci_emit_state(); break; - case HCI_ACL_DATA_PACKET: - hci_send_acl_packet(data, length); - // printf("ACL from client: "); + case HCI_BTSTACK_SET_POWER_MODE: + hci_power_control(packet[3]); + break; + case L2CAP_CREATE_CHANNEL: + bt_flip_addr(addr, &packet[3]); + psm = READ_BT_16(packet, 9); + l2cap_create_channel_internal( connection, addr, psm ); + break; + case L2CAP_DISCONNECT: + cid = READ_BT_16(packet, 3); + reason = packet[5]; + l2cap_disconnect_internal(cid, reason); + break; + default: + //@TODO: log into hci dump as vendor specific "event" + printf("Error: command %u not implemented\n:", READ_CMD_OCF(packet)); break; } - // hexdump(data, length); - // printf("\n"); return 0; } +static int daemon_client_handler(connection_t *connection, uint16_t packet_type, uint16_t channel, uint8_t *data, uint16_t length){ + switch (packet_type){ + case HCI_COMMAND_DATA_PACKET: + if (READ_CMD_OGF(data) != OGF_BTSTACK) { + // HCI Command + hci_send_cmd_packet(data, length); + } else { + // BTstack command + btstack_command_handler(connection, data, length); + } + break; + case HCI_ACL_DATA_PACKET: + // process l2cap packet... + channel = READ_BT_16(data, 0); + l2cap_send_internal(channel, data, length); + break; + } + return 0; +} + +static void event_handler( uint8_t *packet, uint16_t size ){ + // already passed by HCI, send to L2CAP & client + l2cap_event_handler(packet, size); + socket_connection_send_packet_all(HCI_EVENT_PACKET, 0, packet, size); +} + int main (int argc, const char * argv[]){ bt_control_t * control = NULL; @@ -72,11 +115,11 @@ int main (int argc, const char * argv[]){ control = &bt_control_iphone; #endif - // @TODO allow configuration per HCI CMD + // @TODO: allow configuration per HCI CMD // use logger: format HCI_DUMP_PACKETLOGGER, HCI_DUMP_BLUEZ or HCI_DUMP_STDOUT - hci_dump_open("/tmp/hci_dump.pklg", HCI_DUMP_PACKETLOGGER); - // hci_dump_open(NULL, HCI_DUMP_STDOUT); + // hci_dump_open("/tmp/hci_dump.pklg", HCI_DUMP_PACKETLOGGER); + hci_dump_open(NULL, HCI_DUMP_STDOUT); // init HCI hci_init(transport, &config, control); @@ -87,15 +130,15 @@ int main (int argc, const char * argv[]){ // // register callbacks // - hci_register_event_packet_handler(&socket_connection_send_event_all); - hci_register_acl_packet_handler(&socket_connection_send_acl_all); + hci_register_event_packet_handler(&event_handler); + hci_register_acl_packet_handler(&l2cap_acl_handler); - // @TODO make choice of socket server configurable (TCP and/or Unix Domain Socket) - // @TODO make port and/or socket configurable per config.h + // @TODO: make choice of socket server configurable (TCP and/or Unix Domain Socket) + // @TODO: make port and/or socket configurable per config.h // create server socket_connection_create_tcp(BTSTACK_PORT); - socket_connection_register_packet_callback(daemon_packet_handler); + socket_connection_register_packet_callback(daemon_client_handler); // go! run_loop_execute(); diff --git a/src/hci.c b/src/hci.c index 019e0fe61..b587fbf8a 100644 --- a/src/hci.c +++ b/src/hci.c @@ -12,6 +12,9 @@ #include "hci.h" #include "hci_dump.h" +// temp +#include "l2cap.h" + // the STACK is here static hci_stack_t hci_stack; @@ -192,7 +195,7 @@ void hci_init(hci_transport_t *transport, void *config, bt_control_t *control){ transport->register_acl_packet_handler( acl_handler); } -static void hci_emit_state(){ +void hci_emit_state(){ uint8_t event[3]; event[0] = HCI_EVENT_BTSTACK_STATE; event[1] = 1; @@ -286,22 +289,6 @@ int hci_send_cmd_packet(uint8_t *packet, int size){ hci_stack.num_cmd_packets--; return hci_stack.hci_transport->send_cmd_packet(packet, size); } - - hci_dump_packet( HCI_COMMAND_DATA_PACKET, 1, packet, size); - - // BTstack internal commands - 16 Bit OpCode, 8 Bit ParamLen, Params... - switch (READ_CMD_OCF(packet)){ - case HCI_BTSTACK_GET_STATE: - hci_emit_state(); - break; - case HCI_BTSTACK_SET_POWER_MODE: - hci_power_control(packet[3]); - break; - default: - // TODO log into hci dump as vendor specific "event" - printf("Error: command %u not implemented\n:", READ_CMD_OCF(packet)); - break; - } return 0; } diff --git a/src/hci.h b/src/hci.h index f12a31080..0d79cb797 100644 --- a/src/hci.h +++ b/src/hci.h @@ -94,4 +94,5 @@ int hci_send_cmd_packet(uint8_t *packet, int size); // send ACL packet int hci_send_acl_packet(uint8_t *packet, int size); -// \ No newline at end of file +// +void hci_emit_state(); diff --git a/src/hci_cmds.c b/src/hci_cmds.c index 34ff9eb9a..33dac642c 100644 --- a/src/hci_cmds.c +++ b/src/hci_cmds.c @@ -196,14 +196,24 @@ OPCODE(OGF_INFORMATIONAL_PARAMETERS, 0x09), "" // BTstack commands -hci_cmd_t hci_get_btstack_state = { +hci_cmd_t btstack_get_state = { OPCODE(OGF_BTSTACK, HCI_BTSTACK_GET_STATE), "" // no params -> }; -hci_cmd_t hci_set_power_mode = { +hci_cmd_t btstack_set_power_mode = { OPCODE(OGF_BTSTACK, HCI_BTSTACK_SET_POWER_MODE), "1" // mode: 0 = off, 1 = on }; +hci_cmd_t l2cap_create_channel = { +OPCODE(OGF_BTSTACK, L2CAP_CREATE_CHANNEL), "B2" +// @param bd_addr(48), psm (16) +}; + +hci_cmd_t l2cap_disconnect = { +OPCODE(OGF_BTSTACK, L2CAP_DISCONNECT), "21" +// @param channel(16), reason(8) +}; + diff --git a/src/hci_cmds.h b/src/hci_cmds.h index ccc0b02ee..e5a43c841 100644 --- a/src/hci_cmds.h +++ b/src/hci_cmds.h @@ -38,6 +38,12 @@ // set power mode: @param HCI_POWER_MODE #define HCI_BTSTACK_SET_POWER_MODE 0x02 +// create l2cap channel: @param bd_addr(48), psm (16) +#define L2CAP_CREATE_CHANNEL 0x03 + +// disconnect l2cap disconnect, @param channel(16), reason(8) +#define L2CAP_DISCONNECT 0x04 + // Events from host controller to host #define HCI_EVENT_INQUIRY_COMPLETE 0x01 #define HCI_EVENT_INQUIRY_RESULT 0x02 @@ -138,5 +144,7 @@ extern hci_cmd_t hci_write_extended_inquiry_response; extern hci_cmd_t hci_write_simple_pairing_mode; // BTSTACK client/server commands - see hci.c for info on parameters -extern hci_cmd_t hci_get_btstack_state; -extern hci_cmd_t hci_set_power_mode; +extern hci_cmd_t btstack_get_state; +extern hci_cmd_t btstack_set_power_mode; +extern hci_cmd_t l2cap_create_channel; +extern hci_cmd_t l2cap_disconnect; diff --git a/src/hci_transport.h b/src/hci_transport.h index 73dbf2aed..a62a2949b 100644 --- a/src/hci_transport.h +++ b/src/hci_transport.h @@ -34,6 +34,9 @@ */ #define HCI_EVENT_PACKET 0x04 +// extension for client/server communication +#define L2CAP_DATA_PACKET 0x05 + typedef struct { int (*open)(void *transport_config); int (*close)(); diff --git a/src/hci_transport_usb.c b/src/hci_transport_usb.c index 9c6bd293d..c95ee50ed 100644 --- a/src/hci_transport_usb.c +++ b/src/hci_transport_usb.c @@ -74,7 +74,7 @@ int find_bt(libusb_device **devs) libusb_get_bus_number(dev), libusb_get_device_address(dev), desc.bDeviceClass, desc.bDeviceSubClass, desc.bDeviceProtocol); - // @TODO detect BT USB Dongle based on character and not by id + // @TODO: detect BT USB Dongle based on character and not by id // The class code (bDeviceClass) is 0xE0 – Wireless Controller. // The SubClass code (bDeviceSubClass) is 0x01 – RF Controller. // The Protocol code (bDeviceProtocol) is 0x01 – Bluetooth programming. @@ -259,7 +259,7 @@ static int usb_open(void *transport_config){ #endif static int usb_close(){ - // @TODO remove all run loops! + // @TODO: remove all run loops! switch (libusb_state){ case LIB_USB_TRANSFERS_ALLOCATED: diff --git a/src/l2cap.c b/src/l2cap.c index 22f8b14f6..7ec27d213 100644 --- a/src/l2cap.c +++ b/src/l2cap.c @@ -13,23 +13,216 @@ #include -static uint8_t * sig_buffer; +static uint8_t * sig_buffer = NULL; +static linked_list_t l2cap_channels = NULL; +static uint8_t * acl_buffer = NULL; + +void l2cap_init(){ + sig_buffer = malloc( 48 ); + acl_buffer = malloc( 255 + 8 ); +} + int l2cap_send_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, ...){ va_list argptr; va_start(argptr, identifier); uint16_t len = l2cap_create_signaling_internal(sig_buffer, handle, cmd, identifier, argptr); + va_end(argptr); return hci_send_acl_packet(sig_buffer, len); } -uint16_t l2cap_create_signaling_packet(uint8_t *acl_buffer, hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, ...){ - va_list argptr; - va_start(argptr, identifier); - uint16_t len = l2cap_create_signaling_internal(acl_buffer, handle, cmd, identifier, argptr); - va_end(argptr); - return len; +// open outgoing L2CAP channel +void l2cap_create_channel_internal(connection_t * connection, bd_addr_t address, uint16_t psm){ + + // alloc structure + l2cap_channel_t * chan = malloc(sizeof(l2cap_channel_t)); + // TODO: emit error event + if (!chan) return; + + // fill in + BD_ADDR_COPY(chan->address, address); + chan->psm = psm; + chan->handle = 0; + chan->connection = connection; + + // set initial state + chan->state = L2CAP_STATE_CLOSED; + chan->sig_id = L2CAP_SIG_ID_INVALID; + + // add to connections list + linked_list_add(&l2cap_channels, (linked_item_t *) chan); + + // send connection request + // BD_ADDR, Packet_Type, Page_Scan_Repetition_Mode, Reserved, Clock_Offset, Allow_Role_Switch + hci_send_cmd(&hci_create_connection, address, 0x18, 0, 0, 0, 0); } -void l2cap_init(){ - sig_buffer = malloc( 48 ); -} \ No newline at end of file +void l2cap_disconnect_internal(uint16_t source_cid, uint8_t reason){ + // TODO: implement +} + + +void l2cap_event_handler( uint8_t *packet, uint16_t size ){ + // handle connection complete events + if (packet[0] == HCI_EVENT_CONNECTION_COMPLETE && packet[2] == 0){ + bd_addr_t address; + bt_flip_addr(address, &packet[5]); + + linked_item_t *it; + for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ + l2cap_channel_t * chan = (l2cap_channel_t *) it; + if ( ! BD_ADDR_CMP( chan->address, address) ){ + if (chan->state == L2CAP_STATE_CLOSED) { + chan->handle = READ_BT_16(packet, 3); + chan->sig_id = l2cap_next_sig_id(); + chan->source_cid = l2cap_next_source_cid(); + + l2cap_send_signaling_packet( chan->handle, CONNECTION_REQUEST, chan->sig_id, chan->psm, chan->source_cid); + + chan->state = L2CAP_STATE_WAIT_CONNECT_RSP; + } + } + } + } + // handle disconnection complete events + //@TODO:... +} + +void l2cap_signaling_handler(l2cap_channel_t *channel, uint8_t *packet, uint16_t size){ + + static uint8_t config_options[] = { 1, 2, 150, 0}; // mtu = 48 + + uint8_t code = READ_L2CAP_SIGNALING_CODE( packet ); + uint8_t identifier = READ_L2CAP_SIGNALING_IDENTIFIER( packet ); + + switch (channel->state) { + + case L2CAP_STATE_WAIT_CONNECT_RSP: + switch (code){ + case CONNECTION_RESPONSE: + if ( READ_BT_16 (packet, L2CAP_SIGNALING_DATA_OFFSET+3) == 0){ + // successfull connection + channel->dest_cid = READ_BT_16(packet, L2CAP_SIGNALING_DATA_OFFSET + 0); + channel->sig_id = l2cap_next_sig_id(); + l2cap_send_signaling_packet(channel->handle, CONFIGURE_REQUEST, channel->sig_id, channel->dest_cid, 0, 4, &config_options); + channel->state = L2CAP_STATE_WAIT_CONFIG_REQ_RSP; + } else { + //@TODO: implement failed + } + break; + //@TODO: implement other signaling packets + } + break; + + case L2CAP_STATE_WAIT_CONFIG_REQ_RSP: + switch (code) { + case CONFIGURE_RESPONSE: + channel->state = L2CAP_STATE_WAIT_CONFIG_REQ; + break; + } + break; + + case L2CAP_STATE_WAIT_CONFIG_REQ: + switch (code) { + case CONFIGURE_REQUEST: + + // accept the other's configuration options + l2cap_send_signaling_packet(channel->handle, CONFIGURE_RESPONSE, identifier, channel->dest_cid, 0, 0, size - 16, &packet[16]); + + channel->state = L2CAP_STATE_OPEN; + + // notify client + uint8_t event[8]; + event[0] = HCI_EVENT_L2CAP_CHANNEL_OPENED; + event[1] = 6; + bt_store_16(event, 2, channel->handle); + bt_store_16(event, 4, channel->source_cid); + bt_store_16(event, 6, channel->dest_cid); + socket_connection_send_packet(channel->connection, HCI_EVENT_PACKET, 0, event, sizeof(event)); + break; + } + break; + } +} + +void l2cap_acl_handler( uint8_t *packet, uint16_t size ){ + + // Get Channel ID and command code + uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); + uint8_t code = READ_L2CAP_SIGNALING_CODE( packet ); + + // Get Connection + hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet); + + // Signaling Packet? + if (channel_id == 1) { + + if (code < 1 || code == 2 || code >= 8){ + // not for a particular channel + return; + } + + // Get Signaling Identifier and potential destination CID + uint8_t sig_id = READ_L2CAP_SIGNALING_IDENTIFIER(packet); + uint16_t dest_cid = READ_BT_16(packet, L2CAP_SIGNALING_DATA_OFFSET); + + // Find channel for this sig_id and connection handle + linked_item_t *it; + for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ + l2cap_channel_t * chan = (l2cap_channel_t *) it; + if (chan->handle == handle) { + if (code & 1) { + // match odd commands by previous signaling identifier + if (chan->sig_id == sig_id) { + l2cap_signaling_handler( chan, packet, size); + } + } else { + // match even commands by source channel id + if (chan->source_cid == dest_cid) { + l2cap_signaling_handler( chan, packet, size); + } + } + } + } + return; + } + + // Find channel for this channel_id and connection handle + linked_item_t *it; + for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ + l2cap_channel_t * channel = (l2cap_channel_t *) it; + if ( channel->source_cid == channel_id && channel->handle == handle) { + // send data packet back + socket_connection_send_packet(channel->connection, HCI_ACL_DATA_PACKET, 0, packet, size); + } + } +} + +void l2cap_send_internal(uint16_t source_cid, uint8_t *data, uint16_t len){ + // find channel for source_cid, construct l2cap packet and send + linked_item_t *it; + for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ + l2cap_channel_t * channel = (l2cap_channel_t *) it; + if ( channel->source_cid == source_cid) { + + // use hci_cmd_buffer for now + + // 0 - Connection handle : PB=10 : BC=00 + bt_store_16(acl_buffer, 0, channel->handle | (2 << 12) | (0 << 14)); + // 2 - ACL length + bt_store_16(acl_buffer, 2, len + 4); + // 4 - L2CAP packet length + bt_store_16(acl_buffer, 4, len + 0); + // 6 - L2CAP channel DEST + bt_store_16(acl_buffer, 6, channel->dest_cid); + // 8 - data + memcpy(&acl_buffer[8], data, len); + // send + hci_send_acl_packet(acl_buffer, len+8); + + return; + } + } +} + + diff --git a/src/l2cap.h b/src/l2cap.h index 2c57f5dd9..7f3fe3ced 100644 --- a/src/l2cap.h +++ b/src/l2cap.h @@ -11,6 +11,7 @@ #include "hci.h" #include "l2cap_signaling.h" #include "utils.h" +#include "socket_connection.h" #define L2CAP_SIG_ID_INVALID 0 @@ -35,8 +36,7 @@ typedef struct { bd_addr_t address; uint16_t psm; hci_con_handle_t handle; - void (*event_callback)(uint8_t *packet, uint16_t size); - void (*data_callback)(uint8_t *packet, uint16_t size); + connection_t *connection; // uint16_t mtu_incoming; // uint16_t mtu_outgoing; // uint16_t flush_timeout_incoming; @@ -48,4 +48,10 @@ typedef struct { } l2cap_service_t; void l2cap_init(); -int l2cap_send_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, ...); +void l2cap_create_channel_internal(connection_t * connection, bd_addr_t address, uint16_t psm); +void l2cap_disconnect_internal(uint16_t source_cid, uint8_t reason); +void l2cap_send_internal(uint16_t source_cid, uint8_t *data, uint16_t len); +void l2cap_acl_handler( uint8_t *packet, uint16_t size ); +void l2cap_event_handler( uint8_t *packet, uint16_t size ); + + diff --git a/src/l2cap_signaling.c b/src/l2cap_signaling.c index 9d2fec22c..b6f88ccd1 100644 --- a/src/l2cap_signaling.c +++ b/src/l2cap_signaling.c @@ -22,7 +22,7 @@ static char *l2cap_signaling_commands_format[] = { "22D", // 0x0b information response: InfoType, Result, Data }; -uint8_t sig_seq_nr = 0xff; +uint8_t sig_seq_nr = 0xff; uint16_t source_cid = 0x40; uint8_t l2cap_next_sig_id(void){ @@ -38,7 +38,7 @@ uint16_t l2cap_next_source_cid(void){ return source_cid++; } -uint16_t l2cap_create_signaling_internal(uint8_t * acl_buffer,hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, va_list argptr){ +uint16_t l2cap_create_signaling_internal(uint8_t * acl_buffer, hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, va_list argptr){ // 0 - Connection handle : PB=10 : BC=00 bt_store_16(acl_buffer, 0, handle | (2 << 12) | (0 << 14)); diff --git a/src/l2cap_signaling.h b/src/l2cap_signaling.h index e66b391ae..f544f4f32 100644 --- a/src/l2cap_signaling.h +++ b/src/l2cap_signaling.h @@ -24,7 +24,6 @@ typedef enum { INFORMATIONAL_RESPONSE } L2CAP_SIGNALING_COMMANDS; -uint16_t l2cap_create_signaling_packet(uint8_t *acl_buffer, hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, ...); uint16_t l2cap_create_signaling_internal(uint8_t * acl_buffer,hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, va_list argptr); uint8_t l2cap_next_sig_id(); uint16_t l2cap_next_source_cid(); diff --git a/src/socket_connection.c b/src/socket_connection.c index cd6aaee30..eb7bbb1d0 100644 --- a/src/socket_connection.c +++ b/src/socket_connection.c @@ -26,14 +26,15 @@ /** prototypes */ static int socket_connection_hci_process(struct data_source *ds, int ready); -static int socket_connection_dummy_handler(connection_t *connection, uint8_t packet_type, uint8_t *data, uint16_t length); +static int socket_connection_dummy_handler(connection_t *connection, uint16_t packet_type, uint16_t channel, uint8_t *data, uint16_t length); /** globals */ /** packet header used over socket connections, in front of the HCI packet */ typedef struct packet_header { + uint16_t type; + uint16_t channel; uint16_t length; - uint8_t type; uint8_t data[0]; } packet_header_t; @@ -57,10 +58,9 @@ static linked_list_t connections = NULL; /** client packet handler */ -static int (*socket_connection_packet_callback)(connection_t *connection, uint8_t packet_type, uint8_t *data, uint16_t length) = socket_connection_dummy_handler; +static int (*socket_connection_packet_callback)(connection_t *connection, uint16_t packet_type, uint16_t channel, uint8_t *data, uint16_t length) = socket_connection_dummy_handler; - -static int socket_connection_dummy_handler(connection_t *connection, uint8_t packet_type, uint8_t *data, uint16_t length){ +static int socket_connection_dummy_handler(connection_t *connection, uint16_t packet_type, uint16_t channel, uint8_t *data, uint16_t length){ return 0; } @@ -134,12 +134,12 @@ int socket_connection_hci_process(struct data_source *ds, int ready) { switch (conn->state){ case SOCKET_W4_HEADER: conn->state = SOCKET_W4_DATA; - conn->bytes_to_read = READ_BT_16( conn->buffer, 0); + conn->bytes_to_read = READ_BT_16( conn->buffer, 4); break; case SOCKET_W4_DATA: - // dispatch packet !!! - (*socket_connection_packet_callback)(conn, conn->buffer[2], &conn->buffer[sizeof(packet_header_t)], - READ_BT_16( conn->buffer, 0)); + // dispatch packet !!! connection, type, channel, data, size + (*socket_connection_packet_callback)(conn, READ_BT_16( conn->buffer, 0), READ_BT_16( conn->buffer, 2), + &conn->buffer[sizeof(packet_header_t)], READ_BT_16( conn->buffer, 4)); // reset state machine socket_connection_init_statemachine(conn); break; @@ -225,49 +225,33 @@ int socket_connection_create_unix(char *path){ /** * set packet handler for all auto-accepted connections */ -void socket_connection_register_packet_callback( int (*packet_callback)(connection_t *connection, uint8_t packet_type, uint8_t *data, uint16_t length) ){ +void socket_connection_register_packet_callback( int (*packet_callback)(connection_t *connection, uint16_t packet_type, uint16_t channel, uint8_t *data, uint16_t length) ){ socket_connection_packet_callback = packet_callback; } /** * send HCI packet to single connection */ -void socket_connection_send_packet(connection_t *conn, uint8_t type, uint8_t *packet, uint16_t size){ - uint8_t length[2]; - bt_store_16( (uint8_t *) &length, 0, size); +void socket_connection_send_packet(connection_t *conn, uint16_t type, uint16_t channel, uint8_t *packet, uint16_t size){ - write(conn->ds.fd, &length, 2); - write(conn->ds.fd, &type, 1); - write(conn->ds.fd, &type, 1); // padding for now + uint8_t header[sizeof(packet_header_t)]; + bt_store_16(header, 0, type); + bt_store_16(header, 2, channel); + bt_store_16(header, 4, size); + write(conn->ds.fd, header, 6); write(conn->ds.fd, packet, size); } /** * send HCI packet to all connections */ -int socket_connection_send_packet_all(uint8_t type, uint8_t *packet, uint16_t size){ +void socket_connection_send_packet_all(uint16_t type, uint16_t channel, uint8_t *packet, uint16_t size){ linked_item_t *next; linked_item_t *it; for (it = (linked_item_t *) connections; it != NULL ; it = next){ next = it->next; // cache pointer to next connection_t to allow for removal - socket_connection_send_packet( (connection_t *) linked_item_get_user(it), type, packet, size); + socket_connection_send_packet( (connection_t *) linked_item_get_user(it), type, channel, packet, size); } - return 0; -} - -/** - * send HCI ACL packet to all connections - */ -void socket_connection_send_acl_all(uint8_t *packet, uint16_t size){ - socket_connection_send_packet_all( HCI_ACL_DATA_PACKET, packet, size); - return; -} -/** - * send HCI Event packet to all connections - */ -void socket_connection_send_event_all(uint8_t *packet, uint16_t size){ - socket_connection_send_packet_all( HCI_EVENT_PACKET, packet, size); - return; } /** diff --git a/src/socket_connection.h b/src/socket_connection.h index 3b96e5ee9..fc5592d40 100644 --- a/src/socket_connection.h +++ b/src/socket_connection.h @@ -39,15 +39,14 @@ int socket_connection_close_tcp(connection_t *connection); /** * set packet handler for all auto-accepted connections */ -void socket_connection_register_packet_callback( int (*packet_callback)(connection_t *connection, uint8_t packet_type, uint8_t *data, uint16_t length) ); +void socket_connection_register_packet_callback( int (*packet_callback)(connection_t *connection, uint16_t packet_type, uint16_t channel, uint8_t *data, uint16_t length) ); /** * send HCI packet to single connection */ -void socket_connection_send_packet(connection_t *connection, uint8_t packet_type, uint8_t *data, uint16_t size); +void socket_connection_send_packet(connection_t *connection, uint16_t packet_type, uint16_t channel, uint8_t *data, uint16_t size); /** - * send event/acl data to all clients + * send event data to all clients */ -void socket_connection_send_event_all(uint8_t *packet, uint16_t size); -void socket_connection_send_acl_all(uint8_t *packet, uint16_t size); +void socket_connection_send_packet_all(uint16_t type, uint16_t channel, uint8_t *packet, uint16_t size);