1 /* 2 * Copyright (C) 2014 BlueKitchen GmbH 3 * 4 * Redistribution and use in source and binary forms, with or without 5 * modification, are permitted provided that the following conditions 6 * are met: 7 * 8 * 1. Redistributions of source code must retain the above copyright 9 * notice, this list of conditions and the following disclaimer. 10 * 2. Redistributions in binary form must reproduce the above copyright 11 * notice, this list of conditions and the following disclaimer in the 12 * documentation and/or other materials provided with the distribution. 13 * 3. Neither the name of the copyright holders nor the names of 14 * contributors may be used to endorse or promote products derived 15 * from this software without specific prior written permission. 16 * 4. Any redistribution, use, or modification is done solely for 17 * personal benefit and not for any commercial purpose or for 18 * monetary gain. 19 * 20 * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS 21 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS 24 * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF 30 * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 31 * SUCH DAMAGE. 32 * 33 * Please inquire about commercial licensing options at 34 * [email protected] 35 * 36 */ 37 38 /* 39 * bnep.c 40 * Author: Ole Reinhardt <[email protected]> 41 * 42 */ 43 44 #include <stdio.h> 45 #include <stdlib.h> 46 #include <string.h> // memcpy 47 #include <stdint.h> 48 49 #include "bnep.h" 50 #include "btstack_debug.h" 51 #include "btstack_event.h" 52 #include "btstack_memory.h" 53 #include "btstack_util.h" 54 #include "classic/core.h" 55 #include "classic/sdp_util.h" 56 #include "hci.h" 57 #include "hci_cmd.h" 58 #include "hci_dump.h" 59 #include "l2cap.h" 60 61 #define BNEP_CONNECTION_TIMEOUT_MS 10000 62 #define BNEP_CONNECTION_MAX_RETRIES 1 63 64 static btstack_linked_list_t bnep_services = NULL; 65 static btstack_linked_list_t bnep_channels = NULL; 66 67 static gap_security_level_t bnep_security_level; 68 69 static void (*app_packet_handler)(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size); 70 71 72 static bnep_channel_t * bnep_channel_for_l2cap_cid(uint16_t l2cap_cid); 73 static void bnep_channel_finalize(bnep_channel_t *channel); 74 static void bnep_run(void); 75 static void bnep_channel_start_timer(bnep_channel_t *channel, int timeout); 76 inline static void bnep_channel_state_add(bnep_channel_t *channel, BNEP_CHANNEL_STATE_VAR event); 77 78 static void bnep_emit_open_channel_complete(bnep_channel_t *channel, uint8_t status) 79 { 80 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); 81 if (!channel->packet_handler) return; 82 83 uint8_t event[3 + sizeof(bd_addr_t) + 4 * sizeof(uint16_t)]; 84 event[0] = BNEP_EVENT_CHANNEL_OPENED; 85 event[1] = sizeof(event) - 2; 86 event[2] = status; 87 little_endian_store_16(event, 3, channel->l2cap_cid); 88 little_endian_store_16(event, 5, channel->uuid_source); 89 little_endian_store_16(event, 7, channel->uuid_dest); 90 little_endian_store_16(event, 9, channel->max_frame_size); 91 bd_addr_copy(&event[11], channel->remote_addr); 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 bd_addr_copy(&event[8], channel->remote_addr); 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 bd_addr_copy(&event[8], channel->remote_addr); 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 reqeuest on next statemachine cycle */ 598 bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_SET); 599 bnep_run(); 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 reqeuest on next statemachine cycle */ 631 bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_SET); 632 bnep_run(); 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 bnep_run(); 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 /* Initialize the channel struct */ 708 memset(channel, 0, sizeof(bnep_channel_t)); 709 710 channel->state = BNEP_CHANNEL_STATE_CLOSED; 711 channel->max_frame_size = bnep_max_frame_size_for_l2cap_mtu(l2cap_max_mtu()); 712 bd_addr_copy(channel->remote_addr, addr); 713 gap_local_bd_addr(channel->local_addr); 714 715 channel->net_filter_count = 0; 716 channel->multicast_filter_count = 0; 717 channel->retry_count = 0; 718 719 /* Finally add it to the channel list */ 720 btstack_linked_list_add(&bnep_channels, (btstack_linked_item_t *) channel); 721 722 return channel; 723 } 724 725 static bnep_channel_t* bnep_channel_for_addr(bd_addr_t addr) 726 { 727 btstack_linked_item_t *it; 728 for (it = (btstack_linked_item_t *) bnep_channels; it ; it = it->next){ 729 bnep_channel_t *channel = ((bnep_channel_t *) it); 730 if (bd_addr_cmp(addr, channel->remote_addr) == 0) { 731 return channel; 732 } 733 } 734 return NULL; 735 } 736 737 static bnep_channel_t * bnep_channel_for_l2cap_cid(uint16_t l2cap_cid) 738 { 739 btstack_linked_item_t *it; 740 for (it = (btstack_linked_item_t *) bnep_channels; it ; it = it->next){ 741 bnep_channel_t *channel = ((bnep_channel_t *) it); 742 if (channel->l2cap_cid == l2cap_cid) { 743 return channel; 744 } 745 } 746 return NULL; 747 } 748 749 static bnep_service_t * bnep_service_for_uuid(uint16_t uuid) 750 { 751 btstack_linked_item_t *it; 752 for (it = (btstack_linked_item_t *) bnep_services; it ; it = it->next){ 753 bnep_service_t * service = ((bnep_service_t *) it); 754 if ( service->service_uuid == uuid){ 755 return service; 756 } 757 } 758 return NULL; 759 } 760 761 static void bnep_channel_free(bnep_channel_t *channel) 762 { 763 btstack_linked_list_remove( &bnep_channels, (btstack_linked_item_t *) channel); 764 btstack_memory_bnep_channel_free(channel); 765 } 766 767 static void bnep_channel_finalize(bnep_channel_t *channel) 768 { 769 uint16_t l2cap_cid; 770 771 /* Inform application about closed channel */ 772 if (channel->state == BNEP_CHANNEL_STATE_CONNECTED) { 773 bnep_emit_channel_closed(channel); 774 } 775 776 l2cap_cid = channel->l2cap_cid; 777 778 /* Stop any eventually running timer */ 779 bnep_channel_stop_timer(channel); 780 781 /* Free ressources and then close the l2cap channel */ 782 bnep_channel_free(channel); 783 l2cap_disconnect(l2cap_cid, 0x13); 784 } 785 786 static int bnep_handle_connection_request(bnep_channel_t *channel, uint8_t *packet, uint16_t size) 787 { 788 uint16_t uuid_size; 789 uint16_t uuid_offset; 790 uuid_size = packet[1]; 791 uint16_t response_code = BNEP_RESP_SETUP_SUCCESS; 792 bnep_service_t * service; 793 794 /* Sanity check packet size */ 795 if (size < 1 + 1 + 2 * uuid_size) { 796 return 0; 797 } 798 799 if ((channel->state != BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST) && 800 (channel->state != BNEP_CHANNEL_STATE_CONNECTED)) { 801 /* Ignore a connection request if not waiting for or still connected */ 802 log_error("BNEP_CONNECTION_REQUEST: ignored in state %d, l2cap_cid: %d!", channel->state, channel->l2cap_cid); 803 return 0; 804 } 805 806 /* Extract source and destination UUID and convert them to UUID16 format */ 807 switch (uuid_size) { 808 case 2: /* UUID16 */ 809 uuid_offset = 0; 810 break; 811 case 4: /* UUID32 */ 812 case 16: /* UUID128 */ 813 uuid_offset = 2; 814 break; 815 default: 816 log_error("BNEP_CONNECTION_REQUEST: Invalid UUID size %d, l2cap_cid: %d!", channel->state, channel->l2cap_cid); 817 response_code = BNEP_RESP_SETUP_INVALID_SERVICE_UUID_SIZE; 818 break; 819 } 820 821 /* Check source and destination UUIDs for valid combinations */ 822 if (response_code == BNEP_RESP_SETUP_SUCCESS) { 823 channel->uuid_dest = big_endian_read_16(packet, 2 + uuid_offset); 824 channel->uuid_source = big_endian_read_16(packet, 2 + uuid_offset + uuid_size); 825 826 if ((channel->uuid_dest != SDP_PANU) && 827 (channel->uuid_dest != SDP_NAP) && 828 (channel->uuid_dest != SDP_GN)) { 829 log_error("BNEP_CONNECTION_REQUEST: Invalid destination service UUID: %04x", channel->uuid_dest); 830 channel->uuid_dest = 0; 831 } 832 if ((channel->uuid_source != SDP_PANU) && 833 (channel->uuid_source != SDP_NAP) && 834 (channel->uuid_source != SDP_GN)) { 835 log_error("BNEP_CONNECTION_REQUEST: Invalid source service UUID: %04x", channel->uuid_source); 836 channel->uuid_source = 0; 837 } 838 839 /* Check if we have registered a service for the requested destination UUID */ 840 service = bnep_service_for_uuid(channel->uuid_dest); 841 if (service == NULL) { 842 response_code = BNEP_RESP_SETUP_INVALID_DEST_UUID; 843 } else { 844 // use packet handler for service 845 channel->packet_handler = service->packet_handler; 846 847 if ((channel->uuid_source != SDP_PANU) && (channel->uuid_dest != SDP_PANU)) { 848 response_code = BNEP_RESP_SETUP_INVALID_SOURCE_UUID; 849 } 850 } 851 } 852 853 /* Set flag to send out the connection response on next statemachine cycle */ 854 bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_RESPONSE); 855 channel->response_code = response_code; 856 857 /* Return the number of processed package bytes = BNEP Type, BNEP Control Type, UUID-Size + 2 * UUID */ 858 return 1 + 1 + 2 * uuid_size; 859 } 860 861 static int bnep_handle_connection_response(bnep_channel_t *channel, uint8_t *packet, uint16_t size) 862 { 863 uint16_t response_code; 864 865 /* Sanity check packet size */ 866 if (size < 1 + 2) { 867 return 0; 868 } 869 870 if (channel->state != BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_RESPONSE) { 871 /* Ignore a connection response in any state but WAIT_FOR_CONNECTION_RESPONSE */ 872 log_error("BNEP_CONNECTION_RESPONSE: Ignored in channel state %d", channel->state); 873 return 1 + 2; 874 } 875 876 response_code = big_endian_read_16(packet, 1); 877 878 if (response_code == BNEP_RESP_SETUP_SUCCESS) { 879 log_info("BNEP_CONNECTION_RESPONSE: Channel established to %s", bd_addr_to_str(channel->remote_addr)); 880 channel->state = BNEP_CHANNEL_STATE_CONNECTED; 881 /* Stop timeout timer! */ 882 bnep_channel_stop_timer(channel); 883 bnep_emit_open_channel_complete(channel, 0); 884 } else { 885 log_error("BNEP_CONNECTION_RESPONSE: Connection to %s failed. Err: %d", bd_addr_to_str(channel->remote_addr), response_code); 886 bnep_channel_finalize(channel); 887 } 888 return 1 + 2; 889 } 890 891 static int bnep_can_handle_extensions(bnep_channel_t * channel){ 892 /* Extension are primarily handled in CONNECTED state */ 893 if (channel->state == BNEP_CHANNEL_STATE_CONNECTED) return 1; 894 /* and if we've received connection request, but haven't sent the reponse yet. */ 895 if ((channel->state == BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST) && 896 (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_RESPONSE)) { 897 return 1; 898 } 899 return 0; 900 } 901 902 static int bnep_handle_filter_net_type_set(bnep_channel_t *channel, uint8_t *packet, uint16_t size) 903 { 904 uint16_t list_length; 905 uint16_t response_code = BNEP_RESP_FILTER_SUCCESS; 906 907 /* Sanity check packet size */ 908 if (size < 3) { 909 return 0; 910 } 911 912 list_length = big_endian_read_16(packet, 1); 913 /* Sanity check packet size again with known package size */ 914 if (size < 3 + list_length) { 915 return 0; 916 } 917 918 if (!bnep_can_handle_extensions(channel)){ 919 log_error("BNEP_FILTER_NET_TYPE_SET: Ignored in channel state %d", channel->state); 920 return 3 + list_length; 921 } 922 923 /* Check if we have enough space for more filters */ 924 if ((list_length / (2*2)) > MAX_BNEP_NETFILTER) { 925 log_info("BNEP_FILTER_NET_TYPE_SET: Too many filter"); 926 response_code = BNEP_RESP_FILTER_ERR_TOO_MANY_FILTERS; 927 } else { 928 int i; 929 channel->net_filter_count = 0; 930 /* There is still enough space, copy the filters to our filter list */ 931 /* There is still enough space, copy the filters to our filter list */ 932 for (i = 0; i < list_length / (2 * 2); i ++) { 933 channel->net_filter[channel->net_filter_count].range_start = big_endian_read_16(packet, 1 + 2 + i * 4); 934 channel->net_filter[channel->net_filter_count].range_end = big_endian_read_16(packet, 1 + 2 + i * 4 + 2); 935 if (channel->net_filter[channel->net_filter_count].range_start > channel->net_filter[channel->net_filter_count].range_end) { 936 /* Invalid filter range, ignore this filter rule */ 937 log_error("BNEP_FILTER_NET_TYPE_SET: Invalid filter: start: %d, end: %d", 938 channel->net_filter[channel->net_filter_count].range_start, 939 channel->net_filter[channel->net_filter_count].range_end); 940 response_code = BNEP_RESP_FILTER_ERR_INVALID_RANGE; 941 } else { 942 /* Valid filter, increase the filter count */ 943 log_info("BNEP_FILTER_NET_TYPE_SET: Add filter: start: %d, end: %d", 944 channel->net_filter[channel->net_filter_count].range_start, 945 channel->net_filter[channel->net_filter_count].range_end); 946 channel->net_filter_count ++; 947 } 948 } 949 } 950 951 /* Set flag to send out the set net filter response on next statemachine cycle */ 952 bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_RESPONSE); 953 channel->response_code = response_code; 954 955 return 3 + list_length; 956 } 957 958 static int bnep_handle_filter_net_type_response(bnep_channel_t *channel, uint8_t *packet, uint16_t size) 959 { 960 uint16_t response_code; 961 962 // TODO: Currently we do not support setting a network filter. 963 964 /* Sanity check packet size */ 965 if (size < 1 + 2) { 966 return 0; 967 } 968 969 if (!bnep_can_handle_extensions(channel)){ 970 log_error("BNEP_FILTER_NET_TYPE_RESPONSE: Ignored in channel state %d", channel->state); 971 return 1 + 2; 972 } 973 974 response_code = big_endian_read_16(packet, 1); 975 976 if (response_code == BNEP_RESP_FILTER_SUCCESS) { 977 log_info("BNEP_FILTER_NET_TYPE_RESPONSE: Net filter set successfully for %s", bd_addr_to_str(channel->remote_addr)); 978 } else { 979 log_error("BNEP_FILTER_NET_TYPE_RESPONSE: Net filter setting for %s failed. Err: %d", bd_addr_to_str(channel->remote_addr), response_code); 980 } 981 982 return 1 + 2; 983 } 984 985 static int bnep_handle_multi_addr_set(bnep_channel_t *channel, uint8_t *packet, uint16_t size) 986 { 987 uint16_t list_length; 988 uint16_t response_code = BNEP_RESP_FILTER_SUCCESS; 989 990 /* Sanity check packet size */ 991 if (size < 3) { 992 return 0; 993 } 994 995 list_length = big_endian_read_16(packet, 1); 996 /* Sanity check packet size again with known package size */ 997 if (size < 3 + list_length) { 998 return 0; 999 } 1000 1001 if (!bnep_can_handle_extensions(channel)){ 1002 log_error("BNEP_MULTI_ADDR_SET: Ignored in channel state %d", channel->state); 1003 return 3 + list_length; 1004 } 1005 1006 /* Check if we have enough space for more filters */ 1007 if ((list_length / (2 * ETHER_ADDR_LEN)) > MAX_BNEP_MULTICAST_FILTER) { 1008 log_info("BNEP_MULTI_ADDR_SET: Too many filter"); 1009 response_code = BNEP_RESP_FILTER_ERR_TOO_MANY_FILTERS; 1010 } else { 1011 unsigned int i; 1012 channel->multicast_filter_count = 0; 1013 /* There is enough space, copy the filters to our filter list */ 1014 for (i = 0; i < list_length / (2 * ETHER_ADDR_LEN); i ++) { 1015 bd_addr_copy(channel->multicast_filter[channel->multicast_filter_count].addr_start, packet + 1 + 2 + i * ETHER_ADDR_LEN * 2); 1016 bd_addr_copy(channel->multicast_filter[channel->multicast_filter_count].addr_end, packet + 1 + 2 + i * ETHER_ADDR_LEN * 2 + ETHER_ADDR_LEN); 1017 1018 if (memcmp(channel->multicast_filter[channel->multicast_filter_count].addr_start, 1019 channel->multicast_filter[channel->multicast_filter_count].addr_end, ETHER_ADDR_LEN) > 0) { 1020 /* Invalid filter range, ignore this filter rule */ 1021 log_error("BNEP_MULTI_ADDR_SET: Invalid filter: start: %s", 1022 bd_addr_to_str(channel->multicast_filter[channel->multicast_filter_count].addr_start)); 1023 log_error("BNEP_MULTI_ADDR_SET: Invalid filter: end: %s", 1024 bd_addr_to_str(channel->multicast_filter[channel->multicast_filter_count].addr_end)); 1025 response_code = BNEP_RESP_FILTER_ERR_INVALID_RANGE; 1026 } else { 1027 /* Valid filter, increase the filter count */ 1028 log_info("BNEP_MULTI_ADDR_SET: Add filter: start: %s", 1029 bd_addr_to_str(channel->multicast_filter[channel->multicast_filter_count].addr_start)); 1030 log_info("BNEP_MULTI_ADDR_SET: Add filter: end: %s", 1031 bd_addr_to_str(channel->multicast_filter[channel->multicast_filter_count].addr_end)); 1032 channel->multicast_filter_count ++; 1033 } 1034 } 1035 } 1036 /* Set flag to send out the set multi addr response on next statemachine cycle */ 1037 bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_RESPONSE); 1038 channel->response_code = response_code; 1039 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 (*app_packet_handler)(BNEP_DATA_PACKET, channel->uuid_source, 1092 ethernet_packet, size + sizeof(uint16_t) + 2 * sizeof(bd_addr_t)); 1093 1094 return size; 1095 } 1096 1097 static int bnep_handle_control_packet(bnep_channel_t *channel, uint8_t *packet, uint16_t size, int is_extension) 1098 { 1099 uint16_t len = 0; 1100 uint8_t bnep_control_type; 1101 1102 bnep_control_type = packet[0]; 1103 /* Save last control type. Needed by statemachin in case of unknown control code */ 1104 1105 channel->last_control_type = bnep_control_type; 1106 log_info("BNEP_CONTROL: Type: %d, size: %d, is_extension: %d", bnep_control_type, size, is_extension); 1107 switch (bnep_control_type) { 1108 case BNEP_CONTROL_TYPE_COMMAND_NOT_UNDERSTOOD: 1109 /* The last command we send was not understood. We should close the connection */ 1110 log_error("BNEP_CONTROL: Received COMMAND_NOT_UNDERSTOOD: l2cap_cid: %d, cmd: %d", channel->l2cap_cid, packet[3]); 1111 bnep_channel_finalize(channel); 1112 len = 2; // Length of command not understood packet - bnep-type field 1113 break; 1114 case BNEP_CONTROL_TYPE_SETUP_CONNECTION_REQUEST: 1115 if (is_extension) { 1116 /* Connection requests are not allowed to be send in an extension header 1117 * ignore, do not set "COMMAND_NOT_UNDERSTOOD" 1118 */ 1119 log_error("BNEP_CONTROL: Received SETUP_CONNECTION_REQUEST in extension header: l2cap_cid: %d", channel->l2cap_cid); 1120 return 0; 1121 } else { 1122 len = bnep_handle_connection_request(channel, packet, size); 1123 } 1124 break; 1125 case BNEP_CONTROL_TYPE_SETUP_CONNECTION_RESPONSE: 1126 if (is_extension) { 1127 /* Connection requests are not allowed to be send in an 1128 * extension header, ignore, do not set "COMMAND_NOT_UNDERSTOOD" 1129 */ 1130 log_error("BNEP_CONTROL: Received SETUP_CONNECTION_RESPONSE in extension header: l2cap_cid: %d", channel->l2cap_cid); 1131 return 0; 1132 } else { 1133 len = bnep_handle_connection_response(channel, packet, size); 1134 } 1135 break; 1136 case BNEP_CONTROL_TYPE_FILTER_NET_TYPE_SET: 1137 len = bnep_handle_filter_net_type_set(channel, packet, size); 1138 break; 1139 case BNEP_CONTROL_TYPE_FILTER_NET_TYPE_RESPONSE: 1140 len = bnep_handle_filter_net_type_response(channel, packet, size); 1141 break; 1142 case BNEP_CONTROL_TYPE_FILTER_MULTI_ADDR_SET: 1143 len = bnep_handle_multi_addr_set(channel, packet, size); 1144 break; 1145 case BNEP_CONTROL_TYPE_FILTER_MULTI_ADDR_RESPONSE: 1146 len = bnep_handle_multi_addr_response(channel, packet, size); 1147 break; 1148 default: 1149 log_error("BNEP_CONTROL: Invalid bnep control type: l2cap_cid: %d, cmd: %d", channel->l2cap_cid, bnep_control_type); 1150 len = 0; 1151 break; 1152 } 1153 1154 if (len == 0) { 1155 /* In case the command could not be handled, send a 1156 COMMAND_NOT_UNDERSTOOD message. 1157 Set flag to process the request in the next statemachine loop 1158 */ 1159 bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_NOT_UNDERSTOOD); 1160 } 1161 1162 return len; 1163 } 1164 1165 /** 1166 * @return handled packet 1167 */ 1168 static int bnep_hci_event_handler(uint8_t *packet, uint16_t size) 1169 { 1170 bd_addr_t event_addr; 1171 uint16_t psm; 1172 uint16_t l2cap_cid; 1173 hci_con_handle_t con_handle; 1174 bnep_channel_t *channel = NULL; 1175 uint8_t status; 1176 1177 switch (hci_event_packet_get_type(packet)) { 1178 1179 /* Accept an incoming L2CAP connection on PSM_BNEP */ 1180 case L2CAP_EVENT_INCOMING_CONNECTION: 1181 /* L2CAP event data: event(8), len(8), address(48), handle (16), psm (16), source cid(16) dest cid(16) */ 1182 reverse_bd_addr(&packet[2], event_addr); 1183 con_handle = little_endian_read_16(packet, 8); 1184 psm = little_endian_read_16(packet, 10); 1185 l2cap_cid = little_endian_read_16(packet, 12); 1186 1187 if (psm != PSM_BNEP) break; 1188 1189 channel = bnep_channel_for_addr(event_addr); 1190 1191 if (channel) { 1192 log_error("INCOMING_CONNECTION (l2cap_cid 0x%02x) for PSM_BNEP => decline - channel already exists", l2cap_cid); 1193 l2cap_decline_connection(l2cap_cid, 0x04); // no resources available 1194 return 1; 1195 } 1196 1197 /* Create a new BNEP channel instance (incoming) */ 1198 channel = bnep_channel_create_for_addr(event_addr); 1199 1200 if (!channel) { 1201 log_error("INCOMING_CONNECTION (l2cap_cid 0x%02x) for PSM_BNEP => decline - no memory left", l2cap_cid); 1202 l2cap_decline_connection(l2cap_cid, 0x04); // no resources available 1203 return 1; 1204 } 1205 1206 /* Assign connection handle and l2cap cid */ 1207 channel->con_handle = con_handle; 1208 channel->l2cap_cid = l2cap_cid; 1209 1210 /* Set channel into accept state */ 1211 channel->state = BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST; 1212 1213 /* Start connection timeout timer */ 1214 bnep_channel_start_timer(channel, BNEP_CONNECTION_TIMEOUT_MS); 1215 1216 log_info("L2CAP_EVENT_INCOMING_CONNECTION (l2cap_cid 0x%02x) for PSM_BNEP => accept", l2cap_cid); 1217 l2cap_accept_connection(l2cap_cid); 1218 return 1; 1219 1220 /* Outgoing L2CAP connection has been opened -> store l2cap_cid, remote_addr */ 1221 case L2CAP_EVENT_CHANNEL_OPENED: 1222 /* Check if the l2cap channel has been opened for PSM_BNEP */ 1223 if (little_endian_read_16(packet, 11) != PSM_BNEP) { 1224 break; 1225 } 1226 1227 status = packet[2]; 1228 log_info("L2CAP_EVENT_CHANNEL_OPENED for PSM_BNEP, status %u", status); 1229 1230 /* Get the bnep channel fpr remote address */ 1231 con_handle = little_endian_read_16(packet, 9); 1232 l2cap_cid = little_endian_read_16(packet, 13); 1233 reverse_bd_addr(&packet[3], event_addr); 1234 channel = bnep_channel_for_addr(event_addr); 1235 if (!channel) { 1236 log_error("L2CAP_EVENT_CHANNEL_OPENED but no BNEP channel prepared"); 1237 return 1; 1238 } 1239 1240 /* On L2CAP open error discard everything */ 1241 if (status) { 1242 /* Emit bnep_open_channel_complete with status and free channel */ 1243 bnep_emit_open_channel_complete(channel, status); 1244 1245 /* Free BNEP channel mempory */ 1246 bnep_channel_free(channel); 1247 return 1; 1248 } 1249 1250 switch (channel->state){ 1251 case BNEP_CHANNEL_STATE_CLOSED: 1252 log_info("L2CAP_EVENT_CHANNEL_OPENED: outgoing connection"); 1253 1254 bnep_channel_start_timer(channel, BNEP_CONNECTION_TIMEOUT_MS); 1255 1256 /* Assign connection handle and l2cap cid */ 1257 channel->l2cap_cid = l2cap_cid; 1258 channel->con_handle = con_handle; 1259 1260 /* Initiate the connection request */ 1261 channel->state = BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_RESPONSE; 1262 bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_REQUEST); 1263 channel->max_frame_size = bnep_max_frame_size_for_l2cap_mtu(little_endian_read_16(packet, 17)); 1264 bnep_run(); 1265 break; 1266 case BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST: 1267 /* New information: channel mtu */ 1268 channel->max_frame_size = bnep_max_frame_size_for_l2cap_mtu(little_endian_read_16(packet, 17)); 1269 break; 1270 default: 1271 log_error("L2CAP_EVENT_CHANNEL_OPENED: Invalid state: %d", channel->state); 1272 break; 1273 } 1274 return 1; 1275 1276 case L2CAP_EVENT_CAN_SEND_NOW: 1277 bnep_run(); 1278 break; 1279 1280 case L2CAP_EVENT_CHANNEL_CLOSED: 1281 // data: event (8), len(8), channel (16) 1282 l2cap_cid = little_endian_read_16(packet, 2); 1283 channel = bnep_channel_for_l2cap_cid(l2cap_cid); 1284 log_info("L2CAP_EVENT_CHANNEL_CLOSED cid 0x%0x, channel %p", l2cap_cid, channel); 1285 1286 if (!channel) { 1287 break; 1288 } 1289 1290 log_info("L2CAP_EVENT_CHANNEL_CLOSED state %u", channel->state); 1291 switch (channel->state) { 1292 case BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST: 1293 case BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_RESPONSE: 1294 case BNEP_CHANNEL_STATE_CONNECTED: 1295 bnep_channel_finalize(channel); 1296 return 1; 1297 default: 1298 break; 1299 } 1300 break; 1301 default: 1302 bnep_run(); 1303 break; 1304 } 1305 return 0; 1306 } 1307 1308 static int bnep_l2cap_packet_handler(uint16_t l2cap_cid, uint8_t *packet, uint16_t size) 1309 { 1310 int rc = 0; 1311 uint8_t bnep_type; 1312 uint8_t bnep_header_has_ext; 1313 uint8_t extension_type; 1314 uint16_t pos = 0; 1315 bd_addr_t addr_source; 1316 bd_addr_t addr_dest; 1317 uint16_t network_protocol_type = 0xffff; 1318 bnep_channel_t *channel = NULL; 1319 1320 /* Get the bnep channel for this package */ 1321 channel = bnep_channel_for_l2cap_cid(l2cap_cid); 1322 if (!channel) { 1323 return rc; 1324 } 1325 1326 /* Sort out short packages */ 1327 if (size < 2) { 1328 return rc; 1329 } 1330 1331 bnep_type = BNEP_TYPE(packet[pos]); 1332 bnep_header_has_ext = BNEP_HEADER_HAS_EXT(packet[pos]); 1333 pos ++; 1334 1335 switch(bnep_type) { 1336 case BNEP_PKT_TYPE_GENERAL_ETHERNET: 1337 bd_addr_copy(addr_dest, &packet[pos]); 1338 pos += sizeof(bd_addr_t); 1339 bd_addr_copy(addr_source, &packet[pos]); 1340 pos += sizeof(bd_addr_t); 1341 network_protocol_type = big_endian_read_16(packet, pos); 1342 pos += 2; 1343 break; 1344 case BNEP_PKT_TYPE_COMPRESSED_ETHERNET: 1345 bd_addr_copy(addr_dest, channel->local_addr); 1346 bd_addr_copy(addr_source, channel->remote_addr); 1347 network_protocol_type = big_endian_read_16(packet, pos); 1348 pos += 2; 1349 break; 1350 case BNEP_PKT_TYPE_COMPRESSED_ETHERNET_SOURCE_ONLY: 1351 bd_addr_copy(addr_dest, channel->local_addr); 1352 bd_addr_copy(addr_source, &packet[pos]); 1353 pos += sizeof(bd_addr_t); 1354 network_protocol_type = big_endian_read_16(packet, pos); 1355 pos += 2; 1356 break; 1357 case BNEP_PKT_TYPE_COMPRESSED_ETHERNET_DEST_ONLY: 1358 bd_addr_copy(addr_dest, &packet[pos]); 1359 pos += sizeof(bd_addr_t); 1360 bd_addr_copy(addr_source, channel->remote_addr); 1361 network_protocol_type = big_endian_read_16(packet, pos); 1362 pos += 2; 1363 break; 1364 case BNEP_PKT_TYPE_CONTROL: 1365 rc = bnep_handle_control_packet(channel, packet + pos, size - pos, 0); 1366 pos += rc; 1367 break; 1368 default: 1369 break; 1370 } 1371 1372 if (bnep_header_has_ext) { 1373 do { 1374 uint8_t ext_len; 1375 1376 /* Read extension type and check for further extensions */ 1377 extension_type = BNEP_TYPE(packet[pos]); 1378 bnep_header_has_ext = BNEP_HEADER_HAS_EXT(packet[pos]); 1379 pos ++; 1380 1381 /* Read extension header length */ 1382 ext_len = packet[pos]; 1383 pos ++; 1384 1385 if (size - pos < ext_len) { 1386 log_error("BNEP pkt handler: Invalid extension length! Packet ignored"); 1387 /* Invalid packet size! */ 1388 return 0; 1389 } 1390 1391 switch (extension_type) { 1392 case BNEP_EXT_HEADER_TYPE_EXTENSION_CONTROL: 1393 if (ext_len != bnep_handle_control_packet(channel, packet + pos, ext_len, 1)) { 1394 log_error("BNEP pkt handler: Ignore invalid control packet in extension header"); 1395 } 1396 1397 pos += ext_len; 1398 break; 1399 1400 default: 1401 /* Extension header type unknown. Unknown extension SHALL be 1402 * SHALL be forwarded in any way. But who shall handle these 1403 * extension packets? 1404 * For now: We ignore them and just drop them! 1405 */ 1406 log_error("BNEP pkt handler: Unknown extension type ignored, data dropped!"); 1407 pos += ext_len; 1408 break; 1409 } 1410 1411 } while (bnep_header_has_ext); 1412 } 1413 1414 if (bnep_type != BNEP_PKT_TYPE_CONTROL && network_protocol_type != 0xffff) { 1415 if (channel->state == BNEP_CHANNEL_STATE_CONNECTED) { 1416 rc = bnep_handle_ethernet_packet(channel, addr_dest, addr_source, network_protocol_type, packet + pos, size - pos); 1417 } else { 1418 rc = 0; 1419 } 1420 } 1421 1422 return rc; 1423 1424 } 1425 1426 void bnep_packet_handler(uint8_t packet_type, uint16_t l2cap_cid, uint8_t *packet, uint16_t size) 1427 { 1428 switch (packet_type) { 1429 case HCI_EVENT_PACKET: 1430 bnep_hci_event_handler(packet, size); 1431 break; 1432 case L2CAP_DATA_PACKET: 1433 bnep_l2cap_packet_handler(l2cap_cid, packet, size); 1434 break; 1435 default: 1436 break; 1437 } 1438 1439 bnep_run(); 1440 } 1441 1442 static void bnep_channel_state_machine(bnep_channel_t* channel, bnep_channel_event_t *event) 1443 { 1444 log_info("bnep_state_machine: state %u, state var: %02x, event %u", channel->state, channel->state_var, event->type); 1445 1446 if (event->type == BNEP_CH_EVT_READY_TO_SEND) { 1447 /* Send outstanding packets. */ 1448 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_NOT_UNDERSTOOD) { 1449 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_NOT_UNDERSTOOD); 1450 bnep_send_command_not_understood(channel, channel->last_control_type); 1451 return; 1452 } 1453 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_REQUEST) { 1454 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_REQUEST); 1455 channel->state = BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_RESPONSE; 1456 bnep_send_connection_request(channel, channel->uuid_source, channel->uuid_dest); 1457 } 1458 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_RESPONSE) { 1459 int emit_connected = 0; 1460 if ((channel->state == BNEP_CHANNEL_STATE_CLOSED) || 1461 (channel->state == BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST)) { 1462 /* Set channel state to STATE_CONNECTED */ 1463 channel->state = BNEP_CHANNEL_STATE_CONNECTED; 1464 /* Stop timeout timer! */ 1465 bnep_channel_stop_timer(channel); 1466 emit_connected = 1; 1467 } 1468 1469 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_RESPONSE); 1470 bnep_send_connection_response(channel, channel->response_code); 1471 if (emit_connected){ 1472 bnep_emit_open_channel_complete(channel, 0); 1473 } 1474 return; 1475 } 1476 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_SET) { 1477 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_SET); 1478 if ((channel->net_filter_out_count > 0) && (channel->net_filter_out != NULL)) { 1479 bnep_send_filter_net_type_set(channel, channel->net_filter_out, channel->net_filter_out_count); 1480 channel->net_filter_out_count = 0; 1481 channel->net_filter_out = NULL; 1482 } 1483 return; 1484 } 1485 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_RESPONSE) { 1486 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_RESPONSE); 1487 bnep_send_filter_net_type_response(channel, channel->response_code); 1488 return; 1489 } 1490 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_SET) { 1491 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_SET); 1492 if ((channel->multicast_filter_out_count > 0) && (channel->multicast_filter_out != NULL)) { 1493 bnep_send_filter_multi_addr_set(channel, channel->multicast_filter_out, channel->multicast_filter_out_count); 1494 channel->multicast_filter_out_count = 0; 1495 channel->multicast_filter_out = NULL; 1496 } 1497 return; 1498 } 1499 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_RESPONSE) { 1500 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_RESPONSE); 1501 bnep_send_filter_multi_addr_response(channel, channel->response_code); 1502 return; 1503 } 1504 1505 /* If the event was not yet handled, notify the application layer */ 1506 if (channel->waiting_for_can_send_now){ 1507 channel->waiting_for_can_send_now = 0; 1508 bnep_emit_ready_to_send(channel); 1509 } 1510 } 1511 } 1512 1513 1514 /* Process oustanding signaling tasks */ 1515 static void bnep_run(void) 1516 { 1517 btstack_linked_item_t *it; 1518 btstack_linked_item_t *next; 1519 1520 for (it = (btstack_linked_item_t *) bnep_channels; it ; it = next){ 1521 1522 next = it->next; // be prepared for removal of channel in state machine 1523 1524 bnep_channel_t * channel = ((bnep_channel_t *) it); 1525 1526 if (!l2cap_can_send_packet_now(channel->l2cap_cid)) { 1527 continue; 1528 } 1529 1530 bnep_channel_event_t channel_event = { BNEP_CH_EVT_READY_TO_SEND }; 1531 bnep_channel_state_machine(channel, &channel_event); 1532 } 1533 } 1534 1535 /* BNEP BTStack API */ 1536 void bnep_init(void) 1537 { 1538 bnep_security_level = LEVEL_0; 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 bnep_run(); 1577 } 1578 1579 1580 uint8_t bnep_register_service(btstack_packet_handler_t packet_handler, uint16_t service_uuid, uint16_t max_frame_size) 1581 { 1582 log_info("BNEP_REGISTER_SERVICE mtu %d", max_frame_size); 1583 1584 /* Check if we already registered a service */ 1585 bnep_service_t * service = bnep_service_for_uuid(service_uuid); 1586 if (service) { 1587 return BNEP_SERVICE_ALREADY_REGISTERED; 1588 } 1589 1590 /* Only alow one the three service types: PANU, NAP, GN */ 1591 if ((service_uuid != SDP_PANU) && 1592 (service_uuid != SDP_NAP) && 1593 (service_uuid != SDP_GN)) { 1594 log_info("BNEP_REGISTER_SERVICE: Invalid service UUID: %04x", service_uuid); 1595 return BNEP_SERVICE_ALREADY_REGISTERED; // TODO: define own error 1596 } 1597 1598 /* Allocate service memory */ 1599 service = (bnep_service_t*) btstack_memory_bnep_service_get(); 1600 if (!service) { 1601 return BTSTACK_MEMORY_ALLOC_FAILED; 1602 } 1603 memset(service, 0, sizeof(bnep_service_t)); 1604 1605 /* register with l2cap if not registered before, max MTU */ 1606 l2cap_register_service(bnep_packet_handler, PSM_BNEP, 0xffff, bnep_security_level); 1607 1608 /* Setup the service struct */ 1609 service->max_frame_size = max_frame_size; 1610 service->service_uuid = service_uuid; 1611 service->packet_handler = packet_handler; 1612 1613 1614 /* Add to services list */ 1615 btstack_linked_list_add(&bnep_services, (btstack_linked_item_t *) service); 1616 1617 return 0; 1618 } 1619 1620 void bnep_unregister_service(uint16_t service_uuid) 1621 { 1622 log_info("BNEP_UNREGISTER_SERVICE #%04x", service_uuid); 1623 1624 bnep_service_t *service = bnep_service_for_uuid(service_uuid); 1625 if (!service) { 1626 return; 1627 } 1628 1629 btstack_linked_list_remove(&bnep_services, (btstack_linked_item_t *) service); 1630 btstack_memory_bnep_service_free(service); 1631 service = NULL; 1632 1633 l2cap_unregister_service(PSM_BNEP); 1634 } 1635 1636