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 "hci_cmds.h" 50 #include "utils.h" 51 #include "classic/sdp_util.h" 52 53 #include "btstack_memory.h" 54 #include "hci.h" 55 #include "hci_dump.h" 56 #include "debug.h" 57 #include "bnep.h" 58 59 #include "l2cap.h" 60 61 #define BNEP_CONNECTION_TIMEOUT_MS 10000 62 #define BNEP_CONNECTION_MAX_RETRIES 1 63 64 static linked_list_t bnep_services = NULL; 65 static 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_OPEN_CHANNEL_COMPLETE status 0x%02x bd_addr: %s", status, bd_addr_to_str(channel->remote_addr)); 81 uint8_t event[3 + sizeof(bd_addr_t) + 3 * sizeof(uint16_t)]; 82 event[0] = BNEP_EVENT_OPEN_CHANNEL_COMPLETE; 83 event[1] = sizeof(event) - 2; 84 event[2] = status; 85 bt_store_16(event, 3, channel->uuid_source); 86 bt_store_16(event, 5, channel->uuid_dest); 87 bt_store_16(event, 7, channel->max_frame_size); 88 BD_ADDR_COPY(&event[9], channel->remote_addr); 89 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 90 (*app_packet_handler)(HCI_EVENT_PACKET, channel->l2cap_cid, (uint8_t *) event, sizeof(event)); 91 } 92 93 static void bnep_emit_channel_timeout(bnep_channel_t *channel) 94 { 95 log_info("BNEP_EVENT_CHANNEL_TIMEOUT bd_addr: %s", bd_addr_to_str(channel->remote_addr)); 96 uint8_t event[2 + sizeof(bd_addr_t) + 2 * sizeof(uint16_t) + sizeof(uint8_t)]; 97 event[0] = BNEP_EVENT_CHANNEL_TIMEOUT; 98 event[1] = sizeof(event) - 2; 99 bt_store_16(event, 2, channel->uuid_source); 100 bt_store_16(event, 4, channel->uuid_dest); 101 BD_ADDR_COPY(&event[6], channel->remote_addr); 102 event[12] = channel->state; 103 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 104 (*app_packet_handler)(HCI_EVENT_PACKET, channel->l2cap_cid, (uint8_t *) event, sizeof(event)); 105 } 106 107 static void bnep_emit_channel_closed(bnep_channel_t *channel) 108 { 109 log_info("BNEP_EVENT_CHANNEL_CLOSED bd_addr: %s", bd_addr_to_str(channel->remote_addr)); 110 uint8_t event[2 + sizeof(bd_addr_t) + 2 * sizeof(uint16_t)]; 111 event[0] = BNEP_EVENT_CHANNEL_CLOSED; 112 event[1] = sizeof(event) - 2; 113 bt_store_16(event, 2, channel->uuid_source); 114 bt_store_16(event, 4, channel->uuid_dest); 115 BD_ADDR_COPY(&event[6], channel->remote_addr); 116 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 117 (*app_packet_handler)(HCI_EVENT_PACKET, channel->l2cap_cid, (uint8_t *) event, sizeof(event)); 118 } 119 120 static void bnep_emit_ready_to_send(bnep_channel_t *channel) 121 { 122 uint8_t event[2]; 123 event[0] = BNEP_EVENT_READY_TO_SEND; 124 event[1] = sizeof(event) - 2; 125 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 126 (*app_packet_handler)(HCI_EVENT_PACKET, channel->l2cap_cid, (uint8_t *) event, sizeof(event)); 127 } 128 129 /* Send BNEP connection request */ 130 static int bnep_send_command_not_understood(bnep_channel_t *channel, uint8_t control_type) 131 { 132 uint8_t *bnep_out_buffer = NULL; 133 uint16_t pos = 0; 134 int err = 0; 135 136 if (channel->state == BNEP_CHANNEL_STATE_CLOSED) { 137 return -1; // TODO 138 } 139 140 l2cap_reserve_packet_buffer(); 141 bnep_out_buffer = l2cap_get_outgoing_buffer(); 142 143 /* Setup control packet type */ 144 bnep_out_buffer[pos++] = BNEP_PKT_TYPE_CONTROL; 145 bnep_out_buffer[pos++] = BNEP_CONTROL_TYPE_COMMAND_NOT_UNDERSTOOD; 146 147 /* Add not understood control type */ 148 bnep_out_buffer[pos++] = control_type; 149 150 err = l2cap_send_prepared(channel->l2cap_cid, pos); 151 152 if (err) { 153 // TODO: Log error 154 } 155 return err; 156 } 157 158 159 /* Send BNEP connection request */ 160 static int bnep_send_connection_request(bnep_channel_t *channel, uint16_t uuid_source, uint16_t uuid_dest) 161 { 162 uint8_t *bnep_out_buffer = NULL; 163 uint16_t pos = 0; 164 int err = 0; 165 166 if (channel->state == BNEP_CHANNEL_STATE_CLOSED) { 167 return -1; // TODO 168 } 169 170 l2cap_reserve_packet_buffer(); 171 bnep_out_buffer = l2cap_get_outgoing_buffer(); 172 173 /* Setup control packet type */ 174 bnep_out_buffer[pos++] = BNEP_PKT_TYPE_CONTROL; 175 bnep_out_buffer[pos++] = BNEP_CONTROL_TYPE_SETUP_CONNECTION_REQUEST; 176 177 /* Add UUID Size */ 178 bnep_out_buffer[pos++] = 2; 179 180 /* Add dest and source UUID */ 181 net_store_16(bnep_out_buffer, pos, uuid_dest); 182 pos += 2; 183 184 net_store_16(bnep_out_buffer, pos, uuid_source); 185 pos += 2; 186 187 err = l2cap_send_prepared(channel->l2cap_cid, pos); 188 189 if (err) { 190 // TODO: Log error 191 } 192 return err; 193 } 194 195 /* Send BNEP connection response */ 196 static int bnep_send_connection_response(bnep_channel_t *channel, uint16_t response_code) 197 { 198 uint8_t *bnep_out_buffer = NULL; 199 uint16_t pos = 0; 200 int err = 0; 201 202 if (channel->state == BNEP_CHANNEL_STATE_CLOSED) { 203 return -1; // TODO 204 } 205 206 l2cap_reserve_packet_buffer(); 207 bnep_out_buffer = l2cap_get_outgoing_buffer(); 208 209 /* Setup control packet type */ 210 bnep_out_buffer[pos++] = BNEP_PKT_TYPE_CONTROL; 211 bnep_out_buffer[pos++] = BNEP_CONTROL_TYPE_SETUP_CONNECTION_RESPONSE; 212 213 /* Add response code */ 214 net_store_16(bnep_out_buffer, pos, response_code); 215 pos += 2; 216 217 err = l2cap_send_prepared(channel->l2cap_cid, pos); 218 219 if (err) { 220 // TODO: Log error 221 } 222 return err; 223 } 224 225 /* Send BNEP filter net type set message */ 226 static int bnep_send_filter_net_type_set(bnep_channel_t *channel, bnep_net_filter_t *filter, uint16_t len) 227 { 228 uint8_t *bnep_out_buffer = NULL; 229 uint16_t pos = 0; 230 int err = 0; 231 int i; 232 233 if (channel->state == BNEP_CHANNEL_STATE_CLOSED) { 234 return -1; 235 } 236 237 l2cap_reserve_packet_buffer(); 238 bnep_out_buffer = l2cap_get_outgoing_buffer(); 239 240 /* Setup control packet type */ 241 bnep_out_buffer[pos++] = BNEP_PKT_TYPE_CONTROL; 242 bnep_out_buffer[pos++] = BNEP_CONTROL_TYPE_FILTER_NET_TYPE_SET; 243 244 net_store_16(bnep_out_buffer, pos, len * 2 * 2); 245 pos += 2; 246 247 for (i = 0; i < len; i ++) { 248 net_store_16(bnep_out_buffer, pos, filter[i].range_start); 249 pos += 2; 250 net_store_16(bnep_out_buffer, pos, filter[i].range_end); 251 pos += 2; 252 } 253 254 err = l2cap_send_prepared(channel->l2cap_cid, pos); 255 256 if (err) { 257 // TODO: Log error 258 } 259 return err; 260 } 261 262 /* Send BNEP filter net type response message */ 263 static int bnep_send_filter_net_type_response(bnep_channel_t *channel, uint16_t response_code) 264 { 265 uint8_t *bnep_out_buffer = NULL; 266 uint16_t pos = 0; 267 int err = 0; 268 269 if (channel->state == BNEP_CHANNEL_STATE_CLOSED) { 270 return -1; 271 } 272 273 l2cap_reserve_packet_buffer(); 274 bnep_out_buffer = l2cap_get_outgoing_buffer(); 275 276 /* Setup control packet type */ 277 bnep_out_buffer[pos++] = BNEP_PKT_TYPE_CONTROL; 278 bnep_out_buffer[pos++] = BNEP_CONTROL_TYPE_FILTER_NET_TYPE_RESPONSE; 279 280 /* Add response code */ 281 net_store_16(bnep_out_buffer, pos, response_code); 282 pos += 2; 283 284 err = l2cap_send_prepared(channel->l2cap_cid, pos); 285 286 if (err) { 287 // TODO: Log error 288 } 289 return err; 290 } 291 292 /* Send BNEP filter multicast address set message */ 293 294 static int bnep_send_filter_multi_addr_set(bnep_channel_t *channel, bnep_multi_filter_t *filter, uint16_t len) 295 { 296 uint8_t *bnep_out_buffer = NULL; 297 uint16_t pos = 0; 298 int err = 0; 299 int i; 300 301 if (channel->state == BNEP_CHANNEL_STATE_CLOSED) { 302 return -1; 303 } 304 305 l2cap_reserve_packet_buffer(); 306 bnep_out_buffer = l2cap_get_outgoing_buffer(); 307 308 /* Setup control packet type */ 309 bnep_out_buffer[pos++] = BNEP_PKT_TYPE_CONTROL; 310 bnep_out_buffer[pos++] = BNEP_CONTROL_TYPE_FILTER_MULTI_ADDR_SET; 311 312 net_store_16(bnep_out_buffer, pos, len * 2 * ETHER_ADDR_LEN); 313 pos += 2; 314 315 for (i = 0; i < len; i ++) { 316 BD_ADDR_COPY(bnep_out_buffer + pos, filter[i].addr_start); 317 pos += ETHER_ADDR_LEN; 318 BD_ADDR_COPY(bnep_out_buffer + pos, filter[i].addr_end); 319 pos += ETHER_ADDR_LEN; 320 } 321 322 err = l2cap_send_prepared(channel->l2cap_cid, pos); 323 324 if (err) { 325 // TODO: Log error 326 } 327 return err; 328 } 329 330 /* Send BNEP filter multicast address response message */ 331 static int bnep_send_filter_multi_addr_response(bnep_channel_t *channel, uint16_t response_code) 332 { 333 uint8_t *bnep_out_buffer = NULL; 334 uint16_t pos = 0; 335 int err = 0; 336 337 if (channel->state == BNEP_CHANNEL_STATE_CLOSED) { 338 return -1; 339 } 340 341 l2cap_reserve_packet_buffer(); 342 bnep_out_buffer = l2cap_get_outgoing_buffer(); 343 344 /* Setup control packet type */ 345 bnep_out_buffer[pos++] = BNEP_PKT_TYPE_CONTROL; 346 bnep_out_buffer[pos++] = BNEP_CONTROL_TYPE_FILTER_MULTI_ADDR_RESPONSE; 347 348 /* Add response code */ 349 net_store_16(bnep_out_buffer, pos, response_code); 350 pos += 2; 351 352 err = l2cap_send_prepared(channel->l2cap_cid, pos); 353 354 if (err) { 355 // TODO: Log error 356 } 357 return err; 358 } 359 360 int bnep_can_send_packet_now(uint16_t bnep_cid) 361 { 362 bnep_channel_t *channel = bnep_channel_for_l2cap_cid(bnep_cid); 363 364 if (!channel){ 365 log_error("bnep_can_send_packet_now cid 0x%02x doesn't exist!", bnep_cid); 366 return 0; 367 } 368 369 return l2cap_can_send_packet_now(channel->l2cap_cid); 370 } 371 372 373 static int bnep_filter_protocol(bnep_channel_t *channel, uint16_t network_protocol_type) 374 { 375 int i; 376 377 if (channel->net_filter_count == 0) { 378 /* No filter set */ 379 return 1; 380 } 381 382 for (i = 0; i < channel->net_filter_count; i ++) { 383 if ((network_protocol_type >= channel->net_filter[i].range_start) && 384 (network_protocol_type <= channel->net_filter[i].range_end)) { 385 return 1; 386 } 387 } 388 389 return 0; 390 } 391 392 static int bnep_filter_multicast(bnep_channel_t *channel, bd_addr_t addr_dest) 393 { 394 int i; 395 396 /* Check if the multicast flag is set int the destination address */ 397 if ((addr_dest[0] & 0x01) == 0x00) { 398 /* Not a multicast frame, do not apply filtering and send it in any case */ 399 return 1; 400 } 401 402 if (channel->multicast_filter_count == 0) { 403 /* No filter set */ 404 return 1; 405 } 406 407 for (i = 0; i < channel->multicast_filter_count; i ++) { 408 if ((memcmp(addr_dest, channel->multicast_filter[i].addr_start, sizeof(bd_addr_t)) >= 0) && 409 (memcmp(addr_dest, channel->multicast_filter[i].addr_end, sizeof(bd_addr_t)) <= 0)) { 410 return 1; 411 } 412 } 413 414 return 0; 415 } 416 417 418 /* Send BNEP ethernet packet */ 419 int bnep_send(uint16_t bnep_cid, uint8_t *packet, uint16_t len) 420 { 421 bnep_channel_t *channel; 422 uint8_t *bnep_out_buffer = NULL; 423 uint16_t pos = 0; 424 uint16_t pos_out = 0; 425 uint16_t payload_len; 426 int err = 0; 427 int has_source; 428 int has_dest; 429 430 bd_addr_t addr_dest; 431 bd_addr_t addr_source; 432 uint16_t network_protocol_type; 433 434 channel = bnep_channel_for_l2cap_cid(bnep_cid); 435 if (channel == NULL) { 436 log_error("bnep_send cid 0x%02x doesn't exist!", bnep_cid); 437 return 1; 438 } 439 440 if (channel->state != BNEP_CHANNEL_STATE_CONNECTED) { 441 return BNEP_CHANNEL_NOT_CONNECTED; 442 } 443 444 /* Check for free ACL buffers */ 445 if (!l2cap_can_send_packet_now(channel->l2cap_cid)) { 446 return BTSTACK_ACL_BUFFERS_FULL; 447 } 448 449 /* Extract destination and source address from the ethernet packet */ 450 pos = 0; 451 BD_ADDR_COPY(addr_dest, &packet[pos]); 452 pos += sizeof(bd_addr_t); 453 BD_ADDR_COPY(addr_source, &packet[pos]); 454 pos += sizeof(bd_addr_t); 455 network_protocol_type = READ_NET_16(packet, pos); 456 pos += sizeof(uint16_t); 457 458 payload_len = len - pos; 459 460 if (network_protocol_type == ETHERTYPE_VLAN) { /* IEEE 802.1Q tag header */ 461 if (payload_len < 4) { 462 /* Omit this packet */ 463 return 0; 464 } 465 /* The "real" network protocol type is 4 bytes ahead in a VLAN packet */ 466 network_protocol_type = READ_NET_16(packet, pos + 2); 467 } 468 469 /* Check network protocol and multicast filters before sending */ 470 if (!bnep_filter_protocol(channel, network_protocol_type) || 471 !bnep_filter_multicast(channel, addr_dest)) { 472 /* Packet did not pass filter... */ 473 if ((network_protocol_type == ETHERTYPE_VLAN) && 474 (payload_len >= 4)) { 475 /* The packet has been tagged as a with IEE 802.1Q tag and has been filtered out. 476 According to the spec the IEE802.1Q tag header shall be sended without ethernet payload. 477 So limit the payload_len to 4. 478 */ 479 payload_len = 4; 480 } else { 481 /* Packet is not tagged with IEE802.1Q header and was filtered out. Omit this packet */ 482 return 0; 483 } 484 } 485 486 /* Reserve l2cap packet buffer */ 487 l2cap_reserve_packet_buffer(); 488 bnep_out_buffer = l2cap_get_outgoing_buffer(); 489 490 /* Check if source address is the same as our local address and if the 491 destination address is the same as the remote addr. Maybe we can use 492 the compressed data format 493 */ 494 has_source = (memcmp(addr_source, channel->local_addr, ETHER_ADDR_LEN) != 0); 495 has_dest = (memcmp(addr_dest, channel->remote_addr, ETHER_ADDR_LEN) != 0); 496 497 /* Check for MTU limits */ 498 if (payload_len > channel->max_frame_size) { 499 log_error("bnep_send: Max frame size (%d) exceeded: %d", channel->max_frame_size, payload_len); 500 return BNEP_DATA_LEN_EXCEEDS_MTU; 501 } 502 503 /* Fill in the package type depending on the given source and destination address */ 504 if (has_source && has_dest) { 505 bnep_out_buffer[pos_out++] = BNEP_PKT_TYPE_GENERAL_ETHERNET; 506 } else 507 if (has_source && !has_dest) { 508 bnep_out_buffer[pos_out++] = BNEP_PKT_TYPE_COMPRESSED_ETHERNET_SOURCE_ONLY; 509 } else 510 if (!has_source && has_dest) { 511 bnep_out_buffer[pos_out++] = BNEP_PKT_TYPE_COMPRESSED_ETHERNET_DEST_ONLY; 512 } else { 513 bnep_out_buffer[pos_out++] = BNEP_PKT_TYPE_COMPRESSED_ETHERNET; 514 } 515 516 /* Add the destination address if needed */ 517 if (has_dest) { 518 BD_ADDR_COPY(bnep_out_buffer + pos_out, addr_dest); 519 pos_out += sizeof(bd_addr_t); 520 } 521 522 /* Add the source address if needed */ 523 if (has_source) { 524 BD_ADDR_COPY(bnep_out_buffer + pos_out, addr_source); 525 pos_out += sizeof(bd_addr_t); 526 } 527 528 /* Add protocol type */ 529 net_store_16(bnep_out_buffer, pos_out, network_protocol_type); 530 pos_out += 2; 531 532 /* TODO: Add extension headers, if we may support them at a later stage */ 533 /* Add the payload and then send out the package */ 534 memcpy(bnep_out_buffer + pos_out, packet + pos, payload_len); 535 pos_out += payload_len; 536 537 err = l2cap_send_prepared(channel->l2cap_cid, pos_out); 538 539 if (err) { 540 log_error("bnep_send: error %d", err); 541 } 542 return err; 543 } 544 545 546 /* Set BNEP network protocol type filter */ 547 int bnep_set_net_type_filter(uint16_t bnep_cid, bnep_net_filter_t *filter, uint16_t len) 548 { 549 bnep_channel_t *channel; 550 551 if (filter == NULL) { 552 return -1; 553 } 554 555 channel = bnep_channel_for_l2cap_cid(bnep_cid); 556 if (channel == NULL) { 557 log_error("bnep_set_net_type_filter cid 0x%02x doesn't exist!", bnep_cid); 558 return 1; 559 } 560 561 if (channel->state != BNEP_CHANNEL_STATE_CONNECTED) { 562 return BNEP_CHANNEL_NOT_CONNECTED; 563 } 564 565 if (len > MAX_BNEP_NETFILTER_OUT) { 566 return BNEP_DATA_LEN_EXCEEDS_MTU; 567 } 568 569 channel->net_filter_out = filter; 570 channel->net_filter_out_count = len; 571 572 /* Set flag to send out the network protocol type filter set reqeuest on next statemachine cycle */ 573 bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_SET); 574 bnep_run(); 575 576 return 0; 577 } 578 579 /* Set BNEP network protocol type filter */ 580 int bnep_set_multicast_filter(uint16_t bnep_cid, bnep_multi_filter_t *filter, uint16_t len) 581 { 582 bnep_channel_t *channel; 583 584 if (filter == NULL) { 585 return -1; 586 } 587 588 channel = bnep_channel_for_l2cap_cid(bnep_cid); 589 if (channel == NULL) { 590 log_error("bnep_set_net_type_filter cid 0x%02x doesn't exist!", bnep_cid); 591 return 1; 592 } 593 594 if (channel->state != BNEP_CHANNEL_STATE_CONNECTED) { 595 return BNEP_CHANNEL_NOT_CONNECTED; 596 } 597 598 if (len > MAX_BNEP_MULTICAST_FULTER_OUT) { 599 return BNEP_DATA_LEN_EXCEEDS_MTU; 600 } 601 602 channel->multicast_filter_out = filter; 603 channel->multicast_filter_out_count = len; 604 605 /* Set flag to send out the multicast filter set reqeuest on next statemachine cycle */ 606 bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_SET); 607 bnep_run(); 608 609 return 0; 610 } 611 612 /* BNEP timeout timer helper function */ 613 static void bnep_channel_timer_handler(timer_source_t *timer) 614 { 615 bnep_channel_t *channel = (bnep_channel_t *)linked_item_get_user((linked_item_t *) timer); 616 // retry send setup connection at least one time 617 if (channel->state == BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_RESPONSE){ 618 if (channel->retry_count < BNEP_CONNECTION_MAX_RETRIES){ 619 channel->retry_count++; 620 bnep_channel_start_timer(channel, BNEP_CONNECTION_TIMEOUT_MS); 621 bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_REQUEST); 622 bnep_run(); 623 return; 624 } 625 } 626 627 log_info( "bnep_channel_timeout_handler callback: shutting down connection!"); 628 bnep_emit_channel_timeout(channel); 629 bnep_channel_finalize(channel); 630 } 631 632 633 static void bnep_channel_stop_timer(bnep_channel_t *channel) 634 { 635 if (channel->timer_active) { 636 run_loop_remove_timer(&channel->timer); 637 channel->timer_active = 0; 638 } 639 } 640 641 static void bnep_channel_start_timer(bnep_channel_t *channel, int timeout) 642 { 643 /* Stop any eventually running timeout timer */ 644 bnep_channel_stop_timer(channel); 645 646 /* Start bnep channel timeout check timer */ 647 run_loop_set_timer(&channel->timer, timeout); 648 channel->timer.process = bnep_channel_timer_handler; 649 linked_item_set_user((linked_item_t*) &channel->timer, channel); 650 run_loop_add_timer(&channel->timer); 651 channel->timer_active = 1; 652 } 653 654 /* BNEP statemachine functions */ 655 656 inline static void bnep_channel_state_add(bnep_channel_t *channel, BNEP_CHANNEL_STATE_VAR event){ 657 channel->state_var = (BNEP_CHANNEL_STATE_VAR) (channel->state_var | event); 658 } 659 inline static void bnep_channel_state_remove(bnep_channel_t *channel, BNEP_CHANNEL_STATE_VAR event){ 660 channel->state_var = (BNEP_CHANNEL_STATE_VAR) (channel->state_var & ~event); 661 } 662 663 static uint16_t bnep_max_frame_size_for_l2cap_mtu(uint16_t l2cap_mtu){ 664 665 /* Assume a standard BNEP header, containing BNEP Type (1 Byte), dest and 666 source address (6 bytes each) and networking protocol type (2 bytes) 667 */ 668 uint16_t max_frame_size = l2cap_mtu - 15; // 15 bytes BNEP header 669 670 log_info("bnep_max_frame_size_for_l2cap_mtu: %u -> %u", l2cap_mtu, max_frame_size); 671 return max_frame_size; 672 } 673 674 static bnep_channel_t * bnep_channel_create_for_addr(bd_addr_t addr) 675 { 676 /* Allocate new channel structure */ 677 bnep_channel_t *channel = btstack_memory_bnep_channel_get(); 678 if (!channel) { 679 return NULL; 680 } 681 682 /* Initialize the channel struct */ 683 memset(channel, 0, sizeof(bnep_channel_t)); 684 685 channel->state = BNEP_CHANNEL_STATE_CLOSED; 686 channel->max_frame_size = bnep_max_frame_size_for_l2cap_mtu(l2cap_max_mtu()); 687 BD_ADDR_COPY(&channel->remote_addr, addr); 688 hci_local_bd_addr(channel->local_addr); 689 690 channel->net_filter_count = 0; 691 channel->multicast_filter_count = 0; 692 channel->retry_count = 0; 693 694 /* Finally add it to the channel list */ 695 linked_list_add(&bnep_channels, (linked_item_t *) channel); 696 697 return channel; 698 } 699 700 static bnep_channel_t* bnep_channel_for_addr(bd_addr_t addr) 701 { 702 linked_item_t *it; 703 for (it = (linked_item_t *) bnep_channels; it ; it = it->next){ 704 bnep_channel_t *channel = ((bnep_channel_t *) it); 705 if (BD_ADDR_CMP(addr, channel->remote_addr) == 0) { 706 return channel; 707 } 708 } 709 return NULL; 710 } 711 712 static bnep_channel_t * bnep_channel_for_l2cap_cid(uint16_t l2cap_cid) 713 { 714 linked_item_t *it; 715 for (it = (linked_item_t *) bnep_channels; it ; it = it->next){ 716 bnep_channel_t *channel = ((bnep_channel_t *) it); 717 if (channel->l2cap_cid == l2cap_cid) { 718 return channel; 719 } 720 } 721 return NULL; 722 } 723 724 static bnep_service_t * bnep_service_for_uuid(uint16_t uuid) 725 { 726 linked_item_t *it; 727 for (it = (linked_item_t *) bnep_services; it ; it = it->next){ 728 bnep_service_t * service = ((bnep_service_t *) it); 729 if ( service->service_uuid == uuid){ 730 return service; 731 } 732 } 733 return NULL; 734 } 735 736 static void bnep_channel_free(bnep_channel_t *channel) 737 { 738 linked_list_remove( &bnep_channels, (linked_item_t *) channel); 739 btstack_memory_bnep_channel_free(channel); 740 } 741 742 static void bnep_channel_finalize(bnep_channel_t *channel) 743 { 744 uint16_t l2cap_cid; 745 746 /* Inform application about closed channel */ 747 if (channel->state == BNEP_CHANNEL_STATE_CONNECTED) { 748 bnep_emit_channel_closed(channel); 749 } 750 751 l2cap_cid = channel->l2cap_cid; 752 753 /* Stop any eventually running timer */ 754 bnep_channel_stop_timer(channel); 755 756 /* Free ressources and then close the l2cap channel */ 757 bnep_channel_free(channel); 758 l2cap_disconnect_internal(l2cap_cid, 0x13); 759 } 760 761 static int bnep_handle_connection_request(bnep_channel_t *channel, uint8_t *packet, uint16_t size) 762 { 763 uint16_t uuid_size; 764 uint16_t uuid_offset; 765 uuid_size = packet[1]; 766 uint16_t response_code = BNEP_RESP_SETUP_SUCCESS; 767 bnep_service_t * service; 768 769 /* Sanity check packet size */ 770 if (size < 1 + 1 + 2 * uuid_size) { 771 return 0; 772 } 773 774 if ((channel->state != BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST) && 775 (channel->state != BNEP_CHANNEL_STATE_CONNECTED)) { 776 /* Ignore a connection request if not waiting for or still connected */ 777 log_error("BNEP_CONNECTION_REQUEST: ignored in state %d, l2cap_cid: %d!", channel->state, channel->l2cap_cid); 778 return 0; 779 } 780 781 /* Extract source and destination UUID and convert them to UUID16 format */ 782 switch (uuid_size) { 783 case 2: /* UUID16 */ 784 uuid_offset = 0; 785 break; 786 case 4: /* UUID32 */ 787 case 16: /* UUID128 */ 788 uuid_offset = 2; 789 break; 790 default: 791 log_error("BNEP_CONNECTION_REQUEST: Invalid UUID size %d, l2cap_cid: %d!", channel->state, channel->l2cap_cid); 792 response_code = BNEP_RESP_SETUP_INVALID_SERVICE_UUID_SIZE; 793 break; 794 } 795 796 /* Check source and destination UUIDs for valid combinations */ 797 if (response_code == BNEP_RESP_SETUP_SUCCESS) { 798 channel->uuid_dest = READ_NET_16(packet, 2 + uuid_offset); 799 channel->uuid_source = READ_NET_16(packet, 2 + uuid_offset + uuid_size); 800 801 if ((channel->uuid_dest != SDP_PANU) && 802 (channel->uuid_dest != SDP_NAP) && 803 (channel->uuid_dest != SDP_GN)) { 804 log_error("BNEP_CONNECTION_REQUEST: Invalid destination service UUID: %04x", channel->uuid_dest); 805 channel->uuid_dest = 0; 806 } 807 if ((channel->uuid_source != SDP_PANU) && 808 (channel->uuid_source != SDP_NAP) && 809 (channel->uuid_source != SDP_GN)) { 810 log_error("BNEP_CONNECTION_REQUEST: Invalid source service UUID: %04x", channel->uuid_source); 811 channel->uuid_source = 0; 812 } 813 814 /* Check if we have registered a service for the requested destination UUID */ 815 service = bnep_service_for_uuid(channel->uuid_dest); 816 if (service == NULL) { 817 response_code = BNEP_RESP_SETUP_INVALID_DEST_UUID; 818 } else 819 if ((channel->uuid_source != SDP_PANU) && (channel->uuid_dest != SDP_PANU)) { 820 response_code = BNEP_RESP_SETUP_INVALID_SOURCE_UUID; 821 } 822 } 823 824 /* Set flag to send out the connection response on next statemachine cycle */ 825 bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_RESPONSE); 826 channel->response_code = response_code; 827 828 /* Return the number of processed package bytes = BNEP Type, BNEP Control Type, UUID-Size + 2 * UUID */ 829 return 1 + 1 + 2 * uuid_size; 830 } 831 832 static int bnep_handle_connection_response(bnep_channel_t *channel, uint8_t *packet, uint16_t size) 833 { 834 uint16_t response_code; 835 836 /* Sanity check packet size */ 837 if (size < 1 + 2) { 838 return 0; 839 } 840 841 if (channel->state != BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_RESPONSE) { 842 /* Ignore a connection response in any state but WAIT_FOR_CONNECTION_RESPONSE */ 843 log_error("BNEP_CONNECTION_RESPONSE: Ignored in channel state %d", channel->state); 844 return 1 + 2; 845 } 846 847 response_code = READ_NET_16(packet, 1); 848 849 if (response_code == BNEP_RESP_SETUP_SUCCESS) { 850 log_info("BNEP_CONNECTION_RESPONSE: Channel established to %s", bd_addr_to_str(channel->remote_addr)); 851 channel->state = BNEP_CHANNEL_STATE_CONNECTED; 852 /* Stop timeout timer! */ 853 bnep_channel_stop_timer(channel); 854 bnep_emit_open_channel_complete(channel, 0); 855 } else { 856 log_error("BNEP_CONNECTION_RESPONSE: Connection to %s failed. Err: %d", bd_addr_to_str(channel->remote_addr), response_code); 857 bnep_channel_finalize(channel); 858 } 859 return 1 + 2; 860 } 861 862 static int bnep_can_handle_extensions(bnep_channel_t * channel){ 863 /* Extension are primarily handled in CONNECTED state */ 864 if (channel->state == BNEP_CHANNEL_STATE_CONNECTED) return 1; 865 /* and if we've received connection request, but haven't sent the reponse yet. */ 866 if ((channel->state == BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST) && 867 (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_RESPONSE)) { 868 return 1; 869 } 870 return 0; 871 } 872 873 static int bnep_handle_filter_net_type_set(bnep_channel_t *channel, uint8_t *packet, uint16_t size) 874 { 875 uint16_t list_length; 876 uint16_t response_code = BNEP_RESP_FILTER_SUCCESS; 877 878 /* Sanity check packet size */ 879 if (size < 3) { 880 return 0; 881 } 882 883 list_length = READ_NET_16(packet, 1); 884 /* Sanity check packet size again with known package size */ 885 if (size < 3 + list_length) { 886 return 0; 887 } 888 889 if (!bnep_can_handle_extensions(channel)){ 890 log_error("BNEP_FILTER_NET_TYPE_SET: Ignored in channel state %d", channel->state); 891 return 3 + list_length; 892 } 893 894 /* Check if we have enough space for more filters */ 895 if ((list_length / (2*2)) > MAX_BNEP_NETFILTER) { 896 log_info("BNEP_FILTER_NET_TYPE_SET: Too many filter"); 897 response_code = BNEP_RESP_FILTER_ERR_TOO_MANY_FILTERS; 898 } else { 899 int i; 900 channel->net_filter_count = 0; 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 = READ_NET_16(packet, 1 + 2 + i * 4); 904 channel->net_filter[channel->net_filter_count].range_end = READ_NET_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 = READ_NET_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 = READ_NET_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 = READ_NET_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 net_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 net_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 (packet[0]) { 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 bt_flip_addr(event_addr, &packet[2]); 1165 con_handle = READ_BT_16(packet, 8); 1166 psm = READ_BT_16(packet, 10); 1167 l2cap_cid = READ_BT_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_internal(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_internal(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_internal(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 (READ_BT_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 = READ_BT_16(packet, 9); 1214 l2cap_cid = READ_BT_16(packet, 13); 1215 bt_flip_addr(event_addr, &packet[3]); 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(READ_BT_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(READ_BT_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 DAEMON_EVENT_HCI_PACKET_SENT: 1259 bnep_run(); 1260 break; 1261 1262 case L2CAP_EVENT_CHANNEL_CLOSED: 1263 // data: event (8), len(8), channel (16) 1264 l2cap_cid = READ_BT_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 = READ_NET_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 = READ_NET_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 = READ_NET_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 = READ_NET_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 if (handled) { 1423 bnep_run(); 1424 return; 1425 } 1426 1427 /* Forward non l2cap packages to application handler */ 1428 if (packet_type != L2CAP_DATA_PACKET) { 1429 (*app_packet_handler)(packet_type, l2cap_cid, packet, size); 1430 return; 1431 } 1432 1433 bnep_run(); 1434 } 1435 1436 static void bnep_channel_state_machine(bnep_channel_t* channel, bnep_channel_event_t *event) 1437 { 1438 log_info("bnep_state_machine: state %u, state var: %02x, event %u", channel->state, channel->state_var, event->type); 1439 1440 if (event->type == BNEP_CH_EVT_READY_TO_SEND) { 1441 /* Send outstanding packets. */ 1442 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_NOT_UNDERSTOOD) { 1443 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_NOT_UNDERSTOOD); 1444 bnep_send_command_not_understood(channel, channel->last_control_type); 1445 return; 1446 } 1447 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_REQUEST) { 1448 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_REQUEST); 1449 channel->state = BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_RESPONSE; 1450 bnep_send_connection_request(channel, channel->uuid_source, channel->uuid_dest); 1451 } 1452 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_RESPONSE) { 1453 int emit_connected = 0; 1454 if ((channel->state == BNEP_CHANNEL_STATE_CLOSED) || 1455 (channel->state == BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST)) { 1456 /* Set channel state to STATE_CONNECTED */ 1457 channel->state = BNEP_CHANNEL_STATE_CONNECTED; 1458 /* Stop timeout timer! */ 1459 bnep_channel_stop_timer(channel); 1460 emit_connected = 1; 1461 } 1462 1463 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_RESPONSE); 1464 bnep_send_connection_response(channel, channel->response_code); 1465 if (emit_connected){ 1466 bnep_emit_open_channel_complete(channel, 0); 1467 } 1468 return; 1469 } 1470 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_SET) { 1471 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_SET); 1472 if ((channel->net_filter_out_count > 0) && (channel->net_filter_out != NULL)) { 1473 bnep_send_filter_net_type_set(channel, channel->net_filter_out, channel->net_filter_out_count); 1474 channel->net_filter_out_count = 0; 1475 channel->net_filter_out = NULL; 1476 } 1477 return; 1478 } 1479 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_RESPONSE) { 1480 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_RESPONSE); 1481 bnep_send_filter_net_type_response(channel, channel->response_code); 1482 return; 1483 } 1484 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_SET) { 1485 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_SET); 1486 if ((channel->multicast_filter_out_count > 0) && (channel->multicast_filter_out != NULL)) { 1487 bnep_send_filter_multi_addr_set(channel, channel->multicast_filter_out, channel->multicast_filter_out_count); 1488 channel->multicast_filter_out_count = 0; 1489 channel->multicast_filter_out = NULL; 1490 } 1491 return; 1492 } 1493 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_RESPONSE) { 1494 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_RESPONSE); 1495 bnep_send_filter_multi_addr_response(channel, channel->response_code); 1496 return; 1497 } 1498 1499 1500 /* If the event was not yet handled, notify the application layer */ 1501 bnep_emit_ready_to_send(channel); 1502 } 1503 } 1504 1505 1506 /* Process oustanding signaling tasks */ 1507 static void bnep_run(void) 1508 { 1509 linked_item_t *it; 1510 linked_item_t *next; 1511 1512 for (it = (linked_item_t *) bnep_channels; it ; it = next){ 1513 1514 next = it->next; // be prepared for removal of channel in state machine 1515 1516 bnep_channel_t * channel = ((bnep_channel_t *) it); 1517 1518 if (!l2cap_can_send_packet_now(channel->l2cap_cid)) { 1519 continue; 1520 } 1521 1522 bnep_channel_event_t channel_event = { BNEP_CH_EVT_READY_TO_SEND }; 1523 bnep_channel_state_machine(channel, &channel_event); 1524 } 1525 } 1526 1527 /* BNEP BTStack API */ 1528 void bnep_init(void) 1529 { 1530 bnep_security_level = LEVEL_0; 1531 } 1532 1533 void bnep_set_required_security_level(gap_security_level_t security_level) 1534 { 1535 bnep_security_level = security_level; 1536 } 1537 1538 /* Register application packet handler */ 1539 void bnep_register_packet_handler(void (*handler)(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size)){ 1540 app_packet_handler = handler; 1541 } 1542 1543 int bnep_connect(bd_addr_t addr, uint16_t l2cap_psm, uint16_t uuid_src, uint16_t uuid_dest) 1544 { 1545 bnep_channel_t *channel; 1546 log_info("BNEP_CONNECT addr %s", bd_addr_to_str(addr)); 1547 1548 channel = bnep_channel_create_for_addr(addr); 1549 if (channel == NULL) { 1550 return -1; 1551 } 1552 1553 channel->uuid_source = uuid_src; 1554 channel->uuid_dest = uuid_dest; 1555 1556 uint8_t status = l2cap_create_channel(bnep_packet_handler, addr, l2cap_psm, l2cap_max_mtu(), NULL); 1557 if (status){ 1558 return -1; 1559 } 1560 return 0; 1561 } 1562 1563 void bnep_disconnect(bd_addr_t addr) 1564 { 1565 bnep_channel_t *channel; 1566 log_info("BNEP_DISCONNECT"); 1567 1568 channel = bnep_channel_for_addr(addr); 1569 1570 bnep_channel_finalize(channel); 1571 1572 bnep_run(); 1573 } 1574 1575 1576 uint8_t bnep_register_service(uint16_t service_uuid, uint16_t max_frame_size) 1577 { 1578 log_info("BNEP_REGISTER_SERVICE mtu %d", max_frame_size); 1579 1580 /* Check if we already registered a service */ 1581 bnep_service_t * service = bnep_service_for_uuid(service_uuid); 1582 if (service) { 1583 return BNEP_SERVICE_ALREADY_REGISTERED; 1584 } 1585 1586 /* Only alow one the three service types: PANU, NAP, GN */ 1587 if ((service_uuid != SDP_PANU) && 1588 (service_uuid != SDP_NAP) && 1589 (service_uuid != SDP_GN)) { 1590 log_info("BNEP_REGISTER_SERVICE: Invalid service UUID: %04x", service_uuid); 1591 return BNEP_SERVICE_ALREADY_REGISTERED; // TODO: define own error 1592 } 1593 1594 /* Allocate service memory */ 1595 service = (bnep_service_t*) btstack_memory_bnep_service_get(); 1596 if (!service) { 1597 return BTSTACK_MEMORY_ALLOC_FAILED; 1598 } 1599 memset(service, 0, sizeof(bnep_service_t)); 1600 1601 /* register with l2cap if not registered before, max MTU */ 1602 l2cap_register_service(bnep_packet_handler, PSM_BNEP, 0xffff, bnep_security_level); 1603 1604 /* Setup the service struct */ 1605 service->max_frame_size = max_frame_size; 1606 service->service_uuid = service_uuid; 1607 1608 /* Add to services list */ 1609 linked_list_add(&bnep_services, (linked_item_t *) service); 1610 1611 return 0; 1612 } 1613 1614 void bnep_unregister_service(uint16_t service_uuid) 1615 { 1616 log_info("BNEP_UNREGISTER_SERVICE #%04x", service_uuid); 1617 1618 bnep_service_t *service = bnep_service_for_uuid(service_uuid); 1619 if (!service) { 1620 return; 1621 } 1622 1623 linked_list_remove(&bnep_services, (linked_item_t *) service); 1624 btstack_memory_bnep_service_free(service); 1625 service = NULL; 1626 1627 l2cap_unregister_service(PSM_BNEP); 1628 } 1629 1630