From: Luiz Augusto von Dentz <luiz.dentz@gmail.com>
To: linux-bluetooth@vger.kernel.org
Subject: [PATCH BlueZ] monitor: Add proper decoding of LE flowctl mode
Date: Fri, 13 Sep 2019 10:23:45 +0300 [thread overview]
Message-ID: <20190913072345.24118-1-luiz.dentz@gmail.com> (raw)
From: Luiz Augusto von Dentz <luiz.von.dentz@intel.com>
This enables decoding LE flowctl mode properly and print out the
SDU.
---
monitor/l2cap.c | 65 ++++++++++++++++++++++++++++++++++++++++---------
1 file changed, 53 insertions(+), 12 deletions(-)
diff --git a/monitor/l2cap.c b/monitor/l2cap.c
index 26719ac5e..ad2499851 100644
--- a/monitor/l2cap.c
+++ b/monitor/l2cap.c
@@ -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) {
--
2.21.0
next reply other threads:[~2019-09-13 7:23 UTC|newest]
Thread overview: 2+ messages / expand[flat|nested] mbox.gz Atom feed top
2019-09-13 7:23 Luiz Augusto von Dentz [this message]
2019-09-16 9:29 ` [PATCH BlueZ] monitor: Add proper decoding of LE flowctl mode Luiz Augusto von Dentz
Reply instructions:
You may reply publicly to this message via plain-text email
using any one of the following methods:
* Save the following mbox file, import it into your mail client,
and reply-to-all from there: mbox
Avoid top-posting and favor interleaved quoting:
https://en.wikipedia.org/wiki/Posting_style#Interleaved_style
* Reply using the --to, --cc, and --in-reply-to
switches of git-send-email(1):
git send-email \
--in-reply-to=20190913072345.24118-1-luiz.dentz@gmail.com \
--to=luiz.dentz@gmail.com \
--cc=linux-bluetooth@vger.kernel.org \
/path/to/YOUR_REPLY
https://kernel.org/pub/software/scm/git/docs/git-send-email.html
* If your mail client supports setting the In-Reply-To header
via mailto: links, try the mailto: link
Be sure your reply has a Subject: header at the top and a blank line
before the message body.
This is a public inbox, see mirroring instructions
for how to clone and mirror all data and code used for this inbox;
as well as URLs for NNTP newsgroup(s).