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