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