Skip to contentSkip to conversation

monitor: Add proper decoding of LE flowctl mode

PATCH In review Luiz Augusto von Dentz 1 participant 2 messages Sep 13, 2019 – Sep 16, 2019 Bluetooth drivers

lore
Share & tools
lore

Luiz Augusto von Dentz

author maintainer
#0 lore raw

PATCH 1/1 monitor: Add proper decoding of LE flowctl mode

From: Luiz Augusto von Dentz <[email protected]>

This enables decoding LE flowctl mode properly and print out the SDU.

---
 monitor/l2cap.c | 65 ++++++++++++++++++++++++++++++++++++++++---------
 1 file changed, 53 insertions(+), 12 deletions(-)
monitor/l2cap.c +53 -12
index 26719ac5e..ad2499851 100644
@@ -47,6 +47,14 @@
 #include "rfcomm.h"
 #include "bnep.h"
 
+
+#define L2CAP_MODE_BASIC		0x00
+#define L2CAP_MODE_RETRANS		0x01
+#define L2CAP_MODE_FLOWCTL		0x02
+#define L2CAP_MODE_ERTM			0x03
+#define L2CAP_MODE_STREAMING		0x04
+#define L2CAP_MODE_LE_FLOWCTL		0x80
+
 /* L2CAP Control Field bit masks */
 #define L2CAP_CTRL_SAR_MASK		0xC000
 #define L2CAP_CTRL_REQSEQ_MASK		0x3F00
@@ -102,12 +110,13 @@ struct chan_data {
 	uint8_t  mode;
 	uint8_t  ext_ctrl;
 	uint8_t  seq_num;
+	uint16_t sdu;
 };
 
 static struct chan_data chan_list[MAX_CHAN];
 
-static void assign_scid(const struct l2cap_frame *frame,
-				uint16_t scid, uint16_t psm, uint8_t ctrlid)
+static void assign_scid(const struct l2cap_frame *frame, uint16_t scid,
+			uint16_t psm, uint8_t mode, uint8_t ctrlid)
 {
 	int i, n = -1;
 	uint8_t seq_num = 1;
@@ -154,7 +163,7 @@ static void assign_scid(const struct l2cap_frame *frame,
 
 	chan_list[n].psm = psm;
 	chan_list[n].ctrlid = ctrlid;
-	chan_list[n].mode = 0;
+	chan_list[n].mode = mode;
 
 	chan_list[n].seq_num = seq_num;
 }
@@ -257,6 +266,9 @@ static int get_chan_data_index(const struct l2cap_frame *frame)
 {
 	int i;
 
+	if (frame->chan)
+		return frame->chan;
+
 	for (i = 0; i < MAX_CHAN; i++) {
 		if (chan_list[i].index != frame->index &&
 					chan_list[i].ctrlid == 0)
@@ -1091,7 +1103,8 @@ static void sig_conn_req(const struct l2cap_frame *frame)
 	print_psm(pdu->psm);
 	print_cid("Source", pdu->scid);
 
-	assign_scid(frame, le16_to_cpu(pdu->scid), le16_to_cpu(pdu->psm), 0);
+	assign_scid(frame, le16_to_cpu(pdu->scid), le16_to_cpu(pdu->psm),
+						L2CAP_MODE_BASIC, 0);
 }
 
 static void sig_conn_rsp(const struct l2cap_frame *frame)
@@ -1220,7 +1233,7 @@ static void sig_create_chan_req(const struct l2cap_frame *frame)
 	print_field("Controller ID: %d", pdu->ctrlid);
 
 	assign_scid(frame, le16_to_cpu(pdu->scid), le16_to_cpu(pdu->psm),
-								pdu->ctrlid);
+						L2CAP_MODE_BASIC, pdu->ctrlid);
 }
 
 static void sig_create_chan_rsp(const struct l2cap_frame *frame)
@@ -1293,7 +1306,8 @@ static void sig_le_conn_req(const struct l2cap_frame *frame)
 	print_field("MPS: %u", le16_to_cpu(pdu->mps));
 	print_field("Credits: %u", le16_to_cpu(pdu->credits));
 
-	assign_scid(frame, le16_to_cpu(pdu->scid), le16_to_cpu(pdu->psm), 0);
+	assign_scid(frame, le16_to_cpu(pdu->scid), le16_to_cpu(pdu->psm),
+						L2CAP_MODE_LE_FLOWCTL, 0);
 }
 
 static void sig_le_conn_rsp(const struct l2cap_frame *frame)
@@ -3015,10 +3029,21 @@ static void smp_packet(uint16_t index, bool in, uint16_t handle,
 	opcode_data->func(&frame);
 }
 
