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 177 switch (channel->state) { 178 179 case L2CAP_STATE_WAIT_CONNECT_RSP: 180 switch (code){ 181 case CONNECTION_RESPONSE: 182 if ( READ_BT_16 (packet, L2CAP_SIGNALING_DATA_OFFSET+3) == 0){ 183 // successfull connection 184 channel->dest_cid = READ_BT_16(packet, L2CAP_SIGNALING_DATA_OFFSET + 0); 185 channel->sig_id = l2cap_next_sig_id(); 186 l2cap_send_signaling_packet(channel->handle, CONFIGURE_REQUEST, channel->sig_id, channel->dest_cid, 0, 4, &config_options); 187 channel->state = L2CAP_STATE_WAIT_CONFIG_REQ_RSP; 188 } else { 189 //@TODO use separate error codes 190 l2cap_emit_channel_opened(channel, READ_BT_16 (packet, L2CAP_SIGNALING_DATA_OFFSET+3)); // failure, forward error code 191 } 192 break; 193 //@TODO: implement other signaling packets 194 } 195 break; 196 197 case L2CAP_STATE_WAIT_CONFIG_REQ_RSP: 198 switch (code) { 199 case CONFIGURE_RESPONSE: 200 channel->state = L2CAP_STATE_WAIT_CONFIG_REQ; 201 break; 202 } 203 break; 204 205 case L2CAP_STATE_WAIT_CONFIG_REQ: 206 switch (code) { 207 case CONFIGURE_REQUEST: 208 209 // accept the other's configuration options 210 l2cap_send_signaling_packet(channel->handle, CONFIGURE_RESPONSE, identifier, channel->dest_cid, 0, 0, size - 16, &packet[16]); 211 212 channel->state = L2CAP_STATE_OPEN; 213 l2cap_emit_channel_opened(channel, 0); // success 214 break; 215 } 216 break; 217 218 case L2CAP_STATE_WAIT_DISCONNECT: 219 switch (code) { 220 case DISCONNECTION_RESPONSE: 221 l2cap_finialize_channel_close(channel); 222 break; 223 } 224 break; 225 } 226 } 227 228 // finalize closed channel 229 void l2cap_finialize_channel_close(l2cap_channel_t *channel){ 230 channel->state = L2CAP_STATE_CLOSED; 231 l2cap_emit_channel_closed(channel); 232 233 // discard channel 234 linked_list_remove(&l2cap_channels, (linked_item_t *) channel); 235 free (channel); 236 } 237 238 // 239 void l2cap_close_channels_for_connection(connection_t *connection){ 240 linked_item_t *it; 241 l2cap_channel_t * channel; 242 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 243 channel = (l2cap_channel_t *) it; 244 if ( channel->connection == connection) { 245 channel->sig_id = l2cap_next_sig_id(); 246 l2cap_send_signaling_packet( channel->handle, DISCONNECTION_REQUEST, channel->sig_id, channel->dest_cid, channel->source_cid); 247 channel->state = L2CAP_STATE_WAIT_DISCONNECT; 248 } 249 } 250 } 251 252 // notify client 253 void l2cap_emit_channel_opened(l2cap_channel_t *channel, uint8_t status) { 254 uint8_t event[17]; 255 event[0] = L2CAP_EVENT_CHANNEL_OPENED; 256 event[1] = sizeof(event) - 2; 257 event[2] = status; 258 bt_flip_addr(&event[3], channel->address); 259 bt_store_16(event, 9, channel->handle); 260 bt_store_16(event, 11, channel->psm); 261 bt_store_16(event, 13, channel->source_cid); 262 bt_store_16(event, 15, channel->dest_cid); 263 socket_connection_send_packet(channel->connection, HCI_EVENT_PACKET, 0, event, sizeof(event)); 264 } 265 266 void l2cap_emit_channel_closed(l2cap_channel_t *channel) { 267 uint8_t event[4]; 268 event[0] = L2CAP_EVENT_CHANNEL_CLOSED; 269 event[1] = sizeof(event) - 2; 270 bt_store_16(event, 2, channel->source_cid); 271 socket_connection_send_packet(channel->connection, HCI_EVENT_PACKET, 0, event, sizeof(event)); 272 } 273 274 void l2cap_acl_handler( uint8_t *packet, uint16_t size ){ 275 276 // Capturing? 277 if (capture_connection) { 278 socket_connection_send_packet(capture_connection, HCI_ACL_DATA_PACKET, 0, packet, size); 279 } 280 281 // forward to higher layers - not needed yet 282 // (*data_packet_handler)(channel_id, packet, size); 283 284 // Get Channel ID and command code 285 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 286 uint8_t code = READ_L2CAP_SIGNALING_CODE( packet ); 287 288 // Get Connection 289 hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet); 290 291 // Signaling Packet? 292 if (channel_id == 1) { 293 294 if (code < 1 || code == 2 || code >= 8){ 295 // not for a particular channel 296 return; 297 } 298 299 // Get Signaling Identifier and potential destination CID 300 uint8_t sig_id = READ_L2CAP_SIGNALING_IDENTIFIER(packet); 301 uint16_t dest_cid = READ_BT_16(packet, L2CAP_SIGNALING_DATA_OFFSET); 302 303 // Find channel for this sig_id and connection handle 304 linked_item_t *it; 305 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 306 l2cap_channel_t * chan = (l2cap_channel_t *) it; 307 if (chan->handle == handle) { 308 if (code & 1) { 309 // match odd commands by previous signaling identifier 310 if (chan->sig_id == sig_id) { 311 l2cap_signaling_handler( chan, packet, size); 312 } 313 } else { 314 // match even commands by source channel id 315 if (chan->source_cid == dest_cid) { 316 l2cap_signaling_handler( chan, packet, size); 317 } 318 } 319 } 320 } 321 return; 322 } 323 324 // Find channel for this channel_id and connection handle 325 l2cap_channel_t * channel = l2cap_get_channel_for_source_cid(channel_id); 326 if (channel) { 327 socket_connection_send_packet(channel->connection, L2CAP_DATA_PACKET, channel_id, 328 &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 329 } 330 } 331 332 333 void l2cap_send_internal(uint16_t source_cid, uint8_t *data, uint16_t len){ 334 // find channel for source_cid, construct l2cap packet and send 335 l2cap_channel_t * channel = l2cap_get_channel_for_source_cid(source_cid); 336 if (channel) { 337 // 0 - Connection handle : PB=10 : BC=00 338 bt_store_16(acl_buffer, 0, channel->handle | (2 << 12) | (0 << 14)); 339 // 2 - ACL length 340 bt_store_16(acl_buffer, 2, len + 4); 341 // 4 - L2CAP packet length 342 bt_store_16(acl_buffer, 4, len + 0); 343 // 6 - L2CAP channel DEST 344 bt_store_16(acl_buffer, 6, channel->dest_cid); 345 // 8 - data 346 memcpy(&acl_buffer[8], data, len); 347 // send 348 hci_send_acl_packet(acl_buffer, len+8); 349 } 350 } 351 352 void l2cap_set_capture_connection(connection_t * connection){ 353 capture_connection = connection; 354 } 355 356