/* * *  BlueZ - Bluetooth protocol stack for Linux * *  Copyright (C) 2000-2002  Maxim Krasnyansky <maxk@qualcomm.com> *  Copyright (C) 2003-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 <fcntl.h>#include <unistd.h>#include <stdlib.h>#include <string.h>#include <getopt.h>#include <sys/stat.h>#include <sys/types.h>#include <sys/ioctl.h>#include <sys/socket.h>#include <bluetooth/bluetooth.h>#include <bluetooth/hci.h>#include <bluetooth/hci_lib.h>#include <arpa/inet.h>#include <netinet/in.h>#include <netdb.h>#include "parser/parser.h"#include "parser/sdp.h"#if __BYTE_ORDER == __LITTLE_ENDIANstatic inline uint64_t ntoh64(uint64_t n){	uint64_t h;	uint64_t tmp = ntohl(n & 0x00000000ffffffff);	h = ntohl(n >> 32);	h |= tmp << 32;	return h;}#elif __BYTE_ORDER == __BIG_ENDIAN#define ntoh64(x) (x)#else#error "Unknown byte order"#endif#define hton64(x) ntoh64(x)#define SNAP_LEN 	HCI_MAX_FRAME_SIZE#define DEFAULT_PORT	10839;/* Modes */enum {	PARSE,	READ,	WRITE,	RECEIVE,	SEND,	AUDIO};/* Default options */static int  device;static int  snap_len = SNAP_LEN;static int  defpsm = 0;static int  defcompid = DEFAULT_COMPID;static int  mode = PARSE;static int  permcheck = 1;static long flags;static long filter;static char *dump_file = NULL;static char *audio_file = NULL;static in_addr_t dump_addr = INADDR_LOOPBACK;static in_port_t dump_port = DEFAULT_PORT;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;static inline int read_n(int fd, char *buf, int len){	register 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){	register 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 void 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;	char *buf, *ctrl;	int len, hdr_size = HCIDUMP_HDR_SIZE;	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");		exit(1);	}	dh = (void *) buf;	dp = (void *) buf;	frm.data = buf + hdr_size;	ctrl = malloc(100);	if (!ctrl) {		perror("Can't allocate control buffer");		exit(1);	}	if (dev == HCI_DEV_NONE)		printf("system: ");	else		printf("device: hci%d ", dev);	printf("snap_len: %d filter: 0x%lx\n", snap_len, filter);	memset(&msg, 0, sizeof(msg));	while (1) {		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, 0);		if (len < 0) {			perror("Receive failed");			exit(1);		}		/* Process control message */		frm.data_len = len;		frm.dev_id = dev;		frm.in = 0;		frm.audio_fd = parser.audio_fd;		cmsg = CMSG_FIRSTHDR(&msg);		while (cmsg) {			switch (cmsg->cmsg_type) {			case HCI_CMSG_DIR:				frm.in = *((int *) CMSG_DATA(cmsg));				break;			case HCI_CMSG_TSTAMP:				frm.ts = *((struct timeval *) CMSG_DATA(cmsg));				break;			}			cmsg = CMSG_NXTHDR(&msg, cmsg);		}		frm.ptr = frm.data;		frm.len = frm.data_len;		switch (mode) {		case WRITE:		case SEND:			/* Save or send dump */			if (flags & DUMP_BTSNOOP) {				uint64_t ts;				uint8_t pkt_type = ((uint8_t *) frm.data)[0];				dp->size = htonl(frm.data_len);				dp->len  = dp->size;				dp->flags = ntohl(frm.in & 0x01);				dp->drops = 0;				ts = (frm.ts.tv_sec - 946684800ll) * 1000000ll + frm.ts.tv_usec;				dp->ts = hton64(ts + 0x00E03AB44A676000ll);				if (pkt_type == HCI_COMMAND_PKT ||						pkt_type == HCI_EVENT_PKT)					dp->flags |= ntohl(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");				exit(1);			}			break;		default:			/* Parse and print */			parse(&frm);			break;		}	}}static void read_dump(int fd){	struct hcidump_hdr dh;	struct btsnoop_pkt dp;	struct frame frm;	uint8_t pkt_type;	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_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)			return;		if (parser.flags & DUMP_BTSNOOP) {			switch (btsnoop_type) {			case 1001:				if (ntohl(dp.flags) & 0x02) {					if (ntohl(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 = ntohl(dp.len) + 1;				err = read_n(fd, frm.data + 1, frm.data_len - 1);				break;			case 1002:				frm.data_len = ntohl(dp.len);				err = read_n(fd, frm.data, frm.data_len);				break;			}		} else {			frm.data_len = btohs(dh.len);			err = read_n(fd, frm.data, frm.data_len);		}		if (err < 0)			goto failed;		if (!err)			return;		frm.ptr = frm.data;		frm.len = frm.data_len;		if (parser.flags & DUMP_BTSNOOP) {			uint64_t ts;			frm.in = ntohl(dp.flags) & 0x01;			ts = ntoh64(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);	}failed:	perror("Read failed");	exit(1);}static int open_file(char *file, int mode, unsigned long flags){	struct btsnoop_hdr hdr;	int fd, len, open_flags;	if (mode == WRITE || mode == AUDIO) {		if (flags & DUMP_BTSNOOP)			open_flags = O_WRONLY | O_CREAT;		else			open_flags = O_WRONLY | O_CREAT | O_APPEND;	} 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, &hdr, 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 = ntohl(hdr.version);			btsnoop_type = ntohl(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) {				fprintf(stderr, "Unsupported BTSnoop datalink type\n");				exit(1);			}		} else {			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 = htonl(btsnoop_version);			hdr.type = htonl(btsnoop_type);			printf("btsnoop version: %d datalink type: %d\n",						btsnoop_version, btsnoop_type);			len = write(fd, &hdr, 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);