xref: /btstack/src/l2cap.c (revision 9a011532c26583cba816da62e35d8ad3ecbd8440)
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 #include "debug.h"
44 
45 #include <stdarg.h>
46 #include <string.h>
47 
48 #include <stdio.h>
49 
50 // size of HCI ACL + L2CAP Header for regular data packets
51 #define COMPLETE_L2CAP_HEADER 8
52 
53 // minimum signaling MTU
54 #define L2CAP_MINIMAL_MTU 48
55 #define L2CAP_DEFAULT_MTU 672
56 
57 // nr of buffered acl packets in outgoing queue to get max performance
58 #define NR_BUFFERED_ACL_PACKETS 3
59 
60 // offsets for L2CAP SIGNALING COMMANDS
61 #define L2CAP_SIGNALING_COMMAND_CODE_OFFSET   0
62 #define L2CAP_SIGNALING_COMMAND_SIGID_OFFSET  1
63 #define L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET 2
64 #define L2CAP_SIGNALING_COMMAND_DATA_OFFSET   4
65 
66 static void null_packet_handler(void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size);
67 static void l2cap_packet_handler(uint8_t packet_type, uint8_t *packet, uint16_t size);
68 
69 static uint8_t * sig_buffer = NULL;
70 static linked_list_t l2cap_channels = NULL;
71 static linked_list_t l2cap_services = NULL;
72 static uint8_t * acl_buffer = NULL;
73 static void (*packet_handler) (void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) = null_packet_handler;
74 static int new_credits_blocked = 0;
75 
76 void l2cap_init(){
77     sig_buffer = malloc( L2CAP_MINIMAL_MTU );
78     acl_buffer = malloc( HCI_ACL_3DH5_SIZE);
79 
80     new_credits_blocked = 0;
81 
82     //
83     // register callback with HCI
84     //
85     hci_register_packet_handler(&l2cap_packet_handler);
86 }
87 
88 
89 /** Register L2CAP packet handlers */
90 static void null_packet_handler(void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
91 }
92 void l2cap_register_packet_handler(void (*handler)(void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size)){
93     packet_handler = handler;
94 }
95 
96 //  notify client/protocol handler
97 void l2cap_dispatch(l2cap_channel_t *channel, uint8_t type, uint8_t * data, uint16_t size){
98     if (channel->packet_handler) {
99         (* (channel->packet_handler))(type, channel->local_cid, data, size);
100     } else {
101         (*packet_handler)(channel->connection, type, channel->local_cid, data, size);
102     }
103 }
104 
105 void l2cap_emit_channel_opened(l2cap_channel_t *channel, uint8_t status) {
106     uint8_t event[21];
107     event[0] = L2CAP_EVENT_CHANNEL_OPENED;
108     event[1] = sizeof(event) - 2;
109     event[2] = status;
110     bt_flip_addr(&event[3], channel->address);
111     bt_store_16(event,  9, channel->handle);
112     bt_store_16(event, 11, channel->psm);
113     bt_store_16(event, 13, channel->local_cid);
114     bt_store_16(event, 15, channel->remote_cid);
115     bt_store_16(event, 17, channel->local_mtu);
116     bt_store_16(event, 19, channel->remote_mtu);
117     hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
118     l2cap_dispatch(channel, HCI_EVENT_PACKET, event, sizeof(event));
119 }
120 
121 void l2cap_emit_channel_closed(l2cap_channel_t *channel) {
122     uint8_t event[4];
123     event[0] = L2CAP_EVENT_CHANNEL_CLOSED;
124     event[1] = sizeof(event) - 2;
125     bt_store_16(event, 2, channel->local_cid);
126     hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
127     l2cap_dispatch(channel, HCI_EVENT_PACKET, event, sizeof(event));
128 }
129 
130 void l2cap_emit_connection_request(l2cap_channel_t *channel) {
131     uint8_t event[16];
132     event[0] = L2CAP_EVENT_INCOMING_CONNECTION;
133     event[1] = sizeof(event) - 2;
134     bt_flip_addr(&event[2], channel->address);
135     bt_store_16(event,  8, channel->handle);
136     bt_store_16(event, 10, channel->psm);
137     bt_store_16(event, 12, channel->local_cid);
138     bt_store_16(event, 14, channel->remote_cid);
139     hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
140     l2cap_dispatch(channel, HCI_EVENT_PACKET, event, sizeof(event));
141 }
142 
143 void l2cap_emit_credits(l2cap_channel_t *channel, uint8_t credits) {
144     // track credits
145     channel->packets_granted += credits;
146     // log_dbg("l2cap_emit_credits for cid %u, credits given: %u (+%u)\n", channel->local_cid, channel->packets_granted, credits);
147 
148     uint8_t event[5];
149     event[0] = L2CAP_EVENT_CREDITS;
150     event[1] = sizeof(event) - 2;
151     bt_store_16(event, 2, channel->local_cid);
152     event[4] = credits;
153     hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
154     l2cap_dispatch(channel, HCI_EVENT_PACKET, event, sizeof(event));
155 }
156 
157 void l2cap_block_new_credits(uint8_t blocked){
158     new_credits_blocked = blocked;
159 }
160 
161 void l2cap_hand_out_credits(void){
162 
163     if (new_credits_blocked) return;    // we're told not to. used by daemon
164 
165     linked_item_t *it;
166     for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){
167         if (!hci_number_free_acl_slots()) return;
168         l2cap_channel_t * channel = (l2cap_channel_t *) it;
169         if (channel->state != L2CAP_STATE_OPEN) continue;
170         if (hci_number_outgoing_packets(channel->handle) < NR_BUFFERED_ACL_PACKETS && channel->packets_granted == 0) {
171             l2cap_emit_credits(channel, 1);
172         }
173     }
174 }
175 
176 l2cap_channel_t * l2cap_get_channel_for_local_cid(uint16_t local_cid){
177     linked_item_t *it;
178     for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){
179         l2cap_channel_t * channel = (l2cap_channel_t *) it;
180         if ( channel->local_cid == local_cid) {
181             return channel;
182         }
183     }
184     return NULL;
185 }
186 
187 uint16_t l2cap_get_remote_mtu_for_local_cid(uint16_t local_cid){
188     l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid);
189     if (channel) {
190         return channel->remote_mtu;
191     }
192     return 0;
193 }
194 
195 int l2cap_send_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, ...){
196     // log_dbg("l2cap_send_signaling_packet type %u\n", cmd);
197     va_list argptr;
198     va_start(argptr, identifier);
199     uint16_t len = l2cap_create_signaling_internal(sig_buffer, handle, cmd, identifier, argptr);
200     va_end(argptr);
201     // log_dbg("l2cap_send_signaling_packet con %u!\n", handle);
202     return hci_send_acl_packet(sig_buffer, len);
203 }
204 
205 int l2cap_send_internal(uint16_t local_cid, uint8_t *data, uint16_t len){
206 
207     // check for free places on BT module
208     if (!hci_number_free_acl_slots()) {
209         log_dbg("l2cap_send_internal cid %u, BT module full <-----\n", local_cid);
210         return BTSTACK_ACL_BUFFERS_FULL;
211     }
212     int err = 0;
213 
214     // find channel for local_cid, construct l2cap packet and send
215     l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid);
216     if (channel) {
217         if (channel->packets_granted > 0){
218             --channel->packets_granted;
219             // log_dbg("l2cap_send_internal cid %u, handle %u, 1 credit used, credits left %u;\n",
220             //        local_cid, channel->handle, channel->packets_granted);
221         } else {
222             log_err("l2cap_send_internal cid %u, no credits!\n", local_cid);
223         }
224 
225         // 0 - Connection handle : PB=10 : BC=00
226         bt_store_16(acl_buffer, 0, channel->handle | (2 << 12) | (0 << 14));
227         // 2 - ACL length
228         bt_store_16(acl_buffer, 2,  len + 4);
229         // 4 - L2CAP packet length
230         bt_store_16(acl_buffer, 4,  len + 0);
231         // 6 - L2CAP channel DEST
232         bt_store_16(acl_buffer, 6, channel->remote_cid);
233         // 8 - data
234         memcpy(&acl_buffer[8], data, len);
235 
236         // send
237         err = hci_send_acl_packet(acl_buffer, len+8);
238     }
239 
240     l2cap_hand_out_credits();
241 
242     return err;
243 }
244 
245 // process outstanding signaling tasks
246 void l2cap_run(void){
247     uint8_t  config_options[4];
248     linked_item_t *it;
249     for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){
250 
251         // can send?
252 
253         l2cap_channel_t * channel = (l2cap_channel_t *) it;
254         switch (channel->state){
255 
256             case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE:
257                 l2cap_send_signaling_packet(channel->handle, CONNECTION_RESPONSE, channel->remote_sig_id, 0, 0, channel->reason, 0);
258                 // discard channel - l2cap_finialize_channel_close without sending l2cap close event
259                 linked_list_remove(&l2cap_channels, (linked_item_t *) channel);
260                 free (channel);
261                 break;
262 
263             case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT:
264                 l2cap_send_signaling_packet(channel->handle, CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid, 0, 0);
265                 channel->state = L2CAP_STATE_WAIT_CONFIG_REQ_OR_SEND_CONFIG_REQ;
266                 break;
267 
268             case L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST:
269                 // success, start l2cap handshake
270                 channel->local_sig_id = l2cap_next_sig_id();
271                 l2cap_send_signaling_packet( channel->handle, CONNECTION_REQUEST, channel->local_sig_id, channel->psm, channel->local_cid);
272                 channel->state = L2CAP_STATE_WAIT_CONNECT_RSP;
273                 break;
274 
275             case L2CAP_STATE_WAIT_CONFIG_REQ_OR_SEND_CONFIG_REQ:
276                 // after connection response was received
277                 channel->local_sig_id = l2cap_next_sig_id();
278                 config_options[0] = 1; // MTU
279                 config_options[1] = 2; // len param
280                 bt_store_16( (uint8_t*)&config_options, 2, channel->local_mtu);
281                 l2cap_send_signaling_packet(channel->handle, CONFIGURE_REQUEST, channel->local_sig_id, channel->remote_cid, 0, 4, &config_options);
282                 channel->state = L2CAP_STATE_WAIT_CONFIG_REQ_RSP_OR_CONFIG_REQ;
283                 break;
284 
285             case L2CAP_STATE_WILL_SEND_CONFIG_REQ:
286                 channel->local_sig_id = l2cap_next_sig_id();
287                 config_options[0] = 1; // MTU
288                 config_options[1] = 2; // len param
289                 bt_store_16( (uint8_t*)&config_options, 2, channel->local_mtu);
290                 l2cap_send_signaling_packet(channel->handle, CONFIGURE_REQUEST, channel->local_sig_id, channel->remote_cid, 0, 4, &config_options);
291                 channel->state = L2CAP_STATE_WAIT_CONFIG_REQ_RSP;
292                 break;
293 
294             case L2CAP_STATE_WAIT_CONFIG_REQ_RSP_OR_WILL_SEND_CONFIG_REQ_RSP:
295                 l2cap_send_signaling_packet(channel->handle, CONFIGURE_RESPONSE, channel->remote_sig_id, channel->remote_cid, 0, 0, 0, NULL);
296                 channel->state = L2CAP_STATE_WAIT_CONFIG_REQ_RSP;
297                 break;
298 
299             case L2CAP_STATE_WILL_SEND_CONFIG_REQ_RSP:
300                 l2cap_send_signaling_packet(channel->handle, CONFIGURE_RESPONSE, channel->remote_sig_id, channel->remote_cid, 0, 0, 0, NULL);
301                 channel->state = L2CAP_STATE_OPEN;
302                 l2cap_emit_channel_opened(channel, 0);  // success
303                 l2cap_emit_credits(channel, 1);
304                 break;
305 
306             case L2CAP_STATE_WILL_SEND_CONFIG_REQ_AND_CONFIG_REQ_RSP:
307                 l2cap_send_signaling_packet(channel->handle, CONFIGURE_RESPONSE, channel->remote_sig_id, channel->remote_cid, 0, 0, 0, NULL);
308                 channel->state = L2CAP_STATE_WILL_SEND_CONFIG_REQ;
309                 break;
310 
311             case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE:
312                 l2cap_send_signaling_packet( channel->handle, DISCONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid);
313                 l2cap_finialize_channel_close(channel);
314                 break;
315 
316             case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST:
317                 channel->local_sig_id = l2cap_next_sig_id();
318                 l2cap_send_signaling_packet( channel->handle, DISCONNECTION_REQUEST, channel->local_sig_id, channel->remote_cid, channel->local_cid);
319                 channel->state = L2CAP_STATE_WAIT_DISCONNECT;
320                 break;
321             default:
322                 break;
323         }
324     }
325 }
326 
327 // open outgoing L2CAP channel
328 void l2cap_create_channel_internal(void * connection, btstack_packet_handler_t packet_handler,
329                                    bd_addr_t address, uint16_t psm, uint16_t mtu){
330 
331     // alloc structure
332     l2cap_channel_t * chan = malloc(sizeof(l2cap_channel_t));
333     // TODO: emit error event
334     if (!chan) return;
335 
336     // limit local mtu to max acl packet length
337     if (mtu > hci_max_acl_data_packet_length()) {
338         mtu = hci_max_acl_data_packet_length();
339     }
340 
341     // fill in
342     BD_ADDR_COPY(chan->address, address);
343     chan->psm = psm;
344     chan->handle = 0;
345     chan->connection = connection;
346     chan->packet_handler = packet_handler;
347     chan->remote_mtu = L2CAP_MINIMAL_MTU;
348     chan->local_mtu = mtu;
349     chan->packets_granted = 0;
350 
351     // set initial state
352     chan->state = L2CAP_STATE_CLOSED;
353     chan->remote_sig_id = L2CAP_SIG_ID_INVALID;
354     chan->local_sig_id = L2CAP_SIG_ID_INVALID;
355 
356     // add to connections list
357     linked_list_add(&l2cap_channels, (linked_item_t *) chan);
358 
359     // send connection request
360     // BD_ADDR, Packet_Type, Page_Scan_Repetition_Mode, Reserved, Clock_Offset, Allow_Role_Switch
361     hci_send_cmd(&hci_create_connection, address, 0xcc18, 0, 0, 0, 1);
362 }
363 
364 void l2cap_disconnect_internal(uint16_t local_cid, uint8_t reason){
365     // find channel for local_cid
366     l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid);
367     if (channel) {
368         channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST;
369     }
370     // process
371     l2cap_run();
372 }
373 
374 static void l2cap_handle_connection_failed_for_addr(bd_addr_t address, uint8_t status){
375     linked_item_t *it = (linked_item_t *) &l2cap_channels;
376     while (it->next){
377         l2cap_channel_t * channel = (l2cap_channel_t *) it->next;
378         if ( ! BD_ADDR_CMP( channel->address, address) ){
379             if (channel->state == L2CAP_STATE_CLOSED) {
380                 // failure, forward error code
381                 l2cap_emit_channel_opened(channel, status);
382                 // discard channel
383                 it->next = it->next->next;
384                 free (channel);
385             }
386         } else {
387             it = it->next;
388         }
389     }
390 }
391 
392 static void l2cap_handle_connection_success_for_addr(bd_addr_t address, hci_con_handle_t handle){
393     linked_item_t *it;
394     for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){
395         l2cap_channel_t * channel = (l2cap_channel_t *) it;
396         if ( ! BD_ADDR_CMP( channel->address, address) ){
397             if (channel->state == L2CAP_STATE_CLOSED) {
398                 // success, start l2cap handshake
399                 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST;
400                 channel->handle = handle;
401                 channel->local_cid = l2cap_next_local_cid();
402             }
403         }
404     }
405     // process
406     l2cap_run();
407 }
408 
409 void l2cap_event_handler( uint8_t *packet, uint16_t size ){
410 
411     bd_addr_t address;
412     hci_con_handle_t handle;
413     linked_item_t *it;
414 
415     switch(packet[0]){
416 
417         // handle connection complete events
418         case HCI_EVENT_CONNECTION_COMPLETE:
419             bt_flip_addr(address, &packet[5]);
420             if (packet[2] == 0){
421                 handle = READ_BT_16(packet, 3);
422                 l2cap_handle_connection_success_for_addr(address, handle);
423             } else {
424                 l2cap_handle_connection_failed_for_addr(address, packet[2]);
425             }
426             break;
427 
428         // handle successful create connection cancel command
429         case HCI_EVENT_COMMAND_COMPLETE:
430             if ( COMMAND_COMPLETE_EVENT(packet, hci_create_connection_cancel) ) {
431                 if (packet[5] == 0){
432                     bt_flip_addr(address, &packet[6]);
433                     // CONNECTION TERMINATED BY LOCAL HOST (0X16)
434                     l2cap_handle_connection_failed_for_addr(address, 0x16);
435                 }
436             }
437             break;
438 
439         // handle disconnection complete events
440         case HCI_EVENT_DISCONNECTION_COMPLETE:
441             // send l2cap disconnect events for all channels on this handle
442             handle = READ_BT_16(packet, 3);
443             it = (linked_item_t *) &l2cap_channels;
444             while (it->next){
445                 l2cap_channel_t * channel = (l2cap_channel_t *) it->next;
446                 if ( channel->handle == handle ){
447                     // update prev item before free'ing next element - don't call l2cap_finalize_channel_close
448                     it->next = it->next->next;
449                     l2cap_emit_channel_closed(channel);
450                     free (channel);
451                 } else {
452                     it = it->next;
453                 }
454             }
455             break;
456 
457         case HCI_EVENT_NUMBER_OF_COMPLETED_PACKETS:
458             l2cap_hand_out_credits();
459             break;
460 
461         // HCI Connection Timeouts
462         case L2CAP_EVENT_TIMEOUT_CHECK:
463             handle = READ_BT_16(packet, 2);
464             if (hci_authentication_active_for_handle(handle)) break;
465             l2cap_channel_t * channel;
466             int used = 0;
467             for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){
468                 channel = (l2cap_channel_t *) it;
469                 if (channel->handle == handle) {
470                     used = 1;
471                 }
472             }
473             if (!used) {
474                 hci_send_cmd(&hci_disconnect, handle, 0x13); // remote closed connection
475             }
476             break;
477 
478         default:
479             break;
480     }
481 
482     // pass on
483     (*packet_handler)(NULL, HCI_EVENT_PACKET, 0, packet, size);
484 }
485 
486 static void l2cap_handle_disconnect_request(l2cap_channel_t *channel, uint16_t identifier){
487     channel->remote_sig_id = identifier;
488     channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE;
489     l2cap_run();
490 }
491 
492 static void l2cap_handle_connection_request(hci_con_handle_t handle, uint8_t sig_id, uint16_t psm, uint16_t source_cid){
493 
494     // log_dbg("l2cap_handle_connection_request for handle %u, psm %u cid %u\n", handle, psm, source_cid);
495     l2cap_service_t *service = l2cap_get_service(psm);
496     if (!service) {
497         // 0x0002 PSM not supported
498         // log_dbg("l2cap_handle_connection_request no PSM for psm %u/n", psm);
499         l2cap_send_signaling_packet(handle, CONNECTION_RESPONSE, sig_id, 0, 0, 0x0002, 0);
500         return;
501     }
502 
503     hci_connection_t * hci_connection = connection_for_handle( handle );
504     if (!hci_connection) {
505         log_err("no hci_connection for handle %u\n", handle);
506         // TODO: emit error
507         return;
508     }
509     // alloc structure
510     // log_dbg("l2cap_handle_connection_request register channel\n");
511     l2cap_channel_t * channel = malloc(sizeof(l2cap_channel_t));
512     // TODO: emit error event
513     if (!channel) return;
514 
515     // fill in
516     BD_ADDR_COPY(channel->address, hci_connection->address);
517     channel->psm = psm;
518     channel->handle = handle;
519     channel->connection = service->connection;
520     channel->packet_handler = service->packet_handler;
521     channel->local_cid  = l2cap_next_local_cid();
522     channel->remote_cid = source_cid;
523     channel->local_mtu  = service->mtu;
524     channel->remote_mtu = L2CAP_DEFAULT_MTU;
525     channel->packets_granted = 0;
526     channel->remote_sig_id = sig_id;
527 
528     // limit local mtu to max acl packet length
529     if (channel->local_mtu > hci_max_acl_data_packet_length()) {
530         channel->local_mtu = hci_max_acl_data_packet_length();
531     }
532 
533     // set initial state
534     channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT;
535 
536     // add to connections list
537     linked_list_add(&l2cap_channels, (linked_item_t *) channel);
538 
539     // emit incoming connection request
540     l2cap_emit_connection_request(channel);
541 }
542 
543 void l2cap_accept_connection_internal(uint16_t local_cid){
544     l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid);
545     if (!channel) {
546         log_err("l2cap_accept_connection_internal called but local_cid 0x%x not found", local_cid);
547         return;
548     }
549 
550     channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT;
551 
552     // process
553     l2cap_run();
554 }
555 
556 void l2cap_decline_connection_internal(uint16_t local_cid, uint8_t reason){
557     l2cap_channel_t * channel = l2cap_get_channel_for_local_cid( local_cid);
558     if (!channel) {
559         log_err( "l2cap_decline_connection_internal called but local_cid 0x%x not found", local_cid);
560         return;
561     }
562     channel->state  = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE;
563     channel->reason = reason;
564     l2cap_run();
565 }
566 
567 void l2cap_signaling_handle_configure_request(l2cap_channel_t *channel, uint8_t *command){
568 
569     channel->remote_sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET];
570 
571     // accept the other's configuration options
572     uint16_t end_pos = 4 + READ_BT_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET);
573     uint16_t pos     = 8;
574     while (pos < end_pos){
575         uint8_t type   = command[pos++];
576         uint8_t length = command[pos++];
577         // MTU { type(8): 1, len(8):2, MTU(16) }
578         if ((type & 0x7f) == 1 && length == 2){
579             channel->remote_mtu = READ_BT_16(command, pos);
580             // log_dbg("l2cap cid %u, remote mtu %u\n", channel->local_cid, channel->remote_mtu);
581         }
582         pos += length;
583     }
584 }
585 
586 void l2cap_signaling_handler_channel(l2cap_channel_t *channel, uint8_t *command){
587 
588     uint8_t  code       = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET];
589     uint8_t  identifier = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET];
590     uint16_t result = 0;
591 
592     // log_dbg("signaling handler code %u\n", code);
593 
594     // handle DISCONNECT REQUESTS seperately
595     if (code == DISCONNECTION_REQUEST){
596         switch (channel->state){
597             case L2CAP_STATE_WAIT_CONFIG_REQ_OR_SEND_CONFIG_REQ:
598             case L2CAP_STATE_WAIT_CONFIG_REQ_RSP_OR_WILL_SEND_CONFIG_REQ_RSP,
599             case L2CAP_STATE_WAIT_CONFIG_REQ_RSP_OR_CONFIG_REQ:
600             case L2CAP_STATE_WAIT_CONFIG_REQ:
601             case L2CAP_STATE_WAIT_CONFIG_REQ_RSP:
602             case L2CAP_STATE_WILL_SEND_CONFIG_REQ,
603             case L2CAP_STATE_WILL_SEND_CONFIG_REQ_RSP,
604             case L2CAP_STATE_WILL_SEND_CONFIG_REQ_AND_CONFIG_REQ_RSP,
605             case L2CAP_STATE_OPEN:
606             case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST,
607             case L2CAP_STATE_WAIT_DISCONNECT:
608                 l2cap_handle_disconnect_request(channel, identifier);
609                 break;
610 
611             default:
612                 // ignore in other states
613                 break;
614         }
615         return;
616     }
617 
618     switch (channel->state) {
619 
620         case L2CAP_STATE_WAIT_CONNECT_RSP:
621             switch (code){
622                 case CONNECTION_RESPONSE:
623                     result = READ_BT_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4);
624                     switch (result) {
625                         case 0:
626                             // successful connection
627                             channel->remote_cid = READ_BT_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET);
628                             channel->state = L2CAP_STATE_WAIT_CONFIG_REQ_OR_SEND_CONFIG_REQ;
629 #if 0
630     channel->state = L2CAP_STATE_OPEN;
631     l2cap_emit_channel_opened(channel, 0);  // success
632     l2cap_emit_credits(channel, 1);
633 #endif
634                             break;
635                         case 1:
636                             // connection pending. get some coffee
637                             break;
638                         default:
639                             // channel closed
640                             channel->state = L2CAP_STATE_CLOSED;
641 
642                             // map l2cap connection response result to BTstack status enumeration
643                             l2cap_emit_channel_opened(channel, L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result);
644 
645                             // drop link key if security block
646                             if (L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result == L2CAP_CONNECTION_RESPONSE_RESULT_REFUSED_SECURITY){
647                                 hci_drop_link_key_for_bd_addr(&channel->address);
648                             }
649 
650                             // discard channel
651                             linked_list_remove(&l2cap_channels, (linked_item_t *) channel);
652                             free (channel);
653                             break;
654                     }
655                     break;
656 
657                 default:
658                     //@TODO: implement other signaling packets
659                     break;
660             }
661             break;
662 
663         case L2CAP_STATE_WAIT_CONFIG_REQ_OR_SEND_CONFIG_REQ:
664             switch (code) {
665                 case CONFIGURE_REQUEST:
666                     l2cap_signaling_handle_configure_request(channel, command);
667                     channel->state = L2CAP_STATE_WILL_SEND_CONFIG_REQ_AND_CONFIG_REQ_RSP;
668                     break;
669                 default:
670                     //@TODO: implement other signaling packets
671                     break;
672             }
673             break;
674 
675         case L2CAP_STATE_WAIT_CONFIG_REQ_RSP_OR_CONFIG_REQ:
676             switch (code) {
677                 case CONFIGURE_RESPONSE:
678                     channel->state = L2CAP_STATE_WAIT_CONFIG_REQ;
679                     break;
680                 case CONFIGURE_REQUEST:
681                     l2cap_signaling_handle_configure_request(channel, command);
682                     channel->state = L2CAP_STATE_WAIT_CONFIG_REQ_RSP_OR_WILL_SEND_CONFIG_REQ_RSP;
683                     break;
684                 default:
685                     //@TODO: implement other signaling packets
686                     break;
687             }
688             break;
689 
690         case L2CAP_STATE_WAIT_CONFIG_REQ:
691             switch (code) {
692                 case CONFIGURE_REQUEST:
693                     l2cap_signaling_handle_configure_request(channel, command);
694                     channel->state = L2CAP_STATE_WILL_SEND_CONFIG_REQ_RSP;
695                     break;
696                 default:
697                     //@TODO: implement other signaling packets
698                     break;
699             }
700             break;
701 
702         case L2CAP_STATE_WAIT_CONFIG_REQ_RSP:
703             switch (code) {
704                 case CONFIGURE_RESPONSE:
705                     channel->state = L2CAP_STATE_OPEN;
706                     l2cap_emit_channel_opened(channel, 0);  // success
707                     l2cap_emit_credits(channel, 1);
708                     break;
709                 default:
710                     //@TODO: implement other signaling packets
711                     break;
712             }
713             break;
714 
715         case L2CAP_STATE_WAIT_DISCONNECT:
716             switch (code) {
717                 case DISCONNECTION_RESPONSE:
718                     l2cap_finialize_channel_close(channel);
719                     break;
720                 default:
721                     //@TODO: implement other signaling packets
722                     break;
723             }
724             break;
725 
726         case L2CAP_STATE_CLOSED:
727             // @TODO handle incoming requests
728             break;
729 
730         case L2CAP_STATE_OPEN:
731             //@TODO: implement other signaling packets, e.g. re-configure
732             break;
733         default:
734             break;
735     }
736     // log_dbg("new state %u\n", channel->state);
737 }
738 
739 
740 void l2cap_signaling_handler_dispatch( hci_con_handle_t handle, uint8_t * command){
741 
742     // get code, signalind identifier and command len
743     uint8_t code   = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET];
744     uint8_t sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET];
745     uint16_t len   = READ_BT_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET);
746 
747     // not for a particular channel, and not CONNECTION_REQUEST, ECHO_[REQUEST|RESPONSE], INFORMATION_REQUEST
748     if (code < 1 || code == ECHO_RESPONSE || code > INFORMATION_REQUEST){
749         return;
750     }
751 
752     // general commands without an assigned channel
753     switch(code) {
754 
755         case CONNECTION_REQUEST: {
756             uint16_t psm =        READ_BT_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET);
757             uint16_t source_cid = READ_BT_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2);
758             l2cap_handle_connection_request(handle, sig_id, psm, source_cid);
759             break;
760         }
761 
762         case ECHO_REQUEST: {
763             // send back packet
764             l2cap_send_signaling_packet(handle, ECHO_RESPONSE, sig_id, len, &command[L2CAP_SIGNALING_COMMAND_DATA_OFFSET]);
765             break;
766         }
767 
768         case INFORMATION_REQUEST: {
769             // we neither support connectionless L2CAP data nor support any flow control modes yet
770             uint16_t infoType = READ_BT_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET);
771             if (infoType == 2) {
772                 uint32_t features = 0;
773                 // extended features request supported, however no features present
774                 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 0, 4, &features);
775             } else {
776                 // all other types are not supported
777                 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 1, 0, NULL);
778             }
779             break;;
780         }
781 
782         default:
783             break;
784     }
785 
786 
787     // Get potential destination CID
788     uint16_t dest_cid = READ_BT_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET);
789 
790     // Find channel for this sig_id and connection handle
791     linked_item_t *it;
792     for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){
793         l2cap_channel_t * channel = (l2cap_channel_t *) it;
794         if (channel->handle == handle) {
795             if (code & 1) {
796                 // match odd commands (responses) by previous signaling identifier
797                 if (channel->local_sig_id == sig_id) {
798                     l2cap_signaling_handler_channel(channel, command);
799                     break;
800                 }
801             } else {
802                 // match even commands (requests) by local channel id
803                 if (channel->local_cid == dest_cid) {
804                     l2cap_signaling_handler_channel(channel, command);
805                     break;
806                 }
807             }
808         }
809     }
810 }
811 
812 void l2cap_acl_handler( uint8_t *packet, uint16_t size ){
813 
814     // Get Channel ID
815     uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet);
816 
817     // Signaling Packet?
818     if (channel_id == 1) {
819 
820         // Get Connection
821         hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet);
822 
823         uint16_t command_offset = 8;
824         while (command_offset < size) {
825 
826             // handle signaling commands
827             l2cap_signaling_handler_dispatch(handle, &packet[command_offset]);
828 
829             // increment command_offset
830             command_offset += L2CAP_SIGNALING_COMMAND_DATA_OFFSET + READ_BT_16(packet, command_offset + L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET);
831         }
832         return;
833     }
834 
835     // Find channel for this channel_id and connection handle
836     l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(channel_id);
837     if (channel) {
838         l2cap_dispatch(channel, L2CAP_DATA_PACKET, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER);
839     }
840 }
841 
842 static void l2cap_packet_handler(uint8_t packet_type, uint8_t *packet, uint16_t size){
843     switch (packet_type) {
844         case HCI_EVENT_PACKET:
845             l2cap_event_handler(packet, size);
846             break;
847         case HCI_ACL_DATA_PACKET:
848             l2cap_acl_handler(packet, size);
849             break;
850         default:
851             break;
852     }
853 }
854 
855 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE
856 void l2cap_finialize_channel_close(l2cap_channel_t *channel){
857     channel->state = L2CAP_STATE_CLOSED;
858     l2cap_emit_channel_closed(channel);
859     // discard channel
860     linked_list_remove(&l2cap_channels, (linked_item_t *) channel);
861     free (channel);
862 }
863 
864 l2cap_service_t * l2cap_get_service(uint16_t psm){
865     linked_item_t *it;
866 
867     // close open channels
868     for (it = (linked_item_t *) l2cap_services; it ; it = it->next){
869         l2cap_service_t * service = ((l2cap_service_t *) it);
870         if ( service->psm == psm){
871             return service;
872         };
873     }
874     return NULL;
875 }
876 
877 void l2cap_register_service_internal(void *connection, btstack_packet_handler_t packet_handler, uint16_t psm, uint16_t mtu){
878     // check for alread registered psm // TODO: emit error event
879     l2cap_service_t *service = l2cap_get_service(psm);
880     if (service) return;
881 
882     // alloc structure     // TODO: emit error event
883     service = malloc(sizeof(l2cap_service_t));
884     if (!service) return;
885 
886     // fill in
887     service->psm = psm;
888     service->mtu = mtu;
889     service->connection = connection;
890     service->packet_handler = packet_handler;
891 
892     // add to services list
893     linked_list_add(&l2cap_services, (linked_item_t *) service);
894 }
895 
896 void l2cap_unregister_service_internal(void *connection, uint16_t psm){
897     l2cap_service_t *service = l2cap_get_service(psm);
898     if (!service) return;
899     linked_list_remove(&l2cap_services, (linked_item_t *) service);
900     free(service);
901 }
902 
903 //
904 void l2cap_close_connection(void *connection){
905     linked_item_t *it;
906 
907     // close open channels - note to myself: no channel is freed, so no new for fancy iterator tricks
908     l2cap_channel_t * channel;
909     for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){
910         channel = (l2cap_channel_t *) it;
911         if (channel->connection == connection) {
912             channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST;
913         }
914     }
915 
916     // unregister services
917     it = (linked_item_t *) &l2cap_services;
918     while (it->next) {
919         l2cap_service_t * service = (l2cap_service_t *) it->next;
920         if (service->connection == connection){
921             it->next = it->next->next;
922             free(service);
923         } else {
924             it = it->next;
925         }
926     }
927 
928     // process
929     l2cap_run();
930 }
931