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){ 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 void l2cap_signaling_handler(l2cap_channel_t *channel, uint8_t *packet, uint16_t size){ 202 203 static uint8_t config_options[] = { 1, 2, 150, 0}; // mtu = 48 204 205 uint8_t code = READ_L2CAP_SIGNALING_CODE( packet ); 206 uint8_t identifier = READ_L2CAP_SIGNALING_IDENTIFIER( packet ); 207 uint16_t result = 0; 208 209 switch (channel->state) { 210 211 case L2CAP_STATE_WAIT_CONNECT_RSP: 212 switch (code){ 213 case CONNECTION_RESPONSE: 214 result = READ_BT_16 (packet, L2CAP_SIGNALING_DATA_OFFSET+4); 215 switch (result) { 216 case 0: 217 // successfull connection 218 channel->dest_cid = READ_BT_16(packet, L2CAP_SIGNALING_DATA_OFFSET); 219 channel->sig_id = l2cap_next_sig_id(); 220 l2cap_send_signaling_packet(channel->handle, CONFIGURE_REQUEST, channel->sig_id, channel->dest_cid, 0, 4, &config_options); 221 channel->state = L2CAP_STATE_WAIT_CONFIG_REQ_RSP; 222 break; 223 case 1: 224 // connection pending. get some coffee 225 break; 226 default: 227 // map l2cap connection response result to BTstack status enumeration 228 l2cap_emit_channel_opened(channel, L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result); 229 break; 230 } 231 break; 232 233 default: 234 //@TODO: implement other signaling packets 235 break; 236 } 237 break; 238 239 case L2CAP_STATE_WAIT_CONFIG_REQ_RSP: 240 switch (code) { 241 case CONFIGURE_RESPONSE: 242 channel->state = L2CAP_STATE_WAIT_CONFIG_REQ; 243 break; 244 } 245 break; 246 247 case L2CAP_STATE_WAIT_CONFIG_REQ: 248 switch (code) { 249 case CONFIGURE_REQUEST: 250 251 // accept the other's configuration options 252 l2cap_send_signaling_packet(channel->handle, CONFIGURE_RESPONSE, identifier, channel->dest_cid, 0, 0, size - 16, &packet[16]); 253 254 channel->state = L2CAP_STATE_OPEN; 255 l2cap_emit_channel_opened(channel, 0); // success 256 break; 257 } 258 break; 259 260 case L2CAP_STATE_WAIT_DISCONNECT: 261 switch (code) { 262 case DISCONNECTION_RESPONSE: 263 l2cap_finialize_channel_close(channel); 264 break; 265 } 266 break; 267 } 268 } 269 270 // finalize closed channel 271 void l2cap_finialize_channel_close(l2cap_channel_t *channel){ 272 channel->state = L2CAP_STATE_CLOSED; 273 l2cap_emit_channel_closed(channel); 274 275 // discard channel 276 linked_list_remove(&l2cap_channels, (linked_item_t *) channel); 277 free (channel); 278 } 279 280 // 281 void l2cap_close_channels_for_connection(connection_t *connection){ 282 linked_item_t *it; 283 l2cap_channel_t * channel; 284 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 285 channel = (l2cap_channel_t *) it; 286 if ( channel->connection == connection) { 287 channel->sig_id = l2cap_next_sig_id(); 288 l2cap_send_signaling_packet( channel->handle, DISCONNECTION_REQUEST, channel->sig_id, channel->dest_cid, channel->source_cid); 289 channel->state = L2CAP_STATE_WAIT_DISCONNECT; 290 } 291 } 292 } 293 294 // notify client 295 void l2cap_emit_channel_opened(l2cap_channel_t *channel, uint8_t status) { 296 uint8_t event[17]; 297 event[0] = L2CAP_EVENT_CHANNEL_OPENED; 298 event[1] = sizeof(event) - 2; 299 event[2] = status; 300 bt_flip_addr(&event[3], channel->address); 301 bt_store_16(event, 9, channel->handle); 302 bt_store_16(event, 11, channel->psm); 303 bt_store_16(event, 13, channel->source_cid); 304 bt_store_16(event, 15, channel->dest_cid); 305 socket_connection_send_packet(channel->connection, HCI_EVENT_PACKET, 0, event, sizeof(event)); 306 } 307 308 void l2cap_emit_channel_closed(l2cap_channel_t *channel) { 309 uint8_t event[4]; 310 event[0] = L2CAP_EVENT_CHANNEL_CLOSED; 311 event[1] = sizeof(event) - 2; 312 bt_store_16(event, 2, channel->source_cid); 313 socket_connection_send_packet(channel->connection, HCI_EVENT_PACKET, 0, event, sizeof(event)); 314 } 315 316 void l2cap_acl_handler( uint8_t *packet, uint16_t size ){ 317 318 // Capturing? 319 if (capture_connection) { 320 socket_connection_send_packet(capture_connection, HCI_ACL_DATA_PACKET, 0, packet, size); 321 } 322 323 // forward to higher layers - not needed yet 324 // (*data_packet_handler)(channel_id, packet, size); 325 326 // Get Channel ID and command code 327 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 328 uint8_t code = READ_L2CAP_SIGNALING_CODE( packet ); 329 330 // Get Connection 331 hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet); 332 333 // Signaling Packet? 334 if (channel_id == 1) { 335 336 if (code < 1 || code == 2 || code >= 8){ 337 // not for a particular channel 338 return; 339 } 340 341 // Get Signaling Identifier and potential destination CID 342 uint8_t sig_id = READ_L2CAP_SIGNALING_IDENTIFIER(packet); 343 uint16_t dest_cid = READ_BT_16(packet, L2CAP_SIGNALING_DATA_OFFSET); 344 345 // Find channel for this sig_id and connection handle 346 linked_item_t *it; 347 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 348 l2cap_channel_t * chan = (l2cap_channel_t *) it; 349 if (chan->handle == handle) { 350 if (code & 1) { 351 // match odd commands by previous signaling identifier 352 if (chan->sig_id == sig_id) { 353 l2cap_signaling_handler( chan, packet, size); 354 } 355 } else { 356 // match even commands by source channel id 357 if (chan->source_cid == dest_cid) { 358 l2cap_signaling_handler( chan, packet, size); 359 } 360 } 361 } 362 } 363 return; 364 } 365 366 // Find channel for this channel_id and connection handle 367 l2cap_channel_t * channel = l2cap_get_channel_for_source_cid(channel_id); 368 if (channel) { 369 socket_connection_send_packet(channel->connection, L2CAP_DATA_PACKET, channel_id, 370 &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 371 } 372 } 373 374 375 void l2cap_send_internal(uint16_t source_cid, uint8_t *data, uint16_t len){ 376 // find channel for source_cid, construct l2cap packet and send 377 l2cap_channel_t * channel = l2cap_get_channel_for_source_cid(source_cid); 378 if (channel) { 379 // 0 - Connection handle : PB=10 : BC=00 380 bt_store_16(acl_buffer, 0, channel->handle | (2 << 12) | (0 << 14)); 381 // 2 - ACL length 382 bt_store_16(acl_buffer, 2, len + 4); 383 // 4 - L2CAP packet length 384 bt_store_16(acl_buffer, 4, len + 0); 385 // 6 - L2CAP channel DEST 386 bt_store_16(acl_buffer, 6, channel->dest_cid); 387 // 8 - data 388 memcpy(&acl_buffer[8], data, len); 389 // send 390 hci_send_acl_packet(acl_buffer, len+8); 391 } 392 } 393 394 void l2cap_set_capture_connection(connection_t * connection){ 395 capture_connection = connection; 396 } 397 398