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