qrtr: Add FIFO based communication between VMs (7772a904) · Commits · e / devices / android_kernel_oneplus_sm7250

net/qrtr/Kconfig

+8 −0

Original line number	Diff line number	Diff line
		@@ -47,4 +47,12 @@ config QRTR_USB
		transport provides bulk endpoints to facilitate sending and receiving
		IPC Router data packets.

		config QRTR_FIFO
		tristate "FIFO IPC Router channels"
		help
		Say Y here to support FIFO based ipcrouter channels. FIFO Transport
		Layer enables IPC Router communication between two virtual machines.
		The shared memory between virtual machines will be allocated by the
		hypervisor and signal other VMs through virtualized interrupts.

		endif # QRTR

net/qrtr/Makefile

+3 −0

Original line number	Diff line number	Diff line
		@@ -9,3 +9,6 @@ qrtr-mhi-y := mhi.o

		obj-$(CONFIG_QRTR_USB) += qrtr-usb.o
		qrtr-usb-y := usb.o

		obj-$(CONFIG_QRTR_FIFO) += qrtr-fifo.o
		qrtr-fifo-y := fifo.o

net/qrtr/fifo.c

0 → 100644

+382 −0

Original line number	Diff line number	Diff line
		// SPDX-License-Identifier: GPL-2.0-only
		/* Copyright (c) 2018-2019, The Linux Foundation. All rights reserved.
		*/

		#define pr_fmt(fmt) "%s: " fmt, __func__

		#include <linux/delay.h>
		#include <linux/interrupt.h>
		#include <linux/io.h>
		#include <linux/of_device.h>
		#include <linux/module.h>
		#include <linux/platform_device.h>
		#include <linux/types.h>
		#include <linux/skbuff.h>
		#include <linux/sched.h>
		#include <microvisor/microvisor.h>

		#include "qrtr.h"

		#define FIFO_MAGIC_KEY 0x24495043 /* "$IPC" */
		#define FIFO_SIZE 0x4000
		#define FIFO_0_START 0x1000
		#define FIFO_1_START (FIFO_0_START + FIFO_SIZE)
		#define FIFO_MAGIC_IDX 0x0
		#define TAIL_0_IDX 0x1
		#define HEAD_0_IDX 0x2
		#define TAIL_1_IDX 0x3
		#define HEAD_1_IDX 0x4

		struct fifo_pipe {
		__le32 *tail;
		__le32 *head;

		void *fifo;
		size_t length;
		};

		/**
		* qrtr_fifo_xprt - qrtr FIFO transport structure
		* @ep: qrtr endpoint specific info.
		* @tx_pipe: TX FIFO specific info.
		* @rx_pipe: RX FIFO specific info.
		* @fifo_base: Base of the shared FIFO.
		* @fifo_size: FIFO Size.
		* @tx_fifo_idx: TX FIFO index.
		* @kcap: Register info to raise irq to other VM.
		*/
		struct qrtr_fifo_xprt {
		struct qrtr_endpoint ep;
		struct fifo_pipe tx_pipe;
		struct fifo_pipe rx_pipe;
		void *fifo_base;
		size_t fifo_size;
		int tx_fifo_idx;
		okl4_kcap_t kcap;
		};

		static void qrtr_fifo_raise_virq(struct qrtr_fifo_xprt *xprtp);

		static size_t fifo_rx_avail(struct fifo_pipe *pipe)
		{
		u32 head;
		u32 tail;

		head = le32_to_cpu(*pipe->head);
		tail = le32_to_cpu(*pipe->tail);

		if (head < tail)
		return pipe->length - tail + head;

		return head - tail;
		}

		static void fifo_rx_peak(struct fifo_pipe *pipe,
		void *data, unsigned int offset, size_t count)
		{
		size_t len;
		u32 tail;

		tail = le32_to_cpu(*pipe->tail);
		tail += offset;
		if (tail >= pipe->length)
		tail -= pipe->length;

		len = min_t(size_t, count, pipe->length - tail);
		if (len)
		memcpy_fromio(data, pipe->fifo + tail, len);

		if (len != count)
		memcpy_fromio(data + len, pipe->fifo, (count - len));
		}

		static void fifo_rx_advance(struct fifo_pipe *pipe, size_t count)
		{
		u32 tail;

		tail = le32_to_cpu(*pipe->tail);

		tail += count;
		if (tail > pipe->length)
		tail -= pipe->length;

		*pipe->tail = cpu_to_le32(tail);
		}

		static size_t fifo_tx_avail(struct fifo_pipe *pipe)
		{
		u32 head;
		u32 tail;
		u32 avail;

		head = le32_to_cpu(*pipe->head);
		tail = le32_to_cpu(*pipe->tail);

		if (tail <= head)
		avail = pipe->length - head + tail;
		else
		avail = tail - head;

		return avail;
		}

		static void fifo_tx_write(struct fifo_pipe *pipe,
		const void *data, size_t count)
		{
		size_t len;
		u32 head;

		head = le32_to_cpu(*pipe->head);

		len = min_t(size_t, count, pipe->length - head);
		if (len)
		memcpy_toio(pipe->fifo + head, data, len);

		if (len != count)
		memcpy_toio(pipe->fifo, data + len, count - len);

		head += count;
		if (head >= pipe->length)
		head -= pipe->length;

		/* Ensure ordering of fifo and head update */
		wmb();

		*pipe->head = cpu_to_le32(head);
		}

		/* from qrtr to FIFO */
		static int xprt_write(struct qrtr_endpoint ep, struct sk_buff skb)
		{
		struct qrtr_fifo_xprt *xprtp;
		int rc;

		xprtp = container_of(ep, struct qrtr_fifo_xprt, ep);

		rc = skb_linearize(skb);
		if (rc) {
		kfree_skb(skb);
		return rc;
		}

		if (fifo_tx_avail(&xprtp->tx_pipe) < skb->len) {
		pr_err("No Space in FIFO\n");
		return -EAGAIN;
		}

		fifo_tx_write(&xprtp->tx_pipe, skb->data, skb->len);
		kfree_skb(skb);

		qrtr_fifo_raise_virq(xprtp);

		return 0;
		}

		static void xprt_read_data(struct qrtr_fifo_xprt *xprtp)
		{
		int rc;
		u32 hdr[8];
		void *data;
		size_t pkt_len;
		size_t rx_avail;
		size_t hdr_len = sizeof(hdr);

		while (fifo_rx_avail(&xprtp->rx_pipe)) {
		fifo_rx_peak(&xprtp->rx_pipe, &hdr, 0, hdr_len);
		pkt_len = qrtr_peek_pkt_size((void *)&hdr);
		if ((int)pkt_len < 0) {
		pr_err("invalid pkt_len %zu\n", pkt_len);
		break;
		}

		data = kzalloc(pkt_len, GFP_ATOMIC);
		if (!data)
		break;

		rx_avail = fifo_rx_avail(&xprtp->rx_pipe);
		if (rx_avail < pkt_len) {
		pr_err_ratelimited("Not FULL pkt in FIFO %zu %zu\n",
		rx_avail, pkt_len);
		break;
		}

		fifo_rx_peak(&xprtp->rx_pipe, data, 0, pkt_len);
		fifo_rx_advance(&xprtp->rx_pipe, pkt_len);

		rc = qrtr_endpoint_post(&xprtp->ep, data, pkt_len);
		if (rc == -EINVAL)
		pr_err("invalid ipcrouter packet\n");
		kfree(data);
		data = NULL;
		}
		}

		static void qrtr_fifo_raise_virq(struct qrtr_fifo_xprt *xprtp)
		{
		okl4_error_t err;
		unsigned long payload = 0xffff;

		err = _okl4_sys_vinterrupt_raise(xprtp->kcap, payload);
		}

		static irqreturn_t qrtr_fifo_virq_handler(int irq, void *dev_id)
		{
		xprt_read_data((struct qrtr_fifo_xprt *)dev_id);
		return IRQ_HANDLED;
		}

		/**
		* qrtr_fifo_config_init() - init FIFO xprt configs
		*
		* @return: 0 on success, standard Linux error codes on error.
		*
		* This function is called to initialize the FIFO XPRT pointer with
		* the FIFO XPRT configurations either from device tree or static arrays.
		*/
		static void qrtr_fifo_config_init(struct qrtr_fifo_xprt *xprtp)
		{
		__le32 *descs;

		descs = xprtp->fifo_base;
		descs[FIFO_MAGIC_IDX] = FIFO_MAGIC_KEY;

		if (xprtp->tx_fifo_idx) {
		xprtp->tx_pipe.tail = &descs[TAIL_0_IDX];
		xprtp->tx_pipe.head = &descs[HEAD_0_IDX];
		xprtp->tx_pipe.fifo = xprtp->fifo_base + FIFO_0_START;
		xprtp->tx_pipe.length = FIFO_SIZE;

		xprtp->rx_pipe.tail = &descs[TAIL_1_IDX];
		xprtp->rx_pipe.head = &descs[HEAD_1_IDX];
		xprtp->rx_pipe.fifo = xprtp->fifo_base + FIFO_1_START;
		xprtp->rx_pipe.length = FIFO_SIZE;
		} else {
		xprtp->tx_pipe.tail = &descs[TAIL_1_IDX];
		xprtp->tx_pipe.head = &descs[HEAD_1_IDX];
		xprtp->tx_pipe.fifo = xprtp->fifo_base + FIFO_1_START;
		xprtp->tx_pipe.length = FIFO_SIZE;

		xprtp->rx_pipe.tail = &descs[TAIL_0_IDX];
		xprtp->rx_pipe.head = &descs[HEAD_0_IDX];
		xprtp->rx_pipe.fifo = xprtp->fifo_base + FIFO_0_START;
		xprtp->rx_pipe.length = FIFO_SIZE;
		}

		/* Reset respective index */
		*xprtp->tx_pipe.head = 0;
		*xprtp->rx_pipe.tail = 0;
		}

		/**
		* qrtr_fifo_xprt_probe() - Probe an FIFO xprt
		*
		* @pdev: Platform device corresponding to FIFO xprt.
		*
		* @return: 0 on success, standard Linux error codes on error.
		*
		* This function is called when the underlying device tree driver registers
		* a platform device, mapped to an FIFO transport.
		*/
		static int qrtr_fifo_xprt_probe(struct platform_device *pdev)
		{
		int irq;
		int ret;
		struct resource *r;
		struct device *parent;
		struct qrtr_fifo_xprt *xprtp;
		struct device_node *ipc_irq_np;
		struct device_node *ipc_shm_np;
		struct platform_device *ipc_shm_dev;

		xprtp = devm_kzalloc(&pdev->dev, sizeof(*xprtp), GFP_KERNEL);
		if (!xprtp)
		return -ENOMEM;

		parent = &pdev->dev;
		ipc_irq_np = parent->of_node;

		irq = platform_get_irq(pdev, 0);
		if (irq < 0)
		return -ENODEV;

		ret = devm_request_irq(parent, irq, qrtr_fifo_virq_handler,
		IRQF_TRIGGER_RISING, dev_name(parent),
		xprtp);
		if (ret < 0)
		return -ENODEV;

		/* this kcap is required to raise VIRQ */
		ret = of_property_read_u32(ipc_irq_np, "reg", &xprtp->kcap);
		if (ret < 0)
		return -ENODEV;

		ipc_shm_np = of_parse_phandle(ipc_irq_np, "qcom,ipc-shm", 0);
		if (!ipc_shm_np)
		return -ENODEV;

		ipc_shm_dev = of_find_device_by_node(ipc_shm_np);
		if (!ipc_shm_dev) {
		of_node_put(ipc_shm_np);
		return -ENODEV;
		}

		r = platform_get_resource(ipc_shm_dev, IORESOURCE_MEM, 0);
		if (!r) {
		pr_err("failed to get shared FIFO\n");
		of_node_put(ipc_shm_np);
		return -ENODEV;
		}

		xprtp->tx_fifo_idx = of_property_read_bool(ipc_shm_np,
		"qcom,tx-is-first");
		of_node_put(ipc_shm_np);

		xprtp->fifo_size = resource_size(r);
		xprtp->fifo_base = devm_ioremap_nocache(&pdev->dev, r->start,
		resource_size(r));
		if (!xprtp->fifo_base) {
		pr_err("ioreamp_nocache() failed\n");
		return -ENOMEM;
		}
		qrtr_fifo_config_init(xprtp);

		xprtp->ep.xmit = xprt_write;
		ret = qrtr_endpoint_register(&xprtp->ep, QRTR_EP_NID_AUTO);
		if (ret)
		return ret;

		if (fifo_rx_avail(&xprtp->rx_pipe))
		xprt_read_data(xprtp);

		return 0;
		}

		static const struct of_device_id qrtr_fifo_xprt_match_table[] = {
		{ .compatible = "qcom,ipcr-fifo-xprt" },
		{},
		};

		static struct platform_driver qrtr_fifo_xprt_driver = {
		.probe = qrtr_fifo_xprt_probe,
		.driver = {
		.name = "qcom_fifo_qrtr",
		.of_match_table = qrtr_fifo_xprt_match_table,
		},
		};

		static int __init qrtr_fifo_xprt_init(void)
		{
		int rc;

		rc = platform_driver_register(&qrtr_fifo_xprt_driver);
		if (rc) {
		pr_err("driver register failed %d\n", rc);
		return rc;
		}

		return 0;
		}

		module_init(qrtr_fifo_xprt_init);
		MODULE_DESCRIPTION("QTI IPC-router FIFO XPRT");
		MODULE_LICENSE("GPL v2");