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