monitor: Add support for user input/output data

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.

Change-Id: Idc21d2f1473f4b77569cea49c4a8ccd6c5e7c78a
Signed-off-by: himanshu <h.himanshu@samsung.com>

diff --git a/monitor/l2cap.c b/monitor/l2cap.c
index c85934a..f667335 100755 (executable)
--- a/monitor/l2cap.c
+++ b/monitor/l2cap.c
@@ -1404,7 +1404,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;
@@ -1413,10 +1414,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,
@@ -1499,7 +1500,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);
 
@@ -1581,7 +1582,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);
 }
 
@@ -1612,7 +1614,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)
@@ -1981,7 +1983,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);
 }
 
@@ -2564,7 +2566,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);
 }
 
@@ -3020,12 +3023,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;
@@ -3053,7 +3057,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);
@@ -3156,7 +3161,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;
                }
 
@@ -3201,7 +3206,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);
@@ -3236,7 +3241,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 100755 (executable)
--- 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 fff04b4..97c21fd 100755 (executable)
--- a/monitor/packet.c
+++ b/monitor/packet.c
@@ -4092,7 +4092,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();
@@ -10078,9 +10079,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;
@@ -10135,7 +10163,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 545d817..c0780cd 100755 (executable)
--- 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);