path: root/target/linux/bcm27xx/patches-5.15/950-0885-media-i2c-Add-driver-of-Arducam-Pivariety-series-cam.patch

From 94203b7e5bbb814c1d18fc5d95fea18b66041c03 Mon Sep 17 00:00:00 2001
From: Lee Jackson <info@arducam.com>
Date: Thu, 14 Apr 2022 17:31:01 +0800
Subject: [PATCH] media: i2c: Add driver of Arducam Pivariety series
 camera

Add a driver for the Arducam Pivariety series CSI2 camera sensor.

Signed-off-by: Lee Jackson <info@arducam.com>
---
 drivers/media/i2c/Kconfig             |   11 +
 drivers/media/i2c/Makefile            |    1 +
 drivers/media/i2c/arducam-pivariety.c | 1469 +++++++++++++++++++++++++
 drivers/media/i2c/arducam-pivariety.h |  107 ++
 4 files changed, 1588 insertions(+)
 create mode 100644 drivers/media/i2c/arducam-pivariety.c
 create mode 100644 drivers/media/i2c/arducam-pivariety.h

--- a/drivers/media/i2c/Kconfig
+++ b/drivers/media/i2c/Kconfig
@@ -730,6 +730,17 @@ config VIDEO_APTINA_PLL
 config VIDEO_CCS_PLL
 	tristate
 
+config VIDEO_ARDUCAM_PIVARIETY
+	tristate "Arducam Pivariety sensor support"
+	depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
+	depends on MEDIA_CAMERA_SUPPORT
+	help
+	  This is a Video4Linux2 sensor driver for the Arducam
+	  Pivariety camera series.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called arducam-pivariety.
+
 config VIDEO_HI556
 	tristate "Hynix Hi-556 sensor support"
 	depends on I2C && VIDEO_V4L2
--- a/drivers/media/i2c/Makefile
+++ b/drivers/media/i2c/Makefile
@@ -7,6 +7,7 @@ obj-$(CONFIG_VIDEO_ET8EK8)	+= et8ek8/
 obj-$(CONFIG_VIDEO_CX25840) += cx25840/
 obj-$(CONFIG_VIDEO_M5MOLS)	+= m5mols/
 
+obj-$(CONFIG_VIDEO_ARDUCAM_PIVARIETY) += arducam-pivariety.o
 obj-$(CONFIG_VIDEO_APTINA_PLL) += aptina-pll.o
 obj-$(CONFIG_VIDEO_TVAUDIO) += tvaudio.o
 obj-$(CONFIG_VIDEO_TDA7432) += tda7432.o
