diff mbox

[2/2] v4l: cadence: Add Cadence MIPI-CSI2 TX driver

Message ID 20170922114703.30511-3-maxime.ripard@free-electrons.com (mailing list archive)
State New, archived
Headers show

Commit Message

Maxime Ripard Sept. 22, 2017, 11:47 a.m. UTC 
The Cadence MIPI-CSI2 TX controller is an hardware block meant to be used
as a bridge between pixel interfaces and a CSI-2 bus.

It supports operating with an internal or external D-PHY, with up to 4
lanes, or without any D-PHY. The current code only supports the former
case.

While the virtual channel input on the pixel interface can be directly
mapped to CSI2, the datatype input is actually a selection signal (3-bits)
mapping to a table of up to 8 preconfigured datatypes/formats (programmed
at start-up)

The block supports up to 8 input datatypes.

Signed-off-by: Maxime Ripard <maxime.ripard@free-electrons.com>
---
 drivers/media/platform/cadence/Kconfig       |   6 +
 drivers/media/platform/cadence/Makefile      |   1 +
 drivers/media/platform/cadence/cdns-csi2tx.c | 479 +++++++++++++++++++++++++++
 3 files changed, 486 insertions(+)
 create mode 100644 drivers/media/platform/cadence/cdns-csi2tx.c

Comments

Sakari Ailus Sept. 22, 2017, 12:38 p.m. UTC | #1 
Hi Maxime,

