Junichi Katsu / Mbed 2 deprecated WallbotWii

うおーるぼっとをWiiリモコンでコントロールする新しいプログラムです。 以前のものより、Wiiリモコンが早く繋がる様になりました。 It is a program which controls A with the Wii remote. ※ A Bluetooth dongle and a Wii remote control are needed.

Dependencies:   USBHost mbed FATFileSystem mbed-rtos

btstack/l2cap.c@0:fccb789424fc, 2013-06-10 (annotated)

Committer:
jksoft
Date:
Mon Jun 10 16:01:50 2013 +0000
Revision:
0:fccb789424fc
1.0

Who changed what in which revision?

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