[BlueZ,2/7] monitor: Make use of MSFT packet definitions

Commit Message

Luiz Augusto von Dentz Oct. 15, 2021, 5:09 a.m. UTC

From: Luiz Augusto von Dentz <[e-mail]>

This make use of the packet definitions for MSFT extension.
---
 monitor/msft.c | 144 ++++++++++++++++++++++++++++++++++++++++---------
 monitor/msft.h |  11 ++--
 2 files changed, 125 insertions(+), 30 deletions(-)

Patch

diff --git a/monitor/msft.c b/monitor/msft.c
index 46e765489..ccb232f3b 100644
--- a/monitor/msft.c
+++ b/monitor/msft.c
@@ -30,6 +30,9 @@ 
 #include <stdio.h>
 #include <inttypes.h>
 
+#include "lib/bluetooth.h"
+#include "lib/uuid.h"
+
 #include "src/shared/util.h"
 #include "display.h"
 #include "packet.h"
@@ -49,46 +52,132 @@  static void null_rsp(const void *data, uint16_t size)
 
 static void read_supported_features_rsp(const void *data, uint16_t size)
 {
-	uint8_t evt_prefix_len = get_u8(data + 8);
+	const struct msft_rsp_read_supported_features *rsp = data;
+
+	packet_print_features_msft(rsp->features);
+	print_field("Event prefix length: %u", rsp->evt_prefix_len);
+	print_field("Event prefix:");
+	packet_hexdump(rsp->evt_prefix, rsp->evt_prefix_len);
+	packet_set_msft_evt_prefix(rsp->evt_prefix, rsp->evt_prefix_len);
+}
+
+static void monitor_rssi_cmd(const void *data, uint16_t size)
+{
+	const struct msft_cmd_monitor_rssi *cmd = data;
+
+	print_field("Connection handle: 0x%04x", cmd->handle);
+	packet_print_rssi("RSSI threshold high", cmd->rssi_high);
+	packet_print_rssi("RSSI threshold low", cmd->rssi_low);
+	print_field("RSSI threshold low time interval: %u sec (0x%2.2x)",
+						cmd->rssi_low_interval,
+						cmd->rssi_low_interval);
+	print_field("RSSI sampling period: %u msec (0x%2.2x)",
+						cmd->rssi_period * 100,
+						cmd->rssi_period);
+}
 
