From a4ac338896d590921edf79be43d5f3cdaca9949a Mon Sep 17 00:00:00 2001
From: Marcel Holtmann <marcel@holtmann.org>
Date: Fri, 9 Nov 2012 21:23:21 +0100
Subject: [PATCH] monitor: Use more explicit L2CAP header parsing

---
 monitor/l2cap.c | 34 ++++++++++++++++++++++------------
 1 file changed, 22 insertions(+), 12 deletions(-)

diff --git a/monitor/l2cap.c b/monitor/l2cap.c
index 70667ce72..57ef4acfb 100644
--- a/monitor/l2cap.c
+++ b/monitor/l2cap.c
@@ -314,37 +314,47 @@ static void smp_packet(const void *data, uint16_t size)
 void l2cap_packet(uint16_t handle, const void *data, uint16_t size)
 {
 	const struct bt_l2cap_hdr *hdr = data;
+	uint16_t len, cid;
 
 	if (size < sizeof(*hdr)) {
 		print_text(COLOR_ERROR, "malformed packet");
-		packet_hexdump(data, size);
-		return;
+		goto done;
 	}
 
-	if (btohs(hdr->len) != size - sizeof(*hdr)) {
+	len = btohs(hdr->len);
+	cid = btohs(hdr->cid);
+
+	data += sizeof(*hdr);
+	size -= sizeof(*hdr);
+
+	if (len != size) {
 		print_text(COLOR_ERROR, "invalid packet size");
-		packet_hexdump(data + sizeof(*hdr), size - sizeof(*hdr));
-		return;
+		goto done;
 	}
 
 	switch (btohs(hdr->cid)) {
 	case 0x0001:
 	case 0x0005:
-		sig_packet(data + sizeof(*hdr), size - sizeof(*hdr));
+		sig_packet(data, len);
 		break;
 	case 0x0003:
-		amp_packet(data + sizeof(*hdr), size - sizeof(*hdr));
+		amp_packet(data, len);
 		break;
 	case 0x0004:
-		att_packet(data + sizeof(*hdr), size - sizeof(*hdr));
+		att_packet(data, len);
 		break;
 	case 0x0006:
-		smp_packet(data + sizeof(*hdr), size - sizeof(*hdr));
+		smp_packet(data, len);
 		break;
 	default:
-		print_field("Channel: %d dlen %d", btohs(hdr->cid),
-							btohs(hdr->len));
-		packet_hexdump(data + sizeof(*hdr), size - sizeof(*hdr));
+		print_field("Channel: %d len %d", cid, len);
+		packet_hexdump(data, len);
 		break;
 	}
+
+	data += len;
+	size += len;
+
+done:
+	packet_hexdump(data, size);
 }
-- 
2.47.3