monitor: Add proper decoding of LE flowctl mode
Luiz Augusto von Dentz
maintainerPATCH 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
@@ -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
Luiz Augusto von Dentz
maintainerHi,
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@@ -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