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