Diff between 0aabe64aafca212ae9067f6a02705befc5c1bc26 and 78b3106989dd57a30906b9a00940f74a1df609dc

Changed Files

File Additions Deletions Status
monitor/l2cap.c +20 -15 modified
monitor/l2cap.h +3 -0 modified
monitor/packet.c +38 -3 modified
monitor/packet.h +2 -1 modified

Full Patch

diff --git a/monitor/l2cap.c b/monitor/l2cap.c
index ef2a144..05a33b9 100644
--- a/monitor/l2cap.c
+++ b/monitor/l2cap.c
@@ -1384,7 +1384,8 @@ 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, uint8_t ident,
-				uint16_t cid, const void *data, uint16_t size)
+				uint16_t cid, uint16_t psm,
+				const void *data, uint16_t size)
 {
 	frame->index   = index;
 	frame->in      = in;
@@ -1393,10 +1394,10 @@ static void l2cap_frame_init(struct l2cap_frame *frame, uint16_t index, bool in,
 	frame->cid     = cid;
 	frame->data    = data;
 	frame->size    = size;
-	frame->psm     = get_psm(frame);
+	frame->psm     = psm ? psm : get_psm(frame);
 	frame->mode    = get_mode(frame);
 	frame->chan    = get_chan(frame);
-	frame->seq_num = get_seq_num(frame);
+	frame->seq_num = psm ? 1 : get_seq_num(frame);
 }
 
 static void bredr_sig_packet(uint16_t index, bool in, uint16_t handle,
@@ -1479,7 +1480,7 @@ static void bredr_sig_packet(uint16_t index, bool in, uint16_t handle,
 			}
 		}
 
-		l2cap_frame_init(&frame, index, in, handle, hdr->ident, cid,
+		l2cap_frame_init(&frame, index, in, handle, hdr->ident, cid, 0,
 								data, len);
 		opcode_data->func(&frame);
 
@@ -1561,7 +1562,8 @@ static void le_sig_packet(uint16_t index, bool in, uint16_t handle,
 		}
 	}
 
-	l2cap_frame_init(&frame, index, in, handle, hdr->ident, cid, data, len);
+	l2cap_frame_init(&frame, index, in, handle, hdr->ident, cid, 0,
+							data, len);
 	opcode_data->func(&frame);
 }
 
@@ -1592,7 +1594,7 @@ static void connless_packet(uint16_t index, bool in, uint16_t handle,
 		break;
 	}
 
-	l2cap_frame_init(&frame, index, in, handle, 0, cid, data, size);
+	l2cap_frame_init(&frame, index, in, handle, 0, cid, 0, data, size);
 }
 
 static void print_controller_list(const uint8_t *data, uint16_t size)
@@ -1961,7 +1963,7 @@ static void amp_packet(uint16_t index, bool in, uint16_t handle,
 		}
 	}
 
-	l2cap_frame_init(&frame, index, in, handle, 0, cid, data + 6, len);
+	l2cap_frame_init(&frame, index, in, handle, 0, cid, 0, data + 6, len);
 	opcode_data->func(&frame);
 }
 
@@ -2544,7 +2546,8 @@ static void att_packet(uint16_t index, bool in, uint16_t handle,
 		}
 	}
 
-	l2cap_frame_init(&frame, index, in, handle, 0, cid, data + 1, size - 1);
+	l2cap_frame_init(&frame, index, in, handle, 0, cid, 0,
+						data + 1, size - 1);
 	opcode_data->func(&frame);
 }
 
@@ -3000,12 +3003,13 @@ static void smp_packet(uint16_t index, bool in, uint16_t handle,
 		}
 	}
 
-	l2cap_frame_init(&frame, index, in, handle, 0, cid, data + 1, size - 1);
+	l2cap_frame_init(&frame, index, in, handle, 0, cid, 0,
+						data + 1, size - 1);
 	opcode_data->func(&frame);
 }
 
-static void l2cap_frame(uint16_t index, bool in, uint16_t handle,
-			uint16_t cid, const void *data, uint16_t size)
+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;
 	uint32_t ctrl32 = 0;
