bpasniff.c

Linux下蓝牙探测工具

/*
 *
 *  BlueZ - Bluetooth protocol stack for Linux
 *
 *  Copyright (C) 2004-2007  Marcel Holtmann <[e-mail]>
 *
 *
 *  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;
}