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