korg/bluez
0
0
Fork 0
mirror of https://git.kernel.org/pub/scm/bluetooth/bluez.git synced 2024-12-17 07:55:04 +08:00
Code Issues Packages Projects Releases Wiki Activity
fb5d75e1ab
bluez/tools/hcidump.c
Luiz Augusto von Dentz 457d44d3de tools/hcidump: Fix resource leaks
2015-08-13 14:05:47 +03:00

822 lines
17 KiB
C
Raw Blame History

/*
*
* BlueZ - Bluetooth protocol stack for Linux
*
* Copyright (C) 2000-2002 Maxim Krasnyansky <maxk@qualcomm.com>
* Copyright (C) 2003-2011 Marcel Holtmann <marcel@holtmann.org>
*
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif
#include <stdio.h>
#include <errno.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <getopt.h>
#include <poll.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include "parser/parser.h"
#include "parser/sdp.h"
#include "lib/hci.h"
#include "lib/hci_lib.h"
#define SNAP_LEN HCI_MAX_FRAME_SIZE
/* Modes */
enum {
PARSE,
READ,
WRITE,
PPPDUMP,
AUDIO
};
/* Default options */
static int snap_len = SNAP_LEN;
static int mode = PARSE;
static char *dump_file = NULL;
static char *pppdump_file = NULL;
static char *audio_file = NULL;
struct hcidump_hdr {
uint16_t len;
uint8_t in;
uint8_t pad;
uint32_t ts_sec;
uint32_t ts_usec;
} __attribute__ ((packed));
#define HCIDUMP_HDR_SIZE (sizeof(struct hcidump_hdr))
struct btsnoop_hdr {
uint8_t id[8]; /* Identification Pattern */
uint32_t version; /* Version Number = 1 */
uint32_t type; /* Datalink Type */
} __attribute__ ((packed));
#define BTSNOOP_HDR_SIZE (sizeof(struct btsnoop_hdr))
struct btsnoop_pkt {
uint32_t size; /* Original Length */
uint32_t len; /* Included Length */
uint32_t flags; /* Packet Flags */
uint32_t drops; /* Cumulative Drops */
uint64_t ts; /* Timestamp microseconds */
uint8_t data[0]; /* Packet Data */
} __attribute__ ((packed));
#define BTSNOOP_PKT_SIZE (sizeof(struct btsnoop_pkt))
static uint8_t btsnoop_id[] = { 0x62, 0x74, 0x73, 0x6e, 0x6f, 0x6f, 0x70, 0x00 };
static uint32_t btsnoop_version = 0;
static uint32_t btsnoop_type = 0;
struct pktlog_hdr {
uint32_t len;
uint64_t ts;
uint8_t type;
} __attribute__ ((packed));
#define PKTLOG_HDR_SIZE (sizeof(struct pktlog_hdr))
static inline int read_n(int fd, char *buf, int len)
{
int t = 0, w;
while (len > 0) {
if ((w = read(fd, buf, len)) < 0) {
if (errno == EINTR || errno == EAGAIN)
continue;
return -1;
}
if (!w)
return 0;
len -= w; buf += w; t += w;
}
return t;
}
static inline int write_n(int fd, char *buf, int len)
{
int t = 0, w;
while (len > 0) {
if ((w = write(fd, buf, len)) < 0) {
if (errno == EINTR || errno == EAGAIN)
continue;
return -1;
}
if (!w)
return 0;
len -= w; buf += w; t += w;
}
return t;
}
static int process_frames(int dev, int sock, int fd, unsigned long flags)
{
struct cmsghdr *cmsg;
struct msghdr msg;
struct iovec iv;
struct hcidump_hdr *dh;
struct btsnoop_pkt *dp;
struct frame frm;
struct pollfd fds[2];
int nfds = 0;
char *buf;
char ctrl[100];
int len, hdr_size = HCIDUMP_HDR_SIZE;
if (sock < 0)
return -1;
if (snap_len < SNAP_LEN)
snap_len = SNAP_LEN;
if (flags & DUMP_BTSNOOP)
hdr_size = BTSNOOP_PKT_SIZE;
buf = malloc(snap_len + hdr_size);
if (!buf) {
perror("Can't allocate data buffer");
return -1;
}
dh = (void *) buf;
dp = (void *) buf;
frm.data = buf + hdr_size;
if (dev == HCI_DEV_NONE)
printf("system: ");
else
printf("device: hci%d ", dev);
printf("snap_len: %d filter: 0x%lx\n", snap_len, parser.filter);
memset(&msg, 0, sizeof(msg));
fds[nfds].fd = sock;
fds[nfds].events = POLLIN;
fds[nfds].revents = 0;
nfds++;
while (1) {
int i, n = poll(fds, nfds, -1);
if (n <= 0)
continue;
for (i = 0; i < nfds; i++) {
if (fds[i].revents & (POLLHUP | POLLERR | POLLNVAL)) {
if (fds[i].fd == sock)
printf("device: disconnected\n");
else
printf("client: disconnect\n");
return 0;
}
}
iv.iov_base = frm.data;
iv.iov_len = snap_len;
msg.msg_iov = &iv;
msg.msg_iovlen = 1;
msg.msg_control = ctrl;
msg.msg_controllen = 100;
len = recvmsg(sock, &msg, MSG_DONTWAIT);
if (len < 0) {
if (errno == EAGAIN || errno == EINTR)
continue;
perror("Receive failed");
return -1;
}
/* Process control message */
frm.data_len = len;
frm.dev_id = dev;
frm.in = 0;
frm.pppdump_fd = parser.pppdump_fd;
frm.audio_fd = parser.audio_fd;
cmsg = CMSG_FIRSTHDR(&msg);
while (cmsg) {
int dir;
switch (cmsg->cmsg_type) {
case HCI_CMSG_DIR:
memcpy(&dir, CMSG_DATA(cmsg), sizeof(int));
frm.in = (uint8_t) dir;
break;
case HCI_CMSG_TSTAMP:
memcpy(&frm.ts, CMSG_DATA(cmsg),
sizeof(struct timeval));
break;
}
cmsg = CMSG_NXTHDR(&msg, cmsg);
}
frm.ptr = frm.data;
frm.len = frm.data_len;
switch (mode) {
case WRITE:
/* Save or send dump */
if (flags & DUMP_BTSNOOP) {
uint64_t ts;
uint8_t pkt_type = ((uint8_t *) frm.data)[0];
dp->size = htobe32(frm.data_len);
dp->len = dp->size;
dp->flags = be32toh(frm.in & 0x01);
dp->drops = 0;
ts = (frm.ts.tv_sec - 946684800ll) * 1000000ll + frm.ts.tv_usec;
dp->ts = htobe64(ts + 0x00E03AB44A676000ll);
if (pkt_type == HCI_COMMAND_PKT ||
pkt_type == HCI_EVENT_PKT)
dp->flags |= be32toh(0x02);
} else {
dh->len = htobs(frm.data_len);
dh->in = frm.in;
dh->ts_sec = htobl(frm.ts.tv_sec);
dh->ts_usec = htobl(frm.ts.tv_usec);
}
if (write_n(fd, buf, frm.data_len + hdr_size) < 0) {
perror("Write error");
return -1;
}
break;
default:
/* Parse and print */
parse(&frm);
break;
}
}
return 0;
}
static void read_dump(int fd)
{
struct hcidump_hdr dh;
struct btsnoop_pkt dp;
struct pktlog_hdr ph;
struct frame frm;
int err;
frm.data = malloc(HCI_MAX_FRAME_SIZE);
if (!frm.data) {
perror("Can't allocate data buffer");
exit(1);
}
while (1) {
if (parser.flags & DUMP_PKTLOG)
err = read_n(fd, (void *) &ph, PKTLOG_HDR_SIZE);
else if (parser.flags & DUMP_BTSNOOP)
err = read_n(fd, (void *) &dp, BTSNOOP_PKT_SIZE);
else
err = read_n(fd, (void *) &dh, HCIDUMP_HDR_SIZE);
if (err < 0)
goto failed;
if (!err)
goto done;
if (parser.flags & DUMP_PKTLOG) {
switch (ph.type) {
case 0x00:
((uint8_t *) frm.data)[0] = HCI_COMMAND_PKT;
frm.in = 0;
break;
case 0x01:
((uint8_t *) frm.data)[0] = HCI_EVENT_PKT;
frm.in = 1;
break;
case 0x02:
((uint8_t *) frm.data)[0] = HCI_ACLDATA_PKT;
frm.in = 0;
break;
case 0x03:
((uint8_t *) frm.data)[0] = HCI_ACLDATA_PKT;
frm.in = 1;
break;
default:
lseek(fd, be32toh(ph.len) - 9, SEEK_CUR);
continue;
}
frm.data_len = be32toh(ph.len) - 8;
err = read_n(fd, frm.data + 1, frm.data_len - 1);
} else if (parser.flags & DUMP_BTSNOOP) {
uint32_t opcode;
uint8_t pkt_type;
switch (btsnoop_type) {
case 1001:
if (be32toh(dp.flags) & 0x02) {
if (be32toh(dp.flags) & 0x01)
pkt_type = HCI_EVENT_PKT;
else
pkt_type = HCI_COMMAND_PKT;
} else
pkt_type = HCI_ACLDATA_PKT;
((uint8_t *) frm.data)[0] = pkt_type;
frm.data_len = be32toh(dp.len) + 1;
err = read_n(fd, frm.data + 1, frm.data_len - 1);
break;
case 1002:
frm.data_len = be32toh(dp.len);
err = read_n(fd, frm.data, frm.data_len);
break;
case 2001:
opcode = be32toh(dp.flags) & 0xffff;
switch (opcode) {
case 2:
pkt_type = HCI_COMMAND_PKT;
frm.in = 0;
break;
case 3:
pkt_type = HCI_EVENT_PKT;
frm.in = 1;
break;
case 4:
pkt_type = HCI_ACLDATA_PKT;
frm.in = 0;
break;
case 5:
pkt_type = HCI_ACLDATA_PKT;
frm.in = 1;
break;
case 6:
pkt_type = HCI_SCODATA_PKT;
frm.in = 0;
break;
case 7:
pkt_type = HCI_SCODATA_PKT;
frm.in = 1;
break;
default:
pkt_type = 0xff;
break;
}
((uint8_t *) frm.data)[0] = pkt_type;
frm.data_len = be32toh(dp.len) + 1;
err = read_n(fd, frm.data + 1, frm.data_len - 1);
}
} else {
frm.data_len = btohs(dh.len);
err = read_n(fd, frm.data, frm.data_len);
}
if (err < 0)
goto failed;
if (!err)
goto done;
frm.ptr = frm.data;
frm.len = frm.data_len;
if (parser.flags & DUMP_PKTLOG) {
uint64_t ts;
ts = be64toh(ph.ts);
frm.ts.tv_sec = ts >> 32;
frm.ts.tv_usec = ts & 0xffffffff;
} else if (parser.flags & DUMP_BTSNOOP) {
uint64_t ts;
frm.in = be32toh(dp.flags) & 0x01;
ts = be64toh(dp.ts) - 0x00E03AB44A676000ll;
frm.ts.tv_sec = (ts / 1000000ll) + 946684800ll;
frm.ts.tv_usec = ts % 1000000ll;
} else {
frm.in = dh.in;
frm.ts.tv_sec = btohl(dh.ts_sec);
frm.ts.tv_usec = btohl(dh.ts_usec);
}
parse(&frm);
}
done:
free(frm.data);
return;
failed:
perror("Read failed");
free(frm.data);
exit(1);
}
static int open_file(char *file, int mode, unsigned long flags)
{
unsigned char buf[BTSNOOP_HDR_SIZE];
struct btsnoop_hdr *hdr = (struct btsnoop_hdr *) buf;
int fd, len, open_flags;
if (mode == WRITE || mode == PPPDUMP || mode == AUDIO)
open_flags = O_WRONLY | O_CREAT | O_TRUNC;
else
open_flags = O_RDONLY;
fd = open(file, open_flags, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
if (fd < 0) {
perror("Can't open dump file");
exit(1);
}
if (mode == READ) {
len = read(fd, buf, BTSNOOP_HDR_SIZE);
if (len != BTSNOOP_HDR_SIZE) {
lseek(fd, 0, SEEK_SET);
return fd;
}
if (!memcmp(hdr->id, btsnoop_id, sizeof(btsnoop_id))) {
parser.flags |= DUMP_BTSNOOP;
btsnoop_version = be32toh(hdr->version);
btsnoop_type = be32toh(hdr->type);
printf("btsnoop version: %d datalink type: %d\n",
btsnoop_version, btsnoop_type);
if (btsnoop_version != 1) {
fprintf(stderr, "Unsupported BTSnoop version\n");
exit(1);
}
if (btsnoop_type != 1001 && btsnoop_type != 1002 &&
btsnoop_type != 2001) {
fprintf(stderr, "Unsupported BTSnoop datalink type\n");
exit(1);
}
} else {
if (buf[0] == 0x00 && buf[1] == 0x00) {
parser.flags |= DUMP_PKTLOG;
printf("packet logger data format\n");
}
parser.flags &= ~DUMP_BTSNOOP;
lseek(fd, 0, SEEK_SET);
return fd;
}
} else {
if (flags & DUMP_BTSNOOP) {
btsnoop_version = 1;
btsnoop_type = 1002;
memcpy(hdr->id, btsnoop_id, sizeof(btsnoop_id));
hdr->version = htobe32(btsnoop_version);
hdr->type = htobe32(btsnoop_type);
printf("btsnoop version: %d datalink type: %d\n",
btsnoop_version, btsnoop_type);
len = write(fd, buf, BTSNOOP_HDR_SIZE);
if (len < 0) {
perror("Can't create dump header");
exit(1);
}
if (len != BTSNOOP_HDR_SIZE) {
fprintf(stderr, "Header size mismatch\n");
exit(1);
}
}
}
return fd;
}
static int open_socket(int dev, unsigned long flags)
{
struct sockaddr_hci addr;
struct hci_filter flt;
int sk, opt;
/* Create HCI socket */
sk = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HCI);
if (sk < 0) {
perror("Can't create raw socket");
return -1;
}
opt = 1;
if (setsockopt(sk, SOL_HCI, HCI_DATA_DIR, &opt, sizeof(opt)) < 0) {
perror("Can't enable data direction info");
goto fail;
}
opt = 1;
if (setsockopt(sk, SOL_HCI, HCI_TIME_STAMP, &opt, sizeof(opt)) < 0) {
perror("Can't enable time stamp");
goto fail;
}
/* Setup filter */
hci_filter_clear(&flt);
hci_filter_all_ptypes(&flt);
hci_filter_all_events(&flt);
if (setsockopt(sk, SOL_HCI, HCI_FILTER, &flt, sizeof(flt)) < 0) {
perror("Can't set filter");
goto fail;
}
/* Bind socket to the HCI device */
memset(&addr, 0, sizeof(addr));
addr.hci_family = AF_BLUETOOTH;
addr.hci_dev = dev;
if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
printf("Can't attach to device hci%d. %s(%d)\n",
dev, strerror(errno), errno);
goto fail;
}
return sk;
fail:
close(sk);
return -1;
}
static struct {
char *name;
int flag;
} filters[] = {
{ "lmp", FILT_LMP },
{ "hci", FILT_HCI },
{ "sco", FILT_SCO },
{ "l2cap", FILT_L2CAP },
{ "a2mp", FILT_A2MP },
{ "rfcomm", FILT_RFCOMM },
{ "sdp", FILT_SDP },
{ "bnep", FILT_BNEP },
{ "cmtp", FILT_CMTP },
{ "hidp", FILT_HIDP },
{ "hcrp", FILT_HCRP },
{ "att", FILT_ATT },
{ "smp", FILT_SMP },
{ "avdtp", FILT_AVDTP },
{ "avctp", FILT_AVCTP },
{ "obex", FILT_OBEX },
{ "capi", FILT_CAPI },
{ "ppp", FILT_PPP },
{ "sap", FILT_SAP },
{ "csr", FILT_CSR },
{ "dga", FILT_DGA },
{ 0 }
};
static unsigned long parse_filter(int argc, char **argv)
{
unsigned long filter = 0;
int i,n;
for (i = 0; i < argc; i++) {
for (n = 0; filters[n].name; n++) {
if (!strcasecmp(filters[n].name, argv[i])) {
filter |= filters[n].flag;
break;
}
}
}
return filter;
}
static void usage(void)
{
printf(
"Usage: hcidump [OPTION...] [filter]\n"
" -i, --device=hci_dev HCI device\n"
" -l, --snap-len=len Snap len (in bytes)\n"
" -p, --psm=psm Default PSM\n"
" -m, --manufacturer=compid Default manufacturer\n"
" -w, --save-dump=file Save dump to a file\n"
" -r, --read-dump=file Read dump from a file\n"
" -t, --ts Display time stamps\n"
" -a, --ascii Dump data in ascii\n"
" -x, --hex Dump data in hex\n"
" -X, --ext Dump data in hex and ascii\n"
" -R, --raw Dump raw data\n"
" -C, --cmtp=psm PSM for CMTP\n"
" -H, --hcrp=psm PSM for HCRP\n"
" -O, --obex=port Channel/PSM for OBEX\n"
" -P, --ppp=channel Channel for PPP\n"
" -S, --sap=channel Channel for SAP\n"
" -D, --pppdump=file Extract PPP traffic\n"
" -A, --audio=file Extract SCO audio data\n"
" -Y, --novendor No vendor commands or events\n"
" -h, --help Give this help list\n"
" -v, --version Give version information\n"
" --usage Give a short usage message\n"
);
}
static struct option main_options[] = {
{ "device", 1, 0, 'i' },
{ "snap-len", 1, 0, 'l' },
{ "psm", 1, 0, 'p' },
{ "manufacturer", 1, 0, 'm' },
{ "save-dump", 1, 0, 'w' },
{ "read-dump", 1, 0, 'r' },
{ "timestamp", 0, 0, 't' },
{ "ascii", 0, 0, 'a' },
{ "hex", 0, 0, 'x' },
{ "ext", 0, 0, 'X' },
{ "raw", 0, 0, 'R' },
{ "cmtp", 1, 0, 'C' },
{ "hcrp", 1, 0, 'H' },
{ "obex", 1, 0, 'O' },
{ "ppp", 1, 0, 'P' },
{ "sap", 1, 0, 'S' },
{ "pppdump", 1, 0, 'D' },
{ "audio", 1, 0, 'A' },
{ "novendor", 0, 0, 'Y' },
{ "help", 0, 0, 'h' },
{ "version", 0, 0, 'v' },
{ 0 }
};
int main(int argc, char *argv[])
{
unsigned long flags = 0;
unsigned long filter = 0;
int device = 0;
int defpsm = 0;
int defcompid = DEFAULT_COMPID;
int opt, pppdump_fd = -1, audio_fd = -1;
uint16_t obex_port;
while ((opt = getopt_long(argc, argv,
"i:l:p:m:w:r:taxXRC:H:O:P:S:D:A:Yhv",
main_options, NULL)) != -1) {
switch(opt) {
case 'i':
if (strcasecmp(optarg, "none") && strcasecmp(optarg, "system"))
device = atoi(optarg + 3);
else
device = HCI_DEV_NONE;
break;
case 'l':
snap_len = atoi(optarg);
break;
case 'p':
defpsm = atoi(optarg);
break;
case 'm':
defcompid = atoi(optarg);
break;
case 'w':
mode = WRITE;
dump_file = strdup(optarg);
break;
case 'r':
mode = READ;
dump_file = strdup(optarg);
break;
case 't':
flags |= DUMP_TSTAMP;
break;
case 'a':
flags |= DUMP_ASCII;
break;
case 'x':
flags |= DUMP_HEX;
break;
case 'X':
flags |= DUMP_EXT;
break;
case 'R':
flags |= DUMP_RAW;
break;
case 'C':
set_proto(0, atoi(optarg), 0, SDP_UUID_CMTP);
break;
case 'H':
set_proto(0, atoi(optarg), 0, SDP_UUID_HARDCOPY_CONTROL_CHANNEL);
break;
case 'O':
obex_port = atoi(optarg);
if (obex_port > 31)
set_proto(0, obex_port, 0, SDP_UUID_OBEX);
else
set_proto(0, 0, obex_port, SDP_UUID_OBEX);
break;
case 'P':
set_proto(0, 0, atoi(optarg), SDP_UUID_LAN_ACCESS_PPP);
break;
case 'S':
set_proto(0, 0, atoi(optarg), SDP_UUID_SIM_ACCESS);
break;
case 'D':
pppdump_file = strdup(optarg);
break;
case 'A':
audio_file = strdup(optarg);
break;
case 'Y':
flags |= DUMP_NOVENDOR;
break;
case 'v':
printf("%s\n", VERSION);
exit(0);
case 'h':
default:
usage();
exit(0);
}
}
argc -= optind;
argv += optind;
optind = 0;
printf("HCI sniffer - Bluetooth packet analyzer ver %s\n", VERSION);
if (argc > 0)
filter = parse_filter(argc, argv);
/* Default settings */
if (!filter)
filter = ~0L;
if (pppdump_file)
pppdump_fd = open_file(pppdump_file, PPPDUMP, flags);
if (audio_file)
audio_fd = open_file(audio_file, AUDIO, flags);
switch (mode) {
case PARSE:
flags |= DUMP_VERBOSE;
init_parser(flags, filter, defpsm, defcompid,
pppdump_fd, audio_fd);
process_frames(device, open_socket(device, flags), -1, flags);
break;
case READ:
flags |= DUMP_VERBOSE;
init_parser(flags, filter, defpsm, defcompid,
pppdump_fd, audio_fd);
read_dump(open_file(dump_file, mode, flags));
break;
case WRITE:
flags |= DUMP_BTSNOOP;
process_frames(device, open_socket(device, flags),
open_file(dump_file, mode, flags), flags);
break;
}
return 0;
}
Reference in New Issue View Git Blame Copy Permalink