[PATCH BlueZ] monitor: Add unknown options decoding for Configure Response

[Andrey Smirnov <andrew.smirnov@gmail.com>]

Unknown options respose for Configure Respose packet has a different
layout than that of unaccepted options, so it needs special code to
handle it.

Before:

> ACL Data RX: Handle 12 flags 0x02 dlen 15
      L2CAP: Configure Response (0x05) ident 2 len 7
        Source CID: 64
        Flags: 0x0000
        Result: Failure - unknown options (0x0003)
        04

After:

> ACL Data RX: Handle 12 flags 0x02 dlen 15
      L2CAP: Configure Response (0x05) ident 3 len 7
        Source CID: 65
        Flags: 0x0000
        Result: Failure - unknown options (0x0003)
        Option: Retransmission and Flow Control (0x04)
---
 monitor/l2cap.c | 56 ++++++++++++++++++++++++++++++++++++++-----------
 1 file changed, 44 insertions(+), 12 deletions(-)

diff --git a/monitor/l2cap.c b/monitor/l2cap.c
index 26719ac5e..6983f80f0 100644
--- a/monitor/l2cap.c
+++ b/monitor/l2cap.c
@@ -705,6 +705,42 @@ static struct {
         { }
 };
 
+static void lookup_option_by_type(uint8_t type, const char **str,
+				  uint8_t *expect_len)
+{
+	int i;
+
+	for (i = 0; options_table[i].str; i++) {
+		if (options_table[i].type == type) {
+			*str = options_table[i].str;
+			if (expect_len)
+				*expect_len = options_table[i].len;
+			return;
+		}
+	}
+
+	*str = "Unknown";
+	if (expect_len)
+		*expect_len = 0;
+}
+
+static void print_unknown_options(const struct l2cap_frame *frame,
+				  uint8_t offset)
+{
+	const uint8_t *data = frame->data + offset;
+	uint16_t size = frame->size - offset;
+	uint16_t consumed = 0;
+
+	while (size--) {
+		const char *str;
+		uint8_t type = data[consumed++] & 0x7f;
+
+		lookup_option_by_type(type, &str, NULL);
+
+		print_field("Option: %s (0x%2.2x)", str, type);
+	}
+}
+
 static void print_config_options(const struct l2cap_frame *frame,
 				uint8_t offset, uint16_t cid, bool response)
 {
@@ -713,24 +749,16 @@ static void print_config_options(const struct l2cap_frame *frame,
 	uint16_t consumed = 0;
 
 	while (consumed < size - 2) {
-		const char *str = "Unknown";
+		const char *str;
 		uint8_t type = data[consumed] & 0x7f;
 		uint8_t hint = data[consumed] & 0x80;
 		uint8_t len = data[consumed + 1];
-		uint8_t expect_len = 0;
-		int i;
+		uint8_t expect_len;
 
-		for (i = 0; options_table[i].str; i++) {
-			if (options_table[i].type == type) {
-				str = options_table[i].str;
-				expect_len = options_table[i].len;
-				break;
-			}
-		}
+		lookup_option_by_type(type, &str, &expect_len);
 
 		print_field("Option: %s (0x%2.2x) [%s]", str, type,
 						hint ? "hint" : "mandatory");
-
 		if (expect_len == 0) {
 			consumed += 2;
 			break;
@@ -1122,7 +1150,11 @@ static void sig_config_rsp(const struct l2cap_frame *frame)
 	print_cid("Source", pdu->scid);
 	print_config_flags(pdu->flags);
 	print_config_result(pdu->result);
-	print_config_options(frame, 6, le16_to_cpu(pdu->scid), true);
+	if (pdu->result == 0x0003)
+		print_unknown_options(frame, 6);
+	else
+		print_config_options(frame, 6, le16_to_cpu(pdu->scid), true);
+
 }
 
 static void sig_disconn_req(const struct l2cap_frame *frame)
-- 
2.21.0