tmp_kernel_5.15/drivers/remoteproc/fmql_remoteproc.c

/*
 * FMQL Remote Processor driver
 *
 * Based on origin OMAP Remote Processor driver
 *
 * Copyright (C) 2011 Texas Instruments, Inc.
 * Copyright (C) 2011 Google, Inc.
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * version 2 as published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 */

#define DEBUG
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/err.h>
#include <linux/platform_device.h>
#include <linux/dma-mapping.h>
#include <linux/remoteproc.h>
#include <linux/interrupt.h>
#include <linux/of_address.h>
#include <linux/of_irq.h>
#include <linux/of_reserved_mem.h>
#include <linux/smp.h>
#include <linux/irqchip/arm-gic.h>
#include <linux/slab.h>
#include <linux/cpu.h>
#include <linux/genalloc.h>
#ifdef CONFIG_ARCH_FMSH
#include <../../arch/arm/mach-fmsh/common.h>
#endif

#include "remoteproc_internal.h"

#define MAX_NUM_VRINGS 2
#define NOTIFYID_ANY (-1)
/* Maximum on chip memories used by the driver*/
#define MAX_ON_CHIP_MEMS        32
#define MAX_REMOTE_CORE	(3)

#ifdef CONFIG_ARCH_FMSH
#define NO_CPU_AUTO_HOTPLUG
#endif

/* Structure for storing IRQs */
struct irq_list {
	int irq;
	struct list_head list;
};

/* Structure for IPIs */
struct ipi_info {
	u32 irq;
	u32 notifyid;
	bool pending;
};

/**
 * struct fmql_mem_res - fmql memory resource for firmware memory
 * @res: memory resource
 * @node: list node
 */
struct fmql_mem_res {
	struct resource res;
	struct list_head node;
};

/**
 * struct fmql_rproc_data - fmql rproc private data
 * @irqs: inter processor soft IRQs
 * @rproc: pointer to remoteproc instance
 * @ipis: interrupt processor interrupts statistics
 * @fw_mems: list of firmware memories
 */
struct fmql_rproc_pdata {
	struct irq_list irqs;
	struct rproc *rproc;
	struct ipi_info ipis[MAX_NUM_VRINGS];
	struct list_head fw_mems;
	struct work_struct workqueue;
	int core_id;
};

static bool autoboot __read_mostly;
static struct work_struct* workqueue_array[MAX_REMOTE_CORE];

static void handle_event(struct work_struct *work)
{
	struct fmql_rproc_pdata *local = container_of(work,
							struct fmql_rproc_pdata, workqueue);
	struct rproc *rproc = local->rproc;

	if (rproc_vq_interrupt(local->rproc, local->ipis[0].notifyid) ==
				IRQ_NONE)
		dev_dbg(rproc->dev.parent, "no message found in vqid 0\n");
}

static void ipi_kick(void)
{
	int i;
	pr_debug("Remoteproc KICK Linux because of pending message\n");

	for (i = 0; i < MAX_REMOTE_CORE; i++) {
		if (workqueue_array[i]) {
			schedule_work(workqueue_array[i]);
		}
	}
}

static void kick_pending_ipi(struct rproc *rproc)
{
	struct fmql_rproc_pdata *local = rproc->priv;
	int i;

	for (i = 0; i < MAX_NUM_VRINGS; i++) {
		/* Send swirq to firmware */
		if (local->ipis[i].pending == true) {
			smp_kick_ipi(cpumask_of(local->core_id),
					local->ipis[i].irq);
			local->ipis[i].pending = false;
		}
	}
}

#ifdef CONFIG_ARCH_FMSH

static int cpu_onoff_raw(unsigned int cpu, unsigned long entry_point, bool on)
{
	if (on) {
		fmsh_cpun_start(entry_point, cpu);
	} else {
		// not support stop this cpu
	}
	return 0;
}

#elif CONFIG_ARCH_FMQLMP

