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 uint32_t seq; 421 422 syslog(LOG_INFO, "Receiving ..."); 423 424 memset(ts, 0, sizeof(ts)); 425 426 seq = 0; 427 while (1) { 428 gettimeofday(&tv_beg,NULL); 429 total = 0; 430 while (total < data_size) { 431 //uint32_t sq; 432 //uint16_t l; 433 int r; 434 435 if ((r = recv(sk, buf, data_size, 0)) < 0) { 436 if (r < 0) 437 syslog(LOG_ERR, "Read failed: %s (%d)", 438 strerror(errno), errno); 439 return; 440 } 441 442 if (timestamp) { 443 struct timeval tv; 444 445 if (ioctl(sk, SIOCGSTAMP, &tv) < 0) { 446 timestamp = 0; 447 memset(ts, 0, sizeof(ts)); 448 } else { 449 sprintf(ts, "[%ld.%ld] ", 450 tv.tv_sec, tv.tv_usec); 451 } 452 } 453 454 #if 0 455 /* Check sequence */ 456 sq = btohl(*(uint32_t *) buf); 457 if (seq != sq) { 458 syslog(LOG_INFO, "seq missmatch: %d -> %d", seq, sq); 459 seq = sq; 460 } 461 seq++; 462 463 /* Check length */ 464 l = btohs(*(uint16_t *) (buf + 4)); 465 if (r != l) { 466 syslog(LOG_INFO, "size missmatch: %d -> %d", r, l); 467 continue; 468 } 469 470 /* Verify data */ 471 for (i = 6; i < r; i++) { 472 if (buf[i] != 0x7f) 473 syslog(LOG_INFO, "data missmatch: byte %d 0x%2.2x", i, buf[i]); 474 } 475 #endif 476 total += r; 477 } 478 gettimeofday(&tv_end,NULL); 479 480 timersub(&tv_end,&tv_beg,&tv_diff); 481 482 syslog(LOG_INFO,"%s%ld bytes in %.2f sec, %.2f kB/s", ts, total, 483 tv2fl(tv_diff), (float)(total / tv2fl(tv_diff) ) / 1024.0); 484 } 485 } 486 487 static void do_send(int sk) 488 { 489 uint32_t seq; 490 int i, fd, len; 491 492 syslog(LOG_INFO,"Sending ..."); 493 494 if (filename) { 495 fd = open(filename, O_RDONLY); 496 if (fd < 0) { 497 syslog(LOG_ERR, "Open failed: %s (%d)", 498 strerror(errno), errno); 499 exit(1); 500 } 501 len = read(fd, buf, data_size); 502 send(sk, buf, len, 0); 503 return; 504 } else { 505 for (i = 6; i < data_size; i++) 506 buf[i] = 0x7f; 507 } 508 509 seq = 0; 510 while ((num_frames == -1) || (num_frames-- > 0)) { 511 *(uint32_t *) buf = htobl(seq); 512 *(uint16_t *) (buf + 4) = htobs(data_size); 513 seq++; 514 515 if (send(sk, buf, data_size, 0) <= 0) { 516 syslog(LOG_ERR, "Send failed: %s (%d)", 517 strerror(errno), errno); 518 exit(1); 519 } 520 521 if (num_frames && delay && count && !(seq % count)) 522 usleep(delay); 523 } 524 } 525 526 static void send_mode(int sk) 527 { 528 do_send(sk); 529 530 syslog(LOG_INFO, "Closing channel ..."); 531 if (shutdown(sk, SHUT_RDWR) < 0) 532 syslog(LOG_INFO, "Close failed: %m"); 533 else 534 syslog(LOG_INFO, "Done"); 535 } 536 537 static void reconnect_mode(char *svr) 538 { 539 while(1) { 540 int sk = do_connect(svr); 541 close(sk); 542 } 543 } 544 545 static void multi_connect_mode(int argc, char *argv[]) 546 { 547 int i, n, sk; 548 549 while (1) { 550 for (n = 0; n < argc; n++) { 551 for (i = 0; i < count; i++) { 552 if (fork()) 553 continue; 554 555 /* Child */ 556 sk = do_connect(argv[n]); 557 usleep(500); 558 close(sk); 559 exit(0); 560 } 561 } 562 sleep(4); 563 } 564 } 565 566 static void usage(void) 567 { 568 printf("rctest - RFCOMM testing\n" 569 "Usage:\n"); 570 printf("\trctest <mode> [options] [bdaddr]\n"); 571 printf("Modes:\n" 572 "\t-r listen and receive\n" 573 "\t-w listen and send\n" 574 "\t-d listen and dump incoming data\n" 575 "\t-s connect and send\n" 576 "\t-u connect and receive\n" 577 "\t-n connect and be silent\n" 578 "\t-c connect, disconnect, connect, ...\n" 579 "\t-m multiple connects\n"); 580 581 printf("Options:\n" 582 "\t[-b bytes] [-i device] [-P channel] [-U uuid]\n" 583 "\t[-L seconds] enabled SO_LINGER option\n" 584 "\t[-W seconds] enable deferred setup\n" 585 "\t[-B filename] use data packets from file\n" 586 "\t[-N num] number of frames to send\n" 587 "\t[-C num] send num frames before delay (default = 1)\n" 588 "\t[-D milliseconds] delay after sending num frames (default = 0)\n" 589 "\t[-A] request authentication\n" 590 "\t[-E] request encryption\n" 591 "\t[-S] secure connection\n" 592 "\t[-M] become master\n" 593 "\t[-T] enable timestamps\n"); 594 } 595 596 int main(int argc, char *argv[]) 597 { 598 struct sigaction sa; 599 int opt, sk, mode = RECV, need_addr = 0; 600 601 bacpy(&bdaddr, BDADDR_ANY); 602 603 while ((opt=getopt(argc,argv,"rdscuwmnb:i:P:U:B:N:MAESL:W:C:D:T")) != EOF) { 604 switch (opt) { 605 case 'r': 606 mode = RECV; 607 break; 608 609 case 's': 610 mode = SEND; 611 need_addr = 1; 612 break; 613 614 case 'w': 615 mode = LSEND; 616 break; 617 618 case 'u': 619 mode = CRECV; 620 need_addr = 1; 621 break; 622 623 case 'd': 624 mode = DUMP; 625 break; 626 627 case 'c': 628 mode = RECONNECT; 629 need_addr = 1; 630 break; 631 632 case 'n': 633 mode = CONNECT; 634 need_addr = 1; 635 break; 636 637 case 'm': 638 mode = MULTY; 639 need_addr = 1; 640 break; 641 642 case 'b': 643 data_size = atoi(optarg); 644 break; 645 646 case 'i': 647 if (!strncasecmp(optarg, "hci", 3)) 648 hci_devba(atoi(optarg + 3), &bdaddr); 649 else 650 str2ba(optarg, &bdaddr); 651 break; 652 653 case 'P': 654 channel = atoi(optarg); 655 break; 656 657 case 'U': 658 if (!strcasecmp(optarg, "spp")) 659 uuid = SERIAL_PORT_SVCLASS_ID; 660 else if (!strncasecmp(optarg, "0x", 2)) 661 uuid = strtoul(optarg + 2, NULL, 16); 662 else 663 uuid = atoi(optarg); 664 break; 665 666 case 'M': 667 master = 1; 668 break; 669 670 case 'A': 671 auth = 1; 672 break; 673 674 case 'E': 675 encrypt = 1; 676 break; 677 678 case 'S': 679 secure = 1; 680 break; 681 682 case 'L': 683 linger = atoi(optarg); 684 break; 685 686 case 'W': 687 defer_setup = atoi(optarg); 688 break; 689 690 case 'B': 691 filename = strdup(optarg); 692 break; 693 694 case 'N': 695 num_frames = atoi(optarg); 696 break; 697 698 case 'C': 699 count = atoi(optarg); 700 break; 701 702 case 'D': 703 delay = atoi(optarg) * 1000; 704 break; 705 706 case 'T': 707 timestamp = 1; 708 break; 709 710 default: 711 usage(); 712 exit(1); 713 } 714 } 715 716 if (need_addr && !(argc - optind)) { 717 usage(); 718 exit(1); 719 } 720 721 if (!(buf = malloc(data_size))) { 722 perror("Can't allocate data buffer"); 723 exit(1); 724 } 725 726 memset(&sa, 0, sizeof(sa)); 727 sa.sa_handler = SIG_IGN; 728 sa.sa_flags = SA_NOCLDSTOP; 729 sigaction(SIGCHLD, &sa, NULL); 730 731 openlog("rctest", LOG_PERROR | LOG_PID, LOG_LOCAL0); 732 733 switch (mode) { 734 case RECV: 735 do_listen(recv_mode); 736 break; 737 738 case CRECV: 739 sk = do_connect(argv[optind]); 740 if (sk < 0) 741 exit(1); 742 recv_mode(sk); 743 break; 744 745 case DUMP: 746 do_listen(dump_mode); 747 break; 748 749 case SEND: 750 sk = do_connect(argv[optind]); 751 if (sk < 0) 752 exit(1); 753 send_mode(sk); 754 break; 755 756 case LSEND: 757 do_listen(send_mode); 758 break; 759 760 case RECONNECT: 761 reconnect_mode(argv[optind]); 762 break; 763 764 case MULTY: 765 multi_connect_mode(argc - optind, argv + optind); 766 break; 767 768 case CONNECT: 769 sk = do_connect(argv[optind]); 770 if (sk < 0) 771 exit(1); 772 dump_mode(sk); 773 break; 774 } 775 776 syslog(LOG_INFO, "Exit"); 777 778 closelog(); 779 780 return 0; 781 } 782
