Diff between cd64eb533abea0e2a25d0f74dc66a9ab26c32da7 and 64dffbe4312a2892193ac408d691692ec16b15b5

Changed Files

File Additions Deletions Status
monitor/l2cap.c +36 -17 modified
monitor/l2cap.h +1 -0 modified

Full Patch

diff --git a/monitor/l2cap.c b/monitor/l2cap.c
index 894c741..79f7051 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,
diff --git a/monitor/l2cap.h b/monitor/l2cap.h
index 0364454..813c793 100644
--- a/monitor/l2cap.h
+++ b/monitor/l2cap.h
@@ -34,6 +34,7 @@ struct l2cap_frame {
 	uint16_t psm;
 	uint16_t chan;
 	uint8_t mode;
+	uint8_t seq_num;
 	const void *data;
 	uint16_t size;
 };