[WIP]: net: qrtr: Add support for packet forwarding

Signed-off-by: Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>

1 files changed, 135 insertions, 28 deletions

diff --git a/net/qrtr/qrtr.c b/net/qrtr/qrtr.c
index 57fc0e7c1c5c..384d6e3eca07 100644
--- a/net/qrtr/qrtr.c
+++ b/net/qrtr/qrtr.c
@@ -132,6 +132,7 @@ struct qrtr_node {
 
 	struct sk_buff_head rx_queue;
 	struct list_head item;
+	struct work_struct read_data;
 	struct work_struct say_hello;
 	struct workqueue_struct *wq;
 	struct task_struct *task;
@@ -438,6 +439,137 @@ static void qrtr_node_assign(struct qrtr_node *node, unsigned int nid)
 	spin_unlock_irqrestore(&qrtr_nodes_lock, flags);
 }
 
+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->nid - src->nid) > 1)
+		return true;
+
+	return false;
+}
+
+static void qrtr_fwd_ctrl_pkt(struct sk_buff *skb)
+{
+	struct qrtr_cb *cb = (struct qrtr_cb *)skb->cb;
+	struct radix_tree_iter iter;
+	struct qrtr_node *node;
+	struct qrtr_node *src;
+	void __rcu **slot;
+
+	mutex_lock(&qrtr_node_lock);
+	src = qrtr_node_lookup(cb->src_node);
+	radix_tree_for_each_slot(slot, &qrtr_nodes, &iter, 0) {
+		node = *slot;
+		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);
+	}
+	mutex_unlock(&qrtr_node_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);
+	qrtr_node_release(node);
+}
+
+/* Handle and route a received packet.
+ *
+ * This will auto-reply with resume-tx packet as necessary.
+ */
+static void qrtr_node_rx_work(struct work_struct *work)
+{
+	struct qrtr_node *node = container_of(work, struct qrtr_node,
+					      read_data);
+	struct qrtr_ctrl_pkt *pkt;
+	struct sk_buff *skb;
+
+	while ((skb = skb_dequeue(&node->rx_queue)) != NULL) {
+		struct qrtr_sock *ipc;
+		struct qrtr_cb *cb;
+
+		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) {
+			/* Remote node endpoint can bridge other distant nodes */
+			pkt = (struct qrtr_ctrl_pkt *)skb->data;
+			qrtr_node_assign(node, le32_to_cpu(pkt->server.node));
+		}
+
+		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) {
+				kfree_skb(skb);
+				return;
+			}
+
+			if (sock_queue_rcv_skb(&ipc->sk, skb)) {
+				qrtr_port_put(ipc);
+				kfree_skb(skb);
+			}
+
+			qrtr_port_put(ipc);
+		}
+	}
+}
+
 /**
  * qrtr_endpoint_post() - post incoming data
  * @ep: endpoint handle
@@ -451,7 +583,6 @@ int qrtr_endpoint_post(struct qrtr_endpoint *ep, const void *data, size_t len)
 	struct qrtr_node *node = ep->node;
 	const struct qrtr_hdr_v1 *v1;
 	const struct qrtr_hdr_v2 *v2;
-	struct qrtr_sock *ipc;
 	struct sk_buff *skb;
 	struct qrtr_cb *cb;
 	unsigned int size;
@@ -520,33 +651,8 @@ int qrtr_endpoint_post(struct qrtr_endpoint *ep, const void *data, size_t len)
 
 	skb_put_data(skb, data + hdrlen, size);
 
-	qrtr_node_assign(node, cb->src_node);
-
-	if (cb->type == QRTR_TYPE_NEW_SERVER) {
-		/* Remote node endpoint can bridge other distant nodes */
-		const struct qrtr_ctrl_pkt *pkt;
-
-		if (size < sizeof(*pkt))
-			goto err;
-
-		pkt = data + hdrlen;
-		qrtr_node_assign(node, le32_to_cpu(pkt->server.node));
-	}
-
-	if (cb->type == QRTR_TYPE_RESUME_TX) {
-		qrtr_tx_resume(node, skb);
-	} else {
-		ipc = qrtr_port_lookup(cb->dst_port);
-		if (!ipc)
-			goto err;
-
-		if (sock_queue_rcv_skb(&ipc->sk, skb)) {
-			qrtr_port_put(ipc);
-			goto err;
-		}
-
-		qrtr_port_put(ipc);
-	}
+	skb_queue_tail(&node->rx_queue, skb);
+	queue_work(node->wq, &node->read_data);
 
 	return 0;
 
@@ -635,6 +741,7 @@ int qrtr_endpoint_register(struct qrtr_endpoint *ep, unsigned int nid)
 	node->nid = QRTR_EP_NID_AUTO;
 	node->ep = ep;
 
+	INIT_WORK(&node->read_data, qrtr_node_rx_work);
 	INIT_WORK(&node->say_hello, qrtr_hello_work);
 	node->wq = alloc_ordered_workqueue("qrtr_wq", 0);
 	if (!node->wq)