/* * *  BlueZ - Bluetooth protocol stack for Linux * *  Copyright (C) 2004-2005  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);	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;}