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