bluez/tools/btsnoop.c

/*
 *
 *  BlueZ - Bluetooth protocol stack for Linux
 *
 *  Copyright (C) 2011-2012  Intel Corporation
 *  Copyright (C) 2004-2010  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 <stdint.h>
#include <stdbool.h>
#include <string.h>
#include <getopt.h>
#include <arpa/inet.h>
#include <sys/stat.h>

#include "monitor/btsnoop.h"

static 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;
}

#define hton64(x)     ntoh64(x)

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 const uint8_t btsnoop_id[] = { 0x62, 0x74, 0x73, 0x6e,
				      0x6f, 0x6f, 0x70, 0x00 };

static const uint32_t btsnoop_version = 1;

static int create_btsnoop(const char *path)
{
	struct btsnoop_hdr hdr;
	ssize_t written;
	int fd;

	fd = open(path, O_WRONLY | O_CREAT | O_TRUNC | O_CLOEXEC,
				S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
	if (fd < 0) {
		perror("failed to output file");
		return -1;
	}

	memcpy(hdr.id, btsnoop_id, sizeof(btsnoop_id));
	hdr.version = htonl(btsnoop_version);
	hdr.type = htonl(2001);

	written = write(fd, &hdr, BTSNOOP_HDR_SIZE);
	if (written < 0) {
		perror("failed to write output header");
		close(fd);
		return -1;
	}

	return fd;
}

static int open_btsnoop(const char *path, uint32_t *type)
{
	struct btsnoop_hdr hdr;
	ssize_t len;
	int fd;

	fd = open(path, O_RDONLY | O_CLOEXEC);
	if (fd < 0) {
		perror("failed to open input file");
		return -1;
	}

	len = read(fd, &hdr, BTSNOOP_HDR_SIZE);
	if (len < 0 || len != BTSNOOP_HDR_SIZE) {
		perror("failed to read input header");
		close(fd);
		return -1;
	}

	if (memcmp(hdr.id, btsnoop_id, sizeof(btsnoop_id))) {
		fprintf(stderr, "not a valid btsnoop header\n");
		close(fd);
		return -1;
	}

	if (ntohl(hdr.version) != btsnoop_version) {
		fprintf(stderr, "invalid btsnoop version\n");
		close(fd);
		return -1;
	}

	if (type)
		*type = ntohl(hdr.type);

	return fd;
}

#define MAX_MERGE 8

static void command_merge(const char *output, int argc, char *argv[])
{
	struct btsnoop_pkt input_pkt[MAX_MERGE];
	unsigned char buf[2048];
	int output_fd, input_fd[MAX_MERGE], num_input = 0;
	int i, select_input;
	ssize_t len, written;
	uint32_t toread, flags;
	uint16_t index, opcode;

	if (argc > MAX_MERGE) {
		fprintf(stderr, "only up to %d files allowed\n", MAX_MERGE);
		return;
	}

	for (i = 0; i < argc; i++) {
		uint32_t type;
		int fd;

		fd = open_btsnoop(argv[i], &type);
		if (fd < 0)
			break;

		if (type != 1002) {
			fprintf(stderr, "unsupported link data type %u\n",
									type);
			close(fd);
			break;
		}

		input_fd[num_input++] = fd;
	}

	if (num_input != argc) {
		fprintf(stderr, "failed to open all input files\n");
		goto close_input;
	}

	output_fd = create_btsnoop(output);
	if (output_fd < 0)
		goto close_input;

	for (i = 0; i < num_input; i++) {
		len = read(input_fd[i], &input_pkt[i], BTSNOOP_PKT_SIZE);
		if (len < 0 || len != BTSNOOP_PKT_SIZE) {
			close(input_fd[i]);
			input_fd[i] = -1;
		}
	}

next_packet:
	select_input = -1;

	for (i = 0; i < num_input; i++) {
		uint64_t ts;

		if (input_fd[i] < 0)
			continue;

		if (select_input < 0) {
			select_input = i;
			continue;
		}

		ts = ntoh64(input_pkt[i].ts);

		if (ts < ntoh64(input_pkt[select_input].ts))
			select_input = i;
	}

	if (select_input < 0)
		goto close_output;

	toread = ntohl(input_pkt[select_input].size);
	flags = ntohl(input_pkt[select_input].flags);

	len = read(input_fd[select_input], buf, toread);
	if (len < 0 || len != (ssize_t) toread) {
		close(input_fd[select_input]);
		input_fd[select_input] = -1;
		goto next_packet;
	}

	written = input_pkt[select_input].size = htonl(toread - 1);
	written = input_pkt[select_input].len = htonl(toread - 1);

	switch (buf[0]) {
	case 0x01:
		opcode = BTSNOOP_OPCODE_COMMAND_PKT;
		break;
	case 0x02:
		if (flags & 0x01)
			opcode = BTSNOOP_OPCODE_ACL_RX_PKT;
		else
			opcode = BTSNOOP_OPCODE_ACL_TX_PKT;
		break;
	case 0x03:
		if (flags & 0x01)
			opcode = BTSNOOP_OPCODE_SCO_RX_PKT;
		else
			opcode = BTSNOOP_OPCODE_ACL_TX_PKT;
		break;
	case 0x04:
		opcode = BTSNOOP_OPCODE_EVENT_PKT;
		break;
	default:
		goto skip_write;
	}

	index = select_input;
	input_pkt[select_input].flags = htonl((index << 16) | opcode);

	written = write(output_fd, &input_pkt[select_input], BTSNOOP_PKT_SIZE);
	if (written != BTSNOOP_PKT_SIZE) {
		fprintf(stderr, "write of packet header failed\n");
		goto close_output;
	}

	written = write(output_fd, buf + 1, toread - 1);
	if (written != (ssize_t) toread - 1) {
		fprintf(stderr, "write of packet data failed\n");
		goto close_output;
	}

skip_write:
	len = read(input_fd[select_input],
				&input_pkt[select_input], BTSNOOP_PKT_SIZE);
	if (len < 0 || len != BTSNOOP_PKT_SIZE) {
		close(input_fd[select_input]);
		input_fd[select_input] = -1;
	}

	goto next_packet;

close_output:
	close(output_fd);

close_input:
	for (i = 0; i < num_input; i++)
		close(input_fd[i]);
}

static void command_extract_eir(const char *input)
{
	struct btsnoop_pkt pkt;
	unsigned char buf[2048];
	ssize_t len;
	uint32_t type, toread, flags;
	uint16_t opcode;
	int fd, count = 0;

	fd = open_btsnoop(input, &type);
	if (fd < 0)
		return;

	if (type != 2001) {
		fprintf(stderr, "unsupported link data type %u\n", type);
		close(fd);
		return;
	}

next_packet:
	len = read(fd, &pkt, BTSNOOP_PKT_SIZE);
	if (len < 0 || len != BTSNOOP_PKT_SIZE)
		goto close_input;

	toread = ntohl(pkt.size);
	flags = ntohl(pkt.flags);

	opcode = flags & 0x00ff;

	len = read(fd, buf, toread);
	if (len < 0 || len != (ssize_t) toread) {
		fprintf(stderr, "failed to read packet data\n");
		goto close_input;
	}

	switch (opcode) {
	case BTSNOOP_OPCODE_EVENT_PKT:
		/* extended inquiry result event */
		if (buf[0] == 0x2f) {
			uint8_t *eir_ptr, eir_len, i;

			eir_len = buf[1] - 15;
			eir_ptr = buf + 17;

			if (eir_len < 1 || eir_len > 240)
				break;

			printf("\t[Extended Inquiry Data with %u bytes]\n",
								eir_len);
			printf("\t\t");
			for (i = 0; i < eir_len; i++) {
				printf("0x%02x", eir_ptr[i]);
				if (((i + 1) % 8) == 0) {
					if (i < eir_len - 1)
						printf(",\n\t\t");
				} else {
					if (i < eir_len - 1)
						printf(", ");
				}
			}
			printf("\n");

			count++;
		}
		break;
	}

	goto next_packet;

