// SPDX-License-Identifier: GPL-2.0
/*
 * isp.c
 *
 * Copyright (C) 2018 Linaro Ltd.
 */

#include <linux/clk.h>
#include <linux/dma-mapping.h>
#include <linux/media-bus-format.h>
#include <linux/media.h>
#include <linux/module.h>
#include <linux/platform_device.h>
#include <linux/of.h>
#include <linux/of_graph.h>
#include <linux/pm_runtime.h>
#include <linux/pm_domain.h>
#include <linux/slab.h>
#include <linux/videodev2.h>

#include <media/media-device.h>
#include <media/v4l2-async.h>
#include <media/v4l2-ctrls.h>
#include <media/v4l2-device.h>
#include <media/v4l2-mc.h>
#include <media/v4l2-fwnode.h>

#include "isp.h"

#define DOVDD_VOLTAGE 1800000
#define ISP_CLK_FREQ 24000000

const struct resources isp_res[] = {
	{
		.reg = "ispss_ctrl",
	},
	{
		.reg = "smmu_ctrl",
	},
	{
		.reg = "irq_merger2",
	},
};

const struct resources csiphy_res[] = {
	{
		.reg = "csi0",
	},
	{
		.reg = "csi1",
	},
};

const struct resources sr_res[] = {
	{
		.reg = "sr",
	},
};

const struct resources cvdr_res[] = {
	{
		.reg = "cvdr_rt",
	},
	{
		.reg = "cvdr_srt",
	},
	{
		.reg = "sub_ctrl",
	},
};

void isp_writel(void __iomem *base , u32 reg, u32 value)
{
	writel(value, base + reg);
}

u32 isp_readl(void __iomem *base , u32 reg)
{
	return readl(base + reg);
}

u32 isp_clear_irq(struct isp *isp, enum IRQ_MERGER_TYPE irq)
{
	u32 val;

	val = isp_readl(isp->irq_merger2, irq + IRQ_MERGER_FUNC_RIS);
	isp_writel(isp->irq_merger2, irq+IRQ_MERGER_FUNC_ICR, val);

	return val;
}

int frame_num2Offset(struct isp *isp, int frame_num)
{
	return (isp->frame_size * (frame_num % isp->frame_count));
}

void isp_config_smmu_bypass(struct isp *isp)
{
	pr_info("%s: %d\n", __func__, __LINE__);
	isp_writel(isp->smmu_ctrl, 0x0, 0x1);
}

void isp_ispss_enable_irq(struct isp *isp)
{
	pr_info("%s: %d\n", __func__, __LINE__);
	isp_writel(isp->irq_merger2, IRQ_MERGER_IMSC_DEBUG1, 0xFFFFFFFF);
	isp_writel(isp->irq_merger2, IRQ_MERGER_IMSC_FRPROC0, 0xFFFFFFFF);
}

void isp_ispss_clear_irq_state(struct isp *isp)
{
	pr_info("%s: %d\n", __func__, __LINE__);
	isp_writel(isp->irq_merger2, IRQ_MERGER_ICR_DEBUG0, 0xFFFFFFFF);
	isp_writel(isp->irq_merger2, IRQ_MERGER_ICR_DEBUG1, 0xFFFFFFFF);
	isp_writel(isp->irq_merger2, IRQ_MERGER_ICR_DEBUG2, 0xFFFFFFFF);
	isp_writel(isp->irq_merger2, IRQ_MERGER_ICR_DEBUG3, 0xFFFFFFFF);
	isp_writel(isp->irq_merger2, IRQ_MERGER_ICR_ERROR0, 0xFFFFFFFF);
	isp_writel(isp->irq_merger2, IRQ_MERGER_ICR_ERROR1, 0xFFFFFFFF);
	isp_writel(isp->irq_merger2, IRQ_MERGER_ICR_FRPROC0, 0xFFFFFFFF);
	isp_writel(isp->irq_merger2, IRQ_MERGER_ICR_FRPROC1, 0xFFFFFFFF);
	isp_writel(isp->irq_merger2, IRQ_MERGER_ICR_FRPROC2, 0xFFFFFFFF);
}

/*
 * isp_enable_clocks - Enable multiple clocks
 * @nclocks: Number of clocks in clock array
 * @clock: Clock array
 * @dev: Device
 *
 * Return 0 on success or a negative error code otherwise
 */
