Re: [PATCH v8 03/12] media: staging/imx7: add imx7 CSI subdev driver

[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]

 



Hi Rui,

On Wed, Nov 21, 2018 at 11:15:49AM +0000, Rui Miguel Silva wrote:
> This add the media entity subdevice and control driver for the i.MX7
> CMOS Sensor Interface.
> 
> Signed-off-by: Rui Miguel Silva <rui.silva@xxxxxxxxxx>
> ---
>  drivers/staging/media/imx/Kconfig          |    9 +-
>  drivers/staging/media/imx/Makefile         |    2 +
>  drivers/staging/media/imx/imx7-media-csi.c | 1352 ++++++++++++++++++++
>  3 files changed, 1362 insertions(+), 1 deletion(-)
>  create mode 100644 drivers/staging/media/imx/imx7-media-csi.c
> 
> diff --git a/drivers/staging/media/imx/Kconfig b/drivers/staging/media/imx/Kconfig
> index bfc17de56b17..40a11f988fc6 100644
> --- a/drivers/staging/media/imx/Kconfig
> +++ b/drivers/staging/media/imx/Kconfig
> @@ -11,7 +11,7 @@ config VIDEO_IMX_MEDIA
>  	  driver for the i.MX5/6 SOC.
>  
>  if VIDEO_IMX_MEDIA
> -menu "i.MX5/6 Media Sub devices"
> +menu "i.MX5/6/7 Media Sub devices"
>  
>  config VIDEO_IMX_CSI
>  	tristate "i.MX5/6 Camera Sensor Interface driver"
> @@ -20,5 +20,12 @@ config VIDEO_IMX_CSI
>  	---help---
>  	  A video4linux camera sensor interface driver for i.MX5/6.
>  
> +config VIDEO_IMX7_CSI
> +	tristate "i.MX7 Camera Sensor Interface driver"
> +	depends on VIDEO_IMX_MEDIA && VIDEO_DEV && I2C

If you need V4L2 fwnode, you should select V4L2_FWNODE. (I ignore if it's
already selected by VIDEO_IMX_MEDIA.)

