1 /* 2 * Copyright (C) 2009 by Matthias Ringwald 3 * 4 * Redistribution and use in source and binary forms, with or without 5 * modification, are permitted provided that the following conditions 6 * are met: 7 * 8 * 1. Redistributions of source code must retain the above copyright 9 * notice, this list of conditions and the following disclaimer. 10 * 2. Redistributions in binary form must reproduce the above copyright 11 * notice, this list of conditions and the following disclaimer in the 12 * documentation and/or other materials provided with the distribution. 13 * 3. Neither the name of the copyright holders nor the names of 14 * contributors may be used to endorse or promote products derived 15 * from this software without specific prior written permission. 16 * 17 * THIS SOFTWARE IS PROVIDED BY MATTHIAS RINGWALD AND CONTRIBUTORS 18 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 20 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS 21 * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 22 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 23 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 24 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 25 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF 27 * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 28 * SUCH DAMAGE. 29 * 30 */ 31 32 /* 33 * l2cap.c 34 * 35 * Logical Link Control and Adaption Protocl (L2CAP) 36 * 37 * Created by Matthias Ringwald on 5/16/09. 38 */ 39 40 #include "l2cap.h" 41 42 #include <stdarg.h> 43 #include <string.h> 44 45 #include <stdio.h> 46 47 // size of HCI ACL + L2CAP Header for regular data packets 48 #define COMPLETE_L2CAP_HEADER 8 49 50 static void null_event_handler(uint8_t *packet, uint16_t size); 51 static void null_data_handler(uint16_t source_cid, uint8_t *packet, uint16_t size); 52 53 static uint8_t * sig_buffer = NULL; 54 static linked_list_t l2cap_channels = NULL; 55 static uint8_t * acl_buffer = NULL; 56 static void (*event_packet_handler) (uint8_t *packet, uint16_t size) = null_event_handler; 57 static void (*data_packet_handler) (uint16_t source_cid, uint8_t *packet, uint16_t size) = null_data_handler; 58 static connection_t * capture_connection = NULL; 59 60 void l2cap_init(){ 61 sig_buffer = malloc( 48 ); 62 acl_buffer = malloc( 255 + 8 ); 63 64 // 65 // register callbacks with HCI 66 // 67 hci_register_event_packet_handler(&l2cap_event_handler); 68 hci_register_acl_packet_handler(&l2cap_acl_handler); 69 } 70 71 72 /** Register L2CAP packet handlers */ 73 static void null_event_handler(uint8_t *packet, uint16_t size){ 74 } 75 static void null_data_handler(uint16_t source_cid, uint8_t *packet, uint16_t size){ 76 } 77 void l2cap_register_event_packet_handler(void (*handler)(uint8_t *packet, uint16_t size)){ 78 event_packet_handler = handler; 79 } 80 void l2cap_register_data_packet_handler (void (*handler)(uint16_t source_cid, uint8_t *packet, uint16_t size)){ 81 data_packet_handler = handler; 82 } 83 84 int l2cap_send_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, ...){ 85 va_list argptr; 86 va_start(argptr, identifier); 87 uint16_t len = l2cap_create_signaling_internal(sig_buffer, handle, cmd, identifier, argptr); 88 va_end(argptr); 89 return hci_send_acl_packet(sig_buffer, len); 90 } 91 92 l2cap_channel_t * l2cap_get_channel_for_source_cid(uint16_t source_cid){ 93 linked_item_t *it; 94 l2cap_channel_t * channel; 95 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 96 channel = (l2cap_channel_t *) it; 97 if ( channel->source_cid == source_cid) { 98 return channel; 99 } 100 } 101 return NULL; 102 } 103 104 // open outgoing L2CAP channel 105 void l2cap_create_channel_internal(connection_t * connection, bd_addr_t address, uint16_t psm){ 106 107 // alloc structure 108 l2cap_channel_t * chan = malloc(sizeof(l2cap_channel_t)); 109 // TODO: emit error event 110 if (!chan) return; 111 112 // fill in 113 BD_ADDR_COPY(chan->address, address); 114 chan->psm = psm; 115 chan->handle = 0; 116 chan->connection = connection; 117 118 // set initial state 119 chan->state = L2CAP_STATE_CLOSED; 120 chan->sig_id = L2CAP_SIG_ID_INVALID; 121 122 // add to connections list 123 linked_list_add(&l2cap_channels, (linked_item_t *) chan); 124 125 // send connection request 126 // BD_ADDR, Packet_Type, Page_Scan_Repetition_Mode, Reserved, Clock_Offset, Allow_Role_Switch 127 hci_send_cmd(&hci_create_connection, address, 0x18, 0, 0, 0, 0); 128 } 129 130 void l2cap_disconnect_internal(uint16_t source_cid, uint8_t reason){ 131 // find channel for source_cid 132 l2cap_channel_t * channel = l2cap_get_channel_for_source_cid(source_cid); 133 if (channel) { 134 channel->sig_id = l2cap_next_sig_id(); 135 l2cap_send_signaling_packet( channel->handle, DISCONNECTION_REQUEST, channel->sig_id, channel->dest_cid, channel->source_cid); 136 channel->state = L2CAP_STATE_WAIT_DISCONNECT; 137 } 138 } 139 140 141 void l2cap_event_handler( uint8_t *packet, uint16_t size ){ 142 // handle connection complete events 143 if (packet[0] == HCI_EVENT_CONNECTION_COMPLETE) { 144 bd_addr_t address; 145 bt_flip_addr(address, &packet[5]); 146 147 linked_item_t *it; 148 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 149 l2cap_channel_t * channel = (l2cap_channel_t *) it; 150 if ( ! BD_ADDR_CMP( channel->address, address) ){ 151 if (channel->state == L2CAP_STATE_CLOSED) { 152 if (packet[2] == 0){ 153 // success, start l2cap handshake 154 channel->handle = READ_BT_16(packet, 3); 155 channel->sig_id = l2cap_next_sig_id(); 156 channel->source_cid = l2cap_next_source_cid(); 157 channel->state = L2CAP_STATE_WAIT_CONNECT_RSP; 158 l2cap_send_signaling_packet( channel->handle, CONNECTION_REQUEST, channel->sig_id, channel->psm, channel->source_cid); 159 } else { 160 // failure, forward error code 161 l2cap_emit_channel_opened(channel, packet[2]); 162 // discard channel 163 linked_list_remove(&l2cap_channels, (linked_item_t *) channel); 164 free (channel); 165 166 } 167 } 168 } 169 } 170 } 171 172 // handle disconnection complete events 173 if (packet[0] == HCI_EVENT_DISCONNECTION_COMPLETE) { 174 // send l2cap disconnect events for all channels on this handle 175 hci_con_handle_t handle = READ_BT_16(packet, 3); 176 linked_item_t *it; 177 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 178 l2cap_channel_t * channel = (l2cap_channel_t *) it; 179 if ( channel->handle == handle ){ 180 l2cap_finialize_channel_close(channel); 181 } 182 } 183 } 184 185 // HCI Connection Timeouts 186 if (packet[0] == L2CAP_EVENT_TIMEOUT_CHECK && !capture_connection){ 187 hci_con_handle_t handle = READ_BT_16(packet, 2); 188 linked_item_t *it; 189 l2cap_channel_t * channel; 190 int used = 0; 191 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 192 channel = (l2cap_channel_t *) it; 193 if (channel->handle == handle) { 194 used = 1; 195 } 196 } 197 if (!used) { 198 hci_send_cmd(&hci_disconnect, handle, 0x13); // remote closed connection 199 } 200 } 201 202 (*event_packet_handler)(packet, size); 203 } 204 205 static l2cap_handle_disconnect_request(l2cap_channel_t *channel, uint16_t identifier){ 206 l2cap_send_signaling_packet( channel->handle, DISCONNECTION_RESPONSE, identifier, channel->dest_cid, channel->source_cid); 207 l2cap_finialize_channel_close(channel); 208 } 209 210 void l2cap_signaling_handler(l2cap_channel_t *channel, uint8_t *packet, uint16_t size){ 211 212 static uint8_t config_options[] = { 1, 2, 150, 0}; // mtu = 48 213 214 uint8_t code = READ_L2CAP_SIGNALING_CODE( packet ); 215 uint8_t identifier = READ_L2CAP_SIGNALING_IDENTIFIER( packet ); 216 uint16_t result = 0; 217 218 switch (channel->state) { 219 220 case L2CAP_STATE_WAIT_CONNECT_RSP: 221 switch (code){ 222 case CONNECTION_RESPONSE: 223 result = READ_BT_16 (packet, L2CAP_SIGNALING_DATA_OFFSET+4); 224 switch (result) { 225 case 0: 226 // successfull connection 227 channel->dest_cid = READ_BT_16(packet, L2CAP_SIGNALING_DATA_OFFSET); 228 channel->sig_id = l2cap_next_sig_id(); 229 l2cap_send_signaling_packet(channel->handle, CONFIGURE_REQUEST, channel->sig_id, channel->dest_cid, 0, 4, &config_options); 230 channel->state = L2CAP_STATE_WAIT_CONFIG_REQ_RSP_OR_CONFIG_REQ; 231 break; 232 case 1: 233 // connection pending. get some coffee 234 break; 235 default: 236 // map l2cap connection response result to BTstack status enumeration 237 l2cap_emit_channel_opened(channel, L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result); 238 break; 239 } 240 break; 241 242 case DISCONNECTION_REQUEST: 243 l2cap_handle_disconnect_request(channel, identifier); 244 break; 245 246 default: 247 //@TODO: implement other signaling packets 248 break; 249 } 250 break; 251 252 case L2CAP_STATE_WAIT_CONFIG_REQ_RSP_OR_CONFIG_REQ: 253 switch (code) { 254 case CONFIGURE_RESPONSE: 255 channel->state = L2CAP_STATE_WAIT_CONFIG_REQ; 256 break; 257 case CONFIGURE_REQUEST: 258 // accept the other's configuration options 259 l2cap_send_signaling_packet(channel->handle, CONFIGURE_RESPONSE, identifier, channel->dest_cid, 0, 0, size - 16, &packet[16]); 260 channel->state = L2CAP_STATE_WAIT_CONFIG_REQ_RSP; 261 break; 262 case DISCONNECTION_REQUEST: 263 l2cap_handle_disconnect_request(channel, identifier); 264 break; 265 default: 266 //@TODO: implement other signaling packets 267 break; 268 } 269 break; 270 271 case L2CAP_STATE_WAIT_CONFIG_REQ: 272 switch (code) { 273 case CONFIGURE_REQUEST: 274 // accept the other's configuration options 275 l2cap_send_signaling_packet(channel->handle, CONFIGURE_RESPONSE, identifier, channel->dest_cid, 0, 0, size - 16, &packet[16]); 276 channel->state = L2CAP_STATE_OPEN; 277 l2cap_emit_channel_opened(channel, 0); // success 278 break; 279 case DISCONNECTION_REQUEST: 280 l2cap_handle_disconnect_request(channel, identifier); 281 break; 282 default: 283 //@TODO: implement other signaling packets 284 break; 285 } 286 break; 287 288 case L2CAP_STATE_WAIT_CONFIG_REQ_RSP: 289 switch (code) { 290 case CONFIGURE_RESPONSE: 291 channel->state = L2CAP_STATE_OPEN; 292 l2cap_emit_channel_opened(channel, 0); // success 293 break; 294 case DISCONNECTION_REQUEST: 295 l2cap_handle_disconnect_request(channel, identifier); 296 break; 297 default: 298 //@TODO: implement other signaling packets 299 break; 300 } 301 break; 302 303 case L2CAP_STATE_WAIT_DISCONNECT: 304 switch (code) { 305 case DISCONNECTION_RESPONSE: 306 l2cap_finialize_channel_close(channel); 307 break; 308 case DISCONNECTION_REQUEST: 309 l2cap_handle_disconnect_request(channel, identifier); 310 break; 311 default: 312 //@TODO: implement other signaling packets 313 break; 314 } 315 break; 316 317 case L2CAP_STATE_CLOSED: 318 // @TODO handle incoming requests 319 break; 320 321 case L2CAP_STATE_OPEN: 322 switch (code) { 323 case DISCONNECTION_REQUEST: 324 l2cap_handle_disconnect_request(channel, identifier); 325 break; 326 default: 327 //@TODO: implement other signaling packets, e.g. re-configure 328 break; 329 } 330 break; 331 } 332 } 333 334 // finalize closed channel 335 void l2cap_finialize_channel_close(l2cap_channel_t *channel){ 336 channel->state = L2CAP_STATE_CLOSED; 337 l2cap_emit_channel_closed(channel); 338 339 // discard channel 340 linked_list_remove(&l2cap_channels, (linked_item_t *) channel); 341 free (channel); 342 } 343 344 // 345 void l2cap_close_channels_for_connection(connection_t *connection){ 346 linked_item_t *it; 347 l2cap_channel_t * channel; 348 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 349 channel = (l2cap_channel_t *) it; 350 if ( channel->connection == connection) { 351 channel->sig_id = l2cap_next_sig_id(); 352 l2cap_send_signaling_packet( channel->handle, DISCONNECTION_REQUEST, channel->sig_id, channel->dest_cid, channel->source_cid); 353 channel->state = L2CAP_STATE_WAIT_DISCONNECT; 354 } 355 } 356 } 357 358 // notify client 359 void l2cap_emit_channel_opened(l2cap_channel_t *channel, uint8_t status) { 360 uint8_t event[17]; 361 event[0] = L2CAP_EVENT_CHANNEL_OPENED; 362 event[1] = sizeof(event) - 2; 363 event[2] = status; 364 bt_flip_addr(&event[3], channel->address); 365 bt_store_16(event, 9, channel->handle); 366 bt_store_16(event, 11, channel->psm); 367 bt_store_16(event, 13, channel->source_cid); 368 bt_store_16(event, 15, channel->dest_cid); 369 socket_connection_send_packet(channel->connection, HCI_EVENT_PACKET, 0, event, sizeof(event)); 370 } 371 372 void l2cap_emit_channel_closed(l2cap_channel_t *channel) { 373 uint8_t event[4]; 374 event[0] = L2CAP_EVENT_CHANNEL_CLOSED; 375 event[1] = sizeof(event) - 2; 376 bt_store_16(event, 2, channel->source_cid); 377 socket_connection_send_packet(channel->connection, HCI_EVENT_PACKET, 0, event, sizeof(event)); 378 } 379 380 void l2cap_acl_handler( uint8_t *packet, uint16_t size ){ 381 382 // Capturing? 383 if (capture_connection) { 384 socket_connection_send_packet(capture_connection, HCI_ACL_DATA_PACKET, 0, packet, size); 385 return; 386 } 387 388 // forward to higher layers - not needed yet 389 // (*data_packet_handler)(channel_id, packet, size); 390 391 // Get Channel ID and command code 392 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 393 uint8_t code = READ_L2CAP_SIGNALING_CODE( packet ); 394 395 // Get Connection 396 hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet); 397 398 // Signaling Packet? 399 if (channel_id == 1) { 400 401 if (code < 1 || code == 2 || code >= 8){ 402 // not for a particular channel 403 return; 404 } 405 406 // Get Signaling Identifier and potential destination CID 407 uint8_t sig_id = READ_L2CAP_SIGNALING_IDENTIFIER(packet); 408 uint16_t dest_cid = READ_BT_16(packet, L2CAP_SIGNALING_DATA_OFFSET); 409 410 // Find channel for this sig_id and connection handle 411 linked_item_t *it; 412 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 413 l2cap_channel_t * channel = (l2cap_channel_t *) it; 414 if (channel->handle == handle) { 415 if (code & 1) { 416 // match odd commands by previous signaling identifier 417 if (channel->sig_id == sig_id) { 418 l2cap_signaling_handler( channel, packet, size); 419 } 420 } else { 421 // match even commands by source channel id 422 if (channel->source_cid == dest_cid) { 423 l2cap_signaling_handler( channel, packet, size); 424 } 425 } 426 } 427 } 428 return; 429 } 430 431 // Find channel for this channel_id and connection handle 432 l2cap_channel_t * channel = l2cap_get_channel_for_source_cid(channel_id); 433 if (channel) { 434 socket_connection_send_packet(channel->connection, L2CAP_DATA_PACKET, channel_id, 435 &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 436 } 437 } 438 439 440 void l2cap_send_internal(uint16_t source_cid, uint8_t *data, uint16_t len){ 441 // find channel for source_cid, construct l2cap packet and send 442 l2cap_channel_t * channel = l2cap_get_channel_for_source_cid(source_cid); 443 if (channel) { 444 // 0 - Connection handle : PB=10 : BC=00 445 bt_store_16(acl_buffer, 0, channel->handle | (2 << 12) | (0 << 14)); 446 // 2 - ACL length 447 bt_store_16(acl_buffer, 2, len + 4); 448 // 4 - L2CAP packet length 449 bt_store_16(acl_buffer, 4, len + 0); 450 // 6 - L2CAP channel DEST 451 bt_store_16(acl_buffer, 6, channel->dest_cid); 452 // 8 - data 453 memcpy(&acl_buffer[8], data, len); 454 // send 455 hci_send_acl_packet(acl_buffer, len+8); 456 } 457 } 458 459 void l2cap_set_capture_connection(connection_t * connection){ 460 capture_connection = connection; 461 } 462 463