Hi,
On Fri, Sep 13, 2019 at 10:23 AM Luiz Augusto von Dentz
<luiz.dentz@xxxxxxxxx> wrote:
>
> From: Luiz Augusto von Dentz <luiz.von.dentz@xxxxxxxxx>
>
> 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
