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