diff options
Diffstat (limited to 'monitor/l2cap.c')
| -rw-r--r-- | monitor/l2cap.c | 53 |
1 files changed, 36 insertions, 17 deletions
diff --git a/monitor/l2cap.c b/monitor/l2cap.c index 894c7413a..79f705167 100644 --- a/monitor/l2cap.c +++ b/monitor/l2cap.c @@ -99,6 +99,7 @@ struct chan_data { uint8_t ctrlid; uint8_t mode; uint8_t ext_ctrl; + uint8_t seq_num; }; static struct chan_data chan_list[MAX_CHAN]; @@ -107,10 +108,13 @@ static void assign_scid(const struct l2cap_frame *frame, uint16_t scid, uint16_t psm, uint8_t ctrlid) { int i, n = -1; + uint8_t seq_num = 1; for (i = 0; i < MAX_CHAN; i++) { - if (n < 0 && chan_list[i].handle == 0x0000) + if (n < 0 && chan_list[i].handle == 0x0000) { n = i; + continue; + } if (chan_list[i].index != frame->index) continue; @@ -118,16 +122,18 @@ static void assign_scid(const struct l2cap_frame *frame, if (chan_list[i].handle != frame->handle) continue; + if (chan_list[i].psm == psm) + seq_num++; + + /* Don't break on match - we still need to go through all + * channels to find proper seq_num. + */ if (frame->in) { - if (chan_list[i].dcid == scid) { + if (chan_list[i].dcid == scid) n = i; - break; - } } else { - if (chan_list[i].scid == scid) { + if (chan_list[i].scid == scid) n = i; - break; - } } } @@ -147,6 +153,8 @@ 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].seq_num = seq_num; } static void release_scid(const struct l2cap_frame *frame, uint16_t scid) @@ -301,6 +309,16 @@ static uint16_t get_chan(const struct l2cap_frame *frame) return i; } +static uint8_t get_seq_num(const struct l2cap_frame *frame) +{ + int i = get_chan_data_index(frame); + + if (i < 0) + return 0; + + return chan_list[i].seq_num; +} + static void assign_ext_ctrl(const struct l2cap_frame *frame, uint8_t ext_ctrl, uint16_t dcid) { @@ -1384,16 +1402,17 @@ static void l2cap_frame_init(struct l2cap_frame *frame, uint16_t index, bool in, uint16_t handle, uint8_t ident, uint16_t cid, const void *data, uint16_t size) { - frame->index = index; - frame->in = in; - frame->handle = handle; - frame->ident = ident; - frame->cid = cid; - frame->data = data; - frame->size = size; - frame->psm = get_psm(frame); - frame->mode = get_mode(frame); - frame->chan = get_chan(frame); + frame->index = index; + frame->in = in; + frame->handle = handle; + frame->ident = ident; + frame->cid = cid; + frame->data = data; + frame->size = size; + frame->psm = get_psm(frame); + frame->mode = get_mode(frame); + frame->chan = get_chan(frame); + frame->seq_num = get_seq_num(frame); } static void bredr_sig_packet(uint16_t index, bool in, uint16_t handle, |