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 #include #include +#include #include #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; }