[PATCH 2/4] monitor/rfcomm: Add support for printing RFCOMM hdr

[Gowtham Anandha Babu <gowtham.ab@xxxxxxxxxxx>]

Changes made to decode RFCOMM hdr and print the same.

      RFCOMM: Unnumbered Info with Header Check (UIH)(0xef)
         Address : (0x01)
           Command/Response Bit: 0
           DLCI : (0x00)
         Control : (0xef)
           Poll/Final Bit : 0
         Length : 10
         FCS : (0xaa)
        81 11 20 e0 27 00 9a 02 00 07 aa                 .. .'......             

---
 monitor/rfcomm.c | 55 ++++++++++++++++++++++++++++++++++++++++++++++++++++---
 1 file changed, 52 insertions(+), 3 deletions(-)

diff --git a/monitor/rfcomm.c b/monitor/rfcomm.c
index 155bde2..ee1eede 100644
--- a/monitor/rfcomm.c
+++ b/monitor/rfcomm.c
@@ -44,6 +44,11 @@
 #include "sdp.h"
 #include "rfcomm.h"
 
+#define GET_LEN8(length) ((length & 0xfe) >> 1)
+#define GET_LEN16(length) ((length & 0xfffe) >> 1)
+#define GET_CR(type)	((type & 0x02) >> 1)
+#define GET_PF(ctr) (((ctr) >> 4) & 0x1)
+
 struct rfcomm_lhdr {
 	uint8_t address;
 	uint8_t control;
@@ -57,6 +62,29 @@ struct rfcomm_frame {
 	struct l2cap_frame l2cap_frame;
 };
 
+static void print_rfcomm_hdr(struct rfcomm_frame *rfcomm_frame, uint8_t indent)
+{
+	struct rfcomm_lhdr hdr = rfcomm_frame->hdr;
+
+	/* Address field */
+	print_field("%*cAddress : (0x%2.2x)", indent, ' ', hdr.address);
+
+	print_field("%*cCommand/Response Bit: %d", indent+2, ' ',
+						GET_CR(hdr.address));
+
+	print_field("%*cDLCI : (0x%2.2x)", indent+2, ' ',
+					RFCOMM_GET_DLCI(hdr.address));
+
+	/* Control field */
+	print_field("%*cControl : (0x%2.2x)", indent, ' ', hdr.control);
+	print_field("%*cPoll/Final Bit : %d", indent+2, ' ',
+						GET_PF(hdr.control));
+
+	/* Length and FCS */
+	print_field("%*cLength : %d", indent, ' ', hdr.length);
+	print_field("%*cFCS : (0x%2.2x)", indent, ' ', hdr.fcs);
+}
+
 struct rfcomm_data {
 	uint8_t frame;
 	const char *str;
@@ -73,12 +101,13 @@ static const struct rfcomm_data rfcomm_table[] = {
 
 void rfcomm_packet(const struct l2cap_frame *frame)
 {
-	uint8_t ctype;
+	uint8_t ctype, length, ex_length, indent = 1;
 	const char *frame_str, *frame_color;
 	struct l2cap_frame *l2cap_frame;
 	struct rfcomm_frame rfcomm_frame;
 	struct rfcomm_lhdr hdr;
 	const struct rfcomm_data *rfcomm_data = NULL;
+	const void *ptr;
 	int i;
 
 	l2cap_frame_pull(&rfcomm_frame.l2cap_frame, frame, 0);
@@ -89,9 +118,24 @@ void rfcomm_packet(const struct l2cap_frame *frame)
 		goto fail;
 
 	if (!l2cap_frame_get_u8(l2cap_frame, &hdr.address) ||
-			!l2cap_frame_get_u8(l2cap_frame, &hdr.control))
+			!l2cap_frame_get_u8(l2cap_frame, &hdr.control) ||
+			!l2cap_frame_get_u8(l2cap_frame, &length))
 		goto fail;
 
+	/* length maybe 1 or 2 octets */
+	if (RFCOMM_TEST_EA(length))
+		hdr.length = (uint16_t) GET_LEN8(length);
+	else {
+		if (!l2cap_frame_get_u8(l2cap_frame, &ex_length))
+			goto fail;
+		hdr.length = ((uint16_t)length << 8) | ex_length;
+		hdr.length = GET_LEN16(hdr.length);
+	}
+
+	/* fetching FCS by frame offset */
+	ptr = (l2cap_frame->data)+l2cap_frame->size-1;
+	hdr.fcs = *(uint8_t *)(ptr);
+
 	/* Decoding frame type */
 	ctype = RFCOMM_GET_TYPE(hdr.control);
 
@@ -122,7 +166,12 @@ void rfcomm_packet(const struct l2cap_frame *frame)
 						"(0x%2.2x)", ctype);
 
 	rfcomm_frame.hdr = hdr;
-	packet_hexdump(l2cap_frame->data, l2cap_frame->size);
+	print_rfcomm_hdr(&rfcomm_frame, indent);
+
+	/* UIH frame */
+	if (ctype == 0xef)
+		packet_hexdump(l2cap_frame->data, l2cap_frame->size);
+
 	return;
 
 fail:
-- 
1.9.1

--
To unsubscribe from this list: send the line "unsubscribe linux-bluetooth" in
the body of a message to majordomo@xxxxxxxxxxxxxxx
More majordomo info at  http://vger.kernel.org/majordomo-info.html