monitor: Add support for decoding bnep Control pkt

      BNEP: Control (0x01|0)
         Filter MultAddr Set (0x05)
        00 30 ff ff ff ff ff ff ff ff ff ff ff ff 33 33
        00 00 00 01 33 33 00 00 00 01 01 00 5e 00 00 01

1 files changed, 48 insertions, 1 deletions

diff --git a/monitor/bnep.c b/monitor/bnep.c
index 61b5d7123..d417e2fa4 100644
--- a/monitor/bnep.c
+++ b/monitor/bnep.c
@@ -95,6 +95,53 @@ static bool bnep_general(struct bnep_frame *bnep_frame,
 
 }
 
+struct bnep_control_data {
+	uint8_t type;
+	const char *str;
+};
+
+static const struct bnep_control_data bnep_control_table[] = {
+	{ 0x00, "Command Not Understood",	},
+	{ 0x01, "Setup Conn Req",		},
+	{ 0x02, "Setup Conn Rsp",		},
+	{ 0x03, "Filter NetType Set",		},
+	{ 0x04, "Filter NetType Rsp",		},
+	{ 0x05, "Filter MultAddr Set",		},
+	{ 0x06, "Filter MultAddr Rsp",		},
+	{ }
+};
+
+static bool bnep_control(struct bnep_frame *bnep_frame,
+					uint8_t indent,	int hdr_len)
+{
+	uint8_t ctype;
+	struct l2cap_frame *frame = &bnep_frame->l2cap_frame;
+	const struct bnep_control_data *bnep_control_data = NULL;
+	const char *type_str;
+	int i;
+
+	if (!l2cap_frame_get_u8(frame, &ctype))
+		return false;
+
+	for (i = 0; bnep_control_table[i].str; i++) {
+		if (bnep_control_table[i].type == ctype) {
+			bnep_control_data = &bnep_control_table[i];
+			break;
+		}
+	}
+
+	if (bnep_control_data)
+		type_str = bnep_control_data->str;
+	else
+		type_str = "Unknown control type";
+
+	print_field("%*c%s (0x%02x) ", indent, ' ', type_str, ctype);
+
+	/* TODO: Handle BNEP control types */
+
+	return true;
+}
+
 struct bnep_data {
 	uint8_t type;
 	const char *str;
@@ -103,7 +150,7 @@ struct bnep_data {
 
 static const struct bnep_data bnep_table[] = {
 	{ 0x00, "General Ethernet",		bnep_general	},
-	{ 0x01, "Control",					},
+	{ 0x01, "Control",			bnep_control	},
 	{ 0x02, "Compressed Ethernet",				},
 	{ 0x03, "Compressed Ethernet SrcOnly",			},
 	{ 0x04, "Compressed Ethernet DestOnly",			},