static int cpu_onoff_raw(unsigned int cpu, unsigned long entry_point, bool on)
{
#define SLCR_REG_BASE_ADDR (0xe0026000)
#define APU_RST_CTRL     (SLCR_REG_BASE_ADDR + 0x380)
#define APU_RVBAR_L_0    (SLCR_REG_BASE_ADDR + 0xA40)
#define APU_RVBAR_H_0    (SLCR_REG_BASE_ADDR + 0xA44)
#define APU_RVBAR_L_1    (SLCR_REG_BASE_ADDR + 0xA48)
#define APU_RVBAR_H_1    (SLCR_REG_BASE_ADDR + 0xA4C)
#define APU_RVBAR_L_2    (SLCR_REG_BASE_ADDR + 0xA50)
#define APU_RVBAR_H_2    (SLCR_REG_BASE_ADDR + 0xA54)
#define APU_RVBAR_L_3    (SLCR_REG_BASE_ADDR + 0xA58)
#define APU_RVBAR_H_3    (SLCR_REG_BASE_ADDR + 0xA5C)

#define APU0_RESET_BIT_MASK   ((1<<10)|(1<<0))
#define APU1_RESET_BIT_MASK   ((1<<11)|(1<<1))
#define APU2_RESET_BIT_MASK   ((1<<12)|(1<<2))
#define APU3_RESET_BIT_MASK   ((1<<13)|(1<<3))
	static u8 __iomem *apu_crtl;
	static u8 __iomem *base_addr;
	static u32 apu_rst_control;
	static u32 apu_rst_control_mask;

	apu_crtl = ioremap(APU_RST_CTRL, 4);
	base_addr = ioremap(APU_RVBAR_L_0 + (cpu * 8), 8);

	pr_debug("remote cpu%d: %s, entry:0x%lx\n", cpu, on ? "on" : "off",
									entry_point);

	apu_rst_control = readl(apu_crtl);
	apu_rst_control_mask = (1 << cpu) | (1 << (cpu + 10));

	if (on) {
		apu_rst_control &= ~apu_rst_control_mask;
	} else {
		apu_rst_control |= apu_rst_control_mask;
	}

	writel(entry_point, base_addr);
	writel(apu_rst_control, apu_crtl);

	smp_wmb();
	iounmap(base_addr);
	iounmap(apu_crtl);
	sev();

	return 0;
}
#else

static int cpu_onoff_raw(unsigned int cpu, unsigned long entry_point, bool on)
{
	pr_info("The architecture does not support this feature.\n");
}
#endif

static int fmql_rproc_start(struct rproc *rproc)
{
	struct device *dev = rproc->dev.parent;
	struct fmql_rproc_pdata *local = rproc->priv;
	int ret = 0;

	dev_dbg(dev, "%s\n", __func__);

	INIT_WORK(&local->workqueue, handle_event);
	workqueue_array[local->core_id - 1] = &local->workqueue;

#ifndef NO_CPU_AUTO_HOTPLUG
	// ret = cpu_down(local->core_id);
	ret = cpu_device_down(dev);
	/* EBUSY means CPU is already released */
	if (ret && (ret != -EBUSY)) {
		dev_err(dev, "Can't release cpu1\n");
		return ret;
	}
#endif

	/* start up cpu */
	// smp_cpu_start_raw(local->core_id, rproc->bootaddr);
	cpu_onoff_raw(local->core_id, rproc->bootaddr, true);

	/* Trigger pending kicks */
	kick_pending_ipi(rproc);

	return ret;
}

/* kick a firmware */
static void fmql_rproc_kick(struct rproc *rproc, int vqid)
{
	struct device *dev = rproc->dev.parent;
	struct fmql_rproc_pdata *local = rproc->priv;
	struct rproc_vdev *rvdev, *rvtmp;
	int i;

	dev_dbg(dev, "KICK Firmware to start send messages vqid %d\n", vqid);

	list_for_each_entry_safe(rvdev, rvtmp, &rproc->rvdevs, node) {
		for (i = 0; i < MAX_NUM_VRINGS; i++) {
			struct rproc_vring *rvring = &rvdev->vring[i];

			/* Send swirq to firmware */
			if (rvring->notifyid == vqid) {
				local->ipis[i].notifyid = vqid;
				/* As we do not turn off CPU1 until start,
				 * we delay firmware kick
				 */
				if (rproc->state == RPROC_RUNNING) {
					smp_kick_ipi(cpumask_of(local->core_id),
						local->ipis[i].irq);
				} else {
					local->ipis[i].pending = true;
				}
			}
		}
	}
}

