l2cap incoming working

TODO.txt

@@ -3,16 +3,6 @@
 2009-11-08: Release 0.1
 
 NEXT:
-- implement rest of L2CAP state machine
-  - incoming connections
-    - manage list of supported PSM
-	- new commands
-	  - l2cap_register_service(psm, mtu)
-	  - l2cap_unregister_service(psm)
-	  - l2cap_accept_connection_internal( bd_addr_t, dest cid )
-	  - l2cap_deny_connection( bd_addr_t, dest cid, reason )
-	- events
-	  - l2cap_incoming_connection(psm, handle, dest cid)
 - Add improved RFCOMM user-client
   - use switch on RFCOMM control field in rfcomm.c packet handler
 - add configure option for uart flowcontrol

example/l2cap-server.c

@@ -78,14 +78,19 @@ void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint
 					break;
 					
 				case L2CAP_EVENT_INCOMING_CONNECTION:
-					// data: event(8), len(8), address(48), handle (16),  psm (16), dest cid(16)
+					// data: event(8), len(8), address(48), handle (16),  psm (16), source cid(16) dest cid(16)
 					bt_flip_addr(event_addr, &packet[2]);
-					handle   = READ_BT_16(packet, 8); 
+					handle     = READ_BT_16(packet, 8); 
-					psm      = READ_BT_16(packet, 10); 
+					psm        = READ_BT_16(packet, 10); 
-					dest_cid = READ_BT_16(packet, 13); 
+					source_cid = READ_BT_16(packet, 12); 
+					dest_cid   = READ_BT_16(packet, 14); 
 					printf("L2CAP_EVENT_INCOMING_CONNECTION ");
 					print_bd_addr(event_addr);
-					printf(", handle 0x%02x, psm 0x%02x, dest cid 0x%02x\n", handle, psm, dest_cid);
+					printf(", handle 0x%02x, psm 0x%02x, src cid 0x%02x, dest cid 0x%02x\n",
+						   handle, psm, source_cid, dest_cid);
+
+					// accept
+					bt_send_cmd(&l2cap_accept_connection, source_cid);
 					break;
 					
 				case L2CAP_EVENT_CHANNEL_OPENED:

include/btstack/hci_cmds.h

@@ -115,7 +115,7 @@
 // data: event (8), len(8), channel (16)
 #define L2CAP_EVENT_CHANNEL_CLOSED                         0x71
 
-// data: event(8), len(8), address(48), handle (16),  psm (16), dest cid(16)
+// data: event(8), len(8), address(48), handle (16),  psm (16), source_cid(16), dest cid(16)
 #define L2CAP_EVENT_INCOMING_CONNECTION					   0x72
 
 // data: event(8), len(8), handle(16)

src/daemon.c

@@ -153,15 +153,13 @@ static int btstack_command_handler(connection_t *connection, uint8_t *packet, ui
             l2cap_unregister_service_internal(connection, psm);
             break;
         case L2CAP_ACCEPT_CONNECTION:
-            handle = READ_BT_16(packet, 3);
+            cid    = READ_BT_16(packet, 3);
-            cid    = READ_BT_16(packet, 5);
+            l2cap_accept_connection_internal(cid);
-            l2cap_accept_connection_internal(handle, cid);
             break;
         case L2CAP_DECLINE_CONNECTION:
-            handle = READ_BT_16(packet, 3);
+            cid    = READ_BT_16(packet, 3);
-            cid    = READ_BT_16(packet, 5);
             reason = packet[7];
-            l2cap_decline_connection_internal(handle, cid, reason);
+            l2cap_decline_connection_internal(cid, reason);
             break;
         default:
             //@TODO: log into hci dump as vendor specific "event"

src/hci_cmds.c

@@ -320,12 +320,12 @@ OPCODE(OGF_BTSTACK, L2CAP_UNREGISTER_SERVICE), "2"
 // @param psm (16)
 };
 hci_cmd_t l2cap_accept_connection = {
-OPCODE(OGF_BTSTACK, L2CAP_ACCEPT_CONNECTION), "22"
+OPCODE(OGF_BTSTACK, L2CAP_ACCEPT_CONNECTION), "2"
-// @param handle(16), dest cid (16)
+// @param source cid (16)
 };
 hci_cmd_t l2cap_decline_connection = {
-OPCODE(OGF_BTSTACK, L2CAP_DECLINE_CONNECTION), "221"
+OPCODE(OGF_BTSTACK, L2CAP_DECLINE_CONNECTION), "21"
-// @param handle(16), dest cid (16), reason(8)
+// @param source cid (16), reason(8)
 };
 
 

src/l2cap.c

@@ -265,7 +265,6 @@ static void l2cap_handle_connection_request(hci_con_handle_t handle, uint8_t sig
         // TODO: emit error
         return;
     }
-    
     // alloc structure
     // printf("l2cap_handle_connection_request register channel\n");
     l2cap_channel_t * channel = malloc(sizeof(l2cap_channel_t));
