1 /* 2 * 3 * BlueZ - Bluetooth protocol stack for Linux 4 * 5 * Copyright (C) 2002-2003 Maxim Krasnyansky <maxk (at) qualcomm.com> 6 * Copyright (C) 2002-2010 Marcel Holtmann <marcel (at) holtmann.org> 7 * 8 * 9 * This program is free software; you can redistribute it and/or modify 10 * it under the terms of the GNU General Public License as published by 11 * the Free Software Foundation; either version 2 of the License, or 12 * (at your option) any later version. 13 * 14 * This program is distributed in the hope that it will be useful, 15 * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 * GNU General Public License for more details. 18 * 19 * You should have received a copy of the GNU General Public License 20 * along with this program; if not, write to the Free Software 21 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 22 * 23 */ 24 25 #ifdef HAVE_CONFIG_H 26 #include <config.h> 27 #endif 28 29 #include <stdio.h> 30 #include <errno.h> 31 #include <ctype.h> 32 #include <fcntl.h> 33 #include <unistd.h> 34 #include <stdlib.h> 35 #include <getopt.h> 36 #include <syslog.h> 37 #include <signal.h> 38 #include <sys/time.h> 39 #include <sys/ioctl.h> 40 #include <sys/socket.h> 41 42 #include <bluetooth/bluetooth.h> 43 #include <bluetooth/hci.h> 44 #include <bluetooth/hci_lib.h> 45 #include <bluetooth/rfcomm.h> 46 #include <bluetooth/sdp.h> 47 #include <bluetooth/sdp_lib.h> 48 49 /* Test modes */ 50 enum { 51 SEND, 52 RECV, 53 RECONNECT, 54 MULTY, 55 DUMP, 56 CONNECT, 57 CRECV, 58 LSEND 59 }; 60 61 static unsigned char *buf; 62 63 /* Default data size */ 64 static long data_size = 127; 65 static long num_frames = -1; 66 67 /* Default number of consecutive frames before the delay */ 68 static int count = 1; 69 70 /* Default delay after sending count number of frames */ 71 static unsigned long delay = 0; 72 73 /* Default addr and channel */ 74 static bdaddr_t bdaddr; 75 static uint16_t uuid = 0x0000; 76 static uint8_t channel = 10; 77 78 static char *filename = NULL; 79 80 static int master = 0; 81 static int auth = 0; 82 static int encrypt = 0; 83 static int secure = 0; 84 static int socktype = SOCK_STREAM; 85 static int linger = 0; 86 static int timestamp = 0; 87 static int defer_setup = 0; 88 89 static float tv2fl(struct timeval tv) 90 { 91 return (float)tv.tv_sec + (float)(tv.tv_usec/1000000.0); 92 } 93 94 static uint8_t get_channel(const char *svr, uint16_t uuid) 95 { 96 sdp_session_t *sdp; 97 sdp_list_t *srch, *attrs, *rsp; 98 uuid_t svclass; 99 uint16_t attr; 100 bdaddr_t dst; 101 uint8_t channel = 0; 102 int err; 103 104 str2ba(svr, &dst); 105 106 sdp = sdp_connect(&bdaddr, &dst, SDP_RETRY_IF_BUSY); 107 if (!sdp) 108 return 0; 109 110 sdp_uuid16_create(&svclass, uuid); 111 srch = sdp_list_append(NULL, &svclass); 112 113 attr = SDP_ATTR_PROTO_DESC_LIST; 114 attrs = sdp_list_append(NULL, &attr); 115 116 err = sdp_service_search_attr_req(sdp, srch, 117 SDP_ATTR_REQ_INDIVIDUAL, attrs, &rsp); 118 if (err) 119 goto done; 120 121 for (; rsp; rsp = rsp->next) { 122 sdp_record_t *rec = (sdp_record_t *) rsp->data; 123 sdp_list_t *protos; 124 125 if (!sdp_get_access_protos(rec, &protos)) { 126 channel = sdp_get_proto_port(protos, RFCOMM_UUID); 127 if (channel > 0) 128 break; 129 } 130 } 131 132 done: 133 sdp_close(sdp); 134 135 return channel; 136 } 137 138 static int do_connect(const char *svr) 139 { 140 struct sockaddr_rc addr; 141 struct rfcomm_conninfo conn; 142 socklen_t optlen; 143 int sk, opt; 144 145 if (uuid != 0x0000) 146 channel = get_channel(svr, uuid); 147 148 if (channel == 0) { 149 syslog(LOG_ERR, "Can't get channel number"); 150 return -1; 151 } 152 153 /* Create socket */ 154 sk = socket(PF_BLUETOOTH, socktype, BTPROTO_RFCOMM); 155 if (sk < 0) { 156 syslog(LOG_ERR, "Can't create socket: %s (%d)", 157 strerror(errno), errno); 158 return -1; 159 } 160 161 /* Bind to local address */ 162 memset(&addr, 0, sizeof(addr)); 163 addr.rc_family = AF_BLUETOOTH; 164 bacpy(&addr.rc_bdaddr, &bdaddr); 165 166 if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) { 167 syslog(LOG_ERR, "Can't bind socket: %s (%d)", 168 strerror(errno), errno); 169 goto error; 170 } 171 172 #if 0 173 /* Enable SO_TIMESTAMP */ 174 if (timestamp) { 175 int t = 1; 176 177 if (setsockopt(sk, SOL_SOCKET, SO_TIMESTAMP, &t, sizeof(t)) < 0) { 178 syslog(LOG_ERR, "Can't enable SO_TIMESTAMP: %s (%d)", 179 strerror(errno), errno); 180 goto error; 181 } 182 } 183 #endif 184 185 /* Enable SO_LINGER */ 186 if (linger) { 187 struct linger l = { .l_onoff = 1, .l_linger = linger }; 188 189 if (setsockopt(sk, SOL_SOCKET, SO_LINGER, &l, sizeof(l)) < 0) { 190 syslog(LOG_ERR, "Can't enable SO_LINGER: %s (%d)", 191 strerror(errno), errno); 192 goto error; 193 } 194 } 195 196 /* Set link mode */ 197 opt = 0; 198 if (master) 199 opt |= RFCOMM_LM_MASTER; 200 if (auth) 201 opt |= RFCOMM_LM_AUTH; 202 if (encrypt) 203 opt |= RFCOMM_LM_ENCRYPT; 204 if (secure) 205 opt |= RFCOMM_LM_SECURE; 206 207 if (opt && setsockopt(sk, SOL_RFCOMM, RFCOMM_LM, &opt, sizeof(opt)) < 0) { 208 syslog(LOG_ERR, "Can't set RFCOMM link mode: %s (%d)", 209 strerror(errno), errno); 210 goto error; 211 } 212 213 /* Connect to remote device */ 214 memset(&addr, 0, sizeof(addr)); 215 addr.rc_family = AF_BLUETOOTH; 216 str2ba(svr, &addr.rc_bdaddr); 217 addr.rc_channel = channel; 218 219 if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) { 220 syslog(LOG_ERR, "Can't connect: %s (%d)", 221 strerror(errno), errno); 222 goto error; 223 } 224 225 /* Get connection information */ 226 memset(&conn, 0, sizeof(conn)); 227 optlen = sizeof(conn); 228 229 if (getsockopt(sk, SOL_RFCOMM, RFCOMM_CONNINFO, &conn, &optlen) < 0) { 230 syslog(LOG_ERR, "Can't get RFCOMM connection information: %s (%d)", 231 strerror(errno), errno); 232 //goto error; 233 } 234 235 syslog(LOG_INFO, "Connected [handle %d, class 0x%02x%02x%02x]", 236 conn.hci_handle, 237 conn.dev_class[2], conn.dev_class[1], conn.dev_class[0]); 238 239 return sk; 240 241 error: 242 close(sk); 243 return -1; 244 } 245 246 static void do_listen(void (*handler)(int sk)) 247 { 248 struct sockaddr_rc addr; 249 struct rfcomm_conninfo conn; 250 socklen_t optlen; 251 int sk, nsk, opt; 252 char ba[18]; 253 254 /* Create socket */ 255 sk = socket(PF_BLUETOOTH, socktype, BTPROTO_RFCOMM); 256 if (sk < 0) { 257 syslog(LOG_ERR, "Can't create socket: %s (%d)", 258 strerror(errno), errno); 259 exit(1); 260 } 261 262 /* Bind to local address */ 263 memset(&addr, 0, sizeof(addr)); 264 addr.rc_family = AF_BLUETOOTH; 265 bacpy(&addr.rc_bdaddr, &bdaddr); 266 addr.rc_channel = channel; 267 268 if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) { 269 syslog(LOG_ERR, "Can't bind socket: %s (%d)", 270 strerror(errno), errno); 271 goto error; 272 } 273 274 /* Set link mode */ 275 opt = 0; 276 if (master) 277 opt |= RFCOMM_LM_MASTER; 278 if (auth) 279 opt |= RFCOMM_LM_AUTH; 280 if (encrypt) 281 opt |= RFCOMM_LM_ENCRYPT; 282 if (secure) 283 opt |= RFCOMM_LM_SECURE; 284 285 if (opt && setsockopt(sk, SOL_RFCOMM, RFCOMM_LM, &opt, sizeof(opt)) < 0) { 286 syslog(LOG_ERR, "Can't set RFCOMM link mode: %s (%d)", 287 strerror(errno), errno); 288 goto error; 289 } 290 291 /* Enable deferred setup */ 292 opt = defer_setup; 293 294 if (opt && setsockopt(sk, SOL_BLUETOOTH, BT_DEFER_SETUP, 295 &opt, sizeof(opt)) < 0) { 296 syslog(LOG_ERR, "Can't enable deferred setup : %s (%d)", 297 strerror(errno), errno); 298 goto error; 299 } 300 301 /* Listen for connections */ 302 if (listen(sk, 10)) { 303 syslog(LOG_ERR,"Can not listen on the socket: %s (%d)", 304 strerror(errno), errno); 305 goto error; 306 } 307 308 /* Check for socket address */ 309 memset(&addr, 0, sizeof(addr)); 310 optlen = sizeof(addr); 311 312 if (getsockname(sk, (struct sockaddr *) &addr, &optlen) < 0) { 313 syslog(LOG_ERR, "Can't get socket name: %s (%d)", 314 strerror(errno), errno); 315 goto error; 316 } 317 318 channel = addr.rc_channel; 319 320 syslog(LOG_INFO, "Waiting for connection on channel %d ...", channel); 321 322 while (1) { 323 memset(&addr, 0, sizeof(addr)); 324 optlen = sizeof(addr); 325 326 nsk = accept(sk, (struct sockaddr *) &addr, &optlen); 327 if (nsk < 0) { 328 syslog(LOG_ERR,"Accept failed: %s (%d)", 329 strerror(errno), errno); 330 goto error; 331 } 332 if (fork()) { 333 /* Parent */ 334 close(nsk); 335 continue; 336 } 337 /* Child */ 338 close(sk); 339 340 /* Get connection information */ 341 memset(&conn, 0, sizeof(conn)); 342 optlen = sizeof(conn); 343 344 if (getsockopt(nsk, SOL_RFCOMM, RFCOMM_CONNINFO, &conn, &optlen) < 0) { 345 syslog(LOG_ERR, "Can't get RFCOMM connection information: %s (%d)", 346 strerror(errno), errno); 347 //close(nsk); 348 //goto error; 349 } 350 351 ba2str(&addr.rc_bdaddr, ba); 352 syslog(LOG_INFO, "Connect from %s [handle %d, class 0x%02x%02x%02x]", 353 ba, conn.hci_handle, 354 conn.dev_class[2], conn.dev_class[1], conn.dev_class[0]); 355 356 #if 0 357 /* Enable SO_TIMESTAMP */ 358 if (timestamp) { 359 int t = 1; 360 361 if (setsockopt(nsk, SOL_SOCKET, SO_TIMESTAMP, &t, sizeof(t)) < 0) { 362 syslog(LOG_ERR, "Can't enable SO_TIMESTAMP: %s (%d)", 363 strerror(errno), errno); 364 goto error; 365 } 366 } 367 #endif 368 369 /* Enable SO_LINGER */ 370 if (linger) { 371 struct linger l = { .l_onoff = 1, .l_linger = linger }; 372 373 if (setsockopt(nsk, SOL_SOCKET, SO_LINGER, &l, sizeof(l)) < 0) { 374 syslog(LOG_ERR, "Can't enable SO_LINGER: %s (%d)", 375 strerror(errno), errno); 376 close(nsk); 377 goto error; 378 } 379 } 380 381 /* Handle deferred setup */ 382 if (defer_setup) { 383 syslog(LOG_INFO, "Waiting for %d seconds", 384 abs(defer_setup) - 1); 385 sleep(abs(defer_setup) - 1); 386 387 if (defer_setup < 0) { 388 close(nsk); 389 goto error; 390 } 391 } 392 393 handler(nsk); 394 395 syslog(LOG_INFO, "Disconnect: %m"); 396 exit(0); 397 } 398 399 return; 400 401 error: 402 close(sk); 403 exit(1); 404 } 405 406 static void dump_mode(int sk) 407 { 408 int len; 409 410 syslog(LOG_INFO, "Receiving ..."); 411 while ((len = read(sk, buf, data_size)) > 0) 412 syslog(LOG_INFO, "Recevied %d bytes", len); 413 } 414 415 static void recv_mode(int sk) 416 { 417 struct timeval tv_beg, tv_end, tv_diff; 418 char ts[30]; 419 long total; 420 421 syslog(LOG_INFO, "Receiving ..."); 422 423 memset(ts, 0, sizeof(ts)); 424 425 while (1) { 426 gettimeofday(&tv_beg,NULL); 427 total = 0; 428 while (total < data_size) { 429 //uint32_t sq; 430 //uint16_t l; 431 int r; 432 433 if ((r = recv(sk, buf, data_size, 0)) < 0) { 434 if (r < 0) 435 syslog(LOG_ERR, "Read failed: %s (%d)", 436 strerror(errno), errno); 437 return; 438 } 439 440 if (timestamp) { 441 struct timeval tv; 442 443 if (ioctl(sk, SIOCGSTAMP, &tv) < 0) { 444 timestamp = 0; 445 memset(ts, 0, sizeof(ts)); 446 } else { 447 sprintf(ts, "[%ld.%ld] ", 448 tv.tv_sec, tv.tv_usec); 449 } 450 } 451 452 #if 0 453 /* Check sequence */ 454 sq = btohl(*(uint32_t *) buf); 455 if (seq != sq) { 456 syslog(LOG_INFO, "seq missmatch: %d -> %d", seq, sq); 457 seq = sq; 458 } 459 seq++; 460 461 /* Check length */ 462 l = btohs(*(uint16_t *) (buf + 4)); 463 if (r != l) { 464 syslog(LOG_INFO, "size missmatch: %d -> %d", r, l); 465 continue; 466 } 467 468 /* Verify data */ 469 for (i = 6; i < r; i++) { 470 if (buf[i] != 0x7f) 471 syslog(LOG_INFO, "data missmatch: byte %d 0x%2.2x", i, buf[i]); 472 } 473 #endif 474 total += r; 475 } 476 gettimeofday(&tv_end,NULL); 477 478 timersub(&tv_end,&tv_beg,&tv_diff); 479 480 syslog(LOG_INFO,"%s%ld bytes in %.2f sec, %.2f kB/s", ts, total, 481 tv2fl(tv_diff), (float)(total / tv2fl(tv_diff) ) / 1024.0); 482 } 483 } 484 485 static void do_send(int sk) 486 { 487 uint32_t seq; 488 int i, fd, len; 489 490 syslog(LOG_INFO,"Sending ..."); 491 492 if (filename) { 493 fd = open(filename, O_RDONLY); 494 if (fd < 0) { 495 syslog(LOG_ERR, "Open failed: %s (%d)", 496 strerror(errno), errno); 497 exit(1); 498 } 499 len = read(fd, buf, data_size); 500 send(sk, buf, len, 0); 501 return; 502 } else { 503 for (i = 6; i < data_size; i++) 504 buf[i] = 0x7f; 505 } 506 507 seq = 0; 508 while ((num_frames == -1) || (num_frames-- > 0)) { 509 *(uint32_t *) buf = htobl(seq); 510 *(uint16_t *) (buf + 4) = htobs(data_size); 511 seq++; 512 513 if (send(sk, buf, data_size, 0) <= 0) { 514 syslog(LOG_ERR, "Send failed: %s (%d)", 515 strerror(errno), errno); 516 exit(1); 517 } 518 519 if (num_frames && delay && count && !(seq % count)) 520 usleep(delay); 521 } 522 } 523 524 static void send_mode(int sk) 525 { 526 do_send(sk); 527 528 syslog(LOG_INFO, "Closing channel ..."); 529 if (shutdown(sk, SHUT_RDWR) < 0) 530 syslog(LOG_INFO, "Close failed: %m"); 531 else 532 syslog(LOG_INFO, "Done"); 533 } 534 535 static void reconnect_mode(char *svr) 536 { 537 while(1) { 538 int sk = do_connect(svr); 539 close(sk); 540 } 541 } 542 543 static void multi_connect_mode(int argc, char *argv[]) 544 { 545 int i, n, sk; 546 547 while (1) { 548 for (n = 0; n < argc; n++) { 549 for (i = 0; i < count; i++) { 550 if (fork()) 551 continue; 552 553 /* Child */ 554 sk = do_connect(argv[n]); 555 usleep(500); 556 close(sk); 557 exit(0); 558 } 559 } 560 sleep(4); 561 } 562 } 563 564 static void usage(void) 565 { 566 printf("rctest - RFCOMM testing\n" 567 "Usage:\n"); 568 printf("\trctest <mode> [options] [bdaddr]\n"); 569 printf("Modes:\n" 570 "\t-r listen and receive\n" 571 "\t-w listen and send\n" 572 "\t-d listen and dump incoming data\n" 573 "\t-s connect and send\n" 574 "\t-u connect and receive\n" 575 "\t-n connect and be silent\n" 576 "\t-c connect, disconnect, connect, ...\n" 577 "\t-m multiple connects\n"); 578 579 printf("Options:\n" 580 "\t[-b bytes] [-i device] [-P channel] [-U uuid]\n" 581 "\t[-L seconds] enabled SO_LINGER option\n" 582 "\t[-W seconds] enable deferred setup\n" 583 "\t[-B filename] use data packets from file\n" 584 "\t[-N num] number of frames to send\n" 585 "\t[-C num] send num frames before delay (default = 1)\n" 586 "\t[-D milliseconds] delay after sending num frames (default = 0)\n" 587 "\t[-A] request authentication\n" 588 "\t[-E] request encryption\n" 589 "\t[-S] secure connection\n" 590 "\t[-M] become master\n" 591 "\t[-T] enable timestamps\n"); 592 } 593 594 int main(int argc, char *argv[]) 595 { 596 struct sigaction sa; 597 int opt, sk, mode = RECV, need_addr = 0; 598 599 bacpy(&bdaddr, BDADDR_ANY); 600 601 while ((opt=getopt(argc,argv,"rdscuwmnb:i:P:U:B:N:MAESL:W:C:D:T")) != EOF) { 602 switch (opt) { 603 case 'r': 604 mode = RECV; 605 break; 606 607 case 's': 608 mode = SEND; 609 need_addr = 1; 610 break; 611 612 case 'w': 613 mode = LSEND; 614 break; 615 616 case 'u': 617 mode = CRECV; 618 need_addr = 1; 619 break; 620 621 case 'd': 622 mode = DUMP; 623 break; 624 625 case 'c': 626 mode = RECONNECT; 627 need_addr = 1; 628 break; 629 630 case 'n': 631 mode = CONNECT; 632 need_addr = 1; 633 break; 634 635 case 'm': 636 mode = MULTY; 637 need_addr = 1; 638 break; 639 640 case 'b': 641 data_size = atoi(optarg); 642 break; 643 644 case 'i': 645 if (!strncasecmp(optarg, "hci", 3)) 646 hci_devba(atoi(optarg + 3), &bdaddr); 647 else 648 str2ba(optarg, &bdaddr); 649 break; 650 651 case 'P': 652 channel = atoi(optarg); 653 break; 654 655 case 'U': 656 if (!strcasecmp(optarg, "spp")) 657 uuid = SERIAL_PORT_SVCLASS_ID; 658 else if (!strncasecmp(optarg, "0x", 2)) 659 uuid = strtoul(optarg + 2, NULL, 16); 660 else 661 uuid = atoi(optarg); 662 break; 663 664 case 'M': 665 master = 1; 666 break; 667 668 case 'A': 669 auth = 1; 670 break; 671 672 case 'E': 673 encrypt = 1; 674 break; 675 676 case 'S': 677 secure = 1; 678 break; 679 680 case 'L': 681 linger = atoi(optarg); 682 break; 683 684 case 'W': 685 defer_setup = atoi(optarg); 686 break; 687 688 case 'B': 689 filename = strdup(optarg); 690 break; 691 692 case 'N': 693 num_frames = atoi(optarg); 694 break; 695 696 case 'C': 697 count = atoi(optarg); 698 break; 699 700 case 'D': 701 delay = atoi(optarg) * 1000; 702 break; 703 704 case 'T': 705 timestamp = 1; 706 break; 707 708 default: 709 usage(); 710 exit(1); 711 } 712 } 713 714 if (need_addr && !(argc - optind)) { 715 usage(); 716 exit(1); 717 } 718 719 if (!(buf = malloc(data_size))) { 720 perror("Can't allocate data buffer"); 721 exit(1); 722 } 723 724 memset(&sa, 0, sizeof(sa)); 725 sa.sa_handler = SIG_IGN; 726 sa.sa_flags = SA_NOCLDSTOP; 727 sigaction(SIGCHLD, &sa, NULL); 728 729 openlog("rctest", LOG_PERROR | LOG_PID, LOG_LOCAL0); 730 731 switch (mode) { 732 case RECV: 733 do_listen(recv_mode); 734 break; 735 736 case CRECV: 737 sk = do_connect(argv[optind]); 738 if (sk < 0) 739 exit(1); 740 recv_mode(sk); 741 break; 742 743 case DUMP: 744 do_listen(dump_mode); 745 break; 746 747 case SEND: 748 sk = do_connect(argv[optind]); 749 if (sk < 0) 750 exit(1); 751 send_mode(sk); 752 break; 753 754 case LSEND: 755 do_listen(send_mode); 756 break; 757 758 case RECONNECT: 759 reconnect_mode(argv[optind]); 760 break; 761 762 case MULTY: 763 multi_connect_mode(argc - optind, argv + optind); 764 break; 765 766 case CONNECT: 767 sk = do_connect(argv[optind]); 768 if (sk < 0) 769 exit(1); 770 dump_mode(sk); 771 break; 772 } 773 774 syslog(LOG_INFO, "Exit"); 775 776 closelog(); 777 778 return 0; 779 } 780
