diff mbox series

[v4,09/22] media: camss: Refactor CSID HW version support

Message ID 20210205104414.299732-10-robert.foss@linaro.org (mailing list archive)
State New, archived
Headers show
Series Add support for the SDM845 Camera Subsystem | expand

Commit Message

Robert Foss Feb. 5, 2021, 10:44 a.m. UTC 
In order to support Qualcomm ISP hardware architectures that diverge
from older architectures, the CSID subdevice drivers needs to be refactored
to better abstract the different ISP hardware architectures.

Signed-off-by: Robert Foss <robert.foss@linaro.org>
---

Changes since v1
 - kernel test robot: Add missing include, interrupt.h


 drivers/media/platform/qcom/camss/Makefile    |   2 +
 .../platform/qcom/camss/camss-csid-4-1.c      | 338 ++++++++++
 .../platform/qcom/camss/camss-csid-4-7.c      | 406 ++++++++++++
 .../media/platform/qcom/camss/camss-csid.c    | 616 +-----------------
 .../media/platform/qcom/camss/camss-csid.h    | 126 +++-
 5 files changed, 898 insertions(+), 590 deletions(-)
 create mode 100644 drivers/media/platform/qcom/camss/camss-csid-4-1.c
 create mode 100644 drivers/media/platform/qcom/camss/camss-csid-4-7.c

Comments

Andrey Konovalov Feb. 11, 2021, 8:59 a.m. UTC | #1 
Hi Robert,

Thank you for your patch!

On 05.02.2021 13:44, Robert Foss wrote:
> In order to support Qualcomm ISP hardware architectures that diverge
> from older architectures, the CSID subdevice drivers needs to be refactored
> to better abstract the different ISP hardware architectures.
> 
> Signed-off-by: Robert Foss <robert.foss@linaro.org>
> ---
> 
> Changes since v1
>   - kernel test robot: Add missing include, interrupt.h
> 
> 
>   drivers/media/platform/qcom/camss/Makefile    |   2 +
>   .../platform/qcom/camss/camss-csid-4-1.c      | 338 ++++++++++
>   .../platform/qcom/camss/camss-csid-4-7.c      | 406 ++++++++++++
>   .../media/platform/qcom/camss/camss-csid.c    | 616 +-----------------
>   .../media/platform/qcom/camss/camss-csid.h    | 126 +++-
>   5 files changed, 898 insertions(+), 590 deletions(-)
>   create mode 100644 drivers/media/platform/qcom/camss/camss-csid-4-1.c
>   create mode 100644 drivers/media/platform/qcom/camss/camss-csid-4-7.c
> 
> diff --git a/drivers/media/platform/qcom/camss/Makefile b/drivers/media/platform/qcom/camss/Makefile
> index 052c4f405fa3..cff388b653ba 100644
> --- a/drivers/media/platform/qcom/camss/Makefile
> +++ b/drivers/media/platform/qcom/camss/Makefile
> @@ -4,6 +4,8 @@
>   qcom-camss-objs += \
>   		camss.o \
>   		camss-csid.o \
> +		camss-csid-4-1.o \
> +		camss-csid-4-7.o \
>   		camss-csiphy-2ph-1-0.o \
>   		camss-csiphy-3ph-1-0.o \
>   		camss-csiphy.o \
> diff --git a/drivers/media/platform/qcom/camss/camss-csid-4-1.c b/drivers/media/platform/qcom/camss/camss-csid-4-1.c
> new file mode 100644
> index 000000000000..84b415137555
> --- /dev/null
> +++ b/drivers/media/platform/qcom/camss/camss-csid-4-1.c
> @@ -0,0 +1,338 @@
> +// SPDX-License-Identifier: GPL-2.0
> +/*
> + * camss-csid-4-1.c
> + *
> + * Qualcomm MSM Camera Subsystem - CSID (CSI Decoder) Module
> + *
> + * Copyright (C) 2020 Linaro Ltd.
> + */

Why is this extra space before each #include below:

> + #include <linux/completion.h>
> + #include <linux/interrupt.h>
> + #include <linux/io.h>
> + #include <linux/kernel.h>
> + #include <linux/of.h>
    ^
    here?

> +
> +#include "camss-csid.h"
> +#include "camss.h"
> +
> +#define CAMSS_CSID_HW_VERSION		0x0
> +#define CAMSS_CSID_CORE_CTRL_0		0x004
> +#define CAMSS_CSID_CORE_CTRL_1		0x008
> +#define CAMSS_CSID_RST_CMD		0x00c
> +#define CAMSS_CSID_CID_LUT_VC_n(n)	(0x010 + 0x4 * (n))
> +#define CAMSS_CSID_CID_n_CFG(n)		(0x020 + 0x4 * (n))
> +#define CAMSS_CSID_CID_n_CFG_ISPIF_EN	BIT(0)
> +#define CAMSS_CSID_CID_n_CFG_RDI_EN	BIT(1)
> +#define CAMSS_CSID_CID_n_CFG_DECODE_FORMAT_SHIFT	4
> +#define CAMSS_CSID_CID_n_CFG_PLAIN_FORMAT_8		(0 << 8)
> +#define CAMSS_CSID_CID_n_CFG_PLAIN_FORMAT_16		(1 << 8)
> +#define CAMSS_CSID_CID_n_CFG_PLAIN_ALIGNMENT_LSB	(0 << 9)
> +#define CAMSS_CSID_CID_n_CFG_PLAIN_ALIGNMENT_MSB	(1 << 9)
> +#define CAMSS_CSID_CID_n_CFG_RDI_MODE_RAW_DUMP		(0 << 10)
> +#define CAMSS_CSID_CID_n_CFG_RDI_MODE_PLAIN_PACKING	(1 << 10)
> +#define CAMSS_CSID_IRQ_CLEAR_CMD	0x060
> +#define CAMSS_CSID_IRQ_MASK		0x064
> +#define CAMSS_CSID_IRQ_STATUS		0x068
> +#define CAMSS_CSID_TG_CTRL		0x0a0
> +#define CAMSS_CSID_TG_CTRL_DISABLE	0xa06436
> +#define CAMSS_CSID_TG_CTRL_ENABLE	0xa06437
> +#define CAMSS_CSID_TG_VC_CFG		0x0a4
> +#define CAMSS_CSID_TG_VC_CFG_H_BLANKING		0x3ff
> +#define CAMSS_CSID_TG_VC_CFG_V_BLANKING		0x7f
> +#define CAMSS_CSID_TG_DT_n_CGG_0(n)	(0x0ac + 0xc * (n))
> +#define CAMSS_CSID_TG_DT_n_CGG_1(n)	(0x0b0 + 0xc * (n))
> +#define CAMSS_CSID_TG_DT_n_CGG_2(n)	(0x0b4 + 0xc * (n))
> +
> +
> +enum csid_testgen_pattern {
> +	TESTGEN_PATTERN_INCREMENTING = 0,
> +	TESTGEN_PATTERN_ALTERNATING_55_AA = 1,
> +	TESTGEN_PATTERN_ALL_ZEROES = 2,
> +	TESTGEN_PATTERN_ALL_ONES = 3,
> +	TESTGEN_PATTERN_RANDOM = 4,
> +	TESTGEN_PATTERN_USER_SPECIFIED = 5,
> +};

- this enum is unused.

Thanks,
Andrey

