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