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 #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, 0, 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, 0, 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, 0, 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, 0, 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 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, 0x13); 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 if ((list_length / (2 * ETHER_ADDR_LEN)) > MAX_BNEP_MULTICAST_FILTER) { 1044 log_info("BNEP_MULTI_ADDR_SET: Too many filter"); 1045 response_code = BNEP_RESP_FILTER_ERR_TOO_MANY_FILTERS; 1046 } else { 1047 unsigned int i; 1048 channel->multicast_filter_count = 0; 1049 /* There is enough space, copy the filters to our filter list */ 1050 for (i = 0; i < list_length / (2 * ETHER_ADDR_LEN); i ++) { 1051 bd_addr_copy(channel->multicast_filter[channel->multicast_filter_count].addr_start, packet + 1 + 2 + i * ETHER_ADDR_LEN * 2); 1052 bd_addr_copy(channel->multicast_filter[channel->multicast_filter_count].addr_end, packet + 1 + 2 + i * ETHER_ADDR_LEN * 2 + ETHER_ADDR_LEN); 1053 1054 if (memcmp(channel->multicast_filter[channel->multicast_filter_count].addr_start, 1055 channel->multicast_filter[channel->multicast_filter_count].addr_end, ETHER_ADDR_LEN) > 0) { 1056 /* Invalid filter range, ignore this filter rule */ 1057 log_error("BNEP_MULTI_ADDR_SET: Invalid filter: start: %s", 1058 bd_addr_to_str(channel->multicast_filter[channel->multicast_filter_count].addr_start)); 1059 log_error("BNEP_MULTI_ADDR_SET: Invalid filter: end: %s", 1060 bd_addr_to_str(channel->multicast_filter[channel->multicast_filter_count].addr_end)); 1061 response_code = BNEP_RESP_FILTER_ERR_INVALID_RANGE; 1062 } else { 1063 /* Valid filter, increase the filter count */ 1064 log_info("BNEP_MULTI_ADDR_SET: Add filter: start: %s", 1065 bd_addr_to_str(channel->multicast_filter[channel->multicast_filter_count].addr_start)); 1066 log_info("BNEP_MULTI_ADDR_SET: Add filter: end: %s", 1067 bd_addr_to_str(channel->multicast_filter[channel->multicast_filter_count].addr_end)); 1068 channel->multicast_filter_count ++; 1069 } 1070 } 1071 } 1072 /* Set flag to send out the set multi addr response on next statemachine cycle */ 1073 bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_RESPONSE); 1074 channel->response_code = response_code; 1075 l2cap_request_can_send_now_event(channel->l2cap_cid); 1076 1077 return 3 + list_length; 1078 } 1079 1080 static int bnep_handle_multi_addr_response(bnep_channel_t *channel, uint8_t *packet, uint16_t size) 1081 { 1082 uint16_t response_code; 1083 1084 // TODO: Currently we do not support setting multicast address filter. 1085 1086 /* Sanity check packet size */ 1087 if (size < 1 + 2) { 1088 return 0; 1089 } 1090 1091 if (!bnep_can_handle_extensions(channel)){ 1092 log_error("BNEP_MULTI_ADDR_RESPONSE: Ignored in channel state %d", channel->state); 1093 return 1 + 2; 1094 } 1095 1096 response_code = big_endian_read_16(packet, 1); 1097 1098 if (response_code == BNEP_RESP_FILTER_SUCCESS) { 1099 log_info("BNEP_MULTI_ADDR_RESPONSE: Multicast address filter set successfully for %s", bd_addr_to_str(channel->remote_addr)); 1100 } else { 1101 log_error("BNEP_MULTI_ADDR_RESPONSE: Multicast address filter setting for %s failed. Err: %d", bd_addr_to_str(channel->remote_addr), response_code); 1102 } 1103 1104 return 1 + 2; 1105 } 1106 1107 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) 1108 { 1109 uint16_t pos = 0; 1110 1111 #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) 1112 /* In-place modify the package and add the ethernet header in front of the payload. 1113 * WARNING: This modifies the data in front of the payload and may overwrite 14 bytes there! 1114 */ 1115 uint8_t *ethernet_packet = payload - 2 * sizeof(bd_addr_t) - sizeof(uint16_t); 1116 /* Restore the ethernet packet header */ 1117 bd_addr_copy(ethernet_packet + pos, addr_dest); 1118 pos += sizeof(bd_addr_t); 1119 bd_addr_copy(ethernet_packet + pos, addr_source); 1120 pos += sizeof(bd_addr_t); 1121 big_endian_store_16(ethernet_packet, pos, network_protocol_type); 1122 /* Payload is just in place... */ 1123 #else 1124 #error "BNEP requires HCI_INCOMING_PRE_BUFFER_SIZE >= 6. Please update bstack_config.h" 1125 #endif 1126 1127 /* Notify application layer and deliver the ethernet packet */ 1128 if (channel->packet_handler){ 1129 (*channel->packet_handler)(BNEP_DATA_PACKET, channel->l2cap_cid, ethernet_packet, 1130 size + sizeof(uint16_t) + 2 * sizeof(bd_addr_t)); 1131 } 1132 1133 return size; 1134 } 1135 1136 static int bnep_handle_control_packet(bnep_channel_t *channel, uint8_t *packet, uint16_t size, int is_extension) 1137 { 1138 uint16_t len = 0; 1139 uint8_t bnep_control_type; 1140 1141 bnep_control_type = packet[0]; 1142 /* Save last control type. Needed by statemachin in case of unknown control code */ 1143 1144 channel->last_control_type = bnep_control_type; 1145 log_info("BNEP_CONTROL: Type: %d, size: %d, is_extension: %d", bnep_control_type, size, is_extension); 1146 switch (bnep_control_type) { 1147 case BNEP_CONTROL_TYPE_COMMAND_NOT_UNDERSTOOD: 1148 /* The last command we send was not understood. We should close the connection */ 1149 log_error("BNEP_CONTROL: Received COMMAND_NOT_UNDERSTOOD: l2cap_cid: %d, cmd: %d", channel->l2cap_cid, packet[3]); 1150 bnep_channel_finalize(channel); 1151 len = 2; // Length of command not understood packet - bnep-type field 1152 break; 1153 case BNEP_CONTROL_TYPE_SETUP_CONNECTION_REQUEST: 1154 if (is_extension) { 1155 /* Connection requests are not allowed to be send in an extension header 1156 * ignore, do not set "COMMAND_NOT_UNDERSTOOD" 1157 */ 1158 log_error("BNEP_CONTROL: Received SETUP_CONNECTION_REQUEST in extension header: l2cap_cid: %d", channel->l2cap_cid); 1159 return 0; 1160 } else { 1161 len = bnep_handle_connection_request(channel, packet, size); 1162 } 1163 break; 1164 case BNEP_CONTROL_TYPE_SETUP_CONNECTION_RESPONSE: 1165 if (is_extension) { 1166 /* Connection requests are not allowed to be send in an 1167 * extension header, ignore, do not set "COMMAND_NOT_UNDERSTOOD" 1168 */ 1169 log_error("BNEP_CONTROL: Received SETUP_CONNECTION_RESPONSE in extension header: l2cap_cid: %d", channel->l2cap_cid); 1170 return 0; 1171 } else { 1172 len = bnep_handle_connection_response(channel, packet, size); 1173 } 1174 break; 1175 case BNEP_CONTROL_TYPE_FILTER_NET_TYPE_SET: 1176 len = bnep_handle_filter_net_type_set(channel, packet, size); 1177 break; 1178 case BNEP_CONTROL_TYPE_FILTER_NET_TYPE_RESPONSE: 1179 len = bnep_handle_filter_net_type_response(channel, packet, size); 1180 break; 1181 case BNEP_CONTROL_TYPE_FILTER_MULTI_ADDR_SET: 1182 len = bnep_handle_multi_addr_set(channel, packet, size); 1183 break; 1184 case BNEP_CONTROL_TYPE_FILTER_MULTI_ADDR_RESPONSE: 1185 len = bnep_handle_multi_addr_response(channel, packet, size); 1186 break; 1187 default: 1188 log_error("BNEP_CONTROL: Invalid bnep control type: l2cap_cid: %d, cmd: %d", channel->l2cap_cid, bnep_control_type); 1189 len = 0; 1190 break; 1191 } 1192 1193 if (len == 0) { 1194 /* In case the command could not be handled, send a 1195 COMMAND_NOT_UNDERSTOOD message. 1196 Set flag to process the request in the next statemachine loop 1197 */ 1198 bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_NOT_UNDERSTOOD); 1199 l2cap_request_can_send_now_event(channel->l2cap_cid); 1200 } 1201 1202 return len; 1203 } 1204 1205 /** 1206 * @return handled packet 1207 */ 1208 static int bnep_hci_event_handler(uint8_t *packet, uint16_t size) 1209 { 1210 UNUSED(size); // ok: handling own l2cap events 1211 1212 bd_addr_t event_addr; 1213 uint16_t psm; 1214 uint16_t l2cap_cid; 1215 hci_con_handle_t con_handle; 1216 bnep_channel_t *channel = NULL; 1217 uint8_t status; 1218 1219 switch (hci_event_packet_get_type(packet)) { 1220 1221 /* Accept an incoming L2CAP connection on BLUETOOTH_PSM_BNEP */ 1222 case L2CAP_EVENT_INCOMING_CONNECTION: 1223 /* L2CAP event data: event(8), len(8), address(48), handle (16), psm (16), source cid(16) dest cid(16) */ 1224 reverse_bd_addr(&packet[2], event_addr); 1225 con_handle = little_endian_read_16(packet, 8); 1226 psm = little_endian_read_16(packet, 10); 1227 l2cap_cid = little_endian_read_16(packet, 12); 1228 1229 if (psm != BLUETOOTH_PSM_BNEP) break; 1230 1231 channel = bnep_channel_for_addr(event_addr); 1232 1233 if (channel) { 1234 log_error("INCOMING_CONNECTION (l2cap_cid 0x%02x) for BLUETOOTH_PSM_BNEP => decline - channel already exists", l2cap_cid); 1235 l2cap_decline_connection(l2cap_cid); 1236 return 1; 1237 } 1238 1239 /* Create a new BNEP channel instance (incoming) */ 1240 channel = bnep_channel_create_for_addr(event_addr); 1241 1242 if (!channel) { 1243 log_error("INCOMING_CONNECTION (l2cap_cid 0x%02x) for BLUETOOTH_PSM_BNEP => decline - no memory left", l2cap_cid); 1244 l2cap_decline_connection(l2cap_cid); 1245 return 1; 1246 } 1247 1248 /* Assign connection handle and l2cap cid */ 1249 channel->con_handle = con_handle; 1250 channel->l2cap_cid = l2cap_cid; 1251 1252 /* Set channel into accept state */ 1253 channel->state = BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST; 1254 1255 /* Start connection timeout timer */ 1256 bnep_channel_start_timer(channel, BNEP_CONNECTION_TIMEOUT_MS); 1257 1258 log_info("L2CAP_EVENT_INCOMING_CONNECTION (l2cap_cid 0x%02x) for BLUETOOTH_PSM_BNEP => accept", l2cap_cid); 1259 l2cap_accept_connection(l2cap_cid); 1260 return 1; 1261 1262 /* Outgoing L2CAP connection has been opened -> store l2cap_cid, remote_addr */ 1263 case L2CAP_EVENT_CHANNEL_OPENED: 1264 status = packet[2]; 1265 log_info("L2CAP_EVENT_CHANNEL_OPENED for BLUETOOTH_PSM_BNEP, status %u", status); 1266 1267 /* Get the bnep channel fpr remote address */ 1268 con_handle = little_endian_read_16(packet, 9); 1269 l2cap_cid = little_endian_read_16(packet, 13); 1270 reverse_bd_addr(&packet[3], event_addr); 1271 channel = bnep_channel_for_addr(event_addr); 1272 if (!channel) { 1273 log_error("L2CAP_EVENT_CHANNEL_OPENED but no BNEP channel prepared"); 1274 return 1; 1275 } 1276 1277 /* On L2CAP open error discard everything */ 1278 if (status) { 1279 /* Emit bnep_open_channel_complete with status and free channel */ 1280 bnep_emit_open_channel_complete(channel, status); 1281 1282 /* Free BNEP channel mempory */ 1283 bnep_channel_free(channel); 1284 return 1; 1285 } 1286 1287 switch (channel->state){ 1288 case BNEP_CHANNEL_STATE_CLOSED: 1289 log_info("L2CAP_EVENT_CHANNEL_OPENED: outgoing connection"); 1290 1291 bnep_channel_start_timer(channel, BNEP_CONNECTION_TIMEOUT_MS); 1292 1293 /* Assign connection handle and l2cap cid */ 1294 channel->l2cap_cid = l2cap_cid; 1295 channel->con_handle = con_handle; 1296 1297 /* Initiate the connection request */ 1298 channel->state = BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_RESPONSE; 1299 bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_REQUEST); 1300 channel->max_frame_size = bnep_max_frame_size_for_l2cap_mtu(little_endian_read_16(packet, 17)); 1301 l2cap_request_can_send_now_event(channel->l2cap_cid); 1302 break; 1303 case BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST: 1304 /* New information: channel mtu */ 1305 channel->max_frame_size = bnep_max_frame_size_for_l2cap_mtu(little_endian_read_16(packet, 17)); 1306 break; 1307 default: 1308 log_error("L2CAP_EVENT_CHANNEL_OPENED: Invalid state: %d", channel->state); 1309 break; 1310 } 1311 return 1; 1312 1313 case L2CAP_EVENT_CAN_SEND_NOW: 1314 bnep_handle_can_send_now(l2cap_event_can_send_now_get_local_cid(packet)); 1315 break; 1316 1317 case L2CAP_EVENT_CHANNEL_CLOSED: 1318 // data: event (8), len(8), channel (16) 1319 l2cap_cid = little_endian_read_16(packet, 2); 1320 channel = bnep_channel_for_l2cap_cid(l2cap_cid); 1321 log_info("L2CAP_EVENT_CHANNEL_CLOSED cid 0x%0x, channel %p", l2cap_cid, channel); 1322 1323 if (!channel) { 1324 break; 1325 } 1326 1327 log_info("L2CAP_EVENT_CHANNEL_CLOSED state %u", channel->state); 1328 switch (channel->state) { 1329 case BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST: 1330 case BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_RESPONSE: 1331 case BNEP_CHANNEL_STATE_CONNECTED: 1332 bnep_channel_finalize(channel); 1333 return 1; 1334 default: 1335 break; 1336 } 1337 break; 1338 default: 1339 break; 1340 } 1341 return 0; 1342 } 1343 1344 static int bnep_l2cap_packet_handler(uint16_t l2cap_cid, uint8_t *packet, uint16_t size) 1345 { 1346 int rc = 0; 1347 uint8_t bnep_type; 1348 uint8_t bnep_header_has_ext; 1349 uint8_t extension_type; 1350 uint16_t pos = 0; 1351 bd_addr_t addr_source; 1352 bd_addr_t addr_dest; 1353 uint16_t network_protocol_type = 0xffff; 1354 bnep_channel_t *channel = NULL; 1355 1356 /* Get the bnep channel for this package */ 1357 channel = bnep_channel_for_l2cap_cid(l2cap_cid); 1358 if (!channel) { 1359 return rc; 1360 } 1361 1362 /* Sort out short packages */ 1363 if (size < 2) { 1364 return rc; 1365 } 1366 1367 bnep_type = BNEP_TYPE(packet[pos]); 1368 bnep_header_has_ext = BNEP_HEADER_HAS_EXT(packet[pos]); 1369 pos ++; 1370 1371 switch(bnep_type) { 1372 case BNEP_PKT_TYPE_GENERAL_ETHERNET: 1373 bd_addr_copy(addr_dest, &packet[pos]); 1374 pos += sizeof(bd_addr_t); 1375 bd_addr_copy(addr_source, &packet[pos]); 1376 pos += sizeof(bd_addr_t); 1377 network_protocol_type = big_endian_read_16(packet, pos); 1378 pos += 2; 1379 break; 1380 case BNEP_PKT_TYPE_COMPRESSED_ETHERNET: 1381 bd_addr_copy(addr_dest, channel->local_addr); 1382 bd_addr_copy(addr_source, channel->remote_addr); 1383 network_protocol_type = big_endian_read_16(packet, pos); 1384 pos += 2; 1385 break; 1386 case BNEP_PKT_TYPE_COMPRESSED_ETHERNET_SOURCE_ONLY: 1387 bd_addr_copy(addr_dest, channel->local_addr); 1388 bd_addr_copy(addr_source, &packet[pos]); 1389 pos += sizeof(bd_addr_t); 1390 network_protocol_type = big_endian_read_16(packet, pos); 1391 pos += 2; 1392 break; 1393 case BNEP_PKT_TYPE_COMPRESSED_ETHERNET_DEST_ONLY: 1394 bd_addr_copy(addr_dest, &packet[pos]); 1395 pos += sizeof(bd_addr_t); 1396 bd_addr_copy(addr_source, channel->remote_addr); 1397 network_protocol_type = big_endian_read_16(packet, pos); 1398 pos += 2; 1399 break; 1400 case BNEP_PKT_TYPE_CONTROL: 1401 rc = bnep_handle_control_packet(channel, packet + pos, size - pos, 0); 1402 pos += rc; 1403 break; 1404 default: 1405 break; 1406 } 1407 1408 if (bnep_header_has_ext) { 1409 do { 1410 uint8_t ext_len; 1411 1412 /* Read extension type and check for further extensions */ 1413 extension_type = BNEP_TYPE(packet[pos]); 1414 bnep_header_has_ext = BNEP_HEADER_HAS_EXT(packet[pos]); 1415 pos ++; 1416 1417 /* Read extension header length */ 1418 ext_len = packet[pos]; 1419 pos ++; 1420 1421 if (size - pos < ext_len) { 1422 log_error("BNEP pkt handler: Invalid extension length! Packet ignored"); 1423 /* Invalid packet size! */ 1424 return 0; 1425 } 1426 1427 switch (extension_type) { 1428 case BNEP_EXT_HEADER_TYPE_EXTENSION_CONTROL: 1429 if (ext_len != bnep_handle_control_packet(channel, packet + pos, ext_len, 1)) { 1430 log_error("BNEP pkt handler: Ignore invalid control packet in extension header"); 1431 } 1432 1433 pos += ext_len; 1434 break; 1435 1436 default: 1437 /* Extension header type unknown. Unknown extension SHALL be 1438 * SHALL be forwarded in any way. But who shall handle these 1439 * extension packets? 1440 * For now: We ignore them and just drop them! 1441 */ 1442 log_error("BNEP pkt handler: Unknown extension type ignored, data dropped!"); 1443 pos += ext_len; 1444 break; 1445 } 1446 1447 } while (bnep_header_has_ext); 1448 } 1449 1450 if (bnep_type != BNEP_PKT_TYPE_CONTROL && network_protocol_type != 0xffff) { 1451 if (channel->state == BNEP_CHANNEL_STATE_CONNECTED) { 1452 rc = bnep_handle_ethernet_packet(channel, addr_dest, addr_source, network_protocol_type, packet + pos, size - pos); 1453 } else { 1454 rc = 0; 1455 } 1456 } 1457 1458 return rc; 1459 1460 } 1461 1462 void bnep_packet_handler(uint8_t packet_type, uint16_t l2cap_cid, uint8_t *packet, uint16_t size) 1463 { 1464 switch (packet_type) { 1465 case HCI_EVENT_PACKET: 1466 bnep_hci_event_handler(packet, size); 1467 break; 1468 case L2CAP_DATA_PACKET: 1469 bnep_l2cap_packet_handler(l2cap_cid, packet, size); 1470 break; 1471 default: 1472 break; 1473 } 1474 } 1475 1476 static void bnep_channel_state_machine(bnep_channel_t* channel, bnep_channel_event_t *event) 1477 { 1478 log_debug("bnep_state_machine: state %u, state var: %02x, event %u", channel->state, channel->state_var, event->type); 1479 1480 if (event->type == BNEP_CH_EVT_READY_TO_SEND) { 1481 /* Send outstanding packets. */ 1482 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_NOT_UNDERSTOOD) { 1483 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_NOT_UNDERSTOOD); 1484 bnep_send_command_not_understood(channel, channel->last_control_type); 1485 return; 1486 } 1487 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_REQUEST) { 1488 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_REQUEST); 1489 channel->state = BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_RESPONSE; 1490 bnep_send_connection_request(channel, channel->uuid_source, channel->uuid_dest); 1491 } 1492 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_RESPONSE) { 1493 int emit_connected = 0; 1494 if ((channel->state == BNEP_CHANNEL_STATE_CLOSED) || 1495 (channel->state == BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST)) { 1496 /* Set channel state to STATE_CONNECTED */ 1497 channel->state = BNEP_CHANNEL_STATE_CONNECTED; 1498 /* Stop timeout timer! */ 1499 bnep_channel_stop_timer(channel); 1500 emit_connected = 1; 1501 } 1502 1503 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_RESPONSE); 1504 bnep_send_connection_response(channel, channel->response_code); 1505 if (emit_connected){ 1506 bnep_emit_open_channel_complete(channel, 0); 1507 } 1508 return; 1509 } 1510 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_SET) { 1511 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_SET); 1512 if ((channel->net_filter_out_count > 0) && (channel->net_filter_out != NULL)) { 1513 bnep_send_filter_net_type_set(channel, channel->net_filter_out, channel->net_filter_out_count); 1514 channel->net_filter_out_count = 0; 1515 channel->net_filter_out = NULL; 1516 } 1517 return; 1518 } 1519 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_RESPONSE) { 1520 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_RESPONSE); 1521 bnep_send_filter_net_type_response(channel, channel->response_code); 1522 return; 1523 } 1524 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_SET) { 1525 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_SET); 1526 if ((channel->multicast_filter_out_count > 0) && (channel->multicast_filter_out != NULL)) { 1527 bnep_send_filter_multi_addr_set(channel, channel->multicast_filter_out, channel->multicast_filter_out_count); 1528 channel->multicast_filter_out_count = 0; 1529 channel->multicast_filter_out = NULL; 1530 } 1531 return; 1532 } 1533 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_RESPONSE) { 1534 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_RESPONSE); 1535 bnep_send_filter_multi_addr_response(channel, channel->response_code); 1536 return; 1537 } 1538 1539 /* If the event was not yet handled, notify the application layer */ 1540 if (channel->waiting_for_can_send_now){ 1541 channel->waiting_for_can_send_now = 0; 1542 bnep_emit_ready_to_send(channel); 1543 } 1544 } 1545 } 1546 1547 static void bnep_handle_can_send_now(uint16_t l2cap_cid){ 1548 btstack_linked_item_t *it; 1549 btstack_linked_item_t *next; 1550 1551 for (it = (btstack_linked_item_t *) bnep_channels; it ; it = next){ 1552 next = it->next; // be prepared for removal of channel in state machine 1553 bnep_channel_t * channel = ((bnep_channel_t *) it); 1554 if (channel->l2cap_cid != l2cap_cid) continue; 1555 // 1556 bnep_channel_event_t channel_event = { BNEP_CH_EVT_READY_TO_SEND }; 1557 bnep_channel_state_machine(channel, &channel_event); 1558 1559 if (!l2cap_can_send_packet_now(channel->l2cap_cid)) { 1560 l2cap_request_can_send_now_event(channel->l2cap_cid); 1561 return; 1562 } 1563 } 1564 } 1565 1566 1567 /* BNEP BTStack API */ 1568 void bnep_init(void) 1569 { 1570 bnep_security_level = LEVEL_2; 1571 } 1572 1573 void bnep_set_required_security_level(gap_security_level_t security_level) 1574 { 1575 bnep_security_level = security_level; 1576 } 1577 1578 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) 1579 { 1580 bnep_channel_t *channel; 1581 log_info("BNEP_CONNECT addr %s", bd_addr_to_str(addr)); 1582 1583 channel = bnep_channel_create_for_addr(addr); 1584 if (channel == NULL) { 1585 return -1; 1586 } 1587 1588 channel->uuid_source = uuid_src; 1589 channel->uuid_dest = uuid_dest; 1590 channel->packet_handler = packet_handler; 1591 1592 uint8_t status = l2cap_create_channel(bnep_packet_handler, addr, l2cap_psm, l2cap_max_mtu(), NULL); 1593 if (status){ 1594 return -1; 1595 } 1596 return 0; 1597 } 1598 1599 void bnep_disconnect(bd_addr_t addr) 1600 { 1601 bnep_channel_t *channel; 1602 log_info("BNEP_DISCONNECT"); 1603 1604 channel = bnep_channel_for_addr(addr); 1605 1606 bnep_channel_finalize(channel); 1607 } 1608 1609 1610 uint8_t bnep_register_service(btstack_packet_handler_t packet_handler, uint16_t service_uuid, uint16_t max_frame_size) 1611 { 1612 log_info("BNEP_REGISTER_SERVICE mtu %d", max_frame_size); 1613 1614 /* Check if we already registered a service */ 1615 bnep_service_t * service = bnep_service_for_uuid(service_uuid); 1616 if (service) { 1617 return BNEP_SERVICE_ALREADY_REGISTERED; 1618 } 1619 1620 /* Only alow one the three service types: PANU, NAP, GN */ 1621 if ((service_uuid != BLUETOOTH_SERVICE_CLASS_PANU) && 1622 (service_uuid != BLUETOOTH_SERVICE_CLASS_NAP) && 1623 (service_uuid != BLUETOOTH_SERVICE_CLASS_GN)) { 1624 log_info("BNEP_REGISTER_SERVICE: Invalid service UUID: %04x", service_uuid); 1625 return BNEP_SERVICE_ALREADY_REGISTERED; // TODO: define own error 1626 } 1627 1628 /* Allocate service memory */ 1629 service = (bnep_service_t*) btstack_memory_bnep_service_get(); 1630 if (!service) { 1631 return BTSTACK_MEMORY_ALLOC_FAILED; 1632 } 1633 1634 /* register with l2cap if not registered before, max MTU */ 1635 l2cap_register_service(bnep_packet_handler, BLUETOOTH_PSM_BNEP, 0xffff, bnep_security_level); 1636 1637 /* Setup the service struct */ 1638 service->max_frame_size = max_frame_size; 1639 service->service_uuid = service_uuid; 1640 service->packet_handler = packet_handler; 1641 1642 1643 /* Add to services list */ 1644 btstack_linked_list_add(&bnep_services, (btstack_linked_item_t *) service); 1645 1646 return 0; 1647 } 1648 1649 void bnep_unregister_service(uint16_t service_uuid) 1650 { 1651 log_info("BNEP_UNREGISTER_SERVICE #%04x", service_uuid); 1652 1653 bnep_service_t *service = bnep_service_for_uuid(service_uuid); 1654 if (!service) { 1655 return; 1656 } 1657 1658 btstack_linked_list_remove(&bnep_services, (btstack_linked_item_t *) service); 1659 btstack_memory_bnep_service_free(service); 1660 service = NULL; 1661 1662 l2cap_unregister_service(BLUETOOTH_PSM_BNEP); 1663 } 1664 1665