On Fri, Sep 22, 2017 at 01:47:03PM +0200, Maxime Ripard wrote:
> The Cadence MIPI-CSI2 TX controller is an hardware block meant to be used
> as a bridge between pixel interfaces and a CSI-2 bus.
> 
> It supports operating with an internal or external D-PHY, with up to 4
> lanes, or without any D-PHY. The current code only supports the former
> case.
> 
> While the virtual channel input on the pixel interface can be directly
> mapped to CSI2, the datatype input is actually a selection signal (3-bits)
> mapping to a table of up to 8 preconfigured datatypes/formats (programmed
> at start-up)
> 
> The block supports up to 8 input datatypes.
> 
> Signed-off-by: Maxime Ripard <maxime.ripard@free-electrons.com>
> ---
>  drivers/media/platform/cadence/Kconfig       |   6 +
>  drivers/media/platform/cadence/Makefile      |   1 +
>  drivers/media/platform/cadence/cdns-csi2tx.c | 479 +++++++++++++++++++++++++++
>  3 files changed, 486 insertions(+)
>  create mode 100644 drivers/media/platform/cadence/cdns-csi2tx.c
> 
> diff --git a/drivers/media/platform/cadence/Kconfig b/drivers/media/platform/cadence/Kconfig
> index d1b6bbb6a0eb..db49328ee8b2 100644
> --- a/drivers/media/platform/cadence/Kconfig
> +++ b/drivers/media/platform/cadence/Kconfig
> @@ -9,4 +9,10 @@ config VIDEO_CADENCE_CSI2RX
>  	depends on VIDEO_V4L2_SUBDEV_API
>  	select V4L2_FWNODE
>  
> +config VIDEO_CADENCE_CSI2TX
> +	tristate "Cadence MIPI-CSI2 TX Controller"
> +	depends on MEDIA_CONTROLLER
> +	depends on VIDEO_V4L2_SUBDEV_API
> +	select V4L2_FWNODE
> +
>  endif
> diff --git a/drivers/media/platform/cadence/Makefile b/drivers/media/platform/cadence/Makefile
> index 99a4086b7448..7fe992273162 100644
> --- a/drivers/media/platform/cadence/Makefile
> +++ b/drivers/media/platform/cadence/Makefile
> @@ -1 +1,2 @@
>  obj-$(CONFIG_VIDEO_CADENCE_CSI2RX)	+= cdns-csi2rx.o
> +obj-$(CONFIG_VIDEO_CADENCE_CSI2TX)	+= cdns-csi2tx.o
> diff --git a/drivers/media/platform/cadence/cdns-csi2tx.c b/drivers/media/platform/cadence/cdns-csi2tx.c
> new file mode 100644
> index 000000000000..859bbce8772b
> --- /dev/null
> +++ b/drivers/media/platform/cadence/cdns-csi2tx.c
> @@ -0,0 +1,479 @@
> +/*
> + * Driver for Cadence MIPI-CSI2 TX Controller
> + *
> + * Copyright (C) 2017 Cadence Design Systems Inc.
> + *
> + * This program is free software; you can redistribute  it and/or modify it
> + * under  the terms of  the GNU General  Public License as published by the
> + * Free Software Foundation;  either version 2 of the  License, or (at your
> + * option) any later version.
> + */
> +
> +#include <linux/atomic.h>
> +#include <linux/clk.h>
> +#include <linux/delay.h>
> +#include <linux/io.h>
> +#include <linux/module.h>
> +#include <linux/of.h>
> +#include <linux/of_graph.h>
> +#include <linux/platform_device.h>
> +#include <linux/slab.h>
> +
> +#include <media/v4l2-ctrls.h>
> +#include <media/v4l2-device.h>
> +#include <media/v4l2-fwnode.h>
> +#include <media/v4l2-subdev.h>
> +
> +#define CSI2TX_DEVICE_CONFIG_REG	0x00
> +
> +#define CSI2TX_CONFIG_REG		0x20
> +#define CSI2TX_CONFIG_CFG_REQ			BIT(2)
> +#define CSI2TX_CONFIG_SRST_REQ			BIT(1)
> +
> +#define CSI2TX_DPHY_CFG_REG		0x28
> +#define CSI2TX_DPHY_CFG_CLK_RESET		BIT(16)
> +#define CSI2TX_DPHY_CFG_LANE_RESET(n)		BIT((n) + 12)
> +#define CSI2TX_DPHY_CFG_MODE_MASK		GENMASK(9, 8)
> +#define CSI2TX_DPHY_CFG_MODE_LPDT		(2 << 8)
> +#define CSI2TX_DPHY_CFG_MODE_HS			(1 << 8)
> +#define CSI2TX_DPHY_CFG_MODE_ULPS		(0 << 8)
> +#define CSI2TX_DPHY_CFG_CLK_ENABLE		BIT(4)
> +#define CSI2TX_DPHY_CFG_LANE_ENABLE(n)		BIT(n)
> +
> +#define CSI2TX_DPHY_CLK_WAKEUP_REG	0x2c
> +#define CSI2TX_DPHY_CLK_WAKEUP_ULPS_CYCLES(n)	((n) & 0xffff)
> +
> +#define CSI2TX_DT_CFG_REG(n)		(0x80 + (n) * 8)
> +#define CSI2TX_DT_CFG_DT(n)			(((n) & 0x3f) << 2)
> +
> +#define CSI2TX_DT_FORMAT_REG(n)		(0x84 + (n) * 8)
> +#define CSI2TX_DT_FORMAT_BYTES_PER_LINE(n)	(((n) & 0xffff) << 16)
> +#define CSI2TX_DT_FORMAT_MAX_LINE_NUM(n)	((n) & 0xffff)
> +
> +#define CSI2TX_STREAM_IF_CFG_REG(n)	(0x100 + (n) * 4)
> +#define CSI2TX_STREAM_IF_CFG_FILL_LEVEL(n)	((n) & 0x1f)
> +
> +#define CSI2TX_STREAMS_MAX	4
> +
> +enum csi2tx_pads {
> +	CSI2TX_PAD_SOURCE,
> +	CSI2TX_PAD_SINK_STREAM0,
> +	CSI2TX_PAD_SINK_STREAM1,
> +	CSI2TX_PAD_SINK_STREAM2,
> +	CSI2TX_PAD_SINK_STREAM3,
> +	CSI2TX_PAD_MAX,
> +};
> +
> +struct csi2tx_fmt {
> +	u32	mbus;
> +	u32	dt;
> +	u32	bpp;
> +};
> +
> +struct csi2tx_priv {
> +	struct device			*dev;
> +	atomic_t			count;
> +
> +	void __iomem			*base;
> +
> +	struct clk			*esc_clk;
> +	struct clk			*p_clk;
> +	struct clk			*pixel_clk[CSI2TX_STREAMS_MAX];
> +
> +	struct v4l2_subdev		subdev;
> +	struct media_pad		pads[CSI2TX_PAD_MAX];
> +	struct v4l2_mbus_framefmt	pad_fmts[CSI2TX_PAD_MAX];
> +
> +	bool				has_internal_dphy;
> +	unsigned int			lanes;
> +	unsigned int			max_lanes;
> +	unsigned int			max_streams;
> +};
> +
> +static struct csi2tx_fmt csi2tx_formats[] = {

const?

> +	{
> +		.mbus	= MEDIA_BUS_FMT_UYVY8_1X16,
> +		.bpp	= 2,
> +		.dt	= 0x1e,
> +	},
> +	{
> +		.mbus	= MEDIA_BUS_FMT_RGB888_1X24,
> +		.bpp	= 3,
> +		.dt	= 0x24,
> +	},
> +};
> +
> +static inline
> +struct csi2tx_priv *v4l2_subdev_to_csi2tx(struct v4l2_subdev *subdev)
> +{
> +	return container_of(subdev, struct csi2tx_priv, subdev);
> +}
> +
> +static void csi2tx_reset(struct csi2tx_priv *csi2tx)
> +{
> +	writel(CSI2TX_CONFIG_SRST_REQ, csi2tx->base + CSI2TX_CONFIG_REG);
> +
> +	usleep_range(10, 20);
> +}
> +
> +static struct csi2tx_fmt *csitx_get_fmt_from_mbus(struct v4l2_mbus_framefmt *mfmt)
> +{
> +	int i;
> +
> +	if (!mfmt)
> +		return NULL;
> +
> +	for (i = 0; i < ARRAY_SIZE(csi2tx_formats); i++)
> +		if (csi2tx_formats[i].mbus == mfmt->code)
> +			return &csi2tx_formats[i];
> +
> +	return NULL;
> +}
> +
> +static int csi2tx_enum_mbus_code(struct v4l2_subdev *subdev,
> +				 struct v4l2_subdev_pad_config *cfg,
> +				 struct v4l2_subdev_mbus_code_enum *code)
> +{
> +	if (code->pad || code->index >= ARRAY_SIZE(csi2tx_formats))
> +		return -EINVAL;
> +
> +	code->code = csi2tx_formats[code->index].mbus;
> +
> +	return 0;
> +}
> +
> +static int csi2tx_get_pad_format(struct v4l2_subdev *subdev,
> +				 struct v4l2_subdev_pad_config *cfg,
> +				 struct v4l2_subdev_format *fmt)
> +{
> +	struct csi2tx_priv *csi2tx = v4l2_subdev_to_csi2tx(subdev);
> +
> +	fmt->format = csi2tx->pad_fmts[fmt->pad];
> +
> +	return 0;
> +}
> +
> +static int csi2tx_set_pad_format(struct v4l2_subdev *subdev,
> +				 struct v4l2_subdev_pad_config *cfg,
> +				 struct v4l2_subdev_format *fmt)
> +{
> +	struct csi2tx_priv *csi2tx = v4l2_subdev_to_csi2tx(subdev);
> +
> +	csi2tx->pad_fmts[fmt->pad] = fmt->format;
> +
> +	return 0;
> +}
> +
> +static const struct v4l2_subdev_pad_ops csi2tx_pad_ops = {
> +	.enum_mbus_code	= csi2tx_enum_mbus_code,
> +	.get_fmt	= csi2tx_get_pad_format,
> +	.set_fmt	= csi2tx_set_pad_format,
> +};
> +
> +static int csi2tx_start(struct csi2tx_priv *csi2tx)
> +{
> +	u32 reg;
> +	int i;
> +
> +	/*
> +	 * We're not the first users, there's no need to enable the
> +	 * whole controller.
> +	 */
> +	if (atomic_inc_return(&csi2tx->count) > 1)
> +		return 0;
> +
> +	csi2tx_reset(csi2tx);
> +
> +	writel(CSI2TX_CONFIG_CFG_REQ, csi2tx->base + CSI2TX_CONFIG_REG);
> +
> +	usleep_range(10, 20);
> +
> +	writel(CSI2TX_DPHY_CLK_WAKEUP_ULPS_CYCLES(32),
> +	       csi2tx->base + CSI2TX_DPHY_CLK_WAKEUP_REG);
> +
> +	/* Put our lanes (clock and data) out of reset */
> +	reg = CSI2TX_DPHY_CFG_CLK_RESET | CSI2TX_DPHY_CFG_MODE_LPDT;
> +	for (i = 0; i < csi2tx->lanes; i++)
> +		reg |= CSI2TX_DPHY_CFG_LANE_RESET(i);
> +	writel(reg, csi2tx->base + CSI2TX_DPHY_CFG_REG);
> +
> +	usleep_range(10, 20);
> +
> +	/* Enable our (clock and data) lanes */
> +	reg |= CSI2TX_DPHY_CFG_CLK_ENABLE;
> +	for (i = 0; i < csi2tx->lanes; i++)
> +		reg |= CSI2TX_DPHY_CFG_LANE_ENABLE(i);
> +	writel(reg, csi2tx->base + CSI2TX_DPHY_CFG_REG);
> +
> +	usleep_range(10, 20);
> +
> +	/* Switch to HS mode */
> +	reg &= ~CSI2TX_DPHY_CFG_MODE_MASK;
> +	writel(reg | CSI2TX_DPHY_CFG_MODE_HS,
> +	       csi2tx->base + CSI2TX_DPHY_CFG_REG);
> +
> +	usleep_range(10, 20);
> +
> +	/*
> +	 * Create a static mapping between the CSI virtual channels
> +	 * and the input streams.

Which virtual channel is used here?

> +	 *
> +	 * This should be enhanced, but v4l2 lacks the support for
> +	 * changing that mapping dynamically.
> +	 */
> +	for (i = CSI2TX_PAD_SINK_STREAM0; i < CSI2TX_PAD_MAX; i++) {
> +		struct v4l2_mbus_framefmt *mfmt = &csi2tx->pad_fmts[i];
> +		struct csi2tx_fmt *fmt = csitx_get_fmt_from_mbus(mfmt);
> +		int stream = i - CSI2TX_PAD_SINK_STREAM0;

unsigned?

> +
> +		/*
> +		 * If no-one set a format, we consider this pad
> +		 * disabled and just skip it.
> +		 */
> +		if (!fmt)

The pad should have a valid format even if the user didn't configure it.

Instead you should use the link state to determine whether the link is
active or not.

> +			continue;
> +
> +		/*
> +		 * We use the stream ID there, but it's wrong.
> +		 *
> +		 * A stream could very well send a data type that is
> +		 * not equal to its stream ID. We need to find a
> +		 * proper way to address it.

Stream IDs will presumably be used in V4L2 for a different purpose. Does
the hardware documentation call them such?

> +		 */
> +		writel(CSI2TX_DT_CFG_DT(fmt->dt),
> +		       csi2tx->base + CSI2TX_DT_CFG_REG(stream));
> +
> +		writel(CSI2TX_DT_FORMAT_BYTES_PER_LINE(mfmt->width * fmt->bpp) |
> +		       CSI2TX_DT_FORMAT_MAX_LINE_NUM(mfmt->height + 1),
> +		       csi2tx->base + CSI2TX_DT_FORMAT_REG(stream));
> +
> +		/*
> +		 * TODO: This needs to be calculated based on the
> +		 * clock rate.

Clock rate of what? Input?

> +		 */
> +		writel(CSI2TX_STREAM_IF_CFG_FILL_LEVEL(4),
> +		       csi2tx->base + CSI2TX_STREAM_IF_CFG_REG(stream));
> +	}
> +
> +	/* Disable the configuration mode */
> +	writel(0, csi2tx->base + CSI2TX_CONFIG_REG);

Shouldn't you start streaming on the downstream sub-device as well?

> +
> +	return 0;
> +}
> +
> +static int csi2tx_stop(struct csi2tx_priv *csi2tx)
> +{
> +	/*
> +	 * Let the last user turn off the lights
> +	 */
> +	if (!atomic_dec_and_test(&csi2tx->count))
> +		return 0;
> +
> +	/* FIXME: Disable the IP here */

Shouldn't this be addressed?

> +
> +	return 0;
> +}
> +
> +static int csi2tx_s_stream(struct v4l2_subdev *subdev, int enable)
> +{
> +	struct csi2tx_priv *csi2tx = v4l2_subdev_to_csi2tx(subdev);
> +	int ret;
> +
> +	if (enable)
> +		ret = csi2tx_start(csi2tx);
> +	else
> +		ret = csi2tx_stop(csi2tx);
> +
> +	return ret;
> +}
> +
> +static const struct v4l2_subdev_video_ops csi2tx_video_ops = {
> +	.s_stream	= csi2tx_s_stream,
> +};
> +
> +static struct v4l2_subdev_ops csi2tx_subdev_ops = {

const?

> +	.pad		= &csi2tx_pad_ops,
> +	.video		= &csi2tx_video_ops,
> +};
> +
> +static int csi2tx_get_resources(struct csi2tx_priv *csi2tx,
> +				struct platform_device *pdev)
> +{
> +	struct resource *res;
> +	u32 reg;
> +	int i;
> +
> +	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
> +	csi2tx->base = devm_ioremap_resource(&pdev->dev, res);
> +	if (IS_ERR(csi2tx->base)) {
> +		dev_err(&pdev->dev, "Couldn't map our registers\n");
> +		return PTR_ERR(csi2tx->base);
> +	}
> +
> +	csi2tx->p_clk = devm_clk_get(&pdev->dev, "p_clk");
> +	if (IS_ERR(csi2tx->p_clk)) {
> +		dev_err(&pdev->dev, "Couldn't get p_clk\n");
> +		return PTR_ERR(csi2tx->p_clk);
> +	}
> +
> +	csi2tx->esc_clk = devm_clk_get(&pdev->dev, "esc_clk");
> +	if (IS_ERR(csi2tx->esc_clk)) {
> +		dev_err(&pdev->dev, "Couldn't get the esc_clk\n");
> +		return PTR_ERR(csi2tx->esc_clk);
> +	}
> +
> +	clk_prepare_enable(csi2tx->p_clk);
> +	reg = readl(csi2tx->base + CSI2TX_DEVICE_CONFIG_REG);
> +	clk_disable_unprepare(csi2tx->p_clk);
> +
> +	csi2tx->max_lanes = (reg & 7);
> +	if (csi2tx->max_lanes > 4) {
> +		dev_err(&pdev->dev, "Invalid number of lanes: %u\n",
> +			csi2tx->max_lanes);
> +		return -EINVAL;
> +	}
> +
> +	csi2tx->max_streams = ((reg >> 4) & 7);
> +	if (csi2tx->max_streams > CSI2TX_STREAMS_MAX) {
> +		dev_err(&pdev->dev, "Invalid number of streams: %u\n",
> +			csi2tx->max_streams);
> +		return -EINVAL;
> +	}
> +
> +	csi2tx->has_internal_dphy = (reg & BIT(3)) ? true : false;
> +
> +	for (i = 0; i < csi2tx->max_streams; i++) {
> +		char clk_name[16];
> +
> +		snprintf(clk_name, sizeof(clk_name), "pixel_if%u_clk", i);
> +		csi2tx->pixel_clk[i] = devm_clk_get(&pdev->dev, clk_name);
> +		if (IS_ERR(csi2tx->pixel_clk[i])) {
> +			dev_err(&pdev->dev, "Couldn't get clock %s\n",
> +				clk_name);
> +			return PTR_ERR(csi2tx->pixel_clk[i]);
> +		}
> +	}
> +
> +	return 0;
> +}
> +
> +static int csi2tx_get_num_lanes(struct device *dev)
> +{
> +	struct v4l2_fwnode_endpoint v4l2_ep;
> +	struct device_node *ep, *np = dev->of_node;

np is only used once as a function argument. I wouldn't use a local
variable for this. Up to you.

> +	int ret = 0;

You could omit initialisation. ret is always assigned below.

> +
> +	ep = of_graph_get_endpoint_by_regs(np, 0, 0);
> +	if (!ep)
> +		return -EINVAL;
> +
> +	ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(ep), &v4l2_ep);
> +	if (ret) {
> +		dev_err(dev, "Could not parse v4l2 endpoint\n");
> +		goto out;
> +	}
> +
> +	if (v4l2_ep.bus_type != V4L2_MBUS_CSI2) {
> +		dev_err(dev, "Unsupported media bus type: 0x%x\n",
> +			v4l2_ep.bus_type);
> +		ret = -EINVAL;
> +		goto out;
> +	}
> +
> +	ret = v4l2_ep.bus.mipi_csi2.num_data_lanes;
> +
> +out:
> +	of_node_put(ep);
> +	return ret;
> +}
> +
> +static int csi2tx_probe(struct platform_device *pdev)
> +{
> +	struct csi2tx_priv *csi2tx;
> +	int i, ret;
> +
> +	csi2tx = kzalloc(sizeof(*csi2tx), GFP_KERNEL);
> +	if (!csi2tx)
> +		return -ENOMEM;
> +	platform_set_drvdata(pdev, csi2tx);
> +	csi2tx->dev = &pdev->dev;
> +
> +	ret = csi2tx_get_resources(csi2tx, pdev);
> +	if (ret)
> +		goto err_free_priv;
> +
> +	v4l2_subdev_init(&csi2tx->subdev, &csi2tx_subdev_ops);
> +	csi2tx->subdev.owner = THIS_MODULE;
> +	csi2tx->subdev.dev = &pdev->dev;
> +	csi2tx->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
> +	v4l2_set_subdevdata(&csi2tx->subdev, &pdev->dev);
> +	snprintf(csi2tx->subdev.name, V4L2_SUBDEV_NAME_SIZE, "%s.%s",
> +		 KBUILD_MODNAME, dev_name(&pdev->dev));
> +
> +	csi2tx->lanes = csi2tx_get_num_lanes(&pdev->dev);
> +	if (csi2tx->lanes < 0) {
> +		dev_err(&pdev->dev, "Invalid number of lanes, bailing out.\n");
> +		ret = csi2tx->lanes;
> +		goto err_free_priv;
> +	}
> +
> +	if (csi2tx->lanes > csi2tx->max_lanes) {
> +		dev_err(&pdev->dev,
> +			"Current configuration uses more lanes than supported\n");
> +		ret = -EINVAL;
> +		goto err_free_priv;
> +	}
> +
> +	/* Create our media pads */
> +	csi2tx->subdev.entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
> +	csi2tx->pads[CSI2TX_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
> +	for (i = CSI2TX_PAD_SINK_STREAM0; i < CSI2TX_PAD_MAX; i++)
> +		csi2tx->pads[i].flags = MEDIA_PAD_FL_SINK;
> +
> +	ret = media_entity_pads_init(&csi2tx->subdev.entity, CSI2TX_PAD_MAX,
> +				     csi2tx->pads);
> +	if (ret)
> +		return ret;
> +
> +	ret = v4l2_async_register_subdev(&csi2tx->subdev);
> +	if (ret < 0)
> +		goto err_free_priv;
> +
> +	dev_info(&pdev->dev,
> +		 "Probed CSI2TX with %u/%u lanes, %u streams, %s D-PHY\n",
> +		 csi2tx->lanes, csi2tx->max_lanes, csi2tx->max_streams,
> +		 csi2tx->has_internal_dphy ? "internal" : "no");
> +
> +	return 0;
> +
> +err_free_priv:
> +	kfree(csi2tx);
> +	return ret;
> +}
> +
> +static int csi2tx_remove(struct platform_device *pdev)
> +{
> +	struct csi2tx_priv *csi2tx = platform_get_drvdata(pdev);
> +
> +	v4l2_async_unregister_subdev(&csi2tx->subdev);
> +	kfree(csi2tx);
> +
> +	return 0;
> +}
> +
> +static const struct of_device_id csi2tx_of_table[] = {
> +	{ .compatible = "cdns,csi2tx" },
> +	{ },
> +};
> +MODULE_DEVICE_TABLE(of, csi2tx_of_table);
> +
> +static struct platform_driver csi2tx_driver = {
> +	.probe	= csi2tx_probe,
> +	.remove	= csi2tx_remove,
> +
> +	.driver	= {
> +		.name		= "cdns-csi2tx",
> +		.of_match_table	= csi2tx_of_table,
> +	},
> +};
> +module_platform_driver(csi2tx_driver);
Maxime Ripard Sept. 22, 2017, 3:30 p.m. UTC | #2 
Hi Sakari,

I'll address the minor comments you had and that I stripped.

On Fri, Sep 22, 2017 at 12:38:49PM +0000, Sakari Ailus wrote:
> > +	/*
> > +	 * Create a static mapping between the CSI virtual channels
> > +	 * and the input streams.
> 
> Which virtual channel is used here?

Like I was trying to explain in the cover letter, the virtual channel
is not under that block's control. The input video interfaces have an
additional signal that comes from the upstream device which sets the
virtual channel.

It's transparent to the CSI2-TX block, even though it's
there. Depending on the implementation, it can be either fixed or can
change, it's up to the other block's designer. The only restriction is
that it cannot change while a streaming is occuring.

> > +
> > +		/*
> > +		 * If no-one set a format, we consider this pad
> > +		 * disabled and just skip it.
> > +		 */
> > +		if (!fmt)
> 
> The pad should have a valid format even if the user didn't configure
> it.

Which format should be by default then?

> Instead you should use the link state to determine whether the link is
> active or not.

Ok, will do.

> > +			continue;
> > +
> > +		/*
> > +		 * We use the stream ID there, but it's wrong.
> > +		 *
> > +		 * A stream could very well send a data type that is
> > +		 * not equal to its stream ID. We need to find a
> > +		 * proper way to address it.
> 
> Stream IDs will presumably be used in V4L2 for a different purpose. Does
> the hardware documentation call them such?

Input video interfaces are called streams, yes, and then they are
numbered. If it's just confusing because of a collision with one of
v4l2's nomenclature, I'm totally fine to change it to some other name.

> > +		 */
> > +		writel(CSI2TX_DT_CFG_DT(fmt->dt),
> > +		       csi2tx->base + CSI2TX_DT_CFG_REG(stream));
> > +
> > +		writel(CSI2TX_DT_FORMAT_BYTES_PER_LINE(mfmt->width * fmt->bpp) |
> > +		       CSI2TX_DT_FORMAT_MAX_LINE_NUM(mfmt->height + 1),
> > +		       csi2tx->base + CSI2TX_DT_FORMAT_REG(stream));
> > +
> > +		/*
> > +		 * TODO: This needs to be calculated based on the
> > +		 * clock rate.
> 
> Clock rate of what? Input?

Of the CSI2 link, so output. I guess I should make that clearer.

> 
> > +		 */
> > +		writel(CSI2TX_STREAM_IF_CFG_FILL_LEVEL(4),
> > +		       csi2tx->base + CSI2TX_STREAM_IF_CFG_REG(stream));
> > +	}
> > +
> > +	/* Disable the configuration mode */
> > +	writel(0, csi2tx->base + CSI2TX_CONFIG_REG);
> 
> Shouldn't you start streaming on the downstream sub-device as well?

