diff --git a/example/test.c b/example/test.c index e37241a51..832880c39 100644 --- a/example/test.c +++ b/example/test.c @@ -19,6 +19,7 @@ hci_con_handle_t con_handle_out = 0; hci_con_handle_t con_handle_in = 0; uint16_t dest_cid; +uint16_t local_cid; #define BT_HID // #define POWER_CYCLE_TEST @@ -115,7 +116,8 @@ if (packet[0] == HCI_EVENT_CONNECTION_COMPLETE){ #ifndef MITM // request l2cap connection printf("> CONNECTION REQUEST\n"); - bt_send_l2cap_signaling_packet(con_handle_out, CONNECTION_REQUEST, sig_seq_nr++, 0x13, local_cid); + local_cid = l2cap_next_local_cid(); + bt_send_l2cap_signaling_packet(con_handle_out, CONNECTION_REQUEST, l2cap_next_sig_id(), 0x13, local_cid); #else printf("Connected to target device, please start!\n"); #endif @@ -174,8 +176,8 @@ void acl_handler(uint8_t *packet, int size){ uint16_t status = READ_BT_16(packet, 18); printf("< CONNECTION_RESPONSE: id %u, dest cid %u, src cid %u, result %u, status %u\n", packet[9], dest_cid, source_cid, result, status); if (result == 0){ - printf("> CONFIGURE_REQUEST: id %u\n", sig_seq_nr); - bt_send_l2cap_signaling_packet(con_handle_out, CONFIGURE_REQUEST, sig_seq_nr++, dest_cid, 0, 4, &config_options); + // printf("> CONFIGURE_REQUEST: id %u\n", sig_seq_nr); + bt_send_l2cap_signaling_packet(con_handle_out, CONFIGURE_REQUEST, l2cap_next_sig_id(), dest_cid, 0, 4, &config_options); } } else if (packet[8] == CONFIGURE_RESPONSE){ diff --git a/src/btstack.c b/src/btstack.c index c75a3a120..f3c25cd1a 100644 --- a/src/btstack.c +++ b/src/btstack.c @@ -23,6 +23,8 @@ 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, int size); int btstack_packet_handler(connection_t *connection, uint8_t packet_type, uint8_t *data, uint16_t size); @@ -31,12 +33,14 @@ int btstack_packet_handler(connection_t *connection, uint8_t packet_type, uint8_ static void (*event_packet_handler)(uint8_t *packet, int size) = dummy_handler; static void (*acl_packet_handler) (uint8_t *packet, int size) = dummy_handler; - // init BTstack library int bt_open(){ + + // BTdaemon socket_connection_register_packet_callback(btstack_packet_handler); btstack_connection = socket_connection_open_tcp(); if (!btstack_connection) return -1; + return 0; } @@ -45,12 +49,52 @@ 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){ + linked_item_t *it; + bd_addr_t address; + bt_flip_addr(address, &packet[5]); + 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->state = L2CAP_STATE_CONNECTED; + chan->handle = READ_BT_16(packet, 3); + chan->sig_id = l2cap_next_sig_id(); + chan->local_cid = l2cap_next_local_cid(); + bt_send_l2cap_signaling_packet( chan->handle, CONNECTION_REQUEST, chan->sig_id, chan->psm, chan->local_cid); + } + } + } + } + // handle disconnection complete events + // TODO ... +} +void l2cap_data_handler( uint8_t *packet, uint16_t size ){ + // Get Channel ID + + // Signaling Packet? + + // Get Signaling Identifier + + // Find channel for this sig_id + + // Handle CONNECTION_RESPONSE + + // Handle CONFIGURE_RESPONSE + + // Handle CONFIGURE_REQUEST +} + 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: @@ -96,3 +140,35 @@ void bt_register_acl_packet_handler (void (*handler)(uint8_t *packet, int 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_disconnect(l2cap_channel_t *channel, uint8_t reason){ + // TODO implement +} + diff --git a/src/btstack.h b/src/btstack.h index 84937dc17..3c1153aa2 100644 --- a/src/btstack.h +++ b/src/btstack.h @@ -8,9 +8,12 @@ */ #pragma once + #include "hci.h" #include "l2cap.h" +#include + // init BTstack library int bt_open(); @@ -23,10 +26,14 @@ int bt_send_cmd(hci_cmd_t *cmd, ...); // send hci acl packet int bt_send_acl_packet(uint8_t *packet, int size); -// TODO: temp -int bt_send_l2cap_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, ...); - // register packet and event handler void bt_register_event_packet_handler(void (*handler)(uint8_t *packet, int size)); void bt_register_acl_packet_handler (void (*handler)(uint8_t *packet, int 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(l2cap_channel_t *channel, uint8_t reason); diff --git a/src/l2cap.h b/src/l2cap.h index 4f5592412..8d84a219a 100644 --- a/src/l2cap.h +++ b/src/l2cap.h @@ -10,9 +10,34 @@ #include "hci.h" #include "l2cap_signaling.h" +#include "utils.h" + +#define L2CAP_SIG_ID_INVALID 0 + +typedef enum { + L2CAP_STATE_CLOSED, // no baseband + L2CAP_STATE_CONNECTED, // baseband connection + L2CAP_STATE_W4_L2CA_CONNECTION_RSP, // from application + L2CAP_STATE_W4_L2CAP_CONNECTION_RSP, // from peer + L2CAP_STATE_CONFIG, + L2CAP_STATE_OPEN, + L2CAP_STATE_W4_L2CA_DISCON_RSP, // from application + L2CAP_STATE_W4_L2CAP_DISCON_RSP, // from peer +} L2CAP_STATE; typedef struct { - + L2CAP_STATE state; + uint8_t sig_id; + uint16_t local_cid; + 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); + // uint16_t mtu_incoming; + // uint16_t mtu_outgoing; + // uint16_t flush_timeout_incoming; + // uint16_t flush_timeout_outgoing; } l2cap_channel_t; typedef struct { diff --git a/src/l2cap_signaling.c b/src/l2cap_signaling.c index b222d8ca0..a968a223b 100644 --- a/src/l2cap_signaling.c +++ b/src/l2cap_signaling.c @@ -22,9 +22,22 @@ static char *l2cap_signaling_commands_format[] = { "22D", // 0x0b information response: InfoType, Result, Data }; -uint8_t sig_seq_nr = 1; +uint8_t sig_seq_nr = 0xff; uint16_t local_cid = 0x40; +uint8_t l2cap_next_sig_id(void){ + if (sig_seq_nr == 0xff) { + sig_seq_nr = 1; + } else { + sig_seq_nr++; + } + return sig_seq_nr; +} + +uint16_t l2cap_next_local_cid(void){ + return local_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){ // 0 - Connection handle : PB=10 : BC=00 diff --git a/src/l2cap_signaling.h b/src/l2cap_signaling.h index 6703a816d..cf60ada8c 100644 --- a/src/l2cap_signaling.h +++ b/src/l2cap_signaling.h @@ -24,8 +24,8 @@ typedef enum { INFORMATIONAL_RESPONSE } L2CAP_SIGNALING_COMMANDS; -extern uint16_t local_cid; -extern uint8_t sig_seq_nr; - 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_local_cid(); + diff --git a/src/utils.c b/src/utils.c index 9ef02d6aa..03d78d61d 100644 --- a/src/utils.c +++ b/src/utils.c @@ -45,3 +45,4 @@ void print_bd_addr( bd_addr_t addr){ } printf("%02X", ((uint8_t *)addr)[i]); } + diff --git a/src/utils.h b/src/utils.h index 3bc5d3bdd..81df2a703 100644 --- a/src/utils.h +++ b/src/utils.h @@ -40,4 +40,4 @@ void hexdump(void *data, int size); void print_bd_addr( bd_addr_t addr); #define BD_ADDR_CMP(a,b) memcmp(a,b, BD_ADDR_LEN) -#define BD_ADDR_COPY(dest,src) memcpy(dest,src,BD_ADDR_LEN) +#define BD_ADDR_COPY(dest,src) memcpy(dest,src,BD_ADDR_LEN) \ No newline at end of file