xref: /btstack/src/l2cap.c (revision 02b22dc4680e8d64c8c5643adb1e29cd48045e00)
143625864Smatthias.ringwald /*
21713bceaSmatthias.ringwald  * Copyright (C) 2009 by Matthias Ringwald
31713bceaSmatthias.ringwald  *
41713bceaSmatthias.ringwald  * Redistribution and use in source and binary forms, with or without
51713bceaSmatthias.ringwald  * modification, are permitted provided that the following conditions
61713bceaSmatthias.ringwald  * are met:
71713bceaSmatthias.ringwald  *
81713bceaSmatthias.ringwald  * 1. Redistributions of source code must retain the above copyright
91713bceaSmatthias.ringwald  *    notice, this list of conditions and the following disclaimer.
101713bceaSmatthias.ringwald  * 2. Redistributions in binary form must reproduce the above copyright
111713bceaSmatthias.ringwald  *    notice, this list of conditions and the following disclaimer in the
121713bceaSmatthias.ringwald  *    documentation and/or other materials provided with the distribution.
131713bceaSmatthias.ringwald  * 3. Neither the name of the copyright holders nor the names of
141713bceaSmatthias.ringwald  *    contributors may be used to endorse or promote products derived
151713bceaSmatthias.ringwald  *    from this software without specific prior written permission.
161713bceaSmatthias.ringwald  *
171713bceaSmatthias.ringwald  * THIS SOFTWARE IS PROVIDED BY MATTHIAS RINGWALD AND CONTRIBUTORS
181713bceaSmatthias.ringwald  * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
191713bceaSmatthias.ringwald  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
201713bceaSmatthias.ringwald  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS
211713bceaSmatthias.ringwald  * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
221713bceaSmatthias.ringwald  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
231713bceaSmatthias.ringwald  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
241713bceaSmatthias.ringwald  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
251713bceaSmatthias.ringwald  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
261713bceaSmatthias.ringwald  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
271713bceaSmatthias.ringwald  * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
281713bceaSmatthias.ringwald  * SUCH DAMAGE.
291713bceaSmatthias.ringwald  *
301713bceaSmatthias.ringwald  */
311713bceaSmatthias.ringwald 
321713bceaSmatthias.ringwald /*
3343625864Smatthias.ringwald  *  l2cap.c
3443625864Smatthias.ringwald  *
3543625864Smatthias.ringwald  *  Logical Link Control and Adaption Protocl (L2CAP)
3643625864Smatthias.ringwald  *
3743625864Smatthias.ringwald  *  Created by Matthias Ringwald on 5/16/09.
3843625864Smatthias.ringwald  */
3943625864Smatthias.ringwald 
4043625864Smatthias.ringwald #include "l2cap.h"
41645658c9Smatthias.ringwald #include "hci.h"
422b3c6c9bSmatthias.ringwald #include "hci_dump.h"
436218e6f1Smatthias.ringwald #include "debug.h"
4443625864Smatthias.ringwald 
4543625864Smatthias.ringwald #include <stdarg.h>
4643625864Smatthias.ringwald #include <string.h>
4743625864Smatthias.ringwald 
4843625864Smatthias.ringwald #include <stdio.h>
4943625864Smatthias.ringwald 
506f60b3f4Smatthias.ringwald // size of HCI ACL + L2CAP Header for regular data packets
516f60b3f4Smatthias.ringwald #define COMPLETE_L2CAP_HEADER 8
526f60b3f4Smatthias.ringwald 
532784b77dSmatthias.ringwald // minimum signaling MTU
542784b77dSmatthias.ringwald #define L2CAP_MINIMAL_MTU 48
551dc511deSmatthias.ringwald #define L2CAP_DEFAULT_MTU 672
562784b77dSmatthias.ringwald 
574c744e21Smatthias.ringwald // nr of buffered acl packets in outgoing queue to get max performance
584c744e21Smatthias.ringwald #define NR_BUFFERED_ACL_PACKETS 3
594c744e21Smatthias.ringwald 
6000d93d79Smatthias.ringwald // offsets for L2CAP SIGNALING COMMANDS
6100d93d79Smatthias.ringwald #define L2CAP_SIGNALING_COMMAND_CODE_OFFSET   0
6200d93d79Smatthias.ringwald #define L2CAP_SIGNALING_COMMAND_SIGID_OFFSET  1
6300d93d79Smatthias.ringwald #define L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET 2
6400d93d79Smatthias.ringwald #define L2CAP_SIGNALING_COMMAND_DATA_OFFSET   4
6500d93d79Smatthias.ringwald 
6636944dffSmatthias.ringwald static void null_packet_handler(void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size);
672718e2e7Smatthias.ringwald static void l2cap_packet_handler(uint8_t packet_type, uint8_t *packet, uint16_t size);
68fcadd0caSmatthias.ringwald 
692b83fb7dSmatthias.ringwald // used to cache l2cap rejects, echo, and informational requests
702b83fb7dSmatthias.ringwald #define NR_PENDING_SIGNALING_RESPONSES 10
712b83fb7dSmatthias.ringwald typedef struct l2cap_signaling_response {
722b83fb7dSmatthias.ringwald     hci_con_handle_t handle;
732b83fb7dSmatthias.ringwald     uint8_t  sig_id;
742b83fb7dSmatthias.ringwald     uint8_t  code;
752b83fb7dSmatthias.ringwald     uint16_t infoType;
762b83fb7dSmatthias.ringwald } l2cap_signaling_response_t;
772b83fb7dSmatthias.ringwald static l2cap_signaling_response_t signaling_responses[NR_PENDING_SIGNALING_RESPONSES];
782b83fb7dSmatthias.ringwald static int signaling_responses_pending;
792b83fb7dSmatthias.ringwald 
801e6aba47Smatthias.ringwald static uint8_t * sig_buffer = NULL;
811e6aba47Smatthias.ringwald static linked_list_t l2cap_channels = NULL;
829d9bbc01Smatthias.ringwald static linked_list_t l2cap_services = NULL;
831e6aba47Smatthias.ringwald static uint8_t * acl_buffer = NULL;
8436944dffSmatthias.ringwald static void (*packet_handler) (void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) = null_packet_handler;
85808a48abSmatthias.ringwald static int new_credits_blocked = 0;
861e6aba47Smatthias.ringwald 
871e6aba47Smatthias.ringwald void l2cap_init(){
882784b77dSmatthias.ringwald     sig_buffer = malloc( L2CAP_MINIMAL_MTU );
8911c41d51Smatthias.ringwald     acl_buffer = malloc( HCI_ACL_3DH5_SIZE);
90fcadd0caSmatthias.ringwald 
91808a48abSmatthias.ringwald     new_credits_blocked = 0;
922b83fb7dSmatthias.ringwald     signaling_responses_pending = 0;
93808a48abSmatthias.ringwald 
94fcadd0caSmatthias.ringwald     //
952718e2e7Smatthias.ringwald     // register callback with HCI
96fcadd0caSmatthias.ringwald     //
972718e2e7Smatthias.ringwald     hci_register_packet_handler(&l2cap_packet_handler);
98fcadd0caSmatthias.ringwald }
99fcadd0caSmatthias.ringwald 
100fcadd0caSmatthias.ringwald 
101fcadd0caSmatthias.ringwald /** Register L2CAP packet handlers */
10236944dffSmatthias.ringwald static void null_packet_handler(void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
103fcadd0caSmatthias.ringwald }
10436944dffSmatthias.ringwald void l2cap_register_packet_handler(void (*handler)(void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size)){
105b502e1b0Smatthias.ringwald     packet_handler = handler;
1061e6aba47Smatthias.ringwald }
1071e6aba47Smatthias.ringwald 
10858de5610Smatthias.ringwald //  notify client/protocol handler
10958de5610Smatthias.ringwald void l2cap_dispatch(l2cap_channel_t *channel, uint8_t type, uint8_t * data, uint16_t size){
11058de5610Smatthias.ringwald     if (channel->packet_handler) {
11158de5610Smatthias.ringwald         (* (channel->packet_handler))(type, channel->local_cid, data, size);
11258de5610Smatthias.ringwald     } else {
11336944dffSmatthias.ringwald         (*packet_handler)(channel->connection, type, channel->local_cid, data, size);
11458de5610Smatthias.ringwald     }
11558de5610Smatthias.ringwald }
11658de5610Smatthias.ringwald 
11758de5610Smatthias.ringwald void l2cap_emit_channel_opened(l2cap_channel_t *channel, uint8_t status) {
1189893b714Smatthias.ringwald     uint8_t event[21];
11958de5610Smatthias.ringwald     event[0] = L2CAP_EVENT_CHANNEL_OPENED;
12058de5610Smatthias.ringwald     event[1] = sizeof(event) - 2;
12158de5610Smatthias.ringwald     event[2] = status;
12258de5610Smatthias.ringwald     bt_flip_addr(&event[3], channel->address);
12358de5610Smatthias.ringwald     bt_store_16(event,  9, channel->handle);
12458de5610Smatthias.ringwald     bt_store_16(event, 11, channel->psm);
12558de5610Smatthias.ringwald     bt_store_16(event, 13, channel->local_cid);
12658de5610Smatthias.ringwald     bt_store_16(event, 15, channel->remote_cid);
1274c98aa43Smatthias.ringwald     bt_store_16(event, 17, channel->local_mtu);
1284c98aa43Smatthias.ringwald     bt_store_16(event, 19, channel->remote_mtu);
12958de5610Smatthias.ringwald     hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
13058de5610Smatthias.ringwald     l2cap_dispatch(channel, HCI_EVENT_PACKET, event, sizeof(event));
13158de5610Smatthias.ringwald }
13258de5610Smatthias.ringwald 
13358de5610Smatthias.ringwald void l2cap_emit_channel_closed(l2cap_channel_t *channel) {
13458de5610Smatthias.ringwald     uint8_t event[4];
13558de5610Smatthias.ringwald     event[0] = L2CAP_EVENT_CHANNEL_CLOSED;
13658de5610Smatthias.ringwald     event[1] = sizeof(event) - 2;
13758de5610Smatthias.ringwald     bt_store_16(event, 2, channel->local_cid);
13858de5610Smatthias.ringwald     hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
13958de5610Smatthias.ringwald     l2cap_dispatch(channel, HCI_EVENT_PACKET, event, sizeof(event));
14058de5610Smatthias.ringwald }
14158de5610Smatthias.ringwald 
14258de5610Smatthias.ringwald void l2cap_emit_connection_request(l2cap_channel_t *channel) {
14358de5610Smatthias.ringwald     uint8_t event[16];
14458de5610Smatthias.ringwald     event[0] = L2CAP_EVENT_INCOMING_CONNECTION;
14558de5610Smatthias.ringwald     event[1] = sizeof(event) - 2;
14658de5610Smatthias.ringwald     bt_flip_addr(&event[2], channel->address);
14758de5610Smatthias.ringwald     bt_store_16(event,  8, channel->handle);
14858de5610Smatthias.ringwald     bt_store_16(event, 10, channel->psm);
14958de5610Smatthias.ringwald     bt_store_16(event, 12, channel->local_cid);
15058de5610Smatthias.ringwald     bt_store_16(event, 14, channel->remote_cid);
15158de5610Smatthias.ringwald     hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
15258de5610Smatthias.ringwald     l2cap_dispatch(channel, HCI_EVENT_PACKET, event, sizeof(event));
1530af41d30Smatthias.ringwald }
154808a48abSmatthias.ringwald 
1556218e6f1Smatthias.ringwald void l2cap_emit_credits(l2cap_channel_t *channel, uint8_t credits) {
1566218e6f1Smatthias.ringwald     // track credits
1576218e6f1Smatthias.ringwald     channel->packets_granted += credits;
158808a48abSmatthias.ringwald     // log_dbg("l2cap_emit_credits for cid %u, credits given: %u (+%u)\n", channel->local_cid, channel->packets_granted, credits);
1596218e6f1Smatthias.ringwald 
1606218e6f1Smatthias.ringwald     uint8_t event[5];
1616218e6f1Smatthias.ringwald     event[0] = L2CAP_EVENT_CREDITS;
1626218e6f1Smatthias.ringwald     event[1] = sizeof(event) - 2;
1636218e6f1Smatthias.ringwald     bt_store_16(event, 2, channel->local_cid);
1646218e6f1Smatthias.ringwald     event[4] = credits;
1656218e6f1Smatthias.ringwald     hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
1666218e6f1Smatthias.ringwald     l2cap_dispatch(channel, HCI_EVENT_PACKET, event, sizeof(event));
1676218e6f1Smatthias.ringwald }
1686218e6f1Smatthias.ringwald 
169808a48abSmatthias.ringwald void l2cap_block_new_credits(uint8_t blocked){
170808a48abSmatthias.ringwald     new_credits_blocked = blocked;
171808a48abSmatthias.ringwald }
172808a48abSmatthias.ringwald 
17340d1c7a4Smatthias.ringwald void l2cap_hand_out_credits(void){
174808a48abSmatthias.ringwald 
175808a48abSmatthias.ringwald     if (new_credits_blocked) return;    // we're told not to. used by daemon
176808a48abSmatthias.ringwald 
1778d371091Smatthias.ringwald     linked_item_t *it;
1788d371091Smatthias.ringwald     for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){
1798d371091Smatthias.ringwald         if (!hci_number_free_acl_slots()) return;
1808d371091Smatthias.ringwald         l2cap_channel_t * channel = (l2cap_channel_t *) it;
1810e7bc007Smatthias.ringwald         if (channel->state != L2CAP_STATE_OPEN) continue;
1824c744e21Smatthias.ringwald         if (hci_number_outgoing_packets(channel->handle) < NR_BUFFERED_ACL_PACKETS && channel->packets_granted == 0) {
1838d371091Smatthias.ringwald             l2cap_emit_credits(channel, 1);
1848d371091Smatthias.ringwald         }
1858d371091Smatthias.ringwald     }
1868d371091Smatthias.ringwald }
1878d371091Smatthias.ringwald 
188b35f641cSmatthias.ringwald l2cap_channel_t * l2cap_get_channel_for_local_cid(uint16_t local_cid){
189f62db1e3Smatthias.ringwald     linked_item_t *it;
190f62db1e3Smatthias.ringwald     for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){
1918d371091Smatthias.ringwald         l2cap_channel_t * channel = (l2cap_channel_t *) it;
192b35f641cSmatthias.ringwald         if ( channel->local_cid == local_cid) {
193f62db1e3Smatthias.ringwald             return channel;
194f62db1e3Smatthias.ringwald         }
195f62db1e3Smatthias.ringwald     }
196f62db1e3Smatthias.ringwald     return NULL;
197f62db1e3Smatthias.ringwald }
198f62db1e3Smatthias.ringwald 
19996cbd662Smatthias.ringwald uint16_t l2cap_get_remote_mtu_for_local_cid(uint16_t local_cid){
20096cbd662Smatthias.ringwald     l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid);
20196cbd662Smatthias.ringwald     if (channel) {
20296cbd662Smatthias.ringwald         return channel->remote_mtu;
20396cbd662Smatthias.ringwald     }
20496cbd662Smatthias.ringwald     return 0;
20596cbd662Smatthias.ringwald }
20696cbd662Smatthias.ringwald 
20758de5610Smatthias.ringwald int l2cap_send_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, ...){
208fe35119dSmatthias.ringwald     // log_dbg("l2cap_send_signaling_packet type %u\n", cmd);
20958de5610Smatthias.ringwald     va_list argptr;
21058de5610Smatthias.ringwald     va_start(argptr, identifier);
21158de5610Smatthias.ringwald     uint16_t len = l2cap_create_signaling_internal(sig_buffer, handle, cmd, identifier, argptr);
21258de5610Smatthias.ringwald     va_end(argptr);
213808a48abSmatthias.ringwald     // log_dbg("l2cap_send_signaling_packet con %u!\n", handle);
21458de5610Smatthias.ringwald     return hci_send_acl_packet(sig_buffer, len);
21558de5610Smatthias.ringwald }
21658de5610Smatthias.ringwald 
2176218e6f1Smatthias.ringwald int l2cap_send_internal(uint16_t local_cid, uint8_t *data, uint16_t len){
2186218e6f1Smatthias.ringwald 
2196218e6f1Smatthias.ringwald     // check for free places on BT module
2208ea03fa5Smatthias.ringwald     if (!hci_number_free_acl_slots()) {
2218ea03fa5Smatthias.ringwald         log_dbg("l2cap_send_internal cid %u, BT module full <-----\n", local_cid);
222808a48abSmatthias.ringwald         return BTSTACK_ACL_BUFFERS_FULL;
2238ea03fa5Smatthias.ringwald     }
2246218e6f1Smatthias.ringwald     int err = 0;
2256218e6f1Smatthias.ringwald 
22658de5610Smatthias.ringwald     // find channel for local_cid, construct l2cap packet and send
22758de5610Smatthias.ringwald     l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid);
22858de5610Smatthias.ringwald     if (channel) {
2296218e6f1Smatthias.ringwald         if (channel->packets_granted > 0){
2306218e6f1Smatthias.ringwald             --channel->packets_granted;
231808a48abSmatthias.ringwald             // log_dbg("l2cap_send_internal cid %u, handle %u, 1 credit used, credits left %u;\n",
232808a48abSmatthias.ringwald             //        local_cid, channel->handle, channel->packets_granted);
2336218e6f1Smatthias.ringwald         } else {
2346218e6f1Smatthias.ringwald             log_err("l2cap_send_internal cid %u, no credits!\n", local_cid);
2356218e6f1Smatthias.ringwald         }
2366218e6f1Smatthias.ringwald 
23758de5610Smatthias.ringwald         // 0 - Connection handle : PB=10 : BC=00
23858de5610Smatthias.ringwald         bt_store_16(acl_buffer, 0, channel->handle | (2 << 12) | (0 << 14));
23958de5610Smatthias.ringwald         // 2 - ACL length
24058de5610Smatthias.ringwald         bt_store_16(acl_buffer, 2,  len + 4);
24158de5610Smatthias.ringwald         // 4 - L2CAP packet length
24258de5610Smatthias.ringwald         bt_store_16(acl_buffer, 4,  len + 0);
24358de5610Smatthias.ringwald         // 6 - L2CAP channel DEST
24458de5610Smatthias.ringwald         bt_store_16(acl_buffer, 6, channel->remote_cid);
24558de5610Smatthias.ringwald         // 8 - data
24658de5610Smatthias.ringwald         memcpy(&acl_buffer[8], data, len);
2476218e6f1Smatthias.ringwald 
24858de5610Smatthias.ringwald         // send
2496218e6f1Smatthias.ringwald         err = hci_send_acl_packet(acl_buffer, len+8);
25058de5610Smatthias.ringwald     }
25191b99603Smatthias.ringwald 
25291b99603Smatthias.ringwald     l2cap_hand_out_credits();
25391b99603Smatthias.ringwald 
2546218e6f1Smatthias.ringwald     return err;
25558de5610Smatthias.ringwald }
25658de5610Smatthias.ringwald 
2572cd0be45Smatthias.ringwald // process outstanding signaling tasks
2582cd0be45Smatthias.ringwald void l2cap_run(void){
2592b83fb7dSmatthias.ringwald 
2602b83fb7dSmatthias.ringwald 
2612b83fb7dSmatthias.ringwald     // check pending signaling responses
2622b83fb7dSmatthias.ringwald     while (signaling_responses_pending){
2632b83fb7dSmatthias.ringwald 
264*02b22dc4Smatthias.ringwald         if (!hci_can_send_packet_now(HCI_ACL_DATA_PACKET)) break;
2652b83fb7dSmatthias.ringwald 
2662b83fb7dSmatthias.ringwald         hci_con_handle_t handle = signaling_responses[0].handle;
2672b83fb7dSmatthias.ringwald         uint8_t sig_id = signaling_responses[0].sig_id;
2682b83fb7dSmatthias.ringwald         uint8_t infoType = signaling_responses[0].infoType;
2692b83fb7dSmatthias.ringwald 
2702b83fb7dSmatthias.ringwald         switch (signaling_responses[0].code){
2712b83fb7dSmatthias.ringwald             case ECHO_REQUEST:
2722b83fb7dSmatthias.ringwald                 l2cap_send_signaling_packet(handle, ECHO_RESPONSE, sig_id, 0, NULL);
2732b83fb7dSmatthias.ringwald                 break;
2742b83fb7dSmatthias.ringwald             case INFORMATION_REQUEST:
2752b83fb7dSmatthias.ringwald                 if (infoType == 2) {
2762b83fb7dSmatthias.ringwald                     uint32_t features = 0;
2772b83fb7dSmatthias.ringwald                     // extended features request supported, however no features present
2782b83fb7dSmatthias.ringwald                     l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 0, 4, &features);
2792b83fb7dSmatthias.ringwald                 } else {
2802b83fb7dSmatthias.ringwald                     // all other types are not supported
2812b83fb7dSmatthias.ringwald                     l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 1, 0, NULL);
2822b83fb7dSmatthias.ringwald                 }
2832b83fb7dSmatthias.ringwald                 break;
2842b83fb7dSmatthias.ringwald             default:
2852b83fb7dSmatthias.ringwald                 // should not happen
2862b83fb7dSmatthias.ringwald                 break;
2872b83fb7dSmatthias.ringwald         }
2882b83fb7dSmatthias.ringwald 
2892b83fb7dSmatthias.ringwald         // remove first item
2902b83fb7dSmatthias.ringwald         signaling_responses_pending--;
2912b83fb7dSmatthias.ringwald         int i;
2922b83fb7dSmatthias.ringwald         for (i=0; i < signaling_responses_pending; i++){
2932b83fb7dSmatthias.ringwald             signaling_responses[i] = signaling_responses[i+1];
2942b83fb7dSmatthias.ringwald         }
2952b83fb7dSmatthias.ringwald     }
2962b83fb7dSmatthias.ringwald 
297ae280e73Smatthias.ringwald     uint8_t  config_options[4];
2982cd0be45Smatthias.ringwald     linked_item_t *it;
2992cd0be45Smatthias.ringwald     for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){
3002cd0be45Smatthias.ringwald 
301*02b22dc4Smatthias.ringwald         if (!hci_can_send_packet_now(HCI_COMMAND_DATA_PACKET)) break;
302*02b22dc4Smatthias.ringwald         if (!hci_can_send_packet_now(HCI_ACL_DATA_PACKET)) break;
3032cd0be45Smatthias.ringwald 
3042cd0be45Smatthias.ringwald         l2cap_channel_t * channel = (l2cap_channel_t *) it;
3052cd0be45Smatthias.ringwald         switch (channel->state){
3062cd0be45Smatthias.ringwald 
307*02b22dc4Smatthias.ringwald             case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION:
308*02b22dc4Smatthias.ringwald                 // send connection request
309*02b22dc4Smatthias.ringwald                 // BD_ADDR, Packet_Type, Page_Scan_Repetition_Mode, Reserved, Clock_Offset, Allow_Role_Switch
310*02b22dc4Smatthias.ringwald                 hci_send_cmd(&hci_create_connection, channel->address, 0xcc18, 0, 0, 0, 1);
311*02b22dc4Smatthias.ringwald                 channel->state = L2CAP_STATE_WAIT_CONNECTION_COMPLETE;
312*02b22dc4Smatthias.ringwald                 break;
313*02b22dc4Smatthias.ringwald 
314e7ff783cSmatthias.ringwald             case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE:
315b1988dceSmatthias.ringwald                 l2cap_send_signaling_packet(channel->handle, CONNECTION_RESPONSE, channel->remote_sig_id, 0, 0, channel->reason, 0);
316e7ff783cSmatthias.ringwald                 // discard channel - l2cap_finialize_channel_close without sending l2cap close event
317e7ff783cSmatthias.ringwald                 linked_list_remove(&l2cap_channels, (linked_item_t *) channel);
318e7ff783cSmatthias.ringwald                 free (channel);
319e7ff783cSmatthias.ringwald                 break;
320e7ff783cSmatthias.ringwald 
321552d92a1Smatthias.ringwald             case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT:
322b1988dceSmatthias.ringwald                 l2cap_send_signaling_packet(channel->handle, CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid, 0, 0);
323552d92a1Smatthias.ringwald                 channel->state = L2CAP_STATE_WAIT_CONFIG_REQ_OR_SEND_CONFIG_REQ;
324552d92a1Smatthias.ringwald                 break;
325552d92a1Smatthias.ringwald 
3266fdcc387Smatthias.ringwald             case L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST:
3276fdcc387Smatthias.ringwald                 // success, start l2cap handshake
328b1988dceSmatthias.ringwald                 channel->local_sig_id = l2cap_next_sig_id();
329b1988dceSmatthias.ringwald                 l2cap_send_signaling_packet( channel->handle, CONNECTION_REQUEST, channel->local_sig_id, channel->psm, channel->local_cid);
3306fdcc387Smatthias.ringwald                 channel->state = L2CAP_STATE_WAIT_CONNECT_RSP;
3316fdcc387Smatthias.ringwald                 break;
3326fdcc387Smatthias.ringwald 
333ae280e73Smatthias.ringwald             case L2CAP_STATE_WAIT_CONFIG_REQ_OR_SEND_CONFIG_REQ:
334ae280e73Smatthias.ringwald                 // after connection response was received
335b1988dceSmatthias.ringwald                 channel->local_sig_id = l2cap_next_sig_id();
336ae280e73Smatthias.ringwald                 config_options[0] = 1; // MTU
337ae280e73Smatthias.ringwald                 config_options[1] = 2; // len param
338ae280e73Smatthias.ringwald                 bt_store_16( (uint8_t*)&config_options, 2, channel->local_mtu);
339b1988dceSmatthias.ringwald                 l2cap_send_signaling_packet(channel->handle, CONFIGURE_REQUEST, channel->local_sig_id, channel->remote_cid, 0, 4, &config_options);
340ae280e73Smatthias.ringwald                 channel->state = L2CAP_STATE_WAIT_CONFIG_REQ_RSP_OR_CONFIG_REQ;
341ae280e73Smatthias.ringwald                 break;
342ae280e73Smatthias.ringwald 
343b1988dceSmatthias.ringwald             case L2CAP_STATE_WILL_SEND_CONFIG_REQ:
344b1988dceSmatthias.ringwald                 channel->local_sig_id = l2cap_next_sig_id();
345b1988dceSmatthias.ringwald                 config_options[0] = 1; // MTU
346b1988dceSmatthias.ringwald                 config_options[1] = 2; // len param
347b1988dceSmatthias.ringwald                 bt_store_16( (uint8_t*)&config_options, 2, channel->local_mtu);
348b1988dceSmatthias.ringwald                 l2cap_send_signaling_packet(channel->handle, CONFIGURE_REQUEST, channel->local_sig_id, channel->remote_cid, 0, 4, &config_options);
349b1988dceSmatthias.ringwald                 channel->state = L2CAP_STATE_WAIT_CONFIG_REQ_RSP;
350b1988dceSmatthias.ringwald                 break;
351b1988dceSmatthias.ringwald 
352552d92a1Smatthias.ringwald             case L2CAP_STATE_WAIT_CONFIG_REQ_RSP_OR_WILL_SEND_CONFIG_REQ_RSP:
353b1988dceSmatthias.ringwald                 l2cap_send_signaling_packet(channel->handle, CONFIGURE_RESPONSE, channel->remote_sig_id, channel->remote_cid, 0, 0, 0, NULL);
354552d92a1Smatthias.ringwald                 channel->state = L2CAP_STATE_WAIT_CONFIG_REQ_RSP;
355552d92a1Smatthias.ringwald                 break;
356552d92a1Smatthias.ringwald 
357552d92a1Smatthias.ringwald             case L2CAP_STATE_WILL_SEND_CONFIG_REQ_RSP:
358b1988dceSmatthias.ringwald                 l2cap_send_signaling_packet(channel->handle, CONFIGURE_RESPONSE, channel->remote_sig_id, channel->remote_cid, 0, 0, 0, NULL);
359552d92a1Smatthias.ringwald                 channel->state = L2CAP_STATE_OPEN;
360552d92a1Smatthias.ringwald                 l2cap_emit_channel_opened(channel, 0);  // success
361552d92a1Smatthias.ringwald                 l2cap_emit_credits(channel, 1);
362552d92a1Smatthias.ringwald                 break;
363552d92a1Smatthias.ringwald 
364ae280e73Smatthias.ringwald             case L2CAP_STATE_WILL_SEND_CONFIG_REQ_AND_CONFIG_REQ_RSP:
365b1988dceSmatthias.ringwald                 l2cap_send_signaling_packet(channel->handle, CONFIGURE_RESPONSE, channel->remote_sig_id, channel->remote_cid, 0, 0, 0, NULL);
366552d92a1Smatthias.ringwald                 channel->state = L2CAP_STATE_WILL_SEND_CONFIG_REQ;
367552d92a1Smatthias.ringwald                 break;
368552d92a1Smatthias.ringwald 
369e7ff783cSmatthias.ringwald             case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE:
370b1988dceSmatthias.ringwald                 l2cap_send_signaling_packet( channel->handle, DISCONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid);
371e7ff783cSmatthias.ringwald                 l2cap_finialize_channel_close(channel);
372e7ff783cSmatthias.ringwald                 break;
373e7ff783cSmatthias.ringwald 
374e7ff783cSmatthias.ringwald             case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST:
375b1988dceSmatthias.ringwald                 channel->local_sig_id = l2cap_next_sig_id();
376b1988dceSmatthias.ringwald                 l2cap_send_signaling_packet( channel->handle, DISCONNECTION_REQUEST, channel->local_sig_id, channel->remote_cid, channel->local_cid);
3772cd0be45Smatthias.ringwald                 channel->state = L2CAP_STATE_WAIT_DISCONNECT;
3782cd0be45Smatthias.ringwald                 break;
3792cd0be45Smatthias.ringwald             default:
3802cd0be45Smatthias.ringwald                 break;
3812cd0be45Smatthias.ringwald         }
3822cd0be45Smatthias.ringwald     }
3832cd0be45Smatthias.ringwald }
3842cd0be45Smatthias.ringwald 
3851e6aba47Smatthias.ringwald // open outgoing L2CAP channel
38615470d27Smatthias.ringwald void l2cap_create_channel_internal(void * connection, btstack_packet_handler_t packet_handler,
38715470d27Smatthias.ringwald                                    bd_addr_t address, uint16_t psm, uint16_t mtu){
3881e6aba47Smatthias.ringwald 
3891e6aba47Smatthias.ringwald     // alloc structure
3901e6aba47Smatthias.ringwald     l2cap_channel_t * chan = malloc(sizeof(l2cap_channel_t));
3911e6aba47Smatthias.ringwald     // TODO: emit error event
3921e6aba47Smatthias.ringwald     if (!chan) return;
3931e6aba47Smatthias.ringwald 
3941d279b20Smatthias.ringwald     // limit local mtu to max acl packet length
3951d279b20Smatthias.ringwald     if (mtu > hci_max_acl_data_packet_length()) {
3969775e25bSmatthias.ringwald         mtu = hci_max_acl_data_packet_length();
3979775e25bSmatthias.ringwald     }
3989775e25bSmatthias.ringwald 
3991e6aba47Smatthias.ringwald     // fill in
4001e6aba47Smatthias.ringwald     BD_ADDR_COPY(chan->address, address);
4011e6aba47Smatthias.ringwald     chan->psm = psm;
4021e6aba47Smatthias.ringwald     chan->handle = 0;
4031e6aba47Smatthias.ringwald     chan->connection = connection;
4046b296a27Smatthias.ringwald     chan->packet_handler = packet_handler;
4052784b77dSmatthias.ringwald     chan->remote_mtu = L2CAP_MINIMAL_MTU;
40615470d27Smatthias.ringwald     chan->local_mtu = mtu;
4076218e6f1Smatthias.ringwald     chan->packets_granted = 0;
4086218e6f1Smatthias.ringwald 
4091e6aba47Smatthias.ringwald     // set initial state
410*02b22dc4Smatthias.ringwald     chan->state = L2CAP_STATE_WILL_SEND_CREATE_CONNECTION;
411b1988dceSmatthias.ringwald     chan->remote_sig_id = L2CAP_SIG_ID_INVALID;
412b1988dceSmatthias.ringwald     chan->local_sig_id = L2CAP_SIG_ID_INVALID;
4131e6aba47Smatthias.ringwald 
4141e6aba47Smatthias.ringwald     // add to connections list
4151e6aba47Smatthias.ringwald     linked_list_add(&l2cap_channels, (linked_item_t *) chan);
4161e6aba47Smatthias.ringwald 
417*02b22dc4Smatthias.ringwald     l2cap_run();
41843625864Smatthias.ringwald }
41943625864Smatthias.ringwald 
420b35f641cSmatthias.ringwald void l2cap_disconnect_internal(uint16_t local_cid, uint8_t reason){
421b35f641cSmatthias.ringwald     // find channel for local_cid
422b35f641cSmatthias.ringwald     l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid);
423f62db1e3Smatthias.ringwald     if (channel) {
424e7ff783cSmatthias.ringwald         channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST;
425f62db1e3Smatthias.ringwald     }
4262cd0be45Smatthias.ringwald     // process
4272cd0be45Smatthias.ringwald     l2cap_run();
42843625864Smatthias.ringwald }
4291e6aba47Smatthias.ringwald 
430afde0c52Smatthias.ringwald static void l2cap_handle_connection_failed_for_addr(bd_addr_t address, uint8_t status){
43115ec09bbSmatthias.ringwald     linked_item_t *it = (linked_item_t *) &l2cap_channels;
43215ec09bbSmatthias.ringwald     while (it->next){
43315ec09bbSmatthias.ringwald         l2cap_channel_t * channel = (l2cap_channel_t *) it->next;
434b448a0e7Smatthias.ringwald         if ( ! BD_ADDR_CMP( channel->address, address) ){
435*02b22dc4Smatthias.ringwald             if (channel->state == L2CAP_STATE_WAIT_CONNECTION_COMPLETE || channel->state == L2CAP_STATE_WILL_SEND_CREATE_CONNECTION) {
436afde0c52Smatthias.ringwald                 // failure, forward error code
437afde0c52Smatthias.ringwald                 l2cap_emit_channel_opened(channel, status);
438afde0c52Smatthias.ringwald                 // discard channel
43915ec09bbSmatthias.ringwald                 it->next = it->next->next;
440afde0c52Smatthias.ringwald                 free (channel);
441afde0c52Smatthias.ringwald             }
44215ec09bbSmatthias.ringwald         } else {
44315ec09bbSmatthias.ringwald             it = it->next;
444afde0c52Smatthias.ringwald         }
445afde0c52Smatthias.ringwald     }
446afde0c52Smatthias.ringwald }
447afde0c52Smatthias.ringwald 
448afde0c52Smatthias.ringwald static void l2cap_handle_connection_success_for_addr(bd_addr_t address, hci_con_handle_t handle){
449afde0c52Smatthias.ringwald     linked_item_t *it;
450afde0c52Smatthias.ringwald     for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){
451afde0c52Smatthias.ringwald         l2cap_channel_t * channel = (l2cap_channel_t *) it;
452afde0c52Smatthias.ringwald         if ( ! BD_ADDR_CMP( channel->address, address) ){
453*02b22dc4Smatthias.ringwald             if (channel->state == L2CAP_STATE_WAIT_CONNECTION_COMPLETE || channel->state == L2CAP_STATE_WILL_SEND_CREATE_CONNECTION) {
454b448a0e7Smatthias.ringwald                 // success, start l2cap handshake
4556fdcc387Smatthias.ringwald                 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST;
456afde0c52Smatthias.ringwald                 channel->handle = handle;
457b35f641cSmatthias.ringwald                 channel->local_cid = l2cap_next_local_cid();
458afde0c52Smatthias.ringwald             }
459afde0c52Smatthias.ringwald         }
460afde0c52Smatthias.ringwald     }
4616fdcc387Smatthias.ringwald     // process
4626fdcc387Smatthias.ringwald     l2cap_run();
463afde0c52Smatthias.ringwald }
464b448a0e7Smatthias.ringwald 
465afde0c52Smatthias.ringwald void l2cap_event_handler( uint8_t *packet, uint16_t size ){
466afde0c52Smatthias.ringwald 
467afde0c52Smatthias.ringwald     bd_addr_t address;
468afde0c52Smatthias.ringwald     hci_con_handle_t handle;
46936944dffSmatthias.ringwald     linked_item_t *it;
470afde0c52Smatthias.ringwald 
471afde0c52Smatthias.ringwald     switch(packet[0]){
472afde0c52Smatthias.ringwald 
473afde0c52Smatthias.ringwald         // handle connection complete events
474afde0c52Smatthias.ringwald         case HCI_EVENT_CONNECTION_COMPLETE:
475afde0c52Smatthias.ringwald             bt_flip_addr(address, &packet[5]);
476afde0c52Smatthias.ringwald             if (packet[2] == 0){
477afde0c52Smatthias.ringwald                 handle = READ_BT_16(packet, 3);
478afde0c52Smatthias.ringwald                 l2cap_handle_connection_success_for_addr(address, handle);
479afde0c52Smatthias.ringwald             } else {
480afde0c52Smatthias.ringwald                 l2cap_handle_connection_failed_for_addr(address, packet[2]);
481afde0c52Smatthias.ringwald             }
482afde0c52Smatthias.ringwald             break;
483afde0c52Smatthias.ringwald 
484afde0c52Smatthias.ringwald         // handle successful create connection cancel command
485afde0c52Smatthias.ringwald         case HCI_EVENT_COMMAND_COMPLETE:
486afde0c52Smatthias.ringwald             if ( COMMAND_COMPLETE_EVENT(packet, hci_create_connection_cancel) ) {
487afde0c52Smatthias.ringwald                 if (packet[5] == 0){
488afde0c52Smatthias.ringwald                     bt_flip_addr(address, &packet[6]);
489afde0c52Smatthias.ringwald                     // CONNECTION TERMINATED BY LOCAL HOST (0X16)
490afde0c52Smatthias.ringwald                     l2cap_handle_connection_failed_for_addr(address, 0x16);
49103cfbabcSmatthias.ringwald                 }
4921e6aba47Smatthias.ringwald             }
493afde0c52Smatthias.ringwald             break;
49427a923d0Smatthias.ringwald 
4951e6aba47Smatthias.ringwald         // handle disconnection complete events
496afde0c52Smatthias.ringwald         case HCI_EVENT_DISCONNECTION_COMPLETE:
49727a923d0Smatthias.ringwald             // send l2cap disconnect events for all channels on this handle
498afde0c52Smatthias.ringwald             handle = READ_BT_16(packet, 3);
49915ec09bbSmatthias.ringwald             it = (linked_item_t *) &l2cap_channels;
50015ec09bbSmatthias.ringwald             while (it->next){
50139ce35a7Smatthias.ringwald                 l2cap_channel_t * channel = (l2cap_channel_t *) it->next;
50227a923d0Smatthias.ringwald                 if ( channel->handle == handle ){
50315ec09bbSmatthias.ringwald                     // update prev item before free'ing next element - don't call l2cap_finalize_channel_close
5042060cf14Smatthias.ringwald                     it->next = it->next->next;
50515ec09bbSmatthias.ringwald                     l2cap_emit_channel_closed(channel);
50615ec09bbSmatthias.ringwald                     free (channel);
50715ec09bbSmatthias.ringwald                 } else {
50815ec09bbSmatthias.ringwald                     it = it->next;
50927a923d0Smatthias.ringwald                 }
51027a923d0Smatthias.ringwald             }
511afde0c52Smatthias.ringwald             break;
512fcadd0caSmatthias.ringwald 
5136218e6f1Smatthias.ringwald         case HCI_EVENT_NUMBER_OF_COMPLETED_PACKETS:
514*02b22dc4Smatthias.ringwald             l2cap_run();    // try sending signaling packets first
5158d371091Smatthias.ringwald             l2cap_hand_out_credits();
5166218e6f1Smatthias.ringwald             break;
5176218e6f1Smatthias.ringwald 
518ee091cf1Smatthias.ringwald         // HCI Connection Timeouts
519afde0c52Smatthias.ringwald         case L2CAP_EVENT_TIMEOUT_CHECK:
52036944dffSmatthias.ringwald             handle = READ_BT_16(packet, 2);
52180ca58a0Smatthias.ringwald             if (hci_authentication_active_for_handle(handle)) break;
522ee091cf1Smatthias.ringwald             l2cap_channel_t * channel;
523ee091cf1Smatthias.ringwald             int used = 0;
524ee091cf1Smatthias.ringwald             for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){
525ee091cf1Smatthias.ringwald                 channel = (l2cap_channel_t *) it;
526ee091cf1Smatthias.ringwald                 if (channel->handle == handle) {
527ee091cf1Smatthias.ringwald                     used = 1;
528ee091cf1Smatthias.ringwald                 }
529ee091cf1Smatthias.ringwald             }
530ee091cf1Smatthias.ringwald             if (!used) {
5319edc8742Smatthias.ringwald                 hci_send_cmd(&hci_disconnect, handle, 0x13); // remote closed connection
532ee091cf1Smatthias.ringwald             }
533afde0c52Smatthias.ringwald             break;
534ee091cf1Smatthias.ringwald 
535afde0c52Smatthias.ringwald         default:
536afde0c52Smatthias.ringwald             break;
537afde0c52Smatthias.ringwald     }
538afde0c52Smatthias.ringwald 
539afde0c52Smatthias.ringwald     // pass on
540b502e1b0Smatthias.ringwald     (*packet_handler)(NULL, HCI_EVENT_PACKET, 0, packet, size);
5411e6aba47Smatthias.ringwald }
5421e6aba47Smatthias.ringwald 
543afde0c52Smatthias.ringwald static void l2cap_handle_disconnect_request(l2cap_channel_t *channel, uint16_t identifier){
544b1988dceSmatthias.ringwald     channel->remote_sig_id = identifier;
545e7ff783cSmatthias.ringwald     channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE;
546e7ff783cSmatthias.ringwald     l2cap_run();
54784836b65Smatthias.ringwald }
54884836b65Smatthias.ringwald 
549b35f641cSmatthias.ringwald static void l2cap_handle_connection_request(hci_con_handle_t handle, uint8_t sig_id, uint16_t psm, uint16_t source_cid){
550645658c9Smatthias.ringwald 
551fe35119dSmatthias.ringwald     // log_dbg("l2cap_handle_connection_request for handle %u, psm %u cid %u\n", handle, psm, source_cid);
552645658c9Smatthias.ringwald     l2cap_service_t *service = l2cap_get_service(psm);
553645658c9Smatthias.ringwald     if (!service) {
554645658c9Smatthias.ringwald         // 0x0002 PSM not supported
555fe35119dSmatthias.ringwald         // log_dbg("l2cap_handle_connection_request no PSM for psm %u/n", psm);
556645658c9Smatthias.ringwald         l2cap_send_signaling_packet(handle, CONNECTION_RESPONSE, sig_id, 0, 0, 0x0002, 0);
557645658c9Smatthias.ringwald         return;
558645658c9Smatthias.ringwald     }
559645658c9Smatthias.ringwald 
560645658c9Smatthias.ringwald     hci_connection_t * hci_connection = connection_for_handle( handle );
561645658c9Smatthias.ringwald     if (!hci_connection) {
562fe35119dSmatthias.ringwald         log_err("no hci_connection for handle %u\n", handle);
563645658c9Smatthias.ringwald         // TODO: emit error
564645658c9Smatthias.ringwald         return;
565645658c9Smatthias.ringwald     }
566645658c9Smatthias.ringwald     // alloc structure
567fe35119dSmatthias.ringwald     // log_dbg("l2cap_handle_connection_request register channel\n");
568169f8b28Smatthias.ringwald     l2cap_channel_t * channel = malloc(sizeof(l2cap_channel_t));
569645658c9Smatthias.ringwald     // TODO: emit error event
570169f8b28Smatthias.ringwald     if (!channel) return;
571645658c9Smatthias.ringwald 
572645658c9Smatthias.ringwald     // fill in
573169f8b28Smatthias.ringwald     BD_ADDR_COPY(channel->address, hci_connection->address);
574169f8b28Smatthias.ringwald     channel->psm = psm;
575169f8b28Smatthias.ringwald     channel->handle = handle;
576169f8b28Smatthias.ringwald     channel->connection = service->connection;
577f8dd2f72Smatthias.ringwald     channel->packet_handler = service->packet_handler;
578b35f641cSmatthias.ringwald     channel->local_cid  = l2cap_next_local_cid();
579b35f641cSmatthias.ringwald     channel->remote_cid = source_cid;
580fa2b2627Smatthias.ringwald     channel->local_mtu  = service->mtu;
5810a18a8e9Smatthias.ringwald     channel->remote_mtu = L2CAP_DEFAULT_MTU;
582761b0451Smatthias.ringwald     channel->packets_granted = 0;
583b1988dceSmatthias.ringwald     channel->remote_sig_id = sig_id;
584645658c9Smatthias.ringwald 
5851d279b20Smatthias.ringwald     // limit local mtu to max acl packet length
5861d279b20Smatthias.ringwald     if (channel->local_mtu > hci_max_acl_data_packet_length()) {
5879775e25bSmatthias.ringwald         channel->local_mtu = hci_max_acl_data_packet_length();
5889775e25bSmatthias.ringwald     }
5899775e25bSmatthias.ringwald 
590645658c9Smatthias.ringwald     // set initial state
591e405ae81Smatthias.ringwald     channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT;
592e405ae81Smatthias.ringwald 
593645658c9Smatthias.ringwald     // add to connections list
594169f8b28Smatthias.ringwald     linked_list_add(&l2cap_channels, (linked_item_t *) channel);
595645658c9Smatthias.ringwald 
596e405ae81Smatthias.ringwald     // emit incoming connection request
597e405ae81Smatthias.ringwald     l2cap_emit_connection_request(channel);
598e405ae81Smatthias.ringwald }
599645658c9Smatthias.ringwald 
600b35f641cSmatthias.ringwald void l2cap_accept_connection_internal(uint16_t local_cid){
601b35f641cSmatthias.ringwald     l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid);
602e405ae81Smatthias.ringwald     if (!channel) {
603fe35119dSmatthias.ringwald         log_err("l2cap_accept_connection_internal called but local_cid 0x%x not found", local_cid);
604e405ae81Smatthias.ringwald         return;
605e405ae81Smatthias.ringwald     }
606e405ae81Smatthias.ringwald 
607552d92a1Smatthias.ringwald     channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT;
608e405ae81Smatthias.ringwald 
609552d92a1Smatthias.ringwald     // process
610552d92a1Smatthias.ringwald     l2cap_run();
611e405ae81Smatthias.ringwald }
612645658c9Smatthias.ringwald 
613b35f641cSmatthias.ringwald void l2cap_decline_connection_internal(uint16_t local_cid, uint8_t reason){
614b35f641cSmatthias.ringwald     l2cap_channel_t * channel = l2cap_get_channel_for_local_cid( local_cid);
615e405ae81Smatthias.ringwald     if (!channel) {
616fe35119dSmatthias.ringwald         log_err( "l2cap_decline_connection_internal called but local_cid 0x%x not found", local_cid);
617e405ae81Smatthias.ringwald         return;
618e405ae81Smatthias.ringwald     }
619e7ff783cSmatthias.ringwald     channel->state  = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE;
620e7ff783cSmatthias.ringwald     channel->reason = reason;
621e7ff783cSmatthias.ringwald     l2cap_run();
622645658c9Smatthias.ringwald }
623645658c9Smatthias.ringwald 
6242784b77dSmatthias.ringwald void l2cap_signaling_handle_configure_request(l2cap_channel_t *channel, uint8_t *command){
625b1988dceSmatthias.ringwald 
626b1988dceSmatthias.ringwald     channel->remote_sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET];
627b1988dceSmatthias.ringwald 
6282784b77dSmatthias.ringwald     // accept the other's configuration options
6293de7c0caSmatthias.ringwald     uint16_t end_pos = 4 + READ_BT_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET);
6303de7c0caSmatthias.ringwald     uint16_t pos     = 8;
6313de7c0caSmatthias.ringwald     while (pos < end_pos){
6321dc511deSmatthias.ringwald         uint8_t type   = command[pos++];
6331dc511deSmatthias.ringwald         uint8_t length = command[pos++];
6341dc511deSmatthias.ringwald         // MTU { type(8): 1, len(8):2, MTU(16) }
6351dc511deSmatthias.ringwald         if ((type & 0x7f) == 1 && length == 2){
6361dc511deSmatthias.ringwald             channel->remote_mtu = READ_BT_16(command, pos);
637fe35119dSmatthias.ringwald             // log_dbg("l2cap cid %u, remote mtu %u\n", channel->local_cid, channel->remote_mtu);
6381dc511deSmatthias.ringwald         }
6391dc511deSmatthias.ringwald         pos += length;
6401dc511deSmatthias.ringwald     }
6412784b77dSmatthias.ringwald }
6422784b77dSmatthias.ringwald 
64300d93d79Smatthias.ringwald void l2cap_signaling_handler_channel(l2cap_channel_t *channel, uint8_t *command){
6441e6aba47Smatthias.ringwald 
64500d93d79Smatthias.ringwald     uint8_t  code       = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET];
64600d93d79Smatthias.ringwald     uint8_t  identifier = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET];
64738e5900eSmatthias.ringwald     uint16_t result = 0;
6481e6aba47Smatthias.ringwald 
649fe35119dSmatthias.ringwald     // log_dbg("signaling handler code %u\n", code);
650b35f641cSmatthias.ringwald 
6519a011532Smatthias.ringwald     // handle DISCONNECT REQUESTS seperately
6529a011532Smatthias.ringwald     if (code == DISCONNECTION_REQUEST){
6539a011532Smatthias.ringwald         switch (channel->state){
6549a011532Smatthias.ringwald             case L2CAP_STATE_WAIT_CONFIG_REQ_OR_SEND_CONFIG_REQ:
6552b83fb7dSmatthias.ringwald             case L2CAP_STATE_WAIT_CONFIG_REQ_RSP_OR_WILL_SEND_CONFIG_REQ_RSP:
6569a011532Smatthias.ringwald             case L2CAP_STATE_WAIT_CONFIG_REQ_RSP_OR_CONFIG_REQ:
6579a011532Smatthias.ringwald             case L2CAP_STATE_WAIT_CONFIG_REQ:
6589a011532Smatthias.ringwald             case L2CAP_STATE_WAIT_CONFIG_REQ_RSP:
6592b83fb7dSmatthias.ringwald             case L2CAP_STATE_WILL_SEND_CONFIG_REQ:
6602b83fb7dSmatthias.ringwald             case L2CAP_STATE_WILL_SEND_CONFIG_REQ_RSP:
6612b83fb7dSmatthias.ringwald             case L2CAP_STATE_WILL_SEND_CONFIG_REQ_AND_CONFIG_REQ_RSP:
6629a011532Smatthias.ringwald             case L2CAP_STATE_OPEN:
6632b83fb7dSmatthias.ringwald             case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST:
6649a011532Smatthias.ringwald             case L2CAP_STATE_WAIT_DISCONNECT:
6659a011532Smatthias.ringwald                 l2cap_handle_disconnect_request(channel, identifier);
6669a011532Smatthias.ringwald                 break;
6679a011532Smatthias.ringwald 
6689a011532Smatthias.ringwald             default:
6699a011532Smatthias.ringwald                 // ignore in other states
6709a011532Smatthias.ringwald                 break;
6719a011532Smatthias.ringwald         }
6729a011532Smatthias.ringwald         return;
6739a011532Smatthias.ringwald     }
6749a011532Smatthias.ringwald 
6751e6aba47Smatthias.ringwald     switch (channel->state) {
6761e6aba47Smatthias.ringwald 
6771e6aba47Smatthias.ringwald         case L2CAP_STATE_WAIT_CONNECT_RSP:
6781e6aba47Smatthias.ringwald             switch (code){
6791e6aba47Smatthias.ringwald                 case CONNECTION_RESPONSE:
68000d93d79Smatthias.ringwald                     result = READ_BT_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4);
68138e5900eSmatthias.ringwald                     switch (result) {
68238e5900eSmatthias.ringwald                         case 0:
683169f8b28Smatthias.ringwald                             // successful connection
68400d93d79Smatthias.ringwald                             channel->remote_cid = READ_BT_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET);
685ae280e73Smatthias.ringwald                             channel->state = L2CAP_STATE_WAIT_CONFIG_REQ_OR_SEND_CONFIG_REQ;
686d418e203Smatthias.ringwald #if 0
687d418e203Smatthias.ringwald     channel->state = L2CAP_STATE_OPEN;
688d418e203Smatthias.ringwald     l2cap_emit_channel_opened(channel, 0);  // success
689d418e203Smatthias.ringwald     l2cap_emit_credits(channel, 1);
690d418e203Smatthias.ringwald #endif
69138e5900eSmatthias.ringwald                             break;
69238e5900eSmatthias.ringwald                         case 1:
69338e5900eSmatthias.ringwald                             // connection pending. get some coffee
69438e5900eSmatthias.ringwald                             break;
69538e5900eSmatthias.ringwald                         default:
696eb920dbeSmatthias.ringwald                             // channel closed
697eb920dbeSmatthias.ringwald                             channel->state = L2CAP_STATE_CLOSED;
698eb920dbeSmatthias.ringwald 
699f32b992eSmatthias.ringwald                             // map l2cap connection response result to BTstack status enumeration
70038e5900eSmatthias.ringwald                             l2cap_emit_channel_opened(channel, L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result);
701eb920dbeSmatthias.ringwald 
702eb920dbeSmatthias.ringwald                             // drop link key if security block
703eb920dbeSmatthias.ringwald                             if (L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result == L2CAP_CONNECTION_RESPONSE_RESULT_REFUSED_SECURITY){
704eb920dbeSmatthias.ringwald                                 hci_drop_link_key_for_bd_addr(&channel->address);
705eb920dbeSmatthias.ringwald                             }
706eb920dbeSmatthias.ringwald 
707eb920dbeSmatthias.ringwald                             // discard channel
708eb920dbeSmatthias.ringwald                             linked_list_remove(&l2cap_channels, (linked_item_t *) channel);
709eb920dbeSmatthias.ringwald                             free (channel);
71038e5900eSmatthias.ringwald                             break;
7111e6aba47Smatthias.ringwald                     }
7121e6aba47Smatthias.ringwald                     break;
71338e5900eSmatthias.ringwald 
71438e5900eSmatthias.ringwald                 default:
7151e6aba47Smatthias.ringwald                     //@TODO: implement other signaling packets
71638e5900eSmatthias.ringwald                     break;
7171e6aba47Smatthias.ringwald             }
7181e6aba47Smatthias.ringwald             break;
7191e6aba47Smatthias.ringwald 
720ae280e73Smatthias.ringwald         case L2CAP_STATE_WAIT_CONFIG_REQ_OR_SEND_CONFIG_REQ:
721ae280e73Smatthias.ringwald             switch (code) {
722ae280e73Smatthias.ringwald                 case CONFIGURE_REQUEST:
723ae280e73Smatthias.ringwald                     l2cap_signaling_handle_configure_request(channel, command);
724ae280e73Smatthias.ringwald                     channel->state = L2CAP_STATE_WILL_SEND_CONFIG_REQ_AND_CONFIG_REQ_RSP;
725ae280e73Smatthias.ringwald                     break;
726ae280e73Smatthias.ringwald                 default:
727ae280e73Smatthias.ringwald                     //@TODO: implement other signaling packets
728ae280e73Smatthias.ringwald                     break;
729ae280e73Smatthias.ringwald             }
730ae280e73Smatthias.ringwald             break;
731ae280e73Smatthias.ringwald 
7325a67bd4aSmatthias.ringwald         case L2CAP_STATE_WAIT_CONFIG_REQ_RSP_OR_CONFIG_REQ:
7331e6aba47Smatthias.ringwald             switch (code) {
7341e6aba47Smatthias.ringwald                 case CONFIGURE_RESPONSE:
7351e6aba47Smatthias.ringwald                     channel->state = L2CAP_STATE_WAIT_CONFIG_REQ;
7361e6aba47Smatthias.ringwald                     break;
7375a67bd4aSmatthias.ringwald                 case CONFIGURE_REQUEST:
7382784b77dSmatthias.ringwald                     l2cap_signaling_handle_configure_request(channel, command);
739552d92a1Smatthias.ringwald                     channel->state = L2CAP_STATE_WAIT_CONFIG_REQ_RSP_OR_WILL_SEND_CONFIG_REQ_RSP;
7405a67bd4aSmatthias.ringwald                     break;
7415a67bd4aSmatthias.ringwald                 default:
7425a67bd4aSmatthias.ringwald                     //@TODO: implement other signaling packets
7435a67bd4aSmatthias.ringwald                     break;
7441e6aba47Smatthias.ringwald             }
7451e6aba47Smatthias.ringwald             break;
7461e6aba47Smatthias.ringwald 
7471e6aba47Smatthias.ringwald         case L2CAP_STATE_WAIT_CONFIG_REQ:
7481e6aba47Smatthias.ringwald             switch (code) {
7491e6aba47Smatthias.ringwald                 case CONFIGURE_REQUEST:
7502784b77dSmatthias.ringwald                     l2cap_signaling_handle_configure_request(channel, command);
751552d92a1Smatthias.ringwald                     channel->state = L2CAP_STATE_WILL_SEND_CONFIG_REQ_RSP;
752c8e4258aSmatthias.ringwald                     break;
7535a67bd4aSmatthias.ringwald                 default:
7545a67bd4aSmatthias.ringwald                     //@TODO: implement other signaling packets
7555a67bd4aSmatthias.ringwald                     break;
7565a67bd4aSmatthias.ringwald             }
7575a67bd4aSmatthias.ringwald             break;
7585a67bd4aSmatthias.ringwald 
7595a67bd4aSmatthias.ringwald         case L2CAP_STATE_WAIT_CONFIG_REQ_RSP:
7605a67bd4aSmatthias.ringwald             switch (code) {
7615a67bd4aSmatthias.ringwald                 case CONFIGURE_RESPONSE:
7625a67bd4aSmatthias.ringwald                     channel->state = L2CAP_STATE_OPEN;
7635a67bd4aSmatthias.ringwald                     l2cap_emit_channel_opened(channel, 0);  // success
7646218e6f1Smatthias.ringwald                     l2cap_emit_credits(channel, 1);
7655a67bd4aSmatthias.ringwald                     break;
7665a67bd4aSmatthias.ringwald                 default:
7675a67bd4aSmatthias.ringwald                     //@TODO: implement other signaling packets
7685a67bd4aSmatthias.ringwald                     break;
769c8e4258aSmatthias.ringwald             }
770c8e4258aSmatthias.ringwald             break;
771f62db1e3Smatthias.ringwald 
772f62db1e3Smatthias.ringwald         case L2CAP_STATE_WAIT_DISCONNECT:
773f62db1e3Smatthias.ringwald             switch (code) {
774f62db1e3Smatthias.ringwald                 case DISCONNECTION_RESPONSE:
77527a923d0Smatthias.ringwald                     l2cap_finialize_channel_close(channel);
77627a923d0Smatthias.ringwald                     break;
7775a67bd4aSmatthias.ringwald                 default:
7785a67bd4aSmatthias.ringwald                     //@TODO: implement other signaling packets
7795a67bd4aSmatthias.ringwald                     break;
78027a923d0Smatthias.ringwald             }
78127a923d0Smatthias.ringwald             break;
78284836b65Smatthias.ringwald 
78384836b65Smatthias.ringwald         case L2CAP_STATE_CLOSED:
78484836b65Smatthias.ringwald             // @TODO handle incoming requests
78584836b65Smatthias.ringwald             break;
78684836b65Smatthias.ringwald 
78784836b65Smatthias.ringwald         case L2CAP_STATE_OPEN:
78884836b65Smatthias.ringwald             //@TODO: implement other signaling packets, e.g. re-configure
78984836b65Smatthias.ringwald             break;
79010642e45Smatthias.ringwald         default:
79110642e45Smatthias.ringwald             break;
79227a923d0Smatthias.ringwald     }
793fe35119dSmatthias.ringwald     // log_dbg("new state %u\n", channel->state);
79427a923d0Smatthias.ringwald }
79527a923d0Smatthias.ringwald 
79600d93d79Smatthias.ringwald 
79700d93d79Smatthias.ringwald void l2cap_signaling_handler_dispatch( hci_con_handle_t handle, uint8_t * command){
79800d93d79Smatthias.ringwald 
79900d93d79Smatthias.ringwald     // get code, signalind identifier and command len
80000d93d79Smatthias.ringwald     uint8_t code   = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET];
80100d93d79Smatthias.ringwald     uint8_t sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET];
80200d93d79Smatthias.ringwald 
80300d93d79Smatthias.ringwald     // not for a particular channel, and not CONNECTION_REQUEST, ECHO_[REQUEST|RESPONSE], INFORMATION_REQUEST
80400d93d79Smatthias.ringwald     if (code < 1 || code == ECHO_RESPONSE || code > INFORMATION_REQUEST){
80500d93d79Smatthias.ringwald         return;
80600d93d79Smatthias.ringwald     }
80700d93d79Smatthias.ringwald 
80800d93d79Smatthias.ringwald     // general commands without an assigned channel
80900d93d79Smatthias.ringwald     switch(code) {
81000d93d79Smatthias.ringwald 
81100d93d79Smatthias.ringwald         case CONNECTION_REQUEST: {
81200d93d79Smatthias.ringwald             uint16_t psm =        READ_BT_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET);
81300d93d79Smatthias.ringwald             uint16_t source_cid = READ_BT_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2);
81400d93d79Smatthias.ringwald             l2cap_handle_connection_request(handle, sig_id, psm, source_cid);
8152b83fb7dSmatthias.ringwald             return;
81600d93d79Smatthias.ringwald         }
81700d93d79Smatthias.ringwald 
81800d93d79Smatthias.ringwald         case ECHO_REQUEST: {
8192b83fb7dSmatthias.ringwald             if (signaling_responses_pending < NR_PENDING_SIGNALING_RESPONSES) {
8202b83fb7dSmatthias.ringwald                 signaling_responses[signaling_responses_pending].handle = handle;
8212b83fb7dSmatthias.ringwald                 signaling_responses[signaling_responses_pending].code = code;
8222b83fb7dSmatthias.ringwald                 signaling_responses[signaling_responses_pending].sig_id = sig_id;
8232b83fb7dSmatthias.ringwald                 signaling_responses_pending++;
8242b83fb7dSmatthias.ringwald             }
8252b83fb7dSmatthias.ringwald             l2cap_run();
8262b83fb7dSmatthias.ringwald             return;
82700d93d79Smatthias.ringwald         }
82800d93d79Smatthias.ringwald 
82900d93d79Smatthias.ringwald         case INFORMATION_REQUEST: {
8302b83fb7dSmatthias.ringwald             if (signaling_responses_pending < NR_PENDING_SIGNALING_RESPONSES) {
83100d93d79Smatthias.ringwald                 // we neither support connectionless L2CAP data nor support any flow control modes yet
83200d93d79Smatthias.ringwald                 uint16_t infoType = READ_BT_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET);
8332b83fb7dSmatthias.ringwald                 signaling_responses[signaling_responses_pending].handle = handle;
8342b83fb7dSmatthias.ringwald                 signaling_responses[signaling_responses_pending].code = code;
8352b83fb7dSmatthias.ringwald                 signaling_responses[signaling_responses_pending].sig_id = sig_id;
8362b83fb7dSmatthias.ringwald                 signaling_responses[signaling_responses_pending].infoType = infoType;
8372b83fb7dSmatthias.ringwald                 signaling_responses_pending++;
83800d93d79Smatthias.ringwald             }
8392b83fb7dSmatthias.ringwald             l2cap_run();
8402b83fb7dSmatthias.ringwald             return;
84100d93d79Smatthias.ringwald         }
84200d93d79Smatthias.ringwald 
84300d93d79Smatthias.ringwald         default:
84400d93d79Smatthias.ringwald             break;
84500d93d79Smatthias.ringwald     }
84600d93d79Smatthias.ringwald 
84700d93d79Smatthias.ringwald 
84800d93d79Smatthias.ringwald     // Get potential destination CID
84900d93d79Smatthias.ringwald     uint16_t dest_cid = READ_BT_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET);
85000d93d79Smatthias.ringwald 
85100d93d79Smatthias.ringwald     // Find channel for this sig_id and connection handle
85200d93d79Smatthias.ringwald     linked_item_t *it;
85300d93d79Smatthias.ringwald     for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){
85400d93d79Smatthias.ringwald         l2cap_channel_t * channel = (l2cap_channel_t *) it;
85500d93d79Smatthias.ringwald         if (channel->handle == handle) {
85600d93d79Smatthias.ringwald             if (code & 1) {
857b1988dceSmatthias.ringwald                 // match odd commands (responses) by previous signaling identifier
858b1988dceSmatthias.ringwald                 if (channel->local_sig_id == sig_id) {
85900d93d79Smatthias.ringwald                     l2cap_signaling_handler_channel(channel, command);
8604e32727eSmatthias.ringwald                     break;
86100d93d79Smatthias.ringwald                 }
86200d93d79Smatthias.ringwald             } else {
863b1988dceSmatthias.ringwald                 // match even commands (requests) by local channel id
86400d93d79Smatthias.ringwald                 if (channel->local_cid == dest_cid) {
86500d93d79Smatthias.ringwald                     l2cap_signaling_handler_channel(channel, command);
8664e32727eSmatthias.ringwald                     break;
86700d93d79Smatthias.ringwald                 }
86800d93d79Smatthias.ringwald             }
86900d93d79Smatthias.ringwald         }
87000d93d79Smatthias.ringwald     }
87100d93d79Smatthias.ringwald }
87200d93d79Smatthias.ringwald 
87300d93d79Smatthias.ringwald void l2cap_acl_handler( uint8_t *packet, uint16_t size ){
87400d93d79Smatthias.ringwald 
87500d93d79Smatthias.ringwald     // Get Channel ID
87600d93d79Smatthias.ringwald     uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet);
87700d93d79Smatthias.ringwald 
87800d93d79Smatthias.ringwald     // Signaling Packet?
87900d93d79Smatthias.ringwald     if (channel_id == 1) {
88000d93d79Smatthias.ringwald 
88100d93d79Smatthias.ringwald         // Get Connection
88200d93d79Smatthias.ringwald         hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet);
88300d93d79Smatthias.ringwald 
88400d93d79Smatthias.ringwald         uint16_t command_offset = 8;
88500d93d79Smatthias.ringwald         while (command_offset < size) {
88600d93d79Smatthias.ringwald 
88700d93d79Smatthias.ringwald             // handle signaling commands
88800d93d79Smatthias.ringwald             l2cap_signaling_handler_dispatch(handle, &packet[command_offset]);
88900d93d79Smatthias.ringwald 
89000d93d79Smatthias.ringwald             // increment command_offset
89100d93d79Smatthias.ringwald             command_offset += L2CAP_SIGNALING_COMMAND_DATA_OFFSET + READ_BT_16(packet, command_offset + L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET);
89200d93d79Smatthias.ringwald         }
89300d93d79Smatthias.ringwald         return;
89400d93d79Smatthias.ringwald     }
89500d93d79Smatthias.ringwald 
89600d93d79Smatthias.ringwald     // Find channel for this channel_id and connection handle
89700d93d79Smatthias.ringwald     l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(channel_id);
89800d93d79Smatthias.ringwald     if (channel) {
89958de5610Smatthias.ringwald         l2cap_dispatch(channel, L2CAP_DATA_PACKET, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER);
90000d93d79Smatthias.ringwald     }
90100d93d79Smatthias.ringwald }
90200d93d79Smatthias.ringwald 
9032718e2e7Smatthias.ringwald static void l2cap_packet_handler(uint8_t packet_type, uint8_t *packet, uint16_t size){
9042718e2e7Smatthias.ringwald     switch (packet_type) {
9052718e2e7Smatthias.ringwald         case HCI_EVENT_PACKET:
9062718e2e7Smatthias.ringwald             l2cap_event_handler(packet, size);
9072718e2e7Smatthias.ringwald             break;
9082718e2e7Smatthias.ringwald         case HCI_ACL_DATA_PACKET:
9092718e2e7Smatthias.ringwald             l2cap_acl_handler(packet, size);
9102718e2e7Smatthias.ringwald             break;
9112718e2e7Smatthias.ringwald         default:
9122718e2e7Smatthias.ringwald             break;
9132718e2e7Smatthias.ringwald     }
9142718e2e7Smatthias.ringwald }
91500d93d79Smatthias.ringwald 
91615ec09bbSmatthias.ringwald // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE
91727a923d0Smatthias.ringwald void l2cap_finialize_channel_close(l2cap_channel_t *channel){
918f62db1e3Smatthias.ringwald     channel->state = L2CAP_STATE_CLOSED;
919f62db1e3Smatthias.ringwald     l2cap_emit_channel_closed(channel);
920f62db1e3Smatthias.ringwald     // discard channel
921f62db1e3Smatthias.ringwald     linked_list_remove(&l2cap_channels, (linked_item_t *) channel);
922f62db1e3Smatthias.ringwald     free (channel);
923c8e4258aSmatthias.ringwald }
9241e6aba47Smatthias.ringwald 
9259d9bbc01Smatthias.ringwald l2cap_service_t * l2cap_get_service(uint16_t psm){
926c52bf64dSmatthias.ringwald     linked_item_t *it;
9279d9bbc01Smatthias.ringwald 
9289d9bbc01Smatthias.ringwald     // close open channels
9299d9bbc01Smatthias.ringwald     for (it = (linked_item_t *) l2cap_services; it ; it = it->next){
9309d9bbc01Smatthias.ringwald         l2cap_service_t * service = ((l2cap_service_t *) it);
9319d9bbc01Smatthias.ringwald         if ( service->psm == psm){
9329d9bbc01Smatthias.ringwald             return service;
9339d9bbc01Smatthias.ringwald         };
9349d9bbc01Smatthias.ringwald     }
9359d9bbc01Smatthias.ringwald     return NULL;
9369d9bbc01Smatthias.ringwald }
9379d9bbc01Smatthias.ringwald 
93836944dffSmatthias.ringwald void l2cap_register_service_internal(void *connection, btstack_packet_handler_t packet_handler, uint16_t psm, uint16_t mtu){
9399d9bbc01Smatthias.ringwald     // check for alread registered psm // TODO: emit error event
9409d9bbc01Smatthias.ringwald     l2cap_service_t *service = l2cap_get_service(psm);
9419d9bbc01Smatthias.ringwald     if (service) return;
9429d9bbc01Smatthias.ringwald 
9439d9bbc01Smatthias.ringwald     // alloc structure     // TODO: emit error event
9449d9bbc01Smatthias.ringwald     service = malloc(sizeof(l2cap_service_t));
9459d9bbc01Smatthias.ringwald     if (!service) return;
9469d9bbc01Smatthias.ringwald 
9479d9bbc01Smatthias.ringwald     // fill in
9489d9bbc01Smatthias.ringwald     service->psm = psm;
9499d9bbc01Smatthias.ringwald     service->mtu = mtu;
9509d9bbc01Smatthias.ringwald     service->connection = connection;
951d8497f19Smatthias.ringwald     service->packet_handler = packet_handler;
9529d9bbc01Smatthias.ringwald 
9539d9bbc01Smatthias.ringwald     // add to services list
9549d9bbc01Smatthias.ringwald     linked_list_add(&l2cap_services, (linked_item_t *) service);
9559d9bbc01Smatthias.ringwald }
9569d9bbc01Smatthias.ringwald 
95736944dffSmatthias.ringwald void l2cap_unregister_service_internal(void *connection, uint16_t psm){
9589d9bbc01Smatthias.ringwald     l2cap_service_t *service = l2cap_get_service(psm);
959037d6e48Smatthias.ringwald     if (!service) return;
9609d9bbc01Smatthias.ringwald     linked_list_remove(&l2cap_services, (linked_item_t *) service);
9619d9bbc01Smatthias.ringwald     free(service);
9629d9bbc01Smatthias.ringwald }
9639d9bbc01Smatthias.ringwald 
9649d9bbc01Smatthias.ringwald //
96536944dffSmatthias.ringwald void l2cap_close_connection(void *connection){
9669d9bbc01Smatthias.ringwald     linked_item_t *it;
9679d9bbc01Smatthias.ringwald 
9683a9e0a58Smatthias.ringwald     // close open channels - note to myself: no channel is freed, so no new for fancy iterator tricks
969c52bf64dSmatthias.ringwald     l2cap_channel_t * channel;
970c52bf64dSmatthias.ringwald     for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){
971c52bf64dSmatthias.ringwald         channel = (l2cap_channel_t *) it;
972c52bf64dSmatthias.ringwald         if (channel->connection == connection) {
973cb8eb7e6Smatthias.ringwald             channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST;
974c52bf64dSmatthias.ringwald         }
975c52bf64dSmatthias.ringwald     }
9769d9bbc01Smatthias.ringwald 
977645658c9Smatthias.ringwald     // unregister services
97869025de8Smatthias.ringwald     it = (linked_item_t *) &l2cap_services;
97969025de8Smatthias.ringwald     while (it->next) {
98069025de8Smatthias.ringwald         l2cap_service_t * service = (l2cap_service_t *) it->next;
981645658c9Smatthias.ringwald         if (service->connection == connection){
98269025de8Smatthias.ringwald             it->next = it->next->next;
9838149d2c7Smatthias.ringwald             free(service);
9848149d2c7Smatthias.ringwald         } else {
9858149d2c7Smatthias.ringwald             it = it->next;
986645658c9Smatthias.ringwald         }
987645658c9Smatthias.ringwald     }
988cb8eb7e6Smatthias.ringwald 
989cb8eb7e6Smatthias.ringwald     // process
990cb8eb7e6Smatthias.ringwald     l2cap_run();
991c52bf64dSmatthias.ringwald }
992c52bf64dSmatthias.ringwald