+static struct chan_data *get_data(const struct l2cap_frame *frame)
+{
+	int i = get_chan_data_index(frame);
+
+	if (i < 0)
+		return NULL;
+
+	return &chan_list[i];
+}
+
 void l2cap_frame(uint16_t index, bool in, uint16_t handle, uint16_t cid,
 			uint16_t psm, const void *data, uint16_t size)
 {
 	struct l2cap_frame frame;
+	struct chan_data *chan;
 	uint32_t ctrl32 = 0;
 	uint16_t ctrl16 = 0;
 	uint8_t ext_ctrl;
@@ -3047,7 +3072,27 @@ void l2cap_frame(uint16_t index, bool in, uint16_t handle, uint16_t cid,
 		l2cap_frame_init(&frame, index, in, handle, 0, cid, psm,
 							data, size);
 
-		if (frame.mode > 0) {
+		switch (frame.mode) {
+		case L2CAP_MODE_LE_FLOWCTL:
+			chan = get_data(&frame);
+			if (!chan->sdu) {
+				if (!l2cap_frame_get_le16(&frame, &chan->sdu))
+					return;
+			}
+			print_indent(6, COLOR_CYAN, "Channel:", "",
+					COLOR_OFF, " %d len %d sdu %d"
+					" [PSM %d mode %d] {chan %d}",
+					cid, size, chan->sdu, frame.psm,
+					frame.mode, frame.chan);
+			chan->sdu -= frame.size;
+			break;
+		case L2CAP_MODE_BASIC:
+			print_indent(6, COLOR_CYAN, "Channel:", "", COLOR_OFF,
+					" %d len %d [PSM %d mode %d] {chan %d}",
+						cid, size, frame.psm,
+						frame.mode, frame.chan);
+			break;
+		default:
 			ext_ctrl = get_ext_ctrl(&frame);
 
 			if (ext_ctrl) {
@@ -3077,11 +3122,7 @@ void l2cap_frame(uint16_t index, bool in, uint16_t handle, uint16_t cid,
 			}
 
 			printf("\n");
-		} else {
-			print_indent(6, COLOR_CYAN, "Channel:", "", COLOR_OFF,
-					" %d len %d [PSM %d mode %d] {chan %d}",
-						cid, size, frame.psm,
-						frame.mode, frame.chan);
+			break;
 		}
 
 		switch (frame.psm) {
Signature · 1 line
2.21.0
author maintainer
#1 lore raw

Hi,

Quoted 157 lines · Luiz Augusto von Dentz

From: Luiz Augusto von Dentz <[email protected]>

This enables decoding LE flowctl mode properly and print out the SDU.

---
 monitor/l2cap.c | 65 ++++++++++++++++++++++++++++++++++++++++---------
 1 file changed, 53 insertions(+), 12 deletions(-)
monitor/l2cap.c +53 -13
index 26719ac5e..ad2499851 100644
@@ -47,6 +47,14 @@
 #include "rfcomm.h"
 #include "bnep.h"
 
+
+#define L2CAP_MODE_BASIC               0x00
+#define L2CAP_MODE_RETRANS             0x01
+#define L2CAP_MODE_FLOWCTL             0x02
+#define L2CAP_MODE_ERTM                        0x03
+#define L2CAP_MODE_STREAMING           0x04
+#define L2CAP_MODE_LE_FLOWCTL          0x80
+
 /* L2CAP Control Field bit masks */
 #define L2CAP_CTRL_SAR_MASK            0xC000
 #define L2CAP_CTRL_REQSEQ_MASK         0x3F00
@@ -102,12 +110,13 @@ struct chan_data {
        uint8_t  mode;
        uint8_t  ext_ctrl;
        uint8_t  seq_num;
+       uint16_t sdu;
 };
 
 static struct chan_data chan_list[MAX_CHAN];
 
-static void assign_scid(const struct l2cap_frame *frame,
-                               uint16_t scid, uint16_t psm, uint8_t ctrlid)
+static void assign_scid(const struct l2cap_frame *frame, uint16_t scid,
+                       uint16_t psm, uint8_t mode, uint8_t ctrlid)
 {
        int i, n = -1;
        uint8_t seq_num = 1;
@@ -154,7 +163,7 @@ static void assign_scid(const struct l2cap_frame *frame,
 
        chan_list[n].psm = psm;
        chan_list[n].ctrlid = ctrlid;
-       chan_list[n].mode = 0;
+       chan_list[n].mode = mode;
 
        chan_list[n].seq_num = seq_num;
 }
@@ -257,6 +266,9 @@ static int get_chan_data_index(const struct l2cap_frame *frame)
 {
        int i;
 
+       if (frame->chan)
+               return frame->chan;
+
        for (i = 0; i < MAX_CHAN; i++) {
                if (chan_list[i].index != frame->index &&
                                        chan_list[i].ctrlid == 0)
@@ -1091,7 +1103,8 @@ static void sig_conn_req(const struct l2cap_frame *frame)
        print_psm(pdu->psm);
        print_cid("Source", pdu->scid);
 
-       assign_scid(frame, le16_to_cpu(pdu->scid), le16_to_cpu(pdu->psm), 0);
+       assign_scid(frame, le16_to_cpu(pdu->scid), le16_to_cpu(pdu->psm),
+                                               L2CAP_MODE_BASIC, 0);
 }
 
 static void sig_conn_rsp(const struct l2cap_frame *frame)
@@ -1220,7 +1233,7 @@ static void sig_create_chan_req(const struct l2cap_frame *frame)
        print_field("Controller ID: %d", pdu->ctrlid);
 
        assign_scid(frame, le16_to_cpu(pdu->scid), le16_to_cpu(pdu->psm),
-                                                               pdu->ctrlid);
+                                               L2CAP_MODE_BASIC, pdu->ctrlid);
 }
 
 static void sig_create_chan_rsp(const struct l2cap_frame *frame)
@@ -1293,7 +1306,8 @@ static void sig_le_conn_req(const struct l2cap_frame *frame)
        print_field("MPS: %u", le16_to_cpu(pdu->mps));
        print_field("Credits: %u", le16_to_cpu(pdu->credits));
 
-       assign_scid(frame, le16_to_cpu(pdu->scid), le16_to_cpu(pdu->psm), 0);
+       assign_scid(frame, le16_to_cpu(pdu->scid), le16_to_cpu(pdu->psm),
+                                               L2CAP_MODE_LE_FLOWCTL, 0);
 }
 
 static void sig_le_conn_rsp(const struct l2cap_frame *frame)
@@ -3015,10 +3029,21 @@ static void smp_packet(uint16_t index, bool in, uint16_t handle,
        opcode_data->func(&frame);
 }
 
+static struct chan_data *get_data(const struct l2cap_frame *frame)
+{
+       int i = get_chan_data_index(frame);
+
+       if (i < 0)
+               return NULL;
+
+       return &chan_list[i];
+}
+
 void l2cap_frame(uint16_t index, bool in, uint16_t handle, uint16_t cid,
                        uint16_t psm, const void *data, uint16_t size)
 {
        struct l2cap_frame frame;
+       struct chan_data *chan;
        uint32_t ctrl32 = 0;
        uint16_t ctrl16 = 0;
        uint8_t ext_ctrl;
@@ -3047,7 +3072,27 @@ void l2cap_frame(uint16_t index, bool in, uint16_t handle, uint16_t cid,
                l2cap_frame_init(&frame, index, in, handle, 0, cid, psm,
                                                        data, size);
 
-               if (frame.mode > 0) {
+               switch (frame.mode) {
+               case L2CAP_MODE_LE_FLOWCTL:
+                       chan = get_data(&frame);
+                       if (!chan->sdu) {
+                               if (!l2cap_frame_get_le16(&frame, &chan->sdu))
+                                       return;
+                       }
+                       print_indent(6, COLOR_CYAN, "Channel:", "",
+                                       COLOR_OFF, " %d len %d sdu %d"
+                                       " [PSM %d mode %d] {chan %d}",
+                                       cid, size, chan->sdu, frame.psm,
+                                       frame.mode, frame.chan);
+                       chan->sdu -= frame.size;
+                       break;
+               case L2CAP_MODE_BASIC:
+                       print_indent(6, COLOR_CYAN, "Channel:", "", COLOR_OFF,
+                                       " %d len %d [PSM %d mode %d] {chan %d}",
+                                               cid, size, frame.psm,
+                                               frame.mode, frame.chan);
+                       break;
+               default:
                        ext_ctrl = get_ext_ctrl(&frame);
 
                        if (ext_ctrl) {
@@ -3077,11 +3122,7 @@ void l2cap_frame(uint16_t index, bool in, uint16_t handle, uint16_t cid,
                        }
 
                        printf("\n");
-               } else {
-                       print_indent(6, COLOR_CYAN, "Channel:", "", COLOR_OFF,
-                                       " %d len %d [PSM %d mode %d] {chan %d}",
-                                               cid, size, frame.psm,
-                                               frame.mode, frame.chan);
+                       break;
                }
 
                switch (frame.psm) {
--
Quoted 1 line

2.21.0

Pushed.

Signature · 1 line
Luiz Augusto von Dentz