From 188c64d2603e5d320afc063f266be4c1903568c3 Mon Sep 17 00:00:00 2001 From: Chris Lew Date: Tue, 11 Oct 2022 13:34:35 -0700 Subject: [PATCH] net: qrtr: Add forwarding support based on net id Add support for QRTR to forward messages between network clusters. The network subnet id's are attached to the QRTR subnodes in the MHI and RPMSG/GLINK nodes. The current forwarding decisions are done based on the net ids and are wrapped in a single function for easier maintenance of forwarding decisions later on. The function will return true once it determines a subnet has no connection to another subnet. The NEW_SERVER, DEL_SERVER, and DEL_CLIENT control messages should be forwarded while the DATA and RESUME_TX commands should be passed along to their destination node. The nameservice is expected to send NEW_SERVER commands for it's entire database instead of just the local service database with these changes. This is to make sure new nodes get service notifications that came earlier and were meant to be forwarded to the new node. This change squashes the following commits from msm-4.14: commit e5f6a6769ceb ("net: qrtr: Add forwarding support based on net id") commit 4516ab18718d ("net: qrtr: avoid deadlock in case of recursive lock for node list") commit dc2d47cd12d3 ("net: qrtr: Only update pkts with broadcast node ID") commit 4277aebeff91 ("net: qrtr: Free skb if qrtr_node_lookup fails during qrtr_fwd_pkt") In addition fix minor format issues and update copyright. Change-Id: Iaf01dd212b07dfbb13ec087ea0aae980cda7c2fd Signed-off-by: Chris Lew --- net/qrtr/af_qrtr.c | 140 ++++++++++++++++++++++++++++++++++++++++++--- net/qrtr/mhi.c | 36 +++++++++++- net/qrtr/qrtr.h | 3 +- net/qrtr/smd.c | 9 ++- net/qrtr/tun.c | 2 +- 5 files changed, 177 insertions(+), 13 deletions(-) diff --git a/net/qrtr/af_qrtr.c b/net/qrtr/af_qrtr.c index 9d47c1b6b017..c6455ce5364f 100644 --- a/net/qrtr/af_qrtr.c +++ b/net/qrtr/af_qrtr.c @@ -132,6 +132,7 @@ static DEFINE_SPINLOCK(qrtr_port_lock); * @ep: endpoint * @ref: reference count for node * @nid: node id + * @net_id: network cluster identifer * @qrtr_tx_flow: tree of qrtr_tx_flow, keyed by node << 32 | port * @qrtr_tx_lock: lock for qrtr_tx_flow inserts * @hello_sent: hello packet sent to endpoint @@ -146,6 +147,7 @@ struct qrtr_node { struct qrtr_endpoint *ep; struct kref ref; unsigned int nid; + unsigned int net_id; atomic_t hello_sent; struct radix_tree_root qrtr_tx_flow; @@ -405,10 +407,17 @@ static int qrtr_node_enqueue(struct qrtr_node *node, struct sk_buff *skb, return 0; } - confirm_rx = qrtr_tx_wait(node, to->sq_node, to->sq_port, type); - if (confirm_rx < 0) { - kfree_skb(skb); - return confirm_rx; + /* If sk is null, this is a forwarded packet and should not wait */ + if (!skb->sk) { + struct qrtr_cb *cb = (struct qrtr_cb *)skb->cb; + + confirm_rx = cb->confirm_rx; + } else { + confirm_rx = qrtr_tx_wait(node, to->sq_node, to->sq_port, type); + if (confirm_rx < 0) { + kfree_skb(skb); + return confirm_rx; + } } hdr = skb_push(skb, sizeof(*hdr)); @@ -630,7 +639,10 @@ int qrtr_endpoint_post(struct qrtr_endpoint *ep, const void *data, size_t len) qrtr_node_assign(node, le32_to_cpu(pkt->server.node)); } - if (cb->type == QRTR_TYPE_RESUME_TX) { + /* All control packets and non-local destined data packets should be + * queued to the worker for forwarding handling. + */ + if (cb->type != QRTR_TYPE_DATA || cb->dst_node != qrtr_local_nid) { skb_queue_tail(&node->rx_queue, skb); kthread_queue_work(&node->kworker, &node->read_data); } else { @@ -683,6 +695,77 @@ static struct sk_buff *qrtr_alloc_ctrl_packet(struct qrtr_ctrl_pkt **pkt, return skb; } +static bool qrtr_must_forward(struct qrtr_node *src, + struct qrtr_node *dst, u32 type) +{ + /* Node structure is not maintained for local processor. + * Hence src is null in that case. + */ + if (!src) + return true; + + if (!dst) + return false; + + if (type == QRTR_TYPE_HELLO || type == QRTR_TYPE_RESUME_TX) + return false; + + if (dst == src || dst->nid == QRTR_EP_NID_AUTO) + return false; + + if (abs(dst->net_id - src->net_id) > 1) + return true; + + return false; +} + +static void qrtr_fwd_ctrl_pkt(struct qrtr_node *src, struct sk_buff *skb) +{ + struct qrtr_node *node; + struct qrtr_cb *cb = (struct qrtr_cb *)skb->cb; + + down_read(&qrtr_epts_lock); + list_for_each_entry(node, &qrtr_all_epts, item) { + struct sockaddr_qrtr from; + struct sockaddr_qrtr to; + struct sk_buff *skbn; + + if (!qrtr_must_forward(src, node, cb->type)) + continue; + + skbn = skb_clone(skb, GFP_KERNEL); + if (!skbn) + break; + + from.sq_family = AF_QIPCRTR; + from.sq_node = cb->src_node; + from.sq_port = cb->src_port; + + to.sq_family = AF_QIPCRTR; + to.sq_node = node->nid; + to.sq_port = QRTR_PORT_CTRL; + + qrtr_node_enqueue(node, skbn, cb->type, &from, &to); + } + up_read(&qrtr_epts_lock); +} + +static void qrtr_fwd_pkt(struct sk_buff *skb, struct qrtr_cb *cb) +{ + struct sockaddr_qrtr from = {AF_QIPCRTR, cb->src_node, cb->src_port}; + struct sockaddr_qrtr to = {AF_QIPCRTR, cb->dst_node, cb->dst_port}; + struct qrtr_node *node; + + node = qrtr_node_lookup(cb->dst_node); + if (!node) { + kfree_skb(skb); + return; + } + + qrtr_node_enqueue(node, skb, cb->type, &from, &to); + qrtr_node_release(node); +} + /* Handle not atomic operations for a received packet. */ static void qrtr_node_rx_work(struct kthread_work *work) { @@ -692,9 +775,31 @@ static void qrtr_node_rx_work(struct kthread_work *work) while ((skb = skb_dequeue(&node->rx_queue)) != NULL) { struct qrtr_cb *cb = (struct qrtr_cb *)skb->cb; + struct qrtr_sock *ipc; - if (cb->type == QRTR_TYPE_RESUME_TX) + if (cb->type != QRTR_TYPE_DATA) + qrtr_fwd_ctrl_pkt(node, skb); + + if (cb->type == QRTR_TYPE_RESUME_TX) { + if (cb->dst_node != qrtr_local_nid) { + qrtr_fwd_pkt(skb, cb); + continue; + } qrtr_tx_resume(node, skb); + } else if (cb->dst_node != qrtr_local_nid && + cb->type == QRTR_TYPE_DATA) { + qrtr_fwd_pkt(skb, cb); + } else { + ipc = qrtr_port_lookup(cb->dst_port); + if (!ipc) { + kfree_skb(skb); + } else { + if (sock_queue_rcv_skb(&ipc->sk, skb)) + kfree_skb(skb); + + qrtr_port_put(ipc); + } + } } } @@ -706,7 +811,7 @@ static void qrtr_node_rx_work(struct kthread_work *work) * * The specified endpoint must have the xmit function pointer set on call. */ -int qrtr_endpoint_register(struct qrtr_endpoint *ep, unsigned int nid) +int qrtr_endpoint_register(struct qrtr_endpoint *ep, unsigned int net_id) { struct qrtr_node *node; @@ -735,7 +840,8 @@ int qrtr_endpoint_register(struct qrtr_endpoint *ep, unsigned int nid) INIT_RADIX_TREE(&node->qrtr_tx_flow, GFP_KERNEL); mutex_init(&node->qrtr_tx_lock); - qrtr_node_assign(node, nid); + qrtr_node_assign(node, node->nid); + node->net_id = net_id; down_write(&qrtr_epts_lock); list_add(&node->item, &qrtr_all_epts); @@ -1094,7 +1200,9 @@ static int qrtr_sendmsg(struct socket *sock, struct msghdr *msg, size_t len) __le32 qrtr_type = cpu_to_le32(QRTR_TYPE_DATA); struct qrtr_sock *ipc = qrtr_sk(sock->sk); struct sock *sk = sock->sk; + struct qrtr_ctrl_pkt pkt; struct qrtr_node *node; + struct qrtr_node *srv_node; struct sk_buff *skb; size_t plen; u32 type; @@ -1132,6 +1240,7 @@ static int qrtr_sendmsg(struct socket *sock, struct msghdr *msg, size_t len) } node = NULL; + srv_node = NULL; if (addr->sq_node == QRTR_NODE_BCAST) { if (addr->sq_port != QRTR_PORT_CTRL && qrtr_local_nid != QRTR_NODE_BCAST) { @@ -1184,10 +1293,23 @@ static int qrtr_sendmsg(struct socket *sock, struct msghdr *msg, size_t len) } type = le32_to_cpu(qrtr_type); - if (addr->sq_port == QRTR_PORT_CTRL && type == QRTR_TYPE_NEW_SERVER) + if (addr->sq_port == QRTR_PORT_CTRL && type == QRTR_TYPE_NEW_SERVER) { ipc->state = QRTR_STATE_MULTI; + /* drop new server cmds that are not forwardable to dst node*/ + skb_copy_bits(skb, 0, &pkt, sizeof(pkt)); + srv_node = qrtr_node_lookup(pkt.server.node); + if (!qrtr_must_forward(srv_node, node, type)) { + rc = 0; + kfree_skb(skb); + qrtr_node_release(srv_node); + goto out_node; + } + qrtr_node_release(srv_node); + } + rc = enqueue_fn(node, skb, type, &ipc->us, addr); + if (rc >= 0) rc = len; diff --git a/net/qrtr/mhi.c b/net/qrtr/mhi.c index 142e675eb496..e722967fea77 100644 --- a/net/qrtr/mhi.c +++ b/net/qrtr/mhi.c @@ -7,7 +7,10 @@ #include #include #include +#include +#include #include +#include #include #include "qrtr.h" @@ -78,10 +81,38 @@ static int qcom_mhi_qrtr_send(struct qrtr_endpoint *ep, struct sk_buff *skb) return rc; } +static void qrtr_mhi_of_parse(struct mhi_device *mhi_dev, + u32 *net_id) +{ + struct mhi_controller *mhi_cntrl = mhi_dev->mhi_cntrl; + struct device_node *np = NULL; + struct pci_dev *pci_device; + u32 dev_id, nid; + int rc; + + *net_id = QRTR_EP_NET_ID_AUTO; + + np = of_find_compatible_node(np, NULL, "qcom,qrtr-mhi"); + if (!np) + return; + + rc = of_property_read_u32(np, "qcom,dev-id", &dev_id); + if (!rc) { + pci_device = to_pci_dev(mhi_cntrl->cntrl_dev); + if (pci_device->device == dev_id) { + rc = of_property_read_u32(np, "qcom,net-id", &nid); + if (!rc) + *net_id = nid; + } + } + of_node_put(np); +} + static int qcom_mhi_qrtr_probe(struct mhi_device *mhi_dev, const struct mhi_device_id *id) { struct qrtr_mhi_dev *qdev; + u32 net_id; int rc; qdev = devm_kzalloc(&mhi_dev->dev, sizeof(*qdev), GFP_KERNEL); @@ -94,7 +125,10 @@ static int qcom_mhi_qrtr_probe(struct mhi_device *mhi_dev, init_completion(&qdev->prepared); dev_set_drvdata(&mhi_dev->dev, qdev); - rc = qrtr_endpoint_register(&qdev->ep, QRTR_EP_NID_AUTO); + + qrtr_mhi_of_parse(mhi_dev, &net_id); + + rc = qrtr_endpoint_register(&qdev->ep, net_id); if (rc) return rc; diff --git a/net/qrtr/qrtr.h b/net/qrtr/qrtr.h index 2f7bd02b21ae..322a46498e40 100644 --- a/net/qrtr/qrtr.h +++ b/net/qrtr/qrtr.h @@ -8,6 +8,7 @@ struct sk_buff; /* endpoint node id auto assignment */ #define QRTR_EP_NID_AUTO (-1) +#define QRTR_EP_NET_ID_AUTO (1) /** * struct qrtr_endpoint - endpoint handle @@ -23,7 +24,7 @@ struct qrtr_endpoint { struct qrtr_node *node; }; -int qrtr_endpoint_register(struct qrtr_endpoint *ep, unsigned int nid); +int qrtr_endpoint_register(struct qrtr_endpoint *ep, unsigned int net_id); void qrtr_endpoint_unregister(struct qrtr_endpoint *ep); diff --git a/net/qrtr/smd.c b/net/qrtr/smd.c index 9c7d6085a1dc..46865b66e7ec 100644 --- a/net/qrtr/smd.c +++ b/net/qrtr/smd.c @@ -2,11 +2,13 @@ /* * Copyright (c) 2015, Sony Mobile Communications Inc. * Copyright (c) 2013, The Linux Foundation. All rights reserved. + * Copyright (c) 2022 Qualcomm Innovation Center, Inc. All rights reserved. */ #include #include #include +#include #include "qrtr.h" @@ -61,6 +63,7 @@ static int qcom_smd_qrtr_send(struct qrtr_endpoint *ep, struct sk_buff *skb) static int qcom_smd_qrtr_probe(struct rpmsg_device *rpdev) { struct qrtr_smd_dev *qdev; + u32 net_id; int rc; qdev = devm_kzalloc(&rpdev->dev, sizeof(*qdev), GFP_KERNEL); @@ -71,7 +74,11 @@ static int qcom_smd_qrtr_probe(struct rpmsg_device *rpdev) qdev->dev = &rpdev->dev; qdev->ep.xmit = qcom_smd_qrtr_send; - rc = qrtr_endpoint_register(&qdev->ep, QRTR_EP_NID_AUTO); + rc = of_property_read_u32(rpdev->dev.of_node, "qcom,net-id", &net_id); + if (rc < 0) + net_id = QRTR_EP_NET_ID_AUTO; + + rc = qrtr_endpoint_register(&qdev->ep, net_id); if (rc) { dev_err(qdev->dev, "endpoint register failed: %d\n", rc); return rc; diff --git a/net/qrtr/tun.c b/net/qrtr/tun.c index 304b41fea5ab..6e8524e10c67 100644 --- a/net/qrtr/tun.c +++ b/net/qrtr/tun.c @@ -44,7 +44,7 @@ static int qrtr_tun_open(struct inode *inode, struct file *filp) filp->private_data = tun; - ret = qrtr_endpoint_register(&tun->ep, QRTR_EP_NID_AUTO); + ret = qrtr_endpoint_register(&tun->ep, QRTR_EP_NET_ID_AUTO); if (ret) goto out;