xref: /btstack/src/l2cap.c (revision 6b296a27124c6286dd3386b510d67e34f4d7d1bb)
1 /*
2  * Copyright (C) 2009 by Matthias Ringwald
3  *
4  * Redistribution and use in source and binary forms, with or without
5  * modification, are permitted provided that the following conditions
6  * are met:
7  *
8  * 1. Redistributions of source code must retain the above copyright
9  *    notice, this list of conditions and the following disclaimer.
10  * 2. Redistributions in binary form must reproduce the above copyright
11  *    notice, this list of conditions and the following disclaimer in the
12  *    documentation and/or other materials provided with the distribution.
13  * 3. Neither the name of the copyright holders nor the names of
14  *    contributors may be used to endorse or promote products derived
15  *    from this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY MATTHIAS RINGWALD AND CONTRIBUTORS
18  * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
19  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
20  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS
21  * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
22  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
23  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
24  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
25  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
26  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
27  * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
28  * SUCH DAMAGE.
29  *
30  */
31 
32 /*
33  *  l2cap.c
34  *
35  *  Logical Link Control and Adaption Protocl (L2CAP)
36  *
37  *  Created by Matthias Ringwald on 5/16/09.
38  */
39 
40 #include "l2cap.h"
41 #include "hci.h"
42 #include "hci_dump.h"
43 
44 #include <stdarg.h>
45 #include <string.h>
46 
47 #include <stdio.h>
48 
49 // size of HCI ACL + L2CAP Header for regular data packets
50 #define COMPLETE_L2CAP_HEADER 8
51 
52 // minimum signaling MTU
53 #define L2CAP_MINIMAL_MTU 48
54 #define L2CAP_DEFAULT_MTU 672
55 
56 // offsets for L2CAP SIGNALING COMMANDS
57 #define L2CAP_SIGNALING_COMMAND_CODE_OFFSET   0
58 #define L2CAP_SIGNALING_COMMAND_SIGID_OFFSET  1
59 #define L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET 2
60 #define L2CAP_SIGNALING_COMMAND_DATA_OFFSET   4
61 
62 static void null_packet_handler(void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size);
63 static void l2cap_packet_handler(uint8_t packet_type, uint8_t *packet, uint16_t size);
64 
65 static uint8_t * sig_buffer = NULL;
66 static linked_list_t l2cap_channels = NULL;
67 static linked_list_t l2cap_services = NULL;
68 static uint8_t * acl_buffer = NULL;
69 static void (*packet_handler) (void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) = null_packet_handler;
70 
71 static uint8_t config_options[] = { 1, 2, 150, 0}; // mtu = 48
72 
73 void l2cap_init(){
74     sig_buffer = malloc( L2CAP_MINIMAL_MTU );
75     // acl_buffer = malloc( 255 + 8 );
76     acl_buffer = malloc( 400 + 8 );
77 
78     //
79     // register callback with HCI
80     //
81     hci_register_packet_handler(&l2cap_packet_handler);
82 }
83 
84 
85 /** Register L2CAP packet handlers */
86 static void null_packet_handler(void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
87 }
88 void l2cap_register_packet_handler(void (*handler)(void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size)){
89     packet_handler = handler;
90 }
91 
92 //  notify client/protocol handler
93 void l2cap_dispatch(l2cap_channel_t *channel, uint8_t type, uint8_t * data, uint16_t size){
94     if (channel->packet_handler) {
95         (* (channel->packet_handler))(type, channel->local_cid, data, size);
96     } else {
97         (*packet_handler)(channel->connection, type, channel->local_cid, data, size);
98     }
99 }
100 
101 void l2cap_emit_channel_opened(l2cap_channel_t *channel, uint8_t status) {
102     uint8_t event[17];
103     event[0] = L2CAP_EVENT_CHANNEL_OPENED;
104     event[1] = sizeof(event) - 2;
105     event[2] = status;
106     bt_flip_addr(&event[3], channel->address);
107     bt_store_16(event,  9, channel->handle);
108     bt_store_16(event, 11, channel->psm);
109     bt_store_16(event, 13, channel->local_cid);
110     bt_store_16(event, 15, channel->remote_cid);
111     hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
112     l2cap_dispatch(channel, HCI_EVENT_PACKET, event, sizeof(event));
113 }
114 
115 void l2cap_emit_channel_closed(l2cap_channel_t *channel) {
116     uint8_t event[4];
117     event[0] = L2CAP_EVENT_CHANNEL_CLOSED;
118     event[1] = sizeof(event) - 2;
119     bt_store_16(event, 2, channel->local_cid);
120     hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
121     l2cap_dispatch(channel, HCI_EVENT_PACKET, event, sizeof(event));
122 }
123 
124 void l2cap_emit_connection_request(l2cap_channel_t *channel) {
125     uint8_t event[16];
126     event[0] = L2CAP_EVENT_INCOMING_CONNECTION;
127     event[1] = sizeof(event) - 2;
128     bt_flip_addr(&event[2], channel->address);
129     bt_store_16(event,  8, channel->handle);
130     bt_store_16(event, 10, channel->psm);
131     bt_store_16(event, 12, channel->local_cid);
132     bt_store_16(event, 14, channel->remote_cid);
133     hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
134     l2cap_dispatch(channel, HCI_EVENT_PACKET, event, sizeof(event));
135 }
136 
137 l2cap_channel_t * l2cap_get_channel_for_local_cid(uint16_t local_cid){
138     linked_item_t *it;
139     l2cap_channel_t * channel;
140     for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){
141         channel = (l2cap_channel_t *) it;
142         if ( channel->local_cid == local_cid) {
143             return channel;
144         }
145     }
146     return NULL;
147 }
148 
149 uint16_t l2cap_get_remote_mtu_for_local_cid(uint16_t local_cid){
150     l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid);
151     if (channel) {
152         return channel->remote_mtu;
153     }
154     return 0;
155 }
156 
157 int l2cap_send_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, ...){
158     // printf("l2cap_send_signaling_packet type %u\n", cmd);
159     va_list argptr;
160     va_start(argptr, identifier);
161     uint16_t len = l2cap_create_signaling_internal(sig_buffer, handle, cmd, identifier, argptr);
162     va_end(argptr);
163     return hci_send_acl_packet(sig_buffer, len);
164 }
165 
166 void l2cap_send_internal(uint16_t local_cid, uint8_t *data, uint16_t len){
167     // find channel for local_cid, construct l2cap packet and send
168     l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid);
169     if (channel) {
170         // 0 - Connection handle : PB=10 : BC=00
171         bt_store_16(acl_buffer, 0, channel->handle | (2 << 12) | (0 << 14));
172         // 2 - ACL length
173         bt_store_16(acl_buffer, 2,  len + 4);
174         // 4 - L2CAP packet length
175         bt_store_16(acl_buffer, 4,  len + 0);
176         // 6 - L2CAP channel DEST
177         bt_store_16(acl_buffer, 6, channel->remote_cid);
178         // 8 - data
179         memcpy(&acl_buffer[8], data, len);
180         // send
181         hci_send_acl_packet(acl_buffer, len+8);
182     }
183 }
184 
185 // open outgoing L2CAP channel
186 void l2cap_create_channel_internal(void * connection, btstack_packet_handler_t packet_handler, bd_addr_t address, uint16_t psm){
187 
188     // alloc structure
189     l2cap_channel_t * chan = malloc(sizeof(l2cap_channel_t));
190     // TODO: emit error event
191     if (!chan) return;
192 
193     // fill in
194     BD_ADDR_COPY(chan->address, address);
195     chan->psm = psm;
196     chan->handle = 0;
197     chan->connection = connection;
198     chan->packet_handler = packet_handler;
199     chan->remote_mtu = L2CAP_MINIMAL_MTU;
200 
201     // set initial state
202     chan->state = L2CAP_STATE_CLOSED;
203     chan->sig_id = L2CAP_SIG_ID_INVALID;
204 
205     // add to connections list
206     linked_list_add(&l2cap_channels, (linked_item_t *) chan);
207 
208     // send connection request
209     // BD_ADDR, Packet_Type, Page_Scan_Repetition_Mode, Reserved, Clock_Offset, Allow_Role_Switch
210     hci_send_cmd(&hci_create_connection, address, 0x18, 0, 0, 0, 0);
211 }
212 
213 void l2cap_disconnect_internal(uint16_t local_cid, uint8_t reason){
214     // find channel for local_cid
215     l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid);
216     if (channel) {
217         channel->sig_id = l2cap_next_sig_id();
218         l2cap_send_signaling_packet( channel->handle, DISCONNECTION_REQUEST, channel->sig_id, channel->remote_cid, channel->local_cid);
219         channel->state = L2CAP_STATE_WAIT_DISCONNECT;
220     }
221 }
222 
223 static void l2cap_handle_connection_failed_for_addr(bd_addr_t address, uint8_t status){
224     linked_item_t *it;
225     for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){
226         l2cap_channel_t * channel = (l2cap_channel_t *) it;
227         if ( ! BD_ADDR_CMP( channel->address, address) ){
228             if (channel->state == L2CAP_STATE_CLOSED) {
229                 // failure, forward error code
230                 l2cap_emit_channel_opened(channel, status);
231                 // discard channel
232                 linked_list_remove(&l2cap_channels, (linked_item_t *) channel);
233                 free (channel);
234             }
235         }
236     }
237 }
238 
239 static void l2cap_handle_connection_success_for_addr(bd_addr_t address, hci_con_handle_t handle){
240     linked_item_t *it;
241     for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){
242         l2cap_channel_t * channel = (l2cap_channel_t *) it;
243         if ( ! BD_ADDR_CMP( channel->address, address) ){
244             if (channel->state == L2CAP_STATE_CLOSED) {
245                 // success, start l2cap handshake
246                 channel->handle = handle;
247                 channel->sig_id = l2cap_next_sig_id();
248                 channel->local_cid = l2cap_next_local_cid();
249                 channel->state = L2CAP_STATE_WAIT_CONNECT_RSP;
250                 l2cap_send_signaling_packet( channel->handle, CONNECTION_REQUEST, channel->sig_id, channel->psm, channel->local_cid);
251             }
252         }
253     }
254 }
255 
256 void l2cap_event_handler( uint8_t *packet, uint16_t size ){
257 
258     bd_addr_t address;
259     hci_con_handle_t handle;
260     linked_item_t *it;
261 
262     switch(packet[0]){
263 
264         // handle connection complete events
265         case HCI_EVENT_CONNECTION_COMPLETE:
266             bt_flip_addr(address, &packet[5]);
267             if (packet[2] == 0){
268                 handle = READ_BT_16(packet, 3);
269                 l2cap_handle_connection_success_for_addr(address, handle);
270             } else {
271                 l2cap_handle_connection_failed_for_addr(address, packet[2]);
272             }
273             break;
274 
275         // handle successful create connection cancel command
276         case HCI_EVENT_COMMAND_COMPLETE:
277             if ( COMMAND_COMPLETE_EVENT(packet, hci_create_connection_cancel) ) {
278                 if (packet[5] == 0){
279                     bt_flip_addr(address, &packet[6]);
280                     // CONNECTION TERMINATED BY LOCAL HOST (0X16)
281                     l2cap_handle_connection_failed_for_addr(address, 0x16);
282                 }
283             }
284             break;
285 
286         // handle disconnection complete events
287         case HCI_EVENT_DISCONNECTION_COMPLETE:
288             // send l2cap disconnect events for all channels on this handle
289             handle = READ_BT_16(packet, 3);
290             // only access next element to allows for removal
291             for (it = (linked_item_t *) &l2cap_channels; it->next ; it = it->next){
292                 l2cap_channel_t * channel = (l2cap_channel_t *) it->next;
293                 if ( channel->handle == handle ){
294                     // update prev item before free'ing next element
295                     it->next = it->next->next;
296                     l2cap_finialize_channel_close(channel);
297                 }
298             }
299             break;
300 
301         // HCI Connection Timeouts
302         case L2CAP_EVENT_TIMEOUT_CHECK:
303             handle = READ_BT_16(packet, 2);
304             l2cap_channel_t * channel;
305             int used = 0;
306             for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){
307                 channel = (l2cap_channel_t *) it;
308                 if (channel->handle == handle) {
309                     used = 1;
310                 }
311             }
312             if (!used) {
313                 hci_send_cmd(&hci_disconnect, handle, 0x13); // remote closed connection
314             }
315             break;
316 
317         default:
318             break;
319     }
320 
321     // pass on
322     (*packet_handler)(NULL, HCI_EVENT_PACKET, 0, packet, size);
323 }
324 
325 static void l2cap_handle_disconnect_request(l2cap_channel_t *channel, uint16_t identifier){
326     l2cap_send_signaling_packet( channel->handle, DISCONNECTION_RESPONSE, identifier, channel->local_cid, channel->remote_cid);
327     l2cap_finialize_channel_close(channel);
328 }
329 
330 static void l2cap_handle_connection_request(hci_con_handle_t handle, uint8_t sig_id, uint16_t psm, uint16_t source_cid){
331 
332     // printf("l2cap_handle_connection_request for handle %u, psm %u cid %u\n", handle, psm, source_cid);
333     l2cap_service_t *service = l2cap_get_service(psm);
334     if (!service) {
335         // 0x0002 PSM not supported
336         // printf("l2cap_handle_connection_request no PSM for psm %u/n", psm);
337         l2cap_send_signaling_packet(handle, CONNECTION_RESPONSE, sig_id, 0, 0, 0x0002, 0);
338         return;
339     }
340 
341     hci_connection_t * hci_connection = connection_for_handle( handle );
342     if (!hci_connection) {
343         fprintf(stderr, "no hci_connection for handle %u\n", handle);
344         // TODO: emit error
345         return;
346     }
347     // alloc structure
348     // printf("l2cap_handle_connection_request register channel\n");
349     l2cap_channel_t * channel = malloc(sizeof(l2cap_channel_t));
350     // TODO: emit error event
351     if (!channel) return;
352 
353     // fill in
354     BD_ADDR_COPY(channel->address, hci_connection->address);
355     channel->psm = psm;
356     channel->handle = handle;
357     channel->connection = service->connection;
358     channel->packet_handler = service->packet_handler;
359     channel->local_cid  = l2cap_next_local_cid();
360     channel->remote_cid = source_cid;
361     channel->remote_mtu = L2CAP_MINIMAL_MTU;
362 
363     // set initial state
364     channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT;
365 
366     // temp. store req sig id
367     channel->sig_id = sig_id;
368 
369     // add to connections list
370     linked_list_add(&l2cap_channels, (linked_item_t *) channel);
371 
372     // emit incoming connection request
373     l2cap_emit_connection_request(channel);
374 }
375 
376 void l2cap_accept_connection_internal(uint16_t local_cid){
377     l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid);
378     if (!channel) {
379         fprintf(stderr, "l2cap_accept_connection_internal called but local_cid 0x%x not found", local_cid);
380         return;
381     }
382 
383     // accept connection
384     l2cap_send_signaling_packet(channel->handle, CONNECTION_RESPONSE, channel->sig_id, channel->local_cid, channel->remote_cid, 0, 0);
385 
386     // set real sig and state and start config
387     channel->sig_id = l2cap_next_sig_id();
388     channel->state  = L2CAP_STATE_WAIT_CONFIG_REQ_RSP_OR_CONFIG_REQ;
389     l2cap_send_signaling_packet(channel->handle, CONFIGURE_REQUEST, channel->sig_id, channel->remote_cid, 0, 4, &config_options);
390 
391     // printf("new state %u\n", channel->state);
392 }
393 
394 void l2cap_decline_connection_internal(uint16_t local_cid, uint8_t reason){
395     l2cap_channel_t * channel = l2cap_get_channel_for_local_cid( local_cid);
396     if (!channel) {
397         fprintf(stderr, "l2cap_decline_connection_internal called but local_cid 0x%x not found", local_cid);
398         return;
399     }
400     l2cap_send_signaling_packet(channel->handle, CONNECTION_RESPONSE, channel->sig_id, 0, 0, reason, 0);
401 
402     // discard channel
403     linked_list_remove(&l2cap_channels, (linked_item_t *) channel);
404     free (channel);
405 }
406 
407 void l2cap_signaling_handle_configure_request(l2cap_channel_t *channel, uint8_t *command){
408     // accept the other's configuration options
409     uint16_t end_pos = 4 + READ_BT_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET);
410     uint16_t pos     = 8;
411     while (pos < end_pos){
412         uint8_t type   = command[pos++];
413         uint8_t length = command[pos++];
414         // MTU { type(8): 1, len(8):2, MTU(16) }
415         if ((type & 0x7f) == 1 && length == 2){
416             channel->remote_mtu = READ_BT_16(command, pos);
417             // printf("l2cap cid %u, remote mtu %u\n", channel->local_cid, channel->remote_mtu);
418         }
419         pos += length;
420     }
421     uint8_t identifier = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET];
422     // send back received options
423     // l2cap_send_signaling_packet(channel->handle, CONFIGURE_RESPONSE, identifier, channel->remote_cid, 0, 0, len-4, &command[8]);
424     // send back OK
425     l2cap_send_signaling_packet(channel->handle, CONFIGURE_RESPONSE, identifier, channel->remote_cid, 0, 0, 0, NULL);
426 }
427 
428 void l2cap_signaling_handler_channel(l2cap_channel_t *channel, uint8_t *command){
429 
430     uint8_t code       = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET];
431     uint8_t identifier = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET];
432     uint16_t result = 0;
433 
434     // printf("signaling handler code %u\n", code);
435 
436     switch (channel->state) {
437 
438         case L2CAP_STATE_WAIT_CONNECT_RSP:
439             switch (code){
440                 case CONNECTION_RESPONSE:
441                     result = READ_BT_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4);
442                     switch (result) {
443                         case 0:
444                             // successful connection
445                             channel->remote_cid = READ_BT_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET);
446                             channel->sig_id = l2cap_next_sig_id();
447                             l2cap_send_signaling_packet(channel->handle, CONFIGURE_REQUEST, channel->sig_id, channel->remote_cid, 0, 4, &config_options);
448                             channel->state = L2CAP_STATE_WAIT_CONFIG_REQ_RSP_OR_CONFIG_REQ;
449                             break;
450                         case 1:
451                             // connection pending. get some coffee
452                             break;
453                         default:
454                             // map l2cap connection response result to BTstack status enumeration
455                             l2cap_emit_channel_opened(channel, L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result);
456                             break;
457                     }
458                     break;
459 
460                 case DISCONNECTION_REQUEST:
461                     l2cap_handle_disconnect_request(channel, identifier);
462                     break;
463 
464                 default:
465                     //@TODO: implement other signaling packets
466                     break;
467             }
468             break;
469 
470         case L2CAP_STATE_WAIT_CONFIG_REQ_RSP_OR_CONFIG_REQ:
471             switch (code) {
472                 case CONFIGURE_RESPONSE:
473                     channel->state = L2CAP_STATE_WAIT_CONFIG_REQ;
474                     break;
475                 case CONFIGURE_REQUEST:
476                     l2cap_signaling_handle_configure_request(channel, command);
477                     channel->state = L2CAP_STATE_WAIT_CONFIG_REQ_RSP;
478                     break;
479                 case DISCONNECTION_REQUEST:
480                     l2cap_handle_disconnect_request(channel, identifier);
481                     break;
482                 default:
483                     //@TODO: implement other signaling packets
484                     break;
485             }
486             break;
487 
488         case L2CAP_STATE_WAIT_CONFIG_REQ:
489             switch (code) {
490                 case CONFIGURE_REQUEST:
491                     l2cap_signaling_handle_configure_request(channel, command);
492                     channel->state = L2CAP_STATE_OPEN;
493                     l2cap_emit_channel_opened(channel, 0);  // success
494                     break;
495                 case DISCONNECTION_REQUEST:
496                     l2cap_handle_disconnect_request(channel, identifier);
497                     break;
498                 default:
499                     //@TODO: implement other signaling packets
500                     break;
501             }
502             break;
503 
504         case L2CAP_STATE_WAIT_CONFIG_REQ_RSP:
505             switch (code) {
506                 case CONFIGURE_RESPONSE:
507                     channel->state = L2CAP_STATE_OPEN;
508                     l2cap_emit_channel_opened(channel, 0);  // success
509                     break;
510                 case DISCONNECTION_REQUEST:
511                     l2cap_handle_disconnect_request(channel, identifier);
512                     break;
513                 default:
514                     //@TODO: implement other signaling packets
515                     break;
516             }
517             break;
518 
519         case L2CAP_STATE_WAIT_DISCONNECT:
520             switch (code) {
521                 case DISCONNECTION_RESPONSE:
522                     l2cap_finialize_channel_close(channel);
523                     break;
524                 case DISCONNECTION_REQUEST:
525                     l2cap_handle_disconnect_request(channel, identifier);
526                     break;
527                 default:
528                     //@TODO: implement other signaling packets
529                     break;
530             }
531             break;
532 
533         case L2CAP_STATE_CLOSED:
534             // @TODO handle incoming requests
535             break;
536 
537         case L2CAP_STATE_OPEN:
538             switch (code) {
539                 case DISCONNECTION_REQUEST:
540                     l2cap_handle_disconnect_request(channel, identifier);
541                     break;
542                 default:
543                     //@TODO: implement other signaling packets, e.g. re-configure
544                     break;
545             }
546             break;
547         default:
548             break;
549     }
550     // printf("new state %u\n", channel->state);
551 }
552 
553 
554 void l2cap_signaling_handler_dispatch( hci_con_handle_t handle, uint8_t * command){
555 
556     // get code, signalind identifier and command len
557     uint8_t code   = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET];
558     uint8_t sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET];
559     uint16_t len   = READ_BT_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET);
560 
561     // not for a particular channel, and not CONNECTION_REQUEST, ECHO_[REQUEST|RESPONSE], INFORMATION_REQUEST
562     if (code < 1 || code == ECHO_RESPONSE || code > INFORMATION_REQUEST){
563         return;
564     }
565 
566     // general commands without an assigned channel
567     switch(code) {
568 
569         case CONNECTION_REQUEST: {
570             uint16_t psm =        READ_BT_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET);
571             uint16_t source_cid = READ_BT_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2);
572             l2cap_handle_connection_request(handle, sig_id, psm, source_cid);
573             break;
574         }
575 
576         case ECHO_REQUEST: {
577             // send back packet
578             l2cap_send_signaling_packet(handle, ECHO_RESPONSE, sig_id, len, &command[L2CAP_SIGNALING_COMMAND_DATA_OFFSET]);
579             break;
580         }
581 
582         case INFORMATION_REQUEST: {
583             // we neither support connectionless L2CAP data nor support any flow control modes yet
584             uint16_t infoType = READ_BT_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET);
585             if (infoType == 2) {
586                 uint32_t features = 0;
587                 // extended features request supported, however no features present
588                 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 0, 4, &features);
589             } else {
590                 // all other types are not supported
591                 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 1, 0, NULL);
592             }
593             break;;
594         }
595 
596         default:
597             break;
598     }
599 
600 
601     // Get potential destination CID
602     uint16_t dest_cid = READ_BT_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET);
603 
604     // Find channel for this sig_id and connection handle
605     linked_item_t *it;
606     for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){
607         l2cap_channel_t * channel = (l2cap_channel_t *) it;
608         if (channel->handle == handle) {
609             if (code & 1) {
610                 // match odd commands by previous signaling identifier
611                 if (channel->sig_id == sig_id) {
612                     l2cap_signaling_handler_channel(channel, command);
613                 }
614             } else {
615                 // match even commands by local channel id
616                 if (channel->local_cid == dest_cid) {
617                     l2cap_signaling_handler_channel(channel, command);
618                 }
619             }
620         }
621     }
622 }
623 
624 void l2cap_acl_handler( uint8_t *packet, uint16_t size ){
625 
626     // Get Channel ID
627     uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet);
628 
629     // Signaling Packet?
630     if (channel_id == 1) {
631 
632         // Get Connection
633         hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet);
634 
635         uint16_t command_offset = 8;
636         while (command_offset < size) {
637 
638             // handle signaling commands
639             l2cap_signaling_handler_dispatch(handle, &packet[command_offset]);
640 
641             // increment command_offset
642             command_offset += L2CAP_SIGNALING_COMMAND_DATA_OFFSET + READ_BT_16(packet, command_offset + L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET);
643         }
644         return;
645     }
646 
647     // Find channel for this channel_id and connection handle
648     l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(channel_id);
649     if (channel) {
650         l2cap_dispatch(channel, L2CAP_DATA_PACKET, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER);
651     }
652 }
653 
654 static void l2cap_packet_handler(uint8_t packet_type, uint8_t *packet, uint16_t size){
655     switch (packet_type) {
656         case HCI_EVENT_PACKET:
657             l2cap_event_handler(packet, size);
658             break;
659         case HCI_ACL_DATA_PACKET:
660             l2cap_acl_handler(packet, size);
661             break;
662         default:
663             break;
664     }
665 }
666 
667 // finalize closed channel
668 void l2cap_finialize_channel_close(l2cap_channel_t *channel){
669     channel->state = L2CAP_STATE_CLOSED;
670     l2cap_emit_channel_closed(channel);
671 
672     // discard channel
673     linked_list_remove(&l2cap_channels, (linked_item_t *) channel);
674     free (channel);
675 }
676 
677 l2cap_service_t * l2cap_get_service(uint16_t psm){
678     linked_item_t *it;
679 
680     // close open channels
681     for (it = (linked_item_t *) l2cap_services; it ; it = it->next){
682         l2cap_service_t * service = ((l2cap_service_t *) it);
683         if ( service->psm == psm){
684             return service;
685         };
686     }
687     return NULL;
688 }
689 
690 void l2cap_register_service_internal(void *connection, btstack_packet_handler_t packet_handler, uint16_t psm, uint16_t mtu){
691     // check for alread registered psm // TODO: emit error event
692     l2cap_service_t *service = l2cap_get_service(psm);
693     if (service) return;
694 
695     // alloc structure     // TODO: emit error event
696     service = malloc(sizeof(l2cap_service_t));
697     if (!service) return;
698 
699     // fill in
700     service->psm = psm;
701     service->mtu = mtu;
702     service->connection = connection;
703     service->packet_handler = packet_handler;
704 
705     // add to services list
706     linked_list_add(&l2cap_services, (linked_item_t *) service);
707 }
708 
709 void l2cap_unregister_service_internal(void *connection, uint16_t psm){
710     l2cap_service_t *service = l2cap_get_service(psm);
711     if (!service) return;
712     linked_list_remove(&l2cap_services, (linked_item_t *) service);
713     free(service);
714 }
715 
716 //
717 void l2cap_close_connection(void *connection){
718     linked_item_t *it;
719 
720     // close open channels
721     l2cap_channel_t * channel;
722     for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){
723         channel = (l2cap_channel_t *) it;
724         if (channel->connection == connection) {
725             channel->sig_id = l2cap_next_sig_id();
726             l2cap_send_signaling_packet( channel->handle, DISCONNECTION_REQUEST, channel->sig_id, channel->remote_cid, channel->local_cid);
727             channel->state = L2CAP_STATE_WAIT_DISCONNECT;
728         }
729     }
730 
731     // unregister services
732     it = (linked_item_t *) &l2cap_services;
733     while (it->next){
734         l2cap_service_t * service = (l2cap_service_t *) it->next;
735         if (service->connection == connection){
736             it->next = it->next->next;
737             free(service);
738         } else {
739             it = it->next;
740         }
741     }
742 }
743