start with l2cap implementation

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);
+            // 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);
+            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){

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

src/btstack.h

@@ -8,9 +8,12 @@
  */
 
 #pragma once
+
 #include "hci.h"
 #include "l2cap.h"
 
+#include <stdint.h>
+
 // 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);

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 {

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 

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();
+

src/utils.c

@@ -45,3 +45,4 @@ void print_bd_addr( bd_addr_t addr){
     }
     printf("%02X", ((uint8_t *)addr)[i]);
 }
+

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)