diff --git a/include/uapi/linux/qrtr.h b/include/uapi/linux/qrtr.h index f7e2fb3d752b..a85d5d4a6882 100644 --- a/include/uapi/linux/qrtr.h +++ b/include/uapi/linux/qrtr.h @@ -27,6 +27,7 @@ enum qrtr_pkt_type { QRTR_TYPE_NEW_LOOKUP = 10, QRTR_TYPE_DEL_LOOKUP = 11, }; +#define QRTR_TYPE_DEL_PROC 13 struct qrtr_ctrl_pkt { __le32 cmd; @@ -43,6 +44,11 @@ struct qrtr_ctrl_pkt { __le32 node; __le32 port; } client; + + struct { + __le32 rsvd; + __le32 node; + } proc; }; } __packed; diff --git a/net/qrtr/af_qrtr.c b/net/qrtr/af_qrtr.c index e12833add098..e1d6c1813173 100644 --- a/net/qrtr/af_qrtr.c +++ b/net/qrtr/af_qrtr.c @@ -14,12 +14,17 @@ #include #include #include +#include #include #include #include "qrtr.h" +#define QRTR_LOG_PAGE_CNT 4 +#define QRTR_INFO(ctx, x, ...) \ + ipc_log_string(ctx, x, ##__VA_ARGS__) + #define QRTR_PROTO_VER_1 1 #define QRTR_PROTO_VER_2 3 @@ -156,6 +161,7 @@ static struct work_struct qrtr_backup_work; * @read_data: scheduled work for recv work * @say_hello: scheduled work for initiating hello * @ws: wakeupsource avoid system suspend + * @ilc: ipc logging context reference */ struct qrtr_node { struct mutex ep_lock; @@ -178,6 +184,7 @@ struct qrtr_node { struct kthread_work say_hello; struct wakeup_source *ws; + void *ilc; }; /** @@ -204,6 +211,154 @@ static int qrtr_bcast_enqueue(struct qrtr_node *node, struct sk_buff *skb, static struct qrtr_sock *qrtr_port_lookup(int port); static void qrtr_port_put(struct qrtr_sock *ipc); +static void qrtr_log_tx_msg(struct qrtr_node *node, struct qrtr_hdr_v1 *hdr, + struct sk_buff *skb) +{ + struct qrtr_ctrl_pkt pkt = {0,}; + u64 pl_buf = 0; + int type; + + if (!hdr || !skb) + return; + + type = le32_to_cpu(hdr->type); + if (type == QRTR_TYPE_DATA) { + skb_copy_bits(skb, QRTR_HDR_MAX_SIZE, &pl_buf, sizeof(pl_buf)); + QRTR_INFO(node->ilc, + "TX DATA: Len:0x%x CF:0x%x src[0x%x:0x%x] dst[0x%x:0x%x] [%08x %08x] [%s]\n", + hdr->size, hdr->confirm_rx, + hdr->src_node_id, hdr->src_port_id, + hdr->dst_node_id, hdr->dst_port_id, + (unsigned int)pl_buf, (unsigned int)(pl_buf >> 32), + current->comm); + } else { + skb_copy_bits(skb, QRTR_HDR_MAX_SIZE, &pkt, sizeof(pkt)); + if (type == QRTR_TYPE_NEW_SERVER || + type == QRTR_TYPE_DEL_SERVER) + QRTR_INFO(node->ilc, + "TX CTRL: cmd:0x%x SVC[0x%x:0x%x] addr[0x%x:0x%x]\n", + type, le32_to_cpu(pkt.server.service), + le32_to_cpu(pkt.server.instance), + le32_to_cpu(pkt.server.node), + le32_to_cpu(pkt.server.port)); + else if (type == QRTR_TYPE_DEL_CLIENT || + type == QRTR_TYPE_RESUME_TX) + QRTR_INFO(node->ilc, + "TX CTRL: cmd:0x%x addr[0x%x:0x%x]\n", + type, le32_to_cpu(pkt.client.node), + le32_to_cpu(pkt.client.port)); + else if (type == QRTR_TYPE_HELLO || + type == QRTR_TYPE_BYE) + QRTR_INFO(node->ilc, + "TX CTRL: cmd:0x%x node[0x%x]\n", + type, hdr->src_node_id); + else if (type == QRTR_TYPE_DEL_PROC) + QRTR_INFO(node->ilc, + "TX CTRL: cmd:0x%x node[0x%x]\n", + type, pkt.proc.node); + } +} + +static void qrtr_log_rx_msg(struct qrtr_node *node, struct sk_buff *skb) +{ + struct qrtr_ctrl_pkt pkt = {0,}; + struct qrtr_cb *cb; + u64 pl_buf = 0; + + if (!skb) + return; + + cb = (struct qrtr_cb *)skb->cb; + + if (cb->type == QRTR_TYPE_DATA) { + skb_copy_bits(skb, 0, &pl_buf, sizeof(pl_buf)); + QRTR_INFO(node->ilc, + "RX DATA: Len:0x%x CF:0x%x src[0x%x:0x%x] dst[0x%x:0x%x] [%08x %08x]\n", + skb->len, cb->confirm_rx, cb->src_node, cb->src_port, + cb->dst_node, cb->dst_port, + (unsigned int)pl_buf, (unsigned int)(pl_buf >> 32)); + } else { + skb_copy_bits(skb, 0, &pkt, sizeof(pkt)); + if (cb->type == QRTR_TYPE_NEW_SERVER || + cb->type == QRTR_TYPE_DEL_SERVER) + QRTR_INFO(node->ilc, + "RX CTRL: cmd:0x%x SVC[0x%x:0x%x] addr[0x%x:0x%x]\n", + cb->type, le32_to_cpu(pkt.server.service), + le32_to_cpu(pkt.server.instance), + le32_to_cpu(pkt.server.node), + le32_to_cpu(pkt.server.port)); + else if (cb->type == QRTR_TYPE_DEL_CLIENT || + cb->type == QRTR_TYPE_RESUME_TX) + QRTR_INFO(node->ilc, + "RX CTRL: cmd:0x%x addr[0x%x:0x%x]\n", + cb->type, le32_to_cpu(pkt.client.node), + le32_to_cpu(pkt.client.port)); + else if (cb->type == QRTR_TYPE_HELLO || + cb->type == QRTR_TYPE_BYE) + QRTR_INFO(node->ilc, + "RX CTRL: cmd:0x%x node[0x%x]\n", + cb->type, cb->src_node); + } +} + +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) { @@ -460,6 +615,7 @@ static int qrtr_node_enqueue(struct qrtr_node *node, struct sk_buff *skb, hdr->size = cpu_to_le32(len); hdr->confirm_rx = !!confirm_rx; + qrtr_log_tx_msg(node, hdr, skb); rc = skb_put_padto(skb, ALIGN(len, 4) + sizeof(*hdr)); if (rc) pr_err("%s: failed to pad size %lu to %lu rc:%d\n", __func__, @@ -730,6 +886,7 @@ int qrtr_endpoint_post(struct qrtr_endpoint *ep, const void *data, size_t len) qrtr_node_assign(node, le32_to_cpu(pkt->server.node)); } + qrtr_log_rx_msg(node, skb); /* All control packets and non-local destined data packets should be * queued to the worker for forwarding handling. */ @@ -891,6 +1048,12 @@ static void qrtr_node_rx_work(struct kthread_work *work) struct qrtr_node *node = container_of(work, struct qrtr_node, read_data); struct sk_buff *skb; + char name[32] = {0,}; + + if (unlikely(!node->ilc)) { + snprintf(name, sizeof(name), "qrtr_%d", node->nid); + node->ilc = ipc_log_context_create(QRTR_LOG_PAGE_CNT, name, 0); + } while ((skb = skb_dequeue(&node->rx_queue)) != NULL) { struct qrtr_cb *cb = (struct qrtr_cb *)skb->cb; @@ -1006,6 +1169,60 @@ int qrtr_endpoint_register(struct qrtr_endpoint *ep, unsigned int net_id, } EXPORT_SYMBOL_GPL(qrtr_endpoint_register); +static u32 qrtr_calc_checksum(struct qrtr_ctrl_pkt *pkt) +{ + u32 checksum = 0; + u32 mask = 0xffff; + u16 upper_nb; + u16 lower_nb; + u32 *msg; + int i; + + if (!pkt) + return checksum; + msg = (u32 *)pkt; + + for (i = 0; i < sizeof(*pkt) / sizeof(*msg); i++) { + lower_nb = *msg & mask; + upper_nb = (*msg >> 16) & mask; + checksum += (upper_nb + lower_nb); + msg++; + } + while (checksum > 0xffff) + checksum = (checksum & mask) + ((checksum >> 16) & mask); + + checksum = ~checksum & mask; + + return checksum; +} + +static void qrtr_fwd_del_proc(struct qrtr_node *src, unsigned int nid) +{ + struct sockaddr_qrtr from = {AF_QIPCRTR, 0, QRTR_PORT_CTRL}; + struct sockaddr_qrtr to = {AF_QIPCRTR, 0, QRTR_PORT_CTRL}; + struct qrtr_ctrl_pkt *pkt; + struct qrtr_node *dst; + struct sk_buff *skb; + + list_for_each_entry(dst, &qrtr_all_epts, item) { + if (!qrtr_must_forward(src, dst, QRTR_TYPE_DEL_PROC)) + continue; + + skb = qrtr_alloc_ctrl_packet(&pkt, GFP_KERNEL); + if (!skb) + return; + + pkt->cmd = cpu_to_le32(QRTR_TYPE_DEL_PROC); + pkt->proc.rsvd = QRTR_DEL_PROC_MAGIC; + pkt->proc.node = cpu_to_le32(nid); + pkt->proc.rsvd = cpu_to_le32(qrtr_calc_checksum(pkt)); + + from.sq_node = src->nid; + to.sq_node = dst->nid; + qrtr_node_enqueue(dst, skb, QRTR_TYPE_DEL_PROC, &from, &to); + } +} + /** * qrtr_endpoint_unregister - unregister endpoint * @ep: endpoint to unregister @@ -1037,6 +1254,10 @@ void qrtr_endpoint_unregister(struct qrtr_endpoint *ep) pkt->cmd = cpu_to_le32(QRTR_TYPE_BYE); qrtr_local_enqueue(NULL, skb, QRTR_TYPE_BYE, &src, &dst); } + + spin_unlock_irqrestore(&qrtr_nodes_lock, flags); + qrtr_fwd_del_proc(node, iter.index); + spin_lock_irqsave(&qrtr_nodes_lock, flags); } spin_unlock_irqrestore(&qrtr_nodes_lock, flags); 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 ddf0f11babcf..f0ee970a8546 100644 --- a/net/qrtr/qrtr.h +++ b/net/qrtr/qrtr.h @@ -10,6 +10,8 @@ struct sk_buff; #define QRTR_EP_NID_AUTO (-1) #define QRTR_EP_NET_ID_AUTO (1) +#define QRTR_DEL_PROC_MAGIC 0xe111 + /** * struct qrtr_endpoint - endpoint handle * @xmit: Callback for outgoing packets @@ -36,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; }