From: Luiz Augusto von Dentz <luiz.von.dentz@xxxxxxxxx>
This detects if the user logging is an input/output and then proceed to
decode the header which inform for which CID and PSM the data is for.
---
monitor/l2cap.c | 35 ++++++++++++++++++++---------------
monitor/l2cap.h | 3 +++
monitor/packet.c | 41 ++++++++++++++++++++++++++++++++++++++---
monitor/packet.h | 3 ++-
4 files changed, 63 insertions(+), 19 deletions(-)
diff --git a/monitor/l2cap.c b/monitor/l2cap.c
index ef2a14410..05a33b903 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 813c7932a..07864caee 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 aca0761fd..6932f592d 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 03279e114..199e15e58 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);
--
2.17.1
