xref: /btstack/src/l2cap.c (revision 2bd8b7e7f3593bf17254508b767a24a5ef237620)
1 /*
2  * Copyright (C) 2009-2012 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  * 4. Any redistribution, use, or modification is done solely for
17  *    personal benefit and not for any commercial purpose or for
18  *    monetary gain.
19  *
20  * THIS SOFTWARE IS PROVIDED BY MATTHIAS RINGWALD AND CONTRIBUTORS
21  * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS
24  * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
27  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
28  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
29  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
30  * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
31  * SUCH DAMAGE.
32  *
33  * Please inquire about commercial licensing options at [email protected]
34  *
35  */
36 
37 /*
38  *  l2cap.c
39  *
40  *  Logical Link Control and Adaption Protocl (L2CAP)
41  *
42  *  Created by Matthias Ringwald on 5/16/09.
43  */
44 
45 #include "l2cap.h"
46 #include "hci.h"
47 #include "hci_dump.h"
48 #include "debug.h"
49 #include "btstack_memory.h"
50 
51 #include <stdarg.h>
52 #include <string.h>
53 
54 #include <stdio.h>
55 
56 // nr of buffered acl packets in outgoing queue to get max performance
57 #define NR_BUFFERED_ACL_PACKETS 3
58 
59 // used to cache l2cap rejects, echo, and informational requests
60 #define NR_PENDING_SIGNALING_RESPONSES 3
61 
62 // offsets for L2CAP SIGNALING COMMANDS
63 #define L2CAP_SIGNALING_COMMAND_CODE_OFFSET   0
64 #define L2CAP_SIGNALING_COMMAND_SIGID_OFFSET  1
65 #define L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET 2
66 #define L2CAP_SIGNALING_COMMAND_DATA_OFFSET   4
67 
68 static void null_packet_handler(void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size);
69 static void l2cap_packet_handler(uint8_t packet_type, uint8_t *packet, uint16_t size);
70 
71 // used to cache l2cap rejects, echo, and informational requests
72 static l2cap_signaling_response_t signaling_responses[NR_PENDING_SIGNALING_RESPONSES];
73 static int signaling_responses_pending;
74 
75 static linked_list_t l2cap_channels = NULL;
76 static linked_list_t l2cap_services = NULL;
77 static void (*packet_handler) (void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) = null_packet_handler;
78 static int new_credits_blocked = 0;
79 
80 static btstack_packet_handler_t attribute_protocol_packet_handler = NULL;
81 static btstack_packet_handler_t security_protocol_packet_handler = NULL;
82 
83 // prototypes
84 static void l2cap_finialize_channel_close(l2cap_channel_t *channel);
85 static l2cap_service_t * l2cap_get_service(uint16_t psm);
86 static void l2cap_emit_channel_opened(l2cap_channel_t *channel, uint8_t status);
87 static void l2cap_emit_channel_closed(l2cap_channel_t *channel);
88 static void l2cap_emit_connection_request(l2cap_channel_t *channel);
89 static int l2cap_channel_ready_for_open(l2cap_channel_t *channel);
90 
91 
92 void l2cap_init(){
93     new_credits_blocked = 0;
94     signaling_responses_pending = 0;
95 
96     l2cap_channels = NULL;
97     l2cap_services = NULL;
98 
99     packet_handler = null_packet_handler;
100 
101     //
102     // register callback with HCI
103     //
104     hci_register_packet_handler(&l2cap_packet_handler);
105     hci_connectable_control(0); // no services yet
106 }
107 
108 
109 /** Register L2CAP packet handlers */
110 static void null_packet_handler(void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
111 }
112 void l2cap_register_packet_handler(void (*handler)(void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size)){
113     packet_handler = handler;
114 }
115 
116 //  notify client/protocol handler
117 void l2cap_dispatch(l2cap_channel_t *channel, uint8_t type, uint8_t * data, uint16_t size){
118     if (channel->packet_handler) {
119         (* (channel->packet_handler))(type, channel->local_cid, data, size);
120     } else {
121         (*packet_handler)(channel->connection, type, channel->local_cid, data, size);
122     }
123 }
124 
125 void l2cap_emit_channel_opened(l2cap_channel_t *channel, uint8_t status) {
126     log_info("L2CAP_EVENT_CHANNEL_OPENED status 0x%x addr %s handle 0x%x psm 0x%x local_cid 0x%x remote_cid 0x%x local_mtu %u, remote_mtu %u, flush_timeout %u",
127              status, bd_addr_to_str(channel->address), channel->handle, channel->psm,
128              channel->local_cid, channel->remote_cid, channel->local_mtu, channel->remote_mtu, channel->flush_timeout);
129     uint8_t event[23];
130     event[0] = L2CAP_EVENT_CHANNEL_OPENED;
131     event[1] = sizeof(event) - 2;
132     event[2] = status;
133     bt_flip_addr(&event[3], channel->address);
134     bt_store_16(event,  9, channel->handle);
135     bt_store_16(event, 11, channel->psm);
136     bt_store_16(event, 13, channel->local_cid);
137     bt_store_16(event, 15, channel->remote_cid);
138     bt_store_16(event, 17, channel->local_mtu);
139     bt_store_16(event, 19, channel->remote_mtu);
140     bt_store_16(event, 19, channel->flush_timeout);
141     hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
142     l2cap_dispatch(channel, HCI_EVENT_PACKET, event, sizeof(event));
143 }
144 
145 void l2cap_emit_channel_closed(l2cap_channel_t *channel) {
146     log_info("L2CAP_EVENT_CHANNEL_CLOSED local_cid 0x%x", channel->local_cid);
147     uint8_t event[4];
148     event[0] = L2CAP_EVENT_CHANNEL_CLOSED;
149     event[1] = sizeof(event) - 2;
150     bt_store_16(event, 2, channel->local_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_connection_request(l2cap_channel_t *channel) {
156     log_info("L2CAP_EVENT_INCOMING_CONNECTION addr %s handle 0x%x psm 0x%x local_cid 0x%x remote_cid 0x%x",
157              bd_addr_to_str(channel->address), channel->handle,  channel->psm, channel->local_cid, channel->remote_cid);
158     uint8_t event[16];
159     event[0] = L2CAP_EVENT_INCOMING_CONNECTION;
160     event[1] = sizeof(event) - 2;
161     bt_flip_addr(&event[2], channel->address);
162     bt_store_16(event,  8, channel->handle);
163     bt_store_16(event, 10, channel->psm);
164     bt_store_16(event, 12, channel->local_cid);
165     bt_store_16(event, 14, channel->remote_cid);
166     hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
167     l2cap_dispatch(channel, HCI_EVENT_PACKET, event, sizeof(event));
168 }
169 
170 static void l2cap_emit_service_registered(void *connection, uint8_t status, uint16_t psm){
171     log_info("L2CAP_EVENT_SERVICE_REGISTERED status 0x%x psm 0x%x", status, psm);
172     uint8_t event[5];
173     event[0] = L2CAP_EVENT_SERVICE_REGISTERED;
174     event[1] = sizeof(event) - 2;
175     event[2] = status;
176     bt_store_16(event, 3, psm);
177     hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
178     (*packet_handler)(connection, HCI_EVENT_PACKET, 0, event, sizeof(event));
179 }
180 
181 void l2cap_emit_credits(l2cap_channel_t *channel, uint8_t credits) {
182 
183     log_info("L2CAP_EVENT_CREDITS local_cid 0x%x credits %u", channel->local_cid, credits);
184     // track credits
185     channel->packets_granted += credits;
186 
187     uint8_t event[5];
188     event[0] = L2CAP_EVENT_CREDITS;
189     event[1] = sizeof(event) - 2;
190     bt_store_16(event, 2, channel->local_cid);
191     event[4] = credits;
192     hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
193     l2cap_dispatch(channel, HCI_EVENT_PACKET, event, sizeof(event));
194 }
195 
196 void l2cap_block_new_credits(uint8_t blocked){
197     new_credits_blocked = blocked;
198 }
199 
200 void l2cap_hand_out_credits(void){
201 
202     if (new_credits_blocked) return;    // we're told not to. used by daemon
203 
204     linked_item_t *it;
205     for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){
206         if (!hci_number_free_acl_slots()) return;
207         l2cap_channel_t * channel = (l2cap_channel_t *) it;
208         if (channel->state != L2CAP_STATE_OPEN) continue;
209         if (hci_number_outgoing_packets(channel->handle) < NR_BUFFERED_ACL_PACKETS && channel->packets_granted == 0) {
210             l2cap_emit_credits(channel, 1);
211         }
212     }
213 }
214 
215 l2cap_channel_t * l2cap_get_channel_for_local_cid(uint16_t local_cid){
216     linked_item_t *it;
217     for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){
218         l2cap_channel_t * channel = (l2cap_channel_t *) it;
219         if ( channel->local_cid == local_cid) {
220             return channel;
221         }
222     }
223     return NULL;
224 }
225 
226 int  l2cap_can_send_packet_now(uint16_t local_cid){
227     l2cap_channel_t *channel = l2cap_get_channel_for_local_cid(local_cid);
228     if (!channel) return 0;
229     if (!channel->packets_granted) return 0;
230     return hci_can_send_packet_now(HCI_ACL_DATA_PACKET);
231 }
232 
233 uint16_t l2cap_get_remote_mtu_for_local_cid(uint16_t local_cid){
234     l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid);
235     if (channel) {
236         return channel->remote_mtu;
237     }
238     return 0;
239 }
240 
241 static l2cap_channel_t * l2cap_channel_for_rtx_timer(timer_source_t * ts){
242     linked_item_t *it;
243     for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){
244         l2cap_channel_t * channel = (l2cap_channel_t *) it;
245         if ( &channel->rtx == ts) {
246             return channel;
247         }
248     }
249     return NULL;
250 }
251 
252 static void l2cap_rtx_timeout(timer_source_t * ts){
253     l2cap_channel_t * channel = l2cap_channel_for_rtx_timer(ts);
254     if (!ts) return;
255 
256     log_info("l2cap_rtx_timeout for local cid 0x%02x", channel->local_cid);
257 
258     // "When terminating the channel, it is not necessary to send a L2CAP_DisconnectReq
259     //  and enter WAIT_DISCONNECT state. Channels can be transitioned directly to the CLOSED state."
260     // notify client
261     l2cap_emit_channel_opened(channel, L2CAP_CONNECTION_RESPONSE_RESULT_RTX_TIMEOUT);
262 
263     // discard channel
264     linked_list_remove(&l2cap_channels, (linked_item_t *) channel);
265     btstack_memory_l2cap_channel_free(channel);
266 }
267 
268 static void l2cap_stop_rtx(l2cap_channel_t * channel){
269     log_info("l2cap_stop_rtx for local cid 0x%02x", channel->local_cid);
270     run_loop_remove_timer(&channel->rtx);
271 }
272 
273 static void l2cap_start_rtx(l2cap_channel_t * channel){
274     log_info("l2cap_start_rtx for local cid 0x%02x", channel->local_cid);
275     l2cap_stop_rtx(channel);
276     run_loop_set_timer_handler(&channel->rtx, l2cap_rtx_timeout);
277     run_loop_set_timer(&channel->rtx, L2CAP_RTX_TIMEOUT_MS);
278     run_loop_add_timer(&channel->rtx);
279 }
280 
281 static void l2cap_start_ertx(l2cap_channel_t * channel){
282     log_info("l2cap_start_ertx for local cid 0x%02x", channel->local_cid);
283     l2cap_stop_rtx(channel);
284     run_loop_set_timer_handler(&channel->rtx, l2cap_rtx_timeout);
285     run_loop_set_timer(&channel->rtx, L2CAP_ERTX_TIMEOUT_MS);
286     run_loop_add_timer(&channel->rtx);
287 }
288 
289 
290 int l2cap_send_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, ...){
291 
292     if (!hci_can_send_packet_now(HCI_ACL_DATA_PACKET)){
293         log_info("l2cap_send_signaling_packet, cannot send\n");
294         return BTSTACK_ACL_BUFFERS_FULL;
295     }
296 
297     // log_info("l2cap_send_signaling_packet type %u\n", cmd);
298     uint8_t *acl_buffer = hci_get_outgoing_acl_packet_buffer();
299     va_list argptr;
300     va_start(argptr, identifier);
301     uint16_t len = l2cap_create_signaling_internal(acl_buffer, handle, cmd, identifier, argptr);
302     va_end(argptr);
303     // log_info("l2cap_send_signaling_packet con %u!\n", handle);
304     return hci_send_acl_packet(acl_buffer, len);
305 }
306 
307 uint8_t *l2cap_get_outgoing_buffer(void){
308     return hci_get_outgoing_acl_packet_buffer() + COMPLETE_L2CAP_HEADER; // 8 bytes
309 }
310 
311 int l2cap_send_prepared(uint16_t local_cid, uint16_t len){
312 
313     if (!hci_can_send_packet_now(HCI_ACL_DATA_PACKET)){
314         log_info("l2cap_send_internal cid 0x%02x, cannot send\n", local_cid);
315         return BTSTACK_ACL_BUFFERS_FULL;
316     }
317 
318     l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid);
319     if (!channel) {
320         log_error("l2cap_send_internal no channel for cid 0x%02x\n", local_cid);
321         return -1;   // TODO: define error
322     }
323 
324     if (channel->packets_granted == 0){
325         log_error("l2cap_send_internal cid 0x%02x, no credits!\n", local_cid);
326         return -1;  // TODO: define error
327     }
328 
329     --channel->packets_granted;
330 
331     log_debug("l2cap_send_internal cid 0x%02x, handle %u, 1 credit used, credits left %u;\n",
332                   local_cid, channel->handle, channel->packets_granted);
333 
334     uint8_t *acl_buffer = hci_get_outgoing_acl_packet_buffer();
335 
336     // 0 - Connection handle : PB=10 : BC=00
337     bt_store_16(acl_buffer, 0, channel->handle | (2 << 12) | (0 << 14));
338     // 2 - ACL length
339     bt_store_16(acl_buffer, 2,  len + 4);
340     // 4 - L2CAP packet length
341     bt_store_16(acl_buffer, 4,  len + 0);
342     // 6 - L2CAP channel DEST
343     bt_store_16(acl_buffer, 6, channel->remote_cid);
344     // send
345     int err = hci_send_acl_packet(acl_buffer, len+8);
346 
347     l2cap_hand_out_credits();
348 
349     return err;
350 }
351 
352 int l2cap_send_prepared_connectionless(uint16_t handle, uint16_t cid, uint16_t len){
353 
354     if (!hci_can_send_packet_now(HCI_ACL_DATA_PACKET)){
355         log_info("l2cap_send_prepared_to_handle cid 0x%02x, cannot send\n", cid);
356         return BTSTACK_ACL_BUFFERS_FULL;
357     }
358 
359     log_debug("l2cap_send_prepared_to_handle cid 0x%02x, handle %u\n", cid, handle);
360 
361     uint8_t *acl_buffer = hci_get_outgoing_acl_packet_buffer();
362 
363     // 0 - Connection handle : PB=10 : BC=00
364     bt_store_16(acl_buffer, 0, handle | (2 << 12) | (0 << 14));
365     // 2 - ACL length
366     bt_store_16(acl_buffer, 2,  len + 4);
367     // 4 - L2CAP packet length
368     bt_store_16(acl_buffer, 4,  len + 0);
369     // 6 - L2CAP channel DEST
370     bt_store_16(acl_buffer, 6, cid);
371     // send
372     int err = hci_send_acl_packet(acl_buffer, len+8);
373 
374     l2cap_hand_out_credits();
375 
376     return err;
377 }
378 
379 int l2cap_send_internal(uint16_t local_cid, uint8_t *data, uint16_t len){
380 
381     if (!hci_can_send_packet_now(HCI_ACL_DATA_PACKET)){
382         log_info("l2cap_send_internal cid 0x%02x, cannot send\n", local_cid);
383         return BTSTACK_ACL_BUFFERS_FULL;
384     }
385 
386     uint8_t *acl_buffer = hci_get_outgoing_acl_packet_buffer();
387 
388     memcpy(&acl_buffer[8], data, len);
389 
390     return l2cap_send_prepared(local_cid, len);
391 }
392 
393 int l2cap_send_connectionless(uint16_t handle, uint16_t cid, uint8_t *data, uint16_t len){
394 
395     if (!hci_can_send_packet_now(HCI_ACL_DATA_PACKET)){
396         log_info("l2cap_send_internal cid 0x%02x, cannot send\n", cid);
397         return BTSTACK_ACL_BUFFERS_FULL;
398     }
399 
400     uint8_t *acl_buffer = hci_get_outgoing_acl_packet_buffer();
401 
402     memcpy(&acl_buffer[8], data, len);
403 
404     return l2cap_send_prepared_connectionless(handle, cid, len);
405 }
406 
407 static inline void channelStateVarSetFlag(l2cap_channel_t *channel, L2CAP_CHANNEL_STATE_VAR flag){
408     channel->state_var = (L2CAP_CHANNEL_STATE_VAR) (channel->state_var | flag);
409 }
410 
411 static inline void channelStateVarClearFlag(l2cap_channel_t *channel, L2CAP_CHANNEL_STATE_VAR flag){
412     channel->state_var = (L2CAP_CHANNEL_STATE_VAR) (channel->state_var & ~flag);
413 }
414 
415 
416 
417 // MARK: L2CAP_RUN
418 // process outstanding signaling tasks
419 void l2cap_run(void){
420 
421     // check pending signaling responses
422     while (signaling_responses_pending){
423 
424         if (!hci_can_send_packet_now(HCI_ACL_DATA_PACKET)) break;
425 
426         hci_con_handle_t handle = signaling_responses[0].handle;
427         uint8_t sig_id = signaling_responses[0].sig_id;
428         uint16_t infoType = signaling_responses[0].data;    // INFORMATION_REQUEST
429         uint16_t result   = signaling_responses[0].data;    // CONNECTION_REQUEST, COMMAND_REJECT
430 
431         switch (signaling_responses[0].code){
432             case CONNECTION_REQUEST:
433                 l2cap_send_signaling_packet(handle, CONNECTION_RESPONSE, sig_id, 0, 0, result, 0);
434                 // also disconnect if result is 0x0003 - security blocked
435                 hci_disconnect_security_block(handle);
436                 break;
437             case ECHO_REQUEST:
438                 l2cap_send_signaling_packet(handle, ECHO_RESPONSE, sig_id, 0, NULL);
439                 break;
440             case INFORMATION_REQUEST:
441                 switch (infoType){
442                     case 1: { // Connectionless MTU
443                         uint16_t connectionless_mtu = hci_max_acl_data_packet_length();
444                         l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 0, sizeof(connectionless_mtu), &connectionless_mtu);
445                         break;
446                     }
447                     case 2: { // Extended Features Supported
448                         // extended features request supported, only supporing fixed channel map
449                         uint32_t features = 0x80;
450                         l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 0, sizeof(features), &features);
451                         break;
452                     }
453                     case 3: { // Fixed Channels Supported
454                         uint8_t map[8];
455                         memset(map, 0, 8);
456                         map[0] = 0x01;  // L2CAP Signaling Channel
457                         l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 0, sizeof(map), &map);
458                         break;
459                     }
460                     default:
461                         // all other types are not supported
462                         l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 1, 0, NULL);
463                         break;
464                 }
465                 break;
466             case COMMAND_REJECT:
467                 l2cap_send_signaling_packet(handle, COMMAND_REJECT, sig_id, result);
468                 break;
469             default:
470                 // should not happen
471                 break;
472         }
473 
474         // remove first item
475         signaling_responses_pending--;
476         int i;
477         for (i=0; i < signaling_responses_pending; i++){
478             memcpy(&signaling_responses[i], &signaling_responses[i+1], sizeof(l2cap_signaling_response_t));
479         }
480     }
481 
482     uint8_t  config_options[4];
483     linked_item_t *it;
484     linked_item_t *next;
485     for (it = (linked_item_t *) l2cap_channels; it ; it = next){
486         next = it->next;    // cache next item as current item might get freed
487 
488         if (!hci_can_send_packet_now(HCI_COMMAND_DATA_PACKET)) break;
489         if (!hci_can_send_packet_now(HCI_ACL_DATA_PACKET)) break;
490 
491         l2cap_channel_t * channel = (l2cap_channel_t *) it;
492 
493         // log_info("l2cap_run: state %u, var 0x%02x\n", channel->state, channel->state_var);
494 
495 
496         switch (channel->state){
497 
498             case L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT:
499                 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND) {
500                     channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND);
501                     l2cap_send_signaling_packet(channel->handle, CONNECTION_RESPONSE, channel->remote_sig_id, 0, 0, 1, 0);
502                 }
503                 break;
504 
505             case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION:
506                 // send connection request - set state first
507                 channel->state = L2CAP_STATE_WAIT_CONNECTION_COMPLETE;
508                 // BD_ADDR, Packet_Type, Page_Scan_Repetition_Mode, Reserved, Clock_Offset, Allow_Role_Switch
509                 hci_send_cmd(&hci_create_connection, channel->address, hci_usable_acl_packet_types(), 0, 0, 0, 1);
510                 break;
511 
512             case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE:
513                 l2cap_send_signaling_packet(channel->handle, CONNECTION_RESPONSE, channel->remote_sig_id, 0, 0, channel->reason, 0);
514                 // discard channel - l2cap_finialize_channel_close without sending l2cap close event
515                 linked_list_remove(&l2cap_channels, (linked_item_t *) channel); // -- remove from list
516                 btstack_memory_l2cap_channel_free(channel);
517                 break;
518 
519             case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT:
520                 channel->state = L2CAP_STATE_CONFIG;
521                 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ);
522                 l2cap_send_signaling_packet(channel->handle, CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid, 0, 0);
523                 break;
524 
525             case L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST:
526                 // success, start l2cap handshake
527                 channel->local_sig_id = l2cap_next_sig_id();
528                 channel->state = L2CAP_STATE_WAIT_CONNECT_RSP;
529                 l2cap_send_signaling_packet( channel->handle, CONNECTION_REQUEST, channel->local_sig_id, channel->psm, channel->local_cid);
530                 l2cap_start_rtx(channel);
531                 break;
532 
533             case L2CAP_STATE_CONFIG:
534                 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP){
535                     uint16_t flags = 0;
536                     channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP);
537                     if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT) {
538                         flags = 1;
539                     } else {
540                         channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP);
541                     }
542                     if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID){
543                         l2cap_send_signaling_packet(channel->handle, CONFIGURE_RESPONSE, channel->remote_sig_id, channel->remote_cid, flags, L2CAP_CONF_RESULT_UNKNOWN_OPTIONS, 0, NULL);
544                     } else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU){
545                         config_options[0] = 1; // MTU
546                         config_options[1] = 2; // len param
547                         bt_store_16( (uint8_t*)&config_options, 2, channel->remote_mtu);
548                         l2cap_send_signaling_packet(channel->handle, CONFIGURE_RESPONSE, channel->remote_sig_id, channel->remote_cid, flags, 0, 4, &config_options);
549                         channelStateVarClearFlag(channel,L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU);
550                     } else {
551                         l2cap_send_signaling_packet(channel->handle, CONFIGURE_RESPONSE, channel->remote_sig_id, channel->remote_cid, flags, 0, 0, NULL);
552                     }
553                     channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT);
554                 }
555                 else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ){
556                     channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ);
557                     channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_REQ);
558                     channel->local_sig_id = l2cap_next_sig_id();
559                     config_options[0] = 1; // MTU
560                     config_options[1] = 2; // len param
561                     bt_store_16( (uint8_t*)&config_options, 2, channel->local_mtu);
562                     l2cap_send_signaling_packet(channel->handle, CONFIGURE_REQUEST, channel->local_sig_id, channel->remote_cid, 0, 4, &config_options);
563                     l2cap_start_rtx(channel);
564                 }
565                 if (l2cap_channel_ready_for_open(channel)){
566                     channel->state = L2CAP_STATE_OPEN;
567                     l2cap_emit_channel_opened(channel, 0);  // success
568                     l2cap_emit_credits(channel, 1);
569                 }
570                 break;
571 
572             case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE:
573                 l2cap_send_signaling_packet( channel->handle, DISCONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid);
574                 // we don't start an RTX timer for a disconnect - there's no point in closing the channel if the other side doesn't respond :)
575                 l2cap_finialize_channel_close(channel);  // -- remove from list
576                 break;
577 
578             case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST:
579                 channel->local_sig_id = l2cap_next_sig_id();
580                 channel->state = L2CAP_STATE_WAIT_DISCONNECT;
581                 l2cap_send_signaling_packet( channel->handle, DISCONNECTION_REQUEST, channel->local_sig_id, channel->remote_cid, channel->local_cid);
582                 break;
583             default:
584                 break;
585         }
586     }
587 }
588 
589 uint16_t l2cap_max_mtu(void){
590     return hci_max_acl_data_packet_length() - L2CAP_HEADER_SIZE;
591 }
592 
593 // open outgoing L2CAP channel
594 void l2cap_create_channel_internal(void * connection, btstack_packet_handler_t packet_handler,
595                                    bd_addr_t address, uint16_t psm, uint16_t mtu){
596 
597     log_info("L2CAP_CREATE_CHANNEL_MTU addr %s psm 0x%x mtu %u", bd_addr_to_str(address), psm, mtu);
598 
599     // alloc structure
600     l2cap_channel_t * chan = (l2cap_channel_t*) btstack_memory_l2cap_channel_get();
601     if (!chan) {
602         // emit error event
603         l2cap_channel_t dummy_channel;
604         BD_ADDR_COPY(dummy_channel.address, address);
605         dummy_channel.psm = psm;
606         l2cap_emit_channel_opened(&dummy_channel, BTSTACK_MEMORY_ALLOC_FAILED);
607         return;
608     }
609     // limit local mtu to max acl packet length
610     if (mtu > l2cap_max_mtu()) {
611         mtu = l2cap_max_mtu();
612     }
613 
614     // fill in
615     BD_ADDR_COPY(chan->address, address);
616     chan->psm = psm;
617     chan->handle = 0;
618     chan->connection = connection;
619     chan->packet_handler = packet_handler;
620     chan->remote_mtu = L2CAP_MINIMAL_MTU;
621     chan->local_mtu = mtu;
622     chan->packets_granted = 0;
623 
624     // set initial state
625     chan->state = L2CAP_STATE_WILL_SEND_CREATE_CONNECTION;
626     chan->state_var = L2CAP_CHANNEL_STATE_VAR_NONE;
627     chan->remote_sig_id = L2CAP_SIG_ID_INVALID;
628     chan->local_sig_id = L2CAP_SIG_ID_INVALID;
629 
630     // add to connections list
631     linked_list_add(&l2cap_channels, (linked_item_t *) chan);
632 
633     l2cap_run();
634 }
635 
636 void l2cap_disconnect_internal(uint16_t local_cid, uint8_t reason){
637     log_info("L2CAP_DISCONNECT local_cid 0x%x reason 0x%x", local_cid, reason);
638     // find channel for local_cid
639     l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid);
640     if (channel) {
641         channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST;
642     }
643     // process
644     l2cap_run();
645 }
646 
647 static void l2cap_handle_connection_failed_for_addr(bd_addr_t address, uint8_t status){
648     linked_item_t *it = (linked_item_t *) &l2cap_channels;
649     while (it->next){
650         l2cap_channel_t * channel = (l2cap_channel_t *) it->next;
651         if ( ! BD_ADDR_CMP( channel->address, address) ){
652             if (channel->state == L2CAP_STATE_WAIT_CONNECTION_COMPLETE || channel->state == L2CAP_STATE_WILL_SEND_CREATE_CONNECTION) {
653                 // failure, forward error code
654                 l2cap_emit_channel_opened(channel, status);
655                 // discard channel
656                 it->next = it->next->next;
657                 btstack_memory_l2cap_channel_free(channel);
658             }
659         } else {
660             it = it->next;
661         }
662     }
663 }
664 
665 static void l2cap_handle_connection_success_for_addr(bd_addr_t address, hci_con_handle_t handle){
666     linked_item_t *it;
667     for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){
668         l2cap_channel_t * channel = (l2cap_channel_t *) it;
669         if ( ! BD_ADDR_CMP( channel->address, address) ){
670             if (channel->state == L2CAP_STATE_WAIT_CONNECTION_COMPLETE || channel->state == L2CAP_STATE_WILL_SEND_CREATE_CONNECTION) {
671                 // success, start l2cap handshake
672                 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST;
673                 channel->handle = handle;
674                 channel->local_cid = l2cap_next_local_cid();
675             }
676         }
677     }
678     // process
679     l2cap_run();
680 }
681 
682 void l2cap_event_handler( uint8_t *packet, uint16_t size ){
683 
684     bd_addr_t address;
685     hci_con_handle_t handle;
686     l2cap_channel_t * channel;
687     linked_item_t *it;
688     int hci_con_used;
689 
690     switch(packet[0]){
691 
692         // handle connection complete events
693         case HCI_EVENT_CONNECTION_COMPLETE:
694             bt_flip_addr(address, &packet[5]);
695             if (packet[2] == 0){
696                 handle = READ_BT_16(packet, 3);
697                 l2cap_handle_connection_success_for_addr(address, handle);
698             } else {
699                 l2cap_handle_connection_failed_for_addr(address, packet[2]);
700             }
701             break;
702 
703         // handle successful create connection cancel command
704         case HCI_EVENT_COMMAND_COMPLETE:
705             if ( COMMAND_COMPLETE_EVENT(packet, hci_create_connection_cancel) ) {
706                 if (packet[5] == 0){
707                     bt_flip_addr(address, &packet[6]);
708                     // CONNECTION TERMINATED BY LOCAL HOST (0X16)
709                     l2cap_handle_connection_failed_for_addr(address, 0x16);
710                 }
711             }
712             l2cap_run();    // try sending signaling packets first
713             break;
714 
715         case HCI_EVENT_COMMAND_STATUS:
716             l2cap_run();    // try sending signaling packets first
717             break;
718 
719         // handle disconnection complete events
720         case HCI_EVENT_DISCONNECTION_COMPLETE:
721             // send l2cap disconnect events for all channels on this handle
722             handle = READ_BT_16(packet, 3);
723             it = (linked_item_t *) &l2cap_channels;
724             while (it->next){
725                 l2cap_channel_t * channel = (l2cap_channel_t *) it->next;
726                 if ( channel->handle == handle ){
727                     // update prev item before free'ing next element - don't call l2cap_finalize_channel_close
728                     it->next = it->next->next;
729                     l2cap_emit_channel_closed(channel);
730                     btstack_memory_l2cap_channel_free(channel);
731                 } else {
732                     it = it->next;
733                 }
734             }
735             break;
736 
737         case HCI_EVENT_NUMBER_OF_COMPLETED_PACKETS:
738             l2cap_run();    // try sending signaling packets first
739             l2cap_hand_out_credits();
740             break;
741 
742         // HCI Connection Timeouts
743         case L2CAP_EVENT_TIMEOUT_CHECK:
744             handle = READ_BT_16(packet, 2);
745             if (hci_authentication_active_for_handle(handle)) break;
746             hci_con_used = 0;
747             for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){
748                 channel = (l2cap_channel_t *) it;
749                 if (channel->handle == handle) {
750                     hci_con_used = 1;
751                 }
752             }
753             if (hci_con_used) break;
754             if (!hci_can_send_packet_now(HCI_COMMAND_DATA_PACKET)) break;
755             hci_send_cmd(&hci_disconnect, handle, 0x13); // remote closed connection
756             break;
757 
758         case DAEMON_EVENT_HCI_PACKET_SENT:
759             for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){
760                 channel = (l2cap_channel_t *) it;
761                 if (channel->packet_handler) {
762                     (* (channel->packet_handler))(HCI_EVENT_PACKET, channel->local_cid, packet, size);
763                 }
764             }
765             if (attribute_protocol_packet_handler) {
766                 (*attribute_protocol_packet_handler)(HCI_EVENT_PACKET, 0, packet, size);
767             }
768             if (security_protocol_packet_handler) {
769                 (*security_protocol_packet_handler)(HCI_EVENT_PACKET, 0, packet, size);
770             }
771             break;
772 
773         default:
774             break;
775     }
776 
777     // pass on
778     (*packet_handler)(NULL, HCI_EVENT_PACKET, 0, packet, size);
779 }
780 
781 static void l2cap_handle_disconnect_request(l2cap_channel_t *channel, uint16_t identifier){
782     channel->remote_sig_id = identifier;
783     channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE;
784     l2cap_run();
785 }
786 
787 static void l2cap_register_signaling_response(hci_con_handle_t handle, uint8_t code, uint8_t sig_id, uint16_t data){
788     // Vol 3, Part A, 4.3: "The DCID and SCID fields shall be ignored when the result field indi- cates the connection was refused."
789     if (signaling_responses_pending < NR_PENDING_SIGNALING_RESPONSES) {
790         signaling_responses[signaling_responses_pending].handle = handle;
791         signaling_responses[signaling_responses_pending].code = code;
792         signaling_responses[signaling_responses_pending].sig_id = sig_id;
793         signaling_responses[signaling_responses_pending].data = data;
794         signaling_responses_pending++;
795         l2cap_run();
796     }
797 }
798 
799 static void l2cap_handle_connection_request(hci_con_handle_t handle, uint8_t sig_id, uint16_t psm, uint16_t source_cid){
800 
801     // log_info("l2cap_handle_connection_request for handle %u, psm %u cid 0x%02x\n", handle, psm, source_cid);
802     l2cap_service_t *service = l2cap_get_service(psm);
803     if (!service) {
804         // 0x0002 PSM not supported
805         l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, 0x0002);
806         return;
807     }
808 
809     hci_connection_t * hci_connection = hci_connection_for_handle( handle );
810     if (!hci_connection) {
811         //
812         log_error("no hci_connection for handle %u\n", handle);
813         return;
814     }
815 
816     // reject connection (0x03 security block) and disconnect if both have SSP, connection is not encrypted and PSM != SDP
817     if (psm != PSM_SDP
818         && hci_local_ssp_activated()
819         && hci_remote_ssp_supported(handle)
820         && gap_security_level(handle) == LEVEL_0){
821 
822         // 0x0003 Security Block
823         l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, 0x0003);
824         return;
825     }
826 
827 
828     // alloc structure
829     // log_info("l2cap_handle_connection_request register channel\n");
830     l2cap_channel_t * channel = (l2cap_channel_t*) btstack_memory_l2cap_channel_get();
831     if (!channel){
832         // 0x0004 No resources available
833         l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, 0x0004);
834         return;
835     }
836 
837     // fill in
838     BD_ADDR_COPY(channel->address, hci_connection->address);
839     channel->psm = psm;
840     channel->handle = handle;
841     channel->connection = service->connection;
842     channel->packet_handler = service->packet_handler;
843     channel->local_cid  = l2cap_next_local_cid();
844     channel->remote_cid = source_cid;
845     channel->local_mtu  = service->mtu;
846     channel->remote_mtu = L2CAP_DEFAULT_MTU;
847     channel->packets_granted = 0;
848     channel->remote_sig_id = sig_id;
849 
850     // limit local mtu to max acl packet length
851     if (channel->local_mtu > l2cap_max_mtu()) {
852         channel->local_mtu = l2cap_max_mtu();
853     }
854 
855     // set initial state
856     channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT;
857     channel->state_var = L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND;
858 
859     // add to connections list
860     linked_list_add(&l2cap_channels, (linked_item_t *) channel);
861 
862     // check security requirements
863     // gap_security_level_t current_level  = gap_security_level(handle);
864     // gap_security_level_t required_level = LEVEL_2;
865     // if (current_level < required_level){
866     //     channel->state = L2CAP_STATE_WAIT_AUTHENTICATION_RESULT;
867     //     gap_request_security_level(handle, required_level);
868     //     return;
869     // }
870 
871     // emit incoming connection request
872     l2cap_emit_connection_request(channel);
873 }
874 
875 void l2cap_accept_connection_internal(uint16_t local_cid){
876     log_info("L2CAP_ACCEPT_CONNECTION local_cid 0x%x", local_cid);
877     l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid);
878     if (!channel) {
879         log_error("l2cap_accept_connection_internal called but local_cid 0x%x not found", local_cid);
880         return;
881     }
882 
883     channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT;
884 
885     // process
886     l2cap_run();
887 }
888 
889 void l2cap_decline_connection_internal(uint16_t local_cid, uint8_t reason){
890     log_info("L2CAP_DECLINE_CONNECTION local_cid 0x%x, reason %x", local_cid, reason);
891     l2cap_channel_t * channel = l2cap_get_channel_for_local_cid( local_cid);
892     if (!channel) {
893         log_error( "l2cap_decline_connection_internal called but local_cid 0x%x not found", local_cid);
894         return;
895     }
896     channel->state  = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE;
897     channel->reason = reason;
898     l2cap_run();
899 }
900 
901 void l2cap_signaling_handle_configure_request(l2cap_channel_t *channel, uint8_t *command){
902 
903     channel->remote_sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET];
904 
905     uint16_t flags = READ_BT_16(command, 6);
906     if (flags & 1) {
907         channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT);
908     }
909 
910     // accept the other's configuration options
911     uint16_t end_pos = 4 + READ_BT_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET);
912     uint16_t pos     = 8;
913     while (pos < end_pos){
914         uint8_t option_hint = command[pos] >> 7;
915         uint8_t option_type = command[pos] & 0x7f;
916         log_info("l2cap cid %u, hint %u, type %u", channel->local_cid, option_hint, option_type);
917         pos++;
918         uint8_t length = command[pos++];
919         // MTU { type(8): 1, len(8):2, MTU(16) }
920         if (option_type == 1 && length == 2){
921             channel->remote_mtu = READ_BT_16(command, pos);
922             // log_info("l2cap cid 0x%02x, remote mtu %u\n", channel->local_cid, channel->remote_mtu);
923             channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU);
924         }
925         // Flush timeout { type(8):2, len(8): 2, Flush Timeout(16)}
926         if (option_type == 2 && length == 2){
927             channel->flush_timeout = READ_BT_16(command, pos);
928         }
929         // check for unknown options
930         if (option_hint == 0 && (option_type == 0 || option_type >= 0x07)){
931             log_info("l2cap cid %u, unknown options", channel->local_cid);
932             channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID);
933         }
934         pos += length;
935     }
936 }
937 
938 static int l2cap_channel_ready_for_open(l2cap_channel_t *channel){
939     // log_info("l2cap_channel_ready_for_open 0x%02x\n", channel->state_var);
940     if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP) == 0) return 0;
941     if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP) == 0) return 0;
942     return 1;
943 }
944 
945 
946 void l2cap_signaling_handler_channel(l2cap_channel_t *channel, uint8_t *command){
947 
948     uint8_t  code       = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET];
949     uint8_t  identifier = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET];
950     uint16_t result = 0;
951 
952     log_info("L2CAP signaling handler code %u, state %u\n", code, channel->state);
953 
954     // handle DISCONNECT REQUESTS seperately
955     if (code == DISCONNECTION_REQUEST){
956         switch (channel->state){
957             case L2CAP_STATE_CONFIG:
958             case L2CAP_STATE_OPEN:
959             case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST:
960             case L2CAP_STATE_WAIT_DISCONNECT:
961                 l2cap_handle_disconnect_request(channel, identifier);
962                 break;
963 
964             default:
965                 // ignore in other states
966                 break;
967         }
968         return;
969     }
970 
971     // @STATEMACHINE(l2cap)
972     switch (channel->state) {
973 
974         case L2CAP_STATE_WAIT_CONNECT_RSP:
975             switch (code){
976                 case CONNECTION_RESPONSE:
977                     l2cap_stop_rtx(channel);
978                     result = READ_BT_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4);
979                     switch (result) {
980                         case 0:
981                             // successful connection
982                             channel->remote_cid = READ_BT_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET);
983                             channel->state = L2CAP_STATE_CONFIG;
984                             channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ);
985                             break;
986                         case 1:
987                             // connection pending. get some coffee, but start the ERTX
988                             l2cap_start_ertx(channel);
989                             break;
990                         default:
991                             // channel closed
992                             channel->state = L2CAP_STATE_CLOSED;
993                             // map l2cap connection response result to BTstack status enumeration
994                             l2cap_emit_channel_opened(channel, L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result);
995 
996                             // drop link key if security block
997                             if (L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result == L2CAP_CONNECTION_RESPONSE_RESULT_REFUSED_SECURITY){
998                                 hci_drop_link_key_for_bd_addr(&channel->address);
999                             }
1000 
1001                             // discard channel
1002                             linked_list_remove(&l2cap_channels, (linked_item_t *) channel);
1003                             btstack_memory_l2cap_channel_free(channel);
1004                             break;
1005                     }
1006                     break;
1007 
1008                 default:
1009                     //@TODO: implement other signaling packets
1010                     break;
1011             }
1012             break;
1013 
1014         case L2CAP_STATE_CONFIG:
1015             result = READ_BT_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4);
1016             switch (code) {
1017                 case CONFIGURE_REQUEST:
1018                     channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP);
1019                     l2cap_signaling_handle_configure_request(channel, command);
1020                     if (!(channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT)){
1021                         // only done if continuation not set
1022                         channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_REQ);
1023                     }
1024                     break;
1025                 case CONFIGURE_RESPONSE:
1026                     l2cap_stop_rtx(channel);
1027                     switch (result){
1028                         case 0: // success
1029                             channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP);
1030                             break;
1031                         case 4: // pending
1032                             l2cap_start_ertx(channel);
1033                             break;
1034                         default:
1035                             // retry on negative result
1036                             channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ);
1037                             break;
1038                     }
1039                     break;
1040                 default:
1041                     break;
1042             }
1043             if (l2cap_channel_ready_for_open(channel)){
1044                 // for open:
1045                 channel->state = L2CAP_STATE_OPEN;
1046                 l2cap_emit_channel_opened(channel, 0);
1047                 l2cap_emit_credits(channel, 1);
1048             }
1049             break;
1050 
1051         case L2CAP_STATE_WAIT_DISCONNECT:
1052             switch (code) {
1053                 case DISCONNECTION_RESPONSE:
1054                     l2cap_finialize_channel_close(channel);
1055                     break;
1056                 default:
1057                     //@TODO: implement other signaling packets
1058                     break;
1059             }
1060             break;
1061 
1062         case L2CAP_STATE_CLOSED:
1063             // @TODO handle incoming requests
1064             break;
1065 
1066         case L2CAP_STATE_OPEN:
1067             //@TODO: implement other signaling packets, e.g. re-configure
1068             break;
1069         default:
1070             break;
1071     }
1072     // log_info("new state %u\n", channel->state);
1073 }
1074 
1075 
1076 void l2cap_signaling_handler_dispatch( hci_con_handle_t handle, uint8_t * command){
1077 
1078     // get code, signalind identifier and command len
1079     uint8_t code   = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET];
1080     uint8_t sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET];
1081 
1082     // not for a particular channel, and not CONNECTION_REQUEST, ECHO_[REQUEST|RESPONSE], INFORMATION_REQUEST
1083     if (code < 1 || code == ECHO_RESPONSE || code > INFORMATION_REQUEST){
1084         l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, L2CAP_REJ_CMD_UNKNOWN);
1085         return;
1086     }
1087 
1088     // general commands without an assigned channel
1089     switch(code) {
1090 
1091         case CONNECTION_REQUEST: {
1092             uint16_t psm =        READ_BT_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET);
1093             uint16_t source_cid = READ_BT_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2);
1094             l2cap_handle_connection_request(handle, sig_id, psm, source_cid);
1095             return;
1096         }
1097 
1098         case ECHO_REQUEST:
1099             l2cap_register_signaling_response(handle, code, sig_id, 0);
1100             return;
1101 
1102         case INFORMATION_REQUEST: {
1103             uint16_t infoType = READ_BT_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET);
1104             l2cap_register_signaling_response(handle, code, sig_id, infoType);
1105             return;
1106         }
1107 
1108         default:
1109             break;
1110     }
1111 
1112 
1113     // Get potential destination CID
1114     uint16_t dest_cid = READ_BT_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET);
1115 
1116     // Find channel for this sig_id and connection handle
1117     linked_item_t *it;
1118     for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){
1119         l2cap_channel_t * channel = (l2cap_channel_t *) it;
1120         if (channel->handle == handle) {
1121             if (code & 1) {
1122                 // match odd commands (responses) by previous signaling identifier
1123                 if (channel->local_sig_id == sig_id) {
1124                     l2cap_signaling_handler_channel(channel, command);
1125                     break;
1126                 }
1127             } else {
1128                 // match even commands (requests) by local channel id
1129                 if (channel->local_cid == dest_cid) {
1130                     l2cap_signaling_handler_channel(channel, command);
1131                     break;
1132                 }
1133             }
1134         }
1135     }
1136 }
1137 
1138 void l2cap_acl_handler( uint8_t *packet, uint16_t size ){
1139 
1140     // Get Channel ID
1141     uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet);
1142     hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet);
1143 
1144     switch (channel_id) {
1145 
1146         case L2CAP_CID_SIGNALING: {
1147 
1148             uint16_t command_offset = 8;
1149             while (command_offset < size) {
1150 
1151                 // handle signaling commands
1152                 l2cap_signaling_handler_dispatch(handle, &packet[command_offset]);
1153 
1154                 // increment command_offset
1155                 command_offset += L2CAP_SIGNALING_COMMAND_DATA_OFFSET + READ_BT_16(packet, command_offset + L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET);
1156             }
1157             break;
1158         }
1159 
1160         case L2CAP_CID_ATTRIBUTE_PROTOCOL:
1161             if (attribute_protocol_packet_handler) {
1162                 (*attribute_protocol_packet_handler)(ATT_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER);
1163             }
1164             break;
1165 
1166         case L2CAP_CID_SECURITY_MANAGER_PROTOCOL:
1167             if (security_protocol_packet_handler) {
1168                 (*security_protocol_packet_handler)(SM_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER);
1169             }
1170             break;
1171 
1172         default: {
1173             // Find channel for this channel_id and connection handle
1174             l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(channel_id);
1175             if (channel) {
1176                 l2cap_dispatch(channel, L2CAP_DATA_PACKET, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER);
1177             }
1178             break;
1179         }
1180     }
1181 
1182     l2cap_run();
1183 }
1184 
1185 static void l2cap_packet_handler(uint8_t packet_type, uint8_t *packet, uint16_t size){
1186     switch (packet_type) {
1187         case HCI_EVENT_PACKET:
1188             l2cap_event_handler(packet, size);
1189             break;
1190         case HCI_ACL_DATA_PACKET:
1191             l2cap_acl_handler(packet, size);
1192             break;
1193         default:
1194             break;
1195     }
1196 }
1197 
1198 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE
1199 void l2cap_finialize_channel_close(l2cap_channel_t *channel){
1200     channel->state = L2CAP_STATE_CLOSED;
1201     l2cap_emit_channel_closed(channel);
1202     // discard channel
1203     linked_list_remove(&l2cap_channels, (linked_item_t *) channel);
1204     btstack_memory_l2cap_channel_free(channel);
1205 }
1206 
1207 l2cap_service_t * l2cap_get_service(uint16_t psm){
1208     linked_item_t *it;
1209 
1210     // close open channels
1211     for (it = (linked_item_t *) l2cap_services; it ; it = it->next){
1212         l2cap_service_t * service = ((l2cap_service_t *) it);
1213         if ( service->psm == psm){
1214             return service;
1215         };
1216     }
1217     return NULL;
1218 }
1219 
1220 void l2cap_register_service_internal(void *connection, btstack_packet_handler_t packet_handler, uint16_t psm, uint16_t mtu){
1221 
1222     log_info("L2CAP_REGISTER_SERVICE psm 0x%x mtu %u", psm, mtu);
1223 
1224     // check for alread registered psm
1225     // TODO: emit error event
1226     l2cap_service_t *service = l2cap_get_service(psm);
1227     if (service) {
1228         log_error("l2cap_register_service_internal: PSM %u already registered\n", psm);
1229         l2cap_emit_service_registered(connection, L2CAP_SERVICE_ALREADY_REGISTERED, psm);
1230         return;
1231     }
1232 
1233     // alloc structure
1234     // TODO: emit error event
1235     service = (l2cap_service_t *) btstack_memory_l2cap_service_get();
1236     if (!service) {
1237         log_error("l2cap_register_service_internal: no memory for l2cap_service_t\n");
1238         l2cap_emit_service_registered(connection, BTSTACK_MEMORY_ALLOC_FAILED, psm);
1239         return;
1240     }
1241 
1242     // fill in
1243     service->psm = psm;
1244     service->mtu = mtu;
1245     service->connection = connection;
1246     service->packet_handler = packet_handler;
1247 
1248     // add to services list
1249     linked_list_add(&l2cap_services, (linked_item_t *) service);
1250 
1251     // enable page scan
1252     hci_connectable_control(1);
1253 
1254     // done
1255     l2cap_emit_service_registered(connection, 0, psm);
1256 }
1257 
1258 void l2cap_unregister_service_internal(void *connection, uint16_t psm){
1259 
1260     log_info("L2CAP_UNREGISTER_SERVICE psm 0x%x", psm);
1261 
1262     l2cap_service_t *service = l2cap_get_service(psm);
1263     if (!service) return;
1264     linked_list_remove(&l2cap_services, (linked_item_t *) service);
1265     btstack_memory_l2cap_service_free(service);
1266 
1267     // disable page scan when no services registered
1268     if (!linked_list_empty(&l2cap_services)) return;
1269     hci_connectable_control(0);
1270 }
1271 
1272 //
1273 void l2cap_close_connection(void *connection){
1274     linked_item_t *it;
1275 
1276     // close open channels - note to myself: no channel is freed, so no new for fancy iterator tricks
1277     l2cap_channel_t * channel;
1278     for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){
1279         channel = (l2cap_channel_t *) it;
1280         if (channel->connection == connection) {
1281             channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST;
1282         }
1283     }
1284 
1285     // unregister services
1286     it = (linked_item_t *) &l2cap_services;
1287     while (it->next) {
1288         l2cap_service_t * service = (l2cap_service_t *) it->next;
1289         if (service->connection == connection){
1290             it->next = it->next->next;
1291             btstack_memory_l2cap_service_free(service);
1292         } else {
1293             it = it->next;
1294         }
1295     }
1296 
1297     // process
1298     l2cap_run();
1299 }
1300 
1301 // Bluetooth 4.0 - allows to register handler for Attribute Protocol and Security Manager Protocol
1302 void l2cap_register_fixed_channel(btstack_packet_handler_t packet_handler, uint16_t channel_id) {
1303     switch(channel_id){
1304         case L2CAP_CID_ATTRIBUTE_PROTOCOL:
1305             attribute_protocol_packet_handler = packet_handler;
1306             break;
1307         case L2CAP_CID_SECURITY_MANAGER_PROTOCOL:
1308             security_protocol_packet_handler = packet_handler;
1309             break;
1310     }
1311 }
1312 
1313 #ifdef HAVE_BLE
1314 // Request LE connection parameter update
1315 int l2cap_le_request_connection_parameter_update(uint16_t handle, uint16_t interval_min, uint16_t interval_max, uint16_t slave_latency, uint16_t timeout_multiplier){
1316     if (!hci_can_send_packet_now(HCI_ACL_DATA_PACKET)){
1317         log_info("l2cap_send_signaling_packet, cannot send\n");
1318         return BTSTACK_ACL_BUFFERS_FULL;
1319     }
1320     // log_info("l2cap_send_signaling_packet type %u\n", cmd);
1321     uint8_t *acl_buffer = hci_get_outgoing_acl_packet_buffer();
1322     uint16_t len = l2cap_le_create_connection_parameter_update_request(acl_buffer, handle, interval_min, interval_max, slave_latency, timeout_multiplier);
1323     return hci_send_acl_packet(acl_buffer, len);
1324 }
1325 #endif
1326 
1327