int isp_enable_clocks(struct isp_clock *clks, struct device *dev)
{
	int ret;
	int i;

	for (i = 0; i < ISP_NUM_CLKS; i++) {
		ret = clk_prepare_enable(clks[i].clk);
		if (ret) {
			dev_err(dev, "clock enable failed: %d\n", ret);
			goto error;
		}
	}

	return 0;

error:
	for (i--; i >= 0; i--)
		clk_disable_unprepare(clks[i].clk);

	return ret;
}

/*
 * isp_disable_clocks - Disable multiple clocks
 * @nclocks: Number of clocks in clock array
 * @clock: Clock array
 */
void isp_disable_clocks(struct isp_clock *clks)
{
	int i;

	for (i = ISP_NUM_CLKS - 1; i >= 0; i--)
		clk_disable_unprepare(clks[i].clk);
}

/*
 * isp_find_sensor - Find a linked media entity which represents a sensor
 * @entity: Media entity to start searching from
 *
 * Return a pointer to sensor media entity or NULL if not found
 */
static struct media_entity *isp_find_sensor(struct media_entity *entity)
{
	struct media_pad *pad;

	while (1) {
		pad = &entity->pads[0];
		if (!(pad->flags & MEDIA_PAD_FL_SINK))
			return NULL;

		pad = media_entity_remote_pad(pad);
		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
			return NULL;

		entity = pad->entity;

		if (entity->function == MEDIA_ENT_F_CAM_SENSOR)
			return entity;
	}
}

/*
 * isp_get_pixel_clock - Get pixel clock rate from sensor
 * @entity: Media entity in the current pipeline
 * @pixel_clock: Received pixel clock value
 *
 * Return 0 on success or a negative error code otherwise
 */
int isp_get_pixel_clock(struct media_entity *entity, u32 *pixel_clock)
{
	struct media_entity *sensor;
	struct v4l2_subdev *subdev;
	struct v4l2_ctrl *ctrl;

	sensor = isp_find_sensor(entity);
	if (!sensor)
		return -ENODEV;

	subdev = media_entity_to_v4l2_subdev(sensor);

	ctrl = v4l2_ctrl_find(subdev->ctrl_handler, V4L2_CID_PIXEL_RATE);

	if (!ctrl)
		return -EINVAL;

	*pixel_clock = v4l2_ctrl_g_ctrl_int64(ctrl);

	return 0;
}

/*
 * isp_of_parse_endpoint_node - Parse port endpoint node
 * @dev: Device
 * @node: Device node to be parsed
 * @csd: Parsed data from port endpoint node
 *
 * Return 0 on success or a negative error code on failure
 */
static int isp_of_parse_endpoint_node(struct device *dev,
					struct device_node *node,
					struct isp_async_subdev *csd,
					struct v4l2_async_notifier *async)
{
	struct csiphy_config *cfg = &csd->interface.cfg;
	struct v4l2_fwnode_bus_mipi_csi2 *mipi_csi2;
	struct v4l2_fwnode_endpoint vep = { .bus_type = 0 };
	struct isp *isp = container_of(async, struct isp, notifier);
	unsigned int i;

	v4l2_fwnode_endpoint_parse(of_fwnode_handle(node), &vep);

	/* CSI2PHY port id */
	isp->csiphy_id = vep.base.port;
	pr_info("CSIPHY ID: %d\n", isp->csiphy_id);

	mipi_csi2 = &vep.bus.mipi_csi2;

	/* CSI2PHY data lanes */
	cfg->num_data = mipi_csi2->num_data_lanes;

	return 0;
}

/*
 * isp_of_parse_ports - Parse ports node
 * @dev: Device
 * @notifier: v4l2_device notifier data
 *
 * Return number of "port" nodes found in "ports" node
 */

static int isp_of_parse_ports(struct device *dev,
				struct v4l2_async_notifier *notifier)
{
	struct device_node *node = NULL;
	struct device_node *remote = NULL;
	unsigned int size, i;
	int ret;

	while ((node = of_graph_get_next_endpoint(dev->of_node, node)))
		if (of_device_is_available(node))
			notifier->num_subdevs++;

	size = sizeof(*notifier->subdevs) * notifier->num_subdevs;
	notifier->subdevs = devm_kzalloc(dev, size, GFP_KERNEL);
	if (!notifier->subdevs) {
		dev_err(dev, "Failed to allocate memory\n");
		return -ENOMEM;
	}