> +	default y
> +	---help---
> +	  A video4linux camera sensor interface driver for i.MX7.
> +
>  endmenu
>  endif
> diff --git a/drivers/staging/media/imx/Makefile b/drivers/staging/media/imx/Makefile
> index a30b3033f9a3..074f016d3519 100644
> --- a/drivers/staging/media/imx/Makefile
> +++ b/drivers/staging/media/imx/Makefile
> @@ -12,3 +12,5 @@ obj-$(CONFIG_VIDEO_IMX_MEDIA) += imx-media-ic.o
>  
>  obj-$(CONFIG_VIDEO_IMX_CSI) += imx-media-csi.o
>  obj-$(CONFIG_VIDEO_IMX_CSI) += imx6-mipi-csi2.o
> +
> +obj-$(CONFIG_VIDEO_IMX7_CSI) += imx7-media-csi.o
> diff --git a/drivers/staging/media/imx/imx7-media-csi.c b/drivers/staging/media/imx/imx7-media-csi.c
> new file mode 100644
> index 000000000000..ec5a20880bb6
> --- /dev/null
> +++ b/drivers/staging/media/imx/imx7-media-csi.c
> @@ -0,0 +1,1352 @@
> +// SPDX-License-Identifier: GPL
> +/*
> + * V4L2 Capture CSI Subdev for Freescale i.MX7 SOC
> + *
> + * Copyright (c) 2018 Linaro Ltd
> + *
> + */
> +
> +#include <linux/clk.h>
> +#include <linux/delay.h>
> +#include <linux/gcd.h>
> +#include <linux/interrupt.h>
> +#include <linux/mfd/syscon.h>
> +#include <linux/module.h>
> +#include <linux/of_graph.h>
> +#include <linux/pinctrl/consumer.h>
> +#include <linux/platform_device.h>
> +#include <linux/regmap.h>
> +#include <linux/types.h>
> +
> +#include <media/v4l2-ctrls.h>
> +#include <media/v4l2-device.h>
> +#include <media/v4l2-event.h>
> +#include <media/v4l2-fwnode.h>
> +#include <media/v4l2-mc.h>
> +#include <media/v4l2-subdev.h>
> +#include <media/videobuf2-dma-contig.h>
> +
> +#include <media/imx.h>
> +#include "imx-media.h"
> +
> +#define IMX7_CSI_PAD_SINK	0
> +#define IMX7_CSI_PAD_SRC	1
> +#define IMX7_CSI_PADS_NUM	2
> +
> +/* reset values */
> +#define CSICR1_RESET_VAL	0x40000800
> +#define CSICR2_RESET_VAL	0x0
> +#define CSICR3_RESET_VAL	0x0
> +
> +/* csi control reg 1 */
> +#define BIT_SWAP16_EN		BIT(31)
> +#define BIT_EXT_VSYNC		BIT(30)
> +#define BIT_EOF_INT_EN		BIT(29)
> +#define BIT_PRP_IF_EN		BIT(28)
> +#define BIT_CCIR_MODE		BIT(27)
> +#define BIT_COF_INT_EN		BIT(26)
> +#define BIT_SF_OR_INTEN		BIT(25)
> +#define BIT_RF_OR_INTEN		BIT(24)
> +#define BIT_SFF_DMA_DONE_INTEN  BIT(22)
> +#define BIT_STATFF_INTEN	BIT(21)
> +#define BIT_FB2_DMA_DONE_INTEN  BIT(20)
> +#define BIT_FB1_DMA_DONE_INTEN  BIT(19)
> +#define BIT_RXFF_INTEN		BIT(18)
> +#define BIT_SOF_POL		BIT(17)
> +#define BIT_SOF_INTEN		BIT(16)
> +#define BIT_MCLKDIV		(0xF << 12)
> +#define BIT_HSYNC_POL		BIT(11)
> +#define BIT_CCIR_EN		BIT(10)
> +#define BIT_MCLKEN		BIT(9)
> +#define BIT_FCC			BIT(8)
> +#define BIT_PACK_DIR		BIT(7)
> +#define BIT_CLR_STATFIFO	BIT(6)
> +#define BIT_CLR_RXFIFO		BIT(5)
> +#define BIT_GCLK_MODE		BIT(4)
> +#define BIT_INV_DATA		BIT(3)
> +#define BIT_INV_PCLK		BIT(2)
> +#define BIT_REDGE		BIT(1)
> +#define BIT_PIXEL_BIT		BIT(0)
> +
> +#define SHIFT_MCLKDIV		12
> +
> +/* control reg 3 */
> +#define BIT_FRMCNT		(0xFFFF << 16)
> +#define BIT_FRMCNT_RST		BIT(15)
> +#define BIT_DMA_REFLASH_RFF	BIT(14)
> +#define BIT_DMA_REFLASH_SFF	BIT(13)
> +#define BIT_DMA_REQ_EN_RFF	BIT(12)
> +#define BIT_DMA_REQ_EN_SFF	BIT(11)
> +#define BIT_STATFF_LEVEL	(0x7 << 8)
> +#define BIT_HRESP_ERR_EN	BIT(7)
> +#define BIT_RXFF_LEVEL		(0x7 << 4)
> +#define BIT_TWO_8BIT_SENSOR	BIT(3)
> +#define BIT_ZERO_PACK_EN	BIT(2)
> +#define BIT_ECC_INT_EN		BIT(1)
> +#define BIT_ECC_AUTO_EN		BIT(0)
> +
> +#define SHIFT_FRMCNT		16
> +#define SHIFT_RXFIFO_LEVEL	4
> +
> +/* csi status reg */
> +#define BIT_ADDR_CH_ERR_INT	BIT(28)
> +#define BIT_FIELD0_INT		BIT(27)
> +#define BIT_FIELD1_INT		BIT(26)
> +#define BIT_SFF_OR_INT		BIT(25)
> +#define BIT_RFF_OR_INT		BIT(24)
> +#define BIT_DMA_TSF_DONE_SFF	BIT(22)
> +#define BIT_STATFF_INT		BIT(21)
> +#define BIT_DMA_TSF_DONE_FB2	BIT(20)
> +#define BIT_DMA_TSF_DONE_FB1	BIT(19)
> +#define BIT_RXFF_INT		BIT(18)
> +#define BIT_EOF_INT		BIT(17)
> +#define BIT_SOF_INT		BIT(16)
> +#define BIT_F2_INT		BIT(15)
> +#define BIT_F1_INT		BIT(14)
> +#define BIT_COF_INT		BIT(13)
> +#define BIT_HRESP_ERR_INT	BIT(7)
> +#define BIT_ECC_INT		BIT(1)
> +#define BIT_DRDY		BIT(0)
> +
> +/* csi control reg 18 */
> +#define BIT_CSI_HW_ENABLE		BIT(31)
> +#define BIT_MIPI_DATA_FORMAT_RAW8	(0x2a << 25)
> +#define BIT_MIPI_DATA_FORMAT_RAW10	(0x2b << 25)
> +#define BIT_MIPI_DATA_FORMAT_RAW12	(0x2c << 25)
> +#define BIT_MIPI_DATA_FORMAT_RAW14	(0x2d << 25)
> +#define BIT_MIPI_DATA_FORMAT_YUV422_8B	(0x1e << 25)
> +#define BIT_MIPI_DATA_FORMAT_MASK	(0x3F << 25)
> +#define BIT_MIPI_DATA_FORMAT_OFFSET	25
> +#define BIT_DATA_FROM_MIPI		BIT(22)
> +#define BIT_MIPI_YU_SWAP		BIT(21)
> +#define BIT_MIPI_DOUBLE_CMPNT		BIT(20)
> +#define BIT_BASEADDR_CHG_ERR_EN		BIT(9)
> +#define BIT_BASEADDR_SWITCH_SEL		BIT(5)
> +#define BIT_BASEADDR_SWITCH_EN		BIT(4)
> +#define BIT_PARALLEL24_EN		BIT(3)
> +#define BIT_DEINTERLACE_EN		BIT(2)
> +#define BIT_TVDECODER_IN_EN		BIT(1)
> +#define BIT_NTSC_EN			BIT(0)
> +
> +#define CSI_MCLK_VF		1
> +#define CSI_MCLK_ENC		2
> +#define CSI_MCLK_RAW		4
> +#define CSI_MCLK_I2C		8
> +
> +#define CSI_CSICR1		0x0
> +#define CSI_CSICR2		0x4
> +#define CSI_CSICR3		0x8
> +#define CSI_STATFIFO		0xC
> +#define CSI_CSIRXFIFO		0x10
> +#define CSI_CSIRXCNT		0x14
> +#define CSI_CSISR		0x18
> +
> +#define CSI_CSIDBG		0x1C
> +#define CSI_CSIDMASA_STATFIFO	0x20
> +#define CSI_CSIDMATS_STATFIFO	0x24
> +#define CSI_CSIDMASA_FB1	0x28
> +#define CSI_CSIDMASA_FB2	0x2C
> +#define CSI_CSIFBUF_PARA	0x30
> +#define CSI_CSIIMAG_PARA	0x34
> +
> +#define CSI_CSICR18		0x48
> +#define CSI_CSICR19		0x4c
> +
> +struct imx7_csi {
> +	struct device *dev;
> +	struct imx_media_dev *md;
> +	struct v4l2_subdev sd;
> +	struct imx_media_video_dev *vdev;
> +	struct imx_media_dev *imxmd;
> +	struct media_pad pad[IMX7_CSI_PADS_NUM];
> +
> +	struct mutex lock;
> +	spinlock_t irqlock;
> +
> +	struct v4l2_subdev *src_sd;
> +
> +	struct media_entity *sink;
> +
> +	struct v4l2_fwnode_endpoint upstream_ep;
> +
> +	struct v4l2_mbus_framefmt format_mbus[IMX7_CSI_PADS_NUM];
> +	const struct imx_media_pixfmt *cc[IMX7_CSI_PADS_NUM];
> +	struct v4l2_fract frame_interval[IMX7_CSI_PADS_NUM];
> +
> +	struct v4l2_ctrl_handler ctrl_hdlr;
> +
> +	void __iomem *regbase;
> +	int irq;
> +
> +	struct clk *clk_disp_axi;
> +	struct clk *clk_disp_dcic;
> +	struct clk *clk_csi_mclk;
> +
> +	/* active vb2 buffers to send to video dev sink */
> +	struct imx_media_buffer *active_vb2_buf[2];
> +	struct imx_media_dma_buf underrun_buf;
> +
> +	int buf_num;
> +	u32 frame_sequence;
> +
> +	bool last_eof;
> +	bool is_init;
> +	bool is_streaming;
> +	bool is_csi2;
> +
> +	struct completion last_eof_completion;
> +};
> +
> +#define imx7_csi_reg_read(_csi, _offset) __raw_readl(_csi->regbase + _offset)
> +#define imx7_csi_reg_write(_csi, _val, _offset) \
> +	__raw_writel(_val, _csi->regbase + _offset)
> +
> +static void imx7_csi_clk_enable(struct imx7_csi *csi)
> +{
> +	clk_prepare_enable(csi->clk_disp_axi);
> +	clk_prepare_enable(csi->clk_disp_dcic);
> +	clk_prepare_enable(csi->clk_csi_mclk);

Could you use clk_bulk?prepare_enable()? Same below in ...clk_disable().

> +}
> +
> +static void imx7_csi_clk_disable(struct imx7_csi *csi)
> +{
> +	clk_disable_unprepare(csi->clk_csi_mclk);
> +	clk_disable_unprepare(csi->clk_disp_dcic);
> +	clk_disable_unprepare(csi->clk_disp_axi);
> +}
> +
> +static void imx7_csi_hw_reset(struct imx7_csi *csi)
> +{
> +	imx7_csi_reg_write(csi,
> +			   imx7_csi_reg_read(csi, CSI_CSICR3) | BIT_FRMCNT_RST,
> +			   CSI_CSICR3);
> +
> +	imx7_csi_reg_write(csi, CSICR1_RESET_VAL, CSI_CSICR1);
> +	imx7_csi_reg_write(csi, CSICR2_RESET_VAL, CSI_CSICR2);
> +	imx7_csi_reg_write(csi, CSICR3_RESET_VAL, CSI_CSICR3);
> +}
> +
> +static unsigned long imx7_csi_irq_clear(struct imx7_csi *csi)
> +{
> +	unsigned long isr;
> +
> +	isr = imx7_csi_reg_read(csi, CSI_CSISR);
> +	imx7_csi_reg_write(csi, isr, CSI_CSISR);
> +
> +	return isr;
> +}
> +
> +static void imx7_csi_init_interface(struct imx7_csi *csi)
> +{
> +	unsigned int val = 0;
> +	unsigned int imag_para;
> +
> +	val = BIT_SOF_POL | BIT_REDGE | BIT_GCLK_MODE | BIT_HSYNC_POL |
> +		BIT_FCC | 1 << SHIFT_MCLKDIV | BIT_MCLKEN;
> +	imx7_csi_reg_write(csi, val, CSI_CSICR1);
> +
> +	imag_para = (800 << 16) | 600;
> +	imx7_csi_reg_write(csi, imag_para, CSI_CSIIMAG_PARA);
> +
> +	val = BIT_DMA_REFLASH_RFF;
> +	imx7_csi_reg_write(csi, val, CSI_CSICR3);
> +}
> +
> +static void imx7_csi_hw_enable_irq(struct imx7_csi *csi)
> +{
> +	unsigned long cr1 = imx7_csi_reg_read(csi, CSI_CSICR1);
> +
> +	cr1 |= BIT_SOF_INTEN;
> +	cr1 |= BIT_RFF_OR_INT;
> +
> +	/* still capture needs DMA interrupt */
> +	cr1 |= BIT_FB1_DMA_DONE_INTEN;
> +	cr1 |= BIT_FB2_DMA_DONE_INTEN;
> +
> +	cr1 |= BIT_EOF_INT_EN;
> +
> +	imx7_csi_reg_write(csi, cr1, CSI_CSICR1);
> +}
> +
> +static void imx7_csi_hw_disable_irq(struct imx7_csi *csi)
> +{
> +	unsigned long cr1 = imx7_csi_reg_read(csi, CSI_CSICR1);
> +
> +	cr1 &= ~BIT_SOF_INTEN;
> +	cr1 &= ~BIT_RFF_OR_INT;
> +	cr1 &= ~BIT_FB1_DMA_DONE_INTEN;
> +	cr1 &= ~BIT_FB2_DMA_DONE_INTEN;
> +	cr1 &= ~BIT_EOF_INT_EN;
> +
> +	imx7_csi_reg_write(csi, cr1, CSI_CSICR1);
> +}
> +
> +static void imx7_csi_hw_enable(struct imx7_csi *csi)
> +{
> +	unsigned long cr = imx7_csi_reg_read(csi, CSI_CSICR18);
> +
> +	cr |= BIT_CSI_HW_ENABLE;
> +
> +	imx7_csi_reg_write(csi, cr, CSI_CSICR18);
> +}
> +
> +static void imx7_csi_hw_disable(struct imx7_csi *csi)
> +{
> +	unsigned long cr = imx7_csi_reg_read(csi, CSI_CSICR18);
> +
> +	cr &= ~BIT_CSI_HW_ENABLE;
> +
> +	imx7_csi_reg_write(csi, cr, CSI_CSICR18);
> +}
> +
> +static void imx7_csi_dma_reflash(struct imx7_csi *csi)
> +{
> +	unsigned long cr3 = imx7_csi_reg_read(csi, CSI_CSICR18);
> +
> +	cr3 = imx7_csi_reg_read(csi, CSI_CSICR3);
> +	cr3 |= BIT_DMA_REFLASH_RFF;
> +	imx7_csi_reg_write(csi, cr3, CSI_CSICR3);
> +}
> +
> +static void imx7_csi_rx_fifo_clear(struct imx7_csi *csi)
> +{
> +	unsigned long cr1;
> +
> +	cr1 = imx7_csi_reg_read(csi, CSI_CSICR1);
> +	imx7_csi_reg_write(csi, cr1 & ~BIT_FCC, CSI_CSICR1);
> +	cr1 = imx7_csi_reg_read(csi, CSI_CSICR1);
> +	imx7_csi_reg_write(csi, cr1 | BIT_CLR_RXFIFO, CSI_CSICR1);
> +
> +	cr1 = imx7_csi_reg_read(csi, CSI_CSICR1);
> +	imx7_csi_reg_write(csi, cr1 | BIT_FCC, CSI_CSICR1);
> +}
> +
> +static void imx7_csi_buf_stride_set(struct imx7_csi *csi, u32 stride)
> +{
> +	imx7_csi_reg_write(csi, stride, CSI_CSIFBUF_PARA);
> +}
> +
> +static void imx7_csi_deinterlace_enable(struct imx7_csi *csi, bool enable)
> +{
> +	unsigned long cr18 = imx7_csi_reg_read(csi, CSI_CSICR18);
> +
> +	if (enable)
> +		cr18 |= BIT_DEINTERLACE_EN;
> +	else
> +		cr18 &= ~BIT_DEINTERLACE_EN;
> +
> +	imx7_csi_reg_write(csi, cr18, CSI_CSICR18);
> +}
> +
> +static void imx7_csi_dmareq_rff_enable(struct imx7_csi *csi)
> +{
> +	unsigned long cr3 = imx7_csi_reg_read(csi, CSI_CSICR3);
> +	unsigned long cr2 = imx7_csi_reg_read(csi, CSI_CSICR2);
> +
> +	/* Burst Type of DMA Transfer from RxFIFO. INCR16 */
> +	cr2 |= 0xC0000000;
> +
> +	cr3 |= BIT_DMA_REQ_EN_RFF;
> +	cr3 |= BIT_HRESP_ERR_EN;
> +	cr3 &= ~BIT_RXFF_LEVEL;
> +	cr3 |= 0x2 << 4;
> +
> +	imx7_csi_reg_write(csi, cr3, CSI_CSICR3);
> +	imx7_csi_reg_write(csi, cr2, CSI_CSICR2);
> +}
> +
> +static void imx7_csi_dmareq_rff_disable(struct imx7_csi *csi)
> +{
> +	unsigned long cr3 = imx7_csi_reg_read(csi, CSI_CSICR3);
> +
> +	cr3 &= ~BIT_DMA_REQ_EN_RFF;
> +	cr3 &= ~BIT_HRESP_ERR_EN;
> +	imx7_csi_reg_write(csi, cr3, CSI_CSICR3);
> +}
> +
> +static void imx7_csi_set_imagpara(struct imx7_csi *csi, int width, int height)
> +{
> +	int imag_para;
> +	int rx_count;
> +
> +	rx_count = (width * height) >> 2;
> +	imx7_csi_reg_write(csi, rx_count, CSI_CSIRXCNT);
> +
> +	imag_para = (width << 16) | height;
> +	imx7_csi_reg_write(csi, imag_para, CSI_CSIIMAG_PARA);
> +
> +	/* reflash the embedded DMA controller */
> +	imx7_csi_dma_reflash(csi);
> +}
> +
> +static void imx7_csi_sw_reset(struct imx7_csi *csi)
> +{
> +	imx7_csi_hw_disable(csi);
> +
> +	imx7_csi_rx_fifo_clear(csi);
> +
> +	imx7_csi_dma_reflash(csi);
> +
> +	usleep_range(2000, 3000);
> +
> +	imx7_csi_irq_clear(csi);
> +
> +	imx7_csi_hw_enable(csi);
> +}
> +
> +static void imx7_csi_error_recovery(struct imx7_csi *csi)
> +{
> +	imx7_csi_hw_disable(csi);
> +
> +	imx7_csi_rx_fifo_clear(csi);
> +
> +	imx7_csi_dma_reflash(csi);
> +
> +	imx7_csi_hw_enable(csi);
> +}
> +
> +static void imx7_csi_init(struct imx7_csi *csi)
> +{
> +	if (csi->is_init)
> +		return;
> +
> +	imx7_csi_clk_enable(csi);
> +	imx7_csi_hw_reset(csi);
> +	imx7_csi_init_interface(csi);
> +	imx7_csi_dmareq_rff_enable(csi);
> +
> +	csi->is_init = true;
> +}
> +
> +static void imx7_csi_deinit(struct imx7_csi *csi)
> +{
> +	if (!csi->is_init)
> +		return;
> +
> +	imx7_csi_hw_reset(csi);
> +	imx7_csi_init_interface(csi);
> +	imx7_csi_dmareq_rff_disable(csi);
> +	imx7_csi_clk_disable(csi);
> +
> +	csi->is_init = false;
> +}
> +
> +static int imx7_csi_get_upstream_endpoint(struct imx7_csi *csi,
> +					  struct v4l2_fwnode_endpoint *ep,
> +					  bool skip_mux)
> +{
> +	struct device_node *endpoint, *port;
> +	struct media_entity *src;
> +	struct v4l2_subdev *sd;
> +	struct media_pad *pad;
> +
> +	if (!csi->src_sd)
> +		return -EPIPE;
> +
> +	src = &csi->src_sd->entity;
> +
> +skip_video_mux:
> +	/* get source pad of entity directly upstream from src */
> +	pad = imx_media_find_upstream_pad(csi->md, src, 0);
> +	if (IS_ERR(pad))
> +		return PTR_ERR(pad);
> +
> +	sd = media_entity_to_v4l2_subdev(pad->entity);
> +
> +	/* To get bus type we may need to skip video mux */
> +	if (skip_mux && src->function == MEDIA_ENT_F_VID_MUX) {
> +		src = &sd->entity;
> +		goto skip_video_mux;
> +	}
> +
> +	/*
> +	 * NOTE: this assumes an OF-graph port id is the same as a
> +	 * media pad index.
> +	 */
> +	port = of_graph_get_port_by_id(sd->dev->of_node, pad->index);
> +	if (!port)
> +		return -ENODEV;
> +
> +	endpoint = of_get_next_child(port, NULL);
> +	of_node_put(port);
> +	if (!endpoint)
> +		return -ENODEV;
> +
> +	v4l2_fwnode_endpoint_parse(of_fwnode_handle(endpoint), ep);

I'd suggest to set the bus type before calling this. And do zero the rest of
the struct as well (or put defaults in); otherwise you'll get unexpected
results back.

Just wondering... this seems complicated. What's the exact problem this
function is intended to address?

> +	of_node_put(endpoint);
> +
> +	return 0;
> +}
> +
> +static int imx7_csi_link_setup(struct media_entity *entity,
> +			       const struct media_pad *local,
> +			       const struct media_pad *remote, u32 flags)
> +{
> +	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
> +	struct imx7_csi *csi = v4l2_get_subdevdata(sd);
> +	struct v4l2_subdev *remote_sd;
> +	int ret = 0;
> +
> +	dev_dbg(csi->dev, "link setup %s -> %s\n", remote->entity->name,
> +		local->entity->name);
> +
> +	mutex_lock(&csi->lock);
> +
> +	if (local->flags & MEDIA_PAD_FL_SINK) {
> +		if (!is_media_entity_v4l2_subdev(remote->entity)) {
> +			ret = -EINVAL;
> +			goto unlock;
> +		}
> +
> +		remote_sd = media_entity_to_v4l2_subdev(remote->entity);
> +
> +		if (flags & MEDIA_LNK_FL_ENABLED) {
> +			if (csi->src_sd) {
> +				ret = -EBUSY;
> +				goto unlock;
> +			}
> +			csi->src_sd = remote_sd;
> +		} else {
> +			csi->src_sd = NULL;
> +		}
> +
> +		goto init;
> +	}
> +
> +	/* source pad */
> +	if (flags & MEDIA_LNK_FL_ENABLED) {
> +		if (csi->sink) {
> +			ret = -EBUSY;
> +			goto unlock;
> +		}
> +		csi->sink = remote->entity;
> +	} else {
> +		v4l2_ctrl_handler_free(&csi->ctrl_hdlr);
> +		v4l2_ctrl_handler_init(&csi->ctrl_hdlr, 0);
> +		csi->sink = NULL;
> +	}
> +
> +init:
> +	if (csi->sink || csi->src_sd)
> +		imx7_csi_init(csi);
> +	else
> +		imx7_csi_deinit(csi);
> +
> +unlock:
> +	mutex_unlock(&csi->lock);
> +
> +	return ret;
> +}
> +
> +static int imx7_csi_pad_link_validate(struct v4l2_subdev *sd,
> +				      struct media_link *link,
> +				      struct v4l2_subdev_format *source_fmt,
> +				      struct v4l2_subdev_format *sink_fmt)
> +{
> +	struct imx7_csi *csi = v4l2_get_subdevdata(sd);
> +	struct v4l2_fwnode_endpoint upstream_ep;
> +	int ret;
> +
> +	ret = v4l2_subdev_link_validate_default(sd, link, source_fmt, sink_fmt);
> +	if (ret)
> +		return ret;
> +
> +	ret = imx7_csi_get_upstream_endpoint(csi, &upstream_ep, true);
> +	if (ret) {
> +		v4l2_err(&csi->sd, "failed to find upstream endpoint\n");
> +		return ret;
> +	}
> +
> +	mutex_lock(&csi->lock);
> +
> +	csi->upstream_ep = upstream_ep;
> +	csi->is_csi2 = (upstream_ep.bus_type == V4L2_MBUS_CSI2_DPHY);
> +
> +	mutex_unlock(&csi->lock);
> +
> +	return 0;
> +}
> +
> +static void imx7_csi_update_buf(struct imx7_csi *csi, dma_addr_t phys,
> +				int buf_num)
> +{
> +	if (buf_num == 1)
> +		imx7_csi_reg_write(csi, phys, CSI_CSIDMASA_FB2);
> +	else
> +		imx7_csi_reg_write(csi, phys, CSI_CSIDMASA_FB1);
> +}
> +
> +static void imx7_csi_setup_vb2_buf(struct imx7_csi *csi)
> +{
> +	struct imx_media_video_dev *vdev = csi->vdev;
> +	struct imx_media_buffer *buf;
> +	struct vb2_buffer *vb2_buf;
> +	dma_addr_t phys[2];
> +	int i;
> +
> +	for (i = 0; i < 2; i++) {
> +		buf = imx_media_capture_device_next_buf(vdev);
> +		if (buf) {
> +			csi->active_vb2_buf[i] = buf;
> +			vb2_buf = &buf->vbuf.vb2_buf;
> +			phys[i] = vb2_dma_contig_plane_dma_addr(vb2_buf, 0);
> +		} else {
> +			csi->active_vb2_buf[i] = NULL;
> +			phys[i] = csi->underrun_buf.phys;
> +		}
> +
> +		imx7_csi_update_buf(csi, phys[i], i);
> +	}
> +}
> +
> +static void imx7_csi_dma_unsetup_vb2_buf(struct imx7_csi *csi,
> +					 enum vb2_buffer_state return_status)
> +{
> +	struct imx_media_buffer *buf;
> +	int i;
> +
> +	/* return any remaining active frames with return_status */
> +	for (i = 0; i < 2; i++) {
> +		buf = csi->active_vb2_buf[i];
> +		if (buf) {
> +			struct vb2_buffer *vb = &buf->vbuf.vb2_buf;
> +
> +			vb->timestamp = ktime_get_ns();
> +			vb2_buffer_done(vb, return_status);
> +		}
> +	}
> +}
> +
> +static void imx7_csi_vb2_buf_done(struct imx7_csi *csi)
> +{
> +	struct imx_media_video_dev *vdev = csi->vdev;
> +	struct imx_media_buffer *done, *next;
> +	struct vb2_buffer *vb;
> +	dma_addr_t phys;
> +
> +	done = csi->active_vb2_buf[csi->buf_num];
> +	if (done) {
> +		done->vbuf.field = vdev->fmt.fmt.pix.field;
> +		done->vbuf.sequence = csi->frame_sequence;
> +		vb = &done->vbuf.vb2_buf;
> +		vb->timestamp = ktime_get_ns();
> +		vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
> +	}
> +	csi->frame_sequence++;
> +
> +	/* get next queued buffer */
> +	next = imx_media_capture_device_next_buf(vdev);
> +	if (next) {
> +		phys = vb2_dma_contig_plane_dma_addr(&next->vbuf.vb2_buf, 0);
> +		csi->active_vb2_buf[csi->buf_num] = next;
> +	} else {
> +		phys = csi->underrun_buf.phys;
> +		csi->active_vb2_buf[csi->buf_num] = NULL;
> +	}
> +
> +	imx7_csi_update_buf(csi, phys, csi->buf_num);
> +}
> +
> +static irqreturn_t imx7_csi_irq_handler(int irq, void *data)
> +{
> +	struct imx7_csi *csi =  data;
> +	unsigned long status;
> +
> +	spin_lock(&csi->irqlock);
> +
> +	status = imx7_csi_irq_clear(csi);
> +
> +	if (status & BIT_RFF_OR_INT) {
> +		dev_warn(csi->dev, "Rx fifo overflow\n");
> +		imx7_csi_error_recovery(csi);
> +	}
> +
> +	if (status & BIT_HRESP_ERR_INT) {
> +		dev_warn(csi->dev, "Hresponse error detected\n");
> +		imx7_csi_error_recovery(csi);
> +	}
> +
> +	if (status & BIT_ADDR_CH_ERR_INT) {
> +		imx7_csi_hw_disable(csi);
> +
> +		imx7_csi_dma_reflash(csi);
> +
> +		imx7_csi_hw_enable(csi);
> +	}
> +
> +	if ((status & BIT_DMA_TSF_DONE_FB1) &&
> +	    (status & BIT_DMA_TSF_DONE_FB2)) {
> +		/*
> +		 * For both FB1 and FB2 interrupter bits set case,
> +		 * CSI DMA is work in one of FB1 and FB2 buffer,
> +		 * but software can not know the state.
> +		 * Skip it to avoid base address updated
> +		 * when csi work in field0 and field1 will write to
> +		 * new base address.
> +		 */
> +	} else if (status & BIT_DMA_TSF_DONE_FB1) {
> +		csi->buf_num = 0;
> +	} else if (status & BIT_DMA_TSF_DONE_FB2) {
> +		csi->buf_num = 1;
> +	}
> +
> +	if ((status & BIT_DMA_TSF_DONE_FB1) ||
> +	    (status & BIT_DMA_TSF_DONE_FB2)) {
> +		imx7_csi_vb2_buf_done(csi);
> +
> +		if (csi->last_eof) {
> +			complete(&csi->last_eof_completion);
> +			csi->last_eof = false;
> +		}
> +	}
> +
> +
> +	spin_unlock(&csi->irqlock);
> +
> +	return IRQ_HANDLED;
> +}
> +
> +static int imx7_csi_dma_start(struct imx7_csi *csi)
> +{
> +	struct imx_media_video_dev *vdev = csi->vdev;
> +	struct v4l2_pix_format *out_pix = &vdev->fmt.fmt.pix;
> +	int ret;
> +
> +	ret = imx_media_alloc_dma_buf(csi->md, &csi->underrun_buf,
> +				      out_pix->sizeimage);
> +	if (ret < 0) {
> +		v4l2_warn(&csi->sd, "consider increasing the CMA area\n");
> +		return ret;
> +	}
> +
> +	csi->frame_sequence = 0;
> +	csi->last_eof = false;
> +	init_completion(&csi->last_eof_completion);
> +
> +	imx7_csi_setup_vb2_buf(csi);
> +
> +	return 0;
> +}
> +
> +static void imx7_csi_dma_stop(struct imx7_csi *csi)
> +{
> +	unsigned long flags;
> +	int ret;
> +
> +	/* mark next EOF interrupt as the last before stream off */
> +	spin_lock_irqsave(&csi->irqlock, flags);
> +	csi->last_eof = true;
> +	spin_unlock_irqrestore(&csi->irqlock, flags);
> +
> +	/*
> +	 * and then wait for interrupt handler to mark completion.
> +	 */
> +	ret = wait_for_completion_timeout(&csi->last_eof_completion,
> +				msecs_to_jiffies(IMX_MEDIA_EOF_TIMEOUT));
> +	if (ret == 0)
> +		v4l2_warn(&csi->sd, "wait last EOF timeout\n");
> +
> +	imx7_csi_hw_disable_irq(csi);
> +
> +	imx7_csi_dma_unsetup_vb2_buf(csi, VB2_BUF_STATE_ERROR);
> +
> +	imx_media_free_dma_buf(csi->md, &csi->underrun_buf);
> +}
> +
> +static int imx7_csi_configure(struct imx7_csi *csi)
> +{
> +	struct imx_media_video_dev *vdev = csi->vdev;
> +	struct v4l2_pix_format *out_pix = &vdev->fmt.fmt.pix;
> +	__u32 in_code = csi->format_mbus[IMX7_CSI_PAD_SINK].code;
> +	u32 cr1, cr18;
> +
> +	if (out_pix->field == V4L2_FIELD_INTERLACED) {
> +		imx7_csi_deinterlace_enable(csi, true);
> +		imx7_csi_buf_stride_set(csi, out_pix->width);
> +	} else {
> +		imx7_csi_deinterlace_enable(csi, false);
> +		imx7_csi_buf_stride_set(csi, 0);
> +	}
> +
> +	imx7_csi_set_imagpara(csi, out_pix->width, out_pix->height);
> +
> +	if (!csi->is_csi2)
> +		return 0;
> +
> +	cr1 = imx7_csi_reg_read(csi, CSI_CSICR1);
> +	cr1 &= ~BIT_GCLK_MODE;
> +
> +	cr18 = imx7_csi_reg_read(csi, CSI_CSICR18);
> +	cr18 &= BIT_MIPI_DATA_FORMAT_MASK;
> +	cr18 |= BIT_DATA_FROM_MIPI;
> +
> +	switch (out_pix->pixelformat) {
> +	case V4L2_PIX_FMT_UYVY:
> +	case V4L2_PIX_FMT_YUYV:
> +		cr18 |= BIT_MIPI_DATA_FORMAT_YUV422_8B;
> +		break;
> +	case V4L2_PIX_FMT_SBGGR8:
> +		cr18 |= BIT_MIPI_DATA_FORMAT_RAW8;
> +		break;
> +	case V4L2_PIX_FMT_SBGGR16:
> +		if (in_code == MEDIA_BUS_FMT_SBGGR10_1X10)
> +			cr18 |= BIT_MIPI_DATA_FORMAT_RAW10;
> +		else if (in_code == MEDIA_BUS_FMT_SBGGR12_1X12)
> +			cr18 |= BIT_MIPI_DATA_FORMAT_RAW12;
> +		else if (in_code == MEDIA_BUS_FMT_SBGGR14_1X14)
> +			cr18 |= BIT_MIPI_DATA_FORMAT_RAW14;
> +		cr1 |= BIT_PIXEL_BIT;
> +		break;
> +	default:
> +		return -EINVAL;
> +	}
> +
> +	imx7_csi_reg_write(csi, cr1, CSI_CSICR1);
> +	imx7_csi_reg_write(csi, cr18, CSI_CSICR18);
> +
> +	return 0;
> +}
> +
> +static int imx7_csi_enable(struct imx7_csi *csi)
> +{
> +	imx7_csi_sw_reset(csi);
> +
> +	if (csi->is_csi2) {
> +		imx7_csi_dmareq_rff_enable(csi);
> +		imx7_csi_hw_enable_irq(csi);
> +		imx7_csi_hw_enable(csi);
> +		return 0;
> +	}
> +
> +	return 0;
> +}
> +
> +static void imx7_csi_disable(struct imx7_csi *csi)
> +{
> +	imx7_csi_dmareq_rff_disable(csi);
> +
> +	imx7_csi_hw_disable_irq(csi);
> +
> +	imx7_csi_buf_stride_set(csi, 0);
> +
> +	imx7_csi_hw_disable(csi);
> +}
> +
> +static int imx7_csi_streaming_start(struct imx7_csi *csi)
> +{
> +	int ret;
> +
> +	ret = imx7_csi_dma_start(csi);
> +	if (ret < 0)
> +		return ret;
> +
> +	ret = imx7_csi_configure(csi);
> +	if (ret < 0)
> +		goto dma_stop;
> +
> +	imx7_csi_enable(csi);
> +
> +	return 0;
> +
> +dma_stop:
> +	imx7_csi_dma_stop(csi);
> +
> +	return ret;
> +}
> +
> +static int imx7_csi_streaming_stop(struct imx7_csi *csi)
> +{
> +	imx7_csi_dma_stop(csi);
> +
> +	imx7_csi_disable(csi);
> +
> +	return 0;
> +}
> +
> +static int imx7_csi_s_stream(struct v4l2_subdev *sd, int enable)
> +{
> +	struct imx7_csi *csi = v4l2_get_subdevdata(sd);
> +	int ret = 0;
> +
> +	mutex_lock(&csi->lock);
> +
> +	if (!csi->src_sd || !csi->sink) {
> +		ret = -EPIPE;
> +		goto out_unlock;
> +	}
> +
> +	if (csi->is_streaming == !!enable)
> +		goto out_unlock;
> +
> +	if (enable) {
> +		ret = v4l2_subdev_call(csi->src_sd, video, s_stream, 1);
> +		if (ret < 0)
> +			goto out_unlock;
> +
> +		ret = imx7_csi_streaming_start(csi);
> +		if (ret < 0) {
> +			v4l2_subdev_call(csi->src_sd, video, s_stream, 0);
> +			goto out_unlock;
> +		}
> +	} else {
> +		imx7_csi_streaming_stop(csi);
> +
> +		v4l2_subdev_call(csi->src_sd, video, s_stream, 0);
> +	}
> +
> +	csi->is_streaming = !!enable;
> +
> +out_unlock:
> +	mutex_unlock(&csi->lock);
> +
> +	return ret;
> +}
> +
> +static struct v4l2_mbus_framefmt *imx7_csi_get_format(struct imx7_csi *csi,
> +					struct v4l2_subdev_pad_config *cfg,
> +					unsigned int pad,
> +					enum v4l2_subdev_format_whence which)
> +{
> +	if (which == V4L2_SUBDEV_FORMAT_TRY)
> +		return v4l2_subdev_get_try_format(&csi->sd, cfg, pad);
> +
> +	return &csi->format_mbus[pad];
> +}
> +
> +static int imx7_csi_enum_mbus_code(struct v4l2_subdev *sd,
> +				   struct v4l2_subdev_pad_config *cfg,
> +				   struct v4l2_subdev_mbus_code_enum *code)
> +{
> +	struct imx7_csi *csi = v4l2_get_subdevdata(sd);
> +	const struct imx_media_pixfmt *in_cc;
> +	struct v4l2_mbus_framefmt *in_fmt;
> +	int ret = 0;
> +
> +	mutex_lock(&csi->lock);
> +
> +	in_fmt = imx7_csi_get_format(csi, cfg, IMX7_CSI_PAD_SINK, code->which);
> +
> +	in_cc = imx_media_find_mbus_format(in_fmt->code, CS_SEL_ANY, true);
> +
> +	switch (code->pad) {
> +	case IMX7_CSI_PAD_SINK:
> +		ret = imx_media_enum_mbus_format(&code->code, code->index,
> +						 CS_SEL_ANY, true);
> +		break;
> +	case IMX7_CSI_PAD_SRC:
> +		if (code->index != 0) {
> +			ret = -EINVAL;
> +			goto out_unlock;
> +		}
> +
> +		code->code = in_fmt->code;
> +		break;
> +	default:
> +		ret = -EINVAL;
> +	}
> +
> +out_unlock:
> +	mutex_unlock(&csi->lock);
> +
> +	return ret;
> +}
> +
> +static int imx7_csi_get_fmt(struct v4l2_subdev *sd,
> +			    struct v4l2_subdev_pad_config *cfg,
> +			    struct v4l2_subdev_format *sdformat)
> +{
> +	struct imx7_csi *csi = v4l2_get_subdevdata(sd);
> +	struct v4l2_mbus_framefmt *fmt;
> +	int ret = 0;
> +
> +	if (sdformat->pad >= IMX7_CSI_PADS_NUM)
> +		return -EINVAL;

The framework validates the pad indices already. You can drop this one.

> +
> +	mutex_lock(&csi->lock);
> +
> +	fmt = imx7_csi_get_format(csi, cfg, sdformat->pad, sdformat->which);
> +	if (!fmt) {
> +		ret = -EINVAL;
> +		goto out_unlock;
> +	}
> +
> +	sdformat->format = *fmt;
> +
> +out_unlock:
> +	mutex_unlock(&csi->lock);
> +
> +	return ret;
> +}
> +
> +static void imx7_csi_try_fmt(struct imx7_csi *csi,
> +			     struct v4l2_subdev_pad_config *cfg,
> +			     struct v4l2_subdev_format *sdformat,
> +			     const struct imx_media_pixfmt **cc)
> +{
> +	const struct imx_media_pixfmt *in_cc;
> +	struct v4l2_mbus_framefmt *in_fmt;
> +	u32 code;
> +
> +	in_fmt = imx7_csi_get_format(csi, cfg, IMX7_CSI_PAD_SINK,
> +				     sdformat->which);
> +	if (!in_fmt)
> +		return;
> +
> +	switch (sdformat->pad) {
> +	case IMX7_CSI_PAD_SRC:
> +		in_cc = imx_media_find_mbus_format(in_fmt->code, CS_SEL_ANY,
> +						   true);
> +
> +		sdformat->format.width = in_fmt->width;
> +		sdformat->format.height = in_fmt->height;
> +		sdformat->format.code = in_fmt->code;
> +		*cc = in_cc;
> +
> +		sdformat->format.colorspace = in_fmt->colorspace;
> +		sdformat->format.xfer_func = in_fmt->xfer_func;
> +		sdformat->format.quantization = in_fmt->quantization;
> +		sdformat->format.ycbcr_enc = in_fmt->ycbcr_enc;
> +		break;
> +	case IMX7_CSI_PAD_SINK:
> +		*cc = imx_media_find_mbus_format(sdformat->format.code,
> +						 CS_SEL_ANY, true);
> +		if (!*cc) {
> +			imx_media_enum_mbus_format(&code, 0, CS_SEL_ANY, false);
> +			*cc = imx_media_find_mbus_format(code, CS_SEL_ANY,
> +							 false);
> +			sdformat->format.code = (*cc)->codes[0];
> +		}
> +
> +		imx_media_fill_default_mbus_fields(&sdformat->format, in_fmt,
> +						   false);
> +		break;
> +	default:
> +		break;
> +	}
> +}
> +
> +static int imx7_csi_set_fmt(struct v4l2_subdev *sd,
> +			    struct v4l2_subdev_pad_config *cfg,
> +			    struct v4l2_subdev_format *sdformat)
> +{
> +	struct imx7_csi *csi = v4l2_get_subdevdata(sd);
> +	struct imx_media_video_dev *vdev = csi->vdev;
> +	const struct imx_media_pixfmt *outcc;
> +	struct v4l2_mbus_framefmt *outfmt;
> +	struct v4l2_pix_format vdev_fmt;
> +	const struct imx_media_pixfmt *cc;
> +	struct v4l2_mbus_framefmt *fmt;
> +	struct v4l2_subdev_format format;
> +	int ret = 0;
> +
> +	if (sdformat->pad >= IMX7_CSI_PADS_NUM)
> +		return -EINVAL;
> +
> +	mutex_lock(&csi->lock);
> +
> +	if (csi->is_streaming) {
> +		ret = -EBUSY;
> +		goto out_unlock;
> +	}
> +
> +	imx7_csi_try_fmt(csi, cfg, sdformat, &cc);
> +
> +	fmt = imx7_csi_get_format(csi, cfg, sdformat->pad, sdformat->which);
> +	if (!fmt) {
> +		ret = -EINVAL;
> +		goto out_unlock;
> +	}
> +
> +	*fmt = sdformat->format;
> +
> +	if (sdformat->pad == IMX7_CSI_PAD_SINK) {
> +		/* propagate format to source pads */
> +		format.pad = IMX7_CSI_PAD_SRC;
> +		format.which = sdformat->which;
> +		format.format = sdformat->format;
> +		imx7_csi_try_fmt(csi, cfg, &format, &outcc);
> +
> +		outfmt = imx7_csi_get_format(csi, cfg, IMX7_CSI_PAD_SRC,
> +					     sdformat->which);
> +		*outfmt = format.format;
> +
> +		if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE)
> +			csi->cc[IMX7_CSI_PAD_SRC] = outcc;
> +	}
> +
> +	if (sdformat->which == V4L2_SUBDEV_FORMAT_TRY)
> +		goto out_unlock;
> +
> +	csi->cc[sdformat->pad] = cc;
> +
> +	/* propagate output pad format to capture device */
> +	imx_media_mbus_fmt_to_pix_fmt(&vdev_fmt,
> +				      &csi->format_mbus[IMX7_CSI_PAD_SRC],
> +				      csi->cc[IMX7_CSI_PAD_SRC]);

