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

@@ -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)
 {

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)
 {

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

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;
 }