/* power off the remote processor */
static int fmql_rproc_stop(struct rproc *rproc)
{
	struct device *dev = rproc->dev.parent;
	struct fmql_rproc_pdata *local = rproc->priv;
	int ret = 0;

	dev_dbg(dev, "%s\n", __func__);

	/* stop cpu */
	// smp_cpu_stop_raw(local->core_id);
	cpu_onoff_raw(local->core_id, 0, false);

#ifndef NO_CPU_AUTO_HOTPLUG
	/* Cpu can't be power on - for example in nosmp mode */
	ret =cpu_device_up(dev);
	if (ret)
		dev_err(dev, "Can't power on cpu1 %d\n", ret);
#endif

	return ret;
}

static int fmql_parse_fw(struct rproc *rproc, const struct firmware *fw)
{
	int num_mems, i, ret;
	struct device *dev = rproc->dev.parent;
	struct device_node *np = dev->of_node;
	struct rproc_mem_entry *mem;

	num_mems = of_count_phandle_with_args(np, "memory-region", NULL);
	if (num_mems <= 0)
		return 0;
	for (i = 0; i < num_mems; i++) {
		struct device_node *node;
		struct reserved_mem *rmem;

		node = of_parse_phandle(np, "memory-region", i);
		rmem = of_reserved_mem_lookup(node);
		if (!rmem) {
			dev_err(dev, "unable to acquire memory-region\n");
			return -EINVAL;
		}
		if (strstr(node->name, "vdev") &&
			strstr(node->name, "buffer")) {
			/* Register DMA region */
			mem = rproc_mem_entry_init(dev, NULL,
						   (dma_addr_t)rmem->base,
						   rmem->size, rmem->base,
						   NULL, NULL,
						   node->name);
			if (!mem) {
				dev_err(dev,
					"unable to initialize memory-region %s \n",
					node->name);
				return -ENOMEM;
			}
			rproc_add_carveout(rproc, mem);
		} else if (strstr(node->name, "vdev") &&
			   strstr(node->name, "vring")) {
			/* Register vring */
			mem = rproc_mem_entry_init(dev, NULL,
						   (dma_addr_t)rmem->base,
						   rmem->size, rmem->base,
						   NULL, NULL,
						   node->name);
			mem->va = devm_ioremap_wc(dev, rmem->base, rmem->size);
			if (!mem->va)
				return -ENOMEM;
			if (!mem) {
				dev_err(dev,
					"unable to initialize memory-region %s\n",
					node->name);
				return -ENOMEM;
			}
			rproc_add_carveout(rproc, mem);
		} else {
			mem = rproc_of_resm_mem_entry_init(dev, i,
							rmem->size,
							rmem->base,
							node->name);
			if (!mem) {
				dev_err(dev,
					"unable to initialize memory-region %s \n",
					node->name);
				return -ENOMEM;
			}
			mem->va = devm_ioremap_wc(dev, rmem->base, rmem->size);
			if (!mem->va)
				return -ENOMEM;

			rproc_add_carveout(rproc, mem);
		}
	}

	ret = rproc_elf_load_rsc_table(rproc, fw);
	if (ret == -EINVAL)
		ret = 0;
	return ret;
}

#if 0
static void *fmql_rproc_da_to_va(struct rproc *rproc, u64 da, int len)
{
	struct rproc_mem_entry *mem;
	void *va = 0;
	struct fmql_rproc_pdata *local = rproc->priv;

	dev_dbg(rproc->dev.parent, "%s, da:0x%lx,len:%d\n", __func__, da, len);
	list_for_each_entry(mem, &local->mems, node) {
		int offset = da - mem->da;

		/* try next carveout if da is too small */
		if (offset < 0)
			continue;

		/* try next carveout if da is too large */
		if (offset + len > mem->len)
			continue;

		va = mem->va + offset;

		break;
	}
	return va;
}
#endif