The user is expected to configure the format on the video device; the driver
needs to validate that when starting the stream.

> +	mutex_unlock(&csi->lock);
> +	imx_media_capture_device_set_format(vdev, &vdev_fmt);
> +
> +	return 0;
> +
> +out_unlock:
> +	mutex_unlock(&csi->lock);
> +
> +	return ret;
> +}
> +
> +static int imx7_csi_registered(struct v4l2_subdev *sd)
> +{
> +	struct imx7_csi *csi = v4l2_get_subdevdata(sd);
> +	int ret;
> +	int i;
> +
> +	csi->md = dev_get_drvdata(sd->v4l2_dev->dev);
> +
> +	for (i = 0; i < IMX7_CSI_PADS_NUM; i++) {
> +		csi->pad[i].flags = (i == IMX7_CSI_PAD_SINK) ?
> +			MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
> +
> +		/* set a default mbus format  */
> +		ret = imx_media_init_mbus_fmt(&csi->format_mbus[i],
> +					      800, 600, 0, V4L2_FIELD_NONE,
> +					      &csi->cc[i]);
> +		if (ret < 0)
> +			return ret;
> +
> +		/* init default frame interval */
> +		csi->frame_interval[i].numerator = 1;
> +		csi->frame_interval[i].denominator = 30;
> +	}
> +
> +	ret = media_entity_pads_init(&sd->entity, IMX7_CSI_PADS_NUM, csi->pad);
> +	if (ret < 0)
> +		return ret;
> +
> +	ret = imx_media_capture_device_register(csi->vdev);
> +	if (ret < 0)
> +		return ret;
> +
> +	ret = imx_media_add_video_device(csi->md, csi->vdev);
> +	if (ret < 0) {
> +		imx_media_capture_device_unregister(csi->vdev);
> +		return ret;
> +	}
> +
> +	return 0;
> +}
> +
> +static void imx7_csi_unregistered(struct v4l2_subdev *sd)
> +{
> +	struct imx7_csi *csi = v4l2_get_subdevdata(sd);
> +
> +	imx_media_capture_device_unregister(csi->vdev);
> +}
> +
> +static int imx7_csi_init_cfg(struct v4l2_subdev *sd,
> +			     struct v4l2_subdev_pad_config *cfg)
> +{
> +	struct imx7_csi *csi = v4l2_get_subdevdata(sd);
> +	struct v4l2_mbus_framefmt *mf;
> +	int ret;
> +	int i;
> +
> +	for (i = 0; i < IMX7_CSI_PADS_NUM; i++) {
> +		mf = v4l2_subdev_get_try_format(sd, cfg, i);
> +
> +		ret = imx_media_init_mbus_fmt(mf, 800, 600, 0, V4L2_FIELD_NONE,
> +					      &csi->cc[i]);
> +		if (ret < 0)
> +			return ret;
> +	}
> +
> +	return 0;
> +}
> +
> +static const struct media_entity_operations imx7_csi_entity_ops = {
> +	.link_setup	= imx7_csi_link_setup,
> +	.link_validate	= v4l2_subdev_link_validate,
> +};
> +
> +static const struct v4l2_subdev_video_ops imx7_csi_video_ops = {
> +	.s_stream		= imx7_csi_s_stream,
> +};
> +
> +static const struct v4l2_subdev_pad_ops imx7_csi_pad_ops = {
> +	.init_cfg =		imx7_csi_init_cfg,
> +	.enum_mbus_code =	imx7_csi_enum_mbus_code,
> +	.get_fmt =		imx7_csi_get_fmt,
> +	.set_fmt =		imx7_csi_set_fmt,
> +	.link_validate =	imx7_csi_pad_link_validate,
> +};
> +
> +static const struct v4l2_subdev_ops imx7_csi_subdev_ops = {
> +	.video =	&imx7_csi_video_ops,
> +	.pad =		&imx7_csi_pad_ops,
> +};
> +
> +static const struct v4l2_subdev_internal_ops imx7_csi_internal_ops = {
> +	.registered	= imx7_csi_registered,
> +	.unregistered	= imx7_csi_unregistered,
> +};
> +
> +static int imx7_csi_parse_endpoint(struct device *dev,
> +				   struct v4l2_fwnode_endpoint *vep,
> +				   struct v4l2_async_subdev *asd)
> +{
> +	return fwnode_device_is_available(asd->match.fwnode) ? 0 : -EINVAL;
> +}
> +
> +static int imx7_csi_parse_dt(struct imx7_csi *csi)
> +{
> +	struct device *dev = csi->dev;
> +
> +	csi->clk_disp_axi = devm_clk_get(dev, "axi");
> +	if (IS_ERR(csi->clk_disp_axi)) {
> +		dev_err(dev, "Could not get csi axi clock\n");
> +		return -ENODEV;
> +	}
> +
> +	csi->clk_disp_dcic = devm_clk_get(dev, "dcic");
> +	if (IS_ERR(csi->clk_disp_dcic)) {
> +		dev_err(dev, "Could not get disp dcic clock\n");
> +		return -ENODEV;
> +	}
> +
> +	csi->clk_csi_mclk = devm_clk_get(dev, "mclk");
> +	if (IS_ERR(csi->clk_csi_mclk)) {
> +		dev_err(dev, "Could not get csi mclk clock\n");
> +		return -ENODEV;
> +	}

Could you use devm_clk_bulk_get()?

> +
> +	return 0;
> +}
> +
> +static int imx7_csi_probe(struct platform_device *pdev)
> +{
> +	struct device *dev = &pdev->dev;
> +	struct device_node *node = dev->of_node;
> +	struct imx7_csi *csi;
> +	struct resource *res;
> +	int ret;
> +
> +	csi = devm_kzalloc(&pdev->dev, sizeof(*csi), GFP_KERNEL);
> +	if (!csi)
> +		return -ENOMEM;
> +
> +	platform_set_drvdata(pdev, &csi->sd);
> +	csi->dev = dev;
> +
> +	ret = imx7_csi_parse_dt(csi);
> +	if (ret < 0)
> +		return -ENODEV;
> +
> +	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
> +	csi->irq = platform_get_irq(pdev, 0);
> +	if (!res || csi->irq < 0) {
> +		dev_err(dev, "Missing platform resources data\n");
> +		return -ENODEV;
> +	}
> +
> +	csi->regbase = devm_ioremap_resource(dev, res);
> +	if (IS_ERR(csi->regbase)) {
> +		dev_err(dev, "Failed platform resources map\n");
> +		return -ENODEV;
> +	}
> +
> +	spin_lock_init(&csi->irqlock);
> +	mutex_init(&csi->lock);
> +
> +	/* install interrupt handler */
> +	ret = devm_request_irq(dev, csi->irq, imx7_csi_irq_handler, 0, "csi",
> +			       (void *)csi);
> +	if (ret < 0) {
> +		dev_err(dev, "Request CSI IRQ failed.\n");
> +		return -ENODEV;

mutex_destroy(); better to add a label below.

> +	}
> +
> +	/* add media device */
> +	csi->imxmd = imx_media_dev_init(dev);
> +	if (IS_ERR(csi->imxmd))
> +		return PTR_ERR(csi->imxmd);
> +
> +	ret = imx_media_of_add_csi(csi->imxmd, node);
> +	if (ret < 0)
> +		goto media_cleanup;
> +
> +	ret = imx_media_dev_notifier_register(csi->imxmd);
> +	if (ret < 0)
> +		goto media_cleanup;
> +
> +	v4l2_subdev_init(&csi->sd, &imx7_csi_subdev_ops);
> +	v4l2_set_subdevdata(&csi->sd, csi);
> +	csi->sd.internal_ops = &imx7_csi_internal_ops;
> +	csi->sd.entity.ops = &imx7_csi_entity_ops;
> +	csi->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
> +	csi->sd.dev = &pdev->dev;
> +	csi->sd.owner = THIS_MODULE;
> +	csi->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
> +	csi->sd.grp_id = IMX_MEDIA_GRP_ID_CSI;
> +	snprintf(csi->sd.name, sizeof(csi->sd.name), "csi");
> +
> +	csi->vdev = imx_media_capture_device_init(&csi->sd, IMX7_CSI_PAD_SRC);
> +	if (IS_ERR(csi->vdev))
> +		return PTR_ERR(csi->vdev);
> +
> +	v4l2_ctrl_handler_init(&csi->ctrl_hdlr, 0);
> +	csi->sd.ctrl_handler = &csi->ctrl_hdlr;
> +
> +	ret = v4l2_async_register_fwnode_subdev(&csi->sd,
> +					sizeof(struct v4l2_async_subdev),
> +					NULL, 0, imx7_csi_parse_endpoint);
> +	if (ret)
> +		goto free;
> +
> +	return 0;
> +
> +free:
> +	v4l2_ctrl_handler_free(&csi->ctrl_hdlr);
> +	mutex_destroy(&csi->lock);
> +	imx_media_capture_device_remove(csi->vdev);
> +
> +media_cleanup:
> +	imx_media_dev_cleanup(csi->imxmd);
> +
> +	return ret;
> +}
> +
> +static int imx7_csi_remove(struct platform_device *pdev)
> +{
> +	struct v4l2_subdev *sd = platform_get_drvdata(pdev);
> +	struct imx7_csi *csi = v4l2_get_subdevdata(sd);
> +
> +	v4l2_ctrl_handler_free(&csi->ctrl_hdlr);
> +	mutex_destroy(&csi->lock);

Please unregister the devices before the two above lines.

> +	imx_media_capture_device_remove(csi->vdev);
> +	imx_media_dev_notifier_unregister(csi->imxmd);
> +	imx_media_dev_cleanup(csi->imxmd);
> +	media_entity_cleanup(&sd->entity);
> +	v4l2_async_unregister_subdev(sd);
> +
> +	return 0;
> +}
> +
> +static const struct of_device_id imx7_csi_of_match[] = {
> +	{ .compatible = "fsl,imx7-csi" },
> +	{ },
> +};
> +MODULE_DEVICE_TABLE(of, imx7_csi_of_match);
> +
> +static struct platform_driver imx7_csi_driver = {
> +	.probe = imx7_csi_probe,
> +	.remove = imx7_csi_remove,
> +	.driver = {
> +		.of_match_table = imx7_csi_of_match,
> +		.name = "imx7-csi",
> +	},
> +};
> +module_platform_driver(imx7_csi_driver);
> +
> +MODULE_DESCRIPTION("i.MX7 CSI subdev driver");
> +MODULE_AUTHOR("Rui Miguel Silva <rui.silva@xxxxxxxxxx>");
> +MODULE_LICENSE("GPL");
> +MODULE_ALIAS("platform:imx7-csi");

-- 
Sakari Ailus
sakari.ailus@xxxxxxxxxxxxxxx



[Index of Archives]     [Linux Input]     [Video for Linux]     [Gstreamer Embedded]     [Mplayer Users]     [Linux USB Devel]     [Linux Audio Users]     [Linux Kernel]     [Linux SCSI]     [Yosemite Backpacking]

  Powered by Linux