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