Diff between 491aae5540b41dbdfe67617a911b9e750a60ab13 and 8da7fa1c2c759526e9439d8152b613fff84caa96

Changed Files

File Additions Deletions Status
monitor/btsnoop.c +79 -0 modified
monitor/btsnoop.h +5 -0 modified

Full Patch

diff --git a/monitor/btsnoop.c b/monitor/btsnoop.c
index e11a16c..b92fb74 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 1e5e420..cdefb8c 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);