l2ping.c revision 107120
1107120Sjulian/*
2107120Sjulian * l2ping.c
3107120Sjulian *
4107120Sjulian * Copyright (c) 2001-2002 Maksim Yevmenkin <m_evmenkin@yahoo.com>
5107120Sjulian * All rights reserved.
6107120Sjulian *
7107120Sjulian * Redistribution and use in source and binary forms, with or without
8107120Sjulian * modification, are permitted provided that the following conditions
9107120Sjulian * are met:
10107120Sjulian * 1. Redistributions of source code must retain the above copyright
11107120Sjulian *    notice, this list of conditions and the following disclaimer.
12107120Sjulian * 2. Redistributions in binary form must reproduce the above copyright
13107120Sjulian *    notice, this list of conditions and the following disclaimer in the
14107120Sjulian *    documentation and/or other materials provided with the distribution.
15107120Sjulian *
16107120Sjulian * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
17107120Sjulian * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18107120Sjulian * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
19107120Sjulian * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
20107120Sjulian * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
21107120Sjulian * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
22107120Sjulian * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
23107120Sjulian * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
24107120Sjulian * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
25107120Sjulian * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
26107120Sjulian * SUCH DAMAGE.
27107120Sjulian *
28107120Sjulian * $Id: l2ping.c,v 1.9 2002/09/04 21:28:05 max Exp $
29107120Sjulian * $FreeBSD: head/usr.sbin/bluetooth/l2ping/l2ping.c 107120 2002-11-20 23:01:59Z julian $
30107120Sjulian */
31107120Sjulian
32107120Sjulian#include <sys/types.h>
33107120Sjulian#include <sys/ioctl.h>
34107120Sjulian#include <sys/socket.h>
35107120Sjulian#include <sys/time.h>
36107120Sjulian#include <arpa/inet.h>
37107120Sjulian#include <netinet/in.h>
38107120Sjulian#include <assert.h>
39107120Sjulian#include <bitstring.h>
40107120Sjulian#include <err.h>
41107120Sjulian#include <errno.h>
42107120Sjulian#include <stdio.h>
43107120Sjulian#include <stdlib.h>
44107120Sjulian#include <string.h>
45107120Sjulian#include <unistd.h>
46107120Sjulian#include <ng_hci.h>
47107120Sjulian#include <ng_l2cap.h>
48107120Sjulian#include <ng_btsocket.h>
49107120Sjulian
50107120Sjulianstatic void	usage	(void);
51107120Sjulianstatic void	tv_sub	(struct timeval *, struct timeval const *);
52107120Sjulianstatic double	tv2msec	(struct timeval const *);
53107120Sjulian
54107120Sjulian#undef	min
55107120Sjulian#define	min(x, y)	(((x) > (y))? (y) : (x))
56107120Sjulian
57107120Sjulianstatic char const		pattern[] = "1234567890-";
58107120Sjulian#define PATTERN_SIZE		(sizeof(pattern) - 1)
59107120Sjulian
60107120Sjulian/*
61107120Sjulian * Main
62107120Sjulian */
63107120Sjulian
64107120Sjulianint
65107120Sjulianmain(int argc, char *argv[])
66107120Sjulian{
67107120Sjulian	struct ng_btsocket_l2cap_raw_ping	r;
68107120Sjulian	int					n, s, count, wait, flood, fail;
69107120Sjulian	struct timeval				a, b;
70107120Sjulian
71107120Sjulian	/* Set defaults */
72107120Sjulian	memset(&r, 0, sizeof(r));
73107120Sjulian	r.echo_data = calloc(NG_L2CAP_MAX_ECHO_SIZE, sizeof(u_int8_t));
74107120Sjulian	if (r.echo_data == NULL) {
75107120Sjulian		fprintf(stderr, "Failed to allocate echo data buffer");
76107120Sjulian		exit(1);
77107120Sjulian	}
78107120Sjulian
79107120Sjulian	r.echo_size = 64; /* bytes */
80107120Sjulian	count = -1;       /* unlimited */
81107120Sjulian	wait = 1;         /* sec */
82107120Sjulian	flood = 0;
83107120Sjulian
84107120Sjulian	/* Parse command line arguments */
85107120Sjulian	while ((n = getopt(argc, argv, "a:c:fi:n:s:S:")) != -1) {
86107120Sjulian		switch (n) {
87107120Sjulian		case 'a':
88107120Sjulian		case 'S': {
89107120Sjulian			int	a0, a1, a2, a3, a4, a5;
90107120Sjulian
91107120Sjulian			if (sscanf(optarg, "%x:%x:%x:%x:%x:%x",
92107120Sjulian					&a5, &a4, &a3, &a2, &a1, &a0) != 6)
93107120Sjulian				usage();
94107120Sjulian
95107120Sjulian			if (n == 'a') {
96107120Sjulian				/* destination bdaddr */
97107120Sjulian				r.echo_dst.b[0] = (a0 & 0xff);
98107120Sjulian				r.echo_dst.b[1] = (a1 & 0xff);
99107120Sjulian				r.echo_dst.b[2] = (a2 & 0xff);
100107120Sjulian				r.echo_dst.b[3] = (a3 & 0xff);
101107120Sjulian				r.echo_dst.b[4] = (a4 & 0xff);
102107120Sjulian				r.echo_dst.b[5] = (a5 & 0xff);
103107120Sjulian			} else {
104107120Sjulian				/* source bdaddr */
105107120Sjulian				r.echo_src.b[0] = (a0 & 0xff);
106107120Sjulian				r.echo_src.b[1] = (a1 & 0xff);
107107120Sjulian				r.echo_src.b[2] = (a2 & 0xff);
108107120Sjulian				r.echo_src.b[3] = (a3 & 0xff);
109107120Sjulian				r.echo_src.b[4] = (a4 & 0xff);
110107120Sjulian				r.echo_src.b[5] = (a5 & 0xff);
111107120Sjulian			}
112107120Sjulian			} break;
113107120Sjulian
114107120Sjulian		case 'c':
115107120Sjulian			count = atoi(optarg);
116107120Sjulian			if (count <= 0)
117107120Sjulian				usage();
118107120Sjulian			break;
119107120Sjulian
120107120Sjulian		case 'f':
121107120Sjulian			flood = 1;
122107120Sjulian			break;
123107120Sjulian
124107120Sjulian		case 'i':
125107120Sjulian			wait = atoi(optarg);
126107120Sjulian			if (wait <= 0)
127107120Sjulian				usage();
128107120Sjulian			break;
129107120Sjulian
130107120Sjulian		case 's':
131107120Sjulian			r.echo_size = atoi(optarg);
132107120Sjulian			if ((int) r.echo_size < sizeof(int))
133107120Sjulian				usage();
134107120Sjulian
135107120Sjulian			if (r.echo_size > NG_L2CAP_MAX_ECHO_SIZE)
136107120Sjulian				r.echo_size = NG_L2CAP_MAX_ECHO_SIZE;
137107120Sjulian			break;
138107120Sjulian
139107120Sjulian		default:
140107120Sjulian			usage();
141107120Sjulian			break;
142107120Sjulian		}
143107120Sjulian	}
144107120Sjulian
145107120Sjulian	if (memcmp(&r.echo_dst, NG_HCI_BDADDR_ANY, sizeof(bdaddr_t)) == 0)
146107120Sjulian		usage();
147107120Sjulian
148107120Sjulian	s = socket(PF_BLUETOOTH, SOCK_RAW, BLUETOOTH_PROTO_L2CAP);
149107120Sjulian	if (s < 0)
150107120Sjulian		err(2, "Could not create socket");
151107120Sjulian
152107120Sjulian	if (memcmp(&r.echo_src, NG_HCI_BDADDR_ANY, sizeof(bdaddr_t)) != 0) {
153107120Sjulian		struct sockaddr_l2cap	sa;
154107120Sjulian
155107120Sjulian		memset(&sa, 0, sizeof(sa));
156107120Sjulian		sa.l2cap_len = sizeof(sa);
157107120Sjulian		sa.l2cap_family = AF_BLUETOOTH;
158107120Sjulian		memcpy(&sa.l2cap_bdaddr, &r.echo_src, sizeof(bdaddr_t));
159107120Sjulian
160107120Sjulian		if (bind(s, (struct sockaddr *) &sa, sizeof(sa)) < 0)
161107120Sjulian			err(3,
162107120Sjulian"Could not bind socket, src bdaddr=%x:%x:%x:%x:%x:%x",
163107120Sjulian				sa.l2cap_bdaddr.b[5], sa.l2cap_bdaddr.b[4],
164107120Sjulian				sa.l2cap_bdaddr.b[3], sa.l2cap_bdaddr.b[2],
165107120Sjulian				sa.l2cap_bdaddr.b[1], sa.l2cap_bdaddr.b[0]);
166107120Sjulian
167107120Sjulian		if (connect(s, (struct sockaddr *) &sa, sizeof(sa)) < 0)
168107120Sjulian			err(4,
169107120Sjulian"Could not connect socket, src bdaddr=%x:%x:%x:%x:%x:%x",
170107120Sjulian				sa.l2cap_bdaddr.b[5], sa.l2cap_bdaddr.b[4],
171107120Sjulian				sa.l2cap_bdaddr.b[3], sa.l2cap_bdaddr.b[2],
172107120Sjulian				sa.l2cap_bdaddr.b[1], sa.l2cap_bdaddr.b[0]);
173107120Sjulian	}
174107120Sjulian
175107120Sjulian	/* Fill pattern */
176107120Sjulian	for (n = 0; n < r.echo_size; ) {
177107120Sjulian		int	avail = min(r.echo_size - n, PATTERN_SIZE);
178107120Sjulian
179107120Sjulian		memcpy(r.echo_data + n, pattern, avail);
180107120Sjulian		n += avail;
181107120Sjulian	}
182107120Sjulian
183107120Sjulian	/* Start ping'ing */
184107120Sjulian	for (n = 0; count == -1 || count > 0; n ++) {
185107120Sjulian		if (gettimeofday(&a, NULL) < 0)
186107120Sjulian			err(5, "Could not gettimeofday(a)");
187107120Sjulian
188107120Sjulian		fail = 0;
189107120Sjulian		*((int *)(r.echo_data)) = htonl(n);
190107120Sjulian		if (ioctl(s, SIOC_L2CAP_L2CA_PING, &r, sizeof(r)) < 0) {
191107120Sjulian			r.result = errno;
192107120Sjulian			fail = 1;
193107120Sjulian/*
194107120Sjulian			warn("Could not ping, dst bdaddr=%x:%x:%x:%x:%x:%x",
195107120Sjulian				r.echo_dst.b[5], r.echo_dst.b[4],
196107120Sjulian				r.echo_dst.b[3], r.echo_dst.b[2],
197107120Sjulian				r.echo_dst.b[1], r.echo_dst.b[0]);
198107120Sjulian*/
199107120Sjulian		}
200107120Sjulian
201107120Sjulian		if (gettimeofday(&b, NULL) < 0)
202107120Sjulian			err(7, "Could not gettimeofday(b)");
203107120Sjulian
204107120Sjulian		tv_sub(&b, &a);
205107120Sjulian
206107120Sjulian		fprintf(stdout,
207107120Sjulian"%d bytes from %x:%x:%x:%x:%x:%x seq_no=%d time=%.3f ms result=%#x %s\n",
208107120Sjulian			r.echo_size,
209107120Sjulian			r.echo_dst.b[5], r.echo_dst.b[4],
210107120Sjulian			r.echo_dst.b[3], r.echo_dst.b[2],
211107120Sjulian			r.echo_dst.b[1], r.echo_dst.b[0],
212107120Sjulian			ntohl(*((int *)(r.echo_data))),
213107120Sjulian			tv2msec(&b), r.result,
214107120Sjulian			((fail == 0)? "" : strerror(errno)));
215107120Sjulian
216107120Sjulian		if (!flood) {
217107120Sjulian			/* Wait */
218107120Sjulian			a.tv_sec = wait;
219107120Sjulian			a.tv_usec = 0;
220107120Sjulian			select(0, NULL, NULL, NULL, &a);
221107120Sjulian		}
222107120Sjulian
223107120Sjulian		if (count != -1)
224107120Sjulian			count --;
225107120Sjulian	}
226107120Sjulian
227107120Sjulian	free(r.echo_data);
228107120Sjulian	close(s);
229107120Sjulian
230107120Sjulian	return (0);
231107120Sjulian} /* main */
232107120Sjulian
233107120Sjulian/*
234107120Sjulian * a -= b, for timevals
235107120Sjulian */
236107120Sjulian
237107120Sjulianstatic void
238107120Sjuliantv_sub(struct timeval *a, struct timeval const *b)
239107120Sjulian{
240107120Sjulian	if (a->tv_usec < b->tv_usec) {
241107120Sjulian		a->tv_usec += 1000000;
242107120Sjulian		a->tv_sec -= 1;
243107120Sjulian	}
244107120Sjulian
245107120Sjulian	a->tv_usec -= b->tv_usec;
246107120Sjulian	a->tv_sec -= b->tv_sec;
247107120Sjulian} /* tv_sub */
248107120Sjulian
249107120Sjulian/*
250107120Sjulian * convert tv to msec
251107120Sjulian */
252107120Sjulian
253107120Sjulianstatic double
254107120Sjuliantv2msec(struct timeval const *tvp)
255107120Sjulian{
256107120Sjulian	return(((double)tvp->tv_usec)/1000.0 + ((double)tvp->tv_sec)*1000.0);
257107120Sjulian} /* tv2msec */
258107120Sjulian
259107120Sjulian/*
260107120Sjulian * Usage
261107120Sjulian */
262107120Sjulian
263107120Sjulianstatic void
264107120Sjulianusage(void)
265107120Sjulian{
266107120Sjulian	fprintf(stderr, "Usage: l2ping -a bd_addr " \
267107120Sjulian		"[-S bd_addr -c count -i wait -s size]\n");
268107120Sjulian	fprintf(stderr, "Where:\n");
269107120Sjulian	fprintf(stderr, "\t-S bd_addr         - Source BD_ADDR\n");
270107120Sjulian	fprintf(stderr, "\t-a bd_addr         - Remote BD_ADDR to ping\n");
271107120Sjulian	fprintf(stderr, "\t-c count           - Number of packets to send\n");
272107120Sjulian	fprintf(stderr, "\t-f                 - No delay (soft of flood)\n");
273107120Sjulian	fprintf(stderr, "\t-i wait            - Delay between packets (sec)\n");
274107120Sjulian	fprintf(stderr, "\t-s size            - Packet size (bytes), " \
275107120Sjulian		"between %d and %d\n", sizeof(int), NG_L2CAP_MAX_ECHO_SIZE);
276107120Sjulian
277107120Sjulian	exit(255);
278107120Sjulian} /* usage */
279107120Sjulian
280