I appreciate it's a pretty weak argument, but the current setup we
have is in the FPGA is:

capture <- CSI2-RX <- CSI2-TX <- pattern generator

So far, the CSI2-RX block is calling its remote sub-device, which is
CSI2-TX. If CSI2-RX is calling its remote sub-device (CSI2-RX), we
just found ourselves in an endless loop.

I guess it should be easier, and fixable, when we'll have an actual
device without such a loopback.

> > +
> > +	return 0;
> > +}
> > +
> > +static int csi2tx_stop(struct csi2tx_priv *csi2tx)
> > +{
> > +	/*
> > +	 * Let the last user turn off the lights
> > +	 */
> > +	if (!atomic_dec_and_test(&csi2tx->count))
> > +		return 0;
> > +
> > +	/* FIXME: Disable the IP here */
> 
> Shouldn't this be addressed?

Yes, but it's still unclear how at the moment. It will of course
eventually be implemented.

Thanks!
Maxime
Sakari Ailus Sept. 26, 2017, 8 a.m. UTC | #3 
Hi Maxime,

On Fri, Sep 22, 2017 at 05:30:36PM +0200, Maxime Ripard wrote:
> Hi Sakari,
> 
> I'll address the minor comments you had and that I stripped.
> 
> On Fri, Sep 22, 2017 at 12:38:49PM +0000, Sakari Ailus wrote:
> > > +	/*
> > > +	 * Create a static mapping between the CSI virtual channels
> > > +	 * and the input streams.
> > 
> > Which virtual channel is used here?
> 
> Like I was trying to explain in the cover letter, the virtual channel
> is not under that block's control. The input video interfaces have an
> additional signal that comes from the upstream device which sets the
> virtual channel.

Oh, I missed while reviewing the set.

Presumably either driver would be in control of that then (this one or the
upstream sub-device).

> 
> It's transparent to the CSI2-TX block, even though it's
> there. Depending on the implementation, it can be either fixed or can
> change, it's up to the other block's designer. The only restriction is
> that it cannot change while a streaming is occuring.
> 
> > > +
> > > +		/*
> > > +		 * If no-one set a format, we consider this pad
> > > +		 * disabled and just skip it.
> > > +		 */
> > > +		if (!fmt)
> > 
> > The pad should have a valid format even if the user didn't configure
> > it.
> 
> Which format should be by default then?

Any valid format for the device should be good.

> 
> > Instead you should use the link state to determine whether the link is
> > active or not.
> 
> Ok, will do.
> 
> > > +			continue;
> > > +
> > > +		/*
> > > +		 * We use the stream ID there, but it's wrong.
> > > +		 *
> > > +		 * A stream could very well send a data type that is
> > > +		 * not equal to its stream ID. We need to find a
> > > +		 * proper way to address it.
> > 
> > Stream IDs will presumably be used in V4L2 for a different purpose. Does
> > the hardware documentation call them such?
> 
> Input video interfaces are called streams, yes, and then they are
> numbered. If it's just confusing because of a collision with one of
> v4l2's nomenclature, I'm totally fine to change it to some other name.

If the hardware documentation uses it then let's stick with it.

> 
> > > +		 */
> > > +		writel(CSI2TX_DT_CFG_DT(fmt->dt),
> > > +		       csi2tx->base + CSI2TX_DT_CFG_REG(stream));
> > > +
> > > +		writel(CSI2TX_DT_FORMAT_BYTES_PER_LINE(mfmt->width * fmt->bpp) |
> > > +		       CSI2TX_DT_FORMAT_MAX_LINE_NUM(mfmt->height + 1),
> > > +		       csi2tx->base + CSI2TX_DT_FORMAT_REG(stream));
> > > +
> > > +		/*
> > > +		 * TODO: This needs to be calculated based on the
> > > +		 * clock rate.
> > 
> > Clock rate of what? Input?
> 
> Of the CSI2 link, so output. I guess I should make that clearer.

Please.

> 
> > 
> > > +		 */
> > > +		writel(CSI2TX_STREAM_IF_CFG_FILL_LEVEL(4),
> > > +		       csi2tx->base + CSI2TX_STREAM_IF_CFG_REG(stream));
> > > +	}
> > > +
> > > +	/* Disable the configuration mode */
> > > +	writel(0, csi2tx->base + CSI2TX_CONFIG_REG);
> > 
> > Shouldn't you start streaming on the downstream sub-device as well?
> 
> I appreciate it's a pretty weak argument, but the current setup we
> have is in the FPGA is:
> 
> capture <- CSI2-RX <- CSI2-TX <- pattern generator
> 
> So far, the CSI2-RX block is calling its remote sub-device, which is
> CSI2-TX. If CSI2-RX is calling its remote sub-device (CSI2-RX), we
> just found ourselves in an endless loop.
> 
> I guess it should be easier, and fixable, when we'll have an actual
> device without such a loopback.

