xref: /linux/drivers/media/platform/cadence/cdns-csi2rx.c (revision bc0e8d91feec72b19199298dca470c5816a52105)
11fc3b37fSMaxime Ripard // SPDX-License-Identifier: GPL-2.0+
21fc3b37fSMaxime Ripard /*
31fc3b37fSMaxime Ripard  * Driver for Cadence MIPI-CSI2 RX Controller v1.3
41fc3b37fSMaxime Ripard  *
51fc3b37fSMaxime Ripard  * Copyright (C) 2017 Cadence Design Systems Inc.
61fc3b37fSMaxime Ripard  */
71fc3b37fSMaxime Ripard 
81fc3b37fSMaxime Ripard #include <linux/clk.h>
91fc3b37fSMaxime Ripard #include <linux/delay.h>
101fc3b37fSMaxime Ripard #include <linux/io.h>
11a64175faSPratyush Yadav #include <linux/iopoll.h>
121fc3b37fSMaxime Ripard #include <linux/module.h>
131fc3b37fSMaxime Ripard #include <linux/of.h>
141fc3b37fSMaxime Ripard #include <linux/of_graph.h>
151fc3b37fSMaxime Ripard #include <linux/phy/phy.h>
161fc3b37fSMaxime Ripard #include <linux/platform_device.h>
17e0b9ce38SJack Zhu #include <linux/reset.h>
183c46ab9dSArnd Bergmann #include <linux/slab.h>
191fc3b37fSMaxime Ripard 
201fc3b37fSMaxime Ripard #include <media/v4l2-ctrls.h>
211fc3b37fSMaxime Ripard #include <media/v4l2-device.h>
221fc3b37fSMaxime Ripard #include <media/v4l2-fwnode.h>
231fc3b37fSMaxime Ripard #include <media/v4l2-subdev.h>
241fc3b37fSMaxime Ripard 
251fc3b37fSMaxime Ripard #define CSI2RX_DEVICE_CFG_REG			0x000
261fc3b37fSMaxime Ripard 
271fc3b37fSMaxime Ripard #define CSI2RX_SOFT_RESET_REG			0x004
281fc3b37fSMaxime Ripard #define CSI2RX_SOFT_RESET_PROTOCOL			BIT(1)
291fc3b37fSMaxime Ripard #define CSI2RX_SOFT_RESET_FRONT				BIT(0)
301fc3b37fSMaxime Ripard 
311fc3b37fSMaxime Ripard #define CSI2RX_STATIC_CFG_REG			0x008
321fc3b37fSMaxime Ripard #define CSI2RX_STATIC_CFG_DLANE_MAP(llane, plane)	((plane) << (16 + (llane) * 4))
331fc3b37fSMaxime Ripard #define CSI2RX_STATIC_CFG_LANES_MASK			GENMASK(11, 8)
341fc3b37fSMaxime Ripard 
353295cf12SJack Zhu #define CSI2RX_DPHY_LANE_CTRL_REG		0x40
363295cf12SJack Zhu #define CSI2RX_DPHY_CL_RST			BIT(16)
373295cf12SJack Zhu #define CSI2RX_DPHY_DL_RST(i)			BIT((i) + 12)
383295cf12SJack Zhu #define CSI2RX_DPHY_CL_EN			BIT(4)
393295cf12SJack Zhu #define CSI2RX_DPHY_DL_EN(i)			BIT(i)
403295cf12SJack Zhu 
411fc3b37fSMaxime Ripard #define CSI2RX_STREAM_BASE(n)		(((n) + 1) * 0x100)
421fc3b37fSMaxime Ripard 
431fc3b37fSMaxime Ripard #define CSI2RX_STREAM_CTRL_REG(n)		(CSI2RX_STREAM_BASE(n) + 0x000)
446f28a427SPratyush Yadav #define CSI2RX_STREAM_CTRL_SOFT_RST			BIT(4)
45a64175faSPratyush Yadav #define CSI2RX_STREAM_CTRL_STOP				BIT(1)
461fc3b37fSMaxime Ripard #define CSI2RX_STREAM_CTRL_START			BIT(0)
471fc3b37fSMaxime Ripard 
48a64175faSPratyush Yadav #define CSI2RX_STREAM_STATUS_REG(n)		(CSI2RX_STREAM_BASE(n) + 0x004)
49a64175faSPratyush Yadav #define CSI2RX_STREAM_STATUS_RDY			BIT(31)
50a64175faSPratyush Yadav 
511fc3b37fSMaxime Ripard #define CSI2RX_STREAM_DATA_CFG_REG(n)		(CSI2RX_STREAM_BASE(n) + 0x008)
521fc3b37fSMaxime Ripard #define CSI2RX_STREAM_DATA_CFG_VC_SELECT(n)		BIT((n) + 16)
531fc3b37fSMaxime Ripard 
541fc3b37fSMaxime Ripard #define CSI2RX_STREAM_CFG_REG(n)		(CSI2RX_STREAM_BASE(n) + 0x00c)
551fc3b37fSMaxime Ripard #define CSI2RX_STREAM_CFG_FIFO_MODE_LARGE_BUF		(1 << 8)
561fc3b37fSMaxime Ripard 
571fc3b37fSMaxime Ripard #define CSI2RX_LANES_MAX	4
581fc3b37fSMaxime Ripard #define CSI2RX_STREAMS_MAX	4
591fc3b37fSMaxime Ripard 
601fc3b37fSMaxime Ripard enum csi2rx_pads {
611fc3b37fSMaxime Ripard 	CSI2RX_PAD_SINK,
621fc3b37fSMaxime Ripard 	CSI2RX_PAD_SOURCE_STREAM0,
631fc3b37fSMaxime Ripard 	CSI2RX_PAD_SOURCE_STREAM1,
641fc3b37fSMaxime Ripard 	CSI2RX_PAD_SOURCE_STREAM2,
651fc3b37fSMaxime Ripard 	CSI2RX_PAD_SOURCE_STREAM3,
661fc3b37fSMaxime Ripard 	CSI2RX_PAD_MAX,
671fc3b37fSMaxime Ripard };
681fc3b37fSMaxime Ripard 
69dbca7b3cSPratyush Yadav struct csi2rx_fmt {
70dbca7b3cSPratyush Yadav 	u32				code;
71dbca7b3cSPratyush Yadav 	u8				bpp;
72dbca7b3cSPratyush Yadav };
73dbca7b3cSPratyush Yadav 
741fc3b37fSMaxime Ripard struct csi2rx_priv {
751fc3b37fSMaxime Ripard 	struct device			*dev;
761fc3b37fSMaxime Ripard 	unsigned int			count;
771fc3b37fSMaxime Ripard 
781fc3b37fSMaxime Ripard 	/*
791fc3b37fSMaxime Ripard 	 * Used to prevent race conditions between multiple,
801fc3b37fSMaxime Ripard 	 * concurrent calls to start and stop.
811fc3b37fSMaxime Ripard 	 */
821fc3b37fSMaxime Ripard 	struct mutex			lock;
831fc3b37fSMaxime Ripard 
841fc3b37fSMaxime Ripard 	void __iomem			*base;
851fc3b37fSMaxime Ripard 	struct clk			*sys_clk;
861fc3b37fSMaxime Ripard 	struct clk			*p_clk;
871fc3b37fSMaxime Ripard 	struct clk			*pixel_clk[CSI2RX_STREAMS_MAX];
88e0b9ce38SJack Zhu 	struct reset_control		*sys_rst;
89e0b9ce38SJack Zhu 	struct reset_control		*p_rst;
90e0b9ce38SJack Zhu 	struct reset_control		*pixel_rst[CSI2RX_STREAMS_MAX];
911fc3b37fSMaxime Ripard 	struct phy			*dphy;
921fc3b37fSMaxime Ripard 
931fc3b37fSMaxime Ripard 	u8				lanes[CSI2RX_LANES_MAX];
941fc3b37fSMaxime Ripard 	u8				num_lanes;
951fc3b37fSMaxime Ripard 	u8				max_lanes;
961fc3b37fSMaxime Ripard 	u8				max_streams;
971fc3b37fSMaxime Ripard 	bool				has_internal_dphy;
981fc3b37fSMaxime Ripard 
991fc3b37fSMaxime Ripard 	struct v4l2_subdev		subdev;
1001fc3b37fSMaxime Ripard 	struct v4l2_async_notifier	notifier;
1011fc3b37fSMaxime Ripard 	struct media_pad		pads[CSI2RX_PAD_MAX];
1021fc3b37fSMaxime Ripard 
1031fc3b37fSMaxime Ripard 	/* Remote source */
1041fc3b37fSMaxime Ripard 	struct v4l2_subdev		*source_subdev;
1051fc3b37fSMaxime Ripard 	int				source_pad;
1061fc3b37fSMaxime Ripard };
1071fc3b37fSMaxime Ripard 
108dbca7b3cSPratyush Yadav static const struct csi2rx_fmt formats[] = {
109dbca7b3cSPratyush Yadav 	{ .code	= MEDIA_BUS_FMT_YUYV8_1X16, .bpp = 16, },
110dbca7b3cSPratyush Yadav 	{ .code	= MEDIA_BUS_FMT_UYVY8_1X16, .bpp = 16, },
111dbca7b3cSPratyush Yadav 	{ .code	= MEDIA_BUS_FMT_YVYU8_1X16, .bpp = 16, },
112dbca7b3cSPratyush Yadav 	{ .code	= MEDIA_BUS_FMT_VYUY8_1X16, .bpp = 16, },
113dbca7b3cSPratyush Yadav 	{ .code	= MEDIA_BUS_FMT_SBGGR8_1X8, .bpp = 8, },
114dbca7b3cSPratyush Yadav 	{ .code	= MEDIA_BUS_FMT_SGBRG8_1X8, .bpp = 8, },
115dbca7b3cSPratyush Yadav 	{ .code	= MEDIA_BUS_FMT_SGRBG8_1X8, .bpp = 8, },
116dbca7b3cSPratyush Yadav 	{ .code	= MEDIA_BUS_FMT_SRGGB8_1X8, .bpp = 8, },
117dbca7b3cSPratyush Yadav 	{ .code	= MEDIA_BUS_FMT_SBGGR10_1X10, .bpp = 10, },
118dbca7b3cSPratyush Yadav 	{ .code	= MEDIA_BUS_FMT_SGBRG10_1X10, .bpp = 10, },
119dbca7b3cSPratyush Yadav 	{ .code	= MEDIA_BUS_FMT_SGRBG10_1X10, .bpp = 10, },
120dbca7b3cSPratyush Yadav 	{ .code	= MEDIA_BUS_FMT_SRGGB10_1X10, .bpp = 10, },
121dbca7b3cSPratyush Yadav };
122dbca7b3cSPratyush Yadav 
123dbca7b3cSPratyush Yadav static const struct csi2rx_fmt *csi2rx_get_fmt_by_code(u32 code)
124dbca7b3cSPratyush Yadav {
125dbca7b3cSPratyush Yadav 	unsigned int i;
126dbca7b3cSPratyush Yadav 
127dbca7b3cSPratyush Yadav 	for (i = 0; i < ARRAY_SIZE(formats); i++)
128dbca7b3cSPratyush Yadav 		if (formats[i].code == code)
129dbca7b3cSPratyush Yadav 			return &formats[i];
130dbca7b3cSPratyush Yadav 
131dbca7b3cSPratyush Yadav 	return NULL;
132dbca7b3cSPratyush Yadav }
133dbca7b3cSPratyush Yadav 
1341fc3b37fSMaxime Ripard static inline
1351fc3b37fSMaxime Ripard struct csi2rx_priv *v4l2_subdev_to_csi2rx(struct v4l2_subdev *subdev)
1361fc3b37fSMaxime Ripard {
1371fc3b37fSMaxime Ripard 	return container_of(subdev, struct csi2rx_priv, subdev);
1381fc3b37fSMaxime Ripard }
1391fc3b37fSMaxime Ripard 
1401fc3b37fSMaxime Ripard static void csi2rx_reset(struct csi2rx_priv *csi2rx)
1411fc3b37fSMaxime Ripard {
1426f28a427SPratyush Yadav 	unsigned int i;
1436f28a427SPratyush Yadav 
1446f28a427SPratyush Yadav 	/* Reset module */
1451fc3b37fSMaxime Ripard 	writel(CSI2RX_SOFT_RESET_PROTOCOL | CSI2RX_SOFT_RESET_FRONT,
1461fc3b37fSMaxime Ripard 	       csi2rx->base + CSI2RX_SOFT_RESET_REG);
1476f28a427SPratyush Yadav 	/* Reset individual streams. */
1486f28a427SPratyush Yadav 	for (i = 0; i < csi2rx->max_streams; i++) {
1496f28a427SPratyush Yadav 		writel(CSI2RX_STREAM_CTRL_SOFT_RST,
1506f28a427SPratyush Yadav 		       csi2rx->base + CSI2RX_STREAM_CTRL_REG(i));
1516f28a427SPratyush Yadav 	}
1521fc3b37fSMaxime Ripard 
1536f28a427SPratyush Yadav 	usleep_range(10, 20);
1541fc3b37fSMaxime Ripard 
1556f28a427SPratyush Yadav 	/* Clear resets */
1561fc3b37fSMaxime Ripard 	writel(0, csi2rx->base + CSI2RX_SOFT_RESET_REG);
1576f28a427SPratyush Yadav 	for (i = 0; i < csi2rx->max_streams; i++)
1586f28a427SPratyush Yadav 		writel(0, csi2rx->base + CSI2RX_STREAM_CTRL_REG(i));
1591fc3b37fSMaxime Ripard }
1601fc3b37fSMaxime Ripard 
1613295cf12SJack Zhu static int csi2rx_configure_ext_dphy(struct csi2rx_priv *csi2rx)
1623295cf12SJack Zhu {
1633295cf12SJack Zhu 	union phy_configure_opts opts = { };
164a91d06f4SPratyush Yadav 	struct phy_configure_opts_mipi_dphy *cfg = &opts.mipi_dphy;
165a91d06f4SPratyush Yadav 	struct v4l2_subdev_format sd_fmt = {
166a91d06f4SPratyush Yadav 		.which	= V4L2_SUBDEV_FORMAT_ACTIVE,
167a91d06f4SPratyush Yadav 		.pad	= CSI2RX_PAD_SINK,
168a91d06f4SPratyush Yadav 	};
169a91d06f4SPratyush Yadav 	const struct csi2rx_fmt *fmt;
170a91d06f4SPratyush Yadav 	s64 link_freq;
1713295cf12SJack Zhu 	int ret;
1723295cf12SJack Zhu 
173a91d06f4SPratyush Yadav 	ret = v4l2_subdev_call_state_active(&csi2rx->subdev, pad, get_fmt,
174a91d06f4SPratyush Yadav 					    &sd_fmt);
175a91d06f4SPratyush Yadav 	if (ret < 0)
176a91d06f4SPratyush Yadav 		return ret;
177a91d06f4SPratyush Yadav 
178a91d06f4SPratyush Yadav 	fmt = csi2rx_get_fmt_by_code(sd_fmt.format.code);
179a91d06f4SPratyush Yadav 
180a91d06f4SPratyush Yadav 	link_freq = v4l2_get_link_freq(csi2rx->source_subdev->ctrl_handler,
181a91d06f4SPratyush Yadav 				       fmt->bpp, 2 * csi2rx->num_lanes);
182a91d06f4SPratyush Yadav 	if (link_freq < 0)
183a91d06f4SPratyush Yadav 		return link_freq;
184a91d06f4SPratyush Yadav 
185a91d06f4SPratyush Yadav 	ret = phy_mipi_dphy_get_default_config_for_hsclk(link_freq,
186a91d06f4SPratyush Yadav 							 csi2rx->num_lanes, cfg);
187a91d06f4SPratyush Yadav 	if (ret)
188a91d06f4SPratyush Yadav 		return ret;
189a91d06f4SPratyush Yadav 
1903295cf12SJack Zhu 	ret = phy_power_on(csi2rx->dphy);
1913295cf12SJack Zhu 	if (ret)
1923295cf12SJack Zhu 		return ret;
1933295cf12SJack Zhu 
1943295cf12SJack Zhu 	ret = phy_configure(csi2rx->dphy, &opts);
1953295cf12SJack Zhu 	if (ret) {
1963295cf12SJack Zhu 		phy_power_off(csi2rx->dphy);
1973295cf12SJack Zhu 		return ret;
1983295cf12SJack Zhu 	}
1993295cf12SJack Zhu 
2003295cf12SJack Zhu 	return 0;
2013295cf12SJack Zhu }
2023295cf12SJack Zhu 
2031fc3b37fSMaxime Ripard static int csi2rx_start(struct csi2rx_priv *csi2rx)
2041fc3b37fSMaxime Ripard {
2051fc3b37fSMaxime Ripard 	unsigned int i;
2061fc3b37fSMaxime Ripard 	unsigned long lanes_used = 0;
2071fc3b37fSMaxime Ripard 	u32 reg;
2081fc3b37fSMaxime Ripard 	int ret;
2091fc3b37fSMaxime Ripard 
2101fc3b37fSMaxime Ripard 	ret = clk_prepare_enable(csi2rx->p_clk);
2111fc3b37fSMaxime Ripard 	if (ret)
2121fc3b37fSMaxime Ripard 		return ret;
2131fc3b37fSMaxime Ripard 
214e0b9ce38SJack Zhu 	reset_control_deassert(csi2rx->p_rst);
2151fc3b37fSMaxime Ripard 	csi2rx_reset(csi2rx);
2161fc3b37fSMaxime Ripard 
2171fc3b37fSMaxime Ripard 	reg = csi2rx->num_lanes << 8;
2181fc3b37fSMaxime Ripard 	for (i = 0; i < csi2rx->num_lanes; i++) {
2191fc3b37fSMaxime Ripard 		reg |= CSI2RX_STATIC_CFG_DLANE_MAP(i, csi2rx->lanes[i]);
2201fc3b37fSMaxime Ripard 		set_bit(csi2rx->lanes[i], &lanes_used);
2211fc3b37fSMaxime Ripard 	}
2221fc3b37fSMaxime Ripard 
2231fc3b37fSMaxime Ripard 	/*
2241fc3b37fSMaxime Ripard 	 * Even the unused lanes need to be mapped. In order to avoid
2251fc3b37fSMaxime Ripard 	 * to map twice to the same physical lane, keep the lanes used
2261fc3b37fSMaxime Ripard 	 * in the previous loop, and only map unused physical lanes to
2271fc3b37fSMaxime Ripard 	 * the rest of our logical lanes.
2281fc3b37fSMaxime Ripard 	 */
2291fc3b37fSMaxime Ripard 	for (i = csi2rx->num_lanes; i < csi2rx->max_lanes; i++) {
2301fc3b37fSMaxime Ripard 		unsigned int idx = find_first_zero_bit(&lanes_used,
2312eca8e4cSChristophe JAILLET 						       csi2rx->max_lanes);
2321fc3b37fSMaxime Ripard 		set_bit(idx, &lanes_used);
2331fc3b37fSMaxime Ripard 		reg |= CSI2RX_STATIC_CFG_DLANE_MAP(i, i + 1);
2341fc3b37fSMaxime Ripard 	}
2351fc3b37fSMaxime Ripard 
2361fc3b37fSMaxime Ripard 	writel(reg, csi2rx->base + CSI2RX_STATIC_CFG_REG);
2371fc3b37fSMaxime Ripard 
2381fc3b37fSMaxime Ripard 	ret = v4l2_subdev_call(csi2rx->source_subdev, video, s_stream, true);
2391fc3b37fSMaxime Ripard 	if (ret)
2401fc3b37fSMaxime Ripard 		goto err_disable_pclk;
2411fc3b37fSMaxime Ripard 
2423295cf12SJack Zhu 	/* Enable DPHY clk and data lanes. */
2433295cf12SJack Zhu 	if (csi2rx->dphy) {
2443295cf12SJack Zhu 		reg = CSI2RX_DPHY_CL_EN | CSI2RX_DPHY_CL_RST;
2453295cf12SJack Zhu 		for (i = 0; i < csi2rx->num_lanes; i++) {
2463295cf12SJack Zhu 			reg |= CSI2RX_DPHY_DL_EN(csi2rx->lanes[i] - 1);
2473295cf12SJack Zhu 			reg |= CSI2RX_DPHY_DL_RST(csi2rx->lanes[i] - 1);
2483295cf12SJack Zhu 		}
2493295cf12SJack Zhu 
2503295cf12SJack Zhu 		writel(reg, csi2rx->base + CSI2RX_DPHY_LANE_CTRL_REG);
2513295cf12SJack Zhu 	}
2523295cf12SJack Zhu 
2531fc3b37fSMaxime Ripard 	/*
2541fc3b37fSMaxime Ripard 	 * Create a static mapping between the CSI virtual channels
2551fc3b37fSMaxime Ripard 	 * and the output stream.
2561fc3b37fSMaxime Ripard 	 *
2571fc3b37fSMaxime Ripard 	 * This should be enhanced, but v4l2 lacks the support for
2581fc3b37fSMaxime Ripard 	 * changing that mapping dynamically.
2591fc3b37fSMaxime Ripard 	 *
2601fc3b37fSMaxime Ripard 	 * We also cannot enable and disable independent streams here,
2611fc3b37fSMaxime Ripard 	 * hence the reference counting.
2621fc3b37fSMaxime Ripard 	 */
2631fc3b37fSMaxime Ripard 	for (i = 0; i < csi2rx->max_streams; i++) {
2641fc3b37fSMaxime Ripard 		ret = clk_prepare_enable(csi2rx->pixel_clk[i]);
2651fc3b37fSMaxime Ripard 		if (ret)
2661fc3b37fSMaxime Ripard 			goto err_disable_pixclk;
2671fc3b37fSMaxime Ripard 
268e0b9ce38SJack Zhu 		reset_control_deassert(csi2rx->pixel_rst[i]);
269e0b9ce38SJack Zhu 
2701fc3b37fSMaxime Ripard 		writel(CSI2RX_STREAM_CFG_FIFO_MODE_LARGE_BUF,
2711fc3b37fSMaxime Ripard 		       csi2rx->base + CSI2RX_STREAM_CFG_REG(i));
2721fc3b37fSMaxime Ripard 
2731dd59274SPratyush Yadav 		/*
2741dd59274SPratyush Yadav 		 * Enable one virtual channel. When multiple virtual channels
2751dd59274SPratyush Yadav 		 * are supported this will have to be changed.
2761dd59274SPratyush Yadav 		 */
2771dd59274SPratyush Yadav 		writel(CSI2RX_STREAM_DATA_CFG_VC_SELECT(0),
2781fc3b37fSMaxime Ripard 		       csi2rx->base + CSI2RX_STREAM_DATA_CFG_REG(i));
2791fc3b37fSMaxime Ripard 
2801fc3b37fSMaxime Ripard 		writel(CSI2RX_STREAM_CTRL_START,
2811fc3b37fSMaxime Ripard 		       csi2rx->base + CSI2RX_STREAM_CTRL_REG(i));
2821fc3b37fSMaxime Ripard 	}
2831fc3b37fSMaxime Ripard 
2841fc3b37fSMaxime Ripard 	ret = clk_prepare_enable(csi2rx->sys_clk);
2851fc3b37fSMaxime Ripard 	if (ret)
2861fc3b37fSMaxime Ripard 		goto err_disable_pixclk;
2871fc3b37fSMaxime Ripard 
288e0b9ce38SJack Zhu 	reset_control_deassert(csi2rx->sys_rst);
2893295cf12SJack Zhu 
2903295cf12SJack Zhu 	if (csi2rx->dphy) {
2913295cf12SJack Zhu 		ret = csi2rx_configure_ext_dphy(csi2rx);
2923295cf12SJack Zhu 		if (ret) {
2933295cf12SJack Zhu 			dev_err(csi2rx->dev,
2943295cf12SJack Zhu 				"Failed to configure external DPHY: %d\n", ret);
2953295cf12SJack Zhu 			goto err_disable_sysclk;
2963295cf12SJack Zhu 		}
2973295cf12SJack Zhu 	}
2983295cf12SJack Zhu 
2991fc3b37fSMaxime Ripard 	clk_disable_unprepare(csi2rx->p_clk);
3001fc3b37fSMaxime Ripard 
3011fc3b37fSMaxime Ripard 	return 0;
3021fc3b37fSMaxime Ripard 
3033295cf12SJack Zhu err_disable_sysclk:
3043295cf12SJack Zhu 	clk_disable_unprepare(csi2rx->sys_clk);
3051fc3b37fSMaxime Ripard err_disable_pixclk:
306e0b9ce38SJack Zhu 	for (; i > 0; i--) {
307e0b9ce38SJack Zhu 		reset_control_assert(csi2rx->pixel_rst[i - 1]);
30828d42d2fSSakari Ailus 		clk_disable_unprepare(csi2rx->pixel_clk[i - 1]);
309e0b9ce38SJack Zhu 	}
3101fc3b37fSMaxime Ripard 
3111fc3b37fSMaxime Ripard err_disable_pclk:
3121fc3b37fSMaxime Ripard 	clk_disable_unprepare(csi2rx->p_clk);
3131fc3b37fSMaxime Ripard 
3141fc3b37fSMaxime Ripard 	return ret;
3151fc3b37fSMaxime Ripard }
3161fc3b37fSMaxime Ripard 
3171fc3b37fSMaxime Ripard static void csi2rx_stop(struct csi2rx_priv *csi2rx)
3181fc3b37fSMaxime Ripard {
3191fc3b37fSMaxime Ripard 	unsigned int i;
320a64175faSPratyush Yadav 	u32 val;
321a64175faSPratyush Yadav 	int ret;
3221fc3b37fSMaxime Ripard 
3231fc3b37fSMaxime Ripard 	clk_prepare_enable(csi2rx->p_clk);
324e0b9ce38SJack Zhu 	reset_control_assert(csi2rx->sys_rst);
3251fc3b37fSMaxime Ripard 	clk_disable_unprepare(csi2rx->sys_clk);
3261fc3b37fSMaxime Ripard 
3271fc3b37fSMaxime Ripard 	for (i = 0; i < csi2rx->max_streams; i++) {
328a64175faSPratyush Yadav 		writel(CSI2RX_STREAM_CTRL_STOP,
329a64175faSPratyush Yadav 		       csi2rx->base + CSI2RX_STREAM_CTRL_REG(i));
330a64175faSPratyush Yadav 
331a64175faSPratyush Yadav 		ret = readl_relaxed_poll_timeout(csi2rx->base +
332a64175faSPratyush Yadav 						 CSI2RX_STREAM_STATUS_REG(i),
333a64175faSPratyush Yadav 						 val,
334a64175faSPratyush Yadav 						 !(val & CSI2RX_STREAM_STATUS_RDY),
335a64175faSPratyush Yadav 						 10, 10000);
336a64175faSPratyush Yadav 		if (ret)
337a64175faSPratyush Yadav 			dev_warn(csi2rx->dev,
338a64175faSPratyush Yadav 				 "Failed to stop streaming on pad%u\n", i);
3391fc3b37fSMaxime Ripard 
340e0b9ce38SJack Zhu 		reset_control_assert(csi2rx->pixel_rst[i]);
3411fc3b37fSMaxime Ripard 		clk_disable_unprepare(csi2rx->pixel_clk[i]);
3421fc3b37fSMaxime Ripard 	}
3431fc3b37fSMaxime Ripard 
344e0b9ce38SJack Zhu 	reset_control_assert(csi2rx->p_rst);
3451fc3b37fSMaxime Ripard 	clk_disable_unprepare(csi2rx->p_clk);
3461fc3b37fSMaxime Ripard 
3471fc3b37fSMaxime Ripard 	if (v4l2_subdev_call(csi2rx->source_subdev, video, s_stream, false))
3481fc3b37fSMaxime Ripard 		dev_warn(csi2rx->dev, "Couldn't disable our subdev\n");
3493295cf12SJack Zhu 
3503295cf12SJack Zhu 	if (csi2rx->dphy) {
3513295cf12SJack Zhu 		writel(0, csi2rx->base + CSI2RX_DPHY_LANE_CTRL_REG);
3523295cf12SJack Zhu 
3533295cf12SJack Zhu 		if (phy_power_off(csi2rx->dphy))
3543295cf12SJack Zhu 			dev_warn(csi2rx->dev, "Couldn't power off DPHY\n");
3553295cf12SJack Zhu 	}
3561fc3b37fSMaxime Ripard }
3571fc3b37fSMaxime Ripard 
3581fc3b37fSMaxime Ripard static int csi2rx_s_stream(struct v4l2_subdev *subdev, int enable)
3591fc3b37fSMaxime Ripard {
3601fc3b37fSMaxime Ripard 	struct csi2rx_priv *csi2rx = v4l2_subdev_to_csi2rx(subdev);
3611fc3b37fSMaxime Ripard 	int ret = 0;
3621fc3b37fSMaxime Ripard 
3631fc3b37fSMaxime Ripard 	mutex_lock(&csi2rx->lock);
3641fc3b37fSMaxime Ripard 
3651fc3b37fSMaxime Ripard 	if (enable) {
3661fc3b37fSMaxime Ripard 		/*
3671fc3b37fSMaxime Ripard 		 * If we're not the first users, there's no need to
3681fc3b37fSMaxime Ripard 		 * enable the whole controller.
3691fc3b37fSMaxime Ripard 		 */
3701fc3b37fSMaxime Ripard 		if (!csi2rx->count) {
3711fc3b37fSMaxime Ripard 			ret = csi2rx_start(csi2rx);
3721fc3b37fSMaxime Ripard 			if (ret)
3731fc3b37fSMaxime Ripard 				goto out;
3741fc3b37fSMaxime Ripard 		}
3751fc3b37fSMaxime Ripard 
3761fc3b37fSMaxime Ripard 		csi2rx->count++;
3771fc3b37fSMaxime Ripard 	} else {
3781fc3b37fSMaxime Ripard 		csi2rx->count--;
3791fc3b37fSMaxime Ripard 
3801fc3b37fSMaxime Ripard 		/*
3811fc3b37fSMaxime Ripard 		 * Let the last user turn off the lights.
3821fc3b37fSMaxime Ripard 		 */
3831fc3b37fSMaxime Ripard 		if (!csi2rx->count)
3841fc3b37fSMaxime Ripard 			csi2rx_stop(csi2rx);
3851fc3b37fSMaxime Ripard 	}
3861fc3b37fSMaxime Ripard 
3871fc3b37fSMaxime Ripard out:
3881fc3b37fSMaxime Ripard 	mutex_unlock(&csi2rx->lock);
3891fc3b37fSMaxime Ripard 	return ret;
3901fc3b37fSMaxime Ripard }
3911fc3b37fSMaxime Ripard 
392dbca7b3cSPratyush Yadav static int csi2rx_set_fmt(struct v4l2_subdev *subdev,
393dbca7b3cSPratyush Yadav 			  struct v4l2_subdev_state *state,
394dbca7b3cSPratyush Yadav 			  struct v4l2_subdev_format *format)
395dbca7b3cSPratyush Yadav {
396dbca7b3cSPratyush Yadav 	struct v4l2_mbus_framefmt *fmt;
397dbca7b3cSPratyush Yadav 	unsigned int i;
398dbca7b3cSPratyush Yadav 
399dbca7b3cSPratyush Yadav 	/* No transcoding, source and sink formats must match. */
400dbca7b3cSPratyush Yadav 	if (format->pad != CSI2RX_PAD_SINK)
401dbca7b3cSPratyush Yadav 		return v4l2_subdev_get_fmt(subdev, state, format);
402dbca7b3cSPratyush Yadav 
403dbca7b3cSPratyush Yadav 	if (!csi2rx_get_fmt_by_code(format->format.code))
404dbca7b3cSPratyush Yadav 		format->format.code = formats[0].code;
405dbca7b3cSPratyush Yadav 
406dbca7b3cSPratyush Yadav 	format->format.field = V4L2_FIELD_NONE;
407dbca7b3cSPratyush Yadav 
408dbca7b3cSPratyush Yadav 	/* Set sink format */
409*bc0e8d91SSakari Ailus 	fmt = v4l2_subdev_state_get_format(state, format->pad);
410dbca7b3cSPratyush Yadav 	*fmt = format->format;
411dbca7b3cSPratyush Yadav 
412dbca7b3cSPratyush Yadav 	/* Propagate to source formats */
413dbca7b3cSPratyush Yadav 	for (i = CSI2RX_PAD_SOURCE_STREAM0; i < CSI2RX_PAD_MAX; i++) {
414*bc0e8d91SSakari Ailus 		fmt = v4l2_subdev_state_get_format(state, i);
415dbca7b3cSPratyush Yadav 		*fmt = format->format;
416dbca7b3cSPratyush Yadav 	}
417dbca7b3cSPratyush Yadav 
418dbca7b3cSPratyush Yadav 	return 0;
419dbca7b3cSPratyush Yadav }
420dbca7b3cSPratyush Yadav 
421dbca7b3cSPratyush Yadav static int csi2rx_init_cfg(struct v4l2_subdev *subdev,
422dbca7b3cSPratyush Yadav 			   struct v4l2_subdev_state *state)
423dbca7b3cSPratyush Yadav {
424dbca7b3cSPratyush Yadav 	struct v4l2_subdev_format format = {
425dbca7b3cSPratyush Yadav 		.pad = CSI2RX_PAD_SINK,
426dbca7b3cSPratyush Yadav 		.format = {
427dbca7b3cSPratyush Yadav 			.width = 640,
428dbca7b3cSPratyush Yadav 			.height = 480,
429dbca7b3cSPratyush Yadav 			.code = MEDIA_BUS_FMT_UYVY8_1X16,
430dbca7b3cSPratyush Yadav 			.field = V4L2_FIELD_NONE,
431dbca7b3cSPratyush Yadav 			.colorspace = V4L2_COLORSPACE_SRGB,
432dbca7b3cSPratyush Yadav 			.ycbcr_enc = V4L2_YCBCR_ENC_601,
433dbca7b3cSPratyush Yadav 			.quantization = V4L2_QUANTIZATION_LIM_RANGE,
434dbca7b3cSPratyush Yadav 			.xfer_func = V4L2_XFER_FUNC_SRGB,
435dbca7b3cSPratyush Yadav 		},
436dbca7b3cSPratyush Yadav 	};
437dbca7b3cSPratyush Yadav 
438dbca7b3cSPratyush Yadav 	return csi2rx_set_fmt(subdev, state, &format);
439dbca7b3cSPratyush Yadav }
440dbca7b3cSPratyush Yadav 
441dbca7b3cSPratyush Yadav static const struct v4l2_subdev_pad_ops csi2rx_pad_ops = {
442dbca7b3cSPratyush Yadav 	.get_fmt	= v4l2_subdev_get_fmt,
443dbca7b3cSPratyush Yadav 	.set_fmt	= csi2rx_set_fmt,
444dbca7b3cSPratyush Yadav 	.init_cfg	= csi2rx_init_cfg,
445dbca7b3cSPratyush Yadav };
446dbca7b3cSPratyush Yadav 
4471fc3b37fSMaxime Ripard static const struct v4l2_subdev_video_ops csi2rx_video_ops = {
4481fc3b37fSMaxime Ripard 	.s_stream	= csi2rx_s_stream,
4491fc3b37fSMaxime Ripard };
4501fc3b37fSMaxime Ripard 
4511fc3b37fSMaxime Ripard static const struct v4l2_subdev_ops csi2rx_subdev_ops = {
4521fc3b37fSMaxime Ripard 	.video		= &csi2rx_video_ops,
453dbca7b3cSPratyush Yadav 	.pad		= &csi2rx_pad_ops,
4541fc3b37fSMaxime Ripard };
4551fc3b37fSMaxime Ripard 
456b0f46ad6SPratyush Yadav static const struct media_entity_operations csi2rx_media_ops = {
457b0f46ad6SPratyush Yadav 	.link_validate = v4l2_subdev_link_validate,
458b0f46ad6SPratyush Yadav };
459b0f46ad6SPratyush Yadav 
4601fc3b37fSMaxime Ripard static int csi2rx_async_bound(struct v4l2_async_notifier *notifier,
4611fc3b37fSMaxime Ripard 			      struct v4l2_subdev *s_subdev,
462adb2dcd5SSakari Ailus 			      struct v4l2_async_connection *asd)
4631fc3b37fSMaxime Ripard {
4641fc3b37fSMaxime Ripard 	struct v4l2_subdev *subdev = notifier->sd;
4651fc3b37fSMaxime Ripard 	struct csi2rx_priv *csi2rx = v4l2_subdev_to_csi2rx(subdev);
4661fc3b37fSMaxime Ripard 
4671fc3b37fSMaxime Ripard 	csi2rx->source_pad = media_entity_get_fwnode_pad(&s_subdev->entity,
4681fc3b37fSMaxime Ripard 							 s_subdev->fwnode,
4691fc3b37fSMaxime Ripard 							 MEDIA_PAD_FL_SOURCE);
4701fc3b37fSMaxime Ripard 	if (csi2rx->source_pad < 0) {
4711fc3b37fSMaxime Ripard 		dev_err(csi2rx->dev, "Couldn't find output pad for subdev %s\n",
4721fc3b37fSMaxime Ripard 			s_subdev->name);
4731fc3b37fSMaxime Ripard 		return csi2rx->source_pad;
4741fc3b37fSMaxime Ripard 	}
4751fc3b37fSMaxime Ripard 
4761fc3b37fSMaxime Ripard 	csi2rx->source_subdev = s_subdev;
4771fc3b37fSMaxime Ripard 
4781fc3b37fSMaxime Ripard 	dev_dbg(csi2rx->dev, "Bound %s pad: %d\n", s_subdev->name,
4791fc3b37fSMaxime Ripard 		csi2rx->source_pad);
4801fc3b37fSMaxime Ripard 
4811fc3b37fSMaxime Ripard 	return media_create_pad_link(&csi2rx->source_subdev->entity,
4821fc3b37fSMaxime Ripard 				     csi2rx->source_pad,
4831fc3b37fSMaxime Ripard 				     &csi2rx->subdev.entity, 0,
4841fc3b37fSMaxime Ripard 				     MEDIA_LNK_FL_ENABLED |
4851fc3b37fSMaxime Ripard 				     MEDIA_LNK_FL_IMMUTABLE);
4861fc3b37fSMaxime Ripard }
4871fc3b37fSMaxime Ripard 
4881fc3b37fSMaxime Ripard static const struct v4l2_async_notifier_operations csi2rx_notifier_ops = {
4891fc3b37fSMaxime Ripard 	.bound		= csi2rx_async_bound,
4901fc3b37fSMaxime Ripard };
4911fc3b37fSMaxime Ripard 
4921fc3b37fSMaxime Ripard static int csi2rx_get_resources(struct csi2rx_priv *csi2rx,
4931fc3b37fSMaxime Ripard 				struct platform_device *pdev)
4941fc3b37fSMaxime Ripard {
4951fc3b37fSMaxime Ripard 	unsigned char i;
4961fc3b37fSMaxime Ripard 	u32 dev_cfg;
497cca65f64SEvgeny Novikov 	int ret;
4981fc3b37fSMaxime Ripard 
499f5aae241SCai Huoqing 	csi2rx->base = devm_platform_ioremap_resource(pdev, 0);
5001fc3b37fSMaxime Ripard 	if (IS_ERR(csi2rx->base))
5011fc3b37fSMaxime Ripard 		return PTR_ERR(csi2rx->base);
5021fc3b37fSMaxime Ripard 
5031fc3b37fSMaxime Ripard 	csi2rx->sys_clk = devm_clk_get(&pdev->dev, "sys_clk");
5041fc3b37fSMaxime Ripard 	if (IS_ERR(csi2rx->sys_clk)) {
5051fc3b37fSMaxime Ripard 		dev_err(&pdev->dev, "Couldn't get sys clock\n");
5061fc3b37fSMaxime Ripard 		return PTR_ERR(csi2rx->sys_clk);
5071fc3b37fSMaxime Ripard 	}
5081fc3b37fSMaxime Ripard 
5091fc3b37fSMaxime Ripard 	csi2rx->p_clk = devm_clk_get(&pdev->dev, "p_clk");
5101fc3b37fSMaxime Ripard 	if (IS_ERR(csi2rx->p_clk)) {
5111fc3b37fSMaxime Ripard 		dev_err(&pdev->dev, "Couldn't get P clock\n");
5121fc3b37fSMaxime Ripard 		return PTR_ERR(csi2rx->p_clk);
5131fc3b37fSMaxime Ripard 	}
5141fc3b37fSMaxime Ripard 
515e0b9ce38SJack Zhu 	csi2rx->sys_rst = devm_reset_control_get_optional_exclusive(&pdev->dev,
516e0b9ce38SJack Zhu 								    "sys");
517e0b9ce38SJack Zhu 	if (IS_ERR(csi2rx->sys_rst))
518e0b9ce38SJack Zhu 		return PTR_ERR(csi2rx->sys_rst);
519e0b9ce38SJack Zhu 
520e0b9ce38SJack Zhu 	csi2rx->p_rst = devm_reset_control_get_optional_exclusive(&pdev->dev,
521e0b9ce38SJack Zhu 								  "reg_bank");
522e0b9ce38SJack Zhu 	if (IS_ERR(csi2rx->p_rst))
523e0b9ce38SJack Zhu 		return PTR_ERR(csi2rx->p_rst);
524e0b9ce38SJack Zhu 
5251fc3b37fSMaxime Ripard 	csi2rx->dphy = devm_phy_optional_get(&pdev->dev, "dphy");
5261fc3b37fSMaxime Ripard 	if (IS_ERR(csi2rx->dphy)) {
5271fc3b37fSMaxime Ripard 		dev_err(&pdev->dev, "Couldn't get external D-PHY\n");
5281fc3b37fSMaxime Ripard 		return PTR_ERR(csi2rx->dphy);
5291fc3b37fSMaxime Ripard 	}
5301fc3b37fSMaxime Ripard 
531cca65f64SEvgeny Novikov 	ret = clk_prepare_enable(csi2rx->p_clk);
532cca65f64SEvgeny Novikov 	if (ret) {
533cca65f64SEvgeny Novikov 		dev_err(&pdev->dev, "Couldn't prepare and enable P clock\n");
534cca65f64SEvgeny Novikov 		return ret;
535cca65f64SEvgeny Novikov 	}
536cca65f64SEvgeny Novikov 
5371fc3b37fSMaxime Ripard 	dev_cfg = readl(csi2rx->base + CSI2RX_DEVICE_CFG_REG);
5381fc3b37fSMaxime Ripard 	clk_disable_unprepare(csi2rx->p_clk);
5391fc3b37fSMaxime Ripard 
5401fc3b37fSMaxime Ripard 	csi2rx->max_lanes = dev_cfg & 7;
5411fc3b37fSMaxime Ripard 	if (csi2rx->max_lanes > CSI2RX_LANES_MAX) {
5421fc3b37fSMaxime Ripard 		dev_err(&pdev->dev, "Invalid number of lanes: %u\n",
5431fc3b37fSMaxime Ripard 			csi2rx->max_lanes);
5441fc3b37fSMaxime Ripard 		return -EINVAL;
5451fc3b37fSMaxime Ripard 	}
5461fc3b37fSMaxime Ripard 
5471fc3b37fSMaxime Ripard 	csi2rx->max_streams = (dev_cfg >> 4) & 7;
5481fc3b37fSMaxime Ripard 	if (csi2rx->max_streams > CSI2RX_STREAMS_MAX) {
5491fc3b37fSMaxime Ripard 		dev_err(&pdev->dev, "Invalid number of streams: %u\n",
5501fc3b37fSMaxime Ripard 			csi2rx->max_streams);
5511fc3b37fSMaxime Ripard 		return -EINVAL;
5521fc3b37fSMaxime Ripard 	}
5531fc3b37fSMaxime Ripard 
5541fc3b37fSMaxime Ripard 	csi2rx->has_internal_dphy = dev_cfg & BIT(3) ? true : false;
5551fc3b37fSMaxime Ripard 
5561fc3b37fSMaxime Ripard 	/*
5571fc3b37fSMaxime Ripard 	 * FIXME: Once we'll have internal D-PHY support, the check
5581fc3b37fSMaxime Ripard 	 * will need to be removed.
5591fc3b37fSMaxime Ripard 	 */
5603295cf12SJack Zhu 	if (!csi2rx->dphy && csi2rx->has_internal_dphy) {
5611fc3b37fSMaxime Ripard 		dev_err(&pdev->dev, "Internal D-PHY not supported yet\n");
5621fc3b37fSMaxime Ripard 		return -EINVAL;
5631fc3b37fSMaxime Ripard 	}
5641fc3b37fSMaxime Ripard 
5651fc3b37fSMaxime Ripard 	for (i = 0; i < csi2rx->max_streams; i++) {
566e0b9ce38SJack Zhu 		char name[16];
5671fc3b37fSMaxime Ripard 
568e0b9ce38SJack Zhu 		snprintf(name, sizeof(name), "pixel_if%u_clk", i);
569e0b9ce38SJack Zhu 		csi2rx->pixel_clk[i] = devm_clk_get(&pdev->dev, name);
5701fc3b37fSMaxime Ripard 		if (IS_ERR(csi2rx->pixel_clk[i])) {
571e0b9ce38SJack Zhu 			dev_err(&pdev->dev, "Couldn't get clock %s\n", name);
5721fc3b37fSMaxime Ripard 			return PTR_ERR(csi2rx->pixel_clk[i]);
5731fc3b37fSMaxime Ripard 		}
574e0b9ce38SJack Zhu 
575e0b9ce38SJack Zhu 		snprintf(name, sizeof(name), "pixel_if%u", i);
576e0b9ce38SJack Zhu 		csi2rx->pixel_rst[i] =
577e0b9ce38SJack Zhu 			devm_reset_control_get_optional_exclusive(&pdev->dev,
578e0b9ce38SJack Zhu 								  name);
579e0b9ce38SJack Zhu 		if (IS_ERR(csi2rx->pixel_rst[i]))
580e0b9ce38SJack Zhu 			return PTR_ERR(csi2rx->pixel_rst[i]);
5811fc3b37fSMaxime Ripard 	}
5821fc3b37fSMaxime Ripard 
5831fc3b37fSMaxime Ripard 	return 0;
5841fc3b37fSMaxime Ripard }
5851fc3b37fSMaxime Ripard 
5861fc3b37fSMaxime Ripard static int csi2rx_parse_dt(struct csi2rx_priv *csi2rx)
5871fc3b37fSMaxime Ripard {
58860359a28SSakari Ailus 	struct v4l2_fwnode_endpoint v4l2_ep = { .bus_type = 0 };
589adb2dcd5SSakari Ailus 	struct v4l2_async_connection *asd;
5901fc3b37fSMaxime Ripard 	struct fwnode_handle *fwh;
5911fc3b37fSMaxime Ripard 	struct device_node *ep;
5921fc3b37fSMaxime Ripard 	int ret;
5931fc3b37fSMaxime Ripard 
5941fc3b37fSMaxime Ripard 	ep = of_graph_get_endpoint_by_regs(csi2rx->dev->of_node, 0, 0);
5951fc3b37fSMaxime Ripard 	if (!ep)
5961fc3b37fSMaxime Ripard 		return -EINVAL;
5971fc3b37fSMaxime Ripard 
5981fc3b37fSMaxime Ripard 	fwh = of_fwnode_handle(ep);
5991fc3b37fSMaxime Ripard 	ret = v4l2_fwnode_endpoint_parse(fwh, &v4l2_ep);
6001fc3b37fSMaxime Ripard 	if (ret) {
6011fc3b37fSMaxime Ripard 		dev_err(csi2rx->dev, "Could not parse v4l2 endpoint\n");
6021fc3b37fSMaxime Ripard 		of_node_put(ep);
6031fc3b37fSMaxime Ripard 		return ret;
6041fc3b37fSMaxime Ripard 	}
6051fc3b37fSMaxime Ripard 
6062d95e7edSSakari Ailus 	if (v4l2_ep.bus_type != V4L2_MBUS_CSI2_DPHY) {
6071fc3b37fSMaxime Ripard 		dev_err(csi2rx->dev, "Unsupported media bus type: 0x%x\n",
6081fc3b37fSMaxime Ripard 			v4l2_ep.bus_type);
6091fc3b37fSMaxime Ripard 		of_node_put(ep);
6101fc3b37fSMaxime Ripard 		return -EINVAL;
6111fc3b37fSMaxime Ripard 	}
6121fc3b37fSMaxime Ripard 
6131fc3b37fSMaxime Ripard 	memcpy(csi2rx->lanes, v4l2_ep.bus.mipi_csi2.data_lanes,
6141fc3b37fSMaxime Ripard 	       sizeof(csi2rx->lanes));
6151fc3b37fSMaxime Ripard 	csi2rx->num_lanes = v4l2_ep.bus.mipi_csi2.num_data_lanes;
6161fc3b37fSMaxime Ripard 	if (csi2rx->num_lanes > csi2rx->max_lanes) {
6171fc3b37fSMaxime Ripard 		dev_err(csi2rx->dev, "Unsupported number of data-lanes: %d\n",
6181fc3b37fSMaxime Ripard 			csi2rx->num_lanes);
6191fc3b37fSMaxime Ripard 		of_node_put(ep);
6201fc3b37fSMaxime Ripard 		return -EINVAL;
6211fc3b37fSMaxime Ripard 	}
6221fc3b37fSMaxime Ripard 
623b8ec754aSSakari Ailus 	v4l2_async_subdev_nf_init(&csi2rx->notifier, &csi2rx->subdev);
6241fc3b37fSMaxime Ripard 
6253c8c1539SSakari Ailus 	asd = v4l2_async_nf_add_fwnode_remote(&csi2rx->notifier, fwh,
626adb2dcd5SSakari Ailus 					      struct v4l2_async_connection);
62788367b15SEzequiel Garcia 	of_node_put(ep);
628b2701715SPratyush Yadav 	if (IS_ERR(asd)) {
629b2701715SPratyush Yadav 		v4l2_async_nf_cleanup(&csi2rx->notifier);
63088367b15SEzequiel Garcia 		return PTR_ERR(asd);
631b2701715SPratyush Yadav 	}
632d079f94cSSteve Longerbeam 
6331fc3b37fSMaxime Ripard 	csi2rx->notifier.ops = &csi2rx_notifier_ops;
6341fc3b37fSMaxime Ripard 
635b8ec754aSSakari Ailus 	ret = v4l2_async_nf_register(&csi2rx->notifier);
636d079f94cSSteve Longerbeam 	if (ret)
6373c8c1539SSakari Ailus 		v4l2_async_nf_cleanup(&csi2rx->notifier);
638d079f94cSSteve Longerbeam 
639d079f94cSSteve Longerbeam 	return ret;
6401fc3b37fSMaxime Ripard }
6411fc3b37fSMaxime Ripard 
6421fc3b37fSMaxime Ripard static int csi2rx_probe(struct platform_device *pdev)
6431fc3b37fSMaxime Ripard {
6441fc3b37fSMaxime Ripard 	struct csi2rx_priv *csi2rx;
6451fc3b37fSMaxime Ripard 	unsigned int i;
6461fc3b37fSMaxime Ripard 	int ret;
6471fc3b37fSMaxime Ripard 
6481fc3b37fSMaxime Ripard 	csi2rx = kzalloc(sizeof(*csi2rx), GFP_KERNEL);
6491fc3b37fSMaxime Ripard 	if (!csi2rx)
6501fc3b37fSMaxime Ripard 		return -ENOMEM;
6511fc3b37fSMaxime Ripard 	platform_set_drvdata(pdev, csi2rx);
6521fc3b37fSMaxime Ripard 	csi2rx->dev = &pdev->dev;
6531fc3b37fSMaxime Ripard 	mutex_init(&csi2rx->lock);
6541fc3b37fSMaxime Ripard 
6551fc3b37fSMaxime Ripard 	ret = csi2rx_get_resources(csi2rx, pdev);
6561fc3b37fSMaxime Ripard 	if (ret)
6571fc3b37fSMaxime Ripard 		goto err_free_priv;
6581fc3b37fSMaxime Ripard 
6591fc3b37fSMaxime Ripard 	ret = csi2rx_parse_dt(csi2rx);
6601fc3b37fSMaxime Ripard 	if (ret)
6611fc3b37fSMaxime Ripard 		goto err_free_priv;
6621fc3b37fSMaxime Ripard 
6631fc3b37fSMaxime Ripard 	csi2rx->subdev.owner = THIS_MODULE;
6641fc3b37fSMaxime Ripard 	csi2rx->subdev.dev = &pdev->dev;
6651fc3b37fSMaxime Ripard 	v4l2_subdev_init(&csi2rx->subdev, &csi2rx_subdev_ops);
6661fc3b37fSMaxime Ripard 	v4l2_set_subdevdata(&csi2rx->subdev, &pdev->dev);
6678cdd708fSHans Verkuil 	snprintf(csi2rx->subdev.name, sizeof(csi2rx->subdev.name),
6688cdd708fSHans Verkuil 		 "%s.%s", KBUILD_MODNAME, dev_name(&pdev->dev));
6691fc3b37fSMaxime Ripard 
6701fc3b37fSMaxime Ripard 	/* Create our media pads */
6711fc3b37fSMaxime Ripard 	csi2rx->subdev.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
6721fc3b37fSMaxime Ripard 	csi2rx->pads[CSI2RX_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
6731fc3b37fSMaxime Ripard 	for (i = CSI2RX_PAD_SOURCE_STREAM0; i < CSI2RX_PAD_MAX; i++)
6741fc3b37fSMaxime Ripard 		csi2rx->pads[i].flags = MEDIA_PAD_FL_SOURCE;
675c6ed7a39SPratyush Yadav 	csi2rx->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
676b0f46ad6SPratyush Yadav 	csi2rx->subdev.entity.ops = &csi2rx_media_ops;
6771fc3b37fSMaxime Ripard 
6781fc3b37fSMaxime Ripard 	ret = media_entity_pads_init(&csi2rx->subdev.entity, CSI2RX_PAD_MAX,
6791fc3b37fSMaxime Ripard 				     csi2rx->pads);
6801fc3b37fSMaxime Ripard 	if (ret)
681d079f94cSSteve Longerbeam 		goto err_cleanup;
6821fc3b37fSMaxime Ripard 
683dbca7b3cSPratyush Yadav 	ret = v4l2_subdev_init_finalize(&csi2rx->subdev);
684dbca7b3cSPratyush Yadav 	if (ret)
685dbca7b3cSPratyush Yadav 		goto err_cleanup;
686dbca7b3cSPratyush Yadav 
6871fc3b37fSMaxime Ripard 	ret = v4l2_async_register_subdev(&csi2rx->subdev);
6881fc3b37fSMaxime Ripard 	if (ret < 0)
689dbca7b3cSPratyush Yadav 		goto err_free_state;
6901fc3b37fSMaxime Ripard 
6911fc3b37fSMaxime Ripard 	dev_info(&pdev->dev,
6921fc3b37fSMaxime Ripard 		 "Probed CSI2RX with %u/%u lanes, %u streams, %s D-PHY\n",
6931fc3b37fSMaxime Ripard 		 csi2rx->num_lanes, csi2rx->max_lanes, csi2rx->max_streams,
6943295cf12SJack Zhu 		 csi2rx->dphy ? "external" :
6951fc3b37fSMaxime Ripard 		 csi2rx->has_internal_dphy ? "internal" : "no");
6961fc3b37fSMaxime Ripard 
6971fc3b37fSMaxime Ripard 	return 0;
6981fc3b37fSMaxime Ripard 
699dbca7b3cSPratyush Yadav err_free_state:
700dbca7b3cSPratyush Yadav 	v4l2_subdev_cleanup(&csi2rx->subdev);
701d079f94cSSteve Longerbeam err_cleanup:
702b2701715SPratyush Yadav 	v4l2_async_nf_unregister(&csi2rx->notifier);
7033c8c1539SSakari Ailus 	v4l2_async_nf_cleanup(&csi2rx->notifier);
704aee5b415SPratyush Yadav 	media_entity_cleanup(&csi2rx->subdev.entity);
7051fc3b37fSMaxime Ripard err_free_priv:
7061fc3b37fSMaxime Ripard 	kfree(csi2rx);
7071fc3b37fSMaxime Ripard 	return ret;
7081fc3b37fSMaxime Ripard }
7091fc3b37fSMaxime Ripard 
710bbb3f635SUwe Kleine-König static void csi2rx_remove(struct platform_device *pdev)
7111fc3b37fSMaxime Ripard {
7121fc3b37fSMaxime Ripard 	struct csi2rx_priv *csi2rx = platform_get_drvdata(pdev);
7131fc3b37fSMaxime Ripard 
714b2701715SPratyush Yadav 	v4l2_async_nf_unregister(&csi2rx->notifier);
715b2701715SPratyush Yadav 	v4l2_async_nf_cleanup(&csi2rx->notifier);
7161fc3b37fSMaxime Ripard 	v4l2_async_unregister_subdev(&csi2rx->subdev);
717dbca7b3cSPratyush Yadav 	v4l2_subdev_cleanup(&csi2rx->subdev);
718aee5b415SPratyush Yadav 	media_entity_cleanup(&csi2rx->subdev.entity);
7191fc3b37fSMaxime Ripard 	kfree(csi2rx);
7201fc3b37fSMaxime Ripard }
7211fc3b37fSMaxime Ripard 
7221fc3b37fSMaxime Ripard static const struct of_device_id csi2rx_of_table[] = {
72371e8d6e4SJack Zhu 	{ .compatible = "starfive,jh7110-csi2rx" },
7241fc3b37fSMaxime Ripard 	{ .compatible = "cdns,csi2rx" },
7251fc3b37fSMaxime Ripard 	{ },
7261fc3b37fSMaxime Ripard };
7271fc3b37fSMaxime Ripard MODULE_DEVICE_TABLE(of, csi2rx_of_table);
7281fc3b37fSMaxime Ripard 
7291fc3b37fSMaxime Ripard static struct platform_driver csi2rx_driver = {
7301fc3b37fSMaxime Ripard 	.probe	= csi2rx_probe,
731bbb3f635SUwe Kleine-König 	.remove_new = csi2rx_remove,
7321fc3b37fSMaxime Ripard 
7331fc3b37fSMaxime Ripard 	.driver	= {
7341fc3b37fSMaxime Ripard 		.name		= "cdns-csi2rx",
7351fc3b37fSMaxime Ripard 		.of_match_table	= csi2rx_of_table,
7361fc3b37fSMaxime Ripard 	},
7371fc3b37fSMaxime Ripard };
7381fc3b37fSMaxime Ripard module_platform_driver(csi2rx_driver);
7391fc3b37fSMaxime Ripard MODULE_AUTHOR("Maxime Ripard <maxime.ripard@bootlin.com>");
7401fc3b37fSMaxime Ripard MODULE_DESCRIPTION("Cadence CSI2-RX controller");
7411fc3b37fSMaxime Ripard MODULE_LICENSE("GPL");
742