[PATCH v6 10/10] media: i2c: Add driver for mlx7502x ToF sensor

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

 



Melexis ToF 7502x sensors has a CSI-2 output. It supports 2 and 4 lanes,
and mipi speeds are 300, 600, 704, 800, 904, 960Mbs.
Supported format is RAW12.
Sensor 75026 is QVGA, while 75027 is VGA sensor.

The driver supports:
- Digital binning and cropping
- Horizontal and vertical flip
- phase shifts(up to 8)
- frequency modulation
- time integration
- single trigger mode

Signed-off-by: Volodymyr Kharuk <vkh@xxxxxxxxxxx>
Signed-off-by: Andrii Kyselov <ays@xxxxxxxxxxx>
---
 MAINTAINERS                  |    1 +
 drivers/media/i2c/Kconfig    |   13 +
 drivers/media/i2c/Makefile   |    1 +
 drivers/media/i2c/mlx7502x.c | 1845 ++++++++++++++++++++++++++++++++++
 4 files changed, 1860 insertions(+)
 create mode 100644 drivers/media/i2c/mlx7502x.c

diff --git a/MAINTAINERS b/MAINTAINERS
index 02a764f52a55..9cd630ef89d4 100644
--- a/MAINTAINERS
+++ b/MAINTAINERS
@@ -14020,6 +14020,7 @@ W:	http://www.melexis.com
 F:	Documentation/devicetree/bindings/media/i2c/melexis,mlx7502x.yaml
 F:	Documentation/userspace-api/media/drivers/mlx7502x.rst
 F:	Documentation/userspace-api/media/v4l/ext-ctrls-tof.rst
+F:	drivers/media/i2c/mlx7502x.c
 F:	include/uapi/linux/mlx7502x.h
 
 MELFAS MIP4 TOUCHSCREEN DRIVER
diff --git a/drivers/media/i2c/Kconfig b/drivers/media/i2c/Kconfig
index c6d3ee472d81..3199a9be4d5e 100644
--- a/drivers/media/i2c/Kconfig
+++ b/drivers/media/i2c/Kconfig
@@ -236,6 +236,19 @@ config VIDEO_IMX415
 config VIDEO_MAX9271_LIB
 	tristate
 
+config VIDEO_MLX7502X
+	tristate "Melexis ToF 75026 and 75027 sensors support"
+	depends on I2C && VIDEO_DEV
+	select MEDIA_CONTROLLER
+	select VIDEO_V4L2_SUBDEV_API
+	select V4L2_FWNODE
+	help
+	  This is a V4L2 sensor driver for the Melexis 75026 and 75027
+	  ToF sensors.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called mlx7502x.
+
 config VIDEO_MT9M001
 	tristate "mt9m001 support"
 	help
diff --git a/drivers/media/i2c/Makefile b/drivers/media/i2c/Makefile
index dfbe6448b549..d87f4d8e7284 100644
--- a/drivers/media/i2c/Makefile
+++ b/drivers/media/i2c/Makefile
@@ -65,6 +65,7 @@ obj-$(CONFIG_VIDEO_M52790) += m52790.o
 obj-$(CONFIG_VIDEO_MAX9271_LIB) += max9271.o
 obj-$(CONFIG_VIDEO_MAX9286) += max9286.o
 obj-$(CONFIG_VIDEO_ML86V7667) += ml86v7667.o
+obj-$(CONFIG_VIDEO_MLX7502X) += mlx7502x.o
 obj-$(CONFIG_VIDEO_MSP3400) += msp3400.o
 obj-$(CONFIG_VIDEO_MT9M001) += mt9m001.o
 obj-$(CONFIG_VIDEO_MT9M111) += mt9m111.o
