* [PATCH BlueZ] monitor: Add proper decoding of LE flowctl mode
@ 2019-09-13 7:23 Luiz Augusto von Dentz
2019-09-16 9:29 ` Luiz Augusto von Dentz
0 siblings, 1 reply; 2+ messages in thread
From: Luiz Augusto von Dentz @ 2019-09-13 7:23 UTC (permalink / raw)
To: linux-bluetooth
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
^ permalink raw reply related [flat|nested] 2+ messages in thread
* Re: [PATCH BlueZ] monitor: Add proper decoding of LE flowctl mode
2019-09-13 7:23 [PATCH BlueZ] monitor: Add proper decoding of LE flowctl mode Luiz Augusto von Dentz
@ 2019-09-16 9:29 ` Luiz Augusto von Dentz
0 siblings, 0 replies; 2+ messages in thread
From: Luiz Augusto von Dentz @ 2019-09-16 9:29 UTC (permalink / raw)
To: linux-bluetooth
Hi,
On Fri, Sep 13, 2019 at 10:23 AM Luiz Augusto von Dentz
<luiz.dentz@gmail.com> wrote:
>
> 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
Pushed.
--
Luiz Augusto von Dentz
^ permalink raw reply [flat|nested] 2+ messages in thread
end of thread, other threads:[~2019-09-16 9:30 UTC | newest]
Thread overview: 2+ messages (download: mbox.gz / follow: Atom feed)
-- links below jump to the message on this page --
2019-09-13 7:23 [PATCH BlueZ] monitor: Add proper decoding of LE flowctl mode Luiz Augusto von Dentz
2019-09-16 9:29 ` Luiz Augusto von Dentz
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).