rpmsg: Add AMP rpmsg driver

Add virtio base AMP rpmsg driver.

Signed-off-by: Minda Chen <minda.chen@starfivetech.com>

5 files changed, 406 insertions, 0 deletions

diff --git a/arch/riscv/kernel/smp.c b/arch/riscv/kernel/smp.c
index cba350ae7961..df98505d77b6 100644
--- a/arch/riscv/kernel/smp.c
+++ b/arch/riscv/kernel/smp.c
@@ -149,6 +149,7 @@ void handle_IPI(struct pt_regs *regs)
 #ifdef CONFIG_RISCV_AMP
 	unsigned long *pending_amp_ipis;
 	int cpu_id;
+	extern void rpmsg_handler(void *param);
 #endif
 	irq_enter();
 
@@ -174,6 +175,7 @@ void handle_IPI(struct pt_regs *regs)
 
 		if (amp_ops) {
 			stats[IPI_AMP]++;
+			rpmsg_handler(NULL);
 		}
 #else
 		ops = xchg(pending_ipis, 0);
diff --git a/drivers/rpmsg/Kconfig b/drivers/rpmsg/Kconfig
index 0b4407abdf13..96d430cffb8a 100644
--- a/drivers/rpmsg/Kconfig
+++ b/drivers/rpmsg/Kconfig
@@ -32,6 +32,14 @@ config RPMSG_MTK_SCP
 	  remote processors in MediaTek platforms.
 	  This use IPI and IPC to communicate with remote processors.
 
+config RPMSG_VIRTIO_STARFIVE
+	tristate "Starfive virtio starfive"
+	select RPMSG
+	help
+	  Say y here to enable support providing rpmsg channels to
+	  remote processors in Starfive platforms.
+	  This use IPI and IPC to communicate with remote processors.
+
 config RPMSG_QCOM_GLINK
 	tristate
 	select RPMSG
diff --git a/drivers/rpmsg/Makefile b/drivers/rpmsg/Makefile
index 8d452656f0ee..caad25e84565 100644
--- a/drivers/rpmsg/Makefile
+++ b/drivers/rpmsg/Makefile
@@ -9,3 +9,4 @@ obj-$(CONFIG_RPMSG_QCOM_GLINK_RPM) += qcom_glink_rpm.o
 obj-$(CONFIG_RPMSG_QCOM_GLINK_SMEM) += qcom_glink_smem.o
 obj-$(CONFIG_RPMSG_QCOM_SMD)	+= qcom_smd.o
 obj-$(CONFIG_RPMSG_VIRTIO)	+= virtio_rpmsg_bus.o
+obj-$(CONFIG_RPMSG_VIRTIO_STARFIVE) += virtio_rpmsg_starfive.o
diff --git a/drivers/rpmsg/virtio_rpmsg_bus.c b/drivers/rpmsg/virtio_rpmsg_bus.c
index 8e49a3bacfc7..afbbbf5485a2 100644
--- a/drivers/rpmsg/virtio_rpmsg_bus.c
+++ b/drivers/rpmsg/virtio_rpmsg_bus.c
@@ -903,9 +903,14 @@ static int rpmsg_probe(struct virtio_device *vdev)
 	total_buf_space = vrp->num_bufs * vrp->buf_size;
 
 	/* allocate coherent memory for the buffers */
+#if 0
 	bufs_va = dma_alloc_coherent(vdev->dev.parent,
 				     total_buf_space, &vrp->bufs_dma,
 				     GFP_KERNEL);