-	packet_print_features_msft(data);
-	print_field("Event prefix length: %u", evt_prefix_len);
-	packet_hexdump(data + 9, size - 9);
+static void cancel_monitor_rssi_cmd(const void *data, uint16_t size)
+{
+	const struct msft_cmd_cancel_monitor_rssi *cmd = data;
 
-	packet_set_msft_evt_prefix(data + 9, evt_prefix_len);
+	print_field("Connection handle: 0x%04x", cmd->handle);
 }
 
 static void le_monitor_advertisement_cmd(const void *data, uint16_t size)
 {
-	int8_t threshold_high = get_s8(data);
-	int8_t threshold_low = get_s8(data + 1);
-	uint8_t threshold_low_time_interval = get_u8(data + 2);
-	uint8_t sampling_period = get_u8(data + 3);
+	const struct msft_cmd_le_monitor_adv *cmd = data;
+	const struct msft_le_monitor_adv_patterns *patterns;
+	const struct msft_le_monitor_adv_uuid *uuid;
+	const struct msft_le_monitor_adv_irk *irk;
+	const struct msft_le_monitor_adv_addr *addr;
+	const char *str;
+	char uuidstr[MAX_LEN_UUID_STR];
 
-	packet_print_rssi("RSSI threshold high", threshold_high);
-	packet_print_rssi("RSSI threshold low", threshold_low);
+	packet_print_rssi("RSSI threshold high", cmd->rssi_high);
+	packet_print_rssi("RSSI threshold low", cmd->rssi_low);
 	print_field("RSSI threshold low time interval: %u sec (0x%2.2x)",
-						threshold_low_time_interval,
-						threshold_low_time_interval);
+						cmd->rssi_low_interval,
+						cmd->rssi_low_interval);
 	print_field("RSSI sampling period: %u msec (0x%2.2x)",
-						sampling_period * 100,
-						sampling_period);
-	packet_hexdump(data + 4, size - 4);
+						cmd->rssi_period * 100,
+						cmd->rssi_period);
+
+	switch (cmd->type) {
+	case MSFT_SUBCMD_LE_MONITOR_ADV_PATTERN:
+		print_field("Type: Pattern (0x%2.2x)", cmd->type);
+		patterns = (void *)cmd->data;
+		print_field("Number of patterns: %u", patterns->num);
+		packet_hexdump((void *)patterns->data,
+			       size - (sizeof(*cmd) + sizeof(*patterns)));
+		break;
+	case MSFT_SUBCMD_LE_MONITOR_ADV_UUID:
+		print_field("Type: UUID (0x%2.2x)", cmd->type);
+		uuid = (void *)cmd->data;
+
+		switch (uuid->type) {
+		case 0x01:
+			str = bt_uuid16_to_str(uuid->value.u16);
+			print_field("UUID: %s (0x%4.4x)", str, uuid->value.u16);
+			break;
+		case 0x02:
+			str = bt_uuid32_to_str(uuid->value.u32);
+			print_field("UUID: %s (0x%8.8x)", str, uuid->value.u32);
+			break;
+		case 0x03:
+			sprintf(uuidstr, "%8.8x-%4.4x-%4.4x-%4.4x-%8.8x%4.4x",
+				get_le32(uuid->value.u128 + 12),
+				get_le16(uuid->value.u128 + 10),
+				get_le16(uuid->value.u128 + 8),
+				get_le16(uuid->value.u128 + 6),
+				get_le32(uuid->value.u128 + 2),
+				get_le16(uuid->value.u128 + 0));
+			str = bt_uuidstr_to_str(uuidstr);
+			print_field("UUID: %s (%s)", str, uuidstr);
+			break;
+		default:
+			packet_hexdump((void *)&uuid->value,
+					size - sizeof(*cmd));
+			break;
+		}
+		break;
+	case MSFT_SUBCMD_LE_MONITOR_ADV_IRK:
+		print_field("Type: IRK (0x%2.2x)", cmd->type);
+		irk = (void *)cmd->data;
+		print_field("IRK:");
+		packet_hexdump(irk->irk, size - sizeof(*cmd));
+		break;
+	case MSFT_SUBCMD_LE_MONITOR_ADV_ADDR:
+		print_field("Type: Adderss (0x%2.2x)", cmd->type);
+		addr = (void *)cmd->data;
+		packet_print_addr(NULL, addr->addr, addr->type);
+		break;
+	default:
+		print_field("Type: Unknown (0x%2.2x)", cmd->type);
+		packet_hexdump(cmd->data, size - sizeof(*cmd));
+		break;
+	}
 }
 
 static void le_monitor_advertisement_rsp(const void *data, uint16_t size)
 {
-	uint8_t handle = get_u8(data);
+	const struct msft_rsp_le_monitor_adv *rsp = data;
+
+	print_field("Monitor handle: %u", rsp->handle);
+}
+
+static void le_cancel_monitor_adv_cmd(const void *data, uint16_t size)
+{
+	const struct msft_cmd_le_cancel_monitor_adv *cmd = data;
 
-	print_field("Monitor handle: %u", handle);
+	print_field("Monitor handle: %u", cmd->handle);
 }
 
 static void set_adv_filter_enable_cmd(const void *data, uint16_t size)
 {
-	uint8_t enable = get_u8(data);
+	const struct msft_cmd_le_monitor_adv_enable *cmd = data;
 	const char *str;
 
-	switch (enable) {
+	switch (cmd->enable) {
 	case 0x00:
 		str = "Current allow list";
 		break;
@@ -100,7 +189,7 @@  static void set_adv_filter_enable_cmd(const void *data, uint16_t size)
 		break;
 	}
 
-	print_field("Enable: %s (0x%2.2x)", str, enable);
+	print_field("Enable: %s (0x%2.2x)", str, cmd->enable);
 }
 
 typedef void (*func_t) (const void *data, uint16_t size);
@@ -114,12 +203,15 @@  static const struct {
 	{ 0x00, "Read Supported Features",
 			null_cmd,
 			read_supported_features_rsp },
-	{ 0x01, "Monitor RSSI" },
-	{ 0x02, "Cancel Monitor RSSI" },
+	{ 0x01, "Monitor RSSI",
+			monitor_rssi_cmd },
+	{ 0x02, "Cancel Monitor RSSI",
+			cancel_monitor_rssi_cmd },
 	{ 0x03, "LE Monitor Advertisement",
 			le_monitor_advertisement_cmd,
 			le_monitor_advertisement_rsp },
-	{ 0x04, "LE Cancel Monitor Advertisement" },
+	{ 0x04, "LE Cancel Monitor Advertisement",
+			le_cancel_monitor_adv_cmd },
 	{ 0x05, "LE Set Advertisement Filter Enable",
 			set_adv_filter_enable_cmd,
 			null_rsp },
@@ -156,7 +248,7 @@  static void msft_cmd(const void *data, uint8_t size)
 						" (0x%2.2x)", code);
 
 	if (code_func)
-		code_func(data + 1, size - 1);
+		code_func(data, size);
 	else
 		packet_hexdump(data + 1, size - 1);
 }
@@ -193,7 +285,7 @@  static void msft_rsp(const void *data, uint8_t size)
 	packet_print_error("Status", status);
 
 	if (code_func)
-		code_func(data + 2, size - 2);
+		code_func(data, size);
 	else
 		packet_hexdump(data + 2, size - 2);
 }
diff --git a/monitor/msft.h b/monitor/msft.h
index 90a64117a..4f4bdd992 100644
--- a/monitor/msft.h
+++ b/monitor/msft.h
@@ -68,6 +68,7 @@  struct msft_rsp_cancel_monitor_rssi {
 
 #define MSFT_SUBCMD_LE_MONITOR_ADV		0x03
 
+#define MSFT_SUBCMD_LE_MONITOR_ADV_PATTERN	0x01
 struct msft_le_monitor_pattern {
 	uint8_t  len;
 	uint8_t  type;
@@ -75,12 +76,13 @@  struct msft_le_monitor_pattern {
 	uint8_t  data[];
 } __attribute__((packed));
 
-struct msft_le_monitor_adv_pattern_type {
-	uint8_t num_patterns;
+struct msft_le_monitor_adv_patterns {
+	uint8_t num;
 	struct msft_le_monitor_pattern data[];
 } __attribute__((packed));
 
-struct msft_le_monitor_adv_uuid_type {
+#define MSFT_SUBCMD_LE_MONITOR_ADV_UUID		0x02
+struct msft_le_monitor_adv_uuid {
 	uint8_t  type;
 	union {
 		uint16_t u16;
@@ -89,7 +91,8 @@  struct msft_le_monitor_adv_uuid_type {
 	} value;
 } __attribute__((packed));
 
-struct msft_le_monitor_adv_irk_type {
+#define MSFT_SUBCMD_LE_MONITOR_ADV_IRK		0x03
+struct msft_le_monitor_adv_irk {
 	uint8_t  irk[8];
 } __attribute__((packed));