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 "bluetooth_sdp.h" 53 #include "btstack_debug.h" 54 #include "btstack_event.h" 55 #include "btstack_memory.h" 56 #include "btstack_util.h" 57 #include "classic/core.h" 58 #include "classic/sdp_util.h" 59 #include "hci.h" 60 #include "hci_cmd.h" 61 #include "hci_dump.h" 62 #include "l2cap.h" 63 64 #define BNEP_CONNECTION_TIMEOUT_MS 10000 65 #define BNEP_CONNECTION_MAX_RETRIES 1 66 67 static btstack_linked_list_t bnep_services = NULL; 68 static btstack_linked_list_t bnep_channels = NULL; 69 70 static gap_security_level_t bnep_security_level; 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 != BLUETOOTH_SERVICE_CLASS_PANU) && 826 (channel->uuid_dest != BLUETOOTH_SERVICE_CLASS_NAP) && 827 (channel->uuid_dest != BLUETOOTH_SERVICE_CLASS_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 != BLUETOOTH_SERVICE_CLASS_PANU) && 832 (channel->uuid_source != BLUETOOTH_SERVICE_CLASS_NAP) && 833 (channel->uuid_source != BLUETOOTH_SERVICE_CLASS_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 != BLUETOOTH_SERVICE_CLASS_PANU) && (channel->uuid_dest != BLUETOOTH_SERVICE_CLASS_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 if (channel->packet_handler){ 1094 (*channel->packet_handler)(BNEP_DATA_PACKET, channel->l2cap_cid, ethernet_packet, 1095 size + sizeof(uint16_t) + 2 * sizeof(bd_addr_t)); 1096 } 1097 1098 return size; 1099 } 1100 1101 static int bnep_handle_control_packet(bnep_channel_t *channel, uint8_t *packet, uint16_t size, int is_extension) 1102 { 1103 uint16_t len = 0; 1104 uint8_t bnep_control_type; 1105 1106 bnep_control_type = packet[0]; 1107 /* Save last control type. Needed by statemachin in case of unknown control code */ 1108 1109 channel->last_control_type = bnep_control_type; 1110 log_info("BNEP_CONTROL: Type: %d, size: %d, is_extension: %d", bnep_control_type, size, is_extension); 1111 switch (bnep_control_type) { 1112 case BNEP_CONTROL_TYPE_COMMAND_NOT_UNDERSTOOD: 1113 /* The last command we send was not understood. We should close the connection */ 1114 log_error("BNEP_CONTROL: Received COMMAND_NOT_UNDERSTOOD: l2cap_cid: %d, cmd: %d", channel->l2cap_cid, packet[3]); 1115 bnep_channel_finalize(channel); 1116 len = 2; // Length of command not understood packet - bnep-type field 1117 break; 1118 case BNEP_CONTROL_TYPE_SETUP_CONNECTION_REQUEST: 1119 if (is_extension) { 1120 /* Connection requests are not allowed to be send in an extension header 1121 * ignore, do not set "COMMAND_NOT_UNDERSTOOD" 1122 */ 1123 log_error("BNEP_CONTROL: Received SETUP_CONNECTION_REQUEST in extension header: l2cap_cid: %d", channel->l2cap_cid); 1124 return 0; 1125 } else { 1126 len = bnep_handle_connection_request(channel, packet, size); 1127 } 1128 break; 1129 case BNEP_CONTROL_TYPE_SETUP_CONNECTION_RESPONSE: 1130 if (is_extension) { 1131 /* Connection requests are not allowed to be send in an 1132 * extension header, ignore, do not set "COMMAND_NOT_UNDERSTOOD" 1133 */ 1134 log_error("BNEP_CONTROL: Received SETUP_CONNECTION_RESPONSE in extension header: l2cap_cid: %d", channel->l2cap_cid); 1135 return 0; 1136 } else { 1137 len = bnep_handle_connection_response(channel, packet, size); 1138 } 1139 break; 1140 case BNEP_CONTROL_TYPE_FILTER_NET_TYPE_SET: 1141 len = bnep_handle_filter_net_type_set(channel, packet, size); 1142 break; 1143 case BNEP_CONTROL_TYPE_FILTER_NET_TYPE_RESPONSE: 1144 len = bnep_handle_filter_net_type_response(channel, packet, size); 1145 break; 1146 case BNEP_CONTROL_TYPE_FILTER_MULTI_ADDR_SET: 1147 len = bnep_handle_multi_addr_set(channel, packet, size); 1148 break; 1149 case BNEP_CONTROL_TYPE_FILTER_MULTI_ADDR_RESPONSE: 1150 len = bnep_handle_multi_addr_response(channel, packet, size); 1151 break; 1152 default: 1153 log_error("BNEP_CONTROL: Invalid bnep control type: l2cap_cid: %d, cmd: %d", channel->l2cap_cid, bnep_control_type); 1154 len = 0; 1155 break; 1156 } 1157 1158 if (len == 0) { 1159 /* In case the command could not be handled, send a 1160 COMMAND_NOT_UNDERSTOOD message. 1161 Set flag to process the request in the next statemachine loop 1162 */ 1163 bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_NOT_UNDERSTOOD); 1164 l2cap_request_can_send_now_event(channel->l2cap_cid); 1165 } 1166 1167 return len; 1168 } 1169 1170 /** 1171 * @return handled packet 1172 */ 1173 static int bnep_hci_event_handler(uint8_t *packet, uint16_t size) 1174 { 1175 UNUSED(size); // ok: handling own l2cap events 1176 1177 bd_addr_t event_addr; 1178 uint16_t psm; 1179 uint16_t l2cap_cid; 1180 hci_con_handle_t con_handle; 1181 bnep_channel_t *channel = NULL; 1182 uint8_t status; 1183 1184 switch (hci_event_packet_get_type(packet)) { 1185 1186 /* Accept an incoming L2CAP connection on BLUETOOTH_PROTOCOL_BNEP */ 1187 case L2CAP_EVENT_INCOMING_CONNECTION: 1188 /* L2CAP event data: event(8), len(8), address(48), handle (16), psm (16), source cid(16) dest cid(16) */ 1189 reverse_bd_addr(&packet[2], event_addr); 1190 con_handle = little_endian_read_16(packet, 8); 1191 psm = little_endian_read_16(packet, 10); 1192 l2cap_cid = little_endian_read_16(packet, 12); 1193 1194 if (psm != BLUETOOTH_PROTOCOL_BNEP) break; 1195 1196 channel = bnep_channel_for_addr(event_addr); 1197 1198 if (channel) { 1199 log_error("INCOMING_CONNECTION (l2cap_cid 0x%02x) for BLUETOOTH_PROTOCOL_BNEP => decline - channel already exists", l2cap_cid); 1200 l2cap_decline_connection(l2cap_cid); 1201 return 1; 1202 } 1203 1204 /* Create a new BNEP channel instance (incoming) */ 1205 channel = bnep_channel_create_for_addr(event_addr); 1206 1207 if (!channel) { 1208 log_error("INCOMING_CONNECTION (l2cap_cid 0x%02x) for BLUETOOTH_PROTOCOL_BNEP => decline - no memory left", l2cap_cid); 1209 l2cap_decline_connection(l2cap_cid); 1210 return 1; 1211 } 1212 1213 /* Assign connection handle and l2cap cid */ 1214 channel->con_handle = con_handle; 1215 channel->l2cap_cid = l2cap_cid; 1216 1217 /* Set channel into accept state */ 1218 channel->state = BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST; 1219 1220 /* Start connection timeout timer */ 1221 bnep_channel_start_timer(channel, BNEP_CONNECTION_TIMEOUT_MS); 1222 1223 log_info("L2CAP_EVENT_INCOMING_CONNECTION (l2cap_cid 0x%02x) for BLUETOOTH_PROTOCOL_BNEP => accept", l2cap_cid); 1224 l2cap_accept_connection(l2cap_cid); 1225 return 1; 1226 1227 /* Outgoing L2CAP connection has been opened -> store l2cap_cid, remote_addr */ 1228 case L2CAP_EVENT_CHANNEL_OPENED: 1229 /* Check if the l2cap channel has been opened for BLUETOOTH_PROTOCOL_BNEP */ 1230 if (little_endian_read_16(packet, 11) != BLUETOOTH_PROTOCOL_BNEP) { 1231 break; 1232 } 1233 1234 status = packet[2]; 1235 log_info("L2CAP_EVENT_CHANNEL_OPENED for BLUETOOTH_PROTOCOL_BNEP, status %u", status); 1236 1237 /* Get the bnep channel fpr remote address */ 1238 con_handle = little_endian_read_16(packet, 9); 1239 l2cap_cid = little_endian_read_16(packet, 13); 1240 reverse_bd_addr(&packet[3], event_addr); 1241 channel = bnep_channel_for_addr(event_addr); 1242 if (!channel) { 1243 log_error("L2CAP_EVENT_CHANNEL_OPENED but no BNEP channel prepared"); 1244 return 1; 1245 } 1246 1247 /* On L2CAP open error discard everything */ 1248 if (status) { 1249 /* Emit bnep_open_channel_complete with status and free channel */ 1250 bnep_emit_open_channel_complete(channel, status); 1251 1252 /* Free BNEP channel mempory */ 1253 bnep_channel_free(channel); 1254 return 1; 1255 } 1256 1257 switch (channel->state){ 1258 case BNEP_CHANNEL_STATE_CLOSED: 1259 log_info("L2CAP_EVENT_CHANNEL_OPENED: outgoing connection"); 1260 1261 bnep_channel_start_timer(channel, BNEP_CONNECTION_TIMEOUT_MS); 1262 1263 /* Assign connection handle and l2cap cid */ 1264 channel->l2cap_cid = l2cap_cid; 1265 channel->con_handle = con_handle; 1266 1267 /* Initiate the connection request */ 1268 channel->state = BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_RESPONSE; 1269 bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_REQUEST); 1270 channel->max_frame_size = bnep_max_frame_size_for_l2cap_mtu(little_endian_read_16(packet, 17)); 1271 l2cap_request_can_send_now_event(channel->l2cap_cid); 1272 break; 1273 case BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST: 1274 /* New information: channel mtu */ 1275 channel->max_frame_size = bnep_max_frame_size_for_l2cap_mtu(little_endian_read_16(packet, 17)); 1276 break; 1277 default: 1278 log_error("L2CAP_EVENT_CHANNEL_OPENED: Invalid state: %d", channel->state); 1279 break; 1280 } 1281 return 1; 1282 1283 case L2CAP_EVENT_CAN_SEND_NOW: 1284 bnep_handle_can_send_now(l2cap_event_can_send_now_get_local_cid(packet)); 1285 break; 1286 1287 case L2CAP_EVENT_CHANNEL_CLOSED: 1288 // data: event (8), len(8), channel (16) 1289 l2cap_cid = little_endian_read_16(packet, 2); 1290 channel = bnep_channel_for_l2cap_cid(l2cap_cid); 1291 log_info("L2CAP_EVENT_CHANNEL_CLOSED cid 0x%0x, channel %p", l2cap_cid, channel); 1292 1293 if (!channel) { 1294 break; 1295 } 1296 1297 log_info("L2CAP_EVENT_CHANNEL_CLOSED state %u", channel->state); 1298 switch (channel->state) { 1299 case BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST: 1300 case BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_RESPONSE: 1301 case BNEP_CHANNEL_STATE_CONNECTED: 1302 bnep_channel_finalize(channel); 1303 return 1; 1304 default: 1305 break; 1306 } 1307 break; 1308 default: 1309 break; 1310 } 1311 return 0; 1312 } 1313 1314 static int bnep_l2cap_packet_handler(uint16_t l2cap_cid, uint8_t *packet, uint16_t size) 1315 { 1316 int rc = 0; 1317 uint8_t bnep_type; 1318 uint8_t bnep_header_has_ext; 1319 uint8_t extension_type; 1320 uint16_t pos = 0; 1321 bd_addr_t addr_source; 1322 bd_addr_t addr_dest; 1323 uint16_t network_protocol_type = 0xffff; 1324 bnep_channel_t *channel = NULL; 1325 1326 /* Get the bnep channel for this package */ 1327 channel = bnep_channel_for_l2cap_cid(l2cap_cid); 1328 if (!channel) { 1329 return rc; 1330 } 1331 1332 /* Sort out short packages */ 1333 if (size < 2) { 1334 return rc; 1335 } 1336 1337 bnep_type = BNEP_TYPE(packet[pos]); 1338 bnep_header_has_ext = BNEP_HEADER_HAS_EXT(packet[pos]); 1339 pos ++; 1340 1341 switch(bnep_type) { 1342 case BNEP_PKT_TYPE_GENERAL_ETHERNET: 1343 bd_addr_copy(addr_dest, &packet[pos]); 1344 pos += sizeof(bd_addr_t); 1345 bd_addr_copy(addr_source, &packet[pos]); 1346 pos += sizeof(bd_addr_t); 1347 network_protocol_type = big_endian_read_16(packet, pos); 1348 pos += 2; 1349 break; 1350 case BNEP_PKT_TYPE_COMPRESSED_ETHERNET: 1351 bd_addr_copy(addr_dest, channel->local_addr); 1352 bd_addr_copy(addr_source, channel->remote_addr); 1353 network_protocol_type = big_endian_read_16(packet, pos); 1354 pos += 2; 1355 break; 1356 case BNEP_PKT_TYPE_COMPRESSED_ETHERNET_SOURCE_ONLY: 1357 bd_addr_copy(addr_dest, channel->local_addr); 1358 bd_addr_copy(addr_source, &packet[pos]); 1359 pos += sizeof(bd_addr_t); 1360 network_protocol_type = big_endian_read_16(packet, pos); 1361 pos += 2; 1362 break; 1363 case BNEP_PKT_TYPE_COMPRESSED_ETHERNET_DEST_ONLY: 1364 bd_addr_copy(addr_dest, &packet[pos]); 1365 pos += sizeof(bd_addr_t); 1366 bd_addr_copy(addr_source, channel->remote_addr); 1367 network_protocol_type = big_endian_read_16(packet, pos); 1368 pos += 2; 1369 break; 1370 case BNEP_PKT_TYPE_CONTROL: 1371 rc = bnep_handle_control_packet(channel, packet + pos, size - pos, 0); 1372 pos += rc; 1373 break; 1374 default: 1375 break; 1376 } 1377 1378 if (bnep_header_has_ext) { 1379 do { 1380 uint8_t ext_len; 1381 1382 /* Read extension type and check for further extensions */ 1383 extension_type = BNEP_TYPE(packet[pos]); 1384 bnep_header_has_ext = BNEP_HEADER_HAS_EXT(packet[pos]); 1385 pos ++; 1386 1387 /* Read extension header length */ 1388 ext_len = packet[pos]; 1389 pos ++; 1390 1391 if (size - pos < ext_len) { 1392 log_error("BNEP pkt handler: Invalid extension length! Packet ignored"); 1393 /* Invalid packet size! */ 1394 return 0; 1395 } 1396 1397 switch (extension_type) { 1398 case BNEP_EXT_HEADER_TYPE_EXTENSION_CONTROL: 1399 if (ext_len != bnep_handle_control_packet(channel, packet + pos, ext_len, 1)) { 1400 log_error("BNEP pkt handler: Ignore invalid control packet in extension header"); 1401 } 1402 1403 pos += ext_len; 1404 break; 1405 1406 default: 1407 /* Extension header type unknown. Unknown extension SHALL be 1408 * SHALL be forwarded in any way. But who shall handle these 1409 * extension packets? 1410 * For now: We ignore them and just drop them! 1411 */ 1412 log_error("BNEP pkt handler: Unknown extension type ignored, data dropped!"); 1413 pos += ext_len; 1414 break; 1415 } 1416 1417 } while (bnep_header_has_ext); 1418 } 1419 1420 if (bnep_type != BNEP_PKT_TYPE_CONTROL && network_protocol_type != 0xffff) { 1421 if (channel->state == BNEP_CHANNEL_STATE_CONNECTED) { 1422 rc = bnep_handle_ethernet_packet(channel, addr_dest, addr_source, network_protocol_type, packet + pos, size - pos); 1423 } else { 1424 rc = 0; 1425 } 1426 } 1427 1428 return rc; 1429 1430 } 1431 1432 void bnep_packet_handler(uint8_t packet_type, uint16_t l2cap_cid, uint8_t *packet, uint16_t size) 1433 { 1434 switch (packet_type) { 1435 case HCI_EVENT_PACKET: 1436 bnep_hci_event_handler(packet, size); 1437 break; 1438 case L2CAP_DATA_PACKET: 1439 bnep_l2cap_packet_handler(l2cap_cid, packet, size); 1440 break; 1441 default: 1442 break; 1443 } 1444 } 1445 1446 static void bnep_channel_state_machine(bnep_channel_t* channel, bnep_channel_event_t *event) 1447 { 1448 log_debug("bnep_state_machine: state %u, state var: %02x, event %u", channel->state, channel->state_var, event->type); 1449 1450 if (event->type == BNEP_CH_EVT_READY_TO_SEND) { 1451 /* Send outstanding packets. */ 1452 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_NOT_UNDERSTOOD) { 1453 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_NOT_UNDERSTOOD); 1454 bnep_send_command_not_understood(channel, channel->last_control_type); 1455 return; 1456 } 1457 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_REQUEST) { 1458 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_REQUEST); 1459 channel->state = BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_RESPONSE; 1460 bnep_send_connection_request(channel, channel->uuid_source, channel->uuid_dest); 1461 } 1462 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_RESPONSE) { 1463 int emit_connected = 0; 1464 if ((channel->state == BNEP_CHANNEL_STATE_CLOSED) || 1465 (channel->state == BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST)) { 1466 /* Set channel state to STATE_CONNECTED */ 1467 channel->state = BNEP_CHANNEL_STATE_CONNECTED; 1468 /* Stop timeout timer! */ 1469 bnep_channel_stop_timer(channel); 1470 emit_connected = 1; 1471 } 1472 1473 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_RESPONSE); 1474 bnep_send_connection_response(channel, channel->response_code); 1475 if (emit_connected){ 1476 bnep_emit_open_channel_complete(channel, 0); 1477 } 1478 return; 1479 } 1480 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_SET) { 1481 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_SET); 1482 if ((channel->net_filter_out_count > 0) && (channel->net_filter_out != NULL)) { 1483 bnep_send_filter_net_type_set(channel, channel->net_filter_out, channel->net_filter_out_count); 1484 channel->net_filter_out_count = 0; 1485 channel->net_filter_out = NULL; 1486 } 1487 return; 1488 } 1489 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_RESPONSE) { 1490 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_RESPONSE); 1491 bnep_send_filter_net_type_response(channel, channel->response_code); 1492 return; 1493 } 1494 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_SET) { 1495 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_SET); 1496 if ((channel->multicast_filter_out_count > 0) && (channel->multicast_filter_out != NULL)) { 1497 bnep_send_filter_multi_addr_set(channel, channel->multicast_filter_out, channel->multicast_filter_out_count); 1498 channel->multicast_filter_out_count = 0; 1499 channel->multicast_filter_out = NULL; 1500 } 1501 return; 1502 } 1503 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_RESPONSE) { 1504 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_RESPONSE); 1505 bnep_send_filter_multi_addr_response(channel, channel->response_code); 1506 return; 1507 } 1508 1509 /* If the event was not yet handled, notify the application layer */ 1510 if (channel->waiting_for_can_send_now){ 1511 channel->waiting_for_can_send_now = 0; 1512 bnep_emit_ready_to_send(channel); 1513 } 1514 } 1515 } 1516 1517 static void bnep_handle_can_send_now(uint16_t l2cap_cid){ 1518 btstack_linked_item_t *it; 1519 btstack_linked_item_t *next; 1520 1521 for (it = (btstack_linked_item_t *) bnep_channels; it ; it = next){ 1522 next = it->next; // be prepared for removal of channel in state machine 1523 bnep_channel_t * channel = ((bnep_channel_t *) it); 1524 if (channel->l2cap_cid != l2cap_cid) continue; 1525 // 1526 bnep_channel_event_t channel_event = { BNEP_CH_EVT_READY_TO_SEND }; 1527 bnep_channel_state_machine(channel, &channel_event); 1528 1529 if (!l2cap_can_send_packet_now(channel->l2cap_cid)) { 1530 l2cap_request_can_send_now_event(channel->l2cap_cid); 1531 return; 1532 } 1533 } 1534 } 1535 1536 1537 /* BNEP BTStack API */ 1538 void bnep_init(void) 1539 { 1540 bnep_security_level = LEVEL_0; 1541 } 1542 1543 void bnep_set_required_security_level(gap_security_level_t security_level) 1544 { 1545 bnep_security_level = security_level; 1546 } 1547 1548 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) 1549 { 1550 bnep_channel_t *channel; 1551 log_info("BNEP_CONNECT addr %s", bd_addr_to_str(addr)); 1552 1553 channel = bnep_channel_create_for_addr(addr); 1554 if (channel == NULL) { 1555 return -1; 1556 } 1557 1558 channel->uuid_source = uuid_src; 1559 channel->uuid_dest = uuid_dest; 1560 channel->packet_handler = packet_handler; 1561 1562 uint8_t status = l2cap_create_channel(bnep_packet_handler, addr, l2cap_psm, l2cap_max_mtu(), NULL); 1563 if (status){ 1564 return -1; 1565 } 1566 return 0; 1567 } 1568 1569 void bnep_disconnect(bd_addr_t addr) 1570 { 1571 bnep_channel_t *channel; 1572 log_info("BNEP_DISCONNECT"); 1573 1574 channel = bnep_channel_for_addr(addr); 1575 1576 bnep_channel_finalize(channel); 1577 } 1578 1579 1580 uint8_t bnep_register_service(btstack_packet_handler_t packet_handler, uint16_t service_uuid, uint16_t max_frame_size) 1581 { 1582 log_info("BNEP_REGISTER_SERVICE mtu %d", max_frame_size); 1583 1584 /* Check if we already registered a service */ 1585 bnep_service_t * service = bnep_service_for_uuid(service_uuid); 1586 if (service) { 1587 return BNEP_SERVICE_ALREADY_REGISTERED; 1588 } 1589 1590 /* Only alow one the three service types: PANU, NAP, GN */ 1591 if ((service_uuid != BLUETOOTH_SERVICE_CLASS_PANU) && 1592 (service_uuid != BLUETOOTH_SERVICE_CLASS_NAP) && 1593 (service_uuid != BLUETOOTH_SERVICE_CLASS_GN)) { 1594 log_info("BNEP_REGISTER_SERVICE: Invalid service UUID: %04x", service_uuid); 1595 return BNEP_SERVICE_ALREADY_REGISTERED; // TODO: define own error 1596 } 1597 1598 /* Allocate service memory */ 1599 service = (bnep_service_t*) btstack_memory_bnep_service_get(); 1600 if (!service) { 1601 return BTSTACK_MEMORY_ALLOC_FAILED; 1602 } 1603 memset(service, 0, sizeof(bnep_service_t)); 1604 1605 /* register with l2cap if not registered before, max MTU */ 1606 l2cap_register_service(bnep_packet_handler, BLUETOOTH_PROTOCOL_BNEP, 0xffff, bnep_security_level); 1607 1608 /* Setup the service struct */ 1609 service->max_frame_size = max_frame_size; 1610 service->service_uuid = service_uuid; 1611 service->packet_handler = packet_handler; 1612 1613 1614 /* Add to services list */ 1615 btstack_linked_list_add(&bnep_services, (btstack_linked_item_t *) service); 1616 1617 return 0; 1618 } 1619 1620 void bnep_unregister_service(uint16_t service_uuid) 1621 { 1622 log_info("BNEP_UNREGISTER_SERVICE #%04x", service_uuid); 1623 1624 bnep_service_t *service = bnep_service_for_uuid(service_uuid); 1625 if (!service) { 1626 return; 1627 } 1628 1629 btstack_linked_list_remove(&bnep_services, (btstack_linked_item_t *) service); 1630 btstack_memory_bnep_service_free(service); 1631 service = NULL; 1632 1633 l2cap_unregister_service(BLUETOOTH_PROTOCOL_BNEP); 1634 } 1635 1636