diff --git a/drivers/media/i2c/mlx7502x.c b/drivers/media/i2c/mlx7502x.c
new file mode 100644
index 000000000000..4843c3f39e7a
--- /dev/null
+++ b/drivers/media/i2c/mlx7502x.c
@@ -0,0 +1,1845 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * A V4L2 driver for Melexis 7502x ToF cameras.
+ *
+ * Copyright (C) 2022 Melexis N.V.
+ *
+ */
+
+#include <asm-generic/unaligned.h>
+#include <linux/bitfield.h>
+#include <linux/bsearch.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/mlx7502x.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <linux/units.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-image-sizes.h>
+
+#define MLX7502X_PARAM_HOLD_REG       0x0102
+#define MLX7502X_HMAX_REG             0x0800
+#define MLX7502X_COLUMN_START_REG     0x0804
+#define MLX7502X_COLUMN_LEN_REG       0x0806
+#define MLX7502X_ROW_START_REG        0x0808
+#define MLX7502X_ROW_END_REG          0x080a
+#define MLX7502X_VFLIP_REG            0x080c
+#define MLX7502X_HFLIP_REG            0x080d
+#define MLX7502X_OUTPUT_MODE_REG      0x0828
+#define MLX7502X_STREAM_EN_REG        0x1001
+#define MLX7502X_DATA_LANE_CONFIG_REG 0x1010
+#define MLX7502X_FMOD_REG             0x1048
+#define MLX7502X_PLL_RES_REG          0x104b
+#define MLX7502X_BINNING_REG          0x14a5
+#define MLX7502X_CONTINUOUS_REG       0x1c40
+#define MLX7502X_SW_TRIGGER_REG       0x2100
+#define MLX7502X_FRAME_TIME_REG       0x2108
+#define MLX7502X_TINT0_REG            0x2120
+#define MLX7502X_PX_PHASE_SHIFT_REG   0x21b4
+#define MLX7502X_DIVSELPRE_REG        0x21be
+#define MLX7502X_DIVSEL_REG           0x21bf
+#define MLX7502X_PHASE_COUNT_REG      0x21e8
+#define MLX7502X_PLLSETUP_REG         0x4010
+#define MLX7502X_PRETIME_REG          0x4015
+#define MLX7502X_RANDNM0_REG          0x5265
+
+#define MLX7502X_SW_TRIGGER_DEFAULT 0x0000
+#define MLX7502X_SW_TRIGGER_TRIG 0x0001
+#define MLX7502X_SW_TRIGGER_CONT 0x0008
+
+#define MLX7502X_NIBLE_LOW_MASK		GENMASK(3, 0)
+#define MLX7502X_NIBLE_HIGH_MASK	GENMASK(7, 4)
+
+#define MLX7502X_PHASE_MAX_NUM 8
+
+#define MLX7502X_CLK_RATE_MHZ 120
+#define MLX7502X_XCLK_RATE (8 * HZ_PER_MHZ)
+
+#define MLX7502X_RESET_DELAY_MS 100
+#define MLX7502X_STREAMING_DELAY_MS 2
+
+#define MLX7502X_LINK_FREQ_REG_N 8
+#define MLX7502X_LINK_FREQ_N 6
+#define MLX7502X_LANE_N 2
+#define MLX7502X_OUTPUT_MODE_N 2
+
+#define MLX7502X_PLLSETUP_US 503
+#define MLX7502X_PLLSETUP_TICKS 8
+#define MLX7502X_PRETIME_US 50
+#define MLX7502X_FRAME_ADD_TICKS 13
+
+#define MLX7502X_DEFAULT_FRAME_RATE 25
+
+#define MLX7502X_ROW_START(top) ((top) / 2)
+#define MLX7502X_ROW_END(top, height) ((((top) + (height)) / 2) + 1)
+
+#define MLX7502X_LEFT_ALIGN 0
+#define MLX7502X_LEFT_MIN 0
+
+#define MLX7502X_TOP_ALIGN 1
+#define MLX7502X_TOP_MIN 0
+
+#define MLX7502X_PLL_RES_THR 113
+
+static const char * const mlx7502x_supply_names[] = {
+	"vdda",		/* Analog (2.7V) supply */
+	"vddif",	/* I/O (1.8V) supply */
+	"vddd",		/* Digital core (1.2V) supply */
+	"vdmix",	/* MIX driver (1.2V) supply */
+};
+
+#define MLX7502X_NUM_SUPPLIES ARRAY_SIZE(mlx7502x_supply_names)
+
+struct mlx7502x_regval_list {
+	u16 addr;
+	u8 data;
+};
+
+struct mlx7502x_link_freq_list {
+	s64 link_freq[MLX7502X_LINK_FREQ_N];
+	u16 addr[MLX7502X_LINK_FREQ_REG_N];
+	u8 data[MLX7502X_LANE_N][MLX7502X_LINK_FREQ_N][MLX7502X_LINK_FREQ_REG_N];
+	u16 hmax[MLX7502X_LANE_N][MLX7502X_LINK_FREQ_N][MLX7502X_OUTPUT_MODE_N];
+};
+
+struct mlx7502x_binning_mode {
+	u8 reg_value;
+	u8 ratio;
+	u8 width_align;
+	u8 height_align;
+	u8 width_min;
+	u8 height_min;
+};
+
+struct mlx7502x_output_mode {
+	u8 hmax_index;
+	u8 width_mult;
+};
+
+/* configuration of divider for specific fmod */
+struct mlx7502x_fmod_list {
+	u32 fmod;
+	u8 divselpre;
+	u8 divsel;
+};
+
+struct mlx7502x_sensor_desc {
+	const struct mlx7502x_regval_list *init_cfg;
+	u32 init_cfg_size;
+	const struct mlx7502x_link_freq_list *link_freq_cfg;
+	u32 width;
+	u32 height;
+};
+
+struct mlx7502x {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct v4l2_fwnode_endpoint ep;
+	struct device *dev;
+
+	/* controls */
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *trigger;
+	struct v4l2_ctrl *trigger_mode;
+	struct v4l2_ctrl *tint;
+	struct v4l2_ctrl *phase_seq;
+	struct v4l2_ctrl *output_mode;
+
+	struct clk *xclk;
+	struct regulator_bulk_data supplies[MLX7502X_NUM_SUPPLIES];
+
+	/* pins */
+	struct gpio_desc *reset;
+
+	const struct mlx7502x_sensor_desc *cur_desc;
+	const struct mlx7502x_binning_mode *binning_mode;
+	int streaming;
+	u16 hmax; /* internal sensor frame size in ticks */
+};
+
+static const struct mlx7502x_regval_list mlx7502x_common_init_cfg[] = {
+	{ 0x1006, 0x08 }, { 0x1007, 0x00 }, { 0x1040, 0x00 }, { 0x1041, 0x96 },
+	{ 0x1042, 0x01 }, { 0x1043, 0x00 }, { 0x1044, 0x00 }, { 0x1046, 0x01 },
+	{ 0x104a, 0x01 }, { 0x1000, 0x00 }, { 0x10d3, 0x10 }, { 0x1448, 0x06 },
+	{ 0x1449, 0x40 }, { 0x144a, 0x06 }, { 0x144b, 0x40 }, { 0x144c, 0x06 },
+	{ 0x144d, 0x40 }, { 0x144e, 0x06 }, { 0x144f, 0x40 }, { 0x1450, 0x06 },
+	{ 0x1451, 0x40 }, { 0x1452, 0x06 }, { 0x1453, 0x40 }, { 0x1454, 0x06 },
+	{ 0x1455, 0x40 }, { 0x1456, 0x06 }, { 0x1457, 0x40 }, { 0x2203, 0x1e },
+	{ 0x2c08, 0x01 }, { 0x3c2b, 0x1b }, { 0x400e, 0x01 }, { 0x400f, 0x81 },
+	{ 0x40d1, 0x00 }, { 0x40d2, 0x00 }, { 0x40d3, 0x00 }, { 0x40db, 0x3f },
+	{ 0x40de, 0x40 }, { 0x40df, 0x01 }, { 0x4134, 0x04 }, { 0x4135, 0x04 },
+	{ 0x4136, 0x04 }, { 0x4137, 0x04 }, { 0x4138, 0x04 }, { 0x4139, 0x04 },
+	{ 0x413a, 0x04 }, { 0x413b, 0x04 }, { 0x413c, 0x04 }, { 0x4146, 0x01 },
+	{ 0x4147, 0x01 }, { 0x4148, 0x01 }, { 0x4149, 0x01 }, { 0x414a, 0x01 },
+	{ 0x414b, 0x01 }, { 0x414c, 0x01 }, { 0x414d, 0x01 }, { 0x4158, 0x01 },
+	{ 0x4159, 0x01 }, { 0x415a, 0x01 }, { 0x415b, 0x01 }, { 0x415c, 0x01 },
+	{ 0x415d, 0x01 }, { 0x415e, 0x01 }, { 0x415f, 0x01 }, { 0x4590, 0x00 },
+	{ 0x4591, 0x2e }, { 0x4684, 0x00 }, { 0x4685, 0xa0 }, { 0x4687, 0xa1 },
+	{ 0x471e, 0x07 }, { 0x471f, 0xc9 }, { 0x473a, 0x07 }, { 0x473b, 0xc9 },
+	{ 0x4770, 0x00 }, { 0x4771, 0x00 }, { 0x4772, 0x1f }, { 0x4773, 0xff },
+	{ 0x4778, 0x06 }, { 0x4779, 0xa4 }, { 0x477a, 0x07 }, { 0x477b, 0xae },
+	{ 0x4788, 0x06 }, { 0x4789, 0xa4 }, { 0x478c, 0x1f }, { 0x478d, 0xff },
+	{ 0x478e, 0x00 }, { 0x478f, 0x00 }, { 0x4792, 0x00 }, { 0x4793, 0x00 },
+	{ 0x4796, 0x00 }, { 0x4797, 0x00 }, { 0x479a, 0x00 }, { 0x479b, 0x00 },
+	{ 0x479c, 0x1f }, { 0x479d, 0xff }, { 0x479e, 0x00 }, { 0x479f, 0x00 },
+	{ 0x47a2, 0x00 }, { 0x47a3, 0x00 }, { 0x47a6, 0x00 }, { 0x47a7, 0x00 },
+	{ 0x47aa, 0x00 }, { 0x47ab, 0x00 }, { 0x47ac, 0x1f }, { 0x47ad, 0xff },
+	{ 0x47ae, 0x00 }, { 0x47af, 0x00 }, { 0x47b2, 0x00 }, { 0x47b3, 0x00 },
+	{ 0x47b6, 0x00 }, { 0x47b7, 0x00 }, { 0x47ba, 0x00 }, { 0x47bb, 0x00 },
+	{ 0x47bc, 0x1f }, { 0x47bd, 0xff }, { 0x47be, 0x00 }, { 0x47bf, 0x00 },
+	{ 0x47c2, 0x00 }, { 0x47c3, 0x00 }, { 0x47c6, 0x00 }, { 0x47c7, 0x00 },
+	{ 0x47ca, 0x00 }, { 0x47cb, 0x00 }, { 0x4834, 0x00 }, { 0x4835, 0xa0 },
+	{ 0x4837, 0xa1 }, { 0x4878, 0x00 }, { 0x4879, 0xa0 }, { 0x487b, 0xa1 },
+	{ 0x48bc, 0x00 }, { 0x48bd, 0xa0 }, { 0x48bf, 0xa1 }, { 0x49ff, 0x78 },
+	{ 0x4baf, 0x1a }, { 0x4bc7, 0x1a }, { 0x4d2a, 0x07 }, { 0x4d80, 0x06 },
+	{ 0x4d81, 0xa4 }, { 0x4d82, 0x07 }, { 0x4e39, 0x07 }, { 0x4e7b, 0x64 },
+	{ 0x4e8e, 0x0e }, { 0x4e9c, 0x01 }, { 0x4ea0, 0x01 }, { 0x4ea1, 0x03 },
+	{ 0x4ea5, 0x00 }, { 0x4ea7, 0x00 }, { 0x4f05, 0x04 }, { 0x4f0d, 0x04 },
+	{ 0x4f15, 0x04 }, { 0x4f19, 0x01 }, { 0x4f20, 0x01 }, { 0x4f66, 0x0f },
+	{ 0x500f, 0x01 }, { 0x5225, 0x2f }, { 0x5227, 0x1e }, { 0x5231, 0x19 },
+	{ 0x5245, 0x07 }, { 0x5252, 0x07 }, { 0x5253, 0x08 }, { 0x5254, 0x07 },
+	{ 0x5255, 0xb4 }, { 0x5272, 0x04 }, { 0x5273, 0x2e }, { 0x5282, 0x04 },
+	{ 0x5283, 0x2e }, { 0x5286, 0x00 }, { 0x5287, 0x5d }, { 0x1433, 0x00 },
+	{ 0x3c18, 0x00 }, { 0x100e, 0x00 }, { 0x100f, 0x00 }
+};
+
+static const struct mlx7502x_regval_list mlx7502x_75027_init_cfg[] = {
+	{ 0x477d, 0xd6 }, { 0x4954, 0x00 }, { 0x4955, 0xa0 }, { 0x4957, 0xa1 },
+	{ 0x4984, 0x00 }, { 0x4985, 0xa0 }, { 0x4987, 0xa1 }, { 0x49b9, 0x78 },
+	{ 0x49c3, 0x3c }, { 0x49c9, 0x76 }, { 0x49d3, 0x3f }, { 0x49dc, 0x00 },
+	{ 0x49dd, 0xa0 }, { 0x49df, 0xa1 }, { 0x49ef, 0x78 }, { 0x49f9, 0x3c },
+	{ 0x4a05, 0x3c }, { 0x4a0b, 0x76 }, { 0x4a11, 0x3f }, { 0x4a1a, 0x00 },
+	{ 0x4a1b, 0xa0 }, { 0x4a1d, 0xa1 }, { 0x4a1f, 0x78 }, { 0x4a29, 0x3c },
+	{ 0x4a4a, 0x00 }, { 0x4a4b, 0xa0 }, { 0x4a4d, 0xa1 }, { 0x4a7a, 0x00 },
+	{ 0x4a7b, 0xa0 }, { 0x4a7d, 0xa1 }, { 0x4aee, 0x00 }, { 0x4aef, 0xa0 },
+	{ 0x4af1, 0xa1 }, { 0x4b2e, 0x00 }, { 0x4b2f, 0xa0 }, { 0x4b31, 0xa1 },
+	{ 0x4b5a, 0x00 }, { 0x4b5b, 0xa0 }, { 0x4b5d, 0xa1 }, { 0x4b86, 0x00 },
+	{ 0x4b87, 0xa0 }, { 0x4b89, 0xa1 }, { 0x4b9f, 0x1a }, { 0x4bb7, 0x1a },
+	{ 0x4bcf, 0x1a }, { 0x4bee, 0x00 }, { 0x4bef, 0xa0 }, { 0x4bf1, 0xa1 },
+	{ 0x4bf7, 0x1a }, { 0x4c01, 0x1a }, { 0x4c58, 0x00 }, { 0x4c59, 0xa0 },
+	{ 0x4c5b, 0xa1 }, { 0x4c6e, 0x00 }, { 0x4c6f, 0xa0 }, { 0x4c71, 0xa1 },
+	{ 0x4c7a, 0x01 }, { 0x4c7b, 0x35 }, { 0x4cf2, 0x07 }, { 0x4cf3, 0xc9 },
+	{ 0x4cf8, 0x06 }, { 0x4cf9, 0x9b }, { 0x4cfa, 0x07 }, { 0x4cfb, 0xae },
+	{ 0x4cfe, 0x07 }, { 0x4cff, 0xc9 }, { 0x4d04, 0x06 }, { 0x4d05, 0x98 },
+	{ 0x4d06, 0x07 }, { 0x4d07, 0xb1 }, { 0x4d18, 0x06 }, { 0x4d19, 0xa4 },
+	{ 0x4d1a, 0x07 }, { 0x4d1b, 0x49 }, { 0x4d1e, 0x07 }, { 0x4d1f, 0xc9 },
+	{ 0x4d2b, 0xc9 }, { 0x4d4a, 0x07 }, { 0x4d4b, 0xc9 }, { 0x4d50, 0x06 },
+	{ 0x4d51, 0x9b }, { 0x4d52, 0x07 }, { 0x4d53, 0xae }, { 0x4d56, 0x07 },
+	{ 0x4d57, 0xc9 }, { 0x4d5c, 0x06 }, { 0x4d5d, 0x98 }, { 0x4d5e, 0x07 },
+	{ 0x4d5f, 0xb1 }, { 0x4d70, 0x06 }, { 0x4d71, 0xa4 }, { 0x4d72, 0x07 },
+	{ 0x4d73, 0x49 }, { 0x4d78, 0x06 }, { 0x4d79, 0xa4 }, { 0x4d7a, 0x07 },
+	{ 0x4d7b, 0xae }, { 0x4d7c, 0x1f }, { 0x4d7d, 0xff }, { 0x4d7e, 0x1f },
+	{ 0x4d7f, 0xff }, { 0x4d83, 0xae }, { 0x4d84, 0x1f }, { 0x4d85, 0xff },
+	{ 0x4d86, 0x1f }, { 0x4d87, 0xff },
+};
+
+static const struct mlx7502x_link_freq_list mlx75027_link_freq_cfg = {
+	.link_freq = {
+		300000000,
+		600000000,
+		704000000,
+		800000000,
+		904000000,
+		960000000
+	},
+	.addr = {
+		0x100c, 0x100d, 0x1016, 0x1017, 0x1045, 0x1047, 0x1060, 0x1071
+	},
+	.data = {
+		{ /* lane 2 */
+			/* 300MBps */
+			{ 0x02, 0x58, 0x09, 0x99, 0x4b, 0x02, 0x01, 0x0c },
+			/* 600MBps */
+			{ 0x04, 0xb0, 0x04, 0xcc, 0x4b, 0x02, 0x00, 0x06 },
+			/* 704MBps */
+			{ 0x05, 0x80, 0x04, 0x17, 0x58, 0x02, 0x00, 0x06 },
+			/* 800MBps */
+			{ 0x06, 0x40, 0x03, 0x99, 0x64, 0x02, 0x00, 0x06 },
+			/* 904MBps */
+			{ 0x07, 0x10, 0x03, 0x2f, 0x71, 0x00, 0x00, 0x06 },
+			/* 960MBps */
+			{ 0x07, 0x80, 0x03, 0x00, 0x78, 0x02, 0x00, 0x06 },
+		}, { /* lane 4 */
+			/* 300MBps */
+			{ 0x04, 0xb0, 0x09, 0x99, 0x4b, 0x02, 0x01, 0x0c },
+			/* 600MBps */
+			{ 0x09, 0x60, 0x04, 0xcc, 0x4b, 0x02, 0x00, 0x06 },
+			/* 704MBps */
+			{ 0x0b, 0x00, 0x04, 0x17, 0x58, 0x02, 0x00, 0x06 },
+			/* 800MBps */
+			{ 0x0c, 0x80, 0x03, 0x99, 0x64, 0x02, 0x00, 0x06 },
+			/* 904MBps */
+			{ 0x0e, 0x20, 0x03, 0x2f, 0x71, 0x00, 0x00, 0x06 },
+			/* 960MBps */
+			{ 0x0f, 0x00, 0x03, 0x00, 0x78, 0x00, 0x00, 0x06 },
+		},
+	},
+	.hmax = {
+		{ /* lane 2 */
+			{ 0x0e78, 0x1a80 }, /* 300MBps */
+			{ 0x0750, 0x0d54 }, /* 600MBps */
+			{ 0x0640, 0x0b60 }, /* 704MBps */
+			{ 0x0584, 0x0a06 }, /* 800MBps */
+			{ 0x04e8, 0x08e6 }, /* 904MBps */
+			{ 0x049e, 0x0860 }, /* 960MBps */
+		}, { /* lane 4 */
+			{ 0x0860, 0x0e60 }, /* 300MBps */
+			{ 0x0444, 0x0744 }, /* 600MBps */
+			{ 0x03a8, 0x0636 }, /* 704MBps */
+			{ 0x033a, 0x057a }, /* 800MBps */
+			{ 0x02e2, 0x0514 }, /* 904MBps */
+			{ 0x02b6, 0x0514 }, /* 960MBps */
+		},
+	}
+};
+
+/* VGA sensor */
+static const struct mlx7502x_sensor_desc mlx75027 = {
+	.init_cfg = mlx7502x_75027_init_cfg,
+	.init_cfg_size = ARRAY_SIZE(mlx7502x_75027_init_cfg),
+	.link_freq_cfg = &mlx75027_link_freq_cfg,
+	.width = VGA_WIDTH,
+	.height = VGA_HEIGHT,
+};
+
+static const struct mlx7502x_regval_list mlx7502x_75026_init_cfg[] = {
+	{ 0x477c, 0x0a }, { 0x477d, 0xd4 }, { 0x4964, 0x00 }, { 0x4965, 0xa0 },
+	{ 0x4967, 0xa1 }, { 0x4994, 0x00 }, { 0x4995, 0xa0 }, { 0x4997, 0xa1 },
+	{ 0x49c9, 0x78 }, { 0x49d3, 0x3c }, { 0x49d9, 0x76 }, { 0x49e3, 0x3f },
+	{ 0x49ec, 0x00 }, { 0x49ed, 0xa0 }, { 0x49ef, 0xa1 }, { 0x4a09, 0x3c },
+	{ 0x4a0f, 0x78 }, { 0x4a15, 0x3c }, { 0x4a1b, 0x76 }, { 0x4a21, 0x3f },
+	{ 0x4a2a, 0x00 }, { 0x4a2b, 0xa0 }, { 0x4a2d, 0xa1 }, { 0x4a2f, 0x78 },
+	{ 0x4a39, 0x3c }, { 0x4a5a, 0x00 }, { 0x4a5b, 0xa0 }, { 0x4a5d, 0xa1 },
+	{ 0x4a8a, 0x00 }, { 0x4a8b, 0xa0 }, { 0x4a8d, 0xa1 }, { 0x4afe, 0x00 },
+	{ 0x4aff, 0xa0 }, { 0x4b01, 0xa1 }, { 0x4b3e, 0x00 }, { 0x4b3f, 0xa0 },
+	{ 0x4b41, 0xa1 }, { 0x4b6a, 0x00 }, { 0x4b6b, 0xa0 }, { 0x4b6d, 0xa1 },
+	{ 0x4b96, 0x00 }, { 0x4b97, 0xa0 }, { 0x4b99, 0xa1 }, { 0x4bbf, 0x1a },
+	{ 0x4bd7, 0x1a }, { 0x4bdf, 0x1a }, { 0x4bfe, 0x00 }, { 0x4bff, 0xa0 },
+	{ 0x4c01, 0xa1 }, { 0x4c07, 0x1a }, { 0x4c11, 0x1a }, { 0x4c68, 0x00 },
+	{ 0x4c69, 0xa0 }, { 0x4c6b, 0xa1 }, { 0x4c7e, 0x00 }, { 0x4c7f, 0xa0 },
+	{ 0x4c81, 0xa1 }, { 0x4c8a, 0x01 }, { 0x4c8b, 0x35 }, { 0x4d02, 0x07 },
+	{ 0x4d03, 0xc9 }, { 0x4d08, 0x06 }, { 0x4d09, 0x9b }, { 0x4d0a, 0x07 },
+	{ 0x4d0b, 0xae }, { 0x4d0e, 0x07 }, { 0x4d0f, 0xc9 }, { 0x4d14, 0x06 },
+	{ 0x4d15, 0x98 }, { 0x4d16, 0x07 }, { 0x4d17, 0xb1 }, { 0x4d28, 0x06 },
+	{ 0x4d29, 0xa4 }, { 0x4d2b, 0xa9 }, { 0x4d2e, 0x07 }, { 0x4d2f, 0xc9 },
+	{ 0x4d3a, 0x07 }, { 0x4d3b, 0xc9 }, { 0x4d5a, 0x07 }, { 0x4d5b, 0xc9 },
+	{ 0x4d60, 0x06 }, { 0x4d61, 0x9b }, { 0x4d62, 0x07 }, { 0x4d63, 0xae },
+	{ 0x4d66, 0x07 }, { 0x4d67, 0xc9 }, { 0x4d6c, 0x06 }, { 0x4d6d, 0x98 },
+	{ 0x4d6e, 0x07 }, { 0x4d6f, 0xb1 }, { 0x4d83, 0xa9 }, { 0x4d88, 0x06 },
+	{ 0x4d89, 0xa4 }, { 0x4d8a, 0x07 }, { 0x4d8b, 0xae }, { 0x4d8c, 0x1f },
+	{ 0x4d8d, 0xff }, { 0x4d8e, 0x1f }, { 0x4d8f, 0xff }, { 0x4d90, 0x06 },
+	{ 0x4d91, 0xa4 }, { 0x4d92, 0x07 }, { 0x4d93, 0xae }, { 0x4d94, 0x1f },
+	{ 0x4d95, 0xff }, { 0x4d96, 0x1f }, { 0x4d97, 0xff },
+};
+
+static const struct mlx7502x_link_freq_list mlx75026_link_freq_cfg = {
+	.link_freq = {
+		300000000,
+		600000000,
+		704000000,
+		800000000,
+		904000000,
+		960000000
+	},
+	.addr = {
+		0x100c, 0x100d, 0x1016, 0x1017, 0x1045, 0x1047, 0x1060, 0x1071
+	},
+	.data = {
+		{ /* lane 2 */
+			/* 300MBps */
+			{ 0x02, 0x58, 0x09, 0x99, 0x4b, 0x02, 0x01, 0x0c },
+			/* 600MBps */
+			{ 0x04, 0xb0, 0x04, 0xcc, 0x4b, 0x02, 0x00, 0x06 },
+			/* 704MBps */
+			{ 0x05, 0x80, 0x04, 0x17, 0x58, 0x02, 0x00, 0x06 },
+			/* 800MBps */
+			{ 0x06, 0x40, 0x03, 0x99, 0x64, 0x02, 0x00, 0x06 },
+			/* 904MBps */
+			{ 0x07, 0x10, 0x03, 0x2f, 0x71, 0x00, 0x00, 0x06 },
+			/* 960MBps */
+			{ 0x07, 0x80, 0x03, 0x00, 0x78, 0x02, 0x00, 0x06 },
+		}, { /* lane 4 */
+			/* 300MBps */
+			{ 0x04, 0xb0, 0x09, 0x99, 0x4b, 0x02, 0x01, 0x0c },
+			/* 600MBps */
+			{ 0x09, 0x60, 0x04, 0xcc, 0x4b, 0x02, 0x00, 0x06 },
+			/* 704MBps */
+			{ 0x0b, 0x00, 0x04, 0x17, 0x58, 0x02, 0x00, 0x06 },
+			/* 800MBps */
+			{ 0x0c, 0x80, 0x03, 0x99, 0x64, 0x02, 0x00, 0x06 },
+			/* 904MBps */
+			{ 0x0e, 0x20, 0x03, 0x2f, 0x71, 0x00, 0x00, 0x06 },
+			/* 960MBps */
+			{ 0x0f, 0x00, 0x03, 0x00, 0x78, 0x00, 0x00, 0x06 },
+		},
+	},
+	.hmax = {
+		{ /* lane 2 */
+			{ 0x0878, 0x0e80 }, /* 300MBps */
+			{ 0x0450, 0x0754 }, /* 600MBps */
+			{ 0x03b2, 0x0644 }, /* 704MBps */
+			{ 0x0344, 0x0586 }, /* 800MBps */
+			{ 0x02ea, 0x0514 }, /* 904MBps */
+			{ 0x02be, 0x0514 }, /* 960MBps */
+		}, { /* lane 4 */
+			{ 0x0560, 0x0860 }, /* 300MBps */
+			{ 0x02c4, 0x0444 }, /* 600MBps */
+			{ 0x02b6, 0x03a8 }, /* 704MBps */
+			{ 0x02b6, 0x033a }, /* 800MBps */
+			{ 0x02b6, 0x02e2 }, /* 904MBps */
+			{ 0x02b6, 0x02b6 }, /* 960MBps */
+		},
+	}
+};
+
+/* QVGA sensor */
+static const struct mlx7502x_sensor_desc mlx75026 = {
+	.init_cfg = mlx7502x_75026_init_cfg,
+	.init_cfg_size = ARRAY_SIZE(mlx7502x_75026_init_cfg),
+	.link_freq_cfg = &mlx75026_link_freq_cfg,
+	.width = QVGA_WIDTH,
+	.height = QVGA_HEIGHT,
+};
+
+static const struct mlx7502x_binning_mode binning_mode[] = {
+	{ .reg_value = 0, .ratio = 1, .width_align = 2, .height_align = 1,
+	  .width_min = 8, .height_min = 2 },
+	{ .reg_value = 1, .ratio = 2, .width_align = 3, .height_align = 1,
+	  .width_min = 16, .height_min = 2 },
+	{ .reg_value = 2, .ratio = 4, .width_align = 4, .height_align = 2,
+	  .width_min = 32, .height_min = 4 },
+	{ .reg_value = 3, .ratio = 8, .width_align = 5, .height_align = 3,
+	  .width_min = 64, .height_min = 8 },
+};
+
+static const struct mlx7502x_fmod_list mlx7502x_fmod_cfg[] = {
+	{ .fmod = 100000000, .divselpre = 0, .divsel = 0, },
+	{ .fmod = 75000000,  .divselpre = 0, .divsel = 0, },
+	{ .fmod = 74000000,  .divselpre = 1, .divsel = 0, },
+	{ .fmod = 51000000,  .divselpre = 1, .divsel = 0, },
+	{ .fmod = 50000000,  .divselpre = 0, .divsel = 1, },
+	{ .fmod = 38000000,  .divselpre = 0, .divsel = 1, },
+	{ .fmod = 37000000,  .divselpre = 1, .divsel = 1, },
+	{ .fmod = 21000000,  .divselpre = 1, .divsel = 1, },
+	{ .fmod = 2000000,   .divselpre = 0, .divsel = 2, },
+	{ .fmod = 1000000,   .divselpre = 0, .divsel = 2, },
+	{ .fmod = 1000000,   .divselpre = 1, .divsel = 2, },
+	{ .fmod = 1000000,   .divselpre = 1, .divsel = 2, },
+	{ .fmod = 900000,    .divselpre = 2, .divsel = 2, },
+	{ .fmod = 500000,    .divselpre = 2, .divsel = 2, },
+	{ .fmod = 400000,    .divselpre = 3, .divsel = 2, },
+};
+
+static const struct mlx7502x_regval_list mlx7502x_trigger_cfg[] = {
+	{ 0x2020, 0x01 }, { 0x2f05, 0x01 }, { 0x2f06, 0x09 },
+	{ 0x2f07, 0x7a }, { 0x3071, 0x00 },
+};
+
+static const struct mlx7502x_regval_list mlx7502x_ext_trigger_cfg[] = {
+	{ 0x2020, 0x00 }, { 0x2f05, 0x07 }, { 0x2f06, 0x00 },
+	{ 0x2f07, 0x00 }, { 0x3071, 0x03 },
+};
+
+static const char * const mlx7502x_ctrl_output[] = {
+	[V4L2_MLX7502X_OUTPUT_MODE_A_MINUS_B] = "A minus B",
+	[V4L2_MLX7502X_OUTPUT_MODE_A_PLUS_B] = "A plus B",
+	[V4L2_MLX7502X_OUTPUT_MODE_A] = "Raw A",
+	[V4L2_MLX7502X_OUTPUT_MODE_B] = "Raw B",
+	[V4L2_MLX7502X_OUTPUT_MODE_A_AND_B] = "Raw A and Raw B",
+};
+
+static const struct mlx7502x_output_mode mlx7502x_output_mode_cfg[] = {
+	[V4L2_MLX7502X_OUTPUT_MODE_A_MINUS_B] = {
+		.hmax_index = 0, .width_mult = 1
+	},
+	[V4L2_MLX7502X_OUTPUT_MODE_A_PLUS_B] = {
+		.hmax_index = 0, .width_mult = 1
+	},
+	[V4L2_MLX7502X_OUTPUT_MODE_A] = {
+		.hmax_index = 0, .width_mult = 1
+	},
+	[V4L2_MLX7502X_OUTPUT_MODE_B] = {
+		.hmax_index = 0, .width_mult = 1
+	},
+	[V4L2_MLX7502X_OUTPUT_MODE_A_AND_B] = {
+		.hmax_index = 1, .width_mult = 2
+	}
+};
+
+static inline struct mlx7502x *to_mlx7502x(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct mlx7502x, sd);
+}
+
+static int mlx7502x_read(struct v4l2_subdev *sd, u16 reg, u8 *val, int val_size)
+{
+	int ret;
+	unsigned char data_w[2];
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	struct i2c_msg msg[] = {
+		{
+			.addr = client->addr,
+			.flags = 0,
+			.len = 2,
+			.buf = data_w,
+		}, {
+			.addr = client->addr,
+			.flags = I2C_M_RD,
+			.len = val_size,
+			.buf = val,
+		}
+	};
+
+	/* write reg address into first msg */
+	put_unaligned_be16(reg, data_w);
+
+	/* Using transfer allows skip STOP between messages
+	 * so we have repeated Start here
+	 */
+	ret = i2c_transfer(client->adapter, msg, 2);
+
+	return ret != 2 ? -EIO : 0;
+}
+
+static int mlx7502x_write(struct v4l2_subdev *sd, u8 *data, u32 data_size,
+			  int *err)
+{
+	int ret;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (err && *err)
+		return *err;
+
+	dev_dbg(&client->dev, "%*ph\n", data_size, data);
+
+	ret = i2c_master_send(client, data, data_size);
+	if (ret < 0) {
+		dev_dbg(&client->dev, "%s: i2c write error, reg: 0x%x%x\n",
+			__func__, data[0], data[1]);
+
+		return ret;
+	}
+
+	return ret != data_size ? -EIO : 0;
+}
+
+static int mlx7502x_write8(struct v4l2_subdev *sd, u16 reg, u8 val, int *err)
+{
+	unsigned char data[3];
+
+	put_unaligned_be16(reg, data);
+	data[2] = val;
+
+	return mlx7502x_write(sd, data, 3, err);
+}
+
+static int mlx7502x_write16(struct v4l2_subdev *sd, u16 reg, u16 val, int *err)
+{
+	unsigned char data[4];
+
+	put_unaligned_be16(reg, data);
+	put_unaligned_be16(val, data + 2);
+
+	return mlx7502x_write(sd, data, 4, err);
+}
+
+static int mlx7502x_write24(struct v4l2_subdev *sd, u16 reg, u32 val, int *err)
+{
+	unsigned char data[5];
+
+	put_unaligned_be16(reg, data);
+	put_unaligned_be24(val, data + 2);
+
+	return mlx7502x_write(sd, data, 5, err);
+}
+
+static int mlx7502x_write32(struct v4l2_subdev *sd, u16 reg, u32 val, int *err)
+{
+	unsigned char data[6];
+
+	put_unaligned_be16(reg, data);
+	put_unaligned_be32(val, data + 2);
+
+	return mlx7502x_write(sd, data, 6, err);
+}
+
+static int mlx7502x_write_regval(struct v4l2_subdev *sd,
+				 const struct mlx7502x_regval_list *regs,
+				 int array_size)
+{
+	int i, ret;
+
+	for (i = 0; i < array_size; i++) {
+		ret = mlx7502x_write8(sd, regs[i].addr, regs[i].data, NULL);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int mlx7502x_write_array(struct v4l2_subdev *sd, const u16 *reg,
+				const u8 *data, int size, int *err)
+{
+	int i, ret;
+
+	for (i = 0; i < size; i++) {
+		ret = mlx7502x_write8(sd, reg[i], data[i], err);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+/*
+ * *****************************************************************************
+ *  PM
+ * *****************************************************************************
+ */
+static int mlx7502x_power_off(struct mlx7502x *sensor)
+{
+	gpiod_set_value_cansleep(sensor->reset, 0);
+
+	clk_disable_unprepare(sensor->xclk);
+
+	regulator_bulk_disable(MLX7502X_NUM_SUPPLIES, sensor->supplies);
+
+	dev_dbg(sensor->dev, "power off\n");
+
+	return 0;
+}
+
+static int __maybe_unused mlx7502x_runtime_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct mlx7502x *sensor = to_mlx7502x(sd);
+	int ret;
+
+	gpiod_set_value_cansleep(sensor->reset, 0);
+
+	ret = regulator_bulk_enable(MLX7502X_NUM_SUPPLIES, sensor->supplies);
+	if (ret) {
+		dev_err(sensor->dev, "failed to enable supply: %d\n", ret);
+		return ret;
+	}
+
+	ret = clk_prepare_enable(sensor->xclk);
+	if (ret) {
+		dev_err(sensor->dev, "failed to enable external clock: %d\n",
+			ret);
+		goto fail_clk;
+	}
+
+	gpiod_set_value_cansleep(sensor->reset, 1);
+	msleep(MLX7502X_RESET_DELAY_MS);
+
+	dev_dbg(sensor->dev, "power on\n");
+
+	ret = mlx7502x_write_regval(sd, mlx7502x_common_init_cfg,
+				    ARRAY_SIZE(mlx7502x_common_init_cfg));
+	if (ret < 0) {
+		dev_err(sensor->dev, "failed to write init_cfg\n");
+		goto fail_clk;
+	}
+
+	ret = mlx7502x_write_regval(sd, sensor->cur_desc->init_cfg,
+				    sensor->cur_desc->init_cfg_size);
+	if (ret < 0) {
+		dev_err(sensor->dev, "failed to write sensor specific init_cfg\n");
+		goto fail_clk;
+	}
+
+	return 0;
+
+fail_clk:
+	regulator_bulk_disable(MLX7502X_NUM_SUPPLIES, sensor->supplies);
+	return ret;
+}
+
+static int __maybe_unused mlx7502x_runtime_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct mlx7502x *sensor = to_mlx7502x(sd);
+
+	mlx7502x_power_off(sensor);
+
+	return 0;
+}
+
+/*
+ * *****************************************************************************
+ *  Subdev operations
+ * *****************************************************************************
+ */
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int mlx7502x_get_register(struct v4l2_subdev *sd,
+				 struct v4l2_dbg_register *reg)
+{
+	u8 val[4];
+	int ret;
+
+	/* v4l2-dbg set it to 0 */
+	if (reg->size == 0)
+		reg->size = 1;
+
+	ret = mlx7502x_read(sd, reg->reg & 0xffff, val, reg->size);
+	if (ret < 0)
+		return ret;
+
+	if (reg->size == 1)
+		reg->val = val[0];
+	else if (reg->size == 2)
+		reg->val = get_unaligned_be16(val);
+	else if (reg->size == 3)
+		reg->val = get_unaligned_be24(val);
+	else if (reg->size == 4)
+		reg->val = get_unaligned_be32(val);
+	else
+		return -EINVAL;
+
+	return 0;
+}
+
+static int mlx7502x_set_register(struct v4l2_subdev *sd,
+				 const struct v4l2_dbg_register *reg)
+{
+	if (reg->size <= 1)
+		return mlx7502x_write8(sd, reg->reg & 0xffff, reg->val, NULL);
+	else if (reg->size == 2)
+		return mlx7502x_write16(sd, reg->reg & 0xffff, reg->val, NULL);
+	else if (reg->size == 3)
+		return mlx7502x_write24(sd, reg->reg & 0xffff, reg->val, NULL);
+	else if (reg->size == 4)
+		return mlx7502x_write32(sd, reg->reg & 0xffff, reg->val, NULL);
+
+	return -EINVAL;
+}
+#endif
+
+/* from us into sensor ticks */
+static unsigned int mlx7502x_from_us(struct mlx7502x *sensor, u32 us)
+{
+	return DIV_ROUND_CLOSEST(us * MLX7502X_CLK_RATE_MHZ, sensor->hmax);
+}
+
+static unsigned int mlx7502x_to_us(struct mlx7502x *sensor, u32 reg)
+{
+	return DIV_ROUND_CLOSEST(reg * sensor->hmax, MLX7502X_CLK_RATE_MHZ);
+}
+
+static int mlx7502x_set_link_freq(struct mlx7502x *sensor)
+{
+	struct v4l2_subdev *sd = &sensor->sd;
+	const struct mlx7502x_link_freq_list *lfc;
+	int link_freq = sensor->link_freq->val;
+	struct v4l2_mbus_config_mipi_csi2 *bus = &sensor->ep.bus.mipi_csi2;
+	u8 cont_clock, lane_n = bus->num_data_lanes;
+	u32 pretime;
+	int ret;
+
+	/* lane */
+	ret = mlx7502x_write8(sd, MLX7502X_DATA_LANE_CONFIG_REG,
+			      lane_n - 1, NULL);
+
+	/* link freq */
+	lfc = sensor->cur_desc->link_freq_cfg;
+	ret = mlx7502x_write_array(sd, lfc->addr,
+				   lfc->data[lane_n >> 2][link_freq],
+				   MLX7502X_LINK_FREQ_REG_N, &ret);
+
+	/* clock continuous mode if MIPI receiver requires it */
+	cont_clock = bus->flags & V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK ? 1u : 0u;
+	ret = mlx7502x_write8(sd, MLX7502X_CONTINUOUS_REG, cont_clock, &ret);
+
+	ret = mlx7502x_write16(sd, MLX7502X_HMAX_REG, sensor->hmax, &ret);
+
+	/* timings which depends on hmax and mipi config */
+	ret = mlx7502x_write8(sd, MLX7502X_PLLSETUP_REG,
+			      mlx7502x_from_us(sensor, MLX7502X_PLLSETUP_US)
+			      + MLX7502X_PLLSETUP_TICKS, &ret);
+
+	pretime = mlx7502x_from_us(sensor, MLX7502X_PRETIME_US);
+	ret = mlx7502x_write16(sd, MLX7502X_PRETIME_REG, pretime, &ret);
+
+	return mlx7502x_write24(sd, MLX7502X_RANDNM0_REG,
+				pretime * sensor->hmax - 1070 - 2098, &ret);
+}
+
+static int mlx7502x_update_output_format(struct mlx7502x *sensor,
+					 struct v4l2_subdev_state *state)
+{
+	struct v4l2_subdev *sd = &sensor->sd;
+	struct v4l2_rect *crop = v4l2_subdev_state_get_crop(state, 0);
+	int ret;
+
+	ret = mlx7502x_write16(sd, MLX7502X_COLUMN_START_REG,
+			       crop->left + 1u, NULL);
+	ret = mlx7502x_write16(sd, MLX7502X_COLUMN_LEN_REG,
+			       crop->width, &ret);
+	ret = mlx7502x_write16(sd, MLX7502X_ROW_START_REG,
+			       MLX7502X_ROW_START(crop->top), &ret);
+	ret = mlx7502x_write16(sd, MLX7502X_ROW_END_REG,
+			       MLX7502X_ROW_END(crop->top, crop->height), &ret);
+	ret = mlx7502x_write8(sd, MLX7502X_BINNING_REG,
+			      sensor->binning_mode->reg_value, &ret);
+	return mlx7502x_write8(sd, MLX7502X_OUTPUT_MODE_REG,
+			       sensor->output_mode->val, &ret);
+}
+
+static int __mlx7502x_set_frame_interval(struct mlx7502x *sensor,
+					 struct v4l2_subdev_state *state,
+					 struct v4l2_fract *fi, uint32_t which,
+					 u32 phase_n, u16 *tint)
+{
+	struct v4l2_fract *interval;
+	struct v4l2_rect *crop;
+	u32 req_fi_us, min_fi_us, readout_ticks, tint_sum = 0, pllsetup_us;
+	int ret, i, fi_register = 0;
+
+	if (sensor->trigger_mode->val == V4L2_TRIGGER_MODE_CONTINUOUS) {
+		interval = v4l2_subdev_state_get_interval(state, 0);
+		if (!fi)
+			fi = interval;
+
+		if (fi->denominator == 0 || fi->numerator == 0) {
+			fi->numerator = 1;
+			fi->denominator = MLX7502X_DEFAULT_FRAME_RATE;
+		}
+
+		crop = v4l2_subdev_state_get_crop(state, 0);
+
+		readout_ticks = MLX7502X_ROW_END(crop->top, crop->height)
+				- MLX7502X_ROW_START(crop->top) + 1;
+
+		for (i = 0; i < phase_n; i++)
+			tint_sum += tint[i];
+
+		pllsetup_us = MLX7502X_PLLSETUP_US
+			    + mlx7502x_to_us(sensor, MLX7502X_PLLSETUP_TICKS);
+
+		min_fi_us = pllsetup_us + tint_sum
+			    + (mlx7502x_to_us(sensor,
+					      readout_ticks
+					      + MLX7502X_FRAME_ADD_TICKS)
+			       + MLX7502X_PRETIME_US)
+			      * phase_n;
+
+		req_fi_us = MICRO * fi->numerator / fi->denominator;
+
+		dev_dbg(sensor->dev,
+			"frame interval: min = %dus, requested = %dus\n",
+			min_fi_us, req_fi_us);
+
+		if (req_fi_us < min_fi_us) {
+			req_fi_us = min_fi_us;
+			fi->numerator = 1;
+			fi->denominator = MICRO / req_fi_us;
+		}
+
+		if (fi != interval)
+			*interval = *fi;
+
+		fi_register = mlx7502x_from_us(sensor, req_fi_us - pllsetup_us);
+	}
+
+	if (which != V4L2_SUBDEV_FORMAT_ACTIVE)
+		return 0;
+
+	if (!pm_runtime_get_if_in_use(sensor->dev))
+		return 0;
+
+	ret = mlx7502x_write32(&sensor->sd, MLX7502X_FRAME_TIME_REG,
+			       fi_register, NULL);
+
+	pm_runtime_put_autosuspend(sensor->dev);
+
+	return ret;
+}
+
+static int mlx7502x_s_stream(struct v4l2_subdev *sd, int on)
+{
+	struct mlx7502x *sensor = to_mlx7502x(sd);
+	struct v4l2_subdev_state *state;
+	int ret = 0;
+
+	state = v4l2_subdev_lock_and_get_active_state(sd);
+
+	if (on && !sensor->streaming) {
+		ret = pm_runtime_resume_and_get(sensor->dev);
+		if (ret < 0) {
+			v4l2_subdev_unlock_state(state);
+			return ret;
+		}
+
+		ret = __v4l2_ctrl_handler_setup(sd->ctrl_handler);
+		if (ret < 0)
+			goto error_stream;
+		/*
+		 * the registers below depends on hmax,
+		 * which is configured in controls
+		 */
+		ret = mlx7502x_set_link_freq(sensor);
+		if (ret < 0)
+			goto error_stream;
+		ret = mlx7502x_update_output_format(sensor, state);
+		if (ret < 0)
+			goto error_stream;
+
+		ret = mlx7502x_write8(sd, MLX7502X_STREAM_EN_REG, 1u, NULL);
+		if (ret < 0)
+			goto error_stream;
+
+		sensor->streaming = 1u;
+		dev_dbg(sensor->dev, "stream enabled\n");
+
+		/* we need to wait to stabilize the system after streaming on */
+		msleep(MLX7502X_STREAMING_DELAY_MS);
+	} else if (!on && sensor->streaming) {
+		sensor->streaming = 0u;
+		ret = mlx7502x_write8(sd, MLX7502X_STREAM_EN_REG, 0u, NULL);
+		/* we need to wait in case last buffer is sending */
+		msleep(MLX7502X_STREAMING_DELAY_MS);
+		dev_dbg(sensor->dev, "stream disabled\n");
+
+		pm_runtime_mark_last_busy(sensor->dev);
+		pm_runtime_put_autosuspend(sensor->dev);
+	}
+
+	__v4l2_ctrl_grab(sensor->trigger_mode, on);
+
+	v4l2_subdev_unlock_state(state);
+	return ret;
+
+error_stream:
+	dev_err(sensor->dev, "failed to enable stream(%d)\n", ret);
+	pm_runtime_put(sensor->dev);
+	v4l2_subdev_unlock_state(state);
+	return ret;
+}
+
+static int mlx7502x_set_frame_interval(struct v4l2_subdev *sd,
+				       struct v4l2_subdev_state *state,
+				       struct v4l2_subdev_frame_interval *fi)
+{
+	struct mlx7502x *sensor = to_mlx7502x(sd);
+
+	if (sensor->trigger_mode->val != V4L2_TRIGGER_MODE_CONTINUOUS)
+		return -EBUSY;
+
+	return __mlx7502x_set_frame_interval(sensor, state,
+					     &fi->interval,
+					     fi->which,
+					     sensor->phase_seq->elems,
+					     sensor->tint->p_cur.p_u16);
+}
+
+static int mlx7502x_get_frame_interval(struct v4l2_subdev *sd,
+				       struct v4l2_subdev_state *state,
+				       struct v4l2_subdev_frame_interval *fi)
+{
+	struct mlx7502x *sensor = to_mlx7502x(sd);
+
+	if (sensor->trigger_mode->val != V4L2_TRIGGER_MODE_CONTINUOUS)
+		return -EBUSY;
+
+	return v4l2_subdev_get_frame_interval(sd, state, fi);
+}
+
+static int mlx7502x_enum_mbus_code(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *state,
+				   struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index > 0)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_Y12_1X12;
+
+	return 0;
+}
+
+static int mlx7502x_set_format(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_state *state,
+			       struct v4l2_subdev_format *fmt)
+{
+	struct mlx7502x *sensor = to_mlx7502x(sd);
+	struct v4l2_rect *compose = v4l2_subdev_state_get_compose(state, 0);
+	int output_mode = sensor->output_mode->val;
+	struct v4l2_mbus_framefmt *format;
+	int width_mult;
+
+	format = v4l2_subdev_state_get_format(state, 0);
+
+	width_mult = mlx7502x_output_mode_cfg[output_mode].width_mult;
+
+	memset(format, 0, sizeof(*format));
+	format->code = MEDIA_BUS_FMT_Y12_1X12;
+	format->width = width_mult * compose->width;
+	format->height = compose->height;
+	format->field = V4L2_FIELD_NONE;
+	format->colorspace = V4L2_COLORSPACE_RAW;
+	format->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+	format->quantization = V4L2_QUANTIZATION_FULL_RANGE;
+	format->xfer_func = V4L2_XFER_FUNC_NONE;
+
+	if (fmt)
+		fmt->format = *format;
+
+	return 0;
+}
+
+static const struct mlx7502x_binning_mode *
+mlx7502x_find_binning_mode(struct v4l2_rect *crop, struct v4l2_rect *compose)
+{
+	int i;
+	const struct mlx7502x_binning_mode *bin_mode;
+	u8 ratio = compose->width == 0u ? 1u : crop->width / compose->width;
+
+	for (i = 0; i < ARRAY_SIZE(binning_mode); i++) {
+		bin_mode = &binning_mode[i];
+
+		if (bin_mode->ratio == ratio)
+			break;
+	}
+
+	return bin_mode;
+}
+
+static void mlx7502x_set_crop(struct mlx7502x *sensor,
+			      struct v4l2_subdev_state *state,
+			      struct v4l2_subdev_selection *sel,
+			      struct v4l2_rect *dst_crop,
+			      struct v4l2_rect *dst_compose,
+			      const struct mlx7502x_binning_mode *bin_mode)
+{
+	u32 w, h;
+
+	/*
+	 * cropping will always be in native size,
+	 * even if binning was applied
+	 */
+	w = sensor->cur_desc->width;
+	h = sensor->cur_desc->height;
+
+	v4l_bound_align_image(&sel->r.width, bin_mode->width_min, w,
+			      bin_mode->width_align,
+			      &sel->r.height, bin_mode->height_min, h,
+			      bin_mode->height_align, 0);
+
+	v4l_bound_align_image(&sel->r.left, MLX7502X_LEFT_MIN, w - sel->r.width,
+			      MLX7502X_LEFT_ALIGN,
+			      &sel->r.top, MLX7502X_TOP_MIN, h - sel->r.height,
+			      MLX7502X_TOP_ALIGN, 0);
+	/* fill active or try */
+	*dst_crop = sel->r;
+	/* update binning w/h, as actual crop size could changed */
+	dst_compose->top = 0;
+	dst_compose->left = 0;
+	dst_compose->width = dst_crop->width / bin_mode->ratio;
+	dst_compose->height = dst_crop->height / bin_mode->ratio;
+
+	/* update frame interval, but don't write into sensor yet */
+	if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		__mlx7502x_set_frame_interval(sensor, state,
+					      NULL,
+					      V4L2_SUBDEV_FORMAT_TRY,
+					      sensor->phase_seq->elems,
+					      sensor->tint->p_cur.p_u16);
+}
+
+/*
+ * Binning is applied after cropping inside the sensor
+ */
+static void mlx7502x_set_compose(struct mlx7502x *sensor,
+				 struct v4l2_subdev_state *state,
+				 struct v4l2_subdev_selection *sel,
+				 struct v4l2_rect *dst_crop,
+				 struct v4l2_rect *dst_compose)
+{
+	const struct mlx7502x_binning_mode *bin_mode;
+	u32 w, h, i, bin_w, bin_h;
+
+	w = dst_crop->width;
+	h = dst_crop->height;
+
+	/* select the best binning */
+	for (i = 0; i < ARRAY_SIZE(binning_mode); i++) {
+		bin_mode = &binning_mode[i];
+
+		if (sel->flags & V4L2_SEL_FLAG_LE) {
+			bin_w = w / bin_mode->ratio;
+			bin_h = h / bin_mode->ratio;
+		} else {/* for GE and KEEP choose lower bin_w/h */
+			bin_w = w / bin_mode[1].ratio + 1;
+			bin_h = h / bin_mode[1].ratio + 1;
+		}
+
+		if (sel->r.width >= bin_w && sel->r.height >= bin_h)
+			break;
+	}
+
+	/* save new binning config */
+	if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		sensor->binning_mode = bin_mode;
+
+	/* update crop step and min, based on new binning */
+	sel->r = *dst_crop;
+	mlx7502x_set_crop(sensor, state, sel, dst_crop, dst_compose, bin_mode);
+
+	/* resulted format after applying new binning */
+	sel->r = *dst_compose;
+}
+
+static int mlx7502x_set_selection(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *state,
+				  struct v4l2_subdev_selection *sel)
+{
+	struct mlx7502x *sensor = to_mlx7502x(sd);
+	struct v4l2_rect *crop, *compose;
+	const struct mlx7502x_binning_mode *bin_mode;
+	int ret = 0;
+
+	crop = v4l2_subdev_state_get_crop(state, 0);
+	compose = v4l2_subdev_state_get_compose(state, 0);
+	bin_mode = mlx7502x_find_binning_mode(crop, compose);
+
+	if (sel->target == V4L2_SEL_TGT_CROP)
+		mlx7502x_set_crop(sensor, state, sel, crop, compose, bin_mode);
+	else if (sel->target == V4L2_SEL_TGT_COMPOSE)
+		/* actually this is binning */
+		mlx7502x_set_compose(sensor, state, sel, crop, compose);
+	else
+		ret = -EINVAL;
+
+	/* update format state */
+	if (ret == 0)
+		mlx7502x_set_format(sd, state, NULL);
+
+	return ret;
+}
+
+static int mlx7502x_get_selection(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *state,
+				  struct v4l2_subdev_selection *sel)
+{
+	struct mlx7502x *sensor = to_mlx7502x(sd);
+	struct v4l2_rect *crop, *compose;
+	int ret = 0;
+
+	crop = v4l2_subdev_state_get_crop(state, 0);
+	compose = v4l2_subdev_state_get_compose(state, 0);
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		sel->r = *crop;
+		break;
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_NATIVE_SIZE:
+		sel->r.top = 0;
+		sel->r.left = 0;
+		sel->r.width = sensor->cur_desc->width;
+		sel->r.height = sensor->cur_desc->height;
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+		sel->r = *compose;
+		break;
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+		sel->r.top = 0;
+		sel->r.left = 0;
+		sel->r.width = crop->width;
+		sel->r.height = crop->height;
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static int mlx7502x_init_state(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_state *state)
+{
+	struct mlx7502x *sensor = to_mlx7502x(sd);
+	struct v4l2_fract *interval;
+
+	struct v4l2_subdev_selection sel = {
+		.target = V4L2_SEL_TGT_CROP,
+		.r.width = sensor->cur_desc->width,
+		.r.height = sensor->cur_desc->height,
+	};
+	struct v4l2_fract fi = {
+		.numerator = 1,
+		.denominator = MLX7502X_DEFAULT_FRAME_RATE,
+	};
+
+	// it will also update compose and format state
+	mlx7502x_set_selection(sd, state, &sel);
+
+	interval = v4l2_subdev_state_get_interval(state, 0);
+	*interval = fi;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops mlx7502x_subdev_core_ops = {
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = mlx7502x_get_register,
+	.s_register = mlx7502x_set_register,
+#endif
+};
+
+static const struct v4l2_subdev_video_ops mlx7502x_subdev_video_ops = {
+	.s_stream = mlx7502x_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops mlx7502x_subdev_pad_ops = {
+	.enum_mbus_code = mlx7502x_enum_mbus_code,
+	.set_fmt = mlx7502x_set_format,
+	.get_fmt = v4l2_subdev_get_fmt,
+	.set_selection = mlx7502x_set_selection,
+	.get_selection = mlx7502x_get_selection,
+	.get_frame_interval = mlx7502x_get_frame_interval,
+	.set_frame_interval = mlx7502x_set_frame_interval,
+};
+
+static const struct v4l2_subdev_ops mlx7502x_subdev_ops = {
+	.core = &mlx7502x_subdev_core_ops,
+	.video = &mlx7502x_subdev_video_ops,
+	.pad = &mlx7502x_subdev_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops mlx7502x_internal_ops = {
+	.init_state = mlx7502x_init_state,
+};
+
+/*
+ * *****************************************************************************
+ *  Controls
+ * *****************************************************************************
+ */
+static __always_inline u8 mlx7502x_phase_to_reg(u16 phase)
+{
+	/* Note:
+	 *  Sensor is working in mode when:
+	 *      MIX is a referance,
+	 *      and illumination is shifted
+	 *  To compensate it, we need to add 180 degrees
+	 */
+	if (phase != 0 && phase != 180)
+		phase = (phase + 180) % 360;
+
+	return phase / 45;
+}
+
+static int mlx7502x_set_phase_seq(struct mlx7502x *sensor,
+				  struct v4l2_subdev_state *state,
+				  struct v4l2_ctrl *ctrl)
+{
+	int ret;
+	int i, offset;
+	u16 *p_v;
+	u8 val;
+	u8 *data;
+	/* address + number of phases(1 phases - 1 nibble) */
+	size_t data_size = 2 + DIV_ROUND_CLOSEST(ctrl->new_elems, 2);
+
+	data = kmalloc(data_size, GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	/* the address */
+	put_unaligned_be16(MLX7502X_PX_PHASE_SHIFT_REG, data);
+
+	/* data itself */
+	offset = 2;
+	p_v = ctrl->p_new.p_u16;
+	for (i = 0; i < (ctrl->new_elems - 1); i += 2) {
+		val = FIELD_PREP(MLX7502X_NIBLE_LOW_MASK,
+				 mlx7502x_phase_to_reg(p_v[i]));
+
+		val |= FIELD_PREP(MLX7502X_NIBLE_HIGH_MASK,
+				  mlx7502x_phase_to_reg(p_v[i + 1]));
+
+		data[offset++] = val;
+	}
+
+	/* last odd field if any */
+	if (i < ctrl->new_elems)
+		data[offset] = FIELD_PREP(MLX7502X_NIBLE_LOW_MASK,
+					  mlx7502x_phase_to_reg(p_v[i]));
+
+	/* set phase sequence */
+	ret = mlx7502x_write(&sensor->sd, data, data_size, NULL);
+
+	/* set number of phases */
+	ret = mlx7502x_write8(&sensor->sd, MLX7502X_PHASE_COUNT_REG,
+			      ctrl->new_elems, &ret);
+	kfree(data);
+
+	if (!ret)
+		ret = __mlx7502x_set_frame_interval(sensor, state,
+						    NULL,
+						    V4L2_SUBDEV_FORMAT_ACTIVE,
+						    sensor->phase_seq->new_elems,
+						    sensor->tint->p_cur.p_u16);
+	return ret;
+}
+
+/* comparator for bsearch func */
+static int mlx7502x_fmod_cmp(const void *key, const void *elt)
+{
+	int ret = 1; /* lower then lowest */
+	u32 val = *((u32 *)key);
+	/* need prev elt for range checking, so key requires to be >= 4 */
+	struct mlx7502x_fmod_list *el = (struct mlx7502x_fmod_list *)elt - 1;
+	u32 high = el[0].fmod;
+	u32 low = el[1].fmod;
+
+	if (val > high) /* more then the highest, move to lower index */
+		ret = -1;
+	else if (val >= low) /* we are in a range */
+		ret = 0;
+
+	return ret;
+}
+
+static int mlx7502x_set_fmod(struct mlx7502x *sensor, u32 fmod)
+{
+	int ret;
+	u16 fmod_reg;
+	u8 pll_res_reg;
+	struct v4l2_subdev *sd = &sensor->sd;
+	struct mlx7502x_fmod_list *fc;
+
+	fc = bsearch(&fmod,
+		     mlx7502x_fmod_cfg,
+		     ARRAY_SIZE(mlx7502x_fmod_cfg),
+		     sizeof(struct mlx7502x_fmod_list),
+		     mlx7502x_fmod_cmp);
+
+	ret = mlx7502x_write8(sd, MLX7502X_DIVSELPRE_REG, fc->divselpre, NULL);
+	ret = mlx7502x_write8(sd, MLX7502X_DIVSEL_REG, fc->divsel, &ret);
+
+	fmod_reg = (fmod / 1000000u) << (fc->divselpre + fc->divsel);
+	ret = mlx7502x_write16(sd, MLX7502X_FMOD_REG, fmod_reg, &ret);
+
+	pll_res_reg = fmod_reg < MLX7502X_PLL_RES_THR ? 2 : 0;
+	return mlx7502x_write8(sd, MLX7502X_PLL_RES_REG, pll_res_reg, &ret);
+}
+
+static int mlx7502x_set_tint(struct mlx7502x *sensor,
+			     struct v4l2_subdev_state *state,
+			     struct v4l2_ctrl *ctrl)
+{
+	int i, offset, ret = 0;
+	u16 *p_v;
+	u32 tint_reg;
+	u8 *data;
+	/* address + number of phases(1 phases - 4bytes) */
+	size_t data_size = 2 + ctrl->new_elems * 4;
+
+	data = kmalloc(data_size, GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	/* the address */
+	put_unaligned_be16(MLX7502X_TINT0_REG, data);
+
+	/* data itself */
+	p_v = ctrl->p_new.p_u16;
+	for (i = 0; i < ctrl->new_elems; i++) {
+		offset = 2 + i * 4;
+		tint_reg = mlx7502x_from_us(sensor, p_v[i]) * sensor->hmax;
+		put_unaligned_be32(tint_reg, data + offset);
+	}
+
+	ret = mlx7502x_write(&sensor->sd, data, data_size, NULL);
+	kfree(data);
+
+	if (!ret)
+		ret = __mlx7502x_set_frame_interval(sensor, state,
+						    NULL,
+						    V4L2_SUBDEV_FORMAT_ACTIVE,
+						    sensor->phase_seq->elems,
+						    sensor->tint->p_new.p_u16);
+	return ret;
+}
+
+static int mlx7502x_update_hmax(struct mlx7502x *sensor,
+				int mipi_speed_ind,
+				int output_mode)
+{
+	const struct mlx7502x_link_freq_list *lfc;
+	int hmax_index = mlx7502x_output_mode_cfg[output_mode].hmax_index;
+	u8 lane_n = sensor->ep.bus.mipi_csi2.num_data_lanes;
+	u32 tint_step;
+
+	lfc = sensor->cur_desc->link_freq_cfg;
+	sensor->hmax = lfc->hmax[lane_n >> 2][mipi_speed_ind][hmax_index];
+
+	tint_step = mlx7502x_to_us(sensor, 1);
+	return __v4l2_ctrl_modify_range(sensor->tint,
+					tint_step,
+					sensor->tint->maximum,
+					tint_step,
+					sensor->tint->default_value);
+}
+
+static int mlx7502x_set_trigger_mode(struct mlx7502x *sensor,
+				     enum v4l2_trigger_mode mode)
+{
+	int ret;
+	struct v4l2_subdev *sd = &sensor->sd;
+	bool trigger_grab = false;
+
+	switch (mode) {
+	case V4L2_TRIGGER_MODE_CONTINUOUS:
+		ret = mlx7502x_write_regval(sd, mlx7502x_trigger_cfg,
+					    ARRAY_SIZE(mlx7502x_trigger_cfg));
+		ret = mlx7502x_write8(sd, MLX7502X_SW_TRIGGER_REG,
+				      MLX7502X_SW_TRIGGER_CONT, &ret);
+		trigger_grab = true;
+		break;
+	case V4L2_TRIGGER_MODE_INTERNAL:
+		ret = mlx7502x_write_regval(sd, mlx7502x_trigger_cfg,
+					    ARRAY_SIZE(mlx7502x_trigger_cfg));
+		ret = mlx7502x_write8(sd, MLX7502X_SW_TRIGGER_REG,
+				      MLX7502X_SW_TRIGGER_DEFAULT, &ret);
+		trigger_grab = false;
+		break;
+	case V4L2_TRIGGER_MODE_EXTERNAL:
+		ret = mlx7502x_write_regval(sd, mlx7502x_ext_trigger_cfg,
+					    ARRAY_SIZE(mlx7502x_ext_trigger_cfg));
+		ret = mlx7502x_write8(sd, MLX7502X_SW_TRIGGER_REG,
+				      MLX7502X_SW_TRIGGER_DEFAULT, &ret);
+		trigger_grab = true;
+		break;
+	default:
+		/* should not be there */
+		ret = -EINVAL;
+		break;
+	}
+
+	if (ret == 0)
+		__v4l2_ctrl_grab(sensor->trigger, trigger_grab);
+
+	return ret;
+}
+
+static int mlx7502x_hold(struct mlx7502x *sensor, u32 id, u8 hold)
+{
+	int ret = 0;
+
+	if (!sensor->streaming) /* only during streaming */
+		return 0;
+
+	switch (id) {
+	case V4L2_CID_TOF_PHASE_SEQ:
+	case V4L2_CID_TOF_FREQ_MOD:
+	case V4L2_CID_TOF_TIME_INTEGRATION:
+	case V4L2_CID_VFLIP:
+	case V4L2_CID_HFLIP:
+		ret = mlx7502x_write8(&sensor->sd,
+				      MLX7502X_PARAM_HOLD_REG,
+				      hold, NULL);
+		break;
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+static int mlx7502x_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct mlx7502x *sensor = container_of(ctrl->handler,
+					       struct mlx7502x,
+					       ctrl_handler);
+	struct v4l2_subdev *sd = &sensor->sd;
+	struct v4l2_subdev_state *state;
+	int ret = 0;
+
+	if (!pm_runtime_get_if_in_use(sensor->dev))
+		return 0;
+
+	state = v4l2_subdev_get_locked_active_state(sd);
+
+	ret = mlx7502x_hold(sensor, ctrl->id, 1);
+	if (ret < 0)
+		return ret;
+
+	switch (ctrl->id) {
+	case V4L2_CID_TOF_PHASE_SEQ:
+		ret = mlx7502x_set_phase_seq(sensor, state, ctrl);
+		break;
+	case V4L2_CID_TOF_FREQ_MOD:
+		ret = mlx7502x_set_fmod(sensor, *ctrl->p_new.p_u32);
+		break;
+	case V4L2_CID_TOF_TIME_INTEGRATION:
+		ret = mlx7502x_set_tint(sensor, state, ctrl);
+		break;
+	case V4L2_CID_MLX7502X_OUTPUT_MODE:
+		ret = mlx7502x_update_hmax(sensor,
+					   sensor->link_freq->val,
+					   ctrl->val);
+		/* update format state */
+		if (ret == 0)
+			mlx7502x_set_format(sd, state, NULL);
+		break;
+	case V4L2_CID_LINK_FREQ:
+		ret = mlx7502x_update_hmax(sensor,
+					   ctrl->val,
+					   sensor->output_mode->val);
+		break;
+	case V4L2_CID_VFLIP:
+		ret = mlx7502x_write8(sd, MLX7502X_VFLIP_REG, ctrl->val, NULL);
+		break;
+	case V4L2_CID_HFLIP:
+		ret = mlx7502x_write8(sd, MLX7502X_HFLIP_REG, ctrl->val, NULL);
+		break;
+	case V4L2_CID_CAMERA_TRIGGER:
+		ret = mlx7502x_write8(sd, MLX7502X_SW_TRIGGER_REG,
+				      MLX7502X_SW_TRIGGER_TRIG, NULL);
+		break;
+	case V4L2_CID_CAMERA_TRIGGER_MODE:
+		ret = mlx7502x_set_trigger_mode(sensor, ctrl->val);
+		break;
+	default:
+		dev_err(sensor->dev, "Unknown id: %x\n", ctrl->id);
+		break;
+	}
+
+	mlx7502x_hold(sensor, ctrl->id, 0);
+	pm_runtime_put_autosuspend(sensor->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops mlx7502x_ctrl_ops = {
+	.s_ctrl = mlx7502x_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config mlx7502x_phase_sequence = {
+	.ops = &mlx7502x_ctrl_ops,
+	.id = V4L2_CID_TOF_PHASE_SEQ,
+	.min = 0,
+	.max = 315,
+	.step = 45,
+	.def = 0,
+	.dims = { MLX7502X_PHASE_MAX_NUM },
+};
+
+static const struct v4l2_ctrl_config mlx7502x_fmod = {
+	.ops = &mlx7502x_ctrl_ops,
+	.id = V4L2_CID_TOF_FREQ_MOD,
+	.min = 4000000,
+	.max = 100000000,
+	.step = 1000000, /* in Hz */
+	.def = 40000000,
+	.dims = { 1 },
+};
+
+static const struct v4l2_ctrl_config mlx7502x_tint = {
+	.ops = &mlx7502x_ctrl_ops,
+	.id = V4L2_CID_TOF_TIME_INTEGRATION,
+	.min = 5,
+	.max = 1000,
+	.step = 1, /* in us */
+	.def = 198,
+	.dims = { MLX7502X_PHASE_MAX_NUM },
+};
+
+static const struct v4l2_ctrl_config mlx7502x_output_mode = {
+	.ops = &mlx7502x_ctrl_ops,
+	.id = V4L2_CID_MLX7502X_OUTPUT_MODE,
+	.name = "Output mode",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.max = ARRAY_SIZE(mlx7502x_ctrl_output) - 1,
+	.def = 0,
+	.qmenu = mlx7502x_ctrl_output,
+};
+
+static int mlx7502x_link_freq_init(struct mlx7502x *sensor)
+{
+	struct v4l2_ctrl_handler *hdl = &sensor->ctrl_handler;
+	const s64 *link_freq = sensor->cur_desc->link_freq_cfg->link_freq;
+	int i, j, link_freq_mask;
+
+	link_freq_mask = 0;
+	for (i = 0; i < sensor->ep.nr_of_link_frequencies; i++) {
+		for (j = 0; j < MLX7502X_LINK_FREQ_N; j++) {
+			if (link_freq[j] == sensor->ep.link_frequencies[i])
+				break;
+		}
+
+		if (j == MLX7502X_LINK_FREQ_N) {
+			dev_err(sensor->dev, "no link frequency %lld supported\n",
+				link_freq[j]);
+			return -EINVAL;
+		}
+
+		link_freq_mask |= (1 << j);
+	}
+
+	sensor->link_freq = v4l2_ctrl_new_int_menu(hdl, &mlx7502x_ctrl_ops,
+						   V4L2_CID_LINK_FREQ,
+						   MLX7502X_LINK_FREQ_N - 1,
+						   __fls(link_freq_mask),
+						   link_freq);
+
+	sensor->link_freq->menu_skip_mask = ~link_freq_mask;
+
+	return 0;
+}
+
+static int mlx7502x_ctrls_init(struct mlx7502x *sensor)
+{
+	struct v4l2_ctrl_handler *hdl;
+	int ret = 0;
+
+	hdl = &sensor->ctrl_handler;
+
+	ret = v4l2_ctrl_handler_init(hdl, 9);
+	if (ret) {
+		dev_err(sensor->dev, "Failed to init handler - %d\n", ret);
+		return ret;
+	}
+
+	ret = mlx7502x_link_freq_init(sensor);
+	if (ret)
+		goto error_ctrls;
+
+	v4l2_ctrl_new_std(hdl, &mlx7502x_ctrl_ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(hdl, &mlx7502x_ctrl_ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
+	sensor->trigger = v4l2_ctrl_new_std(hdl, &mlx7502x_ctrl_ops,
+					    V4L2_CID_CAMERA_TRIGGER,
+					    0, 0, 0, 0);
+
+	sensor->trigger_mode = v4l2_ctrl_new_std_menu(hdl, &mlx7502x_ctrl_ops,
+						      V4L2_CID_CAMERA_TRIGGER_MODE,
+						      2, 0, 0);
+
+	v4l2_ctrl_new_custom(hdl, &mlx7502x_fmod, NULL);
+	sensor->phase_seq = v4l2_ctrl_new_custom(hdl, &mlx7502x_phase_sequence,
+						 NULL);
+
+	sensor->tint = v4l2_ctrl_new_custom(hdl, &mlx7502x_tint, NULL);
+	sensor->output_mode = v4l2_ctrl_new_custom(hdl, &mlx7502x_output_mode,
+						   NULL);
+	__v4l2_ctrl_grab(sensor->trigger, true);
+
+	if (hdl->error)	{
+		dev_err(sensor->dev, "Error %d while adding controls\n",
+			hdl->error);
+		ret = hdl->error;
+		goto error_ctrls;
+	}
+
+	sensor->sd.ctrl_handler = hdl;
+	return 0;
+
+error_ctrls:
+	v4l2_ctrl_handler_free(hdl);
+	return ret;
+}
+
+/*
+ * *****************************************************************************
+ *  Probing
+ * *****************************************************************************
+ */
+static int mlx7502x_fwnode_parse(struct mlx7502x *sensor, struct device *dev)
+{
+	struct fwnode_handle *endpoint;
+	int ret, num_data_lanes;
+
+	endpoint = fwnode_graph_get_next_endpoint(dev_fwnode(dev), NULL);
+	if (!endpoint) {
+		dev_err(dev, "endpoint node not found\n");
+		return -EINVAL;
+	}
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(endpoint, &sensor->ep);
+	fwnode_handle_put(endpoint);
+	if (ret < 0) {
+		dev_err(dev, "parsing endpoint node failed\n");
+		return ret;
+	}
+
+	if (sensor->ep.bus_type != V4L2_MBUS_CSI2_DPHY) {
+		dev_err(dev, "invalid bus type, must be MIPI CSI2\n");
+		ret = -EINVAL;
+		goto dt_ep_error;
+	}
+
+	num_data_lanes = sensor->ep.bus.mipi_csi2.num_data_lanes;
+	if (num_data_lanes != 2 && num_data_lanes != 4) {
+		dev_err(dev, "invalid num_data_lanes, must be 2 or 4. But it was %d\n",
+			num_data_lanes);
+		ret = -EINVAL;
+		goto dt_ep_error;
+	}
+
+	if (!sensor->ep.nr_of_link_frequencies) {
+		dev_err(dev, "missing link frequencies property\n");
+		ret = -EINVAL;
+		goto dt_ep_error;
+	}
+
+	return 0;
+
+dt_ep_error:
+	v4l2_fwnode_endpoint_free(&sensor->ep);
+	return ret;
+}
+
+static int mlx7502x_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct mlx7502x *sensor;
+	struct v4l2_subdev *sd;
+	u32 xclk_rate;
+	int ret, i;
+
+	sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
+	if (!sensor)
+		return -ENOMEM;
+
+	if (dev_fwnode(dev))
+		sensor->cur_desc = device_get_match_data(dev);
+	else
+		return -ENODEV;
+
+	ret = mlx7502x_fwnode_parse(sensor, dev);
+	if (ret < 0)
+		return ret;
+
+	sensor->xclk = devm_clk_get(dev, NULL);
+	if (IS_ERR(sensor->xclk)) {
+		dev_err(dev, "failed to get xclk\n");
+		return PTR_ERR(sensor->xclk);
+	}
+
+	xclk_rate = clk_get_rate(sensor->xclk);
+	if (xclk_rate != MLX7502X_XCLK_RATE) {
+		dev_err(dev, "only 8MHz clock is supported\n");
+		return -EINVAL;
+	}
+
+	for (i = 0; i < MLX7502X_NUM_SUPPLIES; i++)
+		sensor->supplies[i].supply = mlx7502x_supply_names[i];
+
+	ret = devm_regulator_bulk_get(dev, MLX7502X_NUM_SUPPLIES,
+				      sensor->supplies);
+	if (ret)
+		return ret;
+
+	sensor->reset = devm_gpiod_get_optional(dev, "reset", GPIOD_OUT_LOW);
+	if (IS_ERR(sensor->reset)) {
+		dev_err(dev, "failed to get reset\n");
+		return PTR_ERR(sensor->reset);
+	}
+
+	sensor->dev = dev;
+	sd = &sensor->sd;
+	v4l2_i2c_subdev_init(sd, client, &mlx7502x_subdev_ops);
+	sd->internal_ops = &mlx7502x_internal_ops;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
+
+	sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&sd->entity, 1, &sensor->pad);
+	if (ret < 0)
+		goto error_ep;
+
+	sensor->binning_mode = &binning_mode[0];
+
+	ret = mlx7502x_ctrls_init(sensor);
+	if (ret < 0)
+		goto error_init;
+
+	sd->state_lock = sensor->ctrl_handler.lock;
+
+	v4l2_subdev_init_finalize(sd);
+
+	pm_runtime_enable(dev);
+
+	ret = v4l2_async_register_subdev(sd);
+	if (ret < 0)
+		goto error_pm;
+
+	/* configure to use autosuspend, delay 1s */
+	pm_runtime_set_autosuspend_delay(dev, 1000);
+	pm_runtime_use_autosuspend(dev);
+
+	dev_dbg(dev, "Melexis ToF camera driver probed\n");
+
+	return 0;
+
+error_pm:
+	pm_runtime_disable(dev);
+error_init:
+	v4l2_subdev_cleanup(sd);
+	media_entity_cleanup(&sd->entity);
+error_ep:
+	v4l2_fwnode_endpoint_free(&sensor->ep);
+	return ret;
+}
+
+static void mlx7502x_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct mlx7502x *sensor = to_mlx7502x(sd);
+
+	pm_runtime_disable(&client->dev);
+	if (!pm_runtime_status_suspended(&client->dev))
+		mlx7502x_power_off(sensor);
+	pm_runtime_set_suspended(&client->dev);
+
+	v4l2_fwnode_endpoint_free(&sensor->ep);
+	v4l2_async_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&sensor->ctrl_handler);
+
+	v4l2_subdev_cleanup(sd);
+	media_entity_cleanup(&sensor->sd.entity);
+}
+
+static const struct dev_pm_ops mlx7502x_pm_ops = {
+	SET_RUNTIME_PM_OPS(mlx7502x_runtime_suspend,
+			   mlx7502x_runtime_resume,
+			   NULL)
+};
+
+static const struct of_device_id mlx7502x_of_match[] = {
+	{ .compatible = "melexis,mlx75026", .data = &mlx75026 },
+	{ .compatible = "melexis,mlx75027", .data = &mlx75027 },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, mlx7502x_of_match);
+
+static struct i2c_driver mlx7502x_driver = {
+	.driver = {
+		.of_match_table = mlx7502x_of_match,
+		.name = "mlx7502x",
+		.pm = &mlx7502x_pm_ops,
+	},
+	.probe = mlx7502x_probe,
+	.remove = mlx7502x_remove,
+};
+
+module_i2c_driver(mlx7502x_driver);
+
+MODULE_AUTHOR("Andrii Kyselov <ays@xxxxxxxxxxx>");
+MODULE_AUTHOR("Volodymyr Kharuk <vkh@xxxxxxxxxxx>");
+MODULE_DESCRIPTION("A low-level driver for Melexis TOF sensors");
+MODULE_LICENSE("GPL");
-- 
BR,
Volodymyr Kharuk





[Index of Archives]     [Device Tree Compilter]     [Device Tree Spec]     [Linux Driver Backports]     [Video for Linux]     [Linux USB Devel]     [Linux PCI Devel]     [Linux Audio Users]     [Linux Kernel]     [Linux SCSI]     [XFree86]     [Yosemite Backpacking]


  Powered by Linux