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