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