l2ping.c revision 114879
1/* 2 * l2ping.c 3 * 4 * Copyright (c) 2001-2002 Maksim Yevmenkin <m_evmenkin@yahoo.com> 5 * All rights reserved. 6 * 7 * Redistribution and use in source and binary forms, with or without 8 * modification, are permitted provided that the following conditions 9 * are met: 10 * 1. Redistributions of source code must retain the above copyright 11 * notice, this list of conditions and the following disclaimer. 12 * 2. Redistributions in binary form must reproduce the above copyright 13 * notice, this list of conditions and the following disclaimer in the 14 * documentation and/or other materials provided with the distribution. 15 * 16 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 17 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 20 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 22 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 23 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 24 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 25 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 26 * SUCH DAMAGE. 27 * 28 * $Id: l2ping.c,v 1.3 2003/04/27 19:45:36 max Exp $ 29 * $FreeBSD: head/usr.sbin/bluetooth/l2ping/l2ping.c 114879 2003-05-10 21:50:37Z julian $ 30 */ 31 32#include <sys/types.h> 33#include <sys/ioctl.h> 34#include <sys/socket.h> 35#include <sys/time.h> 36#include <arpa/inet.h> 37#include <netinet/in.h> 38#include <assert.h> 39#include <bitstring.h> 40#include <err.h> 41#include <errno.h> 42#include <stdio.h> 43#include <stdlib.h> 44#include <string.h> 45#include <unistd.h> 46#include <ng_hci.h> 47#include <ng_l2cap.h> 48#include <ng_btsocket.h> 49 50static void usage (void); 51static void tv_sub (struct timeval *, struct timeval const *); 52static double tv2msec (struct timeval const *); 53 54#undef min 55#define min(x, y) (((x) > (y))? (y) : (x)) 56 57static char const pattern[] = "1234567890-"; 58#define PATTERN_SIZE (sizeof(pattern) - 1) 59 60/* 61 * Main 62 */ 63 64int 65main(int argc, char *argv[]) 66{ 67 bdaddr_t src, dst; 68 struct sockaddr_l2cap sa; 69 struct ng_btsocket_l2cap_raw_ping r; 70 int n, s, count, wait, flood, fail; 71 struct timeval a, b; 72 73 /* Set defaults */ 74 memcpy(&src, NG_HCI_BDADDR_ANY, sizeof(src)); 75 memcpy(&dst, NG_HCI_BDADDR_ANY, sizeof(dst)); 76 77 memset(&r, 0, sizeof(r)); 78 r.echo_data = calloc(NG_L2CAP_MAX_ECHO_SIZE, sizeof(u_int8_t)); 79 if (r.echo_data == NULL) { 80 fprintf(stderr, "Failed to allocate echo data buffer"); 81 exit(1); 82 } 83 84 r.echo_size = 64; /* bytes */ 85 count = -1; /* unlimited */ 86 wait = 1; /* sec */ 87 flood = 0; 88 89 /* Parse command line arguments */ 90 while ((n = getopt(argc, argv, "a:c:fi:n:s:S:h")) != -1) { 91 switch (n) { 92 case 'a': 93 case 'S': { 94 int a0, a1, a2, a3, a4, a5; 95 96 if (sscanf(optarg, "%x:%x:%x:%x:%x:%x", 97 &a5, &a4, &a3, &a2, &a1, &a0) != 6) 98 usage(); 99 100 if (n == 'a') { 101 /* destination bdaddr */ 102 dst.b[0] = (a0 & 0xff); 103 dst.b[1] = (a1 & 0xff); 104 dst.b[2] = (a2 & 0xff); 105 dst.b[3] = (a3 & 0xff); 106 dst.b[4] = (a4 & 0xff); 107 dst.b[5] = (a5 & 0xff); 108 } else { 109 /* source bdaddr */ 110 src.b[0] = (a0 & 0xff); 111 src.b[1] = (a1 & 0xff); 112 src.b[2] = (a2 & 0xff); 113 src.b[3] = (a3 & 0xff); 114 src.b[4] = (a4 & 0xff); 115 src.b[5] = (a5 & 0xff); 116 } 117 } break; 118 119 case 'c': 120 count = atoi(optarg); 121 if (count <= 0) 122 usage(); 123 break; 124 125 case 'f': 126 flood = 1; 127 break; 128 129 case 'i': 130 wait = atoi(optarg); 131 if (wait <= 0) 132 usage(); 133 break; 134 135 case 's': 136 r.echo_size = atoi(optarg); 137 if ((int) r.echo_size < sizeof(int)) 138 usage(); 139 140 if (r.echo_size > NG_L2CAP_MAX_ECHO_SIZE) 141 r.echo_size = NG_L2CAP_MAX_ECHO_SIZE; 142 break; 143 144 case 'h': 145 default: 146 usage(); 147 break; 148 } 149 } 150 151 if (memcmp(&dst, NG_HCI_BDADDR_ANY, sizeof(dst)) == 0) 152 usage(); 153 154 s = socket(PF_BLUETOOTH, SOCK_RAW, BLUETOOTH_PROTO_L2CAP); 155 if (s < 0) 156 err(2, "Could not create socket"); 157 158 memset(&sa, 0, sizeof(sa)); 159 sa.l2cap_len = sizeof(sa); 160 sa.l2cap_family = AF_BLUETOOTH; 161 memcpy(&sa.l2cap_bdaddr, &src, sizeof(sa.l2cap_bdaddr)); 162 163 if (bind(s, (struct sockaddr *) &sa, sizeof(sa)) < 0) 164 err(3, 165"Could not bind socket, src bdaddr=%x:%x:%x:%x:%x:%x", 166 sa.l2cap_bdaddr.b[5], sa.l2cap_bdaddr.b[4], 167 sa.l2cap_bdaddr.b[3], sa.l2cap_bdaddr.b[2], 168 sa.l2cap_bdaddr.b[1], sa.l2cap_bdaddr.b[0]); 169 170 memset(&sa, 0, sizeof(sa)); 171 sa.l2cap_len = sizeof(sa); 172 sa.l2cap_family = AF_BLUETOOTH; 173 memcpy(&sa.l2cap_bdaddr, &dst, sizeof(sa.l2cap_bdaddr)); 174 175 if (connect(s, (struct sockaddr *) &sa, sizeof(sa)) < 0) 176 err(4, 177"Could not connect socket, dst bdaddr=%x:%x:%x:%x:%x:%x", 178 sa.l2cap_bdaddr.b[5], sa.l2cap_bdaddr.b[4], 179 sa.l2cap_bdaddr.b[3], sa.l2cap_bdaddr.b[2], 180 sa.l2cap_bdaddr.b[1], sa.l2cap_bdaddr.b[0]); 181 182 /* Fill pattern */ 183 for (n = 0; n < r.echo_size; ) { 184 int avail = min(r.echo_size - n, PATTERN_SIZE); 185 186 memcpy(r.echo_data + n, pattern, avail); 187 n += avail; 188 } 189 190 /* Start ping'ing */ 191 for (n = 0; count == -1 || count > 0; n ++) { 192 if (gettimeofday(&a, NULL) < 0) 193 err(5, "Could not gettimeofday(a)"); 194 195 fail = 0; 196 *((int *)(r.echo_data)) = htonl(n); 197 if (ioctl(s, SIOC_L2CAP_L2CA_PING, &r, sizeof(r)) < 0) { 198 r.result = errno; 199 fail = 1; 200/* 201 warn("Could not ping, dst bdaddr=%x:%x:%x:%x:%x:%x", 202 r.echo_dst.b[5], r.echo_dst.b[4], 203 r.echo_dst.b[3], r.echo_dst.b[2], 204 r.echo_dst.b[1], r.echo_dst.b[0]); 205*/ 206 } 207 208 if (gettimeofday(&b, NULL) < 0) 209 err(7, "Could not gettimeofday(b)"); 210 211 tv_sub(&b, &a); 212 213 fprintf(stdout, 214"%d bytes from %x:%x:%x:%x:%x:%x seq_no=%d time=%.3f ms result=%#x %s\n", 215 r.echo_size, 216 dst.b[5], dst.b[4], dst.b[3], 217 dst.b[2], dst.b[1], dst.b[0], 218 ntohl(*((int *)(r.echo_data))), 219 tv2msec(&b), r.result, 220 ((fail == 0)? "" : strerror(errno))); 221 222 if (!flood) { 223 /* Wait */ 224 a.tv_sec = wait; 225 a.tv_usec = 0; 226 select(0, NULL, NULL, NULL, &a); 227 } 228 229 if (count != -1) 230 count --; 231 } 232 233 free(r.echo_data); 234 close(s); 235 236 return (0); 237} /* main */ 238 239/* 240 * a -= b, for timevals 241 */ 242 243static void 244tv_sub(struct timeval *a, struct timeval const *b) 245{ 246 if (a->tv_usec < b->tv_usec) { 247 a->tv_usec += 1000000; 248 a->tv_sec -= 1; 249 } 250 251 a->tv_usec -= b->tv_usec; 252 a->tv_sec -= b->tv_sec; 253} /* tv_sub */ 254 255/* 256 * convert tv to msec 257 */ 258 259static double 260tv2msec(struct timeval const *tvp) 261{ 262 return(((double)tvp->tv_usec)/1000.0 + ((double)tvp->tv_sec)*1000.0); 263} /* tv2msec */ 264 265/* 266 * Usage 267 */ 268 269static void 270usage(void) 271{ 272 fprintf(stderr, "Usage: l2ping -a bd_addr " \ 273 "[-S bd_addr -c count -i wait -s size -h]\n"); 274 fprintf(stderr, "Where:\n"); 275 fprintf(stderr, "\t-S bd_addr - Source BD_ADDR\n"); 276 fprintf(stderr, "\t-a bd_addr - Remote BD_ADDR to ping\n"); 277 fprintf(stderr, "\t-c count - Number of packets to send\n"); 278 fprintf(stderr, "\t-f - No delay (soft of flood)\n"); 279 fprintf(stderr, "\t-i wait - Delay between packets (sec)\n"); 280 fprintf(stderr, "\t-s size - Packet size (bytes), " \ 281 "between %d and %d\n", sizeof(int), NG_L2CAP_MAX_ECHO_SIZE); 282 fprintf(stderr, "\t-h - Display this message\n"); 283 284 exit(255); 285} /* usage */ 286 287