1 /* 2 * Copyright (C) 2014 BlueKitchen GmbH 3 * 4 * Redistribution and use in source and binary forms, with or without 5 * modification, are permitted provided that the following conditions 6 * are met: 7 * 8 * 1. Redistributions of source code must retain the above copyright 9 * notice, this list of conditions and the following disclaimer. 10 * 2. Redistributions in binary form must reproduce the above copyright 11 * notice, this list of conditions and the following disclaimer in the 12 * documentation and/or other materials provided with the distribution. 13 * 3. Neither the name of the copyright holders nor the names of 14 * contributors may be used to endorse or promote products derived 15 * from this software without specific prior written permission. 16 * 4. Any redistribution, use, or modification is done solely for 17 * personal benefit and not for any commercial purpose or for 18 * monetary gain. 19 * 20 * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS 21 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS 24 * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF 30 * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 31 * SUCH DAMAGE. 32 * 33 * Please inquire about commercial licensing options at 34 * [email protected] 35 * 36 */ 37 38 #define __BTSTACK_FILE__ "panu_demo.c" 39 40 /* 41 * panu_demo.c 42 * Author: Ole Reinhardt <[email protected]> 43 */ 44 45 /* EXAMPLE_START(panu_demo): PANU Demo 46 * 47 * @text This example implements both a PANU client and a server. In server mode, it 48 * sets up a BNEP server and registers a PANU SDP record and waits for incoming connections. 49 * In client mode, it connects to a remote device, does an SDP Query to identify the PANU 50 * service and initiates a BNEP connection. 51 */ 52 53 #include "btstack_config.h" 54 55 #include <arpa/inet.h> 56 #include <errno.h> 57 #include <fcntl.h> 58 #include <ifaddrs.h> 59 #include <stdint.h> 60 #include <stdio.h> 61 #include <stdlib.h> 62 #include <string.h> 63 #include <unistd.h> 64 65 #include <net/if_arp.h> 66 67 #ifdef __APPLE__ 68 #include <net/if.h> 69 #include <net/if_types.h> 70 71 #include <netinet/if_ether.h> 72 #include <netinet/in.h> 73 #endif 74 75 #include <sys/ioctl.h> 76 #include <sys/param.h> 77 #include <sys/socket.h> 78 #include <sys/stat.h> 79 #include <sys/types.h> 80 81 #ifdef __linux 82 #include <linux/if.h> 83 #include <linux/if_tun.h> 84 #endif 85 86 #include "btstack_memory.h" 87 #include "btstack_event.h" 88 #include "btstack_run_loop.h" 89 #include "classic/sdp_client.h" 90 #include "classic/sdp_util.h" 91 #include "hci.h" 92 #include "hci_cmd.h" 93 #include "hci_dump.h" 94 #include "l2cap.h" 95 #include "pan.h" 96 97 static int record_id = -1; 98 static uint16_t bnep_l2cap_psm = 0; 99 static uint32_t bnep_remote_uuid = 0; 100 static uint16_t bnep_version = 0; 101 static uint16_t bnep_cid = 0; 102 103 static uint8_t attribute_value[1000]; 104 static const unsigned int attribute_value_buffer_size = sizeof(attribute_value); 105 106 //static bd_addr_t remote = {0x04,0x0C,0xCE,0xE4,0x85,0xD3}; 107 // static bd_addr_t remote = {0xE0,0x06,0xE6,0xBB,0x95,0x79}; // Ole Thinkpad 108 static bd_addr_t remote = {0x84,0x38,0x35,0x65,0xD1,0x15}; // MacBook 2013 109 110 static int tap_fd = -1; 111 static uint8_t network_buffer[BNEP_MTU_MIN]; 112 static size_t network_buffer_len = 0; 113 114 #ifdef __APPLE__ 115 // tuntaposx provides fixed set of tapX devices 116 static const char * tap_dev = "/dev/tap0"; 117 static char tap_dev_name[16] = "tap0"; 118 #endif 119 120 #ifdef __linux 121 // Linux uses single control device to bring up tunX or tapX interface 122 static const char * tap_dev = "/dev/net/tun"; 123 static char tap_dev_name[16] = "bnep%d"; 124 #endif 125 126 127 static btstack_data_source_t tap_dev_ds; 128 static btstack_packet_callback_registration_t hci_event_callback_registration; 129 130 /* @section Main application configuration 131 * 132 * @text In the application configuration, L2CAP and BNEP are initialized and a BNEP service, for server mode, 133 * is registered, before the Bluetooth stack gets started, as shown in Listing PanuSetup. 134 */ 135 136 /* LISTING_START(PanuSetup): Panu setup */ 137 static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size); 138 static void handle_sdp_client_query_result(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size); 139 140 static void panu_setup(void){ 141 142 // register for HCI events 143 hci_event_callback_registration.callback = &packet_handler; 144 hci_add_event_handler(&hci_event_callback_registration); 145 146 // Initialize L2CAP 147 l2cap_init(); 148 149 // Initialise BNEP 150 bnep_init(); 151 // Minimum L2CAP MTU for bnep is 1691 bytes 152 bnep_register_service(packet_handler, BLUETOOTH_SERVICE_CLASS_PANU, 1691); 153 } 154 /* LISTING_END */ 155 156 /* @section TUN / TAP interface routines 157 * 158 * @text This example requires a TUN/TAP interface to connect the Bluetooth network interface 159 * with the native system. It has been tested on Linux and OS X, but should work on any 160 * system that provides TUN/TAP with minor modifications. 161 * 162 * On Linux, TUN/TAP is available by default. On OS X, tuntaposx from 163 * http://tuntaposx.sourceforge.net needs to be installed. 164 * 165 * The *tap_alloc* function sets up a virtual network interface with the given Bluetooth Address. 166 * It is rather low-level as it sets up and configures a network interface. 167 */ 168 169 static int tap_alloc(char *dev, bd_addr_t bd_addr) 170 { 171 struct ifreq ifr; 172 int fd_dev; 173 int fd_socket; 174 175 if( (fd_dev = open(tap_dev, O_RDWR)) < 0 ) { 176 fprintf(stderr, "TAP: Error opening %s: %s\n", tap_dev, strerror(errno)); 177 return -1; 178 } 179 180 #ifdef __linux 181 memset(&ifr, 0, sizeof(ifr)); 182 183 ifr.ifr_flags = IFF_TAP | IFF_NO_PI; 184 if( *dev ) { 185 strncpy(ifr.ifr_name, dev, IFNAMSIZ); 186 } 187 188 int err; 189 if( (err = ioctl(fd_dev, TUNSETIFF, (void *) &ifr)) < 0 ) { 190 fprintf(stderr, "TAP: Error setting device name: %s\n", strerror(errno)); 191 close(fd_dev); 192 return -1; 193 } 194 strcpy(dev, ifr.ifr_name); 195 #endif 196 #ifdef __APPLE__ 197 dev = tap_dev_name; 198 #endif 199 200 fd_socket = socket(PF_INET, SOCK_DGRAM, IPPROTO_IP); 201 if (fd_socket < 0) { 202 close(fd_dev); 203 fprintf(stderr, "TAP: Error opening netlink socket: %s\n", strerror(errno)); 204 return -1; 205 } 206 207 // Configure the MAC address of the newly created bnep(x) 208 // device to the local bd_address 209 memset (&ifr, 0, sizeof(struct ifreq)); 210 strcpy(ifr.ifr_name, dev); 211 #ifdef __linux 212 ifr.ifr_hwaddr.sa_family = ARPHRD_ETHER; 213 memcpy(ifr.ifr_hwaddr.sa_data, bd_addr, sizeof(bd_addr_t)); 214 if (ioctl(fd_socket, SIOCSIFHWADDR, &ifr) == -1) { 215 close(fd_dev); 216 close(fd_socket); 217 fprintf(stderr, "TAP: Error setting hw addr: %s\n", strerror(errno)); 218 exit(1); 219 return -1; 220 } 221 #endif 222 #ifdef __APPLE__ 223 ifr.ifr_addr.sa_len = ETHER_ADDR_LEN; 224 ifr.ifr_addr.sa_family = AF_LINK; 225 (void)memcpy(ifr.ifr_addr.sa_data, bd_addr, ETHER_ADDR_LEN); 226 if (ioctl(fd_socket, SIOCSIFLLADDR, &ifr) == -1) { 227 close(fd_dev); 228 close(fd_socket); 229 fprintf(stderr, "TAP: Error setting hw addr: %s\n", strerror(errno)); 230 exit(1); 231 return -1; 232 } 233 #endif 234 235 // Bring the interface up 236 if (ioctl(fd_socket, SIOCGIFFLAGS, &ifr) == -1) { 237 close(fd_dev); 238 close(fd_socket); 239 fprintf(stderr, "TAP: Error reading interface flags: %s\n", strerror(errno)); 240 return -1; 241 } 242 243 if ((ifr.ifr_flags & IFF_UP) == 0) { 244 ifr.ifr_flags |= IFF_UP; 245 246 if (ioctl(fd_socket, SIOCSIFFLAGS, &ifr) == -1) { 247 close(fd_dev); 248 close(fd_socket); 249 fprintf(stderr, "TAP: Error set IFF_UP: %s\n", strerror(errno)); 250 return -1; 251 } 252 } 253 254 close(fd_socket); 255 256 return fd_dev; 257 } 258 259 /* 260 * @text Listing processTapData shows how a packet is received from the TAP network interface 261 * and forwarded over the BNEP connection. 262 * 263 * After successfully reading a network packet, the call to 264 * the *bnep_can_send_packet_now* function checks, if BTstack can forward 265 * a network packet now. If that's not possible, the received data stays 266 * in the network buffer and the data source elements is removed from the 267 * run loop. The *process_tap_dev_data* function will not be called until 268 * the data source is registered again. This provides a basic flow control. 269 */ 270 271 /* LISTING_START(processTapData): Process incoming network packets */ 272 static void process_tap_dev_data(btstack_data_source_t *ds, btstack_data_source_callback_type_t callback_type) 273 { 274 UNUSED(ds); 275 UNUSED(callback_type); 276 277 ssize_t len; 278 len = read(ds->fd, network_buffer, sizeof(network_buffer)); 279 if (len <= 0){ 280 fprintf(stderr, "TAP: Error while reading: %s\n", strerror(errno)); 281 return; 282 } 283 284 network_buffer_len = len; 285 if (bnep_can_send_packet_now(bnep_cid)) { 286 bnep_send(bnep_cid, network_buffer, network_buffer_len); 287 network_buffer_len = 0; 288 } else { 289 // park the current network packet 290 btstack_run_loop_remove_data_source(&tap_dev_ds); 291 } 292 return; 293 } 294 /* LISTING_END */ 295 296 // PANU client routines 297 static char * get_string_from_data_element(uint8_t * element){ 298 de_size_t de_size = de_get_size_type(element); 299 int pos = de_get_header_size(element); 300 int len = 0; 301 switch (de_size){ 302 case DE_SIZE_VAR_8: 303 len = element[1]; 304 break; 305 case DE_SIZE_VAR_16: 306 len = big_endian_read_16(element, 1); 307 break; 308 default: 309 break; 310 } 311 char * str = (char*)malloc(len+1); 312 memcpy(str, &element[pos], len); 313 str[len] ='\0'; 314 return str; 315 } 316 317 318 /* @section SDP parser callback 319 * 320 * @text The SDP parsers retrieves the BNEP PAN UUID as explained in 321 * Section [on SDP BNEP Query example](#sec:sdpbnepqueryExample}. 322 */ 323 static void handle_sdp_client_query_result(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) { 324 325 UNUSED(packet_type); 326 UNUSED(channel); 327 UNUSED(size); 328 329 des_iterator_t des_list_it; 330 des_iterator_t prot_it; 331 char *str; 332 333 switch (hci_event_packet_get_type(packet)){ 334 case SDP_EVENT_QUERY_ATTRIBUTE_VALUE: 335 // Handle new SDP record 336 if (sdp_event_query_attribute_byte_get_record_id(packet) != record_id) { 337 record_id = sdp_event_query_attribute_byte_get_record_id(packet); 338 printf("SDP Record: Nr: %d\n", record_id); 339 } 340 341 if (sdp_event_query_attribute_byte_get_attribute_length(packet) <= attribute_value_buffer_size) { 342 attribute_value[sdp_event_query_attribute_byte_get_data_offset(packet)] = sdp_event_query_attribute_byte_get_data(packet); 343 344 if ((uint16_t)(sdp_event_query_attribute_byte_get_data_offset(packet)+1) == sdp_event_query_attribute_byte_get_attribute_length(packet)) { 345 346 switch(sdp_event_query_attribute_byte_get_attribute_id(packet)) { 347 case BLUETOOTH_ATTRIBUTE_SERVICE_CLASS_ID_LIST: 348 if (de_get_element_type(attribute_value) != DE_DES) break; 349 for (des_iterator_init(&des_list_it, attribute_value); des_iterator_has_more(&des_list_it); des_iterator_next(&des_list_it)) { 350 uint8_t * element = des_iterator_get_element(&des_list_it); 351 if (de_get_element_type(element) != DE_UUID) continue; 352 uint32_t uuid = de_get_uuid32(element); 353 switch (uuid){ 354 case BLUETOOTH_SERVICE_CLASS_PANU: 355 case BLUETOOTH_SERVICE_CLASS_NAP: 356 case BLUETOOTH_SERVICE_CLASS_GN: 357 printf("SDP Attribute 0x%04x: BNEP PAN protocol UUID: %04x\n", sdp_event_query_attribute_byte_get_attribute_id(packet), uuid); 358 bnep_remote_uuid = uuid; 359 break; 360 default: 361 break; 362 } 363 } 364 break; 365 case 0x0100: 366 case 0x0101: 367 str = get_string_from_data_element(attribute_value); 368 printf("SDP Attribute: 0x%04x: %s\n", sdp_event_query_attribute_byte_get_attribute_id(packet), str); 369 free(str); 370 break; 371 case BLUETOOTH_ATTRIBUTE_PROTOCOL_DESCRIPTOR_LIST: { 372 printf("SDP Attribute: 0x%04x\n", sdp_event_query_attribute_byte_get_attribute_id(packet)); 373 374 for (des_iterator_init(&des_list_it, attribute_value); des_iterator_has_more(&des_list_it); des_iterator_next(&des_list_it)) { 375 uint8_t *des_element; 376 uint8_t *element; 377 uint32_t uuid; 378 379 if (des_iterator_get_type(&des_list_it) != DE_DES) continue; 380 381 des_element = des_iterator_get_element(&des_list_it); 382 des_iterator_init(&prot_it, des_element); 383 element = des_iterator_get_element(&prot_it); 384 385 if (de_get_element_type(element) != DE_UUID) continue; 386 387 uuid = de_get_uuid32(element); 388 switch (uuid){ 389 case BLUETOOTH_PROTOCOL_L2CAP: 390 if (!des_iterator_has_more(&prot_it)) continue; 391 des_iterator_next(&prot_it); 392 de_element_get_uint16(des_iterator_get_element(&prot_it), &bnep_l2cap_psm); 393 break; 394 case BLUETOOTH_PROTOCOL_BNEP: 395 if (!des_iterator_has_more(&prot_it)) continue; 396 des_iterator_next(&prot_it); 397 de_element_get_uint16(des_iterator_get_element(&prot_it), &bnep_version); 398 break; 399 default: 400 break; 401 } 402 } 403 printf("l2cap_psm 0x%04x, bnep_version 0x%04x\n", bnep_l2cap_psm, bnep_version); 404 405 /* Create BNEP connection */ 406 bnep_connect(packet_handler, remote, bnep_l2cap_psm, BLUETOOTH_SERVICE_CLASS_PANU, bnep_remote_uuid); 407 } 408 break; 409 default: 410 break; 411 } 412 } 413 } else { 414 fprintf(stderr, "SDP attribute value buffer size exceeded: available %d, required %d\n", attribute_value_buffer_size, sdp_event_query_attribute_byte_get_attribute_length(packet)); 415 } 416 break; 417 418 case SDP_EVENT_QUERY_COMPLETE: 419 fprintf(stderr, "General query done with status %d.\n", sdp_event_query_complete_get_status(packet)); 420 421 break; 422 } 423 } 424 425 /* 426 * @section Packet Handler 427 * 428 * @text The packet handler responds to various HCI Events. 429 */ 430 431 432 /* LISTING_START(packetHandler): Packet Handler */ 433 static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) 434 { 435 /* LISTING_PAUSE */ 436 UNUSED(channel); 437 438 int rc; 439 uint8_t event; 440 bd_addr_t event_addr; 441 bd_addr_t local_addr; 442 uint16_t uuid_source; 443 uint16_t uuid_dest; 444 uint16_t mtu; 445 446 /* LISTING_RESUME */ 447 switch (packet_type) { 448 case HCI_EVENT_PACKET: 449 event = hci_event_packet_get_type(packet); 450 switch (event) { 451 /* @text When BTSTACK_EVENT_STATE with state HCI_STATE_WORKING 452 * is received and the example is started in client mode, the remote SDP BNEP query is started. 453 */ 454 case BTSTACK_EVENT_STATE: 455 if (btstack_event_state_get_state(packet) == HCI_STATE_WORKING){ 456 printf("Start SDP BNEP query.\n"); 457 sdp_client_query_uuid16(&handle_sdp_client_query_result, remote, BLUETOOTH_PROTOCOL_BNEP); 458 } 459 break; 460 461 /* LISTING_PAUSE */ 462 case HCI_EVENT_PIN_CODE_REQUEST: 463 // inform about pin code request 464 printf("Pin code request - using '0000'\n"); 465 hci_event_pin_code_request_get_bd_addr(packet, event_addr); 466 hci_send_cmd(&hci_pin_code_request_reply, &event_addr, 4, "0000"); 467 break; 468 469 case HCI_EVENT_USER_CONFIRMATION_REQUEST: 470 // inform about user confirmation request 471 printf("SSP User Confirmation Request with numeric value '%06u'\n", little_endian_read_32(packet, 8)); 472 printf("SSP User Confirmation Auto accept\n"); 473 break; 474 475 /* LISTING_RESUME */ 476 477 /* @text BNEP_EVENT_CHANNEL_OPENED is received after a BNEP connection was established or 478 * or when the connection fails. The status field returns the error code. 479 * 480 * The TAP network interface is then configured. A data source is set up and registered with the 481 * run loop to receive Ethernet packets from the TAP interface. 482 * 483 * The event contains both the source and destination UUIDs, as well as the MTU for this connection and 484 * the BNEP Channel ID, which is used for sending Ethernet packets over BNEP. 485 */ 486 case BNEP_EVENT_CHANNEL_OPENED: 487 if (bnep_event_channel_opened_get_status(packet)) { 488 printf("BNEP channel open failed, status %02x\n", bnep_event_channel_opened_get_status(packet)); 489 } else { 490 bnep_cid = bnep_event_channel_opened_get_bnep_cid(packet); 491 uuid_source = bnep_event_channel_opened_get_source_uuid(packet); 492 uuid_dest = bnep_event_channel_opened_get_destination_uuid(packet); 493 mtu = bnep_event_channel_opened_get_mtu(packet); 494 //bt_flip_addr(event_addr, &packet[9]); 495 memcpy(&event_addr, &packet[11], sizeof(bd_addr_t)); 496 printf("BNEP connection open succeeded to %s source UUID 0x%04x dest UUID: 0x%04x, max frame size %u\n", bd_addr_to_str(event_addr), uuid_source, uuid_dest, mtu); 497 /* Create the tap interface */ 498 gap_local_bd_addr(local_addr); 499 tap_fd = tap_alloc(tap_dev_name, local_addr); 500 if (tap_fd < 0) { 501 printf("Creating BNEP tap device failed: %s\n", strerror(errno)); 502 } else { 503 printf("BNEP device \"%s\" allocated.\n", tap_dev_name); 504 /* Create and register a new runloop data source */ 505 btstack_run_loop_set_data_source_fd(&tap_dev_ds, tap_fd); 506 btstack_run_loop_set_data_source_handler(&tap_dev_ds, &process_tap_dev_data); 507 btstack_run_loop_add_data_source(&tap_dev_ds); 508 } 509 } 510 break; 511 512 /* @text If there is a timeout during the connection setup, BNEP_EVENT_CHANNEL_TIMEOUT will be received 513 * and the BNEP connection will be closed 514 */ 515 case BNEP_EVENT_CHANNEL_TIMEOUT: 516 printf("BNEP channel timeout! Channel will be closed\n"); 517 break; 518 519 /* @text BNEP_EVENT_CHANNEL_CLOSED is received when the connection gets closed. 520 */ 521 case BNEP_EVENT_CHANNEL_CLOSED: 522 printf("BNEP channel closed\n"); 523 btstack_run_loop_remove_data_source(&tap_dev_ds); 524 if (tap_fd > 0) { 525 close(tap_fd); 526 tap_fd = -1; 527 } 528 break; 529 530 /* @text BNEP_EVENT_CAN_SEND_NOW indicates that a new packet can be send. This triggers the retry of a 531 * parked network packet. If this succeeds, the data source element is added to the run loop again. 532 */ 533 case BNEP_EVENT_CAN_SEND_NOW: 534 // Check for parked network packets and send it out now 535 if (network_buffer_len > 0) { 536 bnep_send(bnep_cid, network_buffer, network_buffer_len); 537 network_buffer_len = 0; 538 // Re-add the tap device data source 539 btstack_run_loop_add_data_source(&tap_dev_ds); 540 } 541 542 break; 543 544 default: 545 break; 546 } 547 break; 548 549 /* @text Ethernet packets from the remote device are received in the packet handler with type BNEP_DATA_PACKET. 550 * It is forwarded to the TAP interface. 551 */ 552 case BNEP_DATA_PACKET: 553 // Write out the ethernet frame to the tap device 554 if (tap_fd > 0) { 555 rc = write(tap_fd, packet, size); 556 if (rc < 0) { 557 fprintf(stderr, "TAP: Could not write to TAP device: %s\n", strerror(errno)); 558 } else 559 if (rc != size) { 560 fprintf(stderr, "TAP: Package written only partially %d of %d bytes\n", rc, size); 561 } 562 } 563 break; 564 565 default: 566 break; 567 } 568 } 569 /* LISTING_END */ 570 571 572 int btstack_main(int argc, const char * argv[]); 573 int btstack_main(int argc, const char * argv[]){ 574 575 UNUSED(argc); 576 UNUSED(argv); 577 578 printf("Client HCI init done\n"); 579 580 panu_setup(); 581 // Turn on the device 582 hci_power_control(HCI_POWER_ON); 583 return 0; 584 } 585 586 /* EXAMPLE_END */ 587 /* -*- Mode: C; indent-tabs-mode: nil; c-basic-offset: 4; tab-width: 4 -*- */ 588 589