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