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