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