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;