static struct rproc_ops fmql_rproc_ops = {
	.start		= fmql_rproc_start,
	.stop		= fmql_rproc_stop,
	.load		= rproc_elf_load_segments,
	.parse_fw	= fmql_parse_fw,
	.find_loaded_rsc_table = rproc_elf_find_loaded_rsc_table,
	.get_boot_addr	= rproc_elf_get_boot_addr,
	.kick		= fmql_rproc_kick,
};

/* Just to detect bug if interrupt forwarding is broken */
static irqreturn_t fmql_remoteproc_interrupt(int irq, void *dev_id)
{
	struct device *dev = dev_id;

	dev_err(dev, "GIC IRQ %d is not forwarded correctly\n", irq);

	/*
	 *  MS: Calling this function doesn't need to be BUG
	 * especially for cases where firmware doesn't disable
	 * interrupts. In next probing can be som interrupts pending.
	 * The next scenario is for cases when you want to monitor
	 * non frequent interrupt through Linux kernel. Interrupt happen
	 * and it is forwarded to Linux which update own statistic
	 * in (/proc/interrupt) and forward it to firmware.
	 *
	 * gic_set_cpu(1, irq);	- setup cpu1 as destination cpu
	 * gic_raise_softirq(cpumask_of(1), irq); - forward irq to firmware
	 */

//	gic_set_cpu(1, irq);
	return IRQ_HANDLED;
}

static void clear_irq(struct rproc *rproc)
{
	struct list_head *pos, *q;
	struct irq_list *tmp;
	struct fmql_rproc_pdata *local = rproc->priv;

	dev_info(rproc->dev.parent, "Deleting the irq_list\n");
	list_for_each_safe(pos, q, &local->irqs.list) {
		tmp = list_entry(pos, struct irq_list, list);
		free_irq(tmp->irq, rproc->dev.parent);
		// gic_set_cpu(0, tmp->irq);
		list_del(pos);
		kfree(tmp);
	}
}

#if 0
static int fmql_rproc_add_mems(struct fmql_rproc_pdata *pdata)
{
	struct mem_pool_st *mem_node;
	size_t mem_size;
	struct gen_pool *mem_pool;
	struct rproc_mem_entry *mem;
	dma_addr_t dma;
	void *va;
	struct device *dev = pdata->rproc->dev.parent;

	list_for_each_entry(mem_node, &pdata->mem_pools, node) {
		mem_pool = mem_node->pool;
		mem_size = gen_pool_size(mem_pool);
		mem  = devm_kzalloc(dev, sizeof(struct rproc_mem_entry),
				GFP_KERNEL);
		if (!mem)
			return -ENOMEM;

		va = gen_pool_dma_alloc(mem_pool, mem_size, &dma);
		if (!va) {
			dev_err(dev, "Failed to allocate dma carveout mem.\n");
			return -ENOMEM;
		}
		mem->priv = (void *)mem_pool;
		mem->va = va;
		mem->len = mem_size;
		mem->dma = dma;
		mem->da = dma;
		dev_dbg(dev, "%s: va = %p, da = 0x%x dma = 0x%x\n",
			__func__, va, mem->da, mem->dma);
		list_add_tail(&mem->node, &pdata->mems);
	}
	return 0;
}
#endif

static int fmql_remoteproc_probe(struct platform_device *pdev)
{
	int ret = 0;
	struct irq_list *tmp;
	int count = 0;
	struct rproc *rproc;
	struct fmql_rproc_pdata *local;

	rproc = rproc_alloc(&pdev->dev, dev_name(&pdev->dev),
		&fmql_rproc_ops, NULL,
		sizeof(struct fmql_rproc_pdata));
	if (!rproc) {
		dev_err(&pdev->dev, "rproc allocation failed\n");
		ret = -ENOMEM;
		return ret;
	}
	local = rproc->priv;
	local->rproc = rproc;

	platform_set_drvdata(pdev, rproc);

	ret = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(32));
	if (ret) {
		dev_err(&pdev->dev, "dma_set_coherent_mask: %d\n", ret);
		goto dma_mask_fault;
	}

	/* Set core id */
	ret = of_property_read_u32(pdev->dev.of_node, "core-id",
				&local->core_id);
	if (ret < 0) {
		dev_warn(&pdev->dev, "unable to read core-id property, use core1");
		local->core_id = 1;
	}

