From f2c5b6e34ce4617924877a6ebc31f7b4341ac5b9 Mon Sep 17 00:00:00 2001
From: Marcel Holtmann <marcel@holtmann.org>
Date: Wed, 8 Jan 2014 01:25:37 -0800
Subject: [PATCH] monitor: Decode LMP AFH and packet type table operations

---
 monitor/bt.h     | 14 +++++++++++++
 monitor/lmp.c    | 53 +++++++++++++++++++++++++++++++++++++++++++++---
 monitor/packet.c |  5 +++++
 monitor/packet.h |  1 +
 4 files changed, 70 insertions(+), 3 deletions(-)

diff --git a/monitor/bt.h b/monitor/bt.h
index 5bf37bbeb..8853fc7e8 100644
--- a/monitor/bt.h
+++ b/monitor/bt.h
@@ -143,6 +143,15 @@ struct bt_lmp_features_res {
 
 #define BT_LMP_SETUP_COMPLETE		49
 
+#define BT_LMP_HOST_CONNECTION_REQ	51
+
+#define BT_LMP_SET_AFH			60
+struct bt_lmp_set_afh {
+	uint32_t instant;
+	uint8_t  mode;
+	uint8_t  map[10];
+} __attribute__ ((packed));
+
 #define BT_LMP_ACCEPTED_EXT		LMP_ESC4(1)
 struct bt_lmp_accepted_ext {
 	uint8_t  escape;
@@ -170,6 +179,11 @@ struct bt_lmp_features_res_ext {
 	uint8_t  features[8];
 } __attribute__ ((packed));
 
+#define BT_LMP_PACKET_TYPE_TABLE_REQ	LMP_ESC4(11)
+struct bt_lmp_packet_type_table_req {
+	uint8_t  table;
+} __attribute__ ((packed));
+
 #define BT_H4_CMD_PKT	0x01
 #define BT_H4_ACL_PKT	0x02
 #define BT_H4_SCO_PKT	0x03
diff --git a/monitor/lmp.c b/monitor/lmp.c
index 325b712ef..e79730c2b 100644
--- a/monitor/lmp.c
+++ b/monitor/lmp.c
@@ -113,6 +113,33 @@ static void setup_complete(const void *data, uint8_t size)
 {
 }
 
+static void host_connection_req(const void *data, uint8_t size)
+{
+}
+
+static void set_afh(const void *data, uint8_t size)
+{
+	const struct bt_lmp_set_afh *pdu = data;
+	const char *str;
+
+	print_field("Instant: %u", htobl(pdu->instant));
+
+	switch (pdu->mode) {
+	case 0x00:
+		str = "Disabled";
+		break;
+	case 0x01:
+		str = "Enabled";
+		break;
+	default:
+		str = "Reserved";
+		break;
+	}
+
+	print_field("Mode: %s (0x%2.2x)", str, pdu->mode);
+	packet_print_channel_map_lmp(pdu->map);
+}
+
 static void accepted_ext(const void *data, uint8_t size)
 {
 	const struct bt_lmp_accepted_ext *pdu = data;
@@ -164,6 +191,26 @@ static void features_res_ext(const void *data, uint8_t size)
 	packet_print_features_lmp(pdu->features, pdu->page);
 }
 
+static void packet_type_table_req(const void *data, uint8_t size)
+{
+	const struct bt_lmp_packet_type_table_req *pdu = data;
+	const char *str;
+
+	switch (pdu->table) {
+	case 0x00:
+		str = "1 Mbps only";
+		break;
+	case 0x01:
+		str = "2/3 Mbps";
+		break;
+	default:
+		str = "Reserved";
+		break;
+	}
+
+	print_field("Table: %s (0x%2.2x)", str, pdu->table);
+}
+
 struct lmp_data {
 	uint16_t opcode;
 	const char *str;
@@ -223,7 +270,7 @@ static const struct lmp_data lmp_table[] = {
 	{ 48, "LMP_timing_accuracy_res" },
 	{ 49, "LMP_setup_complete", setup_complete, 0, true },
 	{ 50, "LMP_use_semi_permanent_key" },
-	{ 51, "LMP_host_connection_req" },
+	{ 51, "LMP_host_connection_req", host_connection_req, 0, true },
 	{ 52, "LMP_slot_offset" },
 	{ 53, "LMP_page_mode_req" },
 	{ 54, "LMP_Page_scan_mode_req" },
@@ -232,7 +279,7 @@ static const struct lmp_data lmp_table[] = {
 	{ 57, "LMP_test_control" },
 	{ 58, "LMP_encryption_key_size_mask_req" },
 	{ 59, "LMP_encryption_key_size_mask_res" },
-	{ 60, "LMP_set_AFH" },
+	{ 60, "LMP_set_AFH", set_afh, 15, true },
 	{ 61, "LMP_encapsulated_header" },
 	{ 62, "LMP_encapsulated_payload" },
 	{ 63, "LMP_simple_pairing_confirm" },
@@ -246,7 +293,7 @@ static const struct lmp_data lmp_table[] = {
 	{ LMP_ESC4(5),  "LMP_clk_adj" },
 	{ LMP_ESC4(6),  "LMP_clk_adj_ack" },
 	{ LMP_ESC4(7),  "LMP_clk_adj_req" },
-	{ LMP_ESC4(11), "LMP_packet_type_table" },
+	{ LMP_ESC4(11), "LMP_packet_type_table_req", packet_type_table_req, 1, true },
 	{ LMP_ESC4(12), "LMP_eSCO_link_req" },
 	{ LMP_ESC4(13), "LMP_remove_eSCO_link_req" },
 	{ LMP_ESC4(16), "LMP_channel_classification_req" },
diff --git a/monitor/packet.c b/monitor/packet.c
index b8a98012c..761536bdb 100644
--- a/monitor/packet.c
+++ b/monitor/packet.c
@@ -2005,6 +2005,11 @@ static void print_channel_map(const uint8_t *map)
 	}
 }
 
+void packet_print_channel_map_lmp(const uint8_t *map)
+{
+	print_channel_map(map);
+}
+
 static void print_flush_timeout(uint16_t timeout)
 {
 	if (timeout)
diff --git a/monitor/packet.h b/monitor/packet.h
index eed417f62..10ba240bc 100644
--- a/monitor/packet.h
+++ b/monitor/packet.h
@@ -48,6 +48,7 @@ void packet_print_addr(const char *label, const void *data, bool random);
 void packet_print_ad(const void *data, uint8_t size);
 void packet_print_features_lmp(const uint8_t *features, uint8_t page);
 void packet_print_features_ll(const uint8_t *features);
+void packet_print_channel_map_lmp(const uint8_t *map);
 void packet_print_channel_map_ll(const uint8_t *map);
 
 void packet_control(struct timeval *tv, uint16_t index, uint16_t opcode,
-- 
2.47.3