test/rctest.c

   1 /*
   2  *
   3  *  BlueZ - Bluetooth protocol stack for Linux
   4  *
   5  *  Copyright (C) 2002-2003  Maxim Krasnyansky <maxk@qualcomm.com>
   6  *  Copyright (C) 2002-2008  Marcel Holtmann <marcel@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
  47 /* Test modes */
  48 enum {
  49         SEND,
  50         RECV,
  51         RECONNECT,
  52         MULTY,
  53         DUMP,
  54         CONNECT,
  55         CRECV,
  56         LSEND
  57 };
  58
  59 static unsigned char *buf;
  60
  61 /* Default data size */
  62 static long data_size = 127;
  63 static long num_frames = -1;
  64
  65 /* Default number of consecutive frames before the delay */
  66 static int count = 1;
  67
  68 /* Default delay after sending count number of frames */
  69 static unsigned long delay = 0;
  70
  71 /* Default addr and channel */
  72 static bdaddr_t bdaddr;
  73 static uint8_t channel = 10;
  74
  75 static char *filename = NULL;
  76
  77 static int master = 0;
  78 static int auth = 0;
  79 static int encrypt = 0;
  80 static int secure = 0;
  81 static int socktype = SOCK_STREAM;
  82 static int linger = 0;
  83 static int timestamp = 0;
  84
  85 static float tv2fl(struct timeval tv)
  86 {
  87         return (float)tv.tv_sec + (float)(tv.tv_usec/1000000.0);
  88 }
  89
  90 static int do_connect(char *svr)
  91 {
  92         struct sockaddr_rc addr;
  93         struct rfcomm_conninfo conn;
  94         socklen_t optlen;
  95         int sk, opt;
  96
  97         /* Create socket */
  98         sk = socket(PF_BLUETOOTH, socktype, BTPROTO_RFCOMM);
  99         if (sk < 0) {
 100                 syslog(LOG_ERR, "Can't create socket: %s (%d)",
 101                                                         strerror(errno), errno);
 102                 return -1;
 103         }
 104
 105         /* Bind to local address */
 106         memset(&addr, 0, sizeof(addr));
 107         addr.rc_family = AF_BLUETOOTH;
 108         bacpy(&addr.rc_bdaddr, &bdaddr);
 109
 110         if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
 111                 syslog(LOG_ERR, "Can't bind socket: %s (%d)",
 112                                                         strerror(errno), errno);
 113                 goto error;
 114         }
 115
 116 #if 0
 117         /* Enable SO_TIMESTAMP */
 118         if (timestamp) {
 119                 int t = 1;
 120
 121                 if (setsockopt(sk, SOL_SOCKET, SO_TIMESTAMP, &t, sizeof(t)) < 0) {
 122                         syslog(LOG_ERR, "Can't enable SO_TIMESTAMP: %s (%d)",
 123                                                         strerror(errno), errno);
 124                         goto error;
 125                 }
 126         }
 127 #endif
 128
 129         /* Enable SO_LINGER */
 130         if (linger) {
 131                 struct linger l = { .l_onoff = 1, .l_linger = linger };
 132
 133                 if (setsockopt(sk, SOL_SOCKET, SO_LINGER, &l, sizeof(l)) < 0) {
 134                         syslog(LOG_ERR, "Can't enable SO_LINGER: %s (%d)",
 135                                                         strerror(errno), errno);
 136                         goto error;
 137                 }
 138         }
 139
 140         /* Set link mode */
 141         opt = 0;
 142         if (master)
 143                 opt |= RFCOMM_LM_MASTER;
 144         if (auth)
 145                 opt |= RFCOMM_LM_AUTH;
 146         if (encrypt)
 147                 opt |= RFCOMM_LM_ENCRYPT;
 148         if (secure)
 149                 opt |= RFCOMM_LM_SECURE;
 150
 151         if (opt && setsockopt(sk, SOL_RFCOMM, RFCOMM_LM, &opt, sizeof(opt)) < 0) {
 152                 syslog(LOG_ERR, "Can't set RFCOMM link mode: %s (%d)",
 153                                                         strerror(errno), errno);
 154                 goto error;
 155         }
 156
 157         /* Connect to remote device */
 158         memset(&addr, 0, sizeof(addr));
 159         addr.rc_family = AF_BLUETOOTH;
 160         str2ba(svr, &addr.rc_bdaddr);
 161         addr.rc_channel = channel;
 162
 163         if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
 164                 syslog(LOG_ERR, "Can't connect: %s (%d)",
 165                                                         strerror(errno), errno);
 166                 goto error;
 167         }
 168
 169         /* Get connection information */
 170         memset(&conn, 0, sizeof(conn));
 171         optlen = sizeof(conn);
 172
 173         if (getsockopt(sk, SOL_RFCOMM, RFCOMM_CONNINFO, &conn, &optlen) < 0) {
 174                 syslog(LOG_ERR, "Can't get RFCOMM connection information: %s (%d)",
 175                                                         strerror(errno), errno);
 176                 //goto error;
 177         }
 178
 179         syslog(LOG_INFO, "Connected [handle %d, class 0x%02x%02x%02x]",
 180                 conn.hci_handle,
 181                 conn.dev_class[2], conn.dev_class[1], conn.dev_class[0]);
 182
 183         return sk;
 184
 185 error:
 186         close(sk);
 187         return -1;
 188 }
 189
 190 static void do_listen(void (*handler)(int sk))
 191 {
 192         struct sockaddr_rc addr;
 193         struct rfcomm_conninfo conn;
 194         socklen_t optlen;
 195         int sk, nsk, opt;
 196         char ba[18];
 197
 198         /* Create socket */
 199         sk = socket(PF_BLUETOOTH, socktype, BTPROTO_RFCOMM);
 200         if (sk < 0) {
 201                 syslog(LOG_ERR, "Can't create socket: %s (%d)",
 202                                                         strerror(errno), errno);
 203                 exit(1);
 204         }
 205
 206         /* Bind to local address */
 207         addr.rc_family = AF_BLUETOOTH;
 208         bacpy(&addr.rc_bdaddr, &bdaddr);
 209         addr.rc_channel = channel;
 210
 211         if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
 212                 syslog(LOG_ERR, "Can't bind socket: %s (%d)",
 213                                                         strerror(errno), errno);
 214                 goto error;
 215         }
 216
 217         /* Set link mode */
 218         opt = 0;
 219         if (master)
 220                 opt |= RFCOMM_LM_MASTER;
 221         if (auth)
 222                 opt |= RFCOMM_LM_AUTH;
 223         if (encrypt)
 224                 opt |= RFCOMM_LM_ENCRYPT;
 225         if (secure)
 226                 opt |= RFCOMM_LM_SECURE;
 227
 228         if (opt && setsockopt(sk, SOL_RFCOMM, RFCOMM_LM, &opt, sizeof(opt)) < 0) {
 229                 syslog(LOG_ERR, "Can't set RFCOMM link mode: %s (%d)",
 230                                                         strerror(errno), errno);
 231                 goto error;
 232         }
 233
 234         /* Listen for connections */
 235         if (listen(sk, 10)) {
 236                 syslog(LOG_ERR,"Can not listen on the socket: %s (%d)",
 237                                                         strerror(errno), errno);
 238                 goto error;
 239         }
 240
 241         /* Check for socket address */
 242         memset(&addr, 0, sizeof(addr));
 243         optlen = sizeof(addr);
 244
 245         if (getsockname(sk, (struct sockaddr *) &addr, &optlen) < 0) {
 246                 syslog(LOG_ERR, "Can't get socket name: %s (%d)",
 247                                                         strerror(errno), errno);
 248                 goto error;
 249         }
 250
 251         channel = addr.rc_channel;
 252
 253         syslog(LOG_INFO, "Waiting for connection on channel %d ...", channel);
 254
 255         while(1) {
 256                 memset(&addr, 0, sizeof(addr));
 257                 optlen = sizeof(addr);
 258
 259                 nsk = accept(sk, (struct sockaddr *) &addr, &optlen);
 260                 if (nsk < 0) {
 261                         syslog(LOG_ERR,"Accept failed: %s (%d)",
 262                                                         strerror(errno), errno);
 263                         goto error;
 264                 }
 265                 if (fork()) {
 266                         /* Parent */
 267                         close(nsk);
 268                         continue;
 269                 }
 270                 /* Child */
 271                 close(sk);
 272
 273                 /* Get connection information */
 274                 memset(&conn, 0, sizeof(conn));
 275                 optlen = sizeof(conn);
 276
 277                 if (getsockopt(nsk, SOL_RFCOMM, RFCOMM_CONNINFO, &conn, &optlen) < 0) {
 278                         syslog(LOG_ERR, "Can't get RFCOMM connection information: %s (%d)",
 279                                                         strerror(errno), errno);
 280                         //close(nsk);
 281                         //goto error;
 282                 }
 283
 284                 ba2str(&addr.rc_bdaddr, ba);
 285                 syslog(LOG_INFO, "Connect from %s [handle %d, class 0x%02x%02x%02x]",
 286                         ba, conn.hci_handle,
 287                         conn.dev_class[2], conn.dev_class[1], conn.dev_class[0]);
 288
 289 #if 0
 290                 /* Enable SO_TIMESTAMP */
 291                 if (timestamp) {
 292                         int t = 1;
 293
 294                         if (setsockopt(nsk, SOL_SOCKET, SO_TIMESTAMP, &t, sizeof(t)) < 0) {
 295                                 syslog(LOG_ERR, "Can't enable SO_TIMESTAMP: %s (%d)",
 296                                                         strerror(errno), errno);
 297                                 goto error;
 298                         }
 299                 }
 300 #endif
 301
 302                 /* Enable SO_LINGER */
 303                 if (linger) {
 304                         struct linger l = { .l_onoff = 1, .l_linger = linger };
 305
 306                         if (setsockopt(nsk, SOL_SOCKET, SO_LINGER, &l, sizeof(l)) < 0) {
 307                                 syslog(LOG_ERR, "Can't enable SO_LINGER: %s (%d)",
 308                                                         strerror(errno), errno);
 309                                 close(nsk);
 310                                 goto error;
 311                         }
 312                 }
 313
 314                 handler(nsk);
 315
 316                 syslog(LOG_INFO, "Disconnect: %m");
 317                 exit(0);
 318         }
 319
 320         return;
 321
 322 error:
 323         close(sk);
 324         exit(1);
 325 }
 326
 327 static void dump_mode(int sk)
 328 {
 329         int len;
 330
 331         syslog(LOG_INFO, "Receiving ...");
 332         while ((len = read(sk, buf, data_size)) > 0)
 333                 syslog(LOG_INFO, "Recevied %d bytes", len);
 334 }
 335
 336 static void recv_mode(int sk)
 337 {
 338         struct timeval tv_beg, tv_end, tv_diff;
 339         char ts[30];
 340         long total;
 341         uint32_t seq;
 342
 343         syslog(LOG_INFO, "Receiving ...");
 344
 345         memset(ts, 0, sizeof(ts));
 346
 347         seq = 0;
 348         while (1) {
 349                 gettimeofday(&tv_beg,NULL);
 350                 total = 0;
 351                 while (total < data_size) {
 352                         //uint32_t sq;
 353                         //uint16_t l;
 354                         int r;
 355
 356                         if ((r = recv(sk, buf, data_size, 0)) <= 0) {
 357                                 if (r < 0)
 358                                         syslog(LOG_ERR, "Read failed: %s (%d)",
 359                                                         strerror(errno), errno);
 360                                 return;
 361                         }
 362
 363                         if (timestamp) {
 364                                 struct timeval tv;
 365
 366                                 if (ioctl(sk, SIOCGSTAMP, &tv) < 0) {
 367                                         timestamp = 0;
 368                                         memset(ts, 0, sizeof(ts));
 369                                 } else {
 370                                         sprintf(ts, "[%ld.%ld] ",
 371                                                         tv.tv_sec, tv.tv_usec);
 372                                 }
 373                         }
 374
 375 #if 0
 376                         /* Check sequence */
 377                         sq = btohl(*(uint32_t *) buf);
 378                         if (seq != sq) {
 379                                 syslog(LOG_INFO, "seq missmatch: %d -> %d", seq, sq);
 380                                 seq = sq;
 381                         }
 382                         seq++;
 383
 384                         /* Check length */
 385                         l = btohs(*(uint16_t *) (buf + 4));
 386                         if (r != l) {
 387                                 syslog(LOG_INFO, "size missmatch: %d -> %d", r, l);
 388                                 continue;
 389                         }
 390
 391                         /* Verify data */
 392                         for (i = 6; i < r; i++) {
 393                                 if (buf[i] != 0x7f)
 394                                         syslog(LOG_INFO, "data missmatch: byte %d 0x%2.2x", i, buf[i]);
 395                         }
 396 #endif
 397                         total += r;
 398                 }
 399                 gettimeofday(&tv_end,NULL);
 400
 401                 timersub(&tv_end,&tv_beg,&tv_diff);
 402
 403                 syslog(LOG_INFO,"%s%ld bytes in %.2f sec, %.2f kB/s", ts, total,
 404                         tv2fl(tv_diff), (float)(total / tv2fl(tv_diff) ) / 1024.0);
 405         }
 406 }
 407
 408 static void do_send(int sk)
 409 {
 410         uint32_t seq;
 411         int i, fd, len;
 412
 413         syslog(LOG_INFO,"Sending ...");
 414
 415         if (filename) {
 416                 fd = open(filename, O_RDONLY);
 417                 if (fd < 0) {
 418                         syslog(LOG_ERR, "Open failed: %s (%d)",
 419                                                         strerror(errno), errno);
 420                         exit(1);
 421                 }
 422                 len = read(fd, buf, data_size);
 423                 send(sk, buf, len, 0);
 424                 return;
 425         } else {
 426                 for (i = 6; i < data_size; i++)
 427                         buf[i] = 0x7f;
 428         }
 429
 430         seq = 0;
 431         while ((num_frames == -1) || (num_frames-- > 0)) {
 432                 *(uint32_t *) buf = htobl(seq);
 433                 *(uint16_t *) (buf + 4) = htobs(data_size);
 434                 seq++;
 435
 436                 if (send(sk, buf, data_size, 0) <= 0) {
 437                         syslog(LOG_ERR, "Send failed: %s (%d)",
 438                                                         strerror(errno), errno);
 439                         exit(1);
 440                 }
 441
 442                 if (num_frames && delay && count && !(seq % count))
 443                         usleep(delay);
 444         }
 445 }
 446
 447 static void send_mode(int sk)
 448 {
 449         do_send(sk);
 450
 451         syslog(LOG_INFO, "Closing channel ...");
 452         if (shutdown(sk, SHUT_RDWR) < 0)
 453                 syslog(LOG_INFO, "Close failed: %m");
 454         else
 455                 syslog(LOG_INFO, "Done");
 456 }
 457
 458 static void reconnect_mode(char *svr)
 459 {
 460         while(1) {
 461                 int sk = do_connect(svr);
 462                 close(sk);
 463         }
 464 }
 465
 466 static void multi_connect_mode(int argc, char *argv[])
 467 {
 468         int i, n, sk;
 469
 470         while (1) {
 471                 for (n = 0; n < argc; n++) {
 472                         for (i = 0; i < count; i++) {
 473                                 if (fork())
 474                                         continue;
 475
 476                                 /* Child */
 477                                 sk = do_connect(argv[n]);
 478                                 usleep(500);
 479                                 close(sk);
 480                                 exit(0);
 481                         }
 482                 }
 483                 sleep(4);
 484         }
 485 }
 486
 487 static void usage(void)
 488 {
 489         printf("rctest - RFCOMM testing\n"
 490                 "Usage:\n");
 491         printf("\trctest <mode> [options] [bdaddr]\n");
 492         printf("Modes:\n"
 493                 "\t-r listen and receive\n"
 494                 "\t-w listen and send\n"
 495                 "\t-d listen and dump incoming data\n"
 496                 "\t-s connect and send\n"
 497                 "\t-u connect and receive\n"
 498                 "\t-n connect and be silent\n"
 499                 "\t-c connect, disconnect, connect, ...\n"
 500                 "\t-m multiple connects\n");
 501
 502         printf("Options:\n"
 503                 "\t[-b bytes] [-i device] [-P channel]\n"
 504                 "\t[-L seconds] enabled SO_LINGER option\n"
 505                 "\t[-B filename] use data packets from file\n"
 506                 "\t[-N num] number of frames to send\n"
 507                 "\t[-C num] send num frames before delay (default = 1)\n"
 508                 "\t[-D milliseconds] delay after sending num frames (default = 0)\n"
 509                 "\t[-A] request authentication\n"
 510                 "\t[-E] request encryption\n"
 511                 "\t[-S] secure connection\n"
 512                 "\t[-M] become master\n"
 513                 "\t[-T] enable timestamps\n");
 514 }
 515
 516 int main(int argc, char *argv[])
 517 {
 518         struct sigaction sa;
 519         int opt, sk, mode = RECV, need_addr = 0;
 520
 521         bacpy(&bdaddr, BDADDR_ANY);
 522
 523         while ((opt=getopt(argc,argv,"rdscuwmnb:i:P:B:N:MAESL:C:D:T")) != EOF) {
 524                 switch (opt) {
 525                 case 'r':
 526                         mode = RECV;
 527                         break;
 528
 529                 case 's':
 530                         mode = SEND;
 531                         need_addr = 1;
 532                         break;
 533
 534                 case 'w':
 535                         mode = LSEND;
 536                         break;
 537
 538                 case 'u':
 539                         mode = CRECV;
 540                         need_addr = 1;
 541                         break;
 542
 543                 case 'd':
 544                         mode = DUMP;
 545                         break;
 546
 547                 case 'c':
 548                         mode = RECONNECT;
 549                         need_addr = 1;
 550                         break;
 551
 552                 case 'n':
 553                         mode = CONNECT;
 554                         need_addr = 1;
 555                         break;
 556
 557                 case 'm':
 558                         mode = MULTY;
 559                         need_addr = 1;
 560                         break;
 561
 562                 case 'b':
 563                         data_size = atoi(optarg);
 564                         break;
 565
 566                 case 'i':
 567                         if (!strncasecmp(optarg, "hci", 3))
 568                                 hci_devba(atoi(optarg + 3), &bdaddr);
 569                         else
 570                                 str2ba(optarg, &bdaddr);
 571                         break;
 572
 573                 case 'P':
 574                         channel = atoi(optarg);
 575                         break;
 576
 577                 case 'M':
 578                         master = 1;
 579                         break;
 580
 581                 case 'A':
 582                         auth = 1;
 583                         break;
 584
 585                 case 'E':
 586                         encrypt = 1;
 587                         break;
 588
 589                 case 'S':
 590                         secure = 1;
 591                         break;
 592
 593                 case 'L':
 594                         linger = atoi(optarg);
 595                         break;
 596
 597                 case 'B':
 598                         filename = strdup(optarg);
 599                         break;
 600
 601                 case 'N':
 602                         num_frames = atoi(optarg);
 603                         break;
 604
 605                 case 'C':
 606                         count = atoi(optarg);
 607                         break;
 608
 609                 case 'D':
 610                         delay = atoi(optarg) * 1000;
 611                         break;
 612
 613                 case 'T':
 614                         timestamp = 1;
 615                         break;
 616
 617                 default:
 618                         usage();
 619                         exit(1);
 620                 }
 621         }
 622
 623         if (need_addr && !(argc - optind)) {
 624                 usage();
 625                 exit(1);
 626         }
 627
 628         if (!(buf = malloc(data_size))) {
 629                 perror("Can't allocate data buffer");
 630                 exit(1);
 631         }
 632
 633         memset(&sa, 0, sizeof(sa));
 634         sa.sa_handler = SIG_IGN;
 635         sa.sa_flags   = SA_NOCLDSTOP;
 636         sigaction(SIGCHLD, &sa, NULL);
 637
 638         openlog("rctest", LOG_PERROR | LOG_PID, LOG_LOCAL0);
 639
 640         switch (mode) {
 641                 case RECV:
 642                         do_listen(recv_mode);
 643                         break;
 644
 645                 case CRECV:
 646                         sk = do_connect(argv[optind]);
 647                         if (sk < 0)
 648                                 exit(1);
 649                         recv_mode(sk);
 650                         break;
 651
 652                 case DUMP:
 653                         do_listen(dump_mode);
 654                         break;
 655
 656                 case SEND:
 657                         sk = do_connect(argv[optind]);
 658                         if (sk < 0)
 659                                 exit(1);
 660                         send_mode(sk);
 661                         break;
 662
 663                 case LSEND:
 664                         do_listen(send_mode);
 665                         break;
 666
 667                 case RECONNECT:
 668                         reconnect_mode(argv[optind]);
 669                         break;
 670
 671                 case MULTY:
 672                         multi_connect_mode(argc - optind, argv + optind);
 673                         break;
 674
 675                 case CONNECT:
 676                         sk = do_connect(argv[optind]);
 677                         if (sk < 0)
 678                                 exit(1);
 679                         dump_mode(sk);
 680                         break;
 681         }
 682
 683         syslog(LOG_INFO, "Exit");
 684
 685         closelog();
 686
 687         return 0;
 688 }