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