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