#if 1
	/* Init list for IRQs - it can be long list */
	INIT_LIST_HEAD(&local->irqs.list);

	/* Alloc IRQ based on DTS to be sure that no other driver will use it */
	while (1) {
		int irq;

		irq = platform_get_irq(pdev, count++);
		if (irq == -ENXIO || irq == -EINVAL)
			break;

		tmp = kzalloc(sizeof(*tmp), GFP_KERNEL);
		if (!tmp) {
			ret = -ENOMEM;
			goto irq_fault;
		}

		tmp->irq = irq;

		dev_dbg(&pdev->dev, "%d: Alloc irq: %d\n", count, tmp->irq);

		/* Allocating shared IRQs will ensure that any module will
		 * use these IRQs
		 */
		ret = request_irq(tmp->irq, fmql_remoteproc_interrupt, 0,
				  dev_name(&pdev->dev), &pdev->dev);
		if (ret) {
			dev_err(&pdev->dev, "IRQ %d already allocated\n",
				tmp->irq);
			goto irq_fault;
		}

		/*
		 * MS: Here is place for detecting problem with firmware
		 * which doesn't work correctly with interrupts
		 *
		 * MS: Comment if you want to count IRQs on Linux
		 */
	//	gic_set_cpu(1, tmp->irq);
		list_add(&tmp->list, &local->irqs.list);
	}
#endif

	/* Allocate free IPI number */
	/* Read vring0 ipi number */
	ret = of_property_read_u32(pdev->dev.of_node, "vring0",
				&local->ipis[0].irq);
	if (ret < 0) {
		dev_err(&pdev->dev, "unable to read property");
		goto irq_fault;
	}

	/* TODO */
	ret = set_ipi_handler(local->ipis[0].irq, ipi_kick,
			"Firmware kick");
	if (ret) {
		dev_err(&pdev->dev, "IPI handler already registered\n");
		goto irq_fault;
	}

	/* Read vring1 ipi number */
	ret = of_property_read_u32(pdev->dev.of_node, "vring1",
				&local->ipis[1].irq);
	if (ret < 0) {
		dev_err(&pdev->dev, "unable to read property");
		goto ipi_fault;
	}

	rproc->auto_boot = autoboot;

	ret = rproc_add(local->rproc);
	if (ret) {
		dev_err(&pdev->dev, "rproc registration failed\n");
		goto ipi_fault;
	}

	return 0;

ipi_fault:
	clear_ipi_handler(local->ipis[0].irq);

irq_fault:
	clear_irq(rproc);

dma_mask_fault:
	rproc_free(rproc);

	return ret;
}

static int fmql_remoteproc_remove(struct platform_device *pdev)
{
	struct rproc *rproc = platform_get_drvdata(pdev);
	struct fmql_rproc_pdata *local = rproc->priv;

	dev_info(&pdev->dev, "%s\n", __func__);

	rproc_del(rproc);

	clear_ipi_handler(local->ipis[0].irq);
	clear_irq(rproc);

	of_reserved_mem_device_release(&pdev->dev);
	rproc_free(rproc);

	return 0;
}

/* Match table for OF platform binding */
static const struct of_device_id fmql_remoteproc_match[] = {
	{ .compatible = "fmsh,fmql_remoteproc", },
	{ /* end of list */ },
};
MODULE_DEVICE_TABLE(of, fmql_remoteproc_match);

static struct platform_driver fmql_remoteproc_driver = {
	.probe = fmql_remoteproc_probe,
	.remove = fmql_remoteproc_remove,
	.driver = {
		.name = "fmql_remoteproc",
		.of_match_table = fmql_remoteproc_match,
	},
};
module_platform_driver(fmql_remoteproc_driver);

module_param_named(autoboot,  autoboot, bool, 0444);
MODULE_PARM_DESC(autoboot,
		 "enable | disable autoboot. (default: false)");

MODULE_AUTHOR("Liu Lei <liulei@fmsh.com.cn");
MODULE_LICENSE("GPL v2");
MODULE_DESCRIPTION("FMQL remote processor control driver");