+#else
+        bufs_va = dma_alloc_noncoherent(vdev->dev.parent, total_buf_space,
+                        &vrp->bufs_dma, DMA_BIDIRECTIONAL, GFP_KERNEL);
+#endif
 	if (!bufs_va) {
 		err = -ENOMEM;
 		goto vqs_del;
diff --git a/drivers/rpmsg/virtio_rpmsg_starfive.c b/drivers/rpmsg/virtio_rpmsg_starfive.c
new file mode 100644
index 000000000000..179e5e6ea7da
--- /dev/null
+++ b/drivers/rpmsg/virtio_rpmsg_starfive.c
@@ -0,0 +1,390 @@
+#include <linux/dma-map-ops.h>
+#include <linux/export.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/remoteproc.h>
+#include <linux/platform_device.h>
+#include <linux/virtio.h>
+#include <linux/virtio_config.h>
+#include <linux/virtio_ids.h>
+#include <linux/virtio_ring.h>
+#include <linux/err.h>
+#include <linux/kref.h>
+#include <linux/slab.h>
+
+
+struct starfive_rpmsg_mem {
+	void __iomem *vaddr;
+	phys_addr_t phy_addr;
+	u32 vq_offset;
+	u32 queue_base;
+	size_t size;
+};
+
+struct starfive_rpmsg_vq {
+	struct virtqueue *vq[2];
+};
+
+struct starfive_rpmsg_dev {
+	struct device *dev;
+	struct virtio_device vdev;
+	struct starfive_rpmsg_mem shm_mem;
+	struct work_struct rx_work;
+	struct work_struct tx_work;
+	unsigned long rx_chan_bits;
+	unsigned long tx_chan_bits;
+	int rtos_hart_id;
+	u8 status;
+};
+
+#define MAX_CH (16)
+
+struct gen_sw_mbox
+{
+    uint32_t rx_status[MAX_CH];
+    uint32_t tx_status[MAX_CH];
+    uint32_t reserved[MAX_CH];
+    uint32_t rx_ch[MAX_CH];
+    uint32_t tx_ch[MAX_CH];
+};
+
+enum sw_mbox_channel_status
+{
+    S_READY,
+    S_BUSY,
+    S_DONE,
+};
+
+struct starfive_rpmsg_vq rpmsg_vq;
+
+extern int sbi_send_ipi_amp(unsigned int hartid);
+
+static inline struct starfive_rpmsg_dev *vdev_to_rpdev(struct virtio_device *dev)
+{
+	return container_of(dev, struct starfive_rpmsg_dev, vdev);
+}
+
+static void rpmsg_rx_work_func(struct work_struct *work)
+{
+	struct starfive_rpmsg_dev *rpdev = container_of(work, struct starfive_rpmsg_dev, rx_work);
+	struct gen_sw_mbox *base = (void *)rpdev->shm_mem.vaddr;
+	int chan;
+
+	for_each_set_bit(chan, &rpdev->rx_chan_bits, MAX_CH) {
+		if (base->rx_status[chan] == S_DONE) {
+			vring_interrupt(0, rpmsg_vq.vq[0]);
+		}
+		clear_bit(chan, &rpdev->rx_chan_bits);
+	}
+}
+
+static void rpmsg_tx_work_func(struct work_struct *work)
+{
+	struct starfive_rpmsg_dev *rpdev = container_of(work, struct starfive_rpmsg_dev, tx_work);
+	struct gen_sw_mbox *base = (void *)rpdev->shm_mem.vaddr;
+	int chan;
+
+	for_each_set_bit(chan, &rpdev->tx_chan_bits, MAX_CH) {
+		if (base->tx_status[chan] == S_DONE) {
+			base->tx_status[chan] = S_READY;
+			vring_interrupt(0, rpmsg_vq.vq[1]);
+		}
+		clear_bit(chan, &rpdev->tx_chan_bits);
+	}
+}
+
+void rpmsg_handler(void *param) /* maybe put this handler to thread */
+{
+	struct virtqueue *vq[2];
+	struct starfive_rpmsg_dev *rpdev;
+	struct gen_sw_mbox *base;
+	int tasklet_sch = 0;
+
+	vq[0] = rpmsg_vq.vq[0]; /* rx queue */
+	rpdev = (void *)vq[0]->priv;
+	base = (void *)rpdev->shm_mem.vaddr;
+
+	if (base->tx_status[0] == S_DONE) {
+		set_bit(0, &rpdev->tx_chan_bits);
+		schedule_work(&rpdev->tx_work);
+	}
+
+	/* Check if the interrupt is for us */
+	if (base->rx_status[0] != S_BUSY) {
+		return;
+	}
+
+	base->rx_status[0] = S_DONE;
+
+	set_bit(0, &rpdev->rx_chan_bits);
+	schedule_work(&rpdev->rx_work);
+}
+
+static inline uint32_t starfive_vring_init(struct vring *vr, unsigned int num, void *p,
+			      unsigned long align)
+
+{
+	unsigned long addr;
+	uint32_t offset;
+
+	vr->num   = num;
+	vr->desc  = (struct vring_desc *)(void *)p;
+
+	addr = (unsigned long) (p + num * sizeof(struct vring_desc) + align - 1UL);
+	addr &= ~(align - 1UL);
+	vr->avail = (void *)addr;
+	addr = (u64)&vr->avail->ring[num];
+	addr += (align - 1UL);
+	addr &= ~(align - 1UL);
+	vr->used  = (struct vring_used *)addr;
+	addr = (unsigned long)&vr->used->ring[num];
+	addr += (64 - 1UL);
+	addr &= ~(align - 1UL);
+
+	return addr - (unsigned long) vr->desc;
+}
+
+static bool starfive_virtio_notify(struct virtqueue *vq)
+{
+	struct starfive_rpmsg_dev *rpdev = (void *)vq->priv;
+	struct gen_sw_mbox *base = (void *)rpdev->shm_mem.vaddr;
+
+	if (!strcmp(vq->name, "output"))
+		base->tx_status[0] = S_BUSY;
+
+	/* sync before trigger interrupt to remote */
+
+	mb();
+
+	sbi_send_ipi_amp(rpdev->rtos_hart_id);
+	return true;
+}
+
+static struct virtqueue *rp_find_vq(struct virtio_device *vdev,
+				    unsigned int id,
+				    void (*callback)(struct virtqueue *vq),
+				    const char *name, bool ctx)
+{
+	struct starfive_rpmsg_dev *rpdev = vdev_to_rpdev(vdev);
+	struct starfive_rpmsg_mem *shm_mem = &rpdev->shm_mem;
+	struct device *dev = rpdev->dev;
+	unsigned long queue_base;
+	struct virtqueue *vq;
+	struct vring vring;
+	void *addr;
+	int len, size;
+
+	if (!name)
+		return NULL;
+	/*
+	 * Create the new vq, and tell virtio we're not interested in
+	 * the 'weak' smp barriers, since we're talking with a real device.
+	 */
+
+	if (virtio_has_feature(vdev, VIRTIO_F_RING_PACKED))
+		return NULL;
+
+	shm_mem->queue_base =
+		starfive_vring_init(&vring, 2, shm_mem->vaddr + shm_mem->vq_offset + shm_mem->queue_base, 64);
+
+	vq  =__vring_new_virtqueue(id, vring, vdev, false, ctx,
+				     starfive_virtio_notify, callback, name);
+	if (!vq) {
+		dev_err(dev, "vring_new_virtqueue %s failed\n", name);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	vq->priv = rpdev;
+	return vq;
+}
+
+static void starfive_virtio_del_vqs(struct virtio_device *vdev)
+{
+	struct virtqueue *vq, *n;
+	struct rproc_vring *rvring;
+
+	list_for_each_entry_safe(vq, n, &vdev->vqs, list) {
+		rvring = vq->priv;
+		rvring->vq = NULL;
+		vring_del_virtqueue(vq);
+	}
+	rpmsg_vq.vq[0] = rpmsg_vq.vq[1] = NULL;
+}
+
+static int starfive_virtio_find_vqs(struct virtio_device *vdev, unsigned int nvqs,
+				 struct virtqueue *vqs[],
+				 vq_callback_t *callbacks[],
+				 const char * const names[],
+				 const bool * ctx,
+				 struct irq_affinity *desc)
+{
+	int i, ret, queue_idx = 0;
+
+	for (i = 0; i < nvqs; ++i) {
+		if (!names[i]) {
+			vqs[i] = NULL;
+			continue;
+		}
+
+		vqs[i] = rp_find_vq(vdev, queue_idx++, callbacks[i], names[i],
+				    ctx ? ctx[i] : false);
+		if (IS_ERR(vqs[i])) {
+			ret = PTR_ERR(vqs[i]);
+			goto error;
+		}
+		rpmsg_vq.vq[i] = vqs[i];
+	}
+
+	return 0;
+
+error:
+	starfive_virtio_del_vqs(vdev);
+	return ret;
+}
+
+static void starfive_virtio_reset(struct virtio_device *vdev)
+{
+}
+
+static u8 starfive_virtio_get_status(struct virtio_device *vdev)
+{
+	struct starfive_rpmsg_dev *rpmsg = vdev_to_rpdev(vdev);
+
+	return rpmsg->status;
+}
+
+static void starfive_virtio_set_status(struct virtio_device *vdev, u8 status)
+{
+	struct starfive_rpmsg_dev *rpmsg = vdev_to_rpdev(vdev);
+
+	rpmsg->status = status;
+}
+
+static u64 starfive_virtio_get_features(struct virtio_device *vdev)
+{
+	return (BIT(0) | BIT(VIRTIO_F_VERSION_1));
+}
+
+static int starfive_virtio_finalize_features(struct virtio_device *vdev)
+{
+	vdev->features |= (BIT(0) | BIT(VIRTIO_F_VERSION_1));
+
+	return 0;
+}
+
+static const struct virtio_config_ops rpmsg_virtio_config_ops = {
+	.get_features	= starfive_virtio_get_features,
+	.finalize_features = starfive_virtio_finalize_features,
+	.find_vqs	= starfive_virtio_find_vqs,
+	.del_vqs	= starfive_virtio_del_vqs,
+	.reset		= starfive_virtio_reset,
+	.set_status	= starfive_virtio_set_status,
+	.get_status	= starfive_virtio_get_status,
+};
+
+static int starfive_alloc_memory_region(struct starfive_rpmsg_dev *rpmsg)
+{
+	struct starfive_rpmsg_mem *shm_mem;
+        struct device_node *node;
+        struct resource r;
+        int ret;
+
+        node = of_parse_phandle(rpmsg->dev->of_node, "memory-region", 0);
+        if (!node) {
+                dev_err(rpmsg->dev, "no memory-region specified\n");
+                return -EINVAL;
+        }
+
+        ret = of_address_to_resource(node, 0, &r);
+        if (ret) {
+		dev_err(rpmsg->dev, "map to  resource fail\n");
+                return ret;
+        }
+
+	shm_mem = &rpmsg->shm_mem;
+        shm_mem->phy_addr = r.start;
+        shm_mem->size = resource_size(&r);
+	shm_mem->vaddr = devm_memremap(rpmsg->dev, shm_mem->phy_addr,
+				      shm_mem->size,
+				      MEMREMAP_WB);
+
+        if (IS_ERR(shm_mem->vaddr)) {
+                dev_err(rpmsg->dev, "unable to map memory region: %pa+%zx\n",
+                        &r.start, shm_mem->size);
+                return -EBUSY;
+        }
+        return 0;
+}
+
+static int starfive_rpmsg_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *np = dev->of_node;
+	struct starfive_rpmsg_dev *rpmsg_dev;
+	struct virtio_device *vdev;
+	int ret;
+
+	/* Allocate virtio device */
+	rpmsg_dev = devm_kzalloc(dev, sizeof(*rpmsg_dev), GFP_KERNEL);
+	if (!rpmsg_dev) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	rpmsg_dev->dev = dev;
+	ret = starfive_alloc_memory_region(rpmsg_dev);
+	if (ret)
+		return ret;
+	rpmsg_dev->shm_mem.vq_offset = 0x10000;
+	rpmsg_dev->rtos_hart_id = 0x4;
+
+	memset(rpmsg_dev->shm_mem.vaddr + rpmsg_dev->shm_mem.vq_offset, 0, 0x10000);
+	vdev = &rpmsg_dev->vdev;
+	vdev->id.device	= VIRTIO_ID_RPMSG;
+	vdev->config = &rpmsg_virtio_config_ops;
+	vdev->dev.parent = dev;
+	INIT_WORK(&rpmsg_dev->tx_work, rpmsg_tx_work_func);
+	INIT_WORK(&rpmsg_dev->rx_work, rpmsg_rx_work_func);
+
+	ret = register_virtio_device(vdev);
+	if (ret) {
+		dev_err(dev, "failed to register vdev: %d\n", ret);
+		goto out;
+	}
+	platform_set_drvdata(pdev, rpmsg_dev);
+
+	dev_info(dev, "registered %s\n", dev_name(&vdev->dev));
+
+out:
+	return ret;
+}
+
+static int starfive_rpmsg_remove(struct platform_device *pdev)
+{
+	struct starfive_rpmsg_dev *vdev = platform_get_drvdata(pdev);
+
+	cancel_work_sync(&vdev->rx_work);
+	cancel_work_sync(&vdev->tx_work);
+	unregister_virtio_device(&vdev->vdev);
+	return 0;
+}
+
+static const struct of_device_id amp_rpmsg_of_match[] = {
+	{ .compatible = "starfive,amp-virtio-rpmsg", .data = NULL },
+	{},
+};
+MODULE_DEVICE_TABLE(of, amp_rpmsg_of_match);
+
+static struct platform_driver starfive_rmpsg_driver = {
+	.probe = starfive_rpmsg_probe,
+	.remove = starfive_rpmsg_remove,
+	.driver = {
+		.name = "starfive-rpmsg",
+		.of_match_table = amp_rpmsg_of_match,
+	},
+};
+module_platform_driver(starfive_rmpsg_driver);
+MODULE_LICENSE("GPL v2");