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