From fe94f61475d702fb367d89098ce5193671c8943a Mon Sep 17 00:00:00 2001
From: Chris Lew <quic_clew@quicinc.com>
Date: Tue, 1 Nov 2022 13:53:38 -0700
Subject: [PATCH] net: qrtr: log rx packet causing system wakeup

Log qrtr rx packet in kernel logs which caused system wakeup.
This will help to easily identify wakeup packet.

The qrtr module is required to be in first stage init to support
the creation of qrtr sockets. The transport layers are often in second
stage init. The current wakeup prints depend on some information from
the second stage init. Rework the logic so the dependency is only
between the qrtr transport and transport drivers.

Change-Id: I4a6a35dd4d5449a671464c00f90712a79d6044fe
Signed-off-by: Deepak Kumar Singh <deesin@codeaurora.org>
Signed-off-by: Chris Lew <quic_clew@quicinc.com>
---
 net/qrtr/af_qrtr.c | 58 ++++++++++++++++++++++++++++++++++++++++++++++
 net/qrtr/ns.c      | 19 +++++++++++++++
 net/qrtr/qrtr.h    |  4 ++++
 net/qrtr/smd.c     |  4 ++++
 4 files changed, 85 insertions(+)

diff --git a/net/qrtr/af_qrtr.c b/net/qrtr/af_qrtr.c
index a80f9621146e..e1d6c1813173 100644
--- a/net/qrtr/af_qrtr.c
+++ b/net/qrtr/af_qrtr.c
@@ -301,6 +301,64 @@ static void qrtr_log_rx_msg(struct qrtr_node *node, struct sk_buff *skb)
 	}
 }
 
+void qrtr_print_wakeup_reason(const void *data)
+{
+	const struct qrtr_hdr_v1 *v1;
+	const struct qrtr_hdr_v2 *v2;
+	struct qrtr_cb cb;
+	unsigned int size;
+	unsigned int ver;
+	int service_id;
+	size_t hdrlen;
+	u64 preview = 0;
+
+	ver = *(u8 *)data;
+	switch (ver) {
+	case QRTR_PROTO_VER_1:
+		v1 = data;
+		hdrlen = sizeof(*v1);
+		cb.src_node = le32_to_cpu(v1->src_node_id);
+		cb.src_port = le32_to_cpu(v1->src_port_id);
+		cb.dst_node = le32_to_cpu(v1->dst_node_id);
+		cb.dst_port = le32_to_cpu(v1->dst_port_id);
+
+		size = le32_to_cpu(v1->size);
+		break;
+	case QRTR_PROTO_VER_2:
+		v2 = data;
+		hdrlen = sizeof(*v2) + v2->optlen;
+		cb.src_node = le16_to_cpu(v2->src_node_id);
+		cb.src_port = le16_to_cpu(v2->src_port_id);
+		cb.dst_node = le16_to_cpu(v2->dst_node_id);
+		cb.dst_port = le16_to_cpu(v2->dst_port_id);
+
+		if (cb.src_port == (u16)QRTR_PORT_CTRL)
+			cb.src_port = QRTR_PORT_CTRL;
+		if (cb.dst_port == (u16)QRTR_PORT_CTRL)
+			cb.dst_port = QRTR_PORT_CTRL;
+
+		size = le32_to_cpu(v2->size);
+		break;
+	default:
+		return;
+	}
+
+	service_id = qrtr_get_service_id(cb.src_node, cb.src_port);
+	if (service_id < 0)
+		service_id = qrtr_get_service_id(cb.dst_node, cb.dst_port);
+
+	size = (sizeof(preview) > size) ? size : sizeof(preview);
+	memcpy(&preview, data + hdrlen, size);
+
+	pr_info("%s: src[0x%x:0x%x] dst[0x%x:0x%x] [%08x %08x] service[0x%x]\n",
+		__func__,
+		cb.src_node, cb.src_port,
+		cb.dst_node, cb.dst_port,
+		(unsigned int)preview, (unsigned int)(preview >> 32),
+		service_id);
+}
+EXPORT_SYMBOL(qrtr_print_wakeup_reason);
+
 static bool refcount_dec_and_rwsem_lock(refcount_t *r,
 					struct rw_semaphore *sem)
 {
diff --git a/net/qrtr/ns.c b/net/qrtr/ns.c
index defd50d1d66e..4f56f64df151 100644
--- a/net/qrtr/ns.c
+++ b/net/qrtr/ns.c
@@ -99,6 +99,25 @@ static struct qrtr_node *node_get(unsigned int node_id)
 	return node;
 }
 
+int qrtr_get_service_id(unsigned int node_id, unsigned int port_id)
+{
+	struct qrtr_server *srv;
+	struct qrtr_node *node;
+	unsigned long index;
+
+	node = node_get(node_id);
+	if (!node)
+		return -EINVAL;
+
+	xa_for_each(&node->servers, index, srv) {
+		if (srv->node == node_id && srv->port == port_id)
+			return srv->service;
+	}
+
+	return -EINVAL;
+}
+EXPORT_SYMBOL(qrtr_get_service_id);
+
 static int server_match(const struct qrtr_server *srv,
 			const struct qrtr_server_filter *f)
 {
diff --git a/net/qrtr/qrtr.h b/net/qrtr/qrtr.h
index fb64973d6775..f0ee970a8546 100644
--- a/net/qrtr/qrtr.h
+++ b/net/qrtr/qrtr.h
@@ -38,4 +38,8 @@ int qrtr_ns_init(void);
 void qrtr_ns_remove(void);
 
 int qrtr_peek_pkt_size(const void *data);
+
+int qrtr_get_service_id(unsigned int node_id, unsigned int port_id);
+
+void qrtr_print_wakeup_reason(const void *data);
 #endif
diff --git a/net/qrtr/smd.c b/net/qrtr/smd.c
index d95bd330bca8..cc2381c08f26 100644
--- a/net/qrtr/smd.c
+++ b/net/qrtr/smd.c
@@ -8,6 +8,7 @@
 #include <linux/module.h>
 #include <linux/skbuff.h>
 #include <linux/rpmsg.h>
+#include <linux/rpmsg/qcom_glink.h>
 #include <linux/of.h>
 
 #include "qrtr.h"
@@ -37,6 +38,9 @@ static int qcom_smd_qrtr_callback(struct rpmsg_device *rpdev,
 		rc = 0;
 	}
 
+	if (qcom_glink_is_wakeup(true))
+		qrtr_print_wakeup_reason(data);
+
 	return rc;
 }