What's the intended use case of the device, capture or output?

How do you currently start the pipeline?

We have a few corner cases in V4L2 for such devices in graph parsing and
stream control. The parsing of the device's fwnode graph endpoints are what
the device can do, but it doesn't know where the parsing should continue
and which part of the graph is already parsed.

That will be addressed but right now a driver just needs to know.

> 
> > > +
> > > +	return 0;
> > > +}
> > > +
> > > +static int csi2tx_stop(struct csi2tx_priv *csi2tx)
> > > +{
> > > +	/*
> > > +	 * Let the last user turn off the lights
> > > +	 */
> > > +	if (!atomic_dec_and_test(&csi2tx->count))
> > > +		return 0;
> > > +
> > > +	/* FIXME: Disable the IP here */
> > 
> > Shouldn't this be addressed?
> 
> Yes, but it's still unclear how at the moment. It will of course
> eventually be implemented.

Ok.
Benoit Parrot Sept. 29, 2017, 6:21 p.m. UTC | #4 
Maxime,

Thank you for the patch.

Maxime Ripard <maxime.ripard@free-electrons.com> wrote on Fri [2017-Sep-22 13:47:03 +0200]:
> The Cadence MIPI-CSI2 TX controller is an hardware block meant to be used
> as a bridge between pixel interfaces and a CSI-2 bus.
> 
> It supports operating with an internal or external D-PHY, with up to 4
> lanes, or without any D-PHY. The current code only supports the former
> case.
> 
> While the virtual channel input on the pixel interface can be directly
> mapped to CSI2, the datatype input is actually a selection signal (3-bits)
> mapping to a table of up to 8 preconfigured datatypes/formats (programmed
> at start-up)
> 
> The block supports up to 8 input datatypes.
> 
> Signed-off-by: Maxime Ripard <maxime.ripard@free-electrons.com>
> ---
>  drivers/media/platform/cadence/Kconfig       |   6 +
>  drivers/media/platform/cadence/Makefile      |   1 +
>  drivers/media/platform/cadence/cdns-csi2tx.c | 479 +++++++++++++++++++++++++++
>  3 files changed, 486 insertions(+)
>  create mode 100644 drivers/media/platform/cadence/cdns-csi2tx.c
> 
> diff --git a/drivers/media/platform/cadence/Kconfig b/drivers/media/platform/cadence/Kconfig
> index d1b6bbb6a0eb..db49328ee8b2 100644
> --- a/drivers/media/platform/cadence/Kconfig
> +++ b/drivers/media/platform/cadence/Kconfig
> @@ -9,4 +9,10 @@ config VIDEO_CADENCE_CSI2RX
>  	depends on VIDEO_V4L2_SUBDEV_API
>  	select V4L2_FWNODE
>  
> +config VIDEO_CADENCE_CSI2TX
> +	tristate "Cadence MIPI-CSI2 TX Controller"
> +	depends on MEDIA_CONTROLLER
> +	depends on VIDEO_V4L2_SUBDEV_API
> +	select V4L2_FWNODE
> +
>  endif
> diff --git a/drivers/media/platform/cadence/Makefile b/drivers/media/platform/cadence/Makefile
> index 99a4086b7448..7fe992273162 100644
> --- a/drivers/media/platform/cadence/Makefile
> +++ b/drivers/media/platform/cadence/Makefile
> @@ -1 +1,2 @@
>  obj-$(CONFIG_VIDEO_CADENCE_CSI2RX)	+= cdns-csi2rx.o
> +obj-$(CONFIG_VIDEO_CADENCE_CSI2TX)	+= cdns-csi2tx.o
> diff --git a/drivers/media/platform/cadence/cdns-csi2tx.c b/drivers/media/platform/cadence/cdns-csi2tx.c
> new file mode 100644
> index 000000000000..859bbce8772b
> --- /dev/null
> +++ b/drivers/media/platform/cadence/cdns-csi2tx.c
> @@ -0,0 +1,479 @@
> +/*
> + * Driver for Cadence MIPI-CSI2 TX Controller
> + *
> + * Copyright (C) 2017 Cadence Design Systems Inc.
> + *
> + * This program is free software; you can redistribute  it and/or modify it
> + * under  the terms of  the GNU General  Public License as published by the
> + * Free Software Foundation;  either version 2 of the  License, or (at your
> + * option) any later version.
> + */
> +
> +#include <linux/atomic.h>
> +#include <linux/clk.h>
> +#include <linux/delay.h>
> +#include <linux/io.h>
> +#include <linux/module.h>
> +#include <linux/of.h>
> +#include <linux/of_graph.h>
> +#include <linux/platform_device.h>
> +#include <linux/slab.h>
> +
> +#include <media/v4l2-ctrls.h>
> +#include <media/v4l2-device.h>
> +#include <media/v4l2-fwnode.h>
> +#include <media/v4l2-subdev.h>
> +
> +#define CSI2TX_DEVICE_CONFIG_REG	0x00
> +
> +#define CSI2TX_CONFIG_REG		0x20
> +#define CSI2TX_CONFIG_CFG_REQ			BIT(2)
> +#define CSI2TX_CONFIG_SRST_REQ			BIT(1)
> +
> +#define CSI2TX_DPHY_CFG_REG		0x28
> +#define CSI2TX_DPHY_CFG_CLK_RESET		BIT(16)
> +#define CSI2TX_DPHY_CFG_LANE_RESET(n)		BIT((n) + 12)
> +#define CSI2TX_DPHY_CFG_MODE_MASK		GENMASK(9, 8)
> +#define CSI2TX_DPHY_CFG_MODE_LPDT		(2 << 8)
> +#define CSI2TX_DPHY_CFG_MODE_HS			(1 << 8)
> +#define CSI2TX_DPHY_CFG_MODE_ULPS		(0 << 8)
> +#define CSI2TX_DPHY_CFG_CLK_ENABLE		BIT(4)
> +#define CSI2TX_DPHY_CFG_LANE_ENABLE(n)		BIT(n)
> +
> +#define CSI2TX_DPHY_CLK_WAKEUP_REG	0x2c
> +#define CSI2TX_DPHY_CLK_WAKEUP_ULPS_CYCLES(n)	((n) & 0xffff)
> +
> +#define CSI2TX_DT_CFG_REG(n)		(0x80 + (n) * 8)
> +#define CSI2TX_DT_CFG_DT(n)			(((n) & 0x3f) << 2)
> +
> +#define CSI2TX_DT_FORMAT_REG(n)		(0x84 + (n) * 8)
> +#define CSI2TX_DT_FORMAT_BYTES_PER_LINE(n)	(((n) & 0xffff) << 16)
> +#define CSI2TX_DT_FORMAT_MAX_LINE_NUM(n)	((n) & 0xffff)
> +
> +#define CSI2TX_STREAM_IF_CFG_REG(n)	(0x100 + (n) * 4)
> +#define CSI2TX_STREAM_IF_CFG_FILL_LEVEL(n)	((n) & 0x1f)
> +
> +#define CSI2TX_STREAMS_MAX	4
> +
> +enum csi2tx_pads {
> +	CSI2TX_PAD_SOURCE,
> +	CSI2TX_PAD_SINK_STREAM0,
> +	CSI2TX_PAD_SINK_STREAM1,
> +	CSI2TX_PAD_SINK_STREAM2,
> +	CSI2TX_PAD_SINK_STREAM3,
> +	CSI2TX_PAD_MAX,
> +};
> +
> +struct csi2tx_fmt {
> +	u32	mbus;
> +	u32	dt;
> +	u32	bpp;
> +};
> +
> +struct csi2tx_priv {
> +	struct device			*dev;
> +	atomic_t			count;
> +
> +	void __iomem			*base;
> +
> +	struct clk			*esc_clk;
> +	struct clk			*p_clk;
> +	struct clk			*pixel_clk[CSI2TX_STREAMS_MAX];
> +
> +	struct v4l2_subdev		subdev;
> +	struct media_pad		pads[CSI2TX_PAD_MAX];
> +	struct v4l2_mbus_framefmt	pad_fmts[CSI2TX_PAD_MAX];
> +
> +	bool				has_internal_dphy;

I assume dphy support is for a subsequent revision?

> +	unsigned int			lanes;
> +	unsigned int			max_lanes;
> +	unsigned int			max_streams;
> +};
> +
> +static struct csi2tx_fmt csi2tx_formats[] = {
> +	{
> +		.mbus	= MEDIA_BUS_FMT_UYVY8_1X16,
> +		.bpp	= 2,
> +		.dt	= 0x1e,
> +	},
> +	{
> +		.mbus	= MEDIA_BUS_FMT_RGB888_1X24,
> +		.bpp	= 3,
> +		.dt	= 0x24,
> +	},
> +};
> +
> +static inline
> +struct csi2tx_priv *v4l2_subdev_to_csi2tx(struct v4l2_subdev *subdev)
> +{
> +	return container_of(subdev, struct csi2tx_priv, subdev);
> +}
> +
> +static void csi2tx_reset(struct csi2tx_priv *csi2tx)
> +{
> +	writel(CSI2TX_CONFIG_SRST_REQ, csi2tx->base + CSI2TX_CONFIG_REG);
> +
> +	usleep_range(10, 20);
> +}
> +
> +static struct csi2tx_fmt *csitx_get_fmt_from_mbus(struct v4l2_mbus_framefmt *mfmt)
> +{
> +	int i;
> +
> +	if (!mfmt)
> +		return NULL;
> +
> +	for (i = 0; i < ARRAY_SIZE(csi2tx_formats); i++)
> +		if (csi2tx_formats[i].mbus == mfmt->code)
> +			return &csi2tx_formats[i];
> +
> +	return NULL;
> +}
> +
> +static int csi2tx_enum_mbus_code(struct v4l2_subdev *subdev,
> +				 struct v4l2_subdev_pad_config *cfg,
> +				 struct v4l2_subdev_mbus_code_enum *code)
> +{
> +	if (code->pad || code->index >= ARRAY_SIZE(csi2tx_formats))
> +		return -EINVAL;
> +
> +	code->code = csi2tx_formats[code->index].mbus;
> +
> +	return 0;
> +}
> +
> +static int csi2tx_get_pad_format(struct v4l2_subdev *subdev,
> +				 struct v4l2_subdev_pad_config *cfg,
> +				 struct v4l2_subdev_format *fmt)
> +{
> +	struct csi2tx_priv *csi2tx = v4l2_subdev_to_csi2tx(subdev);
> +
> +	fmt->format = csi2tx->pad_fmts[fmt->pad];
> +
> +	return 0;
> +}
> +
> +static int csi2tx_set_pad_format(struct v4l2_subdev *subdev,
> +				 struct v4l2_subdev_pad_config *cfg,
> +				 struct v4l2_subdev_format *fmt)
> +{
> +	struct csi2tx_priv *csi2tx = v4l2_subdev_to_csi2tx(subdev);
> +
> +	csi2tx->pad_fmts[fmt->pad] = fmt->format;
> +
> +	return 0;
> +}
> +
> +static const struct v4l2_subdev_pad_ops csi2tx_pad_ops = {
> +	.enum_mbus_code	= csi2tx_enum_mbus_code,
> +	.get_fmt	= csi2tx_get_pad_format,
> +	.set_fmt	= csi2tx_set_pad_format,
> +};
> +
> +static int csi2tx_start(struct csi2tx_priv *csi2tx)
> +{
> +	u32 reg;
> +	int i;
> +
> +	/*
> +	 * We're not the first users, there's no need to enable the
> +	 * whole controller.
> +	 */
> +	if (atomic_inc_return(&csi2tx->count) > 1)
> +		return 0;
> +
> +	csi2tx_reset(csi2tx);
> +
> +	writel(CSI2TX_CONFIG_CFG_REQ, csi2tx->base + CSI2TX_CONFIG_REG);
> +
> +	usleep_range(10, 20);
> +
> +	writel(CSI2TX_DPHY_CLK_WAKEUP_ULPS_CYCLES(32),
> +	       csi2tx->base + CSI2TX_DPHY_CLK_WAKEUP_REG);
> +
> +	/* Put our lanes (clock and data) out of reset */
> +	reg = CSI2TX_DPHY_CFG_CLK_RESET | CSI2TX_DPHY_CFG_MODE_LPDT;
> +	for (i = 0; i < csi2tx->lanes; i++)
> +		reg |= CSI2TX_DPHY_CFG_LANE_RESET(i);
> +	writel(reg, csi2tx->base + CSI2TX_DPHY_CFG_REG);
> +
> +	usleep_range(10, 20);
> +
> +	/* Enable our (clock and data) lanes */
> +	reg |= CSI2TX_DPHY_CFG_CLK_ENABLE;
> +	for (i = 0; i < csi2tx->lanes; i++)
> +		reg |= CSI2TX_DPHY_CFG_LANE_ENABLE(i);
> +	writel(reg, csi2tx->base + CSI2TX_DPHY_CFG_REG);
> +
> +	usleep_range(10, 20);
> +
> +	/* Switch to HS mode */
> +	reg &= ~CSI2TX_DPHY_CFG_MODE_MASK;
> +	writel(reg | CSI2TX_DPHY_CFG_MODE_HS,
> +	       csi2tx->base + CSI2TX_DPHY_CFG_REG);
> +
> +	usleep_range(10, 20);
> +
> +	/*
> +	 * Create a static mapping between the CSI virtual channels
> +	 * and the input streams.
> +	 *
> +	 * This should be enhanced, but v4l2 lacks the support for
> +	 * changing that mapping dynamically.
> +	 */
> +	for (i = CSI2TX_PAD_SINK_STREAM0; i < CSI2TX_PAD_MAX; i++) {
> +		struct v4l2_mbus_framefmt *mfmt = &csi2tx->pad_fmts[i];
> +		struct csi2tx_fmt *fmt = csitx_get_fmt_from_mbus(mfmt);
> +		int stream = i - CSI2TX_PAD_SINK_STREAM0;
> +
> +		/*
> +		 * If no-one set a format, we consider this pad
> +		 * disabled and just skip it.
> +		 */
> +		if (!fmt)
> +			continue;
> +

As Sakari already pointed out setting a valid default is the
usual practice.

> +		/*
> +		 * We use the stream ID there, but it's wrong.
> +		 *
> +		 * A stream could very well send a data type that is
> +		 * not equal to its stream ID. We need to find a
> +		 * proper way to address it.
> +		 */

I don't quite get the above comment, from the code below it looks like
you are using the current fmt as a source to provide the correct DT.
Am I missing soemthing?

> +		writel(CSI2TX_DT_CFG_DT(fmt->dt),
> +		       csi2tx->base + CSI2TX_DT_CFG_REG(stream));
> +
> +		writel(CSI2TX_DT_FORMAT_BYTES_PER_LINE(mfmt->width * fmt->bpp) |
> +		       CSI2TX_DT_FORMAT_MAX_LINE_NUM(mfmt->height + 1),
> +		       csi2tx->base + CSI2TX_DT_FORMAT_REG(stream));
> +
> +		/*
> +		 * TODO: This needs to be calculated based on the
> +		 * clock rate.
> +		 */
> +		writel(CSI2TX_STREAM_IF_CFG_FILL_LEVEL(4),
> +		       csi2tx->base + CSI2TX_STREAM_IF_CFG_REG(stream));
> +	}
> +
> +	/* Disable the configuration mode */
> +	writel(0, csi2tx->base + CSI2TX_CONFIG_REG);
> +
> +	return 0;
> +}
> +
> +static int csi2tx_stop(struct csi2tx_priv *csi2tx)
> +{
> +	/*
> +	 * Let the last user turn off the lights
> +	 */
> +	if (!atomic_dec_and_test(&csi2tx->count))
> +		return 0;
> +
> +	/* FIXME: Disable the IP here */
> +
> +	return 0;
> +}

At this point both _start() and _stop() only return 0.
Are there any cases where any of these might fail to "start" or "stop"?

> +
> +static int csi2tx_s_stream(struct v4l2_subdev *subdev, int enable)
> +{
> +	struct csi2tx_priv *csi2tx = v4l2_subdev_to_csi2tx(subdev);
> +	int ret;
> +
> +	if (enable)
> +		ret = csi2tx_start(csi2tx);
> +	else
> +		ret = csi2tx_stop(csi2tx);
> +
> +	return ret;
> +}
> +
> +static const struct v4l2_subdev_video_ops csi2tx_video_ops = {
> +	.s_stream	= csi2tx_s_stream,
> +};
> +
> +static struct v4l2_subdev_ops csi2tx_subdev_ops = {
> +	.pad		= &csi2tx_pad_ops,
> +	.video		= &csi2tx_video_ops,
> +};
> +
> +static int csi2tx_get_resources(struct csi2tx_priv *csi2tx,
> +				struct platform_device *pdev)
> +{
> +	struct resource *res;
> +	u32 reg;
> +	int i;
> +
> +	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
> +	csi2tx->base = devm_ioremap_resource(&pdev->dev, res);
> +	if (IS_ERR(csi2tx->base)) {
> +		dev_err(&pdev->dev, "Couldn't map our registers\n");
> +		return PTR_ERR(csi2tx->base);
> +	}
> +
> +	csi2tx->p_clk = devm_clk_get(&pdev->dev, "p_clk");
> +	if (IS_ERR(csi2tx->p_clk)) {
> +		dev_err(&pdev->dev, "Couldn't get p_clk\n");
> +		return PTR_ERR(csi2tx->p_clk);
> +	}
> +
> +	csi2tx->esc_clk = devm_clk_get(&pdev->dev, "esc_clk");
> +	if (IS_ERR(csi2tx->esc_clk)) {
> +		dev_err(&pdev->dev, "Couldn't get the esc_clk\n");
> +		return PTR_ERR(csi2tx->esc_clk);
> +	}
> +
> +	clk_prepare_enable(csi2tx->p_clk);
> +	reg = readl(csi2tx->base + CSI2TX_DEVICE_CONFIG_REG);
> +	clk_disable_unprepare(csi2tx->p_clk);
> +
> +	csi2tx->max_lanes = (reg & 7);
> +	if (csi2tx->max_lanes > 4) {
> +		dev_err(&pdev->dev, "Invalid number of lanes: %u\n",
> +			csi2tx->max_lanes);
> +		return -EINVAL;
> +	}
> +
> +	csi2tx->max_streams = ((reg >> 4) & 7);
> +	if (csi2tx->max_streams > CSI2TX_STREAMS_MAX) {
> +		dev_err(&pdev->dev, "Invalid number of streams: %u\n",
> +			csi2tx->max_streams);
> +		return -EINVAL;
> +	}
> +
> +	csi2tx->has_internal_dphy = (reg & BIT(3)) ? true : false;
> +
> +	for (i = 0; i < csi2tx->max_streams; i++) {
> +		char clk_name[16];
> +
> +		snprintf(clk_name, sizeof(clk_name), "pixel_if%u_clk", i);
> +		csi2tx->pixel_clk[i] = devm_clk_get(&pdev->dev, clk_name);
> +		if (IS_ERR(csi2tx->pixel_clk[i])) {
> +			dev_err(&pdev->dev, "Couldn't get clock %s\n",
> +				clk_name);
> +			return PTR_ERR(csi2tx->pixel_clk[i]);
> +		}
> +	}
> +
> +	return 0;
> +}
> +
> +static int csi2tx_get_num_lanes(struct device *dev)
> +{
> +	struct v4l2_fwnode_endpoint v4l2_ep;
> +	struct device_node *ep, *np = dev->of_node;
> +	int ret = 0;
> +
> +	ep = of_graph_get_endpoint_by_regs(np, 0, 0);
> +	if (!ep)
> +		return -EINVAL;
> +
> +	ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(ep), &v4l2_ep);
> +	if (ret) {
> +		dev_err(dev, "Could not parse v4l2 endpoint\n");
> +		goto out;
> +	}
> +
> +	if (v4l2_ep.bus_type != V4L2_MBUS_CSI2) {
> +		dev_err(dev, "Unsupported media bus type: 0x%x\n",
> +			v4l2_ep.bus_type);
> +		ret = -EINVAL;
> +		goto out;
> +	}
> +
> +	ret = v4l2_ep.bus.mipi_csi2.num_data_lanes;
> +
> +out:
> +	of_node_put(ep);
> +	return ret;
> +}
> +
> +static int csi2tx_probe(struct platform_device *pdev)
> +{
> +	struct csi2tx_priv *csi2tx;
> +	int i, ret;
> +
> +	csi2tx = kzalloc(sizeof(*csi2tx), GFP_KERNEL);
> +	if (!csi2tx)
> +		return -ENOMEM;
> +	platform_set_drvdata(pdev, csi2tx);
> +	csi2tx->dev = &pdev->dev;
> +
> +	ret = csi2tx_get_resources(csi2tx, pdev);
> +	if (ret)
> +		goto err_free_priv;
> +
> +	v4l2_subdev_init(&csi2tx->subdev, &csi2tx_subdev_ops);
> +	csi2tx->subdev.owner = THIS_MODULE;
> +	csi2tx->subdev.dev = &pdev->dev;
> +	csi2tx->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
> +	v4l2_set_subdevdata(&csi2tx->subdev, &pdev->dev);
> +	snprintf(csi2tx->subdev.name, V4L2_SUBDEV_NAME_SIZE, "%s.%s",
> +		 KBUILD_MODNAME, dev_name(&pdev->dev));
> +
> +	csi2tx->lanes = csi2tx_get_num_lanes(&pdev->dev);
> +	if (csi2tx->lanes < 0) {
> +		dev_err(&pdev->dev, "Invalid number of lanes, bailing out.\n");
> +		ret = csi2tx->lanes;
> +		goto err_free_priv;
> +	}

csi2tx->lanes is unsigned so it will never be negative.

> +
> +	if (csi2tx->lanes > csi2tx->max_lanes) {
> +		dev_err(&pdev->dev,
> +			"Current configuration uses more lanes than supported\n");
> +		ret = -EINVAL;
> +		goto err_free_priv;
> +	}
> +
> +	/* Create our media pads */
> +	csi2tx->subdev.entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
> +	csi2tx->pads[CSI2TX_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
> +	for (i = CSI2TX_PAD_SINK_STREAM0; i < CSI2TX_PAD_MAX; i++)
> +		csi2tx->pads[i].flags = MEDIA_PAD_FL_SINK;
> +
> +	ret = media_entity_pads_init(&csi2tx->subdev.entity, CSI2TX_PAD_MAX,
> +				     csi2tx->pads);
> +	if (ret)
> +		return ret;
> +
> +	ret = v4l2_async_register_subdev(&csi2tx->subdev);
> +	if (ret < 0)
> +		goto err_free_priv;
> +
> +	dev_info(&pdev->dev,
> +		 "Probed CSI2TX with %u/%u lanes, %u streams, %s D-PHY\n",
> +		 csi2tx->lanes, csi2tx->max_lanes, csi2tx->max_streams,
> +		 csi2tx->has_internal_dphy ? "internal" : "no");
> +
> +	return 0;
> +
> +err_free_priv:
> +	kfree(csi2tx);
> +	return ret;
> +}
> +
> +static int csi2tx_remove(struct platform_device *pdev)
> +{
> +	struct csi2tx_priv *csi2tx = platform_get_drvdata(pdev);
> +
> +	v4l2_async_unregister_subdev(&csi2tx->subdev);
> +	kfree(csi2tx);
> +
> +	return 0;
> +}
> +
> +static const struct of_device_id csi2tx_of_table[] = {
> +	{ .compatible = "cdns,csi2tx" },
> +	{ },
> +};
> +MODULE_DEVICE_TABLE(of, csi2tx_of_table);
> +
> +static struct platform_driver csi2tx_driver = {
> +	.probe	= csi2tx_probe,
> +	.remove	= csi2tx_remove,
> +
> +	.driver	= {
> +		.name		= "cdns-csi2tx",
> +		.of_match_table	= csi2tx_of_table,
> +	},
> +};
> +module_platform_driver(csi2tx_driver);
> -- 
> 2.13.5
> 

Regards,
Benoit
Maxime Ripard Oct. 11, 2017, 9:47 a.m. UTC | #5 
Hi Sakari,

Sorry for the belated answer.

On Tue, Sep 26, 2017 at 08:00:14AM +0000, Sakari Ailus wrote:
> > On Fri, Sep 22, 2017 at 12:38:49PM +0000, Sakari Ailus wrote:
> > > > +	/*
> > > > +	 * Create a static mapping between the CSI virtual channels
> > > > +	 * and the input streams.
> > > 
> > > Which virtual channel is used here?
> > 
> > Like I was trying to explain in the cover letter, the virtual channel
> > is not under that block's control. The input video interfaces have an
> > additional signal that comes from the upstream device which sets the
> > virtual channel.
> 
> Oh, I missed while reviewing the set.
> 
> Presumably either driver would be in control of that then (this one or the
> upstream sub-device).

I don't really see how this driver could be under such control. I
guess it would depend on the integreation, but the upstream (sub-)
device is very likely to be under control of that signal, so I guess
it should be its role to change that. But we should also have that
information available so that the mixing in the CSI link is reported
properly in the media API (looking at Niklas' initial implementation).

> > > 
> > > > +		 */
> > > > +		writel(CSI2TX_STREAM_IF_CFG_FILL_LEVEL(4),
> > > > +		       csi2tx->base + CSI2TX_STREAM_IF_CFG_REG(stream));
> > > > +	}
> > > > +
> > > > +	/* Disable the configuration mode */
> > > > +	writel(0, csi2tx->base + CSI2TX_CONFIG_REG);
> > > 
> > > Shouldn't you start streaming on the downstream sub-device as well?
> > 
> > I appreciate it's a pretty weak argument, but the current setup we
> > have is in the FPGA is:
> > 
> > capture <- CSI2-RX <- CSI2-TX <- pattern generator
> > 
> > So far, the CSI2-RX block is calling its remote sub-device, which is
> > CSI2-TX. If CSI2-RX is calling its remote sub-device (CSI2-RX), we
> > just found ourselves in an endless loop.
> > 
> > I guess it should be easier, and fixable, when we'll have an actual
> > device without such a loopback.
> 
> What's the intended use case of the device, capture or output?

