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