From 8da7fa1c2c759526e9439d8152b613fff84caa96 Mon Sep 17 00:00:00 2001
From: Marcel Holtmann <marcel@holtmann.org>
Date: Tue, 20 Aug 2013 19:43:25 -0700
Subject: [PATCH] monitor: Add btsnoop functions physical datalink type

---
 monitor/btsnoop.c | 79 +++++++++++++++++++++++++++++++++++++++++++++++
 monitor/btsnoop.h |  5 +++
 2 files changed, 84 insertions(+)

diff --git a/monitor/btsnoop.c b/monitor/btsnoop.c
index e11a16c93..b92fb747f 100644
--- a/monitor/btsnoop.c
+++ b/monitor/btsnoop.c
@@ -176,6 +176,29 @@ void btsnoop_write_hci(struct timeval *tv, uint16_t index, uint16_t opcode,
 	btsnoop_write(tv, flags, data, size);
 }
 
+void btsnoop_write_phy(struct timeval *tv, uint16_t frequency,
+					const void *data, uint16_t size)
+{
+	uint32_t flags;
+
+	if (!tv)
+		return;
+
+	if (btsnoop_fd < 0)
+		return;
+
+	switch (btsnoop_type) {
+	case BTSNOOP_TYPE_EXTENDED_PHY:
+		flags = (1 << 16) | frequency;
+		break;
+
+	default:
+		return;
+	}
+
+	btsnoop_write(tv, flags, data, size);
+}
+
 int btsnoop_open(const char *path, uint32_t *type)
 {
 	struct btsnoop_hdr hdr;
@@ -332,6 +355,62 @@ int btsnoop_read_hci(struct timeval *tv, uint16_t *index, uint16_t *opcode,
 	return 0;
 }
 
+int btsnoop_read_phy(struct timeval *tv, uint16_t *frequency,
+						void *data, uint16_t *size)
+{
+	struct btsnoop_pkt pkt;
+	uint32_t toread, flags;
+	uint64_t ts;
+	ssize_t len;
+
+	if (btsnoop_fd < 0)
+		return -1;
+
+	len = read(btsnoop_fd, &pkt, BTSNOOP_PKT_SIZE);
+	if (len == 0)
+		return -1;
+
+	if (len < 0 || len != BTSNOOP_PKT_SIZE) {
+		perror("Failed to read packet");
+		close(btsnoop_fd);
+		btsnoop_fd = -1;
+		return -1;
+	}
+
+	toread = ntohl(pkt.size);
+	flags = ntohl(pkt.flags);
+
+	ts = ntoh64(pkt.ts) - 0x00E03AB44A676000ll;
+	tv->tv_sec = (ts / 1000000ll) + 946684800ll;
+	tv->tv_usec = ts % 1000000ll;
+
+	switch (btsnoop_type) {
+	case BTSNOOP_TYPE_EXTENDED_PHY:
+		if ((flags >> 16) != 1)
+			break;
+		*frequency = flags & 0xffff;
+		break;
+
+	default:
+		fprintf(stderr, "Unknown packet type\n");
+		close(btsnoop_fd);
+		btsnoop_fd = -1;
+		return -1;
+	}
+
+	len = read(btsnoop_fd, data, toread);
+	if (len < 0) {
+		perror("Failed to read data");
+		close(btsnoop_fd);
+		btsnoop_fd = -1;
+		return -1;
+	}
+
+	*size = toread;
+
+	return 0;
+}
+
 void btsnoop_close(void)
 {
 	if (btsnoop_fd < 0)
diff --git a/monitor/btsnoop.h b/monitor/btsnoop.h
index 1e5e42089..cdefb8cc5 100644
--- a/monitor/btsnoop.h
+++ b/monitor/btsnoop.h
@@ -31,6 +31,7 @@
 #define BTSNOOP_TYPE_3WIRE		1004
 
 #define BTSNOOP_TYPE_EXTENDED_HCI	2001
+#define BTSNOOP_TYPE_EXTENDED_PHY	2002
 
 #define BTSNOOP_OPCODE_NEW_INDEX	0
 #define BTSNOOP_OPCODE_DEL_INDEX	1
@@ -46,7 +47,11 @@ void btsnoop_write(struct timeval *tv, uint32_t flags,
 					const void *data, uint16_t size);
 void btsnoop_write_hci(struct timeval *tv, uint16_t index, uint16_t opcode,
 					const void *data, uint16_t size);
+void btsnoop_write_phy(struct timeval *tv, uint16_t frequency,
+					const void *data, uint16_t size);
 int btsnoop_open(const char *path, uint32_t *type);
 int btsnoop_read_hci(struct timeval *tv, uint16_t *index, uint16_t *opcode,
 						void *data, uint16_t *size);
+int btsnoop_read_phy(struct timeval *tv, uint16_t *frequency,
+						void *data, uint16_t *size);
 void btsnoop_close(void);
-- 
2.47.3