diff --git a/net/qrtr/mhi.c b/net/qrtr/mhi.c index 85a8a1a99944..e7e4e420967b 100644 --- a/net/qrtr/mhi.c +++ b/net/qrtr/mhi.c @@ -15,6 +15,7 @@ #include #include #include +#include #include "qrtr.h" @@ -130,6 +131,7 @@ 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); @@ -140,10 +142,14 @@ static int qcom_mhi_qrtr_probe(struct mhi_device *mhi_dev, qdev->dev = &mhi_dev->dev; qdev->ep.xmit = qcom_mhi_qrtr_send; + rc = of_property_read_u32(mhi_dev->dev.of_node, "qcom,net-id", &net_id); + if (rc < 0) + net_id = QRTR_EP_NET_ID_AUTO; + INIT_LIST_HEAD(&qdev->ul_pkts); spin_lock_init(&qdev->ul_lock); - rc = qrtr_endpoint_register(&qdev->ep, QRTR_EP_NID_AUTO); + rc = qrtr_endpoint_register(&qdev->ep, net_id); if (rc) return rc; diff --git a/net/qrtr/qrtr.c b/net/qrtr/qrtr.c index e6e350bb73f0..221a0aad6787 100644 --- a/net/qrtr/qrtr.c +++ b/net/qrtr/qrtr.c @@ -139,6 +139,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: remote port tx flow control list * @resume_tx: wait until remote port acks control flag @@ -155,6 +156,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; @@ -503,10 +505,17 @@ static int qrtr_node_enqueue(struct qrtr_node *node, struct sk_buff *skb, if (!atomic_read(&node->hello_sent) && type != QRTR_TYPE_HELLO) 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)); @@ -764,6 +773,81 @@ static struct sk_buff *qrtr_alloc_ctrl_packet(struct qrtr_ctrl_pkt **pkt) static struct qrtr_sock *qrtr_port_lookup(int port); static void qrtr_port_put(struct qrtr_sock *ipc); +static bool qrtr_must_forward(u32 src_nid, u32 dst_nid, u32 type) +{ + struct qrtr_node *dst; + struct qrtr_node *src; + bool ret = false; + + if (src_nid == qrtr_local_nid) + return true; + + if (type == QRTR_TYPE_HELLO || type == QRTR_TYPE_RESUME_TX) + return ret; + + dst = qrtr_node_lookup(dst_nid); + src = qrtr_node_lookup(src_nid); + if (!dst || !src) + goto out; + if (dst == src) + goto out; + if (dst->nid == QRTR_EP_NID_AUTO) + goto out; + + if (abs(dst->net_id - src->net_id) > 1) + ret = true; + +out: + qrtr_node_release(dst); + qrtr_node_release(src); + + return ret; +} + +static void qrtr_fwd_ctrl_pkt(struct sk_buff *skb) +{ + struct qrtr_node *node; + struct qrtr_cb *cb = (struct qrtr_cb *)skb->cb; + + down_read(&qrtr_node_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(cb->src_node, node->nid, 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_node_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) + return; + + qrtr_node_enqueue(node, skb, cb->type, &from, &to, 0); + qrtr_node_release(node); +} /* Handle and route a received packet. * * This will auto-reply with resume-tx packet as necessary. @@ -782,6 +866,9 @@ static void qrtr_node_rx_work(struct kthread_work *work) cb = (struct qrtr_cb *)skb->cb; qrtr_node_assign(node, cb->src_node); + if (cb->type != QRTR_TYPE_DATA) + qrtr_fwd_ctrl_pkt(skb); + if (cb->type == QRTR_TYPE_NEW_SERVER && skb->len == sizeof(*pkt)) { pkt = (void *)skb->data; @@ -789,8 +876,15 @@ static void qrtr_node_rx_work(struct kthread_work *work) } 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); consume_skb(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) { @@ -813,7 +907,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; @@ -843,7 +937,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_node_lock); list_add(&node->item, &qrtr_all_epts); @@ -1195,6 +1290,7 @@ static int qrtr_sendmsg(struct socket *sock, struct msghdr *msg, size_t len) unsigned int); struct qrtr_sock *ipc = qrtr_sk(sock->sk); struct sock *sk = sock->sk; + struct qrtr_ctrl_pkt *pkt; struct qrtr_node *node; struct sk_buff *skb; size_t plen; @@ -1281,9 +1377,18 @@ static int qrtr_sendmsg(struct socket *sock, struct msghdr *msg, size_t len) skb_copy_bits(skb, 0, &type, 4); type = le32_to_cpu(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*/ + pkt = (struct qrtr_ctrl_pkt *)skb->data; + if (!qrtr_must_forward(pkt->server.node, addr->sq_node, type)) { + rc = 0; + kfree_skb(skb); + goto out_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 5ddb9b3ffc17..5005b1c5b56d 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 9cf089b9754e..a655bc2f67bd 100644 --- a/net/qrtr/smd.c +++ b/net/qrtr/smd.c @@ -1,6 +1,5 @@ -/* - * Copyright (c) 2015, Sony Mobile Communications Inc. - * Copyright (c) 2013, The Linux Foundation. All rights reserved. +/* Copyright (c) 2015, Sony Mobile Communications Inc. + * Copyright (c) 2013, 2018-2019 The Linux Foundation. All rights reserved. * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License version 2 and @@ -15,6 +14,7 @@ #include #include #include +#include #include "qrtr.h" @@ -67,6 +67,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); @@ -77,7 +78,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;