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