implemented l2cap_disconnect command

TODO.txt

@@ -1,12 +1,6 @@
 /* new todo file for BTstack */
 
-UNTIL 13. August 2009 : Google Open Source Jam
-- Talk
-  - 1 Titel, 1 Motivation, 1 Slide Architecture, 1 Slide Status, 1 Demo
-  
 NEXT:
-- l2cap state machine
-  - disconnect channel
 - automatic L2CAP & higher disconnects
   - when client connection breaks, also close open L2CAP channels
 === Restart client app without restarting BTdaemon

example/test.c

@@ -16,9 +16,18 @@
 // bd_addr_t addr = {0x00, 0x03, 0xc9, 0x3d, 0x77, 0x43 };  // Think Outside Keyboard
 bd_addr_t addr = {0x00, 0x19, 0x1d, 0x90, 0x44, 0x68 };  // WiiMote
 
+uint16_t source_cid_interrupt;
+uint16_t source_cid_control;
+
 void data_handler(uint8_t *packet, uint16_t size){
 	// just dump data for now
 	hexdump( packet, size );
+	
+	// HOME => disconnect L2CAP
+	if (packet[11] == 0x080){
+		printf("Closing interrupt channel\n");
+		bt_send_cmd(&l2cap_disconnect, source_cid_interrupt);
+	}
 }
 
 void event_handler(uint8_t *packet, uint16_t size){
@@ -56,12 +65,28 @@ void event_handler(uint8_t *packet, uint16_t size){
 			   READ_BT_16(packet, 8), psm, source_cid,  READ_BT_16(packet, 14));
 
 		if (psm == 0x13) {
+			source_cid_interrupt = source_cid;
 			// interupt channel openedn succesfully, now open control channel, too.
 			bt_send_cmd(&l2cap_create_channel, addr, 0x11);
 		} else {
-			// request acceleration data.. probably has to be sent to control channel 0x11 instead of 0x13
+			source_cid_control = source_cid;
+			// request acceleration data..
 			uint8_t setMode31[] = { 0x52, 0x12, 0x00, 0x31 };
-			l2cap_send( source_cid, setMode31, sizeof(setMode31));
+			// l2cap_send( source_cid, setMode31, sizeof(setMode31));
+		}
+	}
+	
+	if (packet[0] == HCI_EVENT_L2CAP_CHANNEL_CLOSED) {
+		uint16_t source_cid = READ_BT_16(packet, 2); 
+		if (source_cid == source_cid_interrupt){
+			printf("Interrupt channel closed, closing control channel\n");
+			bt_send_cmd(&l2cap_disconnect, source_cid_control);
+		}
+		if (source_cid == source_cid_control){
+			printf("Control channel closed, closing baseband connection\n");
+			printf("To be implemented..\n");
+			exit(0);
+			// bt_send_cmd(&l2cap_disconnect, source_cid_control);
 		}
 	}
 }

src/daemon.c

