1 /* 2 * l2cap.c 3 * 4 * Logical Link Control and Adaption Protocl (L2CAP) 5 * 6 * Created by Matthias Ringwald on 5/16/09. 7 */ 8 9 #include "l2cap.h" 10 11 #include <stdarg.h> 12 #include <string.h> 13 14 #include <stdio.h> 15 16 // size of HCI ACL + L2CAP Header for regular data packets 17 #define COMPLETE_L2CAP_HEADER 8 18 19 static void null_event_handler(uint8_t *packet, uint16_t size); 20 static void null_data_handler(uint16_t source_cid, uint8_t *packet, uint16_t size); 21 22 static uint8_t * sig_buffer = NULL; 23 static linked_list_t l2cap_channels = NULL; 24 static uint8_t * acl_buffer = NULL; 25 static void (*event_packet_handler) (uint8_t *packet, uint16_t size) = null_event_handler; 26 static void (*data_packet_handler) (uint16_t source_cid, uint8_t *packet, uint16_t size) = null_data_handler; 27 static connection_t * capture_connection = NULL; 28 29 void l2cap_init(){ 30 sig_buffer = malloc( 48 ); 31 acl_buffer = malloc( 255 + 8 ); 32 33 // 34 // register callbacks with HCI 35 // 36 hci_register_event_packet_handler(&l2cap_event_handler); 37 hci_register_acl_packet_handler(&l2cap_acl_handler); 38 } 39 40 41 /** Register L2CAP packet handlers */ 42 static void null_event_handler(uint8_t *packet, uint16_t size){ 43 } 44 static void null_data_handler(uint16_t source_cid, uint8_t *packet, uint16_t size){ 45 } 46 void l2cap_register_event_packet_handler(void (*handler)(uint8_t *packet, uint16_t size)){ 47 event_packet_handler = handler; 48 } 49 void l2cap_register_data_packet_handler (void (*handler)(uint16_t source_cid, uint8_t *packet, uint16_t size)){ 50 data_packet_handler = handler; 51 } 52 53 int l2cap_send_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, ...){ 54 va_list argptr; 55 va_start(argptr, identifier); 56 uint16_t len = l2cap_create_signaling_internal(sig_buffer, handle, cmd, identifier, argptr); 57 va_end(argptr); 58 return hci_send_acl_packet(sig_buffer, len); 59 } 60 61 l2cap_channel_t * l2cap_get_channel_for_source_cid(uint16_t source_cid){ 62 linked_item_t *it; 63 l2cap_channel_t * channel; 64 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 65 channel = (l2cap_channel_t *) it; 66 if ( channel->source_cid == source_cid) { 67 return channel; 68 } 69 } 70 return NULL; 71 } 72 73 // open outgoing L2CAP channel 74 void l2cap_create_channel_internal(connection_t * connection, bd_addr_t address, uint16_t psm){ 75 76 // alloc structure 77 l2cap_channel_t * chan = malloc(sizeof(l2cap_channel_t)); 78 // TODO: emit error event 79 if (!chan) return; 80 81 // fill in 82 BD_ADDR_COPY(chan->address, address); 83 chan->psm = psm; 84 chan->handle = 0; 85 chan->connection = connection; 86 87 // set initial state 88 chan->state = L2CAP_STATE_CLOSED; 89 chan->sig_id = L2CAP_SIG_ID_INVALID; 90 91 // add to connections list 92 linked_list_add(&l2cap_channels, (linked_item_t *) chan); 93 94 // send connection request 95 // BD_ADDR, Packet_Type, Page_Scan_Repetition_Mode, Reserved, Clock_Offset, Allow_Role_Switch 96 hci_send_cmd(&hci_create_connection, address, 0x18, 0, 0, 0, 0); 97 } 98 99 void l2cap_disconnect_internal(uint16_t source_cid, uint8_t reason){ 100 // find channel for source_cid 101 l2cap_channel_t * channel = l2cap_get_channel_for_source_cid(source_cid); 102 if (channel) { 103 channel->sig_id = l2cap_next_sig_id(); 104 l2cap_send_signaling_packet( channel->handle, DISCONNECTION_REQUEST, channel->sig_id, channel->dest_cid, channel->source_cid); 105 channel->state = L2CAP_STATE_WAIT_DISCONNECT; 106 } 107 } 108 109 110 void l2cap_event_handler( uint8_t *packet, uint16_t size ){ 111 // handle connection complete events 112 if (packet[0] == HCI_EVENT_CONNECTION_COMPLETE) { 113 bd_addr_t address; 114 bt_flip_addr(address, &packet[5]); 115 116 linked_item_t *it; 117 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 118 l2cap_channel_t * chan = (l2cap_channel_t *) it; 119 if ( ! BD_ADDR_CMP( chan->address, address) ){ 120 if (chan->state == L2CAP_STATE_CLOSED) { 121 if (packet[2] == 0){ 122 chan->handle = READ_BT_16(packet, 3); 123 chan->sig_id = l2cap_next_sig_id(); 124 chan->source_cid = l2cap_next_source_cid(); 125 126 l2cap_send_signaling_packet( chan->handle, CONNECTION_REQUEST, chan->sig_id, chan->psm, chan->source_cid); 127 128 chan->state = L2CAP_STATE_WAIT_CONNECT_RSP; 129 } else { 130 l2cap_emit_channel_opened(chan, packet[2]); // failure, forward error code 131 } 132 } 133 } 134 } 135 } 136 137 // handle disconnection complete events 138 if (packet[0] == HCI_EVENT_DISCONNECTION_COMPLETE) { 139 // send l2cap disconnect events for all channels on this handle 140 hci_con_handle_t handle = READ_BT_16(packet, 3); 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 ( channel->handle == handle ){ 145 l2cap_finialize_channel_close(channel); 146 } 147 } 148 } 149 150 // HCI Connection Timeouts 151 if (packet[0] == L2CAP_EVENT_TIMEOUT_CHECK){ 152 hci_con_handle_t handle = READ_BT_16(packet, 2); 153 linked_item_t *it; 154 l2cap_channel_t * channel; 155 int used = 0; 156 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 157 channel = (l2cap_channel_t *) it; 158 if (channel->handle == handle) { 159 used = 1; 160 } 161 } 162 if (!used) { 163 hci_send_cmd(&hci_disconnect, handle, 0x13); // remote closed connection 164 } 165 } 166 167 (*event_packet_handler)(packet, size); 168 } 169 170 void l2cap_signaling_handler(l2cap_channel_t *channel, uint8_t *packet, uint16_t size){ 171 172 static uint8_t config_options[] = { 1, 2, 150, 0}; // mtu = 48 173 174 uint8_t code = READ_L2CAP_SIGNALING_CODE( packet ); 175 uint8_t identifier = READ_L2CAP_SIGNALING_IDENTIFIER( packet ); 176 uint16_t result = 0; 177 178 switch (channel->state) { 179 180 case L2CAP_STATE_WAIT_CONNECT_RSP: 181 switch (code){ 182 case CONNECTION_RESPONSE: 183 result = READ_BT_16 (packet, L2CAP_SIGNALING_DATA_OFFSET+8); 184 switch (result) { 185 case 0: 186 // successfull connection 187 channel->dest_cid = READ_BT_16(packet, L2CAP_SIGNALING_DATA_OFFSET); 188 channel->sig_id = l2cap_next_sig_id(); 189 l2cap_send_signaling_packet(channel->handle, CONFIGURE_REQUEST, channel->sig_id, channel->dest_cid, 0, 4, &config_options); 190 channel->state = L2CAP_STATE_WAIT_CONFIG_REQ_RSP; 191 break; 192 case 1: 193 // connection pending. get some coffee 194 break; 195 default: 196 // map l2cap connection response result to BTstack status enumeration 197 l2cap_emit_channel_opened(channel, L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result); 198 break; 199 } 200 break; 201 202 default: 203 //@TODO: implement other signaling packets 204 break; 205 } 206 break; 207 208 case L2CAP_STATE_WAIT_CONFIG_REQ_RSP: 209 switch (code) { 210 case CONFIGURE_RESPONSE: 211 channel->state = L2CAP_STATE_WAIT_CONFIG_REQ; 212 break; 213 } 214 break; 215 216 case L2CAP_STATE_WAIT_CONFIG_REQ: 217 switch (code) { 218 case CONFIGURE_REQUEST: 219 220 // accept the other's configuration options 221 l2cap_send_signaling_packet(channel->handle, CONFIGURE_RESPONSE, identifier, channel->dest_cid, 0, 0, size - 16, &packet[16]); 222 223 channel->state = L2CAP_STATE_OPEN; 224 l2cap_emit_channel_opened(channel, 0); // success 225 break; 226 } 227 break; 228 229 case L2CAP_STATE_WAIT_DISCONNECT: 230 switch (code) { 231 case DISCONNECTION_RESPONSE: 232 l2cap_finialize_channel_close(channel); 233 break; 234 } 235 break; 236 } 237 } 238 239 // finalize closed channel 240 void l2cap_finialize_channel_close(l2cap_channel_t *channel){ 241 channel->state = L2CAP_STATE_CLOSED; 242 l2cap_emit_channel_closed(channel); 243 244 // discard channel 245 linked_list_remove(&l2cap_channels, (linked_item_t *) channel); 246 free (channel); 247 } 248 249 // 250 void l2cap_close_channels_for_connection(connection_t *connection){ 251 linked_item_t *it; 252 l2cap_channel_t * channel; 253 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 254 channel = (l2cap_channel_t *) it; 255 if ( channel->connection == connection) { 256 channel->sig_id = l2cap_next_sig_id(); 257 l2cap_send_signaling_packet( channel->handle, DISCONNECTION_REQUEST, channel->sig_id, channel->dest_cid, channel->source_cid); 258 channel->state = L2CAP_STATE_WAIT_DISCONNECT; 259 } 260 } 261 } 262 263 // notify client 264 void l2cap_emit_channel_opened(l2cap_channel_t *channel, uint8_t status) { 265 uint8_t event[17]; 266 event[0] = L2CAP_EVENT_CHANNEL_OPENED; 267 event[1] = sizeof(event) - 2; 268 event[2] = status; 269 bt_flip_addr(&event[3], channel->address); 270 bt_store_16(event, 9, channel->handle); 271 bt_store_16(event, 11, channel->psm); 272 bt_store_16(event, 13, channel->source_cid); 273 bt_store_16(event, 15, channel->dest_cid); 274 socket_connection_send_packet(channel->connection, HCI_EVENT_PACKET, 0, event, sizeof(event)); 275 } 276 277 void l2cap_emit_channel_closed(l2cap_channel_t *channel) { 278 uint8_t event[4]; 279 event[0] = L2CAP_EVENT_CHANNEL_CLOSED; 280 event[1] = sizeof(event) - 2; 281 bt_store_16(event, 2, channel->source_cid); 282 socket_connection_send_packet(channel->connection, HCI_EVENT_PACKET, 0, event, sizeof(event)); 283 } 284 285 void l2cap_acl_handler( uint8_t *packet, uint16_t size ){ 286 287 // Capturing? 288 if (capture_connection) { 289 socket_connection_send_packet(capture_connection, HCI_ACL_DATA_PACKET, 0, packet, size); 290 } 291 292 // forward to higher layers - not needed yet 293 // (*data_packet_handler)(channel_id, packet, size); 294 295 // Get Channel ID and command code 296 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 297 uint8_t code = READ_L2CAP_SIGNALING_CODE( packet ); 298 299 // Get Connection 300 hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet); 301 302 // Signaling Packet? 303 if (channel_id == 1) { 304 305 if (code < 1 || code == 2 || code >= 8){ 306 // not for a particular channel 307 return; 308 } 309 310 // Get Signaling Identifier and potential destination CID 311 uint8_t sig_id = READ_L2CAP_SIGNALING_IDENTIFIER(packet); 312 uint16_t dest_cid = READ_BT_16(packet, L2CAP_SIGNALING_DATA_OFFSET); 313 314 // Find channel for this sig_id and connection handle 315 linked_item_t *it; 316 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 317 l2cap_channel_t * chan = (l2cap_channel_t *) it; 318 if (chan->handle == handle) { 319 if (code & 1) { 320 // match odd commands by previous signaling identifier 321 if (chan->sig_id == sig_id) { 322 l2cap_signaling_handler( chan, packet, size); 323 } 324 } else { 325 // match even commands by source channel id 326 if (chan->source_cid == dest_cid) { 327 l2cap_signaling_handler( chan, packet, size); 328 } 329 } 330 } 331 } 332 return; 333 } 334 335 // Find channel for this channel_id and connection handle 336 l2cap_channel_t * channel = l2cap_get_channel_for_source_cid(channel_id); 337 if (channel) { 338 socket_connection_send_packet(channel->connection, L2CAP_DATA_PACKET, channel_id, 339 &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 340 } 341 } 342 343 344 void l2cap_send_internal(uint16_t source_cid, uint8_t *data, uint16_t len){ 345 // find channel for source_cid, construct l2cap packet and send 346 l2cap_channel_t * channel = l2cap_get_channel_for_source_cid(source_cid); 347 if (channel) { 348 // 0 - Connection handle : PB=10 : BC=00 349 bt_store_16(acl_buffer, 0, channel->handle | (2 << 12) | (0 << 14)); 350 // 2 - ACL length 351 bt_store_16(acl_buffer, 2, len + 4); 352 // 4 - L2CAP packet length 353 bt_store_16(acl_buffer, 4, len + 0); 354 // 6 - L2CAP channel DEST 355 bt_store_16(acl_buffer, 6, channel->dest_cid); 356 // 8 - data 357 memcpy(&acl_buffer[8], data, len); 358 // send 359 hci_send_acl_packet(acl_buffer, len+8); 360 } 361 } 362 363 void l2cap_set_capture_connection(connection_t * connection){ 364 capture_connection = connection; 365 } 366 367