> +
> +static const struct csid_format csid_formats[] = {
> +	{
> +		MEDIA_BUS_FMT_UYVY8_2X8,
> +		DATA_TYPE_YUV422_8BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> +		8,
> +		2,
> +	},
> +	{
> +		MEDIA_BUS_FMT_VYUY8_2X8,
> +		DATA_TYPE_YUV422_8BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> +		8,
> +		2,
> +	},
> +	{
> +		MEDIA_BUS_FMT_YUYV8_2X8,
> +		DATA_TYPE_YUV422_8BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> +		8,
> +		2,
> +	},
> +	{
> +		MEDIA_BUS_FMT_YVYU8_2X8,
> +		DATA_TYPE_YUV422_8BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> +		8,
> +		2,
> +	},
> +	{
> +		MEDIA_BUS_FMT_SBGGR8_1X8,
> +		DATA_TYPE_RAW_8BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> +		8,
> +		1,
> +	},
> +	{
> +		MEDIA_BUS_FMT_SGBRG8_1X8,
> +		DATA_TYPE_RAW_8BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> +		8,
> +		1,
> +	},
> +	{
> +		MEDIA_BUS_FMT_SGRBG8_1X8,
> +		DATA_TYPE_RAW_8BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> +		8,
> +		1,
> +	},
> +	{
> +		MEDIA_BUS_FMT_SRGGB8_1X8,
> +		DATA_TYPE_RAW_8BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> +		8,
> +		1,
> +	},
> +	{
> +		MEDIA_BUS_FMT_SBGGR10_1X10,
> +		DATA_TYPE_RAW_10BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> +		10,
> +		1,
> +	},
> +	{
> +		MEDIA_BUS_FMT_SGBRG10_1X10,
> +		DATA_TYPE_RAW_10BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> +		10,
> +		1,
> +	},
> +	{
> +		MEDIA_BUS_FMT_SGRBG10_1X10,
> +		DATA_TYPE_RAW_10BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> +		10,
> +		1,
> +	},
> +	{
> +		MEDIA_BUS_FMT_SRGGB10_1X10,
> +		DATA_TYPE_RAW_10BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> +		10,
> +		1,
> +	},
> +	{
> +		MEDIA_BUS_FMT_SBGGR12_1X12,
> +		DATA_TYPE_RAW_12BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
> +		12,
> +		1,
> +	},
> +	{
> +		MEDIA_BUS_FMT_SGBRG12_1X12,
> +		DATA_TYPE_RAW_12BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
> +		12,
> +		1,
> +	},
> +	{
> +		MEDIA_BUS_FMT_SGRBG12_1X12,
> +		DATA_TYPE_RAW_12BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
> +		12,
> +		1,
> +	},
> +	{
> +		MEDIA_BUS_FMT_SRGGB12_1X12,
> +		DATA_TYPE_RAW_12BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
> +		12,
> +		1,
> +	},
> +	{
> +		MEDIA_BUS_FMT_Y10_1X10,
> +		DATA_TYPE_RAW_10BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> +		10,
> +		1,
> +	},
> +};
> +
> +static void csid_configure_stream(struct csid_device *csid, u8 enable)
> +{
> +	struct csid_testgen_config *tg = &csid->testgen;
> +	u32 val;
> +
> +	if (enable) {
> +		struct v4l2_mbus_framefmt *input_format;
> +		const struct csid_format *format;
> +		u8 vc = 0; /* Virtual Channel 0 */
> +		u8 cid = vc * 4; /* id of Virtual Channel and Data Type set */
> +		u8 dt_shift;
> +
> +		if (tg->enabled) {
> +			/* Config Test Generator */
> +			u32 num_lines, num_bytes_per_line;
> +
> +			input_format = &csid->fmt[MSM_CSID_PAD_SRC];
> +			format = csid_get_fmt_entry(csid->formats, csid->nformats,
> +						    input_format->code);
> +			num_bytes_per_line = input_format->width * format->bpp * format->spp / 8;
> +			num_lines = input_format->height;
> +
> +			/* 31:24 V blank, 23:13 H blank, 3:2 num of active DT */
> +			/* 1:0 VC */
> +			val = ((CAMSS_CSID_TG_VC_CFG_V_BLANKING & 0xff) << 24) |
> +				  ((CAMSS_CSID_TG_VC_CFG_H_BLANKING & 0x7ff) << 13);
> +			writel_relaxed(val, csid->base + CAMSS_CSID_TG_VC_CFG);
> +
> +			/* 28:16 bytes per lines, 12:0 num of lines */
> +			val = ((num_bytes_per_line & 0x1fff) << 16) |
> +				  (num_lines & 0x1fff);
> +			writel_relaxed(val, csid->base + CAMSS_CSID_TG_DT_n_CGG_0(0));
> +
> +			/* 5:0 data type */
> +			val = format->data_type;
> +			writel_relaxed(val, csid->base + CAMSS_CSID_TG_DT_n_CGG_1(0));
> +
> +			/* 2:0 output test pattern */
> +			val = tg->mode;
> +			writel_relaxed(val, csid->base + CAMSS_CSID_TG_DT_n_CGG_2(0));
> +		} else {
> +			struct csid_phy_config *phy = &csid->phy;
> +
> +			input_format = &csid->fmt[MSM_CSID_PAD_SINK];
> +			format = csid_get_fmt_entry(csid->formats, csid->nformats,
> +						    input_format->code);
> +
> +			val = phy->lane_cnt - 1;
> +			val |= phy->lane_assign << 4;
> +
> +			writel_relaxed(val, csid->base + CAMSS_CSID_CORE_CTRL_0);
> +
> +			val = phy->csiphy_id << 17;
> +			val |= 0x9;
> +
> +			writel_relaxed(val, csid->base + CAMSS_CSID_CORE_CTRL_1);
> +		}
> +
> +		/* Config LUT */
> +
> +		dt_shift = (cid % 4) * 8;
> +		val = readl_relaxed(csid->base + CAMSS_CSID_CID_LUT_VC_n(vc));
> +		val &= ~(0xff << dt_shift);
> +		val |= format->data_type << dt_shift;
> +		writel_relaxed(val, csid->base + CAMSS_CSID_CID_LUT_VC_n(vc));
> +
> +		val = CAMSS_CSID_CID_n_CFG_ISPIF_EN;
> +		val |= CAMSS_CSID_CID_n_CFG_RDI_EN;
> +		val |= format->decode_format << CAMSS_CSID_CID_n_CFG_DECODE_FORMAT_SHIFT;
> +		val |= CAMSS_CSID_CID_n_CFG_RDI_MODE_RAW_DUMP;
> +		writel_relaxed(val, csid->base + CAMSS_CSID_CID_n_CFG(cid));
> +
> +		if (tg->enabled) {
> +			val = CAMSS_CSID_TG_CTRL_ENABLE;
> +			writel_relaxed(val, csid->base + CAMSS_CSID_TG_CTRL);
> +		}
> +	} else {
> +		if (tg->enabled) {
> +			val = CAMSS_CSID_TG_CTRL_DISABLE;
> +			writel_relaxed(val, csid->base + CAMSS_CSID_TG_CTRL);
> +		}
> +	}
> +}
> +
> +static int csid_configure_testgen_pattern(struct csid_device *csid, s32 val)
> +{
> +	s32 regval = val - 1;
> +
> +	if (regval > 0 || regval <= CSID_PAYLOAD_MODE_MAX_SUPPORTED_4_1)
> +		csid->testgen.mode = regval;
> +
> +	return 0;
> +}
> +
> +static u32 csid_hw_version(struct csid_device *csid)
> +{
> +	u32 hw_version = readl_relaxed(csid->base + CAMSS_CSID_HW_VERSION);
> +
> +	dev_dbg(csid->camss->dev, "CSID HW Version = 0x%08x\n", hw_version);
> +
> +	return hw_version;
> +}
> +
> +static irqreturn_t csid_isr(int irq, void *dev)
> +{
> +	struct csid_device *csid = dev;
> +	u32 value;
> +
> +	value = readl_relaxed(csid->base + CAMSS_CSID_IRQ_STATUS);
> +	writel_relaxed(value, csid->base + CAMSS_CSID_IRQ_CLEAR_CMD);
> +
> +	if ((value >> 11) & 0x1)
> +		complete(&csid->reset_complete);
> +
> +	return IRQ_HANDLED;
> +}
> +
> +static int csid_reset(struct csid_device *csid)
> +{
> +	unsigned long time;
> +
> +	reinit_completion(&csid->reset_complete);
> +
> +	writel_relaxed(0x7fff, csid->base + CAMSS_CSID_RST_CMD);
> +
> +	time = wait_for_completion_timeout(&csid->reset_complete,
> +		msecs_to_jiffies(CSID_RESET_TIMEOUT_MS));
> +	if (!time) {
> +		dev_err(csid->camss->dev, "CSID reset timeout\n");
> +		return -EIO;
> +	}
> +
> +	return 0;
> +}
> +
> +static u32 csid_src_pad_code(struct csid_device *csid, u32 sink_code,
> +			     unsigned int match_format_idx, u32 match_code)
> +{
> +	if (match_format_idx > 0)
> +		return 0;
> +
> +	return sink_code;
> +}
> +
> +static void csid_subdev_init(struct csid_device *csid)
> +{
> +	csid->formats = csid_formats;
> +	csid->nformats = ARRAY_SIZE(csid_formats);
> +	csid->testgen.modes = csid_testgen_modes;
> +	csid->testgen.nmodes = CSID_PAYLOAD_MODE_MAX_SUPPORTED_4_1;
> +}
> +
> +const struct csid_hw_ops csid_ops_4_1 = {
> +	.configure_stream = csid_configure_stream,
> +	.configure_testgen_pattern = csid_configure_testgen_pattern,
> +	.hw_version = csid_hw_version,
> +	.isr = csid_isr,
> +	.reset = csid_reset,
> +	.src_pad_code = csid_src_pad_code,
> +	.subdev_init = csid_subdev_init,
> +};
> diff --git a/drivers/media/platform/qcom/camss/camss-csid-4-7.c b/drivers/media/platform/qcom/camss/camss-csid-4-7.c
> new file mode 100644
> index 000000000000..16a69b140f4e
> --- /dev/null
> +++ b/drivers/media/platform/qcom/camss/camss-csid-4-7.c
> @@ -0,0 +1,406 @@
> +// SPDX-License-Identifier: GPL-2.0
> +/*
> + * camss-csid-4-7.c
> + *
> + * Qualcomm MSM Camera Subsystem - CSID (CSI Decoder) Module
> + *
> + * Copyright (C) 2020 Linaro Ltd.
> + */
> +#include <linux/completion.h>
> +#include <linux/interrupt.h>
> +#include <linux/io.h>
> +#include <linux/kernel.h>
> +#include <linux/of.h>
> +
> +#include "camss-csid.h"
> +#include "camss.h"
> +
> +#define CAMSS_CSID_HW_VERSION		0x0
> +#define CAMSS_CSID_CORE_CTRL_0		0x004
> +#define CAMSS_CSID_CORE_CTRL_1		0x008
> +#define CAMSS_CSID_RST_CMD		0x010
> +#define CAMSS_CSID_CID_LUT_VC_n(n)	(0x014 + 0x4 * (n))
> +#define CAMSS_CSID_CID_n_CFG(n)		(0x024 + 0x4 * (n))
> +#define CAMSS_CSID_CID_n_CFG_ISPIF_EN	BIT(0)
> +#define CAMSS_CSID_CID_n_CFG_RDI_EN	BIT(1)
> +#define CAMSS_CSID_CID_n_CFG_DECODE_FORMAT_SHIFT	4
> +#define CAMSS_CSID_CID_n_CFG_PLAIN_FORMAT_8		(0 << 8)
> +#define CAMSS_CSID_CID_n_CFG_PLAIN_FORMAT_16		(1 << 8)
> +#define CAMSS_CSID_CID_n_CFG_PLAIN_ALIGNMENT_LSB	(0 << 9)
> +#define CAMSS_CSID_CID_n_CFG_PLAIN_ALIGNMENT_MSB	(1 << 9)
> +#define CAMSS_CSID_CID_n_CFG_RDI_MODE_RAW_DUMP		(0 << 10)
> +#define CAMSS_CSID_CID_n_CFG_RDI_MODE_PLAIN_PACKING	(1 << 10)
> +#define CAMSS_CSID_IRQ_CLEAR_CMD	0x064
> +#define CAMSS_CSID_IRQ_MASK		0x068
> +#define CAMSS_CSID_IRQ_STATUS		0x06c
> +#define CAMSS_CSID_TG_CTRL		0x0a8
> +#define CAMSS_CSID_TG_CTRL_DISABLE	0xa06436
> +#define CAMSS_CSID_TG_CTRL_ENABLE	0xa06437
> +#define CAMSS_CSID_TG_VC_CFG		0x0ac
> +#define CAMSS_CSID_TG_VC_CFG_H_BLANKING		0x3ff
> +#define CAMSS_CSID_TG_VC_CFG_V_BLANKING		0x7f
> +#define CAMSS_CSID_TG_DT_n_CGG_0(n)	(0x0b4 + 0xc * (n))
> +#define CAMSS_CSID_TG_DT_n_CGG_1(n)	(0x0b8 + 0xc * (n))
> +#define CAMSS_CSID_TG_DT_n_CGG_2(n)	(0x0bc + 0xc * (n))
> +
> +
> +static const struct csid_format csid_formats[] = {
> +	{
> +		MEDIA_BUS_FMT_UYVY8_2X8,
> +		DATA_TYPE_YUV422_8BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> +		8,
> +		2,
> +	},
> +	{
> +		MEDIA_BUS_FMT_VYUY8_2X8,
> +		DATA_TYPE_YUV422_8BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> +		8,
> +		2,
> +	},
> +	{
> +		MEDIA_BUS_FMT_YUYV8_2X8,
> +		DATA_TYPE_YUV422_8BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> +		8,
> +		2,
> +	},
> +	{
> +		MEDIA_BUS_FMT_YVYU8_2X8,
> +		DATA_TYPE_YUV422_8BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> +		8,
> +		2,
> +	},
> +	{
> +		MEDIA_BUS_FMT_SBGGR8_1X8,
> +		DATA_TYPE_RAW_8BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> +		8,
> +		1,
> +	},
> +	{
> +		MEDIA_BUS_FMT_SGBRG8_1X8,
> +		DATA_TYPE_RAW_8BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> +		8,
> +		1,
> +	},
> +	{
> +		MEDIA_BUS_FMT_SGRBG8_1X8,
> +		DATA_TYPE_RAW_8BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> +		8,
> +		1,
> +	},
> +	{
> +		MEDIA_BUS_FMT_SRGGB8_1X8,
> +		DATA_TYPE_RAW_8BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> +		8,
> +		1,
> +	},
> +	{
> +		MEDIA_BUS_FMT_SBGGR10_1X10,
> +		DATA_TYPE_RAW_10BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> +		10,
> +		1,
> +	},
> +	{
> +		MEDIA_BUS_FMT_SGBRG10_1X10,
> +		DATA_TYPE_RAW_10BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> +		10,
> +		1,
> +	},
> +	{
> +		MEDIA_BUS_FMT_SGRBG10_1X10,
> +		DATA_TYPE_RAW_10BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> +		10,
> +		1,
> +	},
> +	{
> +		MEDIA_BUS_FMT_SRGGB10_1X10,
> +		DATA_TYPE_RAW_10BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> +		10,
> +		1,
> +	},
> +	{
> +		MEDIA_BUS_FMT_SBGGR12_1X12,
> +		DATA_TYPE_RAW_12BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
> +		12,
> +		1,
> +	},
> +	{
> +		MEDIA_BUS_FMT_SGBRG12_1X12,
> +		DATA_TYPE_RAW_12BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
> +		12,
> +		1,
> +	},
> +	{
> +		MEDIA_BUS_FMT_SGRBG12_1X12,
> +		DATA_TYPE_RAW_12BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
> +		12,
> +		1,
> +	},
> +	{
> +		MEDIA_BUS_FMT_SRGGB12_1X12,
> +		DATA_TYPE_RAW_12BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
> +		12,
> +		1,
> +	},
> +	{
> +		MEDIA_BUS_FMT_SBGGR14_1X14,
> +		DATA_TYPE_RAW_14BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_14_BIT,
> +		14,
> +		1,
> +	},
> +	{
> +		MEDIA_BUS_FMT_SGBRG14_1X14,
> +		DATA_TYPE_RAW_14BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_14_BIT,
> +		14,
> +		1,
> +	},
> +	{
> +		MEDIA_BUS_FMT_SGRBG14_1X14,
> +		DATA_TYPE_RAW_14BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_14_BIT,
> +		14,
> +		1,
> +	},
> +	{
> +		MEDIA_BUS_FMT_SRGGB14_1X14,
> +		DATA_TYPE_RAW_14BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_14_BIT,
> +		14,
> +		1,
> +	},
> +	{
> +		MEDIA_BUS_FMT_Y10_1X10,
> +		DATA_TYPE_RAW_10BIT,
> +		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> +		10,
> +		1,
> +	},
> +};
> +
> +static void csid_configure_stream(struct csid_device *csid, u8 enable)
> +{
> +	struct csid_testgen_config *tg = &csid->testgen;
> +	u32 sink_code = csid->fmt[MSM_CSID_PAD_SINK].code;
> +	u32 src_code = csid->fmt[MSM_CSID_PAD_SRC].code;
> +	u32 val;
> +
> +	if (enable) {
> +		struct v4l2_mbus_framefmt *input_format;
> +		const struct csid_format *format;
> +		u8 vc = 0; /* Virtual Channel 0 */
> +		u8 cid = vc * 4; /* id of Virtual Channel and Data Type set */
> +		u8 dt_shift;
> +
> +		if (tg->enabled) {
> +			/* Config Test Generator */
> +			u32 num_bytes_per_line, num_lines;
> +
> +			input_format = &csid->fmt[MSM_CSID_PAD_SRC];
> +			format = csid_get_fmt_entry(csid->formats, csid->nformats,
> +						    input_format->code);
> +			num_bytes_per_line = input_format->width * format->bpp * format->spp / 8;
> +			num_lines = input_format->height;
> +
> +			/* 31:24 V blank, 23:13 H blank, 3:2 num of active DT */
> +			/* 1:0 VC */
> +			val = ((CAMSS_CSID_TG_VC_CFG_V_BLANKING & 0xff) << 24) |
> +				  ((CAMSS_CSID_TG_VC_CFG_H_BLANKING & 0x7ff) << 13);
> +			writel_relaxed(val, csid->base + CAMSS_CSID_TG_VC_CFG);
> +
> +			/* 28:16 bytes per lines, 12:0 num of lines */
> +			val = ((num_bytes_per_line & 0x1fff) << 16) |
> +				  (num_lines & 0x1fff);
> +			writel_relaxed(val, csid->base + CAMSS_CSID_TG_DT_n_CGG_0(0));
> +
> +			/* 5:0 data type */
> +			val = format->data_type;
> +			writel_relaxed(val, csid->base + CAMSS_CSID_TG_DT_n_CGG_1(0));
> +
> +			/* 2:0 output test pattern */
> +			val = tg->mode;
> +			writel_relaxed(val, csid->base + CAMSS_CSID_TG_DT_n_CGG_2(0));
> +		} else {
> +			struct csid_phy_config *phy = &csid->phy;
> +
> +			input_format = &csid->fmt[MSM_CSID_PAD_SINK];
> +			format = csid_get_fmt_entry(csid->formats, csid->nformats,
> +						    input_format->code);
> +
> +			val = phy->lane_cnt - 1;
> +			val |= phy->lane_assign << 4;
> +
> +			writel_relaxed(val, csid->base + CAMSS_CSID_CORE_CTRL_0);
> +
> +			val = phy->csiphy_id << 17;
> +			val |= 0x9;
> +
> +			writel_relaxed(val, csid->base + CAMSS_CSID_CORE_CTRL_1);
> +		}
> +
> +		/* Config LUT */
> +
> +		dt_shift = (cid % 4) * 8;
> +
> +		val = readl_relaxed(csid->base + CAMSS_CSID_CID_LUT_VC_n(vc));
> +		val &= ~(0xff << dt_shift);
> +		val |= format->data_type << dt_shift;
> +		writel_relaxed(val, csid->base + CAMSS_CSID_CID_LUT_VC_n(vc));
> +
> +		val = CAMSS_CSID_CID_n_CFG_ISPIF_EN;
> +		val |= CAMSS_CSID_CID_n_CFG_RDI_EN;
> +		val |= format->decode_format << CAMSS_CSID_CID_n_CFG_DECODE_FORMAT_SHIFT;
> +		val |= CAMSS_CSID_CID_n_CFG_RDI_MODE_RAW_DUMP;
> +
> +		if ((sink_code == MEDIA_BUS_FMT_SBGGR10_1X10 &&
> +		     src_code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) ||
> +		    (sink_code == MEDIA_BUS_FMT_Y10_1X10 &&
> +		     src_code == MEDIA_BUS_FMT_Y10_2X8_PADHI_LE)) {
> +			val |= CAMSS_CSID_CID_n_CFG_RDI_MODE_PLAIN_PACKING;
> +			val |= CAMSS_CSID_CID_n_CFG_PLAIN_FORMAT_16;
> +			val |= CAMSS_CSID_CID_n_CFG_PLAIN_ALIGNMENT_LSB;
> +		}
> +
> +		writel_relaxed(val, csid->base + CAMSS_CSID_CID_n_CFG(cid));
> +
> +		if (tg->enabled) {
> +			val = CAMSS_CSID_TG_CTRL_ENABLE;
> +			writel_relaxed(val, csid->base + CAMSS_CSID_TG_CTRL);
> +		}
> +	} else {
> +		if (tg->enabled) {
> +			val = CAMSS_CSID_TG_CTRL_DISABLE;
> +			writel_relaxed(val, csid->base + CAMSS_CSID_TG_CTRL);
> +		}
> +	}
> +}
> +
> +static int csid_configure_testgen_pattern(struct csid_device *csid, s32 val)
> +{
> +	s32 regval = val - 1;
> +
> +	if (regval > 0 || regval <= CSID_PAYLOAD_MODE_MAX_SUPPORTED_4_7)
> +		csid->testgen.mode = regval;
> +
> +	return 0;
> +}
> +
> +static u32 csid_hw_version(struct csid_device *csid)
> +{
> +	u32 hw_version = readl_relaxed(csid->base + CAMSS_CSID_HW_VERSION);
> +
> +	dev_dbg(csid->camss->dev, "CSID HW Version = 0x%08x\n", hw_version);
> +
> +	return hw_version;
> +}
> +
> +/*
> + * isr - CSID module interrupt service routine
> + * @irq: Interrupt line
> + * @dev: CSID device
> + *
> + * Return IRQ_HANDLED on success
> + */
> +static irqreturn_t csid_isr(int irq, void *dev)
> +{
> +	struct csid_device *csid = dev;
> +	u32 value;
> +
> +	value = readl_relaxed(csid->base + CAMSS_CSID_IRQ_STATUS);
> +	writel_relaxed(value, csid->base + CAMSS_CSID_IRQ_CLEAR_CMD);
> +
> +	if ((value >> 11) & 0x1)
> +		complete(&csid->reset_complete);
> +
> +	return IRQ_HANDLED;
> +}
> +
> +/*
> + * csid_reset - Trigger reset on CSID module and wait to complete
> + * @csid: CSID device
> + *
> + * Return 0 on success or a negative error code otherwise
> + */
> +static int csid_reset(struct csid_device *csid)
> +{
> +	unsigned long time;
> +
> +	reinit_completion(&csid->reset_complete);
> +
> +	writel_relaxed(0x7fff, csid->base + CAMSS_CSID_RST_CMD);
> +
> +	time = wait_for_completion_timeout(&csid->reset_complete,
> +		msecs_to_jiffies(CSID_RESET_TIMEOUT_MS));
> +	if (!time) {
> +		dev_err(csid->camss->dev, "CSID reset timeout\n");
> +		return -EIO;
> +	}
> +
> +	return 0;
> +}
> +
> +static u32 csid_src_pad_code(struct csid_device *csid, u32 sink_code,
> +			     unsigned int match_format_idx, u32 match_code)
> +{
> +	switch (sink_code) {
> +	case MEDIA_BUS_FMT_SBGGR10_1X10:
> +	{
> +		u32 src_code[] = {
> +			MEDIA_BUS_FMT_SBGGR10_1X10,
> +			MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE,
> +		};
> +
> +		return csid_find_code(src_code, ARRAY_SIZE(src_code),
> +				      match_format_idx, match_code);
> +	}
> +	case MEDIA_BUS_FMT_Y10_1X10:
> +	{
> +		u32 src_code[] = {
> +			MEDIA_BUS_FMT_Y10_1X10,
> +			MEDIA_BUS_FMT_Y10_2X8_PADHI_LE,
> +		};
> +
> +		return csid_find_code(src_code, ARRAY_SIZE(src_code),
> +				      match_format_idx, match_code);
> +	}
> +	default:
> +		if (match_format_idx > 0)
> +			return 0;
> +
> +		return sink_code;
> +	}
> +}
> +
> +static void csid_subdev_init(struct csid_device *csid)
> +{
> +	csid->formats = csid_formats;
> +	csid->nformats = ARRAY_SIZE(csid_formats);
> +	csid->testgen.modes = csid_testgen_modes;
> +	csid->testgen.nmodes = CSID_PAYLOAD_MODE_MAX_SUPPORTED_4_7;
> +}
> +
> +const struct csid_hw_ops csid_ops_4_7 = {
> +	.configure_stream = csid_configure_stream,
> +	.configure_testgen_pattern = csid_configure_testgen_pattern,
> +	.hw_version = csid_hw_version,
> +	.isr = csid_isr,
> +	.reset = csid_reset,
> +	.src_pad_code = csid_src_pad_code,
> +	.subdev_init = csid_subdev_init,
> +};
> diff --git a/drivers/media/platform/qcom/camss/camss-csid.c b/drivers/media/platform/qcom/camss/camss-csid.c
> index be3fe76f3dc3..601bd810f2b0 100644
> --- a/drivers/media/platform/qcom/camss/camss-csid.c
> +++ b/drivers/media/platform/qcom/camss/camss-csid.c
> @@ -26,405 +26,35 @@
>   
>   #define MSM_CSID_NAME "msm_csid"
>   
> -#define CAMSS_CSID_HW_VERSION		0x0
> -#define CAMSS_CSID_CORE_CTRL_0		0x004
> -#define CAMSS_CSID_CORE_CTRL_1		0x008
> -#define CAMSS_CSID_RST_CMD(v)		((v) == CAMSS_8x16 ? 0x00c : 0x010)
> -#define CAMSS_CSID_CID_LUT_VC_n(v, n)	\
> -			(((v) == CAMSS_8x16 ? 0x010 : 0x014) + 0x4 * (n))
> -#define CAMSS_CSID_CID_n_CFG(v, n)	\
> -			(((v) == CAMSS_8x16 ? 0x020 : 0x024) + 0x4 * (n))
> -#define CAMSS_CSID_CID_n_CFG_ISPIF_EN	BIT(0)
> -#define CAMSS_CSID_CID_n_CFG_RDI_EN	BIT(1)
> -#define CAMSS_CSID_CID_n_CFG_DECODE_FORMAT_SHIFT	4
> -#define CAMSS_CSID_CID_n_CFG_PLAIN_FORMAT_8		(0 << 8)
> -#define CAMSS_CSID_CID_n_CFG_PLAIN_FORMAT_16		(1 << 8)
> -#define CAMSS_CSID_CID_n_CFG_PLAIN_ALIGNMENT_LSB	(0 << 9)
> -#define CAMSS_CSID_CID_n_CFG_PLAIN_ALIGNMENT_MSB	(1 << 9)
> -#define CAMSS_CSID_CID_n_CFG_RDI_MODE_RAW_DUMP		(0 << 10)
> -#define CAMSS_CSID_CID_n_CFG_RDI_MODE_PLAIN_PACKING	(1 << 10)
> -#define CAMSS_CSID_IRQ_CLEAR_CMD(v)	((v) == CAMSS_8x16 ? 0x060 : 0x064)
> -#define CAMSS_CSID_IRQ_MASK(v)		((v) == CAMSS_8x16 ? 0x064 : 0x068)
> -#define CAMSS_CSID_IRQ_STATUS(v)	((v) == CAMSS_8x16 ? 0x068 : 0x06c)
> -#define CAMSS_CSID_TG_CTRL(v)		((v) == CAMSS_8x16 ? 0x0a0 : 0x0a8)
> -#define CAMSS_CSID_TG_CTRL_DISABLE	0xa06436
> -#define CAMSS_CSID_TG_CTRL_ENABLE	0xa06437
> -#define CAMSS_CSID_TG_VC_CFG(v)		((v) == CAMSS_8x16 ? 0x0a4 : 0x0ac)
> -#define CAMSS_CSID_TG_VC_CFG_H_BLANKING		0x3ff
> -#define CAMSS_CSID_TG_VC_CFG_V_BLANKING		0x7f
> -#define CAMSS_CSID_TG_DT_n_CGG_0(v, n)	\
> -			(((v) == CAMSS_8x16 ? 0x0ac : 0x0b4) + 0xc * (n))
> -#define CAMSS_CSID_TG_DT_n_CGG_1(v, n)	\
> -			(((v) == CAMSS_8x16 ? 0x0b0 : 0x0b8) + 0xc * (n))
> -#define CAMSS_CSID_TG_DT_n_CGG_2(v, n)	\
> -			(((v) == CAMSS_8x16 ? 0x0b4 : 0x0bc) + 0xc * (n))
> -
> -#define DATA_TYPE_EMBEDDED_DATA_8BIT	0x12
> -#define DATA_TYPE_YUV422_8BIT		0x1e
> -#define DATA_TYPE_RAW_6BIT		0x28
> -#define DATA_TYPE_RAW_8BIT		0x2a
> -#define DATA_TYPE_RAW_10BIT		0x2b
> -#define DATA_TYPE_RAW_12BIT		0x2c
> -#define DATA_TYPE_RAW_14BIT		0x2d
> -
> -#define DECODE_FORMAT_UNCOMPRESSED_6_BIT	0x0
> -#define DECODE_FORMAT_UNCOMPRESSED_8_BIT	0x1
> -#define DECODE_FORMAT_UNCOMPRESSED_10_BIT	0x2
> -#define DECODE_FORMAT_UNCOMPRESSED_12_BIT	0x3
> -#define DECODE_FORMAT_UNCOMPRESSED_14_BIT	0x8
> -
> -#define CSID_RESET_TIMEOUT_MS 500
> -
> -struct csid_format {
> -	u32 code;
> -	u8 data_type;
> -	u8 decode_format;
> -	u8 bpp;
> -	u8 spp; /* bus samples per pixel */
> -};
> -
> -static const struct csid_format csid_formats_8x16[] = {
> -	{
> -		MEDIA_BUS_FMT_UYVY8_2X8,
> -		DATA_TYPE_YUV422_8BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> -		8,
> -		2,
> -	},
> -	{
> -		MEDIA_BUS_FMT_VYUY8_2X8,
> -		DATA_TYPE_YUV422_8BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> -		8,
> -		2,
> -	},
> -	{
> -		MEDIA_BUS_FMT_YUYV8_2X8,
> -		DATA_TYPE_YUV422_8BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> -		8,
> -		2,
> -	},
> -	{
> -		MEDIA_BUS_FMT_YVYU8_2X8,
> -		DATA_TYPE_YUV422_8BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> -		8,
> -		2,
> -	},
> -	{
> -		MEDIA_BUS_FMT_SBGGR8_1X8,
> -		DATA_TYPE_RAW_8BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> -		8,
> -		1,
> -	},
> -	{
> -		MEDIA_BUS_FMT_SGBRG8_1X8,
> -		DATA_TYPE_RAW_8BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> -		8,
> -		1,
> -	},
> -	{
> -		MEDIA_BUS_FMT_SGRBG8_1X8,
> -		DATA_TYPE_RAW_8BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> -		8,
> -		1,
> -	},
> -	{
> -		MEDIA_BUS_FMT_SRGGB8_1X8,
> -		DATA_TYPE_RAW_8BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> -		8,
> -		1,
> -	},
> -	{
> -		MEDIA_BUS_FMT_SBGGR10_1X10,
> -		DATA_TYPE_RAW_10BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> -		10,
> -		1,
> -	},
> -	{
> -		MEDIA_BUS_FMT_SGBRG10_1X10,
> -		DATA_TYPE_RAW_10BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> -		10,
> -		1,
> -	},
> -	{
> -		MEDIA_BUS_FMT_SGRBG10_1X10,
> -		DATA_TYPE_RAW_10BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> -		10,
> -		1,
> -	},
> -	{
> -		MEDIA_BUS_FMT_SRGGB10_1X10,
> -		DATA_TYPE_RAW_10BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> -		10,
> -		1,
> -	},
> -	{
> -		MEDIA_BUS_FMT_SBGGR12_1X12,
> -		DATA_TYPE_RAW_12BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
> -		12,
> -		1,
> -	},
> -	{
> -		MEDIA_BUS_FMT_SGBRG12_1X12,
> -		DATA_TYPE_RAW_12BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
> -		12,
> -		1,
> -	},
> -	{
> -		MEDIA_BUS_FMT_SGRBG12_1X12,
> -		DATA_TYPE_RAW_12BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
> -		12,
> -		1,
> -	},
> -	{
> -		MEDIA_BUS_FMT_SRGGB12_1X12,
> -		DATA_TYPE_RAW_12BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
> -		12,
> -		1,
> -	},
> -	{
> -		MEDIA_BUS_FMT_Y10_1X10,
> -		DATA_TYPE_RAW_10BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> -		10,
> -		1,
> -	},
> -};
> -
> -static const struct csid_format csid_formats_8x96[] = {
> -	{
> -		MEDIA_BUS_FMT_UYVY8_2X8,
> -		DATA_TYPE_YUV422_8BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> -		8,
> -		2,
> -	},
> -	{
> -		MEDIA_BUS_FMT_VYUY8_2X8,
> -		DATA_TYPE_YUV422_8BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> -		8,
> -		2,
> -	},
> -	{
> -		MEDIA_BUS_FMT_YUYV8_2X8,
> -		DATA_TYPE_YUV422_8BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> -		8,
> -		2,
> -	},
> -	{
> -		MEDIA_BUS_FMT_YVYU8_2X8,
> -		DATA_TYPE_YUV422_8BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> -		8,
> -		2,
> -	},
> -	{
> -		MEDIA_BUS_FMT_SBGGR8_1X8,
> -		DATA_TYPE_RAW_8BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> -		8,
> -		1,
> -	},
> -	{
> -		MEDIA_BUS_FMT_SGBRG8_1X8,
> -		DATA_TYPE_RAW_8BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> -		8,
> -		1,
> -	},
> -	{
> -		MEDIA_BUS_FMT_SGRBG8_1X8,
> -		DATA_TYPE_RAW_8BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> -		8,
> -		1,
> -	},
> -	{
> -		MEDIA_BUS_FMT_SRGGB8_1X8,
> -		DATA_TYPE_RAW_8BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> -		8,
> -		1,
> -	},
> -	{
> -		MEDIA_BUS_FMT_SBGGR10_1X10,
> -		DATA_TYPE_RAW_10BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> -		10,
> -		1,
> -	},
> -	{
> -		MEDIA_BUS_FMT_SGBRG10_1X10,
> -		DATA_TYPE_RAW_10BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> -		10,
> -		1,
> -	},
> -	{
> -		MEDIA_BUS_FMT_SGRBG10_1X10,
> -		DATA_TYPE_RAW_10BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> -		10,
> -		1,
> -	},
> -	{
> -		MEDIA_BUS_FMT_SRGGB10_1X10,
> -		DATA_TYPE_RAW_10BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> -		10,
> -		1,
> -	},
> -	{
> -		MEDIA_BUS_FMT_SBGGR12_1X12,
> -		DATA_TYPE_RAW_12BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
> -		12,
> -		1,
> -	},
> -	{
> -		MEDIA_BUS_FMT_SGBRG12_1X12,
> -		DATA_TYPE_RAW_12BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
> -		12,
> -		1,
> -	},
> -	{
> -		MEDIA_BUS_FMT_SGRBG12_1X12,
> -		DATA_TYPE_RAW_12BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
> -		12,
> -		1,
> -	},
> -	{
> -		MEDIA_BUS_FMT_SRGGB12_1X12,
> -		DATA_TYPE_RAW_12BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
> -		12,
> -		1,
> -	},
> -	{
> -		MEDIA_BUS_FMT_SBGGR14_1X14,
> -		DATA_TYPE_RAW_14BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_14_BIT,
> -		14,
> -		1,
> -	},
> -	{
> -		MEDIA_BUS_FMT_SGBRG14_1X14,
> -		DATA_TYPE_RAW_14BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_14_BIT,
> -		14,
> -		1,
> -	},
> -	{
> -		MEDIA_BUS_FMT_SGRBG14_1X14,
> -		DATA_TYPE_RAW_14BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_14_BIT,
> -		14,
> -		1,
> -	},
> -	{
> -		MEDIA_BUS_FMT_SRGGB14_1X14,
> -		DATA_TYPE_RAW_14BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_14_BIT,
> -		14,
> -		1,
> -	},
> -	{
> -		MEDIA_BUS_FMT_Y10_1X10,
> -		DATA_TYPE_RAW_10BIT,
> -		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> -		10,
> -		1,
> -	},
> -};
>   
> -static u32 csid_find_code(u32 *code, unsigned int n_code,
> -			  unsigned int index, u32 req_code)
> +u32 csid_find_code(u32 *codes, unsigned int ncodes,
> +		   unsigned int match_format_idx, u32 match_code)
>   {
>   	int i;
>   
> -	if (!req_code && (index >= n_code))
> +	if (!match_code && (match_format_idx >= ncodes))
>   		return 0;
>   
> -	for (i = 0; i < n_code; i++)
> -		if (req_code) {
> -			if (req_code == code[i])
> -				return req_code;
> +	for (i = 0; i < ncodes; i++)
> +		if (match_code) {
> +			if (codes[i] == match_code)
> +				return match_code;
>   		} else {
> -			if (i == index)
> -				return code[i];
> -		}
> -
> -	return code[0];
> -}
> -
> -static u32 csid_src_pad_code(struct csid_device *csid, u32 sink_code,
> -			     unsigned int index, u32 src_req_code)
> -{
> -	if (csid->camss->version == CAMSS_8x16) {
> -		if (index > 0)
> -			return 0;
> -
> -		return sink_code;
> -	} else if (csid->camss->version == CAMSS_8x96 ||
> -		   csid->camss->version == CAMSS_660) {
> -		switch (sink_code) {
> -		case MEDIA_BUS_FMT_SBGGR10_1X10:
> -		{
> -			u32 src_code[] = {
> -				MEDIA_BUS_FMT_SBGGR10_1X10,
> -				MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE,
> -			};
> -
> -			return csid_find_code(src_code, ARRAY_SIZE(src_code),
> -					      index, src_req_code);
> -		}
> -		case MEDIA_BUS_FMT_Y10_1X10:
> -		{
> -			u32 src_code[] = {
> -				MEDIA_BUS_FMT_Y10_1X10,
> -				MEDIA_BUS_FMT_Y10_2X8_PADHI_LE,
> -			};
> -
> -			return csid_find_code(src_code, ARRAY_SIZE(src_code),
> -					      index, src_req_code);
> +			if (i == match_format_idx)
> +				return codes[i];
>   		}
> -		default:
> -			if (index > 0)
> -				return 0;
>   
> -			return sink_code;
> -		}
> -	} else {
> -		return 0;
> -	}
> +	return codes[0];
>   }
>   
> -static const struct csid_format *csid_get_fmt_entry(
> +const struct csid_format *csid_get_fmt_entry(
>   					const struct csid_format *formats,
> -					unsigned int nformat,
> +					unsigned int nformats,
>   					u32 code)
>   {
>   	unsigned int i;
>   
> -	for (i = 0; i < nformat; i++)
> +	for (i = 0; i < nformats; i++)
>   		if (code == formats[i].code)
>   			return &formats[i];
>   
> @@ -433,28 +63,6 @@ static const struct csid_format *csid_get_fmt_entry(
>   	return &formats[0];
>   }
>   
> -/*
> - * csid_isr - CSID module interrupt handler
> - * @irq: Interrupt line
> - * @dev: CSID device
> - *
> - * Return IRQ_HANDLED on success
> - */
> -static irqreturn_t csid_isr(int irq, void *dev)
> -{
> -	struct csid_device *csid = dev;
> -	enum camss_version ver = csid->camss->version;
> -	u32 value;
> -
> -	value = readl_relaxed(csid->base + CAMSS_CSID_IRQ_STATUS(ver));
> -	writel_relaxed(value, csid->base + CAMSS_CSID_IRQ_CLEAR_CMD(ver));
> -
> -	if ((value >> 11) & 0x1)
> -		complete(&csid->reset_complete);
> -
> -	return IRQ_HANDLED;
> -}
> -
>   /*
>    * csid_set_clock_rates - Calculate and set clock rates on CSID module
>    * @csiphy: CSID device
> @@ -521,31 +129,6 @@ static int csid_set_clock_rates(struct csid_device *csid)
>   	return 0;
>   }
>   
> -/*
> - * csid_reset - Trigger reset on CSID module and wait to complete
> - * @csid: CSID device
> - *
> - * Return 0 on success or a negative error code otherwise
> - */
> -static int csid_reset(struct csid_device *csid)
> -{
> -	unsigned long time;
> -
> -	reinit_completion(&csid->reset_complete);
> -
> -	writel_relaxed(0x7fff, csid->base +
> -		       CAMSS_CSID_RST_CMD(csid->camss->version));
> -
> -	time = wait_for_completion_timeout(&csid->reset_complete,
> -		msecs_to_jiffies(CSID_RESET_TIMEOUT_MS));
> -	if (!time) {
> -		dev_err(csid->camss->dev, "CSID reset timeout\n");
> -		return -EIO;
> -	}
> -
> -	return 0;
> -}
> -
>   /*
>    * csid_set_power - Power on/off CSID module
>    * @sd: CSID V4L2 subdevice
> @@ -560,8 +143,6 @@ static int csid_set_power(struct v4l2_subdev *sd, int on)
>   	int ret;
>   
>   	if (on) {
> -		u32 hw_version;
> -
>   		ret = pm_runtime_get_sync(dev);
>   		if (ret < 0) {
>   			pm_runtime_put_sync(dev);
> @@ -590,7 +171,7 @@ static int csid_set_power(struct v4l2_subdev *sd, int on)
>   
>   		enable_irq(csid->irq);
>   
> -		ret = csid_reset(csid);
> +		ret = csid->ops->reset(csid);
>   		if (ret < 0) {
>   			disable_irq(csid->irq);
>   			camss_disable_clocks(csid->nclocks, csid->clock);
> @@ -599,8 +180,7 @@ static int csid_set_power(struct v4l2_subdev *sd, int on)
>   			return ret;
>   		}
>   
> -		hw_version = readl_relaxed(csid->base + CAMSS_CSID_HW_VERSION);
> -		dev_dbg(dev, "CSID HW Version = 0x%08x\n", hw_version);
> +		csid->ops->hw_version(csid);
>   	} else {
>   		disable_irq(csid->irq);
>   		camss_disable_clocks(csid->nclocks, csid->clock);
> @@ -623,16 +203,9 @@ static int csid_set_power(struct v4l2_subdev *sd, int on)
>   static int csid_set_stream(struct v4l2_subdev *sd, int enable)
>   {
>   	struct csid_device *csid = v4l2_get_subdevdata(sd);
> -	struct csid_testgen_config *tg = &csid->testgen;
> -	enum camss_version ver = csid->camss->version;
> -	u32 val;
> +	int ret;
>   
>   	if (enable) {
> -		u8 vc = 0; /* Virtual Channel 0 */
> -		u8 cid = vc * 4; /* id of Virtual Channel and Data Type set */
> -		u8 dt, dt_shift, df;
> -		int ret;
> -
>   		ret = v4l2_ctrl_handler_setup(&csid->ctrls);
>   		if (ret < 0) {
>   			dev_err(csid->camss->dev,
> @@ -640,116 +213,13 @@ static int csid_set_stream(struct v4l2_subdev *sd, int enable)
>   			return ret;
>   		}
>   
> -		if (!tg->enabled &&
> +		if (!csid->testgen.enabled &&
>   		    !media_entity_remote_pad(&csid->pads[MSM_CSID_PAD_SINK]))
>   			return -ENOLINK;
> -
> -		if (tg->enabled) {
> -			/* Config Test Generator */
> -			struct v4l2_mbus_framefmt *f =
> -					&csid->fmt[MSM_CSID_PAD_SRC];
> -			const struct csid_format *format = csid_get_fmt_entry(
> -					csid->formats, csid->nformats, f->code);
> -			u32 num_bytes_per_line =
> -				f->width * format->bpp * format->spp / 8;
> -			u32 num_lines = f->height;
> -
> -			/* 31:24 V blank, 23:13 H blank, 3:2 num of active DT */
> -			/* 1:0 VC */
> -			val = ((CAMSS_CSID_TG_VC_CFG_V_BLANKING & 0xff) << 24) |
> -			      ((CAMSS_CSID_TG_VC_CFG_H_BLANKING & 0x7ff) << 13);
> -			writel_relaxed(val, csid->base +
> -				       CAMSS_CSID_TG_VC_CFG(ver));
> -
> -			/* 28:16 bytes per lines, 12:0 num of lines */
> -			val = ((num_bytes_per_line & 0x1fff) << 16) |
> -			      (num_lines & 0x1fff);
> -			writel_relaxed(val, csid->base +
> -				       CAMSS_CSID_TG_DT_n_CGG_0(ver, 0));
> -
> -			dt = format->data_type;
> -
> -			/* 5:0 data type */
> -			val = dt;
> -			writel_relaxed(val, csid->base +
> -				       CAMSS_CSID_TG_DT_n_CGG_1(ver, 0));
> -
> -			/* 2:0 output test pattern */
> -			val = tg->payload_mode;
> -			writel_relaxed(val, csid->base +
> -				       CAMSS_CSID_TG_DT_n_CGG_2(ver, 0));
> -
> -			df = format->decode_format;
> -		} else {
> -			struct v4l2_mbus_framefmt *f =
> -					&csid->fmt[MSM_CSID_PAD_SINK];
> -			const struct csid_format *format = csid_get_fmt_entry(
> -					csid->formats, csid->nformats, f->code);
> -			struct csid_phy_config *phy = &csid->phy;
> -
> -			val = phy->lane_cnt - 1;
> -			val |= phy->lane_assign << 4;
> -
> -			writel_relaxed(val,
> -				       csid->base + CAMSS_CSID_CORE_CTRL_0);
> -
> -			val = phy->csiphy_id << 17;
> -			val |= 0x9;
> -
> -			writel_relaxed(val,
> -				       csid->base + CAMSS_CSID_CORE_CTRL_1);
> -
> -			dt = format->data_type;
> -			df = format->decode_format;
> -		}
> -
> -		/* Config LUT */
> -
> -		dt_shift = (cid % 4) * 8;
> -
> -		val = readl_relaxed(csid->base +
> -				    CAMSS_CSID_CID_LUT_VC_n(ver, vc));
> -		val &= ~(0xff << dt_shift);
> -		val |= dt << dt_shift;
> -		writel_relaxed(val, csid->base +
> -			       CAMSS_CSID_CID_LUT_VC_n(ver, vc));
> -
> -		val = CAMSS_CSID_CID_n_CFG_ISPIF_EN;
> -		val |= CAMSS_CSID_CID_n_CFG_RDI_EN;
> -		val |= df << CAMSS_CSID_CID_n_CFG_DECODE_FORMAT_SHIFT;
> -		val |= CAMSS_CSID_CID_n_CFG_RDI_MODE_RAW_DUMP;
> -
> -		if (csid->camss->version == CAMSS_8x96 ||
> -		    csid->camss->version == CAMSS_660) {
> -			u32 sink_code = csid->fmt[MSM_CSID_PAD_SINK].code;
> -			u32 src_code = csid->fmt[MSM_CSID_PAD_SRC].code;
> -
> -			if ((sink_code == MEDIA_BUS_FMT_SBGGR10_1X10 &&
> -			     src_code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) ||
> -			    (sink_code == MEDIA_BUS_FMT_Y10_1X10 &&
> -			     src_code == MEDIA_BUS_FMT_Y10_2X8_PADHI_LE)) {
> -				val |= CAMSS_CSID_CID_n_CFG_RDI_MODE_PLAIN_PACKING;
> -				val |= CAMSS_CSID_CID_n_CFG_PLAIN_FORMAT_16;
> -				val |= CAMSS_CSID_CID_n_CFG_PLAIN_ALIGNMENT_LSB;
> -			}
> -		}
> -
> -		writel_relaxed(val, csid->base +
> -			       CAMSS_CSID_CID_n_CFG(ver, cid));
> -
> -		if (tg->enabled) {
> -			val = CAMSS_CSID_TG_CTRL_ENABLE;
> -			writel_relaxed(val, csid->base +
> -				       CAMSS_CSID_TG_CTRL(ver));
> -		}
> -	} else {
> -		if (tg->enabled) {
> -			val = CAMSS_CSID_TG_CTRL_DISABLE;
> -			writel_relaxed(val, csid->base +
> -				       CAMSS_CSID_TG_CTRL(ver));
> -		}
>   	}
>   
> +	csid->ops->configure_stream(csid, enable);
> +
>   	return 0;
>   }
>   
> @@ -818,7 +288,7 @@ static void csid_try_format(struct csid_device *csid,
>   
>   			*fmt = *__csid_get_format(csid, cfg,
>   						      MSM_CSID_PAD_SINK, which);
> -			fmt->code = csid_src_pad_code(csid, fmt->code, 0, code);
> +			fmt->code = csid->ops->src_pad_code(csid, fmt->code, 0, code);
>   		} else {
>   			/* Test generator is enabled, set format on source */
>   			/* pad to allow test generator usage */
> @@ -868,7 +338,7 @@ static int csid_enum_mbus_code(struct v4l2_subdev *sd,
>   						     MSM_CSID_PAD_SINK,
>   						     code->which);
>   
> -			code->code = csid_src_pad_code(csid, sink_fmt->code,
> +			code->code = csid->ops->src_pad_code(csid, sink_fmt->code,
>   						       code->index, 0);
>   			if (!code->code)
>   				return -EINVAL;
> @@ -1004,15 +474,6 @@ static int csid_init_formats(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
>   	return csid_set_format(sd, fh ? fh->pad : NULL, &format);
>   }
>   
> -static const char * const csid_test_pattern_menu[] = {
> -	"Disabled",
> -	"Incrementing",
> -	"Alternating 0x55/0xAA",
> -	"All Zeros 0x00",
> -	"All Ones 0xFF",
> -	"Pseudo-random Data",
> -};
> -
>   /*
>    * csid_set_test_pattern - Set test generator's pattern mode
>    * @csid: CSID device
> @@ -1030,25 +491,7 @@ static int csid_set_test_pattern(struct csid_device *csid, s32 value)
>   
>   	tg->enabled = !!value;
>   
> -	switch (value) {
> -	case 1:
> -		tg->payload_mode = CSID_PAYLOAD_MODE_INCREMENTING;
> -		break;
> -	case 2:
> -		tg->payload_mode = CSID_PAYLOAD_MODE_ALTERNATING_55_AA;
> -		break;
> -	case 3:
> -		tg->payload_mode = CSID_PAYLOAD_MODE_ALL_ZEROES;
> -		break;
> -	case 4:
> -		tg->payload_mode = CSID_PAYLOAD_MODE_ALL_ONES;
> -		break;
> -	case 5:
> -		tg->payload_mode = CSID_PAYLOAD_MODE_RANDOM;
> -		break;
> -	}
> -
> -	return 0;
> +	return csid->ops->configure_testgen_pattern(csid, value);
>   }
>   
>   /*
> @@ -1097,17 +540,14 @@ int msm_csid_subdev_init(struct camss *camss, struct csid_device *csid,
>   	csid->id = id;
>   
>   	if (camss->version == CAMSS_8x16) {
> -		csid->formats = csid_formats_8x16;
> -		csid->nformats =
> -				ARRAY_SIZE(csid_formats_8x16);
> +		csid->ops = &csid_ops_4_1;
>   	} else if (camss->version == CAMSS_8x96 ||
>   		   camss->version == CAMSS_660) {
> -		csid->formats = csid_formats_8x96;
> -		csid->nformats =
> -				ARRAY_SIZE(csid_formats_8x96);
> +		csid->ops = &csid_ops_4_7;
>   	} else {
>   		return -EINVAL;
>   	}
> +	csid->ops->subdev_init(csid);
>   
>   	/* Memory */
>   
> @@ -1130,7 +570,7 @@ int msm_csid_subdev_init(struct camss *camss, struct csid_device *csid,
>   	csid->irq = r->start;
>   	snprintf(csid->irq_name, sizeof(csid->irq_name), "%s_%s%d",
>   		 dev_name(dev), MSM_CSID_NAME, csid->id);
> -	ret = devm_request_irq(dev, csid->irq, csid_isr,
> +	ret = devm_request_irq(dev, csid->irq, csid->ops->isr,
>   		IRQF_TRIGGER_RISING, csid->irq_name, csid);
>   	if (ret < 0) {
>   		dev_err(dev, "request_irq failed: %d\n", ret);
> @@ -1341,8 +781,8 @@ int msm_csid_register_entity(struct csid_device *csid,
>   
>   	csid->testgen_mode = v4l2_ctrl_new_std_menu_items(&csid->ctrls,
>   				&csid_ctrl_ops, V4L2_CID_TEST_PATTERN,
> -				ARRAY_SIZE(csid_test_pattern_menu) - 1, 0, 0,
> -				csid_test_pattern_menu);
> +				csid->testgen.nmodes, 0, 0,
> +				csid->testgen.modes);
>   
>   	if (csid->ctrls.error) {
>   		dev_err(dev, "Failed to init ctrl: %d\n", csid->ctrls.error);
> diff --git a/drivers/media/platform/qcom/camss/camss-csid.h b/drivers/media/platform/qcom/camss/camss-csid.h
> index 02fc34ee8a41..d40194e2bed3 100644
> --- a/drivers/media/platform/qcom/camss/camss-csid.h
> +++ b/drivers/media/platform/qcom/camss/camss-csid.h
> @@ -11,6 +11,7 @@
>   #define QC_MSM_CAMSS_CSID_H
>   
>   #include <linux/clk.h>
> +#include <linux/interrupt.h>
>   #include <media/media-entity.h>
>   #include <media/v4l2-ctrls.h>
>   #include <media/v4l2-device.h>
> @@ -70,19 +71,50 @@
>   #define PLAIN_FORMAT_PLAIN16	0x1 /* supports DPCM, UNCOMPRESSED_10/16_BIT */
>   #define PLAIN_FORMAT_PLAIN32	0x2 /* supports UNCOMPRESSED_20_BIT */
>   
> +#define CSID_RESET_TIMEOUT_MS 500
>   
> -enum csid_payload_mode {
> +
> +enum csid_testgen_mode {
>   	CSID_PAYLOAD_MODE_INCREMENTING = 0,
>   	CSID_PAYLOAD_MODE_ALTERNATING_55_AA = 1,
>   	CSID_PAYLOAD_MODE_ALL_ZEROES = 2,
>   	CSID_PAYLOAD_MODE_ALL_ONES = 3,
>   	CSID_PAYLOAD_MODE_RANDOM = 4,
>   	CSID_PAYLOAD_MODE_USER_SPECIFIED = 5,
> +	CSID_PAYLOAD_MODE_MAX_SUPPORTED_4_1 = 5,
> +	CSID_PAYLOAD_MODE_MAX_SUPPORTED_4_7 = 5,
> +	CSID_PAYLOAD_MODE_COMPLEX_PATTERN = 6,
> +	CSID_PAYLOAD_MODE_COLOR_BOX = 7,
> +	CSID_PAYLOAD_MODE_COLOR_BARS = 8,
> +	CSID_PAYLOAD_MODE_MAX_SUPPORTED_170 = 8,
> +};
> +
> +static const char * const csid_testgen_modes[] = {
> +	"Disabled",
> +	"Incrementing",
> +	"Alternating 0x55/0xAA",
> +	"All Zeros 0x00",
> +	"All Ones 0xFF",
> +	"Pseudo-random Data",
> +	"User Specified",
> +	"Complex pattern",
> +	"Color box",
> +	"Color bars",
> +};
> +
> +struct csid_format {
> +	u32 code;
> +	u8 data_type;
> +	u8 decode_format;
> +	u8 bpp;
> +	u8 spp; /* bus samples per pixel */
>   };
>   
>   struct csid_testgen_config {
> +	enum csid_testgen_mode mode;
> +	const char * const*modes;
> +	u8 nmodes;
>   	u8 enabled;
> -	enum csid_payload_mode payload_mode;
>   };
>   
>   struct csid_phy_config {
> @@ -91,6 +123,65 @@ struct csid_phy_config {
>   	u32 lane_assign;
>   };
>   
> +struct csid_device;
> +
> +struct csid_hw_ops {
> +	/*
> +	 * configure_stream - Configures and starts CSID input stream
> +	 * @csid: CSID device
> +	 */
> +	void (*configure_stream)(struct csid_device *csid, u8 enable);
> +
> +	/*
> +	 * configure_testgen_pattern - Validates and configures output pattern mode
> +	 * of test pattern generator
> +	 * @csid: CSID device
> +	 */
> +	int (*configure_testgen_pattern)(struct csid_device *csid, s32 val);
> +
> +	/*
> +	 * hw_version - Read hardware version register from hardware
> +	 * @csid: CSID device
> +	 */
> +	u32 (*hw_version)(struct csid_device *csid);
> +
> +	/*
> +	 * isr - CSID module interrupt service routine
> +	 * @irq: Interrupt line
> +	 * @dev: CSID device
> +	 *
> +	 * Return IRQ_HANDLED on success
> +	 */
> +	irqreturn_t (*isr)(int irq, void *dev);
> +
> +	/*
> +	 * reset - Trigger reset on CSID module and wait to complete
> +	 * @csid: CSID device
> +	 *
> +	 * Return 0 on success or a negative error code otherwise
> +	 */
> +	int (*reset)(struct csid_device *csid);
> +
> +	/*
> +	 * src_pad_code - Pick an output/src format based on the input/sink format
> +	 * @csid: CSID device
> +	 * @sink_code: The sink format of the input
> +	 * @match_format_idx: Request preferred index, as defined by subdevice csid_format.
> +	 *	Set @match_code to 0 if used.
> +	 * @match_code: Request preferred code, set @match_format_idx to 0 if used
> +	 *
> +	 * Return 0 on failure or src format code otherwise
> +	 */
> +	u32 (*src_pad_code)(struct csid_device *csid, u32 sink_code,
> +			    unsigned int match_format_idx, u32 match_code);
> +
> +	/*
> +	 * subdev_init - Initialize CSID device according for hardware revision
> +	 * @csid: CSID device
> +	 */
> +	void (*subdev_init)(struct csid_device *csid);
> +};
> +
>   struct csid_device {
>   	struct camss *camss;
>   	u8 id;
> @@ -110,10 +201,37 @@ struct csid_device {
>   	struct v4l2_ctrl *testgen_mode;
>   	const struct csid_format *formats;
>   	unsigned int nformats;
> +	const struct csid_hw_ops *ops;
>   };
>   
>   struct resources;
>   
> +
> +/*
> + * csid_find_code - Find a format code in an array using array index or format code
> + * @codes: Array of format codes
> + * @ncodes: Length of @code array
> + * @req_format_idx: Request preferred index, as defined by subdevice csid_format.
> + *	Set @match_code to 0 if used.
> + * @match_code: Request preferred code, set @req_format_idx to 0 if used
> + *
> + * Return 0 on failure or format code otherwise
> + */
> +u32 csid_find_code(u32 *codes, unsigned int ncode,
> +		   unsigned int match_format_idx, u32 match_code);
> +
> +/*
> + * csid_get_fmt_entry - Find csid_format entry with matching format code
> + * @formats: Array of format csid_format entries
> + * @nformats: Length of @nformats array
> + * @code: Desired format code
> + *
> + * Return formats[0] on failure to find code
> + */
> +const struct csid_format *csid_get_fmt_entry(const struct csid_format *formats,
> +					     unsigned int nformats,
> +					     u32 code);
> +
>   int msm_csid_subdev_init(struct camss *camss, struct csid_device *csid,
>   			 const struct resources *res, u8 id);
>   
> @@ -124,4 +242,8 @@ void msm_csid_unregister_entity(struct csid_device *csid);
>   
>   void msm_csid_get_csid_id(struct media_entity *entity, u8 *id);
>   
> +
> +extern const struct csid_hw_ops csid_ops_4_1;
> +extern const struct csid_hw_ops csid_ops_4_7;
> +
>   #endif /* QC_MSM_CAMSS_CSID_H */
>
Robert Foss Feb. 15, 2021, 10:23 a.m. UTC | #2 
Thanks Andrey!

On Thu, 11 Feb 2021 at 09:59, Andrey Konovalov
<andrey.konovalov@linaro.org> wrote:
>
> Hi Robert,
>
> Thank you for your patch!
>
> On 05.02.2021 13:44, Robert Foss wrote:
> > In order to support Qualcomm ISP hardware architectures that diverge
> > from older architectures, the CSID subdevice drivers needs to be refactored
> > to better abstract the different ISP hardware architectures.
> >
> > Signed-off-by: Robert Foss <robert.foss@linaro.org>
> > ---
> >
> > Changes since v1
> >   - kernel test robot: Add missing include, interrupt.h
> >
> >
> >   drivers/media/platform/qcom/camss/Makefile    |   2 +
> >   .../platform/qcom/camss/camss-csid-4-1.c      | 338 ++++++++++
> >   .../platform/qcom/camss/camss-csid-4-7.c      | 406 ++++++++++++
> >   .../media/platform/qcom/camss/camss-csid.c    | 616 +-----------------
> >   .../media/platform/qcom/camss/camss-csid.h    | 126 +++-
> >   5 files changed, 898 insertions(+), 590 deletions(-)
> >   create mode 100644 drivers/media/platform/qcom/camss/camss-csid-4-1.c
> >   create mode 100644 drivers/media/platform/qcom/camss/camss-csid-4-7.c
> >
> > diff --git a/drivers/media/platform/qcom/camss/Makefile b/drivers/media/platform/qcom/camss/Makefile
> > index 052c4f405fa3..cff388b653ba 100644
> > --- a/drivers/media/platform/qcom/camss/Makefile
> > +++ b/drivers/media/platform/qcom/camss/Makefile
> > @@ -4,6 +4,8 @@
> >   qcom-camss-objs += \
> >               camss.o \
> >               camss-csid.o \
> > +             camss-csid-4-1.o \
> > +             camss-csid-4-7.o \
> >               camss-csiphy-2ph-1-0.o \
> >               camss-csiphy-3ph-1-0.o \
> >               camss-csiphy.o \
> > diff --git a/drivers/media/platform/qcom/camss/camss-csid-4-1.c b/drivers/media/platform/qcom/camss/camss-csid-4-1.c
> > new file mode 100644
> > index 000000000000..84b415137555
> > --- /dev/null
> > +++ b/drivers/media/platform/qcom/camss/camss-csid-4-1.c
> > @@ -0,0 +1,338 @@
> > +// SPDX-License-Identifier: GPL-2.0
> > +/*
> > + * camss-csid-4-1.c
> > + *
> > + * Qualcomm MSM Camera Subsystem - CSID (CSI Decoder) Module
> > + *
> > + * Copyright (C) 2020 Linaro Ltd.
> > + */
>
> Why is this extra space before each #include below:

Good Q, I suspect an editor stabbed me in the back. Fixing this in v5.

>
> > + #include <linux/completion.h>
> > + #include <linux/interrupt.h>
> > + #include <linux/io.h>
> > + #include <linux/kernel.h>
> > + #include <linux/of.h>
>     ^
>     here?
>
> > +
> > +#include "camss-csid.h"
> > +#include "camss.h"
> > +
> > +#define CAMSS_CSID_HW_VERSION                0x0
> > +#define CAMSS_CSID_CORE_CTRL_0               0x004
> > +#define CAMSS_CSID_CORE_CTRL_1               0x008
> > +#define CAMSS_CSID_RST_CMD           0x00c
> > +#define CAMSS_CSID_CID_LUT_VC_n(n)   (0x010 + 0x4 * (n))
> > +#define CAMSS_CSID_CID_n_CFG(n)              (0x020 + 0x4 * (n))
> > +#define CAMSS_CSID_CID_n_CFG_ISPIF_EN        BIT(0)
> > +#define CAMSS_CSID_CID_n_CFG_RDI_EN  BIT(1)
> > +#define CAMSS_CSID_CID_n_CFG_DECODE_FORMAT_SHIFT     4
> > +#define CAMSS_CSID_CID_n_CFG_PLAIN_FORMAT_8          (0 << 8)
> > +#define CAMSS_CSID_CID_n_CFG_PLAIN_FORMAT_16         (1 << 8)
> > +#define CAMSS_CSID_CID_n_CFG_PLAIN_ALIGNMENT_LSB     (0 << 9)
> > +#define CAMSS_CSID_CID_n_CFG_PLAIN_ALIGNMENT_MSB     (1 << 9)
> > +#define CAMSS_CSID_CID_n_CFG_RDI_MODE_RAW_DUMP               (0 << 10)
> > +#define CAMSS_CSID_CID_n_CFG_RDI_MODE_PLAIN_PACKING  (1 << 10)
> > +#define CAMSS_CSID_IRQ_CLEAR_CMD     0x060
> > +#define CAMSS_CSID_IRQ_MASK          0x064
> > +#define CAMSS_CSID_IRQ_STATUS                0x068
> > +#define CAMSS_CSID_TG_CTRL           0x0a0
> > +#define CAMSS_CSID_TG_CTRL_DISABLE   0xa06436
> > +#define CAMSS_CSID_TG_CTRL_ENABLE    0xa06437
> > +#define CAMSS_CSID_TG_VC_CFG         0x0a4
> > +#define CAMSS_CSID_TG_VC_CFG_H_BLANKING              0x3ff
> > +#define CAMSS_CSID_TG_VC_CFG_V_BLANKING              0x7f
> > +#define CAMSS_CSID_TG_DT_n_CGG_0(n)  (0x0ac + 0xc * (n))
> > +#define CAMSS_CSID_TG_DT_n_CGG_1(n)  (0x0b0 + 0xc * (n))
> > +#define CAMSS_CSID_TG_DT_n_CGG_2(n)  (0x0b4 + 0xc * (n))
> > +
> > +
> > +enum csid_testgen_pattern {
> > +     TESTGEN_PATTERN_INCREMENTING = 0,
> > +     TESTGEN_PATTERN_ALTERNATING_55_AA = 1,
> > +     TESTGEN_PATTERN_ALL_ZEROES = 2,
> > +     TESTGEN_PATTERN_ALL_ONES = 3,
> > +     TESTGEN_PATTERN_RANDOM = 4,
> > +     TESTGEN_PATTERN_USER_SPECIFIED = 5,
> > +};
>
> - this enum is unused.

Ack, fixing in v5.

>
> Thanks,
> Andrey
>
> > +
> > +static const struct csid_format csid_formats[] = {
> > +     {
> > +             MEDIA_BUS_FMT_UYVY8_2X8,
> > +             DATA_TYPE_YUV422_8BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> > +             8,
> > +             2,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_VYUY8_2X8,
> > +             DATA_TYPE_YUV422_8BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> > +             8,
> > +             2,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_YUYV8_2X8,
> > +             DATA_TYPE_YUV422_8BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> > +             8,
> > +             2,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_YVYU8_2X8,
> > +             DATA_TYPE_YUV422_8BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> > +             8,
> > +             2,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_SBGGR8_1X8,
> > +             DATA_TYPE_RAW_8BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> > +             8,
> > +             1,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_SGBRG8_1X8,
> > +             DATA_TYPE_RAW_8BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> > +             8,
> > +             1,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_SGRBG8_1X8,
> > +             DATA_TYPE_RAW_8BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> > +             8,
> > +             1,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_SRGGB8_1X8,
> > +             DATA_TYPE_RAW_8BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> > +             8,
> > +             1,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_SBGGR10_1X10,
> > +             DATA_TYPE_RAW_10BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> > +             10,
> > +             1,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_SGBRG10_1X10,
> > +             DATA_TYPE_RAW_10BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> > +             10,
> > +             1,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_SGRBG10_1X10,
> > +             DATA_TYPE_RAW_10BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> > +             10,
> > +             1,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_SRGGB10_1X10,
> > +             DATA_TYPE_RAW_10BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> > +             10,
> > +             1,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_SBGGR12_1X12,
> > +             DATA_TYPE_RAW_12BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_12_BIT,
> > +             12,
> > +             1,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_SGBRG12_1X12,
> > +             DATA_TYPE_RAW_12BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_12_BIT,
> > +             12,
> > +             1,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_SGRBG12_1X12,
> > +             DATA_TYPE_RAW_12BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_12_BIT,
> > +             12,
> > +             1,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_SRGGB12_1X12,
> > +             DATA_TYPE_RAW_12BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_12_BIT,
> > +             12,
> > +             1,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_Y10_1X10,
> > +             DATA_TYPE_RAW_10BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> > +             10,
> > +             1,
> > +     },
> > +};
> > +
> > +static void csid_configure_stream(struct csid_device *csid, u8 enable)
> > +{
> > +     struct csid_testgen_config *tg = &csid->testgen;
> > +     u32 val;
> > +
> > +     if (enable) {
> > +             struct v4l2_mbus_framefmt *input_format;
> > +             const struct csid_format *format;
> > +             u8 vc = 0; /* Virtual Channel 0 */
> > +             u8 cid = vc * 4; /* id of Virtual Channel and Data Type set */
> > +             u8 dt_shift;
> > +
> > +             if (tg->enabled) {
> > +                     /* Config Test Generator */
> > +                     u32 num_lines, num_bytes_per_line;
> > +
> > +                     input_format = &csid->fmt[MSM_CSID_PAD_SRC];
> > +                     format = csid_get_fmt_entry(csid->formats, csid->nformats,
> > +                                                 input_format->code);
> > +                     num_bytes_per_line = input_format->width * format->bpp * format->spp / 8;
> > +                     num_lines = input_format->height;
> > +
> > +                     /* 31:24 V blank, 23:13 H blank, 3:2 num of active DT */
> > +                     /* 1:0 VC */
> > +                     val = ((CAMSS_CSID_TG_VC_CFG_V_BLANKING & 0xff) << 24) |
> > +                               ((CAMSS_CSID_TG_VC_CFG_H_BLANKING & 0x7ff) << 13);
> > +                     writel_relaxed(val, csid->base + CAMSS_CSID_TG_VC_CFG);
> > +
> > +                     /* 28:16 bytes per lines, 12:0 num of lines */
> > +                     val = ((num_bytes_per_line & 0x1fff) << 16) |
> > +                               (num_lines & 0x1fff);
> > +                     writel_relaxed(val, csid->base + CAMSS_CSID_TG_DT_n_CGG_0(0));
> > +
> > +                     /* 5:0 data type */
> > +                     val = format->data_type;
> > +                     writel_relaxed(val, csid->base + CAMSS_CSID_TG_DT_n_CGG_1(0));
> > +
> > +                     /* 2:0 output test pattern */
> > +                     val = tg->mode;
> > +                     writel_relaxed(val, csid->base + CAMSS_CSID_TG_DT_n_CGG_2(0));
> > +             } else {
> > +                     struct csid_phy_config *phy = &csid->phy;
> > +
> > +                     input_format = &csid->fmt[MSM_CSID_PAD_SINK];
> > +                     format = csid_get_fmt_entry(csid->formats, csid->nformats,
> > +                                                 input_format->code);
> > +
> > +                     val = phy->lane_cnt - 1;
> > +                     val |= phy->lane_assign << 4;
> > +
> > +                     writel_relaxed(val, csid->base + CAMSS_CSID_CORE_CTRL_0);
> > +
> > +                     val = phy->csiphy_id << 17;
> > +                     val |= 0x9;
> > +
> > +                     writel_relaxed(val, csid->base + CAMSS_CSID_CORE_CTRL_1);
> > +             }
> > +
> > +             /* Config LUT */
> > +
> > +             dt_shift = (cid % 4) * 8;
> > +             val = readl_relaxed(csid->base + CAMSS_CSID_CID_LUT_VC_n(vc));
> > +             val &= ~(0xff << dt_shift);
> > +             val |= format->data_type << dt_shift;
> > +             writel_relaxed(val, csid->base + CAMSS_CSID_CID_LUT_VC_n(vc));
> > +
> > +             val = CAMSS_CSID_CID_n_CFG_ISPIF_EN;
> > +             val |= CAMSS_CSID_CID_n_CFG_RDI_EN;
> > +             val |= format->decode_format << CAMSS_CSID_CID_n_CFG_DECODE_FORMAT_SHIFT;
> > +             val |= CAMSS_CSID_CID_n_CFG_RDI_MODE_RAW_DUMP;
> > +             writel_relaxed(val, csid->base + CAMSS_CSID_CID_n_CFG(cid));
> > +
> > +             if (tg->enabled) {
> > +                     val = CAMSS_CSID_TG_CTRL_ENABLE;
> > +                     writel_relaxed(val, csid->base + CAMSS_CSID_TG_CTRL);
> > +             }
> > +     } else {
> > +             if (tg->enabled) {
> > +                     val = CAMSS_CSID_TG_CTRL_DISABLE;
> > +                     writel_relaxed(val, csid->base + CAMSS_CSID_TG_CTRL);
> > +             }
> > +     }
> > +}
> > +
> > +static int csid_configure_testgen_pattern(struct csid_device *csid, s32 val)
> > +{
> > +     s32 regval = val - 1;
> > +
> > +     if (regval > 0 || regval <= CSID_PAYLOAD_MODE_MAX_SUPPORTED_4_1)
> > +             csid->testgen.mode = regval;
> > +
> > +     return 0;
> > +}
> > +
> > +static u32 csid_hw_version(struct csid_device *csid)
> > +{
> > +     u32 hw_version = readl_relaxed(csid->base + CAMSS_CSID_HW_VERSION);
> > +
> > +     dev_dbg(csid->camss->dev, "CSID HW Version = 0x%08x\n", hw_version);
> > +
> > +     return hw_version;
> > +}
> > +
> > +static irqreturn_t csid_isr(int irq, void *dev)
> > +{
> > +     struct csid_device *csid = dev;
> > +     u32 value;
> > +
> > +     value = readl_relaxed(csid->base + CAMSS_CSID_IRQ_STATUS);
> > +     writel_relaxed(value, csid->base + CAMSS_CSID_IRQ_CLEAR_CMD);
> > +
> > +     if ((value >> 11) & 0x1)
> > +             complete(&csid->reset_complete);
> > +
> > +     return IRQ_HANDLED;
> > +}
> > +
> > +static int csid_reset(struct csid_device *csid)
> > +{
> > +     unsigned long time;
> > +
> > +     reinit_completion(&csid->reset_complete);
> > +
> > +     writel_relaxed(0x7fff, csid->base + CAMSS_CSID_RST_CMD);
> > +
> > +     time = wait_for_completion_timeout(&csid->reset_complete,
> > +             msecs_to_jiffies(CSID_RESET_TIMEOUT_MS));
> > +     if (!time) {
> > +             dev_err(csid->camss->dev, "CSID reset timeout\n");
> > +             return -EIO;
> > +     }
> > +
> > +     return 0;
> > +}
> > +
> > +static u32 csid_src_pad_code(struct csid_device *csid, u32 sink_code,
> > +                          unsigned int match_format_idx, u32 match_code)
> > +{
> > +     if (match_format_idx > 0)
> > +             return 0;
> > +
> > +     return sink_code;
> > +}
> > +
> > +static void csid_subdev_init(struct csid_device *csid)
> > +{
> > +     csid->formats = csid_formats;
> > +     csid->nformats = ARRAY_SIZE(csid_formats);
> > +     csid->testgen.modes = csid_testgen_modes;
> > +     csid->testgen.nmodes = CSID_PAYLOAD_MODE_MAX_SUPPORTED_4_1;
> > +}
> > +
> > +const struct csid_hw_ops csid_ops_4_1 = {
> > +     .configure_stream = csid_configure_stream,
> > +     .configure_testgen_pattern = csid_configure_testgen_pattern,
> > +     .hw_version = csid_hw_version,
> > +     .isr = csid_isr,
> > +     .reset = csid_reset,
> > +     .src_pad_code = csid_src_pad_code,
> > +     .subdev_init = csid_subdev_init,
> > +};
> > diff --git a/drivers/media/platform/qcom/camss/camss-csid-4-7.c b/drivers/media/platform/qcom/camss/camss-csid-4-7.c
> > new file mode 100644
> > index 000000000000..16a69b140f4e
> > --- /dev/null
> > +++ b/drivers/media/platform/qcom/camss/camss-csid-4-7.c
> > @@ -0,0 +1,406 @@
> > +// SPDX-License-Identifier: GPL-2.0
> > +/*
> > + * camss-csid-4-7.c
> > + *
> > + * Qualcomm MSM Camera Subsystem - CSID (CSI Decoder) Module
> > + *
> > + * Copyright (C) 2020 Linaro Ltd.
> > + */
> > +#include <linux/completion.h>
> > +#include <linux/interrupt.h>
> > +#include <linux/io.h>
> > +#include <linux/kernel.h>
> > +#include <linux/of.h>
> > +
> > +#include "camss-csid.h"
> > +#include "camss.h"
> > +
> > +#define CAMSS_CSID_HW_VERSION                0x0
> > +#define CAMSS_CSID_CORE_CTRL_0               0x004
> > +#define CAMSS_CSID_CORE_CTRL_1               0x008
> > +#define CAMSS_CSID_RST_CMD           0x010
> > +#define CAMSS_CSID_CID_LUT_VC_n(n)   (0x014 + 0x4 * (n))
> > +#define CAMSS_CSID_CID_n_CFG(n)              (0x024 + 0x4 * (n))
> > +#define CAMSS_CSID_CID_n_CFG_ISPIF_EN        BIT(0)
> > +#define CAMSS_CSID_CID_n_CFG_RDI_EN  BIT(1)
> > +#define CAMSS_CSID_CID_n_CFG_DECODE_FORMAT_SHIFT     4
> > +#define CAMSS_CSID_CID_n_CFG_PLAIN_FORMAT_8          (0 << 8)
> > +#define CAMSS_CSID_CID_n_CFG_PLAIN_FORMAT_16         (1 << 8)
> > +#define CAMSS_CSID_CID_n_CFG_PLAIN_ALIGNMENT_LSB     (0 << 9)
> > +#define CAMSS_CSID_CID_n_CFG_PLAIN_ALIGNMENT_MSB     (1 << 9)
> > +#define CAMSS_CSID_CID_n_CFG_RDI_MODE_RAW_DUMP               (0 << 10)
> > +#define CAMSS_CSID_CID_n_CFG_RDI_MODE_PLAIN_PACKING  (1 << 10)
> > +#define CAMSS_CSID_IRQ_CLEAR_CMD     0x064
> > +#define CAMSS_CSID_IRQ_MASK          0x068
> > +#define CAMSS_CSID_IRQ_STATUS                0x06c
> > +#define CAMSS_CSID_TG_CTRL           0x0a8
> > +#define CAMSS_CSID_TG_CTRL_DISABLE   0xa06436
> > +#define CAMSS_CSID_TG_CTRL_ENABLE    0xa06437
> > +#define CAMSS_CSID_TG_VC_CFG         0x0ac
> > +#define CAMSS_CSID_TG_VC_CFG_H_BLANKING              0x3ff
> > +#define CAMSS_CSID_TG_VC_CFG_V_BLANKING              0x7f
> > +#define CAMSS_CSID_TG_DT_n_CGG_0(n)  (0x0b4 + 0xc * (n))
> > +#define CAMSS_CSID_TG_DT_n_CGG_1(n)  (0x0b8 + 0xc * (n))
> > +#define CAMSS_CSID_TG_DT_n_CGG_2(n)  (0x0bc + 0xc * (n))
> > +
> > +
> > +static const struct csid_format csid_formats[] = {
> > +     {
> > +             MEDIA_BUS_FMT_UYVY8_2X8,
> > +             DATA_TYPE_YUV422_8BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> > +             8,
> > +             2,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_VYUY8_2X8,
> > +             DATA_TYPE_YUV422_8BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> > +             8,
> > +             2,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_YUYV8_2X8,
> > +             DATA_TYPE_YUV422_8BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> > +             8,
> > +             2,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_YVYU8_2X8,
> > +             DATA_TYPE_YUV422_8BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> > +             8,
> > +             2,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_SBGGR8_1X8,
> > +             DATA_TYPE_RAW_8BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> > +             8,
> > +             1,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_SGBRG8_1X8,
> > +             DATA_TYPE_RAW_8BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> > +             8,
> > +             1,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_SGRBG8_1X8,
> > +             DATA_TYPE_RAW_8BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> > +             8,
> > +             1,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_SRGGB8_1X8,
> > +             DATA_TYPE_RAW_8BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> > +             8,
> > +             1,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_SBGGR10_1X10,
> > +             DATA_TYPE_RAW_10BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> > +             10,
> > +             1,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_SGBRG10_1X10,
> > +             DATA_TYPE_RAW_10BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> > +             10,
> > +             1,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_SGRBG10_1X10,
> > +             DATA_TYPE_RAW_10BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> > +             10,
> > +             1,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_SRGGB10_1X10,
> > +             DATA_TYPE_RAW_10BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> > +             10,
> > +             1,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_SBGGR12_1X12,
> > +             DATA_TYPE_RAW_12BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_12_BIT,
> > +             12,
> > +             1,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_SGBRG12_1X12,
> > +             DATA_TYPE_RAW_12BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_12_BIT,
> > +             12,
> > +             1,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_SGRBG12_1X12,
> > +             DATA_TYPE_RAW_12BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_12_BIT,
> > +             12,
> > +             1,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_SRGGB12_1X12,
> > +             DATA_TYPE_RAW_12BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_12_BIT,
> > +             12,
> > +             1,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_SBGGR14_1X14,
> > +             DATA_TYPE_RAW_14BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_14_BIT,
> > +             14,
> > +             1,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_SGBRG14_1X14,
> > +             DATA_TYPE_RAW_14BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_14_BIT,
> > +             14,
> > +             1,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_SGRBG14_1X14,
> > +             DATA_TYPE_RAW_14BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_14_BIT,
> > +             14,
> > +             1,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_SRGGB14_1X14,
> > +             DATA_TYPE_RAW_14BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_14_BIT,
> > +             14,
> > +             1,
> > +     },
> > +     {
> > +             MEDIA_BUS_FMT_Y10_1X10,
> > +             DATA_TYPE_RAW_10BIT,
> > +             DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> > +             10,
> > +             1,
> > +     },
> > +};
> > +
> > +static void csid_configure_stream(struct csid_device *csid, u8 enable)
> > +{
> > +     struct csid_testgen_config *tg = &csid->testgen;
> > +     u32 sink_code = csid->fmt[MSM_CSID_PAD_SINK].code;
> > +     u32 src_code = csid->fmt[MSM_CSID_PAD_SRC].code;
> > +     u32 val;
> > +
> > +     if (enable) {
> > +             struct v4l2_mbus_framefmt *input_format;
> > +             const struct csid_format *format;
> > +             u8 vc = 0; /* Virtual Channel 0 */
> > +             u8 cid = vc * 4; /* id of Virtual Channel and Data Type set */
> > +             u8 dt_shift;
> > +
> > +             if (tg->enabled) {
> > +                     /* Config Test Generator */
> > +                     u32 num_bytes_per_line, num_lines;
> > +
> > +                     input_format = &csid->fmt[MSM_CSID_PAD_SRC];
> > +                     format = csid_get_fmt_entry(csid->formats, csid->nformats,
> > +                                                 input_format->code);
> > +                     num_bytes_per_line = input_format->width * format->bpp * format->spp / 8;
> > +                     num_lines = input_format->height;
> > +
> > +                     /* 31:24 V blank, 23:13 H blank, 3:2 num of active DT */
> > +                     /* 1:0 VC */
> > +                     val = ((CAMSS_CSID_TG_VC_CFG_V_BLANKING & 0xff) << 24) |
> > +                               ((CAMSS_CSID_TG_VC_CFG_H_BLANKING & 0x7ff) << 13);
> > +                     writel_relaxed(val, csid->base + CAMSS_CSID_TG_VC_CFG);
> > +
> > +                     /* 28:16 bytes per lines, 12:0 num of lines */
> > +                     val = ((num_bytes_per_line & 0x1fff) << 16) |
> > +                               (num_lines & 0x1fff);
> > +                     writel_relaxed(val, csid->base + CAMSS_CSID_TG_DT_n_CGG_0(0));
> > +
> > +                     /* 5:0 data type */
> > +                     val = format->data_type;
> > +                     writel_relaxed(val, csid->base + CAMSS_CSID_TG_DT_n_CGG_1(0));
> > +
> > +                     /* 2:0 output test pattern */
> > +                     val = tg->mode;
> > +                     writel_relaxed(val, csid->base + CAMSS_CSID_TG_DT_n_CGG_2(0));
> > +             } else {
> > +                     struct csid_phy_config *phy = &csid->phy;
> > +
> > +                     input_format = &csid->fmt[MSM_CSID_PAD_SINK];
> > +                     format = csid_get_fmt_entry(csid->formats, csid->nformats,
> > +                                                 input_format->code);
> > +
> > +                     val = phy->lane_cnt - 1;
> > +                     val |= phy->lane_assign << 4;
> > +
> > +                     writel_relaxed(val, csid->base + CAMSS_CSID_CORE_CTRL_0);
> > +
> > +                     val = phy->csiphy_id << 17;
> > +                     val |= 0x9;
> > +
> > +                     writel_relaxed(val, csid->base + CAMSS_CSID_CORE_CTRL_1);
> > +             }
> > +
> > +             /* Config LUT */
> > +
> > +             dt_shift = (cid % 4) * 8;
> > +
> > +             val = readl_relaxed(csid->base + CAMSS_CSID_CID_LUT_VC_n(vc));
> > +             val &= ~(0xff << dt_shift);
> > +             val |= format->data_type << dt_shift;
> > +             writel_relaxed(val, csid->base + CAMSS_CSID_CID_LUT_VC_n(vc));
> > +
> > +             val = CAMSS_CSID_CID_n_CFG_ISPIF_EN;
> > +             val |= CAMSS_CSID_CID_n_CFG_RDI_EN;
> > +             val |= format->decode_format << CAMSS_CSID_CID_n_CFG_DECODE_FORMAT_SHIFT;
> > +             val |= CAMSS_CSID_CID_n_CFG_RDI_MODE_RAW_DUMP;
> > +
> > +             if ((sink_code == MEDIA_BUS_FMT_SBGGR10_1X10 &&
> > +                  src_code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) ||
> > +                 (sink_code == MEDIA_BUS_FMT_Y10_1X10 &&
> > +                  src_code == MEDIA_BUS_FMT_Y10_2X8_PADHI_LE)) {
> > +                     val |= CAMSS_CSID_CID_n_CFG_RDI_MODE_PLAIN_PACKING;
> > +                     val |= CAMSS_CSID_CID_n_CFG_PLAIN_FORMAT_16;
> > +                     val |= CAMSS_CSID_CID_n_CFG_PLAIN_ALIGNMENT_LSB;
> > +             }
> > +
> > +             writel_relaxed(val, csid->base + CAMSS_CSID_CID_n_CFG(cid));
> > +
> > +             if (tg->enabled) {
> > +                     val = CAMSS_CSID_TG_CTRL_ENABLE;
> > +                     writel_relaxed(val, csid->base + CAMSS_CSID_TG_CTRL);
> > +             }
> > +     } else {
> > +             if (tg->enabled) {
> > +                     val = CAMSS_CSID_TG_CTRL_DISABLE;
> > +                     writel_relaxed(val, csid->base + CAMSS_CSID_TG_CTRL);
> > +             }
> > +     }
> > +}
> > +
> > +static int csid_configure_testgen_pattern(struct csid_device *csid, s32 val)
> > +{
> > +     s32 regval = val - 1;
> > +
> > +     if (regval > 0 || regval <= CSID_PAYLOAD_MODE_MAX_SUPPORTED_4_7)
> > +             csid->testgen.mode = regval;
> > +
> > +     return 0;
> > +}
> > +
> > +static u32 csid_hw_version(struct csid_device *csid)
> > +{
> > +     u32 hw_version = readl_relaxed(csid->base + CAMSS_CSID_HW_VERSION);
> > +
> > +     dev_dbg(csid->camss->dev, "CSID HW Version = 0x%08x\n", hw_version);
> > +
> > +     return hw_version;
> > +}
> > +
> > +/*
> > + * isr - CSID module interrupt service routine
> > + * @irq: Interrupt line
> > + * @dev: CSID device
> > + *
> > + * Return IRQ_HANDLED on success
> > + */
> > +static irqreturn_t csid_isr(int irq, void *dev)
> > +{
> > +     struct csid_device *csid = dev;
> > +     u32 value;
> > +
> > +     value = readl_relaxed(csid->base + CAMSS_CSID_IRQ_STATUS);
> > +     writel_relaxed(value, csid->base + CAMSS_CSID_IRQ_CLEAR_CMD);
> > +
> > +     if ((value >> 11) & 0x1)
> > +             complete(&csid->reset_complete);
> > +
> > +     return IRQ_HANDLED;
> > +}
> > +
> > +/*
> > + * csid_reset - Trigger reset on CSID module and wait to complete
> > + * @csid: CSID device
> > + *
> > + * Return 0 on success or a negative error code otherwise
> > + */
> > +static int csid_reset(struct csid_device *csid)
> > +{
> > +     unsigned long time;
> > +
> > +     reinit_completion(&csid->reset_complete);
> > +
> > +     writel_relaxed(0x7fff, csid->base + CAMSS_CSID_RST_CMD);
> > +
> > +     time = wait_for_completion_timeout(&csid->reset_complete,
> > +             msecs_to_jiffies(CSID_RESET_TIMEOUT_MS));
> > +     if (!time) {
> > +             dev_err(csid->camss->dev, "CSID reset timeout\n");
> > +             return -EIO;
> > +     }
> > +
> > +     return 0;
> > +}
> > +
> > +static u32 csid_src_pad_code(struct csid_device *csid, u32 sink_code,
> > +                          unsigned int match_format_idx, u32 match_code)
> > +{
> > +     switch (sink_code) {
> > +     case MEDIA_BUS_FMT_SBGGR10_1X10:
> > +     {
> > +             u32 src_code[] = {
> > +                     MEDIA_BUS_FMT_SBGGR10_1X10,
> > +                     MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE,
> > +             };
> > +
> > +             return csid_find_code(src_code, ARRAY_SIZE(src_code),
> > +                                   match_format_idx, match_code);
> > +     }
> > +     case MEDIA_BUS_FMT_Y10_1X10:
> > +     {
> > +             u32 src_code[] = {
> > +                     MEDIA_BUS_FMT_Y10_1X10,
> > +                     MEDIA_BUS_FMT_Y10_2X8_PADHI_LE,
> > +             };
> > +
> > +             return csid_find_code(src_code, ARRAY_SIZE(src_code),
> > +                                   match_format_idx, match_code);
> > +     }
> > +     default:
> > +             if (match_format_idx > 0)
> > +                     return 0;
> > +
> > +             return sink_code;
> > +     }
> > +}
> > +
> > +static void csid_subdev_init(struct csid_device *csid)
> > +{
> > +     csid->formats = csid_formats;
> > +     csid->nformats = ARRAY_SIZE(csid_formats);
> > +     csid->testgen.modes = csid_testgen_modes;
> > +     csid->testgen.nmodes = CSID_PAYLOAD_MODE_MAX_SUPPORTED_4_7;
> > +}
> > +
> > +const struct csid_hw_ops csid_ops_4_7 = {
> > +     .configure_stream = csid_configure_stream,
> > +     .configure_testgen_pattern = csid_configure_testgen_pattern,
> > +     .hw_version = csid_hw_version,
> > +     .isr = csid_isr,
> > +     .reset = csid_reset,
> > +     .src_pad_code = csid_src_pad_code,
> > +     .subdev_init = csid_subdev_init,
> > +};
> > diff --git a/drivers/media/platform/qcom/camss/camss-csid.c b/drivers/media/platform/qcom/camss/camss-csid.c
> > index be3fe76f3dc3..601bd810f2b0 100644
> > --- a/drivers/media/platform/qcom/camss/camss-csid.c
> > +++ b/drivers/media/platform/qcom/camss/camss-csid.c
> > @@ -26,405 +26,35 @@
> >
> >   #define MSM_CSID_NAME "msm_csid"
> >
> > -#define CAMSS_CSID_HW_VERSION                0x0
> > -#define CAMSS_CSID_CORE_CTRL_0               0x004
> > -#define CAMSS_CSID_CORE_CTRL_1               0x008
> > -#define CAMSS_CSID_RST_CMD(v)                ((v) == CAMSS_8x16 ? 0x00c : 0x010)
> > -#define CAMSS_CSID_CID_LUT_VC_n(v, n)        \
> > -                     (((v) == CAMSS_8x16 ? 0x010 : 0x014) + 0x4 * (n))
> > -#define CAMSS_CSID_CID_n_CFG(v, n)   \
> > -                     (((v) == CAMSS_8x16 ? 0x020 : 0x024) + 0x4 * (n))
> > -#define CAMSS_CSID_CID_n_CFG_ISPIF_EN        BIT(0)
> > -#define CAMSS_CSID_CID_n_CFG_RDI_EN  BIT(1)
> > -#define CAMSS_CSID_CID_n_CFG_DECODE_FORMAT_SHIFT     4
> > -#define CAMSS_CSID_CID_n_CFG_PLAIN_FORMAT_8          (0 << 8)
> > -#define CAMSS_CSID_CID_n_CFG_PLAIN_FORMAT_16         (1 << 8)
> > -#define CAMSS_CSID_CID_n_CFG_PLAIN_ALIGNMENT_LSB     (0 << 9)
> > -#define CAMSS_CSID_CID_n_CFG_PLAIN_ALIGNMENT_MSB     (1 << 9)
> > -#define CAMSS_CSID_CID_n_CFG_RDI_MODE_RAW_DUMP               (0 << 10)
> > -#define CAMSS_CSID_CID_n_CFG_RDI_MODE_PLAIN_PACKING  (1 << 10)
> > -#define CAMSS_CSID_IRQ_CLEAR_CMD(v)  ((v) == CAMSS_8x16 ? 0x060 : 0x064)
> > -#define CAMSS_CSID_IRQ_MASK(v)               ((v) == CAMSS_8x16 ? 0x064 : 0x068)
> > -#define CAMSS_CSID_IRQ_STATUS(v)     ((v) == CAMSS_8x16 ? 0x068 : 0x06c)
> > -#define CAMSS_CSID_TG_CTRL(v)                ((v) == CAMSS_8x16 ? 0x0a0 : 0x0a8)
> > -#define CAMSS_CSID_TG_CTRL_DISABLE   0xa06436
> > -#define CAMSS_CSID_TG_CTRL_ENABLE    0xa06437
> > -#define CAMSS_CSID_TG_VC_CFG(v)              ((v) == CAMSS_8x16 ? 0x0a4 : 0x0ac)
> > -#define CAMSS_CSID_TG_VC_CFG_H_BLANKING              0x3ff
> > -#define CAMSS_CSID_TG_VC_CFG_V_BLANKING              0x7f
> > -#define CAMSS_CSID_TG_DT_n_CGG_0(v, n)       \
> > -                     (((v) == CAMSS_8x16 ? 0x0ac : 0x0b4) + 0xc * (n))
> > -#define CAMSS_CSID_TG_DT_n_CGG_1(v, n)       \
> > -                     (((v) == CAMSS_8x16 ? 0x0b0 : 0x0b8) + 0xc * (n))
> > -#define CAMSS_CSID_TG_DT_n_CGG_2(v, n)       \
> > -                     (((v) == CAMSS_8x16 ? 0x0b4 : 0x0bc) + 0xc * (n))
> > -
> > -#define DATA_TYPE_EMBEDDED_DATA_8BIT 0x12
> > -#define DATA_TYPE_YUV422_8BIT                0x1e
> > -#define DATA_TYPE_RAW_6BIT           0x28
> > -#define DATA_TYPE_RAW_8BIT           0x2a
> > -#define DATA_TYPE_RAW_10BIT          0x2b
> > -#define DATA_TYPE_RAW_12BIT          0x2c
> > -#define DATA_TYPE_RAW_14BIT          0x2d
> > -
> > -#define DECODE_FORMAT_UNCOMPRESSED_6_BIT     0x0
> > -#define DECODE_FORMAT_UNCOMPRESSED_8_BIT     0x1
> > -#define DECODE_FORMAT_UNCOMPRESSED_10_BIT    0x2
> > -#define DECODE_FORMAT_UNCOMPRESSED_12_BIT    0x3
> > -#define DECODE_FORMAT_UNCOMPRESSED_14_BIT    0x8
> > -
> > -#define CSID_RESET_TIMEOUT_MS 500
> > -
> > -struct csid_format {
> > -     u32 code;
> > -     u8 data_type;
> > -     u8 decode_format;
> > -     u8 bpp;
> > -     u8 spp; /* bus samples per pixel */
> > -};
> > -
> > -static const struct csid_format csid_formats_8x16[] = {
> > -     {
> > -             MEDIA_BUS_FMT_UYVY8_2X8,
> > -             DATA_TYPE_YUV422_8BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> > -             8,
> > -             2,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_VYUY8_2X8,
> > -             DATA_TYPE_YUV422_8BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> > -             8,
> > -             2,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_YUYV8_2X8,
> > -             DATA_TYPE_YUV422_8BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> > -             8,
> > -             2,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_YVYU8_2X8,
> > -             DATA_TYPE_YUV422_8BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> > -             8,
> > -             2,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_SBGGR8_1X8,
> > -             DATA_TYPE_RAW_8BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> > -             8,
> > -             1,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_SGBRG8_1X8,
> > -             DATA_TYPE_RAW_8BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> > -             8,
> > -             1,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_SGRBG8_1X8,
> > -             DATA_TYPE_RAW_8BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> > -             8,
> > -             1,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_SRGGB8_1X8,
> > -             DATA_TYPE_RAW_8BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> > -             8,
> > -             1,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_SBGGR10_1X10,
> > -             DATA_TYPE_RAW_10BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> > -             10,
> > -             1,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_SGBRG10_1X10,
> > -             DATA_TYPE_RAW_10BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> > -             10,
> > -             1,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_SGRBG10_1X10,
> > -             DATA_TYPE_RAW_10BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> > -             10,
> > -             1,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_SRGGB10_1X10,
> > -             DATA_TYPE_RAW_10BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> > -             10,
> > -             1,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_SBGGR12_1X12,
> > -             DATA_TYPE_RAW_12BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_12_BIT,
> > -             12,
> > -             1,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_SGBRG12_1X12,
> > -             DATA_TYPE_RAW_12BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_12_BIT,
> > -             12,
> > -             1,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_SGRBG12_1X12,
> > -             DATA_TYPE_RAW_12BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_12_BIT,
> > -             12,
> > -             1,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_SRGGB12_1X12,
> > -             DATA_TYPE_RAW_12BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_12_BIT,
> > -             12,
> > -             1,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_Y10_1X10,
> > -             DATA_TYPE_RAW_10BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> > -             10,
> > -             1,
> > -     },
> > -};
> > -
> > -static const struct csid_format csid_formats_8x96[] = {
> > -     {
> > -             MEDIA_BUS_FMT_UYVY8_2X8,
> > -             DATA_TYPE_YUV422_8BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> > -             8,
> > -             2,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_VYUY8_2X8,
> > -             DATA_TYPE_YUV422_8BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> > -             8,
> > -             2,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_YUYV8_2X8,
> > -             DATA_TYPE_YUV422_8BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> > -             8,
> > -             2,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_YVYU8_2X8,
> > -             DATA_TYPE_YUV422_8BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> > -             8,
> > -             2,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_SBGGR8_1X8,
> > -             DATA_TYPE_RAW_8BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> > -             8,
> > -             1,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_SGBRG8_1X8,
> > -             DATA_TYPE_RAW_8BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> > -             8,
> > -             1,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_SGRBG8_1X8,
> > -             DATA_TYPE_RAW_8BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> > -             8,
> > -             1,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_SRGGB8_1X8,
> > -             DATA_TYPE_RAW_8BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_8_BIT,
> > -             8,
> > -             1,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_SBGGR10_1X10,
> > -             DATA_TYPE_RAW_10BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> > -             10,
> > -             1,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_SGBRG10_1X10,
> > -             DATA_TYPE_RAW_10BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> > -             10,
> > -             1,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_SGRBG10_1X10,
> > -             DATA_TYPE_RAW_10BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> > -             10,
> > -             1,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_SRGGB10_1X10,
> > -             DATA_TYPE_RAW_10BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> > -             10,
> > -             1,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_SBGGR12_1X12,
> > -             DATA_TYPE_RAW_12BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_12_BIT,
> > -             12,
> > -             1,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_SGBRG12_1X12,
> > -             DATA_TYPE_RAW_12BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_12_BIT,
> > -             12,
> > -             1,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_SGRBG12_1X12,
> > -             DATA_TYPE_RAW_12BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_12_BIT,
> > -             12,
> > -             1,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_SRGGB12_1X12,
> > -             DATA_TYPE_RAW_12BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_12_BIT,
> > -             12,
> > -             1,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_SBGGR14_1X14,
> > -             DATA_TYPE_RAW_14BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_14_BIT,
> > -             14,
> > -             1,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_SGBRG14_1X14,
> > -             DATA_TYPE_RAW_14BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_14_BIT,
> > -             14,
> > -             1,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_SGRBG14_1X14,
> > -             DATA_TYPE_RAW_14BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_14_BIT,
> > -             14,
> > -             1,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_SRGGB14_1X14,
> > -             DATA_TYPE_RAW_14BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_14_BIT,
> > -             14,
> > -             1,
> > -     },
> > -     {
> > -             MEDIA_BUS_FMT_Y10_1X10,
> > -             DATA_TYPE_RAW_10BIT,
> > -             DECODE_FORMAT_UNCOMPRESSED_10_BIT,
> > -             10,
> > -             1,
> > -     },
> > -};
> >
> > -static u32 csid_find_code(u32 *code, unsigned int n_code,
> > -                       unsigned int index, u32 req_code)
> > +u32 csid_find_code(u32 *codes, unsigned int ncodes,
> > +                unsigned int match_format_idx, u32 match_code)
> >   {
> >       int i;
> >
> > -     if (!req_code && (index >= n_code))
> > +     if (!match_code && (match_format_idx >= ncodes))
> >               return 0;
> >
> > -     for (i = 0; i < n_code; i++)
> > -             if (req_code) {
> > -                     if (req_code == code[i])
> > -                             return req_code;
> > +     for (i = 0; i < ncodes; i++)
> > +             if (match_code) {
> > +                     if (codes[i] == match_code)
> > +                             return match_code;
> >               } else {
> > -                     if (i == index)
> > -                             return code[i];
> > -             }
> > -
> > -     return code[0];
> > -}
> > -
> > -static u32 csid_src_pad_code(struct csid_device *csid, u32 sink_code,
> > -                          unsigned int index, u32 src_req_code)
> > -{
> > -     if (csid->camss->version == CAMSS_8x16) {
> > -             if (index > 0)
> > -                     return 0;
> > -
> > -             return sink_code;
> > -     } else if (csid->camss->version == CAMSS_8x96 ||
> > -                csid->camss->version == CAMSS_660) {
> > -             switch (sink_code) {
> > -             case MEDIA_BUS_FMT_SBGGR10_1X10:
> > -             {
> > -                     u32 src_code[] = {
> > -                             MEDIA_BUS_FMT_SBGGR10_1X10,
> > -                             MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE,
> > -                     };
> > -
> > -                     return csid_find_code(src_code, ARRAY_SIZE(src_code),
> > -                                           index, src_req_code);
> > -             }
> > -             case MEDIA_BUS_FMT_Y10_1X10:
> > -             {
> > -                     u32 src_code[] = {
> > -                             MEDIA_BUS_FMT_Y10_1X10,
> > -                             MEDIA_BUS_FMT_Y10_2X8_PADHI_LE,
> > -                     };
> > -
> > -                     return csid_find_code(src_code, ARRAY_SIZE(src_code),
> > -                                           index, src_req_code);
> > +                     if (i == match_format_idx)
> > +                             return codes[i];
> >               }
> > -             default:
> > -                     if (index > 0)
> > -                             return 0;
> >
> > -                     return sink_code;
> > -             }
> > -     } else {
> > -             return 0;
> > -     }
> > +     return codes[0];
> >   }
> >
> > -static const struct csid_format *csid_get_fmt_entry(
> > +const struct csid_format *csid_get_fmt_entry(
> >                                       const struct csid_format *formats,
> > -                                     unsigned int nformat,
> > +                                     unsigned int nformats,
> >                                       u32 code)
> >   {
> >       unsigned int i;
> >
> > -     for (i = 0; i < nformat; i++)
> > +     for (i = 0; i < nformats; i++)
> >               if (code == formats[i].code)
> >                       return &formats[i];
> >
> > @@ -433,28 +63,6 @@ static const struct csid_format *csid_get_fmt_entry(
> >       return &formats[0];
> >   }
> >
> > -/*
> > - * csid_isr - CSID module interrupt handler
> > - * @irq: Interrupt line
> > - * @dev: CSID device
> > - *
> > - * Return IRQ_HANDLED on success
> > - */
> > -static irqreturn_t csid_isr(int irq, void *dev)
> > -{
> > -     struct csid_device *csid = dev;
> > -     enum camss_version ver = csid->camss->version;
> > -     u32 value;
> > -
> > -     value = readl_relaxed(csid->base + CAMSS_CSID_IRQ_STATUS(ver));
> > -     writel_relaxed(value, csid->base + CAMSS_CSID_IRQ_CLEAR_CMD(ver));
> > -
> > -     if ((value >> 11) & 0x1)
> > -             complete(&csid->reset_complete);
> > -
> > -     return IRQ_HANDLED;
> > -}
> > -
> >   /*
> >    * csid_set_clock_rates - Calculate and set clock rates on CSID module
> >    * @csiphy: CSID device
> > @@ -521,31 +129,6 @@ static int csid_set_clock_rates(struct csid_device *csid)
> >       return 0;
> >   }
> >
> > -/*
> > - * csid_reset - Trigger reset on CSID module and wait to complete
> > - * @csid: CSID device
> > - *
> > - * Return 0 on success or a negative error code otherwise
> > - */
> > -static int csid_reset(struct csid_device *csid)
> > -{
> > -     unsigned long time;
> > -
> > -     reinit_completion(&csid->reset_complete);
> > -
> > -     writel_relaxed(0x7fff, csid->base +
> > -                    CAMSS_CSID_RST_CMD(csid->camss->version));
> > -
> > -     time = wait_for_completion_timeout(&csid->reset_complete,
> > -             msecs_to_jiffies(CSID_RESET_TIMEOUT_MS));
> > -     if (!time) {
> > -             dev_err(csid->camss->dev, "CSID reset timeout\n");
> > -             return -EIO;
> > -     }
> > -
> > -     return 0;
> > -}
> > -
> >   /*
> >    * csid_set_power - Power on/off CSID module
> >    * @sd: CSID V4L2 subdevice
> > @@ -560,8 +143,6 @@ static int csid_set_power(struct v4l2_subdev *sd, int on)
> >       int ret;
> >
> >       if (on) {
> > -             u32 hw_version;
> > -
> >               ret = pm_runtime_get_sync(dev);
> >               if (ret < 0) {
> >                       pm_runtime_put_sync(dev);
> > @@ -590,7 +171,7 @@ static int csid_set_power(struct v4l2_subdev *sd, int on)
> >
> >               enable_irq(csid->irq);
> >
> > -             ret = csid_reset(csid);
> > +             ret = csid->ops->reset(csid);
> >               if (ret < 0) {
> >                       disable_irq(csid->irq);
> >                       camss_disable_clocks(csid->nclocks, csid->clock);
> > @@ -599,8 +180,7 @@ static int csid_set_power(struct v4l2_subdev *sd, int on)
> >                       return ret;
> >               }
> >
> > -             hw_version = readl_relaxed(csid->base + CAMSS_CSID_HW_VERSION);
> > -             dev_dbg(dev, "CSID HW Version = 0x%08x\n", hw_version);
> > +             csid->ops->hw_version(csid);
> >       } else {
> >               disable_irq(csid->irq);
> >               camss_disable_clocks(csid->nclocks, csid->clock);
> > @@ -623,16 +203,9 @@ static int csid_set_power(struct v4l2_subdev *sd, int on)
> >   static int csid_set_stream(struct v4l2_subdev *sd, int enable)
> >   {
> >       struct csid_device *csid = v4l2_get_subdevdata(sd);
> > -     struct csid_testgen_config *tg = &csid->testgen;
> > -     enum camss_version ver = csid->camss->version;
> > -     u32 val;
> > +     int ret;
> >
> >       if (enable) {
> > -             u8 vc = 0; /* Virtual Channel 0 */
> > -             u8 cid = vc * 4; /* id of Virtual Channel and Data Type set */
> > -             u8 dt, dt_shift, df;
> > -             int ret;
> > -
> >               ret = v4l2_ctrl_handler_setup(&csid->ctrls);
> >               if (ret < 0) {
> >                       dev_err(csid->camss->dev,
> > @@ -640,116 +213,13 @@ static int csid_set_stream(struct v4l2_subdev *sd, int enable)
> >                       return ret;
> >               }
> >
> > -             if (!tg->enabled &&
> > +             if (!csid->testgen.enabled &&
> >                   !media_entity_remote_pad(&csid->pads[MSM_CSID_PAD_SINK]))
> >                       return -ENOLINK;
> > -
> > -             if (tg->enabled) {
> > -                     /* Config Test Generator */
> > -                     struct v4l2_mbus_framefmt *f =
> > -                                     &csid->fmt[MSM_CSID_PAD_SRC];
> > -                     const struct csid_format *format = csid_get_fmt_entry(
> > -                                     csid->formats, csid->nformats, f->code);
> > -                     u32 num_bytes_per_line =
> > -                             f->width * format->bpp * format->spp / 8;
> > -                     u32 num_lines = f->height;
> > -
> > -                     /* 31:24 V blank, 23:13 H blank, 3:2 num of active DT */
> > -                     /* 1:0 VC */
> > -                     val = ((CAMSS_CSID_TG_VC_CFG_V_BLANKING & 0xff) << 24) |
> > -                           ((CAMSS_CSID_TG_VC_CFG_H_BLANKING & 0x7ff) << 13);
> > -                     writel_relaxed(val, csid->base +
> > -                                    CAMSS_CSID_TG_VC_CFG(ver));
> > -
> > -                     /* 28:16 bytes per lines, 12:0 num of lines */
> > -                     val = ((num_bytes_per_line & 0x1fff) << 16) |
> > -                           (num_lines & 0x1fff);
> > -                     writel_relaxed(val, csid->base +
> > -                                    CAMSS_CSID_TG_DT_n_CGG_0(ver, 0));
> > -
> > -                     dt = format->data_type;
> > -
> > -                     /* 5:0 data type */
> > -                     val = dt;
> > -                     writel_relaxed(val, csid->base +
> > -                                    CAMSS_CSID_TG_DT_n_CGG_1(ver, 0));
> > -
> > -                     /* 2:0 output test pattern */
> > -                     val = tg->payload_mode;
> > -                     writel_relaxed(val, csid->base +
> > -                                    CAMSS_CSID_TG_DT_n_CGG_2(ver, 0));
> > -
> > -                     df = format->decode_format;
> > -             } else {
> > -                     struct v4l2_mbus_framefmt *f =
> > -                                     &csid->fmt[MSM_CSID_PAD_SINK];
> > -                     const struct csid_format *format = csid_get_fmt_entry(
> > -                                     csid->formats, csid->nformats, f->code);
> > -                     struct csid_phy_config *phy = &csid->phy;
> > -
> > -                     val = phy->lane_cnt - 1;
> > -                     val |= phy->lane_assign << 4;
> > -
> > -                     writel_relaxed(val,
> > -                                    csid->base + CAMSS_CSID_CORE_CTRL_0);
> > -
> > -                     val = phy->csiphy_id << 17;
> > -                     val |= 0x9;
> > -
> > -                     writel_relaxed(val,
> > -                                    csid->base + CAMSS_CSID_CORE_CTRL_1);
> > -
> > -                     dt = format->data_type;
> > -                     df = format->decode_format;
> > -             }
> > -
> > -             /* Config LUT */
> > -
> > -             dt_shift = (cid % 4) * 8;
> > -
> > -             val = readl_relaxed(csid->base +
> > -                                 CAMSS_CSID_CID_LUT_VC_n(ver, vc));
> > -             val &= ~(0xff << dt_shift);
> > -             val |= dt << dt_shift;
> > -             writel_relaxed(val, csid->base +
> > -                            CAMSS_CSID_CID_LUT_VC_n(ver, vc));
> > -
> > -             val = CAMSS_CSID_CID_n_CFG_ISPIF_EN;
> > -             val |= CAMSS_CSID_CID_n_CFG_RDI_EN;
> > -             val |= df << CAMSS_CSID_CID_n_CFG_DECODE_FORMAT_SHIFT;
> > -             val |= CAMSS_CSID_CID_n_CFG_RDI_MODE_RAW_DUMP;
> > -
> > -             if (csid->camss->version == CAMSS_8x96 ||
> > -                 csid->camss->version == CAMSS_660) {
> > -                     u32 sink_code = csid->fmt[MSM_CSID_PAD_SINK].code;
> > -                     u32 src_code = csid->fmt[MSM_CSID_PAD_SRC].code;
> > -
> > -                     if ((sink_code == MEDIA_BUS_FMT_SBGGR10_1X10 &&
> > -                          src_code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) ||
> > -                         (sink_code == MEDIA_BUS_FMT_Y10_1X10 &&
> > -                          src_code == MEDIA_BUS_FMT_Y10_2X8_PADHI_LE)) {
> > -                             val |= CAMSS_CSID_CID_n_CFG_RDI_MODE_PLAIN_PACKING;
> > -                             val |= CAMSS_CSID_CID_n_CFG_PLAIN_FORMAT_16;
> > -                             val |= CAMSS_CSID_CID_n_CFG_PLAIN_ALIGNMENT_LSB;
> > -                     }
> > -             }
> > -
> > -             writel_relaxed(val, csid->base +
> > -                            CAMSS_CSID_CID_n_CFG(ver, cid));
> > -
> > -             if (tg->enabled) {
> > -                     val = CAMSS_CSID_TG_CTRL_ENABLE;
> > -                     writel_relaxed(val, csid->base +
> > -                                    CAMSS_CSID_TG_CTRL(ver));
> > -             }
> > -     } else {
> > -             if (tg->enabled) {
> > -                     val = CAMSS_CSID_TG_CTRL_DISABLE;
> > -                     writel_relaxed(val, csid->base +
> > -                                    CAMSS_CSID_TG_CTRL(ver));
> > -             }
> >       }
> >
> > +     csid->ops->configure_stream(csid, enable);
> > +
> >       return 0;
> >   }
> >
> > @@ -818,7 +288,7 @@ static void csid_try_format(struct csid_device *csid,
> >
> >                       *fmt = *__csid_get_format(csid, cfg,
> >                                                     MSM_CSID_PAD_SINK, which);
> > -                     fmt->code = csid_src_pad_code(csid, fmt->code, 0, code);
> > +                     fmt->code = csid->ops->src_pad_code(csid, fmt->code, 0, code);
> >               } else {
> >                       /* Test generator is enabled, set format on source */
> >                       /* pad to allow test generator usage */
> > @@ -868,7 +338,7 @@ static int csid_enum_mbus_code(struct v4l2_subdev *sd,
> >                                                    MSM_CSID_PAD_SINK,
> >                                                    code->which);
> >
> > -                     code->code = csid_src_pad_code(csid, sink_fmt->code,
> > +                     code->code = csid->ops->src_pad_code(csid, sink_fmt->code,
> >                                                      code->index, 0);
> >                       if (!code->code)
> >                               return -EINVAL;
> > @@ -1004,15 +474,6 @@ static int csid_init_formats(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
> >       return csid_set_format(sd, fh ? fh->pad : NULL, &format);
> >   }
> >
> > -static const char * const csid_test_pattern_menu[] = {
> > -     "Disabled",
> > -     "Incrementing",
> > -     "Alternating 0x55/0xAA",
> > -     "All Zeros 0x00",
> > -     "All Ones 0xFF",
> > -     "Pseudo-random Data",
> > -};
> > -
> >   /*
> >    * csid_set_test_pattern - Set test generator's pattern mode
> >    * @csid: CSID device
> > @@ -1030,25 +491,7 @@ static int csid_set_test_pattern(struct csid_device *csid, s32 value)
> >
> >       tg->enabled = !!value;
> >
> > -     switch (value) {
> > -     case 1:
> > -             tg->payload_mode = CSID_PAYLOAD_MODE_INCREMENTING;
> > -             break;
> > -     case 2:
> > -             tg->payload_mode = CSID_PAYLOAD_MODE_ALTERNATING_55_AA;
> > -             break;
> > -     case 3:
> > -             tg->payload_mode = CSID_PAYLOAD_MODE_ALL_ZEROES;
> > -             break;
> > -     case 4:
> > -             tg->payload_mode = CSID_PAYLOAD_MODE_ALL_ONES;
> > -             break;
> > -     case 5:
> > -             tg->payload_mode = CSID_PAYLOAD_MODE_RANDOM;
> > -             break;
> > -     }
> > -
> > -     return 0;
> > +     return csid->ops->configure_testgen_pattern(csid, value);
> >   }
> >
> >   /*
> > @@ -1097,17 +540,14 @@ int msm_csid_subdev_init(struct camss *camss, struct csid_device *csid,
> >       csid->id = id;
> >
> >       if (camss->version == CAMSS_8x16) {
> > -             csid->formats = csid_formats_8x16;
> > -             csid->nformats =
> > -                             ARRAY_SIZE(csid_formats_8x16);
> > +             csid->ops = &csid_ops_4_1;
> >       } else if (camss->version == CAMSS_8x96 ||
> >                  camss->version == CAMSS_660) {
> > -             csid->formats = csid_formats_8x96;
> > -             csid->nformats =
> > -                             ARRAY_SIZE(csid_formats_8x96);
> > +             csid->ops = &csid_ops_4_7;
> >       } else {
> >               return -EINVAL;
> >       }
> > +     csid->ops->subdev_init(csid);
> >
> >       /* Memory */
> >
> > @@ -1130,7 +570,7 @@ int msm_csid_subdev_init(struct camss *camss, struct csid_device *csid,
> >       csid->irq = r->start;
> >       snprintf(csid->irq_name, sizeof(csid->irq_name), "%s_%s%d",
> >                dev_name(dev), MSM_CSID_NAME, csid->id);
> > -     ret = devm_request_irq(dev, csid->irq, csid_isr,
> > +     ret = devm_request_irq(dev, csid->irq, csid->ops->isr,
> >               IRQF_TRIGGER_RISING, csid->irq_name, csid);
> >       if (ret < 0) {
> >               dev_err(dev, "request_irq failed: %d\n", ret);
> > @@ -1341,8 +781,8 @@ int msm_csid_register_entity(struct csid_device *csid,
> >
> >       csid->testgen_mode = v4l2_ctrl_new_std_menu_items(&csid->ctrls,
> >                               &csid_ctrl_ops, V4L2_CID_TEST_PATTERN,
> > -                             ARRAY_SIZE(csid_test_pattern_menu) - 1, 0, 0,
> > -                             csid_test_pattern_menu);
> > +                             csid->testgen.nmodes, 0, 0,
> > +                             csid->testgen.modes);
> >
> >       if (csid->ctrls.error) {
> >               dev_err(dev, "Failed to init ctrl: %d\n", csid->ctrls.error);
> > diff --git a/drivers/media/platform/qcom/camss/camss-csid.h b/drivers/media/platform/qcom/camss/camss-csid.h
> > index 02fc34ee8a41..d40194e2bed3 100644
> > --- a/drivers/media/platform/qcom/camss/camss-csid.h
> > +++ b/drivers/media/platform/qcom/camss/camss-csid.h
> > @@ -11,6 +11,7 @@
> >   #define QC_MSM_CAMSS_CSID_H
> >
> >   #include <linux/clk.h>
> > +#include <linux/interrupt.h>
> >   #include <media/media-entity.h>
> >   #include <media/v4l2-ctrls.h>
> >   #include <media/v4l2-device.h>
> > @@ -70,19 +71,50 @@
> >   #define PLAIN_FORMAT_PLAIN16        0x1 /* supports DPCM, UNCOMPRESSED_10/16_BIT */
> >   #define PLAIN_FORMAT_PLAIN32        0x2 /* supports UNCOMPRESSED_20_BIT */
> >
> > +#define CSID_RESET_TIMEOUT_MS 500
> >
> > -enum csid_payload_mode {
> > +
> > +enum csid_testgen_mode {
> >       CSID_PAYLOAD_MODE_INCREMENTING = 0,
> >       CSID_PAYLOAD_MODE_ALTERNATING_55_AA = 1,
> >       CSID_PAYLOAD_MODE_ALL_ZEROES = 2,
> >       CSID_PAYLOAD_MODE_ALL_ONES = 3,
> >       CSID_PAYLOAD_MODE_RANDOM = 4,
> >       CSID_PAYLOAD_MODE_USER_SPECIFIED = 5,
> > +     CSID_PAYLOAD_MODE_MAX_SUPPORTED_4_1 = 5,
> > +     CSID_PAYLOAD_MODE_MAX_SUPPORTED_4_7 = 5,
> > +     CSID_PAYLOAD_MODE_COMPLEX_PATTERN = 6,
> > +     CSID_PAYLOAD_MODE_COLOR_BOX = 7,
> > +     CSID_PAYLOAD_MODE_COLOR_BARS = 8,
> > +     CSID_PAYLOAD_MODE_MAX_SUPPORTED_170 = 8,
> > +};
> > +
> > +static const char * const csid_testgen_modes[] = {
> > +     "Disabled",
> > +     "Incrementing",
> > +     "Alternating 0x55/0xAA",
> > +     "All Zeros 0x00",
> > +     "All Ones 0xFF",
> > +     "Pseudo-random Data",
> > +     "User Specified",
> > +     "Complex pattern",
> > +     "Color box",
> > +     "Color bars",
> > +};
> > +
> > +struct csid_format {
> > +     u32 code;
> > +     u8 data_type;
> > +     u8 decode_format;
> > +     u8 bpp;
> > +     u8 spp; /* bus samples per pixel */
> >   };
> >
> >   struct csid_testgen_config {
> > +     enum csid_testgen_mode mode;
> > +     const char * const*modes;
> > +     u8 nmodes;
> >       u8 enabled;
> > -     enum csid_payload_mode payload_mode;
> >   };
> >
> >   struct csid_phy_config {
> > @@ -91,6 +123,65 @@ struct csid_phy_config {
> >       u32 lane_assign;
> >   };
> >
> > +struct csid_device;
> > +
> > +struct csid_hw_ops {
> > +     /*
> > +      * configure_stream - Configures and starts CSID input stream
> > +      * @csid: CSID device
> > +      */
> > +     void (*configure_stream)(struct csid_device *csid, u8 enable);
> > +
> > +     /*
> > +      * configure_testgen_pattern - Validates and configures output pattern mode
> > +      * of test pattern generator
> > +      * @csid: CSID device
> > +      */
> > +     int (*configure_testgen_pattern)(struct csid_device *csid, s32 val);
> > +
> > +     /*
> > +      * hw_version - Read hardware version register from hardware
> > +      * @csid: CSID device
> > +      */
> > +     u32 (*hw_version)(struct csid_device *csid);
> > +
> > +     /*
> > +      * isr - CSID module interrupt service routine
> > +      * @irq: Interrupt line
> > +      * @dev: CSID device
> > +      *
> > +      * Return IRQ_HANDLED on success
> > +      */
> > +     irqreturn_t (*isr)(int irq, void *dev);
> > +
> > +     /*
> > +      * reset - Trigger reset on CSID module and wait to complete
> > +      * @csid: CSID device
> > +      *
> > +      * Return 0 on success or a negative error code otherwise
> > +      */
> > +     int (*reset)(struct csid_device *csid);
> > +
> > +     /*
> > +      * src_pad_code - Pick an output/src format based on the input/sink format
> > +      * @csid: CSID device
> > +      * @sink_code: The sink format of the input
> > +      * @match_format_idx: Request preferred index, as defined by subdevice csid_format.
> > +      *      Set @match_code to 0 if used.
> > +      * @match_code: Request preferred code, set @match_format_idx to 0 if used
> > +      *
> > +      * Return 0 on failure or src format code otherwise
> > +      */
> > +     u32 (*src_pad_code)(struct csid_device *csid, u32 sink_code,
> > +                         unsigned int match_format_idx, u32 match_code);
> > +
> > +     /*
> > +      * subdev_init - Initialize CSID device according for hardware revision
> > +      * @csid: CSID device
> > +      */
> > +     void (*subdev_init)(struct csid_device *csid);
> > +};
> > +
> >   struct csid_device {
> >       struct camss *camss;
> >       u8 id;
> > @@ -110,10 +201,37 @@ struct csid_device {
> >       struct v4l2_ctrl *testgen_mode;
> >       const struct csid_format *formats;
> >       unsigned int nformats;
> > +     const struct csid_hw_ops *ops;
> >   };
> >
> >   struct resources;
> >
> > +
> > +/*
> > + * csid_find_code - Find a format code in an array using array index or format code
> > + * @codes: Array of format codes
> > + * @ncodes: Length of @code array
> > + * @req_format_idx: Request preferred index, as defined by subdevice csid_format.
> > + *   Set @match_code to 0 if used.
> > + * @match_code: Request preferred code, set @req_format_idx to 0 if used
> > + *
> > + * Return 0 on failure or format code otherwise
> > + */
> > +u32 csid_find_code(u32 *codes, unsigned int ncode,
> > +                unsigned int match_format_idx, u32 match_code);
> > +
> > +/*
> > + * csid_get_fmt_entry - Find csid_format entry with matching format code
> > + * @formats: Array of format csid_format entries
> > + * @nformats: Length of @nformats array
> > + * @code: Desired format code
> > + *
> > + * Return formats[0] on failure to find code
> > + */
> > +const struct csid_format *csid_get_fmt_entry(const struct csid_format *formats,
> > +                                          unsigned int nformats,
> > +                                          u32 code);
> > +
> >   int msm_csid_subdev_init(struct camss *camss, struct csid_device *csid,
> >                        const struct resources *res, u8 id);
> >
> > @@ -124,4 +242,8 @@ void msm_csid_unregister_entity(struct csid_device *csid);
> >
> >   void msm_csid_get_csid_id(struct media_entity *entity, u8 *id);
> >
> > +
> > +extern const struct csid_hw_ops csid_ops_4_1;
> > +extern const struct csid_hw_ops csid_ops_4_7;
> > +
> >   #endif /* QC_MSM_CAMSS_CSID_H */
> >
diff mbox series

Patch

diff --git a/drivers/media/platform/qcom/camss/Makefile b/drivers/media/platform/qcom/camss/Makefile
index 052c4f405fa3..cff388b653ba 100644
--- a/drivers/media/platform/qcom/camss/Makefile
+++ b/drivers/media/platform/qcom/camss/Makefile
@@ -4,6 +4,8 @@ 
 qcom-camss-objs += \
 		camss.o \
 		camss-csid.o \
+		camss-csid-4-1.o \
+		camss-csid-4-7.o \
 		camss-csiphy-2ph-1-0.o \
 		camss-csiphy-3ph-1-0.o \
 		camss-csiphy.o \
diff --git a/drivers/media/platform/qcom/camss/camss-csid-4-1.c b/drivers/media/platform/qcom/camss/camss-csid-4-1.c
new file mode 100644
index 000000000000..84b415137555
--- /dev/null
+++ b/drivers/media/platform/qcom/camss/camss-csid-4-1.c
@@ -0,0 +1,338 @@ 
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * camss-csid-4-1.c
+ *
+ * Qualcomm MSM Camera Subsystem - CSID (CSI Decoder) Module
+ *
+ * Copyright (C) 2020 Linaro Ltd.
+ */
+ #include <linux/completion.h>
+ #include <linux/interrupt.h>
+ #include <linux/io.h>
+ #include <linux/kernel.h>
+ #include <linux/of.h>
+
+#include "camss-csid.h"
+#include "camss.h"
+
+#define CAMSS_CSID_HW_VERSION		0x0
+#define CAMSS_CSID_CORE_CTRL_0		0x004
+#define CAMSS_CSID_CORE_CTRL_1		0x008
+#define CAMSS_CSID_RST_CMD		0x00c
+#define CAMSS_CSID_CID_LUT_VC_n(n)	(0x010 + 0x4 * (n))
+#define CAMSS_CSID_CID_n_CFG(n)		(0x020 + 0x4 * (n))
+#define CAMSS_CSID_CID_n_CFG_ISPIF_EN	BIT(0)
+#define CAMSS_CSID_CID_n_CFG_RDI_EN	BIT(1)
+#define CAMSS_CSID_CID_n_CFG_DECODE_FORMAT_SHIFT	4
+#define CAMSS_CSID_CID_n_CFG_PLAIN_FORMAT_8		(0 << 8)
+#define CAMSS_CSID_CID_n_CFG_PLAIN_FORMAT_16		(1 << 8)
+#define CAMSS_CSID_CID_n_CFG_PLAIN_ALIGNMENT_LSB	(0 << 9)
+#define CAMSS_CSID_CID_n_CFG_PLAIN_ALIGNMENT_MSB	(1 << 9)
+#define CAMSS_CSID_CID_n_CFG_RDI_MODE_RAW_DUMP		(0 << 10)
+#define CAMSS_CSID_CID_n_CFG_RDI_MODE_PLAIN_PACKING	(1 << 10)
+#define CAMSS_CSID_IRQ_CLEAR_CMD	0x060
+#define CAMSS_CSID_IRQ_MASK		0x064
+#define CAMSS_CSID_IRQ_STATUS		0x068
+#define CAMSS_CSID_TG_CTRL		0x0a0
+#define CAMSS_CSID_TG_CTRL_DISABLE	0xa06436
+#define CAMSS_CSID_TG_CTRL_ENABLE	0xa06437
+#define CAMSS_CSID_TG_VC_CFG		0x0a4
+#define CAMSS_CSID_TG_VC_CFG_H_BLANKING		0x3ff
+#define CAMSS_CSID_TG_VC_CFG_V_BLANKING		0x7f
+#define CAMSS_CSID_TG_DT_n_CGG_0(n)	(0x0ac + 0xc * (n))
+#define CAMSS_CSID_TG_DT_n_CGG_1(n)	(0x0b0 + 0xc * (n))
+#define CAMSS_CSID_TG_DT_n_CGG_2(n)	(0x0b4 + 0xc * (n))
+
+
+enum csid_testgen_pattern {
+	TESTGEN_PATTERN_INCREMENTING = 0,
+	TESTGEN_PATTERN_ALTERNATING_55_AA = 1,
+	TESTGEN_PATTERN_ALL_ZEROES = 2,
+	TESTGEN_PATTERN_ALL_ONES = 3,
+	TESTGEN_PATTERN_RANDOM = 4,
+	TESTGEN_PATTERN_USER_SPECIFIED = 5,
+};
+
+static const struct csid_format csid_formats[] = {
+	{
+		MEDIA_BUS_FMT_UYVY8_2X8,
+		DATA_TYPE_YUV422_8BIT,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		2,
+	},
+	{
+		MEDIA_BUS_FMT_VYUY8_2X8,
+		DATA_TYPE_YUV422_8BIT,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		2,
+	},
+	{
+		MEDIA_BUS_FMT_YUYV8_2X8,
+		DATA_TYPE_YUV422_8BIT,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		2,
+	},
+	{
+		MEDIA_BUS_FMT_YVYU8_2X8,
+		DATA_TYPE_YUV422_8BIT,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		2,
+	},
+	{
+		MEDIA_BUS_FMT_SBGGR8_1X8,
+		DATA_TYPE_RAW_8BIT,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGBRG8_1X8,
+		DATA_TYPE_RAW_8BIT,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGRBG8_1X8,
+		DATA_TYPE_RAW_8BIT,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SRGGB8_1X8,
+		DATA_TYPE_RAW_8BIT,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SBGGR10_1X10,
+		DATA_TYPE_RAW_10BIT,
+		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
+		10,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGBRG10_1X10,
+		DATA_TYPE_RAW_10BIT,
+		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
+		10,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGRBG10_1X10,
+		DATA_TYPE_RAW_10BIT,
+		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
+		10,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SRGGB10_1X10,
+		DATA_TYPE_RAW_10BIT,
+		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
+		10,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SBGGR12_1X12,
+		DATA_TYPE_RAW_12BIT,
+		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
+		12,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGBRG12_1X12,
+		DATA_TYPE_RAW_12BIT,
+		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
+		12,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGRBG12_1X12,
+		DATA_TYPE_RAW_12BIT,
+		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
+		12,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SRGGB12_1X12,
+		DATA_TYPE_RAW_12BIT,
+		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
+		12,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_Y10_1X10,
+		DATA_TYPE_RAW_10BIT,
+		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
+		10,
+		1,
+	},
+};
+
+static void csid_configure_stream(struct csid_device *csid, u8 enable)
+{
+	struct csid_testgen_config *tg = &csid->testgen;
+	u32 val;
+
+	if (enable) {
+		struct v4l2_mbus_framefmt *input_format;
+		const struct csid_format *format;
+		u8 vc = 0; /* Virtual Channel 0 */
+		u8 cid = vc * 4; /* id of Virtual Channel and Data Type set */
+		u8 dt_shift;
+
+		if (tg->enabled) {
+			/* Config Test Generator */
+			u32 num_lines, num_bytes_per_line;
+
+			input_format = &csid->fmt[MSM_CSID_PAD_SRC];
+			format = csid_get_fmt_entry(csid->formats, csid->nformats,
+						    input_format->code);
+			num_bytes_per_line = input_format->width * format->bpp * format->spp / 8;
+			num_lines = input_format->height;
+
+			/* 31:24 V blank, 23:13 H blank, 3:2 num of active DT */
+			/* 1:0 VC */
+			val = ((CAMSS_CSID_TG_VC_CFG_V_BLANKING & 0xff) << 24) |
+				  ((CAMSS_CSID_TG_VC_CFG_H_BLANKING & 0x7ff) << 13);
+			writel_relaxed(val, csid->base + CAMSS_CSID_TG_VC_CFG);
+
+			/* 28:16 bytes per lines, 12:0 num of lines */
+			val = ((num_bytes_per_line & 0x1fff) << 16) |
+				  (num_lines & 0x1fff);
+			writel_relaxed(val, csid->base + CAMSS_CSID_TG_DT_n_CGG_0(0));
+
+			/* 5:0 data type */
+			val = format->data_type;
+			writel_relaxed(val, csid->base + CAMSS_CSID_TG_DT_n_CGG_1(0));
+
+			/* 2:0 output test pattern */
+			val = tg->mode;
+			writel_relaxed(val, csid->base + CAMSS_CSID_TG_DT_n_CGG_2(0));
+		} else {
+			struct csid_phy_config *phy = &csid->phy;
+
+			input_format = &csid->fmt[MSM_CSID_PAD_SINK];
+			format = csid_get_fmt_entry(csid->formats, csid->nformats,
+						    input_format->code);
+
+			val = phy->lane_cnt - 1;
+			val |= phy->lane_assign << 4;
+
+			writel_relaxed(val, csid->base + CAMSS_CSID_CORE_CTRL_0);
+
+			val = phy->csiphy_id << 17;
+			val |= 0x9;
+
+			writel_relaxed(val, csid->base + CAMSS_CSID_CORE_CTRL_1);
+		}
+
+		/* Config LUT */
+
+		dt_shift = (cid % 4) * 8;
+		val = readl_relaxed(csid->base + CAMSS_CSID_CID_LUT_VC_n(vc));
+		val &= ~(0xff << dt_shift);
+		val |= format->data_type << dt_shift;
+		writel_relaxed(val, csid->base + CAMSS_CSID_CID_LUT_VC_n(vc));
+
+		val = CAMSS_CSID_CID_n_CFG_ISPIF_EN;
+		val |= CAMSS_CSID_CID_n_CFG_RDI_EN;
+		val |= format->decode_format << CAMSS_CSID_CID_n_CFG_DECODE_FORMAT_SHIFT;
+		val |= CAMSS_CSID_CID_n_CFG_RDI_MODE_RAW_DUMP;
+		writel_relaxed(val, csid->base + CAMSS_CSID_CID_n_CFG(cid));
+
+		if (tg->enabled) {
+			val = CAMSS_CSID_TG_CTRL_ENABLE;
+			writel_relaxed(val, csid->base + CAMSS_CSID_TG_CTRL);
+		}
+	} else {
+		if (tg->enabled) {
+			val = CAMSS_CSID_TG_CTRL_DISABLE;
+			writel_relaxed(val, csid->base + CAMSS_CSID_TG_CTRL);
+		}
+	}
+}
+
+static int csid_configure_testgen_pattern(struct csid_device *csid, s32 val)
+{
+	s32 regval = val - 1;
+
+	if (regval > 0 || regval <= CSID_PAYLOAD_MODE_MAX_SUPPORTED_4_1)
+		csid->testgen.mode = regval;
+
+	return 0;
+}
+
+static u32 csid_hw_version(struct csid_device *csid)
+{
+	u32 hw_version = readl_relaxed(csid->base + CAMSS_CSID_HW_VERSION);
+
+	dev_dbg(csid->camss->dev, "CSID HW Version = 0x%08x\n", hw_version);
+
+	return hw_version;
+}
+
+static irqreturn_t csid_isr(int irq, void *dev)
+{
+	struct csid_device *csid = dev;
+	u32 value;
+
+	value = readl_relaxed(csid->base + CAMSS_CSID_IRQ_STATUS);
+	writel_relaxed(value, csid->base + CAMSS_CSID_IRQ_CLEAR_CMD);
+
+	if ((value >> 11) & 0x1)
+		complete(&csid->reset_complete);
+
+	return IRQ_HANDLED;
+}
+
+static int csid_reset(struct csid_device *csid)
+{
+	unsigned long time;
+
+	reinit_completion(&csid->reset_complete);
+
+	writel_relaxed(0x7fff, csid->base + CAMSS_CSID_RST_CMD);
+
+	time = wait_for_completion_timeout(&csid->reset_complete,
+		msecs_to_jiffies(CSID_RESET_TIMEOUT_MS));
+	if (!time) {
+		dev_err(csid->camss->dev, "CSID reset timeout\n");
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static u32 csid_src_pad_code(struct csid_device *csid, u32 sink_code,
+			     unsigned int match_format_idx, u32 match_code)
+{
+	if (match_format_idx > 0)
+		return 0;
+
+	return sink_code;
+}
+
+static void csid_subdev_init(struct csid_device *csid)
+{
+	csid->formats = csid_formats;
+	csid->nformats = ARRAY_SIZE(csid_formats);
+	csid->testgen.modes = csid_testgen_modes;
+	csid->testgen.nmodes = CSID_PAYLOAD_MODE_MAX_SUPPORTED_4_1;
+}
+
+const struct csid_hw_ops csid_ops_4_1 = {
+	.configure_stream = csid_configure_stream,
+	.configure_testgen_pattern = csid_configure_testgen_pattern,
+	.hw_version = csid_hw_version,
+	.isr = csid_isr,
+	.reset = csid_reset,
+	.src_pad_code = csid_src_pad_code,
+	.subdev_init = csid_subdev_init,
+};
diff --git a/drivers/media/platform/qcom/camss/camss-csid-4-7.c b/drivers/media/platform/qcom/camss/camss-csid-4-7.c
new file mode 100644
index 000000000000..16a69b140f4e
--- /dev/null
+++ b/drivers/media/platform/qcom/camss/camss-csid-4-7.c
@@ -0,0 +1,406 @@ 
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * camss-csid-4-7.c
+ *
+ * Qualcomm MSM Camera Subsystem - CSID (CSI Decoder) Module
+ *
+ * Copyright (C) 2020 Linaro Ltd.
+ */
+#include <linux/completion.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/of.h>
+
+#include "camss-csid.h"
+#include "camss.h"
+
+#define CAMSS_CSID_HW_VERSION		0x0
+#define CAMSS_CSID_CORE_CTRL_0		0x004
+#define CAMSS_CSID_CORE_CTRL_1		0x008
+#define CAMSS_CSID_RST_CMD		0x010
+#define CAMSS_CSID_CID_LUT_VC_n(n)	(0x014 + 0x4 * (n))
+#define CAMSS_CSID_CID_n_CFG(n)		(0x024 + 0x4 * (n))
+#define CAMSS_CSID_CID_n_CFG_ISPIF_EN	BIT(0)
+#define CAMSS_CSID_CID_n_CFG_RDI_EN	BIT(1)
+#define CAMSS_CSID_CID_n_CFG_DECODE_FORMAT_SHIFT	4
+#define CAMSS_CSID_CID_n_CFG_PLAIN_FORMAT_8		(0 << 8)
+#define CAMSS_CSID_CID_n_CFG_PLAIN_FORMAT_16		(1 << 8)
+#define CAMSS_CSID_CID_n_CFG_PLAIN_ALIGNMENT_LSB	(0 << 9)
+#define CAMSS_CSID_CID_n_CFG_PLAIN_ALIGNMENT_MSB	(1 << 9)
+#define CAMSS_CSID_CID_n_CFG_RDI_MODE_RAW_DUMP		(0 << 10)
+#define CAMSS_CSID_CID_n_CFG_RDI_MODE_PLAIN_PACKING	(1 << 10)
+#define CAMSS_CSID_IRQ_CLEAR_CMD	0x064
+#define CAMSS_CSID_IRQ_MASK		0x068
+#define CAMSS_CSID_IRQ_STATUS		0x06c
+#define CAMSS_CSID_TG_CTRL		0x0a8
+#define CAMSS_CSID_TG_CTRL_DISABLE	0xa06436
+#define CAMSS_CSID_TG_CTRL_ENABLE	0xa06437
+#define CAMSS_CSID_TG_VC_CFG		0x0ac
+#define CAMSS_CSID_TG_VC_CFG_H_BLANKING		0x3ff
+#define CAMSS_CSID_TG_VC_CFG_V_BLANKING		0x7f
+#define CAMSS_CSID_TG_DT_n_CGG_0(n)	(0x0b4 + 0xc * (n))
+#define CAMSS_CSID_TG_DT_n_CGG_1(n)	(0x0b8 + 0xc * (n))
+#define CAMSS_CSID_TG_DT_n_CGG_2(n)	(0x0bc + 0xc * (n))
+
+
+static const struct csid_format csid_formats[] = {
+	{
+		MEDIA_BUS_FMT_UYVY8_2X8,
+		DATA_TYPE_YUV422_8BIT,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		2,
+	},
+	{
+		MEDIA_BUS_FMT_VYUY8_2X8,
+		DATA_TYPE_YUV422_8BIT,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		2,
+	},
+	{
+		MEDIA_BUS_FMT_YUYV8_2X8,
+		DATA_TYPE_YUV422_8BIT,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		2,
+	},
+	{
+		MEDIA_BUS_FMT_YVYU8_2X8,
+		DATA_TYPE_YUV422_8BIT,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		2,
+	},
+	{
+		MEDIA_BUS_FMT_SBGGR8_1X8,
+		DATA_TYPE_RAW_8BIT,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGBRG8_1X8,
+		DATA_TYPE_RAW_8BIT,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGRBG8_1X8,
+		DATA_TYPE_RAW_8BIT,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SRGGB8_1X8,
+		DATA_TYPE_RAW_8BIT,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SBGGR10_1X10,
+		DATA_TYPE_RAW_10BIT,
+		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
+		10,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGBRG10_1X10,
+		DATA_TYPE_RAW_10BIT,
+		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
+		10,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGRBG10_1X10,
+		DATA_TYPE_RAW_10BIT,
+		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
+		10,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SRGGB10_1X10,
+		DATA_TYPE_RAW_10BIT,
+		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
+		10,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SBGGR12_1X12,
+		DATA_TYPE_RAW_12BIT,
+		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
+		12,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGBRG12_1X12,
+		DATA_TYPE_RAW_12BIT,
+		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
+		12,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGRBG12_1X12,
+		DATA_TYPE_RAW_12BIT,
+		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
+		12,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SRGGB12_1X12,
+		DATA_TYPE_RAW_12BIT,
+		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
+		12,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SBGGR14_1X14,
+		DATA_TYPE_RAW_14BIT,
+		DECODE_FORMAT_UNCOMPRESSED_14_BIT,
+		14,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGBRG14_1X14,
+		DATA_TYPE_RAW_14BIT,
+		DECODE_FORMAT_UNCOMPRESSED_14_BIT,
+		14,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGRBG14_1X14,
+		DATA_TYPE_RAW_14BIT,
+		DECODE_FORMAT_UNCOMPRESSED_14_BIT,
+		14,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SRGGB14_1X14,
+		DATA_TYPE_RAW_14BIT,
+		DECODE_FORMAT_UNCOMPRESSED_14_BIT,
+		14,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_Y10_1X10,
+		DATA_TYPE_RAW_10BIT,
+		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
+		10,
+		1,
+	},
+};
+
+static void csid_configure_stream(struct csid_device *csid, u8 enable)
+{
+	struct csid_testgen_config *tg = &csid->testgen;
+	u32 sink_code = csid->fmt[MSM_CSID_PAD_SINK].code;
+	u32 src_code = csid->fmt[MSM_CSID_PAD_SRC].code;
+	u32 val;
+
+	if (enable) {
+		struct v4l2_mbus_framefmt *input_format;
+		const struct csid_format *format;
+		u8 vc = 0; /* Virtual Channel 0 */
+		u8 cid = vc * 4; /* id of Virtual Channel and Data Type set */
+		u8 dt_shift;
+
+		if (tg->enabled) {
+			/* Config Test Generator */
+			u32 num_bytes_per_line, num_lines;
+
+			input_format = &csid->fmt[MSM_CSID_PAD_SRC];
+			format = csid_get_fmt_entry(csid->formats, csid->nformats,
+						    input_format->code);
+			num_bytes_per_line = input_format->width * format->bpp * format->spp / 8;
+			num_lines = input_format->height;
+
+			/* 31:24 V blank, 23:13 H blank, 3:2 num of active DT */
+			/* 1:0 VC */
+			val = ((CAMSS_CSID_TG_VC_CFG_V_BLANKING & 0xff) << 24) |
+				  ((CAMSS_CSID_TG_VC_CFG_H_BLANKING & 0x7ff) << 13);
+			writel_relaxed(val, csid->base + CAMSS_CSID_TG_VC_CFG);
+
+			/* 28:16 bytes per lines, 12:0 num of lines */
+			val = ((num_bytes_per_line & 0x1fff) << 16) |
+				  (num_lines & 0x1fff);
+			writel_relaxed(val, csid->base + CAMSS_CSID_TG_DT_n_CGG_0(0));
+
+			/* 5:0 data type */
+			val = format->data_type;
+			writel_relaxed(val, csid->base + CAMSS_CSID_TG_DT_n_CGG_1(0));
+
+			/* 2:0 output test pattern */
+			val = tg->mode;
+			writel_relaxed(val, csid->base + CAMSS_CSID_TG_DT_n_CGG_2(0));
+		} else {
+			struct csid_phy_config *phy = &csid->phy;
+
+			input_format = &csid->fmt[MSM_CSID_PAD_SINK];
+			format = csid_get_fmt_entry(csid->formats, csid->nformats,
+						    input_format->code);
+
+			val = phy->lane_cnt - 1;
+			val |= phy->lane_assign << 4;
+
+			writel_relaxed(val, csid->base + CAMSS_CSID_CORE_CTRL_0);
+
+			val = phy->csiphy_id << 17;
+			val |= 0x9;
+
+			writel_relaxed(val, csid->base + CAMSS_CSID_CORE_CTRL_1);
+		}
+
+		/* Config LUT */
+
+		dt_shift = (cid % 4) * 8;
+
+		val = readl_relaxed(csid->base + CAMSS_CSID_CID_LUT_VC_n(vc));
+		val &= ~(0xff << dt_shift);
+		val |= format->data_type << dt_shift;
+		writel_relaxed(val, csid->base + CAMSS_CSID_CID_LUT_VC_n(vc));
+
+		val = CAMSS_CSID_CID_n_CFG_ISPIF_EN;
+		val |= CAMSS_CSID_CID_n_CFG_RDI_EN;
+		val |= format->decode_format << CAMSS_CSID_CID_n_CFG_DECODE_FORMAT_SHIFT;
+		val |= CAMSS_CSID_CID_n_CFG_RDI_MODE_RAW_DUMP;
+
+		if ((sink_code == MEDIA_BUS_FMT_SBGGR10_1X10 &&
+		     src_code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) ||
+		    (sink_code == MEDIA_BUS_FMT_Y10_1X10 &&
+		     src_code == MEDIA_BUS_FMT_Y10_2X8_PADHI_LE)) {
+			val |= CAMSS_CSID_CID_n_CFG_RDI_MODE_PLAIN_PACKING;
+			val |= CAMSS_CSID_CID_n_CFG_PLAIN_FORMAT_16;
+			val |= CAMSS_CSID_CID_n_CFG_PLAIN_ALIGNMENT_LSB;
+		}
+
+		writel_relaxed(val, csid->base + CAMSS_CSID_CID_n_CFG(cid));
+
+		if (tg->enabled) {
+			val = CAMSS_CSID_TG_CTRL_ENABLE;
+			writel_relaxed(val, csid->base + CAMSS_CSID_TG_CTRL);
+		}
+	} else {
+		if (tg->enabled) {
+			val = CAMSS_CSID_TG_CTRL_DISABLE;
+			writel_relaxed(val, csid->base + CAMSS_CSID_TG_CTRL);
+		}
+	}
+}
+
+static int csid_configure_testgen_pattern(struct csid_device *csid, s32 val)
+{
+	s32 regval = val - 1;
+
+	if (regval > 0 || regval <= CSID_PAYLOAD_MODE_MAX_SUPPORTED_4_7)
+		csid->testgen.mode = regval;
+
+	return 0;
+}
+
+static u32 csid_hw_version(struct csid_device *csid)
+{
+	u32 hw_version = readl_relaxed(csid->base + CAMSS_CSID_HW_VERSION);
+
+	dev_dbg(csid->camss->dev, "CSID HW Version = 0x%08x\n", hw_version);
+
+	return hw_version;
+}
+
+/*
+ * isr - CSID module interrupt service routine
+ * @irq: Interrupt line
+ * @dev: CSID device
+ *
+ * Return IRQ_HANDLED on success
+ */
+static irqreturn_t csid_isr(int irq, void *dev)
+{
+	struct csid_device *csid = dev;
+	u32 value;
+
+	value = readl_relaxed(csid->base + CAMSS_CSID_IRQ_STATUS);
+	writel_relaxed(value, csid->base + CAMSS_CSID_IRQ_CLEAR_CMD);
+
+	if ((value >> 11) & 0x1)
+		complete(&csid->reset_complete);
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * csid_reset - Trigger reset on CSID module and wait to complete
+ * @csid: CSID device
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+static int csid_reset(struct csid_device *csid)
+{
+	unsigned long time;
+
+	reinit_completion(&csid->reset_complete);
+
+	writel_relaxed(0x7fff, csid->base + CAMSS_CSID_RST_CMD);
+
+	time = wait_for_completion_timeout(&csid->reset_complete,
+		msecs_to_jiffies(CSID_RESET_TIMEOUT_MS));
+	if (!time) {
+		dev_err(csid->camss->dev, "CSID reset timeout\n");
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static u32 csid_src_pad_code(struct csid_device *csid, u32 sink_code,
+			     unsigned int match_format_idx, u32 match_code)
+{
+	switch (sink_code) {
+	case MEDIA_BUS_FMT_SBGGR10_1X10:
+	{
+		u32 src_code[] = {
+			MEDIA_BUS_FMT_SBGGR10_1X10,
+			MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE,
+		};
+
+		return csid_find_code(src_code, ARRAY_SIZE(src_code),
+				      match_format_idx, match_code);
+	}
+	case MEDIA_BUS_FMT_Y10_1X10:
+	{
+		u32 src_code[] = {
+			MEDIA_BUS_FMT_Y10_1X10,
+			MEDIA_BUS_FMT_Y10_2X8_PADHI_LE,
+		};
+
+		return csid_find_code(src_code, ARRAY_SIZE(src_code),
+				      match_format_idx, match_code);
+	}
+	default:
+		if (match_format_idx > 0)
+			return 0;
+
+		return sink_code;
+	}
+}
+
+static void csid_subdev_init(struct csid_device *csid)
+{
+	csid->formats = csid_formats;
+	csid->nformats = ARRAY_SIZE(csid_formats);
+	csid->testgen.modes = csid_testgen_modes;
+	csid->testgen.nmodes = CSID_PAYLOAD_MODE_MAX_SUPPORTED_4_7;
+}
+
+const struct csid_hw_ops csid_ops_4_7 = {
+	.configure_stream = csid_configure_stream,
+	.configure_testgen_pattern = csid_configure_testgen_pattern,
+	.hw_version = csid_hw_version,
+	.isr = csid_isr,
+	.reset = csid_reset,
+	.src_pad_code = csid_src_pad_code,
+	.subdev_init = csid_subdev_init,
+};
diff --git a/drivers/media/platform/qcom/camss/camss-csid.c b/drivers/media/platform/qcom/camss/camss-csid.c
index be3fe76f3dc3..601bd810f2b0 100644
--- a/drivers/media/platform/qcom/camss/camss-csid.c
+++ b/drivers/media/platform/qcom/camss/camss-csid.c
@@ -26,405 +26,35 @@ 
 
 #define MSM_CSID_NAME "msm_csid"
 
-#define CAMSS_CSID_HW_VERSION		0x0
-#define CAMSS_CSID_CORE_CTRL_0		0x004
-#define CAMSS_CSID_CORE_CTRL_1		0x008
-#define CAMSS_CSID_RST_CMD(v)		((v) == CAMSS_8x16 ? 0x00c : 0x010)
-#define CAMSS_CSID_CID_LUT_VC_n(v, n)	\
-			(((v) == CAMSS_8x16 ? 0x010 : 0x014) + 0x4 * (n))
-#define CAMSS_CSID_CID_n_CFG(v, n)	\
-			(((v) == CAMSS_8x16 ? 0x020 : 0x024) + 0x4 * (n))
-#define CAMSS_CSID_CID_n_CFG_ISPIF_EN	BIT(0)
-#define CAMSS_CSID_CID_n_CFG_RDI_EN	BIT(1)
-#define CAMSS_CSID_CID_n_CFG_DECODE_FORMAT_SHIFT	4
-#define CAMSS_CSID_CID_n_CFG_PLAIN_FORMAT_8		(0 << 8)
-#define CAMSS_CSID_CID_n_CFG_PLAIN_FORMAT_16		(1 << 8)
-#define CAMSS_CSID_CID_n_CFG_PLAIN_ALIGNMENT_LSB	(0 << 9)
-#define CAMSS_CSID_CID_n_CFG_PLAIN_ALIGNMENT_MSB	(1 << 9)
-#define CAMSS_CSID_CID_n_CFG_RDI_MODE_RAW_DUMP		(0 << 10)
-#define CAMSS_CSID_CID_n_CFG_RDI_MODE_PLAIN_PACKING	(1 << 10)
-#define CAMSS_CSID_IRQ_CLEAR_CMD(v)	((v) == CAMSS_8x16 ? 0x060 : 0x064)
-#define CAMSS_CSID_IRQ_MASK(v)		((v) == CAMSS_8x16 ? 0x064 : 0x068)
-#define CAMSS_CSID_IRQ_STATUS(v)	((v) == CAMSS_8x16 ? 0x068 : 0x06c)
-#define CAMSS_CSID_TG_CTRL(v)		((v) == CAMSS_8x16 ? 0x0a0 : 0x0a8)
-#define CAMSS_CSID_TG_CTRL_DISABLE	0xa06436
-#define CAMSS_CSID_TG_CTRL_ENABLE	0xa06437
-#define CAMSS_CSID_TG_VC_CFG(v)		((v) == CAMSS_8x16 ? 0x0a4 : 0x0ac)
-#define CAMSS_CSID_TG_VC_CFG_H_BLANKING		0x3ff
-#define CAMSS_CSID_TG_VC_CFG_V_BLANKING		0x7f
-#define CAMSS_CSID_TG_DT_n_CGG_0(v, n)	\
-			(((v) == CAMSS_8x16 ? 0x0ac : 0x0b4) + 0xc * (n))
-#define CAMSS_CSID_TG_DT_n_CGG_1(v, n)	\
-			(((v) == CAMSS_8x16 ? 0x0b0 : 0x0b8) + 0xc * (n))
-#define CAMSS_CSID_TG_DT_n_CGG_2(v, n)	\
-			(((v) == CAMSS_8x16 ? 0x0b4 : 0x0bc) + 0xc * (n))
-
-#define DATA_TYPE_EMBEDDED_DATA_8BIT	0x12
-#define DATA_TYPE_YUV422_8BIT		0x1e
-#define DATA_TYPE_RAW_6BIT		0x28
-#define DATA_TYPE_RAW_8BIT		0x2a
-#define DATA_TYPE_RAW_10BIT		0x2b
-#define DATA_TYPE_RAW_12BIT		0x2c
-#define DATA_TYPE_RAW_14BIT		0x2d
-
-#define DECODE_FORMAT_UNCOMPRESSED_6_BIT	0x0
-#define DECODE_FORMAT_UNCOMPRESSED_8_BIT	0x1
-#define DECODE_FORMAT_UNCOMPRESSED_10_BIT	0x2
-#define DECODE_FORMAT_UNCOMPRESSED_12_BIT	0x3
-#define DECODE_FORMAT_UNCOMPRESSED_14_BIT	0x8
-
-#define CSID_RESET_TIMEOUT_MS 500
-
-struct csid_format {
-	u32 code;
-	u8 data_type;
-	u8 decode_format;
-	u8 bpp;
-	u8 spp; /* bus samples per pixel */
-};
-
-static const struct csid_format csid_formats_8x16[] = {
-	{
-		MEDIA_BUS_FMT_UYVY8_2X8,
-		DATA_TYPE_YUV422_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		2,
-	},
-	{
-		MEDIA_BUS_FMT_VYUY8_2X8,
-		DATA_TYPE_YUV422_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		2,
-	},
-	{
-		MEDIA_BUS_FMT_YUYV8_2X8,
-		DATA_TYPE_YUV422_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		2,
-	},
-	{
-		MEDIA_BUS_FMT_YVYU8_2X8,
-		DATA_TYPE_YUV422_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		2,
-	},
-	{
-		MEDIA_BUS_FMT_SBGGR8_1X8,
-		DATA_TYPE_RAW_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGBRG8_1X8,
-		DATA_TYPE_RAW_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGRBG8_1X8,
-		DATA_TYPE_RAW_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SRGGB8_1X8,
-		DATA_TYPE_RAW_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SBGGR10_1X10,
-		DATA_TYPE_RAW_10BIT,
-		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
-		10,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGBRG10_1X10,
-		DATA_TYPE_RAW_10BIT,
-		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
-		10,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGRBG10_1X10,
-		DATA_TYPE_RAW_10BIT,
-		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
-		10,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SRGGB10_1X10,
-		DATA_TYPE_RAW_10BIT,
-		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
-		10,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SBGGR12_1X12,
-		DATA_TYPE_RAW_12BIT,
-		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
-		12,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGBRG12_1X12,
-		DATA_TYPE_RAW_12BIT,
-		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
-		12,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGRBG12_1X12,
-		DATA_TYPE_RAW_12BIT,
-		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
-		12,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SRGGB12_1X12,
-		DATA_TYPE_RAW_12BIT,
-		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
-		12,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_Y10_1X10,
-		DATA_TYPE_RAW_10BIT,
-		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
-		10,
-		1,
-	},
-};
-
-static const struct csid_format csid_formats_8x96[] = {
-	{
-		MEDIA_BUS_FMT_UYVY8_2X8,
-		DATA_TYPE_YUV422_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		2,
-	},
-	{
-		MEDIA_BUS_FMT_VYUY8_2X8,
-		DATA_TYPE_YUV422_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		2,
-	},
-	{
-		MEDIA_BUS_FMT_YUYV8_2X8,
-		DATA_TYPE_YUV422_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		2,
-	},
-	{
-		MEDIA_BUS_FMT_YVYU8_2X8,
-		DATA_TYPE_YUV422_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		2,
-	},
-	{
-		MEDIA_BUS_FMT_SBGGR8_1X8,
-		DATA_TYPE_RAW_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGBRG8_1X8,
-		DATA_TYPE_RAW_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGRBG8_1X8,
-		DATA_TYPE_RAW_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SRGGB8_1X8,
-		DATA_TYPE_RAW_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SBGGR10_1X10,
-		DATA_TYPE_RAW_10BIT,
-		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
-		10,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGBRG10_1X10,
-		DATA_TYPE_RAW_10BIT,
-		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
-		10,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGRBG10_1X10,
-		DATA_TYPE_RAW_10BIT,
-		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
-		10,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SRGGB10_1X10,
-		DATA_TYPE_RAW_10BIT,
-		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
-		10,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SBGGR12_1X12,
-		DATA_TYPE_RAW_12BIT,
-		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
-		12,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGBRG12_1X12,
-		DATA_TYPE_RAW_12BIT,
-		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
-		12,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGRBG12_1X12,
-		DATA_TYPE_RAW_12BIT,
-		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
-		12,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SRGGB12_1X12,
-		DATA_TYPE_RAW_12BIT,
-		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
-		12,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SBGGR14_1X14,
-		DATA_TYPE_RAW_14BIT,
-		DECODE_FORMAT_UNCOMPRESSED_14_BIT,
-		14,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGBRG14_1X14,
-		DATA_TYPE_RAW_14BIT,
-		DECODE_FORMAT_UNCOMPRESSED_14_BIT,
-		14,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGRBG14_1X14,
-		DATA_TYPE_RAW_14BIT,
-		DECODE_FORMAT_UNCOMPRESSED_14_BIT,
-		14,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SRGGB14_1X14,
-		DATA_TYPE_RAW_14BIT,
-		DECODE_FORMAT_UNCOMPRESSED_14_BIT,
-		14,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_Y10_1X10,
-		DATA_TYPE_RAW_10BIT,
-		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
-		10,
-		1,
-	},
-};
 
-static u32 csid_find_code(u32 *code, unsigned int n_code,
-			  unsigned int index, u32 req_code)
+u32 csid_find_code(u32 *codes, unsigned int ncodes,
+		   unsigned int match_format_idx, u32 match_code)
 {
 	int i;
 
-	if (!req_code && (index >= n_code))
+	if (!match_code && (match_format_idx >= ncodes))
 		return 0;
 
-	for (i = 0; i < n_code; i++)
-		if (req_code) {
-			if (req_code == code[i])
-				return req_code;
+	for (i = 0; i < ncodes; i++)
+		if (match_code) {
+			if (codes[i] == match_code)
+				return match_code;
 		} else {
-			if (i == index)
-				return code[i];
-		}
-
-	return code[0];
-}
-
-static u32 csid_src_pad_code(struct csid_device *csid, u32 sink_code,
-			     unsigned int index, u32 src_req_code)
-{
-	if (csid->camss->version == CAMSS_8x16) {
-		if (index > 0)
-			return 0;
-
-		return sink_code;
-	} else if (csid->camss->version == CAMSS_8x96 ||
-		   csid->camss->version == CAMSS_660) {
-		switch (sink_code) {
-		case MEDIA_BUS_FMT_SBGGR10_1X10:
-		{
-			u32 src_code[] = {
-				MEDIA_BUS_FMT_SBGGR10_1X10,
-				MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE,
-			};
-
-			return csid_find_code(src_code, ARRAY_SIZE(src_code),
-					      index, src_req_code);
-		}
-		case MEDIA_BUS_FMT_Y10_1X10:
-		{
-			u32 src_code[] = {
-				MEDIA_BUS_FMT_Y10_1X10,
-				MEDIA_BUS_FMT_Y10_2X8_PADHI_LE,
-			};
-
-			return csid_find_code(src_code, ARRAY_SIZE(src_code),
-					      index, src_req_code);
+			if (i == match_format_idx)
+				return codes[i];
 		}
-		default:
-			if (index > 0)
-				return 0;
 
-			return sink_code;
-		}
-	} else {
-		return 0;
-	}
+	return codes[0];
 }
 
-static const struct csid_format *csid_get_fmt_entry(
+const struct csid_format *csid_get_fmt_entry(
 					const struct csid_format *formats,
-					unsigned int nformat,
+					unsigned int nformats,
 					u32 code)
 {
 	unsigned int i;
 
-	for (i = 0; i < nformat; i++)
+	for (i = 0; i < nformats; i++)
 		if (code == formats[i].code)
 			return &formats[i];
 
@@ -433,28 +63,6 @@  static const struct csid_format *csid_get_fmt_entry(
 	return &formats[0];
 }
 
-/*
- * csid_isr - CSID module interrupt handler
- * @irq: Interrupt line
- * @dev: CSID device
- *
- * Return IRQ_HANDLED on success
- */
-static irqreturn_t csid_isr(int irq, void *dev)
-{
-	struct csid_device *csid = dev;
-	enum camss_version ver = csid->camss->version;
-	u32 value;
-
-	value = readl_relaxed(csid->base + CAMSS_CSID_IRQ_STATUS(ver));
-	writel_relaxed(value, csid->base + CAMSS_CSID_IRQ_CLEAR_CMD(ver));
-
-	if ((value >> 11) & 0x1)
-		complete(&csid->reset_complete);
-
-	return IRQ_HANDLED;
-}
-
 /*
  * csid_set_clock_rates - Calculate and set clock rates on CSID module
  * @csiphy: CSID device
@@ -521,31 +129,6 @@  static int csid_set_clock_rates(struct csid_device *csid)
 	return 0;
 }
 
-/*
- * csid_reset - Trigger reset on CSID module and wait to complete
- * @csid: CSID device
- *
- * Return 0 on success or a negative error code otherwise
- */
-static int csid_reset(struct csid_device *csid)
-{
-	unsigned long time;
-
-	reinit_completion(&csid->reset_complete);
-
-	writel_relaxed(0x7fff, csid->base +
-		       CAMSS_CSID_RST_CMD(csid->camss->version));
-
-	time = wait_for_completion_timeout(&csid->reset_complete,
-		msecs_to_jiffies(CSID_RESET_TIMEOUT_MS));
-	if (!time) {
-		dev_err(csid->camss->dev, "CSID reset timeout\n");
-		return -EIO;
-	}
-
-	return 0;
-}
-
 /*
  * csid_set_power - Power on/off CSID module
  * @sd: CSID V4L2 subdevice
@@ -560,8 +143,6 @@  static int csid_set_power(struct v4l2_subdev *sd, int on)
 	int ret;
 
 	if (on) {
-		u32 hw_version;
-
 		ret = pm_runtime_get_sync(dev);
 		if (ret < 0) {
 			pm_runtime_put_sync(dev);
@@ -590,7 +171,7 @@  static int csid_set_power(struct v4l2_subdev *sd, int on)
 
 		enable_irq(csid->irq);
 
-		ret = csid_reset(csid);
+		ret = csid->ops->reset(csid);
 		if (ret < 0) {
 			disable_irq(csid->irq);
 			camss_disable_clocks(csid->nclocks, csid->clock);
@@ -599,8 +180,7 @@  static int csid_set_power(struct v4l2_subdev *sd, int on)
 			return ret;
 		}
 
-		hw_version = readl_relaxed(csid->base + CAMSS_CSID_HW_VERSION);
-		dev_dbg(dev, "CSID HW Version = 0x%08x\n", hw_version);
+		csid->ops->hw_version(csid);
 	} else {
 		disable_irq(csid->irq);
 		camss_disable_clocks(csid->nclocks, csid->clock);
@@ -623,16 +203,9 @@  static int csid_set_power(struct v4l2_subdev *sd, int on)
 static int csid_set_stream(struct v4l2_subdev *sd, int enable)
 {
 	struct csid_device *csid = v4l2_get_subdevdata(sd);
-	struct csid_testgen_config *tg = &csid->testgen;
-	enum camss_version ver = csid->camss->version;
-	u32 val;
+	int ret;
 
 	if (enable) {
-		u8 vc = 0; /* Virtual Channel 0 */
-		u8 cid = vc * 4; /* id of Virtual Channel and Data Type set */
-		u8 dt, dt_shift, df;
-		int ret;
-
 		ret = v4l2_ctrl_handler_setup(&csid->ctrls);
 		if (ret < 0) {
 			dev_err(csid->camss->dev,
@@ -640,116 +213,13 @@  static int csid_set_stream(struct v4l2_subdev *sd, int enable)
 			return ret;
 		}
 
-		if (!tg->enabled &&
+		if (!csid->testgen.enabled &&
 		    !media_entity_remote_pad(&csid->pads[MSM_CSID_PAD_SINK]))
 			return -ENOLINK;
-
-		if (tg->enabled) {
-			/* Config Test Generator */
-			struct v4l2_mbus_framefmt *f =
-					&csid->fmt[MSM_CSID_PAD_SRC];
-			const struct csid_format *format = csid_get_fmt_entry(
-					csid->formats, csid->nformats, f->code);
-			u32 num_bytes_per_line =
-				f->width * format->bpp * format->spp / 8;
-			u32 num_lines = f->height;
-
-			/* 31:24 V blank, 23:13 H blank, 3:2 num of active DT */
-			/* 1:0 VC */
-			val = ((CAMSS_CSID_TG_VC_CFG_V_BLANKING & 0xff) << 24) |
-			      ((CAMSS_CSID_TG_VC_CFG_H_BLANKING & 0x7ff) << 13);
-			writel_relaxed(val, csid->base +
-				       CAMSS_CSID_TG_VC_CFG(ver));
-
-			/* 28:16 bytes per lines, 12:0 num of lines */
-			val = ((num_bytes_per_line & 0x1fff) << 16) |
-			      (num_lines & 0x1fff);
-			writel_relaxed(val, csid->base +
-				       CAMSS_CSID_TG_DT_n_CGG_0(ver, 0));
-
-			dt = format->data_type;
-
-			/* 5:0 data type */
-			val = dt;
-			writel_relaxed(val, csid->base +
-				       CAMSS_CSID_TG_DT_n_CGG_1(ver, 0));
-
-			/* 2:0 output test pattern */
-			val = tg->payload_mode;
-			writel_relaxed(val, csid->base +
-				       CAMSS_CSID_TG_DT_n_CGG_2(ver, 0));
-
-			df = format->decode_format;
-		} else {
-			struct v4l2_mbus_framefmt *f =
-					&csid->fmt[MSM_CSID_PAD_SINK];
-			const struct csid_format *format = csid_get_fmt_entry(
-					csid->formats, csid->nformats, f->code);
-			struct csid_phy_config *phy = &csid->phy;
-
-			val = phy->lane_cnt - 1;
-			val |= phy->lane_assign << 4;
-
-			writel_relaxed(val,
-				       csid->base + CAMSS_CSID_CORE_CTRL_0);
-
-			val = phy->csiphy_id << 17;
-			val |= 0x9;
-
-			writel_relaxed(val,
-				       csid->base + CAMSS_CSID_CORE_CTRL_1);
-
-			dt = format->data_type;
-			df = format->decode_format;
-		}
-
-		/* Config LUT */
-
-		dt_shift = (cid % 4) * 8;
-
-		val = readl_relaxed(csid->base +
-				    CAMSS_CSID_CID_LUT_VC_n(ver, vc));
-		val &= ~(0xff << dt_shift);
-		val |= dt << dt_shift;
-		writel_relaxed(val, csid->base +
-			       CAMSS_CSID_CID_LUT_VC_n(ver, vc));
-
-		val = CAMSS_CSID_CID_n_CFG_ISPIF_EN;
-		val |= CAMSS_CSID_CID_n_CFG_RDI_EN;
-		val |= df << CAMSS_CSID_CID_n_CFG_DECODE_FORMAT_SHIFT;
-		val |= CAMSS_CSID_CID_n_CFG_RDI_MODE_RAW_DUMP;
-
-		if (csid->camss->version == CAMSS_8x96 ||
-		    csid->camss->version == CAMSS_660) {
-			u32 sink_code = csid->fmt[MSM_CSID_PAD_SINK].code;
-			u32 src_code = csid->fmt[MSM_CSID_PAD_SRC].code;
-
-			if ((sink_code == MEDIA_BUS_FMT_SBGGR10_1X10 &&
-			     src_code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) ||
-			    (sink_code == MEDIA_BUS_FMT_Y10_1X10 &&
-			     src_code == MEDIA_BUS_FMT_Y10_2X8_PADHI_LE)) {
-				val |= CAMSS_CSID_CID_n_CFG_RDI_MODE_PLAIN_PACKING;
-				val |= CAMSS_CSID_CID_n_CFG_PLAIN_FORMAT_16;
-				val |= CAMSS_CSID_CID_n_CFG_PLAIN_ALIGNMENT_LSB;
-			}
-		}
-
-		writel_relaxed(val, csid->base +
-			       CAMSS_CSID_CID_n_CFG(ver, cid));
-
-		if (tg->enabled) {
-			val = CAMSS_CSID_TG_CTRL_ENABLE;
-			writel_relaxed(val, csid->base +
-				       CAMSS_CSID_TG_CTRL(ver));
-		}
-	} else {
-		if (tg->enabled) {
-			val = CAMSS_CSID_TG_CTRL_DISABLE;
-			writel_relaxed(val, csid->base +
-				       CAMSS_CSID_TG_CTRL(ver));
-		}
 	}
 
+	csid->ops->configure_stream(csid, enable);
+
 	return 0;
 }
 
@@ -818,7 +288,7 @@  static void csid_try_format(struct csid_device *csid,
 
 			*fmt = *__csid_get_format(csid, cfg,
 						      MSM_CSID_PAD_SINK, which);
-			fmt->code = csid_src_pad_code(csid, fmt->code, 0, code);
+			fmt->code = csid->ops->src_pad_code(csid, fmt->code, 0, code);
 		} else {
 			/* Test generator is enabled, set format on source */
 			/* pad to allow test generator usage */
@@ -868,7 +338,7 @@  static int csid_enum_mbus_code(struct v4l2_subdev *sd,
 						     MSM_CSID_PAD_SINK,
 						     code->which);
 
-			code->code = csid_src_pad_code(csid, sink_fmt->code,
+			code->code = csid->ops->src_pad_code(csid, sink_fmt->code,
 						       code->index, 0);
 			if (!code->code)
 				return -EINVAL;
@@ -1004,15 +474,6 @@  static int csid_init_formats(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
 	return csid_set_format(sd, fh ? fh->pad : NULL, &format);
 }
 
-static const char * const csid_test_pattern_menu[] = {
-	"Disabled",
-	"Incrementing",
-	"Alternating 0x55/0xAA",
-	"All Zeros 0x00",
-	"All Ones 0xFF",
-	"Pseudo-random Data",
-};
-
 /*
  * csid_set_test_pattern - Set test generator's pattern mode
  * @csid: CSID device
@@ -1030,25 +491,7 @@  static int csid_set_test_pattern(struct csid_device *csid, s32 value)
 
 	tg->enabled = !!value;
 
-	switch (value) {
-	case 1:
-		tg->payload_mode = CSID_PAYLOAD_MODE_INCREMENTING;
-		break;
-	case 2:
-		tg->payload_mode = CSID_PAYLOAD_MODE_ALTERNATING_55_AA;
-		break;
-	case 3:
-		tg->payload_mode = CSID_PAYLOAD_MODE_ALL_ZEROES;
-		break;
-	case 4:
-		tg->payload_mode = CSID_PAYLOAD_MODE_ALL_ONES;
-		break;
-	case 5:
-		tg->payload_mode = CSID_PAYLOAD_MODE_RANDOM;
-		break;
-	}
-
-	return 0;
+	return csid->ops->configure_testgen_pattern(csid, value);
 }
 
 /*
@@ -1097,17 +540,14 @@  int msm_csid_subdev_init(struct camss *camss, struct csid_device *csid,
 	csid->id = id;
 
 	if (camss->version == CAMSS_8x16) {
-		csid->formats = csid_formats_8x16;
-		csid->nformats =
-				ARRAY_SIZE(csid_formats_8x16);
+		csid->ops = &csid_ops_4_1;
 	} else if (camss->version == CAMSS_8x96 ||
 		   camss->version == CAMSS_660) {
-		csid->formats = csid_formats_8x96;
-		csid->nformats =
-				ARRAY_SIZE(csid_formats_8x96);
+		csid->ops = &csid_ops_4_7;
 	} else {
 		return -EINVAL;
 	}
+	csid->ops->subdev_init(csid);
 
 	/* Memory */
 
@@ -1130,7 +570,7 @@  int msm_csid_subdev_init(struct camss *camss, struct csid_device *csid,
 	csid->irq = r->start;
 	snprintf(csid->irq_name, sizeof(csid->irq_name), "%s_%s%d",
 		 dev_name(dev), MSM_CSID_NAME, csid->id);
-	ret = devm_request_irq(dev, csid->irq, csid_isr,
+	ret = devm_request_irq(dev, csid->irq, csid->ops->isr,
 		IRQF_TRIGGER_RISING, csid->irq_name, csid);
 	if (ret < 0) {
 		dev_err(dev, "request_irq failed: %d\n", ret);
@@ -1341,8 +781,8 @@  int msm_csid_register_entity(struct csid_device *csid,
 
 	csid->testgen_mode = v4l2_ctrl_new_std_menu_items(&csid->ctrls,
 				&csid_ctrl_ops, V4L2_CID_TEST_PATTERN,
-				ARRAY_SIZE(csid_test_pattern_menu) - 1, 0, 0,
-				csid_test_pattern_menu);
+				csid->testgen.nmodes, 0, 0,
+				csid->testgen.modes);
 
 	if (csid->ctrls.error) {
 		dev_err(dev, "Failed to init ctrl: %d\n", csid->ctrls.error);
diff --git a/drivers/media/platform/qcom/camss/camss-csid.h b/drivers/media/platform/qcom/camss/camss-csid.h
index 02fc34ee8a41..d40194e2bed3 100644
--- a/drivers/media/platform/qcom/camss/camss-csid.h
+++ b/drivers/media/platform/qcom/camss/camss-csid.h
@@ -11,6 +11,7 @@ 
 #define QC_MSM_CAMSS_CSID_H
 
 #include <linux/clk.h>
+#include <linux/interrupt.h>
 #include <media/media-entity.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
@@ -70,19 +71,50 @@ 
 #define PLAIN_FORMAT_PLAIN16	0x1 /* supports DPCM, UNCOMPRESSED_10/16_BIT */
 #define PLAIN_FORMAT_PLAIN32	0x2 /* supports UNCOMPRESSED_20_BIT */
 
+#define CSID_RESET_TIMEOUT_MS 500
 
-enum csid_payload_mode {
+
+enum csid_testgen_mode {
 	CSID_PAYLOAD_MODE_INCREMENTING = 0,
 	CSID_PAYLOAD_MODE_ALTERNATING_55_AA = 1,
 	CSID_PAYLOAD_MODE_ALL_ZEROES = 2,
 	CSID_PAYLOAD_MODE_ALL_ONES = 3,
 	CSID_PAYLOAD_MODE_RANDOM = 4,
 	CSID_PAYLOAD_MODE_USER_SPECIFIED = 5,
+	CSID_PAYLOAD_MODE_MAX_SUPPORTED_4_1 = 5,
+	CSID_PAYLOAD_MODE_MAX_SUPPORTED_4_7 = 5,
+	CSID_PAYLOAD_MODE_COMPLEX_PATTERN = 6,
+	CSID_PAYLOAD_MODE_COLOR_BOX = 7,
+	CSID_PAYLOAD_MODE_COLOR_BARS = 8,
+	CSID_PAYLOAD_MODE_MAX_SUPPORTED_170 = 8,
+};
+
+static const char * const csid_testgen_modes[] = {
+	"Disabled",
+	"Incrementing",
+	"Alternating 0x55/0xAA",
+	"All Zeros 0x00",
+	"All Ones 0xFF",
+	"Pseudo-random Data",
+	"User Specified",
+	"Complex pattern",
+	"Color box",
+	"Color bars",
+};
+
+struct csid_format {
+	u32 code;
+	u8 data_type;
+	u8 decode_format;
+	u8 bpp;
+	u8 spp; /* bus samples per pixel */
 };
 
 struct csid_testgen_config {
+	enum csid_testgen_mode mode;
+	const char * const*modes;
+	u8 nmodes;
 	u8 enabled;
-	enum csid_payload_mode payload_mode;
 };
 
 struct csid_phy_config {
@@ -91,6 +123,65 @@  struct csid_phy_config {
 	u32 lane_assign;
 };
 
+struct csid_device;
+
+struct csid_hw_ops {
+	/*
+	 * configure_stream - Configures and starts CSID input stream
+	 * @csid: CSID device
+	 */
+	void (*configure_stream)(struct csid_device *csid, u8 enable);
+
+	/*
+	 * configure_testgen_pattern - Validates and configures output pattern mode
+	 * of test pattern generator
+	 * @csid: CSID device
+	 */
+	int (*configure_testgen_pattern)(struct csid_device *csid, s32 val);
+
+	/*
+	 * hw_version - Read hardware version register from hardware
+	 * @csid: CSID device
+	 */
+	u32 (*hw_version)(struct csid_device *csid);
+
+	/*
+	 * isr - CSID module interrupt service routine
+	 * @irq: Interrupt line
+	 * @dev: CSID device
+	 *
+	 * Return IRQ_HANDLED on success
+	 */
+	irqreturn_t (*isr)(int irq, void *dev);
+
+	/*
+	 * reset - Trigger reset on CSID module and wait to complete
+	 * @csid: CSID device
+	 *
+	 * Return 0 on success or a negative error code otherwise
+	 */
+	int (*reset)(struct csid_device *csid);
+
+	/*
+	 * src_pad_code - Pick an output/src format based on the input/sink format
+	 * @csid: CSID device
+	 * @sink_code: The sink format of the input
+	 * @match_format_idx: Request preferred index, as defined by subdevice csid_format.
+	 *	Set @match_code to 0 if used.
+	 * @match_code: Request preferred code, set @match_format_idx to 0 if used
+	 *
+	 * Return 0 on failure or src format code otherwise
+	 */
+	u32 (*src_pad_code)(struct csid_device *csid, u32 sink_code,
+			    unsigned int match_format_idx, u32 match_code);
+
+	/*
+	 * subdev_init - Initialize CSID device according for hardware revision
+	 * @csid: CSID device
+	 */
+	void (*subdev_init)(struct csid_device *csid);
+};
+
 struct csid_device {
 	struct camss *camss;
 	u8 id;
@@ -110,10 +201,37 @@  struct csid_device {
 	struct v4l2_ctrl *testgen_mode;
 	const struct csid_format *formats;
 	unsigned int nformats;
+	const struct csid_hw_ops *ops;
 };
 
 struct resources;
 
+
+/*
+ * csid_find_code - Find a format code in an array using array index or format code
+ * @codes: Array of format codes
+ * @ncodes: Length of @code array
+ * @req_format_idx: Request preferred index, as defined by subdevice csid_format.
+ *	Set @match_code to 0 if used.
+ * @match_code: Request preferred code, set @req_format_idx to 0 if used
+ *
+ * Return 0 on failure or format code otherwise
+ */
+u32 csid_find_code(u32 *codes, unsigned int ncode,
+		   unsigned int match_format_idx, u32 match_code);
+
+/*
+ * csid_get_fmt_entry - Find csid_format entry with matching format code
+ * @formats: Array of format csid_format entries
+ * @nformats: Length of @nformats array
+ * @code: Desired format code
+ *
+ * Return formats[0] on failure to find code
+ */
+const struct csid_format *csid_get_fmt_entry(const struct csid_format *formats,
+					     unsigned int nformats,
+					     u32 code);
+
 int msm_csid_subdev_init(struct camss *camss, struct csid_device *csid,
 			 const struct resources *res, u8 id);
 
@@ -124,4 +242,8 @@  void msm_csid_unregister_entity(struct csid_device *csid);
 
 void msm_csid_get_csid_id(struct media_entity *entity, u8 *id);
 
+
+extern const struct csid_hw_ops csid_ops_4_1;
+extern const struct csid_hw_ops csid_ops_4_7;
+
 #endif /* QC_MSM_CAMSS_CSID_H */