By device, you mean the CSI2-TX block, or something else?

If CSI2-TX, I guess it's more likely to be for output, but that might
be used for capture as well.

> How do you currently start the pipeline?

The capture device is the v4l2 device, and when the capture starts, it
enables the CSI2-RX which in turn enables CSI2-TX. The pattern
generator is enabled all the time.

> We have a few corner cases in V4L2 for such devices in graph parsing and
> stream control. The parsing of the device's fwnode graph endpoints are what
> the device can do, but it doesn't know where the parsing should continue
> and which part of the graph is already parsed.
> 
> That will be addressed but right now a driver just needs to know.

I'm not quite sure I got what you wanted me to fix or change.

Thanks!
Maxime
Maxime Ripard Oct. 11, 2017, 11:55 a.m. UTC | #6 
Hi Benoit,

On Fri, Sep 29, 2017 at 06:21:25PM +0000, Benoit Parrot wrote:
> > +struct csi2tx_priv {
> > +	struct device			*dev;
> > +	atomic_t			count;
> > +
> > +	void __iomem			*base;
> > +
> > +	struct clk			*esc_clk;
> > +	struct clk			*p_clk;
> > +	struct clk			*pixel_clk[CSI2TX_STREAMS_MAX];
> > +
> > +	struct v4l2_subdev		subdev;
> > +	struct media_pad		pads[CSI2TX_PAD_MAX];
> > +	struct v4l2_mbus_framefmt	pad_fmts[CSI2TX_PAD_MAX];
> > +
> > +	bool				has_internal_dphy;
> 
> I assume dphy support is for a subsequent revision?

Yes, the situation is similar to the CSI2-RX driver.

> > +		/*
> > +		 * We use the stream ID there, but it's wrong.
> > +		 *
> > +		 * A stream could very well send a data type that is
> > +		 * not equal to its stream ID. We need to find a
> > +		 * proper way to address it.
> > +		 */
> 
> I don't quite get the above comment, from the code below it looks like
> you are using the current fmt as a source to provide the correct DT.
> Am I missing soemthing?

Yes, so far the datatype is inferred from the format set. Is there
anything wrong with that?

> 
> > +		writel(CSI2TX_DT_CFG_DT(fmt->dt),
> > +		       csi2tx->base + CSI2TX_DT_CFG_REG(stream));
> > +
> > +		writel(CSI2TX_DT_FORMAT_BYTES_PER_LINE(mfmt->width * fmt->bpp) |
> > +		       CSI2TX_DT_FORMAT_MAX_LINE_NUM(mfmt->height + 1),
> > +		       csi2tx->base + CSI2TX_DT_FORMAT_REG(stream));
> > +
> > +		/*
> > +		 * TODO: This needs to be calculated based on the
> > +		 * clock rate.
> > +		 */
> > +		writel(CSI2TX_STREAM_IF_CFG_FILL_LEVEL(4),
> > +		       csi2tx->base + CSI2TX_STREAM_IF_CFG_REG(stream));
> > +	}
> > +
> > +	/* Disable the configuration mode */
> > +	writel(0, csi2tx->base + CSI2TX_CONFIG_REG);
> > +
> > +	return 0;
> > +}
> > +
> > +static int csi2tx_stop(struct csi2tx_priv *csi2tx)
> > +{
> > +	/*
> > +	 * Let the last user turn off the lights
> > +	 */
> > +	if (!atomic_dec_and_test(&csi2tx->count))
> > +		return 0;
> > +
> > +	/* FIXME: Disable the IP here */
> > +
> > +	return 0;
> > +}
> 
> At this point both _start() and _stop() only return 0.
> Are there any cases where any of these might fail to "start" or "stop"?

