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>

diff --git a/monitor/display.h b/monitor/display.h
index 1f54c6c..0bb4679 100755 (executable)
--- 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 eeed1f2..fbb47a3 100755 (executable)
--- 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 },
        { }
 };