@@ -3033,7 +3037,8 @@ 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, 0, cid, data, size);
+		l2cap_frame_init(&frame, index, in, handle, 0, cid, psm,
+							data, size);
 
 		if (frame.mode > 0) {
 			ext_ctrl = get_ext_ctrl(&frame);
@@ -3136,7 +3141,7 @@ void l2cap_packet(uint16_t index, bool in, uint16_t handle, uint8_t flags,
 
 		if (len == size) {
 			/* complete frame */
-			l2cap_frame(index, in, handle, cid, data, len);
+			l2cap_frame(index, in, handle, cid, 0, data, len);
 			return;
 		}
 
@@ -3181,7 +3186,7 @@ void l2cap_packet(uint16_t index, bool in, uint16_t handle, uint8_t flags,
 		if (!index_list[index][in].frag_len) {
 			/* complete frame */
 			l2cap_frame(index, in, handle,
-					index_list[index][in].frag_cid,
+					index_list[index][in].frag_cid, 0,
 					index_list[index][in].frag_buf,
 					index_list[index][in].frag_pos);
 			clear_fragment_buffer(index, in);
@@ -3216,7 +3221,7 @@ void l2cap_packet(uint16_t index, bool in, uint16_t handle, uint8_t flags,
 		}
 
 		/* complete frame */
-		l2cap_frame(index, in, handle, cid, data, len);
+		l2cap_frame(index, in, handle, cid, 0, data, len);
 		break;
 
 	default:
diff --git a/monitor/l2cap.h b/monitor/l2cap.h
index 813c793..07864ca 100644
--- a/monitor/l2cap.h
+++ b/monitor/l2cap.h
@@ -170,6 +170,9 @@ static inline bool l2cap_frame_get_be128(struct l2cap_frame *frame,
 	return true;
 }
 
+void l2cap_frame(uint16_t index, bool in, uint16_t handle, uint16_t cid,
+		uint16_t psm, const void *data, uint16_t size);
+
 void l2cap_packet(uint16_t index, bool in, uint16_t handle, uint8_t flags,
 					const void *data, uint16_t size);
 
diff --git a/monitor/packet.c b/monitor/packet.c
index aca0761..6932f59 100644
--- a/monitor/packet.c
+++ b/monitor/packet.c
@@ -3947,7 +3947,8 @@ void packet_monitor(struct timeval *tv, struct ucred *cred,
 		ident = ul->ident_len ? data + sizeof(*ul) : NULL;
 
 		packet_user_logging(tv, cred, index, ul->priority, ident,
-					data + sizeof(*ul) + ul->ident_len);
+					data + sizeof(*ul) + ul->ident_len,
+					size - (sizeof(*ul) + ul->ident_len));
 		break;
 	case BTSNOOP_OPCODE_CTRL_OPEN:
 		control_disable_decoding();
@@ -9873,9 +9874,36 @@ void packet_system_note(struct timeval *tv, struct ucred *cred,
 					"Note", message, NULL);
 }
 
+struct monitor_l2cap_hdr {
+	uint16_t cid;
+	uint16_t psm;
+};
+
+static void packet_decode(struct timeval *tv, struct ucred *cred, char dir,
+				uint16_t index, const char *color,
+				const char *label, const void *data,
+				uint16_t size)
+{
+	const struct monitor_l2cap_hdr *hdr = data;
+
+	if (size < sizeof(*hdr)) {
+		print_packet(tv, cred, '*', index, NULL, COLOR_ERROR,
+			"Malformed User Data packet", NULL, NULL);
+	}
+
+	print_packet(tv, cred, dir, index, NULL, COLOR_HCI_ACLDATA, label,
+				dir == '>' ? "User Data RX" : "User Data TX",
+				NULL);
+
+	/* Discard last byte since it just a filler */
+	l2cap_frame(index, dir == '>', 0, hdr->cid, hdr->psm,
+			data + sizeof(*hdr), size - (sizeof(*hdr) + 1));
+}
+
 void packet_user_logging(struct timeval *tv, struct ucred *cred,
 					uint16_t index, uint8_t priority,
-					const char *ident, const char *message)
+					const char *ident, const void *data,
+					uint16_t size)
 {
 	char pid_str[140];
 	const char *label;
@@ -9930,7 +9958,14 @@ void packet_user_logging(struct timeval *tv, struct ucred *cred,
 			label = "Message";
 	}
 
-	print_packet(tv, cred, '=', index, NULL, color, label, message, NULL);
+	if (ident[0] == '<' || ident[0] == '>') {
+		packet_decode(tv, cred, ident[0], index, color,
+				label == ident ? &ident[2] : label,
+				data, size);
+		return;
+	}
+
+	print_packet(tv, cred, '=', index, NULL, color, label, data, NULL);
 }
 
 void packet_hci_command(struct timeval *tv, struct ucred *cred, uint16_t index,
diff --git a/monitor/packet.h b/monitor/packet.h
index 03279e1..199e15e 100644
--- a/monitor/packet.h
+++ b/monitor/packet.h
@@ -83,7 +83,8 @@ void packet_system_note(struct timeval *tv, struct ucred *cred,
 					uint16_t index, const void *message);
 void packet_user_logging(struct timeval *tv, struct ucred *cred,
 					uint16_t index, uint8_t priority,
-					const char *ident, const char *message);
+					const char *ident, const void *data,
+					uint16_t size);
 
 void packet_hci_command(struct timeval *tv, struct ucred *cred, uint16_t index,
 					const void *data, uint16_t size);