Merge "net: qrtr: Add forwarding support based on net id" (3ccbdc61) · Commits · e / devices / android_kernel_fairphone_FP5

net/qrtr/qrtr.c

+136 −14

Original line number	Diff line number	Diff line
		@@ -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,25 +447,30 @@ static int qrtr_node_enqueue(struct qrtr_node node, struct sk_buff skb,
		return rc;
		}

		/* 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));
		hdr->version = cpu_to_le32(QRTR_PROTO_VER_1);
		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;

net/qrtr/qrtr.h

+2 −1

Original line number	Diff line number	Diff line
		@@ -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);

net/qrtr/smd.c

+8 −2

Original line number	Diff line number	Diff line
		// 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 <linux/module.h>
		#include <linux/skbuff.h>
		#include <linux/rpmsg.h>
		#include <linux/of.h>

		#include "qrtr.h"

		@@ -59,6 +60,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);
		@@ -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;

net/qrtr/tun.c

+1 −1

Original line number	Diff line number	Diff line
		@@ -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)