monitor: Add PHY management support

This patch adds support of PHY management
and consolidation of code printing bitfield

Taken from following upstream commits-
 commit dad3ca2f417d9330f6f3bc33cfee5d0b24985a5d
 commit 426693e8457326ee686e46a2c9f535b9367a55d7
 commit e566be15ce03283978c35b2ab8462504da3fee63
 commit e862f704eeff14818fcc00a57271bd6907285ad7

Change-Id: I3582308f50423b916cec7d900fcf41dc9b4c4dc6
Signed-off-by: Anupam Roy <anupam.r@samsung.com>

2 files changed, 89 insertions, 1 deletions

diff --git a/monitor/display.h b/monitor/display.h
index 1f54c6c1..0bb4679d 100755
--- a/monitor/display.h
+++ b/monitor/display.h
@@ -23,6 +23,7 @@
  */
 
 #include <stdbool.h>
+#include <inttypes.h>
 
 bool use_color(void);
 
@@ -68,6 +69,27 @@ do { \
 #define print_field(fmt, args...) \
 		print_indent(8, COLOR_OFF, "", "", COLOR_OFF, fmt, ## args)
 
+struct bitfield_data {
+	uint64_t bit;
+	const char *str;
+};
+
+inline uint64_t print_bitfield(int indent, uint64_t val,
+				const struct bitfield_data *table)
+{
+	uint64_t mask = val;
+	int i;
+
+	for (i = 0; table[i].str; i++) {
+		if (val & (((uint64_t) 1) << table[i].bit)) {
+			print_field("%*c%s", indent, ' ', table[i].str);
+			mask &= ~(((uint64_t) 1) << table[i].bit);
+		}
+	}
+
+	return mask;
+}
+
 int num_columns(void);
 
 void open_pager(void);
diff --git a/monitor/packet.c b/monitor/packet.c
index eeed1f23..fbb47a3a 100755
--- a/monitor/packet.c
+++ b/monitor/packet.c
@@ -97,6 +97,7 @@
 #define COLOR_UNKNOWN_ADDRESS_TYPE	COLOR_WHITE_BG
 #define COLOR_UNKNOWN_DEVICE_FLAG	COLOR_WHITE_BG
 #define COLOR_UNKNOWN_ADV_FLAG		COLOR_WHITE_BG
+#define COLOR_UNKNOWN_PHY		COLOR_WHITE_BG
 
 #define COLOR_PHY_PACKET		COLOR_BLUE
 
@@ -745,7 +746,7 @@ static const struct {
 	{  9, "3-DH3 may not be used"	},
 	{ 10, "DM3 may be used"		},
 	{ 11, "DH3 may be used"		},
-	{ 12, "3-DH5 may not be used"	},
+	{ 12, "2-DH5 may not be used"	},
 	{ 13, "3-DH5 may not be used"	},
 	{ 14, "DM5 may be used"		},
 	{ 15, "DH5 may be used"		},
@@ -10623,6 +10624,7 @@ static const struct {
 	{ 13, "Privacy"			},
 	{ 14, "Controller Configuration"},
 	{ 15, "Static Address"		},
+	{ 16, "PHY Configuration"	},
 	{ }
 };
 
@@ -11917,6 +11919,55 @@ static void mgmt_set_apperance_cmd(const void *data, uint16_t size)
 	print_appearance(appearance);
 }
 
+static const struct bitfield_data mgmt_phy_table[] = {
+	{  0, "BR 1M 1SLOT"	},
+	{  1, "BR 1M 3SLOT"	},
+	{  2, "BR 1M 5SLOT"	},
+	{  3, "EDR 2M 1SLOT"	},
+	{  4, "EDR 2M 3SLOT"	},
+	{  5, "EDR 2M 5SLOT"	},
+	{  6, "EDR 3M 1SLOT"	},
+	{  7, "EDR 3M 3SLOT"	},
+	{  8, "EDR 3M 5SLOT"	},
+	{  9, "LE 1M TX"	},
+	{  10, "LE 1M RX"	},
+	{  11, "LE 2M TX"	},
+	{  12, "LE 2M RX"	},
+	{  13, "LE CODED TX"	},
+	{  14, "LE CODED RX"	},
+	{ }
+};
+
+static void mgmt_print_phys(const char *label, uint16_t phys)
+{
+	uint16_t mask;
+
+	print_field("%s: 0x%4.4x", label, phys);
+
+	mask = print_bitfield(2, phys, mgmt_phy_table);
+	if (mask)
+		print_text(COLOR_UNKNOWN_PHY, "  Unknown PHYs"
+							" (0x%8.8x)", mask);
+}
+
+static void mgmt_get_phy_rsp(const void *data, uint16_t size)
+{
+	uint32_t supported_phys = get_le32(data);
+	uint32_t configurable_phys = get_le32(data + 4);
+	uint32_t selected_phys = get_le32(data + 8);
+
+	mgmt_print_phys("Supported PHYs", supported_phys);
+	mgmt_print_phys("Configurable PHYs", configurable_phys);
+	mgmt_print_phys("Selected PHYs", selected_phys);
+}
+
+static void mgmt_set_phy_cmd(const void *data, uint16_t size)
+{
+	uint32_t selected_phys = get_le32(data);
+
+	mgmt_print_phys("Selected PHYs", selected_phys);
+}
+
 struct mgmt_data {
 	uint16_t opcode;
 	const char *str;
@@ -12130,6 +12181,12 @@ static const struct mgmt_data mgmt_command_table[] = {
 	{ 0x0043, "Set Appearance",
 				mgmt_set_apperance_cmd, 2, true,
 				mgmt_null_rsp, 0, true },
+	{ 0x0044, "Get PHY Configuration",
+				mgmt_null_cmd, 0, true,
+				mgmt_get_phy_rsp, 12, true },
+	{ 0x0045, "Set PHY Configuration",
+				mgmt_set_phy_cmd, 4, true,
+				mgmt_null_rsp, 0, true },
 	{ }
 };
 
@@ -12508,6 +12565,13 @@ static void mgmt_ext_controller_info_changed_evt(const void *data, uint16_t size
 	print_eir(data + 2, size - 2, false);
 }
 
+static void mgmt_phy_changed_evt(const void *data, uint16_t size)
+{
+	uint32_t selected_phys = get_le32(data);
+
+	mgmt_print_phys("Selected PHYs", selected_phys);
+}
+
 static const struct mgmt_data mgmt_event_table[] = {
 	{ 0x0001, "Command Complete",
 			mgmt_command_complete_evt, 3, false },
@@ -12583,6 +12647,8 @@ static const struct mgmt_data mgmt_event_table[] = {
 			mgmt_advertising_removed_evt, 1, true },
 	{ 0x0025, "Extended Controller Information Changed",
 			mgmt_ext_controller_info_changed_evt, 2, false },
+	{ 0x0026, "PHY Configuration Changed",
+			mgmt_phy_changed_evt, 4, true },
 	{ }
 };