close_input:
	close(fd);
}

static void command_extract_ad(const char *input)
{
	struct btsnoop_pkt pkt;
	unsigned char buf[2048];
	ssize_t len;
	uint32_t type, toread, flags;
	uint16_t opcode;
	int fd, count = 0;

	fd = open_btsnoop(input, &type);
	if (fd < 0)
		return;

	if (type != 2001) {
		fprintf(stderr, "unsupported link data type %u\n", type);
		close(fd);
		return;
	}

next_packet:
	len = read(fd, &pkt, BTSNOOP_PKT_SIZE);
	if (len < 0 || len != BTSNOOP_PKT_SIZE)
		goto close_input;

	toread = ntohl(pkt.size);
	flags = ntohl(pkt.flags);

	opcode = flags & 0x00ff;

	len = read(fd, buf, toread);
	if (len < 0 || len != (ssize_t) toread) {
		fprintf(stderr, "failed to read packet data\n");
		goto close_input;
	}

	switch (opcode) {
	case BTSNOOP_OPCODE_EVENT_PKT:
		/* advertising report */
		if (buf[0] == 0x3e && buf[2] == 0x02) {
			uint8_t *ad_ptr, ad_len, i;

			ad_len = buf[12];
			ad_ptr = buf + 13;

			if (ad_len < 1 || ad_len > 40)
				break;

			printf("\t[Advertising Data with %u bytes]\n", ad_len);
			printf("\t\t");
			for (i = 0; i < ad_len; i++) {
				printf("0x%02x", ad_ptr[i]);
				if (((i + 1) % 8) == 0) {
					if (i < ad_len - 1)
						printf(",\n\t\t");
				} else {
					if (i < ad_len - 1)
						printf(", ");
				}
			}
			printf("\n");

			count++;
		}
		break;
	}

	goto next_packet;

close_input:
	close(fd);
}
static const uint8_t conn_complete[] = { 0x04, 0x03, 0x0B, 0x00 };
static const uint8_t disc_complete[] = { 0x04, 0x05, 0x04, 0x00 };