	i = 0;
	while ((node = of_graph_get_next_endpoint(dev->of_node, node))) {
		struct isp_async_subdev *csd;

		if (!of_device_is_available(node))
			continue;

		csd = devm_kzalloc(dev, sizeof(*csd), GFP_KERNEL);
		if (!csd) {
			of_node_put(node);
			dev_err(dev, "Failed to allocate memory\n");
			return -ENOMEM;
		}

		notifier->subdevs[i++] = &csd->asd;

		ret = isp_of_parse_endpoint_node(dev, node, csd, notifier);
		if (ret < 0) {
			of_node_put(node);
			return ret;
		}

		remote = of_graph_get_remote_port_parent(node);
		of_node_put(node);

		if (!remote) {
			dev_err(dev, "Cannot get remote parent\n");
			return -EINVAL;
		}

		csd->asd.match_type = V4L2_ASYNC_MATCH_FWNODE;
		csd->asd.match.fwnode.fwnode = of_fwnode_handle(remote);
	}

	return notifier->num_subdevs;
}

/*
 * isp_init_subdevices - Initialize subdev structures and resources
 *
 * Return 0 on success or a negative error code on failure
 */
static int isp_init_subdevices(struct isp *isp)
{
	unsigned int i;
	int ret;

	for (i = 0; i < isp->csiphy_num; i++) {
		ret = isp_csiphy_subdev_init(isp, &isp->csiphy[i],
				&csiphy_res[i], i);
		if (ret < 0) {
			dev_err(isp->dev,
				"Failed to init csiphy%d sub-device: %d\n",
				i, ret);
			return ret;
		}
	}

	ret = isp_sr_subdev_init(isp, isp->sr, sr_res);
	if (ret < 0) {
		dev_err(isp->dev, "Failed to init SR sub-device: %d\n",
			ret);
		return ret;
	}

	ret = isp_cvdr_subdev_init(isp, isp->cvdr, cvdr_res);
	if (ret < 0) {
		dev_err(isp->dev, "Failed to init CVDR sub-device: %d\n",
			ret);
		return ret;
	}

	return 0;
}

/*
 * isp_register_entities - Register subdev nodes and create links
 *
 * Return 0 on success or a negative error code on failure
 */
static int isp_register_entities(struct isp *isp)
{
	int i;
	int ret;

	for (i = 0; i < isp->csiphy_num; i++) {
		ret = isp_csiphy_register_entity(&isp->csiphy[i],
						 &isp->v4l2_dev);
		if (ret < 0) {
			dev_err(isp->dev,
				"Failed to register csiphy%d entity: %d\n",
				i, ret);
			goto err_reg_csiphy;
		}
	}

	ret = isp_sr_register_entity(isp->sr, &isp->v4l2_dev);
	if (ret < 0) {
		dev_err(isp->dev, "Failed to register sr entity: %d\n",
			ret);
		goto err_reg_sr;
	}

	ret = isp_cvdr_register_entity(isp->cvdr, &isp->v4l2_dev);
	if (ret < 0) {
		dev_err(isp->dev, "Failed to register cvdr entity: %d\n",
			ret);
		goto err_reg_sr;
	}

	for (i = 0; i < isp->csiphy_num; i++) {
		ret = media_create_pad_link(
			&isp->csiphy[i].subdev.entity,
			ISP_CSIPHY_PAD_SRC,
			&isp->sr->subdev.entity,
			ISP_SR_PAD_SINK,
			0);
		if (ret < 0) {
			dev_err(isp->dev,
				"Failed to link %s->%s entities: %d\n",
				isp->csiphy[i].subdev.entity.name,
				isp->sr->subdev.entity.name,
				ret);
			goto err_reg_sr;
		}
	}

	ret = media_create_pad_link(&isp->sr->subdev.entity,
				    ISP_SR_PAD_SRC,
				    &isp->cvdr->subdev.entity,
				    ISP_CVDR_PAD_SINK,
				    0);
	if (ret < 0) {
		dev_err(isp->dev, "Failed to link %s->%s entities: %d\n",
			isp->sr->subdev.entity.name,
			isp->cvdr->subdev.entity.name,
			ret);
		goto err_reg_sr;
	}
	
	return 0;

err_reg_sr:
	isp_sr_unregister_entity(isp->sr);

	i = isp->csiphy_num;
err_reg_csiphy:
	for (i--; i >= 0; i--)
		isp_csiphy_unregister_entity(&isp->csiphy[i]);

	return ret;
}

static void isp_unregister_entities(struct isp *isp)
{
	unsigned int i;

	for (i = 0; i < isp->csiphy_num; i++)
		isp_csiphy_unregister_entity(&isp->csiphy[i]);
	isp_sr_unregister_entity(isp->sr);
	isp_cvdr_unregister_entity(isp->cvdr);
}

