1 /* 2 * Copyright (C) 2014 BlueKitchen GmbH 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 * 4. Any redistribution, use, or modification is done solely for 17 * personal benefit and not for any commercial purpose or for 18 * monetary gain. 19 * 20 * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS 21 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS 24 * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF 30 * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 31 * SUCH DAMAGE. 32 * 33 * Please inquire about commercial licensing options at 34 * [email protected] 35 * 36 */ 37 38 /* 39 * bnep.c 40 * Author: Ole Reinhardt <[email protected]> 41 * 42 */ 43 44 #include <stdio.h> 45 #include <stdlib.h> 46 #include <string.h> // memcpy 47 #include <stdint.h> 48 49 #include "hci_cmds.h" 50 #include "utils.h" 51 #include "classic/sdp_util.h" 52 53 #include "btstack_memory.h" 54 #include "hci.h" 55 #include "hci_dump.h" 56 #include "btstack_debug.h" 57 #include "bnep.h" 58 59 #include "l2cap.h" 60 61 #define BNEP_CONNECTION_TIMEOUT_MS 10000 62 #define BNEP_CONNECTION_MAX_RETRIES 1 63 64 static btstack_btstack_linked_list_t bnep_services = NULL; 65 static btstack_btstack_linked_list_t bnep_channels = NULL; 66 67 static gap_security_level_t bnep_security_level; 68 69 static void (*app_packet_handler)(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size); 70 71 72 static bnep_channel_t * bnep_channel_for_l2cap_cid(uint16_t l2cap_cid); 73 static void bnep_channel_finalize(bnep_channel_t *channel); 74 static void bnep_run(void); 75 static void bnep_channel_start_timer(bnep_channel_t *channel, int timeout); 76 inline static void bnep_channel_state_add(bnep_channel_t *channel, BNEP_CHANNEL_STATE_VAR event); 77 78 static void bnep_emit_open_channel_complete(bnep_channel_t *channel, uint8_t status) 79 { 80 log_info("BNEP_EVENT_OPEN_CHANNEL_COMPLETE status 0x%02x bd_addr: %s", status, bd_addr_to_str(channel->remote_addr)); 81 uint8_t event[3 + sizeof(bd_addr_t) + 3 * sizeof(uint16_t)]; 82 event[0] = BNEP_EVENT_OPEN_CHANNEL_COMPLETE; 83 event[1] = sizeof(event) - 2; 84 event[2] = status; 85 bt_store_16(event, 3, channel->uuid_source); 86 bt_store_16(event, 5, channel->uuid_dest); 87 bt_store_16(event, 7, channel->max_frame_size); 88 BD_ADDR_COPY(&event[9], channel->remote_addr); 89 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 90 (*app_packet_handler)(HCI_EVENT_PACKET, channel->l2cap_cid, (uint8_t *) event, sizeof(event)); 91 } 92 93 static void bnep_emit_channel_timeout(bnep_channel_t *channel) 94 { 95 log_info("BNEP_EVENT_CHANNEL_TIMEOUT bd_addr: %s", bd_addr_to_str(channel->remote_addr)); 96 uint8_t event[2 + sizeof(bd_addr_t) + 2 * sizeof(uint16_t) + sizeof(uint8_t)]; 97 event[0] = BNEP_EVENT_CHANNEL_TIMEOUT; 98 event[1] = sizeof(event) - 2; 99 bt_store_16(event, 2, channel->uuid_source); 100 bt_store_16(event, 4, channel->uuid_dest); 101 BD_ADDR_COPY(&event[6], channel->remote_addr); 102 event[12] = channel->state; 103 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 104 (*app_packet_handler)(HCI_EVENT_PACKET, channel->l2cap_cid, (uint8_t *) event, sizeof(event)); 105 } 106 107 static void bnep_emit_channel_closed(bnep_channel_t *channel) 108 { 109 log_info("BNEP_EVENT_CHANNEL_CLOSED bd_addr: %s", bd_addr_to_str(channel->remote_addr)); 110 uint8_t event[2 + sizeof(bd_addr_t) + 2 * sizeof(uint16_t)]; 111 event[0] = BNEP_EVENT_CHANNEL_CLOSED; 112 event[1] = sizeof(event) - 2; 113 bt_store_16(event, 2, channel->uuid_source); 114 bt_store_16(event, 4, channel->uuid_dest); 115 BD_ADDR_COPY(&event[6], channel->remote_addr); 116 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 117 (*app_packet_handler)(HCI_EVENT_PACKET, channel->l2cap_cid, (uint8_t *) event, sizeof(event)); 118 } 119 120 static void bnep_emit_ready_to_send(bnep_channel_t *channel) 121 { 122 uint8_t event[2]; 123 event[0] = BNEP_EVENT_READY_TO_SEND; 124 event[1] = sizeof(event) - 2; 125 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 126 (*app_packet_handler)(HCI_EVENT_PACKET, channel->l2cap_cid, (uint8_t *) event, sizeof(event)); 127 } 128 129 /* Send BNEP connection request */ 130 static int bnep_send_command_not_understood(bnep_channel_t *channel, uint8_t control_type) 131 { 132 uint8_t *bnep_out_buffer = NULL; 133 uint16_t pos = 0; 134 int err = 0; 135 136 if (channel->state == BNEP_CHANNEL_STATE_CLOSED) { 137 return -1; // TODO 138 } 139 140 l2cap_reserve_packet_buffer(); 141 bnep_out_buffer = l2cap_get_outgoing_buffer(); 142 143 /* Setup control packet type */ 144 bnep_out_buffer[pos++] = BNEP_PKT_TYPE_CONTROL; 145 bnep_out_buffer[pos++] = BNEP_CONTROL_TYPE_COMMAND_NOT_UNDERSTOOD; 146 147 /* Add not understood control type */ 148 bnep_out_buffer[pos++] = control_type; 149 150 err = l2cap_send_prepared(channel->l2cap_cid, pos); 151 152 if (err) { 153 // TODO: Log error 154 } 155 return err; 156 } 157 158 159 /* Send BNEP connection request */ 160 static int bnep_send_connection_request(bnep_channel_t *channel, uint16_t uuid_source, uint16_t uuid_dest) 161 { 162 uint8_t *bnep_out_buffer = NULL; 163 uint16_t pos = 0; 164 int err = 0; 165 166 if (channel->state == BNEP_CHANNEL_STATE_CLOSED) { 167 return -1; // TODO 168 } 169 170 l2cap_reserve_packet_buffer(); 171 bnep_out_buffer = l2cap_get_outgoing_buffer(); 172 173 /* Setup control packet type */ 174 bnep_out_buffer[pos++] = BNEP_PKT_TYPE_CONTROL; 175 bnep_out_buffer[pos++] = BNEP_CONTROL_TYPE_SETUP_CONNECTION_REQUEST; 176 177 /* Add UUID Size */ 178 bnep_out_buffer[pos++] = 2; 179 180 /* Add dest and source UUID */ 181 net_store_16(bnep_out_buffer, pos, uuid_dest); 182 pos += 2; 183 184 net_store_16(bnep_out_buffer, pos, uuid_source); 185 pos += 2; 186 187 err = l2cap_send_prepared(channel->l2cap_cid, pos); 188 189 if (err) { 190 // TODO: Log error 191 } 192 return err; 193 } 194 195 /* Send BNEP connection response */ 196 static int bnep_send_connection_response(bnep_channel_t *channel, uint16_t response_code) 197 { 198 uint8_t *bnep_out_buffer = NULL; 199 uint16_t pos = 0; 200 int err = 0; 201 202 if (channel->state == BNEP_CHANNEL_STATE_CLOSED) { 203 return -1; // TODO 204 } 205 206 l2cap_reserve_packet_buffer(); 207 bnep_out_buffer = l2cap_get_outgoing_buffer(); 208 209 /* Setup control packet type */ 210 bnep_out_buffer[pos++] = BNEP_PKT_TYPE_CONTROL; 211 bnep_out_buffer[pos++] = BNEP_CONTROL_TYPE_SETUP_CONNECTION_RESPONSE; 212 213 /* Add response code */ 214 net_store_16(bnep_out_buffer, pos, response_code); 215 pos += 2; 216 217 err = l2cap_send_prepared(channel->l2cap_cid, pos); 218 219 if (err) { 220 // TODO: Log error 221 } 222 return err; 223 } 224 225 /* Send BNEP filter net type set message */ 226 static int bnep_send_filter_net_type_set(bnep_channel_t *channel, bnep_net_filter_t *filter, uint16_t len) 227 { 228 uint8_t *bnep_out_buffer = NULL; 229 uint16_t pos = 0; 230 int err = 0; 231 int i; 232 233 if (channel->state == BNEP_CHANNEL_STATE_CLOSED) { 234 return -1; 235 } 236 237 l2cap_reserve_packet_buffer(); 238 bnep_out_buffer = l2cap_get_outgoing_buffer(); 239 240 /* Setup control packet type */ 241 bnep_out_buffer[pos++] = BNEP_PKT_TYPE_CONTROL; 242 bnep_out_buffer[pos++] = BNEP_CONTROL_TYPE_FILTER_NET_TYPE_SET; 243 244 net_store_16(bnep_out_buffer, pos, len * 2 * 2); 245 pos += 2; 246 247 for (i = 0; i < len; i ++) { 248 net_store_16(bnep_out_buffer, pos, filter[i].range_start); 249 pos += 2; 250 net_store_16(bnep_out_buffer, pos, filter[i].range_end); 251 pos += 2; 252 } 253 254 err = l2cap_send_prepared(channel->l2cap_cid, pos); 255 256 if (err) { 257 // TODO: Log error 258 } 259 return err; 260 } 261 262 /* Send BNEP filter net type response message */ 263 static int bnep_send_filter_net_type_response(bnep_channel_t *channel, uint16_t response_code) 264 { 265 uint8_t *bnep_out_buffer = NULL; 266 uint16_t pos = 0; 267 int err = 0; 268 269 if (channel->state == BNEP_CHANNEL_STATE_CLOSED) { 270 return -1; 271 } 272 273 l2cap_reserve_packet_buffer(); 274 bnep_out_buffer = l2cap_get_outgoing_buffer(); 275 276 /* Setup control packet type */ 277 bnep_out_buffer[pos++] = BNEP_PKT_TYPE_CONTROL; 278 bnep_out_buffer[pos++] = BNEP_CONTROL_TYPE_FILTER_NET_TYPE_RESPONSE; 279 280 /* Add response code */ 281 net_store_16(bnep_out_buffer, pos, response_code); 282 pos += 2; 283 284 err = l2cap_send_prepared(channel->l2cap_cid, pos); 285 286 if (err) { 287 // TODO: Log error 288 } 289 return err; 290 } 291 292 /* Send BNEP filter multicast address set message */ 293 294 static int bnep_send_filter_multi_addr_set(bnep_channel_t *channel, bnep_multi_filter_t *filter, uint16_t len) 295 { 296 uint8_t *bnep_out_buffer = NULL; 297 uint16_t pos = 0; 298 int err = 0; 299 int i; 300 301 if (channel->state == BNEP_CHANNEL_STATE_CLOSED) { 302 return -1; 303 } 304 305 l2cap_reserve_packet_buffer(); 306 bnep_out_buffer = l2cap_get_outgoing_buffer(); 307 308 /* Setup control packet type */ 309 bnep_out_buffer[pos++] = BNEP_PKT_TYPE_CONTROL; 310 bnep_out_buffer[pos++] = BNEP_CONTROL_TYPE_FILTER_MULTI_ADDR_SET; 311 312 net_store_16(bnep_out_buffer, pos, len * 2 * ETHER_ADDR_LEN); 313 pos += 2; 314 315 for (i = 0; i < len; i ++) { 316 BD_ADDR_COPY(bnep_out_buffer + pos, filter[i].addr_start); 317 pos += ETHER_ADDR_LEN; 318 BD_ADDR_COPY(bnep_out_buffer + pos, filter[i].addr_end); 319 pos += ETHER_ADDR_LEN; 320 } 321 322 err = l2cap_send_prepared(channel->l2cap_cid, pos); 323 324 if (err) { 325 // TODO: Log error 326 } 327 return err; 328 } 329 330 /* Send BNEP filter multicast address response message */ 331 static int bnep_send_filter_multi_addr_response(bnep_channel_t *channel, uint16_t response_code) 332 { 333 uint8_t *bnep_out_buffer = NULL; 334 uint16_t pos = 0; 335 int err = 0; 336 337 if (channel->state == BNEP_CHANNEL_STATE_CLOSED) { 338 return -1; 339 } 340 341 l2cap_reserve_packet_buffer(); 342 bnep_out_buffer = l2cap_get_outgoing_buffer(); 343 344 /* Setup control packet type */ 345 bnep_out_buffer[pos++] = BNEP_PKT_TYPE_CONTROL; 346 bnep_out_buffer[pos++] = BNEP_CONTROL_TYPE_FILTER_MULTI_ADDR_RESPONSE; 347 348 /* Add response code */ 349 net_store_16(bnep_out_buffer, pos, response_code); 350 pos += 2; 351 352 err = l2cap_send_prepared(channel->l2cap_cid, pos); 353 354 if (err) { 355 // TODO: Log error 356 } 357 return err; 358 } 359 360 int bnep_can_send_packet_now(uint16_t bnep_cid) 361 { 362 bnep_channel_t *channel = bnep_channel_for_l2cap_cid(bnep_cid); 363 364 if (!channel){ 365 log_error("bnep_can_send_packet_now cid 0x%02x doesn't exist!", bnep_cid); 366 return 0; 367 } 368 369 return l2cap_can_send_packet_now(channel->l2cap_cid); 370 } 371 372 373 static int bnep_filter_protocol(bnep_channel_t *channel, uint16_t network_protocol_type) 374 { 375 int i; 376 377 if (channel->net_filter_count == 0) { 378 /* No filter set */ 379 return 1; 380 } 381 382 for (i = 0; i < channel->net_filter_count; i ++) { 383 if ((network_protocol_type >= channel->net_filter[i].range_start) && 384 (network_protocol_type <= channel->net_filter[i].range_end)) { 385 return 1; 386 } 387 } 388 389 return 0; 390 } 391 392 static int bnep_filter_multicast(bnep_channel_t *channel, bd_addr_t addr_dest) 393 { 394 int i; 395 396 /* Check if the multicast flag is set int the destination address */ 397 if ((addr_dest[0] & 0x01) == 0x00) { 398 /* Not a multicast frame, do not apply filtering and send it in any case */ 399 return 1; 400 } 401 402 if (channel->multicast_filter_count == 0) { 403 /* No filter set */ 404 return 1; 405 } 406 407 for (i = 0; i < channel->multicast_filter_count; i ++) { 408 if ((memcmp(addr_dest, channel->multicast_filter[i].addr_start, sizeof(bd_addr_t)) >= 0) && 409 (memcmp(addr_dest, channel->multicast_filter[i].addr_end, sizeof(bd_addr_t)) <= 0)) { 410 return 1; 411 } 412 } 413 414 return 0; 415 } 416 417 418 /* Send BNEP ethernet packet */ 419 int bnep_send(uint16_t bnep_cid, uint8_t *packet, uint16_t len) 420 { 421 bnep_channel_t *channel; 422 uint8_t *bnep_out_buffer = NULL; 423 uint16_t pos = 0; 424 uint16_t pos_out = 0; 425 uint16_t payload_len; 426 int err = 0; 427 int has_source; 428 int has_dest; 429 430 bd_addr_t addr_dest; 431 bd_addr_t addr_source; 432 uint16_t network_protocol_type; 433 434 channel = bnep_channel_for_l2cap_cid(bnep_cid); 435 if (channel == NULL) { 436 log_error("bnep_send cid 0x%02x doesn't exist!", bnep_cid); 437 return 1; 438 } 439 440 if (channel->state != BNEP_CHANNEL_STATE_CONNECTED) { 441 return BNEP_CHANNEL_NOT_CONNECTED; 442 } 443 444 /* Check for free ACL buffers */ 445 if (!l2cap_can_send_packet_now(channel->l2cap_cid)) { 446 return BTSTACK_ACL_BUFFERS_FULL; 447 } 448 449 /* Extract destination and source address from the ethernet packet */ 450 pos = 0; 451 BD_ADDR_COPY(addr_dest, &packet[pos]); 452 pos += sizeof(bd_addr_t); 453 BD_ADDR_COPY(addr_source, &packet[pos]); 454 pos += sizeof(bd_addr_t); 455 network_protocol_type = READ_NET_16(packet, pos); 456 pos += sizeof(uint16_t); 457 458 payload_len = len - pos; 459 460 if (network_protocol_type == ETHERTYPE_VLAN) { /* IEEE 802.1Q tag header */ 461 if (payload_len < 4) { 462 /* Omit this packet */ 463 return 0; 464 } 465 /* The "real" network protocol type is 4 bytes ahead in a VLAN packet */ 466 network_protocol_type = READ_NET_16(packet, pos + 2); 467 } 468 469 /* Check network protocol and multicast filters before sending */ 470 if (!bnep_filter_protocol(channel, network_protocol_type) || 471 !bnep_filter_multicast(channel, addr_dest)) { 472 /* Packet did not pass filter... */ 473 if ((network_protocol_type == ETHERTYPE_VLAN) && 474 (payload_len >= 4)) { 475 /* The packet has been tagged as a with IEE 802.1Q tag and has been filtered out. 476 According to the spec the IEE802.1Q tag header shall be sended without ethernet payload. 477 So limit the payload_len to 4. 478 */ 479 payload_len = 4; 480 } else { 481 /* Packet is not tagged with IEE802.1Q header and was filtered out. Omit this packet */ 482 return 0; 483 } 484 } 485 486 /* Reserve l2cap packet buffer */ 487 l2cap_reserve_packet_buffer(); 488 bnep_out_buffer = l2cap_get_outgoing_buffer(); 489 490 /* Check if source address is the same as our local address and if the 491 destination address is the same as the remote addr. Maybe we can use 492 the compressed data format 493 */ 494 has_source = (memcmp(addr_source, channel->local_addr, ETHER_ADDR_LEN) != 0); 495 has_dest = (memcmp(addr_dest, channel->remote_addr, ETHER_ADDR_LEN) != 0); 496 497 /* Check for MTU limits */ 498 if (payload_len > channel->max_frame_size) { 499 log_error("bnep_send: Max frame size (%d) exceeded: %d", channel->max_frame_size, payload_len); 500 return BNEP_DATA_LEN_EXCEEDS_MTU; 501 } 502 503 /* Fill in the package type depending on the given source and destination address */ 504 if (has_source && has_dest) { 505 bnep_out_buffer[pos_out++] = BNEP_PKT_TYPE_GENERAL_ETHERNET; 506 } else 507 if (has_source && !has_dest) { 508 bnep_out_buffer[pos_out++] = BNEP_PKT_TYPE_COMPRESSED_ETHERNET_SOURCE_ONLY; 509 } else 510 if (!has_source && has_dest) { 511 bnep_out_buffer[pos_out++] = BNEP_PKT_TYPE_COMPRESSED_ETHERNET_DEST_ONLY; 512 } else { 513 bnep_out_buffer[pos_out++] = BNEP_PKT_TYPE_COMPRESSED_ETHERNET; 514 } 515 516 /* Add the destination address if needed */ 517 if (has_dest) { 518 BD_ADDR_COPY(bnep_out_buffer + pos_out, addr_dest); 519 pos_out += sizeof(bd_addr_t); 520 } 521 522 /* Add the source address if needed */ 523 if (has_source) { 524 BD_ADDR_COPY(bnep_out_buffer + pos_out, addr_source); 525 pos_out += sizeof(bd_addr_t); 526 } 527 528 /* Add protocol type */ 529 net_store_16(bnep_out_buffer, pos_out, network_protocol_type); 530 pos_out += 2; 531 532 /* TODO: Add extension headers, if we may support them at a later stage */ 533 /* Add the payload and then send out the package */ 534 memcpy(bnep_out_buffer + pos_out, packet + pos, payload_len); 535 pos_out += payload_len; 536 537 err = l2cap_send_prepared(channel->l2cap_cid, pos_out); 538 539 if (err) { 540 log_error("bnep_send: error %d", err); 541 } 542 return err; 543 } 544 545 546 /* Set BNEP network protocol type filter */ 547 int bnep_set_net_type_filter(uint16_t bnep_cid, bnep_net_filter_t *filter, uint16_t len) 548 { 549 bnep_channel_t *channel; 550 551 if (filter == NULL) { 552 return -1; 553 } 554 555 channel = bnep_channel_for_l2cap_cid(bnep_cid); 556 if (channel == NULL) { 557 log_error("bnep_set_net_type_filter cid 0x%02x doesn't exist!", bnep_cid); 558 return 1; 559 } 560 561 if (channel->state != BNEP_CHANNEL_STATE_CONNECTED) { 562 return BNEP_CHANNEL_NOT_CONNECTED; 563 } 564 565 if (len > MAX_BNEP_NETFILTER_OUT) { 566 return BNEP_DATA_LEN_EXCEEDS_MTU; 567 } 568 569 channel->net_filter_out = filter; 570 channel->net_filter_out_count = len; 571 572 /* Set flag to send out the network protocol type filter set reqeuest on next statemachine cycle */ 573 bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_SET); 574 bnep_run(); 575 576 return 0; 577 } 578 579 /* Set BNEP network protocol type filter */ 580 int bnep_set_multicast_filter(uint16_t bnep_cid, bnep_multi_filter_t *filter, uint16_t len) 581 { 582 bnep_channel_t *channel; 583 584 if (filter == NULL) { 585 return -1; 586 } 587 588 channel = bnep_channel_for_l2cap_cid(bnep_cid); 589 if (channel == NULL) { 590 log_error("bnep_set_net_type_filter cid 0x%02x doesn't exist!", bnep_cid); 591 return 1; 592 } 593 594 if (channel->state != BNEP_CHANNEL_STATE_CONNECTED) { 595 return BNEP_CHANNEL_NOT_CONNECTED; 596 } 597 598 if (len > MAX_BNEP_MULTICAST_FILTER_OUT) { 599 return BNEP_DATA_LEN_EXCEEDS_MTU; 600 } 601 602 channel->multicast_filter_out = filter; 603 channel->multicast_filter_out_count = len; 604 605 /* Set flag to send out the multicast filter set reqeuest on next statemachine cycle */ 606 bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_SET); 607 bnep_run(); 608 609 return 0; 610 } 611 612 /* BNEP timeout timer helper function */ 613 static void bnep_channel_timer_handler(timer_source_t *timer) 614 { 615 bnep_channel_t *channel = (bnep_channel_t *)btstack_linked_item_get_user((btstack_linked_item_t *) timer); 616 // retry send setup connection at least one time 617 if (channel->state == BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_RESPONSE){ 618 if (channel->retry_count < BNEP_CONNECTION_MAX_RETRIES){ 619 channel->retry_count++; 620 bnep_channel_start_timer(channel, BNEP_CONNECTION_TIMEOUT_MS); 621 bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_REQUEST); 622 bnep_run(); 623 return; 624 } 625 } 626 627 log_info( "bnep_channel_timeout_handler callback: shutting down connection!"); 628 bnep_emit_channel_timeout(channel); 629 bnep_channel_finalize(channel); 630 } 631 632 633 static void bnep_channel_stop_timer(bnep_channel_t *channel) 634 { 635 if (channel->timer_active) { 636 run_loop_remove_timer(&channel->timer); 637 channel->timer_active = 0; 638 } 639 } 640 641 static void bnep_channel_start_timer(bnep_channel_t *channel, int timeout) 642 { 643 /* Stop any eventually running timeout timer */ 644 bnep_channel_stop_timer(channel); 645 646 /* Start bnep channel timeout check timer */ 647 run_loop_set_timer(&channel->timer, timeout); 648 channel->timer.process = bnep_channel_timer_handler; 649 btstack_linked_item_set_user((btstack_linked_item_t*) &channel->timer, channel); 650 run_loop_add_timer(&channel->timer); 651 channel->timer_active = 1; 652 } 653 654 /* BNEP statemachine functions */ 655 656 inline static void bnep_channel_state_add(bnep_channel_t *channel, BNEP_CHANNEL_STATE_VAR event){ 657 channel->state_var = (BNEP_CHANNEL_STATE_VAR) (channel->state_var | event); 658 } 659 inline static void bnep_channel_state_remove(bnep_channel_t *channel, BNEP_CHANNEL_STATE_VAR event){ 660 channel->state_var = (BNEP_CHANNEL_STATE_VAR) (channel->state_var & ~event); 661 } 662 663 static uint16_t bnep_max_frame_size_for_l2cap_mtu(uint16_t l2cap_mtu){ 664 665 /* Assume a standard BNEP header, containing BNEP Type (1 Byte), dest and 666 source address (6 bytes each) and networking protocol type (2 bytes) 667 */ 668 uint16_t max_frame_size = l2cap_mtu - 15; // 15 bytes BNEP header 669 670 log_info("bnep_max_frame_size_for_l2cap_mtu: %u -> %u", l2cap_mtu, max_frame_size); 671 return max_frame_size; 672 } 673 674 static bnep_channel_t * bnep_channel_create_for_addr(bd_addr_t addr) 675 { 676 /* Allocate new channel structure */ 677 bnep_channel_t *channel = btstack_memory_bnep_channel_get(); 678 if (!channel) { 679 return NULL; 680 } 681 682 /* Initialize the channel struct */ 683 memset(channel, 0, sizeof(bnep_channel_t)); 684 685 channel->state = BNEP_CHANNEL_STATE_CLOSED; 686 channel->max_frame_size = bnep_max_frame_size_for_l2cap_mtu(l2cap_max_mtu()); 687 BD_ADDR_COPY(&channel->remote_addr, addr); 688 hci_local_bd_addr(channel->local_addr); 689 690 channel->net_filter_count = 0; 691 channel->multicast_filter_count = 0; 692 channel->retry_count = 0; 693 694 /* Finally add it to the channel list */ 695 btstack_linked_list_add(&bnep_channels, (btstack_linked_item_t *) channel); 696 697 return channel; 698 } 699 700 static bnep_channel_t* bnep_channel_for_addr(bd_addr_t addr) 701 { 702 btstack_linked_item_t *it; 703 for (it = (btstack_linked_item_t *) bnep_channels; it ; it = it->next){ 704 bnep_channel_t *channel = ((bnep_channel_t *) it); 705 if (BD_ADDR_CMP(addr, channel->remote_addr) == 0) { 706 return channel; 707 } 708 } 709 return NULL; 710 } 711 712 static bnep_channel_t * bnep_channel_for_l2cap_cid(uint16_t l2cap_cid) 713 { 714 btstack_linked_item_t *it; 715 for (it = (btstack_linked_item_t *) bnep_channels; it ; it = it->next){ 716 bnep_channel_t *channel = ((bnep_channel_t *) it); 717 if (channel->l2cap_cid == l2cap_cid) { 718 return channel; 719 } 720 } 721 return NULL; 722 } 723 724 static bnep_service_t * bnep_service_for_uuid(uint16_t uuid) 725 { 726 btstack_linked_item_t *it; 727 for (it = (btstack_linked_item_t *) bnep_services; it ; it = it->next){ 728 bnep_service_t * service = ((bnep_service_t *) it); 729 if ( service->service_uuid == uuid){ 730 return service; 731 } 732 } 733 return NULL; 734 } 735 736 static void bnep_channel_free(bnep_channel_t *channel) 737 { 738 btstack_linked_list_remove( &bnep_channels, (btstack_linked_item_t *) channel); 739 btstack_memory_bnep_channel_free(channel); 740 } 741 742 static void bnep_channel_finalize(bnep_channel_t *channel) 743 { 744 uint16_t l2cap_cid; 745 746 /* Inform application about closed channel */ 747 if (channel->state == BNEP_CHANNEL_STATE_CONNECTED) { 748 bnep_emit_channel_closed(channel); 749 } 750 751 l2cap_cid = channel->l2cap_cid; 752 753 /* Stop any eventually running timer */ 754 bnep_channel_stop_timer(channel); 755 756 /* Free ressources and then close the l2cap channel */ 757 bnep_channel_free(channel); 758 l2cap_disconnect_internal(l2cap_cid, 0x13); 759 } 760 761 static int bnep_handle_connection_request(bnep_channel_t *channel, uint8_t *packet, uint16_t size) 762 { 763 uint16_t uuid_size; 764 uint16_t uuid_offset; 765 uuid_size = packet[1]; 766 uint16_t response_code = BNEP_RESP_SETUP_SUCCESS; 767 bnep_service_t * service; 768 769 /* Sanity check packet size */ 770 if (size < 1 + 1 + 2 * uuid_size) { 771 return 0; 772 } 773 774 if ((channel->state != BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST) && 775 (channel->state != BNEP_CHANNEL_STATE_CONNECTED)) { 776 /* Ignore a connection request if not waiting for or still connected */ 777 log_error("BNEP_CONNECTION_REQUEST: ignored in state %d, l2cap_cid: %d!", channel->state, channel->l2cap_cid); 778 return 0; 779 } 780 781 /* Extract source and destination UUID and convert them to UUID16 format */ 782 switch (uuid_size) { 783 case 2: /* UUID16 */ 784 uuid_offset = 0; 785 break; 786 case 4: /* UUID32 */ 787 case 16: /* UUID128 */ 788 uuid_offset = 2; 789 break; 790 default: 791 log_error("BNEP_CONNECTION_REQUEST: Invalid UUID size %d, l2cap_cid: %d!", channel->state, channel->l2cap_cid); 792 response_code = BNEP_RESP_SETUP_INVALID_SERVICE_UUID_SIZE; 793 break; 794 } 795 796 /* Check source and destination UUIDs for valid combinations */ 797 if (response_code == BNEP_RESP_SETUP_SUCCESS) { 798 channel->uuid_dest = READ_NET_16(packet, 2 + uuid_offset); 799 channel->uuid_source = READ_NET_16(packet, 2 + uuid_offset + uuid_size); 800 801 if ((channel->uuid_dest != SDP_PANU) && 802 (channel->uuid_dest != SDP_NAP) && 803 (channel->uuid_dest != SDP_GN)) { 804 log_error("BNEP_CONNECTION_REQUEST: Invalid destination service UUID: %04x", channel->uuid_dest); 805 channel->uuid_dest = 0; 806 } 807 if ((channel->uuid_source != SDP_PANU) && 808 (channel->uuid_source != SDP_NAP) && 809 (channel->uuid_source != SDP_GN)) { 810 log_error("BNEP_CONNECTION_REQUEST: Invalid source service UUID: %04x", channel->uuid_source); 811 channel->uuid_source = 0; 812 } 813 814 /* Check if we have registered a service for the requested destination UUID */ 815 service = bnep_service_for_uuid(channel->uuid_dest); 816 if (service == NULL) { 817 response_code = BNEP_RESP_SETUP_INVALID_DEST_UUID; 818 } else 819 if ((channel->uuid_source != SDP_PANU) && (channel->uuid_dest != SDP_PANU)) { 820 response_code = BNEP_RESP_SETUP_INVALID_SOURCE_UUID; 821 } 822 } 823 824 /* Set flag to send out the connection response on next statemachine cycle */ 825 bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_RESPONSE); 826 channel->response_code = response_code; 827 828 /* Return the number of processed package bytes = BNEP Type, BNEP Control Type, UUID-Size + 2 * UUID */ 829 return 1 + 1 + 2 * uuid_size; 830 } 831 832 static int bnep_handle_connection_response(bnep_channel_t *channel, uint8_t *packet, uint16_t size) 833 { 834 uint16_t response_code; 835 836 /* Sanity check packet size */ 837 if (size < 1 + 2) { 838 return 0; 839 } 840 841 if (channel->state != BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_RESPONSE) { 842 /* Ignore a connection response in any state but WAIT_FOR_CONNECTION_RESPONSE */ 843 log_error("BNEP_CONNECTION_RESPONSE: Ignored in channel state %d", channel->state); 844 return 1 + 2; 845 } 846 847 response_code = READ_NET_16(packet, 1); 848 849 if (response_code == BNEP_RESP_SETUP_SUCCESS) { 850 log_info("BNEP_CONNECTION_RESPONSE: Channel established to %s", bd_addr_to_str(channel->remote_addr)); 851 channel->state = BNEP_CHANNEL_STATE_CONNECTED; 852 /* Stop timeout timer! */ 853 bnep_channel_stop_timer(channel); 854 bnep_emit_open_channel_complete(channel, 0); 855 } else { 856 log_error("BNEP_CONNECTION_RESPONSE: Connection to %s failed. Err: %d", bd_addr_to_str(channel->remote_addr), response_code); 857 bnep_channel_finalize(channel); 858 } 859 return 1 + 2; 860 } 861 862 static int bnep_can_handle_extensions(bnep_channel_t * channel){ 863 /* Extension are primarily handled in CONNECTED state */ 864 if (channel->state == BNEP_CHANNEL_STATE_CONNECTED) return 1; 865 /* and if we've received connection request, but haven't sent the reponse yet. */ 866 if ((channel->state == BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST) && 867 (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_RESPONSE)) { 868 return 1; 869 } 870 return 0; 871 } 872 873 static int bnep_handle_filter_net_type_set(bnep_channel_t *channel, uint8_t *packet, uint16_t size) 874 { 875 uint16_t list_length; 876 uint16_t response_code = BNEP_RESP_FILTER_SUCCESS; 877 878 /* Sanity check packet size */ 879 if (size < 3) { 880 return 0; 881 } 882 883 list_length = READ_NET_16(packet, 1); 884 /* Sanity check packet size again with known package size */ 885 if (size < 3 + list_length) { 886 return 0; 887 } 888 889 if (!bnep_can_handle_extensions(channel)){ 890 log_error("BNEP_FILTER_NET_TYPE_SET: Ignored in channel state %d", channel->state); 891 return 3 + list_length; 892 } 893 894 /* Check if we have enough space for more filters */ 895 if ((list_length / (2*2)) > MAX_BNEP_NETFILTER) { 896 log_info("BNEP_FILTER_NET_TYPE_SET: Too many filter"); 897 response_code = BNEP_RESP_FILTER_ERR_TOO_MANY_FILTERS; 898 } else { 899 int i; 900 channel->net_filter_count = 0; 901 /* There is still enough space, copy the filters to our filter list */ 902 /* There is still enough space, copy the filters to our filter list */ 903 for (i = 0; i < list_length / (2 * 2); i ++) { 904 channel->net_filter[channel->net_filter_count].range_start = READ_NET_16(packet, 1 + 2 + i * 4); 905 channel->net_filter[channel->net_filter_count].range_end = READ_NET_16(packet, 1 + 2 + i * 4 + 2); 906 if (channel->net_filter[channel->net_filter_count].range_start > channel->net_filter[channel->net_filter_count].range_end) { 907 /* Invalid filter range, ignore this filter rule */ 908 log_error("BNEP_FILTER_NET_TYPE_SET: Invalid filter: start: %d, end: %d", 909 channel->net_filter[channel->net_filter_count].range_start, 910 channel->net_filter[channel->net_filter_count].range_end); 911 response_code = BNEP_RESP_FILTER_ERR_INVALID_RANGE; 912 } else { 913 /* Valid filter, increase the filter count */ 914 log_info("BNEP_FILTER_NET_TYPE_SET: Add filter: start: %d, end: %d", 915 channel->net_filter[channel->net_filter_count].range_start, 916 channel->net_filter[channel->net_filter_count].range_end); 917 channel->net_filter_count ++; 918 } 919 } 920 } 921 922 /* Set flag to send out the set net filter response on next statemachine cycle */ 923 bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_RESPONSE); 924 channel->response_code = response_code; 925 926 return 3 + list_length; 927 } 928 929 static int bnep_handle_filter_net_type_response(bnep_channel_t *channel, uint8_t *packet, uint16_t size) 930 { 931 uint16_t response_code; 932 933 // TODO: Currently we do not support setting a network filter. 934 935 /* Sanity check packet size */ 936 if (size < 1 + 2) { 937 return 0; 938 } 939 940 if (!bnep_can_handle_extensions(channel)){ 941 log_error("BNEP_FILTER_NET_TYPE_RESPONSE: Ignored in channel state %d", channel->state); 942 return 1 + 2; 943 } 944 945 response_code = READ_NET_16(packet, 1); 946 947 if (response_code == BNEP_RESP_FILTER_SUCCESS) { 948 log_info("BNEP_FILTER_NET_TYPE_RESPONSE: Net filter set successfully for %s", bd_addr_to_str(channel->remote_addr)); 949 } else { 950 log_error("BNEP_FILTER_NET_TYPE_RESPONSE: Net filter setting for %s failed. Err: %d", bd_addr_to_str(channel->remote_addr), response_code); 951 } 952 953 return 1 + 2; 954 } 955 956 static int bnep_handle_multi_addr_set(bnep_channel_t *channel, uint8_t *packet, uint16_t size) 957 { 958 uint16_t list_length; 959 uint16_t response_code = BNEP_RESP_FILTER_SUCCESS; 960 961 /* Sanity check packet size */ 962 if (size < 3) { 963 return 0; 964 } 965 966 list_length = READ_NET_16(packet, 1); 967 /* Sanity check packet size again with known package size */ 968 if (size < 3 + list_length) { 969 return 0; 970 } 971 972 if (!bnep_can_handle_extensions(channel)){ 973 log_error("BNEP_MULTI_ADDR_SET: Ignored in channel state %d", channel->state); 974 return 3 + list_length; 975 } 976 977 /* Check if we have enough space for more filters */ 978 if ((list_length / (2 * ETHER_ADDR_LEN)) > MAX_BNEP_MULTICAST_FILTER) { 979 log_info("BNEP_MULTI_ADDR_SET: Too many filter"); 980 response_code = BNEP_RESP_FILTER_ERR_TOO_MANY_FILTERS; 981 } else { 982 unsigned int i; 983 channel->multicast_filter_count = 0; 984 /* There is enough space, copy the filters to our filter list */ 985 for (i = 0; i < list_length / (2 * ETHER_ADDR_LEN); i ++) { 986 BD_ADDR_COPY(channel->multicast_filter[channel->multicast_filter_count].addr_start, packet + 1 + 2 + i * ETHER_ADDR_LEN * 2); 987 BD_ADDR_COPY(channel->multicast_filter[channel->multicast_filter_count].addr_end, packet + 1 + 2 + i * ETHER_ADDR_LEN * 2 + ETHER_ADDR_LEN); 988 989 if (memcmp(channel->multicast_filter[channel->multicast_filter_count].addr_start, 990 channel->multicast_filter[channel->multicast_filter_count].addr_end, ETHER_ADDR_LEN) > 0) { 991 /* Invalid filter range, ignore this filter rule */ 992 log_error("BNEP_MULTI_ADDR_SET: Invalid filter: start: %s", 993 bd_addr_to_str(channel->multicast_filter[channel->multicast_filter_count].addr_start)); 994 log_error("BNEP_MULTI_ADDR_SET: Invalid filter: end: %s", 995 bd_addr_to_str(channel->multicast_filter[channel->multicast_filter_count].addr_end)); 996 response_code = BNEP_RESP_FILTER_ERR_INVALID_RANGE; 997 } else { 998 /* Valid filter, increase the filter count */ 999 log_info("BNEP_MULTI_ADDR_SET: Add filter: start: %s", 1000 bd_addr_to_str(channel->multicast_filter[channel->multicast_filter_count].addr_start)); 1001 log_info("BNEP_MULTI_ADDR_SET: Add filter: end: %s", 1002 bd_addr_to_str(channel->multicast_filter[channel->multicast_filter_count].addr_end)); 1003 channel->multicast_filter_count ++; 1004 } 1005 } 1006 } 1007 /* Set flag to send out the set multi addr response on next statemachine cycle */ 1008 bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_RESPONSE); 1009 channel->response_code = response_code; 1010 1011 return 3 + list_length; 1012 } 1013 1014 static int bnep_handle_multi_addr_response(bnep_channel_t *channel, uint8_t *packet, uint16_t size) 1015 { 1016 uint16_t response_code; 1017 1018 // TODO: Currently we do not support setting multicast address filter. 1019 1020 /* Sanity check packet size */ 1021 if (size < 1 + 2) { 1022 return 0; 1023 } 1024 1025 if (!bnep_can_handle_extensions(channel)){ 1026 log_error("BNEP_MULTI_ADDR_RESPONSE: Ignored in channel state %d", channel->state); 1027 return 1 + 2; 1028 } 1029 1030 response_code = READ_NET_16(packet, 1); 1031 1032 if (response_code == BNEP_RESP_FILTER_SUCCESS) { 1033 log_info("BNEP_MULTI_ADDR_RESPONSE: Multicast address filter set successfully for %s", bd_addr_to_str(channel->remote_addr)); 1034 } else { 1035 log_error("BNEP_MULTI_ADDR_RESPONSE: Multicast address filter setting for %s failed. Err: %d", bd_addr_to_str(channel->remote_addr), response_code); 1036 } 1037 1038 return 1 + 2; 1039 } 1040 1041 static int bnep_handle_ethernet_packet(bnep_channel_t *channel, bd_addr_t addr_dest, bd_addr_t addr_source, uint16_t network_protocol_type, uint8_t *payload, uint16_t size) 1042 { 1043 uint16_t pos = 0; 1044 1045 #if (HCI_INCOMING_PRE_BUFFER_SIZE) && (HCI_INCOMING_PRE_BUFFER_SIZE >= 14 - 8) // 2 * sizeof(bd_addr_t) + sizeof(uint16_t) - L2CAP Header (4) - ACL Header (4) 1046 /* In-place modify the package and add the ethernet header in front of the payload. 1047 * WARNING: This modifies the data in front of the payload and may overwrite 14 bytes there! 1048 */ 1049 uint8_t *ethernet_packet = payload - 2 * sizeof(bd_addr_t) - sizeof(uint16_t); 1050 /* Restore the ethernet packet header */ 1051 BD_ADDR_COPY(ethernet_packet + pos, addr_dest); 1052 pos += sizeof(bd_addr_t); 1053 BD_ADDR_COPY(ethernet_packet + pos, addr_source); 1054 pos += sizeof(bd_addr_t); 1055 net_store_16(ethernet_packet, pos, network_protocol_type); 1056 /* Payload is just in place... */ 1057 #else 1058 /* Copy ethernet frame to statically allocated buffer. This solution is more 1059 * save, but needs an extra copy and more stack! 1060 */ 1061 uint8_t ethernet_packet[BNEP_MTU_MIN]; 1062 1063 /* Restore the ethernet packet header */ 1064 BD_ADDR_COPY(ethernet_packet + pos, addr_dest); 1065 pos += sizeof(bd_addr_t); 1066 BD_ADDR_COPY(ethernet_packet + pos, addr_source); 1067 pos += sizeof(bd_addr_t); 1068 net_store_16(ethernet_packet, pos, network_protocol_type); 1069 pos += 2; 1070 memcpy(ethernet_packet + pos, payload, size); 1071 #endif 1072 1073 /* Notify application layer and deliver the ethernet packet */ 1074 (*app_packet_handler)(BNEP_DATA_PACKET, channel->uuid_source, 1075 ethernet_packet, size + sizeof(uint16_t) + 2 * sizeof(bd_addr_t)); 1076 1077 return size; 1078 } 1079 1080 static int bnep_handle_control_packet(bnep_channel_t *channel, uint8_t *packet, uint16_t size, int is_extension) 1081 { 1082 uint16_t len = 0; 1083 uint8_t bnep_control_type; 1084 1085 bnep_control_type = packet[0]; 1086 /* Save last control type. Needed by statemachin in case of unknown control code */ 1087 1088 channel->last_control_type = bnep_control_type; 1089 log_info("BNEP_CONTROL: Type: %d, size: %d, is_extension: %d", bnep_control_type, size, is_extension); 1090 switch (bnep_control_type) { 1091 case BNEP_CONTROL_TYPE_COMMAND_NOT_UNDERSTOOD: 1092 /* The last command we send was not understood. We should close the connection */ 1093 log_error("BNEP_CONTROL: Received COMMAND_NOT_UNDERSTOOD: l2cap_cid: %d, cmd: %d", channel->l2cap_cid, packet[3]); 1094 bnep_channel_finalize(channel); 1095 len = 2; // Length of command not understood packet - bnep-type field 1096 break; 1097 case BNEP_CONTROL_TYPE_SETUP_CONNECTION_REQUEST: 1098 if (is_extension) { 1099 /* Connection requests are not allowed to be send in an extension header 1100 * ignore, do not set "COMMAND_NOT_UNDERSTOOD" 1101 */ 1102 log_error("BNEP_CONTROL: Received SETUP_CONNECTION_REQUEST in extension header: l2cap_cid: %d", channel->l2cap_cid); 1103 return 0; 1104 } else { 1105 len = bnep_handle_connection_request(channel, packet, size); 1106 } 1107 break; 1108 case BNEP_CONTROL_TYPE_SETUP_CONNECTION_RESPONSE: 1109 if (is_extension) { 1110 /* Connection requests are not allowed to be send in an 1111 * extension header, ignore, do not set "COMMAND_NOT_UNDERSTOOD" 1112 */ 1113 log_error("BNEP_CONTROL: Received SETUP_CONNECTION_RESPONSE in extension header: l2cap_cid: %d", channel->l2cap_cid); 1114 return 0; 1115 } else { 1116 len = bnep_handle_connection_response(channel, packet, size); 1117 } 1118 break; 1119 case BNEP_CONTROL_TYPE_FILTER_NET_TYPE_SET: 1120 len = bnep_handle_filter_net_type_set(channel, packet, size); 1121 break; 1122 case BNEP_CONTROL_TYPE_FILTER_NET_TYPE_RESPONSE: 1123 len = bnep_handle_filter_net_type_response(channel, packet, size); 1124 break; 1125 case BNEP_CONTROL_TYPE_FILTER_MULTI_ADDR_SET: 1126 len = bnep_handle_multi_addr_set(channel, packet, size); 1127 break; 1128 case BNEP_CONTROL_TYPE_FILTER_MULTI_ADDR_RESPONSE: 1129 len = bnep_handle_multi_addr_response(channel, packet, size); 1130 break; 1131 default: 1132 log_error("BNEP_CONTROL: Invalid bnep control type: l2cap_cid: %d, cmd: %d", channel->l2cap_cid, bnep_control_type); 1133 len = 0; 1134 break; 1135 } 1136 1137 if (len == 0) { 1138 /* In case the command could not be handled, send a 1139 COMMAND_NOT_UNDERSTOOD message. 1140 Set flag to process the request in the next statemachine loop 1141 */ 1142 bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_NOT_UNDERSTOOD); 1143 } 1144 1145 return len; 1146 } 1147 1148 /** 1149 * @return handled packet 1150 */ 1151 static int bnep_hci_event_handler(uint8_t *packet, uint16_t size) 1152 { 1153 bd_addr_t event_addr; 1154 uint16_t psm; 1155 uint16_t l2cap_cid; 1156 hci_con_handle_t con_handle; 1157 bnep_channel_t *channel = NULL; 1158 uint8_t status; 1159 1160 switch (packet[0]) { 1161 1162 /* Accept an incoming L2CAP connection on PSM_BNEP */ 1163 case L2CAP_EVENT_INCOMING_CONNECTION: 1164 /* L2CAP event data: event(8), len(8), address(48), handle (16), psm (16), source cid(16) dest cid(16) */ 1165 bt_flip_addr(event_addr, &packet[2]); 1166 con_handle = READ_BT_16(packet, 8); 1167 psm = READ_BT_16(packet, 10); 1168 l2cap_cid = READ_BT_16(packet, 12); 1169 1170 if (psm != PSM_BNEP) break; 1171 1172 channel = bnep_channel_for_addr(event_addr); 1173 1174 if (channel) { 1175 log_error("INCOMING_CONNECTION (l2cap_cid 0x%02x) for PSM_BNEP => decline - channel already exists", l2cap_cid); 1176 l2cap_decline_connection_internal(l2cap_cid, 0x04); // no resources available 1177 return 1; 1178 } 1179 1180 /* Create a new BNEP channel instance (incoming) */ 1181 channel = bnep_channel_create_for_addr(event_addr); 1182 1183 if (!channel) { 1184 log_error("INCOMING_CONNECTION (l2cap_cid 0x%02x) for PSM_BNEP => decline - no memory left", l2cap_cid); 1185 l2cap_decline_connection_internal(l2cap_cid, 0x04); // no resources available 1186 return 1; 1187 } 1188 1189 /* Assign connection handle and l2cap cid */ 1190 channel->con_handle = con_handle; 1191 channel->l2cap_cid = l2cap_cid; 1192 1193 /* Set channel into accept state */ 1194 channel->state = BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST; 1195 1196 /* Start connection timeout timer */ 1197 bnep_channel_start_timer(channel, BNEP_CONNECTION_TIMEOUT_MS); 1198 1199 log_info("L2CAP_EVENT_INCOMING_CONNECTION (l2cap_cid 0x%02x) for PSM_BNEP => accept", l2cap_cid); 1200 l2cap_accept_connection_internal(l2cap_cid); 1201 return 1; 1202 1203 /* Outgoing L2CAP connection has been opened -> store l2cap_cid, remote_addr */ 1204 case L2CAP_EVENT_CHANNEL_OPENED: 1205 /* Check if the l2cap channel has been opened for PSM_BNEP */ 1206 if (READ_BT_16(packet, 11) != PSM_BNEP) { 1207 break; 1208 } 1209 1210 status = packet[2]; 1211 log_info("L2CAP_EVENT_CHANNEL_OPENED for PSM_BNEP, status %u", status); 1212 1213 /* Get the bnep channel fpr remote address */ 1214 con_handle = READ_BT_16(packet, 9); 1215 l2cap_cid = READ_BT_16(packet, 13); 1216 bt_flip_addr(event_addr, &packet[3]); 1217 channel = bnep_channel_for_addr(event_addr); 1218 if (!channel) { 1219 log_error("L2CAP_EVENT_CHANNEL_OPENED but no BNEP channel prepared"); 1220 return 1; 1221 } 1222 1223 /* On L2CAP open error discard everything */ 1224 if (status) { 1225 /* Emit bnep_open_channel_complete with status and free channel */ 1226 bnep_emit_open_channel_complete(channel, status); 1227 1228 /* Free BNEP channel mempory */ 1229 bnep_channel_free(channel); 1230 return 1; 1231 } 1232 1233 switch (channel->state){ 1234 case BNEP_CHANNEL_STATE_CLOSED: 1235 log_info("L2CAP_EVENT_CHANNEL_OPENED: outgoing connection"); 1236 1237 bnep_channel_start_timer(channel, BNEP_CONNECTION_TIMEOUT_MS); 1238 1239 /* Assign connection handle and l2cap cid */ 1240 channel->l2cap_cid = l2cap_cid; 1241 channel->con_handle = con_handle; 1242 1243 /* Initiate the connection request */ 1244 channel->state = BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_RESPONSE; 1245 bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_REQUEST); 1246 channel->max_frame_size = bnep_max_frame_size_for_l2cap_mtu(READ_BT_16(packet, 17)); 1247 bnep_run(); 1248 break; 1249 case BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST: 1250 /* New information: channel mtu */ 1251 channel->max_frame_size = bnep_max_frame_size_for_l2cap_mtu(READ_BT_16(packet, 17)); 1252 break; 1253 default: 1254 log_error("L2CAP_EVENT_CHANNEL_OPENED: Invalid state: %d", channel->state); 1255 break; 1256 } 1257 return 1; 1258 1259 case DAEMON_EVENT_HCI_PACKET_SENT: 1260 bnep_run(); 1261 break; 1262 1263 case L2CAP_EVENT_CHANNEL_CLOSED: 1264 // data: event (8), len(8), channel (16) 1265 l2cap_cid = READ_BT_16(packet, 2); 1266 channel = bnep_channel_for_l2cap_cid(l2cap_cid); 1267 log_info("L2CAP_EVENT_CHANNEL_CLOSED cid 0x%0x, channel %p", l2cap_cid, channel); 1268 1269 if (!channel) { 1270 break; 1271 } 1272 1273 log_info("L2CAP_EVENT_CHANNEL_CLOSED state %u", channel->state); 1274 switch (channel->state) { 1275 case BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST: 1276 case BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_RESPONSE: 1277 case BNEP_CHANNEL_STATE_CONNECTED: 1278 bnep_channel_finalize(channel); 1279 return 1; 1280 default: 1281 break; 1282 } 1283 break; 1284 default: 1285 bnep_run(); 1286 break; 1287 } 1288 return 0; 1289 } 1290 1291 static int bnep_l2cap_packet_handler(uint16_t l2cap_cid, uint8_t *packet, uint16_t size) 1292 { 1293 int rc = 0; 1294 uint8_t bnep_type; 1295 uint8_t bnep_header_has_ext; 1296 uint8_t extension_type; 1297 uint16_t pos = 0; 1298 bd_addr_t addr_source; 1299 bd_addr_t addr_dest; 1300 uint16_t network_protocol_type = 0xffff; 1301 bnep_channel_t *channel = NULL; 1302 1303 /* Get the bnep channel for this package */ 1304 channel = bnep_channel_for_l2cap_cid(l2cap_cid); 1305 if (!channel) { 1306 return rc; 1307 } 1308 1309 /* Sort out short packages */ 1310 if (size < 2) { 1311 return rc; 1312 } 1313 1314 bnep_type = BNEP_TYPE(packet[pos]); 1315 bnep_header_has_ext = BNEP_HEADER_HAS_EXT(packet[pos]); 1316 pos ++; 1317 1318 switch(bnep_type) { 1319 case BNEP_PKT_TYPE_GENERAL_ETHERNET: 1320 BD_ADDR_COPY(addr_dest, &packet[pos]); 1321 pos += sizeof(bd_addr_t); 1322 BD_ADDR_COPY(addr_source, &packet[pos]); 1323 pos += sizeof(bd_addr_t); 1324 network_protocol_type = READ_NET_16(packet, pos); 1325 pos += 2; 1326 break; 1327 case BNEP_PKT_TYPE_COMPRESSED_ETHERNET: 1328 BD_ADDR_COPY(addr_dest, channel->local_addr); 1329 BD_ADDR_COPY(addr_source, channel->remote_addr); 1330 network_protocol_type = READ_NET_16(packet, pos); 1331 pos += 2; 1332 break; 1333 case BNEP_PKT_TYPE_COMPRESSED_ETHERNET_SOURCE_ONLY: 1334 BD_ADDR_COPY(addr_dest, channel->local_addr); 1335 BD_ADDR_COPY(addr_source, &packet[pos]); 1336 pos += sizeof(bd_addr_t); 1337 network_protocol_type = READ_NET_16(packet, pos); 1338 pos += 2; 1339 break; 1340 case BNEP_PKT_TYPE_COMPRESSED_ETHERNET_DEST_ONLY: 1341 BD_ADDR_COPY(addr_dest, &packet[pos]); 1342 pos += sizeof(bd_addr_t); 1343 BD_ADDR_COPY(addr_source, channel->remote_addr); 1344 network_protocol_type = READ_NET_16(packet, pos); 1345 pos += 2; 1346 break; 1347 case BNEP_PKT_TYPE_CONTROL: 1348 rc = bnep_handle_control_packet(channel, packet + pos, size - pos, 0); 1349 pos += rc; 1350 break; 1351 default: 1352 break; 1353 } 1354 1355 if (bnep_header_has_ext) { 1356 do { 1357 uint8_t ext_len; 1358 1359 /* Read extension type and check for further extensions */ 1360 extension_type = BNEP_TYPE(packet[pos]); 1361 bnep_header_has_ext = BNEP_HEADER_HAS_EXT(packet[pos]); 1362 pos ++; 1363 1364 /* Read extension header length */ 1365 ext_len = packet[pos]; 1366 pos ++; 1367 1368 if (size - pos < ext_len) { 1369 log_error("BNEP pkt handler: Invalid extension length! Packet ignored"); 1370 /* Invalid packet size! */ 1371 return 0; 1372 } 1373 1374 switch (extension_type) { 1375 case BNEP_EXT_HEADER_TYPE_EXTENSION_CONTROL: 1376 if (ext_len != bnep_handle_control_packet(channel, packet + pos, ext_len, 1)) { 1377 log_error("BNEP pkt handler: Ignore invalid control packet in extension header"); 1378 } 1379 1380 pos += ext_len; 1381 break; 1382 1383 default: 1384 /* Extension header type unknown. Unknown extension SHALL be 1385 * SHALL be forwarded in any way. But who shall handle these 1386 * extension packets? 1387 * For now: We ignore them and just drop them! 1388 */ 1389 log_error("BNEP pkt handler: Unknown extension type ignored, data dropped!"); 1390 pos += ext_len; 1391 break; 1392 } 1393 1394 } while (bnep_header_has_ext); 1395 } 1396 1397 if (bnep_type != BNEP_PKT_TYPE_CONTROL && network_protocol_type != 0xffff) { 1398 if (channel->state == BNEP_CHANNEL_STATE_CONNECTED) { 1399 rc = bnep_handle_ethernet_packet(channel, addr_dest, addr_source, network_protocol_type, packet + pos, size - pos); 1400 } else { 1401 rc = 0; 1402 } 1403 } 1404 1405 return rc; 1406 1407 } 1408 1409 void bnep_packet_handler(uint8_t packet_type, uint16_t l2cap_cid, uint8_t *packet, uint16_t size) 1410 { 1411 int handled = 0; 1412 switch (packet_type) { 1413 case HCI_EVENT_PACKET: 1414 handled = bnep_hci_event_handler(packet, size); 1415 break; 1416 case L2CAP_DATA_PACKET: 1417 handled = bnep_l2cap_packet_handler(l2cap_cid, packet, size); 1418 break; 1419 default: 1420 break; 1421 } 1422 1423 if (handled) { 1424 bnep_run(); 1425 return; 1426 } 1427 1428 /* Forward non l2cap packages to application handler */ 1429 if (packet_type != L2CAP_DATA_PACKET) { 1430 (*app_packet_handler)(packet_type, l2cap_cid, packet, size); 1431 return; 1432 } 1433 1434 bnep_run(); 1435 } 1436 1437 static void bnep_channel_state_machine(bnep_channel_t* channel, bnep_channel_event_t *event) 1438 { 1439 log_info("bnep_state_machine: state %u, state var: %02x, event %u", channel->state, channel->state_var, event->type); 1440 1441 if (event->type == BNEP_CH_EVT_READY_TO_SEND) { 1442 /* Send outstanding packets. */ 1443 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_NOT_UNDERSTOOD) { 1444 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_NOT_UNDERSTOOD); 1445 bnep_send_command_not_understood(channel, channel->last_control_type); 1446 return; 1447 } 1448 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_REQUEST) { 1449 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_REQUEST); 1450 channel->state = BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_RESPONSE; 1451 bnep_send_connection_request(channel, channel->uuid_source, channel->uuid_dest); 1452 } 1453 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_RESPONSE) { 1454 int emit_connected = 0; 1455 if ((channel->state == BNEP_CHANNEL_STATE_CLOSED) || 1456 (channel->state == BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST)) { 1457 /* Set channel state to STATE_CONNECTED */ 1458 channel->state = BNEP_CHANNEL_STATE_CONNECTED; 1459 /* Stop timeout timer! */ 1460 bnep_channel_stop_timer(channel); 1461 emit_connected = 1; 1462 } 1463 1464 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_RESPONSE); 1465 bnep_send_connection_response(channel, channel->response_code); 1466 if (emit_connected){ 1467 bnep_emit_open_channel_complete(channel, 0); 1468 } 1469 return; 1470 } 1471 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_SET) { 1472 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_SET); 1473 if ((channel->net_filter_out_count > 0) && (channel->net_filter_out != NULL)) { 1474 bnep_send_filter_net_type_set(channel, channel->net_filter_out, channel->net_filter_out_count); 1475 channel->net_filter_out_count = 0; 1476 channel->net_filter_out = NULL; 1477 } 1478 return; 1479 } 1480 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_RESPONSE) { 1481 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_RESPONSE); 1482 bnep_send_filter_net_type_response(channel, channel->response_code); 1483 return; 1484 } 1485 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_SET) { 1486 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_SET); 1487 if ((channel->multicast_filter_out_count > 0) && (channel->multicast_filter_out != NULL)) { 1488 bnep_send_filter_multi_addr_set(channel, channel->multicast_filter_out, channel->multicast_filter_out_count); 1489 channel->multicast_filter_out_count = 0; 1490 channel->multicast_filter_out = NULL; 1491 } 1492 return; 1493 } 1494 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_RESPONSE) { 1495 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_RESPONSE); 1496 bnep_send_filter_multi_addr_response(channel, channel->response_code); 1497 return; 1498 } 1499 1500 1501 /* If the event was not yet handled, notify the application layer */ 1502 bnep_emit_ready_to_send(channel); 1503 } 1504 } 1505 1506 1507 /* Process oustanding signaling tasks */ 1508 static void bnep_run(void) 1509 { 1510 btstack_linked_item_t *it; 1511 btstack_linked_item_t *next; 1512 1513 for (it = (btstack_linked_item_t *) bnep_channels; it ; it = next){ 1514 1515 next = it->next; // be prepared for removal of channel in state machine 1516 1517 bnep_channel_t * channel = ((bnep_channel_t *) it); 1518 1519 if (!l2cap_can_send_packet_now(channel->l2cap_cid)) { 1520 continue; 1521 } 1522 1523 bnep_channel_event_t channel_event = { BNEP_CH_EVT_READY_TO_SEND }; 1524 bnep_channel_state_machine(channel, &channel_event); 1525 } 1526 } 1527 1528 /* BNEP BTStack API */ 1529 void bnep_init(void) 1530 { 1531 bnep_security_level = LEVEL_0; 1532 } 1533 1534 void bnep_set_required_security_level(gap_security_level_t security_level) 1535 { 1536 bnep_security_level = security_level; 1537 } 1538 1539 /* Register application packet handler */ 1540 void bnep_register_packet_handler(void (*handler)(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size)){ 1541 app_packet_handler = handler; 1542 } 1543 1544 int bnep_connect(bd_addr_t addr, uint16_t l2cap_psm, uint16_t uuid_src, uint16_t uuid_dest) 1545 { 1546 bnep_channel_t *channel; 1547 log_info("BNEP_CONNECT addr %s", bd_addr_to_str(addr)); 1548 1549 channel = bnep_channel_create_for_addr(addr); 1550 if (channel == NULL) { 1551 return -1; 1552 } 1553 1554 channel->uuid_source = uuid_src; 1555 channel->uuid_dest = uuid_dest; 1556 1557 uint8_t status = l2cap_create_channel(bnep_packet_handler, addr, l2cap_psm, l2cap_max_mtu(), NULL); 1558 if (status){ 1559 return -1; 1560 } 1561 return 0; 1562 } 1563 1564 void bnep_disconnect(bd_addr_t addr) 1565 { 1566 bnep_channel_t *channel; 1567 log_info("BNEP_DISCONNECT"); 1568 1569 channel = bnep_channel_for_addr(addr); 1570 1571 bnep_channel_finalize(channel); 1572 1573 bnep_run(); 1574 } 1575 1576 1577 uint8_t bnep_register_service(uint16_t service_uuid, uint16_t max_frame_size) 1578 { 1579 log_info("BNEP_REGISTER_SERVICE mtu %d", max_frame_size); 1580 1581 /* Check if we already registered a service */ 1582 bnep_service_t * service = bnep_service_for_uuid(service_uuid); 1583 if (service) { 1584 return BNEP_SERVICE_ALREADY_REGISTERED; 1585 } 1586 1587 /* Only alow one the three service types: PANU, NAP, GN */ 1588 if ((service_uuid != SDP_PANU) && 1589 (service_uuid != SDP_NAP) && 1590 (service_uuid != SDP_GN)) { 1591 log_info("BNEP_REGISTER_SERVICE: Invalid service UUID: %04x", service_uuid); 1592 return BNEP_SERVICE_ALREADY_REGISTERED; // TODO: define own error 1593 } 1594 1595 /* Allocate service memory */ 1596 service = (bnep_service_t*) btstack_memory_bnep_service_get(); 1597 if (!service) { 1598 return BTSTACK_MEMORY_ALLOC_FAILED; 1599 } 1600 memset(service, 0, sizeof(bnep_service_t)); 1601 1602 /* register with l2cap if not registered before, max MTU */ 1603 l2cap_register_service(bnep_packet_handler, PSM_BNEP, 0xffff, bnep_security_level); 1604 1605 /* Setup the service struct */ 1606 service->max_frame_size = max_frame_size; 1607 service->service_uuid = service_uuid; 1608 1609 /* Add to services list */ 1610 btstack_linked_list_add(&bnep_services, (btstack_linked_item_t *) service); 1611 1612 return 0; 1613 } 1614 1615 void bnep_unregister_service(uint16_t service_uuid) 1616 { 1617 log_info("BNEP_UNREGISTER_SERVICE #%04x", service_uuid); 1618 1619 bnep_service_t *service = bnep_service_for_uuid(service_uuid); 1620 if (!service) { 1621 return; 1622 } 1623 1624 btstack_linked_list_remove(&bnep_services, (btstack_linked_item_t *) service); 1625 btstack_memory_bnep_service_free(service); 1626 service = NULL; 1627 1628 l2cap_unregister_service(PSM_BNEP); 1629 } 1630 1631