diff --git a/net/qrtr/qrtr.c b/net/qrtr/qrtr.c index 455cb29d14f8..4b39d0a70231 100644 --- a/net/qrtr/qrtr.c +++ b/net/qrtr/qrtr.c @@ -123,6 +123,7 @@ static DEFINE_MUTEX(qrtr_port_lock); * @ep: endpoint * @ref: reference count for node * @nid: node id + * @net_id: network cluster identifer * @hello_sent: hello packet sent to endpoint * @qrtr_tx_flow: tree with tx counts per flow * @resume_tx: waiters for a resume tx from the remote @@ -136,6 +137,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; @@ -445,10 +447,17 @@ static int qrtr_node_enqueue(struct qrtr_node *node, struct sk_buff *skb, return rc; } - confirm_rx = qrtr_tx_wait(node, to, skb->sk, type, flags); - 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, skb->sk, type, flags); + if (confirm_rx < 0) { + kfree_skb(skb); + return confirm_rx; + } } hdr = skb_push(skb, sizeof(*hdr)); @@ -456,14 +465,12 @@ static int qrtr_node_enqueue(struct qrtr_node *node, struct sk_buff *skb, hdr->type = cpu_to_le32(type); hdr->src_node_id = cpu_to_le32(from->sq_node); hdr->src_port_id = cpu_to_le32(from->sq_port); - if (to->sq_port == QRTR_PORT_CTRL) { + if (to->sq_node == QRTR_NODE_BCAST) hdr->dst_node_id = cpu_to_le32(node->nid); - hdr->dst_port_id = cpu_to_le32(QRTR_NODE_BCAST); - } else { + else hdr->dst_node_id = cpu_to_le32(to->sq_node); - hdr->dst_port_id = cpu_to_le32(to->sq_port); - } + hdr->dst_port_id = cpu_to_le32(to->sq_port); hdr->size = cpu_to_le32(len); hdr->confirm_rx = !!confirm_rx; @@ -613,7 +620,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); schedule_work(&node->work); } else { @@ -660,6 +670,80 @@ 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 sk_buff *skb) +{ + struct qrtr_node *node; + struct qrtr_node *src; + struct qrtr_cb *cb = (struct qrtr_cb *)skb->cb; + + src = qrtr_node_lookup(cb->src_node); + 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, 0); + } + up_read(&qrtr_epts_lock); + qrtr_node_release(src); +} + +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, 0); + qrtr_node_release(node); +} + /* Handle not atomic operations for a received packet. */ static void qrtr_node_rx_work(struct work_struct *work) { @@ -668,9 +752,31 @@ static void qrtr_node_rx_work(struct work_struct *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(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); + } + } } } @@ -682,7 +788,7 @@ static void qrtr_node_rx_work(struct work_struct *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; @@ -705,7 +811,8 @@ int qrtr_endpoint_register(struct qrtr_endpoint *ep, unsigned int nid) INIT_RADIX_TREE(&node->qrtr_tx_flow, GFP_KERNEL); init_waitqueue_head(&node->resume_tx); - 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); @@ -1068,7 +1175,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; @@ -1106,6 +1215,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) { enqueue_fn = qrtr_bcast_enqueue; if (addr->sq_port != QRTR_PORT_CTRL) { @@ -1155,9 +1265,21 @@ 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, msg->msg_flags); if (rc >= 0) rc = len; diff --git a/net/qrtr/qrtr.h b/net/qrtr/qrtr.h index b81e6953c04b..fc6dcb368419 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 c91bf030fbc7..bf0bed8a8cf7 100644 --- a/net/qrtr/smd.c +++ b/net/qrtr/smd.c @@ -1,12 +1,13 @@ // SPDX-License-Identifier: GPL-2.0-only /* * Copyright (c) 2015, Sony Mobile Communications Inc. - * Copyright (c) 2013, The Linux Foundation. All rights reserved. + * Copyright (c) 2013, 2018 The Linux Foundation. All rights reserved. */ #include #include #include +#include #include "qrtr.h" @@ -59,6 +60,7 @@ out: 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); @@ -69,7 +71,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) return rc; diff --git a/net/qrtr/tun.c b/net/qrtr/tun.c index e35869e81766..39fed2056617 100644 --- a/net/qrtr/tun.c +++ b/net/qrtr/tun.c @@ -43,7 +43,7 @@ static int qrtr_tun_open(struct inode *inode, struct file *filp) filp->private_data = tun; - return qrtr_endpoint_register(&tun->ep, QRTR_EP_NID_AUTO); + return qrtr_endpoint_register(&tun->ep, QRTR_EP_NET_ID_AUTO); } static ssize_t qrtr_tun_read_iter(struct kiocb *iocb, struct iov_iter *to)