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