@@ -279,19 +278,47 @@ static void l2cap_handle_connection_request(hci_con_handle_t handle, uint8_t sig
     channel->connection = service->connection;
     channel->source_cid = l2cap_next_source_cid();
     channel->dest_cid   = dest_cid;
-    
+        
     // set initial state
-    channel->state = L2CAP_STATE_WAIT_CONFIG_REQ_RSP_OR_CONFIG_REQ;
+    channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT;
-    channel->sig_id = l2cap_next_sig_id();
+    
+    // temp. store req sig id
+    channel->sig_id = sig_id; 
     
     // add to connections list
     linked_list_add(&l2cap_channels, (linked_item_t *) channel);
-
-    // TODO: emit incoming connection request instead of answering directly
-
-    l2cap_send_signaling_packet(handle, CONNECTION_RESPONSE, sig_id, dest_cid, channel->source_cid, 0, 0);
-    l2cap_send_signaling_packet(channel->handle, CONFIGURE_REQUEST, channel->sig_id, channel->dest_cid, 0, 4, &config_options);
     
+    // emit incoming connection request
+    l2cap_emit_connection_request(channel);
+}
+
+void l2cap_accept_connection_internal(uint16_t source_cid){
+    l2cap_channel_t * channel = l2cap_get_channel_for_source_cid(source_cid);
+    if (!channel) {
+        fprintf(stderr, "l2cap_accept_connection_internal called but source_cid 0x%x not found", source_cid);
+        return;
+    }
+
+    // accept connection
+    l2cap_send_signaling_packet(channel->handle, CONNECTION_RESPONSE, channel->sig_id, channel->dest_cid, channel->source_cid, 0, 0);
+
+    // set real sig and state and start config
+    channel->sig_id = l2cap_next_sig_id();
+    channel->state  = L2CAP_STATE_WAIT_CONFIG_REQ_RSP_OR_CONFIG_REQ;
+    l2cap_send_signaling_packet(channel->handle, CONFIGURE_REQUEST, channel->sig_id, channel->dest_cid, 0, 4, &config_options);
+}
+
+void l2cap_decline_connection_internal(uint16_t source_cid, uint8_t reason){
+    l2cap_channel_t * channel = l2cap_get_channel_for_source_cid( source_cid);
+    if (!channel) {
+        fprintf(stderr, "l2cap_decline_connection_internal called but source_cid 0x%x not found", source_cid,reason);
+        return;
+    }
+    l2cap_send_signaling_packet(channel->handle, CONNECTION_RESPONSE, channel->sig_id, 0, 0, reason, 0);
+
+    // discard channel
+    linked_list_remove(&l2cap_channels, (linked_item_t *) channel);
+    free (channel);
 }
 
 void l2cap_signaling_handler(l2cap_channel_t *channel, uint8_t *packet, uint16_t size){
@@ -490,14 +517,6 @@ void l2cap_close_connection(connection_t *connection){
     }
 }
 
-void l2cap_accept_connection_internal(hci_con_handle_t handle,  uint16_t dest_cid){
-    printf("l2cap_accept_connection_internal called but not implemented yet 0x%x, 0x%x\n", handle, dest_cid);
-}
-
-void l2cap_decline_connection_internal(hci_con_handle_t handle, uint16_t dest_cid, uint8_t reason){
-    printf("l2cap_decline_connection_internal called but not implemented yet 0x%x, 0x%x, %u", handle, dest_cid,reason);
-}
-
 //  notify client
 void l2cap_emit_channel_opened(l2cap_channel_t *channel, uint8_t status) {
     uint8_t event[17];
@@ -520,6 +539,17 @@ void l2cap_emit_channel_closed(l2cap_channel_t *channel) {
     socket_connection_send_packet(channel->connection, HCI_EVENT_PACKET, 0, event, sizeof(event));
 }
 
+void l2cap_emit_connection_request(l2cap_channel_t *channel) {
+    uint8_t event[16];
+    event[0] = L2CAP_EVENT_INCOMING_CONNECTION;
+    event[1] = sizeof(event) - 2;
+    bt_flip_addr(&event[2], channel->address);
+    bt_store_16(event,  8, channel->handle);
+    bt_store_16(event, 10, channel->psm);
+    bt_store_16(event, 12, channel->source_cid);
+    bt_store_16(event, 14, channel->dest_cid);
+    socket_connection_send_packet(channel->connection, HCI_EVENT_PACKET, 0, event, sizeof(event));
+}
 void l2cap_acl_handler( uint8_t *packet, uint16_t size ){
     
     // Capturing?

src/l2cap.h

@@ -48,6 +48,7 @@
 
 typedef enum {
     L2CAP_STATE_CLOSED,           // no baseband
+    L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT,
     L2CAP_STATE_WAIT_CONNECT_RSP, // from peer
     L2CAP_STATE_WAIT_CONFIG_REQ_RSP_OR_CONFIG_REQ,
     L2CAP_STATE_WAIT_CONFIG_REQ_RSP,
@@ -62,7 +63,7 @@ typedef struct {
     linked_item_t    item;
 
     L2CAP_STATE state;
-    uint8_t   sig_id;
+    uint8_t   sig_id;  // other sig for conn requests
     uint16_t  source_cid;
     uint16_t  dest_cid;
     bd_addr_t address;
@@ -107,8 +108,9 @@ l2cap_service_t * l2cap_get_service(uint16_t psm);
 void l2cap_register_service_internal(connection_t *connection, uint16_t psm, uint16_t);
 void l2cap_unregister_service_internal(connection_t *connection, uint16_t psm);
 
-void l2cap_accept_connection_internal(hci_con_handle_t handle,  uint16_t dest_cid);
+void l2cap_accept_connection_internal(uint16_t source_cid);
-void l2cap_decline_connection_internal(hci_con_handle_t handle, uint16_t dest_cid, uint8_t reason);
+void l2cap_decline_connection_internal(uint16_t source_cid, uint8_t reason);
 
 void l2cap_emit_channel_opened(l2cap_channel_t *channel, uint8_t status);
 void l2cap_emit_channel_closed(l2cap_channel_t *channel);
+void l2cap_emit_connection_request(l2cap_channel_t *channel);