static int isp_subdev_notifier_bound(struct v4l2_async_notifier *async,
				       struct v4l2_subdev *subdev,
				       struct v4l2_async_subdev *asd)
{
	struct isp *isp = container_of(async, struct isp, notifier);
	struct isp_async_subdev *csd =
		container_of(asd, struct isp_async_subdev, asd);
	u8 id = isp->csiphy_id;
	struct csiphy_device *csiphy = &isp->csiphy[id];

	pr_info("Notifier bound  CSIPHY id: %d\n", id);
	csiphy->cfg.num_data = csd->interface.cfg.num_data;
	subdev->host_priv = csiphy;

	return 0;
}

static int isp_subdev_notifier_complete(struct v4l2_async_notifier *async)
{
	struct isp *isp = container_of(async, struct isp, notifier);
	struct v4l2_device *v4l2_dev = &isp->v4l2_dev;
	struct v4l2_subdev *sd;
	int ret;

	pr_info("Notifier complete\n");
	list_for_each_entry(sd, &v4l2_dev->subdevs, list) {
		if (sd->host_priv) {
			struct media_entity *sensor = &sd->entity;
			struct csiphy_device *csiphy =
					(struct csiphy_device *) sd->host_priv;
			struct media_entity *input = &csiphy->subdev.entity;
			unsigned int i;

			for (i = 0; i < sensor->num_pads; i++) {
				if (sensor->pads[i].flags & MEDIA_PAD_FL_SOURCE)
					break;
			}
			if (i == sensor->num_pads) {
				dev_err(isp->dev,
					"No source pad in external entity\n");
				return -EINVAL;
			}

			ret = media_create_pad_link(sensor, i,
				input, ISP_CSIPHY_PAD_SINK,
				MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
			if (ret < 0) {
				dev_err(isp->dev,
					"Failed to link %s->%s entities: %d\n",
					sensor->name, input->name, ret);
				return ret;
			}
		}
	}

	ret = v4l2_device_register_subdev_nodes(&isp->v4l2_dev);
	if (ret < 0)
		return ret;

	return media_device_register(&isp->media_dev);
}

static const struct v4l2_async_notifier_operations isp_subdev_notifier_ops = {
	.bound = isp_subdev_notifier_bound,
	.complete = isp_subdev_notifier_complete,
};

static const struct media_device_ops isp_media_ops = {
	.link_notify = v4l2_pipeline_link_notify,
};

/*
 * isp_probe - Probe ISP platform device
 * @pdev: Pointer to ISP platform device
 *
 * Return 0 on success or a negative error code on failure
 */
static int isp_probe(struct platform_device *pdev)
{
	struct device *dev = &pdev->dev;
	struct isp *isp;
	struct resource *r;
	int ret, i;

	isp = devm_kzalloc(dev, sizeof(*isp), GFP_KERNEL);
	if (!isp)
		return -ENOMEM;

	isp->dev = dev;
	platform_set_drvdata(pdev, isp);

	isp->csiphy_num = 2;
	isp->csiphy = devm_kcalloc(dev, isp->csiphy_num,
				   sizeof(*isp->csiphy), GFP_KERNEL);
	if (!isp->csiphy)
		return -ENOMEM;

	isp->sr = devm_kzalloc(dev, sizeof(*isp->sr), GFP_KERNEL);
	if (!isp->sr)
		return -ENOMEM;

	isp->cvdr = devm_kzalloc(dev, sizeof(*isp->cvdr), GFP_KERNEL);
	if (!isp->cvdr)
		return -ENOMEM;

	r = platform_get_resource_byname(pdev, IORESOURCE_MEM, isp_res[0].reg);
	isp->ispss_ctrl = devm_ioremap_resource(dev, r);
	if (IS_ERR(isp->ispss_ctrl)) {
		dev_err(dev, "could not map ispss_ctrl memory\n");
		ret = PTR_ERR(isp->ispss_ctrl);
		goto err_cleanup;
	}

	r = platform_get_resource_byname(pdev, IORESOURCE_MEM, isp_res[1].reg);
	isp->smmu_ctrl = devm_ioremap_resource(dev, r);
	if (IS_ERR(isp->smmu_ctrl)) {
		dev_err(dev, "could not map smmu_ctrl memory\n");
		ret = PTR_ERR(isp->smmu_ctrl);
		goto err_cleanup;
	}

	r = platform_get_resource_byname(pdev, IORESOURCE_MEM, isp_res[2].reg);
	isp->irq_merger2 = devm_ioremap_resource(dev, r);
	if (IS_ERR(isp->irq_merger2)) {
		dev_err(dev, "could not map irq_merger2 memory\n");
		ret = PTR_ERR(isp->irq_merger2);
		goto err_cleanup;
	}

	isp->clks[0].name = "isp_snclk0";
	isp->clks[0].freq = ISP_CLK_FREQ;
	isp->clks[1].name = "isp_snclk1";
	isp->clks[1].freq = ISP_CLK_FREQ;

	for (i = 0; i < ISP_NUM_CLKS; i++) {
		isp->clks[i].clk = devm_clk_get(&pdev->dev, isp->clks[i].name);
		if (IS_ERR(isp->clks[i].clk)) {
			dev_err(isp->dev, "could not get clock: %s\n",
				isp->clks[i].name);
			ret = -EINVAL;
			goto err_cleanup;
		}

		clk_set_rate(isp->clks[i].clk, isp->clks[i].freq);
	}

        ret = of_property_read_u32(dev->of_node, "pool-size", &isp->pool_size);
	if (ret < 0)
		goto err_cleanup;

	pr_info("%s: %d\n", __func__, __LINE__);

	ret = isp_of_parse_ports(dev, &isp->notifier);
	if (ret < 0) {
		goto err_cleanup;
	}

	pr_info("%s: %d\n", __func__, __LINE__);
	ret = isp_init_subdevices(isp);
	if (ret < 0)
		goto err_cleanup;

	pr_info("%s: %d\n", __func__, __LINE__);
	isp->media_dev.dev = isp->dev;
	strlcpy(isp->media_dev.model, "HiSilicon ISP",
		sizeof(isp->media_dev.model));
	isp->media_dev.ops = &isp_media_ops;
	media_device_init(&isp->media_dev);

	pr_info("%s: %d\n", __func__, __LINE__);
	/* FIXME: Get these from userspace */
	isp->frame_size = 0x3F4800;
	isp->frame_count = 4; 
	isp->frame_num = 0; 
	isp->v4l2_dev.mdev = &isp->media_dev;
	ret = v4l2_device_register(isp->dev, &isp->v4l2_dev);
	if (ret < 0) {
		dev_err(dev, "Failed to register V4L2 device: %d\n", ret);
		goto err_cleanup;
	}

	pr_info("%s: %d\n", __func__, __LINE__);
	ret = isp_register_entities(isp);
	if (ret < 0)
		goto err_register_entities;

	pr_info("%s: %d\n", __func__, __LINE__);

	if (isp->notifier.num_subdevs) {
	pr_info("%s: %d\n", __func__, __LINE__);
		isp->notifier.ops = &isp_subdev_notifier_ops;

		ret = v4l2_async_notifier_register(&isp->v4l2_dev,
						   &isp->notifier);
		if (ret) {
			dev_err(dev,
				"Failed to register async subdev nodes: %d\n",
				ret);
			goto err_register_subdevs;
		}
	} else {
	pr_info("%s: %d\n", __func__, __LINE__);
		ret = v4l2_device_register_subdev_nodes(&isp->v4l2_dev);
		if (ret < 0) {
			dev_err(dev, "Failed to register subdev nodes: %d\n",
				ret);
			goto err_register_subdevs;
		}

		ret = media_device_register(&isp->media_dev);
		if (ret < 0) {
			dev_err(dev, "Failed to register media device: %d\n",
				ret);
			goto err_register_subdevs;
		}
	}

	pr_info("%s: %d\n", __func__, __LINE__);
	return 0;

err_register_subdevs:
	isp_unregister_entities(isp);
err_register_entities:
	v4l2_device_unregister(&isp->v4l2_dev);
err_cleanup:

	return ret;
}

void isp_delete(struct isp *isp)
{
	v4l2_device_unregister(&isp->v4l2_dev);
	media_device_unregister(&isp->media_dev);
	media_device_cleanup(&isp->media_dev);
}

static int isp_remove(struct platform_device *pdev)
{
	return 0;
}

static const struct of_device_id isp_dt_match[] = {
	{ .compatible = "hisilicon,hisi-isp" },
	{ }
};

MODULE_DEVICE_TABLE(of, isp_dt_match);

static struct platform_driver hisi_isp_driver = {
	.probe = isp_probe,
	.remove = isp_remove,
	.driver = {
		.name = "hisi-isp",
		.of_match_table = isp_dt_match,
	},
};

module_platform_driver(hisi_isp_driver);

MODULE_LICENSE("GPL v2");