None that I know of.

There might be some errors with the video stream itself, but that
can be detected after the block has been enabled.

> > +	csi2tx->lanes = csi2tx_get_num_lanes(&pdev->dev);
> > +	if (csi2tx->lanes < 0) {
> > +		dev_err(&pdev->dev, "Invalid number of lanes, bailing out.\n");
> > +		ret = csi2tx->lanes;
> > +		goto err_free_priv;
> > +	}
> 
> csi2tx->lanes is unsigned so it will never be negative.

Ah, right, I'll change that.

Thanks!
Maxime
Benoit Parrot Oct. 11, 2017, 1:36 p.m. UTC | #7 
Hi Maxime,

Maxime Ripard <maxime.ripard@free-electrons.com> wrote on Wed [2017-Oct-11 13:55:44 +0200]:
> Hi Benoit,
> 
> On Fri, Sep 29, 2017 at 06:21:25PM +0000, Benoit Parrot wrote:
> > > +struct csi2tx_priv {
> > > +	struct device			*dev;
> > > +	atomic_t			count;
> > > +
> > > +	void __iomem			*base;
> > > +
> > > +	struct clk			*esc_clk;
> > > +	struct clk			*p_clk;
> > > +	struct clk			*pixel_clk[CSI2TX_STREAMS_MAX];
> > > +
> > > +	struct v4l2_subdev		subdev;
> > > +	struct media_pad		pads[CSI2TX_PAD_MAX];
> > > +	struct v4l2_mbus_framefmt	pad_fmts[CSI2TX_PAD_MAX];
> > > +
> > > +	bool				has_internal_dphy;
> > 
> > I assume dphy support is for a subsequent revision?
> 
> Yes, the situation is similar to the CSI2-RX driver.
> 
> > > +		/*
> > > +		 * We use the stream ID there, but it's wrong.
> > > +		 *
> > > +		 * A stream could very well send a data type that is
> > > +		 * not equal to its stream ID. We need to find a
> > > +		 * proper way to address it.
> > > +		 */
> > 
> > I don't quite get the above comment, from the code below it looks like
> > you are using the current fmt as a source to provide the correct DT.
> > Am I missing soemthing?
> 
> Yes, so far the datatype is inferred from the format set. Is there
> anything wrong with that?

No, nothing wrong with that behavior it just doesn't not match the comment
above, where it is says that the DT is set to the stream ID...

Regards,
Benoit
Maxime Ripard Oct. 13, 2017, 11:38 a.m. UTC | #8 
On Wed, Oct 11, 2017 at 01:36:39PM +0000, Benoit Parrot wrote:
> > > > +		/*
> > > > +		 * We use the stream ID there, but it's wrong.
> > > > +		 *
> > > > +		 * A stream could very well send a data type that is
> > > > +		 * not equal to its stream ID. We need to find a
> > > > +		 * proper way to address it.
> > > > +		 */
> > > 
> > > I don't quite get the above comment, from the code below it looks like
> > > you are using the current fmt as a source to provide the correct DT.
> > > Am I missing soemthing?
> > 
> > Yes, so far the datatype is inferred from the format set. Is there
> > anything wrong with that?
> 
> No, nothing wrong with that behavior it just doesn't not match the comment
> above, where it is says that the DT is set to the stream ID...

As explained in the cover letter, you actually have two datatypes, the
input one that is in the 0-8 range, which is then mapped through that
register to a MIPI-CSI2 datatype. The comment refers to the input
datatype, not the CSI's.

Maxime
diff mbox

Patch

diff --git a/drivers/media/platform/cadence/Kconfig b/drivers/media/platform/cadence/Kconfig
index d1b6bbb6a0eb..db49328ee8b2 100644
--- a/drivers/media/platform/cadence/Kconfig
+++ b/drivers/media/platform/cadence/Kconfig
@@ -9,4 +9,10 @@  config VIDEO_CADENCE_CSI2RX
 	depends on VIDEO_V4L2_SUBDEV_API
 	select V4L2_FWNODE
 
+config VIDEO_CADENCE_CSI2TX
+	tristate "Cadence MIPI-CSI2 TX Controller"
+	depends on MEDIA_CONTROLLER
+	depends on VIDEO_V4L2_SUBDEV_API
+	select V4L2_FWNODE
+
 endif
diff --git a/drivers/media/platform/cadence/Makefile b/drivers/media/platform/cadence/Makefile
index 99a4086b7448..7fe992273162 100644
--- a/drivers/media/platform/cadence/Makefile
+++ b/drivers/media/platform/cadence/Makefile
@@ -1 +1,2 @@ 
 obj-$(CONFIG_VIDEO_CADENCE_CSI2RX)	+= cdns-csi2rx.o
+obj-$(CONFIG_VIDEO_CADENCE_CSI2TX)	+= cdns-csi2tx.o
diff --git a/drivers/media/platform/cadence/cdns-csi2tx.c b/drivers/media/platform/cadence/cdns-csi2tx.c
new file mode 100644
index 000000000000..859bbce8772b
--- /dev/null
+++ b/drivers/media/platform/cadence/cdns-csi2tx.c
@@ -0,0 +1,479 @@ 
+/*
+ * Driver for Cadence MIPI-CSI2 TX Controller
+ *
+ * Copyright (C) 2017 Cadence Design Systems Inc.
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ */
+
+#include <linux/atomic.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_graph.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+#define CSI2TX_DEVICE_CONFIG_REG	0x00
+
+#define CSI2TX_CONFIG_REG		0x20
+#define CSI2TX_CONFIG_CFG_REQ			BIT(2)
+#define CSI2TX_CONFIG_SRST_REQ			BIT(1)
+
+#define CSI2TX_DPHY_CFG_REG		0x28
+#define CSI2TX_DPHY_CFG_CLK_RESET		BIT(16)
+#define CSI2TX_DPHY_CFG_LANE_RESET(n)		BIT((n) + 12)
+#define CSI2TX_DPHY_CFG_MODE_MASK		GENMASK(9, 8)
+#define CSI2TX_DPHY_CFG_MODE_LPDT		(2 << 8)
+#define CSI2TX_DPHY_CFG_MODE_HS			(1 << 8)
+#define CSI2TX_DPHY_CFG_MODE_ULPS		(0 << 8)
+#define CSI2TX_DPHY_CFG_CLK_ENABLE		BIT(4)
+#define CSI2TX_DPHY_CFG_LANE_ENABLE(n)		BIT(n)
+
+#define CSI2TX_DPHY_CLK_WAKEUP_REG	0x2c
+#define CSI2TX_DPHY_CLK_WAKEUP_ULPS_CYCLES(n)	((n) & 0xffff)
+
+#define CSI2TX_DT_CFG_REG(n)		(0x80 + (n) * 8)
+#define CSI2TX_DT_CFG_DT(n)			(((n) & 0x3f) << 2)
+
+#define CSI2TX_DT_FORMAT_REG(n)		(0x84 + (n) * 8)
+#define CSI2TX_DT_FORMAT_BYTES_PER_LINE(n)	(((n) & 0xffff) << 16)
+#define CSI2TX_DT_FORMAT_MAX_LINE_NUM(n)	((n) & 0xffff)
+
+#define CSI2TX_STREAM_IF_CFG_REG(n)	(0x100 + (n) * 4)
+#define CSI2TX_STREAM_IF_CFG_FILL_LEVEL(n)	((n) & 0x1f)
+
+#define CSI2TX_STREAMS_MAX	4
+
+enum csi2tx_pads {
+	CSI2TX_PAD_SOURCE,
+	CSI2TX_PAD_SINK_STREAM0,
+	CSI2TX_PAD_SINK_STREAM1,
+	CSI2TX_PAD_SINK_STREAM2,
+	CSI2TX_PAD_SINK_STREAM3,
+	CSI2TX_PAD_MAX,
+};
+
+struct csi2tx_fmt {
+	u32	mbus;
+	u32	dt;
+	u32	bpp;
+};
+
+struct csi2tx_priv {
+	struct device			*dev;
+	atomic_t			count;
+
+	void __iomem			*base;
+
+	struct clk			*esc_clk;
+	struct clk			*p_clk;
+	struct clk			*pixel_clk[CSI2TX_STREAMS_MAX];
+
+	struct v4l2_subdev		subdev;
+	struct media_pad		pads[CSI2TX_PAD_MAX];
+	struct v4l2_mbus_framefmt	pad_fmts[CSI2TX_PAD_MAX];
+
+	bool				has_internal_dphy;
+	unsigned int			lanes;
+	unsigned int			max_lanes;
+	unsigned int			max_streams;
+};
+
+static struct csi2tx_fmt csi2tx_formats[] = {
+	{
+		.mbus	= MEDIA_BUS_FMT_UYVY8_1X16,
+		.bpp	= 2,
+		.dt	= 0x1e,
+	},
+	{
+		.mbus	= MEDIA_BUS_FMT_RGB888_1X24,
+		.bpp	= 3,
+		.dt	= 0x24,
+	},
+};
+
+static inline
+struct csi2tx_priv *v4l2_subdev_to_csi2tx(struct v4l2_subdev *subdev)
+{
+	return container_of(subdev, struct csi2tx_priv, subdev);
+}
+
+static void csi2tx_reset(struct csi2tx_priv *csi2tx)
+{
+	writel(CSI2TX_CONFIG_SRST_REQ, csi2tx->base + CSI2TX_CONFIG_REG);
+
+	usleep_range(10, 20);
+}
+
+static struct csi2tx_fmt *csitx_get_fmt_from_mbus(struct v4l2_mbus_framefmt *mfmt)
+{
+	int i;
+
+	if (!mfmt)
+		return NULL;
+
+	for (i = 0; i < ARRAY_SIZE(csi2tx_formats); i++)
+		if (csi2tx_formats[i].mbus == mfmt->code)
+			return &csi2tx_formats[i];
+
+	return NULL;
+}
+
+static int csi2tx_enum_mbus_code(struct v4l2_subdev *subdev,
+				 struct v4l2_subdev_pad_config *cfg,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->pad || code->index >= ARRAY_SIZE(csi2tx_formats))
+		return -EINVAL;
+
+	code->code = csi2tx_formats[code->index].mbus;
+
+	return 0;
+}
+
+static int csi2tx_get_pad_format(struct v4l2_subdev *subdev,
+				 struct v4l2_subdev_pad_config *cfg,
+				 struct v4l2_subdev_format *fmt)
+{
+	struct csi2tx_priv *csi2tx = v4l2_subdev_to_csi2tx(subdev);
+
+	fmt->format = csi2tx->pad_fmts[fmt->pad];
+
+	return 0;
+}
+
+static int csi2tx_set_pad_format(struct v4l2_subdev *subdev,
+				 struct v4l2_subdev_pad_config *cfg,
+				 struct v4l2_subdev_format *fmt)
+{
+	struct csi2tx_priv *csi2tx = v4l2_subdev_to_csi2tx(subdev);
+
+	csi2tx->pad_fmts[fmt->pad] = fmt->format;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_pad_ops csi2tx_pad_ops = {
+	.enum_mbus_code	= csi2tx_enum_mbus_code,
+	.get_fmt	= csi2tx_get_pad_format,
+	.set_fmt	= csi2tx_set_pad_format,
+};
+
+static int csi2tx_start(struct csi2tx_priv *csi2tx)
+{
+	u32 reg;
+	int i;
+
+	/*
+	 * We're not the first users, there's no need to enable the
+	 * whole controller.
+	 */
+	if (atomic_inc_return(&csi2tx->count) > 1)
+		return 0;
+
+	csi2tx_reset(csi2tx);
+
+	writel(CSI2TX_CONFIG_CFG_REQ, csi2tx->base + CSI2TX_CONFIG_REG);
+
+	usleep_range(10, 20);
+
+	writel(CSI2TX_DPHY_CLK_WAKEUP_ULPS_CYCLES(32),
+	       csi2tx->base + CSI2TX_DPHY_CLK_WAKEUP_REG);
+
+	/* Put our lanes (clock and data) out of reset */
+	reg = CSI2TX_DPHY_CFG_CLK_RESET | CSI2TX_DPHY_CFG_MODE_LPDT;
+	for (i = 0; i < csi2tx->lanes; i++)
+		reg |= CSI2TX_DPHY_CFG_LANE_RESET(i);
+	writel(reg, csi2tx->base + CSI2TX_DPHY_CFG_REG);
+
+	usleep_range(10, 20);
+
+	/* Enable our (clock and data) lanes */
+	reg |= CSI2TX_DPHY_CFG_CLK_ENABLE;
+	for (i = 0; i < csi2tx->lanes; i++)
+		reg |= CSI2TX_DPHY_CFG_LANE_ENABLE(i);
+	writel(reg, csi2tx->base + CSI2TX_DPHY_CFG_REG);
+
+	usleep_range(10, 20);
+
+	/* Switch to HS mode */
+	reg &= ~CSI2TX_DPHY_CFG_MODE_MASK;
+	writel(reg | CSI2TX_DPHY_CFG_MODE_HS,
+	       csi2tx->base + CSI2TX_DPHY_CFG_REG);
+
+	usleep_range(10, 20);
+
+	/*
+	 * Create a static mapping between the CSI virtual channels
+	 * and the input streams.
+	 *
+	 * This should be enhanced, but v4l2 lacks the support for
+	 * changing that mapping dynamically.
+	 */
+	for (i = CSI2TX_PAD_SINK_STREAM0; i < CSI2TX_PAD_MAX; i++) {
+		struct v4l2_mbus_framefmt *mfmt = &csi2tx->pad_fmts[i];
+		struct csi2tx_fmt *fmt = csitx_get_fmt_from_mbus(mfmt);
+		int stream = i - CSI2TX_PAD_SINK_STREAM0;
+
+		/*
+		 * If no-one set a format, we consider this pad
+		 * disabled and just skip it.
+		 */
+		if (!fmt)
+			continue;
+
+		/*
+		 * We use the stream ID there, but it's wrong.
+		 *
+		 * A stream could very well send a data type that is
+		 * not equal to its stream ID. We need to find a
+		 * proper way to address it.
+		 */
+		writel(CSI2TX_DT_CFG_DT(fmt->dt),
+		       csi2tx->base + CSI2TX_DT_CFG_REG(stream));
+
+		writel(CSI2TX_DT_FORMAT_BYTES_PER_LINE(mfmt->width * fmt->bpp) |
+		       CSI2TX_DT_FORMAT_MAX_LINE_NUM(mfmt->height + 1),
+		       csi2tx->base + CSI2TX_DT_FORMAT_REG(stream));
+
+		/*
+		 * TODO: This needs to be calculated based on the
+		 * clock rate.
+		 */
+		writel(CSI2TX_STREAM_IF_CFG_FILL_LEVEL(4),
+		       csi2tx->base + CSI2TX_STREAM_IF_CFG_REG(stream));
+	}
+
+	/* Disable the configuration mode */
+	writel(0, csi2tx->base + CSI2TX_CONFIG_REG);
+
+	return 0;
+}
+
+static int csi2tx_stop(struct csi2tx_priv *csi2tx)
+{
+	/*
+	 * Let the last user turn off the lights
+	 */
+	if (!atomic_dec_and_test(&csi2tx->count))
+		return 0;
+
+	/* FIXME: Disable the IP here */
+
+	return 0;
+}
+
+static int csi2tx_s_stream(struct v4l2_subdev *subdev, int enable)
+{
+	struct csi2tx_priv *csi2tx = v4l2_subdev_to_csi2tx(subdev);
+	int ret;
+
+	if (enable)
+		ret = csi2tx_start(csi2tx);
+	else
+		ret = csi2tx_stop(csi2tx);
+
+	return ret;
+}
+
+static const struct v4l2_subdev_video_ops csi2tx_video_ops = {
+	.s_stream	= csi2tx_s_stream,
+};
+
+static struct v4l2_subdev_ops csi2tx_subdev_ops = {
+	.pad		= &csi2tx_pad_ops,
+	.video		= &csi2tx_video_ops,
+};
+
+static int csi2tx_get_resources(struct csi2tx_priv *csi2tx,
+				struct platform_device *pdev)
+{
+	struct resource *res;
+	u32 reg;
+	int i;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	csi2tx->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(csi2tx->base)) {
+		dev_err(&pdev->dev, "Couldn't map our registers\n");
+		return PTR_ERR(csi2tx->base);
+	}
+
+	csi2tx->p_clk = devm_clk_get(&pdev->dev, "p_clk");
+	if (IS_ERR(csi2tx->p_clk)) {
+		dev_err(&pdev->dev, "Couldn't get p_clk\n");
+		return PTR_ERR(csi2tx->p_clk);
+	}
+
+	csi2tx->esc_clk = devm_clk_get(&pdev->dev, "esc_clk");
+	if (IS_ERR(csi2tx->esc_clk)) {
+		dev_err(&pdev->dev, "Couldn't get the esc_clk\n");
+		return PTR_ERR(csi2tx->esc_clk);
+	}
+
+	clk_prepare_enable(csi2tx->p_clk);
+	reg = readl(csi2tx->base + CSI2TX_DEVICE_CONFIG_REG);
+	clk_disable_unprepare(csi2tx->p_clk);
+
+	csi2tx->max_lanes = (reg & 7);
+	if (csi2tx->max_lanes > 4) {
+		dev_err(&pdev->dev, "Invalid number of lanes: %u\n",
+			csi2tx->max_lanes);
+		return -EINVAL;
+	}
+
+	csi2tx->max_streams = ((reg >> 4) & 7);
+	if (csi2tx->max_streams > CSI2TX_STREAMS_MAX) {
+		dev_err(&pdev->dev, "Invalid number of streams: %u\n",
+			csi2tx->max_streams);
+		return -EINVAL;
+	}
+
+	csi2tx->has_internal_dphy = (reg & BIT(3)) ? true : false;
+
+	for (i = 0; i < csi2tx->max_streams; i++) {
+		char clk_name[16];
+
+		snprintf(clk_name, sizeof(clk_name), "pixel_if%u_clk", i);
+		csi2tx->pixel_clk[i] = devm_clk_get(&pdev->dev, clk_name);
+		if (IS_ERR(csi2tx->pixel_clk[i])) {
+			dev_err(&pdev->dev, "Couldn't get clock %s\n",
+				clk_name);
+			return PTR_ERR(csi2tx->pixel_clk[i]);
+		}
+	}
+
+	return 0;
+}
+
+static int csi2tx_get_num_lanes(struct device *dev)
+{
+	struct v4l2_fwnode_endpoint v4l2_ep;
+	struct device_node *ep, *np = dev->of_node;
+	int ret = 0;
+
+	ep = of_graph_get_endpoint_by_regs(np, 0, 0);
+	if (!ep)
+		return -EINVAL;
+
+	ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(ep), &v4l2_ep);
+	if (ret) {
+		dev_err(dev, "Could not parse v4l2 endpoint\n");
+		goto out;
+	}
+
+	if (v4l2_ep.bus_type != V4L2_MBUS_CSI2) {
+		dev_err(dev, "Unsupported media bus type: 0x%x\n",
+			v4l2_ep.bus_type);
+		ret = -EINVAL;
+		goto out;
+	}
+
+	ret = v4l2_ep.bus.mipi_csi2.num_data_lanes;
+
+out:
+	of_node_put(ep);
+	return ret;
+}
+
+static int csi2tx_probe(struct platform_device *pdev)
+{
+	struct csi2tx_priv *csi2tx;
+	int i, ret;
+
+	csi2tx = kzalloc(sizeof(*csi2tx), GFP_KERNEL);
+	if (!csi2tx)
+		return -ENOMEM;
+	platform_set_drvdata(pdev, csi2tx);
+	csi2tx->dev = &pdev->dev;
+
+	ret = csi2tx_get_resources(csi2tx, pdev);
+	if (ret)
+		goto err_free_priv;
+
+	v4l2_subdev_init(&csi2tx->subdev, &csi2tx_subdev_ops);
+	csi2tx->subdev.owner = THIS_MODULE;
+	csi2tx->subdev.dev = &pdev->dev;
+	csi2tx->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	v4l2_set_subdevdata(&csi2tx->subdev, &pdev->dev);
+	snprintf(csi2tx->subdev.name, V4L2_SUBDEV_NAME_SIZE, "%s.%s",
+		 KBUILD_MODNAME, dev_name(&pdev->dev));
+
+	csi2tx->lanes = csi2tx_get_num_lanes(&pdev->dev);
+	if (csi2tx->lanes < 0) {
+		dev_err(&pdev->dev, "Invalid number of lanes, bailing out.\n");
+		ret = csi2tx->lanes;
+		goto err_free_priv;
+	}
+
+	if (csi2tx->lanes > csi2tx->max_lanes) {
+		dev_err(&pdev->dev,
+			"Current configuration uses more lanes than supported\n");
+		ret = -EINVAL;
+		goto err_free_priv;
+	}
+
+	/* Create our media pads */
+	csi2tx->subdev.entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
+	csi2tx->pads[CSI2TX_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
+	for (i = CSI2TX_PAD_SINK_STREAM0; i < CSI2TX_PAD_MAX; i++)
+		csi2tx->pads[i].flags = MEDIA_PAD_FL_SINK;
+
+	ret = media_entity_pads_init(&csi2tx->subdev.entity, CSI2TX_PAD_MAX,
+				     csi2tx->pads);
+	if (ret)
+		return ret;
+
+	ret = v4l2_async_register_subdev(&csi2tx->subdev);
+	if (ret < 0)
+		goto err_free_priv;
+
+	dev_info(&pdev->dev,
+		 "Probed CSI2TX with %u/%u lanes, %u streams, %s D-PHY\n",
+		 csi2tx->lanes, csi2tx->max_lanes, csi2tx->max_streams,
+		 csi2tx->has_internal_dphy ? "internal" : "no");
+
+	return 0;
+
+err_free_priv:
+	kfree(csi2tx);
+	return ret;
+}
+
+static int csi2tx_remove(struct platform_device *pdev)
+{
+	struct csi2tx_priv *csi2tx = platform_get_drvdata(pdev);
+
+	v4l2_async_unregister_subdev(&csi2tx->subdev);
+	kfree(csi2tx);
+
+	return 0;
+}
+
+static const struct of_device_id csi2tx_of_table[] = {
+	{ .compatible = "cdns,csi2tx" },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, csi2tx_of_table);
+
+static struct platform_driver csi2tx_driver = {
+	.probe	= csi2tx_probe,
+	.remove	= csi2tx_remove,
+
+	.driver	= {
+		.name		= "cdns-csi2tx",
+		.of_match_table	= csi2tx_of_table,
+	},
+};
+module_platform_driver(csi2tx_driver);