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