--- /dev/null
+++ b/drivers/media/i2c/arducam-pivariety.c
@@ -0,0 +1,1469 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * A V4L2 driver for Arducam Pivariety Cameras
+ * Copyright (C) 2022 Arducam Technology co., Ltd.
+ *
+ * Based on Sony IMX219 camera driver
+ * Copyright (C) 2019, Raspberry Pi (Trading) Ltd
+ *
+ * I2C read and write method is taken from the OV9281 driver
+ * Copyright (C) 2017 Fuzhou Rockchip Electronics Co., Ltd.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <linux/regulator/consumer.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+#include "arducam-pivariety.h"
+
+static int debug;
+module_param(debug, int, 0644);
+
+/* regulator supplies */
+static const char * const pivariety_supply_name[] = {
+	/* Supplies can be enabled in any order */
+	"VANA",  /* Analog (2.8V) supply */
+	"VDIG",  /* Digital Core (1.8V) supply */
+	"VDDL",  /* IF (1.2V) supply */
+};
+
+/* The supported raw formats. */
+static const u32 codes[] = {
+	MEDIA_BUS_FMT_SBGGR8_1X8,
+	MEDIA_BUS_FMT_SGBRG8_1X8,
+	MEDIA_BUS_FMT_SGRBG8_1X8,
+	MEDIA_BUS_FMT_SRGGB8_1X8,
+	MEDIA_BUS_FMT_Y8_1X8,
+
+	MEDIA_BUS_FMT_SBGGR10_1X10,
+	MEDIA_BUS_FMT_SGBRG10_1X10,
+	MEDIA_BUS_FMT_SGRBG10_1X10,
+	MEDIA_BUS_FMT_SRGGB10_1X10,
+	MEDIA_BUS_FMT_Y10_1X10,
+
+	MEDIA_BUS_FMT_SBGGR12_1X12,
+	MEDIA_BUS_FMT_SGBRG12_1X12,
+	MEDIA_BUS_FMT_SGRBG12_1X12,
+	MEDIA_BUS_FMT_SRGGB12_1X12,
+	MEDIA_BUS_FMT_Y12_1X12,
+};
+
+#define ARDUCAM_NUM_SUPPLIES ARRAY_SIZE(pivariety_supply_name)
+
+#define ARDUCAM_XCLR_MIN_DELAY_US	10000
+#define ARDUCAM_XCLR_DELAY_RANGE_US	1000
+
+#define MAX_CTRLS 32
+
+struct pivariety {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+
+	struct v4l2_fwnode_bus_mipi_csi2 bus;
+	struct clk *xclk;
+	u32 xclk_freq;
+
+	struct gpio_desc *reset_gpio;
+	struct regulator_bulk_data supplies[ARDUCAM_NUM_SUPPLIES];
+
+	struct arducam_format *supported_formats;
+	int num_supported_formats;
+	int current_format_idx;
+	int current_resolution_idx;
+	int lanes;
+	int bayer_order_volatile;
+	bool wait_until_free;
+
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_ctrl *ctrls[MAX_CTRLS];
+	/* V4L2 Controls */
+	struct v4l2_ctrl *vflip;
+	struct v4l2_ctrl *hflip;
+
+	struct v4l2_rect crop;
+	/*
+	 * Mutex for serialized access:
+	 * Protect sensor module set pad format and start/stop streaming safely.
+	 */
+	struct mutex mutex;
+
+	/* Streaming on/off */
+	bool streaming;
+};
+
+static inline struct pivariety *to_pivariety(struct v4l2_subdev *_sd)
+{
+	return container_of(_sd, struct pivariety, sd);
+}
+
+/* Write registers up to 4 at a time */
+static int pivariety_write_reg(struct i2c_client *client, u16 reg, u32 val)
+{
+	unsigned int len = sizeof(u32);
+	u32 buf_i, val_i = 0;
+	u8 buf[6];
+	u8 *val_p;
+	__be32 val_be;
+
+	buf[0] = reg >> 8;
+	buf[1] = reg & 0xff;
+
+	val_be = cpu_to_be32(val);
+	val_p = (u8 *)&val_be;
+	buf_i = 2;
+
+	while (val_i < 4)
+		buf[buf_i++] = val_p[val_i++];
+
+	if (i2c_master_send(client, buf, len + 2) != len + 2)
+		return -EIO;
+
+	return 0;
+}
+
+/* Read registers up to 4 at a time */
+static int pivariety_read_reg(struct i2c_client *client, u16 reg, u32 *val)
+{
+	struct i2c_msg msgs[2];
+	unsigned int len = sizeof(u32);
+	u8 *data_be_p;
+	__be32 data_be = 0;
+	__be16 reg_addr_be = cpu_to_be16(reg);
+	int ret;
+
+	data_be_p = (u8 *)&data_be;
+	/* Write register address */
+	msgs[0].addr = client->addr;
+	msgs[0].flags = 0;
+	msgs[0].len = 2;
+	msgs[0].buf = (u8 *)&reg_addr_be;
+
+	/* Read data from register */
+	msgs[1].addr = client->addr;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].len = len;
+	msgs[1].buf = data_be_p;
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret != ARRAY_SIZE(msgs))
+		return -EIO;
+
+	*val = be32_to_cpu(data_be);
+
+	return 0;
+}
+
+static int
+pivariety_read(struct pivariety *pivariety, u16 addr, u32 *value)
+{
+	struct v4l2_subdev *sd = &pivariety->sd;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret, count = 0;
+
+	while (count++ < I2C_READ_RETRY_COUNT) {
+		ret = pivariety_read_reg(client, addr, value);
+		if (!ret) {
+			v4l2_dbg(2, debug, sd, "%s: 0x%02x 0x%04x\n",
+				 __func__, addr, *value);
+			return ret;
+		}
+	}
+
+	v4l2_err(sd, "%s: Reading register 0x%02x failed\n",
+		 __func__, addr);
+
+	return ret;
+}
+
+static int pivariety_write(struct pivariety *pivariety, u16 addr, u32 value)
+{
+	struct v4l2_subdev *sd = &pivariety->sd;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret, count = 0;
+
+	while (count++ < I2C_WRITE_RETRY_COUNT) {
+		ret = pivariety_write_reg(client, addr, value);
+		if (!ret)
+			return ret;
+	}
+
+	v4l2_err(sd, "%s: Write 0x%04x to register 0x%02x failed\n",
+		 __func__, value, addr);
+
+	return ret;
+}
+
+static int wait_for_free(struct pivariety *pivariety, int interval)
+{
+	u32 value;
+	u32 count = 0;
+
+	while (count++ < (1000 / interval)) {
+		int ret = pivariety_read(pivariety, SYSTEM_IDLE_REG, &value);
+
+		if (!ret && !value)
+			break;
+		msleep(interval);
+	}
+
+	v4l2_dbg(2, debug, &pivariety->sd, "%s: End wait, Count: %d.\n",
+		 __func__, count);
+
+	return 0;
+}
+
+static int is_raw(int pixformat)
+{
+	return pixformat >= 0x28 && pixformat <= 0x2D;
+}
+
+static u32 bayer_to_mbus_code(int data_type, int bayer_order)
+{
+	const u32 depth8[] = {
+		MEDIA_BUS_FMT_SBGGR8_1X8,
+		MEDIA_BUS_FMT_SGBRG8_1X8,
+		MEDIA_BUS_FMT_SGRBG8_1X8,
+		MEDIA_BUS_FMT_SRGGB8_1X8,
+		MEDIA_BUS_FMT_Y8_1X8,
+	};
+
+	const u32 depth10[] = {
+		MEDIA_BUS_FMT_SBGGR10_1X10,
+		MEDIA_BUS_FMT_SGBRG10_1X10,
+		MEDIA_BUS_FMT_SGRBG10_1X10,
+		MEDIA_BUS_FMT_SRGGB10_1X10,
+		MEDIA_BUS_FMT_Y10_1X10,
+	};
+
+	const u32 depth12[] = {
+		MEDIA_BUS_FMT_SBGGR12_1X12,
+		MEDIA_BUS_FMT_SGBRG12_1X12,
+		MEDIA_BUS_FMT_SGRBG12_1X12,
+		MEDIA_BUS_FMT_SRGGB12_1X12,
+		MEDIA_BUS_FMT_Y12_1X12,
+	};
+
+	if (bayer_order < 0 || bayer_order > 4)
+		return 0;
+
+	switch (data_type) {
+	case IMAGE_DT_RAW8:
+		return depth8[bayer_order];
+	case IMAGE_DT_RAW10:
+		return depth10[bayer_order];
+	case IMAGE_DT_RAW12:
+		return depth12[bayer_order];
+	}
+
+	return 0;
+}
+
+static u32 yuv422_to_mbus_code(int data_type, int order)
+{
+	const u32 depth8[] = {
+		MEDIA_BUS_FMT_YUYV8_1X16,
+		MEDIA_BUS_FMT_YVYU8_1X16,
+		MEDIA_BUS_FMT_UYVY8_1X16,
+		MEDIA_BUS_FMT_VYUY8_1X16,
+	};
+
+	const u32 depth10[] = {
+		MEDIA_BUS_FMT_YUYV10_1X20,
+		MEDIA_BUS_FMT_YVYU10_1X20,
+		MEDIA_BUS_FMT_UYVY10_1X20,
+		MEDIA_BUS_FMT_VYUY10_1X20,
+	};
+
+	if (order < 0 || order > 3)
+		return 0;
+
+	switch (data_type) {
+	case IMAGE_DT_YUV422_8:
+		return depth8[order];
+	case IMAGE_DT_YUV422_10:
+		return depth10[order];
+	}
+
+	return 0;
+}
+
+static u32 data_type_to_mbus_code(int data_type, int bayer_order)
+{
+	if (is_raw(data_type))
+		return bayer_to_mbus_code(data_type, bayer_order);
+
+	switch (data_type) {
+	case IMAGE_DT_YUV422_8:
+	case IMAGE_DT_YUV422_10:
+		return yuv422_to_mbus_code(data_type, bayer_order);
+	case IMAGE_DT_RGB565:
+		return MEDIA_BUS_FMT_RGB565_2X8_LE;
+	case IMAGE_DT_RGB888:
+		return MEDIA_BUS_FMT_RGB888_1X24;
+	}
+
+	return 0;
+}
+
+/* Get bayer order based on flip setting. */
+static u32 pivariety_get_format_code(struct pivariety *pivariety,
+				     struct arducam_format *format)
+{
+	unsigned int order, origin_order;
+
+	lockdep_assert_held(&pivariety->mutex);
+
+	/*
+	 * Only the bayer format needs to transform the format.
+	 */
+	if (!is_raw(format->data_type) ||
+	    !pivariety->bayer_order_volatile ||
+	    format->bayer_order == BAYER_ORDER_GRAY)
+		return data_type_to_mbus_code(format->data_type,
+					      format->bayer_order);
+
+	order = format->bayer_order;
+
+	origin_order = order;
+
+	order = (pivariety->hflip && pivariety->hflip->val ? order ^ 1 : order);
+	order = (pivariety->vflip && pivariety->vflip->val ? order ^ 2 : order);
+
+	v4l2_dbg(1, debug, &pivariety->sd, "%s: before: %d, after: %d.\n",
+		 __func__, origin_order, order);
+
+	return data_type_to_mbus_code(format->data_type, order);
+}
+
+/* Power/clock management functions */
+static int pivariety_power_on(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct pivariety *pivariety = to_pivariety(sd);
+	int ret;
+
+	ret = regulator_bulk_enable(ARDUCAM_NUM_SUPPLIES,
+				    pivariety->supplies);
+	if (ret) {
+		dev_err(dev, "%s: failed to enable regulators\n",
+			__func__);
+		return ret;
+	}
+
+	ret = clk_prepare_enable(pivariety->xclk);
+	if (ret) {
+		dev_err(dev, "%s: failed to enable clock\n",
+			__func__);
+		goto reg_off;
+	}
+
+	gpiod_set_value_cansleep(pivariety->reset_gpio, 1);
+	usleep_range(ARDUCAM_XCLR_MIN_DELAY_US,
+		     ARDUCAM_XCLR_MIN_DELAY_US + ARDUCAM_XCLR_DELAY_RANGE_US);
+
+	return 0;
+
+reg_off:
+	regulator_bulk_disable(ARDUCAM_NUM_SUPPLIES, pivariety->supplies);
+
+	return ret;
+}
+
+static int pivariety_power_off(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct pivariety *pivariety = to_pivariety(sd);
+
+	gpiod_set_value_cansleep(pivariety->reset_gpio, 0);
+	regulator_bulk_disable(ARDUCAM_NUM_SUPPLIES, pivariety->supplies);
+	clk_disable_unprepare(pivariety->xclk);
+
+	return 0;
+}
+
+static int pivariety_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct pivariety *pivariety = to_pivariety(sd);
+	struct v4l2_mbus_framefmt *try_fmt =
+		v4l2_subdev_get_try_format(sd, fh->state, 0);
+	struct arducam_format *def_fmt = &pivariety->supported_formats[0];
+
+	/* Initialize try_fmt */
+	try_fmt->width = def_fmt->resolution_set->width;
+	try_fmt->height = def_fmt->resolution_set->height;
+	try_fmt->code = def_fmt->mbus_code;
+	try_fmt->field = V4L2_FIELD_NONE;
+
+	return 0;
+}
+
+static int pivariety_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	int ret, i;
+	struct pivariety *pivariety =
+		container_of(ctrl->handler, struct pivariety,
+			     ctrl_handler);
+	struct arducam_format *supported_fmts = pivariety->supported_formats;
+	int num_supported_formats = pivariety->num_supported_formats;
+
+	v4l2_dbg(3, debug, &pivariety->sd, "%s: cid = (0x%X), value = (%d).\n",
+		 __func__, ctrl->id, ctrl->val);
+
+	ret = pivariety_write(pivariety, CTRL_ID_REG, ctrl->id);
+	ret += pivariety_write(pivariety, CTRL_VALUE_REG, ctrl->val);
+	if (ret < 0)
+		return -EINVAL;
+
+	/* When flip is set, modify all bayer formats */
+	if (ctrl->id == V4L2_CID_VFLIP || ctrl->id == V4L2_CID_HFLIP) {
+		for (i = 0; i < num_supported_formats; i++) {
+			supported_fmts[i].mbus_code =
+				pivariety_get_format_code(pivariety,
+							  &supported_fmts[i]);
+		}
+	}
+
+	/*
+	 * When starting streaming, controls are set in batches,
+	 * and the short interval will cause some controls to be unsuccessfully
+	 * set.
+	 */
+	if (pivariety->wait_until_free)
+		wait_for_free(pivariety, 1);
+	else
+		usleep_range(200, 210);
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops pivariety_ctrl_ops = {
+	.s_ctrl = pivariety_s_ctrl,
+};
+
+static int pivariety_enum_mbus_code(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_state *sd_state,
+				    struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct pivariety *pivariety = to_pivariety(sd);
+	struct arducam_format *supported_formats = pivariety->supported_formats;
+	int num_supported_formats = pivariety->num_supported_formats;
+
+	v4l2_dbg(1, debug, sd, "%s: index = (%d)\n", __func__, code->index);
+
+	if (code->index >= num_supported_formats)
+		return -EINVAL;
+
+	code->code = supported_formats[code->index].mbus_code;
+
+	return 0;
+}
+
+static int pivariety_enum_framesizes(struct v4l2_subdev *sd,
+				     struct v4l2_subdev_state *sd_state,
+				     struct v4l2_subdev_frame_size_enum *fse)
+{
+	int i;
+	struct pivariety *pivariety = to_pivariety(sd);
+	struct arducam_format *supported_formats = pivariety->supported_formats;
+	int num_supported_formats = pivariety->num_supported_formats;
+	struct arducam_format *format;
+	struct arducam_resolution *resolution;
+
+	v4l2_dbg(1, debug, sd, "%s: code = (0x%X), index = (%d)\n",
+		 __func__, fse->code, fse->index);
+
+	for (i = 0; i < num_supported_formats; i++) {
+		format = &supported_formats[i];
+		if (fse->code == format->mbus_code) {
+			if (fse->index >= format->num_resolution_set)
+				return -EINVAL;
+
+			resolution = &format->resolution_set[fse->index];
+			fse->min_width = resolution->width;
+			fse->max_width = resolution->width;
+			fse->min_height = resolution->height;
+			fse->max_height = resolution->height;
+
+			return 0;
+		}
+	}
+
+	return -EINVAL;
+}
+
+static int pivariety_get_fmt(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *sd_state,
+			     struct v4l2_subdev_format *format)
+{
+	struct pivariety *pivariety = to_pivariety(sd);
+	struct arducam_format *current_format;
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+	int cur_res_idx;
+
+	if (format->pad != 0)
+		return -EINVAL;
+
+	mutex_lock(&pivariety->mutex);
+
+	current_format =
+		&pivariety->supported_formats[pivariety->current_format_idx];
+	cur_res_idx = pivariety->current_resolution_idx;
+	format->format.width =
+		current_format->resolution_set[cur_res_idx].width;
+	format->format.height =
+		current_format->resolution_set[cur_res_idx].height;
+	format->format.code = current_format->mbus_code;
+	format->format.field = V4L2_FIELD_NONE;
+	fmt->colorspace = V4L2_COLORSPACE_RAW;
+	fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(fmt->colorspace);
+	fmt->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(true,
+							  fmt->colorspace,
+							  fmt->ycbcr_enc);
+	fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(fmt->colorspace);
+
+	v4l2_dbg(1, debug, sd, "%s: width: (%d) height: (%d) code: (0x%X)\n",
+		 __func__, format->format.width, format->format.height,
+		 format->format.code);
+
+	mutex_unlock(&pivariety->mutex);
+	return 0;
+}
+
+static int pivariety_get_fmt_idx_by_code(struct pivariety *pivariety,
+					 u32 mbus_code)
+{
+	int i;
+	u32 data_type;
+	struct arducam_format *formats = pivariety->supported_formats;
+
+	for (i = 0; i < pivariety->num_supported_formats; i++) {
+		if (formats[i].mbus_code == mbus_code)
+			return i;
+	}
+
+	/*
+	 * If the specified format is not found in the list of supported
+	 * formats, try to find a format of the same data type.
+	 */
+	for (i = 0; i < ARRAY_SIZE(codes); i++)
+		if (codes[i] == mbus_code)
+			break;
+
+	if (i >= ARRAY_SIZE(codes))
+		return -EINVAL;
+
+	data_type = i / 5 + IMAGE_DT_RAW8;
+
+	for (i = 0; i < pivariety->num_supported_formats; i++) {
+		if (formats[i].data_type == data_type)
+			return i;
+	}
+
+	return -EINVAL;
+}
+
+static struct v4l2_ctrl *get_control(struct pivariety *pivariety,
+				     u32 id)
+{
+	int index = 0;
+
+	while (index < MAX_CTRLS && pivariety->ctrls[index]) {
+		if (pivariety->ctrls[index]->id == id)
+			return pivariety->ctrls[index];
+		index++;
+	}
+
+	return NULL;
+}
+
+static int update_control(struct pivariety *pivariety, u32 id)
+{
+	struct v4l2_subdev *sd = &pivariety->sd;
+	struct v4l2_ctrl *ctrl;
+	u32 min, max, step, def, id2;
+	int ret = 0;
+
+	pivariety_write(pivariety, CTRL_ID_REG, id);
+	pivariety_read(pivariety, CTRL_ID_REG, &id2);
+
+	v4l2_dbg(1, debug, sd, "%s: Write ID: 0x%08X Read ID: 0x%08X\n",
+		 __func__, id, id2);
+
+	pivariety_write(pivariety, CTRL_VALUE_REG, 0);
+	wait_for_free(pivariety, 1);
+
+	ret += pivariety_read(pivariety, CTRL_MAX_REG, &max);
+	ret += pivariety_read(pivariety, CTRL_MIN_REG, &min);
+	ret += pivariety_read(pivariety, CTRL_DEF_REG, &def);
+	ret += pivariety_read(pivariety, CTRL_STEP_REG, &step);
+
+	if (ret < 0)
+		goto err;
+
+	if (id == NO_DATA_AVAILABLE || max == NO_DATA_AVAILABLE ||
+	    min == NO_DATA_AVAILABLE || def == NO_DATA_AVAILABLE ||
+	    step == NO_DATA_AVAILABLE)
+		goto err;
+
+	v4l2_dbg(1, debug, sd, "%s: min: %d, max: %d, step: %d, def: %d\n",
+		 __func__, min, max, step, def);
+
+	ctrl = get_control(pivariety, id);
+	return __v4l2_ctrl_modify_range(ctrl, min, max, step, def);
+
+err:
+	return -EINVAL;
+}
+
+static int update_controls(struct pivariety *pivariety)
+{
+	int ret = 0;
+	int index = 0;
+
+	wait_for_free(pivariety, 5);
+
+	while (index < MAX_CTRLS && pivariety->ctrls[index]) {
+		ret += update_control(pivariety, pivariety->ctrls[index]->id);
+		index++;
+	}
+
+	return ret;
+}
+
+static int pivariety_set_fmt(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *sd_state,
+			     struct v4l2_subdev_format *format)
+{
+	int i, j;
+	struct pivariety *pivariety = to_pivariety(sd);
+	struct arducam_format *supported_formats = pivariety->supported_formats;
+
+	if (format->pad != 0)
+		return -EINVAL;
+
+	mutex_lock(&pivariety->mutex);
+
+	format->format.colorspace = V4L2_COLORSPACE_RAW;
+	format->format.field = V4L2_FIELD_NONE;
+
+	v4l2_dbg(1, debug, sd, "%s: code: 0x%X, width: %d, height: %d\n",
+		 __func__, format->format.code, format->format.width,
+		 format->format.height);
+
+	i = pivariety_get_fmt_idx_by_code(pivariety, format->format.code);
+	if (i < 0)
+		i = 0;
+
+	format->format.code = supported_formats[i].mbus_code;
+
+	for (j = 0; j < supported_formats[i].num_resolution_set; j++) {
+		if (supported_formats[i].resolution_set[j].width ==
+						format->format.width &&
+			supported_formats[i].resolution_set[j].height ==
+						format->format.height) {
+			v4l2_dbg(1, debug, sd,
+				 "%s: format match.\n", __func__);
+			v4l2_dbg(1, debug, sd,
+				 "%s: set format to device: %d %d.\n",
+				 __func__, supported_formats[i].index, j);
+
+			pivariety_write(pivariety, PIXFORMAT_INDEX_REG,
+					supported_formats[i].index);
+			pivariety_write(pivariety, RESOLUTION_INDEX_REG, j);
+
+			pivariety->current_format_idx = i;
+			pivariety->current_resolution_idx = j;
+
+			update_controls(pivariety);
+
+			goto unlock;
+		}
+	}
+
+	format->format.width = supported_formats[i].resolution_set[0].width;
+	format->format.height = supported_formats[i].resolution_set[0].height;
+
+	pivariety_write(pivariety, PIXFORMAT_INDEX_REG,
+			supported_formats[i].index);
+	pivariety_write(pivariety, RESOLUTION_INDEX_REG, 0);
+
+	pivariety->current_format_idx = i;
+	pivariety->current_resolution_idx = 0;
+	update_controls(pivariety);
+
+unlock:
+
+	mutex_unlock(&pivariety->mutex);
+
+	return 0;
+}
+
+/* Start streaming */
+static int pivariety_start_streaming(struct pivariety *pivariety)
+{
+	int ret;
+
+	/* set stream on register */
+	ret = pivariety_write(pivariety, MODE_SELECT_REG,
+			      ARDUCAM_MODE_STREAMING);
+
+	if (ret)
+		return ret;
+
+	wait_for_free(pivariety, 2);
+
+	/*
+	 * When starting streaming, controls are set in batches,
+	 * and the short interval will cause some controls to be unsuccessfully
+	 * set.
+	 */
+	pivariety->wait_until_free = true;
+	/* Apply customized values from user */
+	ret =  __v4l2_ctrl_handler_setup(pivariety->sd.ctrl_handler);
+
+	pivariety->wait_until_free = false;
+	if (ret)
+		return ret;
+
+	wait_for_free(pivariety, 2);
+
+	return ret;
+}
+
+static int pivariety_read_sel(struct pivariety *pivariety,
+			      struct v4l2_rect *rect)
+{
+	int ret = 0;
+
+	ret += pivariety_read(pivariety, IPC_SEL_TOP_REG, &rect->top);
+	ret += pivariety_read(pivariety, IPC_SEL_LEFT_REG, &rect->left);
+	ret += pivariety_read(pivariety, IPC_SEL_WIDTH_REG, &rect->width);
+	ret += pivariety_read(pivariety, IPC_SEL_HEIGHT_REG, &rect->height);
+
+	if (ret || rect->top == NO_DATA_AVAILABLE ||
+	    rect->left == NO_DATA_AVAILABLE ||
+	    rect->width == NO_DATA_AVAILABLE ||
+	    rect->height == NO_DATA_AVAILABLE) {
+		v4l2_err(&pivariety->sd, "%s: Failed to read selection.\n",
+			 __func__);
+		return -EINVAL;
+		}
+
+	return 0;
+}
+
+static const struct v4l2_rect *
+__pivariety_get_pad_crop(struct pivariety *pivariety,
+			 struct v4l2_subdev_state *sd_state,
+			 unsigned int pad,
+			 enum v4l2_subdev_format_whence which)
+{
+	int ret;
+
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_crop(&pivariety->sd, sd_state, pad);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		ret = pivariety_read_sel(pivariety, &pivariety->crop);
+		if (ret)
+			return NULL;
+		return &pivariety->crop;
+	}
+
+	return NULL;
+}
+
+static int pivariety_get_selection(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_selection *sel)
+{
+	int ret = 0;
+	struct v4l2_rect rect;
+	struct pivariety *pivariety = to_pivariety(sd);
+
+	ret = pivariety_write(pivariety, IPC_SEL_TARGET_REG, sel->target);
+	if (ret) {
+		v4l2_err(sd, "%s: Write register 0x%02x failed\n",
+			 __func__, IPC_SEL_TARGET_REG);
+		return -EINVAL;
+	}
+
+	wait_for_free(pivariety, 2);
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP: {
+		mutex_lock(&pivariety->mutex);
+		sel->r = *__pivariety_get_pad_crop(pivariety, sd_state,
+						   sel->pad,
+						   sel->which);
+		mutex_unlock(&pivariety->mutex);
+
+		return 0;
+	}
+
+	case V4L2_SEL_TGT_NATIVE_SIZE:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		ret = pivariety_read_sel(pivariety, &rect);
+		if (ret)
+			return -EINVAL;
+
+		sel->r = rect;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+/* Stop streaming */
+static int pivariety_stop_streaming(struct pivariety *pivariety)
+{
+	int ret;
+
+	/* set stream off register */
+	ret = pivariety_write(pivariety, MODE_SELECT_REG, ARDUCAM_MODE_STANDBY);
+	if (ret)
+		v4l2_err(&pivariety->sd, "%s failed to set stream\n", __func__);
+
+	/*
+	 * Return success even if it was an error, as there is nothing the
+	 * caller can do about it.
+	 */
+	return 0;
+}
+
+static int pivariety_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct pivariety *pivariety = to_pivariety(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	mutex_lock(&pivariety->mutex);
+	if (pivariety->streaming == enable) {
+		mutex_unlock(&pivariety->mutex);
+		return 0;
+	}
+
+	if (enable) {
+		ret = pm_runtime_get_sync(&client->dev);
+		if (ret < 0) {
+			pm_runtime_put_noidle(&client->dev);
+			goto err_unlock;
+		}
+
+		/*
+		 * Apply default & customized values
+		 * and then start streaming.
+		 */
+		ret = pivariety_start_streaming(pivariety);
+		if (ret)
+			goto err_rpm_put;
+	} else {
+		pivariety_stop_streaming(pivariety);
+		pm_runtime_put(&client->dev);
+	}
+
+	pivariety->streaming = enable;
+
+	/*
+	 * vflip and hflip cannot change during streaming
+	 * Pivariety may not implement flip control.
+	 */
+	if (pivariety->vflip)
+		__v4l2_ctrl_grab(pivariety->vflip, enable);
+
+	if (pivariety->hflip)
+		__v4l2_ctrl_grab(pivariety->hflip, enable);
+
+	mutex_unlock(&pivariety->mutex);
+
+	return ret;
+
+err_rpm_put:
+	pm_runtime_put(&client->dev);
+err_unlock:
+	mutex_unlock(&pivariety->mutex);
+
+	return ret;
+}
+
+static int __maybe_unused pivariety_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct pivariety *pivariety = to_pivariety(sd);
+
+	if (pivariety->streaming)
+		pivariety_stop_streaming(pivariety);
+
+	return 0;
+}
+
+static int __maybe_unused pivariety_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct pivariety *pivariety = to_pivariety(sd);
+	int ret;
+
+	if (pivariety->streaming) {
+		ret = pivariety_start_streaming(pivariety);
+		if (ret)
+			goto error;
+	}
+
+	return 0;
+
+error:
+	pivariety_stop_streaming(pivariety);
+	pivariety->streaming = 0;
+	return ret;
+}
+
+static int pivariety_get_regulators(struct pivariety *pivariety)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&pivariety->sd);
+	int i;
+
+	for (i = 0; i < ARDUCAM_NUM_SUPPLIES; i++)
+		pivariety->supplies[i].supply = pivariety_supply_name[i];
+
+	return devm_regulator_bulk_get(&client->dev,
+				       ARDUCAM_NUM_SUPPLIES,
+				       pivariety->supplies);
+}
+
+static int pivariety_get_mbus_config(struct v4l2_subdev *sd, unsigned int pad,
+				     struct v4l2_mbus_config *cfg)
+{
+	struct pivariety *pivariety = to_pivariety(sd);
+	const u32 mask = V4L2_MBUS_CSI2_LANE_MASK;
+
+	if (pivariety->lanes > pivariety->bus.num_data_lanes)
+		return -EINVAL;
+
+	cfg->type = V4L2_MBUS_CSI2_DPHY;
+	cfg->flags = (pivariety->lanes << __ffs(mask)) & mask;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops pivariety_core_ops = {
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_video_ops pivariety_video_ops = {
+	.s_stream = pivariety_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops pivariety_pad_ops = {
+	.enum_mbus_code = pivariety_enum_mbus_code,
+	.get_fmt = pivariety_get_fmt,
+	.set_fmt = pivariety_set_fmt,
+	.enum_frame_size = pivariety_enum_framesizes,
+	.get_selection = pivariety_get_selection,
+	.get_mbus_config = pivariety_get_mbus_config,
+};
+
+static const struct v4l2_subdev_ops pivariety_subdev_ops = {
+	.core = &pivariety_core_ops,
+	.video = &pivariety_video_ops,
+	.pad = &pivariety_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops pivariety_internal_ops = {
+	.open = pivariety_open,
+};
+
+static void pivariety_free_controls(struct pivariety *pivariety)
+{
+	v4l2_ctrl_handler_free(pivariety->sd.ctrl_handler);
+	mutex_destroy(&pivariety->mutex);
+}
+
+static int pivariety_get_length_of_set(struct pivariety *pivariety,
+				       u16 idx_reg, u16 val_reg)
+{
+	int ret;
+	int index = 0;
+	u32 val;
+
+	while (1) {
+		ret = pivariety_write(pivariety, idx_reg, index);
+		ret += pivariety_read(pivariety, val_reg, &val);
+
+		if (ret < 0)
+			return -1;
+
+		if (val == NO_DATA_AVAILABLE)
+			break;
+		index++;
+	}
+	pivariety_write(pivariety, idx_reg, 0);
+	return index;
+}
+
+static int pivariety_enum_resolution(struct pivariety *pivariety,
+				     struct arducam_format *format)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&pivariety->sd);
+	int index = 0;
+	u32 width, height;
+	int num_resolution = 0;
+	int ret;
+
+	num_resolution = pivariety_get_length_of_set(pivariety,
+						     RESOLUTION_INDEX_REG,
+						     FORMAT_WIDTH_REG);
+	if (num_resolution < 0)
+		goto err;
+
+	format->resolution_set = devm_kzalloc(&client->dev,
+					      sizeof(*format->resolution_set) *
+								num_resolution,
+					      GFP_KERNEL);
+	while (1) {
+		ret = pivariety_write(pivariety, RESOLUTION_INDEX_REG, index);
+		ret += pivariety_read(pivariety, FORMAT_WIDTH_REG, &width);
+		ret += pivariety_read(pivariety, FORMAT_HEIGHT_REG, &height);
+
+		if (ret < 0)
+			goto err;
+
+		if (width == NO_DATA_AVAILABLE || height == NO_DATA_AVAILABLE)
+			break;
+
+		format->resolution_set[index].width = width;
+		format->resolution_set[index].height = height;
+
+		index++;
+	}
+
+	format->num_resolution_set = index;
+	pivariety_write(pivariety, RESOLUTION_INDEX_REG, 0);
+	return 0;
+err:
+	return -ENODEV;
+}
+
+static int pivariety_enum_pixformat(struct pivariety *pivariety)
+{
+	int ret = 0;
+	u32 mbus_code = 0;
+	int pixfmt_type;
+	int bayer_order;
+	int bayer_order_not_volatile;
+	int lanes;
+	int index = 0;
+	int num_pixformat = 0;
+	struct arducam_format *arducam_fmt;
+	struct i2c_client *client = v4l2_get_subdevdata(&pivariety->sd);
+
+	num_pixformat = pivariety_get_length_of_set(pivariety,
+						    PIXFORMAT_INDEX_REG,
+						    PIXFORMAT_TYPE_REG);
+
+	if (num_pixformat < 0)
+		goto err;
+
+	ret = pivariety_read(pivariety, FLIPS_DONT_CHANGE_ORDER_REG,
+			     &bayer_order_not_volatile);
+	if (bayer_order_not_volatile == NO_DATA_AVAILABLE)
+		pivariety->bayer_order_volatile = 1;
+	else
+		pivariety->bayer_order_volatile = !bayer_order_not_volatile;
+
+	if (ret < 0)
+		goto err;
+
+	pivariety->supported_formats =
+		devm_kzalloc(&client->dev,
+			     sizeof(*pivariety->supported_formats) *
+								num_pixformat,
+			     GFP_KERNEL);
+
+	while (1) {
+		ret = pivariety_write(pivariety, PIXFORMAT_INDEX_REG, index);
+		ret += pivariety_read(pivariety, PIXFORMAT_TYPE_REG,
+				      &pixfmt_type);
+
+		if (pixfmt_type == NO_DATA_AVAILABLE)
+			break;
+
+		ret += pivariety_read(pivariety, MIPI_LANES_REG, &lanes);
+		if (lanes == NO_DATA_AVAILABLE)
+			break;
+
+		ret += pivariety_read(pivariety, PIXFORMAT_ORDER_REG,
+				      &bayer_order);
+		if (ret < 0)
+			goto err;
+
+		mbus_code = data_type_to_mbus_code(pixfmt_type, bayer_order);
+		arducam_fmt = &pivariety->supported_formats[index];
+		arducam_fmt->index = index;
+		arducam_fmt->mbus_code = mbus_code;
+		arducam_fmt->bayer_order = bayer_order;
+		arducam_fmt->data_type = pixfmt_type;
+		if (pivariety_enum_resolution(pivariety, arducam_fmt))
+			goto err;
+
+		index++;
+	}
+
+	pivariety_write(pivariety, PIXFORMAT_INDEX_REG, 0);
+	pivariety->num_supported_formats = index;
+	pivariety->current_format_idx = 0;
+	pivariety->current_resolution_idx = 0;
+	pivariety->lanes = lanes;
+
+	return 0;
+
+err:
+	return -ENODEV;
+}
+
+static const char *pivariety_ctrl_get_name(u32 id)
+{
+	switch (id) {
+	case V4L2_CID_ARDUCAM_EXT_TRI:
+		return "trigger_mode";
+	case V4L2_CID_ARDUCAM_IRCUT:
+		return "ircut";
+	default:
+		return NULL;
+	}
+}
+
+enum v4l2_ctrl_type pivariety_get_v4l2_ctrl_type(u32 id)
+{
+	switch (id) {
+	case V4L2_CID_ARDUCAM_EXT_TRI:
+		return V4L2_CTRL_TYPE_BOOLEAN;
+	case V4L2_CID_ARDUCAM_IRCUT:
+		return V4L2_CTRL_TYPE_BOOLEAN;
+	default:
+		return V4L2_CTRL_TYPE_INTEGER;
+	}
+}
+
+static struct v4l2_ctrl *v4l2_ctrl_new_arducam(struct v4l2_ctrl_handler *hdl,
+					       const struct v4l2_ctrl_ops *ops,
+					       u32 id, s64 min, s64 max,
+					       u64 step, s64 def)
+{
+	struct v4l2_ctrl_config ctrl_cfg = {
+		.ops = ops,
+		.id = id,
+		.name = NULL,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.flags = 0,
+		.min = min,
+		.max = max,
+		.def = def,
+		.step = step,
+	};
+
+	ctrl_cfg.name = pivariety_ctrl_get_name(id);
+	ctrl_cfg.type = pivariety_get_v4l2_ctrl_type(id);
+
+	return v4l2_ctrl_new_custom(hdl, &ctrl_cfg, NULL);
+}
+
+static int pivariety_enum_controls(struct pivariety *pivariety)
+{
+	struct v4l2_subdev *sd = &pivariety->sd;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct v4l2_ctrl_handler *ctrl_hdlr = &pivariety->ctrl_handler;
+	struct v4l2_fwnode_device_properties props;
+	struct v4l2_ctrl **ctrl = pivariety->ctrls;
+	int ret, index, num_ctrls;
+	u32 id, min, max, def, step;
+
+	num_ctrls = pivariety_get_length_of_set(pivariety, CTRL_INDEX_REG,
+						CTRL_ID_REG);
+	if (num_ctrls < 0)
+		goto err;
+
+	v4l2_dbg(1, debug, sd, "%s: num_ctrls = %d\n",
+		 __func__, num_ctrls);
+
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, num_ctrls);
+	if (ret)
+		return ret;
+
+	index = 0;
+	while (1) {
+		ret = pivariety_write(pivariety, CTRL_INDEX_REG, index);
+		pivariety_write(pivariety, CTRL_VALUE_REG, 0);
+		wait_for_free(pivariety, 1);
+
+		ret += pivariety_read(pivariety, CTRL_ID_REG, &id);
+		ret += pivariety_read(pivariety, CTRL_MAX_REG, &max);
+		ret += pivariety_read(pivariety, CTRL_MIN_REG, &min);
+		ret += pivariety_read(pivariety, CTRL_DEF_REG, &def);
+		ret += pivariety_read(pivariety, CTRL_STEP_REG, &step);
+		if (ret < 0)
+			goto err;
+
+		if (id == NO_DATA_AVAILABLE || max == NO_DATA_AVAILABLE ||
+		    min == NO_DATA_AVAILABLE || def == NO_DATA_AVAILABLE ||
+		    step == NO_DATA_AVAILABLE)
+			break;
+
+		v4l2_dbg(1, debug, sd,
+			 "%s: index = %d, id = 0x%x, max = %d, min = %d, def = %d, step = %d\n",
+			 __func__, index, id, max, min, def, step);
+
+		if (v4l2_ctrl_get_name(id)) {
+			*ctrl = v4l2_ctrl_new_std(ctrl_hdlr,
+						  &pivariety_ctrl_ops,
+						  id, min,
+						  max, step,
+						  def);
+			v4l2_dbg(1, debug, sd, "%s: ctrl: 0x%p\n",
+				 __func__, *ctrl);
+		} else if (pivariety_ctrl_get_name(id)) {
+			*ctrl = v4l2_ctrl_new_arducam(ctrl_hdlr,
+						      &pivariety_ctrl_ops,
+						      id, min, max, step, def);
+
+			v4l2_dbg(1, debug, sd,
+				 "%s: new custom ctrl, ctrl: 0x%p.\n",
+				 __func__, *ctrl);
+		} else {
+			index++;
+			continue;
+		}
+
+		if (!*ctrl)
+			goto err;
+
+		switch (id) {
+		case V4L2_CID_HFLIP:
+			pivariety->hflip = *ctrl;
+			if (pivariety->bayer_order_volatile)
+				pivariety->hflip->flags |=
+						V4L2_CTRL_FLAG_MODIFY_LAYOUT;
+			break;
+
+		case V4L2_CID_VFLIP:
+			pivariety->vflip = *ctrl;
+			if (pivariety->bayer_order_volatile)
+				pivariety->vflip->flags |=
+						V4L2_CTRL_FLAG_MODIFY_LAYOUT;
+			break;
+
+		case V4L2_CID_HBLANK:
+			(*ctrl)->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+			break;
+		}
+
+		ctrl++;
+		index++;
+	}
+
+	pivariety_write(pivariety, CTRL_INDEX_REG, 0);
+
+	ret = v4l2_fwnode_device_parse(&client->dev, &props);
+	if (ret)
+		goto err;
+
+	ret = v4l2_ctrl_new_fwnode_properties(ctrl_hdlr,
+					      &pivariety_ctrl_ops,
+					      &props);
+	if (ret)
+		goto err;
+
+	pivariety->sd.ctrl_handler = ctrl_hdlr;
+	v4l2_ctrl_handler_setup(ctrl_hdlr);
+	return 0;
+err:
+	return -ENODEV;
+}
+
+static int pivariety_parse_dt(struct pivariety *pivariety, struct device *dev)
+{
+	struct fwnode_handle *endpoint;
+	struct v4l2_fwnode_endpoint ep_cfg = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY
+	};
+	int ret = -EINVAL;
+
+	/* Get CSI2 bus config */
+	endpoint = fwnode_graph_get_next_endpoint(dev_fwnode(dev), NULL);
+	if (!endpoint) {
+		dev_err(dev, "endpoint node not found\n");
+		return -EINVAL;
+	}
+
+	if (v4l2_fwnode_endpoint_alloc_parse(endpoint, &ep_cfg)) {
+		dev_err(dev, "could not parse endpoint\n");
+		goto error_out;
+	}
+
+	pivariety->bus = ep_cfg.bus.mipi_csi2;
+
+	ret = 0;
+
+error_out:
+	v4l2_fwnode_endpoint_free(&ep_cfg);
+	fwnode_handle_put(endpoint);
+
+	return ret;
+}
+
+static int pivariety_probe(struct i2c_client *client,
+			   const struct i2c_device_id *id)
+{
+	struct device *dev = &client->dev;
+	struct pivariety *pivariety;
+	u32 device_id, firmware_version;
+	int ret;
+
+	pivariety = devm_kzalloc(&client->dev, sizeof(*pivariety), GFP_KERNEL);
+	if (!pivariety)
+		return -ENOMEM;
+
+	/* Initialize subdev */
+	v4l2_i2c_subdev_init(&pivariety->sd, client,
+			     &pivariety_subdev_ops);
+
+	if (pivariety_parse_dt(pivariety, dev))
+		return -EINVAL;
+
+	/* Get system clock (xclk) */
+	pivariety->xclk = devm_clk_get(dev, "xclk");
+	if (IS_ERR(pivariety->xclk)) {
+		dev_err(dev, "failed to get xclk\n");
+		return PTR_ERR(pivariety->xclk);
+	}
+
+	pivariety->xclk_freq = clk_get_rate(pivariety->xclk);
+	if (pivariety->xclk_freq != 24000000) {
+		dev_err(dev, "xclk frequency not supported: %d Hz\n",
+			pivariety->xclk_freq);
+		return -EINVAL;
+	}
+
+	ret = pivariety_get_regulators(pivariety);
+	if (ret)
+		return ret;
+
+	/* Request optional enable pin */
+	pivariety->reset_gpio = devm_gpiod_get_optional(dev, "reset",
+							GPIOD_OUT_HIGH);
+
+	ret = pivariety_power_on(dev);
+	if (ret)
+		return ret;
+
+	ret = pivariety_read(pivariety, DEVICE_ID_REG, &device_id);
+	if (ret || device_id != DEVICE_ID) {
+		dev_err(dev, "probe failed\n");
+		ret = -ENODEV;
+		goto error_power_off;
+	}
+
+	ret = pivariety_read(pivariety, DEVICE_VERSION_REG, &firmware_version);
+	if (ret)
+		dev_err(dev, "read firmware version failed\n");
+
+	dev_info(dev, "firmware version: 0x%04X\n", firmware_version);
+
+	if (pivariety_enum_pixformat(pivariety)) {
+		dev_err(dev, "enum pixformat failed.\n");
+		ret = -ENODEV;
+		goto error_power_off;
+	}
+
+	if (pivariety_enum_controls(pivariety)) {
+		dev_err(dev, "enum controls failed.\n");
+		ret = -ENODEV;
+		goto error_power_off;
+	}
+
+	/* Initialize subdev */
+	pivariety->sd.internal_ops = &pivariety_internal_ops;
+	pivariety->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	pivariety->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	/* Initialize source pad */
+	pivariety->pad.flags = MEDIA_PAD_FL_SOURCE;
+
+	ret = media_entity_pads_init(&pivariety->sd.entity, 1, &pivariety->pad);
+	if (ret)
+		goto error_handler_free;
+
+	ret = v4l2_async_register_subdev_sensor(&pivariety->sd);
+	if (ret < 0)
+		goto error_media_entity;
+
+	pm_runtime_set_active(dev);
+	pm_runtime_enable(dev);
+	pm_runtime_idle(dev);
+
+	return 0;
+
+error_media_entity:
+	media_entity_cleanup(&pivariety->sd.entity);
+
+error_handler_free:
+	pivariety_free_controls(pivariety);
+
+error_power_off:
+	pivariety_power_off(dev);
+
+	return ret;
+}
+
+static int pivariety_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct pivariety *pivariety = to_pivariety(sd);
+
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	pivariety_free_controls(pivariety);
+
+	pm_runtime_disable(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+
+	return 0;
+}
+
+static const struct dev_pm_ops pivariety_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(pivariety_suspend, pivariety_resume)
+	SET_RUNTIME_PM_OPS(pivariety_power_off, pivariety_power_on, NULL)
+};
+
+static const struct of_device_id arducam_pivariety_dt_ids[] = {
+	{ .compatible = "arducam,arducam-pivariety" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, arducam_pivariety_dt_ids);
+
+static struct i2c_driver arducam_pivariety_i2c_driver = {
+	.driver = {
+		.name = "arducam-pivariety",
+		.of_match_table	= arducam_pivariety_dt_ids,
+		.pm = &pivariety_pm_ops,
+	},
+	.probe = pivariety_probe,
+	.remove = pivariety_remove,
+};
+
+module_i2c_driver(arducam_pivariety_i2c_driver);
+
+MODULE_AUTHOR("Lee Jackson <info@arducam.com>");
+MODULE_DESCRIPTION("Arducam Pivariety v4l2 driver");
+MODULE_LICENSE("GPL v2");
--- /dev/null
+++ b/drivers/media/i2c/arducam-pivariety.h
@@ -0,0 +1,107 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef _ARDUCAM_PIVARIETY_H_
+#define _ARDUCAM_PIVARIETY_H_
+
+#define DEVICE_REG_BASE		0x0100
+#define PIXFORMAT_REG_BASE	0x0200
+#define FORMAT_REG_BASE		0x0300
+#define CTRL_REG_BASE		0x0400
+#define IPC_REG_BASE		0x0600
+
+#define ARDUCAM_MODE_STANDBY		0x00
+#define ARDUCAM_MODE_STREAMING		0x01
+
+#define MODE_SELECT_REG		(DEVICE_REG_BASE | 0x0000)
+#define DEVICE_VERSION_REG	(DEVICE_REG_BASE | 0x0001)
+#define SENSOR_ID_REG		(DEVICE_REG_BASE | 0x0002)
+#define DEVICE_ID_REG		(DEVICE_REG_BASE | 0x0003)
+#define SYSTEM_IDLE_REG		(DEVICE_REG_BASE | 0x0007)
+
+#define PIXFORMAT_INDEX_REG		(PIXFORMAT_REG_BASE | 0x0000)
+#define PIXFORMAT_TYPE_REG		(PIXFORMAT_REG_BASE | 0x0001)
+#define PIXFORMAT_ORDER_REG		(PIXFORMAT_REG_BASE | 0x0002)
+#define MIPI_LANES_REG			(PIXFORMAT_REG_BASE | 0x0003)
+#define FLIPS_DONT_CHANGE_ORDER_REG	(PIXFORMAT_REG_BASE | 0x0004)
+
+#define RESOLUTION_INDEX_REG	(FORMAT_REG_BASE | 0x0000)
+#define FORMAT_WIDTH_REG	(FORMAT_REG_BASE | 0x0001)
+#define FORMAT_HEIGHT_REG	(FORMAT_REG_BASE | 0x0002)
+
+#define CTRL_INDEX_REG	(CTRL_REG_BASE | 0x0000)
+#define CTRL_ID_REG	(CTRL_REG_BASE | 0x0001)
+#define CTRL_MIN_REG	(CTRL_REG_BASE | 0x0002)
+#define CTRL_MAX_REG	(CTRL_REG_BASE | 0x0003)
+#define CTRL_STEP_REG	(CTRL_REG_BASE | 0x0004)
+#define CTRL_DEF_REG	(CTRL_REG_BASE | 0x0005)
+#define CTRL_VALUE_REG	(CTRL_REG_BASE | 0x0006)
+
+#define IPC_SEL_TARGET_REG	(IPC_REG_BASE | 0x0000)
+#define IPC_SEL_TOP_REG		(IPC_REG_BASE | 0x0001)
+#define IPC_SEL_LEFT_REG	(IPC_REG_BASE | 0x0002)
+#define IPC_SEL_WIDTH_REG	(IPC_REG_BASE | 0x0003)
+#define IPC_SEL_HEIGHT_REG	(IPC_REG_BASE | 0x0004)
+#define IPC_DELAY_REG		(IPC_REG_BASE | 0x0005)
+
+#define NO_DATA_AVAILABLE	0xFFFFFFFE
+
+#define DEVICE_ID	0x0030
+
+#define I2C_READ_RETRY_COUNT	3
+#define I2C_WRITE_RETRY_COUNT	2
+
+#define V4L2_CID_ARDUCAM_BASE		(V4L2_CID_USER_BASE + 0x1000)
+#define V4L2_CID_ARDUCAM_EXT_TRI	(V4L2_CID_ARDUCAM_BASE + 1)
+#define V4L2_CID_ARDUCAM_IRCUT		(V4L2_CID_ARDUCAM_BASE + 8)
+
+enum image_dt {
+	IMAGE_DT_YUV420_8 = 0x18,
+	IMAGE_DT_YUV420_10,
+
+	IMAGE_DT_YUV420CSPS_8 = 0x1C,
+	IMAGE_DT_YUV420CSPS_10,
+	IMAGE_DT_YUV422_8,
+	IMAGE_DT_YUV422_10,
+	IMAGE_DT_RGB444,
+	IMAGE_DT_RGB555,
+	IMAGE_DT_RGB565,
+	IMAGE_DT_RGB666,
+	IMAGE_DT_RGB888,
+
+	IMAGE_DT_RAW6 = 0x28,
+	IMAGE_DT_RAW7,
+	IMAGE_DT_RAW8,
+	IMAGE_DT_RAW10,
+	IMAGE_DT_RAW12,
+	IMAGE_DT_RAW14,
+};
+
+enum bayer_order {
+	BAYER_ORDER_BGGR = 0,
+	BAYER_ORDER_GBRG = 1,
+	BAYER_ORDER_GRBG = 2,
+	BAYER_ORDER_RGGB = 3,
+	BAYER_ORDER_GRAY = 4,
+};
+
+enum yuv_order {
+	YUV_ORDER_YUYV = 0,
+	YUV_ORDER_YVYU = 1,
+	YUV_ORDER_UYVY = 2,
+	YUV_ORDER_VYUY = 3,
+};
+
+struct arducam_resolution {
+	u32 width;
+	u32 height;
+};
+
+struct arducam_format {
+	u32 index;
+	u32 mbus_code;
+	u32 bayer_order;
+	u32 data_type;
+	u32 num_resolution_set;
+	struct arducam_resolution *resolution_set;
+};
+
+#endif