Diff between 16ad0a71a26fc47afab66d5d882492d0c8312bab and 56753316a4b67b66feb9d1d9fc6502e9ed043b1e

Changed Files

File Additions Deletions Status
monitor/l2cap.c +39 -18 modified
monitor/l2cap.h +2 -0 modified

Full Patch

diff --git a/monitor/l2cap.c b/monitor/l2cap.c
index 7a6edbe..5faa26f 100644
--- a/monitor/l2cap.c
+++ b/monitor/l2cap.c
@@ -91,6 +91,7 @@
 struct chan_data {
 	uint16_t index;
 	uint16_t handle;
+	uint8_t ident;
 	uint16_t scid;
 	uint16_t dcid;
 	uint16_t psm;
@@ -135,6 +136,7 @@ static void assign_scid(const struct l2cap_frame *frame,
 	memset(&chan_list[n], 0, sizeof(chan_list[n]));
 	chan_list[n].index = frame->index;
 	chan_list[n].handle = frame->handle;
+	chan_list[n].ident = frame->ident;
 
 	if (frame->in)
 		chan_list[n].dcid = scid;
@@ -171,8 +173,8 @@ static void release_scid(const struct l2cap_frame *frame, uint16_t scid)
 	}
 }
 
-static void assign_dcid(const struct l2cap_frame *frame,
-					uint16_t dcid, uint16_t scid)
+static void assign_dcid(const struct l2cap_frame *frame, uint16_t dcid,
+								uint16_t scid)
 {
 	int i;
 
@@ -183,15 +185,32 @@ static void assign_dcid(const struct l2cap_frame *frame,
 		if (chan_list[i].handle != frame->handle)
 			continue;
 
+		if (frame->ident != 0 && chan_list[i].ident != frame->ident)
+			continue;
+
 		if (frame->in) {
-			if (chan_list[i].scid == scid) {
-				chan_list[i].dcid = dcid;
-				break;
+			if (scid) {
+				if (chan_list[i].scid == scid) {
+					chan_list[i].dcid = dcid;
+					break;
+				}
+			} else {
+				if (chan_list[i].scid && !chan_list[i].dcid) {
+					chan_list[i].dcid = dcid;
+					break;
+				}
 			}
 		} else {
-			if (chan_list[i].dcid == scid) {
-				chan_list[i].scid = dcid;
-				break;
+			if (scid) {
+				if (chan_list[i].dcid == scid) {
+					chan_list[i].scid = dcid;
+					break;
+				}
+			} else {
+				if (chan_list[i].dcid && !chan_list[i].scid) {
+					chan_list[i].scid = dcid;
+					break;
+				}
 			}
 		}
 	}
@@ -1204,7 +1223,7 @@ static void sig_le_conn_rsp(const struct l2cap_frame *frame)
 	print_field("Credits: %u", le16_to_cpu(pdu->credits));
 	print_conn_result(pdu->result);
 
-	/*assign_dcid(frame, le16_to_cpu(pdu->dcid), le16_to_cpu(pdu->scid));*/
+	assign_dcid(frame, le16_to_cpu(pdu->dcid), 0);
 }
 
 static void sig_le_flowctl_creds(const struct l2cap_frame *frame)
@@ -1281,13 +1300,14 @@ static const struct sig_opcode_data le_sig_opcode_table[] = {
 	{ },
 };
 
-static void l2cap_frame_init(struct l2cap_frame *frame,
-				uint16_t index, bool in, uint16_t handle,
+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;
@@ -1376,7 +1396,8 @@ static void bredr_sig_packet(uint16_t index, bool in, uint16_t handle,
 			}
 		}
 
-		l2cap_frame_init(&frame, index, in, handle, cid, data, len);
+		l2cap_frame_init(&frame, index, in, handle, hdr->ident, cid,
+								data, len);
 		opcode_data->func(&frame);
 
 		data += len;
@@ -1457,7 +1478,7 @@ static void le_sig_packet(uint16_t index, bool in, uint16_t handle,
 		}
 	}
 
-	l2cap_frame_init(&frame, index, in, handle, cid, data, len);
+	l2cap_frame_init(&frame, index, in, handle, hdr->ident, cid, data, len);
 	opcode_data->func(&frame);
 }
 
@@ -1488,7 +1509,7 @@ static void connless_packet(uint16_t index, bool in, uint16_t handle,
 		break;
 	}
 
-	l2cap_frame_init(&frame, index, in, handle, cid, data, size);
+	l2cap_frame_init(&frame, index, in, handle, 0, cid, data, size);
 }
 
 static void print_controller_list(const uint8_t *data, uint16_t size)
@@ -1857,7 +1878,7 @@ static void amp_packet(uint16_t index, bool in, uint16_t handle,
 		}
 	}
 
-	l2cap_frame_init(&frame, index, in, handle, cid, data + 6, len);
+	l2cap_frame_init(&frame, index, in, handle, 0, cid, data + 6, len);
 	opcode_data->func(&frame);
 }
 
@@ -2400,7 +2421,7 @@ static void att_packet(uint16_t index, bool in, uint16_t handle,
 		}
 	}
 
-	l2cap_frame_init(&frame, index, in, handle, cid, data + 1, size - 1);
+	l2cap_frame_init(&frame, index, in, handle, 0, cid, data + 1, size - 1);
 	opcode_data->func(&frame);
 }
 
@@ -2851,7 +2872,7 @@ static void smp_packet(uint16_t index, bool in, uint16_t handle,
 		}
 	}
 
-	l2cap_frame_init(&frame, index, in, handle, cid, data + 1, size - 1);
+	l2cap_frame_init(&frame, index, in, handle, 0, cid, data + 1, size - 1);
 	opcode_data->func(&frame);
 }
 
@@ -2884,7 +2905,7 @@ static void l2cap_frame(uint16_t index, bool in, uint16_t handle,
 		smp_packet(index, in, handle, cid, data, size);
 		break;
 	default:
-		l2cap_frame_init(&frame, index, in, handle, cid, data, size);
+		l2cap_frame_init(&frame, index, in, handle, 0, cid, data, size);
 
 		if (frame.mode > 0) {
 			ext_ctrl = get_ext_ctrl(&frame);
diff --git a/monitor/l2cap.h b/monitor/l2cap.h
index 975f78a..711bcb1 100644
--- a/monitor/l2cap.h
+++ b/monitor/l2cap.h
@@ -29,6 +29,7 @@ struct l2cap_frame {
 	uint16_t index;
 	bool in;
 	uint16_t handle;
+	uint8_t ident;
 	uint16_t cid;
 	uint16_t psm;
 	uint16_t chan;
@@ -44,6 +45,7 @@ static inline void l2cap_frame_pull(struct l2cap_frame *frame,
 		frame->index   = source->index;
 		frame->in      = source->in;
 		frame->handle  = source->handle;
+		frame->ident   = source->ident;
 		frame->cid     = source->cid;
 		frame->psm     = source->psm;
 		frame->chan    = source->chan;