monitor: Add support for decoding bnep General pkt

      BNEP: General Ethernet (0x00|0)
         dst 00:00:00:00:00:00 src 00:00:00:00:00:00 [proto 0x0300]
        3c 3b 3a 39 38 37 36 35 34 33 32 31 30 2f 2e 2d
        2c 2b 2a 29 28 27 26 25 24 23 22 21 20 1f 1e 1d

1 files changed, 66 insertions, 10 deletions

diff --git a/monitor/bnep.c b/monitor/bnep.c
index 1450f8581..61b5d7123 100644
--- a/monitor/bnep.c
+++ b/monitor/bnep.c
@@ -47,29 +47,72 @@
 #define GET_PKT_TYPE(type) (type & 0x7f)
 #define GET_EXTENSION(type) (type & 0x80)
 
+uint16_t proto = 0x0000;
+
 struct bnep_frame {
 	uint8_t type;
 	int extension;
 	struct l2cap_frame l2cap_frame;
 };
 
+static bool get_macaddr(struct bnep_frame *bnep_frame, char *str)
+{
+	uint8_t addr[6];
+	struct l2cap_frame *frame = &bnep_frame->l2cap_frame;
+	int i;
+
+	for (i = 0; i < 6; i++)
+		if (!l2cap_frame_get_u8(frame, &addr[i]))
+			return false;
+
+	sprintf(str, "%02x:%02x:%02x:%02x:%02x:%02x",
+		addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]);
+
+	return true;
+}
+
+static bool bnep_general(struct bnep_frame *bnep_frame,
+					uint8_t indent,	int hdr_len)
+{
+	struct l2cap_frame *frame;
+	char src_addr[20], dest_addr[20];
+
+	if (!get_macaddr(bnep_frame, dest_addr))
+		return false;
+
+	if (!get_macaddr(bnep_frame, src_addr))
+		return false;
+
+	frame = &bnep_frame->l2cap_frame;
+
+	if (!l2cap_frame_get_be16(frame, &proto))
+		return false;
+
+	print_field("%*cdst %s src %s [proto 0x%04x] ", indent,
+					' ', dest_addr, src_addr, proto);
+
+	return true;
+
+}
+
 struct bnep_data {
 	uint8_t type;
 	const char *str;
+	bool (*func) (struct bnep_frame *frame, uint8_t indent, int hdr_len);
 };
 
 static const struct bnep_data bnep_table[] = {
-	{ 0x00, "General Ethernet"},
-	{ 0x01, "Control"},
-	{ 0x02, "Compressed Ethernet"},
-	{ 0x03, "Compressed Ethernet Src Only"},
-	{ 0x04, "Compressed Ethernet Dest Only"},
+	{ 0x00, "General Ethernet",		bnep_general	},
+	{ 0x01, "Control",					},
+	{ 0x02, "Compressed Ethernet",				},
+	{ 0x03, "Compressed Ethernet SrcOnly",			},
+	{ 0x04, "Compressed Ethernet DestOnly",			},
 	{ }
 };
 
 void bnep_packet(const struct l2cap_frame *frame)
 {
-	uint8_t type;
+	uint8_t type, indent = 1;
 	struct bnep_frame bnep_frame;
 	struct l2cap_frame *l2cap_frame;
 	const struct bnep_data *bnep_data = NULL;
@@ -93,10 +136,13 @@ void bnep_packet(const struct l2cap_frame *frame)
 	}
 
 	if (bnep_data) {
-		if (frame->in)
-			pdu_color = COLOR_MAGENTA;
-		else
-			pdu_color = COLOR_BLUE;
+		if (bnep_data->func) {
+			if (frame->in)
+				pdu_color = COLOR_MAGENTA;
+			else
+				pdu_color = COLOR_BLUE;
+		} else
+			pdu_color = COLOR_WHITE_BG;
 		pdu_str = bnep_data->str;
 	} else {
 		pdu_color = COLOR_WHITE_BG;
@@ -107,7 +153,17 @@ void bnep_packet(const struct l2cap_frame *frame)
 				" (0x%02x|%s)", bnep_frame.type,
 				bnep_frame.extension ? "1" : "0");
 
+	if (!bnep_data || !bnep_data->func) {
+		packet_hexdump(l2cap_frame->data, l2cap_frame->size);
+		return;
+	}
+
+	if (!bnep_data->func(&bnep_frame, indent, -1))
+		goto fail;
+
+	/* TODO: Handle BNEP packet with Extension Header */
 	packet_hexdump(l2cap_frame->data, l2cap_frame->size);
+
 	return;
 
 fail: