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