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 (at) qualcomm.com> 7 * Copyright (C) 2002-2010 Marcel Holtmann <marcel (at) 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 <unistd.h> 33 #include <stdlib.h> 34 #include <string.h> 35 #include <getopt.h> 36 #include <signal.h> 37 #include <sys/time.h> 38 #include <sys/poll.h> 39 #include <sys/socket.h> 40 41 #include <bluetooth/bluetooth.h> 42 #include <bluetooth/hci.h> 43 #include <bluetooth/hci_lib.h> 44 #include <bluetooth/l2cap.h> 45 46 /* Defaults */ 47 static bdaddr_t bdaddr; 48 static int size = 44; 49 static int ident = 200; 50 static int delay = 1; 51 static int count = -1; 52 static int timeout = 10; 53 static int reverse = 0; 54 static int verify = 0; 55 56 /* Stats */ 57 static int sent_pkt = 0; 58 static int recv_pkt = 0; 59 60 static float tv2fl(struct timeval tv) 61 { 62 return (float)(tv.tv_sec*1000.0) + (float)(tv.tv_usec/1000.0); 63 } 64 65 static void stat(int sig) 66 { 67 int loss = sent_pkt ? (float)((sent_pkt-recv_pkt)/(sent_pkt/100.0)) : 0; 68 printf("%d sent, %d received, %d%% loss\n", sent_pkt, recv_pkt, loss); 69 exit(0); 70 } 71 72 static void ping(char *svr) 73 { 74 struct sigaction sa; 75 struct sockaddr_l2 addr; 76 socklen_t optlen; 77 unsigned char *send_buf; 78 unsigned char *recv_buf; 79 char str[18]; 80 int i, sk, lost; 81 uint8_t id; 82 83 memset(&sa, 0, sizeof(sa)); 84 sa.sa_handler = stat; 85 sigaction(SIGINT, &sa, NULL); 86 87 send_buf = malloc(L2CAP_CMD_HDR_SIZE + size); 88 recv_buf = malloc(L2CAP_CMD_HDR_SIZE + size); 89 if (!send_buf || !recv_buf) { 90 perror("Can't allocate buffer"); 91 exit(1); 92 } 93 94 /* Create socket */ 95 sk = socket(PF_BLUETOOTH, SOCK_RAW, BTPROTO_L2CAP); 96 if (sk < 0) { 97 perror("Can't create socket"); 98 goto error; 99 } 100 101 /* Bind to local address */ 102 memset(&addr, 0, sizeof(addr)); 103 addr.l2_family = AF_BLUETOOTH; 104 bacpy(&addr.l2_bdaddr, &bdaddr); 105 106 if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) { 107 perror("Can't bind socket"); 108 goto error; 109 } 110 111 /* Connect to remote device */ 112 memset(&addr, 0, sizeof(addr)); 113 addr.l2_family = AF_BLUETOOTH; 114 str2ba(svr, &addr.l2_bdaddr); 115 116 if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) { 117 perror("Can't connect"); 118 goto error; 119 } 120 121 /* Get local address */ 122 memset(&addr, 0, sizeof(addr)); 123 optlen = sizeof(addr); 124 125 if (getsockname(sk, (struct sockaddr *) &addr, &optlen) < 0) { 126 perror("Can't get local address"); 127 goto error; 128 } 129 130 ba2str(&addr.l2_bdaddr, str); 131 printf("Ping: %s from %s (data size %d) ...\n", svr, str, size); 132 133 /* Initialize send buffer */ 134 for (i = 0; i < size; i++) 135 send_buf[L2CAP_CMD_HDR_SIZE + i] = (i % 40) + 'A'; 136 137 id = ident; 138 139 while (count == -1 || count-- > 0) { 140 struct timeval tv_send, tv_recv, tv_diff; 141 l2cap_cmd_hdr *send_cmd = (l2cap_cmd_hdr *) send_buf; 142 l2cap_cmd_hdr *recv_cmd = (l2cap_cmd_hdr *) recv_buf; 143 144 /* Build command header */ 145 send_cmd->ident = id; 146 send_cmd->len = htobs(size); 147 148 if (reverse) 149 send_cmd->code = L2CAP_ECHO_RSP; 150 else 151 send_cmd->code = L2CAP_ECHO_REQ; 152 153 gettimeofday(&tv_send, NULL); 154 155 /* Send Echo Command */ 156 if (send(sk, send_buf, L2CAP_CMD_HDR_SIZE + size, 0) <= 0) { 157 perror("Send failed"); 158 goto error; 159 } 160 161 /* Wait for Echo Response */ 162 lost = 0; 163 while (1) { 164 struct pollfd pf[1]; 165 int err; 166 167 pf[0].fd = sk; 168 pf[0].events = POLLIN; 169 170 if ((err = poll(pf, 1, timeout * 1000)) < 0) { 171 perror("Poll failed"); 172 goto error; 173 } 174 175 if (!err) { 176 lost = 1; 177 break; 178 } 179 180 if ((err = recv(sk, recv_buf, L2CAP_CMD_HDR_SIZE + size, 0)) < 0) { 181 perror("Recv failed"); 182 goto error; 183 } 184 185 if (!err){ 186 printf("Disconnected\n"); 187 goto error; 188 } 189 190 recv_cmd->len = btohs(recv_cmd->len); 191 192 /* Check for our id */ 193 if (recv_cmd->ident != id) 194 continue; 195 196 /* Check type */ 197 if (!reverse && recv_cmd->code == L2CAP_ECHO_RSP) 198 break; 199 200 if (recv_cmd->code == L2CAP_COMMAND_REJ) { 201 printf("Peer doesn't support Echo packets\n"); 202 goto error; 203 } 204 205 } 206 sent_pkt++; 207 208 if (!lost) { 209 recv_pkt++; 210 211 gettimeofday(&tv_recv, NULL); 212 timersub(&tv_recv, &tv_send, &tv_diff); 213 214 if (verify) { 215 /* Check payload length */ 216 if (recv_cmd->len != size) { 217 fprintf(stderr, "Received %d bytes, expected %d\n", 218 recv_cmd->len, size); 219 goto error; 220 } 221 222 /* Check payload */ 223 if (memcmp(&send_buf[L2CAP_CMD_HDR_SIZE], 224 &recv_buf[L2CAP_CMD_HDR_SIZE], size)) { 225 fprintf(stderr, "Response payload different.\n"); 226 goto error; 227 } 228 } 229 230 printf("%d bytes from %s id %d time %.2fms\n", recv_cmd->len, svr, 231 id - ident, tv2fl(tv_diff)); 232 233 if (delay) 234 sleep(delay); 235 } else { 236 printf("no response from %s: id %d\n", svr, id - ident); 237 } 238 239 if (++id > 254) 240 id = ident; 241 } 242 stat(0); 243 free(send_buf); 244 free(recv_buf); 245 return; 246 247 error: 248 close(sk); 249 free(send_buf); 250 free(recv_buf); 251 exit(1); 252 } 253 254 static void usage(void) 255 { 256 printf("l2ping - L2CAP ping\n"); 257 printf("Usage:\n"); 258 printf("\tl2ping [-i device] [-s size] [-c count] [-t timeout] [-d delay] [-f] [-r] [-v] <bdaddr>\n"); 259 printf("\t-f Flood ping (delay = 0)\n"); 260 printf("\t-r Reverse ping\n"); 261 printf("\t-v Verify request and response payload\n"); 262 } 263 264 int main(int argc, char *argv[]) 265 { 266 int opt; 267 268 /* Default options */ 269 bacpy(&bdaddr, BDADDR_ANY); 270 271 while ((opt=getopt(argc,argv,"i:d:s:c:t:frv")) != EOF) { 272 switch(opt) { 273 case 'i': 274 if (!strncasecmp(optarg, "hci", 3)) 275 hci_devba(atoi(optarg + 3), &bdaddr); 276 else 277 str2ba(optarg, &bdaddr); 278 break; 279 280 case 'd': 281 delay = atoi(optarg); 282 break; 283 284 case 'f': 285 /* Kinda flood ping */ 286 delay = 0; 287 break; 288 289 case 'r': 290 /* Use responses instead of requests */ 291 reverse = 1; 292 break; 293 294 case 'v': 295 verify = 1; 296 break; 297 298 case 'c': 299 count = atoi(optarg); 300 break; 301 302 case 't': 303 timeout = atoi(optarg); 304 break; 305 306 case 's': 307 size = atoi(optarg); 308 break; 309 310 default: 311 usage(); 312 exit(1); 313 } 314 } 315 316 if (!(argc - optind)) { 317 usage(); 318 exit(1); 319 } 320 321 ping(argv[optind]); 322 323 return 0; 324 } 325