@@ -111,8 +111,8 @@ int main (int argc, const char * argv[]){
     // @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);

src/hci_cmds.h

@@ -80,8 +80,10 @@
 #define HCI_EVENT_BTSTACK_WORKING                          0x80
 #define HCI_EVENT_BTSTACK_STATE                            0x81
 
-// data: event (8), len(8), handle(16), channel(16) 
+// data: event (8), len(8), address(48), handle (16), psm (16), source_cid(16), dest_cid (16) 
 #define HCI_EVENT_L2CAP_CHANNEL_OPENED                     0x82
+// data: event (8), len(8), channel (16)
+#define HCI_EVENT_L2CAP_CHANNEL_CLOSED                     0x83
 
 #define COMMAND_COMPLETE_EVENT(event,cmd) ( event[0] == HCI_EVENT_COMMAND_COMPLETE && READ_BT_16(event,3) == cmd.opcode)
 

src/l2cap.c

@@ -46,7 +46,6 @@ void l2cap_register_data_packet_handler  (void (*handler)(uint16_t source_cid, u
     data_packet_handler = handler;
 }
 
-
 int l2cap_send_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, ...){
     va_list argptr;
     va_start(argptr, identifier);
@@ -55,6 +54,18 @@ int l2cap_send_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMAND
     return hci_send_acl_packet(sig_buffer, len);
 }
 
+l2cap_channel_t * l2cap_get_channel_for_source_cid(uint16_t source_cid){
+    linked_item_t *it;
+    l2cap_channel_t * channel;
+    for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){
+        channel = (l2cap_channel_t *) it;
+        if ( channel->source_cid == source_cid) {
+            return channel;
+        }
+    }
+    return NULL;
+}
+            
 // open outgoing L2CAP channel
 void l2cap_create_channel_internal(connection_t * connection, bd_addr_t address, uint16_t psm){
     
@@ -82,7 +93,13 @@ void l2cap_create_channel_internal(connection_t * connection, bd_addr_t address,
 }
 
 void l2cap_disconnect_internal(uint16_t source_cid, uint8_t reason){
-    // TODO: implement
+    // find channel for source_cid
+    l2cap_channel_t * channel = l2cap_get_channel_for_source_cid(source_cid);
+    if (channel) {
+        channel->sig_id = l2cap_next_sig_id();
+        l2cap_send_signaling_packet( channel->handle, DISCONNECTION_REQUEST, channel->sig_id, channel->dest_cid, channel->source_cid);   
+        channel->state = L2CAP_STATE_WAIT_DISCONNECT;
+    }
 }
 
 
@@ -164,6 +181,19 @@ void l2cap_signaling_handler(l2cap_channel_t *channel, uint8_t *packet, uint16_t
                     break;
             }
             break;
+            
+        case L2CAP_STATE_WAIT_DISCONNECT:
+            switch (code) {
+                case DISCONNECTION_RESPONSE:
+                    channel->state = L2CAP_STATE_CLOSED;
+                    l2cap_emit_channel_closed(channel);
+                    
+                    // discard channel
+                    linked_list_remove(&l2cap_channels, (linked_item_t *) channel);
+                    free (channel);
+                    break;
+            }
+            break;
     }
 }
 
@@ -180,6 +210,14 @@ void l2cap_emit_channel_opened(l2cap_channel_t *channel) {
     socket_connection_send_packet(channel->connection, HCI_EVENT_PACKET, 0, event, sizeof(event));
 }
 
+void l2cap_emit_channel_closed(l2cap_channel_t *channel) {
+    uint8_t event[4];
+    event[0] = HCI_EVENT_L2CAP_CHANNEL_CLOSED;
+    event[1] = sizeof(event) - 2;
+    bt_store_16(event, 2, channel->source_cid);
+    socket_connection_send_packet(channel->connection, HCI_EVENT_PACKET, 0, event, sizeof(event));
+}
+
 void l2cap_acl_handler( uint8_t *packet, uint16_t size ){
     
     // Get Channel ID and command code 
@@ -223,30 +261,19 @@ void l2cap_acl_handler( uint8_t *packet, uint16_t size ){
     }
     
     // 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);
-        }
+    l2cap_channel_t * channel = l2cap_get_channel_for_source_cid(channel_id);
+    if (channel) {
+        socket_connection_send_packet(channel->connection, HCI_ACL_DATA_PACKET, 0, packet, size);
     }
 
      // forward to higher layers
     (*data_packet_handler)(channel_id, 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;
-     l2cap_channel_t * channel = NULL;
-    for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){
-        if ( ((l2cap_channel_t *) it)->source_cid == source_cid) {
-            channel = (l2cap_channel_t *) it;
-            break;
-        }
-    }
-     
+    l2cap_channel_t * channel = l2cap_get_channel_for_source_cid(source_cid);
     if (channel) {
          // 0 - Connection handle : PB=10 : BC=00 
          bt_store_16(acl_buffer, 0, channel->handle | (2 << 12) | (0 << 14));

src/l2cap.h

@@ -56,3 +56,4 @@ void l2cap_event_handler( uint8_t *packet, uint16_t size );
 
 
 void l2cap_emit_channel_opened(l2cap_channel_t *channel);
+void l2cap_emit_channel_closed(l2cap_channel_t *channel);