/*
*
* BlueZ - Bluetooth protocol stack for Linux
*
* Copyright (C) 2004-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 <ctype.h>
#include <unistd.h>
#include <stdlib.h>
#include <malloc.h>
#include <getopt.h>
#include <signal.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <bluetooth/bluetooth.h>
#include <bluetooth/hci.h>
#include <bluetooth/hci_lib.h>
#include <netinet/in.h>
#include "parser/parser.h"
static volatile sig_atomic_t __io_canceled = 0;
static void sig_hup(int sig)
{
}
static void sig_term(int sig)
{
__io_canceled = 1;
}
static int read_revision(int dd, char *revision, int size)
{
struct hci_request rq;
unsigned char req[] = { 0x07 };
unsigned char buf[46];
memset(&rq, 0, sizeof(rq));
rq.ogf = OGF_VENDOR_CMD;
rq.ocf = 0x000e;
rq.cparam = req;
rq.clen = sizeof(req);
rq.rparam = &buf;
rq.rlen = sizeof(buf);
if (hci_send_req(dd, &rq, 1000) < 0)
return -1;
if (buf[0] > 0) {
errno = EIO;
return -1;
}
if (revision)
strncpy(revision, (char *) (buf + 1), size);
return 0;
}
static int enable_sniffer(int dd, uint8_t enable)
{
struct hci_request rq;
unsigned char req[] = { 0x00, enable };
unsigned char buf[1];
memset(&rq, 0, sizeof(rq));
rq.ogf = OGF_VENDOR_CMD;
rq.ocf = 0x000e;
rq.cparam = req;
rq.clen = sizeof(req);
rq.rparam = &buf;
rq.rlen = sizeof(buf);
if (hci_send_req(dd, &rq, 1000) < 0)
return -1;
if (buf[0] > 0) {
errno = EIO;
return -1;
}
return 0;
}
static int enable_sync(int dd, uint8_t enable, bdaddr_t *bdaddr)
{
struct hci_request rq;
unsigned char req[] = { 0x01, enable,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0xfa, 0x00 };
memcpy(req + 2, bdaddr, 6);
memset(&rq, 0, sizeof(rq));
rq.ogf = OGF_VENDOR_CMD;
rq.ocf = 0x000e;
rq.cparam = req;
rq.clen = sizeof(req);
hci_send_req(dd, &rq, 1000);
return 0;
}
static char *type2str(uint8_t type)
{
switch (type) {
case 0x00:
return "NULL";
case 0x01:
return "POLL";
case 0x02:
return "FHS";
case 0x03:
return "DM1";
case 0x04:
return "DH1";
case 0x05:
return "HV1";
case 0x06:
return "HV2";
case 0x07:
return "HV3";
case 0x08:
return "DV";
case 0x09:
return "AUX1";
case 0x0a:
return "DM3";
case 0x0b:
return "DH3";
case 0x0c:
return "EV4";
case 0x0d:
return "EV5";
case 0x0e:
return "DM5";
case 0x0f:
return "DH5";
case 0xff:
return "ID";
default:
return "UNK";
}
}
static void decode(unsigned char *buf, int count)
{
struct frame frm;
uint8_t id, status, channel;
uint16_t num, len;
uint32_t time;
uint8_t type, addr, temp, hdr;
uint8_t flow, arqn, seqn, hec, llid, pflow;
uint16_t plen;
if (count < 7)
return;
id = buf[0];
num = ntohs(bt_get_unaligned((uint16_t *) (buf + 1)));
len = btohs(bt_get_unaligned((uint16_t *) (buf + 3)));
status = buf[5];
time = ntohl(bt_get_unaligned((uint32_t *) (buf + 6)));
channel = buf[10];
if (len < 8)
return;
type = (len < 7) ? 0xff : bt_get_unaligned((uint8_t *) (buf + 11));
if (type < 2)
return;
p_indent(-1, NULL);
memset(&frm, 0, sizeof(frm));
frm.data = buf + 12;
frm.data_len = count - 12;
frm.ptr = frm.data;
frm.len = frm.data_len;
frm.in = 0;
frm.master = 0;
frm.handle = 0;
frm.flags = 0;
p_indent(0, &frm);
printf("BPA: id %d num %d status 0x%02x time %d channel %2d len %d\n",
id, num, status, time, channel, len - 6);
if (type < 3) {
printf(" %s\n", type2str(type));
raw_dump(1, &frm);
return;
}
addr = bt_get_unaligned((uint8_t *) (buf + 12));
temp = bt_get_unaligned((uint8_t *) (buf + 13));
flow = (temp & 0x04) >> 2;
arqn = (temp & 0x02) >> 1;
seqn = (temp & 0x01);
hec = bt_get_unaligned((uint8_t *) (buf + 14));
hdr = bt_get_unaligned((uint8_t *) (buf + 20));
plen = ((hdr & 0x10) >> 4) | ((hdr & 0x08) >> 2) | (hdr & 0x04) | ((hdr & 0x02) << 2) | ((hdr & 0x01) << 4);
pflow = ((hdr & 0x20) >> 5);
llid = ((hdr & 0x80) >> 7) | ((hdr & 0x40) >> 5);
hdr = bt_get_unaligned((uint8_t *) (buf + 21));
plen = plen | ((hdr & 0x80) >> 2) | (hdr & 0x40) | ((hdr & 0x20) << 2) | ((hdr & 0x08) << 4);
p_indent(0, &frm);
printf("%s: addr 0x%02x flow %d arqn %d seqn %d hec 0x%02x llid %d pflow %d plen %d\n",
type2str(type), addr, flow, arqn, seqn, hec, llid, pflow, plen);
if (type == 0x03 && llid == 3) {
memset(&frm, 0, sizeof(frm));
frm.data = buf + 22;
frm.data_len = plen;
frm.ptr = frm.data;
frm.len = frm.data_len;
frm.in = 0;
frm.master = 1;
frm.handle = 0;
frm.flags = llid;
lmp_dump(1, &frm);
return;
}
raw_dump(1, &frm);
}
static void process_frames(int dev)
{
struct sigaction sa;
struct hci_filter flt;
unsigned char *buf;
int dd, size = 2048;
buf = malloc(size);
if (!buf) {
fprintf(stderr, "Can't allocate buffer for hci%d: %s (%d)\n",
dev, strerror(errno), errno);
return;
}
dd = hci_open_dev(dev);
if (dd < 0) {
fprintf(stderr, "Can't open device hci%d: %s (%d)\n",
dev, strerror(errno), errno);
free(buf);
return;
}
hci_filter_clear(&flt);
hci_filter_set_ptype(HCI_VENDOR_PKT, &flt);
hci_filter_set_ptype(HCI_EVENT_PKT, &flt);
hci_filter_set_event(EVT_VENDOR, &flt);
if (setsockopt(dd, SOL_HCI, HCI_FILTER, &flt, sizeof(flt)) < 0) {
fprintf(stderr, "Can't set filter for hci%d: %s (%d)\n",
dev, strerror(errno), errno);
hci_close_dev(dd);
free(buf);
return;
}
memset(&sa, 0, sizeof(sa));
sa.sa_flags = SA_NOCLDSTOP;
sa.sa_handler = SIG_IGN;
sigaction(SIGCHLD, &sa, NULL);
sigaction(SIGPIPE, &sa, NULL);
sa.sa_handler = sig_term;
sigaction(SIGTERM, &sa, NULL);
sigaction(SIGINT, &sa, NULL);
sa.sa_handler = sig_hup;
sigaction(SIGHUP, &sa, NULL);
while (!__io_canceled) {
int len;
len = read(dd, buf, size);
if (len < 0)
break;
if (len < 2)
continue;
if (buf[0] == 0x04 && buf[1] == 0xff) {
if (buf[3] == 0x02) {
switch (buf[4]) {
case 0x00:
printf("Waiting for synchronization...\n");
break;
case 0x08:
printf("Synchronization lost\n");
__io_canceled = 1;
break;
default:
printf("Unknown event 0x%02x\n", buf[4]);
break;
}
}
}
if (buf[0] != 0xff)
continue;
decode(buf + 1, len - 1);
}
hci_close_dev(dd);
free(buf);
}
static void usage(void)
{
printf("bpasniff - Utility for the BPA 100/105 sniffers\n\n");
printf("Usage:\n"
"\tbpasniff [-i <dev>] <master-bdaddr>\n");
}
static struct option main_options[] = {
{ "help", 0, 0, 'h' },
{ "device", 1, 0, 'i' },
{ 0, 0, 0, 0}
};
int main(int argc, char *argv[])
{
struct hci_dev_info di;
struct hci_version ver;
char rev[46];
bdaddr_t bdaddr;
int dd, opt, dev = 0;
bacpy(&bdaddr, BDADDR_ANY);
while ((opt=getopt_long(argc, argv, "+i:h", main_options, NULL)) != -1) {
switch (opt) {
case 'i':
dev = hci_devid(optarg);
if (dev < 0) {
perror("Invalid device");
exit(1);
}
break;
case 'h':
default:
usage();
exit(0);
}
}
argc -= optind;
argv += optind;
optind = 0;
argc -= optind;
argv += optind;
optind = 0;
if (argc < 1) {
usage();
exit(1);
}
str2ba(argv[0], &bdaddr);
dd = hci_open_dev(dev);
if (dd < 0) {
fprintf(stderr, "Can't open device hci%d: %s (%d)\n",
dev, strerror(errno), errno);
exit(1);
}
if (hci_devinfo(dev, &di) < 0) {
fprintf(stderr, "Can't get device info for hci%d: %s (%d)\n",
dev, strerror(errno), errno);
hci_close_dev(dd);
exit(1);
}
if (hci_read_local_version(dd, &ver, 1000) < 0) {
fprintf(stderr, "Can't read version info for hci%d: %s (%d)\n",
dev, strerror(errno), errno);
hci_close_dev(dd);
exit(1);
}
if (ver.manufacturer != 12) {
fprintf(stderr, "Can't find sniffer at hci%d: %s (%d)\n",
dev, strerror(ENOSYS), ENOSYS);
hci_close_dev(dd);
exit(1);
}
if (read_revision(dd, rev, sizeof(rev)) < 0) {
fprintf(stderr, "Can't read revision info for hci%d: %s (%d)\n",
dev, strerror(errno), errno);
hci_close_dev(dd);
exit(1);
}
printf("%s\n", rev);
if (enable_sniffer(dd, 0x01) < 0) {
fprintf(stderr, "Can't enable sniffer for hci%d: %s (%d)\n",
dev, strerror(errno), errno);
hci_close_dev(dd);
exit(1);
}
if (enable_sync(dd, 0x01, &bdaddr) < 0) {
fprintf(stderr, "Can't enable sync for hci%d: %s (%d)\n",
dev, strerror(errno), errno);
enable_sniffer(dd, 0x00);
hci_close_dev(dd);
exit(1);
}
init_parser(DUMP_EXT | DUMP_VERBOSE, ~0L, 0, DEFAULT_COMPID, -1, -1);
process_frames(dev);
if (enable_sync(dd, 0x00, &bdaddr) < 0) {
fprintf(stderr, "Can't disable sync for hci%d: %s (%d)\n",
dev, strerror(errno), errno);
enable_sniffer(dd, 0x00);
hci_close_dev(dd);
exit(1);
}
if (enable_sniffer(dd, 0x00) < 0) {
fprintf(stderr, "Can't disable sniffer for hci%d: %s (%d)\n",
dev, strerror(errno), errno);
hci_close_dev(dd);
exit(1);
}
hci_close_dev(dd);
return 0;
}