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