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 static void l2cap_handle_connection_failed_for_addr(bd_addr_t address, uint8_t status){ 141 linked_item_t *it; 142 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 143 l2cap_channel_t * channel = (l2cap_channel_t *) it; 144 if ( ! BD_ADDR_CMP( channel->address, address) ){ 145 if (channel->state == L2CAP_STATE_CLOSED) { 146 // failure, forward error code 147 l2cap_emit_channel_opened(channel, status); 148 // discard channel 149 linked_list_remove(&l2cap_channels, (linked_item_t *) channel); 150 free (channel); 151 } 152 } 153 } 154 } 155 156 static void l2cap_handle_connection_success_for_addr(bd_addr_t address, hci_con_handle_t handle){ 157 linked_item_t *it; 158 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 159 l2cap_channel_t * channel = (l2cap_channel_t *) it; 160 if ( ! BD_ADDR_CMP( channel->address, address) ){ 161 if (channel->state == L2CAP_STATE_CLOSED) { 162 // success, start l2cap handshake 163 channel->handle = handle; 164 channel->sig_id = l2cap_next_sig_id(); 165 channel->source_cid = l2cap_next_source_cid(); 166 channel->state = L2CAP_STATE_WAIT_CONNECT_RSP; 167 l2cap_send_signaling_packet( channel->handle, CONNECTION_REQUEST, channel->sig_id, channel->psm, channel->source_cid); 168 } 169 } 170 } 171 } 172 173 void l2cap_event_handler( uint8_t *packet, uint16_t size ){ 174 175 bd_addr_t address; 176 hci_con_handle_t handle; 177 178 switch(packet[0]){ 179 180 // handle connection complete events 181 case HCI_EVENT_CONNECTION_COMPLETE: 182 bt_flip_addr(address, &packet[5]); 183 if (packet[2] == 0){ 184 handle = READ_BT_16(packet, 3); 185 l2cap_handle_connection_success_for_addr(address, handle); 186 } else { 187 l2cap_handle_connection_failed_for_addr(address, packet[2]); 188 } 189 break; 190 191 // handle successful create connection cancel command 192 case HCI_EVENT_COMMAND_COMPLETE: 193 if ( COMMAND_COMPLETE_EVENT(packet, hci_create_connection_cancel) ) { 194 if (packet[5] == 0){ 195 bt_flip_addr(address, &packet[6]); 196 // CONNECTION TERMINATED BY LOCAL HOST (0X16) 197 l2cap_handle_connection_failed_for_addr(address, 0x16); 198 } 199 } 200 break; 201 202 // handle disconnection complete events 203 case HCI_EVENT_DISCONNECTION_COMPLETE: 204 // send l2cap disconnect events for all channels on this handle 205 handle = READ_BT_16(packet, 3); 206 linked_item_t *it; 207 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 208 l2cap_channel_t * channel = (l2cap_channel_t *) it; 209 if ( channel->handle == handle ){ 210 l2cap_finialize_channel_close(channel); 211 } 212 } 213 break; 214 215 // HCI Connection Timeouts 216 case L2CAP_EVENT_TIMEOUT_CHECK: 217 if (!capture_connection){ 218 hci_con_handle_t handle = READ_BT_16(packet, 2); 219 linked_item_t *it; 220 l2cap_channel_t * channel; 221 int used = 0; 222 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 223 channel = (l2cap_channel_t *) it; 224 if (channel->handle == handle) { 225 used = 1; 226 } 227 } 228 if (!used) { 229 hci_send_cmd(&hci_disconnect, handle, 0x13); // remote closed connection 230 } 231 } 232 break; 233 234 default: 235 break; 236 } 237 238 // pass on 239 (*event_packet_handler)(packet, size); 240 } 241 242 static void l2cap_handle_disconnect_request(l2cap_channel_t *channel, uint16_t identifier){ 243 l2cap_send_signaling_packet( channel->handle, DISCONNECTION_RESPONSE, identifier, channel->dest_cid, channel->source_cid); 244 l2cap_finialize_channel_close(channel); 245 } 246 247 void l2cap_signaling_handler(l2cap_channel_t *channel, uint8_t *packet, uint16_t size){ 248 249 static uint8_t config_options[] = { 1, 2, 150, 0}; // mtu = 48 250 251 uint8_t code = READ_L2CAP_SIGNALING_CODE( packet ); 252 uint8_t identifier = READ_L2CAP_SIGNALING_IDENTIFIER( packet ); 253 uint16_t result = 0; 254 255 switch (channel->state) { 256 257 case L2CAP_STATE_WAIT_CONNECT_RSP: 258 switch (code){ 259 case CONNECTION_RESPONSE: 260 result = READ_BT_16 (packet, L2CAP_SIGNALING_DATA_OFFSET+4); 261 switch (result) { 262 case 0: 263 // successfull connection 264 channel->dest_cid = READ_BT_16(packet, L2CAP_SIGNALING_DATA_OFFSET); 265 channel->sig_id = l2cap_next_sig_id(); 266 l2cap_send_signaling_packet(channel->handle, CONFIGURE_REQUEST, channel->sig_id, channel->dest_cid, 0, 4, &config_options); 267 channel->state = L2CAP_STATE_WAIT_CONFIG_REQ_RSP_OR_CONFIG_REQ; 268 break; 269 case 1: 270 // connection pending. get some coffee 271 break; 272 default: 273 // map l2cap connection response result to BTstack status enumeration 274 l2cap_emit_channel_opened(channel, L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result); 275 break; 276 } 277 break; 278 279 case DISCONNECTION_REQUEST: 280 l2cap_handle_disconnect_request(channel, identifier); 281 break; 282 283 default: 284 //@TODO: implement other signaling packets 285 break; 286 } 287 break; 288 289 case L2CAP_STATE_WAIT_CONFIG_REQ_RSP_OR_CONFIG_REQ: 290 switch (code) { 291 case CONFIGURE_RESPONSE: 292 channel->state = L2CAP_STATE_WAIT_CONFIG_REQ; 293 break; 294 case CONFIGURE_REQUEST: 295 // accept the other's configuration options 296 l2cap_send_signaling_packet(channel->handle, CONFIGURE_RESPONSE, identifier, channel->dest_cid, 0, 0, size - 16, &packet[16]); 297 channel->state = L2CAP_STATE_WAIT_CONFIG_REQ_RSP; 298 break; 299 case DISCONNECTION_REQUEST: 300 l2cap_handle_disconnect_request(channel, identifier); 301 break; 302 default: 303 //@TODO: implement other signaling packets 304 break; 305 } 306 break; 307 308 case L2CAP_STATE_WAIT_CONFIG_REQ: 309 switch (code) { 310 case CONFIGURE_REQUEST: 311 // accept the other's configuration options 312 l2cap_send_signaling_packet(channel->handle, CONFIGURE_RESPONSE, identifier, channel->dest_cid, 0, 0, size - 16, &packet[16]); 313 channel->state = L2CAP_STATE_OPEN; 314 l2cap_emit_channel_opened(channel, 0); // success 315 break; 316 case DISCONNECTION_REQUEST: 317 l2cap_handle_disconnect_request(channel, identifier); 318 break; 319 default: 320 //@TODO: implement other signaling packets 321 break; 322 } 323 break; 324 325 case L2CAP_STATE_WAIT_CONFIG_REQ_RSP: 326 switch (code) { 327 case CONFIGURE_RESPONSE: 328 channel->state = L2CAP_STATE_OPEN; 329 l2cap_emit_channel_opened(channel, 0); // success 330 break; 331 case DISCONNECTION_REQUEST: 332 l2cap_handle_disconnect_request(channel, identifier); 333 break; 334 default: 335 //@TODO: implement other signaling packets 336 break; 337 } 338 break; 339 340 case L2CAP_STATE_WAIT_DISCONNECT: 341 switch (code) { 342 case DISCONNECTION_RESPONSE: 343 l2cap_finialize_channel_close(channel); 344 break; 345 case DISCONNECTION_REQUEST: 346 l2cap_handle_disconnect_request(channel, identifier); 347 break; 348 default: 349 //@TODO: implement other signaling packets 350 break; 351 } 352 break; 353 354 case L2CAP_STATE_CLOSED: 355 // @TODO handle incoming requests 356 break; 357 358 case L2CAP_STATE_OPEN: 359 switch (code) { 360 case DISCONNECTION_REQUEST: 361 l2cap_handle_disconnect_request(channel, identifier); 362 break; 363 default: 364 //@TODO: implement other signaling packets, e.g. re-configure 365 break; 366 } 367 break; 368 } 369 } 370 371 // finalize closed channel 372 void l2cap_finialize_channel_close(l2cap_channel_t *channel){ 373 channel->state = L2CAP_STATE_CLOSED; 374 l2cap_emit_channel_closed(channel); 375 376 // discard channel 377 linked_list_remove(&l2cap_channels, (linked_item_t *) channel); 378 free (channel); 379 } 380 381 // 382 void l2cap_close_channels_for_connection(connection_t *connection){ 383 linked_item_t *it; 384 l2cap_channel_t * channel; 385 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 386 channel = (l2cap_channel_t *) it; 387 if ( channel->connection == connection) { 388 channel->sig_id = l2cap_next_sig_id(); 389 l2cap_send_signaling_packet( channel->handle, DISCONNECTION_REQUEST, channel->sig_id, channel->dest_cid, channel->source_cid); 390 channel->state = L2CAP_STATE_WAIT_DISCONNECT; 391 } 392 } 393 } 394 395 // notify client 396 void l2cap_emit_channel_opened(l2cap_channel_t *channel, uint8_t status) { 397 uint8_t event[17]; 398 event[0] = L2CAP_EVENT_CHANNEL_OPENED; 399 event[1] = sizeof(event) - 2; 400 event[2] = status; 401 bt_flip_addr(&event[3], channel->address); 402 bt_store_16(event, 9, channel->handle); 403 bt_store_16(event, 11, channel->psm); 404 bt_store_16(event, 13, channel->source_cid); 405 bt_store_16(event, 15, channel->dest_cid); 406 socket_connection_send_packet(channel->connection, HCI_EVENT_PACKET, 0, event, sizeof(event)); 407 } 408 409 void l2cap_emit_channel_closed(l2cap_channel_t *channel) { 410 uint8_t event[4]; 411 event[0] = L2CAP_EVENT_CHANNEL_CLOSED; 412 event[1] = sizeof(event) - 2; 413 bt_store_16(event, 2, channel->source_cid); 414 socket_connection_send_packet(channel->connection, HCI_EVENT_PACKET, 0, event, sizeof(event)); 415 } 416 417 void l2cap_acl_handler( uint8_t *packet, uint16_t size ){ 418 419 // Capturing? 420 if (capture_connection) { 421 socket_connection_send_packet(capture_connection, HCI_ACL_DATA_PACKET, 0, packet, size); 422 return; 423 } 424 425 // forward to higher layers - not needed yet 426 // (*data_packet_handler)(channel_id, packet, size); 427 428 // Get Channel ID and command code 429 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 430 uint8_t code = READ_L2CAP_SIGNALING_CODE( packet ); 431 432 // Get Connection 433 hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet); 434 435 // Signaling Packet? 436 if (channel_id == 1) { 437 438 if (code < 1 || code == 2 || code >= 8){ 439 // not for a particular channel 440 return; 441 } 442 443 // Get Signaling Identifier and potential destination CID 444 uint8_t sig_id = READ_L2CAP_SIGNALING_IDENTIFIER(packet); 445 uint16_t dest_cid = READ_BT_16(packet, L2CAP_SIGNALING_DATA_OFFSET); 446 447 // Find channel for this sig_id and connection handle 448 linked_item_t *it; 449 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 450 l2cap_channel_t * channel = (l2cap_channel_t *) it; 451 if (channel->handle == handle) { 452 if (code & 1) { 453 // match odd commands by previous signaling identifier 454 if (channel->sig_id == sig_id) { 455 l2cap_signaling_handler( channel, packet, size); 456 } 457 } else { 458 // match even commands by source channel id 459 if (channel->source_cid == dest_cid) { 460 l2cap_signaling_handler( channel, packet, size); 461 } 462 } 463 } 464 } 465 return; 466 } 467 468 // Find channel for this channel_id and connection handle 469 l2cap_channel_t * channel = l2cap_get_channel_for_source_cid(channel_id); 470 if (channel) { 471 socket_connection_send_packet(channel->connection, L2CAP_DATA_PACKET, channel_id, 472 &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 473 } 474 } 475 476 477 void l2cap_send_internal(uint16_t source_cid, uint8_t *data, uint16_t len){ 478 // find channel for source_cid, construct l2cap packet and send 479 l2cap_channel_t * channel = l2cap_get_channel_for_source_cid(source_cid); 480 if (channel) { 481 // 0 - Connection handle : PB=10 : BC=00 482 bt_store_16(acl_buffer, 0, channel->handle | (2 << 12) | (0 << 14)); 483 // 2 - ACL length 484 bt_store_16(acl_buffer, 2, len + 4); 485 // 4 - L2CAP packet length 486 bt_store_16(acl_buffer, 4, len + 0); 487 // 6 - L2CAP channel DEST 488 bt_store_16(acl_buffer, 6, channel->dest_cid); 489 // 8 - data 490 memcpy(&acl_buffer[8], data, len); 491 // send 492 hci_send_acl_packet(acl_buffer, len+8); 493 } 494 } 495 496 void l2cap_set_capture_connection(connection_t * connection){ 497 capture_connection = connection; 498 } 499 500