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