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 (void)memcpy(bnep_out_buffer + pos_out, packet + pos, payload_len); 597 pos_out += payload_len; 598 599 err = l2cap_send_prepared(channel->l2cap_cid, pos_out); 600 601 if (err) { 602 log_error("bnep_send: error %d", err); 603 } 604 return err; 605 } 606 607 608 /* Set BNEP network protocol type filter */ 609 int bnep_set_net_type_filter(uint16_t bnep_cid, bnep_net_filter_t *filter, uint16_t len) 610 { 611 bnep_channel_t *channel; 612 613 if (filter == NULL) { 614 return -1; 615 } 616 617 channel = bnep_channel_for_l2cap_cid(bnep_cid); 618 if (channel == NULL) { 619 log_error("bnep_set_net_type_filter cid 0x%02x doesn't exist!", bnep_cid); 620 return 1; 621 } 622 623 if (channel->state != BNEP_CHANNEL_STATE_CONNECTED) { 624 return BNEP_CHANNEL_NOT_CONNECTED; 625 } 626 627 if (len > MAX_BNEP_NETFILTER_OUT) { 628 return BNEP_DATA_LEN_EXCEEDS_MTU; 629 } 630 631 channel->net_filter_out = filter; 632 channel->net_filter_out_count = len; 633 634 /* Set flag to send out the network protocol type filter set request */ 635 bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_SET); 636 l2cap_request_can_send_now_event(channel->l2cap_cid); 637 638 return 0; 639 } 640 641 /* Set BNEP network protocol type filter */ 642 int bnep_set_multicast_filter(uint16_t bnep_cid, bnep_multi_filter_t *filter, uint16_t len) 643 { 644 bnep_channel_t *channel; 645 646 if (filter == NULL) { 647 return -1; 648 } 649 650 channel = bnep_channel_for_l2cap_cid(bnep_cid); 651 if (channel == NULL) { 652 log_error("bnep_set_net_type_filter cid 0x%02x doesn't exist!", bnep_cid); 653 return 1; 654 } 655 656 if (channel->state != BNEP_CHANNEL_STATE_CONNECTED) { 657 return BNEP_CHANNEL_NOT_CONNECTED; 658 } 659 660 if (len > MAX_BNEP_MULTICAST_FILTER_OUT) { 661 return BNEP_DATA_LEN_EXCEEDS_MTU; 662 } 663 664 channel->multicast_filter_out = filter; 665 channel->multicast_filter_out_count = len; 666 667 /* Set flag to send out the multicast filter set request */ 668 bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_SET); 669 l2cap_request_can_send_now_event(channel->l2cap_cid); 670 671 return 0; 672 } 673 674 /* BNEP timeout timer helper function */ 675 static void bnep_channel_timer_handler(btstack_timer_source_t *timer) 676 { 677 bnep_channel_t *channel = btstack_run_loop_get_timer_context(timer); 678 // retry send setup connection at least one time 679 if (channel->state == BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_RESPONSE){ 680 if (channel->retry_count < BNEP_CONNECTION_MAX_RETRIES){ 681 channel->retry_count++; 682 bnep_channel_start_timer(channel, BNEP_CONNECTION_TIMEOUT_MS); 683 bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_REQUEST); 684 l2cap_request_can_send_now_event(channel->l2cap_cid); 685 return; 686 } 687 } 688 689 log_info( "bnep_channel_timeout_handler callback: shutting down connection!"); 690 bnep_emit_channel_timeout(channel); 691 bnep_channel_finalize(channel); 692 } 693 694 695 static void bnep_channel_stop_timer(bnep_channel_t *channel) 696 { 697 if (channel->timer_active) { 698 btstack_run_loop_remove_timer(&channel->timer); 699 channel->timer_active = 0; 700 } 701 } 702 703 static void bnep_channel_start_timer(bnep_channel_t *channel, int timeout) 704 { 705 /* Stop any eventually running timeout timer */ 706 bnep_channel_stop_timer(channel); 707 708 /* Start bnep channel timeout check timer */ 709 btstack_run_loop_set_timer(&channel->timer, timeout); 710 btstack_run_loop_set_timer_handler(&channel->timer, bnep_channel_timer_handler); 711 btstack_run_loop_set_timer_context(&channel->timer, channel); 712 btstack_run_loop_add_timer(&channel->timer); 713 channel->timer_active = 1; 714 } 715 716 /* BNEP statemachine functions */ 717 718 inline static void bnep_channel_state_add(bnep_channel_t *channel, BNEP_CHANNEL_STATE_VAR event){ 719 channel->state_var = (BNEP_CHANNEL_STATE_VAR) (channel->state_var | event); 720 } 721 inline static void bnep_channel_state_remove(bnep_channel_t *channel, BNEP_CHANNEL_STATE_VAR event){ 722 channel->state_var = (BNEP_CHANNEL_STATE_VAR) (channel->state_var & ~event); 723 } 724 725 static uint16_t bnep_max_frame_size_for_l2cap_mtu(uint16_t l2cap_mtu){ 726 727 /* Assume a standard BNEP header, containing BNEP Type (1 Byte), dest and 728 source address (6 bytes each) and networking protocol type (2 bytes) 729 */ 730 uint16_t max_frame_size = l2cap_mtu - 15; // 15 bytes BNEP header 731 732 log_info("bnep_max_frame_size_for_l2cap_mtu: %u -> %u", l2cap_mtu, max_frame_size); 733 return max_frame_size; 734 } 735 736 static bnep_channel_t * bnep_channel_create_for_addr(bd_addr_t addr) 737 { 738 /* Allocate new channel structure */ 739 bnep_channel_t *channel = btstack_memory_bnep_channel_get(); 740 if (!channel) { 741 return NULL; 742 } 743 744 channel->state = BNEP_CHANNEL_STATE_CLOSED; 745 channel->max_frame_size = bnep_max_frame_size_for_l2cap_mtu(l2cap_max_mtu()); 746 bd_addr_copy(channel->remote_addr, addr); 747 gap_local_bd_addr(channel->local_addr); 748 749 channel->net_filter_count = 0; 750 channel->multicast_filter_count = 0; 751 channel->retry_count = 0; 752 753 /* Finally add it to the channel list */ 754 btstack_linked_list_add(&bnep_channels, (btstack_linked_item_t *) channel); 755 756 return channel; 757 } 758 759 static bnep_channel_t* bnep_channel_for_addr(bd_addr_t addr) 760 { 761 btstack_linked_item_t *it; 762 for (it = (btstack_linked_item_t *) bnep_channels; it ; it = it->next){ 763 bnep_channel_t *channel = ((bnep_channel_t *) it); 764 if (bd_addr_cmp(addr, channel->remote_addr) == 0) { 765 return channel; 766 } 767 } 768 return NULL; 769 } 770 771 static bnep_channel_t * bnep_channel_for_l2cap_cid(uint16_t l2cap_cid) 772 { 773 btstack_linked_item_t *it; 774 for (it = (btstack_linked_item_t *) bnep_channels; it ; it = it->next){ 775 bnep_channel_t *channel = ((bnep_channel_t *) it); 776 if (channel->l2cap_cid == l2cap_cid) { 777 return channel; 778 } 779 } 780 return NULL; 781 } 782 783 static bnep_service_t * bnep_service_for_uuid(uint16_t uuid) 784 { 785 btstack_linked_item_t *it; 786 for (it = (btstack_linked_item_t *) bnep_services; it ; it = it->next){ 787 bnep_service_t * service = ((bnep_service_t *) it); 788 if ( service->service_uuid == uuid){ 789 return service; 790 } 791 } 792 return NULL; 793 } 794 795 static void bnep_channel_free(bnep_channel_t *channel) 796 { 797 btstack_linked_list_remove( &bnep_channels, (btstack_linked_item_t *) channel); 798 btstack_memory_bnep_channel_free(channel); 799 } 800 801 static void bnep_channel_finalize(bnep_channel_t *channel) 802 { 803 uint16_t l2cap_cid; 804 805 /* Inform application about closed channel */ 806 if (channel->state == BNEP_CHANNEL_STATE_CONNECTED) { 807 bnep_emit_channel_closed(channel); 808 } 809 810 l2cap_cid = channel->l2cap_cid; 811 812 /* Stop any eventually running timer */ 813 bnep_channel_stop_timer(channel); 814 815 /* Free ressources and then close the l2cap channel */ 816 bnep_channel_free(channel); 817 l2cap_disconnect(l2cap_cid, 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 1140 if (size > 0) { 1141 1142 uint8_t 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, 1151 packet[3]); 1152 bnep_channel_finalize(channel); 1153 len = 2; // Length of command not understood packet - bnep-type field 1154 break; 1155 case BNEP_CONTROL_TYPE_SETUP_CONNECTION_REQUEST: 1156 if (is_extension) { 1157 /* Connection requests are not allowed to be send in an extension header 1158 * ignore, do not set "COMMAND_NOT_UNDERSTOOD" 1159 */ 1160 log_error("BNEP_CONTROL: Received SETUP_CONNECTION_REQUEST in extension header: l2cap_cid: %d", 1161 channel->l2cap_cid); 1162 return 0; 1163 } else { 1164 len = bnep_handle_connection_request(channel, packet, size); 1165 } 1166 break; 1167 case BNEP_CONTROL_TYPE_SETUP_CONNECTION_RESPONSE: 1168 if (is_extension) { 1169 /* Connection requests are not allowed to be send in an 1170 * extension header, ignore, do not set "COMMAND_NOT_UNDERSTOOD" 1171 */ 1172 log_error("BNEP_CONTROL: Received SETUP_CONNECTION_RESPONSE in extension header: l2cap_cid: %d", 1173 channel->l2cap_cid); 1174 return 0; 1175 } else { 1176 len = bnep_handle_connection_response(channel, packet, size); 1177 } 1178 break; 1179 case BNEP_CONTROL_TYPE_FILTER_NET_TYPE_SET: 1180 len = bnep_handle_filter_net_type_set(channel, packet, size); 1181 break; 1182 case BNEP_CONTROL_TYPE_FILTER_NET_TYPE_RESPONSE: 1183 len = bnep_handle_filter_net_type_response(channel, packet, size); 1184 break; 1185 case BNEP_CONTROL_TYPE_FILTER_MULTI_ADDR_SET: 1186 len = bnep_handle_multi_addr_set(channel, packet, size); 1187 break; 1188 case BNEP_CONTROL_TYPE_FILTER_MULTI_ADDR_RESPONSE: 1189 len = bnep_handle_multi_addr_response(channel, packet, size); 1190 break; 1191 default: 1192 log_error("BNEP_CONTROL: Invalid bnep control type: l2cap_cid: %d, cmd: %d", channel->l2cap_cid, 1193 bnep_control_type); 1194 len = 0; 1195 break; 1196 } 1197 } 1198 1199 if (len == 0) { 1200 /* In case the command could not be handled, send a 1201 COMMAND_NOT_UNDERSTOOD message. 1202 Set flag to process the request in the next statemachine loop 1203 */ 1204 bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_NOT_UNDERSTOOD); 1205 l2cap_request_can_send_now_event(channel->l2cap_cid); 1206 } 1207 1208 return len; 1209 } 1210 1211 /** 1212 * @return handled packet 1213 */ 1214 static int bnep_hci_event_handler(uint8_t *packet, uint16_t size) 1215 { 1216 UNUSED(size); // ok: handling own l2cap events 1217 1218 bd_addr_t event_addr; 1219 uint16_t psm; 1220 uint16_t l2cap_cid; 1221 hci_con_handle_t con_handle; 1222 bnep_channel_t *channel = NULL; 1223 uint8_t status; 1224 1225 switch (hci_event_packet_get_type(packet)) { 1226 1227 /* Accept an incoming L2CAP connection on BLUETOOTH_PSM_BNEP */ 1228 case L2CAP_EVENT_INCOMING_CONNECTION: 1229 /* L2CAP event data: event(8), len(8), address(48), handle (16), psm (16), source cid(16) dest cid(16) */ 1230 reverse_bd_addr(&packet[2], event_addr); 1231 con_handle = little_endian_read_16(packet, 8); 1232 psm = little_endian_read_16(packet, 10); 1233 l2cap_cid = little_endian_read_16(packet, 12); 1234 1235 if (psm != BLUETOOTH_PSM_BNEP) break; 1236 1237 channel = bnep_channel_for_addr(event_addr); 1238 1239 if (channel) { 1240 log_error("INCOMING_CONNECTION (l2cap_cid 0x%02x) for BLUETOOTH_PSM_BNEP => decline - channel already exists", l2cap_cid); 1241 l2cap_decline_connection(l2cap_cid); 1242 return 1; 1243 } 1244 1245 /* Create a new BNEP channel instance (incoming) */ 1246 channel = bnep_channel_create_for_addr(event_addr); 1247 1248 if (!channel) { 1249 log_error("INCOMING_CONNECTION (l2cap_cid 0x%02x) for BLUETOOTH_PSM_BNEP => decline - no memory left", l2cap_cid); 1250 l2cap_decline_connection(l2cap_cid); 1251 return 1; 1252 } 1253 1254 /* Assign connection handle and l2cap cid */ 1255 channel->con_handle = con_handle; 1256 channel->l2cap_cid = l2cap_cid; 1257 1258 /* Set channel into accept state */ 1259 channel->state = BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST; 1260 1261 /* Start connection timeout timer */ 1262 bnep_channel_start_timer(channel, BNEP_CONNECTION_TIMEOUT_MS); 1263 1264 log_info("L2CAP_EVENT_INCOMING_CONNECTION (l2cap_cid 0x%02x) for BLUETOOTH_PSM_BNEP => accept", l2cap_cid); 1265 l2cap_accept_connection(l2cap_cid); 1266 return 1; 1267 1268 /* Outgoing L2CAP connection has been opened -> store l2cap_cid, remote_addr */ 1269 case L2CAP_EVENT_CHANNEL_OPENED: 1270 status = packet[2]; 1271 log_info("L2CAP_EVENT_CHANNEL_OPENED for BLUETOOTH_PSM_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 if ((pos + 14) > size) { 1380 return rc; 1381 } 1382 bd_addr_copy(addr_dest, &packet[pos]); 1383 pos += sizeof(bd_addr_t); 1384 bd_addr_copy(addr_source, &packet[pos]); 1385 pos += sizeof(bd_addr_t); 1386 network_protocol_type = big_endian_read_16(packet, pos); 1387 pos += 2; 1388 break; 1389 case BNEP_PKT_TYPE_COMPRESSED_ETHERNET: 1390 if ((pos + 2) > size) { 1391 return rc; 1392 } 1393 bd_addr_copy(addr_dest, channel->local_addr); 1394 bd_addr_copy(addr_source, channel->remote_addr); 1395 network_protocol_type = big_endian_read_16(packet, pos); 1396 pos += 2; 1397 break; 1398 case BNEP_PKT_TYPE_COMPRESSED_ETHERNET_SOURCE_ONLY: 1399 if ((pos + 8) > size) { 1400 return rc; 1401 } 1402 bd_addr_copy(addr_dest, channel->local_addr); 1403 bd_addr_copy(addr_source, &packet[pos]); 1404 pos += sizeof(bd_addr_t); 1405 network_protocol_type = big_endian_read_16(packet, pos); 1406 pos += 2; 1407 break; 1408 case BNEP_PKT_TYPE_COMPRESSED_ETHERNET_DEST_ONLY: 1409 if ((pos + 8) > size) { 1410 return rc; 1411 } 1412 bd_addr_copy(addr_dest, &packet[pos]); 1413 pos += sizeof(bd_addr_t); 1414 bd_addr_copy(addr_source, channel->remote_addr); 1415 network_protocol_type = big_endian_read_16(packet, pos); 1416 pos += 2; 1417 break; 1418 case BNEP_PKT_TYPE_CONTROL: 1419 rc = bnep_handle_control_packet(channel, packet + pos, size - pos, 0); 1420 if (rc == 0){ 1421 // invalid control packet 1422 return 0; 1423 } 1424 pos += rc; 1425 break; 1426 default: 1427 break; 1428 } 1429 1430 if (bnep_header_has_ext) { 1431 do { 1432 uint8_t ext_len; 1433 1434 if (pos + 2 > size) { 1435 return rc; 1436 } 1437 1438 /* Read extension type and check for further extensions */ 1439 extension_type = BNEP_TYPE(packet[pos]); 1440 bnep_header_has_ext = BNEP_HEADER_HAS_EXT(packet[pos]); 1441 pos ++; 1442 1443 /* Read extension header length */ 1444 ext_len = packet[pos]; 1445 pos ++; 1446 1447 if ((size - pos) < ext_len) { 1448 return 0; 1449 } 1450 1451 switch (extension_type) { 1452 case BNEP_EXT_HEADER_TYPE_EXTENSION_CONTROL: 1453 if (ext_len != bnep_handle_control_packet(channel, packet + pos, ext_len, 1)) { 1454 log_error("BNEP pkt handler: Ignore invalid control packet in extension header"); 1455 } 1456 1457 pos += ext_len; 1458 break; 1459 1460 default: 1461 /* Extension header type unknown. Unknown extension SHALL be forwarded 1462 * in any way. But who shall handle these extension packets? 1463 * For now: We ignore them and just drop them! 1464 */ 1465 log_error("BNEP pkt handler: Unknown extension type ignored, data dropped!"); 1466 pos += ext_len; 1467 break; 1468 } 1469 1470 } while (bnep_header_has_ext); 1471 } 1472 1473 if ((bnep_type != BNEP_PKT_TYPE_CONTROL) && (network_protocol_type != 0xffff)) { 1474 if (channel->state == BNEP_CHANNEL_STATE_CONNECTED) { 1475 rc = bnep_handle_ethernet_packet(channel, addr_dest, addr_source, network_protocol_type, packet + pos, size - pos); 1476 } else { 1477 rc = 0; 1478 } 1479 } 1480 1481 return rc; 1482 1483 } 1484 1485 void bnep_packet_handler(uint8_t packet_type, uint16_t l2cap_cid, uint8_t *packet, uint16_t size) 1486 { 1487 switch (packet_type) { 1488 case HCI_EVENT_PACKET: 1489 bnep_hci_event_handler(packet, size); 1490 break; 1491 case L2CAP_DATA_PACKET: 1492 bnep_l2cap_packet_handler(l2cap_cid, packet, size); 1493 break; 1494 default: 1495 break; 1496 } 1497 } 1498 1499 static void bnep_channel_state_machine(bnep_channel_t* channel, bnep_channel_event_t *event) 1500 { 1501 log_debug("bnep_state_machine: state %u, state var: %02x, event %u", channel->state, channel->state_var, event->type); 1502 1503 if (event->type == BNEP_CH_EVT_READY_TO_SEND) { 1504 /* Send outstanding packets. */ 1505 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_NOT_UNDERSTOOD) { 1506 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_NOT_UNDERSTOOD); 1507 bnep_send_command_not_understood(channel, channel->last_control_type); 1508 return; 1509 } 1510 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_REQUEST) { 1511 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_REQUEST); 1512 channel->state = BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_RESPONSE; 1513 bnep_send_connection_request(channel, channel->uuid_source, channel->uuid_dest); 1514 } 1515 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_RESPONSE) { 1516 int emit_connected = 0; 1517 if ((channel->state == BNEP_CHANNEL_STATE_CLOSED) || 1518 (channel->state == BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST)) { 1519 /* Set channel state to STATE_CONNECTED */ 1520 channel->state = BNEP_CHANNEL_STATE_CONNECTED; 1521 /* Stop timeout timer! */ 1522 bnep_channel_stop_timer(channel); 1523 emit_connected = 1; 1524 } 1525 1526 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_RESPONSE); 1527 bnep_send_connection_response(channel, channel->response_code); 1528 if (emit_connected){ 1529 bnep_emit_open_channel_complete(channel, 0); 1530 } 1531 return; 1532 } 1533 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_SET) { 1534 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_SET); 1535 if ((channel->net_filter_out_count > 0) && (channel->net_filter_out != NULL)) { 1536 bnep_send_filter_net_type_set(channel, channel->net_filter_out, channel->net_filter_out_count); 1537 channel->net_filter_out_count = 0; 1538 channel->net_filter_out = NULL; 1539 } 1540 return; 1541 } 1542 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_RESPONSE) { 1543 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_RESPONSE); 1544 bnep_send_filter_net_type_response(channel, channel->response_code); 1545 return; 1546 } 1547 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_SET) { 1548 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_SET); 1549 if ((channel->multicast_filter_out_count > 0) && (channel->multicast_filter_out != NULL)) { 1550 bnep_send_filter_multi_addr_set(channel, channel->multicast_filter_out, channel->multicast_filter_out_count); 1551 channel->multicast_filter_out_count = 0; 1552 channel->multicast_filter_out = NULL; 1553 } 1554 return; 1555 } 1556 if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_RESPONSE) { 1557 bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_RESPONSE); 1558 bnep_send_filter_multi_addr_response(channel, channel->response_code); 1559 return; 1560 } 1561 1562 /* If the event was not yet handled, notify the application layer */ 1563 if (channel->waiting_for_can_send_now){ 1564 channel->waiting_for_can_send_now = 0; 1565 bnep_emit_ready_to_send(channel); 1566 } 1567 } 1568 } 1569 1570 static void bnep_handle_can_send_now(uint16_t l2cap_cid){ 1571 btstack_linked_item_t *it; 1572 btstack_linked_item_t *next; 1573 1574 for (it = (btstack_linked_item_t *) bnep_channels; it ; it = next){ 1575 next = it->next; // be prepared for removal of channel in state machine 1576 bnep_channel_t * channel = ((bnep_channel_t *) it); 1577 if (channel->l2cap_cid != l2cap_cid) continue; 1578 // 1579 bnep_channel_event_t channel_event = { BNEP_CH_EVT_READY_TO_SEND }; 1580 bnep_channel_state_machine(channel, &channel_event); 1581 1582 if (!l2cap_can_send_packet_now(channel->l2cap_cid)) { 1583 l2cap_request_can_send_now_event(channel->l2cap_cid); 1584 return; 1585 } 1586 } 1587 } 1588 1589 1590 /* BNEP BTStack API */ 1591 void bnep_init(void) 1592 { 1593 bnep_services = NULL; 1594 bnep_channels = NULL; 1595 bnep_security_level = gap_get_security_level(); 1596 } 1597 1598 void bnep_deinit(void){ 1599 } 1600 1601 void bnep_set_required_security_level(gap_security_level_t security_level) 1602 { 1603 bnep_security_level = security_level; 1604 } 1605 1606 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) 1607 { 1608 bnep_channel_t *channel; 1609 log_info("BNEP_CONNECT addr %s", bd_addr_to_str(addr)); 1610 1611 channel = bnep_channel_create_for_addr(addr); 1612 if (channel == NULL) { 1613 return -1; 1614 } 1615 1616 channel->uuid_source = uuid_src; 1617 channel->uuid_dest = uuid_dest; 1618 channel->packet_handler = packet_handler; 1619 1620 uint8_t status = l2cap_create_channel(bnep_packet_handler, addr, l2cap_psm, l2cap_max_mtu(), NULL); 1621 if (status){ 1622 return -1; 1623 } 1624 return 0; 1625 } 1626 1627 void bnep_disconnect(bd_addr_t addr) 1628 { 1629 bnep_channel_t *channel; 1630 log_info("BNEP_DISCONNECT"); 1631 1632 channel = bnep_channel_for_addr(addr); 1633 1634 bnep_channel_finalize(channel); 1635 } 1636 1637 1638 uint8_t bnep_register_service(btstack_packet_handler_t packet_handler, uint16_t service_uuid, uint16_t max_frame_size) 1639 { 1640 log_info("BNEP_REGISTER_SERVICE mtu %d", max_frame_size); 1641 1642 /* Check if we already registered a service */ 1643 bnep_service_t * service = bnep_service_for_uuid(service_uuid); 1644 if (service) { 1645 return BNEP_SERVICE_ALREADY_REGISTERED; 1646 } 1647 1648 /* Only alow one the three service types: PANU, NAP, GN */ 1649 if ((service_uuid != BLUETOOTH_SERVICE_CLASS_PANU) && 1650 (service_uuid != BLUETOOTH_SERVICE_CLASS_NAP) && 1651 (service_uuid != BLUETOOTH_SERVICE_CLASS_GN)) { 1652 log_info("BNEP_REGISTER_SERVICE: Invalid service UUID: %04x", service_uuid); 1653 return BNEP_SERVICE_ALREADY_REGISTERED; // TODO: define own error 1654 } 1655 1656 /* Allocate service memory */ 1657 service = (bnep_service_t*) btstack_memory_bnep_service_get(); 1658 if (!service) { 1659 return BTSTACK_MEMORY_ALLOC_FAILED; 1660 } 1661 1662 /* register with l2cap if not registered before, max MTU */ 1663 l2cap_register_service(bnep_packet_handler, BLUETOOTH_PSM_BNEP, 0xffff, bnep_security_level); 1664 1665 /* Setup the service struct */ 1666 service->max_frame_size = max_frame_size; 1667 service->service_uuid = service_uuid; 1668 service->packet_handler = packet_handler; 1669 1670 1671 /* Add to services list */ 1672 btstack_linked_list_add(&bnep_services, (btstack_linked_item_t *) service); 1673 1674 return 0; 1675 } 1676 1677 void bnep_unregister_service(uint16_t service_uuid) 1678 { 1679 log_info("BNEP_UNREGISTER_SERVICE #%04x", service_uuid); 1680 1681 bnep_service_t *service = bnep_service_for_uuid(service_uuid); 1682 if (!service) { 1683 return; 1684 } 1685 1686 btstack_linked_list_remove(&bnep_services, (btstack_linked_item_t *) service); 1687 btstack_memory_bnep_service_free(service); 1688 service = NULL; 1689 1690 l2cap_unregister_service(BLUETOOTH_PSM_BNEP); 1691 } 1692 1693