static void command_extract_sdp(const char *input)
{
	struct btsnoop_pkt pkt;
	unsigned char buf[2048];
	ssize_t len;
	uint32_t type, toread;
	uint16_t current_cid = 0x0000;
	uint8_t pdu_buf[512];
	uint16_t pdu_len = 0;
	bool pdu_first = false;
	int fd, count = 0;

	fd = open_btsnoop(input, &type);
	if (fd < 0)
		return;

	if (type != 1002) {
		fprintf(stderr, "unsupported link data type %u\n", type);
		close(fd);
		return;
	}

next_packet:
	len = read(fd, &pkt, BTSNOOP_PKT_SIZE);
	if (len < 0 || len != BTSNOOP_PKT_SIZE)
		goto close_input;

	toread = ntohl(pkt.size);

	len = read(fd, buf, toread);
	if (len < 0 || len != (ssize_t) toread) {
		fprintf(stderr, "failed to read packet data\n");
		goto close_input;
	}

	if (buf[0] == 0x02) {
		uint8_t acl_flags;

		/* first 4 bytes are handle and data len */
		acl_flags = buf[2] >> 4;

		/* use only packet with ACL start flag */
		if (acl_flags & 0x02) {
			if (current_cid == 0x0040 && pdu_len > 0) {
				int i;
				if (!pdu_first)
					printf(",\n");
				printf("\t\traw_pdu(");
				for (i = 0; i < pdu_len; i++) {
					printf("0x%02x", pdu_buf[i]);
					if (((i + 1) % 8) == 0) {
						if (i < pdu_len - 1)
							printf(",\n\t\t\t");
					} else {
						if (i < pdu_len - 1)
							printf(", ");
					}
				}
				printf(")");
				pdu_first = false;
			}

			/* next 4 bytes are data len and cid */
			current_cid = buf[8] << 8 | buf[7];
			memcpy(pdu_buf, buf + 9, len - 9);
			pdu_len = len - 9;
		} else if (acl_flags & 0x01) {
			memcpy(pdu_buf + pdu_len, buf + 5, len - 5);
			pdu_len += len - 5;
		}
	}

	if ((size_t) len > sizeof(conn_complete)) {
		if (memcmp(buf, conn_complete, sizeof(conn_complete)) == 0) {
			printf("\tdefine_test(\"/test/%u\",\n", ++count);
			pdu_first = true;
		}
	}

	if ((size_t) len > sizeof(disc_complete)) {
		if (memcmp(buf, disc_complete, sizeof(disc_complete)) == 0) {
			printf(");\n");
		}
	}

	goto next_packet;

close_input:
	close(fd);
}

static void usage(void)
{
	printf("btsnoop trace file handling tool\n"
		"Usage:\n");
	printf("\tbtsnoop <command> [files]\n");
	printf("commands:\n"
		"\t-m, --merge <output>   Merge multiple btsnoop files\n"
		"\t-e, --extract <input>  Extract data from btsnoop file\n"
		"\t-h, --help             Show help options\n");
}

static const struct option main_options[] = {
	{ "merge",   required_argument, NULL, 'm' },
	{ "extract", required_argument, NULL, 'e' },
	{ "type",    required_argument, NULL, 't' },
	{ "version", no_argument,       NULL, 'v' },
	{ "help",    no_argument,       NULL, 'h' },
	{ }
};

enum { INVALID, MERGE, EXTRACT };

int main(int argc, char *argv[])
{
	const char *output_path = NULL;
	const char *input_path = NULL;
	const char *type = NULL;
	unsigned short command = INVALID;

	for (;;) {
		int opt;

		opt = getopt_long(argc, argv, "m:e:t:vh", main_options, NULL);
		if (opt < 0)
			break;

		switch (opt) {
		case 'm':
			command = MERGE;
			output_path = optarg;
			break;
		case 'e':
			command = EXTRACT;
			input_path = optarg;
			break;
		case 't':
			type = optarg;
			break;
		case 'v':
			printf("%s\n", VERSION);
			return EXIT_SUCCESS;
		case 'h':
			usage();
			return EXIT_SUCCESS;
		default:
			return EXIT_FAILURE;
		}
	}

	switch (command) {
	case MERGE:
		if (argc - optind < 1) {
			fprintf(stderr, "input files required\n");
			return EXIT_FAILURE;
		}

		command_merge(output_path, argc - optind, argv + optind);
		break;

	case EXTRACT:
		if (argc - optind > 0) {
			fprintf(stderr, "extra arguments not allowed\n");
			return EXIT_FAILURE;
		}

		if (!type) {
			fprintf(stderr, "no extract type specified\n");
			return EXIT_FAILURE;
		}

		if (!strcasecmp(type, "eir"))
			command_extract_eir(input_path);
		else if (!strcasecmp(type, "ad"))
			command_extract_ad(input_path);
		else if (!strcasecmp(type, "sdp"))
			command_extract_sdp(input_path);
		else
			fprintf(stderr, "extract type not supported\n");
		break;

	default:
		usage();
		return EXIT_FAILURE;
	}

	return EXIT_SUCCESS;
}