1/* 2 * 3 * BlueZ - Bluetooth protocol stack for Linux 4 * 5 * Copyright (C) 2000-2001 Qualcomm Incorporated 6 * Copyright (C) 2002-2003 Maxim Krasnyansky <maxk@qualcomm.com> 7 * Copyright (C) 2002-2010 Marcel Holtmann <marcel@holtmann.org> 8 * 9 * 10 * This program is free software; you can redistribute it and/or modify 11 * it under the terms of the GNU General Public License as published by 12 * the Free Software Foundation; either version 2 of the License, or 13 * (at your option) any later version. 14 * 15 * This program is distributed in the hope that it will be useful, 16 * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 * GNU General Public License for more details. 19 * 20 * You should have received a copy of the GNU General Public License 21 * along with this program; if not, write to the Free Software 22 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 23 * 24 */ 25 26#ifdef HAVE_CONFIG_H 27#include <config.h> 28#endif 29 30#include <stdio.h> 31#include <errno.h> 32#include <ctype.h> 33#include <fcntl.h> 34#include <unistd.h> 35#include <stdlib.h> 36#include <getopt.h> 37#include <syslog.h> 38#include <signal.h> 39#include <sys/time.h> 40#include <sys/poll.h> 41#include <sys/ioctl.h> 42#include <sys/socket.h> 43 44#include <bluetooth/bluetooth.h> 45#include <bluetooth/hci.h> 46#include <bluetooth/hci_lib.h> 47#include <bluetooth/l2cap.h> 48 49#define NIBBLE_TO_ASCII(c) ((c) < 0x0a ? (c) + 0x30 : (c) + 0x57) 50 51/* Test modes */ 52enum { 53 SEND, 54 RECV, 55 RECONNECT, 56 MULTY, 57 DUMP, 58 CONNECT, 59 CRECV, 60 LSEND, 61 SENDDUMP, 62 LSENDDUMP, 63 LSENDRECV, 64 CSENDRECV, 65 INFOREQ, 66 PAIRING, 67}; 68 69static unsigned char *buf; 70 71/* Default mtu */ 72static int imtu = 672; 73static int omtu = 0; 74 75/* Default FCS option */ 76static int fcs = 0x01; 77 78/* Default Transmission Window */ 79static int txwin_size = 63; 80 81/* Default Max Transmission */ 82static int max_transmit = 3; 83 84/* Default data size */ 85static long data_size = -1; 86static long buffer_size = 2048; 87 88/* Default addr and psm */ 89static bdaddr_t bdaddr; 90static unsigned short psm = 10; 91 92/* Default number of frames to send (-1 = infinite) */ 93static int num_frames = -1; 94 95/* Default number of consecutive frames before the delay */ 96static int count = 1; 97 98/* Default delay after sending count number of frames */ 99static unsigned long delay = 0; 100 101static char *filename = NULL; 102 103static int rfcmode = 0; 104static int master = 0; 105static int auth = 0; 106static int encrypt = 0; 107static int secure = 0; 108static int socktype = SOCK_SEQPACKET; 109static int linger = 0; 110static int reliable = 0; 111static int timestamp = 0; 112static int defer_setup = 0; 113 114static float tv2fl(struct timeval tv) 115{ 116 return (float)tv.tv_sec + (float)(tv.tv_usec/1000000.0); 117} 118 119static char *ltoh(unsigned long c, char* s) 120{ 121 int c1; 122 123 c1 = (c >> 28) & 0x0f; 124 *(s++) = NIBBLE_TO_ASCII (c1); 125 c1 = (c >> 24) & 0x0f; 126 *(s++) = NIBBLE_TO_ASCII (c1); 127 c1 = (c >> 20) & 0x0f; 128 *(s++) = NIBBLE_TO_ASCII (c1); 129 c1 = (c >> 16) & 0x0f; 130 *(s++) = NIBBLE_TO_ASCII (c1); 131 c1 = (c >> 12) & 0x0f; 132 *(s++) = NIBBLE_TO_ASCII (c1); 133 c1 = (c >> 8) & 0x0f; 134 *(s++) = NIBBLE_TO_ASCII (c1); 135 c1 = (c >> 4) & 0x0f; 136 *(s++) = NIBBLE_TO_ASCII (c1); 137 c1 = c & 0x0f; 138 *(s++) = NIBBLE_TO_ASCII (c1); 139 *s = 0; 140 return s; 141} 142 143static char *ctoh(char c, char* s) 144{ 145 char c1; 146 147 c1 = (c >> 4) & 0x0f; 148 *(s++) = NIBBLE_TO_ASCII (c1); 149 c1 = c & 0x0f; 150 *(s++) = NIBBLE_TO_ASCII (c1); 151 *s = 0; 152 return s; 153} 154 155static void hexdump(unsigned char *s, unsigned long l) 156{ 157 char bfr[80]; 158 char *pb; 159 unsigned long i, n = 0; 160 161 if (l == 0) 162 return; 163 164 while (n < l) { 165 pb = bfr; 166 pb = ltoh (n, pb); 167 *(pb++) = ':'; 168 *(pb++) = ' '; 169 for (i = 0; i < 16; i++) { 170 if (n + i >= l) { 171 *(pb++) = ' '; 172 *(pb++) = ' '; 173 } else 174 pb = ctoh (*(s + i), pb); 175 *(pb++) = ' '; 176 } 177 *(pb++) = ' '; 178 for (i = 0; i < 16; i++) { 179 if (n + i >= l) 180 break; 181 else 182 *(pb++) = (isprint (*(s + i)) ? *(s + i) : '.'); 183 } 184 *pb = 0; 185 n += 16; 186 s += 16; 187 puts(bfr); 188 } 189} 190 191static int do_connect(char *svr) 192{ 193 struct sockaddr_l2 addr; 194 struct l2cap_options opts; 195 struct l2cap_conninfo conn; 196 socklen_t optlen; 197 int sk, opt; 198 199 /* Create socket */ 200 sk = socket(PF_BLUETOOTH, socktype, BTPROTO_L2CAP); 201 if (sk < 0) { 202 syslog(LOG_ERR, "Can't create socket: %s (%d)", 203 strerror(errno), errno); 204 return -1; 205 } 206 207 /* Bind to local address */ 208 memset(&addr, 0, sizeof(addr)); 209 addr.l2_family = AF_BLUETOOTH; 210 bacpy(&addr.l2_bdaddr, &bdaddr); 211 212 if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) { 213 syslog(LOG_ERR, "Can't bind socket: %s (%d)", 214 strerror(errno), errno); 215 goto error; 216 } 217 218 /* Get default options */ 219 memset(&opts, 0, sizeof(opts)); 220 optlen = sizeof(opts); 221 222 if (getsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, &optlen) < 0) { 223 syslog(LOG_ERR, "Can't get default L2CAP options: %s (%d)", 224 strerror(errno), errno); 225 goto error; 226 } 227 228 /* Set new options */ 229 opts.omtu = omtu; 230 opts.imtu = imtu; 231 opts.mode = rfcmode; 232 233 opts.fcs = fcs; 234 opts.txwin_size = txwin_size; 235 opts.max_tx = max_transmit; 236 237 if (setsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, sizeof(opts)) < 0) { 238 syslog(LOG_ERR, "Can't set L2CAP options: %s (%d)", 239 strerror(errno), errno); 240 goto error; 241 } 242 243#if 0 244 /* Enable SO_TIMESTAMP */ 245 if (timestamp) { 246 int t = 1; 247 248 if (setsockopt(sk, SOL_SOCKET, SO_TIMESTAMP, &t, sizeof(t)) < 0) { 249 syslog(LOG_ERR, "Can't enable SO_TIMESTAMP: %s (%d)", 250 strerror(errno), errno); 251 goto error; 252 } 253 } 254#endif 255 256 /* Enable SO_LINGER */ 257 if (linger) { 258 struct linger l = { .l_onoff = 1, .l_linger = linger }; 259 260 if (setsockopt(sk, SOL_SOCKET, SO_LINGER, &l, sizeof(l)) < 0) { 261 syslog(LOG_ERR, "Can't enable SO_LINGER: %s (%d)", 262 strerror(errno), errno); 263 goto error; 264 } 265 } 266 267 /* Set link mode */ 268 opt = 0; 269 if (reliable) 270 opt |= L2CAP_LM_RELIABLE; 271 if (master) 272 opt |= L2CAP_LM_MASTER; 273 if (auth) 274 opt |= L2CAP_LM_AUTH; 275 if (encrypt) 276 opt |= L2CAP_LM_ENCRYPT; 277 if (secure) 278 opt |= L2CAP_LM_SECURE; 279 280 if (setsockopt(sk, SOL_L2CAP, L2CAP_LM, &opt, sizeof(opt)) < 0) { 281 syslog(LOG_ERR, "Can't set L2CAP link mode: %s (%d)", 282 strerror(errno), errno); 283 goto error; 284 } 285 286 /* Connect to remote device */ 287 memset(&addr, 0, sizeof(addr)); 288 addr.l2_family = AF_BLUETOOTH; 289 str2ba(svr, &addr.l2_bdaddr); 290 addr.l2_psm = htobs(psm); 291 292 if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0 ) { 293 syslog(LOG_ERR, "Can't connect: %s (%d)", 294 strerror(errno), errno); 295 goto error; 296 } 297 298 /* Get current options */ 299 memset(&opts, 0, sizeof(opts)); 300 optlen = sizeof(opts); 301 302 if (getsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, &optlen) < 0) { 303 syslog(LOG_ERR, "Can't get L2CAP options: %s (%d)", 304 strerror(errno), errno); 305 goto error; 306 } 307 308 /* Get connection information */ 309 memset(&conn, 0, sizeof(conn)); 310 optlen = sizeof(conn); 311 312 if (getsockopt(sk, SOL_L2CAP, L2CAP_CONNINFO, &conn, &optlen) < 0) { 313 syslog(LOG_ERR, "Can't get L2CAP connection information: %s (%d)", 314 strerror(errno), errno); 315 goto error; 316 } 317 318 syslog(LOG_INFO, "Connected [imtu %d, omtu %d, flush_to %d, " 319 "mode %d, handle %d, class 0x%02x%02x%02x]", 320 opts.imtu, opts.omtu, opts.flush_to, opts.mode, conn.hci_handle, 321 conn.dev_class[2], conn.dev_class[1], conn.dev_class[0]); 322 323 omtu = (opts.omtu > buffer_size) ? buffer_size : opts.omtu; 324 imtu = (opts.imtu > buffer_size) ? buffer_size : opts.imtu; 325 326 return sk; 327 328error: 329 close(sk); 330 return -1; 331} 332 333static void do_listen(void (*handler)(int sk)) 334{ 335 struct sockaddr_l2 addr; 336 struct l2cap_options opts; 337 struct l2cap_conninfo conn; 338 socklen_t optlen; 339 int sk, nsk, opt; 340 char ba[18]; 341 342 /* Create socket */ 343 sk = socket(PF_BLUETOOTH, socktype, BTPROTO_L2CAP); 344 if (sk < 0) { 345 syslog(LOG_ERR, "Can't create socket: %s (%d)", 346 strerror(errno), errno); 347 exit(1); 348 } 349 350 /* Bind to local address */ 351 memset(&addr, 0, sizeof(addr)); 352 addr.l2_family = AF_BLUETOOTH; 353 bacpy(&addr.l2_bdaddr, &bdaddr); 354 addr.l2_psm = htobs(psm); 355 356 if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) { 357 syslog(LOG_ERR, "Can't bind socket: %s (%d)", 358 strerror(errno), errno); 359 goto error; 360 } 361 362 /* Set link mode */ 363 opt = 0; 364 if (reliable) 365 opt |= L2CAP_LM_RELIABLE; 366 if (master) 367 opt |= L2CAP_LM_MASTER; 368 if (auth) 369 opt |= L2CAP_LM_AUTH; 370 if (encrypt) 371 opt |= L2CAP_LM_ENCRYPT; 372 if (secure) 373 opt |= L2CAP_LM_SECURE; 374 375 if (opt && setsockopt(sk, SOL_L2CAP, L2CAP_LM, &opt, sizeof(opt)) < 0) { 376 syslog(LOG_ERR, "Can't set L2CAP link mode: %s (%d)", 377 strerror(errno), errno); 378 goto error; 379 } 380 381 /* Get default options */ 382 memset(&opts, 0, sizeof(opts)); 383 optlen = sizeof(opts); 384 385 if (getsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, &optlen) < 0) { 386 syslog(LOG_ERR, "Can't get default L2CAP options: %s (%d)", 387 strerror(errno), errno); 388 goto error; 389 } 390 391 /* Set new options */ 392 opts.omtu = omtu; 393 opts.imtu = imtu; 394 if (rfcmode > 0) 395 opts.mode = rfcmode; 396 397 opts.fcs = fcs; 398 opts.txwin_size = txwin_size; 399 opts.max_tx = max_transmit; 400 401 if (setsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, sizeof(opts)) < 0) { 402 syslog(LOG_ERR, "Can't set L2CAP options: %s (%d)", 403 strerror(errno), errno); 404 goto error; 405 } 406 407 if (socktype == SOCK_DGRAM) { 408 handler(sk); 409 return; 410 } 411 412 /* Enable deferred setup */ 413 opt = defer_setup; 414 415 if (opt && setsockopt(sk, SOL_BLUETOOTH, BT_DEFER_SETUP, 416 &opt, sizeof(opt)) < 0) { 417 syslog(LOG_ERR, "Can't enable deferred setup : %s (%d)", 418 strerror(errno), errno); 419 goto error; 420 } 421 422 /* Listen for connections */ 423 if (listen(sk, 10)) { 424 syslog(LOG_ERR, "Can not listen on the socket: %s (%d)", 425 strerror(errno), errno); 426 goto error; 427 } 428 429 /* Check for socket address */ 430 memset(&addr, 0, sizeof(addr)); 431 optlen = sizeof(addr); 432 433 if (getsockname(sk, (struct sockaddr *) &addr, &optlen) < 0) { 434 syslog(LOG_ERR, "Can't get socket name: %s (%d)", 435 strerror(errno), errno); 436 goto error; 437 } 438 439 psm = btohs(addr.l2_psm); 440 441 syslog(LOG_INFO, "Waiting for connection on psm %d ...", psm); 442 443 while (1) { 444 memset(&addr, 0, sizeof(addr)); 445 optlen = sizeof(addr); 446 447 nsk = accept(sk, (struct sockaddr *) &addr, &optlen); 448 if (nsk < 0) { 449 syslog(LOG_ERR, "Accept failed: %s (%d)", 450 strerror(errno), errno); 451 goto error; 452 } 453 if (fork()) { 454 /* Parent */ 455 close(nsk); 456 continue; 457 } 458 /* Child */ 459 close(sk); 460 461 /* Get current options */ 462 memset(&opts, 0, sizeof(opts)); 463 optlen = sizeof(opts); 464 465 if (getsockopt(nsk, SOL_L2CAP, L2CAP_OPTIONS, &opts, &optlen) < 0) { 466 syslog(LOG_ERR, "Can't get L2CAP options: %s (%d)", 467 strerror(errno), errno); 468 if (!defer_setup) { 469 close(nsk); 470 goto error; 471 } 472 } 473 474 /* Get connection information */ 475 memset(&conn, 0, sizeof(conn)); 476 optlen = sizeof(conn); 477 478 if (getsockopt(nsk, SOL_L2CAP, L2CAP_CONNINFO, &conn, &optlen) < 0) { 479 syslog(LOG_ERR, "Can't get L2CAP connection information: %s (%d)", 480 strerror(errno), errno); 481 if (!defer_setup) { 482 close(nsk); 483 goto error; 484 } 485 } 486 487 ba2str(&addr.l2_bdaddr, ba); 488 syslog(LOG_INFO, "Connect from %s [imtu %d, omtu %d, flush_to %d, " 489 "mode %d, handle %d, class 0x%02x%02x%02x]", 490 ba, opts.imtu, opts.omtu, opts.flush_to, opts.mode, conn.hci_handle, 491 conn.dev_class[2], conn.dev_class[1], conn.dev_class[0]); 492 493 omtu = (opts.omtu > buffer_size) ? buffer_size : opts.omtu; 494 imtu = (opts.imtu > buffer_size) ? buffer_size : opts.imtu; 495 496#if 0 497 /* Enable SO_TIMESTAMP */ 498 if (timestamp) { 499 int t = 1; 500 501 if (setsockopt(nsk, SOL_SOCKET, SO_TIMESTAMP, &t, sizeof(t)) < 0) { 502 syslog(LOG_ERR, "Can't enable SO_TIMESTAMP: %s (%d)", 503 strerror(errno), errno); 504 goto error; 505 } 506 } 507#endif 508 509 /* Enable SO_LINGER */ 510 if (linger) { 511 struct linger l = { .l_onoff = 1, .l_linger = linger }; 512 513 if (setsockopt(nsk, SOL_SOCKET, SO_LINGER, &l, sizeof(l)) < 0) { 514 syslog(LOG_ERR, "Can't enable SO_LINGER: %s (%d)", 515 strerror(errno), errno); 516 close(nsk); 517 goto error; 518 } 519 } 520 521 /* Handle deferred setup */ 522 if (defer_setup) { 523 syslog(LOG_INFO, "Waiting for %d seconds", 524 abs(defer_setup) - 1); 525 sleep(abs(defer_setup) - 1); 526 527 if (defer_setup < 0) { 528 close(nsk); 529 goto error; 530 } 531 } 532 533 handler(nsk); 534 535 syslog(LOG_INFO, "Disconnect: %m"); 536 exit(0); 537 } 538 539 return; 540 541error: 542 close(sk); 543 exit(1); 544} 545 546static void dump_mode(int sk) 547{ 548 socklen_t optlen; 549 int opt, len; 550 551 if (data_size < 0) 552 data_size = imtu; 553 554 if (defer_setup) { 555 len = read(sk, buf, sizeof(buf)); 556 if (len < 0) 557 syslog(LOG_ERR, "Initial read error: %s (%d)", 558 strerror(errno), errno); 559 else 560 syslog(LOG_INFO, "Initial bytes %d", len); 561 } 562 563 syslog(LOG_INFO, "Receiving ..."); 564 while (1) { 565 fd_set rset; 566 567 FD_ZERO(&rset); 568 FD_SET(sk, &rset); 569 570 if (select(sk + 1, &rset, NULL, NULL, NULL) < 0) 571 return; 572 573 if (!FD_ISSET(sk, &rset)) 574 continue; 575 576 len = read(sk, buf, data_size); 577 if (len <= 0) { 578 if (len < 0) { 579 if (reliable && (errno == ECOMM)) { 580 syslog(LOG_INFO, "L2CAP Error ECOMM - clearing error and continuing."); 581 optlen = sizeof(opt); 582 if (getsockopt(sk, SOL_SOCKET, SO_ERROR, &opt, &optlen) < 0) { 583 syslog(LOG_ERR, "Couldn't getsockopt(SO_ERROR): %s (%d)", 584 strerror(errno), errno); 585 return; 586 } 587 continue; 588 } else { 589 syslog(LOG_ERR, "Read error: %s(%d)", 590 strerror(errno), errno); 591 } 592 } 593 return; 594 } 595 596 syslog(LOG_INFO, "Recevied %d bytes", len); 597 hexdump(buf, len); 598 } 599} 600 601static void recv_mode(int sk) 602{ 603 struct timeval tv_beg, tv_end, tv_diff; 604 struct pollfd p; 605 char ts[30]; 606 long total; 607 uint32_t seq; 608 socklen_t optlen; 609 int opt, len; 610 611 if (data_size < 0) 612 data_size = imtu; 613 614 if (defer_setup) { 615 len = read(sk, buf, sizeof(buf)); 616 if (len < 0) 617 syslog(LOG_ERR, "Initial read error: %s (%d)", 618 strerror(errno), errno); 619 else 620 syslog(LOG_INFO, "Initial bytes %d", len); 621 } 622 623 syslog(LOG_INFO, "Receiving ..."); 624 625 memset(ts, 0, sizeof(ts)); 626 627 p.fd = sk; 628 p.events = POLLIN | POLLERR | POLLHUP; 629 630 seq = 0; 631 while (1) { 632 gettimeofday(&tv_beg, NULL); 633 total = 0; 634 while (total < data_size) { 635 uint32_t sq; 636 uint16_t l; 637 int i; 638 639 p.revents = 0; 640 if (poll(&p, 1, -1) <= 0) 641 return; 642 643 if (p.revents & (POLLERR | POLLHUP)) 644 return; 645 646 len = recv(sk, buf, data_size, 0); 647 if (len < 0) { 648 if (reliable && (errno == ECOMM)) { 649 syslog(LOG_INFO, "L2CAP Error ECOMM - clearing error and continuing.\n"); 650 optlen = sizeof(opt); 651 if (getsockopt(sk, SOL_SOCKET, SO_ERROR, &opt, &optlen) < 0) { 652 syslog(LOG_ERR, "Couldn't getsockopt(SO_ERROR): %s (%d)", 653 strerror(errno), errno); 654 return; 655 } 656 continue; 657 } else { 658 syslog(LOG_ERR, "Read failed: %s (%d)", 659 strerror(errno), errno); 660 } 661 } 662 663 if (len < 6) 664 break; 665 666 if (timestamp) { 667 struct timeval tv; 668 669 if (ioctl(sk, SIOCGSTAMP, &tv) < 0) { 670 timestamp = 0; 671 memset(ts, 0, sizeof(ts)); 672 } else { 673 sprintf(ts, "[%ld.%ld] ", 674 tv.tv_sec, tv.tv_usec); 675 } 676 } 677 678 /* Check sequence */ 679 sq = btohl(*(uint32_t *) buf); 680 if (seq != sq) { 681 syslog(LOG_INFO, "seq missmatch: %d -> %d", seq, sq); 682 seq = sq; 683 } 684 seq++; 685 686 /* Check length */ 687 l = btohs(*(uint16_t *) (buf + 4)); 688 if (len != l) { 689 syslog(LOG_INFO, "size missmatch: %d -> %d", len, l); 690 continue; 691 } 692 693 /* Verify data */ 694 for (i = 6; i < len; i++) { 695 if (buf[i] != 0x7f) 696 syslog(LOG_INFO, "data missmatch: byte %d 0x%2.2x", i, buf[i]); 697 } 698 699 total += len; 700 } 701 gettimeofday(&tv_end, NULL); 702 703 timersub(&tv_end, &tv_beg, &tv_diff); 704 705 syslog(LOG_INFO,"%s%ld bytes in %.2f sec, %.2f kB/s", ts, total, 706 tv2fl(tv_diff), (float)(total / tv2fl(tv_diff) ) / 1024.0); 707 } 708} 709 710static void do_send(int sk) 711{ 712 uint32_t seq; 713 int i, fd, len, buflen, size, sent; 714 715 syslog(LOG_INFO, "Sending ..."); 716 717 if (data_size < 0) 718 data_size = omtu; 719 720 if (filename) { 721 fd = open(filename, O_RDONLY); 722 if (fd < 0) { 723 syslog(LOG_ERR, "Open failed: %s (%d)", 724 strerror(errno), errno); 725 exit(1); 726 } 727 728 sent = 0; 729 size = read(fd, buf, data_size); 730 while (size > 0) { 731 buflen = (size > omtu) ? omtu : size; 732 733 len = send(sk, buf + sent, buflen, 0); 734 735 sent += len; 736 size -= len; 737 } 738 return; 739 } else { 740 for (i = 6; i < data_size; i++) 741 buf[i] = 0x7f; 742 } 743 744 seq = 0; 745 while ((num_frames == -1) || (num_frames-- > 0)) { 746 *(uint32_t *) buf = htobl(seq); 747 *(uint16_t *) (buf + 4) = htobs(data_size); 748 seq++; 749 750 sent = 0; 751 size = data_size; 752 while (size > 0) { 753 buflen = (size > omtu) ? omtu : size; 754 755 len = send(sk, buf, buflen, 0); 756 if (len < 0 || len != buflen) { 757 syslog(LOG_ERR, "Send failed: %s (%d)", 758 strerror(errno), errno); 759 exit(1); 760 } 761 762 sent += len; 763 size -= len; 764 } 765 766 if (num_frames && delay && count && !(seq % count)) 767 usleep(delay); 768 } 769} 770 771static void send_mode(int sk) 772{ 773 do_send(sk); 774 775 syslog(LOG_INFO, "Closing channel ..."); 776 if (shutdown(sk, SHUT_RDWR) < 0) 777 syslog(LOG_INFO, "Close failed: %m"); 778 else 779 syslog(LOG_INFO, "Done"); 780} 781 782static void senddump_mode(int sk) 783{ 784 do_send(sk); 785 786 dump_mode(sk); 787} 788 789static void send_and_recv_mode(int sk) 790{ 791 int flags; 792 793 if ((flags = fcntl(sk, F_GETFL, 0)) < 0) 794 flags = 0; 795 fcntl(sk, F_SETFL, flags | O_NONBLOCK); 796 797 /* fork for duplex channel */ 798 if (fork()) 799 send_mode(sk); 800 else 801 recv_mode(sk); 802 return; 803} 804 805static void reconnect_mode(char *svr) 806{ 807 while (1) { 808 int sk = do_connect(svr); 809 close(sk); 810 } 811} 812 813static void connect_mode(char *svr) 814{ 815 struct pollfd p; 816 int sk; 817 818 if ((sk = do_connect(svr)) < 0) 819 exit(1); 820 821 p.fd = sk; 822 p.events = POLLERR | POLLHUP; 823 824 while (1) { 825 p.revents = 0; 826 if (poll(&p, 1, 500)) 827 break; 828 } 829 830 syslog(LOG_INFO, "Disconnected"); 831 832 close(sk); 833} 834 835static void multi_connect_mode(int argc, char *argv[]) 836{ 837 int i, n, sk; 838 839 while (1) { 840 for (n = 0; n < argc; n++) { 841 for (i = 0; i < count; i++) { 842 if (fork()) 843 continue; 844 845 /* Child */ 846 sk = do_connect(argv[n]); 847 usleep(500); 848 close(sk); 849 exit(0); 850 } 851 } 852 sleep(4); 853 } 854} 855 856static void info_request(char *svr) 857{ 858 unsigned char buf[48]; 859 l2cap_cmd_hdr *cmd = (l2cap_cmd_hdr *) buf; 860 l2cap_info_req *req = (l2cap_info_req *) (buf + L2CAP_CMD_HDR_SIZE); 861 l2cap_info_rsp *rsp = (l2cap_info_rsp *) (buf + L2CAP_CMD_HDR_SIZE); 862 uint16_t mtu; 863 uint32_t channels, mask = 0x0000; 864 struct sockaddr_l2 addr; 865 int sk, err; 866 867 sk = socket(PF_BLUETOOTH, SOCK_RAW, BTPROTO_L2CAP); 868 if (sk < 0) { 869 perror("Can't create socket"); 870 return; 871 } 872 873 memset(&addr, 0, sizeof(addr)); 874 addr.l2_family = AF_BLUETOOTH; 875 bacpy(&addr.l2_bdaddr, &bdaddr); 876 877 if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) { 878 perror("Can't bind socket"); 879 goto failed; 880 } 881 882 memset(&addr, 0, sizeof(addr)); 883 addr.l2_family = AF_BLUETOOTH; 884 str2ba(svr, &addr.l2_bdaddr); 885 886 if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0 ) { 887 perror("Can't connect socket"); 888 goto failed; 889 } 890 891 memset(buf, 0, sizeof(buf)); 892 cmd->code = L2CAP_INFO_REQ; 893 cmd->ident = 141; 894 cmd->len = htobs(2); 895 req->type = htobs(0x0001); 896 897 if (send(sk, buf, L2CAP_CMD_HDR_SIZE + L2CAP_INFO_REQ_SIZE, 0) < 0) { 898 perror("Can't send info request"); 899 goto failed; 900 } 901 902 err = recv(sk, buf, L2CAP_CMD_HDR_SIZE + L2CAP_INFO_RSP_SIZE + 2, 0); 903 if (err < 0) { 904 perror("Can't receive info response"); 905 goto failed; 906 } 907 908 switch (btohs(rsp->result)) { 909 case 0x0000: 910 memcpy(&mtu, rsp->data, sizeof(mtu)); 911 printf("Connectionless MTU size is %d\n", btohs(mtu)); 912 break; 913 case 0x0001: 914 printf("Connectionless MTU is not supported\n"); 915 break; 916 } 917 918 memset(buf, 0, sizeof(buf)); 919 cmd->code = L2CAP_INFO_REQ; 920 cmd->ident = 142; 921 cmd->len = htobs(2); 922 req->type = htobs(0x0002); 923 924 if (send(sk, buf, L2CAP_CMD_HDR_SIZE + L2CAP_INFO_REQ_SIZE, 0) < 0) { 925 perror("Can't send info request"); 926 goto failed; 927 } 928 929 err = recv(sk, buf, L2CAP_CMD_HDR_SIZE + L2CAP_INFO_RSP_SIZE + 4, 0); 930 if (err < 0) { 931 perror("Can't receive info response"); 932 goto failed; 933 } 934 935 switch (btohs(rsp->result)) { 936 case 0x0000: 937 memcpy(&mask, rsp->data, sizeof(mask)); 938 printf("Extended feature mask is 0x%04x\n", btohl(mask)); 939 if (mask & 0x01) 940 printf(" Flow control mode\n"); 941 if (mask & 0x02) 942 printf(" Retransmission mode\n"); 943 if (mask & 0x04) 944 printf(" Bi-directional QoS\n"); 945 if (mask & 0x08) 946 printf(" Enhanced Retransmission mode\n"); 947 if (mask & 0x10) 948 printf(" Streaming mode\n"); 949 if (mask & 0x20) 950 printf(" FCS Option\n"); 951 if (mask & 0x40) 952 printf(" Extended Flow Specification\n"); 953 if (mask & 0x80) 954 printf(" Fixed Channels\n"); 955 if (mask & 0x0100) 956 printf(" Extended Window Size\n"); 957 if (mask & 0x0200) 958 printf(" Unicast Connectionless Data Reception\n"); 959 break; 960 case 0x0001: 961 printf("Extended feature mask is not supported\n"); 962 break; 963 } 964 965 if (!(mask & 0x80)) 966 goto failed; 967 968 memset(buf, 0, sizeof(buf)); 969 cmd->code = L2CAP_INFO_REQ; 970 cmd->ident = 143; 971 cmd->len = htobs(2); 972 req->type = htobs(0x0003); 973 974 if (send(sk, buf, L2CAP_CMD_HDR_SIZE + L2CAP_INFO_REQ_SIZE, 0) < 0) { 975 perror("Can't send info request"); 976 goto failed; 977 } 978 979 err = recv(sk, buf, L2CAP_CMD_HDR_SIZE + L2CAP_INFO_RSP_SIZE + 8, 0); 980 if (err < 0) { 981 perror("Can't receive info response"); 982 goto failed; 983 } 984 985 switch (btohs(rsp->result)) { 986 case 0x0000: 987 memcpy(&channels, rsp->data, sizeof(channels)); 988 printf("Fixed channels list is 0x%04x\n", btohl(channels)); 989 break; 990 case 0x0001: 991 printf("Fixed channels list is not supported\n"); 992 break; 993 } 994 995failed: 996 close(sk); 997} 998 999static void do_pairing(char *svr) 1000{ 1001 struct sockaddr_l2 addr; 1002 int sk, opt; 1003 1004 sk = socket(PF_BLUETOOTH, SOCK_RAW, BTPROTO_L2CAP); 1005 if (sk < 0) { 1006 perror("Can't create socket"); 1007 return; 1008 } 1009 1010 memset(&addr, 0, sizeof(addr)); 1011 addr.l2_family = AF_BLUETOOTH; 1012 bacpy(&addr.l2_bdaddr, &bdaddr); 1013 1014 if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) { 1015 perror("Can't bind socket"); 1016 goto failed; 1017 } 1018 1019 if (secure) 1020 opt = L2CAP_LM_SECURE; 1021 else 1022 opt = L2CAP_LM_ENCRYPT; 1023 1024 if (setsockopt(sk, SOL_L2CAP, L2CAP_LM, &opt, sizeof(opt)) < 0) { 1025 perror("Can't set link mode"); 1026 goto failed; 1027 } 1028 1029 memset(&addr, 0, sizeof(addr)); 1030 addr.l2_family = AF_BLUETOOTH; 1031 str2ba(svr, &addr.l2_bdaddr); 1032 1033 if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0 ) { 1034 perror("Can't connect socket"); 1035 goto failed; 1036 } 1037 1038 printf("Pairing successful\n"); 1039 1040failed: 1041 close(sk); 1042} 1043 1044static void usage(void) 1045{ 1046 printf("l2test - L2CAP testing\n" 1047 "Usage:\n"); 1048 printf("\tl2test <mode> [options] [bdaddr]\n"); 1049 printf("Modes:\n" 1050 "\t-r listen and receive\n" 1051 "\t-w listen and send\n" 1052 "\t-d listen and dump incoming data\n" 1053 "\t-x listen, then send, then dump incoming data\n" 1054 "\t-t listen, then send and receive at the same time\n" 1055 "\t-q connect, then send and receive at the same time\n" 1056 "\t-s connect and send\n" 1057 "\t-u connect and receive\n" 1058 "\t-n connect and be silent\n" 1059 "\t-y connect, then send, then dump incoming data\n" 1060 "\t-c connect, disconnect, connect, ...\n" 1061 "\t-m multiple connects\n" 1062 "\t-p trigger dedicated bonding\n" 1063 "\t-z information request\n"); 1064 1065 printf("Options:\n" 1066 "\t[-b bytes] [-i device] [-P psm]\n" 1067 "\t[-I imtu] [-O omtu]\n" 1068 "\t[-L seconds] enable SO_LINGER\n" 1069 "\t[-W seconds] enable deferred setup\n" 1070 "\t[-B filename] use data packets from file\n" 1071 "\t[-N num] send num frames (default = infinite)\n" 1072 "\t[-C num] send num frames before delay (default = 1)\n" 1073 "\t[-D milliseconds] delay after sending num frames (default = 0)\n" 1074 "\t[-X mode] select retransmission/flow-control mode\n" 1075 "\t[-F fcs] use CRC16 check (default = 1)\n" 1076 "\t[-Q num] Max Transmit value (default = 3)\n" 1077 "\t[-Z size] Transmission Window size (default = 63)\n" 1078 "\t[-R] reliable mode\n" 1079 "\t[-G] use connectionless channel (datagram)\n" 1080 "\t[-U] use sock stream\n" 1081 "\t[-A] request authentication\n" 1082 "\t[-E] request encryption\n" 1083 "\t[-S] secure connection\n" 1084 "\t[-M] become master\n" 1085 "\t[-T] enable timestamps\n"); 1086} 1087 1088int main(int argc, char *argv[]) 1089{ 1090 struct sigaction sa; 1091 int opt, sk, mode = RECV, need_addr = 0; 1092 1093 bacpy(&bdaddr, BDADDR_ANY); 1094 1095 while ((opt=getopt(argc,argv,"rdscuwmntqxyzpb:i:P:I:O:B:N:L:W:C:D:X:F:Q:Z:RUGAESMT")) != EOF) { 1096 switch(opt) { 1097 case 'r': 1098 mode = RECV; 1099 break; 1100 1101 case 's': 1102 mode = SEND; 1103 need_addr = 1; 1104 break; 1105 1106 case 'w': 1107 mode = LSEND; 1108 break; 1109 1110 case 'u': 1111 mode = CRECV; 1112 need_addr = 1; 1113 break; 1114 1115 case 'd': 1116 mode = DUMP; 1117 break; 1118 1119 case 'c': 1120 mode = RECONNECT; 1121 need_addr = 1; 1122 break; 1123 1124 case 'n': 1125 mode = CONNECT; 1126 need_addr = 1; 1127 break; 1128 1129 case 'm': 1130 mode = MULTY; 1131 need_addr = 1; 1132 break; 1133 1134 case 't': 1135 mode = LSENDRECV; 1136 break; 1137 1138 case 'q': 1139 mode = CSENDRECV; 1140 need_addr = 1; 1141 break; 1142 1143 case 'x': 1144 mode = LSENDDUMP; 1145 break; 1146 1147 case 'y': 1148 mode = SENDDUMP; 1149 break; 1150 1151 case 'z': 1152 mode = INFOREQ; 1153 need_addr = 1; 1154 break; 1155 1156 case 'p': 1157 mode = PAIRING; 1158 need_addr = 1; 1159 break; 1160 1161 case 'b': 1162 data_size = atoi(optarg); 1163 break; 1164 1165 case 'i': 1166 if (!strncasecmp(optarg, "hci", 3)) 1167 hci_devba(atoi(optarg + 3), &bdaddr); 1168 else 1169 str2ba(optarg, &bdaddr); 1170 break; 1171 1172 case 'P': 1173 psm = atoi(optarg); 1174 break; 1175 1176 case 'I': 1177 imtu = atoi(optarg); 1178 break; 1179 1180 case 'O': 1181 omtu = atoi(optarg); 1182 break; 1183 1184 case 'L': 1185 linger = atoi(optarg); 1186 break; 1187 1188 case 'W': 1189 defer_setup = atoi(optarg); 1190 break; 1191 1192 case 'B': 1193 filename = strdup(optarg); 1194 break; 1195 1196 case 'N': 1197 num_frames = atoi(optarg); 1198 break; 1199 1200 case 'C': 1201 count = atoi(optarg); 1202 break; 1203 1204 case 'D': 1205 delay = atoi(optarg) * 1000; 1206 break; 1207 1208 case 'X': 1209 if (strcasecmp(optarg, "ertm") == 0) 1210 rfcmode = L2CAP_MODE_ERTM; 1211 else 1212 rfcmode = atoi(optarg); 1213 break; 1214 1215 case 'F': 1216 fcs = atoi(optarg); 1217 break; 1218 1219 case 'R': 1220 reliable = 1; 1221 break; 1222 1223 case 'M': 1224 master = 1; 1225 break; 1226 1227 case 'A': 1228 auth = 1; 1229 break; 1230 1231 case 'E': 1232 encrypt = 1; 1233 break; 1234 1235 case 'S': 1236 secure = 1; 1237 break; 1238 1239 case 'G': 1240 socktype = SOCK_DGRAM; 1241 break; 1242 1243 case 'U': 1244 socktype = SOCK_STREAM; 1245 break; 1246 1247 case 'T': 1248 timestamp = 1; 1249 break; 1250 1251 case 'Q': 1252 max_transmit = atoi(optarg); 1253 break; 1254 1255 case 'Z': 1256 txwin_size = atoi(optarg); 1257 break; 1258 1259 default: 1260 usage(); 1261 exit(1); 1262 } 1263 } 1264 1265 if (need_addr && !(argc - optind)) { 1266 usage(); 1267 exit(1); 1268 } 1269 1270 if (data_size < 0) 1271 buffer_size = (omtu > imtu) ? omtu : imtu; 1272 else 1273 buffer_size = data_size; 1274 1275 if (!(buf = malloc(buffer_size))) { 1276 perror("Can't allocate data buffer"); 1277 exit(1); 1278 } 1279 1280 memset(&sa, 0, sizeof(sa)); 1281 sa.sa_handler = SIG_IGN; 1282 sa.sa_flags = SA_NOCLDSTOP; 1283 sigaction(SIGCHLD, &sa, NULL); 1284 1285 openlog("l2test", LOG_PERROR | LOG_PID, LOG_LOCAL0); 1286 1287 switch (mode) { 1288 case RECV: 1289 do_listen(recv_mode); 1290 break; 1291 1292 case CRECV: 1293 sk = do_connect(argv[optind]); 1294 if (sk < 0) 1295 exit(1); 1296 recv_mode(sk); 1297 break; 1298 1299 case DUMP: 1300 do_listen(dump_mode); 1301 break; 1302 1303 case SEND: 1304 sk = do_connect(argv[optind]); 1305 if (sk < 0) 1306 exit(1); 1307 send_mode(sk); 1308 break; 1309 1310 case LSEND: 1311 do_listen(send_mode); 1312 break; 1313 1314 case RECONNECT: 1315 reconnect_mode(argv[optind]); 1316 break; 1317 1318 case MULTY: 1319 multi_connect_mode(argc - optind, argv + optind); 1320 break; 1321 1322 case CONNECT: 1323 connect_mode(argv[optind]); 1324 break; 1325 1326 case SENDDUMP: 1327 sk = do_connect(argv[optind]); 1328 if (sk < 0) 1329 exit(1); 1330 senddump_mode(sk); 1331 break; 1332 1333 case LSENDDUMP: 1334 do_listen(senddump_mode); 1335 break; 1336 1337 case LSENDRECV: 1338 do_listen(send_and_recv_mode); 1339 break; 1340 1341 case CSENDRECV: 1342 sk = do_connect(argv[optind]); 1343 if (sk < 0) 1344 exit(1); 1345 1346 send_and_recv_mode(sk); 1347 break; 1348 1349 case INFOREQ: 1350 info_request(argv[optind]); 1351 exit(0); 1352 1353 case PAIRING: 1354 do_pairing(argv[optind]); 1355 exit(0); 1356 } 1357 1358 syslog(LOG_INFO, "Exit"); 1359 1360 closelog(); 1361 1362 return 0; 1363} 1364