summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--Makefile.tools1
-rw-r--r--monitor/btsnoop.c145
-rw-r--r--monitor/btsnoop.h30
-rw-r--r--monitor/main.c24
-rw-r--r--monitor/packet.c7
5 files changed, 207 insertions, 0 deletions
diff --git a/Makefile.tools b/Makefile.tools
index 36e6aaff..f997a3f4 100644
--- a/Makefile.tools
+++ b/Makefile.tools
@@ -57,6 +57,7 @@ mgmt_btmgmt_LDADD = lib/libbluetooth-private.la @GLIB_LIBS@
monitor_btmon_SOURCES = monitor/main.c monitor/bt.h \
monitor/mainloop.h monitor/mainloop.c \
monitor/hcidump.h monitor/hcidump.c \
+ monitor/btsnoop.h monitor/btsnoop.c \
monitor/control.h monitor/control.c \
monitor/packet.h monitor/packet.c
monitor_btmon_LDADD = lib/libbluetooth-private.la
diff --git a/monitor/btsnoop.c b/monitor/btsnoop.c
new file mode 100644
index 00000000..09c5e257
--- /dev/null
+++ b/monitor/btsnoop.c
@@ -0,0 +1,145 @@
+/*
+ *
+ * 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 <stdint.h>
+#include <string.h>
+#include <sys/stat.h>
+#include <arpa/inet.h>
+
+#include "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 const uint32_t btsnoop_type = 1001;
+
+static int btsnoop_fd = -1;
+static uint16_t btsnoop_index = 0xffff;
+
+void btsnoop_open(const char *path)
+{
+ if (btsnoop_fd >= 0)
+ return;
+
+ btsnoop_fd = open(path, O_WRONLY | O_CREAT | O_TRUNC,
+ S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
+}
+
+void btsnoop_write(struct timeval *tv, uint16_t index, uint32_t flags,
+ const void *data, uint16_t size)
+{
+ struct btsnoop_hdr hdr;
+ struct btsnoop_pkt pkt;
+ uint64_t ts;
+ ssize_t written;
+
+ if (!tv)
+ return;
+
+ if (btsnoop_fd < 0)
+ return;
+
+ if (btsnoop_index == 0xffff) {
+ memcpy(hdr.id, btsnoop_id, sizeof(btsnoop_id));
+ hdr.version = htonl(btsnoop_version);
+ hdr.type = htonl(btsnoop_type);
+
+ written = write(btsnoop_fd, &hdr, BTSNOOP_HDR_SIZE);
+ if (written < 0)
+ return;
+
+ btsnoop_index = index;
+ }
+
+ if (index != btsnoop_index)
+ return;
+
+ ts = (tv->tv_sec - 946684800ll) * 1000000ll + tv->tv_usec;
+
+ pkt.size = htonl(size);
+ pkt.len = htonl(size);
+ pkt.flags = htonl(flags);
+ pkt.drops = htonl(0);
+ pkt.ts = hton64(ts + 0x00E03AB44A676000ll);
+
+ written = write(btsnoop_fd, &pkt, BTSNOOP_PKT_SIZE);
+ if (written < 0)
+ return;
+
+ if (data && size > 0) {
+ written = write(btsnoop_fd, data, size);
+ if (written < 0)
+ return;
+ }
+}
+
+void btsnoop_close(void)
+{
+ if (btsnoop_fd < 0)
+ return;
+
+ close(btsnoop_fd);
+ btsnoop_fd = -1;
+
+ btsnoop_index = 0xffff;
+}
diff --git a/monitor/btsnoop.h b/monitor/btsnoop.h
new file mode 100644
index 00000000..9472d1a2
--- /dev/null
+++ b/monitor/btsnoop.h
@@ -0,0 +1,30 @@
+/*
+ *
+ * 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
+ *
+ */
+
+#include <sys/time.h>
+
+void btsnoop_open(const char *path);
+void btsnoop_write(struct timeval *tv, uint16_t index, uint32_t flags,
+ const void *data, uint16_t size);
+void btsnoop_close(void);
diff --git a/monitor/main.c b/monitor/main.c
index b4781e8b..9021133c 100644
--- a/monitor/main.c
+++ b/monitor/main.c
@@ -28,11 +28,13 @@
#include <stdio.h>
#include <stdlib.h>
+#include <getopt.h>
#include "mainloop.h"
#include "packet.h"
#include "control.h"
#include "hcidump.h"
+#include "btsnoop.h"
static void signal_callback(int signum, void *user_data)
{
@@ -44,6 +46,11 @@ static void signal_callback(int signum, void *user_data)
}
}
+static const struct option main_options[] = {
+ { "btsnoop", required_argument, NULL, 'b' },
+ { }
+};
+
int main(int argc, char *argv[])
{
unsigned long filter_mask = 0;
@@ -51,6 +58,23 @@ int main(int argc, char *argv[])
mainloop_init();
+ for (;;) {
+ int opt;
+
+ opt = getopt_long(argc, argv, "b", main_options, NULL);
+ if (opt < 0)
+ break;
+
+ switch (opt) {
+ case 'b':
+ btsnoop_open(optarg);
+ break;
+ default:
+ return EXIT_FAILURE;
+ }
+ }
+
+
sigemptyset(&mask);
sigaddset(&mask, SIGINT);
sigaddset(&mask, SIGTERM);
diff --git a/monitor/packet.c b/monitor/packet.c
index a445bd70..356e0a30 100644
--- a/monitor/packet.c
+++ b/monitor/packet.c
@@ -41,6 +41,7 @@
#include <bluetooth/hci_lib.h>
#include "control.h"
+#include "btsnoop.h"
#include "packet.h"
static unsigned long filter_mask = 0;
@@ -570,6 +571,8 @@ void packet_hci_command(struct timeval *tv, uint16_t index,
uint16_t ogf = cmd_opcode_ogf(opcode);
uint16_t ocf = cmd_opcode_ocf(opcode);
+ btsnoop_write(tv, index, 0x02, data, size);
+
print_header(tv, index);
if (size < HCI_COMMAND_HDR_SIZE) {
@@ -591,6 +594,8 @@ void packet_hci_event(struct timeval *tv, uint16_t index,
{
const hci_event_hdr *hdr = data;
+ btsnoop_write(tv, index, 0x03, data, size);
+
print_header(tv, index);
if (size < HCI_EVENT_HDR_SIZE) {
@@ -615,6 +620,8 @@ void packet_hci_acldata(struct timeval *tv, uint16_t index, bool in,
uint16_t dlen = btohs(hdr->dlen);
uint8_t flags = acl_flags(handle);
+ btsnoop_write(tv, index, in ? 0x01 : 0x00, data, size);
+
print_header(tv, index);
if (size < HCI_ACL_HDR_SIZE) {
generated by cgit v1.2.3 (git 2.39.1) at 2024-11-29 23:02:26 +0000