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 channel->state = BNEP_CHANNEL_STATE_CLOSED; 707 channel->max_frame_size = bnep_max_frame_size_for_l2cap_mtu(l2cap_max_mtu()); 708 bd_addr_copy(channel->remote_addr, addr); 709 gap_local_bd_addr(channel->local_addr); 710 711 channel->net_filter_count = 0; 712 channel->multicast_filter_count = 0; 713 channel->retry_count = 0; 714 715 /* Finally add it to the channel list */ 716 btstack_linked_list_add(&bnep_channels, (btstack_linked_item_t *) channel); 717 718 return channel; 719 } 720 721 static bnep_channel_t* bnep_channel_for_addr(bd_addr_t addr) 722 { 723 btstack_linked_item_t *it; 724 for (it = (btstack_linked_item_t *) bnep_channels; it ; it = it->next){ 725 bnep_channel_t *channel = ((bnep_channel_t *) it); 726 if (bd_addr_cmp(addr, channel->remote_addr) == 0) { 727 return channel; 728 } 729 } 730 return NULL; 731 } 732 733 static bnep_channel_t * bnep_channel_for_l2cap_cid(uint16_t l2cap_cid) 734 { 735 btstack_linked_item_t *it; 736 for (it = (btstack_linked_item_t *) bnep_channels; it ; it = it->next){ 737 bnep_channel_t *channel = ((bnep_channel_t *) it); 738 if (channel->l2cap_cid == l2cap_cid) { 739 return channel; 740 } 741 } 742 return NULL; 743 } 744 745 static bnep_service_t * bnep_service_for_uuid(uint16_t uuid) 746 { 747 btstack_linked_item_t *it; 748 for (it = (btstack_linked_item_t *) bnep_services; it ; it = it->next){ 749 bnep_service_t * service = ((bnep_service_t *) it); 750 if ( service->service_uuid == uuid){ 751 return service; 752 } 753 } 754 return NULL; 755 } 756 757 static void bnep_channel_free(bnep_channel_t *channel) 758 { 759 btstack_linked_list_remove( &bnep_channels, (btstack_linked_item_t *) channel); 760 btstack_memory_bnep_channel_free(channel); 761 } 762 763 static void bnep_channel_finalize(bnep_channel_t *channel) 764 { 765 uint16_t l2cap_cid; 766 767 /* Inform application about closed channel */ 768 if (channel->state == BNEP_CHANNEL_STATE_CONNECTED) { 769 bnep_emit_channel_closed(channel); 770 } 771 772 l2cap_cid = channel->l2cap_cid; 773 774 /* Stop any eventually running timer */ 775 bnep_channel_stop_timer(channel); 776 777 /* Free ressources and then close the l2cap channel */ 778 bnep_channel_free(channel); 779 l2cap_disconnect(l2cap_cid, 0x13); 780 } 781 782 static int bnep_handle_connection_request(bnep_channel_t *channel, uint8_t *packet, uint16_t size) 783 { 784 uint16_t uuid_size; 785 uint16_t uuid_offset = 0; // avoid "may be unitialized when used" in clang 786 uuid_size = packet[1]; 787 uint16_t response_code = BNEP_RESP_SETUP_SUCCESS; 788 bnep_service_t * service; 789 790 /* Sanity check packet size */ 791 if (size < 1 + 1 + 2 * uuid_size) { 792 return 0; 793 } 794 795 if ((channel->state != BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST) && 796 (channel->state != BNEP_CHANNEL_STATE_CONNECTED)) { 797 /* Ignore a connection request if not waiting for or still connected */ 798 log_error("BNEP_CONNECTION_REQUEST: ignored in state %d, l2cap_cid: %d!", channel->state, channel->l2cap_cid); 799 return 0; 800 } 801 802 /* Extract source and destination UUID and convert them to UUID16 format */ 803 switch (uuid_size) { 804 case 2: /* UUID16 */ 805 uuid_offset = 0; 806 break; 807 case 4: /* UUID32 */ 808 case 16: /* UUID128 */ 809 uuid_offset = 2; 810 break; 811 default: 812 log_error("BNEP_CONNECTION_REQUEST: Invalid UUID size %d, l2cap_cid: %d!", channel->state, channel->l2cap_cid); 813 response_code = BNEP_RESP_SETUP_INVALID_SERVICE_UUID_SIZE; 814 break; 815 } 816 817 /* Check source and destination UUIDs for valid combinations */ 818 if (response_code == BNEP_RESP_SETUP_SUCCESS) { 819 channel->uuid_dest = big_endian_read_16(packet, 2 + uuid_offset); 820 channel->uuid_source = big_endian_read_16(packet, 2 + uuid_offset + uuid_size); 821 822 if ((channel->uuid_dest != BLUETOOTH_SERVICE_CLASS_PANU) && 823 (channel->uuid_dest != BLUETOOTH_SERVICE_CLASS_NAP) && 824 (channel->uuid_dest != BLUETOOTH_SERVICE_CLASS_GN)) { 825 log_error("BNEP_CONNECTION_REQUEST: Invalid destination service UUID: %04x", channel->uuid_dest); 826 channel->uuid_dest = 0; 827 } 828 if ((channel->uuid_source != BLUETOOTH_SERVICE_CLASS_PANU) && 829 (channel->uuid_source != BLUETOOTH_SERVICE_CLASS_NAP) && 830 (channel->uuid_source != BLUETOOTH_SERVICE_CLASS_GN)) { 831 log_error("BNEP_CONNECTION_REQUEST: Invalid source service UUID: %04x", channel->uuid_source); 832 channel->uuid_source = 0; 833 } 834 835 /* Check if we have registered a service for the requested destination UUID */ 836 service = bnep_service_for_uuid(channel->uuid_dest); 837 if (service == NULL) { 838 response_code = BNEP_RESP_SETUP_INVALID_DEST_UUID; 839 } else { 840 // use packet handler for service 841 channel->packet_handler = service->packet_handler; 842 843 if ((channel->uuid_source != BLUETOOTH_SERVICE_CLASS_PANU) && (channel->uuid_dest != BLUETOOTH_SERVICE_CLASS_PANU)) { 844 response_code = BNEP_RESP_SETUP_INVALID_SOURCE_UUID; 845 } 846 } 847 } 848 849 /* Set flag to send out the connection response on next statemachine cycle */ 850 bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_RESPONSE); 851 channel->response_code = response_code; 852 l2cap_request_can_send_now_event(channel->l2cap_cid); 853 854 /* Return the number of processed package bytes = BNEP Type, BNEP Control Type, UUID-Size + 2 * UUID */ 855 return 1 + 1 + 2 * uuid_size; 856 } 857 858 static int bnep_handle_connection_response(bnep_channel_t *channel, uint8_t *packet, uint16_t size) 859 { 860 uint16_t response_code; 861 862 /* Sanity check packet size */ 863 if (size < 1 + 2) { 864 return 0; 865 } 866 867 if (channel->state != BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_RESPONSE) { 868 /* Ignore a connection response in any state but WAIT_FOR_CONNECTION_RESPONSE */ 869 log_error("BNEP_CONNECTION_RESPONSE: Ignored in channel state %d", channel->state); 870 return 1 + 2; 871 } 872 873 response_code = big_endian_read_16(packet, 1); 874 875 if (response_code == BNEP_RESP_SETUP_SUCCESS) { 876 log_info("BNEP_CONNECTION_RESPONSE: Channel established to %s", bd_addr_to_str(channel->remote_addr)); 877 channel->state = BNEP_CHANNEL_STATE_CONNECTED; 878 /* Stop timeout timer! */ 879 bnep_channel_stop_timer(channel); 880 bnep_emit_open_channel_complete(channel, 0); 881 } else { 882 log_error("BNEP_CONNECTION_RESPONSE: Connection to %s failed. Err: %d", bd_addr_to_str(channel->remote_addr), response_code); 883 bnep_channel_finalize(channel); 884 } 885 return 1 + 2; 886 } 887 888 static int bnep_can_handle_extensions(bnep_channel_t * channel){ 889 /* Extension are primarily handled in CONNECTED state */ 890 if (channel->state == BNEP_CHANNEL_STATE_CONNECTED) return 1; 891 /* and if we've received connection request, but haven't sent the reponse yet. */ 892 if ((channel->state == BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST) && 893 (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_RESPONSE)) { 894 return 1; 895 } 896 return 0; 897 } 898 899 static int bnep_handle_filter_net_type_set(bnep_channel_t *channel, uint8_t *packet, uint16_t size) 900 { 901 uint16_t list_length; 902 uint16_t response_code = BNEP_RESP_FILTER_SUCCESS; 903 904 /* Sanity check packet size */ 905 if (size < 3) { 906 return 0; 907 } 908 909 list_length = big_endian_read_16(packet, 1); 910 /* Sanity check packet size again with known package size */ 911 if (size < 3 + list_length) { 912 return 0; 913 } 914 915 if (!bnep_can_handle_extensions(channel)){ 916 log_error("BNEP_FILTER_NET_TYPE_SET: Ignored in channel state %d", channel->state); 917 return 3 + list_length; 918 } 919 920 /* Check if we have enough space for more filters */ 921 if ((list_length / (2*2)) > MAX_BNEP_NETFILTER) { 922 log_info("BNEP_FILTER_NET_TYPE_SET: Too many filter"); 923 response_code = BNEP_RESP_FILTER_ERR_TOO_MANY_FILTERS; 924 } else { 925 int i; 926 channel->net_filter_count = 0; 927 /* There is still enough space, copy the filters to our filter list */ 928 /* There is still enough space, copy the filters to our filter list */ 929 for (i = 0; i < list_length / (2 * 2); i ++) { 930 channel->net_filter[channel->net_filter_count].range_start = big_endian_read_16(packet, 1 + 2 + i * 4); 931 channel->net_filter[channel->net_filter_count].range_end = big_endian_read_16(packet, 1 + 2 + i * 4 + 2); 932 if (channel->net_filter[channel->net_filter_count].range_start > channel->net_filter[channel->net_filter_count].range_end) { 933 /* Invalid filter range, ignore this filter rule */ 934 log_error("BNEP_FILTER_NET_TYPE_SET: Invalid filter: start: %d, end: %d", 935 channel->net_filter[channel->net_filter_count].range_start, 936 channel->net_filter[channel->net_filter_count].range_end); 937 response_code = BNEP_RESP_FILTER_ERR_INVALID_RANGE; 938 } else { 939 /* Valid filter, increase the filter count */ 940 log_info("BNEP_FILTER_NET_TYPE_SET: Add filter: start: %d, end: %d", 941 channel->net_filter[channel->net_filter_count].range_start, 942 channel->net_filter[channel->net_filter_count].range_end); 943 channel->net_filter_count ++; 944 } 945 } 946 } 947 948 /* Set flag to send out the set net filter response on next statemachine cycle */ 949 bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_RESPONSE); 950 channel->response_code = response_code; 951 l2cap_request_can_send_now_event(channel->l2cap_cid); 952 953 return 3 + list_length; 954 } 955 956 static int bnep_handle_filter_net_type_response(bnep_channel_t *channel, uint8_t *packet, uint16_t size) 957 { 958 uint16_t response_code; 959 960 // TODO: Currently we do not support setting a network filter. 961 962 /* Sanity check packet size */ 963 if (size < 1 + 2) { 964 return 0; 965 } 966 967 if (!bnep_can_handle_extensions(channel)){ 968 log_error("BNEP_FILTER_NET_TYPE_RESPONSE: Ignored in channel state %d", channel->state); 969 return 1 + 2; 970 } 971 972 response_code = big_endian_read_16(packet, 1); 973 974 if (response_code == BNEP_RESP_FILTER_SUCCESS) { 975 log_info("BNEP_FILTER_NET_TYPE_RESPONSE: Net filter set successfully for %s", bd_addr_to_str(channel->remote_addr)); 976 } else { 977 log_error("BNEP_FILTER_NET_TYPE_RESPONSE: Net filter setting for %s failed. Err: %d", bd_addr_to_str(channel->remote_addr), response_code); 978 } 979 980 return 1 + 2; 981 } 982 983 static int bnep_handle_multi_addr_set(bnep_channel_t *channel, uint8_t *packet, uint16_t size) 984 { 985 uint16_t list_length; 986 uint16_t response_code = BNEP_RESP_FILTER_SUCCESS; 987 988 /* Sanity check packet size */ 989 if (size < 3) { 990 return 0; 991 } 992 993 list_length = big_endian_read_16(packet, 1); 994 /* Sanity check packet size again with known package size */ 995 if (size < 3 + list_length) { 996 return 0; 997 } 998 999 if (!bnep_can_handle_extensions(channel)){ 1000 log_error("BNEP_MULTI_ADDR_SET: Ignored in channel state %d", channel->state); 1001 return 3 + list_length; 1002 } 1003 1004 /* Check if we have enough space for more filters */ 1005 if ((list_length / (2 * ETHER_ADDR_LEN)) > MAX_BNEP_MULTICAST_FILTER) { 1006 log_info("BNEP_MULTI_ADDR_SET: Too many filter"); 1007 response_code = BNEP_RESP_FILTER_ERR_TOO_MANY_FILTERS; 1008 } else { 1009 unsigned int i; 1010 channel->multicast_filter_count = 0; 1011 /* There is enough space, copy the filters to our filter list */ 1012 for (i = 0; i < list_length / (2 * ETHER_ADDR_LEN); i ++) { 1013 bd_addr_copy(channel->multicast_filter[channel->multicast_filter_count].addr_start, packet + 1 + 2 + i * ETHER_ADDR_LEN * 2); 1014 bd_addr_copy(channel->multicast_filter[channel->multicast_filter_count].addr_end, packet + 1 + 2 + i * ETHER_ADDR_LEN * 2 + ETHER_ADDR_LEN); 1015 1016 if (memcmp(channel->multicast_filter[channel->multicast_filter_count].addr_start, 1017 channel->multicast_filter[channel->multicast_filter_count].addr_end, ETHER_ADDR_LEN) > 0) { 1018 /* Invalid filter range, ignore this filter rule */ 1019 log_error("BNEP_MULTI_ADDR_SET: Invalid filter: start: %s", 1020 bd_addr_to_str(channel->multicast_filter[channel->multicast_filter_count].addr_start)); 1021 log_error("BNEP_MULTI_ADDR_SET: Invalid filter: end: %s", 1022 bd_addr_to_str(channel->multicast_filter[channel->multicast_filter_count].addr_end)); 1023 response_code = BNEP_RESP_FILTER_ERR_INVALID_RANGE; 1024 } else { 1025 /* Valid filter, increase the filter count */ 1026 log_info("BNEP_MULTI_ADDR_SET: Add filter: start: %s", 1027 bd_addr_to_str(channel->multicast_filter[channel->multicast_filter_count].addr_start)); 1028 log_info("BNEP_MULTI_ADDR_SET: Add filter: end: %s", 1029 bd_addr_to_str(channel->multicast_filter[channel->multicast_filter_count].addr_end)); 1030 channel->multicast_filter_count ++; 1031 } 1032 } 1033 } 1034 /* Set flag to send out the set multi addr response on next statemachine cycle */ 1035 bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_RESPONSE); 1036 channel->response_code = response_code; 1037 l2cap_request_can_send_now_event(channel->l2cap_cid); 1038 1039 return 3 + list_length; 1040 } 1041 1042 static int bnep_handle_multi_addr_response(bnep_channel_t *channel, uint8_t *packet, uint16_t size) 1043 { 1044 uint16_t response_code; 1045 1046 // TODO: Currently we do not support setting multicast address filter. 1047 1048 /* Sanity check packet size */ 1049 if (size < 1 + 2) { 1050 return 0; 1051 } 1052 1053 if (!bnep_can_handle_extensions(channel)){ 1054 log_error("BNEP_MULTI_ADDR_RESPONSE: Ignored in channel state %d", channel->state); 1055 return 1 + 2; 1056 } 1057 1058 response_code = big_endian_read_16(packet, 1); 1059 1060 if (response_code == BNEP_RESP_FILTER_SUCCESS) { 1061 log_info("BNEP_MULTI_ADDR_RESPONSE: Multicast address filter set successfully for %s", bd_addr_to_str(channel->remote_addr)); 1062 } else { 1063 log_error("BNEP_MULTI_ADDR_RESPONSE: Multicast address filter setting for %s failed. Err: %d", bd_addr_to_str(channel->remote_addr), response_code); 1064 } 1065 1066 return 1 + 2; 1067 } 1068 1069 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) 1070 { 1071 uint16_t pos = 0; 1072 1073 #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) 1074 /* In-place modify the package and add the ethernet header in front of the payload. 1075 * WARNING: This modifies the data in front of the payload and may overwrite 14 bytes there! 1076 */ 1077 uint8_t *ethernet_packet = payload - 2 * sizeof(bd_addr_t) - sizeof(uint16_t); 1078 /* Restore the ethernet packet header */ 1079 bd_addr_copy(ethernet_packet + pos, addr_dest); 1080 pos += sizeof(bd_addr_t); 1081 bd_addr_copy(ethernet_packet + pos, addr_source); 1082 pos += sizeof(bd_addr_t); 1083 big_endian_store_16(ethernet_packet, pos, network_protocol_type); 1084 /* Payload is just in place... */ 1085 #else 1086 #error "BNEP requires HCI_INCOMING_PRE_BUFFER_SIZE >= 6. Please update bstack_config.h" 1087 #endif 1088 1089 /* Notify application layer and deliver the ethernet packet */ 1090 if (channel->packet_handler){ 1091 (*channel->packet_handler)(BNEP_DATA_PACKET, channel->l2cap_cid, ethernet_packet, 1092 size + sizeof(uint16_t) + 2 * sizeof(bd_addr_t)); 1093 } 1094 1095 return size; 1096 } 1097 1098 static int bnep_handle_control_packet(bnep_channel_t *channel, uint8_t *packet, uint16_t size, int is_extension) 1099 { 1100 uint16_t len = 0; 1101 uint8_t bnep_control_type; 1102 1103 bnep_control_type = packet[0]; 1104 /* Save last control type. Needed by statemachin in case of unknown control code */ 1105 1106 channel->last_control_type = bnep_control_type; 1107 log_info("BNEP_CONTROL: Type: %d, size: %d, is_extension: %d", bnep_control_type, size, is_extension); 1108 switch (bnep_control_type) { 1109 case BNEP_CONTROL_TYPE_COMMAND_NOT_UNDERSTOOD: 1110 /* The last command we send was not understood. We should close the connection */ 1111 log_error("BNEP_CONTROL: Received COMMAND_NOT_UNDERSTOOD: l2cap_cid: %d, cmd: %d", channel->l2cap_cid, packet[3]); 1112 bnep_channel_finalize(channel); 1113 len = 2; // Length of command not understood packet - bnep-type field 1114 break; 1115 case BNEP_CONTROL_TYPE_SETUP_CONNECTION_REQUEST: 1116 if (is_extension) { 1117 /* Connection requests are not allowed to be send in an extension header 1118 * ignore, do not set "COMMAND_NOT_UNDERSTOOD" 1119 */ 1120 log_error("BNEP_CONTROL: Received SETUP_CONNECTION_REQUEST in extension header: l2cap_cid: %d", channel->l2cap_cid); 1121 return 0; 1122 } else { 1123 len = bnep_handle_connection_request(channel, packet, size); 1124 } 1125 break; 1126 case BNEP_CONTROL_TYPE_SETUP_CONNECTION_RESPONSE: 1127 if (is_extension) { 1128 /* Connection requests are not allowed to be send in an 1129 * extension header, ignore, do not set "COMMAND_NOT_UNDERSTOOD" 1130 */ 1131 log_error("BNEP_CONTROL: Received SETUP_CONNECTION_RESPONSE in extension header: l2cap_cid: %d", channel->l2cap_cid); 1132 return 0; 1133 } else { 1134 len = bnep_handle_connection_response(channel, packet, size); 1135 } 1136 break; 1137 case BNEP_CONTROL_TYPE_FILTER_NET_TYPE_SET: 1138 len = bnep_handle_filter_net_type_set(channel, packet, size); 1139 break; 1140 case BNEP_CONTROL_TYPE_FILTER_NET_TYPE_RESPONSE: 1141 len = bnep_handle_filter_net_type_response(channel, packet, size); 1142 break; 1143 case BNEP_CONTROL_TYPE_FILTER_MULTI_ADDR_SET: 1144 len = bnep_handle_multi_addr_set(channel, packet, size); 1145 break; 1146 case BNEP_CONTROL_TYPE_FILTER_MULTI_ADDR_RESPONSE: 1147 len = bnep_handle_multi_addr_response(channel, packet, size); 1148 break; 1149 default: 1150 log_error("BNEP_CONTROL: Invalid bnep control type: l2cap_cid: %d, cmd: %d", channel->l2cap_cid, bnep_control_type); 1151 len = 0; 1152 break; 1153 } 1154 1155 if (len == 0) { 1156 /* In case the command could not be handled, send a 1157 COMMAND_NOT_UNDERSTOOD message. 1158 Set flag to process the request in the next statemachine loop 1159 */ 1160 bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_NOT_UNDERSTOOD); 1161 l2cap_request_can_send_now_event(channel->l2cap_cid); 1162 } 1163 1164 return len; 1165 } 1166 1167 /** 1168 * @return handled packet 1169 */ 1170 static int bnep_hci_event_handler(uint8_t *packet, uint16_t size) 1171 { 1172 UNUSED(size); // ok: handling own l2cap events 1173 1174 bd_addr_t event_addr; 1175 uint16_t psm; 1176 uint16_t l2cap_cid; 1177 hci_con_handle_t con_handle; 1178 bnep_channel_t *channel = NULL; 1179 uint8_t status; 1180 1181 switch (hci_event_packet_get_type(packet)) { 1182 1183 /* Accept an incoming L2CAP connection on BLUETOOTH_PROTOCOL_BNEP */ 1184 case L2CAP_EVENT_INCOMING_CONNECTION: 1185 /* L2CAP event data: event(8), len(8), address(48), handle (16), psm (16), source cid(16) dest cid(16) */ 1186 reverse_bd_addr(&packet[2], event_addr); 1187 con_handle = little_endian_read_16(packet, 8); 1188 psm = little_endian_read_16(packet, 10); 1189 l2cap_cid = little_endian_read_16(packet, 12); 1190 1191 if (psm != BLUETOOTH_PROTOCOL_BNEP) break; 1192 1193 channel = bnep_channel_for_addr(event_addr); 1194 1195 if (channel) { 1196 log_error("INCOMING_CONNECTION (l2cap_cid 0x%02x) for BLUETOOTH_PROTOCOL_BNEP => decline - channel already exists", l2cap_cid); 1197 l2cap_decline_connection(l2cap_cid); 1198 return 1; 1199 } 1200 1201 /* Create a new BNEP channel instance (incoming) */ 1202 channel = bnep_channel_create_for_addr(event_addr); 1203 1204 if (!channel) { 1205 log_error("INCOMING_CONNECTION (l2cap_cid 0x%02x) for BLUETOOTH_PROTOCOL_BNEP => decline - no memory left", l2cap_cid); 1206 l2cap_decline_connection(l2cap_cid); 1207 return 1; 1208 } 1209 1210 /* Assign connection handle and l2cap cid */ 1211 channel->con_handle = con_handle; 1212 channel->l2cap_cid = l2cap_cid; 1213 1214 /* Set channel into accept state */ 1215 channel->state = BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST; 1216 1217 /* Start connection timeout timer */ 1218 bnep_channel_start_timer(channel, BNEP_CONNECTION_TIMEOUT_MS); 1219 1220 log_info("L2CAP_EVENT_INCOMING_CONNECTION (l2cap_cid 0x%02x) for BLUETOOTH_PROTOCOL_BNEP => accept", l2cap_cid); 1221 l2cap_accept_connection(l2cap_cid); 1222 return 1; 1223 1224 /* Outgoing L2CAP connection has been opened -> store l2cap_cid, remote_addr */ 1225 case L2CAP_EVENT_CHANNEL_OPENED: 1226 /* Check if the l2cap channel has been opened for BLUETOOTH_PROTOCOL_BNEP */ 1227 if (little_endian_read_16(packet, 11) != BLUETOOTH_PROTOCOL_BNEP) { 1228 break; 1229 } 1230 1231 status = packet[2]; 1232 log_info("L2CAP_EVENT_CHANNEL_OPENED for BLUETOOTH_PROTOCOL_BNEP, status %u", status); 1233 1234 /* Get the bnep channel fpr remote address */ 1235 con_handle = little_endian_read_16(packet, 9); 1236 l2cap_cid = little_endian_read_16(packet, 13); 1237 reverse_bd_addr(&packet[3], event_addr); 1238 channel = bnep_channel_for_addr(event_addr); 1239 if (!channel) { 1240 log_error("L2CAP_EVENT_CHANNEL_OPENED but no BNEP channel prepared"); 1241 return 1; 1242 } 1243 1244 /* On L2CAP open error discard everything */ 1245 if (status) { 1246 /* Emit bnep_open_channel_complete with status and free channel */ 1247 bnep_emit_open_channel_complete(channel, status); 1248 1249 /* Free BNEP channel mempory */ 1250 bnep_channel_free(channel); 1251 return 1; 1252 } 1253 1254 switch (channel->state){ 1255 case BNEP_CHANNEL_STATE_CLOSED: 1256 log_info("L2CAP_EVENT_CHANNEL_OPENED: outgoing connection"); 1257 1258 bnep_channel_start_timer(channel, BNEP_CONNECTION_TIMEOUT_MS); 1259 1260 /* Assign connection handle and l2cap cid */ 1261 channel->l2cap_cid = l2cap_cid; 1262 channel->con_handle = con_handle; 1263 1264 /* Initiate the connection request */ 1265 channel->state = BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_RESPONSE; 1266 bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_REQUEST); 1267 channel->max_frame_size = bnep_max_frame_size_for_l2cap_mtu(little_endian_read_16(packet, 17)); 1268 l2cap_request_can_send_now_event(channel->l2cap_cid); 1269 break; 1270 case BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST: 1271 /* New information: channel mtu */ 1272 channel->max_frame_size = bnep_max_frame_size_for_l2cap_mtu(little_endian_read_16(packet, 17)); 1273 break; 1274 default: 1275 log_error("L2CAP_EVENT_CHANNEL_OPENED: Invalid state: %d", channel->state); 1276 break; 1277 } 1278 return 1; 1279 1280 case L2CAP_EVENT_CAN_SEND_NOW: 1281 bnep_handle_can_send_now(l2cap_event_can_send_now_get_local_cid(packet)); 1282 break; 1283 1284 case L2CAP_EVENT_CHANNEL_CLOSED: 1285 // data: event (8), len(8), channel (16) 1286 l2cap_cid = little_endian_read_16(packet, 2); 1287 channel = bnep_channel_for_l2cap_cid(l2cap_cid); 1288 log_info("L2CAP_EVENT_CHANNEL_CLOSED cid 0x%0x, channel %p", l2cap_cid, channel); 1289 1290 if (!channel) { 1291 break; 1292 } 1293 1294 log_info("L2CAP_EVENT_CHANNEL_CLOSED state %u", channel->state); 1295 switch (channel->state) { 1296 case BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST: 1297 case BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_RESPONSE: 1298 case BNEP_CHANNEL_STATE_CONNECTED: 1299 bnep_channel_finalize(channel); 1300 return 1; 1301 default: 1302 break; 1303 } 1304 break; 1305 default: 1306 break; 1307 } 1308 return 0; 1309 } 1310 1311 static int bnep_l2cap_packet_handler(uint16_t l2cap_cid, uint8_t *packet, uint16_t size) 1312 { 1313 int rc = 0; 1314 uint8_t bnep_type; 1315 uint8_t bnep_header_has_ext; 1316 uint8_t extension_type; 1317 uint16_t pos = 0; 1318 bd_addr_t addr_source; 1319 bd_addr_t addr_dest; 1320 uint16_t network_protocol_type = 0xffff; 1321 bnep_channel_t *channel = NULL; 1322 1323 /* Get the bnep channel for this package */ 1324 channel = bnep_channel_for_l2cap_cid(l2cap_cid); 1325 if (!channel) { 1326 return rc; 1327 } 1328 1329 /* Sort out short packages */ 1330 if (size < 2) { 1331 return rc; 1332 } 1333 1334 bnep_type = BNEP_TYPE(packet[pos]); 1335 bnep_header_has_ext = BNEP_HEADER_HAS_EXT(packet[pos]); 1336 pos ++; 1337 1338 switch(bnep_type) { 1339 case BNEP_PKT_TYPE_GENERAL_ETHERNET: 1340 bd_addr_copy(addr_dest, &packet[pos]); 1341 pos += sizeof(bd_addr_t); 1342 bd_addr_copy(addr_source, &packet[pos]); 1343 pos += sizeof(bd_addr_t); 1344 network_protocol_type = big_endian_read_16(packet, pos); 1345 pos += 2; 1346 break; 1347 case BNEP_PKT_TYPE_COMPRESSED_ETHERNET: 1348 bd_addr_copy(addr_dest, channel->local_addr); 1349 bd_addr_copy(addr_source, channel->remote_addr); 1350 network_protocol_type = big_endian_read_16(packet, pos); 1351 pos += 2; 1352 break; 1353 case BNEP_PKT_TYPE_COMPRESSED_ETHERNET_SOURCE_ONLY: 1354 bd_addr_copy(addr_dest, channel->local_addr); 1355 bd_addr_copy(addr_source, &packet[pos]); 1356 pos += sizeof(bd_addr_t); 1357 network_protocol_type = big_endian_read_16(packet, pos); 1358 pos += 2; 1359 break; 1360 case BNEP_PKT_TYPE_COMPRESSED_ETHERNET_DEST_ONLY: 1361 bd_addr_copy(addr_dest, &packet[pos]); 1362 pos += sizeof(bd_addr_t); 1363 bd_addr_copy(addr_source, channel->remote_addr); 1364 network_protocol_type = big_endian_read_16(packet, pos); 1365 pos += 2; 1366 break; 1367 case BNEP_PKT_TYPE_CONTROL: 1368 rc = bnep_handle_control_packet(channel, packet + pos, size - pos, 0); 1369 pos += rc; 1370 break; 1371 default: 1372 break; 1373 } 1374 1375 if (bnep_header_has_ext) { 1376 do { 1377 uint8_t ext_len; 1378 1379 /* Read extension type and check for further extensions */ 1380 extension_type = BNEP_TYPE(packet[pos]); 1381 bnep_header_has_ext = BNEP_HEADER_HAS_EXT(packet[pos]); 1382 pos ++; 1383 1384 /* Read extension header length */ 1385 ext_len = packet[pos]; 1386 pos ++; 1387 1388 if (size - pos < ext_len) { 1389 log_error("BNEP pkt handler: Invalid extension length! Packet ignored"); 1390 /* Invalid packet size! */ 1391 return 0; 1392 } 1393 1394 switch (extension_type) { 1395 case BNEP_EXT_HEADER_TYPE_EXTENSION_CONTROL: 1396 if (ext_len != bnep_handle_control_packet(channel, packet + pos, ext_len, 1)) { 1397 log_error("BNEP pkt handler: Ignore invalid control packet in extension header"); 1398 } 1399 1400 pos += ext_len; 1401 break; 1402 1403 default: 1404 /* Extension header type unknown. Unknown extension SHALL be 1405 * SHALL be forwarded in any way. But who shall handle these 1406 * extension packets? 1407 * For now: We ignore them and just drop them! 1408 */ 1409 log_error("BNEP pkt handler: Unknown extension type ignored, data dropped!"); 1410 pos += ext_len; 1411 break; 1412 } 1413 1414 } while (bnep_header_has_ext); 1415 } 1416 1417 if (bnep_type != BNEP_PKT_TYPE_CONTROL && network_protocol_type != 0xffff) { 1418 if (channel->state == BNEP_CHANNEL_STATE_CONNECTED) { 1419 rc = bnep_handle_ethernet_packet(channel, addr_dest, addr_source, network_protocol_type, packet + pos, size - pos); 1420 } else { 1421 rc = 0; 1422 } 1423 } 1424 1425 return rc; 1426 1427 } 1428 1429 void bnep_packet_handler(uint8_t packet_type, uint16_t l2cap_cid, uint8_t *packet, uint16_t size) 1430 { 1431 switch (packet_type) { 1432 case HCI_EVENT_PACKET: 1433 bnep_hci_event_handler(packet, size); 1434 break; 1435 case L2CAP_DATA_PACKET: 1436 bnep_l2cap_packet_handler(l2cap_cid, packet, size); 1437 break; 1438 default: 1439 break; 1440 } 1441 } 1442 1443 static void bnep_channel_state_machine(bnep_channel_t* channel, bnep_channel_event_t *event) 1444 { 1445 log_debug("bnep_state_machine: state %u, state var: %02x, event %u", channel->state, channel->state_var, event->type); 1446 1447 if (event->type == BNEP_CH_EVT_READY_TO_SEND) { 1448 /* Send outstanding packets. */ 1449 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_NOT_UNDERSTOOD) { 1450 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_NOT_UNDERSTOOD); 1451 bnep_send_command_not_understood(channel, channel->last_control_type); 1452 return; 1453 } 1454 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_REQUEST) { 1455 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_REQUEST); 1456 channel->state = BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_RESPONSE; 1457 bnep_send_connection_request(channel, channel->uuid_source, channel->uuid_dest); 1458 } 1459 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_RESPONSE) { 1460 int emit_connected = 0; 1461 if ((channel->state == BNEP_CHANNEL_STATE_CLOSED) || 1462 (channel->state == BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST)) { 1463 /* Set channel state to STATE_CONNECTED */ 1464 channel->state = BNEP_CHANNEL_STATE_CONNECTED; 1465 /* Stop timeout timer! */ 1466 bnep_channel_stop_timer(channel); 1467 emit_connected = 1; 1468 } 1469 1470 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_RESPONSE); 1471 bnep_send_connection_response(channel, channel->response_code); 1472 if (emit_connected){ 1473 bnep_emit_open_channel_complete(channel, 0); 1474 } 1475 return; 1476 } 1477 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_SET) { 1478 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_SET); 1479 if ((channel->net_filter_out_count > 0) && (channel->net_filter_out != NULL)) { 1480 bnep_send_filter_net_type_set(channel, channel->net_filter_out, channel->net_filter_out_count); 1481 channel->net_filter_out_count = 0; 1482 channel->net_filter_out = NULL; 1483 } 1484 return; 1485 } 1486 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_RESPONSE) { 1487 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_RESPONSE); 1488 bnep_send_filter_net_type_response(channel, channel->response_code); 1489 return; 1490 } 1491 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_SET) { 1492 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_SET); 1493 if ((channel->multicast_filter_out_count > 0) && (channel->multicast_filter_out != NULL)) { 1494 bnep_send_filter_multi_addr_set(channel, channel->multicast_filter_out, channel->multicast_filter_out_count); 1495 channel->multicast_filter_out_count = 0; 1496 channel->multicast_filter_out = NULL; 1497 } 1498 return; 1499 } 1500 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_RESPONSE) { 1501 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_RESPONSE); 1502 bnep_send_filter_multi_addr_response(channel, channel->response_code); 1503 return; 1504 } 1505 1506 /* If the event was not yet handled, notify the application layer */ 1507 if (channel->waiting_for_can_send_now){ 1508 channel->waiting_for_can_send_now = 0; 1509 bnep_emit_ready_to_send(channel); 1510 } 1511 } 1512 } 1513 1514 static void bnep_handle_can_send_now(uint16_t l2cap_cid){ 1515 btstack_linked_item_t *it; 1516 btstack_linked_item_t *next; 1517 1518 for (it = (btstack_linked_item_t *) bnep_channels; it ; it = next){ 1519 next = it->next; // be prepared for removal of channel in state machine 1520 bnep_channel_t * channel = ((bnep_channel_t *) it); 1521 if (channel->l2cap_cid != l2cap_cid) continue; 1522 // 1523 bnep_channel_event_t channel_event = { BNEP_CH_EVT_READY_TO_SEND }; 1524 bnep_channel_state_machine(channel, &channel_event); 1525 1526 if (!l2cap_can_send_packet_now(channel->l2cap_cid)) { 1527 l2cap_request_can_send_now_event(channel->l2cap_cid); 1528 return; 1529 } 1530 } 1531 } 1532 1533 1534 /* BNEP BTStack API */ 1535 void bnep_init(void) 1536 { 1537 bnep_security_level = LEVEL_2; 1538 } 1539 1540 void bnep_set_required_security_level(gap_security_level_t security_level) 1541 { 1542 bnep_security_level = security_level; 1543 } 1544 1545 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) 1546 { 1547 bnep_channel_t *channel; 1548 log_info("BNEP_CONNECT addr %s", bd_addr_to_str(addr)); 1549 1550 channel = bnep_channel_create_for_addr(addr); 1551 if (channel == NULL) { 1552 return -1; 1553 } 1554 1555 channel->uuid_source = uuid_src; 1556 channel->uuid_dest = uuid_dest; 1557 channel->packet_handler = packet_handler; 1558 1559 uint8_t status = l2cap_create_channel(bnep_packet_handler, addr, l2cap_psm, l2cap_max_mtu(), NULL); 1560 if (status){ 1561 return -1; 1562 } 1563 return 0; 1564 } 1565 1566 void bnep_disconnect(bd_addr_t addr) 1567 { 1568 bnep_channel_t *channel; 1569 log_info("BNEP_DISCONNECT"); 1570 1571 channel = bnep_channel_for_addr(addr); 1572 1573 bnep_channel_finalize(channel); 1574 } 1575 1576 1577 uint8_t bnep_register_service(btstack_packet_handler_t packet_handler, uint16_t service_uuid, uint16_t max_frame_size) 1578 { 1579 log_info("BNEP_REGISTER_SERVICE mtu %d", max_frame_size); 1580 1581 /* Check if we already registered a service */ 1582 bnep_service_t * service = bnep_service_for_uuid(service_uuid); 1583 if (service) { 1584 return BNEP_SERVICE_ALREADY_REGISTERED; 1585 } 1586 1587 /* Only alow one the three service types: PANU, NAP, GN */ 1588 if ((service_uuid != BLUETOOTH_SERVICE_CLASS_PANU) && 1589 (service_uuid != BLUETOOTH_SERVICE_CLASS_NAP) && 1590 (service_uuid != BLUETOOTH_SERVICE_CLASS_GN)) { 1591 log_info("BNEP_REGISTER_SERVICE: Invalid service UUID: %04x", service_uuid); 1592 return BNEP_SERVICE_ALREADY_REGISTERED; // TODO: define own error 1593 } 1594 1595 /* Allocate service memory */ 1596 service = (bnep_service_t*) btstack_memory_bnep_service_get(); 1597 if (!service) { 1598 return BTSTACK_MEMORY_ALLOC_FAILED; 1599 } 1600 1601 /* register with l2cap if not registered before, max MTU */ 1602 l2cap_register_service(bnep_packet_handler, BLUETOOTH_PROTOCOL_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(BLUETOOTH_PROTOCOL_BNEP); 1630 } 1631 1632