From e5f6a6769ceb8a30d1680303994be85edb7c2dd5 Mon Sep 17 00:00:00 2001 From: Chris Lew Date: Thu, 6 Dec 2018 14:24:21 -0800 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. Change-Id: Ia6a1e952430f7d63bf361bc3f2427d492e982d96 Signed-off-by: Chris Lew --- net/qrtr/mhi.c | 8 +++- net/qrtr/qrtr.c | 119 +++++++++++++++++++++++++++++++++++++++++++++--- net/qrtr/qrtr.h | 3 +- net/qrtr/smd.c | 13 ++++-- 4 files changed, 130 insertions(+), 13 deletions(-) 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;