Adding upstream version 6.6.15.upstream/6.6.15

Signed-off-by: Daniel Baumann <daniel.baumann@progress-linux.org>

199 files changed, 192810 insertions, 0 deletions

diff --git a/drivers/media/i2c/Kconfig b/drivers/media/i2c/Kconfig
new file mode 100644
index 000000000..53b443be5
--- /dev/null
+++ b/drivers/media/i2c/Kconfig
@@ -0,0 +1,1493 @@
+# SPDX-License-Identifier: GPL-2.0-only
+#
+# Multimedia Video device configuration
+#
+
+if VIDEO_DEV
+
+comment "IR I2C driver auto-selected by 'Autoselect ancillary drivers'"
+	depends on MEDIA_SUBDRV_AUTOSELECT && I2C && RC_CORE
+
+config VIDEO_IR_I2C
+	tristate "I2C module for IR" if !MEDIA_SUBDRV_AUTOSELECT || EXPERT
+	depends on I2C && RC_CORE
+	default y
+	help
+	  Most boards have an IR chip directly connected via GPIO. However,
+	  some video boards have the IR connected via I2C bus.
+
+	  If your board doesn't have an I2C IR chip, you may disable this
+	  option.
+
+	  In doubt, say Y.
+
+#
+# V4L2 I2C drivers that are related with Camera support
+#
+
+menuconfig VIDEO_CAMERA_SENSOR
+	bool "Camera sensor devices"
+	depends on MEDIA_CAMERA_SUPPORT && I2C
+	select MEDIA_CONTROLLER
+	select V4L2_FWNODE
+	select VIDEO_V4L2_SUBDEV_API
+	default y
+
+if VIDEO_CAMERA_SENSOR
+
+config VIDEO_APTINA_PLL
+	tristate
+
+config VIDEO_CCS_PLL
+	tristate
+
+config VIDEO_AR0521
+	tristate "ON Semiconductor AR0521 sensor support"
+	help
+	  This is a Video4Linux2 sensor driver for the ON Semiconductor
+	  AR0521 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ar0521.
+
+config VIDEO_HI556
+	tristate "Hynix Hi-556 sensor support"
+	help
+	  This is a Video4Linux2 sensor driver for the Hynix
+	  Hi-556 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called hi556.
+
+config VIDEO_HI846
+	tristate "Hynix Hi-846 sensor support"
+	help
+	  This is a Video4Linux2 sensor driver for the Hynix
+	  Hi-846 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called hi846.
+
+config VIDEO_HI847
+        tristate "Hynix Hi-847 sensor support"
+        help
+          This is a Video4Linux2 sensor driver for the Hynix
+          Hi-847 camera.
+
+          To compile this driver as a module, choose M here: the
+          module will be called hi847.
+
+config VIDEO_IMX208
+	tristate "Sony IMX208 sensor support"
+	help
+	  This is a Video4Linux2 sensor driver for the Sony
+	  IMX208 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called imx208.
+
+config VIDEO_IMX214
+	tristate "Sony IMX214 sensor support"
+	depends on GPIOLIB
+	select REGMAP_I2C
+	help
+	  This is a Video4Linux2 sensor driver for the Sony
+	  IMX214 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called imx214.
+
+config VIDEO_IMX219
+	tristate "Sony IMX219 sensor support"
+	select V4L2_CCI_I2C
+	help
+	  This is a Video4Linux2 sensor driver for the Sony
+	  IMX219 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called imx219.
+
+config VIDEO_IMX258
+	tristate "Sony IMX258 sensor support"
+	help
+	  This is a Video4Linux2 sensor driver for the Sony
+	  IMX258 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called imx258.
+
+config VIDEO_IMX274
+	tristate "Sony IMX274 sensor support"
+	select REGMAP_I2C
+	help
+	  This is a V4L2 sensor driver for the Sony IMX274
+	  CMOS image sensor.
+
+config VIDEO_IMX290
+	tristate "Sony IMX290 sensor support"
+	select REGMAP_I2C
+	select V4L2_CCI_I2C
+	help
+	  This is a Video4Linux2 sensor driver for the Sony
+	  IMX290 camera sensor.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called imx290.
+
+config VIDEO_IMX296
+	tristate "Sony IMX296 sensor support"
+	help
+	  This is a Video4Linux2 sensor driver for the Sony
+	  IMX296 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called imx296.
+
+config VIDEO_IMX319
+	tristate "Sony IMX319 sensor support"
+	help
+	  This is a Video4Linux2 sensor driver for the Sony
+	  IMX319 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called imx319.
+
+config VIDEO_IMX334
+	tristate "Sony IMX334 sensor support"
+	depends on OF_GPIO
+	help
+	  This is a Video4Linux2 sensor driver for the Sony
+	  IMX334 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called imx334.
+
+config VIDEO_IMX335
+	tristate "Sony IMX335 sensor support"
+	depends on OF_GPIO
+	help
+	  This is a Video4Linux2 sensor driver for the Sony
+	  IMX335 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called imx335.
+
+config VIDEO_IMX355
+	tristate "Sony IMX355 sensor support"
+	help
+	  This is a Video4Linux2 sensor driver for the Sony
+	  IMX355 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called imx355.
+
+config VIDEO_IMX412
+	tristate "Sony IMX412 sensor support"
+	depends on OF_GPIO
+	help
+	  This is a Video4Linux2 sensor driver for the Sony
+	  IMX412 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called imx412.
+
+config VIDEO_IMX415
+	tristate "Sony IMX415 sensor support"
+	depends on OF_GPIO
+	help
+	  This is a Video4Linux2 sensor driver for the Sony
+	  IMX415 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called imx415.
+
+config VIDEO_MAX9271_LIB
+	tristate
+
+config VIDEO_MT9M001
+	tristate "mt9m001 support"
+	help
+	  This driver supports MT9M001 cameras from Micron, monochrome
+	  and colour models.
+
+config VIDEO_MT9M111
+	tristate "mt9m111, mt9m112 and mt9m131 support"
+	help
+	  This driver supports MT9M111, MT9M112 and MT9M131 cameras from
+	  Micron/Aptina
+
+config VIDEO_MT9P031
+	tristate "Aptina MT9P031 support"
+	select VIDEO_APTINA_PLL
+	help
+	  This is a Video4Linux2 sensor driver for the Aptina
+	  (Micron) mt9p031 5 Mpixel camera.
+
+config VIDEO_MT9T112
+	tristate "Aptina MT9T111/MT9T112 support"
+	help
+	  This is a Video4Linux2 sensor driver for the Aptina
+	  (Micron) MT9T111 and MT9T112 3 Mpixel camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called mt9t112.
+
+config VIDEO_MT9V011
+	tristate "Micron mt9v011 sensor support"
+	help
+	  This is a Video4Linux2 sensor driver for the Micron
+	  mt0v011 1.3 Mpixel camera.  It currently only works with the
+	  em28xx driver.
+
+config VIDEO_MT9V032
+	tristate "Micron MT9V032 sensor support"
+	select REGMAP_I2C
+	help
+	  This is a Video4Linux2 sensor driver for the Micron
+	  MT9V032 752x480 CMOS sensor.
+
+config VIDEO_MT9V111
+	tristate "Aptina MT9V111 sensor support"
+	help
+	  This is a Video4Linux2 sensor driver for the Aptina/Micron
+	  MT9V111 sensor.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called mt9v111.
+
+config VIDEO_OG01A1B
+	tristate "OmniVision OG01A1B sensor support"
+	help
+	  This is a Video4Linux2 sensor driver for the OmniVision
+	  OG01A1B camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called og01a1b.
+
+config VIDEO_OV01A10
+	tristate "OmniVision OV01A10 sensor support"
+	help
+	  This is a Video4Linux2 sensor driver for the OmniVision
+	  OV01A10 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ov01a10.
+
+config VIDEO_OV02A10
+	tristate "OmniVision OV02A10 sensor support"
+	help
+	  This is a Video4Linux2 sensor driver for the OmniVision
+	  OV02A10 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ov02a10.
+
+config VIDEO_OV08D10
+        tristate "OmniVision OV08D10 sensor support"
+        help
+          This is a Video4Linux2 sensor driver for the OmniVision
+          OV08D10 camera sensor.
+
+          To compile this driver as a module, choose M here: the
+          module will be called ov08d10.
+
+config VIDEO_OV08X40
+	tristate "OmniVision OV08X40 sensor support"
+	help
+	  This is a Video4Linux2 sensor driver for the OmniVision
+	  OV08X40 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ov08x40.
+
+config VIDEO_OV13858
+	tristate "OmniVision OV13858 sensor support"
+	help
+	  This is a Video4Linux2 sensor driver for the OmniVision
+	  OV13858 camera.
+
+config VIDEO_OV13B10
+	tristate "OmniVision OV13B10 sensor support"
+	help
+	  This is a Video4Linux2 sensor driver for the OmniVision
+	  OV13B10 camera.
+
+config VIDEO_OV2640
+	tristate "OmniVision OV2640 sensor support"
+	help
+	  This is a Video4Linux2 sensor driver for the OmniVision
+	  OV2640 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ov2640.
+
+config VIDEO_OV2659
+	tristate "OmniVision OV2659 sensor support"
+	depends on GPIOLIB
+	help
+	  This is a Video4Linux2 sensor driver for the OmniVision
+	  OV2659 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ov2659.
+
+config VIDEO_OV2680
+	tristate "OmniVision OV2680 sensor support"
+	select V4L2_CCI_I2C
+	help
+	  This is a Video4Linux2 sensor driver for the OmniVision
+	  OV2680 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ov2680.
+
+config VIDEO_OV2685
+	tristate "OmniVision OV2685 sensor support"
+	help
+	  This is a Video4Linux2 sensor driver for the OmniVision
+	  OV2685 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ov2685.
+
+config VIDEO_OV2740
+	tristate "OmniVision OV2740 sensor support"
+	depends on ACPI || COMPILE_TEST
+	select REGMAP_I2C
+	help
+	  This is a Video4Linux2 sensor driver for the OmniVision
+	  OV2740 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ov2740.
+
+config VIDEO_OV4689
+	tristate "OmniVision OV4689 sensor support"
+	depends on GPIOLIB
+	help
+	  This is a Video4Linux2 sensor-level driver for the OmniVision
+	  OV4689 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ov4689.
+
+config VIDEO_OV5640
+	tristate "OmniVision OV5640 sensor support"
+	depends on OF
+	depends on GPIOLIB
+	help
+	  This is a Video4Linux2 sensor driver for the Omnivision
+	  OV5640 camera sensor with a MIPI CSI-2 interface.
+
+config VIDEO_OV5645
+	tristate "OmniVision OV5645 sensor support"
+	depends on OF
+	help
+	  This is a Video4Linux2 sensor driver for the OmniVision
+	  OV5645 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ov5645.
+
+config VIDEO_OV5647
+	tristate "OmniVision OV5647 sensor support"
+	help
+	  This is a Video4Linux2 sensor driver for the OmniVision
+	  OV5647 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ov5647.
+
+config VIDEO_OV5648
+	tristate "OmniVision OV5648 sensor support"
+	depends on PM
+	help
+	  This is a Video4Linux2 sensor driver for the OmniVision
+	  OV5648 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ov5648.
+
+config VIDEO_OV5670
+	tristate "OmniVision OV5670 sensor support"
+	help
+	  This is a Video4Linux2 sensor driver for the OmniVision
+	  OV5670 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ov5670.
+
+config VIDEO_OV5675
+	tristate "OmniVision OV5675 sensor support"
+	help
+	  This is a Video4Linux2 sensor driver for the OmniVision
+	  OV5675 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ov5675.
+
+config VIDEO_OV5693
+	tristate "OmniVision OV5693 sensor support"
+	select V4L2_CCI_I2C
+	help
+	  This is a Video4Linux2 sensor driver for the OmniVision
+	  OV5693 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ov5693.
+
+config VIDEO_OV5695
+	tristate "OmniVision OV5695 sensor support"
+	help
+	  This is a Video4Linux2 sensor driver for the OmniVision
+	  OV5695 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ov5695.
+
+config VIDEO_OV6650
+	tristate "OmniVision OV6650 sensor support"
+	help
+	  This is a Video4Linux2 sensor driver for the OmniVision
+	  OV6650 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ov6650.
+
+config VIDEO_OV7251
+	tristate "OmniVision OV7251 sensor support"
+	help
+	  This is a Video4Linux2 sensor driver for the OmniVision
+	  OV7251 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ov7251.
+
+config VIDEO_OV7640
+	tristate "OmniVision OV7640 sensor support"
+	help
+	  This is a Video4Linux2 sensor driver for the OmniVision
+	  OV7640 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ov7640.
+
+config VIDEO_OV7670
+	tristate "OmniVision OV7670 sensor support"
+	help
+	  This is a Video4Linux2 sensor driver for the OmniVision
+	  OV7670 VGA camera.  It currently only works with the M88ALP01
+	  controller.
+
+config VIDEO_OV772X
+	tristate "OmniVision OV772x sensor support"
+	select REGMAP_SCCB
+	help
+	  This is a Video4Linux2 sensor driver for the OmniVision
+	  OV772x camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ov772x.
+
+config VIDEO_OV7740
+	tristate "OmniVision OV7740 sensor support"
+	select REGMAP_SCCB
+	help
+	  This is a Video4Linux2 sensor driver for the OmniVision
+	  OV7740 VGA camera sensor.
+
+config VIDEO_OV8856
+	tristate "OmniVision OV8856 sensor support"
+	help
+	  This is a Video4Linux2 sensor driver for the OmniVision
+	  OV8856 camera sensor.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ov8856.
+
+config VIDEO_OV8858
+	tristate "OmniVision OV8858 sensor support"
+	depends on PM
+	help
+	  This is a Video4Linux2 sensor driver for OmniVision
+	  OV8858 camera sensor.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ov8858.
+
+config VIDEO_OV8865
+	tristate "OmniVision OV8865 sensor support"
+	depends on PM
+	help
+	  This is a Video4Linux2 sensor driver for OmniVision
+	  OV8865 camera sensor.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ov8865.
+
+config VIDEO_OV9282
+	tristate "OmniVision OV9282 sensor support"
+	depends on OF_GPIO
+	help
+	  This is a Video4Linux2 sensor driver for the OmniVision
+	  OV9282 camera sensor.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ov9282.
+
+config VIDEO_OV9640
+	tristate "OmniVision OV9640 sensor support"
+	help
+	  This is a Video4Linux2 sensor driver for the OmniVision
+	  OV9640 camera sensor.
+
+config VIDEO_OV9650
+	tristate "OmniVision OV9650/OV9652 sensor support"
+	select REGMAP_SCCB
+	help
+	  This is a V4L2 sensor driver for the Omnivision
+	  OV9650 and OV9652 camera sensors.
+
+config VIDEO_OV9734
+	tristate "OmniVision OV9734 sensor support"
+	depends on ACPI || COMPILE_TEST
+	help
+	  This is a Video4Linux2 sensor driver for the OmniVision
+	  OV9734 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module's name is ov9734.
+
+config VIDEO_RDACM20
+	tristate "IMI RDACM20 camera support"
+	select VIDEO_MAX9271_LIB
+	help
+	  This driver supports the IMI RDACM20 GMSL camera, used in
+	  ADAS systems.
+
+	  This camera should be used in conjunction with a GMSL
+	  deserialiser such as the MAX9286.
+
+config VIDEO_RDACM21
+	tristate "IMI RDACM21 camera support"
+	select VIDEO_MAX9271_LIB
+	help
+	  This driver supports the IMI RDACM21 GMSL camera, used in
+	  ADAS systems.
+
+	  This camera should be used in conjunction with a GMSL
+	  deserialiser such as the MAX9286.
+
+config VIDEO_RJ54N1
+	tristate "Sharp RJ54N1CB0C sensor support"
+	help
+	  This is a V4L2 sensor driver for Sharp RJ54N1CB0C CMOS image
+	  sensor.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called rj54n1.
+
+config VIDEO_S5C73M3
+	tristate "Samsung S5C73M3 sensor support"
+	depends on SPI
+	help
+	  This is a V4L2 sensor driver for Samsung S5C73M3
+	  8 Mpixel camera.
+
+config VIDEO_S5K5BAF
+	tristate "Samsung S5K5BAF sensor support"
+	help
+	  This is a V4L2 sensor driver for Samsung S5K5BAF 2M
+	  camera sensor with an embedded SoC image signal processor.
+
+config VIDEO_S5K6A3
+	tristate "Samsung S5K6A3 sensor support"
+	help
+	  This is a V4L2 sensor driver for Samsung S5K6A3 raw
+	  camera sensor.
+
+config VIDEO_ST_VGXY61
+	tristate "ST VGXY61 sensor support"
+	depends on OF && GPIOLIB
+	help
+	  This is a Video4Linux2 sensor driver for the ST VGXY61
+	  camera sensor.
+
+source "drivers/media/i2c/ccs/Kconfig"
+source "drivers/media/i2c/et8ek8/Kconfig"
+
+endif
+
+menu "Lens drivers"
+	visible if MEDIA_CAMERA_SUPPORT
+
+config VIDEO_AD5820
+	tristate "AD5820 lens voice coil support"
+	depends on GPIOLIB && I2C && VIDEO_DEV
+	select MEDIA_CONTROLLER
+	select V4L2_ASYNC
+	help
+	  This is a driver for the AD5820 camera lens voice coil.
+	  It is used for example in Nokia N900 (RX-51).
+
+config VIDEO_AK7375
+	tristate "AK7375 lens voice coil support"
+	depends on I2C && VIDEO_DEV
+	select MEDIA_CONTROLLER
+	select VIDEO_V4L2_SUBDEV_API
+	select V4L2_ASYNC
+	help
+	  This is a driver for the AK7375 camera lens voice coil.
+	  AK7375 is a 12 bit DAC with 120mA output current sink
+	  capability. This is designed for linear control of
+	  voice coil motors, controlled via I2C serial interface.
+
+config VIDEO_DW9714
+	tristate "DW9714 lens voice coil support"
+	depends on I2C && VIDEO_DEV
+	select MEDIA_CONTROLLER
+	select VIDEO_V4L2_SUBDEV_API
+	select V4L2_ASYNC
+	help
+	  This is a driver for the DW9714 camera lens voice coil.
+	  DW9714 is a 10 bit DAC with 120mA output current sink
+	  capability. This is designed for linear control of
+	  voice coil motors, controlled via I2C serial interface.
+
+config VIDEO_DW9719
+	tristate "DW9719 lens voice coil support"
+	depends on I2C && VIDEO_DEV
+	select MEDIA_CONTROLLER
+	select VIDEO_V4L2_SUBDEV_API
+	select V4L2_ASYNC
+	select V4L2_CCI_I2C
+	help
+	  This is a driver for the DW9719 camera lens voice coil.
+	  This is designed for linear control of voice coil motors,
+	  controlled via I2C serial interface.
+
+config VIDEO_DW9768
+	tristate "DW9768 lens voice coil support"
+	depends on I2C && VIDEO_DEV
+	select MEDIA_CONTROLLER
+	select VIDEO_V4L2_SUBDEV_API
+	select V4L2_FWNODE
+	help
+	  This is a driver for the DW9768 camera lens voice coil.
+	  DW9768 is a 10 bit DAC with 100mA output current sink
+	  capability. This is designed for linear control of
+	  voice coil motors, controlled via I2C serial interface.
+
+config VIDEO_DW9807_VCM
+	tristate "DW9807 lens voice coil support"
+	depends on I2C && VIDEO_DEV
+	select MEDIA_CONTROLLER
+	select VIDEO_V4L2_SUBDEV_API
+	select V4L2_ASYNC
+	help
+	  This is a driver for the DW9807 camera lens voice coil.
+	  DW9807 is a 10 bit DAC with 100mA output current sink
+	  capability. This is designed for linear control of
+	  voice coil motors, controlled via I2C serial interface.
+
+endmenu
+
+menu "Flash devices"
+	visible if MEDIA_CAMERA_SUPPORT
+
+config VIDEO_ADP1653
+	tristate "ADP1653 flash support"
+	depends on I2C && VIDEO_DEV
+	select MEDIA_CONTROLLER
+	select V4L2_ASYNC
+	help
+	  This is a driver for the ADP1653 flash controller. It is used for
+	  example in Nokia N900.
+
+config VIDEO_LM3560
+	tristate "LM3560 dual flash driver support"
+	depends on I2C && VIDEO_DEV
+	select MEDIA_CONTROLLER
+	select REGMAP_I2C
+	select V4L2_ASYNC
+	help
+	  This is a driver for the lm3560 dual flash controllers. It controls
+	  flash, torch LEDs.
+
+config VIDEO_LM3646
+	tristate "LM3646 dual flash driver support"
+	depends on I2C && VIDEO_DEV
+	select MEDIA_CONTROLLER
+	select REGMAP_I2C
+	select V4L2_ASYNC
+	help
+	  This is a driver for the lm3646 dual flash controllers. It controls
+	  flash, torch LEDs.
+
+endmenu
+
+#
+# V4L2 I2C drivers that aren't related with Camera support
+#
+
+comment "audio, video and radio I2C drivers auto-selected by 'Autoselect ancillary drivers'"
+	depends on MEDIA_HIDE_ANCILLARY_SUBDRV
+#
+# Encoder / Decoder module configuration
+#
+
+menu "Audio decoders, processors and mixers"
+	visible if !MEDIA_HIDE_ANCILLARY_SUBDRV
+
+config VIDEO_CS3308
+	tristate "Cirrus Logic CS3308 audio ADC"
+	depends on VIDEO_DEV && I2C
+	help
+	  Support for the Cirrus Logic CS3308 High Performance 8-Channel
+	  Analog Volume Control
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called cs3308.
+
+config VIDEO_CS5345
+	tristate "Cirrus Logic CS5345 audio ADC"
+	depends on VIDEO_DEV && I2C
+	help
+	  Support for the Cirrus Logic CS5345 24-bit, 192 kHz
+	  stereo A/D converter.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called cs5345.
+
+config VIDEO_CS53L32A
+	tristate "Cirrus Logic CS53L32A audio ADC"
+	depends on VIDEO_DEV && I2C
+	help
+	  Support for the Cirrus Logic CS53L32A low voltage
+	  stereo A/D converter.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called cs53l32a.
+
+config VIDEO_MSP3400
+	tristate "Micronas MSP34xx audio decoders"
+	depends on VIDEO_DEV && I2C
+	help
+	  Support for the Micronas MSP34xx series of audio decoders.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called msp3400.
+
+config VIDEO_SONY_BTF_MPX
+	tristate "Sony BTF's internal MPX"
+	depends on VIDEO_DEV && I2C
+	help
+	  Support for the internal MPX of the Sony BTF-PG472Z tuner.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called sony-btf-mpx.
+
+config VIDEO_TDA1997X
+	tristate "NXP TDA1997x HDMI receiver"
+	depends on VIDEO_DEV && I2C
+	depends on SND_SOC
+	select HDMI
+	select SND_PCM
+	select V4L2_FWNODE
+	select MEDIA_CONTROLLER
+	select VIDEO_V4L2_SUBDEV_API
+	help
+	  V4L2 subdevice driver for the NXP TDA1997x HDMI receivers.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called tda1997x.
+
+config VIDEO_TDA7432
+	tristate "Philips TDA7432 audio processor"
+	depends on VIDEO_DEV && I2C
+	help
+	  Support for tda7432 audio decoder chip found on some bt8xx boards.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called tda7432.
+
+config VIDEO_TDA9840
+	tristate "Philips TDA9840 audio processor"
+	depends on I2C
+	help
+	  Support for tda9840 audio decoder chip found on some Zoran boards.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called tda9840.
+
+config VIDEO_TEA6415C
+	tristate "Philips TEA6415C audio processor"
+	depends on I2C
+	help
+	  Support for tea6415c audio decoder chip found on some bt8xx boards.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called tea6415c.
+
+config VIDEO_TEA6420
+	tristate "Philips TEA6420 audio processor"
+	depends on I2C
+	help
+	  Support for tea6420 audio decoder chip found on some bt8xx boards.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called tea6420.
+
+config VIDEO_TLV320AIC23B
+	tristate "Texas Instruments TLV320AIC23B audio codec"
+	depends on VIDEO_DEV && I2C
+	help
+	  Support for the Texas Instruments TLV320AIC23B audio codec.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called tlv320aic23b.
+
+config VIDEO_TVAUDIO
+	tristate "Simple audio decoder chips"
+	depends on VIDEO_DEV && I2C
+	help
+	  Support for several audio decoder chips found on some bt8xx boards:
+	  Philips: tda9840, tda9873h, tda9874h/a, tda9850, tda985x, tea6300,
+		   tea6320, tea6420, tda8425, ta8874z.
+	  Microchip: pic16c54 based design on ProVideo PV951 board.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called tvaudio.
+
+config VIDEO_UDA1342
+	tristate "Philips UDA1342 audio codec"
+	depends on VIDEO_DEV && I2C
+	help
+	  Support for the Philips UDA1342 audio codec.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called uda1342.
+
+config VIDEO_VP27SMPX
+	tristate "Panasonic VP27's internal MPX"
+	depends on VIDEO_DEV && I2C
+	help
+	  Support for the internal MPX of the Panasonic VP27s tuner.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called vp27smpx.
+
+config VIDEO_WM8739
+	tristate "Wolfson Microelectronics WM8739 stereo audio ADC"
+	depends on VIDEO_DEV && I2C
+	help
+	  Support for the Wolfson Microelectronics WM8739
+	  stereo A/D Converter.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called wm8739.
+
+config VIDEO_WM8775
+	tristate "Wolfson Microelectronics WM8775 audio ADC with input mixer"
+	depends on VIDEO_DEV && I2C
+	help
+	  Support for the Wolfson Microelectronics WM8775 high
+	  performance stereo A/D Converter with a 4 channel input mixer.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called wm8775.
+
+endmenu
+
+menu "RDS decoders"
+	visible if !MEDIA_HIDE_ANCILLARY_SUBDRV
+
+config VIDEO_SAA6588
+	tristate "SAA6588 Radio Chip RDS decoder support"
+	depends on VIDEO_DEV && I2C
+
+	help
+	  Support for this Radio Data System (RDS) decoder. This allows
+	  seeing radio station identification transmitted using this
+	  standard.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called saa6588.
+
+endmenu
+
+menu "Video decoders"
+	visible if !MEDIA_HIDE_ANCILLARY_SUBDRV
+
+config VIDEO_ADV7180
+	tristate "Analog Devices ADV7180 decoder"
+	depends on GPIOLIB && VIDEO_DEV && I2C
+	select MEDIA_CONTROLLER
+	select VIDEO_V4L2_SUBDEV_API
+	select V4L2_ASYNC
+	help
+	  Support for the Analog Devices ADV7180 video decoder.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called adv7180.
+
+config VIDEO_ADV7183
+	tristate "Analog Devices ADV7183 decoder"
+	depends on VIDEO_DEV && I2C
+	help
+	  V4l2 subdevice driver for the Analog Devices
+	  ADV7183 video decoder.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called adv7183.
+
+config VIDEO_ADV748X
+	tristate "Analog Devices ADV748x decoder"
+	depends on VIDEO_DEV && I2C
+	depends on OF
+	select MEDIA_CONTROLLER
+	select VIDEO_V4L2_SUBDEV_API
+	select REGMAP_I2C
+	select V4L2_FWNODE
+	help
+	  V4L2 subdevice driver for the Analog Devices
+	  ADV7481 and ADV7482 HDMI/Analog video decoders.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called adv748x.
+
+config VIDEO_ADV7604
+	tristate "Analog Devices ADV7604 decoder"
+	depends on VIDEO_DEV && I2C
+	depends on GPIOLIB || COMPILE_TEST
+	select MEDIA_CONTROLLER
+	select VIDEO_V4L2_SUBDEV_API
+	select REGMAP_I2C
+	select HDMI
+	select V4L2_FWNODE
+	help
+	  Support for the Analog Devices ADV7604 video decoder.
+
+	  This is a Analog Devices Component/Graphics Digitizer
+	  with 4:1 Multiplexed HDMI Receiver.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called adv7604.
+
+config VIDEO_ADV7604_CEC
+	bool "Enable Analog Devices ADV7604 CEC support"
+	depends on VIDEO_ADV7604
+	select CEC_CORE
+	help
+	  When selected the adv7604 will support the optional
+	  HDMI CEC feature.
+
+config VIDEO_ADV7842
+	tristate "Analog Devices ADV7842 decoder"
+	depends on VIDEO_DEV && I2C
+	select MEDIA_CONTROLLER
+	select VIDEO_V4L2_SUBDEV_API
+	select HDMI
+	help
+	  Support for the Analog Devices ADV7842 video decoder.
+
+	  This is a Analog Devices Component/Graphics/SD Digitizer
+	  with 2:1 Multiplexed HDMI Receiver.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called adv7842.
+
+config VIDEO_ADV7842_CEC
+	bool "Enable Analog Devices ADV7842 CEC support"
+	depends on VIDEO_ADV7842
+	select CEC_CORE
+	help
+	  When selected the adv7842 will support the optional
+	  HDMI CEC feature.
+
+config VIDEO_BT819
+	tristate "BT819A VideoStream decoder"
+	depends on VIDEO_DEV && I2C
+	help
+	  Support for BT819A video decoder.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called bt819.
+
+config VIDEO_BT856
+	tristate "BT856 VideoStream decoder"
+	depends on VIDEO_DEV && I2C
+	help
+	  Support for BT856 video decoder.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called bt856.
+
+config VIDEO_BT866
+	tristate "BT866 VideoStream decoder"
+	depends on VIDEO_DEV && I2C
+	help
+	  Support for BT866 video decoder.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called bt866.
+
+config VIDEO_ISL7998X
+	tristate "Intersil ISL7998x video decoder"
+	depends on VIDEO_DEV && I2C
+	depends on OF_GPIO
+	select MEDIA_CONTROLLER
+	select VIDEO_V4L2_SUBDEV_API
+	select V4L2_FWNODE
+	help
+	  Support for Intersil ISL7998x analog to MIPI-CSI2 or
+	  BT.656 decoder.
+
+config VIDEO_KS0127
+	tristate "KS0127 video decoder"
+	depends on VIDEO_DEV && I2C
+	help
+	  Support for KS0127 video decoder.
+
+	  This chip is used on AverMedia AVS6EYES Zoran-based MJPEG
+	  cards.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ks0127.
+
+config VIDEO_MAX9286
+	tristate "Maxim MAX9286 GMSL deserializer support"
+	depends on I2C && I2C_MUX
+	depends on VIDEO_DEV
+	depends on OF_GPIO
+	select V4L2_FWNODE
+	select VIDEO_V4L2_SUBDEV_API
+	select MEDIA_CONTROLLER
+	help
+	  This driver supports the Maxim MAX9286 GMSL deserializer.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called max9286.
+
+config VIDEO_ML86V7667
+	tristate "OKI ML86V7667 video decoder"
+	depends on VIDEO_DEV && I2C
+	help
+	  Support for the OKI Semiconductor ML86V7667 video decoder.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ml86v7667.
+
+config VIDEO_SAA7110
+	tristate "Philips SAA7110 video decoder"
+	depends on VIDEO_DEV && I2C
+	help
+	  Support for the Philips SAA7110 video decoders.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called saa7110.
+
+config VIDEO_SAA711X
+	tristate "Philips SAA7111/3/4/5 video decoders"
+	depends on VIDEO_DEV && I2C
+	help
+	  Support for the Philips SAA7111/3/4/5 video decoders.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called saa7115.
+
+config VIDEO_TC358743
+	tristate "Toshiba TC358743 decoder"
+	depends on VIDEO_DEV && I2C
+	select MEDIA_CONTROLLER
+	select VIDEO_V4L2_SUBDEV_API
+	select HDMI
+	select V4L2_FWNODE
+	help
+	  Support for the Toshiba TC358743 HDMI to MIPI CSI-2 bridge.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called tc358743.
+
+config VIDEO_TC358743_CEC
+	bool "Enable Toshiba TC358743 CEC support"
+	depends on VIDEO_TC358743
+	select CEC_CORE
+	help
+	  When selected the tc358743 will support the optional
+	  HDMI CEC feature.
+
+config VIDEO_TC358746
+	tristate "Toshiba TC358746 parallel-CSI2 bridge"
+	depends on VIDEO_DEV && PM && I2C
+	select VIDEO_V4L2_SUBDEV_API
+	select MEDIA_CONTROLLER
+	select V4L2_FWNODE
+	select GENERIC_PHY
+	select GENERIC_PHY_MIPI_DPHY
+	select REGMAP_I2C
+	help
+	  Support for the Toshiba TC358746 parallel to MIPI CSI-2 bridge.
+	  The bridge can work in both directions but currently only the
+	  parallel-in / csi-out path is supported.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called tc358746.
+
+config VIDEO_TVP514X
+	tristate "Texas Instruments TVP514x video decoder"
+	depends on VIDEO_DEV && I2C
+	select V4L2_FWNODE
+	help
+	  This is a Video4Linux2 sensor driver for the TI TVP5146/47
+	  decoder. It is currently working with the TI OMAP3 camera
+	  controller.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called tvp514x.
+
+config VIDEO_TVP5150
+	tristate "Texas Instruments TVP5150 video decoder"
+	depends on VIDEO_DEV && I2C
+	select V4L2_FWNODE
+	select REGMAP_I2C
+	help
+	  Support for the Texas Instruments TVP5150 video decoder.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called tvp5150.
+
+config VIDEO_TVP7002
+	tristate "Texas Instruments TVP7002 video decoder"
+	depends on VIDEO_DEV && I2C
+	select V4L2_FWNODE
+	help
+	  Support for the Texas Instruments TVP7002 video decoder.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called tvp7002.
+
+config VIDEO_TW2804
+	tristate "Techwell TW2804 multiple video decoder"
+	depends on VIDEO_DEV && I2C
+	help
+	  Support for the Techwell tw2804 multiple video decoder.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called tw2804.
+
+config VIDEO_TW9903
+	tristate "Techwell TW9903 video decoder"
+	depends on VIDEO_DEV && I2C
+	help
+	  Support for the Techwell tw9903 multi-standard video decoder
+	  with high quality down scaler.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called tw9903.
+
+config VIDEO_TW9906
+	tristate "Techwell TW9906 video decoder"
+	depends on VIDEO_DEV && I2C
+	help
+	  Support for the Techwell tw9906 enhanced multi-standard comb filter
+	  video decoder with YCbCr input support.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called tw9906.
+
+config VIDEO_TW9910
+	tristate "Techwell TW9910 video decoder"
+	depends on VIDEO_DEV && I2C
+	select V4L2_ASYNC
+	help
+	  Support for Techwell TW9910 NTSC/PAL/SECAM video decoder.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called tw9910.
+
+config VIDEO_VPX3220
+	tristate "vpx3220a, vpx3216b & vpx3214c video decoders"
+	depends on VIDEO_DEV && I2C
+	help
+	  Support for VPX322x video decoders.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called vpx3220.
+
+comment "Video and audio decoders"
+
+config VIDEO_SAA717X
+	tristate "Philips SAA7171/3/4 audio/video decoders"
+	depends on VIDEO_DEV && I2C
+	help
+	  Support for the Philips SAA7171/3/4 audio/video decoders.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called saa717x.
+
+source "drivers/media/i2c/cx25840/Kconfig"
+
+endmenu
+
+menu "Video encoders"
+	visible if !MEDIA_HIDE_ANCILLARY_SUBDRV
+
+config VIDEO_ADV7170
+	tristate "Analog Devices ADV7170 video encoder"
+	depends on VIDEO_DEV && I2C
+	help
+	  Support for the Analog Devices ADV7170 video encoder driver
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called adv7170.
+
+config VIDEO_ADV7175
+	tristate "Analog Devices ADV7175 video encoder"
+	depends on VIDEO_DEV && I2C
+	help
+	  Support for the Analog Devices ADV7175 video encoder driver
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called adv7175.
+
+config VIDEO_ADV7343
+	tristate "ADV7343 video encoder"
+	depends on I2C
+	select V4L2_ASYNC
+	help
+	  Support for Analog Devices I2C bus based ADV7343 encoder.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called adv7343.
+
+config VIDEO_ADV7393
+	tristate "ADV7393 video encoder"
+	depends on I2C
+	help
+	  Support for Analog Devices I2C bus based ADV7393 encoder.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called adv7393.
+
+config VIDEO_ADV7511
+	tristate "Analog Devices ADV7511 encoder"
+	depends on VIDEO_DEV && I2C
+	depends on DRM_I2C_ADV7511=n || COMPILE_TEST
+	select MEDIA_CONTROLLER
+	select VIDEO_V4L2_SUBDEV_API
+	select HDMI
+	help
+	  Support for the Analog Devices ADV7511 video encoder.
+
+	  This is a Analog Devices HDMI transmitter.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called adv7511.
+
+config VIDEO_ADV7511_CEC
+	bool "Enable Analog Devices ADV7511 CEC support"
+	depends on VIDEO_ADV7511
+	select CEC_CORE
+	help
+	  When selected the adv7511 will support the optional
+	  HDMI CEC feature.
+
+config VIDEO_AK881X
+	tristate "AK8813/AK8814 video encoders"
+	depends on I2C
+	help
+	  Video output driver for AKM AK8813 and AK8814 TV encoders
+
+config VIDEO_SAA7127
+	tristate "Philips SAA7127/9 digital video encoders"
+	depends on VIDEO_DEV && I2C
+	help
+	  Support for the Philips SAA7127/9 digital video encoders.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called saa7127.
+
+config VIDEO_SAA7185
+	tristate "Philips SAA7185 video encoder"
+	depends on VIDEO_DEV && I2C
+	help
+	  Support for the Philips SAA7185 video encoder.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called saa7185.
+
+config VIDEO_THS8200
+	tristate "Texas Instruments THS8200 video encoder"
+	depends on VIDEO_DEV && I2C
+	select V4L2_ASYNC
+	help
+	  Support for the Texas Instruments THS8200 video encoder.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ths8200.
+
+endmenu
+
+menu "Video improvement chips"
+	visible if !MEDIA_HIDE_ANCILLARY_SUBDRV
+
+config VIDEO_UPD64031A
+	tristate "NEC Electronics uPD64031A Ghost Reduction"
+	depends on VIDEO_DEV && I2C
+	select V4L2_ASYNC
+	help
+	  Support for the NEC Electronics uPD64031A Ghost Reduction
+	  video chip. It is most often found in NTSC TV cards made for
+	  Japan and is used to reduce the 'ghosting' effect that can
+	  be present in analog TV broadcasts.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called upd64031a.
+
+config VIDEO_UPD64083
+	tristate "NEC Electronics uPD64083 3-Dimensional Y/C separation"
+	depends on VIDEO_DEV && I2C
+	help
+	  Support for the NEC Electronics uPD64083 3-Dimensional Y/C
+	  separation video chip. It is used to improve the quality of
+	  the colors of a composite signal.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called upd64083.
+
+endmenu
+
+menu "Audio/Video compression chips"
+	visible if !MEDIA_HIDE_ANCILLARY_SUBDRV
+
+config VIDEO_SAA6752HS
+	tristate "Philips SAA6752HS MPEG-2 Audio/Video Encoder"
+	depends on VIDEO_DEV && I2C
+	select CRC32
+	help
+	  Support for the Philips SAA6752HS MPEG-2 video and MPEG-audio/AC-3
+	  audio encoder with multiplexer.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called saa6752hs.
+
+endmenu
+
+menu "SDR tuner chips"
+	visible if !MEDIA_HIDE_ANCILLARY_SUBDRV
+
+config SDR_MAX2175
+	tristate "Maxim 2175 RF to Bits tuner"
+	depends on VIDEO_DEV && MEDIA_SDR_SUPPORT && I2C
+	select REGMAP_I2C
+	select V4L2_ASYNC
+	help
+	  Support for Maxim 2175 tuner. It is an advanced analog/digital
+	  radio receiver with RF-to-Bits front-end designed for SDR solutions.
+
+	  To compile this driver as a module, choose M here; the
+	  module will be called max2175.
+
+endmenu
+
+menu "Miscellaneous helper chips"
+	visible if !MEDIA_HIDE_ANCILLARY_SUBDRV
+
+config VIDEO_I2C
+	tristate "I2C transport video support"
+	depends on VIDEO_DEV && I2C
+	select VIDEOBUF2_VMALLOC
+	imply HWMON
+	help
+	  Enable the I2C transport video support which supports the
+	  following:
+	   * Panasonic AMG88xx Grid-Eye Sensors
+	   * Melexis MLX90640 Thermal Cameras
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called video-i2c
+
+config VIDEO_M52790
+	tristate "Mitsubishi M52790 A/V switch"
+	depends on VIDEO_DEV && I2C
+	help
+	 Support for the Mitsubishi M52790 A/V switch.
+
+	 To compile this driver as a module, choose M here: the
+	 module will be called m52790.
+
+config VIDEO_ST_MIPID02
+	tristate "STMicroelectronics MIPID02 CSI-2 to PARALLEL bridge"
+	depends on I2C && VIDEO_DEV
+	select MEDIA_CONTROLLER
+	select VIDEO_V4L2_SUBDEV_API
+	select V4L2_FWNODE
+	help
+	  Support for STMicroelectronics MIPID02 CSI-2 to PARALLEL bridge.
+	  It is used to allow usage of CSI-2 sensor with PARALLEL port
+	  controller.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called st-mipid02.
+
+config VIDEO_THS7303
+	tristate "THS7303/53 Video Amplifier"
+	depends on VIDEO_DEV && I2C
+	select V4L2_ASYNC
+	help
+	  Support for TI THS7303/53 video amplifier
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ths7303.
+
+endmenu
+
+#
+# Video serializers and deserializers (e.g. FPD-Link)
+#
+
+menu "Video serializers and deserializers"
+
+config VIDEO_DS90UB913
+	tristate "TI DS90UB913 FPD-Link III Serializer"
+	depends on OF && I2C && VIDEO_DEV && COMMON_CLK
+	select I2C_ATR
+	select MEDIA_CONTROLLER
+	select GPIOLIB
+	select REGMAP_I2C
+	select V4L2_FWNODE
+	select VIDEO_V4L2_SUBDEV_API
+	help
+	  Device driver for the Texas Instruments DS90UB913
+	  FPD-Link III Serializer.
+
+config VIDEO_DS90UB953
+	tristate "TI FPD-Link III/IV CSI-2 Serializers"
+	depends on OF && I2C && VIDEO_DEV && COMMON_CLK
+	select I2C_ATR
+	select MEDIA_CONTROLLER
+	select GPIOLIB
+	select REGMAP_I2C
+	select V4L2_FWNODE
+	select VIDEO_V4L2_SUBDEV_API
+	help
+	  Device driver for the Texas Instruments DS90UB953
+	  FPD-Link III Serializer and DS90UB971 FPD-Link IV Serializer.
+
+config VIDEO_DS90UB960
+	tristate "TI FPD-Link III/IV Deserializers"
+	depends on OF && I2C && VIDEO_DEV && COMMON_CLK
+	select I2C_ATR
+	select MEDIA_CONTROLLER
+	select GPIOLIB
+	select REGMAP_I2C
+	select V4L2_FWNODE
+	select VIDEO_V4L2_SUBDEV_API
+	help
+	  Device driver for the Texas Instruments DS90UB960
+	  FPD-Link III Deserializer and DS90UB9702 FPD-Link IV Deserializer.
+
+endmenu
+
+endif # VIDEO_DEV
diff --git a/drivers/media/i2c/Makefile b/drivers/media/i2c/Makefile
new file mode 100644
index 000000000..80b00d39b
--- /dev/null
+++ b/drivers/media/i2c/Makefile
@@ -0,0 +1,147 @@
+# SPDX-License-Identifier: GPL-2.0
+
+msp3400-objs	:=	msp3400-driver.o msp3400-kthreads.o
+
+obj-$(CONFIG_SDR_MAX2175) += max2175.o
+obj-$(CONFIG_VIDEO_AD5820) += ad5820.o
+obj-$(CONFIG_VIDEO_ADP1653) += adp1653.o
+obj-$(CONFIG_VIDEO_ADV7170) += adv7170.o
+obj-$(CONFIG_VIDEO_ADV7175) += adv7175.o
+obj-$(CONFIG_VIDEO_ADV7180) += adv7180.o
+obj-$(CONFIG_VIDEO_ADV7183) += adv7183.o
+obj-$(CONFIG_VIDEO_ADV7343) += adv7343.o
+obj-$(CONFIG_VIDEO_ADV7393) += adv7393.o
+obj-$(CONFIG_VIDEO_ADV748X) += adv748x/
+obj-$(CONFIG_VIDEO_ADV7511) += adv7511-v4l2.o
+obj-$(CONFIG_VIDEO_ADV7604) += adv7604.o
+obj-$(CONFIG_VIDEO_ADV7842) += adv7842.o
+obj-$(CONFIG_VIDEO_AK7375) += ak7375.o
+obj-$(CONFIG_VIDEO_AK881X) += ak881x.o
+obj-$(CONFIG_VIDEO_APTINA_PLL) += aptina-pll.o
+obj-$(CONFIG_VIDEO_AR0521) += ar0521.o
+obj-$(CONFIG_VIDEO_BT819) += bt819.o
+obj-$(CONFIG_VIDEO_BT856) += bt856.o
+obj-$(CONFIG_VIDEO_BT866) += bt866.o
+obj-$(CONFIG_VIDEO_CCS) += ccs/
+obj-$(CONFIG_VIDEO_CCS_PLL) += ccs-pll.o
+obj-$(CONFIG_VIDEO_CS3308) += cs3308.o
+obj-$(CONFIG_VIDEO_CS5345) += cs5345.o
+obj-$(CONFIG_VIDEO_CS53L32A) += cs53l32a.o
+obj-$(CONFIG_VIDEO_CX25840) += cx25840/
+obj-$(CONFIG_VIDEO_DS90UB913) += ds90ub913.o
+obj-$(CONFIG_VIDEO_DS90UB953) += ds90ub953.o
+obj-$(CONFIG_VIDEO_DS90UB960) += ds90ub960.o
+obj-$(CONFIG_VIDEO_DW9714) += dw9714.o
+obj-$(CONFIG_VIDEO_DW9719) += dw9719.o
+obj-$(CONFIG_VIDEO_DW9768) += dw9768.o
+obj-$(CONFIG_VIDEO_DW9807_VCM) += dw9807-vcm.o
+obj-$(CONFIG_VIDEO_ET8EK8) += et8ek8/
+obj-$(CONFIG_VIDEO_HI556) += hi556.o
+obj-$(CONFIG_VIDEO_HI846) += hi846.o
+obj-$(CONFIG_VIDEO_HI847) += hi847.o
+obj-$(CONFIG_VIDEO_I2C) += video-i2c.o
+obj-$(CONFIG_VIDEO_IMX208) += imx208.o
+obj-$(CONFIG_VIDEO_IMX214) += imx214.o
+obj-$(CONFIG_VIDEO_IMX219) += imx219.o
+obj-$(CONFIG_VIDEO_IMX258) += imx258.o
+obj-$(CONFIG_VIDEO_IMX274) += imx274.o
+obj-$(CONFIG_VIDEO_IMX290) += imx290.o
+obj-$(CONFIG_VIDEO_IMX296) += imx296.o
+obj-$(CONFIG_VIDEO_IMX319) += imx319.o
+obj-$(CONFIG_VIDEO_IMX334) += imx334.o
+obj-$(CONFIG_VIDEO_IMX335) += imx335.o
+obj-$(CONFIG_VIDEO_IMX355) += imx355.o
+obj-$(CONFIG_VIDEO_IMX412) += imx412.o
+obj-$(CONFIG_VIDEO_IMX415) += imx415.o
+obj-$(CONFIG_VIDEO_IR_I2C) += ir-kbd-i2c.o
+obj-$(CONFIG_VIDEO_ISL7998X) += isl7998x.o
+obj-$(CONFIG_VIDEO_KS0127) += ks0127.o
+obj-$(CONFIG_VIDEO_LM3560) += lm3560.o
+obj-$(CONFIG_VIDEO_LM3646) += lm3646.o
+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_MSP3400) += msp3400.o
+obj-$(CONFIG_VIDEO_MT9M001) += mt9m001.o
+obj-$(CONFIG_VIDEO_MT9M111) += mt9m111.o
+obj-$(CONFIG_VIDEO_MT9P031) += mt9p031.o
+obj-$(CONFIG_VIDEO_MT9T112) += mt9t112.o
+obj-$(CONFIG_VIDEO_MT9V011) += mt9v011.o
+obj-$(CONFIG_VIDEO_MT9V032) += mt9v032.o
+obj-$(CONFIG_VIDEO_MT9V111) += mt9v111.o
+obj-$(CONFIG_VIDEO_OG01A1B) += og01a1b.o
+obj-$(CONFIG_VIDEO_OV01A10) += ov01a10.o
+obj-$(CONFIG_VIDEO_OV02A10) += ov02a10.o
+obj-$(CONFIG_VIDEO_OV08D10) += ov08d10.o
+obj-$(CONFIG_VIDEO_OV08X40) += ov08x40.o
+obj-$(CONFIG_VIDEO_OV13858) += ov13858.o
+obj-$(CONFIG_VIDEO_OV13B10) += ov13b10.o
+obj-$(CONFIG_VIDEO_OV2640) += ov2640.o
+obj-$(CONFIG_VIDEO_OV2659) += ov2659.o
+obj-$(CONFIG_VIDEO_OV2680) += ov2680.o
+obj-$(CONFIG_VIDEO_OV2685) += ov2685.o
+obj-$(CONFIG_VIDEO_OV2740) += ov2740.o
+obj-$(CONFIG_VIDEO_OV4689) += ov4689.o
+obj-$(CONFIG_VIDEO_OV5640) += ov5640.o
+obj-$(CONFIG_VIDEO_OV5645) += ov5645.o
+obj-$(CONFIG_VIDEO_OV5647) += ov5647.o
+obj-$(CONFIG_VIDEO_OV5648) += ov5648.o
+obj-$(CONFIG_VIDEO_OV5670) += ov5670.o
+obj-$(CONFIG_VIDEO_OV5675) += ov5675.o
+obj-$(CONFIG_VIDEO_OV5693) += ov5693.o
+obj-$(CONFIG_VIDEO_OV5695) += ov5695.o
+obj-$(CONFIG_VIDEO_OV6650) += ov6650.o
+obj-$(CONFIG_VIDEO_OV7251) += ov7251.o
+obj-$(CONFIG_VIDEO_OV7640) += ov7640.o
+obj-$(CONFIG_VIDEO_OV7670) += ov7670.o
+obj-$(CONFIG_VIDEO_OV772X) += ov772x.o
+obj-$(CONFIG_VIDEO_OV7740) += ov7740.o
+obj-$(CONFIG_VIDEO_OV8856) += ov8856.o
+obj-$(CONFIG_VIDEO_OV8858) += ov8858.o
+obj-$(CONFIG_VIDEO_OV8865) += ov8865.o
+obj-$(CONFIG_VIDEO_OV9282) += ov9282.o
+obj-$(CONFIG_VIDEO_OV9640) += ov9640.o
+obj-$(CONFIG_VIDEO_OV9650) += ov9650.o
+obj-$(CONFIG_VIDEO_OV9734) += ov9734.o
+obj-$(CONFIG_VIDEO_RDACM20) += rdacm20.o
+obj-$(CONFIG_VIDEO_RDACM21) += rdacm21.o
+obj-$(CONFIG_VIDEO_RJ54N1) += rj54n1cb0c.o
+obj-$(CONFIG_VIDEO_S5C73M3) += s5c73m3/
+obj-$(CONFIG_VIDEO_S5K5BAF) += s5k5baf.o
+obj-$(CONFIG_VIDEO_S5K6A3) += s5k6a3.o
+obj-$(CONFIG_VIDEO_SAA6588) += saa6588.o
+obj-$(CONFIG_VIDEO_SAA6752HS) += saa6752hs.o
+obj-$(CONFIG_VIDEO_SAA7110) += saa7110.o
+obj-$(CONFIG_VIDEO_SAA711X) += saa7115.o
+obj-$(CONFIG_VIDEO_SAA7127) += saa7127.o
+obj-$(CONFIG_VIDEO_SAA717X) += saa717x.o
+obj-$(CONFIG_VIDEO_SAA7185) += saa7185.o
+obj-$(CONFIG_VIDEO_SONY_BTF_MPX) += sony-btf-mpx.o
+obj-$(CONFIG_VIDEO_ST_MIPID02) += st-mipid02.o
+obj-$(CONFIG_VIDEO_ST_VGXY61) += st-vgxy61.o
+obj-$(CONFIG_VIDEO_TC358743) += tc358743.o
+obj-$(CONFIG_VIDEO_TC358746) += tc358746.o
+obj-$(CONFIG_VIDEO_TDA1997X) += tda1997x.o
+obj-$(CONFIG_VIDEO_TDA7432) += tda7432.o
+obj-$(CONFIG_VIDEO_TDA9840) += tda9840.o
+obj-$(CONFIG_VIDEO_TEA6415C) += tea6415c.o
+obj-$(CONFIG_VIDEO_TEA6420) += tea6420.o
+obj-$(CONFIG_VIDEO_THS7303) += ths7303.o
+obj-$(CONFIG_VIDEO_THS8200) += ths8200.o
+obj-$(CONFIG_VIDEO_TLV320AIC23B) += tlv320aic23b.o
+obj-$(CONFIG_VIDEO_TVAUDIO) += tvaudio.o
+obj-$(CONFIG_VIDEO_TVP514X) += tvp514x.o
+obj-$(CONFIG_VIDEO_TVP5150) += tvp5150.o
+obj-$(CONFIG_VIDEO_TVP7002) += tvp7002.o
+obj-$(CONFIG_VIDEO_TW2804) += tw2804.o
+obj-$(CONFIG_VIDEO_TW9903) += tw9903.o
+obj-$(CONFIG_VIDEO_TW9906) += tw9906.o
+obj-$(CONFIG_VIDEO_TW9910) += tw9910.o
+obj-$(CONFIG_VIDEO_UDA1342) += uda1342.o
+obj-$(CONFIG_VIDEO_UPD64031A) += upd64031a.o
+obj-$(CONFIG_VIDEO_UPD64083) += upd64083.o
+obj-$(CONFIG_VIDEO_VP27SMPX) += vp27smpx.o
+obj-$(CONFIG_VIDEO_VPX3220) += vpx3220.o
+obj-$(CONFIG_VIDEO_WM8739) += wm8739.o
+obj-$(CONFIG_VIDEO_WM8775) += wm8775.o
diff --git a/drivers/media/i2c/ad5820.c b/drivers/media/i2c/ad5820.c
new file mode 100644
index 000000000..1543d24f5
--- /dev/null
+++ b/drivers/media/i2c/ad5820.c
@@ -0,0 +1,380 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * drivers/media/i2c/ad5820.c
+ *
+ * AD5820 DAC driver for camera voice coil focus.
+ *
+ * Copyright (C) 2008 Nokia Corporation
+ * Copyright (C) 2007 Texas Instruments
+ * Copyright (C) 2016 Pavel Machek <pavel@ucw.cz>
+ *
+ * Contact: Tuukka Toivonen <tuukkat76@gmail.com>
+ *	    Sakari Ailus <sakari.ailus@iki.fi>
+ *
+ * Based on af_d88.c by Texas Instruments.
+ */
+
+#include <linux/errno.h>
+#include <linux/i2c.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/regulator/consumer.h>
+#include <linux/gpio/consumer.h>
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+
+/* Register definitions */
+#define AD5820_POWER_DOWN		(1 << 15)
+#define AD5820_DAC_SHIFT		4
+#define AD5820_RAMP_MODE_LINEAR		(0 << 3)
+#define AD5820_RAMP_MODE_64_16		(1 << 3)
+
+#define CODE_TO_RAMP_US(s)	((s) == 0 ? 0 : (1 << ((s) - 1)) * 50)
+#define RAMP_US_TO_CODE(c)	fls(((c) + ((c)>>1)) / 50)
+
+#define to_ad5820_device(sd)	container_of(sd, struct ad5820_device, subdev)
+
+struct ad5820_device {
+	struct v4l2_subdev subdev;
+	struct ad5820_platform_data *platform_data;
+	struct regulator *vana;
+
+	struct v4l2_ctrl_handler ctrls;
+	u32 focus_absolute;
+	u32 focus_ramp_time;
+	u32 focus_ramp_mode;
+
+	struct gpio_desc *enable_gpio;
+
+	struct mutex power_lock;
+	int power_count;
+
+	bool standby;
+};
+
+static int ad5820_write(struct ad5820_device *coil, u16 data)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&coil->subdev);
+	struct i2c_msg msg;
+	__be16 be_data;
+	int r;
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	be_data = cpu_to_be16(data);
+	msg.addr  = client->addr;
+	msg.flags = 0;
+	msg.len   = 2;
+	msg.buf   = (u8 *)&be_data;
+
+	r = i2c_transfer(client->adapter, &msg, 1);
+	if (r < 0) {
+		dev_err(&client->dev, "write failed, error %d\n", r);
+		return r;
+	}
+
+	return 0;
+}
+
+/*
+ * Calculate status word and write it to the device based on current
+ * values of V4L2 controls. It is assumed that the stored V4L2 control
+ * values are properly limited and rounded.
+ */
+static int ad5820_update_hw(struct ad5820_device *coil)
+{
+	u16 status;
+
+	status = RAMP_US_TO_CODE(coil->focus_ramp_time);
+	status |= coil->focus_ramp_mode
+		? AD5820_RAMP_MODE_64_16 : AD5820_RAMP_MODE_LINEAR;
+	status |= coil->focus_absolute << AD5820_DAC_SHIFT;
+
+	if (coil->standby)
+		status |= AD5820_POWER_DOWN;
+
+	return ad5820_write(coil, status);
+}
+
+/*
+ * Power handling
+ */
+static int ad5820_power_off(struct ad5820_device *coil, bool standby)
+{
+	int ret = 0, ret2;
+
+	/*
+	 * Go to standby first as real power off my be denied by the hardware
+	 * (single power line control for both coil and sensor).
+	 */
+	if (standby) {
+		coil->standby = true;
+		ret = ad5820_update_hw(coil);
+	}
+
+	gpiod_set_value_cansleep(coil->enable_gpio, 0);
+
+	ret2 = regulator_disable(coil->vana);
+	if (ret)
+		return ret;
+	return ret2;
+}
+
+static int ad5820_power_on(struct ad5820_device *coil, bool restore)
+{
+	int ret;
+
+	ret = regulator_enable(coil->vana);
+	if (ret < 0)
+		return ret;
+
+	gpiod_set_value_cansleep(coil->enable_gpio, 1);
+
+	if (restore) {
+		/* Restore the hardware settings. */
+		coil->standby = false;
+		ret = ad5820_update_hw(coil);
+		if (ret)
+			goto fail;
+	}
+	return 0;
+
+fail:
+	gpiod_set_value_cansleep(coil->enable_gpio, 0);
+	coil->standby = true;
+	regulator_disable(coil->vana);
+
+	return ret;
+}
+
+/*
+ * V4L2 controls
+ */
+static int ad5820_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ad5820_device *coil =
+		container_of(ctrl->handler, struct ad5820_device, ctrls);
+
+	switch (ctrl->id) {
+	case V4L2_CID_FOCUS_ABSOLUTE:
+		coil->focus_absolute = ctrl->val;
+		return ad5820_update_hw(coil);
+	}
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops ad5820_ctrl_ops = {
+	.s_ctrl = ad5820_set_ctrl,
+};
+
+
+static int ad5820_init_controls(struct ad5820_device *coil)
+{
+	v4l2_ctrl_handler_init(&coil->ctrls, 1);
+
+	/*
+	 * V4L2_CID_FOCUS_ABSOLUTE
+	 *
+	 * Minimum current is 0 mA, maximum is 100 mA. Thus, 1 code is
+	 * equivalent to 100/1023 = 0.0978 mA. Nevertheless, we do not use [mA]
+	 * for focus position, because it is meaningless for user. Meaningful
+	 * would be to use focus distance or even its inverse, but since the
+	 * driver doesn't have sufficiently knowledge to do the conversion, we
+	 * will just use abstract codes here. In any case, smaller value = focus
+	 * position farther from camera. The default zero value means focus at
+	 * infinity, and also least current consumption.
+	 */
+	v4l2_ctrl_new_std(&coil->ctrls, &ad5820_ctrl_ops,
+			  V4L2_CID_FOCUS_ABSOLUTE, 0, 1023, 1, 0);
+
+	if (coil->ctrls.error)
+		return coil->ctrls.error;
+
+	coil->focus_absolute = 0;
+	coil->focus_ramp_time = 0;
+	coil->focus_ramp_mode = 0;
+
+	coil->subdev.ctrl_handler = &coil->ctrls;
+
+	return 0;
+}
+
+/*
+ * V4L2 subdev operations
+ */
+static int ad5820_registered(struct v4l2_subdev *subdev)
+{
+	struct ad5820_device *coil = to_ad5820_device(subdev);
+
+	return ad5820_init_controls(coil);
+}
+
+static int
+ad5820_set_power(struct v4l2_subdev *subdev, int on)
+{
+	struct ad5820_device *coil = to_ad5820_device(subdev);
+	int ret = 0;
+
+	mutex_lock(&coil->power_lock);
+
+	/*
+	 * If the power count is modified from 0 to != 0 or from != 0 to 0,
+	 * update the power state.
+	 */
+	if (coil->power_count == !on) {
+		ret = on ? ad5820_power_on(coil, true) :
+			ad5820_power_off(coil, true);
+		if (ret < 0)
+			goto done;
+	}
+
+	/* Update the power count. */
+	coil->power_count += on ? 1 : -1;
+	WARN_ON(coil->power_count < 0);
+
+done:
+	mutex_unlock(&coil->power_lock);
+	return ret;
+}
+
+static int ad5820_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	return ad5820_set_power(sd, 1);
+}
+
+static int ad5820_close(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	return ad5820_set_power(sd, 0);
+}
+
+static const struct v4l2_subdev_core_ops ad5820_core_ops = {
+	.s_power = ad5820_set_power,
+};
+
+static const struct v4l2_subdev_ops ad5820_ops = {
+	.core = &ad5820_core_ops,
+};
+
+static const struct v4l2_subdev_internal_ops ad5820_internal_ops = {
+	.registered = ad5820_registered,
+	.open = ad5820_open,
+	.close = ad5820_close,
+};
+
+/*
+ * I2C driver
+ */
+static int __maybe_unused ad5820_suspend(struct device *dev)
+{
+	struct v4l2_subdev *subdev = dev_get_drvdata(dev);
+	struct ad5820_device *coil = to_ad5820_device(subdev);
+
+	if (!coil->power_count)
+		return 0;
+
+	return ad5820_power_off(coil, false);
+}
+
+static int __maybe_unused ad5820_resume(struct device *dev)
+{
+	struct v4l2_subdev *subdev = dev_get_drvdata(dev);
+	struct ad5820_device *coil = to_ad5820_device(subdev);
+
+	if (!coil->power_count)
+		return 0;
+
+	return ad5820_power_on(coil, true);
+}
+
+static int ad5820_probe(struct i2c_client *client)
+{
+	struct ad5820_device *coil;
+	int ret;
+
+	coil = devm_kzalloc(&client->dev, sizeof(*coil), GFP_KERNEL);
+	if (!coil)
+		return -ENOMEM;
+
+	coil->vana = devm_regulator_get(&client->dev, "VANA");
+	if (IS_ERR(coil->vana))
+		return dev_err_probe(&client->dev, PTR_ERR(coil->vana),
+				     "could not get regulator for vana\n");
+
+	coil->enable_gpio = devm_gpiod_get_optional(&client->dev, "enable",
+						    GPIOD_OUT_LOW);
+	if (IS_ERR(coil->enable_gpio))
+		return dev_err_probe(&client->dev, PTR_ERR(coil->enable_gpio),
+				     "could not get enable gpio\n");
+
+	mutex_init(&coil->power_lock);
+
+	v4l2_i2c_subdev_init(&coil->subdev, client, &ad5820_ops);
+	coil->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	coil->subdev.internal_ops = &ad5820_internal_ops;
+	coil->subdev.entity.function = MEDIA_ENT_F_LENS;
+	strscpy(coil->subdev.name, "ad5820 focus", sizeof(coil->subdev.name));
+
+	ret = media_entity_pads_init(&coil->subdev.entity, 0, NULL);
+	if (ret < 0)
+		goto clean_mutex;
+
+	ret = v4l2_async_register_subdev(&coil->subdev);
+	if (ret < 0)
+		goto clean_entity;
+
+	return ret;
+
+clean_entity:
+	media_entity_cleanup(&coil->subdev.entity);
+clean_mutex:
+	mutex_destroy(&coil->power_lock);
+	return ret;
+}
+
+static void ad5820_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct ad5820_device *coil = to_ad5820_device(subdev);
+
+	v4l2_async_unregister_subdev(&coil->subdev);
+	v4l2_ctrl_handler_free(&coil->ctrls);
+	media_entity_cleanup(&coil->subdev.entity);
+	mutex_destroy(&coil->power_lock);
+}
+
+static const struct i2c_device_id ad5820_id_table[] = {
+	{ "ad5820", 0 },
+	{ "ad5821", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ad5820_id_table);
+
+static const struct of_device_id ad5820_of_table[] = {
+	{ .compatible = "adi,ad5820" },
+	{ .compatible = "adi,ad5821" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, ad5820_of_table);
+
+static SIMPLE_DEV_PM_OPS(ad5820_pm, ad5820_suspend, ad5820_resume);
+
+static struct i2c_driver ad5820_i2c_driver = {
+	.driver		= {
+		.name	= "ad5820",
+		.pm	= &ad5820_pm,
+		.of_match_table = ad5820_of_table,
+	},
+	.probe		= ad5820_probe,
+	.remove		= ad5820_remove,
+	.id_table	= ad5820_id_table,
+};
+
+module_i2c_driver(ad5820_i2c_driver);
+
+MODULE_AUTHOR("Tuukka Toivonen");
+MODULE_DESCRIPTION("AD5820 camera lens driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/adp1653.c b/drivers/media/i2c/adp1653.c
new file mode 100644
index 000000000..98ca417b8
--- /dev/null
+++ b/drivers/media/i2c/adp1653.c
@@ -0,0 +1,547 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * drivers/media/i2c/adp1653.c
+ *
+ * Copyright (C) 2008--2011 Nokia Corporation
+ *
+ * Contact: Sakari Ailus <sakari.ailus@iki.fi>
+ *
+ * Contributors:
+ *	Sakari Ailus <sakari.ailus@iki.fi>
+ *	Tuukka Toivonen <tuukkat76@gmail.com>
+ *	Pavel Machek <pavel@ucw.cz>
+ *
+ * TODO:
+ * - fault interrupt handling
+ * - hardware strobe
+ * - power doesn't need to be ON if all lights are off
+ */
+
+#include <linux/delay.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+#include <linux/gpio/consumer.h>
+#include <media/i2c/adp1653.h>
+#include <media/v4l2-device.h>
+
+#define TIMEOUT_MAX		820000
+#define TIMEOUT_STEP		54600
+#define TIMEOUT_MIN		(TIMEOUT_MAX - ADP1653_REG_CONFIG_TMR_SET_MAX \
+				 * TIMEOUT_STEP)
+#define TIMEOUT_US_TO_CODE(t)	((TIMEOUT_MAX + (TIMEOUT_STEP / 2) - (t)) \
+				 / TIMEOUT_STEP)
+#define TIMEOUT_CODE_TO_US(c)	(TIMEOUT_MAX - (c) * TIMEOUT_STEP)
+
+/* Write values into ADP1653 registers. */
+static int adp1653_update_hw(struct adp1653_flash *flash)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&flash->subdev);
+	u8 out_sel;
+	u8 config = 0;
+	int rval;
+
+	out_sel = ADP1653_INDICATOR_INTENSITY_uA_TO_REG(
+		flash->indicator_intensity->val)
+		<< ADP1653_REG_OUT_SEL_ILED_SHIFT;
+
+	switch (flash->led_mode->val) {
+	case V4L2_FLASH_LED_MODE_NONE:
+		break;
+	case V4L2_FLASH_LED_MODE_FLASH:
+		/* Flash mode, light on with strobe, duration from timer */
+		config = ADP1653_REG_CONFIG_TMR_CFG;
+		config |= TIMEOUT_US_TO_CODE(flash->flash_timeout->val)
+			  << ADP1653_REG_CONFIG_TMR_SET_SHIFT;
+		break;
+	case V4L2_FLASH_LED_MODE_TORCH:
+		/* Torch mode, light immediately on, duration indefinite */
+		out_sel |= ADP1653_FLASH_INTENSITY_mA_TO_REG(
+			flash->torch_intensity->val)
+			<< ADP1653_REG_OUT_SEL_HPLED_SHIFT;
+		break;
+	}
+
+	rval = i2c_smbus_write_byte_data(client, ADP1653_REG_OUT_SEL, out_sel);
+	if (rval < 0)
+		return rval;
+
+	rval = i2c_smbus_write_byte_data(client, ADP1653_REG_CONFIG, config);
+	if (rval < 0)
+		return rval;
+
+	return 0;
+}
+
+static int adp1653_get_fault(struct adp1653_flash *flash)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&flash->subdev);
+	int fault;
+	int rval;
+
+	fault = i2c_smbus_read_byte_data(client, ADP1653_REG_FAULT);
+	if (fault < 0)
+		return fault;
+
+	flash->fault |= fault;
+
+	if (!flash->fault)
+		return 0;
+
+	/* Clear faults. */
+	rval = i2c_smbus_write_byte_data(client, ADP1653_REG_OUT_SEL, 0);
+	if (rval < 0)
+		return rval;
+
+	flash->led_mode->val = V4L2_FLASH_LED_MODE_NONE;
+
+	rval = adp1653_update_hw(flash);
+	if (rval)
+		return rval;
+
+	return flash->fault;
+}
+
+static int adp1653_strobe(struct adp1653_flash *flash, int enable)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&flash->subdev);
+	u8 out_sel = ADP1653_INDICATOR_INTENSITY_uA_TO_REG(
+		flash->indicator_intensity->val)
+		<< ADP1653_REG_OUT_SEL_ILED_SHIFT;
+	int rval;
+
+	if (flash->led_mode->val != V4L2_FLASH_LED_MODE_FLASH)
+		return -EBUSY;
+
+	if (!enable)
+		return i2c_smbus_write_byte_data(client, ADP1653_REG_OUT_SEL,
+						 out_sel);
+
+	out_sel |= ADP1653_FLASH_INTENSITY_mA_TO_REG(
+		flash->flash_intensity->val)
+		<< ADP1653_REG_OUT_SEL_HPLED_SHIFT;
+	rval = i2c_smbus_write_byte_data(client, ADP1653_REG_OUT_SEL, out_sel);
+	if (rval)
+		return rval;
+
+	/* Software strobe using i2c */
+	rval = i2c_smbus_write_byte_data(client, ADP1653_REG_SW_STROBE,
+		ADP1653_REG_SW_STROBE_SW_STROBE);
+	if (rval)
+		return rval;
+	return i2c_smbus_write_byte_data(client, ADP1653_REG_SW_STROBE, 0);
+}
+
+/* --------------------------------------------------------------------------
+ * V4L2 controls
+ */
+
+static int adp1653_get_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct adp1653_flash *flash =
+		container_of(ctrl->handler, struct adp1653_flash, ctrls);
+	int rval;
+
+	rval = adp1653_get_fault(flash);
+	if (rval)
+		return rval;
+
+	ctrl->cur.val = 0;
+
+	if (flash->fault & ADP1653_REG_FAULT_FLT_SCP)
+		ctrl->cur.val |= V4L2_FLASH_FAULT_SHORT_CIRCUIT;
+	if (flash->fault & ADP1653_REG_FAULT_FLT_OT)
+		ctrl->cur.val |= V4L2_FLASH_FAULT_OVER_TEMPERATURE;
+	if (flash->fault & ADP1653_REG_FAULT_FLT_TMR)
+		ctrl->cur.val |= V4L2_FLASH_FAULT_TIMEOUT;
+	if (flash->fault & ADP1653_REG_FAULT_FLT_OV)
+		ctrl->cur.val |= V4L2_FLASH_FAULT_OVER_VOLTAGE;
+
+	flash->fault = 0;
+
+	return 0;
+}
+
+static int adp1653_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct adp1653_flash *flash =
+		container_of(ctrl->handler, struct adp1653_flash, ctrls);
+	int rval;
+
+	rval = adp1653_get_fault(flash);
+	if (rval)
+		return rval;
+	if ((rval & (ADP1653_REG_FAULT_FLT_SCP |
+		     ADP1653_REG_FAULT_FLT_OT |
+		     ADP1653_REG_FAULT_FLT_OV)) &&
+	    (ctrl->id == V4L2_CID_FLASH_STROBE ||
+	     ctrl->id == V4L2_CID_FLASH_TORCH_INTENSITY ||
+	     ctrl->id == V4L2_CID_FLASH_LED_MODE))
+		return -EBUSY;
+
+	switch (ctrl->id) {
+	case V4L2_CID_FLASH_STROBE:
+		return adp1653_strobe(flash, 1);
+	case V4L2_CID_FLASH_STROBE_STOP:
+		return adp1653_strobe(flash, 0);
+	}
+
+	return adp1653_update_hw(flash);
+}
+
+static const struct v4l2_ctrl_ops adp1653_ctrl_ops = {
+	.g_volatile_ctrl = adp1653_get_ctrl,
+	.s_ctrl = adp1653_set_ctrl,
+};
+
+static int adp1653_init_controls(struct adp1653_flash *flash)
+{
+	struct v4l2_ctrl *fault;
+
+	v4l2_ctrl_handler_init(&flash->ctrls, 9);
+
+	flash->led_mode =
+		v4l2_ctrl_new_std_menu(&flash->ctrls, &adp1653_ctrl_ops,
+				       V4L2_CID_FLASH_LED_MODE,
+				       V4L2_FLASH_LED_MODE_TORCH, ~0x7, 0);
+	v4l2_ctrl_new_std_menu(&flash->ctrls, &adp1653_ctrl_ops,
+			       V4L2_CID_FLASH_STROBE_SOURCE,
+			       V4L2_FLASH_STROBE_SOURCE_SOFTWARE, ~0x1, 0);
+	v4l2_ctrl_new_std(&flash->ctrls, &adp1653_ctrl_ops,
+			  V4L2_CID_FLASH_STROBE, 0, 0, 0, 0);
+	v4l2_ctrl_new_std(&flash->ctrls, &adp1653_ctrl_ops,
+			  V4L2_CID_FLASH_STROBE_STOP, 0, 0, 0, 0);
+	flash->flash_timeout =
+		v4l2_ctrl_new_std(&flash->ctrls, &adp1653_ctrl_ops,
+				  V4L2_CID_FLASH_TIMEOUT, TIMEOUT_MIN,
+				  flash->platform_data->max_flash_timeout,
+				  TIMEOUT_STEP,
+				  flash->platform_data->max_flash_timeout);
+	flash->flash_intensity =
+		v4l2_ctrl_new_std(&flash->ctrls, &adp1653_ctrl_ops,
+				  V4L2_CID_FLASH_INTENSITY,
+				  ADP1653_FLASH_INTENSITY_MIN,
+				  flash->platform_data->max_flash_intensity,
+				  1, flash->platform_data->max_flash_intensity);
+	flash->torch_intensity =
+		v4l2_ctrl_new_std(&flash->ctrls, &adp1653_ctrl_ops,
+				  V4L2_CID_FLASH_TORCH_INTENSITY,
+				  ADP1653_TORCH_INTENSITY_MIN,
+				  flash->platform_data->max_torch_intensity,
+				  ADP1653_FLASH_INTENSITY_STEP,
+				  flash->platform_data->max_torch_intensity);
+	flash->indicator_intensity =
+		v4l2_ctrl_new_std(&flash->ctrls, &adp1653_ctrl_ops,
+				  V4L2_CID_FLASH_INDICATOR_INTENSITY,
+				  ADP1653_INDICATOR_INTENSITY_MIN,
+				  flash->platform_data->max_indicator_intensity,
+				  ADP1653_INDICATOR_INTENSITY_STEP,
+				  ADP1653_INDICATOR_INTENSITY_MIN);
+	fault = v4l2_ctrl_new_std(&flash->ctrls, &adp1653_ctrl_ops,
+				  V4L2_CID_FLASH_FAULT, 0,
+				  V4L2_FLASH_FAULT_OVER_VOLTAGE
+				  | V4L2_FLASH_FAULT_OVER_TEMPERATURE
+				  | V4L2_FLASH_FAULT_SHORT_CIRCUIT, 0, 0);
+
+	if (flash->ctrls.error)
+		return flash->ctrls.error;
+
+	fault->flags |= V4L2_CTRL_FLAG_VOLATILE;
+
+	flash->subdev.ctrl_handler = &flash->ctrls;
+	return 0;
+}
+
+/* --------------------------------------------------------------------------
+ * V4L2 subdev operations
+ */
+
+static int
+adp1653_init_device(struct adp1653_flash *flash)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&flash->subdev);
+	int rval;
+
+	/* Clear FAULT register by writing zero to OUT_SEL */
+	rval = i2c_smbus_write_byte_data(client, ADP1653_REG_OUT_SEL, 0);
+	if (rval < 0) {
+		dev_err(&client->dev, "failed writing fault register\n");
+		return -EIO;
+	}
+
+	mutex_lock(flash->ctrls.lock);
+	/* Reset faults before reading new ones. */
+	flash->fault = 0;
+	rval = adp1653_get_fault(flash);
+	mutex_unlock(flash->ctrls.lock);
+	if (rval > 0) {
+		dev_err(&client->dev, "faults detected: 0x%1.1x\n", rval);
+		return -EIO;
+	}
+
+	mutex_lock(flash->ctrls.lock);
+	rval = adp1653_update_hw(flash);
+	mutex_unlock(flash->ctrls.lock);
+	if (rval) {
+		dev_err(&client->dev,
+			"adp1653_update_hw failed at %s\n", __func__);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int
+__adp1653_set_power(struct adp1653_flash *flash, int on)
+{
+	int ret;
+
+	if (flash->platform_data->power) {
+		ret = flash->platform_data->power(&flash->subdev, on);
+		if (ret < 0)
+			return ret;
+	} else {
+		gpiod_set_value(flash->platform_data->enable_gpio, on);
+		if (on)
+			/* Some delay is apparently required. */
+			udelay(20);
+	}
+
+	if (!on)
+		return 0;
+
+	ret = adp1653_init_device(flash);
+	if (ret >= 0)
+		return ret;
+
+	if (flash->platform_data->power)
+		flash->platform_data->power(&flash->subdev, 0);
+	else
+		gpiod_set_value(flash->platform_data->enable_gpio, 0);
+
+	return ret;
+}
+
+static int
+adp1653_set_power(struct v4l2_subdev *subdev, int on)
+{
+	struct adp1653_flash *flash = to_adp1653_flash(subdev);
+	int ret = 0;
+
+	mutex_lock(&flash->power_lock);
+
+	/* If the power count is modified from 0 to != 0 or from != 0 to 0,
+	 * update the power state.
+	 */
+	if (flash->power_count == !on) {
+		ret = __adp1653_set_power(flash, !!on);
+		if (ret < 0)
+			goto done;
+	}
+
+	/* Update the power count. */
+	flash->power_count += on ? 1 : -1;
+	WARN_ON(flash->power_count < 0);
+
+done:
+	mutex_unlock(&flash->power_lock);
+	return ret;
+}
+
+static int adp1653_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	return adp1653_set_power(sd, 1);
+}
+
+static int adp1653_close(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	return adp1653_set_power(sd, 0);
+}
+
+static const struct v4l2_subdev_core_ops adp1653_core_ops = {
+	.s_power = adp1653_set_power,
+};
+
+static const struct v4l2_subdev_ops adp1653_ops = {
+	.core = &adp1653_core_ops,
+};
+
+static const struct v4l2_subdev_internal_ops adp1653_internal_ops = {
+	.open = adp1653_open,
+	.close = adp1653_close,
+};
+
+/* --------------------------------------------------------------------------
+ * I2C driver
+ */
+#ifdef CONFIG_PM
+
+static int adp1653_suspend(struct device *dev)
+{
+	struct v4l2_subdev *subdev = dev_get_drvdata(dev);
+	struct adp1653_flash *flash = to_adp1653_flash(subdev);
+
+	if (!flash->power_count)
+		return 0;
+
+	return __adp1653_set_power(flash, 0);
+}
+
+static int adp1653_resume(struct device *dev)
+{
+	struct v4l2_subdev *subdev = dev_get_drvdata(dev);
+	struct adp1653_flash *flash = to_adp1653_flash(subdev);
+
+	if (!flash->power_count)
+		return 0;
+
+	return __adp1653_set_power(flash, 1);
+}
+
+#else
+
+#define adp1653_suspend	NULL
+#define adp1653_resume	NULL
+
+#endif /* CONFIG_PM */
+
+static int adp1653_of_init(struct i2c_client *client,
+			   struct adp1653_flash *flash,
+			   struct device_node *node)
+{
+	struct adp1653_platform_data *pd;
+	struct device_node *child;
+
+	pd = devm_kzalloc(&client->dev, sizeof(*pd), GFP_KERNEL);
+	if (!pd)
+		return -ENOMEM;
+	flash->platform_data = pd;
+
+	child = of_get_child_by_name(node, "flash");
+	if (!child)
+		return -EINVAL;
+
+	if (of_property_read_u32(child, "flash-timeout-us",
+				 &pd->max_flash_timeout))
+		goto err;
+
+	if (of_property_read_u32(child, "flash-max-microamp",
+				 &pd->max_flash_intensity))
+		goto err;
+
+	pd->max_flash_intensity /= 1000;
+
+	if (of_property_read_u32(child, "led-max-microamp",
+				 &pd->max_torch_intensity))
+		goto err;
+
+	pd->max_torch_intensity /= 1000;
+	of_node_put(child);
+
+	child = of_get_child_by_name(node, "indicator");
+	if (!child)
+		return -EINVAL;
+
+	if (of_property_read_u32(child, "led-max-microamp",
+				 &pd->max_indicator_intensity))
+		goto err;
+
+	of_node_put(child);
+
+	pd->enable_gpio = devm_gpiod_get(&client->dev, "enable", GPIOD_OUT_LOW);
+	if (IS_ERR(pd->enable_gpio)) {
+		dev_err(&client->dev, "Error getting GPIO\n");
+		return PTR_ERR(pd->enable_gpio);
+	}
+
+	return 0;
+err:
+	dev_err(&client->dev, "Required property not found\n");
+	of_node_put(child);
+	return -EINVAL;
+}
+
+
+static int adp1653_probe(struct i2c_client *client)
+{
+	struct adp1653_flash *flash;
+	int ret;
+
+	flash = devm_kzalloc(&client->dev, sizeof(*flash), GFP_KERNEL);
+	if (flash == NULL)
+		return -ENOMEM;
+
+	if (client->dev.of_node) {
+		ret = adp1653_of_init(client, flash, client->dev.of_node);
+		if (ret)
+			return ret;
+	} else {
+		if (!client->dev.platform_data) {
+			dev_err(&client->dev,
+				"Neither DT not platform data provided\n");
+			return -EINVAL;
+		}
+		flash->platform_data = client->dev.platform_data;
+	}
+
+	mutex_init(&flash->power_lock);
+
+	v4l2_i2c_subdev_init(&flash->subdev, client, &adp1653_ops);
+	flash->subdev.internal_ops = &adp1653_internal_ops;
+	flash->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	ret = adp1653_init_controls(flash);
+	if (ret)
+		goto free_and_quit;
+
+	ret = media_entity_pads_init(&flash->subdev.entity, 0, NULL);
+	if (ret < 0)
+		goto free_and_quit;
+
+	flash->subdev.entity.function = MEDIA_ENT_F_FLASH;
+
+	return 0;
+
+free_and_quit:
+	dev_err(&client->dev, "adp1653: failed to register device\n");
+	v4l2_ctrl_handler_free(&flash->ctrls);
+	return ret;
+}
+
+static void adp1653_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct adp1653_flash *flash = to_adp1653_flash(subdev);
+
+	v4l2_device_unregister_subdev(&flash->subdev);
+	v4l2_ctrl_handler_free(&flash->ctrls);
+	media_entity_cleanup(&flash->subdev.entity);
+}
+
+static const struct i2c_device_id adp1653_id_table[] = {
+	{ ADP1653_NAME, 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, adp1653_id_table);
+
+static const struct dev_pm_ops adp1653_pm_ops = {
+	.suspend	= adp1653_suspend,
+	.resume		= adp1653_resume,
+};
+
+static struct i2c_driver adp1653_i2c_driver = {
+	.driver		= {
+		.name	= ADP1653_NAME,
+		.pm	= &adp1653_pm_ops,
+	},
+	.probe		= adp1653_probe,
+	.remove		= adp1653_remove,
+	.id_table	= adp1653_id_table,
+};
+
+module_i2c_driver(adp1653_i2c_driver);
+
+MODULE_AUTHOR("Sakari Ailus <sakari.ailus@nokia.com>");
+MODULE_DESCRIPTION("Analog Devices ADP1653 LED flash driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/adv7170.c b/drivers/media/i2c/adv7170.c
new file mode 100644
index 000000000..4a2b9fd9e
--- /dev/null
+++ b/drivers/media/i2c/adv7170.c
@@ -0,0 +1,395 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * adv7170 - adv7170, adv7171 video encoder driver version 0.0.1
+ *
+ * Copyright (C) 2002 Maxim Yevtyushkin <max@linuxmedialabs.com>
+ *
+ * Based on adv7176 driver by:
+ *
+ * Copyright (C) 1998 Dave Perks <dperks@ibm.net>
+ * Copyright (C) 1999 Wolfgang Scherr <scherr@net4you.net>
+ * Copyright (C) 2000 Serguei Miridonov <mirsev@cicese.mx>
+ *    - some corrections for Pinnacle Systems Inc. DC10plus card.
+ *
+ * Changes by Ronald Bultje <rbultje@ronald.bitfreak.net>
+ *    - moved over to linux>=2.4.x i2c protocol (1/1/2003)
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <linux/ioctl.h>
+#include <linux/uaccess.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+
+MODULE_DESCRIPTION("Analog Devices ADV7170 video encoder driver");
+MODULE_AUTHOR("Maxim Yevtyushkin");
+MODULE_LICENSE("GPL");
+
+
+static int debug;
+module_param(debug, int, 0);
+MODULE_PARM_DESC(debug, "Debug level (0-1)");
+
+/* ----------------------------------------------------------------------- */
+
+struct adv7170 {
+	struct v4l2_subdev sd;
+	unsigned char reg[128];
+
+	v4l2_std_id norm;
+	int input;
+};
+
+static inline struct adv7170 *to_adv7170(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct adv7170, sd);
+}
+
+static char *inputs[] = { "pass_through", "play_back" };
+
+static u32 adv7170_codes[] = {
+	MEDIA_BUS_FMT_UYVY8_2X8,
+	MEDIA_BUS_FMT_UYVY8_1X16,
+};
+
+/* ----------------------------------------------------------------------- */
+
+static inline int adv7170_write(struct v4l2_subdev *sd, u8 reg, u8 value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct adv7170 *encoder = to_adv7170(sd);
+
+	encoder->reg[reg] = value;
+	return i2c_smbus_write_byte_data(client, reg, value);
+}
+
+static inline int adv7170_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return i2c_smbus_read_byte_data(client, reg);
+}
+
+static int adv7170_write_block(struct v4l2_subdev *sd,
+		     const u8 *data, unsigned int len)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct adv7170 *encoder = to_adv7170(sd);
+	int ret = -1;
+	u8 reg;
+
+	/* the adv7170 has an autoincrement function, use it if
+	 * the adapter understands raw I2C */
+	if (i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		/* do raw I2C, not smbus compatible */
+		u8 block_data[32];
+		int block_len;
+
+		while (len >= 2) {
+			block_len = 0;
+			block_data[block_len++] = reg = data[0];
+			do {
+				block_data[block_len++] =
+				    encoder->reg[reg++] = data[1];
+				len -= 2;
+				data += 2;
+			} while (len >= 2 && data[0] == reg && block_len < 32);
+			ret = i2c_master_send(client, block_data, block_len);
+			if (ret < 0)
+				break;
+		}
+	} else {
+		/* do some slow I2C emulation kind of thing */
+		while (len >= 2) {
+			reg = *data++;
+			ret = adv7170_write(sd, reg, *data++);
+			if (ret < 0)
+				break;
+			len -= 2;
+		}
+	}
+	return ret;
+}
+
+/* ----------------------------------------------------------------------- */
+
+#define TR0MODE     0x4c
+#define TR0RST	    0x80
+
+#define TR1CAPT	    0x00
+#define TR1PLAY	    0x00
+
+static const unsigned char init_NTSC[] = {
+	0x00, 0x10,		/* MR0 */
+	0x01, 0x20,		/* MR1 */
+	0x02, 0x0e,		/* MR2 RTC control: bits 2 and 1 */
+	0x03, 0x80,		/* MR3 */
+	0x04, 0x30,		/* MR4 */
+	0x05, 0x00,		/* Reserved */
+	0x06, 0x00,		/* Reserved */
+	0x07, TR0MODE,		/* TM0 */
+	0x08, TR1CAPT,		/* TM1 */
+	0x09, 0x16,		/* Fsc0 */
+	0x0a, 0x7c,		/* Fsc1 */
+	0x0b, 0xf0,		/* Fsc2 */
+	0x0c, 0x21,		/* Fsc3 */
+	0x0d, 0x00,		/* Subcarrier Phase */
+	0x0e, 0x00,		/* Closed Capt. Ext 0 */
+	0x0f, 0x00,		/* Closed Capt. Ext 1 */
+	0x10, 0x00,		/* Closed Capt. 0 */
+	0x11, 0x00,		/* Closed Capt. 1 */
+	0x12, 0x00,		/* Pedestal Ctl 0 */
+	0x13, 0x00,		/* Pedestal Ctl 1 */
+	0x14, 0x00,		/* Pedestal Ctl 2 */
+	0x15, 0x00,		/* Pedestal Ctl 3 */
+	0x16, 0x00,		/* CGMS_WSS_0 */
+	0x17, 0x00,		/* CGMS_WSS_1 */
+	0x18, 0x00,		/* CGMS_WSS_2 */
+	0x19, 0x00,		/* Teletext Ctl */
+};
+
+static const unsigned char init_PAL[] = {
+	0x00, 0x71,		/* MR0 */
+	0x01, 0x20,		/* MR1 */
+	0x02, 0x0e,		/* MR2 RTC control: bits 2 and 1 */
+	0x03, 0x80,		/* MR3 */
+	0x04, 0x30,		/* MR4 */
+	0x05, 0x00,		/* Reserved */
+	0x06, 0x00,		/* Reserved */
+	0x07, TR0MODE,		/* TM0 */
+	0x08, TR1CAPT,		/* TM1 */
+	0x09, 0xcb,		/* Fsc0 */
+	0x0a, 0x8a,		/* Fsc1 */
+	0x0b, 0x09,		/* Fsc2 */
+	0x0c, 0x2a,		/* Fsc3 */
+	0x0d, 0x00,		/* Subcarrier Phase */
+	0x0e, 0x00,		/* Closed Capt. Ext 0 */
+	0x0f, 0x00,		/* Closed Capt. Ext 1 */
+	0x10, 0x00,		/* Closed Capt. 0 */
+	0x11, 0x00,		/* Closed Capt. 1 */
+	0x12, 0x00,		/* Pedestal Ctl 0 */
+	0x13, 0x00,		/* Pedestal Ctl 1 */
+	0x14, 0x00,		/* Pedestal Ctl 2 */
+	0x15, 0x00,		/* Pedestal Ctl 3 */
+	0x16, 0x00,		/* CGMS_WSS_0 */
+	0x17, 0x00,		/* CGMS_WSS_1 */
+	0x18, 0x00,		/* CGMS_WSS_2 */
+	0x19, 0x00,		/* Teletext Ctl */
+};
+
+
+static int adv7170_s_std_output(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct adv7170 *encoder = to_adv7170(sd);
+
+	v4l2_dbg(1, debug, sd, "set norm %llx\n", (unsigned long long)std);
+
+	if (std & V4L2_STD_NTSC) {
+		adv7170_write_block(sd, init_NTSC, sizeof(init_NTSC));
+		if (encoder->input == 0)
+			adv7170_write(sd, 0x02, 0x0e);	/* Enable genlock */
+		adv7170_write(sd, 0x07, TR0MODE | TR0RST);
+		adv7170_write(sd, 0x07, TR0MODE);
+	} else if (std & V4L2_STD_PAL) {
+		adv7170_write_block(sd, init_PAL, sizeof(init_PAL));
+		if (encoder->input == 0)
+			adv7170_write(sd, 0x02, 0x0e);	/* Enable genlock */
+		adv7170_write(sd, 0x07, TR0MODE | TR0RST);
+		adv7170_write(sd, 0x07, TR0MODE);
+	} else {
+		v4l2_dbg(1, debug, sd, "illegal norm: %llx\n",
+				(unsigned long long)std);
+		return -EINVAL;
+	}
+	v4l2_dbg(1, debug, sd, "switched to %llx\n", (unsigned long long)std);
+	encoder->norm = std;
+	return 0;
+}
+
+static int adv7170_s_routing(struct v4l2_subdev *sd,
+			     u32 input, u32 output, u32 config)
+{
+	struct adv7170 *encoder = to_adv7170(sd);
+
+	/* RJ: input = 0: input is from decoder
+	   input = 1: input is from ZR36060
+	   input = 2: color bar */
+
+	v4l2_dbg(1, debug, sd, "set input from %s\n",
+			input == 0 ? "decoder" : "ZR36060");
+
+	switch (input) {
+	case 0:
+		adv7170_write(sd, 0x01, 0x20);
+		adv7170_write(sd, 0x08, TR1CAPT);	/* TR1 */
+		adv7170_write(sd, 0x02, 0x0e);	/* Enable genlock */
+		adv7170_write(sd, 0x07, TR0MODE | TR0RST);
+		adv7170_write(sd, 0x07, TR0MODE);
+		/* udelay(10); */
+		break;
+
+	case 1:
+		adv7170_write(sd, 0x01, 0x00);
+		adv7170_write(sd, 0x08, TR1PLAY);	/* TR1 */
+		adv7170_write(sd, 0x02, 0x08);
+		adv7170_write(sd, 0x07, TR0MODE | TR0RST);
+		adv7170_write(sd, 0x07, TR0MODE);
+		/* udelay(10); */
+		break;
+
+	default:
+		v4l2_dbg(1, debug, sd, "illegal input: %d\n", input);
+		return -EINVAL;
+	}
+	v4l2_dbg(1, debug, sd, "switched to %s\n", inputs[input]);
+	encoder->input = input;
+	return 0;
+}
+
+static int adv7170_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->pad || code->index >= ARRAY_SIZE(adv7170_codes))
+		return -EINVAL;
+
+	code->code = adv7170_codes[code->index];
+	return 0;
+}
+
+static int adv7170_get_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *mf = &format->format;
+	u8 val = adv7170_read(sd, 0x7);
+
+	if (format->pad)
+		return -EINVAL;
+
+	if ((val & 0x40) == (1 << 6))
+		mf->code = MEDIA_BUS_FMT_UYVY8_1X16;
+	else
+		mf->code = MEDIA_BUS_FMT_UYVY8_2X8;
+
+	mf->colorspace  = V4L2_COLORSPACE_SMPTE170M;
+	mf->width       = 0;
+	mf->height      = 0;
+	mf->field       = V4L2_FIELD_ANY;
+
+	return 0;
+}
+
+static int adv7170_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *mf = &format->format;
+	u8 val = adv7170_read(sd, 0x7);
+
+	if (format->pad)
+		return -EINVAL;
+
+	switch (mf->code) {
+	case MEDIA_BUS_FMT_UYVY8_2X8:
+		val &= ~0x40;
+		break;
+
+	case MEDIA_BUS_FMT_UYVY8_1X16:
+		val |= 0x40;
+		break;
+
+	default:
+		v4l2_dbg(1, debug, sd,
+			"illegal v4l2_mbus_framefmt code: %d\n", mf->code);
+		return -EINVAL;
+	}
+
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		return adv7170_write(sd, 0x7, val);
+
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_video_ops adv7170_video_ops = {
+	.s_std_output = adv7170_s_std_output,
+	.s_routing = adv7170_s_routing,
+};
+
+static const struct v4l2_subdev_pad_ops adv7170_pad_ops = {
+	.enum_mbus_code = adv7170_enum_mbus_code,
+	.get_fmt = adv7170_get_fmt,
+	.set_fmt = adv7170_set_fmt,
+};
+
+static const struct v4l2_subdev_ops adv7170_ops = {
+	.video = &adv7170_video_ops,
+	.pad = &adv7170_pad_ops,
+};
+
+/* ----------------------------------------------------------------------- */
+
+static int adv7170_probe(struct i2c_client *client)
+{
+	struct adv7170 *encoder;
+	struct v4l2_subdev *sd;
+	int i;
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -ENODEV;
+
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	encoder = devm_kzalloc(&client->dev, sizeof(*encoder), GFP_KERNEL);
+	if (encoder == NULL)
+		return -ENOMEM;
+	sd = &encoder->sd;
+	v4l2_i2c_subdev_init(sd, client, &adv7170_ops);
+	encoder->norm = V4L2_STD_NTSC;
+	encoder->input = 0;
+
+	i = adv7170_write_block(sd, init_NTSC, sizeof(init_NTSC));
+	if (i >= 0) {
+		i = adv7170_write(sd, 0x07, TR0MODE | TR0RST);
+		i = adv7170_write(sd, 0x07, TR0MODE);
+		i = adv7170_read(sd, 0x12);
+		v4l2_dbg(1, debug, sd, "revision %d\n", i & 1);
+	}
+	if (i < 0)
+		v4l2_dbg(1, debug, sd, "init error 0x%x\n", i);
+	return 0;
+}
+
+static void adv7170_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id adv7170_id[] = {
+	{ "adv7170", 0 },
+	{ "adv7171", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, adv7170_id);
+
+static struct i2c_driver adv7170_driver = {
+	.driver = {
+		.name	= "adv7170",
+	},
+	.probe		= adv7170_probe,
+	.remove		= adv7170_remove,
+	.id_table	= adv7170_id,
+};
+
+module_i2c_driver(adv7170_driver);
diff --git a/drivers/media/i2c/adv7175.c b/drivers/media/i2c/adv7175.c
new file mode 100644
index 000000000..e454cba4b
--- /dev/null
+++ b/drivers/media/i2c/adv7175.c
@@ -0,0 +1,450 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ *  adv7175 - adv7175a video encoder driver version 0.0.3
+ *
+ * Copyright (C) 1998 Dave Perks <dperks@ibm.net>
+ * Copyright (C) 1999 Wolfgang Scherr <scherr@net4you.net>
+ * Copyright (C) 2000 Serguei Miridonov <mirsev@cicese.mx>
+ *    - some corrections for Pinnacle Systems Inc. DC10plus card.
+ *
+ * Changes by Ronald Bultje <rbultje@ronald.bitfreak.net>
+ *    - moved over to linux>=2.4.x i2c protocol (9/9/2002)
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <linux/ioctl.h>
+#include <linux/uaccess.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+
+MODULE_DESCRIPTION("Analog Devices ADV7175 video encoder driver");
+MODULE_AUTHOR("Dave Perks");
+MODULE_LICENSE("GPL");
+
+#define   I2C_ADV7175        0xd4
+#define   I2C_ADV7176        0x54
+
+
+static int debug;
+module_param(debug, int, 0);
+MODULE_PARM_DESC(debug, "Debug level (0-1)");
+
+/* ----------------------------------------------------------------------- */
+
+struct adv7175 {
+	struct v4l2_subdev sd;
+	v4l2_std_id norm;
+	int input;
+};
+
+static inline struct adv7175 *to_adv7175(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct adv7175, sd);
+}
+
+static char *inputs[] = { "pass_through", "play_back", "color_bar" };
+
+static u32 adv7175_codes[] = {
+	MEDIA_BUS_FMT_UYVY8_2X8,
+	MEDIA_BUS_FMT_UYVY8_1X16,
+};
+
+/* ----------------------------------------------------------------------- */
+
+static inline int adv7175_write(struct v4l2_subdev *sd, u8 reg, u8 value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return i2c_smbus_write_byte_data(client, reg, value);
+}
+
+static inline int adv7175_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return i2c_smbus_read_byte_data(client, reg);
+}
+
+static int adv7175_write_block(struct v4l2_subdev *sd,
+		     const u8 *data, unsigned int len)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = -1;
+	u8 reg;
+
+	/* the adv7175 has an autoincrement function, use it if
+	 * the adapter understands raw I2C */
+	if (i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		/* do raw I2C, not smbus compatible */
+		u8 block_data[32];
+		int block_len;
+
+		while (len >= 2) {
+			block_len = 0;
+			block_data[block_len++] = reg = data[0];
+			do {
+				block_data[block_len++] = data[1];
+				reg++;
+				len -= 2;
+				data += 2;
+			} while (len >= 2 && data[0] == reg && block_len < 32);
+			ret = i2c_master_send(client, block_data, block_len);
+			if (ret < 0)
+				break;
+		}
+	} else {
+		/* do some slow I2C emulation kind of thing */
+		while (len >= 2) {
+			reg = *data++;
+			ret = adv7175_write(sd, reg, *data++);
+			if (ret < 0)
+				break;
+			len -= 2;
+		}
+	}
+
+	return ret;
+}
+
+static void set_subcarrier_freq(struct v4l2_subdev *sd, int pass_through)
+{
+	/* for some reason pass_through NTSC needs
+	 * a different sub-carrier freq to remain stable. */
+	if (pass_through)
+		adv7175_write(sd, 0x02, 0x00);
+	else
+		adv7175_write(sd, 0x02, 0x55);
+
+	adv7175_write(sd, 0x03, 0x55);
+	adv7175_write(sd, 0x04, 0x55);
+	adv7175_write(sd, 0x05, 0x25);
+}
+
+/* ----------------------------------------------------------------------- */
+/* Output filter:  S-Video  Composite */
+
+#define MR050       0x11	/* 0x09 */
+#define MR060       0x14	/* 0x0c */
+
+/* ----------------------------------------------------------------------- */
+
+#define TR0MODE     0x46
+#define TR0RST	    0x80
+
+#define TR1CAPT	    0x80
+#define TR1PLAY	    0x00
+
+static const unsigned char init_common[] = {
+
+	0x00, MR050,		/* MR0, PAL enabled */
+	0x01, 0x00,		/* MR1 */
+	0x02, 0x0c,		/* subc. freq. */
+	0x03, 0x8c,		/* subc. freq. */
+	0x04, 0x79,		/* subc. freq. */
+	0x05, 0x26,		/* subc. freq. */
+	0x06, 0x40,		/* subc. phase */
+
+	0x07, TR0MODE,		/* TR0, 16bit */
+	0x08, 0x21,		/*  */
+	0x09, 0x00,		/*  */
+	0x0a, 0x00,		/*  */
+	0x0b, 0x00,		/*  */
+	0x0c, TR1CAPT,		/* TR1 */
+	0x0d, 0x4f,		/* MR2 */
+	0x0e, 0x00,		/*  */
+	0x0f, 0x00,		/*  */
+	0x10, 0x00,		/*  */
+	0x11, 0x00,		/*  */
+};
+
+static const unsigned char init_pal[] = {
+	0x00, MR050,		/* MR0, PAL enabled */
+	0x01, 0x00,		/* MR1 */
+	0x02, 0x0c,		/* subc. freq. */
+	0x03, 0x8c,		/* subc. freq. */
+	0x04, 0x79,		/* subc. freq. */
+	0x05, 0x26,		/* subc. freq. */
+	0x06, 0x40,		/* subc. phase */
+};
+
+static const unsigned char init_ntsc[] = {
+	0x00, MR060,		/* MR0, NTSC enabled */
+	0x01, 0x00,		/* MR1 */
+	0x02, 0x55,		/* subc. freq. */
+	0x03, 0x55,		/* subc. freq. */
+	0x04, 0x55,		/* subc. freq. */
+	0x05, 0x25,		/* subc. freq. */
+	0x06, 0x1a,		/* subc. phase */
+};
+
+static int adv7175_init(struct v4l2_subdev *sd, u32 val)
+{
+	/* This is just for testing!!! */
+	adv7175_write_block(sd, init_common, sizeof(init_common));
+	adv7175_write(sd, 0x07, TR0MODE | TR0RST);
+	adv7175_write(sd, 0x07, TR0MODE);
+	return 0;
+}
+
+static int adv7175_s_std_output(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct adv7175 *encoder = to_adv7175(sd);
+
+	if (std & V4L2_STD_NTSC) {
+		adv7175_write_block(sd, init_ntsc, sizeof(init_ntsc));
+		if (encoder->input == 0)
+			adv7175_write(sd, 0x0d, 0x4f);	/* Enable genlock */
+		adv7175_write(sd, 0x07, TR0MODE | TR0RST);
+		adv7175_write(sd, 0x07, TR0MODE);
+	} else if (std & V4L2_STD_PAL) {
+		adv7175_write_block(sd, init_pal, sizeof(init_pal));
+		if (encoder->input == 0)
+			adv7175_write(sd, 0x0d, 0x4f);	/* Enable genlock */
+		adv7175_write(sd, 0x07, TR0MODE | TR0RST);
+		adv7175_write(sd, 0x07, TR0MODE);
+	} else if (std & V4L2_STD_SECAM) {
+		/* This is an attempt to convert
+		 * SECAM->PAL (typically it does not work
+		 * due to genlock: when decoder is in SECAM
+		 * and encoder in in PAL the subcarrier can
+		 * not be synchronized with horizontal
+		 * quency) */
+		adv7175_write_block(sd, init_pal, sizeof(init_pal));
+		if (encoder->input == 0)
+			adv7175_write(sd, 0x0d, 0x49);	/* Disable genlock */
+		adv7175_write(sd, 0x07, TR0MODE | TR0RST);
+		adv7175_write(sd, 0x07, TR0MODE);
+	} else {
+		v4l2_dbg(1, debug, sd, "illegal norm: %llx\n",
+				(unsigned long long)std);
+		return -EINVAL;
+	}
+	v4l2_dbg(1, debug, sd, "switched to %llx\n", (unsigned long long)std);
+	encoder->norm = std;
+	return 0;
+}
+
+static int adv7175_s_routing(struct v4l2_subdev *sd,
+			     u32 input, u32 output, u32 config)
+{
+	struct adv7175 *encoder = to_adv7175(sd);
+
+	/* RJ: input = 0: input is from decoder
+	   input = 1: input is from ZR36060
+	   input = 2: color bar */
+
+	switch (input) {
+	case 0:
+		adv7175_write(sd, 0x01, 0x00);
+
+		if (encoder->norm & V4L2_STD_NTSC)
+			set_subcarrier_freq(sd, 1);
+
+		adv7175_write(sd, 0x0c, TR1CAPT);	/* TR1 */
+		if (encoder->norm & V4L2_STD_SECAM)
+			adv7175_write(sd, 0x0d, 0x49);	/* Disable genlock */
+		else
+			adv7175_write(sd, 0x0d, 0x4f);	/* Enable genlock */
+		adv7175_write(sd, 0x07, TR0MODE | TR0RST);
+		adv7175_write(sd, 0x07, TR0MODE);
+		/*udelay(10);*/
+		break;
+
+	case 1:
+		adv7175_write(sd, 0x01, 0x00);
+
+		if (encoder->norm & V4L2_STD_NTSC)
+			set_subcarrier_freq(sd, 0);
+
+		adv7175_write(sd, 0x0c, TR1PLAY);	/* TR1 */
+		adv7175_write(sd, 0x0d, 0x49);
+		adv7175_write(sd, 0x07, TR0MODE | TR0RST);
+		adv7175_write(sd, 0x07, TR0MODE);
+		/* udelay(10); */
+		break;
+
+	case 2:
+		adv7175_write(sd, 0x01, 0x80);
+
+		if (encoder->norm & V4L2_STD_NTSC)
+			set_subcarrier_freq(sd, 0);
+
+		adv7175_write(sd, 0x0d, 0x49);
+		adv7175_write(sd, 0x07, TR0MODE | TR0RST);
+		adv7175_write(sd, 0x07, TR0MODE);
+		/* udelay(10); */
+		break;
+
+	default:
+		v4l2_dbg(1, debug, sd, "illegal input: %d\n", input);
+		return -EINVAL;
+	}
+	v4l2_dbg(1, debug, sd, "switched to %s\n", inputs[input]);
+	encoder->input = input;
+	return 0;
+}
+
+static int adv7175_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->pad || code->index >= ARRAY_SIZE(adv7175_codes))
+		return -EINVAL;
+
+	code->code = adv7175_codes[code->index];
+	return 0;
+}
+
+static int adv7175_get_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *mf = &format->format;
+	u8 val = adv7175_read(sd, 0x7);
+
+	if (format->pad)
+		return -EINVAL;
+
+	if ((val & 0x40) == (1 << 6))
+		mf->code = MEDIA_BUS_FMT_UYVY8_1X16;
+	else
+		mf->code = MEDIA_BUS_FMT_UYVY8_2X8;
+
+	mf->colorspace  = V4L2_COLORSPACE_SMPTE170M;
+	mf->width       = 0;
+	mf->height      = 0;
+	mf->field       = V4L2_FIELD_ANY;
+
+	return 0;
+}
+
+static int adv7175_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *mf = &format->format;
+	u8 val = adv7175_read(sd, 0x7);
+	int ret = 0;
+
+	if (format->pad)
+		return -EINVAL;
+
+	switch (mf->code) {
+	case MEDIA_BUS_FMT_UYVY8_2X8:
+		val &= ~0x40;
+		break;
+
+	case MEDIA_BUS_FMT_UYVY8_1X16:
+		val |= 0x40;
+		break;
+
+	default:
+		v4l2_dbg(1, debug, sd,
+			"illegal v4l2_mbus_framefmt code: %d\n", mf->code);
+		return -EINVAL;
+	}
+
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		ret = adv7175_write(sd, 0x7, val);
+
+	return ret;
+}
+
+static int adv7175_s_power(struct v4l2_subdev *sd, int on)
+{
+	if (on)
+		adv7175_write(sd, 0x01, 0x00);
+	else
+		adv7175_write(sd, 0x01, 0x78);
+
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_core_ops adv7175_core_ops = {
+	.init = adv7175_init,
+	.s_power = adv7175_s_power,
+};
+
+static const struct v4l2_subdev_video_ops adv7175_video_ops = {
+	.s_std_output = adv7175_s_std_output,
+	.s_routing = adv7175_s_routing,
+};
+
+static const struct v4l2_subdev_pad_ops adv7175_pad_ops = {
+	.enum_mbus_code = adv7175_enum_mbus_code,
+	.get_fmt = adv7175_get_fmt,
+	.set_fmt = adv7175_set_fmt,
+};
+
+static const struct v4l2_subdev_ops adv7175_ops = {
+	.core = &adv7175_core_ops,
+	.video = &adv7175_video_ops,
+	.pad = &adv7175_pad_ops,
+};
+
+/* ----------------------------------------------------------------------- */
+
+static int adv7175_probe(struct i2c_client *client)
+{
+	int i;
+	struct adv7175 *encoder;
+	struct v4l2_subdev *sd;
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -ENODEV;
+
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	encoder = devm_kzalloc(&client->dev, sizeof(*encoder), GFP_KERNEL);
+	if (encoder == NULL)
+		return -ENOMEM;
+	sd = &encoder->sd;
+	v4l2_i2c_subdev_init(sd, client, &adv7175_ops);
+	encoder->norm = V4L2_STD_NTSC;
+	encoder->input = 0;
+
+	i = adv7175_write_block(sd, init_common, sizeof(init_common));
+	if (i >= 0) {
+		i = adv7175_write(sd, 0x07, TR0MODE | TR0RST);
+		i = adv7175_write(sd, 0x07, TR0MODE);
+		i = adv7175_read(sd, 0x12);
+		v4l2_dbg(1, debug, sd, "revision %d\n", i & 1);
+	}
+	if (i < 0)
+		v4l2_dbg(1, debug, sd, "init error 0x%x\n", i);
+	return 0;
+}
+
+static void adv7175_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id adv7175_id[] = {
+	{ "adv7175", 0 },
+	{ "adv7176", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, adv7175_id);
+
+static struct i2c_driver adv7175_driver = {
+	.driver = {
+		.name	= "adv7175",
+	},
+	.probe		= adv7175_probe,
+	.remove		= adv7175_remove,
+	.id_table	= adv7175_id,
+};
+
+module_i2c_driver(adv7175_driver);
diff --git a/drivers/media/i2c/adv7180.c b/drivers/media/i2c/adv7180.c
new file mode 100644
index 000000000..99ba925e8
--- /dev/null
+++ b/drivers/media/i2c/adv7180.c
@@ -0,0 +1,1622 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * adv7180.c Analog Devices ADV7180 video decoder driver
+ * Copyright (c) 2009 Intel Corporation
+ * Copyright (C) 2013 Cogent Embedded, Inc.
+ * Copyright (C) 2013 Renesas Solutions Corp.
+ */
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+#include <linux/gpio/consumer.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <linux/mutex.h>
+#include <linux/delay.h>
+
+#define ADV7180_STD_AD_PAL_BG_NTSC_J_SECAM		0x0
+#define ADV7180_STD_AD_PAL_BG_NTSC_J_SECAM_PED		0x1
+#define ADV7180_STD_AD_PAL_N_NTSC_J_SECAM		0x2
+#define ADV7180_STD_AD_PAL_N_NTSC_M_SECAM		0x3
+#define ADV7180_STD_NTSC_J				0x4
+#define ADV7180_STD_NTSC_M				0x5
+#define ADV7180_STD_PAL60				0x6
+#define ADV7180_STD_NTSC_443				0x7
+#define ADV7180_STD_PAL_BG				0x8
+#define ADV7180_STD_PAL_N				0x9
+#define ADV7180_STD_PAL_M				0xa
+#define ADV7180_STD_PAL_M_PED				0xb
+#define ADV7180_STD_PAL_COMB_N				0xc
+#define ADV7180_STD_PAL_COMB_N_PED			0xd
+#define ADV7180_STD_PAL_SECAM				0xe
+#define ADV7180_STD_PAL_SECAM_PED			0xf
+
+#define ADV7180_REG_INPUT_CONTROL			0x0000
+#define ADV7180_INPUT_CONTROL_INSEL_MASK		0x0f
+
+#define ADV7182_REG_INPUT_VIDSEL			0x0002
+#define ADV7182_REG_INPUT_RESERVED			BIT(2)
+
+#define ADV7180_REG_OUTPUT_CONTROL			0x0003
+#define ADV7180_REG_EXTENDED_OUTPUT_CONTROL		0x0004
+#define ADV7180_EXTENDED_OUTPUT_CONTROL_NTSCDIS		0xC5
+
+#define ADV7180_REG_AUTODETECT_ENABLE			0x0007
+#define ADV7180_AUTODETECT_DEFAULT			0x7f
+/* Contrast */
+#define ADV7180_REG_CON		0x0008	/*Unsigned */
+#define ADV7180_CON_MIN		0
+#define ADV7180_CON_DEF		128
+#define ADV7180_CON_MAX		255
+/* Brightness*/
+#define ADV7180_REG_BRI		0x000a	/*Signed */
+#define ADV7180_BRI_MIN		-128
+#define ADV7180_BRI_DEF		0
+#define ADV7180_BRI_MAX		127
+/* Hue */
+#define ADV7180_REG_HUE		0x000b	/*Signed, inverted */
+#define ADV7180_HUE_MIN		-127
+#define ADV7180_HUE_DEF		0
+#define ADV7180_HUE_MAX		128
+
+#define ADV7180_REG_DEF_VALUE_Y	0x000c
+#define ADV7180_DEF_VAL_EN		0x1
+#define ADV7180_DEF_VAL_AUTO_EN	0x2
+#define ADV7180_REG_CTRL		0x000e
+#define ADV7180_CTRL_IRQ_SPACE		0x20
+
+#define ADV7180_REG_PWR_MAN		0x0f
+#define ADV7180_PWR_MAN_ON		0x04
+#define ADV7180_PWR_MAN_OFF		0x24
+#define ADV7180_PWR_MAN_RES		0x80
+
+#define ADV7180_REG_STATUS1		0x0010
+#define ADV7180_STATUS1_IN_LOCK		0x01
+#define ADV7180_STATUS1_AUTOD_MASK	0x70
+#define ADV7180_STATUS1_AUTOD_NTSM_M_J	0x00
+#define ADV7180_STATUS1_AUTOD_NTSC_4_43 0x10
+#define ADV7180_STATUS1_AUTOD_PAL_M	0x20
+#define ADV7180_STATUS1_AUTOD_PAL_60	0x30
+#define ADV7180_STATUS1_AUTOD_PAL_B_G	0x40
+#define ADV7180_STATUS1_AUTOD_SECAM	0x50
+#define ADV7180_STATUS1_AUTOD_PAL_COMB	0x60
+#define ADV7180_STATUS1_AUTOD_SECAM_525	0x70
+
+#define ADV7180_REG_IDENT 0x0011
+#define ADV7180_ID_7180 0x18
+
+#define ADV7180_REG_STATUS3		0x0013
+#define ADV7180_REG_ANALOG_CLAMP_CTL	0x0014
+#define ADV7180_REG_SHAP_FILTER_CTL_1	0x0017
+#define ADV7180_REG_CTRL_2		0x001d
+#define ADV7180_REG_VSYNC_FIELD_CTL_1	0x0031
+#define ADV7180_VSYNC_FIELD_CTL_1_NEWAV 0x12
+#define ADV7180_REG_MANUAL_WIN_CTL_1	0x003d
+#define ADV7180_REG_MANUAL_WIN_CTL_2	0x003e
+#define ADV7180_REG_MANUAL_WIN_CTL_3	0x003f
+#define ADV7180_REG_LOCK_CNT		0x0051
+#define ADV7180_REG_CVBS_TRIM		0x0052
+#define ADV7180_REG_CLAMP_ADJ		0x005a
+#define ADV7180_REG_RES_CIR		0x005f
+#define ADV7180_REG_DIFF_MODE		0x0060
+
+#define ADV7180_REG_ICONF1		0x2040
+#define ADV7180_ICONF1_ACTIVE_LOW	0x01
+#define ADV7180_ICONF1_PSYNC_ONLY	0x10
+#define ADV7180_ICONF1_ACTIVE_TO_CLR	0xC0
+/* Saturation */
+#define ADV7180_REG_SD_SAT_CB	0x00e3	/*Unsigned */
+#define ADV7180_REG_SD_SAT_CR	0x00e4	/*Unsigned */
+#define ADV7180_SAT_MIN		0
+#define ADV7180_SAT_DEF		128
+#define ADV7180_SAT_MAX		255
+
+#define ADV7180_IRQ1_LOCK	0x01
+#define ADV7180_IRQ1_UNLOCK	0x02
+#define ADV7180_REG_ISR1	0x2042
+#define ADV7180_REG_ICR1	0x2043
+#define ADV7180_REG_IMR1	0x2044
+#define ADV7180_REG_IMR2	0x2048
+#define ADV7180_IRQ3_AD_CHANGE	0x08
+#define ADV7180_REG_ISR3	0x204A
+#define ADV7180_REG_ICR3	0x204B
+#define ADV7180_REG_IMR3	0x204C
+#define ADV7180_REG_IMR4	0x2050
+
+#define ADV7180_REG_NTSC_V_BIT_END	0x00E6
+#define ADV7180_NTSC_V_BIT_END_MANUAL_NVEND	0x4F
+
+#define ADV7180_REG_VPP_SLAVE_ADDR	0xFD
+#define ADV7180_REG_CSI_SLAVE_ADDR	0xFE
+
+#define ADV7180_REG_ACE_CTRL1		0x4080
+#define ADV7180_REG_ACE_CTRL5		0x4084
+#define ADV7180_REG_FLCONTROL		0x40e0
+#define ADV7180_FLCONTROL_FL_ENABLE 0x1
+
+#define ADV7180_REG_RST_CLAMP	0x809c
+#define ADV7180_REG_AGC_ADJ1	0x80b6
+#define ADV7180_REG_AGC_ADJ2	0x80c0
+
+#define ADV7180_CSI_REG_PWRDN	0x00
+#define ADV7180_CSI_PWRDN	0x80
+
+#define ADV7180_INPUT_CVBS_AIN1 0x00
+#define ADV7180_INPUT_CVBS_AIN2 0x01
+#define ADV7180_INPUT_CVBS_AIN3 0x02
+#define ADV7180_INPUT_CVBS_AIN4 0x03
+#define ADV7180_INPUT_CVBS_AIN5 0x04
+#define ADV7180_INPUT_CVBS_AIN6 0x05
+#define ADV7180_INPUT_SVIDEO_AIN1_AIN2 0x06
+#define ADV7180_INPUT_SVIDEO_AIN3_AIN4 0x07
+#define ADV7180_INPUT_SVIDEO_AIN5_AIN6 0x08
+#define ADV7180_INPUT_YPRPB_AIN1_AIN2_AIN3 0x09
+#define ADV7180_INPUT_YPRPB_AIN4_AIN5_AIN6 0x0a
+
+#define ADV7182_INPUT_CVBS_AIN1 0x00
+#define ADV7182_INPUT_CVBS_AIN2 0x01
+#define ADV7182_INPUT_CVBS_AIN3 0x02
+#define ADV7182_INPUT_CVBS_AIN4 0x03
+#define ADV7182_INPUT_CVBS_AIN5 0x04
+#define ADV7182_INPUT_CVBS_AIN6 0x05
+#define ADV7182_INPUT_CVBS_AIN7 0x06
+#define ADV7182_INPUT_CVBS_AIN8 0x07
+#define ADV7182_INPUT_SVIDEO_AIN1_AIN2 0x08
+#define ADV7182_INPUT_SVIDEO_AIN3_AIN4 0x09
+#define ADV7182_INPUT_SVIDEO_AIN5_AIN6 0x0a
+#define ADV7182_INPUT_SVIDEO_AIN7_AIN8 0x0b
+#define ADV7182_INPUT_YPRPB_AIN1_AIN2_AIN3 0x0c
+#define ADV7182_INPUT_YPRPB_AIN4_AIN5_AIN6 0x0d
+#define ADV7182_INPUT_DIFF_CVBS_AIN1_AIN2 0x0e
+#define ADV7182_INPUT_DIFF_CVBS_AIN3_AIN4 0x0f
+#define ADV7182_INPUT_DIFF_CVBS_AIN5_AIN6 0x10
+#define ADV7182_INPUT_DIFF_CVBS_AIN7_AIN8 0x11
+
+#define ADV7180_DEFAULT_CSI_I2C_ADDR 0x44
+#define ADV7180_DEFAULT_VPP_I2C_ADDR 0x42
+
+#define V4L2_CID_ADV_FAST_SWITCH	(V4L2_CID_USER_ADV7180_BASE + 0x00)
+
+/* Initial number of frames to skip to avoid possible garbage */
+#define ADV7180_NUM_OF_SKIP_FRAMES       2
+
+struct adv7180_state;
+
+#define ADV7180_FLAG_RESET_POWERED	BIT(0)
+#define ADV7180_FLAG_V2			BIT(1)
+#define ADV7180_FLAG_MIPI_CSI2		BIT(2)
+#define ADV7180_FLAG_I2P		BIT(3)
+
+struct adv7180_chip_info {
+	unsigned int flags;
+	unsigned int valid_input_mask;
+	int (*set_std)(struct adv7180_state *st, unsigned int std);
+	int (*select_input)(struct adv7180_state *st, unsigned int input);
+	int (*init)(struct adv7180_state *state);
+};
+
+struct adv7180_state {
+	struct v4l2_ctrl_handler ctrl_hdl;
+	struct v4l2_subdev	sd;
+	struct media_pad	pad;
+	struct mutex		mutex; /* mutual excl. when accessing chip */
+	int			irq;
+	struct gpio_desc	*pwdn_gpio;
+	struct gpio_desc	*rst_gpio;
+	v4l2_std_id		curr_norm;
+	bool			powered;
+	bool			streaming;
+	u8			input;
+
+	struct i2c_client	*client;
+	unsigned int		register_page;
+	struct i2c_client	*csi_client;
+	struct i2c_client	*vpp_client;
+	const struct adv7180_chip_info *chip_info;
+	enum v4l2_field		field;
+	bool			force_bt656_4;
+};
+#define to_adv7180_sd(_ctrl) (&container_of(_ctrl->handler,		\
+					    struct adv7180_state,	\
+					    ctrl_hdl)->sd)
+
+static int adv7180_select_page(struct adv7180_state *state, unsigned int page)
+{
+	if (state->register_page != page) {
+		i2c_smbus_write_byte_data(state->client, ADV7180_REG_CTRL,
+			page);
+		state->register_page = page;
+	}
+
+	return 0;
+}
+
+static int adv7180_write(struct adv7180_state *state, unsigned int reg,
+	unsigned int value)
+{
+	lockdep_assert_held(&state->mutex);
+	adv7180_select_page(state, reg >> 8);
+	return i2c_smbus_write_byte_data(state->client, reg & 0xff, value);
+}
+
+static int adv7180_read(struct adv7180_state *state, unsigned int reg)
+{
+	lockdep_assert_held(&state->mutex);
+	adv7180_select_page(state, reg >> 8);
+	return i2c_smbus_read_byte_data(state->client, reg & 0xff);
+}
+
+static int adv7180_csi_write(struct adv7180_state *state, unsigned int reg,
+	unsigned int value)
+{
+	return i2c_smbus_write_byte_data(state->csi_client, reg, value);
+}
+
+static int adv7180_set_video_standard(struct adv7180_state *state,
+	unsigned int std)
+{
+	return state->chip_info->set_std(state, std);
+}
+
+static int adv7180_vpp_write(struct adv7180_state *state, unsigned int reg,
+	unsigned int value)
+{
+	return i2c_smbus_write_byte_data(state->vpp_client, reg, value);
+}
+
+static v4l2_std_id adv7180_std_to_v4l2(u8 status1)
+{
+	/* in case V4L2_IN_ST_NO_SIGNAL */
+	if (!(status1 & ADV7180_STATUS1_IN_LOCK))
+		return V4L2_STD_UNKNOWN;
+
+	switch (status1 & ADV7180_STATUS1_AUTOD_MASK) {
+	case ADV7180_STATUS1_AUTOD_NTSM_M_J:
+		return V4L2_STD_NTSC;
+	case ADV7180_STATUS1_AUTOD_NTSC_4_43:
+		return V4L2_STD_NTSC_443;
+	case ADV7180_STATUS1_AUTOD_PAL_M:
+		return V4L2_STD_PAL_M;
+	case ADV7180_STATUS1_AUTOD_PAL_60:
+		return V4L2_STD_PAL_60;
+	case ADV7180_STATUS1_AUTOD_PAL_B_G:
+		return V4L2_STD_PAL;
+	case ADV7180_STATUS1_AUTOD_SECAM:
+		return V4L2_STD_SECAM;
+	case ADV7180_STATUS1_AUTOD_PAL_COMB:
+		return V4L2_STD_PAL_Nc | V4L2_STD_PAL_N;
+	case ADV7180_STATUS1_AUTOD_SECAM_525:
+		return V4L2_STD_SECAM;
+	default:
+		return V4L2_STD_UNKNOWN;
+	}
+}
+
+static int v4l2_std_to_adv7180(v4l2_std_id std)
+{
+	if (std == V4L2_STD_PAL_60)
+		return ADV7180_STD_PAL60;
+	if (std == V4L2_STD_NTSC_443)
+		return ADV7180_STD_NTSC_443;
+	if (std == V4L2_STD_PAL_N)
+		return ADV7180_STD_PAL_N;
+	if (std == V4L2_STD_PAL_M)
+		return ADV7180_STD_PAL_M;
+	if (std == V4L2_STD_PAL_Nc)
+		return ADV7180_STD_PAL_COMB_N;
+
+	if (std & V4L2_STD_PAL)
+		return ADV7180_STD_PAL_BG;
+	if (std & V4L2_STD_NTSC)
+		return ADV7180_STD_NTSC_M;
+	if (std & V4L2_STD_SECAM)
+		return ADV7180_STD_PAL_SECAM;
+
+	return -EINVAL;
+}
+
+static u32 adv7180_status_to_v4l2(u8 status1)
+{
+	if (!(status1 & ADV7180_STATUS1_IN_LOCK))
+		return V4L2_IN_ST_NO_SIGNAL;
+
+	return 0;
+}
+
+static int __adv7180_status(struct adv7180_state *state, u32 *status,
+			    v4l2_std_id *std)
+{
+	int status1 = adv7180_read(state, ADV7180_REG_STATUS1);
+
+	if (status1 < 0)
+		return status1;
+
+	if (status)
+		*status = adv7180_status_to_v4l2(status1);
+	if (std)
+		*std = adv7180_std_to_v4l2(status1);
+
+	return 0;
+}
+
+static inline struct adv7180_state *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct adv7180_state, sd);
+}
+
+static int adv7180_querystd(struct v4l2_subdev *sd, v4l2_std_id *std)
+{
+	struct adv7180_state *state = to_state(sd);
+	int err = mutex_lock_interruptible(&state->mutex);
+	if (err)
+		return err;
+
+	if (state->streaming) {
+		err = -EBUSY;
+		goto unlock;
+	}
+
+	err = adv7180_set_video_standard(state,
+			ADV7180_STD_AD_PAL_BG_NTSC_J_SECAM);
+	if (err)
+		goto unlock;
+
+	msleep(100);
+	__adv7180_status(state, NULL, std);
+
+	err = v4l2_std_to_adv7180(state->curr_norm);
+	if (err < 0)
+		goto unlock;
+
+	err = adv7180_set_video_standard(state, err);
+
+unlock:
+	mutex_unlock(&state->mutex);
+	return err;
+}
+
+static int adv7180_s_routing(struct v4l2_subdev *sd, u32 input,
+			     u32 output, u32 config)
+{
+	struct adv7180_state *state = to_state(sd);
+	int ret = mutex_lock_interruptible(&state->mutex);
+
+	if (ret)
+		return ret;
+
+	if (input > 31 || !(BIT(input) & state->chip_info->valid_input_mask)) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	ret = state->chip_info->select_input(state, input);
+
+	if (ret == 0)
+		state->input = input;
+out:
+	mutex_unlock(&state->mutex);
+	return ret;
+}
+
+static int adv7180_g_input_status(struct v4l2_subdev *sd, u32 *status)
+{
+	struct adv7180_state *state = to_state(sd);
+	int ret = mutex_lock_interruptible(&state->mutex);
+	if (ret)
+		return ret;
+
+	ret = __adv7180_status(state, status, NULL);
+	mutex_unlock(&state->mutex);
+	return ret;
+}
+
+static int adv7180_program_std(struct adv7180_state *state)
+{
+	int ret;
+
+	ret = v4l2_std_to_adv7180(state->curr_norm);
+	if (ret < 0)
+		return ret;
+
+	ret = adv7180_set_video_standard(state, ret);
+	if (ret < 0)
+		return ret;
+	return 0;
+}
+
+static int adv7180_s_std(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct adv7180_state *state = to_state(sd);
+	int ret = mutex_lock_interruptible(&state->mutex);
+
+	if (ret)
+		return ret;
+
+	/* Make sure we can support this std */
+	ret = v4l2_std_to_adv7180(std);
+	if (ret < 0)
+		goto out;
+
+	state->curr_norm = std;
+
+	ret = adv7180_program_std(state);
+out:
+	mutex_unlock(&state->mutex);
+	return ret;
+}
+
+static int adv7180_g_std(struct v4l2_subdev *sd, v4l2_std_id *norm)
+{
+	struct adv7180_state *state = to_state(sd);
+
+	*norm = state->curr_norm;
+
+	return 0;
+}
+
+static int adv7180_g_frame_interval(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_frame_interval *fi)
+{
+	struct adv7180_state *state = to_state(sd);
+
+	if (state->curr_norm & V4L2_STD_525_60) {
+		fi->interval.numerator = 1001;
+		fi->interval.denominator = 30000;
+	} else {
+		fi->interval.numerator = 1;
+		fi->interval.denominator = 25;
+	}
+
+	return 0;
+}
+
+static void adv7180_set_power_pin(struct adv7180_state *state, bool on)
+{
+	if (!state->pwdn_gpio)
+		return;
+
+	if (on) {
+		gpiod_set_value_cansleep(state->pwdn_gpio, 0);
+		usleep_range(5000, 10000);
+	} else {
+		gpiod_set_value_cansleep(state->pwdn_gpio, 1);
+	}
+}
+
+static void adv7180_set_reset_pin(struct adv7180_state *state, bool on)
+{
+	if (!state->rst_gpio)
+		return;
+
+	if (on) {
+		gpiod_set_value_cansleep(state->rst_gpio, 1);
+	} else {
+		gpiod_set_value_cansleep(state->rst_gpio, 0);
+		usleep_range(5000, 10000);
+	}
+}
+
+static int adv7180_set_power(struct adv7180_state *state, bool on)
+{
+	u8 val;
+	int ret;
+
+	if (on)
+		val = ADV7180_PWR_MAN_ON;
+	else
+		val = ADV7180_PWR_MAN_OFF;
+
+	ret = adv7180_write(state, ADV7180_REG_PWR_MAN, val);
+	if (ret)
+		return ret;
+
+	if (state->chip_info->flags & ADV7180_FLAG_MIPI_CSI2) {
+		if (on) {
+			adv7180_csi_write(state, 0xDE, 0x02);
+			adv7180_csi_write(state, 0xD2, 0xF7);
+			adv7180_csi_write(state, 0xD8, 0x65);
+			adv7180_csi_write(state, 0xE0, 0x09);
+			adv7180_csi_write(state, 0x2C, 0x00);
+			if (state->field == V4L2_FIELD_NONE)
+				adv7180_csi_write(state, 0x1D, 0x80);
+			adv7180_csi_write(state, 0x00, 0x00);
+		} else {
+			adv7180_csi_write(state, 0x00, 0x80);
+		}
+	}
+
+	return 0;
+}
+
+static int adv7180_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct adv7180_state *state = to_state(sd);
+	int ret;
+
+	ret = mutex_lock_interruptible(&state->mutex);
+	if (ret)
+		return ret;
+
+	ret = adv7180_set_power(state, on);
+	if (ret == 0)
+		state->powered = on;
+
+	mutex_unlock(&state->mutex);
+	return ret;
+}
+
+static const char * const test_pattern_menu[] = {
+	"Single color",
+	"Color bars",
+	"Luma ramp",
+	"Boundary box",
+	"Disable",
+};
+
+static int adv7180_test_pattern(struct adv7180_state *state, int value)
+{
+	unsigned int reg = 0;
+
+	/* Map menu value into register value */
+	if (value < 3)
+		reg = value;
+	if (value == 3)
+		reg = 5;
+
+	adv7180_write(state, ADV7180_REG_ANALOG_CLAMP_CTL, reg);
+
+	if (value == ARRAY_SIZE(test_pattern_menu) - 1) {
+		reg = adv7180_read(state, ADV7180_REG_DEF_VALUE_Y);
+		reg &= ~ADV7180_DEF_VAL_EN;
+		adv7180_write(state, ADV7180_REG_DEF_VALUE_Y, reg);
+		return 0;
+	}
+
+	reg = adv7180_read(state, ADV7180_REG_DEF_VALUE_Y);
+	reg |= ADV7180_DEF_VAL_EN | ADV7180_DEF_VAL_AUTO_EN;
+	adv7180_write(state, ADV7180_REG_DEF_VALUE_Y, reg);
+
+	return 0;
+}
+
+static int adv7180_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_adv7180_sd(ctrl);
+	struct adv7180_state *state = to_state(sd);
+	int ret = mutex_lock_interruptible(&state->mutex);
+	int val;
+
+	if (ret)
+		return ret;
+	val = ctrl->val;
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		ret = adv7180_write(state, ADV7180_REG_BRI, val);
+		break;
+	case V4L2_CID_HUE:
+		/*Hue is inverted according to HSL chart */
+		ret = adv7180_write(state, ADV7180_REG_HUE, -val);
+		break;
+	case V4L2_CID_CONTRAST:
+		ret = adv7180_write(state, ADV7180_REG_CON, val);
+		break;
+	case V4L2_CID_SATURATION:
+		/*
+		 *This could be V4L2_CID_BLUE_BALANCE/V4L2_CID_RED_BALANCE
+		 *Let's not confuse the user, everybody understands saturation
+		 */
+		ret = adv7180_write(state, ADV7180_REG_SD_SAT_CB, val);
+		if (ret < 0)
+			break;
+		ret = adv7180_write(state, ADV7180_REG_SD_SAT_CR, val);
+		break;
+	case V4L2_CID_ADV_FAST_SWITCH:
+		if (ctrl->val) {
+			/* ADI required write */
+			adv7180_write(state, 0x80d9, 0x44);
+			adv7180_write(state, ADV7180_REG_FLCONTROL,
+				ADV7180_FLCONTROL_FL_ENABLE);
+		} else {
+			/* ADI required write */
+			adv7180_write(state, 0x80d9, 0xc4);
+			adv7180_write(state, ADV7180_REG_FLCONTROL, 0x00);
+		}
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		ret = adv7180_test_pattern(state, val);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	mutex_unlock(&state->mutex);
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops adv7180_ctrl_ops = {
+	.s_ctrl = adv7180_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config adv7180_ctrl_fast_switch = {
+	.ops = &adv7180_ctrl_ops,
+	.id = V4L2_CID_ADV_FAST_SWITCH,
+	.name = "Fast Switching",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.min = 0,
+	.max = 1,
+	.step = 1,
+};
+
+static int adv7180_init_controls(struct adv7180_state *state)
+{
+	v4l2_ctrl_handler_init(&state->ctrl_hdl, 4);
+
+	v4l2_ctrl_new_std(&state->ctrl_hdl, &adv7180_ctrl_ops,
+			  V4L2_CID_BRIGHTNESS, ADV7180_BRI_MIN,
+			  ADV7180_BRI_MAX, 1, ADV7180_BRI_DEF);
+	v4l2_ctrl_new_std(&state->ctrl_hdl, &adv7180_ctrl_ops,
+			  V4L2_CID_CONTRAST, ADV7180_CON_MIN,
+			  ADV7180_CON_MAX, 1, ADV7180_CON_DEF);
+	v4l2_ctrl_new_std(&state->ctrl_hdl, &adv7180_ctrl_ops,
+			  V4L2_CID_SATURATION, ADV7180_SAT_MIN,
+			  ADV7180_SAT_MAX, 1, ADV7180_SAT_DEF);
+	v4l2_ctrl_new_std(&state->ctrl_hdl, &adv7180_ctrl_ops,
+			  V4L2_CID_HUE, ADV7180_HUE_MIN,
+			  ADV7180_HUE_MAX, 1, ADV7180_HUE_DEF);
+	v4l2_ctrl_new_custom(&state->ctrl_hdl, &adv7180_ctrl_fast_switch, NULL);
+
+	v4l2_ctrl_new_std_menu_items(&state->ctrl_hdl, &adv7180_ctrl_ops,
+				      V4L2_CID_TEST_PATTERN,
+				      ARRAY_SIZE(test_pattern_menu) - 1,
+				      0, ARRAY_SIZE(test_pattern_menu) - 1,
+				      test_pattern_menu);
+
+	state->sd.ctrl_handler = &state->ctrl_hdl;
+	if (state->ctrl_hdl.error) {
+		int err = state->ctrl_hdl.error;
+
+		v4l2_ctrl_handler_free(&state->ctrl_hdl);
+		return err;
+	}
+	v4l2_ctrl_handler_setup(&state->ctrl_hdl);
+
+	return 0;
+}
+static void adv7180_exit_controls(struct adv7180_state *state)
+{
+	v4l2_ctrl_handler_free(&state->ctrl_hdl);
+}
+
+static int adv7180_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index != 0)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_UYVY8_2X8;
+
+	return 0;
+}
+
+static int adv7180_mbus_fmt(struct v4l2_subdev *sd,
+			    struct v4l2_mbus_framefmt *fmt)
+{
+	struct adv7180_state *state = to_state(sd);
+
+	fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
+	fmt->colorspace = V4L2_COLORSPACE_SMPTE170M;
+	fmt->width = 720;
+	fmt->height = state->curr_norm & V4L2_STD_525_60 ? 480 : 576;
+
+	if (state->field == V4L2_FIELD_ALTERNATE)
+		fmt->height /= 2;
+
+	return 0;
+}
+
+static int adv7180_set_field_mode(struct adv7180_state *state)
+{
+	if (!(state->chip_info->flags & ADV7180_FLAG_I2P))
+		return 0;
+
+	if (state->field == V4L2_FIELD_NONE) {
+		if (state->chip_info->flags & ADV7180_FLAG_MIPI_CSI2) {
+			adv7180_csi_write(state, 0x01, 0x20);
+			adv7180_csi_write(state, 0x02, 0x28);
+			adv7180_csi_write(state, 0x03, 0x38);
+			adv7180_csi_write(state, 0x04, 0x30);
+			adv7180_csi_write(state, 0x05, 0x30);
+			adv7180_csi_write(state, 0x06, 0x80);
+			adv7180_csi_write(state, 0x07, 0x70);
+			adv7180_csi_write(state, 0x08, 0x50);
+		}
+		adv7180_vpp_write(state, 0xa3, 0x00);
+		adv7180_vpp_write(state, 0x5b, 0x00);
+		adv7180_vpp_write(state, 0x55, 0x80);
+	} else {
+		if (state->chip_info->flags & ADV7180_FLAG_MIPI_CSI2) {
+			adv7180_csi_write(state, 0x01, 0x18);
+			adv7180_csi_write(state, 0x02, 0x18);
+			adv7180_csi_write(state, 0x03, 0x30);
+			adv7180_csi_write(state, 0x04, 0x20);
+			adv7180_csi_write(state, 0x05, 0x28);
+			adv7180_csi_write(state, 0x06, 0x40);
+			adv7180_csi_write(state, 0x07, 0x58);
+			adv7180_csi_write(state, 0x08, 0x30);
+		}
+		adv7180_vpp_write(state, 0xa3, 0x70);
+		adv7180_vpp_write(state, 0x5b, 0x80);
+		adv7180_vpp_write(state, 0x55, 0x00);
+	}
+
+	return 0;
+}
+
+static int adv7180_get_pad_format(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_format *format)
+{
+	struct adv7180_state *state = to_state(sd);
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		format->format = *v4l2_subdev_get_try_format(sd, sd_state, 0);
+	} else {
+		adv7180_mbus_fmt(sd, &format->format);
+		format->format.field = state->field;
+	}
+
+	return 0;
+}
+
+static int adv7180_set_pad_format(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_format *format)
+{
+	struct adv7180_state *state = to_state(sd);
+	struct v4l2_mbus_framefmt *framefmt;
+	int ret;
+
+	switch (format->format.field) {
+	case V4L2_FIELD_NONE:
+		if (state->chip_info->flags & ADV7180_FLAG_I2P)
+			break;
+		fallthrough;
+	default:
+		format->format.field = V4L2_FIELD_ALTERNATE;
+		break;
+	}
+
+	ret = adv7180_mbus_fmt(sd,  &format->format);
+
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		if (state->field != format->format.field) {
+			state->field = format->format.field;
+			adv7180_set_power(state, false);
+			adv7180_set_field_mode(state);
+			adv7180_set_power(state, true);
+		}
+	} else {
+		framefmt = v4l2_subdev_get_try_format(sd, sd_state, 0);
+		*framefmt = format->format;
+	}
+
+	return ret;
+}
+
+static int adv7180_init_cfg(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_state *sd_state)
+{
+	struct v4l2_subdev_format fmt = {
+		.which = sd_state ? V4L2_SUBDEV_FORMAT_TRY
+		: V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+
+	return adv7180_set_pad_format(sd, sd_state, &fmt);
+}
+
+static int adv7180_get_mbus_config(struct v4l2_subdev *sd,
+				   unsigned int pad,
+				   struct v4l2_mbus_config *cfg)
+{
+	struct adv7180_state *state = to_state(sd);
+
+	if (state->chip_info->flags & ADV7180_FLAG_MIPI_CSI2) {
+		cfg->type = V4L2_MBUS_CSI2_DPHY;
+		cfg->bus.mipi_csi2.num_data_lanes = 1;
+		cfg->bus.mipi_csi2.flags = 0;
+	} else {
+		/*
+		 * The ADV7180 sensor supports BT.601/656 output modes.
+		 * The BT.656 is default and not yet configurable by s/w.
+		 */
+		cfg->bus.parallel.flags = V4L2_MBUS_MASTER |
+					  V4L2_MBUS_PCLK_SAMPLE_RISING |
+					  V4L2_MBUS_DATA_ACTIVE_HIGH;
+		cfg->type = V4L2_MBUS_BT656;
+	}
+
+	return 0;
+}
+
+static int adv7180_get_skip_frames(struct v4l2_subdev *sd, u32 *frames)
+{
+	*frames = ADV7180_NUM_OF_SKIP_FRAMES;
+
+	return 0;
+}
+
+static int adv7180_g_pixelaspect(struct v4l2_subdev *sd, struct v4l2_fract *aspect)
+{
+	struct adv7180_state *state = to_state(sd);
+
+	if (state->curr_norm & V4L2_STD_525_60) {
+		aspect->numerator = 11;
+		aspect->denominator = 10;
+	} else {
+		aspect->numerator = 54;
+		aspect->denominator = 59;
+	}
+
+	return 0;
+}
+
+static int adv7180_g_tvnorms(struct v4l2_subdev *sd, v4l2_std_id *norm)
+{
+	*norm = V4L2_STD_ALL;
+	return 0;
+}
+
+static int adv7180_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct adv7180_state *state = to_state(sd);
+	int ret;
+
+	/* It's always safe to stop streaming, no need to take the lock */
+	if (!enable) {
+		state->streaming = enable;
+		return 0;
+	}
+
+	/* Must wait until querystd released the lock */
+	ret = mutex_lock_interruptible(&state->mutex);
+	if (ret)
+		return ret;
+	state->streaming = enable;
+	mutex_unlock(&state->mutex);
+	return 0;
+}
+
+static int adv7180_subscribe_event(struct v4l2_subdev *sd,
+				   struct v4l2_fh *fh,
+				   struct v4l2_event_subscription *sub)
+{
+	switch (sub->type) {
+	case V4L2_EVENT_SOURCE_CHANGE:
+		return v4l2_src_change_event_subdev_subscribe(sd, fh, sub);
+	case V4L2_EVENT_CTRL:
+		return v4l2_ctrl_subdev_subscribe_event(sd, fh, sub);
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct v4l2_subdev_video_ops adv7180_video_ops = {
+	.s_std = adv7180_s_std,
+	.g_std = adv7180_g_std,
+	.g_frame_interval = adv7180_g_frame_interval,
+	.querystd = adv7180_querystd,
+	.g_input_status = adv7180_g_input_status,
+	.s_routing = adv7180_s_routing,
+	.g_pixelaspect = adv7180_g_pixelaspect,
+	.g_tvnorms = adv7180_g_tvnorms,
+	.s_stream = adv7180_s_stream,
+};
+
+static const struct v4l2_subdev_core_ops adv7180_core_ops = {
+	.s_power = adv7180_s_power,
+	.subscribe_event = adv7180_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_pad_ops adv7180_pad_ops = {
+	.init_cfg = adv7180_init_cfg,
+	.enum_mbus_code = adv7180_enum_mbus_code,
+	.set_fmt = adv7180_set_pad_format,
+	.get_fmt = adv7180_get_pad_format,
+	.get_mbus_config = adv7180_get_mbus_config,
+};
+
+static const struct v4l2_subdev_sensor_ops adv7180_sensor_ops = {
+	.g_skip_frames = adv7180_get_skip_frames,
+};
+
+static const struct v4l2_subdev_ops adv7180_ops = {
+	.core = &adv7180_core_ops,
+	.video = &adv7180_video_ops,
+	.pad = &adv7180_pad_ops,
+	.sensor = &adv7180_sensor_ops,
+};
+
+static irqreturn_t adv7180_irq(int irq, void *devid)
+{
+	struct adv7180_state *state = devid;
+	u8 isr3;
+
+	mutex_lock(&state->mutex);
+	isr3 = adv7180_read(state, ADV7180_REG_ISR3);
+	/* clear */
+	adv7180_write(state, ADV7180_REG_ICR3, isr3);
+
+	if (isr3 & ADV7180_IRQ3_AD_CHANGE) {
+		static const struct v4l2_event src_ch = {
+			.type = V4L2_EVENT_SOURCE_CHANGE,
+			.u.src_change.changes = V4L2_EVENT_SRC_CH_RESOLUTION,
+		};
+
+		v4l2_subdev_notify_event(&state->sd, &src_ch);
+	}
+	mutex_unlock(&state->mutex);
+
+	return IRQ_HANDLED;
+}
+
+static int adv7180_init(struct adv7180_state *state)
+{
+	int ret;
+
+	/* ITU-R BT.656-4 compatible */
+	ret = adv7180_write(state, ADV7180_REG_EXTENDED_OUTPUT_CONTROL,
+			ADV7180_EXTENDED_OUTPUT_CONTROL_NTSCDIS);
+	if (ret < 0)
+		return ret;
+
+	/* Manually set V bit end position in NTSC mode */
+	return adv7180_write(state, ADV7180_REG_NTSC_V_BIT_END,
+					ADV7180_NTSC_V_BIT_END_MANUAL_NVEND);
+}
+
+static int adv7180_set_std(struct adv7180_state *state, unsigned int std)
+{
+	return adv7180_write(state, ADV7180_REG_INPUT_CONTROL,
+		(std << 4) | state->input);
+}
+
+static int adv7180_select_input(struct adv7180_state *state, unsigned int input)
+{
+	int ret;
+
+	ret = adv7180_read(state, ADV7180_REG_INPUT_CONTROL);
+	if (ret < 0)
+		return ret;
+
+	ret &= ~ADV7180_INPUT_CONTROL_INSEL_MASK;
+	ret |= input;
+	return adv7180_write(state, ADV7180_REG_INPUT_CONTROL, ret);
+}
+
+static int adv7182_init(struct adv7180_state *state)
+{
+	if (state->chip_info->flags & ADV7180_FLAG_MIPI_CSI2)
+		adv7180_write(state, ADV7180_REG_CSI_SLAVE_ADDR,
+			ADV7180_DEFAULT_CSI_I2C_ADDR << 1);
+
+	if (state->chip_info->flags & ADV7180_FLAG_I2P)
+		adv7180_write(state, ADV7180_REG_VPP_SLAVE_ADDR,
+			ADV7180_DEFAULT_VPP_I2C_ADDR << 1);
+
+	if (state->chip_info->flags & ADV7180_FLAG_V2) {
+		/* ADI recommended writes for improved video quality */
+		adv7180_write(state, 0x0080, 0x51);
+		adv7180_write(state, 0x0081, 0x51);
+		adv7180_write(state, 0x0082, 0x68);
+	}
+
+	/* ADI required writes */
+	if (state->chip_info->flags & ADV7180_FLAG_MIPI_CSI2) {
+		adv7180_write(state, ADV7180_REG_OUTPUT_CONTROL, 0x4e);
+		adv7180_write(state, ADV7180_REG_EXTENDED_OUTPUT_CONTROL, 0x57);
+		adv7180_write(state, ADV7180_REG_CTRL_2, 0xc0);
+	} else {
+		if (state->chip_info->flags & ADV7180_FLAG_V2) {
+			if (state->force_bt656_4) {
+				/* ITU-R BT.656-4 compatible */
+				adv7180_write(state,
+					      ADV7180_REG_EXTENDED_OUTPUT_CONTROL,
+					      ADV7180_EXTENDED_OUTPUT_CONTROL_NTSCDIS);
+				/* Manually set NEWAVMODE */
+				adv7180_write(state,
+					      ADV7180_REG_VSYNC_FIELD_CTL_1,
+					      ADV7180_VSYNC_FIELD_CTL_1_NEWAV);
+				/* Manually set V bit end position in NTSC mode */
+				adv7180_write(state,
+					      ADV7180_REG_NTSC_V_BIT_END,
+					      ADV7180_NTSC_V_BIT_END_MANUAL_NVEND);
+			} else {
+				adv7180_write(state,
+					      ADV7180_REG_EXTENDED_OUTPUT_CONTROL,
+					      0x17);
+			}
+		}
+		else
+			adv7180_write(state,
+				      ADV7180_REG_EXTENDED_OUTPUT_CONTROL,
+				      0x07);
+		adv7180_write(state, ADV7180_REG_OUTPUT_CONTROL, 0x0c);
+		adv7180_write(state, ADV7180_REG_CTRL_2, 0x40);
+	}
+
+	adv7180_write(state, 0x0013, 0x00);
+
+	return 0;
+}
+
+static int adv7182_set_std(struct adv7180_state *state, unsigned int std)
+{
+	/* Failing to set the reserved bit can result in increased video noise */
+	return adv7180_write(state, ADV7182_REG_INPUT_VIDSEL,
+			     (std << 4) | ADV7182_REG_INPUT_RESERVED);
+}
+
+enum adv7182_input_type {
+	ADV7182_INPUT_TYPE_CVBS,
+	ADV7182_INPUT_TYPE_DIFF_CVBS,
+	ADV7182_INPUT_TYPE_SVIDEO,
+	ADV7182_INPUT_TYPE_YPBPR,
+};
+
+static enum adv7182_input_type adv7182_get_input_type(unsigned int input)
+{
+	switch (input) {
+	case ADV7182_INPUT_CVBS_AIN1:
+	case ADV7182_INPUT_CVBS_AIN2:
+	case ADV7182_INPUT_CVBS_AIN3:
+	case ADV7182_INPUT_CVBS_AIN4:
+	case ADV7182_INPUT_CVBS_AIN5:
+	case ADV7182_INPUT_CVBS_AIN6:
+	case ADV7182_INPUT_CVBS_AIN7:
+	case ADV7182_INPUT_CVBS_AIN8:
+		return ADV7182_INPUT_TYPE_CVBS;
+	case ADV7182_INPUT_SVIDEO_AIN1_AIN2:
+	case ADV7182_INPUT_SVIDEO_AIN3_AIN4:
+	case ADV7182_INPUT_SVIDEO_AIN5_AIN6:
+	case ADV7182_INPUT_SVIDEO_AIN7_AIN8:
+		return ADV7182_INPUT_TYPE_SVIDEO;
+	case ADV7182_INPUT_YPRPB_AIN1_AIN2_AIN3:
+	case ADV7182_INPUT_YPRPB_AIN4_AIN5_AIN6:
+		return ADV7182_INPUT_TYPE_YPBPR;
+	case ADV7182_INPUT_DIFF_CVBS_AIN1_AIN2:
+	case ADV7182_INPUT_DIFF_CVBS_AIN3_AIN4:
+	case ADV7182_INPUT_DIFF_CVBS_AIN5_AIN6:
+	case ADV7182_INPUT_DIFF_CVBS_AIN7_AIN8:
+		return ADV7182_INPUT_TYPE_DIFF_CVBS;
+	default: /* Will never happen */
+		return 0;
+	}
+}
+
+/* ADI recommended writes to registers 0x52, 0x53, 0x54 */
+static unsigned int adv7182_lbias_settings[][3] = {
+	[ADV7182_INPUT_TYPE_CVBS] = { 0xCB, 0x4E, 0x80 },
+	[ADV7182_INPUT_TYPE_DIFF_CVBS] = { 0xC0, 0x4E, 0x80 },
+	[ADV7182_INPUT_TYPE_SVIDEO] = { 0x0B, 0xCE, 0x80 },
+	[ADV7182_INPUT_TYPE_YPBPR] = { 0x0B, 0x4E, 0xC0 },
+};
+
+static unsigned int adv7280_lbias_settings[][3] = {
+	[ADV7182_INPUT_TYPE_CVBS] = { 0xCD, 0x4E, 0x80 },
+	[ADV7182_INPUT_TYPE_DIFF_CVBS] = { 0xC0, 0x4E, 0x80 },
+	[ADV7182_INPUT_TYPE_SVIDEO] = { 0x0B, 0xCE, 0x80 },
+	[ADV7182_INPUT_TYPE_YPBPR] = { 0x0B, 0x4E, 0xC0 },
+};
+
+static int adv7182_select_input(struct adv7180_state *state, unsigned int input)
+{
+	enum adv7182_input_type input_type;
+	unsigned int *lbias;
+	unsigned int i;
+	int ret;
+
+	ret = adv7180_write(state, ADV7180_REG_INPUT_CONTROL, input);
+	if (ret)
+		return ret;
+
+	/* Reset clamp circuitry - ADI recommended writes */
+	adv7180_write(state, ADV7180_REG_RST_CLAMP, 0x00);
+	adv7180_write(state, ADV7180_REG_RST_CLAMP, 0xff);
+
+	input_type = adv7182_get_input_type(input);
+
+	switch (input_type) {
+	case ADV7182_INPUT_TYPE_CVBS:
+	case ADV7182_INPUT_TYPE_DIFF_CVBS:
+		/* ADI recommends to use the SH1 filter */
+		adv7180_write(state, ADV7180_REG_SHAP_FILTER_CTL_1, 0x41);
+		break;
+	default:
+		adv7180_write(state, ADV7180_REG_SHAP_FILTER_CTL_1, 0x01);
+		break;
+	}
+
+	if (state->chip_info->flags & ADV7180_FLAG_V2)
+		lbias = adv7280_lbias_settings[input_type];
+	else
+		lbias = adv7182_lbias_settings[input_type];
+
+	for (i = 0; i < ARRAY_SIZE(adv7182_lbias_settings[0]); i++)
+		adv7180_write(state, ADV7180_REG_CVBS_TRIM + i, lbias[i]);
+
+	if (input_type == ADV7182_INPUT_TYPE_DIFF_CVBS) {
+		/* ADI required writes to make differential CVBS work */
+		adv7180_write(state, ADV7180_REG_RES_CIR, 0xa8);
+		adv7180_write(state, ADV7180_REG_CLAMP_ADJ, 0x90);
+		adv7180_write(state, ADV7180_REG_DIFF_MODE, 0xb0);
+		adv7180_write(state, ADV7180_REG_AGC_ADJ1, 0x08);
+		adv7180_write(state, ADV7180_REG_AGC_ADJ2, 0xa0);
+	} else {
+		adv7180_write(state, ADV7180_REG_RES_CIR, 0xf0);
+		adv7180_write(state, ADV7180_REG_CLAMP_ADJ, 0xd0);
+		adv7180_write(state, ADV7180_REG_DIFF_MODE, 0x10);
+		adv7180_write(state, ADV7180_REG_AGC_ADJ1, 0x9c);
+		adv7180_write(state, ADV7180_REG_AGC_ADJ2, 0x00);
+	}
+
+	return 0;
+}
+
+static const struct adv7180_chip_info adv7180_info = {
+	.flags = ADV7180_FLAG_RESET_POWERED,
+	/* We cannot discriminate between LQFP and 40-pin LFCSP, so accept
+	 * all inputs and let the card driver take care of validation
+	 */
+	.valid_input_mask = BIT(ADV7180_INPUT_CVBS_AIN1) |
+		BIT(ADV7180_INPUT_CVBS_AIN2) |
+		BIT(ADV7180_INPUT_CVBS_AIN3) |
+		BIT(ADV7180_INPUT_CVBS_AIN4) |
+		BIT(ADV7180_INPUT_CVBS_AIN5) |
+		BIT(ADV7180_INPUT_CVBS_AIN6) |
+		BIT(ADV7180_INPUT_SVIDEO_AIN1_AIN2) |
+		BIT(ADV7180_INPUT_SVIDEO_AIN3_AIN4) |
+		BIT(ADV7180_INPUT_SVIDEO_AIN5_AIN6) |
+		BIT(ADV7180_INPUT_YPRPB_AIN1_AIN2_AIN3) |
+		BIT(ADV7180_INPUT_YPRPB_AIN4_AIN5_AIN6),
+	.init = adv7180_init,
+	.set_std = adv7180_set_std,
+	.select_input = adv7180_select_input,
+};
+
+static const struct adv7180_chip_info adv7182_info = {
+	.valid_input_mask = BIT(ADV7182_INPUT_CVBS_AIN1) |
+		BIT(ADV7182_INPUT_CVBS_AIN2) |
+		BIT(ADV7182_INPUT_CVBS_AIN3) |
+		BIT(ADV7182_INPUT_CVBS_AIN4) |
+		BIT(ADV7182_INPUT_SVIDEO_AIN1_AIN2) |
+		BIT(ADV7182_INPUT_SVIDEO_AIN3_AIN4) |
+		BIT(ADV7182_INPUT_YPRPB_AIN1_AIN2_AIN3) |
+		BIT(ADV7182_INPUT_DIFF_CVBS_AIN1_AIN2) |
+		BIT(ADV7182_INPUT_DIFF_CVBS_AIN3_AIN4),
+	.init = adv7182_init,
+	.set_std = adv7182_set_std,
+	.select_input = adv7182_select_input,
+};
+
+static const struct adv7180_chip_info adv7280_info = {
+	.flags = ADV7180_FLAG_V2 | ADV7180_FLAG_I2P,
+	.valid_input_mask = BIT(ADV7182_INPUT_CVBS_AIN1) |
+		BIT(ADV7182_INPUT_CVBS_AIN2) |
+		BIT(ADV7182_INPUT_CVBS_AIN3) |
+		BIT(ADV7182_INPUT_CVBS_AIN4) |
+		BIT(ADV7182_INPUT_SVIDEO_AIN1_AIN2) |
+		BIT(ADV7182_INPUT_SVIDEO_AIN3_AIN4) |
+		BIT(ADV7182_INPUT_YPRPB_AIN1_AIN2_AIN3),
+	.init = adv7182_init,
+	.set_std = adv7182_set_std,
+	.select_input = adv7182_select_input,
+};
+
+static const struct adv7180_chip_info adv7280_m_info = {
+	.flags = ADV7180_FLAG_V2 | ADV7180_FLAG_MIPI_CSI2 | ADV7180_FLAG_I2P,
+	.valid_input_mask = BIT(ADV7182_INPUT_CVBS_AIN1) |
+		BIT(ADV7182_INPUT_CVBS_AIN2) |
+		BIT(ADV7182_INPUT_CVBS_AIN3) |
+		BIT(ADV7182_INPUT_CVBS_AIN4) |
+		BIT(ADV7182_INPUT_CVBS_AIN5) |
+		BIT(ADV7182_INPUT_CVBS_AIN6) |
+		BIT(ADV7182_INPUT_CVBS_AIN7) |
+		BIT(ADV7182_INPUT_CVBS_AIN8) |
+		BIT(ADV7182_INPUT_SVIDEO_AIN1_AIN2) |
+		BIT(ADV7182_INPUT_SVIDEO_AIN3_AIN4) |
+		BIT(ADV7182_INPUT_SVIDEO_AIN5_AIN6) |
+		BIT(ADV7182_INPUT_SVIDEO_AIN7_AIN8) |
+		BIT(ADV7182_INPUT_YPRPB_AIN1_AIN2_AIN3) |
+		BIT(ADV7182_INPUT_YPRPB_AIN4_AIN5_AIN6),
+	.init = adv7182_init,
+	.set_std = adv7182_set_std,
+	.select_input = adv7182_select_input,
+};
+
+static const struct adv7180_chip_info adv7281_info = {
+	.flags = ADV7180_FLAG_V2 | ADV7180_FLAG_MIPI_CSI2,
+	.valid_input_mask = BIT(ADV7182_INPUT_CVBS_AIN1) |
+		BIT(ADV7182_INPUT_CVBS_AIN2) |
+		BIT(ADV7182_INPUT_CVBS_AIN7) |
+		BIT(ADV7182_INPUT_CVBS_AIN8) |
+		BIT(ADV7182_INPUT_SVIDEO_AIN1_AIN2) |
+		BIT(ADV7182_INPUT_SVIDEO_AIN7_AIN8) |
+		BIT(ADV7182_INPUT_DIFF_CVBS_AIN1_AIN2) |
+		BIT(ADV7182_INPUT_DIFF_CVBS_AIN7_AIN8),
+	.init = adv7182_init,
+	.set_std = adv7182_set_std,
+	.select_input = adv7182_select_input,
+};
+
+static const struct adv7180_chip_info adv7281_m_info = {
+	.flags = ADV7180_FLAG_V2 | ADV7180_FLAG_MIPI_CSI2,
+	.valid_input_mask = BIT(ADV7182_INPUT_CVBS_AIN1) |
+		BIT(ADV7182_INPUT_CVBS_AIN2) |
+		BIT(ADV7182_INPUT_CVBS_AIN3) |
+		BIT(ADV7182_INPUT_CVBS_AIN4) |
+		BIT(ADV7182_INPUT_CVBS_AIN7) |
+		BIT(ADV7182_INPUT_CVBS_AIN8) |
+		BIT(ADV7182_INPUT_SVIDEO_AIN1_AIN2) |
+		BIT(ADV7182_INPUT_SVIDEO_AIN3_AIN4) |
+		BIT(ADV7182_INPUT_SVIDEO_AIN7_AIN8) |
+		BIT(ADV7182_INPUT_YPRPB_AIN1_AIN2_AIN3) |
+		BIT(ADV7182_INPUT_DIFF_CVBS_AIN1_AIN2) |
+		BIT(ADV7182_INPUT_DIFF_CVBS_AIN3_AIN4) |
+		BIT(ADV7182_INPUT_DIFF_CVBS_AIN7_AIN8),
+	.init = adv7182_init,
+	.set_std = adv7182_set_std,
+	.select_input = adv7182_select_input,
+};
+
+static const struct adv7180_chip_info adv7281_ma_info = {
+	.flags = ADV7180_FLAG_V2 | ADV7180_FLAG_MIPI_CSI2,
+	.valid_input_mask = BIT(ADV7182_INPUT_CVBS_AIN1) |
+		BIT(ADV7182_INPUT_CVBS_AIN2) |
+		BIT(ADV7182_INPUT_CVBS_AIN3) |
+		BIT(ADV7182_INPUT_CVBS_AIN4) |
+		BIT(ADV7182_INPUT_CVBS_AIN5) |
+		BIT(ADV7182_INPUT_CVBS_AIN6) |
+		BIT(ADV7182_INPUT_CVBS_AIN7) |
+		BIT(ADV7182_INPUT_CVBS_AIN8) |
+		BIT(ADV7182_INPUT_SVIDEO_AIN1_AIN2) |
+		BIT(ADV7182_INPUT_SVIDEO_AIN3_AIN4) |
+		BIT(ADV7182_INPUT_SVIDEO_AIN5_AIN6) |
+		BIT(ADV7182_INPUT_SVIDEO_AIN7_AIN8) |
+		BIT(ADV7182_INPUT_YPRPB_AIN1_AIN2_AIN3) |
+		BIT(ADV7182_INPUT_YPRPB_AIN4_AIN5_AIN6) |
+		BIT(ADV7182_INPUT_DIFF_CVBS_AIN1_AIN2) |
+		BIT(ADV7182_INPUT_DIFF_CVBS_AIN3_AIN4) |
+		BIT(ADV7182_INPUT_DIFF_CVBS_AIN5_AIN6) |
+		BIT(ADV7182_INPUT_DIFF_CVBS_AIN7_AIN8),
+	.init = adv7182_init,
+	.set_std = adv7182_set_std,
+	.select_input = adv7182_select_input,
+};
+
+static const struct adv7180_chip_info adv7282_info = {
+	.flags = ADV7180_FLAG_V2 | ADV7180_FLAG_I2P,
+	.valid_input_mask = BIT(ADV7182_INPUT_CVBS_AIN1) |
+		BIT(ADV7182_INPUT_CVBS_AIN2) |
+		BIT(ADV7182_INPUT_CVBS_AIN7) |
+		BIT(ADV7182_INPUT_CVBS_AIN8) |
+		BIT(ADV7182_INPUT_SVIDEO_AIN1_AIN2) |
+		BIT(ADV7182_INPUT_SVIDEO_AIN7_AIN8) |
+		BIT(ADV7182_INPUT_DIFF_CVBS_AIN1_AIN2) |
+		BIT(ADV7182_INPUT_DIFF_CVBS_AIN7_AIN8),
+	.init = adv7182_init,
+	.set_std = adv7182_set_std,
+	.select_input = adv7182_select_input,
+};
+
+static const struct adv7180_chip_info adv7282_m_info = {
+	.flags = ADV7180_FLAG_V2 | ADV7180_FLAG_MIPI_CSI2 | ADV7180_FLAG_I2P,
+	.valid_input_mask = BIT(ADV7182_INPUT_CVBS_AIN1) |
+		BIT(ADV7182_INPUT_CVBS_AIN2) |
+		BIT(ADV7182_INPUT_CVBS_AIN3) |
+		BIT(ADV7182_INPUT_CVBS_AIN4) |
+		BIT(ADV7182_INPUT_CVBS_AIN7) |
+		BIT(ADV7182_INPUT_CVBS_AIN8) |
+		BIT(ADV7182_INPUT_SVIDEO_AIN1_AIN2) |
+		BIT(ADV7182_INPUT_SVIDEO_AIN3_AIN4) |
+		BIT(ADV7182_INPUT_SVIDEO_AIN7_AIN8) |
+		BIT(ADV7182_INPUT_DIFF_CVBS_AIN1_AIN2) |
+		BIT(ADV7182_INPUT_DIFF_CVBS_AIN3_AIN4) |
+		BIT(ADV7182_INPUT_DIFF_CVBS_AIN7_AIN8),
+	.init = adv7182_init,
+	.set_std = adv7182_set_std,
+	.select_input = adv7182_select_input,
+};
+
+static int init_device(struct adv7180_state *state)
+{
+	int ret;
+
+	mutex_lock(&state->mutex);
+
+	adv7180_set_power_pin(state, true);
+	adv7180_set_reset_pin(state, false);
+
+	adv7180_write(state, ADV7180_REG_PWR_MAN, ADV7180_PWR_MAN_RES);
+	usleep_range(5000, 10000);
+
+	ret = state->chip_info->init(state);
+	if (ret)
+		goto out_unlock;
+
+	ret = adv7180_program_std(state);
+	if (ret)
+		goto out_unlock;
+
+	adv7180_set_field_mode(state);
+
+	/* register for interrupts */
+	if (state->irq > 0) {
+		/* config the Interrupt pin to be active low */
+		ret = adv7180_write(state, ADV7180_REG_ICONF1,
+						ADV7180_ICONF1_ACTIVE_LOW |
+						ADV7180_ICONF1_PSYNC_ONLY);
+		if (ret < 0)
+			goto out_unlock;
+
+		ret = adv7180_write(state, ADV7180_REG_IMR1, 0);
+		if (ret < 0)
+			goto out_unlock;
+
+		ret = adv7180_write(state, ADV7180_REG_IMR2, 0);
+		if (ret < 0)
+			goto out_unlock;
+
+		/* enable AD change interrupts interrupts */
+		ret = adv7180_write(state, ADV7180_REG_IMR3,
+						ADV7180_IRQ3_AD_CHANGE);
+		if (ret < 0)
+			goto out_unlock;
+
+		ret = adv7180_write(state, ADV7180_REG_IMR4, 0);
+		if (ret < 0)
+			goto out_unlock;
+	}
+
+out_unlock:
+	mutex_unlock(&state->mutex);
+
+	return ret;
+}
+
+static int adv7180_probe(struct i2c_client *client)
+{
+	const struct i2c_device_id *id = i2c_client_get_device_id(client);
+	struct device_node *np = client->dev.of_node;
+	struct adv7180_state *state;
+	struct v4l2_subdev *sd;
+	int ret;
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+
+	state = devm_kzalloc(&client->dev, sizeof(*state), GFP_KERNEL);
+	if (state == NULL)
+		return -ENOMEM;
+
+	state->client = client;
+	state->field = V4L2_FIELD_ALTERNATE;
+	state->chip_info = (struct adv7180_chip_info *)id->driver_data;
+
+	state->pwdn_gpio = devm_gpiod_get_optional(&client->dev, "powerdown",
+						   GPIOD_OUT_HIGH);
+	if (IS_ERR(state->pwdn_gpio)) {
+		ret = PTR_ERR(state->pwdn_gpio);
+		v4l_err(client, "request for power pin failed: %d\n", ret);
+		return ret;
+	}
+
+	state->rst_gpio = devm_gpiod_get_optional(&client->dev, "reset",
+						  GPIOD_OUT_HIGH);
+	if (IS_ERR(state->rst_gpio)) {
+		ret = PTR_ERR(state->rst_gpio);
+		v4l_err(client, "request for reset pin failed: %d\n", ret);
+		return ret;
+	}
+
+	if (of_property_read_bool(np, "adv,force-bt656-4"))
+		state->force_bt656_4 = true;
+
+	if (state->chip_info->flags & ADV7180_FLAG_MIPI_CSI2) {
+		state->csi_client = i2c_new_dummy_device(client->adapter,
+				ADV7180_DEFAULT_CSI_I2C_ADDR);
+		if (IS_ERR(state->csi_client))
+			return PTR_ERR(state->csi_client);
+	}
+
+	if (state->chip_info->flags & ADV7180_FLAG_I2P) {
+		state->vpp_client = i2c_new_dummy_device(client->adapter,
+				ADV7180_DEFAULT_VPP_I2C_ADDR);
+		if (IS_ERR(state->vpp_client)) {
+			ret = PTR_ERR(state->vpp_client);
+			goto err_unregister_csi_client;
+		}
+	}
+
+	state->irq = client->irq;
+	mutex_init(&state->mutex);
+	state->curr_norm = V4L2_STD_NTSC;
+	if (state->chip_info->flags & ADV7180_FLAG_RESET_POWERED)
+		state->powered = true;
+	else
+		state->powered = false;
+	state->input = 0;
+	sd = &state->sd;
+	v4l2_i2c_subdev_init(sd, client, &adv7180_ops);
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
+
+	ret = adv7180_init_controls(state);
+	if (ret)
+		goto err_unregister_vpp_client;
+
+	state->pad.flags = MEDIA_PAD_FL_SOURCE;
+	sd->entity.function = MEDIA_ENT_F_ATV_DECODER;
+	ret = media_entity_pads_init(&sd->entity, 1, &state->pad);
+	if (ret)
+		goto err_free_ctrl;
+
+	ret = init_device(state);
+	if (ret)
+		goto err_media_entity_cleanup;
+
+	if (state->irq) {
+		ret = request_threaded_irq(client->irq, NULL, adv7180_irq,
+					   IRQF_ONESHOT | IRQF_TRIGGER_FALLING,
+					   KBUILD_MODNAME, state);
+		if (ret)
+			goto err_media_entity_cleanup;
+	}
+
+	ret = v4l2_async_register_subdev(sd);
+	if (ret)
+		goto err_free_irq;
+
+	mutex_lock(&state->mutex);
+	ret = adv7180_read(state, ADV7180_REG_IDENT);
+	mutex_unlock(&state->mutex);
+	if (ret < 0)
+		goto err_v4l2_async_unregister;
+
+	v4l_info(client, "chip id 0x%x found @ 0x%02x (%s)\n",
+		 ret, client->addr, client->adapter->name);
+
+	return 0;
+
+err_v4l2_async_unregister:
+	v4l2_async_unregister_subdev(sd);
+err_free_irq:
+	if (state->irq > 0)
+		free_irq(client->irq, state);
+err_media_entity_cleanup:
+	media_entity_cleanup(&sd->entity);
+err_free_ctrl:
+	adv7180_exit_controls(state);
+err_unregister_vpp_client:
+	i2c_unregister_device(state->vpp_client);
+err_unregister_csi_client:
+	i2c_unregister_device(state->csi_client);
+	mutex_destroy(&state->mutex);
+	return ret;
+}
+
+static void adv7180_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct adv7180_state *state = to_state(sd);
+
+	v4l2_async_unregister_subdev(sd);
+
+	if (state->irq > 0)
+		free_irq(client->irq, state);
+
+	media_entity_cleanup(&sd->entity);
+	adv7180_exit_controls(state);
+
+	i2c_unregister_device(state->vpp_client);
+	i2c_unregister_device(state->csi_client);
+
+	adv7180_set_reset_pin(state, true);
+	adv7180_set_power_pin(state, false);
+
+	mutex_destroy(&state->mutex);
+}
+
+static const struct i2c_device_id adv7180_id[] = {
+	{ "adv7180", (kernel_ulong_t)&adv7180_info },
+	{ "adv7180cp", (kernel_ulong_t)&adv7180_info },
+	{ "adv7180st", (kernel_ulong_t)&adv7180_info },
+	{ "adv7182", (kernel_ulong_t)&adv7182_info },
+	{ "adv7280", (kernel_ulong_t)&adv7280_info },
+	{ "adv7280-m", (kernel_ulong_t)&adv7280_m_info },
+	{ "adv7281", (kernel_ulong_t)&adv7281_info },
+	{ "adv7281-m", (kernel_ulong_t)&adv7281_m_info },
+	{ "adv7281-ma", (kernel_ulong_t)&adv7281_ma_info },
+	{ "adv7282", (kernel_ulong_t)&adv7282_info },
+	{ "adv7282-m", (kernel_ulong_t)&adv7282_m_info },
+	{},
+};
+MODULE_DEVICE_TABLE(i2c, adv7180_id);
+
+#ifdef CONFIG_PM_SLEEP
+static int adv7180_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct adv7180_state *state = to_state(sd);
+
+	return adv7180_set_power(state, false);
+}
+
+static int adv7180_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct adv7180_state *state = to_state(sd);
+	int ret;
+
+	ret = init_device(state);
+	if (ret < 0)
+		return ret;
+
+	ret = adv7180_set_power(state, state->powered);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(adv7180_pm_ops, adv7180_suspend, adv7180_resume);
+#define ADV7180_PM_OPS (&adv7180_pm_ops)
+
+#else
+#define ADV7180_PM_OPS NULL
+#endif
+
+#ifdef CONFIG_OF
+static const struct of_device_id adv7180_of_id[] = {
+	{ .compatible = "adi,adv7180", },
+	{ .compatible = "adi,adv7180cp", },
+	{ .compatible = "adi,adv7180st", },
+	{ .compatible = "adi,adv7182", },
+	{ .compatible = "adi,adv7280", },
+	{ .compatible = "adi,adv7280-m", },
+	{ .compatible = "adi,adv7281", },
+	{ .compatible = "adi,adv7281-m", },
+	{ .compatible = "adi,adv7281-ma", },
+	{ .compatible = "adi,adv7282", },
+	{ .compatible = "adi,adv7282-m", },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(of, adv7180_of_id);
+#endif
+
+static struct i2c_driver adv7180_driver = {
+	.driver = {
+		   .name = KBUILD_MODNAME,
+		   .pm = ADV7180_PM_OPS,
+		   .of_match_table = of_match_ptr(adv7180_of_id),
+		   },
+	.probe = adv7180_probe,
+	.remove = adv7180_remove,
+	.id_table = adv7180_id,
+};
+
+module_i2c_driver(adv7180_driver);
+
+MODULE_DESCRIPTION("Analog Devices ADV7180 video decoder driver");
+MODULE_AUTHOR("Mocean Laboratories");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/adv7183.c b/drivers/media/i2c/adv7183.c
new file mode 100644
index 000000000..3659feafa
--- /dev/null
+++ b/drivers/media/i2c/adv7183.c
@@ -0,0 +1,643 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * adv7183.c Analog Devices ADV7183 video decoder driver
+ *
+ * Copyright (c) 2011 Analog Devices Inc.
+ */
+
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <linux/videodev2.h>
+
+#include <media/i2c/adv7183.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+
+#include "adv7183_regs.h"
+
+struct adv7183 {
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
+
+	v4l2_std_id std; /* Current set standard */
+	u32 input;
+	u32 output;
+	struct gpio_desc *reset_pin;
+	struct gpio_desc *oe_pin;
+	struct v4l2_mbus_framefmt fmt;
+};
+
+/* EXAMPLES USING 27 MHz CLOCK
+ * Mode 1 CVBS Input (Composite Video on AIN5)
+ * All standards are supported through autodetect, 8-bit, 4:2:2, ITU-R BT.656 output on P15 to P8.
+ */
+static const unsigned char adv7183_init_regs[] = {
+	ADV7183_IN_CTRL, 0x04,           /* CVBS input on AIN5 */
+	ADV7183_DIGI_CLAMP_CTRL_1, 0x00, /* Slow down digital clamps */
+	ADV7183_SHAP_FILT_CTRL, 0x41,    /* Set CSFM to SH1 */
+	ADV7183_ADC_CTRL, 0x16,          /* Power down ADC 1 and ADC 2 */
+	ADV7183_CTI_DNR_CTRL_4, 0x04,    /* Set DNR threshold to 4 for flat response */
+	/* ADI recommended programming sequence */
+	ADV7183_ADI_CTRL, 0x80,
+	ADV7183_CTI_DNR_CTRL_4, 0x20,
+	0x52, 0x18,
+	0x58, 0xED,
+	0x77, 0xC5,
+	0x7C, 0x93,
+	0x7D, 0x00,
+	0xD0, 0x48,
+	0xD5, 0xA0,
+	0xD7, 0xEA,
+	ADV7183_SD_SATURATION_CR, 0x3E,
+	ADV7183_PAL_V_END, 0x3E,
+	ADV7183_PAL_F_TOGGLE, 0x0F,
+	ADV7183_ADI_CTRL, 0x00,
+};
+
+static inline struct adv7183 *to_adv7183(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct adv7183, sd);
+}
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct adv7183, hdl)->sd;
+}
+
+static inline int adv7183_read(struct v4l2_subdev *sd, unsigned char reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return i2c_smbus_read_byte_data(client, reg);
+}
+
+static inline int adv7183_write(struct v4l2_subdev *sd, unsigned char reg,
+				unsigned char value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return i2c_smbus_write_byte_data(client, reg, value);
+}
+
+static int adv7183_writeregs(struct v4l2_subdev *sd,
+		const unsigned char *regs, unsigned int num)
+{
+	unsigned char reg, data;
+	unsigned int cnt = 0;
+
+	if (num & 0x1) {
+		v4l2_err(sd, "invalid regs array\n");
+		return -1;
+	}
+
+	while (cnt < num) {
+		reg = *regs++;
+		data = *regs++;
+		cnt += 2;
+
+		adv7183_write(sd, reg, data);
+	}
+	return 0;
+}
+
+static int adv7183_log_status(struct v4l2_subdev *sd)
+{
+	struct adv7183 *decoder = to_adv7183(sd);
+
+	v4l2_info(sd, "adv7183: Input control = 0x%02x\n",
+			adv7183_read(sd, ADV7183_IN_CTRL));
+	v4l2_info(sd, "adv7183: Video selection = 0x%02x\n",
+			adv7183_read(sd, ADV7183_VD_SEL));
+	v4l2_info(sd, "adv7183: Output control = 0x%02x\n",
+			adv7183_read(sd, ADV7183_OUT_CTRL));
+	v4l2_info(sd, "adv7183: Extended output control = 0x%02x\n",
+			adv7183_read(sd, ADV7183_EXT_OUT_CTRL));
+	v4l2_info(sd, "adv7183: Autodetect enable = 0x%02x\n",
+			adv7183_read(sd, ADV7183_AUTO_DET_EN));
+	v4l2_info(sd, "adv7183: Contrast = 0x%02x\n",
+			adv7183_read(sd, ADV7183_CONTRAST));
+	v4l2_info(sd, "adv7183: Brightness = 0x%02x\n",
+			adv7183_read(sd, ADV7183_BRIGHTNESS));
+	v4l2_info(sd, "adv7183: Hue = 0x%02x\n",
+			adv7183_read(sd, ADV7183_HUE));
+	v4l2_info(sd, "adv7183: Default value Y = 0x%02x\n",
+			adv7183_read(sd, ADV7183_DEF_Y));
+	v4l2_info(sd, "adv7183: Default value C = 0x%02x\n",
+			adv7183_read(sd, ADV7183_DEF_C));
+	v4l2_info(sd, "adv7183: ADI control = 0x%02x\n",
+			adv7183_read(sd, ADV7183_ADI_CTRL));
+	v4l2_info(sd, "adv7183: Power Management = 0x%02x\n",
+			adv7183_read(sd, ADV7183_POW_MANAGE));
+	v4l2_info(sd, "adv7183: Status 1 2 and 3 = 0x%02x 0x%02x 0x%02x\n",
+			adv7183_read(sd, ADV7183_STATUS_1),
+			adv7183_read(sd, ADV7183_STATUS_2),
+			adv7183_read(sd, ADV7183_STATUS_3));
+	v4l2_info(sd, "adv7183: Ident = 0x%02x\n",
+			adv7183_read(sd, ADV7183_IDENT));
+	v4l2_info(sd, "adv7183: Analog clamp control = 0x%02x\n",
+			adv7183_read(sd, ADV7183_ANAL_CLAMP_CTRL));
+	v4l2_info(sd, "adv7183: Digital clamp control 1 = 0x%02x\n",
+			adv7183_read(sd, ADV7183_DIGI_CLAMP_CTRL_1));
+	v4l2_info(sd, "adv7183: Shaping filter control 1 and 2 = 0x%02x 0x%02x\n",
+			adv7183_read(sd, ADV7183_SHAP_FILT_CTRL),
+			adv7183_read(sd, ADV7183_SHAP_FILT_CTRL_2));
+	v4l2_info(sd, "adv7183: Comb filter control = 0x%02x\n",
+			adv7183_read(sd, ADV7183_COMB_FILT_CTRL));
+	v4l2_info(sd, "adv7183: ADI control 2 = 0x%02x\n",
+			adv7183_read(sd, ADV7183_ADI_CTRL_2));
+	v4l2_info(sd, "adv7183: Pixel delay control = 0x%02x\n",
+			adv7183_read(sd, ADV7183_PIX_DELAY_CTRL));
+	v4l2_info(sd, "adv7183: Misc gain control = 0x%02x\n",
+			adv7183_read(sd, ADV7183_MISC_GAIN_CTRL));
+	v4l2_info(sd, "adv7183: AGC mode control = 0x%02x\n",
+			adv7183_read(sd, ADV7183_AGC_MODE_CTRL));
+	v4l2_info(sd, "adv7183: Chroma gain control 1 and 2 = 0x%02x 0x%02x\n",
+			adv7183_read(sd, ADV7183_CHRO_GAIN_CTRL_1),
+			adv7183_read(sd, ADV7183_CHRO_GAIN_CTRL_2));
+	v4l2_info(sd, "adv7183: Luma gain control 1 and 2 = 0x%02x 0x%02x\n",
+			adv7183_read(sd, ADV7183_LUMA_GAIN_CTRL_1),
+			adv7183_read(sd, ADV7183_LUMA_GAIN_CTRL_2));
+	v4l2_info(sd, "adv7183: Vsync field control 1 2 and 3 = 0x%02x 0x%02x 0x%02x\n",
+			adv7183_read(sd, ADV7183_VS_FIELD_CTRL_1),
+			adv7183_read(sd, ADV7183_VS_FIELD_CTRL_2),
+			adv7183_read(sd, ADV7183_VS_FIELD_CTRL_3));
+	v4l2_info(sd, "adv7183: Hsync position control 1 2 and 3 = 0x%02x 0x%02x 0x%02x\n",
+			adv7183_read(sd, ADV7183_HS_POS_CTRL_1),
+			adv7183_read(sd, ADV7183_HS_POS_CTRL_2),
+			adv7183_read(sd, ADV7183_HS_POS_CTRL_3));
+	v4l2_info(sd, "adv7183: Polarity = 0x%02x\n",
+			adv7183_read(sd, ADV7183_POLARITY));
+	v4l2_info(sd, "adv7183: ADC control = 0x%02x\n",
+			adv7183_read(sd, ADV7183_ADC_CTRL));
+	v4l2_info(sd, "adv7183: SD offset Cb and Cr = 0x%02x 0x%02x\n",
+			adv7183_read(sd, ADV7183_SD_OFFSET_CB),
+			adv7183_read(sd, ADV7183_SD_OFFSET_CR));
+	v4l2_info(sd, "adv7183: SD saturation Cb and Cr = 0x%02x 0x%02x\n",
+			adv7183_read(sd, ADV7183_SD_SATURATION_CB),
+			adv7183_read(sd, ADV7183_SD_SATURATION_CR));
+	v4l2_info(sd, "adv7183: Drive strength = 0x%02x\n",
+			adv7183_read(sd, ADV7183_DRIVE_STR));
+	v4l2_ctrl_handler_log_status(&decoder->hdl, sd->name);
+	return 0;
+}
+
+static int adv7183_g_std(struct v4l2_subdev *sd, v4l2_std_id *std)
+{
+	struct adv7183 *decoder = to_adv7183(sd);
+
+	*std = decoder->std;
+	return 0;
+}
+
+static int adv7183_s_std(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct adv7183 *decoder = to_adv7183(sd);
+	int reg;
+
+	reg = adv7183_read(sd, ADV7183_IN_CTRL) & 0xF;
+	if (std == V4L2_STD_PAL_60)
+		reg |= 0x60;
+	else if (std == V4L2_STD_NTSC_443)
+		reg |= 0x70;
+	else if (std == V4L2_STD_PAL_N)
+		reg |= 0x90;
+	else if (std == V4L2_STD_PAL_M)
+		reg |= 0xA0;
+	else if (std == V4L2_STD_PAL_Nc)
+		reg |= 0xC0;
+	else if (std & V4L2_STD_PAL)
+		reg |= 0x80;
+	else if (std & V4L2_STD_NTSC)
+		reg |= 0x50;
+	else if (std & V4L2_STD_SECAM)
+		reg |= 0xE0;
+	else
+		return -EINVAL;
+	adv7183_write(sd, ADV7183_IN_CTRL, reg);
+
+	decoder->std = std;
+
+	return 0;
+}
+
+static int adv7183_reset(struct v4l2_subdev *sd, u32 val)
+{
+	int reg;
+
+	reg = adv7183_read(sd, ADV7183_POW_MANAGE) | 0x80;
+	adv7183_write(sd, ADV7183_POW_MANAGE, reg);
+	/* wait 5ms before any further i2c writes are performed */
+	usleep_range(5000, 10000);
+	return 0;
+}
+
+static int adv7183_s_routing(struct v4l2_subdev *sd,
+				u32 input, u32 output, u32 config)
+{
+	struct adv7183 *decoder = to_adv7183(sd);
+	int reg;
+
+	if ((input > ADV7183_COMPONENT1) || (output > ADV7183_16BIT_OUT))
+		return -EINVAL;
+
+	if (input != decoder->input) {
+		decoder->input = input;
+		reg = adv7183_read(sd, ADV7183_IN_CTRL) & 0xF0;
+		switch (input) {
+		case ADV7183_COMPOSITE1:
+			reg |= 0x1;
+			break;
+		case ADV7183_COMPOSITE2:
+			reg |= 0x2;
+			break;
+		case ADV7183_COMPOSITE3:
+			reg |= 0x3;
+			break;
+		case ADV7183_COMPOSITE4:
+			reg |= 0x4;
+			break;
+		case ADV7183_COMPOSITE5:
+			reg |= 0x5;
+			break;
+		case ADV7183_COMPOSITE6:
+			reg |= 0xB;
+			break;
+		case ADV7183_COMPOSITE7:
+			reg |= 0xC;
+			break;
+		case ADV7183_COMPOSITE8:
+			reg |= 0xD;
+			break;
+		case ADV7183_COMPOSITE9:
+			reg |= 0xE;
+			break;
+		case ADV7183_COMPOSITE10:
+			reg |= 0xF;
+			break;
+		case ADV7183_SVIDEO0:
+			reg |= 0x6;
+			break;
+		case ADV7183_SVIDEO1:
+			reg |= 0x7;
+			break;
+		case ADV7183_SVIDEO2:
+			reg |= 0x8;
+			break;
+		case ADV7183_COMPONENT0:
+			reg |= 0x9;
+			break;
+		case ADV7183_COMPONENT1:
+			reg |= 0xA;
+			break;
+		default:
+			break;
+		}
+		adv7183_write(sd, ADV7183_IN_CTRL, reg);
+	}
+
+	if (output != decoder->output) {
+		decoder->output = output;
+		reg = adv7183_read(sd, ADV7183_OUT_CTRL) & 0xC0;
+		switch (output) {
+		case ADV7183_16BIT_OUT:
+			reg |= 0x9;
+			break;
+		default:
+			reg |= 0xC;
+			break;
+		}
+		adv7183_write(sd, ADV7183_OUT_CTRL, reg);
+	}
+
+	return 0;
+}
+
+static int adv7183_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+	int val = ctrl->val;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		if (val < 0)
+			val = 127 - val;
+		adv7183_write(sd, ADV7183_BRIGHTNESS, val);
+		break;
+	case V4L2_CID_CONTRAST:
+		adv7183_write(sd, ADV7183_CONTRAST, val);
+		break;
+	case V4L2_CID_SATURATION:
+		adv7183_write(sd, ADV7183_SD_SATURATION_CB, val >> 8);
+		adv7183_write(sd, ADV7183_SD_SATURATION_CR, (val & 0xFF));
+		break;
+	case V4L2_CID_HUE:
+		adv7183_write(sd, ADV7183_SD_OFFSET_CB, val >> 8);
+		adv7183_write(sd, ADV7183_SD_OFFSET_CR, (val & 0xFF));
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int adv7183_querystd(struct v4l2_subdev *sd, v4l2_std_id *std)
+{
+	struct adv7183 *decoder = to_adv7183(sd);
+	int reg;
+
+	/* enable autodetection block */
+	reg = adv7183_read(sd, ADV7183_IN_CTRL) & 0xF;
+	adv7183_write(sd, ADV7183_IN_CTRL, reg);
+
+	/* wait autodetection switch */
+	mdelay(10);
+
+	/* get autodetection result */
+	reg = adv7183_read(sd, ADV7183_STATUS_1);
+	switch ((reg >> 0x4) & 0x7) {
+	case 0:
+		*std &= V4L2_STD_NTSC;
+		break;
+	case 1:
+		*std &= V4L2_STD_NTSC_443;
+		break;
+	case 2:
+		*std &= V4L2_STD_PAL_M;
+		break;
+	case 3:
+		*std &= V4L2_STD_PAL_60;
+		break;
+	case 4:
+		*std &= V4L2_STD_PAL;
+		break;
+	case 5:
+		*std &= V4L2_STD_SECAM;
+		break;
+	case 6:
+		*std &= V4L2_STD_PAL_Nc;
+		break;
+	case 7:
+		*std &= V4L2_STD_SECAM;
+		break;
+	default:
+		*std = V4L2_STD_UNKNOWN;
+		break;
+	}
+
+	/* after std detection, write back user set std */
+	adv7183_s_std(sd, decoder->std);
+	return 0;
+}
+
+static int adv7183_g_input_status(struct v4l2_subdev *sd, u32 *status)
+{
+	int reg;
+
+	*status = V4L2_IN_ST_NO_SIGNAL;
+	reg = adv7183_read(sd, ADV7183_STATUS_1);
+	if (reg < 0)
+		return reg;
+	if (reg & 0x1)
+		*status = 0;
+	return 0;
+}
+
+static int adv7183_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->pad || code->index > 0)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_UYVY8_2X8;
+	return 0;
+}
+
+static int adv7183_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_format *format)
+{
+	struct adv7183 *decoder = to_adv7183(sd);
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+
+	if (format->pad)
+		return -EINVAL;
+
+	fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
+	fmt->colorspace = V4L2_COLORSPACE_SMPTE170M;
+	if (decoder->std & V4L2_STD_525_60) {
+		fmt->field = V4L2_FIELD_SEQ_TB;
+		fmt->width = 720;
+		fmt->height = 480;
+	} else {
+		fmt->field = V4L2_FIELD_SEQ_BT;
+		fmt->width = 720;
+		fmt->height = 576;
+	}
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		decoder->fmt = *fmt;
+	else
+		sd_state->pads->try_fmt = *fmt;
+	return 0;
+}
+
+static int adv7183_get_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_format *format)
+{
+	struct adv7183 *decoder = to_adv7183(sd);
+
+	if (format->pad)
+		return -EINVAL;
+
+	format->format = decoder->fmt;
+	return 0;
+}
+
+static int adv7183_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct adv7183 *decoder = to_adv7183(sd);
+
+	if (enable)
+		gpiod_set_value(decoder->oe_pin, 1);
+	else
+		gpiod_set_value(decoder->oe_pin, 0);
+	udelay(1);
+	return 0;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int adv7183_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+{
+	reg->val = adv7183_read(sd, reg->reg & 0xff);
+	reg->size = 1;
+	return 0;
+}
+
+static int adv7183_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
+{
+	adv7183_write(sd, reg->reg & 0xff, reg->val & 0xff);
+	return 0;
+}
+#endif
+
+static const struct v4l2_ctrl_ops adv7183_ctrl_ops = {
+	.s_ctrl = adv7183_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops adv7183_core_ops = {
+	.log_status = adv7183_log_status,
+	.reset = adv7183_reset,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = adv7183_g_register,
+	.s_register = adv7183_s_register,
+#endif
+};
+
+static const struct v4l2_subdev_video_ops adv7183_video_ops = {
+	.g_std = adv7183_g_std,
+	.s_std = adv7183_s_std,
+	.s_routing = adv7183_s_routing,
+	.querystd = adv7183_querystd,
+	.g_input_status = adv7183_g_input_status,
+	.s_stream = adv7183_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops adv7183_pad_ops = {
+	.enum_mbus_code = adv7183_enum_mbus_code,
+	.get_fmt = adv7183_get_fmt,
+	.set_fmt = adv7183_set_fmt,
+};
+
+static const struct v4l2_subdev_ops adv7183_ops = {
+	.core = &adv7183_core_ops,
+	.video = &adv7183_video_ops,
+	.pad = &adv7183_pad_ops,
+};
+
+static int adv7183_probe(struct i2c_client *client)
+{
+	struct adv7183 *decoder;
+	struct v4l2_subdev *sd;
+	struct v4l2_ctrl_handler *hdl;
+	int ret;
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+
+	v4l_info(client, "chip found @ 0x%02x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	decoder = devm_kzalloc(&client->dev, sizeof(*decoder), GFP_KERNEL);
+	if (decoder == NULL)
+		return -ENOMEM;
+
+	/*
+	 * Requesting high will assert reset, the line should be
+	 * flagged as active low in descriptor table or machine description.
+	 */
+	decoder->reset_pin = devm_gpiod_get(&client->dev, "reset",
+					    GPIOD_OUT_HIGH);
+	if (IS_ERR(decoder->reset_pin))
+		return PTR_ERR(decoder->reset_pin);
+	gpiod_set_consumer_name(decoder->reset_pin, "ADV7183 Reset");
+	/*
+	 * Requesting low will start with output disabled, the line should be
+	 * flagged as active low in descriptor table or machine description.
+	 */
+	decoder->oe_pin = devm_gpiod_get(&client->dev, "oe",
+					 GPIOD_OUT_LOW);
+	if (IS_ERR(decoder->oe_pin))
+		return PTR_ERR(decoder->oe_pin);
+	gpiod_set_consumer_name(decoder->reset_pin, "ADV7183 Output Enable");
+
+	sd = &decoder->sd;
+	v4l2_i2c_subdev_init(sd, client, &adv7183_ops);
+
+	hdl = &decoder->hdl;
+	v4l2_ctrl_handler_init(hdl, 4);
+	v4l2_ctrl_new_std(hdl, &adv7183_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, -128, 127, 1, 0);
+	v4l2_ctrl_new_std(hdl, &adv7183_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 0xFF, 1, 0x80);
+	v4l2_ctrl_new_std(hdl, &adv7183_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 0xFFFF, 1, 0x8080);
+	v4l2_ctrl_new_std(hdl, &adv7183_ctrl_ops,
+			V4L2_CID_HUE, 0, 0xFFFF, 1, 0x8080);
+	/* hook the control handler into the driver */
+	sd->ctrl_handler = hdl;
+	if (hdl->error) {
+		ret = hdl->error;
+
+		v4l2_ctrl_handler_free(hdl);
+		return ret;
+	}
+
+	/* v4l2 doesn't support an autodetect standard, pick PAL as default */
+	decoder->std = V4L2_STD_PAL;
+	decoder->input = ADV7183_COMPOSITE4;
+	decoder->output = ADV7183_8BIT_OUT;
+
+	/* reset chip */
+	/* reset pulse width at least 5ms */
+	mdelay(10);
+	/* De-assert reset line (descriptor tagged active low) */
+	gpiod_set_value(decoder->reset_pin, 0);
+	/* wait 5ms before any further i2c writes are performed */
+	mdelay(5);
+
+	adv7183_writeregs(sd, adv7183_init_regs, ARRAY_SIZE(adv7183_init_regs));
+	adv7183_s_std(sd, decoder->std);
+	fmt.format.width = 720;
+	fmt.format.height = 576;
+	adv7183_set_fmt(sd, NULL, &fmt);
+
+	/* initialize the hardware to the default control values */
+	ret = v4l2_ctrl_handler_setup(hdl);
+	if (ret) {
+		v4l2_ctrl_handler_free(hdl);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void adv7183_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+}
+
+static const struct i2c_device_id adv7183_id[] = {
+	{"adv7183", 0},
+	{},
+};
+
+MODULE_DEVICE_TABLE(i2c, adv7183_id);
+
+static struct i2c_driver adv7183_driver = {
+	.driver = {
+		.name   = "adv7183",
+	},
+	.probe          = adv7183_probe,
+	.remove         = adv7183_remove,
+	.id_table       = adv7183_id,
+};
+
+module_i2c_driver(adv7183_driver);
+
+MODULE_DESCRIPTION("Analog Devices ADV7183 video decoder driver");
+MODULE_AUTHOR("Scott Jiang <Scott.Jiang.Linux@gmail.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/adv7183_regs.h b/drivers/media/i2c/adv7183_regs.h
new file mode 100644
index 000000000..d241efe73
--- /dev/null
+++ b/drivers/media/i2c/adv7183_regs.h
@@ -0,0 +1,95 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * adv7183 - Analog Devices ADV7183 video decoder registers
+ *
+ * Copyright (c) 2011 Analog Devices Inc.
+ */
+
+#ifndef _ADV7183_REGS_H_
+#define _ADV7183_REGS_H_
+
+#define ADV7183_IN_CTRL            0x00 /* Input control */
+#define ADV7183_VD_SEL             0x01 /* Video selection */
+#define ADV7183_OUT_CTRL           0x03 /* Output control */
+#define ADV7183_EXT_OUT_CTRL       0x04 /* Extended output control */
+#define ADV7183_AUTO_DET_EN        0x07 /* Autodetect enable */
+#define ADV7183_CONTRAST           0x08 /* Contrast */
+#define ADV7183_BRIGHTNESS         0x0A /* Brightness */
+#define ADV7183_HUE                0x0B /* Hue */
+#define ADV7183_DEF_Y              0x0C /* Default value Y */
+#define ADV7183_DEF_C              0x0D /* Default value C */
+#define ADV7183_ADI_CTRL           0x0E /* ADI control */
+#define ADV7183_POW_MANAGE         0x0F /* Power Management */
+#define ADV7183_STATUS_1           0x10 /* Status 1 */
+#define ADV7183_IDENT              0x11 /* Ident */
+#define ADV7183_STATUS_2           0x12 /* Status 2 */
+#define ADV7183_STATUS_3           0x13 /* Status 3 */
+#define ADV7183_ANAL_CLAMP_CTRL    0x14 /* Analog clamp control */
+#define ADV7183_DIGI_CLAMP_CTRL_1  0x15 /* Digital clamp control 1 */
+#define ADV7183_SHAP_FILT_CTRL     0x17 /* Shaping filter control */
+#define ADV7183_SHAP_FILT_CTRL_2   0x18 /* Shaping filter control 2 */
+#define ADV7183_COMB_FILT_CTRL     0x19 /* Comb filter control */
+#define ADV7183_ADI_CTRL_2         0x1D /* ADI control 2 */
+#define ADV7183_PIX_DELAY_CTRL     0x27 /* Pixel delay control */
+#define ADV7183_MISC_GAIN_CTRL     0x2B /* Misc gain control */
+#define ADV7183_AGC_MODE_CTRL      0x2C /* AGC mode control */
+#define ADV7183_CHRO_GAIN_CTRL_1   0x2D /* Chroma gain control 1 */
+#define ADV7183_CHRO_GAIN_CTRL_2   0x2E /* Chroma gain control 2 */
+#define ADV7183_LUMA_GAIN_CTRL_1   0x2F /* Luma gain control 1 */
+#define ADV7183_LUMA_GAIN_CTRL_2   0x30 /* Luma gain control 2 */
+#define ADV7183_VS_FIELD_CTRL_1    0x31 /* Vsync field control 1 */
+#define ADV7183_VS_FIELD_CTRL_2    0x32 /* Vsync field control 2 */
+#define ADV7183_VS_FIELD_CTRL_3    0x33 /* Vsync field control 3 */
+#define ADV7183_HS_POS_CTRL_1      0x34 /* Hsync position control 1 */
+#define ADV7183_HS_POS_CTRL_2      0x35 /* Hsync position control 2 */
+#define ADV7183_HS_POS_CTRL_3      0x36 /* Hsync position control 3 */
+#define ADV7183_POLARITY           0x37 /* Polarity */
+#define ADV7183_NTSC_COMB_CTRL     0x38 /* NTSC comb control */
+#define ADV7183_PAL_COMB_CTRL      0x39 /* PAL comb control */
+#define ADV7183_ADC_CTRL           0x3A /* ADC control */
+#define ADV7183_MAN_WIN_CTRL       0x3D /* Manual window control */
+#define ADV7183_RESAMPLE_CTRL      0x41 /* Resample control */
+#define ADV7183_GEMSTAR_CTRL_1     0x48 /* Gemstar ctrl 1 */
+#define ADV7183_GEMSTAR_CTRL_2     0x49 /* Gemstar ctrl 2 */
+#define ADV7183_GEMSTAR_CTRL_3     0x4A /* Gemstar ctrl 3 */
+#define ADV7183_GEMSTAR_CTRL_4     0x4B /* Gemstar ctrl 4 */
+#define ADV7183_GEMSTAR_CTRL_5     0x4C /* Gemstar ctrl 5 */
+#define ADV7183_CTI_DNR_CTRL_1     0x4D /* CTI DNR ctrl 1 */
+#define ADV7183_CTI_DNR_CTRL_2     0x4E /* CTI DNR ctrl 2 */
+#define ADV7183_CTI_DNR_CTRL_4     0x50 /* CTI DNR ctrl 4 */
+#define ADV7183_LOCK_CNT           0x51 /* Lock count */
+#define ADV7183_FREE_LINE_LEN      0x8F /* Free-Run line length 1 */
+#define ADV7183_VBI_INFO           0x90 /* VBI info */
+#define ADV7183_WSS_1              0x91 /* WSS 1 */
+#define ADV7183_WSS_2              0x92 /* WSS 2 */
+#define ADV7183_EDTV_1             0x93 /* EDTV 1 */
+#define ADV7183_EDTV_2             0x94 /* EDTV 2 */
+#define ADV7183_EDTV_3             0x95 /* EDTV 3 */
+#define ADV7183_CGMS_1             0x96 /* CGMS 1 */
+#define ADV7183_CGMS_2             0x97 /* CGMS 2 */
+#define ADV7183_CGMS_3             0x98 /* CGMS 3 */
+#define ADV7183_CCAP_1             0x99 /* CCAP 1 */
+#define ADV7183_CCAP_2             0x9A /* CCAP 2 */
+#define ADV7183_LETTERBOX_1        0x9B /* Letterbox 1 */
+#define ADV7183_LETTERBOX_2        0x9C /* Letterbox 2 */
+#define ADV7183_LETTERBOX_3        0x9D /* Letterbox 3 */
+#define ADV7183_CRC_EN             0xB2 /* CRC enable */
+#define ADV7183_ADC_SWITCH_1       0xC3 /* ADC switch 1 */
+#define ADV7183_ADC_SWITCH_2       0xC4 /* ADC switch 2 */
+#define ADV7183_LETTERBOX_CTRL_1   0xDC /* Letterbox control 1 */
+#define ADV7183_LETTERBOX_CTRL_2   0xDD /* Letterbox control 2 */
+#define ADV7183_SD_OFFSET_CB       0xE1 /* SD offset Cb */
+#define ADV7183_SD_OFFSET_CR       0xE2 /* SD offset Cr */
+#define ADV7183_SD_SATURATION_CB   0xE3 /* SD saturation Cb */
+#define ADV7183_SD_SATURATION_CR   0xE4 /* SD saturation Cr */
+#define ADV7183_NTSC_V_BEGIN       0xE5 /* NTSC V bit begin */
+#define ADV7183_NTSC_V_END         0xE6 /* NTSC V bit end */
+#define ADV7183_NTSC_F_TOGGLE      0xE7 /* NTSC F bit toggle */
+#define ADV7183_PAL_V_BEGIN        0xE8 /* PAL V bit begin */
+#define ADV7183_PAL_V_END          0xE9 /* PAL V bit end */
+#define ADV7183_PAL_F_TOGGLE       0xEA /* PAL F bit toggle */
+#define ADV7183_DRIVE_STR          0xF4 /* Drive strength */
+#define ADV7183_IF_COMP_CTRL       0xF8 /* IF comp control */
+#define ADV7183_VS_MODE_CTRL       0xF9 /* VS mode control */
+
+#endif
diff --git a/drivers/media/i2c/adv7343.c b/drivers/media/i2c/adv7343.c
new file mode 100644
index 000000000..ff21cd474
--- /dev/null
+++ b/drivers/media/i2c/adv7343.c
@@ -0,0 +1,529 @@
+/*
+ * adv7343 - ADV7343 Video Encoder Driver
+ *
+ * The encoder hardware does not support SECAM.
+ *
+ * Copyright (C) 2009 Texas Instruments Incorporated - http://www.ti.com/
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed .as is. WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/ctype.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/module.h>
+#include <linux/videodev2.h>
+#include <linux/uaccess.h>
+#include <linux/of.h>
+#include <linux/of_graph.h>
+
+#include <media/i2c/adv7343.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+
+#include "adv7343_regs.h"
+
+MODULE_DESCRIPTION("ADV7343 video encoder driver");
+MODULE_LICENSE("GPL");
+
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "Debug level 0-1");
+
+struct adv7343_state {
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
+	const struct adv7343_platform_data *pdata;
+	u8 reg00;
+	u8 reg01;
+	u8 reg02;
+	u8 reg35;
+	u8 reg80;
+	u8 reg82;
+	u32 output;
+	v4l2_std_id std;
+};
+
+static inline struct adv7343_state *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct adv7343_state, sd);
+}
+
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct adv7343_state, hdl)->sd;
+}
+
+static inline int adv7343_write(struct v4l2_subdev *sd, u8 reg, u8 value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return i2c_smbus_write_byte_data(client, reg, value);
+}
+
+static const u8 adv7343_init_reg_val[] = {
+	ADV7343_SOFT_RESET, ADV7343_SOFT_RESET_DEFAULT,
+	ADV7343_POWER_MODE_REG, ADV7343_POWER_MODE_REG_DEFAULT,
+
+	ADV7343_HD_MODE_REG1, ADV7343_HD_MODE_REG1_DEFAULT,
+	ADV7343_HD_MODE_REG2, ADV7343_HD_MODE_REG2_DEFAULT,
+	ADV7343_HD_MODE_REG3, ADV7343_HD_MODE_REG3_DEFAULT,
+	ADV7343_HD_MODE_REG4, ADV7343_HD_MODE_REG4_DEFAULT,
+	ADV7343_HD_MODE_REG5, ADV7343_HD_MODE_REG5_DEFAULT,
+	ADV7343_HD_MODE_REG6, ADV7343_HD_MODE_REG6_DEFAULT,
+	ADV7343_HD_MODE_REG7, ADV7343_HD_MODE_REG7_DEFAULT,
+
+	ADV7343_SD_MODE_REG1, ADV7343_SD_MODE_REG1_DEFAULT,
+	ADV7343_SD_MODE_REG2, ADV7343_SD_MODE_REG2_DEFAULT,
+	ADV7343_SD_MODE_REG3, ADV7343_SD_MODE_REG3_DEFAULT,
+	ADV7343_SD_MODE_REG4, ADV7343_SD_MODE_REG4_DEFAULT,
+	ADV7343_SD_MODE_REG5, ADV7343_SD_MODE_REG5_DEFAULT,
+	ADV7343_SD_MODE_REG6, ADV7343_SD_MODE_REG6_DEFAULT,
+	ADV7343_SD_MODE_REG7, ADV7343_SD_MODE_REG7_DEFAULT,
+	ADV7343_SD_MODE_REG8, ADV7343_SD_MODE_REG8_DEFAULT,
+
+	ADV7343_SD_HUE_REG, ADV7343_SD_HUE_REG_DEFAULT,
+	ADV7343_SD_CGMS_WSS0, ADV7343_SD_CGMS_WSS0_DEFAULT,
+	ADV7343_SD_BRIGHTNESS_WSS, ADV7343_SD_BRIGHTNESS_WSS_DEFAULT,
+};
+
+/*
+ *			    2^32
+ * FSC(reg) =  FSC (HZ) * --------
+ *			  27000000
+ */
+static const struct adv7343_std_info stdinfo[] = {
+	{
+		/* FSC(Hz) = 3,579,545.45 Hz */
+		SD_STD_NTSC, 569408542, V4L2_STD_NTSC,
+	}, {
+		/* FSC(Hz) = 3,575,611.00 Hz */
+		SD_STD_PAL_M, 568782678, V4L2_STD_PAL_M,
+	}, {
+		/* FSC(Hz) = 3,582,056.00 */
+		SD_STD_PAL_N, 569807903, V4L2_STD_PAL_Nc,
+	}, {
+		/* FSC(Hz) = 4,433,618.75 Hz */
+		SD_STD_PAL_N, 705268427, V4L2_STD_PAL_N,
+	}, {
+		/* FSC(Hz) = 4,433,618.75 Hz */
+		SD_STD_PAL_BDGHI, 705268427, V4L2_STD_PAL,
+	}, {
+		/* FSC(Hz) = 4,433,618.75 Hz */
+		SD_STD_NTSC, 705268427, V4L2_STD_NTSC_443,
+	}, {
+		/* FSC(Hz) = 4,433,618.75 Hz */
+		SD_STD_PAL_M, 705268427, V4L2_STD_PAL_60,
+	},
+};
+
+static int adv7343_setstd(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct adv7343_state *state = to_state(sd);
+	struct adv7343_std_info *std_info;
+	int num_std;
+	char *fsc_ptr;
+	u8 reg, val;
+	int err = 0;
+	int i = 0;
+
+	std_info = (struct adv7343_std_info *)stdinfo;
+	num_std = ARRAY_SIZE(stdinfo);
+
+	for (i = 0; i < num_std; i++) {
+		if (std_info[i].stdid & std)
+			break;
+	}
+
+	if (i == num_std) {
+		v4l2_dbg(1, debug, sd,
+				"Invalid std or std is not supported: %llx\n",
+						(unsigned long long)std);
+		return -EINVAL;
+	}
+
+	/* Set the standard */
+	val = state->reg80 & (~(SD_STD_MASK));
+	val |= std_info[i].standard_val3;
+	err = adv7343_write(sd, ADV7343_SD_MODE_REG1, val);
+	if (err < 0)
+		goto setstd_exit;
+
+	state->reg80 = val;
+
+	/* Configure the input mode register */
+	val = state->reg01 & (~((u8) INPUT_MODE_MASK));
+	val |= SD_INPUT_MODE;
+	err = adv7343_write(sd, ADV7343_MODE_SELECT_REG, val);
+	if (err < 0)
+		goto setstd_exit;
+
+	state->reg01 = val;
+
+	/* Program the sub carrier frequency registers */
+	fsc_ptr = (unsigned char *)&std_info[i].fsc_val;
+	reg = ADV7343_FSC_REG0;
+	for (i = 0; i < 4; i++, reg++, fsc_ptr++) {
+		err = adv7343_write(sd, reg, *fsc_ptr);
+		if (err < 0)
+			goto setstd_exit;
+	}
+
+	val = state->reg80;
+
+	/* Filter settings */
+	if (std & (V4L2_STD_NTSC | V4L2_STD_NTSC_443))
+		val &= 0x03;
+	else if (std & ~V4L2_STD_SECAM)
+		val |= 0x04;
+
+	err = adv7343_write(sd, ADV7343_SD_MODE_REG1, val);
+	if (err < 0)
+		goto setstd_exit;
+
+	state->reg80 = val;
+
+setstd_exit:
+	if (err != 0)
+		v4l2_err(sd, "Error setting std, write failed\n");
+
+	return err;
+}
+
+static int adv7343_setoutput(struct v4l2_subdev *sd, u32 output_type)
+{
+	struct adv7343_state *state = to_state(sd);
+	unsigned char val;
+	int err = 0;
+
+	if (output_type > ADV7343_SVIDEO_ID) {
+		v4l2_dbg(1, debug, sd,
+			"Invalid output type or output type not supported:%d\n",
+								output_type);
+		return -EINVAL;
+	}
+
+	/* Enable Appropriate DAC */
+	val = state->reg00 & 0x03;
+
+	/* configure default configuration */
+	if (!state->pdata)
+		if (output_type == ADV7343_COMPOSITE_ID)
+			val |= ADV7343_COMPOSITE_POWER_VALUE;
+		else if (output_type == ADV7343_COMPONENT_ID)
+			val |= ADV7343_COMPONENT_POWER_VALUE;
+		else
+			val |= ADV7343_SVIDEO_POWER_VALUE;
+	else
+		val = state->pdata->mode_config.sleep_mode << 0 |
+		      state->pdata->mode_config.pll_control << 1 |
+		      state->pdata->mode_config.dac[2] << 2 |
+		      state->pdata->mode_config.dac[1] << 3 |
+		      state->pdata->mode_config.dac[0] << 4 |
+		      state->pdata->mode_config.dac[5] << 5 |
+		      state->pdata->mode_config.dac[4] << 6 |
+		      state->pdata->mode_config.dac[3] << 7;
+
+	err = adv7343_write(sd, ADV7343_POWER_MODE_REG, val);
+	if (err < 0)
+		goto setoutput_exit;
+
+	state->reg00 = val;
+
+	/* Enable YUV output */
+	val = state->reg02 | YUV_OUTPUT_SELECT;
+	err = adv7343_write(sd, ADV7343_MODE_REG0, val);
+	if (err < 0)
+		goto setoutput_exit;
+
+	state->reg02 = val;
+
+	/* configure SD DAC Output 2 and SD DAC Output 1 bit to zero */
+	val = state->reg82 & (SD_DAC_1_DI & SD_DAC_2_DI);
+
+	if (state->pdata && state->pdata->sd_config.sd_dac_out[0])
+		val = val | (state->pdata->sd_config.sd_dac_out[0] << 1);
+	else if (state->pdata && !state->pdata->sd_config.sd_dac_out[0])
+		val = val & ~(state->pdata->sd_config.sd_dac_out[0] << 1);
+
+	if (state->pdata && state->pdata->sd_config.sd_dac_out[1])
+		val = val | (state->pdata->sd_config.sd_dac_out[1] << 2);
+	else if (state->pdata && !state->pdata->sd_config.sd_dac_out[1])
+		val = val & ~(state->pdata->sd_config.sd_dac_out[1] << 2);
+
+	err = adv7343_write(sd, ADV7343_SD_MODE_REG2, val);
+	if (err < 0)
+		goto setoutput_exit;
+
+	state->reg82 = val;
+
+	/* configure ED/HD Color DAC Swap and ED/HD RGB Input Enable bit to
+	 * zero */
+	val = state->reg35 & (HD_RGB_INPUT_DI & HD_DAC_SWAP_DI);
+	err = adv7343_write(sd, ADV7343_HD_MODE_REG6, val);
+	if (err < 0)
+		goto setoutput_exit;
+
+	state->reg35 = val;
+
+setoutput_exit:
+	if (err != 0)
+		v4l2_err(sd, "Error setting output, write failed\n");
+
+	return err;
+}
+
+static int adv7343_log_status(struct v4l2_subdev *sd)
+{
+	struct adv7343_state *state = to_state(sd);
+
+	v4l2_info(sd, "Standard: %llx\n", (unsigned long long)state->std);
+	v4l2_info(sd, "Output: %s\n", (state->output == 0) ? "Composite" :
+			((state->output == 1) ? "Component" : "S-Video"));
+	return 0;
+}
+
+static int adv7343_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		return adv7343_write(sd, ADV7343_SD_BRIGHTNESS_WSS,
+					ctrl->val);
+
+	case V4L2_CID_HUE:
+		return adv7343_write(sd, ADV7343_SD_HUE_REG, ctrl->val);
+
+	case V4L2_CID_GAIN:
+		return adv7343_write(sd, ADV7343_DAC2_OUTPUT_LEVEL, ctrl->val);
+	}
+	return -EINVAL;
+}
+
+static const struct v4l2_ctrl_ops adv7343_ctrl_ops = {
+	.s_ctrl = adv7343_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops adv7343_core_ops = {
+	.log_status = adv7343_log_status,
+};
+
+static int adv7343_s_std_output(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct adv7343_state *state = to_state(sd);
+	int err = 0;
+
+	if (state->std == std)
+		return 0;
+
+	err = adv7343_setstd(sd, std);
+	if (!err)
+		state->std = std;
+
+	return err;
+}
+
+static int adv7343_s_routing(struct v4l2_subdev *sd,
+		u32 input, u32 output, u32 config)
+{
+	struct adv7343_state *state = to_state(sd);
+	int err = 0;
+
+	if (state->output == output)
+		return 0;
+
+	err = adv7343_setoutput(sd, output);
+	if (!err)
+		state->output = output;
+
+	return err;
+}
+
+static const struct v4l2_subdev_video_ops adv7343_video_ops = {
+	.s_std_output	= adv7343_s_std_output,
+	.s_routing	= adv7343_s_routing,
+};
+
+static const struct v4l2_subdev_ops adv7343_ops = {
+	.core	= &adv7343_core_ops,
+	.video	= &adv7343_video_ops,
+};
+
+static int adv7343_initialize(struct v4l2_subdev *sd)
+{
+	struct adv7343_state *state = to_state(sd);
+	int err = 0;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(adv7343_init_reg_val); i += 2) {
+
+		err = adv7343_write(sd, adv7343_init_reg_val[i],
+					adv7343_init_reg_val[i+1]);
+		if (err) {
+			v4l2_err(sd, "Error initializing\n");
+			return err;
+		}
+	}
+
+	/* Configure for default video standard */
+	err = adv7343_setoutput(sd, state->output);
+	if (err < 0) {
+		v4l2_err(sd, "Error setting output during init\n");
+		return -EINVAL;
+	}
+
+	err = adv7343_setstd(sd, state->std);
+	if (err < 0) {
+		v4l2_err(sd, "Error setting std during init\n");
+		return -EINVAL;
+	}
+
+	return err;
+}
+
+static struct adv7343_platform_data *
+adv7343_get_pdata(struct i2c_client *client)
+{
+	struct adv7343_platform_data *pdata;
+	struct device_node *np;
+
+	if (!IS_ENABLED(CONFIG_OF) || !client->dev.of_node)
+		return client->dev.platform_data;
+
+	np = of_graph_get_next_endpoint(client->dev.of_node, NULL);
+	if (!np)
+		return NULL;
+
+	pdata = devm_kzalloc(&client->dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		goto done;
+
+	pdata->mode_config.sleep_mode =
+			of_property_read_bool(np, "adi,power-mode-sleep-mode");
+
+	pdata->mode_config.pll_control =
+			of_property_read_bool(np, "adi,power-mode-pll-ctrl");
+
+	of_property_read_u32_array(np, "adi,dac-enable",
+				   pdata->mode_config.dac, 6);
+
+	of_property_read_u32_array(np, "adi,sd-dac-enable",
+				   pdata->sd_config.sd_dac_out, 2);
+
+done:
+	of_node_put(np);
+	return pdata;
+}
+
+static int adv7343_probe(struct i2c_client *client)
+{
+	struct adv7343_state *state;
+	int err;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -ENODEV;
+
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	state = devm_kzalloc(&client->dev, sizeof(struct adv7343_state),
+			     GFP_KERNEL);
+	if (state == NULL)
+		return -ENOMEM;
+
+	/* Copy board specific information here */
+	state->pdata = adv7343_get_pdata(client);
+
+	state->reg00	= 0x80;
+	state->reg01	= 0x00;
+	state->reg02	= 0x20;
+	state->reg35	= 0x00;
+	state->reg80	= ADV7343_SD_MODE_REG1_DEFAULT;
+	state->reg82	= ADV7343_SD_MODE_REG2_DEFAULT;
+
+	state->output = ADV7343_COMPOSITE_ID;
+	state->std = V4L2_STD_NTSC;
+
+	v4l2_i2c_subdev_init(&state->sd, client, &adv7343_ops);
+
+	v4l2_ctrl_handler_init(&state->hdl, 2);
+	v4l2_ctrl_new_std(&state->hdl, &adv7343_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, ADV7343_BRIGHTNESS_MIN,
+					     ADV7343_BRIGHTNESS_MAX, 1,
+					     ADV7343_BRIGHTNESS_DEF);
+	v4l2_ctrl_new_std(&state->hdl, &adv7343_ctrl_ops,
+			V4L2_CID_HUE, ADV7343_HUE_MIN,
+				      ADV7343_HUE_MAX, 1,
+				      ADV7343_HUE_DEF);
+	v4l2_ctrl_new_std(&state->hdl, &adv7343_ctrl_ops,
+			V4L2_CID_GAIN, ADV7343_GAIN_MIN,
+				       ADV7343_GAIN_MAX, 1,
+				       ADV7343_GAIN_DEF);
+	state->sd.ctrl_handler = &state->hdl;
+	if (state->hdl.error) {
+		err = state->hdl.error;
+		goto done;
+	}
+	v4l2_ctrl_handler_setup(&state->hdl);
+
+	err = adv7343_initialize(&state->sd);
+	if (err)
+		goto done;
+
+	err = v4l2_async_register_subdev(&state->sd);
+
+done:
+	if (err < 0)
+		v4l2_ctrl_handler_free(&state->hdl);
+
+	return err;
+}
+
+static void adv7343_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct adv7343_state *state = to_state(sd);
+
+	v4l2_async_unregister_subdev(&state->sd);
+	v4l2_ctrl_handler_free(&state->hdl);
+}
+
+static const struct i2c_device_id adv7343_id[] = {
+	{"adv7343", 0},
+	{},
+};
+
+MODULE_DEVICE_TABLE(i2c, adv7343_id);
+
+#if IS_ENABLED(CONFIG_OF)
+static const struct of_device_id adv7343_of_match[] = {
+	{.compatible = "adi,adv7343", },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, adv7343_of_match);
+#endif
+
+static struct i2c_driver adv7343_driver = {
+	.driver = {
+		.of_match_table = of_match_ptr(adv7343_of_match),
+		.name	= "adv7343",
+	},
+	.probe		= adv7343_probe,
+	.remove		= adv7343_remove,
+	.id_table	= adv7343_id,
+};
+
+module_i2c_driver(adv7343_driver);
diff --git a/drivers/media/i2c/adv7343_regs.h b/drivers/media/i2c/adv7343_regs.h
new file mode 100644
index 000000000..e0357e627
--- /dev/null
+++ b/drivers/media/i2c/adv7343_regs.h
@@ -0,0 +1,173 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * ADV7343 encoder related structure and register definitions
+ *
+ * Copyright (C) 2009 Texas Instruments Incorporated - http://www.ti.com/
+ */
+
+#ifndef ADV7343_REGS_H
+#define ADV7343_REGS_H
+
+struct adv7343_std_info {
+	u32 standard_val3;
+	u32 fsc_val;
+	v4l2_std_id stdid;
+};
+
+/* Register offset macros */
+#define ADV7343_POWER_MODE_REG		(0x00)
+#define ADV7343_MODE_SELECT_REG		(0x01)
+#define ADV7343_MODE_REG0		(0x02)
+
+#define ADV7343_DAC2_OUTPUT_LEVEL	(0x0b)
+
+#define ADV7343_SOFT_RESET		(0x17)
+
+#define ADV7343_HD_MODE_REG1		(0x30)
+#define ADV7343_HD_MODE_REG2		(0x31)
+#define ADV7343_HD_MODE_REG3		(0x32)
+#define ADV7343_HD_MODE_REG4		(0x33)
+#define ADV7343_HD_MODE_REG5		(0x34)
+#define ADV7343_HD_MODE_REG6		(0x35)
+
+#define ADV7343_HD_MODE_REG7		(0x39)
+
+#define ADV7343_SD_MODE_REG1		(0x80)
+#define ADV7343_SD_MODE_REG2		(0x82)
+#define ADV7343_SD_MODE_REG3		(0x83)
+#define ADV7343_SD_MODE_REG4		(0x84)
+#define ADV7343_SD_MODE_REG5		(0x86)
+#define ADV7343_SD_MODE_REG6		(0x87)
+#define ADV7343_SD_MODE_REG7		(0x88)
+#define ADV7343_SD_MODE_REG8		(0x89)
+
+#define ADV7343_FSC_REG0		(0x8C)
+#define ADV7343_FSC_REG1		(0x8D)
+#define ADV7343_FSC_REG2		(0x8E)
+#define ADV7343_FSC_REG3		(0x8F)
+
+#define ADV7343_SD_CGMS_WSS0		(0x99)
+
+#define ADV7343_SD_HUE_REG		(0xA0)
+#define ADV7343_SD_BRIGHTNESS_WSS	(0xA1)
+
+/* Default values for the registers */
+#define ADV7343_POWER_MODE_REG_DEFAULT		(0x10)
+#define ADV7343_HD_MODE_REG1_DEFAULT		(0x3C)	/* Changed Default
+							   720p EAVSAV code*/
+#define ADV7343_HD_MODE_REG2_DEFAULT		(0x01)	/* Changed Pixel data
+							   valid */
+#define ADV7343_HD_MODE_REG3_DEFAULT		(0x00)	/* Color delay 0 clks */
+#define ADV7343_HD_MODE_REG4_DEFAULT		(0xE8)	/* Changed */
+#define ADV7343_HD_MODE_REG5_DEFAULT		(0x08)
+#define ADV7343_HD_MODE_REG6_DEFAULT		(0x00)
+#define ADV7343_HD_MODE_REG7_DEFAULT		(0x00)
+#define ADV7343_SD_MODE_REG8_DEFAULT		(0x00)
+#define ADV7343_SOFT_RESET_DEFAULT		(0x02)
+#define ADV7343_COMPOSITE_POWER_VALUE		(0x80)
+#define ADV7343_COMPONENT_POWER_VALUE		(0x1C)
+#define ADV7343_SVIDEO_POWER_VALUE		(0x60)
+#define ADV7343_SD_HUE_REG_DEFAULT		(127)
+#define ADV7343_SD_BRIGHTNESS_WSS_DEFAULT	(0x03)
+
+#define ADV7343_SD_CGMS_WSS0_DEFAULT		(0x10)
+
+#define ADV7343_SD_MODE_REG1_DEFAULT		(0x00)
+#define ADV7343_SD_MODE_REG2_DEFAULT		(0xC9)
+#define ADV7343_SD_MODE_REG3_DEFAULT		(0x10)
+#define ADV7343_SD_MODE_REG4_DEFAULT		(0x01)
+#define ADV7343_SD_MODE_REG5_DEFAULT		(0x02)
+#define ADV7343_SD_MODE_REG6_DEFAULT		(0x0C)
+#define ADV7343_SD_MODE_REG7_DEFAULT		(0x04)
+#define ADV7343_SD_MODE_REG8_DEFAULT		(0x00)
+
+/* Bit masks for Mode Select Register */
+#define INPUT_MODE_MASK			(0x70)
+#define SD_INPUT_MODE			(0x00)
+#define HD_720P_INPUT_MODE		(0x10)
+#define HD_1080I_INPUT_MODE		(0x10)
+
+/* Bit masks for Mode Register 0 */
+#define TEST_PATTERN_BLACK_BAR_EN	(0x04)
+#define YUV_OUTPUT_SELECT		(0x20)
+#define RGB_OUTPUT_SELECT		(0xDF)
+
+/* Bit masks for DAC output levels */
+#define DAC_OUTPUT_LEVEL_MASK		(0xFF)
+
+/* Bit masks for soft reset register */
+#define SOFT_RESET			(0x02)
+
+/* Bit masks for HD Mode Register 1 */
+#define OUTPUT_STD_MASK		(0x03)
+#define OUTPUT_STD_SHIFT	(0)
+#define OUTPUT_STD_EIA0_2	(0x00)
+#define OUTPUT_STD_EIA0_1	(0x01)
+#define OUTPUT_STD_FULL		(0x02)
+#define EMBEDDED_SYNC		(0x04)
+#define EXTERNAL_SYNC		(0xFB)
+#define STD_MODE_SHIFT		(3)
+#define STD_MODE_MASK		(0x1F)
+#define STD_MODE_720P		(0x05)
+#define STD_MODE_720P_25	(0x08)
+#define STD_MODE_720P_30	(0x07)
+#define STD_MODE_720P_50	(0x06)
+#define STD_MODE_1080I		(0x0D)
+#define STD_MODE_1080I_25fps	(0x0E)
+#define STD_MODE_1080P_24	(0x12)
+#define STD_MODE_1080P_25	(0x10)
+#define STD_MODE_1080P_30	(0x0F)
+#define STD_MODE_525P		(0x00)
+#define STD_MODE_625P		(0x03)
+
+/* Bit masks for SD Mode Register 1 */
+#define SD_STD_MASK		(0x03)
+#define SD_STD_NTSC		(0x00)
+#define SD_STD_PAL_BDGHI	(0x01)
+#define SD_STD_PAL_M		(0x02)
+#define SD_STD_PAL_N		(0x03)
+#define SD_LUMA_FLTR_MASK	(0x7)
+#define SD_LUMA_FLTR_SHIFT	(0x2)
+#define SD_CHROMA_FLTR_MASK	(0x7)
+#define SD_CHROMA_FLTR_SHIFT	(0x5)
+
+/* Bit masks for SD Mode Register 2 */
+#define SD_PBPR_SSAF_EN		(0x01)
+#define SD_PBPR_SSAF_DI		(0xFE)
+#define SD_DAC_1_DI		(0xFD)
+#define SD_DAC_2_DI		(0xFB)
+#define SD_PEDESTAL_EN		(0x08)
+#define SD_PEDESTAL_DI		(0xF7)
+#define SD_SQUARE_PIXEL_EN	(0x10)
+#define SD_SQUARE_PIXEL_DI	(0xEF)
+#define SD_PIXEL_DATA_VALID	(0x40)
+#define SD_ACTIVE_EDGE_EN	(0x80)
+#define SD_ACTIVE_EDGE_DI	(0x7F)
+
+/* Bit masks for HD Mode Register 6 */
+#define HD_RGB_INPUT_EN		(0x02)
+#define HD_RGB_INPUT_DI		(0xFD)
+#define HD_PBPR_SYNC_EN		(0x04)
+#define HD_PBPR_SYNC_DI		(0xFB)
+#define HD_DAC_SWAP_EN		(0x08)
+#define HD_DAC_SWAP_DI		(0xF7)
+#define HD_GAMMA_CURVE_A	(0xEF)
+#define HD_GAMMA_CURVE_B	(0x10)
+#define HD_GAMMA_EN		(0x20)
+#define HD_GAMMA_DI		(0xDF)
+#define HD_ADPT_FLTR_MODEB	(0x40)
+#define HD_ADPT_FLTR_MODEA	(0xBF)
+#define HD_ADPT_FLTR_EN		(0x80)
+#define HD_ADPT_FLTR_DI		(0x7F)
+
+#define ADV7343_BRIGHTNESS_MAX	(127)
+#define ADV7343_BRIGHTNESS_MIN	(0)
+#define ADV7343_BRIGHTNESS_DEF	(3)
+#define ADV7343_HUE_MAX		(255)
+#define ADV7343_HUE_MIN		(0)
+#define ADV7343_HUE_DEF		(127)
+#define ADV7343_GAIN_MAX	(64)
+#define ADV7343_GAIN_MIN	(-64)
+#define ADV7343_GAIN_DEF	(0)
+
+#endif
diff --git a/drivers/media/i2c/adv7393.c b/drivers/media/i2c/adv7393.c
new file mode 100644
index 000000000..7638af455
--- /dev/null
+++ b/drivers/media/i2c/adv7393.c
@@ -0,0 +1,462 @@
+/*
+ * adv7393 - ADV7393 Video Encoder Driver
+ *
+ * The encoder hardware does not support SECAM.
+ *
+ * Copyright (C) 2010-2012 ADVANSEE - http://www.advansee.com/
+ * Benoît Thébaudeau <benoit.thebaudeau@advansee.com>
+ *
+ * Based on ADV7343 driver,
+ *
+ * Copyright (C) 2009 Texas Instruments Incorporated - http://www.ti.com/
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed .as is. WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/ctype.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/module.h>
+#include <linux/videodev2.h>
+#include <linux/uaccess.h>
+
+#include <media/i2c/adv7393.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+
+#include "adv7393_regs.h"
+
+MODULE_DESCRIPTION("ADV7393 video encoder driver");
+MODULE_LICENSE("GPL");
+
+static bool debug;
+module_param(debug, bool, 0644);
+MODULE_PARM_DESC(debug, "Debug level 0-1");
+
+struct adv7393_state {
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
+	u8 reg00;
+	u8 reg01;
+	u8 reg02;
+	u8 reg35;
+	u8 reg80;
+	u8 reg82;
+	u32 output;
+	v4l2_std_id std;
+};
+
+static inline struct adv7393_state *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct adv7393_state, sd);
+}
+
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct adv7393_state, hdl)->sd;
+}
+
+static inline int adv7393_write(struct v4l2_subdev *sd, u8 reg, u8 value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return i2c_smbus_write_byte_data(client, reg, value);
+}
+
+static const u8 adv7393_init_reg_val[] = {
+	ADV7393_SOFT_RESET, ADV7393_SOFT_RESET_DEFAULT,
+	ADV7393_POWER_MODE_REG, ADV7393_POWER_MODE_REG_DEFAULT,
+
+	ADV7393_HD_MODE_REG1, ADV7393_HD_MODE_REG1_DEFAULT,
+	ADV7393_HD_MODE_REG2, ADV7393_HD_MODE_REG2_DEFAULT,
+	ADV7393_HD_MODE_REG3, ADV7393_HD_MODE_REG3_DEFAULT,
+	ADV7393_HD_MODE_REG4, ADV7393_HD_MODE_REG4_DEFAULT,
+	ADV7393_HD_MODE_REG5, ADV7393_HD_MODE_REG5_DEFAULT,
+	ADV7393_HD_MODE_REG6, ADV7393_HD_MODE_REG6_DEFAULT,
+	ADV7393_HD_MODE_REG7, ADV7393_HD_MODE_REG7_DEFAULT,
+
+	ADV7393_SD_MODE_REG1, ADV7393_SD_MODE_REG1_DEFAULT,
+	ADV7393_SD_MODE_REG2, ADV7393_SD_MODE_REG2_DEFAULT,
+	ADV7393_SD_MODE_REG3, ADV7393_SD_MODE_REG3_DEFAULT,
+	ADV7393_SD_MODE_REG4, ADV7393_SD_MODE_REG4_DEFAULT,
+	ADV7393_SD_MODE_REG5, ADV7393_SD_MODE_REG5_DEFAULT,
+	ADV7393_SD_MODE_REG6, ADV7393_SD_MODE_REG6_DEFAULT,
+	ADV7393_SD_MODE_REG7, ADV7393_SD_MODE_REG7_DEFAULT,
+	ADV7393_SD_MODE_REG8, ADV7393_SD_MODE_REG8_DEFAULT,
+
+	ADV7393_SD_TIMING_REG0, ADV7393_SD_TIMING_REG0_DEFAULT,
+
+	ADV7393_SD_HUE_ADJUST, ADV7393_SD_HUE_ADJUST_DEFAULT,
+	ADV7393_SD_CGMS_WSS0, ADV7393_SD_CGMS_WSS0_DEFAULT,
+	ADV7393_SD_BRIGHTNESS_WSS, ADV7393_SD_BRIGHTNESS_WSS_DEFAULT,
+};
+
+/*
+ *			    2^32
+ * FSC(reg) =  FSC (HZ) * --------
+ *			  27000000
+ */
+static const struct adv7393_std_info stdinfo[] = {
+	{
+		/* FSC(Hz) = 4,433,618.75 Hz */
+		SD_STD_NTSC, 705268427, V4L2_STD_NTSC_443,
+	}, {
+		/* FSC(Hz) = 3,579,545.45 Hz */
+		SD_STD_NTSC, 569408542, V4L2_STD_NTSC,
+	}, {
+		/* FSC(Hz) = 3,575,611.00 Hz */
+		SD_STD_PAL_M, 568782678, V4L2_STD_PAL_M,
+	}, {
+		/* FSC(Hz) = 3,582,056.00 Hz */
+		SD_STD_PAL_N, 569807903, V4L2_STD_PAL_Nc,
+	}, {
+		/* FSC(Hz) = 4,433,618.75 Hz */
+		SD_STD_PAL_N, 705268427, V4L2_STD_PAL_N,
+	}, {
+		/* FSC(Hz) = 4,433,618.75 Hz */
+		SD_STD_PAL_M, 705268427, V4L2_STD_PAL_60,
+	}, {
+		/* FSC(Hz) = 4,433,618.75 Hz */
+		SD_STD_PAL_BDGHI, 705268427, V4L2_STD_PAL,
+	},
+};
+
+static int adv7393_setstd(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct adv7393_state *state = to_state(sd);
+	const struct adv7393_std_info *std_info;
+	int num_std;
+	u8 reg;
+	u32 val;
+	int err = 0;
+	int i;
+
+	num_std = ARRAY_SIZE(stdinfo);
+
+	for (i = 0; i < num_std; i++) {
+		if (stdinfo[i].stdid & std)
+			break;
+	}
+
+	if (i == num_std) {
+		v4l2_dbg(1, debug, sd,
+				"Invalid std or std is not supported: %llx\n",
+						(unsigned long long)std);
+		return -EINVAL;
+	}
+
+	std_info = &stdinfo[i];
+
+	/* Set the standard */
+	val = state->reg80 & ~SD_STD_MASK;
+	val |= std_info->standard_val3;
+	err = adv7393_write(sd, ADV7393_SD_MODE_REG1, val);
+	if (err < 0)
+		goto setstd_exit;
+
+	state->reg80 = val;
+
+	/* Configure the input mode register */
+	val = state->reg01 & ~INPUT_MODE_MASK;
+	val |= SD_INPUT_MODE;
+	err = adv7393_write(sd, ADV7393_MODE_SELECT_REG, val);
+	if (err < 0)
+		goto setstd_exit;
+
+	state->reg01 = val;
+
+	/* Program the sub carrier frequency registers */
+	val = std_info->fsc_val;
+	for (reg = ADV7393_FSC_REG0; reg <= ADV7393_FSC_REG3; reg++) {
+		err = adv7393_write(sd, reg, val);
+		if (err < 0)
+			goto setstd_exit;
+		val >>= 8;
+	}
+
+	val = state->reg82;
+
+	/* Pedestal settings */
+	if (std & (V4L2_STD_NTSC | V4L2_STD_NTSC_443))
+		val |= SD_PEDESTAL_EN;
+	else
+		val &= SD_PEDESTAL_DI;
+
+	err = adv7393_write(sd, ADV7393_SD_MODE_REG2, val);
+	if (err < 0)
+		goto setstd_exit;
+
+	state->reg82 = val;
+
+setstd_exit:
+	if (err != 0)
+		v4l2_err(sd, "Error setting std, write failed\n");
+
+	return err;
+}
+
+static int adv7393_setoutput(struct v4l2_subdev *sd, u32 output_type)
+{
+	struct adv7393_state *state = to_state(sd);
+	u8 val;
+	int err = 0;
+
+	if (output_type > ADV7393_SVIDEO_ID) {
+		v4l2_dbg(1, debug, sd,
+			"Invalid output type or output type not supported:%d\n",
+								output_type);
+		return -EINVAL;
+	}
+
+	/* Enable Appropriate DAC */
+	val = state->reg00 & 0x03;
+
+	if (output_type == ADV7393_COMPOSITE_ID)
+		val |= ADV7393_COMPOSITE_POWER_VALUE;
+	else if (output_type == ADV7393_COMPONENT_ID)
+		val |= ADV7393_COMPONENT_POWER_VALUE;
+	else
+		val |= ADV7393_SVIDEO_POWER_VALUE;
+
+	err = adv7393_write(sd, ADV7393_POWER_MODE_REG, val);
+	if (err < 0)
+		goto setoutput_exit;
+
+	state->reg00 = val;
+
+	/* Enable YUV output */
+	val = state->reg02 | YUV_OUTPUT_SELECT;
+	err = adv7393_write(sd, ADV7393_MODE_REG0, val);
+	if (err < 0)
+		goto setoutput_exit;
+
+	state->reg02 = val;
+
+	/* configure SD DAC Output 1 bit */
+	val = state->reg82;
+	if (output_type == ADV7393_COMPONENT_ID)
+		val &= SD_DAC_OUT1_DI;
+	else
+		val |= SD_DAC_OUT1_EN;
+	err = adv7393_write(sd, ADV7393_SD_MODE_REG2, val);
+	if (err < 0)
+		goto setoutput_exit;
+
+	state->reg82 = val;
+
+	/* configure ED/HD Color DAC Swap bit to zero */
+	val = state->reg35 & HD_DAC_SWAP_DI;
+	err = adv7393_write(sd, ADV7393_HD_MODE_REG6, val);
+	if (err < 0)
+		goto setoutput_exit;
+
+	state->reg35 = val;
+
+setoutput_exit:
+	if (err != 0)
+		v4l2_err(sd, "Error setting output, write failed\n");
+
+	return err;
+}
+
+static int adv7393_log_status(struct v4l2_subdev *sd)
+{
+	struct adv7393_state *state = to_state(sd);
+
+	v4l2_info(sd, "Standard: %llx\n", (unsigned long long)state->std);
+	v4l2_info(sd, "Output: %s\n", (state->output == 0) ? "Composite" :
+			((state->output == 1) ? "Component" : "S-Video"));
+	return 0;
+}
+
+static int adv7393_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		return adv7393_write(sd, ADV7393_SD_BRIGHTNESS_WSS,
+					ctrl->val & SD_BRIGHTNESS_VALUE_MASK);
+
+	case V4L2_CID_HUE:
+		return adv7393_write(sd, ADV7393_SD_HUE_ADJUST,
+					ctrl->val - ADV7393_HUE_MIN);
+
+	case V4L2_CID_GAIN:
+		return adv7393_write(sd, ADV7393_DAC123_OUTPUT_LEVEL,
+					ctrl->val);
+	}
+	return -EINVAL;
+}
+
+static const struct v4l2_ctrl_ops adv7393_ctrl_ops = {
+	.s_ctrl = adv7393_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops adv7393_core_ops = {
+	.log_status = adv7393_log_status,
+};
+
+static int adv7393_s_std_output(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct adv7393_state *state = to_state(sd);
+	int err = 0;
+
+	if (state->std == std)
+		return 0;
+
+	err = adv7393_setstd(sd, std);
+	if (!err)
+		state->std = std;
+
+	return err;
+}
+
+static int adv7393_s_routing(struct v4l2_subdev *sd,
+		u32 input, u32 output, u32 config)
+{
+	struct adv7393_state *state = to_state(sd);
+	int err = 0;
+
+	if (state->output == output)
+		return 0;
+
+	err = adv7393_setoutput(sd, output);
+	if (!err)
+		state->output = output;
+
+	return err;
+}
+
+static const struct v4l2_subdev_video_ops adv7393_video_ops = {
+	.s_std_output	= adv7393_s_std_output,
+	.s_routing	= adv7393_s_routing,
+};
+
+static const struct v4l2_subdev_ops adv7393_ops = {
+	.core	= &adv7393_core_ops,
+	.video	= &adv7393_video_ops,
+};
+
+static int adv7393_initialize(struct v4l2_subdev *sd)
+{
+	struct adv7393_state *state = to_state(sd);
+	int err = 0;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(adv7393_init_reg_val); i += 2) {
+
+		err = adv7393_write(sd, adv7393_init_reg_val[i],
+					adv7393_init_reg_val[i+1]);
+		if (err) {
+			v4l2_err(sd, "Error initializing\n");
+			return err;
+		}
+	}
+
+	/* Configure for default video standard */
+	err = adv7393_setoutput(sd, state->output);
+	if (err < 0) {
+		v4l2_err(sd, "Error setting output during init\n");
+		return -EINVAL;
+	}
+
+	err = adv7393_setstd(sd, state->std);
+	if (err < 0) {
+		v4l2_err(sd, "Error setting std during init\n");
+		return -EINVAL;
+	}
+
+	return err;
+}
+
+static int adv7393_probe(struct i2c_client *client)
+{
+	struct adv7393_state *state;
+	int err;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -ENODEV;
+
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	state = devm_kzalloc(&client->dev, sizeof(*state), GFP_KERNEL);
+	if (state == NULL)
+		return -ENOMEM;
+
+	state->reg00	= ADV7393_POWER_MODE_REG_DEFAULT;
+	state->reg01	= 0x00;
+	state->reg02	= 0x20;
+	state->reg35	= ADV7393_HD_MODE_REG6_DEFAULT;
+	state->reg80	= ADV7393_SD_MODE_REG1_DEFAULT;
+	state->reg82	= ADV7393_SD_MODE_REG2_DEFAULT;
+
+	state->output = ADV7393_COMPOSITE_ID;
+	state->std = V4L2_STD_NTSC;
+
+	v4l2_i2c_subdev_init(&state->sd, client, &adv7393_ops);
+
+	v4l2_ctrl_handler_init(&state->hdl, 3);
+	v4l2_ctrl_new_std(&state->hdl, &adv7393_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, ADV7393_BRIGHTNESS_MIN,
+					     ADV7393_BRIGHTNESS_MAX, 1,
+					     ADV7393_BRIGHTNESS_DEF);
+	v4l2_ctrl_new_std(&state->hdl, &adv7393_ctrl_ops,
+			V4L2_CID_HUE, ADV7393_HUE_MIN,
+				      ADV7393_HUE_MAX, 1,
+				      ADV7393_HUE_DEF);
+	v4l2_ctrl_new_std(&state->hdl, &adv7393_ctrl_ops,
+			V4L2_CID_GAIN, ADV7393_GAIN_MIN,
+				       ADV7393_GAIN_MAX, 1,
+				       ADV7393_GAIN_DEF);
+	state->sd.ctrl_handler = &state->hdl;
+	if (state->hdl.error) {
+		int err = state->hdl.error;
+
+		v4l2_ctrl_handler_free(&state->hdl);
+		return err;
+	}
+	v4l2_ctrl_handler_setup(&state->hdl);
+
+	err = adv7393_initialize(&state->sd);
+	if (err)
+		v4l2_ctrl_handler_free(&state->hdl);
+	return err;
+}
+
+static void adv7393_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct adv7393_state *state = to_state(sd);
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&state->hdl);
+}
+
+static const struct i2c_device_id adv7393_id[] = {
+	{"adv7393", 0},
+	{},
+};
+MODULE_DEVICE_TABLE(i2c, adv7393_id);
+
+static struct i2c_driver adv7393_driver = {
+	.driver = {
+		.name	= "adv7393",
+	},
+	.probe		= adv7393_probe,
+	.remove		= adv7393_remove,
+	.id_table	= adv7393_id,
+};
+module_i2c_driver(adv7393_driver);
diff --git a/drivers/media/i2c/adv7393_regs.h b/drivers/media/i2c/adv7393_regs.h
new file mode 100644
index 000000000..6eb8732b5
--- /dev/null
+++ b/drivers/media/i2c/adv7393_regs.h
@@ -0,0 +1,180 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * ADV7393 encoder related structure and register definitions
+ *
+ * Copyright (C) 2010-2012 ADVANSEE - http://www.advansee.com/
+ * Benoît Thébaudeau <benoit.thebaudeau@advansee.com>
+ *
+ * Based on ADV7343 driver,
+ *
+ * Copyright (C) 2009 Texas Instruments Incorporated - http://www.ti.com/
+ */
+
+#ifndef ADV7393_REGS_H
+#define ADV7393_REGS_H
+
+struct adv7393_std_info {
+	u32 standard_val3;
+	u32 fsc_val;
+	v4l2_std_id stdid;
+};
+
+/* Register offset macros */
+#define ADV7393_POWER_MODE_REG		(0x00)
+#define ADV7393_MODE_SELECT_REG		(0x01)
+#define ADV7393_MODE_REG0		(0x02)
+
+#define ADV7393_DAC123_OUTPUT_LEVEL	(0x0B)
+
+#define ADV7393_SOFT_RESET		(0x17)
+
+#define ADV7393_HD_MODE_REG1		(0x30)
+#define ADV7393_HD_MODE_REG2		(0x31)
+#define ADV7393_HD_MODE_REG3		(0x32)
+#define ADV7393_HD_MODE_REG4		(0x33)
+#define ADV7393_HD_MODE_REG5		(0x34)
+#define ADV7393_HD_MODE_REG6		(0x35)
+
+#define ADV7393_HD_MODE_REG7		(0x39)
+
+#define ADV7393_SD_MODE_REG1		(0x80)
+#define ADV7393_SD_MODE_REG2		(0x82)
+#define ADV7393_SD_MODE_REG3		(0x83)
+#define ADV7393_SD_MODE_REG4		(0x84)
+#define ADV7393_SD_MODE_REG5		(0x86)
+#define ADV7393_SD_MODE_REG6		(0x87)
+#define ADV7393_SD_MODE_REG7		(0x88)
+#define ADV7393_SD_MODE_REG8		(0x89)
+
+#define ADV7393_SD_TIMING_REG0		(0x8A)
+
+#define ADV7393_FSC_REG0		(0x8C)
+#define ADV7393_FSC_REG1		(0x8D)
+#define ADV7393_FSC_REG2		(0x8E)
+#define ADV7393_FSC_REG3		(0x8F)
+
+#define ADV7393_SD_CGMS_WSS0		(0x99)
+
+#define ADV7393_SD_HUE_ADJUST		(0xA0)
+#define ADV7393_SD_BRIGHTNESS_WSS	(0xA1)
+
+/* Default values for the registers */
+#define ADV7393_POWER_MODE_REG_DEFAULT		(0x10)
+#define ADV7393_HD_MODE_REG1_DEFAULT		(0x3C)	/* Changed Default
+							   720p EAV/SAV code*/
+#define ADV7393_HD_MODE_REG2_DEFAULT		(0x01)	/* Changed Pixel data
+							   valid */
+#define ADV7393_HD_MODE_REG3_DEFAULT		(0x00)	/* Color delay 0 clks */
+#define ADV7393_HD_MODE_REG4_DEFAULT		(0xEC)	/* Changed */
+#define ADV7393_HD_MODE_REG5_DEFAULT		(0x08)
+#define ADV7393_HD_MODE_REG6_DEFAULT		(0x00)
+#define ADV7393_HD_MODE_REG7_DEFAULT		(0x00)
+#define ADV7393_SOFT_RESET_DEFAULT		(0x02)
+#define ADV7393_COMPOSITE_POWER_VALUE		(0x10)
+#define ADV7393_COMPONENT_POWER_VALUE		(0x1C)
+#define ADV7393_SVIDEO_POWER_VALUE		(0x0C)
+#define ADV7393_SD_HUE_ADJUST_DEFAULT		(0x80)
+#define ADV7393_SD_BRIGHTNESS_WSS_DEFAULT	(0x00)
+
+#define ADV7393_SD_CGMS_WSS0_DEFAULT		(0x10)
+
+#define ADV7393_SD_MODE_REG1_DEFAULT		(0x10)
+#define ADV7393_SD_MODE_REG2_DEFAULT		(0xC9)
+#define ADV7393_SD_MODE_REG3_DEFAULT		(0x00)
+#define ADV7393_SD_MODE_REG4_DEFAULT		(0x00)
+#define ADV7393_SD_MODE_REG5_DEFAULT		(0x02)
+#define ADV7393_SD_MODE_REG6_DEFAULT		(0x8C)
+#define ADV7393_SD_MODE_REG7_DEFAULT		(0x14)
+#define ADV7393_SD_MODE_REG8_DEFAULT		(0x00)
+
+#define ADV7393_SD_TIMING_REG0_DEFAULT		(0x0C)
+
+/* Bit masks for Mode Select Register */
+#define INPUT_MODE_MASK			(0x70)
+#define SD_INPUT_MODE			(0x00)
+#define HD_720P_INPUT_MODE		(0x10)
+#define HD_1080I_INPUT_MODE		(0x10)
+
+/* Bit masks for Mode Register 0 */
+#define TEST_PATTERN_BLACK_BAR_EN	(0x04)
+#define YUV_OUTPUT_SELECT		(0x20)
+#define RGB_OUTPUT_SELECT		(0xDF)
+
+/* Bit masks for SD brightness/WSS */
+#define SD_BRIGHTNESS_VALUE_MASK	(0x7F)
+#define SD_BLANK_WSS_DATA_MASK		(0x80)
+
+/* Bit masks for soft reset register */
+#define SOFT_RESET			(0x02)
+
+/* Bit masks for HD Mode Register 1 */
+#define OUTPUT_STD_MASK		(0x03)
+#define OUTPUT_STD_SHIFT	(0)
+#define OUTPUT_STD_EIA0_2	(0x00)
+#define OUTPUT_STD_EIA0_1	(0x01)
+#define OUTPUT_STD_FULL		(0x02)
+#define EMBEDDED_SYNC		(0x04)
+#define EXTERNAL_SYNC		(0xFB)
+#define STD_MODE_MASK		(0x1F)
+#define STD_MODE_SHIFT		(3)
+#define STD_MODE_720P		(0x05)
+#define STD_MODE_720P_25	(0x08)
+#define STD_MODE_720P_30	(0x07)
+#define STD_MODE_720P_50	(0x06)
+#define STD_MODE_1080I		(0x0D)
+#define STD_MODE_1080I_25	(0x0E)
+#define STD_MODE_1080P_24	(0x11)
+#define STD_MODE_1080P_25	(0x10)
+#define STD_MODE_1080P_30	(0x0F)
+#define STD_MODE_525P		(0x00)
+#define STD_MODE_625P		(0x03)
+
+/* Bit masks for SD Mode Register 1 */
+#define SD_STD_MASK		(0x03)
+#define SD_STD_NTSC		(0x00)
+#define SD_STD_PAL_BDGHI	(0x01)
+#define SD_STD_PAL_M		(0x02)
+#define SD_STD_PAL_N		(0x03)
+#define SD_LUMA_FLTR_MASK	(0x07)
+#define SD_LUMA_FLTR_SHIFT	(2)
+#define SD_CHROMA_FLTR_MASK	(0x07)
+#define SD_CHROMA_FLTR_SHIFT	(5)
+
+/* Bit masks for SD Mode Register 2 */
+#define SD_PRPB_SSAF_EN		(0x01)
+#define SD_PRPB_SSAF_DI		(0xFE)
+#define SD_DAC_OUT1_EN		(0x02)
+#define SD_DAC_OUT1_DI		(0xFD)
+#define SD_PEDESTAL_EN		(0x08)
+#define SD_PEDESTAL_DI		(0xF7)
+#define SD_SQUARE_PIXEL_EN	(0x10)
+#define SD_SQUARE_PIXEL_DI	(0xEF)
+#define SD_PIXEL_DATA_VALID	(0x40)
+#define SD_ACTIVE_EDGE_EN	(0x80)
+#define SD_ACTIVE_EDGE_DI	(0x7F)
+
+/* Bit masks for HD Mode Register 6 */
+#define HD_PRPB_SYNC_EN		(0x04)
+#define HD_PRPB_SYNC_DI		(0xFB)
+#define HD_DAC_SWAP_EN		(0x08)
+#define HD_DAC_SWAP_DI		(0xF7)
+#define HD_GAMMA_CURVE_A	(0xEF)
+#define HD_GAMMA_CURVE_B	(0x10)
+#define HD_GAMMA_EN		(0x20)
+#define HD_GAMMA_DI		(0xDF)
+#define HD_ADPT_FLTR_MODEA	(0xBF)
+#define HD_ADPT_FLTR_MODEB	(0x40)
+#define HD_ADPT_FLTR_EN		(0x80)
+#define HD_ADPT_FLTR_DI		(0x7F)
+
+#define ADV7393_BRIGHTNESS_MAX	(63)
+#define ADV7393_BRIGHTNESS_MIN	(-64)
+#define ADV7393_BRIGHTNESS_DEF	(0)
+#define ADV7393_HUE_MAX		(127)
+#define ADV7393_HUE_MIN		(-128)
+#define ADV7393_HUE_DEF		(0)
+#define ADV7393_GAIN_MAX	(64)
+#define ADV7393_GAIN_MIN	(-64)
+#define ADV7393_GAIN_DEF	(0)
+
+#endif
diff --git a/drivers/media/i2c/adv748x/Makefile b/drivers/media/i2c/adv748x/Makefile
new file mode 100644
index 000000000..93844f14c
--- /dev/null
+++ b/drivers/media/i2c/adv748x/Makefile
@@ -0,0 +1,8 @@
+# SPDX-License-Identifier: GPL-2.0-only
+adv748x-objs	:= \
+		adv748x-afe.o \
+		adv748x-core.o \
+		adv748x-csi2.o \
+		adv748x-hdmi.o
+
+obj-$(CONFIG_VIDEO_ADV748X)	+= adv748x.o
diff --git a/drivers/media/i2c/adv748x/adv748x-afe.c b/drivers/media/i2c/adv748x/adv748x-afe.c
new file mode 100644
index 000000000..00095c776
--- /dev/null
+++ b/drivers/media/i2c/adv748x/adv748x-afe.c
@@ -0,0 +1,556 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Driver for Analog Devices ADV748X 8 channel analog front end (AFE) receiver
+ * with standard definition processor (SDP)
+ *
+ * Copyright (C) 2017 Renesas Electronics Corp.
+ */
+
+#include <linux/delay.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/v4l2-dv-timings.h>
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-dv-timings.h>
+#include <media/v4l2-ioctl.h>
+
+#include "adv748x.h"
+
+/* -----------------------------------------------------------------------------
+ * SDP
+ */
+
+#define ADV748X_AFE_STD_AD_PAL_BG_NTSC_J_SECAM		0x0
+#define ADV748X_AFE_STD_AD_PAL_BG_NTSC_J_SECAM_PED	0x1
+#define ADV748X_AFE_STD_AD_PAL_N_NTSC_J_SECAM		0x2
+#define ADV748X_AFE_STD_AD_PAL_N_NTSC_M_SECAM		0x3
+#define ADV748X_AFE_STD_NTSC_J				0x4
+#define ADV748X_AFE_STD_NTSC_M				0x5
+#define ADV748X_AFE_STD_PAL60				0x6
+#define ADV748X_AFE_STD_NTSC_443			0x7
+#define ADV748X_AFE_STD_PAL_BG				0x8
+#define ADV748X_AFE_STD_PAL_N				0x9
+#define ADV748X_AFE_STD_PAL_M				0xa
+#define ADV748X_AFE_STD_PAL_M_PED			0xb
+#define ADV748X_AFE_STD_PAL_COMB_N			0xc
+#define ADV748X_AFE_STD_PAL_COMB_N_PED			0xd
+#define ADV748X_AFE_STD_PAL_SECAM			0xe
+#define ADV748X_AFE_STD_PAL_SECAM_PED			0xf
+
+static int adv748x_afe_read_ro_map(struct adv748x_state *state, u8 reg)
+{
+	int ret;
+
+	/* Select SDP Read-Only Main Map */
+	ret = sdp_write(state, ADV748X_SDP_MAP_SEL,
+			ADV748X_SDP_MAP_SEL_RO_MAIN);
+	if (ret < 0)
+		return ret;
+
+	return sdp_read(state, reg);
+}
+
+static int adv748x_afe_status(struct adv748x_afe *afe, u32 *signal,
+			      v4l2_std_id *std)
+{
+	struct adv748x_state *state = adv748x_afe_to_state(afe);
+	int info;
+
+	/* Read status from reg 0x10 of SDP RO Map */
+	info = adv748x_afe_read_ro_map(state, ADV748X_SDP_RO_10);
+	if (info < 0)
+		return info;
+
+	if (signal)
+		*signal = info & ADV748X_SDP_RO_10_IN_LOCK ?
+				0 : V4L2_IN_ST_NO_SIGNAL;
+
+	if (!std)
+		return 0;
+
+	/* Standard not valid if there is no signal */
+	if (!(info & ADV748X_SDP_RO_10_IN_LOCK)) {
+		*std = V4L2_STD_UNKNOWN;
+		return 0;
+	}
+
+	switch (info & 0x70) {
+	case 0x00:
+		*std = V4L2_STD_NTSC;
+		break;
+	case 0x10:
+		*std = V4L2_STD_NTSC_443;
+		break;
+	case 0x20:
+		*std = V4L2_STD_PAL_M;
+		break;
+	case 0x30:
+		*std = V4L2_STD_PAL_60;
+		break;
+	case 0x40:
+		*std = V4L2_STD_PAL;
+		break;
+	case 0x50:
+		*std = V4L2_STD_SECAM;
+		break;
+	case 0x60:
+		*std = V4L2_STD_PAL_Nc | V4L2_STD_PAL_N;
+		break;
+	case 0x70:
+		*std = V4L2_STD_SECAM;
+		break;
+	default:
+		*std = V4L2_STD_UNKNOWN;
+		break;
+	}
+
+	return 0;
+}
+
+static void adv748x_afe_fill_format(struct adv748x_afe *afe,
+				    struct v4l2_mbus_framefmt *fmt)
+{
+	memset(fmt, 0, sizeof(*fmt));
+
+	fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
+	fmt->colorspace = V4L2_COLORSPACE_SMPTE170M;
+	fmt->field = V4L2_FIELD_ALTERNATE;
+
+	fmt->width = 720;
+	fmt->height = afe->curr_norm & V4L2_STD_525_60 ? 480 : 576;
+
+	/* Field height */
+	fmt->height /= 2;
+}
+
+static int adv748x_afe_std(v4l2_std_id std)
+{
+	if (std == V4L2_STD_PAL_60)
+		return ADV748X_AFE_STD_PAL60;
+	if (std == V4L2_STD_NTSC_443)
+		return ADV748X_AFE_STD_NTSC_443;
+	if (std == V4L2_STD_PAL_N)
+		return ADV748X_AFE_STD_PAL_N;
+	if (std == V4L2_STD_PAL_M)
+		return ADV748X_AFE_STD_PAL_M;
+	if (std == V4L2_STD_PAL_Nc)
+		return ADV748X_AFE_STD_PAL_COMB_N;
+	if (std & V4L2_STD_NTSC)
+		return ADV748X_AFE_STD_NTSC_M;
+	if (std & V4L2_STD_PAL)
+		return ADV748X_AFE_STD_PAL_BG;
+	if (std & V4L2_STD_SECAM)
+		return ADV748X_AFE_STD_PAL_SECAM;
+
+	return -EINVAL;
+}
+
+static void adv748x_afe_set_video_standard(struct adv748x_state *state,
+					  int sdpstd)
+{
+	sdp_clrset(state, ADV748X_SDP_VID_SEL, ADV748X_SDP_VID_SEL_MASK,
+		   (sdpstd & 0xf) << ADV748X_SDP_VID_SEL_SHIFT);
+}
+
+int adv748x_afe_s_input(struct adv748x_afe *afe, unsigned int input)
+{
+	struct adv748x_state *state = adv748x_afe_to_state(afe);
+
+	return sdp_write(state, ADV748X_SDP_INSEL, input);
+}
+
+static int adv748x_afe_g_pixelaspect(struct v4l2_subdev *sd,
+				     struct v4l2_fract *aspect)
+{
+	struct adv748x_afe *afe = adv748x_sd_to_afe(sd);
+
+	if (afe->curr_norm & V4L2_STD_525_60) {
+		aspect->numerator = 11;
+		aspect->denominator = 10;
+	} else {
+		aspect->numerator = 54;
+		aspect->denominator = 59;
+	}
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * v4l2_subdev_video_ops
+ */
+
+static int adv748x_afe_g_std(struct v4l2_subdev *sd, v4l2_std_id *norm)
+{
+	struct adv748x_afe *afe = adv748x_sd_to_afe(sd);
+
+	*norm = afe->curr_norm;
+
+	return 0;
+}
+
+static int adv748x_afe_s_std(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct adv748x_afe *afe = adv748x_sd_to_afe(sd);
+	struct adv748x_state *state = adv748x_afe_to_state(afe);
+	int afe_std = adv748x_afe_std(std);
+
+	if (afe_std < 0)
+		return afe_std;
+
+	mutex_lock(&state->mutex);
+
+	adv748x_afe_set_video_standard(state, afe_std);
+	afe->curr_norm = std;
+
+	mutex_unlock(&state->mutex);
+
+	return 0;
+}
+
+static int adv748x_afe_querystd(struct v4l2_subdev *sd, v4l2_std_id *std)
+{
+	struct adv748x_afe *afe = adv748x_sd_to_afe(sd);
+	struct adv748x_state *state = adv748x_afe_to_state(afe);
+	int afe_std;
+	int ret;
+
+	mutex_lock(&state->mutex);
+
+	if (afe->streaming) {
+		ret = -EBUSY;
+		goto unlock;
+	}
+
+	/* Set auto detect mode */
+	adv748x_afe_set_video_standard(state,
+				       ADV748X_AFE_STD_AD_PAL_BG_NTSC_J_SECAM);
+
+	msleep(100);
+
+	/* Read detected standard */
+	ret = adv748x_afe_status(afe, NULL, std);
+
+	afe_std = adv748x_afe_std(afe->curr_norm);
+	if (afe_std < 0)
+		goto unlock;
+
+	/* Restore original state */
+	adv748x_afe_set_video_standard(state, afe_std);
+
+unlock:
+	mutex_unlock(&state->mutex);
+
+	return ret;
+}
+
+static int adv748x_afe_g_tvnorms(struct v4l2_subdev *sd, v4l2_std_id *norm)
+{
+	*norm = V4L2_STD_ALL;
+
+	return 0;
+}
+
+static int adv748x_afe_g_input_status(struct v4l2_subdev *sd, u32 *status)
+{
+	struct adv748x_afe *afe = adv748x_sd_to_afe(sd);
+	struct adv748x_state *state = adv748x_afe_to_state(afe);
+	int ret;
+
+	mutex_lock(&state->mutex);
+
+	ret = adv748x_afe_status(afe, status, NULL);
+
+	mutex_unlock(&state->mutex);
+
+	return ret;
+}
+
+static int adv748x_afe_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct adv748x_afe *afe = adv748x_sd_to_afe(sd);
+	struct adv748x_state *state = adv748x_afe_to_state(afe);
+	u32 signal = V4L2_IN_ST_NO_SIGNAL;
+	int ret;
+
+	mutex_lock(&state->mutex);
+
+	if (enable) {
+		ret = adv748x_afe_s_input(afe, afe->input);
+		if (ret)
+			goto unlock;
+	}
+
+	ret = adv748x_tx_power(afe->tx, enable);
+	if (ret)
+		goto unlock;
+
+	afe->streaming = enable;
+
+	adv748x_afe_status(afe, &signal, NULL);
+	if (signal != V4L2_IN_ST_NO_SIGNAL)
+		adv_dbg(state, "Detected SDP signal\n");
+	else
+		adv_dbg(state, "Couldn't detect SDP video signal\n");
+
+unlock:
+	mutex_unlock(&state->mutex);
+
+	return ret;
+}
+
+static const struct v4l2_subdev_video_ops adv748x_afe_video_ops = {
+	.g_std = adv748x_afe_g_std,
+	.s_std = adv748x_afe_s_std,
+	.querystd = adv748x_afe_querystd,
+	.g_tvnorms = adv748x_afe_g_tvnorms,
+	.g_input_status = adv748x_afe_g_input_status,
+	.s_stream = adv748x_afe_s_stream,
+	.g_pixelaspect = adv748x_afe_g_pixelaspect,
+};
+
+/* -----------------------------------------------------------------------------
+ * v4l2_subdev_pad_ops
+ */
+
+static int adv748x_afe_propagate_pixelrate(struct adv748x_afe *afe)
+{
+	struct v4l2_subdev *tx;
+
+	tx = adv748x_get_remote_sd(&afe->pads[ADV748X_AFE_SOURCE]);
+	if (!tx)
+		return -ENOLINK;
+
+	/*
+	 * The ADV748x ADC sampling frequency is twice the externally supplied
+	 * clock whose frequency is required to be 28.63636 MHz. It oversamples
+	 * with a factor of 4 resulting in a pixel rate of 14.3180180 MHz.
+	 */
+	return adv748x_csi2_set_pixelrate(tx, 14318180);
+}
+
+static int adv748x_afe_enum_mbus_code(struct v4l2_subdev *sd,
+				      struct v4l2_subdev_state *sd_state,
+				      struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index != 0)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_UYVY8_2X8;
+
+	return 0;
+}
+
+static int adv748x_afe_get_format(struct v4l2_subdev *sd,
+				      struct v4l2_subdev_state *sd_state,
+				      struct v4l2_subdev_format *sdformat)
+{
+	struct adv748x_afe *afe = adv748x_sd_to_afe(sd);
+	struct v4l2_mbus_framefmt *mbusformat;
+
+	/* It makes no sense to get the format of the analog sink pads */
+	if (sdformat->pad != ADV748X_AFE_SOURCE)
+		return -EINVAL;
+
+	if (sdformat->which == V4L2_SUBDEV_FORMAT_TRY) {
+		mbusformat = v4l2_subdev_get_try_format(sd, sd_state,
+							sdformat->pad);
+		sdformat->format = *mbusformat;
+	} else {
+		adv748x_afe_fill_format(afe, &sdformat->format);
+		adv748x_afe_propagate_pixelrate(afe);
+	}
+
+	return 0;
+}
+
+static int adv748x_afe_set_format(struct v4l2_subdev *sd,
+				      struct v4l2_subdev_state *sd_state,
+				      struct v4l2_subdev_format *sdformat)
+{
+	struct v4l2_mbus_framefmt *mbusformat;
+
+	/* It makes no sense to get the format of the analog sink pads */
+	if (sdformat->pad != ADV748X_AFE_SOURCE)
+		return -EINVAL;
+
+	if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		return adv748x_afe_get_format(sd, sd_state, sdformat);
+
+	mbusformat = v4l2_subdev_get_try_format(sd, sd_state, sdformat->pad);
+	*mbusformat = sdformat->format;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_pad_ops adv748x_afe_pad_ops = {
+	.enum_mbus_code = adv748x_afe_enum_mbus_code,
+	.set_fmt = adv748x_afe_set_format,
+	.get_fmt = adv748x_afe_get_format,
+};
+
+/* -----------------------------------------------------------------------------
+ * v4l2_subdev_ops
+ */
+
+static const struct v4l2_subdev_ops adv748x_afe_ops = {
+	.video = &adv748x_afe_video_ops,
+	.pad = &adv748x_afe_pad_ops,
+};
+
+/* -----------------------------------------------------------------------------
+ * Controls
+ */
+
+static const char * const afe_ctrl_frp_menu[] = {
+	"Disabled",
+	"Solid Blue",
+	"Color Bars",
+	"Grey Ramp",
+	"Cb Ramp",
+	"Cr Ramp",
+	"Boundary"
+};
+
+static int adv748x_afe_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct adv748x_afe *afe = adv748x_ctrl_to_afe(ctrl);
+	struct adv748x_state *state = adv748x_afe_to_state(afe);
+	bool enable;
+	int ret;
+
+	ret = sdp_write(state, 0x0e, 0x00);
+	if (ret < 0)
+		return ret;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		ret = sdp_write(state, ADV748X_SDP_BRI, ctrl->val);
+		break;
+	case V4L2_CID_HUE:
+		/* Hue is inverted according to HSL chart */
+		ret = sdp_write(state, ADV748X_SDP_HUE, -ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		ret = sdp_write(state, ADV748X_SDP_CON, ctrl->val);
+		break;
+	case V4L2_CID_SATURATION:
+		ret = sdp_write(state, ADV748X_SDP_SD_SAT_U, ctrl->val);
+		if (ret)
+			break;
+		ret = sdp_write(state, ADV748X_SDP_SD_SAT_V, ctrl->val);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		enable = !!ctrl->val;
+
+		/* Enable/Disable Color bar test patterns */
+		ret = sdp_clrset(state, ADV748X_SDP_DEF, ADV748X_SDP_DEF_VAL_EN,
+				enable);
+		if (ret)
+			break;
+		ret = sdp_clrset(state, ADV748X_SDP_FRP, ADV748X_SDP_FRP_MASK,
+				enable ? ctrl->val - 1 : 0);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops adv748x_afe_ctrl_ops = {
+	.s_ctrl = adv748x_afe_s_ctrl,
+};
+
+static int adv748x_afe_init_controls(struct adv748x_afe *afe)
+{
+	struct adv748x_state *state = adv748x_afe_to_state(afe);
+
+	v4l2_ctrl_handler_init(&afe->ctrl_hdl, 5);
+
+	/* Use our mutex for the controls */
+	afe->ctrl_hdl.lock = &state->mutex;
+
+	v4l2_ctrl_new_std(&afe->ctrl_hdl, &adv748x_afe_ctrl_ops,
+			  V4L2_CID_BRIGHTNESS, ADV748X_SDP_BRI_MIN,
+			  ADV748X_SDP_BRI_MAX, 1, ADV748X_SDP_BRI_DEF);
+	v4l2_ctrl_new_std(&afe->ctrl_hdl, &adv748x_afe_ctrl_ops,
+			  V4L2_CID_CONTRAST, ADV748X_SDP_CON_MIN,
+			  ADV748X_SDP_CON_MAX, 1, ADV748X_SDP_CON_DEF);
+	v4l2_ctrl_new_std(&afe->ctrl_hdl, &adv748x_afe_ctrl_ops,
+			  V4L2_CID_SATURATION, ADV748X_SDP_SAT_MIN,
+			  ADV748X_SDP_SAT_MAX, 1, ADV748X_SDP_SAT_DEF);
+	v4l2_ctrl_new_std(&afe->ctrl_hdl, &adv748x_afe_ctrl_ops,
+			  V4L2_CID_HUE, ADV748X_SDP_HUE_MIN,
+			  ADV748X_SDP_HUE_MAX, 1, ADV748X_SDP_HUE_DEF);
+
+	v4l2_ctrl_new_std_menu_items(&afe->ctrl_hdl, &adv748x_afe_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(afe_ctrl_frp_menu) - 1,
+				     0, 0, afe_ctrl_frp_menu);
+
+	afe->sd.ctrl_handler = &afe->ctrl_hdl;
+	if (afe->ctrl_hdl.error) {
+		v4l2_ctrl_handler_free(&afe->ctrl_hdl);
+		return afe->ctrl_hdl.error;
+	}
+
+	return v4l2_ctrl_handler_setup(&afe->ctrl_hdl);
+}
+
+int adv748x_afe_init(struct adv748x_afe *afe)
+{
+	struct adv748x_state *state = adv748x_afe_to_state(afe);
+	int ret;
+	unsigned int i;
+
+	afe->input = 0;
+	afe->streaming = false;
+	afe->curr_norm = V4L2_STD_NTSC_M;
+
+	adv748x_subdev_init(&afe->sd, state, &adv748x_afe_ops,
+			    MEDIA_ENT_F_ATV_DECODER, "afe");
+
+	/* Identify the first connector found as a default input if set */
+	for (i = ADV748X_PORT_AIN0; i <= ADV748X_PORT_AIN7; i++) {
+		/* Inputs and ports are 1-indexed to match the data sheet */
+		if (state->endpoints[i]) {
+			afe->input = i;
+			break;
+		}
+	}
+
+	adv748x_afe_s_input(afe, afe->input);
+
+	adv_dbg(state, "AFE Default input set to %d\n", afe->input);
+
+	/* Entity pads and sinks are 0-indexed to match the pads */
+	for (i = ADV748X_AFE_SINK_AIN0; i <= ADV748X_AFE_SINK_AIN7; i++)
+		afe->pads[i].flags = MEDIA_PAD_FL_SINK;
+
+	afe->pads[ADV748X_AFE_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
+
+	ret = media_entity_pads_init(&afe->sd.entity, ADV748X_AFE_NR_PADS,
+			afe->pads);
+	if (ret)
+		return ret;
+
+	ret = adv748x_afe_init_controls(afe);
+	if (ret)
+		goto error;
+
+	return 0;
+
+error:
+	media_entity_cleanup(&afe->sd.entity);
+
+	return ret;
+}
+
+void adv748x_afe_cleanup(struct adv748x_afe *afe)
+{
+	v4l2_device_unregister_subdev(&afe->sd);
+	media_entity_cleanup(&afe->sd.entity);
+	v4l2_ctrl_handler_free(&afe->ctrl_hdl);
+}
diff --git a/drivers/media/i2c/adv748x/adv748x-core.c b/drivers/media/i2c/adv748x/adv748x-core.c
new file mode 100644
index 000000000..3eb6d5e8f
--- /dev/null
+++ b/drivers/media/i2c/adv748x/adv748x-core.c
@@ -0,0 +1,858 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Driver for Analog Devices ADV748X HDMI receiver with AFE
+ *
+ * Copyright (C) 2017 Renesas Electronics Corp.
+ *
+ * Authors:
+ *	Koji Matsuoka <koji.matsuoka.xm@renesas.com>
+ *	Niklas Söderlund <niklas.soderlund@ragnatech.se>
+ *	Kieran Bingham <kieran.bingham@ideasonboard.com>
+ */
+
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/of_graph.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+#include <linux/v4l2-dv-timings.h>
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-dv-timings.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-ioctl.h>
+
+#include "adv748x.h"
+
+/* -----------------------------------------------------------------------------
+ * Register manipulation
+ */
+
+#define ADV748X_REGMAP_CONF(n) \
+{ \
+	.name = n, \
+	.reg_bits = 8, \
+	.val_bits = 8, \
+	.max_register = 0xff, \
+	.cache_type = REGCACHE_NONE, \
+}
+
+static const struct regmap_config adv748x_regmap_cnf[] = {
+	ADV748X_REGMAP_CONF("io"),
+	ADV748X_REGMAP_CONF("dpll"),
+	ADV748X_REGMAP_CONF("cp"),
+	ADV748X_REGMAP_CONF("hdmi"),
+	ADV748X_REGMAP_CONF("edid"),
+	ADV748X_REGMAP_CONF("repeater"),
+	ADV748X_REGMAP_CONF("infoframe"),
+	ADV748X_REGMAP_CONF("cbus"),
+	ADV748X_REGMAP_CONF("cec"),
+	ADV748X_REGMAP_CONF("sdp"),
+	ADV748X_REGMAP_CONF("txa"),
+	ADV748X_REGMAP_CONF("txb"),
+};
+
+static int adv748x_configure_regmap(struct adv748x_state *state, int region)
+{
+	int err;
+
+	if (!state->i2c_clients[region])
+		return -ENODEV;
+
+	state->regmap[region] =
+		devm_regmap_init_i2c(state->i2c_clients[region],
+				     &adv748x_regmap_cnf[region]);
+
+	if (IS_ERR(state->regmap[region])) {
+		err = PTR_ERR(state->regmap[region]);
+		adv_err(state,
+			"Error initializing regmap %d with error %d\n",
+			region, err);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+struct adv748x_register_map {
+	const char *name;
+	u8 default_addr;
+};
+
+static const struct adv748x_register_map adv748x_default_addresses[] = {
+	[ADV748X_PAGE_IO] = { "main", 0x70 },
+	[ADV748X_PAGE_DPLL] = { "dpll", 0x26 },
+	[ADV748X_PAGE_CP] = { "cp", 0x22 },
+	[ADV748X_PAGE_HDMI] = { "hdmi", 0x34 },
+	[ADV748X_PAGE_EDID] = { "edid", 0x36 },
+	[ADV748X_PAGE_REPEATER] = { "repeater", 0x32 },
+	[ADV748X_PAGE_INFOFRAME] = { "infoframe", 0x31 },
+	[ADV748X_PAGE_CBUS] = { "cbus", 0x30 },
+	[ADV748X_PAGE_CEC] = { "cec", 0x41 },
+	[ADV748X_PAGE_SDP] = { "sdp", 0x79 },
+	[ADV748X_PAGE_TXB] = { "txb", 0x48 },
+	[ADV748X_PAGE_TXA] = { "txa", 0x4a },
+};
+
+static int adv748x_read_check(struct adv748x_state *state,
+			      int client_page, u8 reg)
+{
+	struct i2c_client *client = state->i2c_clients[client_page];
+	int err;
+	unsigned int val;
+
+	err = regmap_read(state->regmap[client_page], reg, &val);
+
+	if (err) {
+		adv_err(state, "error reading %02x, %02x\n",
+				client->addr, reg);
+		return err;
+	}
+
+	return val;
+}
+
+int adv748x_read(struct adv748x_state *state, u8 page, u8 reg)
+{
+	return adv748x_read_check(state, page, reg);
+}
+
+int adv748x_write(struct adv748x_state *state, u8 page, u8 reg, u8 value)
+{
+	return regmap_write(state->regmap[page], reg, value);
+}
+
+static int adv748x_write_check(struct adv748x_state *state, u8 page, u8 reg,
+			       u8 value, int *error)
+{
+	if (*error)
+		return *error;
+
+	*error = adv748x_write(state, page, reg, value);
+	return *error;
+}
+
+/* adv748x_write_block(): Write raw data with a maximum of I2C_SMBUS_BLOCK_MAX
+ * size to one or more registers.
+ *
+ * A value of zero will be returned on success, a negative errno will
+ * be returned in error cases.
+ */
+int adv748x_write_block(struct adv748x_state *state, int client_page,
+			unsigned int init_reg, const void *val,
+			size_t val_len)
+{
+	struct regmap *regmap = state->regmap[client_page];
+
+	if (val_len > I2C_SMBUS_BLOCK_MAX)
+		val_len = I2C_SMBUS_BLOCK_MAX;
+
+	return regmap_raw_write(regmap, init_reg, val, val_len);
+}
+
+static int adv748x_set_slave_addresses(struct adv748x_state *state)
+{
+	struct i2c_client *client;
+	unsigned int i;
+	u8 io_reg;
+
+	for (i = ADV748X_PAGE_DPLL; i < ADV748X_PAGE_MAX; ++i) {
+		io_reg = ADV748X_IO_SLAVE_ADDR_BASE + i;
+		client = state->i2c_clients[i];
+
+		io_write(state, io_reg, client->addr << 1);
+	}
+
+	return 0;
+}
+
+static void adv748x_unregister_clients(struct adv748x_state *state)
+{
+	unsigned int i;
+
+	for (i = 1; i < ARRAY_SIZE(state->i2c_clients); ++i)
+		i2c_unregister_device(state->i2c_clients[i]);
+}
+
+static int adv748x_initialise_clients(struct adv748x_state *state)
+{
+	unsigned int i;
+	int ret;
+
+	for (i = ADV748X_PAGE_DPLL; i < ADV748X_PAGE_MAX; ++i) {
+		state->i2c_clients[i] = i2c_new_ancillary_device(
+				state->client,
+				adv748x_default_addresses[i].name,
+				adv748x_default_addresses[i].default_addr);
+
+		if (IS_ERR(state->i2c_clients[i])) {
+			adv_err(state, "failed to create i2c client %u\n", i);
+			return PTR_ERR(state->i2c_clients[i]);
+		}
+
+		ret = adv748x_configure_regmap(state, i);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+/**
+ * struct adv748x_reg_value - Register write instruction
+ * @page:		Regmap page identifier
+ * @reg:		I2C register
+ * @value:		value to write to @page at @reg
+ */
+struct adv748x_reg_value {
+	u8 page;
+	u8 reg;
+	u8 value;
+};
+
+static int adv748x_write_regs(struct adv748x_state *state,
+			      const struct adv748x_reg_value *regs)
+{
+	int ret;
+
+	for (; regs->page != ADV748X_PAGE_EOR; regs++) {
+		ret = adv748x_write(state, regs->page, regs->reg, regs->value);
+		if (ret < 0) {
+			adv_err(state, "Error regs page: 0x%02x reg: 0x%02x\n",
+				regs->page, regs->reg);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * TXA and TXB
+ */
+
+static int adv748x_power_up_tx(struct adv748x_csi2 *tx)
+{
+	struct adv748x_state *state = tx->state;
+	u8 page = is_txa(tx) ? ADV748X_PAGE_TXA : ADV748X_PAGE_TXB;
+	int ret = 0;
+
+	/* Enable n-lane MIPI */
+	adv748x_write_check(state, page, 0x00, 0x80 | tx->active_lanes, &ret);
+
+	/* Set Auto DPHY Timing */
+	adv748x_write_check(state, page, 0x00, 0xa0 | tx->active_lanes, &ret);
+
+	/* ADI Required Write */
+	if (tx->src == &state->hdmi.sd) {
+		adv748x_write_check(state, page, 0xdb, 0x10, &ret);
+		adv748x_write_check(state, page, 0xd6, 0x07, &ret);
+	} else {
+		adv748x_write_check(state, page, 0xd2, 0x40, &ret);
+	}
+
+	adv748x_write_check(state, page, 0xc4, 0x0a, &ret);
+	adv748x_write_check(state, page, 0x71, 0x33, &ret);
+	adv748x_write_check(state, page, 0x72, 0x11, &ret);
+
+	/* i2c_dphy_pwdn - 1'b0 */
+	adv748x_write_check(state, page, 0xf0, 0x00, &ret);
+
+	/* ADI Required Writes*/
+	adv748x_write_check(state, page, 0x31, 0x82, &ret);
+	adv748x_write_check(state, page, 0x1e, 0x40, &ret);
+
+	/* i2c_mipi_pll_en - 1'b1 */
+	adv748x_write_check(state, page, 0xda, 0x01, &ret);
+	usleep_range(2000, 2500);
+
+	/* Power-up CSI-TX */
+	adv748x_write_check(state, page, 0x00, 0x20 | tx->active_lanes, &ret);
+	usleep_range(1000, 1500);
+
+	/* ADI Required Writes */
+	adv748x_write_check(state, page, 0xc1, 0x2b, &ret);
+	usleep_range(1000, 1500);
+	adv748x_write_check(state, page, 0x31, 0x80, &ret);
+
+	return ret;
+}
+
+static int adv748x_power_down_tx(struct adv748x_csi2 *tx)
+{
+	struct adv748x_state *state = tx->state;
+	u8 page = is_txa(tx) ? ADV748X_PAGE_TXA : ADV748X_PAGE_TXB;
+	int ret = 0;
+
+	/* ADI Required Writes */
+	adv748x_write_check(state, page, 0x31, 0x82, &ret);
+	adv748x_write_check(state, page, 0x1e, 0x00, &ret);
+
+	/* Enable n-lane MIPI */
+	adv748x_write_check(state, page, 0x00, 0x80 | tx->active_lanes, &ret);
+
+	/* i2c_mipi_pll_en - 1'b1 */
+	adv748x_write_check(state, page, 0xda, 0x01, &ret);
+
+	/* ADI Required Write */
+	adv748x_write_check(state, page, 0xc1, 0x3b, &ret);
+
+	return ret;
+}
+
+int adv748x_tx_power(struct adv748x_csi2 *tx, bool on)
+{
+	int val;
+
+	if (!is_tx_enabled(tx))
+		return 0;
+
+	val = tx_read(tx, ADV748X_CSI_FS_AS_LS);
+	if (val < 0)
+		return val;
+
+	/*
+	 * This test against BIT(6) is not documented by the datasheet, but was
+	 * specified in the downstream driver.
+	 * Track with a WARN_ONCE to determine if it is ever set by HW.
+	 */
+	WARN_ONCE((on && val & ADV748X_CSI_FS_AS_LS_UNKNOWN),
+			"Enabling with unknown bit set");
+
+	return on ? adv748x_power_up_tx(tx) : adv748x_power_down_tx(tx);
+}
+
+/* -----------------------------------------------------------------------------
+ * Media Operations
+ */
+static int adv748x_link_setup(struct media_entity *entity,
+			      const struct media_pad *local,
+			      const struct media_pad *remote, u32 flags)
+{
+	struct v4l2_subdev *rsd = media_entity_to_v4l2_subdev(remote->entity);
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+	struct adv748x_state *state = v4l2_get_subdevdata(sd);
+	struct adv748x_csi2 *tx = adv748x_sd_to_csi2(sd);
+	bool enable = flags & MEDIA_LNK_FL_ENABLED;
+	u8 io10_mask = ADV748X_IO_10_CSI1_EN |
+		       ADV748X_IO_10_CSI4_EN |
+		       ADV748X_IO_10_CSI4_IN_SEL_AFE;
+	u8 io10 = 0;
+
+	/* Refuse to enable multiple links to the same TX at the same time. */
+	if (enable && tx->src)
+		return -EINVAL;
+
+	/* Set or clear the source (HDMI or AFE) and the current TX. */
+	if (rsd == &state->afe.sd)
+		state->afe.tx = enable ? tx : NULL;
+	else
+		state->hdmi.tx = enable ? tx : NULL;
+
+	tx->src = enable ? rsd : NULL;
+
+	if (state->afe.tx) {
+		/* AFE Requires TXA enabled, even when output to TXB */
+		io10 |= ADV748X_IO_10_CSI4_EN;
+		if (is_txa(tx)) {
+			/*
+			 * Output from the SD-core (480i and 576i) from the TXA
+			 * interface requires reducing the number of enabled
+			 * data lanes in order to guarantee a valid link
+			 * frequency.
+			 */
+			tx->active_lanes = min(tx->num_lanes, 2U);
+			io10 |= ADV748X_IO_10_CSI4_IN_SEL_AFE;
+		} else {
+			/* TXB has a single data lane, no need to adjust. */
+			io10 |= ADV748X_IO_10_CSI1_EN;
+		}
+	}
+
+	if (state->hdmi.tx) {
+		/*
+		 * Restore the number of active lanes, in case we have gone
+		 * through an AFE->TXA streaming sessions.
+		 */
+		tx->active_lanes = tx->num_lanes;
+		io10 |= ADV748X_IO_10_CSI4_EN;
+	}
+
+	return io_clrset(state, ADV748X_IO_10, io10_mask, io10);
+}
+
+static const struct media_entity_operations adv748x_tx_media_ops = {
+	.link_setup	= adv748x_link_setup,
+	.link_validate	= v4l2_subdev_link_validate,
+};
+
+static const struct media_entity_operations adv748x_media_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+/* -----------------------------------------------------------------------------
+ * HW setup
+ */
+
+/* Initialize CP Core with RGB888 format. */
+static const struct adv748x_reg_value adv748x_init_hdmi[] = {
+	/* Disable chip powerdown & Enable HDMI Rx block */
+	{ADV748X_PAGE_IO, 0x00, 0x40},
+
+	{ADV748X_PAGE_REPEATER, 0x40, 0x83}, /* Enable HDCP 1.1 */
+
+	{ADV748X_PAGE_HDMI, 0x00, 0x08},/* Foreground Channel = A */
+	{ADV748X_PAGE_HDMI, 0x98, 0xff},/* ADI Required Write */
+	{ADV748X_PAGE_HDMI, 0x99, 0xa3},/* ADI Required Write */
+	{ADV748X_PAGE_HDMI, 0x9a, 0x00},/* ADI Required Write */
+	{ADV748X_PAGE_HDMI, 0x9b, 0x0a},/* ADI Required Write */
+	{ADV748X_PAGE_HDMI, 0x9d, 0x40},/* ADI Required Write */
+	{ADV748X_PAGE_HDMI, 0xcb, 0x09},/* ADI Required Write */
+	{ADV748X_PAGE_HDMI, 0x3d, 0x10},/* ADI Required Write */
+	{ADV748X_PAGE_HDMI, 0x3e, 0x7b},/* ADI Required Write */
+	{ADV748X_PAGE_HDMI, 0x3f, 0x5e},/* ADI Required Write */
+	{ADV748X_PAGE_HDMI, 0x4e, 0xfe},/* ADI Required Write */
+	{ADV748X_PAGE_HDMI, 0x4f, 0x18},/* ADI Required Write */
+	{ADV748X_PAGE_HDMI, 0x57, 0xa3},/* ADI Required Write */
+	{ADV748X_PAGE_HDMI, 0x58, 0x04},/* ADI Required Write */
+	{ADV748X_PAGE_HDMI, 0x85, 0x10},/* ADI Required Write */
+
+	{ADV748X_PAGE_HDMI, 0x83, 0x00},/* Enable All Terminations */
+	{ADV748X_PAGE_HDMI, 0xa3, 0x01},/* ADI Required Write */
+	{ADV748X_PAGE_HDMI, 0xbe, 0x00},/* ADI Required Write */
+
+	{ADV748X_PAGE_HDMI, 0x6c, 0x01},/* HPA Manual Enable */
+	{ADV748X_PAGE_HDMI, 0xf8, 0x01},/* HPA Asserted */
+	{ADV748X_PAGE_HDMI, 0x0f, 0x00},/* Audio Mute Speed Set to Fastest */
+	/* (Smallest Step Size) */
+
+	{ADV748X_PAGE_IO, 0x04, 0x02},	/* RGB Out of CP */
+	{ADV748X_PAGE_IO, 0x12, 0xf0},	/* CSC Depends on ip Packets, SDR 444 */
+	{ADV748X_PAGE_IO, 0x17, 0x80},	/* Luma & Chroma can reach 254d */
+	{ADV748X_PAGE_IO, 0x03, 0x86},	/* CP-Insert_AV_Code */
+
+	{ADV748X_PAGE_CP, 0x7c, 0x00},	/* ADI Required Write */
+
+	{ADV748X_PAGE_IO, 0x0c, 0xe0},	/* Enable LLC_DLL & Double LLC Timing */
+	{ADV748X_PAGE_IO, 0x0e, 0xdd},	/* LLC/PIX/SPI PINS TRISTATED AUD */
+
+	{ADV748X_PAGE_EOR, 0xff, 0xff}	/* End of register table */
+};
+
+/* Initialize AFE core with YUV8 format. */
+static const struct adv748x_reg_value adv748x_init_afe[] = {
+	{ADV748X_PAGE_IO, 0x00, 0x30},	/* Disable chip powerdown Rx */
+	{ADV748X_PAGE_IO, 0xf2, 0x01},	/* Enable I2C Read Auto-Increment */
+
+	{ADV748X_PAGE_IO, 0x0e, 0xff},	/* LLC/PIX/AUD/SPI PINS TRISTATED */
+
+	{ADV748X_PAGE_SDP, 0x0f, 0x00},	/* Exit Power Down Mode */
+	{ADV748X_PAGE_SDP, 0x52, 0xcd},	/* ADI Required Write */
+
+	{ADV748X_PAGE_SDP, 0x0e, 0x80},	/* ADI Required Write */
+	{ADV748X_PAGE_SDP, 0x9c, 0x00},	/* ADI Required Write */
+	{ADV748X_PAGE_SDP, 0x9c, 0xff},	/* ADI Required Write */
+	{ADV748X_PAGE_SDP, 0x0e, 0x00},	/* ADI Required Write */
+
+	/* ADI recommended writes for improved video quality */
+	{ADV748X_PAGE_SDP, 0x80, 0x51},	/* ADI Required Write */
+	{ADV748X_PAGE_SDP, 0x81, 0x51},	/* ADI Required Write */
+	{ADV748X_PAGE_SDP, 0x82, 0x68},	/* ADI Required Write */
+
+	{ADV748X_PAGE_SDP, 0x03, 0x42},	/* Tri-S Output , PwrDwn 656 pads */
+	{ADV748X_PAGE_SDP, 0x04, 0xb5},	/* ITU-R BT.656-4 compatible */
+	{ADV748X_PAGE_SDP, 0x13, 0x00},	/* ADI Required Write */
+
+	{ADV748X_PAGE_SDP, 0x17, 0x41},	/* Select SH1 */
+	{ADV748X_PAGE_SDP, 0x31, 0x12},	/* ADI Required Write */
+	{ADV748X_PAGE_SDP, 0xe6, 0x4f},  /* V bit end pos manually in NTSC */
+
+	{ADV748X_PAGE_EOR, 0xff, 0xff}	/* End of register table */
+};
+
+static int adv748x_sw_reset(struct adv748x_state *state)
+{
+	int ret;
+
+	ret = io_write(state, ADV748X_IO_REG_FF, ADV748X_IO_REG_FF_MAIN_RESET);
+	if (ret)
+		return ret;
+
+	usleep_range(5000, 6000);
+
+	/* Disable CEC Wakeup from power-down mode */
+	ret = io_clrset(state, ADV748X_IO_REG_01, ADV748X_IO_REG_01_PWRDN_MASK,
+			ADV748X_IO_REG_01_PWRDNB);
+	if (ret)
+		return ret;
+
+	/* Enable I2C Read Auto-Increment for consecutive reads */
+	return io_write(state, ADV748X_IO_REG_F2,
+			ADV748X_IO_REG_F2_READ_AUTO_INC);
+}
+
+static int adv748x_reset(struct adv748x_state *state)
+{
+	int ret;
+	u8 regval = 0;
+
+	ret = adv748x_sw_reset(state);
+	if (ret < 0)
+		return ret;
+
+	ret = adv748x_set_slave_addresses(state);
+	if (ret < 0)
+		return ret;
+
+	/* Initialize CP and AFE cores. */
+	ret = adv748x_write_regs(state, adv748x_init_hdmi);
+	if (ret)
+		return ret;
+
+	ret = adv748x_write_regs(state, adv748x_init_afe);
+	if (ret)
+		return ret;
+
+	adv748x_afe_s_input(&state->afe, state->afe.input);
+
+	adv_dbg(state, "AFE Default input set to %d\n", state->afe.input);
+
+	/* Reset TXA and TXB */
+	adv748x_tx_power(&state->txa, 1);
+	adv748x_tx_power(&state->txa, 0);
+	adv748x_tx_power(&state->txb, 1);
+	adv748x_tx_power(&state->txb, 0);
+
+	/* Disable chip powerdown & Enable HDMI Rx block */
+	io_write(state, ADV748X_IO_PD, ADV748X_IO_PD_RX_EN);
+
+	/* Conditionally enable TXa and TXb. */
+	if (is_tx_enabled(&state->txa)) {
+		regval |= ADV748X_IO_10_CSI4_EN;
+		adv748x_csi2_set_virtual_channel(&state->txa, 0);
+	}
+	if (is_tx_enabled(&state->txb)) {
+		regval |= ADV748X_IO_10_CSI1_EN;
+		adv748x_csi2_set_virtual_channel(&state->txb, 0);
+	}
+	io_write(state, ADV748X_IO_10, regval);
+
+	/* Use vid_std and v_freq as freerun resolution for CP */
+	cp_clrset(state, ADV748X_CP_CLMP_POS, ADV748X_CP_CLMP_POS_DIS_AUTO,
+					      ADV748X_CP_CLMP_POS_DIS_AUTO);
+
+	return 0;
+}
+
+static int adv748x_identify_chip(struct adv748x_state *state)
+{
+	int msb, lsb;
+
+	lsb = io_read(state, ADV748X_IO_CHIP_REV_ID_1);
+	msb = io_read(state, ADV748X_IO_CHIP_REV_ID_2);
+
+	if (lsb < 0 || msb < 0) {
+		adv_err(state, "Failed to read chip revision\n");
+		return -EIO;
+	}
+
+	adv_info(state, "chip found @ 0x%02x revision %02x%02x\n",
+		 state->client->addr << 1, lsb, msb);
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * Suspend / Resume
+ */
+
+static int __maybe_unused adv748x_resume_early(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct adv748x_state *state = i2c_get_clientdata(client);
+
+	return adv748x_reset(state);
+}
+
+/* -----------------------------------------------------------------------------
+ * i2c driver
+ */
+
+void adv748x_subdev_init(struct v4l2_subdev *sd, struct adv748x_state *state,
+			 const struct v4l2_subdev_ops *ops, u32 function,
+			 const char *ident)
+{
+	v4l2_subdev_init(sd, ops);
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	/* the owner is the same as the i2c_client's driver owner */
+	sd->owner = state->dev->driver->owner;
+	sd->dev = state->dev;
+
+	v4l2_set_subdevdata(sd, state);
+
+	/* initialize name */
+	snprintf(sd->name, sizeof(sd->name), "%s %d-%04x %s",
+		state->dev->driver->name,
+		i2c_adapter_id(state->client->adapter),
+		state->client->addr, ident);
+
+	sd->entity.function = function;
+	sd->entity.ops = is_tx(adv748x_sd_to_csi2(sd)) ?
+			 &adv748x_tx_media_ops : &adv748x_media_ops;
+}
+
+static int adv748x_parse_csi2_lanes(struct adv748x_state *state,
+				    unsigned int port,
+				    struct device_node *ep)
+{
+	struct v4l2_fwnode_endpoint vep = { .bus_type = V4L2_MBUS_CSI2_DPHY };
+	unsigned int num_lanes;
+	int ret;
+
+	if (port != ADV748X_PORT_TXA && port != ADV748X_PORT_TXB)
+		return 0;
+
+	ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(ep), &vep);
+	if (ret)
+		return ret;
+
+	num_lanes = vep.bus.mipi_csi2.num_data_lanes;
+
+	if (vep.base.port == ADV748X_PORT_TXA) {
+		if (num_lanes != 1 && num_lanes != 2 && num_lanes != 4) {
+			adv_err(state, "TXA: Invalid number (%u) of lanes\n",
+				num_lanes);
+			return -EINVAL;
+		}
+
+		state->txa.num_lanes = num_lanes;
+		state->txa.active_lanes = num_lanes;
+		adv_dbg(state, "TXA: using %u lanes\n", state->txa.num_lanes);
+	}
+
+	if (vep.base.port == ADV748X_PORT_TXB) {
+		if (num_lanes != 1) {
+			adv_err(state, "TXB: Invalid number (%u) of lanes\n",
+				num_lanes);
+			return -EINVAL;
+		}
+
+		state->txb.num_lanes = num_lanes;
+		state->txb.active_lanes = num_lanes;
+		adv_dbg(state, "TXB: using %u lanes\n", state->txb.num_lanes);
+	}
+
+	return 0;
+}
+
+static int adv748x_parse_dt(struct adv748x_state *state)
+{
+	struct device_node *ep_np = NULL;
+	struct of_endpoint ep;
+	bool out_found = false;
+	bool in_found = false;
+	int ret;
+
+	for_each_endpoint_of_node(state->dev->of_node, ep_np) {
+		of_graph_parse_endpoint(ep_np, &ep);
+		adv_info(state, "Endpoint %pOF on port %d", ep.local_node,
+			 ep.port);
+
+		if (ep.port >= ADV748X_PORT_MAX) {
+			adv_err(state, "Invalid endpoint %pOF on port %d",
+				ep.local_node, ep.port);
+
+			continue;
+		}
+
+		if (state->endpoints[ep.port]) {
+			adv_err(state,
+				"Multiple port endpoints are not supported");
+			continue;
+		}
+
+		of_node_get(ep_np);
+		state->endpoints[ep.port] = ep_np;
+
+		/*
+		 * At least one input endpoint and one output endpoint shall
+		 * be defined.
+		 */
+		if (ep.port < ADV748X_PORT_TXA)
+			in_found = true;
+		else
+			out_found = true;
+
+		/* Store number of CSI-2 lanes used for TXA and TXB. */
+		ret = adv748x_parse_csi2_lanes(state, ep.port, ep_np);
+		if (ret)
+			return ret;
+	}
+
+	return in_found && out_found ? 0 : -ENODEV;
+}
+
+static void adv748x_dt_cleanup(struct adv748x_state *state)
+{
+	unsigned int i;
+
+	for (i = 0; i < ADV748X_PORT_MAX; i++)
+		of_node_put(state->endpoints[i]);
+}
+
+static int adv748x_probe(struct i2c_client *client)
+{
+	struct adv748x_state *state;
+	int ret;
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+
+	state = devm_kzalloc(&client->dev, sizeof(*state), GFP_KERNEL);
+	if (!state)
+		return -ENOMEM;
+
+	mutex_init(&state->mutex);
+
+	state->dev = &client->dev;
+	state->client = client;
+	state->i2c_clients[ADV748X_PAGE_IO] = client;
+	i2c_set_clientdata(client, state);
+
+	/*
+	 * We can not use container_of to get back to the state with two TXs;
+	 * Initialize the TXs's fields unconditionally on the endpoint
+	 * presence to access them later.
+	 */
+	state->txa.state = state->txb.state = state;
+	state->txa.page = ADV748X_PAGE_TXA;
+	state->txb.page = ADV748X_PAGE_TXB;
+	state->txa.port = ADV748X_PORT_TXA;
+	state->txb.port = ADV748X_PORT_TXB;
+
+	/* Discover and process ports declared by the Device tree endpoints */
+	ret = adv748x_parse_dt(state);
+	if (ret) {
+		adv_err(state, "Failed to parse device tree");
+		goto err_free_mutex;
+	}
+
+	/* Configure IO Regmap region */
+	ret = adv748x_configure_regmap(state, ADV748X_PAGE_IO);
+	if (ret) {
+		adv_err(state, "Error configuring IO regmap region");
+		goto err_cleanup_dt;
+	}
+
+	ret = adv748x_identify_chip(state);
+	if (ret) {
+		adv_err(state, "Failed to identify chip");
+		goto err_cleanup_dt;
+	}
+
+	/* Configure remaining pages as I2C clients with regmap access */
+	ret = adv748x_initialise_clients(state);
+	if (ret) {
+		adv_err(state, "Failed to setup client regmap pages");
+		goto err_cleanup_clients;
+	}
+
+	/* SW reset ADV748X to its default values */
+	ret = adv748x_reset(state);
+	if (ret) {
+		adv_err(state, "Failed to reset hardware");
+		goto err_cleanup_clients;
+	}
+
+	/* Initialise HDMI */
+	ret = adv748x_hdmi_init(&state->hdmi);
+	if (ret) {
+		adv_err(state, "Failed to probe HDMI");
+		goto err_cleanup_clients;
+	}
+
+	/* Initialise AFE */
+	ret = adv748x_afe_init(&state->afe);
+	if (ret) {
+		adv_err(state, "Failed to probe AFE");
+		goto err_cleanup_hdmi;
+	}
+
+	/* Initialise TXA */
+	ret = adv748x_csi2_init(state, &state->txa);
+	if (ret) {
+		adv_err(state, "Failed to probe TXA");
+		goto err_cleanup_afe;
+	}
+
+	/* Initialise TXB */
+	ret = adv748x_csi2_init(state, &state->txb);
+	if (ret) {
+		adv_err(state, "Failed to probe TXB");
+		goto err_cleanup_txa;
+	}
+
+	return 0;
+
+err_cleanup_txa:
+	adv748x_csi2_cleanup(&state->txa);
+err_cleanup_afe:
+	adv748x_afe_cleanup(&state->afe);
+err_cleanup_hdmi:
+	adv748x_hdmi_cleanup(&state->hdmi);
+err_cleanup_clients:
+	adv748x_unregister_clients(state);
+err_cleanup_dt:
+	adv748x_dt_cleanup(state);
+err_free_mutex:
+	mutex_destroy(&state->mutex);
+
+	return ret;
+}
+
+static void adv748x_remove(struct i2c_client *client)
+{
+	struct adv748x_state *state = i2c_get_clientdata(client);
+
+	adv748x_afe_cleanup(&state->afe);
+	adv748x_hdmi_cleanup(&state->hdmi);
+
+	adv748x_csi2_cleanup(&state->txa);
+	adv748x_csi2_cleanup(&state->txb);
+
+	adv748x_unregister_clients(state);
+	adv748x_dt_cleanup(state);
+	mutex_destroy(&state->mutex);
+}
+
+static const struct of_device_id adv748x_of_table[] = {
+	{ .compatible = "adi,adv7481", },
+	{ .compatible = "adi,adv7482", },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, adv748x_of_table);
+
+static const struct dev_pm_ops adv748x_pm_ops = {
+	SET_LATE_SYSTEM_SLEEP_PM_OPS(NULL, adv748x_resume_early)
+};
+
+static struct i2c_driver adv748x_driver = {
+	.driver = {
+		.name = "adv748x",
+		.of_match_table = adv748x_of_table,
+		.pm = &adv748x_pm_ops,
+	},
+	.probe = adv748x_probe,
+	.remove = adv748x_remove,
+};
+
+module_i2c_driver(adv748x_driver);
+
+MODULE_AUTHOR("Kieran Bingham <kieran.bingham@ideasonboard.com>");
+MODULE_DESCRIPTION("ADV748X video decoder");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/adv748x/adv748x-csi2.c b/drivers/media/i2c/adv748x/adv748x-csi2.c
new file mode 100644
index 000000000..a5a7cb228
--- /dev/null
+++ b/drivers/media/i2c/adv748x/adv748x-csi2.c
@@ -0,0 +1,348 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Driver for Analog Devices ADV748X CSI-2 Transmitter
+ *
+ * Copyright (C) 2017 Renesas Electronics Corp.
+ */
+
+#include <linux/module.h>
+#include <linux/mutex.h>
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+
+#include "adv748x.h"
+
+int adv748x_csi2_set_virtual_channel(struct adv748x_csi2 *tx, unsigned int vc)
+{
+	return tx_write(tx, ADV748X_CSI_VC_REF, vc << ADV748X_CSI_VC_REF_SHIFT);
+}
+
+/**
+ * adv748x_csi2_register_link : Register and link internal entities
+ *
+ * @tx: CSI2 private entity
+ * @v4l2_dev: Video registration device
+ * @src: Source subdevice to establish link
+ * @src_pad: Pad number of source to link to this @tx
+ * @enable: Link enabled flag
+ *
+ * Ensure that the subdevice is registered against the v4l2_device, and link the
+ * source pad to the sink pad of the CSI2 bus entity.
+ */
+static int adv748x_csi2_register_link(struct adv748x_csi2 *tx,
+				      struct v4l2_device *v4l2_dev,
+				      struct v4l2_subdev *src,
+				      unsigned int src_pad,
+				      bool enable)
+{
+	int ret;
+
+	if (!src->v4l2_dev) {
+		ret = v4l2_device_register_subdev(v4l2_dev, src);
+		if (ret)
+			return ret;
+	}
+
+	ret = media_create_pad_link(&src->entity, src_pad,
+				    &tx->sd.entity, ADV748X_CSI2_SINK,
+				    enable ? MEDIA_LNK_FL_ENABLED : 0);
+	if (ret)
+		return ret;
+
+	if (enable)
+		tx->src = src;
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * v4l2_subdev_internal_ops
+ *
+ * We use the internal registered operation to be able to ensure that our
+ * incremental subdevices (not connected in the forward path) can be registered
+ * against the resulting video path and media device.
+ */
+
+static int adv748x_csi2_registered(struct v4l2_subdev *sd)
+{
+	struct adv748x_csi2 *tx = adv748x_sd_to_csi2(sd);
+	struct adv748x_state *state = tx->state;
+	int ret;
+
+	adv_dbg(state, "Registered %s (%s)", is_txa(tx) ? "TXA":"TXB",
+			sd->name);
+
+	/*
+	 * Link TXA to AFE and HDMI, and TXB to AFE only as TXB cannot output
+	 * HDMI.
+	 *
+	 * The HDMI->TXA link is enabled by default, as is the AFE->TXB one.
+	 */
+	if (is_afe_enabled(state)) {
+		ret = adv748x_csi2_register_link(tx, sd->v4l2_dev,
+						 &state->afe.sd,
+						 ADV748X_AFE_SOURCE,
+						 is_txb(tx));
+		if (ret)
+			return ret;
+
+		/* TXB can output AFE signals only. */
+		if (is_txb(tx))
+			state->afe.tx = tx;
+	}
+
+	/* Register link to HDMI for TXA only. */
+	if (is_txb(tx) || !is_hdmi_enabled(state))
+		return 0;
+
+	ret = adv748x_csi2_register_link(tx, sd->v4l2_dev, &state->hdmi.sd,
+					 ADV748X_HDMI_SOURCE, true);
+	if (ret)
+		return ret;
+
+	/* The default HDMI output is TXA. */
+	state->hdmi.tx = tx;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_internal_ops adv748x_csi2_internal_ops = {
+	.registered = adv748x_csi2_registered,
+};
+
+/* -----------------------------------------------------------------------------
+ * v4l2_subdev_video_ops
+ */
+
+static int adv748x_csi2_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct adv748x_csi2 *tx = adv748x_sd_to_csi2(sd);
+	struct v4l2_subdev *src;
+
+	src = adv748x_get_remote_sd(&tx->pads[ADV748X_CSI2_SINK]);
+	if (!src)
+		return -EPIPE;
+
+	return v4l2_subdev_call(src, video, s_stream, enable);
+}
+
+static const struct v4l2_subdev_video_ops adv748x_csi2_video_ops = {
+	.s_stream = adv748x_csi2_s_stream,
+};
+
+/* -----------------------------------------------------------------------------
+ * v4l2_subdev_pad_ops
+ *
+ * The CSI2 bus pads are ignorant to the data sizes or formats.
+ * But we must support setting the pad formats for format propagation.
+ */
+
+static struct v4l2_mbus_framefmt *
+adv748x_csi2_get_pad_format(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_state *sd_state,
+			    unsigned int pad, u32 which)
+{
+	struct adv748x_csi2 *tx = adv748x_sd_to_csi2(sd);
+
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(sd, sd_state, pad);
+
+	return &tx->format;
+}
+
+static int adv748x_csi2_get_format(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_format *sdformat)
+{
+	struct adv748x_csi2 *tx = adv748x_sd_to_csi2(sd);
+	struct adv748x_state *state = tx->state;
+	struct v4l2_mbus_framefmt *mbusformat;
+
+	mbusformat = adv748x_csi2_get_pad_format(sd, sd_state, sdformat->pad,
+						 sdformat->which);
+	if (!mbusformat)
+		return -EINVAL;
+
+	mutex_lock(&state->mutex);
+
+	sdformat->format = *mbusformat;
+
+	mutex_unlock(&state->mutex);
+
+	return 0;
+}
+
+static int adv748x_csi2_set_format(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_format *sdformat)
+{
+	struct adv748x_csi2 *tx = adv748x_sd_to_csi2(sd);
+	struct adv748x_state *state = tx->state;
+	struct v4l2_mbus_framefmt *mbusformat;
+	int ret = 0;
+
+	mbusformat = adv748x_csi2_get_pad_format(sd, sd_state, sdformat->pad,
+						 sdformat->which);
+	if (!mbusformat)
+		return -EINVAL;
+
+	mutex_lock(&state->mutex);
+
+	if (sdformat->pad == ADV748X_CSI2_SOURCE) {
+		const struct v4l2_mbus_framefmt *sink_fmt;
+
+		sink_fmt = adv748x_csi2_get_pad_format(sd, sd_state,
+						       ADV748X_CSI2_SINK,
+						       sdformat->which);
+
+		if (!sink_fmt) {
+			ret = -EINVAL;
+			goto unlock;
+		}
+
+		sdformat->format = *sink_fmt;
+	}
+
+	*mbusformat = sdformat->format;
+
+unlock:
+	mutex_unlock(&state->mutex);
+
+	return ret;
+}
+
+static int adv748x_csi2_get_mbus_config(struct v4l2_subdev *sd, unsigned int pad,
+					struct v4l2_mbus_config *config)
+{
+	struct adv748x_csi2 *tx = adv748x_sd_to_csi2(sd);
+
+	if (pad != ADV748X_CSI2_SOURCE)
+		return -EINVAL;
+
+	config->type = V4L2_MBUS_CSI2_DPHY;
+	config->bus.mipi_csi2.num_data_lanes = tx->active_lanes;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_pad_ops adv748x_csi2_pad_ops = {
+	.get_fmt = adv748x_csi2_get_format,
+	.set_fmt = adv748x_csi2_set_format,
+	.get_mbus_config = adv748x_csi2_get_mbus_config,
+};
+
+/* -----------------------------------------------------------------------------
+ * v4l2_subdev_ops
+ */
+
+static const struct v4l2_subdev_ops adv748x_csi2_ops = {
+	.video = &adv748x_csi2_video_ops,
+	.pad = &adv748x_csi2_pad_ops,
+};
+
+/* -----------------------------------------------------------------------------
+ * Subdev module and controls
+ */
+
+int adv748x_csi2_set_pixelrate(struct v4l2_subdev *sd, s64 rate)
+{
+	struct adv748x_csi2 *tx = adv748x_sd_to_csi2(sd);
+
+	if (!tx->pixel_rate)
+		return -EINVAL;
+
+	return v4l2_ctrl_s_ctrl_int64(tx->pixel_rate, rate);
+}
+
+static int adv748x_csi2_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	switch (ctrl->id) {
+	case V4L2_CID_PIXEL_RATE:
+		return 0;
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct v4l2_ctrl_ops adv748x_csi2_ctrl_ops = {
+	.s_ctrl = adv748x_csi2_s_ctrl,
+};
+
+static int adv748x_csi2_init_controls(struct adv748x_csi2 *tx)
+{
+
+	v4l2_ctrl_handler_init(&tx->ctrl_hdl, 1);
+
+	tx->pixel_rate = v4l2_ctrl_new_std(&tx->ctrl_hdl,
+					   &adv748x_csi2_ctrl_ops,
+					   V4L2_CID_PIXEL_RATE, 1, INT_MAX,
+					   1, 1);
+
+	tx->sd.ctrl_handler = &tx->ctrl_hdl;
+	if (tx->ctrl_hdl.error) {
+		v4l2_ctrl_handler_free(&tx->ctrl_hdl);
+		return tx->ctrl_hdl.error;
+	}
+
+	return v4l2_ctrl_handler_setup(&tx->ctrl_hdl);
+}
+
+int adv748x_csi2_init(struct adv748x_state *state, struct adv748x_csi2 *tx)
+{
+	int ret;
+
+	if (!is_tx_enabled(tx))
+		return 0;
+
+	adv748x_subdev_init(&tx->sd, state, &adv748x_csi2_ops,
+			    MEDIA_ENT_F_VID_IF_BRIDGE,
+			    is_txa(tx) ? "txa" : "txb");
+
+	/* Register internal ops for incremental subdev registration */
+	tx->sd.internal_ops = &adv748x_csi2_internal_ops;
+
+	tx->pads[ADV748X_CSI2_SINK].flags = MEDIA_PAD_FL_SINK;
+	tx->pads[ADV748X_CSI2_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
+
+	ret = media_entity_pads_init(&tx->sd.entity, ADV748X_CSI2_NR_PADS,
+				     tx->pads);
+	if (ret)
+		return ret;
+
+	ret = v4l2_async_subdev_endpoint_add(&tx->sd,
+					     of_fwnode_handle(state->endpoints[tx->port]));
+	if (ret)
+		goto err_free_media;
+
+	ret = adv748x_csi2_init_controls(tx);
+	if (ret)
+		goto err_cleanup_subdev;
+
+	ret = v4l2_async_register_subdev(&tx->sd);
+	if (ret)
+		goto err_free_ctrl;
+
+	return 0;
+
+err_free_ctrl:
+	v4l2_ctrl_handler_free(&tx->ctrl_hdl);
+err_cleanup_subdev:
+	v4l2_subdev_cleanup(&tx->sd);
+err_free_media:
+	media_entity_cleanup(&tx->sd.entity);
+
+	return ret;
+}
+
+void adv748x_csi2_cleanup(struct adv748x_csi2 *tx)
+{
+	if (!is_tx_enabled(tx))
+		return;
+
+	v4l2_async_unregister_subdev(&tx->sd);
+	media_entity_cleanup(&tx->sd.entity);
+	v4l2_ctrl_handler_free(&tx->ctrl_hdl);
+	v4l2_subdev_cleanup(&tx->sd);
+}
diff --git a/drivers/media/i2c/adv748x/adv748x-hdmi.c b/drivers/media/i2c/adv748x/adv748x-hdmi.c
new file mode 100644
index 000000000..400d71c27
--- /dev/null
+++ b/drivers/media/i2c/adv748x/adv748x-hdmi.c
@@ -0,0 +1,771 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Driver for Analog Devices ADV748X HDMI receiver and Component Processor (CP)
+ *
+ * Copyright (C) 2017 Renesas Electronics Corp.
+ */
+
+#include <linux/module.h>
+#include <linux/mutex.h>
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-dv-timings.h>
+#include <media/v4l2-ioctl.h>
+
+#include <uapi/linux/v4l2-dv-timings.h>
+
+#include "adv748x.h"
+
+/* -----------------------------------------------------------------------------
+ * HDMI and CP
+ */
+
+#define ADV748X_HDMI_MIN_WIDTH		640
+#define ADV748X_HDMI_MAX_WIDTH		1920
+#define ADV748X_HDMI_MIN_HEIGHT		480
+#define ADV748X_HDMI_MAX_HEIGHT		1200
+
+/* V4L2_DV_BT_CEA_720X480I59_94 - 0.5 MHz */
+#define ADV748X_HDMI_MIN_PIXELCLOCK	13000000
+/* V4L2_DV_BT_DMT_1600X1200P60 */
+#define ADV748X_HDMI_MAX_PIXELCLOCK	162000000
+
+static const struct v4l2_dv_timings_cap adv748x_hdmi_timings_cap = {
+	.type = V4L2_DV_BT_656_1120,
+	/* keep this initialization for compatibility with GCC < 4.4.6 */
+	.reserved = { 0 },
+
+	V4L2_INIT_BT_TIMINGS(ADV748X_HDMI_MIN_WIDTH, ADV748X_HDMI_MAX_WIDTH,
+			     ADV748X_HDMI_MIN_HEIGHT, ADV748X_HDMI_MAX_HEIGHT,
+			     ADV748X_HDMI_MIN_PIXELCLOCK,
+			     ADV748X_HDMI_MAX_PIXELCLOCK,
+			     V4L2_DV_BT_STD_CEA861 | V4L2_DV_BT_STD_DMT,
+			     V4L2_DV_BT_CAP_PROGRESSIVE)
+};
+
+struct adv748x_hdmi_video_standards {
+	struct v4l2_dv_timings timings;
+	u8 vid_std;
+	u8 v_freq;
+};
+
+static const struct adv748x_hdmi_video_standards
+adv748x_hdmi_video_standards[] = {
+	{ V4L2_DV_BT_CEA_720X480P59_94, 0x4a, 0x00 },
+	{ V4L2_DV_BT_CEA_720X576P50, 0x4b, 0x00 },
+	{ V4L2_DV_BT_CEA_1280X720P60, 0x53, 0x00 },
+	{ V4L2_DV_BT_CEA_1280X720P50, 0x53, 0x01 },
+	{ V4L2_DV_BT_CEA_1280X720P30, 0x53, 0x02 },
+	{ V4L2_DV_BT_CEA_1280X720P25, 0x53, 0x03 },
+	{ V4L2_DV_BT_CEA_1280X720P24, 0x53, 0x04 },
+	{ V4L2_DV_BT_CEA_1920X1080P60, 0x5e, 0x00 },
+	{ V4L2_DV_BT_CEA_1920X1080P50, 0x5e, 0x01 },
+	{ V4L2_DV_BT_CEA_1920X1080P30, 0x5e, 0x02 },
+	{ V4L2_DV_BT_CEA_1920X1080P25, 0x5e, 0x03 },
+	{ V4L2_DV_BT_CEA_1920X1080P24, 0x5e, 0x04 },
+	/* SVGA */
+	{ V4L2_DV_BT_DMT_800X600P56, 0x80, 0x00 },
+	{ V4L2_DV_BT_DMT_800X600P60, 0x81, 0x00 },
+	{ V4L2_DV_BT_DMT_800X600P72, 0x82, 0x00 },
+	{ V4L2_DV_BT_DMT_800X600P75, 0x83, 0x00 },
+	{ V4L2_DV_BT_DMT_800X600P85, 0x84, 0x00 },
+	/* SXGA */
+	{ V4L2_DV_BT_DMT_1280X1024P60, 0x85, 0x00 },
+	{ V4L2_DV_BT_DMT_1280X1024P75, 0x86, 0x00 },
+	/* VGA */
+	{ V4L2_DV_BT_DMT_640X480P60, 0x88, 0x00 },
+	{ V4L2_DV_BT_DMT_640X480P72, 0x89, 0x00 },
+	{ V4L2_DV_BT_DMT_640X480P75, 0x8a, 0x00 },
+	{ V4L2_DV_BT_DMT_640X480P85, 0x8b, 0x00 },
+	/* XGA */
+	{ V4L2_DV_BT_DMT_1024X768P60, 0x8c, 0x00 },
+	{ V4L2_DV_BT_DMT_1024X768P70, 0x8d, 0x00 },
+	{ V4L2_DV_BT_DMT_1024X768P75, 0x8e, 0x00 },
+	{ V4L2_DV_BT_DMT_1024X768P85, 0x8f, 0x00 },
+	/* UXGA */
+	{ V4L2_DV_BT_DMT_1600X1200P60, 0x96, 0x00 },
+};
+
+static void adv748x_hdmi_fill_format(struct adv748x_hdmi *hdmi,
+				     struct v4l2_mbus_framefmt *fmt)
+{
+	memset(fmt, 0, sizeof(*fmt));
+
+	fmt->code = MEDIA_BUS_FMT_RGB888_1X24;
+	fmt->field = hdmi->timings.bt.interlaced ?
+			V4L2_FIELD_ALTERNATE : V4L2_FIELD_NONE;
+
+	/* TODO: The colorspace depends on the AVI InfoFrame contents */
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+
+	fmt->width = hdmi->timings.bt.width;
+	fmt->height = hdmi->timings.bt.height;
+
+	if (fmt->field == V4L2_FIELD_ALTERNATE)
+		fmt->height /= 2;
+}
+
+static void adv748x_fill_optional_dv_timings(struct v4l2_dv_timings *timings)
+{
+	v4l2_find_dv_timings_cap(timings, &adv748x_hdmi_timings_cap,
+				 250000, NULL, NULL);
+}
+
+static bool adv748x_hdmi_has_signal(struct adv748x_state *state)
+{
+	int val;
+
+	/* Check that VERT_FILTER and DE_REGEN is locked */
+	val = hdmi_read(state, ADV748X_HDMI_LW1);
+	return (val & ADV748X_HDMI_LW1_VERT_FILTER) &&
+	       (val & ADV748X_HDMI_LW1_DE_REGEN);
+}
+
+static int adv748x_hdmi_read_pixelclock(struct adv748x_state *state)
+{
+	int a, b;
+
+	a = hdmi_read(state, ADV748X_HDMI_TMDS_1);
+	b = hdmi_read(state, ADV748X_HDMI_TMDS_2);
+	if (a < 0 || b < 0)
+		return -ENODATA;
+
+	/*
+	 * The high 9 bits store TMDS frequency measurement in MHz
+	 * The low 7 bits of TMDS_2 store the 7-bit TMDS fractional frequency
+	 * measurement in 1/128 MHz
+	 */
+	return ((a << 1) | (b >> 7)) * 1000000 + (b & 0x7f) * 1000000 / 128;
+}
+
+/*
+ * adv748x_hdmi_set_de_timings: Adjust horizontal picture offset through DE
+ *
+ * HDMI CP uses a Data Enable synchronisation timing reference
+ *
+ * Vary the leading and trailing edge position of the DE signal output by the CP
+ * core. Values are stored as signed-twos-complement in one-pixel-clock units
+ *
+ * The start and end are shifted equally by the 10-bit shift value.
+ */
+static void adv748x_hdmi_set_de_timings(struct adv748x_state *state, int shift)
+{
+	u8 high, low;
+
+	/* POS_HIGH stores bits 8 and 9 of both the start and end */
+	high = ADV748X_CP_DE_POS_HIGH_SET;
+	high |= (shift & 0x300) >> 8;
+	low = shift & 0xff;
+
+	/* The sequence of the writes is important and must be followed */
+	cp_write(state, ADV748X_CP_DE_POS_HIGH, high);
+	cp_write(state, ADV748X_CP_DE_POS_END_LOW, low);
+
+	high |= (shift & 0x300) >> 6;
+
+	cp_write(state, ADV748X_CP_DE_POS_HIGH, high);
+	cp_write(state, ADV748X_CP_DE_POS_START_LOW, low);
+}
+
+static int adv748x_hdmi_set_video_timings(struct adv748x_state *state,
+					  const struct v4l2_dv_timings *timings)
+{
+	const struct adv748x_hdmi_video_standards *stds =
+		adv748x_hdmi_video_standards;
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(adv748x_hdmi_video_standards); i++) {
+		if (v4l2_match_dv_timings(timings, &stds[i].timings, 250000,
+					  false))
+			break;
+	}
+
+	if (i >= ARRAY_SIZE(adv748x_hdmi_video_standards))
+		return -EINVAL;
+
+	/*
+	 * When setting cp_vid_std to either 720p, 1080i, or 1080p, the video
+	 * will get shifted horizontally to the left in active video mode.
+	 * The de_h_start and de_h_end controls are used to centre the picture
+	 * correctly
+	 */
+	switch (stds[i].vid_std) {
+	case 0x53: /* 720p */
+		adv748x_hdmi_set_de_timings(state, -40);
+		break;
+	case 0x54: /* 1080i */
+	case 0x5e: /* 1080p */
+		adv748x_hdmi_set_de_timings(state, -44);
+		break;
+	default:
+		adv748x_hdmi_set_de_timings(state, 0);
+		break;
+	}
+
+	io_write(state, ADV748X_IO_VID_STD, stds[i].vid_std);
+	io_clrset(state, ADV748X_IO_DATAPATH, ADV748X_IO_DATAPATH_VFREQ_M,
+		  stds[i].v_freq << ADV748X_IO_DATAPATH_VFREQ_SHIFT);
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * v4l2_subdev_video_ops
+ */
+
+static int adv748x_hdmi_s_dv_timings(struct v4l2_subdev *sd,
+				     struct v4l2_dv_timings *timings)
+{
+	struct adv748x_hdmi *hdmi = adv748x_sd_to_hdmi(sd);
+	struct adv748x_state *state = adv748x_hdmi_to_state(hdmi);
+	int ret;
+
+	if (!timings)
+		return -EINVAL;
+
+	if (v4l2_match_dv_timings(&hdmi->timings, timings, 0, false))
+		return 0;
+
+	if (!v4l2_valid_dv_timings(timings, &adv748x_hdmi_timings_cap,
+				   NULL, NULL))
+		return -ERANGE;
+
+	adv748x_fill_optional_dv_timings(timings);
+
+	mutex_lock(&state->mutex);
+
+	ret = adv748x_hdmi_set_video_timings(state, timings);
+	if (ret)
+		goto error;
+
+	hdmi->timings = *timings;
+
+	cp_clrset(state, ADV748X_CP_VID_ADJ_2, ADV748X_CP_VID_ADJ_2_INTERLACED,
+		  timings->bt.interlaced ?
+				  ADV748X_CP_VID_ADJ_2_INTERLACED : 0);
+
+	mutex_unlock(&state->mutex);
+
+	return 0;
+
+error:
+	mutex_unlock(&state->mutex);
+	return ret;
+}
+
+static int adv748x_hdmi_g_dv_timings(struct v4l2_subdev *sd,
+				     struct v4l2_dv_timings *timings)
+{
+	struct adv748x_hdmi *hdmi = adv748x_sd_to_hdmi(sd);
+	struct adv748x_state *state = adv748x_hdmi_to_state(hdmi);
+
+	mutex_lock(&state->mutex);
+
+	*timings = hdmi->timings;
+
+	mutex_unlock(&state->mutex);
+
+	return 0;
+}
+
+static int adv748x_hdmi_query_dv_timings(struct v4l2_subdev *sd,
+					 struct v4l2_dv_timings *timings)
+{
+	struct adv748x_hdmi *hdmi = adv748x_sd_to_hdmi(sd);
+	struct adv748x_state *state = adv748x_hdmi_to_state(hdmi);
+	struct v4l2_bt_timings *bt = &timings->bt;
+	int pixelclock;
+	int polarity;
+
+	if (!timings)
+		return -EINVAL;
+
+	memset(timings, 0, sizeof(struct v4l2_dv_timings));
+
+	/*
+	 * If the pattern generator is enabled the device shall not be queried
+	 * for timings. Instead the timings programmed shall be reported as they
+	 * are the ones being used to generate the pattern.
+	 */
+	if (cp_read(state, ADV748X_CP_PAT_GEN) & ADV748X_CP_PAT_GEN_EN) {
+		*timings = hdmi->timings;
+		return 0;
+	}
+
+	if (!adv748x_hdmi_has_signal(state))
+		return -ENOLINK;
+
+	pixelclock = adv748x_hdmi_read_pixelclock(state);
+	if (pixelclock < 0)
+		return -ENODATA;
+
+	timings->type = V4L2_DV_BT_656_1120;
+
+	bt->pixelclock = pixelclock;
+	bt->interlaced = hdmi_read(state, ADV748X_HDMI_F1H1) &
+				ADV748X_HDMI_F1H1_INTERLACED ?
+				V4L2_DV_INTERLACED : V4L2_DV_PROGRESSIVE;
+	bt->width = hdmi_read16(state, ADV748X_HDMI_LW1,
+				ADV748X_HDMI_LW1_WIDTH_MASK);
+	bt->height = hdmi_read16(state, ADV748X_HDMI_F0H1,
+				 ADV748X_HDMI_F0H1_HEIGHT_MASK);
+	bt->hfrontporch = hdmi_read16(state, ADV748X_HDMI_HFRONT_PORCH,
+				      ADV748X_HDMI_HFRONT_PORCH_MASK);
+	bt->hsync = hdmi_read16(state, ADV748X_HDMI_HSYNC_WIDTH,
+				ADV748X_HDMI_HSYNC_WIDTH_MASK);
+	bt->hbackporch = hdmi_read16(state, ADV748X_HDMI_HBACK_PORCH,
+				     ADV748X_HDMI_HBACK_PORCH_MASK);
+	bt->vfrontporch = hdmi_read16(state, ADV748X_HDMI_VFRONT_PORCH,
+				      ADV748X_HDMI_VFRONT_PORCH_MASK) / 2;
+	bt->vsync = hdmi_read16(state, ADV748X_HDMI_VSYNC_WIDTH,
+				ADV748X_HDMI_VSYNC_WIDTH_MASK) / 2;
+	bt->vbackporch = hdmi_read16(state, ADV748X_HDMI_VBACK_PORCH,
+				     ADV748X_HDMI_VBACK_PORCH_MASK) / 2;
+
+	polarity = hdmi_read(state, 0x05);
+	bt->polarities = (polarity & BIT(4) ? V4L2_DV_VSYNC_POS_POL : 0) |
+		(polarity & BIT(5) ? V4L2_DV_HSYNC_POS_POL : 0);
+
+	if (bt->interlaced == V4L2_DV_INTERLACED) {
+		bt->height += hdmi_read16(state, 0x0b, 0x1fff);
+		bt->il_vfrontporch = hdmi_read16(state, 0x2c, 0x3fff) / 2;
+		bt->il_vsync = hdmi_read16(state, 0x30, 0x3fff) / 2;
+		bt->il_vbackporch = hdmi_read16(state, 0x34, 0x3fff) / 2;
+	}
+
+	adv748x_fill_optional_dv_timings(timings);
+
+	/*
+	 * No interrupt handling is implemented yet.
+	 * There should be an IRQ when a cable is plugged and the new timings
+	 * should be figured out and stored to state.
+	 */
+	hdmi->timings = *timings;
+
+	return 0;
+}
+
+static int adv748x_hdmi_g_input_status(struct v4l2_subdev *sd, u32 *status)
+{
+	struct adv748x_hdmi *hdmi = adv748x_sd_to_hdmi(sd);
+	struct adv748x_state *state = adv748x_hdmi_to_state(hdmi);
+
+	mutex_lock(&state->mutex);
+
+	*status = adv748x_hdmi_has_signal(state) ? 0 : V4L2_IN_ST_NO_SIGNAL;
+
+	mutex_unlock(&state->mutex);
+
+	return 0;
+}
+
+static int adv748x_hdmi_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct adv748x_hdmi *hdmi = adv748x_sd_to_hdmi(sd);
+	struct adv748x_state *state = adv748x_hdmi_to_state(hdmi);
+	int ret;
+
+	mutex_lock(&state->mutex);
+
+	ret = adv748x_tx_power(hdmi->tx, enable);
+	if (ret)
+		goto done;
+
+	if (adv748x_hdmi_has_signal(state))
+		adv_dbg(state, "Detected HDMI signal\n");
+	else
+		adv_dbg(state, "Couldn't detect HDMI video signal\n");
+
+done:
+	mutex_unlock(&state->mutex);
+	return ret;
+}
+
+static int adv748x_hdmi_g_pixelaspect(struct v4l2_subdev *sd,
+				      struct v4l2_fract *aspect)
+{
+	aspect->numerator = 1;
+	aspect->denominator = 1;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops adv748x_video_ops_hdmi = {
+	.s_dv_timings = adv748x_hdmi_s_dv_timings,
+	.g_dv_timings = adv748x_hdmi_g_dv_timings,
+	.query_dv_timings = adv748x_hdmi_query_dv_timings,
+	.g_input_status = adv748x_hdmi_g_input_status,
+	.s_stream = adv748x_hdmi_s_stream,
+	.g_pixelaspect = adv748x_hdmi_g_pixelaspect,
+};
+
+/* -----------------------------------------------------------------------------
+ * v4l2_subdev_pad_ops
+ */
+
+static int adv748x_hdmi_propagate_pixelrate(struct adv748x_hdmi *hdmi)
+{
+	struct v4l2_subdev *tx;
+	struct v4l2_dv_timings timings;
+
+	tx = adv748x_get_remote_sd(&hdmi->pads[ADV748X_HDMI_SOURCE]);
+	if (!tx)
+		return -ENOLINK;
+
+	adv748x_hdmi_query_dv_timings(&hdmi->sd, &timings);
+
+	return adv748x_csi2_set_pixelrate(tx, timings.bt.pixelclock);
+}
+
+static int adv748x_hdmi_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index != 0)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_RGB888_1X24;
+
+	return 0;
+}
+
+static int adv748x_hdmi_get_format(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_format *sdformat)
+{
+	struct adv748x_hdmi *hdmi = adv748x_sd_to_hdmi(sd);
+	struct v4l2_mbus_framefmt *mbusformat;
+
+	if (sdformat->pad != ADV748X_HDMI_SOURCE)
+		return -EINVAL;
+
+	if (sdformat->which == V4L2_SUBDEV_FORMAT_TRY) {
+		mbusformat = v4l2_subdev_get_try_format(sd, sd_state,
+							sdformat->pad);
+		sdformat->format = *mbusformat;
+	} else {
+		adv748x_hdmi_fill_format(hdmi, &sdformat->format);
+		adv748x_hdmi_propagate_pixelrate(hdmi);
+	}
+
+	return 0;
+}
+
+static int adv748x_hdmi_set_format(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_format *sdformat)
+{
+	struct v4l2_mbus_framefmt *mbusformat;
+
+	if (sdformat->pad != ADV748X_HDMI_SOURCE)
+		return -EINVAL;
+
+	if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		return adv748x_hdmi_get_format(sd, sd_state, sdformat);
+
+	mbusformat = v4l2_subdev_get_try_format(sd, sd_state, sdformat->pad);
+	*mbusformat = sdformat->format;
+
+	return 0;
+}
+
+static int adv748x_hdmi_get_edid(struct v4l2_subdev *sd, struct v4l2_edid *edid)
+{
+	struct adv748x_hdmi *hdmi = adv748x_sd_to_hdmi(sd);
+
+	memset(edid->reserved, 0, sizeof(edid->reserved));
+
+	if (!hdmi->edid.present)
+		return -ENODATA;
+
+	if (edid->start_block == 0 && edid->blocks == 0) {
+		edid->blocks = hdmi->edid.blocks;
+		return 0;
+	}
+
+	if (edid->start_block >= hdmi->edid.blocks)
+		return -EINVAL;
+
+	if (edid->start_block + edid->blocks > hdmi->edid.blocks)
+		edid->blocks = hdmi->edid.blocks - edid->start_block;
+
+	memcpy(edid->edid, hdmi->edid.edid + edid->start_block * 128,
+			edid->blocks * 128);
+
+	return 0;
+}
+
+static inline int adv748x_hdmi_edid_write_block(struct adv748x_hdmi *hdmi,
+					unsigned int total_len, const u8 *val)
+{
+	struct adv748x_state *state = adv748x_hdmi_to_state(hdmi);
+	int err = 0;
+	int i = 0;
+	int len = 0;
+
+	adv_dbg(state, "%s: write EDID block (%d byte)\n",
+				__func__, total_len);
+
+	while (!err && i < total_len) {
+		len = (total_len - i) > I2C_SMBUS_BLOCK_MAX ?
+				I2C_SMBUS_BLOCK_MAX :
+				(total_len - i);
+
+		err = adv748x_write_block(state, ADV748X_PAGE_EDID,
+				i, val + i, len);
+		i += len;
+	}
+
+	return err;
+}
+
+static int adv748x_hdmi_set_edid(struct v4l2_subdev *sd, struct v4l2_edid *edid)
+{
+	struct adv748x_hdmi *hdmi = adv748x_sd_to_hdmi(sd);
+	struct adv748x_state *state = adv748x_hdmi_to_state(hdmi);
+	int err;
+
+	memset(edid->reserved, 0, sizeof(edid->reserved));
+
+	if (edid->start_block != 0)
+		return -EINVAL;
+
+	if (edid->blocks == 0) {
+		hdmi->edid.blocks = 0;
+		hdmi->edid.present = 0;
+
+		/* Fall back to a 16:9 aspect ratio */
+		hdmi->aspect_ratio.numerator = 16;
+		hdmi->aspect_ratio.denominator = 9;
+
+		/* Disable the EDID */
+		repeater_write(state, ADV748X_REPEATER_EDID_SZ,
+			       edid->blocks << ADV748X_REPEATER_EDID_SZ_SHIFT);
+
+		repeater_write(state, ADV748X_REPEATER_EDID_CTL, 0);
+
+		return 0;
+	}
+
+	if (edid->blocks > 4) {
+		edid->blocks = 4;
+		return -E2BIG;
+	}
+
+	memcpy(hdmi->edid.edid, edid->edid, 128 * edid->blocks);
+	hdmi->edid.blocks = edid->blocks;
+	hdmi->edid.present = true;
+
+	hdmi->aspect_ratio = v4l2_calc_aspect_ratio(edid->edid[0x15],
+			edid->edid[0x16]);
+
+	err = adv748x_hdmi_edid_write_block(hdmi, 128 * edid->blocks,
+			hdmi->edid.edid);
+	if (err < 0) {
+		v4l2_err(sd, "error %d writing edid pad %d\n", err, edid->pad);
+		return err;
+	}
+
+	repeater_write(state, ADV748X_REPEATER_EDID_SZ,
+		       edid->blocks << ADV748X_REPEATER_EDID_SZ_SHIFT);
+
+	repeater_write(state, ADV748X_REPEATER_EDID_CTL,
+		       ADV748X_REPEATER_EDID_CTL_EN);
+
+	return 0;
+}
+
+static bool adv748x_hdmi_check_dv_timings(const struct v4l2_dv_timings *timings,
+					  void *hdl)
+{
+	const struct adv748x_hdmi_video_standards *stds =
+		adv748x_hdmi_video_standards;
+	unsigned int i;
+
+	for (i = 0; stds[i].timings.bt.width; i++)
+		if (v4l2_match_dv_timings(timings, &stds[i].timings, 0, false))
+			return true;
+
+	return false;
+}
+
+static int adv748x_hdmi_enum_dv_timings(struct v4l2_subdev *sd,
+					struct v4l2_enum_dv_timings *timings)
+{
+	return v4l2_enum_dv_timings_cap(timings, &adv748x_hdmi_timings_cap,
+					adv748x_hdmi_check_dv_timings, NULL);
+}
+
+static int adv748x_hdmi_dv_timings_cap(struct v4l2_subdev *sd,
+				       struct v4l2_dv_timings_cap *cap)
+{
+	*cap = adv748x_hdmi_timings_cap;
+	return 0;
+}
+
+static const struct v4l2_subdev_pad_ops adv748x_pad_ops_hdmi = {
+	.enum_mbus_code = adv748x_hdmi_enum_mbus_code,
+	.set_fmt = adv748x_hdmi_set_format,
+	.get_fmt = adv748x_hdmi_get_format,
+	.get_edid = adv748x_hdmi_get_edid,
+	.set_edid = adv748x_hdmi_set_edid,
+	.dv_timings_cap = adv748x_hdmi_dv_timings_cap,
+	.enum_dv_timings = adv748x_hdmi_enum_dv_timings,
+};
+
+/* -----------------------------------------------------------------------------
+ * v4l2_subdev_ops
+ */
+
+static const struct v4l2_subdev_ops adv748x_ops_hdmi = {
+	.video = &adv748x_video_ops_hdmi,
+	.pad = &adv748x_pad_ops_hdmi,
+};
+
+/* -----------------------------------------------------------------------------
+ * Controls
+ */
+
+static const char * const hdmi_ctrl_patgen_menu[] = {
+	"Disabled",
+	"Solid Color",
+	"Color Bars",
+	"Ramp Grey",
+	"Ramp Blue",
+	"Ramp Red",
+	"Checkered"
+};
+
+static int adv748x_hdmi_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct adv748x_hdmi *hdmi = adv748x_ctrl_to_hdmi(ctrl);
+	struct adv748x_state *state = adv748x_hdmi_to_state(hdmi);
+	int ret;
+	u8 pattern;
+
+	/* Enable video adjustment first */
+	ret = cp_clrset(state, ADV748X_CP_VID_ADJ,
+			ADV748X_CP_VID_ADJ_ENABLE,
+			ADV748X_CP_VID_ADJ_ENABLE);
+	if (ret < 0)
+		return ret;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		ret = cp_write(state, ADV748X_CP_BRI, ctrl->val);
+		break;
+	case V4L2_CID_HUE:
+		ret = cp_write(state, ADV748X_CP_HUE, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		ret = cp_write(state, ADV748X_CP_CON, ctrl->val);
+		break;
+	case V4L2_CID_SATURATION:
+		ret = cp_write(state, ADV748X_CP_SAT, ctrl->val);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		pattern = ctrl->val;
+
+		/* Pattern is 0-indexed. Ctrl Menu is 1-indexed */
+		if (pattern) {
+			pattern--;
+			pattern |= ADV748X_CP_PAT_GEN_EN;
+		}
+
+		ret = cp_write(state, ADV748X_CP_PAT_GEN, pattern);
+
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops adv748x_hdmi_ctrl_ops = {
+	.s_ctrl = adv748x_hdmi_s_ctrl,
+};
+
+static int adv748x_hdmi_init_controls(struct adv748x_hdmi *hdmi)
+{
+	struct adv748x_state *state = adv748x_hdmi_to_state(hdmi);
+
+	v4l2_ctrl_handler_init(&hdmi->ctrl_hdl, 5);
+
+	/* Use our mutex for the controls */
+	hdmi->ctrl_hdl.lock = &state->mutex;
+
+	v4l2_ctrl_new_std(&hdmi->ctrl_hdl, &adv748x_hdmi_ctrl_ops,
+			  V4L2_CID_BRIGHTNESS, ADV748X_CP_BRI_MIN,
+			  ADV748X_CP_BRI_MAX, 1, ADV748X_CP_BRI_DEF);
+	v4l2_ctrl_new_std(&hdmi->ctrl_hdl, &adv748x_hdmi_ctrl_ops,
+			  V4L2_CID_CONTRAST, ADV748X_CP_CON_MIN,
+			  ADV748X_CP_CON_MAX, 1, ADV748X_CP_CON_DEF);
+	v4l2_ctrl_new_std(&hdmi->ctrl_hdl, &adv748x_hdmi_ctrl_ops,
+			  V4L2_CID_SATURATION, ADV748X_CP_SAT_MIN,
+			  ADV748X_CP_SAT_MAX, 1, ADV748X_CP_SAT_DEF);
+	v4l2_ctrl_new_std(&hdmi->ctrl_hdl, &adv748x_hdmi_ctrl_ops,
+			  V4L2_CID_HUE, ADV748X_CP_HUE_MIN,
+			  ADV748X_CP_HUE_MAX, 1, ADV748X_CP_HUE_DEF);
+
+	/*
+	 * Todo: V4L2_CID_DV_RX_POWER_PRESENT should also be supported when
+	 * interrupts are handled correctly
+	 */
+
+	v4l2_ctrl_new_std_menu_items(&hdmi->ctrl_hdl, &adv748x_hdmi_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(hdmi_ctrl_patgen_menu) - 1,
+				     0, 0, hdmi_ctrl_patgen_menu);
+
+	hdmi->sd.ctrl_handler = &hdmi->ctrl_hdl;
+	if (hdmi->ctrl_hdl.error) {
+		v4l2_ctrl_handler_free(&hdmi->ctrl_hdl);
+		return hdmi->ctrl_hdl.error;
+	}
+
+	return v4l2_ctrl_handler_setup(&hdmi->ctrl_hdl);
+}
+
+int adv748x_hdmi_init(struct adv748x_hdmi *hdmi)
+{
+	struct adv748x_state *state = adv748x_hdmi_to_state(hdmi);
+	struct v4l2_dv_timings cea1280x720 = V4L2_DV_BT_CEA_1280X720P30;
+	int ret;
+
+	adv748x_hdmi_s_dv_timings(&hdmi->sd, &cea1280x720);
+
+	/* Initialise a default 16:9 aspect ratio */
+	hdmi->aspect_ratio.numerator = 16;
+	hdmi->aspect_ratio.denominator = 9;
+
+	adv748x_subdev_init(&hdmi->sd, state, &adv748x_ops_hdmi,
+			    MEDIA_ENT_F_IO_DTV, "hdmi");
+
+	hdmi->pads[ADV748X_HDMI_SINK].flags = MEDIA_PAD_FL_SINK;
+	hdmi->pads[ADV748X_HDMI_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
+
+	ret = media_entity_pads_init(&hdmi->sd.entity,
+				     ADV748X_HDMI_NR_PADS, hdmi->pads);
+	if (ret)
+		return ret;
+
+	ret = adv748x_hdmi_init_controls(hdmi);
+	if (ret)
+		goto err_free_media;
+
+	return 0;
+
+err_free_media:
+	media_entity_cleanup(&hdmi->sd.entity);
+
+	return ret;
+}
+
+void adv748x_hdmi_cleanup(struct adv748x_hdmi *hdmi)
+{
+	v4l2_device_unregister_subdev(&hdmi->sd);
+	media_entity_cleanup(&hdmi->sd.entity);
+	v4l2_ctrl_handler_free(&hdmi->ctrl_hdl);
+}
diff --git a/drivers/media/i2c/adv748x/adv748x.h b/drivers/media/i2c/adv748x/adv748x.h
new file mode 100644
index 000000000..6f90f78f5
--- /dev/null
+++ b/drivers/media/i2c/adv748x/adv748x.h
@@ -0,0 +1,445 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Driver for Analog Devices ADV748X video decoder and HDMI receiver
+ *
+ * Copyright (C) 2017 Renesas Electronics Corp.
+ *
+ * Authors:
+ *	Koji Matsuoka <koji.matsuoka.xm@renesas.com>
+ *	Niklas Söderlund <niklas.soderlund@ragnatech.se>
+ *	Kieran Bingham <kieran.bingham@ideasonboard.com>
+ *
+ * The ADV748x range of receivers have the following configurations:
+ *
+ *                  Analog   HDMI  MHL  4-Lane  1-Lane
+ *                    In      In         CSI     CSI
+ *       ADV7480               X    X     X
+ *       ADV7481      X        X    X     X       X
+ *       ADV7482      X        X          X       X
+ */
+
+#include <linux/i2c.h>
+
+#ifndef _ADV748X_H_
+#define _ADV748X_H_
+
+enum adv748x_page {
+	ADV748X_PAGE_IO,
+	ADV748X_PAGE_DPLL,
+	ADV748X_PAGE_CP,
+	ADV748X_PAGE_HDMI,
+	ADV748X_PAGE_EDID,
+	ADV748X_PAGE_REPEATER,
+	ADV748X_PAGE_INFOFRAME,
+	ADV748X_PAGE_CBUS,
+	ADV748X_PAGE_CEC,
+	ADV748X_PAGE_SDP,
+	ADV748X_PAGE_TXB,
+	ADV748X_PAGE_TXA,
+	ADV748X_PAGE_MAX,
+
+	/* Fake pages for register sequences */
+	ADV748X_PAGE_EOR,		/* End Mark */
+};
+
+/*
+ * Device tree port number definitions
+ *
+ * The ADV748X ports define the mapping between subdevices
+ * and the device tree specification
+ */
+enum adv748x_ports {
+	ADV748X_PORT_AIN0 = 0,
+	ADV748X_PORT_AIN1 = 1,
+	ADV748X_PORT_AIN2 = 2,
+	ADV748X_PORT_AIN3 = 3,
+	ADV748X_PORT_AIN4 = 4,
+	ADV748X_PORT_AIN5 = 5,
+	ADV748X_PORT_AIN6 = 6,
+	ADV748X_PORT_AIN7 = 7,
+	ADV748X_PORT_HDMI = 8,
+	ADV748X_PORT_TTL = 9,
+	ADV748X_PORT_TXA = 10,
+	ADV748X_PORT_TXB = 11,
+	ADV748X_PORT_MAX = 12,
+};
+
+enum adv748x_csi2_pads {
+	ADV748X_CSI2_SINK,
+	ADV748X_CSI2_SOURCE,
+	ADV748X_CSI2_NR_PADS,
+};
+
+/* CSI2 transmitters can have 2 internal connections, HDMI/AFE */
+#define ADV748X_CSI2_MAX_SUBDEVS 2
+
+struct adv748x_csi2 {
+	struct adv748x_state *state;
+	struct v4l2_mbus_framefmt format;
+	unsigned int page;
+	unsigned int port;
+	unsigned int num_lanes;
+	unsigned int active_lanes;
+
+	struct media_pad pads[ADV748X_CSI2_NR_PADS];
+	struct v4l2_ctrl_handler ctrl_hdl;
+	struct v4l2_ctrl *pixel_rate;
+	struct v4l2_subdev *src;
+	struct v4l2_subdev sd;
+};
+
+#define notifier_to_csi2(n) container_of(n, struct adv748x_csi2, notifier)
+#define adv748x_sd_to_csi2(sd) container_of(sd, struct adv748x_csi2, sd)
+
+#define is_tx_enabled(_tx) ((_tx)->state->endpoints[(_tx)->port] != NULL)
+#define is_txa(_tx) ((_tx) == &(_tx)->state->txa)
+#define is_txb(_tx) ((_tx) == &(_tx)->state->txb)
+#define is_tx(_tx) (is_txa(_tx) || is_txb(_tx))
+
+#define is_afe_enabled(_state)					\
+	((_state)->endpoints[ADV748X_PORT_AIN0] != NULL ||	\
+	 (_state)->endpoints[ADV748X_PORT_AIN1] != NULL ||	\
+	 (_state)->endpoints[ADV748X_PORT_AIN2] != NULL ||	\
+	 (_state)->endpoints[ADV748X_PORT_AIN3] != NULL ||	\
+	 (_state)->endpoints[ADV748X_PORT_AIN4] != NULL ||	\
+	 (_state)->endpoints[ADV748X_PORT_AIN5] != NULL ||	\
+	 (_state)->endpoints[ADV748X_PORT_AIN6] != NULL ||	\
+	 (_state)->endpoints[ADV748X_PORT_AIN7] != NULL)
+#define is_hdmi_enabled(_state) ((_state)->endpoints[ADV748X_PORT_HDMI] != NULL)
+
+enum adv748x_hdmi_pads {
+	ADV748X_HDMI_SINK,
+	ADV748X_HDMI_SOURCE,
+	ADV748X_HDMI_NR_PADS,
+};
+
+struct adv748x_hdmi {
+	struct media_pad pads[ADV748X_HDMI_NR_PADS];
+	struct v4l2_ctrl_handler ctrl_hdl;
+	struct v4l2_subdev sd;
+	struct v4l2_mbus_framefmt format;
+
+	struct v4l2_dv_timings timings;
+	struct v4l2_fract aspect_ratio;
+
+	struct adv748x_csi2 *tx;
+
+	struct {
+		u8 edid[512];
+		u32 present;
+		unsigned int blocks;
+	} edid;
+};
+
+#define adv748x_ctrl_to_hdmi(ctrl) \
+	container_of(ctrl->handler, struct adv748x_hdmi, ctrl_hdl)
+#define adv748x_sd_to_hdmi(sd) container_of(sd, struct adv748x_hdmi, sd)
+
+enum adv748x_afe_pads {
+	ADV748X_AFE_SINK_AIN0,
+	ADV748X_AFE_SINK_AIN1,
+	ADV748X_AFE_SINK_AIN2,
+	ADV748X_AFE_SINK_AIN3,
+	ADV748X_AFE_SINK_AIN4,
+	ADV748X_AFE_SINK_AIN5,
+	ADV748X_AFE_SINK_AIN6,
+	ADV748X_AFE_SINK_AIN7,
+	ADV748X_AFE_SOURCE,
+	ADV748X_AFE_NR_PADS,
+};
+
+struct adv748x_afe {
+	struct media_pad pads[ADV748X_AFE_NR_PADS];
+	struct v4l2_ctrl_handler ctrl_hdl;
+	struct v4l2_subdev sd;
+	struct v4l2_mbus_framefmt format;
+
+	struct adv748x_csi2 *tx;
+
+	bool streaming;
+	v4l2_std_id curr_norm;
+	unsigned int input;
+};
+
+#define adv748x_ctrl_to_afe(ctrl) \
+	container_of(ctrl->handler, struct adv748x_afe, ctrl_hdl)
+#define adv748x_sd_to_afe(sd) container_of(sd, struct adv748x_afe, sd)
+
+/**
+ * struct adv748x_state - State of ADV748X
+ * @dev:		(OF) device
+ * @client:		I2C client
+ * @mutex:		protect global state
+ *
+ * @endpoints:		parsed device node endpoints for each port
+ *
+ * @i2c_addresses:	I2C Page addresses
+ * @i2c_clients:	I2C clients for the page accesses
+ * @regmap:		regmap configuration pages.
+ *
+ * @hdmi:		state of HDMI receiver context
+ * @afe:		state of AFE receiver context
+ * @txa:		state of TXA transmitter context
+ * @txb:		state of TXB transmitter context
+ */
+struct adv748x_state {
+	struct device *dev;
+	struct i2c_client *client;
+	struct mutex mutex;
+
+	struct device_node *endpoints[ADV748X_PORT_MAX];
+
+	struct i2c_client *i2c_clients[ADV748X_PAGE_MAX];
+	struct regmap *regmap[ADV748X_PAGE_MAX];
+
+	struct adv748x_hdmi hdmi;
+	struct adv748x_afe afe;
+	struct adv748x_csi2 txa;
+	struct adv748x_csi2 txb;
+};
+
+#define adv748x_hdmi_to_state(h) container_of(h, struct adv748x_state, hdmi)
+#define adv748x_afe_to_state(a) container_of(a, struct adv748x_state, afe)
+
+#define adv_err(a, fmt, arg...)	dev_err(a->dev, fmt, ##arg)
+#define adv_info(a, fmt, arg...) dev_info(a->dev, fmt, ##arg)
+#define adv_dbg(a, fmt, arg...)	dev_dbg(a->dev, fmt, ##arg)
+
+/* Register Mappings */
+
+/* IO Map */
+#define ADV748X_IO_PD			0x00	/* power down controls */
+#define ADV748X_IO_PD_RX_EN		BIT(6)
+
+#define ADV748X_IO_REG_01		0x01	/* pwrdn{2}b, prog_xtal_freq */
+#define ADV748X_IO_REG_01_PWRDN_MASK	(BIT(7) | BIT(6))
+#define ADV748X_IO_REG_01_PWRDN2B	BIT(7)	/* CEC Wakeup Support */
+#define ADV748X_IO_REG_01_PWRDNB	BIT(6)	/* CEC Wakeup Support */
+
+#define ADV748X_IO_REG_04		0x04
+#define ADV748X_IO_REG_04_FORCE_FR	BIT(0)	/* Force CP free-run */
+
+#define ADV748X_IO_DATAPATH		0x03	/* datapath cntrl */
+#define ADV748X_IO_DATAPATH_VFREQ_M	0x70
+#define ADV748X_IO_DATAPATH_VFREQ_SHIFT	4
+
+#define ADV748X_IO_VID_STD		0x05
+
+#define ADV748X_IO_10			0x10	/* io_reg_10 */
+#define ADV748X_IO_10_CSI4_EN		BIT(7)
+#define ADV748X_IO_10_CSI1_EN		BIT(6)
+#define ADV748X_IO_10_PIX_OUT_EN	BIT(5)
+#define ADV748X_IO_10_CSI4_IN_SEL_AFE	BIT(3)
+
+#define ADV748X_IO_CHIP_REV_ID_1	0xdf
+#define ADV748X_IO_CHIP_REV_ID_2	0xe0
+
+#define ADV748X_IO_REG_F2		0xf2
+#define ADV748X_IO_REG_F2_READ_AUTO_INC	BIT(0)
+
+/* For PAGE slave address offsets */
+#define ADV748X_IO_SLAVE_ADDR_BASE	0xf2
+
+/*
+ * The ADV748x_Recommended_Settings_PrA_2014-08-20.pdf details both 0x80 and
+ * 0xff as examples for performing a software reset.
+ */
+#define ADV748X_IO_REG_FF		0xff
+#define ADV748X_IO_REG_FF_MAIN_RESET	0xff
+
+/* HDMI RX Map */
+#define ADV748X_HDMI_LW1		0x07	/* line width_1 */
+#define ADV748X_HDMI_LW1_VERT_FILTER	BIT(7)
+#define ADV748X_HDMI_LW1_DE_REGEN	BIT(5)
+#define ADV748X_HDMI_LW1_WIDTH_MASK	0x1fff
+
+#define ADV748X_HDMI_F0H1		0x09	/* field0 height_1 */
+#define ADV748X_HDMI_F0H1_HEIGHT_MASK	0x1fff
+
+#define ADV748X_HDMI_F1H1		0x0b	/* field1 height_1 */
+#define ADV748X_HDMI_F1H1_INTERLACED	BIT(5)
+
+#define ADV748X_HDMI_HFRONT_PORCH	0x20	/* hsync_front_porch_1 */
+#define ADV748X_HDMI_HFRONT_PORCH_MASK	0x1fff
+
+#define ADV748X_HDMI_HSYNC_WIDTH	0x22	/* hsync_pulse_width_1 */
+#define ADV748X_HDMI_HSYNC_WIDTH_MASK	0x1fff
+
+#define ADV748X_HDMI_HBACK_PORCH	0x24	/* hsync_back_porch_1 */
+#define ADV748X_HDMI_HBACK_PORCH_MASK	0x1fff
+
+#define ADV748X_HDMI_VFRONT_PORCH	0x2a	/* field0_vs_front_porch_1 */
+#define ADV748X_HDMI_VFRONT_PORCH_MASK	0x3fff
+
+#define ADV748X_HDMI_VSYNC_WIDTH	0x2e	/* field0_vs_pulse_width_1 */
+#define ADV748X_HDMI_VSYNC_WIDTH_MASK	0x3fff
+
+#define ADV748X_HDMI_VBACK_PORCH	0x32	/* field0_vs_back_porch_1 */
+#define ADV748X_HDMI_VBACK_PORCH_MASK	0x3fff
+
+#define ADV748X_HDMI_TMDS_1		0x51	/* hdmi_reg_51 */
+#define ADV748X_HDMI_TMDS_2		0x52	/* hdmi_reg_52 */
+
+/* HDMI RX Repeater Map */
+#define ADV748X_REPEATER_EDID_SZ	0x70	/* primary_edid_size */
+#define ADV748X_REPEATER_EDID_SZ_SHIFT	4
+
+#define ADV748X_REPEATER_EDID_CTL	0x74	/* hdcp edid controls */
+#define ADV748X_REPEATER_EDID_CTL_EN	BIT(0)	/* man_edid_a_enable */
+
+/* SDP Main Map */
+#define ADV748X_SDP_INSEL		0x00	/* user_map_rw_reg_00 */
+
+#define ADV748X_SDP_VID_SEL		0x02	/* user_map_rw_reg_02 */
+#define ADV748X_SDP_VID_SEL_MASK	0xf0
+#define ADV748X_SDP_VID_SEL_SHIFT	4
+
+/* Contrast - Unsigned*/
+#define ADV748X_SDP_CON			0x08	/* user_map_rw_reg_08 */
+#define ADV748X_SDP_CON_MIN		0
+#define ADV748X_SDP_CON_DEF		128
+#define ADV748X_SDP_CON_MAX		255
+
+/* Brightness - Signed */
+#define ADV748X_SDP_BRI			0x0a	/* user_map_rw_reg_0a */
+#define ADV748X_SDP_BRI_MIN		-128
+#define ADV748X_SDP_BRI_DEF		0
+#define ADV748X_SDP_BRI_MAX		127
+
+/* Hue - Signed, inverted*/
+#define ADV748X_SDP_HUE			0x0b	/* user_map_rw_reg_0b */
+#define ADV748X_SDP_HUE_MIN		-127
+#define ADV748X_SDP_HUE_DEF		0
+#define ADV748X_SDP_HUE_MAX		128
+
+/* Test Patterns / Default Values */
+#define ADV748X_SDP_DEF			0x0c	/* user_map_rw_reg_0c */
+#define ADV748X_SDP_DEF_VAL_EN		BIT(0)	/* Force free run mode */
+#define ADV748X_SDP_DEF_VAL_AUTO_EN	BIT(1)	/* Free run when no signal */
+
+#define ADV748X_SDP_MAP_SEL		0x0e	/* user_map_rw_reg_0e */
+#define ADV748X_SDP_MAP_SEL_RO_MAIN	1
+
+/* Free run pattern select */
+#define ADV748X_SDP_FRP			0x14
+#define ADV748X_SDP_FRP_MASK		GENMASK(3, 1)
+
+/* Saturation */
+#define ADV748X_SDP_SD_SAT_U		0xe3	/* user_map_rw_reg_e3 */
+#define ADV748X_SDP_SD_SAT_V		0xe4	/* user_map_rw_reg_e4 */
+#define ADV748X_SDP_SAT_MIN		0
+#define ADV748X_SDP_SAT_DEF		128
+#define ADV748X_SDP_SAT_MAX		255
+
+/* SDP RO Main Map */
+#define ADV748X_SDP_RO_10		0x10
+#define ADV748X_SDP_RO_10_IN_LOCK	BIT(0)
+
+/* CP Map */
+#define ADV748X_CP_PAT_GEN		0x37	/* int_pat_gen_1 */
+#define ADV748X_CP_PAT_GEN_EN		BIT(7)
+
+/* Contrast Control - Unsigned */
+#define ADV748X_CP_CON			0x3a	/* contrast_cntrl */
+#define ADV748X_CP_CON_MIN		0	/* Minimum contrast */
+#define ADV748X_CP_CON_DEF		128	/* Default */
+#define ADV748X_CP_CON_MAX		255	/* Maximum contrast */
+
+/* Saturation Control - Unsigned */
+#define ADV748X_CP_SAT			0x3b	/* saturation_cntrl */
+#define ADV748X_CP_SAT_MIN		0	/* Minimum saturation */
+#define ADV748X_CP_SAT_DEF		128	/* Default */
+#define ADV748X_CP_SAT_MAX		255	/* Maximum saturation */
+
+/* Brightness Control - Signed */
+#define ADV748X_CP_BRI			0x3c	/* brightness_cntrl */
+#define ADV748X_CP_BRI_MIN		-128	/* Luma is -512d */
+#define ADV748X_CP_BRI_DEF		0	/* Luma is 0 */
+#define ADV748X_CP_BRI_MAX		127	/* Luma is 508d */
+
+/* Hue Control */
+#define ADV748X_CP_HUE			0x3d	/* hue_cntrl */
+#define ADV748X_CP_HUE_MIN		0	/* -90 degree */
+#define ADV748X_CP_HUE_DEF		0	/* -90 degree */
+#define ADV748X_CP_HUE_MAX		255	/* +90 degree */
+
+#define ADV748X_CP_VID_ADJ		0x3e	/* vid_adj_0 */
+#define ADV748X_CP_VID_ADJ_ENABLE	BIT(7)	/* Enable colour controls */
+
+#define ADV748X_CP_DE_POS_HIGH		0x8b	/* de_pos_adj_6 */
+#define ADV748X_CP_DE_POS_HIGH_SET	BIT(6)
+#define ADV748X_CP_DE_POS_END_LOW	0x8c	/* de_pos_adj_7 */
+#define ADV748X_CP_DE_POS_START_LOW	0x8d	/* de_pos_adj_8 */
+
+#define ADV748X_CP_VID_ADJ_2			0x91
+#define ADV748X_CP_VID_ADJ_2_INTERLACED		BIT(6)
+#define ADV748X_CP_VID_ADJ_2_INTERLACED_3D	BIT(4)
+
+#define ADV748X_CP_CLMP_POS		0xc9	/* clmp_pos_cntrl_4 */
+#define ADV748X_CP_CLMP_POS_DIS_AUTO	BIT(0)	/* dis_auto_param_buff */
+
+/* CSI : TXA/TXB Maps */
+#define ADV748X_CSI_VC_REF		0x0d	/* csi_tx_top_reg_0d */
+#define ADV748X_CSI_VC_REF_SHIFT	6
+
+#define ADV748X_CSI_FS_AS_LS		0x1e	/* csi_tx_top_reg_1e */
+#define ADV748X_CSI_FS_AS_LS_UNKNOWN	BIT(6)	/* Undocumented bit */
+
+/* Register handling */
+
+int adv748x_read(struct adv748x_state *state, u8 addr, u8 reg);
+int adv748x_write(struct adv748x_state *state, u8 page, u8 reg, u8 value);
+int adv748x_write_block(struct adv748x_state *state, int client_page,
+			unsigned int init_reg, const void *val,
+			size_t val_len);
+
+#define io_read(s, r) adv748x_read(s, ADV748X_PAGE_IO, r)
+#define io_write(s, r, v) adv748x_write(s, ADV748X_PAGE_IO, r, v)
+#define io_clrset(s, r, m, v) io_write(s, r, (io_read(s, r) & ~(m)) | (v))
+
+#define hdmi_read(s, r) adv748x_read(s, ADV748X_PAGE_HDMI, r)
+#define hdmi_read16(s, r, m) (((hdmi_read(s, r) << 8) | hdmi_read(s, (r)+1)) & (m))
+#define hdmi_write(s, r, v) adv748x_write(s, ADV748X_PAGE_HDMI, r, v)
+
+#define repeater_read(s, r) adv748x_read(s, ADV748X_PAGE_REPEATER, r)
+#define repeater_write(s, r, v) adv748x_write(s, ADV748X_PAGE_REPEATER, r, v)
+
+#define sdp_read(s, r) adv748x_read(s, ADV748X_PAGE_SDP, r)
+#define sdp_write(s, r, v) adv748x_write(s, ADV748X_PAGE_SDP, r, v)
+#define sdp_clrset(s, r, m, v) sdp_write(s, r, (sdp_read(s, r) & ~(m)) | (v))
+
+#define cp_read(s, r) adv748x_read(s, ADV748X_PAGE_CP, r)
+#define cp_write(s, r, v) adv748x_write(s, ADV748X_PAGE_CP, r, v)
+#define cp_clrset(s, r, m, v) cp_write(s, r, (cp_read(s, r) & ~(m)) | (v))
+
+#define tx_read(t, r) adv748x_read(t->state, t->page, r)
+#define tx_write(t, r, v) adv748x_write(t->state, t->page, r, v)
+
+static inline struct v4l2_subdev *adv748x_get_remote_sd(struct media_pad *pad)
+{
+	pad = media_pad_remote_pad_first(pad);
+	if (!pad)
+		return NULL;
+
+	return media_entity_to_v4l2_subdev(pad->entity);
+}
+
+void adv748x_subdev_init(struct v4l2_subdev *sd, struct adv748x_state *state,
+			 const struct v4l2_subdev_ops *ops, u32 function,
+			 const char *ident);
+
+int adv748x_tx_power(struct adv748x_csi2 *tx, bool on);
+
+int adv748x_afe_init(struct adv748x_afe *afe);
+void adv748x_afe_cleanup(struct adv748x_afe *afe);
+int adv748x_afe_s_input(struct adv748x_afe *afe, unsigned int input);
+
+int adv748x_csi2_init(struct adv748x_state *state, struct adv748x_csi2 *tx);
+void adv748x_csi2_cleanup(struct adv748x_csi2 *tx);
+int adv748x_csi2_set_virtual_channel(struct adv748x_csi2 *tx, unsigned int vc);
+int adv748x_csi2_set_pixelrate(struct v4l2_subdev *sd, s64 rate);
+
+int adv748x_hdmi_init(struct adv748x_hdmi *hdmi);
+void adv748x_hdmi_cleanup(struct adv748x_hdmi *hdmi);
+
+#endif /* _ADV748X_H_ */
diff --git a/drivers/media/i2c/adv7511-v4l2.c b/drivers/media/i2c/adv7511-v4l2.c
new file mode 100644
index 000000000..a9183d928
--- /dev/null
+++ b/drivers/media/i2c/adv7511-v4l2.c
@@ -0,0 +1,1965 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Analog Devices ADV7511 HDMI Transmitter Device Driver
+ *
+ * Copyright 2013 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+/*
+ * This file is named adv7511-v4l2.c so it doesn't conflict with the Analog
+ * Device ADV7511 (config fragment CONFIG_DRM_I2C_ADV7511).
+ */
+
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <linux/videodev2.h>
+#include <linux/workqueue.h>
+#include <linux/hdmi.h>
+#include <linux/v4l2-dv-timings.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-dv-timings.h>
+#include <media/i2c/adv7511.h>
+#include <media/cec.h>
+
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "debug level (0-2)");
+
+MODULE_DESCRIPTION("Analog Devices ADV7511 HDMI Transmitter Device Driver");
+MODULE_AUTHOR("Hans Verkuil");
+MODULE_LICENSE("GPL v2");
+
+#define MASK_ADV7511_EDID_RDY_INT   0x04
+#define MASK_ADV7511_MSEN_INT       0x40
+#define MASK_ADV7511_HPD_INT        0x80
+
+#define MASK_ADV7511_HPD_DETECT     0x40
+#define MASK_ADV7511_MSEN_DETECT    0x20
+#define MASK_ADV7511_EDID_RDY       0x10
+
+#define EDID_MAX_RETRIES (8)
+#define EDID_DELAY 250
+#define EDID_MAX_SEGM 8
+
+#define ADV7511_MAX_WIDTH 1920
+#define ADV7511_MAX_HEIGHT 1200
+#define ADV7511_MIN_PIXELCLOCK 20000000
+#define ADV7511_MAX_PIXELCLOCK 225000000
+
+#define ADV7511_MAX_ADDRS (3)
+
+/*
+**********************************************************************
+*
+*  Arrays with configuration parameters for the ADV7511
+*
+**********************************************************************
+*/
+
+struct i2c_reg_value {
+	unsigned char reg;
+	unsigned char value;
+};
+
+struct adv7511_state_edid {
+	/* total number of blocks */
+	u32 blocks;
+	/* Number of segments read */
+	u32 segments;
+	u8 data[EDID_MAX_SEGM * 256];
+	/* Number of EDID read retries left */
+	unsigned read_retries;
+	bool complete;
+};
+
+struct adv7511_state {
+	struct adv7511_platform_data pdata;
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct v4l2_ctrl_handler hdl;
+	int chip_revision;
+	u8 i2c_edid_addr;
+	u8 i2c_pktmem_addr;
+	u8 i2c_cec_addr;
+
+	struct i2c_client *i2c_cec;
+	struct cec_adapter *cec_adap;
+	u8   cec_addr[ADV7511_MAX_ADDRS];
+	u8   cec_valid_addrs;
+	bool cec_enabled_adap;
+
+	/* Is the adv7511 powered on? */
+	bool power_on;
+	/* Did we receive hotplug and rx-sense signals? */
+	bool have_monitor;
+	bool enabled_irq;
+	/* timings from s_dv_timings */
+	struct v4l2_dv_timings dv_timings;
+	u32 fmt_code;
+	u32 colorspace;
+	u32 ycbcr_enc;
+	u32 quantization;
+	u32 xfer_func;
+	u32 content_type;
+	/* controls */
+	struct v4l2_ctrl *hdmi_mode_ctrl;
+	struct v4l2_ctrl *hotplug_ctrl;
+	struct v4l2_ctrl *rx_sense_ctrl;
+	struct v4l2_ctrl *have_edid0_ctrl;
+	struct v4l2_ctrl *rgb_quantization_range_ctrl;
+	struct v4l2_ctrl *content_type_ctrl;
+	struct i2c_client *i2c_edid;
+	struct i2c_client *i2c_pktmem;
+	struct adv7511_state_edid edid;
+	/* Running counter of the number of detected EDIDs (for debugging) */
+	unsigned edid_detect_counter;
+	struct workqueue_struct *work_queue;
+	struct delayed_work edid_handler; /* work entry */
+};
+
+static void adv7511_check_monitor_present_status(struct v4l2_subdev *sd);
+static bool adv7511_check_edid_status(struct v4l2_subdev *sd);
+static void adv7511_setup(struct v4l2_subdev *sd);
+static int adv7511_s_i2s_clock_freq(struct v4l2_subdev *sd, u32 freq);
+static int adv7511_s_clock_freq(struct v4l2_subdev *sd, u32 freq);
+
+
+static const struct v4l2_dv_timings_cap adv7511_timings_cap = {
+	.type = V4L2_DV_BT_656_1120,
+	/* keep this initialization for compatibility with GCC < 4.4.6 */
+	.reserved = { 0 },
+	V4L2_INIT_BT_TIMINGS(640, ADV7511_MAX_WIDTH, 350, ADV7511_MAX_HEIGHT,
+		ADV7511_MIN_PIXELCLOCK, ADV7511_MAX_PIXELCLOCK,
+		V4L2_DV_BT_STD_CEA861 | V4L2_DV_BT_STD_DMT |
+			V4L2_DV_BT_STD_GTF | V4L2_DV_BT_STD_CVT,
+		V4L2_DV_BT_CAP_PROGRESSIVE | V4L2_DV_BT_CAP_REDUCED_BLANKING |
+			V4L2_DV_BT_CAP_CUSTOM)
+};
+
+static inline struct adv7511_state *get_adv7511_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct adv7511_state, sd);
+}
+
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct adv7511_state, hdl)->sd;
+}
+
+/* ------------------------ I2C ----------------------------------------------- */
+
+static s32 adv_smbus_read_byte_data_check(struct i2c_client *client,
+					  u8 command, bool check)
+{
+	union i2c_smbus_data data;
+
+	if (!i2c_smbus_xfer(client->adapter, client->addr, client->flags,
+			    I2C_SMBUS_READ, command,
+			    I2C_SMBUS_BYTE_DATA, &data))
+		return data.byte;
+	if (check)
+		v4l_err(client, "error reading %02x, %02x\n",
+			client->addr, command);
+	return -1;
+}
+
+static s32 adv_smbus_read_byte_data(struct i2c_client *client, u8 command)
+{
+	int i;
+	for (i = 0; i < 3; i++) {
+		int ret = adv_smbus_read_byte_data_check(client, command, true);
+		if (ret >= 0) {
+			if (i)
+				v4l_err(client, "read ok after %d retries\n", i);
+			return ret;
+		}
+	}
+	v4l_err(client, "read failed\n");
+	return -1;
+}
+
+static int adv7511_rd(struct v4l2_subdev *sd, u8 reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return adv_smbus_read_byte_data(client, reg);
+}
+
+static int adv7511_wr(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+	int i;
+
+	for (i = 0; i < 3; i++) {
+		ret = i2c_smbus_write_byte_data(client, reg, val);
+		if (ret == 0)
+			return 0;
+	}
+	v4l2_err(sd, "%s: i2c write error\n", __func__);
+	return ret;
+}
+
+/* To set specific bits in the register, a clear-mask is given (to be AND-ed),
+   and then the value-mask (to be OR-ed). */
+static inline void adv7511_wr_and_or(struct v4l2_subdev *sd, u8 reg, u8 clr_mask, u8 val_mask)
+{
+	adv7511_wr(sd, reg, (adv7511_rd(sd, reg) & clr_mask) | val_mask);
+}
+
+static int adv7511_edid_rd(struct v4l2_subdev *sd, uint16_t len, uint8_t *buf)
+{
+	struct adv7511_state *state = get_adv7511_state(sd);
+	int i;
+
+	v4l2_dbg(1, debug, sd, "%s:\n", __func__);
+
+	for (i = 0; i < len; i += I2C_SMBUS_BLOCK_MAX) {
+		s32 ret;
+
+		ret = i2c_smbus_read_i2c_block_data(state->i2c_edid, i,
+						    I2C_SMBUS_BLOCK_MAX, buf + i);
+		if (ret < 0) {
+			v4l2_err(sd, "%s: i2c read error\n", __func__);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static inline int adv7511_cec_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct adv7511_state *state = get_adv7511_state(sd);
+
+	return i2c_smbus_read_byte_data(state->i2c_cec, reg);
+}
+
+static int adv7511_cec_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct adv7511_state *state = get_adv7511_state(sd);
+	int ret;
+	int i;
+
+	for (i = 0; i < 3; i++) {
+		ret = i2c_smbus_write_byte_data(state->i2c_cec, reg, val);
+		if (ret == 0)
+			return 0;
+	}
+	v4l2_err(sd, "%s: I2C Write Problem\n", __func__);
+	return ret;
+}
+
+static inline int adv7511_cec_write_and_or(struct v4l2_subdev *sd, u8 reg, u8 mask,
+				   u8 val)
+{
+	return adv7511_cec_write(sd, reg, (adv7511_cec_read(sd, reg) & mask) | val);
+}
+
+static int adv7511_pktmem_rd(struct v4l2_subdev *sd, u8 reg)
+{
+	struct adv7511_state *state = get_adv7511_state(sd);
+
+	return adv_smbus_read_byte_data(state->i2c_pktmem, reg);
+}
+
+static inline bool adv7511_have_hotplug(struct v4l2_subdev *sd)
+{
+	return adv7511_rd(sd, 0x42) & MASK_ADV7511_HPD_DETECT;
+}
+
+static inline bool adv7511_have_rx_sense(struct v4l2_subdev *sd)
+{
+	return adv7511_rd(sd, 0x42) & MASK_ADV7511_MSEN_DETECT;
+}
+
+static void adv7511_csc_conversion_mode(struct v4l2_subdev *sd, u8 mode)
+{
+	adv7511_wr_and_or(sd, 0x18, 0x9f, (mode & 0x3)<<5);
+}
+
+static void adv7511_csc_coeff(struct v4l2_subdev *sd,
+			      u16 A1, u16 A2, u16 A3, u16 A4,
+			      u16 B1, u16 B2, u16 B3, u16 B4,
+			      u16 C1, u16 C2, u16 C3, u16 C4)
+{
+	/* A */
+	adv7511_wr_and_or(sd, 0x18, 0xe0, A1>>8);
+	adv7511_wr(sd, 0x19, A1);
+	adv7511_wr_and_or(sd, 0x1A, 0xe0, A2>>8);
+	adv7511_wr(sd, 0x1B, A2);
+	adv7511_wr_and_or(sd, 0x1c, 0xe0, A3>>8);
+	adv7511_wr(sd, 0x1d, A3);
+	adv7511_wr_and_or(sd, 0x1e, 0xe0, A4>>8);
+	adv7511_wr(sd, 0x1f, A4);
+
+	/* B */
+	adv7511_wr_and_or(sd, 0x20, 0xe0, B1>>8);
+	adv7511_wr(sd, 0x21, B1);
+	adv7511_wr_and_or(sd, 0x22, 0xe0, B2>>8);
+	adv7511_wr(sd, 0x23, B2);
+	adv7511_wr_and_or(sd, 0x24, 0xe0, B3>>8);
+	adv7511_wr(sd, 0x25, B3);
+	adv7511_wr_and_or(sd, 0x26, 0xe0, B4>>8);
+	adv7511_wr(sd, 0x27, B4);
+
+	/* C */
+	adv7511_wr_and_or(sd, 0x28, 0xe0, C1>>8);
+	adv7511_wr(sd, 0x29, C1);
+	adv7511_wr_and_or(sd, 0x2A, 0xe0, C2>>8);
+	adv7511_wr(sd, 0x2B, C2);
+	adv7511_wr_and_or(sd, 0x2C, 0xe0, C3>>8);
+	adv7511_wr(sd, 0x2D, C3);
+	adv7511_wr_and_or(sd, 0x2E, 0xe0, C4>>8);
+	adv7511_wr(sd, 0x2F, C4);
+}
+
+static void adv7511_csc_rgb_full2limit(struct v4l2_subdev *sd, bool enable)
+{
+	if (enable) {
+		u8 csc_mode = 0;
+		adv7511_csc_conversion_mode(sd, csc_mode);
+		adv7511_csc_coeff(sd,
+				  4096-564, 0, 0, 256,
+				  0, 4096-564, 0, 256,
+				  0, 0, 4096-564, 256);
+		/* enable CSC */
+		adv7511_wr_and_or(sd, 0x18, 0x7f, 0x80);
+		/* AVI infoframe: Limited range RGB (16-235) */
+		adv7511_wr_and_or(sd, 0x57, 0xf3, 0x04);
+	} else {
+		/* disable CSC */
+		adv7511_wr_and_or(sd, 0x18, 0x7f, 0x0);
+		/* AVI infoframe: Full range RGB (0-255) */
+		adv7511_wr_and_or(sd, 0x57, 0xf3, 0x08);
+	}
+}
+
+static void adv7511_set_rgb_quantization_mode(struct v4l2_subdev *sd, struct v4l2_ctrl *ctrl)
+{
+	struct adv7511_state *state = get_adv7511_state(sd);
+
+	/* Only makes sense for RGB formats */
+	if (state->fmt_code != MEDIA_BUS_FMT_RGB888_1X24) {
+		/* so just keep quantization */
+		adv7511_csc_rgb_full2limit(sd, false);
+		return;
+	}
+
+	switch (ctrl->val) {
+	case V4L2_DV_RGB_RANGE_AUTO:
+		/* automatic */
+		if (state->dv_timings.bt.flags & V4L2_DV_FL_IS_CE_VIDEO) {
+			/* CE format, RGB limited range (16-235) */
+			adv7511_csc_rgb_full2limit(sd, true);
+		} else {
+			/* not CE format, RGB full range (0-255) */
+			adv7511_csc_rgb_full2limit(sd, false);
+		}
+		break;
+	case V4L2_DV_RGB_RANGE_LIMITED:
+		/* RGB limited range (16-235) */
+		adv7511_csc_rgb_full2limit(sd, true);
+		break;
+	case V4L2_DV_RGB_RANGE_FULL:
+		/* RGB full range (0-255) */
+		adv7511_csc_rgb_full2limit(sd, false);
+		break;
+	}
+}
+
+/* ------------------------------ CTRL OPS ------------------------------ */
+
+static int adv7511_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+	struct adv7511_state *state = get_adv7511_state(sd);
+
+	v4l2_dbg(1, debug, sd, "%s: ctrl id: %d, ctrl->val %d\n", __func__, ctrl->id, ctrl->val);
+
+	if (state->hdmi_mode_ctrl == ctrl) {
+		/* Set HDMI or DVI-D */
+		adv7511_wr_and_or(sd, 0xaf, 0xfd, ctrl->val == V4L2_DV_TX_MODE_HDMI ? 0x02 : 0x00);
+		return 0;
+	}
+	if (state->rgb_quantization_range_ctrl == ctrl) {
+		adv7511_set_rgb_quantization_mode(sd, ctrl);
+		return 0;
+	}
+	if (state->content_type_ctrl == ctrl) {
+		u8 itc, cn;
+
+		state->content_type = ctrl->val;
+		itc = state->content_type != V4L2_DV_IT_CONTENT_TYPE_NO_ITC;
+		cn = itc ? state->content_type : V4L2_DV_IT_CONTENT_TYPE_GRAPHICS;
+		adv7511_wr_and_or(sd, 0x57, 0x7f, itc << 7);
+		adv7511_wr_and_or(sd, 0x59, 0xcf, cn << 4);
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static const struct v4l2_ctrl_ops adv7511_ctrl_ops = {
+	.s_ctrl = adv7511_s_ctrl,
+};
+
+/* ---------------------------- CORE OPS ------------------------------------------- */
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static void adv7511_inv_register(struct v4l2_subdev *sd)
+{
+	struct adv7511_state *state = get_adv7511_state(sd);
+
+	v4l2_info(sd, "0x000-0x0ff: Main Map\n");
+	if (state->i2c_cec)
+		v4l2_info(sd, "0x100-0x1ff: CEC Map\n");
+}
+
+static int adv7511_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+{
+	struct adv7511_state *state = get_adv7511_state(sd);
+
+	reg->size = 1;
+	switch (reg->reg >> 8) {
+	case 0:
+		reg->val = adv7511_rd(sd, reg->reg & 0xff);
+		break;
+	case 1:
+		if (state->i2c_cec) {
+			reg->val = adv7511_cec_read(sd, reg->reg & 0xff);
+			break;
+		}
+		fallthrough;
+	default:
+		v4l2_info(sd, "Register %03llx not supported\n", reg->reg);
+		adv7511_inv_register(sd);
+		break;
+	}
+	return 0;
+}
+
+static int adv7511_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
+{
+	struct adv7511_state *state = get_adv7511_state(sd);
+
+	switch (reg->reg >> 8) {
+	case 0:
+		adv7511_wr(sd, reg->reg & 0xff, reg->val & 0xff);
+		break;
+	case 1:
+		if (state->i2c_cec) {
+			adv7511_cec_write(sd, reg->reg & 0xff, reg->val & 0xff);
+			break;
+		}
+		fallthrough;
+	default:
+		v4l2_info(sd, "Register %03llx not supported\n", reg->reg);
+		adv7511_inv_register(sd);
+		break;
+	}
+	return 0;
+}
+#endif
+
+struct adv7511_cfg_read_infoframe {
+	const char *desc;
+	u8 present_reg;
+	u8 present_mask;
+	u8 header[3];
+	u16 payload_addr;
+};
+
+static u8 hdmi_infoframe_checksum(u8 *ptr, size_t size)
+{
+	u8 csum = 0;
+	size_t i;
+
+	/* compute checksum */
+	for (i = 0; i < size; i++)
+		csum += ptr[i];
+
+	return 256 - csum;
+}
+
+static void log_infoframe(struct v4l2_subdev *sd, const struct adv7511_cfg_read_infoframe *cri)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct device *dev = &client->dev;
+	union hdmi_infoframe frame;
+	u8 buffer[32];
+	u8 len;
+	int i;
+
+	if (!(adv7511_rd(sd, cri->present_reg) & cri->present_mask)) {
+		v4l2_info(sd, "%s infoframe not transmitted\n", cri->desc);
+		return;
+	}
+
+	memcpy(buffer, cri->header, sizeof(cri->header));
+
+	len = buffer[2];
+
+	if (len + 4 > sizeof(buffer)) {
+		v4l2_err(sd, "%s: invalid %s infoframe length %d\n", __func__, cri->desc, len);
+		return;
+	}
+
+	if (cri->payload_addr >= 0x100) {
+		for (i = 0; i < len; i++)
+			buffer[i + 4] = adv7511_pktmem_rd(sd, cri->payload_addr + i - 0x100);
+	} else {
+		for (i = 0; i < len; i++)
+			buffer[i + 4] = adv7511_rd(sd, cri->payload_addr + i);
+	}
+	buffer[3] = 0;
+	buffer[3] = hdmi_infoframe_checksum(buffer, len + 4);
+
+	if (hdmi_infoframe_unpack(&frame, buffer, len + 4) < 0) {
+		v4l2_err(sd, "%s: unpack of %s infoframe failed\n", __func__, cri->desc);
+		return;
+	}
+
+	hdmi_infoframe_log(KERN_INFO, dev, &frame);
+}
+
+static void adv7511_log_infoframes(struct v4l2_subdev *sd)
+{
+	static const struct adv7511_cfg_read_infoframe cri[] = {
+		{ "AVI", 0x44, 0x10, { 0x82, 2, 13 }, 0x55 },
+		{ "Audio", 0x44, 0x08, { 0x84, 1, 10 }, 0x73 },
+		{ "SDP", 0x40, 0x40, { 0x83, 1, 25 }, 0x103 },
+	};
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(cri); i++)
+		log_infoframe(sd, &cri[i]);
+}
+
+static int adv7511_log_status(struct v4l2_subdev *sd)
+{
+	struct adv7511_state *state = get_adv7511_state(sd);
+	struct adv7511_state_edid *edid = &state->edid;
+	int i;
+
+	static const char * const states[] = {
+		"in reset",
+		"reading EDID",
+		"idle",
+		"initializing HDCP",
+		"HDCP enabled",
+		"initializing HDCP repeater",
+		"6", "7", "8", "9", "A", "B", "C", "D", "E", "F"
+	};
+	static const char * const errors[] = {
+		"no error",
+		"bad receiver BKSV",
+		"Ri mismatch",
+		"Pj mismatch",
+		"i2c error",
+		"timed out",
+		"max repeater cascade exceeded",
+		"hash check failed",
+		"too many devices",
+		"9", "A", "B", "C", "D", "E", "F"
+	};
+
+	v4l2_info(sd, "power %s\n", state->power_on ? "on" : "off");
+	v4l2_info(sd, "%s hotplug, %s Rx Sense, %s EDID (%d block(s))\n",
+		  (adv7511_rd(sd, 0x42) & MASK_ADV7511_HPD_DETECT) ? "detected" : "no",
+		  (adv7511_rd(sd, 0x42) & MASK_ADV7511_MSEN_DETECT) ? "detected" : "no",
+		  edid->segments ? "found" : "no",
+		  edid->blocks);
+	v4l2_info(sd, "%s output %s\n",
+		  (adv7511_rd(sd, 0xaf) & 0x02) ?
+		  "HDMI" : "DVI-D",
+		  (adv7511_rd(sd, 0xa1) & 0x3c) ?
+		  "disabled" : "enabled");
+	v4l2_info(sd, "state: %s, error: %s, detect count: %u, msk/irq: %02x/%02x\n",
+			  states[adv7511_rd(sd, 0xc8) & 0xf],
+			  errors[adv7511_rd(sd, 0xc8) >> 4], state->edid_detect_counter,
+			  adv7511_rd(sd, 0x94), adv7511_rd(sd, 0x96));
+	v4l2_info(sd, "RGB quantization: %s range\n", adv7511_rd(sd, 0x18) & 0x80 ? "limited" : "full");
+	if (adv7511_rd(sd, 0xaf) & 0x02) {
+		/* HDMI only */
+		u8 manual_cts = adv7511_rd(sd, 0x0a) & 0x80;
+		u32 N = (adv7511_rd(sd, 0x01) & 0xf) << 16 |
+			adv7511_rd(sd, 0x02) << 8 |
+			adv7511_rd(sd, 0x03);
+		u8 vic_detect = adv7511_rd(sd, 0x3e) >> 2;
+		u8 vic_sent = adv7511_rd(sd, 0x3d) & 0x3f;
+		u32 CTS;
+
+		if (manual_cts)
+			CTS = (adv7511_rd(sd, 0x07) & 0xf) << 16 |
+			      adv7511_rd(sd, 0x08) << 8 |
+			      adv7511_rd(sd, 0x09);
+		else
+			CTS = (adv7511_rd(sd, 0x04) & 0xf) << 16 |
+			      adv7511_rd(sd, 0x05) << 8 |
+			      adv7511_rd(sd, 0x06);
+		v4l2_info(sd, "CTS %s mode: N %d, CTS %d\n",
+			  manual_cts ? "manual" : "automatic", N, CTS);
+		v4l2_info(sd, "VIC: detected %d, sent %d\n",
+			  vic_detect, vic_sent);
+		adv7511_log_infoframes(sd);
+	}
+	if (state->dv_timings.type == V4L2_DV_BT_656_1120)
+		v4l2_print_dv_timings(sd->name, "timings: ",
+				&state->dv_timings, false);
+	else
+		v4l2_info(sd, "no timings set\n");
+	v4l2_info(sd, "i2c edid addr: 0x%x\n", state->i2c_edid_addr);
+
+	if (state->i2c_cec == NULL)
+		return 0;
+
+	v4l2_info(sd, "i2c cec addr: 0x%x\n", state->i2c_cec_addr);
+
+	v4l2_info(sd, "CEC: %s\n", state->cec_enabled_adap ?
+			"enabled" : "disabled");
+	if (state->cec_enabled_adap) {
+		for (i = 0; i < ADV7511_MAX_ADDRS; i++) {
+			bool is_valid = state->cec_valid_addrs & (1 << i);
+
+			if (is_valid)
+				v4l2_info(sd, "CEC Logical Address: 0x%x\n",
+					  state->cec_addr[i]);
+		}
+	}
+	v4l2_info(sd, "i2c pktmem addr: 0x%x\n", state->i2c_pktmem_addr);
+	return 0;
+}
+
+/* Power up/down adv7511 */
+static int adv7511_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct adv7511_state *state = get_adv7511_state(sd);
+	const int retries = 20;
+	int i;
+
+	v4l2_dbg(1, debug, sd, "%s: power %s\n", __func__, on ? "on" : "off");
+
+	state->power_on = on;
+
+	if (!on) {
+		/* Power down */
+		adv7511_wr_and_or(sd, 0x41, 0xbf, 0x40);
+		return true;
+	}
+
+	/* Power up */
+	/* The adv7511 does not always come up immediately.
+	   Retry multiple times. */
+	for (i = 0; i < retries; i++) {
+		adv7511_wr_and_or(sd, 0x41, 0xbf, 0x0);
+		if ((adv7511_rd(sd, 0x41) & 0x40) == 0)
+			break;
+		adv7511_wr_and_or(sd, 0x41, 0xbf, 0x40);
+		msleep(10);
+	}
+	if (i == retries) {
+		v4l2_dbg(1, debug, sd, "%s: failed to powerup the adv7511!\n", __func__);
+		adv7511_s_power(sd, 0);
+		return false;
+	}
+	if (i > 1)
+		v4l2_dbg(1, debug, sd, "%s: needed %d retries to powerup the adv7511\n", __func__, i);
+
+	/* Reserved registers that must be set */
+	adv7511_wr(sd, 0x98, 0x03);
+	adv7511_wr_and_or(sd, 0x9a, 0xfe, 0x70);
+	adv7511_wr(sd, 0x9c, 0x30);
+	adv7511_wr_and_or(sd, 0x9d, 0xfc, 0x01);
+	adv7511_wr(sd, 0xa2, 0xa4);
+	adv7511_wr(sd, 0xa3, 0xa4);
+	adv7511_wr(sd, 0xe0, 0xd0);
+	adv7511_wr(sd, 0xf9, 0x00);
+
+	adv7511_wr(sd, 0x43, state->i2c_edid_addr);
+	adv7511_wr(sd, 0x45, state->i2c_pktmem_addr);
+
+	/* Set number of attempts to read the EDID */
+	adv7511_wr(sd, 0xc9, 0xf);
+	return true;
+}
+
+#if IS_ENABLED(CONFIG_VIDEO_ADV7511_CEC)
+static int adv7511_cec_adap_enable(struct cec_adapter *adap, bool enable)
+{
+	struct adv7511_state *state = cec_get_drvdata(adap);
+	struct v4l2_subdev *sd = &state->sd;
+
+	if (state->i2c_cec == NULL)
+		return -EIO;
+
+	if (!state->cec_enabled_adap && enable) {
+		/* power up cec section */
+		adv7511_cec_write_and_or(sd, 0x4e, 0xfc, 0x01);
+		/* legacy mode and clear all rx buffers */
+		adv7511_cec_write(sd, 0x4a, 0x00);
+		adv7511_cec_write(sd, 0x4a, 0x07);
+		adv7511_cec_write_and_or(sd, 0x11, 0xfe, 0); /* initially disable tx */
+		/* enabled irqs: */
+		/* tx: ready */
+		/* tx: arbitration lost */
+		/* tx: retry timeout */
+		/* rx: ready 1 */
+		if (state->enabled_irq)
+			adv7511_wr_and_or(sd, 0x95, 0xc0, 0x39);
+	} else if (state->cec_enabled_adap && !enable) {
+		if (state->enabled_irq)
+			adv7511_wr_and_or(sd, 0x95, 0xc0, 0x00);
+		/* disable address mask 1-3 */
+		adv7511_cec_write_and_or(sd, 0x4b, 0x8f, 0x00);
+		/* power down cec section */
+		adv7511_cec_write_and_or(sd, 0x4e, 0xfc, 0x00);
+		state->cec_valid_addrs = 0;
+	}
+	state->cec_enabled_adap = enable;
+	return 0;
+}
+
+static int adv7511_cec_adap_log_addr(struct cec_adapter *adap, u8 addr)
+{
+	struct adv7511_state *state = cec_get_drvdata(adap);
+	struct v4l2_subdev *sd = &state->sd;
+	unsigned int i, free_idx = ADV7511_MAX_ADDRS;
+
+	if (!state->cec_enabled_adap)
+		return addr == CEC_LOG_ADDR_INVALID ? 0 : -EIO;
+
+	if (addr == CEC_LOG_ADDR_INVALID) {
+		adv7511_cec_write_and_or(sd, 0x4b, 0x8f, 0);
+		state->cec_valid_addrs = 0;
+		return 0;
+	}
+
+	for (i = 0; i < ADV7511_MAX_ADDRS; i++) {
+		bool is_valid = state->cec_valid_addrs & (1 << i);
+
+		if (free_idx == ADV7511_MAX_ADDRS && !is_valid)
+			free_idx = i;
+		if (is_valid && state->cec_addr[i] == addr)
+			return 0;
+	}
+	if (i == ADV7511_MAX_ADDRS) {
+		i = free_idx;
+		if (i == ADV7511_MAX_ADDRS)
+			return -ENXIO;
+	}
+	state->cec_addr[i] = addr;
+	state->cec_valid_addrs |= 1 << i;
+
+	switch (i) {
+	case 0:
+		/* enable address mask 0 */
+		adv7511_cec_write_and_or(sd, 0x4b, 0xef, 0x10);
+		/* set address for mask 0 */
+		adv7511_cec_write_and_or(sd, 0x4c, 0xf0, addr);
+		break;
+	case 1:
+		/* enable address mask 1 */
+		adv7511_cec_write_and_or(sd, 0x4b, 0xdf, 0x20);
+		/* set address for mask 1 */
+		adv7511_cec_write_and_or(sd, 0x4c, 0x0f, addr << 4);
+		break;
+	case 2:
+		/* enable address mask 2 */
+		adv7511_cec_write_and_or(sd, 0x4b, 0xbf, 0x40);
+		/* set address for mask 1 */
+		adv7511_cec_write_and_or(sd, 0x4d, 0xf0, addr);
+		break;
+	}
+	return 0;
+}
+
+static int adv7511_cec_adap_transmit(struct cec_adapter *adap, u8 attempts,
+				     u32 signal_free_time, struct cec_msg *msg)
+{
+	struct adv7511_state *state = cec_get_drvdata(adap);
+	struct v4l2_subdev *sd = &state->sd;
+	u8 len = msg->len;
+	unsigned int i;
+
+	v4l2_dbg(1, debug, sd, "%s: len %d\n", __func__, len);
+
+	if (len > 16) {
+		v4l2_err(sd, "%s: len exceeded 16 (%d)\n", __func__, len);
+		return -EINVAL;
+	}
+
+	/*
+	 * The number of retries is the number of attempts - 1, but retry
+	 * at least once. It's not clear if a value of 0 is allowed, so
+	 * let's do at least one retry.
+	 */
+	adv7511_cec_write_and_or(sd, 0x12, ~0x70, max(1, attempts - 1) << 4);
+
+	/* clear cec tx irq status */
+	adv7511_wr(sd, 0x97, 0x38);
+
+	/* write data */
+	for (i = 0; i < len; i++)
+		adv7511_cec_write(sd, i, msg->msg[i]);
+
+	/* set length (data + header) */
+	adv7511_cec_write(sd, 0x10, len);
+	/* start transmit, enable tx */
+	adv7511_cec_write(sd, 0x11, 0x01);
+	return 0;
+}
+
+static void adv_cec_tx_raw_status(struct v4l2_subdev *sd, u8 tx_raw_status)
+{
+	struct adv7511_state *state = get_adv7511_state(sd);
+
+	if ((adv7511_cec_read(sd, 0x11) & 0x01) == 0) {
+		v4l2_dbg(1, debug, sd, "%s: tx raw: tx disabled\n", __func__);
+		return;
+	}
+
+	if (tx_raw_status & 0x10) {
+		v4l2_dbg(1, debug, sd,
+			 "%s: tx raw: arbitration lost\n", __func__);
+		cec_transmit_done(state->cec_adap, CEC_TX_STATUS_ARB_LOST,
+				  1, 0, 0, 0);
+		return;
+	}
+	if (tx_raw_status & 0x08) {
+		u8 status;
+		u8 nack_cnt;
+		u8 low_drive_cnt;
+
+		v4l2_dbg(1, debug, sd, "%s: tx raw: retry failed\n", __func__);
+		/*
+		 * We set this status bit since this hardware performs
+		 * retransmissions.
+		 */
+		status = CEC_TX_STATUS_MAX_RETRIES;
+		nack_cnt = adv7511_cec_read(sd, 0x14) & 0xf;
+		if (nack_cnt)
+			status |= CEC_TX_STATUS_NACK;
+		low_drive_cnt = adv7511_cec_read(sd, 0x14) >> 4;
+		if (low_drive_cnt)
+			status |= CEC_TX_STATUS_LOW_DRIVE;
+		cec_transmit_done(state->cec_adap, status,
+				  0, nack_cnt, low_drive_cnt, 0);
+		return;
+	}
+	if (tx_raw_status & 0x20) {
+		v4l2_dbg(1, debug, sd, "%s: tx raw: ready ok\n", __func__);
+		cec_transmit_done(state->cec_adap, CEC_TX_STATUS_OK, 0, 0, 0, 0);
+		return;
+	}
+}
+
+static const struct cec_adap_ops adv7511_cec_adap_ops = {
+	.adap_enable = adv7511_cec_adap_enable,
+	.adap_log_addr = adv7511_cec_adap_log_addr,
+	.adap_transmit = adv7511_cec_adap_transmit,
+};
+#endif
+
+/* Enable interrupts */
+static void adv7511_set_isr(struct v4l2_subdev *sd, bool enable)
+{
+	struct adv7511_state *state = get_adv7511_state(sd);
+	u8 irqs = MASK_ADV7511_HPD_INT | MASK_ADV7511_MSEN_INT;
+	u8 irqs_rd;
+	int retries = 100;
+
+	v4l2_dbg(2, debug, sd, "%s: %s\n", __func__, enable ? "enable" : "disable");
+
+	if (state->enabled_irq == enable)
+		return;
+	state->enabled_irq = enable;
+
+	/* The datasheet says that the EDID ready interrupt should be
+	   disabled if there is no hotplug. */
+	if (!enable)
+		irqs = 0;
+	else if (adv7511_have_hotplug(sd))
+		irqs |= MASK_ADV7511_EDID_RDY_INT;
+
+	/*
+	 * This i2c write can fail (approx. 1 in 1000 writes). But it
+	 * is essential that this register is correct, so retry it
+	 * multiple times.
+	 *
+	 * Note that the i2c write does not report an error, but the readback
+	 * clearly shows the wrong value.
+	 */
+	do {
+		adv7511_wr(sd, 0x94, irqs);
+		irqs_rd = adv7511_rd(sd, 0x94);
+	} while (retries-- && irqs_rd != irqs);
+
+	if (irqs_rd != irqs)
+		v4l2_err(sd, "Could not set interrupts: hw failure?\n");
+
+	adv7511_wr_and_or(sd, 0x95, 0xc0,
+			  (state->cec_enabled_adap && enable) ? 0x39 : 0x00);
+}
+
+/* Interrupt handler */
+static int adv7511_isr(struct v4l2_subdev *sd, u32 status, bool *handled)
+{
+	u8 irq_status;
+	u8 cec_irq;
+
+	/* disable interrupts to prevent a race condition */
+	adv7511_set_isr(sd, false);
+	irq_status = adv7511_rd(sd, 0x96);
+	cec_irq = adv7511_rd(sd, 0x97);
+	/* clear detected interrupts */
+	adv7511_wr(sd, 0x96, irq_status);
+	adv7511_wr(sd, 0x97, cec_irq);
+
+	v4l2_dbg(1, debug, sd, "%s: irq 0x%x, cec-irq 0x%x\n", __func__,
+		 irq_status, cec_irq);
+
+	if (irq_status & (MASK_ADV7511_HPD_INT | MASK_ADV7511_MSEN_INT))
+		adv7511_check_monitor_present_status(sd);
+	if (irq_status & MASK_ADV7511_EDID_RDY_INT)
+		adv7511_check_edid_status(sd);
+
+#if IS_ENABLED(CONFIG_VIDEO_ADV7511_CEC)
+	if (cec_irq & 0x38)
+		adv_cec_tx_raw_status(sd, cec_irq);
+
+	if (cec_irq & 1) {
+		struct adv7511_state *state = get_adv7511_state(sd);
+		struct cec_msg msg;
+
+		msg.len = adv7511_cec_read(sd, 0x25) & 0x1f;
+
+		v4l2_dbg(1, debug, sd, "%s: cec msg len %d\n", __func__,
+			 msg.len);
+
+		if (msg.len > CEC_MAX_MSG_SIZE)
+			msg.len = CEC_MAX_MSG_SIZE;
+
+		if (msg.len) {
+			u8 i;
+
+			for (i = 0; i < msg.len; i++)
+				msg.msg[i] = adv7511_cec_read(sd, i + 0x15);
+
+			adv7511_cec_write(sd, 0x4a, 0); /* toggle to re-enable rx 1 */
+			adv7511_cec_write(sd, 0x4a, 1);
+			cec_received_msg(state->cec_adap, &msg);
+		}
+	}
+#endif
+
+	/* enable interrupts */
+	adv7511_set_isr(sd, true);
+
+	if (handled)
+		*handled = true;
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops adv7511_core_ops = {
+	.log_status = adv7511_log_status,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = adv7511_g_register,
+	.s_register = adv7511_s_register,
+#endif
+	.s_power = adv7511_s_power,
+	.interrupt_service_routine = adv7511_isr,
+};
+
+/* ------------------------------ VIDEO OPS ------------------------------ */
+
+/* Enable/disable adv7511 output */
+static int adv7511_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct adv7511_state *state = get_adv7511_state(sd);
+
+	v4l2_dbg(1, debug, sd, "%s: %sable\n", __func__, (enable ? "en" : "dis"));
+	adv7511_wr_and_or(sd, 0xa1, ~0x3c, (enable ? 0 : 0x3c));
+	if (enable) {
+		adv7511_check_monitor_present_status(sd);
+	} else {
+		adv7511_s_power(sd, 0);
+		state->have_monitor = false;
+	}
+	return 0;
+}
+
+static int adv7511_s_dv_timings(struct v4l2_subdev *sd,
+			       struct v4l2_dv_timings *timings)
+{
+	struct adv7511_state *state = get_adv7511_state(sd);
+	struct v4l2_bt_timings *bt = &timings->bt;
+	u32 fps;
+
+	v4l2_dbg(1, debug, sd, "%s:\n", __func__);
+
+	/* quick sanity check */
+	if (!v4l2_valid_dv_timings(timings, &adv7511_timings_cap, NULL, NULL))
+		return -EINVAL;
+
+	/* Fill the optional fields .standards and .flags in struct v4l2_dv_timings
+	   if the format is one of the CEA or DMT timings. */
+	v4l2_find_dv_timings_cap(timings, &adv7511_timings_cap, 0, NULL, NULL);
+
+	/* save timings */
+	state->dv_timings = *timings;
+
+	/* set h/vsync polarities */
+	adv7511_wr_and_or(sd, 0x17, 0x9f,
+		((bt->polarities & V4L2_DV_VSYNC_POS_POL) ? 0 : 0x40) |
+		((bt->polarities & V4L2_DV_HSYNC_POS_POL) ? 0 : 0x20));
+
+	fps = (u32)bt->pixelclock / (V4L2_DV_BT_FRAME_WIDTH(bt) * V4L2_DV_BT_FRAME_HEIGHT(bt));
+	switch (fps) {
+	case 24:
+		adv7511_wr_and_or(sd, 0xfb, 0xf9, 1 << 1);
+		break;
+	case 25:
+		adv7511_wr_and_or(sd, 0xfb, 0xf9, 2 << 1);
+		break;
+	case 30:
+		adv7511_wr_and_or(sd, 0xfb, 0xf9, 3 << 1);
+		break;
+	default:
+		adv7511_wr_and_or(sd, 0xfb, 0xf9, 0);
+		break;
+	}
+
+	/* update quantization range based on new dv_timings */
+	adv7511_set_rgb_quantization_mode(sd, state->rgb_quantization_range_ctrl);
+
+	return 0;
+}
+
+static int adv7511_g_dv_timings(struct v4l2_subdev *sd,
+				struct v4l2_dv_timings *timings)
+{
+	struct adv7511_state *state = get_adv7511_state(sd);
+
+	v4l2_dbg(1, debug, sd, "%s:\n", __func__);
+
+	if (!timings)
+		return -EINVAL;
+
+	*timings = state->dv_timings;
+
+	return 0;
+}
+
+static int adv7511_enum_dv_timings(struct v4l2_subdev *sd,
+				   struct v4l2_enum_dv_timings *timings)
+{
+	if (timings->pad != 0)
+		return -EINVAL;
+
+	return v4l2_enum_dv_timings_cap(timings, &adv7511_timings_cap, NULL, NULL);
+}
+
+static int adv7511_dv_timings_cap(struct v4l2_subdev *sd,
+				  struct v4l2_dv_timings_cap *cap)
+{
+	if (cap->pad != 0)
+		return -EINVAL;
+
+	*cap = adv7511_timings_cap;
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops adv7511_video_ops = {
+	.s_stream = adv7511_s_stream,
+	.s_dv_timings = adv7511_s_dv_timings,
+	.g_dv_timings = adv7511_g_dv_timings,
+};
+
+/* ------------------------------ AUDIO OPS ------------------------------ */
+static int adv7511_s_audio_stream(struct v4l2_subdev *sd, int enable)
+{
+	v4l2_dbg(1, debug, sd, "%s: %sable\n", __func__, (enable ? "en" : "dis"));
+
+	if (enable)
+		adv7511_wr_and_or(sd, 0x4b, 0x3f, 0x80);
+	else
+		adv7511_wr_and_or(sd, 0x4b, 0x3f, 0x40);
+
+	return 0;
+}
+
+static int adv7511_s_clock_freq(struct v4l2_subdev *sd, u32 freq)
+{
+	u32 N;
+
+	switch (freq) {
+	case 32000:  N = 4096;  break;
+	case 44100:  N = 6272;  break;
+	case 48000:  N = 6144;  break;
+	case 88200:  N = 12544; break;
+	case 96000:  N = 12288; break;
+	case 176400: N = 25088; break;
+	case 192000: N = 24576; break;
+	default:
+		return -EINVAL;
+	}
+
+	/* Set N (used with CTS to regenerate the audio clock) */
+	adv7511_wr(sd, 0x01, (N >> 16) & 0xf);
+	adv7511_wr(sd, 0x02, (N >> 8) & 0xff);
+	adv7511_wr(sd, 0x03, N & 0xff);
+
+	return 0;
+}
+
+static int adv7511_s_i2s_clock_freq(struct v4l2_subdev *sd, u32 freq)
+{
+	u32 i2s_sf;
+
+	switch (freq) {
+	case 32000:  i2s_sf = 0x30; break;
+	case 44100:  i2s_sf = 0x00; break;
+	case 48000:  i2s_sf = 0x20; break;
+	case 88200:  i2s_sf = 0x80; break;
+	case 96000:  i2s_sf = 0xa0; break;
+	case 176400: i2s_sf = 0xc0; break;
+	case 192000: i2s_sf = 0xe0; break;
+	default:
+		return -EINVAL;
+	}
+
+	/* Set sampling frequency for I2S audio to 48 kHz */
+	adv7511_wr_and_or(sd, 0x15, 0xf, i2s_sf);
+
+	return 0;
+}
+
+static int adv7511_s_routing(struct v4l2_subdev *sd, u32 input, u32 output, u32 config)
+{
+	/* Only 2 channels in use for application */
+	adv7511_wr_and_or(sd, 0x73, 0xf8, 0x1);
+	/* Speaker mapping */
+	adv7511_wr(sd, 0x76, 0x00);
+
+	/* 16 bit audio word length */
+	adv7511_wr_and_or(sd, 0x14, 0xf0, 0x02);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_audio_ops adv7511_audio_ops = {
+	.s_stream = adv7511_s_audio_stream,
+	.s_clock_freq = adv7511_s_clock_freq,
+	.s_i2s_clock_freq = adv7511_s_i2s_clock_freq,
+	.s_routing = adv7511_s_routing,
+};
+
+/* ---------------------------- PAD OPS ------------------------------------- */
+
+static int adv7511_get_edid(struct v4l2_subdev *sd, struct v4l2_edid *edid)
+{
+	struct adv7511_state *state = get_adv7511_state(sd);
+
+	memset(edid->reserved, 0, sizeof(edid->reserved));
+
+	if (edid->pad != 0)
+		return -EINVAL;
+
+	if (edid->start_block == 0 && edid->blocks == 0) {
+		edid->blocks = state->edid.blocks;
+		return 0;
+	}
+
+	if (state->edid.blocks == 0)
+		return -ENODATA;
+
+	if (edid->start_block >= state->edid.blocks)
+		return -EINVAL;
+
+	if (edid->start_block + edid->blocks > state->edid.blocks)
+		edid->blocks = state->edid.blocks - edid->start_block;
+
+	memcpy(edid->edid, &state->edid.data[edid->start_block * 128],
+	       128 * edid->blocks);
+
+	return 0;
+}
+
+static int adv7511_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->pad != 0)
+		return -EINVAL;
+
+	switch (code->index) {
+	case 0:
+		code->code = MEDIA_BUS_FMT_RGB888_1X24;
+		break;
+	case 1:
+		code->code = MEDIA_BUS_FMT_YUYV8_1X16;
+		break;
+	case 2:
+		code->code = MEDIA_BUS_FMT_UYVY8_1X16;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static void adv7511_fill_format(struct adv7511_state *state,
+				struct v4l2_mbus_framefmt *format)
+{
+	format->width = state->dv_timings.bt.width;
+	format->height = state->dv_timings.bt.height;
+	format->field = V4L2_FIELD_NONE;
+}
+
+static int adv7511_get_fmt(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_state *sd_state,
+			   struct v4l2_subdev_format *format)
+{
+	struct adv7511_state *state = get_adv7511_state(sd);
+
+	if (format->pad != 0)
+		return -EINVAL;
+
+	memset(&format->format, 0, sizeof(format->format));
+	adv7511_fill_format(state, &format->format);
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		struct v4l2_mbus_framefmt *fmt;
+
+		fmt = v4l2_subdev_get_try_format(sd, sd_state, format->pad);
+		format->format.code = fmt->code;
+		format->format.colorspace = fmt->colorspace;
+		format->format.ycbcr_enc = fmt->ycbcr_enc;
+		format->format.quantization = fmt->quantization;
+		format->format.xfer_func = fmt->xfer_func;
+	} else {
+		format->format.code = state->fmt_code;
+		format->format.colorspace = state->colorspace;
+		format->format.ycbcr_enc = state->ycbcr_enc;
+		format->format.quantization = state->quantization;
+		format->format.xfer_func = state->xfer_func;
+	}
+
+	return 0;
+}
+
+static int adv7511_set_fmt(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_state *sd_state,
+			   struct v4l2_subdev_format *format)
+{
+	struct adv7511_state *state = get_adv7511_state(sd);
+	/*
+	 * Bitfield namings come the CEA-861-F standard, table 8 "Auxiliary
+	 * Video Information (AVI) InfoFrame Format"
+	 *
+	 * c = Colorimetry
+	 * ec = Extended Colorimetry
+	 * y = RGB or YCbCr
+	 * q = RGB Quantization Range
+	 * yq = YCC Quantization Range
+	 */
+	u8 c = HDMI_COLORIMETRY_NONE;
+	u8 ec = HDMI_EXTENDED_COLORIMETRY_XV_YCC_601;
+	u8 y = HDMI_COLORSPACE_RGB;
+	u8 q = HDMI_QUANTIZATION_RANGE_DEFAULT;
+	u8 yq = HDMI_YCC_QUANTIZATION_RANGE_LIMITED;
+	u8 itc = state->content_type != V4L2_DV_IT_CONTENT_TYPE_NO_ITC;
+	u8 cn = itc ? state->content_type : V4L2_DV_IT_CONTENT_TYPE_GRAPHICS;
+
+	if (format->pad != 0)
+		return -EINVAL;
+	switch (format->format.code) {
+	case MEDIA_BUS_FMT_UYVY8_1X16:
+	case MEDIA_BUS_FMT_YUYV8_1X16:
+	case MEDIA_BUS_FMT_RGB888_1X24:
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	adv7511_fill_format(state, &format->format);
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		struct v4l2_mbus_framefmt *fmt;
+
+		fmt = v4l2_subdev_get_try_format(sd, sd_state, format->pad);
+		fmt->code = format->format.code;
+		fmt->colorspace = format->format.colorspace;
+		fmt->ycbcr_enc = format->format.ycbcr_enc;
+		fmt->quantization = format->format.quantization;
+		fmt->xfer_func = format->format.xfer_func;
+		return 0;
+	}
+
+	switch (format->format.code) {
+	case MEDIA_BUS_FMT_UYVY8_1X16:
+		adv7511_wr_and_or(sd, 0x15, 0xf0, 0x01);
+		adv7511_wr_and_or(sd, 0x16, 0x03, 0xb8);
+		y = HDMI_COLORSPACE_YUV422;
+		break;
+	case MEDIA_BUS_FMT_YUYV8_1X16:
+		adv7511_wr_and_or(sd, 0x15, 0xf0, 0x01);
+		adv7511_wr_and_or(sd, 0x16, 0x03, 0xbc);
+		y = HDMI_COLORSPACE_YUV422;
+		break;
+	case MEDIA_BUS_FMT_RGB888_1X24:
+	default:
+		adv7511_wr_and_or(sd, 0x15, 0xf0, 0x00);
+		adv7511_wr_and_or(sd, 0x16, 0x03, 0x00);
+		break;
+	}
+	state->fmt_code = format->format.code;
+	state->colorspace = format->format.colorspace;
+	state->ycbcr_enc = format->format.ycbcr_enc;
+	state->quantization = format->format.quantization;
+	state->xfer_func = format->format.xfer_func;
+
+	switch (format->format.colorspace) {
+	case V4L2_COLORSPACE_OPRGB:
+		c = HDMI_COLORIMETRY_EXTENDED;
+		ec = y ? HDMI_EXTENDED_COLORIMETRY_OPYCC_601 :
+			 HDMI_EXTENDED_COLORIMETRY_OPRGB;
+		break;
+	case V4L2_COLORSPACE_SMPTE170M:
+		c = y ? HDMI_COLORIMETRY_ITU_601 : HDMI_COLORIMETRY_NONE;
+		if (y && format->format.ycbcr_enc == V4L2_YCBCR_ENC_XV601) {
+			c = HDMI_COLORIMETRY_EXTENDED;
+			ec = HDMI_EXTENDED_COLORIMETRY_XV_YCC_601;
+		}
+		break;
+	case V4L2_COLORSPACE_REC709:
+		c = y ? HDMI_COLORIMETRY_ITU_709 : HDMI_COLORIMETRY_NONE;
+		if (y && format->format.ycbcr_enc == V4L2_YCBCR_ENC_XV709) {
+			c = HDMI_COLORIMETRY_EXTENDED;
+			ec = HDMI_EXTENDED_COLORIMETRY_XV_YCC_709;
+		}
+		break;
+	case V4L2_COLORSPACE_SRGB:
+		c = y ? HDMI_COLORIMETRY_EXTENDED : HDMI_COLORIMETRY_NONE;
+		ec = y ? HDMI_EXTENDED_COLORIMETRY_S_YCC_601 :
+			 HDMI_EXTENDED_COLORIMETRY_XV_YCC_601;
+		break;
+	case V4L2_COLORSPACE_BT2020:
+		c = HDMI_COLORIMETRY_EXTENDED;
+		if (y && format->format.ycbcr_enc == V4L2_YCBCR_ENC_BT2020_CONST_LUM)
+			ec = 5; /* Not yet available in hdmi.h */
+		else
+			ec = 6; /* Not yet available in hdmi.h */
+		break;
+	default:
+		break;
+	}
+
+	/*
+	 * CEA-861-F says that for RGB formats the YCC range must match the
+	 * RGB range, although sources should ignore the YCC range.
+	 *
+	 * The RGB quantization range shouldn't be non-zero if the EDID doesn't
+	 * have the Q bit set in the Video Capabilities Data Block, however this
+	 * isn't checked at the moment. The assumption is that the application
+	 * knows the EDID and can detect this.
+	 *
+	 * The same is true for the YCC quantization range: non-standard YCC
+	 * quantization ranges should only be sent if the EDID has the YQ bit
+	 * set in the Video Capabilities Data Block.
+	 */
+	switch (format->format.quantization) {
+	case V4L2_QUANTIZATION_FULL_RANGE:
+		q = y ? HDMI_QUANTIZATION_RANGE_DEFAULT :
+			HDMI_QUANTIZATION_RANGE_FULL;
+		yq = q ? q - 1 : HDMI_YCC_QUANTIZATION_RANGE_FULL;
+		break;
+	case V4L2_QUANTIZATION_LIM_RANGE:
+		q = y ? HDMI_QUANTIZATION_RANGE_DEFAULT :
+			HDMI_QUANTIZATION_RANGE_LIMITED;
+		yq = q ? q - 1 : HDMI_YCC_QUANTIZATION_RANGE_LIMITED;
+		break;
+	}
+
+	adv7511_wr_and_or(sd, 0x4a, 0xbf, 0);
+	adv7511_wr_and_or(sd, 0x55, 0x9f, y << 5);
+	adv7511_wr_and_or(sd, 0x56, 0x3f, c << 6);
+	adv7511_wr_and_or(sd, 0x57, 0x83, (ec << 4) | (q << 2) | (itc << 7));
+	adv7511_wr_and_or(sd, 0x59, 0x0f, (yq << 6) | (cn << 4));
+	adv7511_wr_and_or(sd, 0x4a, 0xff, 1);
+	adv7511_set_rgb_quantization_mode(sd, state->rgb_quantization_range_ctrl);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_pad_ops adv7511_pad_ops = {
+	.get_edid = adv7511_get_edid,
+	.enum_mbus_code = adv7511_enum_mbus_code,
+	.get_fmt = adv7511_get_fmt,
+	.set_fmt = adv7511_set_fmt,
+	.enum_dv_timings = adv7511_enum_dv_timings,
+	.dv_timings_cap = adv7511_dv_timings_cap,
+};
+
+/* --------------------- SUBDEV OPS --------------------------------------- */
+
+static const struct v4l2_subdev_ops adv7511_ops = {
+	.core  = &adv7511_core_ops,
+	.pad  = &adv7511_pad_ops,
+	.video = &adv7511_video_ops,
+	.audio = &adv7511_audio_ops,
+};
+
+/* ----------------------------------------------------------------------- */
+static void adv7511_dbg_dump_edid(int lvl, int debug, struct v4l2_subdev *sd, int segment, u8 *buf)
+{
+	if (debug >= lvl) {
+		int i, j;
+		v4l2_dbg(lvl, debug, sd, "edid segment %d\n", segment);
+		for (i = 0; i < 256; i += 16) {
+			u8 b[128];
+			u8 *bp = b;
+			if (i == 128)
+				v4l2_dbg(lvl, debug, sd, "\n");
+			for (j = i; j < i + 16; j++) {
+				sprintf(bp, "0x%02x, ", buf[j]);
+				bp += 6;
+			}
+			bp[0] = '\0';
+			v4l2_dbg(lvl, debug, sd, "%s\n", b);
+		}
+	}
+}
+
+static void adv7511_notify_no_edid(struct v4l2_subdev *sd)
+{
+	struct adv7511_state *state = get_adv7511_state(sd);
+	struct adv7511_edid_detect ed;
+
+	/* We failed to read the EDID, so send an event for this. */
+	ed.present = false;
+	ed.segment = adv7511_rd(sd, 0xc4);
+	ed.phys_addr = CEC_PHYS_ADDR_INVALID;
+	cec_s_phys_addr(state->cec_adap, ed.phys_addr, false);
+	v4l2_subdev_notify(sd, ADV7511_EDID_DETECT, (void *)&ed);
+	v4l2_ctrl_s_ctrl(state->have_edid0_ctrl, 0x0);
+}
+
+static void adv7511_edid_handler(struct work_struct *work)
+{
+	struct delayed_work *dwork = to_delayed_work(work);
+	struct adv7511_state *state = container_of(dwork, struct adv7511_state, edid_handler);
+	struct v4l2_subdev *sd = &state->sd;
+
+	v4l2_dbg(1, debug, sd, "%s:\n", __func__);
+
+	if (adv7511_check_edid_status(sd)) {
+		/* Return if we received the EDID. */
+		return;
+	}
+
+	if (adv7511_have_hotplug(sd)) {
+		/* We must retry reading the EDID several times, it is possible
+		 * that initially the EDID couldn't be read due to i2c errors
+		 * (DVI connectors are particularly prone to this problem). */
+		if (state->edid.read_retries) {
+			state->edid.read_retries--;
+			v4l2_dbg(1, debug, sd, "%s: edid read failed\n", __func__);
+			state->have_monitor = false;
+			adv7511_s_power(sd, false);
+			adv7511_s_power(sd, true);
+			queue_delayed_work(state->work_queue, &state->edid_handler, EDID_DELAY);
+			return;
+		}
+	}
+
+	/* We failed to read the EDID, so send an event for this. */
+	adv7511_notify_no_edid(sd);
+	v4l2_dbg(1, debug, sd, "%s: no edid found\n", __func__);
+}
+
+static void adv7511_audio_setup(struct v4l2_subdev *sd)
+{
+	v4l2_dbg(1, debug, sd, "%s\n", __func__);
+
+	adv7511_s_i2s_clock_freq(sd, 48000);
+	adv7511_s_clock_freq(sd, 48000);
+	adv7511_s_routing(sd, 0, 0, 0);
+}
+
+/* Configure hdmi transmitter. */
+static void adv7511_setup(struct v4l2_subdev *sd)
+{
+	struct adv7511_state *state = get_adv7511_state(sd);
+	v4l2_dbg(1, debug, sd, "%s\n", __func__);
+
+	/* Input format: RGB 4:4:4 */
+	adv7511_wr_and_or(sd, 0x15, 0xf0, 0x0);
+	/* Output format: RGB 4:4:4 */
+	adv7511_wr_and_or(sd, 0x16, 0x7f, 0x0);
+	/* 1st order interpolation 4:2:2 -> 4:4:4 up conversion, Aspect ratio: 16:9 */
+	adv7511_wr_and_or(sd, 0x17, 0xf9, 0x06);
+	/* Disable pixel repetition */
+	adv7511_wr_and_or(sd, 0x3b, 0x9f, 0x0);
+	/* Disable CSC */
+	adv7511_wr_and_or(sd, 0x18, 0x7f, 0x0);
+	/* Output format: RGB 4:4:4, Active Format Information is valid,
+	 * underscanned */
+	adv7511_wr_and_or(sd, 0x55, 0x9c, 0x12);
+	/* AVI Info frame packet enable, Audio Info frame disable */
+	adv7511_wr_and_or(sd, 0x44, 0xe7, 0x10);
+	/* Colorimetry, Active format aspect ratio: same as picure. */
+	adv7511_wr(sd, 0x56, 0xa8);
+	/* No encryption */
+	adv7511_wr_and_or(sd, 0xaf, 0xed, 0x0);
+
+	/* Positive clk edge capture for input video clock */
+	adv7511_wr_and_or(sd, 0xba, 0x1f, 0x60);
+
+	adv7511_audio_setup(sd);
+
+	v4l2_ctrl_handler_setup(&state->hdl);
+}
+
+static void adv7511_notify_monitor_detect(struct v4l2_subdev *sd)
+{
+	struct adv7511_monitor_detect mdt;
+	struct adv7511_state *state = get_adv7511_state(sd);
+
+	mdt.present = state->have_monitor;
+	v4l2_subdev_notify(sd, ADV7511_MONITOR_DETECT, (void *)&mdt);
+}
+
+static void adv7511_check_monitor_present_status(struct v4l2_subdev *sd)
+{
+	struct adv7511_state *state = get_adv7511_state(sd);
+	/* read hotplug and rx-sense state */
+	u8 status = adv7511_rd(sd, 0x42);
+
+	v4l2_dbg(1, debug, sd, "%s: status: 0x%x%s%s\n",
+			 __func__,
+			 status,
+			 status & MASK_ADV7511_HPD_DETECT ? ", hotplug" : "",
+			 status & MASK_ADV7511_MSEN_DETECT ? ", rx-sense" : "");
+
+	/* update read only ctrls */
+	v4l2_ctrl_s_ctrl(state->hotplug_ctrl, adv7511_have_hotplug(sd) ? 0x1 : 0x0);
+	v4l2_ctrl_s_ctrl(state->rx_sense_ctrl, adv7511_have_rx_sense(sd) ? 0x1 : 0x0);
+
+	if ((status & MASK_ADV7511_HPD_DETECT) && ((status & MASK_ADV7511_MSEN_DETECT) || state->edid.segments)) {
+		v4l2_dbg(1, debug, sd, "%s: hotplug and (rx-sense or edid)\n", __func__);
+		if (!state->have_monitor) {
+			v4l2_dbg(1, debug, sd, "%s: monitor detected\n", __func__);
+			state->have_monitor = true;
+			adv7511_set_isr(sd, true);
+			if (!adv7511_s_power(sd, true)) {
+				v4l2_dbg(1, debug, sd, "%s: monitor detected, powerup failed\n", __func__);
+				return;
+			}
+			adv7511_setup(sd);
+			adv7511_notify_monitor_detect(sd);
+			state->edid.read_retries = EDID_MAX_RETRIES;
+			queue_delayed_work(state->work_queue, &state->edid_handler, EDID_DELAY);
+		}
+	} else if (status & MASK_ADV7511_HPD_DETECT) {
+		v4l2_dbg(1, debug, sd, "%s: hotplug detected\n", __func__);
+		state->edid.read_retries = EDID_MAX_RETRIES;
+		queue_delayed_work(state->work_queue, &state->edid_handler, EDID_DELAY);
+	} else if (!(status & MASK_ADV7511_HPD_DETECT)) {
+		v4l2_dbg(1, debug, sd, "%s: hotplug not detected\n", __func__);
+		if (state->have_monitor) {
+			v4l2_dbg(1, debug, sd, "%s: monitor not detected\n", __func__);
+			state->have_monitor = false;
+			adv7511_notify_monitor_detect(sd);
+		}
+		adv7511_s_power(sd, false);
+		memset(&state->edid, 0, sizeof(struct adv7511_state_edid));
+		adv7511_notify_no_edid(sd);
+	}
+}
+
+static bool edid_block_verify_crc(u8 *edid_block)
+{
+	u8 sum = 0;
+	int i;
+
+	for (i = 0; i < 128; i++)
+		sum += edid_block[i];
+	return sum == 0;
+}
+
+static bool edid_verify_crc(struct v4l2_subdev *sd, u32 segment)
+{
+	struct adv7511_state *state = get_adv7511_state(sd);
+	u32 blocks = state->edid.blocks;
+	u8 *data = state->edid.data;
+
+	if (!edid_block_verify_crc(&data[segment * 256]))
+		return false;
+	if ((segment + 1) * 2 <= blocks)
+		return edid_block_verify_crc(&data[segment * 256 + 128]);
+	return true;
+}
+
+static bool edid_verify_header(struct v4l2_subdev *sd, u32 segment)
+{
+	static const u8 hdmi_header[] = {
+		0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00
+	};
+	struct adv7511_state *state = get_adv7511_state(sd);
+	u8 *data = state->edid.data;
+
+	if (segment != 0)
+		return true;
+	return !memcmp(data, hdmi_header, sizeof(hdmi_header));
+}
+
+static bool adv7511_check_edid_status(struct v4l2_subdev *sd)
+{
+	struct adv7511_state *state = get_adv7511_state(sd);
+	u8 edidRdy = adv7511_rd(sd, 0xc5);
+
+	v4l2_dbg(1, debug, sd, "%s: edid ready (retries: %d)\n",
+			 __func__, EDID_MAX_RETRIES - state->edid.read_retries);
+
+	if (state->edid.complete)
+		return true;
+
+	if (edidRdy & MASK_ADV7511_EDID_RDY) {
+		int segment = adv7511_rd(sd, 0xc4);
+		struct adv7511_edid_detect ed;
+		int err;
+
+		if (segment >= EDID_MAX_SEGM) {
+			v4l2_err(sd, "edid segment number too big\n");
+			return false;
+		}
+		v4l2_dbg(1, debug, sd, "%s: got segment %d\n", __func__, segment);
+		err = adv7511_edid_rd(sd, 256, &state->edid.data[segment * 256]);
+		if (!err) {
+			adv7511_dbg_dump_edid(2, debug, sd, segment, &state->edid.data[segment * 256]);
+			if (segment == 0) {
+				state->edid.blocks = state->edid.data[0x7e] + 1;
+				v4l2_dbg(1, debug, sd, "%s: %d blocks in total\n",
+					 __func__, state->edid.blocks);
+			}
+		}
+
+		if (err || !edid_verify_crc(sd, segment) || !edid_verify_header(sd, segment)) {
+			/* Couldn't read EDID or EDID is invalid. Force retry! */
+			if (!err)
+				v4l2_err(sd, "%s: edid crc or header error\n", __func__);
+			state->have_monitor = false;
+			adv7511_s_power(sd, false);
+			adv7511_s_power(sd, true);
+			return false;
+		}
+		/* one more segment read ok */
+		state->edid.segments = segment + 1;
+		v4l2_ctrl_s_ctrl(state->have_edid0_ctrl, 0x1);
+		if (((state->edid.data[0x7e] >> 1) + 1) > state->edid.segments) {
+			/* Request next EDID segment */
+			v4l2_dbg(1, debug, sd, "%s: request segment %d\n", __func__, state->edid.segments);
+			adv7511_wr(sd, 0xc9, 0xf);
+			adv7511_wr(sd, 0xc4, state->edid.segments);
+			state->edid.read_retries = EDID_MAX_RETRIES;
+			queue_delayed_work(state->work_queue, &state->edid_handler, EDID_DELAY);
+			return false;
+		}
+
+		v4l2_dbg(1, debug, sd, "%s: edid complete with %d segment(s)\n", __func__, state->edid.segments);
+		state->edid.complete = true;
+		ed.phys_addr = cec_get_edid_phys_addr(state->edid.data,
+						      state->edid.segments * 256,
+						      NULL);
+		/* report when we have all segments
+		   but report only for segment 0
+		 */
+		ed.present = true;
+		ed.segment = 0;
+		state->edid_detect_counter++;
+		cec_s_phys_addr(state->cec_adap, ed.phys_addr, false);
+		v4l2_subdev_notify(sd, ADV7511_EDID_DETECT, (void *)&ed);
+		return ed.present;
+	}
+
+	return false;
+}
+
+static int adv7511_registered(struct v4l2_subdev *sd)
+{
+	struct adv7511_state *state = get_adv7511_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int err;
+
+	err = cec_register_adapter(state->cec_adap, &client->dev);
+	if (err)
+		cec_delete_adapter(state->cec_adap);
+	return err;
+}
+
+static void adv7511_unregistered(struct v4l2_subdev *sd)
+{
+	struct adv7511_state *state = get_adv7511_state(sd);
+
+	cec_unregister_adapter(state->cec_adap);
+}
+
+static const struct v4l2_subdev_internal_ops adv7511_int_ops = {
+	.registered = adv7511_registered,
+	.unregistered = adv7511_unregistered,
+};
+
+/* ----------------------------------------------------------------------- */
+/* Setup ADV7511 */
+static void adv7511_init_setup(struct v4l2_subdev *sd)
+{
+	struct adv7511_state *state = get_adv7511_state(sd);
+	struct adv7511_state_edid *edid = &state->edid;
+	u32 cec_clk = state->pdata.cec_clk;
+	u8 ratio;
+
+	v4l2_dbg(1, debug, sd, "%s\n", __func__);
+
+	/* clear all interrupts */
+	adv7511_wr(sd, 0x96, 0xff);
+	adv7511_wr(sd, 0x97, 0xff);
+	/*
+	 * Stop HPD from resetting a lot of registers.
+	 * It might leave the chip in a partly un-initialized state,
+	 * in particular with regards to hotplug bounces.
+	 */
+	adv7511_wr_and_or(sd, 0xd6, 0x3f, 0xc0);
+	memset(edid, 0, sizeof(struct adv7511_state_edid));
+	state->have_monitor = false;
+	adv7511_set_isr(sd, false);
+	adv7511_s_stream(sd, false);
+	adv7511_s_audio_stream(sd, false);
+
+	if (state->i2c_cec == NULL)
+		return;
+
+	v4l2_dbg(1, debug, sd, "%s: cec_clk %d\n", __func__, cec_clk);
+
+	/* cec soft reset */
+	adv7511_cec_write(sd, 0x50, 0x01);
+	adv7511_cec_write(sd, 0x50, 0x00);
+
+	/* legacy mode */
+	adv7511_cec_write(sd, 0x4a, 0x00);
+	adv7511_cec_write(sd, 0x4a, 0x07);
+
+	if (cec_clk % 750000 != 0)
+		v4l2_err(sd, "%s: cec_clk %d, not multiple of 750 Khz\n",
+			 __func__, cec_clk);
+
+	ratio = (cec_clk / 750000) - 1;
+	adv7511_cec_write(sd, 0x4e, ratio << 2);
+}
+
+static int adv7511_probe(struct i2c_client *client)
+{
+	struct adv7511_state *state;
+	struct adv7511_platform_data *pdata = client->dev.platform_data;
+	struct v4l2_ctrl_handler *hdl;
+	struct v4l2_subdev *sd;
+	u8 chip_id[2];
+	int err = -EIO;
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+
+	state = devm_kzalloc(&client->dev, sizeof(struct adv7511_state), GFP_KERNEL);
+	if (!state)
+		return -ENOMEM;
+
+	/* Platform data */
+	if (!pdata) {
+		v4l_err(client, "No platform data!\n");
+		return -ENODEV;
+	}
+	memcpy(&state->pdata, pdata, sizeof(state->pdata));
+	state->fmt_code = MEDIA_BUS_FMT_RGB888_1X24;
+	state->colorspace = V4L2_COLORSPACE_SRGB;
+
+	sd = &state->sd;
+
+	v4l2_dbg(1, debug, sd, "detecting adv7511 client on address 0x%x\n",
+			 client->addr << 1);
+
+	v4l2_i2c_subdev_init(sd, client, &adv7511_ops);
+	sd->internal_ops = &adv7511_int_ops;
+
+	hdl = &state->hdl;
+	v4l2_ctrl_handler_init(hdl, 10);
+	/* add in ascending ID order */
+	state->hdmi_mode_ctrl = v4l2_ctrl_new_std_menu(hdl, &adv7511_ctrl_ops,
+			V4L2_CID_DV_TX_MODE, V4L2_DV_TX_MODE_HDMI,
+			0, V4L2_DV_TX_MODE_DVI_D);
+	state->hotplug_ctrl = v4l2_ctrl_new_std(hdl, NULL,
+			V4L2_CID_DV_TX_HOTPLUG, 0, 1, 0, 0);
+	state->rx_sense_ctrl = v4l2_ctrl_new_std(hdl, NULL,
+			V4L2_CID_DV_TX_RXSENSE, 0, 1, 0, 0);
+	state->have_edid0_ctrl = v4l2_ctrl_new_std(hdl, NULL,
+			V4L2_CID_DV_TX_EDID_PRESENT, 0, 1, 0, 0);
+	state->rgb_quantization_range_ctrl =
+		v4l2_ctrl_new_std_menu(hdl, &adv7511_ctrl_ops,
+			V4L2_CID_DV_TX_RGB_RANGE, V4L2_DV_RGB_RANGE_FULL,
+			0, V4L2_DV_RGB_RANGE_AUTO);
+	state->content_type_ctrl =
+		v4l2_ctrl_new_std_menu(hdl, &adv7511_ctrl_ops,
+			V4L2_CID_DV_TX_IT_CONTENT_TYPE, V4L2_DV_IT_CONTENT_TYPE_NO_ITC,
+			0, V4L2_DV_IT_CONTENT_TYPE_NO_ITC);
+	sd->ctrl_handler = hdl;
+	if (hdl->error) {
+		err = hdl->error;
+		goto err_hdl;
+	}
+	state->pad.flags = MEDIA_PAD_FL_SINK;
+	sd->entity.function = MEDIA_ENT_F_DV_ENCODER;
+	err = media_entity_pads_init(&sd->entity, 1, &state->pad);
+	if (err)
+		goto err_hdl;
+
+	/* EDID and CEC i2c addr */
+	state->i2c_edid_addr = state->pdata.i2c_edid << 1;
+	state->i2c_cec_addr = state->pdata.i2c_cec << 1;
+	state->i2c_pktmem_addr = state->pdata.i2c_pktmem << 1;
+
+	state->chip_revision = adv7511_rd(sd, 0x0);
+	chip_id[0] = adv7511_rd(sd, 0xf5);
+	chip_id[1] = adv7511_rd(sd, 0xf6);
+	if (chip_id[0] != 0x75 || chip_id[1] != 0x11) {
+		v4l2_err(sd, "chip_id != 0x7511, read 0x%02x%02x\n", chip_id[0],
+			 chip_id[1]);
+		err = -EIO;
+		goto err_entity;
+	}
+
+	state->i2c_edid = i2c_new_dummy_device(client->adapter,
+					state->i2c_edid_addr >> 1);
+	if (IS_ERR(state->i2c_edid)) {
+		v4l2_err(sd, "failed to register edid i2c client\n");
+		err = PTR_ERR(state->i2c_edid);
+		goto err_entity;
+	}
+
+	adv7511_wr(sd, 0xe1, state->i2c_cec_addr);
+	if (state->pdata.cec_clk < 3000000 ||
+	    state->pdata.cec_clk > 100000000) {
+		v4l2_err(sd, "%s: cec_clk %u outside range, disabling cec\n",
+				__func__, state->pdata.cec_clk);
+		state->pdata.cec_clk = 0;
+	}
+
+	if (state->pdata.cec_clk) {
+		state->i2c_cec = i2c_new_dummy_device(client->adapter,
+					       state->i2c_cec_addr >> 1);
+		if (IS_ERR(state->i2c_cec)) {
+			v4l2_err(sd, "failed to register cec i2c client\n");
+			err = PTR_ERR(state->i2c_cec);
+			goto err_unreg_edid;
+		}
+		adv7511_wr(sd, 0xe2, 0x00); /* power up cec section */
+	} else {
+		adv7511_wr(sd, 0xe2, 0x01); /* power down cec section */
+	}
+
+	state->i2c_pktmem = i2c_new_dummy_device(client->adapter, state->i2c_pktmem_addr >> 1);
+	if (IS_ERR(state->i2c_pktmem)) {
+		v4l2_err(sd, "failed to register pktmem i2c client\n");
+		err = PTR_ERR(state->i2c_pktmem);
+		goto err_unreg_cec;
+	}
+
+	state->work_queue = create_singlethread_workqueue(sd->name);
+	if (state->work_queue == NULL) {
+		v4l2_err(sd, "could not create workqueue\n");
+		err = -ENOMEM;
+		goto err_unreg_pktmem;
+	}
+
+	INIT_DELAYED_WORK(&state->edid_handler, adv7511_edid_handler);
+
+	adv7511_init_setup(sd);
+
+#if IS_ENABLED(CONFIG_VIDEO_ADV7511_CEC)
+	state->cec_adap = cec_allocate_adapter(&adv7511_cec_adap_ops,
+		state, dev_name(&client->dev), CEC_CAP_DEFAULTS,
+		ADV7511_MAX_ADDRS);
+	err = PTR_ERR_OR_ZERO(state->cec_adap);
+	if (err) {
+		destroy_workqueue(state->work_queue);
+		goto err_unreg_pktmem;
+	}
+#endif
+
+	adv7511_set_isr(sd, true);
+	adv7511_check_monitor_present_status(sd);
+
+	v4l2_info(sd, "%s found @ 0x%x (%s)\n", client->name,
+			  client->addr << 1, client->adapter->name);
+	return 0;
+
+err_unreg_pktmem:
+	i2c_unregister_device(state->i2c_pktmem);
+err_unreg_cec:
+	i2c_unregister_device(state->i2c_cec);
+err_unreg_edid:
+	i2c_unregister_device(state->i2c_edid);
+err_entity:
+	media_entity_cleanup(&sd->entity);
+err_hdl:
+	v4l2_ctrl_handler_free(&state->hdl);
+	return err;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static void adv7511_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct adv7511_state *state = get_adv7511_state(sd);
+
+	state->chip_revision = -1;
+
+	v4l2_dbg(1, debug, sd, "%s removed @ 0x%x (%s)\n", client->name,
+		 client->addr << 1, client->adapter->name);
+
+	adv7511_set_isr(sd, false);
+	adv7511_init_setup(sd);
+	cancel_delayed_work_sync(&state->edid_handler);
+	i2c_unregister_device(state->i2c_edid);
+	i2c_unregister_device(state->i2c_cec);
+	i2c_unregister_device(state->i2c_pktmem);
+	destroy_workqueue(state->work_queue);
+	v4l2_device_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id adv7511_id[] = {
+	{ "adv7511-v4l2", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, adv7511_id);
+
+static struct i2c_driver adv7511_driver = {
+	.driver = {
+		.name = "adv7511-v4l2",
+	},
+	.probe = adv7511_probe,
+	.remove = adv7511_remove,
+	.id_table = adv7511_id,
+};
+
+module_i2c_driver(adv7511_driver);
diff --git a/drivers/media/i2c/adv7604.c b/drivers/media/i2c/adv7604.c
new file mode 100644
index 000000000..b202a85fb
--- /dev/null
+++ b/drivers/media/i2c/adv7604.c
@@ -0,0 +1,3697 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * adv7604 - Analog Devices ADV7604 video decoder driver
+ *
+ * Copyright 2012 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ */
+
+/*
+ * References (c = chapter, p = page):
+ * REF_01 - Analog devices, ADV7604, Register Settings Recommendations,
+ *		Revision 2.5, June 2010
+ * REF_02 - Analog devices, Register map documentation, Documentation of
+ *		the register maps, Software manual, Rev. F, June 2010
+ * REF_03 - Analog devices, ADV7604, Hardware Manual, Rev. F, August 2010
+ */
+
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/hdmi.h>
+#include <linux/i2c.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of_graph.h>
+#include <linux/slab.h>
+#include <linux/v4l2-dv-timings.h>
+#include <linux/videodev2.h>
+#include <linux/workqueue.h>
+#include <linux/regmap.h>
+#include <linux/interrupt.h>
+
+#include <media/i2c/adv7604.h>
+#include <media/cec.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-dv-timings.h>
+#include <media/v4l2-fwnode.h>
+
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "debug level (0-2)");
+
+MODULE_DESCRIPTION("Analog Devices ADV7604/10/11/12 video decoder driver");
+MODULE_AUTHOR("Hans Verkuil <hans.verkuil@cisco.com>");
+MODULE_AUTHOR("Mats Randgaard <mats.randgaard@cisco.com>");
+MODULE_LICENSE("GPL");
+
+/* ADV7604 system clock frequency */
+#define ADV76XX_FSC (28636360)
+
+#define ADV76XX_RGB_OUT					(1 << 1)
+
+#define ADV76XX_OP_FORMAT_SEL_8BIT			(0 << 0)
+#define ADV7604_OP_FORMAT_SEL_10BIT			(1 << 0)
+#define ADV76XX_OP_FORMAT_SEL_12BIT			(2 << 0)
+
+#define ADV76XX_OP_MODE_SEL_SDR_422			(0 << 5)
+#define ADV7604_OP_MODE_SEL_DDR_422			(1 << 5)
+#define ADV76XX_OP_MODE_SEL_SDR_444			(2 << 5)
+#define ADV7604_OP_MODE_SEL_DDR_444			(3 << 5)
+#define ADV76XX_OP_MODE_SEL_SDR_422_2X			(4 << 5)
+#define ADV7604_OP_MODE_SEL_ADI_CM			(5 << 5)
+
+#define ADV76XX_OP_CH_SEL_GBR				(0 << 5)
+#define ADV76XX_OP_CH_SEL_GRB				(1 << 5)
+#define ADV76XX_OP_CH_SEL_BGR				(2 << 5)
+#define ADV76XX_OP_CH_SEL_RGB				(3 << 5)
+#define ADV76XX_OP_CH_SEL_BRG				(4 << 5)
+#define ADV76XX_OP_CH_SEL_RBG				(5 << 5)
+
+#define ADV76XX_OP_SWAP_CB_CR				(1 << 0)
+
+#define ADV76XX_MAX_ADDRS (3)
+
+#define ADV76XX_MAX_EDID_BLOCKS 4
+
+enum adv76xx_type {
+	ADV7604,
+	ADV7611, // including ADV7610
+	ADV7612,
+};
+
+struct adv76xx_reg_seq {
+	unsigned int reg;
+	u8 val;
+};
+
+struct adv76xx_format_info {
+	u32 code;
+	u8 op_ch_sel;
+	bool rgb_out;
+	bool swap_cb_cr;
+	u8 op_format_sel;
+};
+
+struct adv76xx_cfg_read_infoframe {
+	const char *desc;
+	u8 present_mask;
+	u8 head_addr;
+	u8 payload_addr;
+};
+
+struct adv76xx_chip_info {
+	enum adv76xx_type type;
+
+	bool has_afe;
+	unsigned int max_port;
+	unsigned int num_dv_ports;
+
+	unsigned int edid_enable_reg;
+	unsigned int edid_status_reg;
+	unsigned int edid_segment_reg;
+	unsigned int edid_segment_mask;
+	unsigned int edid_spa_loc_reg;
+	unsigned int edid_spa_loc_msb_mask;
+	unsigned int edid_spa_port_b_reg;
+	unsigned int lcf_reg;
+
+	unsigned int cable_det_mask;
+	unsigned int tdms_lock_mask;
+	unsigned int fmt_change_digital_mask;
+	unsigned int cp_csc;
+
+	unsigned int cec_irq_status;
+	unsigned int cec_rx_enable;
+	unsigned int cec_rx_enable_mask;
+	bool cec_irq_swap;
+
+	const struct adv76xx_format_info *formats;
+	unsigned int nformats;
+
+	void (*set_termination)(struct v4l2_subdev *sd, bool enable);
+	void (*setup_irqs)(struct v4l2_subdev *sd);
+	unsigned int (*read_hdmi_pixelclock)(struct v4l2_subdev *sd);
+	unsigned int (*read_cable_det)(struct v4l2_subdev *sd);
+
+	/* 0 = AFE, 1 = HDMI */
+	const struct adv76xx_reg_seq *recommended_settings[2];
+	unsigned int num_recommended_settings[2];
+
+	unsigned long page_mask;
+
+	/* Masks for timings */
+	unsigned int linewidth_mask;
+	unsigned int field0_height_mask;
+	unsigned int field1_height_mask;
+	unsigned int hfrontporch_mask;
+	unsigned int hsync_mask;
+	unsigned int hbackporch_mask;
+	unsigned int field0_vfrontporch_mask;
+	unsigned int field1_vfrontporch_mask;
+	unsigned int field0_vsync_mask;
+	unsigned int field1_vsync_mask;
+	unsigned int field0_vbackporch_mask;
+	unsigned int field1_vbackporch_mask;
+};
+
+/*
+ **********************************************************************
+ *
+ *  Arrays with configuration parameters for the ADV7604
+ *
+ **********************************************************************
+ */
+
+struct adv76xx_state {
+	const struct adv76xx_chip_info *info;
+	struct adv76xx_platform_data pdata;
+
+	struct gpio_desc *hpd_gpio[4];
+	struct gpio_desc *reset_gpio;
+
+	struct v4l2_subdev sd;
+	struct media_pad pads[ADV76XX_PAD_MAX];
+	unsigned int source_pad;
+
+	struct v4l2_ctrl_handler hdl;
+
+	enum adv76xx_pad selected_input;
+
+	struct v4l2_dv_timings timings;
+	const struct adv76xx_format_info *format;
+
+	struct {
+		u8 edid[ADV76XX_MAX_EDID_BLOCKS * 128];
+		u32 present;
+		unsigned blocks;
+	} edid;
+	u16 spa_port_a[2];
+	struct v4l2_fract aspect_ratio;
+	u32 rgb_quantization_range;
+	struct delayed_work delayed_work_enable_hotplug;
+	bool restart_stdi_once;
+
+	/* CEC */
+	struct cec_adapter *cec_adap;
+	u8   cec_addr[ADV76XX_MAX_ADDRS];
+	u8   cec_valid_addrs;
+	bool cec_enabled_adap;
+
+	/* i2c clients */
+	struct i2c_client *i2c_clients[ADV76XX_PAGE_MAX];
+
+	/* Regmaps */
+	struct regmap *regmap[ADV76XX_PAGE_MAX];
+
+	/* controls */
+	struct v4l2_ctrl *detect_tx_5v_ctrl;
+	struct v4l2_ctrl *analog_sampling_phase_ctrl;
+	struct v4l2_ctrl *free_run_color_manual_ctrl;
+	struct v4l2_ctrl *free_run_color_ctrl;
+	struct v4l2_ctrl *rgb_quantization_range_ctrl;
+};
+
+static bool adv76xx_has_afe(struct adv76xx_state *state)
+{
+	return state->info->has_afe;
+}
+
+/* Unsupported timings. This device cannot support 720p30. */
+static const struct v4l2_dv_timings adv76xx_timings_exceptions[] = {
+	V4L2_DV_BT_CEA_1280X720P30,
+	{ }
+};
+
+static bool adv76xx_check_dv_timings(const struct v4l2_dv_timings *t, void *hdl)
+{
+	int i;
+
+	for (i = 0; adv76xx_timings_exceptions[i].bt.width; i++)
+		if (v4l2_match_dv_timings(t, adv76xx_timings_exceptions + i, 0, false))
+			return false;
+	return true;
+}
+
+struct adv76xx_video_standards {
+	struct v4l2_dv_timings timings;
+	u8 vid_std;
+	u8 v_freq;
+};
+
+/* sorted by number of lines */
+static const struct adv76xx_video_standards adv7604_prim_mode_comp[] = {
+	/* { V4L2_DV_BT_CEA_720X480P59_94, 0x0a, 0x00 }, TODO flickering */
+	{ V4L2_DV_BT_CEA_720X576P50, 0x0b, 0x00 },
+	{ V4L2_DV_BT_CEA_1280X720P50, 0x19, 0x01 },
+	{ V4L2_DV_BT_CEA_1280X720P60, 0x19, 0x00 },
+	{ V4L2_DV_BT_CEA_1920X1080P24, 0x1e, 0x04 },
+	{ V4L2_DV_BT_CEA_1920X1080P25, 0x1e, 0x03 },
+	{ V4L2_DV_BT_CEA_1920X1080P30, 0x1e, 0x02 },
+	{ V4L2_DV_BT_CEA_1920X1080P50, 0x1e, 0x01 },
+	{ V4L2_DV_BT_CEA_1920X1080P60, 0x1e, 0x00 },
+	/* TODO add 1920x1080P60_RB (CVT timing) */
+	{ },
+};
+
+/* sorted by number of lines */
+static const struct adv76xx_video_standards adv7604_prim_mode_gr[] = {
+	{ V4L2_DV_BT_DMT_640X480P60, 0x08, 0x00 },
+	{ V4L2_DV_BT_DMT_640X480P72, 0x09, 0x00 },
+	{ V4L2_DV_BT_DMT_640X480P75, 0x0a, 0x00 },
+	{ V4L2_DV_BT_DMT_640X480P85, 0x0b, 0x00 },
+	{ V4L2_DV_BT_DMT_800X600P56, 0x00, 0x00 },
+	{ V4L2_DV_BT_DMT_800X600P60, 0x01, 0x00 },
+	{ V4L2_DV_BT_DMT_800X600P72, 0x02, 0x00 },
+	{ V4L2_DV_BT_DMT_800X600P75, 0x03, 0x00 },
+	{ V4L2_DV_BT_DMT_800X600P85, 0x04, 0x00 },
+	{ V4L2_DV_BT_DMT_1024X768P60, 0x0c, 0x00 },
+	{ V4L2_DV_BT_DMT_1024X768P70, 0x0d, 0x00 },
+	{ V4L2_DV_BT_DMT_1024X768P75, 0x0e, 0x00 },
+	{ V4L2_DV_BT_DMT_1024X768P85, 0x0f, 0x00 },
+	{ V4L2_DV_BT_DMT_1280X1024P60, 0x05, 0x00 },
+	{ V4L2_DV_BT_DMT_1280X1024P75, 0x06, 0x00 },
+	{ V4L2_DV_BT_DMT_1360X768P60, 0x12, 0x00 },
+	{ V4L2_DV_BT_DMT_1366X768P60, 0x13, 0x00 },
+	{ V4L2_DV_BT_DMT_1400X1050P60, 0x14, 0x00 },
+	{ V4L2_DV_BT_DMT_1400X1050P75, 0x15, 0x00 },
+	{ V4L2_DV_BT_DMT_1600X1200P60, 0x16, 0x00 }, /* TODO not tested */
+	/* TODO add 1600X1200P60_RB (not a DMT timing) */
+	{ V4L2_DV_BT_DMT_1680X1050P60, 0x18, 0x00 },
+	{ V4L2_DV_BT_DMT_1920X1200P60_RB, 0x19, 0x00 }, /* TODO not tested */
+	{ },
+};
+
+/* sorted by number of lines */
+static const struct adv76xx_video_standards adv76xx_prim_mode_hdmi_comp[] = {
+	{ V4L2_DV_BT_CEA_720X480P59_94, 0x0a, 0x00 },
+	{ V4L2_DV_BT_CEA_720X576P50, 0x0b, 0x00 },
+	{ V4L2_DV_BT_CEA_1280X720P50, 0x13, 0x01 },
+	{ V4L2_DV_BT_CEA_1280X720P60, 0x13, 0x00 },
+	{ V4L2_DV_BT_CEA_1920X1080P24, 0x1e, 0x04 },
+	{ V4L2_DV_BT_CEA_1920X1080P25, 0x1e, 0x03 },
+	{ V4L2_DV_BT_CEA_1920X1080P30, 0x1e, 0x02 },
+	{ V4L2_DV_BT_CEA_1920X1080P50, 0x1e, 0x01 },
+	{ V4L2_DV_BT_CEA_1920X1080P60, 0x1e, 0x00 },
+	{ },
+};
+
+/* sorted by number of lines */
+static const struct adv76xx_video_standards adv76xx_prim_mode_hdmi_gr[] = {
+	{ V4L2_DV_BT_DMT_640X480P60, 0x08, 0x00 },
+	{ V4L2_DV_BT_DMT_640X480P72, 0x09, 0x00 },
+	{ V4L2_DV_BT_DMT_640X480P75, 0x0a, 0x00 },
+	{ V4L2_DV_BT_DMT_640X480P85, 0x0b, 0x00 },
+	{ V4L2_DV_BT_DMT_800X600P56, 0x00, 0x00 },
+	{ V4L2_DV_BT_DMT_800X600P60, 0x01, 0x00 },
+	{ V4L2_DV_BT_DMT_800X600P72, 0x02, 0x00 },
+	{ V4L2_DV_BT_DMT_800X600P75, 0x03, 0x00 },
+	{ V4L2_DV_BT_DMT_800X600P85, 0x04, 0x00 },
+	{ V4L2_DV_BT_DMT_1024X768P60, 0x0c, 0x00 },
+	{ V4L2_DV_BT_DMT_1024X768P70, 0x0d, 0x00 },
+	{ V4L2_DV_BT_DMT_1024X768P75, 0x0e, 0x00 },
+	{ V4L2_DV_BT_DMT_1024X768P85, 0x0f, 0x00 },
+	{ V4L2_DV_BT_DMT_1280X1024P60, 0x05, 0x00 },
+	{ V4L2_DV_BT_DMT_1280X1024P75, 0x06, 0x00 },
+	{ },
+};
+
+static const struct v4l2_event adv76xx_ev_fmt = {
+	.type = V4L2_EVENT_SOURCE_CHANGE,
+	.u.src_change.changes = V4L2_EVENT_SRC_CH_RESOLUTION,
+};
+
+/* ----------------------------------------------------------------------- */
+
+static inline struct adv76xx_state *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct adv76xx_state, sd);
+}
+
+static inline unsigned htotal(const struct v4l2_bt_timings *t)
+{
+	return V4L2_DV_BT_FRAME_WIDTH(t);
+}
+
+static inline unsigned vtotal(const struct v4l2_bt_timings *t)
+{
+	return V4L2_DV_BT_FRAME_HEIGHT(t);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static int adv76xx_read_check(struct adv76xx_state *state,
+			     int client_page, u8 reg)
+{
+	struct i2c_client *client = state->i2c_clients[client_page];
+	int err;
+	unsigned int val;
+
+	err = regmap_read(state->regmap[client_page], reg, &val);
+
+	if (err) {
+		v4l_err(client, "error reading %02x, %02x\n",
+				client->addr, reg);
+		return err;
+	}
+	return val;
+}
+
+/* adv76xx_write_block(): Write raw data with a maximum of I2C_SMBUS_BLOCK_MAX
+ * size to one or more registers.
+ *
+ * A value of zero will be returned on success, a negative errno will
+ * be returned in error cases.
+ */
+static int adv76xx_write_block(struct adv76xx_state *state, int client_page,
+			      unsigned int init_reg, const void *val,
+			      size_t val_len)
+{
+	struct regmap *regmap = state->regmap[client_page];
+
+	if (val_len > I2C_SMBUS_BLOCK_MAX)
+		val_len = I2C_SMBUS_BLOCK_MAX;
+
+	return regmap_raw_write(regmap, init_reg, val, val_len);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static inline int io_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	return adv76xx_read_check(state, ADV76XX_PAGE_IO, reg);
+}
+
+static inline int io_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	return regmap_write(state->regmap[ADV76XX_PAGE_IO], reg, val);
+}
+
+static inline int io_write_clr_set(struct v4l2_subdev *sd, u8 reg, u8 mask,
+				   u8 val)
+{
+	return io_write(sd, reg, (io_read(sd, reg) & ~mask) | val);
+}
+
+static inline int __always_unused avlink_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	return adv76xx_read_check(state, ADV7604_PAGE_AVLINK, reg);
+}
+
+static inline int __always_unused avlink_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	return regmap_write(state->regmap[ADV7604_PAGE_AVLINK], reg, val);
+}
+
+static inline int cec_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	return adv76xx_read_check(state, ADV76XX_PAGE_CEC, reg);
+}
+
+static inline int cec_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	return regmap_write(state->regmap[ADV76XX_PAGE_CEC], reg, val);
+}
+
+static inline int cec_write_clr_set(struct v4l2_subdev *sd, u8 reg, u8 mask,
+				   u8 val)
+{
+	return cec_write(sd, reg, (cec_read(sd, reg) & ~mask) | val);
+}
+
+static inline int infoframe_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	return adv76xx_read_check(state, ADV76XX_PAGE_INFOFRAME, reg);
+}
+
+static inline int __always_unused infoframe_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	return regmap_write(state->regmap[ADV76XX_PAGE_INFOFRAME], reg, val);
+}
+
+static inline int __always_unused afe_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	return adv76xx_read_check(state, ADV76XX_PAGE_AFE, reg);
+}
+
+static inline int afe_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	return regmap_write(state->regmap[ADV76XX_PAGE_AFE], reg, val);
+}
+
+static inline int rep_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	return adv76xx_read_check(state, ADV76XX_PAGE_REP, reg);
+}
+
+static inline int rep_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	return regmap_write(state->regmap[ADV76XX_PAGE_REP], reg, val);
+}
+
+static inline int rep_write_clr_set(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
+{
+	return rep_write(sd, reg, (rep_read(sd, reg) & ~mask) | val);
+}
+
+static inline int __always_unused edid_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	return adv76xx_read_check(state, ADV76XX_PAGE_EDID, reg);
+}
+
+static inline int __always_unused edid_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	return regmap_write(state->regmap[ADV76XX_PAGE_EDID], reg, val);
+}
+
+static inline int edid_write_block(struct v4l2_subdev *sd,
+					unsigned int total_len, const u8 *val)
+{
+	struct adv76xx_state *state = to_state(sd);
+	int err = 0;
+	int i = 0;
+	int len = 0;
+
+	v4l2_dbg(2, debug, sd, "%s: write EDID block (%d byte)\n",
+				__func__, total_len);
+
+	while (!err && i < total_len) {
+		len = (total_len - i) > I2C_SMBUS_BLOCK_MAX ?
+				I2C_SMBUS_BLOCK_MAX :
+				(total_len - i);
+
+		err = adv76xx_write_block(state, ADV76XX_PAGE_EDID,
+				i, val + i, len);
+		i += len;
+	}
+
+	return err;
+}
+
+static void adv76xx_set_hpd(struct adv76xx_state *state, unsigned int hpd)
+{
+	const struct adv76xx_chip_info *info = state->info;
+	unsigned int i;
+
+	if (info->type == ADV7604) {
+		for (i = 0; i < state->info->num_dv_ports; ++i)
+			gpiod_set_value_cansleep(state->hpd_gpio[i], hpd & BIT(i));
+	} else {
+		for (i = 0; i < state->info->num_dv_ports; ++i)
+			io_write_clr_set(&state->sd, 0x20, 0x80 >> i,
+					 (!!(hpd & BIT(i))) << (7 - i));
+	}
+
+	v4l2_subdev_notify(&state->sd, ADV76XX_HOTPLUG, &hpd);
+}
+
+static void adv76xx_delayed_work_enable_hotplug(struct work_struct *work)
+{
+	struct delayed_work *dwork = to_delayed_work(work);
+	struct adv76xx_state *state = container_of(dwork, struct adv76xx_state,
+						delayed_work_enable_hotplug);
+	struct v4l2_subdev *sd = &state->sd;
+
+	v4l2_dbg(2, debug, sd, "%s: enable hotplug\n", __func__);
+
+	adv76xx_set_hpd(state, state->edid.present);
+}
+
+static inline int hdmi_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	return adv76xx_read_check(state, ADV76XX_PAGE_HDMI, reg);
+}
+
+static u16 hdmi_read16(struct v4l2_subdev *sd, u8 reg, u16 mask)
+{
+	return ((hdmi_read(sd, reg) << 8) | hdmi_read(sd, reg + 1)) & mask;
+}
+
+static inline int hdmi_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	return regmap_write(state->regmap[ADV76XX_PAGE_HDMI], reg, val);
+}
+
+static inline int hdmi_write_clr_set(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
+{
+	return hdmi_write(sd, reg, (hdmi_read(sd, reg) & ~mask) | val);
+}
+
+static inline int __always_unused test_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	return regmap_write(state->regmap[ADV76XX_PAGE_TEST], reg, val);
+}
+
+static inline int cp_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	return adv76xx_read_check(state, ADV76XX_PAGE_CP, reg);
+}
+
+static u16 cp_read16(struct v4l2_subdev *sd, u8 reg, u16 mask)
+{
+	return ((cp_read(sd, reg) << 8) | cp_read(sd, reg + 1)) & mask;
+}
+
+static inline int cp_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	return regmap_write(state->regmap[ADV76XX_PAGE_CP], reg, val);
+}
+
+static inline int cp_write_clr_set(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
+{
+	return cp_write(sd, reg, (cp_read(sd, reg) & ~mask) | val);
+}
+
+static inline int __always_unused vdp_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	return adv76xx_read_check(state, ADV7604_PAGE_VDP, reg);
+}
+
+static inline int __always_unused vdp_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	return regmap_write(state->regmap[ADV7604_PAGE_VDP], reg, val);
+}
+
+#define ADV76XX_REG(page, offset)	(((page) << 8) | (offset))
+#define ADV76XX_REG_SEQ_TERM		0xffff
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int adv76xx_read_reg(struct v4l2_subdev *sd, unsigned int reg)
+{
+	struct adv76xx_state *state = to_state(sd);
+	unsigned int page = reg >> 8;
+	unsigned int val;
+	int err;
+
+	if (page >= ADV76XX_PAGE_MAX || !(BIT(page) & state->info->page_mask))
+		return -EINVAL;
+
+	reg &= 0xff;
+	err = regmap_read(state->regmap[page], reg, &val);
+
+	return err ? err : val;
+}
+#endif
+
+static int adv76xx_write_reg(struct v4l2_subdev *sd, unsigned int reg, u8 val)
+{
+	struct adv76xx_state *state = to_state(sd);
+	unsigned int page = reg >> 8;
+
+	if (page >= ADV76XX_PAGE_MAX || !(BIT(page) & state->info->page_mask))
+		return -EINVAL;
+
+	reg &= 0xff;
+
+	return regmap_write(state->regmap[page], reg, val);
+}
+
+static void adv76xx_write_reg_seq(struct v4l2_subdev *sd,
+				  const struct adv76xx_reg_seq *reg_seq)
+{
+	unsigned int i;
+
+	for (i = 0; reg_seq[i].reg != ADV76XX_REG_SEQ_TERM; i++)
+		adv76xx_write_reg(sd, reg_seq[i].reg, reg_seq[i].val);
+}
+
+/* -----------------------------------------------------------------------------
+ * Format helpers
+ */
+
+static const struct adv76xx_format_info adv7604_formats[] = {
+	{ MEDIA_BUS_FMT_RGB888_1X24, ADV76XX_OP_CH_SEL_RGB, true, false,
+	  ADV76XX_OP_MODE_SEL_SDR_444 | ADV76XX_OP_FORMAT_SEL_8BIT },
+	{ MEDIA_BUS_FMT_YUYV8_2X8, ADV76XX_OP_CH_SEL_RGB, false, false,
+	  ADV76XX_OP_MODE_SEL_SDR_422 | ADV76XX_OP_FORMAT_SEL_8BIT },
+	{ MEDIA_BUS_FMT_YVYU8_2X8, ADV76XX_OP_CH_SEL_RGB, false, true,
+	  ADV76XX_OP_MODE_SEL_SDR_422 | ADV76XX_OP_FORMAT_SEL_8BIT },
+	{ MEDIA_BUS_FMT_YUYV10_2X10, ADV76XX_OP_CH_SEL_RGB, false, false,
+	  ADV76XX_OP_MODE_SEL_SDR_422 | ADV7604_OP_FORMAT_SEL_10BIT },
+	{ MEDIA_BUS_FMT_YVYU10_2X10, ADV76XX_OP_CH_SEL_RGB, false, true,
+	  ADV76XX_OP_MODE_SEL_SDR_422 | ADV7604_OP_FORMAT_SEL_10BIT },
+	{ MEDIA_BUS_FMT_YUYV12_2X12, ADV76XX_OP_CH_SEL_RGB, false, false,
+	  ADV76XX_OP_MODE_SEL_SDR_422 | ADV76XX_OP_FORMAT_SEL_12BIT },
+	{ MEDIA_BUS_FMT_YVYU12_2X12, ADV76XX_OP_CH_SEL_RGB, false, true,
+	  ADV76XX_OP_MODE_SEL_SDR_422 | ADV76XX_OP_FORMAT_SEL_12BIT },
+	{ MEDIA_BUS_FMT_UYVY8_1X16, ADV76XX_OP_CH_SEL_RBG, false, false,
+	  ADV76XX_OP_MODE_SEL_SDR_422_2X | ADV76XX_OP_FORMAT_SEL_8BIT },
+	{ MEDIA_BUS_FMT_VYUY8_1X16, ADV76XX_OP_CH_SEL_RBG, false, true,
+	  ADV76XX_OP_MODE_SEL_SDR_422_2X | ADV76XX_OP_FORMAT_SEL_8BIT },
+	{ MEDIA_BUS_FMT_YUYV8_1X16, ADV76XX_OP_CH_SEL_RGB, false, false,
+	  ADV76XX_OP_MODE_SEL_SDR_422_2X | ADV76XX_OP_FORMAT_SEL_8BIT },
+	{ MEDIA_BUS_FMT_YVYU8_1X16, ADV76XX_OP_CH_SEL_RGB, false, true,
+	  ADV76XX_OP_MODE_SEL_SDR_422_2X | ADV76XX_OP_FORMAT_SEL_8BIT },
+	{ MEDIA_BUS_FMT_UYVY10_1X20, ADV76XX_OP_CH_SEL_RBG, false, false,
+	  ADV76XX_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_10BIT },
+	{ MEDIA_BUS_FMT_VYUY10_1X20, ADV76XX_OP_CH_SEL_RBG, false, true,
+	  ADV76XX_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_10BIT },
+	{ MEDIA_BUS_FMT_YUYV10_1X20, ADV76XX_OP_CH_SEL_RGB, false, false,
+	  ADV76XX_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_10BIT },
+	{ MEDIA_BUS_FMT_YVYU10_1X20, ADV76XX_OP_CH_SEL_RGB, false, true,
+	  ADV76XX_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_10BIT },
+	{ MEDIA_BUS_FMT_UYVY12_1X24, ADV76XX_OP_CH_SEL_RBG, false, false,
+	  ADV76XX_OP_MODE_SEL_SDR_422_2X | ADV76XX_OP_FORMAT_SEL_12BIT },
+	{ MEDIA_BUS_FMT_VYUY12_1X24, ADV76XX_OP_CH_SEL_RBG, false, true,
+	  ADV76XX_OP_MODE_SEL_SDR_422_2X | ADV76XX_OP_FORMAT_SEL_12BIT },
+	{ MEDIA_BUS_FMT_YUYV12_1X24, ADV76XX_OP_CH_SEL_RGB, false, false,
+	  ADV76XX_OP_MODE_SEL_SDR_422_2X | ADV76XX_OP_FORMAT_SEL_12BIT },
+	{ MEDIA_BUS_FMT_YVYU12_1X24, ADV76XX_OP_CH_SEL_RGB, false, true,
+	  ADV76XX_OP_MODE_SEL_SDR_422_2X | ADV76XX_OP_FORMAT_SEL_12BIT },
+};
+
+static const struct adv76xx_format_info adv7611_formats[] = {
+	{ MEDIA_BUS_FMT_RGB888_1X24, ADV76XX_OP_CH_SEL_RGB, true, false,
+	  ADV76XX_OP_MODE_SEL_SDR_444 | ADV76XX_OP_FORMAT_SEL_8BIT },
+	{ MEDIA_BUS_FMT_YUYV8_2X8, ADV76XX_OP_CH_SEL_RGB, false, false,
+	  ADV76XX_OP_MODE_SEL_SDR_422 | ADV76XX_OP_FORMAT_SEL_8BIT },
+	{ MEDIA_BUS_FMT_YVYU8_2X8, ADV76XX_OP_CH_SEL_RGB, false, true,
+	  ADV76XX_OP_MODE_SEL_SDR_422 | ADV76XX_OP_FORMAT_SEL_8BIT },
+	{ MEDIA_BUS_FMT_YUYV12_2X12, ADV76XX_OP_CH_SEL_RGB, false, false,
+	  ADV76XX_OP_MODE_SEL_SDR_422 | ADV76XX_OP_FORMAT_SEL_12BIT },
+	{ MEDIA_BUS_FMT_YVYU12_2X12, ADV76XX_OP_CH_SEL_RGB, false, true,
+	  ADV76XX_OP_MODE_SEL_SDR_422 | ADV76XX_OP_FORMAT_SEL_12BIT },
+	{ MEDIA_BUS_FMT_UYVY8_1X16, ADV76XX_OP_CH_SEL_RBG, false, false,
+	  ADV76XX_OP_MODE_SEL_SDR_422_2X | ADV76XX_OP_FORMAT_SEL_8BIT },
+	{ MEDIA_BUS_FMT_VYUY8_1X16, ADV76XX_OP_CH_SEL_RBG, false, true,
+	  ADV76XX_OP_MODE_SEL_SDR_422_2X | ADV76XX_OP_FORMAT_SEL_8BIT },
+	{ MEDIA_BUS_FMT_YUYV8_1X16, ADV76XX_OP_CH_SEL_RGB, false, false,
+	  ADV76XX_OP_MODE_SEL_SDR_422_2X | ADV76XX_OP_FORMAT_SEL_8BIT },
+	{ MEDIA_BUS_FMT_YVYU8_1X16, ADV76XX_OP_CH_SEL_RGB, false, true,
+	  ADV76XX_OP_MODE_SEL_SDR_422_2X | ADV76XX_OP_FORMAT_SEL_8BIT },
+	{ MEDIA_BUS_FMT_UYVY12_1X24, ADV76XX_OP_CH_SEL_RBG, false, false,
+	  ADV76XX_OP_MODE_SEL_SDR_422_2X | ADV76XX_OP_FORMAT_SEL_12BIT },
+	{ MEDIA_BUS_FMT_VYUY12_1X24, ADV76XX_OP_CH_SEL_RBG, false, true,
+	  ADV76XX_OP_MODE_SEL_SDR_422_2X | ADV76XX_OP_FORMAT_SEL_12BIT },
+	{ MEDIA_BUS_FMT_YUYV12_1X24, ADV76XX_OP_CH_SEL_RGB, false, false,
+	  ADV76XX_OP_MODE_SEL_SDR_422_2X | ADV76XX_OP_FORMAT_SEL_12BIT },
+	{ MEDIA_BUS_FMT_YVYU12_1X24, ADV76XX_OP_CH_SEL_RGB, false, true,
+	  ADV76XX_OP_MODE_SEL_SDR_422_2X | ADV76XX_OP_FORMAT_SEL_12BIT },
+};
+
+static const struct adv76xx_format_info adv7612_formats[] = {
+	{ MEDIA_BUS_FMT_RGB888_1X24, ADV76XX_OP_CH_SEL_RGB, true, false,
+	  ADV76XX_OP_MODE_SEL_SDR_444 | ADV76XX_OP_FORMAT_SEL_8BIT },
+	{ MEDIA_BUS_FMT_YUYV8_2X8, ADV76XX_OP_CH_SEL_RGB, false, false,
+	  ADV76XX_OP_MODE_SEL_SDR_422 | ADV76XX_OP_FORMAT_SEL_8BIT },
+	{ MEDIA_BUS_FMT_YVYU8_2X8, ADV76XX_OP_CH_SEL_RGB, false, true,
+	  ADV76XX_OP_MODE_SEL_SDR_422 | ADV76XX_OP_FORMAT_SEL_8BIT },
+	{ MEDIA_BUS_FMT_UYVY8_1X16, ADV76XX_OP_CH_SEL_RBG, false, false,
+	  ADV76XX_OP_MODE_SEL_SDR_422_2X | ADV76XX_OP_FORMAT_SEL_8BIT },
+	{ MEDIA_BUS_FMT_VYUY8_1X16, ADV76XX_OP_CH_SEL_RBG, false, true,
+	  ADV76XX_OP_MODE_SEL_SDR_422_2X | ADV76XX_OP_FORMAT_SEL_8BIT },
+	{ MEDIA_BUS_FMT_YUYV8_1X16, ADV76XX_OP_CH_SEL_RGB, false, false,
+	  ADV76XX_OP_MODE_SEL_SDR_422_2X | ADV76XX_OP_FORMAT_SEL_8BIT },
+	{ MEDIA_BUS_FMT_YVYU8_1X16, ADV76XX_OP_CH_SEL_RGB, false, true,
+	  ADV76XX_OP_MODE_SEL_SDR_422_2X | ADV76XX_OP_FORMAT_SEL_8BIT },
+};
+
+static const struct adv76xx_format_info *
+adv76xx_format_info(struct adv76xx_state *state, u32 code)
+{
+	unsigned int i;
+
+	for (i = 0; i < state->info->nformats; ++i) {
+		if (state->info->formats[i].code == code)
+			return &state->info->formats[i];
+	}
+
+	return NULL;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static inline bool is_analog_input(struct v4l2_subdev *sd)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	return state->selected_input == ADV7604_PAD_VGA_RGB ||
+	       state->selected_input == ADV7604_PAD_VGA_COMP;
+}
+
+static inline bool is_digital_input(struct v4l2_subdev *sd)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	return state->selected_input == ADV76XX_PAD_HDMI_PORT_A ||
+	       state->selected_input == ADV7604_PAD_HDMI_PORT_B ||
+	       state->selected_input == ADV7604_PAD_HDMI_PORT_C ||
+	       state->selected_input == ADV7604_PAD_HDMI_PORT_D;
+}
+
+static const struct v4l2_dv_timings_cap adv7604_timings_cap_analog = {
+	.type = V4L2_DV_BT_656_1120,
+	/* keep this initialization for compatibility with GCC < 4.4.6 */
+	.reserved = { 0 },
+	V4L2_INIT_BT_TIMINGS(640, 1920, 350, 1200, 25000000, 170000000,
+		V4L2_DV_BT_STD_CEA861 | V4L2_DV_BT_STD_DMT |
+			V4L2_DV_BT_STD_GTF | V4L2_DV_BT_STD_CVT,
+		V4L2_DV_BT_CAP_PROGRESSIVE | V4L2_DV_BT_CAP_REDUCED_BLANKING |
+			V4L2_DV_BT_CAP_CUSTOM)
+};
+
+static const struct v4l2_dv_timings_cap adv76xx_timings_cap_digital = {
+	.type = V4L2_DV_BT_656_1120,
+	/* keep this initialization for compatibility with GCC < 4.4.6 */
+	.reserved = { 0 },
+	V4L2_INIT_BT_TIMINGS(640, 1920, 350, 1200, 25000000, 225000000,
+		V4L2_DV_BT_STD_CEA861 | V4L2_DV_BT_STD_DMT |
+			V4L2_DV_BT_STD_GTF | V4L2_DV_BT_STD_CVT,
+		V4L2_DV_BT_CAP_PROGRESSIVE | V4L2_DV_BT_CAP_REDUCED_BLANKING |
+			V4L2_DV_BT_CAP_CUSTOM)
+};
+
+/*
+ * Return the DV timings capabilities for the requested sink pad. As a special
+ * case, pad value -1 returns the capabilities for the currently selected input.
+ */
+static const struct v4l2_dv_timings_cap *
+adv76xx_get_dv_timings_cap(struct v4l2_subdev *sd, int pad)
+{
+	if (pad == -1) {
+		struct adv76xx_state *state = to_state(sd);
+
+		pad = state->selected_input;
+	}
+
+	switch (pad) {
+	case ADV76XX_PAD_HDMI_PORT_A:
+	case ADV7604_PAD_HDMI_PORT_B:
+	case ADV7604_PAD_HDMI_PORT_C:
+	case ADV7604_PAD_HDMI_PORT_D:
+		return &adv76xx_timings_cap_digital;
+
+	case ADV7604_PAD_VGA_RGB:
+	case ADV7604_PAD_VGA_COMP:
+	default:
+		return &adv7604_timings_cap_analog;
+	}
+}
+
+
+/* ----------------------------------------------------------------------- */
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static void adv76xx_inv_register(struct v4l2_subdev *sd)
+{
+	v4l2_info(sd, "0x000-0x0ff: IO Map\n");
+	v4l2_info(sd, "0x100-0x1ff: AVLink Map\n");
+	v4l2_info(sd, "0x200-0x2ff: CEC Map\n");
+	v4l2_info(sd, "0x300-0x3ff: InfoFrame Map\n");
+	v4l2_info(sd, "0x400-0x4ff: ESDP Map\n");
+	v4l2_info(sd, "0x500-0x5ff: DPP Map\n");
+	v4l2_info(sd, "0x600-0x6ff: AFE Map\n");
+	v4l2_info(sd, "0x700-0x7ff: Repeater Map\n");
+	v4l2_info(sd, "0x800-0x8ff: EDID Map\n");
+	v4l2_info(sd, "0x900-0x9ff: HDMI Map\n");
+	v4l2_info(sd, "0xa00-0xaff: Test Map\n");
+	v4l2_info(sd, "0xb00-0xbff: CP Map\n");
+	v4l2_info(sd, "0xc00-0xcff: VDP Map\n");
+}
+
+static int adv76xx_g_register(struct v4l2_subdev *sd,
+					struct v4l2_dbg_register *reg)
+{
+	int ret;
+
+	ret = adv76xx_read_reg(sd, reg->reg);
+	if (ret < 0) {
+		v4l2_info(sd, "Register %03llx not supported\n", reg->reg);
+		adv76xx_inv_register(sd);
+		return ret;
+	}
+
+	reg->size = 1;
+	reg->val = ret;
+
+	return 0;
+}
+
+static int adv76xx_s_register(struct v4l2_subdev *sd,
+					const struct v4l2_dbg_register *reg)
+{
+	int ret;
+
+	ret = adv76xx_write_reg(sd, reg->reg, reg->val);
+	if (ret < 0) {
+		v4l2_info(sd, "Register %03llx not supported\n", reg->reg);
+		adv76xx_inv_register(sd);
+		return ret;
+	}
+
+	return 0;
+}
+#endif
+
+static unsigned int adv7604_read_cable_det(struct v4l2_subdev *sd)
+{
+	u8 value = io_read(sd, 0x6f);
+
+	return ((value & 0x10) >> 4)
+	     | ((value & 0x08) >> 2)
+	     | ((value & 0x04) << 0)
+	     | ((value & 0x02) << 2);
+}
+
+static unsigned int adv7611_read_cable_det(struct v4l2_subdev *sd)
+{
+	u8 value = io_read(sd, 0x6f);
+
+	return value & 1;
+}
+
+static unsigned int adv7612_read_cable_det(struct v4l2_subdev *sd)
+{
+	/*  Reads CABLE_DET_A_RAW. For input B support, need to
+	 *  account for bit 7 [MSB] of 0x6a (ie. CABLE_DET_B_RAW)
+	 */
+	u8 value = io_read(sd, 0x6f);
+
+	return value & 1;
+}
+
+static int adv76xx_s_detect_tx_5v_ctrl(struct v4l2_subdev *sd)
+{
+	struct adv76xx_state *state = to_state(sd);
+	const struct adv76xx_chip_info *info = state->info;
+	u16 cable_det = info->read_cable_det(sd);
+
+	return v4l2_ctrl_s_ctrl(state->detect_tx_5v_ctrl, cable_det);
+}
+
+static int find_and_set_predefined_video_timings(struct v4l2_subdev *sd,
+		u8 prim_mode,
+		const struct adv76xx_video_standards *predef_vid_timings,
+		const struct v4l2_dv_timings *timings)
+{
+	int i;
+
+	for (i = 0; predef_vid_timings[i].timings.bt.width; i++) {
+		if (!v4l2_match_dv_timings(timings, &predef_vid_timings[i].timings,
+				is_digital_input(sd) ? 250000 : 1000000, false))
+			continue;
+		io_write(sd, 0x00, predef_vid_timings[i].vid_std); /* video std */
+		io_write(sd, 0x01, (predef_vid_timings[i].v_freq << 4) +
+				prim_mode); /* v_freq and prim mode */
+		return 0;
+	}
+
+	return -1;
+}
+
+static int configure_predefined_video_timings(struct v4l2_subdev *sd,
+		struct v4l2_dv_timings *timings)
+{
+	struct adv76xx_state *state = to_state(sd);
+	int err;
+
+	v4l2_dbg(1, debug, sd, "%s", __func__);
+
+	if (adv76xx_has_afe(state)) {
+		/* reset to default values */
+		io_write(sd, 0x16, 0x43);
+		io_write(sd, 0x17, 0x5a);
+	}
+	/* disable embedded syncs for auto graphics mode */
+	cp_write_clr_set(sd, 0x81, 0x10, 0x00);
+	cp_write(sd, 0x8f, 0x00);
+	cp_write(sd, 0x90, 0x00);
+	cp_write(sd, 0xa2, 0x00);
+	cp_write(sd, 0xa3, 0x00);
+	cp_write(sd, 0xa4, 0x00);
+	cp_write(sd, 0xa5, 0x00);
+	cp_write(sd, 0xa6, 0x00);
+	cp_write(sd, 0xa7, 0x00);
+	cp_write(sd, 0xab, 0x00);
+	cp_write(sd, 0xac, 0x00);
+
+	if (is_analog_input(sd)) {
+		err = find_and_set_predefined_video_timings(sd,
+				0x01, adv7604_prim_mode_comp, timings);
+		if (err)
+			err = find_and_set_predefined_video_timings(sd,
+					0x02, adv7604_prim_mode_gr, timings);
+	} else if (is_digital_input(sd)) {
+		err = find_and_set_predefined_video_timings(sd,
+				0x05, adv76xx_prim_mode_hdmi_comp, timings);
+		if (err)
+			err = find_and_set_predefined_video_timings(sd,
+					0x06, adv76xx_prim_mode_hdmi_gr, timings);
+	} else {
+		v4l2_dbg(2, debug, sd, "%s: Unknown port %d selected\n",
+				__func__, state->selected_input);
+		err = -1;
+	}
+
+
+	return err;
+}
+
+static void configure_custom_video_timings(struct v4l2_subdev *sd,
+		const struct v4l2_bt_timings *bt)
+{
+	struct adv76xx_state *state = to_state(sd);
+	u32 width = htotal(bt);
+	u32 height = vtotal(bt);
+	u16 cp_start_sav = bt->hsync + bt->hbackporch - 4;
+	u16 cp_start_eav = width - bt->hfrontporch;
+	u16 cp_start_vbi = height - bt->vfrontporch;
+	u16 cp_end_vbi = bt->vsync + bt->vbackporch;
+	u16 ch1_fr_ll = (((u32)bt->pixelclock / 100) > 0) ?
+		((width * (ADV76XX_FSC / 100)) / ((u32)bt->pixelclock / 100)) : 0;
+	const u8 pll[2] = {
+		0xc0 | ((width >> 8) & 0x1f),
+		width & 0xff
+	};
+
+	v4l2_dbg(2, debug, sd, "%s\n", __func__);
+
+	if (is_analog_input(sd)) {
+		/* auto graphics */
+		io_write(sd, 0x00, 0x07); /* video std */
+		io_write(sd, 0x01, 0x02); /* prim mode */
+		/* enable embedded syncs for auto graphics mode */
+		cp_write_clr_set(sd, 0x81, 0x10, 0x10);
+
+		/* Should only be set in auto-graphics mode [REF_02, p. 91-92] */
+		/* setup PLL_DIV_MAN_EN and PLL_DIV_RATIO */
+		/* IO-map reg. 0x16 and 0x17 should be written in sequence */
+		if (regmap_raw_write(state->regmap[ADV76XX_PAGE_IO],
+					0x16, pll, 2))
+			v4l2_err(sd, "writing to reg 0x16 and 0x17 failed\n");
+
+		/* active video - horizontal timing */
+		cp_write(sd, 0xa2, (cp_start_sav >> 4) & 0xff);
+		cp_write(sd, 0xa3, ((cp_start_sav & 0x0f) << 4) |
+				   ((cp_start_eav >> 8) & 0x0f));
+		cp_write(sd, 0xa4, cp_start_eav & 0xff);
+
+		/* active video - vertical timing */
+		cp_write(sd, 0xa5, (cp_start_vbi >> 4) & 0xff);
+		cp_write(sd, 0xa6, ((cp_start_vbi & 0xf) << 4) |
+				   ((cp_end_vbi >> 8) & 0xf));
+		cp_write(sd, 0xa7, cp_end_vbi & 0xff);
+	} else if (is_digital_input(sd)) {
+		/* set default prim_mode/vid_std for HDMI
+		   according to [REF_03, c. 4.2] */
+		io_write(sd, 0x00, 0x02); /* video std */
+		io_write(sd, 0x01, 0x06); /* prim mode */
+	} else {
+		v4l2_dbg(2, debug, sd, "%s: Unknown port %d selected\n",
+				__func__, state->selected_input);
+	}
+
+	cp_write(sd, 0x8f, (ch1_fr_ll >> 8) & 0x7);
+	cp_write(sd, 0x90, ch1_fr_ll & 0xff);
+	cp_write(sd, 0xab, (height >> 4) & 0xff);
+	cp_write(sd, 0xac, (height & 0x0f) << 4);
+}
+
+static void adv76xx_set_offset(struct v4l2_subdev *sd, bool auto_offset, u16 offset_a, u16 offset_b, u16 offset_c)
+{
+	struct adv76xx_state *state = to_state(sd);
+	u8 offset_buf[4];
+
+	if (auto_offset) {
+		offset_a = 0x3ff;
+		offset_b = 0x3ff;
+		offset_c = 0x3ff;
+	}
+
+	v4l2_dbg(2, debug, sd, "%s: %s offset: a = 0x%x, b = 0x%x, c = 0x%x\n",
+			__func__, auto_offset ? "Auto" : "Manual",
+			offset_a, offset_b, offset_c);
+
+	offset_buf[0] = (cp_read(sd, 0x77) & 0xc0) | ((offset_a & 0x3f0) >> 4);
+	offset_buf[1] = ((offset_a & 0x00f) << 4) | ((offset_b & 0x3c0) >> 6);
+	offset_buf[2] = ((offset_b & 0x03f) << 2) | ((offset_c & 0x300) >> 8);
+	offset_buf[3] = offset_c & 0x0ff;
+
+	/* Registers must be written in this order with no i2c access in between */
+	if (regmap_raw_write(state->regmap[ADV76XX_PAGE_CP],
+			0x77, offset_buf, 4))
+		v4l2_err(sd, "%s: i2c error writing to CP reg 0x77, 0x78, 0x79, 0x7a\n", __func__);
+}
+
+static void adv76xx_set_gain(struct v4l2_subdev *sd, bool auto_gain, u16 gain_a, u16 gain_b, u16 gain_c)
+{
+	struct adv76xx_state *state = to_state(sd);
+	u8 gain_buf[4];
+	u8 gain_man = 1;
+	u8 agc_mode_man = 1;
+
+	if (auto_gain) {
+		gain_man = 0;
+		agc_mode_man = 0;
+		gain_a = 0x100;
+		gain_b = 0x100;
+		gain_c = 0x100;
+	}
+
+	v4l2_dbg(2, debug, sd, "%s: %s gain: a = 0x%x, b = 0x%x, c = 0x%x\n",
+			__func__, auto_gain ? "Auto" : "Manual",
+			gain_a, gain_b, gain_c);
+
+	gain_buf[0] = ((gain_man << 7) | (agc_mode_man << 6) | ((gain_a & 0x3f0) >> 4));
+	gain_buf[1] = (((gain_a & 0x00f) << 4) | ((gain_b & 0x3c0) >> 6));
+	gain_buf[2] = (((gain_b & 0x03f) << 2) | ((gain_c & 0x300) >> 8));
+	gain_buf[3] = ((gain_c & 0x0ff));
+
+	/* Registers must be written in this order with no i2c access in between */
+	if (regmap_raw_write(state->regmap[ADV76XX_PAGE_CP],
+			     0x73, gain_buf, 4))
+		v4l2_err(sd, "%s: i2c error writing to CP reg 0x73, 0x74, 0x75, 0x76\n", __func__);
+}
+
+static void set_rgb_quantization_range(struct v4l2_subdev *sd)
+{
+	struct adv76xx_state *state = to_state(sd);
+	bool rgb_output = io_read(sd, 0x02) & 0x02;
+	bool hdmi_signal = hdmi_read(sd, 0x05) & 0x80;
+	u8 y = HDMI_COLORSPACE_RGB;
+
+	if (hdmi_signal && (io_read(sd, 0x60) & 1))
+		y = infoframe_read(sd, 0x01) >> 5;
+
+	v4l2_dbg(2, debug, sd, "%s: RGB quantization range: %d, RGB out: %d, HDMI: %d\n",
+			__func__, state->rgb_quantization_range,
+			rgb_output, hdmi_signal);
+
+	adv76xx_set_gain(sd, true, 0x0, 0x0, 0x0);
+	adv76xx_set_offset(sd, true, 0x0, 0x0, 0x0);
+	io_write_clr_set(sd, 0x02, 0x04, rgb_output ? 0 : 4);
+
+	switch (state->rgb_quantization_range) {
+	case V4L2_DV_RGB_RANGE_AUTO:
+		if (state->selected_input == ADV7604_PAD_VGA_RGB) {
+			/* Receiving analog RGB signal
+			 * Set RGB full range (0-255) */
+			io_write_clr_set(sd, 0x02, 0xf0, 0x10);
+			break;
+		}
+
+		if (state->selected_input == ADV7604_PAD_VGA_COMP) {
+			/* Receiving analog YPbPr signal
+			 * Set automode */
+			io_write_clr_set(sd, 0x02, 0xf0, 0xf0);
+			break;
+		}
+
+		if (hdmi_signal) {
+			/* Receiving HDMI signal
+			 * Set automode */
+			io_write_clr_set(sd, 0x02, 0xf0, 0xf0);
+			break;
+		}
+
+		/* Receiving DVI-D signal
+		 * ADV7604 selects RGB limited range regardless of
+		 * input format (CE/IT) in automatic mode */
+		if (state->timings.bt.flags & V4L2_DV_FL_IS_CE_VIDEO) {
+			/* RGB limited range (16-235) */
+			io_write_clr_set(sd, 0x02, 0xf0, 0x00);
+		} else {
+			/* RGB full range (0-255) */
+			io_write_clr_set(sd, 0x02, 0xf0, 0x10);
+
+			if (is_digital_input(sd) && rgb_output) {
+				adv76xx_set_offset(sd, false, 0x40, 0x40, 0x40);
+			} else {
+				adv76xx_set_gain(sd, false, 0xe0, 0xe0, 0xe0);
+				adv76xx_set_offset(sd, false, 0x70, 0x70, 0x70);
+			}
+		}
+		break;
+	case V4L2_DV_RGB_RANGE_LIMITED:
+		if (state->selected_input == ADV7604_PAD_VGA_COMP) {
+			/* YCrCb limited range (16-235) */
+			io_write_clr_set(sd, 0x02, 0xf0, 0x20);
+			break;
+		}
+
+		if (y != HDMI_COLORSPACE_RGB)
+			break;
+
+		/* RGB limited range (16-235) */
+		io_write_clr_set(sd, 0x02, 0xf0, 0x00);
+
+		break;
+	case V4L2_DV_RGB_RANGE_FULL:
+		if (state->selected_input == ADV7604_PAD_VGA_COMP) {
+			/* YCrCb full range (0-255) */
+			io_write_clr_set(sd, 0x02, 0xf0, 0x60);
+			break;
+		}
+
+		if (y != HDMI_COLORSPACE_RGB)
+			break;
+
+		/* RGB full range (0-255) */
+		io_write_clr_set(sd, 0x02, 0xf0, 0x10);
+
+		if (is_analog_input(sd) || hdmi_signal)
+			break;
+
+		/* Adjust gain/offset for DVI-D signals only */
+		if (rgb_output) {
+			adv76xx_set_offset(sd, false, 0x40, 0x40, 0x40);
+		} else {
+			adv76xx_set_gain(sd, false, 0xe0, 0xe0, 0xe0);
+			adv76xx_set_offset(sd, false, 0x70, 0x70, 0x70);
+		}
+		break;
+	}
+}
+
+static int adv76xx_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd =
+		&container_of(ctrl->handler, struct adv76xx_state, hdl)->sd;
+
+	struct adv76xx_state *state = to_state(sd);
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		cp_write(sd, 0x3c, ctrl->val);
+		return 0;
+	case V4L2_CID_CONTRAST:
+		cp_write(sd, 0x3a, ctrl->val);
+		return 0;
+	case V4L2_CID_SATURATION:
+		cp_write(sd, 0x3b, ctrl->val);
+		return 0;
+	case V4L2_CID_HUE:
+		cp_write(sd, 0x3d, ctrl->val);
+		return 0;
+	case  V4L2_CID_DV_RX_RGB_RANGE:
+		state->rgb_quantization_range = ctrl->val;
+		set_rgb_quantization_range(sd);
+		return 0;
+	case V4L2_CID_ADV_RX_ANALOG_SAMPLING_PHASE:
+		if (!adv76xx_has_afe(state))
+			return -EINVAL;
+		/* Set the analog sampling phase. This is needed to find the
+		   best sampling phase for analog video: an application or
+		   driver has to try a number of phases and analyze the picture
+		   quality before settling on the best performing phase. */
+		afe_write(sd, 0xc8, ctrl->val);
+		return 0;
+	case V4L2_CID_ADV_RX_FREE_RUN_COLOR_MANUAL:
+		/* Use the default blue color for free running mode,
+		   or supply your own. */
+		cp_write_clr_set(sd, 0xbf, 0x04, ctrl->val << 2);
+		return 0;
+	case V4L2_CID_ADV_RX_FREE_RUN_COLOR:
+		cp_write(sd, 0xc0, (ctrl->val & 0xff0000) >> 16);
+		cp_write(sd, 0xc1, (ctrl->val & 0x00ff00) >> 8);
+		cp_write(sd, 0xc2, (u8)(ctrl->val & 0x0000ff));
+		return 0;
+	}
+	return -EINVAL;
+}
+
+static int adv76xx_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd =
+		&container_of(ctrl->handler, struct adv76xx_state, hdl)->sd;
+
+	if (ctrl->id == V4L2_CID_DV_RX_IT_CONTENT_TYPE) {
+		ctrl->val = V4L2_DV_IT_CONTENT_TYPE_NO_ITC;
+		if ((io_read(sd, 0x60) & 1) && (infoframe_read(sd, 0x03) & 0x80))
+			ctrl->val = (infoframe_read(sd, 0x05) >> 4) & 3;
+		return 0;
+	}
+	return -EINVAL;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static inline bool no_power(struct v4l2_subdev *sd)
+{
+	/* Entire chip or CP powered off */
+	return io_read(sd, 0x0c) & 0x24;
+}
+
+static inline bool no_signal_tmds(struct v4l2_subdev *sd)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	return !(io_read(sd, 0x6a) & (0x10 >> state->selected_input));
+}
+
+static inline bool no_lock_tmds(struct v4l2_subdev *sd)
+{
+	struct adv76xx_state *state = to_state(sd);
+	const struct adv76xx_chip_info *info = state->info;
+
+	return (io_read(sd, 0x6a) & info->tdms_lock_mask) != info->tdms_lock_mask;
+}
+
+static inline bool is_hdmi(struct v4l2_subdev *sd)
+{
+	return hdmi_read(sd, 0x05) & 0x80;
+}
+
+static inline bool no_lock_sspd(struct v4l2_subdev *sd)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	/*
+	 * Chips without a AFE don't expose registers for the SSPD, so just assume
+	 * that we have a lock.
+	 */
+	if (adv76xx_has_afe(state))
+		return false;
+
+	/* TODO channel 2 */
+	return ((cp_read(sd, 0xb5) & 0xd0) != 0xd0);
+}
+
+static inline bool no_lock_stdi(struct v4l2_subdev *sd)
+{
+	/* TODO channel 2 */
+	return !(cp_read(sd, 0xb1) & 0x80);
+}
+
+static inline bool no_signal(struct v4l2_subdev *sd)
+{
+	bool ret;
+
+	ret = no_power(sd);
+
+	ret |= no_lock_stdi(sd);
+	ret |= no_lock_sspd(sd);
+
+	if (is_digital_input(sd)) {
+		ret |= no_lock_tmds(sd);
+		ret |= no_signal_tmds(sd);
+	}
+
+	return ret;
+}
+
+static inline bool no_lock_cp(struct v4l2_subdev *sd)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	if (!adv76xx_has_afe(state))
+		return false;
+
+	/* CP has detected a non standard number of lines on the incoming
+	   video compared to what it is configured to receive by s_dv_timings */
+	return io_read(sd, 0x12) & 0x01;
+}
+
+static inline bool in_free_run(struct v4l2_subdev *sd)
+{
+	return cp_read(sd, 0xff) & 0x10;
+}
+
+static int adv76xx_g_input_status(struct v4l2_subdev *sd, u32 *status)
+{
+	*status = 0;
+	*status |= no_power(sd) ? V4L2_IN_ST_NO_POWER : 0;
+	*status |= no_signal(sd) ? V4L2_IN_ST_NO_SIGNAL : 0;
+	if (!in_free_run(sd) && no_lock_cp(sd))
+		*status |= is_digital_input(sd) ?
+			   V4L2_IN_ST_NO_SYNC : V4L2_IN_ST_NO_H_LOCK;
+
+	v4l2_dbg(1, debug, sd, "%s: status = 0x%x\n", __func__, *status);
+
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+struct stdi_readback {
+	u16 bl, lcf, lcvs;
+	u8 hs_pol, vs_pol;
+	bool interlaced;
+};
+
+static int stdi2dv_timings(struct v4l2_subdev *sd,
+		struct stdi_readback *stdi,
+		struct v4l2_dv_timings *timings)
+{
+	struct adv76xx_state *state = to_state(sd);
+	u32 hfreq = (ADV76XX_FSC * 8) / stdi->bl;
+	u32 pix_clk;
+	int i;
+
+	for (i = 0; v4l2_dv_timings_presets[i].bt.width; i++) {
+		const struct v4l2_bt_timings *bt = &v4l2_dv_timings_presets[i].bt;
+
+		if (!v4l2_valid_dv_timings(&v4l2_dv_timings_presets[i],
+					   adv76xx_get_dv_timings_cap(sd, -1),
+					   adv76xx_check_dv_timings, NULL))
+			continue;
+		if (vtotal(bt) != stdi->lcf + 1)
+			continue;
+		if (bt->vsync != stdi->lcvs)
+			continue;
+
+		pix_clk = hfreq * htotal(bt);
+
+		if ((pix_clk < bt->pixelclock + 1000000) &&
+		    (pix_clk > bt->pixelclock - 1000000)) {
+			*timings = v4l2_dv_timings_presets[i];
+			return 0;
+		}
+	}
+
+	if (v4l2_detect_cvt(stdi->lcf + 1, hfreq, stdi->lcvs, 0,
+			(stdi->hs_pol == '+' ? V4L2_DV_HSYNC_POS_POL : 0) |
+			(stdi->vs_pol == '+' ? V4L2_DV_VSYNC_POS_POL : 0),
+			false, timings))
+		return 0;
+	if (v4l2_detect_gtf(stdi->lcf + 1, hfreq, stdi->lcvs,
+			(stdi->hs_pol == '+' ? V4L2_DV_HSYNC_POS_POL : 0) |
+			(stdi->vs_pol == '+' ? V4L2_DV_VSYNC_POS_POL : 0),
+			false, state->aspect_ratio, timings))
+		return 0;
+
+	v4l2_dbg(2, debug, sd,
+		"%s: No format candidate found for lcvs = %d, lcf=%d, bl = %d, %chsync, %cvsync\n",
+		__func__, stdi->lcvs, stdi->lcf, stdi->bl,
+		stdi->hs_pol, stdi->vs_pol);
+	return -1;
+}
+
+
+static int read_stdi(struct v4l2_subdev *sd, struct stdi_readback *stdi)
+{
+	struct adv76xx_state *state = to_state(sd);
+	const struct adv76xx_chip_info *info = state->info;
+	u8 polarity;
+
+	if (no_lock_stdi(sd) || no_lock_sspd(sd)) {
+		v4l2_dbg(2, debug, sd, "%s: STDI and/or SSPD not locked\n", __func__);
+		return -1;
+	}
+
+	/* read STDI */
+	stdi->bl = cp_read16(sd, 0xb1, 0x3fff);
+	stdi->lcf = cp_read16(sd, info->lcf_reg, 0x7ff);
+	stdi->lcvs = cp_read(sd, 0xb3) >> 3;
+	stdi->interlaced = io_read(sd, 0x12) & 0x10;
+
+	if (adv76xx_has_afe(state)) {
+		/* read SSPD */
+		polarity = cp_read(sd, 0xb5);
+		if ((polarity & 0x03) == 0x01) {
+			stdi->hs_pol = polarity & 0x10
+				     ? (polarity & 0x08 ? '+' : '-') : 'x';
+			stdi->vs_pol = polarity & 0x40
+				     ? (polarity & 0x20 ? '+' : '-') : 'x';
+		} else {
+			stdi->hs_pol = 'x';
+			stdi->vs_pol = 'x';
+		}
+	} else {
+		polarity = hdmi_read(sd, 0x05);
+		stdi->hs_pol = polarity & 0x20 ? '+' : '-';
+		stdi->vs_pol = polarity & 0x10 ? '+' : '-';
+	}
+
+	if (no_lock_stdi(sd) || no_lock_sspd(sd)) {
+		v4l2_dbg(2, debug, sd,
+			"%s: signal lost during readout of STDI/SSPD\n", __func__);
+		return -1;
+	}
+
+	if (stdi->lcf < 239 || stdi->bl < 8 || stdi->bl == 0x3fff) {
+		v4l2_dbg(2, debug, sd, "%s: invalid signal\n", __func__);
+		memset(stdi, 0, sizeof(struct stdi_readback));
+		return -1;
+	}
+
+	v4l2_dbg(2, debug, sd,
+		"%s: lcf (frame height - 1) = %d, bl = %d, lcvs (vsync) = %d, %chsync, %cvsync, %s\n",
+		__func__, stdi->lcf, stdi->bl, stdi->lcvs,
+		stdi->hs_pol, stdi->vs_pol,
+		stdi->interlaced ? "interlaced" : "progressive");
+
+	return 0;
+}
+
+static int adv76xx_enum_dv_timings(struct v4l2_subdev *sd,
+			struct v4l2_enum_dv_timings *timings)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	if (timings->pad >= state->source_pad)
+		return -EINVAL;
+
+	return v4l2_enum_dv_timings_cap(timings,
+		adv76xx_get_dv_timings_cap(sd, timings->pad),
+		adv76xx_check_dv_timings, NULL);
+}
+
+static int adv76xx_dv_timings_cap(struct v4l2_subdev *sd,
+			struct v4l2_dv_timings_cap *cap)
+{
+	struct adv76xx_state *state = to_state(sd);
+	unsigned int pad = cap->pad;
+
+	if (cap->pad >= state->source_pad)
+		return -EINVAL;
+
+	*cap = *adv76xx_get_dv_timings_cap(sd, pad);
+	cap->pad = pad;
+
+	return 0;
+}
+
+/* Fill the optional fields .standards and .flags in struct v4l2_dv_timings
+   if the format is listed in adv76xx_timings[] */
+static void adv76xx_fill_optional_dv_timings_fields(struct v4l2_subdev *sd,
+		struct v4l2_dv_timings *timings)
+{
+	v4l2_find_dv_timings_cap(timings, adv76xx_get_dv_timings_cap(sd, -1),
+				 is_digital_input(sd) ? 250000 : 1000000,
+				 adv76xx_check_dv_timings, NULL);
+}
+
+static unsigned int adv7604_read_hdmi_pixelclock(struct v4l2_subdev *sd)
+{
+	int a, b;
+
+	a = hdmi_read(sd, 0x06);
+	b = hdmi_read(sd, 0x3b);
+	if (a < 0 || b < 0)
+		return 0;
+
+	return a * 1000000 + ((b & 0x30) >> 4) * 250000;
+}
+
+static unsigned int adv7611_read_hdmi_pixelclock(struct v4l2_subdev *sd)
+{
+	int a, b;
+
+	a = hdmi_read(sd, 0x51);
+	b = hdmi_read(sd, 0x52);
+	if (a < 0 || b < 0)
+		return 0;
+
+	return ((a << 1) | (b >> 7)) * 1000000 + (b & 0x7f) * 1000000 / 128;
+}
+
+static unsigned int adv76xx_read_hdmi_pixelclock(struct v4l2_subdev *sd)
+{
+	struct adv76xx_state *state = to_state(sd);
+	const struct adv76xx_chip_info *info = state->info;
+	unsigned int freq, bits_per_channel, pixelrepetition;
+
+	freq = info->read_hdmi_pixelclock(sd);
+	if (is_hdmi(sd)) {
+		/* adjust for deep color mode and pixel repetition */
+		bits_per_channel = ((hdmi_read(sd, 0x0b) & 0x60) >> 4) + 8;
+		pixelrepetition = (hdmi_read(sd, 0x05) & 0x0f) + 1;
+
+		freq = freq * 8 / bits_per_channel / pixelrepetition;
+	}
+
+	return freq;
+}
+
+static int adv76xx_query_dv_timings(struct v4l2_subdev *sd,
+			struct v4l2_dv_timings *timings)
+{
+	struct adv76xx_state *state = to_state(sd);
+	const struct adv76xx_chip_info *info = state->info;
+	struct v4l2_bt_timings *bt = &timings->bt;
+	struct stdi_readback stdi;
+
+	if (!timings)
+		return -EINVAL;
+
+	memset(timings, 0, sizeof(struct v4l2_dv_timings));
+
+	if (no_signal(sd)) {
+		state->restart_stdi_once = true;
+		v4l2_dbg(1, debug, sd, "%s: no valid signal\n", __func__);
+		return -ENOLINK;
+	}
+
+	/* read STDI */
+	if (read_stdi(sd, &stdi)) {
+		v4l2_dbg(1, debug, sd, "%s: STDI/SSPD not locked\n", __func__);
+		return -ENOLINK;
+	}
+	bt->interlaced = stdi.interlaced ?
+		V4L2_DV_INTERLACED : V4L2_DV_PROGRESSIVE;
+
+	if (is_digital_input(sd)) {
+		bool hdmi_signal = hdmi_read(sd, 0x05) & 0x80;
+		u8 vic = 0;
+		u32 w, h;
+
+		w = hdmi_read16(sd, 0x07, info->linewidth_mask);
+		h = hdmi_read16(sd, 0x09, info->field0_height_mask);
+
+		if (hdmi_signal && (io_read(sd, 0x60) & 1))
+			vic = infoframe_read(sd, 0x04);
+
+		if (vic && v4l2_find_dv_timings_cea861_vic(timings, vic) &&
+		    bt->width == w && bt->height == h)
+			goto found;
+
+		timings->type = V4L2_DV_BT_656_1120;
+
+		bt->width = w;
+		bt->height = h;
+		bt->pixelclock = adv76xx_read_hdmi_pixelclock(sd);
+		bt->hfrontporch = hdmi_read16(sd, 0x20, info->hfrontporch_mask);
+		bt->hsync = hdmi_read16(sd, 0x22, info->hsync_mask);
+		bt->hbackporch = hdmi_read16(sd, 0x24, info->hbackporch_mask);
+		bt->vfrontporch = hdmi_read16(sd, 0x2a,
+			info->field0_vfrontporch_mask) / 2;
+		bt->vsync = hdmi_read16(sd, 0x2e, info->field0_vsync_mask) / 2;
+		bt->vbackporch = hdmi_read16(sd, 0x32,
+			info->field0_vbackporch_mask) / 2;
+		bt->polarities = ((hdmi_read(sd, 0x05) & 0x10) ? V4L2_DV_VSYNC_POS_POL : 0) |
+			((hdmi_read(sd, 0x05) & 0x20) ? V4L2_DV_HSYNC_POS_POL : 0);
+		if (bt->interlaced == V4L2_DV_INTERLACED) {
+			bt->height += hdmi_read16(sd, 0x0b,
+				info->field1_height_mask);
+			bt->il_vfrontporch = hdmi_read16(sd, 0x2c,
+				info->field1_vfrontporch_mask) / 2;
+			bt->il_vsync = hdmi_read16(sd, 0x30,
+				info->field1_vsync_mask) / 2;
+			bt->il_vbackporch = hdmi_read16(sd, 0x34,
+				info->field1_vbackporch_mask) / 2;
+		}
+		adv76xx_fill_optional_dv_timings_fields(sd, timings);
+	} else {
+		/* find format
+		 * Since LCVS values are inaccurate [REF_03, p. 275-276],
+		 * stdi2dv_timings() is called with lcvs +-1 if the first attempt fails.
+		 */
+		if (!stdi2dv_timings(sd, &stdi, timings))
+			goto found;
+		stdi.lcvs += 1;
+		v4l2_dbg(1, debug, sd, "%s: lcvs + 1 = %d\n", __func__, stdi.lcvs);
+		if (!stdi2dv_timings(sd, &stdi, timings))
+			goto found;
+		stdi.lcvs -= 2;
+		v4l2_dbg(1, debug, sd, "%s: lcvs - 1 = %d\n", __func__, stdi.lcvs);
+		if (stdi2dv_timings(sd, &stdi, timings)) {
+			/*
+			 * The STDI block may measure wrong values, especially
+			 * for lcvs and lcf. If the driver can not find any
+			 * valid timing, the STDI block is restarted to measure
+			 * the video timings again. The function will return an
+			 * error, but the restart of STDI will generate a new
+			 * STDI interrupt and the format detection process will
+			 * restart.
+			 */
+			if (state->restart_stdi_once) {
+				v4l2_dbg(1, debug, sd, "%s: restart STDI\n", __func__);
+				/* TODO restart STDI for Sync Channel 2 */
+				/* enter one-shot mode */
+				cp_write_clr_set(sd, 0x86, 0x06, 0x00);
+				/* trigger STDI restart */
+				cp_write_clr_set(sd, 0x86, 0x06, 0x04);
+				/* reset to continuous mode */
+				cp_write_clr_set(sd, 0x86, 0x06, 0x02);
+				state->restart_stdi_once = false;
+				return -ENOLINK;
+			}
+			v4l2_dbg(1, debug, sd, "%s: format not supported\n", __func__);
+			return -ERANGE;
+		}
+		state->restart_stdi_once = true;
+	}
+found:
+
+	if (no_signal(sd)) {
+		v4l2_dbg(1, debug, sd, "%s: signal lost during readout\n", __func__);
+		memset(timings, 0, sizeof(struct v4l2_dv_timings));
+		return -ENOLINK;
+	}
+
+	if ((is_analog_input(sd) && bt->pixelclock > 170000000) ||
+			(is_digital_input(sd) && bt->pixelclock > 225000000)) {
+		v4l2_dbg(1, debug, sd, "%s: pixelclock out of range %d\n",
+				__func__, (u32)bt->pixelclock);
+		return -ERANGE;
+	}
+
+	if (debug > 1)
+		v4l2_print_dv_timings(sd->name, "adv76xx_query_dv_timings: ",
+				      timings, true);
+
+	return 0;
+}
+
+static int adv76xx_s_dv_timings(struct v4l2_subdev *sd,
+		struct v4l2_dv_timings *timings)
+{
+	struct adv76xx_state *state = to_state(sd);
+	struct v4l2_bt_timings *bt;
+	int err;
+
+	if (!timings)
+		return -EINVAL;
+
+	if (v4l2_match_dv_timings(&state->timings, timings, 0, false)) {
+		v4l2_dbg(1, debug, sd, "%s: no change\n", __func__);
+		return 0;
+	}
+
+	bt = &timings->bt;
+
+	if (!v4l2_valid_dv_timings(timings, adv76xx_get_dv_timings_cap(sd, -1),
+				   adv76xx_check_dv_timings, NULL))
+		return -ERANGE;
+
+	adv76xx_fill_optional_dv_timings_fields(sd, timings);
+
+	state->timings = *timings;
+
+	cp_write_clr_set(sd, 0x91, 0x40, bt->interlaced ? 0x40 : 0x00);
+
+	/* Use prim_mode and vid_std when available */
+	err = configure_predefined_video_timings(sd, timings);
+	if (err) {
+		/* custom settings when the video format
+		 does not have prim_mode/vid_std */
+		configure_custom_video_timings(sd, bt);
+	}
+
+	set_rgb_quantization_range(sd);
+
+	if (debug > 1)
+		v4l2_print_dv_timings(sd->name, "adv76xx_s_dv_timings: ",
+				      timings, true);
+	return 0;
+}
+
+static int adv76xx_g_dv_timings(struct v4l2_subdev *sd,
+		struct v4l2_dv_timings *timings)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	*timings = state->timings;
+	return 0;
+}
+
+static void adv7604_set_termination(struct v4l2_subdev *sd, bool enable)
+{
+	hdmi_write(sd, 0x01, enable ? 0x00 : 0x78);
+}
+
+static void adv7611_set_termination(struct v4l2_subdev *sd, bool enable)
+{
+	hdmi_write(sd, 0x83, enable ? 0xfe : 0xff);
+}
+
+static void enable_input(struct v4l2_subdev *sd)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	if (is_analog_input(sd)) {
+		io_write(sd, 0x15, 0xb0);   /* Disable Tristate of Pins (no audio) */
+	} else if (is_digital_input(sd)) {
+		hdmi_write_clr_set(sd, 0x00, 0x03, state->selected_input);
+		state->info->set_termination(sd, true);
+		io_write(sd, 0x15, 0xa0);   /* Disable Tristate of Pins */
+		hdmi_write_clr_set(sd, 0x1a, 0x10, 0x00); /* Unmute audio */
+	} else {
+		v4l2_dbg(2, debug, sd, "%s: Unknown port %d selected\n",
+				__func__, state->selected_input);
+	}
+}
+
+static void disable_input(struct v4l2_subdev *sd)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	hdmi_write_clr_set(sd, 0x1a, 0x10, 0x10); /* Mute audio */
+	msleep(16); /* 512 samples with >= 32 kHz sample rate [REF_03, c. 7.16.10] */
+	io_write(sd, 0x15, 0xbe);   /* Tristate all outputs from video core */
+	state->info->set_termination(sd, false);
+}
+
+static void select_input(struct v4l2_subdev *sd)
+{
+	struct adv76xx_state *state = to_state(sd);
+	const struct adv76xx_chip_info *info = state->info;
+
+	if (is_analog_input(sd)) {
+		adv76xx_write_reg_seq(sd, info->recommended_settings[0]);
+
+		afe_write(sd, 0x00, 0x08); /* power up ADC */
+		afe_write(sd, 0x01, 0x06); /* power up Analog Front End */
+		afe_write(sd, 0xc8, 0x00); /* phase control */
+	} else if (is_digital_input(sd)) {
+		hdmi_write(sd, 0x00, state->selected_input & 0x03);
+
+		adv76xx_write_reg_seq(sd, info->recommended_settings[1]);
+
+		if (adv76xx_has_afe(state)) {
+			afe_write(sd, 0x00, 0xff); /* power down ADC */
+			afe_write(sd, 0x01, 0xfe); /* power down Analog Front End */
+			afe_write(sd, 0xc8, 0x40); /* phase control */
+		}
+
+		cp_write(sd, 0x3e, 0x00); /* CP core pre-gain control */
+		cp_write(sd, 0xc3, 0x39); /* CP coast control. Graphics mode */
+		cp_write(sd, 0x40, 0x80); /* CP core pre-gain control. Graphics mode */
+	} else {
+		v4l2_dbg(2, debug, sd, "%s: Unknown port %d selected\n",
+				__func__, state->selected_input);
+	}
+
+	/* Enable video adjustment (contrast, saturation, brightness and hue) */
+	cp_write_clr_set(sd, 0x3e, 0x80, 0x80);
+}
+
+static int adv76xx_s_routing(struct v4l2_subdev *sd,
+		u32 input, u32 output, u32 config)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	v4l2_dbg(2, debug, sd, "%s: input %d, selected input %d",
+			__func__, input, state->selected_input);
+
+	if (input == state->selected_input)
+		return 0;
+
+	if (input > state->info->max_port)
+		return -EINVAL;
+
+	state->selected_input = input;
+
+	disable_input(sd);
+	select_input(sd);
+	enable_input(sd);
+
+	v4l2_subdev_notify_event(sd, &adv76xx_ev_fmt);
+
+	return 0;
+}
+
+static int adv76xx_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	if (code->index >= state->info->nformats)
+		return -EINVAL;
+
+	code->code = state->info->formats[code->index].code;
+
+	return 0;
+}
+
+static void adv76xx_fill_format(struct adv76xx_state *state,
+				struct v4l2_mbus_framefmt *format)
+{
+	memset(format, 0, sizeof(*format));
+
+	format->width = state->timings.bt.width;
+	format->height = state->timings.bt.height;
+	format->field = V4L2_FIELD_NONE;
+	format->colorspace = V4L2_COLORSPACE_SRGB;
+
+	if (state->timings.bt.flags & V4L2_DV_FL_IS_CE_VIDEO)
+		format->colorspace = (state->timings.bt.height <= 576) ?
+			V4L2_COLORSPACE_SMPTE170M : V4L2_COLORSPACE_REC709;
+}
+
+/*
+ * Compute the op_ch_sel value required to obtain on the bus the component order
+ * corresponding to the selected format taking into account bus reordering
+ * applied by the board at the output of the device.
+ *
+ * The following table gives the op_ch_value from the format component order
+ * (expressed as op_ch_sel value in column) and the bus reordering (expressed as
+ * adv76xx_bus_order value in row).
+ *
+ *           |	GBR(0)	GRB(1)	BGR(2)	RGB(3)	BRG(4)	RBG(5)
+ * ----------+-------------------------------------------------
+ * RGB (NOP) |	GBR	GRB	BGR	RGB	BRG	RBG
+ * GRB (1-2) |	BGR	RGB	GBR	GRB	RBG	BRG
+ * RBG (2-3) |	GRB	GBR	BRG	RBG	BGR	RGB
+ * BGR (1-3) |	RBG	BRG	RGB	BGR	GRB	GBR
+ * BRG (ROR) |	BRG	RBG	GRB	GBR	RGB	BGR
+ * GBR (ROL) |	RGB	BGR	RBG	BRG	GBR	GRB
+ */
+static unsigned int adv76xx_op_ch_sel(struct adv76xx_state *state)
+{
+#define _SEL(a,b,c,d,e,f)	{ \
+	ADV76XX_OP_CH_SEL_##a, ADV76XX_OP_CH_SEL_##b, ADV76XX_OP_CH_SEL_##c, \
+	ADV76XX_OP_CH_SEL_##d, ADV76XX_OP_CH_SEL_##e, ADV76XX_OP_CH_SEL_##f }
+#define _BUS(x)			[ADV7604_BUS_ORDER_##x]
+
+	static const unsigned int op_ch_sel[6][6] = {
+		_BUS(RGB) /* NOP */ = _SEL(GBR, GRB, BGR, RGB, BRG, RBG),
+		_BUS(GRB) /* 1-2 */ = _SEL(BGR, RGB, GBR, GRB, RBG, BRG),
+		_BUS(RBG) /* 2-3 */ = _SEL(GRB, GBR, BRG, RBG, BGR, RGB),
+		_BUS(BGR) /* 1-3 */ = _SEL(RBG, BRG, RGB, BGR, GRB, GBR),
+		_BUS(BRG) /* ROR */ = _SEL(BRG, RBG, GRB, GBR, RGB, BGR),
+		_BUS(GBR) /* ROL */ = _SEL(RGB, BGR, RBG, BRG, GBR, GRB),
+	};
+
+	return op_ch_sel[state->pdata.bus_order][state->format->op_ch_sel >> 5];
+}
+
+static void adv76xx_setup_format(struct adv76xx_state *state)
+{
+	struct v4l2_subdev *sd = &state->sd;
+
+	io_write_clr_set(sd, 0x02, 0x02,
+			state->format->rgb_out ? ADV76XX_RGB_OUT : 0);
+	io_write(sd, 0x03, state->format->op_format_sel |
+		 state->pdata.op_format_mode_sel);
+	io_write_clr_set(sd, 0x04, 0xe0, adv76xx_op_ch_sel(state));
+	io_write_clr_set(sd, 0x05, 0x01,
+			state->format->swap_cb_cr ? ADV76XX_OP_SWAP_CB_CR : 0);
+	set_rgb_quantization_range(sd);
+}
+
+static int adv76xx_get_format(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_format *format)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	if (format->pad != state->source_pad)
+		return -EINVAL;
+
+	adv76xx_fill_format(state, &format->format);
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		struct v4l2_mbus_framefmt *fmt;
+
+		fmt = v4l2_subdev_get_try_format(sd, sd_state, format->pad);
+		format->format.code = fmt->code;
+	} else {
+		format->format.code = state->format->code;
+	}
+
+	return 0;
+}
+
+static int adv76xx_get_selection(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_selection *sel)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
+		return -EINVAL;
+	/* Only CROP, CROP_DEFAULT and CROP_BOUNDS are supported */
+	if (sel->target > V4L2_SEL_TGT_CROP_BOUNDS)
+		return -EINVAL;
+
+	sel->r.left	= 0;
+	sel->r.top	= 0;
+	sel->r.width	= state->timings.bt.width;
+	sel->r.height	= state->timings.bt.height;
+
+	return 0;
+}
+
+static int adv76xx_set_format(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_format *format)
+{
+	struct adv76xx_state *state = to_state(sd);
+	const struct adv76xx_format_info *info;
+
+	if (format->pad != state->source_pad)
+		return -EINVAL;
+
+	info = adv76xx_format_info(state, format->format.code);
+	if (!info)
+		info = adv76xx_format_info(state, MEDIA_BUS_FMT_YUYV8_2X8);
+
+	adv76xx_fill_format(state, &format->format);
+	format->format.code = info->code;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		struct v4l2_mbus_framefmt *fmt;
+
+		fmt = v4l2_subdev_get_try_format(sd, sd_state, format->pad);
+		fmt->code = format->format.code;
+	} else {
+		state->format = info;
+		adv76xx_setup_format(state);
+	}
+
+	return 0;
+}
+
+#if IS_ENABLED(CONFIG_VIDEO_ADV7604_CEC)
+static void adv76xx_cec_tx_raw_status(struct v4l2_subdev *sd, u8 tx_raw_status)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	if ((cec_read(sd, 0x11) & 0x01) == 0) {
+		v4l2_dbg(1, debug, sd, "%s: tx raw: tx disabled\n", __func__);
+		return;
+	}
+
+	if (tx_raw_status & 0x02) {
+		v4l2_dbg(1, debug, sd, "%s: tx raw: arbitration lost\n",
+			 __func__);
+		cec_transmit_done(state->cec_adap, CEC_TX_STATUS_ARB_LOST,
+				  1, 0, 0, 0);
+		return;
+	}
+	if (tx_raw_status & 0x04) {
+		u8 status;
+		u8 nack_cnt;
+		u8 low_drive_cnt;
+
+		v4l2_dbg(1, debug, sd, "%s: tx raw: retry failed\n", __func__);
+		/*
+		 * We set this status bit since this hardware performs
+		 * retransmissions.
+		 */
+		status = CEC_TX_STATUS_MAX_RETRIES;
+		nack_cnt = cec_read(sd, 0x14) & 0xf;
+		if (nack_cnt)
+			status |= CEC_TX_STATUS_NACK;
+		low_drive_cnt = cec_read(sd, 0x14) >> 4;
+		if (low_drive_cnt)
+			status |= CEC_TX_STATUS_LOW_DRIVE;
+		cec_transmit_done(state->cec_adap, status,
+				  0, nack_cnt, low_drive_cnt, 0);
+		return;
+	}
+	if (tx_raw_status & 0x01) {
+		v4l2_dbg(1, debug, sd, "%s: tx raw: ready ok\n", __func__);
+		cec_transmit_done(state->cec_adap, CEC_TX_STATUS_OK, 0, 0, 0, 0);
+		return;
+	}
+}
+
+static void adv76xx_cec_isr(struct v4l2_subdev *sd, bool *handled)
+{
+	struct adv76xx_state *state = to_state(sd);
+	const struct adv76xx_chip_info *info = state->info;
+	u8 cec_irq;
+
+	/* cec controller */
+	cec_irq = io_read(sd, info->cec_irq_status) & 0x0f;
+	if (!cec_irq)
+		return;
+
+	v4l2_dbg(1, debug, sd, "%s: cec: irq 0x%x\n", __func__, cec_irq);
+	adv76xx_cec_tx_raw_status(sd, cec_irq);
+	if (cec_irq & 0x08) {
+		struct cec_msg msg;
+
+		msg.len = cec_read(sd, 0x25) & 0x1f;
+		if (msg.len > CEC_MAX_MSG_SIZE)
+			msg.len = CEC_MAX_MSG_SIZE;
+
+		if (msg.len) {
+			u8 i;
+
+			for (i = 0; i < msg.len; i++)
+				msg.msg[i] = cec_read(sd, i + 0x15);
+			cec_write(sd, info->cec_rx_enable,
+				  info->cec_rx_enable_mask); /* re-enable rx */
+			cec_received_msg(state->cec_adap, &msg);
+		}
+	}
+
+	if (info->cec_irq_swap) {
+		/*
+		 * Note: the bit order is swapped between 0x4d and 0x4e
+		 * on adv7604
+		 */
+		cec_irq = ((cec_irq & 0x08) >> 3) | ((cec_irq & 0x04) >> 1) |
+			  ((cec_irq & 0x02) << 1) | ((cec_irq & 0x01) << 3);
+	}
+	io_write(sd, info->cec_irq_status + 1, cec_irq);
+
+	if (handled)
+		*handled = true;
+}
+
+static int adv76xx_cec_adap_enable(struct cec_adapter *adap, bool enable)
+{
+	struct adv76xx_state *state = cec_get_drvdata(adap);
+	const struct adv76xx_chip_info *info = state->info;
+	struct v4l2_subdev *sd = &state->sd;
+
+	if (!state->cec_enabled_adap && enable) {
+		cec_write_clr_set(sd, 0x2a, 0x01, 0x01); /* power up cec */
+		cec_write(sd, 0x2c, 0x01);	/* cec soft reset */
+		cec_write_clr_set(sd, 0x11, 0x01, 0); /* initially disable tx */
+		/* enabled irqs: */
+		/* tx: ready */
+		/* tx: arbitration lost */
+		/* tx: retry timeout */
+		/* rx: ready */
+		io_write_clr_set(sd, info->cec_irq_status + 3, 0x0f, 0x0f);
+		cec_write(sd, info->cec_rx_enable, info->cec_rx_enable_mask);
+	} else if (state->cec_enabled_adap && !enable) {
+		/* disable cec interrupts */
+		io_write_clr_set(sd, info->cec_irq_status + 3, 0x0f, 0x00);
+		/* disable address mask 1-3 */
+		cec_write_clr_set(sd, 0x27, 0x70, 0x00);
+		/* power down cec section */
+		cec_write_clr_set(sd, 0x2a, 0x01, 0x00);
+		state->cec_valid_addrs = 0;
+	}
+	state->cec_enabled_adap = enable;
+	adv76xx_s_detect_tx_5v_ctrl(sd);
+	return 0;
+}
+
+static int adv76xx_cec_adap_log_addr(struct cec_adapter *adap, u8 addr)
+{
+	struct adv76xx_state *state = cec_get_drvdata(adap);
+	struct v4l2_subdev *sd = &state->sd;
+	unsigned int i, free_idx = ADV76XX_MAX_ADDRS;
+
+	if (!state->cec_enabled_adap)
+		return addr == CEC_LOG_ADDR_INVALID ? 0 : -EIO;
+
+	if (addr == CEC_LOG_ADDR_INVALID) {
+		cec_write_clr_set(sd, 0x27, 0x70, 0);
+		state->cec_valid_addrs = 0;
+		return 0;
+	}
+
+	for (i = 0; i < ADV76XX_MAX_ADDRS; i++) {
+		bool is_valid = state->cec_valid_addrs & (1 << i);
+
+		if (free_idx == ADV76XX_MAX_ADDRS && !is_valid)
+			free_idx = i;
+		if (is_valid && state->cec_addr[i] == addr)
+			return 0;
+	}
+	if (i == ADV76XX_MAX_ADDRS) {
+		i = free_idx;
+		if (i == ADV76XX_MAX_ADDRS)
+			return -ENXIO;
+	}
+	state->cec_addr[i] = addr;
+	state->cec_valid_addrs |= 1 << i;
+
+	switch (i) {
+	case 0:
+		/* enable address mask 0 */
+		cec_write_clr_set(sd, 0x27, 0x10, 0x10);
+		/* set address for mask 0 */
+		cec_write_clr_set(sd, 0x28, 0x0f, addr);
+		break;
+	case 1:
+		/* enable address mask 1 */
+		cec_write_clr_set(sd, 0x27, 0x20, 0x20);
+		/* set address for mask 1 */
+		cec_write_clr_set(sd, 0x28, 0xf0, addr << 4);
+		break;
+	case 2:
+		/* enable address mask 2 */
+		cec_write_clr_set(sd, 0x27, 0x40, 0x40);
+		/* set address for mask 1 */
+		cec_write_clr_set(sd, 0x29, 0x0f, addr);
+		break;
+	}
+	return 0;
+}
+
+static int adv76xx_cec_adap_transmit(struct cec_adapter *adap, u8 attempts,
+				     u32 signal_free_time, struct cec_msg *msg)
+{
+	struct adv76xx_state *state = cec_get_drvdata(adap);
+	struct v4l2_subdev *sd = &state->sd;
+	u8 len = msg->len;
+	unsigned int i;
+
+	/*
+	 * The number of retries is the number of attempts - 1, but retry
+	 * at least once. It's not clear if a value of 0 is allowed, so
+	 * let's do at least one retry.
+	 */
+	cec_write_clr_set(sd, 0x12, 0x70, max(1, attempts - 1) << 4);
+
+	if (len > 16) {
+		v4l2_err(sd, "%s: len exceeded 16 (%d)\n", __func__, len);
+		return -EINVAL;
+	}
+
+	/* write data */
+	for (i = 0; i < len; i++)
+		cec_write(sd, i, msg->msg[i]);
+
+	/* set length (data + header) */
+	cec_write(sd, 0x10, len);
+	/* start transmit, enable tx */
+	cec_write(sd, 0x11, 0x01);
+	return 0;
+}
+
+static const struct cec_adap_ops adv76xx_cec_adap_ops = {
+	.adap_enable = adv76xx_cec_adap_enable,
+	.adap_log_addr = adv76xx_cec_adap_log_addr,
+	.adap_transmit = adv76xx_cec_adap_transmit,
+};
+#endif
+
+static int adv76xx_isr(struct v4l2_subdev *sd, u32 status, bool *handled)
+{
+	struct adv76xx_state *state = to_state(sd);
+	const struct adv76xx_chip_info *info = state->info;
+	const u8 irq_reg_0x43 = io_read(sd, 0x43);
+	const u8 irq_reg_0x6b = io_read(sd, 0x6b);
+	const u8 irq_reg_0x70 = io_read(sd, 0x70);
+	u8 fmt_change_digital;
+	u8 fmt_change;
+	u8 tx_5v;
+
+	if (irq_reg_0x43)
+		io_write(sd, 0x44, irq_reg_0x43);
+	if (irq_reg_0x70)
+		io_write(sd, 0x71, irq_reg_0x70);
+	if (irq_reg_0x6b)
+		io_write(sd, 0x6c, irq_reg_0x6b);
+
+	v4l2_dbg(2, debug, sd, "%s: ", __func__);
+
+	/* format change */
+	fmt_change = irq_reg_0x43 & 0x98;
+	fmt_change_digital = is_digital_input(sd)
+			   ? irq_reg_0x6b & info->fmt_change_digital_mask
+			   : 0;
+
+	if (fmt_change || fmt_change_digital) {
+		v4l2_dbg(1, debug, sd,
+			"%s: fmt_change = 0x%x, fmt_change_digital = 0x%x\n",
+			__func__, fmt_change, fmt_change_digital);
+
+		v4l2_subdev_notify_event(sd, &adv76xx_ev_fmt);
+
+		if (handled)
+			*handled = true;
+	}
+	/* HDMI/DVI mode */
+	if (irq_reg_0x6b & 0x01) {
+		v4l2_dbg(1, debug, sd, "%s: irq %s mode\n", __func__,
+			(io_read(sd, 0x6a) & 0x01) ? "HDMI" : "DVI");
+		set_rgb_quantization_range(sd);
+		if (handled)
+			*handled = true;
+	}
+
+#if IS_ENABLED(CONFIG_VIDEO_ADV7604_CEC)
+	/* cec */
+	adv76xx_cec_isr(sd, handled);
+#endif
+
+	/* tx 5v detect */
+	tx_5v = irq_reg_0x70 & info->cable_det_mask;
+	if (tx_5v) {
+		v4l2_dbg(1, debug, sd, "%s: tx_5v: 0x%x\n", __func__, tx_5v);
+		adv76xx_s_detect_tx_5v_ctrl(sd);
+		if (handled)
+			*handled = true;
+	}
+	return 0;
+}
+
+static irqreturn_t adv76xx_irq_handler(int irq, void *dev_id)
+{
+	struct adv76xx_state *state = dev_id;
+	bool handled = false;
+
+	adv76xx_isr(&state->sd, 0, &handled);
+
+	return handled ? IRQ_HANDLED : IRQ_NONE;
+}
+
+static int adv76xx_get_edid(struct v4l2_subdev *sd, struct v4l2_edid *edid)
+{
+	struct adv76xx_state *state = to_state(sd);
+	u8 *data = NULL;
+
+	memset(edid->reserved, 0, sizeof(edid->reserved));
+
+	switch (edid->pad) {
+	case ADV76XX_PAD_HDMI_PORT_A:
+	case ADV7604_PAD_HDMI_PORT_B:
+	case ADV7604_PAD_HDMI_PORT_C:
+	case ADV7604_PAD_HDMI_PORT_D:
+		if (state->edid.present & (1 << edid->pad))
+			data = state->edid.edid;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (edid->start_block == 0 && edid->blocks == 0) {
+		edid->blocks = data ? state->edid.blocks : 0;
+		return 0;
+	}
+
+	if (!data)
+		return -ENODATA;
+
+	if (edid->start_block >= state->edid.blocks)
+		return -EINVAL;
+
+	if (edid->start_block + edid->blocks > state->edid.blocks)
+		edid->blocks = state->edid.blocks - edid->start_block;
+
+	memcpy(edid->edid, data + edid->start_block * 128, edid->blocks * 128);
+
+	return 0;
+}
+
+static int adv76xx_set_edid(struct v4l2_subdev *sd, struct v4l2_edid *edid)
+{
+	struct adv76xx_state *state = to_state(sd);
+	const struct adv76xx_chip_info *info = state->info;
+	unsigned int spa_loc;
+	u16 pa, parent_pa;
+	int err;
+	int i;
+
+	memset(edid->reserved, 0, sizeof(edid->reserved));
+
+	if (edid->pad > ADV7604_PAD_HDMI_PORT_D)
+		return -EINVAL;
+	if (edid->start_block != 0)
+		return -EINVAL;
+	if (edid->blocks == 0) {
+		/* Disable hotplug and I2C access to EDID RAM from DDC port */
+		state->edid.present &= ~(1 << edid->pad);
+		adv76xx_set_hpd(state, state->edid.present);
+		rep_write_clr_set(sd, info->edid_enable_reg, 0x0f, state->edid.present);
+
+		/* Fall back to a 16:9 aspect ratio */
+		state->aspect_ratio.numerator = 16;
+		state->aspect_ratio.denominator = 9;
+
+		if (!state->edid.present) {
+			state->edid.blocks = 0;
+			cec_phys_addr_invalidate(state->cec_adap);
+		}
+
+		v4l2_dbg(2, debug, sd, "%s: clear EDID pad %d, edid.present = 0x%x\n",
+				__func__, edid->pad, state->edid.present);
+		return 0;
+	}
+	if (edid->blocks > ADV76XX_MAX_EDID_BLOCKS) {
+		edid->blocks = ADV76XX_MAX_EDID_BLOCKS;
+		return -E2BIG;
+	}
+
+	pa = v4l2_get_edid_phys_addr(edid->edid, edid->blocks * 128, &spa_loc);
+	err = v4l2_phys_addr_validate(pa, &parent_pa, NULL);
+	if (err)
+		return err;
+
+	if (!spa_loc) {
+		/*
+		 * There is no SPA, so just set spa_loc to 128 and pa to whatever
+		 * data is there.
+		 */
+		spa_loc = 128;
+		pa = (edid->edid[spa_loc] << 8) | edid->edid[spa_loc + 1];
+	}
+
+	v4l2_dbg(2, debug, sd, "%s: write EDID pad %d, edid.present = 0x%x\n",
+			__func__, edid->pad, state->edid.present);
+
+	/* Disable hotplug and I2C access to EDID RAM from DDC port */
+	cancel_delayed_work_sync(&state->delayed_work_enable_hotplug);
+	adv76xx_set_hpd(state, 0);
+	rep_write_clr_set(sd, info->edid_enable_reg, 0x0f, 0x00);
+
+	switch (edid->pad) {
+	case ADV76XX_PAD_HDMI_PORT_A:
+		state->spa_port_a[0] = pa >> 8;
+		state->spa_port_a[1] = pa & 0xff;
+		break;
+	case ADV7604_PAD_HDMI_PORT_B:
+		rep_write(sd, info->edid_spa_port_b_reg, pa >> 8);
+		rep_write(sd, info->edid_spa_port_b_reg + 1, pa & 0xff);
+		break;
+	case ADV7604_PAD_HDMI_PORT_C:
+		rep_write(sd, info->edid_spa_port_b_reg + 2, pa >> 8);
+		rep_write(sd, info->edid_spa_port_b_reg + 3, pa & 0xff);
+		break;
+	case ADV7604_PAD_HDMI_PORT_D:
+		rep_write(sd, info->edid_spa_port_b_reg + 4, pa >> 8);
+		rep_write(sd, info->edid_spa_port_b_reg + 5, pa & 0xff);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (info->edid_spa_loc_reg) {
+		u8 mask = info->edid_spa_loc_msb_mask;
+
+		rep_write(sd, info->edid_spa_loc_reg, spa_loc & 0xff);
+		rep_write_clr_set(sd, info->edid_spa_loc_reg + 1,
+				  mask, (spa_loc & 0x100) ? mask : 0);
+	}
+
+	edid->edid[spa_loc] = state->spa_port_a[0];
+	edid->edid[spa_loc + 1] = state->spa_port_a[1];
+
+	memcpy(state->edid.edid, edid->edid, 128 * edid->blocks);
+	state->edid.blocks = edid->blocks;
+	state->aspect_ratio = v4l2_calc_aspect_ratio(edid->edid[0x15],
+			edid->edid[0x16]);
+	state->edid.present |= 1 << edid->pad;
+
+	rep_write_clr_set(sd, info->edid_segment_reg,
+			  info->edid_segment_mask, 0);
+	err = edid_write_block(sd, 128 * min(edid->blocks, 2U), state->edid.edid);
+	if (err < 0) {
+		v4l2_err(sd, "error %d writing edid pad %d\n", err, edid->pad);
+		return err;
+	}
+	if (edid->blocks > 2) {
+		rep_write_clr_set(sd, info->edid_segment_reg,
+				  info->edid_segment_mask,
+				  info->edid_segment_mask);
+		err = edid_write_block(sd, 128 * (edid->blocks - 2),
+				       state->edid.edid + 256);
+		if (err < 0) {
+			v4l2_err(sd, "error %d writing edid pad %d\n",
+				 err, edid->pad);
+			return err;
+		}
+	}
+
+	/* adv76xx calculates the checksums and enables I2C access to internal
+	   EDID RAM from DDC port. */
+	rep_write_clr_set(sd, info->edid_enable_reg, 0x0f, state->edid.present);
+
+	for (i = 0; i < 1000; i++) {
+		if (rep_read(sd, info->edid_status_reg) & state->edid.present)
+			break;
+		mdelay(1);
+	}
+	if (i == 1000) {
+		v4l2_err(sd, "error enabling edid (0x%x)\n", state->edid.present);
+		return -EIO;
+	}
+	cec_s_phys_addr(state->cec_adap, parent_pa, false);
+
+	/* enable hotplug after 100 ms */
+	schedule_delayed_work(&state->delayed_work_enable_hotplug, HZ / 10);
+	return 0;
+}
+
+/*********** avi info frame CEA-861-E **************/
+
+static const struct adv76xx_cfg_read_infoframe adv76xx_cri[] = {
+	{ "AVI", 0x01, 0xe0, 0x00 },
+	{ "Audio", 0x02, 0xe3, 0x1c },
+	{ "SDP", 0x04, 0xe6, 0x2a },
+	{ "Vendor", 0x10, 0xec, 0x54 }
+};
+
+static int adv76xx_read_infoframe(struct v4l2_subdev *sd, int index,
+				  union hdmi_infoframe *frame)
+{
+	uint8_t buffer[32];
+	u8 len;
+	int i;
+
+	if (!(io_read(sd, 0x60) & adv76xx_cri[index].present_mask)) {
+		v4l2_info(sd, "%s infoframe not received\n",
+			  adv76xx_cri[index].desc);
+		return -ENOENT;
+	}
+
+	for (i = 0; i < 3; i++)
+		buffer[i] = infoframe_read(sd,
+					   adv76xx_cri[index].head_addr + i);
+
+	len = buffer[2] + 1;
+
+	if (len + 3 > sizeof(buffer)) {
+		v4l2_err(sd, "%s: invalid %s infoframe length %d\n", __func__,
+			 adv76xx_cri[index].desc, len);
+		return -ENOENT;
+	}
+
+	for (i = 0; i < len; i++)
+		buffer[i + 3] = infoframe_read(sd,
+				       adv76xx_cri[index].payload_addr + i);
+
+	if (hdmi_infoframe_unpack(frame, buffer, len + 3) < 0) {
+		v4l2_err(sd, "%s: unpack of %s infoframe failed\n", __func__,
+			 adv76xx_cri[index].desc);
+		return -ENOENT;
+	}
+	return 0;
+}
+
+static void adv76xx_log_infoframes(struct v4l2_subdev *sd)
+{
+	int i;
+
+	if (!is_hdmi(sd)) {
+		v4l2_info(sd, "receive DVI-D signal, no infoframes\n");
+		return;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(adv76xx_cri); i++) {
+		union hdmi_infoframe frame;
+		struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+		if (!adv76xx_read_infoframe(sd, i, &frame))
+			hdmi_infoframe_log(KERN_INFO, &client->dev, &frame);
+	}
+}
+
+static int adv76xx_log_status(struct v4l2_subdev *sd)
+{
+	struct adv76xx_state *state = to_state(sd);
+	const struct adv76xx_chip_info *info = state->info;
+	struct v4l2_dv_timings timings;
+	struct stdi_readback stdi;
+	u8 reg_io_0x02 = io_read(sd, 0x02);
+	u8 edid_enabled;
+	u8 cable_det;
+
+	static const char * const csc_coeff_sel_rb[16] = {
+		"bypassed", "YPbPr601 -> RGB", "reserved", "YPbPr709 -> RGB",
+		"reserved", "RGB -> YPbPr601", "reserved", "RGB -> YPbPr709",
+		"reserved", "YPbPr709 -> YPbPr601", "YPbPr601 -> YPbPr709",
+		"reserved", "reserved", "reserved", "reserved", "manual"
+	};
+	static const char * const input_color_space_txt[16] = {
+		"RGB limited range (16-235)", "RGB full range (0-255)",
+		"YCbCr Bt.601 (16-235)", "YCbCr Bt.709 (16-235)",
+		"xvYCC Bt.601", "xvYCC Bt.709",
+		"YCbCr Bt.601 (0-255)", "YCbCr Bt.709 (0-255)",
+		"invalid", "invalid", "invalid", "invalid", "invalid",
+		"invalid", "invalid", "automatic"
+	};
+	static const char * const hdmi_color_space_txt[16] = {
+		"RGB limited range (16-235)", "RGB full range (0-255)",
+		"YCbCr Bt.601 (16-235)", "YCbCr Bt.709 (16-235)",
+		"xvYCC Bt.601", "xvYCC Bt.709",
+		"YCbCr Bt.601 (0-255)", "YCbCr Bt.709 (0-255)",
+		"sYCC", "opYCC 601", "opRGB", "invalid", "invalid",
+		"invalid", "invalid", "invalid"
+	};
+	static const char * const rgb_quantization_range_txt[] = {
+		"Automatic",
+		"RGB limited range (16-235)",
+		"RGB full range (0-255)",
+	};
+	static const char * const deep_color_mode_txt[4] = {
+		"8-bits per channel",
+		"10-bits per channel",
+		"12-bits per channel",
+		"16-bits per channel (not supported)"
+	};
+
+	v4l2_info(sd, "-----Chip status-----\n");
+	v4l2_info(sd, "Chip power: %s\n", no_power(sd) ? "off" : "on");
+	edid_enabled = rep_read(sd, info->edid_status_reg);
+	v4l2_info(sd, "EDID enabled port A: %s, B: %s, C: %s, D: %s\n",
+			((edid_enabled & 0x01) ? "Yes" : "No"),
+			((edid_enabled & 0x02) ? "Yes" : "No"),
+			((edid_enabled & 0x04) ? "Yes" : "No"),
+			((edid_enabled & 0x08) ? "Yes" : "No"));
+	v4l2_info(sd, "CEC: %s\n", state->cec_enabled_adap ?
+			"enabled" : "disabled");
+	if (state->cec_enabled_adap) {
+		int i;
+
+		for (i = 0; i < ADV76XX_MAX_ADDRS; i++) {
+			bool is_valid = state->cec_valid_addrs & (1 << i);
+
+			if (is_valid)
+				v4l2_info(sd, "CEC Logical Address: 0x%x\n",
+					  state->cec_addr[i]);
+		}
+	}
+
+	v4l2_info(sd, "-----Signal status-----\n");
+	cable_det = info->read_cable_det(sd);
+	v4l2_info(sd, "Cable detected (+5V power) port A: %s, B: %s, C: %s, D: %s\n",
+			((cable_det & 0x01) ? "Yes" : "No"),
+			((cable_det & 0x02) ? "Yes" : "No"),
+			((cable_det & 0x04) ? "Yes" : "No"),
+			((cable_det & 0x08) ? "Yes" : "No"));
+	v4l2_info(sd, "TMDS signal detected: %s\n",
+			no_signal_tmds(sd) ? "false" : "true");
+	v4l2_info(sd, "TMDS signal locked: %s\n",
+			no_lock_tmds(sd) ? "false" : "true");
+	v4l2_info(sd, "SSPD locked: %s\n", no_lock_sspd(sd) ? "false" : "true");
+	v4l2_info(sd, "STDI locked: %s\n", no_lock_stdi(sd) ? "false" : "true");
+	v4l2_info(sd, "CP locked: %s\n", no_lock_cp(sd) ? "false" : "true");
+	v4l2_info(sd, "CP free run: %s\n",
+			(in_free_run(sd)) ? "on" : "off");
+	v4l2_info(sd, "Prim-mode = 0x%x, video std = 0x%x, v_freq = 0x%x\n",
+			io_read(sd, 0x01) & 0x0f, io_read(sd, 0x00) & 0x3f,
+			(io_read(sd, 0x01) & 0x70) >> 4);
+
+	v4l2_info(sd, "-----Video Timings-----\n");
+	if (read_stdi(sd, &stdi))
+		v4l2_info(sd, "STDI: not locked\n");
+	else
+		v4l2_info(sd, "STDI: lcf (frame height - 1) = %d, bl = %d, lcvs (vsync) = %d, %s, %chsync, %cvsync\n",
+				stdi.lcf, stdi.bl, stdi.lcvs,
+				stdi.interlaced ? "interlaced" : "progressive",
+				stdi.hs_pol, stdi.vs_pol);
+	if (adv76xx_query_dv_timings(sd, &timings))
+		v4l2_info(sd, "No video detected\n");
+	else
+		v4l2_print_dv_timings(sd->name, "Detected format: ",
+				      &timings, true);
+	v4l2_print_dv_timings(sd->name, "Configured format: ",
+			      &state->timings, true);
+
+	if (no_signal(sd))
+		return 0;
+
+	v4l2_info(sd, "-----Color space-----\n");
+	v4l2_info(sd, "RGB quantization range ctrl: %s\n",
+			rgb_quantization_range_txt[state->rgb_quantization_range]);
+	v4l2_info(sd, "Input color space: %s\n",
+			input_color_space_txt[reg_io_0x02 >> 4]);
+	v4l2_info(sd, "Output color space: %s %s, alt-gamma %s\n",
+			(reg_io_0x02 & 0x02) ? "RGB" : "YCbCr",
+			(((reg_io_0x02 >> 2) & 0x01) ^ (reg_io_0x02 & 0x01)) ?
+				"(16-235)" : "(0-255)",
+			(reg_io_0x02 & 0x08) ? "enabled" : "disabled");
+	v4l2_info(sd, "Color space conversion: %s\n",
+			csc_coeff_sel_rb[cp_read(sd, info->cp_csc) >> 4]);
+
+	if (!is_digital_input(sd))
+		return 0;
+
+	v4l2_info(sd, "-----%s status-----\n", is_hdmi(sd) ? "HDMI" : "DVI-D");
+	v4l2_info(sd, "Digital video port selected: %c\n",
+			(hdmi_read(sd, 0x00) & 0x03) + 'A');
+	v4l2_info(sd, "HDCP encrypted content: %s\n",
+			(hdmi_read(sd, 0x05) & 0x40) ? "true" : "false");
+	v4l2_info(sd, "HDCP keys read: %s%s\n",
+			(hdmi_read(sd, 0x04) & 0x20) ? "yes" : "no",
+			(hdmi_read(sd, 0x04) & 0x10) ? "ERROR" : "");
+	if (is_hdmi(sd)) {
+		bool audio_pll_locked = hdmi_read(sd, 0x04) & 0x01;
+		bool audio_sample_packet_detect = hdmi_read(sd, 0x18) & 0x01;
+		bool audio_mute = io_read(sd, 0x65) & 0x40;
+
+		v4l2_info(sd, "Audio: pll %s, samples %s, %s\n",
+				audio_pll_locked ? "locked" : "not locked",
+				audio_sample_packet_detect ? "detected" : "not detected",
+				audio_mute ? "muted" : "enabled");
+		if (audio_pll_locked && audio_sample_packet_detect) {
+			v4l2_info(sd, "Audio format: %s\n",
+					(hdmi_read(sd, 0x07) & 0x20) ? "multi-channel" : "stereo");
+		}
+		v4l2_info(sd, "Audio CTS: %u\n", (hdmi_read(sd, 0x5b) << 12) +
+				(hdmi_read(sd, 0x5c) << 8) +
+				(hdmi_read(sd, 0x5d) & 0xf0));
+		v4l2_info(sd, "Audio N: %u\n", ((hdmi_read(sd, 0x5d) & 0x0f) << 16) +
+				(hdmi_read(sd, 0x5e) << 8) +
+				hdmi_read(sd, 0x5f));
+		v4l2_info(sd, "AV Mute: %s\n", (hdmi_read(sd, 0x04) & 0x40) ? "on" : "off");
+
+		v4l2_info(sd, "Deep color mode: %s\n", deep_color_mode_txt[(hdmi_read(sd, 0x0b) & 0x60) >> 5]);
+		v4l2_info(sd, "HDMI colorspace: %s\n", hdmi_color_space_txt[hdmi_read(sd, 0x53) & 0xf]);
+
+		adv76xx_log_infoframes(sd);
+	}
+
+	return 0;
+}
+
+static int adv76xx_subscribe_event(struct v4l2_subdev *sd,
+				   struct v4l2_fh *fh,
+				   struct v4l2_event_subscription *sub)
+{
+	switch (sub->type) {
+	case V4L2_EVENT_SOURCE_CHANGE:
+		return v4l2_src_change_event_subdev_subscribe(sd, fh, sub);
+	case V4L2_EVENT_CTRL:
+		return v4l2_ctrl_subdev_subscribe_event(sd, fh, sub);
+	default:
+		return -EINVAL;
+	}
+}
+
+static int adv76xx_registered(struct v4l2_subdev *sd)
+{
+	struct adv76xx_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int err;
+
+	err = cec_register_adapter(state->cec_adap, &client->dev);
+	if (err)
+		cec_delete_adapter(state->cec_adap);
+	return err;
+}
+
+static void adv76xx_unregistered(struct v4l2_subdev *sd)
+{
+	struct adv76xx_state *state = to_state(sd);
+
+	cec_unregister_adapter(state->cec_adap);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_ctrl_ops adv76xx_ctrl_ops = {
+	.s_ctrl = adv76xx_s_ctrl,
+	.g_volatile_ctrl = adv76xx_g_volatile_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops adv76xx_core_ops = {
+	.log_status = adv76xx_log_status,
+	.interrupt_service_routine = adv76xx_isr,
+	.subscribe_event = adv76xx_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = adv76xx_g_register,
+	.s_register = adv76xx_s_register,
+#endif
+};
+
+static const struct v4l2_subdev_video_ops adv76xx_video_ops = {
+	.s_routing = adv76xx_s_routing,
+	.g_input_status = adv76xx_g_input_status,
+	.s_dv_timings = adv76xx_s_dv_timings,
+	.g_dv_timings = adv76xx_g_dv_timings,
+	.query_dv_timings = adv76xx_query_dv_timings,
+};
+
+static const struct v4l2_subdev_pad_ops adv76xx_pad_ops = {
+	.enum_mbus_code = adv76xx_enum_mbus_code,
+	.get_selection = adv76xx_get_selection,
+	.get_fmt = adv76xx_get_format,
+	.set_fmt = adv76xx_set_format,
+	.get_edid = adv76xx_get_edid,
+	.set_edid = adv76xx_set_edid,
+	.dv_timings_cap = adv76xx_dv_timings_cap,
+	.enum_dv_timings = adv76xx_enum_dv_timings,
+};
+
+static const struct v4l2_subdev_ops adv76xx_ops = {
+	.core = &adv76xx_core_ops,
+	.video = &adv76xx_video_ops,
+	.pad = &adv76xx_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops adv76xx_int_ops = {
+	.registered = adv76xx_registered,
+	.unregistered = adv76xx_unregistered,
+};
+
+/* -------------------------- custom ctrls ---------------------------------- */
+
+static const struct v4l2_ctrl_config adv7604_ctrl_analog_sampling_phase = {
+	.ops = &adv76xx_ctrl_ops,
+	.id = V4L2_CID_ADV_RX_ANALOG_SAMPLING_PHASE,
+	.name = "Analog Sampling Phase",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.min = 0,
+	.max = 0x1f,
+	.step = 1,
+	.def = 0,
+};
+
+static const struct v4l2_ctrl_config adv76xx_ctrl_free_run_color_manual = {
+	.ops = &adv76xx_ctrl_ops,
+	.id = V4L2_CID_ADV_RX_FREE_RUN_COLOR_MANUAL,
+	.name = "Free Running Color, Manual",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.min = false,
+	.max = true,
+	.step = 1,
+	.def = false,
+};
+
+static const struct v4l2_ctrl_config adv76xx_ctrl_free_run_color = {
+	.ops = &adv76xx_ctrl_ops,
+	.id = V4L2_CID_ADV_RX_FREE_RUN_COLOR,
+	.name = "Free Running Color",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.min = 0x0,
+	.max = 0xffffff,
+	.step = 0x1,
+	.def = 0x0,
+};
+
+/* ----------------------------------------------------------------------- */
+
+struct adv76xx_register_map {
+	const char *name;
+	u8 default_addr;
+};
+
+static const struct adv76xx_register_map adv76xx_default_addresses[] = {
+	[ADV76XX_PAGE_IO] = { "main", 0x4c },
+	[ADV7604_PAGE_AVLINK] = { "avlink", 0x42 },
+	[ADV76XX_PAGE_CEC] = { "cec", 0x40 },
+	[ADV76XX_PAGE_INFOFRAME] = { "infoframe", 0x3e },
+	[ADV7604_PAGE_ESDP] = { "esdp", 0x38 },
+	[ADV7604_PAGE_DPP] = { "dpp", 0x3c },
+	[ADV76XX_PAGE_AFE] = { "afe", 0x26 },
+	[ADV76XX_PAGE_REP] = { "rep", 0x32 },
+	[ADV76XX_PAGE_EDID] = { "edid", 0x36 },
+	[ADV76XX_PAGE_HDMI] = { "hdmi", 0x34 },
+	[ADV76XX_PAGE_TEST] = { "test", 0x30 },
+	[ADV76XX_PAGE_CP] = { "cp", 0x22 },
+	[ADV7604_PAGE_VDP] = { "vdp", 0x24 },
+};
+
+static int adv76xx_core_init(struct v4l2_subdev *sd)
+{
+	struct adv76xx_state *state = to_state(sd);
+	const struct adv76xx_chip_info *info = state->info;
+	struct adv76xx_platform_data *pdata = &state->pdata;
+
+	hdmi_write(sd, 0x48,
+		(pdata->disable_pwrdnb ? 0x80 : 0) |
+		(pdata->disable_cable_det_rst ? 0x40 : 0));
+
+	disable_input(sd);
+
+	if (pdata->default_input >= 0 &&
+	    pdata->default_input < state->source_pad) {
+		state->selected_input = pdata->default_input;
+		select_input(sd);
+		enable_input(sd);
+	}
+
+	/* power */
+	io_write(sd, 0x0c, 0x42);   /* Power up part and power down VDP */
+	io_write(sd, 0x0b, 0x44);   /* Power down ESDP block */
+	cp_write(sd, 0xcf, 0x01);   /* Power down macrovision */
+
+	/* HPD */
+	if (info->type != ADV7604) {
+		/* Set manual HPD values to 0 */
+		io_write_clr_set(sd, 0x20, 0xc0, 0);
+		/*
+		 * Set HPA_DELAY to 200 ms and set automatic HPD control
+		 * to: internal EDID is active AND a cable is detected
+		 * AND the manual HPD control is set to 1.
+		 */
+		hdmi_write_clr_set(sd, 0x6c, 0xf6, 0x26);
+	}
+
+	/* video format */
+	io_write_clr_set(sd, 0x02, 0x0f, pdata->alt_gamma << 3);
+	io_write_clr_set(sd, 0x05, 0x0e, pdata->blank_data << 3 |
+			pdata->insert_av_codes << 2 |
+			pdata->replicate_av_codes << 1);
+	adv76xx_setup_format(state);
+
+	cp_write(sd, 0x69, 0x30);   /* Enable CP CSC */
+
+	/* VS, HS polarities */
+	io_write(sd, 0x06, 0xa0 | pdata->inv_vs_pol << 2 |
+		 pdata->inv_hs_pol << 1 | pdata->inv_llc_pol);
+
+	/* Adjust drive strength */
+	io_write(sd, 0x14, 0x40 | pdata->dr_str_data << 4 |
+				pdata->dr_str_clk << 2 |
+				pdata->dr_str_sync);
+
+	cp_write(sd, 0xba, (pdata->hdmi_free_run_mode << 1) | 0x01); /* HDMI free run */
+	cp_write(sd, 0xf3, 0xdc); /* Low threshold to enter/exit free run mode */
+	cp_write(sd, 0xf9, 0x23); /*  STDI ch. 1 - LCVS change threshold -
+				      ADI recommended setting [REF_01, c. 2.3.3] */
+	cp_write(sd, 0x45, 0x23); /*  STDI ch. 2 - LCVS change threshold -
+				      ADI recommended setting [REF_01, c. 2.3.3] */
+	cp_write(sd, 0xc9, 0x2d); /* use prim_mode and vid_std as free run resolution
+				     for digital formats */
+
+	/* HDMI audio */
+	hdmi_write_clr_set(sd, 0x15, 0x03, 0x03); /* Mute on FIFO over-/underflow [REF_01, c. 1.2.18] */
+	hdmi_write_clr_set(sd, 0x1a, 0x0e, 0x08); /* Wait 1 s before unmute */
+	hdmi_write_clr_set(sd, 0x68, 0x06, 0x06); /* FIFO reset on over-/underflow [REF_01, c. 1.2.19] */
+
+	/* TODO from platform data */
+	afe_write(sd, 0xb5, 0x01);  /* Setting MCLK to 256Fs */
+
+	if (adv76xx_has_afe(state)) {
+		afe_write(sd, 0x02, pdata->ain_sel); /* Select analog input muxing mode */
+		io_write_clr_set(sd, 0x30, 1 << 4, pdata->output_bus_lsb_to_msb << 4);
+	}
+
+	/* interrupts */
+	io_write(sd, 0x40, 0xc0 | pdata->int1_config); /* Configure INT1 */
+	io_write(sd, 0x46, 0x98); /* Enable SSPD, STDI and CP unlocked interrupts */
+	io_write(sd, 0x6e, info->fmt_change_digital_mask); /* Enable V_LOCKED and DE_REGEN_LCK interrupts */
+	io_write(sd, 0x73, info->cable_det_mask); /* Enable cable detection (+5v) interrupts */
+	info->setup_irqs(sd);
+
+	return v4l2_ctrl_handler_setup(sd->ctrl_handler);
+}
+
+static void adv7604_setup_irqs(struct v4l2_subdev *sd)
+{
+	io_write(sd, 0x41, 0xd7); /* STDI irq for any change, disable INT2 */
+}
+
+static void adv7611_setup_irqs(struct v4l2_subdev *sd)
+{
+	io_write(sd, 0x41, 0xd0); /* STDI irq for any change, disable INT2 */
+}
+
+static void adv7612_setup_irqs(struct v4l2_subdev *sd)
+{
+	io_write(sd, 0x41, 0xd0); /* disable INT2 */
+}
+
+static void adv76xx_unregister_clients(struct adv76xx_state *state)
+{
+	unsigned int i;
+
+	for (i = 1; i < ARRAY_SIZE(state->i2c_clients); ++i)
+		i2c_unregister_device(state->i2c_clients[i]);
+}
+
+static struct i2c_client *adv76xx_dummy_client(struct v4l2_subdev *sd,
+					       unsigned int page)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct adv76xx_state *state = to_state(sd);
+	struct adv76xx_platform_data *pdata = &state->pdata;
+	unsigned int io_reg = 0xf2 + page;
+	struct i2c_client *new_client;
+
+	if (pdata && pdata->i2c_addresses[page])
+		new_client = i2c_new_dummy_device(client->adapter,
+					   pdata->i2c_addresses[page]);
+	else
+		new_client = i2c_new_ancillary_device(client,
+				adv76xx_default_addresses[page].name,
+				adv76xx_default_addresses[page].default_addr);
+
+	if (!IS_ERR(new_client))
+		io_write(sd, io_reg, new_client->addr << 1);
+
+	return new_client;
+}
+
+static const struct adv76xx_reg_seq adv7604_recommended_settings_afe[] = {
+	/* reset ADI recommended settings for HDMI: */
+	/* "ADV7604 Register Settings Recommendations (rev. 2.5, June 2010)" p. 4. */
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x0d), 0x04 }, /* HDMI filter optimization */
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x0d), 0x04 }, /* HDMI filter optimization */
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x3d), 0x00 }, /* DDC bus active pull-up control */
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x3e), 0x74 }, /* TMDS PLL optimization */
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x4e), 0x3b }, /* TMDS PLL optimization */
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x57), 0x74 }, /* TMDS PLL optimization */
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x58), 0x63 }, /* TMDS PLL optimization */
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x8d), 0x18 }, /* equaliser */
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x8e), 0x34 }, /* equaliser */
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x93), 0x88 }, /* equaliser */
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x94), 0x2e }, /* equaliser */
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x96), 0x00 }, /* enable automatic EQ changing */
+
+	/* set ADI recommended settings for digitizer */
+	/* "ADV7604 Register Settings Recommendations (rev. 2.5, June 2010)" p. 17. */
+	{ ADV76XX_REG(ADV76XX_PAGE_AFE, 0x12), 0x7b }, /* ADC noise shaping filter controls */
+	{ ADV76XX_REG(ADV76XX_PAGE_AFE, 0x0c), 0x1f }, /* CP core gain controls */
+	{ ADV76XX_REG(ADV76XX_PAGE_CP, 0x3e), 0x04 }, /* CP core pre-gain control */
+	{ ADV76XX_REG(ADV76XX_PAGE_CP, 0xc3), 0x39 }, /* CP coast control. Graphics mode */
+	{ ADV76XX_REG(ADV76XX_PAGE_CP, 0x40), 0x5c }, /* CP core pre-gain control. Graphics mode */
+
+	{ ADV76XX_REG_SEQ_TERM, 0 },
+};
+
+static const struct adv76xx_reg_seq adv7604_recommended_settings_hdmi[] = {
+	/* set ADI recommended settings for HDMI: */
+	/* "ADV7604 Register Settings Recommendations (rev. 2.5, June 2010)" p. 4. */
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x0d), 0x84 }, /* HDMI filter optimization */
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x3d), 0x10 }, /* DDC bus active pull-up control */
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x3e), 0x39 }, /* TMDS PLL optimization */
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x4e), 0x3b }, /* TMDS PLL optimization */
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x57), 0xb6 }, /* TMDS PLL optimization */
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x58), 0x03 }, /* TMDS PLL optimization */
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x8d), 0x18 }, /* equaliser */
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x8e), 0x34 }, /* equaliser */
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x93), 0x8b }, /* equaliser */
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x94), 0x2d }, /* equaliser */
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x96), 0x01 }, /* enable automatic EQ changing */
+
+	/* reset ADI recommended settings for digitizer */
+	/* "ADV7604 Register Settings Recommendations (rev. 2.5, June 2010)" p. 17. */
+	{ ADV76XX_REG(ADV76XX_PAGE_AFE, 0x12), 0xfb }, /* ADC noise shaping filter controls */
+	{ ADV76XX_REG(ADV76XX_PAGE_AFE, 0x0c), 0x0d }, /* CP core gain controls */
+
+	{ ADV76XX_REG_SEQ_TERM, 0 },
+};
+
+static const struct adv76xx_reg_seq adv7611_recommended_settings_hdmi[] = {
+	/* ADV7611 Register Settings Recommendations Rev 1.5, May 2014 */
+	{ ADV76XX_REG(ADV76XX_PAGE_CP, 0x6c), 0x00 },
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x9b), 0x03 },
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x6f), 0x08 },
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x85), 0x1f },
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x87), 0x70 },
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x57), 0xda },
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x58), 0x01 },
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x03), 0x98 },
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x4c), 0x44 },
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x8d), 0x04 },
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x8e), 0x1e },
+
+	{ ADV76XX_REG_SEQ_TERM, 0 },
+};
+
+static const struct adv76xx_reg_seq adv7612_recommended_settings_hdmi[] = {
+	{ ADV76XX_REG(ADV76XX_PAGE_CP, 0x6c), 0x00 },
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x9b), 0x03 },
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x6f), 0x08 },
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x85), 0x1f },
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x87), 0x70 },
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x57), 0xda },
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x58), 0x01 },
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x03), 0x98 },
+	{ ADV76XX_REG(ADV76XX_PAGE_HDMI, 0x4c), 0x44 },
+	{ ADV76XX_REG_SEQ_TERM, 0 },
+};
+
+static const struct adv76xx_chip_info adv76xx_chip_info[] = {
+	[ADV7604] = {
+		.type = ADV7604,
+		.has_afe = true,
+		.max_port = ADV7604_PAD_VGA_COMP,
+		.num_dv_ports = 4,
+		.edid_enable_reg = 0x77,
+		.edid_status_reg = 0x7d,
+		.edid_segment_reg = 0x77,
+		.edid_segment_mask = 0x10,
+		.edid_spa_loc_reg = 0x76,
+		.edid_spa_loc_msb_mask = 0x40,
+		.edid_spa_port_b_reg = 0x70,
+		.lcf_reg = 0xb3,
+		.tdms_lock_mask = 0xe0,
+		.cable_det_mask = 0x1e,
+		.fmt_change_digital_mask = 0xc1,
+		.cp_csc = 0xfc,
+		.cec_irq_status = 0x4d,
+		.cec_rx_enable = 0x26,
+		.cec_rx_enable_mask = 0x01,
+		.cec_irq_swap = true,
+		.formats = adv7604_formats,
+		.nformats = ARRAY_SIZE(adv7604_formats),
+		.set_termination = adv7604_set_termination,
+		.setup_irqs = adv7604_setup_irqs,
+		.read_hdmi_pixelclock = adv7604_read_hdmi_pixelclock,
+		.read_cable_det = adv7604_read_cable_det,
+		.recommended_settings = {
+		    [0] = adv7604_recommended_settings_afe,
+		    [1] = adv7604_recommended_settings_hdmi,
+		},
+		.num_recommended_settings = {
+		    [0] = ARRAY_SIZE(adv7604_recommended_settings_afe),
+		    [1] = ARRAY_SIZE(adv7604_recommended_settings_hdmi),
+		},
+		.page_mask = BIT(ADV76XX_PAGE_IO) | BIT(ADV7604_PAGE_AVLINK) |
+			BIT(ADV76XX_PAGE_CEC) | BIT(ADV76XX_PAGE_INFOFRAME) |
+			BIT(ADV7604_PAGE_ESDP) | BIT(ADV7604_PAGE_DPP) |
+			BIT(ADV76XX_PAGE_AFE) | BIT(ADV76XX_PAGE_REP) |
+			BIT(ADV76XX_PAGE_EDID) | BIT(ADV76XX_PAGE_HDMI) |
+			BIT(ADV76XX_PAGE_TEST) | BIT(ADV76XX_PAGE_CP) |
+			BIT(ADV7604_PAGE_VDP),
+		.linewidth_mask = 0xfff,
+		.field0_height_mask = 0xfff,
+		.field1_height_mask = 0xfff,
+		.hfrontporch_mask = 0x3ff,
+		.hsync_mask = 0x3ff,
+		.hbackporch_mask = 0x3ff,
+		.field0_vfrontporch_mask = 0x1fff,
+		.field0_vsync_mask = 0x1fff,
+		.field0_vbackporch_mask = 0x1fff,
+		.field1_vfrontporch_mask = 0x1fff,
+		.field1_vsync_mask = 0x1fff,
+		.field1_vbackporch_mask = 0x1fff,
+	},
+	[ADV7611] = {
+		.type = ADV7611,
+		.has_afe = false,
+		.max_port = ADV76XX_PAD_HDMI_PORT_A,
+		.num_dv_ports = 1,
+		.edid_enable_reg = 0x74,
+		.edid_status_reg = 0x76,
+		.edid_segment_reg = 0x7a,
+		.edid_segment_mask = 0x01,
+		.lcf_reg = 0xa3,
+		.tdms_lock_mask = 0x43,
+		.cable_det_mask = 0x01,
+		.fmt_change_digital_mask = 0x03,
+		.cp_csc = 0xf4,
+		.cec_irq_status = 0x93,
+		.cec_rx_enable = 0x2c,
+		.cec_rx_enable_mask = 0x02,
+		.formats = adv7611_formats,
+		.nformats = ARRAY_SIZE(adv7611_formats),
+		.set_termination = adv7611_set_termination,
+		.setup_irqs = adv7611_setup_irqs,
+		.read_hdmi_pixelclock = adv7611_read_hdmi_pixelclock,
+		.read_cable_det = adv7611_read_cable_det,
+		.recommended_settings = {
+		    [1] = adv7611_recommended_settings_hdmi,
+		},
+		.num_recommended_settings = {
+		    [1] = ARRAY_SIZE(adv7611_recommended_settings_hdmi),
+		},
+		.page_mask = BIT(ADV76XX_PAGE_IO) | BIT(ADV76XX_PAGE_CEC) |
+			BIT(ADV76XX_PAGE_INFOFRAME) | BIT(ADV76XX_PAGE_AFE) |
+			BIT(ADV76XX_PAGE_REP) |  BIT(ADV76XX_PAGE_EDID) |
+			BIT(ADV76XX_PAGE_HDMI) | BIT(ADV76XX_PAGE_CP),
+		.linewidth_mask = 0x1fff,
+		.field0_height_mask = 0x1fff,
+		.field1_height_mask = 0x1fff,
+		.hfrontporch_mask = 0x1fff,
+		.hsync_mask = 0x1fff,
+		.hbackporch_mask = 0x1fff,
+		.field0_vfrontporch_mask = 0x3fff,
+		.field0_vsync_mask = 0x3fff,
+		.field0_vbackporch_mask = 0x3fff,
+		.field1_vfrontporch_mask = 0x3fff,
+		.field1_vsync_mask = 0x3fff,
+		.field1_vbackporch_mask = 0x3fff,
+	},
+	[ADV7612] = {
+		.type = ADV7612,
+		.has_afe = false,
+		.max_port = ADV76XX_PAD_HDMI_PORT_A,	/* B not supported */
+		.num_dv_ports = 1,			/* normally 2 */
+		.edid_enable_reg = 0x74,
+		.edid_status_reg = 0x76,
+		.edid_segment_reg = 0x7a,
+		.edid_segment_mask = 0x01,
+		.edid_spa_loc_reg = 0x70,
+		.edid_spa_loc_msb_mask = 0x01,
+		.edid_spa_port_b_reg = 0x52,
+		.lcf_reg = 0xa3,
+		.tdms_lock_mask = 0x43,
+		.cable_det_mask = 0x01,
+		.fmt_change_digital_mask = 0x03,
+		.cp_csc = 0xf4,
+		.cec_irq_status = 0x93,
+		.cec_rx_enable = 0x2c,
+		.cec_rx_enable_mask = 0x02,
+		.formats = adv7612_formats,
+		.nformats = ARRAY_SIZE(adv7612_formats),
+		.set_termination = adv7611_set_termination,
+		.setup_irqs = adv7612_setup_irqs,
+		.read_hdmi_pixelclock = adv7611_read_hdmi_pixelclock,
+		.read_cable_det = adv7612_read_cable_det,
+		.recommended_settings = {
+		    [1] = adv7612_recommended_settings_hdmi,
+		},
+		.num_recommended_settings = {
+		    [1] = ARRAY_SIZE(adv7612_recommended_settings_hdmi),
+		},
+		.page_mask = BIT(ADV76XX_PAGE_IO) | BIT(ADV76XX_PAGE_CEC) |
+			BIT(ADV76XX_PAGE_INFOFRAME) | BIT(ADV76XX_PAGE_AFE) |
+			BIT(ADV76XX_PAGE_REP) |  BIT(ADV76XX_PAGE_EDID) |
+			BIT(ADV76XX_PAGE_HDMI) | BIT(ADV76XX_PAGE_CP),
+		.linewidth_mask = 0x1fff,
+		.field0_height_mask = 0x1fff,
+		.field1_height_mask = 0x1fff,
+		.hfrontporch_mask = 0x1fff,
+		.hsync_mask = 0x1fff,
+		.hbackporch_mask = 0x1fff,
+		.field0_vfrontporch_mask = 0x3fff,
+		.field0_vsync_mask = 0x3fff,
+		.field0_vbackporch_mask = 0x3fff,
+		.field1_vfrontporch_mask = 0x3fff,
+		.field1_vsync_mask = 0x3fff,
+		.field1_vbackporch_mask = 0x3fff,
+	},
+};
+
+static const struct i2c_device_id adv76xx_i2c_id[] = {
+	{ "adv7604", (kernel_ulong_t)&adv76xx_chip_info[ADV7604] },
+	{ "adv7610", (kernel_ulong_t)&adv76xx_chip_info[ADV7611] },
+	{ "adv7611", (kernel_ulong_t)&adv76xx_chip_info[ADV7611] },
+	{ "adv7612", (kernel_ulong_t)&adv76xx_chip_info[ADV7612] },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, adv76xx_i2c_id);
+
+static const struct of_device_id adv76xx_of_id[] __maybe_unused = {
+	{ .compatible = "adi,adv7610", .data = &adv76xx_chip_info[ADV7611] },
+	{ .compatible = "adi,adv7611", .data = &adv76xx_chip_info[ADV7611] },
+	{ .compatible = "adi,adv7612", .data = &adv76xx_chip_info[ADV7612] },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, adv76xx_of_id);
+
+static int adv76xx_parse_dt(struct adv76xx_state *state)
+{
+	struct v4l2_fwnode_endpoint bus_cfg = { .bus_type = 0 };
+	struct device_node *endpoint;
+	struct device_node *np;
+	unsigned int flags;
+	int ret;
+	u32 v;
+
+	np = state->i2c_clients[ADV76XX_PAGE_IO]->dev.of_node;
+
+	/* Parse the endpoint. */
+	endpoint = of_graph_get_next_endpoint(np, NULL);
+	if (!endpoint)
+		return -EINVAL;
+
+	ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(endpoint), &bus_cfg);
+	of_node_put(endpoint);
+	if (ret)
+		return ret;
+
+	if (!of_property_read_u32(np, "default-input", &v))
+		state->pdata.default_input = v;
+	else
+		state->pdata.default_input = -1;
+
+	flags = bus_cfg.bus.parallel.flags;
+
+	if (flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
+		state->pdata.inv_hs_pol = 1;
+
+	if (flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH)
+		state->pdata.inv_vs_pol = 1;
+
+	if (flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
+		state->pdata.inv_llc_pol = 1;
+
+	if (bus_cfg.bus_type == V4L2_MBUS_BT656)
+		state->pdata.insert_av_codes = 1;
+
+	/* Disable the interrupt for now as no DT-based board uses it. */
+	state->pdata.int1_config = ADV76XX_INT1_CONFIG_ACTIVE_HIGH;
+
+	/* Hardcode the remaining platform data fields. */
+	state->pdata.disable_pwrdnb = 0;
+	state->pdata.disable_cable_det_rst = 0;
+	state->pdata.blank_data = 1;
+	state->pdata.op_format_mode_sel = ADV7604_OP_FORMAT_MODE0;
+	state->pdata.bus_order = ADV7604_BUS_ORDER_RGB;
+	state->pdata.dr_str_data = ADV76XX_DR_STR_MEDIUM_HIGH;
+	state->pdata.dr_str_clk = ADV76XX_DR_STR_MEDIUM_HIGH;
+	state->pdata.dr_str_sync = ADV76XX_DR_STR_MEDIUM_HIGH;
+
+	return 0;
+}
+
+static const struct regmap_config adv76xx_regmap_cnf[] = {
+	{
+		.name			= "io",
+		.reg_bits		= 8,
+		.val_bits		= 8,
+
+		.max_register		= 0xff,
+		.cache_type		= REGCACHE_NONE,
+	},
+	{
+		.name			= "avlink",
+		.reg_bits		= 8,
+		.val_bits		= 8,
+
+		.max_register		= 0xff,
+		.cache_type		= REGCACHE_NONE,
+	},
+	{
+		.name			= "cec",
+		.reg_bits		= 8,
+		.val_bits		= 8,
+
+		.max_register		= 0xff,
+		.cache_type		= REGCACHE_NONE,
+	},
+	{
+		.name			= "infoframe",
+		.reg_bits		= 8,
+		.val_bits		= 8,
+
+		.max_register		= 0xff,
+		.cache_type		= REGCACHE_NONE,
+	},
+	{
+		.name			= "esdp",
+		.reg_bits		= 8,
+		.val_bits		= 8,
+
+		.max_register		= 0xff,
+		.cache_type		= REGCACHE_NONE,
+	},
+	{
+		.name			= "epp",
+		.reg_bits		= 8,
+		.val_bits		= 8,
+
+		.max_register		= 0xff,
+		.cache_type		= REGCACHE_NONE,
+	},
+	{
+		.name			= "afe",
+		.reg_bits		= 8,
+		.val_bits		= 8,
+
+		.max_register		= 0xff,
+		.cache_type		= REGCACHE_NONE,
+	},
+	{
+		.name			= "rep",
+		.reg_bits		= 8,
+		.val_bits		= 8,
+
+		.max_register		= 0xff,
+		.cache_type		= REGCACHE_NONE,
+	},
+	{
+		.name			= "edid",
+		.reg_bits		= 8,
+		.val_bits		= 8,
+
+		.max_register		= 0xff,
+		.cache_type		= REGCACHE_NONE,
+	},
+
+	{
+		.name			= "hdmi",
+		.reg_bits		= 8,
+		.val_bits		= 8,
+
+		.max_register		= 0xff,
+		.cache_type		= REGCACHE_NONE,
+	},
+	{
+		.name			= "test",
+		.reg_bits		= 8,
+		.val_bits		= 8,
+
+		.max_register		= 0xff,
+		.cache_type		= REGCACHE_NONE,
+	},
+	{
+		.name			= "cp",
+		.reg_bits		= 8,
+		.val_bits		= 8,
+
+		.max_register		= 0xff,
+		.cache_type		= REGCACHE_NONE,
+	},
+	{
+		.name			= "vdp",
+		.reg_bits		= 8,
+		.val_bits		= 8,
+
+		.max_register		= 0xff,
+		.cache_type		= REGCACHE_NONE,
+	},
+};
+
+static int configure_regmap(struct adv76xx_state *state, int region)
+{
+	int err;
+
+	if (!state->i2c_clients[region])
+		return -ENODEV;
+
+	state->regmap[region] =
+		devm_regmap_init_i2c(state->i2c_clients[region],
+				     &adv76xx_regmap_cnf[region]);
+
+	if (IS_ERR(state->regmap[region])) {
+		err = PTR_ERR(state->regmap[region]);
+		v4l_err(state->i2c_clients[region],
+			"Error initializing regmap %d with error %d\n",
+			region, err);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int configure_regmaps(struct adv76xx_state *state)
+{
+	int i, err;
+
+	for (i = ADV7604_PAGE_AVLINK ; i < ADV76XX_PAGE_MAX; i++) {
+		err = configure_regmap(state, i);
+		if (err && (err != -ENODEV))
+			return err;
+	}
+	return 0;
+}
+
+static void adv76xx_reset(struct adv76xx_state *state)
+{
+	if (state->reset_gpio) {
+		/* ADV76XX can be reset by a low reset pulse of minimum 5 ms. */
+		gpiod_set_value_cansleep(state->reset_gpio, 0);
+		usleep_range(5000, 10000);
+		gpiod_set_value_cansleep(state->reset_gpio, 1);
+		/* It is recommended to wait 5 ms after the low pulse before */
+		/* an I2C write is performed to the ADV76XX. */
+		usleep_range(5000, 10000);
+	}
+}
+
+static int adv76xx_probe(struct i2c_client *client)
+{
+	const struct i2c_device_id *id = i2c_client_get_device_id(client);
+	static const struct v4l2_dv_timings cea640x480 =
+		V4L2_DV_BT_CEA_640X480P59_94;
+	struct adv76xx_state *state;
+	struct v4l2_ctrl_handler *hdl;
+	struct v4l2_ctrl *ctrl;
+	struct v4l2_subdev *sd;
+	unsigned int i;
+	unsigned int val, val2;
+	int err;
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+	v4l_dbg(1, debug, client, "detecting adv76xx client on address 0x%x\n",
+			client->addr << 1);
+
+	state = devm_kzalloc(&client->dev, sizeof(*state), GFP_KERNEL);
+	if (!state)
+		return -ENOMEM;
+
+	state->i2c_clients[ADV76XX_PAGE_IO] = client;
+
+	/* initialize variables */
+	state->restart_stdi_once = true;
+	state->selected_input = ~0;
+
+	if (IS_ENABLED(CONFIG_OF) && client->dev.of_node) {
+		const struct of_device_id *oid;
+
+		oid = of_match_node(adv76xx_of_id, client->dev.of_node);
+		state->info = oid->data;
+
+		err = adv76xx_parse_dt(state);
+		if (err < 0) {
+			v4l_err(client, "DT parsing error\n");
+			return err;
+		}
+	} else if (client->dev.platform_data) {
+		struct adv76xx_platform_data *pdata = client->dev.platform_data;
+
+		state->info = (const struct adv76xx_chip_info *)id->driver_data;
+		state->pdata = *pdata;
+	} else {
+		v4l_err(client, "No platform data!\n");
+		return -ENODEV;
+	}
+
+	/* Request GPIOs. */
+	for (i = 0; i < state->info->num_dv_ports; ++i) {
+		state->hpd_gpio[i] =
+			devm_gpiod_get_index_optional(&client->dev, "hpd", i,
+						      GPIOD_OUT_LOW);
+		if (IS_ERR(state->hpd_gpio[i]))
+			return PTR_ERR(state->hpd_gpio[i]);
+
+		if (state->hpd_gpio[i])
+			v4l_info(client, "Handling HPD %u GPIO\n", i);
+	}
+	state->reset_gpio = devm_gpiod_get_optional(&client->dev, "reset",
+								GPIOD_OUT_HIGH);
+	if (IS_ERR(state->reset_gpio))
+		return PTR_ERR(state->reset_gpio);
+
+	adv76xx_reset(state);
+
+	state->timings = cea640x480;
+	state->format = adv76xx_format_info(state, MEDIA_BUS_FMT_YUYV8_2X8);
+
+	sd = &state->sd;
+	v4l2_i2c_subdev_init(sd, client, &adv76xx_ops);
+	snprintf(sd->name, sizeof(sd->name), "%s %d-%04x",
+		id->name, i2c_adapter_id(client->adapter),
+		client->addr);
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
+	sd->internal_ops = &adv76xx_int_ops;
+
+	/* Configure IO Regmap region */
+	err = configure_regmap(state, ADV76XX_PAGE_IO);
+
+	if (err) {
+		v4l2_err(sd, "Error configuring IO regmap region\n");
+		return -ENODEV;
+	}
+
+	/*
+	 * Verify that the chip is present. On ADV7604 the RD_INFO register only
+	 * identifies the revision, while on ADV7611 it identifies the model as
+	 * well. Use the HDMI slave address on ADV7604 and RD_INFO on ADV7611.
+	 */
+	switch (state->info->type) {
+	case ADV7604:
+		err = regmap_read(state->regmap[ADV76XX_PAGE_IO], 0xfb, &val);
+		if (err) {
+			v4l2_err(sd, "Error %d reading IO Regmap\n", err);
+			return -ENODEV;
+		}
+		if (val != 0x68) {
+			v4l2_err(sd, "not an ADV7604 on address 0x%x\n",
+				 client->addr << 1);
+			return -ENODEV;
+		}
+		break;
+	case ADV7611:
+	case ADV7612:
+		err = regmap_read(state->regmap[ADV76XX_PAGE_IO],
+				0xea,
+				&val);
+		if (err) {
+			v4l2_err(sd, "Error %d reading IO Regmap\n", err);
+			return -ENODEV;
+		}
+		val2 = val << 8;
+		err = regmap_read(state->regmap[ADV76XX_PAGE_IO],
+			    0xeb,
+			    &val);
+		if (err) {
+			v4l2_err(sd, "Error %d reading IO Regmap\n", err);
+			return -ENODEV;
+		}
+		val |= val2;
+		if ((state->info->type == ADV7611 && val != 0x2051) ||
+			(state->info->type == ADV7612 && val != 0x2041)) {
+			v4l2_err(sd, "not an %s on address 0x%x\n",
+				 state->info->type == ADV7611 ? "ADV7610/11" : "ADV7612",
+				 client->addr << 1);
+			return -ENODEV;
+		}
+		break;
+	}
+
+	/* control handlers */
+	hdl = &state->hdl;
+	v4l2_ctrl_handler_init(hdl, adv76xx_has_afe(state) ? 9 : 8);
+
+	v4l2_ctrl_new_std(hdl, &adv76xx_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, -128, 127, 1, 0);
+	v4l2_ctrl_new_std(hdl, &adv76xx_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(hdl, &adv76xx_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(hdl, &adv76xx_ctrl_ops,
+			V4L2_CID_HUE, 0, 255, 1, 0);
+	ctrl = v4l2_ctrl_new_std_menu(hdl, &adv76xx_ctrl_ops,
+			V4L2_CID_DV_RX_IT_CONTENT_TYPE, V4L2_DV_IT_CONTENT_TYPE_NO_ITC,
+			0, V4L2_DV_IT_CONTENT_TYPE_NO_ITC);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_VOLATILE;
+
+	state->detect_tx_5v_ctrl = v4l2_ctrl_new_std(hdl, NULL,
+			V4L2_CID_DV_RX_POWER_PRESENT, 0,
+			(1 << state->info->num_dv_ports) - 1, 0, 0);
+	state->rgb_quantization_range_ctrl =
+		v4l2_ctrl_new_std_menu(hdl, &adv76xx_ctrl_ops,
+			V4L2_CID_DV_RX_RGB_RANGE, V4L2_DV_RGB_RANGE_FULL,
+			0, V4L2_DV_RGB_RANGE_AUTO);
+
+	/* custom controls */
+	if (adv76xx_has_afe(state))
+		state->analog_sampling_phase_ctrl =
+			v4l2_ctrl_new_custom(hdl, &adv7604_ctrl_analog_sampling_phase, NULL);
+	state->free_run_color_manual_ctrl =
+		v4l2_ctrl_new_custom(hdl, &adv76xx_ctrl_free_run_color_manual, NULL);
+	state->free_run_color_ctrl =
+		v4l2_ctrl_new_custom(hdl, &adv76xx_ctrl_free_run_color, NULL);
+
+	sd->ctrl_handler = hdl;
+	if (hdl->error) {
+		err = hdl->error;
+		goto err_hdl;
+	}
+	if (adv76xx_s_detect_tx_5v_ctrl(sd)) {
+		err = -ENODEV;
+		goto err_hdl;
+	}
+
+	for (i = 1; i < ADV76XX_PAGE_MAX; ++i) {
+		struct i2c_client *dummy_client;
+
+		if (!(BIT(i) & state->info->page_mask))
+			continue;
+
+		dummy_client = adv76xx_dummy_client(sd, i);
+		if (IS_ERR(dummy_client)) {
+			err = PTR_ERR(dummy_client);
+			v4l2_err(sd, "failed to create i2c client %u\n", i);
+			goto err_i2c;
+		}
+
+		state->i2c_clients[i] = dummy_client;
+	}
+
+	INIT_DELAYED_WORK(&state->delayed_work_enable_hotplug,
+			adv76xx_delayed_work_enable_hotplug);
+
+	state->source_pad = state->info->num_dv_ports
+			  + (state->info->has_afe ? 2 : 0);
+	for (i = 0; i < state->source_pad; ++i)
+		state->pads[i].flags = MEDIA_PAD_FL_SINK;
+	state->pads[state->source_pad].flags = MEDIA_PAD_FL_SOURCE;
+	sd->entity.function = MEDIA_ENT_F_DV_DECODER;
+
+	err = media_entity_pads_init(&sd->entity, state->source_pad + 1,
+				state->pads);
+	if (err)
+		goto err_work_queues;
+
+	/* Configure regmaps */
+	err = configure_regmaps(state);
+	if (err)
+		goto err_entity;
+
+	err = adv76xx_core_init(sd);
+	if (err)
+		goto err_entity;
+
+	if (client->irq) {
+		err = devm_request_threaded_irq(&client->dev,
+						client->irq,
+						NULL, adv76xx_irq_handler,
+						IRQF_TRIGGER_HIGH | IRQF_ONESHOT,
+						client->name, state);
+		if (err)
+			goto err_entity;
+	}
+
+#if IS_ENABLED(CONFIG_VIDEO_ADV7604_CEC)
+	state->cec_adap = cec_allocate_adapter(&adv76xx_cec_adap_ops,
+		state, dev_name(&client->dev),
+		CEC_CAP_DEFAULTS, ADV76XX_MAX_ADDRS);
+	err = PTR_ERR_OR_ZERO(state->cec_adap);
+	if (err)
+		goto err_entity;
+#endif
+
+	v4l2_info(sd, "%s found @ 0x%x (%s)\n", client->name,
+			client->addr << 1, client->adapter->name);
+
+	err = v4l2_async_register_subdev(sd);
+	if (err)
+		goto err_entity;
+
+	return 0;
+
+err_entity:
+	media_entity_cleanup(&sd->entity);
+err_work_queues:
+	cancel_delayed_work(&state->delayed_work_enable_hotplug);
+err_i2c:
+	adv76xx_unregister_clients(state);
+err_hdl:
+	v4l2_ctrl_handler_free(hdl);
+	return err;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static void adv76xx_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct adv76xx_state *state = to_state(sd);
+
+	/* disable interrupts */
+	io_write(sd, 0x40, 0);
+	io_write(sd, 0x41, 0);
+	io_write(sd, 0x46, 0);
+	io_write(sd, 0x6e, 0);
+	io_write(sd, 0x73, 0);
+
+	cancel_delayed_work_sync(&state->delayed_work_enable_hotplug);
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	adv76xx_unregister_clients(to_state(sd));
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static struct i2c_driver adv76xx_driver = {
+	.driver = {
+		.name = "adv7604",
+		.of_match_table = of_match_ptr(adv76xx_of_id),
+	},
+	.probe = adv76xx_probe,
+	.remove = adv76xx_remove,
+	.id_table = adv76xx_i2c_id,
+};
+
+module_i2c_driver(adv76xx_driver);
diff --git a/drivers/media/i2c/adv7842.c b/drivers/media/i2c/adv7842.c
new file mode 100644
index 000000000..c1664a362
--- /dev/null
+++ b/drivers/media/i2c/adv7842.c
@@ -0,0 +1,3627 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * adv7842 - Analog Devices ADV7842 video decoder driver
+ *
+ * Copyright 2013 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+/*
+ * References (c = chapter, p = page):
+ * REF_01 - Analog devices, ADV7842,
+ *		Register Settings Recommendations, Rev. 1.9, April 2011
+ * REF_02 - Analog devices, Software User Guide, UG-206,
+ *		ADV7842 I2C Register Maps, Rev. 0, November 2010
+ * REF_03 - Analog devices, Hardware User Guide, UG-214,
+ *		ADV7842 Fast Switching 2:1 HDMI 1.4 Receiver with 3D-Comb
+ *		Decoder and Digitizer , Rev. 0, January 2011
+ */
+
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <linux/videodev2.h>
+#include <linux/workqueue.h>
+#include <linux/v4l2-dv-timings.h>
+#include <linux/hdmi.h>
+#include <media/cec.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-dv-timings.h>
+#include <media/i2c/adv7842.h>
+
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "debug level (0-2)");
+
+MODULE_DESCRIPTION("Analog Devices ADV7842 video decoder driver");
+MODULE_AUTHOR("Hans Verkuil <hans.verkuil@cisco.com>");
+MODULE_AUTHOR("Martin Bugge <marbugge@cisco.com>");
+MODULE_LICENSE("GPL");
+
+/* ADV7842 system clock frequency */
+#define ADV7842_fsc (28636360)
+
+#define ADV7842_RGB_OUT					(1 << 1)
+
+#define ADV7842_OP_FORMAT_SEL_8BIT			(0 << 0)
+#define ADV7842_OP_FORMAT_SEL_10BIT			(1 << 0)
+#define ADV7842_OP_FORMAT_SEL_12BIT			(2 << 0)
+
+#define ADV7842_OP_MODE_SEL_SDR_422			(0 << 5)
+#define ADV7842_OP_MODE_SEL_DDR_422			(1 << 5)
+#define ADV7842_OP_MODE_SEL_SDR_444			(2 << 5)
+#define ADV7842_OP_MODE_SEL_DDR_444			(3 << 5)
+#define ADV7842_OP_MODE_SEL_SDR_422_2X			(4 << 5)
+#define ADV7842_OP_MODE_SEL_ADI_CM			(5 << 5)
+
+#define ADV7842_OP_CH_SEL_GBR				(0 << 5)
+#define ADV7842_OP_CH_SEL_GRB				(1 << 5)
+#define ADV7842_OP_CH_SEL_BGR				(2 << 5)
+#define ADV7842_OP_CH_SEL_RGB				(3 << 5)
+#define ADV7842_OP_CH_SEL_BRG				(4 << 5)
+#define ADV7842_OP_CH_SEL_RBG				(5 << 5)
+
+#define ADV7842_OP_SWAP_CB_CR				(1 << 0)
+
+#define ADV7842_MAX_ADDRS (3)
+
+/*
+**********************************************************************
+*
+*  Arrays with configuration parameters for the ADV7842
+*
+**********************************************************************
+*/
+
+struct adv7842_format_info {
+	u32 code;
+	u8 op_ch_sel;
+	bool rgb_out;
+	bool swap_cb_cr;
+	u8 op_format_sel;
+};
+
+struct adv7842_state {
+	struct adv7842_platform_data pdata;
+	struct v4l2_subdev sd;
+	struct media_pad pads[ADV7842_PAD_SOURCE + 1];
+	struct v4l2_ctrl_handler hdl;
+	enum adv7842_mode mode;
+	struct v4l2_dv_timings timings;
+	enum adv7842_vid_std_select vid_std_select;
+
+	const struct adv7842_format_info *format;
+
+	v4l2_std_id norm;
+	struct {
+		u8 edid[512];
+		u32 blocks;
+		u32 present;
+	} hdmi_edid;
+	struct {
+		u8 edid[128];
+		u32 blocks;
+		u32 present;
+	} vga_edid;
+	struct v4l2_fract aspect_ratio;
+	u32 rgb_quantization_range;
+	bool is_cea_format;
+	struct delayed_work delayed_work_enable_hotplug;
+	bool restart_stdi_once;
+	bool hdmi_port_a;
+
+	/* i2c clients */
+	struct i2c_client *i2c_sdp_io;
+	struct i2c_client *i2c_sdp;
+	struct i2c_client *i2c_cp;
+	struct i2c_client *i2c_vdp;
+	struct i2c_client *i2c_afe;
+	struct i2c_client *i2c_hdmi;
+	struct i2c_client *i2c_repeater;
+	struct i2c_client *i2c_edid;
+	struct i2c_client *i2c_infoframe;
+	struct i2c_client *i2c_cec;
+	struct i2c_client *i2c_avlink;
+
+	/* controls */
+	struct v4l2_ctrl *detect_tx_5v_ctrl;
+	struct v4l2_ctrl *analog_sampling_phase_ctrl;
+	struct v4l2_ctrl *free_run_color_ctrl_manual;
+	struct v4l2_ctrl *free_run_color_ctrl;
+	struct v4l2_ctrl *rgb_quantization_range_ctrl;
+
+	struct cec_adapter *cec_adap;
+	u8   cec_addr[ADV7842_MAX_ADDRS];
+	u8   cec_valid_addrs;
+	bool cec_enabled_adap;
+};
+
+/* Unsupported timings. This device cannot support 720p30. */
+static const struct v4l2_dv_timings adv7842_timings_exceptions[] = {
+	V4L2_DV_BT_CEA_1280X720P30,
+	{ }
+};
+
+static bool adv7842_check_dv_timings(const struct v4l2_dv_timings *t, void *hdl)
+{
+	int i;
+
+	for (i = 0; adv7842_timings_exceptions[i].bt.width; i++)
+		if (v4l2_match_dv_timings(t, adv7842_timings_exceptions + i, 0, false))
+			return false;
+	return true;
+}
+
+struct adv7842_video_standards {
+	struct v4l2_dv_timings timings;
+	u8 vid_std;
+	u8 v_freq;
+};
+
+/* sorted by number of lines */
+static const struct adv7842_video_standards adv7842_prim_mode_comp[] = {
+	/* { V4L2_DV_BT_CEA_720X480P59_94, 0x0a, 0x00 }, TODO flickering */
+	{ V4L2_DV_BT_CEA_720X576P50, 0x0b, 0x00 },
+	{ V4L2_DV_BT_CEA_1280X720P50, 0x19, 0x01 },
+	{ V4L2_DV_BT_CEA_1280X720P60, 0x19, 0x00 },
+	{ V4L2_DV_BT_CEA_1920X1080P24, 0x1e, 0x04 },
+	{ V4L2_DV_BT_CEA_1920X1080P25, 0x1e, 0x03 },
+	{ V4L2_DV_BT_CEA_1920X1080P30, 0x1e, 0x02 },
+	{ V4L2_DV_BT_CEA_1920X1080P50, 0x1e, 0x01 },
+	{ V4L2_DV_BT_CEA_1920X1080P60, 0x1e, 0x00 },
+	/* TODO add 1920x1080P60_RB (CVT timing) */
+	{ },
+};
+
+/* sorted by number of lines */
+static const struct adv7842_video_standards adv7842_prim_mode_gr[] = {
+	{ V4L2_DV_BT_DMT_640X480P60, 0x08, 0x00 },
+	{ V4L2_DV_BT_DMT_640X480P72, 0x09, 0x00 },
+	{ V4L2_DV_BT_DMT_640X480P75, 0x0a, 0x00 },
+	{ V4L2_DV_BT_DMT_640X480P85, 0x0b, 0x00 },
+	{ V4L2_DV_BT_DMT_800X600P56, 0x00, 0x00 },
+	{ V4L2_DV_BT_DMT_800X600P60, 0x01, 0x00 },
+	{ V4L2_DV_BT_DMT_800X600P72, 0x02, 0x00 },
+	{ V4L2_DV_BT_DMT_800X600P75, 0x03, 0x00 },
+	{ V4L2_DV_BT_DMT_800X600P85, 0x04, 0x00 },
+	{ V4L2_DV_BT_DMT_1024X768P60, 0x0c, 0x00 },
+	{ V4L2_DV_BT_DMT_1024X768P70, 0x0d, 0x00 },
+	{ V4L2_DV_BT_DMT_1024X768P75, 0x0e, 0x00 },
+	{ V4L2_DV_BT_DMT_1024X768P85, 0x0f, 0x00 },
+	{ V4L2_DV_BT_DMT_1280X1024P60, 0x05, 0x00 },
+	{ V4L2_DV_BT_DMT_1280X1024P75, 0x06, 0x00 },
+	{ V4L2_DV_BT_DMT_1360X768P60, 0x12, 0x00 },
+	{ V4L2_DV_BT_DMT_1366X768P60, 0x13, 0x00 },
+	{ V4L2_DV_BT_DMT_1400X1050P60, 0x14, 0x00 },
+	{ V4L2_DV_BT_DMT_1400X1050P75, 0x15, 0x00 },
+	{ V4L2_DV_BT_DMT_1600X1200P60, 0x16, 0x00 }, /* TODO not tested */
+	/* TODO add 1600X1200P60_RB (not a DMT timing) */
+	{ V4L2_DV_BT_DMT_1680X1050P60, 0x18, 0x00 },
+	{ V4L2_DV_BT_DMT_1920X1200P60_RB, 0x19, 0x00 }, /* TODO not tested */
+	{ },
+};
+
+/* sorted by number of lines */
+static const struct adv7842_video_standards adv7842_prim_mode_hdmi_comp[] = {
+	{ V4L2_DV_BT_CEA_720X480P59_94, 0x0a, 0x00 },
+	{ V4L2_DV_BT_CEA_720X576P50, 0x0b, 0x00 },
+	{ V4L2_DV_BT_CEA_1280X720P50, 0x13, 0x01 },
+	{ V4L2_DV_BT_CEA_1280X720P60, 0x13, 0x00 },
+	{ V4L2_DV_BT_CEA_1920X1080P24, 0x1e, 0x04 },
+	{ V4L2_DV_BT_CEA_1920X1080P25, 0x1e, 0x03 },
+	{ V4L2_DV_BT_CEA_1920X1080P30, 0x1e, 0x02 },
+	{ V4L2_DV_BT_CEA_1920X1080P50, 0x1e, 0x01 },
+	{ V4L2_DV_BT_CEA_1920X1080P60, 0x1e, 0x00 },
+	{ },
+};
+
+/* sorted by number of lines */
+static const struct adv7842_video_standards adv7842_prim_mode_hdmi_gr[] = {
+	{ V4L2_DV_BT_DMT_640X480P60, 0x08, 0x00 },
+	{ V4L2_DV_BT_DMT_640X480P72, 0x09, 0x00 },
+	{ V4L2_DV_BT_DMT_640X480P75, 0x0a, 0x00 },
+	{ V4L2_DV_BT_DMT_640X480P85, 0x0b, 0x00 },
+	{ V4L2_DV_BT_DMT_800X600P56, 0x00, 0x00 },
+	{ V4L2_DV_BT_DMT_800X600P60, 0x01, 0x00 },
+	{ V4L2_DV_BT_DMT_800X600P72, 0x02, 0x00 },
+	{ V4L2_DV_BT_DMT_800X600P75, 0x03, 0x00 },
+	{ V4L2_DV_BT_DMT_800X600P85, 0x04, 0x00 },
+	{ V4L2_DV_BT_DMT_1024X768P60, 0x0c, 0x00 },
+	{ V4L2_DV_BT_DMT_1024X768P70, 0x0d, 0x00 },
+	{ V4L2_DV_BT_DMT_1024X768P75, 0x0e, 0x00 },
+	{ V4L2_DV_BT_DMT_1024X768P85, 0x0f, 0x00 },
+	{ V4L2_DV_BT_DMT_1280X1024P60, 0x05, 0x00 },
+	{ V4L2_DV_BT_DMT_1280X1024P75, 0x06, 0x00 },
+	{ },
+};
+
+static const struct v4l2_event adv7842_ev_fmt = {
+	.type = V4L2_EVENT_SOURCE_CHANGE,
+	.u.src_change.changes = V4L2_EVENT_SRC_CH_RESOLUTION,
+};
+
+/* ----------------------------------------------------------------------- */
+
+static inline struct adv7842_state *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct adv7842_state, sd);
+}
+
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct adv7842_state, hdl)->sd;
+}
+
+static inline unsigned htotal(const struct v4l2_bt_timings *t)
+{
+	return V4L2_DV_BT_FRAME_WIDTH(t);
+}
+
+static inline unsigned vtotal(const struct v4l2_bt_timings *t)
+{
+	return V4L2_DV_BT_FRAME_HEIGHT(t);
+}
+
+
+/* ----------------------------------------------------------------------- */
+
+static s32 adv_smbus_read_byte_data_check(struct i2c_client *client,
+					  u8 command, bool check)
+{
+	union i2c_smbus_data data;
+
+	if (!i2c_smbus_xfer(client->adapter, client->addr, client->flags,
+			    I2C_SMBUS_READ, command,
+			    I2C_SMBUS_BYTE_DATA, &data))
+		return data.byte;
+	if (check)
+		v4l_err(client, "error reading %02x, %02x\n",
+			client->addr, command);
+	return -EIO;
+}
+
+static s32 adv_smbus_read_byte_data(struct i2c_client *client, u8 command)
+{
+	int i;
+
+	for (i = 0; i < 3; i++) {
+		int ret = adv_smbus_read_byte_data_check(client, command, true);
+
+		if (ret >= 0) {
+			if (i)
+				v4l_err(client, "read ok after %d retries\n", i);
+			return ret;
+		}
+	}
+	v4l_err(client, "read failed\n");
+	return -EIO;
+}
+
+static s32 adv_smbus_write_byte_data(struct i2c_client *client,
+				     u8 command, u8 value)
+{
+	union i2c_smbus_data data;
+	int err;
+	int i;
+
+	data.byte = value;
+	for (i = 0; i < 3; i++) {
+		err = i2c_smbus_xfer(client->adapter, client->addr,
+				     client->flags,
+				     I2C_SMBUS_WRITE, command,
+				     I2C_SMBUS_BYTE_DATA, &data);
+		if (!err)
+			break;
+	}
+	if (err < 0)
+		v4l_err(client, "error writing %02x, %02x, %02x\n",
+			client->addr, command, value);
+	return err;
+}
+
+static void adv_smbus_write_byte_no_check(struct i2c_client *client,
+					  u8 command, u8 value)
+{
+	union i2c_smbus_data data;
+	data.byte = value;
+
+	i2c_smbus_xfer(client->adapter, client->addr,
+		       client->flags,
+		       I2C_SMBUS_WRITE, command,
+		       I2C_SMBUS_BYTE_DATA, &data);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static inline int io_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return adv_smbus_read_byte_data(client, reg);
+}
+
+static inline int io_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return adv_smbus_write_byte_data(client, reg, val);
+}
+
+static inline int io_write_and_or(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
+{
+	return io_write(sd, reg, (io_read(sd, reg) & mask) | val);
+}
+
+static inline int io_write_clr_set(struct v4l2_subdev *sd,
+				   u8 reg, u8 mask, u8 val)
+{
+	return io_write(sd, reg, (io_read(sd, reg) & ~mask) | val);
+}
+
+static inline int avlink_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct adv7842_state *state = to_state(sd);
+
+	return adv_smbus_read_byte_data(state->i2c_avlink, reg);
+}
+
+static inline int avlink_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct adv7842_state *state = to_state(sd);
+
+	return adv_smbus_write_byte_data(state->i2c_avlink, reg, val);
+}
+
+static inline int cec_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct adv7842_state *state = to_state(sd);
+
+	return adv_smbus_read_byte_data(state->i2c_cec, reg);
+}
+
+static inline int cec_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct adv7842_state *state = to_state(sd);
+
+	return adv_smbus_write_byte_data(state->i2c_cec, reg, val);
+}
+
+static inline int cec_write_clr_set(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
+{
+	return cec_write(sd, reg, (cec_read(sd, reg) & ~mask) | val);
+}
+
+static inline int infoframe_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct adv7842_state *state = to_state(sd);
+
+	return adv_smbus_read_byte_data(state->i2c_infoframe, reg);
+}
+
+static inline int infoframe_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct adv7842_state *state = to_state(sd);
+
+	return adv_smbus_write_byte_data(state->i2c_infoframe, reg, val);
+}
+
+static inline int sdp_io_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct adv7842_state *state = to_state(sd);
+
+	return adv_smbus_read_byte_data(state->i2c_sdp_io, reg);
+}
+
+static inline int sdp_io_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct adv7842_state *state = to_state(sd);
+
+	return adv_smbus_write_byte_data(state->i2c_sdp_io, reg, val);
+}
+
+static inline int sdp_io_write_and_or(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
+{
+	return sdp_io_write(sd, reg, (sdp_io_read(sd, reg) & mask) | val);
+}
+
+static inline int sdp_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct adv7842_state *state = to_state(sd);
+
+	return adv_smbus_read_byte_data(state->i2c_sdp, reg);
+}
+
+static inline int sdp_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct adv7842_state *state = to_state(sd);
+
+	return adv_smbus_write_byte_data(state->i2c_sdp, reg, val);
+}
+
+static inline int sdp_write_and_or(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
+{
+	return sdp_write(sd, reg, (sdp_read(sd, reg) & mask) | val);
+}
+
+static inline int afe_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct adv7842_state *state = to_state(sd);
+
+	return adv_smbus_read_byte_data(state->i2c_afe, reg);
+}
+
+static inline int afe_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct adv7842_state *state = to_state(sd);
+
+	return adv_smbus_write_byte_data(state->i2c_afe, reg, val);
+}
+
+static inline int afe_write_and_or(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
+{
+	return afe_write(sd, reg, (afe_read(sd, reg) & mask) | val);
+}
+
+static inline int rep_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct adv7842_state *state = to_state(sd);
+
+	return adv_smbus_read_byte_data(state->i2c_repeater, reg);
+}
+
+static inline int rep_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct adv7842_state *state = to_state(sd);
+
+	return adv_smbus_write_byte_data(state->i2c_repeater, reg, val);
+}
+
+static inline int rep_write_and_or(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
+{
+	return rep_write(sd, reg, (rep_read(sd, reg) & mask) | val);
+}
+
+static inline int edid_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct adv7842_state *state = to_state(sd);
+
+	return adv_smbus_read_byte_data(state->i2c_edid, reg);
+}
+
+static inline int edid_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct adv7842_state *state = to_state(sd);
+
+	return adv_smbus_write_byte_data(state->i2c_edid, reg, val);
+}
+
+static inline int hdmi_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct adv7842_state *state = to_state(sd);
+
+	return adv_smbus_read_byte_data(state->i2c_hdmi, reg);
+}
+
+static inline int hdmi_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct adv7842_state *state = to_state(sd);
+
+	return adv_smbus_write_byte_data(state->i2c_hdmi, reg, val);
+}
+
+static inline int hdmi_write_and_or(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
+{
+	return hdmi_write(sd, reg, (hdmi_read(sd, reg) & mask) | val);
+}
+
+static inline int cp_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct adv7842_state *state = to_state(sd);
+
+	return adv_smbus_read_byte_data(state->i2c_cp, reg);
+}
+
+static inline int cp_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct adv7842_state *state = to_state(sd);
+
+	return adv_smbus_write_byte_data(state->i2c_cp, reg, val);
+}
+
+static inline int cp_write_and_or(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
+{
+	return cp_write(sd, reg, (cp_read(sd, reg) & mask) | val);
+}
+
+static inline int vdp_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct adv7842_state *state = to_state(sd);
+
+	return adv_smbus_read_byte_data(state->i2c_vdp, reg);
+}
+
+static inline int vdp_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct adv7842_state *state = to_state(sd);
+
+	return adv_smbus_write_byte_data(state->i2c_vdp, reg, val);
+}
+
+static void main_reset(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	v4l2_dbg(1, debug, sd, "%s:\n", __func__);
+
+	adv_smbus_write_byte_no_check(client, 0xff, 0x80);
+
+	mdelay(5);
+}
+
+/* -----------------------------------------------------------------------------
+ * Format helpers
+ */
+
+static const struct adv7842_format_info adv7842_formats[] = {
+	{ MEDIA_BUS_FMT_RGB888_1X24, ADV7842_OP_CH_SEL_RGB, true, false,
+	  ADV7842_OP_MODE_SEL_SDR_444 | ADV7842_OP_FORMAT_SEL_8BIT },
+	{ MEDIA_BUS_FMT_YUYV8_2X8, ADV7842_OP_CH_SEL_RGB, false, false,
+	  ADV7842_OP_MODE_SEL_SDR_422 | ADV7842_OP_FORMAT_SEL_8BIT },
+	{ MEDIA_BUS_FMT_YVYU8_2X8, ADV7842_OP_CH_SEL_RGB, false, true,
+	  ADV7842_OP_MODE_SEL_SDR_422 | ADV7842_OP_FORMAT_SEL_8BIT },
+	{ MEDIA_BUS_FMT_YUYV10_2X10, ADV7842_OP_CH_SEL_RGB, false, false,
+	  ADV7842_OP_MODE_SEL_SDR_422 | ADV7842_OP_FORMAT_SEL_10BIT },
+	{ MEDIA_BUS_FMT_YVYU10_2X10, ADV7842_OP_CH_SEL_RGB, false, true,
+	  ADV7842_OP_MODE_SEL_SDR_422 | ADV7842_OP_FORMAT_SEL_10BIT },
+	{ MEDIA_BUS_FMT_YUYV12_2X12, ADV7842_OP_CH_SEL_RGB, false, false,
+	  ADV7842_OP_MODE_SEL_SDR_422 | ADV7842_OP_FORMAT_SEL_12BIT },
+	{ MEDIA_BUS_FMT_YVYU12_2X12, ADV7842_OP_CH_SEL_RGB, false, true,
+	  ADV7842_OP_MODE_SEL_SDR_422 | ADV7842_OP_FORMAT_SEL_12BIT },
+	{ MEDIA_BUS_FMT_UYVY8_1X16, ADV7842_OP_CH_SEL_RBG, false, false,
+	  ADV7842_OP_MODE_SEL_SDR_422_2X | ADV7842_OP_FORMAT_SEL_8BIT },
+	{ MEDIA_BUS_FMT_VYUY8_1X16, ADV7842_OP_CH_SEL_RBG, false, true,
+	  ADV7842_OP_MODE_SEL_SDR_422_2X | ADV7842_OP_FORMAT_SEL_8BIT },
+	{ MEDIA_BUS_FMT_YUYV8_1X16, ADV7842_OP_CH_SEL_RGB, false, false,
+	  ADV7842_OP_MODE_SEL_SDR_422_2X | ADV7842_OP_FORMAT_SEL_8BIT },
+	{ MEDIA_BUS_FMT_YVYU8_1X16, ADV7842_OP_CH_SEL_RGB, false, true,
+	  ADV7842_OP_MODE_SEL_SDR_422_2X | ADV7842_OP_FORMAT_SEL_8BIT },
+	{ MEDIA_BUS_FMT_UYVY10_1X20, ADV7842_OP_CH_SEL_RBG, false, false,
+	  ADV7842_OP_MODE_SEL_SDR_422_2X | ADV7842_OP_FORMAT_SEL_10BIT },
+	{ MEDIA_BUS_FMT_VYUY10_1X20, ADV7842_OP_CH_SEL_RBG, false, true,
+	  ADV7842_OP_MODE_SEL_SDR_422_2X | ADV7842_OP_FORMAT_SEL_10BIT },
+	{ MEDIA_BUS_FMT_YUYV10_1X20, ADV7842_OP_CH_SEL_RGB, false, false,
+	  ADV7842_OP_MODE_SEL_SDR_422_2X | ADV7842_OP_FORMAT_SEL_10BIT },
+	{ MEDIA_BUS_FMT_YVYU10_1X20, ADV7842_OP_CH_SEL_RGB, false, true,
+	  ADV7842_OP_MODE_SEL_SDR_422_2X | ADV7842_OP_FORMAT_SEL_10BIT },
+	{ MEDIA_BUS_FMT_UYVY12_1X24, ADV7842_OP_CH_SEL_RBG, false, false,
+	  ADV7842_OP_MODE_SEL_SDR_422_2X | ADV7842_OP_FORMAT_SEL_12BIT },
+	{ MEDIA_BUS_FMT_VYUY12_1X24, ADV7842_OP_CH_SEL_RBG, false, true,
+	  ADV7842_OP_MODE_SEL_SDR_422_2X | ADV7842_OP_FORMAT_SEL_12BIT },
+	{ MEDIA_BUS_FMT_YUYV12_1X24, ADV7842_OP_CH_SEL_RGB, false, false,
+	  ADV7842_OP_MODE_SEL_SDR_422_2X | ADV7842_OP_FORMAT_SEL_12BIT },
+	{ MEDIA_BUS_FMT_YVYU12_1X24, ADV7842_OP_CH_SEL_RGB, false, true,
+	  ADV7842_OP_MODE_SEL_SDR_422_2X | ADV7842_OP_FORMAT_SEL_12BIT },
+};
+
+static const struct adv7842_format_info *
+adv7842_format_info(struct adv7842_state *state, u32 code)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(adv7842_formats); ++i) {
+		if (adv7842_formats[i].code == code)
+			return &adv7842_formats[i];
+	}
+
+	return NULL;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static inline bool is_analog_input(struct v4l2_subdev *sd)
+{
+	struct adv7842_state *state = to_state(sd);
+
+	return ((state->mode == ADV7842_MODE_RGB) ||
+		(state->mode == ADV7842_MODE_COMP));
+}
+
+static inline bool is_digital_input(struct v4l2_subdev *sd)
+{
+	struct adv7842_state *state = to_state(sd);
+
+	return state->mode == ADV7842_MODE_HDMI;
+}
+
+static const struct v4l2_dv_timings_cap adv7842_timings_cap_analog = {
+	.type = V4L2_DV_BT_656_1120,
+	/* keep this initialization for compatibility with GCC < 4.4.6 */
+	.reserved = { 0 },
+	V4L2_INIT_BT_TIMINGS(640, 1920, 350, 1200, 25000000, 170000000,
+		V4L2_DV_BT_STD_CEA861 | V4L2_DV_BT_STD_DMT |
+			V4L2_DV_BT_STD_GTF | V4L2_DV_BT_STD_CVT,
+		V4L2_DV_BT_CAP_PROGRESSIVE | V4L2_DV_BT_CAP_REDUCED_BLANKING |
+			V4L2_DV_BT_CAP_CUSTOM)
+};
+
+static const struct v4l2_dv_timings_cap adv7842_timings_cap_digital = {
+	.type = V4L2_DV_BT_656_1120,
+	/* keep this initialization for compatibility with GCC < 4.4.6 */
+	.reserved = { 0 },
+	V4L2_INIT_BT_TIMINGS(640, 1920, 350, 1200, 25000000, 225000000,
+		V4L2_DV_BT_STD_CEA861 | V4L2_DV_BT_STD_DMT |
+			V4L2_DV_BT_STD_GTF | V4L2_DV_BT_STD_CVT,
+		V4L2_DV_BT_CAP_PROGRESSIVE | V4L2_DV_BT_CAP_REDUCED_BLANKING |
+			V4L2_DV_BT_CAP_CUSTOM)
+};
+
+static inline const struct v4l2_dv_timings_cap *
+adv7842_get_dv_timings_cap(struct v4l2_subdev *sd)
+{
+	return is_digital_input(sd) ? &adv7842_timings_cap_digital :
+				      &adv7842_timings_cap_analog;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static u16 adv7842_read_cable_det(struct v4l2_subdev *sd)
+{
+	u8 reg = io_read(sd, 0x6f);
+	u16 val = 0;
+
+	if (reg & 0x02)
+		val |= 1; /* port A */
+	if (reg & 0x01)
+		val |= 2; /* port B */
+	return val;
+}
+
+static void adv7842_delayed_work_enable_hotplug(struct work_struct *work)
+{
+	struct delayed_work *dwork = to_delayed_work(work);
+	struct adv7842_state *state = container_of(dwork,
+			struct adv7842_state, delayed_work_enable_hotplug);
+	struct v4l2_subdev *sd = &state->sd;
+	int present = state->hdmi_edid.present;
+	u8 mask = 0;
+
+	v4l2_dbg(2, debug, sd, "%s: enable hotplug on ports: 0x%x\n",
+			__func__, present);
+
+	if (present & (0x04 << ADV7842_EDID_PORT_A))
+		mask |= 0x20;
+	if (present & (0x04 << ADV7842_EDID_PORT_B))
+		mask |= 0x10;
+	io_write_and_or(sd, 0x20, 0xcf, mask);
+}
+
+static int edid_write_vga_segment(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct adv7842_state *state = to_state(sd);
+	const u8 *edid = state->vga_edid.edid;
+	u32 blocks = state->vga_edid.blocks;
+	int err = 0;
+	int i;
+
+	v4l2_dbg(2, debug, sd, "%s: write EDID on VGA port\n", __func__);
+
+	if (!state->vga_edid.present)
+		return 0;
+
+	/* HPA disable on port A and B */
+	io_write_and_or(sd, 0x20, 0xcf, 0x00);
+
+	/* Disable I2C access to internal EDID ram from VGA DDC port */
+	rep_write_and_or(sd, 0x7f, 0x7f, 0x00);
+
+	/* edid segment pointer '1' for VGA port */
+	rep_write_and_or(sd, 0x77, 0xef, 0x10);
+
+	for (i = 0; !err && i < blocks * 128; i += I2C_SMBUS_BLOCK_MAX)
+		err = i2c_smbus_write_i2c_block_data(state->i2c_edid, i,
+						     I2C_SMBUS_BLOCK_MAX,
+						     edid + i);
+	if (err)
+		return err;
+
+	/* Calculates the checksums and enables I2C access
+	 * to internal EDID ram from VGA DDC port.
+	 */
+	rep_write_and_or(sd, 0x7f, 0x7f, 0x80);
+
+	for (i = 0; i < 1000; i++) {
+		if (rep_read(sd, 0x79) & 0x20)
+			break;
+		mdelay(1);
+	}
+	if (i == 1000) {
+		v4l_err(client, "error enabling edid on VGA port\n");
+		return -EIO;
+	}
+
+	/* enable hotplug after 200 ms */
+	schedule_delayed_work(&state->delayed_work_enable_hotplug, HZ / 5);
+
+	return 0;
+}
+
+static int edid_write_hdmi_segment(struct v4l2_subdev *sd, u8 port)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct adv7842_state *state = to_state(sd);
+	const u8 *edid = state->hdmi_edid.edid;
+	u32 blocks = state->hdmi_edid.blocks;
+	unsigned int spa_loc;
+	u16 pa, parent_pa;
+	int err = 0;
+	int i;
+
+	v4l2_dbg(2, debug, sd, "%s: write EDID on port %c\n",
+			__func__, (port == ADV7842_EDID_PORT_A) ? 'A' : 'B');
+
+	/* HPA disable on port A and B */
+	io_write_and_or(sd, 0x20, 0xcf, 0x00);
+
+	/* Disable I2C access to internal EDID ram from HDMI DDC ports */
+	rep_write_and_or(sd, 0x77, 0xf3, 0x00);
+
+	if (!state->hdmi_edid.present) {
+		cec_phys_addr_invalidate(state->cec_adap);
+		return 0;
+	}
+
+	pa = v4l2_get_edid_phys_addr(edid, blocks * 128, &spa_loc);
+	err = v4l2_phys_addr_validate(pa, &parent_pa, NULL);
+	if (err)
+		return err;
+
+	if (!spa_loc) {
+		/*
+		 * There is no SPA, so just set spa_loc to 128 and pa to whatever
+		 * data is there.
+		 */
+		spa_loc = 128;
+		pa = (edid[spa_loc] << 8) | edid[spa_loc + 1];
+	}
+
+
+	for (i = 0; !err && i < blocks * 128; i += I2C_SMBUS_BLOCK_MAX) {
+		/* set edid segment pointer for HDMI ports */
+		if (i % 256 == 0)
+			rep_write_and_or(sd, 0x77, 0xef, i >= 256 ? 0x10 : 0x00);
+		err = i2c_smbus_write_i2c_block_data(state->i2c_edid, i,
+						     I2C_SMBUS_BLOCK_MAX, edid + i);
+	}
+	if (err)
+		return err;
+
+	if (port == ADV7842_EDID_PORT_A) {
+		rep_write(sd, 0x72, pa >> 8);
+		rep_write(sd, 0x73, pa & 0xff);
+	} else {
+		rep_write(sd, 0x74, pa >> 8);
+		rep_write(sd, 0x75, pa & 0xff);
+	}
+	rep_write(sd, 0x76, spa_loc & 0xff);
+	rep_write_and_or(sd, 0x77, 0xbf, (spa_loc >> 2) & 0x40);
+
+	/* Calculates the checksums and enables I2C access to internal
+	 * EDID ram from HDMI DDC ports
+	 */
+	rep_write_and_or(sd, 0x77, 0xf3, state->hdmi_edid.present);
+
+	for (i = 0; i < 1000; i++) {
+		if (rep_read(sd, 0x7d) & state->hdmi_edid.present)
+			break;
+		mdelay(1);
+	}
+	if (i == 1000) {
+		v4l_err(client, "error enabling edid on port %c\n",
+				(port == ADV7842_EDID_PORT_A) ? 'A' : 'B');
+		return -EIO;
+	}
+	cec_s_phys_addr(state->cec_adap, parent_pa, false);
+
+	/* enable hotplug after 200 ms */
+	schedule_delayed_work(&state->delayed_work_enable_hotplug, HZ / 5);
+
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static void adv7842_inv_register(struct v4l2_subdev *sd)
+{
+	v4l2_info(sd, "0x000-0x0ff: IO Map\n");
+	v4l2_info(sd, "0x100-0x1ff: AVLink Map\n");
+	v4l2_info(sd, "0x200-0x2ff: CEC Map\n");
+	v4l2_info(sd, "0x300-0x3ff: InfoFrame Map\n");
+	v4l2_info(sd, "0x400-0x4ff: SDP_IO Map\n");
+	v4l2_info(sd, "0x500-0x5ff: SDP Map\n");
+	v4l2_info(sd, "0x600-0x6ff: AFE Map\n");
+	v4l2_info(sd, "0x700-0x7ff: Repeater Map\n");
+	v4l2_info(sd, "0x800-0x8ff: EDID Map\n");
+	v4l2_info(sd, "0x900-0x9ff: HDMI Map\n");
+	v4l2_info(sd, "0xa00-0xaff: CP Map\n");
+	v4l2_info(sd, "0xb00-0xbff: VDP Map\n");
+}
+
+static int adv7842_g_register(struct v4l2_subdev *sd,
+			      struct v4l2_dbg_register *reg)
+{
+	reg->size = 1;
+	switch (reg->reg >> 8) {
+	case 0:
+		reg->val = io_read(sd, reg->reg & 0xff);
+		break;
+	case 1:
+		reg->val = avlink_read(sd, reg->reg & 0xff);
+		break;
+	case 2:
+		reg->val = cec_read(sd, reg->reg & 0xff);
+		break;
+	case 3:
+		reg->val = infoframe_read(sd, reg->reg & 0xff);
+		break;
+	case 4:
+		reg->val = sdp_io_read(sd, reg->reg & 0xff);
+		break;
+	case 5:
+		reg->val = sdp_read(sd, reg->reg & 0xff);
+		break;
+	case 6:
+		reg->val = afe_read(sd, reg->reg & 0xff);
+		break;
+	case 7:
+		reg->val = rep_read(sd, reg->reg & 0xff);
+		break;
+	case 8:
+		reg->val = edid_read(sd, reg->reg & 0xff);
+		break;
+	case 9:
+		reg->val = hdmi_read(sd, reg->reg & 0xff);
+		break;
+	case 0xa:
+		reg->val = cp_read(sd, reg->reg & 0xff);
+		break;
+	case 0xb:
+		reg->val = vdp_read(sd, reg->reg & 0xff);
+		break;
+	default:
+		v4l2_info(sd, "Register %03llx not supported\n", reg->reg);
+		adv7842_inv_register(sd);
+		break;
+	}
+	return 0;
+}
+
+static int adv7842_s_register(struct v4l2_subdev *sd,
+		const struct v4l2_dbg_register *reg)
+{
+	u8 val = reg->val & 0xff;
+
+	switch (reg->reg >> 8) {
+	case 0:
+		io_write(sd, reg->reg & 0xff, val);
+		break;
+	case 1:
+		avlink_write(sd, reg->reg & 0xff, val);
+		break;
+	case 2:
+		cec_write(sd, reg->reg & 0xff, val);
+		break;
+	case 3:
+		infoframe_write(sd, reg->reg & 0xff, val);
+		break;
+	case 4:
+		sdp_io_write(sd, reg->reg & 0xff, val);
+		break;
+	case 5:
+		sdp_write(sd, reg->reg & 0xff, val);
+		break;
+	case 6:
+		afe_write(sd, reg->reg & 0xff, val);
+		break;
+	case 7:
+		rep_write(sd, reg->reg & 0xff, val);
+		break;
+	case 8:
+		edid_write(sd, reg->reg & 0xff, val);
+		break;
+	case 9:
+		hdmi_write(sd, reg->reg & 0xff, val);
+		break;
+	case 0xa:
+		cp_write(sd, reg->reg & 0xff, val);
+		break;
+	case 0xb:
+		vdp_write(sd, reg->reg & 0xff, val);
+		break;
+	default:
+		v4l2_info(sd, "Register %03llx not supported\n", reg->reg);
+		adv7842_inv_register(sd);
+		break;
+	}
+	return 0;
+}
+#endif
+
+static int adv7842_s_detect_tx_5v_ctrl(struct v4l2_subdev *sd)
+{
+	struct adv7842_state *state = to_state(sd);
+	u16 cable_det = adv7842_read_cable_det(sd);
+
+	v4l2_dbg(1, debug, sd, "%s: 0x%x\n", __func__, cable_det);
+
+	return v4l2_ctrl_s_ctrl(state->detect_tx_5v_ctrl, cable_det);
+}
+
+static int find_and_set_predefined_video_timings(struct v4l2_subdev *sd,
+		u8 prim_mode,
+		const struct adv7842_video_standards *predef_vid_timings,
+		const struct v4l2_dv_timings *timings)
+{
+	int i;
+
+	for (i = 0; predef_vid_timings[i].timings.bt.width; i++) {
+		if (!v4l2_match_dv_timings(timings, &predef_vid_timings[i].timings,
+				  is_digital_input(sd) ? 250000 : 1000000, false))
+			continue;
+		/* video std */
+		io_write(sd, 0x00, predef_vid_timings[i].vid_std);
+		/* v_freq and prim mode */
+		io_write(sd, 0x01, (predef_vid_timings[i].v_freq << 4) + prim_mode);
+		return 0;
+	}
+
+	return -1;
+}
+
+static int configure_predefined_video_timings(struct v4l2_subdev *sd,
+		struct v4l2_dv_timings *timings)
+{
+	struct adv7842_state *state = to_state(sd);
+	int err;
+
+	v4l2_dbg(1, debug, sd, "%s\n", __func__);
+
+	/* reset to default values */
+	io_write(sd, 0x16, 0x43);
+	io_write(sd, 0x17, 0x5a);
+	/* disable embedded syncs for auto graphics mode */
+	cp_write_and_or(sd, 0x81, 0xef, 0x00);
+	cp_write(sd, 0x26, 0x00);
+	cp_write(sd, 0x27, 0x00);
+	cp_write(sd, 0x28, 0x00);
+	cp_write(sd, 0x29, 0x00);
+	cp_write(sd, 0x8f, 0x40);
+	cp_write(sd, 0x90, 0x00);
+	cp_write(sd, 0xa5, 0x00);
+	cp_write(sd, 0xa6, 0x00);
+	cp_write(sd, 0xa7, 0x00);
+	cp_write(sd, 0xab, 0x00);
+	cp_write(sd, 0xac, 0x00);
+
+	switch (state->mode) {
+	case ADV7842_MODE_COMP:
+	case ADV7842_MODE_RGB:
+		err = find_and_set_predefined_video_timings(sd,
+				0x01, adv7842_prim_mode_comp, timings);
+		if (err)
+			err = find_and_set_predefined_video_timings(sd,
+					0x02, adv7842_prim_mode_gr, timings);
+		break;
+	case ADV7842_MODE_HDMI:
+		err = find_and_set_predefined_video_timings(sd,
+				0x05, adv7842_prim_mode_hdmi_comp, timings);
+		if (err)
+			err = find_and_set_predefined_video_timings(sd,
+					0x06, adv7842_prim_mode_hdmi_gr, timings);
+		break;
+	default:
+		v4l2_dbg(2, debug, sd, "%s: Unknown mode %d\n",
+				__func__, state->mode);
+		err = -1;
+		break;
+	}
+
+
+	return err;
+}
+
+static void configure_custom_video_timings(struct v4l2_subdev *sd,
+		const struct v4l2_bt_timings *bt)
+{
+	struct adv7842_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u32 width = htotal(bt);
+	u32 height = vtotal(bt);
+	u16 cp_start_sav = bt->hsync + bt->hbackporch - 4;
+	u16 cp_start_eav = width - bt->hfrontporch;
+	u16 cp_start_vbi = height - bt->vfrontporch + 1;
+	u16 cp_end_vbi = bt->vsync + bt->vbackporch + 1;
+	u16 ch1_fr_ll = (((u32)bt->pixelclock / 100) > 0) ?
+		((width * (ADV7842_fsc / 100)) / ((u32)bt->pixelclock / 100)) : 0;
+	const u8 pll[2] = {
+		0xc0 | ((width >> 8) & 0x1f),
+		width & 0xff
+	};
+
+	v4l2_dbg(2, debug, sd, "%s\n", __func__);
+
+	switch (state->mode) {
+	case ADV7842_MODE_COMP:
+	case ADV7842_MODE_RGB:
+		/* auto graphics */
+		io_write(sd, 0x00, 0x07); /* video std */
+		io_write(sd, 0x01, 0x02); /* prim mode */
+		/* enable embedded syncs for auto graphics mode */
+		cp_write_and_or(sd, 0x81, 0xef, 0x10);
+
+		/* Should only be set in auto-graphics mode [REF_02, p. 91-92] */
+		/* setup PLL_DIV_MAN_EN and PLL_DIV_RATIO */
+		/* IO-map reg. 0x16 and 0x17 should be written in sequence */
+		if (i2c_smbus_write_i2c_block_data(client, 0x16, 2, pll)) {
+			v4l2_err(sd, "writing to reg 0x16 and 0x17 failed\n");
+			break;
+		}
+
+		/* active video - horizontal timing */
+		cp_write(sd, 0x26, (cp_start_sav >> 8) & 0xf);
+		cp_write(sd, 0x27, (cp_start_sav & 0xff));
+		cp_write(sd, 0x28, (cp_start_eav >> 8) & 0xf);
+		cp_write(sd, 0x29, (cp_start_eav & 0xff));
+
+		/* active video - vertical timing */
+		cp_write(sd, 0xa5, (cp_start_vbi >> 4) & 0xff);
+		cp_write(sd, 0xa6, ((cp_start_vbi & 0xf) << 4) |
+					((cp_end_vbi >> 8) & 0xf));
+		cp_write(sd, 0xa7, cp_end_vbi & 0xff);
+		break;
+	case ADV7842_MODE_HDMI:
+		/* set default prim_mode/vid_std for HDMI
+		   according to [REF_03, c. 4.2] */
+		io_write(sd, 0x00, 0x02); /* video std */
+		io_write(sd, 0x01, 0x06); /* prim mode */
+		break;
+	default:
+		v4l2_dbg(2, debug, sd, "%s: Unknown mode %d\n",
+				__func__, state->mode);
+		break;
+	}
+
+	cp_write(sd, 0x8f, (ch1_fr_ll >> 8) & 0x7);
+	cp_write(sd, 0x90, ch1_fr_ll & 0xff);
+	cp_write(sd, 0xab, (height >> 4) & 0xff);
+	cp_write(sd, 0xac, (height & 0x0f) << 4);
+}
+
+static void adv7842_set_offset(struct v4l2_subdev *sd, bool auto_offset, u16 offset_a, u16 offset_b, u16 offset_c)
+{
+	struct adv7842_state *state = to_state(sd);
+	u8 offset_buf[4];
+
+	if (auto_offset) {
+		offset_a = 0x3ff;
+		offset_b = 0x3ff;
+		offset_c = 0x3ff;
+	}
+
+	v4l2_dbg(2, debug, sd, "%s: %s offset: a = 0x%x, b = 0x%x, c = 0x%x\n",
+		 __func__, auto_offset ? "Auto" : "Manual",
+		 offset_a, offset_b, offset_c);
+
+	offset_buf[0]= (cp_read(sd, 0x77) & 0xc0) | ((offset_a & 0x3f0) >> 4);
+	offset_buf[1] = ((offset_a & 0x00f) << 4) | ((offset_b & 0x3c0) >> 6);
+	offset_buf[2] = ((offset_b & 0x03f) << 2) | ((offset_c & 0x300) >> 8);
+	offset_buf[3] = offset_c & 0x0ff;
+
+	/* Registers must be written in this order with no i2c access in between */
+	if (i2c_smbus_write_i2c_block_data(state->i2c_cp, 0x77, 4, offset_buf))
+		v4l2_err(sd, "%s: i2c error writing to CP reg 0x77, 0x78, 0x79, 0x7a\n", __func__);
+}
+
+static void adv7842_set_gain(struct v4l2_subdev *sd, bool auto_gain, u16 gain_a, u16 gain_b, u16 gain_c)
+{
+	struct adv7842_state *state = to_state(sd);
+	u8 gain_buf[4];
+	u8 gain_man = 1;
+	u8 agc_mode_man = 1;
+
+	if (auto_gain) {
+		gain_man = 0;
+		agc_mode_man = 0;
+		gain_a = 0x100;
+		gain_b = 0x100;
+		gain_c = 0x100;
+	}
+
+	v4l2_dbg(2, debug, sd, "%s: %s gain: a = 0x%x, b = 0x%x, c = 0x%x\n",
+		 __func__, auto_gain ? "Auto" : "Manual",
+		 gain_a, gain_b, gain_c);
+
+	gain_buf[0] = ((gain_man << 7) | (agc_mode_man << 6) | ((gain_a & 0x3f0) >> 4));
+	gain_buf[1] = (((gain_a & 0x00f) << 4) | ((gain_b & 0x3c0) >> 6));
+	gain_buf[2] = (((gain_b & 0x03f) << 2) | ((gain_c & 0x300) >> 8));
+	gain_buf[3] = ((gain_c & 0x0ff));
+
+	/* Registers must be written in this order with no i2c access in between */
+	if (i2c_smbus_write_i2c_block_data(state->i2c_cp, 0x73, 4, gain_buf))
+		v4l2_err(sd, "%s: i2c error writing to CP reg 0x73, 0x74, 0x75, 0x76\n", __func__);
+}
+
+static void set_rgb_quantization_range(struct v4l2_subdev *sd)
+{
+	struct adv7842_state *state = to_state(sd);
+	bool rgb_output = io_read(sd, 0x02) & 0x02;
+	bool hdmi_signal = hdmi_read(sd, 0x05) & 0x80;
+	u8 y = HDMI_COLORSPACE_RGB;
+
+	if (hdmi_signal && (io_read(sd, 0x60) & 1))
+		y = infoframe_read(sd, 0x01) >> 5;
+
+	v4l2_dbg(2, debug, sd, "%s: RGB quantization range: %d, RGB out: %d, HDMI: %d\n",
+			__func__, state->rgb_quantization_range,
+			rgb_output, hdmi_signal);
+
+	adv7842_set_gain(sd, true, 0x0, 0x0, 0x0);
+	adv7842_set_offset(sd, true, 0x0, 0x0, 0x0);
+	io_write_clr_set(sd, 0x02, 0x04, rgb_output ? 0 : 4);
+
+	switch (state->rgb_quantization_range) {
+	case V4L2_DV_RGB_RANGE_AUTO:
+		if (state->mode == ADV7842_MODE_RGB) {
+			/* Receiving analog RGB signal
+			 * Set RGB full range (0-255) */
+			io_write_and_or(sd, 0x02, 0x0f, 0x10);
+			break;
+		}
+
+		if (state->mode == ADV7842_MODE_COMP) {
+			/* Receiving analog YPbPr signal
+			 * Set automode */
+			io_write_and_or(sd, 0x02, 0x0f, 0xf0);
+			break;
+		}
+
+		if (hdmi_signal) {
+			/* Receiving HDMI signal
+			 * Set automode */
+			io_write_and_or(sd, 0x02, 0x0f, 0xf0);
+			break;
+		}
+
+		/* Receiving DVI-D signal
+		 * ADV7842 selects RGB limited range regardless of
+		 * input format (CE/IT) in automatic mode */
+		if (state->timings.bt.flags & V4L2_DV_FL_IS_CE_VIDEO) {
+			/* RGB limited range (16-235) */
+			io_write_and_or(sd, 0x02, 0x0f, 0x00);
+		} else {
+			/* RGB full range (0-255) */
+			io_write_and_or(sd, 0x02, 0x0f, 0x10);
+
+			if (is_digital_input(sd) && rgb_output) {
+				adv7842_set_offset(sd, false, 0x40, 0x40, 0x40);
+			} else {
+				adv7842_set_gain(sd, false, 0xe0, 0xe0, 0xe0);
+				adv7842_set_offset(sd, false, 0x70, 0x70, 0x70);
+			}
+		}
+		break;
+	case V4L2_DV_RGB_RANGE_LIMITED:
+		if (state->mode == ADV7842_MODE_COMP) {
+			/* YCrCb limited range (16-235) */
+			io_write_and_or(sd, 0x02, 0x0f, 0x20);
+			break;
+		}
+
+		if (y != HDMI_COLORSPACE_RGB)
+			break;
+
+		/* RGB limited range (16-235) */
+		io_write_and_or(sd, 0x02, 0x0f, 0x00);
+
+		break;
+	case V4L2_DV_RGB_RANGE_FULL:
+		if (state->mode == ADV7842_MODE_COMP) {
+			/* YCrCb full range (0-255) */
+			io_write_and_or(sd, 0x02, 0x0f, 0x60);
+			break;
+		}
+
+		if (y != HDMI_COLORSPACE_RGB)
+			break;
+
+		/* RGB full range (0-255) */
+		io_write_and_or(sd, 0x02, 0x0f, 0x10);
+
+		if (is_analog_input(sd) || hdmi_signal)
+			break;
+
+		/* Adjust gain/offset for DVI-D signals only */
+		if (rgb_output) {
+			adv7842_set_offset(sd, false, 0x40, 0x40, 0x40);
+		} else {
+			adv7842_set_gain(sd, false, 0xe0, 0xe0, 0xe0);
+			adv7842_set_offset(sd, false, 0x70, 0x70, 0x70);
+		}
+		break;
+	}
+}
+
+static int adv7842_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+	struct adv7842_state *state = to_state(sd);
+
+	/* TODO SDP ctrls
+	   contrast/brightness/hue/free run is acting a bit strange,
+	   not sure if sdp csc is correct.
+	 */
+	switch (ctrl->id) {
+	/* standard ctrls */
+	case V4L2_CID_BRIGHTNESS:
+		cp_write(sd, 0x3c, ctrl->val);
+		sdp_write(sd, 0x14, ctrl->val);
+		/* ignore lsb sdp 0x17[3:2] */
+		return 0;
+	case V4L2_CID_CONTRAST:
+		cp_write(sd, 0x3a, ctrl->val);
+		sdp_write(sd, 0x13, ctrl->val);
+		/* ignore lsb sdp 0x17[1:0] */
+		return 0;
+	case V4L2_CID_SATURATION:
+		cp_write(sd, 0x3b, ctrl->val);
+		sdp_write(sd, 0x15, ctrl->val);
+		/* ignore lsb sdp 0x17[5:4] */
+		return 0;
+	case V4L2_CID_HUE:
+		cp_write(sd, 0x3d, ctrl->val);
+		sdp_write(sd, 0x16, ctrl->val);
+		/* ignore lsb sdp 0x17[7:6] */
+		return 0;
+		/* custom ctrls */
+	case V4L2_CID_ADV_RX_ANALOG_SAMPLING_PHASE:
+		afe_write(sd, 0xc8, ctrl->val);
+		return 0;
+	case V4L2_CID_ADV_RX_FREE_RUN_COLOR_MANUAL:
+		cp_write_and_or(sd, 0xbf, ~0x04, (ctrl->val << 2));
+		sdp_write_and_or(sd, 0xdd, ~0x04, (ctrl->val << 2));
+		return 0;
+	case V4L2_CID_ADV_RX_FREE_RUN_COLOR: {
+		u8 R = (ctrl->val & 0xff0000) >> 16;
+		u8 G = (ctrl->val & 0x00ff00) >> 8;
+		u8 B = (ctrl->val & 0x0000ff);
+		/* RGB -> YUV, numerical approximation */
+		int Y = 66 * R + 129 * G + 25 * B;
+		int U = -38 * R - 74 * G + 112 * B;
+		int V = 112 * R - 94 * G - 18 * B;
+
+		/* Scale down to 8 bits with rounding */
+		Y = (Y + 128) >> 8;
+		U = (U + 128) >> 8;
+		V = (V + 128) >> 8;
+		/* make U,V positive */
+		Y += 16;
+		U += 128;
+		V += 128;
+
+		v4l2_dbg(1, debug, sd, "R %x, G %x, B %x\n", R, G, B);
+		v4l2_dbg(1, debug, sd, "Y %x, U %x, V %x\n", Y, U, V);
+
+		/* CP */
+		cp_write(sd, 0xc1, R);
+		cp_write(sd, 0xc0, G);
+		cp_write(sd, 0xc2, B);
+		/* SDP */
+		sdp_write(sd, 0xde, Y);
+		sdp_write(sd, 0xdf, (V & 0xf0) | ((U >> 4) & 0x0f));
+		return 0;
+	}
+	case V4L2_CID_DV_RX_RGB_RANGE:
+		state->rgb_quantization_range = ctrl->val;
+		set_rgb_quantization_range(sd);
+		return 0;
+	}
+	return -EINVAL;
+}
+
+static int adv7842_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+
+	if (ctrl->id == V4L2_CID_DV_RX_IT_CONTENT_TYPE) {
+		ctrl->val = V4L2_DV_IT_CONTENT_TYPE_NO_ITC;
+		if ((io_read(sd, 0x60) & 1) && (infoframe_read(sd, 0x03) & 0x80))
+			ctrl->val = (infoframe_read(sd, 0x05) >> 4) & 3;
+		return 0;
+	}
+	return -EINVAL;
+}
+
+static inline bool no_power(struct v4l2_subdev *sd)
+{
+	return io_read(sd, 0x0c) & 0x24;
+}
+
+static inline bool no_cp_signal(struct v4l2_subdev *sd)
+{
+	return ((cp_read(sd, 0xb5) & 0xd0) != 0xd0) || !(cp_read(sd, 0xb1) & 0x80);
+}
+
+static inline bool is_hdmi(struct v4l2_subdev *sd)
+{
+	return hdmi_read(sd, 0x05) & 0x80;
+}
+
+static int adv7842_g_input_status(struct v4l2_subdev *sd, u32 *status)
+{
+	struct adv7842_state *state = to_state(sd);
+
+	*status = 0;
+
+	if (io_read(sd, 0x0c) & 0x24)
+		*status |= V4L2_IN_ST_NO_POWER;
+
+	if (state->mode == ADV7842_MODE_SDP) {
+		/* status from SDP block */
+		if (!(sdp_read(sd, 0x5A) & 0x01))
+			*status |= V4L2_IN_ST_NO_SIGNAL;
+
+		v4l2_dbg(1, debug, sd, "%s: SDP status = 0x%x\n",
+				__func__, *status);
+		return 0;
+	}
+	/* status from CP block */
+	if ((cp_read(sd, 0xb5) & 0xd0) != 0xd0 ||
+			!(cp_read(sd, 0xb1) & 0x80))
+		/* TODO channel 2 */
+		*status |= V4L2_IN_ST_NO_SIGNAL;
+
+	if (is_digital_input(sd) && ((io_read(sd, 0x74) & 0x03) != 0x03))
+		*status |= V4L2_IN_ST_NO_SIGNAL;
+
+	v4l2_dbg(1, debug, sd, "%s: CP status = 0x%x\n",
+			__func__, *status);
+
+	return 0;
+}
+
+struct stdi_readback {
+	u16 bl, lcf, lcvs;
+	u8 hs_pol, vs_pol;
+	bool interlaced;
+};
+
+static int stdi2dv_timings(struct v4l2_subdev *sd,
+		struct stdi_readback *stdi,
+		struct v4l2_dv_timings *timings)
+{
+	struct adv7842_state *state = to_state(sd);
+	u32 hfreq = (ADV7842_fsc * 8) / stdi->bl;
+	u32 pix_clk;
+	int i;
+
+	for (i = 0; v4l2_dv_timings_presets[i].bt.width; i++) {
+		const struct v4l2_bt_timings *bt = &v4l2_dv_timings_presets[i].bt;
+
+		if (!v4l2_valid_dv_timings(&v4l2_dv_timings_presets[i],
+					   adv7842_get_dv_timings_cap(sd),
+					   adv7842_check_dv_timings, NULL))
+			continue;
+		if (vtotal(bt) != stdi->lcf + 1)
+			continue;
+		if (bt->vsync != stdi->lcvs)
+			continue;
+
+		pix_clk = hfreq * htotal(bt);
+
+		if ((pix_clk < bt->pixelclock + 1000000) &&
+		    (pix_clk > bt->pixelclock - 1000000)) {
+			*timings = v4l2_dv_timings_presets[i];
+			return 0;
+		}
+	}
+
+	if (v4l2_detect_cvt(stdi->lcf + 1, hfreq, stdi->lcvs, 0,
+			(stdi->hs_pol == '+' ? V4L2_DV_HSYNC_POS_POL : 0) |
+			(stdi->vs_pol == '+' ? V4L2_DV_VSYNC_POS_POL : 0),
+			false, timings))
+		return 0;
+	if (v4l2_detect_gtf(stdi->lcf + 1, hfreq, stdi->lcvs,
+			(stdi->hs_pol == '+' ? V4L2_DV_HSYNC_POS_POL : 0) |
+			(stdi->vs_pol == '+' ? V4L2_DV_VSYNC_POS_POL : 0),
+			false, state->aspect_ratio, timings))
+		return 0;
+
+	v4l2_dbg(2, debug, sd,
+		"%s: No format candidate found for lcvs = %d, lcf=%d, bl = %d, %chsync, %cvsync\n",
+		__func__, stdi->lcvs, stdi->lcf, stdi->bl,
+		stdi->hs_pol, stdi->vs_pol);
+	return -1;
+}
+
+static int read_stdi(struct v4l2_subdev *sd, struct stdi_readback *stdi)
+{
+	u32 status;
+
+	adv7842_g_input_status(sd, &status);
+	if (status & V4L2_IN_ST_NO_SIGNAL) {
+		v4l2_dbg(2, debug, sd, "%s: no signal\n", __func__);
+		return -ENOLINK;
+	}
+
+	stdi->bl = ((cp_read(sd, 0xb1) & 0x3f) << 8) | cp_read(sd, 0xb2);
+	stdi->lcf = ((cp_read(sd, 0xb3) & 0x7) << 8) | cp_read(sd, 0xb4);
+	stdi->lcvs = cp_read(sd, 0xb3) >> 3;
+
+	if ((cp_read(sd, 0xb5) & 0x80) && ((cp_read(sd, 0xb5) & 0x03) == 0x01)) {
+		stdi->hs_pol = ((cp_read(sd, 0xb5) & 0x10) ?
+			((cp_read(sd, 0xb5) & 0x08) ? '+' : '-') : 'x');
+		stdi->vs_pol = ((cp_read(sd, 0xb5) & 0x40) ?
+			((cp_read(sd, 0xb5) & 0x20) ? '+' : '-') : 'x');
+	} else {
+		stdi->hs_pol = 'x';
+		stdi->vs_pol = 'x';
+	}
+	stdi->interlaced = (cp_read(sd, 0xb1) & 0x40) ? true : false;
+
+	if (stdi->lcf < 239 || stdi->bl < 8 || stdi->bl == 0x3fff) {
+		v4l2_dbg(2, debug, sd, "%s: invalid signal\n", __func__);
+		return -ENOLINK;
+	}
+
+	v4l2_dbg(2, debug, sd,
+		"%s: lcf (frame height - 1) = %d, bl = %d, lcvs (vsync) = %d, %chsync, %cvsync, %s\n",
+		 __func__, stdi->lcf, stdi->bl, stdi->lcvs,
+		 stdi->hs_pol, stdi->vs_pol,
+		 stdi->interlaced ? "interlaced" : "progressive");
+
+	return 0;
+}
+
+static int adv7842_enum_dv_timings(struct v4l2_subdev *sd,
+				   struct v4l2_enum_dv_timings *timings)
+{
+	if (timings->pad != 0)
+		return -EINVAL;
+
+	return v4l2_enum_dv_timings_cap(timings,
+		adv7842_get_dv_timings_cap(sd), adv7842_check_dv_timings, NULL);
+}
+
+static int adv7842_dv_timings_cap(struct v4l2_subdev *sd,
+				  struct v4l2_dv_timings_cap *cap)
+{
+	if (cap->pad != 0)
+		return -EINVAL;
+
+	*cap = *adv7842_get_dv_timings_cap(sd);
+	return 0;
+}
+
+/* Fill the optional fields .standards and .flags in struct v4l2_dv_timings
+   if the format is listed in adv7842_timings[] */
+static void adv7842_fill_optional_dv_timings_fields(struct v4l2_subdev *sd,
+		struct v4l2_dv_timings *timings)
+{
+	v4l2_find_dv_timings_cap(timings, adv7842_get_dv_timings_cap(sd),
+			is_digital_input(sd) ? 250000 : 1000000,
+			adv7842_check_dv_timings, NULL);
+	timings->bt.flags |= V4L2_DV_FL_CAN_DETECT_REDUCED_FPS;
+}
+
+static int adv7842_query_dv_timings(struct v4l2_subdev *sd,
+				    struct v4l2_dv_timings *timings)
+{
+	struct adv7842_state *state = to_state(sd);
+	struct v4l2_bt_timings *bt = &timings->bt;
+	struct stdi_readback stdi = { 0 };
+
+	v4l2_dbg(1, debug, sd, "%s:\n", __func__);
+
+	memset(timings, 0, sizeof(struct v4l2_dv_timings));
+
+	/* SDP block */
+	if (state->mode == ADV7842_MODE_SDP)
+		return -ENODATA;
+
+	/* read STDI */
+	if (read_stdi(sd, &stdi)) {
+		state->restart_stdi_once = true;
+		v4l2_dbg(1, debug, sd, "%s: no valid signal\n", __func__);
+		return -ENOLINK;
+	}
+	bt->interlaced = stdi.interlaced ?
+		V4L2_DV_INTERLACED : V4L2_DV_PROGRESSIVE;
+	bt->standards = V4L2_DV_BT_STD_CEA861 | V4L2_DV_BT_STD_DMT |
+			V4L2_DV_BT_STD_GTF | V4L2_DV_BT_STD_CVT;
+
+	if (is_digital_input(sd)) {
+		u32 freq;
+
+		timings->type = V4L2_DV_BT_656_1120;
+
+		bt->width = (hdmi_read(sd, 0x07) & 0x0f) * 256 + hdmi_read(sd, 0x08);
+		bt->height = (hdmi_read(sd, 0x09) & 0x0f) * 256 + hdmi_read(sd, 0x0a);
+		freq = ((hdmi_read(sd, 0x51) << 1) + (hdmi_read(sd, 0x52) >> 7)) * 1000000;
+		freq += ((hdmi_read(sd, 0x52) & 0x7f) * 7813);
+		if (is_hdmi(sd)) {
+			/* adjust for deep color mode */
+			freq = freq * 8 / (((hdmi_read(sd, 0x0b) & 0xc0) >> 6) * 2 + 8);
+		}
+		bt->pixelclock = freq;
+		bt->hfrontporch = (hdmi_read(sd, 0x20) & 0x03) * 256 +
+			hdmi_read(sd, 0x21);
+		bt->hsync = (hdmi_read(sd, 0x22) & 0x03) * 256 +
+			hdmi_read(sd, 0x23);
+		bt->hbackporch = (hdmi_read(sd, 0x24) & 0x03) * 256 +
+			hdmi_read(sd, 0x25);
+		bt->vfrontporch = ((hdmi_read(sd, 0x2a) & 0x1f) * 256 +
+			hdmi_read(sd, 0x2b)) / 2;
+		bt->vsync = ((hdmi_read(sd, 0x2e) & 0x1f) * 256 +
+			hdmi_read(sd, 0x2f)) / 2;
+		bt->vbackporch = ((hdmi_read(sd, 0x32) & 0x1f) * 256 +
+			hdmi_read(sd, 0x33)) / 2;
+		bt->polarities = ((hdmi_read(sd, 0x05) & 0x10) ? V4L2_DV_VSYNC_POS_POL : 0) |
+			((hdmi_read(sd, 0x05) & 0x20) ? V4L2_DV_HSYNC_POS_POL : 0);
+		if (bt->interlaced == V4L2_DV_INTERLACED) {
+			bt->height += (hdmi_read(sd, 0x0b) & 0x0f) * 256 +
+					hdmi_read(sd, 0x0c);
+			bt->il_vfrontporch = ((hdmi_read(sd, 0x2c) & 0x1f) * 256 +
+					hdmi_read(sd, 0x2d)) / 2;
+			bt->il_vsync = ((hdmi_read(sd, 0x30) & 0x1f) * 256 +
+					hdmi_read(sd, 0x31)) / 2;
+			bt->il_vbackporch = ((hdmi_read(sd, 0x34) & 0x1f) * 256 +
+					hdmi_read(sd, 0x35)) / 2;
+		} else {
+			bt->il_vfrontporch = 0;
+			bt->il_vsync = 0;
+			bt->il_vbackporch = 0;
+		}
+		adv7842_fill_optional_dv_timings_fields(sd, timings);
+		if ((timings->bt.flags & V4L2_DV_FL_CAN_REDUCE_FPS) &&
+		    freq < bt->pixelclock) {
+			u32 reduced_freq = ((u32)bt->pixelclock / 1001) * 1000;
+			u32 delta_freq = abs(freq - reduced_freq);
+
+			if (delta_freq < ((u32)bt->pixelclock - reduced_freq) / 2)
+				timings->bt.flags |= V4L2_DV_FL_REDUCED_FPS;
+		}
+	} else {
+		/* find format
+		 * Since LCVS values are inaccurate [REF_03, p. 339-340],
+		 * stdi2dv_timings() is called with lcvs +-1 if the first attempt fails.
+		 */
+		if (!stdi2dv_timings(sd, &stdi, timings))
+			goto found;
+		stdi.lcvs += 1;
+		v4l2_dbg(1, debug, sd, "%s: lcvs + 1 = %d\n", __func__, stdi.lcvs);
+		if (!stdi2dv_timings(sd, &stdi, timings))
+			goto found;
+		stdi.lcvs -= 2;
+		v4l2_dbg(1, debug, sd, "%s: lcvs - 1 = %d\n", __func__, stdi.lcvs);
+		if (stdi2dv_timings(sd, &stdi, timings)) {
+			/*
+			 * The STDI block may measure wrong values, especially
+			 * for lcvs and lcf. If the driver can not find any
+			 * valid timing, the STDI block is restarted to measure
+			 * the video timings again. The function will return an
+			 * error, but the restart of STDI will generate a new
+			 * STDI interrupt and the format detection process will
+			 * restart.
+			 */
+			if (state->restart_stdi_once) {
+				v4l2_dbg(1, debug, sd, "%s: restart STDI\n", __func__);
+				/* TODO restart STDI for Sync Channel 2 */
+				/* enter one-shot mode */
+				cp_write_and_or(sd, 0x86, 0xf9, 0x00);
+				/* trigger STDI restart */
+				cp_write_and_or(sd, 0x86, 0xf9, 0x04);
+				/* reset to continuous mode */
+				cp_write_and_or(sd, 0x86, 0xf9, 0x02);
+				state->restart_stdi_once = false;
+				return -ENOLINK;
+			}
+			v4l2_dbg(1, debug, sd, "%s: format not supported\n", __func__);
+			return -ERANGE;
+		}
+		state->restart_stdi_once = true;
+	}
+found:
+
+	if (debug > 1)
+		v4l2_print_dv_timings(sd->name, "adv7842_query_dv_timings:",
+				timings, true);
+	return 0;
+}
+
+static int adv7842_s_dv_timings(struct v4l2_subdev *sd,
+				struct v4l2_dv_timings *timings)
+{
+	struct adv7842_state *state = to_state(sd);
+	struct v4l2_bt_timings *bt;
+	int err;
+
+	v4l2_dbg(1, debug, sd, "%s:\n", __func__);
+
+	if (state->mode == ADV7842_MODE_SDP)
+		return -ENODATA;
+
+	if (v4l2_match_dv_timings(&state->timings, timings, 0, false)) {
+		v4l2_dbg(1, debug, sd, "%s: no change\n", __func__);
+		return 0;
+	}
+
+	bt = &timings->bt;
+
+	if (!v4l2_valid_dv_timings(timings, adv7842_get_dv_timings_cap(sd),
+				   adv7842_check_dv_timings, NULL))
+		return -ERANGE;
+
+	adv7842_fill_optional_dv_timings_fields(sd, timings);
+
+	state->timings = *timings;
+
+	cp_write(sd, 0x91, bt->interlaced ? 0x40 : 0x00);
+
+	/* Use prim_mode and vid_std when available */
+	err = configure_predefined_video_timings(sd, timings);
+	if (err) {
+		/* custom settings when the video format
+		  does not have prim_mode/vid_std */
+		configure_custom_video_timings(sd, bt);
+	}
+
+	set_rgb_quantization_range(sd);
+
+
+	if (debug > 1)
+		v4l2_print_dv_timings(sd->name, "adv7842_s_dv_timings: ",
+				      timings, true);
+	return 0;
+}
+
+static int adv7842_g_dv_timings(struct v4l2_subdev *sd,
+				struct v4l2_dv_timings *timings)
+{
+	struct adv7842_state *state = to_state(sd);
+
+	if (state->mode == ADV7842_MODE_SDP)
+		return -ENODATA;
+	*timings = state->timings;
+	return 0;
+}
+
+static void enable_input(struct v4l2_subdev *sd)
+{
+	struct adv7842_state *state = to_state(sd);
+
+	set_rgb_quantization_range(sd);
+	switch (state->mode) {
+	case ADV7842_MODE_SDP:
+	case ADV7842_MODE_COMP:
+	case ADV7842_MODE_RGB:
+		io_write(sd, 0x15, 0xb0);   /* Disable Tristate of Pins (no audio) */
+		break;
+	case ADV7842_MODE_HDMI:
+		hdmi_write(sd, 0x01, 0x00); /* Enable HDMI clock terminators */
+		io_write(sd, 0x15, 0xa0);   /* Disable Tristate of Pins */
+		hdmi_write_and_or(sd, 0x1a, 0xef, 0x00); /* Unmute audio */
+		break;
+	default:
+		v4l2_dbg(2, debug, sd, "%s: Unknown mode %d\n",
+			 __func__, state->mode);
+		break;
+	}
+}
+
+static void disable_input(struct v4l2_subdev *sd)
+{
+	hdmi_write_and_or(sd, 0x1a, 0xef, 0x10); /* Mute audio [REF_01, c. 2.2.2] */
+	msleep(16); /* 512 samples with >= 32 kHz sample rate [REF_03, c. 8.29] */
+	io_write(sd, 0x15, 0xbe);   /* Tristate all outputs from video core */
+	hdmi_write(sd, 0x01, 0x78); /* Disable HDMI clock terminators */
+}
+
+static void sdp_csc_coeff(struct v4l2_subdev *sd,
+			  const struct adv7842_sdp_csc_coeff *c)
+{
+	/* csc auto/manual */
+	sdp_io_write_and_or(sd, 0xe0, 0xbf, c->manual ? 0x00 : 0x40);
+
+	if (!c->manual)
+		return;
+
+	/* csc scaling */
+	sdp_io_write_and_or(sd, 0xe0, 0x7f, c->scaling == 2 ? 0x80 : 0x00);
+
+	/* A coeff */
+	sdp_io_write_and_or(sd, 0xe0, 0xe0, c->A1 >> 8);
+	sdp_io_write(sd, 0xe1, c->A1);
+	sdp_io_write_and_or(sd, 0xe2, 0xe0, c->A2 >> 8);
+	sdp_io_write(sd, 0xe3, c->A2);
+	sdp_io_write_and_or(sd, 0xe4, 0xe0, c->A3 >> 8);
+	sdp_io_write(sd, 0xe5, c->A3);
+
+	/* A scale */
+	sdp_io_write_and_or(sd, 0xe6, 0x80, c->A4 >> 8);
+	sdp_io_write(sd, 0xe7, c->A4);
+
+	/* B coeff */
+	sdp_io_write_and_or(sd, 0xe8, 0xe0, c->B1 >> 8);
+	sdp_io_write(sd, 0xe9, c->B1);
+	sdp_io_write_and_or(sd, 0xea, 0xe0, c->B2 >> 8);
+	sdp_io_write(sd, 0xeb, c->B2);
+	sdp_io_write_and_or(sd, 0xec, 0xe0, c->B3 >> 8);
+	sdp_io_write(sd, 0xed, c->B3);
+
+	/* B scale */
+	sdp_io_write_and_or(sd, 0xee, 0x80, c->B4 >> 8);
+	sdp_io_write(sd, 0xef, c->B4);
+
+	/* C coeff */
+	sdp_io_write_and_or(sd, 0xf0, 0xe0, c->C1 >> 8);
+	sdp_io_write(sd, 0xf1, c->C1);
+	sdp_io_write_and_or(sd, 0xf2, 0xe0, c->C2 >> 8);
+	sdp_io_write(sd, 0xf3, c->C2);
+	sdp_io_write_and_or(sd, 0xf4, 0xe0, c->C3 >> 8);
+	sdp_io_write(sd, 0xf5, c->C3);
+
+	/* C scale */
+	sdp_io_write_and_or(sd, 0xf6, 0x80, c->C4 >> 8);
+	sdp_io_write(sd, 0xf7, c->C4);
+}
+
+static void select_input(struct v4l2_subdev *sd,
+			 enum adv7842_vid_std_select vid_std_select)
+{
+	struct adv7842_state *state = to_state(sd);
+
+	switch (state->mode) {
+	case ADV7842_MODE_SDP:
+		io_write(sd, 0x00, vid_std_select); /* video std: CVBS or YC mode */
+		io_write(sd, 0x01, 0); /* prim mode */
+		/* enable embedded syncs for auto graphics mode */
+		cp_write_and_or(sd, 0x81, 0xef, 0x10);
+
+		afe_write(sd, 0x00, 0x00); /* power up ADC */
+		afe_write(sd, 0xc8, 0x00); /* phase control */
+
+		io_write(sd, 0xdd, 0x90); /* Manual 2x output clock */
+		/* script says register 0xde, which don't exist in manual */
+
+		/* Manual analog input muxing mode, CVBS (6.4)*/
+		afe_write_and_or(sd, 0x02, 0x7f, 0x80);
+		if (vid_std_select == ADV7842_SDP_VID_STD_CVBS_SD_4x1) {
+			afe_write(sd, 0x03, 0xa0); /* ADC0 to AIN10 (CVBS), ADC1 N/C*/
+			afe_write(sd, 0x04, 0x00); /* ADC2 N/C,ADC3 N/C*/
+		} else {
+			afe_write(sd, 0x03, 0xa0); /* ADC0 to AIN10 (CVBS), ADC1 N/C*/
+			afe_write(sd, 0x04, 0xc0); /* ADC2 to AIN12, ADC3 N/C*/
+		}
+		afe_write(sd, 0x0c, 0x1f); /* ADI recommend write */
+		afe_write(sd, 0x12, 0x63); /* ADI recommend write */
+
+		sdp_io_write(sd, 0xb2, 0x60); /* Disable AV codes */
+		sdp_io_write(sd, 0xc8, 0xe3); /* Disable Ancillary data */
+
+		/* SDP recommended settings */
+		sdp_write(sd, 0x00, 0x3F); /* Autodetect PAL NTSC (not SECAM) */
+		sdp_write(sd, 0x01, 0x00); /* Pedestal Off */
+
+		sdp_write(sd, 0x03, 0xE4); /* Manual VCR Gain Luma 0x40B */
+		sdp_write(sd, 0x04, 0x0B); /* Manual Luma setting */
+		sdp_write(sd, 0x05, 0xC3); /* Manual Chroma setting 0x3FE */
+		sdp_write(sd, 0x06, 0xFE); /* Manual Chroma setting */
+		sdp_write(sd, 0x12, 0x0D); /* Frame TBC,I_P, 3D comb enabled */
+		sdp_write(sd, 0xA7, 0x00); /* ADI Recommended Write */
+		sdp_io_write(sd, 0xB0, 0x00); /* Disable H and v blanking */
+
+		/* deinterlacer enabled and 3D comb */
+		sdp_write_and_or(sd, 0x12, 0xf6, 0x09);
+
+		break;
+
+	case ADV7842_MODE_COMP:
+	case ADV7842_MODE_RGB:
+		/* Automatic analog input muxing mode */
+		afe_write_and_or(sd, 0x02, 0x7f, 0x00);
+		/* set mode and select free run resolution */
+		io_write(sd, 0x00, vid_std_select); /* video std */
+		io_write(sd, 0x01, 0x02); /* prim mode */
+		cp_write_and_or(sd, 0x81, 0xef, 0x10); /* enable embedded syncs
+							  for auto graphics mode */
+
+		afe_write(sd, 0x00, 0x00); /* power up ADC */
+		afe_write(sd, 0xc8, 0x00); /* phase control */
+		if (state->mode == ADV7842_MODE_COMP) {
+			/* force to YCrCb */
+			io_write_and_or(sd, 0x02, 0x0f, 0x60);
+		} else {
+			/* force to RGB */
+			io_write_and_or(sd, 0x02, 0x0f, 0x10);
+		}
+
+		/* set ADI recommended settings for digitizer */
+		/* "ADV7842 Register Settings Recommendations
+		 * (rev. 1.8, November 2010)" p. 9. */
+		afe_write(sd, 0x0c, 0x1f); /* ADC Range improvement */
+		afe_write(sd, 0x12, 0x63); /* ADC Range improvement */
+
+		/* set to default gain for RGB */
+		cp_write(sd, 0x73, 0x10);
+		cp_write(sd, 0x74, 0x04);
+		cp_write(sd, 0x75, 0x01);
+		cp_write(sd, 0x76, 0x00);
+
+		cp_write(sd, 0x3e, 0x04); /* CP core pre-gain control */
+		cp_write(sd, 0xc3, 0x39); /* CP coast control. Graphics mode */
+		cp_write(sd, 0x40, 0x5c); /* CP core pre-gain control. Graphics mode */
+		break;
+
+	case ADV7842_MODE_HDMI:
+		/* Automatic analog input muxing mode */
+		afe_write_and_or(sd, 0x02, 0x7f, 0x00);
+		/* set mode and select free run resolution */
+		if (state->hdmi_port_a)
+			hdmi_write(sd, 0x00, 0x02); /* select port A */
+		else
+			hdmi_write(sd, 0x00, 0x03); /* select port B */
+		io_write(sd, 0x00, vid_std_select); /* video std */
+		io_write(sd, 0x01, 5); /* prim mode */
+		cp_write_and_or(sd, 0x81, 0xef, 0x00); /* disable embedded syncs
+							  for auto graphics mode */
+
+		/* set ADI recommended settings for HDMI: */
+		/* "ADV7842 Register Settings Recommendations
+		 * (rev. 1.8, November 2010)" p. 3. */
+		hdmi_write(sd, 0xc0, 0x00);
+		hdmi_write(sd, 0x0d, 0x34); /* ADI recommended write */
+		hdmi_write(sd, 0x3d, 0x10); /* ADI recommended write */
+		hdmi_write(sd, 0x44, 0x85); /* TMDS PLL optimization */
+		hdmi_write(sd, 0x46, 0x1f); /* ADI recommended write */
+		hdmi_write(sd, 0x57, 0xb6); /* TMDS PLL optimization */
+		hdmi_write(sd, 0x58, 0x03); /* TMDS PLL optimization */
+		hdmi_write(sd, 0x60, 0x88); /* TMDS PLL optimization */
+		hdmi_write(sd, 0x61, 0x88); /* TMDS PLL optimization */
+		hdmi_write(sd, 0x6c, 0x18); /* Disable ISRC clearing bit,
+					       Improve robustness */
+		hdmi_write(sd, 0x75, 0x10); /* DDC drive strength */
+		hdmi_write(sd, 0x85, 0x1f); /* equaliser */
+		hdmi_write(sd, 0x87, 0x70); /* ADI recommended write */
+		hdmi_write(sd, 0x89, 0x04); /* equaliser */
+		hdmi_write(sd, 0x8a, 0x1e); /* equaliser */
+		hdmi_write(sd, 0x93, 0x04); /* equaliser */
+		hdmi_write(sd, 0x94, 0x1e); /* equaliser */
+		hdmi_write(sd, 0x99, 0xa1); /* ADI recommended write */
+		hdmi_write(sd, 0x9b, 0x09); /* ADI recommended write */
+		hdmi_write(sd, 0x9d, 0x02); /* equaliser */
+
+		afe_write(sd, 0x00, 0xff); /* power down ADC */
+		afe_write(sd, 0xc8, 0x40); /* phase control */
+
+		/* set to default gain for HDMI */
+		cp_write(sd, 0x73, 0x10);
+		cp_write(sd, 0x74, 0x04);
+		cp_write(sd, 0x75, 0x01);
+		cp_write(sd, 0x76, 0x00);
+
+		/* reset ADI recommended settings for digitizer */
+		/* "ADV7842 Register Settings Recommendations
+		 * (rev. 2.5, June 2010)" p. 17. */
+		afe_write(sd, 0x12, 0xfb); /* ADC noise shaping filter controls */
+		afe_write(sd, 0x0c, 0x0d); /* CP core gain controls */
+		cp_write(sd, 0x3e, 0x00); /* CP core pre-gain control */
+
+		/* CP coast control */
+		cp_write(sd, 0xc3, 0x33); /* Component mode */
+
+		/* color space conversion, autodetect color space */
+		io_write_and_or(sd, 0x02, 0x0f, 0xf0);
+		break;
+
+	default:
+		v4l2_dbg(2, debug, sd, "%s: Unknown mode %d\n",
+			 __func__, state->mode);
+		break;
+	}
+}
+
+static int adv7842_s_routing(struct v4l2_subdev *sd,
+		u32 input, u32 output, u32 config)
+{
+	struct adv7842_state *state = to_state(sd);
+
+	v4l2_dbg(2, debug, sd, "%s: input %d\n", __func__, input);
+
+	switch (input) {
+	case ADV7842_SELECT_HDMI_PORT_A:
+		state->mode = ADV7842_MODE_HDMI;
+		state->vid_std_select = ADV7842_HDMI_COMP_VID_STD_HD_1250P;
+		state->hdmi_port_a = true;
+		break;
+	case ADV7842_SELECT_HDMI_PORT_B:
+		state->mode = ADV7842_MODE_HDMI;
+		state->vid_std_select = ADV7842_HDMI_COMP_VID_STD_HD_1250P;
+		state->hdmi_port_a = false;
+		break;
+	case ADV7842_SELECT_VGA_COMP:
+		state->mode = ADV7842_MODE_COMP;
+		state->vid_std_select = ADV7842_RGB_VID_STD_AUTO_GRAPH_MODE;
+		break;
+	case ADV7842_SELECT_VGA_RGB:
+		state->mode = ADV7842_MODE_RGB;
+		state->vid_std_select = ADV7842_RGB_VID_STD_AUTO_GRAPH_MODE;
+		break;
+	case ADV7842_SELECT_SDP_CVBS:
+		state->mode = ADV7842_MODE_SDP;
+		state->vid_std_select = ADV7842_SDP_VID_STD_CVBS_SD_4x1;
+		break;
+	case ADV7842_SELECT_SDP_YC:
+		state->mode = ADV7842_MODE_SDP;
+		state->vid_std_select = ADV7842_SDP_VID_STD_YC_SD4_x1;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	disable_input(sd);
+	select_input(sd, state->vid_std_select);
+	enable_input(sd);
+
+	v4l2_subdev_notify_event(sd, &adv7842_ev_fmt);
+
+	return 0;
+}
+
+static int adv7842_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index >= ARRAY_SIZE(adv7842_formats))
+		return -EINVAL;
+	code->code = adv7842_formats[code->index].code;
+	return 0;
+}
+
+static void adv7842_fill_format(struct adv7842_state *state,
+				struct v4l2_mbus_framefmt *format)
+{
+	memset(format, 0, sizeof(*format));
+
+	format->width = state->timings.bt.width;
+	format->height = state->timings.bt.height;
+	format->field = V4L2_FIELD_NONE;
+	format->colorspace = V4L2_COLORSPACE_SRGB;
+
+	if (state->timings.bt.flags & V4L2_DV_FL_IS_CE_VIDEO)
+		format->colorspace = (state->timings.bt.height <= 576) ?
+			V4L2_COLORSPACE_SMPTE170M : V4L2_COLORSPACE_REC709;
+}
+
+/*
+ * Compute the op_ch_sel value required to obtain on the bus the component order
+ * corresponding to the selected format taking into account bus reordering
+ * applied by the board at the output of the device.
+ *
+ * The following table gives the op_ch_value from the format component order
+ * (expressed as op_ch_sel value in column) and the bus reordering (expressed as
+ * adv7842_bus_order value in row).
+ *
+ *           |	GBR(0)	GRB(1)	BGR(2)	RGB(3)	BRG(4)	RBG(5)
+ * ----------+-------------------------------------------------
+ * RGB (NOP) |	GBR	GRB	BGR	RGB	BRG	RBG
+ * GRB (1-2) |	BGR	RGB	GBR	GRB	RBG	BRG
+ * RBG (2-3) |	GRB	GBR	BRG	RBG	BGR	RGB
+ * BGR (1-3) |	RBG	BRG	RGB	BGR	GRB	GBR
+ * BRG (ROR) |	BRG	RBG	GRB	GBR	RGB	BGR
+ * GBR (ROL) |	RGB	BGR	RBG	BRG	GBR	GRB
+ */
+static unsigned int adv7842_op_ch_sel(struct adv7842_state *state)
+{
+#define _SEL(a, b, c, d, e, f)	{ \
+	ADV7842_OP_CH_SEL_##a, ADV7842_OP_CH_SEL_##b, ADV7842_OP_CH_SEL_##c, \
+	ADV7842_OP_CH_SEL_##d, ADV7842_OP_CH_SEL_##e, ADV7842_OP_CH_SEL_##f }
+#define _BUS(x)			[ADV7842_BUS_ORDER_##x]
+
+	static const unsigned int op_ch_sel[6][6] = {
+		_BUS(RGB) /* NOP */ = _SEL(GBR, GRB, BGR, RGB, BRG, RBG),
+		_BUS(GRB) /* 1-2 */ = _SEL(BGR, RGB, GBR, GRB, RBG, BRG),
+		_BUS(RBG) /* 2-3 */ = _SEL(GRB, GBR, BRG, RBG, BGR, RGB),
+		_BUS(BGR) /* 1-3 */ = _SEL(RBG, BRG, RGB, BGR, GRB, GBR),
+		_BUS(BRG) /* ROR */ = _SEL(BRG, RBG, GRB, GBR, RGB, BGR),
+		_BUS(GBR) /* ROL */ = _SEL(RGB, BGR, RBG, BRG, GBR, GRB),
+	};
+
+	return op_ch_sel[state->pdata.bus_order][state->format->op_ch_sel >> 5];
+}
+
+static void adv7842_setup_format(struct adv7842_state *state)
+{
+	struct v4l2_subdev *sd = &state->sd;
+
+	io_write_clr_set(sd, 0x02, 0x02,
+			state->format->rgb_out ? ADV7842_RGB_OUT : 0);
+	io_write(sd, 0x03, state->format->op_format_sel |
+		 state->pdata.op_format_mode_sel);
+	io_write_clr_set(sd, 0x04, 0xe0, adv7842_op_ch_sel(state));
+	io_write_clr_set(sd, 0x05, 0x01,
+			state->format->swap_cb_cr ? ADV7842_OP_SWAP_CB_CR : 0);
+	set_rgb_quantization_range(sd);
+}
+
+static int adv7842_get_format(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_format *format)
+{
+	struct adv7842_state *state = to_state(sd);
+
+	if (format->pad != ADV7842_PAD_SOURCE)
+		return -EINVAL;
+
+	if (state->mode == ADV7842_MODE_SDP) {
+		/* SPD block */
+		if (!(sdp_read(sd, 0x5a) & 0x01))
+			return -EINVAL;
+		format->format.code = MEDIA_BUS_FMT_YUYV8_2X8;
+		format->format.width = 720;
+		/* valid signal */
+		if (state->norm & V4L2_STD_525_60)
+			format->format.height = 480;
+		else
+			format->format.height = 576;
+		format->format.colorspace = V4L2_COLORSPACE_SMPTE170M;
+		return 0;
+	}
+
+	adv7842_fill_format(state, &format->format);
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		struct v4l2_mbus_framefmt *fmt;
+
+		fmt = v4l2_subdev_get_try_format(sd, sd_state, format->pad);
+		format->format.code = fmt->code;
+	} else {
+		format->format.code = state->format->code;
+	}
+
+	return 0;
+}
+
+static int adv7842_set_format(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_format *format)
+{
+	struct adv7842_state *state = to_state(sd);
+	const struct adv7842_format_info *info;
+
+	if (format->pad != ADV7842_PAD_SOURCE)
+		return -EINVAL;
+
+	if (state->mode == ADV7842_MODE_SDP)
+		return adv7842_get_format(sd, sd_state, format);
+
+	info = adv7842_format_info(state, format->format.code);
+	if (info == NULL)
+		info = adv7842_format_info(state, MEDIA_BUS_FMT_YUYV8_2X8);
+
+	adv7842_fill_format(state, &format->format);
+	format->format.code = info->code;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		struct v4l2_mbus_framefmt *fmt;
+
+		fmt = v4l2_subdev_get_try_format(sd, sd_state, format->pad);
+		fmt->code = format->format.code;
+	} else {
+		state->format = info;
+		adv7842_setup_format(state);
+	}
+
+	return 0;
+}
+
+static void adv7842_irq_enable(struct v4l2_subdev *sd, bool enable)
+{
+	if (enable) {
+		/* Enable SSPD, STDI and CP locked/unlocked interrupts */
+		io_write(sd, 0x46, 0x9c);
+		/* ESDP_50HZ_DET interrupt */
+		io_write(sd, 0x5a, 0x10);
+		/* Enable CABLE_DET_A/B_ST (+5v) interrupt */
+		io_write(sd, 0x73, 0x03);
+		/* Enable V_LOCKED and DE_REGEN_LCK interrupts */
+		io_write(sd, 0x78, 0x03);
+		/* Enable SDP Standard Detection Change and SDP Video Detected */
+		io_write(sd, 0xa0, 0x09);
+		/* Enable HDMI_MODE interrupt */
+		io_write(sd, 0x69, 0x08);
+	} else {
+		io_write(sd, 0x46, 0x0);
+		io_write(sd, 0x5a, 0x0);
+		io_write(sd, 0x73, 0x0);
+		io_write(sd, 0x78, 0x0);
+		io_write(sd, 0xa0, 0x0);
+		io_write(sd, 0x69, 0x0);
+	}
+}
+
+#if IS_ENABLED(CONFIG_VIDEO_ADV7842_CEC)
+static void adv7842_cec_tx_raw_status(struct v4l2_subdev *sd, u8 tx_raw_status)
+{
+	struct adv7842_state *state = to_state(sd);
+
+	if ((cec_read(sd, 0x11) & 0x01) == 0) {
+		v4l2_dbg(1, debug, sd, "%s: tx raw: tx disabled\n", __func__);
+		return;
+	}
+
+	if (tx_raw_status & 0x02) {
+		v4l2_dbg(1, debug, sd, "%s: tx raw: arbitration lost\n",
+			 __func__);
+		cec_transmit_done(state->cec_adap, CEC_TX_STATUS_ARB_LOST,
+				  1, 0, 0, 0);
+		return;
+	}
+	if (tx_raw_status & 0x04) {
+		u8 status;
+		u8 nack_cnt;
+		u8 low_drive_cnt;
+
+		v4l2_dbg(1, debug, sd, "%s: tx raw: retry failed\n", __func__);
+		/*
+		 * We set this status bit since this hardware performs
+		 * retransmissions.
+		 */
+		status = CEC_TX_STATUS_MAX_RETRIES;
+		nack_cnt = cec_read(sd, 0x14) & 0xf;
+		if (nack_cnt)
+			status |= CEC_TX_STATUS_NACK;
+		low_drive_cnt = cec_read(sd, 0x14) >> 4;
+		if (low_drive_cnt)
+			status |= CEC_TX_STATUS_LOW_DRIVE;
+		cec_transmit_done(state->cec_adap, status,
+				  0, nack_cnt, low_drive_cnt, 0);
+		return;
+	}
+	if (tx_raw_status & 0x01) {
+		v4l2_dbg(1, debug, sd, "%s: tx raw: ready ok\n", __func__);
+		cec_transmit_done(state->cec_adap, CEC_TX_STATUS_OK, 0, 0, 0, 0);
+		return;
+	}
+}
+
+static void adv7842_cec_isr(struct v4l2_subdev *sd, bool *handled)
+{
+	u8 cec_irq;
+
+	/* cec controller */
+	cec_irq = io_read(sd, 0x93) & 0x0f;
+	if (!cec_irq)
+		return;
+
+	v4l2_dbg(1, debug, sd, "%s: cec: irq 0x%x\n", __func__, cec_irq);
+	adv7842_cec_tx_raw_status(sd, cec_irq);
+	if (cec_irq & 0x08) {
+		struct adv7842_state *state = to_state(sd);
+		struct cec_msg msg;
+
+		msg.len = cec_read(sd, 0x25) & 0x1f;
+		if (msg.len > CEC_MAX_MSG_SIZE)
+			msg.len = CEC_MAX_MSG_SIZE;
+
+		if (msg.len) {
+			u8 i;
+
+			for (i = 0; i < msg.len; i++)
+				msg.msg[i] = cec_read(sd, i + 0x15);
+			cec_write(sd, 0x26, 0x01); /* re-enable rx */
+			cec_received_msg(state->cec_adap, &msg);
+		}
+	}
+
+	io_write(sd, 0x94, cec_irq);
+
+	if (handled)
+		*handled = true;
+}
+
+static int adv7842_cec_adap_enable(struct cec_adapter *adap, bool enable)
+{
+	struct adv7842_state *state = cec_get_drvdata(adap);
+	struct v4l2_subdev *sd = &state->sd;
+
+	if (!state->cec_enabled_adap && enable) {
+		cec_write_clr_set(sd, 0x2a, 0x01, 0x01); /* power up cec */
+		cec_write(sd, 0x2c, 0x01);	/* cec soft reset */
+		cec_write_clr_set(sd, 0x11, 0x01, 0); /* initially disable tx */
+		/* enabled irqs: */
+		/* tx: ready */
+		/* tx: arbitration lost */
+		/* tx: retry timeout */
+		/* rx: ready */
+		io_write_clr_set(sd, 0x96, 0x0f, 0x0f);
+		cec_write(sd, 0x26, 0x01);            /* enable rx */
+	} else if (state->cec_enabled_adap && !enable) {
+		/* disable cec interrupts */
+		io_write_clr_set(sd, 0x96, 0x0f, 0x00);
+		/* disable address mask 1-3 */
+		cec_write_clr_set(sd, 0x27, 0x70, 0x00);
+		/* power down cec section */
+		cec_write_clr_set(sd, 0x2a, 0x01, 0x00);
+		state->cec_valid_addrs = 0;
+	}
+	state->cec_enabled_adap = enable;
+	return 0;
+}
+
+static int adv7842_cec_adap_log_addr(struct cec_adapter *adap, u8 addr)
+{
+	struct adv7842_state *state = cec_get_drvdata(adap);
+	struct v4l2_subdev *sd = &state->sd;
+	unsigned int i, free_idx = ADV7842_MAX_ADDRS;
+
+	if (!state->cec_enabled_adap)
+		return addr == CEC_LOG_ADDR_INVALID ? 0 : -EIO;
+
+	if (addr == CEC_LOG_ADDR_INVALID) {
+		cec_write_clr_set(sd, 0x27, 0x70, 0);
+		state->cec_valid_addrs = 0;
+		return 0;
+	}
+
+	for (i = 0; i < ADV7842_MAX_ADDRS; i++) {
+		bool is_valid = state->cec_valid_addrs & (1 << i);
+
+		if (free_idx == ADV7842_MAX_ADDRS && !is_valid)
+			free_idx = i;
+		if (is_valid && state->cec_addr[i] == addr)
+			return 0;
+	}
+	if (i == ADV7842_MAX_ADDRS) {
+		i = free_idx;
+		if (i == ADV7842_MAX_ADDRS)
+			return -ENXIO;
+	}
+	state->cec_addr[i] = addr;
+	state->cec_valid_addrs |= 1 << i;
+
+	switch (i) {
+	case 0:
+		/* enable address mask 0 */
+		cec_write_clr_set(sd, 0x27, 0x10, 0x10);
+		/* set address for mask 0 */
+		cec_write_clr_set(sd, 0x28, 0x0f, addr);
+		break;
+	case 1:
+		/* enable address mask 1 */
+		cec_write_clr_set(sd, 0x27, 0x20, 0x20);
+		/* set address for mask 1 */
+		cec_write_clr_set(sd, 0x28, 0xf0, addr << 4);
+		break;
+	case 2:
+		/* enable address mask 2 */
+		cec_write_clr_set(sd, 0x27, 0x40, 0x40);
+		/* set address for mask 1 */
+		cec_write_clr_set(sd, 0x29, 0x0f, addr);
+		break;
+	}
+	return 0;
+}
+
+static int adv7842_cec_adap_transmit(struct cec_adapter *adap, u8 attempts,
+				     u32 signal_free_time, struct cec_msg *msg)
+{
+	struct adv7842_state *state = cec_get_drvdata(adap);
+	struct v4l2_subdev *sd = &state->sd;
+	u8 len = msg->len;
+	unsigned int i;
+
+	/*
+	 * The number of retries is the number of attempts - 1, but retry
+	 * at least once. It's not clear if a value of 0 is allowed, so
+	 * let's do at least one retry.
+	 */
+	cec_write_clr_set(sd, 0x12, 0x70, max(1, attempts - 1) << 4);
+
+	if (len > 16) {
+		v4l2_err(sd, "%s: len exceeded 16 (%d)\n", __func__, len);
+		return -EINVAL;
+	}
+
+	/* write data */
+	for (i = 0; i < len; i++)
+		cec_write(sd, i, msg->msg[i]);
+
+	/* set length (data + header) */
+	cec_write(sd, 0x10, len);
+	/* start transmit, enable tx */
+	cec_write(sd, 0x11, 0x01);
+	return 0;
+}
+
+static const struct cec_adap_ops adv7842_cec_adap_ops = {
+	.adap_enable = adv7842_cec_adap_enable,
+	.adap_log_addr = adv7842_cec_adap_log_addr,
+	.adap_transmit = adv7842_cec_adap_transmit,
+};
+#endif
+
+static int adv7842_isr(struct v4l2_subdev *sd, u32 status, bool *handled)
+{
+	struct adv7842_state *state = to_state(sd);
+	u8 fmt_change_cp, fmt_change_digital, fmt_change_sdp;
+	u8 irq_status[6];
+
+	adv7842_irq_enable(sd, false);
+
+	/* read status */
+	irq_status[0] = io_read(sd, 0x43);
+	irq_status[1] = io_read(sd, 0x57);
+	irq_status[2] = io_read(sd, 0x70);
+	irq_status[3] = io_read(sd, 0x75);
+	irq_status[4] = io_read(sd, 0x9d);
+	irq_status[5] = io_read(sd, 0x66);
+
+	/* and clear */
+	if (irq_status[0])
+		io_write(sd, 0x44, irq_status[0]);
+	if (irq_status[1])
+		io_write(sd, 0x58, irq_status[1]);
+	if (irq_status[2])
+		io_write(sd, 0x71, irq_status[2]);
+	if (irq_status[3])
+		io_write(sd, 0x76, irq_status[3]);
+	if (irq_status[4])
+		io_write(sd, 0x9e, irq_status[4]);
+	if (irq_status[5])
+		io_write(sd, 0x67, irq_status[5]);
+
+	adv7842_irq_enable(sd, true);
+
+	v4l2_dbg(1, debug, sd, "%s: irq %x, %x, %x, %x, %x, %x\n", __func__,
+		 irq_status[0], irq_status[1], irq_status[2],
+		 irq_status[3], irq_status[4], irq_status[5]);
+
+	/* format change CP */
+	fmt_change_cp = irq_status[0] & 0x9c;
+
+	/* format change SDP */
+	if (state->mode == ADV7842_MODE_SDP)
+		fmt_change_sdp = (irq_status[1] & 0x30) | (irq_status[4] & 0x09);
+	else
+		fmt_change_sdp = 0;
+
+	/* digital format CP */
+	if (is_digital_input(sd))
+		fmt_change_digital = irq_status[3] & 0x03;
+	else
+		fmt_change_digital = 0;
+
+	/* format change */
+	if (fmt_change_cp || fmt_change_digital || fmt_change_sdp) {
+		v4l2_dbg(1, debug, sd,
+			 "%s: fmt_change_cp = 0x%x, fmt_change_digital = 0x%x, fmt_change_sdp = 0x%x\n",
+			 __func__, fmt_change_cp, fmt_change_digital,
+			 fmt_change_sdp);
+		v4l2_subdev_notify_event(sd, &adv7842_ev_fmt);
+		if (handled)
+			*handled = true;
+	}
+
+	/* HDMI/DVI mode */
+	if (irq_status[5] & 0x08) {
+		v4l2_dbg(1, debug, sd, "%s: irq %s mode\n", __func__,
+			 (io_read(sd, 0x65) & 0x08) ? "HDMI" : "DVI");
+		set_rgb_quantization_range(sd);
+		if (handled)
+			*handled = true;
+	}
+
+#if IS_ENABLED(CONFIG_VIDEO_ADV7842_CEC)
+	/* cec */
+	adv7842_cec_isr(sd, handled);
+#endif
+
+	/* tx 5v detect */
+	if (irq_status[2] & 0x3) {
+		v4l2_dbg(1, debug, sd, "%s: irq tx_5v\n", __func__);
+		adv7842_s_detect_tx_5v_ctrl(sd);
+		if (handled)
+			*handled = true;
+	}
+	return 0;
+}
+
+static int adv7842_get_edid(struct v4l2_subdev *sd, struct v4l2_edid *edid)
+{
+	struct adv7842_state *state = to_state(sd);
+	u32 blocks = 0;
+	u8 *data = NULL;
+
+	memset(edid->reserved, 0, sizeof(edid->reserved));
+
+	switch (edid->pad) {
+	case ADV7842_EDID_PORT_A:
+	case ADV7842_EDID_PORT_B:
+		if (state->hdmi_edid.present & (0x04 << edid->pad)) {
+			data = state->hdmi_edid.edid;
+			blocks = state->hdmi_edid.blocks;
+		}
+		break;
+	case ADV7842_EDID_PORT_VGA:
+		if (state->vga_edid.present) {
+			data = state->vga_edid.edid;
+			blocks = state->vga_edid.blocks;
+		}
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (edid->start_block == 0 && edid->blocks == 0) {
+		edid->blocks = blocks;
+		return 0;
+	}
+
+	if (!data)
+		return -ENODATA;
+
+	if (edid->start_block >= blocks)
+		return -EINVAL;
+
+	if (edid->start_block + edid->blocks > blocks)
+		edid->blocks = blocks - edid->start_block;
+
+	memcpy(edid->edid, data + edid->start_block * 128, edid->blocks * 128);
+
+	return 0;
+}
+
+/*
+ * If the VGA_EDID_ENABLE bit is set (Repeater Map 0x7f, bit 7), then
+ * the first two blocks of the EDID are for the HDMI, and the first block
+ * of segment 1 (i.e. the third block of the EDID) is for VGA.
+ * So if a VGA EDID is installed, then the maximum size of the HDMI EDID
+ * is 2 blocks.
+ */
+static int adv7842_set_edid(struct v4l2_subdev *sd, struct v4l2_edid *e)
+{
+	struct adv7842_state *state = to_state(sd);
+	unsigned int max_blocks = e->pad == ADV7842_EDID_PORT_VGA ? 1 : 4;
+	int err = 0;
+
+	memset(e->reserved, 0, sizeof(e->reserved));
+
+	if (e->pad > ADV7842_EDID_PORT_VGA)
+		return -EINVAL;
+	if (e->start_block != 0)
+		return -EINVAL;
+	if (e->pad < ADV7842_EDID_PORT_VGA && state->vga_edid.blocks)
+		max_blocks = 2;
+	if (e->pad == ADV7842_EDID_PORT_VGA && state->hdmi_edid.blocks > 2)
+		return -EBUSY;
+	if (e->blocks > max_blocks) {
+		e->blocks = max_blocks;
+		return -E2BIG;
+	}
+
+	/* todo, per edid */
+	if (e->blocks)
+		state->aspect_ratio = v4l2_calc_aspect_ratio(e->edid[0x15],
+							     e->edid[0x16]);
+
+	switch (e->pad) {
+	case ADV7842_EDID_PORT_VGA:
+		memset(state->vga_edid.edid, 0, sizeof(state->vga_edid.edid));
+		state->vga_edid.blocks = e->blocks;
+		state->vga_edid.present = e->blocks ? 0x1 : 0x0;
+		if (e->blocks)
+			memcpy(state->vga_edid.edid, e->edid, 128);
+		err = edid_write_vga_segment(sd);
+		break;
+	case ADV7842_EDID_PORT_A:
+	case ADV7842_EDID_PORT_B:
+		memset(state->hdmi_edid.edid, 0, sizeof(state->hdmi_edid.edid));
+		state->hdmi_edid.blocks = e->blocks;
+		if (e->blocks) {
+			state->hdmi_edid.present |= 0x04 << e->pad;
+			memcpy(state->hdmi_edid.edid, e->edid, 128 * e->blocks);
+		} else {
+			state->hdmi_edid.present &= ~(0x04 << e->pad);
+			adv7842_s_detect_tx_5v_ctrl(sd);
+		}
+		err = edid_write_hdmi_segment(sd, e->pad);
+		break;
+	default:
+		return -EINVAL;
+	}
+	if (err < 0)
+		v4l2_err(sd, "error %d writing edid on port %d\n", err, e->pad);
+	return err;
+}
+
+struct adv7842_cfg_read_infoframe {
+	const char *desc;
+	u8 present_mask;
+	u8 head_addr;
+	u8 payload_addr;
+};
+
+static void log_infoframe(struct v4l2_subdev *sd, const struct adv7842_cfg_read_infoframe *cri)
+{
+	int i;
+	u8 buffer[32];
+	union hdmi_infoframe frame;
+	u8 len;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct device *dev = &client->dev;
+
+	if (!(io_read(sd, 0x60) & cri->present_mask)) {
+		v4l2_info(sd, "%s infoframe not received\n", cri->desc);
+		return;
+	}
+
+	for (i = 0; i < 3; i++)
+		buffer[i] = infoframe_read(sd, cri->head_addr + i);
+
+	len = buffer[2] + 1;
+
+	if (len + 3 > sizeof(buffer)) {
+		v4l2_err(sd, "%s: invalid %s infoframe length %d\n", __func__, cri->desc, len);
+		return;
+	}
+
+	for (i = 0; i < len; i++)
+		buffer[i + 3] = infoframe_read(sd, cri->payload_addr + i);
+
+	if (hdmi_infoframe_unpack(&frame, buffer, len + 3) < 0) {
+		v4l2_err(sd, "%s: unpack of %s infoframe failed\n", __func__, cri->desc);
+		return;
+	}
+
+	hdmi_infoframe_log(KERN_INFO, dev, &frame);
+}
+
+static void adv7842_log_infoframes(struct v4l2_subdev *sd)
+{
+	int i;
+	static const struct adv7842_cfg_read_infoframe cri[] = {
+		{ "AVI", 0x01, 0xe0, 0x00 },
+		{ "Audio", 0x02, 0xe3, 0x1c },
+		{ "SDP", 0x04, 0xe6, 0x2a },
+		{ "Vendor", 0x10, 0xec, 0x54 }
+	};
+
+	if (!(hdmi_read(sd, 0x05) & 0x80)) {
+		v4l2_info(sd, "receive DVI-D signal, no infoframes\n");
+		return;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(cri); i++)
+		log_infoframe(sd, &cri[i]);
+}
+
+#if 0
+/* Let's keep it here for now, as it could be useful for debug */
+static const char * const prim_mode_txt[] = {
+	"SDP",
+	"Component",
+	"Graphics",
+	"Reserved",
+	"CVBS & HDMI AUDIO",
+	"HDMI-Comp",
+	"HDMI-GR",
+	"Reserved",
+	"Reserved",
+	"Reserved",
+	"Reserved",
+	"Reserved",
+	"Reserved",
+	"Reserved",
+	"Reserved",
+	"Reserved",
+};
+#endif
+
+static int adv7842_sdp_log_status(struct v4l2_subdev *sd)
+{
+	/* SDP (Standard definition processor) block */
+	u8 sdp_signal_detected = sdp_read(sd, 0x5A) & 0x01;
+
+	v4l2_info(sd, "Chip powered %s\n", no_power(sd) ? "off" : "on");
+	v4l2_info(sd, "Prim-mode = 0x%x, video std = 0x%x\n",
+		  io_read(sd, 0x01) & 0x0f, io_read(sd, 0x00) & 0x3f);
+
+	v4l2_info(sd, "SDP: free run: %s\n",
+		(sdp_read(sd, 0x56) & 0x01) ? "on" : "off");
+	v4l2_info(sd, "SDP: %s\n", sdp_signal_detected ?
+		"valid SD/PR signal detected" : "invalid/no signal");
+	if (sdp_signal_detected) {
+		static const char * const sdp_std_txt[] = {
+			"NTSC-M/J",
+			"1?",
+			"NTSC-443",
+			"60HzSECAM",
+			"PAL-M",
+			"5?",
+			"PAL-60",
+			"7?", "8?", "9?", "a?", "b?",
+			"PAL-CombN",
+			"d?",
+			"PAL-BGHID",
+			"SECAM"
+		};
+		v4l2_info(sd, "SDP: standard %s\n",
+			sdp_std_txt[sdp_read(sd, 0x52) & 0x0f]);
+		v4l2_info(sd, "SDP: %s\n",
+			(sdp_read(sd, 0x59) & 0x08) ? "50Hz" : "60Hz");
+		v4l2_info(sd, "SDP: %s\n",
+			(sdp_read(sd, 0x57) & 0x08) ? "Interlaced" : "Progressive");
+		v4l2_info(sd, "SDP: deinterlacer %s\n",
+			(sdp_read(sd, 0x12) & 0x08) ? "enabled" : "disabled");
+		v4l2_info(sd, "SDP: csc %s mode\n",
+			(sdp_io_read(sd, 0xe0) & 0x40) ? "auto" : "manual");
+	}
+	return 0;
+}
+
+static int adv7842_cp_log_status(struct v4l2_subdev *sd)
+{
+	/* CP block */
+	struct adv7842_state *state = to_state(sd);
+	struct v4l2_dv_timings timings;
+	u8 reg_io_0x02 = io_read(sd, 0x02);
+	u8 reg_io_0x21 = io_read(sd, 0x21);
+	u8 reg_rep_0x77 = rep_read(sd, 0x77);
+	u8 reg_rep_0x7d = rep_read(sd, 0x7d);
+	bool audio_pll_locked = hdmi_read(sd, 0x04) & 0x01;
+	bool audio_sample_packet_detect = hdmi_read(sd, 0x18) & 0x01;
+	bool audio_mute = io_read(sd, 0x65) & 0x40;
+
+	static const char * const csc_coeff_sel_rb[16] = {
+		"bypassed", "YPbPr601 -> RGB", "reserved", "YPbPr709 -> RGB",
+		"reserved", "RGB -> YPbPr601", "reserved", "RGB -> YPbPr709",
+		"reserved", "YPbPr709 -> YPbPr601", "YPbPr601 -> YPbPr709",
+		"reserved", "reserved", "reserved", "reserved", "manual"
+	};
+	static const char * const input_color_space_txt[16] = {
+		"RGB limited range (16-235)", "RGB full range (0-255)",
+		"YCbCr Bt.601 (16-235)", "YCbCr Bt.709 (16-235)",
+		"xvYCC Bt.601", "xvYCC Bt.709",
+		"YCbCr Bt.601 (0-255)", "YCbCr Bt.709 (0-255)",
+		"invalid", "invalid", "invalid", "invalid", "invalid",
+		"invalid", "invalid", "automatic"
+	};
+	static const char * const rgb_quantization_range_txt[] = {
+		"Automatic",
+		"RGB limited range (16-235)",
+		"RGB full range (0-255)",
+	};
+	static const char * const deep_color_mode_txt[4] = {
+		"8-bits per channel",
+		"10-bits per channel",
+		"12-bits per channel",
+		"16-bits per channel (not supported)"
+	};
+
+	v4l2_info(sd, "-----Chip status-----\n");
+	v4l2_info(sd, "Chip power: %s\n", no_power(sd) ? "off" : "on");
+	v4l2_info(sd, "HDMI/DVI-D port selected: %s\n",
+			state->hdmi_port_a ? "A" : "B");
+	v4l2_info(sd, "EDID A %s, B %s\n",
+		  ((reg_rep_0x7d & 0x04) && (reg_rep_0x77 & 0x04)) ?
+		  "enabled" : "disabled",
+		  ((reg_rep_0x7d & 0x08) && (reg_rep_0x77 & 0x08)) ?
+		  "enabled" : "disabled");
+	v4l2_info(sd, "HPD A %s, B %s\n",
+		  reg_io_0x21 & 0x02 ? "enabled" : "disabled",
+		  reg_io_0x21 & 0x01 ? "enabled" : "disabled");
+	v4l2_info(sd, "CEC: %s\n", state->cec_enabled_adap ?
+			"enabled" : "disabled");
+	if (state->cec_enabled_adap) {
+		int i;
+
+		for (i = 0; i < ADV7842_MAX_ADDRS; i++) {
+			bool is_valid = state->cec_valid_addrs & (1 << i);
+
+			if (is_valid)
+				v4l2_info(sd, "CEC Logical Address: 0x%x\n",
+					  state->cec_addr[i]);
+		}
+	}
+
+	v4l2_info(sd, "-----Signal status-----\n");
+	if (state->hdmi_port_a) {
+		v4l2_info(sd, "Cable detected (+5V power): %s\n",
+			  io_read(sd, 0x6f) & 0x02 ? "true" : "false");
+		v4l2_info(sd, "TMDS signal detected: %s\n",
+			  (io_read(sd, 0x6a) & 0x02) ? "true" : "false");
+		v4l2_info(sd, "TMDS signal locked: %s\n",
+			  (io_read(sd, 0x6a) & 0x20) ? "true" : "false");
+	} else {
+		v4l2_info(sd, "Cable detected (+5V power):%s\n",
+			  io_read(sd, 0x6f) & 0x01 ? "true" : "false");
+		v4l2_info(sd, "TMDS signal detected: %s\n",
+			  (io_read(sd, 0x6a) & 0x01) ? "true" : "false");
+		v4l2_info(sd, "TMDS signal locked: %s\n",
+			  (io_read(sd, 0x6a) & 0x10) ? "true" : "false");
+	}
+	v4l2_info(sd, "CP free run: %s\n",
+		  (!!(cp_read(sd, 0xff) & 0x10) ? "on" : "off"));
+	v4l2_info(sd, "Prim-mode = 0x%x, video std = 0x%x, v_freq = 0x%x\n",
+		  io_read(sd, 0x01) & 0x0f, io_read(sd, 0x00) & 0x3f,
+		  (io_read(sd, 0x01) & 0x70) >> 4);
+
+	v4l2_info(sd, "-----Video Timings-----\n");
+	if (no_cp_signal(sd)) {
+		v4l2_info(sd, "STDI: not locked\n");
+	} else {
+		u32 bl = ((cp_read(sd, 0xb1) & 0x3f) << 8) | cp_read(sd, 0xb2);
+		u32 lcf = ((cp_read(sd, 0xb3) & 0x7) << 8) | cp_read(sd, 0xb4);
+		u32 lcvs = cp_read(sd, 0xb3) >> 3;
+		u32 fcl = ((cp_read(sd, 0xb8) & 0x1f) << 8) | cp_read(sd, 0xb9);
+		char hs_pol = ((cp_read(sd, 0xb5) & 0x10) ?
+				((cp_read(sd, 0xb5) & 0x08) ? '+' : '-') : 'x');
+		char vs_pol = ((cp_read(sd, 0xb5) & 0x40) ?
+				((cp_read(sd, 0xb5) & 0x20) ? '+' : '-') : 'x');
+		v4l2_info(sd,
+			"STDI: lcf (frame height - 1) = %d, bl = %d, lcvs (vsync) = %d, fcl = %d, %s, %chsync, %cvsync\n",
+			lcf, bl, lcvs, fcl,
+			(cp_read(sd, 0xb1) & 0x40) ?
+				"interlaced" : "progressive",
+			hs_pol, vs_pol);
+	}
+	if (adv7842_query_dv_timings(sd, &timings))
+		v4l2_info(sd, "No video detected\n");
+	else
+		v4l2_print_dv_timings(sd->name, "Detected format: ",
+				      &timings, true);
+	v4l2_print_dv_timings(sd->name, "Configured format: ",
+			&state->timings, true);
+
+	if (no_cp_signal(sd))
+		return 0;
+
+	v4l2_info(sd, "-----Color space-----\n");
+	v4l2_info(sd, "RGB quantization range ctrl: %s\n",
+		  rgb_quantization_range_txt[state->rgb_quantization_range]);
+	v4l2_info(sd, "Input color space: %s\n",
+		  input_color_space_txt[reg_io_0x02 >> 4]);
+	v4l2_info(sd, "Output color space: %s %s, alt-gamma %s\n",
+		  (reg_io_0x02 & 0x02) ? "RGB" : "YCbCr",
+		  (((reg_io_0x02 >> 2) & 0x01) ^ (reg_io_0x02 & 0x01)) ?
+			"(16-235)" : "(0-255)",
+		  (reg_io_0x02 & 0x08) ? "enabled" : "disabled");
+	v4l2_info(sd, "Color space conversion: %s\n",
+		  csc_coeff_sel_rb[cp_read(sd, 0xf4) >> 4]);
+
+	if (!is_digital_input(sd))
+		return 0;
+
+	v4l2_info(sd, "-----%s status-----\n", is_hdmi(sd) ? "HDMI" : "DVI-D");
+	v4l2_info(sd, "HDCP encrypted content: %s\n",
+			(hdmi_read(sd, 0x05) & 0x40) ? "true" : "false");
+	v4l2_info(sd, "HDCP keys read: %s%s\n",
+			(hdmi_read(sd, 0x04) & 0x20) ? "yes" : "no",
+			(hdmi_read(sd, 0x04) & 0x10) ? "ERROR" : "");
+	if (!is_hdmi(sd))
+		return 0;
+
+	v4l2_info(sd, "Audio: pll %s, samples %s, %s\n",
+			audio_pll_locked ? "locked" : "not locked",
+			audio_sample_packet_detect ? "detected" : "not detected",
+			audio_mute ? "muted" : "enabled");
+	if (audio_pll_locked && audio_sample_packet_detect) {
+		v4l2_info(sd, "Audio format: %s\n",
+			(hdmi_read(sd, 0x07) & 0x40) ? "multi-channel" : "stereo");
+	}
+	v4l2_info(sd, "Audio CTS: %u\n", (hdmi_read(sd, 0x5b) << 12) +
+			(hdmi_read(sd, 0x5c) << 8) +
+			(hdmi_read(sd, 0x5d) & 0xf0));
+	v4l2_info(sd, "Audio N: %u\n", ((hdmi_read(sd, 0x5d) & 0x0f) << 16) +
+			(hdmi_read(sd, 0x5e) << 8) +
+			hdmi_read(sd, 0x5f));
+	v4l2_info(sd, "AV Mute: %s\n",
+			(hdmi_read(sd, 0x04) & 0x40) ? "on" : "off");
+	v4l2_info(sd, "Deep color mode: %s\n",
+			deep_color_mode_txt[hdmi_read(sd, 0x0b) >> 6]);
+
+	adv7842_log_infoframes(sd);
+
+	return 0;
+}
+
+static int adv7842_log_status(struct v4l2_subdev *sd)
+{
+	struct adv7842_state *state = to_state(sd);
+
+	if (state->mode == ADV7842_MODE_SDP)
+		return adv7842_sdp_log_status(sd);
+	return adv7842_cp_log_status(sd);
+}
+
+static int adv7842_querystd(struct v4l2_subdev *sd, v4l2_std_id *std)
+{
+	struct adv7842_state *state = to_state(sd);
+
+	v4l2_dbg(1, debug, sd, "%s:\n", __func__);
+
+	if (state->mode != ADV7842_MODE_SDP)
+		return -ENODATA;
+
+	if (!(sdp_read(sd, 0x5A) & 0x01)) {
+		*std = 0;
+		v4l2_dbg(1, debug, sd, "%s: no valid signal\n", __func__);
+		return 0;
+	}
+
+	switch (sdp_read(sd, 0x52) & 0x0f) {
+	case 0:
+		/* NTSC-M/J */
+		*std &= V4L2_STD_NTSC;
+		break;
+	case 2:
+		/* NTSC-443 */
+		*std &= V4L2_STD_NTSC_443;
+		break;
+	case 3:
+		/* 60HzSECAM */
+		*std &= V4L2_STD_SECAM;
+		break;
+	case 4:
+		/* PAL-M */
+		*std &= V4L2_STD_PAL_M;
+		break;
+	case 6:
+		/* PAL-60 */
+		*std &= V4L2_STD_PAL_60;
+		break;
+	case 0xc:
+		/* PAL-CombN */
+		*std &= V4L2_STD_PAL_Nc;
+		break;
+	case 0xe:
+		/* PAL-BGHID */
+		*std &= V4L2_STD_PAL;
+		break;
+	case 0xf:
+		/* SECAM */
+		*std &= V4L2_STD_SECAM;
+		break;
+	default:
+		*std &= V4L2_STD_ALL;
+		break;
+	}
+	return 0;
+}
+
+static void adv7842_s_sdp_io(struct v4l2_subdev *sd, struct adv7842_sdp_io_sync_adjustment *s)
+{
+	if (s && s->adjust) {
+		sdp_io_write(sd, 0x94, (s->hs_beg >> 8) & 0xf);
+		sdp_io_write(sd, 0x95, s->hs_beg & 0xff);
+		sdp_io_write(sd, 0x96, (s->hs_width >> 8) & 0xf);
+		sdp_io_write(sd, 0x97, s->hs_width & 0xff);
+		sdp_io_write(sd, 0x98, (s->de_beg >> 8) & 0xf);
+		sdp_io_write(sd, 0x99, s->de_beg & 0xff);
+		sdp_io_write(sd, 0x9a, (s->de_end >> 8) & 0xf);
+		sdp_io_write(sd, 0x9b, s->de_end & 0xff);
+		sdp_io_write(sd, 0xa8, s->vs_beg_o);
+		sdp_io_write(sd, 0xa9, s->vs_beg_e);
+		sdp_io_write(sd, 0xaa, s->vs_end_o);
+		sdp_io_write(sd, 0xab, s->vs_end_e);
+		sdp_io_write(sd, 0xac, s->de_v_beg_o);
+		sdp_io_write(sd, 0xad, s->de_v_beg_e);
+		sdp_io_write(sd, 0xae, s->de_v_end_o);
+		sdp_io_write(sd, 0xaf, s->de_v_end_e);
+	} else {
+		/* set to default */
+		sdp_io_write(sd, 0x94, 0x00);
+		sdp_io_write(sd, 0x95, 0x00);
+		sdp_io_write(sd, 0x96, 0x00);
+		sdp_io_write(sd, 0x97, 0x20);
+		sdp_io_write(sd, 0x98, 0x00);
+		sdp_io_write(sd, 0x99, 0x00);
+		sdp_io_write(sd, 0x9a, 0x00);
+		sdp_io_write(sd, 0x9b, 0x00);
+		sdp_io_write(sd, 0xa8, 0x04);
+		sdp_io_write(sd, 0xa9, 0x04);
+		sdp_io_write(sd, 0xaa, 0x04);
+		sdp_io_write(sd, 0xab, 0x04);
+		sdp_io_write(sd, 0xac, 0x04);
+		sdp_io_write(sd, 0xad, 0x04);
+		sdp_io_write(sd, 0xae, 0x04);
+		sdp_io_write(sd, 0xaf, 0x04);
+	}
+}
+
+static int adv7842_s_std(struct v4l2_subdev *sd, v4l2_std_id norm)
+{
+	struct adv7842_state *state = to_state(sd);
+	struct adv7842_platform_data *pdata = &state->pdata;
+
+	v4l2_dbg(1, debug, sd, "%s:\n", __func__);
+
+	if (state->mode != ADV7842_MODE_SDP)
+		return -ENODATA;
+
+	if (norm & V4L2_STD_625_50)
+		adv7842_s_sdp_io(sd, &pdata->sdp_io_sync_625);
+	else if (norm & V4L2_STD_525_60)
+		adv7842_s_sdp_io(sd, &pdata->sdp_io_sync_525);
+	else
+		adv7842_s_sdp_io(sd, NULL);
+
+	if (norm & V4L2_STD_ALL) {
+		state->norm = norm;
+		return 0;
+	}
+	return -EINVAL;
+}
+
+static int adv7842_g_std(struct v4l2_subdev *sd, v4l2_std_id *norm)
+{
+	struct adv7842_state *state = to_state(sd);
+
+	v4l2_dbg(1, debug, sd, "%s:\n", __func__);
+
+	if (state->mode != ADV7842_MODE_SDP)
+		return -ENODATA;
+
+	*norm = state->norm;
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static int adv7842_core_init(struct v4l2_subdev *sd)
+{
+	struct adv7842_state *state = to_state(sd);
+	struct adv7842_platform_data *pdata = &state->pdata;
+	hdmi_write(sd, 0x48,
+		   (pdata->disable_pwrdnb ? 0x80 : 0) |
+		   (pdata->disable_cable_det_rst ? 0x40 : 0));
+
+	disable_input(sd);
+
+	/*
+	 * Disable I2C access to internal EDID ram from HDMI DDC ports
+	 * Disable auto edid enable when leaving powerdown mode
+	 */
+	rep_write_and_or(sd, 0x77, 0xd3, 0x20);
+
+	/* power */
+	io_write(sd, 0x0c, 0x42);   /* Power up part and power down VDP */
+	io_write(sd, 0x15, 0x80);   /* Power up pads */
+
+	/* video format */
+	io_write(sd, 0x02, 0xf0 | pdata->alt_gamma << 3);
+	io_write_and_or(sd, 0x05, 0xf0, pdata->blank_data << 3 |
+			pdata->insert_av_codes << 2 |
+			pdata->replicate_av_codes << 1);
+	adv7842_setup_format(state);
+
+	/* HDMI audio */
+	hdmi_write_and_or(sd, 0x1a, 0xf1, 0x08); /* Wait 1 s before unmute */
+
+	/* Drive strength */
+	io_write_and_or(sd, 0x14, 0xc0,
+			pdata->dr_str_data << 4 |
+			pdata->dr_str_clk << 2 |
+			pdata->dr_str_sync);
+
+	/* HDMI free run */
+	cp_write_and_or(sd, 0xba, 0xfc, pdata->hdmi_free_run_enable |
+					(pdata->hdmi_free_run_mode << 1));
+
+	/* SPD free run */
+	sdp_write_and_or(sd, 0xdd, 0xf0, pdata->sdp_free_run_force |
+					 (pdata->sdp_free_run_cbar_en << 1) |
+					 (pdata->sdp_free_run_man_col_en << 2) |
+					 (pdata->sdp_free_run_auto << 3));
+
+	/* TODO from platform data */
+	cp_write(sd, 0x69, 0x14);   /* Enable CP CSC */
+	io_write(sd, 0x06, 0xa6);   /* positive VS and HS and DE */
+	cp_write(sd, 0xf3, 0xdc); /* Low threshold to enter/exit free run mode */
+	afe_write(sd, 0xb5, 0x01);  /* Setting MCLK to 256Fs */
+
+	afe_write(sd, 0x02, pdata->ain_sel); /* Select analog input muxing mode */
+	io_write_and_or(sd, 0x30, ~(1 << 4), pdata->output_bus_lsb_to_msb << 4);
+
+	sdp_csc_coeff(sd, &pdata->sdp_csc_coeff);
+
+	/* todo, improve settings for sdram */
+	if (pdata->sd_ram_size >= 128) {
+		sdp_write(sd, 0x12, 0x0d); /* Frame TBC,3D comb enabled */
+		if (pdata->sd_ram_ddr) {
+			/* SDP setup for the AD eval board */
+			sdp_io_write(sd, 0x6f, 0x00); /* DDR mode */
+			sdp_io_write(sd, 0x75, 0x0a); /* 128 MB memory size */
+			sdp_io_write(sd, 0x7a, 0xa5); /* Timing Adjustment */
+			sdp_io_write(sd, 0x7b, 0x8f); /* Timing Adjustment */
+			sdp_io_write(sd, 0x60, 0x01); /* SDRAM reset */
+		} else {
+			sdp_io_write(sd, 0x75, 0x0a); /* 64 MB memory size ?*/
+			sdp_io_write(sd, 0x74, 0x00); /* must be zero for sdr sdram */
+			sdp_io_write(sd, 0x79, 0x33); /* CAS latency to 3,
+							 depends on memory */
+			sdp_io_write(sd, 0x6f, 0x01); /* SDR mode */
+			sdp_io_write(sd, 0x7a, 0xa5); /* Timing Adjustment */
+			sdp_io_write(sd, 0x7b, 0x8f); /* Timing Adjustment */
+			sdp_io_write(sd, 0x60, 0x01); /* SDRAM reset */
+		}
+	} else {
+		/*
+		 * Manual UG-214, rev 0 is bit confusing on this bit
+		 * but a '1' disables any signal if the Ram is active.
+		 */
+		sdp_io_write(sd, 0x29, 0x10); /* Tristate memory interface */
+	}
+
+	select_input(sd, pdata->vid_std_select);
+
+	enable_input(sd);
+
+	if (pdata->hpa_auto) {
+		/* HPA auto, HPA 0.5s after Edid set and Cable detect */
+		hdmi_write(sd, 0x69, 0x5c);
+	} else {
+		/* HPA manual */
+		hdmi_write(sd, 0x69, 0xa3);
+		/* HPA disable on port A and B */
+		io_write_and_or(sd, 0x20, 0xcf, 0x00);
+	}
+
+	/* LLC */
+	io_write(sd, 0x19, 0x80 | pdata->llc_dll_phase);
+	io_write(sd, 0x33, 0x40);
+
+	/* interrupts */
+	io_write(sd, 0x40, 0xf2); /* Configure INT1 */
+
+	adv7842_irq_enable(sd, true);
+
+	return v4l2_ctrl_handler_setup(sd->ctrl_handler);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static int adv7842_ddr_ram_test(struct v4l2_subdev *sd)
+{
+	/*
+	 * From ADV784x external Memory test.pdf
+	 *
+	 * Reset must just been performed before running test.
+	 * Recommended to reset after test.
+	 */
+	int i;
+	int pass = 0;
+	int fail = 0;
+	int complete = 0;
+
+	io_write(sd, 0x00, 0x01);  /* Program SDP 4x1 */
+	io_write(sd, 0x01, 0x00);  /* Program SDP mode */
+	afe_write(sd, 0x80, 0x92); /* SDP Recommended Write */
+	afe_write(sd, 0x9B, 0x01); /* SDP Recommended Write ADV7844ES1 */
+	afe_write(sd, 0x9C, 0x60); /* SDP Recommended Write ADV7844ES1 */
+	afe_write(sd, 0x9E, 0x02); /* SDP Recommended Write ADV7844ES1 */
+	afe_write(sd, 0xA0, 0x0B); /* SDP Recommended Write ADV7844ES1 */
+	afe_write(sd, 0xC3, 0x02); /* Memory BIST Initialisation */
+	io_write(sd, 0x0C, 0x40);  /* Power up ADV7844 */
+	io_write(sd, 0x15, 0xBA);  /* Enable outputs */
+	sdp_write(sd, 0x12, 0x00); /* Disable 3D comb, Frame TBC & 3DNR */
+	io_write(sd, 0xFF, 0x04);  /* Reset memory controller */
+
+	usleep_range(5000, 6000);
+
+	sdp_write(sd, 0x12, 0x00);    /* Disable 3D Comb, Frame TBC & 3DNR */
+	sdp_io_write(sd, 0x2A, 0x01); /* Memory BIST Initialisation */
+	sdp_io_write(sd, 0x7c, 0x19); /* Memory BIST Initialisation */
+	sdp_io_write(sd, 0x80, 0x87); /* Memory BIST Initialisation */
+	sdp_io_write(sd, 0x81, 0x4a); /* Memory BIST Initialisation */
+	sdp_io_write(sd, 0x82, 0x2c); /* Memory BIST Initialisation */
+	sdp_io_write(sd, 0x83, 0x0e); /* Memory BIST Initialisation */
+	sdp_io_write(sd, 0x84, 0x94); /* Memory BIST Initialisation */
+	sdp_io_write(sd, 0x85, 0x62); /* Memory BIST Initialisation */
+	sdp_io_write(sd, 0x7d, 0x00); /* Memory BIST Initialisation */
+	sdp_io_write(sd, 0x7e, 0x1a); /* Memory BIST Initialisation */
+
+	usleep_range(5000, 6000);
+
+	sdp_io_write(sd, 0xd9, 0xd5); /* Enable BIST Test */
+	sdp_write(sd, 0x12, 0x05); /* Enable FRAME TBC & 3D COMB */
+
+	msleep(20);
+
+	for (i = 0; i < 10; i++) {
+		u8 result = sdp_io_read(sd, 0xdb);
+		if (result & 0x10) {
+			complete++;
+			if (result & 0x20)
+				fail++;
+			else
+				pass++;
+		}
+		msleep(20);
+	}
+
+	v4l2_dbg(1, debug, sd,
+		"Ram Test: completed %d of %d: pass %d, fail %d\n",
+		complete, i, pass, fail);
+
+	if (!complete || fail)
+		return -EIO;
+	return 0;
+}
+
+static void adv7842_rewrite_i2c_addresses(struct v4l2_subdev *sd,
+		struct adv7842_platform_data *pdata)
+{
+	io_write(sd, 0xf1, pdata->i2c_sdp << 1);
+	io_write(sd, 0xf2, pdata->i2c_sdp_io << 1);
+	io_write(sd, 0xf3, pdata->i2c_avlink << 1);
+	io_write(sd, 0xf4, pdata->i2c_cec << 1);
+	io_write(sd, 0xf5, pdata->i2c_infoframe << 1);
+
+	io_write(sd, 0xf8, pdata->i2c_afe << 1);
+	io_write(sd, 0xf9, pdata->i2c_repeater << 1);
+	io_write(sd, 0xfa, pdata->i2c_edid << 1);
+	io_write(sd, 0xfb, pdata->i2c_hdmi << 1);
+
+	io_write(sd, 0xfd, pdata->i2c_cp << 1);
+	io_write(sd, 0xfe, pdata->i2c_vdp << 1);
+}
+
+static int adv7842_command_ram_test(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct adv7842_state *state = to_state(sd);
+	struct adv7842_platform_data *pdata = client->dev.platform_data;
+	struct v4l2_dv_timings timings;
+	int ret = 0;
+
+	if (!pdata)
+		return -ENODEV;
+
+	if (!pdata->sd_ram_size || !pdata->sd_ram_ddr) {
+		v4l2_info(sd, "no sdram or no ddr sdram\n");
+		return -EINVAL;
+	}
+
+	main_reset(sd);
+
+	adv7842_rewrite_i2c_addresses(sd, pdata);
+
+	/* run ram test */
+	ret = adv7842_ddr_ram_test(sd);
+
+	main_reset(sd);
+
+	adv7842_rewrite_i2c_addresses(sd, pdata);
+
+	/* and re-init chip and state */
+	adv7842_core_init(sd);
+
+	disable_input(sd);
+
+	select_input(sd, state->vid_std_select);
+
+	enable_input(sd);
+
+	edid_write_vga_segment(sd);
+	edid_write_hdmi_segment(sd, ADV7842_EDID_PORT_A);
+	edid_write_hdmi_segment(sd, ADV7842_EDID_PORT_B);
+
+	timings = state->timings;
+
+	memset(&state->timings, 0, sizeof(struct v4l2_dv_timings));
+
+	adv7842_s_dv_timings(sd, &timings);
+
+	return ret;
+}
+
+static long adv7842_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
+{
+	switch (cmd) {
+	case ADV7842_CMD_RAM_TEST:
+		return adv7842_command_ram_test(sd);
+	}
+	return -ENOTTY;
+}
+
+static int adv7842_subscribe_event(struct v4l2_subdev *sd,
+				   struct v4l2_fh *fh,
+				   struct v4l2_event_subscription *sub)
+{
+	switch (sub->type) {
+	case V4L2_EVENT_SOURCE_CHANGE:
+		return v4l2_src_change_event_subdev_subscribe(sd, fh, sub);
+	case V4L2_EVENT_CTRL:
+		return v4l2_ctrl_subdev_subscribe_event(sd, fh, sub);
+	default:
+		return -EINVAL;
+	}
+}
+
+static int adv7842_registered(struct v4l2_subdev *sd)
+{
+	struct adv7842_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int err;
+
+	err = cec_register_adapter(state->cec_adap, &client->dev);
+	if (err)
+		cec_delete_adapter(state->cec_adap);
+	return err;
+}
+
+static void adv7842_unregistered(struct v4l2_subdev *sd)
+{
+	struct adv7842_state *state = to_state(sd);
+
+	cec_unregister_adapter(state->cec_adap);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_ctrl_ops adv7842_ctrl_ops = {
+	.s_ctrl = adv7842_s_ctrl,
+	.g_volatile_ctrl = adv7842_g_volatile_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops adv7842_core_ops = {
+	.log_status = adv7842_log_status,
+	.ioctl = adv7842_ioctl,
+	.interrupt_service_routine = adv7842_isr,
+	.subscribe_event = adv7842_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = adv7842_g_register,
+	.s_register = adv7842_s_register,
+#endif
+};
+
+static const struct v4l2_subdev_video_ops adv7842_video_ops = {
+	.g_std = adv7842_g_std,
+	.s_std = adv7842_s_std,
+	.s_routing = adv7842_s_routing,
+	.querystd = adv7842_querystd,
+	.g_input_status = adv7842_g_input_status,
+	.s_dv_timings = adv7842_s_dv_timings,
+	.g_dv_timings = adv7842_g_dv_timings,
+	.query_dv_timings = adv7842_query_dv_timings,
+};
+
+static const struct v4l2_subdev_pad_ops adv7842_pad_ops = {
+	.enum_mbus_code = adv7842_enum_mbus_code,
+	.get_fmt = adv7842_get_format,
+	.set_fmt = adv7842_set_format,
+	.get_edid = adv7842_get_edid,
+	.set_edid = adv7842_set_edid,
+	.enum_dv_timings = adv7842_enum_dv_timings,
+	.dv_timings_cap = adv7842_dv_timings_cap,
+};
+
+static const struct v4l2_subdev_ops adv7842_ops = {
+	.core = &adv7842_core_ops,
+	.video = &adv7842_video_ops,
+	.pad = &adv7842_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops adv7842_int_ops = {
+	.registered = adv7842_registered,
+	.unregistered = adv7842_unregistered,
+};
+
+/* -------------------------- custom ctrls ---------------------------------- */
+
+static const struct v4l2_ctrl_config adv7842_ctrl_analog_sampling_phase = {
+	.ops = &adv7842_ctrl_ops,
+	.id = V4L2_CID_ADV_RX_ANALOG_SAMPLING_PHASE,
+	.name = "Analog Sampling Phase",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.min = 0,
+	.max = 0x1f,
+	.step = 1,
+	.def = 0,
+};
+
+static const struct v4l2_ctrl_config adv7842_ctrl_free_run_color_manual = {
+	.ops = &adv7842_ctrl_ops,
+	.id = V4L2_CID_ADV_RX_FREE_RUN_COLOR_MANUAL,
+	.name = "Free Running Color, Manual",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.step = 1,
+	.def = 1,
+};
+
+static const struct v4l2_ctrl_config adv7842_ctrl_free_run_color = {
+	.ops = &adv7842_ctrl_ops,
+	.id = V4L2_CID_ADV_RX_FREE_RUN_COLOR,
+	.name = "Free Running Color",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.max = 0xffffff,
+	.step = 0x1,
+};
+
+
+static void adv7842_unregister_clients(struct v4l2_subdev *sd)
+{
+	struct adv7842_state *state = to_state(sd);
+	i2c_unregister_device(state->i2c_avlink);
+	i2c_unregister_device(state->i2c_cec);
+	i2c_unregister_device(state->i2c_infoframe);
+	i2c_unregister_device(state->i2c_sdp_io);
+	i2c_unregister_device(state->i2c_sdp);
+	i2c_unregister_device(state->i2c_afe);
+	i2c_unregister_device(state->i2c_repeater);
+	i2c_unregister_device(state->i2c_edid);
+	i2c_unregister_device(state->i2c_hdmi);
+	i2c_unregister_device(state->i2c_cp);
+	i2c_unregister_device(state->i2c_vdp);
+
+	state->i2c_avlink = NULL;
+	state->i2c_cec = NULL;
+	state->i2c_infoframe = NULL;
+	state->i2c_sdp_io = NULL;
+	state->i2c_sdp = NULL;
+	state->i2c_afe = NULL;
+	state->i2c_repeater = NULL;
+	state->i2c_edid = NULL;
+	state->i2c_hdmi = NULL;
+	state->i2c_cp = NULL;
+	state->i2c_vdp = NULL;
+}
+
+static struct i2c_client *adv7842_dummy_client(struct v4l2_subdev *sd, const char *desc,
+					       u8 addr, u8 io_reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct i2c_client *cp;
+
+	io_write(sd, io_reg, addr << 1);
+
+	if (addr == 0) {
+		v4l2_err(sd, "no %s i2c addr configured\n", desc);
+		return NULL;
+	}
+
+	cp = i2c_new_dummy_device(client->adapter, io_read(sd, io_reg) >> 1);
+	if (IS_ERR(cp)) {
+		v4l2_err(sd, "register %s on i2c addr 0x%x failed with %ld\n",
+			 desc, addr, PTR_ERR(cp));
+		cp = NULL;
+	}
+
+	return cp;
+}
+
+static int adv7842_register_clients(struct v4l2_subdev *sd)
+{
+	struct adv7842_state *state = to_state(sd);
+	struct adv7842_platform_data *pdata = &state->pdata;
+
+	state->i2c_avlink = adv7842_dummy_client(sd, "avlink", pdata->i2c_avlink, 0xf3);
+	state->i2c_cec = adv7842_dummy_client(sd, "cec", pdata->i2c_cec, 0xf4);
+	state->i2c_infoframe = adv7842_dummy_client(sd, "infoframe", pdata->i2c_infoframe, 0xf5);
+	state->i2c_sdp_io = adv7842_dummy_client(sd, "sdp_io", pdata->i2c_sdp_io, 0xf2);
+	state->i2c_sdp = adv7842_dummy_client(sd, "sdp", pdata->i2c_sdp, 0xf1);
+	state->i2c_afe = adv7842_dummy_client(sd, "afe", pdata->i2c_afe, 0xf8);
+	state->i2c_repeater = adv7842_dummy_client(sd, "repeater", pdata->i2c_repeater, 0xf9);
+	state->i2c_edid = adv7842_dummy_client(sd, "edid", pdata->i2c_edid, 0xfa);
+	state->i2c_hdmi = adv7842_dummy_client(sd, "hdmi", pdata->i2c_hdmi, 0xfb);
+	state->i2c_cp = adv7842_dummy_client(sd, "cp", pdata->i2c_cp, 0xfd);
+	state->i2c_vdp = adv7842_dummy_client(sd, "vdp", pdata->i2c_vdp, 0xfe);
+
+	if (!state->i2c_avlink ||
+	    !state->i2c_cec ||
+	    !state->i2c_infoframe ||
+	    !state->i2c_sdp_io ||
+	    !state->i2c_sdp ||
+	    !state->i2c_afe ||
+	    !state->i2c_repeater ||
+	    !state->i2c_edid ||
+	    !state->i2c_hdmi ||
+	    !state->i2c_cp ||
+	    !state->i2c_vdp)
+		return -1;
+
+	return 0;
+}
+
+static int adv7842_probe(struct i2c_client *client)
+{
+	struct adv7842_state *state;
+	static const struct v4l2_dv_timings cea640x480 =
+		V4L2_DV_BT_CEA_640X480P59_94;
+	struct adv7842_platform_data *pdata = client->dev.platform_data;
+	struct v4l2_ctrl_handler *hdl;
+	struct v4l2_ctrl *ctrl;
+	struct v4l2_subdev *sd;
+	unsigned int i;
+	u16 rev;
+	int err;
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+
+	v4l_dbg(1, debug, client, "detecting adv7842 client on address 0x%x\n",
+		client->addr << 1);
+
+	if (!pdata) {
+		v4l_err(client, "No platform data!\n");
+		return -ENODEV;
+	}
+
+	state = devm_kzalloc(&client->dev, sizeof(*state), GFP_KERNEL);
+	if (!state)
+		return -ENOMEM;
+
+	/* platform data */
+	state->pdata = *pdata;
+	state->timings = cea640x480;
+	state->format = adv7842_format_info(state, MEDIA_BUS_FMT_YUYV8_2X8);
+
+	sd = &state->sd;
+	v4l2_i2c_subdev_init(sd, client, &adv7842_ops);
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
+	sd->internal_ops = &adv7842_int_ops;
+	state->mode = pdata->mode;
+
+	state->hdmi_port_a = pdata->input == ADV7842_SELECT_HDMI_PORT_A;
+	state->restart_stdi_once = true;
+
+	/* i2c access to adv7842? */
+	rev = adv_smbus_read_byte_data_check(client, 0xea, false) << 8 |
+		adv_smbus_read_byte_data_check(client, 0xeb, false);
+	if (rev != 0x2012) {
+		v4l2_info(sd, "got rev=0x%04x on first read attempt\n", rev);
+		rev = adv_smbus_read_byte_data_check(client, 0xea, false) << 8 |
+			adv_smbus_read_byte_data_check(client, 0xeb, false);
+	}
+	if (rev != 0x2012) {
+		v4l2_info(sd, "not an adv7842 on address 0x%x (rev=0x%04x)\n",
+			  client->addr << 1, rev);
+		return -ENODEV;
+	}
+
+	if (pdata->chip_reset)
+		main_reset(sd);
+
+	/* control handlers */
+	hdl = &state->hdl;
+	v4l2_ctrl_handler_init(hdl, 6);
+
+	/* add in ascending ID order */
+	v4l2_ctrl_new_std(hdl, &adv7842_ctrl_ops,
+			  V4L2_CID_BRIGHTNESS, -128, 127, 1, 0);
+	v4l2_ctrl_new_std(hdl, &adv7842_ctrl_ops,
+			  V4L2_CID_CONTRAST, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(hdl, &adv7842_ctrl_ops,
+			  V4L2_CID_SATURATION, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(hdl, &adv7842_ctrl_ops,
+			  V4L2_CID_HUE, 0, 128, 1, 0);
+	ctrl = v4l2_ctrl_new_std_menu(hdl, &adv7842_ctrl_ops,
+			V4L2_CID_DV_RX_IT_CONTENT_TYPE, V4L2_DV_IT_CONTENT_TYPE_NO_ITC,
+			0, V4L2_DV_IT_CONTENT_TYPE_NO_ITC);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_VOLATILE;
+
+	/* custom controls */
+	state->detect_tx_5v_ctrl = v4l2_ctrl_new_std(hdl, NULL,
+			V4L2_CID_DV_RX_POWER_PRESENT, 0, 3, 0, 0);
+	state->analog_sampling_phase_ctrl = v4l2_ctrl_new_custom(hdl,
+			&adv7842_ctrl_analog_sampling_phase, NULL);
+	state->free_run_color_ctrl_manual = v4l2_ctrl_new_custom(hdl,
+			&adv7842_ctrl_free_run_color_manual, NULL);
+	state->free_run_color_ctrl = v4l2_ctrl_new_custom(hdl,
+			&adv7842_ctrl_free_run_color, NULL);
+	state->rgb_quantization_range_ctrl =
+		v4l2_ctrl_new_std_menu(hdl, &adv7842_ctrl_ops,
+			V4L2_CID_DV_RX_RGB_RANGE, V4L2_DV_RGB_RANGE_FULL,
+			0, V4L2_DV_RGB_RANGE_AUTO);
+	sd->ctrl_handler = hdl;
+	if (hdl->error) {
+		err = hdl->error;
+		goto err_hdl;
+	}
+	if (adv7842_s_detect_tx_5v_ctrl(sd)) {
+		err = -ENODEV;
+		goto err_hdl;
+	}
+
+	if (adv7842_register_clients(sd) < 0) {
+		err = -ENOMEM;
+		v4l2_err(sd, "failed to create all i2c clients\n");
+		goto err_i2c;
+	}
+
+
+	INIT_DELAYED_WORK(&state->delayed_work_enable_hotplug,
+			adv7842_delayed_work_enable_hotplug);
+
+	sd->entity.function = MEDIA_ENT_F_DV_DECODER;
+	for (i = 0; i < ADV7842_PAD_SOURCE; ++i)
+		state->pads[i].flags = MEDIA_PAD_FL_SINK;
+	state->pads[ADV7842_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
+	err = media_entity_pads_init(&sd->entity, ADV7842_PAD_SOURCE + 1,
+				     state->pads);
+	if (err)
+		goto err_work_queues;
+
+	err = adv7842_core_init(sd);
+	if (err)
+		goto err_entity;
+
+#if IS_ENABLED(CONFIG_VIDEO_ADV7842_CEC)
+	state->cec_adap = cec_allocate_adapter(&adv7842_cec_adap_ops,
+		state, dev_name(&client->dev),
+		CEC_CAP_DEFAULTS, ADV7842_MAX_ADDRS);
+	err = PTR_ERR_OR_ZERO(state->cec_adap);
+	if (err)
+		goto err_entity;
+#endif
+
+	v4l2_info(sd, "%s found @ 0x%x (%s)\n", client->name,
+		  client->addr << 1, client->adapter->name);
+	return 0;
+
+err_entity:
+	media_entity_cleanup(&sd->entity);
+err_work_queues:
+	cancel_delayed_work(&state->delayed_work_enable_hotplug);
+err_i2c:
+	adv7842_unregister_clients(sd);
+err_hdl:
+	v4l2_ctrl_handler_free(hdl);
+	return err;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static void adv7842_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct adv7842_state *state = to_state(sd);
+
+	adv7842_irq_enable(sd, false);
+	cancel_delayed_work_sync(&state->delayed_work_enable_hotplug);
+	v4l2_device_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	adv7842_unregister_clients(sd);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id adv7842_id[] = {
+	{ "adv7842", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, adv7842_id);
+
+/* ----------------------------------------------------------------------- */
+
+static struct i2c_driver adv7842_driver = {
+	.driver = {
+		.name = "adv7842",
+	},
+	.probe = adv7842_probe,
+	.remove = adv7842_remove,
+	.id_table = adv7842_id,
+};
+
+module_i2c_driver(adv7842_driver);
diff --git a/drivers/media/i2c/ak7375.c b/drivers/media/i2c/ak7375.c
new file mode 100644
index 000000000..463b51d46
--- /dev/null
+++ b/drivers/media/i2c/ak7375.c
@@ -0,0 +1,317 @@
+// SPDX-License-Identifier: GPL-2.0
+// Copyright (C) 2018 Intel Corporation
+
+#include <linux/acpi.h>
+#include <linux/delay.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>
+
+#define AK7375_MAX_FOCUS_POS	4095
+/*
+ * This sets the minimum granularity for the focus positions.
+ * A value of 1 gives maximum accuracy for a desired focus position
+ */
+#define AK7375_FOCUS_STEPS	1
+/*
+ * This acts as the minimum granularity of lens movement.
+ * Keep this value power of 2, so the control steps can be
+ * uniformly adjusted for gradual lens movement, with desired
+ * number of control steps.
+ */
+#define AK7375_CTRL_STEPS	64
+#define AK7375_CTRL_DELAY_US	1000
+/*
+ * The vcm may take up 10 ms (tDELAY) to power on and start taking
+ * I2C messages. Based on AK7371 datasheet.
+ */
+#define AK7375_POWER_DELAY_US	10000
+
+#define AK7375_REG_POSITION	0x0
+#define AK7375_REG_CONT		0x2
+#define AK7375_MODE_ACTIVE	0x0
+#define AK7375_MODE_STANDBY	0x40
+
+static const char * const ak7375_supply_names[] = {
+	"vdd",
+	"vio",
+};
+
+/* ak7375 device structure */
+struct ak7375_device {
+	struct v4l2_ctrl_handler ctrls_vcm;
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl *focus;
+	struct regulator_bulk_data supplies[ARRAY_SIZE(ak7375_supply_names)];
+
+	/* active or standby mode */
+	bool active;
+};
+
+static inline struct ak7375_device *to_ak7375_vcm(struct v4l2_ctrl *ctrl)
+{
+	return container_of(ctrl->handler, struct ak7375_device, ctrls_vcm);
+}
+
+static inline struct ak7375_device *sd_to_ak7375_vcm(struct v4l2_subdev *subdev)
+{
+	return container_of(subdev, struct ak7375_device, sd);
+}
+
+static int ak7375_i2c_write(struct ak7375_device *ak7375,
+	u8 addr, u16 data, u8 size)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ak7375->sd);
+	u8 buf[3];
+	int ret;
+
+	if (size != 1 && size != 2)
+		return -EINVAL;
+	buf[0] = addr;
+	buf[size] = data & 0xff;
+	if (size == 2)
+		buf[1] = (data >> 8) & 0xff;
+	ret = i2c_master_send(client, (const char *)buf, size + 1);
+	if (ret < 0)
+		return ret;
+	if (ret != size + 1)
+		return -EIO;
+
+	return 0;
+}
+
+static int ak7375_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ak7375_device *dev_vcm = to_ak7375_vcm(ctrl);
+
+	if (ctrl->id == V4L2_CID_FOCUS_ABSOLUTE)
+		return ak7375_i2c_write(dev_vcm, AK7375_REG_POSITION,
+					ctrl->val << 4, 2);
+
+	return -EINVAL;
+}
+
+static const struct v4l2_ctrl_ops ak7375_vcm_ctrl_ops = {
+	.s_ctrl = ak7375_set_ctrl,
+};
+
+static int ak7375_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	return pm_runtime_resume_and_get(sd->dev);
+}
+
+static int ak7375_close(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	pm_runtime_put(sd->dev);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_internal_ops ak7375_int_ops = {
+	.open = ak7375_open,
+	.close = ak7375_close,
+};
+
+static const struct v4l2_subdev_ops ak7375_ops = { };
+
+static void ak7375_subdev_cleanup(struct ak7375_device *ak7375_dev)
+{
+	v4l2_async_unregister_subdev(&ak7375_dev->sd);
+	v4l2_ctrl_handler_free(&ak7375_dev->ctrls_vcm);
+	media_entity_cleanup(&ak7375_dev->sd.entity);
+}
+
+static int ak7375_init_controls(struct ak7375_device *dev_vcm)
+{
+	struct v4l2_ctrl_handler *hdl = &dev_vcm->ctrls_vcm;
+	const struct v4l2_ctrl_ops *ops = &ak7375_vcm_ctrl_ops;
+
+	v4l2_ctrl_handler_init(hdl, 1);
+
+	dev_vcm->focus = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_FOCUS_ABSOLUTE,
+		0, AK7375_MAX_FOCUS_POS, AK7375_FOCUS_STEPS, 0);
+
+	if (hdl->error)
+		dev_err(dev_vcm->sd.dev, "%s fail error: 0x%x\n",
+			__func__, hdl->error);
+	dev_vcm->sd.ctrl_handler = hdl;
+
+	return hdl->error;
+}
+
+static int ak7375_probe(struct i2c_client *client)
+{
+	struct ak7375_device *ak7375_dev;
+	int ret;
+	unsigned int i;
+
+	ak7375_dev = devm_kzalloc(&client->dev, sizeof(*ak7375_dev),
+				  GFP_KERNEL);
+	if (!ak7375_dev)
+		return -ENOMEM;
+
+	for (i = 0; i < ARRAY_SIZE(ak7375_supply_names); i++)
+		ak7375_dev->supplies[i].supply = ak7375_supply_names[i];
+
+	ret = devm_regulator_bulk_get(&client->dev,
+				      ARRAY_SIZE(ak7375_supply_names),
+				      ak7375_dev->supplies);
+	if (ret) {
+		dev_err_probe(&client->dev, ret, "Failed to get regulators\n");
+		return ret;
+	}
+
+	v4l2_i2c_subdev_init(&ak7375_dev->sd, client, &ak7375_ops);
+	ak7375_dev->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	ak7375_dev->sd.internal_ops = &ak7375_int_ops;
+	ak7375_dev->sd.entity.function = MEDIA_ENT_F_LENS;
+
+	ret = ak7375_init_controls(ak7375_dev);
+	if (ret)
+		goto err_cleanup;
+
+	ret = media_entity_pads_init(&ak7375_dev->sd.entity, 0, NULL);
+	if (ret < 0)
+		goto err_cleanup;
+
+	ret = v4l2_async_register_subdev(&ak7375_dev->sd);
+	if (ret < 0)
+		goto err_cleanup;
+
+	pm_runtime_set_active(&client->dev);
+	pm_runtime_enable(&client->dev);
+	pm_runtime_idle(&client->dev);
+
+	return 0;
+
+err_cleanup:
+	v4l2_ctrl_handler_free(&ak7375_dev->ctrls_vcm);
+	media_entity_cleanup(&ak7375_dev->sd.entity);
+
+	return ret;
+}
+
+static void ak7375_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ak7375_device *ak7375_dev = sd_to_ak7375_vcm(sd);
+
+	ak7375_subdev_cleanup(ak7375_dev);
+	pm_runtime_disable(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+}
+
+/*
+ * This function sets the vcm position, so it consumes least current
+ * The lens position is gradually moved in units of AK7375_CTRL_STEPS,
+ * to make the movements smoothly.
+ */
+static int __maybe_unused ak7375_vcm_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ak7375_device *ak7375_dev = sd_to_ak7375_vcm(sd);
+	int ret, val;
+
+	if (!ak7375_dev->active)
+		return 0;
+
+	for (val = ak7375_dev->focus->val & ~(AK7375_CTRL_STEPS - 1);
+	     val >= 0; val -= AK7375_CTRL_STEPS) {
+		ret = ak7375_i2c_write(ak7375_dev, AK7375_REG_POSITION,
+				       val << 4, 2);
+		if (ret)
+			dev_err_once(dev, "%s I2C failure: %d\n",
+				     __func__, ret);
+		usleep_range(AK7375_CTRL_DELAY_US, AK7375_CTRL_DELAY_US + 10);
+	}
+
+	ret = ak7375_i2c_write(ak7375_dev, AK7375_REG_CONT,
+			       AK7375_MODE_STANDBY, 1);
+	if (ret)
+		dev_err(dev, "%s I2C failure: %d\n", __func__, ret);
+
+	ret = regulator_bulk_disable(ARRAY_SIZE(ak7375_supply_names),
+				     ak7375_dev->supplies);
+	if (ret)
+		return ret;
+
+	ak7375_dev->active = false;
+
+	return 0;
+}
+
+/*
+ * This function sets the vcm position to the value set by the user
+ * through v4l2_ctrl_ops s_ctrl handler
+ * The lens position is gradually moved in units of AK7375_CTRL_STEPS,
+ * to make the movements smoothly.
+ */
+static int __maybe_unused ak7375_vcm_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ak7375_device *ak7375_dev = sd_to_ak7375_vcm(sd);
+	int ret, val;
+
+	if (ak7375_dev->active)
+		return 0;
+
+	ret = regulator_bulk_enable(ARRAY_SIZE(ak7375_supply_names),
+				    ak7375_dev->supplies);
+	if (ret)
+		return ret;
+
+	/* Wait for vcm to become ready */
+	usleep_range(AK7375_POWER_DELAY_US, AK7375_POWER_DELAY_US + 500);
+
+	ret = ak7375_i2c_write(ak7375_dev, AK7375_REG_CONT,
+		AK7375_MODE_ACTIVE, 1);
+	if (ret) {
+		dev_err(dev, "%s I2C failure: %d\n", __func__, ret);
+		return ret;
+	}
+
+	for (val = ak7375_dev->focus->val % AK7375_CTRL_STEPS;
+	     val <= ak7375_dev->focus->val;
+	     val += AK7375_CTRL_STEPS) {
+		ret = ak7375_i2c_write(ak7375_dev, AK7375_REG_POSITION,
+				       val << 4, 2);
+		if (ret)
+			dev_err_ratelimited(dev, "%s I2C failure: %d\n",
+						__func__, ret);
+		usleep_range(AK7375_CTRL_DELAY_US, AK7375_CTRL_DELAY_US + 10);
+	}
+
+	ak7375_dev->active = true;
+
+	return 0;
+}
+
+static const struct of_device_id ak7375_of_table[] = {
+	{ .compatible = "asahi-kasei,ak7375" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, ak7375_of_table);
+
+static const struct dev_pm_ops ak7375_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(ak7375_vcm_suspend, ak7375_vcm_resume)
+	SET_RUNTIME_PM_OPS(ak7375_vcm_suspend, ak7375_vcm_resume, NULL)
+};
+
+static struct i2c_driver ak7375_i2c_driver = {
+	.driver = {
+		.name = "ak7375",
+		.pm = &ak7375_pm_ops,
+		.of_match_table = ak7375_of_table,
+	},
+	.probe = ak7375_probe,
+	.remove = ak7375_remove,
+};
+module_i2c_driver(ak7375_i2c_driver);
+
+MODULE_AUTHOR("Tianshu Qiu <tian.shu.qiu@intel.com>");
+MODULE_AUTHOR("Bingbu Cao <bingbu.cao@intel.com>");
+MODULE_DESCRIPTION("AK7375 VCM driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/ak881x.c b/drivers/media/i2c/ak881x.c
new file mode 100644
index 000000000..ce840adc2
--- /dev/null
+++ b/drivers/media/i2c/ak881x.c
@@ -0,0 +1,326 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Driver for AK8813 / AK8814 TV-ecoders from Asahi Kasei Microsystems Co., Ltd. (AKM)
+ *
+ * Copyright (C) 2010, Guennadi Liakhovetski <g.liakhovetski@gmx.de>
+ */
+
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <linux/module.h>
+
+#include <media/i2c/ak881x.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-device.h>
+
+#define AK881X_INTERFACE_MODE	0
+#define AK881X_VIDEO_PROCESS1	1
+#define AK881X_VIDEO_PROCESS2	2
+#define AK881X_VIDEO_PROCESS3	3
+#define AK881X_DAC_MODE		5
+#define AK881X_STATUS		0x24
+#define AK881X_DEVICE_ID	0x25
+#define AK881X_DEVICE_REVISION	0x26
+
+struct ak881x {
+	struct v4l2_subdev subdev;
+	struct ak881x_pdata *pdata;
+	unsigned int lines;
+	char revision;	/* DEVICE_REVISION content */
+};
+
+static int reg_read(struct i2c_client *client, const u8 reg)
+{
+	return i2c_smbus_read_byte_data(client, reg);
+}
+
+static int reg_write(struct i2c_client *client, const u8 reg,
+		     const u8 data)
+{
+	return i2c_smbus_write_byte_data(client, reg, data);
+}
+
+static int reg_set(struct i2c_client *client, const u8 reg,
+		   const u8 data, u8 mask)
+{
+	int ret = reg_read(client, reg);
+	if (ret < 0)
+		return ret;
+	return reg_write(client, reg, (ret & ~mask) | (data & mask));
+}
+
+static struct ak881x *to_ak881x(const struct i2c_client *client)
+{
+	return container_of(i2c_get_clientdata(client), struct ak881x, subdev);
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int ak881x_g_register(struct v4l2_subdev *sd,
+			     struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (reg->reg > 0x26)
+		return -EINVAL;
+
+	reg->size = 1;
+	reg->val = reg_read(client, reg->reg);
+
+	if (reg->val > 0xffff)
+		return -EIO;
+
+	return 0;
+}
+
+static int ak881x_s_register(struct v4l2_subdev *sd,
+			     const struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (reg->reg > 0x26)
+		return -EINVAL;
+
+	if (reg_write(client, reg->reg, reg->val) < 0)
+		return -EIO;
+
+	return 0;
+}
+#endif
+
+static int ak881x_fill_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *mf = &format->format;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ak881x *ak881x = to_ak881x(client);
+
+	if (format->pad)
+		return -EINVAL;
+
+	v4l_bound_align_image(&mf->width, 0, 720, 2,
+			      &mf->height, 0, ak881x->lines, 1, 0);
+	mf->field	= V4L2_FIELD_INTERLACED;
+	mf->code	= MEDIA_BUS_FMT_YUYV8_2X8;
+	mf->colorspace	= V4L2_COLORSPACE_SMPTE170M;
+
+	return 0;
+}
+
+static int ak881x_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->pad || code->index)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_YUYV8_2X8;
+	return 0;
+}
+
+static int ak881x_get_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *sd_state,
+				struct v4l2_subdev_selection *sel)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ak881x *ak881x = to_ak881x(client);
+
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
+		return -EINVAL;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = 720;
+		sel->r.height = ak881x->lines;
+		return 0;
+	default:
+		return -EINVAL;
+	}
+}
+
+static int ak881x_s_std_output(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ak881x *ak881x = to_ak881x(client);
+	u8 vp1;
+
+	if (std == V4L2_STD_NTSC_443) {
+		vp1 = 3;
+		ak881x->lines = 480;
+	} else if (std == V4L2_STD_PAL_M) {
+		vp1 = 5;
+		ak881x->lines = 480;
+	} else if (std == V4L2_STD_PAL_60) {
+		vp1 = 7;
+		ak881x->lines = 480;
+	} else if (std & V4L2_STD_NTSC) {
+		vp1 = 0;
+		ak881x->lines = 480;
+	} else if (std & V4L2_STD_PAL) {
+		vp1 = 0xf;
+		ak881x->lines = 576;
+	} else {
+		/* No SECAM or PAL_N/Nc supported */
+		return -EINVAL;
+	}
+
+	reg_set(client, AK881X_VIDEO_PROCESS1, vp1, 0xf);
+
+	return 0;
+}
+
+static int ak881x_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ak881x *ak881x = to_ak881x(client);
+
+	if (enable) {
+		u8 dac;
+		/* For colour-bar testing set bit 6 of AK881X_VIDEO_PROCESS1 */
+		/* Default: composite output */
+		if (ak881x->pdata->flags & AK881X_COMPONENT)
+			dac = 3;
+		else
+			dac = 4;
+		/* Turn on the DAC(s) */
+		reg_write(client, AK881X_DAC_MODE, dac);
+		dev_dbg(&client->dev, "chip status 0x%x\n",
+			reg_read(client, AK881X_STATUS));
+	} else {
+		/* ...and clear bit 6 of AK881X_VIDEO_PROCESS1 here */
+		reg_write(client, AK881X_DAC_MODE, 0);
+		dev_dbg(&client->dev, "chip status 0x%x\n",
+			reg_read(client, AK881X_STATUS));
+	}
+
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops ak881x_subdev_core_ops = {
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register	= ak881x_g_register,
+	.s_register	= ak881x_s_register,
+#endif
+};
+
+static const struct v4l2_subdev_video_ops ak881x_subdev_video_ops = {
+	.s_std_output	= ak881x_s_std_output,
+	.s_stream	= ak881x_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops ak881x_subdev_pad_ops = {
+	.enum_mbus_code = ak881x_enum_mbus_code,
+	.get_selection	= ak881x_get_selection,
+	.set_fmt	= ak881x_fill_fmt,
+	.get_fmt	= ak881x_fill_fmt,
+};
+
+static const struct v4l2_subdev_ops ak881x_subdev_ops = {
+	.core	= &ak881x_subdev_core_ops,
+	.video	= &ak881x_subdev_video_ops,
+	.pad	= &ak881x_subdev_pad_ops,
+};
+
+static int ak881x_probe(struct i2c_client *client)
+{
+	struct i2c_adapter *adapter = client->adapter;
+	struct ak881x *ak881x;
+	u8 ifmode, data;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA)) {
+		dev_warn(&adapter->dev,
+			 "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
+		return -EIO;
+	}
+
+	ak881x = devm_kzalloc(&client->dev, sizeof(*ak881x), GFP_KERNEL);
+	if (!ak881x)
+		return -ENOMEM;
+
+	v4l2_i2c_subdev_init(&ak881x->subdev, client, &ak881x_subdev_ops);
+
+	data = reg_read(client, AK881X_DEVICE_ID);
+
+	switch (data) {
+	case 0x13:
+	case 0x14:
+		break;
+	default:
+		dev_err(&client->dev,
+			"No ak881x chip detected, register read %x\n", data);
+		return -ENODEV;
+	}
+
+	ak881x->revision = reg_read(client, AK881X_DEVICE_REVISION);
+	ak881x->pdata = client->dev.platform_data;
+
+	if (ak881x->pdata) {
+		if (ak881x->pdata->flags & AK881X_FIELD)
+			ifmode = 4;
+		else
+			ifmode = 0;
+
+		switch (ak881x->pdata->flags & AK881X_IF_MODE_MASK) {
+		case AK881X_IF_MODE_BT656:
+			ifmode |= 1;
+			break;
+		case AK881X_IF_MODE_MASTER:
+			ifmode |= 2;
+			break;
+		case AK881X_IF_MODE_SLAVE:
+		default:
+			break;
+		}
+
+		dev_dbg(&client->dev, "IF mode %x\n", ifmode);
+
+		/*
+		 * "Line Blanking No." seems to be the same as the number of
+		 * "black" lines on, e.g., SuperH VOU, whose default value of 20
+		 * "incidentally" matches ak881x' default
+		 */
+		reg_write(client, AK881X_INTERFACE_MODE, ifmode | (20 << 3));
+	}
+
+	/* Hardware default: NTSC-M */
+	ak881x->lines = 480;
+
+	dev_info(&client->dev, "Detected an ak881x chip ID %x, revision %x\n",
+		 data, ak881x->revision);
+
+	return 0;
+}
+
+static void ak881x_remove(struct i2c_client *client)
+{
+	struct ak881x *ak881x = to_ak881x(client);
+
+	v4l2_device_unregister_subdev(&ak881x->subdev);
+}
+
+static const struct i2c_device_id ak881x_id[] = {
+	{ "ak8813", 0 },
+	{ "ak8814", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ak881x_id);
+
+static struct i2c_driver ak881x_i2c_driver = {
+	.driver = {
+		.name = "ak881x",
+	},
+	.probe		= ak881x_probe,
+	.remove		= ak881x_remove,
+	.id_table	= ak881x_id,
+};
+
+module_i2c_driver(ak881x_i2c_driver);
+
+MODULE_DESCRIPTION("TV-output driver for ak8813/ak8814");
+MODULE_AUTHOR("Guennadi Liakhovetski <g.liakhovetski@gmx.de>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/aptina-pll.c b/drivers/media/i2c/aptina-pll.c
new file mode 100644
index 000000000..b1f89bbf9
--- /dev/null
+++ b/drivers/media/i2c/aptina-pll.c
@@ -0,0 +1,159 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Aptina Sensor PLL Configuration
+ *
+ * Copyright (C) 2012 Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ */
+
+#include <linux/device.h>
+#include <linux/gcd.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+
+#include "aptina-pll.h"
+
+int aptina_pll_calculate(struct device *dev,
+			 const struct aptina_pll_limits *limits,
+			 struct aptina_pll *pll)
+{
+	unsigned int mf_min;
+	unsigned int mf_max;
+	unsigned int p1_min;
+	unsigned int p1_max;
+	unsigned int p1;
+	unsigned int div;
+
+	dev_dbg(dev, "PLL: ext clock %u pix clock %u\n",
+		pll->ext_clock, pll->pix_clock);
+
+	if (pll->ext_clock < limits->ext_clock_min ||
+	    pll->ext_clock > limits->ext_clock_max) {
+		dev_err(dev, "pll: invalid external clock frequency.\n");
+		return -EINVAL;
+	}
+
+	if (pll->pix_clock == 0 || pll->pix_clock > limits->pix_clock_max) {
+		dev_err(dev, "pll: invalid pixel clock frequency.\n");
+		return -EINVAL;
+	}
+
+	/* Compute the multiplier M and combined N*P1 divisor. */
+	div = gcd(pll->pix_clock, pll->ext_clock);
+	pll->m = pll->pix_clock / div;
+	div = pll->ext_clock / div;
+
+	/* We now have the smallest M and N*P1 values that will result in the
+	 * desired pixel clock frequency, but they might be out of the valid
+	 * range. Compute the factor by which we should multiply them given the
+	 * following constraints:
+	 *
+	 * - minimum/maximum multiplier
+	 * - minimum/maximum multiplier output clock frequency assuming the
+	 *   minimum/maximum N value
+	 * - minimum/maximum combined N*P1 divisor
+	 */
+	mf_min = DIV_ROUND_UP(limits->m_min, pll->m);
+	mf_min = max(mf_min, limits->out_clock_min /
+		     (pll->ext_clock / limits->n_min * pll->m));
+	mf_min = max(mf_min, limits->n_min * limits->p1_min / div);
+	mf_max = limits->m_max / pll->m;
+	mf_max = min(mf_max, limits->out_clock_max /
+		    (pll->ext_clock / limits->n_max * pll->m));
+	mf_max = min(mf_max, DIV_ROUND_UP(limits->n_max * limits->p1_max, div));
+
+	dev_dbg(dev, "pll: mf min %u max %u\n", mf_min, mf_max);
+	if (mf_min > mf_max) {
+		dev_err(dev, "pll: no valid combined N*P1 divisor.\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * We're looking for the highest acceptable P1 value for which a
+	 * multiplier factor MF exists that fulfills the following conditions:
+	 *
+	 * 1. p1 is in the [p1_min, p1_max] range given by the limits and is
+	 *    even
+	 * 2. mf is in the [mf_min, mf_max] range computed above
+	 * 3. div * mf is a multiple of p1, in order to compute
+	 *	n = div * mf / p1
+	 *	m = pll->m * mf
+	 * 4. the internal clock frequency, given by ext_clock / n, is in the
+	 *    [int_clock_min, int_clock_max] range given by the limits
+	 * 5. the output clock frequency, given by ext_clock / n * m, is in the
+	 *    [out_clock_min, out_clock_max] range given by the limits
+	 *
+	 * The first naive approach is to iterate over all p1 values acceptable
+	 * according to (1) and all mf values acceptable according to (2), and
+	 * stop at the first combination that fulfills (3), (4) and (5). This
+	 * has a O(n^2) complexity.
+	 *
+	 * Instead of iterating over all mf values in the [mf_min, mf_max] range
+	 * we can compute the mf increment between two acceptable values
+	 * according to (3) with
+	 *
+	 *	mf_inc = p1 / gcd(div, p1)			(6)
+	 *
+	 * and round the minimum up to the nearest multiple of mf_inc. This will
+	 * restrict the number of mf values to be checked.
+	 *
+	 * Furthermore, conditions (4) and (5) only restrict the range of
+	 * acceptable p1 and mf values by modifying the minimum and maximum
+	 * limits. (5) can be expressed as
+	 *
+	 *	ext_clock / (div * mf / p1) * m * mf >= out_clock_min
+	 *	ext_clock / (div * mf / p1) * m * mf <= out_clock_max
+	 *
+	 * or
+	 *
+	 *	p1 >= out_clock_min * div / (ext_clock * m)	(7)
+	 *	p1 <= out_clock_max * div / (ext_clock * m)
+	 *
+	 * Similarly, (4) can be expressed as
+	 *
+	 *	mf >= ext_clock * p1 / (int_clock_max * div)	(8)
+	 *	mf <= ext_clock * p1 / (int_clock_min * div)
+	 *
+	 * We can thus iterate over the restricted p1 range defined by the
+	 * combination of (1) and (7), and then compute the restricted mf range
+	 * defined by the combination of (2), (6) and (8). If the resulting mf
+	 * range is not empty, any value in the mf range is acceptable. We thus
+	 * select the mf lwoer bound and the corresponding p1 value.
+	 */
+	if (limits->p1_min == 0) {
+		dev_err(dev, "pll: P1 minimum value must be >0.\n");
+		return -EINVAL;
+	}
+
+	p1_min = max(limits->p1_min, DIV_ROUND_UP(limits->out_clock_min * div,
+		     pll->ext_clock * pll->m));
+	p1_max = min(limits->p1_max, limits->out_clock_max * div /
+		     (pll->ext_clock * pll->m));
+
+	for (p1 = p1_max & ~1; p1 >= p1_min; p1 -= 2) {
+		unsigned int mf_inc = p1 / gcd(div, p1);
+		unsigned int mf_high;
+		unsigned int mf_low;
+
+		mf_low = roundup(max(mf_min, DIV_ROUND_UP(pll->ext_clock * p1,
+					limits->int_clock_max * div)), mf_inc);
+		mf_high = min(mf_max, pll->ext_clock * p1 /
+			      (limits->int_clock_min * div));
+
+		if (mf_low > mf_high)
+			continue;
+
+		pll->n = div * mf_low / p1;
+		pll->m *= mf_low;
+		pll->p1 = p1;
+		dev_dbg(dev, "PLL: N %u M %u P1 %u\n", pll->n, pll->m, pll->p1);
+		return 0;
+	}
+
+	dev_err(dev, "pll: no valid N and P1 divisors found.\n");
+	return -EINVAL;
+}
+EXPORT_SYMBOL_GPL(aptina_pll_calculate);
+
+MODULE_DESCRIPTION("Aptina PLL Helpers");
+MODULE_AUTHOR("Laurent Pinchart <laurent.pinchart@ideasonboard.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/aptina-pll.h b/drivers/media/i2c/aptina-pll.h
new file mode 100644
index 000000000..54c0e185a
--- /dev/null
+++ b/drivers/media/i2c/aptina-pll.h
@@ -0,0 +1,43 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Aptina Sensor PLL Configuration
+ *
+ * Copyright (C) 2012 Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ */
+
+#ifndef __APTINA_PLL_H
+#define __APTINA_PLL_H
+
+struct aptina_pll {
+	unsigned int ext_clock;
+	unsigned int pix_clock;
+
+	unsigned int n;
+	unsigned int m;
+	unsigned int p1;
+};
+
+struct aptina_pll_limits {
+	unsigned int ext_clock_min;
+	unsigned int ext_clock_max;
+	unsigned int int_clock_min;
+	unsigned int int_clock_max;
+	unsigned int out_clock_min;
+	unsigned int out_clock_max;
+	unsigned int pix_clock_max;
+
+	unsigned int n_min;
+	unsigned int n_max;
+	unsigned int m_min;
+	unsigned int m_max;
+	unsigned int p1_min;
+	unsigned int p1_max;
+};
+
+struct device;
+
+int aptina_pll_calculate(struct device *dev,
+			 const struct aptina_pll_limits *limits,
+			 struct aptina_pll *pll);
+
+#endif /* __APTINA_PLL_H */
diff --git a/drivers/media/i2c/ar0521.c b/drivers/media/i2c/ar0521.c
new file mode 100644
index 000000000..a4e39871e
--- /dev/null
+++ b/drivers/media/i2c/ar0521.c
@@ -0,0 +1,1209 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 Sieć Badawcza Łukasiewicz
+ * - Przemysłowy Instytut Automatyki i Pomiarów PIAP
+ * Written by Krzysztof Hałasa
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/pm_runtime.h>
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+/* External clock (extclk) frequencies */
+#define AR0521_EXTCLK_MIN		(10 * 1000 * 1000)
+#define AR0521_EXTCLK_MAX		(48 * 1000 * 1000)
+
+/* PLL and PLL2 */
+#define AR0521_PLL_MIN			(320 * 1000 * 1000)
+#define AR0521_PLL_MAX			(1280 * 1000 * 1000)
+
+/* Effective pixel sample rate on the pixel array. */
+#define AR0521_PIXEL_CLOCK_RATE		(184 * 1000 * 1000)
+#define AR0521_PIXEL_CLOCK_MIN		(168 * 1000 * 1000)
+#define AR0521_PIXEL_CLOCK_MAX		(414 * 1000 * 1000)
+
+#define AR0521_NATIVE_WIDTH		2604u
+#define AR0521_NATIVE_HEIGHT		1964u
+#define AR0521_MIN_X_ADDR_START		0u
+#define AR0521_MIN_Y_ADDR_START		0u
+#define AR0521_MAX_X_ADDR_END		2603u
+#define AR0521_MAX_Y_ADDR_END		1955u
+
+#define AR0521_WIDTH_MIN		8u
+#define AR0521_WIDTH_MAX		2592u
+#define AR0521_HEIGHT_MIN		8u
+#define AR0521_HEIGHT_MAX		1944u
+
+#define AR0521_WIDTH_BLANKING_MIN	572u
+#define AR0521_HEIGHT_BLANKING_MIN	38u /* must be even */
+#define AR0521_TOTAL_HEIGHT_MAX		65535u /* max_frame_length_lines */
+#define AR0521_TOTAL_WIDTH_MAX		65532u /* max_line_length_pck */
+
+#define AR0521_ANA_GAIN_MIN		0x00
+#define AR0521_ANA_GAIN_MAX		0x3f
+#define AR0521_ANA_GAIN_STEP		0x01
+#define AR0521_ANA_GAIN_DEFAULT		0x00
+
+/* AR0521 registers */
+#define AR0521_REG_VT_PIX_CLK_DIV		0x0300
+#define AR0521_REG_FRAME_LENGTH_LINES		0x0340
+
+#define AR0521_REG_CHIP_ID			0x3000
+#define AR0521_REG_COARSE_INTEGRATION_TIME	0x3012
+#define AR0521_REG_ROW_SPEED			0x3016
+#define AR0521_REG_EXTRA_DELAY			0x3018
+#define AR0521_REG_RESET			0x301A
+#define   AR0521_REG_RESET_DEFAULTS		  0x0238
+#define   AR0521_REG_RESET_GROUP_PARAM_HOLD	  0x8000
+#define   AR0521_REG_RESET_STREAM		  BIT(2)
+#define   AR0521_REG_RESET_RESTART		  BIT(1)
+#define   AR0521_REG_RESET_INIT			  BIT(0)
+
+#define AR0521_REG_ANA_GAIN_CODE_GLOBAL		0x3028
+
+#define AR0521_REG_GREEN1_GAIN			0x3056
+#define AR0521_REG_BLUE_GAIN			0x3058
+#define AR0521_REG_RED_GAIN			0x305A
+#define AR0521_REG_GREEN2_GAIN			0x305C
+#define AR0521_REG_GLOBAL_GAIN			0x305E
+
+#define AR0521_REG_HISPI_TEST_MODE		0x3066
+#define AR0521_REG_HISPI_TEST_MODE_LP11		  0x0004
+
+#define AR0521_REG_TEST_PATTERN_MODE		0x3070
+
+#define AR0521_REG_SERIAL_FORMAT		0x31AE
+#define AR0521_REG_SERIAL_FORMAT_MIPI		  0x0200
+
+#define AR0521_REG_HISPI_CONTROL_STATUS		0x31C6
+#define AR0521_REG_HISPI_CONTROL_STATUS_FRAMER_TEST_MODE_ENABLE 0x80
+
+#define be		cpu_to_be16
+
+static const char * const ar0521_supply_names[] = {
+	"vdd_io",	/* I/O (1.8V) supply */
+	"vdd",		/* Core, PLL and MIPI (1.2V) supply */
+	"vaa",		/* Analog (2.7V) supply */
+};
+
+static const s64 ar0521_link_frequencies[] = {
+	184000000,
+};
+
+struct ar0521_ctrls {
+	struct v4l2_ctrl_handler handler;
+	struct {
+		struct v4l2_ctrl *gain;
+		struct v4l2_ctrl *red_balance;
+		struct v4l2_ctrl *blue_balance;
+	};
+	struct {
+		struct v4l2_ctrl *hblank;
+		struct v4l2_ctrl *vblank;
+	};
+	struct v4l2_ctrl *pixrate;
+	struct v4l2_ctrl *exposure;
+	struct v4l2_ctrl *test_pattern;
+};
+
+struct ar0521_dev {
+	struct i2c_client *i2c_client;
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct clk *extclk;
+	u32 extclk_freq;
+
+	struct regulator *supplies[ARRAY_SIZE(ar0521_supply_names)];
+	struct gpio_desc *reset_gpio;
+
+	/* lock to protect all members below */
+	struct mutex lock;
+
+	struct v4l2_mbus_framefmt fmt;
+	struct ar0521_ctrls ctrls;
+	unsigned int lane_count;
+	struct {
+		u16 pre;
+		u16 mult;
+		u16 pre2;
+		u16 mult2;
+		u16 vt_pix;
+	} pll;
+
+	bool streaming;
+};
+
+static inline struct ar0521_dev *to_ar0521_dev(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct ar0521_dev, sd);
+}
+
+static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct ar0521_dev,
+			     ctrls.handler)->sd;
+}
+
+static u32 div64_round(u64 v, u32 d)
+{
+	return div_u64(v + (d >> 1), d);
+}
+
+static u32 div64_round_up(u64 v, u32 d)
+{
+	return div_u64(v + d - 1, d);
+}
+
+static int ar0521_code_to_bpp(struct ar0521_dev *sensor)
+{
+	switch (sensor->fmt.code) {
+	case MEDIA_BUS_FMT_SGRBG8_1X8:
+		return 8;
+	}
+
+	return -EINVAL;
+}
+
+/* Data must be BE16, the first value is the register address */
+static int ar0521_write_regs(struct ar0521_dev *sensor, const __be16 *data,
+			     unsigned int count)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	struct i2c_msg msg;
+	int ret;
+
+	msg.addr = client->addr;
+	msg.flags = client->flags;
+	msg.buf = (u8 *)data;
+	msg.len = count * sizeof(*data);
+
+	ret = i2c_transfer(client->adapter, &msg, 1);
+
+	if (ret < 0) {
+		v4l2_err(&sensor->sd, "%s: I2C write error\n", __func__);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ar0521_write_reg(struct ar0521_dev *sensor, u16 reg, u16 val)
+{
+	__be16 buf[2] = {be(reg), be(val)};
+
+	return ar0521_write_regs(sensor, buf, 2);
+}
+
+static int ar0521_set_geometry(struct ar0521_dev *sensor)
+{
+	/* Center the image in the visible output window. */
+	u16 x = clamp((AR0521_WIDTH_MAX - sensor->fmt.width) / 2,
+		       AR0521_MIN_X_ADDR_START, AR0521_MAX_X_ADDR_END);
+	u16 y = clamp(((AR0521_HEIGHT_MAX - sensor->fmt.height) / 2) & ~1,
+		       AR0521_MIN_Y_ADDR_START, AR0521_MAX_Y_ADDR_END);
+
+	/* All dimensions are unsigned 12-bit integers */
+	__be16 regs[] = {
+		be(AR0521_REG_FRAME_LENGTH_LINES),
+		be(sensor->fmt.height + sensor->ctrls.vblank->val),
+		be(sensor->fmt.width + sensor->ctrls.hblank->val),
+		be(x),
+		be(y),
+		be(x + sensor->fmt.width - 1),
+		be(y + sensor->fmt.height - 1),
+		be(sensor->fmt.width),
+		be(sensor->fmt.height)
+	};
+
+	return ar0521_write_regs(sensor, regs, ARRAY_SIZE(regs));
+}
+
+static int ar0521_set_gains(struct ar0521_dev *sensor)
+{
+	int green = sensor->ctrls.gain->val;
+	int red = max(green + sensor->ctrls.red_balance->val, 0);
+	int blue = max(green + sensor->ctrls.blue_balance->val, 0);
+	unsigned int gain = min(red, min(green, blue));
+	unsigned int analog = min(gain, 64u); /* range is 0 - 127 */
+	__be16 regs[5];
+
+	red   = min(red   - analog + 64, 511u);
+	green = min(green - analog + 64, 511u);
+	blue  = min(blue  - analog + 64, 511u);
+	regs[0] = be(AR0521_REG_GREEN1_GAIN);
+	regs[1] = be(green << 7 | analog);
+	regs[2] = be(blue  << 7 | analog);
+	regs[3] = be(red   << 7 | analog);
+	regs[4] = be(green << 7 | analog);
+
+	return ar0521_write_regs(sensor, regs, ARRAY_SIZE(regs));
+}
+
+static u32 calc_pll(struct ar0521_dev *sensor, u32 freq, u16 *pre_ptr, u16 *mult_ptr)
+{
+	u16 pre = 1, mult = 1, new_pre;
+	u32 pll = AR0521_PLL_MAX + 1;
+
+	for (new_pre = 1; new_pre < 64; new_pre++) {
+		u32 new_pll;
+		u32 new_mult = div64_round_up((u64)freq * new_pre,
+					      sensor->extclk_freq);
+
+		if (new_mult < 32)
+			continue; /* Minimum value */
+		if (new_mult > 254)
+			break; /* Maximum, larger pre won't work either */
+		if (sensor->extclk_freq * (u64)new_mult < AR0521_PLL_MIN *
+		    new_pre)
+			continue;
+		if (sensor->extclk_freq * (u64)new_mult > AR0521_PLL_MAX *
+		    new_pre)
+			break; /* Larger pre won't work either */
+		new_pll = div64_round_up(sensor->extclk_freq * (u64)new_mult,
+					 new_pre);
+		if (new_pll < pll) {
+			pll = new_pll;
+			pre = new_pre;
+			mult = new_mult;
+		}
+	}
+
+	pll = div64_round(sensor->extclk_freq * (u64)mult, pre);
+	*pre_ptr = pre;
+	*mult_ptr = mult;
+	return pll;
+}
+
+static void ar0521_calc_pll(struct ar0521_dev *sensor)
+{
+	unsigned int pixel_clock;
+	u16 pre, mult;
+	u32 vco;
+	int bpp;
+
+	/*
+	 * PLL1 and PLL2 are computed equally even if the application note
+	 * suggests a slower PLL1 clock. Maintain pll1 and pll2 divider and
+	 * multiplier separated to later specialize the calculation procedure.
+	 *
+	 * PLL1:
+	 * - mclk -> / pre_div1 * pre_mul1 = VCO1 = COUNTER_CLOCK
+	 *
+	 * PLL2:
+	 * - mclk -> / pre_div * pre_mul = VCO
+	 *
+	 *   VCO -> / vt_pix = PIXEL_CLOCK
+	 *   VCO -> / vt_pix / 2 = WORD_CLOCK
+	 *   VCO -> / op_sys = SERIAL_CLOCK
+	 *
+	 * With:
+	 * - vt_pix = bpp / 2
+	 * - WORD_CLOCK = PIXEL_CLOCK / 2
+	 * - SERIAL_CLOCK = MIPI data rate (Mbps / lane) = WORD_CLOCK * bpp
+	 *   NOTE: this implies the MIPI clock is divided internally by 2
+	 *         to account for DDR.
+	 *
+	 * As op_sys_div is fixed to 1:
+	 *
+	 * SERIAL_CLOCK = VCO
+	 * VCO = 2 * MIPI_CLK
+	 * VCO = PIXEL_CLOCK * bpp / 2
+	 *
+	 * In the clock tree:
+	 * MIPI_CLK = PIXEL_CLOCK * bpp / 2 / 2
+	 *
+	 * Generic pixel_rate to bus clock frequencey equation:
+	 * MIPI_CLK = V4L2_CID_PIXEL_RATE * bpp / lanes / 2
+	 *
+	 * From which we derive the PIXEL_CLOCK to use in the clock tree:
+	 * PIXEL_CLOCK = V4L2_CID_PIXEL_RATE * 2 / lanes
+	 *
+	 * Documented clock ranges:
+	 *   WORD_CLOCK = (35MHz - 120 MHz)
+	 *   PIXEL_CLOCK = (84MHz - 207MHz)
+	 *   VCO = (320MHz - 1280MHz)
+	 *
+	 * TODO: in case we have less data lanes we have to reduce the desired
+	 * VCO not to exceed the limits specified by the datasheet and
+	 * consequentially reduce the obtained pixel clock.
+	 */
+	pixel_clock = AR0521_PIXEL_CLOCK_RATE * 2 / sensor->lane_count;
+	bpp = ar0521_code_to_bpp(sensor);
+	sensor->pll.vt_pix = bpp / 2;
+	vco = pixel_clock * sensor->pll.vt_pix;
+
+	calc_pll(sensor, vco, &pre, &mult);
+
+	sensor->pll.pre = sensor->pll.pre2 = pre;
+	sensor->pll.mult = sensor->pll.mult2 = mult;
+}
+
+static int ar0521_pll_config(struct ar0521_dev *sensor)
+{
+	__be16 pll_regs[] = {
+		be(AR0521_REG_VT_PIX_CLK_DIV),
+		/* 0x300 */ be(sensor->pll.vt_pix), /* vt_pix_clk_div = bpp / 2 */
+		/* 0x302 */ be(1), /* vt_sys_clk_div */
+		/* 0x304 */ be((sensor->pll.pre2 << 8) | sensor->pll.pre),
+		/* 0x306 */ be((sensor->pll.mult2 << 8) | sensor->pll.mult),
+		/* 0x308 */ be(sensor->pll.vt_pix * 2), /* op_pix_clk_div = 2 * vt_pix_clk_div */
+		/* 0x30A */ be(1)  /* op_sys_clk_div */
+	};
+
+	ar0521_calc_pll(sensor);
+	return ar0521_write_regs(sensor, pll_regs, ARRAY_SIZE(pll_regs));
+}
+
+static int ar0521_set_stream(struct ar0521_dev *sensor, bool on)
+{
+	int ret;
+
+	if (on) {
+		ret = pm_runtime_resume_and_get(&sensor->i2c_client->dev);
+		if (ret < 0)
+			return ret;
+
+		/* Stop streaming for just a moment */
+		ret = ar0521_write_reg(sensor, AR0521_REG_RESET,
+				       AR0521_REG_RESET_DEFAULTS);
+		if (ret)
+			return ret;
+
+		ret = ar0521_set_geometry(sensor);
+		if (ret)
+			return ret;
+
+		ret = ar0521_pll_config(sensor);
+		if (ret)
+			goto err;
+
+		ret =  __v4l2_ctrl_handler_setup(&sensor->ctrls.handler);
+		if (ret)
+			goto err;
+
+		/* Exit LP-11 mode on clock and data lanes */
+		ret = ar0521_write_reg(sensor, AR0521_REG_HISPI_CONTROL_STATUS,
+				       0);
+		if (ret)
+			goto err;
+
+		/* Start streaming */
+		ret = ar0521_write_reg(sensor, AR0521_REG_RESET,
+				       AR0521_REG_RESET_DEFAULTS |
+				       AR0521_REG_RESET_STREAM);
+		if (ret)
+			goto err;
+
+		return 0;
+
+err:
+		pm_runtime_put(&sensor->i2c_client->dev);
+		return ret;
+
+	} else {
+		/*
+		 * Reset gain, the sensor may produce all white pixels without
+		 * this
+		 */
+		ret = ar0521_write_reg(sensor, AR0521_REG_GLOBAL_GAIN, 0x2000);
+		if (ret)
+			return ret;
+
+		/* Stop streaming */
+		ret = ar0521_write_reg(sensor, AR0521_REG_RESET,
+				       AR0521_REG_RESET_DEFAULTS);
+		if (ret)
+			return ret;
+
+		pm_runtime_put(&sensor->i2c_client->dev);
+		return 0;
+	}
+}
+
+static void ar0521_adj_fmt(struct v4l2_mbus_framefmt *fmt)
+{
+	fmt->width = clamp(ALIGN(fmt->width, 4), AR0521_WIDTH_MIN,
+			   AR0521_WIDTH_MAX);
+	fmt->height = clamp(ALIGN(fmt->height, 4), AR0521_HEIGHT_MIN,
+			    AR0521_HEIGHT_MAX);
+	fmt->code = MEDIA_BUS_FMT_SGRBG8_1X8;
+	fmt->field = V4L2_FIELD_NONE;
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+	fmt->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+	fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
+	fmt->xfer_func = V4L2_XFER_FUNC_DEFAULT;
+}
+
+static int ar0521_get_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *format)
+{
+	struct ar0521_dev *sensor = to_ar0521_dev(sd);
+	struct v4l2_mbus_framefmt *fmt;
+
+	mutex_lock(&sensor->lock);
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+		fmt = v4l2_subdev_get_try_format(&sensor->sd, sd_state, 0
+						 /* pad */);
+	else
+		fmt = &sensor->fmt;
+
+	format->format = *fmt;
+
+	mutex_unlock(&sensor->lock);
+	return 0;
+}
+
+static int ar0521_set_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *format)
+{
+	struct ar0521_dev *sensor = to_ar0521_dev(sd);
+	int max_vblank, max_hblank, exposure_max;
+	int ret;
+
+	ar0521_adj_fmt(&format->format);
+
+	mutex_lock(&sensor->lock);
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		struct v4l2_mbus_framefmt *fmt;
+
+		fmt = v4l2_subdev_get_try_format(sd, sd_state, 0 /* pad */);
+		*fmt = format->format;
+
+		mutex_unlock(&sensor->lock);
+
+		return 0;
+	}
+
+	sensor->fmt = format->format;
+	ar0521_calc_pll(sensor);
+
+	/*
+	 * Update the exposure and blankings limits. Blankings are also reset
+	 * to the minimum.
+	 */
+	max_hblank = AR0521_TOTAL_WIDTH_MAX - sensor->fmt.width;
+	ret = __v4l2_ctrl_modify_range(sensor->ctrls.hblank,
+				       sensor->ctrls.hblank->minimum,
+				       max_hblank, sensor->ctrls.hblank->step,
+				       sensor->ctrls.hblank->minimum);
+	if (ret)
+		goto unlock;
+
+	ret = __v4l2_ctrl_s_ctrl(sensor->ctrls.hblank,
+				 sensor->ctrls.hblank->minimum);
+	if (ret)
+		goto unlock;
+
+	max_vblank = AR0521_TOTAL_HEIGHT_MAX - sensor->fmt.height;
+	ret = __v4l2_ctrl_modify_range(sensor->ctrls.vblank,
+				       sensor->ctrls.vblank->minimum,
+				       max_vblank, sensor->ctrls.vblank->step,
+				       sensor->ctrls.vblank->minimum);
+	if (ret)
+		goto unlock;
+
+	ret = __v4l2_ctrl_s_ctrl(sensor->ctrls.vblank,
+				 sensor->ctrls.vblank->minimum);
+	if (ret)
+		goto unlock;
+
+	exposure_max = sensor->fmt.height + AR0521_HEIGHT_BLANKING_MIN - 4;
+	ret = __v4l2_ctrl_modify_range(sensor->ctrls.exposure,
+				       sensor->ctrls.exposure->minimum,
+				       exposure_max,
+				       sensor->ctrls.exposure->step,
+				       sensor->ctrls.exposure->default_value);
+unlock:
+	mutex_unlock(&sensor->lock);
+
+	return ret;
+}
+
+static int ar0521_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
+	struct ar0521_dev *sensor = to_ar0521_dev(sd);
+	int exp_max;
+	int ret;
+
+	/* v4l2_ctrl_lock() locks our own mutex */
+
+	switch (ctrl->id) {
+	case V4L2_CID_VBLANK:
+		exp_max = sensor->fmt.height + ctrl->val - 4;
+		__v4l2_ctrl_modify_range(sensor->ctrls.exposure,
+					 sensor->ctrls.exposure->minimum,
+					 exp_max, sensor->ctrls.exposure->step,
+					 sensor->ctrls.exposure->default_value);
+		break;
+	}
+
+	/* access the sensor only if it's powered up */
+	if (!pm_runtime_get_if_in_use(&sensor->i2c_client->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_HBLANK:
+	case V4L2_CID_VBLANK:
+		ret = ar0521_set_geometry(sensor);
+		break;
+	case V4L2_CID_ANALOGUE_GAIN:
+		ret = ar0521_write_reg(sensor, AR0521_REG_ANA_GAIN_CODE_GLOBAL,
+				       ctrl->val);
+		break;
+	case V4L2_CID_GAIN:
+	case V4L2_CID_RED_BALANCE:
+	case V4L2_CID_BLUE_BALANCE:
+		ret = ar0521_set_gains(sensor);
+		break;
+	case V4L2_CID_EXPOSURE:
+		ret = ar0521_write_reg(sensor,
+				       AR0521_REG_COARSE_INTEGRATION_TIME,
+				       ctrl->val);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		ret = ar0521_write_reg(sensor, AR0521_REG_TEST_PATTERN_MODE,
+				       ctrl->val);
+		break;
+	default:
+		dev_err(&sensor->i2c_client->dev,
+			"Unsupported control %x\n", ctrl->id);
+		ret = -EINVAL;
+		break;
+	}
+
+	pm_runtime_put(&sensor->i2c_client->dev);
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ar0521_ctrl_ops = {
+	.s_ctrl = ar0521_s_ctrl,
+};
+
+static const char * const test_pattern_menu[] = {
+	"Disabled",
+	"Solid color",
+	"Color bars",
+	"Faded color bars"
+};
+
+static int ar0521_init_controls(struct ar0521_dev *sensor)
+{
+	const struct v4l2_ctrl_ops *ops = &ar0521_ctrl_ops;
+	struct ar0521_ctrls *ctrls = &sensor->ctrls;
+	struct v4l2_ctrl_handler *hdl = &ctrls->handler;
+	int max_vblank, max_hblank, exposure_max;
+	struct v4l2_ctrl *link_freq;
+	int ret;
+
+	v4l2_ctrl_handler_init(hdl, 32);
+
+	/* We can use our own mutex for the ctrl lock */
+	hdl->lock = &sensor->lock;
+
+	/* Analog gain */
+	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_ANALOGUE_GAIN,
+			  AR0521_ANA_GAIN_MIN, AR0521_ANA_GAIN_MAX,
+			  AR0521_ANA_GAIN_STEP, AR0521_ANA_GAIN_DEFAULT);
+
+	/* Manual gain */
+	ctrls->gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_GAIN, 0, 511, 1, 0);
+	ctrls->red_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_RED_BALANCE,
+					       -512, 511, 1, 0);
+	ctrls->blue_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BLUE_BALANCE,
+						-512, 511, 1, 0);
+	v4l2_ctrl_cluster(3, &ctrls->gain);
+
+	/* Initialize blanking limits using the default 2592x1944 format. */
+	max_hblank = AR0521_TOTAL_WIDTH_MAX - AR0521_WIDTH_MAX;
+	ctrls->hblank = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HBLANK,
+					  AR0521_WIDTH_BLANKING_MIN,
+					  max_hblank, 1,
+					  AR0521_WIDTH_BLANKING_MIN);
+
+	max_vblank = AR0521_TOTAL_HEIGHT_MAX - AR0521_HEIGHT_MAX;
+	ctrls->vblank = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VBLANK,
+					  AR0521_HEIGHT_BLANKING_MIN,
+					  max_vblank, 2,
+					  AR0521_HEIGHT_BLANKING_MIN);
+	v4l2_ctrl_cluster(2, &ctrls->hblank);
+
+	/* Read-only */
+	ctrls->pixrate = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_PIXEL_RATE,
+					   AR0521_PIXEL_CLOCK_MIN,
+					   AR0521_PIXEL_CLOCK_MAX, 1,
+					   AR0521_PIXEL_CLOCK_RATE);
+
+	/* Manual exposure time: max exposure time = visible + blank - 4 */
+	exposure_max = AR0521_HEIGHT_MAX + AR0521_HEIGHT_BLANKING_MIN - 4;
+	ctrls->exposure = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE, 0,
+					    exposure_max, 1, 0x70);
+
+	link_freq = v4l2_ctrl_new_int_menu(hdl, ops, V4L2_CID_LINK_FREQ,
+					ARRAY_SIZE(ar0521_link_frequencies) - 1,
+					0, ar0521_link_frequencies);
+	if (link_freq)
+		link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	ctrls->test_pattern = v4l2_ctrl_new_std_menu_items(hdl, ops,
+					V4L2_CID_TEST_PATTERN,
+					ARRAY_SIZE(test_pattern_menu) - 1,
+					0, 0, test_pattern_menu);
+
+	if (hdl->error) {
+		ret = hdl->error;
+		goto free_ctrls;
+	}
+
+	sensor->sd.ctrl_handler = hdl;
+	return 0;
+
+free_ctrls:
+	v4l2_ctrl_handler_free(hdl);
+	return ret;
+}
+
+#define REGS_ENTRY(a)	{(a), ARRAY_SIZE(a)}
+#define REGS(...)	REGS_ENTRY(((const __be16[]){__VA_ARGS__}))
+
+static const struct initial_reg {
+	const __be16 *data; /* data[0] is register address */
+	unsigned int count;
+} initial_regs[] = {
+	REGS(be(0x0112), be(0x0808)), /* 8-bit/8-bit mode */
+
+	/* PEDESTAL+2 :+2 is a workaround for 10bit mode +0.5 rounding */
+	REGS(be(0x301E), be(0x00AA)),
+
+	/* corrections_recommended_bayer */
+	REGS(be(0x3042),
+	     be(0x0004),  /* 3042: RNC: enable b/w rnc mode */
+	     be(0x4580)), /* 3044: RNC: enable row noise correction */
+
+	REGS(be(0x30D2),
+	     be(0x0000),  /* 30D2: CRM/CC: enable crm on Visible and CC rows */
+	     be(0x0000),  /* 30D4: CC: CC enabled with 16 samples per column */
+	     /* 30D6: CC: bw mode enabled/12 bit data resolution/bw mode */
+	     be(0x2FFF)),
+
+	REGS(be(0x30DA),
+	     be(0x0FFF),  /* 30DA: CC: column correction clip level 2 is 0 */
+	     be(0x0FFF),  /* 30DC: CC: column correction clip level 3 is 0 */
+	     be(0x0000)), /* 30DE: CC: Group FPN correction */
+
+	/* RNC: rnc scaling factor = * 54 / 64 (32 / 38 * 64 = 53.9) */
+	REGS(be(0x30EE), be(0x1136)),
+	REGS(be(0x30FA), be(0xFD00)), /* GPIO0 = flash, GPIO1 = shutter */
+	REGS(be(0x3120), be(0x0005)), /* p1 dither enabled for 10bit mode */
+	REGS(be(0x3172), be(0x0206)), /* txlo clk divider options */
+	/* FDOC:fdoc settings with fdoc every frame turned of */
+	REGS(be(0x3180), be(0x9434)),
+
+	REGS(be(0x31B0),
+	     be(0x008B),  /* 31B0: frame_preamble - FIXME check WRT lanes# */
+	     be(0x0050)), /* 31B2: line_preamble - FIXME check WRT lanes# */
+
+	/* don't use continuous clock mode while shut down */
+	REGS(be(0x31BC), be(0x068C)),
+	REGS(be(0x31E0), be(0x0781)), /* Fuse/2DDC: enable 2ddc */
+
+	/* analog_setup_recommended_10bit */
+	REGS(be(0x341A), be(0x4735)), /* Samp&Hold pulse in ADC */
+	REGS(be(0x3420), be(0x4735)), /* Samp&Hold pulse in ADC */
+	REGS(be(0x3426), be(0x8A1A)), /* ADC offset distribution pulse */
+	REGS(be(0x342A), be(0x0018)), /* pulse_config */
+
+	/* pixel_timing_recommended */
+	REGS(be(0x3D00),
+	     /* 3D00 */ be(0x043E), be(0x4760), be(0xFFFF), be(0xFFFF),
+	     /* 3D08 */ be(0x8000), be(0x0510), be(0xAF08), be(0x0252),
+	     /* 3D10 */ be(0x486F), be(0x5D5D), be(0x8056), be(0x8313),
+	     /* 3D18 */ be(0x0087), be(0x6A48), be(0x6982), be(0x0280),
+	     /* 3D20 */ be(0x8359), be(0x8D02), be(0x8020), be(0x4882),
+	     /* 3D28 */ be(0x4269), be(0x6A95), be(0x5988), be(0x5A83),
+	     /* 3D30 */ be(0x5885), be(0x6280), be(0x6289), be(0x6097),
+	     /* 3D38 */ be(0x5782), be(0x605C), be(0xBF18), be(0x0961),
+	     /* 3D40 */ be(0x5080), be(0x2090), be(0x4390), be(0x4382),
+	     /* 3D48 */ be(0x5F8A), be(0x5D5D), be(0x9C63), be(0x8063),
+	     /* 3D50 */ be(0xA960), be(0x9757), be(0x8260), be(0x5CFF),
+	     /* 3D58 */ be(0xBF10), be(0x1681), be(0x0802), be(0x8000),
+	     /* 3D60 */ be(0x141C), be(0x6000), be(0x6022), be(0x4D80),
+	     /* 3D68 */ be(0x5C97), be(0x6A69), be(0xAC6F), be(0x4645),
+	     /* 3D70 */ be(0x4400), be(0x0513), be(0x8069), be(0x6AC6),
+	     /* 3D78 */ be(0x5F95), be(0x5F70), be(0x8040), be(0x4A81),
+	     /* 3D80 */ be(0x0300), be(0xE703), be(0x0088), be(0x4A83),
+	     /* 3D88 */ be(0x40FF), be(0xFFFF), be(0xFD70), be(0x8040),
+	     /* 3D90 */ be(0x4A85), be(0x4FA8), be(0x4F8C), be(0x0070),
+	     /* 3D98 */ be(0xBE47), be(0x8847), be(0xBC78), be(0x6B89),
+	     /* 3DA0 */ be(0x6A80), be(0x6986), be(0x6B8E), be(0x6B80),
+	     /* 3DA8 */ be(0x6980), be(0x6A88), be(0x7C9F), be(0x866B),
+	     /* 3DB0 */ be(0x8765), be(0x46FF), be(0xE365), be(0xA679),
+	     /* 3DB8 */ be(0x4A40), be(0x4580), be(0x44BC), be(0x7000),
+	     /* 3DC0 */ be(0x8040), be(0x0802), be(0x10EF), be(0x0104),
+	     /* 3DC8 */ be(0x3860), be(0x5D5D), be(0x5682), be(0x1300),
+	     /* 3DD0 */ be(0x8648), be(0x8202), be(0x8082), be(0x598A),
+	     /* 3DD8 */ be(0x0280), be(0x2048), be(0x3060), be(0x8042),
+	     /* 3DE0 */ be(0x9259), be(0x865A), be(0x8258), be(0x8562),
+	     /* 3DE8 */ be(0x8062), be(0x8560), be(0x9257), be(0x8221),
+	     /* 3DF0 */ be(0x10FF), be(0xB757), be(0x9361), be(0x1019),
+	     /* 3DF8 */ be(0x8020), be(0x9043), be(0x8E43), be(0x845F),
+	     /* 3E00 */ be(0x835D), be(0x805D), be(0x8163), be(0x8063),
+	     /* 3E08 */ be(0xA060), be(0x9157), be(0x8260), be(0x5CFF),
+	     /* 3E10 */ be(0xFFFF), be(0xFFE5), be(0x1016), be(0x2048),
+	     /* 3E18 */ be(0x0802), be(0x1C60), be(0x0014), be(0x0060),
+	     /* 3E20 */ be(0x2205), be(0x8120), be(0x908F), be(0x6A80),
+	     /* 3E28 */ be(0x6982), be(0x5F9F), be(0x6F46), be(0x4544),
+	     /* 3E30 */ be(0x0005), be(0x8013), be(0x8069), be(0x6A80),
+	     /* 3E38 */ be(0x7000), be(0x0000), be(0x0000), be(0x0000),
+	     /* 3E40 */ be(0x0000), be(0x0000), be(0x0000), be(0x0000),
+	     /* 3E48 */ be(0x0000), be(0x0000), be(0x0000), be(0x0000),
+	     /* 3E50 */ be(0x0000), be(0x0000), be(0x0000), be(0x0000),
+	     /* 3E58 */ be(0x0000), be(0x0000), be(0x0000), be(0x0000),
+	     /* 3E60 */ be(0x0000), be(0x0000), be(0x0000), be(0x0000),
+	     /* 3E68 */ be(0x0000), be(0x0000), be(0x0000), be(0x0000),
+	     /* 3E70 */ be(0x0000), be(0x0000), be(0x0000), be(0x0000),
+	     /* 3E78 */ be(0x0000), be(0x0000), be(0x0000), be(0x0000),
+	     /* 3E80 */ be(0x0000), be(0x0000), be(0x0000), be(0x0000),
+	     /* 3E88 */ be(0x0000), be(0x0000), be(0x0000), be(0x0000),
+	     /* 3E90 */ be(0x0000), be(0x0000), be(0x0000), be(0x0000),
+	     /* 3E98 */ be(0x0000), be(0x0000), be(0x0000), be(0x0000),
+	     /* 3EA0 */ be(0x0000), be(0x0000), be(0x0000), be(0x0000),
+	     /* 3EA8 */ be(0x0000), be(0x0000), be(0x0000), be(0x0000),
+	     /* 3EB0 */ be(0x0000), be(0x0000), be(0x0000)),
+
+	REGS(be(0x3EB6), be(0x004C)), /* ECL */
+
+	REGS(be(0x3EBA),
+	     be(0xAAAD),  /* 3EBA */
+	     be(0x0086)), /* 3EBC: Bias currents for FSC/ECL */
+
+	REGS(be(0x3EC0),
+	     be(0x1E00),  /* 3EC0: SFbin/SH mode settings */
+	     be(0x100A),  /* 3EC2: CLK divider for ramp for 10 bit 400MH */
+	     /* 3EC4: FSC clamps for HDR mode and adc comp power down co */
+	     be(0x3300),
+	     be(0xEA44),  /* 3EC6: VLN and clk gating controls */
+	     be(0x6F6F),  /* 3EC8: Txl0 and Txlo1 settings for normal mode */
+	     be(0x2F4A),  /* 3ECA: CDAC/Txlo2/RSTGHI/RSTGLO settings */
+	     be(0x0506),  /* 3ECC: RSTDHI/RSTDLO/CDAC/TXHI settings */
+	     /* 3ECE: Ramp buffer settings and Booster enable (bits 0-5) */
+	     be(0x203B),
+	     be(0x13F0),  /* 3ED0: TXLO from atest/sf bin settings */
+	     be(0xA53D),  /* 3ED2: Ramp offset */
+	     be(0x862F),  /* 3ED4: TXLO open loop/row driver settings */
+	     be(0x4081),  /* 3ED6: Txlatch fr cfpn rows/vln bias */
+	     be(0x8003),  /* 3ED8: Ramp step setting for 10 bit 400 Mhz */
+	     be(0xA580),  /* 3EDA: Ramp Offset */
+	     be(0xC000),  /* 3EDC: over range for rst and under range for sig */
+	     be(0xC103)), /* 3EDE: over range for sig and col dec clk settings */
+
+	/* corrections_recommended_bayer */
+	REGS(be(0x3F00),
+	     be(0x0017),  /* 3F00: BM_T0 */
+	     be(0x02DD),  /* 3F02: BM_T1 */
+	     /* 3F04: if Ana_gain less than 2, use noise_floor0, multipl */
+	     be(0x0020),
+	     /* 3F06: if Ana_gain between 4 and 7, use noise_floor2 and */
+	     be(0x0040),
+	     /* 3F08: if Ana_gain between 4 and 7, use noise_floor2 and */
+	     be(0x0070),
+	     /* 3F0A: Define noise_floor0(low address) and noise_floor1 */
+	     be(0x0101),
+	     be(0x0302)), /* 3F0C: Define noise_floor2 and noise_floor3 */
+
+	REGS(be(0x3F10),
+	     be(0x0505),  /* 3F10: single k factor 0 */
+	     be(0x0505),  /* 3F12: single k factor 1 */
+	     be(0x0505),  /* 3F14: single k factor 2 */
+	     be(0x01FF),  /* 3F16: cross factor 0 */
+	     be(0x01FF),  /* 3F18: cross factor 1 */
+	     be(0x01FF),  /* 3F1A: cross factor 2 */
+	     be(0x0022)), /* 3F1E */
+
+	/* GTH_THRES_RTN: 4max,4min filtered out of every 46 samples and */
+	REGS(be(0x3F2C), be(0x442E)),
+
+	REGS(be(0x3F3E),
+	     be(0x0000),  /* 3F3E: Switch ADC from 12 bit to 10 bit mode */
+	     be(0x1511),  /* 3F40: couple k factor 0 */
+	     be(0x1511),  /* 3F42: couple k factor 1 */
+	     be(0x0707)), /* 3F44: couple k factor 2 */
+};
+
+static int ar0521_power_off(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ar0521_dev *sensor = to_ar0521_dev(sd);
+	int i;
+
+	clk_disable_unprepare(sensor->extclk);
+
+	if (sensor->reset_gpio)
+		gpiod_set_value(sensor->reset_gpio, 1); /* assert RESET signal */
+
+	for (i = ARRAY_SIZE(ar0521_supply_names) - 1; i >= 0; i--) {
+		if (sensor->supplies[i])
+			regulator_disable(sensor->supplies[i]);
+	}
+	return 0;
+}
+
+static int ar0521_power_on(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ar0521_dev *sensor = to_ar0521_dev(sd);
+	unsigned int cnt;
+	int ret;
+
+	for (cnt = 0; cnt < ARRAY_SIZE(ar0521_supply_names); cnt++)
+		if (sensor->supplies[cnt]) {
+			ret = regulator_enable(sensor->supplies[cnt]);
+			if (ret < 0)
+				goto off;
+
+			usleep_range(1000, 1500); /* min 1 ms */
+		}
+
+	ret = clk_prepare_enable(sensor->extclk);
+	if (ret < 0) {
+		v4l2_err(&sensor->sd, "error enabling sensor clock\n");
+		goto off;
+	}
+	usleep_range(1000, 1500); /* min 1 ms */
+
+	if (sensor->reset_gpio)
+		/* deassert RESET signal */
+		gpiod_set_value(sensor->reset_gpio, 0);
+	usleep_range(4500, 5000); /* min 45000 clocks */
+
+	for (cnt = 0; cnt < ARRAY_SIZE(initial_regs); cnt++) {
+		ret = ar0521_write_regs(sensor, initial_regs[cnt].data,
+					initial_regs[cnt].count);
+		if (ret)
+			goto off;
+	}
+
+	ret = ar0521_write_reg(sensor, AR0521_REG_SERIAL_FORMAT,
+			       AR0521_REG_SERIAL_FORMAT_MIPI |
+			       sensor->lane_count);
+	if (ret)
+		goto off;
+
+	/* set MIPI test mode - disabled for now */
+	ret = ar0521_write_reg(sensor, AR0521_REG_HISPI_TEST_MODE,
+			       ((0x40 << sensor->lane_count) - 0x40) |
+			       AR0521_REG_HISPI_TEST_MODE_LP11);
+	if (ret)
+		goto off;
+
+	ret = ar0521_write_reg(sensor, AR0521_REG_ROW_SPEED, 0x110 |
+			       4 / sensor->lane_count);
+	if (ret)
+		goto off;
+
+	return 0;
+off:
+	ar0521_power_off(dev);
+	return ret;
+}
+
+static int ar0521_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct ar0521_dev *sensor = to_ar0521_dev(sd);
+
+	if (code->index)
+		return -EINVAL;
+
+	code->code = sensor->fmt.code;
+	return 0;
+}
+
+static int ar0521_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->index)
+		return -EINVAL;
+
+	if (fse->code != MEDIA_BUS_FMT_SGRBG8_1X8)
+		return -EINVAL;
+
+	fse->min_width = AR0521_WIDTH_MIN;
+	fse->max_width = AR0521_WIDTH_MAX;
+	fse->min_height = AR0521_HEIGHT_MIN;
+	fse->max_height = AR0521_HEIGHT_MAX;
+
+	return 0;
+}
+
+static int ar0521_pre_streamon(struct v4l2_subdev *sd, u32 flags)
+{
+	struct ar0521_dev *sensor = to_ar0521_dev(sd);
+	int ret;
+
+	if (!(flags & V4L2_SUBDEV_PRE_STREAMON_FL_MANUAL_LP))
+		return -EACCES;
+
+	ret = pm_runtime_resume_and_get(&sensor->i2c_client->dev);
+	if (ret < 0)
+		return ret;
+
+	/* Set LP-11 on clock and data lanes */
+	ret = ar0521_write_reg(sensor, AR0521_REG_HISPI_CONTROL_STATUS,
+			AR0521_REG_HISPI_CONTROL_STATUS_FRAMER_TEST_MODE_ENABLE);
+	if (ret)
+		goto err;
+
+	/* Start streaming LP-11 */
+	ret = ar0521_write_reg(sensor, AR0521_REG_RESET,
+			       AR0521_REG_RESET_DEFAULTS |
+			       AR0521_REG_RESET_STREAM);
+	if (ret)
+		goto err;
+	return 0;
+
+err:
+	pm_runtime_put(&sensor->i2c_client->dev);
+	return ret;
+}
+
+static int ar0521_post_streamoff(struct v4l2_subdev *sd)
+{
+	struct ar0521_dev *sensor = to_ar0521_dev(sd);
+
+	pm_runtime_put(&sensor->i2c_client->dev);
+	return 0;
+}
+
+static int ar0521_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct ar0521_dev *sensor = to_ar0521_dev(sd);
+	int ret;
+
+	mutex_lock(&sensor->lock);
+
+	ret = ar0521_set_stream(sensor, enable);
+	if (!ret)
+		sensor->streaming = enable;
+
+	mutex_unlock(&sensor->lock);
+	return ret;
+}
+
+static const struct v4l2_subdev_core_ops ar0521_core_ops = {
+	.log_status = v4l2_ctrl_subdev_log_status,
+};
+
+static const struct v4l2_subdev_video_ops ar0521_video_ops = {
+	.s_stream = ar0521_s_stream,
+	.pre_streamon = ar0521_pre_streamon,
+	.post_streamoff = ar0521_post_streamoff,
+};
+
+static const struct v4l2_subdev_pad_ops ar0521_pad_ops = {
+	.enum_mbus_code = ar0521_enum_mbus_code,
+	.enum_frame_size = ar0521_enum_frame_size,
+	.get_fmt = ar0521_get_fmt,
+	.set_fmt = ar0521_set_fmt,
+};
+
+static const struct v4l2_subdev_ops ar0521_subdev_ops = {
+	.core = &ar0521_core_ops,
+	.video = &ar0521_video_ops,
+	.pad = &ar0521_pad_ops,
+};
+
+static int __maybe_unused ar0521_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ar0521_dev *sensor = to_ar0521_dev(sd);
+
+	if (sensor->streaming)
+		ar0521_set_stream(sensor, 0);
+
+	return 0;
+}
+
+static int __maybe_unused ar0521_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ar0521_dev *sensor = to_ar0521_dev(sd);
+
+	if (sensor->streaming)
+		return ar0521_set_stream(sensor, 1);
+
+	return 0;
+}
+
+static int ar0521_probe(struct i2c_client *client)
+{
+	struct v4l2_fwnode_endpoint ep = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY
+	};
+	struct device *dev = &client->dev;
+	struct fwnode_handle *endpoint;
+	struct ar0521_dev *sensor;
+	unsigned int cnt;
+	int ret;
+
+	sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
+	if (!sensor)
+		return -ENOMEM;
+
+	sensor->i2c_client = client;
+	sensor->fmt.width = AR0521_WIDTH_MAX;
+	sensor->fmt.height = AR0521_HEIGHT_MAX;
+
+	endpoint = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev), 0, 0,
+						   FWNODE_GRAPH_ENDPOINT_NEXT);
+	if (!endpoint) {
+		dev_err(dev, "endpoint node not found\n");
+		return -EINVAL;
+	}
+
+	ret = v4l2_fwnode_endpoint_parse(endpoint, &ep);
+	fwnode_handle_put(endpoint);
+	if (ret) {
+		dev_err(dev, "could not parse endpoint\n");
+		return ret;
+	}
+
+	if (ep.bus_type != V4L2_MBUS_CSI2_DPHY) {
+		dev_err(dev, "invalid bus type, must be MIPI CSI2\n");
+		return -EINVAL;
+	}
+
+	sensor->lane_count = ep.bus.mipi_csi2.num_data_lanes;
+	switch (sensor->lane_count) {
+	case 1:
+	case 2:
+	case 4:
+		break;
+	default:
+		dev_err(dev, "invalid number of MIPI data lanes\n");
+		return -EINVAL;
+	}
+
+	/* Get master clock (extclk) */
+	sensor->extclk = devm_clk_get(dev, "extclk");
+	if (IS_ERR(sensor->extclk)) {
+		dev_err(dev, "failed to get extclk\n");
+		return PTR_ERR(sensor->extclk);
+	}
+
+	sensor->extclk_freq = clk_get_rate(sensor->extclk);
+
+	if (sensor->extclk_freq < AR0521_EXTCLK_MIN ||
+	    sensor->extclk_freq > AR0521_EXTCLK_MAX) {
+		dev_err(dev, "extclk frequency out of range: %u Hz\n",
+			sensor->extclk_freq);
+		return -EINVAL;
+	}
+
+	/* Request optional reset pin (usually active low) and assert it */
+	sensor->reset_gpio = devm_gpiod_get_optional(dev, "reset",
+						     GPIOD_OUT_HIGH);
+
+	v4l2_i2c_subdev_init(&sensor->sd, client, &ar0521_subdev_ops);
+
+	sensor->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
+	sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
+	sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret = media_entity_pads_init(&sensor->sd.entity, 1, &sensor->pad);
+	if (ret)
+		return ret;
+
+	for (cnt = 0; cnt < ARRAY_SIZE(ar0521_supply_names); cnt++) {
+		struct regulator *supply = devm_regulator_get(dev,
+						ar0521_supply_names[cnt]);
+
+		if (IS_ERR(supply)) {
+			dev_info(dev, "no %s regulator found: %li\n",
+				 ar0521_supply_names[cnt], PTR_ERR(supply));
+			return PTR_ERR(supply);
+		}
+		sensor->supplies[cnt] = supply;
+	}
+
+	mutex_init(&sensor->lock);
+
+	ret = ar0521_init_controls(sensor);
+	if (ret)
+		goto entity_cleanup;
+
+	ar0521_adj_fmt(&sensor->fmt);
+
+	ret = v4l2_async_register_subdev(&sensor->sd);
+	if (ret)
+		goto free_ctrls;
+
+	/* Turn on the device and enable runtime PM */
+	ret = ar0521_power_on(&client->dev);
+	if (ret)
+		goto disable;
+	pm_runtime_set_active(&client->dev);
+	pm_runtime_enable(&client->dev);
+	pm_runtime_idle(&client->dev);
+	return 0;
+
+disable:
+	v4l2_async_unregister_subdev(&sensor->sd);
+	media_entity_cleanup(&sensor->sd.entity);
+free_ctrls:
+	v4l2_ctrl_handler_free(&sensor->ctrls.handler);
+entity_cleanup:
+	media_entity_cleanup(&sensor->sd.entity);
+	mutex_destroy(&sensor->lock);
+	return ret;
+}
+
+static void ar0521_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ar0521_dev *sensor = to_ar0521_dev(sd);
+
+	v4l2_async_unregister_subdev(&sensor->sd);
+	media_entity_cleanup(&sensor->sd.entity);
+	v4l2_ctrl_handler_free(&sensor->ctrls.handler);
+	pm_runtime_disable(&client->dev);
+	if (!pm_runtime_status_suspended(&client->dev))
+		ar0521_power_off(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+	mutex_destroy(&sensor->lock);
+}
+
+static const struct dev_pm_ops ar0521_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(ar0521_suspend, ar0521_resume)
+	SET_RUNTIME_PM_OPS(ar0521_power_off, ar0521_power_on, NULL)
+};
+static const struct of_device_id ar0521_dt_ids[] = {
+	{.compatible = "onnn,ar0521"},
+	{}
+};
+MODULE_DEVICE_TABLE(of, ar0521_dt_ids);
+
+static struct i2c_driver ar0521_i2c_driver = {
+	.driver = {
+		.name  = "ar0521",
+		.pm = &ar0521_pm_ops,
+		.of_match_table = ar0521_dt_ids,
+	},
+	.probe = ar0521_probe,
+	.remove = ar0521_remove,
+};
+
+module_i2c_driver(ar0521_i2c_driver);
+
+MODULE_DESCRIPTION("AR0521 MIPI Camera subdev driver");
+MODULE_AUTHOR("Krzysztof Hałasa <khalasa@piap.pl>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/bt819.c b/drivers/media/i2c/bt819.c
new file mode 100644
index 000000000..b4a25cc99
--- /dev/null
+++ b/drivers/media/i2c/bt819.c
@@ -0,0 +1,476 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ *  bt819 - BT819A VideoStream Decoder (Rockwell Part)
+ *
+ * Copyright (C) 1999 Mike Bernson <mike@mlb.org>
+ * Copyright (C) 1998 Dave Perks <dperks@ibm.net>
+ *
+ * Modifications for LML33/DC10plus unified driver
+ * Copyright (C) 2000 Serguei Miridonov <mirsev@cicese.mx>
+ *
+ * Changes by Ronald Bultje <rbultje@ronald.bitfreak.net>
+ *    - moved over to linux>=2.4.x i2c protocol (9/9/2002)
+ *
+ * This code was modify/ported from the saa7111 driver written
+ * by Dave Perks.
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/ioctl.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <linux/slab.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <media/i2c/bt819.h>
+
+MODULE_DESCRIPTION("Brooktree-819 video decoder driver");
+MODULE_AUTHOR("Mike Bernson & Dave Perks");
+MODULE_LICENSE("GPL");
+
+static int debug;
+module_param(debug, int, 0);
+MODULE_PARM_DESC(debug, "Debug level (0-1)");
+
+
+/* ----------------------------------------------------------------------- */
+
+struct bt819 {
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
+	unsigned char reg[32];
+
+	v4l2_std_id norm;
+	int input;
+	int enable;
+};
+
+static inline struct bt819 *to_bt819(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct bt819, sd);
+}
+
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct bt819, hdl)->sd;
+}
+
+struct timing {
+	int hactive;
+	int hdelay;
+	int vactive;
+	int vdelay;
+	int hscale;
+	int vscale;
+};
+
+/* for values, see the bt819 datasheet */
+static struct timing timing_data[] = {
+	{864 - 24, 20, 625 - 2, 1, 0x0504, 0x0000},
+	{858 - 24, 20, 525 - 2, 1, 0x00f8, 0x0000},
+};
+
+/* ----------------------------------------------------------------------- */
+
+static inline int bt819_write(struct bt819 *decoder, u8 reg, u8 value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&decoder->sd);
+
+	decoder->reg[reg] = value;
+	return i2c_smbus_write_byte_data(client, reg, value);
+}
+
+static inline int bt819_setbit(struct bt819 *decoder, u8 reg, u8 bit, u8 value)
+{
+	return bt819_write(decoder, reg,
+		(decoder->reg[reg] & ~(1 << bit)) | (value ? (1 << bit) : 0));
+}
+
+static int bt819_write_block(struct bt819 *decoder, const u8 *data, unsigned int len)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&decoder->sd);
+	int ret = -1;
+	u8 reg;
+
+	/* the bt819 has an autoincrement function, use it if
+	 * the adapter understands raw I2C */
+	if (i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		/* do raw I2C, not smbus compatible */
+		u8 block_data[32];
+		int block_len;
+
+		while (len >= 2) {
+			block_len = 0;
+			block_data[block_len++] = reg = data[0];
+			do {
+				block_data[block_len++] =
+				    decoder->reg[reg++] = data[1];
+				len -= 2;
+				data += 2;
+			} while (len >= 2 && data[0] == reg && block_len < 32);
+			ret = i2c_master_send(client, block_data, block_len);
+			if (ret < 0)
+				break;
+		}
+	} else {
+		/* do some slow I2C emulation kind of thing */
+		while (len >= 2) {
+			reg = *data++;
+			ret = bt819_write(decoder, reg, *data++);
+			if (ret < 0)
+				break;
+			len -= 2;
+		}
+	}
+
+	return ret;
+}
+
+static inline int bt819_read(struct bt819 *decoder, u8 reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&decoder->sd);
+
+	return i2c_smbus_read_byte_data(client, reg);
+}
+
+static int bt819_init(struct v4l2_subdev *sd)
+{
+	static unsigned char init[] = {
+		/*0x1f, 0x00,*/     /* Reset */
+		0x01, 0x59,	/* 0x01 input format */
+		0x02, 0x00,	/* 0x02 temporal decimation */
+		0x03, 0x12,	/* 0x03 Cropping msb */
+		0x04, 0x16,	/* 0x04 Vertical Delay, lsb */
+		0x05, 0xe0,	/* 0x05 Vertical Active lsb */
+		0x06, 0x80,	/* 0x06 Horizontal Delay lsb */
+		0x07, 0xd0,	/* 0x07 Horizontal Active lsb */
+		0x08, 0x00,	/* 0x08 Horizontal Scaling msb */
+		0x09, 0xf8,	/* 0x09 Horizontal Scaling lsb */
+		0x0a, 0x00,	/* 0x0a Brightness control */
+		0x0b, 0x30,	/* 0x0b Miscellaneous control */
+		0x0c, 0xd8,	/* 0x0c Luma Gain lsb */
+		0x0d, 0xfe,	/* 0x0d Chroma Gain (U) lsb */
+		0x0e, 0xb4,	/* 0x0e Chroma Gain (V) msb */
+		0x0f, 0x00,	/* 0x0f Hue control */
+		0x12, 0x04,	/* 0x12 Output Format */
+		0x13, 0x20,	/* 0x13 Vertical Scaling msb 0x00
+					   chroma comb OFF, line drop scaling, interlace scaling
+					   BUG? Why does turning the chroma comb on screw up color?
+					   Bug in the bt819 stepping on my board?
+					*/
+		0x14, 0x00,	/* 0x14 Vertical Scaling lsb */
+		0x16, 0x07,	/* 0x16 Video Timing Polarity
+					   ACTIVE=active low
+					   FIELD: high=odd,
+					   vreset=active high,
+					   hreset=active high */
+		0x18, 0x68,	/* 0x18 AGC Delay */
+		0x19, 0x5d,	/* 0x19 Burst Gate Delay */
+		0x1a, 0x80,	/* 0x1a ADC Interface */
+	};
+
+	struct bt819 *decoder = to_bt819(sd);
+	struct timing *timing = &timing_data[(decoder->norm & V4L2_STD_525_60) ? 1 : 0];
+
+	init[0x03 * 2 - 1] =
+	    (((timing->vdelay >> 8) & 0x03) << 6) |
+	    (((timing->vactive >> 8) & 0x03) << 4) |
+	    (((timing->hdelay >> 8) & 0x03) << 2) |
+	    ((timing->hactive >> 8) & 0x03);
+	init[0x04 * 2 - 1] = timing->vdelay & 0xff;
+	init[0x05 * 2 - 1] = timing->vactive & 0xff;
+	init[0x06 * 2 - 1] = timing->hdelay & 0xff;
+	init[0x07 * 2 - 1] = timing->hactive & 0xff;
+	init[0x08 * 2 - 1] = timing->hscale >> 8;
+	init[0x09 * 2 - 1] = timing->hscale & 0xff;
+	/* 0x15 in array is address 0x19 */
+	init[0x15 * 2 - 1] = (decoder->norm & V4L2_STD_625_50) ? 115 : 93;	/* Chroma burst delay */
+	/* reset */
+	bt819_write(decoder, 0x1f, 0x00);
+	mdelay(1);
+
+	/* init */
+	return bt819_write_block(decoder, init, sizeof(init));
+}
+
+/* ----------------------------------------------------------------------- */
+
+static int bt819_status(struct v4l2_subdev *sd, u32 *pstatus, v4l2_std_id *pstd)
+{
+	struct bt819 *decoder = to_bt819(sd);
+	int status = bt819_read(decoder, 0x00);
+	int res = V4L2_IN_ST_NO_SIGNAL;
+	v4l2_std_id std = pstd ? *pstd : V4L2_STD_ALL;
+
+	if ((status & 0x80))
+		res = 0;
+	else
+		std = V4L2_STD_UNKNOWN;
+
+	if ((status & 0x10))
+		std &= V4L2_STD_PAL;
+	else
+		std &= V4L2_STD_NTSC;
+	if (pstd)
+		*pstd = std;
+	if (pstatus)
+		*pstatus = res;
+
+	v4l2_dbg(1, debug, sd, "get status %x\n", status);
+	return 0;
+}
+
+static int bt819_querystd(struct v4l2_subdev *sd, v4l2_std_id *std)
+{
+	return bt819_status(sd, NULL, std);
+}
+
+static int bt819_g_input_status(struct v4l2_subdev *sd, u32 *status)
+{
+	return bt819_status(sd, status, NULL);
+}
+
+static int bt819_s_std(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct bt819 *decoder = to_bt819(sd);
+	struct timing *timing = NULL;
+
+	v4l2_dbg(1, debug, sd, "set norm %llx\n", (unsigned long long)std);
+
+	if (sd->v4l2_dev == NULL || sd->v4l2_dev->notify == NULL)
+		v4l2_err(sd, "no notify found!\n");
+
+	if (std & V4L2_STD_NTSC) {
+		v4l2_subdev_notify(sd, BT819_FIFO_RESET_LOW, NULL);
+		bt819_setbit(decoder, 0x01, 0, 1);
+		bt819_setbit(decoder, 0x01, 1, 0);
+		bt819_setbit(decoder, 0x01, 5, 0);
+		bt819_write(decoder, 0x18, 0x68);
+		bt819_write(decoder, 0x19, 0x5d);
+		/* bt819_setbit(decoder, 0x1a,  5, 1); */
+		timing = &timing_data[1];
+	} else if (std & V4L2_STD_PAL) {
+		v4l2_subdev_notify(sd, BT819_FIFO_RESET_LOW, NULL);
+		bt819_setbit(decoder, 0x01, 0, 1);
+		bt819_setbit(decoder, 0x01, 1, 1);
+		bt819_setbit(decoder, 0x01, 5, 1);
+		bt819_write(decoder, 0x18, 0x7f);
+		bt819_write(decoder, 0x19, 0x72);
+		/* bt819_setbit(decoder, 0x1a,  5, 0); */
+		timing = &timing_data[0];
+	} else {
+		v4l2_dbg(1, debug, sd, "unsupported norm %llx\n",
+				(unsigned long long)std);
+		return -EINVAL;
+	}
+	bt819_write(decoder, 0x03,
+			(((timing->vdelay >> 8) & 0x03) << 6) |
+			(((timing->vactive >> 8) & 0x03) << 4) |
+			(((timing->hdelay >> 8) & 0x03) << 2) |
+			((timing->hactive >> 8) & 0x03));
+	bt819_write(decoder, 0x04, timing->vdelay & 0xff);
+	bt819_write(decoder, 0x05, timing->vactive & 0xff);
+	bt819_write(decoder, 0x06, timing->hdelay & 0xff);
+	bt819_write(decoder, 0x07, timing->hactive & 0xff);
+	bt819_write(decoder, 0x08, (timing->hscale >> 8) & 0xff);
+	bt819_write(decoder, 0x09, timing->hscale & 0xff);
+	decoder->norm = std;
+	v4l2_subdev_notify(sd, BT819_FIFO_RESET_HIGH, NULL);
+	return 0;
+}
+
+static int bt819_s_routing(struct v4l2_subdev *sd,
+			   u32 input, u32 output, u32 config)
+{
+	struct bt819 *decoder = to_bt819(sd);
+
+	v4l2_dbg(1, debug, sd, "set input %x\n", input);
+
+	if (input > 7)
+		return -EINVAL;
+
+	if (sd->v4l2_dev == NULL || sd->v4l2_dev->notify == NULL)
+		v4l2_err(sd, "no notify found!\n");
+
+	if (decoder->input != input) {
+		v4l2_subdev_notify(sd, BT819_FIFO_RESET_LOW, NULL);
+		decoder->input = input;
+		/* select mode */
+		if (decoder->input == 0) {
+			bt819_setbit(decoder, 0x0b, 6, 0);
+			bt819_setbit(decoder, 0x1a, 1, 1);
+		} else {
+			bt819_setbit(decoder, 0x0b, 6, 1);
+			bt819_setbit(decoder, 0x1a, 1, 0);
+		}
+		v4l2_subdev_notify(sd, BT819_FIFO_RESET_HIGH, NULL);
+	}
+	return 0;
+}
+
+static int bt819_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct bt819 *decoder = to_bt819(sd);
+
+	v4l2_dbg(1, debug, sd, "enable output %x\n", enable);
+
+	if (decoder->enable != enable) {
+		decoder->enable = enable;
+		bt819_setbit(decoder, 0x16, 7, !enable);
+	}
+	return 0;
+}
+
+static int bt819_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+	struct bt819 *decoder = to_bt819(sd);
+	int temp;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		bt819_write(decoder, 0x0a, ctrl->val);
+		break;
+
+	case V4L2_CID_CONTRAST:
+		bt819_write(decoder, 0x0c, ctrl->val & 0xff);
+		bt819_setbit(decoder, 0x0b, 2, ((ctrl->val >> 8) & 0x01));
+		break;
+
+	case V4L2_CID_SATURATION:
+		bt819_write(decoder, 0x0d, (ctrl->val >> 7) & 0xff);
+		bt819_setbit(decoder, 0x0b, 1, ((ctrl->val >> 15) & 0x01));
+
+		/* Ratio between U gain and V gain must stay the same as
+		   the ratio between the default U and V gain values. */
+		temp = (ctrl->val * 180) / 254;
+		bt819_write(decoder, 0x0e, (temp >> 7) & 0xff);
+		bt819_setbit(decoder, 0x0b, 0, (temp >> 15) & 0x01);
+		break;
+
+	case V4L2_CID_HUE:
+		bt819_write(decoder, 0x0f, ctrl->val);
+		break;
+
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_ctrl_ops bt819_ctrl_ops = {
+	.s_ctrl = bt819_s_ctrl,
+};
+
+static const struct v4l2_subdev_video_ops bt819_video_ops = {
+	.s_std = bt819_s_std,
+	.s_routing = bt819_s_routing,
+	.s_stream = bt819_s_stream,
+	.querystd = bt819_querystd,
+	.g_input_status = bt819_g_input_status,
+};
+
+static const struct v4l2_subdev_ops bt819_ops = {
+	.video = &bt819_video_ops,
+};
+
+/* ----------------------------------------------------------------------- */
+
+static int bt819_probe(struct i2c_client *client)
+{
+	int i, ver;
+	struct bt819 *decoder;
+	struct v4l2_subdev *sd;
+	const char *name;
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -ENODEV;
+
+	decoder = devm_kzalloc(&client->dev, sizeof(*decoder), GFP_KERNEL);
+	if (decoder == NULL)
+		return -ENOMEM;
+	sd = &decoder->sd;
+	v4l2_i2c_subdev_init(sd, client, &bt819_ops);
+
+	ver = bt819_read(decoder, 0x17);
+	switch (ver & 0xf0) {
+	case 0x70:
+		name = "bt819a";
+		break;
+	case 0x60:
+		name = "bt817a";
+		break;
+	case 0x20:
+		name = "bt815a";
+		break;
+	default:
+		v4l2_dbg(1, debug, sd,
+			"unknown chip version 0x%02x\n", ver);
+		return -ENODEV;
+	}
+
+	v4l_info(client, "%s found @ 0x%x (%s)\n", name,
+			client->addr << 1, client->adapter->name);
+
+	decoder->norm = V4L2_STD_NTSC;
+	decoder->input = 0;
+	decoder->enable = 1;
+
+	i = bt819_init(sd);
+	if (i < 0)
+		v4l2_dbg(1, debug, sd, "init status %d\n", i);
+
+	v4l2_ctrl_handler_init(&decoder->hdl, 4);
+	v4l2_ctrl_new_std(&decoder->hdl, &bt819_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, -128, 127, 1, 0);
+	v4l2_ctrl_new_std(&decoder->hdl, &bt819_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 511, 1, 0xd8);
+	v4l2_ctrl_new_std(&decoder->hdl, &bt819_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 511, 1, 0xfe);
+	v4l2_ctrl_new_std(&decoder->hdl, &bt819_ctrl_ops,
+			V4L2_CID_HUE, -128, 127, 1, 0);
+	sd->ctrl_handler = &decoder->hdl;
+	if (decoder->hdl.error) {
+		int err = decoder->hdl.error;
+
+		v4l2_ctrl_handler_free(&decoder->hdl);
+		return err;
+	}
+	v4l2_ctrl_handler_setup(&decoder->hdl);
+	return 0;
+}
+
+static void bt819_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct bt819 *decoder = to_bt819(sd);
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&decoder->hdl);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id bt819_id[] = {
+	{ "bt819a", 0 },
+	{ "bt817a", 0 },
+	{ "bt815a", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, bt819_id);
+
+static struct i2c_driver bt819_driver = {
+	.driver = {
+		.name	= "bt819",
+	},
+	.probe		= bt819_probe,
+	.remove		= bt819_remove,
+	.id_table	= bt819_id,
+};
+
+module_i2c_driver(bt819_driver);
diff --git a/drivers/media/i2c/bt856.c b/drivers/media/i2c/bt856.c
new file mode 100644
index 000000000..814acbd6a
--- /dev/null
+++ b/drivers/media/i2c/bt856.c
@@ -0,0 +1,247 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * bt856 - BT856A Digital Video Encoder (Rockwell Part)
+ *
+ * Copyright (C) 1999 Mike Bernson <mike@mlb.org>
+ * Copyright (C) 1998 Dave Perks <dperks@ibm.net>
+ *
+ * Modifications for LML33/DC10plus unified driver
+ * Copyright (C) 2000 Serguei Miridonov <mirsev@cicese.mx>
+ *
+ * This code was modify/ported from the saa7111 driver written
+ * by Dave Perks.
+ *
+ * Changes by Ronald Bultje <rbultje@ronald.bitfreak.net>
+ *   - moved over to linux>=2.4.x i2c protocol (9/9/2002)
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <linux/ioctl.h>
+#include <linux/uaccess.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+
+MODULE_DESCRIPTION("Brooktree-856A video encoder driver");
+MODULE_AUTHOR("Mike Bernson & Dave Perks");
+MODULE_LICENSE("GPL");
+
+static int debug;
+module_param(debug, int, 0);
+MODULE_PARM_DESC(debug, "Debug level (0-1)");
+
+
+/* ----------------------------------------------------------------------- */
+
+#define BT856_REG_OFFSET	0xDA
+#define BT856_NR_REG		6
+
+struct bt856 {
+	struct v4l2_subdev sd;
+	unsigned char reg[BT856_NR_REG];
+
+	v4l2_std_id norm;
+};
+
+static inline struct bt856 *to_bt856(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct bt856, sd);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static inline int bt856_write(struct bt856 *encoder, u8 reg, u8 value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&encoder->sd);
+
+	encoder->reg[reg - BT856_REG_OFFSET] = value;
+	return i2c_smbus_write_byte_data(client, reg, value);
+}
+
+static inline int bt856_setbit(struct bt856 *encoder, u8 reg, u8 bit, u8 value)
+{
+	return bt856_write(encoder, reg,
+		(encoder->reg[reg - BT856_REG_OFFSET] & ~(1 << bit)) |
+				(value ? (1 << bit) : 0));
+}
+
+static void bt856_dump(struct bt856 *encoder)
+{
+	int i;
+
+	v4l2_info(&encoder->sd, "register dump:\n");
+	for (i = 0; i < BT856_NR_REG; i += 2)
+		printk(KERN_CONT " %02x", encoder->reg[i]);
+	printk(KERN_CONT "\n");
+}
+
+/* ----------------------------------------------------------------------- */
+
+static int bt856_init(struct v4l2_subdev *sd, u32 arg)
+{
+	struct bt856 *encoder = to_bt856(sd);
+
+	/* This is just for testing!!! */
+	v4l2_dbg(1, debug, sd, "init\n");
+	bt856_write(encoder, 0xdc, 0x18);
+	bt856_write(encoder, 0xda, 0);
+	bt856_write(encoder, 0xde, 0);
+
+	bt856_setbit(encoder, 0xdc, 3, 1);
+	/*bt856_setbit(encoder, 0xdc, 6, 0);*/
+	bt856_setbit(encoder, 0xdc, 4, 1);
+
+	if (encoder->norm & V4L2_STD_NTSC)
+		bt856_setbit(encoder, 0xdc, 2, 0);
+	else
+		bt856_setbit(encoder, 0xdc, 2, 1);
+
+	bt856_setbit(encoder, 0xdc, 1, 1);
+	bt856_setbit(encoder, 0xde, 4, 0);
+	bt856_setbit(encoder, 0xde, 3, 1);
+	if (debug != 0)
+		bt856_dump(encoder);
+	return 0;
+}
+
+static int bt856_s_std_output(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct bt856 *encoder = to_bt856(sd);
+
+	v4l2_dbg(1, debug, sd, "set norm %llx\n", (unsigned long long)std);
+
+	if (std & V4L2_STD_NTSC) {
+		bt856_setbit(encoder, 0xdc, 2, 0);
+	} else if (std & V4L2_STD_PAL) {
+		bt856_setbit(encoder, 0xdc, 2, 1);
+		bt856_setbit(encoder, 0xda, 0, 0);
+		/*bt856_setbit(encoder, 0xda, 0, 1);*/
+	} else {
+		return -EINVAL;
+	}
+	encoder->norm = std;
+	if (debug != 0)
+		bt856_dump(encoder);
+	return 0;
+}
+
+static int bt856_s_routing(struct v4l2_subdev *sd,
+			   u32 input, u32 output, u32 config)
+{
+	struct bt856 *encoder = to_bt856(sd);
+
+	v4l2_dbg(1, debug, sd, "set input %d\n", input);
+
+	/* We only have video bus.
+	 * input= 0: input is from bt819
+	 * input= 1: input is from ZR36060 */
+	switch (input) {
+	case 0:
+		bt856_setbit(encoder, 0xde, 4, 0);
+		bt856_setbit(encoder, 0xde, 3, 1);
+		bt856_setbit(encoder, 0xdc, 3, 1);
+		bt856_setbit(encoder, 0xdc, 6, 0);
+		break;
+	case 1:
+		bt856_setbit(encoder, 0xde, 4, 0);
+		bt856_setbit(encoder, 0xde, 3, 1);
+		bt856_setbit(encoder, 0xdc, 3, 1);
+		bt856_setbit(encoder, 0xdc, 6, 1);
+		break;
+	case 2:	/* Color bar */
+		bt856_setbit(encoder, 0xdc, 3, 0);
+		bt856_setbit(encoder, 0xde, 4, 1);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (debug != 0)
+		bt856_dump(encoder);
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_core_ops bt856_core_ops = {
+	.init = bt856_init,
+};
+
+static const struct v4l2_subdev_video_ops bt856_video_ops = {
+	.s_std_output = bt856_s_std_output,
+	.s_routing = bt856_s_routing,
+};
+
+static const struct v4l2_subdev_ops bt856_ops = {
+	.core = &bt856_core_ops,
+	.video = &bt856_video_ops,
+};
+
+/* ----------------------------------------------------------------------- */
+
+static int bt856_probe(struct i2c_client *client)
+{
+	struct bt856 *encoder;
+	struct v4l2_subdev *sd;
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -ENODEV;
+
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	encoder = devm_kzalloc(&client->dev, sizeof(*encoder), GFP_KERNEL);
+	if (encoder == NULL)
+		return -ENOMEM;
+	sd = &encoder->sd;
+	v4l2_i2c_subdev_init(sd, client, &bt856_ops);
+	encoder->norm = V4L2_STD_NTSC;
+
+	bt856_write(encoder, 0xdc, 0x18);
+	bt856_write(encoder, 0xda, 0);
+	bt856_write(encoder, 0xde, 0);
+
+	bt856_setbit(encoder, 0xdc, 3, 1);
+	/*bt856_setbit(encoder, 0xdc, 6, 0);*/
+	bt856_setbit(encoder, 0xdc, 4, 1);
+
+	if (encoder->norm & V4L2_STD_NTSC)
+		bt856_setbit(encoder, 0xdc, 2, 0);
+	else
+		bt856_setbit(encoder, 0xdc, 2, 1);
+
+	bt856_setbit(encoder, 0xdc, 1, 1);
+	bt856_setbit(encoder, 0xde, 4, 0);
+	bt856_setbit(encoder, 0xde, 3, 1);
+
+	if (debug != 0)
+		bt856_dump(encoder);
+	return 0;
+}
+
+static void bt856_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+}
+
+static const struct i2c_device_id bt856_id[] = {
+	{ "bt856", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, bt856_id);
+
+static struct i2c_driver bt856_driver = {
+	.driver = {
+		.name	= "bt856",
+	},
+	.probe		= bt856_probe,
+	.remove		= bt856_remove,
+	.id_table	= bt856_id,
+};
+
+module_i2c_driver(bt856_driver);
diff --git a/drivers/media/i2c/bt866.c b/drivers/media/i2c/bt866.c
new file mode 100644
index 000000000..dada059cb
--- /dev/null
+++ b/drivers/media/i2c/bt866.c
@@ -0,0 +1,214 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+    bt866 - BT866 Digital Video Encoder (Rockwell Part)
+
+    Copyright (C) 1999 Mike Bernson <mike@mlb.org>
+    Copyright (C) 1998 Dave Perks <dperks@ibm.net>
+
+    Modifications for LML33/DC10plus unified driver
+    Copyright (C) 2000 Serguei Miridonov <mirsev@cicese.mx>
+
+    This code was modify/ported from the saa7111 driver written
+    by Dave Perks.
+
+    This code was adapted for the bt866 by Christer Weinigel and ported
+    to 2.6 by Martin Samuelsson.
+
+*/
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <linux/ioctl.h>
+#include <linux/uaccess.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+
+MODULE_DESCRIPTION("Brooktree-866 video encoder driver");
+MODULE_AUTHOR("Mike Bernson & Dave Perks");
+MODULE_LICENSE("GPL");
+
+static int debug;
+module_param(debug, int, 0);
+MODULE_PARM_DESC(debug, "Debug level (0-1)");
+
+
+/* ----------------------------------------------------------------------- */
+
+struct bt866 {
+	struct v4l2_subdev sd;
+	u8 reg[256];
+};
+
+static inline struct bt866 *to_bt866(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct bt866, sd);
+}
+
+static int bt866_write(struct bt866 *encoder, u8 subaddr, u8 data)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&encoder->sd);
+	u8 buffer[2];
+	int err;
+
+	buffer[0] = subaddr;
+	buffer[1] = data;
+
+	encoder->reg[subaddr] = data;
+
+	v4l_dbg(1, debug, client, "write 0x%02x = 0x%02x\n", subaddr, data);
+
+	for (err = 0; err < 3;) {
+		if (i2c_master_send(client, buffer, 2) == 2)
+			break;
+		err++;
+		v4l_warn(client, "error #%d writing to 0x%02x\n",
+				err, subaddr);
+		schedule_timeout_interruptible(msecs_to_jiffies(100));
+	}
+	if (err == 3) {
+		v4l_warn(client, "giving up\n");
+		return -1;
+	}
+
+	return 0;
+}
+
+static int bt866_s_std_output(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	v4l2_dbg(1, debug, sd, "set norm %llx\n", (unsigned long long)std);
+
+	/* Only PAL supported by this driver at the moment! */
+	if (!(std & V4L2_STD_NTSC))
+		return -EINVAL;
+	return 0;
+}
+
+static int bt866_s_routing(struct v4l2_subdev *sd,
+			   u32 input, u32 output, u32 config)
+{
+	static const __u8 init[] = {
+		0xc8, 0xcc, /* CRSCALE */
+		0xca, 0x91, /* CBSCALE */
+		0xcc, 0x24, /* YC16 | OSDNUM */
+		0xda, 0x00, /*  */
+		0xdc, 0x24, /* SETMODE | PAL */
+		0xde, 0x02, /* EACTIVE */
+
+		/* overlay colors */
+		0x70, 0xEB, 0x90, 0x80, 0xB0, 0x80, /* white */
+		0x72, 0xA2, 0x92, 0x8E, 0xB2, 0x2C, /* yellow */
+		0x74, 0x83, 0x94, 0x2C, 0xB4, 0x9C, /* cyan */
+		0x76, 0x70, 0x96, 0x3A, 0xB6, 0x48, /* green */
+		0x78, 0x54, 0x98, 0xC6, 0xB8, 0xB8, /* magenta */
+		0x7A, 0x41, 0x9A, 0xD4, 0xBA, 0x64, /* red */
+		0x7C, 0x23, 0x9C, 0x72, 0xBC, 0xD4, /* blue */
+		0x7E, 0x10, 0x9E, 0x80, 0xBE, 0x80, /* black */
+
+		0x60, 0xEB, 0x80, 0x80, 0xc0, 0x80, /* white */
+		0x62, 0xA2, 0x82, 0x8E, 0xc2, 0x2C, /* yellow */
+		0x64, 0x83, 0x84, 0x2C, 0xc4, 0x9C, /* cyan */
+		0x66, 0x70, 0x86, 0x3A, 0xc6, 0x48, /* green */
+		0x68, 0x54, 0x88, 0xC6, 0xc8, 0xB8, /* magenta */
+		0x6A, 0x41, 0x8A, 0xD4, 0xcA, 0x64, /* red */
+		0x6C, 0x23, 0x8C, 0x72, 0xcC, 0xD4, /* blue */
+		0x6E, 0x10, 0x8E, 0x80, 0xcE, 0x80, /* black */
+	};
+	struct bt866 *encoder = to_bt866(sd);
+	u8 val;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(init) / 2; i += 2)
+		bt866_write(encoder, init[i], init[i+1]);
+
+	val = encoder->reg[0xdc];
+
+	if (input == 0)
+		val |= 0x40; /* CBSWAP */
+	else
+		val &= ~0x40; /* !CBSWAP */
+
+	bt866_write(encoder, 0xdc, val);
+
+	val = encoder->reg[0xcc];
+	if (input == 2)
+		val |= 0x01; /* OSDBAR */
+	else
+		val &= ~0x01; /* !OSDBAR */
+	bt866_write(encoder, 0xcc, val);
+
+	v4l2_dbg(1, debug, sd, "set input %d\n", input);
+
+	switch (input) {
+	case 0:
+	case 1:
+	case 2:
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+#if 0
+/* Code to setup square pixels, might be of some use in the future,
+   but is currently unused. */
+	val = encoder->reg[0xdc];
+	if (*iarg)
+		val |= 1; /* SQUARE */
+	else
+		val &= ~1; /* !SQUARE */
+	bt866_write(client, 0xdc, val);
+#endif
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_video_ops bt866_video_ops = {
+	.s_std_output = bt866_s_std_output,
+	.s_routing = bt866_s_routing,
+};
+
+static const struct v4l2_subdev_ops bt866_ops = {
+	.video = &bt866_video_ops,
+};
+
+static int bt866_probe(struct i2c_client *client)
+{
+	struct bt866 *encoder;
+	struct v4l2_subdev *sd;
+
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	encoder = devm_kzalloc(&client->dev, sizeof(*encoder), GFP_KERNEL);
+	if (encoder == NULL)
+		return -ENOMEM;
+	sd = &encoder->sd;
+	v4l2_i2c_subdev_init(sd, client, &bt866_ops);
+	return 0;
+}
+
+static void bt866_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+}
+
+static const struct i2c_device_id bt866_id[] = {
+	{ "bt866", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, bt866_id);
+
+static struct i2c_driver bt866_driver = {
+	.driver = {
+		.name	= "bt866",
+	},
+	.probe		= bt866_probe,
+	.remove		= bt866_remove,
+	.id_table	= bt866_id,
+};
+
+module_i2c_driver(bt866_driver);
diff --git a/drivers/media/i2c/ccs-pll.c b/drivers/media/i2c/ccs-pll.c
new file mode 100644
index 000000000..cf8858cb1
--- /dev/null
+++ b/drivers/media/i2c/ccs-pll.c
@@ -0,0 +1,880 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * drivers/media/i2c/ccs-pll.c
+ *
+ * Generic MIPI CCS/SMIA/SMIA++ PLL calculator
+ *
+ * Copyright (C) 2020 Intel Corporation
+ * Copyright (C) 2011--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@linux.intel.com>
+ */
+
+#include <linux/device.h>
+#include <linux/gcd.h>
+#include <linux/lcm.h>
+#include <linux/module.h>
+
+#include "ccs-pll.h"
+
+/* Return an even number or one. */
+static inline u32 clk_div_even(u32 a)
+{
+	return max_t(u32, 1, a & ~1);
+}
+
+/* Return an even number or one. */
+static inline u32 clk_div_even_up(u32 a)
+{
+	if (a == 1)
+		return 1;
+	return (a + 1) & ~1;
+}
+
+static inline u32 is_one_or_even(u32 a)
+{
+	if (a == 1)
+		return 1;
+	if (a & 1)
+		return 0;
+
+	return 1;
+}
+
+static inline u32 one_or_more(u32 a)
+{
+	return a ?: 1;
+}
+
+static int bounds_check(struct device *dev, u32 val,
+			u32 min, u32 max, const char *prefix,
+			char *str)
+{
+	if (val >= min && val <= max)
+		return 0;
+
+	dev_dbg(dev, "%s_%s out of bounds: %d (%d--%d)\n", prefix,
+		str, val, min, max);
+
+	return -EINVAL;
+}
+
+#define PLL_OP 1
+#define PLL_VT 2
+
+static const char *pll_string(unsigned int which)
+{
+	switch (which) {
+	case PLL_OP:
+		return "op";
+	case PLL_VT:
+		return "vt";
+	}
+
+	return NULL;
+}
+
+#define PLL_FL(f) CCS_PLL_FLAG_##f
+
+static void print_pll(struct device *dev, struct ccs_pll *pll)
+{
+	const struct {
+		struct ccs_pll_branch_fr *fr;
+		struct ccs_pll_branch_bk *bk;
+		unsigned int which;
+	} branches[] = {
+		{ &pll->vt_fr, &pll->vt_bk, PLL_VT },
+		{ &pll->op_fr, &pll->op_bk, PLL_OP }
+	}, *br;
+	unsigned int i;
+
+	dev_dbg(dev, "ext_clk_freq_hz\t\t%u\n", pll->ext_clk_freq_hz);
+
+	for (i = 0, br = branches; i < ARRAY_SIZE(branches); i++, br++) {
+		const char *s = pll_string(br->which);
+
+		if (pll->flags & CCS_PLL_FLAG_DUAL_PLL ||
+		    br->which == PLL_VT) {
+			dev_dbg(dev, "%s_pre_pll_clk_div\t\t%u\n",  s,
+				br->fr->pre_pll_clk_div);
+			dev_dbg(dev, "%s_pll_multiplier\t\t%u\n",  s,
+				br->fr->pll_multiplier);
+
+			dev_dbg(dev, "%s_pll_ip_clk_freq_hz\t%u\n", s,
+				br->fr->pll_ip_clk_freq_hz);
+			dev_dbg(dev, "%s_pll_op_clk_freq_hz\t%u\n", s,
+				br->fr->pll_op_clk_freq_hz);
+		}
+
+		if (!(pll->flags & CCS_PLL_FLAG_NO_OP_CLOCKS) ||
+		    br->which == PLL_VT) {
+			dev_dbg(dev, "%s_sys_clk_div\t\t%u\n",  s,
+				br->bk->sys_clk_div);
+			dev_dbg(dev, "%s_pix_clk_div\t\t%u\n", s,
+				br->bk->pix_clk_div);
+
+			dev_dbg(dev, "%s_sys_clk_freq_hz\t%u\n", s,
+				br->bk->sys_clk_freq_hz);
+			dev_dbg(dev, "%s_pix_clk_freq_hz\t%u\n", s,
+				br->bk->pix_clk_freq_hz);
+		}
+	}
+
+	dev_dbg(dev, "pixel rate in pixel array:\t%u\n",
+		pll->pixel_rate_pixel_array);
+	dev_dbg(dev, "pixel rate on CSI-2 bus:\t%u\n",
+		pll->pixel_rate_csi);
+
+	dev_dbg(dev, "flags%s%s%s%s%s%s%s%s%s\n",
+		pll->flags & PLL_FL(LANE_SPEED_MODEL) ? " lane-speed" : "",
+		pll->flags & PLL_FL(LINK_DECOUPLED) ? " link-decoupled" : "",
+		pll->flags & PLL_FL(EXT_IP_PLL_DIVIDER) ?
+		" ext-ip-pll-divider" : "",
+		pll->flags & PLL_FL(FLEXIBLE_OP_PIX_CLK_DIV) ?
+		" flexible-op-pix-div" : "",
+		pll->flags & PLL_FL(FIFO_DERATING) ? " fifo-derating" : "",
+		pll->flags & PLL_FL(FIFO_OVERRATING) ? " fifo-overrating" : "",
+		pll->flags & PLL_FL(DUAL_PLL) ? " dual-pll" : "",
+		pll->flags & PLL_FL(OP_SYS_DDR) ? " op-sys-ddr" : "",
+		pll->flags & PLL_FL(OP_PIX_DDR) ? " op-pix-ddr" : "");
+}
+
+static u32 op_sys_ddr(u32 flags)
+{
+	return flags & CCS_PLL_FLAG_OP_SYS_DDR ? 1 : 0;
+}
+
+static u32 op_pix_ddr(u32 flags)
+{
+	return flags & CCS_PLL_FLAG_OP_PIX_DDR ? 1 : 0;
+}
+
+static int check_fr_bounds(struct device *dev,
+			   const struct ccs_pll_limits *lim,
+			   struct ccs_pll *pll, unsigned int which)
+{
+	const struct ccs_pll_branch_limits_fr *lim_fr;
+	struct ccs_pll_branch_fr *pll_fr;
+	const char *s = pll_string(which);
+	int rval;
+
+	if (which == PLL_OP) {
+		lim_fr = &lim->op_fr;
+		pll_fr = &pll->op_fr;
+	} else {
+		lim_fr = &lim->vt_fr;
+		pll_fr = &pll->vt_fr;
+	}
+
+	rval = bounds_check(dev, pll_fr->pre_pll_clk_div,
+			    lim_fr->min_pre_pll_clk_div,
+			    lim_fr->max_pre_pll_clk_div, s, "pre_pll_clk_div");
+
+	if (!rval)
+		rval = bounds_check(dev, pll_fr->pll_ip_clk_freq_hz,
+				    lim_fr->min_pll_ip_clk_freq_hz,
+				    lim_fr->max_pll_ip_clk_freq_hz,
+				    s, "pll_ip_clk_freq_hz");
+	if (!rval)
+		rval = bounds_check(dev, pll_fr->pll_multiplier,
+				    lim_fr->min_pll_multiplier,
+				    lim_fr->max_pll_multiplier,
+				    s, "pll_multiplier");
+	if (!rval)
+		rval = bounds_check(dev, pll_fr->pll_op_clk_freq_hz,
+				    lim_fr->min_pll_op_clk_freq_hz,
+				    lim_fr->max_pll_op_clk_freq_hz,
+				    s, "pll_op_clk_freq_hz");
+
+	return rval;
+}
+
+static int check_bk_bounds(struct device *dev,
+			   const struct ccs_pll_limits *lim,
+			   struct ccs_pll *pll, unsigned int which)
+{
+	const struct ccs_pll_branch_limits_bk *lim_bk;
+	struct ccs_pll_branch_bk *pll_bk;
+	const char *s = pll_string(which);
+	int rval;
+
+	if (which == PLL_OP) {
+		if (pll->flags & CCS_PLL_FLAG_NO_OP_CLOCKS)
+			return 0;
+
+		lim_bk = &lim->op_bk;
+		pll_bk = &pll->op_bk;
+	} else {
+		lim_bk = &lim->vt_bk;
+		pll_bk = &pll->vt_bk;
+	}
+
+	rval = bounds_check(dev, pll_bk->sys_clk_div,
+			    lim_bk->min_sys_clk_div,
+			    lim_bk->max_sys_clk_div, s, "op_sys_clk_div");
+	if (!rval)
+		rval = bounds_check(dev, pll_bk->sys_clk_freq_hz,
+				    lim_bk->min_sys_clk_freq_hz,
+				    lim_bk->max_sys_clk_freq_hz,
+				    s, "sys_clk_freq_hz");
+	if (!rval)
+		rval = bounds_check(dev, pll_bk->sys_clk_div,
+				    lim_bk->min_sys_clk_div,
+				    lim_bk->max_sys_clk_div,
+				    s, "sys_clk_div");
+	if (!rval)
+		rval = bounds_check(dev, pll_bk->pix_clk_freq_hz,
+				    lim_bk->min_pix_clk_freq_hz,
+				    lim_bk->max_pix_clk_freq_hz,
+				    s, "pix_clk_freq_hz");
+
+	return rval;
+}
+
+static int check_ext_bounds(struct device *dev, struct ccs_pll *pll)
+{
+	if (!(pll->flags & CCS_PLL_FLAG_FIFO_DERATING) &&
+	    pll->pixel_rate_pixel_array > pll->pixel_rate_csi) {
+		dev_dbg(dev, "device does not support derating\n");
+		return -EINVAL;
+	}
+
+	if (!(pll->flags & CCS_PLL_FLAG_FIFO_OVERRATING) &&
+	    pll->pixel_rate_pixel_array < pll->pixel_rate_csi) {
+		dev_dbg(dev, "device does not support overrating\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void
+ccs_pll_find_vt_sys_div(struct device *dev, const struct ccs_pll_limits *lim,
+			struct ccs_pll *pll, struct ccs_pll_branch_fr *pll_fr,
+			u16 min_vt_div, u16 max_vt_div,
+			u16 *min_sys_div, u16 *max_sys_div)
+{
+	/*
+	 * Find limits for sys_clk_div. Not all values are possible with all
+	 * values of pix_clk_div.
+	 */
+	*min_sys_div = lim->vt_bk.min_sys_clk_div;
+	dev_dbg(dev, "min_sys_div: %u\n", *min_sys_div);
+	*min_sys_div = max_t(u16, *min_sys_div,
+			     DIV_ROUND_UP(min_vt_div,
+					  lim->vt_bk.max_pix_clk_div));
+	dev_dbg(dev, "min_sys_div: max_vt_pix_clk_div: %u\n", *min_sys_div);
+	*min_sys_div = max_t(u16, *min_sys_div,
+			     pll_fr->pll_op_clk_freq_hz
+			     / lim->vt_bk.max_sys_clk_freq_hz);
+	dev_dbg(dev, "min_sys_div: max_pll_op_clk_freq_hz: %u\n", *min_sys_div);
+	*min_sys_div = clk_div_even_up(*min_sys_div);
+	dev_dbg(dev, "min_sys_div: one or even: %u\n", *min_sys_div);
+
+	*max_sys_div = lim->vt_bk.max_sys_clk_div;
+	dev_dbg(dev, "max_sys_div: %u\n", *max_sys_div);
+	*max_sys_div = min_t(u16, *max_sys_div,
+			     DIV_ROUND_UP(max_vt_div,
+					  lim->vt_bk.min_pix_clk_div));
+	dev_dbg(dev, "max_sys_div: min_vt_pix_clk_div: %u\n", *max_sys_div);
+	*max_sys_div = min_t(u16, *max_sys_div,
+			     DIV_ROUND_UP(pll_fr->pll_op_clk_freq_hz,
+					  lim->vt_bk.min_pix_clk_freq_hz));
+	dev_dbg(dev, "max_sys_div: min_vt_pix_clk_freq_hz: %u\n", *max_sys_div);
+}
+
+#define CPHY_CONST		7
+#define DPHY_CONST		16
+#define PHY_CONST_DIV		16
+
+static inline int
+__ccs_pll_calculate_vt_tree(struct device *dev,
+			    const struct ccs_pll_limits *lim,
+			    struct ccs_pll *pll, u32 mul, u32 div)
+{
+	const struct ccs_pll_branch_limits_fr *lim_fr = &lim->vt_fr;
+	const struct ccs_pll_branch_limits_bk *lim_bk = &lim->vt_bk;
+	struct ccs_pll_branch_fr *pll_fr = &pll->vt_fr;
+	struct ccs_pll_branch_bk *pll_bk = &pll->vt_bk;
+	u32 more_mul;
+	u16 best_pix_div = SHRT_MAX >> 1, best_div = lim_bk->max_sys_clk_div;
+	u16 vt_div, min_sys_div, max_sys_div, sys_div;
+
+	pll_fr->pll_ip_clk_freq_hz =
+		pll->ext_clk_freq_hz / pll_fr->pre_pll_clk_div;
+
+	dev_dbg(dev, "vt_pll_ip_clk_freq_hz %u\n", pll_fr->pll_ip_clk_freq_hz);
+
+	more_mul = one_or_more(DIV_ROUND_UP(lim_fr->min_pll_op_clk_freq_hz,
+					    pll_fr->pll_ip_clk_freq_hz * mul));
+
+	dev_dbg(dev, "more_mul: %u\n", more_mul);
+	more_mul *= DIV_ROUND_UP(lim_fr->min_pll_multiplier, mul * more_mul);
+	dev_dbg(dev, "more_mul2: %u\n", more_mul);
+
+	pll_fr->pll_multiplier = mul * more_mul;
+
+	if (pll_fr->pll_multiplier * pll_fr->pll_ip_clk_freq_hz >
+	    lim_fr->max_pll_op_clk_freq_hz)
+		return -EINVAL;
+
+	pll_fr->pll_op_clk_freq_hz =
+		pll_fr->pll_ip_clk_freq_hz * pll_fr->pll_multiplier;
+
+	vt_div = div * more_mul;
+
+	ccs_pll_find_vt_sys_div(dev, lim, pll, pll_fr, vt_div, vt_div,
+				&min_sys_div, &max_sys_div);
+
+	max_sys_div = (vt_div & 1) ? 1 : max_sys_div;
+
+	dev_dbg(dev, "vt min/max_sys_div: %u,%u\n", min_sys_div, max_sys_div);
+
+	for (sys_div = min_sys_div; sys_div <= max_sys_div;
+	     sys_div += 2 - (sys_div & 1)) {
+		u16 pix_div;
+
+		if (vt_div % sys_div)
+			continue;
+
+		pix_div = vt_div / sys_div;
+
+		if (pix_div < lim_bk->min_pix_clk_div ||
+		    pix_div > lim_bk->max_pix_clk_div) {
+			dev_dbg(dev,
+				"pix_div %u too small or too big (%u--%u)\n",
+				pix_div,
+				lim_bk->min_pix_clk_div,
+				lim_bk->max_pix_clk_div);
+			continue;
+		}
+
+		dev_dbg(dev, "sys/pix/best_pix: %u,%u,%u\n", sys_div, pix_div,
+			best_pix_div);
+
+		if (pix_div * sys_div <= best_pix_div) {
+			best_pix_div = pix_div;
+			best_div = pix_div * sys_div;
+		}
+	}
+	if (best_pix_div == SHRT_MAX >> 1)
+		return -EINVAL;
+
+	pll_bk->sys_clk_div = best_div / best_pix_div;
+	pll_bk->pix_clk_div = best_pix_div;
+
+	pll_bk->sys_clk_freq_hz =
+		pll_fr->pll_op_clk_freq_hz / pll_bk->sys_clk_div;
+	pll_bk->pix_clk_freq_hz =
+		pll_bk->sys_clk_freq_hz / pll_bk->pix_clk_div;
+
+	pll->pixel_rate_pixel_array =
+		pll_bk->pix_clk_freq_hz * pll->vt_lanes;
+
+	return 0;
+}
+
+static int ccs_pll_calculate_vt_tree(struct device *dev,
+				     const struct ccs_pll_limits *lim,
+				     struct ccs_pll *pll)
+{
+	const struct ccs_pll_branch_limits_fr *lim_fr = &lim->vt_fr;
+	struct ccs_pll_branch_fr *pll_fr = &pll->vt_fr;
+	u16 min_pre_pll_clk_div = lim_fr->min_pre_pll_clk_div;
+	u16 max_pre_pll_clk_div = lim_fr->max_pre_pll_clk_div;
+	u32 pre_mul, pre_div;
+
+	pre_div = gcd(pll->pixel_rate_csi,
+		      pll->ext_clk_freq_hz * pll->vt_lanes);
+	pre_mul = pll->pixel_rate_csi / pre_div;
+	pre_div = pll->ext_clk_freq_hz * pll->vt_lanes / pre_div;
+
+	/* Make sure PLL input frequency is within limits */
+	max_pre_pll_clk_div =
+		min_t(u16, max_pre_pll_clk_div,
+		      DIV_ROUND_UP(pll->ext_clk_freq_hz,
+				   lim_fr->min_pll_ip_clk_freq_hz));
+
+	min_pre_pll_clk_div = max_t(u16, min_pre_pll_clk_div,
+				    pll->ext_clk_freq_hz /
+				    lim_fr->max_pll_ip_clk_freq_hz);
+
+	dev_dbg(dev, "vt min/max_pre_pll_clk_div: %u,%u\n",
+		min_pre_pll_clk_div, max_pre_pll_clk_div);
+
+	for (pll_fr->pre_pll_clk_div = min_pre_pll_clk_div;
+	     pll_fr->pre_pll_clk_div <= max_pre_pll_clk_div;
+	     pll_fr->pre_pll_clk_div +=
+		     (pll->flags & CCS_PLL_FLAG_EXT_IP_PLL_DIVIDER) ? 1 :
+		     2 - (pll_fr->pre_pll_clk_div & 1)) {
+		u32 mul, div;
+		int rval;
+
+		div = gcd(pre_mul * pll_fr->pre_pll_clk_div, pre_div);
+		mul = pre_mul * pll_fr->pre_pll_clk_div / div;
+		div = pre_div / div;
+
+		dev_dbg(dev, "vt pre-div/mul/div: %u,%u,%u\n",
+			pll_fr->pre_pll_clk_div, mul, div);
+
+		rval = __ccs_pll_calculate_vt_tree(dev, lim, pll,
+						   mul, div);
+		if (rval)
+			continue;
+
+		rval = check_fr_bounds(dev, lim, pll, PLL_VT);
+		if (rval)
+			continue;
+
+		rval = check_bk_bounds(dev, lim, pll, PLL_VT);
+		if (rval)
+			continue;
+
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static void
+ccs_pll_calculate_vt(struct device *dev, const struct ccs_pll_limits *lim,
+		     const struct ccs_pll_branch_limits_bk *op_lim_bk,
+		     struct ccs_pll *pll, struct ccs_pll_branch_fr *pll_fr,
+		     struct ccs_pll_branch_bk *op_pll_bk, bool cphy,
+		     u32 phy_const)
+{
+	u16 sys_div;
+	u16 best_pix_div = SHRT_MAX >> 1;
+	u16 vt_op_binning_div;
+	u16 min_vt_div, max_vt_div, vt_div;
+	u16 min_sys_div, max_sys_div;
+
+	if (pll->flags & CCS_PLL_FLAG_NO_OP_CLOCKS)
+		goto out_calc_pixel_rate;
+
+	/*
+	 * Find out whether a sensor supports derating. If it does not, VT and
+	 * OP domains are required to run at the same pixel rate.
+	 */
+	if (!(pll->flags & CCS_PLL_FLAG_FIFO_DERATING)) {
+		min_vt_div =
+			op_pll_bk->sys_clk_div * op_pll_bk->pix_clk_div
+			* pll->vt_lanes * phy_const / pll->op_lanes
+			/ (PHY_CONST_DIV << op_pix_ddr(pll->flags));
+	} else {
+		/*
+		 * Some sensors perform analogue binning and some do this
+		 * digitally. The ones doing this digitally can be roughly be
+		 * found out using this formula. The ones doing this digitally
+		 * should run at higher clock rate, so smaller divisor is used
+		 * on video timing side.
+		 */
+		if (lim->min_line_length_pck_bin > lim->min_line_length_pck
+		    / pll->binning_horizontal)
+			vt_op_binning_div = pll->binning_horizontal;
+		else
+			vt_op_binning_div = 1;
+		dev_dbg(dev, "vt_op_binning_div: %u\n", vt_op_binning_div);
+
+		/*
+		 * Profile 2 supports vt_pix_clk_div E [4, 10]
+		 *
+		 * Horizontal binning can be used as a base for difference in
+		 * divisors. One must make sure that horizontal blanking is
+		 * enough to accommodate the CSI-2 sync codes.
+		 *
+		 * Take scaling factor and number of VT lanes into account as well.
+		 *
+		 * Find absolute limits for the factor of vt divider.
+		 */
+		dev_dbg(dev, "scale_m: %u\n", pll->scale_m);
+		min_vt_div =
+			DIV_ROUND_UP(pll->bits_per_pixel
+				     * op_pll_bk->sys_clk_div * pll->scale_n
+				     * pll->vt_lanes * phy_const,
+				     (pll->flags &
+				      CCS_PLL_FLAG_LANE_SPEED_MODEL ?
+				      pll->csi2.lanes : 1)
+				     * vt_op_binning_div * pll->scale_m
+				     * PHY_CONST_DIV << op_pix_ddr(pll->flags));
+	}
+
+	/* Find smallest and biggest allowed vt divisor. */
+	dev_dbg(dev, "min_vt_div: %u\n", min_vt_div);
+	min_vt_div = max_t(u16, min_vt_div,
+			   DIV_ROUND_UP(pll_fr->pll_op_clk_freq_hz,
+					lim->vt_bk.max_pix_clk_freq_hz));
+	dev_dbg(dev, "min_vt_div: max_vt_pix_clk_freq_hz: %u\n",
+		min_vt_div);
+	min_vt_div = max_t(u16, min_vt_div, lim->vt_bk.min_pix_clk_div
+					    * lim->vt_bk.min_sys_clk_div);
+	dev_dbg(dev, "min_vt_div: min_vt_clk_div: %u\n", min_vt_div);
+
+	max_vt_div = lim->vt_bk.max_sys_clk_div * lim->vt_bk.max_pix_clk_div;
+	dev_dbg(dev, "max_vt_div: %u\n", max_vt_div);
+	max_vt_div = min_t(u16, max_vt_div,
+			   DIV_ROUND_UP(pll_fr->pll_op_clk_freq_hz,
+				      lim->vt_bk.min_pix_clk_freq_hz));
+	dev_dbg(dev, "max_vt_div: min_vt_pix_clk_freq_hz: %u\n",
+		max_vt_div);
+
+	ccs_pll_find_vt_sys_div(dev, lim, pll, pll_fr, min_vt_div,
+				max_vt_div, &min_sys_div, &max_sys_div);
+
+	/*
+	 * Find pix_div such that a legal pix_div * sys_div results
+	 * into a value which is not smaller than div, the desired
+	 * divisor.
+	 */
+	for (vt_div = min_vt_div; vt_div <= max_vt_div; vt_div++) {
+		u16 __max_sys_div = vt_div & 1 ? 1 : max_sys_div;
+
+		for (sys_div = min_sys_div; sys_div <= __max_sys_div;
+		     sys_div += 2 - (sys_div & 1)) {
+			u16 pix_div;
+			u16 rounded_div;
+
+			pix_div = DIV_ROUND_UP(vt_div, sys_div);
+
+			if (pix_div < lim->vt_bk.min_pix_clk_div
+			    || pix_div > lim->vt_bk.max_pix_clk_div) {
+				dev_dbg(dev,
+					"pix_div %u too small or too big (%u--%u)\n",
+					pix_div,
+					lim->vt_bk.min_pix_clk_div,
+					lim->vt_bk.max_pix_clk_div);
+				continue;
+			}
+
+			rounded_div = roundup(vt_div, best_pix_div);
+
+			/* Check if this one is better. */
+			if (pix_div * sys_div <= rounded_div)
+				best_pix_div = pix_div;
+
+			/* Bail out if we've already found the best value. */
+			if (vt_div == rounded_div)
+				break;
+		}
+		if (best_pix_div < SHRT_MAX >> 1)
+			break;
+	}
+
+	pll->vt_bk.sys_clk_div = DIV_ROUND_UP(vt_div, best_pix_div);
+	pll->vt_bk.pix_clk_div = best_pix_div;
+
+	pll->vt_bk.sys_clk_freq_hz =
+		pll_fr->pll_op_clk_freq_hz / pll->vt_bk.sys_clk_div;
+	pll->vt_bk.pix_clk_freq_hz =
+		pll->vt_bk.sys_clk_freq_hz / pll->vt_bk.pix_clk_div;
+
+out_calc_pixel_rate:
+	pll->pixel_rate_pixel_array =
+		pll->vt_bk.pix_clk_freq_hz * pll->vt_lanes;
+}
+
+/*
+ * Heuristically guess the PLL tree for a given common multiplier and
+ * divisor. Begin with the operational timing and continue to video
+ * timing once operational timing has been verified.
+ *
+ * @mul is the PLL multiplier and @div is the common divisor
+ * (pre_pll_clk_div and op_sys_clk_div combined). The final PLL
+ * multiplier will be a multiple of @mul.
+ *
+ * @return Zero on success, error code on error.
+ */
+static int
+ccs_pll_calculate_op(struct device *dev, const struct ccs_pll_limits *lim,
+		     const struct ccs_pll_branch_limits_fr *op_lim_fr,
+		     const struct ccs_pll_branch_limits_bk *op_lim_bk,
+		     struct ccs_pll *pll, struct ccs_pll_branch_fr *op_pll_fr,
+		     struct ccs_pll_branch_bk *op_pll_bk, u32 mul,
+		     u32 div, u32 op_sys_clk_freq_hz_sdr, u32 l,
+		     bool cphy, u32 phy_const)
+{
+	/*
+	 * Higher multipliers (and divisors) are often required than
+	 * necessitated by the external clock and the output clocks.
+	 * There are limits for all values in the clock tree. These
+	 * are the minimum and maximum multiplier for mul.
+	 */
+	u32 more_mul_min, more_mul_max;
+	u32 more_mul_factor;
+	u32 i;
+
+	/*
+	 * Get pre_pll_clk_div so that our pll_op_clk_freq_hz won't be
+	 * too high.
+	 */
+	dev_dbg(dev, "op_pre_pll_clk_div %u\n", op_pll_fr->pre_pll_clk_div);
+
+	/* Don't go above max pll multiplier. */
+	more_mul_max = op_lim_fr->max_pll_multiplier / mul;
+	dev_dbg(dev, "more_mul_max: max_op_pll_multiplier check: %u\n",
+		more_mul_max);
+	/* Don't go above max pll op frequency. */
+	more_mul_max =
+		min_t(u32,
+		      more_mul_max,
+		      op_lim_fr->max_pll_op_clk_freq_hz
+		      / (pll->ext_clk_freq_hz /
+			 op_pll_fr->pre_pll_clk_div * mul));
+	dev_dbg(dev, "more_mul_max: max_pll_op_clk_freq_hz check: %u\n",
+		more_mul_max);
+	/* Don't go above the division capability of op sys clock divider. */
+	more_mul_max = min(more_mul_max,
+			   op_lim_bk->max_sys_clk_div * op_pll_fr->pre_pll_clk_div
+			   / div);
+	dev_dbg(dev, "more_mul_max: max_op_sys_clk_div check: %u\n",
+		more_mul_max);
+	/* Ensure we won't go above max_pll_multiplier. */
+	more_mul_max = min(more_mul_max, op_lim_fr->max_pll_multiplier / mul);
+	dev_dbg(dev, "more_mul_max: min_pll_multiplier check: %u\n",
+		more_mul_max);
+
+	/* Ensure we won't go below min_pll_op_clk_freq_hz. */
+	more_mul_min = DIV_ROUND_UP(op_lim_fr->min_pll_op_clk_freq_hz,
+				    pll->ext_clk_freq_hz /
+				    op_pll_fr->pre_pll_clk_div * mul);
+	dev_dbg(dev, "more_mul_min: min_op_pll_op_clk_freq_hz check: %u\n",
+		more_mul_min);
+	/* Ensure we won't go below min_pll_multiplier. */
+	more_mul_min = max(more_mul_min,
+			   DIV_ROUND_UP(op_lim_fr->min_pll_multiplier, mul));
+	dev_dbg(dev, "more_mul_min: min_op_pll_multiplier check: %u\n",
+		more_mul_min);
+
+	if (more_mul_min > more_mul_max) {
+		dev_dbg(dev,
+			"unable to compute more_mul_min and more_mul_max\n");
+		return -EINVAL;
+	}
+
+	more_mul_factor = lcm(div, op_pll_fr->pre_pll_clk_div) / div;
+	dev_dbg(dev, "more_mul_factor: %u\n", more_mul_factor);
+	more_mul_factor = lcm(more_mul_factor, op_lim_bk->min_sys_clk_div);
+	dev_dbg(dev, "more_mul_factor: min_op_sys_clk_div: %d\n",
+		more_mul_factor);
+	i = roundup(more_mul_min, more_mul_factor);
+	if (!is_one_or_even(i))
+		i <<= 1;
+
+	dev_dbg(dev, "final more_mul: %u\n", i);
+	if (i > more_mul_max) {
+		dev_dbg(dev, "final more_mul is bad, max %u\n", more_mul_max);
+		return -EINVAL;
+	}
+
+	op_pll_fr->pll_multiplier = mul * i;
+	op_pll_bk->sys_clk_div = div * i / op_pll_fr->pre_pll_clk_div;
+	dev_dbg(dev, "op_sys_clk_div: %u\n", op_pll_bk->sys_clk_div);
+
+	op_pll_fr->pll_ip_clk_freq_hz = pll->ext_clk_freq_hz
+		/ op_pll_fr->pre_pll_clk_div;
+
+	op_pll_fr->pll_op_clk_freq_hz = op_pll_fr->pll_ip_clk_freq_hz
+		* op_pll_fr->pll_multiplier;
+
+	if (pll->flags & CCS_PLL_FLAG_LANE_SPEED_MODEL)
+		op_pll_bk->pix_clk_div =
+			(pll->bits_per_pixel
+			 * pll->op_lanes * (phy_const << op_sys_ddr(pll->flags))
+			 / PHY_CONST_DIV / pll->csi2.lanes / l)
+			>> op_pix_ddr(pll->flags);
+	else
+		op_pll_bk->pix_clk_div =
+			(pll->bits_per_pixel
+			 * (phy_const << op_sys_ddr(pll->flags))
+			 / PHY_CONST_DIV / l) >> op_pix_ddr(pll->flags);
+
+	op_pll_bk->pix_clk_freq_hz =
+		(op_sys_clk_freq_hz_sdr >> op_pix_ddr(pll->flags))
+		/ op_pll_bk->pix_clk_div;
+	op_pll_bk->sys_clk_freq_hz =
+		op_sys_clk_freq_hz_sdr >> op_sys_ddr(pll->flags);
+
+	dev_dbg(dev, "op_pix_clk_div: %u\n", op_pll_bk->pix_clk_div);
+
+	return 0;
+}
+
+int ccs_pll_calculate(struct device *dev, const struct ccs_pll_limits *lim,
+		      struct ccs_pll *pll)
+{
+	const struct ccs_pll_branch_limits_fr *op_lim_fr;
+	const struct ccs_pll_branch_limits_bk *op_lim_bk;
+	struct ccs_pll_branch_fr *op_pll_fr;
+	struct ccs_pll_branch_bk *op_pll_bk;
+	bool cphy = pll->bus_type == CCS_PLL_BUS_TYPE_CSI2_CPHY;
+	u32 phy_const = cphy ? CPHY_CONST : DPHY_CONST;
+	u32 op_sys_clk_freq_hz_sdr;
+	u16 min_op_pre_pll_clk_div;
+	u16 max_op_pre_pll_clk_div;
+	u32 mul, div;
+	u32 l = (!pll->op_bits_per_lane ||
+		 pll->op_bits_per_lane >= pll->bits_per_pixel) ? 1 : 2;
+	u32 i;
+	int rval = -EINVAL;
+
+	if (!(pll->flags & CCS_PLL_FLAG_LANE_SPEED_MODEL)) {
+		pll->op_lanes = 1;
+		pll->vt_lanes = 1;
+	}
+
+	if (pll->flags & CCS_PLL_FLAG_DUAL_PLL) {
+		op_lim_fr = &lim->op_fr;
+		op_lim_bk = &lim->op_bk;
+		op_pll_fr = &pll->op_fr;
+		op_pll_bk = &pll->op_bk;
+	} else if (pll->flags & CCS_PLL_FLAG_NO_OP_CLOCKS) {
+		/*
+		 * If there's no OP PLL at all, use the VT values
+		 * instead. The OP values are ignored for the rest of
+		 * the PLL calculation.
+		 */
+		op_lim_fr = &lim->vt_fr;
+		op_lim_bk = &lim->vt_bk;
+		op_pll_fr = &pll->vt_fr;
+		op_pll_bk = &pll->vt_bk;
+	} else {
+		op_lim_fr = &lim->vt_fr;
+		op_lim_bk = &lim->op_bk;
+		op_pll_fr = &pll->vt_fr;
+		op_pll_bk = &pll->op_bk;
+	}
+
+	if (!pll->op_lanes || !pll->vt_lanes || !pll->bits_per_pixel ||
+	    !pll->ext_clk_freq_hz || !pll->link_freq || !pll->scale_m ||
+	    !op_lim_fr->min_pll_ip_clk_freq_hz ||
+	    !op_lim_fr->max_pll_ip_clk_freq_hz ||
+	    !op_lim_fr->min_pll_op_clk_freq_hz ||
+	    !op_lim_fr->max_pll_op_clk_freq_hz ||
+	    !op_lim_bk->max_sys_clk_div || !op_lim_fr->max_pll_multiplier)
+		return -EINVAL;
+
+	/*
+	 * Make sure op_pix_clk_div will be integer --- unless flexible
+	 * op_pix_clk_div is supported
+	 */
+	if (!(pll->flags & CCS_PLL_FLAG_FLEXIBLE_OP_PIX_CLK_DIV) &&
+	    (pll->bits_per_pixel * pll->op_lanes) %
+	    (pll->csi2.lanes * l << op_pix_ddr(pll->flags))) {
+		dev_dbg(dev, "op_pix_clk_div not an integer (bpp %u, op lanes %u, lanes %u, l %u)\n",
+			pll->bits_per_pixel, pll->op_lanes, pll->csi2.lanes, l);
+		return -EINVAL;
+	}
+
+	dev_dbg(dev, "vt_lanes: %u\n", pll->vt_lanes);
+	dev_dbg(dev, "op_lanes: %u\n", pll->op_lanes);
+
+	dev_dbg(dev, "binning: %ux%u\n", pll->binning_horizontal,
+		pll->binning_vertical);
+
+	switch (pll->bus_type) {
+	case CCS_PLL_BUS_TYPE_CSI2_DPHY:
+	case CCS_PLL_BUS_TYPE_CSI2_CPHY:
+		op_sys_clk_freq_hz_sdr = pll->link_freq * 2
+			* (pll->flags & CCS_PLL_FLAG_LANE_SPEED_MODEL ?
+			   1 : pll->csi2.lanes);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	pll->pixel_rate_csi =
+		div_u64((uint64_t)op_sys_clk_freq_hz_sdr
+			* (pll->flags & CCS_PLL_FLAG_LANE_SPEED_MODEL ?
+			   pll->csi2.lanes : 1) * PHY_CONST_DIV,
+			phy_const * pll->bits_per_pixel * l);
+
+	/* Figure out limits for OP pre-pll divider based on extclk */
+	dev_dbg(dev, "min / max op_pre_pll_clk_div: %u / %u\n",
+		op_lim_fr->min_pre_pll_clk_div, op_lim_fr->max_pre_pll_clk_div);
+	max_op_pre_pll_clk_div =
+		min_t(u16, op_lim_fr->max_pre_pll_clk_div,
+		      clk_div_even(pll->ext_clk_freq_hz /
+				   op_lim_fr->min_pll_ip_clk_freq_hz));
+	min_op_pre_pll_clk_div =
+		max_t(u16, op_lim_fr->min_pre_pll_clk_div,
+		      clk_div_even_up(
+			      DIV_ROUND_UP(pll->ext_clk_freq_hz,
+					   op_lim_fr->max_pll_ip_clk_freq_hz)));
+	dev_dbg(dev, "pre-pll check: min / max op_pre_pll_clk_div: %u / %u\n",
+		min_op_pre_pll_clk_div, max_op_pre_pll_clk_div);
+
+	i = gcd(op_sys_clk_freq_hz_sdr,
+		pll->ext_clk_freq_hz << op_pix_ddr(pll->flags));
+	mul = op_sys_clk_freq_hz_sdr / i;
+	div = (pll->ext_clk_freq_hz << op_pix_ddr(pll->flags)) / i;
+	dev_dbg(dev, "mul %u / div %u\n", mul, div);
+
+	min_op_pre_pll_clk_div =
+		max_t(u16, min_op_pre_pll_clk_div,
+		      clk_div_even_up(
+			      mul /
+			      one_or_more(
+				      DIV_ROUND_UP(op_lim_fr->max_pll_op_clk_freq_hz,
+						   pll->ext_clk_freq_hz))));
+	dev_dbg(dev, "pll_op check: min / max op_pre_pll_clk_div: %u / %u\n",
+		min_op_pre_pll_clk_div, max_op_pre_pll_clk_div);
+
+	for (op_pll_fr->pre_pll_clk_div = min_op_pre_pll_clk_div;
+	     op_pll_fr->pre_pll_clk_div <= max_op_pre_pll_clk_div;
+	     op_pll_fr->pre_pll_clk_div +=
+		     (pll->flags & CCS_PLL_FLAG_EXT_IP_PLL_DIVIDER) ? 1 :
+		     2 - (op_pll_fr->pre_pll_clk_div & 1)) {
+		rval = ccs_pll_calculate_op(dev, lim, op_lim_fr, op_lim_bk, pll,
+					    op_pll_fr, op_pll_bk, mul, div,
+					    op_sys_clk_freq_hz_sdr, l, cphy,
+					    phy_const);
+		if (rval)
+			continue;
+
+		rval = check_fr_bounds(dev, lim, pll,
+				       pll->flags & CCS_PLL_FLAG_DUAL_PLL ?
+				       PLL_OP : PLL_VT);
+		if (rval)
+			continue;
+
+		rval = check_bk_bounds(dev, lim, pll, PLL_OP);
+		if (rval)
+			continue;
+
+		if (pll->flags & CCS_PLL_FLAG_DUAL_PLL)
+			break;
+
+		ccs_pll_calculate_vt(dev, lim, op_lim_bk, pll, op_pll_fr,
+				     op_pll_bk, cphy, phy_const);
+
+		rval = check_bk_bounds(dev, lim, pll, PLL_VT);
+		if (rval)
+			continue;
+		rval = check_ext_bounds(dev, pll);
+		if (rval)
+			continue;
+
+		break;
+	}
+
+	if (rval) {
+		dev_dbg(dev, "unable to compute pre_pll divisor\n");
+
+		return rval;
+	}
+
+	if (pll->flags & CCS_PLL_FLAG_DUAL_PLL) {
+		rval = ccs_pll_calculate_vt_tree(dev, lim, pll);
+
+		if (rval)
+			return rval;
+	}
+
+	print_pll(dev, pll);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ccs_pll_calculate);
+
+MODULE_AUTHOR("Sakari Ailus <sakari.ailus@linux.intel.com>");
+MODULE_DESCRIPTION("Generic MIPI CCS/SMIA/SMIA++ PLL calculator");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/ccs-pll.h b/drivers/media/i2c/ccs-pll.h
new file mode 100644
index 000000000..6eb1b1c68
--- /dev/null
+++ b/drivers/media/i2c/ccs-pll.h
@@ -0,0 +1,214 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * drivers/media/i2c/ccs-pll.h
+ *
+ * Generic MIPI CCS/SMIA/SMIA++ PLL calculator
+ *
+ * Copyright (C) 2020 Intel Corporation
+ * Copyright (C) 2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@linux.intel.com>
+ */
+
+#ifndef CCS_PLL_H
+#define CCS_PLL_H
+
+#include <linux/bits.h>
+
+/* CSI-2 or CCP-2 */
+#define CCS_PLL_BUS_TYPE_CSI2_DPHY				0x00
+#define CCS_PLL_BUS_TYPE_CSI2_CPHY				0x01
+
+/* Old SMIA and implementation specific flags */
+/* op pix clock is for all lanes in total normally */
+#define CCS_PLL_FLAG_OP_PIX_CLOCK_PER_LANE			BIT(0)
+#define CCS_PLL_FLAG_NO_OP_CLOCKS				BIT(1)
+/* CCS PLL flags */
+#define CCS_PLL_FLAG_LANE_SPEED_MODEL				BIT(2)
+#define CCS_PLL_FLAG_LINK_DECOUPLED				BIT(3)
+#define CCS_PLL_FLAG_EXT_IP_PLL_DIVIDER				BIT(4)
+#define CCS_PLL_FLAG_FLEXIBLE_OP_PIX_CLK_DIV			BIT(5)
+#define CCS_PLL_FLAG_FIFO_DERATING				BIT(6)
+#define CCS_PLL_FLAG_FIFO_OVERRATING				BIT(7)
+#define CCS_PLL_FLAG_DUAL_PLL					BIT(8)
+#define CCS_PLL_FLAG_OP_SYS_DDR					BIT(9)
+#define CCS_PLL_FLAG_OP_PIX_DDR					BIT(10)
+
+/**
+ * struct ccs_pll_branch_fr - CCS PLL configuration (front)
+ *
+ * A single branch front-end of the CCS PLL tree.
+ *
+ * @pre_pll_clk_div: Pre-PLL clock divisor
+ * @pll_multiplier: PLL multiplier
+ * @pll_ip_clk_freq_hz: PLL input clock frequency
+ * @pll_op_clk_freq_hz: PLL output clock frequency
+ */
+struct ccs_pll_branch_fr {
+	u16 pre_pll_clk_div;
+	u16 pll_multiplier;
+	u32 pll_ip_clk_freq_hz;
+	u32 pll_op_clk_freq_hz;
+};
+
+/**
+ * struct ccs_pll_branch_bk - CCS PLL configuration (back)
+ *
+ * A single branch back-end of the CCS PLL tree.
+ *
+ * @sys_clk_div: System clock divider
+ * @pix_clk_div: Pixel clock divider
+ * @sys_clk_freq_hz: System clock frequency
+ * @pix_clk_freq_hz: Pixel clock frequency
+ */
+struct ccs_pll_branch_bk {
+	u16 sys_clk_div;
+	u16 pix_clk_div;
+	u32 sys_clk_freq_hz;
+	u32 pix_clk_freq_hz;
+};
+
+/**
+ * struct ccs_pll - Full CCS PLL configuration
+ *
+ * All information required to calculate CCS PLL configuration.
+ *
+ * @bus_type: Type of the data bus, CCS_PLL_BUS_TYPE_* (input)
+ * @op_lanes: Number of operational lanes (input)
+ * @vt_lanes: Number of video timing lanes (input)
+ * @csi2: CSI-2 related parameters
+ * @csi2.lanes: The number of the CSI-2 data lanes (input)
+ * @binning_vertical: Vertical binning factor (input)
+ * @binning_horizontal: Horizontal binning factor (input)
+ * @scale_m: Downscaling factor, M component, [16, max] (input)
+ * @scale_n: Downscaling factor, N component, typically 16 (input)
+ * @bits_per_pixel: Bits per pixel on the output data bus (input)
+ * @op_bits_per_lane: Number of bits per OP lane (input)
+ * @flags: CCS_PLL_FLAG_* (input)
+ * @link_freq: Chosen link frequency (input)
+ * @ext_clk_freq_hz: External clock frequency, i.e. the sensor's input clock
+ *		     (input)
+ * @vt_fr: Video timing front-end configuration (output)
+ * @vt_bk: Video timing back-end configuration (output)
+ * @op_fr: Operational timing front-end configuration (output)
+ * @op_bk: Operational timing back-end configuration (output)
+ * @pixel_rate_csi: Pixel rate on the output data bus (output)
+ * @pixel_rate_pixel_array: Nominal pixel rate in the sensor's pixel array
+ *			    (output)
+ */
+struct ccs_pll {
+	/* input values */
+	u8 bus_type;
+	u8 op_lanes;
+	u8 vt_lanes;
+	struct {
+		u8 lanes;
+	} csi2;
+	u8 binning_horizontal;
+	u8 binning_vertical;
+	u8 scale_m;
+	u8 scale_n;
+	u8 bits_per_pixel;
+	u8 op_bits_per_lane;
+	u16 flags;
+	u32 link_freq;
+	u32 ext_clk_freq_hz;
+
+	/* output values */
+	struct ccs_pll_branch_fr vt_fr;
+	struct ccs_pll_branch_bk vt_bk;
+	struct ccs_pll_branch_fr op_fr;
+	struct ccs_pll_branch_bk op_bk;
+
+	u32 pixel_rate_csi;
+	u32 pixel_rate_pixel_array;
+};
+
+/**
+ * struct ccs_pll_branch_limits_fr - CCS PLL front-end limits
+ *
+ * @min_pre_pll_clk_div: Minimum pre-PLL clock divider
+ * @max_pre_pll_clk_div: Maximum pre-PLL clock divider
+ * @min_pll_ip_clk_freq_hz: Minimum PLL input clock frequency
+ * @max_pll_ip_clk_freq_hz: Maximum PLL input clock frequency
+ * @min_pll_multiplier: Minimum PLL multiplier
+ * @max_pll_multiplier: Maximum PLL multiplier
+ * @min_pll_op_clk_freq_hz: Minimum PLL output clock frequency
+ * @max_pll_op_clk_freq_hz: Maximum PLL output clock frequency
+ */
+struct ccs_pll_branch_limits_fr {
+	u16 min_pre_pll_clk_div;
+	u16 max_pre_pll_clk_div;
+	u32 min_pll_ip_clk_freq_hz;
+	u32 max_pll_ip_clk_freq_hz;
+	u16 min_pll_multiplier;
+	u16 max_pll_multiplier;
+	u32 min_pll_op_clk_freq_hz;
+	u32 max_pll_op_clk_freq_hz;
+};
+
+/**
+ * struct ccs_pll_branch_limits_bk - CCS PLL back-end limits
+ *
+ * @min_sys_clk_div: Minimum system clock divider
+ * @max_sys_clk_div: Maximum system clock divider
+ * @min_sys_clk_freq_hz: Minimum system clock frequency
+ * @max_sys_clk_freq_hz: Maximum system clock frequency
+ * @min_pix_clk_div: Minimum pixel clock divider
+ * @max_pix_clk_div: Maximum pixel clock divider
+ * @min_pix_clk_freq_hz: Minimum pixel clock frequency
+ * @max_pix_clk_freq_hz: Maximum pixel clock frequency
+ */
+struct ccs_pll_branch_limits_bk {
+	u16 min_sys_clk_div;
+	u16 max_sys_clk_div;
+	u32 min_sys_clk_freq_hz;
+	u32 max_sys_clk_freq_hz;
+	u16 min_pix_clk_div;
+	u16 max_pix_clk_div;
+	u32 min_pix_clk_freq_hz;
+	u32 max_pix_clk_freq_hz;
+};
+
+/**
+ * struct ccs_pll_limits - CCS PLL limits
+ *
+ * @min_ext_clk_freq_hz: Minimum external clock frequency
+ * @max_ext_clk_freq_hz: Maximum external clock frequency
+ * @vt_fr: Video timing front-end limits
+ * @vt_bk: Video timing back-end limits
+ * @op_fr: Operational timing front-end limits
+ * @op_bk: Operational timing back-end limits
+ * @min_line_length_pck_bin: Minimum line length in pixels, with binning
+ * @min_line_length_pck: Minimum line length in pixels without binning
+ */
+struct ccs_pll_limits {
+	/* Strict PLL limits */
+	u32 min_ext_clk_freq_hz;
+	u32 max_ext_clk_freq_hz;
+
+	struct ccs_pll_branch_limits_fr vt_fr;
+	struct ccs_pll_branch_limits_bk vt_bk;
+	struct ccs_pll_branch_limits_fr op_fr;
+	struct ccs_pll_branch_limits_bk op_bk;
+
+	/* Other relevant limits */
+	u32 min_line_length_pck_bin;
+	u32 min_line_length_pck;
+};
+
+struct device;
+
+/**
+ * ccs_pll_calculate - Calculate CCS PLL configuration based on input parameters
+ *
+ * @dev: Device pointer, used for printing messages
+ * @limits: Limits specific to the sensor
+ * @pll: Given PLL configuration
+ *
+ * Calculate the CCS PLL configuration based on the limits as well as given
+ * device specific, system specific or user configured input data.
+ */
+int ccs_pll_calculate(struct device *dev, const struct ccs_pll_limits *limits,
+		      struct ccs_pll *pll);
+
+#endif /* CCS_PLL_H */
diff --git a/drivers/media/i2c/ccs/Kconfig b/drivers/media/i2c/ccs/Kconfig
new file mode 100644
index 000000000..b55c93a2e
--- /dev/null
+++ b/drivers/media/i2c/ccs/Kconfig
@@ -0,0 +1,8 @@
+# SPDX-License-Identifier: GPL-2.0-only
+config VIDEO_CCS
+	tristate "MIPI CCS/SMIA++/SMIA sensor support"
+	depends on HAVE_CLK
+	select VIDEO_CCS_PLL
+	help
+	  This is a generic driver for MIPI CCS, SMIA++ and SMIA compliant
+	  camera sensors.
diff --git a/drivers/media/i2c/ccs/Makefile b/drivers/media/i2c/ccs/Makefile
new file mode 100644
index 000000000..44601ba8c
--- /dev/null
+++ b/drivers/media/i2c/ccs/Makefile
@@ -0,0 +1,6 @@
+# SPDX-License-Identifier: GPL-2.0-only
+ccs-objs			+= ccs-core.o ccs-reg-access.o \
+				   ccs-quirk.o ccs-limits.o ccs-data.o
+obj-$(CONFIG_VIDEO_CCS)		+= ccs.o
+
+ccflags-y += -I $(srctree)/drivers/media/i2c
diff --git a/drivers/media/i2c/ccs/ccs-core.c b/drivers/media/i2c/ccs/ccs-core.c
new file mode 100644
index 000000000..6f8fbd82e
--- /dev/null
+++ b/drivers/media/i2c/ccs/ccs-core.c
@@ -0,0 +1,3775 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * drivers/media/i2c/ccs/ccs-core.c
+ *
+ * Generic driver for MIPI CCS/SMIA/SMIA++ compliant camera sensors
+ *
+ * Copyright (C) 2020 Intel Corporation
+ * Copyright (C) 2010--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@linux.intel.com>
+ *
+ * Based on smiapp driver by Vimarsh Zutshi
+ * Based on jt8ev1.c by Vimarsh Zutshi
+ * Based on smia-sensor.c by Tuukka Toivonen <tuukkat76@gmail.com>
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/firmware.h>
+#include <linux/gpio/consumer.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <linux/property.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/smiapp.h>
+#include <linux/v4l2-mediabus.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-device.h>
+#include <uapi/linux/ccs.h>
+
+#include "ccs.h"
+
+#define CCS_ALIGN_DIM(dim, flags)	\
+	((flags) & V4L2_SEL_FLAG_GE	\
+	 ? ALIGN((dim), 2)		\
+	 : (dim) & ~1)
+
+static struct ccs_limit_offset {
+	u16	lim;
+	u16	info;
+} ccs_limit_offsets[CCS_L_LAST + 1];
+
+/*
+ * ccs_module_idents - supported camera modules
+ */
+static const struct ccs_module_ident ccs_module_idents[] = {
+	CCS_IDENT_L(0x01, 0x022b, -1, "vs6555"),
+	CCS_IDENT_L(0x01, 0x022e, -1, "vw6558"),
+	CCS_IDENT_L(0x07, 0x7698, -1, "ovm7698"),
+	CCS_IDENT_L(0x0b, 0x4242, -1, "smiapp-003"),
+	CCS_IDENT_L(0x0c, 0x208a, -1, "tcm8330md"),
+	CCS_IDENT_LQ(0x0c, 0x2134, -1, "tcm8500md", &smiapp_tcm8500md_quirk),
+	CCS_IDENT_L(0x0c, 0x213e, -1, "et8en2"),
+	CCS_IDENT_L(0x0c, 0x2184, -1, "tcm8580md"),
+	CCS_IDENT_LQ(0x0c, 0x560f, -1, "jt8ew9", &smiapp_jt8ew9_quirk),
+	CCS_IDENT_LQ(0x10, 0x4141, -1, "jt8ev1", &smiapp_jt8ev1_quirk),
+	CCS_IDENT_LQ(0x10, 0x4241, -1, "imx125es", &smiapp_imx125es_quirk),
+};
+
+#define CCS_DEVICE_FLAG_IS_SMIA		BIT(0)
+
+struct ccs_device {
+	unsigned char flags;
+};
+
+static const char * const ccs_regulators[] = { "vcore", "vio", "vana" };
+
+/*
+ *
+ * Dynamic Capability Identification
+ *
+ */
+
+static void ccs_assign_limit(void *ptr, unsigned int width, u32 val)
+{
+	switch (width) {
+	case sizeof(u8):
+		*(u8 *)ptr = val;
+		break;
+	case sizeof(u16):
+		*(u16 *)ptr = val;
+		break;
+	case sizeof(u32):
+		*(u32 *)ptr = val;
+		break;
+	}
+}
+
+static int ccs_limit_ptr(struct ccs_sensor *sensor, unsigned int limit,
+			 unsigned int offset, void **__ptr)
+{
+	const struct ccs_limit *linfo;
+
+	if (WARN_ON(limit >= CCS_L_LAST))
+		return -EINVAL;
+
+	linfo = &ccs_limits[ccs_limit_offsets[limit].info];
+
+	if (WARN_ON(!sensor->ccs_limits) ||
+	    WARN_ON(offset + ccs_reg_width(linfo->reg) >
+		    ccs_limit_offsets[limit + 1].lim))
+		return -EINVAL;
+
+	*__ptr = sensor->ccs_limits + ccs_limit_offsets[limit].lim + offset;
+
+	return 0;
+}
+
+void ccs_replace_limit(struct ccs_sensor *sensor,
+		       unsigned int limit, unsigned int offset, u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	const struct ccs_limit *linfo;
+	void *ptr;
+	int ret;
+
+	ret = ccs_limit_ptr(sensor, limit, offset, &ptr);
+	if (ret)
+		return;
+
+	linfo = &ccs_limits[ccs_limit_offsets[limit].info];
+
+	dev_dbg(&client->dev, "quirk: 0x%8.8x \"%s\" %u = %u, 0x%x\n",
+		linfo->reg, linfo->name, offset, val, val);
+
+	ccs_assign_limit(ptr, ccs_reg_width(linfo->reg), val);
+}
+
+u32 ccs_get_limit(struct ccs_sensor *sensor, unsigned int limit,
+		  unsigned int offset)
+{
+	void *ptr;
+	u32 val;
+	int ret;
+
+	ret = ccs_limit_ptr(sensor, limit, offset, &ptr);
+	if (ret)
+		return 0;
+
+	switch (ccs_reg_width(ccs_limits[ccs_limit_offsets[limit].info].reg)) {
+	case sizeof(u8):
+		val = *(u8 *)ptr;
+		break;
+	case sizeof(u16):
+		val = *(u16 *)ptr;
+		break;
+	case sizeof(u32):
+		val = *(u32 *)ptr;
+		break;
+	default:
+		WARN_ON(1);
+		return 0;
+	}
+
+	return ccs_reg_conv(sensor, ccs_limits[limit].reg, val);
+}
+
+static int ccs_read_all_limits(struct ccs_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	void *ptr, *alloc, *end;
+	unsigned int i, l;
+	int ret;
+
+	kfree(sensor->ccs_limits);
+	sensor->ccs_limits = NULL;
+
+	alloc = kzalloc(ccs_limit_offsets[CCS_L_LAST].lim, GFP_KERNEL);
+	if (!alloc)
+		return -ENOMEM;
+
+	end = alloc + ccs_limit_offsets[CCS_L_LAST].lim;
+
+	for (i = 0, l = 0, ptr = alloc; ccs_limits[i].size; i++) {
+		u32 reg = ccs_limits[i].reg;
+		unsigned int width = ccs_reg_width(reg);
+		unsigned int j;
+
+		if (l == CCS_L_LAST) {
+			dev_err(&client->dev,
+				"internal error --- end of limit array\n");
+			ret = -EINVAL;
+			goto out_err;
+		}
+
+		for (j = 0; j < ccs_limits[i].size / width;
+		     j++, reg += width, ptr += width) {
+			u32 val;
+
+			ret = ccs_read_addr_noconv(sensor, reg, &val);
+			if (ret)
+				goto out_err;
+
+			if (ptr + width > end) {
+				dev_err(&client->dev,
+					"internal error --- no room for regs\n");
+				ret = -EINVAL;
+				goto out_err;
+			}
+
+			if (!val && j)
+				break;
+
+			ccs_assign_limit(ptr, width, val);
+
+			dev_dbg(&client->dev, "0x%8.8x \"%s\" = %u, 0x%x\n",
+				reg, ccs_limits[i].name, val, val);
+		}
+
+		if (ccs_limits[i].flags & CCS_L_FL_SAME_REG)
+			continue;
+
+		l++;
+		ptr = alloc + ccs_limit_offsets[l].lim;
+	}
+
+	if (l != CCS_L_LAST) {
+		dev_err(&client->dev,
+			"internal error --- insufficient limits\n");
+		ret = -EINVAL;
+		goto out_err;
+	}
+
+	sensor->ccs_limits = alloc;
+
+	if (CCS_LIM(sensor, SCALER_N_MIN) < 16)
+		ccs_replace_limit(sensor, CCS_L_SCALER_N_MIN, 0, 16);
+
+	return 0;
+
+out_err:
+	kfree(alloc);
+
+	return ret;
+}
+
+static int ccs_read_frame_fmt(struct ccs_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	u8 fmt_model_type, fmt_model_subtype, ncol_desc, nrow_desc;
+	unsigned int i;
+	int pixel_count = 0;
+	int line_count = 0;
+
+	fmt_model_type = CCS_LIM(sensor, FRAME_FORMAT_MODEL_TYPE);
+	fmt_model_subtype = CCS_LIM(sensor, FRAME_FORMAT_MODEL_SUBTYPE);
+
+	ncol_desc = (fmt_model_subtype
+		     & CCS_FRAME_FORMAT_MODEL_SUBTYPE_COLUMNS_MASK)
+		>> CCS_FRAME_FORMAT_MODEL_SUBTYPE_COLUMNS_SHIFT;
+	nrow_desc = fmt_model_subtype
+		& CCS_FRAME_FORMAT_MODEL_SUBTYPE_ROWS_MASK;
+
+	dev_dbg(&client->dev, "format_model_type %s\n",
+		fmt_model_type == CCS_FRAME_FORMAT_MODEL_TYPE_2_BYTE
+		? "2 byte" :
+		fmt_model_type == CCS_FRAME_FORMAT_MODEL_TYPE_4_BYTE
+		? "4 byte" : "is simply bad");
+
+	dev_dbg(&client->dev, "%u column and %u row descriptors\n",
+		ncol_desc, nrow_desc);
+
+	for (i = 0; i < ncol_desc + nrow_desc; i++) {
+		u32 desc;
+		u32 pixelcode;
+		u32 pixels;
+		char *which;
+		char *what;
+
+		if (fmt_model_type == CCS_FRAME_FORMAT_MODEL_TYPE_2_BYTE) {
+			desc = CCS_LIM_AT(sensor, FRAME_FORMAT_DESCRIPTOR, i);
+
+			pixelcode =
+				(desc
+				 & CCS_FRAME_FORMAT_DESCRIPTOR_PCODE_MASK)
+				>> CCS_FRAME_FORMAT_DESCRIPTOR_PCODE_SHIFT;
+			pixels = desc & CCS_FRAME_FORMAT_DESCRIPTOR_PIXELS_MASK;
+		} else if (fmt_model_type
+			   == CCS_FRAME_FORMAT_MODEL_TYPE_4_BYTE) {
+			desc = CCS_LIM_AT(sensor, FRAME_FORMAT_DESCRIPTOR_4, i);
+
+			pixelcode =
+				(desc
+				 & CCS_FRAME_FORMAT_DESCRIPTOR_4_PCODE_MASK)
+				>> CCS_FRAME_FORMAT_DESCRIPTOR_4_PCODE_SHIFT;
+			pixels = desc &
+				CCS_FRAME_FORMAT_DESCRIPTOR_4_PIXELS_MASK;
+		} else {
+			dev_dbg(&client->dev,
+				"invalid frame format model type %u\n",
+				fmt_model_type);
+			return -EINVAL;
+		}
+
+		if (i < ncol_desc)
+			which = "columns";
+		else
+			which = "rows";
+
+		switch (pixelcode) {
+		case CCS_FRAME_FORMAT_DESCRIPTOR_PCODE_EMBEDDED:
+			what = "embedded";
+			break;
+		case CCS_FRAME_FORMAT_DESCRIPTOR_PCODE_DUMMY_PIXEL:
+			what = "dummy";
+			break;
+		case CCS_FRAME_FORMAT_DESCRIPTOR_PCODE_BLACK_PIXEL:
+			what = "black";
+			break;
+		case CCS_FRAME_FORMAT_DESCRIPTOR_PCODE_DARK_PIXEL:
+			what = "dark";
+			break;
+		case CCS_FRAME_FORMAT_DESCRIPTOR_PCODE_VISIBLE_PIXEL:
+			what = "visible";
+			break;
+		default:
+			what = "invalid";
+			break;
+		}
+
+		dev_dbg(&client->dev,
+			"%s pixels: %u %s (pixelcode %u)\n",
+			what, pixels, which, pixelcode);
+
+		if (i < ncol_desc) {
+			if (pixelcode ==
+			    CCS_FRAME_FORMAT_DESCRIPTOR_PCODE_VISIBLE_PIXEL)
+				sensor->visible_pixel_start = pixel_count;
+			pixel_count += pixels;
+			continue;
+		}
+
+		/* Handle row descriptors */
+		switch (pixelcode) {
+		case CCS_FRAME_FORMAT_DESCRIPTOR_PCODE_EMBEDDED:
+			if (sensor->embedded_end)
+				break;
+			sensor->embedded_start = line_count;
+			sensor->embedded_end = line_count + pixels;
+			break;
+		case CCS_FRAME_FORMAT_DESCRIPTOR_PCODE_VISIBLE_PIXEL:
+			sensor->image_start = line_count;
+			break;
+		}
+		line_count += pixels;
+	}
+
+	if (sensor->embedded_end > sensor->image_start) {
+		dev_dbg(&client->dev,
+			"adjusting image start line to %u (was %u)\n",
+			sensor->embedded_end, sensor->image_start);
+		sensor->image_start = sensor->embedded_end;
+	}
+
+	dev_dbg(&client->dev, "embedded data from lines %u to %u\n",
+		sensor->embedded_start, sensor->embedded_end);
+	dev_dbg(&client->dev, "image data starts at line %u\n",
+		sensor->image_start);
+
+	return 0;
+}
+
+static int ccs_pll_configure(struct ccs_sensor *sensor)
+{
+	struct ccs_pll *pll = &sensor->pll;
+	int rval;
+
+	rval = ccs_write(sensor, VT_PIX_CLK_DIV, pll->vt_bk.pix_clk_div);
+	if (rval < 0)
+		return rval;
+
+	rval = ccs_write(sensor, VT_SYS_CLK_DIV, pll->vt_bk.sys_clk_div);
+	if (rval < 0)
+		return rval;
+
+	rval = ccs_write(sensor, PRE_PLL_CLK_DIV, pll->vt_fr.pre_pll_clk_div);
+	if (rval < 0)
+		return rval;
+
+	rval = ccs_write(sensor, PLL_MULTIPLIER, pll->vt_fr.pll_multiplier);
+	if (rval < 0)
+		return rval;
+
+	if (!(CCS_LIM(sensor, PHY_CTRL_CAPABILITY) &
+	      CCS_PHY_CTRL_CAPABILITY_AUTO_PHY_CTL)) {
+		/* Lane op clock ratio does not apply here. */
+		rval = ccs_write(sensor, REQUESTED_LINK_RATE,
+				 DIV_ROUND_UP(pll->op_bk.sys_clk_freq_hz,
+					      1000000 / 256 / 256) *
+				 (pll->flags & CCS_PLL_FLAG_LANE_SPEED_MODEL ?
+				  sensor->pll.csi2.lanes : 1) <<
+				 (pll->flags & CCS_PLL_FLAG_OP_SYS_DDR ?
+				  1 : 0));
+		if (rval < 0)
+			return rval;
+	}
+
+	if (sensor->pll.flags & CCS_PLL_FLAG_NO_OP_CLOCKS)
+		return 0;
+
+	rval = ccs_write(sensor, OP_PIX_CLK_DIV, pll->op_bk.pix_clk_div);
+	if (rval < 0)
+		return rval;
+
+	rval = ccs_write(sensor, OP_SYS_CLK_DIV, pll->op_bk.sys_clk_div);
+	if (rval < 0)
+		return rval;
+
+	if (!(pll->flags & CCS_PLL_FLAG_DUAL_PLL))
+		return 0;
+
+	rval = ccs_write(sensor, PLL_MODE, CCS_PLL_MODE_DUAL);
+	if (rval < 0)
+		return rval;
+
+	rval = ccs_write(sensor, OP_PRE_PLL_CLK_DIV,
+			 pll->op_fr.pre_pll_clk_div);
+	if (rval < 0)
+		return rval;
+
+	return ccs_write(sensor, OP_PLL_MULTIPLIER, pll->op_fr.pll_multiplier);
+}
+
+static int ccs_pll_try(struct ccs_sensor *sensor, struct ccs_pll *pll)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	struct ccs_pll_limits lim = {
+		.vt_fr = {
+			.min_pre_pll_clk_div = CCS_LIM(sensor, MIN_PRE_PLL_CLK_DIV),
+			.max_pre_pll_clk_div = CCS_LIM(sensor, MAX_PRE_PLL_CLK_DIV),
+			.min_pll_ip_clk_freq_hz = CCS_LIM(sensor, MIN_PLL_IP_CLK_FREQ_MHZ),
+			.max_pll_ip_clk_freq_hz = CCS_LIM(sensor, MAX_PLL_IP_CLK_FREQ_MHZ),
+			.min_pll_multiplier = CCS_LIM(sensor, MIN_PLL_MULTIPLIER),
+			.max_pll_multiplier = CCS_LIM(sensor, MAX_PLL_MULTIPLIER),
+			.min_pll_op_clk_freq_hz = CCS_LIM(sensor, MIN_PLL_OP_CLK_FREQ_MHZ),
+			.max_pll_op_clk_freq_hz = CCS_LIM(sensor, MAX_PLL_OP_CLK_FREQ_MHZ),
+		},
+		.op_fr = {
+			.min_pre_pll_clk_div = CCS_LIM(sensor, MIN_OP_PRE_PLL_CLK_DIV),
+			.max_pre_pll_clk_div = CCS_LIM(sensor, MAX_OP_PRE_PLL_CLK_DIV),
+			.min_pll_ip_clk_freq_hz = CCS_LIM(sensor, MIN_OP_PLL_IP_CLK_FREQ_MHZ),
+			.max_pll_ip_clk_freq_hz = CCS_LIM(sensor, MAX_OP_PLL_IP_CLK_FREQ_MHZ),
+			.min_pll_multiplier = CCS_LIM(sensor, MIN_OP_PLL_MULTIPLIER),
+			.max_pll_multiplier = CCS_LIM(sensor, MAX_OP_PLL_MULTIPLIER),
+			.min_pll_op_clk_freq_hz = CCS_LIM(sensor, MIN_OP_PLL_OP_CLK_FREQ_MHZ),
+			.max_pll_op_clk_freq_hz = CCS_LIM(sensor, MAX_OP_PLL_OP_CLK_FREQ_MHZ),
+		},
+		.op_bk = {
+			 .min_sys_clk_div = CCS_LIM(sensor, MIN_OP_SYS_CLK_DIV),
+			 .max_sys_clk_div = CCS_LIM(sensor, MAX_OP_SYS_CLK_DIV),
+			 .min_pix_clk_div = CCS_LIM(sensor, MIN_OP_PIX_CLK_DIV),
+			 .max_pix_clk_div = CCS_LIM(sensor, MAX_OP_PIX_CLK_DIV),
+			 .min_sys_clk_freq_hz = CCS_LIM(sensor, MIN_OP_SYS_CLK_FREQ_MHZ),
+			 .max_sys_clk_freq_hz = CCS_LIM(sensor, MAX_OP_SYS_CLK_FREQ_MHZ),
+			 .min_pix_clk_freq_hz = CCS_LIM(sensor, MIN_OP_PIX_CLK_FREQ_MHZ),
+			 .max_pix_clk_freq_hz = CCS_LIM(sensor, MAX_OP_PIX_CLK_FREQ_MHZ),
+		 },
+		.vt_bk = {
+			 .min_sys_clk_div = CCS_LIM(sensor, MIN_VT_SYS_CLK_DIV),
+			 .max_sys_clk_div = CCS_LIM(sensor, MAX_VT_SYS_CLK_DIV),
+			 .min_pix_clk_div = CCS_LIM(sensor, MIN_VT_PIX_CLK_DIV),
+			 .max_pix_clk_div = CCS_LIM(sensor, MAX_VT_PIX_CLK_DIV),
+			 .min_sys_clk_freq_hz = CCS_LIM(sensor, MIN_VT_SYS_CLK_FREQ_MHZ),
+			 .max_sys_clk_freq_hz = CCS_LIM(sensor, MAX_VT_SYS_CLK_FREQ_MHZ),
+			 .min_pix_clk_freq_hz = CCS_LIM(sensor, MIN_VT_PIX_CLK_FREQ_MHZ),
+			 .max_pix_clk_freq_hz = CCS_LIM(sensor, MAX_VT_PIX_CLK_FREQ_MHZ),
+		 },
+		.min_line_length_pck_bin = CCS_LIM(sensor, MIN_LINE_LENGTH_PCK_BIN),
+		.min_line_length_pck = CCS_LIM(sensor, MIN_LINE_LENGTH_PCK),
+	};
+
+	return ccs_pll_calculate(&client->dev, &lim, pll);
+}
+
+static int ccs_pll_update(struct ccs_sensor *sensor)
+{
+	struct ccs_pll *pll = &sensor->pll;
+	int rval;
+
+	pll->binning_horizontal = sensor->binning_horizontal;
+	pll->binning_vertical = sensor->binning_vertical;
+	pll->link_freq =
+		sensor->link_freq->qmenu_int[sensor->link_freq->val];
+	pll->scale_m = sensor->scale_m;
+	pll->bits_per_pixel = sensor->csi_format->compressed;
+
+	rval = ccs_pll_try(sensor, pll);
+	if (rval < 0)
+		return rval;
+
+	__v4l2_ctrl_s_ctrl_int64(sensor->pixel_rate_parray,
+				 pll->pixel_rate_pixel_array);
+	__v4l2_ctrl_s_ctrl_int64(sensor->pixel_rate_csi, pll->pixel_rate_csi);
+
+	return 0;
+}
+
+
+/*
+ *
+ * V4L2 Controls handling
+ *
+ */
+
+static void __ccs_update_exposure_limits(struct ccs_sensor *sensor)
+{
+	struct v4l2_ctrl *ctrl = sensor->exposure;
+	int max;
+
+	max = sensor->pixel_array->crop[CCS_PA_PAD_SRC].height
+		+ sensor->vblank->val
+		- CCS_LIM(sensor, COARSE_INTEGRATION_TIME_MAX_MARGIN);
+
+	__v4l2_ctrl_modify_range(ctrl, ctrl->minimum, max, ctrl->step, max);
+}
+
+/*
+ * Order matters.
+ *
+ * 1. Bits-per-pixel, descending.
+ * 2. Bits-per-pixel compressed, descending.
+ * 3. Pixel order, same as in pixel_order_str. Formats for all four pixel
+ *    orders must be defined.
+ */
+static const struct ccs_csi_data_format ccs_csi_data_formats[] = {
+	{ MEDIA_BUS_FMT_SGRBG16_1X16, 16, 16, CCS_PIXEL_ORDER_GRBG, },
+	{ MEDIA_BUS_FMT_SRGGB16_1X16, 16, 16, CCS_PIXEL_ORDER_RGGB, },
+	{ MEDIA_BUS_FMT_SBGGR16_1X16, 16, 16, CCS_PIXEL_ORDER_BGGR, },
+	{ MEDIA_BUS_FMT_SGBRG16_1X16, 16, 16, CCS_PIXEL_ORDER_GBRG, },
+	{ MEDIA_BUS_FMT_SGRBG14_1X14, 14, 14, CCS_PIXEL_ORDER_GRBG, },
+	{ MEDIA_BUS_FMT_SRGGB14_1X14, 14, 14, CCS_PIXEL_ORDER_RGGB, },
+	{ MEDIA_BUS_FMT_SBGGR14_1X14, 14, 14, CCS_PIXEL_ORDER_BGGR, },
+	{ MEDIA_BUS_FMT_SGBRG14_1X14, 14, 14, CCS_PIXEL_ORDER_GBRG, },
+	{ MEDIA_BUS_FMT_SGRBG12_1X12, 12, 12, CCS_PIXEL_ORDER_GRBG, },
+	{ MEDIA_BUS_FMT_SRGGB12_1X12, 12, 12, CCS_PIXEL_ORDER_RGGB, },
+	{ MEDIA_BUS_FMT_SBGGR12_1X12, 12, 12, CCS_PIXEL_ORDER_BGGR, },
+	{ MEDIA_BUS_FMT_SGBRG12_1X12, 12, 12, CCS_PIXEL_ORDER_GBRG, },
+	{ MEDIA_BUS_FMT_SGRBG10_1X10, 10, 10, CCS_PIXEL_ORDER_GRBG, },
+	{ MEDIA_BUS_FMT_SRGGB10_1X10, 10, 10, CCS_PIXEL_ORDER_RGGB, },
+	{ MEDIA_BUS_FMT_SBGGR10_1X10, 10, 10, CCS_PIXEL_ORDER_BGGR, },
+	{ MEDIA_BUS_FMT_SGBRG10_1X10, 10, 10, CCS_PIXEL_ORDER_GBRG, },
+	{ MEDIA_BUS_FMT_SGRBG10_DPCM8_1X8, 10, 8, CCS_PIXEL_ORDER_GRBG, },
+	{ MEDIA_BUS_FMT_SRGGB10_DPCM8_1X8, 10, 8, CCS_PIXEL_ORDER_RGGB, },
+	{ MEDIA_BUS_FMT_SBGGR10_DPCM8_1X8, 10, 8, CCS_PIXEL_ORDER_BGGR, },
+	{ MEDIA_BUS_FMT_SGBRG10_DPCM8_1X8, 10, 8, CCS_PIXEL_ORDER_GBRG, },
+	{ MEDIA_BUS_FMT_SGRBG8_1X8, 8, 8, CCS_PIXEL_ORDER_GRBG, },
+	{ MEDIA_BUS_FMT_SRGGB8_1X8, 8, 8, CCS_PIXEL_ORDER_RGGB, },
+	{ MEDIA_BUS_FMT_SBGGR8_1X8, 8, 8, CCS_PIXEL_ORDER_BGGR, },
+	{ MEDIA_BUS_FMT_SGBRG8_1X8, 8, 8, CCS_PIXEL_ORDER_GBRG, },
+};
+
+static const char *pixel_order_str[] = { "GRBG", "RGGB", "BGGR", "GBRG" };
+
+#define to_csi_format_idx(fmt) (((unsigned long)(fmt)			\
+				 - (unsigned long)ccs_csi_data_formats) \
+				/ sizeof(*ccs_csi_data_formats))
+
+static u32 ccs_pixel_order(struct ccs_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	int flip = 0;
+
+	if (sensor->hflip) {
+		if (sensor->hflip->val)
+			flip |= CCS_IMAGE_ORIENTATION_HORIZONTAL_MIRROR;
+
+		if (sensor->vflip->val)
+			flip |= CCS_IMAGE_ORIENTATION_VERTICAL_FLIP;
+	}
+
+	dev_dbg(&client->dev, "flip %u\n", flip);
+	return sensor->default_pixel_order ^ flip;
+}
+
+static void ccs_update_mbus_formats(struct ccs_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	unsigned int csi_format_idx =
+		to_csi_format_idx(sensor->csi_format) & ~3;
+	unsigned int internal_csi_format_idx =
+		to_csi_format_idx(sensor->internal_csi_format) & ~3;
+	unsigned int pixel_order = ccs_pixel_order(sensor);
+
+	if (WARN_ON_ONCE(max(internal_csi_format_idx, csi_format_idx) +
+			 pixel_order >= ARRAY_SIZE(ccs_csi_data_formats)))
+		return;
+
+	sensor->mbus_frame_fmts =
+		sensor->default_mbus_frame_fmts << pixel_order;
+	sensor->csi_format =
+		&ccs_csi_data_formats[csi_format_idx + pixel_order];
+	sensor->internal_csi_format =
+		&ccs_csi_data_formats[internal_csi_format_idx
+					 + pixel_order];
+
+	dev_dbg(&client->dev, "new pixel order %s\n",
+		pixel_order_str[pixel_order]);
+}
+
+static const char * const ccs_test_patterns[] = {
+	"Disabled",
+	"Solid Colour",
+	"Eight Vertical Colour Bars",
+	"Colour Bars With Fade to Grey",
+	"Pseudorandom Sequence (PN9)",
+};
+
+static int ccs_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ccs_sensor *sensor =
+		container_of(ctrl->handler, struct ccs_subdev, ctrl_handler)
+			->sensor;
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	int pm_status;
+	u32 orient = 0;
+	unsigned int i;
+	int exposure;
+	int rval;
+
+	switch (ctrl->id) {
+	case V4L2_CID_HFLIP:
+	case V4L2_CID_VFLIP:
+		if (sensor->streaming)
+			return -EBUSY;
+
+		if (sensor->hflip->val)
+			orient |= CCS_IMAGE_ORIENTATION_HORIZONTAL_MIRROR;
+
+		if (sensor->vflip->val)
+			orient |= CCS_IMAGE_ORIENTATION_VERTICAL_FLIP;
+
+		ccs_update_mbus_formats(sensor);
+
+		break;
+	case V4L2_CID_VBLANK:
+		exposure = sensor->exposure->val;
+
+		__ccs_update_exposure_limits(sensor);
+
+		if (exposure > sensor->exposure->maximum) {
+			sensor->exposure->val =	sensor->exposure->maximum;
+			rval = ccs_set_ctrl(sensor->exposure);
+			if (rval < 0)
+				return rval;
+		}
+
+		break;
+	case V4L2_CID_LINK_FREQ:
+		if (sensor->streaming)
+			return -EBUSY;
+
+		rval = ccs_pll_update(sensor);
+		if (rval)
+			return rval;
+
+		return 0;
+	case V4L2_CID_TEST_PATTERN:
+		for (i = 0; i < ARRAY_SIZE(sensor->test_data); i++)
+			v4l2_ctrl_activate(
+				sensor->test_data[i],
+				ctrl->val ==
+				V4L2_SMIAPP_TEST_PATTERN_MODE_SOLID_COLOUR);
+
+		break;
+	}
+
+	pm_status = pm_runtime_get_if_active(&client->dev, true);
+	if (!pm_status)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_ANALOGUE_GAIN:
+		rval = ccs_write(sensor, ANALOG_GAIN_CODE_GLOBAL, ctrl->val);
+
+		break;
+
+	case V4L2_CID_CCS_ANALOGUE_LINEAR_GAIN:
+		rval = ccs_write(sensor, ANALOG_LINEAR_GAIN_GLOBAL, ctrl->val);
+
+		break;
+
+	case V4L2_CID_CCS_ANALOGUE_EXPONENTIAL_GAIN:
+		rval = ccs_write(sensor, ANALOG_EXPONENTIAL_GAIN_GLOBAL,
+				 ctrl->val);
+
+		break;
+
+	case V4L2_CID_DIGITAL_GAIN:
+		if (CCS_LIM(sensor, DIGITAL_GAIN_CAPABILITY) ==
+		    CCS_DIGITAL_GAIN_CAPABILITY_GLOBAL) {
+			rval = ccs_write(sensor, DIGITAL_GAIN_GLOBAL,
+					 ctrl->val);
+			break;
+		}
+
+		rval = ccs_write_addr(sensor,
+				      SMIAPP_REG_U16_DIGITAL_GAIN_GREENR,
+				      ctrl->val);
+		if (rval)
+			break;
+
+		rval = ccs_write_addr(sensor,
+				      SMIAPP_REG_U16_DIGITAL_GAIN_RED,
+				      ctrl->val);
+		if (rval)
+			break;
+
+		rval = ccs_write_addr(sensor,
+				      SMIAPP_REG_U16_DIGITAL_GAIN_BLUE,
+				      ctrl->val);
+		if (rval)
+			break;
+
+		rval = ccs_write_addr(sensor,
+				      SMIAPP_REG_U16_DIGITAL_GAIN_GREENB,
+				      ctrl->val);
+
+		break;
+	case V4L2_CID_EXPOSURE:
+		rval = ccs_write(sensor, COARSE_INTEGRATION_TIME, ctrl->val);
+
+		break;
+	case V4L2_CID_HFLIP:
+	case V4L2_CID_VFLIP:
+		rval = ccs_write(sensor, IMAGE_ORIENTATION, orient);
+
+		break;
+	case V4L2_CID_VBLANK:
+		rval = ccs_write(sensor, FRAME_LENGTH_LINES,
+				 sensor->pixel_array->crop[
+					 CCS_PA_PAD_SRC].height
+				 + ctrl->val);
+
+		break;
+	case V4L2_CID_HBLANK:
+		rval = ccs_write(sensor, LINE_LENGTH_PCK,
+				 sensor->pixel_array->crop[CCS_PA_PAD_SRC].width
+				 + ctrl->val);
+
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		rval = ccs_write(sensor, TEST_PATTERN_MODE, ctrl->val);
+
+		break;
+	case V4L2_CID_TEST_PATTERN_RED:
+		rval = ccs_write(sensor, TEST_DATA_RED, ctrl->val);
+
+		break;
+	case V4L2_CID_TEST_PATTERN_GREENR:
+		rval = ccs_write(sensor, TEST_DATA_GREENR, ctrl->val);
+
+		break;
+	case V4L2_CID_TEST_PATTERN_BLUE:
+		rval = ccs_write(sensor, TEST_DATA_BLUE, ctrl->val);
+
+		break;
+	case V4L2_CID_TEST_PATTERN_GREENB:
+		rval = ccs_write(sensor, TEST_DATA_GREENB, ctrl->val);
+
+		break;
+	case V4L2_CID_CCS_SHADING_CORRECTION:
+		rval = ccs_write(sensor, SHADING_CORRECTION_EN,
+				 ctrl->val ? CCS_SHADING_CORRECTION_EN_ENABLE :
+				 0);
+
+		if (!rval && sensor->luminance_level)
+			v4l2_ctrl_activate(sensor->luminance_level, ctrl->val);
+
+		break;
+	case V4L2_CID_CCS_LUMINANCE_CORRECTION_LEVEL:
+		rval = ccs_write(sensor, LUMINANCE_CORRECTION_LEVEL, ctrl->val);
+
+		break;
+	case V4L2_CID_PIXEL_RATE:
+		/* For v4l2_ctrl_s_ctrl_int64() used internally. */
+		rval = 0;
+
+		break;
+	default:
+		rval = -EINVAL;
+	}
+
+	if (pm_status > 0) {
+		pm_runtime_mark_last_busy(&client->dev);
+		pm_runtime_put_autosuspend(&client->dev);
+	}
+
+	return rval;
+}
+
+static const struct v4l2_ctrl_ops ccs_ctrl_ops = {
+	.s_ctrl = ccs_set_ctrl,
+};
+
+static int ccs_init_controls(struct ccs_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	struct v4l2_fwnode_device_properties props;
+	int rval;
+
+	rval = v4l2_ctrl_handler_init(&sensor->pixel_array->ctrl_handler, 19);
+	if (rval)
+		return rval;
+
+	sensor->pixel_array->ctrl_handler.lock = &sensor->mutex;
+
+	rval = v4l2_fwnode_device_parse(&client->dev, &props);
+	if (rval)
+		return rval;
+
+	rval = v4l2_ctrl_new_fwnode_properties(&sensor->pixel_array->ctrl_handler,
+					       &ccs_ctrl_ops, &props);
+	if (rval)
+		return rval;
+
+	switch (CCS_LIM(sensor, ANALOG_GAIN_CAPABILITY)) {
+	case CCS_ANALOG_GAIN_CAPABILITY_GLOBAL: {
+		struct {
+			const char *name;
+			u32 id;
+			s32 value;
+		} const gain_ctrls[] = {
+			{ "Analogue Gain m0", V4L2_CID_CCS_ANALOGUE_GAIN_M0,
+			  CCS_LIM(sensor, ANALOG_GAIN_M0), },
+			{ "Analogue Gain c0", V4L2_CID_CCS_ANALOGUE_GAIN_C0,
+			  CCS_LIM(sensor, ANALOG_GAIN_C0), },
+			{ "Analogue Gain m1", V4L2_CID_CCS_ANALOGUE_GAIN_M1,
+			  CCS_LIM(sensor, ANALOG_GAIN_M1), },
+			{ "Analogue Gain c1", V4L2_CID_CCS_ANALOGUE_GAIN_C1,
+			  CCS_LIM(sensor, ANALOG_GAIN_C1), },
+		};
+		struct v4l2_ctrl_config ctrl_cfg = {
+			.type = V4L2_CTRL_TYPE_INTEGER,
+			.ops = &ccs_ctrl_ops,
+			.flags = V4L2_CTRL_FLAG_READ_ONLY,
+			.step = 1,
+		};
+		unsigned int i;
+
+		for (i = 0; i < ARRAY_SIZE(gain_ctrls); i++) {
+			ctrl_cfg.name = gain_ctrls[i].name;
+			ctrl_cfg.id = gain_ctrls[i].id;
+			ctrl_cfg.min = ctrl_cfg.max = ctrl_cfg.def =
+				gain_ctrls[i].value;
+
+			v4l2_ctrl_new_custom(&sensor->pixel_array->ctrl_handler,
+					     &ctrl_cfg, NULL);
+		}
+
+		v4l2_ctrl_new_std(&sensor->pixel_array->ctrl_handler,
+				  &ccs_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
+				  CCS_LIM(sensor, ANALOG_GAIN_CODE_MIN),
+				  CCS_LIM(sensor, ANALOG_GAIN_CODE_MAX),
+				  max(CCS_LIM(sensor, ANALOG_GAIN_CODE_STEP),
+				      1U),
+				  CCS_LIM(sensor, ANALOG_GAIN_CODE_MIN));
+	}
+		break;
+
+	case CCS_ANALOG_GAIN_CAPABILITY_ALTERNATE_GLOBAL: {
+		struct {
+			const char *name;
+			u32 id;
+			u16 min, max, step;
+		} const gain_ctrls[] = {
+			{
+				"Analogue Linear Gain",
+				V4L2_CID_CCS_ANALOGUE_LINEAR_GAIN,
+				CCS_LIM(sensor, ANALOG_LINEAR_GAIN_MIN),
+				CCS_LIM(sensor, ANALOG_LINEAR_GAIN_MAX),
+				max(CCS_LIM(sensor,
+					    ANALOG_LINEAR_GAIN_STEP_SIZE),
+				    1U),
+			},
+			{
+				"Analogue Exponential Gain",
+				V4L2_CID_CCS_ANALOGUE_EXPONENTIAL_GAIN,
+				CCS_LIM(sensor, ANALOG_EXPONENTIAL_GAIN_MIN),
+				CCS_LIM(sensor, ANALOG_EXPONENTIAL_GAIN_MAX),
+				max(CCS_LIM(sensor,
+					    ANALOG_EXPONENTIAL_GAIN_STEP_SIZE),
+				    1U),
+			},
+		};
+		struct v4l2_ctrl_config ctrl_cfg = {
+			.type = V4L2_CTRL_TYPE_INTEGER,
+			.ops = &ccs_ctrl_ops,
+		};
+		unsigned int i;
+
+		for (i = 0; i < ARRAY_SIZE(gain_ctrls); i++) {
+			ctrl_cfg.name = gain_ctrls[i].name;
+			ctrl_cfg.min = ctrl_cfg.def = gain_ctrls[i].min;
+			ctrl_cfg.max = gain_ctrls[i].max;
+			ctrl_cfg.step = gain_ctrls[i].step;
+			ctrl_cfg.id = gain_ctrls[i].id;
+
+			v4l2_ctrl_new_custom(&sensor->pixel_array->ctrl_handler,
+					     &ctrl_cfg, NULL);
+		}
+	}
+	}
+
+	if (CCS_LIM(sensor, SHADING_CORRECTION_CAPABILITY) &
+	    (CCS_SHADING_CORRECTION_CAPABILITY_COLOR_SHADING |
+	     CCS_SHADING_CORRECTION_CAPABILITY_LUMINANCE_CORRECTION)) {
+		const struct v4l2_ctrl_config ctrl_cfg = {
+			.name = "Shading Correction",
+			.type = V4L2_CTRL_TYPE_BOOLEAN,
+			.id = V4L2_CID_CCS_SHADING_CORRECTION,
+			.ops = &ccs_ctrl_ops,
+			.max = 1,
+			.step = 1,
+		};
+
+		v4l2_ctrl_new_custom(&sensor->pixel_array->ctrl_handler,
+				     &ctrl_cfg, NULL);
+	}
+
+	if (CCS_LIM(sensor, SHADING_CORRECTION_CAPABILITY) &
+	    CCS_SHADING_CORRECTION_CAPABILITY_LUMINANCE_CORRECTION) {
+		const struct v4l2_ctrl_config ctrl_cfg = {
+			.name = "Luminance Correction Level",
+			.type = V4L2_CTRL_TYPE_BOOLEAN,
+			.id = V4L2_CID_CCS_LUMINANCE_CORRECTION_LEVEL,
+			.ops = &ccs_ctrl_ops,
+			.max = 255,
+			.step = 1,
+			.def = 128,
+		};
+
+		sensor->luminance_level =
+			v4l2_ctrl_new_custom(&sensor->pixel_array->ctrl_handler,
+					     &ctrl_cfg, NULL);
+	}
+
+	if (CCS_LIM(sensor, DIGITAL_GAIN_CAPABILITY) ==
+	    CCS_DIGITAL_GAIN_CAPABILITY_GLOBAL ||
+	    CCS_LIM(sensor, DIGITAL_GAIN_CAPABILITY) ==
+	    SMIAPP_DIGITAL_GAIN_CAPABILITY_PER_CHANNEL)
+		v4l2_ctrl_new_std(&sensor->pixel_array->ctrl_handler,
+				  &ccs_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
+				  CCS_LIM(sensor, DIGITAL_GAIN_MIN),
+				  CCS_LIM(sensor, DIGITAL_GAIN_MAX),
+				  max(CCS_LIM(sensor, DIGITAL_GAIN_STEP_SIZE),
+				      1U),
+				  0x100);
+
+	/* Exposure limits will be updated soon, use just something here. */
+	sensor->exposure = v4l2_ctrl_new_std(
+		&sensor->pixel_array->ctrl_handler, &ccs_ctrl_ops,
+		V4L2_CID_EXPOSURE, 0, 0, 1, 0);
+
+	sensor->hflip = v4l2_ctrl_new_std(
+		&sensor->pixel_array->ctrl_handler, &ccs_ctrl_ops,
+		V4L2_CID_HFLIP, 0, 1, 1, 0);
+	sensor->vflip = v4l2_ctrl_new_std(
+		&sensor->pixel_array->ctrl_handler, &ccs_ctrl_ops,
+		V4L2_CID_VFLIP, 0, 1, 1, 0);
+
+	sensor->vblank = v4l2_ctrl_new_std(
+		&sensor->pixel_array->ctrl_handler, &ccs_ctrl_ops,
+		V4L2_CID_VBLANK, 0, 1, 1, 0);
+
+	if (sensor->vblank)
+		sensor->vblank->flags |= V4L2_CTRL_FLAG_UPDATE;
+
+	sensor->hblank = v4l2_ctrl_new_std(
+		&sensor->pixel_array->ctrl_handler, &ccs_ctrl_ops,
+		V4L2_CID_HBLANK, 0, 1, 1, 0);
+
+	if (sensor->hblank)
+		sensor->hblank->flags |= V4L2_CTRL_FLAG_UPDATE;
+
+	sensor->pixel_rate_parray = v4l2_ctrl_new_std(
+		&sensor->pixel_array->ctrl_handler, &ccs_ctrl_ops,
+		V4L2_CID_PIXEL_RATE, 1, INT_MAX, 1, 1);
+
+	v4l2_ctrl_new_std_menu_items(&sensor->pixel_array->ctrl_handler,
+				     &ccs_ctrl_ops, V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(ccs_test_patterns) - 1,
+				     0, 0, ccs_test_patterns);
+
+	if (sensor->pixel_array->ctrl_handler.error) {
+		dev_err(&client->dev,
+			"pixel array controls initialization failed (%d)\n",
+			sensor->pixel_array->ctrl_handler.error);
+		return sensor->pixel_array->ctrl_handler.error;
+	}
+
+	sensor->pixel_array->sd.ctrl_handler =
+		&sensor->pixel_array->ctrl_handler;
+
+	v4l2_ctrl_cluster(2, &sensor->hflip);
+
+	rval = v4l2_ctrl_handler_init(&sensor->src->ctrl_handler, 0);
+	if (rval)
+		return rval;
+
+	sensor->src->ctrl_handler.lock = &sensor->mutex;
+
+	sensor->pixel_rate_csi = v4l2_ctrl_new_std(
+		&sensor->src->ctrl_handler, &ccs_ctrl_ops,
+		V4L2_CID_PIXEL_RATE, 1, INT_MAX, 1, 1);
+
+	if (sensor->src->ctrl_handler.error) {
+		dev_err(&client->dev,
+			"src controls initialization failed (%d)\n",
+			sensor->src->ctrl_handler.error);
+		return sensor->src->ctrl_handler.error;
+	}
+
+	sensor->src->sd.ctrl_handler = &sensor->src->ctrl_handler;
+
+	return 0;
+}
+
+/*
+ * For controls that require information on available media bus codes
+ * and linke frequencies.
+ */
+static int ccs_init_late_controls(struct ccs_sensor *sensor)
+{
+	unsigned long *valid_link_freqs = &sensor->valid_link_freqs[
+		sensor->csi_format->compressed - sensor->compressed_min_bpp];
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(sensor->test_data); i++) {
+		int max_value = (1 << sensor->csi_format->width) - 1;
+
+		sensor->test_data[i] = v4l2_ctrl_new_std(
+				&sensor->pixel_array->ctrl_handler,
+				&ccs_ctrl_ops, V4L2_CID_TEST_PATTERN_RED + i,
+				0, max_value, 1, max_value);
+	}
+
+	sensor->link_freq = v4l2_ctrl_new_int_menu(
+		&sensor->src->ctrl_handler, &ccs_ctrl_ops,
+		V4L2_CID_LINK_FREQ, __fls(*valid_link_freqs),
+		__ffs(*valid_link_freqs), sensor->hwcfg.op_sys_clock);
+
+	return sensor->src->ctrl_handler.error;
+}
+
+static void ccs_free_controls(struct ccs_sensor *sensor)
+{
+	unsigned int i;
+
+	for (i = 0; i < sensor->ssds_used; i++)
+		v4l2_ctrl_handler_free(&sensor->ssds[i].ctrl_handler);
+}
+
+static int ccs_get_mbus_formats(struct ccs_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	struct ccs_pll *pll = &sensor->pll;
+	u8 compressed_max_bpp = 0;
+	unsigned int type, n;
+	unsigned int i, pixel_order;
+	int rval;
+
+	type = CCS_LIM(sensor, DATA_FORMAT_MODEL_TYPE);
+
+	dev_dbg(&client->dev, "data_format_model_type %u\n", type);
+
+	rval = ccs_read(sensor, PIXEL_ORDER, &pixel_order);
+	if (rval)
+		return rval;
+
+	if (pixel_order >= ARRAY_SIZE(pixel_order_str)) {
+		dev_dbg(&client->dev, "bad pixel order %u\n", pixel_order);
+		return -EINVAL;
+	}
+
+	dev_dbg(&client->dev, "pixel order %u (%s)\n", pixel_order,
+		pixel_order_str[pixel_order]);
+
+	switch (type) {
+	case CCS_DATA_FORMAT_MODEL_TYPE_NORMAL:
+		n = SMIAPP_DATA_FORMAT_MODEL_TYPE_NORMAL_N;
+		break;
+	case CCS_DATA_FORMAT_MODEL_TYPE_EXTENDED:
+		n = CCS_LIM_DATA_FORMAT_DESCRIPTOR_MAX_N + 1;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	sensor->default_pixel_order = pixel_order;
+	sensor->mbus_frame_fmts = 0;
+
+	for (i = 0; i < n; i++) {
+		unsigned int fmt, j;
+
+		fmt = CCS_LIM_AT(sensor, DATA_FORMAT_DESCRIPTOR, i);
+
+		dev_dbg(&client->dev, "%u: bpp %u, compressed %u\n",
+			i, fmt >> 8, (u8)fmt);
+
+		for (j = 0; j < ARRAY_SIZE(ccs_csi_data_formats); j++) {
+			const struct ccs_csi_data_format *f =
+				&ccs_csi_data_formats[j];
+
+			if (f->pixel_order != CCS_PIXEL_ORDER_GRBG)
+				continue;
+
+			if (f->width != fmt >>
+			    CCS_DATA_FORMAT_DESCRIPTOR_UNCOMPRESSED_SHIFT ||
+			    f->compressed !=
+			    (fmt & CCS_DATA_FORMAT_DESCRIPTOR_COMPRESSED_MASK))
+				continue;
+
+			dev_dbg(&client->dev, "jolly good! %u\n", j);
+
+			sensor->default_mbus_frame_fmts |= 1 << j;
+		}
+	}
+
+	/* Figure out which BPP values can be used with which formats. */
+	pll->binning_horizontal = 1;
+	pll->binning_vertical = 1;
+	pll->scale_m = sensor->scale_m;
+
+	for (i = 0; i < ARRAY_SIZE(ccs_csi_data_formats); i++) {
+		sensor->compressed_min_bpp =
+			min(ccs_csi_data_formats[i].compressed,
+			    sensor->compressed_min_bpp);
+		compressed_max_bpp =
+			max(ccs_csi_data_formats[i].compressed,
+			    compressed_max_bpp);
+	}
+
+	sensor->valid_link_freqs = devm_kcalloc(
+		&client->dev,
+		compressed_max_bpp - sensor->compressed_min_bpp + 1,
+		sizeof(*sensor->valid_link_freqs), GFP_KERNEL);
+	if (!sensor->valid_link_freqs)
+		return -ENOMEM;
+
+	for (i = 0; i < ARRAY_SIZE(ccs_csi_data_formats); i++) {
+		const struct ccs_csi_data_format *f =
+			&ccs_csi_data_formats[i];
+		unsigned long *valid_link_freqs =
+			&sensor->valid_link_freqs[
+				f->compressed - sensor->compressed_min_bpp];
+		unsigned int j;
+
+		if (!(sensor->default_mbus_frame_fmts & 1 << i))
+			continue;
+
+		pll->bits_per_pixel = f->compressed;
+
+		for (j = 0; sensor->hwcfg.op_sys_clock[j]; j++) {
+			pll->link_freq = sensor->hwcfg.op_sys_clock[j];
+
+			rval = ccs_pll_try(sensor, pll);
+			dev_dbg(&client->dev, "link freq %u Hz, bpp %u %s\n",
+				pll->link_freq, pll->bits_per_pixel,
+				rval ? "not ok" : "ok");
+			if (rval)
+				continue;
+
+			set_bit(j, valid_link_freqs);
+		}
+
+		if (!*valid_link_freqs) {
+			dev_info(&client->dev,
+				 "no valid link frequencies for %u bpp\n",
+				 f->compressed);
+			sensor->default_mbus_frame_fmts &= ~BIT(i);
+			continue;
+		}
+
+		if (!sensor->csi_format
+		    || f->width > sensor->csi_format->width
+		    || (f->width == sensor->csi_format->width
+			&& f->compressed > sensor->csi_format->compressed)) {
+			sensor->csi_format = f;
+			sensor->internal_csi_format = f;
+		}
+	}
+
+	if (!sensor->csi_format) {
+		dev_err(&client->dev, "no supported mbus code found\n");
+		return -EINVAL;
+	}
+
+	ccs_update_mbus_formats(sensor);
+
+	return 0;
+}
+
+static void ccs_update_blanking(struct ccs_sensor *sensor)
+{
+	struct v4l2_ctrl *vblank = sensor->vblank;
+	struct v4l2_ctrl *hblank = sensor->hblank;
+	u16 min_fll, max_fll, min_llp, max_llp, min_lbp;
+	int min, max;
+
+	if (sensor->binning_vertical > 1 || sensor->binning_horizontal > 1) {
+		min_fll = CCS_LIM(sensor, MIN_FRAME_LENGTH_LINES_BIN);
+		max_fll = CCS_LIM(sensor, MAX_FRAME_LENGTH_LINES_BIN);
+		min_llp = CCS_LIM(sensor, MIN_LINE_LENGTH_PCK_BIN);
+		max_llp = CCS_LIM(sensor, MAX_LINE_LENGTH_PCK_BIN);
+		min_lbp = CCS_LIM(sensor, MIN_LINE_BLANKING_PCK_BIN);
+	} else {
+		min_fll = CCS_LIM(sensor, MIN_FRAME_LENGTH_LINES);
+		max_fll = CCS_LIM(sensor, MAX_FRAME_LENGTH_LINES);
+		min_llp = CCS_LIM(sensor, MIN_LINE_LENGTH_PCK);
+		max_llp = CCS_LIM(sensor, MAX_LINE_LENGTH_PCK);
+		min_lbp = CCS_LIM(sensor, MIN_LINE_BLANKING_PCK);
+	}
+
+	min = max_t(int,
+		    CCS_LIM(sensor, MIN_FRAME_BLANKING_LINES),
+		    min_fll - sensor->pixel_array->crop[CCS_PA_PAD_SRC].height);
+	max = max_fll -	sensor->pixel_array->crop[CCS_PA_PAD_SRC].height;
+
+	__v4l2_ctrl_modify_range(vblank, min, max, vblank->step, min);
+
+	min = max_t(int,
+		    min_llp - sensor->pixel_array->crop[CCS_PA_PAD_SRC].width,
+		    min_lbp);
+	max = max_llp - sensor->pixel_array->crop[CCS_PA_PAD_SRC].width;
+
+	__v4l2_ctrl_modify_range(hblank, min, max, hblank->step, min);
+
+	__ccs_update_exposure_limits(sensor);
+}
+
+static int ccs_pll_blanking_update(struct ccs_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	int rval;
+
+	rval = ccs_pll_update(sensor);
+	if (rval < 0)
+		return rval;
+
+	/* Output from pixel array, including blanking */
+	ccs_update_blanking(sensor);
+
+	dev_dbg(&client->dev, "vblank\t\t%d\n", sensor->vblank->val);
+	dev_dbg(&client->dev, "hblank\t\t%d\n", sensor->hblank->val);
+
+	dev_dbg(&client->dev, "real timeperframe\t100/%d\n",
+		sensor->pll.pixel_rate_pixel_array /
+		((sensor->pixel_array->crop[CCS_PA_PAD_SRC].width
+		  + sensor->hblank->val) *
+		 (sensor->pixel_array->crop[CCS_PA_PAD_SRC].height
+		  + sensor->vblank->val) / 100));
+
+	return 0;
+}
+
+/*
+ *
+ * SMIA++ NVM handling
+ *
+ */
+
+static int ccs_read_nvm_page(struct ccs_sensor *sensor, u32 p, u8 *nvm,
+			     u8 *status)
+{
+	unsigned int i;
+	int rval;
+	u32 s;
+
+	*status = 0;
+
+	rval = ccs_write(sensor, DATA_TRANSFER_IF_1_PAGE_SELECT, p);
+	if (rval)
+		return rval;
+
+	rval = ccs_write(sensor, DATA_TRANSFER_IF_1_CTRL,
+			 CCS_DATA_TRANSFER_IF_1_CTRL_ENABLE);
+	if (rval)
+		return rval;
+
+	rval = ccs_read(sensor, DATA_TRANSFER_IF_1_STATUS, &s);
+	if (rval)
+		return rval;
+
+	if (s & CCS_DATA_TRANSFER_IF_1_STATUS_IMPROPER_IF_USAGE) {
+		*status = s;
+		return -ENODATA;
+	}
+
+	if (CCS_LIM(sensor, DATA_TRANSFER_IF_CAPABILITY) &
+	    CCS_DATA_TRANSFER_IF_CAPABILITY_POLLING) {
+		for (i = 1000; i > 0; i--) {
+			if (s & CCS_DATA_TRANSFER_IF_1_STATUS_READ_IF_READY)
+				break;
+
+			rval = ccs_read(sensor, DATA_TRANSFER_IF_1_STATUS, &s);
+			if (rval)
+				return rval;
+		}
+
+		if (!i)
+			return -ETIMEDOUT;
+	}
+
+	for (i = 0; i <= CCS_LIM_DATA_TRANSFER_IF_1_DATA_MAX_P; i++) {
+		u32 v;
+
+		rval = ccs_read(sensor, DATA_TRANSFER_IF_1_DATA(i), &v);
+		if (rval)
+			return rval;
+
+		*nvm++ = v;
+	}
+
+	return 0;
+}
+
+static int ccs_read_nvm(struct ccs_sensor *sensor, unsigned char *nvm,
+			size_t nvm_size)
+{
+	u8 status = 0;
+	u32 p;
+	int rval = 0, rval2;
+
+	for (p = 0; p < nvm_size / (CCS_LIM_DATA_TRANSFER_IF_1_DATA_MAX_P + 1)
+		     && !rval; p++) {
+		rval = ccs_read_nvm_page(sensor, p, nvm, &status);
+		nvm += CCS_LIM_DATA_TRANSFER_IF_1_DATA_MAX_P + 1;
+	}
+
+	if (rval == -ENODATA &&
+	    status & CCS_DATA_TRANSFER_IF_1_STATUS_IMPROPER_IF_USAGE)
+		rval = 0;
+
+	rval2 = ccs_write(sensor, DATA_TRANSFER_IF_1_CTRL, 0);
+	if (rval < 0)
+		return rval;
+	else
+		return rval2 ?: p * (CCS_LIM_DATA_TRANSFER_IF_1_DATA_MAX_P + 1);
+}
+
+/*
+ *
+ * SMIA++ CCI address control
+ *
+ */
+static int ccs_change_cci_addr(struct ccs_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	int rval;
+	u32 val;
+
+	client->addr = sensor->hwcfg.i2c_addr_dfl;
+
+	rval = ccs_write(sensor, CCI_ADDRESS_CTRL,
+			 sensor->hwcfg.i2c_addr_alt << 1);
+	if (rval)
+		return rval;
+
+	client->addr = sensor->hwcfg.i2c_addr_alt;
+
+	/* verify addr change went ok */
+	rval = ccs_read(sensor, CCI_ADDRESS_CTRL, &val);
+	if (rval)
+		return rval;
+
+	if (val != sensor->hwcfg.i2c_addr_alt << 1)
+		return -ENODEV;
+
+	return 0;
+}
+
+/*
+ *
+ * SMIA++ Mode Control
+ *
+ */
+static int ccs_setup_flash_strobe(struct ccs_sensor *sensor)
+{
+	struct ccs_flash_strobe_parms *strobe_setup;
+	unsigned int ext_freq = sensor->hwcfg.ext_clk;
+	u32 tmp;
+	u32 strobe_adjustment;
+	u32 strobe_width_high_rs;
+	int rval;
+
+	strobe_setup = sensor->hwcfg.strobe_setup;
+
+	/*
+	 * How to calculate registers related to strobe length. Please
+	 * do not change, or if you do at least know what you're
+	 * doing. :-)
+	 *
+	 * Sakari Ailus <sakari.ailus@linux.intel.com> 2010-10-25
+	 *
+	 * flash_strobe_length [us] / 10^6 = (tFlash_strobe_width_ctrl
+	 *	/ EXTCLK freq [Hz]) * flash_strobe_adjustment
+	 *
+	 * tFlash_strobe_width_ctrl E N, [1 - 0xffff]
+	 * flash_strobe_adjustment E N, [1 - 0xff]
+	 *
+	 * The formula above is written as below to keep it on one
+	 * line:
+	 *
+	 * l / 10^6 = w / e * a
+	 *
+	 * Let's mark w * a by x:
+	 *
+	 * x = w * a
+	 *
+	 * Thus, we get:
+	 *
+	 * x = l * e / 10^6
+	 *
+	 * The strobe width must be at least as long as requested,
+	 * thus rounding upwards is needed.
+	 *
+	 * x = (l * e + 10^6 - 1) / 10^6
+	 * -----------------------------
+	 *
+	 * Maximum possible accuracy is wanted at all times. Thus keep
+	 * a as small as possible.
+	 *
+	 * Calculate a, assuming maximum w, with rounding upwards:
+	 *
+	 * a = (x + (2^16 - 1) - 1) / (2^16 - 1)
+	 * -------------------------------------
+	 *
+	 * Thus, we also get w, with that a, with rounding upwards:
+	 *
+	 * w = (x + a - 1) / a
+	 * -------------------
+	 *
+	 * To get limits:
+	 *
+	 * x E [1, (2^16 - 1) * (2^8 - 1)]
+	 *
+	 * Substituting maximum x to the original formula (with rounding),
+	 * the maximum l is thus
+	 *
+	 * (2^16 - 1) * (2^8 - 1) * 10^6 = l * e + 10^6 - 1
+	 *
+	 * l = (10^6 * (2^16 - 1) * (2^8 - 1) - 10^6 + 1) / e
+	 * --------------------------------------------------
+	 *
+	 * flash_strobe_length must be clamped between 1 and
+	 * (10^6 * (2^16 - 1) * (2^8 - 1) - 10^6 + 1) / EXTCLK freq.
+	 *
+	 * Then,
+	 *
+	 * flash_strobe_adjustment = ((flash_strobe_length *
+	 *	EXTCLK freq + 10^6 - 1) / 10^6 + (2^16 - 1) - 1) / (2^16 - 1)
+	 *
+	 * tFlash_strobe_width_ctrl = ((flash_strobe_length *
+	 *	EXTCLK freq + 10^6 - 1) / 10^6 +
+	 *	flash_strobe_adjustment - 1) / flash_strobe_adjustment
+	 */
+	tmp = div_u64(1000000ULL * ((1 << 16) - 1) * ((1 << 8) - 1) -
+		      1000000 + 1, ext_freq);
+	strobe_setup->strobe_width_high_us =
+		clamp_t(u32, strobe_setup->strobe_width_high_us, 1, tmp);
+
+	tmp = div_u64(((u64)strobe_setup->strobe_width_high_us * (u64)ext_freq +
+			1000000 - 1), 1000000ULL);
+	strobe_adjustment = (tmp + (1 << 16) - 1 - 1) / ((1 << 16) - 1);
+	strobe_width_high_rs = (tmp + strobe_adjustment - 1) /
+				strobe_adjustment;
+
+	rval = ccs_write(sensor, FLASH_MODE_RS, strobe_setup->mode);
+	if (rval < 0)
+		goto out;
+
+	rval = ccs_write(sensor, FLASH_STROBE_ADJUSTMENT, strobe_adjustment);
+	if (rval < 0)
+		goto out;
+
+	rval = ccs_write(sensor, TFLASH_STROBE_WIDTH_HIGH_RS_CTRL,
+			 strobe_width_high_rs);
+	if (rval < 0)
+		goto out;
+
+	rval = ccs_write(sensor, TFLASH_STROBE_DELAY_RS_CTRL,
+			 strobe_setup->strobe_delay);
+	if (rval < 0)
+		goto out;
+
+	rval = ccs_write(sensor, FLASH_STROBE_START_POINT,
+			 strobe_setup->stobe_start_point);
+	if (rval < 0)
+		goto out;
+
+	rval = ccs_write(sensor, FLASH_TRIGGER_RS, strobe_setup->trigger);
+
+out:
+	sensor->hwcfg.strobe_setup->trigger = 0;
+
+	return rval;
+}
+
+/* -----------------------------------------------------------------------------
+ * Power management
+ */
+
+static int ccs_write_msr_regs(struct ccs_sensor *sensor)
+{
+	int rval;
+
+	rval = ccs_write_data_regs(sensor,
+				   sensor->sdata.sensor_manufacturer_regs,
+				   sensor->sdata.num_sensor_manufacturer_regs);
+	if (rval)
+		return rval;
+
+	return ccs_write_data_regs(sensor,
+				   sensor->mdata.module_manufacturer_regs,
+				   sensor->mdata.num_module_manufacturer_regs);
+}
+
+static int ccs_update_phy_ctrl(struct ccs_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	u8 val;
+
+	if (!sensor->ccs_limits)
+		return 0;
+
+	if (CCS_LIM(sensor, PHY_CTRL_CAPABILITY) &
+	    CCS_PHY_CTRL_CAPABILITY_AUTO_PHY_CTL) {
+		val = CCS_PHY_CTRL_AUTO;
+	} else if (CCS_LIM(sensor, PHY_CTRL_CAPABILITY) &
+		   CCS_PHY_CTRL_CAPABILITY_UI_PHY_CTL) {
+		val = CCS_PHY_CTRL_UI;
+	} else {
+		dev_err(&client->dev, "manual PHY control not supported\n");
+		return -EINVAL;
+	}
+
+	return ccs_write(sensor, PHY_CTRL, val);
+}
+
+static int ccs_power_on(struct device *dev)
+{
+	struct v4l2_subdev *subdev = dev_get_drvdata(dev);
+	struct ccs_subdev *ssd = to_ccs_subdev(subdev);
+	/*
+	 * The sub-device related to the I2C device is always the
+	 * source one, i.e. ssds[0].
+	 */
+	struct ccs_sensor *sensor =
+		container_of(ssd, struct ccs_sensor, ssds[0]);
+	const struct ccs_device *ccsdev = device_get_match_data(dev);
+	int rval;
+
+	rval = regulator_bulk_enable(ARRAY_SIZE(ccs_regulators),
+				     sensor->regulators);
+	if (rval) {
+		dev_err(dev, "failed to enable vana regulator\n");
+		return rval;
+	}
+
+	if (sensor->reset || sensor->xshutdown || sensor->ext_clk) {
+		unsigned int sleep;
+
+		rval = clk_prepare_enable(sensor->ext_clk);
+		if (rval < 0) {
+			dev_dbg(dev, "failed to enable xclk\n");
+			goto out_xclk_fail;
+		}
+
+		gpiod_set_value(sensor->reset, 0);
+		gpiod_set_value(sensor->xshutdown, 1);
+
+		if (ccsdev->flags & CCS_DEVICE_FLAG_IS_SMIA)
+			sleep = SMIAPP_RESET_DELAY(sensor->hwcfg.ext_clk);
+		else
+			sleep = 5000;
+
+		usleep_range(sleep, sleep);
+	}
+
+	/*
+	 * Failures to respond to the address change command have been noticed.
+	 * Those failures seem to be caused by the sensor requiring a longer
+	 * boot time than advertised. An additional 10ms delay seems to work
+	 * around the issue, but the SMIA++ I2C write retry hack makes the delay
+	 * unnecessary. The failures need to be investigated to find a proper
+	 * fix, and a delay will likely need to be added here if the I2C write
+	 * retry hack is reverted before the root cause of the boot time issue
+	 * is found.
+	 */
+
+	if (!sensor->reset && !sensor->xshutdown) {
+		u8 retry = 100;
+		u32 reset;
+
+		rval = ccs_write(sensor, SOFTWARE_RESET, CCS_SOFTWARE_RESET_ON);
+		if (rval < 0) {
+			dev_err(dev, "software reset failed\n");
+			goto out_cci_addr_fail;
+		}
+
+		do {
+			rval = ccs_read(sensor, SOFTWARE_RESET, &reset);
+			reset = !rval && reset == CCS_SOFTWARE_RESET_OFF;
+			if (reset)
+				break;
+
+			usleep_range(1000, 2000);
+		} while (--retry);
+
+		if (!reset) {
+			dev_err(dev, "software reset failed\n");
+			rval = -EIO;
+			goto out_cci_addr_fail;
+		}
+	}
+
+	if (sensor->hwcfg.i2c_addr_alt) {
+		rval = ccs_change_cci_addr(sensor);
+		if (rval) {
+			dev_err(dev, "cci address change error\n");
+			goto out_cci_addr_fail;
+		}
+	}
+
+	rval = ccs_write(sensor, COMPRESSION_MODE,
+			 CCS_COMPRESSION_MODE_DPCM_PCM_SIMPLE);
+	if (rval) {
+		dev_err(dev, "compression mode set failed\n");
+		goto out_cci_addr_fail;
+	}
+
+	rval = ccs_write(sensor, EXTCLK_FREQUENCY_MHZ,
+			 sensor->hwcfg.ext_clk / (1000000 / (1 << 8)));
+	if (rval) {
+		dev_err(dev, "extclk frequency set failed\n");
+		goto out_cci_addr_fail;
+	}
+
+	rval = ccs_write(sensor, CSI_LANE_MODE, sensor->hwcfg.lanes - 1);
+	if (rval) {
+		dev_err(dev, "csi lane mode set failed\n");
+		goto out_cci_addr_fail;
+	}
+
+	rval = ccs_write(sensor, FAST_STANDBY_CTRL,
+			 CCS_FAST_STANDBY_CTRL_FRAME_TRUNCATION);
+	if (rval) {
+		dev_err(dev, "fast standby set failed\n");
+		goto out_cci_addr_fail;
+	}
+
+	rval = ccs_write(sensor, CSI_SIGNALING_MODE,
+			 sensor->hwcfg.csi_signalling_mode);
+	if (rval) {
+		dev_err(dev, "csi signalling mode set failed\n");
+		goto out_cci_addr_fail;
+	}
+
+	rval = ccs_update_phy_ctrl(sensor);
+	if (rval < 0)
+		goto out_cci_addr_fail;
+
+	rval = ccs_write_msr_regs(sensor);
+	if (rval)
+		goto out_cci_addr_fail;
+
+	rval = ccs_call_quirk(sensor, post_poweron);
+	if (rval) {
+		dev_err(dev, "post_poweron quirks failed\n");
+		goto out_cci_addr_fail;
+	}
+
+	return 0;
+
+out_cci_addr_fail:
+	gpiod_set_value(sensor->reset, 1);
+	gpiod_set_value(sensor->xshutdown, 0);
+	clk_disable_unprepare(sensor->ext_clk);
+
+out_xclk_fail:
+	regulator_bulk_disable(ARRAY_SIZE(ccs_regulators),
+			       sensor->regulators);
+
+	return rval;
+}
+
+static int ccs_power_off(struct device *dev)
+{
+	struct v4l2_subdev *subdev = dev_get_drvdata(dev);
+	struct ccs_subdev *ssd = to_ccs_subdev(subdev);
+	struct ccs_sensor *sensor =
+		container_of(ssd, struct ccs_sensor, ssds[0]);
+
+	/*
+	 * Currently power/clock to lens are enable/disabled separately
+	 * but they are essentially the same signals. So if the sensor is
+	 * powered off while the lens is powered on the sensor does not
+	 * really see a power off and next time the cci address change
+	 * will fail. So do a soft reset explicitly here.
+	 */
+	if (sensor->hwcfg.i2c_addr_alt)
+		ccs_write(sensor, SOFTWARE_RESET, CCS_SOFTWARE_RESET_ON);
+
+	gpiod_set_value(sensor->reset, 1);
+	gpiod_set_value(sensor->xshutdown, 0);
+	clk_disable_unprepare(sensor->ext_clk);
+	usleep_range(5000, 5000);
+	regulator_bulk_disable(ARRAY_SIZE(ccs_regulators),
+			       sensor->regulators);
+	sensor->streaming = false;
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * Video stream management
+ */
+
+static int ccs_start_streaming(struct ccs_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	unsigned int binning_mode;
+	int rval;
+
+	mutex_lock(&sensor->mutex);
+
+	rval = ccs_write(sensor, CSI_DATA_FORMAT,
+			 (sensor->csi_format->width << 8) |
+			 sensor->csi_format->compressed);
+	if (rval)
+		goto out;
+
+	/* Binning configuration */
+	if (sensor->binning_horizontal == 1 &&
+	    sensor->binning_vertical == 1) {
+		binning_mode = 0;
+	} else {
+		u8 binning_type =
+			(sensor->binning_horizontal << 4)
+			| sensor->binning_vertical;
+
+		rval = ccs_write(sensor, BINNING_TYPE, binning_type);
+		if (rval < 0)
+			goto out;
+
+		binning_mode = 1;
+	}
+	rval = ccs_write(sensor, BINNING_MODE, binning_mode);
+	if (rval < 0)
+		goto out;
+
+	/* Set up PLL */
+	rval = ccs_pll_configure(sensor);
+	if (rval)
+		goto out;
+
+	/* Analog crop start coordinates */
+	rval = ccs_write(sensor, X_ADDR_START,
+			 sensor->pixel_array->crop[CCS_PA_PAD_SRC].left);
+	if (rval < 0)
+		goto out;
+
+	rval = ccs_write(sensor, Y_ADDR_START,
+			 sensor->pixel_array->crop[CCS_PA_PAD_SRC].top);
+	if (rval < 0)
+		goto out;
+
+	/* Analog crop end coordinates */
+	rval = ccs_write(
+		sensor, X_ADDR_END,
+		sensor->pixel_array->crop[CCS_PA_PAD_SRC].left
+		+ sensor->pixel_array->crop[CCS_PA_PAD_SRC].width - 1);
+	if (rval < 0)
+		goto out;
+
+	rval = ccs_write(
+		sensor, Y_ADDR_END,
+		sensor->pixel_array->crop[CCS_PA_PAD_SRC].top
+		+ sensor->pixel_array->crop[CCS_PA_PAD_SRC].height - 1);
+	if (rval < 0)
+		goto out;
+
+	/*
+	 * Output from pixel array, including blanking, is set using
+	 * controls below. No need to set here.
+	 */
+
+	/* Digital crop */
+	if (CCS_LIM(sensor, DIGITAL_CROP_CAPABILITY)
+	    == CCS_DIGITAL_CROP_CAPABILITY_INPUT_CROP) {
+		rval = ccs_write(
+			sensor, DIGITAL_CROP_X_OFFSET,
+			sensor->scaler->crop[CCS_PAD_SINK].left);
+		if (rval < 0)
+			goto out;
+
+		rval = ccs_write(
+			sensor, DIGITAL_CROP_Y_OFFSET,
+			sensor->scaler->crop[CCS_PAD_SINK].top);
+		if (rval < 0)
+			goto out;
+
+		rval = ccs_write(
+			sensor, DIGITAL_CROP_IMAGE_WIDTH,
+			sensor->scaler->crop[CCS_PAD_SINK].width);
+		if (rval < 0)
+			goto out;
+
+		rval = ccs_write(
+			sensor, DIGITAL_CROP_IMAGE_HEIGHT,
+			sensor->scaler->crop[CCS_PAD_SINK].height);
+		if (rval < 0)
+			goto out;
+	}
+
+	/* Scaling */
+	if (CCS_LIM(sensor, SCALING_CAPABILITY)
+	    != CCS_SCALING_CAPABILITY_NONE) {
+		rval = ccs_write(sensor, SCALING_MODE, sensor->scaling_mode);
+		if (rval < 0)
+			goto out;
+
+		rval = ccs_write(sensor, SCALE_M, sensor->scale_m);
+		if (rval < 0)
+			goto out;
+	}
+
+	/* Output size from sensor */
+	rval = ccs_write(sensor, X_OUTPUT_SIZE,
+			 sensor->src->crop[CCS_PAD_SRC].width);
+	if (rval < 0)
+		goto out;
+	rval = ccs_write(sensor, Y_OUTPUT_SIZE,
+			 sensor->src->crop[CCS_PAD_SRC].height);
+	if (rval < 0)
+		goto out;
+
+	if (CCS_LIM(sensor, FLASH_MODE_CAPABILITY) &
+	    (CCS_FLASH_MODE_CAPABILITY_SINGLE_STROBE |
+	     SMIAPP_FLASH_MODE_CAPABILITY_MULTIPLE_STROBE) &&
+	    sensor->hwcfg.strobe_setup != NULL &&
+	    sensor->hwcfg.strobe_setup->trigger != 0) {
+		rval = ccs_setup_flash_strobe(sensor);
+		if (rval)
+			goto out;
+	}
+
+	rval = ccs_call_quirk(sensor, pre_streamon);
+	if (rval) {
+		dev_err(&client->dev, "pre_streamon quirks failed\n");
+		goto out;
+	}
+
+	rval = ccs_write(sensor, MODE_SELECT, CCS_MODE_SELECT_STREAMING);
+
+out:
+	mutex_unlock(&sensor->mutex);
+
+	return rval;
+}
+
+static int ccs_stop_streaming(struct ccs_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	int rval;
+
+	mutex_lock(&sensor->mutex);
+	rval = ccs_write(sensor, MODE_SELECT, CCS_MODE_SELECT_SOFTWARE_STANDBY);
+	if (rval)
+		goto out;
+
+	rval = ccs_call_quirk(sensor, post_streamoff);
+	if (rval)
+		dev_err(&client->dev, "post_streamoff quirks failed\n");
+
+out:
+	mutex_unlock(&sensor->mutex);
+	return rval;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev video operations
+ */
+
+static int ccs_pm_get_init(struct ccs_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	int rval;
+
+	/*
+	 * It can't use pm_runtime_resume_and_get() here, as the driver
+	 * relies at the returned value to detect if the device was already
+	 * active or not.
+	 */
+	rval = pm_runtime_get_sync(&client->dev);
+	if (rval < 0)
+		goto error;
+
+	/* Device was already active, so don't set controls */
+	if (rval == 1)
+		return 0;
+
+	/* Restore V4L2 controls to the previously suspended device */
+	rval = v4l2_ctrl_handler_setup(&sensor->pixel_array->ctrl_handler);
+	if (rval)
+		goto error;
+
+	rval = v4l2_ctrl_handler_setup(&sensor->src->ctrl_handler);
+	if (rval)
+		goto error;
+
+	/* Keep PM runtime usage_count incremented on success */
+	return 0;
+error:
+	pm_runtime_put(&client->dev);
+	return rval;
+}
+
+static int ccs_set_stream(struct v4l2_subdev *subdev, int enable)
+{
+	struct ccs_sensor *sensor = to_ccs_sensor(subdev);
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	int rval;
+
+	if (sensor->streaming == enable)
+		return 0;
+
+	if (!enable) {
+		ccs_stop_streaming(sensor);
+		sensor->streaming = false;
+		pm_runtime_mark_last_busy(&client->dev);
+		pm_runtime_put_autosuspend(&client->dev);
+
+		return 0;
+	}
+
+	rval = ccs_pm_get_init(sensor);
+	if (rval)
+		return rval;
+
+	sensor->streaming = true;
+
+	rval = ccs_start_streaming(sensor);
+	if (rval < 0) {
+		sensor->streaming = false;
+		pm_runtime_mark_last_busy(&client->dev);
+		pm_runtime_put_autosuspend(&client->dev);
+	}
+
+	return rval;
+}
+
+static int ccs_pre_streamon(struct v4l2_subdev *subdev, u32 flags)
+{
+	struct ccs_sensor *sensor = to_ccs_sensor(subdev);
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	int rval;
+
+	if (flags & V4L2_SUBDEV_PRE_STREAMON_FL_MANUAL_LP) {
+		switch (sensor->hwcfg.csi_signalling_mode) {
+		case CCS_CSI_SIGNALING_MODE_CSI_2_DPHY:
+			if (!(CCS_LIM(sensor, PHY_CTRL_CAPABILITY_2) &
+			      CCS_PHY_CTRL_CAPABILITY_2_MANUAL_LP_DPHY))
+				return -EACCES;
+			break;
+		case CCS_CSI_SIGNALING_MODE_CSI_2_CPHY:
+			if (!(CCS_LIM(sensor, PHY_CTRL_CAPABILITY_2) &
+			      CCS_PHY_CTRL_CAPABILITY_2_MANUAL_LP_CPHY))
+				return -EACCES;
+			break;
+		default:
+			return -EACCES;
+		}
+	}
+
+	rval = ccs_pm_get_init(sensor);
+	if (rval)
+		return rval;
+
+	if (flags & V4L2_SUBDEV_PRE_STREAMON_FL_MANUAL_LP) {
+		rval = ccs_write(sensor, MANUAL_LP_CTRL,
+				 CCS_MANUAL_LP_CTRL_ENABLE);
+		if (rval)
+			pm_runtime_put(&client->dev);
+	}
+
+	return rval;
+}
+
+static int ccs_post_streamoff(struct v4l2_subdev *subdev)
+{
+	struct ccs_sensor *sensor = to_ccs_sensor(subdev);
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+
+	return pm_runtime_put(&client->dev);
+}
+
+static int ccs_enum_mbus_code(struct v4l2_subdev *subdev,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
+	struct ccs_sensor *sensor = to_ccs_sensor(subdev);
+	unsigned int i;
+	int idx = -1;
+	int rval = -EINVAL;
+
+	mutex_lock(&sensor->mutex);
+
+	dev_err(&client->dev, "subdev %s, pad %u, index %u\n",
+		subdev->name, code->pad, code->index);
+
+	if (subdev != &sensor->src->sd || code->pad != CCS_PAD_SRC) {
+		if (code->index)
+			goto out;
+
+		code->code = sensor->internal_csi_format->code;
+		rval = 0;
+		goto out;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(ccs_csi_data_formats); i++) {
+		if (sensor->mbus_frame_fmts & (1 << i))
+			idx++;
+
+		if (idx == code->index) {
+			code->code = ccs_csi_data_formats[i].code;
+			dev_err(&client->dev, "found index %u, i %u, code %x\n",
+				code->index, i, code->code);
+			rval = 0;
+			break;
+		}
+	}
+
+out:
+	mutex_unlock(&sensor->mutex);
+
+	return rval;
+}
+
+static u32 __ccs_get_mbus_code(struct v4l2_subdev *subdev, unsigned int pad)
+{
+	struct ccs_sensor *sensor = to_ccs_sensor(subdev);
+
+	if (subdev == &sensor->src->sd && pad == CCS_PAD_SRC)
+		return sensor->csi_format->code;
+	else
+		return sensor->internal_csi_format->code;
+}
+
+static int __ccs_get_format(struct v4l2_subdev *subdev,
+			    struct v4l2_subdev_state *sd_state,
+			    struct v4l2_subdev_format *fmt)
+{
+	struct ccs_subdev *ssd = to_ccs_subdev(subdev);
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		fmt->format = *v4l2_subdev_get_try_format(subdev, sd_state,
+							  fmt->pad);
+	} else {
+		struct v4l2_rect *r;
+
+		if (fmt->pad == ssd->source_pad)
+			r = &ssd->crop[ssd->source_pad];
+		else
+			r = &ssd->sink_fmt;
+
+		fmt->format.code = __ccs_get_mbus_code(subdev, fmt->pad);
+		fmt->format.width = r->width;
+		fmt->format.height = r->height;
+		fmt->format.field = V4L2_FIELD_NONE;
+	}
+
+	return 0;
+}
+
+static int ccs_get_format(struct v4l2_subdev *subdev,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct ccs_sensor *sensor = to_ccs_sensor(subdev);
+	int rval;
+
+	mutex_lock(&sensor->mutex);
+	rval = __ccs_get_format(subdev, sd_state, fmt);
+	mutex_unlock(&sensor->mutex);
+
+	return rval;
+}
+
+static void ccs_get_crop_compose(struct v4l2_subdev *subdev,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_rect **crops,
+				 struct v4l2_rect **comps, int which)
+{
+	struct ccs_subdev *ssd = to_ccs_subdev(subdev);
+	unsigned int i;
+
+	if (which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		if (crops)
+			for (i = 0; i < subdev->entity.num_pads; i++)
+				crops[i] = &ssd->crop[i];
+		if (comps)
+			*comps = &ssd->compose;
+	} else {
+		if (crops) {
+			for (i = 0; i < subdev->entity.num_pads; i++)
+				crops[i] = v4l2_subdev_get_try_crop(subdev,
+								    sd_state,
+								    i);
+		}
+		if (comps)
+			*comps = v4l2_subdev_get_try_compose(subdev, sd_state,
+							     CCS_PAD_SINK);
+	}
+}
+
+/* Changes require propagation only on sink pad. */
+static void ccs_propagate(struct v4l2_subdev *subdev,
+			  struct v4l2_subdev_state *sd_state, int which,
+			  int target)
+{
+	struct ccs_sensor *sensor = to_ccs_sensor(subdev);
+	struct ccs_subdev *ssd = to_ccs_subdev(subdev);
+	struct v4l2_rect *comp, *crops[CCS_PADS];
+
+	ccs_get_crop_compose(subdev, sd_state, crops, &comp, which);
+
+	switch (target) {
+	case V4L2_SEL_TGT_CROP:
+		comp->width = crops[CCS_PAD_SINK]->width;
+		comp->height = crops[CCS_PAD_SINK]->height;
+		if (which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+			if (ssd == sensor->scaler) {
+				sensor->scale_m = CCS_LIM(sensor, SCALER_N_MIN);
+				sensor->scaling_mode =
+					CCS_SCALING_MODE_NO_SCALING;
+			} else if (ssd == sensor->binner) {
+				sensor->binning_horizontal = 1;
+				sensor->binning_vertical = 1;
+			}
+		}
+		fallthrough;
+	case V4L2_SEL_TGT_COMPOSE:
+		*crops[CCS_PAD_SRC] = *comp;
+		break;
+	default:
+		WARN_ON_ONCE(1);
+	}
+}
+
+static const struct ccs_csi_data_format
+*ccs_validate_csi_data_format(struct ccs_sensor *sensor, u32 code)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(ccs_csi_data_formats); i++) {
+		if (sensor->mbus_frame_fmts & (1 << i) &&
+		    ccs_csi_data_formats[i].code == code)
+			return &ccs_csi_data_formats[i];
+	}
+
+	return sensor->csi_format;
+}
+
+static int ccs_set_format_source(struct v4l2_subdev *subdev,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_format *fmt)
+{
+	struct ccs_sensor *sensor = to_ccs_sensor(subdev);
+	const struct ccs_csi_data_format *csi_format,
+		*old_csi_format = sensor->csi_format;
+	unsigned long *valid_link_freqs;
+	u32 code = fmt->format.code;
+	unsigned int i;
+	int rval;
+
+	rval = __ccs_get_format(subdev, sd_state, fmt);
+	if (rval)
+		return rval;
+
+	/*
+	 * Media bus code is changeable on src subdev's source pad. On
+	 * other source pads we just get format here.
+	 */
+	if (subdev != &sensor->src->sd)
+		return 0;
+
+	csi_format = ccs_validate_csi_data_format(sensor, code);
+
+	fmt->format.code = csi_format->code;
+
+	if (fmt->which != V4L2_SUBDEV_FORMAT_ACTIVE)
+		return 0;
+
+	sensor->csi_format = csi_format;
+
+	if (csi_format->width != old_csi_format->width)
+		for (i = 0; i < ARRAY_SIZE(sensor->test_data); i++)
+			__v4l2_ctrl_modify_range(
+				sensor->test_data[i], 0,
+				(1 << csi_format->width) - 1, 1, 0);
+
+	if (csi_format->compressed == old_csi_format->compressed)
+		return 0;
+
+	valid_link_freqs =
+		&sensor->valid_link_freqs[sensor->csi_format->compressed
+					  - sensor->compressed_min_bpp];
+
+	__v4l2_ctrl_modify_range(
+		sensor->link_freq, 0,
+		__fls(*valid_link_freqs), ~*valid_link_freqs,
+		__ffs(*valid_link_freqs));
+
+	return ccs_pll_update(sensor);
+}
+
+static int ccs_set_format(struct v4l2_subdev *subdev,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct ccs_sensor *sensor = to_ccs_sensor(subdev);
+	struct ccs_subdev *ssd = to_ccs_subdev(subdev);
+	struct v4l2_rect *crops[CCS_PADS];
+
+	mutex_lock(&sensor->mutex);
+
+	if (fmt->pad == ssd->source_pad) {
+		int rval;
+
+		rval = ccs_set_format_source(subdev, sd_state, fmt);
+
+		mutex_unlock(&sensor->mutex);
+
+		return rval;
+	}
+
+	/* Sink pad. Width and height are changeable here. */
+	fmt->format.code = __ccs_get_mbus_code(subdev, fmt->pad);
+	fmt->format.width &= ~1;
+	fmt->format.height &= ~1;
+	fmt->format.field = V4L2_FIELD_NONE;
+
+	fmt->format.width =
+		clamp(fmt->format.width,
+		      CCS_LIM(sensor, MIN_X_OUTPUT_SIZE),
+		      CCS_LIM(sensor, MAX_X_OUTPUT_SIZE));
+	fmt->format.height =
+		clamp(fmt->format.height,
+		      CCS_LIM(sensor, MIN_Y_OUTPUT_SIZE),
+		      CCS_LIM(sensor, MAX_Y_OUTPUT_SIZE));
+
+	ccs_get_crop_compose(subdev, sd_state, crops, NULL, fmt->which);
+
+	crops[ssd->sink_pad]->left = 0;
+	crops[ssd->sink_pad]->top = 0;
+	crops[ssd->sink_pad]->width = fmt->format.width;
+	crops[ssd->sink_pad]->height = fmt->format.height;
+	if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		ssd->sink_fmt = *crops[ssd->sink_pad];
+	ccs_propagate(subdev, sd_state, fmt->which, V4L2_SEL_TGT_CROP);
+
+	mutex_unlock(&sensor->mutex);
+
+	return 0;
+}
+
+/*
+ * Calculate goodness of scaled image size compared to expected image
+ * size and flags provided.
+ */
+#define SCALING_GOODNESS		100000
+#define SCALING_GOODNESS_EXTREME	100000000
+static int scaling_goodness(struct v4l2_subdev *subdev, int w, int ask_w,
+			    int h, int ask_h, u32 flags)
+{
+	struct ccs_sensor *sensor = to_ccs_sensor(subdev);
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
+	int val = 0;
+
+	w &= ~1;
+	ask_w &= ~1;
+	h &= ~1;
+	ask_h &= ~1;
+
+	if (flags & V4L2_SEL_FLAG_GE) {
+		if (w < ask_w)
+			val -= SCALING_GOODNESS;
+		if (h < ask_h)
+			val -= SCALING_GOODNESS;
+	}
+
+	if (flags & V4L2_SEL_FLAG_LE) {
+		if (w > ask_w)
+			val -= SCALING_GOODNESS;
+		if (h > ask_h)
+			val -= SCALING_GOODNESS;
+	}
+
+	val -= abs(w - ask_w);
+	val -= abs(h - ask_h);
+
+	if (w < CCS_LIM(sensor, MIN_X_OUTPUT_SIZE))
+		val -= SCALING_GOODNESS_EXTREME;
+
+	dev_dbg(&client->dev, "w %d ask_w %d h %d ask_h %d goodness %d\n",
+		w, ask_w, h, ask_h, val);
+
+	return val;
+}
+
+static void ccs_set_compose_binner(struct v4l2_subdev *subdev,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_selection *sel,
+				   struct v4l2_rect **crops,
+				   struct v4l2_rect *comp)
+{
+	struct ccs_sensor *sensor = to_ccs_sensor(subdev);
+	unsigned int i;
+	unsigned int binh = 1, binv = 1;
+	int best = scaling_goodness(
+		subdev,
+		crops[CCS_PAD_SINK]->width, sel->r.width,
+		crops[CCS_PAD_SINK]->height, sel->r.height, sel->flags);
+
+	for (i = 0; i < sensor->nbinning_subtypes; i++) {
+		int this = scaling_goodness(
+			subdev,
+			crops[CCS_PAD_SINK]->width
+			/ sensor->binning_subtypes[i].horizontal,
+			sel->r.width,
+			crops[CCS_PAD_SINK]->height
+			/ sensor->binning_subtypes[i].vertical,
+			sel->r.height, sel->flags);
+
+		if (this > best) {
+			binh = sensor->binning_subtypes[i].horizontal;
+			binv = sensor->binning_subtypes[i].vertical;
+			best = this;
+		}
+	}
+	if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		sensor->binning_vertical = binv;
+		sensor->binning_horizontal = binh;
+	}
+
+	sel->r.width = (crops[CCS_PAD_SINK]->width / binh) & ~1;
+	sel->r.height = (crops[CCS_PAD_SINK]->height / binv) & ~1;
+}
+
+/*
+ * Calculate best scaling ratio and mode for given output resolution.
+ *
+ * Try all of these: horizontal ratio, vertical ratio and smallest
+ * size possible (horizontally).
+ *
+ * Also try whether horizontal scaler or full scaler gives a better
+ * result.
+ */
+static void ccs_set_compose_scaler(struct v4l2_subdev *subdev,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_selection *sel,
+				   struct v4l2_rect **crops,
+				   struct v4l2_rect *comp)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
+	struct ccs_sensor *sensor = to_ccs_sensor(subdev);
+	u32 min, max, a, b, max_m;
+	u32 scale_m = CCS_LIM(sensor, SCALER_N_MIN);
+	int mode = CCS_SCALING_MODE_HORIZONTAL;
+	u32 try[4];
+	u32 ntry = 0;
+	unsigned int i;
+	int best = INT_MIN;
+
+	sel->r.width = min_t(unsigned int, sel->r.width,
+			     crops[CCS_PAD_SINK]->width);
+	sel->r.height = min_t(unsigned int, sel->r.height,
+			      crops[CCS_PAD_SINK]->height);
+
+	a = crops[CCS_PAD_SINK]->width
+		* CCS_LIM(sensor, SCALER_N_MIN) / sel->r.width;
+	b = crops[CCS_PAD_SINK]->height
+		* CCS_LIM(sensor, SCALER_N_MIN) / sel->r.height;
+	max_m = crops[CCS_PAD_SINK]->width
+		* CCS_LIM(sensor, SCALER_N_MIN)
+		/ CCS_LIM(sensor, MIN_X_OUTPUT_SIZE);
+
+	a = clamp(a, CCS_LIM(sensor, SCALER_M_MIN),
+		  CCS_LIM(sensor, SCALER_M_MAX));
+	b = clamp(b, CCS_LIM(sensor, SCALER_M_MIN),
+		  CCS_LIM(sensor, SCALER_M_MAX));
+	max_m = clamp(max_m, CCS_LIM(sensor, SCALER_M_MIN),
+		      CCS_LIM(sensor, SCALER_M_MAX));
+
+	dev_dbg(&client->dev, "scaling: a %u b %u max_m %u\n", a, b, max_m);
+
+	min = min(max_m, min(a, b));
+	max = min(max_m, max(a, b));
+
+	try[ntry] = min;
+	ntry++;
+	if (min != max) {
+		try[ntry] = max;
+		ntry++;
+	}
+	if (max != max_m) {
+		try[ntry] = min + 1;
+		ntry++;
+		if (min != max) {
+			try[ntry] = max + 1;
+			ntry++;
+		}
+	}
+
+	for (i = 0; i < ntry; i++) {
+		int this = scaling_goodness(
+			subdev,
+			crops[CCS_PAD_SINK]->width
+			/ try[i] * CCS_LIM(sensor, SCALER_N_MIN),
+			sel->r.width,
+			crops[CCS_PAD_SINK]->height,
+			sel->r.height,
+			sel->flags);
+
+		dev_dbg(&client->dev, "trying factor %u (%u)\n", try[i], i);
+
+		if (this > best) {
+			scale_m = try[i];
+			mode = CCS_SCALING_MODE_HORIZONTAL;
+			best = this;
+		}
+
+		if (CCS_LIM(sensor, SCALING_CAPABILITY)
+		    == CCS_SCALING_CAPABILITY_HORIZONTAL)
+			continue;
+
+		this = scaling_goodness(
+			subdev, crops[CCS_PAD_SINK]->width
+			/ try[i]
+			* CCS_LIM(sensor, SCALER_N_MIN),
+			sel->r.width,
+			crops[CCS_PAD_SINK]->height
+			/ try[i]
+			* CCS_LIM(sensor, SCALER_N_MIN),
+			sel->r.height,
+			sel->flags);
+
+		if (this > best) {
+			scale_m = try[i];
+			mode = SMIAPP_SCALING_MODE_BOTH;
+			best = this;
+		}
+	}
+
+	sel->r.width =
+		(crops[CCS_PAD_SINK]->width
+		 / scale_m
+		 * CCS_LIM(sensor, SCALER_N_MIN)) & ~1;
+	if (mode == SMIAPP_SCALING_MODE_BOTH)
+		sel->r.height =
+			(crops[CCS_PAD_SINK]->height
+			 / scale_m
+			 * CCS_LIM(sensor, SCALER_N_MIN))
+			& ~1;
+	else
+		sel->r.height = crops[CCS_PAD_SINK]->height;
+
+	if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		sensor->scale_m = scale_m;
+		sensor->scaling_mode = mode;
+	}
+}
+/* We're only called on source pads. This function sets scaling. */
+static int ccs_set_compose(struct v4l2_subdev *subdev,
+			   struct v4l2_subdev_state *sd_state,
+			   struct v4l2_subdev_selection *sel)
+{
+	struct ccs_sensor *sensor = to_ccs_sensor(subdev);
+	struct ccs_subdev *ssd = to_ccs_subdev(subdev);
+	struct v4l2_rect *comp, *crops[CCS_PADS];
+
+	ccs_get_crop_compose(subdev, sd_state, crops, &comp, sel->which);
+
+	sel->r.top = 0;
+	sel->r.left = 0;
+
+	if (ssd == sensor->binner)
+		ccs_set_compose_binner(subdev, sd_state, sel, crops, comp);
+	else
+		ccs_set_compose_scaler(subdev, sd_state, sel, crops, comp);
+
+	*comp = sel->r;
+	ccs_propagate(subdev, sd_state, sel->which, V4L2_SEL_TGT_COMPOSE);
+
+	if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		return ccs_pll_blanking_update(sensor);
+
+	return 0;
+}
+
+static int __ccs_sel_supported(struct v4l2_subdev *subdev,
+			       struct v4l2_subdev_selection *sel)
+{
+	struct ccs_sensor *sensor = to_ccs_sensor(subdev);
+	struct ccs_subdev *ssd = to_ccs_subdev(subdev);
+
+	/* We only implement crop in three places. */
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		if (ssd == sensor->pixel_array && sel->pad == CCS_PA_PAD_SRC)
+			return 0;
+		if (ssd == sensor->src && sel->pad == CCS_PAD_SRC)
+			return 0;
+		if (ssd == sensor->scaler && sel->pad == CCS_PAD_SINK &&
+		    CCS_LIM(sensor, DIGITAL_CROP_CAPABILITY)
+		    == CCS_DIGITAL_CROP_CAPABILITY_INPUT_CROP)
+			return 0;
+		return -EINVAL;
+	case V4L2_SEL_TGT_NATIVE_SIZE:
+		if (ssd == sensor->pixel_array && sel->pad == CCS_PA_PAD_SRC)
+			return 0;
+		return -EINVAL;
+	case V4L2_SEL_TGT_COMPOSE:
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+		if (sel->pad == ssd->source_pad)
+			return -EINVAL;
+		if (ssd == sensor->binner)
+			return 0;
+		if (ssd == sensor->scaler && CCS_LIM(sensor, SCALING_CAPABILITY)
+		    != CCS_SCALING_CAPABILITY_NONE)
+			return 0;
+		fallthrough;
+	default:
+		return -EINVAL;
+	}
+}
+
+static int ccs_set_crop(struct v4l2_subdev *subdev,
+			struct v4l2_subdev_state *sd_state,
+			struct v4l2_subdev_selection *sel)
+{
+	struct ccs_sensor *sensor = to_ccs_sensor(subdev);
+	struct ccs_subdev *ssd = to_ccs_subdev(subdev);
+	struct v4l2_rect *src_size, *crops[CCS_PADS];
+	struct v4l2_rect _r;
+
+	ccs_get_crop_compose(subdev, sd_state, crops, NULL, sel->which);
+
+	if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		if (sel->pad == ssd->sink_pad)
+			src_size = &ssd->sink_fmt;
+		else
+			src_size = &ssd->compose;
+	} else {
+		if (sel->pad == ssd->sink_pad) {
+			_r.left = 0;
+			_r.top = 0;
+			_r.width = v4l2_subdev_get_try_format(subdev,
+							      sd_state,
+							      sel->pad)
+				->width;
+			_r.height = v4l2_subdev_get_try_format(subdev,
+							       sd_state,
+							       sel->pad)
+				->height;
+			src_size = &_r;
+		} else {
+			src_size = v4l2_subdev_get_try_compose(
+				subdev, sd_state, ssd->sink_pad);
+		}
+	}
+
+	if (ssd == sensor->src && sel->pad == CCS_PAD_SRC) {
+		sel->r.left = 0;
+		sel->r.top = 0;
+	}
+
+	sel->r.width = min(sel->r.width, src_size->width);
+	sel->r.height = min(sel->r.height, src_size->height);
+
+	sel->r.left = min_t(int, sel->r.left, src_size->width - sel->r.width);
+	sel->r.top = min_t(int, sel->r.top, src_size->height - sel->r.height);
+
+	*crops[sel->pad] = sel->r;
+
+	if (ssd != sensor->pixel_array && sel->pad == CCS_PAD_SINK)
+		ccs_propagate(subdev, sd_state, sel->which, V4L2_SEL_TGT_CROP);
+
+	return 0;
+}
+
+static void ccs_get_native_size(struct ccs_subdev *ssd, struct v4l2_rect *r)
+{
+	r->top = 0;
+	r->left = 0;
+	r->width = CCS_LIM(ssd->sensor, X_ADDR_MAX) + 1;
+	r->height = CCS_LIM(ssd->sensor, Y_ADDR_MAX) + 1;
+}
+
+static int __ccs_get_selection(struct v4l2_subdev *subdev,
+			       struct v4l2_subdev_state *sd_state,
+			       struct v4l2_subdev_selection *sel)
+{
+	struct ccs_sensor *sensor = to_ccs_sensor(subdev);
+	struct ccs_subdev *ssd = to_ccs_subdev(subdev);
+	struct v4l2_rect *comp, *crops[CCS_PADS];
+	struct v4l2_rect sink_fmt;
+	int ret;
+
+	ret = __ccs_sel_supported(subdev, sel);
+	if (ret)
+		return ret;
+
+	ccs_get_crop_compose(subdev, sd_state, crops, &comp, sel->which);
+
+	if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		sink_fmt = ssd->sink_fmt;
+	} else {
+		struct v4l2_mbus_framefmt *fmt =
+			v4l2_subdev_get_try_format(subdev, sd_state,
+						   ssd->sink_pad);
+
+		sink_fmt.left = 0;
+		sink_fmt.top = 0;
+		sink_fmt.width = fmt->width;
+		sink_fmt.height = fmt->height;
+	}
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_NATIVE_SIZE:
+		if (ssd == sensor->pixel_array)
+			ccs_get_native_size(ssd, &sel->r);
+		else if (sel->pad == ssd->sink_pad)
+			sel->r = sink_fmt;
+		else
+			sel->r = *comp;
+		break;
+	case V4L2_SEL_TGT_CROP:
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+		sel->r = *crops[sel->pad];
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+		sel->r = *comp;
+		break;
+	}
+
+	return 0;
+}
+
+static int ccs_get_selection(struct v4l2_subdev *subdev,
+			     struct v4l2_subdev_state *sd_state,
+			     struct v4l2_subdev_selection *sel)
+{
+	struct ccs_sensor *sensor = to_ccs_sensor(subdev);
+	int rval;
+
+	mutex_lock(&sensor->mutex);
+	rval = __ccs_get_selection(subdev, sd_state, sel);
+	mutex_unlock(&sensor->mutex);
+
+	return rval;
+}
+
+static int ccs_set_selection(struct v4l2_subdev *subdev,
+			     struct v4l2_subdev_state *sd_state,
+			     struct v4l2_subdev_selection *sel)
+{
+	struct ccs_sensor *sensor = to_ccs_sensor(subdev);
+	int ret;
+
+	ret = __ccs_sel_supported(subdev, sel);
+	if (ret)
+		return ret;
+
+	mutex_lock(&sensor->mutex);
+
+	sel->r.left = max(0, sel->r.left & ~1);
+	sel->r.top = max(0, sel->r.top & ~1);
+	sel->r.width = CCS_ALIGN_DIM(sel->r.width, sel->flags);
+	sel->r.height =	CCS_ALIGN_DIM(sel->r.height, sel->flags);
+
+	sel->r.width = max_t(unsigned int, CCS_LIM(sensor, MIN_X_OUTPUT_SIZE),
+			     sel->r.width);
+	sel->r.height = max_t(unsigned int, CCS_LIM(sensor, MIN_Y_OUTPUT_SIZE),
+			      sel->r.height);
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		ret = ccs_set_crop(subdev, sd_state, sel);
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+		ret = ccs_set_compose(subdev, sd_state, sel);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	mutex_unlock(&sensor->mutex);
+	return ret;
+}
+
+static int ccs_get_skip_frames(struct v4l2_subdev *subdev, u32 *frames)
+{
+	struct ccs_sensor *sensor = to_ccs_sensor(subdev);
+
+	*frames = sensor->frame_skip;
+	return 0;
+}
+
+static int ccs_get_skip_top_lines(struct v4l2_subdev *subdev, u32 *lines)
+{
+	struct ccs_sensor *sensor = to_ccs_sensor(subdev);
+
+	*lines = sensor->image_start;
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * sysfs attributes
+ */
+
+static ssize_t
+nvm_show(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct v4l2_subdev *subdev = i2c_get_clientdata(to_i2c_client(dev));
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
+	struct ccs_sensor *sensor = to_ccs_sensor(subdev);
+	int rval;
+
+	if (!sensor->dev_init_done)
+		return -EBUSY;
+
+	rval = ccs_pm_get_init(sensor);
+	if (rval < 0)
+		return -ENODEV;
+
+	rval = ccs_read_nvm(sensor, buf, PAGE_SIZE);
+	if (rval < 0) {
+		pm_runtime_put(&client->dev);
+		dev_err(&client->dev, "nvm read failed\n");
+		return -ENODEV;
+	}
+
+	pm_runtime_mark_last_busy(&client->dev);
+	pm_runtime_put_autosuspend(&client->dev);
+
+	/*
+	 * NVM is still way below a PAGE_SIZE, so we can safely
+	 * assume this for now.
+	 */
+	return rval;
+}
+static DEVICE_ATTR_RO(nvm);
+
+static ssize_t
+ident_show(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct v4l2_subdev *subdev = i2c_get_clientdata(to_i2c_client(dev));
+	struct ccs_sensor *sensor = to_ccs_sensor(subdev);
+	struct ccs_module_info *minfo = &sensor->minfo;
+
+	if (minfo->mipi_manufacturer_id)
+		return sysfs_emit(buf, "%4.4x%4.4x%2.2x\n",
+				    minfo->mipi_manufacturer_id, minfo->model_id,
+				    minfo->revision_number) + 1;
+	else
+		return sysfs_emit(buf, "%2.2x%4.4x%2.2x\n",
+				    minfo->smia_manufacturer_id, minfo->model_id,
+				    minfo->revision_number) + 1;
+}
+static DEVICE_ATTR_RO(ident);
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev core operations
+ */
+
+static int ccs_identify_module(struct ccs_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	struct ccs_module_info *minfo = &sensor->minfo;
+	unsigned int i;
+	u32 rev;
+	int rval = 0;
+
+	/* Module info */
+	rval = ccs_read(sensor, MODULE_MANUFACTURER_ID,
+			&minfo->mipi_manufacturer_id);
+	if (!rval && !minfo->mipi_manufacturer_id)
+		rval = ccs_read_addr_8only(sensor,
+					   SMIAPP_REG_U8_MANUFACTURER_ID,
+					   &minfo->smia_manufacturer_id);
+	if (!rval)
+		rval = ccs_read_addr_8only(sensor, CCS_R_MODULE_MODEL_ID,
+					   &minfo->model_id);
+	if (!rval)
+		rval = ccs_read_addr_8only(sensor,
+					   CCS_R_MODULE_REVISION_NUMBER_MAJOR,
+					   &rev);
+	if (!rval) {
+		rval = ccs_read_addr_8only(sensor,
+					   CCS_R_MODULE_REVISION_NUMBER_MINOR,
+					   &minfo->revision_number);
+		minfo->revision_number |= rev << 8;
+	}
+	if (!rval)
+		rval = ccs_read_addr_8only(sensor, CCS_R_MODULE_DATE_YEAR,
+					   &minfo->module_year);
+	if (!rval)
+		rval = ccs_read_addr_8only(sensor, CCS_R_MODULE_DATE_MONTH,
+					   &minfo->module_month);
+	if (!rval)
+		rval = ccs_read_addr_8only(sensor, CCS_R_MODULE_DATE_DAY,
+					   &minfo->module_day);
+
+	/* Sensor info */
+	if (!rval)
+		rval = ccs_read(sensor, SENSOR_MANUFACTURER_ID,
+				&minfo->sensor_mipi_manufacturer_id);
+	if (!rval && !minfo->sensor_mipi_manufacturer_id)
+		rval = ccs_read_addr_8only(sensor,
+					   CCS_R_SENSOR_MANUFACTURER_ID,
+					   &minfo->sensor_smia_manufacturer_id);
+	if (!rval)
+		rval = ccs_read_addr_8only(sensor,
+					   CCS_R_SENSOR_MODEL_ID,
+					   &minfo->sensor_model_id);
+	if (!rval)
+		rval = ccs_read_addr_8only(sensor,
+					   CCS_R_SENSOR_REVISION_NUMBER,
+					   &minfo->sensor_revision_number);
+	if (!rval && !minfo->sensor_revision_number)
+		rval = ccs_read_addr_8only(sensor,
+					   CCS_R_SENSOR_REVISION_NUMBER_16,
+					   &minfo->sensor_revision_number);
+	if (!rval)
+		rval = ccs_read_addr_8only(sensor,
+					   CCS_R_SENSOR_FIRMWARE_VERSION,
+					   &minfo->sensor_firmware_version);
+
+	/* SMIA */
+	if (!rval)
+		rval = ccs_read(sensor, MIPI_CCS_VERSION, &minfo->ccs_version);
+	if (!rval && !minfo->ccs_version)
+		rval = ccs_read_addr_8only(sensor, SMIAPP_REG_U8_SMIA_VERSION,
+					   &minfo->smia_version);
+	if (!rval && !minfo->ccs_version)
+		rval = ccs_read_addr_8only(sensor, SMIAPP_REG_U8_SMIAPP_VERSION,
+					   &minfo->smiapp_version);
+
+	if (rval) {
+		dev_err(&client->dev, "sensor detection failed\n");
+		return -ENODEV;
+	}
+
+	if (minfo->mipi_manufacturer_id)
+		dev_dbg(&client->dev, "MIPI CCS module 0x%4.4x-0x%4.4x\n",
+			minfo->mipi_manufacturer_id, minfo->model_id);
+	else
+		dev_dbg(&client->dev, "SMIA module 0x%2.2x-0x%4.4x\n",
+			minfo->smia_manufacturer_id, minfo->model_id);
+
+	dev_dbg(&client->dev,
+		"module revision 0x%4.4x date %2.2d-%2.2d-%2.2d\n",
+		minfo->revision_number, minfo->module_year, minfo->module_month,
+		minfo->module_day);
+
+	if (minfo->sensor_mipi_manufacturer_id)
+		dev_dbg(&client->dev, "MIPI CCS sensor 0x%4.4x-0x%4.4x\n",
+			minfo->sensor_mipi_manufacturer_id,
+			minfo->sensor_model_id);
+	else
+		dev_dbg(&client->dev, "SMIA sensor 0x%2.2x-0x%4.4x\n",
+			minfo->sensor_smia_manufacturer_id,
+			minfo->sensor_model_id);
+
+	dev_dbg(&client->dev,
+		"sensor revision 0x%4.4x firmware version 0x%2.2x\n",
+		minfo->sensor_revision_number, minfo->sensor_firmware_version);
+
+	if (minfo->ccs_version) {
+		dev_dbg(&client->dev, "MIPI CCS version %u.%u",
+			(minfo->ccs_version & CCS_MIPI_CCS_VERSION_MAJOR_MASK)
+			>> CCS_MIPI_CCS_VERSION_MAJOR_SHIFT,
+			(minfo->ccs_version & CCS_MIPI_CCS_VERSION_MINOR_MASK));
+		minfo->name = CCS_NAME;
+	} else {
+		dev_dbg(&client->dev,
+			"smia version %2.2d smiapp version %2.2d\n",
+			minfo->smia_version, minfo->smiapp_version);
+		minfo->name = SMIAPP_NAME;
+		/*
+		 * Some modules have bad data in the lvalues below. Hope the
+		 * rvalues have better stuff. The lvalues are module
+		 * parameters whereas the rvalues are sensor parameters.
+		 */
+		if (minfo->sensor_smia_manufacturer_id &&
+		    !minfo->smia_manufacturer_id && !minfo->model_id) {
+			minfo->smia_manufacturer_id =
+				minfo->sensor_smia_manufacturer_id;
+			minfo->model_id = minfo->sensor_model_id;
+			minfo->revision_number = minfo->sensor_revision_number;
+		}
+	}
+
+	for (i = 0; i < ARRAY_SIZE(ccs_module_idents); i++) {
+		if (ccs_module_idents[i].mipi_manufacturer_id &&
+		    ccs_module_idents[i].mipi_manufacturer_id
+		    != minfo->mipi_manufacturer_id)
+			continue;
+		if (ccs_module_idents[i].smia_manufacturer_id &&
+		    ccs_module_idents[i].smia_manufacturer_id
+		    != minfo->smia_manufacturer_id)
+			continue;
+		if (ccs_module_idents[i].model_id != minfo->model_id)
+			continue;
+		if (ccs_module_idents[i].flags
+		    & CCS_MODULE_IDENT_FLAG_REV_LE) {
+			if (ccs_module_idents[i].revision_number_major
+			    < (minfo->revision_number >> 8))
+				continue;
+		} else {
+			if (ccs_module_idents[i].revision_number_major
+			    != (minfo->revision_number >> 8))
+				continue;
+		}
+
+		minfo->name = ccs_module_idents[i].name;
+		minfo->quirk = ccs_module_idents[i].quirk;
+		break;
+	}
+
+	if (i >= ARRAY_SIZE(ccs_module_idents))
+		dev_warn(&client->dev,
+			 "no quirks for this module; let's hope it's fully compliant\n");
+
+	dev_dbg(&client->dev, "the sensor is called %s\n", minfo->name);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_ops ccs_ops;
+static const struct v4l2_subdev_internal_ops ccs_internal_ops;
+static const struct media_entity_operations ccs_entity_ops;
+
+static int ccs_register_subdev(struct ccs_sensor *sensor,
+			       struct ccs_subdev *ssd,
+			       struct ccs_subdev *sink_ssd,
+			       u16 source_pad, u16 sink_pad, u32 link_flags)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	int rval;
+
+	if (!sink_ssd)
+		return 0;
+
+	rval = media_entity_pads_init(&ssd->sd.entity, ssd->npads, ssd->pads);
+	if (rval) {
+		dev_err(&client->dev, "media_entity_pads_init failed\n");
+		return rval;
+	}
+
+	rval = v4l2_device_register_subdev(sensor->src->sd.v4l2_dev, &ssd->sd);
+	if (rval) {
+		dev_err(&client->dev, "v4l2_device_register_subdev failed\n");
+		return rval;
+	}
+
+	rval = media_create_pad_link(&ssd->sd.entity, source_pad,
+				     &sink_ssd->sd.entity, sink_pad,
+				     link_flags);
+	if (rval) {
+		dev_err(&client->dev, "media_create_pad_link failed\n");
+		v4l2_device_unregister_subdev(&ssd->sd);
+		return rval;
+	}
+
+	return 0;
+}
+
+static void ccs_unregistered(struct v4l2_subdev *subdev)
+{
+	struct ccs_sensor *sensor = to_ccs_sensor(subdev);
+	unsigned int i;
+
+	for (i = 1; i < sensor->ssds_used; i++)
+		v4l2_device_unregister_subdev(&sensor->ssds[i].sd);
+}
+
+static int ccs_registered(struct v4l2_subdev *subdev)
+{
+	struct ccs_sensor *sensor = to_ccs_sensor(subdev);
+	int rval;
+
+	if (sensor->scaler) {
+		rval = ccs_register_subdev(sensor, sensor->binner,
+					   sensor->scaler,
+					   CCS_PAD_SRC, CCS_PAD_SINK,
+					   MEDIA_LNK_FL_ENABLED |
+					   MEDIA_LNK_FL_IMMUTABLE);
+		if (rval < 0)
+			return rval;
+	}
+
+	rval = ccs_register_subdev(sensor, sensor->pixel_array, sensor->binner,
+				   CCS_PA_PAD_SRC, CCS_PAD_SINK,
+				   MEDIA_LNK_FL_ENABLED |
+				   MEDIA_LNK_FL_IMMUTABLE);
+	if (rval)
+		goto out_err;
+
+	return 0;
+
+out_err:
+	ccs_unregistered(subdev);
+
+	return rval;
+}
+
+static void ccs_cleanup(struct ccs_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+
+	device_remove_file(&client->dev, &dev_attr_nvm);
+	device_remove_file(&client->dev, &dev_attr_ident);
+
+	ccs_free_controls(sensor);
+}
+
+static void ccs_create_subdev(struct ccs_sensor *sensor,
+			      struct ccs_subdev *ssd, const char *name,
+			      unsigned short num_pads, u32 function)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+
+	if (!ssd)
+		return;
+
+	if (ssd != sensor->src)
+		v4l2_subdev_init(&ssd->sd, &ccs_ops);
+
+	ssd->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	ssd->sd.entity.function = function;
+	ssd->sensor = sensor;
+
+	ssd->npads = num_pads;
+	ssd->source_pad = num_pads - 1;
+
+	v4l2_i2c_subdev_set_name(&ssd->sd, client, sensor->minfo.name, name);
+
+	ccs_get_native_size(ssd, &ssd->sink_fmt);
+
+	ssd->compose.width = ssd->sink_fmt.width;
+	ssd->compose.height = ssd->sink_fmt.height;
+	ssd->crop[ssd->source_pad] = ssd->compose;
+	ssd->pads[ssd->source_pad].flags = MEDIA_PAD_FL_SOURCE;
+	if (ssd != sensor->pixel_array) {
+		ssd->crop[ssd->sink_pad] = ssd->compose;
+		ssd->pads[ssd->sink_pad].flags = MEDIA_PAD_FL_SINK;
+	}
+
+	ssd->sd.entity.ops = &ccs_entity_ops;
+
+	if (ssd == sensor->src)
+		return;
+
+	ssd->sd.internal_ops = &ccs_internal_ops;
+	ssd->sd.owner = THIS_MODULE;
+	ssd->sd.dev = &client->dev;
+	v4l2_set_subdevdata(&ssd->sd, client);
+}
+
+static int ccs_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct ccs_subdev *ssd = to_ccs_subdev(sd);
+	struct ccs_sensor *sensor = ssd->sensor;
+	unsigned int i;
+
+	mutex_lock(&sensor->mutex);
+
+	for (i = 0; i < ssd->npads; i++) {
+		struct v4l2_mbus_framefmt *try_fmt =
+			v4l2_subdev_get_try_format(sd, fh->state, i);
+		struct v4l2_rect *try_crop =
+			v4l2_subdev_get_try_crop(sd, fh->state, i);
+		struct v4l2_rect *try_comp;
+
+		ccs_get_native_size(ssd, try_crop);
+
+		try_fmt->width = try_crop->width;
+		try_fmt->height = try_crop->height;
+		try_fmt->code = sensor->internal_csi_format->code;
+		try_fmt->field = V4L2_FIELD_NONE;
+
+		if (ssd == sensor->pixel_array)
+			continue;
+
+		try_comp = v4l2_subdev_get_try_compose(sd, fh->state, i);
+		*try_comp = *try_crop;
+	}
+
+	mutex_unlock(&sensor->mutex);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops ccs_video_ops = {
+	.s_stream = ccs_set_stream,
+	.pre_streamon = ccs_pre_streamon,
+	.post_streamoff = ccs_post_streamoff,
+};
+
+static const struct v4l2_subdev_pad_ops ccs_pad_ops = {
+	.enum_mbus_code = ccs_enum_mbus_code,
+	.get_fmt = ccs_get_format,
+	.set_fmt = ccs_set_format,
+	.get_selection = ccs_get_selection,
+	.set_selection = ccs_set_selection,
+};
+
+static const struct v4l2_subdev_sensor_ops ccs_sensor_ops = {
+	.g_skip_frames = ccs_get_skip_frames,
+	.g_skip_top_lines = ccs_get_skip_top_lines,
+};
+
+static const struct v4l2_subdev_ops ccs_ops = {
+	.video = &ccs_video_ops,
+	.pad = &ccs_pad_ops,
+	.sensor = &ccs_sensor_ops,
+};
+
+static const struct media_entity_operations ccs_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static const struct v4l2_subdev_internal_ops ccs_internal_src_ops = {
+	.registered = ccs_registered,
+	.unregistered = ccs_unregistered,
+	.open = ccs_open,
+};
+
+static const struct v4l2_subdev_internal_ops ccs_internal_ops = {
+	.open = ccs_open,
+};
+
+/* -----------------------------------------------------------------------------
+ * I2C Driver
+ */
+
+static int __maybe_unused ccs_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct ccs_sensor *sensor = to_ccs_sensor(subdev);
+	bool streaming = sensor->streaming;
+	int rval;
+
+	rval = pm_runtime_resume_and_get(dev);
+	if (rval < 0)
+		return rval;
+
+	if (sensor->streaming)
+		ccs_stop_streaming(sensor);
+
+	/* save state for resume */
+	sensor->streaming = streaming;
+
+	return 0;
+}
+
+static int __maybe_unused ccs_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct ccs_sensor *sensor = to_ccs_sensor(subdev);
+	int rval = 0;
+
+	pm_runtime_put(dev);
+
+	if (sensor->streaming)
+		rval = ccs_start_streaming(sensor);
+
+	return rval;
+}
+
+static int ccs_get_hwconfig(struct ccs_sensor *sensor, struct device *dev)
+{
+	struct ccs_hwconfig *hwcfg = &sensor->hwcfg;
+	struct v4l2_fwnode_endpoint bus_cfg = { .bus_type = V4L2_MBUS_UNKNOWN };
+	struct fwnode_handle *ep;
+	struct fwnode_handle *fwnode = dev_fwnode(dev);
+	unsigned int i;
+	int rval;
+
+	ep = fwnode_graph_get_endpoint_by_id(fwnode, 0, 0,
+					     FWNODE_GRAPH_ENDPOINT_NEXT);
+	if (!ep)
+		return -ENODEV;
+
+	/*
+	 * Note that we do need to rely on detecting the bus type between CSI-2
+	 * D-PHY and CCP2 as the old bindings did not require it.
+	 */
+	rval = v4l2_fwnode_endpoint_alloc_parse(ep, &bus_cfg);
+	if (rval)
+		goto out_err;
+
+	switch (bus_cfg.bus_type) {
+	case V4L2_MBUS_CSI2_DPHY:
+		hwcfg->csi_signalling_mode = CCS_CSI_SIGNALING_MODE_CSI_2_DPHY;
+		hwcfg->lanes = bus_cfg.bus.mipi_csi2.num_data_lanes;
+		break;
+	case V4L2_MBUS_CSI2_CPHY:
+		hwcfg->csi_signalling_mode = CCS_CSI_SIGNALING_MODE_CSI_2_CPHY;
+		hwcfg->lanes = bus_cfg.bus.mipi_csi2.num_data_lanes;
+		break;
+	case V4L2_MBUS_CSI1:
+	case V4L2_MBUS_CCP2:
+		hwcfg->csi_signalling_mode = (bus_cfg.bus.mipi_csi1.strobe) ?
+		SMIAPP_CSI_SIGNALLING_MODE_CCP2_DATA_STROBE :
+		SMIAPP_CSI_SIGNALLING_MODE_CCP2_DATA_CLOCK;
+		hwcfg->lanes = 1;
+		break;
+	default:
+		dev_err(dev, "unsupported bus %u\n", bus_cfg.bus_type);
+		rval = -EINVAL;
+		goto out_err;
+	}
+
+	rval = fwnode_property_read_u32(dev_fwnode(dev), "clock-frequency",
+					&hwcfg->ext_clk);
+	if (rval)
+		dev_info(dev, "can't get clock-frequency\n");
+
+	dev_dbg(dev, "clk %u, mode %u\n", hwcfg->ext_clk,
+		hwcfg->csi_signalling_mode);
+
+	if (!bus_cfg.nr_of_link_frequencies) {
+		dev_warn(dev, "no link frequencies defined\n");
+		rval = -EINVAL;
+		goto out_err;
+	}
+
+	hwcfg->op_sys_clock = devm_kcalloc(
+		dev, bus_cfg.nr_of_link_frequencies + 1 /* guardian */,
+		sizeof(*hwcfg->op_sys_clock), GFP_KERNEL);
+	if (!hwcfg->op_sys_clock) {
+		rval = -ENOMEM;
+		goto out_err;
+	}
+
+	for (i = 0; i < bus_cfg.nr_of_link_frequencies; i++) {
+		hwcfg->op_sys_clock[i] = bus_cfg.link_frequencies[i];
+		dev_dbg(dev, "freq %u: %lld\n", i, hwcfg->op_sys_clock[i]);
+	}
+
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+	fwnode_handle_put(ep);
+
+	return 0;
+
+out_err:
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+	fwnode_handle_put(ep);
+
+	return rval;
+}
+
+static int ccs_firmware_name(struct i2c_client *client,
+			     struct ccs_sensor *sensor, char *filename,
+			     size_t filename_size, bool is_module)
+{
+	const struct ccs_device *ccsdev = device_get_match_data(&client->dev);
+	bool is_ccs = !(ccsdev->flags & CCS_DEVICE_FLAG_IS_SMIA);
+	bool is_smiapp = sensor->minfo.smiapp_version;
+	u16 manufacturer_id;
+	u16 model_id;
+	u16 revision_number;
+
+	/*
+	 * Old SMIA is module-agnostic. Its sensor identification is based on
+	 * what now are those of the module.
+	 */
+	if (is_module || (!is_ccs && !is_smiapp)) {
+		manufacturer_id = is_ccs ?
+			sensor->minfo.mipi_manufacturer_id :
+			sensor->minfo.smia_manufacturer_id;
+		model_id = sensor->minfo.model_id;
+		revision_number = sensor->minfo.revision_number;
+	} else {
+		manufacturer_id = is_ccs ?
+			sensor->minfo.sensor_mipi_manufacturer_id :
+			sensor->minfo.sensor_smia_manufacturer_id;
+		model_id = sensor->minfo.sensor_model_id;
+		revision_number = sensor->minfo.sensor_revision_number;
+	}
+
+	return snprintf(filename, filename_size,
+			"ccs/%s-%s-%0*x-%4.4x-%0*x.fw",
+			is_ccs ? "ccs" : is_smiapp ? "smiapp" : "smia",
+			is_module || (!is_ccs && !is_smiapp) ?
+				"module" : "sensor",
+			is_ccs ? 4 : 2, manufacturer_id, model_id,
+			!is_ccs && !is_module ? 2 : 4, revision_number);
+}
+
+static int ccs_probe(struct i2c_client *client)
+{
+	const struct ccs_device *ccsdev = device_get_match_data(&client->dev);
+	struct ccs_sensor *sensor;
+	const struct firmware *fw;
+	char filename[40];
+	unsigned int i;
+	int rval;
+
+	sensor = devm_kzalloc(&client->dev, sizeof(*sensor), GFP_KERNEL);
+	if (sensor == NULL)
+		return -ENOMEM;
+
+	rval = ccs_get_hwconfig(sensor, &client->dev);
+	if (rval)
+		return rval;
+
+	sensor->src = &sensor->ssds[sensor->ssds_used];
+
+	v4l2_i2c_subdev_init(&sensor->src->sd, client, &ccs_ops);
+	sensor->src->sd.internal_ops = &ccs_internal_src_ops;
+
+	sensor->regulators = devm_kcalloc(&client->dev,
+					  ARRAY_SIZE(ccs_regulators),
+					  sizeof(*sensor->regulators),
+					  GFP_KERNEL);
+	if (!sensor->regulators)
+		return -ENOMEM;
+
+	for (i = 0; i < ARRAY_SIZE(ccs_regulators); i++)
+		sensor->regulators[i].supply = ccs_regulators[i];
+
+	rval = devm_regulator_bulk_get(&client->dev, ARRAY_SIZE(ccs_regulators),
+				       sensor->regulators);
+	if (rval) {
+		dev_err(&client->dev, "could not get regulators\n");
+		return rval;
+	}
+
+	sensor->ext_clk = devm_clk_get(&client->dev, NULL);
+	if (PTR_ERR(sensor->ext_clk) == -ENOENT) {
+		dev_info(&client->dev, "no clock defined, continuing...\n");
+		sensor->ext_clk = NULL;
+	} else if (IS_ERR(sensor->ext_clk)) {
+		dev_err(&client->dev, "could not get clock (%ld)\n",
+			PTR_ERR(sensor->ext_clk));
+		return -EPROBE_DEFER;
+	}
+
+	if (sensor->ext_clk) {
+		if (sensor->hwcfg.ext_clk) {
+			unsigned long rate;
+
+			rval = clk_set_rate(sensor->ext_clk,
+					    sensor->hwcfg.ext_clk);
+			if (rval < 0) {
+				dev_err(&client->dev,
+					"unable to set clock freq to %u\n",
+					sensor->hwcfg.ext_clk);
+				return rval;
+			}
+
+			rate = clk_get_rate(sensor->ext_clk);
+			if (rate != sensor->hwcfg.ext_clk) {
+				dev_err(&client->dev,
+					"can't set clock freq, asked for %u but got %lu\n",
+					sensor->hwcfg.ext_clk, rate);
+				return -EINVAL;
+			}
+		} else {
+			sensor->hwcfg.ext_clk = clk_get_rate(sensor->ext_clk);
+			dev_dbg(&client->dev, "obtained clock freq %u\n",
+				sensor->hwcfg.ext_clk);
+		}
+	} else if (sensor->hwcfg.ext_clk) {
+		dev_dbg(&client->dev, "assuming clock freq %u\n",
+			sensor->hwcfg.ext_clk);
+	} else {
+		dev_err(&client->dev, "unable to obtain clock freq\n");
+		return -EINVAL;
+	}
+
+	if (!sensor->hwcfg.ext_clk) {
+		dev_err(&client->dev, "cannot work with xclk frequency 0\n");
+		return -EINVAL;
+	}
+
+	sensor->reset = devm_gpiod_get_optional(&client->dev, "reset",
+						GPIOD_OUT_HIGH);
+	if (IS_ERR(sensor->reset))
+		return PTR_ERR(sensor->reset);
+	/* Support old users that may have used "xshutdown" property. */
+	if (!sensor->reset)
+		sensor->xshutdown = devm_gpiod_get_optional(&client->dev,
+							    "xshutdown",
+							    GPIOD_OUT_LOW);
+	if (IS_ERR(sensor->xshutdown))
+		return PTR_ERR(sensor->xshutdown);
+
+	rval = ccs_power_on(&client->dev);
+	if (rval < 0)
+		return rval;
+
+	mutex_init(&sensor->mutex);
+
+	rval = ccs_identify_module(sensor);
+	if (rval) {
+		rval = -ENODEV;
+		goto out_power_off;
+	}
+
+	rval = ccs_firmware_name(client, sensor, filename, sizeof(filename),
+				 false);
+	if (rval >= sizeof(filename)) {
+		rval = -ENOMEM;
+		goto out_power_off;
+	}
+
+	rval = request_firmware(&fw, filename, &client->dev);
+	if (!rval) {
+		ccs_data_parse(&sensor->sdata, fw->data, fw->size, &client->dev,
+			       true);
+		release_firmware(fw);
+	}
+
+	if (!(ccsdev->flags & CCS_DEVICE_FLAG_IS_SMIA) ||
+	    sensor->minfo.smiapp_version) {
+		rval = ccs_firmware_name(client, sensor, filename,
+					 sizeof(filename), true);
+		if (rval >= sizeof(filename)) {
+			rval = -ENOMEM;
+			goto out_release_sdata;
+		}
+
+		rval = request_firmware(&fw, filename, &client->dev);
+		if (!rval) {
+			ccs_data_parse(&sensor->mdata, fw->data, fw->size,
+				       &client->dev, true);
+			release_firmware(fw);
+		}
+	}
+
+	rval = ccs_read_all_limits(sensor);
+	if (rval)
+		goto out_release_mdata;
+
+	rval = ccs_read_frame_fmt(sensor);
+	if (rval) {
+		rval = -ENODEV;
+		goto out_free_ccs_limits;
+	}
+
+	rval = ccs_update_phy_ctrl(sensor);
+	if (rval < 0)
+		goto out_free_ccs_limits;
+
+	rval = ccs_call_quirk(sensor, limits);
+	if (rval) {
+		dev_err(&client->dev, "limits quirks failed\n");
+		goto out_free_ccs_limits;
+	}
+
+	if (CCS_LIM(sensor, BINNING_CAPABILITY)) {
+		sensor->nbinning_subtypes =
+			min_t(u8, CCS_LIM(sensor, BINNING_SUB_TYPES),
+			      CCS_LIM_BINNING_SUB_TYPE_MAX_N);
+
+		for (i = 0; i < sensor->nbinning_subtypes; i++) {
+			sensor->binning_subtypes[i].horizontal =
+				CCS_LIM_AT(sensor, BINNING_SUB_TYPE, i) >>
+				CCS_BINNING_SUB_TYPE_COLUMN_SHIFT;
+			sensor->binning_subtypes[i].vertical =
+				CCS_LIM_AT(sensor, BINNING_SUB_TYPE, i) &
+				CCS_BINNING_SUB_TYPE_ROW_MASK;
+
+			dev_dbg(&client->dev, "binning %xx%x\n",
+				sensor->binning_subtypes[i].horizontal,
+				sensor->binning_subtypes[i].vertical);
+		}
+	}
+	sensor->binning_horizontal = 1;
+	sensor->binning_vertical = 1;
+
+	if (device_create_file(&client->dev, &dev_attr_ident) != 0) {
+		dev_err(&client->dev, "sysfs ident entry creation failed\n");
+		rval = -ENOENT;
+		goto out_free_ccs_limits;
+	}
+
+	if (sensor->minfo.smiapp_version &&
+	    CCS_LIM(sensor, DATA_TRANSFER_IF_CAPABILITY) &
+	    CCS_DATA_TRANSFER_IF_CAPABILITY_SUPPORTED) {
+		if (device_create_file(&client->dev, &dev_attr_nvm) != 0) {
+			dev_err(&client->dev, "sysfs nvm entry failed\n");
+			rval = -EBUSY;
+			goto out_cleanup;
+		}
+	}
+
+	if (!CCS_LIM(sensor, MIN_OP_SYS_CLK_DIV) ||
+	    !CCS_LIM(sensor, MAX_OP_SYS_CLK_DIV) ||
+	    !CCS_LIM(sensor, MIN_OP_PIX_CLK_DIV) ||
+	    !CCS_LIM(sensor, MAX_OP_PIX_CLK_DIV)) {
+		/* No OP clock branch */
+		sensor->pll.flags |= CCS_PLL_FLAG_NO_OP_CLOCKS;
+	} else if (CCS_LIM(sensor, SCALING_CAPABILITY)
+		   != CCS_SCALING_CAPABILITY_NONE ||
+		   CCS_LIM(sensor, DIGITAL_CROP_CAPABILITY)
+		   == CCS_DIGITAL_CROP_CAPABILITY_INPUT_CROP) {
+		/* We have a scaler or digital crop. */
+		sensor->scaler = &sensor->ssds[sensor->ssds_used];
+		sensor->ssds_used++;
+	}
+	sensor->binner = &sensor->ssds[sensor->ssds_used];
+	sensor->ssds_used++;
+	sensor->pixel_array = &sensor->ssds[sensor->ssds_used];
+	sensor->ssds_used++;
+
+	sensor->scale_m = CCS_LIM(sensor, SCALER_N_MIN);
+
+	/* prepare PLL configuration input values */
+	sensor->pll.bus_type = CCS_PLL_BUS_TYPE_CSI2_DPHY;
+	sensor->pll.csi2.lanes = sensor->hwcfg.lanes;
+	if (CCS_LIM(sensor, CLOCK_CALCULATION) &
+	    CCS_CLOCK_CALCULATION_LANE_SPEED) {
+		sensor->pll.flags |= CCS_PLL_FLAG_LANE_SPEED_MODEL;
+		if (CCS_LIM(sensor, CLOCK_CALCULATION) &
+		    CCS_CLOCK_CALCULATION_LINK_DECOUPLED) {
+			sensor->pll.vt_lanes =
+				CCS_LIM(sensor, NUM_OF_VT_LANES) + 1;
+			sensor->pll.op_lanes =
+				CCS_LIM(sensor, NUM_OF_OP_LANES) + 1;
+			sensor->pll.flags |= CCS_PLL_FLAG_LINK_DECOUPLED;
+		} else {
+			sensor->pll.vt_lanes = sensor->pll.csi2.lanes;
+			sensor->pll.op_lanes = sensor->pll.csi2.lanes;
+		}
+	}
+	if (CCS_LIM(sensor, CLOCK_TREE_PLL_CAPABILITY) &
+	    CCS_CLOCK_TREE_PLL_CAPABILITY_EXT_DIVIDER)
+		sensor->pll.flags |= CCS_PLL_FLAG_EXT_IP_PLL_DIVIDER;
+	if (CCS_LIM(sensor, CLOCK_TREE_PLL_CAPABILITY) &
+	    CCS_CLOCK_TREE_PLL_CAPABILITY_FLEXIBLE_OP_PIX_CLK_DIV)
+		sensor->pll.flags |= CCS_PLL_FLAG_FLEXIBLE_OP_PIX_CLK_DIV;
+	if (CCS_LIM(sensor, FIFO_SUPPORT_CAPABILITY) &
+	    CCS_FIFO_SUPPORT_CAPABILITY_DERATING)
+		sensor->pll.flags |= CCS_PLL_FLAG_FIFO_DERATING;
+	if (CCS_LIM(sensor, FIFO_SUPPORT_CAPABILITY) &
+	    CCS_FIFO_SUPPORT_CAPABILITY_DERATING_OVERRATING)
+		sensor->pll.flags |= CCS_PLL_FLAG_FIFO_DERATING |
+				     CCS_PLL_FLAG_FIFO_OVERRATING;
+	if (CCS_LIM(sensor, CLOCK_TREE_PLL_CAPABILITY) &
+	    CCS_CLOCK_TREE_PLL_CAPABILITY_DUAL_PLL) {
+		if (CCS_LIM(sensor, CLOCK_TREE_PLL_CAPABILITY) &
+		    CCS_CLOCK_TREE_PLL_CAPABILITY_SINGLE_PLL) {
+			u32 v;
+
+			/* Use sensor default in PLL mode selection */
+			rval = ccs_read(sensor, PLL_MODE, &v);
+			if (rval)
+				goto out_cleanup;
+
+			if (v == CCS_PLL_MODE_DUAL)
+				sensor->pll.flags |= CCS_PLL_FLAG_DUAL_PLL;
+		} else {
+			sensor->pll.flags |= CCS_PLL_FLAG_DUAL_PLL;
+		}
+		if (CCS_LIM(sensor, CLOCK_CALCULATION) &
+		    CCS_CLOCK_CALCULATION_DUAL_PLL_OP_SYS_DDR)
+			sensor->pll.flags |= CCS_PLL_FLAG_OP_SYS_DDR;
+		if (CCS_LIM(sensor, CLOCK_CALCULATION) &
+		    CCS_CLOCK_CALCULATION_DUAL_PLL_OP_PIX_DDR)
+			sensor->pll.flags |= CCS_PLL_FLAG_OP_PIX_DDR;
+	}
+	sensor->pll.op_bits_per_lane = CCS_LIM(sensor, OP_BITS_PER_LANE);
+	sensor->pll.ext_clk_freq_hz = sensor->hwcfg.ext_clk;
+	sensor->pll.scale_n = CCS_LIM(sensor, SCALER_N_MIN);
+
+	ccs_create_subdev(sensor, sensor->scaler, " scaler", 2,
+			  MEDIA_ENT_F_PROC_VIDEO_SCALER);
+	ccs_create_subdev(sensor, sensor->binner, " binner", 2,
+			  MEDIA_ENT_F_PROC_VIDEO_SCALER);
+	ccs_create_subdev(sensor, sensor->pixel_array, " pixel_array", 1,
+			  MEDIA_ENT_F_CAM_SENSOR);
+
+	rval = ccs_init_controls(sensor);
+	if (rval < 0)
+		goto out_cleanup;
+
+	rval = ccs_call_quirk(sensor, init);
+	if (rval)
+		goto out_cleanup;
+
+	rval = ccs_get_mbus_formats(sensor);
+	if (rval) {
+		rval = -ENODEV;
+		goto out_cleanup;
+	}
+
+	rval = ccs_init_late_controls(sensor);
+	if (rval) {
+		rval = -ENODEV;
+		goto out_cleanup;
+	}
+
+	mutex_lock(&sensor->mutex);
+	rval = ccs_pll_blanking_update(sensor);
+	mutex_unlock(&sensor->mutex);
+	if (rval) {
+		dev_err(&client->dev, "update mode failed\n");
+		goto out_cleanup;
+	}
+
+	sensor->streaming = false;
+	sensor->dev_init_done = true;
+
+	rval = media_entity_pads_init(&sensor->src->sd.entity, 2,
+				 sensor->src->pads);
+	if (rval < 0)
+		goto out_media_entity_cleanup;
+
+	rval = ccs_write_msr_regs(sensor);
+	if (rval)
+		goto out_media_entity_cleanup;
+
+	pm_runtime_set_active(&client->dev);
+	pm_runtime_get_noresume(&client->dev);
+	pm_runtime_enable(&client->dev);
+
+	rval = v4l2_async_register_subdev_sensor(&sensor->src->sd);
+	if (rval < 0)
+		goto out_disable_runtime_pm;
+
+	pm_runtime_set_autosuspend_delay(&client->dev, 1000);
+	pm_runtime_use_autosuspend(&client->dev);
+	pm_runtime_put_autosuspend(&client->dev);
+
+	return 0;
+
+out_disable_runtime_pm:
+	pm_runtime_put_noidle(&client->dev);
+	pm_runtime_disable(&client->dev);
+
+out_media_entity_cleanup:
+	media_entity_cleanup(&sensor->src->sd.entity);
+
+out_cleanup:
+	ccs_cleanup(sensor);
+
+out_release_mdata:
+	kvfree(sensor->mdata.backing);
+
+out_release_sdata:
+	kvfree(sensor->sdata.backing);
+
+out_free_ccs_limits:
+	kfree(sensor->ccs_limits);
+
+out_power_off:
+	ccs_power_off(&client->dev);
+	mutex_destroy(&sensor->mutex);
+
+	return rval;
+}
+
+static void ccs_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct ccs_sensor *sensor = to_ccs_sensor(subdev);
+	unsigned int i;
+
+	v4l2_async_unregister_subdev(subdev);
+
+	pm_runtime_disable(&client->dev);
+	if (!pm_runtime_status_suspended(&client->dev))
+		ccs_power_off(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+
+	for (i = 0; i < sensor->ssds_used; i++) {
+		v4l2_device_unregister_subdev(&sensor->ssds[i].sd);
+		media_entity_cleanup(&sensor->ssds[i].sd.entity);
+	}
+	ccs_cleanup(sensor);
+	mutex_destroy(&sensor->mutex);
+	kfree(sensor->ccs_limits);
+	kvfree(sensor->sdata.backing);
+	kvfree(sensor->mdata.backing);
+}
+
+static const struct ccs_device smia_device = {
+	.flags = CCS_DEVICE_FLAG_IS_SMIA,
+};
+
+static const struct ccs_device ccs_device = {};
+
+static const struct acpi_device_id ccs_acpi_table[] = {
+	{ .id = "MIPI0200", .driver_data = (unsigned long)&ccs_device },
+	{ },
+};
+MODULE_DEVICE_TABLE(acpi, ccs_acpi_table);
+
+static const struct of_device_id ccs_of_table[] = {
+	{ .compatible = "mipi-ccs-1.1", .data = &ccs_device },
+	{ .compatible = "mipi-ccs-1.0", .data = &ccs_device },
+	{ .compatible = "mipi-ccs", .data = &ccs_device },
+	{ .compatible = "nokia,smia", .data = &smia_device },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, ccs_of_table);
+
+static const struct dev_pm_ops ccs_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(ccs_suspend, ccs_resume)
+	SET_RUNTIME_PM_OPS(ccs_power_off, ccs_power_on, NULL)
+};
+
+static struct i2c_driver ccs_i2c_driver = {
+	.driver	= {
+		.acpi_match_table = ccs_acpi_table,
+		.of_match_table = ccs_of_table,
+		.name = CCS_NAME,
+		.pm = &ccs_pm_ops,
+	},
+	.probe = ccs_probe,
+	.remove	= ccs_remove,
+};
+
+static int ccs_module_init(void)
+{
+	unsigned int i, l;
+
+	for (i = 0, l = 0; ccs_limits[i].size && l < CCS_L_LAST; i++) {
+		if (!(ccs_limits[i].flags & CCS_L_FL_SAME_REG)) {
+			ccs_limit_offsets[l + 1].lim =
+				ALIGN(ccs_limit_offsets[l].lim +
+				      ccs_limits[i].size,
+				      ccs_reg_width(ccs_limits[i + 1].reg));
+			ccs_limit_offsets[l].info = i;
+			l++;
+		} else {
+			ccs_limit_offsets[l].lim += ccs_limits[i].size;
+		}
+	}
+
+	if (WARN_ON(ccs_limits[i].size))
+		return -EINVAL;
+
+	if (WARN_ON(l != CCS_L_LAST))
+		return -EINVAL;
+
+	return i2c_register_driver(THIS_MODULE, &ccs_i2c_driver);
+}
+
+static void ccs_module_cleanup(void)
+{
+	i2c_del_driver(&ccs_i2c_driver);
+}
+
+module_init(ccs_module_init);
+module_exit(ccs_module_cleanup);
+
+MODULE_AUTHOR("Sakari Ailus <sakari.ailus@linux.intel.com>");
+MODULE_DESCRIPTION("Generic MIPI CCS/SMIA/SMIA++ camera sensor driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("smiapp");
diff --git a/drivers/media/i2c/ccs/ccs-data-defs.h b/drivers/media/i2c/ccs/ccs-data-defs.h
new file mode 100644
index 000000000..1c9b1d1ac
--- /dev/null
+++ b/drivers/media/i2c/ccs/ccs-data-defs.h
@@ -0,0 +1,221 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/*
+ * CCS static data binary format definitions
+ *
+ * Copyright 2019--2020 Intel Corporation
+ */
+
+#ifndef __CCS_DATA_DEFS_H__
+#define __CCS_DATA_DEFS_H__
+
+#include "ccs-data.h"
+
+#define CCS_STATIC_DATA_VERSION	0
+
+enum __ccs_data_length_specifier_id {
+	CCS_DATA_LENGTH_SPECIFIER_1 = 0,
+	CCS_DATA_LENGTH_SPECIFIER_2 = 1,
+	CCS_DATA_LENGTH_SPECIFIER_3 = 2
+};
+
+#define CCS_DATA_LENGTH_SPECIFIER_SIZE_SHIFT	6
+
+struct __ccs_data_length_specifier {
+	u8 length;
+} __packed;
+
+struct __ccs_data_length_specifier2 {
+	u8 length[2];
+} __packed;
+
+struct __ccs_data_length_specifier3 {
+	u8 length[3];
+} __packed;
+
+struct __ccs_data_block {
+	u8 id;
+	struct __ccs_data_length_specifier length;
+} __packed;
+
+#define CCS_DATA_BLOCK_HEADER_ID_VERSION_SHIFT	5
+
+struct __ccs_data_block3 {
+	u8 id;
+	struct __ccs_data_length_specifier2 length;
+} __packed;
+
+struct __ccs_data_block4 {
+	u8 id;
+	struct __ccs_data_length_specifier3 length;
+} __packed;
+
+enum __ccs_data_block_id {
+	CCS_DATA_BLOCK_ID_DUMMY	= 1,
+	CCS_DATA_BLOCK_ID_DATA_VERSION = 2,
+	CCS_DATA_BLOCK_ID_SENSOR_READ_ONLY_REGS = 3,
+	CCS_DATA_BLOCK_ID_MODULE_READ_ONLY_REGS = 4,
+	CCS_DATA_BLOCK_ID_SENSOR_MANUFACTURER_REGS = 5,
+	CCS_DATA_BLOCK_ID_MODULE_MANUFACTURER_REGS = 6,
+	CCS_DATA_BLOCK_ID_SENSOR_RULE_BASED_BLOCK = 32,
+	CCS_DATA_BLOCK_ID_MODULE_RULE_BASED_BLOCK = 33,
+	CCS_DATA_BLOCK_ID_SENSOR_PDAF_PIXEL_LOCATION = 36,
+	CCS_DATA_BLOCK_ID_MODULE_PDAF_PIXEL_LOCATION = 37,
+	CCS_DATA_BLOCK_ID_LICENSE = 40,
+	CCS_DATA_BLOCK_ID_END = 127,
+};
+
+struct __ccs_data_block_version {
+	u8 static_data_version_major[2];
+	u8 static_data_version_minor[2];
+	u8 year[2];
+	u8 month;
+	u8 day;
+} __packed;
+
+struct __ccs_data_block_regs {
+	u8 reg_len;
+} __packed;
+
+#define CCS_DATA_BLOCK_REGS_ADDR_MASK		0x07
+#define CCS_DATA_BLOCK_REGS_LEN_SHIFT		3
+#define CCS_DATA_BLOCK_REGS_LEN_MASK		0x38
+#define CCS_DATA_BLOCK_REGS_SEL_SHIFT		6
+
+enum ccs_data_block_regs_sel {
+	CCS_DATA_BLOCK_REGS_SEL_REGS = 0,
+	CCS_DATA_BLOCK_REGS_SEL_REGS2 = 1,
+	CCS_DATA_BLOCK_REGS_SEL_REGS3 = 2,
+};
+
+struct __ccs_data_block_regs2 {
+	u8 reg_len;
+	u8 addr;
+} __packed;
+
+#define CCS_DATA_BLOCK_REGS_2_ADDR_MASK		0x01
+#define CCS_DATA_BLOCK_REGS_2_LEN_SHIFT		1
+#define CCS_DATA_BLOCK_REGS_2_LEN_MASK		0x3e
+
+struct __ccs_data_block_regs3 {
+	u8 reg_len;
+	u8 addr[2];
+} __packed;
+
+#define CCS_DATA_BLOCK_REGS_3_LEN_MASK		0x3f
+
+enum __ccs_data_ffd_pixelcode {
+	CCS_DATA_BLOCK_FFD_PIXELCODE_EMBEDDED = 1,
+	CCS_DATA_BLOCK_FFD_PIXELCODE_DUMMY = 2,
+	CCS_DATA_BLOCK_FFD_PIXELCODE_BLACK = 3,
+	CCS_DATA_BLOCK_FFD_PIXELCODE_DARK = 4,
+	CCS_DATA_BLOCK_FFD_PIXELCODE_VISIBLE = 5,
+	CCS_DATA_BLOCK_FFD_PIXELCODE_MS_0 = 8,
+	CCS_DATA_BLOCK_FFD_PIXELCODE_MS_1 = 9,
+	CCS_DATA_BLOCK_FFD_PIXELCODE_MS_2 = 10,
+	CCS_DATA_BLOCK_FFD_PIXELCODE_MS_3 = 11,
+	CCS_DATA_BLOCK_FFD_PIXELCODE_MS_4 = 12,
+	CCS_DATA_BLOCK_FFD_PIXELCODE_MS_5 = 13,
+	CCS_DATA_BLOCK_FFD_PIXELCODE_MS_6 = 14,
+	CCS_DATA_BLOCK_FFD_PIXELCODE_TOP_OB = 16,
+	CCS_DATA_BLOCK_FFD_PIXELCODE_BOTTOM_OB = 17,
+	CCS_DATA_BLOCK_FFD_PIXELCODE_LEFT_OB = 18,
+	CCS_DATA_BLOCK_FFD_PIXELCODE_RIGHT_OB = 19,
+	CCS_DATA_BLOCK_FFD_PIXELCODE_TOP_LEFT_OB = 20,
+	CCS_DATA_BLOCK_FFD_PIXELCODE_TOP_RIGHT_OB = 21,
+	CCS_DATA_BLOCK_FFD_PIXELCODE_BOTTOM_LEFT_OB = 22,
+	CCS_DATA_BLOCK_FFD_PIXELCODE_BOTTOM_RIGHT_OB = 23,
+	CCS_DATA_BLOCK_FFD_PIXELCODE_TOTAL = 24,
+	CCS_DATA_BLOCK_FFD_PIXELCODE_TOP_PDAF = 32,
+	CCS_DATA_BLOCK_FFD_PIXELCODE_BOTTOM_PDAF = 33,
+	CCS_DATA_BLOCK_FFD_PIXELCODE_LEFT_PDAF = 34,
+	CCS_DATA_BLOCK_FFD_PIXELCODE_RIGHT_PDAF = 35,
+	CCS_DATA_BLOCK_FFD_PIXELCODE_TOP_LEFT_PDAF = 36,
+	CCS_DATA_BLOCK_FFD_PIXELCODE_TOP_RIGHT_PDAF = 37,
+	CCS_DATA_BLOCK_FFD_PIXELCODE_BOTTOM_LEFT_PDAF = 38,
+	CCS_DATA_BLOCK_FFD_PIXELCODE_BOTTOM_RIGHT_PDAF = 39,
+	CCS_DATA_BLOCK_FFD_PIXELCODE_SEPARATED_PDAF = 40,
+	CCS_DATA_BLOCK_FFD_PIXELCODE_ORIGINAL_ORDER_PDAF = 41,
+	CCS_DATA_BLOCK_FFD_PIXELCODE_VENDOR_PDAF = 41,
+};
+
+struct __ccs_data_block_ffd_entry {
+	u8 pixelcode;
+	u8 reserved;
+	u8 value[2];
+} __packed;
+
+struct __ccs_data_block_ffd {
+	u8 num_column_descs;
+	u8 num_row_descs;
+} __packed;
+
+enum __ccs_data_block_rule_id {
+	CCS_DATA_BLOCK_RULE_ID_IF = 1,
+	CCS_DATA_BLOCK_RULE_ID_READ_ONLY_REGS = 2,
+	CCS_DATA_BLOCK_RULE_ID_FFD = 3,
+	CCS_DATA_BLOCK_RULE_ID_MSR = 4,
+	CCS_DATA_BLOCK_RULE_ID_PDAF_READOUT = 5,
+};
+
+struct __ccs_data_block_rule_if {
+	u8 addr[2];
+	u8 value;
+	u8 mask;
+} __packed;
+
+enum __ccs_data_block_pdaf_readout_order {
+	CCS_DATA_BLOCK_PDAF_READOUT_ORDER_ORIGINAL = 1,
+	CCS_DATA_BLOCK_PDAF_READOUT_ORDER_SEPARATE_WITHIN_LINE = 2,
+	CCS_DATA_BLOCK_PDAF_READOUT_ORDER_SEPARATE_TYPES_SEPARATE_LINES = 3,
+};
+
+struct __ccs_data_block_pdaf_readout {
+	u8 pdaf_readout_info_reserved;
+	u8 pdaf_readout_info_order;
+} __packed;
+
+struct __ccs_data_block_pdaf_pix_loc_block_desc {
+	u8 block_type_id;
+	u8 repeat_x[2];
+} __packed;
+
+struct __ccs_data_block_pdaf_pix_loc_block_desc_group {
+	u8 num_block_descs[2];
+	u8 repeat_y;
+} __packed;
+
+enum __ccs_data_block_pdaf_pix_loc_pixel_type {
+	CCS_DATA_PDAF_PIXEL_TYPE_LEFT_SEPARATED = 0,
+	CCS_DATA_PDAF_PIXEL_TYPE_RIGHT_SEPARATED = 1,
+	CCS_DATA_PDAF_PIXEL_TYPE_TOP_SEPARATED = 2,
+	CCS_DATA_PDAF_PIXEL_TYPE_BOTTOM_SEPARATED = 3,
+	CCS_DATA_PDAF_PIXEL_TYPE_LEFT_SIDE_BY_SIDE = 4,
+	CCS_DATA_PDAF_PIXEL_TYPE_RIGHT_SIDE_BY_SIDE = 5,
+	CCS_DATA_PDAF_PIXEL_TYPE_TOP_SIDE_BY_SIDE = 6,
+	CCS_DATA_PDAF_PIXEL_TYPE_BOTTOM_SIDE_BY_SIDE = 7,
+	CCS_DATA_PDAF_PIXEL_TYPE_TOP_LEFT = 8,
+	CCS_DATA_PDAF_PIXEL_TYPE_TOP_RIGHT = 9,
+	CCS_DATA_PDAF_PIXEL_TYPE_BOTTOM_LEFT = 10,
+	CCS_DATA_PDAF_PIXEL_TYPE_BOTTOM_RIGHT = 11,
+};
+
+struct __ccs_data_block_pdaf_pix_loc_pixel_desc {
+	u8 pixel_type;
+	u8 small_offset_x;
+	u8 small_offset_y;
+} __packed;
+
+struct __ccs_data_block_pdaf_pix_loc {
+	u8 main_offset_x[2];
+	u8 main_offset_y[2];
+	u8 global_pdaf_type;
+	u8 block_width;
+	u8 block_height;
+	u8 num_block_desc_groups[2];
+} __packed;
+
+struct __ccs_data_block_end {
+	u8 crc[4];
+} __packed;
+
+#endif /* __CCS_DATA_DEFS_H__ */
diff --git a/drivers/media/i2c/ccs/ccs-data.c b/drivers/media/i2c/ccs/ccs-data.c
new file mode 100644
index 000000000..08400edf7
--- /dev/null
+++ b/drivers/media/i2c/ccs/ccs-data.c
@@ -0,0 +1,979 @@
+// SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
+/*
+ * CCS static data binary parser library
+ *
+ * Copyright 2019--2020 Intel Corporation
+ */
+
+#include <linux/device.h>
+#include <linux/errno.h>
+#include <linux/limits.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+
+#include "ccs-data-defs.h"
+
+struct bin_container {
+	void *base;
+	void *now;
+	void *end;
+	size_t size;
+};
+
+static void *bin_alloc(struct bin_container *bin, size_t len)
+{
+	void *ptr;
+
+	len = ALIGN(len, 8);
+
+	if (bin->end - bin->now < len)
+		return NULL;
+
+	ptr = bin->now;
+	bin->now += len;
+
+	return ptr;
+}
+
+static void bin_reserve(struct bin_container *bin, size_t len)
+{
+	bin->size += ALIGN(len, 8);
+}
+
+static int bin_backing_alloc(struct bin_container *bin)
+{
+	bin->base = bin->now = kvzalloc(bin->size, GFP_KERNEL);
+	if (!bin->base)
+		return -ENOMEM;
+
+	bin->end = bin->base + bin->size;
+
+	return 0;
+}
+
+#define is_contained(var, endp)				\
+	(sizeof(*var) <= (endp) - (void *)(var))
+#define has_headroom(ptr, headroom, endp)	\
+	((headroom) <= (endp) - (void *)(ptr))
+#define is_contained_with_headroom(var, headroom, endp)		\
+	(sizeof(*var) + (headroom) <= (endp) - (void *)(var))
+
+static int
+ccs_data_parse_length_specifier(const struct __ccs_data_length_specifier *__len,
+				size_t *__hlen, size_t *__plen,
+				const void *endp)
+{
+	size_t hlen, plen;
+
+	if (!is_contained(__len, endp))
+		return -ENODATA;
+
+	switch (__len->length >> CCS_DATA_LENGTH_SPECIFIER_SIZE_SHIFT) {
+	case CCS_DATA_LENGTH_SPECIFIER_1:
+		hlen = sizeof(*__len);
+		plen = __len->length &
+			((1 << CCS_DATA_LENGTH_SPECIFIER_SIZE_SHIFT) - 1);
+		break;
+	case CCS_DATA_LENGTH_SPECIFIER_2: {
+		struct __ccs_data_length_specifier2 *__len2 = (void *)__len;
+
+		if (!is_contained(__len2, endp))
+			return -ENODATA;
+
+		hlen = sizeof(*__len2);
+		plen = ((size_t)
+			(__len2->length[0] &
+			 ((1 << CCS_DATA_LENGTH_SPECIFIER_SIZE_SHIFT) - 1))
+			<< 8) + __len2->length[1];
+		break;
+	}
+	case CCS_DATA_LENGTH_SPECIFIER_3: {
+		struct __ccs_data_length_specifier3 *__len3 = (void *)__len;
+
+		if (!is_contained(__len3, endp))
+			return -ENODATA;
+
+		hlen = sizeof(*__len3);
+		plen = ((size_t)
+			(__len3->length[0] &
+			 ((1 << CCS_DATA_LENGTH_SPECIFIER_SIZE_SHIFT) - 1))
+			<< 16) + (__len3->length[0] << 8) + __len3->length[1];
+		break;
+	}
+	default:
+		return -EINVAL;
+	}
+
+	if (!has_headroom(__len, hlen + plen, endp))
+		return -ENODATA;
+
+	*__hlen = hlen;
+	*__plen = plen;
+
+	return 0;
+}
+
+static u8
+ccs_data_parse_format_version(const struct __ccs_data_block *block)
+{
+	return block->id >> CCS_DATA_BLOCK_HEADER_ID_VERSION_SHIFT;
+}
+
+static u8 ccs_data_parse_block_id(const struct __ccs_data_block *block,
+				       bool is_first)
+{
+	if (!is_first)
+		return block->id;
+
+	return block->id & ((1 << CCS_DATA_BLOCK_HEADER_ID_VERSION_SHIFT) - 1);
+}
+
+static int ccs_data_parse_version(struct bin_container *bin,
+				  struct ccs_data_container *ccsdata,
+				  const void *payload, const void *endp)
+{
+	const struct __ccs_data_block_version *v = payload;
+	struct ccs_data_block_version *vv;
+
+	if (v + 1 != endp)
+		return -ENODATA;
+
+	if (!bin->base) {
+		bin_reserve(bin, sizeof(*ccsdata->version));
+		return 0;
+	}
+
+	ccsdata->version = bin_alloc(bin, sizeof(*ccsdata->version));
+	if (!ccsdata->version)
+		return -ENOMEM;
+
+	vv = ccsdata->version;
+	vv->version_major = ((u16)v->static_data_version_major[0] << 8) +
+		v->static_data_version_major[1];
+	vv->version_minor = ((u16)v->static_data_version_minor[0] << 8) +
+		v->static_data_version_minor[1];
+	vv->date_year =  ((u16)v->year[0] << 8) + v->year[1];
+	vv->date_month = v->month;
+	vv->date_day = v->day;
+
+	return 0;
+}
+
+static void print_ccs_data_version(struct device *dev,
+				   struct ccs_data_block_version *v)
+{
+	dev_dbg(dev,
+		"static data version %4.4x.%4.4x, date %4.4u-%2.2u-%2.2u\n",
+		v->version_major, v->version_minor,
+		v->date_year, v->date_month, v->date_day);
+}
+
+static int ccs_data_block_parse_header(const struct __ccs_data_block *block,
+				       bool is_first, unsigned int *__block_id,
+				       const void **payload,
+				       const struct __ccs_data_block **next_block,
+				       const void *endp, struct device *dev,
+				       bool verbose)
+{
+	size_t plen, hlen;
+	u8 block_id;
+	int rval;
+
+	if (!is_contained(block, endp))
+		return -ENODATA;
+
+	rval = ccs_data_parse_length_specifier(&block->length, &hlen, &plen,
+					       endp);
+	if (rval < 0)
+		return rval;
+
+	block_id = ccs_data_parse_block_id(block, is_first);
+
+	if (verbose)
+		dev_dbg(dev,
+			"Block ID 0x%2.2x, header length %zu, payload length %zu\n",
+			block_id, hlen, plen);
+
+	if (!has_headroom(&block->length, hlen + plen, endp))
+		return -ENODATA;
+
+	if (__block_id)
+		*__block_id = block_id;
+
+	if (payload)
+		*payload = (void *)&block->length + hlen;
+
+	if (next_block)
+		*next_block = (void *)&block->length + hlen + plen;
+
+	return 0;
+}
+
+static int ccs_data_parse_regs(struct bin_container *bin,
+			       struct ccs_reg **__regs,
+			       size_t *__num_regs, const void *payload,
+			       const void *endp, struct device *dev)
+{
+	struct ccs_reg *regs_base = NULL, *regs = NULL;
+	size_t num_regs = 0;
+	u16 addr = 0;
+
+	if (bin->base && __regs) {
+		regs = regs_base = bin_alloc(bin, sizeof(*regs) * *__num_regs);
+		if (!regs)
+			return -ENOMEM;
+	}
+
+	while (payload < endp && num_regs < INT_MAX) {
+		const struct __ccs_data_block_regs *r = payload;
+		size_t len;
+		const void *data;
+
+		if (!is_contained(r, endp))
+			return -ENODATA;
+
+		switch (r->reg_len >> CCS_DATA_BLOCK_REGS_SEL_SHIFT) {
+		case CCS_DATA_BLOCK_REGS_SEL_REGS:
+			addr += r->reg_len & CCS_DATA_BLOCK_REGS_ADDR_MASK;
+			len = ((r->reg_len & CCS_DATA_BLOCK_REGS_LEN_MASK)
+			       >> CCS_DATA_BLOCK_REGS_LEN_SHIFT) + 1;
+
+			if (!is_contained_with_headroom(r, len, endp))
+				return -ENODATA;
+
+			data = r + 1;
+			break;
+		case CCS_DATA_BLOCK_REGS_SEL_REGS2: {
+			const struct __ccs_data_block_regs2 *r2 = payload;
+
+			if (!is_contained(r2, endp))
+				return -ENODATA;
+
+			addr += ((u16)(r2->reg_len &
+				       CCS_DATA_BLOCK_REGS_2_ADDR_MASK) << 8)
+				+ r2->addr;
+			len = ((r2->reg_len & CCS_DATA_BLOCK_REGS_2_LEN_MASK)
+			       >> CCS_DATA_BLOCK_REGS_2_LEN_SHIFT) + 1;
+
+			if (!is_contained_with_headroom(r2, len, endp))
+				return -ENODATA;
+
+			data = r2 + 1;
+			break;
+		}
+		case CCS_DATA_BLOCK_REGS_SEL_REGS3: {
+			const struct __ccs_data_block_regs3 *r3 = payload;
+
+			if (!is_contained(r3, endp))
+				return -ENODATA;
+
+			addr = ((u16)r3->addr[0] << 8) + r3->addr[1];
+			len = (r3->reg_len & CCS_DATA_BLOCK_REGS_3_LEN_MASK) + 1;
+
+			if (!is_contained_with_headroom(r3, len, endp))
+				return -ENODATA;
+
+			data = r3 + 1;
+			break;
+		}
+		default:
+			return -EINVAL;
+		}
+
+		num_regs++;
+
+		if (!bin->base) {
+			bin_reserve(bin, len);
+		} else if (__regs) {
+			if (!regs)
+				return -EIO;
+
+			regs->addr = addr;
+			regs->len = len;
+			regs->value = bin_alloc(bin, len);
+			if (!regs->value)
+				return -ENOMEM;
+
+			memcpy(regs->value, data, len);
+			regs++;
+		}
+
+		addr += len;
+		payload = data + len;
+	}
+
+	if (!bin->base)
+		bin_reserve(bin, sizeof(*regs) * num_regs);
+
+	if (__num_regs)
+		*__num_regs = num_regs;
+
+	if (bin->base && __regs) {
+		if (!regs_base)
+			return -EIO;
+
+		*__regs = regs_base;
+	}
+
+	return 0;
+}
+
+static int ccs_data_parse_reg_rules(struct bin_container *bin,
+				    struct ccs_reg **__regs,
+				    size_t *__num_regs,
+				    const void *payload,
+				    const void *endp, struct device *dev)
+{
+	int rval;
+
+	if (!bin->base)
+		return ccs_data_parse_regs(bin, NULL, NULL, payload, endp, dev);
+
+	rval = ccs_data_parse_regs(bin, NULL, __num_regs, payload, endp, dev);
+	if (rval)
+		return rval;
+
+	return ccs_data_parse_regs(bin, __regs, __num_regs, payload, endp,
+				   dev);
+}
+
+static void assign_ffd_entry(struct ccs_frame_format_desc *desc,
+			     const struct __ccs_data_block_ffd_entry *ent)
+{
+	desc->pixelcode = ent->pixelcode;
+	desc->value = ((u16)ent->value[0] << 8) + ent->value[1];
+}
+
+static int ccs_data_parse_ffd(struct bin_container *bin,
+			      struct ccs_frame_format_descs **ffd,
+			      const void *payload,
+			      const void *endp, struct device *dev)
+{
+	const struct __ccs_data_block_ffd *__ffd = payload;
+	const struct __ccs_data_block_ffd_entry *__entry;
+	unsigned int i;
+
+	if (!is_contained(__ffd, endp))
+		return -ENODATA;
+
+	if ((void *)__ffd + sizeof(*__ffd) +
+	    ((u32)__ffd->num_column_descs +
+	     (u32)__ffd->num_row_descs) *
+	    sizeof(struct __ccs_data_block_ffd_entry) != endp)
+		return -ENODATA;
+
+	if (!bin->base) {
+		bin_reserve(bin, sizeof(**ffd));
+		bin_reserve(bin, __ffd->num_column_descs *
+			    sizeof(struct ccs_frame_format_desc));
+		bin_reserve(bin, __ffd->num_row_descs *
+			    sizeof(struct ccs_frame_format_desc));
+
+		return 0;
+	}
+
+	*ffd = bin_alloc(bin, sizeof(**ffd));
+	if (!*ffd)
+		return -ENOMEM;
+
+	(*ffd)->num_column_descs = __ffd->num_column_descs;
+	(*ffd)->num_row_descs = __ffd->num_row_descs;
+	__entry = (void *)(__ffd + 1);
+
+	(*ffd)->column_descs = bin_alloc(bin, __ffd->num_column_descs *
+					 sizeof(*(*ffd)->column_descs));
+	if (!(*ffd)->column_descs)
+		return -ENOMEM;
+
+	for (i = 0; i < __ffd->num_column_descs; i++, __entry++)
+		assign_ffd_entry(&(*ffd)->column_descs[i], __entry);
+
+	(*ffd)->row_descs = bin_alloc(bin, __ffd->num_row_descs *
+				      sizeof(*(*ffd)->row_descs));
+	if (!(*ffd)->row_descs)
+		return -ENOMEM;
+
+	for (i = 0; i < __ffd->num_row_descs; i++, __entry++)
+		assign_ffd_entry(&(*ffd)->row_descs[i], __entry);
+
+	if (__entry != endp)
+		return -EPROTO;
+
+	return 0;
+}
+
+static int ccs_data_parse_pdaf_readout(struct bin_container *bin,
+				       struct ccs_pdaf_readout **pdaf_readout,
+				       const void *payload,
+				       const void *endp, struct device *dev)
+{
+	const struct __ccs_data_block_pdaf_readout *__pdaf = payload;
+
+	if (!is_contained(__pdaf, endp))
+		return -ENODATA;
+
+	if (!bin->base) {
+		bin_reserve(bin, sizeof(**pdaf_readout));
+	} else {
+		*pdaf_readout = bin_alloc(bin, sizeof(**pdaf_readout));
+		if (!*pdaf_readout)
+			return -ENOMEM;
+
+		(*pdaf_readout)->pdaf_readout_info_order =
+			__pdaf->pdaf_readout_info_order;
+	}
+
+	return ccs_data_parse_ffd(bin, !bin->base ? NULL : &(*pdaf_readout)->ffd,
+				  __pdaf + 1, endp, dev);
+}
+
+static int ccs_data_parse_rules(struct bin_container *bin,
+				struct ccs_rule **__rules,
+				size_t *__num_rules, const void *payload,
+				const void *endp, struct device *dev)
+{
+	struct ccs_rule *rules_base = NULL, *rules = NULL, *next_rule = NULL;
+	size_t num_rules = 0;
+	const void *__next_rule = payload;
+	int rval;
+
+	if (bin->base) {
+		rules_base = next_rule =
+			bin_alloc(bin, sizeof(*rules) * *__num_rules);
+		if (!rules_base)
+			return -ENOMEM;
+	}
+
+	while (__next_rule < endp) {
+		size_t rule_hlen, rule_plen, rule_plen2;
+		const u8 *__rule_type;
+		const void *rule_payload;
+
+		/* Size of a single rule */
+		rval = ccs_data_parse_length_specifier(__next_rule, &rule_hlen,
+						       &rule_plen, endp);
+
+		if (rval < 0)
+			return rval;
+
+		__rule_type = __next_rule + rule_hlen;
+
+		if (!is_contained(__rule_type, endp))
+			return -ENODATA;
+
+		rule_payload = __rule_type + 1;
+		rule_plen2 = rule_plen - sizeof(*__rule_type);
+
+		if (*__rule_type == CCS_DATA_BLOCK_RULE_ID_IF) {
+			const struct __ccs_data_block_rule_if *__if_rules =
+				rule_payload;
+			const size_t __num_if_rules =
+				rule_plen2 / sizeof(*__if_rules);
+			struct ccs_if_rule *if_rule;
+
+			if (!has_headroom(__if_rules,
+					  sizeof(*__if_rules) * __num_if_rules,
+					  rule_payload + rule_plen2))
+				return -ENODATA;
+
+			/* Also check there is no extra data */
+			if (__if_rules + __num_if_rules !=
+			    rule_payload + rule_plen2)
+				return -EINVAL;
+
+			if (!bin->base) {
+				bin_reserve(bin,
+					    sizeof(*if_rule) *
+					    __num_if_rules);
+				num_rules++;
+			} else {
+				unsigned int i;
+
+				if (!next_rule)
+					return -EIO;
+
+				rules = next_rule;
+				next_rule++;
+
+				if_rule = bin_alloc(bin,
+						    sizeof(*if_rule) *
+						    __num_if_rules);
+				if (!if_rule)
+					return -ENOMEM;
+
+				for (i = 0; i < __num_if_rules; i++) {
+					if_rule[i].addr =
+						((u16)__if_rules[i].addr[0]
+						 << 8) +
+						__if_rules[i].addr[1];
+					if_rule[i].value = __if_rules[i].value;
+					if_rule[i].mask = __if_rules[i].mask;
+				}
+
+				rules->if_rules = if_rule;
+				rules->num_if_rules = __num_if_rules;
+			}
+		} else {
+			/* Check there was an if rule before any other rules */
+			if (bin->base && !rules)
+				return -EINVAL;
+
+			switch (*__rule_type) {
+			case CCS_DATA_BLOCK_RULE_ID_READ_ONLY_REGS:
+				rval = ccs_data_parse_reg_rules(bin,
+								rules ?
+								&rules->read_only_regs : NULL,
+								rules ?
+								&rules->num_read_only_regs : NULL,
+								rule_payload,
+								rule_payload + rule_plen2,
+								dev);
+				if (rval)
+					return rval;
+				break;
+			case CCS_DATA_BLOCK_RULE_ID_FFD:
+				rval = ccs_data_parse_ffd(bin, rules ?
+							  &rules->frame_format : NULL,
+							  rule_payload,
+							  rule_payload + rule_plen2,
+							  dev);
+				if (rval)
+					return rval;
+				break;
+			case CCS_DATA_BLOCK_RULE_ID_MSR:
+				rval = ccs_data_parse_reg_rules(bin,
+								rules ?
+								&rules->manufacturer_regs : NULL,
+								rules ?
+								&rules->num_manufacturer_regs : NULL,
+								rule_payload,
+								rule_payload + rule_plen2,
+								dev);
+				if (rval)
+					return rval;
+				break;
+			case CCS_DATA_BLOCK_RULE_ID_PDAF_READOUT:
+				rval = ccs_data_parse_pdaf_readout(bin,
+								   rules ?
+								   &rules->pdaf_readout : NULL,
+								   rule_payload,
+								   rule_payload + rule_plen2,
+								   dev);
+				if (rval)
+					return rval;
+				break;
+			default:
+				dev_dbg(dev,
+					"Don't know how to handle rule type %u!\n",
+					*__rule_type);
+				return -EINVAL;
+			}
+		}
+		__next_rule = __next_rule + rule_hlen + rule_plen;
+	}
+
+	if (!bin->base) {
+		bin_reserve(bin, sizeof(*rules) * num_rules);
+		*__num_rules = num_rules;
+	} else {
+		if (!rules_base)
+			return -EIO;
+
+		*__rules = rules_base;
+	}
+
+	return 0;
+}
+
+static int ccs_data_parse_pdaf(struct bin_container *bin, struct ccs_pdaf_pix_loc **pdaf,
+			       const void *payload, const void *endp,
+			       struct device *dev)
+{
+	const struct __ccs_data_block_pdaf_pix_loc *__pdaf = payload;
+	const struct __ccs_data_block_pdaf_pix_loc_block_desc_group *__bdesc_group;
+	const struct __ccs_data_block_pdaf_pix_loc_pixel_desc *__pixel_desc;
+	unsigned int i;
+	u16 num_block_desc_groups;
+	u8 max_block_type_id = 0;
+	const u8 *__num_pixel_descs;
+
+	if (!is_contained(__pdaf, endp))
+		return -ENODATA;
+
+	if (bin->base) {
+		*pdaf = bin_alloc(bin, sizeof(**pdaf));
+		if (!*pdaf)
+			return -ENOMEM;
+	} else {
+		bin_reserve(bin, sizeof(**pdaf));
+	}
+
+	num_block_desc_groups =
+		((u16)__pdaf->num_block_desc_groups[0] << 8) +
+		__pdaf->num_block_desc_groups[1];
+
+	if (bin->base) {
+		(*pdaf)->main_offset_x =
+			((u16)__pdaf->main_offset_x[0] << 8) +
+			__pdaf->main_offset_x[1];
+		(*pdaf)->main_offset_y =
+			((u16)__pdaf->main_offset_y[0] << 8) +
+			__pdaf->main_offset_y[1];
+		(*pdaf)->global_pdaf_type = __pdaf->global_pdaf_type;
+		(*pdaf)->block_width = __pdaf->block_width;
+		(*pdaf)->block_height = __pdaf->block_height;
+		(*pdaf)->num_block_desc_groups = num_block_desc_groups;
+	}
+
+	__bdesc_group = (const void *)(__pdaf + 1);
+
+	if (bin->base) {
+		(*pdaf)->block_desc_groups =
+			bin_alloc(bin,
+				  sizeof(struct ccs_pdaf_pix_loc_block_desc_group) *
+				  num_block_desc_groups);
+		if (!(*pdaf)->block_desc_groups)
+			return -ENOMEM;
+	} else {
+		bin_reserve(bin, sizeof(struct ccs_pdaf_pix_loc_block_desc_group) *
+			    num_block_desc_groups);
+	}
+
+	for (i = 0; i < num_block_desc_groups; i++) {
+		const struct __ccs_data_block_pdaf_pix_loc_block_desc *__bdesc;
+		u16 num_block_descs;
+		unsigned int j;
+
+		if (!is_contained(__bdesc_group, endp))
+			return -ENODATA;
+
+		num_block_descs =
+			((u16)__bdesc_group->num_block_descs[0] << 8) +
+			__bdesc_group->num_block_descs[1];
+
+		if (bin->base) {
+			(*pdaf)->block_desc_groups[i].repeat_y =
+				__bdesc_group->repeat_y;
+			(*pdaf)->block_desc_groups[i].num_block_descs =
+				num_block_descs;
+		}
+
+		__bdesc = (const void *)(__bdesc_group + 1);
+
+		if (bin->base) {
+			(*pdaf)->block_desc_groups[i].block_descs =
+				bin_alloc(bin,
+					  sizeof(struct ccs_pdaf_pix_loc_block_desc) *
+					  num_block_descs);
+			if (!(*pdaf)->block_desc_groups[i].block_descs)
+				return -ENOMEM;
+		} else {
+			bin_reserve(bin, sizeof(struct ccs_pdaf_pix_loc_block_desc) *
+				    num_block_descs);
+		}
+
+		for (j = 0; j < num_block_descs; j++, __bdesc++) {
+			struct ccs_pdaf_pix_loc_block_desc *bdesc;
+
+			if (!is_contained(__bdesc, endp))
+				return -ENODATA;
+
+			if (max_block_type_id <= __bdesc->block_type_id)
+				max_block_type_id = __bdesc->block_type_id + 1;
+
+			if (!bin->base)
+				continue;
+
+			bdesc = &(*pdaf)->block_desc_groups[i].block_descs[j];
+
+			bdesc->repeat_x = ((u16)__bdesc->repeat_x[0] << 8)
+				+ __bdesc->repeat_x[1];
+
+			if (__bdesc->block_type_id >= num_block_descs)
+				return -EINVAL;
+
+			bdesc->block_type_id = __bdesc->block_type_id;
+		}
+
+		__bdesc_group = (const void *)__bdesc;
+	}
+
+	__num_pixel_descs = (const void *)__bdesc_group;
+
+	if (bin->base) {
+		(*pdaf)->pixel_desc_groups =
+			bin_alloc(bin,
+				  sizeof(struct ccs_pdaf_pix_loc_pixel_desc_group) *
+				  max_block_type_id);
+		if (!(*pdaf)->pixel_desc_groups)
+			return -ENOMEM;
+		(*pdaf)->num_pixel_desc_grups = max_block_type_id;
+	} else {
+		bin_reserve(bin, sizeof(struct ccs_pdaf_pix_loc_pixel_desc_group) *
+			    max_block_type_id);
+	}
+
+	for (i = 0; i < max_block_type_id; i++) {
+		struct ccs_pdaf_pix_loc_pixel_desc_group *pdgroup = NULL;
+		unsigned int j;
+
+		if (!is_contained(__num_pixel_descs, endp))
+			return -ENODATA;
+
+		if (bin->base) {
+			pdgroup = &(*pdaf)->pixel_desc_groups[i];
+			pdgroup->descs =
+				bin_alloc(bin,
+					  sizeof(struct ccs_pdaf_pix_loc_pixel_desc) *
+					  *__num_pixel_descs);
+			if (!pdgroup->descs)
+				return -ENOMEM;
+			pdgroup->num_descs = *__num_pixel_descs;
+		} else {
+			bin_reserve(bin, sizeof(struct ccs_pdaf_pix_loc_pixel_desc) *
+				    *__num_pixel_descs);
+		}
+
+		__pixel_desc = (const void *)(__num_pixel_descs + 1);
+
+		for (j = 0; j < *__num_pixel_descs; j++, __pixel_desc++) {
+			struct ccs_pdaf_pix_loc_pixel_desc *pdesc;
+
+			if (!is_contained(__pixel_desc, endp))
+				return -ENODATA;
+
+			if (!bin->base)
+				continue;
+
+			if (!pdgroup)
+				return -EIO;
+
+			pdesc = &pdgroup->descs[j];
+			pdesc->pixel_type = __pixel_desc->pixel_type;
+			pdesc->small_offset_x = __pixel_desc->small_offset_x;
+			pdesc->small_offset_y = __pixel_desc->small_offset_y;
+		}
+
+		__num_pixel_descs = (const void *)(__pixel_desc + 1);
+	}
+
+	return 0;
+}
+
+static int ccs_data_parse_license(struct bin_container *bin,
+				  char **__license,
+				  size_t *__license_length,
+				  const void *payload, const void *endp)
+{
+	size_t size = endp - payload;
+	char *license;
+
+	if (!bin->base) {
+		bin_reserve(bin, size);
+		return 0;
+	}
+
+	license = bin_alloc(bin, size);
+	if (!license)
+		return -ENOMEM;
+
+	memcpy(license, payload, size);
+
+	*__license = license;
+	*__license_length = size;
+
+	return 0;
+}
+
+static int ccs_data_parse_end(bool *end, const void *payload, const void *endp,
+			      struct device *dev)
+{
+	const struct __ccs_data_block_end *__end = payload;
+
+	if (__end + 1 != endp) {
+		dev_dbg(dev, "Invalid end block length %u\n",
+			(unsigned int)(endp - payload));
+		return -ENODATA;
+	}
+
+	*end = true;
+
+	return 0;
+}
+
+static int __ccs_data_parse(struct bin_container *bin,
+			    struct ccs_data_container *ccsdata,
+			    const void *data, size_t len, struct device *dev,
+			    bool verbose)
+{
+	const struct __ccs_data_block *block = data;
+	const struct __ccs_data_block *endp = data + len;
+	unsigned int version;
+	bool is_first = true;
+	int rval;
+
+	version = ccs_data_parse_format_version(block);
+	if (version != CCS_STATIC_DATA_VERSION) {
+		dev_dbg(dev, "Don't know how to handle version %u\n", version);
+		return -EINVAL;
+	}
+
+	if (verbose)
+		dev_dbg(dev, "Parsing CCS static data version %u\n", version);
+
+	if (!bin->base)
+		*ccsdata = (struct ccs_data_container){ 0 };
+
+	while (block < endp) {
+		const struct __ccs_data_block *next_block;
+		unsigned int block_id;
+		const void *payload;
+
+		rval = ccs_data_block_parse_header(block, is_first, &block_id,
+						   &payload, &next_block, endp,
+						   dev,
+						   bin->base ? false : verbose);
+
+		if (rval < 0)
+			return rval;
+
+		switch (block_id) {
+		case CCS_DATA_BLOCK_ID_DUMMY:
+			break;
+		case CCS_DATA_BLOCK_ID_DATA_VERSION:
+			rval = ccs_data_parse_version(bin, ccsdata, payload,
+						      next_block);
+			if (rval < 0)
+				return rval;
+			break;
+		case CCS_DATA_BLOCK_ID_SENSOR_READ_ONLY_REGS:
+			rval = ccs_data_parse_regs(
+				bin, &ccsdata->sensor_read_only_regs,
+				&ccsdata->num_sensor_read_only_regs, payload,
+				next_block, dev);
+			if (rval < 0)
+				return rval;
+			break;
+		case CCS_DATA_BLOCK_ID_SENSOR_MANUFACTURER_REGS:
+			rval = ccs_data_parse_regs(
+				bin, &ccsdata->sensor_manufacturer_regs,
+				&ccsdata->num_sensor_manufacturer_regs, payload,
+				next_block, dev);
+			if (rval < 0)
+				return rval;
+			break;
+		case CCS_DATA_BLOCK_ID_MODULE_READ_ONLY_REGS:
+			rval = ccs_data_parse_regs(
+				bin, &ccsdata->module_read_only_regs,
+				&ccsdata->num_module_read_only_regs, payload,
+				next_block, dev);
+			if (rval < 0)
+				return rval;
+			break;
+		case CCS_DATA_BLOCK_ID_MODULE_MANUFACTURER_REGS:
+			rval = ccs_data_parse_regs(
+				bin, &ccsdata->module_manufacturer_regs,
+				&ccsdata->num_module_manufacturer_regs, payload,
+				next_block, dev);
+			if (rval < 0)
+				return rval;
+			break;
+		case CCS_DATA_BLOCK_ID_SENSOR_PDAF_PIXEL_LOCATION:
+			rval = ccs_data_parse_pdaf(bin, &ccsdata->sensor_pdaf,
+						   payload, next_block, dev);
+			if (rval < 0)
+				return rval;
+			break;
+		case CCS_DATA_BLOCK_ID_MODULE_PDAF_PIXEL_LOCATION:
+			rval = ccs_data_parse_pdaf(bin, &ccsdata->module_pdaf,
+						   payload, next_block, dev);
+			if (rval < 0)
+				return rval;
+			break;
+		case CCS_DATA_BLOCK_ID_SENSOR_RULE_BASED_BLOCK:
+			rval = ccs_data_parse_rules(
+				bin, &ccsdata->sensor_rules,
+				&ccsdata->num_sensor_rules, payload, next_block,
+				dev);
+			if (rval < 0)
+				return rval;
+			break;
+		case CCS_DATA_BLOCK_ID_MODULE_RULE_BASED_BLOCK:
+			rval = ccs_data_parse_rules(
+				bin, &ccsdata->module_rules,
+				&ccsdata->num_module_rules, payload, next_block,
+				dev);
+			if (rval < 0)
+				return rval;
+			break;
+		case CCS_DATA_BLOCK_ID_LICENSE:
+			rval = ccs_data_parse_license(bin, &ccsdata->license,
+						      &ccsdata->license_length,
+						      payload, next_block);
+			if (rval < 0)
+				return rval;
+			break;
+		case CCS_DATA_BLOCK_ID_END:
+			rval = ccs_data_parse_end(&ccsdata->end, payload,
+						  next_block, dev);
+			if (rval < 0)
+				return rval;
+			break;
+		default:
+			dev_dbg(dev, "WARNING: not handling block ID 0x%2.2x\n",
+				block_id);
+		}
+
+		block = next_block;
+		is_first = false;
+	}
+
+	return 0;
+}
+
+/**
+ * ccs_data_parse - Parse a CCS static data file into a usable in-memory
+ *		    data structure
+ * @ccsdata:	CCS static data in-memory data structure
+ * @data:	CCS static data binary
+ * @len:	Length of @data
+ * @dev:	Device the data is related to (used for printing debug messages)
+ * @verbose:	Whether to be verbose or not
+ */
+int ccs_data_parse(struct ccs_data_container *ccsdata, const void *data,
+		   size_t len, struct device *dev, bool verbose)
+{
+	struct bin_container bin = { 0 };
+	int rval;
+
+	rval = __ccs_data_parse(&bin, ccsdata, data, len, dev, verbose);
+	if (rval)
+		return rval;
+
+	rval = bin_backing_alloc(&bin);
+	if (rval)
+		return rval;
+
+	rval = __ccs_data_parse(&bin, ccsdata, data, len, dev, false);
+	if (rval)
+		goto out_free;
+
+	if (verbose && ccsdata->version)
+		print_ccs_data_version(dev, ccsdata->version);
+
+	if (bin.now != bin.end) {
+		rval = -EPROTO;
+		dev_dbg(dev, "parsing mismatch; base %p; now %p; end %p\n",
+			bin.base, bin.now, bin.end);
+		goto out_free;
+	}
+
+	ccsdata->backing = bin.base;
+
+	return 0;
+
+out_free:
+	kvfree(bin.base);
+
+	return rval;
+}
diff --git a/drivers/media/i2c/ccs/ccs-data.h b/drivers/media/i2c/ccs/ccs-data.h
new file mode 100644
index 000000000..638df6980
--- /dev/null
+++ b/drivers/media/i2c/ccs/ccs-data.h
@@ -0,0 +1,230 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/*
+ * CCS static data in-memory data structure definitions
+ *
+ * Copyright 2019--2020 Intel Corporation
+ */
+
+#ifndef __CCS_DATA_H__
+#define __CCS_DATA_H__
+
+#include <linux/types.h>
+
+struct device;
+
+/**
+ * struct ccs_data_block_version - CCS static data version
+ * @version_major: Major version number
+ * @version_minor: Minor version number
+ * @date_year: Year
+ * @date_month: Month
+ * @date_day: Day
+ */
+struct ccs_data_block_version {
+	u16 version_major;
+	u16 version_minor;
+	u16 date_year;
+	u8 date_month;
+	u8 date_day;
+};
+
+/**
+ * struct ccs_reg - CCS register value
+ * @addr: The 16-bit address of the register
+ * @len: Length of the data
+ * @value: Data
+ */
+struct ccs_reg {
+	u16 addr;
+	u16 len;
+	u8 *value;
+};
+
+/**
+ * struct ccs_if_rule - CCS static data if rule
+ * @addr: Register address
+ * @value: Register value
+ * @mask: Value applied to both actual register value and @value
+ */
+struct ccs_if_rule {
+	u16 addr;
+	u8 value;
+	u8 mask;
+};
+
+/**
+ * struct ccs_frame_format_desc - CCS frame format descriptor
+ * @pixelcode: The pixelcode; CCS_DATA_BLOCK_FFD_PIXELCODE_*
+ * @value: Value related to the pixelcode
+ */
+struct ccs_frame_format_desc {
+	u8 pixelcode;
+	u16 value;
+};
+
+/**
+ * struct ccs_frame_format_descs - A series of CCS frame format descriptors
+ * @num_column_descs: Number of column descriptors
+ * @num_row_descs: Number of row descriptors
+ * @column_descs: Column descriptors
+ * @row_descs: Row descriptors
+ */
+struct ccs_frame_format_descs {
+	u8 num_column_descs;
+	u8 num_row_descs;
+	struct ccs_frame_format_desc *column_descs;
+	struct ccs_frame_format_desc *row_descs;
+};
+
+/**
+ * struct ccs_pdaf_readout - CCS PDAF data readout descriptor
+ * @pdaf_readout_info_order: PDAF readout order
+ * @ffd: Frame format of PDAF data
+ */
+struct ccs_pdaf_readout {
+	u8 pdaf_readout_info_order;
+	struct ccs_frame_format_descs *ffd;
+};
+
+/**
+ * struct ccs_rule - A CCS static data rule
+ * @num_if_rules: Number of if rules
+ * @if_rules: If rules
+ * @num_read_only_regs: Number of read-only registers
+ * @read_only_regs: Read-only registers
+ * @num_manufacturer_regs: Number of manufacturer-specific registers
+ * @manufacturer_regs: Manufacturer-specific registers
+ * @frame_format: Frame format
+ * @pdaf_readout: PDAF readout
+ */
+struct ccs_rule {
+	size_t num_if_rules;
+	struct ccs_if_rule *if_rules;
+	size_t num_read_only_regs;
+	struct ccs_reg *read_only_regs;
+	size_t num_manufacturer_regs;
+	struct ccs_reg *manufacturer_regs;
+	struct ccs_frame_format_descs *frame_format;
+	struct ccs_pdaf_readout *pdaf_readout;
+};
+
+/**
+ * struct ccs_pdaf_pix_loc_block_desc - PDAF pixel location block descriptor
+ * @block_type_id: Block type identifier, from 0 to n
+ * @repeat_x: Number of times this block is repeated to right
+ */
+struct ccs_pdaf_pix_loc_block_desc {
+	u8 block_type_id;
+	u16 repeat_x;
+};
+
+/**
+ * struct ccs_pdaf_pix_loc_block_desc_group - PDAF pixel location block
+ *					      descriptor group
+ * @repeat_y: Number of times the group is repeated down
+ * @num_block_descs: Number of block descriptors in @block_descs
+ * @block_descs: Block descriptors
+ */
+struct ccs_pdaf_pix_loc_block_desc_group {
+	u8 repeat_y;
+	u16 num_block_descs;
+	struct ccs_pdaf_pix_loc_block_desc *block_descs;
+};
+
+/**
+ * struct ccs_pdaf_pix_loc_pixel_desc - PDAF pixel location block descriptor
+ * @pixel_type: Type of the pixel; CCS_DATA_PDAF_PIXEL_TYPE_*
+ * @small_offset_x: offset X coordinate
+ * @small_offset_y: offset Y coordinate
+ */
+struct ccs_pdaf_pix_loc_pixel_desc {
+	u8 pixel_type;
+	u8 small_offset_x;
+	u8 small_offset_y;
+};
+
+/**
+ * struct ccs_pdaf_pix_loc_pixel_desc_group - PDAF pixel location pixel
+ *					      descriptor group
+ * @num_descs: Number of descriptors in @descs
+ * @descs: PDAF pixel location pixel descriptors
+ */
+struct ccs_pdaf_pix_loc_pixel_desc_group {
+	u8 num_descs;
+	struct ccs_pdaf_pix_loc_pixel_desc *descs;
+};
+
+/**
+ * struct ccs_pdaf_pix_loc - PDAF pixel locations
+ * @main_offset_x: Start X coordinate of PDAF pixel blocks
+ * @main_offset_y: Start Y coordinate of PDAF pixel blocks
+ * @global_pdaf_type: PDAF pattern type
+ * @block_width: Width of a block in pixels
+ * @block_height: Heigth of a block in pixels
+ * @num_block_desc_groups: Number of block descriptor groups
+ * @block_desc_groups: Block descriptor groups
+ * @num_pixel_desc_grups: Number of pixel descriptor groups
+ * @pixel_desc_groups: Pixel descriptor groups
+ */
+struct ccs_pdaf_pix_loc {
+	u16 main_offset_x;
+	u16 main_offset_y;
+	u8 global_pdaf_type;
+	u8 block_width;
+	u8 block_height;
+	u16 num_block_desc_groups;
+	struct ccs_pdaf_pix_loc_block_desc_group *block_desc_groups;
+	u8 num_pixel_desc_grups;
+	struct ccs_pdaf_pix_loc_pixel_desc_group *pixel_desc_groups;
+};
+
+/**
+ * struct ccs_data_container - In-memory CCS static data
+ * @version: CCS static data version
+ * @num_sensor_read_only_regs: Number of the read-only registers for the sensor
+ * @sensor_read_only_regs: Read-only registers for the sensor
+ * @num_sensor_manufacturer_regs: Number of the manufacturer-specific registers
+ *				  for the sensor
+ * @sensor_manufacturer_regs: Manufacturer-specific registers for the sensor
+ * @num_sensor_rules: Number of rules for the sensor
+ * @sensor_rules: Rules for the sensor
+ * @num_module_read_only_regs: Number of the read-only registers for the module
+ * @module_read_only_regs: Read-only registers for the module
+ * @num_module_manufacturer_regs: Number of the manufacturer-specific registers
+ *				  for the module
+ * @module_manufacturer_regs: Manufacturer-specific registers for the module
+ * @num_module_rules: Number of rules for the module
+ * @module_rules: Rules for the module
+ * @sensor_pdaf: PDAF data for the sensor
+ * @module_pdaf: PDAF data for the module
+ * @license_length: Lenght of the license data
+ * @license: License data
+ * @end: Whether or not there's an end block
+ * @backing: Raw data, pointed to from elsewhere so keep it around
+ */
+struct ccs_data_container {
+	struct ccs_data_block_version *version;
+	size_t num_sensor_read_only_regs;
+	struct ccs_reg *sensor_read_only_regs;
+	size_t num_sensor_manufacturer_regs;
+	struct ccs_reg *sensor_manufacturer_regs;
+	size_t num_sensor_rules;
+	struct ccs_rule *sensor_rules;
+	size_t num_module_read_only_regs;
+	struct ccs_reg *module_read_only_regs;
+	size_t num_module_manufacturer_regs;
+	struct ccs_reg *module_manufacturer_regs;
+	size_t num_module_rules;
+	struct ccs_rule *module_rules;
+	struct ccs_pdaf_pix_loc *sensor_pdaf;
+	struct ccs_pdaf_pix_loc *module_pdaf;
+	size_t license_length;
+	char *license;
+	bool end;
+	void *backing;
+};
+
+int ccs_data_parse(struct ccs_data_container *ccsdata, const void *data,
+		   size_t len, struct device *dev, bool verbose);
+
+#endif /* __CCS_DATA_H__ */
diff --git a/drivers/media/i2c/ccs/ccs-limits.c b/drivers/media/i2c/ccs/ccs-limits.c
new file mode 100644
index 000000000..4969fa425
--- /dev/null
+++ b/drivers/media/i2c/ccs/ccs-limits.c
@@ -0,0 +1,243 @@
+// SPDX-License-Identifier: GPL-2.0-only OR BSD-3-Clause
+/* Copyright (C) 2019--2020 Intel Corporation */
+/*
+ * Generated by Documentation/driver-api/media/drivers/ccs/mk-ccs-regs;
+ * do not modify.
+ */
+
+#include "ccs-limits.h"
+#include "ccs-regs.h"
+
+const struct ccs_limit ccs_limits[] = {
+	{ CCS_R_FRAME_FORMAT_MODEL_TYPE, 1, 0, "frame_format_model_type" },
+	{ CCS_R_FRAME_FORMAT_MODEL_SUBTYPE, 1, 0, "frame_format_model_subtype" },
+	{ CCS_R_FRAME_FORMAT_DESCRIPTOR(0), 30, 0, "frame_format_descriptor" },
+	{ CCS_R_FRAME_FORMAT_DESCRIPTOR_4(0), 32, 0, "frame_format_descriptor_4" },
+	{ CCS_R_ANALOG_GAIN_CAPABILITY, 2, 0, "analog_gain_capability" },
+	{ CCS_R_ANALOG_GAIN_CODE_MIN, 2, 0, "analog_gain_code_min" },
+	{ CCS_R_ANALOG_GAIN_CODE_MAX, 2, 0, "analog_gain_code_max" },
+	{ CCS_R_ANALOG_GAIN_CODE_STEP, 2, 0, "analog_gain_code_step" },
+	{ CCS_R_ANALOG_GAIN_TYPE, 2, 0, "analog_gain_type" },
+	{ CCS_R_ANALOG_GAIN_M0, 2, 0, "analog_gain_m0" },
+	{ CCS_R_ANALOG_GAIN_C0, 2, 0, "analog_gain_c0" },
+	{ CCS_R_ANALOG_GAIN_M1, 2, 0, "analog_gain_m1" },
+	{ CCS_R_ANALOG_GAIN_C1, 2, 0, "analog_gain_c1" },
+	{ CCS_R_ANALOG_LINEAR_GAIN_MIN, 2, 0, "analog_linear_gain_min" },
+	{ CCS_R_ANALOG_LINEAR_GAIN_MAX, 2, 0, "analog_linear_gain_max" },
+	{ CCS_R_ANALOG_LINEAR_GAIN_STEP_SIZE, 2, 0, "analog_linear_gain_step_size" },
+	{ CCS_R_ANALOG_EXPONENTIAL_GAIN_MIN, 2, 0, "analog_exponential_gain_min" },
+	{ CCS_R_ANALOG_EXPONENTIAL_GAIN_MAX, 2, 0, "analog_exponential_gain_max" },
+	{ CCS_R_ANALOG_EXPONENTIAL_GAIN_STEP_SIZE, 2, 0, "analog_exponential_gain_step_size" },
+	{ CCS_R_DATA_FORMAT_MODEL_TYPE, 1, 0, "data_format_model_type" },
+	{ CCS_R_DATA_FORMAT_MODEL_SUBTYPE, 1, 0, "data_format_model_subtype" },
+	{ CCS_R_DATA_FORMAT_DESCRIPTOR(0), 32, 0, "data_format_descriptor" },
+	{ CCS_R_INTEGRATION_TIME_CAPABILITY, 2, 0, "integration_time_capability" },
+	{ CCS_R_COARSE_INTEGRATION_TIME_MIN, 2, 0, "coarse_integration_time_min" },
+	{ CCS_R_COARSE_INTEGRATION_TIME_MAX_MARGIN, 2, 0, "coarse_integration_time_max_margin" },
+	{ CCS_R_FINE_INTEGRATION_TIME_MIN, 2, 0, "fine_integration_time_min" },
+	{ CCS_R_FINE_INTEGRATION_TIME_MAX_MARGIN, 2, 0, "fine_integration_time_max_margin" },
+	{ CCS_R_DIGITAL_GAIN_CAPABILITY, 1, 0, "digital_gain_capability" },
+	{ CCS_R_DIGITAL_GAIN_MIN, 2, 0, "digital_gain_min" },
+	{ CCS_R_DIGITAL_GAIN_MAX, 2, 0, "digital_gain_max" },
+	{ CCS_R_DIGITAL_GAIN_STEP_SIZE, 2, 0, "digital_gain_step_size" },
+	{ CCS_R_PEDESTAL_CAPABILITY, 1, 0, "Pedestal_capability" },
+	{ CCS_R_ADC_CAPABILITY, 1, 0, "ADC_capability" },
+	{ CCS_R_ADC_BIT_DEPTH_CAPABILITY, 4, 0, "ADC_bit_depth_capability" },
+	{ CCS_R_MIN_EXT_CLK_FREQ_MHZ, 4, 0, "min_ext_clk_freq_mhz" },
+	{ CCS_R_MAX_EXT_CLK_FREQ_MHZ, 4, 0, "max_ext_clk_freq_mhz" },
+	{ CCS_R_MIN_PRE_PLL_CLK_DIV, 2, 0, "min_pre_pll_clk_div" },
+	{ CCS_R_MAX_PRE_PLL_CLK_DIV, 2, 0, "max_pre_pll_clk_div" },
+	{ CCS_R_MIN_PLL_IP_CLK_FREQ_MHZ, 4, 0, "min_pll_ip_clk_freq_mhz" },
+	{ CCS_R_MAX_PLL_IP_CLK_FREQ_MHZ, 4, 0, "max_pll_ip_clk_freq_mhz" },
+	{ CCS_R_MIN_PLL_MULTIPLIER, 2, 0, "min_pll_multiplier" },
+	{ CCS_R_MAX_PLL_MULTIPLIER, 2, 0, "max_pll_multiplier" },
+	{ CCS_R_MIN_PLL_OP_CLK_FREQ_MHZ, 4, 0, "min_pll_op_clk_freq_mhz" },
+	{ CCS_R_MAX_PLL_OP_CLK_FREQ_MHZ, 4, 0, "max_pll_op_clk_freq_mhz" },
+	{ CCS_R_MIN_VT_SYS_CLK_DIV, 2, 0, "min_vt_sys_clk_div" },
+	{ CCS_R_MAX_VT_SYS_CLK_DIV, 2, 0, "max_vt_sys_clk_div" },
+	{ CCS_R_MIN_VT_SYS_CLK_FREQ_MHZ, 4, 0, "min_vt_sys_clk_freq_mhz" },
+	{ CCS_R_MAX_VT_SYS_CLK_FREQ_MHZ, 4, 0, "max_vt_sys_clk_freq_mhz" },
+	{ CCS_R_MIN_VT_PIX_CLK_FREQ_MHZ, 4, 0, "min_vt_pix_clk_freq_mhz" },
+	{ CCS_R_MAX_VT_PIX_CLK_FREQ_MHZ, 4, 0, "max_vt_pix_clk_freq_mhz" },
+	{ CCS_R_MIN_VT_PIX_CLK_DIV, 2, 0, "min_vt_pix_clk_div" },
+	{ CCS_R_MAX_VT_PIX_CLK_DIV, 2, 0, "max_vt_pix_clk_div" },
+	{ CCS_R_CLOCK_CALCULATION, 1, 0, "clock_calculation" },
+	{ CCS_R_NUM_OF_VT_LANES, 1, 0, "num_of_vt_lanes" },
+	{ CCS_R_NUM_OF_OP_LANES, 1, 0, "num_of_op_lanes" },
+	{ CCS_R_OP_BITS_PER_LANE, 1, 0, "op_bits_per_lane" },
+	{ CCS_R_MIN_FRAME_LENGTH_LINES, 2, 0, "min_frame_length_lines" },
+	{ CCS_R_MAX_FRAME_LENGTH_LINES, 2, 0, "max_frame_length_lines" },
+	{ CCS_R_MIN_LINE_LENGTH_PCK, 2, 0, "min_line_length_pck" },
+	{ CCS_R_MAX_LINE_LENGTH_PCK, 2, 0, "max_line_length_pck" },
+	{ CCS_R_MIN_LINE_BLANKING_PCK, 2, 0, "min_line_blanking_pck" },
+	{ CCS_R_MIN_FRAME_BLANKING_LINES, 2, 0, "min_frame_blanking_lines" },
+	{ CCS_R_MIN_LINE_LENGTH_PCK_STEP_SIZE, 1, 0, "min_line_length_pck_step_size" },
+	{ CCS_R_TIMING_MODE_CAPABILITY, 1, 0, "timing_mode_capability" },
+	{ CCS_R_FRAME_MARGIN_MAX_VALUE, 2, 0, "frame_margin_max_value" },
+	{ CCS_R_FRAME_MARGIN_MIN_VALUE, 1, 0, "frame_margin_min_value" },
+	{ CCS_R_GAIN_DELAY_TYPE, 1, 0, "gain_delay_type" },
+	{ CCS_R_MIN_OP_SYS_CLK_DIV, 2, 0, "min_op_sys_clk_div" },
+	{ CCS_R_MAX_OP_SYS_CLK_DIV, 2, 0, "max_op_sys_clk_div" },
+	{ CCS_R_MIN_OP_SYS_CLK_FREQ_MHZ, 4, 0, "min_op_sys_clk_freq_mhz" },
+	{ CCS_R_MAX_OP_SYS_CLK_FREQ_MHZ, 4, 0, "max_op_sys_clk_freq_mhz" },
+	{ CCS_R_MIN_OP_PIX_CLK_DIV, 2, 0, "min_op_pix_clk_div" },
+	{ CCS_R_MAX_OP_PIX_CLK_DIV, 2, 0, "max_op_pix_clk_div" },
+	{ CCS_R_MIN_OP_PIX_CLK_FREQ_MHZ, 4, 0, "min_op_pix_clk_freq_mhz" },
+	{ CCS_R_MAX_OP_PIX_CLK_FREQ_MHZ, 4, 0, "max_op_pix_clk_freq_mhz" },
+	{ CCS_R_X_ADDR_MIN, 2, 0, "x_addr_min" },
+	{ CCS_R_Y_ADDR_MIN, 2, 0, "y_addr_min" },
+	{ CCS_R_X_ADDR_MAX, 2, 0, "x_addr_max" },
+	{ CCS_R_Y_ADDR_MAX, 2, 0, "y_addr_max" },
+	{ CCS_R_MIN_X_OUTPUT_SIZE, 2, 0, "min_x_output_size" },
+	{ CCS_R_MIN_Y_OUTPUT_SIZE, 2, 0, "min_y_output_size" },
+	{ CCS_R_MAX_X_OUTPUT_SIZE, 2, 0, "max_x_output_size" },
+	{ CCS_R_MAX_Y_OUTPUT_SIZE, 2, 0, "max_y_output_size" },
+	{ CCS_R_X_ADDR_START_DIV_CONSTANT, 1, 0, "x_addr_start_div_constant" },
+	{ CCS_R_Y_ADDR_START_DIV_CONSTANT, 1, 0, "y_addr_start_div_constant" },
+	{ CCS_R_X_ADDR_END_DIV_CONSTANT, 1, 0, "x_addr_end_div_constant" },
+	{ CCS_R_Y_ADDR_END_DIV_CONSTANT, 1, 0, "y_addr_end_div_constant" },
+	{ CCS_R_X_SIZE_DIV, 1, 0, "x_size_div" },
+	{ CCS_R_Y_SIZE_DIV, 1, 0, "y_size_div" },
+	{ CCS_R_X_OUTPUT_DIV, 1, 0, "x_output_div" },
+	{ CCS_R_Y_OUTPUT_DIV, 1, 0, "y_output_div" },
+	{ CCS_R_NON_FLEXIBLE_RESOLUTION_SUPPORT, 1, 0, "non_flexible_resolution_support" },
+	{ CCS_R_MIN_OP_PRE_PLL_CLK_DIV, 2, 0, "min_op_pre_pll_clk_div" },
+	{ CCS_R_MAX_OP_PRE_PLL_CLK_DIV, 2, 0, "max_op_pre_pll_clk_div" },
+	{ CCS_R_MIN_OP_PLL_IP_CLK_FREQ_MHZ, 4, 0, "min_op_pll_ip_clk_freq_mhz" },
+	{ CCS_R_MAX_OP_PLL_IP_CLK_FREQ_MHZ, 4, 0, "max_op_pll_ip_clk_freq_mhz" },
+	{ CCS_R_MIN_OP_PLL_MULTIPLIER, 2, 0, "min_op_pll_multiplier" },
+	{ CCS_R_MAX_OP_PLL_MULTIPLIER, 2, 0, "max_op_pll_multiplier" },
+	{ CCS_R_MIN_OP_PLL_OP_CLK_FREQ_MHZ, 4, 0, "min_op_pll_op_clk_freq_mhz" },
+	{ CCS_R_MAX_OP_PLL_OP_CLK_FREQ_MHZ, 4, 0, "max_op_pll_op_clk_freq_mhz" },
+	{ CCS_R_CLOCK_TREE_PLL_CAPABILITY, 1, 0, "clock_tree_pll_capability" },
+	{ CCS_R_CLOCK_CAPA_TYPE_CAPABILITY, 1, 0, "clock_capa_type_capability" },
+	{ CCS_R_MIN_EVEN_INC, 2, 0, "min_even_inc" },
+	{ CCS_R_MIN_ODD_INC, 2, 0, "min_odd_inc" },
+	{ CCS_R_MAX_EVEN_INC, 2, 0, "max_even_inc" },
+	{ CCS_R_MAX_ODD_INC, 2, 0, "max_odd_inc" },
+	{ CCS_R_AUX_SUBSAMP_CAPABILITY, 1, 0, "aux_subsamp_capability" },
+	{ CCS_R_AUX_SUBSAMP_MONO_CAPABILITY, 1, 0, "aux_subsamp_mono_capability" },
+	{ CCS_R_MONOCHROME_CAPABILITY, 1, 0, "monochrome_capability" },
+	{ CCS_R_PIXEL_READOUT_CAPABILITY, 1, 0, "pixel_readout_capability" },
+	{ CCS_R_MIN_EVEN_INC_MONO, 2, 0, "min_even_inc_mono" },
+	{ CCS_R_MAX_EVEN_INC_MONO, 2, 0, "max_even_inc_mono" },
+	{ CCS_R_MIN_ODD_INC_MONO, 2, 0, "min_odd_inc_mono" },
+	{ CCS_R_MAX_ODD_INC_MONO, 2, 0, "max_odd_inc_mono" },
+	{ CCS_R_MIN_EVEN_INC_BC2, 2, 0, "min_even_inc_bc2" },
+	{ CCS_R_MAX_EVEN_INC_BC2, 2, 0, "max_even_inc_bc2" },
+	{ CCS_R_MIN_ODD_INC_BC2, 2, 0, "min_odd_inc_bc2" },
+	{ CCS_R_MAX_ODD_INC_BC2, 2, 0, "max_odd_inc_bc2" },
+	{ CCS_R_MIN_EVEN_INC_MONO_BC2, 2, 0, "min_even_inc_mono_bc2" },
+	{ CCS_R_MAX_EVEN_INC_MONO_BC2, 2, 0, "max_even_inc_mono_bc2" },
+	{ CCS_R_MIN_ODD_INC_MONO_BC2, 2, 0, "min_odd_inc_mono_bc2" },
+	{ CCS_R_MAX_ODD_INC_MONO_BC2, 2, 0, "max_odd_inc_mono_bc2" },
+	{ CCS_R_SCALING_CAPABILITY, 2, 0, "scaling_capability" },
+	{ CCS_R_SCALER_M_MIN, 2, 0, "scaler_m_min" },
+	{ CCS_R_SCALER_M_MAX, 2, 0, "scaler_m_max" },
+	{ CCS_R_SCALER_N_MIN, 2, 0, "scaler_n_min" },
+	{ CCS_R_SCALER_N_MAX, 2, 0, "scaler_n_max" },
+	{ CCS_R_DIGITAL_CROP_CAPABILITY, 1, 0, "digital_crop_capability" },
+	{ CCS_R_HDR_CAPABILITY_1, 1, 0, "hdr_capability_1" },
+	{ CCS_R_MIN_HDR_BIT_DEPTH, 1, 0, "min_hdr_bit_depth" },
+	{ CCS_R_HDR_RESOLUTION_SUB_TYPES, 1, 0, "hdr_resolution_sub_types" },
+	{ CCS_R_HDR_RESOLUTION_SUB_TYPE(0), 2, 0, "hdr_resolution_sub_type" },
+	{ CCS_R_HDR_CAPABILITY_2, 1, 0, "hdr_capability_2" },
+	{ CCS_R_MAX_HDR_BIT_DEPTH, 1, 0, "max_hdr_bit_depth" },
+	{ CCS_R_USL_SUPPORT_CAPABILITY, 1, 0, "usl_support_capability" },
+	{ CCS_R_USL_CLOCK_MODE_D_CAPABILITY, 1, 0, "usl_clock_mode_d_capability" },
+	{ CCS_R_MIN_OP_SYS_CLK_DIV_REV, 1, 0, "min_op_sys_clk_div_rev" },
+	{ CCS_R_MAX_OP_SYS_CLK_DIV_REV, 1, 0, "max_op_sys_clk_div_rev" },
+	{ CCS_R_MIN_OP_PIX_CLK_DIV_REV, 1, 0, "min_op_pix_clk_div_rev" },
+	{ CCS_R_MAX_OP_PIX_CLK_DIV_REV, 1, 0, "max_op_pix_clk_div_rev" },
+	{ CCS_R_MIN_OP_SYS_CLK_FREQ_REV_MHZ, 4, 0, "min_op_sys_clk_freq_rev_mhz" },
+	{ CCS_R_MAX_OP_SYS_CLK_FREQ_REV_MHZ, 4, 0, "max_op_sys_clk_freq_rev_mhz" },
+	{ CCS_R_MIN_OP_PIX_CLK_FREQ_REV_MHZ, 4, 0, "min_op_pix_clk_freq_rev_mhz" },
+	{ CCS_R_MAX_OP_PIX_CLK_FREQ_REV_MHZ, 4, 0, "max_op_pix_clk_freq_rev_mhz" },
+	{ CCS_R_MAX_BITRATE_REV_D_MODE_MBPS, 4, 0, "max_bitrate_rev_d_mode_mbps" },
+	{ CCS_R_MAX_SYMRATE_REV_C_MODE_MSPS, 4, 0, "max_symrate_rev_c_mode_msps" },
+	{ CCS_R_COMPRESSION_CAPABILITY, 1, 0, "compression_capability" },
+	{ CCS_R_TEST_MODE_CAPABILITY, 2, 0, "test_mode_capability" },
+	{ CCS_R_PN9_DATA_FORMAT1, 1, 0, "pn9_data_format1" },
+	{ CCS_R_PN9_DATA_FORMAT2, 1, 0, "pn9_data_format2" },
+	{ CCS_R_PN9_DATA_FORMAT3, 1, 0, "pn9_data_format3" },
+	{ CCS_R_PN9_DATA_FORMAT4, 1, 0, "pn9_data_format4" },
+	{ CCS_R_PN9_MISC_CAPABILITY, 1, 0, "pn9_misc_capability" },
+	{ CCS_R_TEST_PATTERN_CAPABILITY, 1, 0, "test_pattern_capability" },
+	{ CCS_R_PATTERN_SIZE_DIV_M1, 1, 0, "pattern_size_div_m1" },
+	{ CCS_R_FIFO_SUPPORT_CAPABILITY, 1, 0, "fifo_support_capability" },
+	{ CCS_R_PHY_CTRL_CAPABILITY, 1, 0, "phy_ctrl_capability" },
+	{ CCS_R_CSI_DPHY_LANE_MODE_CAPABILITY, 1, 0, "csi_dphy_lane_mode_capability" },
+	{ CCS_R_CSI_SIGNALING_MODE_CAPABILITY, 1, 0, "csi_signaling_mode_capability" },
+	{ CCS_R_FAST_STANDBY_CAPABILITY, 1, 0, "fast_standby_capability" },
+	{ CCS_R_CSI_ADDRESS_CONTROL_CAPABILITY, 1, 0, "csi_address_control_capability" },
+	{ CCS_R_DATA_TYPE_CAPABILITY, 1, 0, "data_type_capability" },
+	{ CCS_R_CSI_CPHY_LANE_MODE_CAPABILITY, 1, 0, "csi_cphy_lane_mode_capability" },
+	{ CCS_R_EMB_DATA_CAPABILITY, 1, 0, "emb_data_capability" },
+	{ CCS_R_MAX_PER_LANE_BITRATE_LANE_D_MODE_MBPS(0), 16, 0, "max_per_lane_bitrate_lane_d_mode_mbps 0" },
+	{ CCS_R_MAX_PER_LANE_BITRATE_LANE_D_MODE_MBPS(4), 16, CCS_L_FL_SAME_REG, "max_per_lane_bitrate_lane_d_mode_mbps 4" },
+	{ CCS_R_TEMP_SENSOR_CAPABILITY, 1, 0, "temp_sensor_capability" },
+	{ CCS_R_MAX_PER_LANE_BITRATE_LANE_C_MODE_MBPS(0), 16, 0, "max_per_lane_bitrate_lane_c_mode_mbps 0" },
+	{ CCS_R_MAX_PER_LANE_BITRATE_LANE_C_MODE_MBPS(4), 16, CCS_L_FL_SAME_REG, "max_per_lane_bitrate_lane_c_mode_mbps 4" },
+	{ CCS_R_DPHY_EQUALIZATION_CAPABILITY, 1, 0, "dphy_equalization_capability" },
+	{ CCS_R_CPHY_EQUALIZATION_CAPABILITY, 1, 0, "cphy_equalization_capability" },
+	{ CCS_R_DPHY_PREAMBLE_CAPABILITY, 1, 0, "dphy_preamble_capability" },
+	{ CCS_R_DPHY_SSC_CAPABILITY, 1, 0, "dphy_ssc_capability" },
+	{ CCS_R_CPHY_CALIBRATION_CAPABILITY, 1, 0, "cphy_calibration_capability" },
+	{ CCS_R_DPHY_CALIBRATION_CAPABILITY, 1, 0, "dphy_calibration_capability" },
+	{ CCS_R_PHY_CTRL_CAPABILITY_2, 1, 0, "phy_ctrl_capability_2" },
+	{ CCS_R_LRTE_CPHY_CAPABILITY, 1, 0, "lrte_cphy_capability" },
+	{ CCS_R_LRTE_DPHY_CAPABILITY, 1, 0, "lrte_dphy_capability" },
+	{ CCS_R_ALPS_CAPABILITY_DPHY, 1, 0, "alps_capability_dphy" },
+	{ CCS_R_ALPS_CAPABILITY_CPHY, 1, 0, "alps_capability_cphy" },
+	{ CCS_R_SCRAMBLING_CAPABILITY, 1, 0, "scrambling_capability" },
+	{ CCS_R_DPHY_MANUAL_CONSTANT, 1, 0, "dphy_manual_constant" },
+	{ CCS_R_CPHY_MANUAL_CONSTANT, 1, 0, "cphy_manual_constant" },
+	{ CCS_R_CSI2_INTERFACE_CAPABILITY_MISC, 1, 0, "CSI2_interface_capability_misc" },
+	{ CCS_R_PHY_CTRL_CAPABILITY_3, 1, 0, "PHY_ctrl_capability_3" },
+	{ CCS_R_DPHY_SF, 1, 0, "dphy_sf" },
+	{ CCS_R_CPHY_SF, 1, 0, "cphy_sf" },
+	{ CCS_R_DPHY_LIMITS_1, 1, 0, "dphy_limits_1" },
+	{ CCS_R_DPHY_LIMITS_2, 1, 0, "dphy_limits_2" },
+	{ CCS_R_DPHY_LIMITS_3, 1, 0, "dphy_limits_3" },
+	{ CCS_R_DPHY_LIMITS_4, 1, 0, "dphy_limits_4" },
+	{ CCS_R_DPHY_LIMITS_5, 1, 0, "dphy_limits_5" },
+	{ CCS_R_DPHY_LIMITS_6, 1, 0, "dphy_limits_6" },
+	{ CCS_R_CPHY_LIMITS_1, 1, 0, "cphy_limits_1" },
+	{ CCS_R_CPHY_LIMITS_2, 1, 0, "cphy_limits_2" },
+	{ CCS_R_CPHY_LIMITS_3, 1, 0, "cphy_limits_3" },
+	{ CCS_R_MIN_FRAME_LENGTH_LINES_BIN, 2, 0, "min_frame_length_lines_bin" },
+	{ CCS_R_MAX_FRAME_LENGTH_LINES_BIN, 2, 0, "max_frame_length_lines_bin" },
+	{ CCS_R_MIN_LINE_LENGTH_PCK_BIN, 2, 0, "min_line_length_pck_bin" },
+	{ CCS_R_MAX_LINE_LENGTH_PCK_BIN, 2, 0, "max_line_length_pck_bin" },
+	{ CCS_R_MIN_LINE_BLANKING_PCK_BIN, 2, 0, "min_line_blanking_pck_bin" },
+	{ CCS_R_FINE_INTEGRATION_TIME_MIN_BIN, 2, 0, "fine_integration_time_min_bin" },
+	{ CCS_R_FINE_INTEGRATION_TIME_MAX_MARGIN_BIN, 2, 0, "fine_integration_time_max_margin_bin" },
+	{ CCS_R_BINNING_CAPABILITY, 1, 0, "binning_capability" },
+	{ CCS_R_BINNING_WEIGHTING_CAPABILITY, 1, 0, "binning_weighting_capability" },
+	{ CCS_R_BINNING_SUB_TYPES, 1, 0, "binning_sub_types" },
+	{ CCS_R_BINNING_SUB_TYPE(0), 64, 0, "binning_sub_type" },
+	{ CCS_R_BINNING_WEIGHTING_MONO_CAPABILITY, 1, 0, "binning_weighting_mono_capability" },
+	{ CCS_R_BINNING_SUB_TYPES_MONO, 1, 0, "binning_sub_types_mono" },
+	{ CCS_R_BINNING_SUB_TYPE_MONO(0), 64, 0, "binning_sub_type_mono" },
+	{ CCS_R_DATA_TRANSFER_IF_CAPABILITY, 1, 0, "data_transfer_if_capability" },
+	{ CCS_R_SHADING_CORRECTION_CAPABILITY, 1, 0, "shading_correction_capability" },
+	{ CCS_R_GREEN_IMBALANCE_CAPABILITY, 1, 0, "green_imbalance_capability" },
+	{ CCS_R_MODULE_SPECIFIC_CORRECTION_CAPABILITY, 1, 0, "module_specific_correction_capability" },
+	{ CCS_R_DEFECT_CORRECTION_CAPABILITY, 2, 0, "defect_correction_capability" },
+	{ CCS_R_DEFECT_CORRECTION_CAPABILITY_2, 2, 0, "defect_correction_capability_2" },
+	{ CCS_R_NF_CAPABILITY, 1, 0, "nf_capability" },
+	{ CCS_R_OB_READOUT_CAPABILITY, 1, 0, "ob_readout_capability" },
+	{ CCS_R_COLOR_FEEDBACK_CAPABILITY, 1, 0, "color_feedback_capability" },
+	{ CCS_R_CFA_PATTERN_CAPABILITY, 1, 0, "CFA_pattern_capability" },
+	{ CCS_R_CFA_PATTERN_CONVERSION_CAPABILITY, 1, 0, "CFA_pattern_conversion_capability" },
+	{ CCS_R_FLASH_MODE_CAPABILITY, 1, 0, "flash_mode_capability" },
+	{ CCS_R_SA_STROBE_MODE_CAPABILITY, 1, 0, "sa_strobe_mode_capability" },
+	{ CCS_R_RESET_MAX_DELAY, 1, 0, "reset_max_delay" },
+	{ CCS_R_RESET_MIN_TIME, 1, 0, "reset_min_time" },
+	{ CCS_R_PDAF_CAPABILITY_1, 1, 0, "pdaf_capability_1" },
+	{ CCS_R_PDAF_CAPABILITY_2, 1, 0, "pdaf_capability_2" },
+	{ CCS_R_BRACKETING_LUT_CAPABILITY_1, 1, 0, "bracketing_lut_capability_1" },
+	{ CCS_R_BRACKETING_LUT_CAPABILITY_2, 1, 0, "bracketing_lut_capability_2" },
+	{ CCS_R_BRACKETING_LUT_SIZE, 1, 0, "bracketing_lut_size" },
+	{ 0 } /* Guardian */
+};
diff --git a/drivers/media/i2c/ccs/ccs-limits.h b/drivers/media/i2c/ccs/ccs-limits.h
new file mode 100644
index 000000000..551d3ee9d
--- /dev/null
+++ b/drivers/media/i2c/ccs/ccs-limits.h
@@ -0,0 +1,263 @@
+/* SPDX-License-Identifier: GPL-2.0-only OR BSD-3-Clause */
+/* Copyright (C) 2019--2020 Intel Corporation */
+/*
+ * Generated by Documentation/driver-api/media/drivers/ccs/mk-ccs-regs;
+ * do not modify.
+ */
+
+#ifndef __CCS_LIMITS_H__
+#define __CCS_LIMITS_H__
+
+#include <linux/bits.h>
+#include <linux/types.h>
+
+struct ccs_limit {
+	u32 reg;
+	u16 size;
+	u16 flags;
+	const char *name;
+};
+
+#define CCS_L_FL_SAME_REG	BIT(0)
+
+extern const struct ccs_limit ccs_limits[];
+
+#define CCS_L_FRAME_FORMAT_MODEL_TYPE				0
+#define CCS_L_FRAME_FORMAT_MODEL_SUBTYPE			1
+#define CCS_L_FRAME_FORMAT_DESCRIPTOR				2
+#define CCS_L_FRAME_FORMAT_DESCRIPTOR_OFFSET(n)			((n) * 2)
+#define CCS_L_FRAME_FORMAT_DESCRIPTOR_4				3
+#define CCS_L_FRAME_FORMAT_DESCRIPTOR_4_OFFSET(n)		((n) * 4)
+#define CCS_L_ANALOG_GAIN_CAPABILITY				4
+#define CCS_L_ANALOG_GAIN_CODE_MIN				5
+#define CCS_L_ANALOG_GAIN_CODE_MAX				6
+#define CCS_L_ANALOG_GAIN_CODE_STEP				7
+#define CCS_L_ANALOG_GAIN_TYPE					8
+#define CCS_L_ANALOG_GAIN_M0					9
+#define CCS_L_ANALOG_GAIN_C0					10
+#define CCS_L_ANALOG_GAIN_M1					11
+#define CCS_L_ANALOG_GAIN_C1					12
+#define CCS_L_ANALOG_LINEAR_GAIN_MIN				13
+#define CCS_L_ANALOG_LINEAR_GAIN_MAX				14
+#define CCS_L_ANALOG_LINEAR_GAIN_STEP_SIZE			15
+#define CCS_L_ANALOG_EXPONENTIAL_GAIN_MIN			16
+#define CCS_L_ANALOG_EXPONENTIAL_GAIN_MAX			17
+#define CCS_L_ANALOG_EXPONENTIAL_GAIN_STEP_SIZE			18
+#define CCS_L_DATA_FORMAT_MODEL_TYPE				19
+#define CCS_L_DATA_FORMAT_MODEL_SUBTYPE				20
+#define CCS_L_DATA_FORMAT_DESCRIPTOR				21
+#define CCS_L_DATA_FORMAT_DESCRIPTOR_OFFSET(n)			((n) * 2)
+#define CCS_L_INTEGRATION_TIME_CAPABILITY			22
+#define CCS_L_COARSE_INTEGRATION_TIME_MIN			23
+#define CCS_L_COARSE_INTEGRATION_TIME_MAX_MARGIN		24
+#define CCS_L_FINE_INTEGRATION_TIME_MIN				25
+#define CCS_L_FINE_INTEGRATION_TIME_MAX_MARGIN			26
+#define CCS_L_DIGITAL_GAIN_CAPABILITY				27
+#define CCS_L_DIGITAL_GAIN_MIN					28
+#define CCS_L_DIGITAL_GAIN_MAX					29
+#define CCS_L_DIGITAL_GAIN_STEP_SIZE				30
+#define CCS_L_PEDESTAL_CAPABILITY				31
+#define CCS_L_ADC_CAPABILITY					32
+#define CCS_L_ADC_BIT_DEPTH_CAPABILITY				33
+#define CCS_L_MIN_EXT_CLK_FREQ_MHZ				34
+#define CCS_L_MAX_EXT_CLK_FREQ_MHZ				35
+#define CCS_L_MIN_PRE_PLL_CLK_DIV				36
+#define CCS_L_MAX_PRE_PLL_CLK_DIV				37
+#define CCS_L_MIN_PLL_IP_CLK_FREQ_MHZ				38
+#define CCS_L_MAX_PLL_IP_CLK_FREQ_MHZ				39
+#define CCS_L_MIN_PLL_MULTIPLIER				40
+#define CCS_L_MAX_PLL_MULTIPLIER				41
+#define CCS_L_MIN_PLL_OP_CLK_FREQ_MHZ				42
+#define CCS_L_MAX_PLL_OP_CLK_FREQ_MHZ				43
+#define CCS_L_MIN_VT_SYS_CLK_DIV				44
+#define CCS_L_MAX_VT_SYS_CLK_DIV				45
+#define CCS_L_MIN_VT_SYS_CLK_FREQ_MHZ				46
+#define CCS_L_MAX_VT_SYS_CLK_FREQ_MHZ				47
+#define CCS_L_MIN_VT_PIX_CLK_FREQ_MHZ				48
+#define CCS_L_MAX_VT_PIX_CLK_FREQ_MHZ				49
+#define CCS_L_MIN_VT_PIX_CLK_DIV				50
+#define CCS_L_MAX_VT_PIX_CLK_DIV				51
+#define CCS_L_CLOCK_CALCULATION					52
+#define CCS_L_NUM_OF_VT_LANES					53
+#define CCS_L_NUM_OF_OP_LANES					54
+#define CCS_L_OP_BITS_PER_LANE					55
+#define CCS_L_MIN_FRAME_LENGTH_LINES				56
+#define CCS_L_MAX_FRAME_LENGTH_LINES				57
+#define CCS_L_MIN_LINE_LENGTH_PCK				58
+#define CCS_L_MAX_LINE_LENGTH_PCK				59
+#define CCS_L_MIN_LINE_BLANKING_PCK				60
+#define CCS_L_MIN_FRAME_BLANKING_LINES				61
+#define CCS_L_MIN_LINE_LENGTH_PCK_STEP_SIZE			62
+#define CCS_L_TIMING_MODE_CAPABILITY				63
+#define CCS_L_FRAME_MARGIN_MAX_VALUE				64
+#define CCS_L_FRAME_MARGIN_MIN_VALUE				65
+#define CCS_L_GAIN_DELAY_TYPE					66
+#define CCS_L_MIN_OP_SYS_CLK_DIV				67
+#define CCS_L_MAX_OP_SYS_CLK_DIV				68
+#define CCS_L_MIN_OP_SYS_CLK_FREQ_MHZ				69
+#define CCS_L_MAX_OP_SYS_CLK_FREQ_MHZ				70
+#define CCS_L_MIN_OP_PIX_CLK_DIV				71
+#define CCS_L_MAX_OP_PIX_CLK_DIV				72
+#define CCS_L_MIN_OP_PIX_CLK_FREQ_MHZ				73
+#define CCS_L_MAX_OP_PIX_CLK_FREQ_MHZ				74
+#define CCS_L_X_ADDR_MIN					75
+#define CCS_L_Y_ADDR_MIN					76
+#define CCS_L_X_ADDR_MAX					77
+#define CCS_L_Y_ADDR_MAX					78
+#define CCS_L_MIN_X_OUTPUT_SIZE					79
+#define CCS_L_MIN_Y_OUTPUT_SIZE					80
+#define CCS_L_MAX_X_OUTPUT_SIZE					81
+#define CCS_L_MAX_Y_OUTPUT_SIZE					82
+#define CCS_L_X_ADDR_START_DIV_CONSTANT				83
+#define CCS_L_Y_ADDR_START_DIV_CONSTANT				84
+#define CCS_L_X_ADDR_END_DIV_CONSTANT				85
+#define CCS_L_Y_ADDR_END_DIV_CONSTANT				86
+#define CCS_L_X_SIZE_DIV					87
+#define CCS_L_Y_SIZE_DIV					88
+#define CCS_L_X_OUTPUT_DIV					89
+#define CCS_L_Y_OUTPUT_DIV					90
+#define CCS_L_NON_FLEXIBLE_RESOLUTION_SUPPORT			91
+#define CCS_L_MIN_OP_PRE_PLL_CLK_DIV				92
+#define CCS_L_MAX_OP_PRE_PLL_CLK_DIV				93
+#define CCS_L_MIN_OP_PLL_IP_CLK_FREQ_MHZ			94
+#define CCS_L_MAX_OP_PLL_IP_CLK_FREQ_MHZ			95
+#define CCS_L_MIN_OP_PLL_MULTIPLIER				96
+#define CCS_L_MAX_OP_PLL_MULTIPLIER				97
+#define CCS_L_MIN_OP_PLL_OP_CLK_FREQ_MHZ			98
+#define CCS_L_MAX_OP_PLL_OP_CLK_FREQ_MHZ			99
+#define CCS_L_CLOCK_TREE_PLL_CAPABILITY				100
+#define CCS_L_CLOCK_CAPA_TYPE_CAPABILITY			101
+#define CCS_L_MIN_EVEN_INC					102
+#define CCS_L_MIN_ODD_INC					103
+#define CCS_L_MAX_EVEN_INC					104
+#define CCS_L_MAX_ODD_INC					105
+#define CCS_L_AUX_SUBSAMP_CAPABILITY				106
+#define CCS_L_AUX_SUBSAMP_MONO_CAPABILITY			107
+#define CCS_L_MONOCHROME_CAPABILITY				108
+#define CCS_L_PIXEL_READOUT_CAPABILITY				109
+#define CCS_L_MIN_EVEN_INC_MONO					110
+#define CCS_L_MAX_EVEN_INC_MONO					111
+#define CCS_L_MIN_ODD_INC_MONO					112
+#define CCS_L_MAX_ODD_INC_MONO					113
+#define CCS_L_MIN_EVEN_INC_BC2					114
+#define CCS_L_MAX_EVEN_INC_BC2					115
+#define CCS_L_MIN_ODD_INC_BC2					116
+#define CCS_L_MAX_ODD_INC_BC2					117
+#define CCS_L_MIN_EVEN_INC_MONO_BC2				118
+#define CCS_L_MAX_EVEN_INC_MONO_BC2				119
+#define CCS_L_MIN_ODD_INC_MONO_BC2				120
+#define CCS_L_MAX_ODD_INC_MONO_BC2				121
+#define CCS_L_SCALING_CAPABILITY				122
+#define CCS_L_SCALER_M_MIN					123
+#define CCS_L_SCALER_M_MAX					124
+#define CCS_L_SCALER_N_MIN					125
+#define CCS_L_SCALER_N_MAX					126
+#define CCS_L_DIGITAL_CROP_CAPABILITY				127
+#define CCS_L_HDR_CAPABILITY_1					128
+#define CCS_L_MIN_HDR_BIT_DEPTH					129
+#define CCS_L_HDR_RESOLUTION_SUB_TYPES				130
+#define CCS_L_HDR_RESOLUTION_SUB_TYPE				131
+#define CCS_L_HDR_RESOLUTION_SUB_TYPE_OFFSET(n)			(n)
+#define CCS_L_HDR_CAPABILITY_2					132
+#define CCS_L_MAX_HDR_BIT_DEPTH					133
+#define CCS_L_USL_SUPPORT_CAPABILITY				134
+#define CCS_L_USL_CLOCK_MODE_D_CAPABILITY			135
+#define CCS_L_MIN_OP_SYS_CLK_DIV_REV				136
+#define CCS_L_MAX_OP_SYS_CLK_DIV_REV				137
+#define CCS_L_MIN_OP_PIX_CLK_DIV_REV				138
+#define CCS_L_MAX_OP_PIX_CLK_DIV_REV				139
+#define CCS_L_MIN_OP_SYS_CLK_FREQ_REV_MHZ			140
+#define CCS_L_MAX_OP_SYS_CLK_FREQ_REV_MHZ			141
+#define CCS_L_MIN_OP_PIX_CLK_FREQ_REV_MHZ			142
+#define CCS_L_MAX_OP_PIX_CLK_FREQ_REV_MHZ			143
+#define CCS_L_MAX_BITRATE_REV_D_MODE_MBPS			144
+#define CCS_L_MAX_SYMRATE_REV_C_MODE_MSPS			145
+#define CCS_L_COMPRESSION_CAPABILITY				146
+#define CCS_L_TEST_MODE_CAPABILITY				147
+#define CCS_L_PN9_DATA_FORMAT1					148
+#define CCS_L_PN9_DATA_FORMAT2					149
+#define CCS_L_PN9_DATA_FORMAT3					150
+#define CCS_L_PN9_DATA_FORMAT4					151
+#define CCS_L_PN9_MISC_CAPABILITY				152
+#define CCS_L_TEST_PATTERN_CAPABILITY				153
+#define CCS_L_PATTERN_SIZE_DIV_M1				154
+#define CCS_L_FIFO_SUPPORT_CAPABILITY				155
+#define CCS_L_PHY_CTRL_CAPABILITY				156
+#define CCS_L_CSI_DPHY_LANE_MODE_CAPABILITY			157
+#define CCS_L_CSI_SIGNALING_MODE_CAPABILITY			158
+#define CCS_L_FAST_STANDBY_CAPABILITY				159
+#define CCS_L_CSI_ADDRESS_CONTROL_CAPABILITY			160
+#define CCS_L_DATA_TYPE_CAPABILITY				161
+#define CCS_L_CSI_CPHY_LANE_MODE_CAPABILITY			162
+#define CCS_L_EMB_DATA_CAPABILITY				163
+#define CCS_L_MAX_PER_LANE_BITRATE_LANE_D_MODE_MBPS		164
+#define CCS_L_MAX_PER_LANE_BITRATE_LANE_D_MODE_MBPS_OFFSET(n)	((n) * 4)
+#define CCS_L_TEMP_SENSOR_CAPABILITY				165
+#define CCS_L_MAX_PER_LANE_BITRATE_LANE_C_MODE_MBPS		166
+#define CCS_L_MAX_PER_LANE_BITRATE_LANE_C_MODE_MBPS_OFFSET(n)	((n) * 4)
+#define CCS_L_DPHY_EQUALIZATION_CAPABILITY			167
+#define CCS_L_CPHY_EQUALIZATION_CAPABILITY			168
+#define CCS_L_DPHY_PREAMBLE_CAPABILITY				169
+#define CCS_L_DPHY_SSC_CAPABILITY				170
+#define CCS_L_CPHY_CALIBRATION_CAPABILITY			171
+#define CCS_L_DPHY_CALIBRATION_CAPABILITY			172
+#define CCS_L_PHY_CTRL_CAPABILITY_2				173
+#define CCS_L_LRTE_CPHY_CAPABILITY				174
+#define CCS_L_LRTE_DPHY_CAPABILITY				175
+#define CCS_L_ALPS_CAPABILITY_DPHY				176
+#define CCS_L_ALPS_CAPABILITY_CPHY				177
+#define CCS_L_SCRAMBLING_CAPABILITY				178
+#define CCS_L_DPHY_MANUAL_CONSTANT				179
+#define CCS_L_CPHY_MANUAL_CONSTANT				180
+#define CCS_L_CSI2_INTERFACE_CAPABILITY_MISC			181
+#define CCS_L_PHY_CTRL_CAPABILITY_3				182
+#define CCS_L_DPHY_SF						183
+#define CCS_L_CPHY_SF						184
+#define CCS_L_DPHY_LIMITS_1					185
+#define CCS_L_DPHY_LIMITS_2					186
+#define CCS_L_DPHY_LIMITS_3					187
+#define CCS_L_DPHY_LIMITS_4					188
+#define CCS_L_DPHY_LIMITS_5					189
+#define CCS_L_DPHY_LIMITS_6					190
+#define CCS_L_CPHY_LIMITS_1					191
+#define CCS_L_CPHY_LIMITS_2					192
+#define CCS_L_CPHY_LIMITS_3					193
+#define CCS_L_MIN_FRAME_LENGTH_LINES_BIN			194
+#define CCS_L_MAX_FRAME_LENGTH_LINES_BIN			195
+#define CCS_L_MIN_LINE_LENGTH_PCK_BIN				196
+#define CCS_L_MAX_LINE_LENGTH_PCK_BIN				197
+#define CCS_L_MIN_LINE_BLANKING_PCK_BIN				198
+#define CCS_L_FINE_INTEGRATION_TIME_MIN_BIN			199
+#define CCS_L_FINE_INTEGRATION_TIME_MAX_MARGIN_BIN		200
+#define CCS_L_BINNING_CAPABILITY				201
+#define CCS_L_BINNING_WEIGHTING_CAPABILITY			202
+#define CCS_L_BINNING_SUB_TYPES					203
+#define CCS_L_BINNING_SUB_TYPE					204
+#define CCS_L_BINNING_SUB_TYPE_OFFSET(n)			(n)
+#define CCS_L_BINNING_WEIGHTING_MONO_CAPABILITY			205
+#define CCS_L_BINNING_SUB_TYPES_MONO				206
+#define CCS_L_BINNING_SUB_TYPE_MONO				207
+#define CCS_L_BINNING_SUB_TYPE_MONO_OFFSET(n)			(n)
+#define CCS_L_DATA_TRANSFER_IF_CAPABILITY			208
+#define CCS_L_SHADING_CORRECTION_CAPABILITY			209
+#define CCS_L_GREEN_IMBALANCE_CAPABILITY			210
+#define CCS_L_MODULE_SPECIFIC_CORRECTION_CAPABILITY		211
+#define CCS_L_DEFECT_CORRECTION_CAPABILITY			212
+#define CCS_L_DEFECT_CORRECTION_CAPABILITY_2			213
+#define CCS_L_NF_CAPABILITY					214
+#define CCS_L_OB_READOUT_CAPABILITY				215
+#define CCS_L_COLOR_FEEDBACK_CAPABILITY				216
+#define CCS_L_CFA_PATTERN_CAPABILITY				217
+#define CCS_L_CFA_PATTERN_CONVERSION_CAPABILITY			218
+#define CCS_L_FLASH_MODE_CAPABILITY				219
+#define CCS_L_SA_STROBE_MODE_CAPABILITY				220
+#define CCS_L_RESET_MAX_DELAY					221
+#define CCS_L_RESET_MIN_TIME					222
+#define CCS_L_PDAF_CAPABILITY_1					223
+#define CCS_L_PDAF_CAPABILITY_2					224
+#define CCS_L_BRACKETING_LUT_CAPABILITY_1			225
+#define CCS_L_BRACKETING_LUT_CAPABILITY_2			226
+#define CCS_L_BRACKETING_LUT_SIZE				227
+#define CCS_L_LAST						228
+
+#endif /* __CCS_LIMITS_H__ */
diff --git a/drivers/media/i2c/ccs/ccs-quirk.c b/drivers/media/i2c/ccs/ccs-quirk.c
new file mode 100644
index 000000000..e3d4c7a27
--- /dev/null
+++ b/drivers/media/i2c/ccs/ccs-quirk.c
@@ -0,0 +1,218 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * drivers/media/i2c/ccs/ccs-quirk.c
+ *
+ * Generic driver for MIPI CCS/SMIA/SMIA++ compliant camera sensors
+ *
+ * Copyright (C) 2020 Intel Corporation
+ * Copyright (C) 2011--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@linux.intel.com>
+ */
+
+#include <linux/delay.h>
+
+#include "ccs.h"
+#include "ccs-limits.h"
+
+static int ccs_write_addr_8s(struct ccs_sensor *sensor,
+			     const struct ccs_reg_8 *regs, int len)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	int rval;
+
+	for (; len > 0; len--, regs++) {
+		rval = ccs_write_addr(sensor, regs->reg, regs->val);
+		if (rval < 0) {
+			dev_err(&client->dev,
+				"error %d writing reg 0x%4.4x, val 0x%2.2x",
+				rval, regs->reg, regs->val);
+			return rval;
+		}
+	}
+
+	return 0;
+}
+
+static int jt8ew9_limits(struct ccs_sensor *sensor)
+{
+	if (sensor->minfo.revision_number < 0x0300)
+		sensor->frame_skip = 1;
+
+	/* Below 24 gain doesn't have effect at all, */
+	/* but ~59 is needed for full dynamic range */
+	ccs_replace_limit(sensor, CCS_L_ANALOG_GAIN_CODE_MIN, 0, 59);
+	ccs_replace_limit(sensor, CCS_L_ANALOG_GAIN_CODE_MAX, 0, 6000);
+
+	return 0;
+}
+
+static int jt8ew9_post_poweron(struct ccs_sensor *sensor)
+{
+	static const struct ccs_reg_8 regs[] = {
+		{ 0x30a3, 0xd8 }, /* Output port control : LVDS ports only */
+		{ 0x30ae, 0x00 }, /* 0x0307 pll_multiplier maximum value on PLL input 9.6MHz ( 19.2MHz is divided on pre_pll_div) */
+		{ 0x30af, 0xd0 }, /* 0x0307 pll_multiplier maximum value on PLL input 9.6MHz ( 19.2MHz is divided on pre_pll_div) */
+		{ 0x322d, 0x04 }, /* Adjusting Processing Image Size to Scaler Toshiba Recommendation Setting */
+		{ 0x3255, 0x0f }, /* Horizontal Noise Reduction Control Toshiba Recommendation Setting */
+		{ 0x3256, 0x15 }, /* Horizontal Noise Reduction Control Toshiba Recommendation Setting */
+		{ 0x3258, 0x70 }, /* Analog Gain Control Toshiba Recommendation Setting */
+		{ 0x3259, 0x70 }, /* Analog Gain Control Toshiba Recommendation Setting */
+		{ 0x325f, 0x7c }, /* Analog Gain Control Toshiba Recommendation Setting */
+		{ 0x3302, 0x06 }, /* Pixel Reference Voltage Control Toshiba Recommendation Setting */
+		{ 0x3304, 0x00 }, /* Pixel Reference Voltage Control Toshiba Recommendation Setting */
+		{ 0x3307, 0x22 }, /* Pixel Reference Voltage Control Toshiba Recommendation Setting */
+		{ 0x3308, 0x8d }, /* Pixel Reference Voltage Control Toshiba Recommendation Setting */
+		{ 0x331e, 0x0f }, /* Black Hole Sun Correction Control Toshiba Recommendation Setting */
+		{ 0x3320, 0x30 }, /* Black Hole Sun Correction Control Toshiba Recommendation Setting */
+		{ 0x3321, 0x11 }, /* Black Hole Sun Correction Control Toshiba Recommendation Setting */
+		{ 0x3322, 0x98 }, /* Black Hole Sun Correction Control Toshiba Recommendation Setting */
+		{ 0x3323, 0x64 }, /* Black Hole Sun Correction Control Toshiba Recommendation Setting */
+		{ 0x3325, 0x83 }, /* Read Out Timing Control Toshiba Recommendation Setting */
+		{ 0x3330, 0x18 }, /* Read Out Timing Control Toshiba Recommendation Setting */
+		{ 0x333c, 0x01 }, /* Read Out Timing Control Toshiba Recommendation Setting */
+		{ 0x3345, 0x2f }, /* Black Hole Sun Correction Control Toshiba Recommendation Setting */
+		{ 0x33de, 0x38 }, /* Horizontal Noise Reduction Control Toshiba Recommendation Setting */
+		/* Taken from v03. No idea what the rest are. */
+		{ 0x32e0, 0x05 },
+		{ 0x32e1, 0x05 },
+		{ 0x32e2, 0x04 },
+		{ 0x32e5, 0x04 },
+		{ 0x32e6, 0x04 },
+
+	};
+
+	return ccs_write_addr_8s(sensor, regs, ARRAY_SIZE(regs));
+}
+
+const struct ccs_quirk smiapp_jt8ew9_quirk = {
+	.limits = jt8ew9_limits,
+	.post_poweron = jt8ew9_post_poweron,
+};
+
+static int imx125es_post_poweron(struct ccs_sensor *sensor)
+{
+	/* Taken from v02. No idea what the other two are. */
+	static const struct ccs_reg_8 regs[] = {
+		/*
+		 * 0x3302: clk during frame blanking:
+		 * 0x00 - HS mode, 0x01 - LP11
+		 */
+		{ 0x3302, 0x01 },
+		{ 0x302d, 0x00 },
+		{ 0x3b08, 0x8c },
+	};
+
+	return ccs_write_addr_8s(sensor, regs, ARRAY_SIZE(regs));
+}
+
+const struct ccs_quirk smiapp_imx125es_quirk = {
+	.post_poweron = imx125es_post_poweron,
+};
+
+static int jt8ev1_limits(struct ccs_sensor *sensor)
+{
+	ccs_replace_limit(sensor, CCS_L_X_ADDR_MAX, 0, 4271);
+	ccs_replace_limit(sensor, CCS_L_MIN_LINE_BLANKING_PCK_BIN, 0, 184);
+
+	return 0;
+}
+
+static int jt8ev1_post_poweron(struct ccs_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	int rval;
+	static const struct ccs_reg_8 regs[] = {
+		{ 0x3031, 0xcd }, /* For digital binning (EQ_MONI) */
+		{ 0x30a3, 0xd0 }, /* FLASH STROBE enable */
+		{ 0x3237, 0x00 }, /* For control of pulse timing for ADC */
+		{ 0x3238, 0x43 },
+		{ 0x3301, 0x06 }, /* For analog bias for sensor */
+		{ 0x3302, 0x06 },
+		{ 0x3304, 0x00 },
+		{ 0x3305, 0x88 },
+		{ 0x332a, 0x14 },
+		{ 0x332c, 0x6b },
+		{ 0x3336, 0x01 },
+		{ 0x333f, 0x1f },
+		{ 0x3355, 0x00 },
+		{ 0x3356, 0x20 },
+		{ 0x33bf, 0x20 }, /* Adjust the FBC speed */
+		{ 0x33c9, 0x20 },
+		{ 0x33ce, 0x30 }, /* Adjust the parameter for logic function */
+		{ 0x33cf, 0xec }, /* For Black sun */
+		{ 0x3328, 0x80 }, /* Ugh. No idea what's this. */
+	};
+	static const struct ccs_reg_8 regs_96[] = {
+		{ 0x30ae, 0x00 }, /* For control of ADC clock */
+		{ 0x30af, 0xd0 },
+		{ 0x30b0, 0x01 },
+	};
+
+	rval = ccs_write_addr_8s(sensor, regs, ARRAY_SIZE(regs));
+	if (rval < 0)
+		return rval;
+
+	switch (sensor->hwcfg.ext_clk) {
+	case 9600000:
+		return ccs_write_addr_8s(sensor, regs_96,
+				       ARRAY_SIZE(regs_96));
+	default:
+		dev_warn(&client->dev, "no MSRs for %d Hz ext_clk\n",
+			 sensor->hwcfg.ext_clk);
+		return 0;
+	}
+}
+
+static int jt8ev1_pre_streamon(struct ccs_sensor *sensor)
+{
+	return ccs_write_addr(sensor, 0x3328, 0x00);
+}
+
+static int jt8ev1_post_streamoff(struct ccs_sensor *sensor)
+{
+	int rval;
+
+	/* Workaround: allows fast standby to work properly */
+	rval = ccs_write_addr(sensor, 0x3205, 0x04);
+	if (rval < 0)
+		return rval;
+
+	/* Wait for 1 ms + one line => 2 ms is likely enough */
+	usleep_range(2000, 2050);
+
+	/* Restore it */
+	rval = ccs_write_addr(sensor, 0x3205, 0x00);
+	if (rval < 0)
+		return rval;
+
+	return ccs_write_addr(sensor, 0x3328, 0x80);
+}
+
+static int jt8ev1_init(struct ccs_sensor *sensor)
+{
+	sensor->pll.flags |= CCS_PLL_FLAG_LANE_SPEED_MODEL |
+		CCS_PLL_FLAG_LINK_DECOUPLED;
+	sensor->pll.vt_lanes = 1;
+	sensor->pll.op_lanes = sensor->pll.csi2.lanes;
+
+	return 0;
+}
+
+const struct ccs_quirk smiapp_jt8ev1_quirk = {
+	.limits = jt8ev1_limits,
+	.post_poweron = jt8ev1_post_poweron,
+	.pre_streamon = jt8ev1_pre_streamon,
+	.post_streamoff = jt8ev1_post_streamoff,
+	.init = jt8ev1_init,
+};
+
+static int tcm8500md_limits(struct ccs_sensor *sensor)
+{
+	ccs_replace_limit(sensor, CCS_L_MIN_PLL_IP_CLK_FREQ_MHZ, 0, 2700000);
+
+	return 0;
+}
+
+const struct ccs_quirk smiapp_tcm8500md_quirk = {
+	.limits = tcm8500md_limits,
+};
diff --git a/drivers/media/i2c/ccs/ccs-quirk.h b/drivers/media/i2c/ccs/ccs-quirk.h
new file mode 100644
index 000000000..0b1a64958
--- /dev/null
+++ b/drivers/media/i2c/ccs/ccs-quirk.h
@@ -0,0 +1,79 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * drivers/media/i2c/ccs/ccs-quirk.h
+ *
+ * Generic driver for MIPI CCS/SMIA/SMIA++ compliant camera sensors
+ *
+ * Copyright (C) 2020 Intel Corporation
+ * Copyright (C) 2011--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@linux.intel.com>
+ */
+
+#ifndef __CCS_QUIRK__
+#define __CCS_QUIRK__
+
+struct ccs_sensor;
+
+/**
+ * struct ccs_quirk - quirks for sensors that deviate from SMIA++ standard
+ *
+ * @limits: Replace sensor->limits with values which can't be read from
+ *	    sensor registers. Called the first time the sensor is powered up.
+ * @post_poweron: Called always after the sensor has been fully powered on.
+ * @pre_streamon: Called just before streaming is enabled.
+ * @post_streamoff: Called right after stopping streaming.
+ * @pll_flags: Return flags for the PLL calculator.
+ * @init: Quirk initialisation, called the last in probe(). This is
+ *	  also appropriate for adding sensor specific controls, for instance.
+ * @reg_access: Register access quirk. The quirk may divert the access
+ *		to another register, or no register at all.
+ *
+ *		@write: Is this read (false) or write (true) access?
+ *		@reg: Pointer to the register to access
+ *		@value: Register value, set by the caller on write, or
+ *			by the quirk on read
+ *		@return: 0 on success, -ENOIOCTLCMD if no register
+ *			 access may be done by the caller (default read
+ *			 value is zero), else negative error code on error
+ * @flags: Quirk flags
+ */
+struct ccs_quirk {
+	int (*limits)(struct ccs_sensor *sensor);
+	int (*post_poweron)(struct ccs_sensor *sensor);
+	int (*pre_streamon)(struct ccs_sensor *sensor);
+	int (*post_streamoff)(struct ccs_sensor *sensor);
+	unsigned long (*pll_flags)(struct ccs_sensor *sensor);
+	int (*init)(struct ccs_sensor *sensor);
+	int (*reg_access)(struct ccs_sensor *sensor, bool write, u32 *reg,
+			  u32 *val);
+	unsigned long flags;
+};
+
+#define CCS_QUIRK_FLAG_8BIT_READ_ONLY			(1 << 0)
+
+struct ccs_reg_8 {
+	u16 reg;
+	u8 val;
+};
+
+#define CCS_MK_QUIRK_REG_8(_reg, _val) \
+	{				\
+		.reg = (u16)_reg,	\
+		.val = _val,		\
+	}
+
+#define ccs_call_quirk(sensor, _quirk, ...)				\
+	((sensor)->minfo.quirk &&					\
+	 (sensor)->minfo.quirk->_quirk ?				\
+	 (sensor)->minfo.quirk->_quirk(sensor, ##__VA_ARGS__) : 0)
+
+#define ccs_needs_quirk(sensor, _quirk)		\
+	((sensor)->minfo.quirk ?			\
+	 (sensor)->minfo.quirk->flags & _quirk : 0)
+
+extern const struct ccs_quirk smiapp_jt8ev1_quirk;
+extern const struct ccs_quirk smiapp_imx125es_quirk;
+extern const struct ccs_quirk smiapp_jt8ew9_quirk;
+extern const struct ccs_quirk smiapp_tcm8500md_quirk;
+
+#endif /* __CCS_QUIRK__ */
diff --git a/drivers/media/i2c/ccs/ccs-reg-access.c b/drivers/media/i2c/ccs/ccs-reg-access.c
new file mode 100644
index 000000000..25993445f
--- /dev/null
+++ b/drivers/media/i2c/ccs/ccs-reg-access.c
@@ -0,0 +1,416 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * drivers/media/i2c/ccs/ccs-reg-access.c
+ *
+ * Generic driver for MIPI CCS/SMIA/SMIA++ compliant camera sensors
+ *
+ * Copyright (C) 2020 Intel Corporation
+ * Copyright (C) 2011--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@linux.intel.com>
+ */
+
+#include <asm/unaligned.h>
+
+#include <linux/delay.h>
+#include <linux/i2c.h>
+
+#include "ccs.h"
+#include "ccs-limits.h"
+
+static u32 float_to_u32_mul_1000000(struct i2c_client *client, u32 phloat)
+{
+	s32 exp;
+	u64 man;
+
+	if (phloat >= 0x80000000) {
+		dev_err(&client->dev, "this is a negative number\n");
+		return 0;
+	}
+
+	if (phloat == 0x7f800000)
+		return ~0; /* Inf. */
+
+	if ((phloat & 0x7f800000) == 0x7f800000) {
+		dev_err(&client->dev, "NaN or other special number\n");
+		return 0;
+	}
+
+	/* Valid cases begin here */
+	if (phloat == 0)
+		return 0; /* Valid zero */
+
+	if (phloat > 0x4f800000)
+		return ~0; /* larger than 4294967295 */
+
+	/*
+	 * Unbias exponent (note how phloat is now guaranteed to
+	 * have 0 in the high bit)
+	 */
+	exp = ((int32_t)phloat >> 23) - 127;
+
+	/* Extract mantissa, add missing '1' bit and it's in MHz */
+	man = ((phloat & 0x7fffff) | 0x800000) * 1000000ULL;
+
+	if (exp < 0)
+		man >>= -exp;
+	else
+		man <<= exp;
+
+	man >>= 23; /* Remove mantissa bias */
+
+	return man & 0xffffffff;
+}
+
+
+/*
+ * Read a 8/16/32-bit i2c register.  The value is returned in 'val'.
+ * Returns zero if successful, or non-zero otherwise.
+ */
+static int ____ccs_read_addr(struct ccs_sensor *sensor, u16 reg, u16 len,
+			     u32 *val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	struct i2c_msg msg;
+	unsigned char data_buf[sizeof(u32)] = { 0 };
+	unsigned char offset_buf[sizeof(u16)];
+	int r;
+
+	if (len > sizeof(data_buf))
+		return -EINVAL;
+
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = sizeof(offset_buf);
+	msg.buf = offset_buf;
+	put_unaligned_be16(reg, offset_buf);
+
+	r = i2c_transfer(client->adapter, &msg, 1);
+	if (r != 1) {
+		if (r >= 0)
+			r = -EBUSY;
+		goto err;
+	}
+
+	msg.len = len;
+	msg.flags = I2C_M_RD;
+	msg.buf = &data_buf[sizeof(data_buf) - len];
+
+	r = i2c_transfer(client->adapter, &msg, 1);
+	if (r != 1) {
+		if (r >= 0)
+			r = -EBUSY;
+		goto err;
+	}
+
+	*val = get_unaligned_be32(data_buf);
+
+	return 0;
+
+err:
+	dev_err(&client->dev, "read from offset 0x%x error %d\n", reg, r);
+
+	return r;
+}
+
+/* Read a register using 8-bit access only. */
+static int ____ccs_read_addr_8only(struct ccs_sensor *sensor, u16 reg,
+				   u16 len, u32 *val)
+{
+	unsigned int i;
+	int rval;
+
+	*val = 0;
+
+	for (i = 0; i < len; i++) {
+		u32 val8;
+
+		rval = ____ccs_read_addr(sensor, reg + i, 1, &val8);
+		if (rval < 0)
+			return rval;
+		*val |= val8 << ((len - i - 1) << 3);
+	}
+
+	return 0;
+}
+
+unsigned int ccs_reg_width(u32 reg)
+{
+	if (reg & CCS_FL_16BIT)
+		return sizeof(u16);
+	if (reg & CCS_FL_32BIT)
+		return sizeof(u32);
+
+	return sizeof(u8);
+}
+
+static u32 ireal32_to_u32_mul_1000000(struct i2c_client *client, u32 val)
+{
+	if (val >> 10 > U32_MAX / 15625) {
+		dev_warn(&client->dev, "value %u overflows!\n", val);
+		return U32_MAX;
+	}
+
+	return ((val >> 10) * 15625) +
+		(val & GENMASK(9, 0)) * 15625 / 1024;
+}
+
+u32 ccs_reg_conv(struct ccs_sensor *sensor, u32 reg, u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+
+	if (reg & CCS_FL_FLOAT_IREAL) {
+		if (CCS_LIM(sensor, CLOCK_CAPA_TYPE_CAPABILITY) &
+		    CCS_CLOCK_CAPA_TYPE_CAPABILITY_IREAL)
+			val = ireal32_to_u32_mul_1000000(client, val);
+		else
+			val = float_to_u32_mul_1000000(client, val);
+	} else if (reg & CCS_FL_IREAL) {
+		val = ireal32_to_u32_mul_1000000(client, val);
+	}
+
+	return val;
+}
+
+/*
+ * Read a 8/16/32-bit i2c register.  The value is returned in 'val'.
+ * Returns zero if successful, or non-zero otherwise.
+ */
+static int __ccs_read_addr(struct ccs_sensor *sensor, u32 reg, u32 *val,
+			   bool only8, bool conv)
+{
+	unsigned int len = ccs_reg_width(reg);
+	int rval;
+
+	if (!only8)
+		rval = ____ccs_read_addr(sensor, CCS_REG_ADDR(reg), len, val);
+	else
+		rval = ____ccs_read_addr_8only(sensor, CCS_REG_ADDR(reg), len,
+					       val);
+	if (rval < 0)
+		return rval;
+
+	if (!conv)
+		return 0;
+
+	*val = ccs_reg_conv(sensor, reg, *val);
+
+	return 0;
+}
+
+static int __ccs_read_data(struct ccs_reg *regs, size_t num_regs,
+			   u32 reg, u32 *val)
+{
+	unsigned int width = ccs_reg_width(reg);
+	size_t i;
+
+	for (i = 0; i < num_regs; i++, regs++) {
+		u8 *data;
+
+		if (regs->addr + regs->len < CCS_REG_ADDR(reg) + width)
+			continue;
+
+		if (regs->addr > CCS_REG_ADDR(reg))
+			break;
+
+		data = &regs->value[CCS_REG_ADDR(reg) - regs->addr];
+
+		switch (width) {
+		case sizeof(u8):
+			*val = *data;
+			break;
+		case sizeof(u16):
+			*val = get_unaligned_be16(data);
+			break;
+		case sizeof(u32):
+			*val = get_unaligned_be32(data);
+			break;
+		default:
+			WARN_ON(1);
+			return -EINVAL;
+		}
+
+		return 0;
+	}
+
+	return -ENOENT;
+}
+
+static int ccs_read_data(struct ccs_sensor *sensor, u32 reg, u32 *val)
+{
+	if (!__ccs_read_data(sensor->sdata.sensor_read_only_regs,
+			     sensor->sdata.num_sensor_read_only_regs,
+			     reg, val))
+		return 0;
+
+	return __ccs_read_data(sensor->mdata.module_read_only_regs,
+			       sensor->mdata.num_module_read_only_regs,
+			       reg, val);
+}
+
+static int ccs_read_addr_raw(struct ccs_sensor *sensor, u32 reg, u32 *val,
+			     bool force8, bool quirk, bool conv, bool data)
+{
+	int rval;
+
+	if (data) {
+		rval = ccs_read_data(sensor, reg, val);
+		if (!rval)
+			return 0;
+	}
+
+	if (quirk) {
+		*val = 0;
+		rval = ccs_call_quirk(sensor, reg_access, false, &reg, val);
+		if (rval == -ENOIOCTLCMD)
+			return 0;
+		if (rval < 0)
+			return rval;
+
+		if (force8)
+			return __ccs_read_addr(sensor, reg, val, true, conv);
+	}
+
+	return __ccs_read_addr(sensor, reg, val,
+			       ccs_needs_quirk(sensor,
+					       CCS_QUIRK_FLAG_8BIT_READ_ONLY),
+			       conv);
+}
+
+int ccs_read_addr(struct ccs_sensor *sensor, u32 reg, u32 *val)
+{
+	return ccs_read_addr_raw(sensor, reg, val, false, true, true, true);
+}
+
+int ccs_read_addr_8only(struct ccs_sensor *sensor, u32 reg, u32 *val)
+{
+	return ccs_read_addr_raw(sensor, reg, val, true, true, true, true);
+}
+
+int ccs_read_addr_noconv(struct ccs_sensor *sensor, u32 reg, u32 *val)
+{
+	return ccs_read_addr_raw(sensor, reg, val, false, true, false, true);
+}
+
+static int ccs_write_retry(struct i2c_client *client, struct i2c_msg *msg)
+{
+	unsigned int retries;
+	int r;
+
+	for (retries = 0; retries < 10; retries++) {
+		/*
+		 * Due to unknown reason sensor stops responding. This
+		 * loop is a temporaty solution until the root cause
+		 * is found.
+		 */
+		r = i2c_transfer(client->adapter, msg, 1);
+		if (r != 1) {
+			usleep_range(1000, 2000);
+			continue;
+		}
+
+		if (retries)
+			dev_err(&client->dev,
+				"sensor i2c stall encountered. retries: %d\n",
+				retries);
+		return 0;
+	}
+
+	return r;
+}
+
+int ccs_write_addr_no_quirk(struct ccs_sensor *sensor, u32 reg, u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	struct i2c_msg msg;
+	unsigned char data[6];
+	unsigned int len = ccs_reg_width(reg);
+	int r;
+
+	if (len > sizeof(data) - 2)
+		return -EINVAL;
+
+	msg.addr = client->addr;
+	msg.flags = 0; /* Write */
+	msg.len = 2 + len;
+	msg.buf = data;
+
+	put_unaligned_be16(CCS_REG_ADDR(reg), data);
+	put_unaligned_be32(val << (8 * (sizeof(val) - len)), data + 2);
+
+	dev_dbg(&client->dev, "writing reg 0x%4.4x value 0x%*.*x (%u)\n",
+		CCS_REG_ADDR(reg), ccs_reg_width(reg) << 1,
+		ccs_reg_width(reg) << 1, val, val);
+
+	r = ccs_write_retry(client, &msg);
+	if (r)
+		dev_err(&client->dev,
+			"wrote 0x%x to offset 0x%x error %d\n", val,
+			CCS_REG_ADDR(reg), r);
+
+	return r;
+}
+
+/*
+ * Write to a 8/16-bit register.
+ * Returns zero if successful, or non-zero otherwise.
+ */
+int ccs_write_addr(struct ccs_sensor *sensor, u32 reg, u32 val)
+{
+	int rval;
+
+	rval = ccs_call_quirk(sensor, reg_access, true, &reg, &val);
+	if (rval == -ENOIOCTLCMD)
+		return 0;
+	if (rval < 0)
+		return rval;
+
+	return ccs_write_addr_no_quirk(sensor, reg, val);
+}
+
+#define MAX_WRITE_LEN	32U
+
+int ccs_write_data_regs(struct ccs_sensor *sensor, struct ccs_reg *regs,
+			size_t num_regs)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	unsigned char buf[2 + MAX_WRITE_LEN];
+	struct i2c_msg msg = {
+		.addr = client->addr,
+		.buf = buf,
+	};
+	size_t i;
+
+	for (i = 0; i < num_regs; i++, regs++) {
+		unsigned char *regdata = regs->value;
+		unsigned int j;
+
+		for (j = 0; j < regs->len;
+		     j += msg.len - 2, regdata += msg.len - 2) {
+			char printbuf[(MAX_WRITE_LEN << 1) +
+				      1 /* \0 */] = { 0 };
+			int rval;
+
+			msg.len = min(regs->len - j, MAX_WRITE_LEN);
+
+			bin2hex(printbuf, regdata, msg.len);
+			dev_dbg(&client->dev,
+				"writing msr reg 0x%4.4x value 0x%s\n",
+				regs->addr + j, printbuf);
+
+			put_unaligned_be16(regs->addr + j, buf);
+			memcpy(buf + 2, regdata, msg.len);
+
+			msg.len += 2;
+
+			rval = ccs_write_retry(client, &msg);
+			if (rval) {
+				dev_err(&client->dev,
+					"error writing %u octets to address 0x%4.4x\n",
+					msg.len, regs->addr + j);
+				return rval;
+			}
+		}
+	}
+
+	return 0;
+}
diff --git a/drivers/media/i2c/ccs/ccs-reg-access.h b/drivers/media/i2c/ccs/ccs-reg-access.h
new file mode 100644
index 000000000..78c43f92d
--- /dev/null
+++ b/drivers/media/i2c/ccs/ccs-reg-access.h
@@ -0,0 +1,42 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * include/media/ccs/ccs-reg-access.h
+ *
+ * Generic driver for MIPI CCS/SMIA/SMIA++ compliant camera sensors
+ *
+ * Copyright (C) 2020 Intel Corporation
+ * Copyright (C) 2011--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@linux.intel.com>
+ */
+
+#ifndef SMIAPP_REGS_H
+#define SMIAPP_REGS_H
+
+#include <linux/i2c.h>
+#include <linux/types.h>
+
+#include "ccs-regs.h"
+
+#define CCS_REG_ADDR(reg)		((u16)reg)
+
+struct ccs_sensor;
+
+int ccs_read_addr_no_quirk(struct ccs_sensor *sensor, u32 reg, u32 *val);
+int ccs_read_addr(struct ccs_sensor *sensor, u32 reg, u32 *val);
+int ccs_read_addr_8only(struct ccs_sensor *sensor, u32 reg, u32 *val);
+int ccs_read_addr_noconv(struct ccs_sensor *sensor, u32 reg, u32 *val);
+int ccs_write_addr_no_quirk(struct ccs_sensor *sensor, u32 reg, u32 val);
+int ccs_write_addr(struct ccs_sensor *sensor, u32 reg, u32 val);
+int ccs_write_data_regs(struct ccs_sensor *sensor, struct ccs_reg *regs,
+			size_t num_regs);
+
+unsigned int ccs_reg_width(u32 reg);
+u32 ccs_reg_conv(struct ccs_sensor *sensor, u32 reg, u32 val);
+
+#define ccs_read(sensor, reg_name, val) \
+	ccs_read_addr(sensor, CCS_R_##reg_name, val)
+
+#define ccs_write(sensor, reg_name, val) \
+	ccs_write_addr(sensor, CCS_R_##reg_name, val)
+
+#endif
diff --git a/drivers/media/i2c/ccs/ccs-regs.h b/drivers/media/i2c/ccs/ccs-regs.h
new file mode 100644
index 000000000..6ce84c5ec
--- /dev/null
+++ b/drivers/media/i2c/ccs/ccs-regs.h
@@ -0,0 +1,958 @@
+/* SPDX-License-Identifier: GPL-2.0-only OR BSD-3-Clause */
+/* Copyright (C) 2019--2020 Intel Corporation */
+/*
+ * Generated by Documentation/driver-api/media/drivers/ccs/mk-ccs-regs;
+ * do not modify.
+ */
+
+#ifndef __CCS_REGS_H__
+#define __CCS_REGS_H__
+
+#include <linux/bits.h>
+
+#define CCS_FL_BASE		16
+#define CCS_FL_16BIT		BIT(CCS_FL_BASE)
+#define CCS_FL_32BIT		BIT(CCS_FL_BASE + 1)
+#define CCS_FL_FLOAT_IREAL	BIT(CCS_FL_BASE + 2)
+#define CCS_FL_IREAL		BIT(CCS_FL_BASE + 3)
+#define CCS_R_ADDR(r)		((r) & 0xffff)
+
+#define CCS_R_MODULE_MODEL_ID					(0x0000 | CCS_FL_16BIT)
+#define CCS_R_MODULE_REVISION_NUMBER_MAJOR			0x0002
+#define CCS_R_FRAME_COUNT					0x0005
+#define CCS_R_PIXEL_ORDER					0x0006
+#define CCS_PIXEL_ORDER_GRBG					0U
+#define CCS_PIXEL_ORDER_RGGB					1U
+#define CCS_PIXEL_ORDER_BGGR					2U
+#define CCS_PIXEL_ORDER_GBRG					3U
+#define CCS_R_MIPI_CCS_VERSION					0x0007
+#define CCS_MIPI_CCS_VERSION_V1_0				0x10
+#define CCS_MIPI_CCS_VERSION_V1_1				0x11
+#define CCS_MIPI_CCS_VERSION_MAJOR_SHIFT			4U
+#define CCS_MIPI_CCS_VERSION_MAJOR_MASK				0xf0
+#define CCS_MIPI_CCS_VERSION_MINOR_SHIFT			0U
+#define CCS_MIPI_CCS_VERSION_MINOR_MASK				0xf
+#define CCS_R_DATA_PEDESTAL					(0x0008 | CCS_FL_16BIT)
+#define CCS_R_MODULE_MANUFACTURER_ID				(0x000e | CCS_FL_16BIT)
+#define CCS_R_MODULE_REVISION_NUMBER_MINOR			0x0010
+#define CCS_R_MODULE_DATE_YEAR					0x0012
+#define CCS_R_MODULE_DATE_MONTH					0x0013
+#define CCS_R_MODULE_DATE_DAY					0x0014
+#define CCS_R_MODULE_DATE_PHASE					0x0015
+#define CCS_MODULE_DATE_PHASE_SHIFT				0U
+#define CCS_MODULE_DATE_PHASE_MASK				0x7
+#define CCS_MODULE_DATE_PHASE_TS				0U
+#define CCS_MODULE_DATE_PHASE_ES				1U
+#define CCS_MODULE_DATE_PHASE_CS				2U
+#define CCS_MODULE_DATE_PHASE_MP				3U
+#define CCS_R_SENSOR_MODEL_ID					(0x0016 | CCS_FL_16BIT)
+#define CCS_R_SENSOR_REVISION_NUMBER				0x0018
+#define CCS_R_SENSOR_FIRMWARE_VERSION				0x001a
+#define CCS_R_SERIAL_NUMBER					(0x001c | CCS_FL_32BIT)
+#define CCS_R_SENSOR_MANUFACTURER_ID				(0x0020 | CCS_FL_16BIT)
+#define CCS_R_SENSOR_REVISION_NUMBER_16				(0x0022 | CCS_FL_16BIT)
+#define CCS_R_FRAME_FORMAT_MODEL_TYPE				0x0040
+#define CCS_FRAME_FORMAT_MODEL_TYPE_2_BYTE			1U
+#define CCS_FRAME_FORMAT_MODEL_TYPE_4_BYTE			2U
+#define CCS_R_FRAME_FORMAT_MODEL_SUBTYPE			0x0041
+#define CCS_FRAME_FORMAT_MODEL_SUBTYPE_ROWS_SHIFT		0U
+#define CCS_FRAME_FORMAT_MODEL_SUBTYPE_ROWS_MASK		0xf
+#define CCS_FRAME_FORMAT_MODEL_SUBTYPE_COLUMNS_SHIFT		4U
+#define CCS_FRAME_FORMAT_MODEL_SUBTYPE_COLUMNS_MASK		0xf0
+#define CCS_R_FRAME_FORMAT_DESCRIPTOR(n)			((0x0042 | CCS_FL_16BIT) + (n) * 2)
+#define CCS_LIM_FRAME_FORMAT_DESCRIPTOR_MIN_N			0U
+#define CCS_LIM_FRAME_FORMAT_DESCRIPTOR_MAX_N			14U
+#define CCS_R_FRAME_FORMAT_DESCRIPTOR_4(n)			((0x0060 | CCS_FL_32BIT) + (n) * 4)
+#define CCS_FRAME_FORMAT_DESCRIPTOR_PIXELS_SHIFT		0U
+#define CCS_FRAME_FORMAT_DESCRIPTOR_PIXELS_MASK			0xfff
+#define CCS_FRAME_FORMAT_DESCRIPTOR_PCODE_SHIFT			12U
+#define CCS_FRAME_FORMAT_DESCRIPTOR_PCODE_MASK			0xf000
+#define CCS_FRAME_FORMAT_DESCRIPTOR_PCODE_EMBEDDED		1U
+#define CCS_FRAME_FORMAT_DESCRIPTOR_PCODE_DUMMY_PIXEL		2U
+#define CCS_FRAME_FORMAT_DESCRIPTOR_PCODE_BLACK_PIXEL		3U
+#define CCS_FRAME_FORMAT_DESCRIPTOR_PCODE_DARK_PIXEL		4U
+#define CCS_FRAME_FORMAT_DESCRIPTOR_PCODE_VISIBLE_PIXEL		5U
+#define CCS_FRAME_FORMAT_DESCRIPTOR_PCODE_MANUF_SPECIFIC_0	8U
+#define CCS_FRAME_FORMAT_DESCRIPTOR_PCODE_MANUF_SPECIFIC_1	9U
+#define CCS_FRAME_FORMAT_DESCRIPTOR_PCODE_MANUF_SPECIFIC_2	10U
+#define CCS_FRAME_FORMAT_DESCRIPTOR_PCODE_MANUF_SPECIFIC_3	11U
+#define CCS_FRAME_FORMAT_DESCRIPTOR_PCODE_MANUF_SPECIFIC_4	12U
+#define CCS_FRAME_FORMAT_DESCRIPTOR_PCODE_MANUF_SPECIFIC_5	13U
+#define CCS_FRAME_FORMAT_DESCRIPTOR_PCODE_MANUF_SPECIFIC_6	14U
+#define CCS_LIM_FRAME_FORMAT_DESCRIPTOR_4_MIN_N			0U
+#define CCS_LIM_FRAME_FORMAT_DESCRIPTOR_4_MAX_N			7U
+#define CCS_FRAME_FORMAT_DESCRIPTOR_4_PIXELS_SHIFT		0U
+#define CCS_FRAME_FORMAT_DESCRIPTOR_4_PIXELS_MASK		0xffff
+#define CCS_FRAME_FORMAT_DESCRIPTOR_4_PCODE_SHIFT		28U
+#define CCS_FRAME_FORMAT_DESCRIPTOR_4_PCODE_MASK		0xf0000000
+#define CCS_FRAME_FORMAT_DESCRIPTOR_4_PCODE_EMBEDDED		1U
+#define CCS_FRAME_FORMAT_DESCRIPTOR_4_PCODE_DUMMY_PIXEL		2U
+#define CCS_FRAME_FORMAT_DESCRIPTOR_4_PCODE_BLACK_PIXEL		3U
+#define CCS_FRAME_FORMAT_DESCRIPTOR_4_PCODE_DARK_PIXEL		4U
+#define CCS_FRAME_FORMAT_DESCRIPTOR_4_PCODE_VISIBLE_PIXEL	5U
+#define CCS_FRAME_FORMAT_DESCRIPTOR_4_PCODE_MANUF_SPECIFIC_0	8U
+#define CCS_FRAME_FORMAT_DESCRIPTOR_4_PCODE_MANUF_SPECIFIC_1	9U
+#define CCS_FRAME_FORMAT_DESCRIPTOR_4_PCODE_MANUF_SPECIFIC_2	10U
+#define CCS_FRAME_FORMAT_DESCRIPTOR_4_PCODE_MANUF_SPECIFIC_3	11U
+#define CCS_FRAME_FORMAT_DESCRIPTOR_4_PCODE_MANUF_SPECIFIC_4	12U
+#define CCS_FRAME_FORMAT_DESCRIPTOR_4_PCODE_MANUF_SPECIFIC_5	13U
+#define CCS_FRAME_FORMAT_DESCRIPTOR_4_PCODE_MANUF_SPECIFIC_6	14U
+#define CCS_R_ANALOG_GAIN_CAPABILITY				(0x0080 | CCS_FL_16BIT)
+#define CCS_ANALOG_GAIN_CAPABILITY_GLOBAL			0U
+#define CCS_ANALOG_GAIN_CAPABILITY_ALTERNATE_GLOBAL		2U
+#define CCS_R_ANALOG_GAIN_CODE_MIN				(0x0084 | CCS_FL_16BIT)
+#define CCS_R_ANALOG_GAIN_CODE_MAX				(0x0086 | CCS_FL_16BIT)
+#define CCS_R_ANALOG_GAIN_CODE_STEP				(0x0088 | CCS_FL_16BIT)
+#define CCS_R_ANALOG_GAIN_TYPE					(0x008a | CCS_FL_16BIT)
+#define CCS_R_ANALOG_GAIN_M0					(0x008c | CCS_FL_16BIT)
+#define CCS_R_ANALOG_GAIN_C0					(0x008e | CCS_FL_16BIT)
+#define CCS_R_ANALOG_GAIN_M1					(0x0090 | CCS_FL_16BIT)
+#define CCS_R_ANALOG_GAIN_C1					(0x0092 | CCS_FL_16BIT)
+#define CCS_R_ANALOG_LINEAR_GAIN_MIN				(0x0094 | CCS_FL_16BIT)
+#define CCS_R_ANALOG_LINEAR_GAIN_MAX				(0x0096 | CCS_FL_16BIT)
+#define CCS_R_ANALOG_LINEAR_GAIN_STEP_SIZE			(0x0098 | CCS_FL_16BIT)
+#define CCS_R_ANALOG_EXPONENTIAL_GAIN_MIN			(0x009a | CCS_FL_16BIT)
+#define CCS_R_ANALOG_EXPONENTIAL_GAIN_MAX			(0x009c | CCS_FL_16BIT)
+#define CCS_R_ANALOG_EXPONENTIAL_GAIN_STEP_SIZE			(0x009e | CCS_FL_16BIT)
+#define CCS_R_DATA_FORMAT_MODEL_TYPE				0x00c0
+#define CCS_DATA_FORMAT_MODEL_TYPE_NORMAL			1U
+#define CCS_DATA_FORMAT_MODEL_TYPE_EXTENDED			2U
+#define CCS_R_DATA_FORMAT_MODEL_SUBTYPE				0x00c1
+#define CCS_DATA_FORMAT_MODEL_SUBTYPE_ROWS_SHIFT		0U
+#define CCS_DATA_FORMAT_MODEL_SUBTYPE_ROWS_MASK			0xf
+#define CCS_DATA_FORMAT_MODEL_SUBTYPE_COLUMNS_SHIFT		4U
+#define CCS_DATA_FORMAT_MODEL_SUBTYPE_COLUMNS_MASK		0xf0
+#define CCS_R_DATA_FORMAT_DESCRIPTOR(n)				((0x00c2 | CCS_FL_16BIT) + (n) * 2)
+#define CCS_LIM_DATA_FORMAT_DESCRIPTOR_MIN_N			0U
+#define CCS_LIM_DATA_FORMAT_DESCRIPTOR_MAX_N			15U
+#define CCS_DATA_FORMAT_DESCRIPTOR_COMPRESSED_SHIFT		0U
+#define CCS_DATA_FORMAT_DESCRIPTOR_COMPRESSED_MASK		0xff
+#define CCS_DATA_FORMAT_DESCRIPTOR_UNCOMPRESSED_SHIFT		8U
+#define CCS_DATA_FORMAT_DESCRIPTOR_UNCOMPRESSED_MASK		0xff00
+#define CCS_R_MODE_SELECT					0x0100
+#define CCS_MODE_SELECT_SOFTWARE_STANDBY			0U
+#define CCS_MODE_SELECT_STREAMING				1U
+#define CCS_R_IMAGE_ORIENTATION					0x0101
+#define CCS_IMAGE_ORIENTATION_HORIZONTAL_MIRROR			BIT(0)
+#define CCS_IMAGE_ORIENTATION_VERTICAL_FLIP			BIT(1)
+#define CCS_R_SOFTWARE_RESET					0x0103
+#define CCS_SOFTWARE_RESET_OFF					0U
+#define CCS_SOFTWARE_RESET_ON					1U
+#define CCS_R_GROUPED_PARAMETER_HOLD				0x0104
+#define CCS_R_MASK_CORRUPTED_FRAMES				0x0105
+#define CCS_MASK_CORRUPTED_FRAMES_ALLOW				0U
+#define CCS_MASK_CORRUPTED_FRAMES_MASK				1U
+#define CCS_R_FAST_STANDBY_CTRL					0x0106
+#define CCS_FAST_STANDBY_CTRL_COMPLETE_FRAMES			0U
+#define CCS_FAST_STANDBY_CTRL_FRAME_TRUNCATION			1U
+#define CCS_R_CCI_ADDRESS_CTRL					0x0107
+#define CCS_R_2ND_CCI_IF_CTRL					0x0108
+#define CCS_2ND_CCI_IF_CTRL_ENABLE				BIT(0)
+#define CCS_2ND_CCI_IF_CTRL_ACK					BIT(1)
+#define CCS_R_2ND_CCI_ADDRESS_CTRL				0x0109
+#define CCS_R_CSI_CHANNEL_IDENTIFIER				0x0110
+#define CCS_R_CSI_SIGNALING_MODE				0x0111
+#define CCS_CSI_SIGNALING_MODE_CSI_2_DPHY			2U
+#define CCS_CSI_SIGNALING_MODE_CSI_2_CPHY			3U
+#define CCS_R_CSI_DATA_FORMAT					(0x0112 | CCS_FL_16BIT)
+#define CCS_R_CSI_LANE_MODE					0x0114
+#define CCS_R_DPCM_FRAME_DT					0x011d
+#define CCS_R_BOTTOM_EMBEDDED_DATA_DT				0x011e
+#define CCS_R_BOTTOM_EMBEDDED_DATA_VC				0x011f
+#define CCS_R_GAIN_MODE						0x0120
+#define CCS_GAIN_MODE_GLOBAL					0U
+#define CCS_GAIN_MODE_ALTERNATE					1U
+#define CCS_R_ADC_BIT_DEPTH					0x0121
+#define CCS_R_EMB_DATA_CTRL					0x0122
+#define CCS_EMB_DATA_CTRL_RAW8_PACKING_FOR_RAW16		BIT(0)
+#define CCS_EMB_DATA_CTRL_RAW10_PACKING_FOR_RAW20		BIT(1)
+#define CCS_EMB_DATA_CTRL_RAW12_PACKING_FOR_RAW24		BIT(2)
+#define CCS_R_GPIO_TRIG_MODE					0x0130
+#define CCS_R_EXTCLK_FREQUENCY_MHZ				(0x0136 | (CCS_FL_16BIT | CCS_FL_IREAL))
+#define CCS_R_TEMP_SENSOR_CTRL					0x0138
+#define CCS_TEMP_SENSOR_CTRL_ENABLE				BIT(0)
+#define CCS_R_TEMP_SENSOR_MODE					0x0139
+#define CCS_R_TEMP_SENSOR_OUTPUT				0x013a
+#define CCS_R_FINE_INTEGRATION_TIME				(0x0200 | CCS_FL_16BIT)
+#define CCS_R_COARSE_INTEGRATION_TIME				(0x0202 | CCS_FL_16BIT)
+#define CCS_R_ANALOG_GAIN_CODE_GLOBAL				(0x0204 | CCS_FL_16BIT)
+#define CCS_R_ANALOG_LINEAR_GAIN_GLOBAL				(0x0206 | CCS_FL_16BIT)
+#define CCS_R_ANALOG_EXPONENTIAL_GAIN_GLOBAL			(0x0208 | CCS_FL_16BIT)
+#define CCS_R_DIGITAL_GAIN_GLOBAL				(0x020e | CCS_FL_16BIT)
+#define CCS_R_SHORT_ANALOG_GAIN_GLOBAL				(0x0216 | CCS_FL_16BIT)
+#define CCS_R_SHORT_DIGITAL_GAIN_GLOBAL				(0x0218 | CCS_FL_16BIT)
+#define CCS_R_HDR_MODE						0x0220
+#define CCS_HDR_MODE_ENABLED					BIT(0)
+#define CCS_HDR_MODE_SEPARATE_ANALOG_GAIN			BIT(1)
+#define CCS_HDR_MODE_UPSCALING					BIT(2)
+#define CCS_HDR_MODE_RESET_SYNC					BIT(3)
+#define CCS_HDR_MODE_TIMING_MODE				BIT(4)
+#define CCS_HDR_MODE_EXPOSURE_CTRL_DIRECT			BIT(5)
+#define CCS_HDR_MODE_SEPARATE_DIGITAL_GAIN			BIT(6)
+#define CCS_R_HDR_RESOLUTION_REDUCTION				0x0221
+#define CCS_HDR_RESOLUTION_REDUCTION_ROW_SHIFT			0U
+#define CCS_HDR_RESOLUTION_REDUCTION_ROW_MASK			0xf
+#define CCS_HDR_RESOLUTION_REDUCTION_COLUMN_SHIFT		4U
+#define CCS_HDR_RESOLUTION_REDUCTION_COLUMN_MASK		0xf0
+#define CCS_R_EXPOSURE_RATIO					0x0222
+#define CCS_R_HDR_INTERNAL_BIT_DEPTH				0x0223
+#define CCS_R_DIRECT_SHORT_INTEGRATION_TIME			(0x0224 | CCS_FL_16BIT)
+#define CCS_R_SHORT_ANALOG_LINEAR_GAIN_GLOBAL			(0x0226 | CCS_FL_16BIT)
+#define CCS_R_SHORT_ANALOG_EXPONENTIAL_GAIN_GLOBAL		(0x0228 | CCS_FL_16BIT)
+#define CCS_R_VT_PIX_CLK_DIV					(0x0300 | CCS_FL_16BIT)
+#define CCS_R_VT_SYS_CLK_DIV					(0x0302 | CCS_FL_16BIT)
+#define CCS_R_PRE_PLL_CLK_DIV					(0x0304 | CCS_FL_16BIT)
+#define CCS_R_PLL_MULTIPLIER					(0x0306 | CCS_FL_16BIT)
+#define CCS_R_OP_PIX_CLK_DIV					(0x0308 | CCS_FL_16BIT)
+#define CCS_R_OP_SYS_CLK_DIV					(0x030a | CCS_FL_16BIT)
+#define CCS_R_OP_PRE_PLL_CLK_DIV				(0x030c | CCS_FL_16BIT)
+#define CCS_R_OP_PLL_MULTIPLIER					(0x030e | CCS_FL_16BIT)
+#define CCS_R_PLL_MODE						0x0310
+#define CCS_PLL_MODE_SHIFT					0U
+#define CCS_PLL_MODE_MASK					0x1
+#define CCS_PLL_MODE_SINGLE					0U
+#define CCS_PLL_MODE_DUAL					1U
+#define CCS_R_OP_PIX_CLK_DIV_REV				(0x0312 | CCS_FL_16BIT)
+#define CCS_R_OP_SYS_CLK_DIV_REV				(0x0314 | CCS_FL_16BIT)
+#define CCS_R_FRAME_LENGTH_LINES				(0x0340 | CCS_FL_16BIT)
+#define CCS_R_LINE_LENGTH_PCK					(0x0342 | CCS_FL_16BIT)
+#define CCS_R_X_ADDR_START					(0x0344 | CCS_FL_16BIT)
+#define CCS_R_Y_ADDR_START					(0x0346 | CCS_FL_16BIT)
+#define CCS_R_X_ADDR_END					(0x0348 | CCS_FL_16BIT)
+#define CCS_R_Y_ADDR_END					(0x034a | CCS_FL_16BIT)
+#define CCS_R_X_OUTPUT_SIZE					(0x034c | CCS_FL_16BIT)
+#define CCS_R_Y_OUTPUT_SIZE					(0x034e | CCS_FL_16BIT)
+#define CCS_R_FRAME_LENGTH_CTRL					0x0350
+#define CCS_FRAME_LENGTH_CTRL_AUTOMATIC				BIT(0)
+#define CCS_R_TIMING_MODE_CTRL					0x0352
+#define CCS_TIMING_MODE_CTRL_MANUAL_READOUT			BIT(0)
+#define CCS_TIMING_MODE_CTRL_DELAYED_EXPOSURE			BIT(1)
+#define CCS_R_START_READOUT_RS					0x0353
+#define CCS_START_READOUT_RS_MANUAL_READOUT_START		BIT(0)
+#define CCS_R_FRAME_MARGIN					(0x0354 | CCS_FL_16BIT)
+#define CCS_R_X_EVEN_INC					(0x0380 | CCS_FL_16BIT)
+#define CCS_R_X_ODD_INC						(0x0382 | CCS_FL_16BIT)
+#define CCS_R_Y_EVEN_INC					(0x0384 | CCS_FL_16BIT)
+#define CCS_R_Y_ODD_INC						(0x0386 | CCS_FL_16BIT)
+#define CCS_R_MONOCHROME_EN					0x0390
+#define CCS_MONOCHROME_EN_ENABLED				0U
+#define CCS_R_SCALING_MODE					(0x0400 | CCS_FL_16BIT)
+#define CCS_SCALING_MODE_NO_SCALING				0U
+#define CCS_SCALING_MODE_HORIZONTAL				1U
+#define CCS_R_SCALE_M						(0x0404 | CCS_FL_16BIT)
+#define CCS_R_SCALE_N						(0x0406 | CCS_FL_16BIT)
+#define CCS_R_DIGITAL_CROP_X_OFFSET				(0x0408 | CCS_FL_16BIT)
+#define CCS_R_DIGITAL_CROP_Y_OFFSET				(0x040a | CCS_FL_16BIT)
+#define CCS_R_DIGITAL_CROP_IMAGE_WIDTH				(0x040c | CCS_FL_16BIT)
+#define CCS_R_DIGITAL_CROP_IMAGE_HEIGHT				(0x040e | CCS_FL_16BIT)
+#define CCS_R_COMPRESSION_MODE					(0x0500 | CCS_FL_16BIT)
+#define CCS_COMPRESSION_MODE_NONE				0U
+#define CCS_COMPRESSION_MODE_DPCM_PCM_SIMPLE			1U
+#define CCS_R_TEST_PATTERN_MODE					(0x0600 | CCS_FL_16BIT)
+#define CCS_TEST_PATTERN_MODE_NONE				0U
+#define CCS_TEST_PATTERN_MODE_SOLID_COLOR			1U
+#define CCS_TEST_PATTERN_MODE_COLOR_BARS			2U
+#define CCS_TEST_PATTERN_MODE_FADE_TO_GREY			3U
+#define CCS_TEST_PATTERN_MODE_PN9				4U
+#define CCS_TEST_PATTERN_MODE_COLOR_TILE			5U
+#define CCS_R_TEST_DATA_RED					(0x0602 | CCS_FL_16BIT)
+#define CCS_R_TEST_DATA_GREENR					(0x0604 | CCS_FL_16BIT)
+#define CCS_R_TEST_DATA_BLUE					(0x0606 | CCS_FL_16BIT)
+#define CCS_R_TEST_DATA_GREENB					(0x0608 | CCS_FL_16BIT)
+#define CCS_R_VALUE_STEP_SIZE_SMOOTH				0x060a
+#define CCS_R_VALUE_STEP_SIZE_QUANTISED				0x060b
+#define CCS_R_TCLK_POST						0x0800
+#define CCS_R_THS_PREPARE					0x0801
+#define CCS_R_THS_ZERO_MIN					0x0802
+#define CCS_R_THS_TRAIL						0x0803
+#define CCS_R_TCLK_TRAIL_MIN					0x0804
+#define CCS_R_TCLK_PREPARE					0x0805
+#define CCS_R_TCLK_ZERO						0x0806
+#define CCS_R_TLPX						0x0807
+#define CCS_R_PHY_CTRL						0x0808
+#define CCS_PHY_CTRL_AUTO					0U
+#define CCS_PHY_CTRL_UI						1U
+#define CCS_PHY_CTRL_MANUAL					2U
+#define CCS_R_TCLK_POST_EX					(0x080a | CCS_FL_16BIT)
+#define CCS_R_THS_PREPARE_EX					(0x080c | CCS_FL_16BIT)
+#define CCS_R_THS_ZERO_MIN_EX					(0x080e | CCS_FL_16BIT)
+#define CCS_R_THS_TRAIL_EX					(0x0810 | CCS_FL_16BIT)
+#define CCS_R_TCLK_TRAIL_MIN_EX					(0x0812 | CCS_FL_16BIT)
+#define CCS_R_TCLK_PREPARE_EX					(0x0814 | CCS_FL_16BIT)
+#define CCS_R_TCLK_ZERO_EX					(0x0816 | CCS_FL_16BIT)
+#define CCS_R_TLPX_EX						(0x0818 | CCS_FL_16BIT)
+#define CCS_R_REQUESTED_LINK_RATE				(0x0820 | CCS_FL_32BIT)
+#define CCS_R_DPHY_EQUALIZATION_MODE				0x0824
+#define CCS_DPHY_EQUALIZATION_MODE_EQ2				BIT(0)
+#define CCS_R_PHY_EQUALIZATION_CTRL				0x0825
+#define CCS_PHY_EQUALIZATION_CTRL_ENABLE			BIT(0)
+#define CCS_R_DPHY_PREAMBLE_CTRL				0x0826
+#define CCS_DPHY_PREAMBLE_CTRL_ENABLE				BIT(0)
+#define CCS_R_DPHY_PREAMBLE_LENGTH				0x0826
+#define CCS_R_PHY_SSC_CTRL					0x0828
+#define CCS_PHY_SSC_CTRL_ENABLE					BIT(0)
+#define CCS_R_MANUAL_LP_CTRL					0x0829
+#define CCS_MANUAL_LP_CTRL_ENABLE				BIT(0)
+#define CCS_R_TWAKEUP						0x082a
+#define CCS_R_TINIT						0x082b
+#define CCS_R_THS_EXIT						0x082c
+#define CCS_R_THS_EXIT_EX					(0x082e | CCS_FL_16BIT)
+#define CCS_R_PHY_PERIODIC_CALIBRATION_CTRL			0x0830
+#define CCS_PHY_PERIODIC_CALIBRATION_CTRL_FRAME_BLANKING	BIT(0)
+#define CCS_R_PHY_PERIODIC_CALIBRATION_INTERVAL			0x0831
+#define CCS_R_PHY_INIT_CALIBRATION_CTRL				0x0832
+#define CCS_PHY_INIT_CALIBRATION_CTRL_STREAM_START		BIT(0)
+#define CCS_R_DPHY_CALIBRATION_MODE				0x0833
+#define CCS_DPHY_CALIBRATION_MODE_ALSO_ALTERNATE		BIT(0)
+#define CCS_R_CPHY_CALIBRATION_MODE				0x0834
+#define CCS_CPHY_CALIBRATION_MODE_FORMAT_1			0U
+#define CCS_CPHY_CALIBRATION_MODE_FORMAT_2			1U
+#define CCS_CPHY_CALIBRATION_MODE_FORMAT_3			2U
+#define CCS_R_T3_CALPREAMBLE_LENGTH				0x0835
+#define CCS_R_T3_CALPREAMBLE_LENGTH_PER				0x0836
+#define CCS_R_T3_CALALTSEQ_LENGTH				0x0837
+#define CCS_R_T3_CALALTSEQ_LENGTH_PER				0x0838
+#define CCS_R_FM2_INIT_SEED					(0x083a | CCS_FL_16BIT)
+#define CCS_R_T3_CALUDEFSEQ_LENGTH				(0x083c | CCS_FL_16BIT)
+#define CCS_R_T3_CALUDEFSEQ_LENGTH_PER				(0x083e | CCS_FL_16BIT)
+#define CCS_R_TGR_PREAMBLE_LENGTH				0x0841
+#define CCS_TGR_PREAMBLE_LENGTH_PREAMABLE_PROG_SEQ		BIT(7)
+#define CCS_TGR_PREAMBLE_LENGTH_BEGIN_PREAMBLE_LENGTH_SHIFT	0U
+#define CCS_TGR_PREAMBLE_LENGTH_BEGIN_PREAMBLE_LENGTH_MASK	0x3f
+#define CCS_R_TGR_POST_LENGTH					0x0842
+#define CCS_TGR_POST_LENGTH_POST_LENGTH_SHIFT			0U
+#define CCS_TGR_POST_LENGTH_POST_LENGTH_MASK			0x1f
+#define CCS_R_TGR_PREAMBLE_PROG_SEQUENCE(n2)			(0x0843 + (n2))
+#define CCS_LIM_TGR_PREAMBLE_PROG_SEQUENCE_MIN_N2		0U
+#define CCS_LIM_TGR_PREAMBLE_PROG_SEQUENCE_MAX_N2		6U
+#define CCS_TGR_PREAMBLE_PROG_SEQUENCE_SYMBOL_N_1_SHIFT		3U
+#define CCS_TGR_PREAMBLE_PROG_SEQUENCE_SYMBOL_N_1_MASK		0x38
+#define CCS_TGR_PREAMBLE_PROG_SEQUENCE_SYMBOL_N_SHIFT		0U
+#define CCS_TGR_PREAMBLE_PROG_SEQUENCE_SYMBOL_N_MASK		0x7
+#define CCS_R_T3_PREPARE					(0x084e | CCS_FL_16BIT)
+#define CCS_R_T3_LPX						(0x0850 | CCS_FL_16BIT)
+#define CCS_R_ALPS_CTRL						0x085a
+#define CCS_ALPS_CTRL_LVLP_DPHY					BIT(0)
+#define CCS_ALPS_CTRL_LVLP_CPHY					BIT(1)
+#define CCS_ALPS_CTRL_ALP_CPHY					BIT(2)
+#define CCS_R_TX_REG_CSI_EPD_EN_SSP_CPHY			(0x0860 | CCS_FL_16BIT)
+#define CCS_R_TX_REG_CSI_EPD_OP_SLP_CPHY			(0x0862 | CCS_FL_16BIT)
+#define CCS_R_TX_REG_CSI_EPD_EN_SSP_DPHY			(0x0864 | CCS_FL_16BIT)
+#define CCS_R_TX_REG_CSI_EPD_OP_SLP_DPHY			(0x0866 | CCS_FL_16BIT)
+#define CCS_R_TX_REG_CSI_EPD_MISC_OPTION_CPHY			0x0868
+#define CCS_R_TX_REG_CSI_EPD_MISC_OPTION_DPHY			0x0869
+#define CCS_R_SCRAMBLING_CTRL					0x0870
+#define CCS_SCRAMBLING_CTRL_ENABLED				BIT(0)
+#define CCS_SCRAMBLING_CTRL_SHIFT				2U
+#define CCS_SCRAMBLING_CTRL_MASK				0xc
+#define CCS_SCRAMBLING_CTRL_1_SEED_CPHY				0U
+#define CCS_SCRAMBLING_CTRL_4_SEED_CPHY				3U
+#define CCS_R_LANE_SEED_VALUE(seed, lane)			((0x0872 | CCS_FL_16BIT) + (seed) * 16 + (lane) * 2)
+#define CCS_LIM_LANE_SEED_VALUE_MIN_SEED			0U
+#define CCS_LIM_LANE_SEED_VALUE_MAX_SEED			3U
+#define CCS_LIM_LANE_SEED_VALUE_MIN_LANE			0U
+#define CCS_LIM_LANE_SEED_VALUE_MAX_LANE			7U
+#define CCS_R_TX_USL_REV_ENTRY					(0x08c0 | CCS_FL_16BIT)
+#define CCS_R_TX_USL_REV_CLOCK_COUNTER				(0x08c2 | CCS_FL_16BIT)
+#define CCS_R_TX_USL_REV_LP_COUNTER				(0x08c4 | CCS_FL_16BIT)
+#define CCS_R_TX_USL_REV_FRAME_COUNTER				(0x08c6 | CCS_FL_16BIT)
+#define CCS_R_TX_USL_REV_CHRONOLOGICAL_TIMER			(0x08c8 | CCS_FL_16BIT)
+#define CCS_R_TX_USL_FWD_ENTRY					(0x08ca | CCS_FL_16BIT)
+#define CCS_R_TX_USL_GPIO					(0x08cc | CCS_FL_16BIT)
+#define CCS_R_TX_USL_OPERATION					(0x08ce | CCS_FL_16BIT)
+#define CCS_TX_USL_OPERATION_RESET				BIT(0)
+#define CCS_R_TX_USL_ALP_CTRL					(0x08d0 | CCS_FL_16BIT)
+#define CCS_TX_USL_ALP_CTRL_CLOCK_PAUSE				BIT(0)
+#define CCS_R_TX_USL_APP_BTA_ACK_TIMEOUT			(0x08d2 | CCS_FL_16BIT)
+#define CCS_R_TX_USL_SNS_BTA_ACK_TIMEOUT			(0x08d2 | CCS_FL_16BIT)
+#define CCS_R_USL_CLOCK_MODE_D_CTRL				0x08d2
+#define CCS_USL_CLOCK_MODE_D_CTRL_CONT_CLOCK_STANDBY		BIT(0)
+#define CCS_USL_CLOCK_MODE_D_CTRL_CONT_CLOCK_VBLANK		BIT(1)
+#define CCS_USL_CLOCK_MODE_D_CTRL_CONT_CLOCK_HBLANK		BIT(2)
+#define CCS_R_BINNING_MODE					0x0900
+#define CCS_R_BINNING_TYPE					0x0901
+#define CCS_R_BINNING_WEIGHTING					0x0902
+#define CCS_R_DATA_TRANSFER_IF_1_CTRL				0x0a00
+#define CCS_DATA_TRANSFER_IF_1_CTRL_ENABLE			BIT(0)
+#define CCS_DATA_TRANSFER_IF_1_CTRL_WRITE			BIT(1)
+#define CCS_DATA_TRANSFER_IF_1_CTRL_CLEAR_ERROR			BIT(2)
+#define CCS_R_DATA_TRANSFER_IF_1_STATUS				0x0a01
+#define CCS_DATA_TRANSFER_IF_1_STATUS_READ_IF_READY		BIT(0)
+#define CCS_DATA_TRANSFER_IF_1_STATUS_WRITE_IF_READY		BIT(1)
+#define CCS_DATA_TRANSFER_IF_1_STATUS_DATA_CORRUPTED		BIT(2)
+#define CCS_DATA_TRANSFER_IF_1_STATUS_IMPROPER_IF_USAGE		BIT(3)
+#define CCS_R_DATA_TRANSFER_IF_1_PAGE_SELECT			0x0a02
+#define CCS_R_DATA_TRANSFER_IF_1_DATA(p)			(0x0a04 + (p))
+#define CCS_LIM_DATA_TRANSFER_IF_1_DATA_MIN_P			0U
+#define CCS_LIM_DATA_TRANSFER_IF_1_DATA_MAX_P			63U
+#define CCS_R_SHADING_CORRECTION_EN				0x0b00
+#define CCS_SHADING_CORRECTION_EN_ENABLE			BIT(0)
+#define CCS_R_LUMINANCE_CORRECTION_LEVEL			0x0b01
+#define CCS_R_GREEN_IMBALANCE_FILTER_EN				0x0b02
+#define CCS_GREEN_IMBALANCE_FILTER_EN_ENABLE			BIT(0)
+#define CCS_R_MAPPED_DEFECT_CORRECT_EN				0x0b05
+#define CCS_MAPPED_DEFECT_CORRECT_EN_ENABLE			BIT(0)
+#define CCS_R_SINGLE_DEFECT_CORRECT_EN				0x0b06
+#define CCS_SINGLE_DEFECT_CORRECT_EN_ENABLE			BIT(0)
+#define CCS_R_DYNAMIC_COUPLET_CORRECT_EN			0x0b08
+#define CCS_DYNAMIC_COUPLET_CORRECT_EN_ENABLE			BIT(0)
+#define CCS_R_COMBINED_DEFECT_CORRECT_EN			0x0b0a
+#define CCS_COMBINED_DEFECT_CORRECT_EN_ENABLE			BIT(0)
+#define CCS_R_MODULE_SPECIFIC_CORRECTION_EN			0x0b0c
+#define CCS_MODULE_SPECIFIC_CORRECTION_EN_ENABLE		BIT(0)
+#define CCS_R_DYNAMIC_TRIPLET_DEFECT_CORRECT_EN			0x0b13
+#define CCS_DYNAMIC_TRIPLET_DEFECT_CORRECT_EN_ENABLE		BIT(0)
+#define CCS_R_NF_CTRL						0x0b15
+#define CCS_NF_CTRL_LUMA					BIT(0)
+#define CCS_NF_CTRL_CHROMA					BIT(1)
+#define CCS_NF_CTRL_COMBINED					BIT(2)
+#define CCS_R_OB_READOUT_CONTROL				0x0b30
+#define CCS_OB_READOUT_CONTROL_ENABLE				BIT(0)
+#define CCS_OB_READOUT_CONTROL_INTERLEAVING			BIT(1)
+#define CCS_R_OB_VIRTUAL_CHANNEL				0x0b31
+#define CCS_R_OB_DT						0x0b32
+#define CCS_R_OB_DATA_FORMAT					0x0b33
+#define CCS_R_COLOR_TEMPERATURE					(0x0b8c | CCS_FL_16BIT)
+#define CCS_R_ABSOLUTE_GAIN_GREENR				(0x0b8e | CCS_FL_16BIT)
+#define CCS_R_ABSOLUTE_GAIN_RED					(0x0b90 | CCS_FL_16BIT)
+#define CCS_R_ABSOLUTE_GAIN_BLUE				(0x0b92 | CCS_FL_16BIT)
+#define CCS_R_ABSOLUTE_GAIN_GREENB				(0x0b94 | CCS_FL_16BIT)
+#define CCS_R_CFA_CONVERSION_CTRL				0x0ba0
+#define CCS_CFA_CONVERSION_CTRL_BAYER_CONVERSION_ENABLE		BIT(0)
+#define CCS_R_FLASH_STROBE_ADJUSTMENT				0x0c12
+#define CCS_R_FLASH_STROBE_START_POINT				(0x0c14 | CCS_FL_16BIT)
+#define CCS_R_TFLASH_STROBE_DELAY_RS_CTRL			(0x0c16 | CCS_FL_16BIT)
+#define CCS_R_TFLASH_STROBE_WIDTH_HIGH_RS_CTRL			(0x0c18 | CCS_FL_16BIT)
+#define CCS_R_FLASH_MODE_RS					0x0c1a
+#define CCS_FLASH_MODE_RS_CONTINUOUS				BIT(0)
+#define CCS_FLASH_MODE_RS_TRUNCATE				BIT(1)
+#define CCS_FLASH_MODE_RS_ASYNC					BIT(3)
+#define CCS_R_FLASH_TRIGGER_RS					0x0c1b
+#define CCS_R_FLASH_STATUS					0x0c1c
+#define CCS_FLASH_STATUS_RETIMED				BIT(0)
+#define CCS_R_SA_STROBE_MODE					0x0c1d
+#define CCS_SA_STROBE_MODE_CONTINUOUS				BIT(0)
+#define CCS_SA_STROBE_MODE_TRUNCATE				BIT(1)
+#define CCS_SA_STROBE_MODE_ASYNC				BIT(3)
+#define CCS_SA_STROBE_MODE_ADJUST_EDGE				BIT(4)
+#define CCS_R_SA_STROBE_START_POINT				(0x0c1e | CCS_FL_16BIT)
+#define CCS_R_TSA_STROBE_DELAY_CTRL				(0x0c20 | CCS_FL_16BIT)
+#define CCS_R_TSA_STROBE_WIDTH_CTRL				(0x0c22 | CCS_FL_16BIT)
+#define CCS_R_SA_STROBE_TRIGGER					0x0c24
+#define CCS_R_SA_STROBE_STATUS					0x0c25
+#define CCS_SA_STROBE_STATUS_RETIMED				BIT(0)
+#define CCS_R_TSA_STROBE_RE_DELAY_CTRL				(0x0c30 | CCS_FL_16BIT)
+#define CCS_R_TSA_STROBE_FE_DELAY_CTRL				(0x0c32 | CCS_FL_16BIT)
+#define CCS_R_PDAF_CTRL						(0x0d00 | CCS_FL_16BIT)
+#define CCS_PDAF_CTRL_ENABLE					BIT(0)
+#define CCS_PDAF_CTRL_PROCESSED					BIT(1)
+#define CCS_PDAF_CTRL_INTERLEAVED				BIT(2)
+#define CCS_PDAF_CTRL_VISIBLE_PDAF_CORRECTION			BIT(3)
+#define CCS_R_PDAF_VC						0x0d02
+#define CCS_R_PDAF_DT						0x0d03
+#define CCS_R_PD_X_ADDR_START					(0x0d04 | CCS_FL_16BIT)
+#define CCS_R_PD_Y_ADDR_START					(0x0d06 | CCS_FL_16BIT)
+#define CCS_R_PD_X_ADDR_END					(0x0d08 | CCS_FL_16BIT)
+#define CCS_R_PD_Y_ADDR_END					(0x0d0a | CCS_FL_16BIT)
+#define CCS_R_BRACKETING_LUT_CTRL				0x0e00
+#define CCS_R_BRACKETING_LUT_MODE				0x0e01
+#define CCS_BRACKETING_LUT_MODE_CONTINUE_STREAMING		BIT(0)
+#define CCS_BRACKETING_LUT_MODE_LOOP_MODE			BIT(1)
+#define CCS_R_BRACKETING_LUT_ENTRY_CTRL				0x0e02
+#define CCS_R_BRACKETING_LUT_FRAME(n)				(0x0e10 + (n))
+#define CCS_LIM_BRACKETING_LUT_FRAME_MIN_N			0U
+#define CCS_LIM_BRACKETING_LUT_FRAME_MAX_N			239U
+#define CCS_R_INTEGRATION_TIME_CAPABILITY			(0x1000 | CCS_FL_16BIT)
+#define CCS_INTEGRATION_TIME_CAPABILITY_FINE			BIT(0)
+#define CCS_R_COARSE_INTEGRATION_TIME_MIN			(0x1004 | CCS_FL_16BIT)
+#define CCS_R_COARSE_INTEGRATION_TIME_MAX_MARGIN		(0x1006 | CCS_FL_16BIT)
+#define CCS_R_FINE_INTEGRATION_TIME_MIN				(0x1008 | CCS_FL_16BIT)
+#define CCS_R_FINE_INTEGRATION_TIME_MAX_MARGIN			(0x100a | CCS_FL_16BIT)
+#define CCS_R_DIGITAL_GAIN_CAPABILITY				0x1081
+#define CCS_DIGITAL_GAIN_CAPABILITY_NONE			0U
+#define CCS_DIGITAL_GAIN_CAPABILITY_GLOBAL			2U
+#define CCS_R_DIGITAL_GAIN_MIN					(0x1084 | CCS_FL_16BIT)
+#define CCS_R_DIGITAL_GAIN_MAX					(0x1086 | CCS_FL_16BIT)
+#define CCS_R_DIGITAL_GAIN_STEP_SIZE				(0x1088 | CCS_FL_16BIT)
+#define CCS_R_PEDESTAL_CAPABILITY				0x10e0
+#define CCS_R_ADC_CAPABILITY					0x10f0
+#define CCS_ADC_CAPABILITY_BIT_DEPTH_CTRL			BIT(0)
+#define CCS_R_ADC_BIT_DEPTH_CAPABILITY				(0x10f4 | CCS_FL_32BIT)
+#define CCS_R_MIN_EXT_CLK_FREQ_MHZ				(0x1100 | (CCS_FL_32BIT | CCS_FL_FLOAT_IREAL))
+#define CCS_R_MAX_EXT_CLK_FREQ_MHZ				(0x1104 | (CCS_FL_32BIT | CCS_FL_FLOAT_IREAL))
+#define CCS_R_MIN_PRE_PLL_CLK_DIV				(0x1108 | CCS_FL_16BIT)
+#define CCS_R_MAX_PRE_PLL_CLK_DIV				(0x110a | CCS_FL_16BIT)
+#define CCS_R_MIN_PLL_IP_CLK_FREQ_MHZ				(0x110c | (CCS_FL_32BIT | CCS_FL_FLOAT_IREAL))
+#define CCS_R_MAX_PLL_IP_CLK_FREQ_MHZ				(0x1110 | (CCS_FL_32BIT | CCS_FL_FLOAT_IREAL))
+#define CCS_R_MIN_PLL_MULTIPLIER				(0x1114 | CCS_FL_16BIT)
+#define CCS_R_MAX_PLL_MULTIPLIER				(0x1116 | CCS_FL_16BIT)
+#define CCS_R_MIN_PLL_OP_CLK_FREQ_MHZ				(0x1118 | (CCS_FL_32BIT | CCS_FL_FLOAT_IREAL))
+#define CCS_R_MAX_PLL_OP_CLK_FREQ_MHZ				(0x111c | (CCS_FL_32BIT | CCS_FL_FLOAT_IREAL))
+#define CCS_R_MIN_VT_SYS_CLK_DIV				(0x1120 | CCS_FL_16BIT)
+#define CCS_R_MAX_VT_SYS_CLK_DIV				(0x1122 | CCS_FL_16BIT)
+#define CCS_R_MIN_VT_SYS_CLK_FREQ_MHZ				(0x1124 | (CCS_FL_32BIT | CCS_FL_FLOAT_IREAL))
+#define CCS_R_MAX_VT_SYS_CLK_FREQ_MHZ				(0x1128 | (CCS_FL_32BIT | CCS_FL_FLOAT_IREAL))
+#define CCS_R_MIN_VT_PIX_CLK_FREQ_MHZ				(0x112c | (CCS_FL_32BIT | CCS_FL_FLOAT_IREAL))
+#define CCS_R_MAX_VT_PIX_CLK_FREQ_MHZ				(0x1130 | (CCS_FL_32BIT | CCS_FL_FLOAT_IREAL))
+#define CCS_R_MIN_VT_PIX_CLK_DIV				(0x1134 | CCS_FL_16BIT)
+#define CCS_R_MAX_VT_PIX_CLK_DIV				(0x1136 | CCS_FL_16BIT)
+#define CCS_R_CLOCK_CALCULATION					0x1138
+#define CCS_CLOCK_CALCULATION_LANE_SPEED			BIT(0)
+#define CCS_CLOCK_CALCULATION_LINK_DECOUPLED			BIT(1)
+#define CCS_CLOCK_CALCULATION_DUAL_PLL_OP_SYS_DDR		BIT(2)
+#define CCS_CLOCK_CALCULATION_DUAL_PLL_OP_PIX_DDR		BIT(3)
+#define CCS_R_NUM_OF_VT_LANES					0x1139
+#define CCS_R_NUM_OF_OP_LANES					0x113a
+#define CCS_R_OP_BITS_PER_LANE					0x113b
+#define CCS_R_MIN_FRAME_LENGTH_LINES				(0x1140 | CCS_FL_16BIT)
+#define CCS_R_MAX_FRAME_LENGTH_LINES				(0x1142 | CCS_FL_16BIT)
+#define CCS_R_MIN_LINE_LENGTH_PCK				(0x1144 | CCS_FL_16BIT)
+#define CCS_R_MAX_LINE_LENGTH_PCK				(0x1146 | CCS_FL_16BIT)
+#define CCS_R_MIN_LINE_BLANKING_PCK				(0x1148 | CCS_FL_16BIT)
+#define CCS_R_MIN_FRAME_BLANKING_LINES				(0x114a | CCS_FL_16BIT)
+#define CCS_R_MIN_LINE_LENGTH_PCK_STEP_SIZE			0x114c
+#define CCS_R_TIMING_MODE_CAPABILITY				0x114d
+#define CCS_TIMING_MODE_CAPABILITY_AUTO_FRAME_LENGTH		BIT(0)
+#define CCS_TIMING_MODE_CAPABILITY_ROLLING_SHUTTER_MANUAL_READOUT      BIT(2)
+#define CCS_TIMING_MODE_CAPABILITY_DELAYED_EXPOSURE_START	BIT(3)
+#define CCS_TIMING_MODE_CAPABILITY_MANUAL_EXPOSURE_EMBEDDED_DATA       BIT(4)
+#define CCS_R_FRAME_MARGIN_MAX_VALUE				(0x114e | CCS_FL_16BIT)
+#define CCS_R_FRAME_MARGIN_MIN_VALUE				0x1150
+#define CCS_R_GAIN_DELAY_TYPE					0x1151
+#define CCS_GAIN_DELAY_TYPE_FIXED				0U
+#define CCS_GAIN_DELAY_TYPE_VARIABLE				1U
+#define CCS_R_MIN_OP_SYS_CLK_DIV				(0x1160 | CCS_FL_16BIT)
+#define CCS_R_MAX_OP_SYS_CLK_DIV				(0x1162 | CCS_FL_16BIT)
+#define CCS_R_MIN_OP_SYS_CLK_FREQ_MHZ				(0x1164 | (CCS_FL_32BIT | CCS_FL_FLOAT_IREAL))
+#define CCS_R_MAX_OP_SYS_CLK_FREQ_MHZ				(0x1168 | (CCS_FL_32BIT | CCS_FL_FLOAT_IREAL))
+#define CCS_R_MIN_OP_PIX_CLK_DIV				(0x116c | CCS_FL_16BIT)
+#define CCS_R_MAX_OP_PIX_CLK_DIV				(0x116e | CCS_FL_16BIT)
+#define CCS_R_MIN_OP_PIX_CLK_FREQ_MHZ				(0x1170 | (CCS_FL_32BIT | CCS_FL_FLOAT_IREAL))
+#define CCS_R_MAX_OP_PIX_CLK_FREQ_MHZ				(0x1174 | (CCS_FL_32BIT | CCS_FL_FLOAT_IREAL))
+#define CCS_R_X_ADDR_MIN					(0x1180 | CCS_FL_16BIT)
+#define CCS_R_Y_ADDR_MIN					(0x1182 | CCS_FL_16BIT)
+#define CCS_R_X_ADDR_MAX					(0x1184 | CCS_FL_16BIT)
+#define CCS_R_Y_ADDR_MAX					(0x1186 | CCS_FL_16BIT)
+#define CCS_R_MIN_X_OUTPUT_SIZE					(0x1188 | CCS_FL_16BIT)
+#define CCS_R_MIN_Y_OUTPUT_SIZE					(0x118a | CCS_FL_16BIT)
+#define CCS_R_MAX_X_OUTPUT_SIZE					(0x118c | CCS_FL_16BIT)
+#define CCS_R_MAX_Y_OUTPUT_SIZE					(0x118e | CCS_FL_16BIT)
+#define CCS_R_X_ADDR_START_DIV_CONSTANT				0x1190
+#define CCS_R_Y_ADDR_START_DIV_CONSTANT				0x1191
+#define CCS_R_X_ADDR_END_DIV_CONSTANT				0x1192
+#define CCS_R_Y_ADDR_END_DIV_CONSTANT				0x1193
+#define CCS_R_X_SIZE_DIV					0x1194
+#define CCS_R_Y_SIZE_DIV					0x1195
+#define CCS_R_X_OUTPUT_DIV					0x1196
+#define CCS_R_Y_OUTPUT_DIV					0x1197
+#define CCS_R_NON_FLEXIBLE_RESOLUTION_SUPPORT			0x1198
+#define CCS_NON_FLEXIBLE_RESOLUTION_SUPPORT_NEW_PIX_ADDR	BIT(0)
+#define CCS_NON_FLEXIBLE_RESOLUTION_SUPPORT_NEW_OUTPUT_RES	BIT(1)
+#define CCS_NON_FLEXIBLE_RESOLUTION_SUPPORT_OUTPUT_CROP_NO_PAD	BIT(2)
+#define CCS_NON_FLEXIBLE_RESOLUTION_SUPPORT_OUTPUT_SIZE_LANE_DEP       BIT(3)
+#define CCS_R_MIN_OP_PRE_PLL_CLK_DIV				(0x11a0 | CCS_FL_16BIT)
+#define CCS_R_MAX_OP_PRE_PLL_CLK_DIV				(0x11a2 | CCS_FL_16BIT)
+#define CCS_R_MIN_OP_PLL_IP_CLK_FREQ_MHZ			(0x11a4 | (CCS_FL_32BIT | CCS_FL_FLOAT_IREAL))
+#define CCS_R_MAX_OP_PLL_IP_CLK_FREQ_MHZ			(0x11a8 | (CCS_FL_32BIT | CCS_FL_FLOAT_IREAL))
+#define CCS_R_MIN_OP_PLL_MULTIPLIER				(0x11ac | CCS_FL_16BIT)
+#define CCS_R_MAX_OP_PLL_MULTIPLIER				(0x11ae | CCS_FL_16BIT)
+#define CCS_R_MIN_OP_PLL_OP_CLK_FREQ_MHZ			(0x11b0 | (CCS_FL_32BIT | CCS_FL_FLOAT_IREAL))
+#define CCS_R_MAX_OP_PLL_OP_CLK_FREQ_MHZ			(0x11b4 | (CCS_FL_32BIT | CCS_FL_FLOAT_IREAL))
+#define CCS_R_CLOCK_TREE_PLL_CAPABILITY				0x11b8
+#define CCS_CLOCK_TREE_PLL_CAPABILITY_DUAL_PLL			BIT(0)
+#define CCS_CLOCK_TREE_PLL_CAPABILITY_SINGLE_PLL		BIT(1)
+#define CCS_CLOCK_TREE_PLL_CAPABILITY_EXT_DIVIDER		BIT(2)
+#define CCS_CLOCK_TREE_PLL_CAPABILITY_FLEXIBLE_OP_PIX_CLK_DIV	BIT(3)
+#define CCS_R_CLOCK_CAPA_TYPE_CAPABILITY			0x11b9
+#define CCS_CLOCK_CAPA_TYPE_CAPABILITY_IREAL			BIT(0)
+#define CCS_R_MIN_EVEN_INC					(0x11c0 | CCS_FL_16BIT)
+#define CCS_R_MIN_ODD_INC					(0x11c2 | CCS_FL_16BIT)
+#define CCS_R_MAX_EVEN_INC					(0x11c4 | CCS_FL_16BIT)
+#define CCS_R_MAX_ODD_INC					(0x11c6 | CCS_FL_16BIT)
+#define CCS_R_AUX_SUBSAMP_CAPABILITY				0x11c8
+#define CCS_AUX_SUBSAMP_CAPABILITY_FACTOR_POWER_OF_2		BIT(1)
+#define CCS_R_AUX_SUBSAMP_MONO_CAPABILITY			0x11c9
+#define CCS_AUX_SUBSAMP_MONO_CAPABILITY_FACTOR_POWER_OF_2	BIT(1)
+#define CCS_R_MONOCHROME_CAPABILITY				0x11ca
+#define CCS_MONOCHROME_CAPABILITY_INC_ODD			0U
+#define CCS_MONOCHROME_CAPABILITY_INC_EVEN			1U
+#define CCS_R_PIXEL_READOUT_CAPABILITY				0x11cb
+#define CCS_PIXEL_READOUT_CAPABILITY_BAYER			0U
+#define CCS_PIXEL_READOUT_CAPABILITY_MONOCHROME			1U
+#define CCS_PIXEL_READOUT_CAPABILITY_BAYER_AND_MONO		2U
+#define CCS_R_MIN_EVEN_INC_MONO					(0x11cc | CCS_FL_16BIT)
+#define CCS_R_MAX_EVEN_INC_MONO					(0x11ce | CCS_FL_16BIT)
+#define CCS_R_MIN_ODD_INC_MONO					(0x11d0 | CCS_FL_16BIT)
+#define CCS_R_MAX_ODD_INC_MONO					(0x11d2 | CCS_FL_16BIT)
+#define CCS_R_MIN_EVEN_INC_BC2					(0x11d4 | CCS_FL_16BIT)
+#define CCS_R_MAX_EVEN_INC_BC2					(0x11d6 | CCS_FL_16BIT)
+#define CCS_R_MIN_ODD_INC_BC2					(0x11d8 | CCS_FL_16BIT)
+#define CCS_R_MAX_ODD_INC_BC2					(0x11da | CCS_FL_16BIT)
+#define CCS_R_MIN_EVEN_INC_MONO_BC2				(0x11dc | CCS_FL_16BIT)
+#define CCS_R_MAX_EVEN_INC_MONO_BC2				(0x11de | CCS_FL_16BIT)
+#define CCS_R_MIN_ODD_INC_MONO_BC2				(0x11f0 | CCS_FL_16BIT)
+#define CCS_R_MAX_ODD_INC_MONO_BC2				(0x11f2 | CCS_FL_16BIT)
+#define CCS_R_SCALING_CAPABILITY				(0x1200 | CCS_FL_16BIT)
+#define CCS_SCALING_CAPABILITY_NONE				0U
+#define CCS_SCALING_CAPABILITY_HORIZONTAL			1U
+#define CCS_SCALING_CAPABILITY_RESERVED				2U
+#define CCS_R_SCALER_M_MIN					(0x1204 | CCS_FL_16BIT)
+#define CCS_R_SCALER_M_MAX					(0x1206 | CCS_FL_16BIT)
+#define CCS_R_SCALER_N_MIN					(0x1208 | CCS_FL_16BIT)
+#define CCS_R_SCALER_N_MAX					(0x120a | CCS_FL_16BIT)
+#define CCS_R_DIGITAL_CROP_CAPABILITY				0x120e
+#define CCS_DIGITAL_CROP_CAPABILITY_NONE			0U
+#define CCS_DIGITAL_CROP_CAPABILITY_INPUT_CROP			1U
+#define CCS_R_HDR_CAPABILITY_1					0x1210
+#define CCS_HDR_CAPABILITY_1_2X2_BINNING			BIT(0)
+#define CCS_HDR_CAPABILITY_1_COMBINED_ANALOG_GAIN		BIT(1)
+#define CCS_HDR_CAPABILITY_1_SEPARATE_ANALOG_GAIN		BIT(2)
+#define CCS_HDR_CAPABILITY_1_UPSCALING				BIT(3)
+#define CCS_HDR_CAPABILITY_1_RESET_SYNC				BIT(4)
+#define CCS_HDR_CAPABILITY_1_DIRECT_SHORT_EXP_TIMING		BIT(5)
+#define CCS_HDR_CAPABILITY_1_DIRECT_SHORT_EXP_SYNTHESIS		BIT(6)
+#define CCS_R_MIN_HDR_BIT_DEPTH					0x1211
+#define CCS_R_HDR_RESOLUTION_SUB_TYPES				0x1212
+#define CCS_R_HDR_RESOLUTION_SUB_TYPE(n)			(0x1213 + (n))
+#define CCS_LIM_HDR_RESOLUTION_SUB_TYPE_MIN_N			0U
+#define CCS_LIM_HDR_RESOLUTION_SUB_TYPE_MAX_N			1U
+#define CCS_HDR_RESOLUTION_SUB_TYPE_ROW_SHIFT			0U
+#define CCS_HDR_RESOLUTION_SUB_TYPE_ROW_MASK			0xf
+#define CCS_HDR_RESOLUTION_SUB_TYPE_COLUMN_SHIFT		4U
+#define CCS_HDR_RESOLUTION_SUB_TYPE_COLUMN_MASK			0xf0
+#define CCS_R_HDR_CAPABILITY_2					0x121b
+#define CCS_HDR_CAPABILITY_2_COMBINED_DIGITAL_GAIN		BIT(0)
+#define CCS_HDR_CAPABILITY_2_SEPARATE_DIGITAL_GAIN		BIT(1)
+#define CCS_HDR_CAPABILITY_2_TIMING_MODE			BIT(3)
+#define CCS_HDR_CAPABILITY_2_SYNTHESIS_MODE			BIT(4)
+#define CCS_R_MAX_HDR_BIT_DEPTH					0x121c
+#define CCS_R_USL_SUPPORT_CAPABILITY				0x1230
+#define CCS_USL_SUPPORT_CAPABILITY_CLOCK_TREE			BIT(0)
+#define CCS_USL_SUPPORT_CAPABILITY_REV_CLOCK_TREE		BIT(1)
+#define CCS_USL_SUPPORT_CAPABILITY_REV_CLOCK_CALC		BIT(2)
+#define CCS_R_USL_CLOCK_MODE_D_CAPABILITY			0x1231
+#define CCS_USL_CLOCK_MODE_D_CAPABILITY_CONT_CLOCK_STANDBY	BIT(0)
+#define CCS_USL_CLOCK_MODE_D_CAPABILITY_CONT_CLOCK_VBLANK	BIT(1)
+#define CCS_USL_CLOCK_MODE_D_CAPABILITY_CONT_CLOCK_HBLANK	BIT(2)
+#define CCS_USL_CLOCK_MODE_D_CAPABILITY_NONCONT_CLOCK_STANDBY	BIT(3)
+#define CCS_USL_CLOCK_MODE_D_CAPABILITY_NONCONT_CLOCK_VBLANK	BIT(4)
+#define CCS_USL_CLOCK_MODE_D_CAPABILITY_NONCONT_CLOCK_HBLANK	BIT(5)
+#define CCS_R_MIN_OP_SYS_CLK_DIV_REV				0x1234
+#define CCS_R_MAX_OP_SYS_CLK_DIV_REV				0x1236
+#define CCS_R_MIN_OP_PIX_CLK_DIV_REV				0x1238
+#define CCS_R_MAX_OP_PIX_CLK_DIV_REV				0x123a
+#define CCS_R_MIN_OP_SYS_CLK_FREQ_REV_MHZ			(0x123c | (CCS_FL_32BIT | CCS_FL_FLOAT_IREAL))
+#define CCS_R_MAX_OP_SYS_CLK_FREQ_REV_MHZ			(0x1240 | (CCS_FL_32BIT | CCS_FL_FLOAT_IREAL))
+#define CCS_R_MIN_OP_PIX_CLK_FREQ_REV_MHZ			(0x1244 | (CCS_FL_32BIT | CCS_FL_FLOAT_IREAL))
+#define CCS_R_MAX_OP_PIX_CLK_FREQ_REV_MHZ			(0x1248 | (CCS_FL_32BIT | CCS_FL_FLOAT_IREAL))
+#define CCS_R_MAX_BITRATE_REV_D_MODE_MBPS			(0x124c | (CCS_FL_32BIT | CCS_FL_IREAL))
+#define CCS_R_MAX_SYMRATE_REV_C_MODE_MSPS			(0x1250 | (CCS_FL_32BIT | CCS_FL_IREAL))
+#define CCS_R_COMPRESSION_CAPABILITY				0x1300
+#define CCS_COMPRESSION_CAPABILITY_DPCM_PCM_SIMPLE		BIT(0)
+#define CCS_R_TEST_MODE_CAPABILITY				(0x1310 | CCS_FL_16BIT)
+#define CCS_TEST_MODE_CAPABILITY_SOLID_COLOR			BIT(0)
+#define CCS_TEST_MODE_CAPABILITY_COLOR_BARS			BIT(1)
+#define CCS_TEST_MODE_CAPABILITY_FADE_TO_GREY			BIT(2)
+#define CCS_TEST_MODE_CAPABILITY_PN9				BIT(3)
+#define CCS_TEST_MODE_CAPABILITY_COLOR_TILE			BIT(5)
+#define CCS_R_PN9_DATA_FORMAT1					0x1312
+#define CCS_R_PN9_DATA_FORMAT2					0x1313
+#define CCS_R_PN9_DATA_FORMAT3					0x1314
+#define CCS_R_PN9_DATA_FORMAT4					0x1315
+#define CCS_R_PN9_MISC_CAPABILITY				0x1316
+#define CCS_PN9_MISC_CAPABILITY_NUM_PIXELS_SHIFT		0U
+#define CCS_PN9_MISC_CAPABILITY_NUM_PIXELS_MASK			0x7
+#define CCS_PN9_MISC_CAPABILITY_COMPRESSION			BIT(3)
+#define CCS_R_TEST_PATTERN_CAPABILITY				0x1317
+#define CCS_TEST_PATTERN_CAPABILITY_NO_REPEAT			BIT(1)
+#define CCS_R_PATTERN_SIZE_DIV_M1				0x1318
+#define CCS_R_FIFO_SUPPORT_CAPABILITY				0x1502
+#define CCS_FIFO_SUPPORT_CAPABILITY_NONE			0U
+#define CCS_FIFO_SUPPORT_CAPABILITY_DERATING			1U
+#define CCS_FIFO_SUPPORT_CAPABILITY_DERATING_OVERRATING		2U
+#define CCS_R_PHY_CTRL_CAPABILITY				0x1600
+#define CCS_PHY_CTRL_CAPABILITY_AUTO_PHY_CTL			BIT(0)
+#define CCS_PHY_CTRL_CAPABILITY_UI_PHY_CTL			BIT(1)
+#define CCS_PHY_CTRL_CAPABILITY_DPHY_TIME_UI_REG_1_CTL		BIT(2)
+#define CCS_PHY_CTRL_CAPABILITY_DPHY_TIME_UI_REG_2_CTL		BIT(3)
+#define CCS_PHY_CTRL_CAPABILITY_DPHY_TIME_CTL			BIT(4)
+#define CCS_PHY_CTRL_CAPABILITY_DPHY_EXT_TIME_UI_REG_1_CTL	BIT(5)
+#define CCS_PHY_CTRL_CAPABILITY_DPHY_EXT_TIME_UI_REG_2_CTL	BIT(6)
+#define CCS_PHY_CTRL_CAPABILITY_DPHY_EXT_TIME_CTL		BIT(7)
+#define CCS_R_CSI_DPHY_LANE_MODE_CAPABILITY			0x1601
+#define CCS_CSI_DPHY_LANE_MODE_CAPABILITY_1_LANE		BIT(0)
+#define CCS_CSI_DPHY_LANE_MODE_CAPABILITY_2_LANE		BIT(1)
+#define CCS_CSI_DPHY_LANE_MODE_CAPABILITY_3_LANE		BIT(2)
+#define CCS_CSI_DPHY_LANE_MODE_CAPABILITY_4_LANE		BIT(3)
+#define CCS_CSI_DPHY_LANE_MODE_CAPABILITY_5_LANE		BIT(4)
+#define CCS_CSI_DPHY_LANE_MODE_CAPABILITY_6_LANE		BIT(5)
+#define CCS_CSI_DPHY_LANE_MODE_CAPABILITY_7_LANE		BIT(6)
+#define CCS_CSI_DPHY_LANE_MODE_CAPABILITY_8_LANE		BIT(7)
+#define CCS_R_CSI_SIGNALING_MODE_CAPABILITY			0x1602
+#define CCS_CSI_SIGNALING_MODE_CAPABILITY_CSI_DPHY		BIT(2)
+#define CCS_CSI_SIGNALING_MODE_CAPABILITY_CSI_CPHY		BIT(3)
+#define CCS_R_FAST_STANDBY_CAPABILITY				0x1603
+#define CCS_FAST_STANDBY_CAPABILITY_NO_FRAME_TRUNCATION		0U
+#define CCS_FAST_STANDBY_CAPABILITY_FRAME_TRUNCATION		1U
+#define CCS_R_CSI_ADDRESS_CONTROL_CAPABILITY			0x1604
+#define CCS_CSI_ADDRESS_CONTROL_CAPABILITY_CCI_ADDR_CHANGE	BIT(0)
+#define CCS_CSI_ADDRESS_CONTROL_CAPABILITY_2ND_CCI_ADDR		BIT(1)
+#define CCS_CSI_ADDRESS_CONTROL_CAPABILITY_SW_CHANGEABLE_2ND_CCI_ADDR  BIT(2)
+#define CCS_R_DATA_TYPE_CAPABILITY				0x1605
+#define CCS_DATA_TYPE_CAPABILITY_DPCM_PROGRAMMABLE		BIT(0)
+#define CCS_DATA_TYPE_CAPABILITY_BOTTOM_EMBEDDED_DT_PROGRAMMABLE       BIT(1)
+#define CCS_DATA_TYPE_CAPABILITY_BOTTOM_EMBEDDED_VC_PROGRAMMABLE       BIT(2)
+#define CCS_DATA_TYPE_CAPABILITY_EXT_VC_RANGE			BIT(3)
+#define CCS_R_CSI_CPHY_LANE_MODE_CAPABILITY			0x1606
+#define CCS_CSI_CPHY_LANE_MODE_CAPABILITY_1_LANE		BIT(0)
+#define CCS_CSI_CPHY_LANE_MODE_CAPABILITY_2_LANE		BIT(1)
+#define CCS_CSI_CPHY_LANE_MODE_CAPABILITY_3_LANE		BIT(2)
+#define CCS_CSI_CPHY_LANE_MODE_CAPABILITY_4_LANE		BIT(3)
+#define CCS_CSI_CPHY_LANE_MODE_CAPABILITY_5_LANE		BIT(4)
+#define CCS_CSI_CPHY_LANE_MODE_CAPABILITY_6_LANE		BIT(5)
+#define CCS_CSI_CPHY_LANE_MODE_CAPABILITY_7_LANE		BIT(6)
+#define CCS_CSI_CPHY_LANE_MODE_CAPABILITY_8_LANE		BIT(7)
+#define CCS_R_EMB_DATA_CAPABILITY				0x1607
+#define CCS_EMB_DATA_CAPABILITY_TWO_BYTES_PER_RAW16		BIT(0)
+#define CCS_EMB_DATA_CAPABILITY_TWO_BYTES_PER_RAW20		BIT(1)
+#define CCS_EMB_DATA_CAPABILITY_TWO_BYTES_PER_RAW24		BIT(2)
+#define CCS_EMB_DATA_CAPABILITY_NO_ONE_BYTE_PER_RAW16		BIT(3)
+#define CCS_EMB_DATA_CAPABILITY_NO_ONE_BYTE_PER_RAW20		BIT(4)
+#define CCS_EMB_DATA_CAPABILITY_NO_ONE_BYTE_PER_RAW24		BIT(5)
+#define CCS_R_MAX_PER_LANE_BITRATE_LANE_D_MODE_MBPS(n)		((0x1608 | (CCS_FL_32BIT | CCS_FL_IREAL)) + ((n) < 4 ? (n) * 4 : 0x32 + ((n) - 4) * 4))
+#define CCS_LIM_MAX_PER_LANE_BITRATE_LANE_D_MODE_MBPS_MIN_N	0U
+#define CCS_LIM_MAX_PER_LANE_BITRATE_LANE_D_MODE_MBPS_MAX_N	7U
+#define CCS_R_TEMP_SENSOR_CAPABILITY				0x1618
+#define CCS_TEMP_SENSOR_CAPABILITY_SUPPORTED			BIT(0)
+#define CCS_TEMP_SENSOR_CAPABILITY_CCS_FORMAT			BIT(1)
+#define CCS_TEMP_SENSOR_CAPABILITY_RESET_0X80			BIT(2)
+#define CCS_R_MAX_PER_LANE_BITRATE_LANE_C_MODE_MBPS(n)		((0x161a | (CCS_FL_32BIT | CCS_FL_IREAL)) + ((n) < 4 ? (n) * 4 : 0x30 + ((n) - 4) * 4))
+#define CCS_LIM_MAX_PER_LANE_BITRATE_LANE_C_MODE_MBPS_MIN_N	0U
+#define CCS_LIM_MAX_PER_LANE_BITRATE_LANE_C_MODE_MBPS_MAX_N	7U
+#define CCS_R_DPHY_EQUALIZATION_CAPABILITY			0x162b
+#define CCS_DPHY_EQUALIZATION_CAPABILITY_EQUALIZATION_CTRL	BIT(0)
+#define CCS_DPHY_EQUALIZATION_CAPABILITY_EQ1			BIT(1)
+#define CCS_DPHY_EQUALIZATION_CAPABILITY_EQ2			BIT(2)
+#define CCS_R_CPHY_EQUALIZATION_CAPABILITY			0x162c
+#define CCS_CPHY_EQUALIZATION_CAPABILITY_EQUALIZATION_CTRL	BIT(0)
+#define CCS_R_DPHY_PREAMBLE_CAPABILITY				0x162d
+#define CCS_DPHY_PREAMBLE_CAPABILITY_PREAMBLE_SEQ_CTRL		BIT(0)
+#define CCS_R_DPHY_SSC_CAPABILITY				0x162e
+#define CCS_DPHY_SSC_CAPABILITY_SUPPORTED			BIT(0)
+#define CCS_R_CPHY_CALIBRATION_CAPABILITY			0x162f
+#define CCS_CPHY_CALIBRATION_CAPABILITY_MANUAL			BIT(0)
+#define CCS_CPHY_CALIBRATION_CAPABILITY_MANUAL_STREAMING	BIT(1)
+#define CCS_CPHY_CALIBRATION_CAPABILITY_FORMAT_1_CTRL		BIT(2)
+#define CCS_CPHY_CALIBRATION_CAPABILITY_FORMAT_2_CTRL		BIT(3)
+#define CCS_CPHY_CALIBRATION_CAPABILITY_FORMAT_3_CTRL		BIT(4)
+#define CCS_R_DPHY_CALIBRATION_CAPABILITY			0x1630
+#define CCS_DPHY_CALIBRATION_CAPABILITY_MANUAL			BIT(0)
+#define CCS_DPHY_CALIBRATION_CAPABILITY_MANUAL_STREAMING	BIT(1)
+#define CCS_DPHY_CALIBRATION_CAPABILITY_ALTERNATE_SEQ		BIT(2)
+#define CCS_R_PHY_CTRL_CAPABILITY_2				0x1631
+#define CCS_PHY_CTRL_CAPABILITY_2_TGR_LENGTH			BIT(0)
+#define CCS_PHY_CTRL_CAPABILITY_2_TGR_PREAMBLE_PROG_SEQ		BIT(1)
+#define CCS_PHY_CTRL_CAPABILITY_2_EXTRA_CPHY_MANUAL_TIMING	BIT(2)
+#define CCS_PHY_CTRL_CAPABILITY_2_CLOCK_BASED_MANUAL_CDPHY	BIT(3)
+#define CCS_PHY_CTRL_CAPABILITY_2_CLOCK_BASED_MANUAL_DPHY	BIT(4)
+#define CCS_PHY_CTRL_CAPABILITY_2_CLOCK_BASED_MANUAL_CPHY	BIT(5)
+#define CCS_PHY_CTRL_CAPABILITY_2_MANUAL_LP_DPHY		BIT(6)
+#define CCS_PHY_CTRL_CAPABILITY_2_MANUAL_LP_CPHY		BIT(7)
+#define CCS_R_LRTE_CPHY_CAPABILITY				0x1632
+#define CCS_LRTE_CPHY_CAPABILITY_PDQ_SHORT			BIT(0)
+#define CCS_LRTE_CPHY_CAPABILITY_SPACER_SHORT			BIT(1)
+#define CCS_LRTE_CPHY_CAPABILITY_PDQ_LONG			BIT(2)
+#define CCS_LRTE_CPHY_CAPABILITY_SPACER_LONG			BIT(3)
+#define CCS_LRTE_CPHY_CAPABILITY_SPACER_NO_PDQ			BIT(4)
+#define CCS_R_LRTE_DPHY_CAPABILITY				0x1633
+#define CCS_LRTE_DPHY_CAPABILITY_PDQ_SHORT_OPT1			BIT(0)
+#define CCS_LRTE_DPHY_CAPABILITY_SPACER_SHORT_OPT1		BIT(1)
+#define CCS_LRTE_DPHY_CAPABILITY_PDQ_LONG_OPT1			BIT(2)
+#define CCS_LRTE_DPHY_CAPABILITY_SPACER_LONG_OPT1		BIT(3)
+#define CCS_LRTE_DPHY_CAPABILITY_SPACER_SHORT_OPT2		BIT(4)
+#define CCS_LRTE_DPHY_CAPABILITY_SPACER_LONG_OPT2		BIT(5)
+#define CCS_LRTE_DPHY_CAPABILITY_SPACER_NO_PDQ_OPT1		BIT(6)
+#define CCS_LRTE_DPHY_CAPABILITY_SPACER_VARIABLE_OPT2		BIT(7)
+#define CCS_R_ALPS_CAPABILITY_DPHY				0x1634
+#define CCS_ALPS_CAPABILITY_DPHY_LVLP_NOT_SUPPORTED		0U
+#define CCS_ALPS_CAPABILITY_DPHY_LVLP_SUPPORTED			1U
+#define CCS_ALPS_CAPABILITY_DPHY_CONTROLLABLE_LVLP		2U
+#define CCS_R_ALPS_CAPABILITY_CPHY				0x1635
+#define CCS_ALPS_CAPABILITY_CPHY_LVLP_NOT_SUPPORTED		0U
+#define CCS_ALPS_CAPABILITY_CPHY_LVLP_SUPPORTED			1U
+#define CCS_ALPS_CAPABILITY_CPHY_CONTROLLABLE_LVLP		2U
+#define CCS_ALPS_CAPABILITY_CPHY_ALP_NOT_SUPPORTED		0xc
+#define CCS_ALPS_CAPABILITY_CPHY_ALP_SUPPORTED			0xd
+#define CCS_ALPS_CAPABILITY_CPHY_CONTROLLABLE_ALP		0xe
+#define CCS_R_SCRAMBLING_CAPABILITY				0x1636
+#define CCS_SCRAMBLING_CAPABILITY_SCRAMBLING_SUPPORTED		BIT(0)
+#define CCS_SCRAMBLING_CAPABILITY_MAX_SEEDS_PER_LANE_C_SHIFT	1U
+#define CCS_SCRAMBLING_CAPABILITY_MAX_SEEDS_PER_LANE_C_MASK	0x6
+#define CCS_SCRAMBLING_CAPABILITY_MAX_SEEDS_PER_LANE_C_1	0U
+#define CCS_SCRAMBLING_CAPABILITY_MAX_SEEDS_PER_LANE_C_4	3U
+#define CCS_SCRAMBLING_CAPABILITY_NUM_SEED_REGS_SHIFT		3U
+#define CCS_SCRAMBLING_CAPABILITY_NUM_SEED_REGS_MASK		0x38
+#define CCS_SCRAMBLING_CAPABILITY_NUM_SEED_REGS_0		0U
+#define CCS_SCRAMBLING_CAPABILITY_NUM_SEED_REGS_1		1U
+#define CCS_SCRAMBLING_CAPABILITY_NUM_SEED_REGS_4		4U
+#define CCS_SCRAMBLING_CAPABILITY_NUM_SEED_PER_LANE		BIT(6)
+#define CCS_R_DPHY_MANUAL_CONSTANT				0x1637
+#define CCS_R_CPHY_MANUAL_CONSTANT				0x1638
+#define CCS_R_CSI2_INTERFACE_CAPABILITY_MISC			0x1639
+#define CCS_CSI2_INTERFACE_CAPABILITY_MISC_EOTP_SHORT_PKT_OPT2	BIT(0)
+#define CCS_R_PHY_CTRL_CAPABILITY_3				0x165c
+#define CCS_PHY_CTRL_CAPABILITY_3_DPHY_TIMING_NOT_MULTIPLE	BIT(0)
+#define CCS_PHY_CTRL_CAPABILITY_3_DPHY_MIN_TIMING_VALUE_1	BIT(1)
+#define CCS_PHY_CTRL_CAPABILITY_3_TWAKEUP_SUPPORTED		BIT(2)
+#define CCS_PHY_CTRL_CAPABILITY_3_TINIT_SUPPORTED		BIT(3)
+#define CCS_PHY_CTRL_CAPABILITY_3_THS_EXIT_SUPPORTED		BIT(4)
+#define CCS_PHY_CTRL_CAPABILITY_3_CPHY_TIMING_NOT_MULTIPLE	BIT(5)
+#define CCS_PHY_CTRL_CAPABILITY_3_CPHY_MIN_TIMING_VALUE_1	BIT(6)
+#define CCS_R_DPHY_SF						0x165d
+#define CCS_R_CPHY_SF						0x165e
+#define CCS_CPHY_SF_TWAKEUP_SHIFT				0U
+#define CCS_CPHY_SF_TWAKEUP_MASK				0xf
+#define CCS_CPHY_SF_TINIT_SHIFT					4U
+#define CCS_CPHY_SF_TINIT_MASK					0xf0
+#define CCS_R_DPHY_LIMITS_1					0x165f
+#define CCS_DPHY_LIMITS_1_THS_PREPARE_SHIFT			0U
+#define CCS_DPHY_LIMITS_1_THS_PREPARE_MASK			0xf
+#define CCS_DPHY_LIMITS_1_THS_ZERO_SHIFT			4U
+#define CCS_DPHY_LIMITS_1_THS_ZERO_MASK				0xf0
+#define CCS_R_DPHY_LIMITS_2					0x1660
+#define CCS_DPHY_LIMITS_2_THS_TRAIL_SHIFT			0U
+#define CCS_DPHY_LIMITS_2_THS_TRAIL_MASK			0xf
+#define CCS_DPHY_LIMITS_2_TCLK_TRAIL_MIN_SHIFT			4U
+#define CCS_DPHY_LIMITS_2_TCLK_TRAIL_MIN_MASK			0xf0
+#define CCS_R_DPHY_LIMITS_3					0x1661
+#define CCS_DPHY_LIMITS_3_TCLK_PREPARE_SHIFT			0U
+#define CCS_DPHY_LIMITS_3_TCLK_PREPARE_MASK			0xf
+#define CCS_DPHY_LIMITS_3_TCLK_ZERO_SHIFT			4U
+#define CCS_DPHY_LIMITS_3_TCLK_ZERO_MASK			0xf0
+#define CCS_R_DPHY_LIMITS_4					0x1662
+#define CCS_DPHY_LIMITS_4_TCLK_POST_SHIFT			0U
+#define CCS_DPHY_LIMITS_4_TCLK_POST_MASK			0xf
+#define CCS_DPHY_LIMITS_4_TLPX_SHIFT				4U
+#define CCS_DPHY_LIMITS_4_TLPX_MASK				0xf0
+#define CCS_R_DPHY_LIMITS_5					0x1663
+#define CCS_DPHY_LIMITS_5_THS_EXIT_SHIFT			0U
+#define CCS_DPHY_LIMITS_5_THS_EXIT_MASK				0xf
+#define CCS_DPHY_LIMITS_5_TWAKEUP_SHIFT				4U
+#define CCS_DPHY_LIMITS_5_TWAKEUP_MASK				0xf0
+#define CCS_R_DPHY_LIMITS_6					0x1664
+#define CCS_DPHY_LIMITS_6_TINIT_SHIFT				0U
+#define CCS_DPHY_LIMITS_6_TINIT_MASK				0xf
+#define CCS_R_CPHY_LIMITS_1					0x1665
+#define CCS_CPHY_LIMITS_1_T3_PREPARE_MAX_SHIFT			0U
+#define CCS_CPHY_LIMITS_1_T3_PREPARE_MAX_MASK			0xf
+#define CCS_CPHY_LIMITS_1_T3_LPX_MAX_SHIFT			4U
+#define CCS_CPHY_LIMITS_1_T3_LPX_MAX_MASK			0xf0
+#define CCS_R_CPHY_LIMITS_2					0x1666
+#define CCS_CPHY_LIMITS_2_THS_EXIT_MAX_SHIFT			0U
+#define CCS_CPHY_LIMITS_2_THS_EXIT_MAX_MASK			0xf
+#define CCS_CPHY_LIMITS_2_TWAKEUP_MAX_SHIFT			4U
+#define CCS_CPHY_LIMITS_2_TWAKEUP_MAX_MASK			0xf0
+#define CCS_R_CPHY_LIMITS_3					0x1667
+#define CCS_CPHY_LIMITS_3_TINIT_MAX_SHIFT			0U
+#define CCS_CPHY_LIMITS_3_TINIT_MAX_MASK			0xf
+#define CCS_R_MIN_FRAME_LENGTH_LINES_BIN			(0x1700 | CCS_FL_16BIT)
+#define CCS_R_MAX_FRAME_LENGTH_LINES_BIN			(0x1702 | CCS_FL_16BIT)
+#define CCS_R_MIN_LINE_LENGTH_PCK_BIN				(0x1704 | CCS_FL_16BIT)
+#define CCS_R_MAX_LINE_LENGTH_PCK_BIN				(0x1706 | CCS_FL_16BIT)
+#define CCS_R_MIN_LINE_BLANKING_PCK_BIN				(0x1708 | CCS_FL_16BIT)
+#define CCS_R_FINE_INTEGRATION_TIME_MIN_BIN			(0x170a | CCS_FL_16BIT)
+#define CCS_R_FINE_INTEGRATION_TIME_MAX_MARGIN_BIN		(0x170c | CCS_FL_16BIT)
+#define CCS_R_BINNING_CAPABILITY				0x1710
+#define CCS_BINNING_CAPABILITY_UNSUPPORTED			0U
+#define CCS_BINNING_CAPABILITY_BINNING_THEN_SUBSAMPLING		1U
+#define CCS_BINNING_CAPABILITY_SUBSAMPLING_THEN_BINNING		2U
+#define CCS_R_BINNING_WEIGHTING_CAPABILITY			0x1711
+#define CCS_BINNING_WEIGHTING_CAPABILITY_AVERAGED		BIT(0)
+#define CCS_BINNING_WEIGHTING_CAPABILITY_SUMMED			BIT(1)
+#define CCS_BINNING_WEIGHTING_CAPABILITY_BAYER_CORRECTED	BIT(2)
+#define CCS_BINNING_WEIGHTING_CAPABILITY_MODULE_SPECIFIC_WEIGHT	BIT(3)
+#define CCS_R_BINNING_SUB_TYPES					0x1712
+#define CCS_R_BINNING_SUB_TYPE(n)				(0x1713 + (n))
+#define CCS_LIM_BINNING_SUB_TYPE_MIN_N				0U
+#define CCS_LIM_BINNING_SUB_TYPE_MAX_N				63U
+#define CCS_BINNING_SUB_TYPE_ROW_SHIFT				0U
+#define CCS_BINNING_SUB_TYPE_ROW_MASK				0xf
+#define CCS_BINNING_SUB_TYPE_COLUMN_SHIFT			4U
+#define CCS_BINNING_SUB_TYPE_COLUMN_MASK			0xf0
+#define CCS_R_BINNING_WEIGHTING_MONO_CAPABILITY			0x1771
+#define CCS_BINNING_WEIGHTING_MONO_CAPABILITY_AVERAGED		BIT(0)
+#define CCS_BINNING_WEIGHTING_MONO_CAPABILITY_SUMMED		BIT(1)
+#define CCS_BINNING_WEIGHTING_MONO_CAPABILITY_BAYER_CORRECTED	BIT(2)
+#define CCS_BINNING_WEIGHTING_MONO_CAPABILITY_MODULE_SPECIFIC_WEIGHT   BIT(3)
+#define CCS_R_BINNING_SUB_TYPES_MONO				0x1772
+#define CCS_R_BINNING_SUB_TYPE_MONO(n)				(0x1773 + (n))
+#define CCS_LIM_BINNING_SUB_TYPE_MONO_MIN_N			0U
+#define CCS_LIM_BINNING_SUB_TYPE_MONO_MAX_N			63U
+#define CCS_R_DATA_TRANSFER_IF_CAPABILITY			0x1800
+#define CCS_DATA_TRANSFER_IF_CAPABILITY_SUPPORTED		BIT(0)
+#define CCS_DATA_TRANSFER_IF_CAPABILITY_POLLING			BIT(2)
+#define CCS_R_SHADING_CORRECTION_CAPABILITY			0x1900
+#define CCS_SHADING_CORRECTION_CAPABILITY_COLOR_SHADING		BIT(0)
+#define CCS_SHADING_CORRECTION_CAPABILITY_LUMINANCE_CORRECTION	BIT(1)
+#define CCS_R_GREEN_IMBALANCE_CAPABILITY			0x1901
+#define CCS_GREEN_IMBALANCE_CAPABILITY_SUPPORTED		BIT(0)
+#define CCS_R_MODULE_SPECIFIC_CORRECTION_CAPABILITY		0x1903
+#define CCS_R_DEFECT_CORRECTION_CAPABILITY			(0x1904 | CCS_FL_16BIT)
+#define CCS_DEFECT_CORRECTION_CAPABILITY_MAPPED_DEFECT		BIT(0)
+#define CCS_DEFECT_CORRECTION_CAPABILITY_DYNAMIC_COUPLET	BIT(2)
+#define CCS_DEFECT_CORRECTION_CAPABILITY_DYNAMIC_SINGLE		BIT(5)
+#define CCS_DEFECT_CORRECTION_CAPABILITY_COMBINED_DYNAMIC	BIT(8)
+#define CCS_R_DEFECT_CORRECTION_CAPABILITY_2			(0x1906 | CCS_FL_16BIT)
+#define CCS_DEFECT_CORRECTION_CAPABILITY_2_DYNAMIC_TRIPLET	BIT(3)
+#define CCS_R_NF_CAPABILITY					0x1908
+#define CCS_NF_CAPABILITY_LUMA					BIT(0)
+#define CCS_NF_CAPABILITY_CHROMA				BIT(1)
+#define CCS_NF_CAPABILITY_COMBINED				BIT(2)
+#define CCS_R_OB_READOUT_CAPABILITY				0x1980
+#define CCS_OB_READOUT_CAPABILITY_CONTROLLABLE_READOUT		BIT(0)
+#define CCS_OB_READOUT_CAPABILITY_VISIBLE_PIXEL_READOUT		BIT(1)
+#define CCS_OB_READOUT_CAPABILITY_DIFFERENT_VC_READOUT		BIT(2)
+#define CCS_OB_READOUT_CAPABILITY_DIFFERENT_DT_READOUT		BIT(3)
+#define CCS_OB_READOUT_CAPABILITY_PROG_DATA_FORMAT		BIT(4)
+#define CCS_R_COLOR_FEEDBACK_CAPABILITY				0x1987
+#define CCS_COLOR_FEEDBACK_CAPABILITY_KELVIN			BIT(0)
+#define CCS_COLOR_FEEDBACK_CAPABILITY_AWB_GAIN			BIT(1)
+#define CCS_R_CFA_PATTERN_CAPABILITY				0x1990
+#define CCS_CFA_PATTERN_CAPABILITY_BAYER			0U
+#define CCS_CFA_PATTERN_CAPABILITY_MONOCHROME			1U
+#define CCS_CFA_PATTERN_CAPABILITY_4X4_QUAD_BAYER		2U
+#define CCS_CFA_PATTERN_CAPABILITY_VENDOR_SPECIFIC		3U
+#define CCS_R_CFA_PATTERN_CONVERSION_CAPABILITY			0x1991
+#define CCS_CFA_PATTERN_CONVERSION_CAPABILITY_BAYER		BIT(0)
+#define CCS_R_FLASH_MODE_CAPABILITY				0x1a02
+#define CCS_FLASH_MODE_CAPABILITY_SINGLE_STROBE			BIT(0)
+#define CCS_R_SA_STROBE_MODE_CAPABILITY				0x1a03
+#define CCS_SA_STROBE_MODE_CAPABILITY_FIXED_WIDTH		BIT(0)
+#define CCS_SA_STROBE_MODE_CAPABILITY_EDGE_CTRL			BIT(1)
+#define CCS_R_RESET_MAX_DELAY					0x1a10
+#define CCS_R_RESET_MIN_TIME					0x1a11
+#define CCS_R_PDAF_CAPABILITY_1					0x1b80
+#define CCS_PDAF_CAPABILITY_1_SUPPORTED				BIT(0)
+#define CCS_PDAF_CAPABILITY_1_PROCESSED_BOTTOM_EMBEDDED		BIT(1)
+#define CCS_PDAF_CAPABILITY_1_PROCESSED_INTERLEAVED		BIT(2)
+#define CCS_PDAF_CAPABILITY_1_RAW_BOTTOM_EMBEDDED		BIT(3)
+#define CCS_PDAF_CAPABILITY_1_RAW_INTERLEAVED			BIT(4)
+#define CCS_PDAF_CAPABILITY_1_VISIBLE_PDAF_CORRECTION		BIT(5)
+#define CCS_PDAF_CAPABILITY_1_VC_INTERLEAVING			BIT(6)
+#define CCS_PDAF_CAPABILITY_1_DT_INTERLEAVING			BIT(7)
+#define CCS_R_PDAF_CAPABILITY_2					0x1b81
+#define CCS_PDAF_CAPABILITY_2_ROI				BIT(0)
+#define CCS_PDAF_CAPABILITY_2_AFTER_DIGITAL_CROP		BIT(1)
+#define CCS_PDAF_CAPABILITY_2_CTRL_RETIMED			BIT(2)
+#define CCS_R_BRACKETING_LUT_CAPABILITY_1			0x1c00
+#define CCS_BRACKETING_LUT_CAPABILITY_1_COARSE_INTEGRATION	BIT(0)
+#define CCS_BRACKETING_LUT_CAPABILITY_1_GLOBAL_ANALOG_GAIN	BIT(1)
+#define CCS_BRACKETING_LUT_CAPABILITY_1_FLASH			BIT(4)
+#define CCS_BRACKETING_LUT_CAPABILITY_1_GLOBAL_DIGITAL_GAIN	BIT(5)
+#define CCS_BRACKETING_LUT_CAPABILITY_1_ALTERNATE_GLOBAL_ANALOG_GAIN   BIT(6)
+#define CCS_R_BRACKETING_LUT_CAPABILITY_2			0x1c01
+#define CCS_BRACKETING_LUT_CAPABILITY_2_SINGLE_BRACKETING_MODE	BIT(0)
+#define CCS_BRACKETING_LUT_CAPABILITY_2_LOOPED_BRACKETING_MODE	BIT(1)
+#define CCS_R_BRACKETING_LUT_SIZE				0x1c02
+
+#endif /* __CCS_REGS_H__ */
diff --git a/drivers/media/i2c/ccs/ccs.h b/drivers/media/i2c/ccs/ccs.h
new file mode 100644
index 000000000..a94c796ce
--- /dev/null
+++ b/drivers/media/i2c/ccs/ccs.h
@@ -0,0 +1,276 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * drivers/media/i2c/smiapp/ccs.h
+ *
+ * Generic driver for MIPI CCS/SMIA/SMIA++ compliant camera sensors
+ *
+ * Copyright (C) 2020 Intel Corporation
+ * Copyright (C) 2010--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@linux.intel.com>
+ */
+
+#ifndef __CCS_H__
+#define __CCS_H__
+
+#include <linux/mutex.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-subdev.h>
+
+#include "ccs-data.h"
+#include "ccs-limits.h"
+#include "ccs-quirk.h"
+#include "ccs-regs.h"
+#include "ccs-reg-access.h"
+#include "../ccs-pll.h"
+#include "smiapp-reg-defs.h"
+
+/*
+ * Standard SMIA++ constants
+ */
+#define SMIA_VERSION_1			10
+#define SMIAPP_VERSION_0_8		8 /* Draft 0.8 */
+#define SMIAPP_VERSION_0_9		9 /* Draft 0.9 */
+#define SMIAPP_VERSION_1		10
+
+#define SMIAPP_PROFILE_0		0
+#define SMIAPP_PROFILE_1		1
+#define SMIAPP_PROFILE_2		2
+
+#define SMIAPP_NVM_PAGE_SIZE		64	/* bytes */
+
+#define SMIAPP_RESET_DELAY_CLOCKS	2400
+#define SMIAPP_RESET_DELAY(clk)				\
+	(1000 +	(SMIAPP_RESET_DELAY_CLOCKS * 1000	\
+		 + (clk) / 1000 - 1) / ((clk) / 1000))
+
+#define CCS_COLOUR_COMPONENTS		4
+
+#define SMIAPP_NAME			"smiapp"
+#define CCS_NAME			"ccs"
+
+#define CCS_DFL_I2C_ADDR	(0x20 >> 1) /* Default I2C Address */
+#define CCS_ALT_I2C_ADDR	(0x6e >> 1) /* Alternate I2C Address */
+
+#define CCS_LIM(sensor, limit) \
+	ccs_get_limit(sensor, CCS_L_##limit, 0)
+
+#define CCS_LIM_AT(sensor, limit, offset)	\
+	ccs_get_limit(sensor, CCS_L_##limit, CCS_L_##limit##_OFFSET(offset))
+
+struct ccs_flash_strobe_parms {
+	u8 mode;
+	u32 strobe_width_high_us;
+	u16 strobe_delay;
+	u16 stobe_start_point;
+	u8 trigger;
+};
+
+struct ccs_hwconfig {
+	/*
+	 * Change the cci address if i2c_addr_alt is set.
+	 * Both default and alternate cci addr need to be present
+	 */
+	unsigned short i2c_addr_dfl;	/* Default i2c addr */
+	unsigned short i2c_addr_alt;	/* Alternate i2c addr */
+
+	u32 ext_clk;			/* sensor external clk */
+
+	unsigned int lanes;		/* Number of CSI-2 lanes */
+	u32 csi_signalling_mode;	/* CCS_CSI_SIGNALLING_MODE_* */
+	u64 *op_sys_clock;
+
+	struct ccs_flash_strobe_parms *strobe_setup;
+};
+
+struct ccs_quirk;
+
+#define CCS_MODULE_IDENT_FLAG_REV_LE		(1 << 0)
+
+struct ccs_module_ident {
+	u16 mipi_manufacturer_id;
+	u16 model_id;
+	u8 smia_manufacturer_id;
+	u8 revision_number_major;
+
+	u8 flags;
+
+	char *name;
+	const struct ccs_quirk *quirk;
+};
+
+struct ccs_module_info {
+	u32 smia_manufacturer_id;
+	u32 mipi_manufacturer_id;
+	u32 model_id;
+	u32 revision_number;
+
+	u32 module_year;
+	u32 module_month;
+	u32 module_day;
+
+	u32 sensor_smia_manufacturer_id;
+	u32 sensor_mipi_manufacturer_id;
+	u32 sensor_model_id;
+	u32 sensor_revision_number;
+	u32 sensor_firmware_version;
+
+	u32 smia_version;
+	u32 smiapp_version;
+	u32 ccs_version;
+
+	char *name;
+	const struct ccs_quirk *quirk;
+};
+
+#define CCS_IDENT_FQ(manufacturer, model, rev, fl, _name, _quirk)	\
+	{ .smia_manufacturer_id = manufacturer,				\
+	  .model_id = model,						\
+	  .revision_number_major = rev,					\
+	  .flags = fl,							\
+	  .name = _name,						\
+	  .quirk = _quirk, }
+
+#define CCS_IDENT_LQ(manufacturer, model, rev, _name, _quirk)	\
+	{ .smia_manufacturer_id = manufacturer,				\
+	  .model_id = model,						\
+	  .revision_number_major = rev,					\
+	  .flags = CCS_MODULE_IDENT_FLAG_REV_LE,			\
+	  .name = _name,						\
+	  .quirk = _quirk, }
+
+#define CCS_IDENT_L(manufacturer, model, rev, _name)			\
+	{ .smia_manufacturer_id = manufacturer,				\
+	  .model_id = model,						\
+	  .revision_number_major = rev,					\
+	  .flags = CCS_MODULE_IDENT_FLAG_REV_LE,			\
+	  .name = _name, }
+
+#define CCS_IDENT_Q(manufacturer, model, rev, _name, _quirk)		\
+	{ .smia_manufacturer_id = manufacturer,				\
+	  .model_id = model,						\
+	  .revision_number_major = rev,					\
+	  .flags = 0,							\
+	  .name = _name,						\
+	  .quirk = _quirk, }
+
+#define CCS_IDENT(manufacturer, model, rev, _name)			\
+	{ .smia_manufacturer_id = manufacturer,				\
+	  .model_id = model,						\
+	  .revision_number_major = rev,					\
+	  .flags = 0,							\
+	  .name = _name, }
+
+struct ccs_csi_data_format {
+	u32 code;
+	u8 width;
+	u8 compressed;
+	u8 pixel_order;
+};
+
+#define CCS_SUBDEVS			3
+
+#define CCS_PA_PAD_SRC			0
+#define CCS_PAD_SINK			0
+#define CCS_PAD_SRC			1
+#define CCS_PADS			2
+
+struct ccs_binning_subtype {
+	u8 horizontal:4;
+	u8 vertical:4;
+} __packed;
+
+struct ccs_subdev {
+	struct v4l2_subdev sd;
+	struct media_pad pads[CCS_PADS];
+	struct v4l2_rect sink_fmt;
+	struct v4l2_rect crop[CCS_PADS];
+	struct v4l2_rect compose; /* compose on sink */
+	unsigned short sink_pad;
+	unsigned short source_pad;
+	int npads;
+	struct ccs_sensor *sensor;
+	struct v4l2_ctrl_handler ctrl_handler;
+};
+
+/*
+ * struct ccs_sensor - Main device structure
+ */
+struct ccs_sensor {
+	/*
+	 * "mutex" is used to serialise access to all fields here
+	 * except v4l2_ctrls at the end of the struct. "mutex" is also
+	 * used to serialise access to file handle specific
+	 * information.
+	 */
+	struct mutex mutex;
+	struct ccs_subdev ssds[CCS_SUBDEVS];
+	u32 ssds_used;
+	struct ccs_subdev *src;
+	struct ccs_subdev *binner;
+	struct ccs_subdev *scaler;
+	struct ccs_subdev *pixel_array;
+	struct ccs_hwconfig hwcfg;
+	struct regulator_bulk_data *regulators;
+	struct clk *ext_clk;
+	struct gpio_desc *xshutdown;
+	struct gpio_desc *reset;
+	void *ccs_limits;
+	u8 nbinning_subtypes;
+	struct ccs_binning_subtype binning_subtypes[CCS_LIM_BINNING_SUB_TYPE_MAX_N + 1];
+	u32 mbus_frame_fmts;
+	const struct ccs_csi_data_format *csi_format;
+	const struct ccs_csi_data_format *internal_csi_format;
+	u32 default_mbus_frame_fmts;
+	int default_pixel_order;
+	struct ccs_data_container sdata, mdata;
+
+	u8 binning_horizontal;
+	u8 binning_vertical;
+
+	u8 scale_m;
+	u8 scaling_mode;
+
+	u8 frame_skip;
+	u16 embedded_start; /* embedded data start line */
+	u16 embedded_end;
+	u16 image_start; /* image data start line */
+	u16 visible_pixel_start; /* start pixel of the visible image */
+
+	bool streaming;
+	bool dev_init_done;
+	u8 compressed_min_bpp;
+
+	struct ccs_module_info minfo;
+
+	struct ccs_pll pll;
+
+	/* Is a default format supported for a given BPP? */
+	unsigned long *valid_link_freqs;
+
+	/* Pixel array controls */
+	struct v4l2_ctrl *exposure;
+	struct v4l2_ctrl *hflip;
+	struct v4l2_ctrl *vflip;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *pixel_rate_parray;
+	struct v4l2_ctrl *luminance_level;
+	/* src controls */
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *pixel_rate_csi;
+	/* test pattern colour components */
+	struct v4l2_ctrl *test_data[CCS_COLOUR_COMPONENTS];
+};
+
+#define to_ccs_subdev(_sd)				\
+	container_of(_sd, struct ccs_subdev, sd)
+
+#define to_ccs_sensor(_sd)	\
+	(to_ccs_subdev(_sd)->sensor)
+
+void ccs_replace_limit(struct ccs_sensor *sensor,
+		       unsigned int limit, unsigned int offset, u32 val);
+u32 ccs_get_limit(struct ccs_sensor *sensor, unsigned int limit,
+		  unsigned int offset);
+
+#endif /* __CCS_H__ */
diff --git a/drivers/media/i2c/ccs/smiapp-reg-defs.h b/drivers/media/i2c/ccs/smiapp-reg-defs.h
new file mode 100644
index 000000000..177e3e512
--- /dev/null
+++ b/drivers/media/i2c/ccs/smiapp-reg-defs.h
@@ -0,0 +1,582 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * drivers/media/i2c/smiapp/smiapp-reg-defs.h
+ *
+ * Generic driver for MIPI CCS/SMIA/SMIA++ compliant camera sensors
+ *
+ * Copyright (C) 2020 Intel Corporation
+ * Copyright (C) 2011--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@iki.fi>
+ */
+
+#ifndef __SMIAPP_REG_DEFS_H__
+#define __SMIAPP_REG_DEFS_H__
+
+/* Register addresses */
+#define SMIAPP_REG_U16_MODEL_ID					(0x0000 | CCS_FL_16BIT)
+#define SMIAPP_REG_U8_REVISION_NUMBER_MAJOR			0x0002
+#define SMIAPP_REG_U8_MANUFACTURER_ID				0x0003
+#define SMIAPP_REG_U8_SMIA_VERSION				0x0004
+#define SMIAPP_REG_U8_FRAME_COUNT				0x0005
+#define SMIAPP_REG_U8_PIXEL_ORDER				0x0006
+#define SMIAPP_REG_U16_DATA_PEDESTAL				(0x0008 | CCS_FL_16BIT)
+#define SMIAPP_REG_U8_PIXEL_DEPTH				0x000c
+#define SMIAPP_REG_U8_REVISION_NUMBER_MINOR			0x0010
+#define SMIAPP_REG_U8_SMIAPP_VERSION				0x0011
+#define SMIAPP_REG_U8_MODULE_DATE_YEAR				0x0012
+#define SMIAPP_REG_U8_MODULE_DATE_MONTH				0x0013
+#define SMIAPP_REG_U8_MODULE_DATE_DAY				0x0014
+#define SMIAPP_REG_U8_MODULE_DATE_PHASE				0x0015
+#define SMIAPP_REG_U16_SENSOR_MODEL_ID				(0x0016 | CCS_FL_16BIT)
+#define SMIAPP_REG_U8_SENSOR_REVISION_NUMBER			0x0018
+#define SMIAPP_REG_U8_SENSOR_MANUFACTURER_ID			0x0019
+#define SMIAPP_REG_U8_SENSOR_FIRMWARE_VERSION			0x001a
+#define SMIAPP_REG_U32_SERIAL_NUMBER				(0x001c | CCS_FL_32BIT)
+#define SMIAPP_REG_U8_FRAME_FORMAT_MODEL_TYPE			0x0040
+#define SMIAPP_REG_U8_FRAME_FORMAT_MODEL_SUBTYPE		0x0041
+#define SMIAPP_REG_U16_FRAME_FORMAT_DESCRIPTOR_2(n)		((0x0042 + ((n) << 1)) | CCS_FL_16BIT) /* 0 <= n <= 14 */
+#define SMIAPP_REG_U32_FRAME_FORMAT_DESCRIPTOR_4(n)		((0x0060 + ((n) << 2)) | CCS_FL_32BIT) /* 0 <= n <= 7 */
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CAPABILITY			(0x0080 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_MIN			(0x0084 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_MAX			(0x0086 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_STEP			(0x0088 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_TYPE			(0x008a | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_M0				(0x008c | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_C0				(0x008e | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_M1				(0x0090 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_C1				(0x0092 | CCS_FL_16BIT)
+#define SMIAPP_REG_U8_DATA_FORMAT_MODEL_TYPE			0x00c0
+#define SMIAPP_REG_U8_DATA_FORMAT_MODEL_SUBTYPE			0x00c1
+#define SMIAPP_REG_U16_DATA_FORMAT_DESCRIPTOR(n)		((0x00c2 + ((n) << 1)) | CCS_FL_16BIT)
+#define SMIAPP_REG_U8_MODE_SELECT				0x0100
+#define SMIAPP_REG_U8_IMAGE_ORIENTATION				0x0101
+#define SMIAPP_REG_U8_SOFTWARE_RESET				0x0103
+#define SMIAPP_REG_U8_GROUPED_PARAMETER_HOLD			0x0104
+#define SMIAPP_REG_U8_MASK_CORRUPTED_FRAMES			0x0105
+#define SMIAPP_REG_U8_FAST_STANDBY_CTRL				0x0106
+#define SMIAPP_REG_U8_CCI_ADDRESS_CONTROL			0x0107
+#define SMIAPP_REG_U8_2ND_CCI_IF_CONTROL			0x0108
+#define SMIAPP_REG_U8_2ND_CCI_ADDRESS_CONTROL			0x0109
+#define SMIAPP_REG_U8_CSI_CHANNEL_IDENTIFIER			0x0110
+#define SMIAPP_REG_U8_CSI_SIGNALLING_MODE			0x0111
+#define SMIAPP_REG_U16_CSI_DATA_FORMAT				(0x0112 | CCS_FL_16BIT)
+#define SMIAPP_REG_U8_CSI_LANE_MODE				0x0114
+#define SMIAPP_REG_U8_CSI2_10_TO_8_DT				0x0115
+#define SMIAPP_REG_U8_CSI2_10_TO_7_DT				0x0116
+#define SMIAPP_REG_U8_CSI2_10_TO_6_DT				0x0117
+#define SMIAPP_REG_U8_CSI2_12_TO_8_DT				0x0118
+#define SMIAPP_REG_U8_CSI2_12_TO_7_DT				0x0119
+#define SMIAPP_REG_U8_CSI2_12_TO_6_DT				0x011a
+#define SMIAPP_REG_U8_CSI2_14_TO_10_DT				0x011b
+#define SMIAPP_REG_U8_CSI2_14_TO_8_DT				0x011c
+#define SMIAPP_REG_U8_CSI2_16_TO_10_DT				0x011d
+#define SMIAPP_REG_U8_CSI2_16_TO_8_DT				0x011e
+#define SMIAPP_REG_U8_GAIN_MODE					0x0120
+#define SMIAPP_REG_U16_VANA_VOLTAGE				(0x0130 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_VDIG_VOLTAGE				(0x0132 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_VIO_VOLTAGE				(0x0134 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_EXTCLK_FREQUENCY_MHZ			(0x0136 | CCS_FL_16BIT)
+#define SMIAPP_REG_U8_TEMP_SENSOR_CONTROL			0x0138
+#define SMIAPP_REG_U8_TEMP_SENSOR_MODE				0x0139
+#define SMIAPP_REG_U8_TEMP_SENSOR_OUTPUT			0x013a
+#define SMIAPP_REG_U16_FINE_INTEGRATION_TIME			(0x0200 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_COARSE_INTEGRATION_TIME			(0x0202 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_GLOBAL		(0x0204 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_GREENR		(0x0206 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_RED			(0x0208 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_BLUE			(0x020a | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_GREENB		(0x020c | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_DIGITAL_GAIN_GREENR			(0x020e | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_DIGITAL_GAIN_RED				(0x0210 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_DIGITAL_GAIN_BLUE			(0x0212 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_DIGITAL_GAIN_GREENB			(0x0214 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_VT_PIX_CLK_DIV				(0x0300 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_VT_SYS_CLK_DIV				(0x0302 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_PRE_PLL_CLK_DIV				(0x0304 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_PLL_MULTIPLIER				(0x0306 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_OP_PIX_CLK_DIV				(0x0308 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_OP_SYS_CLK_DIV				(0x030a | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_FRAME_LENGTH_LINES			(0x0340 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_LINE_LENGTH_PCK				(0x0342 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_X_ADDR_START				(0x0344 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_Y_ADDR_START				(0x0346 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_X_ADDR_END				(0x0348 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_Y_ADDR_END				(0x034a | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_X_OUTPUT_SIZE				(0x034c | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_Y_OUTPUT_SIZE				(0x034e | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_X_EVEN_INC				(0x0380 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_X_ODD_INC				(0x0382 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_Y_EVEN_INC				(0x0384 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_Y_ODD_INC				(0x0386 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_SCALING_MODE				(0x0400 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_SPATIAL_SAMPLING				(0x0402 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_SCALE_M					(0x0404 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_SCALE_N					(0x0406 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_DIGITAL_CROP_X_OFFSET			(0x0408 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_DIGITAL_CROP_Y_OFFSET			(0x040a | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_DIGITAL_CROP_IMAGE_WIDTH			(0x040c | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_DIGITAL_CROP_IMAGE_HEIGHT		(0x040e | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_COMPRESSION_MODE				(0x0500 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_TEST_PATTERN_MODE			(0x0600 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_TEST_DATA_RED				(0x0602 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_TEST_DATA_GREENR				(0x0604 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_TEST_DATA_BLUE				(0x0606 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_TEST_DATA_GREENB				(0x0608 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_HORIZONTAL_CURSOR_WIDTH			(0x060a | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_HORIZONTAL_CURSOR_POSITION		(0x060c | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_VERTICAL_CURSOR_WIDTH			(0x060e | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_VERTICAL_CURSOR_POSITION			(0x0610 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_FIFO_WATER_MARK_PIXELS			(0x0700 | CCS_FL_16BIT)
+#define SMIAPP_REG_U8_TCLK_POST					0x0800
+#define SMIAPP_REG_U8_THS_PREPARE				0x0801
+#define SMIAPP_REG_U8_THS_ZERO_MIN				0x0802
+#define SMIAPP_REG_U8_THS_TRAIL					0x0803
+#define SMIAPP_REG_U8_TCLK_TRAIL_MIN				0x0804
+#define SMIAPP_REG_U8_TCLK_PREPARE				0x0805
+#define SMIAPP_REG_U8_TCLK_ZERO					0x0806
+#define SMIAPP_REG_U8_TLPX					0x0807
+#define SMIAPP_REG_U8_DPHY_CTRL					0x0808
+#define SMIAPP_REG_U32_REQUESTED_LINK_BIT_RATE_MBPS		(0x0820 | CCS_FL_32BIT)
+#define SMIAPP_REG_U8_BINNING_MODE				0x0900
+#define SMIAPP_REG_U8_BINNING_TYPE				0x0901
+#define SMIAPP_REG_U8_BINNING_WEIGHTING				0x0902
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_CTRL			0x0a00
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_STATUS			0x0a01
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_PAGE_SELECT		0x0a02
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_0			0x0a04
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_1			0x0a05
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_2			0x0a06
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_3			0x0a07
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_4			0x0a08
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_5			0x0a09
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_12		0x0a10
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_13		0x0a11
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_14		0x0a12
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_15		0x0a13
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_16		0x0a14
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_17		0x0a15
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_18		0x0a16
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_19		0x0a17
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_20		0x0a18
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_21		0x0a19
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_22		0x0a1a
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_23		0x0a1b
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_24		0x0a1c
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_25		0x0a1d
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_26		0x0a1e
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_27		0x0a1f
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_28		0x0a20
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_29		0x0a21
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_30		0x0a22
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_31		0x0a23
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_32		0x0a24
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_33		0x0a25
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_34		0x0a26
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_35		0x0a27
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_36		0x0a28
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_37		0x0a29
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_38		0x0a2a
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_39		0x0a2b
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_40		0x0a2c
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_41		0x0a2d
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_42		0x0a2e
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_43		0x0a2f
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_44		0x0a30
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_45		0x0a31
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_46		0x0a32
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_47		0x0a33
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_48		0x0a34
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_49		0x0a35
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_50		0x0a36
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_51		0x0a37
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_52		0x0a38
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_53		0x0a39
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_54		0x0a3a
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_55		0x0a3b
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_56		0x0a3c
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_57		0x0a3d
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_58		0x0a3e
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_59		0x0a3f
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_60		0x0a40
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_61		0x0a41
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_62		0x0a42
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_63		0x0a43
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_CTRL			0x0a44
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_STATUS			0x0a45
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_PAGE_SELECT		0x0a46
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_0			0x0a48
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_1			0x0a49
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_2			0x0a4a
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_3			0x0a4b
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_4			0x0a4c
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_5			0x0a4d
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_6			0x0a4e
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_7			0x0a4f
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_8			0x0a50
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_9			0x0a51
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_10		0x0a52
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_11		0x0a53
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_12		0x0a54
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_13		0x0a55
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_14		0x0a56
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_15		0x0a57
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_16		0x0a58
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_17		0x0a59
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_18		0x0a5a
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_19		0x0a5b
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_20		0x0a5c
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_21		0x0a5d
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_22		0x0a5e
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_23		0x0a5f
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_24		0x0a60
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_25		0x0a61
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_26		0x0a62
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_27		0x0a63
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_28		0x0a64
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_29		0x0a65
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_30		0x0a66
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_31		0x0a67
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_32		0x0a68
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_33		0x0a69
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_34		0x0a6a
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_35		0x0a6b
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_36		0x0a6c
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_37		0x0a6d
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_38		0x0a6e
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_39		0x0a6f
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_40		0x0a70
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_41		0x0a71
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_42		0x0a72
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_43		0x0a73
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_44		0x0a74
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_45		0x0a75
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_46		0x0a76
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_47		0x0a77
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_48		0x0a78
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_49		0x0a79
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_50		0x0a7a
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_51		0x0a7b
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_52		0x0a7c
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_53		0x0a7d
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_54		0x0a7e
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_55		0x0a7f
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_56		0x0a80
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_57		0x0a81
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_58		0x0a82
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_59		0x0a83
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_60		0x0a84
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_61		0x0a85
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_62		0x0a86
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_63		0x0a87
+#define SMIAPP_REG_U8_SHADING_CORRECTION_ENABLE			0x0b00
+#define SMIAPP_REG_U8_LUMINANCE_CORRECTION_LEVEL		0x0b01
+#define SMIAPP_REG_U8_GREEN_IMBALANCE_FILTER_ENABLE		0x0b02
+#define SMIAPP_REG_U8_GREEN_IMBALANCE_FILTER_WEIGHT		0x0b03
+#define SMIAPP_REG_U8_BLACK_LEVEL_CORRECTION_ENABLE		0x0b04
+#define SMIAPP_REG_U8_MAPPED_COUPLET_CORRECT_ENABLE		0x0b05
+#define SMIAPP_REG_U8_SINGLE_DEFECT_CORRECT_ENABLE		0x0b06
+#define SMIAPP_REG_U8_SINGLE_DEFECT_CORRECT_WEIGHT		0x0b07
+#define SMIAPP_REG_U8_DYNAMIC_COUPLET_CORRECT_ENABLE		0x0b08
+#define SMIAPP_REG_U8_DYNAMIC_COUPLET_CORRECT_WEIGHT		0x0b09
+#define SMIAPP_REG_U8_COMBINED_DEFECT_CORRECT_ENABLE		0x0b0a
+#define SMIAPP_REG_U8_COMBINED_DEFECT_CORRECT_WEIGHT		0x0b0b
+#define SMIAPP_REG_U8_MODULE_SPECIFIC_CORRECTION_ENABLE		0x0b0c
+#define SMIAPP_REG_U8_MODULE_SPECIFIC_CORRECTION_WEIGHT		0x0b0d
+#define SMIAPP_REG_U8_MAPPED_LINE_DEFECT_CORRECT_ENABLE		0x0b0e
+#define SMIAPP_REG_U8_MAPPED_LINE_DEFECT_CORRECT_ADJUST		0x0b0f
+#define SMIAPP_REG_U8_MAPPED_COUPLET_CORRECT_ADJUST		0x0b10
+#define SMIAPP_REG_U8_MAPPED_TRIPLET_DEFECT_CORRECT_ENABLE	0x0b11
+#define SMIAPP_REG_U8_MAPPED_TRIPLET_DEFECT_CORRECT_ADJUST	0x0b12
+#define SMIAPP_REG_U8_DYNAMIC_TRIPLET_DEFECT_CORRECT_ENABLE	0x0b13
+#define SMIAPP_REG_U8_DYNAMIC_TRIPLET_DEFECT_CORRECT_ADJUST	0x0b14
+#define SMIAPP_REG_U8_DYNAMIC_LINE_DEFECT_CORRECT_ENABLE	0x0b15
+#define SMIAPP_REG_U8_DYNAMIC_LINE_DEFECT_CORRECT_ADJUST	0x0b16
+#define SMIAPP_REG_U8_EDOF_MODE					0x0b80
+#define SMIAPP_REG_U8_SHARPNESS					0x0b83
+#define SMIAPP_REG_U8_DENOISING					0x0b84
+#define SMIAPP_REG_U8_MODULE_SPECIFIC				0x0b85
+#define SMIAPP_REG_U16_DEPTH_OF_FIELD				(0x0b86 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_FOCUS_DISTANCE				(0x0b88 | CCS_FL_16BIT)
+#define SMIAPP_REG_U8_ESTIMATION_MODE_CTRL			0x0b8a
+#define SMIAPP_REG_U16_COLOUR_TEMPERATURE			(0x0b8c | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_ABSOLUTE_GAIN_GREENR			(0x0b8e | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_ABSOLUTE_GAIN_RED			(0x0b90 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_ABSOLUTE_GAIN_BLUE			(0x0b92 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_ABSOLUTE_GAIN_GREENB			(0x0b94 | CCS_FL_16BIT)
+#define SMIAPP_REG_U8_ESTIMATION_ZONE_MODE			0x0bc0
+#define SMIAPP_REG_U16_FIXED_ZONE_WEIGHTING			(0x0bc2 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_CUSTOM_ZONE_X_START			(0x0bc4 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_CUSTOM_ZONE_Y_START			(0x0bc6 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_CUSTOM_ZONE_WIDTH			(0x0bc8 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_CUSTOM_ZONE_HEIGHT			(0x0bca | CCS_FL_16BIT)
+#define SMIAPP_REG_U8_GLOBAL_RESET_CTRL1			0x0c00
+#define SMIAPP_REG_U8_GLOBAL_RESET_CTRL2			0x0c01
+#define SMIAPP_REG_U8_GLOBAL_RESET_MODE_CONFIG_1		0x0c02
+#define SMIAPP_REG_U8_GLOBAL_RESET_MODE_CONFIG_2		0x0c03
+#define SMIAPP_REG_U16_TRDY_CTRL				(0x0c04 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_TRDOUT_CTRL				(0x0c06 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_TSHUTTER_STROBE_DELAY_CTRL		(0x0c08 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_TSHUTTER_STROBE_WIDTH_CTRL		(0x0c0a | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_TFLASH_STROBE_DELAY_CTRL			(0x0c0c | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_TFLASH_STROBE_WIDTH_HIGH_CTRL		(0x0c0e | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_TGRST_INTERVAL_CTRL			(0x0c10 | CCS_FL_16BIT)
+#define SMIAPP_REG_U8_FLASH_STROBE_ADJUSTMENT			0x0c12
+#define SMIAPP_REG_U16_FLASH_STROBE_START_POINT			(0x0c14 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_TFLASH_STROBE_DELAY_RS_CTRL		(0x0c16 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_TFLASH_STROBE_WIDTH_HIGH_RS_CTRL		(0x0c18 | CCS_FL_16BIT)
+#define SMIAPP_REG_U8_FLASH_MODE_RS				0x0c1a
+#define SMIAPP_REG_U8_FLASH_TRIGGER_RS				0x0c1b
+#define SMIAPP_REG_U8_FLASH_STATUS				0x0c1c
+#define SMIAPP_REG_U8_SA_STROBE_MODE				0x0c1d
+#define SMIAPP_REG_U16_SA_STROBE_START_POINT			(0x0c1e | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_TSA_STROBE_DELAY_CTRL			(0x0c20 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_TSA_STROBE_WIDTH_CTRL			(0x0c22 | CCS_FL_16BIT)
+#define SMIAPP_REG_U8_SA_STROBE_TRIGGER				0x0c24
+#define SMIAPP_REG_U8_SPECIAL_ACTUATOR_STATUS			0x0c25
+#define SMIAPP_REG_U16_TFLASH_STROBE_WIDTH2_HIGH_RS_CTRL	(0x0c26 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_TFLASH_STROBE_WIDTH_LOW_RS_CTRL		(0x0c28 | CCS_FL_16BIT)
+#define SMIAPP_REG_U8_TFLASH_STROBE_COUNT_RS_CTRL		0x0c2a
+#define SMIAPP_REG_U8_TFLASH_STROBE_COUNT_CTRL			0x0c2b
+#define SMIAPP_REG_U16_TFLASH_STROBE_WIDTH2_HIGH_CTRL		(0x0c2c | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_TFLASH_STROBE_WIDTH_LOW_CTRL		(0x0c2e | CCS_FL_16BIT)
+#define SMIAPP_REG_U8_LOW_LEVEL_CTRL				0x0c80
+#define SMIAPP_REG_U16_MAIN_TRIGGER_REF_POINT			(0x0c82 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_MAIN_TRIGGER_T3				(0x0c84 | CCS_FL_16BIT)
+#define SMIAPP_REG_U8_MAIN_TRIGGER_COUNT			0x0c86
+#define SMIAPP_REG_U16_PHASE1_TRIGGER_T3			(0x0c88 | CCS_FL_16BIT)
+#define SMIAPP_REG_U8_PHASE1_TRIGGER_COUNT			0x0c8a
+#define SMIAPP_REG_U16_PHASE2_TRIGGER_T3			(0x0c8c | CCS_FL_16BIT)
+#define SMIAPP_REG_U8_PHASE2_TRIGGER_COUNT			0x0c8e
+#define SMIAPP_REG_U8_MECH_SHUTTER_CTRL				0x0d00
+#define SMIAPP_REG_U8_OPERATION_MODE				0x0d01
+#define SMIAPP_REG_U8_ACT_STATE1				0x0d02
+#define SMIAPP_REG_U8_ACT_STATE2				0x0d03
+#define SMIAPP_REG_U16_FOCUS_CHANGE				(0x0d80 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_FOCUS_CHANGE_CONTROL			(0x0d82 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_FOCUS_CHANGE_NUMBER_PHASE1		(0x0d84 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_FOCUS_CHANGE_NUMBER_PHASE2		(0x0d86 | CCS_FL_16BIT)
+#define SMIAPP_REG_U8_STROBE_COUNT_PHASE1			0x0d88
+#define SMIAPP_REG_U8_STROBE_COUNT_PHASE2			0x0d89
+#define SMIAPP_REG_U8_POSITION					0x0d8a
+#define SMIAPP_REG_U8_BRACKETING_LUT_CONTROL			0x0e00
+#define SMIAPP_REG_U8_BRACKETING_LUT_MODE			0x0e01
+#define SMIAPP_REG_U8_BRACKETING_LUT_ENTRY_CONTROL		0x0e02
+#define SMIAPP_REG_U8_LUT_PARAMETERS_START			0x0e10
+#define SMIAPP_REG_U8_LUT_PARAMETERS_END			0x0eff
+#define SMIAPP_REG_U16_INTEGRATION_TIME_CAPABILITY		(0x1000 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_COARSE_INTEGRATION_TIME_MIN		(0x1004 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_COARSE_INTEGRATION_TIME_MAX_MARGIN	(0x1006 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_FINE_INTEGRATION_TIME_MIN		(0x1008 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_FINE_INTEGRATION_TIME_MAX_MARGIN		(0x100a | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_DIGITAL_GAIN_CAPABILITY			(0x1080 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_DIGITAL_GAIN_MIN				(0x1084 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_DIGITAL_GAIN_MAX				(0x1086 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_DIGITAL_GAIN_STEP_SIZE			(0x1088 | CCS_FL_16BIT)
+#define SMIAPP_REG_F32_MIN_EXT_CLK_FREQ_HZ			(0x1100 | CCS_FL_FLOAT_IREAL | CCS_FL_32BIT)
+#define SMIAPP_REG_F32_MAX_EXT_CLK_FREQ_HZ			(0x1104 | CCS_FL_FLOAT_IREAL | CCS_FL_32BIT)
+#define SMIAPP_REG_U16_MIN_PRE_PLL_CLK_DIV			(0x1108 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_MAX_PRE_PLL_CLK_DIV			(0x110a | CCS_FL_16BIT)
+#define SMIAPP_REG_F32_MIN_PLL_IP_FREQ_HZ			(0x110c | CCS_FL_FLOAT_IREAL | CCS_FL_32BIT)
+#define SMIAPP_REG_F32_MAX_PLL_IP_FREQ_HZ			(0x1110 | CCS_FL_FLOAT_IREAL | CCS_FL_32BIT)
+#define SMIAPP_REG_U16_MIN_PLL_MULTIPLIER			(0x1114 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_MAX_PLL_MULTIPLIER			(0x1116 | CCS_FL_16BIT)
+#define SMIAPP_REG_F32_MIN_PLL_OP_FREQ_HZ			(0x1118 | CCS_FL_FLOAT_IREAL | CCS_FL_32BIT)
+#define SMIAPP_REG_F32_MAX_PLL_OP_FREQ_HZ			(0x111c | CCS_FL_FLOAT_IREAL | CCS_FL_32BIT)
+#define SMIAPP_REG_U16_MIN_VT_SYS_CLK_DIV			(0x1120 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_MAX_VT_SYS_CLK_DIV			(0x1122 | CCS_FL_16BIT)
+#define SMIAPP_REG_F32_MIN_VT_SYS_CLK_FREQ_HZ			(0x1124 | CCS_FL_FLOAT_IREAL | CCS_FL_32BIT)
+#define SMIAPP_REG_F32_MAX_VT_SYS_CLK_FREQ_HZ			(0x1128 | CCS_FL_FLOAT_IREAL | CCS_FL_32BIT)
+#define SMIAPP_REG_F32_MIN_VT_PIX_CLK_FREQ_HZ			(0x112c | CCS_FL_FLOAT_IREAL | CCS_FL_32BIT)
+#define SMIAPP_REG_F32_MAX_VT_PIX_CLK_FREQ_HZ			(0x1130 | CCS_FL_FLOAT_IREAL | CCS_FL_32BIT)
+#define SMIAPP_REG_U16_MIN_VT_PIX_CLK_DIV			(0x1134 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_MAX_VT_PIX_CLK_DIV			(0x1136 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_MIN_FRAME_LENGTH_LINES			(0x1140 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_MAX_FRAME_LENGTH_LINES			(0x1142 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_MIN_LINE_LENGTH_PCK			(0x1144 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_MAX_LINE_LENGTH_PCK			(0x1146 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_MIN_LINE_BLANKING_PCK			(0x1148 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_MIN_FRAME_BLANKING_LINES			(0x114a | CCS_FL_16BIT)
+#define SMIAPP_REG_U8_MIN_LINE_LENGTH_PCK_STEP_SIZE		0x114c
+#define SMIAPP_REG_U16_MIN_OP_SYS_CLK_DIV			(0x1160 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_MAX_OP_SYS_CLK_DIV			(0x1162 | CCS_FL_16BIT)
+#define SMIAPP_REG_F32_MIN_OP_SYS_CLK_FREQ_HZ			(0x1164 | CCS_FL_FLOAT_IREAL | CCS_FL_32BIT)
+#define SMIAPP_REG_F32_MAX_OP_SYS_CLK_FREQ_HZ			(0x1168 | CCS_FL_FLOAT_IREAL | CCS_FL_32BIT)
+#define SMIAPP_REG_U16_MIN_OP_PIX_CLK_DIV			(0x116c | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_MAX_OP_PIX_CLK_DIV			(0x116e | CCS_FL_16BIT)
+#define SMIAPP_REG_F32_MIN_OP_PIX_CLK_FREQ_HZ			(0x1170 | CCS_FL_FLOAT_IREAL | CCS_FL_32BIT)
+#define SMIAPP_REG_F32_MAX_OP_PIX_CLK_FREQ_HZ			(0x1174 | CCS_FL_FLOAT_IREAL | CCS_FL_32BIT)
+#define SMIAPP_REG_U16_X_ADDR_MIN				(0x1180 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_Y_ADDR_MIN				(0x1182 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_X_ADDR_MAX				(0x1184 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_Y_ADDR_MAX				(0x1186 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_MIN_X_OUTPUT_SIZE			(0x1188 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_MIN_Y_OUTPUT_SIZE			(0x118a | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_MAX_X_OUTPUT_SIZE			(0x118c | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_MAX_Y_OUTPUT_SIZE			(0x118e | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_MIN_EVEN_INC				(0x11c0 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_MAX_EVEN_INC				(0x11c2 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_MIN_ODD_INC				(0x11c4 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_MAX_ODD_INC				(0x11c6 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_SCALING_CAPABILITY			(0x1200 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_SCALER_M_MIN				(0x1204 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_SCALER_M_MAX				(0x1206 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_SCALER_N_MIN				(0x1208 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_SCALER_N_MAX				(0x120a | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_SPATIAL_SAMPLING_CAPABILITY		(0x120c | CCS_FL_16BIT)
+#define SMIAPP_REG_U8_DIGITAL_CROP_CAPABILITY			0x120e
+#define SMIAPP_REG_U16_COMPRESSION_CAPABILITY			(0x1300 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_REDINRED			(0x1400 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_GREENINRED		(0x1402 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_BLUEINRED			(0x1404 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_REDINGREEN		(0x1406 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_GREENINGREEN		(0x1408 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_BLUEINGREEN		(0x140a | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_REDINBLUE			(0x140c | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_GREENINBLUE		(0x140e | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_BLUEINBLUE		(0x1410 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_FIFO_SIZE_PIXELS				(0x1500 | CCS_FL_16BIT)
+#define SMIAPP_REG_U8_FIFO_SUPPORT_CAPABILITY			0x1502
+#define SMIAPP_REG_U8_DPHY_CTRL_CAPABILITY			0x1600
+#define SMIAPP_REG_U8_CSI_LANE_MODE_CAPABILITY			0x1601
+#define SMIAPP_REG_U8_CSI_SIGNALLING_MODE_CAPABILITY		0x1602
+#define SMIAPP_REG_U8_FAST_STANDBY_CAPABILITY			0x1603
+#define SMIAPP_REG_U8_CCI_ADDRESS_CONTROL_CAPABILITY		0x1604
+#define SMIAPP_REG_U32_MAX_PER_LANE_BITRATE_1_LANE_MODE_MBPS	(0x1608 | CCS_FL_32BIT)
+#define SMIAPP_REG_U32_MAX_PER_LANE_BITRATE_2_LANE_MODE_MBPS	(0x160c | CCS_FL_32BIT)
+#define SMIAPP_REG_U32_MAX_PER_LANE_BITRATE_3_LANE_MODE_MBPS	(0x1610 | CCS_FL_32BIT)
+#define SMIAPP_REG_U32_MAX_PER_LANE_BITRATE_4_LANE_MODE_MBPS	(0x1614 | CCS_FL_32BIT)
+#define SMIAPP_REG_U8_TEMP_SENSOR_CAPABILITY			0x1618
+#define SMIAPP_REG_U16_MIN_FRAME_LENGTH_LINES_BIN		(0x1700 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_MAX_FRAME_LENGTH_LINES_BIN		(0x1702 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_MIN_LINE_LENGTH_PCK_BIN			(0x1704 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_MAX_LINE_LENGTH_PCK_BIN			(0x1706 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_MIN_LINE_BLANKING_PCK_BIN		(0x1708 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_FINE_INTEGRATION_TIME_MIN_BIN		(0x170a | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_FINE_INTEGRATION_TIME_MAX_MARGIN_BIN	(0x170c | CCS_FL_16BIT)
+#define SMIAPP_REG_U8_BINNING_CAPABILITY			0x1710
+#define SMIAPP_REG_U8_BINNING_WEIGHTING_CAPABILITY		0x1711
+#define SMIAPP_REG_U8_BINNING_SUBTYPES				0x1712
+#define SMIAPP_REG_U8_BINNING_TYPE_n(n)				(0x1713 + (n)) /* 1 <= n <= 237 */
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_CAPABILITY		0x1800
+#define SMIAPP_REG_U8_SHADING_CORRECTION_CAPABILITY		0x1900
+#define SMIAPP_REG_U8_GREEN_IMBALANCE_CAPABILITY		0x1901
+#define SMIAPP_REG_U8_BLACK_LEVEL_CAPABILITY			0x1902
+#define SMIAPP_REG_U8_MODULE_SPECIFIC_CORRECTION_CAPABILITY	0x1903
+#define SMIAPP_REG_U16_DEFECT_CORRECTION_CAPABILITY		(0x1904 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_DEFECT_CORRECTION_CAPABILITY_2		(0x1906 | CCS_FL_16BIT)
+#define SMIAPP_REG_U8_EDOF_CAPABILITY				0x1980
+#define SMIAPP_REG_U8_ESTIMATION_FRAMES				0x1981
+#define SMIAPP_REG_U8_SUPPORTS_SHARPNESS_ADJ			0x1982
+#define SMIAPP_REG_U8_SUPPORTS_DENOISING_ADJ			0x1983
+#define SMIAPP_REG_U8_SUPPORTS_MODULE_SPECIFIC_ADJ		0x1984
+#define SMIAPP_REG_U8_SUPPORTS_DEPTH_OF_FIELD_ADJ		0x1985
+#define SMIAPP_REG_U8_SUPPORTS_FOCUS_DISTANCE_ADJ		0x1986
+#define SMIAPP_REG_U8_COLOUR_FEEDBACK_CAPABILITY		0x1987
+#define SMIAPP_REG_U8_EDOF_SUPPORT_AB_NXM			0x1988
+#define SMIAPP_REG_U8_ESTIMATION_MODE_CAPABILITY		0x19c0
+#define SMIAPP_REG_U8_ESTIMATION_ZONE_CAPABILITY		0x19c1
+#define SMIAPP_REG_U16_EST_DEPTH_OF_FIELD			(0x19c2 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_EST_FOCUS_DISTANCE			(0x19c4 | CCS_FL_16BIT)
+#define SMIAPP_REG_U16_CAPABILITY_TRDY_MIN			(0x1a00 | CCS_FL_16BIT)
+#define SMIAPP_REG_U8_FLASH_MODE_CAPABILITY			0x1a02
+#define SMIAPP_REG_U16_MECH_SHUT_AND_ACT_START_ADDR		(0x1b02 | CCS_FL_16BIT)
+#define SMIAPP_REG_U8_ACTUATOR_CAPABILITY			0x1b04
+#define SMIAPP_REG_U16_ACTUATOR_TYPE				(0x1b40 | CCS_FL_16BIT)
+#define SMIAPP_REG_U8_AF_DEVICE_ADDRESS				0x1b42
+#define SMIAPP_REG_U16_FOCUS_CHANGE_ADDRESS			(0x1b44 | CCS_FL_16BIT)
+#define SMIAPP_REG_U8_BRACKETING_LUT_CAPABILITY_1		0x1c00
+#define SMIAPP_REG_U8_BRACKETING_LUT_CAPABILITY_2		0x1c01
+#define SMIAPP_REG_U8_BRACKETING_LUT_SIZE			0x1c02
+
+/* Register bit definitions */
+#define SMIAPP_IMAGE_ORIENTATION_HFLIP			BIT(0)
+#define SMIAPP_IMAGE_ORIENTATION_VFLIP			BIT(1)
+
+#define SMIAPP_DATA_TRANSFER_IF_1_CTRL_EN		BIT(0)
+#define SMIAPP_DATA_TRANSFER_IF_1_CTRL_WR_EN		BIT(1)
+#define SMIAPP_DATA_TRANSFER_IF_1_CTRL_ERR_CLEAR	BIT(2)
+#define SMIAPP_DATA_TRANSFER_IF_1_STATUS_RD_READY	BIT(0)
+#define SMIAPP_DATA_TRANSFER_IF_1_STATUS_WR_READY	BIT(1)
+#define SMIAPP_DATA_TRANSFER_IF_1_STATUS_EDATA		BIT(2)
+#define SMIAPP_DATA_TRANSFER_IF_1_STATUS_EUSAGE		BIT(3)
+
+#define SMIAPP_DATA_TRANSFER_IF_CAPABILITY_SUPPORTED	BIT(0)
+#define SMIAPP_DATA_TRANSFER_IF_CAPABILITY_POLL		BIT(2)
+
+#define SMIAPP_SOFTWARE_RESET				BIT(0)
+
+#define SMIAPP_FLASH_MODE_CAPABILITY_SINGLE_STROBE	BIT(0)
+#define SMIAPP_FLASH_MODE_CAPABILITY_MULTIPLE_STROBE	BIT(1)
+
+#define SMIAPP_CSI_SIGNALLING_MODE_CCP2_DATA_CLOCK	0
+#define SMIAPP_CSI_SIGNALLING_MODE_CCP2_DATA_STROBE	1
+#define SMIAPP_CSI_SIGNALLING_MODE_CSI2			2
+
+#define SMIAPP_DPHY_CTRL_AUTOMATIC			0
+/* DPHY control based on REQUESTED_LINK_BIT_RATE_MBPS */
+#define SMIAPP_DPHY_CTRL_UI				1
+#define SMIAPP_DPHY_CTRL_REGISTER			2
+
+#define SMIAPP_COMPRESSION_MODE_SIMPLE_PREDICTOR	1
+#define SMIAPP_COMPRESSION_MODE_ADVANCED_PREDICTOR	2
+
+#define SMIAPP_MODE_SELECT_SOFTWARE_STANDBY		0
+#define SMIAPP_MODE_SELECT_STREAMING			1
+
+#define SMIAPP_SCALING_MODE_NONE			0
+#define SMIAPP_SCALING_MODE_HORIZONTAL			1
+#define SMIAPP_SCALING_MODE_BOTH			2
+
+#define SMIAPP_SCALING_CAPABILITY_NONE			0
+#define SMIAPP_SCALING_CAPABILITY_HORIZONTAL		1
+#define SMIAPP_SCALING_CAPABILITY_BOTH			2 /* horizontal/both */
+
+/* digital crop right before scaler */
+#define SMIAPP_DIGITAL_CROP_CAPABILITY_NONE		0
+#define SMIAPP_DIGITAL_CROP_CAPABILITY_INPUT_CROP	1
+
+#define SMIAPP_DIGITAL_GAIN_CAPABILITY_PER_CHANNEL	1
+
+#define SMIAPP_BINNING_CAPABILITY_NO			0
+#define SMIAPP_BINNING_CAPABILITY_YES			1
+
+/* Maximum number of binning subtypes */
+#define SMIAPP_BINNING_SUBTYPES				253
+
+#define SMIAPP_PIXEL_ORDER_GRBG				0
+#define SMIAPP_PIXEL_ORDER_RGGB				1
+#define SMIAPP_PIXEL_ORDER_BGGR				2
+#define SMIAPP_PIXEL_ORDER_GBRG				3
+
+#define SMIAPP_DATA_FORMAT_MODEL_TYPE_NORMAL		1
+#define SMIAPP_DATA_FORMAT_MODEL_TYPE_EXTENDED		2
+#define SMIAPP_DATA_FORMAT_MODEL_TYPE_NORMAL_N		8
+#define SMIAPP_DATA_FORMAT_MODEL_TYPE_EXTENDED_N	16
+
+#define SMIAPP_FRAME_FORMAT_MODEL_TYPE_2BYTE		0x01
+#define SMIAPP_FRAME_FORMAT_MODEL_TYPE_4BYTE		0x02
+#define SMIAPP_FRAME_FORMAT_MODEL_SUBTYPE_NROWS_MASK	0x0f
+#define SMIAPP_FRAME_FORMAT_MODEL_SUBTYPE_NCOLS_MASK	0xf0
+#define SMIAPP_FRAME_FORMAT_MODEL_SUBTYPE_NCOLS_SHIFT	4
+
+#define SMIAPP_FRAME_FORMAT_DESC_2_PIXELCODE_MASK	0xf000
+#define SMIAPP_FRAME_FORMAT_DESC_2_PIXELCODE_SHIFT	12
+#define SMIAPP_FRAME_FORMAT_DESC_2_PIXELS_MASK		0x0fff
+
+#define SMIAPP_FRAME_FORMAT_DESC_4_PIXELCODE_MASK	0xf0000000
+#define SMIAPP_FRAME_FORMAT_DESC_4_PIXELCODE_SHIFT	28
+#define SMIAPP_FRAME_FORMAT_DESC_4_PIXELS_MASK		0x0000ffff
+
+#define SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_EMBEDDED	1
+#define SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_DUMMY	2
+#define SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_BLACK	3
+#define SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_DARK		4
+#define SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_VISIBLE	5
+
+#define SMIAPP_FAST_STANDBY_CTRL_COMPLETE_FRAMES	0
+#define SMIAPP_FAST_STANDBY_CTRL_IMMEDIATE		1
+
+/* Scaling N factor */
+#define SMIAPP_SCALE_N					16
+
+#endif /* __SMIAPP_REG_DEFS_H__ */
diff --git a/drivers/media/i2c/cs3308.c b/drivers/media/i2c/cs3308.c
new file mode 100644
index 000000000..61afa3d79
--- /dev/null
+++ b/drivers/media/i2c/cs3308.c
@@ -0,0 +1,126 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Cirrus Logic cs3308 8-Channel Analog Volume Control
+ *
+ * Copyright (C) 2010 Devin Heitmueller <dheitmueller@kernellabs.com>
+ * Copyright (C) 2012 Steven Toth <stoth@kernellabs.com>
+ *
+ * Derived from cs5345.c Copyright (C) 2007 Hans Verkuil
+ */
+
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+
+MODULE_DESCRIPTION("i2c device driver for cs3308 8-channel volume control");
+MODULE_AUTHOR("Devin Heitmueller");
+MODULE_LICENSE("GPL");
+
+static inline int cs3308_write(struct v4l2_subdev *sd, u8 reg, u8 value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return i2c_smbus_write_byte_data(client, reg, value);
+}
+
+static inline int cs3308_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return i2c_smbus_read_byte_data(client, reg);
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int cs3308_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+{
+	reg->val = cs3308_read(sd, reg->reg & 0xffff);
+	reg->size = 1;
+	return 0;
+}
+
+static int cs3308_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
+{
+	cs3308_write(sd, reg->reg & 0xffff, reg->val & 0xff);
+	return 0;
+}
+#endif
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_core_ops cs3308_core_ops = {
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = cs3308_g_register,
+	.s_register = cs3308_s_register,
+#endif
+};
+
+static const struct v4l2_subdev_ops cs3308_ops = {
+	.core = &cs3308_core_ops,
+};
+
+/* ----------------------------------------------------------------------- */
+
+static int cs3308_probe(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd;
+	unsigned i;
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+
+	if ((i2c_smbus_read_byte_data(client, 0x1c) & 0xf0) != 0xe0)
+		return -ENODEV;
+
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+		 client->addr << 1, client->adapter->name);
+
+	sd = kzalloc(sizeof(struct v4l2_subdev), GFP_KERNEL);
+	if (sd == NULL)
+		return -ENOMEM;
+
+	v4l2_i2c_subdev_init(sd, client, &cs3308_ops);
+
+	/* Set some reasonable defaults */
+	cs3308_write(sd, 0x0d, 0x00); /* Power up all channels */
+	cs3308_write(sd, 0x0e, 0x00); /* Master Power */
+	cs3308_write(sd, 0x0b, 0x00); /* Device Configuration */
+	/* Set volume for each channel */
+	for (i = 1; i <= 8; i++)
+		cs3308_write(sd, i, 0xd2);
+	cs3308_write(sd, 0x0a, 0x00); /* Unmute all channels */
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static void cs3308_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	kfree(sd);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id cs3308_id[] = {
+	{ "cs3308", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, cs3308_id);
+
+static struct i2c_driver cs3308_driver = {
+	.driver = {
+		.name   = "cs3308",
+	},
+	.probe          = cs3308_probe,
+	.remove         = cs3308_remove,
+	.id_table       = cs3308_id,
+};
+
+module_i2c_driver(cs3308_driver);
diff --git a/drivers/media/i2c/cs5345.c b/drivers/media/i2c/cs5345.c
new file mode 100644
index 000000000..3019a132e
--- /dev/null
+++ b/drivers/media/i2c/cs5345.c
@@ -0,0 +1,206 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * cs5345 Cirrus Logic 24-bit, 192 kHz Stereo Audio ADC
+ * Copyright (C) 2007 Hans Verkuil
+ */
+
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <linux/slab.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+
+MODULE_DESCRIPTION("i2c device driver for cs5345 Audio ADC");
+MODULE_AUTHOR("Hans Verkuil");
+MODULE_LICENSE("GPL");
+
+static bool debug;
+
+module_param(debug, bool, 0644);
+
+MODULE_PARM_DESC(debug, "Debugging messages, 0=Off (default), 1=On");
+
+struct cs5345_state {
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
+};
+
+static inline struct cs5345_state *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct cs5345_state, sd);
+}
+
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct cs5345_state, hdl)->sd;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static inline int cs5345_write(struct v4l2_subdev *sd, u8 reg, u8 value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return i2c_smbus_write_byte_data(client, reg, value);
+}
+
+static inline int cs5345_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return i2c_smbus_read_byte_data(client, reg);
+}
+
+static int cs5345_s_routing(struct v4l2_subdev *sd,
+			    u32 input, u32 output, u32 config)
+{
+	if ((input & 0xf) > 6) {
+		v4l2_err(sd, "Invalid input %d.\n", input);
+		return -EINVAL;
+	}
+	cs5345_write(sd, 0x09, input & 0xf);
+	cs5345_write(sd, 0x05, input & 0xf0);
+	return 0;
+}
+
+static int cs5345_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUDIO_MUTE:
+		cs5345_write(sd, 0x04, ctrl->val ? 0x80 : 0);
+		return 0;
+	case V4L2_CID_AUDIO_VOLUME:
+		cs5345_write(sd, 0x07, ((u8)ctrl->val) & 0x3f);
+		cs5345_write(sd, 0x08, ((u8)ctrl->val) & 0x3f);
+		return 0;
+	}
+	return -EINVAL;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int cs5345_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+{
+	reg->size = 1;
+	reg->val = cs5345_read(sd, reg->reg & 0x1f);
+	return 0;
+}
+
+static int cs5345_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
+{
+	cs5345_write(sd, reg->reg & 0x1f, reg->val & 0xff);
+	return 0;
+}
+#endif
+
+static int cs5345_log_status(struct v4l2_subdev *sd)
+{
+	u8 v = cs5345_read(sd, 0x09) & 7;
+	u8 m = cs5345_read(sd, 0x04);
+	int vol = cs5345_read(sd, 0x08) & 0x3f;
+
+	v4l2_info(sd, "Input:  %d%s\n", v,
+			(m & 0x80) ? " (muted)" : "");
+	if (vol >= 32)
+		vol = vol - 64;
+	v4l2_info(sd, "Volume: %d dB\n", vol);
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_ctrl_ops cs5345_ctrl_ops = {
+	.s_ctrl = cs5345_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops cs5345_core_ops = {
+	.log_status = cs5345_log_status,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = cs5345_g_register,
+	.s_register = cs5345_s_register,
+#endif
+};
+
+static const struct v4l2_subdev_audio_ops cs5345_audio_ops = {
+	.s_routing = cs5345_s_routing,
+};
+
+static const struct v4l2_subdev_ops cs5345_ops = {
+	.core = &cs5345_core_ops,
+	.audio = &cs5345_audio_ops,
+};
+
+/* ----------------------------------------------------------------------- */
+
+static int cs5345_probe(struct i2c_client *client)
+{
+	struct cs5345_state *state;
+	struct v4l2_subdev *sd;
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	state = devm_kzalloc(&client->dev, sizeof(*state), GFP_KERNEL);
+	if (state == NULL)
+		return -ENOMEM;
+	sd = &state->sd;
+	v4l2_i2c_subdev_init(sd, client, &cs5345_ops);
+
+	v4l2_ctrl_handler_init(&state->hdl, 2);
+	v4l2_ctrl_new_std(&state->hdl, &cs5345_ctrl_ops,
+			V4L2_CID_AUDIO_MUTE, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&state->hdl, &cs5345_ctrl_ops,
+			V4L2_CID_AUDIO_VOLUME, -24, 24, 1, 0);
+	sd->ctrl_handler = &state->hdl;
+	if (state->hdl.error) {
+		int err = state->hdl.error;
+
+		v4l2_ctrl_handler_free(&state->hdl);
+		return err;
+	}
+	/* set volume/mute */
+	v4l2_ctrl_handler_setup(&state->hdl);
+
+	cs5345_write(sd, 0x02, 0x00);
+	cs5345_write(sd, 0x04, 0x01);
+	cs5345_write(sd, 0x09, 0x01);
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static void cs5345_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct cs5345_state *state = to_state(sd);
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&state->hdl);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id cs5345_id[] = {
+	{ "cs5345", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, cs5345_id);
+
+static struct i2c_driver cs5345_driver = {
+	.driver = {
+		.name	= "cs5345",
+	},
+	.probe		= cs5345_probe,
+	.remove		= cs5345_remove,
+	.id_table	= cs5345_id,
+};
+
+module_i2c_driver(cs5345_driver);
diff --git a/drivers/media/i2c/cs53l32a.c b/drivers/media/i2c/cs53l32a.c
new file mode 100644
index 000000000..82881b79e
--- /dev/null
+++ b/drivers/media/i2c/cs53l32a.c
@@ -0,0 +1,217 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * cs53l32a (Adaptec AVC-2010 and AVC-2410) i2c ivtv driver.
+ * Copyright (C) 2005  Martin Vaughan
+ *
+ * Audio source switching for Adaptec AVC-2410 added by Trev Jackson
+ */
+
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <linux/ioctl.h>
+#include <linux/uaccess.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+
+MODULE_DESCRIPTION("i2c device driver for cs53l32a Audio ADC");
+MODULE_AUTHOR("Martin Vaughan");
+MODULE_LICENSE("GPL");
+
+static bool debug;
+
+module_param(debug, bool, 0644);
+
+MODULE_PARM_DESC(debug, "Debugging messages, 0=Off (default), 1=On");
+
+
+struct cs53l32a_state {
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
+};
+
+static inline struct cs53l32a_state *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct cs53l32a_state, sd);
+}
+
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct cs53l32a_state, hdl)->sd;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static int cs53l32a_write(struct v4l2_subdev *sd, u8 reg, u8 value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return i2c_smbus_write_byte_data(client, reg, value);
+}
+
+static int cs53l32a_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return i2c_smbus_read_byte_data(client, reg);
+}
+
+static int cs53l32a_s_routing(struct v4l2_subdev *sd,
+			      u32 input, u32 output, u32 config)
+{
+	/* There are 2 physical inputs, but the second input can be
+	   placed in two modes, the first mode bypasses the PGA (gain),
+	   the second goes through the PGA. Hence there are three
+	   possible inputs to choose from. */
+	if (input > 2) {
+		v4l2_err(sd, "Invalid input %d.\n", input);
+		return -EINVAL;
+	}
+	cs53l32a_write(sd, 0x01, 0x01 + (input << 4));
+	return 0;
+}
+
+static int cs53l32a_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUDIO_MUTE:
+		cs53l32a_write(sd, 0x03, ctrl->val ? 0xf0 : 0x30);
+		return 0;
+	case V4L2_CID_AUDIO_VOLUME:
+		cs53l32a_write(sd, 0x04, (u8)ctrl->val);
+		cs53l32a_write(sd, 0x05, (u8)ctrl->val);
+		return 0;
+	}
+	return -EINVAL;
+}
+
+static int cs53l32a_log_status(struct v4l2_subdev *sd)
+{
+	struct cs53l32a_state *state = to_state(sd);
+	u8 v = cs53l32a_read(sd, 0x01);
+
+	v4l2_info(sd, "Input:  %d\n", (v >> 4) & 3);
+	v4l2_ctrl_handler_log_status(&state->hdl, sd->name);
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_ctrl_ops cs53l32a_ctrl_ops = {
+	.s_ctrl = cs53l32a_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops cs53l32a_core_ops = {
+	.log_status = cs53l32a_log_status,
+};
+
+static const struct v4l2_subdev_audio_ops cs53l32a_audio_ops = {
+	.s_routing = cs53l32a_s_routing,
+};
+
+static const struct v4l2_subdev_ops cs53l32a_ops = {
+	.core = &cs53l32a_core_ops,
+	.audio = &cs53l32a_audio_ops,
+};
+
+/* ----------------------------------------------------------------------- */
+
+/* i2c implementation */
+
+/*
+ * Generic i2c probe
+ * concerning the addresses: i2c wants 7 bit (without the r/w bit), so '>>1'
+ */
+
+static int cs53l32a_probe(struct i2c_client *client)
+{
+	const struct i2c_device_id *id = i2c_client_get_device_id(client);
+	struct cs53l32a_state *state;
+	struct v4l2_subdev *sd;
+	int i;
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+
+	if (!id)
+		strscpy(client->name, "cs53l32a", sizeof(client->name));
+
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	state = devm_kzalloc(&client->dev, sizeof(*state), GFP_KERNEL);
+	if (state == NULL)
+		return -ENOMEM;
+	sd = &state->sd;
+	v4l2_i2c_subdev_init(sd, client, &cs53l32a_ops);
+
+	for (i = 1; i <= 7; i++) {
+		u8 v = cs53l32a_read(sd, i);
+
+		v4l2_dbg(1, debug, sd, "Read Reg %d %02x\n", i, v);
+	}
+
+	v4l2_ctrl_handler_init(&state->hdl, 2);
+	v4l2_ctrl_new_std(&state->hdl, &cs53l32a_ctrl_ops,
+			V4L2_CID_AUDIO_VOLUME, -96, 12, 1, 0);
+	v4l2_ctrl_new_std(&state->hdl, &cs53l32a_ctrl_ops,
+			V4L2_CID_AUDIO_MUTE, 0, 1, 1, 0);
+	sd->ctrl_handler = &state->hdl;
+	if (state->hdl.error) {
+		int err = state->hdl.error;
+
+		v4l2_ctrl_handler_free(&state->hdl);
+		return err;
+	}
+
+	/* Set cs53l32a internal register for Adaptec 2010/2410 setup */
+
+	cs53l32a_write(sd, 0x01, 0x21);
+	cs53l32a_write(sd, 0x02, 0x29);
+	cs53l32a_write(sd, 0x03, 0x30);
+	cs53l32a_write(sd, 0x04, 0x00);
+	cs53l32a_write(sd, 0x05, 0x00);
+	cs53l32a_write(sd, 0x06, 0x00);
+	cs53l32a_write(sd, 0x07, 0x00);
+
+	/* Display results, should be 0x21,0x29,0x30,0x00,0x00,0x00,0x00 */
+
+	for (i = 1; i <= 7; i++) {
+		u8 v = cs53l32a_read(sd, i);
+
+		v4l2_dbg(1, debug, sd, "Read Reg %d %02x\n", i, v);
+	}
+	return 0;
+}
+
+static void cs53l32a_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct cs53l32a_state *state = to_state(sd);
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&state->hdl);
+}
+
+static const struct i2c_device_id cs53l32a_id[] = {
+	{ "cs53l32a", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, cs53l32a_id);
+
+static struct i2c_driver cs53l32a_driver = {
+	.driver = {
+		.name	= "cs53l32a",
+	},
+	.probe		= cs53l32a_probe,
+	.remove		= cs53l32a_remove,
+	.id_table	= cs53l32a_id,
+};
+
+module_i2c_driver(cs53l32a_driver);
diff --git a/drivers/media/i2c/cx25840/Kconfig b/drivers/media/i2c/cx25840/Kconfig
new file mode 100644
index 000000000..46f15702c
--- /dev/null
+++ b/drivers/media/i2c/cx25840/Kconfig
@@ -0,0 +1,9 @@
+# SPDX-License-Identifier: GPL-2.0-only
+config VIDEO_CX25840
+	tristate "Conexant CX2584x audio/video decoders"
+	depends on VIDEO_DEV && I2C
+	help
+	  Support for the Conexant CX2584x audio/video decoders.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called cx25840
diff --git a/drivers/media/i2c/cx25840/Makefile b/drivers/media/i2c/cx25840/Makefile
new file mode 100644
index 000000000..3681df295
--- /dev/null
+++ b/drivers/media/i2c/cx25840/Makefile
@@ -0,0 +1,5 @@
+# SPDX-License-Identifier: GPL-2.0-only
+cx25840-objs    := cx25840-core.o cx25840-audio.o cx25840-firmware.o \
+		   cx25840-vbi.o cx25840-ir.o
+
+obj-$(CONFIG_VIDEO_CX25840) += cx25840.o
diff --git a/drivers/media/i2c/cx25840/cx25840-audio.c b/drivers/media/i2c/cx25840/cx25840-audio.c
new file mode 100644
index 000000000..eb77ba088
--- /dev/null
+++ b/drivers/media/i2c/cx25840/cx25840-audio.c
@@ -0,0 +1,558 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/* cx25840 audio functions
+ */
+
+
+#include <linux/videodev2.h>
+#include <linux/i2c.h>
+#include <media/v4l2-common.h>
+#include <media/drv-intf/cx25840.h>
+
+#include "cx25840-core.h"
+
+/*
+ * Note: The PLL and SRC parameters are based on a reference frequency that
+ * would ideally be:
+ *
+ * NTSC Color subcarrier freq * 8 = 4.5 MHz/286 * 455/2 * 8 = 28.63636363... MHz
+ *
+ * However, it's not the exact reference frequency that matters, only that the
+ * firmware and modules that comprise the driver for a particular board all
+ * use the same value (close to the ideal value).
+ *
+ * Comments below will note which reference frequency is assumed for various
+ * parameters.  They will usually be one of
+ *
+ *	ref_freq = 28.636360 MHz
+ *		or
+ *	ref_freq = 28.636363 MHz
+ */
+
+static int cx25840_set_audclk_freq(struct i2c_client *client, u32 freq)
+{
+	struct cx25840_state *state = to_state(i2c_get_clientdata(client));
+
+	if (state->aud_input != CX25840_AUDIO_SERIAL) {
+		switch (freq) {
+		case 32000:
+			/*
+			 * VID_PLL Integer = 0x0f, VID_PLL Post Divider = 0x04
+			 * AUX_PLL Integer = 0x06, AUX PLL Post Divider = 0x10
+			 */
+			cx25840_write4(client, 0x108, 0x1006040f);
+
+			/*
+			 * VID_PLL Fraction (register 0x10c) = 0x2be2fe
+			 * 28636360 * 0xf.15f17f0/4 = 108 MHz
+			 * 432 MHz pre-postdivide
+			 */
+
+			/*
+			 * AUX_PLL Fraction = 0x1bb39ee
+			 * 28636363 * 0x6.dd9cf70/0x10 = 32000 * 384
+			 * 196.6 MHz pre-postdivide
+			 * FIXME < 200 MHz is out of specified valid range
+			 * FIXME 28636363 ref_freq doesn't match VID PLL ref
+			 */
+			cx25840_write4(client, 0x110, 0x01bb39ee);
+
+			/*
+			 * SA_MCLK_SEL = 1
+			 * SA_MCLK_DIV = 0x10 = 384/384 * AUX_PLL post dvivider
+			 */
+			cx25840_write(client, 0x127, 0x50);
+
+			if (is_cx2583x(state))
+				break;
+
+			/* src3/4/6_ctl */
+			/* 0x1.f77f = (4 * 28636360/8 * 2/455) / 32000 */
+			cx25840_write4(client, 0x900, 0x0801f77f);
+			cx25840_write4(client, 0x904, 0x0801f77f);
+			cx25840_write4(client, 0x90c, 0x0801f77f);
+			break;
+
+		case 44100:
+			/*
+			 * VID_PLL Integer = 0x0f, VID_PLL Post Divider = 0x04
+			 * AUX_PLL Integer = 0x09, AUX PLL Post Divider = 0x10
+			 */
+			cx25840_write4(client, 0x108, 0x1009040f);
+
+			/*
+			 * VID_PLL Fraction (register 0x10c) = 0x2be2fe
+			 * 28636360 * 0xf.15f17f0/4 = 108 MHz
+			 * 432 MHz pre-postdivide
+			 */
+
+			/*
+			 * AUX_PLL Fraction = 0x0ec6bd6
+			 * 28636363 * 0x9.7635eb0/0x10 = 44100 * 384
+			 * 271 MHz pre-postdivide
+			 * FIXME 28636363 ref_freq doesn't match VID PLL ref
+			 */
+			cx25840_write4(client, 0x110, 0x00ec6bd6);
+
+			/*
+			 * SA_MCLK_SEL = 1
+			 * SA_MCLK_DIV = 0x10 = 384/384 * AUX_PLL post dvivider
+			 */
+			cx25840_write(client, 0x127, 0x50);
+
+			if (is_cx2583x(state))
+				break;
+
+			/* src3/4/6_ctl */
+			/* 0x1.6d59 = (4 * 28636360/8 * 2/455) / 44100 */
+			cx25840_write4(client, 0x900, 0x08016d59);
+			cx25840_write4(client, 0x904, 0x08016d59);
+			cx25840_write4(client, 0x90c, 0x08016d59);
+			break;
+
+		case 48000:
+			/*
+			 * VID_PLL Integer = 0x0f, VID_PLL Post Divider = 0x04
+			 * AUX_PLL Integer = 0x0a, AUX PLL Post Divider = 0x10
+			 */
+			cx25840_write4(client, 0x108, 0x100a040f);
+
+			/*
+			 * VID_PLL Fraction (register 0x10c) = 0x2be2fe
+			 * 28636360 * 0xf.15f17f0/4 = 108 MHz
+			 * 432 MHz pre-postdivide
+			 */
+
+			/*
+			 * AUX_PLL Fraction = 0x098d6e5
+			 * 28636363 * 0xa.4c6b728/0x10 = 48000 * 384
+			 * 295 MHz pre-postdivide
+			 * FIXME 28636363 ref_freq doesn't match VID PLL ref
+			 */
+			cx25840_write4(client, 0x110, 0x0098d6e5);
+
+			/*
+			 * SA_MCLK_SEL = 1
+			 * SA_MCLK_DIV = 0x10 = 384/384 * AUX_PLL post dvivider
+			 */
+			cx25840_write(client, 0x127, 0x50);
+
+			if (is_cx2583x(state))
+				break;
+
+			/* src3/4/6_ctl */
+			/* 0x1.4faa = (4 * 28636360/8 * 2/455) / 48000 */
+			cx25840_write4(client, 0x900, 0x08014faa);
+			cx25840_write4(client, 0x904, 0x08014faa);
+			cx25840_write4(client, 0x90c, 0x08014faa);
+			break;
+		}
+	} else {
+		switch (freq) {
+		case 32000:
+			/*
+			 * VID_PLL Integer = 0x0f, VID_PLL Post Divider = 0x04
+			 * AUX_PLL Integer = 0x08, AUX PLL Post Divider = 0x1e
+			 */
+			cx25840_write4(client, 0x108, 0x1e08040f);
+
+			/*
+			 * VID_PLL Fraction (register 0x10c) = 0x2be2fe
+			 * 28636360 * 0xf.15f17f0/4 = 108 MHz
+			 * 432 MHz pre-postdivide
+			 */
+
+			/*
+			 * AUX_PLL Fraction = 0x12a0869
+			 * 28636363 * 0x8.9504348/0x1e = 32000 * 256
+			 * 246 MHz pre-postdivide
+			 * FIXME 28636363 ref_freq doesn't match VID PLL ref
+			 */
+			cx25840_write4(client, 0x110, 0x012a0869);
+
+			/*
+			 * SA_MCLK_SEL = 1
+			 * SA_MCLK_DIV = 0x14 = 256/384 * AUX_PLL post dvivider
+			 */
+			cx25840_write(client, 0x127, 0x54);
+
+			if (is_cx2583x(state))
+				break;
+
+			/* src1_ctl */
+			/* 0x1.0000 = 32000/32000 */
+			cx25840_write4(client, 0x8f8, 0x08010000);
+
+			/* src3/4/6_ctl */
+			/* 0x2.0000 = 2 * (32000/32000) */
+			cx25840_write4(client, 0x900, 0x08020000);
+			cx25840_write4(client, 0x904, 0x08020000);
+			cx25840_write4(client, 0x90c, 0x08020000);
+			break;
+
+		case 44100:
+			/*
+			 * VID_PLL Integer = 0x0f, VID_PLL Post Divider = 0x04
+			 * AUX_PLL Integer = 0x09, AUX PLL Post Divider = 0x18
+			 */
+			cx25840_write4(client, 0x108, 0x1809040f);
+
+			/*
+			 * VID_PLL Fraction (register 0x10c) = 0x2be2fe
+			 * 28636360 * 0xf.15f17f0/4 = 108 MHz
+			 * 432 MHz pre-postdivide
+			 */
+
+			/*
+			 * AUX_PLL Fraction = 0x0ec6bd6
+			 * 28636363 * 0x9.7635eb0/0x18 = 44100 * 256
+			 * 271 MHz pre-postdivide
+			 * FIXME 28636363 ref_freq doesn't match VID PLL ref
+			 */
+			cx25840_write4(client, 0x110, 0x00ec6bd6);
+
+			/*
+			 * SA_MCLK_SEL = 1
+			 * SA_MCLK_DIV = 0x10 = 256/384 * AUX_PLL post dvivider
+			 */
+			cx25840_write(client, 0x127, 0x50);
+
+			if (is_cx2583x(state))
+				break;
+
+			/* src1_ctl */
+			/* 0x1.60cd = 44100/32000 */
+			cx25840_write4(client, 0x8f8, 0x080160cd);
+
+			/* src3/4/6_ctl */
+			/* 0x1.7385 = 2 * (32000/44100) */
+			cx25840_write4(client, 0x900, 0x08017385);
+			cx25840_write4(client, 0x904, 0x08017385);
+			cx25840_write4(client, 0x90c, 0x08017385);
+			break;
+
+		case 48000:
+			/*
+			 * VID_PLL Integer = 0x0f, VID_PLL Post Divider = 0x04
+			 * AUX_PLL Integer = 0x0a, AUX PLL Post Divider = 0x18
+			 */
+			cx25840_write4(client, 0x108, 0x180a040f);
+
+			/*
+			 * VID_PLL Fraction (register 0x10c) = 0x2be2fe
+			 * 28636360 * 0xf.15f17f0/4 = 108 MHz
+			 * 432 MHz pre-postdivide
+			 */
+
+			/*
+			 * AUX_PLL Fraction = 0x098d6e5
+			 * 28636363 * 0xa.4c6b728/0x18 = 48000 * 256
+			 * 295 MHz pre-postdivide
+			 * FIXME 28636363 ref_freq doesn't match VID PLL ref
+			 */
+			cx25840_write4(client, 0x110, 0x0098d6e5);
+
+			/*
+			 * SA_MCLK_SEL = 1
+			 * SA_MCLK_DIV = 0x10 = 256/384 * AUX_PLL post dvivider
+			 */
+			cx25840_write(client, 0x127, 0x50);
+
+			if (is_cx2583x(state))
+				break;
+
+			/* src1_ctl */
+			/* 0x1.8000 = 48000/32000 */
+			cx25840_write4(client, 0x8f8, 0x08018000);
+
+			/* src3/4/6_ctl */
+			/* 0x1.5555 = 2 * (32000/48000) */
+			cx25840_write4(client, 0x900, 0x08015555);
+			cx25840_write4(client, 0x904, 0x08015555);
+			cx25840_write4(client, 0x90c, 0x08015555);
+			break;
+		}
+	}
+
+	state->audclk_freq = freq;
+
+	return 0;
+}
+
+static inline int cx25836_set_audclk_freq(struct i2c_client *client, u32 freq)
+{
+	return cx25840_set_audclk_freq(client, freq);
+}
+
+static int cx23885_set_audclk_freq(struct i2c_client *client, u32 freq)
+{
+	struct cx25840_state *state = to_state(i2c_get_clientdata(client));
+
+	if (state->aud_input != CX25840_AUDIO_SERIAL) {
+		switch (freq) {
+		case 32000:
+		case 44100:
+		case 48000:
+			/* We don't have register values
+			 * so avoid destroying registers. */
+			/* FIXME return -EINVAL; */
+			break;
+		}
+	} else {
+		switch (freq) {
+		case 32000:
+		case 44100:
+			/* We don't have register values
+			 * so avoid destroying registers. */
+			/* FIXME return -EINVAL; */
+			break;
+
+		case 48000:
+			/* src1_ctl */
+			/* 0x1.867c = 48000 / (2 * 28636360/8 * 2/455) */
+			cx25840_write4(client, 0x8f8, 0x0801867c);
+
+			/* src3/4/6_ctl */
+			/* 0x1.4faa = (4 * 28636360/8 * 2/455) / 48000 */
+			cx25840_write4(client, 0x900, 0x08014faa);
+			cx25840_write4(client, 0x904, 0x08014faa);
+			cx25840_write4(client, 0x90c, 0x08014faa);
+			break;
+		}
+	}
+
+	state->audclk_freq = freq;
+
+	return 0;
+}
+
+static int cx231xx_set_audclk_freq(struct i2c_client *client, u32 freq)
+{
+	struct cx25840_state *state = to_state(i2c_get_clientdata(client));
+
+	if (state->aud_input != CX25840_AUDIO_SERIAL) {
+		switch (freq) {
+		case 32000:
+			/* src3/4/6_ctl */
+			/* 0x1.f77f = (4 * 28636360/8 * 2/455) / 32000 */
+			cx25840_write4(client, 0x900, 0x0801f77f);
+			cx25840_write4(client, 0x904, 0x0801f77f);
+			cx25840_write4(client, 0x90c, 0x0801f77f);
+			break;
+
+		case 44100:
+			/* src3/4/6_ctl */
+			/* 0x1.6d59 = (4 * 28636360/8 * 2/455) / 44100 */
+			cx25840_write4(client, 0x900, 0x08016d59);
+			cx25840_write4(client, 0x904, 0x08016d59);
+			cx25840_write4(client, 0x90c, 0x08016d59);
+			break;
+
+		case 48000:
+			/* src3/4/6_ctl */
+			/* 0x1.4faa = (4 * 28636360/8 * 2/455) / 48000 */
+			cx25840_write4(client, 0x900, 0x08014faa);
+			cx25840_write4(client, 0x904, 0x08014faa);
+			cx25840_write4(client, 0x90c, 0x08014faa);
+			break;
+		}
+	} else {
+		switch (freq) {
+		/* FIXME These cases make different assumptions about audclk */
+		case 32000:
+			/* src1_ctl */
+			/* 0x1.0000 = 32000/32000 */
+			cx25840_write4(client, 0x8f8, 0x08010000);
+
+			/* src3/4/6_ctl */
+			/* 0x2.0000 = 2 * (32000/32000) */
+			cx25840_write4(client, 0x900, 0x08020000);
+			cx25840_write4(client, 0x904, 0x08020000);
+			cx25840_write4(client, 0x90c, 0x08020000);
+			break;
+
+		case 44100:
+			/* src1_ctl */
+			/* 0x1.60cd = 44100/32000 */
+			cx25840_write4(client, 0x8f8, 0x080160cd);
+
+			/* src3/4/6_ctl */
+			/* 0x1.7385 = 2 * (32000/44100) */
+			cx25840_write4(client, 0x900, 0x08017385);
+			cx25840_write4(client, 0x904, 0x08017385);
+			cx25840_write4(client, 0x90c, 0x08017385);
+			break;
+
+		case 48000:
+			/* src1_ctl */
+			/* 0x1.867c = 48000 / (2 * 28636360/8 * 2/455) */
+			cx25840_write4(client, 0x8f8, 0x0801867c);
+
+			/* src3/4/6_ctl */
+			/* 0x1.4faa = (4 * 28636360/8 * 2/455) / 48000 */
+			cx25840_write4(client, 0x900, 0x08014faa);
+			cx25840_write4(client, 0x904, 0x08014faa);
+			cx25840_write4(client, 0x90c, 0x08014faa);
+			break;
+		}
+	}
+
+	state->audclk_freq = freq;
+
+	return 0;
+}
+
+static int set_audclk_freq(struct i2c_client *client, u32 freq)
+{
+	struct cx25840_state *state = to_state(i2c_get_clientdata(client));
+
+	if (freq != 32000 && freq != 44100 && freq != 48000)
+		return -EINVAL;
+
+	if (is_cx231xx(state))
+		return cx231xx_set_audclk_freq(client, freq);
+
+	if (is_cx2388x(state))
+		return cx23885_set_audclk_freq(client, freq);
+
+	if (is_cx2583x(state))
+		return cx25836_set_audclk_freq(client, freq);
+
+	return cx25840_set_audclk_freq(client, freq);
+}
+
+void cx25840_audio_set_path(struct i2c_client *client)
+{
+	struct cx25840_state *state = to_state(i2c_get_clientdata(client));
+
+	if (!is_cx2583x(state)) {
+		/* assert soft reset */
+		cx25840_and_or(client, 0x810, ~0x1, 0x01);
+
+		/* stop microcontroller */
+		cx25840_and_or(client, 0x803, ~0x10, 0);
+
+		/* Mute everything to prevent the PFFT! */
+		cx25840_write(client, 0x8d3, 0x1f);
+
+		if (state->aud_input == CX25840_AUDIO_SERIAL) {
+			/* Set Path1 to Serial Audio Input */
+			cx25840_write4(client, 0x8d0, 0x01011012);
+
+			/* The microcontroller should not be started for the
+			 * non-tuner inputs: autodetection is specific for
+			 * TV audio. */
+		} else {
+			/* Set Path1 to Analog Demod Main Channel */
+			cx25840_write4(client, 0x8d0, 0x1f063870);
+		}
+	}
+
+	set_audclk_freq(client, state->audclk_freq);
+
+	if (!is_cx2583x(state)) {
+		if (state->aud_input != CX25840_AUDIO_SERIAL) {
+			/* When the microcontroller detects the
+			 * audio format, it will unmute the lines */
+			cx25840_and_or(client, 0x803, ~0x10, 0x10);
+		}
+
+		/* deassert soft reset */
+		cx25840_and_or(client, 0x810, ~0x1, 0x00);
+
+		/* Ensure the controller is running when we exit */
+		if (is_cx2388x(state) || is_cx231xx(state))
+			cx25840_and_or(client, 0x803, ~0x10, 0x10);
+	}
+}
+
+static void set_volume(struct i2c_client *client, int volume)
+{
+	int vol;
+
+	/* Convert the volume to msp3400 values (0-127) */
+	vol = volume >> 9;
+
+	/* now scale it up to cx25840 values
+	 * -114dB to -96dB maps to 0
+	 * this should be 19, but in my testing that was 4dB too loud */
+	if (vol <= 23) {
+		vol = 0;
+	} else {
+		vol -= 23;
+	}
+
+	/* PATH1_VOLUME */
+	cx25840_write(client, 0x8d4, 228 - (vol * 2));
+}
+
+static void set_balance(struct i2c_client *client, int balance)
+{
+	int bal = balance >> 8;
+	if (bal > 0x80) {
+		/* PATH1_BAL_LEFT */
+		cx25840_and_or(client, 0x8d5, 0x7f, 0x80);
+		/* PATH1_BAL_LEVEL */
+		cx25840_and_or(client, 0x8d5, ~0x7f, bal & 0x7f);
+	} else {
+		/* PATH1_BAL_LEFT */
+		cx25840_and_or(client, 0x8d5, 0x7f, 0x00);
+		/* PATH1_BAL_LEVEL */
+		cx25840_and_or(client, 0x8d5, ~0x7f, 0x80 - bal);
+	}
+}
+
+int cx25840_s_clock_freq(struct v4l2_subdev *sd, u32 freq)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct cx25840_state *state = to_state(sd);
+	int retval;
+
+	if (!is_cx2583x(state))
+		cx25840_and_or(client, 0x810, ~0x1, 1);
+	if (state->aud_input != CX25840_AUDIO_SERIAL) {
+		cx25840_and_or(client, 0x803, ~0x10, 0);
+		cx25840_write(client, 0x8d3, 0x1f);
+	}
+	retval = set_audclk_freq(client, freq);
+	if (state->aud_input != CX25840_AUDIO_SERIAL)
+		cx25840_and_or(client, 0x803, ~0x10, 0x10);
+	if (!is_cx2583x(state))
+		cx25840_and_or(client, 0x810, ~0x1, 0);
+	return retval;
+}
+
+static int cx25840_audio_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+	struct cx25840_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUDIO_VOLUME:
+		if (state->mute->val)
+			set_volume(client, 0);
+		else
+			set_volume(client, state->volume->val);
+		break;
+	case V4L2_CID_AUDIO_BASS:
+		/* PATH1_EQ_BASS_VOL */
+		cx25840_and_or(client, 0x8d9, ~0x3f,
+					48 - (ctrl->val * 48 / 0xffff));
+		break;
+	case V4L2_CID_AUDIO_TREBLE:
+		/* PATH1_EQ_TREBLE_VOL */
+		cx25840_and_or(client, 0x8db, ~0x3f,
+					48 - (ctrl->val * 48 / 0xffff));
+		break;
+	case V4L2_CID_AUDIO_BALANCE:
+		set_balance(client, ctrl->val);
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+const struct v4l2_ctrl_ops cx25840_audio_ctrl_ops = {
+	.s_ctrl = cx25840_audio_s_ctrl,
+};
diff --git a/drivers/media/i2c/cx25840/cx25840-core.c b/drivers/media/i2c/cx25840/cx25840-core.c
new file mode 100644
index 000000000..5aec25289
--- /dev/null
+++ b/drivers/media/i2c/cx25840/cx25840-core.c
@@ -0,0 +1,6053 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/* cx25840 - Conexant CX25840 audio/video decoder driver
+ *
+ * Copyright (C) 2004 Ulf Eklund
+ *
+ * Based on the saa7115 driver and on the first version of Chris Kennedy's
+ * cx25840 driver.
+ *
+ * Changes by Tyler Trafford <tatrafford@comcast.net>
+ *    - cleanup/rewrite for V4L2 API (2005)
+ *
+ * VBI support by Hans Verkuil <hverkuil@xs4all.nl>.
+ *
+ * NTSC sliced VBI support by Christopher Neufeld <television@cneufeld.ca>
+ * with additional fixes by Hans Verkuil <hverkuil@xs4all.nl>.
+ *
+ * CX23885 support by Steven Toth <stoth@linuxtv.org>.
+ *
+ * CX2388[578] IRQ handling, IO Pin mux configuration and other small fixes are
+ * Copyright (C) 2010 Andy Walls <awalls@md.metrocast.net>
+ *
+ * CX23888 DIF support for the HVR1850
+ * Copyright (C) 2011 Steven Toth <stoth@kernellabs.com>
+ *
+ * CX2584x pin to pad mapping and output format configuration support are
+ * Copyright (C) 2011 Maciej S. Szmigiero <mail@maciej.szmigiero.name>
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <linux/math64.h>
+#include <media/v4l2-common.h>
+#include <media/drv-intf/cx25840.h>
+
+#include "cx25840-core.h"
+
+MODULE_DESCRIPTION("Conexant CX25840 audio/video decoder driver");
+MODULE_AUTHOR("Ulf Eklund, Chris Kennedy, Hans Verkuil, Tyler Trafford");
+MODULE_LICENSE("GPL");
+
+#define CX25840_VID_INT_STAT_REG 0x410
+#define CX25840_VID_INT_STAT_BITS 0x0000ffff
+#define CX25840_VID_INT_MASK_BITS 0xffff0000
+#define CX25840_VID_INT_MASK_SHFT 16
+#define CX25840_VID_INT_MASK_REG 0x412
+
+#define CX23885_AUD_MC_INT_MASK_REG 0x80c
+#define CX23885_AUD_MC_INT_STAT_BITS 0xffff0000
+#define CX23885_AUD_MC_INT_CTRL_BITS 0x0000ffff
+#define CX23885_AUD_MC_INT_STAT_SHFT 16
+
+#define CX25840_AUD_INT_CTRL_REG 0x812
+#define CX25840_AUD_INT_STAT_REG 0x813
+
+#define CX23885_PIN_CTRL_IRQ_REG 0x123
+#define CX23885_PIN_CTRL_IRQ_IR_STAT  0x40
+#define CX23885_PIN_CTRL_IRQ_AUD_STAT 0x20
+#define CX23885_PIN_CTRL_IRQ_VID_STAT 0x10
+
+#define CX25840_IR_STATS_REG	0x210
+#define CX25840_IR_IRQEN_REG	0x214
+
+static int cx25840_debug;
+
+module_param_named(debug, cx25840_debug, int, 0644);
+
+MODULE_PARM_DESC(debug, "Debugging messages [0=Off (default) 1=On]");
+
+/* ----------------------------------------------------------------------- */
+static void cx23888_std_setup(struct i2c_client *client);
+
+int cx25840_write(struct i2c_client *client, u16 addr, u8 value)
+{
+	u8 buffer[3];
+
+	buffer[0] = addr >> 8;
+	buffer[1] = addr & 0xff;
+	buffer[2] = value;
+	return i2c_master_send(client, buffer, 3);
+}
+
+int cx25840_write4(struct i2c_client *client, u16 addr, u32 value)
+{
+	u8 buffer[6];
+
+	buffer[0] = addr >> 8;
+	buffer[1] = addr & 0xff;
+	buffer[2] = value & 0xff;
+	buffer[3] = (value >> 8) & 0xff;
+	buffer[4] = (value >> 16) & 0xff;
+	buffer[5] = value >> 24;
+	return i2c_master_send(client, buffer, 6);
+}
+
+u8 cx25840_read(struct i2c_client *client, u16 addr)
+{
+	struct i2c_msg msgs[2];
+	u8 tx_buf[2], rx_buf[1];
+
+	/* Write register address */
+	tx_buf[0] = addr >> 8;
+	tx_buf[1] = addr & 0xff;
+	msgs[0].addr = client->addr;
+	msgs[0].flags = 0;
+	msgs[0].len = 2;
+	msgs[0].buf = (char *)tx_buf;
+
+	/* Read data from register */
+	msgs[1].addr = client->addr;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].len = 1;
+	msgs[1].buf = (char *)rx_buf;
+
+	if (i2c_transfer(client->adapter, msgs, 2) < 2)
+		return 0;
+
+	return rx_buf[0];
+}
+
+u32 cx25840_read4(struct i2c_client *client, u16 addr)
+{
+	struct i2c_msg msgs[2];
+	u8 tx_buf[2], rx_buf[4];
+
+	/* Write register address */
+	tx_buf[0] = addr >> 8;
+	tx_buf[1] = addr & 0xff;
+	msgs[0].addr = client->addr;
+	msgs[0].flags = 0;
+	msgs[0].len = 2;
+	msgs[0].buf = (char *)tx_buf;
+
+	/* Read data from registers */
+	msgs[1].addr = client->addr;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].len = 4;
+	msgs[1].buf = (char *)rx_buf;
+
+	if (i2c_transfer(client->adapter, msgs, 2) < 2)
+		return 0;
+
+	return (rx_buf[3] << 24) | (rx_buf[2] << 16) | (rx_buf[1] << 8) |
+		rx_buf[0];
+}
+
+int cx25840_and_or(struct i2c_client *client, u16 addr, unsigned int and_mask,
+		   u8 or_value)
+{
+	return cx25840_write(client, addr,
+			     (cx25840_read(client, addr) & and_mask) |
+			     or_value);
+}
+
+int cx25840_and_or4(struct i2c_client *client, u16 addr, u32 and_mask,
+		    u32 or_value)
+{
+	return cx25840_write4(client, addr,
+			      (cx25840_read4(client, addr) & and_mask) |
+			      or_value);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static int set_input(struct i2c_client *client,
+		     enum cx25840_video_input vid_input,
+		     enum cx25840_audio_input aud_input);
+
+/* ----------------------------------------------------------------------- */
+
+static int cx23885_s_io_pin_config(struct v4l2_subdev *sd, size_t n,
+				   struct v4l2_subdev_io_pin_config *p)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int i;
+	u32 pin_ctrl;
+	u8 gpio_oe, gpio_data, strength;
+
+	pin_ctrl = cx25840_read4(client, 0x120);
+	gpio_oe = cx25840_read(client, 0x160);
+	gpio_data = cx25840_read(client, 0x164);
+
+	for (i = 0; i < n; i++) {
+		strength = p[i].strength;
+		if (strength > CX25840_PIN_DRIVE_FAST)
+			strength = CX25840_PIN_DRIVE_FAST;
+
+		switch (p[i].pin) {
+		case CX23885_PIN_IRQ_N_GPIO16:
+			if (p[i].function != CX23885_PAD_IRQ_N) {
+				/* GPIO16 */
+				pin_ctrl &= ~(0x1 << 25);
+			} else {
+				/* IRQ_N */
+				if (p[i].flags &
+					(BIT(V4L2_SUBDEV_IO_PIN_DISABLE) |
+					 BIT(V4L2_SUBDEV_IO_PIN_INPUT))) {
+					pin_ctrl &= ~(0x1 << 25);
+				} else {
+					pin_ctrl |= (0x1 << 25);
+				}
+				if (p[i].flags &
+					BIT(V4L2_SUBDEV_IO_PIN_ACTIVE_LOW)) {
+					pin_ctrl &= ~(0x1 << 24);
+				} else {
+					pin_ctrl |= (0x1 << 24);
+				}
+			}
+			break;
+		case CX23885_PIN_IR_RX_GPIO19:
+			if (p[i].function != CX23885_PAD_GPIO19) {
+				/* IR_RX */
+				gpio_oe |= (0x1 << 0);
+				pin_ctrl &= ~(0x3 << 18);
+				pin_ctrl |= (strength << 18);
+			} else {
+				/* GPIO19 */
+				gpio_oe &= ~(0x1 << 0);
+				if (p[i].flags & BIT(V4L2_SUBDEV_IO_PIN_SET_VALUE)) {
+					gpio_data &= ~(0x1 << 0);
+					gpio_data |= ((p[i].value & 0x1) << 0);
+				}
+				pin_ctrl &= ~(0x3 << 12);
+				pin_ctrl |= (strength << 12);
+			}
+			break;
+		case CX23885_PIN_IR_TX_GPIO20:
+			if (p[i].function != CX23885_PAD_GPIO20) {
+				/* IR_TX */
+				gpio_oe |= (0x1 << 1);
+				if (p[i].flags & BIT(V4L2_SUBDEV_IO_PIN_DISABLE))
+					pin_ctrl &= ~(0x1 << 10);
+				else
+					pin_ctrl |= (0x1 << 10);
+				pin_ctrl &= ~(0x3 << 18);
+				pin_ctrl |= (strength << 18);
+			} else {
+				/* GPIO20 */
+				gpio_oe &= ~(0x1 << 1);
+				if (p[i].flags & BIT(V4L2_SUBDEV_IO_PIN_SET_VALUE)) {
+					gpio_data &= ~(0x1 << 1);
+					gpio_data |= ((p[i].value & 0x1) << 1);
+				}
+				pin_ctrl &= ~(0x3 << 12);
+				pin_ctrl |= (strength << 12);
+			}
+			break;
+		case CX23885_PIN_I2S_SDAT_GPIO21:
+			if (p[i].function != CX23885_PAD_GPIO21) {
+				/* I2S_SDAT */
+				/* TODO: Input or Output config */
+				gpio_oe |= (0x1 << 2);
+				pin_ctrl &= ~(0x3 << 22);
+				pin_ctrl |= (strength << 22);
+			} else {
+				/* GPIO21 */
+				gpio_oe &= ~(0x1 << 2);
+				if (p[i].flags & BIT(V4L2_SUBDEV_IO_PIN_SET_VALUE)) {
+					gpio_data &= ~(0x1 << 2);
+					gpio_data |= ((p[i].value & 0x1) << 2);
+				}
+				pin_ctrl &= ~(0x3 << 12);
+				pin_ctrl |= (strength << 12);
+			}
+			break;
+		case CX23885_PIN_I2S_WCLK_GPIO22:
+			if (p[i].function != CX23885_PAD_GPIO22) {
+				/* I2S_WCLK */
+				/* TODO: Input or Output config */
+				gpio_oe |= (0x1 << 3);
+				pin_ctrl &= ~(0x3 << 22);
+				pin_ctrl |= (strength << 22);
+			} else {
+				/* GPIO22 */
+				gpio_oe &= ~(0x1 << 3);
+				if (p[i].flags & BIT(V4L2_SUBDEV_IO_PIN_SET_VALUE)) {
+					gpio_data &= ~(0x1 << 3);
+					gpio_data |= ((p[i].value & 0x1) << 3);
+				}
+				pin_ctrl &= ~(0x3 << 12);
+				pin_ctrl |= (strength << 12);
+			}
+			break;
+		case CX23885_PIN_I2S_BCLK_GPIO23:
+			if (p[i].function != CX23885_PAD_GPIO23) {
+				/* I2S_BCLK */
+				/* TODO: Input or Output config */
+				gpio_oe |= (0x1 << 4);
+				pin_ctrl &= ~(0x3 << 22);
+				pin_ctrl |= (strength << 22);
+			} else {
+				/* GPIO23 */
+				gpio_oe &= ~(0x1 << 4);
+				if (p[i].flags & BIT(V4L2_SUBDEV_IO_PIN_SET_VALUE)) {
+					gpio_data &= ~(0x1 << 4);
+					gpio_data |= ((p[i].value & 0x1) << 4);
+				}
+				pin_ctrl &= ~(0x3 << 12);
+				pin_ctrl |= (strength << 12);
+			}
+			break;
+		}
+	}
+
+	cx25840_write(client, 0x164, gpio_data);
+	cx25840_write(client, 0x160, gpio_oe);
+	cx25840_write4(client, 0x120, pin_ctrl);
+	return 0;
+}
+
+static u8 cx25840_function_to_pad(struct i2c_client *client, u8 function)
+{
+	if (function > CX25840_PAD_VRESET) {
+		v4l_err(client, "invalid function %u, assuming default\n",
+			(unsigned int)function);
+		return 0;
+	}
+
+	return function;
+}
+
+static void cx25840_set_invert(u8 *pinctrl3, u8 *voutctrl4, u8 function,
+			       u8 pin, bool invert)
+{
+	switch (function) {
+	case CX25840_PAD_IRQ_N:
+		if (invert)
+			*pinctrl3 &= ~2;
+		else
+			*pinctrl3 |= 2;
+		break;
+
+	case CX25840_PAD_ACTIVE:
+		if (invert)
+			*voutctrl4 |= BIT(2);
+		else
+			*voutctrl4 &= ~BIT(2);
+		break;
+
+	case CX25840_PAD_VACTIVE:
+		if (invert)
+			*voutctrl4 |= BIT(5);
+		else
+			*voutctrl4 &= ~BIT(5);
+		break;
+
+	case CX25840_PAD_CBFLAG:
+		if (invert)
+			*voutctrl4 |= BIT(4);
+		else
+			*voutctrl4 &= ~BIT(4);
+		break;
+
+	case CX25840_PAD_VRESET:
+		if (invert)
+			*voutctrl4 |= BIT(0);
+		else
+			*voutctrl4 &= ~BIT(0);
+		break;
+	}
+
+	if (function != CX25840_PAD_DEFAULT)
+		return;
+
+	switch (pin) {
+	case CX25840_PIN_DVALID_PRGM0:
+		if (invert)
+			*voutctrl4 |= BIT(6);
+		else
+			*voutctrl4 &= ~BIT(6);
+		break;
+
+	case CX25840_PIN_HRESET_PRGM2:
+		if (invert)
+			*voutctrl4 |= BIT(1);
+		else
+			*voutctrl4 &= ~BIT(1);
+		break;
+	}
+}
+
+static int cx25840_s_io_pin_config(struct v4l2_subdev *sd, size_t n,
+				   struct v4l2_subdev_io_pin_config *p)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	unsigned int i;
+	u8 pinctrl[6], pinconf[10], voutctrl4;
+
+	for (i = 0; i < 6; i++)
+		pinctrl[i] = cx25840_read(client, 0x114 + i);
+
+	for (i = 0; i < 10; i++)
+		pinconf[i] = cx25840_read(client, 0x11c + i);
+
+	voutctrl4 = cx25840_read(client, 0x407);
+
+	for (i = 0; i < n; i++) {
+		u8 strength = p[i].strength;
+
+		if (strength != CX25840_PIN_DRIVE_SLOW &&
+		    strength != CX25840_PIN_DRIVE_MEDIUM &&
+		    strength != CX25840_PIN_DRIVE_FAST) {
+			v4l_err(client,
+				"invalid drive speed for pin %u (%u), assuming fast\n",
+				(unsigned int)p[i].pin,
+				(unsigned int)strength);
+
+			strength = CX25840_PIN_DRIVE_FAST;
+		}
+
+		switch (p[i].pin) {
+		case CX25840_PIN_DVALID_PRGM0:
+			if (p[i].flags & BIT(V4L2_SUBDEV_IO_PIN_DISABLE))
+				pinctrl[0] &= ~BIT(6);
+			else
+				pinctrl[0] |= BIT(6);
+
+			pinconf[3] &= 0xf0;
+			pinconf[3] |= cx25840_function_to_pad(client,
+							      p[i].function);
+
+			cx25840_set_invert(&pinctrl[3], &voutctrl4,
+					   p[i].function,
+					   CX25840_PIN_DVALID_PRGM0,
+					   p[i].flags &
+					   BIT(V4L2_SUBDEV_IO_PIN_ACTIVE_LOW));
+
+			pinctrl[4] &= ~(3 << 2); /* CX25840_PIN_DRIVE_MEDIUM */
+			switch (strength) {
+			case CX25840_PIN_DRIVE_SLOW:
+				pinctrl[4] |= 1 << 2;
+				break;
+
+			case CX25840_PIN_DRIVE_FAST:
+				pinctrl[4] |= 2 << 2;
+				break;
+			}
+
+			break;
+
+		case CX25840_PIN_HRESET_PRGM2:
+			if (p[i].flags & BIT(V4L2_SUBDEV_IO_PIN_DISABLE))
+				pinctrl[1] &= ~BIT(0);
+			else
+				pinctrl[1] |= BIT(0);
+
+			pinconf[4] &= 0xf0;
+			pinconf[4] |= cx25840_function_to_pad(client,
+							      p[i].function);
+
+			cx25840_set_invert(&pinctrl[3], &voutctrl4,
+					   p[i].function,
+					   CX25840_PIN_HRESET_PRGM2,
+					   p[i].flags &
+					   BIT(V4L2_SUBDEV_IO_PIN_ACTIVE_LOW));
+
+			pinctrl[4] &= ~(3 << 2); /* CX25840_PIN_DRIVE_MEDIUM */
+			switch (strength) {
+			case CX25840_PIN_DRIVE_SLOW:
+				pinctrl[4] |= 1 << 2;
+				break;
+
+			case CX25840_PIN_DRIVE_FAST:
+				pinctrl[4] |= 2 << 2;
+				break;
+			}
+
+			break;
+
+		case CX25840_PIN_PLL_CLK_PRGM7:
+			if (p[i].flags & BIT(V4L2_SUBDEV_IO_PIN_DISABLE))
+				pinctrl[2] &= ~BIT(2);
+			else
+				pinctrl[2] |= BIT(2);
+
+			switch (p[i].function) {
+			case CX25840_PAD_XTI_X5_DLL:
+				pinconf[6] = 0;
+				break;
+
+			case CX25840_PAD_AUX_PLL:
+				pinconf[6] = 1;
+				break;
+
+			case CX25840_PAD_VID_PLL:
+				pinconf[6] = 5;
+				break;
+
+			case CX25840_PAD_XTI:
+				pinconf[6] = 2;
+				break;
+
+			default:
+				pinconf[6] = 3;
+				pinconf[6] |=
+					cx25840_function_to_pad(client,
+								p[i].function)
+					<< 4;
+			}
+
+			break;
+
+		default:
+			v4l_err(client, "invalid or unsupported pin %u\n",
+				(unsigned int)p[i].pin);
+			break;
+		}
+	}
+
+	cx25840_write(client, 0x407, voutctrl4);
+
+	for (i = 0; i < 6; i++)
+		cx25840_write(client, 0x114 + i, pinctrl[i]);
+
+	for (i = 0; i < 10; i++)
+		cx25840_write(client, 0x11c + i, pinconf[i]);
+
+	return 0;
+}
+
+static int common_s_io_pin_config(struct v4l2_subdev *sd, size_t n,
+				  struct v4l2_subdev_io_pin_config *pincfg)
+{
+	struct cx25840_state *state = to_state(sd);
+
+	if (is_cx2388x(state))
+		return cx23885_s_io_pin_config(sd, n, pincfg);
+	else if (is_cx2584x(state))
+		return cx25840_s_io_pin_config(sd, n, pincfg);
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static void init_dll1(struct i2c_client *client)
+{
+	/*
+	 * This is the Hauppauge sequence used to
+	 * initialize the Delay Lock Loop 1 (ADC DLL).
+	 */
+	cx25840_write(client, 0x159, 0x23);
+	cx25840_write(client, 0x15a, 0x87);
+	cx25840_write(client, 0x15b, 0x06);
+	udelay(10);
+	cx25840_write(client, 0x159, 0xe1);
+	udelay(10);
+	cx25840_write(client, 0x15a, 0x86);
+	cx25840_write(client, 0x159, 0xe0);
+	cx25840_write(client, 0x159, 0xe1);
+	cx25840_write(client, 0x15b, 0x10);
+}
+
+static void init_dll2(struct i2c_client *client)
+{
+	/*
+	 * This is the Hauppauge sequence used to
+	 * initialize the Delay Lock Loop 2 (ADC DLL).
+	 */
+	cx25840_write(client, 0x15d, 0xe3);
+	cx25840_write(client, 0x15e, 0x86);
+	cx25840_write(client, 0x15f, 0x06);
+	udelay(10);
+	cx25840_write(client, 0x15d, 0xe1);
+	cx25840_write(client, 0x15d, 0xe0);
+	cx25840_write(client, 0x15d, 0xe1);
+}
+
+static void cx25836_initialize(struct i2c_client *client)
+{
+	/*
+	 *reset configuration is described on page 3-77
+	 * of the CX25836 datasheet
+	 */
+
+	/* 2. */
+	cx25840_and_or(client, 0x000, ~0x01, 0x01);
+	cx25840_and_or(client, 0x000, ~0x01, 0x00);
+	/* 3a. */
+	cx25840_and_or(client, 0x15a, ~0x70, 0x00);
+	/* 3b. */
+	cx25840_and_or(client, 0x15b, ~0x1e, 0x06);
+	/* 3c. */
+	cx25840_and_or(client, 0x159, ~0x02, 0x02);
+	/* 3d. */
+	udelay(10);
+	/* 3e. */
+	cx25840_and_or(client, 0x159, ~0x02, 0x00);
+	/* 3f. */
+	cx25840_and_or(client, 0x159, ~0xc0, 0xc0);
+	/* 3g. */
+	cx25840_and_or(client, 0x159, ~0x01, 0x00);
+	cx25840_and_or(client, 0x159, ~0x01, 0x01);
+	/* 3h. */
+	cx25840_and_or(client, 0x15b, ~0x1e, 0x10);
+}
+
+static void cx25840_work_handler(struct work_struct *work)
+{
+	struct cx25840_state *state = container_of(work, struct cx25840_state, fw_work);
+
+	cx25840_loadfw(state->c);
+	wake_up(&state->fw_wait);
+}
+
+#define CX25840_VCONFIG_SET_BIT(state, opt_msk, voc, idx, bit, oneval)	\
+	do {								\
+		if ((state)->vid_config & (opt_msk)) {			\
+			if (((state)->vid_config & (opt_msk)) ==	\
+			    (oneval))					\
+				(voc)[idx] |= BIT(bit);		\
+			else						\
+				(voc)[idx] &= ~BIT(bit);		\
+		}							\
+	} while (0)
+
+/* apply current vconfig to hardware regs */
+static void cx25840_vconfig_apply(struct i2c_client *client)
+{
+	struct cx25840_state *state = to_state(i2c_get_clientdata(client));
+	u8 voutctrl[3];
+	unsigned int i;
+
+	for (i = 0; i < 3; i++)
+		voutctrl[i] = cx25840_read(client, 0x404 + i);
+
+	if (state->vid_config & CX25840_VCONFIG_FMT_MASK)
+		voutctrl[0] &= ~3;
+	switch (state->vid_config & CX25840_VCONFIG_FMT_MASK) {
+	case CX25840_VCONFIG_FMT_BT656:
+		voutctrl[0] |= 1;
+		break;
+
+	case CX25840_VCONFIG_FMT_VIP11:
+		voutctrl[0] |= 2;
+		break;
+
+	case CX25840_VCONFIG_FMT_VIP2:
+		voutctrl[0] |= 3;
+		break;
+
+	case CX25840_VCONFIG_FMT_BT601:
+		/* zero */
+	default:
+		break;
+	}
+
+	CX25840_VCONFIG_SET_BIT(state, CX25840_VCONFIG_RES_MASK, voutctrl,
+				0, 2, CX25840_VCONFIG_RES_10BIT);
+	CX25840_VCONFIG_SET_BIT(state, CX25840_VCONFIG_VBIRAW_MASK, voutctrl,
+				0, 3, CX25840_VCONFIG_VBIRAW_ENABLED);
+	CX25840_VCONFIG_SET_BIT(state, CX25840_VCONFIG_ANCDATA_MASK, voutctrl,
+				0, 4, CX25840_VCONFIG_ANCDATA_ENABLED);
+	CX25840_VCONFIG_SET_BIT(state, CX25840_VCONFIG_TASKBIT_MASK, voutctrl,
+				0, 5, CX25840_VCONFIG_TASKBIT_ONE);
+	CX25840_VCONFIG_SET_BIT(state, CX25840_VCONFIG_ACTIVE_MASK, voutctrl,
+				1, 2, CX25840_VCONFIG_ACTIVE_HORIZONTAL);
+	CX25840_VCONFIG_SET_BIT(state, CX25840_VCONFIG_VALID_MASK, voutctrl,
+				1, 3, CX25840_VCONFIG_VALID_ANDACTIVE);
+	CX25840_VCONFIG_SET_BIT(state, CX25840_VCONFIG_HRESETW_MASK, voutctrl,
+				1, 4, CX25840_VCONFIG_HRESETW_PIXCLK);
+
+	if (state->vid_config & CX25840_VCONFIG_CLKGATE_MASK)
+		voutctrl[1] &= ~(3 << 6);
+	switch (state->vid_config & CX25840_VCONFIG_CLKGATE_MASK) {
+	case CX25840_VCONFIG_CLKGATE_VALID:
+		voutctrl[1] |= 2;
+		break;
+
+	case CX25840_VCONFIG_CLKGATE_VALIDACTIVE:
+		voutctrl[1] |= 3;
+		break;
+
+	case CX25840_VCONFIG_CLKGATE_NONE:
+		/* zero */
+	default:
+		break;
+	}
+
+	CX25840_VCONFIG_SET_BIT(state, CX25840_VCONFIG_DCMODE_MASK, voutctrl,
+				2, 0, CX25840_VCONFIG_DCMODE_BYTES);
+	CX25840_VCONFIG_SET_BIT(state, CX25840_VCONFIG_IDID0S_MASK, voutctrl,
+				2, 1, CX25840_VCONFIG_IDID0S_LINECNT);
+	CX25840_VCONFIG_SET_BIT(state, CX25840_VCONFIG_VIPCLAMP_MASK, voutctrl,
+				2, 4, CX25840_VCONFIG_VIPCLAMP_ENABLED);
+
+	for (i = 0; i < 3; i++)
+		cx25840_write(client, 0x404 + i, voutctrl[i]);
+}
+
+static void cx25840_initialize(struct i2c_client *client)
+{
+	DEFINE_WAIT(wait);
+	struct cx25840_state *state = to_state(i2c_get_clientdata(client));
+	struct workqueue_struct *q;
+
+	/* datasheet startup in numbered steps, refer to page 3-77 */
+	/* 2. */
+	cx25840_and_or(client, 0x803, ~0x10, 0x00);
+	/*
+	 * The default of this register should be 4, but I get 0 instead.
+	 * Set this register to 4 manually.
+	 */
+	cx25840_write(client, 0x000, 0x04);
+	/* 3. */
+	init_dll1(client);
+	init_dll2(client);
+	cx25840_write(client, 0x136, 0x0a);
+	/* 4. */
+	cx25840_write(client, 0x13c, 0x01);
+	cx25840_write(client, 0x13c, 0x00);
+	/* 5. */
+	/*
+	 * Do the firmware load in a work handler to prevent.
+	 * Otherwise the kernel is blocked waiting for the
+	 * bit-banging i2c interface to finish uploading the
+	 * firmware.
+	 */
+	INIT_WORK(&state->fw_work, cx25840_work_handler);
+	init_waitqueue_head(&state->fw_wait);
+	q = create_singlethread_workqueue("cx25840_fw");
+	if (q) {
+		prepare_to_wait(&state->fw_wait, &wait, TASK_UNINTERRUPTIBLE);
+		queue_work(q, &state->fw_work);
+		schedule();
+		finish_wait(&state->fw_wait, &wait);
+		destroy_workqueue(q);
+	}
+
+	/* 6. */
+	cx25840_write(client, 0x115, 0x8c);
+	cx25840_write(client, 0x116, 0x07);
+	cx25840_write(client, 0x118, 0x02);
+	/* 7. */
+	cx25840_write(client, 0x4a5, 0x80);
+	cx25840_write(client, 0x4a5, 0x00);
+	cx25840_write(client, 0x402, 0x00);
+	/* 8. */
+	cx25840_and_or(client, 0x401, ~0x18, 0);
+	cx25840_and_or(client, 0x4a2, ~0x10, 0x10);
+	/* steps 8c and 8d are done in change_input() */
+	/* 10. */
+	cx25840_write(client, 0x8d3, 0x1f);
+	cx25840_write(client, 0x8e3, 0x03);
+
+	cx25840_std_setup(client);
+
+	/* trial and error says these are needed to get audio */
+	cx25840_write(client, 0x914, 0xa0);
+	cx25840_write(client, 0x918, 0xa0);
+	cx25840_write(client, 0x919, 0x01);
+
+	/* stereo preferred */
+	cx25840_write(client, 0x809, 0x04);
+	/* AC97 shift */
+	cx25840_write(client, 0x8cf, 0x0f);
+
+	/* (re)set input */
+	set_input(client, state->vid_input, state->aud_input);
+
+	if (state->generic_mode)
+		cx25840_vconfig_apply(client);
+
+	/* start microcontroller */
+	cx25840_and_or(client, 0x803, ~0x10, 0x10);
+}
+
+static void cx23885_initialize(struct i2c_client *client)
+{
+	DEFINE_WAIT(wait);
+	struct cx25840_state *state = to_state(i2c_get_clientdata(client));
+	u32 clk_freq = 0;
+	struct workqueue_struct *q;
+
+	/* cx23885 sets hostdata to clk_freq pointer */
+	if (v4l2_get_subdev_hostdata(&state->sd))
+		clk_freq = *((u32 *)v4l2_get_subdev_hostdata(&state->sd));
+
+	/*
+	 * Come out of digital power down
+	 * The CX23888, at least, needs this, otherwise registers aside from
+	 * 0x0-0x2 can't be read or written.
+	 */
+	cx25840_write(client, 0x000, 0);
+
+	/* Internal Reset */
+	cx25840_and_or(client, 0x102, ~0x01, 0x01);
+	cx25840_and_or(client, 0x102, ~0x01, 0x00);
+
+	/* Stop microcontroller */
+	cx25840_and_or(client, 0x803, ~0x10, 0x00);
+
+	/* DIF in reset? */
+	cx25840_write(client, 0x398, 0);
+
+	/*
+	 * Trust the default xtal, no division
+	 * '885: 28.636363... MHz
+	 * '887: 25.000000 MHz
+	 * '888: 50.000000 MHz
+	 */
+	cx25840_write(client, 0x2, 0x76);
+
+	/* Power up all the PLL's and DLL */
+	cx25840_write(client, 0x1, 0x40);
+
+	/* Sys PLL */
+	switch (state->id) {
+	case CX23888_AV:
+		/*
+		 * 50.0 MHz * (0xb + 0xe8ba26/0x2000000)/4 = 5 * 28.636363 MHz
+		 * 572.73 MHz before post divide
+		 */
+		if (clk_freq == 25000000) {
+			/* 888/ImpactVCBe or 25Mhz xtal */
+			; /* nothing to do */
+		} else {
+			/* HVR1850 or 50MHz xtal */
+			cx25840_write(client, 0x2, 0x71);
+		}
+		cx25840_write4(client, 0x11c, 0x01d1744c);
+		cx25840_write4(client, 0x118, 0x00000416);
+		cx25840_write4(client, 0x404, 0x0010253e);
+		cx25840_write4(client, 0x42c, 0x42600000);
+		cx25840_write4(client, 0x44c, 0x161f1000);
+		break;
+	case CX23887_AV:
+		/*
+		 * 25.0 MHz * (0x16 + 0x1d1744c/0x2000000)/4 = 5 * 28.636363 MHz
+		 * 572.73 MHz before post divide
+		 */
+		cx25840_write4(client, 0x11c, 0x01d1744c);
+		cx25840_write4(client, 0x118, 0x00000416);
+		break;
+	case CX23885_AV:
+	default:
+		/*
+		 * 28.636363 MHz * (0x14 + 0x0/0x2000000)/4 = 5 * 28.636363 MHz
+		 * 572.73 MHz before post divide
+		 */
+		cx25840_write4(client, 0x11c, 0x00000000);
+		cx25840_write4(client, 0x118, 0x00000414);
+		break;
+	}
+
+	/* Disable DIF bypass */
+	cx25840_write4(client, 0x33c, 0x00000001);
+
+	/* DIF Src phase inc */
+	cx25840_write4(client, 0x340, 0x0df7df83);
+
+	/*
+	 * Vid PLL
+	 * Setup for a BT.656 pixel clock of 13.5 Mpixels/second
+	 *
+	 * 28.636363 MHz * (0xf + 0x02be2c9/0x2000000)/4 = 8 * 13.5 MHz
+	 * 432.0 MHz before post divide
+	 */
+
+	/* HVR1850 */
+	switch (state->id) {
+	case CX23888_AV:
+		if (clk_freq == 25000000) {
+			/* 888/ImpactVCBe or 25MHz xtal */
+			cx25840_write4(client, 0x10c, 0x01b6db7b);
+			cx25840_write4(client, 0x108, 0x00000512);
+		} else {
+			/* 888/HVR1250 or 50MHz xtal */
+			cx25840_write4(client, 0x10c, 0x13333333);
+			cx25840_write4(client, 0x108, 0x00000515);
+		}
+		break;
+	default:
+		cx25840_write4(client, 0x10c, 0x002be2c9);
+		cx25840_write4(client, 0x108, 0x0000040f);
+	}
+
+	/* Luma */
+	cx25840_write4(client, 0x414, 0x00107d12);
+
+	/* Chroma */
+	if (is_cx23888(state))
+		cx25840_write4(client, 0x418, 0x1d008282);
+	else
+		cx25840_write4(client, 0x420, 0x3d008282);
+
+	/*
+	 * Aux PLL
+	 * Initial setup for audio sample clock:
+	 * 48 ksps, 16 bits/sample, x160 multiplier = 122.88 MHz
+	 * Initial I2S output/master clock(?):
+	 * 48 ksps, 16 bits/sample, x16 multiplier = 12.288 MHz
+	 */
+	switch (state->id) {
+	case CX23888_AV:
+		/*
+		 * 50.0 MHz * (0x7 + 0x0bedfa4/0x2000000)/3 = 122.88 MHz
+		 * 368.64 MHz before post divide
+		 * 122.88 MHz / 0xa = 12.288 MHz
+		 */
+		/* HVR1850 or 50MHz xtal or 25MHz xtal */
+		cx25840_write4(client, 0x114, 0x017dbf48);
+		cx25840_write4(client, 0x110, 0x000a030e);
+		break;
+	case CX23887_AV:
+		/*
+		 * 25.0 MHz * (0xe + 0x17dbf48/0x2000000)/3 = 122.88 MHz
+		 * 368.64 MHz before post divide
+		 * 122.88 MHz / 0xa = 12.288 MHz
+		 */
+		cx25840_write4(client, 0x114, 0x017dbf48);
+		cx25840_write4(client, 0x110, 0x000a030e);
+		break;
+	case CX23885_AV:
+	default:
+		/*
+		 * 28.636363 MHz * (0xc + 0x1bf0c9e/0x2000000)/3 = 122.88 MHz
+		 * 368.64 MHz before post divide
+		 * 122.88 MHz / 0xa = 12.288 MHz
+		 */
+		cx25840_write4(client, 0x114, 0x01bf0c9e);
+		cx25840_write4(client, 0x110, 0x000a030c);
+		break;
+	}
+
+	/* ADC2 input select */
+	cx25840_write(client, 0x102, 0x10);
+
+	/* VIN1 & VIN5 */
+	cx25840_write(client, 0x103, 0x11);
+
+	/* Enable format auto detect */
+	cx25840_write(client, 0x400, 0);
+	/* Fast subchroma lock */
+	/* White crush, Chroma AGC & Chroma Killer enabled */
+	cx25840_write(client, 0x401, 0xe8);
+
+	/* Select AFE clock pad output source */
+	cx25840_write(client, 0x144, 0x05);
+
+	/* Drive GPIO2 direction and values for HVR1700
+	 * where an onboard mux selects the output of demodulator
+	 * vs the 417. Failure to set this results in no DTV.
+	 * It's safe to set this across all Hauppauge boards
+	 * currently, regardless of the board type.
+	 */
+	cx25840_write(client, 0x160, 0x1d);
+	cx25840_write(client, 0x164, 0x00);
+
+	/*
+	 * Do the firmware load in a work handler to prevent.
+	 * Otherwise the kernel is blocked waiting for the
+	 * bit-banging i2c interface to finish uploading the
+	 * firmware.
+	 */
+	INIT_WORK(&state->fw_work, cx25840_work_handler);
+	init_waitqueue_head(&state->fw_wait);
+	q = create_singlethread_workqueue("cx25840_fw");
+	if (q) {
+		prepare_to_wait(&state->fw_wait, &wait, TASK_UNINTERRUPTIBLE);
+		queue_work(q, &state->fw_work);
+		schedule();
+		finish_wait(&state->fw_wait, &wait);
+		destroy_workqueue(q);
+	}
+
+	/*
+	 * Call the cx23888 specific std setup func, we no longer rely on
+	 * the generic cx24840 func.
+	 */
+	if (is_cx23888(state))
+		cx23888_std_setup(client);
+	else
+		cx25840_std_setup(client);
+
+	/* (re)set input */
+	set_input(client, state->vid_input, state->aud_input);
+
+	/* start microcontroller */
+	cx25840_and_or(client, 0x803, ~0x10, 0x10);
+
+	/* Disable and clear video interrupts - we don't use them */
+	cx25840_write4(client, CX25840_VID_INT_STAT_REG, 0xffffffff);
+
+	/* Disable and clear audio interrupts - we don't use them */
+	cx25840_write(client, CX25840_AUD_INT_CTRL_REG, 0xff);
+	cx25840_write(client, CX25840_AUD_INT_STAT_REG, 0xff);
+
+	/* CC raw enable */
+
+	/*
+	 *  - VIP 1.1 control codes - 10bit, blue field enable.
+	 *  - enable raw data during vertical blanking.
+	 *  - enable ancillary Data insertion for 656 or VIP.
+	 */
+	cx25840_write4(client, 0x404, 0x0010253e);
+
+	/* CC on  - VBI_LINE_CTRL3, FLD_VBI_MD_LINE12 */
+	cx25840_write(client, state->vbi_regs_offset + 0x42f, 0x66);
+
+	/* HVR-1250 / HVR1850 DIF related */
+	/* Power everything up */
+	cx25840_write4(client, 0x130, 0x0);
+
+	/* SRC_COMB_CFG */
+	if (is_cx23888(state))
+		cx25840_write4(client, 0x454, 0x6628021F);
+	else
+		cx25840_write4(client, 0x478, 0x6628021F);
+
+	/* AFE_CLK_OUT_CTRL - Select the clock output source as output */
+	cx25840_write4(client, 0x144, 0x5);
+
+	/* I2C_OUT_CTL - I2S output configuration as
+	 * Master, Sony, Left justified, left sample on WS=1
+	 */
+	cx25840_write4(client, 0x918, 0x1a0);
+
+	/* AFE_DIAG_CTRL1 */
+	cx25840_write4(client, 0x134, 0x000a1800);
+
+	/* AFE_DIAG_CTRL3 - Inverted Polarity for Audio and Video */
+	cx25840_write4(client, 0x13c, 0x00310000);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static void cx231xx_initialize(struct i2c_client *client)
+{
+	DEFINE_WAIT(wait);
+	struct cx25840_state *state = to_state(i2c_get_clientdata(client));
+	struct workqueue_struct *q;
+
+	/* Internal Reset */
+	cx25840_and_or(client, 0x102, ~0x01, 0x01);
+	cx25840_and_or(client, 0x102, ~0x01, 0x00);
+
+	/* Stop microcontroller */
+	cx25840_and_or(client, 0x803, ~0x10, 0x00);
+
+	/* DIF in reset? */
+	cx25840_write(client, 0x398, 0);
+
+	/* Trust the default xtal, no division */
+	/* This changes for the cx23888 products */
+	cx25840_write(client, 0x2, 0x76);
+
+	/* Bring down the regulator for AUX clk */
+	cx25840_write(client, 0x1, 0x40);
+
+	/* Disable DIF bypass */
+	cx25840_write4(client, 0x33c, 0x00000001);
+
+	/* DIF Src phase inc */
+	cx25840_write4(client, 0x340, 0x0df7df83);
+
+	/* Luma */
+	cx25840_write4(client, 0x414, 0x00107d12);
+
+	/* Chroma */
+	cx25840_write4(client, 0x420, 0x3d008282);
+
+	/* ADC2 input select */
+	cx25840_write(client, 0x102, 0x10);
+
+	/* VIN1 & VIN5 */
+	cx25840_write(client, 0x103, 0x11);
+
+	/* Enable format auto detect */
+	cx25840_write(client, 0x400, 0);
+	/* Fast subchroma lock */
+	/* White crush, Chroma AGC & Chroma Killer enabled */
+	cx25840_write(client, 0x401, 0xe8);
+
+	/*
+	 * Do the firmware load in a work handler to prevent.
+	 * Otherwise the kernel is blocked waiting for the
+	 * bit-banging i2c interface to finish uploading the
+	 * firmware.
+	 */
+	INIT_WORK(&state->fw_work, cx25840_work_handler);
+	init_waitqueue_head(&state->fw_wait);
+	q = create_singlethread_workqueue("cx25840_fw");
+	if (q) {
+		prepare_to_wait(&state->fw_wait, &wait, TASK_UNINTERRUPTIBLE);
+		queue_work(q, &state->fw_work);
+		schedule();
+		finish_wait(&state->fw_wait, &wait);
+		destroy_workqueue(q);
+	}
+
+	cx25840_std_setup(client);
+
+	/* (re)set input */
+	set_input(client, state->vid_input, state->aud_input);
+
+	/* start microcontroller */
+	cx25840_and_or(client, 0x803, ~0x10, 0x10);
+
+	/* CC raw enable */
+	cx25840_write(client, 0x404, 0x0b);
+
+	/* CC on */
+	cx25840_write(client, 0x42f, 0x66);
+	cx25840_write4(client, 0x474, 0x1e1e601a);
+}
+
+/* ----------------------------------------------------------------------- */
+
+void cx25840_std_setup(struct i2c_client *client)
+{
+	struct cx25840_state *state = to_state(i2c_get_clientdata(client));
+	v4l2_std_id std = state->std;
+	int hblank, hactive, burst, vblank, vactive, sc;
+	int vblank656, src_decimation;
+	int luma_lpf, uv_lpf, comb;
+	u32 pll_int, pll_frac, pll_post;
+
+	/* datasheet startup, step 8d */
+	if (std & ~V4L2_STD_NTSC)
+		cx25840_write(client, 0x49f, 0x11);
+	else
+		cx25840_write(client, 0x49f, 0x14);
+
+	/* generic mode uses the values that the chip autoconfig would set */
+	if (std & V4L2_STD_625_50) {
+		hblank = 132;
+		hactive = 720;
+		burst = 93;
+		if (state->generic_mode) {
+			vblank = 34;
+			vactive = 576;
+			vblank656 = 38;
+		} else {
+			vblank = 36;
+			vactive = 580;
+			vblank656 = 40;
+		}
+		src_decimation = 0x21f;
+		luma_lpf = 2;
+
+		if (std & V4L2_STD_SECAM) {
+			uv_lpf = 0;
+			comb = 0;
+			sc = 0x0a425f;
+		} else if (std == V4L2_STD_PAL_Nc) {
+			if (state->generic_mode) {
+				burst = 95;
+				luma_lpf = 1;
+			}
+			uv_lpf = 1;
+			comb = 0x20;
+			sc = 556453;
+		} else {
+			uv_lpf = 1;
+			comb = 0x20;
+			sc = 688739;
+		}
+	} else {
+		hactive = 720;
+		hblank = 122;
+		vactive = 487;
+		luma_lpf = 1;
+		uv_lpf = 1;
+		if (state->generic_mode) {
+			vblank = 20;
+			vblank656 = 24;
+		}
+
+		src_decimation = 0x21f;
+		if (std == V4L2_STD_PAL_60) {
+			if (!state->generic_mode) {
+				vblank = 26;
+				vblank656 = 26;
+				burst = 0x5b;
+			} else {
+				burst = 0x59;
+			}
+			luma_lpf = 2;
+			comb = 0x20;
+			sc = 688739;
+		} else if (std == V4L2_STD_PAL_M) {
+			vblank = 20;
+			vblank656 = 24;
+			burst = 0x61;
+			comb = 0x20;
+			sc = 555452;
+		} else {
+			if (!state->generic_mode) {
+				vblank = 26;
+				vblank656 = 26;
+			}
+			burst = 0x5b;
+			comb = 0x66;
+			sc = 556063;
+		}
+	}
+
+	/* DEBUG: Displays configured PLL frequency */
+	if (!is_cx231xx(state)) {
+		pll_int = cx25840_read(client, 0x108);
+		pll_frac = cx25840_read4(client, 0x10c) & 0x1ffffff;
+		pll_post = cx25840_read(client, 0x109);
+		v4l_dbg(1, cx25840_debug, client,
+			"PLL regs = int: %u, frac: %u, post: %u\n",
+			pll_int, pll_frac, pll_post);
+
+		if (pll_post) {
+			int fin, fsc;
+			int pll = (28636363L * ((((u64)pll_int) << 25L) + pll_frac)) >> 25L;
+
+			pll /= pll_post;
+			v4l_dbg(1, cx25840_debug, client,
+				"PLL = %d.%06d MHz\n",
+				pll / 1000000, pll % 1000000);
+			v4l_dbg(1, cx25840_debug, client,
+				"PLL/8 = %d.%06d MHz\n",
+				pll / 8000000, (pll / 8) % 1000000);
+
+			fin = ((u64)src_decimation * pll) >> 12;
+			v4l_dbg(1, cx25840_debug, client,
+				"ADC Sampling freq = %d.%06d MHz\n",
+				fin / 1000000, fin % 1000000);
+
+			fsc = (((u64)sc) * pll) >> 24L;
+			v4l_dbg(1, cx25840_debug, client,
+				"Chroma sub-carrier freq = %d.%06d MHz\n",
+				fsc / 1000000, fsc % 1000000);
+
+			v4l_dbg(1, cx25840_debug, client,
+				"hblank %i, hactive %i, vblank %i, vactive %i, vblank656 %i, src_dec %i, burst 0x%02x, luma_lpf %i, uv_lpf %i, comb 0x%02x, sc 0x%06x\n",
+				hblank, hactive, vblank, vactive, vblank656,
+				src_decimation, burst, luma_lpf, uv_lpf,
+				comb, sc);
+		}
+	}
+
+	/* Sets horizontal blanking delay and active lines */
+	cx25840_write(client, 0x470, hblank);
+	cx25840_write(client, 0x471,
+		      (((hblank >> 8) & 0x3) | (hactive << 4)) & 0xff);
+	cx25840_write(client, 0x472, hactive >> 4);
+
+	/* Sets burst gate delay */
+	cx25840_write(client, 0x473, burst);
+
+	/* Sets vertical blanking delay and active duration */
+	cx25840_write(client, 0x474, vblank);
+	cx25840_write(client, 0x475,
+		      (((vblank >> 8) & 0x3) | (vactive << 4)) & 0xff);
+	cx25840_write(client, 0x476, vactive >> 4);
+	cx25840_write(client, 0x477, vblank656);
+
+	/* Sets src decimation rate */
+	cx25840_write(client, 0x478, src_decimation & 0xff);
+	cx25840_write(client, 0x479, (src_decimation >> 8) & 0xff);
+
+	/* Sets Luma and UV Low pass filters */
+	cx25840_write(client, 0x47a, luma_lpf << 6 | ((uv_lpf << 4) & 0x30));
+
+	/* Enables comb filters */
+	cx25840_write(client, 0x47b, comb);
+
+	/* Sets SC Step*/
+	cx25840_write(client, 0x47c, sc);
+	cx25840_write(client, 0x47d, (sc >> 8) & 0xff);
+	cx25840_write(client, 0x47e, (sc >> 16) & 0xff);
+
+	/* Sets VBI parameters */
+	if (std & V4L2_STD_625_50) {
+		cx25840_write(client, 0x47f, 0x01);
+		state->vbi_line_offset = 5;
+	} else {
+		cx25840_write(client, 0x47f, 0x00);
+		state->vbi_line_offset = 8;
+	}
+}
+
+/* ----------------------------------------------------------------------- */
+
+static void input_change(struct i2c_client *client)
+{
+	struct cx25840_state *state = to_state(i2c_get_clientdata(client));
+	v4l2_std_id std = state->std;
+
+	/* Follow step 8c and 8d of section 3.16 in the cx25840 datasheet */
+	if (std & V4L2_STD_SECAM) {
+		cx25840_write(client, 0x402, 0);
+	} else {
+		cx25840_write(client, 0x402, 0x04);
+		cx25840_write(client, 0x49f,
+			      (std & V4L2_STD_NTSC) ? 0x14 : 0x11);
+	}
+	cx25840_and_or(client, 0x401, ~0x60, 0);
+	cx25840_and_or(client, 0x401, ~0x60, 0x60);
+
+	/* Don't write into audio registers on cx2583x chips */
+	if (is_cx2583x(state))
+		return;
+
+	cx25840_and_or(client, 0x810, ~0x01, 1);
+
+	if (state->radio) {
+		cx25840_write(client, 0x808, 0xf9);
+		cx25840_write(client, 0x80b, 0x00);
+	} else if (std & V4L2_STD_525_60) {
+		/*
+		 * Certain Hauppauge PVR150 models have a hardware bug
+		 * that causes audio to drop out. For these models the
+		 * audio standard must be set explicitly.
+		 * To be precise: it affects cards with tuner models
+		 * 85, 99 and 112 (model numbers from tveeprom).
+		 */
+		int hw_fix = state->pvr150_workaround;
+
+		if (std == V4L2_STD_NTSC_M_JP) {
+			/* Japan uses EIAJ audio standard */
+			cx25840_write(client, 0x808, hw_fix ? 0x2f : 0xf7);
+		} else if (std == V4L2_STD_NTSC_M_KR) {
+			/* South Korea uses A2 audio standard */
+			cx25840_write(client, 0x808, hw_fix ? 0x3f : 0xf8);
+		} else {
+			/* Others use the BTSC audio standard */
+			cx25840_write(client, 0x808, hw_fix ? 0x1f : 0xf6);
+		}
+		cx25840_write(client, 0x80b, 0x00);
+	} else if (std & V4L2_STD_PAL) {
+		/* Autodetect audio standard and audio system */
+		cx25840_write(client, 0x808, 0xff);
+		/*
+		 * Since system PAL-L is pretty much non-existent and
+		 * not used by any public broadcast network, force
+		 * 6.5 MHz carrier to be interpreted as System DK,
+		 * this avoids DK audio detection instability
+		 */
+		cx25840_write(client, 0x80b, 0x00);
+	} else if (std & V4L2_STD_SECAM) {
+		/* Autodetect audio standard and audio system */
+		cx25840_write(client, 0x808, 0xff);
+		/*
+		 * If only one of SECAM-DK / SECAM-L is required, then force
+		 * 6.5MHz carrier, else autodetect it
+		 */
+		if ((std & V4L2_STD_SECAM_DK) &&
+		    !(std & (V4L2_STD_SECAM_L | V4L2_STD_SECAM_LC))) {
+			/* 6.5 MHz carrier to be interpreted as System DK */
+			cx25840_write(client, 0x80b, 0x00);
+		} else if (!(std & V4L2_STD_SECAM_DK) &&
+			   (std & (V4L2_STD_SECAM_L | V4L2_STD_SECAM_LC))) {
+			/* 6.5 MHz carrier to be interpreted as System L */
+			cx25840_write(client, 0x80b, 0x08);
+		} else {
+			/* 6.5 MHz carrier to be autodetected */
+			cx25840_write(client, 0x80b, 0x10);
+		}
+	}
+
+	cx25840_and_or(client, 0x810, ~0x01, 0);
+}
+
+static int set_input(struct i2c_client *client,
+		     enum cx25840_video_input vid_input,
+		     enum cx25840_audio_input aud_input)
+{
+	struct cx25840_state *state = to_state(i2c_get_clientdata(client));
+	u8 is_composite = (vid_input >= CX25840_COMPOSITE1 &&
+			   vid_input <= CX25840_COMPOSITE8);
+	u8 is_component = (vid_input & CX25840_COMPONENT_ON) ==
+			CX25840_COMPONENT_ON;
+	u8 is_dif = (vid_input & CX25840_DIF_ON) ==
+			CX25840_DIF_ON;
+	u8 is_svideo = (vid_input & CX25840_SVIDEO_ON) ==
+			CX25840_SVIDEO_ON;
+	int luma = vid_input & 0xf0;
+	int chroma = vid_input & 0xf00;
+	u8 reg;
+	u32 val;
+
+	v4l_dbg(1, cx25840_debug, client,
+		"decoder set video input %d, audio input %d\n",
+		vid_input, aud_input);
+
+	if (vid_input >= CX25840_VIN1_CH1) {
+		v4l_dbg(1, cx25840_debug, client, "vid_input 0x%x\n",
+			vid_input);
+		reg = vid_input & 0xff;
+		is_composite = !is_component &&
+			       ((vid_input & CX25840_SVIDEO_ON) != CX25840_SVIDEO_ON);
+
+		v4l_dbg(1, cx25840_debug, client, "mux cfg 0x%x comp=%d\n",
+			reg, is_composite);
+	} else if (is_composite) {
+		reg = 0xf0 + (vid_input - CX25840_COMPOSITE1);
+	} else {
+		if ((vid_input & ~0xff0) ||
+		    luma < CX25840_SVIDEO_LUMA1 ||
+		    luma > CX25840_SVIDEO_LUMA8 ||
+		    chroma < CX25840_SVIDEO_CHROMA4 ||
+		    chroma > CX25840_SVIDEO_CHROMA8) {
+			v4l_err(client, "0x%04x is not a valid video input!\n",
+				vid_input);
+			return -EINVAL;
+		}
+		reg = 0xf0 + ((luma - CX25840_SVIDEO_LUMA1) >> 4);
+		if (chroma >= CX25840_SVIDEO_CHROMA7) {
+			reg &= 0x3f;
+			reg |= (chroma - CX25840_SVIDEO_CHROMA7) >> 2;
+		} else {
+			reg &= 0xcf;
+			reg |= (chroma - CX25840_SVIDEO_CHROMA4) >> 4;
+		}
+	}
+
+	/* The caller has previously prepared the correct routing
+	 * configuration in reg (for the cx23885) so we have no
+	 * need to attempt to flip bits for earlier av decoders.
+	 */
+	if (!is_cx2388x(state) && !is_cx231xx(state)) {
+		switch (aud_input) {
+		case CX25840_AUDIO_SERIAL:
+			/* do nothing, use serial audio input */
+			break;
+		case CX25840_AUDIO4:
+			reg &= ~0x30;
+			break;
+		case CX25840_AUDIO5:
+			reg &= ~0x30;
+			reg |= 0x10;
+			break;
+		case CX25840_AUDIO6:
+			reg &= ~0x30;
+			reg |= 0x20;
+			break;
+		case CX25840_AUDIO7:
+			reg &= ~0xc0;
+			break;
+		case CX25840_AUDIO8:
+			reg &= ~0xc0;
+			reg |= 0x40;
+			break;
+		default:
+			v4l_err(client, "0x%04x is not a valid audio input!\n",
+				aud_input);
+			return -EINVAL;
+		}
+	}
+
+	cx25840_write(client, 0x103, reg);
+
+	/* Set INPUT_MODE to Composite, S-Video or Component */
+	if (is_component)
+		cx25840_and_or(client, 0x401, ~0x6, 0x6);
+	else
+		cx25840_and_or(client, 0x401, ~0x6, is_composite ? 0 : 0x02);
+
+	if (is_cx2388x(state)) {
+		/* Enable or disable the DIF for tuner use */
+		if (is_dif) {
+			cx25840_and_or(client, 0x102, ~0x80, 0x80);
+
+			/* Set of defaults for NTSC and PAL */
+			cx25840_write4(client, 0x31c, 0xc2262600);
+			cx25840_write4(client, 0x320, 0xc2262600);
+
+			/* 18271 IF - Nobody else yet uses a different
+			 * tuner with the DIF, so these are reasonable
+			 * assumptions (HVR1250 and HVR1850 specific).
+			 */
+			cx25840_write4(client, 0x318, 0xda262600);
+			cx25840_write4(client, 0x33c, 0x2a24c800);
+			cx25840_write4(client, 0x104, 0x0704dd00);
+		} else {
+			cx25840_write4(client, 0x300, 0x015c28f5);
+
+			cx25840_and_or(client, 0x102, ~0x80, 0);
+			cx25840_write4(client, 0x340, 0xdf7df83);
+			cx25840_write4(client, 0x104, 0x0704dd80);
+			cx25840_write4(client, 0x314, 0x22400600);
+			cx25840_write4(client, 0x318, 0x40002600);
+			cx25840_write4(client, 0x324, 0x40002600);
+			cx25840_write4(client, 0x32c, 0x0250e620);
+			cx25840_write4(client, 0x39c, 0x01FF0B00);
+
+			cx25840_write4(client, 0x410, 0xffff0dbf);
+			cx25840_write4(client, 0x414, 0x00137d03);
+
+			if (is_cx23888(state)) {
+				/* 888 MISC_TIM_CTRL */
+				cx25840_write4(client, 0x42c, 0x42600000);
+				/* 888 FIELD_COUNT */
+				cx25840_write4(client, 0x430, 0x0000039b);
+				/* 888 VSCALE_CTRL */
+				cx25840_write4(client, 0x438, 0x00000000);
+				/* 888 DFE_CTRL1 */
+				cx25840_write4(client, 0x440, 0xF8E3E824);
+				/* 888 DFE_CTRL2 */
+				cx25840_write4(client, 0x444, 0x401040dc);
+				/* 888 DFE_CTRL3 */
+				cx25840_write4(client, 0x448, 0xcd3f02a0);
+				/* 888 PLL_CTRL */
+				cx25840_write4(client, 0x44c, 0x161f1000);
+				/* 888 HTL_CTRL */
+				cx25840_write4(client, 0x450, 0x00000802);
+			}
+			cx25840_write4(client, 0x91c, 0x01000000);
+			cx25840_write4(client, 0x8e0, 0x03063870);
+			cx25840_write4(client, 0x8d4, 0x7FFF0024);
+			cx25840_write4(client, 0x8d0, 0x00063073);
+
+			cx25840_write4(client, 0x8c8, 0x00010000);
+			cx25840_write4(client, 0x8cc, 0x00080023);
+
+			/* DIF BYPASS */
+			cx25840_write4(client, 0x33c, 0x2a04c800);
+		}
+
+		/* Reset the DIF */
+		cx25840_write4(client, 0x398, 0);
+	}
+
+	if (!is_cx2388x(state) && !is_cx231xx(state)) {
+		/* Set CH_SEL_ADC2 to 1 if input comes from CH3 */
+		cx25840_and_or(client, 0x102, ~0x2, (reg & 0x80) == 0 ? 2 : 0);
+		/* Set DUAL_MODE_ADC2 to 1 if input comes from both CH2&CH3 */
+		if ((reg & 0xc0) != 0xc0 && (reg & 0x30) != 0x30)
+			cx25840_and_or(client, 0x102, ~0x4, 4);
+		else
+			cx25840_and_or(client, 0x102, ~0x4, 0);
+	} else {
+		/* Set DUAL_MODE_ADC2 to 1 if component*/
+		cx25840_and_or(client, 0x102, ~0x4, is_component ? 0x4 : 0x0);
+		if (is_composite) {
+			/* ADC2 input select channel 2 */
+			cx25840_and_or(client, 0x102, ~0x2, 0);
+		} else if (!is_component) {
+			/* S-Video */
+			if (chroma >= CX25840_SVIDEO_CHROMA7) {
+				/* ADC2 input select channel 3 */
+				cx25840_and_or(client, 0x102, ~0x2, 2);
+			} else {
+				/* ADC2 input select channel 2 */
+				cx25840_and_or(client, 0x102, ~0x2, 0);
+			}
+		}
+
+		/* cx23885 / SVIDEO */
+		if (is_cx2388x(state) && is_svideo) {
+#define AFE_CTRL  (0x104)
+#define MODE_CTRL (0x400)
+			cx25840_and_or(client, 0x102, ~0x2, 0x2);
+
+			val = cx25840_read4(client, MODE_CTRL);
+			val &= 0xFFFFF9FF;
+
+			/* YC */
+			val |= 0x00000200;
+			val &= ~0x2000;
+			cx25840_write4(client, MODE_CTRL, val);
+
+			val = cx25840_read4(client, AFE_CTRL);
+
+			/* Chroma in select */
+			val |= 0x00001000;
+			val &= 0xfffffe7f;
+			/* Clear VGA_SEL_CH2 and VGA_SEL_CH3 (bits 7 and 8).
+			 * This sets them to use video rather than audio.
+			 * Only one of the two will be in use.
+			 */
+			cx25840_write4(client, AFE_CTRL, val);
+		} else {
+			cx25840_and_or(client, 0x102, ~0x2, 0);
+		}
+	}
+
+	state->vid_input = vid_input;
+	state->aud_input = aud_input;
+	cx25840_audio_set_path(client);
+	input_change(client);
+
+	if (is_cx2388x(state)) {
+		/* Audio channel 1 src : Parallel 1 */
+		cx25840_write(client, 0x124, 0x03);
+
+		/* Select AFE clock pad output source */
+		cx25840_write(client, 0x144, 0x05);
+
+		/* I2S_IN_CTL: I2S_IN_SONY_MODE, LEFT SAMPLE on WS=1 */
+		cx25840_write(client, 0x914, 0xa0);
+
+		/* I2S_OUT_CTL:
+		 * I2S_IN_SONY_MODE, LEFT SAMPLE on WS=1
+		 * I2S_OUT_MASTER_MODE = Master
+		 */
+		cx25840_write(client, 0x918, 0xa0);
+		cx25840_write(client, 0x919, 0x01);
+	} else if (is_cx231xx(state)) {
+		/* Audio channel 1 src : Parallel 1 */
+		cx25840_write(client, 0x124, 0x03);
+
+		/* I2S_IN_CTL: I2S_IN_SONY_MODE, LEFT SAMPLE on WS=1 */
+		cx25840_write(client, 0x914, 0xa0);
+
+		/* I2S_OUT_CTL:
+		 * I2S_IN_SONY_MODE, LEFT SAMPLE on WS=1
+		 * I2S_OUT_MASTER_MODE = Master
+		 */
+		cx25840_write(client, 0x918, 0xa0);
+		cx25840_write(client, 0x919, 0x01);
+	}
+
+	if (is_cx2388x(state) &&
+	    ((aud_input == CX25840_AUDIO7) || (aud_input == CX25840_AUDIO6))) {
+		/* Configure audio from LR1 or LR2 input */
+		cx25840_write4(client, 0x910, 0);
+		cx25840_write4(client, 0x8d0, 0x63073);
+	} else if (is_cx2388x(state) && (aud_input == CX25840_AUDIO8)) {
+		/* Configure audio from tuner/sif input */
+		cx25840_write4(client, 0x910, 0x12b000c9);
+		cx25840_write4(client, 0x8d0, 0x1f063870);
+	}
+
+	if (is_cx23888(state)) {
+		/*
+		 * HVR1850
+		 *
+		 * AUD_IO_CTRL - I2S Input, Parallel1
+		 *  - Channel 1 src - Parallel1 (Merlin out)
+		 *  - Channel 2 src - Parallel2 (Merlin out)
+		 *  - Channel 3 src - Parallel3 (Merlin AC97 out)
+		 *  - I2S source and dir - Merlin, output
+		 */
+		cx25840_write4(client, 0x124, 0x100);
+
+		if (!is_dif) {
+			/*
+			 * Stop microcontroller if we don't need it
+			 * to avoid audio popping on svideo/composite use.
+			 */
+			cx25840_and_or(client, 0x803, ~0x10, 0x00);
+		}
+	}
+
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static int set_v4lstd(struct i2c_client *client)
+{
+	struct cx25840_state *state = to_state(i2c_get_clientdata(client));
+	u8 fmt = 0;	/* zero is autodetect */
+	u8 pal_m = 0;
+
+	/* First tests should be against specific std */
+	if (state->std == V4L2_STD_NTSC_M_JP) {
+		fmt = 0x2;
+	} else if (state->std == V4L2_STD_NTSC_443) {
+		fmt = 0x3;
+	} else if (state->std == V4L2_STD_PAL_M) {
+		pal_m = 1;
+		fmt = 0x5;
+	} else if (state->std == V4L2_STD_PAL_N) {
+		fmt = 0x6;
+	} else if (state->std == V4L2_STD_PAL_Nc) {
+		fmt = 0x7;
+	} else if (state->std == V4L2_STD_PAL_60) {
+		fmt = 0x8;
+	} else {
+		/* Then, test against generic ones */
+		if (state->std & V4L2_STD_NTSC)
+			fmt = 0x1;
+		else if (state->std & V4L2_STD_PAL)
+			fmt = 0x4;
+		else if (state->std & V4L2_STD_SECAM)
+			fmt = 0xc;
+	}
+
+	v4l_dbg(1, cx25840_debug, client,
+		"changing video std to fmt %i\n", fmt);
+
+	/*
+	 * Follow step 9 of section 3.16 in the cx25840 datasheet.
+	 * Without this PAL may display a vertical ghosting effect.
+	 * This happens for example with the Yuan MPC622.
+	 */
+	if (fmt >= 4 && fmt < 8) {
+		/* Set format to NTSC-M */
+		cx25840_and_or(client, 0x400, ~0xf, 1);
+		/* Turn off LCOMB */
+		cx25840_and_or(client, 0x47b, ~6, 0);
+	}
+	cx25840_and_or(client, 0x400, ~0xf, fmt);
+	cx25840_and_or(client, 0x403, ~0x3, pal_m);
+	if (is_cx23888(state))
+		cx23888_std_setup(client);
+	else
+		cx25840_std_setup(client);
+	if (!is_cx2583x(state))
+		input_change(client);
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static int cx25840_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+	struct cx25840_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		cx25840_write(client, 0x414, ctrl->val - 128);
+		break;
+
+	case V4L2_CID_CONTRAST:
+		cx25840_write(client, 0x415, ctrl->val << 1);
+		break;
+
+	case V4L2_CID_SATURATION:
+		if (is_cx23888(state)) {
+			cx25840_write(client, 0x418, ctrl->val << 1);
+			cx25840_write(client, 0x419, ctrl->val << 1);
+		} else {
+			cx25840_write(client, 0x420, ctrl->val << 1);
+			cx25840_write(client, 0x421, ctrl->val << 1);
+		}
+		break;
+
+	case V4L2_CID_HUE:
+		if (is_cx23888(state))
+			cx25840_write(client, 0x41a, ctrl->val);
+		else
+			cx25840_write(client, 0x422, ctrl->val);
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static int cx25840_set_fmt(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_state *sd_state,
+			   struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+	struct cx25840_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u32 hsc, vsc, v_src, h_src, v_add;
+	int filter;
+	int is_50hz = !(state->std & V4L2_STD_525_60);
+
+	if (format->pad || fmt->code != MEDIA_BUS_FMT_FIXED)
+		return -EINVAL;
+
+	fmt->field = V4L2_FIELD_INTERLACED;
+	fmt->colorspace = V4L2_COLORSPACE_SMPTE170M;
+
+	if (is_cx23888(state)) {
+		v_src = (cx25840_read(client, 0x42a) & 0x3f) << 4;
+		v_src |= (cx25840_read(client, 0x429) & 0xf0) >> 4;
+	} else {
+		v_src = (cx25840_read(client, 0x476) & 0x3f) << 4;
+		v_src |= (cx25840_read(client, 0x475) & 0xf0) >> 4;
+	}
+
+	if (is_cx23888(state)) {
+		h_src = (cx25840_read(client, 0x426) & 0x3f) << 4;
+		h_src |= (cx25840_read(client, 0x425) & 0xf0) >> 4;
+	} else {
+		h_src = (cx25840_read(client, 0x472) & 0x3f) << 4;
+		h_src |= (cx25840_read(client, 0x471) & 0xf0) >> 4;
+	}
+
+	if (!state->generic_mode) {
+		v_add = is_50hz ? 4 : 7;
+
+		/*
+		 * cx23888 in 525-line mode is programmed for 486 active lines
+		 * while other chips use 487 active lines.
+		 *
+		 * See reg 0x428 bits [21:12] in cx23888_std_setup() vs
+		 * vactive in cx25840_std_setup().
+		 */
+		if (is_cx23888(state) && !is_50hz)
+			v_add--;
+	} else {
+		v_add = 0;
+	}
+
+	if (h_src == 0 ||
+	    v_src <= v_add) {
+		v4l_err(client,
+			"chip reported picture size (%u x %u) is far too small\n",
+			(unsigned int)h_src, (unsigned int)v_src);
+		/*
+		 * that's the best we can do since the output picture
+		 * size is completely unknown in this case
+		 */
+		return -EINVAL;
+	}
+
+	fmt->width = clamp(fmt->width, (h_src + 15) / 16, h_src);
+
+	if (v_add * 8 >= v_src)
+		fmt->height = clamp(fmt->height, (u32)1, v_src - v_add);
+	else
+		fmt->height = clamp(fmt->height, (v_src - v_add * 8 + 7) / 8,
+				    v_src - v_add);
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+		return 0;
+
+	hsc = (h_src * (1 << 20)) / fmt->width - (1 << 20);
+	vsc = (1 << 16) - (v_src * (1 << 9) / (fmt->height + v_add) - (1 << 9));
+	vsc &= 0x1fff;
+
+	if (fmt->width >= 385)
+		filter = 0;
+	else if (fmt->width > 192)
+		filter = 1;
+	else if (fmt->width > 96)
+		filter = 2;
+	else
+		filter = 3;
+
+	v4l_dbg(1, cx25840_debug, client,
+		"decoder set size %u x %u with scale %x x %x\n",
+		(unsigned int)fmt->width, (unsigned int)fmt->height,
+		(unsigned int)hsc, (unsigned int)vsc);
+
+	/* HSCALE=hsc */
+	if (is_cx23888(state)) {
+		cx25840_write4(client, 0x434, hsc | (1 << 24));
+		/* VSCALE=vsc VS_INTRLACE=1 VFILT=filter */
+		cx25840_write4(client, 0x438, vsc | (1 << 19) | (filter << 16));
+	} else {
+		cx25840_write(client, 0x418, hsc & 0xff);
+		cx25840_write(client, 0x419, (hsc >> 8) & 0xff);
+		cx25840_write(client, 0x41a, hsc >> 16);
+		/* VSCALE=vsc */
+		cx25840_write(client, 0x41c, vsc & 0xff);
+		cx25840_write(client, 0x41d, vsc >> 8);
+		/* VS_INTRLACE=1 VFILT=filter */
+		cx25840_write(client, 0x41e, 0x8 | filter);
+	}
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static void log_video_status(struct i2c_client *client)
+{
+	static const char *const fmt_strs[] = {
+		"0x0",
+		"NTSC-M", "NTSC-J", "NTSC-4.43",
+		"PAL-BDGHI", "PAL-M", "PAL-N", "PAL-Nc", "PAL-60",
+		"0x9", "0xA", "0xB",
+		"SECAM",
+		"0xD", "0xE", "0xF"
+	};
+
+	struct cx25840_state *state = to_state(i2c_get_clientdata(client));
+	u8 vidfmt_sel = cx25840_read(client, 0x400) & 0xf;
+	u8 gen_stat1 = cx25840_read(client, 0x40d);
+	u8 gen_stat2 = cx25840_read(client, 0x40e);
+	int vid_input = state->vid_input;
+
+	v4l_info(client, "Video signal:              %spresent\n",
+		 (gen_stat2 & 0x20) ? "" : "not ");
+	v4l_info(client, "Detected format:           %s\n",
+		 fmt_strs[gen_stat1 & 0xf]);
+
+	v4l_info(client, "Specified standard:        %s\n",
+		 vidfmt_sel ? fmt_strs[vidfmt_sel] : "automatic detection");
+
+	if (vid_input >= CX25840_COMPOSITE1 &&
+	    vid_input <= CX25840_COMPOSITE8) {
+		v4l_info(client, "Specified video input:     Composite %d\n",
+			 vid_input - CX25840_COMPOSITE1 + 1);
+	} else {
+		v4l_info(client,
+			 "Specified video input:     S-Video (Luma In%d, Chroma In%d)\n",
+			 (vid_input & 0xf0) >> 4, (vid_input & 0xf00) >> 8);
+	}
+
+	v4l_info(client, "Specified audioclock freq: %d Hz\n",
+		 state->audclk_freq);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static void log_audio_status(struct i2c_client *client)
+{
+	struct cx25840_state *state = to_state(i2c_get_clientdata(client));
+	u8 download_ctl = cx25840_read(client, 0x803);
+	u8 mod_det_stat0 = cx25840_read(client, 0x804);
+	u8 mod_det_stat1 = cx25840_read(client, 0x805);
+	u8 audio_config = cx25840_read(client, 0x808);
+	u8 pref_mode = cx25840_read(client, 0x809);
+	u8 afc0 = cx25840_read(client, 0x80b);
+	u8 mute_ctl = cx25840_read(client, 0x8d3);
+	int aud_input = state->aud_input;
+	char *p;
+
+	switch (mod_det_stat0) {
+	case 0x00:
+		p = "mono";
+		break;
+	case 0x01:
+		p = "stereo";
+		break;
+	case 0x02:
+		p = "dual";
+		break;
+	case 0x04:
+		p = "tri";
+		break;
+	case 0x10:
+		p = "mono with SAP";
+		break;
+	case 0x11:
+		p = "stereo with SAP";
+		break;
+	case 0x12:
+		p = "dual with SAP";
+		break;
+	case 0x14:
+		p = "tri with SAP";
+		break;
+	case 0xfe:
+		p = "forced mode";
+		break;
+	default:
+		p = "not defined";
+	}
+	v4l_info(client, "Detected audio mode:       %s\n", p);
+
+	switch (mod_det_stat1) {
+	case 0x00:
+		p = "not defined";
+		break;
+	case 0x01:
+		p = "EIAJ";
+		break;
+	case 0x02:
+		p = "A2-M";
+		break;
+	case 0x03:
+		p = "A2-BG";
+		break;
+	case 0x04:
+		p = "A2-DK1";
+		break;
+	case 0x05:
+		p = "A2-DK2";
+		break;
+	case 0x06:
+		p = "A2-DK3";
+		break;
+	case 0x07:
+		p = "A1 (6.0 MHz FM Mono)";
+		break;
+	case 0x08:
+		p = "AM-L";
+		break;
+	case 0x09:
+		p = "NICAM-BG";
+		break;
+	case 0x0a:
+		p = "NICAM-DK";
+		break;
+	case 0x0b:
+		p = "NICAM-I";
+		break;
+	case 0x0c:
+		p = "NICAM-L";
+		break;
+	case 0x0d:
+		p = "BTSC/EIAJ/A2-M Mono (4.5 MHz FMMono)";
+		break;
+	case 0x0e:
+		p = "IF FM Radio";
+		break;
+	case 0x0f:
+		p = "BTSC";
+		break;
+	case 0x10:
+		p = "high-deviation FM";
+		break;
+	case 0x11:
+		p = "very high-deviation FM";
+		break;
+	case 0xfd:
+		p = "unknown audio standard";
+		break;
+	case 0xfe:
+		p = "forced audio standard";
+		break;
+	case 0xff:
+		p = "no detected audio standard";
+		break;
+	default:
+		p = "not defined";
+	}
+	v4l_info(client, "Detected audio standard:   %s\n", p);
+	v4l_info(client, "Audio microcontroller:     %s\n",
+		 (download_ctl & 0x10) ?
+		 ((mute_ctl & 0x2) ? "detecting" : "running") : "stopped");
+
+	switch (audio_config >> 4) {
+	case 0x00:
+		p = "undefined";
+		break;
+	case 0x01:
+		p = "BTSC";
+		break;
+	case 0x02:
+		p = "EIAJ";
+		break;
+	case 0x03:
+		p = "A2-M";
+		break;
+	case 0x04:
+		p = "A2-BG";
+		break;
+	case 0x05:
+		p = "A2-DK1";
+		break;
+	case 0x06:
+		p = "A2-DK2";
+		break;
+	case 0x07:
+		p = "A2-DK3";
+		break;
+	case 0x08:
+		p = "A1 (6.0 MHz FM Mono)";
+		break;
+	case 0x09:
+		p = "AM-L";
+		break;
+	case 0x0a:
+		p = "NICAM-BG";
+		break;
+	case 0x0b:
+		p = "NICAM-DK";
+		break;
+	case 0x0c:
+		p = "NICAM-I";
+		break;
+	case 0x0d:
+		p = "NICAM-L";
+		break;
+	case 0x0e:
+		p = "FM radio";
+		break;
+	case 0x0f:
+		p = "automatic detection";
+		break;
+	default:
+		p = "undefined";
+	}
+	v4l_info(client, "Configured audio standard: %s\n", p);
+
+	if ((audio_config >> 4) < 0xF) {
+		switch (audio_config & 0xF) {
+		case 0x00:
+			p = "MONO1 (LANGUAGE A/Mono L+R channel for BTSC, EIAJ, A2)";
+			break;
+		case 0x01:
+			p = "MONO2 (LANGUAGE B)";
+			break;
+		case 0x02:
+			p = "MONO3 (STEREO forced MONO)";
+			break;
+		case 0x03:
+			p = "MONO4 (NICAM ANALOG-Language C/Analog Fallback)";
+			break;
+		case 0x04:
+			p = "STEREO";
+			break;
+		case 0x05:
+			p = "DUAL1 (AB)";
+			break;
+		case 0x06:
+			p = "DUAL2 (AC) (FM)";
+			break;
+		case 0x07:
+			p = "DUAL3 (BC) (FM)";
+			break;
+		case 0x08:
+			p = "DUAL4 (AC) (AM)";
+			break;
+		case 0x09:
+			p = "DUAL5 (BC) (AM)";
+			break;
+		case 0x0a:
+			p = "SAP";
+			break;
+		default:
+			p = "undefined";
+		}
+		v4l_info(client, "Configured audio mode:     %s\n", p);
+	} else {
+		switch (audio_config & 0xF) {
+		case 0x00:
+			p = "BG";
+			break;
+		case 0x01:
+			p = "DK1";
+			break;
+		case 0x02:
+			p = "DK2";
+			break;
+		case 0x03:
+			p = "DK3";
+			break;
+		case 0x04:
+			p = "I";
+			break;
+		case 0x05:
+			p = "L";
+			break;
+		case 0x06:
+			p = "BTSC";
+			break;
+		case 0x07:
+			p = "EIAJ";
+			break;
+		case 0x08:
+			p = "A2-M";
+			break;
+		case 0x09:
+			p = "FM Radio";
+			break;
+		case 0x0f:
+			p = "automatic standard and mode detection";
+			break;
+		default:
+			p = "undefined";
+		}
+		v4l_info(client, "Configured audio system:   %s\n", p);
+	}
+
+	if (aud_input) {
+		v4l_info(client, "Specified audio input:     Tuner (In%d)\n",
+			 aud_input);
+	} else {
+		v4l_info(client, "Specified audio input:     External\n");
+	}
+
+	switch (pref_mode & 0xf) {
+	case 0:
+		p = "mono/language A";
+		break;
+	case 1:
+		p = "language B";
+		break;
+	case 2:
+		p = "language C";
+		break;
+	case 3:
+		p = "analog fallback";
+		break;
+	case 4:
+		p = "stereo";
+		break;
+	case 5:
+		p = "language AC";
+		break;
+	case 6:
+		p = "language BC";
+		break;
+	case 7:
+		p = "language AB";
+		break;
+	default:
+		p = "undefined";
+	}
+	v4l_info(client, "Preferred audio mode:      %s\n", p);
+
+	if ((audio_config & 0xf) == 0xf) {
+		switch ((afc0 >> 3) & 0x3) {
+		case 0:
+			p = "system DK";
+			break;
+		case 1:
+			p = "system L";
+			break;
+		case 2:
+			p = "autodetect";
+			break;
+		default:
+			p = "undefined";
+		}
+		v4l_info(client, "Selected 65 MHz format:    %s\n", p);
+
+		switch (afc0 & 0x7) {
+		case 0:
+			p = "chroma";
+			break;
+		case 1:
+			p = "BTSC";
+			break;
+		case 2:
+			p = "EIAJ";
+			break;
+		case 3:
+			p = "A2-M";
+			break;
+		case 4:
+			p = "autodetect";
+			break;
+		default:
+			p = "undefined";
+		}
+		v4l_info(client, "Selected 45 MHz format:    %s\n", p);
+	}
+}
+
+#define CX25840_VCONFIG_OPTION(state, cfg_in, opt_msk)			\
+	do {								\
+		if ((cfg_in) & (opt_msk)) {				\
+			(state)->vid_config &= ~(opt_msk);		\
+			(state)->vid_config |= (cfg_in) & (opt_msk);	\
+		}							\
+	} while (0)
+
+/* apply incoming options to the current vconfig */
+static void cx25840_vconfig_add(struct cx25840_state *state, u32 cfg_in)
+{
+	CX25840_VCONFIG_OPTION(state, cfg_in, CX25840_VCONFIG_FMT_MASK);
+	CX25840_VCONFIG_OPTION(state, cfg_in, CX25840_VCONFIG_RES_MASK);
+	CX25840_VCONFIG_OPTION(state, cfg_in, CX25840_VCONFIG_VBIRAW_MASK);
+	CX25840_VCONFIG_OPTION(state, cfg_in, CX25840_VCONFIG_ANCDATA_MASK);
+	CX25840_VCONFIG_OPTION(state, cfg_in, CX25840_VCONFIG_TASKBIT_MASK);
+	CX25840_VCONFIG_OPTION(state, cfg_in, CX25840_VCONFIG_ACTIVE_MASK);
+	CX25840_VCONFIG_OPTION(state, cfg_in, CX25840_VCONFIG_VALID_MASK);
+	CX25840_VCONFIG_OPTION(state, cfg_in, CX25840_VCONFIG_HRESETW_MASK);
+	CX25840_VCONFIG_OPTION(state, cfg_in, CX25840_VCONFIG_CLKGATE_MASK);
+	CX25840_VCONFIG_OPTION(state, cfg_in, CX25840_VCONFIG_DCMODE_MASK);
+	CX25840_VCONFIG_OPTION(state, cfg_in, CX25840_VCONFIG_IDID0S_MASK);
+	CX25840_VCONFIG_OPTION(state, cfg_in, CX25840_VCONFIG_VIPCLAMP_MASK);
+}
+
+/* ----------------------------------------------------------------------- */
+
+/*
+ * Initializes the device in the generic mode.
+ * For cx2584x chips also adds additional video output settings provided
+ * in @val parameter (CX25840_VCONFIG_*).
+ *
+ * The generic mode disables some of the ivtv-related hacks in this driver.
+ * For cx2584x chips it also enables setting video output configuration while
+ * setting it according to datasheet defaults by default.
+ */
+static int cx25840_init(struct v4l2_subdev *sd, u32 val)
+{
+	struct cx25840_state *state = to_state(sd);
+
+	state->generic_mode = true;
+
+	if (is_cx2584x(state)) {
+		/* set datasheet video output defaults */
+		state->vid_config = CX25840_VCONFIG_FMT_BT656 |
+				    CX25840_VCONFIG_RES_8BIT |
+				    CX25840_VCONFIG_VBIRAW_DISABLED |
+				    CX25840_VCONFIG_ANCDATA_ENABLED |
+				    CX25840_VCONFIG_TASKBIT_ONE |
+				    CX25840_VCONFIG_ACTIVE_HORIZONTAL |
+				    CX25840_VCONFIG_VALID_NORMAL |
+				    CX25840_VCONFIG_HRESETW_NORMAL |
+				    CX25840_VCONFIG_CLKGATE_NONE |
+				    CX25840_VCONFIG_DCMODE_DWORDS |
+				    CX25840_VCONFIG_IDID0S_NORMAL |
+				    CX25840_VCONFIG_VIPCLAMP_DISABLED;
+
+		/* add additional settings */
+		cx25840_vconfig_add(state, val);
+	} else {
+		/* TODO: generic mode needs to be developed for other chips */
+		WARN_ON(1);
+	}
+
+	return 0;
+}
+
+static int cx25840_reset(struct v4l2_subdev *sd, u32 val)
+{
+	struct cx25840_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (is_cx2583x(state))
+		cx25836_initialize(client);
+	else if (is_cx2388x(state))
+		cx23885_initialize(client);
+	else if (is_cx231xx(state))
+		cx231xx_initialize(client);
+	else
+		cx25840_initialize(client);
+
+	state->is_initialized = 1;
+
+	return 0;
+}
+
+/*
+ * This load_fw operation must be called to load the driver's firmware.
+ * This will load the firmware on the first invocation (further ones are NOP).
+ * Without this the audio standard detection will fail and you will
+ * only get mono.
+ * Alternatively, you can call the reset operation instead of this one.
+ *
+ * Since loading the firmware is often problematic when the driver is
+ * compiled into the kernel I recommend postponing calling this function
+ * until the first open of the video device. Another reason for
+ * postponing it is that loading this firmware takes a long time (seconds)
+ * due to the slow i2c bus speed. So it will speed up the boot process if
+ * you can avoid loading the fw as long as the video device isn't used.
+ */
+static int cx25840_load_fw(struct v4l2_subdev *sd)
+{
+	struct cx25840_state *state = to_state(sd);
+
+	if (!state->is_initialized) {
+		/* initialize and load firmware */
+		cx25840_reset(sd, 0);
+	}
+	return 0;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int cx25840_g_register(struct v4l2_subdev *sd,
+			      struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	reg->size = 1;
+	reg->val = cx25840_read(client, reg->reg & 0x0fff);
+	return 0;
+}
+
+static int cx25840_s_register(struct v4l2_subdev *sd,
+			      const struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	cx25840_write(client, reg->reg & 0x0fff, reg->val & 0xff);
+	return 0;
+}
+#endif
+
+static int cx25840_s_audio_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct cx25840_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u8 v;
+
+	if (is_cx2583x(state) || is_cx2388x(state) || is_cx231xx(state))
+		return 0;
+
+	v4l_dbg(1, cx25840_debug, client, "%s audio output\n",
+		enable ? "enable" : "disable");
+
+	if (enable) {
+		v = cx25840_read(client, 0x115) | 0x80;
+		cx25840_write(client, 0x115, v);
+		v = cx25840_read(client, 0x116) | 0x03;
+		cx25840_write(client, 0x116, v);
+	} else {
+		v = cx25840_read(client, 0x115) & ~(0x80);
+		cx25840_write(client, 0x115, v);
+		v = cx25840_read(client, 0x116) & ~(0x03);
+		cx25840_write(client, 0x116, v);
+	}
+	return 0;
+}
+
+static int cx25840_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct cx25840_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u8 v;
+
+	v4l_dbg(1, cx25840_debug, client, "%s video output\n",
+		enable ? "enable" : "disable");
+
+	/*
+	 * It's not clear what should be done for these devices.
+	 * The original code used the same addresses as for the cx25840, but
+	 * those addresses do something else entirely on the cx2388x and
+	 * cx231xx. Since it never did anything in the first place, just do
+	 * nothing.
+	 */
+	if (is_cx2388x(state) || is_cx231xx(state))
+		return 0;
+
+	if (enable) {
+		v = cx25840_read(client, 0x115) | 0x0c;
+		cx25840_write(client, 0x115, v);
+		v = cx25840_read(client, 0x116) | 0x04;
+		cx25840_write(client, 0x116, v);
+	} else {
+		v = cx25840_read(client, 0x115) & ~(0x0c);
+		cx25840_write(client, 0x115, v);
+		v = cx25840_read(client, 0x116) & ~(0x04);
+		cx25840_write(client, 0x116, v);
+	}
+	return 0;
+}
+
+/* Query the current detected video format */
+static int cx25840_querystd(struct v4l2_subdev *sd, v4l2_std_id *std)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	static const v4l2_std_id stds[] = {
+		/* 0000 */ V4L2_STD_UNKNOWN,
+
+		/* 0001 */ V4L2_STD_NTSC_M,
+		/* 0010 */ V4L2_STD_NTSC_M_JP,
+		/* 0011 */ V4L2_STD_NTSC_443,
+		/* 0100 */ V4L2_STD_PAL,
+		/* 0101 */ V4L2_STD_PAL_M,
+		/* 0110 */ V4L2_STD_PAL_N,
+		/* 0111 */ V4L2_STD_PAL_Nc,
+		/* 1000 */ V4L2_STD_PAL_60,
+
+		/* 1001 */ V4L2_STD_UNKNOWN,
+		/* 1010 */ V4L2_STD_UNKNOWN,
+		/* 1011 */ V4L2_STD_UNKNOWN,
+		/* 1100 */ V4L2_STD_SECAM,
+		/* 1101 */ V4L2_STD_UNKNOWN,
+		/* 1110 */ V4L2_STD_UNKNOWN,
+		/* 1111 */ V4L2_STD_UNKNOWN
+	};
+
+	u32 fmt = (cx25840_read4(client, 0x40c) >> 8) & 0xf;
+	*std = stds[fmt];
+
+	v4l_dbg(1, cx25840_debug, client,
+		"querystd fmt = %x, v4l2_std_id = 0x%x\n",
+		fmt, (unsigned int)stds[fmt]);
+
+	return 0;
+}
+
+static int cx25840_g_input_status(struct v4l2_subdev *sd, u32 *status)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	/*
+	 * A limited function that checks for signal status and returns
+	 * the state.
+	 */
+
+	/* Check for status of Horizontal lock (SRC lock isn't reliable) */
+	if ((cx25840_read4(client, 0x40c) & 0x00010000) == 0)
+		*status |= V4L2_IN_ST_NO_SIGNAL;
+
+	return 0;
+}
+
+static int cx25840_g_std(struct v4l2_subdev *sd, v4l2_std_id *std)
+{
+	struct cx25840_state *state = to_state(sd);
+
+	*std = state->std;
+
+	return 0;
+}
+
+static int cx25840_s_std(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct cx25840_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (state->radio == 0 && state->std == std)
+		return 0;
+	state->radio = 0;
+	state->std = std;
+	return set_v4lstd(client);
+}
+
+static int cx25840_s_radio(struct v4l2_subdev *sd)
+{
+	struct cx25840_state *state = to_state(sd);
+
+	state->radio = 1;
+	return 0;
+}
+
+static int cx25840_s_video_routing(struct v4l2_subdev *sd,
+				   u32 input, u32 output, u32 config)
+{
+	struct cx25840_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (is_cx23888(state))
+		cx23888_std_setup(client);
+
+	if (is_cx2584x(state) && state->generic_mode && config) {
+		cx25840_vconfig_add(state, config);
+		cx25840_vconfig_apply(client);
+	}
+
+	return set_input(client, input, state->aud_input);
+}
+
+static int cx25840_s_audio_routing(struct v4l2_subdev *sd,
+				   u32 input, u32 output, u32 config)
+{
+	struct cx25840_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (is_cx23888(state))
+		cx23888_std_setup(client);
+	return set_input(client, state->vid_input, input);
+}
+
+static int cx25840_s_frequency(struct v4l2_subdev *sd,
+			       const struct v4l2_frequency *freq)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	input_change(client);
+	return 0;
+}
+
+static int cx25840_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
+{
+	struct cx25840_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u8 vpres = cx25840_read(client, 0x40e) & 0x20;
+	u8 mode;
+	int val = 0;
+
+	if (state->radio)
+		return 0;
+
+	vt->signal = vpres ? 0xffff : 0x0;
+	if (is_cx2583x(state))
+		return 0;
+
+	vt->capability |= V4L2_TUNER_CAP_STEREO | V4L2_TUNER_CAP_LANG1 |
+			  V4L2_TUNER_CAP_LANG2 | V4L2_TUNER_CAP_SAP;
+
+	mode = cx25840_read(client, 0x804);
+
+	/* get rxsubchans and audmode */
+	if ((mode & 0xf) == 1)
+		val |= V4L2_TUNER_SUB_STEREO;
+	else
+		val |= V4L2_TUNER_SUB_MONO;
+
+	if (mode == 2 || mode == 4)
+		val = V4L2_TUNER_SUB_LANG1 | V4L2_TUNER_SUB_LANG2;
+
+	if (mode & 0x10)
+		val |= V4L2_TUNER_SUB_SAP;
+
+	vt->rxsubchans = val;
+	vt->audmode = state->audmode;
+	return 0;
+}
+
+static int cx25840_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *vt)
+{
+	struct cx25840_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (state->radio || is_cx2583x(state))
+		return 0;
+
+	switch (vt->audmode) {
+	case V4L2_TUNER_MODE_MONO:
+		/*
+		 * mono      -> mono
+		 * stereo    -> mono
+		 * bilingual -> lang1
+		 */
+		cx25840_and_or(client, 0x809, ~0xf, 0x00);
+		break;
+	case V4L2_TUNER_MODE_STEREO:
+	case V4L2_TUNER_MODE_LANG1:
+		/*
+		 * mono      -> mono
+		 * stereo    -> stereo
+		 * bilingual -> lang1
+		 */
+		cx25840_and_or(client, 0x809, ~0xf, 0x04);
+		break;
+	case V4L2_TUNER_MODE_LANG1_LANG2:
+		/*
+		 * mono      -> mono
+		 * stereo    -> stereo
+		 * bilingual -> lang1/lang2
+		 */
+		cx25840_and_or(client, 0x809, ~0xf, 0x07);
+		break;
+	case V4L2_TUNER_MODE_LANG2:
+		/*
+		 * mono      -> mono
+		 * stereo    -> stereo
+		 * bilingual -> lang2
+		 */
+		cx25840_and_or(client, 0x809, ~0xf, 0x01);
+		break;
+	default:
+		return -EINVAL;
+	}
+	state->audmode = vt->audmode;
+	return 0;
+}
+
+static int cx25840_log_status(struct v4l2_subdev *sd)
+{
+	struct cx25840_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	log_video_status(client);
+	if (!is_cx2583x(state))
+		log_audio_status(client);
+	cx25840_ir_log_status(sd);
+	v4l2_ctrl_handler_log_status(&state->hdl, sd->name);
+	return 0;
+}
+
+static int cx23885_irq_handler(struct v4l2_subdev *sd, u32 status,
+			       bool *handled)
+{
+	struct cx25840_state *state = to_state(sd);
+	struct i2c_client *c = v4l2_get_subdevdata(sd);
+	u8 irq_stat, aud_stat, aud_en, ir_stat, ir_en;
+	u32 vid_stat, aud_mc_stat;
+	bool block_handled;
+	int ret = 0;
+
+	irq_stat = cx25840_read(c, CX23885_PIN_CTRL_IRQ_REG);
+	v4l_dbg(2, cx25840_debug, c, "AV Core IRQ status (entry): %s %s %s\n",
+		irq_stat & CX23885_PIN_CTRL_IRQ_IR_STAT ? "ir" : "  ",
+		irq_stat & CX23885_PIN_CTRL_IRQ_AUD_STAT ? "aud" : "   ",
+		irq_stat & CX23885_PIN_CTRL_IRQ_VID_STAT ? "vid" : "   ");
+
+	if ((is_cx23885(state) || is_cx23887(state))) {
+		ir_stat = cx25840_read(c, CX25840_IR_STATS_REG);
+		ir_en = cx25840_read(c, CX25840_IR_IRQEN_REG);
+		v4l_dbg(2, cx25840_debug, c,
+			"AV Core ir IRQ status: %#04x disables: %#04x\n",
+			ir_stat, ir_en);
+		if (irq_stat & CX23885_PIN_CTRL_IRQ_IR_STAT) {
+			block_handled = false;
+			ret = cx25840_ir_irq_handler(sd,
+						     status, &block_handled);
+			if (block_handled)
+				*handled = true;
+		}
+	}
+
+	aud_stat = cx25840_read(c, CX25840_AUD_INT_STAT_REG);
+	aud_en = cx25840_read(c, CX25840_AUD_INT_CTRL_REG);
+	v4l_dbg(2, cx25840_debug, c,
+		"AV Core audio IRQ status: %#04x disables: %#04x\n",
+		aud_stat, aud_en);
+	aud_mc_stat = cx25840_read4(c, CX23885_AUD_MC_INT_MASK_REG);
+	v4l_dbg(2, cx25840_debug, c,
+		"AV Core audio MC IRQ status: %#06x enables: %#06x\n",
+		aud_mc_stat >> CX23885_AUD_MC_INT_STAT_SHFT,
+		aud_mc_stat & CX23885_AUD_MC_INT_CTRL_BITS);
+	if (irq_stat & CX23885_PIN_CTRL_IRQ_AUD_STAT) {
+		if (aud_stat) {
+			cx25840_write(c, CX25840_AUD_INT_STAT_REG, aud_stat);
+			*handled = true;
+		}
+	}
+
+	vid_stat = cx25840_read4(c, CX25840_VID_INT_STAT_REG);
+	v4l_dbg(2, cx25840_debug, c,
+		"AV Core video IRQ status: %#06x disables: %#06x\n",
+		vid_stat & CX25840_VID_INT_STAT_BITS,
+		vid_stat >> CX25840_VID_INT_MASK_SHFT);
+	if (irq_stat & CX23885_PIN_CTRL_IRQ_VID_STAT) {
+		if (vid_stat & CX25840_VID_INT_STAT_BITS) {
+			cx25840_write4(c, CX25840_VID_INT_STAT_REG, vid_stat);
+			*handled = true;
+		}
+	}
+
+	irq_stat = cx25840_read(c, CX23885_PIN_CTRL_IRQ_REG);
+	v4l_dbg(2, cx25840_debug, c, "AV Core IRQ status (exit): %s %s %s\n",
+		irq_stat & CX23885_PIN_CTRL_IRQ_IR_STAT ? "ir" : "  ",
+		irq_stat & CX23885_PIN_CTRL_IRQ_AUD_STAT ? "aud" : "   ",
+		irq_stat & CX23885_PIN_CTRL_IRQ_VID_STAT ? "vid" : "   ");
+
+	return ret;
+}
+
+static int cx25840_irq_handler(struct v4l2_subdev *sd, u32 status,
+			       bool *handled)
+{
+	struct cx25840_state *state = to_state(sd);
+
+	*handled = false;
+
+	/* Only support the CX2388[578] AV Core for now */
+	if (is_cx2388x(state))
+		return cx23885_irq_handler(sd, status, handled);
+
+	return -ENODEV;
+}
+
+/* ----------------------------------------------------------------------- */
+
+#define DIF_PLL_FREQ_WORD	(0x300)
+#define DIF_BPF_COEFF01		(0x348)
+#define DIF_BPF_COEFF23		(0x34c)
+#define DIF_BPF_COEFF45		(0x350)
+#define DIF_BPF_COEFF67		(0x354)
+#define DIF_BPF_COEFF89		(0x358)
+#define DIF_BPF_COEFF1011	(0x35c)
+#define DIF_BPF_COEFF1213	(0x360)
+#define DIF_BPF_COEFF1415	(0x364)
+#define DIF_BPF_COEFF1617	(0x368)
+#define DIF_BPF_COEFF1819	(0x36c)
+#define DIF_BPF_COEFF2021	(0x370)
+#define DIF_BPF_COEFF2223	(0x374)
+#define DIF_BPF_COEFF2425	(0x378)
+#define DIF_BPF_COEFF2627	(0x37c)
+#define DIF_BPF_COEFF2829	(0x380)
+#define DIF_BPF_COEFF3031	(0x384)
+#define DIF_BPF_COEFF3233	(0x388)
+#define DIF_BPF_COEFF3435	(0x38c)
+#define DIF_BPF_COEFF36		(0x390)
+
+static void cx23885_dif_setup(struct i2c_client *client, u32 ifHz)
+{
+	u64 pll_freq;
+	u32 pll_freq_word;
+
+	v4l_dbg(1, cx25840_debug, client, "%s(%d)\n", __func__, ifHz);
+
+	/* Assuming TV */
+	/* Calculate the PLL frequency word based on the adjusted ifHz */
+	pll_freq = div_u64((u64)ifHz * 268435456, 50000000);
+	pll_freq_word = (u32)pll_freq;
+
+	cx25840_write4(client, DIF_PLL_FREQ_WORD,  pll_freq_word);
+
+	/* Round down to the nearest 100KHz */
+	ifHz = (ifHz / 100000) * 100000;
+
+	if (ifHz < 3000000)
+		ifHz = 3000000;
+
+	if (ifHz > 16000000)
+		ifHz = 16000000;
+
+	v4l_dbg(1, cx25840_debug, client, "%s(%d) again\n", __func__, ifHz);
+
+	switch (ifHz) {
+	case 3000000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000002);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x00080012);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x001e0024);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x001bfff8);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xffb4ff50);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xfed8fe68);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe24fe34);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfebaffc7);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x014d031f);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x04f0065d);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x07010688);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x04c901d6);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xfe00f9d3);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf600f342);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf235f337);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf64efb22);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0105070f);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0x0c460fce);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 3100000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000001);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x00070012);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x00220032);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x00370026);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xfff0ff91);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xff0efe7c);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe01fdcc);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfe0afedb);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x00440224);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x0434060c);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x0738074e);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x06090361);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xff99fb39);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf6fef3b6);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf21af2a5);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf573fa33);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0034067d);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0x0bfb0fb9);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 3200000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000000);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0004000e);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x00200038);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x004c004f);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x002fffdf);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xff5cfeb6);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe0dfd92);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfd7ffe03);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xff36010a);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x03410575);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x072607d2);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x071804d5);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x0134fcb7);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf81ff451);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf223f22e);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf4a7f94b);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xff6405e8);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0x0bae0fa4);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 3300000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0000ffff);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x00000008);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x001a0036);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x0056006d);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x00670030);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xffbdff10);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe46fd8d);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfd25fd4f);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfe35ffe0);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x0224049f);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x06c9080e);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x07ef0627);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x02c9fe45);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf961f513);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf250f1d2);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf3ecf869);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xfe930552);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0x0b5f0f8f);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 3400000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0xfffffffe);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfffd0001);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x000f002c);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x0054007d);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x0093007c);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x0024ff82);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfea6fdbb);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfd03fcca);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfd51feb9);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x00eb0392);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x06270802);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x08880750);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x044dffdb);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xfabdf5f8);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf2a0f193);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf342f78f);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xfdc404b9);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0x0b0e0f78);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 3500000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0xfffffffd);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfffafff9);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x0002001b);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x0046007d);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x00ad00ba);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x00870000);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xff26fe1a);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfd1bfc7e);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfc99fda4);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xffa5025c);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x054507ad);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x08dd0847);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x05b80172);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xfc2ef6ff);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf313f170);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf2abf6bd);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xfcf6041f);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0x0abc0f61);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 3600000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0xfffffffd);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfff8fff3);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xfff50006);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x002f006c);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x00b200e3);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x00dc007e);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xffb9fea0);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfd6bfc71);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfc17fcb1);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfe65010b);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x042d0713);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x08ec0906);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x07020302);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xfdaff823);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf3a7f16a);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf228f5f5);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xfc2a0384);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0x0a670f4a);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 3700000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0000fffd);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfff7ffef);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffe9fff1);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x0010004d);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x00a100f2);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x011a00f0);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x0053ff44);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfdedfca2);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfbd3fbef);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfd39ffae);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x02ea0638);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x08b50987);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x08230483);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xff39f960);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf45bf180);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf1b8f537);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xfb6102e7);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0x0a110f32);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 3800000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0000fffe);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfff9ffee);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffe1ffdd);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xfff00024);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x007c00e5);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x013a014a);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x00e6fff8);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfe98fd0f);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfbd3fb67);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfc32fe54);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x01880525);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x083909c7);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x091505ee);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x00c7fab3);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf52df1b4);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf15df484);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xfa9b0249);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0x09ba0f19);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 3900000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000000);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfffbfff0);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffdeffcf);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffd1fff6);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x004800be);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x01390184);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x016300ac);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xff5efdb1);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfc17fb23);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfb5cfd0d);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x001703e4);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x077b09c4);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x09d2073c);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0251fc18);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf61cf203);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf118f3dc);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf9d801aa);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0x09600eff);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 4000000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000001);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfffefff4);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffe1ffc8);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffbaffca);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x000b0082);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x01170198);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x01c10152);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x0030fe7b);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfc99fb24);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfac3fbe9);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xfea5027f);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x0683097f);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x0a560867);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x03d2fd89);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf723f26f);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf0e8f341);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf919010a);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0x09060ee5);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 4100000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00010002);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0002fffb);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffe8ffca);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffacffa4);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xffcd0036);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x00d70184);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x01f601dc);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x00ffff60);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfd51fb6d);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfa6efaf5);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xfd410103);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x055708f9);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x0a9e0969);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0543ff02);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf842f2f5);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf0cef2b2);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf85e006b);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0x08aa0ecb);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 4200000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00010003);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x00050003);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xfff3ffd3);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffaaff8b);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff95ffe5);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x0080014a);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x01fe023f);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x01ba0050);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfe35fbf8);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfa62fa3b);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xfbf9ff7e);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x04010836);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x0aa90a3d);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x069f007f);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf975f395);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf0cbf231);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf7a9ffcb);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0x084c0eaf);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 4300000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00010003);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0008000a);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x0000ffe4);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffb4ff81);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff6aff96);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x001c00f0);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x01d70271);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x0254013b);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xff36fcbd);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfa9ff9c5);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xfadbfdfe);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x028c073b);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x0a750adf);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x07e101fa);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xfab8f44e);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf0ddf1be);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf6f9ff2b);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0x07ed0e94);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 4400000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000003);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0009000f);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x000efff8);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffc9ff87);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff52ff54);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xffb5007e);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x01860270);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x02c00210);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x0044fdb2);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfb22f997);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xf9f2fc90);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x0102060f);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x0a050b4c);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0902036e);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xfc0af51e);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf106f15a);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf64efe8b);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0x078d0e77);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 4500000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000002);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x00080012);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x0019000e);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffe5ff9e);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff4fff25);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xff560000);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x0112023b);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x02f702c0);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x014dfec8);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfbe5f9b3);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xf947fb41);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xff7004b9);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x095a0b81);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0a0004d8);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xfd65f603);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf144f104);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf5aafdec);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0x072b0e5a);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 4600000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000001);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x00060012);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x00200022);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x0005ffc1);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff61ff10);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xff09ff82);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x008601d7);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x02f50340);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x0241fff0);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfcddfa19);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xf8e2fa1e);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xfde30343);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x08790b7f);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0ad50631);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xfec7f6fc);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf198f0bd);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf50dfd4e);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0x06c90e3d);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 4700000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0000ffff);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0003000f);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x00220030);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x0025ffed);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff87ff15);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xfed6ff10);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xffed014c);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x02b90386);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x03110119);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfdfefac4);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xf8c6f92f);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xfc6701b7);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x07670b44);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0b7e0776);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x002df807);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf200f086);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf477fcb1);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0x06650e1e);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 4800000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0xfffffffe);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xffff0009);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x001e0038);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x003f001b);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xffbcff36);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xfec2feb6);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xff5600a5);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x0248038d);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x03b00232);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xff39fbab);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xf8f4f87f);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xfb060020);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x062a0ad2);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0bf908a3);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0192f922);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf27df05e);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf3e8fc14);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0x06000e00);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 4900000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0xfffffffd);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfffc0002);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x00160037);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x00510046);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xfff9ff6d);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xfed0fe7c);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfecefff0);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x01aa0356);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x0413032b);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x007ffcc5);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xf96cf812);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xf9cefe87);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x04c90a2c);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0c4309b4);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x02f3fa4a);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf30ef046);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf361fb7a);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0x059b0de0);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 5000000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0xfffffffd);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfff9fffa);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x000a002d);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x00570067);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x0037ffb5);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xfefffe68);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe62ff3d);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x00ec02e3);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x043503f6);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x01befe05);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xfa27f7ee);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xf8c6fcf8);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x034c0954);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0c5c0aa4);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x044cfb7e);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf3b1f03f);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf2e2fae1);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0x05340dc0);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 5100000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0000fffd);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfff8fff4);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xfffd001e);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x0051007b);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x006e0006);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xff48fe7c);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe1bfe9a);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x001d023e);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x04130488);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x02e6ff5b);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xfb1ef812);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xf7f7fb7f);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x01bc084e);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0c430b72);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x059afcba);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf467f046);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf26cfa4a);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0x04cd0da0);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 5200000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0000fffe);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfff8ffef);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xfff00009);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x003f007f);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x00980056);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xffa5feb6);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe00fe15);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xff4b0170);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x03b004d7);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x03e800b9);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xfc48f87f);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xf768fa23);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x0022071f);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0bf90c1b);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x06dafdfd);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf52df05e);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf1fef9b5);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0x04640d7f);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 5300000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0000ffff);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfff9ffee);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffe6fff3);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x00250072);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x00af009c);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x000cff10);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe13fdb8);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfe870089);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x031104e1);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x04b8020f);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xfd98f92f);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xf71df8f0);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xfe8805ce);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0b7e0c9c);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0808ff44);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf603f086);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf19af922);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0x03fb0d5e);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 5400000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000001);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfffcffef);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffe0ffe0);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x00050056);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x00b000d1);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x0071ff82);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe53fd8c);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfddfff99);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x024104a3);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x054a034d);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xff01fa1e);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xf717f7ed);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xfcf50461);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0ad50cf4);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0921008d);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf6e7f0bd);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf13ff891);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0x03920d3b);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 5500000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00010002);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfffffff3);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffdeffd1);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffe5002f);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x009c00ed);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x00cb0000);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfebafd94);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfd61feb0);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x014d0422);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x05970464);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x0074fb41);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xf759f721);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xfb7502de);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0a000d21);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0a2201d4);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf7d9f104);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf0edf804);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0x03280d19);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 5600000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00010003);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0003fffa);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffe3ffc9);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffc90002);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x007500ef);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x010e007e);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xff3dfdcf);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfd16fddd);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x00440365);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x059b0548);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x01e3fc90);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xf7dff691);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xfa0f014d);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x09020d23);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0b0a0318);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf8d7f15a);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf0a5f779);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0x02bd0cf6);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 5700000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00010003);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x00060001);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffecffc9);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffb4ffd4);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x004000d5);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x013600f0);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xffd3fe39);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfd04fd31);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xff360277);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x055605ef);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x033efdfe);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xf8a5f642);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf8cbffb6);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x07e10cfb);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0bd50456);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf9dff1be);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf067f6f2);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0x02520cd2);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 5800000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000003);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x00080009);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xfff8ffd2);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffaaffac);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x000200a3);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x013c014a);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x006dfec9);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfd2bfcb7);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfe350165);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x04cb0651);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x0477ff7e);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xf9a5f635);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf7b1fe20);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x069f0ca8);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0c81058b);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xfaf0f231);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf033f66d);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0x01e60cae);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 5900000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000002);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0009000e);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x0005ffe1);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffacff90);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xffc5005f);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x01210184);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x00fcff72);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfd8afc77);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfd51003f);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x04020669);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x05830103);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xfad7f66b);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf6c8fc93);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x05430c2b);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0d0d06b5);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xfc08f2b2);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf00af5ec);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0x017b0c89);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 6000000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000001);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x00070012);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x0012fff5);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffbaff82);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff8e000f);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x00e80198);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x01750028);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfe18fc75);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfc99ff15);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x03050636);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x0656027f);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xfc32f6e2);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf614fb17);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x03d20b87);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0d7707d2);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xfd26f341);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xefeaf56f);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0x010f0c64);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 6100000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0xffff0000);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x00050012);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x001c000b);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffd1ff84);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff66ffbe);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x00960184);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x01cd00da);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfeccfcb2);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfc17fdf9);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x01e005bc);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x06e703e4);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xfdabf798);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf599f9b3);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x02510abd);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0dbf08df);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xfe48f3dc);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xefd5f4f6);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0x00a20c3e);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 6200000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0xfffffffe);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0002000f);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x0021001f);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xfff0ff97);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff50ff74);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x0034014a);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x01fa0179);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xff97fd2a);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfbd3fcfa);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x00a304fe);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x07310525);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xff37f886);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf55cf86e);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x00c709d0);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0de209db);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xff6df484);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xefcbf481);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0x00360c18);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 6300000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0xfffffffd);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfffe000a);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x0021002f);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x0010ffb8);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff50ff3b);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xffcc00f0);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x01fa01fa);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x0069fdd4);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfbd3fc26);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xff5d0407);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x07310638);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x00c9f9a8);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf55cf74e);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xff3908c3);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0de20ac3);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0093f537);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xefcbf410);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xffca0bf2);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 6400000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0xfffffffd);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfffb0003);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x001c0037);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x002fffe2);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff66ff17);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xff6a007e);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x01cd0251);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x0134fea5);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfc17fb8b);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfe2002e0);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x06e70713);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x0255faf5);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf599f658);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xfdaf0799);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0dbf0b96);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x01b8f5f5);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xefd5f3a3);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xff5e0bca);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 6500000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0000fffd);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfff9fffb);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x00120037);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x00460010);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff8eff0f);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xff180000);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x01750276);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x01e8ff8d);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfc99fb31);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfcfb0198);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x065607ad);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x03cefc64);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf614f592);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xfc2e0656);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0d770c52);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x02daf6bd);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xefeaf33b);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xfef10ba3);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 6600000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0000fffe);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfff7fff5);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x0005002f);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x0054003c);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xffc5ff22);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xfedfff82);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x00fc0267);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x0276007e);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfd51fb1c);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfbfe003e);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x05830802);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x0529fdec);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf6c8f4fe);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xfabd04ff);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0d0d0cf6);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x03f8f78f);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf00af2d7);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xfe850b7b);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 6700000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0000ffff);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfff8fff0);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xfff80020);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x00560060);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x0002ff4e);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xfec4ff10);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x006d0225);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x02d50166);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfe35fb4e);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfb35fee1);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x0477080e);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x065bff82);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf7b1f4a0);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf9610397);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0c810d80);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0510f869);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf033f278);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xfe1a0b52);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 6800000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00010000);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfffaffee);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffec000c);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x004c0078);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x0040ff8e);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xfecafeb6);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xffd301b6);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x02fc0235);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xff36fbc5);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfaaafd90);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x033e07d2);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x075b011b);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf8cbf47a);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf81f0224);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0bd50def);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0621f94b);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf067f21e);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xfdae0b29);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 6900000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00010001);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfffdffef);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffe3fff6);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x0037007f);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x0075ffdc);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xfef2fe7c);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xff3d0122);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x02ea02dd);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x0044fc79);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfa65fc5d);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x01e3074e);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x082102ad);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xfa0ff48c);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf6fe00a9);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0b0a0e43);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0729fa33);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf0a5f1c9);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xfd430b00);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 7000000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00010002);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0001fff3);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffdeffe2);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x001b0076);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x009c002d);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xff35fe68);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfeba0076);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x029f0352);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x014dfd60);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfa69fb53);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x00740688);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x08a7042d);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xfb75f4d6);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf600ff2d);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0a220e7a);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0827fb22);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf0edf17a);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xfcd80ad6);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 7100000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000003);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0004fff9);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffe0ffd2);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xfffb005e);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x00b0007a);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xff8ffe7c);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe53ffc1);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x0221038c);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x0241fe6e);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfab6fa80);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xff010587);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x08e90590);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xfcf5f556);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf52bfdb3);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x09210e95);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0919fc15);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf13ff12f);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xfc6e0aab);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 7200000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000003);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x00070000);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffe6ffc9);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffdb0039);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x00af00b8);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xfff4feb6);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe13ff10);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x01790388);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x0311ff92);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfb48f9ed);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xfd980453);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x08e306cd);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xfe88f60a);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf482fc40);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x08080e93);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x09fdfd0c);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf19af0ea);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xfc050a81);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 7300000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000002);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x00080008);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xfff0ffc9);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffc1000d);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x009800e2);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x005bff10);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe00fe74);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x00b50345);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x03b000bc);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfc18f9a1);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xfc4802f9);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x089807dc);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x0022f6f0);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf407fada);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x06da0e74);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0ad3fe06);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf1fef0ab);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xfb9c0a55);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 7400000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000001);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0008000e);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xfffdffd0);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffafffdf);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x006e00f2);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x00b8ff82);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe1bfdf8);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xffe302c8);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x041301dc);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfd1af99e);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xfb1e0183);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x080908b5);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x01bcf801);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf3bdf985);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x059a0e38);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0b99ff03);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf26cf071);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xfb330a2a);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 7500000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0xffff0000);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x00070011);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x000affdf);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffa9ffb5);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x003700e6);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x01010000);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe62fda8);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xff140219);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x043502e1);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfe42f9e6);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xfa270000);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x073a0953);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x034cf939);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf3a4f845);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x044c0de1);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0c4f0000);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf2e2f03c);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xfacc09fe);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 7600000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0xffffffff);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x00040012);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x0016fff3);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffafff95);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xfff900c0);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x0130007e);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfecefd89);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfe560146);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x041303bc);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xff81fa76);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xf96cfe7d);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x063209b1);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x04c9fa93);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf3bdf71e);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x02f30d6e);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0cf200fd);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf361f00e);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xfa6509d1);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 7700000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0xfffffffe);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x00010010);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x001e0008);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffc1ff84);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xffbc0084);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x013e00f0);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xff56fd9f);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfdb8005c);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x03b00460);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x00c7fb45);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xf8f4fd07);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x04fa09ce);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x062afc07);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf407f614);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x01920ce0);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0d8301fa);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf3e8efe5);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xfa0009a4);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 7800000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0000fffd);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfffd000b);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x0022001d);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffdbff82);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff870039);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x012a014a);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xffedfde7);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfd47ff6b);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x031104c6);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x0202fc4c);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xf8c6fbad);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x039909a7);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x0767fd8e);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf482f52b);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x002d0c39);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0e0002f4);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf477efc2);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf99b0977);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 7900000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0000fffd);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfffa0004);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x0020002d);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xfffbff91);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff61ffe8);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x00f70184);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x0086fe5c);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfd0bfe85);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x024104e5);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x0323fd7d);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xf8e2fa79);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x021d093f);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x0879ff22);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf52bf465);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xfec70b79);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0e6803eb);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf50defa5);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf937094a);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 8000000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0000fffe);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfff8fffd);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x00190036);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x001bffaf);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff4fff99);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x00aa0198);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x0112fef3);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfd09fdb9);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x014d04be);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x041bfecc);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xf947f978);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x00900897);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x095a00b9);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf600f3c5);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xfd650aa3);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0ebc04de);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf5aaef8e);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf8d5091c);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 8100000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0000ffff);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfff7fff6);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x000e0038);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x0037ffd7);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff52ff56);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x004b0184);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x0186ffa1);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfd40fd16);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x00440452);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x04de0029);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xf9f2f8b2);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xfefe07b5);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x0a05024d);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf6fef34d);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xfc0a09b8);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0efa05cd);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf64eef7d);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf87308ed);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 8200000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00010000);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfff8fff0);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x00000031);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x004c0005);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff6aff27);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xffe4014a);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x01d70057);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfdacfca6);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xff3603a7);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x05610184);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xfadbf82e);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xfd74069f);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x0a7503d6);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf81ff2ff);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xfab808b9);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0f2306b5);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf6f9ef72);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf81308bf);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 8300000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00010001);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfffbffee);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xfff30022);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x00560032);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff95ff10);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xff8000f0);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x01fe0106);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfe46fc71);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfe3502c7);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x059e02ce);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xfbf9f7f2);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xfbff055b);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x0aa9054c);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf961f2db);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf97507aa);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0f350797);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf7a9ef6d);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf7b40890);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 8400000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00010002);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfffeffee);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffe8000f);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x00540058);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xffcdff14);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xff29007e);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x01f6019e);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xff01fc7c);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfd5101bf);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x059203f6);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xfd41f7fe);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xfaa903f3);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x0a9e06a9);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xfabdf2e2);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf842068b);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0f320871);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf85eef6e);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf7560860);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 8500000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000003);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0002fff2);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffe1fff9);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x00460073);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x000bff34);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xfee90000);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x01c10215);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xffd0fcc5);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfc99009d);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x053d04f1);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xfea5f853);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xf97d0270);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x0a5607e4);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xfc2ef314);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf723055f);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0f180943);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf919ef75);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf6fa0830);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 8600000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000003);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0005fff8);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffdeffe4);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x002f007f);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x0048ff6b);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xfec7ff82);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x0163025f);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x00a2fd47);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfc17ff73);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x04a405b2);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x0017f8ed);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xf88500dc);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x09d208f9);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xfdaff370);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf61c0429);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0ee80a0b);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xf9d8ef82);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf6a00800);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 8700000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000003);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0007ffff);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffe1ffd4);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x0010007a);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x007cffb2);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xfec6ff10);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x00e60277);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x0168fdf9);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfbd3fe50);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x03ce0631);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x0188f9c8);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xf7c7ff43);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x091509e3);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xff39f3f6);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf52d02ea);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0ea30ac9);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xfa9bef95);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf64607d0);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 8800000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000002);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x00090007);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffe9ffca);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xfff00065);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x00a10003);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xfee6feb6);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x0053025b);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x0213fed0);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfbd3fd46);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x02c70668);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x02eafadb);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xf74bfdae);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x08230a9c);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x00c7f4a3);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf45b01a6);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0e480b7c);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xfb61efae);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf5ef079f);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 8900000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0xffff0000);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0008000d);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xfff5ffc8);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffd10043);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x00b20053);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xff24fe7c);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xffb9020c);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x0295ffbb);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfc17fc64);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x019b0654);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x042dfc1c);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xf714fc2a);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x07020b21);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0251f575);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf3a7005e);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0dd80c24);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xfc2aefcd);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf599076e);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 9000000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0xffffffff);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x00060011);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x0002ffcf);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffba0018);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x00ad009a);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xff79fe68);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xff260192);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x02e500ab);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfc99fbb6);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x005b05f7);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x0545fd81);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xf723fabf);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x05b80b70);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x03d2f669);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf313ff15);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0d550cbf);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xfcf6eff2);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf544073d);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 9100000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0xfffffffe);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x00030012);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x000fffdd);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffacffea);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x009300cf);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xffdcfe7c);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfea600f7);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x02fd0190);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfd51fb46);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xff150554);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x0627fefd);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xf778f978);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x044d0b87);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0543f77d);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf2a0fdcf);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0cbe0d4e);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xfdc4f01d);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf4f2070b);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 9200000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0000fffd);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x00000010);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x001afff0);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffaaffbf);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x006700ed);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x0043feb6);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe460047);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x02db0258);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfe35fb1b);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfddc0473);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x06c90082);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xf811f85e);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x02c90b66);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x069ff8ad);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf250fc8d);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0c140dcf);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xfe93f04d);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf4a106d9);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 9300000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0000fffd);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfffc000c);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x00200006);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffb4ff9c);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x002f00ef);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x00a4ff10);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe0dff92);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x028102f7);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xff36fb37);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfcbf035e);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x07260202);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xf8e8f778);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x01340b0d);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x07e1f9f4);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf223fb51);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0b590e42);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xff64f083);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf45206a7);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 9400000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0000fffd);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfff90005);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x0022001a);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffc9ff86);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xfff000d7);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x00f2ff82);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe01fee5);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x01f60362);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x0044fb99);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfbcc0222);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x07380370);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xf9f7f6cc);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xff990a7e);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0902fb50);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf21afa1f);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0a8d0ea6);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0034f0bf);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf4050675);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 9500000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0000fffe);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfff8fffe);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x001e002b);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffe5ff81);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xffb400a5);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x01280000);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe24fe50);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x01460390);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x014dfc3a);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfb1000ce);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x070104bf);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xfb37f65f);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xfe0009bc);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0a00fcbb);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf235f8f8);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x09b20efc);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0105f101);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf3ba0642);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 9600000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0001ffff);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfff8fff7);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x00150036);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x0005ff8c);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff810061);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x013d007e);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe71fddf);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x007c0380);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x0241fd13);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfa94ff70);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x068005e2);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xfc9bf633);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xfc7308ca);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0ad5fe30);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf274f7e0);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x08c90f43);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x01d4f147);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf371060f);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 9700000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00010001);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfff9fff1);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x00090038);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x0025ffa7);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff5e0012);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x013200f0);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfee3fd9b);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xffaa0331);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x0311fe15);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfa60fe18);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x05bd06d1);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xfe1bf64a);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xfafa07ae);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0b7effab);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf2d5f6d7);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x07d30f7a);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x02a3f194);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf32905dc);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 9800000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00010002);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfffcffee);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xfffb0032);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x003fffcd);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff4effc1);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x0106014a);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xff6efd8a);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfedd02aa);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x03b0ff34);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfa74fcd7);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x04bf0781);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xffaaf6a3);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf99e066b);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0bf90128);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf359f5e1);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x06d20fa2);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0370f1e5);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf2e405a8);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 9900000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000003);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xffffffee);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffef0024);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x0051fffa);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff54ff77);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x00be0184);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x0006fdad);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfe2701f3);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x0413005e);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfad1fbba);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x039007ee);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x013bf73d);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf868050a);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0c4302a1);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf3fdf4fe);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x05c70fba);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x043bf23c);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf2a10575);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 10000000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000003);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0003fff1);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffe50011);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x00570027);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff70ff3c);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x00620198);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x009efe01);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfd95011a);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x04350183);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfb71fad0);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x023c0812);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x02c3f811);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf75e0390);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0c5c0411);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf4c1f432);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x04b30fc1);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0503f297);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf2610541);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 10100000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000003);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0006fff7);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffdffffc);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x00510050);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff9dff18);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xfffc0184);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x0128fe80);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfd32002e);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x04130292);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfc4dfa21);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x00d107ee);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x0435f91c);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf6850205);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0c430573);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf5a1f37d);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x03990fba);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x05c7f2f8);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf222050d);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 10200000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000002);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0008fffe);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffdfffe7);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x003f006e);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xffd6ff0f);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xff96014a);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x0197ff1f);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfd05ff3e);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x03b0037c);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfd59f9b7);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xff5d0781);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x0585fa56);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf5e4006f);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0bf906c4);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf69df2e0);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x02790fa2);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0688f35d);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf1e604d8);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 10300000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0xffff0001);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x00090005);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffe4ffd6);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x0025007e);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x0014ff20);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xff3c00f0);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x01e1ffd0);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfd12fe5c);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x03110433);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfe88f996);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xfdf106d1);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x06aafbb7);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf57efed8);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0b7e07ff);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf7b0f25e);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x01560f7a);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0745f3c7);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf1ac04a4);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 10400000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0xffffffff);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0008000c);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffedffcb);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x0005007d);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x0050ff4c);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xfef6007e);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x01ff0086);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfd58fd97);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x024104ad);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xffcaf9c0);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xfc9905e2);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x079afd35);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf555fd46);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0ad50920);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf8d9f1f6);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x00310f43);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x07fdf435);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf174046f);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 10500000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0xfffffffe);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x00050011);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xfffaffc8);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffe5006b);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x0082ff8c);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xfecc0000);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x01f00130);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfdd2fcfc);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x014d04e3);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x010efa32);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xfb6404bf);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x084efec5);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf569fbc2);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0a000a23);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xfa15f1ab);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xff0b0efc);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x08b0f4a7);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf13f043a);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 10600000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0000fffd);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x00020012);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x0007ffcd);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffc9004c);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x00a4ffd9);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xfec3ff82);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x01b401c1);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfe76fc97);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x004404d2);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x0245fae8);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xfa5f0370);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x08c1005f);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf5bcfa52);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x09020b04);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xfb60f17b);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xfde70ea6);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x095df51e);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf10c0405);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 10700000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0000fffd);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xffff0011);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x0014ffdb);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffb40023);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x00b2002a);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xfedbff10);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x0150022d);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xff38fc6f);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xff36047b);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x035efbda);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xf9940202);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x08ee01f5);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf649f8fe);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x07e10bc2);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xfcb6f169);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xfcc60e42);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0a04f599);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf0db03d0);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 10800000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0000fffd);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfffb000d);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x001dffed);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffaafff5);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x00aa0077);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xff13feb6);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x00ce026b);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x000afc85);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfe3503e3);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x044cfcfb);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xf90c0082);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x08d5037f);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf710f7cc);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x069f0c59);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xfe16f173);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xfbaa0dcf);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0aa5f617);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf0ad039b);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 10900000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0000fffe);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfff90006);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x00210003);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffacffc8);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x008e00b6);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xff63fe7c);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x003a0275);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x00dafcda);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfd510313);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x0501fe40);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xf8cbfefd);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x087604f0);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf80af6c2);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x05430cc8);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xff7af19a);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xfa940d4e);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0b3ff699);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf0810365);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 11000000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0001ffff);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfff8ffff);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x00210018);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffbaffa3);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x006000e1);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xffc4fe68);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xffa0024b);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x019afd66);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfc990216);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x0575ff99);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xf8d4fd81);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x07d40640);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf932f5e6);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x03d20d0d);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x00dff1de);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf9860cbf);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0bd1f71e);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf058032f);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 11100000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00010000);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfff8fff8);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x001b0029);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffd1ff8a);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x002600f2);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x002cfe7c);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xff0f01f0);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x023bfe20);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfc1700fa);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x05a200f7);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xf927fc1c);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x06f40765);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xfa82f53b);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x02510d27);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0243f23d);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf8810c24);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0c5cf7a7);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf03102fa);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 11200000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00010002);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfffafff2);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x00110035);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xfff0ff81);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xffe700e7);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x008ffeb6);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe94016d);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x02b0fefb);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfbd3ffd1);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x05850249);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xf9c1fadb);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x05de0858);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xfbf2f4c4);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x00c70d17);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x03a0f2b8);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf7870b7c);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0cdff833);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf00d02c4);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 11300000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000003);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfffdffee);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x00040038);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x0010ff88);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xffac00c2);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x00e2ff10);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe3900cb);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x02f1ffe9);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfbd3feaa);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x05210381);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xfa9cf9c8);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x04990912);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xfd7af484);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xff390cdb);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x04f4f34d);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf69a0ac9);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0d5af8c1);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xefec028e);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 11400000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000003);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0000ffee);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xfff60033);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x002fff9f);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff7b0087);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x011eff82);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe080018);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x02f900d8);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfc17fd96);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x04790490);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xfbadf8ed);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x032f098e);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xff10f47d);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xfdaf0c75);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x063cf3fc);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf5ba0a0b);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0dccf952);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xefcd0258);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 11500000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000003);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0004fff1);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffea0026);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x0046ffc3);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff5a003c);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x013b0000);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe04ff63);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x02c801b8);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfc99fca6);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x0397056a);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xfcecf853);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x01ad09c9);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x00acf4ad);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xfc2e0be7);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0773f4c2);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf4e90943);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0e35f9e6);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xefb10221);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 11600000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000002);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0007fff6);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffe20014);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x0054ffee);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff4effeb);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x0137007e);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe2efebb);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x0260027a);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfd51fbe6);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x02870605);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xfe4af7fe);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x001d09c1);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x0243f515);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xfabd0b32);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0897f59e);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf4280871);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0e95fa7c);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xef9701eb);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 11700000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0xffff0001);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0008fffd);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffdeffff);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x0056001d);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff57ff9c);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x011300f0);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe82fe2e);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x01ca0310);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfe35fb62);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x0155065a);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xffbaf7f2);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xfe8c0977);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x03cef5b2);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf9610a58);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x09a5f68f);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf3790797);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0eebfb14);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xef8001b5);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 11800000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0xffff0000);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x00080004);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffe0ffe9);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x004c0047);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff75ff58);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x00d1014a);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfef9fdc8);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x0111036f);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xff36fb21);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x00120665);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x012df82e);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xfd0708ec);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x0542f682);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf81f095c);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0a9af792);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf2db06b5);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0f38fbad);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xef6c017e);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 11900000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0xffffffff);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0007000b);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffe7ffd8);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x00370068);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xffa4ff28);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x00790184);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xff87fd91);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x00430392);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x0044fb26);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfece0626);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x0294f8b2);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xfb990825);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x0698f77f);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf6fe0842);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0b73f8a7);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf25105cd);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0f7bfc48);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xef5a0148);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 12000000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0000fffe);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x00050010);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xfff2ffcc);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x001b007b);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xffdfff10);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x00140198);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x0020fd8e);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xff710375);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x014dfb73);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfd9a059f);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x03e0f978);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xfa4e0726);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x07c8f8a7);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf600070c);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0c2ff9c9);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf1db04de);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0fb4fce5);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xef4b0111);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 12100000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0000fffd);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x00010012);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffffffc8);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xfffb007e);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x001dff14);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xffad0184);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x00b7fdbe);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfea9031b);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x0241fc01);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfc8504d6);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x0504fa79);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xf93005f6);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x08caf9f2);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf52b05c0);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0ccbfaf9);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf17903eb);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0fe3fd83);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xef3f00db);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 12200000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0000fffd);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfffe0011);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x000cffcc);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffdb0071);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x0058ff32);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xff4f014a);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x013cfe1f);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfdfb028a);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x0311fcc9);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfb9d03d6);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x05f4fbad);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xf848049d);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x0999fb5b);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf4820461);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0d46fc32);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf12d02f4);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x1007fe21);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xef3600a4);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 12300000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0000fffe);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfffa000e);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x0017ffd9);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffc10055);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x0088ff68);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xff0400f0);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x01a6fea7);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfd7501cc);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x03b0fdc0);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfaef02a8);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x06a7fd07);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xf79d0326);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x0a31fcda);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf40702f3);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0d9ffd72);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf0f601fa);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x1021fec0);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xef2f006d);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 12400000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0001ffff);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfff80007);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x001fffeb);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffaf002d);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x00a8ffb0);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xfed3007e);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x01e9ff4c);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfd2000ee);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x0413fed8);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfa82015c);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x0715fe7d);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xf7340198);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x0a8dfe69);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf3bd017c);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0dd5feb8);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf0d500fd);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x1031ff60);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xef2b0037);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 12500000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00010000);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfff70000);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x00220000);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffa90000);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x00b30000);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xfec20000);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x02000000);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfd030000);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x04350000);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfa5e0000);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x073b0000);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xf7110000);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x0aac0000);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf3a40000);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0de70000);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf0c90000);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x10360000);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xef290000);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 12600000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00010001);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfff8fff9);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x001f0015);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffafffd3);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x00a80050);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xfed3ff82);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x01e900b4);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfd20ff12);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x04130128);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfa82fea4);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x07150183);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xf734fe68);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x0a8d0197);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf3bdfe84);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0dd50148);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf0d5ff03);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x103100a0);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xef2bffc9);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 12700000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000002);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfffafff2);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x00170027);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffc1ffab);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x00880098);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xff04ff10);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x01a60159);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfd75fe34);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x03b00240);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfaeffd58);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x06a702f9);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xf79dfcda);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x0a310326);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf407fd0d);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0d9f028e);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf0f6fe06);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x10210140);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xef2fff93);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 12800000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000003);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfffeffef);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x000c0034);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffdbff8f);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x005800ce);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xff4ffeb6);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x013c01e1);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfdfbfd76);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x03110337);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfb9dfc2a);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x05f40453);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xf848fb63);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x099904a5);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf482fb9f);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0d4603ce);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf12dfd0c);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x100701df);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xef36ff5c);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 12900000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000003);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0001ffee);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffff0038);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xfffbff82);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x001d00ec);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xffadfe7c);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x00b70242);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfea9fce5);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x024103ff);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfc85fb2a);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x05040587);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xf930fa0a);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x08ca060e);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf52bfa40);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0ccb0507);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf179fc15);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0fe3027d);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xef3fff25);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 13000000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000002);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0005fff0);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xfff20034);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x001bff85);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xffdf00f0);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x0014fe68);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x00200272);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xff71fc8b);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x014d048d);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfd9afa61);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x03e00688);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xfa4ef8da);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x07c80759);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf600f8f4);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0c2f0637);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf1dbfb22);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0fb4031b);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xef4bfeef);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 13100000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0xffff0001);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0007fff5);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffe70028);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x0037ff98);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xffa400d8);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x0079fe7c);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xff87026f);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x0043fc6e);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x004404da);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfecef9da);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x0294074e);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xfb99f7db);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x06980881);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf6fef7be);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0b730759);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf251fa33);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0f7b03b8);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xef5afeb8);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 13200000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0xffff0000);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0008fffc);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffe00017);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x004cffb9);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff7500a8);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x00d1feb6);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfef90238);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x0111fc91);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xff3604df);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x0012f99b);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x012d07d2);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xfd07f714);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x0542097e);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf81ff6a4);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x0a9a086e);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf2dbf94b);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0f380453);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xef6cfe82);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 13300000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0xffffffff);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x00080003);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffde0001);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x0056ffe3);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff570064);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x0113ff10);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe8201d2);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x01cafcf0);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfe35049e);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x0155f9a6);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xffba080e);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xfe8cf689);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x03ce0a4e);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xf961f5a8);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x09a50971);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf379f869);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0eeb04ec);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xef80fe4b);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 13400000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0000fffe);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0007000a);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffe2ffec);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x00540012);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff4e0015);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x0137ff82);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe2e0145);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x0260fd86);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfd51041a);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x0287f9fb);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xfe4a0802);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x001df63f);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x02430aeb);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xfabdf4ce);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x08970a62);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf428f78f);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0e950584);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xef97fe15);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 13500000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0000fffd);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0004000f);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffeaffda);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x0046003d);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff5affc4);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x013b0000);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe04009d);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x02c8fe48);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfc99035a);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x0397fa96);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xfcec07ad);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x01adf637);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x00ac0b53);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xfc2ef419);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x07730b3e);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf4e9f6bd);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0e35061a);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xefb1fddf);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 13600000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0000fffd);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x00000012);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xfff6ffcd);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x002f0061);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff7bff79);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x011e007e);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe08ffe8);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x02f9ff28);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfc17026a);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x0479fb70);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xfbad0713);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x032ff672);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xff100b83);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xfdaff38b);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x063c0c04);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf5baf5f5);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0dcc06ae);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xefcdfda8);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 13700000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0000fffd);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfffd0012);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x0004ffc8);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x00100078);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xffacff3e);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x00e200f0);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe39ff35);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x02f10017);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfbd30156);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x0521fc7f);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xfa9c0638);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x0499f6ee);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xfd7a0b7c);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0xff39f325);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x04f40cb3);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf69af537);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0d5a073f);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xefecfd72);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 13800000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0001fffe);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfffa000e);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x0011ffcb);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xfff0007f);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xffe7ff19);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x008f014a);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe94fe93);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x02b00105);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfbd3002f);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x0585fdb7);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xf9c10525);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x05def7a8);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xfbf20b3c);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x00c7f2e9);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x03a00d48);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf787f484);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0cdf07cd);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf00dfd3c);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 13900000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00010000);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfff80008);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x001bffd7);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffd10076);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x0026ff0e);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x002c0184);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xff0ffe10);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x023b01e0);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfc17ff06);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x05a2ff09);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xf92703e4);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x06f4f89b);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xfa820ac5);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0251f2d9);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x02430dc3);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf881f3dc);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0c5c0859);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf031fd06);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 14000000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00010001);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfff80001);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x0021ffe8);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffba005d);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x0060ff1f);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xffc40198);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xffa0fdb5);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x019a029a);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfc99fdea);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x05750067);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xf8d4027f);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x07d4f9c0);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf9320a1a);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x03d2f2f3);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0x00df0e22);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xf986f341);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0bd108e2);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf058fcd1);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 14100000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000002);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfff9fffa);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x0021fffd);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffac0038);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x008eff4a);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xff630184);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x003afd8b);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x00da0326);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfd51fced);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x050101c0);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xf8cb0103);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x0876fb10);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf80a093e);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0543f338);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xff7a0e66);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xfa94f2b2);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0b3f0967);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf081fc9b);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 14200000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000003);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfffbfff3);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x001d0013);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffaa000b);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x00aaff89);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xff13014a);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x00cefd95);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x000a037b);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfe35fc1d);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x044c0305);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xf90cff7e);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x08d5fc81);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf7100834);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x069ff3a7);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xfe160e8d);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xfbaaf231);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0aa509e9);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf0adfc65);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 14300000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000003);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xffffffef);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x00140025);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffb4ffdd);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x00b2ffd6);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xfedb00f0);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x0150fdd3);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xff380391);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xff36fb85);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x035e0426);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xf994fdfe);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x08eefe0b);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf6490702);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x07e1f43e);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xfcb60e97);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xfcc6f1be);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x0a040a67);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf0dbfc30);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 14400000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000003);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0002ffee);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x00070033);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffc9ffb4);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x00a40027);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xfec3007e);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x01b4fe3f);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfe760369);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x0044fb2e);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x02450518);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xfa5ffc90);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x08c1ffa1);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf5bc05ae);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0902f4fc);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xfb600e85);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xfde7f15a);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x095d0ae2);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf10cfbfb);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 14500000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0xffff0002);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0005ffef);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xfffa0038);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffe5ff95);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x00820074);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xfecc0000);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x01f0fed0);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfdd20304);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x014dfb1d);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x010e05ce);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xfb64fb41);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x084e013b);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf569043e);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0a00f5dd);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xfa150e55);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0xff0bf104);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x08b00b59);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf13ffbc6);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 14600000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0xffff0001);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0008fff4);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffed0035);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x0005ff83);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x005000b4);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xfef6ff82);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x01ffff7a);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfd580269);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x0241fb53);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xffca0640);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xfc99fa1e);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x079a02cb);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf55502ba);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0ad5f6e0);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf8d90e0a);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0031f0bd);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x07fd0bcb);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf174fb91);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 14700000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0xffffffff);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0009fffb);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffe4002a);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x0025ff82);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x001400e0);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xff3cff10);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x01e10030);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfd1201a4);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x0311fbcd);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfe88066a);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xfdf1f92f);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x06aa0449);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf57e0128);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0b7ef801);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf7b00da2);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0156f086);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x07450c39);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf1acfb5c);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 14800000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0000fffe);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x00080002);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffdf0019);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x003fff92);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xffd600f1);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xff96feb6);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x019700e1);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfd0500c2);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x03b0fc84);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfd590649);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0xff5df87f);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x058505aa);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf5e4ff91);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0bf9f93c);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf69d0d20);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0279f05e);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x06880ca3);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf1e6fb28);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 14900000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0000fffd);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x00060009);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffdf0004);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x0051ffb0);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff9d00e8);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xfffcfe7c);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x01280180);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfd32ffd2);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x0413fd6e);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfc4d05df);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x00d1f812);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x043506e4);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf685fdfb);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0c43fa8d);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf5a10c83);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0399f046);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x05c70d08);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf222faf3);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 15000000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0000fffd);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0003000f);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffe5ffef);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x0057ffd9);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff7000c4);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x0062fe68);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x009e01ff);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfd95fee6);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x0435fe7d);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfb710530);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x023cf7ee);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x02c307ef);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf75efc70);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0c5cfbef);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf4c10bce);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x04b3f03f);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x05030d69);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf261fabf);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 15100000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0000fffd);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xffff0012);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xffefffdc);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x00510006);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff540089);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x00befe7c);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0x00060253);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfe27fe0d);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x0413ffa2);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfad10446);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x0390f812);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0x013b08c3);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf868faf6);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0c43fd5f);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf3fd0b02);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x05c7f046);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x043b0dc4);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf2a1fa8b);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 15200000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0001fffe);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfffc0012);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0xfffbffce);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x003f0033);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff4e003f);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x0106feb6);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xff6e0276);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xfeddfd56);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x03b000cc);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfa740329);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x04bff87f);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xffaa095d);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xf99ef995);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0bf9fed8);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf3590a1f);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x06d2f05e);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x03700e1b);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf2e4fa58);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 15300000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x0001ffff);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfff9000f);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x0009ffc8);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x00250059);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff5effee);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x0132ff10);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfee30265);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0xffaafccf);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x031101eb);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfa6001e8);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x05bdf92f);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xfe1b09b6);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xfafaf852);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0b7e0055);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf2d50929);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x07d3f086);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x02a30e6c);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf329fa24);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 15400000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00010001);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfff80009);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x0015ffca);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0x00050074);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xff81ff9f);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x013dff82);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe710221);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x007cfc80);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x024102ed);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfa940090);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x0680fa1e);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xfc9b09cd);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xfc73f736);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0ad501d0);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf2740820);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x08c9f0bd);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x01d40eb9);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf371f9f1);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 15500000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000002);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfff80002);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x001effd5);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffe5007f);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xffb4ff5b);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x01280000);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe2401b0);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x0146fc70);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x014d03c6);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfb10ff32);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x0701fb41);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xfb3709a1);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xfe00f644);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x0a000345);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf2350708);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x09b2f104);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x01050eff);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf3baf9be);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 15600000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000003);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfff9fffb);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x0022ffe6);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffc9007a);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0xfff0ff29);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x00f2007e);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe01011b);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x01f6fc9e);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0x00440467);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfbccfdde);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x0738fc90);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xf9f70934);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0xff99f582);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x090204b0);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf21a05e1);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0a8df15a);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0x00340f41);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf405f98b);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 15700000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000003);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0xfffcfff4);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x0020fffa);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffb40064);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x002fff11);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x00a400f0);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe0d006e);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x0281fd09);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xff3604c9);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfcbffca2);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x0726fdfe);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xf8e80888);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x0134f4f3);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x07e1060c);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf22304af);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0b59f1be);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xff640f7d);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf452f959);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 15800000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0x00000003);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0000fff0);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x001a0010);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffaa0041);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x0067ff13);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0x0043014a);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfe46ffb9);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x02dbfda8);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfe3504e5);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xfddcfb8d);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x06c9ff7e);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xf81107a2);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x02c9f49a);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x069f0753);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf2500373);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0c14f231);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xfe930fb3);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf4a1f927);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 15900000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0xffff0002);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0003ffee);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x000f0023);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffac0016);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x0093ff31);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xffdc0184);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xfea6ff09);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x02fdfe70);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfd5104ba);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0xff15faac);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x06270103);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xf7780688);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x044df479);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x05430883);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf2a00231);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0cbef2b2);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xfdc40fe3);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf4f2f8f5);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+
+	case 16000000:
+		cx25840_write4(client, DIF_BPF_COEFF01, 0xffff0001);
+		cx25840_write4(client, DIF_BPF_COEFF23, 0x0006ffef);
+		cx25840_write4(client, DIF_BPF_COEFF45, 0x00020031);
+		cx25840_write4(client, DIF_BPF_COEFF67, 0xffbaffe8);
+		cx25840_write4(client, DIF_BPF_COEFF89, 0x00adff66);
+		cx25840_write4(client, DIF_BPF_COEFF1011, 0xff790198);
+		cx25840_write4(client, DIF_BPF_COEFF1213, 0xff26fe6e);
+		cx25840_write4(client, DIF_BPF_COEFF1415, 0x02e5ff55);
+		cx25840_write4(client, DIF_BPF_COEFF1617, 0xfc99044a);
+		cx25840_write4(client, DIF_BPF_COEFF1819, 0x005bfa09);
+		cx25840_write4(client, DIF_BPF_COEFF2021, 0x0545027f);
+		cx25840_write4(client, DIF_BPF_COEFF2223, 0xf7230541);
+		cx25840_write4(client, DIF_BPF_COEFF2425, 0x05b8f490);
+		cx25840_write4(client, DIF_BPF_COEFF2627, 0x03d20997);
+		cx25840_write4(client, DIF_BPF_COEFF2829, 0xf31300eb);
+		cx25840_write4(client, DIF_BPF_COEFF3031, 0x0d55f341);
+		cx25840_write4(client, DIF_BPF_COEFF3233, 0xfcf6100e);
+		cx25840_write4(client, DIF_BPF_COEFF3435, 0xf544f8c3);
+		cx25840_write4(client, DIF_BPF_COEFF36, 0x110d0000);
+		break;
+	}
+}
+
+static void cx23888_std_setup(struct i2c_client *client)
+{
+	struct cx25840_state *state = to_state(i2c_get_clientdata(client));
+	v4l2_std_id std = state->std;
+	u32 ifHz;
+
+	cx25840_write4(client, 0x478, 0x6628021F);
+	cx25840_write4(client, 0x400, 0x0);
+	cx25840_write4(client, 0x4b4, 0x20524030);
+	cx25840_write4(client, 0x47c, 0x010a8263);
+
+	if (std & V4L2_STD_525_60) {
+		v4l_dbg(1, cx25840_debug, client, "%s() Selecting NTSC",
+			__func__);
+
+		/* Horiz / vert timing */
+		cx25840_write4(client, 0x428, 0x1e1e601a);
+		cx25840_write4(client, 0x424, 0x5b2d007a);
+
+		/* DIF NTSC */
+		cx25840_write4(client, 0x304, 0x6503bc0c);
+		cx25840_write4(client, 0x308, 0xbd038c85);
+		cx25840_write4(client, 0x30c, 0x1db4640a);
+		cx25840_write4(client, 0x310, 0x00008800);
+		cx25840_write4(client, 0x314, 0x44400400);
+		cx25840_write4(client, 0x32c, 0x0c800800);
+		cx25840_write4(client, 0x330, 0x27000100);
+		cx25840_write4(client, 0x334, 0x1f296e1f);
+		cx25840_write4(client, 0x338, 0x009f50c1);
+		cx25840_write4(client, 0x340, 0x1befbf06);
+		cx25840_write4(client, 0x344, 0x000035e8);
+
+		/* DIF I/F */
+		ifHz = 5400000;
+
+	} else {
+		v4l_dbg(1, cx25840_debug, client, "%s() Selecting PAL-BG",
+			__func__);
+
+		/* Horiz / vert timing */
+		cx25840_write4(client, 0x428, 0x28244024);
+		cx25840_write4(client, 0x424, 0x5d2d0084);
+
+		/* DIF */
+		cx25840_write4(client, 0x304, 0x6503bc0c);
+		cx25840_write4(client, 0x308, 0xbd038c85);
+		cx25840_write4(client, 0x30c, 0x1db4640a);
+		cx25840_write4(client, 0x310, 0x00008800);
+		cx25840_write4(client, 0x314, 0x44400600);
+		cx25840_write4(client, 0x32c, 0x0c800800);
+		cx25840_write4(client, 0x330, 0x27000100);
+		cx25840_write4(client, 0x334, 0x213530ec);
+		cx25840_write4(client, 0x338, 0x00a65ba8);
+		cx25840_write4(client, 0x340, 0x1befbf06);
+		cx25840_write4(client, 0x344, 0x000035e8);
+
+		/* DIF I/F */
+		ifHz = 6000000;
+	}
+
+	cx23885_dif_setup(client, ifHz);
+
+	/* Explicitly ensure the inputs are reconfigured after
+	 * a standard change.
+	 */
+	set_input(client, state->vid_input, state->aud_input);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_ctrl_ops cx25840_ctrl_ops = {
+	.s_ctrl = cx25840_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops cx25840_core_ops = {
+	.log_status = cx25840_log_status,
+	.reset = cx25840_reset,
+	/* calling the (optional) init op will turn on the generic mode */
+	.init = cx25840_init,
+	.load_fw = cx25840_load_fw,
+	.s_io_pin_config = common_s_io_pin_config,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = cx25840_g_register,
+	.s_register = cx25840_s_register,
+#endif
+	.interrupt_service_routine = cx25840_irq_handler,
+};
+
+static const struct v4l2_subdev_tuner_ops cx25840_tuner_ops = {
+	.s_frequency = cx25840_s_frequency,
+	.s_radio = cx25840_s_radio,
+	.g_tuner = cx25840_g_tuner,
+	.s_tuner = cx25840_s_tuner,
+};
+
+static const struct v4l2_subdev_audio_ops cx25840_audio_ops = {
+	.s_clock_freq = cx25840_s_clock_freq,
+	.s_routing = cx25840_s_audio_routing,
+	.s_stream = cx25840_s_audio_stream,
+};
+
+static const struct v4l2_subdev_video_ops cx25840_video_ops = {
+	.g_std = cx25840_g_std,
+	.s_std = cx25840_s_std,
+	.querystd = cx25840_querystd,
+	.s_routing = cx25840_s_video_routing,
+	.s_stream = cx25840_s_stream,
+	.g_input_status = cx25840_g_input_status,
+};
+
+static const struct v4l2_subdev_vbi_ops cx25840_vbi_ops = {
+	.decode_vbi_line = cx25840_decode_vbi_line,
+	.s_raw_fmt = cx25840_s_raw_fmt,
+	.s_sliced_fmt = cx25840_s_sliced_fmt,
+	.g_sliced_fmt = cx25840_g_sliced_fmt,
+};
+
+static const struct v4l2_subdev_pad_ops cx25840_pad_ops = {
+	.set_fmt = cx25840_set_fmt,
+};
+
+static const struct v4l2_subdev_ops cx25840_ops = {
+	.core = &cx25840_core_ops,
+	.tuner = &cx25840_tuner_ops,
+	.audio = &cx25840_audio_ops,
+	.video = &cx25840_video_ops,
+	.vbi = &cx25840_vbi_ops,
+	.pad = &cx25840_pad_ops,
+	.ir = &cx25840_ir_ops,
+};
+
+/* ----------------------------------------------------------------------- */
+
+static u32 get_cx2388x_ident(struct i2c_client *client)
+{
+	u32 ret;
+
+	/* Come out of digital power down */
+	cx25840_write(client, 0x000, 0);
+
+	/*
+	 * Detecting whether the part is cx23885/7/8 is more
+	 * difficult than it needs to be. No ID register. Instead we
+	 * probe certain registers indicated in the datasheets to look
+	 * for specific defaults that differ between the silicon designs.
+	 */
+
+	/* It's either 885/7 if the IR Tx Clk Divider register exists */
+	if (cx25840_read4(client, 0x204) & 0xffff) {
+		/*
+		 * CX23885 returns bogus repetitive byte values for the DIF,
+		 * which doesn't exist for it. (Ex. 8a8a8a8a or 31313131)
+		 */
+		ret = cx25840_read4(client, 0x300);
+		if (((ret & 0xffff0000) >> 16) == (ret & 0xffff)) {
+			/* No DIF */
+			ret = CX23885_AV;
+		} else {
+			/*
+			 * CX23887 has a broken DIF, but the registers
+			 * appear valid (but unused), good enough to detect.
+			 */
+			ret = CX23887_AV;
+		}
+	} else if (cx25840_read4(client, 0x300) & 0x0fffffff) {
+		/* DIF PLL Freq Word reg exists; chip must be a CX23888 */
+		ret = CX23888_AV;
+	} else {
+		v4l_err(client, "Unable to detect h/w, assuming cx23887\n");
+		ret = CX23887_AV;
+	}
+
+	/* Back into digital power down */
+	cx25840_write(client, 0x000, 2);
+	return ret;
+}
+
+static int cx25840_probe(struct i2c_client *client)
+{
+	struct cx25840_state *state;
+	struct v4l2_subdev *sd;
+	int default_volume;
+	u32 id;
+	u16 device_id;
+#if defined(CONFIG_MEDIA_CONTROLLER)
+	int ret;
+#endif
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+
+	v4l_dbg(1, cx25840_debug, client,
+		"detecting cx25840 client on address 0x%x\n",
+		client->addr << 1);
+
+	device_id = cx25840_read(client, 0x101) << 8;
+	device_id |= cx25840_read(client, 0x100);
+	v4l_dbg(1, cx25840_debug, client, "device_id = 0x%04x\n", device_id);
+
+	/*
+	 * The high byte of the device ID should be
+	 * 0x83 for the cx2583x and 0x84 for the cx2584x
+	 */
+	if ((device_id & 0xff00) == 0x8300) {
+		id = CX25836 + ((device_id >> 4) & 0xf) - 6;
+	} else if ((device_id & 0xff00) == 0x8400) {
+		id = CX25840 + ((device_id >> 4) & 0xf);
+	} else if (device_id == 0x0000) {
+		id = get_cx2388x_ident(client);
+	} else if ((device_id & 0xfff0) == 0x5A30) {
+		/* The CX23100 (0x5A3C = 23100) doesn't have an A/V decoder */
+		id = CX2310X_AV;
+	} else if ((device_id & 0xff) == (device_id >> 8)) {
+		v4l_err(client,
+			"likely a confused/unresponsive cx2388[578] A/V decoder found @ 0x%x (%s)\n",
+			client->addr << 1, client->adapter->name);
+		v4l_err(client,
+			"A method to reset it from the cx25840 driver software is not known at this time\n");
+		return -ENODEV;
+	} else {
+		v4l_dbg(1, cx25840_debug, client, "cx25840 not found\n");
+		return -ENODEV;
+	}
+
+	state = devm_kzalloc(&client->dev, sizeof(*state), GFP_KERNEL);
+	if (!state)
+		return -ENOMEM;
+
+	sd = &state->sd;
+	v4l2_i2c_subdev_init(sd, client, &cx25840_ops);
+#if defined(CONFIG_MEDIA_CONTROLLER)
+	/*
+	 * TODO: add media controller support for analog video inputs like
+	 * composite, svideo, etc.
+	 * A real input pad for this analog demod would be like:
+	 *                 ___________
+	 * TUNER --------> |         |
+	 *		   |         |
+	 * SVIDEO .......> | cx25840 |
+	 *		   |         |
+	 * COMPOSITE1 ...> |_________|
+	 *
+	 * However, at least for now, there's no much gain on modelling
+	 * those extra inputs. So, let's add it only when needed.
+	 */
+	state->pads[CX25840_PAD_INPUT].flags = MEDIA_PAD_FL_SINK;
+	state->pads[CX25840_PAD_INPUT].sig_type = PAD_SIGNAL_ANALOG;
+	state->pads[CX25840_PAD_VID_OUT].flags = MEDIA_PAD_FL_SOURCE;
+	state->pads[CX25840_PAD_VID_OUT].sig_type = PAD_SIGNAL_DV;
+	sd->entity.function = MEDIA_ENT_F_ATV_DECODER;
+
+	ret = media_entity_pads_init(&sd->entity, ARRAY_SIZE(state->pads),
+				     state->pads);
+	if (ret < 0) {
+		v4l_info(client, "failed to initialize media entity!\n");
+		return ret;
+	}
+#endif
+
+	switch (id) {
+	case CX23885_AV:
+		v4l_info(client, "cx23885 A/V decoder found @ 0x%x (%s)\n",
+			 client->addr << 1, client->adapter->name);
+		break;
+	case CX23887_AV:
+		v4l_info(client, "cx23887 A/V decoder found @ 0x%x (%s)\n",
+			 client->addr << 1, client->adapter->name);
+		break;
+	case CX23888_AV:
+		v4l_info(client, "cx23888 A/V decoder found @ 0x%x (%s)\n",
+			 client->addr << 1, client->adapter->name);
+		break;
+	case CX2310X_AV:
+		v4l_info(client, "cx%d A/V decoder found @ 0x%x (%s)\n",
+			 device_id, client->addr << 1, client->adapter->name);
+		break;
+	case CX25840:
+	case CX25841:
+	case CX25842:
+	case CX25843:
+		/*
+		 * Note: revision '(device_id & 0x0f) == 2' was never built.
+		 * The marking skips from 0x1 == 22 to 0x3 == 23.
+		 */
+		v4l_info(client, "cx25%3x-2%x found @ 0x%x (%s)\n",
+			 (device_id & 0xfff0) >> 4,
+			 (device_id & 0x0f) < 3 ? (device_id & 0x0f) + 1
+						: (device_id & 0x0f),
+			 client->addr << 1, client->adapter->name);
+		break;
+	case CX25836:
+	case CX25837:
+	default:
+		v4l_info(client, "cx25%3x-%x found @ 0x%x (%s)\n",
+			 (device_id & 0xfff0) >> 4, device_id & 0x0f,
+			 client->addr << 1, client->adapter->name);
+		break;
+	}
+
+	state->c = client;
+	state->vid_input = CX25840_COMPOSITE7;
+	state->aud_input = CX25840_AUDIO8;
+	state->audclk_freq = 48000;
+	state->audmode = V4L2_TUNER_MODE_LANG1;
+	state->vbi_line_offset = 8;
+	state->id = id;
+	state->rev = device_id;
+	state->vbi_regs_offset = id == CX23888_AV ? 0x500 - 0x424 : 0;
+	state->std = V4L2_STD_NTSC_M;
+	v4l2_ctrl_handler_init(&state->hdl, 9);
+	v4l2_ctrl_new_std(&state->hdl, &cx25840_ctrl_ops,
+			  V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(&state->hdl, &cx25840_ctrl_ops,
+			  V4L2_CID_CONTRAST, 0, 127, 1, 64);
+	v4l2_ctrl_new_std(&state->hdl, &cx25840_ctrl_ops,
+			  V4L2_CID_SATURATION, 0, 127, 1, 64);
+	v4l2_ctrl_new_std(&state->hdl, &cx25840_ctrl_ops,
+			  V4L2_CID_HUE, -128, 127, 1, 0);
+	if (!is_cx2583x(state)) {
+		default_volume = cx25840_read(client, 0x8d4);
+		/*
+		 * Enforce the legacy PVR-350/MSP3400 to PVR-150/CX25843 volume
+		 * scale mapping limits to avoid -ERANGE errors when
+		 * initializing the volume control
+		 */
+		if (default_volume > 228) {
+			/* Bottom out at -96 dB, v4l2 vol range 0x2e00-0x2fff */
+			default_volume = 228;
+			cx25840_write(client, 0x8d4, 228);
+		} else if (default_volume < 20) {
+			/* Top out at + 8 dB, v4l2 vol range 0xfe00-0xffff */
+			default_volume = 20;
+			cx25840_write(client, 0x8d4, 20);
+		}
+		default_volume = (((228 - default_volume) >> 1) + 23) << 9;
+
+		state->volume = v4l2_ctrl_new_std(&state->hdl,
+						  &cx25840_audio_ctrl_ops,
+						  V4L2_CID_AUDIO_VOLUME,
+						  0, 65535, 65535 / 100,
+						  default_volume);
+		state->mute = v4l2_ctrl_new_std(&state->hdl,
+						&cx25840_audio_ctrl_ops,
+						V4L2_CID_AUDIO_MUTE,
+						0, 1, 1, 0);
+		v4l2_ctrl_new_std(&state->hdl, &cx25840_audio_ctrl_ops,
+				  V4L2_CID_AUDIO_BALANCE,
+				  0, 65535, 65535 / 100, 32768);
+		v4l2_ctrl_new_std(&state->hdl, &cx25840_audio_ctrl_ops,
+				  V4L2_CID_AUDIO_BASS,
+				  0, 65535, 65535 / 100, 32768);
+		v4l2_ctrl_new_std(&state->hdl, &cx25840_audio_ctrl_ops,
+				  V4L2_CID_AUDIO_TREBLE,
+				  0, 65535, 65535 / 100, 32768);
+	}
+	sd->ctrl_handler = &state->hdl;
+	if (state->hdl.error) {
+		int err = state->hdl.error;
+
+		v4l2_ctrl_handler_free(&state->hdl);
+		return err;
+	}
+	if (!is_cx2583x(state))
+		v4l2_ctrl_cluster(2, &state->volume);
+	v4l2_ctrl_handler_setup(&state->hdl);
+
+	if (client->dev.platform_data) {
+		struct cx25840_platform_data *pdata = client->dev.platform_data;
+
+		state->pvr150_workaround = pdata->pvr150_workaround;
+	}
+
+	cx25840_ir_probe(sd);
+	return 0;
+}
+
+static void cx25840_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct cx25840_state *state = to_state(sd);
+
+	cx25840_ir_remove(sd);
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&state->hdl);
+}
+
+static const struct i2c_device_id cx25840_id[] = {
+	{ "cx25840", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, cx25840_id);
+
+static struct i2c_driver cx25840_driver = {
+	.driver = {
+		.name	= "cx25840",
+	},
+	.probe		= cx25840_probe,
+	.remove		= cx25840_remove,
+	.id_table	= cx25840_id,
+};
+
+module_i2c_driver(cx25840_driver);
diff --git a/drivers/media/i2c/cx25840/cx25840-core.h b/drivers/media/i2c/cx25840/cx25840-core.h
new file mode 100644
index 000000000..8b89e9068
--- /dev/null
+++ b/drivers/media/i2c/cx25840/cx25840-core.h
@@ -0,0 +1,194 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/* cx25840 internal API header
+ *
+ * Copyright (C) 2003-2004 Chris Kennedy
+ */
+
+#ifndef _CX25840_CORE_H_
+#define _CX25840_CORE_H_
+
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <linux/i2c.h>
+
+struct cx25840_ir_state;
+
+enum cx25840_model {
+	CX23885_AV,
+	CX23887_AV,
+	CX23888_AV,
+	CX2310X_AV,
+	CX25840,
+	CX25841,
+	CX25842,
+	CX25843,
+	CX25836,
+	CX25837,
+};
+
+enum cx25840_media_pads {
+	CX25840_PAD_INPUT,
+	CX25840_PAD_VID_OUT,
+
+	CX25840_NUM_PADS
+};
+
+/**
+ * struct cx25840_state - a device instance private data
+ * @c:			i2c_client struct representing this device
+ * @sd:		our V4L2 sub-device
+ * @hdl:		our V4L2 control handler
+ * @volume:		audio volume V4L2 control (non-cx2583x devices only)
+ * @mute:		audio mute V4L2 control (non-cx2583x devices only)
+ * @pvr150_workaround:	whether we enable workaround for Hauppauge PVR150
+ *			hardware bug (audio dropping out)
+ * @generic_mode:	whether we disable ivtv-specific hacks
+ *			this mode gets turned on when the bridge driver calls
+ *			cx25840 subdevice init core op
+ * @radio:		set if we are currently in the radio mode, otherwise
+ *			the current mode is non-radio (that is, video)
+ * @std:		currently set video standard
+ * @vid_input:		currently set video input
+ * @vid_config:	currently set video output configuration
+ *			only used in the generic mode
+ * @aud_input:		currently set audio input
+ * @audclk_freq:	currently set audio sample rate
+ * @audmode:		currently set audio mode (when in non-radio mode)
+ * @vbi_line_offset:	vbi line number offset
+ * @id:		exact device model
+ * @rev:		raw device id read from the chip
+ * @is_initialized:	whether we have already loaded firmware into the chip
+ *			and initialized it
+ * @vbi_regs_offset:	offset of vbi regs
+ * @fw_wait:		wait queue to wake an initialization function up when
+ *			firmware loading (on a separate workqueue) finishes
+ * @fw_work:		a work that actually loads the firmware on a separate
+ *			workqueue
+ * @ir_state:		a pointer to chip IR controller private data
+ * @pads:		array of supported chip pads (currently only a stub)
+ */
+struct cx25840_state {
+	struct i2c_client *c;
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
+	struct {
+		/* volume cluster */
+		struct v4l2_ctrl *volume;
+		struct v4l2_ctrl *mute;
+	};
+	int pvr150_workaround;
+	bool generic_mode;
+	int radio;
+	v4l2_std_id std;
+	enum cx25840_video_input vid_input;
+	u32 vid_config;
+	enum cx25840_audio_input aud_input;
+	u32 audclk_freq;
+	int audmode;
+	int vbi_line_offset;
+	enum cx25840_model id;
+	u32 rev;
+	int is_initialized;
+	unsigned int vbi_regs_offset;
+	wait_queue_head_t fw_wait;
+	struct work_struct fw_work;
+	struct cx25840_ir_state *ir_state;
+#if defined(CONFIG_MEDIA_CONTROLLER)
+	struct media_pad	pads[CX25840_NUM_PADS];
+#endif
+};
+
+static inline struct cx25840_state *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct cx25840_state, sd);
+}
+
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct cx25840_state, hdl)->sd;
+}
+
+static inline bool is_cx2583x(struct cx25840_state *state)
+{
+	return state->id == CX25836 ||
+	       state->id == CX25837;
+}
+
+static inline bool is_cx2584x(struct cx25840_state *state)
+{
+	return state->id == CX25840 ||
+	       state->id == CX25841 ||
+	       state->id == CX25842 ||
+	       state->id == CX25843;
+}
+
+static inline bool is_cx231xx(struct cx25840_state *state)
+{
+	return state->id == CX2310X_AV;
+}
+
+static inline bool is_cx2388x(struct cx25840_state *state)
+{
+	return state->id == CX23885_AV ||
+	       state->id == CX23887_AV ||
+	       state->id == CX23888_AV;
+}
+
+static inline bool is_cx23885(struct cx25840_state *state)
+{
+	return state->id == CX23885_AV;
+}
+
+static inline bool is_cx23887(struct cx25840_state *state)
+{
+	return state->id == CX23887_AV;
+}
+
+static inline bool is_cx23888(struct cx25840_state *state)
+{
+	return state->id == CX23888_AV;
+}
+
+/* ----------------------------------------------------------------------- */
+/* cx25850-core.c							   */
+int cx25840_write(struct i2c_client *client, u16 addr, u8 value);
+int cx25840_write4(struct i2c_client *client, u16 addr, u32 value);
+u8 cx25840_read(struct i2c_client *client, u16 addr);
+u32 cx25840_read4(struct i2c_client *client, u16 addr);
+int cx25840_and_or(struct i2c_client *client, u16 addr, unsigned int mask,
+		   u8 value);
+int cx25840_and_or4(struct i2c_client *client, u16 addr, u32 and_mask,
+		    u32 or_value);
+void cx25840_std_setup(struct i2c_client *client);
+
+/* ----------------------------------------------------------------------- */
+/* cx25850-firmware.c                                                      */
+int cx25840_loadfw(struct i2c_client *client);
+
+/* ----------------------------------------------------------------------- */
+/* cx25850-audio.c                                                         */
+void cx25840_audio_set_path(struct i2c_client *client);
+int cx25840_s_clock_freq(struct v4l2_subdev *sd, u32 freq);
+
+extern const struct v4l2_ctrl_ops cx25840_audio_ctrl_ops;
+
+/* ----------------------------------------------------------------------- */
+/* cx25850-vbi.c                                                           */
+int cx25840_s_raw_fmt(struct v4l2_subdev *sd, struct v4l2_vbi_format *fmt);
+int cx25840_s_sliced_fmt(struct v4l2_subdev *sd,
+			 struct v4l2_sliced_vbi_format *fmt);
+int cx25840_g_sliced_fmt(struct v4l2_subdev *sd,
+			 struct v4l2_sliced_vbi_format *fmt);
+int cx25840_decode_vbi_line(struct v4l2_subdev *sd,
+			    struct v4l2_decode_vbi_line *vbi);
+
+/* ----------------------------------------------------------------------- */
+/* cx25850-ir.c                                                            */
+extern const struct v4l2_subdev_ir_ops cx25840_ir_ops;
+int cx25840_ir_log_status(struct v4l2_subdev *sd);
+int cx25840_ir_irq_handler(struct v4l2_subdev *sd, u32 status, bool *handled);
+int cx25840_ir_probe(struct v4l2_subdev *sd);
+int cx25840_ir_remove(struct v4l2_subdev *sd);
+
+#endif
diff --git a/drivers/media/i2c/cx25840/cx25840-firmware.c b/drivers/media/i2c/cx25840/cx25840-firmware.c
new file mode 100644
index 000000000..02df45ccf
--- /dev/null
+++ b/drivers/media/i2c/cx25840/cx25840-firmware.c
@@ -0,0 +1,161 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/* cx25840 firmware functions
+ */
+
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/firmware.h>
+#include <media/v4l2-common.h>
+#include <media/drv-intf/cx25840.h>
+
+#include "cx25840-core.h"
+
+/*
+ * Mike Isely <isely@pobox.com> - The FWSEND parameter controls the
+ * size of the firmware chunks sent down the I2C bus to the chip.
+ * Previously this had been set to 1024 but unfortunately some I2C
+ * implementations can't transfer data in such big gulps.
+ * Specifically, the pvrusb2 driver has a hard limit of around 60
+ * bytes, due to the encapsulation there of I2C traffic into USB
+ * messages.  So we have to significantly reduce this parameter.
+ */
+#define FWSEND 48
+
+#define FWDEV(x) &((x)->dev)
+
+static char *firmware = "";
+
+module_param(firmware, charp, 0444);
+
+MODULE_PARM_DESC(firmware, "Firmware image to load");
+
+static void start_fw_load(struct i2c_client *client)
+{
+	/* DL_ADDR_LB=0 DL_ADDR_HB=0 */
+	cx25840_write(client, 0x800, 0x00);
+	cx25840_write(client, 0x801, 0x00);
+	// DL_MAP=3 DL_AUTO_INC=0 DL_ENABLE=1
+	cx25840_write(client, 0x803, 0x0b);
+	/* AUTO_INC_DIS=1 */
+	cx25840_write(client, 0x000, 0x20);
+}
+
+static void end_fw_load(struct i2c_client *client)
+{
+	/* AUTO_INC_DIS=0 */
+	cx25840_write(client, 0x000, 0x00);
+	/* DL_ENABLE=0 */
+	cx25840_write(client, 0x803, 0x03);
+}
+
+#define CX2388x_FIRMWARE "v4l-cx23885-avcore-01.fw"
+#define CX231xx_FIRMWARE "v4l-cx231xx-avcore-01.fw"
+#define CX25840_FIRMWARE "v4l-cx25840.fw"
+
+static const char *get_fw_name(struct i2c_client *client)
+{
+	struct cx25840_state *state = to_state(i2c_get_clientdata(client));
+
+	if (firmware[0])
+		return firmware;
+	if (is_cx2388x(state))
+		return CX2388x_FIRMWARE;
+	if (is_cx231xx(state))
+		return CX231xx_FIRMWARE;
+	return CX25840_FIRMWARE;
+}
+
+static int check_fw_load(struct i2c_client *client, int size)
+{
+	/* DL_ADDR_HB DL_ADDR_LB */
+	int s = cx25840_read(client, 0x801) << 8;
+	s |= cx25840_read(client, 0x800);
+
+	if (size != s) {
+		v4l_err(client, "firmware %s load failed\n",
+				get_fw_name(client));
+		return -EINVAL;
+	}
+
+	v4l_info(client, "loaded %s firmware (%d bytes)\n",
+			get_fw_name(client), size);
+	return 0;
+}
+
+static int fw_write(struct i2c_client *client, const u8 *data, int size)
+{
+	if (i2c_master_send(client, data, size) < size) {
+		v4l_err(client, "firmware load i2c failure\n");
+		return -ENOSYS;
+	}
+
+	return 0;
+}
+
+int cx25840_loadfw(struct i2c_client *client)
+{
+	struct cx25840_state *state = to_state(i2c_get_clientdata(client));
+	const struct firmware *fw = NULL;
+	u8 buffer[FWSEND];
+	const u8 *ptr;
+	const char *fwname = get_fw_name(client);
+	int size, retval;
+	int max_buf_size = FWSEND;
+	u32 gpio_oe = 0, gpio_da = 0;
+
+	if (is_cx2388x(state)) {
+		/* Preserve the GPIO OE and output bits */
+		gpio_oe = cx25840_read(client, 0x160);
+		gpio_da = cx25840_read(client, 0x164);
+	}
+
+	/* cx231xx cannot accept more than 16 bytes at a time */
+	if (is_cx231xx(state) && max_buf_size > 16)
+		max_buf_size = 16;
+
+	if (request_firmware(&fw, fwname, FWDEV(client)) != 0) {
+		v4l_err(client, "unable to open firmware %s\n", fwname);
+		return -EINVAL;
+	}
+
+	start_fw_load(client);
+
+	buffer[0] = 0x08;
+	buffer[1] = 0x02;
+
+	size = fw->size;
+	ptr = fw->data;
+	while (size > 0) {
+		int len = min(max_buf_size - 2, size);
+
+		memcpy(buffer + 2, ptr, len);
+
+		retval = fw_write(client, buffer, len + 2);
+
+		if (retval < 0) {
+			release_firmware(fw);
+			return retval;
+		}
+
+		size -= len;
+		ptr += len;
+	}
+
+	end_fw_load(client);
+
+	size = fw->size;
+	release_firmware(fw);
+
+	if (is_cx2388x(state)) {
+		/* Restore GPIO configuration after f/w load */
+		cx25840_write(client, 0x160, gpio_oe);
+		cx25840_write(client, 0x164, gpio_da);
+	}
+
+	return check_fw_load(client, size);
+}
+
+MODULE_FIRMWARE(CX2388x_FIRMWARE);
+MODULE_FIRMWARE(CX231xx_FIRMWARE);
+MODULE_FIRMWARE(CX25840_FIRMWARE);
+
diff --git a/drivers/media/i2c/cx25840/cx25840-ir.c b/drivers/media/i2c/cx25840/cx25840-ir.c
new file mode 100644
index 000000000..8cef9656c
--- /dev/null
+++ b/drivers/media/i2c/cx25840/cx25840-ir.c
@@ -0,0 +1,1239 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ *  Driver for the Conexant CX2584x Audio/Video decoder chip and related cores
+ *
+ *  Integrated Consumer Infrared Controller
+ *
+ *  Copyright (C) 2010  Andy Walls <awalls@md.metrocast.net>
+ */
+
+#include <linux/slab.h>
+#include <linux/kfifo.h>
+#include <linux/module.h>
+#include <media/drv-intf/cx25840.h>
+#include <media/rc-core.h>
+
+#include "cx25840-core.h"
+
+static unsigned int ir_debug;
+module_param(ir_debug, int, 0644);
+MODULE_PARM_DESC(ir_debug, "enable integrated IR debug messages");
+
+#define CX25840_IR_REG_BASE	0x200
+
+#define CX25840_IR_CNTRL_REG	0x200
+#define CNTRL_WIN_3_3	0x00000000
+#define CNTRL_WIN_4_3	0x00000001
+#define CNTRL_WIN_3_4	0x00000002
+#define CNTRL_WIN_4_4	0x00000003
+#define CNTRL_WIN	0x00000003
+#define CNTRL_EDG_NONE	0x00000000
+#define CNTRL_EDG_FALL	0x00000004
+#define CNTRL_EDG_RISE	0x00000008
+#define CNTRL_EDG_BOTH	0x0000000C
+#define CNTRL_EDG	0x0000000C
+#define CNTRL_DMD	0x00000010
+#define CNTRL_MOD	0x00000020
+#define CNTRL_RFE	0x00000040
+#define CNTRL_TFE	0x00000080
+#define CNTRL_RXE	0x00000100
+#define CNTRL_TXE	0x00000200
+#define CNTRL_RIC	0x00000400
+#define CNTRL_TIC	0x00000800
+#define CNTRL_CPL	0x00001000
+#define CNTRL_LBM	0x00002000
+#define CNTRL_R		0x00004000
+
+#define CX25840_IR_TXCLK_REG	0x204
+#define TXCLK_TCD	0x0000FFFF
+
+#define CX25840_IR_RXCLK_REG	0x208
+#define RXCLK_RCD	0x0000FFFF
+
+#define CX25840_IR_CDUTY_REG	0x20C
+#define CDUTY_CDC	0x0000000F
+
+#define CX25840_IR_STATS_REG	0x210
+#define STATS_RTO	0x00000001
+#define STATS_ROR	0x00000002
+#define STATS_RBY	0x00000004
+#define STATS_TBY	0x00000008
+#define STATS_RSR	0x00000010
+#define STATS_TSR	0x00000020
+
+#define CX25840_IR_IRQEN_REG	0x214
+#define IRQEN_RTE	0x00000001
+#define IRQEN_ROE	0x00000002
+#define IRQEN_RSE	0x00000010
+#define IRQEN_TSE	0x00000020
+#define IRQEN_MSK	0x00000033
+
+#define CX25840_IR_FILTR_REG	0x218
+#define FILTR_LPF	0x0000FFFF
+
+#define CX25840_IR_FIFO_REG	0x23C
+#define FIFO_RXTX	0x0000FFFF
+#define FIFO_RXTX_LVL	0x00010000
+#define FIFO_RXTX_RTO	0x0001FFFF
+#define FIFO_RX_NDV	0x00020000
+#define FIFO_RX_DEPTH	8
+#define FIFO_TX_DEPTH	8
+
+#define CX25840_VIDCLK_FREQ	108000000 /* 108 MHz, BT.656 */
+#define CX25840_IR_REFCLK_FREQ	(CX25840_VIDCLK_FREQ / 2)
+
+/*
+ * We use this union internally for convenience, but callers to tx_write
+ * and rx_read will be expecting records of type struct ir_raw_event.
+ * Always ensure the size of this union is dictated by struct ir_raw_event.
+ */
+union cx25840_ir_fifo_rec {
+	u32 hw_fifo_data;
+	struct ir_raw_event ir_core_data;
+};
+
+#define CX25840_IR_RX_KFIFO_SIZE    (256 * sizeof(union cx25840_ir_fifo_rec))
+#define CX25840_IR_TX_KFIFO_SIZE    (256 * sizeof(union cx25840_ir_fifo_rec))
+
+struct cx25840_ir_state {
+	struct i2c_client *c;
+
+	struct v4l2_subdev_ir_parameters rx_params;
+	struct mutex rx_params_lock; /* protects Rx parameter settings cache */
+	atomic_t rxclk_divider;
+	atomic_t rx_invert;
+
+	struct kfifo rx_kfifo;
+	spinlock_t rx_kfifo_lock; /* protect Rx data kfifo */
+
+	struct v4l2_subdev_ir_parameters tx_params;
+	struct mutex tx_params_lock; /* protects Tx parameter settings cache */
+	atomic_t txclk_divider;
+};
+
+static inline struct cx25840_ir_state *to_ir_state(struct v4l2_subdev *sd)
+{
+	struct cx25840_state *state = to_state(sd);
+	return state ? state->ir_state : NULL;
+}
+
+
+/*
+ * Rx and Tx Clock Divider register computations
+ *
+ * Note the largest clock divider value of 0xffff corresponds to:
+ *	(0xffff + 1) * 1000 / 108/2 MHz = 1,213,629.629... ns
+ * which fits in 21 bits, so we'll use unsigned int for time arguments.
+ */
+static inline u16 count_to_clock_divider(unsigned int d)
+{
+	if (d > RXCLK_RCD + 1)
+		d = RXCLK_RCD;
+	else if (d < 2)
+		d = 1;
+	else
+		d--;
+	return (u16) d;
+}
+
+static inline u16 carrier_freq_to_clock_divider(unsigned int freq)
+{
+	return count_to_clock_divider(
+			  DIV_ROUND_CLOSEST(CX25840_IR_REFCLK_FREQ, freq * 16));
+}
+
+static inline unsigned int clock_divider_to_carrier_freq(unsigned int divider)
+{
+	return DIV_ROUND_CLOSEST(CX25840_IR_REFCLK_FREQ, (divider + 1) * 16);
+}
+
+static inline unsigned int clock_divider_to_freq(unsigned int divider,
+						 unsigned int rollovers)
+{
+	return DIV_ROUND_CLOSEST(CX25840_IR_REFCLK_FREQ,
+				 (divider + 1) * rollovers);
+}
+
+/*
+ * Low Pass Filter register calculations
+ *
+ * Note the largest count value of 0xffff corresponds to:
+ *	0xffff * 1000 / 108/2 MHz = 1,213,611.11... ns
+ * which fits in 21 bits, so we'll use unsigned int for time arguments.
+ */
+static inline u16 count_to_lpf_count(unsigned int d)
+{
+	if (d > FILTR_LPF)
+		d = FILTR_LPF;
+	else if (d < 4)
+		d = 0;
+	return (u16) d;
+}
+
+static inline u16 ns_to_lpf_count(unsigned int ns)
+{
+	return count_to_lpf_count(
+		DIV_ROUND_CLOSEST(CX25840_IR_REFCLK_FREQ / 1000000 * ns, 1000));
+}
+
+static inline unsigned int lpf_count_to_ns(unsigned int count)
+{
+	/* Duration of the Low Pass Filter rejection window in ns */
+	return DIV_ROUND_CLOSEST(count * 1000,
+				 CX25840_IR_REFCLK_FREQ / 1000000);
+}
+
+static inline unsigned int lpf_count_to_us(unsigned int count)
+{
+	/* Duration of the Low Pass Filter rejection window in us */
+	return DIV_ROUND_CLOSEST(count, CX25840_IR_REFCLK_FREQ / 1000000);
+}
+
+/*
+ * FIFO register pulse width count computations
+ */
+static u32 clock_divider_to_resolution(u16 divider)
+{
+	/*
+	 * Resolution is the duration of 1 tick of the readable portion of
+	 * the pulse width counter as read from the FIFO.  The two lsb's are
+	 * not readable, hence the << 2.  This function returns ns.
+	 */
+	return DIV_ROUND_CLOSEST((1 << 2)  * ((u32) divider + 1) * 1000,
+				 CX25840_IR_REFCLK_FREQ / 1000000);
+}
+
+static u64 pulse_width_count_to_ns(u16 count, u16 divider)
+{
+	u64 n;
+	u32 rem;
+
+	/*
+	 * The 2 lsb's of the pulse width timer count are not readable, hence
+	 * the (count << 2) | 0x3
+	 */
+	n = (((u64) count << 2) | 0x3) * (divider + 1) * 1000; /* millicycles */
+	rem = do_div(n, CX25840_IR_REFCLK_FREQ / 1000000);     /* / MHz => ns */
+	if (rem >= CX25840_IR_REFCLK_FREQ / 1000000 / 2)
+		n++;
+	return n;
+}
+
+#if 0
+/* Keep as we will need this for Transmit functionality */
+static u16 ns_to_pulse_width_count(u32 ns, u16 divider)
+{
+	u64 n;
+	u32 d;
+	u32 rem;
+
+	/*
+	 * The 2 lsb's of the pulse width timer count are not accessible, hence
+	 * the (1 << 2)
+	 */
+	n = ((u64) ns) * CX25840_IR_REFCLK_FREQ / 1000000; /* millicycles */
+	d = (1 << 2) * ((u32) divider + 1) * 1000; /* millicycles/count */
+	rem = do_div(n, d);
+	if (rem >= d / 2)
+		n++;
+
+	if (n > FIFO_RXTX)
+		n = FIFO_RXTX;
+	else if (n == 0)
+		n = 1;
+	return (u16) n;
+}
+
+#endif
+static unsigned int pulse_width_count_to_us(u16 count, u16 divider)
+{
+	u64 n;
+	u32 rem;
+
+	/*
+	 * The 2 lsb's of the pulse width timer count are not readable, hence
+	 * the (count << 2) | 0x3
+	 */
+	n = (((u64) count << 2) | 0x3) * (divider + 1);    /* cycles      */
+	rem = do_div(n, CX25840_IR_REFCLK_FREQ / 1000000); /* / MHz => us */
+	if (rem >= CX25840_IR_REFCLK_FREQ / 1000000 / 2)
+		n++;
+	return (unsigned int) n;
+}
+
+/*
+ * Pulse Clocks computations: Combined Pulse Width Count & Rx Clock Counts
+ *
+ * The total pulse clock count is an 18 bit pulse width timer count as the most
+ * significant part and (up to) 16 bit clock divider count as a modulus.
+ * When the Rx clock divider ticks down to 0, it increments the 18 bit pulse
+ * width timer count's least significant bit.
+ */
+static u64 ns_to_pulse_clocks(u32 ns)
+{
+	u64 clocks;
+	u32 rem;
+	clocks = CX25840_IR_REFCLK_FREQ / 1000000 * (u64) ns; /* millicycles  */
+	rem = do_div(clocks, 1000);                         /* /1000 = cycles */
+	if (rem >= 1000 / 2)
+		clocks++;
+	return clocks;
+}
+
+static u16 pulse_clocks_to_clock_divider(u64 count)
+{
+	do_div(count, (FIFO_RXTX << 2) | 0x3);
+
+	/* net result needs to be rounded down and decremented by 1 */
+	if (count > RXCLK_RCD + 1)
+		count = RXCLK_RCD;
+	else if (count < 2)
+		count = 1;
+	else
+		count--;
+	return (u16) count;
+}
+
+/*
+ * IR Control Register helpers
+ */
+enum tx_fifo_watermark {
+	TX_FIFO_HALF_EMPTY = 0,
+	TX_FIFO_EMPTY      = CNTRL_TIC,
+};
+
+enum rx_fifo_watermark {
+	RX_FIFO_HALF_FULL = 0,
+	RX_FIFO_NOT_EMPTY = CNTRL_RIC,
+};
+
+static inline void control_tx_irq_watermark(struct i2c_client *c,
+					    enum tx_fifo_watermark level)
+{
+	cx25840_and_or4(c, CX25840_IR_CNTRL_REG, ~CNTRL_TIC, level);
+}
+
+static inline void control_rx_irq_watermark(struct i2c_client *c,
+					    enum rx_fifo_watermark level)
+{
+	cx25840_and_or4(c, CX25840_IR_CNTRL_REG, ~CNTRL_RIC, level);
+}
+
+static inline void control_tx_enable(struct i2c_client *c, bool enable)
+{
+	cx25840_and_or4(c, CX25840_IR_CNTRL_REG, ~(CNTRL_TXE | CNTRL_TFE),
+			enable ? (CNTRL_TXE | CNTRL_TFE) : 0);
+}
+
+static inline void control_rx_enable(struct i2c_client *c, bool enable)
+{
+	cx25840_and_or4(c, CX25840_IR_CNTRL_REG, ~(CNTRL_RXE | CNTRL_RFE),
+			enable ? (CNTRL_RXE | CNTRL_RFE) : 0);
+}
+
+static inline void control_tx_modulation_enable(struct i2c_client *c,
+						bool enable)
+{
+	cx25840_and_or4(c, CX25840_IR_CNTRL_REG, ~CNTRL_MOD,
+			enable ? CNTRL_MOD : 0);
+}
+
+static inline void control_rx_demodulation_enable(struct i2c_client *c,
+						  bool enable)
+{
+	cx25840_and_or4(c, CX25840_IR_CNTRL_REG, ~CNTRL_DMD,
+			enable ? CNTRL_DMD : 0);
+}
+
+static inline void control_rx_s_edge_detection(struct i2c_client *c,
+					       u32 edge_types)
+{
+	cx25840_and_or4(c, CX25840_IR_CNTRL_REG, ~CNTRL_EDG_BOTH,
+			edge_types & CNTRL_EDG_BOTH);
+}
+
+static void control_rx_s_carrier_window(struct i2c_client *c,
+					unsigned int carrier,
+					unsigned int *carrier_range_low,
+					unsigned int *carrier_range_high)
+{
+	u32 v;
+	unsigned int c16 = carrier * 16;
+
+	if (*carrier_range_low < DIV_ROUND_CLOSEST(c16, 16 + 3)) {
+		v = CNTRL_WIN_3_4;
+		*carrier_range_low = DIV_ROUND_CLOSEST(c16, 16 + 4);
+	} else {
+		v = CNTRL_WIN_3_3;
+		*carrier_range_low = DIV_ROUND_CLOSEST(c16, 16 + 3);
+	}
+
+	if (*carrier_range_high > DIV_ROUND_CLOSEST(c16, 16 - 3)) {
+		v |= CNTRL_WIN_4_3;
+		*carrier_range_high = DIV_ROUND_CLOSEST(c16, 16 - 4);
+	} else {
+		v |= CNTRL_WIN_3_3;
+		*carrier_range_high = DIV_ROUND_CLOSEST(c16, 16 - 3);
+	}
+	cx25840_and_or4(c, CX25840_IR_CNTRL_REG, ~CNTRL_WIN, v);
+}
+
+static inline void control_tx_polarity_invert(struct i2c_client *c,
+					      bool invert)
+{
+	cx25840_and_or4(c, CX25840_IR_CNTRL_REG, ~CNTRL_CPL,
+			invert ? CNTRL_CPL : 0);
+}
+
+/*
+ * IR Rx & Tx Clock Register helpers
+ */
+static unsigned int txclk_tx_s_carrier(struct i2c_client *c,
+				       unsigned int freq,
+				       u16 *divider)
+{
+	*divider = carrier_freq_to_clock_divider(freq);
+	cx25840_write4(c, CX25840_IR_TXCLK_REG, *divider);
+	return clock_divider_to_carrier_freq(*divider);
+}
+
+static unsigned int rxclk_rx_s_carrier(struct i2c_client *c,
+				       unsigned int freq,
+				       u16 *divider)
+{
+	*divider = carrier_freq_to_clock_divider(freq);
+	cx25840_write4(c, CX25840_IR_RXCLK_REG, *divider);
+	return clock_divider_to_carrier_freq(*divider);
+}
+
+static u32 txclk_tx_s_max_pulse_width(struct i2c_client *c, u32 ns,
+				      u16 *divider)
+{
+	u64 pulse_clocks;
+
+	if (ns > IR_MAX_DURATION)
+		ns = IR_MAX_DURATION;
+	pulse_clocks = ns_to_pulse_clocks(ns);
+	*divider = pulse_clocks_to_clock_divider(pulse_clocks);
+	cx25840_write4(c, CX25840_IR_TXCLK_REG, *divider);
+	return (u32) pulse_width_count_to_ns(FIFO_RXTX, *divider);
+}
+
+static u32 rxclk_rx_s_max_pulse_width(struct i2c_client *c, u32 ns,
+				      u16 *divider)
+{
+	u64 pulse_clocks;
+
+	if (ns > IR_MAX_DURATION)
+		ns = IR_MAX_DURATION;
+	pulse_clocks = ns_to_pulse_clocks(ns);
+	*divider = pulse_clocks_to_clock_divider(pulse_clocks);
+	cx25840_write4(c, CX25840_IR_RXCLK_REG, *divider);
+	return (u32) pulse_width_count_to_ns(FIFO_RXTX, *divider);
+}
+
+/*
+ * IR Tx Carrier Duty Cycle register helpers
+ */
+static unsigned int cduty_tx_s_duty_cycle(struct i2c_client *c,
+					  unsigned int duty_cycle)
+{
+	u32 n;
+	n = DIV_ROUND_CLOSEST(duty_cycle * 100, 625); /* 16ths of 100% */
+	if (n != 0)
+		n--;
+	if (n > 15)
+		n = 15;
+	cx25840_write4(c, CX25840_IR_CDUTY_REG, n);
+	return DIV_ROUND_CLOSEST((n + 1) * 100, 16);
+}
+
+/*
+ * IR Filter Register helpers
+ */
+static u32 filter_rx_s_min_width(struct i2c_client *c, u32 min_width_ns)
+{
+	u32 count = ns_to_lpf_count(min_width_ns);
+	cx25840_write4(c, CX25840_IR_FILTR_REG, count);
+	return lpf_count_to_ns(count);
+}
+
+/*
+ * IR IRQ Enable Register helpers
+ */
+static inline void irqenable_rx(struct v4l2_subdev *sd, u32 mask)
+{
+	struct cx25840_state *state = to_state(sd);
+
+	if (is_cx23885(state) || is_cx23887(state))
+		mask ^= IRQEN_MSK;
+	mask &= (IRQEN_RTE | IRQEN_ROE | IRQEN_RSE);
+	cx25840_and_or4(state->c, CX25840_IR_IRQEN_REG,
+			~(IRQEN_RTE | IRQEN_ROE | IRQEN_RSE), mask);
+}
+
+static inline void irqenable_tx(struct v4l2_subdev *sd, u32 mask)
+{
+	struct cx25840_state *state = to_state(sd);
+
+	if (is_cx23885(state) || is_cx23887(state))
+		mask ^= IRQEN_MSK;
+	mask &= IRQEN_TSE;
+	cx25840_and_or4(state->c, CX25840_IR_IRQEN_REG, ~IRQEN_TSE, mask);
+}
+
+/*
+ * V4L2 Subdevice IR Ops
+ */
+int cx25840_ir_irq_handler(struct v4l2_subdev *sd, u32 status, bool *handled)
+{
+	struct cx25840_state *state = to_state(sd);
+	struct cx25840_ir_state *ir_state = to_ir_state(sd);
+	struct i2c_client *c = NULL;
+	unsigned long flags;
+
+	union cx25840_ir_fifo_rec rx_data[FIFO_RX_DEPTH];
+	unsigned int i, j, k;
+	u32 events, v;
+	int tsr, rsr, rto, ror, tse, rse, rte, roe, kror;
+	u32 cntrl, irqen, stats;
+
+	*handled = false;
+	if (ir_state == NULL)
+		return -ENODEV;
+
+	c = ir_state->c;
+
+	/* Only support the IR controller for the CX2388[57] AV Core for now */
+	if (!(is_cx23885(state) || is_cx23887(state)))
+		return -ENODEV;
+
+	cntrl = cx25840_read4(c, CX25840_IR_CNTRL_REG);
+	irqen = cx25840_read4(c, CX25840_IR_IRQEN_REG);
+	if (is_cx23885(state) || is_cx23887(state))
+		irqen ^= IRQEN_MSK;
+	stats = cx25840_read4(c, CX25840_IR_STATS_REG);
+
+	tsr = stats & STATS_TSR; /* Tx FIFO Service Request */
+	rsr = stats & STATS_RSR; /* Rx FIFO Service Request */
+	rto = stats & STATS_RTO; /* Rx Pulse Width Timer Time Out */
+	ror = stats & STATS_ROR; /* Rx FIFO Over Run */
+
+	tse = irqen & IRQEN_TSE; /* Tx FIFO Service Request IRQ Enable */
+	rse = irqen & IRQEN_RSE; /* Rx FIFO Service Request IRQ Enable */
+	rte = irqen & IRQEN_RTE; /* Rx Pulse Width Timer Time Out IRQ Enable */
+	roe = irqen & IRQEN_ROE; /* Rx FIFO Over Run IRQ Enable */
+
+	v4l2_dbg(2, ir_debug, sd, "IR IRQ Status:  %s %s %s %s %s %s\n",
+		 tsr ? "tsr" : "   ", rsr ? "rsr" : "   ",
+		 rto ? "rto" : "   ", ror ? "ror" : "   ",
+		 stats & STATS_TBY ? "tby" : "   ",
+		 stats & STATS_RBY ? "rby" : "   ");
+
+	v4l2_dbg(2, ir_debug, sd, "IR IRQ Enables: %s %s %s %s\n",
+		 tse ? "tse" : "   ", rse ? "rse" : "   ",
+		 rte ? "rte" : "   ", roe ? "roe" : "   ");
+
+	/*
+	 * Transmitter interrupt service
+	 */
+	if (tse && tsr) {
+		/*
+		 * TODO:
+		 * Check the watermark threshold setting
+		 * Pull FIFO_TX_DEPTH or FIFO_TX_DEPTH/2 entries from tx_kfifo
+		 * Push the data to the hardware FIFO.
+		 * If there was nothing more to send in the tx_kfifo, disable
+		 *	the TSR IRQ and notify the v4l2_device.
+		 * If there was something in the tx_kfifo, check the tx_kfifo
+		 *      level and notify the v4l2_device, if it is low.
+		 */
+		/* For now, inhibit TSR interrupt until Tx is implemented */
+		irqenable_tx(sd, 0);
+		events = V4L2_SUBDEV_IR_TX_FIFO_SERVICE_REQ;
+		v4l2_subdev_notify(sd, V4L2_SUBDEV_IR_TX_NOTIFY, &events);
+		*handled = true;
+	}
+
+	/*
+	 * Receiver interrupt service
+	 */
+	kror = 0;
+	if ((rse && rsr) || (rte && rto)) {
+		/*
+		 * Receive data on RSR to clear the STATS_RSR.
+		 * Receive data on RTO, since we may not have yet hit the RSR
+		 * watermark when we receive the RTO.
+		 */
+		for (i = 0, v = FIFO_RX_NDV;
+		     (v & FIFO_RX_NDV) && !kror; i = 0) {
+			for (j = 0;
+			     (v & FIFO_RX_NDV) && j < FIFO_RX_DEPTH; j++) {
+				v = cx25840_read4(c, CX25840_IR_FIFO_REG);
+				rx_data[i].hw_fifo_data = v & ~FIFO_RX_NDV;
+				i++;
+			}
+			if (i == 0)
+				break;
+			j = i * sizeof(union cx25840_ir_fifo_rec);
+			k = kfifo_in_locked(&ir_state->rx_kfifo,
+					    (unsigned char *) rx_data, j,
+					    &ir_state->rx_kfifo_lock);
+			if (k != j)
+				kror++; /* rx_kfifo over run */
+		}
+		*handled = true;
+	}
+
+	events = 0;
+	v = 0;
+	if (kror) {
+		events |= V4L2_SUBDEV_IR_RX_SW_FIFO_OVERRUN;
+		v4l2_err(sd, "IR receiver software FIFO overrun\n");
+	}
+	if (roe && ror) {
+		/*
+		 * The RX FIFO Enable (CNTRL_RFE) must be toggled to clear
+		 * the Rx FIFO Over Run status (STATS_ROR)
+		 */
+		v |= CNTRL_RFE;
+		events |= V4L2_SUBDEV_IR_RX_HW_FIFO_OVERRUN;
+		v4l2_err(sd, "IR receiver hardware FIFO overrun\n");
+	}
+	if (rte && rto) {
+		/*
+		 * The IR Receiver Enable (CNTRL_RXE) must be toggled to clear
+		 * the Rx Pulse Width Timer Time Out (STATS_RTO)
+		 */
+		v |= CNTRL_RXE;
+		events |= V4L2_SUBDEV_IR_RX_END_OF_RX_DETECTED;
+	}
+	if (v) {
+		/* Clear STATS_ROR & STATS_RTO as needed by resetting hardware */
+		cx25840_write4(c, CX25840_IR_CNTRL_REG, cntrl & ~v);
+		cx25840_write4(c, CX25840_IR_CNTRL_REG, cntrl);
+		*handled = true;
+	}
+	spin_lock_irqsave(&ir_state->rx_kfifo_lock, flags);
+	if (kfifo_len(&ir_state->rx_kfifo) >= CX25840_IR_RX_KFIFO_SIZE / 2)
+		events |= V4L2_SUBDEV_IR_RX_FIFO_SERVICE_REQ;
+	spin_unlock_irqrestore(&ir_state->rx_kfifo_lock, flags);
+
+	if (events)
+		v4l2_subdev_notify(sd, V4L2_SUBDEV_IR_RX_NOTIFY, &events);
+	return 0;
+}
+
+/* Receiver */
+static int cx25840_ir_rx_read(struct v4l2_subdev *sd, u8 *buf, size_t count,
+			      ssize_t *num)
+{
+	struct cx25840_ir_state *ir_state = to_ir_state(sd);
+	bool invert;
+	u16 divider;
+	unsigned int i, n;
+	union cx25840_ir_fifo_rec *p;
+	unsigned u, v, w;
+
+	if (ir_state == NULL)
+		return -ENODEV;
+
+	invert = (bool) atomic_read(&ir_state->rx_invert);
+	divider = (u16) atomic_read(&ir_state->rxclk_divider);
+
+	n = count / sizeof(union cx25840_ir_fifo_rec)
+		* sizeof(union cx25840_ir_fifo_rec);
+	if (n == 0) {
+		*num = 0;
+		return 0;
+	}
+
+	n = kfifo_out_locked(&ir_state->rx_kfifo, buf, n,
+			     &ir_state->rx_kfifo_lock);
+
+	n /= sizeof(union cx25840_ir_fifo_rec);
+	*num = n * sizeof(union cx25840_ir_fifo_rec);
+
+	for (p = (union cx25840_ir_fifo_rec *) buf, i = 0; i < n; p++, i++) {
+
+		if ((p->hw_fifo_data & FIFO_RXTX_RTO) == FIFO_RXTX_RTO) {
+			/* Assume RTO was because of no IR light input */
+			u = 0;
+			w = 1;
+		} else {
+			u = (p->hw_fifo_data & FIFO_RXTX_LVL) ? 1 : 0;
+			if (invert)
+				u = u ? 0 : 1;
+			w = 0;
+		}
+
+		v = (unsigned) pulse_width_count_to_ns(
+				  (u16)(p->hw_fifo_data & FIFO_RXTX), divider) / 1000;
+		if (v > IR_MAX_DURATION)
+			v = IR_MAX_DURATION;
+
+		p->ir_core_data = (struct ir_raw_event)
+			{ .pulse = u, .duration = v, .timeout = w };
+
+		v4l2_dbg(2, ir_debug, sd, "rx read: %10u ns  %s  %s\n",
+			 v, u ? "mark" : "space", w ? "(timed out)" : "");
+		if (w)
+			v4l2_dbg(2, ir_debug, sd, "rx read: end of rx\n");
+	}
+	return 0;
+}
+
+static int cx25840_ir_rx_g_parameters(struct v4l2_subdev *sd,
+				      struct v4l2_subdev_ir_parameters *p)
+{
+	struct cx25840_ir_state *ir_state = to_ir_state(sd);
+
+	if (ir_state == NULL)
+		return -ENODEV;
+
+	mutex_lock(&ir_state->rx_params_lock);
+	memcpy(p, &ir_state->rx_params,
+				      sizeof(struct v4l2_subdev_ir_parameters));
+	mutex_unlock(&ir_state->rx_params_lock);
+	return 0;
+}
+
+static int cx25840_ir_rx_shutdown(struct v4l2_subdev *sd)
+{
+	struct cx25840_ir_state *ir_state = to_ir_state(sd);
+	struct i2c_client *c;
+
+	if (ir_state == NULL)
+		return -ENODEV;
+
+	c = ir_state->c;
+	mutex_lock(&ir_state->rx_params_lock);
+
+	/* Disable or slow down all IR Rx circuits and counters */
+	irqenable_rx(sd, 0);
+	control_rx_enable(c, false);
+	control_rx_demodulation_enable(c, false);
+	control_rx_s_edge_detection(c, CNTRL_EDG_NONE);
+	filter_rx_s_min_width(c, 0);
+	cx25840_write4(c, CX25840_IR_RXCLK_REG, RXCLK_RCD);
+
+	ir_state->rx_params.shutdown = true;
+
+	mutex_unlock(&ir_state->rx_params_lock);
+	return 0;
+}
+
+static int cx25840_ir_rx_s_parameters(struct v4l2_subdev *sd,
+				      struct v4l2_subdev_ir_parameters *p)
+{
+	struct cx25840_ir_state *ir_state = to_ir_state(sd);
+	struct i2c_client *c;
+	struct v4l2_subdev_ir_parameters *o;
+	u16 rxclk_divider;
+
+	if (ir_state == NULL)
+		return -ENODEV;
+
+	if (p->shutdown)
+		return cx25840_ir_rx_shutdown(sd);
+
+	if (p->mode != V4L2_SUBDEV_IR_MODE_PULSE_WIDTH)
+		return -ENOSYS;
+
+	c = ir_state->c;
+	o = &ir_state->rx_params;
+
+	mutex_lock(&ir_state->rx_params_lock);
+
+	o->shutdown = p->shutdown;
+
+	p->mode = V4L2_SUBDEV_IR_MODE_PULSE_WIDTH;
+	o->mode = p->mode;
+
+	p->bytes_per_data_element = sizeof(union cx25840_ir_fifo_rec);
+	o->bytes_per_data_element = p->bytes_per_data_element;
+
+	/* Before we tweak the hardware, we have to disable the receiver */
+	irqenable_rx(sd, 0);
+	control_rx_enable(c, false);
+
+	control_rx_demodulation_enable(c, p->modulation);
+	o->modulation = p->modulation;
+
+	if (p->modulation) {
+		p->carrier_freq = rxclk_rx_s_carrier(c, p->carrier_freq,
+						     &rxclk_divider);
+
+		o->carrier_freq = p->carrier_freq;
+
+		p->duty_cycle = 50;
+		o->duty_cycle = p->duty_cycle;
+
+		control_rx_s_carrier_window(c, p->carrier_freq,
+					    &p->carrier_range_lower,
+					    &p->carrier_range_upper);
+		o->carrier_range_lower = p->carrier_range_lower;
+		o->carrier_range_upper = p->carrier_range_upper;
+
+		p->max_pulse_width =
+			(u32) pulse_width_count_to_ns(FIFO_RXTX, rxclk_divider);
+	} else {
+		p->max_pulse_width =
+			    rxclk_rx_s_max_pulse_width(c, p->max_pulse_width,
+						       &rxclk_divider);
+	}
+	o->max_pulse_width = p->max_pulse_width;
+	atomic_set(&ir_state->rxclk_divider, rxclk_divider);
+
+	p->noise_filter_min_width =
+			    filter_rx_s_min_width(c, p->noise_filter_min_width);
+	o->noise_filter_min_width = p->noise_filter_min_width;
+
+	p->resolution = clock_divider_to_resolution(rxclk_divider);
+	o->resolution = p->resolution;
+
+	/* FIXME - make this dependent on resolution for better performance */
+	control_rx_irq_watermark(c, RX_FIFO_HALF_FULL);
+
+	control_rx_s_edge_detection(c, CNTRL_EDG_BOTH);
+
+	o->invert_level = p->invert_level;
+	atomic_set(&ir_state->rx_invert, p->invert_level);
+
+	o->interrupt_enable = p->interrupt_enable;
+	o->enable = p->enable;
+	if (p->enable) {
+		unsigned long flags;
+
+		spin_lock_irqsave(&ir_state->rx_kfifo_lock, flags);
+		kfifo_reset(&ir_state->rx_kfifo);
+		spin_unlock_irqrestore(&ir_state->rx_kfifo_lock, flags);
+		if (p->interrupt_enable)
+			irqenable_rx(sd, IRQEN_RSE | IRQEN_RTE | IRQEN_ROE);
+		control_rx_enable(c, p->enable);
+	}
+
+	mutex_unlock(&ir_state->rx_params_lock);
+	return 0;
+}
+
+/* Transmitter */
+static int cx25840_ir_tx_write(struct v4l2_subdev *sd, u8 *buf, size_t count,
+			       ssize_t *num)
+{
+	struct cx25840_ir_state *ir_state = to_ir_state(sd);
+
+	if (ir_state == NULL)
+		return -ENODEV;
+
+#if 0
+	/*
+	 * FIXME - the code below is an incomplete and untested sketch of what
+	 * may need to be done.  The critical part is to get 4 (or 8) pulses
+	 * from the tx_kfifo, or converted from ns to the proper units from the
+	 * input, and push them off to the hardware Tx FIFO right away, if the
+	 * HW TX fifo needs service.  The rest can be pushed to the tx_kfifo in
+	 * a less critical timeframe.  Also watch out for overruning the
+	 * tx_kfifo - don't let it happen and let the caller know not all his
+	 * pulses were written.
+	 */
+	u32 *ns_pulse = (u32 *) buf;
+	unsigned int n;
+	u32 fifo_pulse[FIFO_TX_DEPTH];
+	u32 mark;
+
+	/* Compute how much we can fit in the tx kfifo */
+	n = CX25840_IR_TX_KFIFO_SIZE - kfifo_len(ir_state->tx_kfifo);
+	n = min(n, (unsigned int) count);
+	n /= sizeof(u32);
+
+	/* FIXME - turn on Tx Fifo service interrupt
+	 * check hardware fifo level, and other stuff
+	 */
+	for (i = 0; i < n; ) {
+		for (j = 0; j < FIFO_TX_DEPTH / 2 && i < n; j++) {
+			mark = ns_pulse[i] & LEVEL_MASK;
+			fifo_pulse[j] = ns_to_pulse_width_count(
+					 ns_pulse[i] &
+					       ~LEVEL_MASK,
+					 ir_state->txclk_divider);
+			if (mark)
+				fifo_pulse[j] &= FIFO_RXTX_LVL;
+			i++;
+		}
+		kfifo_put(ir_state->tx_kfifo, (u8 *) fifo_pulse,
+							       j * sizeof(u32));
+	}
+	*num = n * sizeof(u32);
+#else
+	/* For now enable the Tx FIFO Service interrupt & pretend we did work */
+	irqenable_tx(sd, IRQEN_TSE);
+	*num = count;
+#endif
+	return 0;
+}
+
+static int cx25840_ir_tx_g_parameters(struct v4l2_subdev *sd,
+				      struct v4l2_subdev_ir_parameters *p)
+{
+	struct cx25840_ir_state *ir_state = to_ir_state(sd);
+
+	if (ir_state == NULL)
+		return -ENODEV;
+
+	mutex_lock(&ir_state->tx_params_lock);
+	memcpy(p, &ir_state->tx_params,
+				      sizeof(struct v4l2_subdev_ir_parameters));
+	mutex_unlock(&ir_state->tx_params_lock);
+	return 0;
+}
+
+static int cx25840_ir_tx_shutdown(struct v4l2_subdev *sd)
+{
+	struct cx25840_ir_state *ir_state = to_ir_state(sd);
+	struct i2c_client *c;
+
+	if (ir_state == NULL)
+		return -ENODEV;
+
+	c = ir_state->c;
+	mutex_lock(&ir_state->tx_params_lock);
+
+	/* Disable or slow down all IR Tx circuits and counters */
+	irqenable_tx(sd, 0);
+	control_tx_enable(c, false);
+	control_tx_modulation_enable(c, false);
+	cx25840_write4(c, CX25840_IR_TXCLK_REG, TXCLK_TCD);
+
+	ir_state->tx_params.shutdown = true;
+
+	mutex_unlock(&ir_state->tx_params_lock);
+	return 0;
+}
+
+static int cx25840_ir_tx_s_parameters(struct v4l2_subdev *sd,
+				      struct v4l2_subdev_ir_parameters *p)
+{
+	struct cx25840_ir_state *ir_state = to_ir_state(sd);
+	struct i2c_client *c;
+	struct v4l2_subdev_ir_parameters *o;
+	u16 txclk_divider;
+
+	if (ir_state == NULL)
+		return -ENODEV;
+
+	if (p->shutdown)
+		return cx25840_ir_tx_shutdown(sd);
+
+	if (p->mode != V4L2_SUBDEV_IR_MODE_PULSE_WIDTH)
+		return -ENOSYS;
+
+	c = ir_state->c;
+	o = &ir_state->tx_params;
+	mutex_lock(&ir_state->tx_params_lock);
+
+	o->shutdown = p->shutdown;
+
+	p->mode = V4L2_SUBDEV_IR_MODE_PULSE_WIDTH;
+	o->mode = p->mode;
+
+	p->bytes_per_data_element = sizeof(union cx25840_ir_fifo_rec);
+	o->bytes_per_data_element = p->bytes_per_data_element;
+
+	/* Before we tweak the hardware, we have to disable the transmitter */
+	irqenable_tx(sd, 0);
+	control_tx_enable(c, false);
+
+	control_tx_modulation_enable(c, p->modulation);
+	o->modulation = p->modulation;
+
+	if (p->modulation) {
+		p->carrier_freq = txclk_tx_s_carrier(c, p->carrier_freq,
+						     &txclk_divider);
+		o->carrier_freq = p->carrier_freq;
+
+		p->duty_cycle = cduty_tx_s_duty_cycle(c, p->duty_cycle);
+		o->duty_cycle = p->duty_cycle;
+
+		p->max_pulse_width =
+			(u32) pulse_width_count_to_ns(FIFO_RXTX, txclk_divider);
+	} else {
+		p->max_pulse_width =
+			    txclk_tx_s_max_pulse_width(c, p->max_pulse_width,
+						       &txclk_divider);
+	}
+	o->max_pulse_width = p->max_pulse_width;
+	atomic_set(&ir_state->txclk_divider, txclk_divider);
+
+	p->resolution = clock_divider_to_resolution(txclk_divider);
+	o->resolution = p->resolution;
+
+	/* FIXME - make this dependent on resolution for better performance */
+	control_tx_irq_watermark(c, TX_FIFO_HALF_EMPTY);
+
+	control_tx_polarity_invert(c, p->invert_carrier_sense);
+	o->invert_carrier_sense = p->invert_carrier_sense;
+
+	/*
+	 * FIXME: we don't have hardware help for IO pin level inversion
+	 * here like we have on the CX23888.
+	 * Act on this with some mix of logical inversion of data levels,
+	 * carrier polarity, and carrier duty cycle.
+	 */
+	o->invert_level = p->invert_level;
+
+	o->interrupt_enable = p->interrupt_enable;
+	o->enable = p->enable;
+	if (p->enable) {
+		/* reset tx_fifo here */
+		if (p->interrupt_enable)
+			irqenable_tx(sd, IRQEN_TSE);
+		control_tx_enable(c, p->enable);
+	}
+
+	mutex_unlock(&ir_state->tx_params_lock);
+	return 0;
+}
+
+
+/*
+ * V4L2 Subdevice Core Ops support
+ */
+int cx25840_ir_log_status(struct v4l2_subdev *sd)
+{
+	struct cx25840_state *state = to_state(sd);
+	struct i2c_client *c = state->c;
+	char *s;
+	int i, j;
+	u32 cntrl, txclk, rxclk, cduty, stats, irqen, filtr;
+
+	/* The CX23888 chip doesn't have an IR controller on the A/V core */
+	if (is_cx23888(state))
+		return 0;
+
+	cntrl = cx25840_read4(c, CX25840_IR_CNTRL_REG);
+	txclk = cx25840_read4(c, CX25840_IR_TXCLK_REG) & TXCLK_TCD;
+	rxclk = cx25840_read4(c, CX25840_IR_RXCLK_REG) & RXCLK_RCD;
+	cduty = cx25840_read4(c, CX25840_IR_CDUTY_REG) & CDUTY_CDC;
+	stats = cx25840_read4(c, CX25840_IR_STATS_REG);
+	irqen = cx25840_read4(c, CX25840_IR_IRQEN_REG);
+	if (is_cx23885(state) || is_cx23887(state))
+		irqen ^= IRQEN_MSK;
+	filtr = cx25840_read4(c, CX25840_IR_FILTR_REG) & FILTR_LPF;
+
+	v4l2_info(sd, "IR Receiver:\n");
+	v4l2_info(sd, "\tEnabled:                           %s\n",
+		  cntrl & CNTRL_RXE ? "yes" : "no");
+	v4l2_info(sd, "\tDemodulation from a carrier:       %s\n",
+		  cntrl & CNTRL_DMD ? "enabled" : "disabled");
+	v4l2_info(sd, "\tFIFO:                              %s\n",
+		  cntrl & CNTRL_RFE ? "enabled" : "disabled");
+	switch (cntrl & CNTRL_EDG) {
+	case CNTRL_EDG_NONE:
+		s = "disabled";
+		break;
+	case CNTRL_EDG_FALL:
+		s = "falling edge";
+		break;
+	case CNTRL_EDG_RISE:
+		s = "rising edge";
+		break;
+	case CNTRL_EDG_BOTH:
+		s = "rising & falling edges";
+		break;
+	default:
+		s = "??? edge";
+		break;
+	}
+	v4l2_info(sd, "\tPulse timers' start/stop trigger:  %s\n", s);
+	v4l2_info(sd, "\tFIFO data on pulse timer overflow: %s\n",
+		  cntrl & CNTRL_R ? "not loaded" : "overflow marker");
+	v4l2_info(sd, "\tFIFO interrupt watermark:          %s\n",
+		  cntrl & CNTRL_RIC ? "not empty" : "half full or greater");
+	v4l2_info(sd, "\tLoopback mode:                     %s\n",
+		  cntrl & CNTRL_LBM ? "loopback active" : "normal receive");
+	if (cntrl & CNTRL_DMD) {
+		v4l2_info(sd, "\tExpected carrier (16 clocks):      %u Hz\n",
+			  clock_divider_to_carrier_freq(rxclk));
+		switch (cntrl & CNTRL_WIN) {
+		case CNTRL_WIN_3_3:
+			i = 3;
+			j = 3;
+			break;
+		case CNTRL_WIN_4_3:
+			i = 4;
+			j = 3;
+			break;
+		case CNTRL_WIN_3_4:
+			i = 3;
+			j = 4;
+			break;
+		case CNTRL_WIN_4_4:
+			i = 4;
+			j = 4;
+			break;
+		default:
+			i = 0;
+			j = 0;
+			break;
+		}
+		v4l2_info(sd, "\tNext carrier edge window:	    16 clocks -%1d/+%1d, %u to %u Hz\n",
+			  i, j,
+			  clock_divider_to_freq(rxclk, 16 + j),
+			  clock_divider_to_freq(rxclk, 16 - i));
+	}
+	v4l2_info(sd, "\tMax measurable pulse width:        %u us, %llu ns\n",
+		  pulse_width_count_to_us(FIFO_RXTX, rxclk),
+		  pulse_width_count_to_ns(FIFO_RXTX, rxclk));
+	v4l2_info(sd, "\tLow pass filter:                   %s\n",
+		  filtr ? "enabled" : "disabled");
+	if (filtr)
+		v4l2_info(sd, "\tMin acceptable pulse width (LPF):  %u us, %u ns\n",
+			  lpf_count_to_us(filtr),
+			  lpf_count_to_ns(filtr));
+	v4l2_info(sd, "\tPulse width timer timed-out:       %s\n",
+		  stats & STATS_RTO ? "yes" : "no");
+	v4l2_info(sd, "\tPulse width timer time-out intr:   %s\n",
+		  irqen & IRQEN_RTE ? "enabled" : "disabled");
+	v4l2_info(sd, "\tFIFO overrun:                      %s\n",
+		  stats & STATS_ROR ? "yes" : "no");
+	v4l2_info(sd, "\tFIFO overrun interrupt:            %s\n",
+		  irqen & IRQEN_ROE ? "enabled" : "disabled");
+	v4l2_info(sd, "\tBusy:                              %s\n",
+		  stats & STATS_RBY ? "yes" : "no");
+	v4l2_info(sd, "\tFIFO service requested:            %s\n",
+		  stats & STATS_RSR ? "yes" : "no");
+	v4l2_info(sd, "\tFIFO service request interrupt:    %s\n",
+		  irqen & IRQEN_RSE ? "enabled" : "disabled");
+
+	v4l2_info(sd, "IR Transmitter:\n");
+	v4l2_info(sd, "\tEnabled:                           %s\n",
+		  cntrl & CNTRL_TXE ? "yes" : "no");
+	v4l2_info(sd, "\tModulation onto a carrier:         %s\n",
+		  cntrl & CNTRL_MOD ? "enabled" : "disabled");
+	v4l2_info(sd, "\tFIFO:                              %s\n",
+		  cntrl & CNTRL_TFE ? "enabled" : "disabled");
+	v4l2_info(sd, "\tFIFO interrupt watermark:          %s\n",
+		  cntrl & CNTRL_TIC ? "not empty" : "half full or less");
+	v4l2_info(sd, "\tCarrier polarity:                  %s\n",
+		  cntrl & CNTRL_CPL ? "space:burst mark:noburst"
+				    : "space:noburst mark:burst");
+	if (cntrl & CNTRL_MOD) {
+		v4l2_info(sd, "\tCarrier (16 clocks):               %u Hz\n",
+			  clock_divider_to_carrier_freq(txclk));
+		v4l2_info(sd, "\tCarrier duty cycle:                %2u/16\n",
+			  cduty + 1);
+	}
+	v4l2_info(sd, "\tMax pulse width:                   %u us, %llu ns\n",
+		  pulse_width_count_to_us(FIFO_RXTX, txclk),
+		  pulse_width_count_to_ns(FIFO_RXTX, txclk));
+	v4l2_info(sd, "\tBusy:                              %s\n",
+		  stats & STATS_TBY ? "yes" : "no");
+	v4l2_info(sd, "\tFIFO service requested:            %s\n",
+		  stats & STATS_TSR ? "yes" : "no");
+	v4l2_info(sd, "\tFIFO service request interrupt:    %s\n",
+		  irqen & IRQEN_TSE ? "enabled" : "disabled");
+
+	return 0;
+}
+
+
+const struct v4l2_subdev_ir_ops cx25840_ir_ops = {
+	.rx_read = cx25840_ir_rx_read,
+	.rx_g_parameters = cx25840_ir_rx_g_parameters,
+	.rx_s_parameters = cx25840_ir_rx_s_parameters,
+
+	.tx_write = cx25840_ir_tx_write,
+	.tx_g_parameters = cx25840_ir_tx_g_parameters,
+	.tx_s_parameters = cx25840_ir_tx_s_parameters,
+};
+
+
+static const struct v4l2_subdev_ir_parameters default_rx_params = {
+	.bytes_per_data_element = sizeof(union cx25840_ir_fifo_rec),
+	.mode = V4L2_SUBDEV_IR_MODE_PULSE_WIDTH,
+
+	.enable = false,
+	.interrupt_enable = false,
+	.shutdown = true,
+
+	.modulation = true,
+	.carrier_freq = 36000, /* 36 kHz - RC-5, and RC-6 carrier */
+
+	/* RC-5: 666,667 ns = 1/36 kHz * 32 cycles * 1 mark * 0.75 */
+	/* RC-6: 333,333 ns = 1/36 kHz * 16 cycles * 1 mark * 0.75 */
+	.noise_filter_min_width = 333333, /* ns */
+	.carrier_range_lower = 35000,
+	.carrier_range_upper = 37000,
+	.invert_level = false,
+};
+
+static const struct v4l2_subdev_ir_parameters default_tx_params = {
+	.bytes_per_data_element = sizeof(union cx25840_ir_fifo_rec),
+	.mode = V4L2_SUBDEV_IR_MODE_PULSE_WIDTH,
+
+	.enable = false,
+	.interrupt_enable = false,
+	.shutdown = true,
+
+	.modulation = true,
+	.carrier_freq = 36000, /* 36 kHz - RC-5 carrier */
+	.duty_cycle = 25,      /* 25 %   - RC-5 carrier */
+	.invert_level = false,
+	.invert_carrier_sense = false,
+};
+
+int cx25840_ir_probe(struct v4l2_subdev *sd)
+{
+	struct cx25840_state *state = to_state(sd);
+	struct cx25840_ir_state *ir_state;
+	struct v4l2_subdev_ir_parameters default_params;
+
+	/* Only init the IR controller for the CX2388[57] AV Core for now */
+	if (!(is_cx23885(state) || is_cx23887(state)))
+		return 0;
+
+	ir_state = devm_kzalloc(&state->c->dev, sizeof(*ir_state), GFP_KERNEL);
+	if (ir_state == NULL)
+		return -ENOMEM;
+
+	spin_lock_init(&ir_state->rx_kfifo_lock);
+	if (kfifo_alloc(&ir_state->rx_kfifo,
+			CX25840_IR_RX_KFIFO_SIZE, GFP_KERNEL))
+		return -ENOMEM;
+
+	ir_state->c = state->c;
+	state->ir_state = ir_state;
+
+	/* Ensure no interrupts arrive yet */
+	if (is_cx23885(state) || is_cx23887(state))
+		cx25840_write4(ir_state->c, CX25840_IR_IRQEN_REG, IRQEN_MSK);
+	else
+		cx25840_write4(ir_state->c, CX25840_IR_IRQEN_REG, 0);
+
+	mutex_init(&ir_state->rx_params_lock);
+	default_params = default_rx_params;
+	v4l2_subdev_call(sd, ir, rx_s_parameters, &default_params);
+
+	mutex_init(&ir_state->tx_params_lock);
+	default_params = default_tx_params;
+	v4l2_subdev_call(sd, ir, tx_s_parameters, &default_params);
+
+	return 0;
+}
+
+int cx25840_ir_remove(struct v4l2_subdev *sd)
+{
+	struct cx25840_state *state = to_state(sd);
+	struct cx25840_ir_state *ir_state = to_ir_state(sd);
+
+	if (ir_state == NULL)
+		return -ENODEV;
+
+	cx25840_ir_rx_shutdown(sd);
+	cx25840_ir_tx_shutdown(sd);
+
+	kfifo_free(&ir_state->rx_kfifo);
+	state->ir_state = NULL;
+	return 0;
+}
diff --git a/drivers/media/i2c/cx25840/cx25840-vbi.c b/drivers/media/i2c/cx25840/cx25840-vbi.c
new file mode 100644
index 000000000..a066d5f0f
--- /dev/null
+++ b/drivers/media/i2c/cx25840/cx25840-vbi.c
@@ -0,0 +1,262 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/* cx25840 VBI functions
+ */
+
+
+#include <linux/videodev2.h>
+#include <linux/i2c.h>
+#include <media/v4l2-common.h>
+#include <media/drv-intf/cx25840.h>
+
+#include "cx25840-core.h"
+
+static int odd_parity(u8 c)
+{
+	c ^= (c >> 4);
+	c ^= (c >> 2);
+	c ^= (c >> 1);
+
+	return c & 1;
+}
+
+static int decode_vps(u8 * dst, u8 * p)
+{
+	static const u8 biphase_tbl[] = {
+		0xf0, 0x78, 0x70, 0xf0, 0xb4, 0x3c, 0x34, 0xb4,
+		0xb0, 0x38, 0x30, 0xb0, 0xf0, 0x78, 0x70, 0xf0,
+		0xd2, 0x5a, 0x52, 0xd2, 0x96, 0x1e, 0x16, 0x96,
+		0x92, 0x1a, 0x12, 0x92, 0xd2, 0x5a, 0x52, 0xd2,
+		0xd0, 0x58, 0x50, 0xd0, 0x94, 0x1c, 0x14, 0x94,
+		0x90, 0x18, 0x10, 0x90, 0xd0, 0x58, 0x50, 0xd0,
+		0xf0, 0x78, 0x70, 0xf0, 0xb4, 0x3c, 0x34, 0xb4,
+		0xb0, 0x38, 0x30, 0xb0, 0xf0, 0x78, 0x70, 0xf0,
+		0xe1, 0x69, 0x61, 0xe1, 0xa5, 0x2d, 0x25, 0xa5,
+		0xa1, 0x29, 0x21, 0xa1, 0xe1, 0x69, 0x61, 0xe1,
+		0xc3, 0x4b, 0x43, 0xc3, 0x87, 0x0f, 0x07, 0x87,
+		0x83, 0x0b, 0x03, 0x83, 0xc3, 0x4b, 0x43, 0xc3,
+		0xc1, 0x49, 0x41, 0xc1, 0x85, 0x0d, 0x05, 0x85,
+		0x81, 0x09, 0x01, 0x81, 0xc1, 0x49, 0x41, 0xc1,
+		0xe1, 0x69, 0x61, 0xe1, 0xa5, 0x2d, 0x25, 0xa5,
+		0xa1, 0x29, 0x21, 0xa1, 0xe1, 0x69, 0x61, 0xe1,
+		0xe0, 0x68, 0x60, 0xe0, 0xa4, 0x2c, 0x24, 0xa4,
+		0xa0, 0x28, 0x20, 0xa0, 0xe0, 0x68, 0x60, 0xe0,
+		0xc2, 0x4a, 0x42, 0xc2, 0x86, 0x0e, 0x06, 0x86,
+		0x82, 0x0a, 0x02, 0x82, 0xc2, 0x4a, 0x42, 0xc2,
+		0xc0, 0x48, 0x40, 0xc0, 0x84, 0x0c, 0x04, 0x84,
+		0x80, 0x08, 0x00, 0x80, 0xc0, 0x48, 0x40, 0xc0,
+		0xe0, 0x68, 0x60, 0xe0, 0xa4, 0x2c, 0x24, 0xa4,
+		0xa0, 0x28, 0x20, 0xa0, 0xe0, 0x68, 0x60, 0xe0,
+		0xf0, 0x78, 0x70, 0xf0, 0xb4, 0x3c, 0x34, 0xb4,
+		0xb0, 0x38, 0x30, 0xb0, 0xf0, 0x78, 0x70, 0xf0,
+		0xd2, 0x5a, 0x52, 0xd2, 0x96, 0x1e, 0x16, 0x96,
+		0x92, 0x1a, 0x12, 0x92, 0xd2, 0x5a, 0x52, 0xd2,
+		0xd0, 0x58, 0x50, 0xd0, 0x94, 0x1c, 0x14, 0x94,
+		0x90, 0x18, 0x10, 0x90, 0xd0, 0x58, 0x50, 0xd0,
+		0xf0, 0x78, 0x70, 0xf0, 0xb4, 0x3c, 0x34, 0xb4,
+		0xb0, 0x38, 0x30, 0xb0, 0xf0, 0x78, 0x70, 0xf0,
+	};
+
+	u8 c, err = 0;
+	int i;
+
+	for (i = 0; i < 2 * 13; i += 2) {
+		err |= biphase_tbl[p[i]] | biphase_tbl[p[i + 1]];
+		c = (biphase_tbl[p[i + 1]] & 0xf) |
+		    ((biphase_tbl[p[i]] & 0xf) << 4);
+		dst[i / 2] = c;
+	}
+
+	return err & 0xf0;
+}
+
+int cx25840_g_sliced_fmt(struct v4l2_subdev *sd, struct v4l2_sliced_vbi_format *svbi)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct cx25840_state *state = to_state(sd);
+	static const u16 lcr2vbi[] = {
+		0, V4L2_SLICED_TELETEXT_B, 0,	/* 1 */
+		0, V4L2_SLICED_WSS_625, 0,	/* 4 */
+		V4L2_SLICED_CAPTION_525,	/* 6 */
+		0, 0, V4L2_SLICED_VPS, 0, 0,	/* 9 */
+		0, 0, 0, 0
+	};
+	int is_pal = !(state->std & V4L2_STD_525_60);
+	int i;
+
+	memset(svbi->service_lines, 0, sizeof(svbi->service_lines));
+	svbi->service_set = 0;
+	/* we're done if raw VBI is active */
+	/* TODO: this will have to be changed for generic_mode VBI */
+	if ((cx25840_read(client, 0x404) & 0x10) == 0)
+		return 0;
+
+	if (is_pal) {
+		for (i = 7; i <= 23; i++) {
+			u8 v = cx25840_read(client,
+				 state->vbi_regs_offset + 0x424 + i - 7);
+
+			svbi->service_lines[0][i] = lcr2vbi[v >> 4];
+			svbi->service_lines[1][i] = lcr2vbi[v & 0xf];
+			svbi->service_set |= svbi->service_lines[0][i] |
+					     svbi->service_lines[1][i];
+		}
+	} else {
+		for (i = 10; i <= 21; i++) {
+			u8 v = cx25840_read(client,
+				state->vbi_regs_offset + 0x424 + i - 10);
+
+			svbi->service_lines[0][i] = lcr2vbi[v >> 4];
+			svbi->service_lines[1][i] = lcr2vbi[v & 0xf];
+			svbi->service_set |= svbi->service_lines[0][i] |
+					     svbi->service_lines[1][i];
+		}
+	}
+	return 0;
+}
+
+int cx25840_s_raw_fmt(struct v4l2_subdev *sd, struct v4l2_vbi_format *fmt)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct cx25840_state *state = to_state(sd);
+	int is_pal = !(state->std & V4L2_STD_525_60);
+	int vbi_offset = is_pal ? 1 : 0;
+
+	/* Setup standard */
+	cx25840_std_setup(client);
+
+	/* VBI Offset */
+	if (is_cx23888(state))
+		cx25840_write(client, 0x54f, vbi_offset);
+	else
+		cx25840_write(client, 0x47f, vbi_offset);
+	/* TODO: this will have to be changed for generic_mode VBI */
+	cx25840_write(client, 0x404, 0x2e);
+	return 0;
+}
+
+int cx25840_s_sliced_fmt(struct v4l2_subdev *sd, struct v4l2_sliced_vbi_format *svbi)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct cx25840_state *state = to_state(sd);
+	int is_pal = !(state->std & V4L2_STD_525_60);
+	int vbi_offset = is_pal ? 1 : 0;
+	int i, x;
+	u8 lcr[24];
+
+	for (x = 0; x <= 23; x++)
+		lcr[x] = 0x00;
+
+	/* Setup standard */
+	cx25840_std_setup(client);
+
+	/* Sliced VBI */
+	/* TODO: this will have to be changed for generic_mode VBI */
+	cx25840_write(client, 0x404, 0x32);	/* Ancillary data */
+	cx25840_write(client, 0x406, 0x13);
+	if (is_cx23888(state))
+		cx25840_write(client, 0x54f, vbi_offset);
+	else
+		cx25840_write(client, 0x47f, vbi_offset);
+
+	if (is_pal) {
+		for (i = 0; i <= 6; i++)
+			svbi->service_lines[0][i] =
+				svbi->service_lines[1][i] = 0;
+	} else {
+		for (i = 0; i <= 9; i++)
+			svbi->service_lines[0][i] =
+				svbi->service_lines[1][i] = 0;
+
+		for (i = 22; i <= 23; i++)
+			svbi->service_lines[0][i] =
+				svbi->service_lines[1][i] = 0;
+	}
+
+	for (i = 7; i <= 23; i++) {
+		for (x = 0; x <= 1; x++) {
+			switch (svbi->service_lines[1-x][i]) {
+			case V4L2_SLICED_TELETEXT_B:
+				lcr[i] |= 1 << (4 * x);
+				break;
+			case V4L2_SLICED_WSS_625:
+				lcr[i] |= 4 << (4 * x);
+				break;
+			case V4L2_SLICED_CAPTION_525:
+				lcr[i] |= 6 << (4 * x);
+				break;
+			case V4L2_SLICED_VPS:
+				lcr[i] |= 9 << (4 * x);
+				break;
+			}
+		}
+	}
+
+	if (is_pal) {
+		for (x = 1, i = state->vbi_regs_offset + 0x424;
+		     i <= state->vbi_regs_offset + 0x434; i++, x++)
+			cx25840_write(client, i, lcr[6 + x]);
+	} else {
+		for (x = 1, i = state->vbi_regs_offset + 0x424;
+		     i <= state->vbi_regs_offset + 0x430; i++, x++)
+			cx25840_write(client, i, lcr[9 + x]);
+		for (i = state->vbi_regs_offset + 0x431;
+		     i <= state->vbi_regs_offset + 0x434; i++)
+			cx25840_write(client, i, 0);
+	}
+
+	cx25840_write(client, state->vbi_regs_offset + 0x43c, 0x16);
+	/* TODO: this will have to be changed for generic_mode VBI */
+	if (is_cx23888(state))
+		cx25840_write(client, 0x428, is_pal ? 0x2a : 0x22);
+	else
+		cx25840_write(client, 0x474, is_pal ? 0x2a : 0x22);
+	return 0;
+}
+
+int cx25840_decode_vbi_line(struct v4l2_subdev *sd, struct v4l2_decode_vbi_line *vbi)
+{
+	struct cx25840_state *state = to_state(sd);
+	u8 *p = vbi->p;
+	int id1, id2, l, err = 0;
+
+	if (p[0] || p[1] != 0xff || p[2] != 0xff ||
+			(p[3] != 0x55 && p[3] != 0x91)) {
+		vbi->line = vbi->type = 0;
+		return 0;
+	}
+
+	p += 4;
+	id1 = p[-1];
+	id2 = p[0] & 0xf;
+	l = p[2] & 0x3f;
+	l += state->vbi_line_offset;
+	p += 4;
+
+	switch (id2) {
+	case 1:
+		id2 = V4L2_SLICED_TELETEXT_B;
+		break;
+	case 4:
+		id2 = V4L2_SLICED_WSS_625;
+		break;
+	case 6:
+		id2 = V4L2_SLICED_CAPTION_525;
+		err = !odd_parity(p[0]) || !odd_parity(p[1]);
+		break;
+	case 9:
+		id2 = V4L2_SLICED_VPS;
+		if (decode_vps(p, p) != 0)
+			err = 1;
+		break;
+	default:
+		id2 = 0;
+		err = 1;
+		break;
+	}
+
+	vbi->type = err ? 0 : id2;
+	vbi->line = err ? 0 : l;
+	vbi->is_second_field = err ? 0 : (id1 == 0x55);
+	vbi->p = p;
+	return 0;
+}
diff --git a/drivers/media/i2c/ds90ub913.c b/drivers/media/i2c/ds90ub913.c
new file mode 100644
index 000000000..4bfa3b3cf
--- /dev/null
+++ b/drivers/media/i2c/ds90ub913.c
@@ -0,0 +1,903 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Driver for the Texas Instruments DS90UB913 video serializer
+ *
+ * Based on a driver from Luca Ceresoli <luca@lucaceresoli.net>
+ *
+ * Copyright (c) 2019 Luca Ceresoli <luca@lucaceresoli.net>
+ * Copyright (c) 2023 Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>
+ */
+
+#include <linux/clk-provider.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/fwnode.h>
+#include <linux/gpio/driver.h>
+#include <linux/i2c-atr.h>
+#include <linux/i2c.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/property.h>
+#include <linux/regmap.h>
+
+#include <media/i2c/ds90ub9xx.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-mediabus.h>
+#include <media/v4l2-subdev.h>
+
+#define UB913_PAD_SINK			0
+#define UB913_PAD_SOURCE		1
+
+/*
+ * UB913 has 4 gpios, but gpios 3 and 4 are reserved for external oscillator
+ * mode. Thus we only support 2 gpios for now.
+ */
+#define UB913_NUM_GPIOS			2
+
+#define UB913_REG_RESET_CTL			0x01
+#define UB913_REG_RESET_CTL_DIGITAL_RESET_1	BIT(1)
+#define UB913_REG_RESET_CTL_DIGITAL_RESET_0	BIT(0)
+
+#define UB913_REG_GENERAL_CFG			0x03
+#define UB913_REG_GENERAL_CFG_CRC_ERR_RESET	BIT(5)
+#define UB913_REG_GENERAL_CFG_PCLK_RISING	BIT(0)
+
+#define UB913_REG_MODE_SEL			0x05
+#define UB913_REG_MODE_SEL_MODE_OVERRIDE	BIT(5)
+#define UB913_REG_MODE_SEL_MODE_UP_TO_DATE	BIT(4)
+#define UB913_REG_MODE_SEL_MODE_MASK		GENMASK(3, 0)
+
+#define UB913_REG_CRC_ERRORS_LSB		0x0a
+#define UB913_REG_CRC_ERRORS_MSB		0x0b
+
+#define UB913_REG_GENERAL_STATUS		0x0c
+
+#define UB913_REG_GPIO_CFG(n)			(0x0d + (n))
+#define UB913_REG_GPIO_CFG_ENABLE(n)		BIT(0 + (n) * 4)
+#define UB913_REG_GPIO_CFG_DIR_INPUT(n)		BIT(1 + (n) * 4)
+#define UB913_REG_GPIO_CFG_REMOTE_EN(n)		BIT(2 + (n) * 4)
+#define UB913_REG_GPIO_CFG_OUT_VAL(n)		BIT(3 + (n) * 4)
+#define UB913_REG_GPIO_CFG_MASK(n)		(0xf << ((n) * 4))
+
+#define UB913_REG_SCL_HIGH_TIME			0x11
+#define UB913_REG_SCL_LOW_TIME			0x12
+
+#define UB913_REG_PLL_OVR			0x35
+
+struct ub913_data {
+	struct i2c_client	*client;
+	struct regmap		*regmap;
+	struct clk		*clkin;
+
+	struct gpio_chip	gpio_chip;
+
+	struct v4l2_subdev	sd;
+	struct media_pad	pads[2];
+
+	struct v4l2_async_notifier	notifier;
+
+	struct v4l2_subdev	*source_sd;
+	u16			source_sd_pad;
+
+	u64			enabled_source_streams;
+
+	struct clk_hw		*clkout_clk_hw;
+
+	struct ds90ub9xx_platform_data *plat_data;
+
+	bool			pclk_polarity_rising;
+};
+
+static inline struct ub913_data *sd_to_ub913(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct ub913_data, sd);
+}
+
+struct ub913_format_info {
+	u32 incode;
+	u32 outcode;
+};
+
+static const struct ub913_format_info ub913_formats[] = {
+	/* Only RAW10 with 8-bit payload is supported at the moment */
+	{ .incode = MEDIA_BUS_FMT_YUYV8_2X8, .outcode = MEDIA_BUS_FMT_YUYV8_1X16 },
+	{ .incode = MEDIA_BUS_FMT_UYVY8_2X8, .outcode = MEDIA_BUS_FMT_UYVY8_1X16 },
+	{ .incode = MEDIA_BUS_FMT_VYUY8_2X8, .outcode = MEDIA_BUS_FMT_VYUY8_1X16 },
+	{ .incode = MEDIA_BUS_FMT_YVYU8_2X8, .outcode = MEDIA_BUS_FMT_YVYU8_1X16 },
+};
+
+static const struct ub913_format_info *ub913_find_format(u32 incode)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(ub913_formats); i++) {
+		if (ub913_formats[i].incode == incode)
+			return &ub913_formats[i];
+	}
+
+	return NULL;
+}
+
+static int ub913_read(const struct ub913_data *priv, u8 reg, u8 *val)
+{
+	unsigned int v;
+	int ret;
+
+	ret = regmap_read(priv->regmap, reg, &v);
+	if (ret < 0) {
+		dev_err(&priv->client->dev,
+			"Cannot read register 0x%02x: %d!\n", reg, ret);
+		return ret;
+	}
+
+	*val = v;
+	return 0;
+}
+
+static int ub913_write(const struct ub913_data *priv, u8 reg, u8 val)
+{
+	int ret;
+
+	ret = regmap_write(priv->regmap, reg, val);
+	if (ret < 0)
+		dev_err(&priv->client->dev,
+			"Cannot write register 0x%02x: %d!\n", reg, ret);
+
+	return ret;
+}
+
+/*
+ * GPIO chip
+ */
+static int ub913_gpio_get_direction(struct gpio_chip *gc, unsigned int offset)
+{
+	return GPIO_LINE_DIRECTION_OUT;
+}
+
+static int ub913_gpio_direction_out(struct gpio_chip *gc, unsigned int offset,
+				    int value)
+{
+	struct ub913_data *priv = gpiochip_get_data(gc);
+	unsigned int reg_idx = offset / 2;
+	unsigned int field_idx = offset % 2;
+
+	return regmap_update_bits(priv->regmap, UB913_REG_GPIO_CFG(reg_idx),
+				  UB913_REG_GPIO_CFG_MASK(field_idx),
+				  UB913_REG_GPIO_CFG_ENABLE(field_idx) |
+					  (value ? UB913_REG_GPIO_CFG_OUT_VAL(field_idx) :
+						   0));
+}
+
+static void ub913_gpio_set(struct gpio_chip *gc, unsigned int offset, int value)
+{
+	ub913_gpio_direction_out(gc, offset, value);
+}
+
+static int ub913_gpio_of_xlate(struct gpio_chip *gc,
+			       const struct of_phandle_args *gpiospec,
+			       u32 *flags)
+{
+	if (flags)
+		*flags = gpiospec->args[1];
+
+	return gpiospec->args[0];
+}
+
+static int ub913_gpiochip_probe(struct ub913_data *priv)
+{
+	struct device *dev = &priv->client->dev;
+	struct gpio_chip *gc = &priv->gpio_chip;
+	int ret;
+
+	/* Initialize GPIOs 0 and 1 to local control, tri-state */
+	ub913_write(priv, UB913_REG_GPIO_CFG(0), 0);
+
+	gc->label = dev_name(dev);
+	gc->parent = dev;
+	gc->owner = THIS_MODULE;
+	gc->base = -1;
+	gc->can_sleep = true;
+	gc->ngpio = UB913_NUM_GPIOS;
+	gc->get_direction = ub913_gpio_get_direction;
+	gc->direction_output = ub913_gpio_direction_out;
+	gc->set = ub913_gpio_set;
+	gc->of_xlate = ub913_gpio_of_xlate;
+	gc->of_gpio_n_cells = 2;
+
+	ret = gpiochip_add_data(gc, priv);
+	if (ret) {
+		dev_err(dev, "Failed to add GPIOs: %d\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void ub913_gpiochip_remove(struct ub913_data *priv)
+{
+	gpiochip_remove(&priv->gpio_chip);
+}
+
+static const struct regmap_config ub913_regmap_config = {
+	.name = "ds90ub913",
+	.reg_bits = 8,
+	.val_bits = 8,
+	.reg_format_endian = REGMAP_ENDIAN_DEFAULT,
+	.val_format_endian = REGMAP_ENDIAN_DEFAULT,
+};
+
+/*
+ * V4L2
+ */
+
+static int ub913_enable_streams(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *state, u32 pad,
+				u64 streams_mask)
+{
+	struct ub913_data *priv = sd_to_ub913(sd);
+	u64 sink_streams;
+	int ret;
+
+	sink_streams = v4l2_subdev_state_xlate_streams(state, UB913_PAD_SOURCE,
+						       UB913_PAD_SINK,
+						       &streams_mask);
+
+	ret = v4l2_subdev_enable_streams(priv->source_sd, priv->source_sd_pad,
+					 sink_streams);
+	if (ret)
+		return ret;
+
+	priv->enabled_source_streams |= streams_mask;
+
+	return 0;
+}
+
+static int ub913_disable_streams(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *state, u32 pad,
+				 u64 streams_mask)
+{
+	struct ub913_data *priv = sd_to_ub913(sd);
+	u64 sink_streams;
+	int ret;
+
+	sink_streams = v4l2_subdev_state_xlate_streams(state, UB913_PAD_SOURCE,
+						       UB913_PAD_SINK,
+						       &streams_mask);
+
+	ret = v4l2_subdev_disable_streams(priv->source_sd, priv->source_sd_pad,
+					  sink_streams);
+	if (ret)
+		return ret;
+
+	priv->enabled_source_streams &= ~streams_mask;
+
+	return 0;
+}
+
+static int _ub913_set_routing(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *state,
+			      struct v4l2_subdev_krouting *routing)
+{
+	static const struct v4l2_mbus_framefmt in_format = {
+		.width = 640,
+		.height = 480,
+		.code = MEDIA_BUS_FMT_UYVY8_2X8,
+		.field = V4L2_FIELD_NONE,
+		.colorspace = V4L2_COLORSPACE_SRGB,
+		.ycbcr_enc = V4L2_YCBCR_ENC_601,
+		.quantization = V4L2_QUANTIZATION_LIM_RANGE,
+		.xfer_func = V4L2_XFER_FUNC_SRGB,
+	};
+	static const struct v4l2_mbus_framefmt out_format = {
+		.width = 640,
+		.height = 480,
+		.code = MEDIA_BUS_FMT_UYVY8_1X16,
+		.field = V4L2_FIELD_NONE,
+		.colorspace = V4L2_COLORSPACE_SRGB,
+		.ycbcr_enc = V4L2_YCBCR_ENC_601,
+		.quantization = V4L2_QUANTIZATION_LIM_RANGE,
+		.xfer_func = V4L2_XFER_FUNC_SRGB,
+	};
+	struct v4l2_subdev_stream_configs *stream_configs;
+	unsigned int i;
+	int ret;
+
+	/*
+	 * Note: we can only support up to V4L2_FRAME_DESC_ENTRY_MAX, until
+	 * frame desc is made dynamically allocated.
+	 */
+
+	if (routing->num_routes > V4L2_FRAME_DESC_ENTRY_MAX)
+		return -EINVAL;
+
+	ret = v4l2_subdev_routing_validate(sd, routing,
+					   V4L2_SUBDEV_ROUTING_ONLY_1_TO_1);
+	if (ret)
+		return ret;
+
+	ret = v4l2_subdev_set_routing(sd, state, routing);
+	if (ret)
+		return ret;
+
+	stream_configs = &state->stream_configs;
+
+	for (i = 0; i < stream_configs->num_configs; i++) {
+		if (stream_configs->configs[i].pad == UB913_PAD_SINK)
+			stream_configs->configs[i].fmt = in_format;
+		else
+			stream_configs->configs[i].fmt = out_format;
+	}
+
+	return 0;
+}
+
+static int ub913_set_routing(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *state,
+			     enum v4l2_subdev_format_whence which,
+			     struct v4l2_subdev_krouting *routing)
+{
+	struct ub913_data *priv = sd_to_ub913(sd);
+
+	if (which == V4L2_SUBDEV_FORMAT_ACTIVE && priv->enabled_source_streams)
+		return -EBUSY;
+
+	return _ub913_set_routing(sd, state, routing);
+}
+
+static int ub913_get_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
+				struct v4l2_mbus_frame_desc *fd)
+{
+	struct ub913_data *priv = sd_to_ub913(sd);
+	const struct v4l2_subdev_krouting *routing;
+	struct v4l2_mbus_frame_desc source_fd;
+	struct v4l2_subdev_route *route;
+	struct v4l2_subdev_state *state;
+	int ret;
+
+	if (pad != UB913_PAD_SOURCE)
+		return -EINVAL;
+
+	ret = v4l2_subdev_call(priv->source_sd, pad, get_frame_desc,
+			       priv->source_sd_pad, &source_fd);
+	if (ret)
+		return ret;
+
+	memset(fd, 0, sizeof(*fd));
+
+	fd->type = V4L2_MBUS_FRAME_DESC_TYPE_PARALLEL;
+
+	state = v4l2_subdev_lock_and_get_active_state(sd);
+
+	routing = &state->routing;
+
+	for_each_active_route(routing, route) {
+		unsigned int i;
+
+		if (route->source_pad != pad)
+			continue;
+
+		for (i = 0; i < source_fd.num_entries; i++) {
+			if (source_fd.entry[i].stream == route->sink_stream)
+				break;
+		}
+
+		if (i == source_fd.num_entries) {
+			dev_err(&priv->client->dev,
+				"Failed to find stream from source frame desc\n");
+			ret = -EPIPE;
+			goto out_unlock;
+		}
+
+		fd->entry[fd->num_entries].stream = route->source_stream;
+		fd->entry[fd->num_entries].flags = source_fd.entry[i].flags;
+		fd->entry[fd->num_entries].length = source_fd.entry[i].length;
+		fd->entry[fd->num_entries].pixelcode =
+			source_fd.entry[i].pixelcode;
+
+		fd->num_entries++;
+	}
+
+out_unlock:
+	v4l2_subdev_unlock_state(state);
+
+	return ret;
+}
+
+static int ub913_set_fmt(struct v4l2_subdev *sd,
+			 struct v4l2_subdev_state *state,
+			 struct v4l2_subdev_format *format)
+{
+	struct ub913_data *priv = sd_to_ub913(sd);
+	struct v4l2_mbus_framefmt *fmt;
+	const struct ub913_format_info *finfo;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE &&
+	    priv->enabled_source_streams)
+		return -EBUSY;
+
+	/* Source format is fully defined by the sink format, so not settable */
+	if (format->pad == UB913_PAD_SOURCE)
+		return v4l2_subdev_get_fmt(sd, state, format);
+
+	finfo = ub913_find_format(format->format.code);
+	if (!finfo) {
+		finfo = &ub913_formats[0];
+		format->format.code = finfo->incode;
+	}
+
+	/* Set sink format */
+	fmt = v4l2_subdev_state_get_stream_format(state, format->pad,
+						  format->stream);
+	if (!fmt)
+		return -EINVAL;
+
+	*fmt = format->format;
+
+	/* Propagate to source format, and adjust the mbus code */
+	fmt = v4l2_subdev_state_get_opposite_stream_format(state, format->pad,
+							   format->stream);
+	if (!fmt)
+		return -EINVAL;
+
+	format->format.code = finfo->outcode;
+
+	*fmt = format->format;
+
+	return 0;
+}
+
+static int ub913_init_cfg(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *state)
+{
+	struct v4l2_subdev_route routes[] = {
+		{
+			.sink_pad = UB913_PAD_SINK,
+			.sink_stream = 0,
+			.source_pad = UB913_PAD_SOURCE,
+			.source_stream = 0,
+			.flags = V4L2_SUBDEV_ROUTE_FL_ACTIVE,
+		},
+	};
+
+	struct v4l2_subdev_krouting routing = {
+		.num_routes = ARRAY_SIZE(routes),
+		.routes = routes,
+	};
+
+	return _ub913_set_routing(sd, state, &routing);
+}
+
+static int ub913_log_status(struct v4l2_subdev *sd)
+{
+	struct ub913_data *priv = sd_to_ub913(sd);
+	struct device *dev = &priv->client->dev;
+	u8 v = 0, v1 = 0, v2 = 0;
+
+	ub913_read(priv, UB913_REG_MODE_SEL, &v);
+	dev_info(dev, "MODE_SEL %#02x\n", v);
+
+	ub913_read(priv, UB913_REG_CRC_ERRORS_LSB, &v1);
+	ub913_read(priv, UB913_REG_CRC_ERRORS_MSB, &v2);
+	dev_info(dev, "CRC errors %u\n", v1 | (v2 << 8));
+
+	/* clear CRC errors */
+	ub913_read(priv, UB913_REG_GENERAL_CFG, &v);
+	ub913_write(priv, UB913_REG_GENERAL_CFG,
+		    v | UB913_REG_GENERAL_CFG_CRC_ERR_RESET);
+	ub913_write(priv, UB913_REG_GENERAL_CFG, v);
+
+	ub913_read(priv, UB913_REG_GENERAL_STATUS, &v);
+	dev_info(dev, "GENERAL_STATUS %#02x\n", v);
+
+	ub913_read(priv, UB913_REG_PLL_OVR, &v);
+	dev_info(dev, "PLL_OVR %#02x\n", v);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops ub913_subdev_core_ops = {
+	.log_status = ub913_log_status,
+};
+
+static const struct v4l2_subdev_pad_ops ub913_pad_ops = {
+	.enable_streams = ub913_enable_streams,
+	.disable_streams = ub913_disable_streams,
+	.set_routing = ub913_set_routing,
+	.get_frame_desc = ub913_get_frame_desc,
+	.get_fmt = v4l2_subdev_get_fmt,
+	.set_fmt = ub913_set_fmt,
+	.init_cfg = ub913_init_cfg,
+};
+
+static const struct v4l2_subdev_ops ub913_subdev_ops = {
+	.core = &ub913_subdev_core_ops,
+	.pad = &ub913_pad_ops,
+};
+
+static const struct media_entity_operations ub913_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static int ub913_notify_bound(struct v4l2_async_notifier *notifier,
+			      struct v4l2_subdev *source_subdev,
+			      struct v4l2_async_connection *asd)
+{
+	struct ub913_data *priv = sd_to_ub913(notifier->sd);
+	struct device *dev = &priv->client->dev;
+	int ret;
+
+	ret = media_entity_get_fwnode_pad(&source_subdev->entity,
+					  source_subdev->fwnode,
+					  MEDIA_PAD_FL_SOURCE);
+	if (ret < 0) {
+		dev_err(dev, "Failed to find pad for %s\n",
+			source_subdev->name);
+		return ret;
+	}
+
+	priv->source_sd = source_subdev;
+	priv->source_sd_pad = ret;
+
+	ret = media_create_pad_link(&source_subdev->entity, priv->source_sd_pad,
+				    &priv->sd.entity, UB913_PAD_SINK,
+				    MEDIA_LNK_FL_ENABLED |
+					    MEDIA_LNK_FL_IMMUTABLE);
+	if (ret) {
+		dev_err(dev, "Unable to link %s:%u -> %s:0\n",
+			source_subdev->name, priv->source_sd_pad,
+			priv->sd.name);
+		return ret;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_async_notifier_operations ub913_notify_ops = {
+	.bound = ub913_notify_bound,
+};
+
+static int ub913_v4l2_notifier_register(struct ub913_data *priv)
+{
+	struct device *dev = &priv->client->dev;
+	struct v4l2_async_connection *asd;
+	struct fwnode_handle *ep_fwnode;
+	int ret;
+
+	ep_fwnode = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev),
+						    UB913_PAD_SINK, 0, 0);
+	if (!ep_fwnode) {
+		dev_err(dev, "No graph endpoint\n");
+		return -ENODEV;
+	}
+
+	v4l2_async_subdev_nf_init(&priv->notifier, &priv->sd);
+
+	asd = v4l2_async_nf_add_fwnode_remote(&priv->notifier, ep_fwnode,
+					      struct v4l2_async_connection);
+
+	fwnode_handle_put(ep_fwnode);
+
+	if (IS_ERR(asd)) {
+		dev_err(dev, "Failed to add subdev: %ld", PTR_ERR(asd));
+		v4l2_async_nf_cleanup(&priv->notifier);
+		return PTR_ERR(asd);
+	}
+
+	priv->notifier.ops = &ub913_notify_ops;
+
+	ret = v4l2_async_nf_register(&priv->notifier);
+	if (ret) {
+		dev_err(dev, "Failed to register subdev_notifier");
+		v4l2_async_nf_cleanup(&priv->notifier);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void ub913_v4l2_nf_unregister(struct ub913_data *priv)
+{
+	v4l2_async_nf_unregister(&priv->notifier);
+	v4l2_async_nf_cleanup(&priv->notifier);
+}
+
+static int ub913_register_clkout(struct ub913_data *priv)
+{
+	struct device *dev = &priv->client->dev;
+	const char *name;
+	int ret;
+
+	name = kasprintf(GFP_KERNEL, "ds90ub913.%s.clk_out", dev_name(dev));
+	if (!name)
+		return -ENOMEM;
+
+	priv->clkout_clk_hw = devm_clk_hw_register_fixed_factor(dev, name,
+		__clk_get_name(priv->clkin), 0, 1, 2);
+
+	kfree(name);
+
+	if (IS_ERR(priv->clkout_clk_hw))
+		return dev_err_probe(dev, PTR_ERR(priv->clkout_clk_hw),
+				     "Cannot register clkout hw\n");
+
+	ret = devm_of_clk_add_hw_provider(dev, of_clk_hw_simple_get,
+					  priv->clkout_clk_hw);
+	if (ret)
+		return dev_err_probe(dev, ret,
+				     "Cannot add OF clock provider\n");
+
+	return 0;
+}
+
+static int ub913_i2c_master_init(struct ub913_data *priv)
+{
+	/* i2c fast mode */
+	u32 scl_high = 600 + 300; /* high period + rise time, ns */
+	u32 scl_low = 1300 + 300; /* low period + fall time, ns */
+	unsigned long ref;
+	int ret;
+
+	ref = clk_get_rate(priv->clkin) / 2;
+
+	scl_high = div64_u64((u64)scl_high * ref, 1000000000);
+	scl_low = div64_u64((u64)scl_low * ref, 1000000000);
+
+	ret = ub913_write(priv, UB913_REG_SCL_HIGH_TIME, scl_high);
+	if (ret)
+		return ret;
+
+	ret = ub913_write(priv, UB913_REG_SCL_LOW_TIME, scl_low);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int ub913_add_i2c_adapter(struct ub913_data *priv)
+{
+	struct device *dev = &priv->client->dev;
+	struct fwnode_handle *i2c_handle;
+	int ret;
+
+	i2c_handle = device_get_named_child_node(dev, "i2c");
+	if (!i2c_handle)
+		return 0;
+
+	ret = i2c_atr_add_adapter(priv->plat_data->atr, priv->plat_data->port,
+				  dev, i2c_handle);
+
+	fwnode_handle_put(i2c_handle);
+
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int ub913_parse_dt(struct ub913_data *priv)
+{
+	struct device *dev = &priv->client->dev;
+	struct v4l2_fwnode_endpoint vep = {
+		.bus_type = V4L2_MBUS_PARALLEL,
+	};
+	struct fwnode_handle *ep_fwnode;
+	int ret;
+
+	ep_fwnode = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev),
+						    UB913_PAD_SINK, 0, 0);
+	if (!ep_fwnode)
+		return dev_err_probe(dev, -ENOENT, "No sink endpoint\n");
+
+	ret = v4l2_fwnode_endpoint_parse(ep_fwnode, &vep);
+
+	fwnode_handle_put(ep_fwnode);
+
+	if (ret)
+		return dev_err_probe(dev, ret,
+				     "failed to parse sink endpoint data\n");
+
+	if (vep.bus.parallel.flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
+		priv->pclk_polarity_rising = true;
+	else if (vep.bus.parallel.flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
+		priv->pclk_polarity_rising = false;
+	else
+		return dev_err_probe(dev, -EINVAL,
+				     "bad value for 'pclk-sample'\n");
+
+	return 0;
+}
+
+static int ub913_hw_init(struct ub913_data *priv)
+{
+	struct device *dev = &priv->client->dev;
+	bool mode_override;
+	u8 mode;
+	int ret;
+	u8 v;
+
+	ret = ub913_read(priv, UB913_REG_MODE_SEL, &v);
+	if (ret)
+		return ret;
+
+	if (!(v & UB913_REG_MODE_SEL_MODE_UP_TO_DATE))
+		return dev_err_probe(dev, -ENODEV,
+				     "Mode value not stabilized\n");
+
+	mode_override = v & UB913_REG_MODE_SEL_MODE_OVERRIDE;
+	mode = v & UB913_REG_MODE_SEL_MODE_MASK;
+
+	dev_dbg(dev, "mode from %s: %#x\n",
+		mode_override ? "reg" : "deserializer", mode);
+
+	ret = ub913_i2c_master_init(priv);
+	if (ret)
+		return dev_err_probe(dev, ret, "i2c master init failed\n");
+
+	ub913_read(priv, UB913_REG_GENERAL_CFG, &v);
+	v &= ~UB913_REG_GENERAL_CFG_PCLK_RISING;
+	v |= priv->pclk_polarity_rising ? UB913_REG_GENERAL_CFG_PCLK_RISING : 0;
+	ub913_write(priv, UB913_REG_GENERAL_CFG, v);
+
+	return 0;
+}
+
+static int ub913_subdev_init(struct ub913_data *priv)
+{
+	struct device *dev = &priv->client->dev;
+	int ret;
+
+	v4l2_i2c_subdev_init(&priv->sd, priv->client, &ub913_subdev_ops);
+	priv->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_STREAMS;
+	priv->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
+	priv->sd.entity.ops = &ub913_entity_ops;
+
+	priv->pads[0].flags = MEDIA_PAD_FL_SINK;
+	priv->pads[1].flags = MEDIA_PAD_FL_SOURCE;
+
+	ret = media_entity_pads_init(&priv->sd.entity, 2, priv->pads);
+	if (ret)
+		return dev_err_probe(dev, ret, "Failed to init pads\n");
+
+	ret = v4l2_subdev_init_finalize(&priv->sd);
+	if (ret)
+		goto err_entity_cleanup;
+
+	ret = ub913_v4l2_notifier_register(priv);
+	if (ret) {
+		dev_err_probe(dev, ret,
+			      "v4l2 subdev notifier register failed\n");
+		goto err_subdev_cleanup;
+	}
+
+	ret = v4l2_async_register_subdev(&priv->sd);
+	if (ret) {
+		dev_err_probe(dev, ret, "v4l2_async_register_subdev error\n");
+		goto err_unreg_notif;
+	}
+
+	return 0;
+
+err_unreg_notif:
+	ub913_v4l2_nf_unregister(priv);
+err_subdev_cleanup:
+	v4l2_subdev_cleanup(&priv->sd);
+err_entity_cleanup:
+	media_entity_cleanup(&priv->sd.entity);
+
+	return ret;
+}
+
+static void ub913_subdev_uninit(struct ub913_data *priv)
+{
+	v4l2_async_unregister_subdev(&priv->sd);
+	ub913_v4l2_nf_unregister(priv);
+	v4l2_subdev_cleanup(&priv->sd);
+	fwnode_handle_put(priv->sd.fwnode);
+	media_entity_cleanup(&priv->sd.entity);
+}
+
+static int ub913_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct ub913_data *priv;
+	int ret;
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->client = client;
+
+	priv->plat_data = dev_get_platdata(&client->dev);
+	if (!priv->plat_data)
+		return dev_err_probe(dev, -ENODEV, "Platform data missing\n");
+
+	priv->regmap = devm_regmap_init_i2c(client, &ub913_regmap_config);
+	if (IS_ERR(priv->regmap))
+		return dev_err_probe(dev, PTR_ERR(priv->regmap),
+				     "Failed to init regmap\n");
+
+	/*
+	 * ub913 can also work without ext clock, but that is not supported by
+	 * the driver yet.
+	 */
+	priv->clkin = devm_clk_get(dev, "clkin");
+	if (IS_ERR(priv->clkin))
+		return dev_err_probe(dev, PTR_ERR(priv->clkin),
+				     "Cannot get CLKIN\n");
+
+	ret = ub913_parse_dt(priv);
+	if (ret)
+		return ret;
+
+	ret = ub913_hw_init(priv);
+	if (ret)
+		return ret;
+
+	ret = ub913_gpiochip_probe(priv);
+	if (ret)
+		return dev_err_probe(dev, ret, "Failed to init gpiochip\n");
+
+	ret = ub913_register_clkout(priv);
+	if (ret) {
+		dev_err_probe(dev, ret, "Failed to register clkout\n");
+		goto err_gpiochip_remove;
+	}
+
+	ret = ub913_subdev_init(priv);
+	if (ret)
+		goto err_gpiochip_remove;
+
+	ret = ub913_add_i2c_adapter(priv);
+	if (ret) {
+		dev_err_probe(dev, ret, "failed to add remote i2c adapter\n");
+		goto err_subdev_uninit;
+	}
+
+	return 0;
+
+err_subdev_uninit:
+	ub913_subdev_uninit(priv);
+err_gpiochip_remove:
+	ub913_gpiochip_remove(priv);
+
+	return ret;
+}
+
+static void ub913_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ub913_data *priv = sd_to_ub913(sd);
+
+	i2c_atr_del_adapter(priv->plat_data->atr, priv->plat_data->port);
+
+	ub913_subdev_uninit(priv);
+
+	ub913_gpiochip_remove(priv);
+}
+
+static const struct i2c_device_id ub913_id[] = { { "ds90ub913a-q1", 0 }, {} };
+MODULE_DEVICE_TABLE(i2c, ub913_id);
+
+static const struct of_device_id ub913_dt_ids[] = {
+	{ .compatible = "ti,ds90ub913a-q1" },
+	{}
+};
+MODULE_DEVICE_TABLE(of, ub913_dt_ids);
+
+static struct i2c_driver ds90ub913_driver = {
+	.probe		= ub913_probe,
+	.remove		= ub913_remove,
+	.id_table	= ub913_id,
+	.driver = {
+		.name	= "ds90ub913a",
+		.of_match_table = ub913_dt_ids,
+	},
+};
+module_i2c_driver(ds90ub913_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Texas Instruments DS90UB913 FPD-Link III Serializer Driver");
+MODULE_AUTHOR("Luca Ceresoli <luca@lucaceresoli.net>");
+MODULE_AUTHOR("Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>");
+MODULE_IMPORT_NS(I2C_ATR);
diff --git a/drivers/media/i2c/ds90ub953.c b/drivers/media/i2c/ds90ub953.c
new file mode 100644
index 000000000..dc394e22a
--- /dev/null
+++ b/drivers/media/i2c/ds90ub953.c
@@ -0,0 +1,1430 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Driver for the Texas Instruments DS90UB953 video serializer
+ *
+ * Based on a driver from Luca Ceresoli <luca@lucaceresoli.net>
+ *
+ * Copyright (c) 2019 Luca Ceresoli <luca@lucaceresoli.net>
+ * Copyright (c) 2023 Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>
+ */
+
+#include <linux/clk-provider.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/fwnode.h>
+#include <linux/gpio/driver.h>
+#include <linux/i2c-atr.h>
+#include <linux/i2c.h>
+#include <linux/kernel.h>
+#include <linux/math64.h>
+#include <linux/module.h>
+#include <linux/property.h>
+#include <linux/rational.h>
+#include <linux/regmap.h>
+
+#include <media/i2c/ds90ub9xx.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-mediabus.h>
+#include <media/v4l2-subdev.h>
+
+#define UB953_PAD_SINK			0
+#define UB953_PAD_SOURCE		1
+
+#define UB953_NUM_GPIOS			4
+
+#define UB953_DEFAULT_CLKOUT_RATE	25000000UL
+
+#define UB953_REG_RESET_CTL			0x01
+#define UB953_REG_RESET_CTL_DIGITAL_RESET_1	BIT(1)
+#define UB953_REG_RESET_CTL_DIGITAL_RESET_0	BIT(0)
+
+#define UB953_REG_GENERAL_CFG			0x02
+#define UB953_REG_GENERAL_CFG_CONT_CLK		BIT(6)
+#define UB953_REG_GENERAL_CFG_CSI_LANE_SEL_SHIFT	4
+#define UB953_REG_GENERAL_CFG_CSI_LANE_SEL_MASK	GENMASK(5, 4)
+#define UB953_REG_GENERAL_CFG_CRC_TX_GEN_ENABLE	BIT(1)
+#define UB953_REG_GENERAL_CFG_I2C_STRAP_MODE	BIT(0)
+
+#define UB953_REG_MODE_SEL			0x03
+#define UB953_REG_MODE_SEL_MODE_DONE		BIT(3)
+#define UB953_REG_MODE_SEL_MODE_OVERRIDE	BIT(4)
+#define UB953_REG_MODE_SEL_MODE_MASK		GENMASK(2, 0)
+
+#define UB953_REG_CLKOUT_CTRL0			0x06
+#define UB953_REG_CLKOUT_CTRL1			0x07
+
+#define UB953_REG_SCL_HIGH_TIME			0x0b
+#define UB953_REG_SCL_LOW_TIME			0x0c
+
+#define UB953_REG_LOCAL_GPIO_DATA		0x0d
+#define UB953_REG_LOCAL_GPIO_DATA_GPIO_RMTEN(n)		BIT(4 + (n))
+#define UB953_REG_LOCAL_GPIO_DATA_GPIO_OUT_SRC(n)	BIT(0 + (n))
+
+#define UB953_REG_GPIO_INPUT_CTRL		0x0e
+#define UB953_REG_GPIO_INPUT_CTRL_OUT_EN(n)	BIT(4 + (n))
+#define UB953_REG_GPIO_INPUT_CTRL_INPUT_EN(n)	BIT(0 + (n))
+
+#define UB953_REG_REV_MASK_ID			0x50
+#define UB953_REG_GENERAL_STATUS		0x52
+
+#define UB953_REG_GPIO_PIN_STS			0x53
+#define UB953_REG_GPIO_PIN_STS_GPIO_STS(n)	BIT(0 + (n))
+
+#define UB953_REG_BIST_ERR_CNT			0x54
+#define UB953_REG_CRC_ERR_CNT1			0x55
+#define UB953_REG_CRC_ERR_CNT2			0x56
+
+#define UB953_REG_CSI_ERR_CNT			0x5c
+#define UB953_REG_CSI_ERR_STATUS		0x5d
+#define UB953_REG_CSI_ERR_DLANE01		0x5e
+#define UB953_REG_CSI_ERR_DLANE23		0x5f
+#define UB953_REG_CSI_ERR_CLK_LANE		0x60
+#define UB953_REG_CSI_PKT_HDR_VC_ID		0x61
+#define UB953_REG_PKT_HDR_WC_LSB		0x62
+#define UB953_REG_PKT_HDR_WC_MSB		0x63
+#define UB953_REG_CSI_ECC			0x64
+
+#define UB953_REG_IND_ACC_CTL			0xb0
+#define UB953_REG_IND_ACC_ADDR			0xb1
+#define UB953_REG_IND_ACC_DATA			0xb2
+
+#define UB953_REG_FPD3_RX_ID(n)			(0xf0 + (n))
+#define UB953_REG_FPD3_RX_ID_LEN		6
+
+/* Indirect register blocks */
+#define UB953_IND_TARGET_PAT_GEN		0x00
+#define UB953_IND_TARGET_FPD3_TX		0x01
+#define UB953_IND_TARGET_DIE_ID			0x02
+
+#define UB953_IND_PGEN_CTL			0x01
+#define UB953_IND_PGEN_CTL_PGEN_ENABLE		BIT(0)
+#define UB953_IND_PGEN_CFG			0x02
+#define UB953_IND_PGEN_CSI_DI			0x03
+#define UB953_IND_PGEN_LINE_SIZE1		0x04
+#define UB953_IND_PGEN_LINE_SIZE0		0x05
+#define UB953_IND_PGEN_BAR_SIZE1		0x06
+#define UB953_IND_PGEN_BAR_SIZE0		0x07
+#define UB953_IND_PGEN_ACT_LPF1			0x08
+#define UB953_IND_PGEN_ACT_LPF0			0x09
+#define UB953_IND_PGEN_TOT_LPF1			0x0a
+#define UB953_IND_PGEN_TOT_LPF0			0x0b
+#define UB953_IND_PGEN_LINE_PD1			0x0c
+#define UB953_IND_PGEN_LINE_PD0			0x0d
+#define UB953_IND_PGEN_VBP			0x0e
+#define UB953_IND_PGEN_VFP			0x0f
+#define UB953_IND_PGEN_COLOR(n)			(0x10 + (n)) /* n <= 15 */
+
+/* Note: Only sync mode supported for now */
+enum ub953_mode {
+	/* FPD-Link III CSI-2 synchronous mode */
+	UB953_MODE_SYNC,
+	/* FPD-Link III CSI-2 non-synchronous mode, external ref clock */
+	UB953_MODE_NONSYNC_EXT,
+	/* FPD-Link III CSI-2 non-synchronous mode, internal ref clock */
+	UB953_MODE_NONSYNC_INT,
+	/* FPD-Link III DVP mode */
+	UB953_MODE_DVP,
+};
+
+struct ub953_hw_data {
+	const char *model;
+	bool is_ub971;
+};
+
+struct ub953_clkout_data {
+	u32 hs_div;
+	u32 m;
+	u32 n;
+	unsigned long rate;
+};
+
+struct ub953_data {
+	const struct ub953_hw_data	*hw_data;
+
+	struct i2c_client	*client;
+	struct regmap		*regmap;
+	struct clk		*clkin;
+
+	u32			num_data_lanes;
+	bool			non_continous_clk;
+
+	struct gpio_chip	gpio_chip;
+
+	struct v4l2_subdev	sd;
+	struct media_pad	pads[2];
+
+	struct v4l2_async_notifier	notifier;
+
+	struct v4l2_subdev	*source_sd;
+	u16			source_sd_pad;
+
+	u64			enabled_source_streams;
+
+	/* lock for register access */
+	struct mutex		reg_lock;
+
+	u8			current_indirect_target;
+
+	struct clk_hw		clkout_clk_hw;
+
+	enum ub953_mode		mode;
+
+	const struct ds90ub9xx_platform_data	*plat_data;
+};
+
+static inline struct ub953_data *sd_to_ub953(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct ub953_data, sd);
+}
+
+/*
+ * HW Access
+ */
+
+static int ub953_read(struct ub953_data *priv, u8 reg, u8 *val)
+{
+	unsigned int v;
+	int ret;
+
+	mutex_lock(&priv->reg_lock);
+
+	ret = regmap_read(priv->regmap, reg, &v);
+	if (ret) {
+		dev_err(&priv->client->dev, "Cannot read register 0x%02x: %d\n",
+			reg, ret);
+		goto out_unlock;
+	}
+
+	*val = v;
+
+out_unlock:
+	mutex_unlock(&priv->reg_lock);
+
+	return ret;
+}
+
+static int ub953_write(struct ub953_data *priv, u8 reg, u8 val)
+{
+	int ret;
+
+	mutex_lock(&priv->reg_lock);
+
+	ret = regmap_write(priv->regmap, reg, val);
+	if (ret)
+		dev_err(&priv->client->dev,
+			"Cannot write register 0x%02x: %d\n", reg, ret);
+
+	mutex_unlock(&priv->reg_lock);
+
+	return ret;
+}
+
+static int ub953_select_ind_reg_block(struct ub953_data *priv, u8 block)
+{
+	struct device *dev = &priv->client->dev;
+	int ret;
+
+	if (priv->current_indirect_target == block)
+		return 0;
+
+	ret = regmap_write(priv->regmap, UB953_REG_IND_ACC_CTL, block << 2);
+	if (ret) {
+		dev_err(dev, "%s: cannot select indirect target %u (%d)\n",
+			__func__, block, ret);
+		return ret;
+	}
+
+	priv->current_indirect_target = block;
+
+	return 0;
+}
+
+__maybe_unused
+static int ub953_read_ind(struct ub953_data *priv, u8 block, u8 reg, u8 *val)
+{
+	unsigned int v;
+	int ret;
+
+	mutex_lock(&priv->reg_lock);
+
+	ret = ub953_select_ind_reg_block(priv, block);
+	if (ret)
+		goto out_unlock;
+
+	ret = regmap_write(priv->regmap, UB953_REG_IND_ACC_ADDR, reg);
+	if (ret) {
+		dev_err(&priv->client->dev,
+			"Write to IND_ACC_ADDR failed when reading %u:%x02x: %d\n",
+			block, reg, ret);
+		goto out_unlock;
+	}
+
+	ret = regmap_read(priv->regmap, UB953_REG_IND_ACC_DATA, &v);
+	if (ret) {
+		dev_err(&priv->client->dev,
+			"Write to IND_ACC_DATA failed when reading %u:%x02x: %d\n",
+			block, reg, ret);
+		goto out_unlock;
+	}
+
+	*val = v;
+
+out_unlock:
+	mutex_unlock(&priv->reg_lock);
+
+	return ret;
+}
+
+__maybe_unused
+static int ub953_write_ind(struct ub953_data *priv, u8 block, u8 reg, u8 val)
+{
+	int ret;
+
+	mutex_lock(&priv->reg_lock);
+
+	ret = ub953_select_ind_reg_block(priv, block);
+	if (ret)
+		goto out_unlock;
+
+	ret = regmap_write(priv->regmap, UB953_REG_IND_ACC_ADDR, reg);
+	if (ret) {
+		dev_err(&priv->client->dev,
+			"Write to IND_ACC_ADDR failed when writing %u:%x02x: %d\n",
+			block, reg, ret);
+		goto out_unlock;
+	}
+
+	ret = regmap_write(priv->regmap, UB953_REG_IND_ACC_DATA, val);
+	if (ret) {
+		dev_err(&priv->client->dev,
+			"Write to IND_ACC_DATA failed when writing %u:%x02x\n: %d\n",
+			block, reg, ret);
+	}
+
+out_unlock:
+	mutex_unlock(&priv->reg_lock);
+
+	return ret;
+}
+
+/*
+ * GPIO chip
+ */
+static int ub953_gpio_get_direction(struct gpio_chip *gc, unsigned int offset)
+{
+	struct ub953_data *priv = gpiochip_get_data(gc);
+	int ret;
+	u8 v;
+
+	ret = ub953_read(priv, UB953_REG_GPIO_INPUT_CTRL, &v);
+	if (ret)
+		return ret;
+
+	if (v & UB953_REG_GPIO_INPUT_CTRL_INPUT_EN(offset))
+		return GPIO_LINE_DIRECTION_IN;
+	else
+		return GPIO_LINE_DIRECTION_OUT;
+}
+
+static int ub953_gpio_direction_in(struct gpio_chip *gc, unsigned int offset)
+{
+	struct ub953_data *priv = gpiochip_get_data(gc);
+
+	return regmap_update_bits(priv->regmap, UB953_REG_GPIO_INPUT_CTRL,
+				  UB953_REG_GPIO_INPUT_CTRL_INPUT_EN(offset) |
+					  UB953_REG_GPIO_INPUT_CTRL_OUT_EN(offset),
+				  UB953_REG_GPIO_INPUT_CTRL_INPUT_EN(offset));
+}
+
+static int ub953_gpio_direction_out(struct gpio_chip *gc, unsigned int offset,
+				    int value)
+{
+	struct ub953_data *priv = gpiochip_get_data(gc);
+	int ret;
+
+	ret = regmap_update_bits(priv->regmap, UB953_REG_LOCAL_GPIO_DATA,
+				 UB953_REG_LOCAL_GPIO_DATA_GPIO_OUT_SRC(offset),
+				 value ? UB953_REG_LOCAL_GPIO_DATA_GPIO_OUT_SRC(offset) :
+					 0);
+
+	if (ret)
+		return ret;
+
+	return regmap_update_bits(priv->regmap, UB953_REG_GPIO_INPUT_CTRL,
+				  UB953_REG_GPIO_INPUT_CTRL_INPUT_EN(offset) |
+					  UB953_REG_GPIO_INPUT_CTRL_OUT_EN(offset),
+				  UB953_REG_GPIO_INPUT_CTRL_OUT_EN(offset));
+}
+
+static int ub953_gpio_get(struct gpio_chip *gc, unsigned int offset)
+{
+	struct ub953_data *priv = gpiochip_get_data(gc);
+	int ret;
+	u8 v;
+
+	ret = ub953_read(priv, UB953_REG_GPIO_PIN_STS, &v);
+	if (ret)
+		return ret;
+
+	return !!(v & UB953_REG_GPIO_PIN_STS_GPIO_STS(offset));
+}
+
+static void ub953_gpio_set(struct gpio_chip *gc, unsigned int offset, int value)
+{
+	struct ub953_data *priv = gpiochip_get_data(gc);
+
+	regmap_update_bits(priv->regmap, UB953_REG_LOCAL_GPIO_DATA,
+			   UB953_REG_LOCAL_GPIO_DATA_GPIO_OUT_SRC(offset),
+			   value ? UB953_REG_LOCAL_GPIO_DATA_GPIO_OUT_SRC(offset) :
+				   0);
+}
+
+static int ub953_gpio_of_xlate(struct gpio_chip *gc,
+			       const struct of_phandle_args *gpiospec,
+			       u32 *flags)
+{
+	if (flags)
+		*flags = gpiospec->args[1];
+
+	return gpiospec->args[0];
+}
+
+static int ub953_gpiochip_probe(struct ub953_data *priv)
+{
+	struct device *dev = &priv->client->dev;
+	struct gpio_chip *gc = &priv->gpio_chip;
+	int ret;
+
+	/* Set all GPIOs to local input mode */
+	ub953_write(priv, UB953_REG_LOCAL_GPIO_DATA, 0);
+	ub953_write(priv, UB953_REG_GPIO_INPUT_CTRL, 0xf);
+
+	gc->label = dev_name(dev);
+	gc->parent = dev;
+	gc->owner = THIS_MODULE;
+	gc->base = -1;
+	gc->can_sleep = true;
+	gc->ngpio = UB953_NUM_GPIOS;
+	gc->get_direction = ub953_gpio_get_direction;
+	gc->direction_input = ub953_gpio_direction_in;
+	gc->direction_output = ub953_gpio_direction_out;
+	gc->get = ub953_gpio_get;
+	gc->set = ub953_gpio_set;
+	gc->of_xlate = ub953_gpio_of_xlate;
+	gc->of_gpio_n_cells = 2;
+
+	ret = gpiochip_add_data(gc, priv);
+	if (ret) {
+		dev_err(dev, "Failed to add GPIOs: %d\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void ub953_gpiochip_remove(struct ub953_data *priv)
+{
+	gpiochip_remove(&priv->gpio_chip);
+}
+
+/*
+ * V4L2
+ */
+
+static int _ub953_set_routing(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *state,
+			      struct v4l2_subdev_krouting *routing)
+{
+	static const struct v4l2_mbus_framefmt format = {
+		.width = 640,
+		.height = 480,
+		.code = MEDIA_BUS_FMT_UYVY8_1X16,
+		.field = V4L2_FIELD_NONE,
+		.colorspace = V4L2_COLORSPACE_SRGB,
+		.ycbcr_enc = V4L2_YCBCR_ENC_601,
+		.quantization = V4L2_QUANTIZATION_LIM_RANGE,
+		.xfer_func = V4L2_XFER_FUNC_SRGB,
+	};
+	int ret;
+
+	/*
+	 * Note: we can only support up to V4L2_FRAME_DESC_ENTRY_MAX, until
+	 * frame desc is made dynamically allocated.
+	 */
+
+	if (routing->num_routes > V4L2_FRAME_DESC_ENTRY_MAX)
+		return -EINVAL;
+
+	ret = v4l2_subdev_routing_validate(sd, routing,
+					   V4L2_SUBDEV_ROUTING_ONLY_1_TO_1);
+	if (ret)
+		return ret;
+
+	ret = v4l2_subdev_set_routing_with_fmt(sd, state, routing, &format);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int ub953_set_routing(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *state,
+			     enum v4l2_subdev_format_whence which,
+			     struct v4l2_subdev_krouting *routing)
+{
+	struct ub953_data *priv = sd_to_ub953(sd);
+
+	if (which == V4L2_SUBDEV_FORMAT_ACTIVE && priv->enabled_source_streams)
+		return -EBUSY;
+
+	return _ub953_set_routing(sd, state, routing);
+}
+
+static int ub953_get_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
+				struct v4l2_mbus_frame_desc *fd)
+{
+	struct ub953_data *priv = sd_to_ub953(sd);
+	struct v4l2_mbus_frame_desc source_fd;
+	struct v4l2_subdev_route *route;
+	struct v4l2_subdev_state *state;
+	int ret;
+
+	if (pad != UB953_PAD_SOURCE)
+		return -EINVAL;
+
+	ret = v4l2_subdev_call(priv->source_sd, pad, get_frame_desc,
+			       priv->source_sd_pad, &source_fd);
+	if (ret)
+		return ret;
+
+	memset(fd, 0, sizeof(*fd));
+
+	fd->type = V4L2_MBUS_FRAME_DESC_TYPE_CSI2;
+
+	state = v4l2_subdev_lock_and_get_active_state(sd);
+
+	for_each_active_route(&state->routing, route) {
+		struct v4l2_mbus_frame_desc_entry *source_entry = NULL;
+		unsigned int i;
+
+		if (route->source_pad != pad)
+			continue;
+
+		for (i = 0; i < source_fd.num_entries; i++) {
+			if (source_fd.entry[i].stream == route->sink_stream) {
+				source_entry = &source_fd.entry[i];
+				break;
+			}
+		}
+
+		if (!source_entry) {
+			dev_err(&priv->client->dev,
+				"Failed to find stream from source frame desc\n");
+			ret = -EPIPE;
+			goto out_unlock;
+		}
+
+		fd->entry[fd->num_entries].stream = route->source_stream;
+		fd->entry[fd->num_entries].flags = source_entry->flags;
+		fd->entry[fd->num_entries].length = source_entry->length;
+		fd->entry[fd->num_entries].pixelcode = source_entry->pixelcode;
+		fd->entry[fd->num_entries].bus.csi2.vc =
+			source_entry->bus.csi2.vc;
+		fd->entry[fd->num_entries].bus.csi2.dt =
+			source_entry->bus.csi2.dt;
+
+		fd->num_entries++;
+	}
+
+out_unlock:
+	v4l2_subdev_unlock_state(state);
+
+	return ret;
+}
+
+static int ub953_set_fmt(struct v4l2_subdev *sd,
+			 struct v4l2_subdev_state *state,
+			 struct v4l2_subdev_format *format)
+{
+	struct ub953_data *priv = sd_to_ub953(sd);
+	struct v4l2_mbus_framefmt *fmt;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE &&
+	    priv->enabled_source_streams)
+		return -EBUSY;
+
+	/* No transcoding, source and sink formats must match. */
+	if (format->pad == UB953_PAD_SOURCE)
+		return v4l2_subdev_get_fmt(sd, state, format);
+
+	/* Set sink format */
+	fmt = v4l2_subdev_state_get_stream_format(state, format->pad,
+						  format->stream);
+	if (!fmt)
+		return -EINVAL;
+
+	*fmt = format->format;
+
+	/* Propagate to source format */
+	fmt = v4l2_subdev_state_get_opposite_stream_format(state, format->pad,
+							   format->stream);
+	if (!fmt)
+		return -EINVAL;
+
+	*fmt = format->format;
+
+	return 0;
+}
+
+static int ub953_init_cfg(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *state)
+{
+	struct v4l2_subdev_route routes[] = {
+		{
+			.sink_pad = UB953_PAD_SINK,
+			.sink_stream = 0,
+			.source_pad = UB953_PAD_SOURCE,
+			.source_stream = 0,
+			.flags = V4L2_SUBDEV_ROUTE_FL_ACTIVE,
+		},
+	};
+
+	struct v4l2_subdev_krouting routing = {
+		.num_routes = ARRAY_SIZE(routes),
+		.routes = routes,
+	};
+
+	return _ub953_set_routing(sd, state, &routing);
+}
+
+static int ub953_log_status(struct v4l2_subdev *sd)
+{
+	struct ub953_data *priv = sd_to_ub953(sd);
+	struct device *dev = &priv->client->dev;
+	u8 v = 0, v1 = 0, v2 = 0;
+	unsigned int i;
+	char id[UB953_REG_FPD3_RX_ID_LEN];
+	u8 gpio_local_data = 0;
+	u8 gpio_input_ctrl = 0;
+	u8 gpio_pin_sts = 0;
+
+	for (i = 0; i < sizeof(id); i++)
+		ub953_read(priv, UB953_REG_FPD3_RX_ID(i), &id[i]);
+
+	dev_info(dev, "ID '%.*s'\n", (int)sizeof(id), id);
+
+	ub953_read(priv, UB953_REG_GENERAL_STATUS, &v);
+	dev_info(dev, "GENERAL_STATUS %#02x\n", v);
+
+	ub953_read(priv, UB953_REG_CRC_ERR_CNT1, &v1);
+	ub953_read(priv, UB953_REG_CRC_ERR_CNT2, &v2);
+	dev_info(dev, "CRC error count %u\n", v1 | (v2 << 8));
+
+	ub953_read(priv, UB953_REG_CSI_ERR_CNT, &v);
+	dev_info(dev, "CSI error count %u\n", v);
+
+	ub953_read(priv, UB953_REG_CSI_ERR_STATUS, &v);
+	dev_info(dev, "CSI_ERR_STATUS %#02x\n", v);
+
+	ub953_read(priv, UB953_REG_CSI_ERR_DLANE01, &v);
+	dev_info(dev, "CSI_ERR_DLANE01 %#02x\n", v);
+
+	ub953_read(priv, UB953_REG_CSI_ERR_DLANE23, &v);
+	dev_info(dev, "CSI_ERR_DLANE23 %#02x\n", v);
+
+	ub953_read(priv, UB953_REG_CSI_ERR_CLK_LANE, &v);
+	dev_info(dev, "CSI_ERR_CLK_LANE %#02x\n", v);
+
+	ub953_read(priv, UB953_REG_CSI_PKT_HDR_VC_ID, &v);
+	dev_info(dev, "CSI packet header VC %u ID %u\n", v >> 6, v & 0x3f);
+
+	ub953_read(priv, UB953_REG_PKT_HDR_WC_LSB, &v1);
+	ub953_read(priv, UB953_REG_PKT_HDR_WC_MSB, &v2);
+	dev_info(dev, "CSI packet header WC %u\n", (v2 << 8) | v1);
+
+	ub953_read(priv, UB953_REG_CSI_ECC, &v);
+	dev_info(dev, "CSI ECC %#02x\n", v);
+
+	ub953_read(priv, UB953_REG_LOCAL_GPIO_DATA, &gpio_local_data);
+	ub953_read(priv, UB953_REG_GPIO_INPUT_CTRL, &gpio_input_ctrl);
+	ub953_read(priv, UB953_REG_GPIO_PIN_STS, &gpio_pin_sts);
+
+	for (i = 0; i < UB953_NUM_GPIOS; i++) {
+		dev_info(dev,
+			 "GPIO%u: remote: %u is_input: %u is_output: %u val: %u sts: %u\n",
+			 i,
+			 !!(gpio_local_data & UB953_REG_LOCAL_GPIO_DATA_GPIO_RMTEN(i)),
+			 !!(gpio_input_ctrl & UB953_REG_GPIO_INPUT_CTRL_INPUT_EN(i)),
+			 !!(gpio_input_ctrl & UB953_REG_GPIO_INPUT_CTRL_OUT_EN(i)),
+			 !!(gpio_local_data & UB953_REG_LOCAL_GPIO_DATA_GPIO_OUT_SRC(i)),
+			 !!(gpio_pin_sts & UB953_REG_GPIO_PIN_STS_GPIO_STS(i)));
+	}
+
+	return 0;
+}
+
+static int ub953_enable_streams(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *state, u32 pad,
+				u64 streams_mask)
+{
+	struct ub953_data *priv = sd_to_ub953(sd);
+	u64 sink_streams;
+	int ret;
+
+	sink_streams = v4l2_subdev_state_xlate_streams(state, UB953_PAD_SOURCE,
+						       UB953_PAD_SINK,
+						       &streams_mask);
+
+	ret = v4l2_subdev_enable_streams(priv->source_sd, priv->source_sd_pad,
+					 sink_streams);
+	if (ret)
+		return ret;
+
+	priv->enabled_source_streams |= streams_mask;
+
+	return 0;
+}
+
+static int ub953_disable_streams(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *state, u32 pad,
+				 u64 streams_mask)
+{
+	struct ub953_data *priv = sd_to_ub953(sd);
+	u64 sink_streams;
+	int ret;
+
+	sink_streams = v4l2_subdev_state_xlate_streams(state, UB953_PAD_SOURCE,
+						       UB953_PAD_SINK,
+						       &streams_mask);
+
+	ret = v4l2_subdev_disable_streams(priv->source_sd, priv->source_sd_pad,
+					  sink_streams);
+	if (ret)
+		return ret;
+
+	priv->enabled_source_streams &= ~streams_mask;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_pad_ops ub953_pad_ops = {
+	.enable_streams = ub953_enable_streams,
+	.disable_streams = ub953_disable_streams,
+	.set_routing = ub953_set_routing,
+	.get_frame_desc = ub953_get_frame_desc,
+	.get_fmt = v4l2_subdev_get_fmt,
+	.set_fmt = ub953_set_fmt,
+	.init_cfg = ub953_init_cfg,
+};
+
+static const struct v4l2_subdev_core_ops ub953_subdev_core_ops = {
+	.log_status = ub953_log_status,
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_ops ub953_subdev_ops = {
+	.core = &ub953_subdev_core_ops,
+	.pad = &ub953_pad_ops,
+};
+
+static const struct media_entity_operations ub953_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static int ub953_notify_bound(struct v4l2_async_notifier *notifier,
+			      struct v4l2_subdev *source_subdev,
+			      struct v4l2_async_connection *asd)
+{
+	struct ub953_data *priv = sd_to_ub953(notifier->sd);
+	struct device *dev = &priv->client->dev;
+	int ret;
+
+	ret = media_entity_get_fwnode_pad(&source_subdev->entity,
+					  source_subdev->fwnode,
+					  MEDIA_PAD_FL_SOURCE);
+	if (ret < 0) {
+		dev_err(dev, "Failed to find pad for %s\n",
+			source_subdev->name);
+		return ret;
+	}
+
+	priv->source_sd = source_subdev;
+	priv->source_sd_pad = ret;
+
+	ret = media_create_pad_link(&source_subdev->entity, priv->source_sd_pad,
+				    &priv->sd.entity, 0,
+				    MEDIA_LNK_FL_ENABLED |
+					    MEDIA_LNK_FL_IMMUTABLE);
+	if (ret) {
+		dev_err(dev, "Unable to link %s:%u -> %s:0\n",
+			source_subdev->name, priv->source_sd_pad,
+			priv->sd.name);
+		return ret;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_async_notifier_operations ub953_notify_ops = {
+	.bound = ub953_notify_bound,
+};
+
+static int ub953_v4l2_notifier_register(struct ub953_data *priv)
+{
+	struct device *dev = &priv->client->dev;
+	struct v4l2_async_connection *asd;
+	struct fwnode_handle *ep_fwnode;
+	int ret;
+
+	ep_fwnode = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev),
+						    UB953_PAD_SINK, 0, 0);
+	if (!ep_fwnode) {
+		dev_err(dev, "No graph endpoint\n");
+		return -ENODEV;
+	}
+
+	v4l2_async_subdev_nf_init(&priv->notifier, &priv->sd);
+
+	asd = v4l2_async_nf_add_fwnode_remote(&priv->notifier, ep_fwnode,
+					      struct v4l2_async_connection);
+
+	fwnode_handle_put(ep_fwnode);
+
+	if (IS_ERR(asd)) {
+		dev_err(dev, "Failed to add subdev: %ld", PTR_ERR(asd));
+		v4l2_async_nf_cleanup(&priv->notifier);
+		return PTR_ERR(asd);
+	}
+
+	priv->notifier.ops = &ub953_notify_ops;
+
+	ret = v4l2_async_nf_register(&priv->notifier);
+	if (ret) {
+		dev_err(dev, "Failed to register subdev_notifier");
+		v4l2_async_nf_cleanup(&priv->notifier);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void ub953_v4l2_notifier_unregister(struct ub953_data *priv)
+{
+	v4l2_async_nf_unregister(&priv->notifier);
+	v4l2_async_nf_cleanup(&priv->notifier);
+}
+
+/*
+ * Probing
+ */
+
+static int ub953_i2c_master_init(struct ub953_data *priv)
+{
+	/* i2c fast mode */
+	u32 ref = 26250000;
+	u32 scl_high = 915; /* ns */
+	u32 scl_low = 1641; /* ns */
+	int ret;
+
+	scl_high = div64_u64((u64)scl_high * ref, 1000000000) - 5;
+	scl_low = div64_u64((u64)scl_low * ref, 1000000000) - 5;
+
+	ret = ub953_write(priv, UB953_REG_SCL_HIGH_TIME, scl_high);
+	if (ret)
+		return ret;
+
+	ret = ub953_write(priv, UB953_REG_SCL_LOW_TIME, scl_low);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static u64 ub953_get_fc_rate(struct ub953_data *priv)
+{
+	switch (priv->mode) {
+	case UB953_MODE_SYNC:
+		if (priv->hw_data->is_ub971)
+			return priv->plat_data->bc_rate * 160ull;
+		else
+			return priv->plat_data->bc_rate / 2 * 160ull;
+
+	case UB953_MODE_NONSYNC_EXT:
+		/* CLKIN_DIV = 1 always */
+		return clk_get_rate(priv->clkin) * 80ull;
+
+	default:
+		/* Not supported */
+		return 0;
+	}
+}
+
+static unsigned long ub953_calc_clkout_ub953(struct ub953_data *priv,
+					     unsigned long target, u64 fc,
+					     u8 *hs_div, u8 *m, u8 *n)
+{
+	/*
+	 * We always use 4 as a pre-divider (HS_CLK_DIV = 2).
+	 *
+	 * According to the datasheet:
+	 * - "HS_CLK_DIV typically should be set to either 16, 8, or 4 (default)."
+	 * - "if it is not possible to have an integer ratio of N/M, it is best to
+	 *    select a smaller value for HS_CLK_DIV.
+	 *
+	 * For above reasons the default HS_CLK_DIV seems the best in the average
+	 * case. Use always that value to keep the code simple.
+	 */
+	static const unsigned long hs_clk_div = 4;
+
+	u64 fc_divided;
+	unsigned long mul, div;
+	unsigned long res;
+
+	/* clkout = fc / hs_clk_div * m / n */
+
+	fc_divided = div_u64(fc, hs_clk_div);
+
+	rational_best_approximation(target, fc_divided, (1 << 5) - 1,
+				    (1 << 8) - 1, &mul, &div);
+
+	res = div_u64(fc_divided * mul, div);
+
+	*hs_div = hs_clk_div;
+	*m = mul;
+	*n = div;
+
+	return res;
+}
+
+static unsigned long ub953_calc_clkout_ub971(struct ub953_data *priv,
+					     unsigned long target, u64 fc,
+					     u8 *m, u8 *n)
+{
+	u64 fc_divided;
+	unsigned long mul, div;
+	unsigned long res;
+
+	/* clkout = fc * m / (8 * n) */
+
+	fc_divided = div_u64(fc, 8);
+
+	rational_best_approximation(target, fc_divided, (1 << 5) - 1,
+				    (1 << 8) - 1, &mul, &div);
+
+	res = div_u64(fc_divided * mul, div);
+
+	*m = mul;
+	*n = div;
+
+	return res;
+}
+
+static void ub953_calc_clkout_params(struct ub953_data *priv,
+				     unsigned long target_rate,
+				     struct ub953_clkout_data *clkout_data)
+{
+	struct device *dev = &priv->client->dev;
+	unsigned long clkout_rate;
+	u64 fc_rate;
+
+	fc_rate = ub953_get_fc_rate(priv);
+
+	if (priv->hw_data->is_ub971) {
+		u8 m, n;
+
+		clkout_rate = ub953_calc_clkout_ub971(priv, target_rate,
+						      fc_rate, &m, &n);
+
+		clkout_data->m = m;
+		clkout_data->n = n;
+
+		dev_dbg(dev, "%s %llu * %u / (8 * %u) = %lu (requested %lu)",
+			__func__, fc_rate, m, n, clkout_rate, target_rate);
+	} else {
+		u8 hs_div, m, n;
+
+		clkout_rate = ub953_calc_clkout_ub953(priv, target_rate,
+						      fc_rate, &hs_div, &m, &n);
+
+		clkout_data->hs_div = hs_div;
+		clkout_data->m = m;
+		clkout_data->n = n;
+
+		dev_dbg(dev, "%s %llu / %u * %u / %u = %lu (requested %lu)",
+			__func__, fc_rate, hs_div, m, n, clkout_rate,
+			target_rate);
+	}
+
+	clkout_data->rate = clkout_rate;
+}
+
+static void ub953_write_clkout_regs(struct ub953_data *priv,
+				    const struct ub953_clkout_data *clkout_data)
+{
+	u8 clkout_ctrl0, clkout_ctrl1;
+
+	if (priv->hw_data->is_ub971)
+		clkout_ctrl0 = clkout_data->m;
+	else
+		clkout_ctrl0 = (__ffs(clkout_data->hs_div) << 5) |
+			       clkout_data->m;
+
+	clkout_ctrl1 = clkout_data->n;
+
+	ub953_write(priv, UB953_REG_CLKOUT_CTRL0, clkout_ctrl0);
+	ub953_write(priv, UB953_REG_CLKOUT_CTRL1, clkout_ctrl1);
+}
+
+static unsigned long ub953_clkout_recalc_rate(struct clk_hw *hw,
+					      unsigned long parent_rate)
+{
+	struct ub953_data *priv = container_of(hw, struct ub953_data, clkout_clk_hw);
+	struct device *dev = &priv->client->dev;
+	u8 ctrl0, ctrl1;
+	u32 mul, div;
+	u64 fc_rate;
+	u32 hs_clk_div;
+	u64 rate;
+	int ret;
+
+	ret = ub953_read(priv, UB953_REG_CLKOUT_CTRL0, &ctrl0);
+	if (ret) {
+		dev_err(dev, "Failed to read CLKOUT_CTRL0: %d\n", ret);
+		return 0;
+	}
+
+	ret = ub953_read(priv, UB953_REG_CLKOUT_CTRL1, &ctrl1);
+	if (ret) {
+		dev_err(dev, "Failed to read CLKOUT_CTRL1: %d\n", ret);
+		return 0;
+	}
+
+	fc_rate = ub953_get_fc_rate(priv);
+
+	if (priv->hw_data->is_ub971) {
+		mul = ctrl0 & 0x1f;
+		div = ctrl1;
+
+		if (div == 0)
+			return 0;
+
+		rate = div_u64(fc_rate * mul, 8 * div);
+
+		dev_dbg(dev, "clkout: fc rate %llu, mul %u, div %u = %llu\n",
+			fc_rate, mul, div, rate);
+	} else {
+		mul = ctrl0 & 0x1f;
+		hs_clk_div = 1 << (ctrl0 >> 5);
+		div = ctrl1;
+
+		if (div == 0)
+			return 0;
+
+		rate = div_u64(div_u64(fc_rate, hs_clk_div) * mul, div);
+
+		dev_dbg(dev,
+			"clkout: fc rate %llu, hs_clk_div %u, mul %u, div %u = %llu\n",
+			fc_rate, hs_clk_div, mul, div, rate);
+	}
+
+	return rate;
+}
+
+static long ub953_clkout_round_rate(struct clk_hw *hw, unsigned long rate,
+				    unsigned long *parent_rate)
+{
+	struct ub953_data *priv = container_of(hw, struct ub953_data, clkout_clk_hw);
+	struct ub953_clkout_data clkout_data;
+
+	ub953_calc_clkout_params(priv, rate, &clkout_data);
+
+	return clkout_data.rate;
+}
+
+static int ub953_clkout_set_rate(struct clk_hw *hw, unsigned long rate,
+				 unsigned long parent_rate)
+{
+	struct ub953_data *priv = container_of(hw, struct ub953_data, clkout_clk_hw);
+	struct ub953_clkout_data clkout_data;
+
+	ub953_calc_clkout_params(priv, rate, &clkout_data);
+
+	dev_dbg(&priv->client->dev, "%s %lu (requested %lu)\n", __func__,
+		clkout_data.rate, rate);
+
+	ub953_write_clkout_regs(priv, &clkout_data);
+
+	return 0;
+}
+
+static const struct clk_ops ub953_clkout_ops = {
+	.recalc_rate	= ub953_clkout_recalc_rate,
+	.round_rate	= ub953_clkout_round_rate,
+	.set_rate	= ub953_clkout_set_rate,
+};
+
+static int ub953_register_clkout(struct ub953_data *priv)
+{
+	struct device *dev = &priv->client->dev;
+	const struct clk_init_data init = {
+		.name = kasprintf(GFP_KERNEL, "ds90%s.%s.clk_out",
+				  priv->hw_data->model, dev_name(dev)),
+		.ops = &ub953_clkout_ops,
+	};
+	struct ub953_clkout_data clkout_data;
+	int ret;
+
+	if (!init.name)
+		return -ENOMEM;
+
+	/* Initialize clkout to 25MHz by default */
+	ub953_calc_clkout_params(priv, UB953_DEFAULT_CLKOUT_RATE, &clkout_data);
+	ub953_write_clkout_regs(priv, &clkout_data);
+
+	priv->clkout_clk_hw.init = &init;
+
+	ret = devm_clk_hw_register(dev, &priv->clkout_clk_hw);
+	kfree(init.name);
+	if (ret)
+		return dev_err_probe(dev, ret, "Cannot register clock HW\n");
+
+	ret = devm_of_clk_add_hw_provider(dev, of_clk_hw_simple_get,
+					  &priv->clkout_clk_hw);
+	if (ret)
+		return dev_err_probe(dev, ret,
+				     "Cannot add OF clock provider\n");
+
+	return 0;
+}
+
+static int ub953_add_i2c_adapter(struct ub953_data *priv)
+{
+	struct device *dev = &priv->client->dev;
+	struct fwnode_handle *i2c_handle;
+	int ret;
+
+	i2c_handle = device_get_named_child_node(dev, "i2c");
+	if (!i2c_handle)
+		return 0;
+
+	ret = i2c_atr_add_adapter(priv->plat_data->atr, priv->plat_data->port,
+				  dev, i2c_handle);
+
+	fwnode_handle_put(i2c_handle);
+
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static const struct regmap_config ub953_regmap_config = {
+	.name = "ds90ub953",
+	.reg_bits = 8,
+	.val_bits = 8,
+	.reg_format_endian = REGMAP_ENDIAN_DEFAULT,
+	.val_format_endian = REGMAP_ENDIAN_DEFAULT,
+};
+
+static int ub953_parse_dt(struct ub953_data *priv)
+{
+	struct device *dev = &priv->client->dev;
+	struct v4l2_fwnode_endpoint vep = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY,
+	};
+	struct fwnode_handle *ep_fwnode;
+	unsigned char nlanes;
+	int ret;
+
+	ep_fwnode = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev),
+						    UB953_PAD_SINK, 0, 0);
+	if (!ep_fwnode)
+		return dev_err_probe(dev, -ENOENT, "no endpoint found\n");
+
+	ret = v4l2_fwnode_endpoint_parse(ep_fwnode, &vep);
+
+	fwnode_handle_put(ep_fwnode);
+
+	if (ret)
+		return dev_err_probe(dev, ret,
+				     "failed to parse sink endpoint data\n");
+
+	nlanes = vep.bus.mipi_csi2.num_data_lanes;
+	if (nlanes != 1 && nlanes != 2 && nlanes != 4)
+		return dev_err_probe(dev, -EINVAL,
+				     "bad number of data-lanes: %u\n", nlanes);
+
+	priv->num_data_lanes = nlanes;
+
+	priv->non_continous_clk = vep.bus.mipi_csi2.flags &
+				  V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK;
+
+	return 0;
+}
+
+static int ub953_hw_init(struct ub953_data *priv)
+{
+	struct device *dev = &priv->client->dev;
+	bool mode_override;
+	int ret;
+	u8 v;
+
+	ret = ub953_read(priv, UB953_REG_MODE_SEL, &v);
+	if (ret)
+		return ret;
+
+	if (!(v & UB953_REG_MODE_SEL_MODE_DONE))
+		return dev_err_probe(dev, -EIO, "Mode value not stabilized\n");
+
+	mode_override = v & UB953_REG_MODE_SEL_MODE_OVERRIDE;
+
+	switch (v & UB953_REG_MODE_SEL_MODE_MASK) {
+	case 0:
+		priv->mode = UB953_MODE_SYNC;
+		break;
+	case 2:
+		priv->mode = UB953_MODE_NONSYNC_EXT;
+		break;
+	case 3:
+		priv->mode = UB953_MODE_NONSYNC_INT;
+		break;
+	case 5:
+		priv->mode = UB953_MODE_DVP;
+		break;
+	default:
+		return dev_err_probe(dev, -EIO,
+				     "Invalid mode in mode register\n");
+	}
+
+	dev_dbg(dev, "mode from %s: %#x\n", mode_override ? "reg" : "strap",
+		priv->mode);
+
+	if (priv->mode != UB953_MODE_SYNC &&
+	    priv->mode != UB953_MODE_NONSYNC_EXT)
+		return dev_err_probe(dev, -ENODEV,
+				     "Unsupported mode selected: %u\n",
+				     priv->mode);
+
+	if (priv->mode == UB953_MODE_NONSYNC_EXT && !priv->clkin)
+		return dev_err_probe(dev, -EINVAL,
+				     "clkin required for non-sync ext mode\n");
+
+	ret = ub953_read(priv, UB953_REG_REV_MASK_ID, &v);
+	if (ret)
+		return dev_err_probe(dev, ret, "Failed to read revision");
+
+	dev_info(dev, "Found %s rev/mask %#04x\n", priv->hw_data->model, v);
+
+	ret = ub953_read(priv, UB953_REG_GENERAL_CFG, &v);
+	if (ret)
+		return ret;
+
+	dev_dbg(dev, "i2c strap setting %s V\n",
+		(v & UB953_REG_GENERAL_CFG_I2C_STRAP_MODE) ? "1.8" : "3.3");
+
+	ret = ub953_i2c_master_init(priv);
+	if (ret)
+		return dev_err_probe(dev, ret, "i2c init failed\n");
+
+	ub953_write(priv, UB953_REG_GENERAL_CFG,
+		    (priv->non_continous_clk ? 0 : UB953_REG_GENERAL_CFG_CONT_CLK) |
+		    ((priv->num_data_lanes - 1) << UB953_REG_GENERAL_CFG_CSI_LANE_SEL_SHIFT) |
+		    UB953_REG_GENERAL_CFG_CRC_TX_GEN_ENABLE);
+
+	return 0;
+}
+
+static int ub953_subdev_init(struct ub953_data *priv)
+{
+	struct device *dev = &priv->client->dev;
+	int ret;
+
+	v4l2_i2c_subdev_init(&priv->sd, priv->client, &ub953_subdev_ops);
+
+	priv->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+			  V4L2_SUBDEV_FL_HAS_EVENTS | V4L2_SUBDEV_FL_STREAMS;
+	priv->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
+	priv->sd.entity.ops = &ub953_entity_ops;
+
+	priv->pads[0].flags = MEDIA_PAD_FL_SINK;
+	priv->pads[1].flags = MEDIA_PAD_FL_SOURCE;
+
+	ret = media_entity_pads_init(&priv->sd.entity, 2, priv->pads);
+	if (ret)
+		return dev_err_probe(dev, ret, "Failed to init pads\n");
+
+	ret = v4l2_subdev_init_finalize(&priv->sd);
+	if (ret)
+		goto err_entity_cleanup;
+
+	ret = ub953_v4l2_notifier_register(priv);
+	if (ret) {
+		dev_err_probe(dev, ret,
+			      "v4l2 subdev notifier register failed\n");
+		goto err_free_state;
+	}
+
+	ret = v4l2_async_register_subdev(&priv->sd);
+	if (ret) {
+		dev_err_probe(dev, ret, "v4l2_async_register_subdev error\n");
+		goto err_unreg_notif;
+	}
+
+	return 0;
+
+err_unreg_notif:
+	ub953_v4l2_notifier_unregister(priv);
+err_free_state:
+	v4l2_subdev_cleanup(&priv->sd);
+err_entity_cleanup:
+	media_entity_cleanup(&priv->sd.entity);
+
+	return ret;
+}
+
+static void ub953_subdev_uninit(struct ub953_data *priv)
+{
+	v4l2_async_unregister_subdev(&priv->sd);
+	ub953_v4l2_notifier_unregister(priv);
+	v4l2_subdev_cleanup(&priv->sd);
+	fwnode_handle_put(priv->sd.fwnode);
+	media_entity_cleanup(&priv->sd.entity);
+}
+
+static int ub953_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct ub953_data *priv;
+	int ret;
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->client = client;
+
+	priv->hw_data = device_get_match_data(dev);
+
+	priv->plat_data = dev_get_platdata(&client->dev);
+	if (!priv->plat_data)
+		return dev_err_probe(dev, -ENODEV, "Platform data missing\n");
+
+	mutex_init(&priv->reg_lock);
+
+	/*
+	 * Initialize to invalid values so that the first reg writes will
+	 * configure the target.
+	 */
+	priv->current_indirect_target = 0xff;
+
+	priv->regmap = devm_regmap_init_i2c(client, &ub953_regmap_config);
+	if (IS_ERR(priv->regmap)) {
+		ret = PTR_ERR(priv->regmap);
+		dev_err_probe(dev, ret, "Failed to init regmap\n");
+		goto err_mutex_destroy;
+	}
+
+	priv->clkin = devm_clk_get_optional(dev, "clkin");
+	if (IS_ERR(priv->clkin)) {
+		ret = PTR_ERR(priv->clkin);
+		dev_err_probe(dev, ret, "failed to parse 'clkin'\n");
+		goto err_mutex_destroy;
+	}
+
+	ret = ub953_parse_dt(priv);
+	if (ret)
+		goto err_mutex_destroy;
+
+	ret = ub953_hw_init(priv);
+	if (ret)
+		goto err_mutex_destroy;
+
+	ret = ub953_gpiochip_probe(priv);
+	if (ret) {
+		dev_err_probe(dev, ret, "Failed to init gpiochip\n");
+		goto err_mutex_destroy;
+	}
+
+	ret = ub953_register_clkout(priv);
+	if (ret) {
+		dev_err_probe(dev, ret, "Failed to register clkout\n");
+		goto err_gpiochip_remove;
+	}
+
+	ret = ub953_subdev_init(priv);
+	if (ret)
+		goto err_gpiochip_remove;
+
+	ret = ub953_add_i2c_adapter(priv);
+	if (ret) {
+		dev_err_probe(dev, ret, "failed to add remote i2c adapter\n");
+		goto err_subdev_uninit;
+	}
+
+	return 0;
+
+err_subdev_uninit:
+	ub953_subdev_uninit(priv);
+err_gpiochip_remove:
+	ub953_gpiochip_remove(priv);
+err_mutex_destroy:
+	mutex_destroy(&priv->reg_lock);
+
+	return ret;
+}
+
+static void ub953_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ub953_data *priv = sd_to_ub953(sd);
+
+	i2c_atr_del_adapter(priv->plat_data->atr, priv->plat_data->port);
+
+	ub953_subdev_uninit(priv);
+
+	ub953_gpiochip_remove(priv);
+	mutex_destroy(&priv->reg_lock);
+}
+
+static const struct ub953_hw_data ds90ub953_hw = {
+	.model = "ub953",
+};
+
+static const struct ub953_hw_data ds90ub971_hw = {
+	.model = "ub971",
+	.is_ub971 = true,
+};
+
+static const struct i2c_device_id ub953_id[] = {
+	{ "ds90ub953-q1", (kernel_ulong_t)&ds90ub953_hw },
+	{ "ds90ub971-q1", (kernel_ulong_t)&ds90ub971_hw },
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, ub953_id);
+
+static const struct of_device_id ub953_dt_ids[] = {
+	{ .compatible = "ti,ds90ub953-q1", .data = &ds90ub953_hw },
+	{ .compatible = "ti,ds90ub971-q1", .data = &ds90ub971_hw },
+	{}
+};
+MODULE_DEVICE_TABLE(of, ub953_dt_ids);
+
+static struct i2c_driver ds90ub953_driver = {
+	.probe		= ub953_probe,
+	.remove		= ub953_remove,
+	.id_table	= ub953_id,
+	.driver = {
+		.name	= "ds90ub953",
+		.of_match_table = ub953_dt_ids,
+	},
+};
+module_i2c_driver(ds90ub953_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Texas Instruments FPD-Link III/IV CSI-2 Serializers Driver");
+MODULE_AUTHOR("Luca Ceresoli <luca@lucaceresoli.net>");
+MODULE_AUTHOR("Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>");
+MODULE_IMPORT_NS(I2C_ATR);
diff --git a/drivers/media/i2c/ds90ub960.c b/drivers/media/i2c/ds90ub960.c
new file mode 100644
index 000000000..8ba5750f5
--- /dev/null
+++ b/drivers/media/i2c/ds90ub960.c
@@ -0,0 +1,4059 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Driver for the Texas Instruments DS90UB960-Q1 video deserializer
+ *
+ * Copyright (c) 2019 Luca Ceresoli <luca@lucaceresoli.net>
+ * Copyright (c) 2023 Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>
+ */
+
+/*
+ * (Possible) TODOs:
+ *
+ * - PM for serializer and remote peripherals. We need to manage:
+ *   - VPOC
+ *     - Power domain? Regulator? Somehow any remote device should be able to
+ *       cause the VPOC to be turned on.
+ *   - Link between the deserializer and the serializer
+ *     - Related to VPOC management. We probably always want to turn on the VPOC
+ *       and then enable the link.
+ *   - Serializer's services: i2c, gpios, power
+ *     - The serializer needs to resume before the remote peripherals can
+ *       e.g. use the i2c.
+ *     - How to handle gpios? Reserving a gpio essentially keeps the provider
+ *       (serializer) always powered on.
+ * - Do we need a new bus for the FPD-Link? At the moment the serializers
+ *   are children of the same i2c-adapter where the deserializer resides.
+ * - i2c-atr could be made embeddable instead of allocatable.
+ */
+
+#include <linux/bitops.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/fwnode.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c-atr.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/kthread.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/property.h>
+#include <linux/regmap.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/workqueue.h>
+
+#include <media/i2c/ds90ub9xx.h>
+#include <media/mipi-csi2.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+#define MHZ(v) ((u32)((v) * 1000000U))
+
+#define UB960_POLL_TIME_MS	500
+
+#define UB960_MAX_RX_NPORTS	4
+#define UB960_MAX_TX_NPORTS	2
+#define UB960_MAX_NPORTS	(UB960_MAX_RX_NPORTS + UB960_MAX_TX_NPORTS)
+
+#define UB960_MAX_PORT_ALIASES	8
+
+#define UB960_NUM_BC_GPIOS		4
+
+/*
+ * Register map
+ *
+ * 0x00-0x32   Shared (UB960_SR)
+ * 0x33-0x3a   CSI-2 TX (per-port paged on DS90UB960, shared on 954) (UB960_TR)
+ * 0x4c        Shared (UB960_SR)
+ * 0x4d-0x7f   FPD-Link RX, per-port paged (UB960_RR)
+ * 0xb0-0xbf   Shared (UB960_SR)
+ * 0xd0-0xdf   FPD-Link RX, per-port paged (UB960_RR)
+ * 0xf0-0xf5   Shared (UB960_SR)
+ * 0xf8-0xfb   Shared (UB960_SR)
+ * All others  Reserved
+ *
+ * Register prefixes:
+ * UB960_SR_* = Shared register
+ * UB960_RR_* = FPD-Link RX, per-port paged register
+ * UB960_TR_* = CSI-2 TX, per-port paged register
+ * UB960_XR_* = Reserved register
+ * UB960_IR_* = Indirect register
+ */
+
+#define UB960_SR_I2C_DEV_ID			0x00
+#define UB960_SR_RESET				0x01
+#define UB960_SR_RESET_DIGITAL_RESET1		BIT(1)
+#define UB960_SR_RESET_DIGITAL_RESET0		BIT(0)
+#define UB960_SR_RESET_GPIO_LOCK_RELEASE	BIT(5)
+
+#define UB960_SR_GEN_CONFIG			0x02
+#define UB960_SR_REV_MASK			0x03
+#define UB960_SR_DEVICE_STS			0x04
+#define UB960_SR_PAR_ERR_THOLD_HI		0x05
+#define UB960_SR_PAR_ERR_THOLD_LO		0x06
+#define UB960_SR_BCC_WDOG_CTL			0x07
+#define UB960_SR_I2C_CTL1			0x08
+#define UB960_SR_I2C_CTL2			0x09
+#define UB960_SR_SCL_HIGH_TIME			0x0a
+#define UB960_SR_SCL_LOW_TIME			0x0b
+#define UB960_SR_RX_PORT_CTL			0x0c
+#define UB960_SR_IO_CTL				0x0d
+#define UB960_SR_GPIO_PIN_STS			0x0e
+#define UB960_SR_GPIO_INPUT_CTL			0x0f
+#define UB960_SR_GPIO_PIN_CTL(n)		(0x10 + (n)) /* n < UB960_NUM_GPIOS */
+#define UB960_SR_GPIO_PIN_CTL_GPIO_OUT_SEL		5
+#define UB960_SR_GPIO_PIN_CTL_GPIO_OUT_SRC_SHIFT	2
+#define UB960_SR_GPIO_PIN_CTL_GPIO_OUT_EN		BIT(0)
+
+#define UB960_SR_FS_CTL				0x18
+#define UB960_SR_FS_HIGH_TIME_1			0x19
+#define UB960_SR_FS_HIGH_TIME_0			0x1a
+#define UB960_SR_FS_LOW_TIME_1			0x1b
+#define UB960_SR_FS_LOW_TIME_0			0x1c
+#define UB960_SR_MAX_FRM_HI			0x1d
+#define UB960_SR_MAX_FRM_LO			0x1e
+#define UB960_SR_CSI_PLL_CTL			0x1f
+
+#define UB960_SR_FWD_CTL1			0x20
+#define UB960_SR_FWD_CTL1_PORT_DIS(n)		BIT((n) + 4)
+
+#define UB960_SR_FWD_CTL2			0x21
+#define UB960_SR_FWD_STS			0x22
+
+#define UB960_SR_INTERRUPT_CTL			0x23
+#define UB960_SR_INTERRUPT_CTL_INT_EN		BIT(7)
+#define UB960_SR_INTERRUPT_CTL_IE_CSI_TX0	BIT(4)
+#define UB960_SR_INTERRUPT_CTL_IE_RX(n)		BIT((n)) /* rxport[n] IRQ */
+
+#define UB960_SR_INTERRUPT_STS			0x24
+#define UB960_SR_INTERRUPT_STS_INT		BIT(7)
+#define UB960_SR_INTERRUPT_STS_IS_CSI_TX(n)	BIT(4 + (n)) /* txport[n] IRQ */
+#define UB960_SR_INTERRUPT_STS_IS_RX(n)		BIT((n)) /* rxport[n] IRQ */
+
+#define UB960_SR_TS_CONFIG			0x25
+#define UB960_SR_TS_CONTROL			0x26
+#define UB960_SR_TS_LINE_HI			0x27
+#define UB960_SR_TS_LINE_LO			0x28
+#define UB960_SR_TS_STATUS			0x29
+#define UB960_SR_TIMESTAMP_P0_HI		0x2a
+#define UB960_SR_TIMESTAMP_P0_LO		0x2b
+#define UB960_SR_TIMESTAMP_P1_HI		0x2c
+#define UB960_SR_TIMESTAMP_P1_LO		0x2d
+
+#define UB960_SR_CSI_PORT_SEL			0x32
+
+#define UB960_TR_CSI_CTL			0x33
+#define UB960_TR_CSI_CTL_CSI_CAL_EN		BIT(6)
+#define UB960_TR_CSI_CTL_CSI_CONTS_CLOCK	BIT(1)
+#define UB960_TR_CSI_CTL_CSI_ENABLE		BIT(0)
+
+#define UB960_TR_CSI_CTL2			0x34
+#define UB960_TR_CSI_STS			0x35
+#define UB960_TR_CSI_TX_ICR			0x36
+
+#define UB960_TR_CSI_TX_ISR			0x37
+#define UB960_TR_CSI_TX_ISR_IS_CSI_SYNC_ERROR	BIT(3)
+#define UB960_TR_CSI_TX_ISR_IS_CSI_PASS_ERROR	BIT(1)
+
+#define UB960_TR_CSI_TEST_CTL			0x38
+#define UB960_TR_CSI_TEST_PATT_HI		0x39
+#define UB960_TR_CSI_TEST_PATT_LO		0x3a
+
+#define UB960_XR_SFILTER_CFG			0x41
+#define UB960_XR_SFILTER_CFG_SFILTER_MAX_SHIFT	4
+#define UB960_XR_SFILTER_CFG_SFILTER_MIN_SHIFT	0
+
+#define UB960_XR_AEQ_CTL1			0x42
+#define UB960_XR_AEQ_CTL1_AEQ_ERR_CTL_FPD_CLK	BIT(6)
+#define UB960_XR_AEQ_CTL1_AEQ_ERR_CTL_ENCODING	BIT(5)
+#define UB960_XR_AEQ_CTL1_AEQ_ERR_CTL_PARITY	BIT(4)
+#define UB960_XR_AEQ_CTL1_AEQ_ERR_CTL_MASK        \
+	(UB960_XR_AEQ_CTL1_AEQ_ERR_CTL_FPD_CLK |  \
+	 UB960_XR_AEQ_CTL1_AEQ_ERR_CTL_ENCODING | \
+	 UB960_XR_AEQ_CTL1_AEQ_ERR_CTL_PARITY)
+#define UB960_XR_AEQ_CTL1_AEQ_SFILTER_EN	BIT(0)
+
+#define UB960_XR_AEQ_ERR_THOLD			0x43
+
+#define UB960_RR_BCC_ERR_CTL			0x46
+#define UB960_RR_BCC_STATUS			0x47
+#define UB960_RR_BCC_STATUS_SEQ_ERROR		BIT(5)
+#define UB960_RR_BCC_STATUS_MASTER_ERR		BIT(4)
+#define UB960_RR_BCC_STATUS_MASTER_TO		BIT(3)
+#define UB960_RR_BCC_STATUS_SLAVE_ERR		BIT(2)
+#define UB960_RR_BCC_STATUS_SLAVE_TO		BIT(1)
+#define UB960_RR_BCC_STATUS_RESP_ERR		BIT(0)
+#define UB960_RR_BCC_STATUS_ERROR_MASK                                    \
+	(UB960_RR_BCC_STATUS_SEQ_ERROR | UB960_RR_BCC_STATUS_MASTER_ERR | \
+	 UB960_RR_BCC_STATUS_MASTER_TO | UB960_RR_BCC_STATUS_SLAVE_ERR |  \
+	 UB960_RR_BCC_STATUS_SLAVE_TO | UB960_RR_BCC_STATUS_RESP_ERR)
+
+#define UB960_RR_FPD3_CAP			0x4a
+#define UB960_RR_RAW_EMBED_DTYPE		0x4b
+#define UB960_RR_RAW_EMBED_DTYPE_LINES_SHIFT	6
+
+#define UB960_SR_FPD3_PORT_SEL			0x4c
+
+#define UB960_RR_RX_PORT_STS1			0x4d
+#define UB960_RR_RX_PORT_STS1_BCC_CRC_ERROR	BIT(5)
+#define UB960_RR_RX_PORT_STS1_LOCK_STS_CHG	BIT(4)
+#define UB960_RR_RX_PORT_STS1_BCC_SEQ_ERROR	BIT(3)
+#define UB960_RR_RX_PORT_STS1_PARITY_ERROR	BIT(2)
+#define UB960_RR_RX_PORT_STS1_PORT_PASS		BIT(1)
+#define UB960_RR_RX_PORT_STS1_LOCK_STS		BIT(0)
+#define UB960_RR_RX_PORT_STS1_ERROR_MASK       \
+	(UB960_RR_RX_PORT_STS1_BCC_CRC_ERROR | \
+	 UB960_RR_RX_PORT_STS1_BCC_SEQ_ERROR | \
+	 UB960_RR_RX_PORT_STS1_PARITY_ERROR)
+
+#define UB960_RR_RX_PORT_STS2			0x4e
+#define UB960_RR_RX_PORT_STS2_LINE_LEN_UNSTABLE	BIT(7)
+#define UB960_RR_RX_PORT_STS2_LINE_LEN_CHG	BIT(6)
+#define UB960_RR_RX_PORT_STS2_FPD3_ENCODE_ERROR	BIT(5)
+#define UB960_RR_RX_PORT_STS2_BUFFER_ERROR	BIT(4)
+#define UB960_RR_RX_PORT_STS2_CSI_ERROR		BIT(3)
+#define UB960_RR_RX_PORT_STS2_FREQ_STABLE	BIT(2)
+#define UB960_RR_RX_PORT_STS2_CABLE_FAULT	BIT(1)
+#define UB960_RR_RX_PORT_STS2_LINE_CNT_CHG	BIT(0)
+#define UB960_RR_RX_PORT_STS2_ERROR_MASK       \
+	UB960_RR_RX_PORT_STS2_BUFFER_ERROR
+
+#define UB960_RR_RX_FREQ_HIGH			0x4f
+#define UB960_RR_RX_FREQ_LOW			0x50
+#define UB960_RR_SENSOR_STS_0			0x51
+#define UB960_RR_SENSOR_STS_1			0x52
+#define UB960_RR_SENSOR_STS_2			0x53
+#define UB960_RR_SENSOR_STS_3			0x54
+#define UB960_RR_RX_PAR_ERR_HI			0x55
+#define UB960_RR_RX_PAR_ERR_LO			0x56
+#define UB960_RR_BIST_ERR_COUNT			0x57
+
+#define UB960_RR_BCC_CONFIG			0x58
+#define UB960_RR_BCC_CONFIG_I2C_PASS_THROUGH	BIT(6)
+#define UB960_RR_BCC_CONFIG_BC_FREQ_SEL_MASK	GENMASK(2, 0)
+
+#define UB960_RR_DATAPATH_CTL1			0x59
+#define UB960_RR_DATAPATH_CTL2			0x5a
+#define UB960_RR_SER_ID				0x5b
+#define UB960_RR_SER_ALIAS_ID			0x5c
+
+/* For these two register sets: n < UB960_MAX_PORT_ALIASES */
+#define UB960_RR_SLAVE_ID(n)			(0x5d + (n))
+#define UB960_RR_SLAVE_ALIAS(n)			(0x65 + (n))
+
+#define UB960_RR_PORT_CONFIG			0x6d
+#define UB960_RR_PORT_CONFIG_FPD3_MODE_MASK	GENMASK(1, 0)
+
+#define UB960_RR_BC_GPIO_CTL(n)			(0x6e + (n)) /* n < 2 */
+#define UB960_RR_RAW10_ID			0x70
+#define UB960_RR_RAW10_ID_VC_SHIFT		6
+#define UB960_RR_RAW10_ID_DT_SHIFT		0
+
+#define UB960_RR_RAW12_ID			0x71
+#define UB960_RR_CSI_VC_MAP			0x72
+#define UB960_RR_CSI_VC_MAP_SHIFT(x)		((x) * 2)
+
+#define UB960_RR_LINE_COUNT_HI			0x73
+#define UB960_RR_LINE_COUNT_LO			0x74
+#define UB960_RR_LINE_LEN_1			0x75
+#define UB960_RR_LINE_LEN_0			0x76
+#define UB960_RR_FREQ_DET_CTL			0x77
+#define UB960_RR_MAILBOX_1			0x78
+#define UB960_RR_MAILBOX_2			0x79
+
+#define UB960_RR_CSI_RX_STS			0x7a
+#define UB960_RR_CSI_RX_STS_LENGTH_ERR		BIT(3)
+#define UB960_RR_CSI_RX_STS_CKSUM_ERR		BIT(2)
+#define UB960_RR_CSI_RX_STS_ECC2_ERR		BIT(1)
+#define UB960_RR_CSI_RX_STS_ECC1_ERR		BIT(0)
+#define UB960_RR_CSI_RX_STS_ERROR_MASK                                    \
+	(UB960_RR_CSI_RX_STS_LENGTH_ERR | UB960_RR_CSI_RX_STS_CKSUM_ERR | \
+	 UB960_RR_CSI_RX_STS_ECC2_ERR | UB960_RR_CSI_RX_STS_ECC1_ERR)
+
+#define UB960_RR_CSI_ERR_COUNTER		0x7b
+#define UB960_RR_PORT_CONFIG2			0x7c
+#define UB960_RR_PORT_CONFIG2_RAW10_8BIT_CTL_MASK GENMASK(7, 6)
+#define UB960_RR_PORT_CONFIG2_RAW10_8BIT_CTL_SHIFT 6
+
+#define UB960_RR_PORT_CONFIG2_LV_POL_LOW	BIT(1)
+#define UB960_RR_PORT_CONFIG2_FV_POL_LOW	BIT(0)
+
+#define UB960_RR_PORT_PASS_CTL			0x7d
+#define UB960_RR_SEN_INT_RISE_CTL		0x7e
+#define UB960_RR_SEN_INT_FALL_CTL		0x7f
+
+#define UB960_SR_CSI_FRAME_COUNT_HI(n)		(0x90 + 8 * (n))
+#define UB960_SR_CSI_FRAME_COUNT_LO(n)		(0x91 + 8 * (n))
+#define UB960_SR_CSI_FRAME_ERR_COUNT_HI(n)	(0x92 + 8 * (n))
+#define UB960_SR_CSI_FRAME_ERR_COUNT_LO(n)	(0x93 + 8 * (n))
+#define UB960_SR_CSI_LINE_COUNT_HI(n)		(0x94 + 8 * (n))
+#define UB960_SR_CSI_LINE_COUNT_LO(n)		(0x95 + 8 * (n))
+#define UB960_SR_CSI_LINE_ERR_COUNT_HI(n)	(0x96 + 8 * (n))
+#define UB960_SR_CSI_LINE_ERR_COUNT_LO(n)	(0x97 + 8 * (n))
+
+#define UB960_XR_REFCLK_FREQ			0xa5	/* UB960 */
+
+#define UB960_RR_VC_ID_MAP(x)			(0xa0 + (x)) /* UB9702 */
+
+#define UB960_SR_IND_ACC_CTL			0xb0
+#define UB960_SR_IND_ACC_CTL_IA_AUTO_INC	BIT(1)
+
+#define UB960_SR_IND_ACC_ADDR			0xb1
+#define UB960_SR_IND_ACC_DATA			0xb2
+#define UB960_SR_BIST_CONTROL			0xb3
+#define UB960_SR_MODE_IDX_STS			0xb8
+#define UB960_SR_LINK_ERROR_COUNT		0xb9
+#define UB960_SR_FPD3_ENC_CTL			0xba
+#define UB960_SR_FV_MIN_TIME			0xbc
+#define UB960_SR_GPIO_PD_CTL			0xbe
+
+#define UB960_SR_FPD_RATE_CFG			0xc2	/* UB9702 */
+#define UB960_SR_CSI_PLL_DIV			0xc9	/* UB9702 */
+
+#define UB960_RR_PORT_DEBUG			0xd0
+#define UB960_RR_AEQ_CTL2			0xd2
+#define UB960_RR_AEQ_CTL2_SET_AEQ_FLOOR		BIT(2)
+
+#define UB960_RR_AEQ_STATUS			0xd3
+#define UB960_RR_AEQ_STATUS_STATUS_2		GENMASK(5, 3)
+#define UB960_RR_AEQ_STATUS_STATUS_1		GENMASK(2, 0)
+
+#define UB960_RR_AEQ_BYPASS			0xd4
+#define UB960_RR_AEQ_BYPASS_EQ_STAGE1_VALUE_SHIFT	5
+#define UB960_RR_AEQ_BYPASS_EQ_STAGE1_VALUE_MASK	GENMASK(7, 5)
+#define UB960_RR_AEQ_BYPASS_EQ_STAGE2_VALUE_SHIFT	1
+#define UB960_RR_AEQ_BYPASS_EQ_STAGE2_VALUE_MASK	GENMASK(3, 1)
+#define UB960_RR_AEQ_BYPASS_ENABLE			BIT(0)
+
+#define UB960_RR_AEQ_MIN_MAX			0xd5
+#define UB960_RR_AEQ_MIN_MAX_AEQ_MAX_SHIFT	4
+#define UB960_RR_AEQ_MIN_MAX_AEQ_FLOOR_SHIFT	0
+
+#define UB960_RR_SFILTER_STS_0			0xd6
+#define UB960_RR_SFILTER_STS_1			0xd7
+#define UB960_RR_PORT_ICR_HI			0xd8
+#define UB960_RR_PORT_ICR_LO			0xd9
+#define UB960_RR_PORT_ISR_HI			0xda
+#define UB960_RR_PORT_ISR_LO			0xdb
+#define UB960_RR_FC_GPIO_STS			0xdc
+#define UB960_RR_FC_GPIO_ICR			0xdd
+#define UB960_RR_SEN_INT_RISE_STS		0xde
+#define UB960_RR_SEN_INT_FALL_STS		0xdf
+
+#define UB960_RR_CHANNEL_MODE			0xe4	/* UB9702 */
+
+#define UB960_SR_FPD3_RX_ID(n)			(0xf0 + (n))
+#define UB960_SR_FPD3_RX_ID_LEN			6
+
+#define UB960_SR_I2C_RX_ID(n)			(0xf8 + (n)) /* < UB960_FPD_RX_NPORTS */
+
+/* Indirect register blocks */
+#define UB960_IND_TARGET_PAT_GEN		0x00
+#define UB960_IND_TARGET_RX_ANA(n)		(0x01 + (n))
+#define UB960_IND_TARGET_CSI_CSIPLL_REG_1	0x92	/* UB9702 */
+#define UB960_IND_TARGET_CSI_ANA		0x07
+
+/* UB960_IR_PGEN_*: Indirect Registers for Test Pattern Generator */
+
+#define UB960_IR_PGEN_CTL			0x01
+#define UB960_IR_PGEN_CTL_PGEN_ENABLE		BIT(0)
+
+#define UB960_IR_PGEN_CFG			0x02
+#define UB960_IR_PGEN_CSI_DI			0x03
+#define UB960_IR_PGEN_LINE_SIZE1		0x04
+#define UB960_IR_PGEN_LINE_SIZE0		0x05
+#define UB960_IR_PGEN_BAR_SIZE1			0x06
+#define UB960_IR_PGEN_BAR_SIZE0			0x07
+#define UB960_IR_PGEN_ACT_LPF1			0x08
+#define UB960_IR_PGEN_ACT_LPF0			0x09
+#define UB960_IR_PGEN_TOT_LPF1			0x0a
+#define UB960_IR_PGEN_TOT_LPF0			0x0b
+#define UB960_IR_PGEN_LINE_PD1			0x0c
+#define UB960_IR_PGEN_LINE_PD0			0x0d
+#define UB960_IR_PGEN_VBP			0x0e
+#define UB960_IR_PGEN_VFP			0x0f
+#define UB960_IR_PGEN_COLOR(n)			(0x10 + (n)) /* n < 15 */
+
+#define UB960_IR_RX_ANA_STROBE_SET_CLK		0x08
+#define UB960_IR_RX_ANA_STROBE_SET_CLK_NO_EXTRA_DELAY	BIT(3)
+#define UB960_IR_RX_ANA_STROBE_SET_CLK_DELAY_MASK	GENMASK(2, 0)
+
+#define UB960_IR_RX_ANA_STROBE_SET_DATA		0x09
+#define UB960_IR_RX_ANA_STROBE_SET_DATA_NO_EXTRA_DELAY	BIT(3)
+#define UB960_IR_RX_ANA_STROBE_SET_DATA_DELAY_MASK	GENMASK(2, 0)
+
+/* EQ related */
+
+#define UB960_MIN_AEQ_STROBE_POS -7
+#define UB960_MAX_AEQ_STROBE_POS  7
+
+#define UB960_MANUAL_STROBE_EXTRA_DELAY 6
+
+#define UB960_MIN_MANUAL_STROBE_POS -(7 + UB960_MANUAL_STROBE_EXTRA_DELAY)
+#define UB960_MAX_MANUAL_STROBE_POS  (7 + UB960_MANUAL_STROBE_EXTRA_DELAY)
+#define UB960_NUM_MANUAL_STROBE_POS  (UB960_MAX_MANUAL_STROBE_POS - UB960_MIN_MANUAL_STROBE_POS + 1)
+
+#define UB960_MIN_EQ_LEVEL  0
+#define UB960_MAX_EQ_LEVEL  14
+#define UB960_NUM_EQ_LEVELS (UB960_MAX_EQ_LEVEL - UB960_MIN_EQ_LEVEL + 1)
+
+struct ub960_hw_data {
+	const char *model;
+	u8 num_rxports;
+	u8 num_txports;
+	bool is_ub9702;
+	bool is_fpdlink4;
+};
+
+enum ub960_rxport_mode {
+	RXPORT_MODE_RAW10 = 0,
+	RXPORT_MODE_RAW12_HF = 1,
+	RXPORT_MODE_RAW12_LF = 2,
+	RXPORT_MODE_CSI2_SYNC = 3,
+	RXPORT_MODE_CSI2_NONSYNC = 4,
+	RXPORT_MODE_LAST = RXPORT_MODE_CSI2_NONSYNC,
+};
+
+enum ub960_rxport_cdr {
+	RXPORT_CDR_FPD3 = 0,
+	RXPORT_CDR_FPD4 = 1,
+	RXPORT_CDR_LAST = RXPORT_CDR_FPD4,
+};
+
+struct ub960_rxport {
+	struct ub960_data      *priv;
+	u8                      nport;	/* RX port number, and index in priv->rxport[] */
+
+	struct {
+		struct v4l2_subdev *sd;
+		u16 pad;
+		struct fwnode_handle *ep_fwnode;
+	} source;
+
+	/* Serializer */
+	struct {
+		struct fwnode_handle *fwnode;
+		struct i2c_client *client;
+		unsigned short alias; /* I2C alias (lower 7 bits) */
+		struct ds90ub9xx_platform_data pdata;
+	} ser;
+
+	enum ub960_rxport_mode  rx_mode;
+	enum ub960_rxport_cdr	cdr_mode;
+
+	u8			lv_fv_pol;	/* LV and FV polarities */
+
+	struct regulator	*vpoc;
+
+	/* EQ settings */
+	struct {
+		bool manual_eq;
+
+		s8 strobe_pos;
+
+		union {
+			struct {
+				u8 eq_level_min;
+				u8 eq_level_max;
+			} aeq;
+
+			struct {
+				u8 eq_level;
+			} manual;
+		};
+	} eq;
+
+	const struct i2c_client *aliased_clients[UB960_MAX_PORT_ALIASES];
+};
+
+struct ub960_asd {
+	struct v4l2_async_connection base;
+	struct ub960_rxport *rxport;
+};
+
+static inline struct ub960_asd *to_ub960_asd(struct v4l2_async_connection *asd)
+{
+	return container_of(asd, struct ub960_asd, base);
+}
+
+struct ub960_txport {
+	struct ub960_data      *priv;
+	u8                      nport;	/* TX port number, and index in priv->txport[] */
+
+	u32 num_data_lanes;
+	bool non_continous_clk;
+};
+
+struct ub960_data {
+	const struct ub960_hw_data	*hw_data;
+	struct i2c_client	*client; /* for shared local registers */
+	struct regmap		*regmap;
+
+	/* lock for register access */
+	struct mutex		reg_lock;
+
+	struct clk		*refclk;
+
+	struct regulator	*vddio;
+
+	struct gpio_desc	*pd_gpio;
+	struct delayed_work	poll_work;
+	struct ub960_rxport	*rxports[UB960_MAX_RX_NPORTS];
+	struct ub960_txport	*txports[UB960_MAX_TX_NPORTS];
+
+	struct v4l2_subdev	sd;
+	struct media_pad	pads[UB960_MAX_NPORTS];
+
+	struct v4l2_ctrl_handler   ctrl_handler;
+	struct v4l2_async_notifier notifier;
+
+	u32 tx_data_rate;		/* Nominal data rate (Gb/s) */
+	s64 tx_link_freq[1];
+
+	struct i2c_atr *atr;
+
+	struct {
+		u8 rxport;
+		u8 txport;
+		u8 indirect_target;
+	} reg_current;
+
+	bool streaming;
+
+	u8 stored_fwd_ctl;
+
+	u64 stream_enable_mask[UB960_MAX_NPORTS];
+
+	/* These are common to all ports */
+	struct {
+		bool manual;
+
+		s8 min;
+		s8 max;
+	} strobe;
+};
+
+static inline struct ub960_data *sd_to_ub960(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct ub960_data, sd);
+}
+
+static inline bool ub960_pad_is_sink(struct ub960_data *priv, u32 pad)
+{
+	return pad < priv->hw_data->num_rxports;
+}
+
+static inline bool ub960_pad_is_source(struct ub960_data *priv, u32 pad)
+{
+	return pad >= priv->hw_data->num_rxports;
+}
+
+static inline unsigned int ub960_pad_to_port(struct ub960_data *priv, u32 pad)
+{
+	if (ub960_pad_is_sink(priv, pad))
+		return pad;
+	else
+		return pad - priv->hw_data->num_rxports;
+}
+
+struct ub960_format_info {
+	u32 code;
+	u32 bpp;
+	u8 datatype;
+	bool meta;
+};
+
+static const struct ub960_format_info ub960_formats[] = {
+	{ .code = MEDIA_BUS_FMT_YUYV8_1X16, .bpp = 16, .datatype = MIPI_CSI2_DT_YUV422_8B, },
+	{ .code = MEDIA_BUS_FMT_UYVY8_1X16, .bpp = 16, .datatype = MIPI_CSI2_DT_YUV422_8B, },
+	{ .code = MEDIA_BUS_FMT_VYUY8_1X16, .bpp = 16, .datatype = MIPI_CSI2_DT_YUV422_8B, },
+	{ .code = MEDIA_BUS_FMT_YVYU8_1X16, .bpp = 16, .datatype = MIPI_CSI2_DT_YUV422_8B, },
+
+	{ .code = MEDIA_BUS_FMT_SBGGR12_1X12, .bpp = 12, .datatype = MIPI_CSI2_DT_RAW12, },
+	{ .code = MEDIA_BUS_FMT_SGBRG12_1X12, .bpp = 12, .datatype = MIPI_CSI2_DT_RAW12, },
+	{ .code = MEDIA_BUS_FMT_SGRBG12_1X12, .bpp = 12, .datatype = MIPI_CSI2_DT_RAW12, },
+	{ .code = MEDIA_BUS_FMT_SRGGB12_1X12, .bpp = 12, .datatype = MIPI_CSI2_DT_RAW12, },
+};
+
+static const struct ub960_format_info *ub960_find_format(u32 code)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(ub960_formats); i++) {
+		if (ub960_formats[i].code == code)
+			return &ub960_formats[i];
+	}
+
+	return NULL;
+}
+
+/* -----------------------------------------------------------------------------
+ * Basic device access
+ */
+
+static int ub960_read(struct ub960_data *priv, u8 reg, u8 *val)
+{
+	struct device *dev = &priv->client->dev;
+	unsigned int v;
+	int ret;
+
+	mutex_lock(&priv->reg_lock);
+
+	ret = regmap_read(priv->regmap, reg, &v);
+	if (ret) {
+		dev_err(dev, "%s: cannot read register 0x%02x (%d)!\n",
+			__func__, reg, ret);
+		goto out_unlock;
+	}
+
+	*val = v;
+
+out_unlock:
+	mutex_unlock(&priv->reg_lock);
+
+	return ret;
+}
+
+static int ub960_write(struct ub960_data *priv, u8 reg, u8 val)
+{
+	struct device *dev = &priv->client->dev;
+	int ret;
+
+	mutex_lock(&priv->reg_lock);
+
+	ret = regmap_write(priv->regmap, reg, val);
+	if (ret)
+		dev_err(dev, "%s: cannot write register 0x%02x (%d)!\n",
+			__func__, reg, ret);
+
+	mutex_unlock(&priv->reg_lock);
+
+	return ret;
+}
+
+static int ub960_update_bits(struct ub960_data *priv, u8 reg, u8 mask, u8 val)
+{
+	struct device *dev = &priv->client->dev;
+	int ret;
+
+	mutex_lock(&priv->reg_lock);
+
+	ret = regmap_update_bits(priv->regmap, reg, mask, val);
+	if (ret)
+		dev_err(dev, "%s: cannot update register 0x%02x (%d)!\n",
+			__func__, reg, ret);
+
+	mutex_unlock(&priv->reg_lock);
+
+	return ret;
+}
+
+static int ub960_read16(struct ub960_data *priv, u8 reg, u16 *val)
+{
+	struct device *dev = &priv->client->dev;
+	__be16 __v;
+	int ret;
+
+	mutex_lock(&priv->reg_lock);
+
+	ret = regmap_bulk_read(priv->regmap, reg, &__v, sizeof(__v));
+	if (ret) {
+		dev_err(dev, "%s: cannot read register 0x%02x (%d)!\n",
+			__func__, reg, ret);
+		goto out_unlock;
+	}
+
+	*val = be16_to_cpu(__v);
+
+out_unlock:
+	mutex_unlock(&priv->reg_lock);
+
+	return ret;
+}
+
+static int ub960_rxport_select(struct ub960_data *priv, u8 nport)
+{
+	struct device *dev = &priv->client->dev;
+	int ret;
+
+	lockdep_assert_held(&priv->reg_lock);
+
+	if (priv->reg_current.rxport == nport)
+		return 0;
+
+	ret = regmap_write(priv->regmap, UB960_SR_FPD3_PORT_SEL,
+			   (nport << 4) | BIT(nport));
+	if (ret) {
+		dev_err(dev, "%s: cannot select rxport %d (%d)!\n", __func__,
+			nport, ret);
+		return ret;
+	}
+
+	priv->reg_current.rxport = nport;
+
+	return 0;
+}
+
+static int ub960_rxport_read(struct ub960_data *priv, u8 nport, u8 reg, u8 *val)
+{
+	struct device *dev = &priv->client->dev;
+	unsigned int v;
+	int ret;
+
+	mutex_lock(&priv->reg_lock);
+
+	ret = ub960_rxport_select(priv, nport);
+	if (ret)
+		goto out_unlock;
+
+	ret = regmap_read(priv->regmap, reg, &v);
+	if (ret) {
+		dev_err(dev, "%s: cannot read register 0x%02x (%d)!\n",
+			__func__, reg, ret);
+		goto out_unlock;
+	}
+
+	*val = v;
+
+out_unlock:
+	mutex_unlock(&priv->reg_lock);
+
+	return ret;
+}
+
+static int ub960_rxport_write(struct ub960_data *priv, u8 nport, u8 reg, u8 val)
+{
+	struct device *dev = &priv->client->dev;
+	int ret;
+
+	mutex_lock(&priv->reg_lock);
+
+	ret = ub960_rxport_select(priv, nport);
+	if (ret)
+		goto out_unlock;
+
+	ret = regmap_write(priv->regmap, reg, val);
+	if (ret)
+		dev_err(dev, "%s: cannot write register 0x%02x (%d)!\n",
+			__func__, reg, ret);
+
+out_unlock:
+	mutex_unlock(&priv->reg_lock);
+
+	return ret;
+}
+
+static int ub960_rxport_update_bits(struct ub960_data *priv, u8 nport, u8 reg,
+				    u8 mask, u8 val)
+{
+	struct device *dev = &priv->client->dev;
+	int ret;
+
+	mutex_lock(&priv->reg_lock);
+
+	ret = ub960_rxport_select(priv, nport);
+	if (ret)
+		goto out_unlock;
+
+	ret = regmap_update_bits(priv->regmap, reg, mask, val);
+	if (ret)
+		dev_err(dev, "%s: cannot update register 0x%02x (%d)!\n",
+			__func__, reg, ret);
+
+out_unlock:
+	mutex_unlock(&priv->reg_lock);
+
+	return ret;
+}
+
+static int ub960_rxport_read16(struct ub960_data *priv, u8 nport, u8 reg,
+			       u16 *val)
+{
+	struct device *dev = &priv->client->dev;
+	__be16 __v;
+	int ret;
+
+	mutex_lock(&priv->reg_lock);
+
+	ret = ub960_rxport_select(priv, nport);
+	if (ret)
+		goto out_unlock;
+
+	ret = regmap_bulk_read(priv->regmap, reg, &__v, sizeof(__v));
+	if (ret) {
+		dev_err(dev, "%s: cannot read register 0x%02x (%d)!\n",
+			__func__, reg, ret);
+		goto out_unlock;
+	}
+
+	*val = be16_to_cpu(__v);
+
+out_unlock:
+	mutex_unlock(&priv->reg_lock);
+
+	return ret;
+}
+
+static int ub960_txport_select(struct ub960_data *priv, u8 nport)
+{
+	struct device *dev = &priv->client->dev;
+	int ret;
+
+	lockdep_assert_held(&priv->reg_lock);
+
+	if (priv->reg_current.txport == nport)
+		return 0;
+
+	ret = regmap_write(priv->regmap, UB960_SR_CSI_PORT_SEL,
+			   (nport << 4) | BIT(nport));
+	if (ret) {
+		dev_err(dev, "%s: cannot select tx port %d (%d)!\n", __func__,
+			nport, ret);
+		return ret;
+	}
+
+	priv->reg_current.txport = nport;
+
+	return 0;
+}
+
+static int ub960_txport_read(struct ub960_data *priv, u8 nport, u8 reg, u8 *val)
+{
+	struct device *dev = &priv->client->dev;
+	unsigned int v;
+	int ret;
+
+	mutex_lock(&priv->reg_lock);
+
+	ret = ub960_txport_select(priv, nport);
+	if (ret)
+		goto out_unlock;
+
+	ret = regmap_read(priv->regmap, reg, &v);
+	if (ret) {
+		dev_err(dev, "%s: cannot read register 0x%02x (%d)!\n",
+			__func__, reg, ret);
+		goto out_unlock;
+	}
+
+	*val = v;
+
+out_unlock:
+	mutex_unlock(&priv->reg_lock);
+
+	return ret;
+}
+
+static int ub960_txport_write(struct ub960_data *priv, u8 nport, u8 reg, u8 val)
+{
+	struct device *dev = &priv->client->dev;
+	int ret;
+
+	mutex_lock(&priv->reg_lock);
+
+	ret = ub960_txport_select(priv, nport);
+	if (ret)
+		goto out_unlock;
+
+	ret = regmap_write(priv->regmap, reg, val);
+	if (ret)
+		dev_err(dev, "%s: cannot write register 0x%02x (%d)!\n",
+			__func__, reg, ret);
+
+out_unlock:
+	mutex_unlock(&priv->reg_lock);
+
+	return ret;
+}
+
+static int ub960_txport_update_bits(struct ub960_data *priv, u8 nport, u8 reg,
+				    u8 mask, u8 val)
+{
+	struct device *dev = &priv->client->dev;
+	int ret;
+
+	mutex_lock(&priv->reg_lock);
+
+	ret = ub960_txport_select(priv, nport);
+	if (ret)
+		goto out_unlock;
+
+	ret = regmap_update_bits(priv->regmap, reg, mask, val);
+	if (ret)
+		dev_err(dev, "%s: cannot update register 0x%02x (%d)!\n",
+			__func__, reg, ret);
+
+out_unlock:
+	mutex_unlock(&priv->reg_lock);
+
+	return ret;
+}
+
+static int ub960_select_ind_reg_block(struct ub960_data *priv, u8 block)
+{
+	struct device *dev = &priv->client->dev;
+	int ret;
+
+	lockdep_assert_held(&priv->reg_lock);
+
+	if (priv->reg_current.indirect_target == block)
+		return 0;
+
+	ret = regmap_write(priv->regmap, UB960_SR_IND_ACC_CTL, block << 2);
+	if (ret) {
+		dev_err(dev, "%s: cannot select indirect target %u (%d)!\n",
+			__func__, block, ret);
+		return ret;
+	}
+
+	priv->reg_current.indirect_target = block;
+
+	return 0;
+}
+
+static int ub960_read_ind(struct ub960_data *priv, u8 block, u8 reg, u8 *val)
+{
+	struct device *dev = &priv->client->dev;
+	unsigned int v;
+	int ret;
+
+	mutex_lock(&priv->reg_lock);
+
+	ret = ub960_select_ind_reg_block(priv, block);
+	if (ret)
+		goto out_unlock;
+
+	ret = regmap_write(priv->regmap, UB960_SR_IND_ACC_ADDR, reg);
+	if (ret) {
+		dev_err(dev,
+			"Write to IND_ACC_ADDR failed when reading %u:%x02x: %d\n",
+			block, reg, ret);
+		goto out_unlock;
+	}
+
+	ret = regmap_read(priv->regmap, UB960_SR_IND_ACC_DATA, &v);
+	if (ret) {
+		dev_err(dev,
+			"Write to IND_ACC_DATA failed when reading %u:%x02x: %d\n",
+			block, reg, ret);
+		goto out_unlock;
+	}
+
+	*val = v;
+
+out_unlock:
+	mutex_unlock(&priv->reg_lock);
+
+	return ret;
+}
+
+static int ub960_write_ind(struct ub960_data *priv, u8 block, u8 reg, u8 val)
+{
+	struct device *dev = &priv->client->dev;
+	int ret;
+
+	mutex_lock(&priv->reg_lock);
+
+	ret = ub960_select_ind_reg_block(priv, block);
+	if (ret)
+		goto out_unlock;
+
+	ret = regmap_write(priv->regmap, UB960_SR_IND_ACC_ADDR, reg);
+	if (ret) {
+		dev_err(dev,
+			"Write to IND_ACC_ADDR failed when writing %u:%x02x: %d\n",
+			block, reg, ret);
+		goto out_unlock;
+	}
+
+	ret = regmap_write(priv->regmap, UB960_SR_IND_ACC_DATA, val);
+	if (ret) {
+		dev_err(dev,
+			"Write to IND_ACC_DATA failed when writing %u:%x02x: %d\n",
+			block, reg, ret);
+		goto out_unlock;
+	}
+
+out_unlock:
+	mutex_unlock(&priv->reg_lock);
+
+	return ret;
+}
+
+static int ub960_ind_update_bits(struct ub960_data *priv, u8 block, u8 reg,
+				 u8 mask, u8 val)
+{
+	struct device *dev = &priv->client->dev;
+	int ret;
+
+	mutex_lock(&priv->reg_lock);
+
+	ret = ub960_select_ind_reg_block(priv, block);
+	if (ret)
+		goto out_unlock;
+
+	ret = regmap_write(priv->regmap, UB960_SR_IND_ACC_ADDR, reg);
+	if (ret) {
+		dev_err(dev,
+			"Write to IND_ACC_ADDR failed when updating %u:%x02x: %d\n",
+			block, reg, ret);
+		goto out_unlock;
+	}
+
+	ret = regmap_update_bits(priv->regmap, UB960_SR_IND_ACC_DATA, mask,
+				 val);
+	if (ret) {
+		dev_err(dev,
+			"Write to IND_ACC_DATA failed when updating %u:%x02x: %d\n",
+			block, reg, ret);
+		goto out_unlock;
+	}
+
+out_unlock:
+	mutex_unlock(&priv->reg_lock);
+
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * I2C-ATR (address translator)
+ */
+
+static int ub960_atr_attach_client(struct i2c_atr *atr, u32 chan_id,
+				   const struct i2c_client *client, u16 alias)
+{
+	struct ub960_data *priv = i2c_atr_get_driver_data(atr);
+	struct ub960_rxport *rxport = priv->rxports[chan_id];
+	struct device *dev = &priv->client->dev;
+	unsigned int reg_idx;
+
+	for (reg_idx = 0; reg_idx < ARRAY_SIZE(rxport->aliased_clients); reg_idx++) {
+		if (!rxport->aliased_clients[reg_idx])
+			break;
+	}
+
+	if (reg_idx == ARRAY_SIZE(rxport->aliased_clients)) {
+		dev_err(dev, "rx%u: alias pool exhausted\n", rxport->nport);
+		return -EADDRNOTAVAIL;
+	}
+
+	rxport->aliased_clients[reg_idx] = client;
+
+	ub960_rxport_write(priv, chan_id, UB960_RR_SLAVE_ID(reg_idx),
+			   client->addr << 1);
+	ub960_rxport_write(priv, chan_id, UB960_RR_SLAVE_ALIAS(reg_idx),
+			   alias << 1);
+
+	dev_dbg(dev, "rx%u: client 0x%02x assigned alias 0x%02x at slot %u\n",
+		rxport->nport, client->addr, alias, reg_idx);
+
+	return 0;
+}
+
+static void ub960_atr_detach_client(struct i2c_atr *atr, u32 chan_id,
+				    const struct i2c_client *client)
+{
+	struct ub960_data *priv = i2c_atr_get_driver_data(atr);
+	struct ub960_rxport *rxport = priv->rxports[chan_id];
+	struct device *dev = &priv->client->dev;
+	unsigned int reg_idx;
+
+	for (reg_idx = 0; reg_idx < ARRAY_SIZE(rxport->aliased_clients); reg_idx++) {
+		if (rxport->aliased_clients[reg_idx] == client)
+			break;
+	}
+
+	if (reg_idx == ARRAY_SIZE(rxport->aliased_clients)) {
+		dev_err(dev, "rx%u: client 0x%02x is not mapped!\n",
+			rxport->nport, client->addr);
+		return;
+	}
+
+	rxport->aliased_clients[reg_idx] = NULL;
+
+	ub960_rxport_write(priv, chan_id, UB960_RR_SLAVE_ALIAS(reg_idx), 0);
+
+	dev_dbg(dev, "rx%u: client 0x%02x released at slot %u\n", rxport->nport,
+		client->addr, reg_idx);
+}
+
+static const struct i2c_atr_ops ub960_atr_ops = {
+	.attach_client = ub960_atr_attach_client,
+	.detach_client = ub960_atr_detach_client,
+};
+
+static int ub960_init_atr(struct ub960_data *priv)
+{
+	struct device *dev = &priv->client->dev;
+	struct i2c_adapter *parent_adap = priv->client->adapter;
+
+	priv->atr = i2c_atr_new(parent_adap, dev, &ub960_atr_ops,
+				priv->hw_data->num_rxports);
+	if (IS_ERR(priv->atr))
+		return PTR_ERR(priv->atr);
+
+	i2c_atr_set_driver_data(priv->atr, priv);
+
+	return 0;
+}
+
+static void ub960_uninit_atr(struct ub960_data *priv)
+{
+	i2c_atr_delete(priv->atr);
+	priv->atr = NULL;
+}
+
+/* -----------------------------------------------------------------------------
+ * TX ports
+ */
+
+static int ub960_parse_dt_txport(struct ub960_data *priv,
+				 struct fwnode_handle *ep_fwnode,
+				 u8 nport)
+{
+	struct device *dev = &priv->client->dev;
+	struct v4l2_fwnode_endpoint vep = {};
+	struct ub960_txport *txport;
+	int ret;
+
+	txport = kzalloc(sizeof(*txport), GFP_KERNEL);
+	if (!txport)
+		return -ENOMEM;
+
+	txport->priv = priv;
+	txport->nport = nport;
+
+	vep.bus_type = V4L2_MBUS_CSI2_DPHY;
+	ret = v4l2_fwnode_endpoint_alloc_parse(ep_fwnode, &vep);
+	if (ret) {
+		dev_err(dev, "tx%u: failed to parse endpoint data\n", nport);
+		goto err_free_txport;
+	}
+
+	txport->non_continous_clk = vep.bus.mipi_csi2.flags &
+				    V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK;
+
+	txport->num_data_lanes = vep.bus.mipi_csi2.num_data_lanes;
+
+	if (vep.nr_of_link_frequencies != 1) {
+		ret = -EINVAL;
+		goto err_free_vep;
+	}
+
+	priv->tx_link_freq[0] = vep.link_frequencies[0];
+	priv->tx_data_rate = priv->tx_link_freq[0] * 2;
+
+	if (priv->tx_data_rate != MHZ(1600) &&
+	    priv->tx_data_rate != MHZ(1200) &&
+	    priv->tx_data_rate != MHZ(800) &&
+	    priv->tx_data_rate != MHZ(400)) {
+		dev_err(dev, "tx%u: invalid 'link-frequencies' value\n", nport);
+		ret = -EINVAL;
+		goto err_free_vep;
+	}
+
+	v4l2_fwnode_endpoint_free(&vep);
+
+	priv->txports[nport] = txport;
+
+	return 0;
+
+err_free_vep:
+	v4l2_fwnode_endpoint_free(&vep);
+err_free_txport:
+	kfree(txport);
+
+	return ret;
+}
+
+static void ub960_csi_handle_events(struct ub960_data *priv, u8 nport)
+{
+	struct device *dev = &priv->client->dev;
+	u8 csi_tx_isr;
+	int ret;
+
+	ret = ub960_txport_read(priv, nport, UB960_TR_CSI_TX_ISR, &csi_tx_isr);
+	if (ret)
+		return;
+
+	if (csi_tx_isr & UB960_TR_CSI_TX_ISR_IS_CSI_SYNC_ERROR)
+		dev_warn(dev, "TX%u: CSI_SYNC_ERROR\n", nport);
+
+	if (csi_tx_isr & UB960_TR_CSI_TX_ISR_IS_CSI_PASS_ERROR)
+		dev_warn(dev, "TX%u: CSI_PASS_ERROR\n", nport);
+}
+
+/* -----------------------------------------------------------------------------
+ * RX ports
+ */
+
+static int ub960_rxport_enable_vpocs(struct ub960_data *priv)
+{
+	unsigned int nport;
+	int ret;
+
+	for (nport = 0; nport < priv->hw_data->num_rxports; nport++) {
+		struct ub960_rxport *rxport = priv->rxports[nport];
+
+		if (!rxport || !rxport->vpoc)
+			continue;
+
+		ret = regulator_enable(rxport->vpoc);
+		if (ret)
+			goto err_disable_vpocs;
+	}
+
+	return 0;
+
+err_disable_vpocs:
+	while (nport--) {
+		struct ub960_rxport *rxport = priv->rxports[nport];
+
+		if (!rxport || !rxport->vpoc)
+			continue;
+
+		regulator_disable(rxport->vpoc);
+	}
+
+	return ret;
+}
+
+static void ub960_rxport_disable_vpocs(struct ub960_data *priv)
+{
+	unsigned int nport;
+
+	for (nport = 0; nport < priv->hw_data->num_rxports; nport++) {
+		struct ub960_rxport *rxport = priv->rxports[nport];
+
+		if (!rxport || !rxport->vpoc)
+			continue;
+
+		regulator_disable(rxport->vpoc);
+	}
+}
+
+static void ub960_rxport_clear_errors(struct ub960_data *priv,
+				      unsigned int nport)
+{
+	u8 v;
+
+	ub960_rxport_read(priv, nport, UB960_RR_RX_PORT_STS1, &v);
+	ub960_rxport_read(priv, nport, UB960_RR_RX_PORT_STS2, &v);
+	ub960_rxport_read(priv, nport, UB960_RR_CSI_RX_STS, &v);
+	ub960_rxport_read(priv, nport, UB960_RR_BCC_STATUS, &v);
+
+	ub960_rxport_read(priv, nport, UB960_RR_RX_PAR_ERR_HI, &v);
+	ub960_rxport_read(priv, nport, UB960_RR_RX_PAR_ERR_LO, &v);
+
+	ub960_rxport_read(priv, nport, UB960_RR_CSI_ERR_COUNTER, &v);
+}
+
+static void ub960_clear_rx_errors(struct ub960_data *priv)
+{
+	unsigned int nport;
+
+	for (nport = 0; nport < priv->hw_data->num_rxports; nport++)
+		ub960_rxport_clear_errors(priv, nport);
+}
+
+static int ub960_rxport_get_strobe_pos(struct ub960_data *priv,
+				       unsigned int nport, s8 *strobe_pos)
+{
+	u8 v;
+	u8 clk_delay, data_delay;
+	int ret;
+
+	ub960_read_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
+		       UB960_IR_RX_ANA_STROBE_SET_CLK, &v);
+
+	clk_delay = (v & UB960_IR_RX_ANA_STROBE_SET_CLK_NO_EXTRA_DELAY) ?
+			    0 : UB960_MANUAL_STROBE_EXTRA_DELAY;
+
+	ub960_read_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
+		       UB960_IR_RX_ANA_STROBE_SET_DATA, &v);
+
+	data_delay = (v & UB960_IR_RX_ANA_STROBE_SET_DATA_NO_EXTRA_DELAY) ?
+			     0 : UB960_MANUAL_STROBE_EXTRA_DELAY;
+
+	ret = ub960_rxport_read(priv, nport, UB960_RR_SFILTER_STS_0, &v);
+	if (ret)
+		return ret;
+
+	clk_delay += v & UB960_IR_RX_ANA_STROBE_SET_CLK_DELAY_MASK;
+
+	ub960_rxport_read(priv, nport, UB960_RR_SFILTER_STS_1, &v);
+	if (ret)
+		return ret;
+
+	data_delay += v & UB960_IR_RX_ANA_STROBE_SET_DATA_DELAY_MASK;
+
+	*strobe_pos = data_delay - clk_delay;
+
+	return 0;
+}
+
+static void ub960_rxport_set_strobe_pos(struct ub960_data *priv,
+					unsigned int nport, s8 strobe_pos)
+{
+	u8 clk_delay, data_delay;
+
+	clk_delay = UB960_IR_RX_ANA_STROBE_SET_CLK_NO_EXTRA_DELAY;
+	data_delay = UB960_IR_RX_ANA_STROBE_SET_DATA_NO_EXTRA_DELAY;
+
+	if (strobe_pos < UB960_MIN_AEQ_STROBE_POS)
+		clk_delay = abs(strobe_pos) - UB960_MANUAL_STROBE_EXTRA_DELAY;
+	else if (strobe_pos > UB960_MAX_AEQ_STROBE_POS)
+		data_delay = strobe_pos - UB960_MANUAL_STROBE_EXTRA_DELAY;
+	else if (strobe_pos < 0)
+		clk_delay = abs(strobe_pos) | UB960_IR_RX_ANA_STROBE_SET_CLK_NO_EXTRA_DELAY;
+	else if (strobe_pos > 0)
+		data_delay = strobe_pos | UB960_IR_RX_ANA_STROBE_SET_DATA_NO_EXTRA_DELAY;
+
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
+			UB960_IR_RX_ANA_STROBE_SET_CLK, clk_delay);
+
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
+			UB960_IR_RX_ANA_STROBE_SET_DATA, data_delay);
+}
+
+static void ub960_rxport_set_strobe_range(struct ub960_data *priv,
+					  s8 strobe_min, s8 strobe_max)
+{
+	/* Convert the signed strobe pos to positive zero based value */
+	strobe_min -= UB960_MIN_AEQ_STROBE_POS;
+	strobe_max -= UB960_MIN_AEQ_STROBE_POS;
+
+	ub960_write(priv, UB960_XR_SFILTER_CFG,
+		    ((u8)strobe_min << UB960_XR_SFILTER_CFG_SFILTER_MIN_SHIFT) |
+		    ((u8)strobe_max << UB960_XR_SFILTER_CFG_SFILTER_MAX_SHIFT));
+}
+
+static int ub960_rxport_get_eq_level(struct ub960_data *priv,
+				     unsigned int nport, u8 *eq_level)
+{
+	int ret;
+	u8 v;
+
+	ret = ub960_rxport_read(priv, nport, UB960_RR_AEQ_STATUS, &v);
+	if (ret)
+		return ret;
+
+	*eq_level = (v & UB960_RR_AEQ_STATUS_STATUS_1) +
+		    (v & UB960_RR_AEQ_STATUS_STATUS_2);
+
+	return 0;
+}
+
+static void ub960_rxport_set_eq_level(struct ub960_data *priv,
+				      unsigned int nport, u8 eq_level)
+{
+	u8 eq_stage_1_select_value, eq_stage_2_select_value;
+	const unsigned int eq_stage_max = 7;
+	u8 v;
+
+	if (eq_level <= eq_stage_max) {
+		eq_stage_1_select_value = eq_level;
+		eq_stage_2_select_value = 0;
+	} else {
+		eq_stage_1_select_value = eq_stage_max;
+		eq_stage_2_select_value = eq_level - eq_stage_max;
+	}
+
+	ub960_rxport_read(priv, nport, UB960_RR_AEQ_BYPASS, &v);
+
+	v &= ~(UB960_RR_AEQ_BYPASS_EQ_STAGE1_VALUE_MASK |
+	       UB960_RR_AEQ_BYPASS_EQ_STAGE2_VALUE_MASK);
+	v |= eq_stage_1_select_value << UB960_RR_AEQ_BYPASS_EQ_STAGE1_VALUE_SHIFT;
+	v |= eq_stage_2_select_value << UB960_RR_AEQ_BYPASS_EQ_STAGE2_VALUE_SHIFT;
+	v |= UB960_RR_AEQ_BYPASS_ENABLE;
+
+	ub960_rxport_write(priv, nport, UB960_RR_AEQ_BYPASS, v);
+}
+
+static void ub960_rxport_set_eq_range(struct ub960_data *priv,
+				      unsigned int nport, u8 eq_min, u8 eq_max)
+{
+	ub960_rxport_write(priv, nport, UB960_RR_AEQ_MIN_MAX,
+			   (eq_min << UB960_RR_AEQ_MIN_MAX_AEQ_FLOOR_SHIFT) |
+			   (eq_max << UB960_RR_AEQ_MIN_MAX_AEQ_MAX_SHIFT));
+
+	/* Enable AEQ min setting */
+	ub960_rxport_update_bits(priv, nport, UB960_RR_AEQ_CTL2,
+				 UB960_RR_AEQ_CTL2_SET_AEQ_FLOOR,
+				 UB960_RR_AEQ_CTL2_SET_AEQ_FLOOR);
+}
+
+static void ub960_rxport_config_eq(struct ub960_data *priv, unsigned int nport)
+{
+	struct ub960_rxport *rxport = priv->rxports[nport];
+
+	/* We also set common settings here. Should be moved elsewhere. */
+
+	if (priv->strobe.manual) {
+		/* Disable AEQ_SFILTER_EN */
+		ub960_update_bits(priv, UB960_XR_AEQ_CTL1,
+				  UB960_XR_AEQ_CTL1_AEQ_SFILTER_EN, 0);
+	} else {
+		/* Enable SFILTER and error control */
+		ub960_write(priv, UB960_XR_AEQ_CTL1,
+			    UB960_XR_AEQ_CTL1_AEQ_ERR_CTL_MASK |
+				    UB960_XR_AEQ_CTL1_AEQ_SFILTER_EN);
+
+		/* Set AEQ strobe range */
+		ub960_rxport_set_strobe_range(priv, priv->strobe.min,
+					      priv->strobe.max);
+	}
+
+	/* The rest are port specific */
+
+	if (priv->strobe.manual)
+		ub960_rxport_set_strobe_pos(priv, nport, rxport->eq.strobe_pos);
+	else
+		ub960_rxport_set_strobe_pos(priv, nport, 0);
+
+	if (rxport->eq.manual_eq) {
+		ub960_rxport_set_eq_level(priv, nport,
+					  rxport->eq.manual.eq_level);
+
+		/* Enable AEQ Bypass */
+		ub960_rxport_update_bits(priv, nport, UB960_RR_AEQ_BYPASS,
+					 UB960_RR_AEQ_BYPASS_ENABLE,
+					 UB960_RR_AEQ_BYPASS_ENABLE);
+	} else {
+		ub960_rxport_set_eq_range(priv, nport,
+					  rxport->eq.aeq.eq_level_min,
+					  rxport->eq.aeq.eq_level_max);
+
+		/* Disable AEQ Bypass */
+		ub960_rxport_update_bits(priv, nport, UB960_RR_AEQ_BYPASS,
+					 UB960_RR_AEQ_BYPASS_ENABLE, 0);
+	}
+}
+
+static int ub960_rxport_link_ok(struct ub960_data *priv, unsigned int nport,
+				bool *ok)
+{
+	u8 rx_port_sts1, rx_port_sts2;
+	u16 parity_errors;
+	u8 csi_rx_sts;
+	u8 csi_err_cnt;
+	u8 bcc_sts;
+	int ret;
+	bool errors;
+
+	ret = ub960_rxport_read(priv, nport, UB960_RR_RX_PORT_STS1,
+				&rx_port_sts1);
+	if (ret)
+		return ret;
+
+	if (!(rx_port_sts1 & UB960_RR_RX_PORT_STS1_LOCK_STS)) {
+		*ok = false;
+		return 0;
+	}
+
+	ret = ub960_rxport_read(priv, nport, UB960_RR_RX_PORT_STS2,
+				&rx_port_sts2);
+	if (ret)
+		return ret;
+
+	ret = ub960_rxport_read(priv, nport, UB960_RR_CSI_RX_STS, &csi_rx_sts);
+	if (ret)
+		return ret;
+
+	ret = ub960_rxport_read(priv, nport, UB960_RR_CSI_ERR_COUNTER,
+				&csi_err_cnt);
+	if (ret)
+		return ret;
+
+	ret = ub960_rxport_read(priv, nport, UB960_RR_BCC_STATUS, &bcc_sts);
+	if (ret)
+		return ret;
+
+	ret = ub960_rxport_read16(priv, nport, UB960_RR_RX_PAR_ERR_HI,
+				  &parity_errors);
+	if (ret)
+		return ret;
+
+	errors = (rx_port_sts1 & UB960_RR_RX_PORT_STS1_ERROR_MASK) ||
+		 (rx_port_sts2 & UB960_RR_RX_PORT_STS2_ERROR_MASK) ||
+		 (bcc_sts & UB960_RR_BCC_STATUS_ERROR_MASK) ||
+		 (csi_rx_sts & UB960_RR_CSI_RX_STS_ERROR_MASK) || csi_err_cnt ||
+		 parity_errors;
+
+	*ok = !errors;
+
+	return 0;
+}
+
+/*
+ * Wait for the RX ports to lock, have no errors and have stable strobe position
+ * and EQ level.
+ */
+static int ub960_rxport_wait_locks(struct ub960_data *priv,
+				   unsigned long port_mask,
+				   unsigned int *lock_mask)
+{
+	struct device *dev = &priv->client->dev;
+	unsigned long timeout;
+	unsigned int link_ok_mask;
+	unsigned int missing;
+	unsigned int loops;
+	u8 nport;
+	int ret;
+
+	if (port_mask == 0) {
+		if (lock_mask)
+			*lock_mask = 0;
+		return 0;
+	}
+
+	if (port_mask >= BIT(priv->hw_data->num_rxports))
+		return -EINVAL;
+
+	timeout = jiffies + msecs_to_jiffies(1000);
+	loops = 0;
+	link_ok_mask = 0;
+
+	while (time_before(jiffies, timeout)) {
+		missing = 0;
+
+		for_each_set_bit(nport, &port_mask,
+				 priv->hw_data->num_rxports) {
+			struct ub960_rxport *rxport = priv->rxports[nport];
+			bool ok;
+
+			if (!rxport)
+				continue;
+
+			ret = ub960_rxport_link_ok(priv, nport, &ok);
+			if (ret)
+				return ret;
+
+			/*
+			 * We want the link to be ok for two consecutive loops,
+			 * as a link could get established just before our test
+			 * and drop soon after.
+			 */
+			if (!ok || !(link_ok_mask & BIT(nport)))
+				missing++;
+
+			if (ok)
+				link_ok_mask |= BIT(nport);
+			else
+				link_ok_mask &= ~BIT(nport);
+		}
+
+		loops++;
+
+		if (missing == 0)
+			break;
+
+		msleep(50);
+	}
+
+	if (lock_mask)
+		*lock_mask = link_ok_mask;
+
+	dev_dbg(dev, "Wait locks done in %u loops\n", loops);
+	for_each_set_bit(nport, &port_mask, priv->hw_data->num_rxports) {
+		struct ub960_rxport *rxport = priv->rxports[nport];
+		s8 strobe_pos, eq_level;
+		u16 v;
+
+		if (!rxport)
+			continue;
+
+		if (!(link_ok_mask & BIT(nport))) {
+			dev_dbg(dev, "\trx%u: not locked\n", nport);
+			continue;
+		}
+
+		ub960_rxport_read16(priv, nport, UB960_RR_RX_FREQ_HIGH, &v);
+
+		ret = ub960_rxport_get_strobe_pos(priv, nport, &strobe_pos);
+		if (ret)
+			return ret;
+
+		ret = ub960_rxport_get_eq_level(priv, nport, &eq_level);
+		if (ret)
+			return ret;
+
+		dev_dbg(dev, "\trx%u: locked, SP: %d, EQ: %u, freq %llu Hz\n",
+			nport, strobe_pos, eq_level, (v * 1000000ULL) >> 8);
+	}
+
+	return 0;
+}
+
+static unsigned long ub960_calc_bc_clk_rate_ub960(struct ub960_data *priv,
+						  struct ub960_rxport *rxport)
+{
+	unsigned int mult;
+	unsigned int div;
+
+	switch (rxport->rx_mode) {
+	case RXPORT_MODE_RAW10:
+	case RXPORT_MODE_RAW12_HF:
+	case RXPORT_MODE_RAW12_LF:
+		mult = 1;
+		div = 10;
+		break;
+
+	case RXPORT_MODE_CSI2_SYNC:
+		mult = 2;
+		div = 1;
+		break;
+
+	case RXPORT_MODE_CSI2_NONSYNC:
+		mult = 2;
+		div = 5;
+		break;
+
+	default:
+		return 0;
+	}
+
+	return clk_get_rate(priv->refclk) * mult / div;
+}
+
+static unsigned long ub960_calc_bc_clk_rate_ub9702(struct ub960_data *priv,
+						   struct ub960_rxport *rxport)
+{
+	switch (rxport->rx_mode) {
+	case RXPORT_MODE_RAW10:
+	case RXPORT_MODE_RAW12_HF:
+	case RXPORT_MODE_RAW12_LF:
+		return 2359400;
+
+	case RXPORT_MODE_CSI2_SYNC:
+		return 47187500;
+
+	case RXPORT_MODE_CSI2_NONSYNC:
+		return 9437500;
+
+	default:
+		return 0;
+	}
+}
+
+static int ub960_rxport_add_serializer(struct ub960_data *priv, u8 nport)
+{
+	struct ub960_rxport *rxport = priv->rxports[nport];
+	struct device *dev = &priv->client->dev;
+	struct ds90ub9xx_platform_data *ser_pdata = &rxport->ser.pdata;
+	struct i2c_board_info ser_info = {
+		.of_node = to_of_node(rxport->ser.fwnode),
+		.fwnode = rxport->ser.fwnode,
+		.platform_data = ser_pdata,
+	};
+
+	ser_pdata->port = nport;
+	ser_pdata->atr = priv->atr;
+	if (priv->hw_data->is_ub9702)
+		ser_pdata->bc_rate = ub960_calc_bc_clk_rate_ub9702(priv, rxport);
+	else
+		ser_pdata->bc_rate = ub960_calc_bc_clk_rate_ub960(priv, rxport);
+
+	/*
+	 * The serializer is added under the same i2c adapter as the
+	 * deserializer. This is not quite right, as the serializer is behind
+	 * the FPD-Link.
+	 */
+	ser_info.addr = rxport->ser.alias;
+	rxport->ser.client =
+		i2c_new_client_device(priv->client->adapter, &ser_info);
+	if (IS_ERR(rxport->ser.client)) {
+		dev_err(dev, "rx%u: cannot add %s i2c device", nport,
+			ser_info.type);
+		return PTR_ERR(rxport->ser.client);
+	}
+
+	dev_dbg(dev, "rx%u: remote serializer at alias 0x%02x (%u-%04x)\n",
+		nport, rxport->ser.client->addr,
+		rxport->ser.client->adapter->nr, rxport->ser.client->addr);
+
+	return 0;
+}
+
+static void ub960_rxport_remove_serializer(struct ub960_data *priv, u8 nport)
+{
+	struct ub960_rxport *rxport = priv->rxports[nport];
+
+	i2c_unregister_device(rxport->ser.client);
+	rxport->ser.client = NULL;
+}
+
+/* Add serializer i2c devices for all initialized ports */
+static int ub960_rxport_add_serializers(struct ub960_data *priv)
+{
+	unsigned int nport;
+	int ret;
+
+	for (nport = 0; nport < priv->hw_data->num_rxports; nport++) {
+		struct ub960_rxport *rxport = priv->rxports[nport];
+
+		if (!rxport)
+			continue;
+
+		ret = ub960_rxport_add_serializer(priv, nport);
+		if (ret)
+			goto err_remove_sers;
+	}
+
+	return 0;
+
+err_remove_sers:
+	while (nport--) {
+		struct ub960_rxport *rxport = priv->rxports[nport];
+
+		if (!rxport)
+			continue;
+
+		ub960_rxport_remove_serializer(priv, nport);
+	}
+
+	return ret;
+}
+
+static void ub960_rxport_remove_serializers(struct ub960_data *priv)
+{
+	unsigned int nport;
+
+	for (nport = 0; nport < priv->hw_data->num_rxports; nport++) {
+		struct ub960_rxport *rxport = priv->rxports[nport];
+
+		if (!rxport)
+			continue;
+
+		ub960_rxport_remove_serializer(priv, nport);
+	}
+}
+
+static void ub960_init_tx_port(struct ub960_data *priv,
+			       struct ub960_txport *txport)
+{
+	unsigned int nport = txport->nport;
+	u8 csi_ctl = 0;
+
+	/*
+	 * From the datasheet: "initial CSI Skew-Calibration
+	 * sequence [...] should be set when operating at 1.6 Gbps"
+	 */
+	if (priv->tx_data_rate == MHZ(1600))
+		csi_ctl |= UB960_TR_CSI_CTL_CSI_CAL_EN;
+
+	csi_ctl |= (4 - txport->num_data_lanes) << 4;
+
+	if (!txport->non_continous_clk)
+		csi_ctl |= UB960_TR_CSI_CTL_CSI_CONTS_CLOCK;
+
+	ub960_txport_write(priv, nport, UB960_TR_CSI_CTL, csi_ctl);
+}
+
+static int ub960_init_tx_ports(struct ub960_data *priv)
+{
+	unsigned int nport;
+	u8 speed_select;
+	u8 pll_div;
+
+	/* TX ports */
+
+	switch (priv->tx_data_rate) {
+	case MHZ(1600):
+	default:
+		speed_select = 0;
+		pll_div = 0x10;
+		break;
+	case MHZ(1200):
+		speed_select = 1;
+		pll_div = 0x18;
+		break;
+	case MHZ(800):
+		speed_select = 2;
+		pll_div = 0x10;
+		break;
+	case MHZ(400):
+		speed_select = 3;
+		pll_div = 0x10;
+		break;
+	}
+
+	ub960_write(priv, UB960_SR_CSI_PLL_CTL, speed_select);
+
+	if (priv->hw_data->is_ub9702) {
+		ub960_write(priv, UB960_SR_CSI_PLL_DIV, pll_div);
+
+		switch (priv->tx_data_rate) {
+		case MHZ(1600):
+		default:
+			ub960_write_ind(priv, UB960_IND_TARGET_CSI_ANA, 0x92, 0x80);
+			ub960_write_ind(priv, UB960_IND_TARGET_CSI_ANA, 0x4b, 0x2a);
+			break;
+		case MHZ(800):
+			ub960_write_ind(priv, UB960_IND_TARGET_CSI_ANA, 0x92, 0x90);
+			ub960_write_ind(priv, UB960_IND_TARGET_CSI_ANA, 0x4f, 0x2a);
+			ub960_write_ind(priv, UB960_IND_TARGET_CSI_ANA, 0x4b, 0x2a);
+			break;
+		case MHZ(400):
+			ub960_write_ind(priv, UB960_IND_TARGET_CSI_ANA, 0x92, 0xa0);
+			break;
+		}
+	}
+
+	for (nport = 0; nport < priv->hw_data->num_txports; nport++) {
+		struct ub960_txport *txport = priv->txports[nport];
+
+		if (!txport)
+			continue;
+
+		ub960_init_tx_port(priv, txport);
+	}
+
+	return 0;
+}
+
+static void ub960_init_rx_port_ub960(struct ub960_data *priv,
+				     struct ub960_rxport *rxport)
+{
+	unsigned int nport = rxport->nport;
+	u32 bc_freq_val;
+
+	/*
+	 * Back channel frequency select.
+	 * Override FREQ_SELECT from the strap.
+	 * 0 - 2.5 Mbps (DS90UB913A-Q1 / DS90UB933-Q1)
+	 * 2 - 10 Mbps
+	 * 6 - 50 Mbps (DS90UB953-Q1)
+	 *
+	 * Note that changing this setting will result in some errors on the back
+	 * channel for a short period of time.
+	 */
+
+	switch (rxport->rx_mode) {
+	case RXPORT_MODE_RAW10:
+	case RXPORT_MODE_RAW12_HF:
+	case RXPORT_MODE_RAW12_LF:
+		bc_freq_val = 0;
+		break;
+
+	case RXPORT_MODE_CSI2_NONSYNC:
+		bc_freq_val = 2;
+		break;
+
+	case RXPORT_MODE_CSI2_SYNC:
+		bc_freq_val = 6;
+		break;
+
+	default:
+		return;
+	}
+
+	ub960_rxport_update_bits(priv, nport, UB960_RR_BCC_CONFIG,
+				 UB960_RR_BCC_CONFIG_BC_FREQ_SEL_MASK,
+				 bc_freq_val);
+
+	switch (rxport->rx_mode) {
+	case RXPORT_MODE_RAW10:
+		/* FPD3_MODE = RAW10 Mode (DS90UB913A-Q1 / DS90UB933-Q1 compatible) */
+		ub960_rxport_update_bits(priv, nport, UB960_RR_PORT_CONFIG,
+					 UB960_RR_PORT_CONFIG_FPD3_MODE_MASK,
+					 0x3);
+
+		/*
+		 * RAW10_8BIT_CTL = 0b10 : 8-bit processing using upper 8 bits
+		 */
+		ub960_rxport_update_bits(priv, nport, UB960_RR_PORT_CONFIG2,
+			UB960_RR_PORT_CONFIG2_RAW10_8BIT_CTL_MASK,
+			0x2 << UB960_RR_PORT_CONFIG2_RAW10_8BIT_CTL_SHIFT);
+
+		break;
+
+	case RXPORT_MODE_RAW12_HF:
+	case RXPORT_MODE_RAW12_LF:
+		/* Not implemented */
+		return;
+
+	case RXPORT_MODE_CSI2_SYNC:
+	case RXPORT_MODE_CSI2_NONSYNC:
+		/* CSI-2 Mode (DS90UB953-Q1 compatible) */
+		ub960_rxport_update_bits(priv, nport, UB960_RR_PORT_CONFIG, 0x3,
+					 0x0);
+
+		break;
+	}
+
+	/* LV_POLARITY & FV_POLARITY */
+	ub960_rxport_update_bits(priv, nport, UB960_RR_PORT_CONFIG2, 0x3,
+				 rxport->lv_fv_pol);
+
+	/* Enable all interrupt sources from this port */
+	ub960_rxport_write(priv, nport, UB960_RR_PORT_ICR_HI, 0x07);
+	ub960_rxport_write(priv, nport, UB960_RR_PORT_ICR_LO, 0x7f);
+
+	/* Enable I2C_PASS_THROUGH */
+	ub960_rxport_update_bits(priv, nport, UB960_RR_BCC_CONFIG,
+				 UB960_RR_BCC_CONFIG_I2C_PASS_THROUGH,
+				 UB960_RR_BCC_CONFIG_I2C_PASS_THROUGH);
+
+	/* Enable I2C communication to the serializer via the alias addr */
+	ub960_rxport_write(priv, nport, UB960_RR_SER_ALIAS_ID,
+			   rxport->ser.alias << 1);
+
+	/* Configure EQ related settings */
+	ub960_rxport_config_eq(priv, nport);
+
+	/* Enable RX port */
+	ub960_update_bits(priv, UB960_SR_RX_PORT_CTL, BIT(nport), BIT(nport));
+}
+
+static void ub960_init_rx_port_ub9702_fpd3(struct ub960_data *priv,
+					   struct ub960_rxport *rxport)
+{
+	unsigned int nport = rxport->nport;
+	u8 bc_freq_val;
+	u8 fpd_func_mode;
+
+	switch (rxport->rx_mode) {
+	case RXPORT_MODE_RAW10:
+		bc_freq_val = 0;
+		fpd_func_mode = 5;
+		break;
+
+	case RXPORT_MODE_RAW12_HF:
+		bc_freq_val = 0;
+		fpd_func_mode = 4;
+		break;
+
+	case RXPORT_MODE_RAW12_LF:
+		bc_freq_val = 0;
+		fpd_func_mode = 6;
+		break;
+
+	case RXPORT_MODE_CSI2_SYNC:
+		bc_freq_val = 6;
+		fpd_func_mode = 2;
+		break;
+
+	case RXPORT_MODE_CSI2_NONSYNC:
+		bc_freq_val = 2;
+		fpd_func_mode = 2;
+		break;
+
+	default:
+		return;
+	}
+
+	ub960_rxport_update_bits(priv, nport, UB960_RR_BCC_CONFIG, 0x7,
+				 bc_freq_val);
+	ub960_rxport_write(priv, nport, UB960_RR_CHANNEL_MODE, fpd_func_mode);
+
+	/* set serdes_eq_mode = 1 */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0xa8, 0x80);
+
+	/* enable serdes driver */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x0d, 0x7f);
+
+	/* set serdes_eq_offset=4 */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x2b, 0x04);
+
+	/* init default serdes_eq_max in 0xa9 */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0xa9, 0x23);
+
+	/* init serdes_eq_min in 0xaa */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0xaa, 0);
+
+	/* serdes_driver_ctl2 control: DS90UB953-Q1/DS90UB933-Q1/DS90UB913A-Q1 */
+	ub960_ind_update_bits(priv, UB960_IND_TARGET_RX_ANA(nport), 0x1b,
+			      BIT(3), BIT(3));
+
+	/* RX port to half-rate */
+	ub960_update_bits(priv, UB960_SR_FPD_RATE_CFG, 0x3 << (nport * 2),
+			  BIT(nport * 2));
+}
+
+static void ub960_init_rx_port_ub9702_fpd4_aeq(struct ub960_data *priv,
+					       struct ub960_rxport *rxport)
+{
+	unsigned int nport = rxport->nport;
+	bool first_time_power_up = true;
+
+	if (first_time_power_up) {
+		u8 v;
+
+		/* AEQ init */
+		ub960_read_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x2c, &v);
+
+		ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x27, v);
+		ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x28, v + 1);
+
+		ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x2b, 0x00);
+	}
+
+	/* enable serdes_eq_ctl2 */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x9e, 0x00);
+
+	/* enable serdes_eq_ctl1 */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x90, 0x40);
+
+	/* enable serdes_eq_en */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x2e, 0x40);
+
+	/* disable serdes_eq_override */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0xf0, 0x00);
+
+	/* disable serdes_gain_override */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x71, 0x00);
+}
+
+static void ub960_init_rx_port_ub9702_fpd4(struct ub960_data *priv,
+					   struct ub960_rxport *rxport)
+{
+	unsigned int nport = rxport->nport;
+	u8 bc_freq_val;
+
+	switch (rxport->rx_mode) {
+	case RXPORT_MODE_RAW10:
+		bc_freq_val = 0;
+		break;
+
+	case RXPORT_MODE_RAW12_HF:
+		bc_freq_val = 0;
+		break;
+
+	case RXPORT_MODE_RAW12_LF:
+		bc_freq_val = 0;
+		break;
+
+	case RXPORT_MODE_CSI2_SYNC:
+		bc_freq_val = 6;
+		break;
+
+	case RXPORT_MODE_CSI2_NONSYNC:
+		bc_freq_val = 2;
+		break;
+
+	default:
+		return;
+	}
+
+	ub960_rxport_update_bits(priv, nport, UB960_RR_BCC_CONFIG, 0x7,
+				 bc_freq_val);
+
+	/* FPD4 Sync Mode */
+	ub960_rxport_write(priv, nport, UB960_RR_CHANNEL_MODE, 0);
+
+	/* add serdes_eq_offset of 4 */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x2b, 0x04);
+
+	/* FPD4 serdes_start_eq in 0x27: assign default */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x27, 0x0);
+	/* FPD4 serdes_end_eq in 0x28: assign default */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x28, 0x23);
+
+	/* set serdes_driver_mode into FPD IV mode */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x04, 0x00);
+	/* set FPD PBC drv into FPD IV mode */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x1b, 0x00);
+
+	/* set serdes_system_init to 0x2f */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x21, 0x2f);
+	/* set serdes_system_rst in reset mode */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x25, 0xc1);
+
+	/* RX port to 7.55G mode */
+	ub960_update_bits(priv, UB960_SR_FPD_RATE_CFG, 0x3 << (nport * 2),
+			  0 << (nport * 2));
+
+	ub960_init_rx_port_ub9702_fpd4_aeq(priv, rxport);
+}
+
+static void ub960_init_rx_port_ub9702(struct ub960_data *priv,
+				      struct ub960_rxport *rxport)
+{
+	unsigned int nport = rxport->nport;
+
+	if (rxport->cdr_mode == RXPORT_CDR_FPD3)
+		ub960_init_rx_port_ub9702_fpd3(priv, rxport);
+	else /* RXPORT_CDR_FPD4 */
+		ub960_init_rx_port_ub9702_fpd4(priv, rxport);
+
+	switch (rxport->rx_mode) {
+	case RXPORT_MODE_RAW10:
+		/*
+		 * RAW10_8BIT_CTL = 0b11 : 8-bit processing using lower 8 bits
+		 * 0b10 : 8-bit processing using upper 8 bits
+		 */
+		ub960_rxport_update_bits(priv, nport, UB960_RR_PORT_CONFIG2,
+					 0x3 << 6, 0x2 << 6);
+
+		break;
+
+	case RXPORT_MODE_RAW12_HF:
+	case RXPORT_MODE_RAW12_LF:
+		/* Not implemented */
+		return;
+
+	case RXPORT_MODE_CSI2_SYNC:
+	case RXPORT_MODE_CSI2_NONSYNC:
+
+		break;
+	}
+
+	/* LV_POLARITY & FV_POLARITY */
+	ub960_rxport_update_bits(priv, nport, UB960_RR_PORT_CONFIG2, 0x3,
+				 rxport->lv_fv_pol);
+
+	/* Enable all interrupt sources from this port */
+	ub960_rxport_write(priv, nport, UB960_RR_PORT_ICR_HI, 0x07);
+	ub960_rxport_write(priv, nport, UB960_RR_PORT_ICR_LO, 0x7f);
+
+	/* Enable I2C_PASS_THROUGH */
+	ub960_rxport_update_bits(priv, nport, UB960_RR_BCC_CONFIG,
+				 UB960_RR_BCC_CONFIG_I2C_PASS_THROUGH,
+				 UB960_RR_BCC_CONFIG_I2C_PASS_THROUGH);
+
+	/* Enable I2C communication to the serializer via the alias addr */
+	ub960_rxport_write(priv, nport, UB960_RR_SER_ALIAS_ID,
+			   rxport->ser.alias << 1);
+
+	/* Enable RX port */
+	ub960_update_bits(priv, UB960_SR_RX_PORT_CTL, BIT(nport), BIT(nport));
+
+	if (rxport->cdr_mode == RXPORT_CDR_FPD4) {
+		/* unreset 960 AEQ */
+		ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x25, 0x41);
+	}
+}
+
+static int ub960_init_rx_ports(struct ub960_data *priv)
+{
+	unsigned int nport;
+
+	for (nport = 0; nport < priv->hw_data->num_rxports; nport++) {
+		struct ub960_rxport *rxport = priv->rxports[nport];
+
+		if (!rxport)
+			continue;
+
+		if (priv->hw_data->is_ub9702)
+			ub960_init_rx_port_ub9702(priv, rxport);
+		else
+			ub960_init_rx_port_ub960(priv, rxport);
+	}
+
+	return 0;
+}
+
+static void ub960_rxport_handle_events(struct ub960_data *priv, u8 nport)
+{
+	struct device *dev = &priv->client->dev;
+	u8 rx_port_sts1;
+	u8 rx_port_sts2;
+	u8 csi_rx_sts;
+	u8 bcc_sts;
+	int ret = 0;
+
+	/* Read interrupts (also clears most of them) */
+	if (!ret)
+		ret = ub960_rxport_read(priv, nport, UB960_RR_RX_PORT_STS1,
+					&rx_port_sts1);
+	if (!ret)
+		ret = ub960_rxport_read(priv, nport, UB960_RR_RX_PORT_STS2,
+					&rx_port_sts2);
+	if (!ret)
+		ret = ub960_rxport_read(priv, nport, UB960_RR_CSI_RX_STS,
+					&csi_rx_sts);
+	if (!ret)
+		ret = ub960_rxport_read(priv, nport, UB960_RR_BCC_STATUS,
+					&bcc_sts);
+
+	if (ret)
+		return;
+
+	if (rx_port_sts1 & UB960_RR_RX_PORT_STS1_PARITY_ERROR) {
+		u16 v;
+
+		ret = ub960_rxport_read16(priv, nport, UB960_RR_RX_PAR_ERR_HI,
+					  &v);
+		if (!ret)
+			dev_err(dev, "rx%u parity errors: %u\n", nport, v);
+	}
+
+	if (rx_port_sts1 & UB960_RR_RX_PORT_STS1_BCC_CRC_ERROR)
+		dev_err(dev, "rx%u BCC CRC error\n", nport);
+
+	if (rx_port_sts1 & UB960_RR_RX_PORT_STS1_BCC_SEQ_ERROR)
+		dev_err(dev, "rx%u BCC SEQ error\n", nport);
+
+	if (rx_port_sts2 & UB960_RR_RX_PORT_STS2_LINE_LEN_UNSTABLE)
+		dev_err(dev, "rx%u line length unstable\n", nport);
+
+	if (rx_port_sts2 & UB960_RR_RX_PORT_STS2_FPD3_ENCODE_ERROR)
+		dev_err(dev, "rx%u FPD3 encode error\n", nport);
+
+	if (rx_port_sts2 & UB960_RR_RX_PORT_STS2_BUFFER_ERROR)
+		dev_err(dev, "rx%u buffer error\n", nport);
+
+	if (csi_rx_sts)
+		dev_err(dev, "rx%u CSI error: %#02x\n", nport, csi_rx_sts);
+
+	if (csi_rx_sts & UB960_RR_CSI_RX_STS_ECC1_ERR)
+		dev_err(dev, "rx%u CSI ECC1 error\n", nport);
+
+	if (csi_rx_sts & UB960_RR_CSI_RX_STS_ECC2_ERR)
+		dev_err(dev, "rx%u CSI ECC2 error\n", nport);
+
+	if (csi_rx_sts & UB960_RR_CSI_RX_STS_CKSUM_ERR)
+		dev_err(dev, "rx%u CSI checksum error\n", nport);
+
+	if (csi_rx_sts & UB960_RR_CSI_RX_STS_LENGTH_ERR)
+		dev_err(dev, "rx%u CSI length error\n", nport);
+
+	if (bcc_sts)
+		dev_err(dev, "rx%u BCC error: %#02x\n", nport, bcc_sts);
+
+	if (bcc_sts & UB960_RR_BCC_STATUS_RESP_ERR)
+		dev_err(dev, "rx%u BCC response error", nport);
+
+	if (bcc_sts & UB960_RR_BCC_STATUS_SLAVE_TO)
+		dev_err(dev, "rx%u BCC slave timeout", nport);
+
+	if (bcc_sts & UB960_RR_BCC_STATUS_SLAVE_ERR)
+		dev_err(dev, "rx%u BCC slave error", nport);
+
+	if (bcc_sts & UB960_RR_BCC_STATUS_MASTER_TO)
+		dev_err(dev, "rx%u BCC master timeout", nport);
+
+	if (bcc_sts & UB960_RR_BCC_STATUS_MASTER_ERR)
+		dev_err(dev, "rx%u BCC master error", nport);
+
+	if (bcc_sts & UB960_RR_BCC_STATUS_SEQ_ERROR)
+		dev_err(dev, "rx%u BCC sequence error", nport);
+
+	if (rx_port_sts2 & UB960_RR_RX_PORT_STS2_LINE_LEN_CHG) {
+		u16 v;
+
+		ret = ub960_rxport_read16(priv, nport, UB960_RR_LINE_LEN_1, &v);
+		if (!ret)
+			dev_dbg(dev, "rx%u line len changed: %u\n", nport, v);
+	}
+
+	if (rx_port_sts2 & UB960_RR_RX_PORT_STS2_LINE_CNT_CHG) {
+		u16 v;
+
+		ret = ub960_rxport_read16(priv, nport, UB960_RR_LINE_COUNT_HI,
+					  &v);
+		if (!ret)
+			dev_dbg(dev, "rx%u line count changed: %u\n", nport, v);
+	}
+
+	if (rx_port_sts1 & UB960_RR_RX_PORT_STS1_LOCK_STS_CHG) {
+		dev_dbg(dev, "rx%u: %s, %s, %s, %s\n", nport,
+			(rx_port_sts1 & UB960_RR_RX_PORT_STS1_LOCK_STS) ?
+				"locked" :
+				"unlocked",
+			(rx_port_sts1 & UB960_RR_RX_PORT_STS1_PORT_PASS) ?
+				"passed" :
+				"not passed",
+			(rx_port_sts2 & UB960_RR_RX_PORT_STS2_CABLE_FAULT) ?
+				"no clock" :
+				"clock ok",
+			(rx_port_sts2 & UB960_RR_RX_PORT_STS2_FREQ_STABLE) ?
+				"stable freq" :
+				"unstable freq");
+	}
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2
+ */
+
+/*
+ * The current implementation only supports a simple VC mapping, where all VCs
+ * from a one RX port will be mapped to the same VC. Also, the hardware
+ * dictates that all streams from an RX port must go to a single TX port.
+ *
+ * This function decides the target VC numbers for each RX port with a simple
+ * algorithm, so that for each TX port, we get VC numbers starting from 0,
+ * and counting up.
+ *
+ * E.g. if all four RX ports are in use, of which the first two go to the
+ * first TX port and the secont two go to the second TX port, we would get
+ * the following VCs for the four RX ports: 0, 1, 0, 1.
+ *
+ * TODO: implement a more sophisticated VC mapping. As the driver cannot know
+ * what VCs the sinks expect (say, an FPGA with hardcoded VC routing), this
+ * probably needs to be somehow configurable. Device tree?
+ */
+static void ub960_get_vc_maps(struct ub960_data *priv,
+			      struct v4l2_subdev_state *state, u8 *vc)
+{
+	u8 cur_vc[UB960_MAX_TX_NPORTS] = {};
+	struct v4l2_subdev_route *route;
+	u8 handled_mask = 0;
+
+	for_each_active_route(&state->routing, route) {
+		unsigned int rx, tx;
+
+		rx = ub960_pad_to_port(priv, route->sink_pad);
+		if (BIT(rx) & handled_mask)
+			continue;
+
+		tx = ub960_pad_to_port(priv, route->source_pad);
+
+		vc[rx] = cur_vc[tx]++;
+		handled_mask |= BIT(rx);
+	}
+}
+
+static int ub960_enable_tx_port(struct ub960_data *priv, unsigned int nport)
+{
+	struct device *dev = &priv->client->dev;
+
+	dev_dbg(dev, "enable TX port %u\n", nport);
+
+	return ub960_txport_update_bits(priv, nport, UB960_TR_CSI_CTL,
+					UB960_TR_CSI_CTL_CSI_ENABLE,
+					UB960_TR_CSI_CTL_CSI_ENABLE);
+}
+
+static void ub960_disable_tx_port(struct ub960_data *priv, unsigned int nport)
+{
+	struct device *dev = &priv->client->dev;
+
+	dev_dbg(dev, "disable TX port %u\n", nport);
+
+	ub960_txport_update_bits(priv, nport, UB960_TR_CSI_CTL,
+				 UB960_TR_CSI_CTL_CSI_ENABLE, 0);
+}
+
+static int ub960_enable_rx_port(struct ub960_data *priv, unsigned int nport)
+{
+	struct device *dev = &priv->client->dev;
+
+	dev_dbg(dev, "enable RX port %u\n", nport);
+
+	/* Enable forwarding */
+	return ub960_update_bits(priv, UB960_SR_FWD_CTL1,
+				 UB960_SR_FWD_CTL1_PORT_DIS(nport), 0);
+}
+
+static void ub960_disable_rx_port(struct ub960_data *priv, unsigned int nport)
+{
+	struct device *dev = &priv->client->dev;
+
+	dev_dbg(dev, "disable RX port %u\n", nport);
+
+	/* Disable forwarding */
+	ub960_update_bits(priv, UB960_SR_FWD_CTL1,
+			  UB960_SR_FWD_CTL1_PORT_DIS(nport),
+			  UB960_SR_FWD_CTL1_PORT_DIS(nport));
+}
+
+/*
+ * The driver only supports using a single VC for each source. This function
+ * checks that each source only provides streams using a single VC.
+ */
+static int ub960_validate_stream_vcs(struct ub960_data *priv)
+{
+	unsigned int nport;
+	unsigned int i;
+
+	for (nport = 0; nport < priv->hw_data->num_rxports; nport++) {
+		struct ub960_rxport *rxport = priv->rxports[nport];
+		struct v4l2_mbus_frame_desc desc;
+		int ret;
+		u8 vc;
+
+		if (!rxport)
+			continue;
+
+		ret = v4l2_subdev_call(rxport->source.sd, pad, get_frame_desc,
+				       rxport->source.pad, &desc);
+		if (ret)
+			return ret;
+
+		if (desc.type != V4L2_MBUS_FRAME_DESC_TYPE_CSI2)
+			continue;
+
+		if (desc.num_entries == 0)
+			continue;
+
+		vc = desc.entry[0].bus.csi2.vc;
+
+		for (i = 1; i < desc.num_entries; i++) {
+			if (vc == desc.entry[i].bus.csi2.vc)
+				continue;
+
+			dev_err(&priv->client->dev,
+				"rx%u: source with multiple virtual-channels is not supported\n",
+				nport);
+			return -ENODEV;
+		}
+	}
+
+	return 0;
+}
+
+static int ub960_configure_ports_for_streaming(struct ub960_data *priv,
+					       struct v4l2_subdev_state *state)
+{
+	u8 fwd_ctl;
+	struct {
+		u32 num_streams;
+		u8 pixel_dt;
+		u8 meta_dt;
+		u32 meta_lines;
+		u32 tx_port;
+	} rx_data[UB960_MAX_RX_NPORTS] = {};
+	u8 vc_map[UB960_MAX_RX_NPORTS] = {};
+	struct v4l2_subdev_route *route;
+	unsigned int nport;
+	int ret;
+
+	ret = ub960_validate_stream_vcs(priv);
+	if (ret)
+		return ret;
+
+	ub960_get_vc_maps(priv, state, vc_map);
+
+	for_each_active_route(&state->routing, route) {
+		struct ub960_rxport *rxport;
+		struct ub960_txport *txport;
+		struct v4l2_mbus_framefmt *fmt;
+		const struct ub960_format_info *ub960_fmt;
+		unsigned int nport;
+
+		nport = ub960_pad_to_port(priv, route->sink_pad);
+
+		rxport = priv->rxports[nport];
+		if (!rxport)
+			return -EINVAL;
+
+		txport = priv->txports[ub960_pad_to_port(priv, route->source_pad)];
+		if (!txport)
+			return -EINVAL;
+
+		rx_data[nport].tx_port = ub960_pad_to_port(priv, route->source_pad);
+
+		rx_data[nport].num_streams++;
+
+		/* For the rest, we are only interested in parallel busses */
+		if (rxport->rx_mode == RXPORT_MODE_CSI2_SYNC ||
+		    rxport->rx_mode == RXPORT_MODE_CSI2_NONSYNC)
+			continue;
+
+		if (rx_data[nport].num_streams > 2)
+			return -EPIPE;
+
+		fmt = v4l2_subdev_state_get_stream_format(state,
+							  route->sink_pad,
+							  route->sink_stream);
+		if (!fmt)
+			return -EPIPE;
+
+		ub960_fmt = ub960_find_format(fmt->code);
+		if (!ub960_fmt)
+			return -EPIPE;
+
+		if (ub960_fmt->meta) {
+			if (fmt->height > 3) {
+				dev_err(&priv->client->dev,
+					"rx%u: unsupported metadata height %u\n",
+					nport, fmt->height);
+				return -EPIPE;
+			}
+
+			rx_data[nport].meta_dt = ub960_fmt->datatype;
+			rx_data[nport].meta_lines = fmt->height;
+		} else {
+			rx_data[nport].pixel_dt = ub960_fmt->datatype;
+		}
+	}
+
+	/* Configure RX ports */
+
+	/*
+	 * Keep all port forwardings disabled by default. Forwarding will be
+	 * enabled in ub960_enable_rx_port.
+	 */
+	fwd_ctl = GENMASK(7, 4);
+
+	for (nport = 0; nport < priv->hw_data->num_rxports; nport++) {
+		struct ub960_rxport *rxport = priv->rxports[nport];
+		u8 vc = vc_map[nport];
+
+		if (rx_data[nport].num_streams == 0)
+			continue;
+
+		switch (rxport->rx_mode) {
+		case RXPORT_MODE_RAW10:
+			ub960_rxport_write(priv, nport, UB960_RR_RAW10_ID,
+				rx_data[nport].pixel_dt | (vc << UB960_RR_RAW10_ID_VC_SHIFT));
+
+			ub960_rxport_write(priv, rxport->nport,
+				UB960_RR_RAW_EMBED_DTYPE,
+				(rx_data[nport].meta_lines << UB960_RR_RAW_EMBED_DTYPE_LINES_SHIFT) |
+					rx_data[nport].meta_dt);
+
+			break;
+
+		case RXPORT_MODE_RAW12_HF:
+		case RXPORT_MODE_RAW12_LF:
+			/* Not implemented */
+			break;
+
+		case RXPORT_MODE_CSI2_SYNC:
+		case RXPORT_MODE_CSI2_NONSYNC:
+			if (!priv->hw_data->is_ub9702) {
+				/* Map all VCs from this port to the same VC */
+				ub960_rxport_write(priv, nport, UB960_RR_CSI_VC_MAP,
+						   (vc << UB960_RR_CSI_VC_MAP_SHIFT(3)) |
+						   (vc << UB960_RR_CSI_VC_MAP_SHIFT(2)) |
+						   (vc << UB960_RR_CSI_VC_MAP_SHIFT(1)) |
+						   (vc << UB960_RR_CSI_VC_MAP_SHIFT(0)));
+			} else {
+				unsigned int i;
+
+				/* Map all VCs from this port to VC(nport) */
+				for (i = 0; i < 8; i++)
+					ub960_rxport_write(priv, nport,
+							   UB960_RR_VC_ID_MAP(i),
+							   nport);
+			}
+
+			break;
+		}
+
+		if (rx_data[nport].tx_port == 1)
+			fwd_ctl |= BIT(nport); /* forward to TX1 */
+		else
+			fwd_ctl &= ~BIT(nport); /* forward to TX0 */
+	}
+
+	ub960_write(priv, UB960_SR_FWD_CTL1, fwd_ctl);
+
+	return 0;
+}
+
+static void ub960_update_streaming_status(struct ub960_data *priv)
+{
+	unsigned int i;
+
+	for (i = 0; i < UB960_MAX_NPORTS; i++) {
+		if (priv->stream_enable_mask[i])
+			break;
+	}
+
+	priv->streaming = i < UB960_MAX_NPORTS;
+}
+
+static int ub960_enable_streams(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *state, u32 source_pad,
+				u64 source_streams_mask)
+{
+	struct ub960_data *priv = sd_to_ub960(sd);
+	struct device *dev = &priv->client->dev;
+	u64 sink_streams[UB960_MAX_RX_NPORTS] = {};
+	struct v4l2_subdev_route *route;
+	unsigned int failed_port;
+	unsigned int nport;
+	int ret;
+
+	if (!priv->streaming) {
+		dev_dbg(dev, "Prepare for streaming\n");
+		ret = ub960_configure_ports_for_streaming(priv, state);
+		if (ret)
+			return ret;
+	}
+
+	/* Enable TX port if not yet enabled */
+	if (!priv->stream_enable_mask[source_pad]) {
+		ret = ub960_enable_tx_port(priv,
+					   ub960_pad_to_port(priv, source_pad));
+		if (ret)
+			return ret;
+	}
+
+	priv->stream_enable_mask[source_pad] |= source_streams_mask;
+
+	/* Collect sink streams per pad which we need to enable */
+	for_each_active_route(&state->routing, route) {
+		if (route->source_pad != source_pad)
+			continue;
+
+		if (!(source_streams_mask & BIT_ULL(route->source_stream)))
+			continue;
+
+		nport = ub960_pad_to_port(priv, route->sink_pad);
+
+		sink_streams[nport] |= BIT_ULL(route->sink_stream);
+	}
+
+	for (nport = 0; nport < priv->hw_data->num_rxports; nport++) {
+		if (!sink_streams[nport])
+			continue;
+
+		/* Enable the RX port if not yet enabled */
+		if (!priv->stream_enable_mask[nport]) {
+			ret = ub960_enable_rx_port(priv, nport);
+			if (ret) {
+				failed_port = nport;
+				goto err;
+			}
+		}
+
+		priv->stream_enable_mask[nport] |= sink_streams[nport];
+
+		dev_dbg(dev, "enable RX port %u streams %#llx\n", nport,
+			sink_streams[nport]);
+
+		ret = v4l2_subdev_enable_streams(
+			priv->rxports[nport]->source.sd,
+			priv->rxports[nport]->source.pad,
+			sink_streams[nport]);
+		if (ret) {
+			priv->stream_enable_mask[nport] &= ~sink_streams[nport];
+
+			if (!priv->stream_enable_mask[nport])
+				ub960_disable_rx_port(priv, nport);
+
+			failed_port = nport;
+			goto err;
+		}
+	}
+
+	priv->streaming = true;
+
+	return 0;
+
+err:
+	for (nport = 0; nport < failed_port; nport++) {
+		if (!sink_streams[nport])
+			continue;
+
+		dev_dbg(dev, "disable RX port %u streams %#llx\n", nport,
+			sink_streams[nport]);
+
+		ret = v4l2_subdev_disable_streams(
+			priv->rxports[nport]->source.sd,
+			priv->rxports[nport]->source.pad,
+			sink_streams[nport]);
+		if (ret)
+			dev_err(dev, "Failed to disable streams: %d\n", ret);
+
+		priv->stream_enable_mask[nport] &= ~sink_streams[nport];
+
+		/* Disable RX port if no active streams */
+		if (!priv->stream_enable_mask[nport])
+			ub960_disable_rx_port(priv, nport);
+	}
+
+	priv->stream_enable_mask[source_pad] &= ~source_streams_mask;
+
+	if (!priv->stream_enable_mask[source_pad])
+		ub960_disable_tx_port(priv,
+				      ub960_pad_to_port(priv, source_pad));
+
+	ub960_update_streaming_status(priv);
+
+	return ret;
+}
+
+static int ub960_disable_streams(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *state,
+				 u32 source_pad, u64 source_streams_mask)
+{
+	struct ub960_data *priv = sd_to_ub960(sd);
+	struct device *dev = &priv->client->dev;
+	u64 sink_streams[UB960_MAX_RX_NPORTS] = {};
+	struct v4l2_subdev_route *route;
+	unsigned int nport;
+	int ret;
+
+	/* Collect sink streams per pad which we need to disable */
+	for_each_active_route(&state->routing, route) {
+		if (route->source_pad != source_pad)
+			continue;
+
+		if (!(source_streams_mask & BIT_ULL(route->source_stream)))
+			continue;
+
+		nport = ub960_pad_to_port(priv, route->sink_pad);
+
+		sink_streams[nport] |= BIT_ULL(route->sink_stream);
+	}
+
+	for (nport = 0; nport < priv->hw_data->num_rxports; nport++) {
+		if (!sink_streams[nport])
+			continue;
+
+		dev_dbg(dev, "disable RX port %u streams %#llx\n", nport,
+			sink_streams[nport]);
+
+		ret = v4l2_subdev_disable_streams(
+			priv->rxports[nport]->source.sd,
+			priv->rxports[nport]->source.pad,
+			sink_streams[nport]);
+		if (ret)
+			dev_err(dev, "Failed to disable streams: %d\n", ret);
+
+		priv->stream_enable_mask[nport] &= ~sink_streams[nport];
+
+		/* Disable RX port if no active streams */
+		if (!priv->stream_enable_mask[nport])
+			ub960_disable_rx_port(priv, nport);
+	}
+
+	/* Disable TX port if no active streams */
+
+	priv->stream_enable_mask[source_pad] &= ~source_streams_mask;
+
+	if (!priv->stream_enable_mask[source_pad])
+		ub960_disable_tx_port(priv,
+				      ub960_pad_to_port(priv, source_pad));
+
+	ub960_update_streaming_status(priv);
+
+	return 0;
+}
+
+static int _ub960_set_routing(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *state,
+			      struct v4l2_subdev_krouting *routing)
+{
+	static const struct v4l2_mbus_framefmt format = {
+		.width = 640,
+		.height = 480,
+		.code = MEDIA_BUS_FMT_UYVY8_1X16,
+		.field = V4L2_FIELD_NONE,
+		.colorspace = V4L2_COLORSPACE_SRGB,
+		.ycbcr_enc = V4L2_YCBCR_ENC_601,
+		.quantization = V4L2_QUANTIZATION_LIM_RANGE,
+		.xfer_func = V4L2_XFER_FUNC_SRGB,
+	};
+	int ret;
+
+	/*
+	 * Note: we can only support up to V4L2_FRAME_DESC_ENTRY_MAX, until
+	 * frame desc is made dynamically allocated.
+	 */
+
+	if (routing->num_routes > V4L2_FRAME_DESC_ENTRY_MAX)
+		return -E2BIG;
+
+	ret = v4l2_subdev_routing_validate(sd, routing,
+					   V4L2_SUBDEV_ROUTING_ONLY_1_TO_1 |
+					   V4L2_SUBDEV_ROUTING_NO_SINK_STREAM_MIX);
+	if (ret)
+		return ret;
+
+	ret = v4l2_subdev_set_routing_with_fmt(sd, state, routing, &format);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int ub960_set_routing(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *state,
+			     enum v4l2_subdev_format_whence which,
+			     struct v4l2_subdev_krouting *routing)
+{
+	struct ub960_data *priv = sd_to_ub960(sd);
+
+	if (which == V4L2_SUBDEV_FORMAT_ACTIVE && priv->streaming)
+		return -EBUSY;
+
+	return _ub960_set_routing(sd, state, routing);
+}
+
+static int ub960_get_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
+				struct v4l2_mbus_frame_desc *fd)
+{
+	struct ub960_data *priv = sd_to_ub960(sd);
+	struct v4l2_subdev_route *route;
+	struct v4l2_subdev_state *state;
+	int ret = 0;
+	struct device *dev = &priv->client->dev;
+	u8 vc_map[UB960_MAX_RX_NPORTS] = {};
+
+	if (!ub960_pad_is_source(priv, pad))
+		return -EINVAL;
+
+	memset(fd, 0, sizeof(*fd));
+
+	fd->type = V4L2_MBUS_FRAME_DESC_TYPE_CSI2;
+
+	state = v4l2_subdev_lock_and_get_active_state(&priv->sd);
+
+	ub960_get_vc_maps(priv, state, vc_map);
+
+	for_each_active_route(&state->routing, route) {
+		struct v4l2_mbus_frame_desc_entry *source_entry = NULL;
+		struct v4l2_mbus_frame_desc source_fd;
+		unsigned int nport;
+		unsigned int i;
+
+		if (route->source_pad != pad)
+			continue;
+
+		nport = ub960_pad_to_port(priv, route->sink_pad);
+
+		ret = v4l2_subdev_call(priv->rxports[nport]->source.sd, pad,
+				       get_frame_desc,
+				       priv->rxports[nport]->source.pad,
+				       &source_fd);
+		if (ret) {
+			dev_err(dev,
+				"Failed to get source frame desc for pad %u\n",
+				route->sink_pad);
+			goto out_unlock;
+		}
+
+		for (i = 0; i < source_fd.num_entries; i++) {
+			if (source_fd.entry[i].stream == route->sink_stream) {
+				source_entry = &source_fd.entry[i];
+				break;
+			}
+		}
+
+		if (!source_entry) {
+			dev_err(dev,
+				"Failed to find stream from source frame desc\n");
+			ret = -EPIPE;
+			goto out_unlock;
+		}
+
+		fd->entry[fd->num_entries].stream = route->source_stream;
+		fd->entry[fd->num_entries].flags = source_entry->flags;
+		fd->entry[fd->num_entries].length = source_entry->length;
+		fd->entry[fd->num_entries].pixelcode = source_entry->pixelcode;
+
+		fd->entry[fd->num_entries].bus.csi2.vc = vc_map[nport];
+
+		if (source_fd.type == V4L2_MBUS_FRAME_DESC_TYPE_CSI2) {
+			fd->entry[fd->num_entries].bus.csi2.dt =
+				source_entry->bus.csi2.dt;
+		} else {
+			const struct ub960_format_info *ub960_fmt;
+			struct v4l2_mbus_framefmt *fmt;
+
+			fmt = v4l2_subdev_state_get_stream_format(state, pad,
+								  route->source_stream);
+
+			if (!fmt) {
+				ret = -EINVAL;
+				goto out_unlock;
+			}
+
+			ub960_fmt = ub960_find_format(fmt->code);
+			if (!ub960_fmt) {
+				dev_err(dev, "Unable to find format\n");
+				ret = -EINVAL;
+				goto out_unlock;
+			}
+
+			fd->entry[fd->num_entries].bus.csi2.dt =
+				ub960_fmt->datatype;
+		}
+
+		fd->num_entries++;
+	}
+
+out_unlock:
+	v4l2_subdev_unlock_state(state);
+
+	return ret;
+}
+
+static int ub960_set_fmt(struct v4l2_subdev *sd,
+			 struct v4l2_subdev_state *state,
+			 struct v4l2_subdev_format *format)
+{
+	struct ub960_data *priv = sd_to_ub960(sd);
+	struct v4l2_mbus_framefmt *fmt;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE && priv->streaming)
+		return -EBUSY;
+
+	/* No transcoding, source and sink formats must match. */
+	if (ub960_pad_is_source(priv, format->pad))
+		return v4l2_subdev_get_fmt(sd, state, format);
+
+	/*
+	 * Default to the first format if the requested media bus code isn't
+	 * supported.
+	 */
+	if (!ub960_find_format(format->format.code))
+		format->format.code = ub960_formats[0].code;
+
+	fmt = v4l2_subdev_state_get_stream_format(state, format->pad,
+						  format->stream);
+	if (!fmt)
+		return -EINVAL;
+
+	*fmt = format->format;
+
+	fmt = v4l2_subdev_state_get_opposite_stream_format(state, format->pad,
+							   format->stream);
+	if (!fmt)
+		return -EINVAL;
+
+	*fmt = format->format;
+
+	return 0;
+}
+
+static int ub960_init_cfg(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *state)
+{
+	struct ub960_data *priv = sd_to_ub960(sd);
+
+	struct v4l2_subdev_route routes[] = {
+		{
+			.sink_pad = 0,
+			.sink_stream = 0,
+			.source_pad = priv->hw_data->num_rxports,
+			.source_stream = 0,
+			.flags = V4L2_SUBDEV_ROUTE_FL_ACTIVE,
+		},
+	};
+
+	struct v4l2_subdev_krouting routing = {
+		.num_routes = ARRAY_SIZE(routes),
+		.routes = routes,
+	};
+
+	return _ub960_set_routing(sd, state, &routing);
+}
+
+static const struct v4l2_subdev_pad_ops ub960_pad_ops = {
+	.enable_streams = ub960_enable_streams,
+	.disable_streams = ub960_disable_streams,
+
+	.set_routing = ub960_set_routing,
+	.get_frame_desc = ub960_get_frame_desc,
+
+	.get_fmt = v4l2_subdev_get_fmt,
+	.set_fmt = ub960_set_fmt,
+
+	.init_cfg = ub960_init_cfg,
+};
+
+static int ub960_log_status(struct v4l2_subdev *sd)
+{
+	struct ub960_data *priv = sd_to_ub960(sd);
+	struct device *dev = &priv->client->dev;
+	struct v4l2_subdev_state *state;
+	unsigned int nport;
+	unsigned int i;
+	u16 v16 = 0;
+	u8 v = 0;
+	u8 id[UB960_SR_FPD3_RX_ID_LEN];
+
+	state = v4l2_subdev_lock_and_get_active_state(sd);
+
+	for (i = 0; i < sizeof(id); i++)
+		ub960_read(priv, UB960_SR_FPD3_RX_ID(i), &id[i]);
+
+	dev_info(dev, "ID '%.*s'\n", (int)sizeof(id), id);
+
+	for (nport = 0; nport < priv->hw_data->num_txports; nport++) {
+		struct ub960_txport *txport = priv->txports[nport];
+
+		dev_info(dev, "TX %u\n", nport);
+
+		if (!txport) {
+			dev_info(dev, "\tNot initialized\n");
+			continue;
+		}
+
+		ub960_txport_read(priv, nport, UB960_TR_CSI_STS, &v);
+		dev_info(dev, "\tsync %u, pass %u\n", v & (u8)BIT(1),
+			 v & (u8)BIT(0));
+
+		ub960_read16(priv, UB960_SR_CSI_FRAME_COUNT_HI(nport), &v16);
+		dev_info(dev, "\tframe counter %u\n", v16);
+
+		ub960_read16(priv, UB960_SR_CSI_FRAME_ERR_COUNT_HI(nport), &v16);
+		dev_info(dev, "\tframe error counter %u\n", v16);
+
+		ub960_read16(priv, UB960_SR_CSI_LINE_COUNT_HI(nport), &v16);
+		dev_info(dev, "\tline counter %u\n", v16);
+
+		ub960_read16(priv, UB960_SR_CSI_LINE_ERR_COUNT_HI(nport), &v16);
+		dev_info(dev, "\tline error counter %u\n", v16);
+	}
+
+	for (nport = 0; nport < priv->hw_data->num_rxports; nport++) {
+		struct ub960_rxport *rxport = priv->rxports[nport];
+		u8 eq_level;
+		s8 strobe_pos;
+		unsigned int i;
+
+		dev_info(dev, "RX %u\n", nport);
+
+		if (!rxport) {
+			dev_info(dev, "\tNot initialized\n");
+			continue;
+		}
+
+		ub960_rxport_read(priv, nport, UB960_RR_RX_PORT_STS1, &v);
+
+		if (v & UB960_RR_RX_PORT_STS1_LOCK_STS)
+			dev_info(dev, "\tLocked\n");
+		else
+			dev_info(dev, "\tNot locked\n");
+
+		dev_info(dev, "\trx_port_sts1 %#02x\n", v);
+		ub960_rxport_read(priv, nport, UB960_RR_RX_PORT_STS2, &v);
+		dev_info(dev, "\trx_port_sts2 %#02x\n", v);
+
+		ub960_rxport_read16(priv, nport, UB960_RR_RX_FREQ_HIGH, &v16);
+		dev_info(dev, "\tlink freq %llu Hz\n", (v16 * 1000000ULL) >> 8);
+
+		ub960_rxport_read16(priv, nport, UB960_RR_RX_PAR_ERR_HI, &v16);
+		dev_info(dev, "\tparity errors %u\n", v16);
+
+		ub960_rxport_read16(priv, nport, UB960_RR_LINE_COUNT_HI, &v16);
+		dev_info(dev, "\tlines per frame %u\n", v16);
+
+		ub960_rxport_read16(priv, nport, UB960_RR_LINE_LEN_1, &v16);
+		dev_info(dev, "\tbytes per line %u\n", v16);
+
+		ub960_rxport_read(priv, nport, UB960_RR_CSI_ERR_COUNTER, &v);
+		dev_info(dev, "\tcsi_err_counter %u\n", v);
+
+		/* Strobe */
+
+		ub960_read(priv, UB960_XR_AEQ_CTL1, &v);
+
+		dev_info(dev, "\t%s strobe\n",
+			 (v & UB960_XR_AEQ_CTL1_AEQ_SFILTER_EN) ? "Adaptive" :
+								  "Manual");
+
+		if (v & UB960_XR_AEQ_CTL1_AEQ_SFILTER_EN) {
+			ub960_read(priv, UB960_XR_SFILTER_CFG, &v);
+
+			dev_info(dev, "\tStrobe range [%d, %d]\n",
+				 ((v >> UB960_XR_SFILTER_CFG_SFILTER_MIN_SHIFT) & 0xf) - 7,
+				 ((v >> UB960_XR_SFILTER_CFG_SFILTER_MAX_SHIFT) & 0xf) - 7);
+		}
+
+		ub960_rxport_get_strobe_pos(priv, nport, &strobe_pos);
+
+		dev_info(dev, "\tStrobe pos %d\n", strobe_pos);
+
+		/* EQ */
+
+		ub960_rxport_read(priv, nport, UB960_RR_AEQ_BYPASS, &v);
+
+		dev_info(dev, "\t%s EQ\n",
+			 (v & UB960_RR_AEQ_BYPASS_ENABLE) ? "Manual" :
+							    "Adaptive");
+
+		if (!(v & UB960_RR_AEQ_BYPASS_ENABLE)) {
+			ub960_rxport_read(priv, nport, UB960_RR_AEQ_MIN_MAX, &v);
+
+			dev_info(dev, "\tEQ range [%u, %u]\n",
+				 (v >> UB960_RR_AEQ_MIN_MAX_AEQ_FLOOR_SHIFT) & 0xf,
+				 (v >> UB960_RR_AEQ_MIN_MAX_AEQ_MAX_SHIFT) & 0xf);
+		}
+
+		if (ub960_rxport_get_eq_level(priv, nport, &eq_level) == 0)
+			dev_info(dev, "\tEQ level %u\n", eq_level);
+
+		/* GPIOs */
+		for (i = 0; i < UB960_NUM_BC_GPIOS; i++) {
+			u8 ctl_reg;
+			u8 ctl_shift;
+
+			ctl_reg = UB960_RR_BC_GPIO_CTL(i / 2);
+			ctl_shift = (i % 2) * 4;
+
+			ub960_rxport_read(priv, nport, ctl_reg, &v);
+
+			dev_info(dev, "\tGPIO%u: mode %u\n", i,
+				 (v >> ctl_shift) & 0xf);
+		}
+	}
+
+	v4l2_subdev_unlock_state(state);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops ub960_subdev_core_ops = {
+	.log_status = ub960_log_status,
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_ops ub960_subdev_ops = {
+	.core = &ub960_subdev_core_ops,
+	.pad = &ub960_pad_ops,
+};
+
+static const struct media_entity_operations ub960_entity_ops = {
+	.get_fwnode_pad = v4l2_subdev_get_fwnode_pad_1_to_1,
+	.link_validate = v4l2_subdev_link_validate,
+	.has_pad_interdep = v4l2_subdev_has_pad_interdep,
+};
+
+/* -----------------------------------------------------------------------------
+ * Core
+ */
+
+static irqreturn_t ub960_handle_events(int irq, void *arg)
+{
+	struct ub960_data *priv = arg;
+	unsigned int i;
+	u8 int_sts;
+	u8 fwd_sts;
+	int ret;
+
+	ret = ub960_read(priv, UB960_SR_INTERRUPT_STS, &int_sts);
+	if (ret || !int_sts)
+		return IRQ_NONE;
+
+	dev_dbg(&priv->client->dev, "INTERRUPT_STS %x\n", int_sts);
+
+	ret = ub960_read(priv, UB960_SR_FWD_STS, &fwd_sts);
+	if (ret)
+		return IRQ_NONE;
+
+	dev_dbg(&priv->client->dev, "FWD_STS %#02x\n", fwd_sts);
+
+	for (i = 0; i < priv->hw_data->num_txports; i++) {
+		if (int_sts & UB960_SR_INTERRUPT_STS_IS_CSI_TX(i))
+			ub960_csi_handle_events(priv, i);
+	}
+
+	for (i = 0; i < priv->hw_data->num_rxports; i++) {
+		if (!priv->rxports[i])
+			continue;
+
+		if (int_sts & UB960_SR_INTERRUPT_STS_IS_RX(i))
+			ub960_rxport_handle_events(priv, i);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static void ub960_handler_work(struct work_struct *work)
+{
+	struct delayed_work *dwork = to_delayed_work(work);
+	struct ub960_data *priv =
+		container_of(dwork, struct ub960_data, poll_work);
+
+	ub960_handle_events(0, priv);
+
+	schedule_delayed_work(&priv->poll_work,
+			      msecs_to_jiffies(UB960_POLL_TIME_MS));
+}
+
+static void ub960_txport_free_ports(struct ub960_data *priv)
+{
+	unsigned int nport;
+
+	for (nport = 0; nport < priv->hw_data->num_txports; nport++) {
+		struct ub960_txport *txport = priv->txports[nport];
+
+		if (!txport)
+			continue;
+
+		kfree(txport);
+		priv->txports[nport] = NULL;
+	}
+}
+
+static void ub960_rxport_free_ports(struct ub960_data *priv)
+{
+	unsigned int nport;
+
+	for (nport = 0; nport < priv->hw_data->num_rxports; nport++) {
+		struct ub960_rxport *rxport = priv->rxports[nport];
+
+		if (!rxport)
+			continue;
+
+		fwnode_handle_put(rxport->source.ep_fwnode);
+		fwnode_handle_put(rxport->ser.fwnode);
+
+		kfree(rxport);
+		priv->rxports[nport] = NULL;
+	}
+}
+
+static int
+ub960_parse_dt_rxport_link_properties(struct ub960_data *priv,
+				      struct fwnode_handle *link_fwnode,
+				      struct ub960_rxport *rxport)
+{
+	struct device *dev = &priv->client->dev;
+	unsigned int nport = rxport->nport;
+	u32 rx_mode;
+	u32 cdr_mode;
+	s32 strobe_pos;
+	u32 eq_level;
+	u32 ser_i2c_alias;
+	int ret;
+
+	cdr_mode = RXPORT_CDR_FPD3;
+
+	ret = fwnode_property_read_u32(link_fwnode, "ti,cdr-mode", &cdr_mode);
+	if (ret < 0 && ret != -EINVAL) {
+		dev_err(dev, "rx%u: failed to read '%s': %d\n", nport,
+			"ti,cdr-mode", ret);
+		return ret;
+	}
+
+	if (cdr_mode > RXPORT_CDR_LAST) {
+		dev_err(dev, "rx%u: bad 'ti,cdr-mode' %u\n", nport, cdr_mode);
+		return -EINVAL;
+	}
+
+	if (!priv->hw_data->is_fpdlink4 && cdr_mode == RXPORT_CDR_FPD4) {
+		dev_err(dev, "rx%u: FPD-Link 4 CDR not supported\n", nport);
+		return -EINVAL;
+	}
+
+	rxport->cdr_mode = cdr_mode;
+
+	ret = fwnode_property_read_u32(link_fwnode, "ti,rx-mode", &rx_mode);
+	if (ret < 0) {
+		dev_err(dev, "rx%u: failed to read '%s': %d\n", nport,
+			"ti,rx-mode", ret);
+		return ret;
+	}
+
+	if (rx_mode > RXPORT_MODE_LAST) {
+		dev_err(dev, "rx%u: bad 'ti,rx-mode' %u\n", nport, rx_mode);
+		return -EINVAL;
+	}
+
+	switch (rx_mode) {
+	case RXPORT_MODE_RAW12_HF:
+	case RXPORT_MODE_RAW12_LF:
+		dev_err(dev, "rx%u: unsupported 'ti,rx-mode' %u\n", nport,
+			rx_mode);
+		return -EINVAL;
+	default:
+		break;
+	}
+
+	rxport->rx_mode = rx_mode;
+
+	/* EQ & Strobe related */
+
+	/* Defaults */
+	rxport->eq.manual_eq = false;
+	rxport->eq.aeq.eq_level_min = UB960_MIN_EQ_LEVEL;
+	rxport->eq.aeq.eq_level_max = UB960_MAX_EQ_LEVEL;
+
+	ret = fwnode_property_read_u32(link_fwnode, "ti,strobe-pos",
+				       &strobe_pos);
+	if (ret) {
+		if (ret != -EINVAL) {
+			dev_err(dev, "rx%u: failed to read '%s': %d\n", nport,
+				"ti,strobe-pos", ret);
+			return ret;
+		}
+	} else {
+		if (strobe_pos < UB960_MIN_MANUAL_STROBE_POS ||
+		    strobe_pos > UB960_MAX_MANUAL_STROBE_POS) {
+			dev_err(dev, "rx%u: illegal 'strobe-pos' value: %d\n",
+				nport, strobe_pos);
+			return -EINVAL;
+		}
+
+		/* NOTE: ignored unless global manual strobe pos is also set */
+		rxport->eq.strobe_pos = strobe_pos;
+		if (!priv->strobe.manual)
+			dev_warn(dev,
+				 "rx%u: 'ti,strobe-pos' ignored as 'ti,manual-strobe' not set\n",
+				 nport);
+	}
+
+	ret = fwnode_property_read_u32(link_fwnode, "ti,eq-level", &eq_level);
+	if (ret) {
+		if (ret != -EINVAL) {
+			dev_err(dev, "rx%u: failed to read '%s': %d\n", nport,
+				"ti,eq-level", ret);
+			return ret;
+		}
+	} else {
+		if (eq_level > UB960_MAX_EQ_LEVEL) {
+			dev_err(dev, "rx%u: illegal 'ti,eq-level' value: %d\n",
+				nport, eq_level);
+			return -EINVAL;
+		}
+
+		rxport->eq.manual_eq = true;
+		rxport->eq.manual.eq_level = eq_level;
+	}
+
+	ret = fwnode_property_read_u32(link_fwnode, "i2c-alias",
+				       &ser_i2c_alias);
+	if (ret) {
+		dev_err(dev, "rx%u: failed to read '%s': %d\n", nport,
+			"i2c-alias", ret);
+		return ret;
+	}
+	rxport->ser.alias = ser_i2c_alias;
+
+	rxport->ser.fwnode = fwnode_get_named_child_node(link_fwnode, "serializer");
+	if (!rxport->ser.fwnode) {
+		dev_err(dev, "rx%u: missing 'serializer' node\n", nport);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int ub960_parse_dt_rxport_ep_properties(struct ub960_data *priv,
+					       struct fwnode_handle *ep_fwnode,
+					       struct ub960_rxport *rxport)
+{
+	struct device *dev = &priv->client->dev;
+	struct v4l2_fwnode_endpoint vep = {};
+	unsigned int nport = rxport->nport;
+	bool hsync_hi;
+	bool vsync_hi;
+	int ret;
+
+	rxport->source.ep_fwnode = fwnode_graph_get_remote_endpoint(ep_fwnode);
+	if (!rxport->source.ep_fwnode) {
+		dev_err(dev, "rx%u: no remote endpoint\n", nport);
+		return -ENODEV;
+	}
+
+	/* We currently have properties only for RAW modes */
+
+	switch (rxport->rx_mode) {
+	case RXPORT_MODE_RAW10:
+	case RXPORT_MODE_RAW12_HF:
+	case RXPORT_MODE_RAW12_LF:
+		break;
+	default:
+		return 0;
+	}
+
+	vep.bus_type = V4L2_MBUS_PARALLEL;
+	ret = v4l2_fwnode_endpoint_parse(ep_fwnode, &vep);
+	if (ret) {
+		dev_err(dev, "rx%u: failed to parse endpoint data\n", nport);
+		goto err_put_source_ep_fwnode;
+	}
+
+	hsync_hi = !!(vep.bus.parallel.flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH);
+	vsync_hi = !!(vep.bus.parallel.flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH);
+
+	/* LineValid and FrameValid are inverse to the h/vsync active */
+	rxport->lv_fv_pol = (hsync_hi ? UB960_RR_PORT_CONFIG2_LV_POL_LOW : 0) |
+			    (vsync_hi ? UB960_RR_PORT_CONFIG2_FV_POL_LOW : 0);
+
+	return 0;
+
+err_put_source_ep_fwnode:
+	fwnode_handle_put(rxport->source.ep_fwnode);
+	return ret;
+}
+
+static int ub960_parse_dt_rxport(struct ub960_data *priv, unsigned int nport,
+				 struct fwnode_handle *link_fwnode,
+				 struct fwnode_handle *ep_fwnode)
+{
+	static const char *vpoc_names[UB960_MAX_RX_NPORTS] = {
+		"vpoc0", "vpoc1", "vpoc2", "vpoc3"
+	};
+	struct device *dev = &priv->client->dev;
+	struct ub960_rxport *rxport;
+	int ret;
+
+	rxport = kzalloc(sizeof(*rxport), GFP_KERNEL);
+	if (!rxport)
+		return -ENOMEM;
+
+	priv->rxports[nport] = rxport;
+
+	rxport->nport = nport;
+	rxport->priv = priv;
+
+	ret = ub960_parse_dt_rxport_link_properties(priv, link_fwnode, rxport);
+	if (ret)
+		goto err_free_rxport;
+
+	rxport->vpoc = devm_regulator_get_optional(dev, vpoc_names[nport]);
+	if (IS_ERR(rxport->vpoc)) {
+		ret = PTR_ERR(rxport->vpoc);
+		if (ret == -ENODEV) {
+			rxport->vpoc = NULL;
+		} else {
+			dev_err(dev, "rx%u: failed to get VPOC supply: %d\n",
+				nport, ret);
+			goto err_put_remote_fwnode;
+		}
+	}
+
+	ret = ub960_parse_dt_rxport_ep_properties(priv, ep_fwnode, rxport);
+	if (ret)
+		goto err_put_remote_fwnode;
+
+	return 0;
+
+err_put_remote_fwnode:
+	fwnode_handle_put(rxport->ser.fwnode);
+err_free_rxport:
+	priv->rxports[nport] = NULL;
+	kfree(rxport);
+	return ret;
+}
+
+static struct fwnode_handle *
+ub960_fwnode_get_link_by_regs(struct fwnode_handle *links_fwnode,
+			      unsigned int nport)
+{
+	struct fwnode_handle *link_fwnode;
+	int ret;
+
+	fwnode_for_each_child_node(links_fwnode, link_fwnode) {
+		u32 link_num;
+
+		if (!str_has_prefix(fwnode_get_name(link_fwnode), "link@"))
+			continue;
+
+		ret = fwnode_property_read_u32(link_fwnode, "reg", &link_num);
+		if (ret) {
+			fwnode_handle_put(link_fwnode);
+			return NULL;
+		}
+
+		if (nport == link_num)
+			return link_fwnode;
+	}
+
+	return NULL;
+}
+
+static int ub960_parse_dt_rxports(struct ub960_data *priv)
+{
+	struct device *dev = &priv->client->dev;
+	struct fwnode_handle *links_fwnode;
+	unsigned int nport;
+	int ret;
+
+	links_fwnode = fwnode_get_named_child_node(dev_fwnode(dev), "links");
+	if (!links_fwnode) {
+		dev_err(dev, "'links' node missing\n");
+		return -ENODEV;
+	}
+
+	/* Defaults, recommended by TI */
+	priv->strobe.min = 2;
+	priv->strobe.max = 3;
+
+	priv->strobe.manual = fwnode_property_read_bool(links_fwnode, "ti,manual-strobe");
+
+	for (nport = 0; nport < priv->hw_data->num_rxports; nport++) {
+		struct fwnode_handle *link_fwnode;
+		struct fwnode_handle *ep_fwnode;
+
+		link_fwnode = ub960_fwnode_get_link_by_regs(links_fwnode, nport);
+		if (!link_fwnode)
+			continue;
+
+		ep_fwnode = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev),
+							    nport, 0, 0);
+		if (!ep_fwnode) {
+			fwnode_handle_put(link_fwnode);
+			continue;
+		}
+
+		ret = ub960_parse_dt_rxport(priv, nport, link_fwnode,
+					    ep_fwnode);
+
+		fwnode_handle_put(link_fwnode);
+		fwnode_handle_put(ep_fwnode);
+
+		if (ret) {
+			dev_err(dev, "rx%u: failed to parse RX port\n", nport);
+			goto err_put_links;
+		}
+	}
+
+	fwnode_handle_put(links_fwnode);
+
+	return 0;
+
+err_put_links:
+	fwnode_handle_put(links_fwnode);
+
+	return ret;
+}
+
+static int ub960_parse_dt_txports(struct ub960_data *priv)
+{
+	struct device *dev = &priv->client->dev;
+	u32 nport;
+	int ret;
+
+	for (nport = 0; nport < priv->hw_data->num_txports; nport++) {
+		unsigned int port = nport + priv->hw_data->num_rxports;
+		struct fwnode_handle *ep_fwnode;
+
+		ep_fwnode = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev),
+							    port, 0, 0);
+		if (!ep_fwnode)
+			continue;
+
+		ret = ub960_parse_dt_txport(priv, ep_fwnode, nport);
+
+		fwnode_handle_put(ep_fwnode);
+
+		if (ret)
+			break;
+	}
+
+	return 0;
+}
+
+static int ub960_parse_dt(struct ub960_data *priv)
+{
+	int ret;
+
+	ret = ub960_parse_dt_rxports(priv);
+	if (ret)
+		return ret;
+
+	ret = ub960_parse_dt_txports(priv);
+	if (ret)
+		goto err_free_rxports;
+
+	return 0;
+
+err_free_rxports:
+	ub960_rxport_free_ports(priv);
+
+	return ret;
+}
+
+static int ub960_notify_bound(struct v4l2_async_notifier *notifier,
+			      struct v4l2_subdev *subdev,
+			      struct v4l2_async_connection *asd)
+{
+	struct ub960_data *priv = sd_to_ub960(notifier->sd);
+	struct ub960_rxport *rxport = to_ub960_asd(asd)->rxport;
+	struct device *dev = &priv->client->dev;
+	u8 nport = rxport->nport;
+	unsigned int i;
+	int ret;
+
+	ret = media_entity_get_fwnode_pad(&subdev->entity,
+					  rxport->source.ep_fwnode,
+					  MEDIA_PAD_FL_SOURCE);
+	if (ret < 0) {
+		dev_err(dev, "Failed to find pad for %s\n", subdev->name);
+		return ret;
+	}
+
+	rxport->source.sd = subdev;
+	rxport->source.pad = ret;
+
+	ret = media_create_pad_link(&rxport->source.sd->entity,
+				    rxport->source.pad, &priv->sd.entity, nport,
+				    MEDIA_LNK_FL_ENABLED |
+					    MEDIA_LNK_FL_IMMUTABLE);
+	if (ret) {
+		dev_err(dev, "Unable to link %s:%u -> %s:%u\n",
+			rxport->source.sd->name, rxport->source.pad,
+			priv->sd.name, nport);
+		return ret;
+	}
+
+	for (i = 0; i < priv->hw_data->num_rxports; i++) {
+		if (priv->rxports[i] && !priv->rxports[i]->source.sd) {
+			dev_dbg(dev, "Waiting for more subdevs to be bound\n");
+			return 0;
+		}
+	}
+
+	return 0;
+}
+
+static void ub960_notify_unbind(struct v4l2_async_notifier *notifier,
+				struct v4l2_subdev *subdev,
+				struct v4l2_async_connection *asd)
+{
+	struct ub960_rxport *rxport = to_ub960_asd(asd)->rxport;
+
+	rxport->source.sd = NULL;
+}
+
+static const struct v4l2_async_notifier_operations ub960_notify_ops = {
+	.bound = ub960_notify_bound,
+	.unbind = ub960_notify_unbind,
+};
+
+static int ub960_v4l2_notifier_register(struct ub960_data *priv)
+{
+	struct device *dev = &priv->client->dev;
+	unsigned int i;
+	int ret;
+
+	v4l2_async_subdev_nf_init(&priv->notifier, &priv->sd);
+
+	for (i = 0; i < priv->hw_data->num_rxports; i++) {
+		struct ub960_rxport *rxport = priv->rxports[i];
+		struct ub960_asd *asd;
+
+		if (!rxport)
+			continue;
+
+		asd = v4l2_async_nf_add_fwnode(&priv->notifier,
+					       rxport->source.ep_fwnode,
+					       struct ub960_asd);
+		if (IS_ERR(asd)) {
+			dev_err(dev, "Failed to add subdev for source %u: %pe",
+				i, asd);
+			v4l2_async_nf_cleanup(&priv->notifier);
+			return PTR_ERR(asd);
+		}
+
+		asd->rxport = rxport;
+	}
+
+	priv->notifier.ops = &ub960_notify_ops;
+
+	ret = v4l2_async_nf_register(&priv->notifier);
+	if (ret) {
+		dev_err(dev, "Failed to register subdev_notifier");
+		v4l2_async_nf_cleanup(&priv->notifier);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void ub960_v4l2_notifier_unregister(struct ub960_data *priv)
+{
+	v4l2_async_nf_unregister(&priv->notifier);
+	v4l2_async_nf_cleanup(&priv->notifier);
+}
+
+static int ub960_create_subdev(struct ub960_data *priv)
+{
+	struct device *dev = &priv->client->dev;
+	unsigned int i;
+	int ret;
+
+	v4l2_i2c_subdev_init(&priv->sd, priv->client, &ub960_subdev_ops);
+
+	v4l2_ctrl_handler_init(&priv->ctrl_handler, 1);
+	priv->sd.ctrl_handler = &priv->ctrl_handler;
+
+	v4l2_ctrl_new_int_menu(&priv->ctrl_handler, NULL, V4L2_CID_LINK_FREQ,
+			       ARRAY_SIZE(priv->tx_link_freq) - 1, 0,
+			       priv->tx_link_freq);
+
+	if (priv->ctrl_handler.error) {
+		ret = priv->ctrl_handler.error;
+		goto err_free_ctrl;
+	}
+
+	priv->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+			  V4L2_SUBDEV_FL_HAS_EVENTS | V4L2_SUBDEV_FL_STREAMS;
+	priv->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
+	priv->sd.entity.ops = &ub960_entity_ops;
+
+	for (i = 0; i < priv->hw_data->num_rxports + priv->hw_data->num_txports; i++) {
+		priv->pads[i].flags = ub960_pad_is_sink(priv, i) ?
+					      MEDIA_PAD_FL_SINK :
+					      MEDIA_PAD_FL_SOURCE;
+	}
+
+	ret = media_entity_pads_init(&priv->sd.entity,
+				     priv->hw_data->num_rxports +
+					     priv->hw_data->num_txports,
+				     priv->pads);
+	if (ret)
+		goto err_free_ctrl;
+
+	priv->sd.state_lock = priv->sd.ctrl_handler->lock;
+
+	ret = v4l2_subdev_init_finalize(&priv->sd);
+	if (ret)
+		goto err_entity_cleanup;
+
+	ret = ub960_v4l2_notifier_register(priv);
+	if (ret) {
+		dev_err(dev, "v4l2 subdev notifier register failed: %d\n", ret);
+		goto err_subdev_cleanup;
+	}
+
+	ret = v4l2_async_register_subdev(&priv->sd);
+	if (ret) {
+		dev_err(dev, "v4l2_async_register_subdev error: %d\n", ret);
+		goto err_unreg_notif;
+	}
+
+	return 0;
+
+err_unreg_notif:
+	ub960_v4l2_notifier_unregister(priv);
+err_subdev_cleanup:
+	v4l2_subdev_cleanup(&priv->sd);
+err_entity_cleanup:
+	media_entity_cleanup(&priv->sd.entity);
+err_free_ctrl:
+	v4l2_ctrl_handler_free(&priv->ctrl_handler);
+
+	return ret;
+}
+
+static void ub960_destroy_subdev(struct ub960_data *priv)
+{
+	ub960_v4l2_notifier_unregister(priv);
+	v4l2_async_unregister_subdev(&priv->sd);
+
+	v4l2_subdev_cleanup(&priv->sd);
+
+	media_entity_cleanup(&priv->sd.entity);
+	v4l2_ctrl_handler_free(&priv->ctrl_handler);
+}
+
+static const struct regmap_config ub960_regmap_config = {
+	.name = "ds90ub960",
+
+	.reg_bits = 8,
+	.val_bits = 8,
+
+	.max_register = 0xff,
+
+	/*
+	 * We do locking in the driver to cover the TX/RX port selection and the
+	 * indirect register access.
+	 */
+	.disable_locking = true,
+};
+
+static void ub960_reset(struct ub960_data *priv, bool reset_regs)
+{
+	struct device *dev = &priv->client->dev;
+	unsigned int v;
+	int ret;
+	u8 bit;
+
+	bit = reset_regs ? UB960_SR_RESET_DIGITAL_RESET1 :
+			   UB960_SR_RESET_DIGITAL_RESET0;
+
+	ub960_write(priv, UB960_SR_RESET, bit);
+
+	mutex_lock(&priv->reg_lock);
+
+	ret = regmap_read_poll_timeout(priv->regmap, UB960_SR_RESET, v,
+				       (v & bit) == 0, 2000, 100000);
+
+	mutex_unlock(&priv->reg_lock);
+
+	if (ret)
+		dev_err(dev, "reset failed: %d\n", ret);
+}
+
+static int ub960_get_hw_resources(struct ub960_data *priv)
+{
+	struct device *dev = &priv->client->dev;
+
+	priv->regmap = devm_regmap_init_i2c(priv->client, &ub960_regmap_config);
+	if (IS_ERR(priv->regmap))
+		return PTR_ERR(priv->regmap);
+
+	priv->vddio = devm_regulator_get(dev, "vddio");
+	if (IS_ERR(priv->vddio))
+		return dev_err_probe(dev, PTR_ERR(priv->vddio),
+				     "cannot get VDDIO regulator\n");
+
+	/* get power-down pin from DT */
+	priv->pd_gpio =
+		devm_gpiod_get_optional(dev, "powerdown", GPIOD_OUT_HIGH);
+	if (IS_ERR(priv->pd_gpio))
+		return dev_err_probe(dev, PTR_ERR(priv->pd_gpio),
+				     "Cannot get powerdown GPIO\n");
+
+	priv->refclk = devm_clk_get(dev, "refclk");
+	if (IS_ERR(priv->refclk))
+		return dev_err_probe(dev, PTR_ERR(priv->refclk),
+				     "Cannot get REFCLK\n");
+
+	return 0;
+}
+
+static int ub960_enable_core_hw(struct ub960_data *priv)
+{
+	struct device *dev = &priv->client->dev;
+	u8 rev_mask;
+	int ret;
+	u8 dev_sts;
+	u8 refclk_freq;
+
+	ret = regulator_enable(priv->vddio);
+	if (ret)
+		return dev_err_probe(dev, ret,
+				     "failed to enable VDDIO regulator\n");
+
+	ret = clk_prepare_enable(priv->refclk);
+	if (ret) {
+		dev_err_probe(dev, ret, "Failed to enable refclk\n");
+		goto err_disable_vddio;
+	}
+
+	if (priv->pd_gpio) {
+		gpiod_set_value_cansleep(priv->pd_gpio, 1);
+		/* wait min 2 ms for reset to complete */
+		fsleep(2000);
+		gpiod_set_value_cansleep(priv->pd_gpio, 0);
+		/* wait min 2 ms for power up to finish */
+		fsleep(2000);
+	}
+
+	ub960_reset(priv, true);
+
+	/* Runtime check register accessibility */
+	ret = ub960_read(priv, UB960_SR_REV_MASK, &rev_mask);
+	if (ret) {
+		dev_err_probe(dev, ret, "Cannot read first register, abort\n");
+		goto err_pd_gpio;
+	}
+
+	dev_dbg(dev, "Found %s (rev/mask %#04x)\n", priv->hw_data->model,
+		rev_mask);
+
+	ret = ub960_read(priv, UB960_SR_DEVICE_STS, &dev_sts);
+	if (ret)
+		goto err_pd_gpio;
+
+	ret = ub960_read(priv, UB960_XR_REFCLK_FREQ, &refclk_freq);
+	if (ret)
+		goto err_pd_gpio;
+
+	dev_dbg(dev, "refclk valid %u freq %u MHz (clk fw freq %lu MHz)\n",
+		!!(dev_sts & BIT(4)), refclk_freq,
+		clk_get_rate(priv->refclk) / 1000000);
+
+	/* Disable all RX ports by default */
+	ret = ub960_write(priv, UB960_SR_RX_PORT_CTL, 0);
+	if (ret)
+		goto err_pd_gpio;
+
+	/* release GPIO lock */
+	if (priv->hw_data->is_ub9702) {
+		ret = ub960_update_bits(priv, UB960_SR_RESET,
+					UB960_SR_RESET_GPIO_LOCK_RELEASE,
+					UB960_SR_RESET_GPIO_LOCK_RELEASE);
+		if (ret)
+			goto err_pd_gpio;
+	}
+
+	return 0;
+
+err_pd_gpio:
+	gpiod_set_value_cansleep(priv->pd_gpio, 1);
+	clk_disable_unprepare(priv->refclk);
+err_disable_vddio:
+	regulator_disable(priv->vddio);
+
+	return ret;
+}
+
+static void ub960_disable_core_hw(struct ub960_data *priv)
+{
+	gpiod_set_value_cansleep(priv->pd_gpio, 1);
+	clk_disable_unprepare(priv->refclk);
+	regulator_disable(priv->vddio);
+}
+
+static int ub960_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct ub960_data *priv;
+	unsigned int port_lock_mask;
+	unsigned int port_mask;
+	unsigned int nport;
+	int ret;
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->client = client;
+
+	priv->hw_data = device_get_match_data(dev);
+
+	mutex_init(&priv->reg_lock);
+
+	INIT_DELAYED_WORK(&priv->poll_work, ub960_handler_work);
+
+	/*
+	 * Initialize these to invalid values so that the first reg writes will
+	 * configure the target.
+	 */
+	priv->reg_current.indirect_target = 0xff;
+	priv->reg_current.rxport = 0xff;
+	priv->reg_current.txport = 0xff;
+
+	ret = ub960_get_hw_resources(priv);
+	if (ret)
+		goto err_mutex_destroy;
+
+	ret = ub960_enable_core_hw(priv);
+	if (ret)
+		goto err_mutex_destroy;
+
+	ret = ub960_parse_dt(priv);
+	if (ret)
+		goto err_disable_core_hw;
+
+	ret = ub960_init_tx_ports(priv);
+	if (ret)
+		goto err_free_ports;
+
+	ret = ub960_rxport_enable_vpocs(priv);
+	if (ret)
+		goto err_free_ports;
+
+	ret = ub960_init_rx_ports(priv);
+	if (ret)
+		goto err_disable_vpocs;
+
+	ub960_reset(priv, false);
+
+	port_mask = 0;
+
+	for (nport = 0; nport < priv->hw_data->num_rxports; nport++) {
+		struct ub960_rxport *rxport = priv->rxports[nport];
+
+		if (!rxport)
+			continue;
+
+		port_mask |= BIT(nport);
+	}
+
+	ret = ub960_rxport_wait_locks(priv, port_mask, &port_lock_mask);
+	if (ret)
+		goto err_disable_vpocs;
+
+	if (port_mask != port_lock_mask) {
+		ret = -EIO;
+		dev_err_probe(dev, ret, "Failed to lock all RX ports\n");
+		goto err_disable_vpocs;
+	}
+
+	/*
+	 * Clear any errors caused by switching the RX port settings while
+	 * probing.
+	 */
+	ub960_clear_rx_errors(priv);
+
+	ret = ub960_init_atr(priv);
+	if (ret)
+		goto err_disable_vpocs;
+
+	ret = ub960_rxport_add_serializers(priv);
+	if (ret)
+		goto err_uninit_atr;
+
+	ret = ub960_create_subdev(priv);
+	if (ret)
+		goto err_free_sers;
+
+	if (client->irq)
+		dev_warn(dev, "irq support not implemented, using polling\n");
+
+	schedule_delayed_work(&priv->poll_work,
+			      msecs_to_jiffies(UB960_POLL_TIME_MS));
+
+	return 0;
+
+err_free_sers:
+	ub960_rxport_remove_serializers(priv);
+err_uninit_atr:
+	ub960_uninit_atr(priv);
+err_disable_vpocs:
+	ub960_rxport_disable_vpocs(priv);
+err_free_ports:
+	ub960_rxport_free_ports(priv);
+	ub960_txport_free_ports(priv);
+err_disable_core_hw:
+	ub960_disable_core_hw(priv);
+err_mutex_destroy:
+	mutex_destroy(&priv->reg_lock);
+	return ret;
+}
+
+static void ub960_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ub960_data *priv = sd_to_ub960(sd);
+
+	cancel_delayed_work_sync(&priv->poll_work);
+
+	ub960_destroy_subdev(priv);
+	ub960_rxport_remove_serializers(priv);
+	ub960_uninit_atr(priv);
+	ub960_rxport_disable_vpocs(priv);
+	ub960_rxport_free_ports(priv);
+	ub960_txport_free_ports(priv);
+	ub960_disable_core_hw(priv);
+	mutex_destroy(&priv->reg_lock);
+}
+
+static const struct ub960_hw_data ds90ub960_hw = {
+	.model = "ub960",
+	.num_rxports = 4,
+	.num_txports = 2,
+};
+
+static const struct ub960_hw_data ds90ub9702_hw = {
+	.model = "ub9702",
+	.num_rxports = 4,
+	.num_txports = 2,
+	.is_ub9702 = true,
+	.is_fpdlink4 = true,
+};
+
+static const struct i2c_device_id ub960_id[] = {
+	{ "ds90ub960-q1", (kernel_ulong_t)&ds90ub960_hw },
+	{ "ds90ub9702-q1", (kernel_ulong_t)&ds90ub9702_hw },
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, ub960_id);
+
+static const struct of_device_id ub960_dt_ids[] = {
+	{ .compatible = "ti,ds90ub960-q1", .data = &ds90ub960_hw },
+	{ .compatible = "ti,ds90ub9702-q1", .data = &ds90ub9702_hw },
+	{}
+};
+MODULE_DEVICE_TABLE(of, ub960_dt_ids);
+
+static struct i2c_driver ds90ub960_driver = {
+	.probe		= ub960_probe,
+	.remove		= ub960_remove,
+	.id_table	= ub960_id,
+	.driver = {
+		.name	= "ds90ub960",
+		.of_match_table = ub960_dt_ids,
+	},
+};
+module_i2c_driver(ds90ub960_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Texas Instruments FPD-Link III/IV Deserializers Driver");
+MODULE_AUTHOR("Luca Ceresoli <luca@lucaceresoli.net>");
+MODULE_AUTHOR("Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>");
+MODULE_IMPORT_NS(I2C_ATR);
diff --git a/drivers/media/i2c/dw9714.c b/drivers/media/i2c/dw9714.c
new file mode 100644
index 000000000..cc09b32ed
--- /dev/null
+++ b/drivers/media/i2c/dw9714.c
@@ -0,0 +1,315 @@
+// SPDX-License-Identifier: GPL-2.0
+// Copyright (c) 2015--2017 Intel Corporation.
+
+#include <linux/delay.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>
+
+#define DW9714_NAME		"dw9714"
+#define DW9714_MAX_FOCUS_POS	1023
+/*
+ * This sets the minimum granularity for the focus positions.
+ * A value of 1 gives maximum accuracy for a desired focus position
+ */
+#define DW9714_FOCUS_STEPS	1
+/*
+ * This acts as the minimum granularity of lens movement.
+ * Keep this value power of 2, so the control steps can be
+ * uniformly adjusted for gradual lens movement, with desired
+ * number of control steps.
+ */
+#define DW9714_CTRL_STEPS	16
+#define DW9714_CTRL_DELAY_US	1000
+/*
+ * S[3:2] = 0x00, codes per step for "Linear Slope Control"
+ * S[1:0] = 0x00, step period
+ */
+#define DW9714_DEFAULT_S 0x0
+#define DW9714_VAL(data, s) ((data) << 4 | (s))
+
+/* dw9714 device structure */
+struct dw9714_device {
+	struct v4l2_ctrl_handler ctrls_vcm;
+	struct v4l2_subdev sd;
+	u16 current_val;
+	struct regulator *vcc;
+};
+
+static inline struct dw9714_device *to_dw9714_vcm(struct v4l2_ctrl *ctrl)
+{
+	return container_of(ctrl->handler, struct dw9714_device, ctrls_vcm);
+}
+
+static inline struct dw9714_device *sd_to_dw9714_vcm(struct v4l2_subdev *subdev)
+{
+	return container_of(subdev, struct dw9714_device, sd);
+}
+
+static int dw9714_i2c_write(struct i2c_client *client, u16 data)
+{
+	int ret;
+	__be16 val = cpu_to_be16(data);
+
+	ret = i2c_master_send(client, (const char *)&val, sizeof(val));
+	if (ret != sizeof(val)) {
+		dev_err(&client->dev, "I2C write fail\n");
+		return -EIO;
+	}
+	return 0;
+}
+
+static int dw9714_t_focus_vcm(struct dw9714_device *dw9714_dev, u16 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&dw9714_dev->sd);
+
+	dw9714_dev->current_val = val;
+
+	return dw9714_i2c_write(client, DW9714_VAL(val, DW9714_DEFAULT_S));
+}
+
+static int dw9714_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct dw9714_device *dev_vcm = to_dw9714_vcm(ctrl);
+
+	if (ctrl->id == V4L2_CID_FOCUS_ABSOLUTE)
+		return dw9714_t_focus_vcm(dev_vcm, ctrl->val);
+
+	return -EINVAL;
+}
+
+static const struct v4l2_ctrl_ops dw9714_vcm_ctrl_ops = {
+	.s_ctrl = dw9714_set_ctrl,
+};
+
+static int dw9714_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	return pm_runtime_resume_and_get(sd->dev);
+}
+
+static int dw9714_close(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	pm_runtime_put(sd->dev);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_internal_ops dw9714_int_ops = {
+	.open = dw9714_open,
+	.close = dw9714_close,
+};
+
+static const struct v4l2_subdev_core_ops dw9714_core_ops = {
+	.log_status = v4l2_ctrl_subdev_log_status,
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_ops dw9714_ops = {
+	.core = &dw9714_core_ops,
+};
+
+static void dw9714_subdev_cleanup(struct dw9714_device *dw9714_dev)
+{
+	v4l2_async_unregister_subdev(&dw9714_dev->sd);
+	v4l2_ctrl_handler_free(&dw9714_dev->ctrls_vcm);
+	media_entity_cleanup(&dw9714_dev->sd.entity);
+}
+
+static int dw9714_init_controls(struct dw9714_device *dev_vcm)
+{
+	struct v4l2_ctrl_handler *hdl = &dev_vcm->ctrls_vcm;
+	const struct v4l2_ctrl_ops *ops = &dw9714_vcm_ctrl_ops;
+
+	v4l2_ctrl_handler_init(hdl, 1);
+
+	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_FOCUS_ABSOLUTE,
+			  0, DW9714_MAX_FOCUS_POS, DW9714_FOCUS_STEPS, 0);
+
+	if (hdl->error)
+		dev_err(dev_vcm->sd.dev, "%s fail error: 0x%x\n",
+			__func__, hdl->error);
+	dev_vcm->sd.ctrl_handler = hdl;
+	return hdl->error;
+}
+
+static int dw9714_probe(struct i2c_client *client)
+{
+	struct dw9714_device *dw9714_dev;
+	int rval;
+
+	dw9714_dev = devm_kzalloc(&client->dev, sizeof(*dw9714_dev),
+				  GFP_KERNEL);
+	if (dw9714_dev == NULL)
+		return -ENOMEM;
+
+	dw9714_dev->vcc = devm_regulator_get(&client->dev, "vcc");
+	if (IS_ERR(dw9714_dev->vcc))
+		return PTR_ERR(dw9714_dev->vcc);
+
+	rval = regulator_enable(dw9714_dev->vcc);
+	if (rval < 0) {
+		dev_err(&client->dev, "failed to enable vcc: %d\n", rval);
+		return rval;
+	}
+
+	v4l2_i2c_subdev_init(&dw9714_dev->sd, client, &dw9714_ops);
+	dw9714_dev->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+				V4L2_SUBDEV_FL_HAS_EVENTS;
+	dw9714_dev->sd.internal_ops = &dw9714_int_ops;
+
+	rval = dw9714_init_controls(dw9714_dev);
+	if (rval)
+		goto err_cleanup;
+
+	rval = media_entity_pads_init(&dw9714_dev->sd.entity, 0, NULL);
+	if (rval < 0)
+		goto err_cleanup;
+
+	dw9714_dev->sd.entity.function = MEDIA_ENT_F_LENS;
+
+	rval = v4l2_async_register_subdev(&dw9714_dev->sd);
+	if (rval < 0)
+		goto err_cleanup;
+
+	pm_runtime_set_active(&client->dev);
+	pm_runtime_enable(&client->dev);
+	pm_runtime_idle(&client->dev);
+
+	return 0;
+
+err_cleanup:
+	regulator_disable(dw9714_dev->vcc);
+	v4l2_ctrl_handler_free(&dw9714_dev->ctrls_vcm);
+	media_entity_cleanup(&dw9714_dev->sd.entity);
+
+	return rval;
+}
+
+static void dw9714_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct dw9714_device *dw9714_dev = sd_to_dw9714_vcm(sd);
+	int ret;
+
+	pm_runtime_disable(&client->dev);
+	if (!pm_runtime_status_suspended(&client->dev)) {
+		ret = regulator_disable(dw9714_dev->vcc);
+		if (ret) {
+			dev_err(&client->dev,
+				"Failed to disable vcc: %d\n", ret);
+		}
+	}
+	pm_runtime_set_suspended(&client->dev);
+	dw9714_subdev_cleanup(dw9714_dev);
+}
+
+/*
+ * This function sets the vcm position, so it consumes least current
+ * The lens position is gradually moved in units of DW9714_CTRL_STEPS,
+ * to make the movements smoothly.
+ */
+static int __maybe_unused dw9714_vcm_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct dw9714_device *dw9714_dev = sd_to_dw9714_vcm(sd);
+	int ret, val;
+
+	if (pm_runtime_suspended(&client->dev))
+		return 0;
+
+	for (val = dw9714_dev->current_val & ~(DW9714_CTRL_STEPS - 1);
+	     val >= 0; val -= DW9714_CTRL_STEPS) {
+		ret = dw9714_i2c_write(client,
+				       DW9714_VAL(val, DW9714_DEFAULT_S));
+		if (ret)
+			dev_err_once(dev, "%s I2C failure: %d", __func__, ret);
+		usleep_range(DW9714_CTRL_DELAY_US, DW9714_CTRL_DELAY_US + 10);
+	}
+
+	ret = regulator_disable(dw9714_dev->vcc);
+	if (ret)
+		dev_err(dev, "Failed to disable vcc: %d\n", ret);
+
+	return ret;
+}
+
+/*
+ * This function sets the vcm position to the value set by the user
+ * through v4l2_ctrl_ops s_ctrl handler
+ * The lens position is gradually moved in units of DW9714_CTRL_STEPS,
+ * to make the movements smoothly.
+ */
+static int  __maybe_unused dw9714_vcm_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct dw9714_device *dw9714_dev = sd_to_dw9714_vcm(sd);
+	int ret, val;
+
+	if (pm_runtime_suspended(&client->dev))
+		return 0;
+
+	ret = regulator_enable(dw9714_dev->vcc);
+	if (ret) {
+		dev_err(dev, "Failed to enable vcc: %d\n", ret);
+		return ret;
+	}
+	usleep_range(1000, 2000);
+
+	for (val = dw9714_dev->current_val % DW9714_CTRL_STEPS;
+	     val < dw9714_dev->current_val + DW9714_CTRL_STEPS - 1;
+	     val += DW9714_CTRL_STEPS) {
+		ret = dw9714_i2c_write(client,
+				       DW9714_VAL(val, DW9714_DEFAULT_S));
+		if (ret)
+			dev_err_ratelimited(dev, "%s I2C failure: %d",
+						__func__, ret);
+		usleep_range(DW9714_CTRL_DELAY_US, DW9714_CTRL_DELAY_US + 10);
+	}
+
+	return 0;
+}
+
+static const struct i2c_device_id dw9714_id_table[] = {
+	{ DW9714_NAME, 0 },
+	{ { 0 } }
+};
+MODULE_DEVICE_TABLE(i2c, dw9714_id_table);
+
+static const struct of_device_id dw9714_of_table[] = {
+	{ .compatible = "dongwoon,dw9714" },
+	{ { 0 } }
+};
+MODULE_DEVICE_TABLE(of, dw9714_of_table);
+
+static const struct dev_pm_ops dw9714_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(dw9714_vcm_suspend, dw9714_vcm_resume)
+	SET_RUNTIME_PM_OPS(dw9714_vcm_suspend, dw9714_vcm_resume, NULL)
+};
+
+static struct i2c_driver dw9714_i2c_driver = {
+	.driver = {
+		.name = DW9714_NAME,
+		.pm = &dw9714_pm_ops,
+		.of_match_table = dw9714_of_table,
+	},
+	.probe = dw9714_probe,
+	.remove = dw9714_remove,
+	.id_table = dw9714_id_table,
+};
+
+module_i2c_driver(dw9714_i2c_driver);
+
+MODULE_AUTHOR("Tianshu Qiu <tian.shu.qiu@intel.com>");
+MODULE_AUTHOR("Jian Xu Zheng");
+MODULE_AUTHOR("Yuning Pu <yuning.pu@intel.com>");
+MODULE_AUTHOR("Jouni Ukkonen <jouni.ukkonen@intel.com>");
+MODULE_AUTHOR("Tommi Franttila <tommi.franttila@intel.com>");
+MODULE_DESCRIPTION("DW9714 VCM driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/dw9719.c b/drivers/media/i2c/dw9719.c
new file mode 100644
index 000000000..c626ed845
--- /dev/null
+++ b/drivers/media/i2c/dw9719.c
@@ -0,0 +1,350 @@
+// SPDX-License-Identifier: GPL-2.0
+// Copyright (c) 2012 Intel Corporation
+
+/*
+ * Based on linux/modules/camera/drivers/media/i2c/imx/dw9719.c in this repo:
+ * https://github.com/ZenfoneArea/android_kernel_asus_zenfone5
+ */
+
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/pm_runtime.h>
+#include <linux/regulator/consumer.h>
+#include <linux/types.h>
+
+#include <media/v4l2-cci.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-subdev.h>
+
+#define DW9719_MAX_FOCUS_POS	1023
+#define DW9719_CTRL_STEPS	16
+#define DW9719_CTRL_DELAY_US	1000
+
+#define DW9719_INFO			CCI_REG8(0)
+#define DW9719_ID			0xF1
+
+#define DW9719_CONTROL			CCI_REG8(2)
+#define DW9719_ENABLE_RINGING		0x02
+
+#define DW9719_VCM_CURRENT		CCI_REG16(3)
+
+#define DW9719_MODE			CCI_REG8(6)
+#define DW9719_MODE_SAC_SHIFT		4
+#define DW9719_MODE_SAC3		4
+
+#define DW9719_VCM_FREQ			CCI_REG8(7)
+#define DW9719_DEFAULT_VCM_FREQ		0x60
+
+#define to_dw9719_device(x) container_of(x, struct dw9719_device, sd)
+
+struct dw9719_device {
+	struct v4l2_subdev sd;
+	struct device *dev;
+	struct regmap *regmap;
+	struct regulator *regulator;
+	u32 sac_mode;
+	u32 vcm_freq;
+
+	struct dw9719_v4l2_ctrls {
+		struct v4l2_ctrl_handler handler;
+		struct v4l2_ctrl *focus;
+	} ctrls;
+};
+
+static int dw9719_detect(struct dw9719_device *dw9719)
+{
+	int ret;
+	u64 val;
+
+	ret = cci_read(dw9719->regmap, DW9719_INFO, &val, NULL);
+	if (ret < 0)
+		return ret;
+
+	if (val != DW9719_ID) {
+		dev_err(dw9719->dev, "Failed to detect correct id\n");
+		return -ENXIO;
+	}
+
+	return 0;
+}
+
+static int dw9719_power_down(struct dw9719_device *dw9719)
+{
+	return regulator_disable(dw9719->regulator);
+}
+
+static int dw9719_power_up(struct dw9719_device *dw9719)
+{
+	int ret;
+
+	ret = regulator_enable(dw9719->regulator);
+	if (ret)
+		return ret;
+
+	/* Jiggle SCL pin to wake up device */
+	cci_write(dw9719->regmap, DW9719_CONTROL, 1, &ret);
+
+	/* Need 100us to transit from SHUTDOWN to STANDBY */
+	fsleep(100);
+
+	cci_write(dw9719->regmap, DW9719_CONTROL, DW9719_ENABLE_RINGING, &ret);
+	cci_write(dw9719->regmap, DW9719_MODE,
+		  dw9719->sac_mode << DW9719_MODE_SAC_SHIFT, &ret);
+	cci_write(dw9719->regmap, DW9719_VCM_FREQ, dw9719->vcm_freq, &ret);
+
+	if (ret)
+		dw9719_power_down(dw9719);
+
+	return ret;
+}
+
+static int dw9719_t_focus_abs(struct dw9719_device *dw9719, s32 value)
+{
+	return cci_write(dw9719->regmap, DW9719_VCM_CURRENT, value, NULL);
+}
+
+static int dw9719_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct dw9719_device *dw9719 = container_of(ctrl->handler,
+						    struct dw9719_device,
+						    ctrls.handler);
+	int ret;
+
+	/* Only apply changes to the controls if the device is powered up */
+	if (!pm_runtime_get_if_in_use(dw9719->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_FOCUS_ABSOLUTE:
+		ret = dw9719_t_focus_abs(dw9719, ctrl->val);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	pm_runtime_put(dw9719->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops dw9719_ctrl_ops = {
+	.s_ctrl = dw9719_set_ctrl,
+};
+
+static int dw9719_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct dw9719_device *dw9719 = to_dw9719_device(sd);
+	int ret;
+	int val;
+
+	for (val = dw9719->ctrls.focus->val; val >= 0;
+	     val -= DW9719_CTRL_STEPS) {
+		ret = dw9719_t_focus_abs(dw9719, val);
+		if (ret)
+			return ret;
+
+		usleep_range(DW9719_CTRL_DELAY_US, DW9719_CTRL_DELAY_US + 10);
+	}
+
+	return dw9719_power_down(dw9719);
+}
+
+static int dw9719_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct dw9719_device *dw9719 = to_dw9719_device(sd);
+	int current_focus = dw9719->ctrls.focus->val;
+	int ret;
+	int val;
+
+	ret = dw9719_power_up(dw9719);
+	if (ret)
+		return ret;
+
+	for (val = current_focus % DW9719_CTRL_STEPS; val < current_focus;
+	     val += DW9719_CTRL_STEPS) {
+		ret = dw9719_t_focus_abs(dw9719, val);
+		if (ret)
+			goto err_power_down;
+
+		usleep_range(DW9719_CTRL_DELAY_US, DW9719_CTRL_DELAY_US + 10);
+	}
+
+	return 0;
+
+err_power_down:
+	dw9719_power_down(dw9719);
+	return ret;
+}
+
+static int dw9719_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	return pm_runtime_resume_and_get(sd->dev);
+}
+
+static int dw9719_close(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	pm_runtime_put(sd->dev);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_internal_ops dw9719_internal_ops = {
+	.open = dw9719_open,
+	.close = dw9719_close,
+};
+
+static int dw9719_init_controls(struct dw9719_device *dw9719)
+{
+	const struct v4l2_ctrl_ops *ops = &dw9719_ctrl_ops;
+	int ret;
+
+	v4l2_ctrl_handler_init(&dw9719->ctrls.handler, 1);
+
+	dw9719->ctrls.focus = v4l2_ctrl_new_std(&dw9719->ctrls.handler, ops,
+						V4L2_CID_FOCUS_ABSOLUTE, 0,
+						DW9719_MAX_FOCUS_POS, 1, 0);
+
+	if (dw9719->ctrls.handler.error) {
+		dev_err(dw9719->dev, "Error initialising v4l2 ctrls\n");
+		ret = dw9719->ctrls.handler.error;
+		goto err_free_handler;
+	}
+
+	dw9719->sd.ctrl_handler = &dw9719->ctrls.handler;
+	return 0;
+
+err_free_handler:
+	v4l2_ctrl_handler_free(&dw9719->ctrls.handler);
+	return ret;
+}
+
+static const struct v4l2_subdev_ops dw9719_ops = { };
+
+static int dw9719_probe(struct i2c_client *client)
+{
+	struct dw9719_device *dw9719;
+	int ret;
+
+	dw9719 = devm_kzalloc(&client->dev, sizeof(*dw9719), GFP_KERNEL);
+	if (!dw9719)
+		return -ENOMEM;
+
+	dw9719->regmap = devm_cci_regmap_init_i2c(client, 8);
+	if (IS_ERR(dw9719->regmap))
+		return PTR_ERR(dw9719->regmap);
+
+	dw9719->dev = &client->dev;
+	dw9719->sac_mode = DW9719_MODE_SAC3;
+	dw9719->vcm_freq = DW9719_DEFAULT_VCM_FREQ;
+
+	/* Optional indication of SAC mode select */
+	device_property_read_u32(&client->dev, "dongwoon,sac-mode",
+				 &dw9719->sac_mode);
+
+	/* Optional indication of VCM frequency */
+	device_property_read_u32(&client->dev, "dongwoon,vcm-freq",
+				 &dw9719->vcm_freq);
+
+	dw9719->regulator = devm_regulator_get(&client->dev, "vdd");
+	if (IS_ERR(dw9719->regulator))
+		return dev_err_probe(&client->dev, PTR_ERR(dw9719->regulator),
+				     "getting regulator\n");
+
+	v4l2_i2c_subdev_init(&dw9719->sd, client, &dw9719_ops);
+	dw9719->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	dw9719->sd.internal_ops = &dw9719_internal_ops;
+
+	ret = dw9719_init_controls(dw9719);
+	if (ret)
+		return ret;
+
+	ret = media_entity_pads_init(&dw9719->sd.entity, 0, NULL);
+	if (ret < 0)
+		goto err_free_ctrl_handler;
+
+	dw9719->sd.entity.function = MEDIA_ENT_F_LENS;
+
+	/*
+	 * We need the driver to work in the event that pm runtime is disable in
+	 * the kernel, so power up and verify the chip now. In the event that
+	 * runtime pm is disabled this will leave the chip on, so that the lens
+	 * will work.
+	 */
+
+	ret = dw9719_power_up(dw9719);
+	if (ret)
+		goto err_cleanup_media;
+
+	ret = dw9719_detect(dw9719);
+	if (ret)
+		goto err_powerdown;
+
+	pm_runtime_set_active(&client->dev);
+	pm_runtime_get_noresume(&client->dev);
+	pm_runtime_enable(&client->dev);
+
+	ret = v4l2_async_register_subdev(&dw9719->sd);
+	if (ret < 0)
+		goto err_pm_runtime;
+
+	pm_runtime_set_autosuspend_delay(&client->dev, 1000);
+	pm_runtime_use_autosuspend(&client->dev);
+	pm_runtime_put_autosuspend(&client->dev);
+
+	return ret;
+
+err_pm_runtime:
+	pm_runtime_disable(&client->dev);
+	pm_runtime_put_noidle(&client->dev);
+err_powerdown:
+	dw9719_power_down(dw9719);
+err_cleanup_media:
+	media_entity_cleanup(&dw9719->sd.entity);
+err_free_ctrl_handler:
+	v4l2_ctrl_handler_free(&dw9719->ctrls.handler);
+
+	return ret;
+}
+
+static void dw9719_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct dw9719_device *dw9719 =
+		container_of(sd, struct dw9719_device, sd);
+
+	v4l2_async_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&dw9719->ctrls.handler);
+	media_entity_cleanup(&dw9719->sd.entity);
+
+	pm_runtime_disable(&client->dev);
+	if (!pm_runtime_status_suspended(&client->dev))
+		dw9719_power_down(dw9719);
+	pm_runtime_set_suspended(&client->dev);
+}
+
+static const struct i2c_device_id dw9719_id_table[] = {
+	{ "dw9719" },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, dw9719_id_table);
+
+static DEFINE_RUNTIME_DEV_PM_OPS(dw9719_pm_ops, dw9719_suspend, dw9719_resume,
+				 NULL);
+
+static struct i2c_driver dw9719_i2c_driver = {
+	.driver = {
+		.name = "dw9719",
+		.pm = pm_sleep_ptr(&dw9719_pm_ops),
+	},
+	.probe = dw9719_probe,
+	.remove = dw9719_remove,
+	.id_table = dw9719_id_table,
+};
+module_i2c_driver(dw9719_i2c_driver);
+
+MODULE_AUTHOR("Daniel Scally <djrscally@gmail.com>");
+MODULE_DESCRIPTION("DW9719 VCM Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/dw9768.c b/drivers/media/i2c/dw9768.c
new file mode 100644
index 000000000..daabbece8
--- /dev/null
+++ b/drivers/media/i2c/dw9768.c
@@ -0,0 +1,559 @@
+// SPDX-License-Identifier: GPL-2.0
+// Copyright (c) 2020 MediaTek Inc.
+
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <linux/regulator/consumer.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+#define DW9768_NAME				"dw9768"
+#define DW9768_MAX_FOCUS_POS			(1024 - 1)
+/*
+ * This sets the minimum granularity for the focus positions.
+ * A value of 1 gives maximum accuracy for a desired focus position
+ */
+#define DW9768_FOCUS_STEPS			1
+
+/*
+ * Ring control and Power control register
+ * Bit[1] RING_EN
+ * 0: Direct mode
+ * 1: AAC mode (ringing control mode)
+ * Bit[0] PD
+ * 0: Normal operation mode
+ * 1: Power down mode
+ * DW9768 requires waiting time of Topr after PD reset takes place.
+ */
+#define DW9768_RING_PD_CONTROL_REG		0x02
+#define DW9768_PD_MODE_OFF			0x00
+#define DW9768_PD_MODE_EN			BIT(0)
+#define DW9768_AAC_MODE_EN			BIT(1)
+
+/*
+ * DW9768 separates two registers to control the VCM position.
+ * One for MSB value, another is LSB value.
+ * DAC_MSB: D[9:8] (ADD: 0x03)
+ * DAC_LSB: D[7:0] (ADD: 0x04)
+ * D[9:0] DAC data input: positive output current = D[9:0] / 1023 * 100[mA]
+ */
+#define DW9768_MSB_ADDR				0x03
+#define DW9768_LSB_ADDR				0x04
+#define DW9768_STATUS_ADDR			0x05
+
+/*
+ * AAC mode control & prescale register
+ * Bit[7:5] Namely AC[2:0], decide the VCM mode and operation time.
+ * 001 AAC2 0.48 x Tvib
+ * 010 AAC3 0.70 x Tvib
+ * 011 AAC4 0.75 x Tvib
+ * 101 AAC8 1.13 x Tvib
+ * Bit[2:0] Namely PRESC[2:0], set the internal clock dividing rate as follow.
+ * 000 2
+ * 001 1
+ * 010 1/2
+ * 011 1/4
+ * 100 8
+ * 101 4
+ */
+#define DW9768_AAC_PRESC_REG			0x06
+#define DW9768_AAC_MODE_SEL_MASK		GENMASK(7, 5)
+#define DW9768_CLOCK_PRE_SCALE_SEL_MASK		GENMASK(2, 0)
+
+/*
+ * VCM period of vibration register
+ * Bit[5:0] Defined as VCM rising periodic time (Tvib) together with PRESC[2:0]
+ * Tvib = (6.3ms + AACT[5:0] * 0.1ms) * Dividing Rate
+ * Dividing Rate is the internal clock dividing rate that is defined at
+ * PRESCALE register (ADD: 0x06)
+ */
+#define DW9768_AAC_TIME_REG			0x07
+
+/*
+ * DW9768 requires waiting time (delay time) of t_OPR after power-up,
+ * or in the case of PD reset taking place.
+ */
+#define DW9768_T_OPR_US				1000
+#define DW9768_TVIB_MS_BASE10			(64 - 1)
+#define DW9768_AAC_MODE_DEFAULT			2
+#define DW9768_AAC_TIME_DEFAULT			0x20
+#define DW9768_CLOCK_PRE_SCALE_DEFAULT		1
+
+/*
+ * This acts as the minimum granularity of lens movement.
+ * Keep this value power of 2, so the control steps can be
+ * uniformly adjusted for gradual lens movement, with desired
+ * number of control steps.
+ */
+#define DW9768_MOVE_STEPS			16
+
+static const char * const dw9768_supply_names[] = {
+	"vin",	/* Digital I/O power */
+	"vdd",	/* Digital core power */
+};
+
+/* dw9768 device structure */
+struct dw9768 {
+	struct regulator_bulk_data supplies[ARRAY_SIZE(dw9768_supply_names)];
+	struct v4l2_ctrl_handler ctrls;
+	struct v4l2_ctrl *focus;
+	struct v4l2_subdev sd;
+
+	u32 aac_mode;
+	u32 aac_timing;
+	u32 clock_presc;
+	u32 move_delay_us;
+};
+
+static inline struct dw9768 *sd_to_dw9768(struct v4l2_subdev *subdev)
+{
+	return container_of(subdev, struct dw9768, sd);
+}
+
+struct regval_list {
+	u8 reg_num;
+	u8 value;
+};
+
+struct dw9768_aac_mode_ot_multi {
+	u32 aac_mode_enum;
+	u32 ot_multi_base100;
+};
+
+struct dw9768_clk_presc_dividing_rate {
+	u32 clk_presc_enum;
+	u32 dividing_rate_base100;
+};
+
+static const struct dw9768_aac_mode_ot_multi aac_mode_ot_multi[] = {
+	{1,  48},
+	{2,  70},
+	{3,  75},
+	{5, 113},
+};
+
+static const struct dw9768_clk_presc_dividing_rate presc_dividing_rate[] = {
+	{0, 200},
+	{1, 100},
+	{2,  50},
+	{3,  25},
+	{4, 800},
+	{5, 400},
+};
+
+static u32 dw9768_find_ot_multi(u32 aac_mode_param)
+{
+	u32 cur_ot_multi_base100 = 70;
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(aac_mode_ot_multi); i++) {
+		if (aac_mode_ot_multi[i].aac_mode_enum == aac_mode_param) {
+			cur_ot_multi_base100 =
+				aac_mode_ot_multi[i].ot_multi_base100;
+		}
+	}
+
+	return cur_ot_multi_base100;
+}
+
+static u32 dw9768_find_dividing_rate(u32 presc_param)
+{
+	u32 cur_clk_dividing_rate_base100 = 100;
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(presc_dividing_rate); i++) {
+		if (presc_dividing_rate[i].clk_presc_enum == presc_param) {
+			cur_clk_dividing_rate_base100 =
+				presc_dividing_rate[i].dividing_rate_base100;
+		}
+	}
+
+	return cur_clk_dividing_rate_base100;
+}
+
+/*
+ * DW9768_AAC_PRESC_REG & DW9768_AAC_TIME_REG determine VCM operation time.
+ * For current VCM mode: AAC3, Operation Time would be 0.70 x Tvib.
+ * Tvib = (6.3ms + AACT[5:0] * 0.1MS) * Dividing Rate.
+ * Below is calculation of the operation delay for each step.
+ */
+static inline u32 dw9768_cal_move_delay(u32 aac_mode_param, u32 presc_param,
+					u32 aac_timing_param)
+{
+	u32 Tvib_us;
+	u32 ot_multi_base100;
+	u32 clk_dividing_rate_base100;
+
+	ot_multi_base100 = dw9768_find_ot_multi(aac_mode_param);
+
+	clk_dividing_rate_base100 = dw9768_find_dividing_rate(presc_param);
+
+	Tvib_us = (DW9768_TVIB_MS_BASE10 + aac_timing_param) *
+		  clk_dividing_rate_base100;
+
+	return Tvib_us * ot_multi_base100 / 100;
+}
+
+static int dw9768_mod_reg(struct dw9768 *dw9768, u8 reg, u8 mask, u8 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&dw9768->sd);
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(client, reg);
+	if (ret < 0)
+		return ret;
+
+	val = ((unsigned char)ret & ~mask) | (val & mask);
+
+	return i2c_smbus_write_byte_data(client, reg, val);
+}
+
+static int dw9768_set_dac(struct dw9768 *dw9768, u16 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&dw9768->sd);
+
+	/* Write VCM position to registers */
+	return i2c_smbus_write_word_swapped(client, DW9768_MSB_ADDR, val);
+}
+
+static int dw9768_init(struct dw9768 *dw9768)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&dw9768->sd);
+	int ret, val;
+
+	/* Reset DW9768_RING_PD_CONTROL_REG to default status 0x00 */
+	ret = i2c_smbus_write_byte_data(client, DW9768_RING_PD_CONTROL_REG,
+					DW9768_PD_MODE_OFF);
+	if (ret < 0)
+		return ret;
+
+	/*
+	 * DW9769 requires waiting delay time of t_OPR
+	 * after PD reset takes place.
+	 */
+	usleep_range(DW9768_T_OPR_US, DW9768_T_OPR_US + 100);
+
+	/* Set DW9768_RING_PD_CONTROL_REG to DW9768_AAC_MODE_EN(0x01) */
+	ret = i2c_smbus_write_byte_data(client, DW9768_RING_PD_CONTROL_REG,
+					DW9768_AAC_MODE_EN);
+	if (ret < 0)
+		return ret;
+
+	/* Set AAC mode */
+	ret = dw9768_mod_reg(dw9768, DW9768_AAC_PRESC_REG,
+			     DW9768_AAC_MODE_SEL_MASK,
+			     dw9768->aac_mode << 5);
+	if (ret < 0)
+		return ret;
+
+	/* Set clock presc */
+	if (dw9768->clock_presc != DW9768_CLOCK_PRE_SCALE_DEFAULT) {
+		ret = dw9768_mod_reg(dw9768, DW9768_AAC_PRESC_REG,
+				     DW9768_CLOCK_PRE_SCALE_SEL_MASK,
+				     dw9768->clock_presc);
+		if (ret < 0)
+			return ret;
+	}
+
+	/* Set AAC Timing */
+	if (dw9768->aac_timing != DW9768_AAC_TIME_DEFAULT) {
+		ret = i2c_smbus_write_byte_data(client, DW9768_AAC_TIME_REG,
+						dw9768->aac_timing);
+		if (ret < 0)
+			return ret;
+	}
+
+	for (val = dw9768->focus->val % DW9768_MOVE_STEPS;
+	     val <= dw9768->focus->val;
+	     val += DW9768_MOVE_STEPS) {
+		ret = dw9768_set_dac(dw9768, val);
+		if (ret) {
+			dev_err(&client->dev, "I2C failure: %d", ret);
+			return ret;
+		}
+		usleep_range(dw9768->move_delay_us,
+			     dw9768->move_delay_us + 1000);
+	}
+
+	return 0;
+}
+
+static int dw9768_release(struct dw9768 *dw9768)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&dw9768->sd);
+	int ret, val;
+
+	val = round_down(dw9768->focus->val, DW9768_MOVE_STEPS);
+	for ( ; val >= 0; val -= DW9768_MOVE_STEPS) {
+		ret = dw9768_set_dac(dw9768, val);
+		if (ret) {
+			dev_err(&client->dev, "I2C write fail: %d", ret);
+			return ret;
+		}
+		usleep_range(dw9768->move_delay_us,
+			     dw9768->move_delay_us + 1000);
+	}
+
+	ret = i2c_smbus_write_byte_data(client, DW9768_RING_PD_CONTROL_REG,
+					DW9768_PD_MODE_EN);
+	if (ret < 0)
+		return ret;
+
+	/*
+	 * DW9769 requires waiting delay time of t_OPR
+	 * after PD reset takes place.
+	 */
+	usleep_range(DW9768_T_OPR_US, DW9768_T_OPR_US + 100);
+
+	return 0;
+}
+
+static int dw9768_runtime_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct dw9768 *dw9768 = sd_to_dw9768(sd);
+
+	dw9768_release(dw9768);
+	regulator_bulk_disable(ARRAY_SIZE(dw9768_supply_names),
+			       dw9768->supplies);
+
+	return 0;
+}
+
+static int dw9768_runtime_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct dw9768 *dw9768 = sd_to_dw9768(sd);
+	int ret;
+
+	ret = regulator_bulk_enable(ARRAY_SIZE(dw9768_supply_names),
+				    dw9768->supplies);
+	if (ret < 0) {
+		dev_err(dev, "failed to enable regulators\n");
+		return ret;
+	}
+
+	/*
+	 * The datasheet refers to t_OPR that needs to be waited before sending
+	 * I2C commands after power-up.
+	 */
+	usleep_range(DW9768_T_OPR_US, DW9768_T_OPR_US + 100);
+
+	ret = dw9768_init(dw9768);
+	if (ret < 0)
+		goto disable_regulator;
+
+	return 0;
+
+disable_regulator:
+	regulator_bulk_disable(ARRAY_SIZE(dw9768_supply_names),
+			       dw9768->supplies);
+
+	return ret;
+}
+
+static int dw9768_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct dw9768 *dw9768 = container_of(ctrl->handler,
+					     struct dw9768, ctrls);
+
+	if (ctrl->id == V4L2_CID_FOCUS_ABSOLUTE)
+		return dw9768_set_dac(dw9768, ctrl->val);
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops dw9768_ctrl_ops = {
+	.s_ctrl = dw9768_set_ctrl,
+};
+
+static int dw9768_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	return pm_runtime_resume_and_get(sd->dev);
+}
+
+static int dw9768_close(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	pm_runtime_put(sd->dev);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_internal_ops dw9768_int_ops = {
+	.open = dw9768_open,
+	.close = dw9768_close,
+};
+
+static const struct v4l2_subdev_ops dw9768_ops = { };
+
+static int dw9768_init_controls(struct dw9768 *dw9768)
+{
+	struct v4l2_ctrl_handler *hdl = &dw9768->ctrls;
+	const struct v4l2_ctrl_ops *ops = &dw9768_ctrl_ops;
+
+	v4l2_ctrl_handler_init(hdl, 1);
+
+	dw9768->focus = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_FOCUS_ABSOLUTE, 0,
+					  DW9768_MAX_FOCUS_POS,
+					  DW9768_FOCUS_STEPS, 0);
+
+	if (hdl->error)
+		return hdl->error;
+
+	dw9768->sd.ctrl_handler = hdl;
+
+	return 0;
+}
+
+static int dw9768_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct dw9768 *dw9768;
+	bool full_power;
+	unsigned int i;
+	int ret;
+
+	dw9768 = devm_kzalloc(dev, sizeof(*dw9768), GFP_KERNEL);
+	if (!dw9768)
+		return -ENOMEM;
+
+	/* Initialize subdev */
+	v4l2_i2c_subdev_init(&dw9768->sd, client, &dw9768_ops);
+
+	dw9768->aac_mode = DW9768_AAC_MODE_DEFAULT;
+	dw9768->aac_timing = DW9768_AAC_TIME_DEFAULT;
+	dw9768->clock_presc = DW9768_CLOCK_PRE_SCALE_DEFAULT;
+
+	/* Optional indication of AAC mode select */
+	fwnode_property_read_u32(dev_fwnode(dev), "dongwoon,aac-mode",
+				 &dw9768->aac_mode);
+
+	/* Optional indication of clock pre-scale select */
+	fwnode_property_read_u32(dev_fwnode(dev), "dongwoon,clock-presc",
+				 &dw9768->clock_presc);
+
+	/* Optional indication of AAC Timing */
+	fwnode_property_read_u32(dev_fwnode(dev), "dongwoon,aac-timing",
+				 &dw9768->aac_timing);
+
+	dw9768->move_delay_us = dw9768_cal_move_delay(dw9768->aac_mode,
+						      dw9768->clock_presc,
+						      dw9768->aac_timing);
+
+	for (i = 0; i < ARRAY_SIZE(dw9768_supply_names); i++)
+		dw9768->supplies[i].supply = dw9768_supply_names[i];
+
+	ret = devm_regulator_bulk_get(dev, ARRAY_SIZE(dw9768_supply_names),
+				      dw9768->supplies);
+	if (ret) {
+		dev_err(dev, "failed to get regulators\n");
+		return ret;
+	}
+
+	/* Initialize controls */
+	ret = dw9768_init_controls(dw9768);
+	if (ret)
+		goto err_free_handler;
+
+	/* Initialize subdev */
+	dw9768->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	dw9768->sd.internal_ops = &dw9768_int_ops;
+
+	ret = media_entity_pads_init(&dw9768->sd.entity, 0, NULL);
+	if (ret < 0)
+		goto err_free_handler;
+
+	dw9768->sd.entity.function = MEDIA_ENT_F_LENS;
+
+	/*
+	 * Figure out whether we're going to power up the device here. Generally
+	 * this is done if CONFIG_PM is disabled in a DT system or the device is
+	 * to be powered on in an ACPI system. Similarly for power off in
+	 * remove.
+	 */
+	pm_runtime_enable(dev);
+	full_power = (is_acpi_node(dev_fwnode(dev)) &&
+		      acpi_dev_state_d0(dev)) ||
+		     (is_of_node(dev_fwnode(dev)) && !pm_runtime_enabled(dev));
+	if (full_power) {
+		ret = dw9768_runtime_resume(dev);
+		if (ret < 0) {
+			dev_err(dev, "failed to power on: %d\n", ret);
+			goto err_clean_entity;
+		}
+		pm_runtime_set_active(dev);
+	}
+
+	ret = v4l2_async_register_subdev(&dw9768->sd);
+	if (ret < 0) {
+		dev_err(dev, "failed to register V4L2 subdev: %d", ret);
+		goto err_power_off;
+	}
+
+	pm_runtime_idle(dev);
+
+	return 0;
+
+err_power_off:
+	if (full_power) {
+		dw9768_runtime_suspend(dev);
+		pm_runtime_set_suspended(dev);
+	}
+err_clean_entity:
+	pm_runtime_disable(dev);
+	media_entity_cleanup(&dw9768->sd.entity);
+err_free_handler:
+	v4l2_ctrl_handler_free(&dw9768->ctrls);
+
+	return ret;
+}
+
+static void dw9768_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct dw9768 *dw9768 = sd_to_dw9768(sd);
+	struct device *dev = &client->dev;
+
+	v4l2_async_unregister_subdev(&dw9768->sd);
+	v4l2_ctrl_handler_free(&dw9768->ctrls);
+	media_entity_cleanup(&dw9768->sd.entity);
+	if ((is_acpi_node(dev_fwnode(dev)) && acpi_dev_state_d0(dev)) ||
+	    (is_of_node(dev_fwnode(dev)) && !pm_runtime_enabled(dev))) {
+		dw9768_runtime_suspend(dev);
+		pm_runtime_set_suspended(dev);
+	}
+	pm_runtime_disable(dev);
+}
+
+static const struct of_device_id dw9768_of_table[] = {
+	{ .compatible = "dongwoon,dw9768" },
+	{ .compatible = "giantec,gt9769" },
+	{}
+};
+MODULE_DEVICE_TABLE(of, dw9768_of_table);
+
+static const struct dev_pm_ops dw9768_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend,
+				pm_runtime_force_resume)
+	SET_RUNTIME_PM_OPS(dw9768_runtime_suspend, dw9768_runtime_resume, NULL)
+};
+
+static struct i2c_driver dw9768_i2c_driver = {
+	.driver = {
+		.name = DW9768_NAME,
+		.pm = &dw9768_pm_ops,
+		.of_match_table = dw9768_of_table,
+	},
+	.probe = dw9768_probe,
+	.remove = dw9768_remove,
+};
+module_i2c_driver(dw9768_i2c_driver);
+
+MODULE_AUTHOR("Dongchun Zhu <dongchun.zhu@mediatek.com>");
+MODULE_DESCRIPTION("DW9768 VCM driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/dw9807-vcm.c b/drivers/media/i2c/dw9807-vcm.c
new file mode 100644
index 000000000..4148009e0
--- /dev/null
+++ b/drivers/media/i2c/dw9807-vcm.c
@@ -0,0 +1,321 @@
+// SPDX-License-Identifier: GPL-2.0
+// Copyright (C) 2018 Intel Corporation
+
+#include <linux/acpi.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/iopoll.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+
+#define DW9807_MAX_FOCUS_POS	1023
+/*
+ * This sets the minimum granularity for the focus positions.
+ * A value of 1 gives maximum accuracy for a desired focus position.
+ */
+#define DW9807_FOCUS_STEPS	1
+/*
+ * This acts as the minimum granularity of lens movement.
+ * Keep this value power of 2, so the control steps can be
+ * uniformly adjusted for gradual lens movement, with desired
+ * number of control steps.
+ */
+#define DW9807_CTRL_STEPS	16
+#define DW9807_CTRL_DELAY_US	1000
+
+#define DW9807_CTL_ADDR		0x02
+/*
+ * DW9807 separates two registers to control the VCM position.
+ * One for MSB value, another is LSB value.
+ */
+#define DW9807_MSB_ADDR		0x03
+#define DW9807_LSB_ADDR		0x04
+#define DW9807_STATUS_ADDR	0x05
+#define DW9807_MODE_ADDR	0x06
+#define DW9807_RESONANCE_ADDR	0x07
+
+#define MAX_RETRY		10
+
+struct dw9807_device {
+	struct v4l2_ctrl_handler ctrls_vcm;
+	struct v4l2_subdev sd;
+	u16 current_val;
+};
+
+static inline struct dw9807_device *sd_to_dw9807_vcm(
+					struct v4l2_subdev *subdev)
+{
+	return container_of(subdev, struct dw9807_device, sd);
+}
+
+static int dw9807_i2c_check(struct i2c_client *client)
+{
+	const char status_addr = DW9807_STATUS_ADDR;
+	char status_result;
+	int ret;
+
+	ret = i2c_master_send(client, &status_addr, sizeof(status_addr));
+	if (ret < 0) {
+		dev_err(&client->dev, "I2C write STATUS address fail ret = %d\n",
+			ret);
+		return ret;
+	}
+
+	ret = i2c_master_recv(client, &status_result, sizeof(status_result));
+	if (ret < 0) {
+		dev_err(&client->dev, "I2C read STATUS value fail ret = %d\n",
+			ret);
+		return ret;
+	}
+
+	return status_result;
+}
+
+static int dw9807_set_dac(struct i2c_client *client, u16 data)
+{
+	const char tx_data[3] = {
+		DW9807_MSB_ADDR, ((data >> 8) & 0x03), (data & 0xff)
+	};
+	int val, ret;
+
+	/*
+	 * According to the datasheet, need to check the bus status before we
+	 * write VCM position. This ensure that we really write the value
+	 * into the register
+	 */
+	ret = readx_poll_timeout(dw9807_i2c_check, client, val, val <= 0,
+			DW9807_CTRL_DELAY_US, MAX_RETRY * DW9807_CTRL_DELAY_US);
+
+	if (ret || val < 0) {
+		if (ret) {
+			dev_warn(&client->dev,
+				"Cannot do the write operation because VCM is busy\n");
+		}
+
+		return ret ? -EBUSY : val;
+	}
+
+	/* Write VCM position to registers */
+	ret = i2c_master_send(client, tx_data, sizeof(tx_data));
+	if (ret < 0) {
+		dev_err(&client->dev,
+			"I2C write MSB fail ret=%d\n", ret);
+
+		return ret;
+	}
+
+	return 0;
+}
+
+static int dw9807_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct dw9807_device *dev_vcm = container_of(ctrl->handler,
+		struct dw9807_device, ctrls_vcm);
+
+	if (ctrl->id == V4L2_CID_FOCUS_ABSOLUTE) {
+		struct i2c_client *client = v4l2_get_subdevdata(&dev_vcm->sd);
+
+		dev_vcm->current_val = ctrl->val;
+		return dw9807_set_dac(client, ctrl->val);
+	}
+
+	return -EINVAL;
+}
+
+static const struct v4l2_ctrl_ops dw9807_vcm_ctrl_ops = {
+	.s_ctrl = dw9807_set_ctrl,
+};
+
+static int dw9807_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	return pm_runtime_resume_and_get(sd->dev);
+}
+
+static int dw9807_close(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	pm_runtime_put(sd->dev);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_internal_ops dw9807_int_ops = {
+	.open = dw9807_open,
+	.close = dw9807_close,
+};
+
+static const struct v4l2_subdev_ops dw9807_ops = { };
+
+static void dw9807_subdev_cleanup(struct dw9807_device *dw9807_dev)
+{
+	v4l2_async_unregister_subdev(&dw9807_dev->sd);
+	v4l2_ctrl_handler_free(&dw9807_dev->ctrls_vcm);
+	media_entity_cleanup(&dw9807_dev->sd.entity);
+}
+
+static int dw9807_init_controls(struct dw9807_device *dev_vcm)
+{
+	struct v4l2_ctrl_handler *hdl = &dev_vcm->ctrls_vcm;
+	const struct v4l2_ctrl_ops *ops = &dw9807_vcm_ctrl_ops;
+	struct i2c_client *client = v4l2_get_subdevdata(&dev_vcm->sd);
+
+	v4l2_ctrl_handler_init(hdl, 1);
+
+	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_FOCUS_ABSOLUTE,
+			  0, DW9807_MAX_FOCUS_POS, DW9807_FOCUS_STEPS, 0);
+
+	dev_vcm->sd.ctrl_handler = hdl;
+	if (hdl->error) {
+		dev_err(&client->dev, "%s fail error: 0x%x\n",
+			__func__, hdl->error);
+		return hdl->error;
+	}
+
+	return 0;
+}
+
+static int dw9807_probe(struct i2c_client *client)
+{
+	struct dw9807_device *dw9807_dev;
+	int rval;
+
+	dw9807_dev = devm_kzalloc(&client->dev, sizeof(*dw9807_dev),
+				  GFP_KERNEL);
+	if (dw9807_dev == NULL)
+		return -ENOMEM;
+
+	v4l2_i2c_subdev_init(&dw9807_dev->sd, client, &dw9807_ops);
+	dw9807_dev->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	dw9807_dev->sd.internal_ops = &dw9807_int_ops;
+
+	rval = dw9807_init_controls(dw9807_dev);
+	if (rval)
+		goto err_cleanup;
+
+	rval = media_entity_pads_init(&dw9807_dev->sd.entity, 0, NULL);
+	if (rval < 0)
+		goto err_cleanup;
+
+	dw9807_dev->sd.entity.function = MEDIA_ENT_F_LENS;
+
+	rval = v4l2_async_register_subdev(&dw9807_dev->sd);
+	if (rval < 0)
+		goto err_cleanup;
+
+	pm_runtime_set_active(&client->dev);
+	pm_runtime_enable(&client->dev);
+	pm_runtime_idle(&client->dev);
+
+	return 0;
+
+err_cleanup:
+	v4l2_ctrl_handler_free(&dw9807_dev->ctrls_vcm);
+	media_entity_cleanup(&dw9807_dev->sd.entity);
+
+	return rval;
+}
+
+static void dw9807_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct dw9807_device *dw9807_dev = sd_to_dw9807_vcm(sd);
+
+	pm_runtime_disable(&client->dev);
+
+	dw9807_subdev_cleanup(dw9807_dev);
+}
+
+/*
+ * This function sets the vcm position, so it consumes least current
+ * The lens position is gradually moved in units of DW9807_CTRL_STEPS,
+ * to make the movements smoothly.
+ */
+static int __maybe_unused dw9807_vcm_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct dw9807_device *dw9807_dev = sd_to_dw9807_vcm(sd);
+	const char tx_data[2] = { DW9807_CTL_ADDR, 0x01 };
+	int ret, val;
+
+	for (val = dw9807_dev->current_val & ~(DW9807_CTRL_STEPS - 1);
+	     val >= 0; val -= DW9807_CTRL_STEPS) {
+		ret = dw9807_set_dac(client, val);
+		if (ret)
+			dev_err_once(dev, "%s I2C failure: %d", __func__, ret);
+		usleep_range(DW9807_CTRL_DELAY_US, DW9807_CTRL_DELAY_US + 10);
+	}
+
+	/* Power down */
+	ret = i2c_master_send(client, tx_data, sizeof(tx_data));
+	if (ret < 0) {
+		dev_err(&client->dev, "I2C write CTL fail ret = %d\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+/*
+ * This function sets the vcm position to the value set by the user
+ * through v4l2_ctrl_ops s_ctrl handler
+ * The lens position is gradually moved in units of DW9807_CTRL_STEPS,
+ * to make the movements smoothly.
+ */
+static int  __maybe_unused dw9807_vcm_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct dw9807_device *dw9807_dev = sd_to_dw9807_vcm(sd);
+	const char tx_data[2] = { DW9807_CTL_ADDR, 0x00 };
+	int ret, val;
+
+	/* Power on */
+	ret = i2c_master_send(client, tx_data, sizeof(tx_data));
+	if (ret < 0) {
+		dev_err(&client->dev, "I2C write CTL fail ret = %d\n", ret);
+		return ret;
+	}
+
+	for (val = dw9807_dev->current_val % DW9807_CTRL_STEPS;
+	     val < dw9807_dev->current_val + DW9807_CTRL_STEPS - 1;
+	     val += DW9807_CTRL_STEPS) {
+		ret = dw9807_set_dac(client, val);
+		if (ret)
+			dev_err_ratelimited(dev, "%s I2C failure: %d",
+						__func__, ret);
+		usleep_range(DW9807_CTRL_DELAY_US, DW9807_CTRL_DELAY_US + 10);
+	}
+
+	return 0;
+}
+
+static const struct of_device_id dw9807_of_table[] = {
+	{ .compatible = "dongwoon,dw9807-vcm" },
+	/* Compatibility for older firmware, NEVER USE THIS IN FIRMWARE! */
+	{ .compatible = "dongwoon,dw9807" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, dw9807_of_table);
+
+static const struct dev_pm_ops dw9807_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(dw9807_vcm_suspend, dw9807_vcm_resume)
+	SET_RUNTIME_PM_OPS(dw9807_vcm_suspend, dw9807_vcm_resume, NULL)
+};
+
+static struct i2c_driver dw9807_i2c_driver = {
+	.driver = {
+		.name = "dw9807",
+		.pm = &dw9807_pm_ops,
+		.of_match_table = dw9807_of_table,
+	},
+	.probe = dw9807_probe,
+	.remove = dw9807_remove,
+};
+
+module_i2c_driver(dw9807_i2c_driver);
+
+MODULE_AUTHOR("Chiang, Alan");
+MODULE_DESCRIPTION("DW9807 VCM driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/et8ek8/Kconfig b/drivers/media/i2c/et8ek8/Kconfig
new file mode 100644
index 000000000..987fc62d5
--- /dev/null
+++ b/drivers/media/i2c/et8ek8/Kconfig
@@ -0,0 +1,6 @@
+# SPDX-License-Identifier: GPL-2.0-only
+config VIDEO_ET8EK8
+	tristate "ET8EK8 camera sensor support"
+	help
+	  This is a driver for the Toshiba ET8EK8 5 MP camera sensor.
+	  It is used for example in Nokia N900 (RX-51).
diff --git a/drivers/media/i2c/et8ek8/Makefile b/drivers/media/i2c/et8ek8/Makefile
new file mode 100644
index 000000000..5e06c308c
--- /dev/null
+++ b/drivers/media/i2c/et8ek8/Makefile
@@ -0,0 +1,3 @@
+# SPDX-License-Identifier: GPL-2.0-only
+et8ek8-objs			+= et8ek8_mode.o et8ek8_driver.o
+obj-$(CONFIG_VIDEO_ET8EK8)	+= et8ek8.o
diff --git a/drivers/media/i2c/et8ek8/et8ek8_driver.c b/drivers/media/i2c/et8ek8/et8ek8_driver.c
new file mode 100644
index 000000000..d6fc843f9
--- /dev/null
+++ b/drivers/media/i2c/et8ek8/et8ek8_driver.c
@@ -0,0 +1,1513 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * et8ek8_driver.c
+ *
+ * Copyright (C) 2008 Nokia Corporation
+ *
+ * Contact: Sakari Ailus <sakari.ailus@iki.fi>
+ *          Tuukka Toivonen <tuukkat76@gmail.com>
+ *          Pavel Machek <pavel@ucw.cz>
+ *
+ * Based on code from Toni Leinonen <toni.leinonen@offcode.fi>.
+ *
+ * This driver is based on the Micron MT9T012 camera imager driver
+ * (C) Texas Instruments.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/sort.h>
+#include <linux/v4l2-mediabus.h>
+
+#include <media/media-entity.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+
+#include "et8ek8_reg.h"
+
+#define ET8EK8_NAME		"et8ek8"
+#define ET8EK8_PRIV_MEM_SIZE	128
+#define ET8EK8_MAX_MSG		8
+
+struct et8ek8_sensor {
+	struct v4l2_subdev subdev;
+	struct media_pad pad;
+	struct v4l2_mbus_framefmt format;
+	struct gpio_desc *reset;
+	struct regulator *vana;
+	struct clk *ext_clk;
+	u32 xclk_freq;
+
+	u16 version;
+
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_ctrl *exposure;
+	struct v4l2_ctrl *pixel_rate;
+	struct et8ek8_reglist *current_reglist;
+
+	u8 priv_mem[ET8EK8_PRIV_MEM_SIZE];
+
+	struct mutex power_lock;
+	int power_count;
+};
+
+#define to_et8ek8_sensor(sd)	container_of(sd, struct et8ek8_sensor, subdev)
+
+enum et8ek8_versions {
+	ET8EK8_REV_1 = 0x0001,
+	ET8EK8_REV_2,
+};
+
+/*
+ * This table describes what should be written to the sensor register
+ * for each gain value. The gain(index in the table) is in terms of
+ * 0.1EV, i.e. 10 indexes in the table give 2 time more gain [0] in
+ * the *analog gain, [1] in the digital gain
+ *
+ * Analog gain [dB] = 20*log10(regvalue/32); 0x20..0x100
+ */
+static struct et8ek8_gain {
+	u16 analog;
+	u16 digital;
+} const et8ek8_gain_table[] = {
+	{ 32,    0},  /* x1 */
+	{ 34,    0},
+	{ 37,    0},
+	{ 39,    0},
+	{ 42,    0},
+	{ 45,    0},
+	{ 49,    0},
+	{ 52,    0},
+	{ 56,    0},
+	{ 60,    0},
+	{ 64,    0},  /* x2 */
+	{ 69,    0},
+	{ 74,    0},
+	{ 79,    0},
+	{ 84,    0},
+	{ 91,    0},
+	{ 97,    0},
+	{104,    0},
+	{111,    0},
+	{119,    0},
+	{128,    0},  /* x4 */
+	{137,    0},
+	{147,    0},
+	{158,    0},
+	{169,    0},
+	{181,    0},
+	{194,    0},
+	{208,    0},
+	{223,    0},
+	{239,    0},
+	{256,    0},  /* x8 */
+	{256,   73},
+	{256,  152},
+	{256,  236},
+	{256,  327},
+	{256,  424},
+	{256,  528},
+	{256,  639},
+	{256,  758},
+	{256,  886},
+	{256, 1023},  /* x16 */
+};
+
+/* Register definitions */
+#define REG_REVISION_NUMBER_L	0x1200
+#define REG_REVISION_NUMBER_H	0x1201
+
+#define PRIV_MEM_START_REG	0x0008
+#define PRIV_MEM_WIN_SIZE	8
+
+#define ET8EK8_I2C_DELAY	3	/* msec delay b/w accesses */
+
+#define USE_CRC			1
+
+/*
+ * Register access helpers
+ *
+ * Read a 8/16/32-bit i2c register.  The value is returned in 'val'.
+ * Returns zero if successful, or non-zero otherwise.
+ */
+static int et8ek8_i2c_read_reg(struct i2c_client *client, u16 data_length,
+			       u16 reg, u32 *val)
+{
+	int r;
+	struct i2c_msg msg;
+	unsigned char data[4];
+
+	if (!client->adapter)
+		return -ENODEV;
+	if (data_length != ET8EK8_REG_8BIT && data_length != ET8EK8_REG_16BIT)
+		return -EINVAL;
+
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = 2;
+	msg.buf = data;
+
+	/* high byte goes out first */
+	data[0] = (u8) (reg >> 8);
+	data[1] = (u8) (reg & 0xff);
+	r = i2c_transfer(client->adapter, &msg, 1);
+	if (r < 0)
+		goto err;
+
+	msg.len = data_length;
+	msg.flags = I2C_M_RD;
+	r = i2c_transfer(client->adapter, &msg, 1);
+	if (r < 0)
+		goto err;
+
+	*val = 0;
+	/* high byte comes first */
+	if (data_length == ET8EK8_REG_8BIT)
+		*val = data[0];
+	else
+		*val = (data[1] << 8) + data[0];
+
+	return 0;
+
+err:
+	dev_err(&client->dev, "read from offset 0x%x error %d\n", reg, r);
+
+	return r;
+}
+
+static void et8ek8_i2c_create_msg(struct i2c_client *client, u16 len, u16 reg,
+				  u32 val, struct i2c_msg *msg,
+				  unsigned char *buf)
+{
+	msg->addr = client->addr;
+	msg->flags = 0; /* Write */
+	msg->len = 2 + len;
+	msg->buf = buf;
+
+	/* high byte goes out first */
+	buf[0] = (u8) (reg >> 8);
+	buf[1] = (u8) (reg & 0xff);
+
+	switch (len) {
+	case ET8EK8_REG_8BIT:
+		buf[2] = (u8) (val) & 0xff;
+		break;
+	case ET8EK8_REG_16BIT:
+		buf[2] = (u8) (val) & 0xff;
+		buf[3] = (u8) (val >> 8) & 0xff;
+		break;
+	default:
+		WARN_ONCE(1, ET8EK8_NAME ": %s: invalid message length.\n",
+			  __func__);
+	}
+}
+
+/*
+ * A buffered write method that puts the wanted register write
+ * commands in smaller number of message lists and passes the lists to
+ * the i2c framework
+ */
+static int et8ek8_i2c_buffered_write_regs(struct i2c_client *client,
+					  const struct et8ek8_reg *wnext,
+					  int cnt)
+{
+	struct i2c_msg msg[ET8EK8_MAX_MSG];
+	unsigned char data[ET8EK8_MAX_MSG][6];
+	int wcnt = 0;
+	u16 reg, data_length;
+	u32 val;
+	int rval;
+
+	/* Create new write messages for all writes */
+	while (wcnt < cnt) {
+		data_length = wnext->type;
+		reg = wnext->reg;
+		val = wnext->val;
+		wnext++;
+
+		et8ek8_i2c_create_msg(client, data_length, reg,
+				    val, &msg[wcnt], &data[wcnt][0]);
+
+		/* Update write count */
+		wcnt++;
+
+		if (wcnt < ET8EK8_MAX_MSG)
+			continue;
+
+		rval = i2c_transfer(client->adapter, msg, wcnt);
+		if (rval < 0)
+			return rval;
+
+		cnt -= wcnt;
+		wcnt = 0;
+	}
+
+	rval = i2c_transfer(client->adapter, msg, wcnt);
+
+	return rval < 0 ? rval : 0;
+}
+
+/*
+ * Write a list of registers to i2c device.
+ *
+ * The list of registers is terminated by ET8EK8_REG_TERM.
+ * Returns zero if successful, or non-zero otherwise.
+ */
+static int et8ek8_i2c_write_regs(struct i2c_client *client,
+				 const struct et8ek8_reg *regs)
+{
+	int r, cnt = 0;
+	const struct et8ek8_reg *next;
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	if (!regs)
+		return -EINVAL;
+
+	/* Initialize list pointers to the start of the list */
+	next = regs;
+
+	do {
+		/*
+		 * We have to go through the list to figure out how
+		 * many regular writes we have in a row
+		 */
+		while (next->type != ET8EK8_REG_TERM &&
+		       next->type != ET8EK8_REG_DELAY) {
+			/*
+			 * Here we check that the actual length fields
+			 * are valid
+			 */
+			if (WARN(next->type != ET8EK8_REG_8BIT &&
+				 next->type != ET8EK8_REG_16BIT,
+				 "Invalid type = %d", next->type)) {
+				return -EINVAL;
+			}
+			/*
+			 * Increment count of successive writes and
+			 * read pointer
+			 */
+			cnt++;
+			next++;
+		}
+
+		/* Now we start writing ... */
+		r = et8ek8_i2c_buffered_write_regs(client, regs, cnt);
+
+		/* ... and then check that everything was OK */
+		if (r < 0) {
+			dev_err(&client->dev, "i2c transfer error!\n");
+			return r;
+		}
+
+		/*
+		 * If we ran into a sleep statement when going through
+		 * the list, this is where we snooze for the required time
+		 */
+		if (next->type == ET8EK8_REG_DELAY) {
+			msleep(next->val);
+			/*
+			 * ZZZ ...
+			 * Update list pointers and cnt and start over ...
+			 */
+			next++;
+			regs = next;
+			cnt = 0;
+		}
+	} while (next->type != ET8EK8_REG_TERM);
+
+	return 0;
+}
+
+/*
+ * Write to a 8/16-bit register.
+ * Returns zero if successful, or non-zero otherwise.
+ */
+static int et8ek8_i2c_write_reg(struct i2c_client *client, u16 data_length,
+				u16 reg, u32 val)
+{
+	int r;
+	struct i2c_msg msg;
+	unsigned char data[6];
+
+	if (!client->adapter)
+		return -ENODEV;
+	if (data_length != ET8EK8_REG_8BIT && data_length != ET8EK8_REG_16BIT)
+		return -EINVAL;
+
+	et8ek8_i2c_create_msg(client, data_length, reg, val, &msg, data);
+
+	r = i2c_transfer(client->adapter, &msg, 1);
+	if (r < 0) {
+		dev_err(&client->dev,
+			"wrote 0x%x to offset 0x%x error %d\n", val, reg, r);
+		return r;
+	}
+
+	return 0;
+}
+
+static struct et8ek8_reglist *et8ek8_reglist_find_type(
+		struct et8ek8_meta_reglist *meta,
+		u16 type)
+{
+	struct et8ek8_reglist **next = &meta->reglist[0].ptr;
+
+	while (*next) {
+		if ((*next)->type == type)
+			return *next;
+
+		next++;
+	}
+
+	return NULL;
+}
+
+static int et8ek8_i2c_reglist_find_write(struct i2c_client *client,
+					 struct et8ek8_meta_reglist *meta,
+					 u16 type)
+{
+	struct et8ek8_reglist *reglist;
+
+	reglist = et8ek8_reglist_find_type(meta, type);
+	if (!reglist)
+		return -EINVAL;
+
+	return et8ek8_i2c_write_regs(client, reglist->regs);
+}
+
+static struct et8ek8_reglist **et8ek8_reglist_first(
+		struct et8ek8_meta_reglist *meta)
+{
+	return &meta->reglist[0].ptr;
+}
+
+static void et8ek8_reglist_to_mbus(const struct et8ek8_reglist *reglist,
+				   struct v4l2_mbus_framefmt *fmt)
+{
+	fmt->width = reglist->mode.window_width;
+	fmt->height = reglist->mode.window_height;
+	fmt->code = reglist->mode.bus_format;
+}
+
+static struct et8ek8_reglist *et8ek8_reglist_find_mode_fmt(
+		struct et8ek8_meta_reglist *meta,
+		struct v4l2_mbus_framefmt *fmt)
+{
+	struct et8ek8_reglist **list = et8ek8_reglist_first(meta);
+	struct et8ek8_reglist *best_match = NULL;
+	struct et8ek8_reglist *best_other = NULL;
+	struct v4l2_mbus_framefmt format;
+	unsigned int max_dist_match = (unsigned int)-1;
+	unsigned int max_dist_other = (unsigned int)-1;
+
+	/*
+	 * Find the mode with the closest image size. The distance between
+	 * image sizes is the size in pixels of the non-overlapping regions
+	 * between the requested size and the frame-specified size.
+	 *
+	 * Store both the closest mode that matches the requested format, and
+	 * the closest mode for all other formats. The best match is returned
+	 * if found, otherwise the best mode with a non-matching format is
+	 * returned.
+	 */
+	for (; *list; list++) {
+		unsigned int dist;
+
+		if ((*list)->type != ET8EK8_REGLIST_MODE)
+			continue;
+
+		et8ek8_reglist_to_mbus(*list, &format);
+
+		dist = min(fmt->width, format.width)
+		     * min(fmt->height, format.height);
+		dist = format.width * format.height
+		     + fmt->width * fmt->height - 2 * dist;
+
+
+		if (fmt->code == format.code) {
+			if (dist < max_dist_match || !best_match) {
+				best_match = *list;
+				max_dist_match = dist;
+			}
+		} else {
+			if (dist < max_dist_other || !best_other) {
+				best_other = *list;
+				max_dist_other = dist;
+			}
+		}
+	}
+
+	return best_match ? best_match : best_other;
+}
+
+#define TIMEPERFRAME_AVG_FPS(t)						\
+	(((t).denominator + ((t).numerator >> 1)) / (t).numerator)
+
+static struct et8ek8_reglist *et8ek8_reglist_find_mode_ival(
+		struct et8ek8_meta_reglist *meta,
+		struct et8ek8_reglist *current_reglist,
+		struct v4l2_fract *timeperframe)
+{
+	int fps = TIMEPERFRAME_AVG_FPS(*timeperframe);
+	struct et8ek8_reglist **list = et8ek8_reglist_first(meta);
+	struct et8ek8_mode *current_mode = &current_reglist->mode;
+
+	for (; *list; list++) {
+		struct et8ek8_mode *mode = &(*list)->mode;
+
+		if ((*list)->type != ET8EK8_REGLIST_MODE)
+			continue;
+
+		if (mode->window_width != current_mode->window_width ||
+		    mode->window_height != current_mode->window_height)
+			continue;
+
+		if (TIMEPERFRAME_AVG_FPS(mode->timeperframe) == fps)
+			return *list;
+	}
+
+	return NULL;
+}
+
+static int et8ek8_reglist_cmp(const void *a, const void *b)
+{
+	const struct et8ek8_reglist **list1 = (const struct et8ek8_reglist **)a,
+		**list2 = (const struct et8ek8_reglist **)b;
+
+	/* Put real modes in the beginning. */
+	if ((*list1)->type == ET8EK8_REGLIST_MODE &&
+	    (*list2)->type != ET8EK8_REGLIST_MODE)
+		return -1;
+	if ((*list1)->type != ET8EK8_REGLIST_MODE &&
+	    (*list2)->type == ET8EK8_REGLIST_MODE)
+		return 1;
+
+	/* Descending width. */
+	if ((*list1)->mode.window_width > (*list2)->mode.window_width)
+		return -1;
+	if ((*list1)->mode.window_width < (*list2)->mode.window_width)
+		return 1;
+
+	if ((*list1)->mode.window_height > (*list2)->mode.window_height)
+		return -1;
+	if ((*list1)->mode.window_height < (*list2)->mode.window_height)
+		return 1;
+
+	return 0;
+}
+
+static int et8ek8_reglist_import(struct i2c_client *client,
+				 struct et8ek8_meta_reglist *meta)
+{
+	int nlists = 0, i;
+
+	dev_info(&client->dev, "meta_reglist version %s\n", meta->version);
+
+	while (meta->reglist[nlists].ptr)
+		nlists++;
+
+	if (!nlists)
+		return -EINVAL;
+
+	sort(&meta->reglist[0].ptr, nlists, sizeof(meta->reglist[0].ptr),
+	     et8ek8_reglist_cmp, NULL);
+
+	i = nlists;
+	nlists = 0;
+
+	while (i--) {
+		struct et8ek8_reglist *list;
+
+		list = meta->reglist[nlists].ptr;
+
+		dev_dbg(&client->dev,
+		       "%s: type %d\tw %d\th %d\tfmt %x\tival %d/%d\tptr %p\n",
+		       __func__,
+		       list->type,
+		       list->mode.window_width, list->mode.window_height,
+		       list->mode.bus_format,
+		       list->mode.timeperframe.numerator,
+		       list->mode.timeperframe.denominator,
+		       (void *)meta->reglist[nlists].ptr);
+
+		nlists++;
+	}
+
+	return 0;
+}
+
+/* Called to change the V4L2 gain control value. This function
+ * rounds and clamps the given value and updates the V4L2 control value.
+ * If power is on, also updates the sensor analog and digital gains.
+ * gain is in 0.1 EV (exposure value) units.
+ */
+static int et8ek8_set_gain(struct et8ek8_sensor *sensor, s32 gain)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->subdev);
+	struct et8ek8_gain new;
+	int r;
+
+	new = et8ek8_gain_table[gain];
+
+	/* FIXME: optimise I2C writes! */
+	r = et8ek8_i2c_write_reg(client, ET8EK8_REG_8BIT,
+				0x124a, new.analog >> 8);
+	if (r)
+		return r;
+	r = et8ek8_i2c_write_reg(client, ET8EK8_REG_8BIT,
+				0x1249, new.analog & 0xff);
+	if (r)
+		return r;
+
+	r = et8ek8_i2c_write_reg(client, ET8EK8_REG_8BIT,
+				0x124d, new.digital >> 8);
+	if (r)
+		return r;
+	r = et8ek8_i2c_write_reg(client, ET8EK8_REG_8BIT,
+				0x124c, new.digital & 0xff);
+
+	return r;
+}
+
+static int et8ek8_set_test_pattern(struct et8ek8_sensor *sensor, s32 mode)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->subdev);
+	int cbh_mode, cbv_mode, tp_mode, din_sw, r1420, rval;
+
+	/* Values for normal mode */
+	cbh_mode = 0;
+	cbv_mode = 0;
+	tp_mode  = 0;
+	din_sw   = 0x00;
+	r1420    = 0xF0;
+
+	if (mode) {
+		/* Test pattern mode */
+		if (mode < 5) {
+			cbh_mode = 1;
+			cbv_mode = 1;
+			tp_mode  = mode + 3;
+		} else {
+			cbh_mode = 0;
+			cbv_mode = 0;
+			tp_mode  = mode - 4 + 3;
+		}
+
+		din_sw   = 0x01;
+		r1420    = 0xE0;
+	}
+
+	rval = et8ek8_i2c_write_reg(client, ET8EK8_REG_8BIT, 0x111B,
+				    tp_mode << 4);
+	if (rval)
+		return rval;
+
+	rval = et8ek8_i2c_write_reg(client, ET8EK8_REG_8BIT, 0x1121,
+				    cbh_mode << 7);
+	if (rval)
+		return rval;
+
+	rval = et8ek8_i2c_write_reg(client, ET8EK8_REG_8BIT, 0x1124,
+				    cbv_mode << 7);
+	if (rval)
+		return rval;
+
+	rval = et8ek8_i2c_write_reg(client, ET8EK8_REG_8BIT, 0x112C, din_sw);
+	if (rval)
+		return rval;
+
+	return et8ek8_i2c_write_reg(client, ET8EK8_REG_8BIT, 0x1420, r1420);
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 controls
+ */
+
+static int et8ek8_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct et8ek8_sensor *sensor =
+		container_of(ctrl->handler, struct et8ek8_sensor, ctrl_handler);
+
+	switch (ctrl->id) {
+	case V4L2_CID_GAIN:
+		return et8ek8_set_gain(sensor, ctrl->val);
+
+	case V4L2_CID_EXPOSURE:
+	{
+		struct i2c_client *client =
+			v4l2_get_subdevdata(&sensor->subdev);
+
+		return et8ek8_i2c_write_reg(client, ET8EK8_REG_16BIT, 0x1243,
+					    ctrl->val);
+	}
+
+	case V4L2_CID_TEST_PATTERN:
+		return et8ek8_set_test_pattern(sensor, ctrl->val);
+
+	case V4L2_CID_PIXEL_RATE:
+		return 0;
+
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct v4l2_ctrl_ops et8ek8_ctrl_ops = {
+	.s_ctrl = et8ek8_set_ctrl,
+};
+
+static const char * const et8ek8_test_pattern_menu[] = {
+	"Normal",
+	"Vertical colorbar",
+	"Horizontal colorbar",
+	"Scale",
+	"Ramp",
+	"Small vertical colorbar",
+	"Small horizontal colorbar",
+	"Small scale",
+	"Small ramp",
+};
+
+static int et8ek8_init_controls(struct et8ek8_sensor *sensor)
+{
+	s32 max_rows;
+
+	v4l2_ctrl_handler_init(&sensor->ctrl_handler, 4);
+
+	/* V4L2_CID_GAIN */
+	v4l2_ctrl_new_std(&sensor->ctrl_handler, &et8ek8_ctrl_ops,
+			  V4L2_CID_GAIN, 0, ARRAY_SIZE(et8ek8_gain_table) - 1,
+			  1, 0);
+
+	max_rows = sensor->current_reglist->mode.max_exp;
+	{
+		u32 min = 1, max = max_rows;
+
+		sensor->exposure =
+			v4l2_ctrl_new_std(&sensor->ctrl_handler,
+					  &et8ek8_ctrl_ops, V4L2_CID_EXPOSURE,
+					  min, max, min, max);
+	}
+
+	/* V4L2_CID_PIXEL_RATE */
+	sensor->pixel_rate =
+		v4l2_ctrl_new_std(&sensor->ctrl_handler, &et8ek8_ctrl_ops,
+		V4L2_CID_PIXEL_RATE, 1, INT_MAX, 1, 1);
+
+	/* V4L2_CID_TEST_PATTERN */
+	v4l2_ctrl_new_std_menu_items(&sensor->ctrl_handler,
+				     &et8ek8_ctrl_ops, V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(et8ek8_test_pattern_menu) - 1,
+				     0, 0, et8ek8_test_pattern_menu);
+
+	if (sensor->ctrl_handler.error)
+		return sensor->ctrl_handler.error;
+
+	sensor->subdev.ctrl_handler = &sensor->ctrl_handler;
+
+	return 0;
+}
+
+static void et8ek8_update_controls(struct et8ek8_sensor *sensor)
+{
+	struct v4l2_ctrl *ctrl;
+	struct et8ek8_mode *mode = &sensor->current_reglist->mode;
+
+	u32 min, max, pixel_rate;
+	static const int S = 8;
+
+	ctrl = sensor->exposure;
+
+	min = 1;
+	max = mode->max_exp;
+
+	/*
+	 * Calculate average pixel clock per line. Assume buffers can spread
+	 * the data over horizontal blanking time. Rounding upwards.
+	 * Formula taken from stock Nokia N900 kernel.
+	 */
+	pixel_rate = ((mode->pixel_clock + (1 << S) - 1) >> S) + mode->width;
+	pixel_rate = mode->window_width * (pixel_rate - 1) / mode->width;
+
+	__v4l2_ctrl_modify_range(ctrl, min, max, min, max);
+	__v4l2_ctrl_s_ctrl_int64(sensor->pixel_rate, pixel_rate << S);
+}
+
+static int et8ek8_configure(struct et8ek8_sensor *sensor)
+{
+	struct v4l2_subdev *subdev = &sensor->subdev;
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
+	int rval;
+
+	rval = et8ek8_i2c_write_regs(client, sensor->current_reglist->regs);
+	if (rval)
+		goto fail;
+
+	/* Controls set while the power to the sensor is turned off are saved
+	 * but not applied to the hardware. Now that we're about to start
+	 * streaming apply all the current values to the hardware.
+	 */
+	rval = v4l2_ctrl_handler_setup(&sensor->ctrl_handler);
+	if (rval)
+		goto fail;
+
+	return 0;
+
+fail:
+	dev_err(&client->dev, "sensor configuration failed\n");
+
+	return rval;
+}
+
+static int et8ek8_stream_on(struct et8ek8_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->subdev);
+
+	return et8ek8_i2c_write_reg(client, ET8EK8_REG_8BIT, 0x1252, 0xb0);
+}
+
+static int et8ek8_stream_off(struct et8ek8_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->subdev);
+
+	return et8ek8_i2c_write_reg(client, ET8EK8_REG_8BIT, 0x1252, 0x30);
+}
+
+static int et8ek8_s_stream(struct v4l2_subdev *subdev, int streaming)
+{
+	struct et8ek8_sensor *sensor = to_et8ek8_sensor(subdev);
+	int ret;
+
+	if (!streaming)
+		return et8ek8_stream_off(sensor);
+
+	ret = et8ek8_configure(sensor);
+	if (ret < 0)
+		return ret;
+
+	return et8ek8_stream_on(sensor);
+}
+
+/* --------------------------------------------------------------------------
+ * V4L2 subdev operations
+ */
+
+static int et8ek8_power_off(struct et8ek8_sensor *sensor)
+{
+	gpiod_set_value(sensor->reset, 0);
+	udelay(1);
+
+	clk_disable_unprepare(sensor->ext_clk);
+
+	return regulator_disable(sensor->vana);
+}
+
+static int et8ek8_power_on(struct et8ek8_sensor *sensor)
+{
+	struct v4l2_subdev *subdev = &sensor->subdev;
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
+	unsigned int xclk_freq;
+	int val, rval;
+
+	rval = regulator_enable(sensor->vana);
+	if (rval) {
+		dev_err(&client->dev, "failed to enable vana regulator\n");
+		return rval;
+	}
+
+	if (sensor->current_reglist)
+		xclk_freq = sensor->current_reglist->mode.ext_clock;
+	else
+		xclk_freq = sensor->xclk_freq;
+
+	rval = clk_set_rate(sensor->ext_clk, xclk_freq);
+	if (rval < 0) {
+		dev_err(&client->dev, "unable to set extclk clock freq to %u\n",
+			xclk_freq);
+		goto out;
+	}
+	rval = clk_prepare_enable(sensor->ext_clk);
+	if (rval < 0) {
+		dev_err(&client->dev, "failed to enable extclk\n");
+		goto out;
+	}
+
+	if (rval)
+		goto out;
+
+	udelay(10); /* I wish this is a good value */
+
+	gpiod_set_value(sensor->reset, 1);
+
+	msleep(5000 * 1000 / xclk_freq + 1); /* Wait 5000 cycles */
+
+	rval = et8ek8_i2c_reglist_find_write(client, &meta_reglist,
+					     ET8EK8_REGLIST_POWERON);
+	if (rval)
+		goto out;
+
+#ifdef USE_CRC
+	rval = et8ek8_i2c_read_reg(client, ET8EK8_REG_8BIT, 0x1263, &val);
+	if (rval)
+		goto out;
+#if USE_CRC /* TODO get crc setting from DT */
+	val |= BIT(4);
+#else
+	val &= ~BIT(4);
+#endif
+	rval = et8ek8_i2c_write_reg(client, ET8EK8_REG_8BIT, 0x1263, val);
+	if (rval)
+		goto out;
+#endif
+
+out:
+	if (rval)
+		et8ek8_power_off(sensor);
+
+	return rval;
+}
+
+/* --------------------------------------------------------------------------
+ * V4L2 subdev video operations
+ */
+#define MAX_FMTS 4
+static int et8ek8_enum_mbus_code(struct v4l2_subdev *subdev,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct et8ek8_reglist **list =
+			et8ek8_reglist_first(&meta_reglist);
+	u32 pixelformat[MAX_FMTS];
+	int npixelformat = 0;
+
+	if (code->index >= MAX_FMTS)
+		return -EINVAL;
+
+	for (; *list; list++) {
+		struct et8ek8_mode *mode = &(*list)->mode;
+		int i;
+
+		if ((*list)->type != ET8EK8_REGLIST_MODE)
+			continue;
+
+		for (i = 0; i < npixelformat; i++) {
+			if (pixelformat[i] == mode->bus_format)
+				break;
+		}
+		if (i != npixelformat)
+			continue;
+
+		if (code->index == npixelformat) {
+			code->code = mode->bus_format;
+			return 0;
+		}
+
+		pixelformat[npixelformat] = mode->bus_format;
+		npixelformat++;
+	}
+
+	return -EINVAL;
+}
+
+static int et8ek8_enum_frame_size(struct v4l2_subdev *subdev,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct et8ek8_reglist **list =
+			et8ek8_reglist_first(&meta_reglist);
+	struct v4l2_mbus_framefmt format;
+	int cmp_width = INT_MAX;
+	int cmp_height = INT_MAX;
+	int index = fse->index;
+
+	for (; *list; list++) {
+		if ((*list)->type != ET8EK8_REGLIST_MODE)
+			continue;
+
+		et8ek8_reglist_to_mbus(*list, &format);
+		if (fse->code != format.code)
+			continue;
+
+		/* Assume that the modes are grouped by frame size. */
+		if (format.width == cmp_width && format.height == cmp_height)
+			continue;
+
+		cmp_width = format.width;
+		cmp_height = format.height;
+
+		if (index-- == 0) {
+			fse->min_width = format.width;
+			fse->min_height = format.height;
+			fse->max_width = format.width;
+			fse->max_height = format.height;
+			return 0;
+		}
+	}
+
+	return -EINVAL;
+}
+
+static int et8ek8_enum_frame_ival(struct v4l2_subdev *subdev,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_interval_enum *fie)
+{
+	struct et8ek8_reglist **list =
+			et8ek8_reglist_first(&meta_reglist);
+	struct v4l2_mbus_framefmt format;
+	int index = fie->index;
+
+	for (; *list; list++) {
+		struct et8ek8_mode *mode = &(*list)->mode;
+
+		if ((*list)->type != ET8EK8_REGLIST_MODE)
+			continue;
+
+		et8ek8_reglist_to_mbus(*list, &format);
+		if (fie->code != format.code)
+			continue;
+
+		if (fie->width != format.width || fie->height != format.height)
+			continue;
+
+		if (index-- == 0) {
+			fie->interval = mode->timeperframe;
+			return 0;
+		}
+	}
+
+	return -EINVAL;
+}
+
+static struct v4l2_mbus_framefmt *
+__et8ek8_get_pad_format(struct et8ek8_sensor *sensor,
+			struct v4l2_subdev_state *sd_state,
+			unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_format(&sensor->subdev, sd_state,
+						  pad);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return &sensor->format;
+	default:
+		return NULL;
+	}
+}
+
+static int et8ek8_get_pad_format(struct v4l2_subdev *subdev,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_format *fmt)
+{
+	struct et8ek8_sensor *sensor = to_et8ek8_sensor(subdev);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __et8ek8_get_pad_format(sensor, sd_state, fmt->pad,
+					 fmt->which);
+	if (!format)
+		return -EINVAL;
+
+	fmt->format = *format;
+
+	return 0;
+}
+
+static int et8ek8_set_pad_format(struct v4l2_subdev *subdev,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_format *fmt)
+{
+	struct et8ek8_sensor *sensor = to_et8ek8_sensor(subdev);
+	struct v4l2_mbus_framefmt *format;
+	struct et8ek8_reglist *reglist;
+
+	format = __et8ek8_get_pad_format(sensor, sd_state, fmt->pad,
+					 fmt->which);
+	if (!format)
+		return -EINVAL;
+
+	reglist = et8ek8_reglist_find_mode_fmt(&meta_reglist, &fmt->format);
+	et8ek8_reglist_to_mbus(reglist, &fmt->format);
+	*format = fmt->format;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		sensor->current_reglist = reglist;
+		et8ek8_update_controls(sensor);
+	}
+
+	return 0;
+}
+
+static int et8ek8_get_frame_interval(struct v4l2_subdev *subdev,
+				     struct v4l2_subdev_frame_interval *fi)
+{
+	struct et8ek8_sensor *sensor = to_et8ek8_sensor(subdev);
+
+	memset(fi, 0, sizeof(*fi));
+	fi->interval = sensor->current_reglist->mode.timeperframe;
+
+	return 0;
+}
+
+static int et8ek8_set_frame_interval(struct v4l2_subdev *subdev,
+				     struct v4l2_subdev_frame_interval *fi)
+{
+	struct et8ek8_sensor *sensor = to_et8ek8_sensor(subdev);
+	struct et8ek8_reglist *reglist;
+
+	reglist = et8ek8_reglist_find_mode_ival(&meta_reglist,
+						sensor->current_reglist,
+						&fi->interval);
+
+	if (!reglist)
+		return -EINVAL;
+
+	if (sensor->current_reglist->mode.ext_clock != reglist->mode.ext_clock)
+		return -EINVAL;
+
+	sensor->current_reglist = reglist;
+	et8ek8_update_controls(sensor);
+
+	return 0;
+}
+
+static int et8ek8_g_priv_mem(struct v4l2_subdev *subdev)
+{
+	struct et8ek8_sensor *sensor = to_et8ek8_sensor(subdev);
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
+	unsigned int length = ET8EK8_PRIV_MEM_SIZE;
+	unsigned int offset = 0;
+	u8 *ptr  = sensor->priv_mem;
+	int rval = 0;
+
+	/* Read the EEPROM window-by-window, each window 8 bytes */
+	do {
+		u8 buffer[PRIV_MEM_WIN_SIZE];
+		struct i2c_msg msg;
+		int bytes, i;
+		int ofs;
+
+		/* Set the current window */
+		rval = et8ek8_i2c_write_reg(client, ET8EK8_REG_8BIT, 0x0001,
+					    0xe0 | (offset >> 3));
+		if (rval < 0)
+			return rval;
+
+		/* Wait for status bit */
+		for (i = 0; i < 1000; ++i) {
+			u32 status;
+
+			rval = et8ek8_i2c_read_reg(client, ET8EK8_REG_8BIT,
+						   0x0003, &status);
+			if (rval < 0)
+				return rval;
+			if (!(status & 0x08))
+				break;
+			usleep_range(1000, 2000);
+		}
+
+		if (i == 1000)
+			return -EIO;
+
+		/* Read window, 8 bytes at once, and copy to user space */
+		ofs = offset & 0x07;	/* Offset within this window */
+		bytes = length + ofs > 8 ? 8-ofs : length;
+		msg.addr = client->addr;
+		msg.flags = 0;
+		msg.len = 2;
+		msg.buf = buffer;
+		ofs += PRIV_MEM_START_REG;
+		buffer[0] = (u8)(ofs >> 8);
+		buffer[1] = (u8)(ofs & 0xFF);
+
+		rval = i2c_transfer(client->adapter, &msg, 1);
+		if (rval < 0)
+			return rval;
+
+		mdelay(ET8EK8_I2C_DELAY);
+		msg.addr = client->addr;
+		msg.len = bytes;
+		msg.flags = I2C_M_RD;
+		msg.buf = buffer;
+		memset(buffer, 0, sizeof(buffer));
+
+		rval = i2c_transfer(client->adapter, &msg, 1);
+		if (rval < 0)
+			return rval;
+
+		rval = 0;
+		memcpy(ptr, buffer, bytes);
+
+		length -= bytes;
+		offset += bytes;
+		ptr += bytes;
+	} while (length > 0);
+
+	return rval;
+}
+
+static int et8ek8_dev_init(struct v4l2_subdev *subdev)
+{
+	struct et8ek8_sensor *sensor = to_et8ek8_sensor(subdev);
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
+	int rval, rev_l, rev_h;
+
+	rval = et8ek8_power_on(sensor);
+	if (rval) {
+		dev_err(&client->dev, "could not power on\n");
+		return rval;
+	}
+
+	rval = et8ek8_i2c_read_reg(client, ET8EK8_REG_8BIT,
+				   REG_REVISION_NUMBER_L, &rev_l);
+	if (!rval)
+		rval = et8ek8_i2c_read_reg(client, ET8EK8_REG_8BIT,
+					   REG_REVISION_NUMBER_H, &rev_h);
+	if (rval) {
+		dev_err(&client->dev, "no et8ek8 sensor detected\n");
+		goto out_poweroff;
+	}
+
+	sensor->version = (rev_h << 8) + rev_l;
+	if (sensor->version != ET8EK8_REV_1 && sensor->version != ET8EK8_REV_2)
+		dev_info(&client->dev,
+			 "unknown version 0x%x detected, continuing anyway\n",
+			 sensor->version);
+
+	rval = et8ek8_reglist_import(client, &meta_reglist);
+	if (rval) {
+		dev_err(&client->dev,
+			"invalid register list %s, import failed\n",
+			ET8EK8_NAME);
+		goto out_poweroff;
+	}
+
+	sensor->current_reglist = et8ek8_reglist_find_type(&meta_reglist,
+							   ET8EK8_REGLIST_MODE);
+	if (!sensor->current_reglist) {
+		dev_err(&client->dev,
+			"invalid register list %s, no mode found\n",
+			ET8EK8_NAME);
+		rval = -ENODEV;
+		goto out_poweroff;
+	}
+
+	et8ek8_reglist_to_mbus(sensor->current_reglist, &sensor->format);
+
+	rval = et8ek8_i2c_reglist_find_write(client, &meta_reglist,
+					     ET8EK8_REGLIST_POWERON);
+	if (rval) {
+		dev_err(&client->dev,
+			"invalid register list %s, no POWERON mode found\n",
+			ET8EK8_NAME);
+		goto out_poweroff;
+	}
+	rval = et8ek8_stream_on(sensor); /* Needed to be able to read EEPROM */
+	if (rval)
+		goto out_poweroff;
+	rval = et8ek8_g_priv_mem(subdev);
+	if (rval)
+		dev_warn(&client->dev,
+			"can not read OTP (EEPROM) memory from sensor\n");
+	rval = et8ek8_stream_off(sensor);
+	if (rval)
+		goto out_poweroff;
+
+	rval = et8ek8_power_off(sensor);
+	if (rval)
+		goto out_poweroff;
+
+	return 0;
+
+out_poweroff:
+	et8ek8_power_off(sensor);
+
+	return rval;
+}
+
+/* --------------------------------------------------------------------------
+ * sysfs attributes
+ */
+static ssize_t
+priv_mem_show(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct v4l2_subdev *subdev = dev_get_drvdata(dev);
+	struct et8ek8_sensor *sensor = to_et8ek8_sensor(subdev);
+
+#if PAGE_SIZE < ET8EK8_PRIV_MEM_SIZE
+#error PAGE_SIZE too small!
+#endif
+
+	memcpy(buf, sensor->priv_mem, ET8EK8_PRIV_MEM_SIZE);
+
+	return ET8EK8_PRIV_MEM_SIZE;
+}
+static DEVICE_ATTR_RO(priv_mem);
+
+/* --------------------------------------------------------------------------
+ * V4L2 subdev core operations
+ */
+
+static int
+et8ek8_registered(struct v4l2_subdev *subdev)
+{
+	struct et8ek8_sensor *sensor = to_et8ek8_sensor(subdev);
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
+	int rval;
+
+	dev_dbg(&client->dev, "registered!");
+
+	rval = device_create_file(&client->dev, &dev_attr_priv_mem);
+	if (rval) {
+		dev_err(&client->dev, "could not register sysfs entry\n");
+		return rval;
+	}
+
+	rval = et8ek8_dev_init(subdev);
+	if (rval)
+		goto err_file;
+
+	rval = et8ek8_init_controls(sensor);
+	if (rval) {
+		dev_err(&client->dev, "controls initialization failed\n");
+		goto err_file;
+	}
+
+	__et8ek8_get_pad_format(sensor, NULL, 0, V4L2_SUBDEV_FORMAT_ACTIVE);
+
+	return 0;
+
+err_file:
+	device_remove_file(&client->dev, &dev_attr_priv_mem);
+
+	return rval;
+}
+
+static int __et8ek8_set_power(struct et8ek8_sensor *sensor, bool on)
+{
+	return on ? et8ek8_power_on(sensor) : et8ek8_power_off(sensor);
+}
+
+static int et8ek8_set_power(struct v4l2_subdev *subdev, int on)
+{
+	struct et8ek8_sensor *sensor = to_et8ek8_sensor(subdev);
+	int ret = 0;
+
+	mutex_lock(&sensor->power_lock);
+
+	/* If the power count is modified from 0 to != 0 or from != 0 to 0,
+	 * update the power state.
+	 */
+	if (sensor->power_count == !on) {
+		ret = __et8ek8_set_power(sensor, !!on);
+		if (ret < 0)
+			goto done;
+	}
+
+	/* Update the power count. */
+	sensor->power_count += on ? 1 : -1;
+	WARN_ON(sensor->power_count < 0);
+
+done:
+	mutex_unlock(&sensor->power_lock);
+
+	return ret;
+}
+
+static int et8ek8_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct et8ek8_sensor *sensor = to_et8ek8_sensor(sd);
+	struct v4l2_mbus_framefmt *format;
+	struct et8ek8_reglist *reglist;
+
+	reglist = et8ek8_reglist_find_type(&meta_reglist, ET8EK8_REGLIST_MODE);
+	format = __et8ek8_get_pad_format(sensor, fh->state, 0,
+					 V4L2_SUBDEV_FORMAT_TRY);
+	et8ek8_reglist_to_mbus(reglist, format);
+
+	return et8ek8_set_power(sd, true);
+}
+
+static int et8ek8_close(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	return et8ek8_set_power(sd, false);
+}
+
+static const struct v4l2_subdev_video_ops et8ek8_video_ops = {
+	.s_stream = et8ek8_s_stream,
+	.g_frame_interval = et8ek8_get_frame_interval,
+	.s_frame_interval = et8ek8_set_frame_interval,
+};
+
+static const struct v4l2_subdev_core_ops et8ek8_core_ops = {
+	.s_power = et8ek8_set_power,
+};
+
+static const struct v4l2_subdev_pad_ops et8ek8_pad_ops = {
+	.enum_mbus_code = et8ek8_enum_mbus_code,
+	.enum_frame_size = et8ek8_enum_frame_size,
+	.enum_frame_interval = et8ek8_enum_frame_ival,
+	.get_fmt = et8ek8_get_pad_format,
+	.set_fmt = et8ek8_set_pad_format,
+};
+
+static const struct v4l2_subdev_ops et8ek8_ops = {
+	.core = &et8ek8_core_ops,
+	.video = &et8ek8_video_ops,
+	.pad = &et8ek8_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops et8ek8_internal_ops = {
+	.registered = et8ek8_registered,
+	.open = et8ek8_open,
+	.close = et8ek8_close,
+};
+
+/* --------------------------------------------------------------------------
+ * I2C driver
+ */
+static int __maybe_unused et8ek8_suspend(struct device *dev)
+{
+	struct v4l2_subdev *subdev = dev_get_drvdata(dev);
+	struct et8ek8_sensor *sensor = to_et8ek8_sensor(subdev);
+
+	if (!sensor->power_count)
+		return 0;
+
+	return __et8ek8_set_power(sensor, false);
+}
+
+static int __maybe_unused et8ek8_resume(struct device *dev)
+{
+	struct v4l2_subdev *subdev = dev_get_drvdata(dev);
+	struct et8ek8_sensor *sensor = to_et8ek8_sensor(subdev);
+
+	if (!sensor->power_count)
+		return 0;
+
+	return __et8ek8_set_power(sensor, true);
+}
+
+static int et8ek8_probe(struct i2c_client *client)
+{
+	struct et8ek8_sensor *sensor;
+	struct device *dev = &client->dev;
+	int ret;
+
+	sensor = devm_kzalloc(&client->dev, sizeof(*sensor), GFP_KERNEL);
+	if (!sensor)
+		return -ENOMEM;
+
+	sensor->reset = devm_gpiod_get(dev, "reset", GPIOD_OUT_LOW);
+	if (IS_ERR(sensor->reset)) {
+		dev_dbg(&client->dev, "could not request reset gpio\n");
+		return PTR_ERR(sensor->reset);
+	}
+
+	sensor->vana = devm_regulator_get(dev, "vana");
+	if (IS_ERR(sensor->vana)) {
+		dev_err(&client->dev, "could not get regulator for vana\n");
+		return PTR_ERR(sensor->vana);
+	}
+
+	sensor->ext_clk = devm_clk_get(dev, NULL);
+	if (IS_ERR(sensor->ext_clk)) {
+		dev_err(&client->dev, "could not get clock\n");
+		return PTR_ERR(sensor->ext_clk);
+	}
+
+	ret = of_property_read_u32(dev->of_node, "clock-frequency",
+				   &sensor->xclk_freq);
+	if (ret) {
+		dev_warn(dev, "can't get clock-frequency\n");
+		return ret;
+	}
+
+	mutex_init(&sensor->power_lock);
+
+	v4l2_i2c_subdev_init(&sensor->subdev, client, &et8ek8_ops);
+	sensor->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	sensor->subdev.internal_ops = &et8ek8_internal_ops;
+
+	sensor->subdev.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&sensor->subdev.entity, 1, &sensor->pad);
+	if (ret < 0) {
+		dev_err(&client->dev, "media entity init failed!\n");
+		goto err_mutex;
+	}
+
+	ret = v4l2_async_register_subdev_sensor(&sensor->subdev);
+	if (ret < 0)
+		goto err_entity;
+
+	dev_dbg(dev, "initialized!\n");
+
+	return 0;
+
+err_entity:
+	media_entity_cleanup(&sensor->subdev.entity);
+err_mutex:
+	mutex_destroy(&sensor->power_lock);
+	return ret;
+}
+
+static void __exit et8ek8_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct et8ek8_sensor *sensor = to_et8ek8_sensor(subdev);
+
+	if (sensor->power_count) {
+		WARN_ON(1);
+		et8ek8_power_off(sensor);
+		sensor->power_count = 0;
+	}
+
+	v4l2_device_unregister_subdev(&sensor->subdev);
+	device_remove_file(&client->dev, &dev_attr_priv_mem);
+	v4l2_ctrl_handler_free(&sensor->ctrl_handler);
+	v4l2_async_unregister_subdev(&sensor->subdev);
+	media_entity_cleanup(&sensor->subdev.entity);
+	mutex_destroy(&sensor->power_lock);
+}
+
+static const struct of_device_id et8ek8_of_table[] = {
+	{ .compatible = "toshiba,et8ek8" },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, et8ek8_of_table);
+
+static const struct i2c_device_id et8ek8_id_table[] = {
+	{ ET8EK8_NAME, 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, et8ek8_id_table);
+
+static const struct dev_pm_ops et8ek8_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(et8ek8_suspend, et8ek8_resume)
+};
+
+static struct i2c_driver et8ek8_i2c_driver = {
+	.driver		= {
+		.name	= ET8EK8_NAME,
+		.pm	= &et8ek8_pm_ops,
+		.of_match_table	= et8ek8_of_table,
+	},
+	.probe		= et8ek8_probe,
+	.remove		= __exit_p(et8ek8_remove),
+	.id_table	= et8ek8_id_table,
+};
+
+module_i2c_driver(et8ek8_i2c_driver);
+
+MODULE_AUTHOR("Sakari Ailus <sakari.ailus@iki.fi>, Pavel Machek <pavel@ucw.cz");
+MODULE_DESCRIPTION("Toshiba ET8EK8 camera sensor driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/et8ek8/et8ek8_mode.c b/drivers/media/i2c/et8ek8/et8ek8_mode.c
new file mode 100644
index 000000000..c9088eb0a
--- /dev/null
+++ b/drivers/media/i2c/et8ek8/et8ek8_mode.c
@@ -0,0 +1,579 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * et8ek8_mode.c
+ *
+ * Copyright (C) 2008 Nokia Corporation
+ *
+ * Contact: Sakari Ailus <sakari.ailus@iki.fi>
+ *          Tuukka Toivonen <tuukkat76@gmail.com>
+ */
+
+#include "et8ek8_reg.h"
+
+/*
+ * Stingray sensor mode settings for Scooby
+ */
+
+/* Mode1_poweron_Mode2_16VGA_2592x1968_12.07fps */
+static struct et8ek8_reglist mode1_poweron_mode2_16vga_2592x1968_12_07fps = {
+/* (without the +1)
+ * SPCK       = 80 MHz
+ * CCP2       = 640 MHz
+ * VCO        = 640 MHz
+ * VCOUNT     = 84 (2016)
+ * HCOUNT     = 137 (3288)
+ * CKREF_DIV  = 2
+ * CKVAR_DIV  = 200
+ * VCO_DIV    = 0
+ * SPCK_DIV   = 7
+ * MRCK_DIV   = 7
+ * LVDSCK_DIV = 0
+ */
+	.type = ET8EK8_REGLIST_POWERON,
+	.mode = {
+		.sensor_width = 2592,
+		.sensor_height = 1968,
+		.sensor_window_origin_x = 0,
+		.sensor_window_origin_y = 0,
+		.sensor_window_width = 2592,
+		.sensor_window_height = 1968,
+		.width = 3288,
+		.height = 2016,
+		.window_origin_x = 0,
+		.window_origin_y = 0,
+		.window_width = 2592,
+		.window_height = 1968,
+		.pixel_clock = 80000000,
+		.ext_clock = 9600000,
+		.timeperframe = {
+			.numerator = 100,
+			.denominator = 1207
+		},
+		.max_exp = 2012,
+		/* .max_gain = 0, */
+		.bus_format = MEDIA_BUS_FMT_SGRBG10_1X10,
+		.sensitivity = 65536
+	},
+	.regs = {
+		/* Need to set firstly */
+		{ ET8EK8_REG_8BIT, 0x126C, 0xCC },
+		/* Strobe and Data of CCP2 delay are minimized. */
+		{ ET8EK8_REG_8BIT, 0x1269, 0x00 },
+		/* Refined value of Min H_COUNT  */
+		{ ET8EK8_REG_8BIT, 0x1220, 0x89 },
+		/* Frequency of SPCK setting (SPCK=MRCK) */
+		{ ET8EK8_REG_8BIT, 0x123A, 0x07 },
+		{ ET8EK8_REG_8BIT, 0x1241, 0x94 },
+		{ ET8EK8_REG_8BIT, 0x1242, 0x02 },
+		{ ET8EK8_REG_8BIT, 0x124B, 0x00 },
+		{ ET8EK8_REG_8BIT, 0x1255, 0xFF },
+		{ ET8EK8_REG_8BIT, 0x1256, 0x9F },
+		{ ET8EK8_REG_8BIT, 0x1258, 0x00 },
+		/* From parallel out to serial out */
+		{ ET8EK8_REG_8BIT, 0x125D, 0x88 },
+		/* From w/ embedded data to w/o embedded data */
+		{ ET8EK8_REG_8BIT, 0x125E, 0xC0 },
+		/* CCP2 out is from STOP to ACTIVE */
+		{ ET8EK8_REG_8BIT, 0x1263, 0x98 },
+		{ ET8EK8_REG_8BIT, 0x1268, 0xC6 },
+		{ ET8EK8_REG_8BIT, 0x1434, 0x00 },
+		{ ET8EK8_REG_8BIT, 0x1163, 0x44 },
+		{ ET8EK8_REG_8BIT, 0x1166, 0x29 },
+		{ ET8EK8_REG_8BIT, 0x1140, 0x02 },
+		{ ET8EK8_REG_8BIT, 0x1011, 0x24 },
+		{ ET8EK8_REG_8BIT, 0x1151, 0x80 },
+		{ ET8EK8_REG_8BIT, 0x1152, 0x23 },
+		/* Initial setting for improvement2 of lower frequency noise */
+		{ ET8EK8_REG_8BIT, 0x1014, 0x05 },
+		{ ET8EK8_REG_8BIT, 0x1033, 0x06 },
+		{ ET8EK8_REG_8BIT, 0x1034, 0x79 },
+		{ ET8EK8_REG_8BIT, 0x1423, 0x3F },
+		{ ET8EK8_REG_8BIT, 0x1424, 0x3F },
+		{ ET8EK8_REG_8BIT, 0x1426, 0x00 },
+		/* Switch of Preset-White-balance (0d:disable / 1d:enable) */
+		{ ET8EK8_REG_8BIT, 0x1439, 0x00 },
+		/* Switch of blemish correction (0d:disable / 1d:enable) */
+		{ ET8EK8_REG_8BIT, 0x161F, 0x60 },
+		/* Switch of auto noise correction (0d:disable / 1d:enable) */
+		{ ET8EK8_REG_8BIT, 0x1634, 0x00 },
+		{ ET8EK8_REG_8BIT, 0x1646, 0x00 },
+		{ ET8EK8_REG_8BIT, 0x1648, 0x00 },
+		{ ET8EK8_REG_8BIT, 0x113E, 0x01 },
+		{ ET8EK8_REG_8BIT, 0x113F, 0x22 },
+		{ ET8EK8_REG_8BIT, 0x1239, 0x64 },
+		{ ET8EK8_REG_8BIT, 0x1238, 0x02 },
+		{ ET8EK8_REG_8BIT, 0x123B, 0x70 },
+		{ ET8EK8_REG_8BIT, 0x123A, 0x07 },
+		{ ET8EK8_REG_8BIT, 0x121B, 0x64 },
+		{ ET8EK8_REG_8BIT, 0x121D, 0x64 },
+		{ ET8EK8_REG_8BIT, 0x1221, 0x00 },
+		{ ET8EK8_REG_8BIT, 0x1220, 0x89 },
+		{ ET8EK8_REG_8BIT, 0x1223, 0x00 },
+		{ ET8EK8_REG_8BIT, 0x1222, 0x54 },
+		{ ET8EK8_REG_8BIT, 0x125D, 0x88 }, /* CCP_LVDS_MODE/  */
+		{ ET8EK8_REG_TERM, 0, 0}
+	}
+};
+
+/* Mode1_16VGA_2592x1968_13.12fps_DPCM10-8 */
+static struct et8ek8_reglist mode1_16vga_2592x1968_13_12fps_dpcm10_8 = {
+/* (without the +1)
+ * SPCK       = 80 MHz
+ * CCP2       = 560 MHz
+ * VCO        = 560 MHz
+ * VCOUNT     = 84 (2016)
+ * HCOUNT     = 128 (3072)
+ * CKREF_DIV  = 2
+ * CKVAR_DIV  = 175
+ * VCO_DIV    = 0
+ * SPCK_DIV   = 6
+ * MRCK_DIV   = 7
+ * LVDSCK_DIV = 0
+ */
+	.type = ET8EK8_REGLIST_MODE,
+	.mode = {
+		.sensor_width = 2592,
+		.sensor_height = 1968,
+		.sensor_window_origin_x = 0,
+		.sensor_window_origin_y = 0,
+		.sensor_window_width = 2592,
+		.sensor_window_height = 1968,
+		.width = 3072,
+		.height = 2016,
+		.window_origin_x = 0,
+		.window_origin_y = 0,
+		.window_width = 2592,
+		.window_height = 1968,
+		.pixel_clock = 80000000,
+		.ext_clock = 9600000,
+		.timeperframe = {
+			.numerator = 100,
+			.denominator = 1292
+		},
+		.max_exp = 2012,
+		/* .max_gain = 0, */
+		.bus_format = MEDIA_BUS_FMT_SGRBG10_DPCM8_1X8,
+		.sensitivity = 65536
+	},
+	.regs = {
+		{ ET8EK8_REG_8BIT, 0x1239, 0x57 },
+		{ ET8EK8_REG_8BIT, 0x1238, 0x82 },
+		{ ET8EK8_REG_8BIT, 0x123B, 0x70 },
+		{ ET8EK8_REG_8BIT, 0x123A, 0x06 },
+		{ ET8EK8_REG_8BIT, 0x121B, 0x64 },
+		{ ET8EK8_REG_8BIT, 0x121D, 0x64 },
+		{ ET8EK8_REG_8BIT, 0x1221, 0x00 },
+		{ ET8EK8_REG_8BIT, 0x1220, 0x80 }, /* <-changed to v14 7E->80 */
+		{ ET8EK8_REG_8BIT, 0x1223, 0x00 },
+		{ ET8EK8_REG_8BIT, 0x1222, 0x54 },
+		{ ET8EK8_REG_8BIT, 0x125D, 0x83 }, /* CCP_LVDS_MODE/  */
+		{ ET8EK8_REG_TERM, 0, 0}
+	}
+};
+
+/* Mode3_4VGA_1296x984_29.99fps_DPCM10-8 */
+static struct et8ek8_reglist mode3_4vga_1296x984_29_99fps_dpcm10_8 = {
+/* (without the +1)
+ * SPCK       = 96.5333333333333 MHz
+ * CCP2       = 579.2 MHz
+ * VCO        = 579.2 MHz
+ * VCOUNT     = 84 (2016)
+ * HCOUNT     = 133 (3192)
+ * CKREF_DIV  = 2
+ * CKVAR_DIV  = 181
+ * VCO_DIV    = 0
+ * SPCK_DIV   = 5
+ * MRCK_DIV   = 7
+ * LVDSCK_DIV = 0
+ */
+	.type = ET8EK8_REGLIST_MODE,
+	.mode = {
+		.sensor_width = 2592,
+		.sensor_height = 1968,
+		.sensor_window_origin_x = 0,
+		.sensor_window_origin_y = 0,
+		.sensor_window_width = 2592,
+		.sensor_window_height = 1968,
+		.width = 3192,
+		.height = 1008,
+		.window_origin_x = 0,
+		.window_origin_y = 0,
+		.window_width = 1296,
+		.window_height = 984,
+		.pixel_clock = 96533333,
+		.ext_clock = 9600000,
+		.timeperframe = {
+			.numerator = 100,
+			.denominator = 3000
+		},
+		.max_exp = 1004,
+		/* .max_gain = 0, */
+		.bus_format = MEDIA_BUS_FMT_SGRBG10_DPCM8_1X8,
+		.sensitivity = 65536
+	},
+	.regs = {
+		{ ET8EK8_REG_8BIT, 0x1239, 0x5A },
+		{ ET8EK8_REG_8BIT, 0x1238, 0x82 },
+		{ ET8EK8_REG_8BIT, 0x123B, 0x70 },
+		{ ET8EK8_REG_8BIT, 0x123A, 0x05 },
+		{ ET8EK8_REG_8BIT, 0x121B, 0x63 },
+		{ ET8EK8_REG_8BIT, 0x1220, 0x85 },
+		{ ET8EK8_REG_8BIT, 0x1221, 0x00 },
+		{ ET8EK8_REG_8BIT, 0x1222, 0x54 },
+		{ ET8EK8_REG_8BIT, 0x1223, 0x00 },
+		{ ET8EK8_REG_8BIT, 0x121D, 0x63 },
+		{ ET8EK8_REG_8BIT, 0x125D, 0x83 }, /* CCP_LVDS_MODE/  */
+		{ ET8EK8_REG_TERM, 0, 0}
+	}
+};
+
+/* Mode4_SVGA_864x656_29.88fps */
+static struct et8ek8_reglist mode4_svga_864x656_29_88fps = {
+/* (without the +1)
+ * SPCK       = 80 MHz
+ * CCP2       = 320 MHz
+ * VCO        = 640 MHz
+ * VCOUNT     = 84 (2016)
+ * HCOUNT     = 166 (3984)
+ * CKREF_DIV  = 2
+ * CKVAR_DIV  = 200
+ * VCO_DIV    = 0
+ * SPCK_DIV   = 7
+ * MRCK_DIV   = 7
+ * LVDSCK_DIV = 1
+ */
+	.type = ET8EK8_REGLIST_MODE,
+	.mode = {
+		.sensor_width = 2592,
+		.sensor_height = 1968,
+		.sensor_window_origin_x = 0,
+		.sensor_window_origin_y = 0,
+		.sensor_window_width = 2592,
+		.sensor_window_height = 1968,
+		.width = 3984,
+		.height = 672,
+		.window_origin_x = 0,
+		.window_origin_y = 0,
+		.window_width = 864,
+		.window_height = 656,
+		.pixel_clock = 80000000,
+		.ext_clock = 9600000,
+		.timeperframe = {
+			.numerator = 100,
+			.denominator = 2988
+		},
+		.max_exp = 668,
+		/* .max_gain = 0, */
+		.bus_format = MEDIA_BUS_FMT_SGRBG10_1X10,
+		.sensitivity = 65536
+	},
+	.regs = {
+		{ ET8EK8_REG_8BIT, 0x1239, 0x64 },
+		{ ET8EK8_REG_8BIT, 0x1238, 0x02 },
+		{ ET8EK8_REG_8BIT, 0x123B, 0x71 },
+		{ ET8EK8_REG_8BIT, 0x123A, 0x07 },
+		{ ET8EK8_REG_8BIT, 0x121B, 0x62 },
+		{ ET8EK8_REG_8BIT, 0x121D, 0x62 },
+		{ ET8EK8_REG_8BIT, 0x1221, 0x00 },
+		{ ET8EK8_REG_8BIT, 0x1220, 0xA6 },
+		{ ET8EK8_REG_8BIT, 0x1223, 0x00 },
+		{ ET8EK8_REG_8BIT, 0x1222, 0x54 },
+		{ ET8EK8_REG_8BIT, 0x125D, 0x88 }, /* CCP_LVDS_MODE/  */
+		{ ET8EK8_REG_TERM, 0, 0}
+	}
+};
+
+/* Mode5_VGA_648x492_29.93fps */
+static struct et8ek8_reglist mode5_vga_648x492_29_93fps = {
+/* (without the +1)
+ * SPCK       = 80 MHz
+ * CCP2       = 320 MHz
+ * VCO        = 640 MHz
+ * VCOUNT     = 84 (2016)
+ * HCOUNT     = 221 (5304)
+ * CKREF_DIV  = 2
+ * CKVAR_DIV  = 200
+ * VCO_DIV    = 0
+ * SPCK_DIV   = 7
+ * MRCK_DIV   = 7
+ * LVDSCK_DIV = 1
+ */
+	.type = ET8EK8_REGLIST_MODE,
+	.mode = {
+		.sensor_width = 2592,
+		.sensor_height = 1968,
+		.sensor_window_origin_x = 0,
+		.sensor_window_origin_y = 0,
+		.sensor_window_width = 2592,
+		.sensor_window_height = 1968,
+		.width = 5304,
+		.height = 504,
+		.window_origin_x = 0,
+		.window_origin_y = 0,
+		.window_width = 648,
+		.window_height = 492,
+		.pixel_clock = 80000000,
+		.ext_clock = 9600000,
+		.timeperframe = {
+			.numerator = 100,
+			.denominator = 2993
+		},
+		.max_exp = 500,
+		/* .max_gain = 0, */
+		.bus_format = MEDIA_BUS_FMT_SGRBG10_1X10,
+		.sensitivity = 65536
+	},
+	.regs = {
+		{ ET8EK8_REG_8BIT, 0x1239, 0x64 },
+		{ ET8EK8_REG_8BIT, 0x1238, 0x02 },
+		{ ET8EK8_REG_8BIT, 0x123B, 0x71 },
+		{ ET8EK8_REG_8BIT, 0x123A, 0x07 },
+		{ ET8EK8_REG_8BIT, 0x121B, 0x61 },
+		{ ET8EK8_REG_8BIT, 0x121D, 0x61 },
+		{ ET8EK8_REG_8BIT, 0x1221, 0x00 },
+		{ ET8EK8_REG_8BIT, 0x1220, 0xDD },
+		{ ET8EK8_REG_8BIT, 0x1223, 0x00 },
+		{ ET8EK8_REG_8BIT, 0x1222, 0x54 },
+		{ ET8EK8_REG_8BIT, 0x125D, 0x88 }, /* CCP_LVDS_MODE/  */
+		{ ET8EK8_REG_TERM, 0, 0}
+	}
+};
+
+/* Mode2_16VGA_2592x1968_3.99fps */
+static struct et8ek8_reglist mode2_16vga_2592x1968_3_99fps = {
+/* (without the +1)
+ * SPCK       = 80 MHz
+ * CCP2       = 640 MHz
+ * VCO        = 640 MHz
+ * VCOUNT     = 254 (6096)
+ * HCOUNT     = 137 (3288)
+ * CKREF_DIV  = 2
+ * CKVAR_DIV  = 200
+ * VCO_DIV    = 0
+ * SPCK_DIV   = 7
+ * MRCK_DIV   = 7
+ * LVDSCK_DIV = 0
+ */
+	.type = ET8EK8_REGLIST_MODE,
+	.mode = {
+		.sensor_width = 2592,
+		.sensor_height = 1968,
+		.sensor_window_origin_x = 0,
+		.sensor_window_origin_y = 0,
+		.sensor_window_width = 2592,
+		.sensor_window_height = 1968,
+		.width = 3288,
+		.height = 6096,
+		.window_origin_x = 0,
+		.window_origin_y = 0,
+		.window_width = 2592,
+		.window_height = 1968,
+		.pixel_clock = 80000000,
+		.ext_clock = 9600000,
+		.timeperframe = {
+			.numerator = 100,
+			.denominator = 399
+		},
+		.max_exp = 6092,
+		/* .max_gain = 0, */
+		.bus_format = MEDIA_BUS_FMT_SGRBG10_1X10,
+		.sensitivity = 65536
+	},
+	.regs = {
+		{ ET8EK8_REG_8BIT, 0x1239, 0x64 },
+		{ ET8EK8_REG_8BIT, 0x1238, 0x02 },
+		{ ET8EK8_REG_8BIT, 0x123B, 0x70 },
+		{ ET8EK8_REG_8BIT, 0x123A, 0x07 },
+		{ ET8EK8_REG_8BIT, 0x121B, 0x64 },
+		{ ET8EK8_REG_8BIT, 0x121D, 0x64 },
+		{ ET8EK8_REG_8BIT, 0x1221, 0x00 },
+		{ ET8EK8_REG_8BIT, 0x1220, 0x89 },
+		{ ET8EK8_REG_8BIT, 0x1223, 0x00 },
+		{ ET8EK8_REG_8BIT, 0x1222, 0xFE },
+		{ ET8EK8_REG_TERM, 0, 0}
+	}
+};
+
+/* Mode_648x492_5fps */
+static struct et8ek8_reglist mode_648x492_5fps = {
+/* (without the +1)
+ * SPCK       = 13.3333333333333 MHz
+ * CCP2       = 53.3333333333333 MHz
+ * VCO        = 640 MHz
+ * VCOUNT     = 84 (2016)
+ * HCOUNT     = 221 (5304)
+ * CKREF_DIV  = 2
+ * CKVAR_DIV  = 200
+ * VCO_DIV    = 5
+ * SPCK_DIV   = 7
+ * MRCK_DIV   = 7
+ * LVDSCK_DIV = 1
+ */
+	.type = ET8EK8_REGLIST_MODE,
+	.mode = {
+		.sensor_width = 2592,
+		.sensor_height = 1968,
+		.sensor_window_origin_x = 0,
+		.sensor_window_origin_y = 0,
+		.sensor_window_width = 2592,
+		.sensor_window_height = 1968,
+		.width = 5304,
+		.height = 504,
+		.window_origin_x = 0,
+		.window_origin_y = 0,
+		.window_width = 648,
+		.window_height = 492,
+		.pixel_clock = 13333333,
+		.ext_clock = 9600000,
+		.timeperframe = {
+			.numerator = 100,
+			.denominator = 499
+		},
+		.max_exp = 500,
+		/* .max_gain = 0, */
+		.bus_format = MEDIA_BUS_FMT_SGRBG10_1X10,
+		.sensitivity = 65536
+	},
+	.regs = {
+		{ ET8EK8_REG_8BIT, 0x1239, 0x64 },
+		{ ET8EK8_REG_8BIT, 0x1238, 0x02 },
+		{ ET8EK8_REG_8BIT, 0x123B, 0x71 },
+		{ ET8EK8_REG_8BIT, 0x123A, 0x57 },
+		{ ET8EK8_REG_8BIT, 0x121B, 0x61 },
+		{ ET8EK8_REG_8BIT, 0x121D, 0x61 },
+		{ ET8EK8_REG_8BIT, 0x1221, 0x00 },
+		{ ET8EK8_REG_8BIT, 0x1220, 0xDD },
+		{ ET8EK8_REG_8BIT, 0x1223, 0x00 },
+		{ ET8EK8_REG_8BIT, 0x1222, 0x54 },
+		{ ET8EK8_REG_8BIT, 0x125D, 0x88 }, /* CCP_LVDS_MODE/  */
+		{ ET8EK8_REG_TERM, 0, 0}
+	}
+};
+
+/* Mode3_4VGA_1296x984_5fps */
+static struct et8ek8_reglist mode3_4vga_1296x984_5fps = {
+/* (without the +1)
+ * SPCK       = 49.4 MHz
+ * CCP2       = 395.2 MHz
+ * VCO        = 790.4 MHz
+ * VCOUNT     = 250 (6000)
+ * HCOUNT     = 137 (3288)
+ * CKREF_DIV  = 2
+ * CKVAR_DIV  = 247
+ * VCO_DIV    = 1
+ * SPCK_DIV   = 7
+ * MRCK_DIV   = 7
+ * LVDSCK_DIV = 0
+ */
+	.type = ET8EK8_REGLIST_MODE,
+	.mode = {
+		.sensor_width = 2592,
+		.sensor_height = 1968,
+		.sensor_window_origin_x = 0,
+		.sensor_window_origin_y = 0,
+		.sensor_window_width = 2592,
+		.sensor_window_height = 1968,
+		.width = 3288,
+		.height = 3000,
+		.window_origin_x = 0,
+		.window_origin_y = 0,
+		.window_width = 1296,
+		.window_height = 984,
+		.pixel_clock = 49400000,
+		.ext_clock = 9600000,
+		.timeperframe = {
+			.numerator = 100,
+			.denominator = 501
+		},
+		.max_exp = 2996,
+		/* .max_gain = 0, */
+		.bus_format = MEDIA_BUS_FMT_SGRBG10_1X10,
+		.sensitivity = 65536
+	},
+	.regs = {
+		{ ET8EK8_REG_8BIT, 0x1239, 0x7B },
+		{ ET8EK8_REG_8BIT, 0x1238, 0x82 },
+		{ ET8EK8_REG_8BIT, 0x123B, 0x70 },
+		{ ET8EK8_REG_8BIT, 0x123A, 0x17 },
+		{ ET8EK8_REG_8BIT, 0x121B, 0x63 },
+		{ ET8EK8_REG_8BIT, 0x121D, 0x63 },
+		{ ET8EK8_REG_8BIT, 0x1221, 0x00 },
+		{ ET8EK8_REG_8BIT, 0x1220, 0x89 },
+		{ ET8EK8_REG_8BIT, 0x1223, 0x00 },
+		{ ET8EK8_REG_8BIT, 0x1222, 0xFA },
+		{ ET8EK8_REG_8BIT, 0x125D, 0x88 }, /* CCP_LVDS_MODE/  */
+		{ ET8EK8_REG_TERM, 0, 0}
+	}
+};
+
+/* Mode_4VGA_1296x984_25fps_DPCM10-8 */
+static struct et8ek8_reglist mode_4vga_1296x984_25fps_dpcm10_8 = {
+/* (without the +1)
+ * SPCK       = 84.2666666666667 MHz
+ * CCP2       = 505.6 MHz
+ * VCO        = 505.6 MHz
+ * VCOUNT     = 88 (2112)
+ * HCOUNT     = 133 (3192)
+ * CKREF_DIV  = 2
+ * CKVAR_DIV  = 158
+ * VCO_DIV    = 0
+ * SPCK_DIV   = 5
+ * MRCK_DIV   = 7
+ * LVDSCK_DIV = 0
+ */
+	.type = ET8EK8_REGLIST_MODE,
+	.mode = {
+		.sensor_width = 2592,
+		.sensor_height = 1968,
+		.sensor_window_origin_x = 0,
+		.sensor_window_origin_y = 0,
+		.sensor_window_width = 2592,
+		.sensor_window_height = 1968,
+		.width = 3192,
+		.height = 1056,
+		.window_origin_x = 0,
+		.window_origin_y = 0,
+		.window_width = 1296,
+		.window_height = 984,
+		.pixel_clock = 84266667,
+		.ext_clock = 9600000,
+		.timeperframe = {
+			.numerator = 100,
+			.denominator = 2500
+		},
+		.max_exp = 1052,
+		/* .max_gain = 0, */
+		.bus_format = MEDIA_BUS_FMT_SGRBG10_DPCM8_1X8,
+		.sensitivity = 65536
+	},
+	.regs = {
+		{ ET8EK8_REG_8BIT, 0x1239, 0x4F },
+		{ ET8EK8_REG_8BIT, 0x1238, 0x02 },
+		{ ET8EK8_REG_8BIT, 0x123B, 0x70 },
+		{ ET8EK8_REG_8BIT, 0x123A, 0x05 },
+		{ ET8EK8_REG_8BIT, 0x121B, 0x63 },
+		{ ET8EK8_REG_8BIT, 0x1220, 0x85 },
+		{ ET8EK8_REG_8BIT, 0x1221, 0x00 },
+		{ ET8EK8_REG_8BIT, 0x1222, 0x58 },
+		{ ET8EK8_REG_8BIT, 0x1223, 0x00 },
+		{ ET8EK8_REG_8BIT, 0x121D, 0x63 },
+		{ ET8EK8_REG_8BIT, 0x125D, 0x83 },
+		{ ET8EK8_REG_TERM, 0, 0}
+	}
+};
+
+struct et8ek8_meta_reglist meta_reglist = {
+	.version = "V14 03-June-2008",
+	.reglist = {
+		{ .ptr = &mode1_poweron_mode2_16vga_2592x1968_12_07fps },
+		{ .ptr = &mode1_16vga_2592x1968_13_12fps_dpcm10_8 },
+		{ .ptr = &mode3_4vga_1296x984_29_99fps_dpcm10_8 },
+		{ .ptr = &mode4_svga_864x656_29_88fps },
+		{ .ptr = &mode5_vga_648x492_29_93fps },
+		{ .ptr = &mode2_16vga_2592x1968_3_99fps },
+		{ .ptr = &mode_648x492_5fps },
+		{ .ptr = &mode3_4vga_1296x984_5fps },
+		{ .ptr = &mode_4vga_1296x984_25fps_dpcm10_8 },
+		{ .ptr = NULL }
+	}
+};
diff --git a/drivers/media/i2c/et8ek8/et8ek8_reg.h b/drivers/media/i2c/et8ek8/et8ek8_reg.h
new file mode 100644
index 000000000..c90e74935
--- /dev/null
+++ b/drivers/media/i2c/et8ek8/et8ek8_reg.h
@@ -0,0 +1,88 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * et8ek8_reg.h
+ *
+ * Copyright (C) 2008 Nokia Corporation
+ *
+ * Contact: Sakari Ailus <sakari.ailus@iki.fi>
+ *          Tuukka Toivonen <tuukkat76@gmail.com>
+ */
+
+#ifndef ET8EK8REGS_H
+#define ET8EK8REGS_H
+
+#include <linux/i2c.h>
+#include <linux/types.h>
+#include <linux/videodev2.h>
+#include <linux/v4l2-subdev.h>
+
+struct v4l2_mbus_framefmt;
+struct v4l2_subdev_pad_mbus_code_enum;
+
+struct et8ek8_mode {
+	/* Physical sensor resolution and current image window */
+	u16 sensor_width;
+	u16 sensor_height;
+	u16 sensor_window_origin_x;
+	u16 sensor_window_origin_y;
+	u16 sensor_window_width;
+	u16 sensor_window_height;
+
+	/* Image data coming from sensor (after scaling) */
+	u16 width;
+	u16 height;
+	u16 window_origin_x;
+	u16 window_origin_y;
+	u16 window_width;
+	u16 window_height;
+
+	u32 pixel_clock;		/* in Hz */
+	u32 ext_clock;			/* in Hz */
+	struct v4l2_fract timeperframe;
+	u32 max_exp;			/* Maximum exposure value */
+	u32 bus_format;			/* MEDIA_BUS_FMT_ */
+	u32 sensitivity;		/* 16.16 fixed point */
+};
+
+#define ET8EK8_REG_8BIT			1
+#define ET8EK8_REG_16BIT		2
+#define ET8EK8_REG_DELAY		100
+#define ET8EK8_REG_TERM			0xff
+struct et8ek8_reg {
+	u16 type;
+	u16 reg;			/* 16-bit offset */
+	u32 val;			/* 8/16/32-bit value */
+};
+
+/* Possible struct smia_reglist types. */
+#define ET8EK8_REGLIST_STANDBY		0
+#define ET8EK8_REGLIST_POWERON		1
+#define ET8EK8_REGLIST_RESUME		2
+#define ET8EK8_REGLIST_STREAMON		3
+#define ET8EK8_REGLIST_STREAMOFF	4
+#define ET8EK8_REGLIST_DISABLED		5
+
+#define ET8EK8_REGLIST_MODE		10
+
+#define ET8EK8_REGLIST_LSC_ENABLE	100
+#define ET8EK8_REGLIST_LSC_DISABLE	101
+#define ET8EK8_REGLIST_ANR_ENABLE	102
+#define ET8EK8_REGLIST_ANR_DISABLE	103
+
+struct et8ek8_reglist {
+	u32 type;
+	struct et8ek8_mode mode;
+	struct et8ek8_reg regs[];
+};
+
+#define ET8EK8_MAX_LEN			32
+struct et8ek8_meta_reglist {
+	char version[ET8EK8_MAX_LEN];
+	union {
+		struct et8ek8_reglist *ptr;
+	} reglist[];
+};
+
+extern struct et8ek8_meta_reglist meta_reglist;
+
+#endif /* ET8EK8REGS */
diff --git a/drivers/media/i2c/hi556.c b/drivers/media/i2c/hi556.c
new file mode 100644
index 000000000..fd56ba138
--- /dev/null
+++ b/drivers/media/i2c/hi556.c
@@ -0,0 +1,1362 @@
+// SPDX-License-Identifier: GPL-2.0
+// Copyright (c) 2019 Intel Corporation.
+
+#include <asm/unaligned.h>
+#include <linux/acpi.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+
+#define HI556_REG_VALUE_08BIT		1
+#define HI556_REG_VALUE_16BIT		2
+#define HI556_REG_VALUE_24BIT		3
+
+#define HI556_LINK_FREQ_437MHZ		437000000ULL
+#define HI556_MCLK			19200000
+#define HI556_DATA_LANES		2
+#define HI556_RGB_DEPTH			10
+
+#define HI556_REG_CHIP_ID		0x0f16
+#define HI556_CHIP_ID			0x0556
+
+#define HI556_REG_MODE_SELECT		0x0a00
+#define HI556_MODE_STANDBY		0x0000
+#define HI556_MODE_STREAMING		0x0100
+
+/* vertical-timings from sensor */
+#define HI556_REG_FLL			0x0006
+#define HI556_FLL_30FPS			0x0814
+#define HI556_FLL_30FPS_MIN		0x0814
+#define HI556_FLL_MAX			0x7fff
+
+/* horizontal-timings from sensor */
+#define HI556_REG_LLP			0x0008
+
+/* Exposure controls from sensor */
+#define HI556_REG_EXPOSURE		0x0074
+#define HI556_EXPOSURE_MIN		6
+#define HI556_EXPOSURE_MAX_MARGIN	2
+#define HI556_EXPOSURE_STEP		1
+
+/* Analog gain controls from sensor */
+#define HI556_REG_ANALOG_GAIN		0x0077
+#define HI556_ANAL_GAIN_MIN		0
+#define HI556_ANAL_GAIN_MAX		240
+#define HI556_ANAL_GAIN_STEP		1
+
+/* Digital gain controls from sensor */
+#define HI556_REG_MWB_GR_GAIN		0x0078
+#define HI556_REG_MWB_GB_GAIN		0x007a
+#define HI556_REG_MWB_R_GAIN		0x007c
+#define HI556_REG_MWB_B_GAIN		0x007e
+#define HI556_DGTL_GAIN_MIN		0
+#define HI556_DGTL_GAIN_MAX		2048
+#define HI556_DGTL_GAIN_STEP		1
+#define HI556_DGTL_GAIN_DEFAULT		256
+
+/* Test Pattern Control */
+#define HI556_REG_ISP			0X0a05
+#define HI556_REG_ISP_TPG_EN		0x01
+#define HI556_REG_TEST_PATTERN		0x0201
+
+/* HI556 native and active pixel array size. */
+#define HI556_NATIVE_WIDTH		2592U
+#define HI556_NATIVE_HEIGHT		1944U
+#define HI556_PIXEL_ARRAY_LEFT		0U
+#define HI556_PIXEL_ARRAY_TOP		0U
+#define HI556_PIXEL_ARRAY_WIDTH	2592U
+#define HI556_PIXEL_ARRAY_HEIGHT	1944U
+
+enum {
+	HI556_LINK_FREQ_437MHZ_INDEX,
+};
+
+struct hi556_reg {
+	u16 address;
+	u16 val;
+};
+
+struct hi556_reg_list {
+	u32 num_of_regs;
+	const struct hi556_reg *regs;
+};
+
+struct hi556_link_freq_config {
+	const struct hi556_reg_list reg_list;
+};
+
+struct hi556_mode {
+	/* Frame width in pixels */
+	u32 width;
+
+	/* Frame height in pixels */
+	u32 height;
+
+	/* Analog crop rectangle. */
+	struct v4l2_rect crop;
+
+	/* Horizontal timining size */
+	u32 llp;
+
+	/* Default vertical timining size */
+	u32 fll_def;
+
+	/* Min vertical timining size */
+	u32 fll_min;
+
+	/* Link frequency needed for this resolution */
+	u32 link_freq_index;
+
+	/* Sensor register settings for this resolution */
+	const struct hi556_reg_list reg_list;
+};
+
+#define to_hi556(_sd) container_of(_sd, struct hi556, sd)
+
+//SENSOR_INITIALIZATION
+static const struct hi556_reg mipi_data_rate_874mbps[] = {
+	{0x0e00, 0x0102},
+	{0x0e02, 0x0102},
+	{0x0e0c, 0x0100},
+	{0x2000, 0x7400},
+	{0x2002, 0x001c},
+	{0x2004, 0x0242},
+	{0x2006, 0x0942},
+	{0x2008, 0x7007},
+	{0x200a, 0x0fd9},
+	{0x200c, 0x0259},
+	{0x200e, 0x7008},
+	{0x2010, 0x160e},
+	{0x2012, 0x0047},
+	{0x2014, 0x2118},
+	{0x2016, 0x0041},
+	{0x2018, 0x00d8},
+	{0x201a, 0x0145},
+	{0x201c, 0x0006},
+	{0x201e, 0x0181},
+	{0x2020, 0x13cc},
+	{0x2022, 0x2057},
+	{0x2024, 0x7001},
+	{0x2026, 0x0fca},
+	{0x2028, 0x00cb},
+	{0x202a, 0x009f},
+	{0x202c, 0x7002},
+	{0x202e, 0x13cc},
+	{0x2030, 0x019b},
+	{0x2032, 0x014d},
+	{0x2034, 0x2987},
+	{0x2036, 0x2766},
+	{0x2038, 0x0020},
+	{0x203a, 0x2060},
+	{0x203c, 0x0e5d},
+	{0x203e, 0x181d},
+	{0x2040, 0x2066},
+	{0x2042, 0x20c4},
+	{0x2044, 0x5000},
+	{0x2046, 0x0005},
+	{0x2048, 0x0000},
+	{0x204a, 0x01db},
+	{0x204c, 0x025a},
+	{0x204e, 0x00c0},
+	{0x2050, 0x0005},
+	{0x2052, 0x0006},
+	{0x2054, 0x0ad9},
+	{0x2056, 0x0259},
+	{0x2058, 0x0618},
+	{0x205a, 0x0258},
+	{0x205c, 0x2266},
+	{0x205e, 0x20c8},
+	{0x2060, 0x2060},
+	{0x2062, 0x707b},
+	{0x2064, 0x0fdd},
+	{0x2066, 0x81b8},
+	{0x2068, 0x5040},
+	{0x206a, 0x0020},
+	{0x206c, 0x5060},
+	{0x206e, 0x3143},
+	{0x2070, 0x5081},
+	{0x2072, 0x025c},
+	{0x2074, 0x7800},
+	{0x2076, 0x7400},
+	{0x2078, 0x001c},
+	{0x207a, 0x0242},
+	{0x207c, 0x0942},
+	{0x207e, 0x0bd9},
+	{0x2080, 0x0259},
+	{0x2082, 0x7008},
+	{0x2084, 0x160e},
+	{0x2086, 0x0047},
+	{0x2088, 0x2118},
+	{0x208a, 0x0041},
+	{0x208c, 0x00d8},
+	{0x208e, 0x0145},
+	{0x2090, 0x0006},
+	{0x2092, 0x0181},
+	{0x2094, 0x13cc},
+	{0x2096, 0x2057},
+	{0x2098, 0x7001},
+	{0x209a, 0x0fca},
+	{0x209c, 0x00cb},
+	{0x209e, 0x009f},
+	{0x20a0, 0x7002},
+	{0x20a2, 0x13cc},
+	{0x20a4, 0x019b},
+	{0x20a6, 0x014d},
+	{0x20a8, 0x2987},
+	{0x20aa, 0x2766},
+	{0x20ac, 0x0020},
+	{0x20ae, 0x2060},
+	{0x20b0, 0x0e5d},
+	{0x20b2, 0x181d},
+	{0x20b4, 0x2066},
+	{0x20b6, 0x20c4},
+	{0x20b8, 0x50a0},
+	{0x20ba, 0x0005},
+	{0x20bc, 0x0000},
+	{0x20be, 0x01db},
+	{0x20c0, 0x025a},
+	{0x20c2, 0x00c0},
+	{0x20c4, 0x0005},
+	{0x20c6, 0x0006},
+	{0x20c8, 0x0ad9},
+	{0x20ca, 0x0259},
+	{0x20cc, 0x0618},
+	{0x20ce, 0x0258},
+	{0x20d0, 0x2266},
+	{0x20d2, 0x20c8},
+	{0x20d4, 0x2060},
+	{0x20d6, 0x707b},
+	{0x20d8, 0x0fdd},
+	{0x20da, 0x86b8},
+	{0x20dc, 0x50e0},
+	{0x20de, 0x0020},
+	{0x20e0, 0x5100},
+	{0x20e2, 0x3143},
+	{0x20e4, 0x5121},
+	{0x20e6, 0x7800},
+	{0x20e8, 0x3140},
+	{0x20ea, 0x01c4},
+	{0x20ec, 0x01c1},
+	{0x20ee, 0x01c0},
+	{0x20f0, 0x01c4},
+	{0x20f2, 0x2700},
+	{0x20f4, 0x3d40},
+	{0x20f6, 0x7800},
+	{0x20f8, 0xffff},
+	{0x27fe, 0xe000},
+	{0x3000, 0x60f8},
+	{0x3002, 0x187f},
+	{0x3004, 0x7060},
+	{0x3006, 0x0114},
+	{0x3008, 0x60b0},
+	{0x300a, 0x1473},
+	{0x300c, 0x0013},
+	{0x300e, 0x140f},
+	{0x3010, 0x0040},
+	{0x3012, 0x100f},
+	{0x3014, 0x60f8},
+	{0x3016, 0x187f},
+	{0x3018, 0x7060},
+	{0x301a, 0x0114},
+	{0x301c, 0x60b0},
+	{0x301e, 0x1473},
+	{0x3020, 0x0013},
+	{0x3022, 0x140f},
+	{0x3024, 0x0040},
+	{0x3026, 0x000f},
+
+	{0x0b00, 0x0000},
+	{0x0b02, 0x0045},
+	{0x0b04, 0xb405},
+	{0x0b06, 0xc403},
+	{0x0b08, 0x0081},
+	{0x0b0a, 0x8252},
+	{0x0b0c, 0xf814},
+	{0x0b0e, 0xc618},
+	{0x0b10, 0xa828},
+	{0x0b12, 0x004c},
+	{0x0b14, 0x4068},
+	{0x0b16, 0x0000},
+	{0x0f30, 0x5b15},
+	{0x0f32, 0x7067},
+	{0x0954, 0x0009},
+	{0x0956, 0x0000},
+	{0x0958, 0xbb80},
+	{0x095a, 0x5140},
+	{0x0c00, 0x1110},
+	{0x0c02, 0x0011},
+	{0x0c04, 0x0000},
+	{0x0c06, 0x0200},
+	{0x0c10, 0x0040},
+	{0x0c12, 0x0040},
+	{0x0c14, 0x0040},
+	{0x0c16, 0x0040},
+	{0x0a10, 0x4000},
+	{0x3068, 0xf800},
+	{0x306a, 0xf876},
+	{0x006c, 0x0000},
+	{0x005e, 0x0200},
+	{0x000e, 0x0100},
+	{0x0e0a, 0x0001},
+	{0x004a, 0x0100},
+	{0x004c, 0x0000},
+	{0x004e, 0x0100},
+	{0x000c, 0x0022},
+	{0x0008, 0x0b00},
+	{0x005a, 0x0202},
+	{0x0012, 0x000e},
+	{0x0018, 0x0a33},
+	{0x0022, 0x0008},
+	{0x0028, 0x0017},
+	{0x0024, 0x0028},
+	{0x002a, 0x002d},
+	{0x0026, 0x0030},
+	{0x002c, 0x07c9},
+	{0x002e, 0x1111},
+	{0x0030, 0x1111},
+	{0x0032, 0x1111},
+	{0x0006, 0x07bc},
+	{0x0a22, 0x0000},
+	{0x0a12, 0x0a20},
+	{0x0a14, 0x0798},
+	{0x003e, 0x0000},
+	{0x0074, 0x080e},
+	{0x0070, 0x0407},
+	{0x0002, 0x0000},
+	{0x0a02, 0x0100},
+	{0x0a24, 0x0100},
+	{0x0046, 0x0000},
+	{0x0076, 0x0000},
+	{0x0060, 0x0000},
+	{0x0062, 0x0530},
+	{0x0064, 0x0500},
+	{0x0066, 0x0530},
+	{0x0068, 0x0500},
+	{0x0122, 0x0300},
+	{0x015a, 0xff08},
+	{0x0804, 0x0300},
+	{0x0806, 0x0100},
+	{0x005c, 0x0102},
+	{0x0a1a, 0x0800},
+};
+
+static const struct hi556_reg mode_2592x1944_regs[] = {
+	{0x0a00, 0x0000},
+	{0x0b0a, 0x8252},
+	{0x0f30, 0x5b15},
+	{0x0f32, 0x7067},
+	{0x004a, 0x0100},
+	{0x004c, 0x0000},
+	{0x004e, 0x0100},
+	{0x000c, 0x0022},
+	{0x0008, 0x0b00},
+	{0x005a, 0x0202},
+	{0x0012, 0x000e},
+	{0x0018, 0x0a33},
+	{0x0022, 0x0008},
+	{0x0028, 0x0017},
+	{0x0024, 0x0028},
+	{0x002a, 0x002d},
+	{0x0026, 0x0030},
+	{0x002c, 0x07c9},
+	{0x002e, 0x1111},
+	{0x0030, 0x1111},
+	{0x0032, 0x1111},
+	{0x0006, 0x0814},
+	{0x0a22, 0x0000},
+	{0x0a12, 0x0a20},
+	{0x0a14, 0x0798},
+	{0x003e, 0x0000},
+	{0x0074, 0x0812},
+	{0x0070, 0x0409},
+	{0x0804, 0x0300},
+	{0x0806, 0x0100},
+	{0x0a04, 0x014a},
+	{0x090c, 0x0fdc},
+	{0x090e, 0x002d},
+
+	{0x0902, 0x4319},
+	{0x0914, 0xc10a},
+	{0x0916, 0x071f},
+	{0x0918, 0x0408},
+	{0x091a, 0x0c0d},
+	{0x091c, 0x0f09},
+	{0x091e, 0x0a00},
+	{0x0958, 0xbb80},
+};
+
+static const struct hi556_reg mode_2592x1444_regs[] = {
+	{0x0a00, 0x0000},
+	{0x0b0a, 0x8252},
+	{0x0f30, 0xe545},
+	{0x0f32, 0x7067},
+	{0x004a, 0x0100},
+	{0x004c, 0x0000},
+	{0x000c, 0x0022},
+	{0x0008, 0x0b00},
+	{0x005a, 0x0202},
+	{0x0012, 0x000e},
+	{0x0018, 0x0a33},
+	{0x0022, 0x0008},
+	{0x0028, 0x0017},
+	{0x0024, 0x0122},
+	{0x002a, 0x0127},
+	{0x0026, 0x012a},
+	{0x002c, 0x06cf},
+	{0x002e, 0x1111},
+	{0x0030, 0x1111},
+	{0x0032, 0x1111},
+	{0x0006, 0x0821},
+	{0x0a22, 0x0000},
+	{0x0a12, 0x0a20},
+	{0x0a14, 0x05a4},
+	{0x003e, 0x0000},
+	{0x0074, 0x081f},
+	{0x0070, 0x040f},
+	{0x0804, 0x0300},
+	{0x0806, 0x0100},
+	{0x0a04, 0x014a},
+	{0x090c, 0x0fdc},
+	{0x090e, 0x002d},
+	{0x0902, 0x4319},
+	{0x0914, 0xc10a},
+	{0x0916, 0x071f},
+	{0x0918, 0x0408},
+	{0x091a, 0x0c0d},
+	{0x091c, 0x0f09},
+	{0x091e, 0x0a00},
+	{0x0958, 0xbb80},
+};
+
+static const struct hi556_reg mode_1296x972_regs[] = {
+	{0x0a00, 0x0000},
+	{0x0b0a, 0x8259},
+	{0x0f30, 0x5b15},
+	{0x0f32, 0x7167},
+	{0x004a, 0x0100},
+	{0x004c, 0x0000},
+	{0x004e, 0x0100},
+	{0x000c, 0x0122},
+	{0x0008, 0x0b00},
+	{0x005a, 0x0404},
+	{0x0012, 0x000c},
+	{0x0018, 0x0a33},
+	{0x0022, 0x0008},
+	{0x0028, 0x0017},
+	{0x0024, 0x0022},
+	{0x002a, 0x002b},
+	{0x0026, 0x0030},
+	{0x002c, 0x07c9},
+	{0x002e, 0x3311},
+	{0x0030, 0x3311},
+	{0x0032, 0x3311},
+	{0x0006, 0x0814},
+	{0x0a22, 0x0000},
+	{0x0a12, 0x0510},
+	{0x0a14, 0x03cc},
+	{0x003e, 0x0000},
+	{0x0074, 0x0812},
+	{0x0070, 0x0409},
+	{0x0804, 0x0308},
+	{0x0806, 0x0100},
+	{0x0a04, 0x016a},
+	{0x090e, 0x0010},
+	{0x090c, 0x09c0},
+
+	{0x0902, 0x4319},
+	{0x0914, 0xc106},
+	{0x0916, 0x040e},
+	{0x0918, 0x0304},
+	{0x091a, 0x0708},
+	{0x091c, 0x0e06},
+	{0x091e, 0x0300},
+	{0x0958, 0xbb80},
+};
+
+static const char * const hi556_test_pattern_menu[] = {
+	"Disabled",
+	"Solid Colour",
+	"100% Colour Bars",
+	"Fade To Grey Colour Bars",
+	"PN9",
+	"Gradient Horizontal",
+	"Gradient Vertical",
+	"Check Board",
+	"Slant Pattern",
+};
+
+static const s64 link_freq_menu_items[] = {
+	HI556_LINK_FREQ_437MHZ,
+};
+
+static const struct hi556_link_freq_config link_freq_configs[] = {
+	[HI556_LINK_FREQ_437MHZ_INDEX] = {
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mipi_data_rate_874mbps),
+			.regs = mipi_data_rate_874mbps,
+		}
+	}
+};
+
+static const struct hi556_mode supported_modes[] = {
+	{
+		.width = HI556_PIXEL_ARRAY_WIDTH,
+		.height = HI556_PIXEL_ARRAY_HEIGHT,
+		.crop = {
+			.left = HI556_PIXEL_ARRAY_LEFT,
+			.top = HI556_PIXEL_ARRAY_TOP,
+			.width = HI556_PIXEL_ARRAY_WIDTH,
+			.height = HI556_PIXEL_ARRAY_HEIGHT
+		},
+		.fll_def = HI556_FLL_30FPS,
+		.fll_min = HI556_FLL_30FPS_MIN,
+		.llp = 0x0b00,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_2592x1944_regs),
+			.regs = mode_2592x1944_regs,
+		},
+		.link_freq_index = HI556_LINK_FREQ_437MHZ_INDEX,
+	},
+	{
+		.width = HI556_PIXEL_ARRAY_WIDTH,
+		.height = 1444,
+		.crop = {
+			.left = HI556_PIXEL_ARRAY_LEFT,
+			.top = 250,
+			.width = HI556_PIXEL_ARRAY_WIDTH,
+			.height = 1444
+		},
+		.fll_def = 0x821,
+		.fll_min = 0x821,
+		.llp = 0x0b00,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_2592x1444_regs),
+			.regs = mode_2592x1444_regs,
+		},
+		.link_freq_index = HI556_LINK_FREQ_437MHZ_INDEX,
+	},
+	{
+		.width = 1296,
+		.height = 972,
+		.crop = {
+			.left = HI556_PIXEL_ARRAY_LEFT,
+			.top = HI556_PIXEL_ARRAY_TOP,
+			.width = HI556_PIXEL_ARRAY_WIDTH,
+			.height = HI556_PIXEL_ARRAY_HEIGHT
+		},
+		.fll_def = HI556_FLL_30FPS,
+		.fll_min = HI556_FLL_30FPS_MIN,
+		.llp = 0x0b00,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1296x972_regs),
+			.regs = mode_1296x972_regs,
+		},
+		.link_freq_index = HI556_LINK_FREQ_437MHZ_INDEX,
+	}
+};
+
+struct hi556 {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct v4l2_ctrl_handler ctrl_handler;
+
+	/* V4L2 Controls */
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *pixel_rate;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *exposure;
+
+	/* Current mode */
+	const struct hi556_mode *cur_mode;
+
+	/* To serialize asynchronus callbacks */
+	struct mutex mutex;
+
+	/* Streaming on/off */
+	bool streaming;
+
+	/* True if the device has been identified */
+	bool identified;
+};
+
+static u64 to_pixel_rate(u32 f_index)
+{
+	u64 pixel_rate = link_freq_menu_items[f_index] * 2 * HI556_DATA_LANES;
+
+	do_div(pixel_rate, HI556_RGB_DEPTH);
+
+	return pixel_rate;
+}
+
+static int hi556_read_reg(struct hi556 *hi556, u16 reg, u16 len, u32 *val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&hi556->sd);
+	struct i2c_msg msgs[2];
+	u8 addr_buf[2];
+	u8 data_buf[4] = {0};
+	int ret;
+
+	if (len > 4)
+		return -EINVAL;
+
+	put_unaligned_be16(reg, addr_buf);
+	msgs[0].addr = client->addr;
+	msgs[0].flags = 0;
+	msgs[0].len = sizeof(addr_buf);
+	msgs[0].buf = addr_buf;
+	msgs[1].addr = client->addr;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].len = len;
+	msgs[1].buf = &data_buf[4 - len];
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret != ARRAY_SIZE(msgs))
+		return -EIO;
+
+	*val = get_unaligned_be32(data_buf);
+
+	return 0;
+}
+
+static int hi556_write_reg(struct hi556 *hi556, u16 reg, u16 len, u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&hi556->sd);
+	u8 buf[6];
+
+	if (len > 4)
+		return -EINVAL;
+
+	put_unaligned_be16(reg, buf);
+	put_unaligned_be32(val << 8 * (4 - len), buf + 2);
+	if (i2c_master_send(client, buf, len + 2) != len + 2)
+		return -EIO;
+
+	return 0;
+}
+
+static int hi556_write_reg_list(struct hi556 *hi556,
+				const struct hi556_reg_list *r_list)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&hi556->sd);
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < r_list->num_of_regs; i++) {
+		ret = hi556_write_reg(hi556, r_list->regs[i].address,
+				      HI556_REG_VALUE_16BIT,
+				      r_list->regs[i].val);
+		if (ret) {
+			dev_err_ratelimited(&client->dev,
+					    "failed to write reg 0x%4.4x. error = %d",
+					    r_list->regs[i].address, ret);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int hi556_update_digital_gain(struct hi556 *hi556, u32 d_gain)
+{
+	int ret;
+
+	ret = hi556_write_reg(hi556, HI556_REG_MWB_GR_GAIN,
+			      HI556_REG_VALUE_16BIT, d_gain);
+	if (ret)
+		return ret;
+
+	ret = hi556_write_reg(hi556, HI556_REG_MWB_GB_GAIN,
+			      HI556_REG_VALUE_16BIT, d_gain);
+	if (ret)
+		return ret;
+
+	ret = hi556_write_reg(hi556, HI556_REG_MWB_R_GAIN,
+			      HI556_REG_VALUE_16BIT, d_gain);
+	if (ret)
+		return ret;
+
+	return hi556_write_reg(hi556, HI556_REG_MWB_B_GAIN,
+			       HI556_REG_VALUE_16BIT, d_gain);
+}
+
+static int hi556_test_pattern(struct hi556 *hi556, u32 pattern)
+{
+	int ret;
+	u32 val;
+
+	if (pattern) {
+		ret = hi556_read_reg(hi556, HI556_REG_ISP,
+				     HI556_REG_VALUE_08BIT, &val);
+		if (ret)
+			return ret;
+
+		ret = hi556_write_reg(hi556, HI556_REG_ISP,
+				      HI556_REG_VALUE_08BIT,
+				      val | HI556_REG_ISP_TPG_EN);
+		if (ret)
+			return ret;
+	}
+
+	return hi556_write_reg(hi556, HI556_REG_TEST_PATTERN,
+			       HI556_REG_VALUE_08BIT, pattern);
+}
+
+static int hi556_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct hi556 *hi556 = container_of(ctrl->handler,
+					     struct hi556, ctrl_handler);
+	struct i2c_client *client = v4l2_get_subdevdata(&hi556->sd);
+	s64 exposure_max;
+	int ret = 0;
+
+	/* Propagate change of current control to all related controls */
+	if (ctrl->id == V4L2_CID_VBLANK) {
+		/* Update max exposure while meeting expected vblanking */
+		exposure_max = hi556->cur_mode->height + ctrl->val -
+			       HI556_EXPOSURE_MAX_MARGIN;
+		__v4l2_ctrl_modify_range(hi556->exposure,
+					 hi556->exposure->minimum,
+					 exposure_max, hi556->exposure->step,
+					 exposure_max);
+	}
+
+	/* V4L2 controls values will be applied only when power is already up */
+	if (!pm_runtime_get_if_in_use(&client->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_ANALOGUE_GAIN:
+		ret = hi556_write_reg(hi556, HI556_REG_ANALOG_GAIN,
+				      HI556_REG_VALUE_16BIT, ctrl->val);
+		break;
+
+	case V4L2_CID_DIGITAL_GAIN:
+		ret = hi556_update_digital_gain(hi556, ctrl->val);
+		break;
+
+	case V4L2_CID_EXPOSURE:
+		ret = hi556_write_reg(hi556, HI556_REG_EXPOSURE,
+				      HI556_REG_VALUE_16BIT, ctrl->val);
+		break;
+
+	case V4L2_CID_VBLANK:
+		/* Update FLL that meets expected vertical blanking */
+		ret = hi556_write_reg(hi556, HI556_REG_FLL,
+				      HI556_REG_VALUE_16BIT,
+				      hi556->cur_mode->height + ctrl->val);
+		break;
+
+	case V4L2_CID_TEST_PATTERN:
+		ret = hi556_test_pattern(hi556, ctrl->val);
+		break;
+
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	pm_runtime_put(&client->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops hi556_ctrl_ops = {
+	.s_ctrl = hi556_set_ctrl,
+};
+
+static int hi556_init_controls(struct hi556 *hi556)
+{
+	struct v4l2_ctrl_handler *ctrl_hdlr;
+	s64 exposure_max, h_blank;
+	int ret;
+
+	ctrl_hdlr = &hi556->ctrl_handler;
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 8);
+	if (ret)
+		return ret;
+
+	ctrl_hdlr->lock = &hi556->mutex;
+	hi556->link_freq = v4l2_ctrl_new_int_menu(ctrl_hdlr, &hi556_ctrl_ops,
+						  V4L2_CID_LINK_FREQ,
+					ARRAY_SIZE(link_freq_menu_items) - 1,
+					0, link_freq_menu_items);
+	if (hi556->link_freq)
+		hi556->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	hi556->pixel_rate = v4l2_ctrl_new_std
+			    (ctrl_hdlr, &hi556_ctrl_ops,
+			     V4L2_CID_PIXEL_RATE, 0,
+			     to_pixel_rate(HI556_LINK_FREQ_437MHZ_INDEX),
+			     1,
+			     to_pixel_rate(HI556_LINK_FREQ_437MHZ_INDEX));
+	hi556->vblank = v4l2_ctrl_new_std(ctrl_hdlr, &hi556_ctrl_ops,
+					  V4L2_CID_VBLANK,
+					  hi556->cur_mode->fll_min -
+					  hi556->cur_mode->height,
+					  HI556_FLL_MAX -
+					  hi556->cur_mode->height, 1,
+					  hi556->cur_mode->fll_def -
+					  hi556->cur_mode->height);
+
+	h_blank = hi556->cur_mode->llp - hi556->cur_mode->width;
+
+	hi556->hblank = v4l2_ctrl_new_std(ctrl_hdlr, &hi556_ctrl_ops,
+					  V4L2_CID_HBLANK, h_blank, h_blank, 1,
+					  h_blank);
+	if (hi556->hblank)
+		hi556->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &hi556_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
+			  HI556_ANAL_GAIN_MIN, HI556_ANAL_GAIN_MAX,
+			  HI556_ANAL_GAIN_STEP, HI556_ANAL_GAIN_MIN);
+	v4l2_ctrl_new_std(ctrl_hdlr, &hi556_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
+			  HI556_DGTL_GAIN_MIN, HI556_DGTL_GAIN_MAX,
+			  HI556_DGTL_GAIN_STEP, HI556_DGTL_GAIN_DEFAULT);
+	exposure_max = hi556->cur_mode->fll_def - HI556_EXPOSURE_MAX_MARGIN;
+	hi556->exposure = v4l2_ctrl_new_std(ctrl_hdlr, &hi556_ctrl_ops,
+					    V4L2_CID_EXPOSURE,
+					    HI556_EXPOSURE_MIN, exposure_max,
+					    HI556_EXPOSURE_STEP,
+					    exposure_max);
+	v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &hi556_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(hi556_test_pattern_menu) - 1,
+				     0, 0, hi556_test_pattern_menu);
+	if (ctrl_hdlr->error)
+		return ctrl_hdlr->error;
+
+	hi556->sd.ctrl_handler = ctrl_hdlr;
+
+	return 0;
+}
+
+static void hi556_assign_pad_format(const struct hi556_mode *mode,
+				    struct v4l2_mbus_framefmt *fmt)
+{
+	fmt->width = mode->width;
+	fmt->height = mode->height;
+	fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	fmt->field = V4L2_FIELD_NONE;
+}
+
+static int hi556_identify_module(struct hi556 *hi556)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&hi556->sd);
+	int ret;
+	u32 val;
+
+	if (hi556->identified)
+		return 0;
+
+	ret = hi556_read_reg(hi556, HI556_REG_CHIP_ID,
+			     HI556_REG_VALUE_16BIT, &val);
+	if (ret)
+		return ret;
+
+	if (val != HI556_CHIP_ID) {
+		dev_err(&client->dev, "chip id mismatch: %x!=%x",
+			HI556_CHIP_ID, val);
+		return -ENXIO;
+	}
+
+	hi556->identified = true;
+
+	return 0;
+}
+
+static const struct v4l2_rect *
+__hi556_get_pad_crop(struct hi556 *hi556,
+		     struct v4l2_subdev_state *sd_state,
+		     unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_crop(&hi556->sd, sd_state, pad);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return &hi556->cur_mode->crop;
+	}
+
+	return NULL;
+}
+
+static int hi556_get_selection(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_state *sd_state,
+			       struct v4l2_subdev_selection *sel)
+{
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP: {
+		struct hi556 *hi556 = to_hi556(sd);
+
+		mutex_lock(&hi556->mutex);
+		sel->r = *__hi556_get_pad_crop(hi556, sd_state, sel->pad,
+						sel->which);
+		mutex_unlock(&hi556->mutex);
+
+		return 0;
+	}
+
+	case V4L2_SEL_TGT_NATIVE_SIZE:
+		sel->r.top = 0;
+		sel->r.left = 0;
+		sel->r.width = HI556_NATIVE_WIDTH;
+		sel->r.height = HI556_NATIVE_HEIGHT;
+
+		return 0;
+
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.top = HI556_PIXEL_ARRAY_TOP;
+		sel->r.left = HI556_PIXEL_ARRAY_LEFT;
+		sel->r.width = HI556_PIXEL_ARRAY_WIDTH;
+		sel->r.height = HI556_PIXEL_ARRAY_HEIGHT;
+
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int hi556_start_streaming(struct hi556 *hi556)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&hi556->sd);
+	const struct hi556_reg_list *reg_list;
+	int link_freq_index, ret;
+
+	ret = hi556_identify_module(hi556);
+	if (ret)
+		return ret;
+
+	link_freq_index = hi556->cur_mode->link_freq_index;
+	reg_list = &link_freq_configs[link_freq_index].reg_list;
+	ret = hi556_write_reg_list(hi556, reg_list);
+	if (ret) {
+		dev_err(&client->dev, "failed to set plls");
+		return ret;
+	}
+
+	reg_list = &hi556->cur_mode->reg_list;
+	ret = hi556_write_reg_list(hi556, reg_list);
+	if (ret) {
+		dev_err(&client->dev, "failed to set mode");
+		return ret;
+	}
+
+	ret = __v4l2_ctrl_handler_setup(hi556->sd.ctrl_handler);
+	if (ret)
+		return ret;
+
+	ret = hi556_write_reg(hi556, HI556_REG_MODE_SELECT,
+			      HI556_REG_VALUE_16BIT, HI556_MODE_STREAMING);
+
+	if (ret) {
+		dev_err(&client->dev, "failed to set stream");
+		return ret;
+	}
+
+	return 0;
+}
+
+static void hi556_stop_streaming(struct hi556 *hi556)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&hi556->sd);
+
+	if (hi556_write_reg(hi556, HI556_REG_MODE_SELECT,
+			    HI556_REG_VALUE_16BIT, HI556_MODE_STANDBY))
+		dev_err(&client->dev, "failed to set stream");
+}
+
+static int hi556_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct hi556 *hi556 = to_hi556(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	if (hi556->streaming == enable)
+		return 0;
+
+	mutex_lock(&hi556->mutex);
+	if (enable) {
+		ret = pm_runtime_resume_and_get(&client->dev);
+		if (ret < 0) {
+			mutex_unlock(&hi556->mutex);
+			return ret;
+		}
+
+		ret = hi556_start_streaming(hi556);
+		if (ret) {
+			enable = 0;
+			hi556_stop_streaming(hi556);
+			pm_runtime_put(&client->dev);
+		}
+	} else {
+		hi556_stop_streaming(hi556);
+		pm_runtime_put(&client->dev);
+	}
+
+	hi556->streaming = enable;
+	mutex_unlock(&hi556->mutex);
+
+	return ret;
+}
+
+static int __maybe_unused hi556_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct hi556 *hi556 = to_hi556(sd);
+
+	mutex_lock(&hi556->mutex);
+	if (hi556->streaming)
+		hi556_stop_streaming(hi556);
+
+	mutex_unlock(&hi556->mutex);
+
+	return 0;
+}
+
+static int __maybe_unused hi556_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct hi556 *hi556 = to_hi556(sd);
+	int ret;
+
+	mutex_lock(&hi556->mutex);
+	if (hi556->streaming) {
+		ret = hi556_start_streaming(hi556);
+		if (ret)
+			goto error;
+	}
+
+	mutex_unlock(&hi556->mutex);
+
+	return 0;
+
+error:
+	hi556_stop_streaming(hi556);
+	hi556->streaming = 0;
+	mutex_unlock(&hi556->mutex);
+	return ret;
+}
+
+static int hi556_set_format(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_state *sd_state,
+			    struct v4l2_subdev_format *fmt)
+{
+	struct hi556 *hi556 = to_hi556(sd);
+	const struct hi556_mode *mode;
+	s32 vblank_def, h_blank;
+
+	mode = v4l2_find_nearest_size(supported_modes,
+				      ARRAY_SIZE(supported_modes), width,
+				      height, fmt->format.width,
+				      fmt->format.height);
+
+	mutex_lock(&hi556->mutex);
+	hi556_assign_pad_format(mode, &fmt->format);
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		*v4l2_subdev_get_try_format(sd, sd_state, fmt->pad) = fmt->format;
+	} else {
+		hi556->cur_mode = mode;
+		__v4l2_ctrl_s_ctrl(hi556->link_freq, mode->link_freq_index);
+		__v4l2_ctrl_s_ctrl_int64(hi556->pixel_rate,
+					 to_pixel_rate(mode->link_freq_index));
+
+		/* Update limits and set FPS to default */
+		vblank_def = mode->fll_def - mode->height;
+		__v4l2_ctrl_modify_range(hi556->vblank,
+					 mode->fll_min - mode->height,
+					 HI556_FLL_MAX - mode->height, 1,
+					 vblank_def);
+		__v4l2_ctrl_s_ctrl(hi556->vblank, vblank_def);
+
+		h_blank = hi556->cur_mode->llp - hi556->cur_mode->width;
+
+		__v4l2_ctrl_modify_range(hi556->hblank, h_blank, h_blank, 1,
+					 h_blank);
+	}
+
+	mutex_unlock(&hi556->mutex);
+
+	return 0;
+}
+
+static int hi556_get_format(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_state *sd_state,
+			    struct v4l2_subdev_format *fmt)
+{
+	struct hi556 *hi556 = to_hi556(sd);
+
+	mutex_lock(&hi556->mutex);
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
+		fmt->format = *v4l2_subdev_get_try_format(&hi556->sd,
+							  sd_state,
+							  fmt->pad);
+	else
+		hi556_assign_pad_format(hi556->cur_mode, &fmt->format);
+
+	mutex_unlock(&hi556->mutex);
+
+	return 0;
+}
+
+static int hi556_enum_mbus_code(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *sd_state,
+				struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index > 0)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+
+	return 0;
+}
+
+static int hi556_enum_frame_size(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->index >= ARRAY_SIZE(supported_modes))
+		return -EINVAL;
+
+	if (fse->code != MEDIA_BUS_FMT_SGRBG10_1X10)
+		return -EINVAL;
+
+	fse->min_width = supported_modes[fse->index].width;
+	fse->max_width = fse->min_width;
+	fse->min_height = supported_modes[fse->index].height;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static int hi556_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct hi556 *hi556 = to_hi556(sd);
+	struct v4l2_rect *try_crop;
+
+	mutex_lock(&hi556->mutex);
+	hi556_assign_pad_format(&supported_modes[0],
+				v4l2_subdev_get_try_format(sd, fh->state, 0));
+
+	/* Initialize try_crop rectangle. */
+	try_crop = v4l2_subdev_get_try_crop(sd, fh->state, 0);
+	try_crop->top = HI556_PIXEL_ARRAY_TOP;
+	try_crop->left = HI556_PIXEL_ARRAY_LEFT;
+	try_crop->width = HI556_PIXEL_ARRAY_WIDTH;
+	try_crop->height = HI556_PIXEL_ARRAY_HEIGHT;
+
+	mutex_unlock(&hi556->mutex);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops hi556_video_ops = {
+	.s_stream = hi556_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops hi556_pad_ops = {
+	.set_fmt = hi556_set_format,
+	.get_fmt = hi556_get_format,
+	.get_selection = hi556_get_selection,
+	.enum_mbus_code = hi556_enum_mbus_code,
+	.enum_frame_size = hi556_enum_frame_size,
+};
+
+static const struct v4l2_subdev_ops hi556_subdev_ops = {
+	.video = &hi556_video_ops,
+	.pad = &hi556_pad_ops,
+};
+
+static const struct media_entity_operations hi556_subdev_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static const struct v4l2_subdev_internal_ops hi556_internal_ops = {
+	.open = hi556_open,
+};
+
+static int hi556_check_hwcfg(struct device *dev)
+{
+	struct fwnode_handle *ep;
+	struct fwnode_handle *fwnode = dev_fwnode(dev);
+	struct v4l2_fwnode_endpoint bus_cfg = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY
+	};
+	u32 mclk;
+	int ret = 0;
+	unsigned int i, j;
+
+	if (!fwnode)
+		return -ENXIO;
+
+	ret = fwnode_property_read_u32(fwnode, "clock-frequency", &mclk);
+	if (ret) {
+		dev_err(dev, "can't get clock frequency");
+		return ret;
+	}
+
+	if (mclk != HI556_MCLK) {
+		dev_err(dev, "external clock %d is not supported", mclk);
+		return -EINVAL;
+	}
+
+	ep = fwnode_graph_get_next_endpoint(fwnode, NULL);
+	if (!ep)
+		return -ENXIO;
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(ep, &bus_cfg);
+	fwnode_handle_put(ep);
+	if (ret)
+		return ret;
+
+	if (bus_cfg.bus.mipi_csi2.num_data_lanes != 2) {
+		dev_err(dev, "number of CSI2 data lanes %d is not supported",
+			bus_cfg.bus.mipi_csi2.num_data_lanes);
+		ret = -EINVAL;
+		goto check_hwcfg_error;
+	}
+
+	if (!bus_cfg.nr_of_link_frequencies) {
+		dev_err(dev, "no link frequencies defined");
+		ret = -EINVAL;
+		goto check_hwcfg_error;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(link_freq_menu_items); i++) {
+		for (j = 0; j < bus_cfg.nr_of_link_frequencies; j++) {
+			if (link_freq_menu_items[i] ==
+				bus_cfg.link_frequencies[j])
+				break;
+		}
+
+		if (j == bus_cfg.nr_of_link_frequencies) {
+			dev_err(dev, "no link frequency %lld supported",
+				link_freq_menu_items[i]);
+			ret = -EINVAL;
+			goto check_hwcfg_error;
+		}
+	}
+
+check_hwcfg_error:
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+
+	return ret;
+}
+
+static void hi556_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct hi556 *hi556 = to_hi556(sd);
+
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+	pm_runtime_disable(&client->dev);
+	mutex_destroy(&hi556->mutex);
+}
+
+static int hi556_probe(struct i2c_client *client)
+{
+	struct hi556 *hi556;
+	bool full_power;
+	int ret;
+
+	ret = hi556_check_hwcfg(&client->dev);
+	if (ret) {
+		dev_err(&client->dev, "failed to check HW configuration: %d",
+			ret);
+		return ret;
+	}
+
+	hi556 = devm_kzalloc(&client->dev, sizeof(*hi556), GFP_KERNEL);
+	if (!hi556)
+		return -ENOMEM;
+
+	v4l2_i2c_subdev_init(&hi556->sd, client, &hi556_subdev_ops);
+
+	full_power = acpi_dev_state_d0(&client->dev);
+	if (full_power) {
+		ret = hi556_identify_module(hi556);
+		if (ret) {
+			dev_err(&client->dev, "failed to find sensor: %d", ret);
+			return ret;
+		}
+	}
+
+	mutex_init(&hi556->mutex);
+	hi556->cur_mode = &supported_modes[0];
+	ret = hi556_init_controls(hi556);
+	if (ret) {
+		dev_err(&client->dev, "failed to init controls: %d", ret);
+		goto probe_error_v4l2_ctrl_handler_free;
+	}
+
+	hi556->sd.internal_ops = &hi556_internal_ops;
+	hi556->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	hi556->sd.entity.ops = &hi556_subdev_entity_ops;
+	hi556->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	hi556->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&hi556->sd.entity, 1, &hi556->pad);
+	if (ret) {
+		dev_err(&client->dev, "failed to init entity pads: %d", ret);
+		goto probe_error_v4l2_ctrl_handler_free;
+	}
+
+	ret = v4l2_async_register_subdev_sensor(&hi556->sd);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed to register V4L2 subdev: %d",
+			ret);
+		goto probe_error_media_entity_cleanup;
+	}
+
+	/* Set the device's state to active if it's in D0 state. */
+	if (full_power)
+		pm_runtime_set_active(&client->dev);
+	pm_runtime_enable(&client->dev);
+	pm_runtime_idle(&client->dev);
+
+	return 0;
+
+probe_error_media_entity_cleanup:
+	media_entity_cleanup(&hi556->sd.entity);
+
+probe_error_v4l2_ctrl_handler_free:
+	v4l2_ctrl_handler_free(hi556->sd.ctrl_handler);
+	mutex_destroy(&hi556->mutex);
+
+	return ret;
+}
+
+static const struct dev_pm_ops hi556_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(hi556_suspend, hi556_resume)
+};
+
+#ifdef CONFIG_ACPI
+static const struct acpi_device_id hi556_acpi_ids[] = {
+	{"INT3537"},
+	{}
+};
+
+MODULE_DEVICE_TABLE(acpi, hi556_acpi_ids);
+#endif
+
+static struct i2c_driver hi556_i2c_driver = {
+	.driver = {
+		.name = "hi556",
+		.pm = &hi556_pm_ops,
+		.acpi_match_table = ACPI_PTR(hi556_acpi_ids),
+	},
+	.probe = hi556_probe,
+	.remove = hi556_remove,
+	.flags = I2C_DRV_ACPI_WAIVE_D0_PROBE,
+};
+
+module_i2c_driver(hi556_i2c_driver);
+
+MODULE_AUTHOR("Shawn Tu");
+MODULE_DESCRIPTION("Hynix HI556 sensor driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/hi846.c b/drivers/media/i2c/hi846.c
new file mode 100644
index 000000000..fa0038749
--- /dev/null
+++ b/drivers/media/i2c/hi846.c
@@ -0,0 +1,2202 @@
+// SPDX-License-Identifier: GPL-2.0
+// Copyright (c) 2021 Purism SPC
+
+#include <asm/unaligned.h>
+#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/pm.h>
+#include <linux/property.h>
+#include <linux/regulator/consumer.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+
+#define HI846_MEDIA_BUS_FORMAT		MEDIA_BUS_FMT_SGBRG10_1X10
+#define HI846_RGB_DEPTH			10
+
+/* Frame length lines / vertical timings */
+#define HI846_REG_FLL			0x0006
+#define HI846_FLL_MAX			0xffff
+
+/* Horizontal timing */
+#define HI846_REG_LLP			0x0008
+#define HI846_LINE_LENGTH		3800
+
+#define HI846_REG_BINNING_MODE		0x000c
+
+#define HI846_REG_IMAGE_ORIENTATION	0x000e
+
+#define HI846_REG_UNKNOWN_0022		0x0022
+
+#define HI846_REG_Y_ADDR_START_VACT_H	0x0026
+#define HI846_REG_Y_ADDR_START_VACT_L	0x0027
+#define HI846_REG_UNKNOWN_0028		0x0028
+
+#define HI846_REG_Y_ADDR_END_VACT_H	0x002c
+#define HI846_REG_Y_ADDR_END_VACT_L	0x002d
+
+#define HI846_REG_Y_ODD_INC_FOBP	0x002e
+#define HI846_REG_Y_EVEN_INC_FOBP	0x002f
+
+#define HI846_REG_Y_ODD_INC_VACT	0x0032
+#define HI846_REG_Y_EVEN_INC_VACT	0x0033
+
+#define HI846_REG_GROUPED_PARA_HOLD	0x0046
+
+#define HI846_REG_TG_ENABLE		0x004c
+
+#define HI846_REG_UNKNOWN_005C		0x005c
+
+#define HI846_REG_UNKNOWN_006A		0x006a
+
+/*
+ * Long exposure time. Actually, exposure is a 20 bit value that
+ * includes the lower 4 bits of 0x0073 too. Only 16 bits are used
+ * right now
+ */
+#define HI846_REG_EXPOSURE		0x0074
+#define HI846_EXPOSURE_MIN		6
+#define HI846_EXPOSURE_MAX_MARGIN	2
+#define HI846_EXPOSURE_STEP		1
+
+/* Analog gain controls from sensor */
+#define HI846_REG_ANALOG_GAIN		0x0077
+#define HI846_ANAL_GAIN_MIN		0
+#define HI846_ANAL_GAIN_MAX		240
+#define HI846_ANAL_GAIN_STEP		8
+
+/* Digital gain controls from sensor */
+#define HI846_REG_MWB_GR_GAIN_H		0x0078
+#define HI846_REG_MWB_GR_GAIN_L		0x0079
+#define HI846_REG_MWB_GB_GAIN_H		0x007a
+#define HI846_REG_MWB_GB_GAIN_L		0x007b
+#define HI846_REG_MWB_R_GAIN_H		0x007c
+#define HI846_REG_MWB_R_GAIN_L		0x007d
+#define HI846_REG_MWB_B_GAIN_H		0x007e
+#define HI846_REG_MWB_B_GAIN_L		0x007f
+#define HI846_DGTL_GAIN_MIN		512
+#define HI846_DGTL_GAIN_MAX		8191
+#define HI846_DGTL_GAIN_STEP		1
+#define HI846_DGTL_GAIN_DEFAULT		512
+
+#define HI846_REG_X_ADDR_START_HACT_H	0x0120
+#define HI846_REG_X_ADDR_END_HACT_H	0x0122
+
+#define HI846_REG_UNKNOWN_012A		0x012a
+
+#define HI846_REG_UNKNOWN_0200		0x0200
+
+#define HI846_REG_UNKNOWN_021C		0x021c
+#define HI846_REG_UNKNOWN_021E		0x021e
+
+#define HI846_REG_UNKNOWN_0402		0x0402
+#define HI846_REG_UNKNOWN_0404		0x0404
+#define HI846_REG_UNKNOWN_0408		0x0408
+#define HI846_REG_UNKNOWN_0410		0x0410
+#define HI846_REG_UNKNOWN_0412		0x0412
+#define HI846_REG_UNKNOWN_0414		0x0414
+
+#define HI846_REG_UNKNOWN_0418		0x0418
+
+#define HI846_REG_UNKNOWN_051E		0x051e
+
+/* Formatter */
+#define HI846_REG_X_START_H		0x0804
+#define HI846_REG_X_START_L		0x0805
+
+/* MIPI */
+#define HI846_REG_UNKNOWN_0900		0x0900
+#define HI846_REG_MIPI_TX_OP_EN		0x0901
+#define HI846_REG_MIPI_TX_OP_MODE	0x0902
+#define HI846_RAW8			BIT(5)
+
+#define HI846_REG_UNKNOWN_090C		0x090c
+#define HI846_REG_UNKNOWN_090E		0x090e
+
+#define HI846_REG_UNKNOWN_0914		0x0914
+#define HI846_REG_TLPX			0x0915
+#define HI846_REG_TCLK_PREPARE		0x0916
+#define HI846_REG_TCLK_ZERO		0x0917
+#define HI846_REG_UNKNOWN_0918		0x0918
+#define HI846_REG_THS_PREPARE		0x0919
+#define HI846_REG_THS_ZERO		0x091a
+#define HI846_REG_THS_TRAIL		0x091b
+#define HI846_REG_TCLK_POST		0x091c
+#define HI846_REG_TCLK_TRAIL_MIN	0x091d
+#define HI846_REG_UNKNOWN_091E		0x091e
+
+#define HI846_REG_UNKNOWN_0954		0x0954
+#define HI846_REG_UNKNOWN_0956		0x0956
+#define HI846_REG_UNKNOWN_0958		0x0958
+#define HI846_REG_UNKNOWN_095A		0x095a
+
+/* ISP Common */
+#define HI846_REG_MODE_SELECT		0x0a00
+#define HI846_MODE_STANDBY		0x00
+#define HI846_MODE_STREAMING		0x01
+#define HI846_REG_FAST_STANDBY_MODE	0x0a02
+#define HI846_REG_ISP_EN_H		0x0a04
+
+/* Test Pattern Control */
+#define HI846_REG_ISP			0x0a05
+#define HI846_REG_ISP_TPG_EN		0x01
+#define HI846_REG_TEST_PATTERN		0x020a /* 1-9 */
+
+#define HI846_REG_UNKNOWN_0A0C		0x0a0c
+
+/* Windowing */
+#define HI846_REG_X_OUTPUT_SIZE_H	0x0a12
+#define HI846_REG_X_OUTPUT_SIZE_L	0x0a13
+#define HI846_REG_Y_OUTPUT_SIZE_H	0x0a14
+#define HI846_REG_Y_OUTPUT_SIZE_L	0x0a15
+
+/* ISP Common */
+#define HI846_REG_PEDESTAL_EN		0x0a1a
+
+#define HI846_REG_UNKNOWN_0A1E		0x0a1e
+
+/* Horizontal Binning Mode */
+#define HI846_REG_HBIN_MODE		0x0a22
+
+#define HI846_REG_UNKNOWN_0A24		0x0a24
+#define HI846_REG_UNKNOWN_0B02		0x0b02
+#define HI846_REG_UNKNOWN_0B10		0x0b10
+#define HI846_REG_UNKNOWN_0B12		0x0b12
+#define HI846_REG_UNKNOWN_0B14		0x0b14
+
+/* BLC (Black Level Calibration) */
+#define HI846_REG_BLC_CTL0		0x0c00
+
+#define HI846_REG_UNKNOWN_0C06		0x0c06
+#define HI846_REG_UNKNOWN_0C10		0x0c10
+#define HI846_REG_UNKNOWN_0C12		0x0c12
+#define HI846_REG_UNKNOWN_0C14		0x0c14
+#define HI846_REG_UNKNOWN_0C16		0x0c16
+
+#define HI846_REG_UNKNOWN_0E04		0x0e04
+
+#define HI846_REG_CHIP_ID_L		0x0f16
+#define HI846_REG_CHIP_ID_H		0x0f17
+#define HI846_CHIP_ID_L			0x46
+#define HI846_CHIP_ID_H			0x08
+
+#define HI846_REG_UNKNOWN_0F04		0x0f04
+#define HI846_REG_UNKNOWN_0F08		0x0f08
+
+/* PLL */
+#define HI846_REG_PLL_CFG_MIPI2_H	0x0f2a
+#define HI846_REG_PLL_CFG_MIPI2_L	0x0f2b
+
+#define HI846_REG_UNKNOWN_0F30		0x0f30
+#define HI846_REG_PLL_CFG_RAMP1_H	0x0f32
+#define HI846_REG_UNKNOWN_0F36		0x0f36
+#define HI846_REG_PLL_CFG_MIPI1_H	0x0f38
+
+#define HI846_REG_UNKNOWN_2008		0x2008
+#define HI846_REG_UNKNOWN_326E		0x326e
+
+struct hi846_reg {
+	u16 address;
+	u16 val;
+};
+
+struct hi846_reg_list {
+	u32 num_of_regs;
+	const struct hi846_reg *regs;
+};
+
+struct hi846_mode {
+	/* Frame width in pixels */
+	u32 width;
+
+	/* Frame height in pixels */
+	u32 height;
+
+	/* Horizontal timing size */
+	u32 llp;
+
+	/* Link frequency needed for this resolution */
+	u8 link_freq_index;
+
+	u16 fps;
+
+	/* Vertical timining size */
+	u16 frame_len;
+
+	const struct hi846_reg_list reg_list_config;
+	const struct hi846_reg_list reg_list_2lane;
+	const struct hi846_reg_list reg_list_4lane;
+
+	/* Position inside of the 3264x2448 pixel array */
+	struct v4l2_rect crop;
+};
+
+static const struct hi846_reg hi846_init_2lane[] = {
+	{HI846_REG_MODE_SELECT,		0x0000},
+	/* regs below are unknown */
+	{0x2000, 0x100a},
+	{0x2002, 0x00ff},
+	{0x2004, 0x0007},
+	{0x2006, 0x3fff},
+	{0x2008, 0x3fff},
+	{0x200a, 0xc216},
+	{0x200c, 0x1292},
+	{0x200e, 0xc01a},
+	{0x2010, 0x403d},
+	{0x2012, 0x000e},
+	{0x2014, 0x403e},
+	{0x2016, 0x0b80},
+	{0x2018, 0x403f},
+	{0x201a, 0x82ae},
+	{0x201c, 0x1292},
+	{0x201e, 0xc00c},
+	{0x2020, 0x4130},
+	{0x2022, 0x43e2},
+	{0x2024, 0x0180},
+	{0x2026, 0x4130},
+	{0x2028, 0x7400},
+	{0x202a, 0x5000},
+	{0x202c, 0x0253},
+	{0x202e, 0x0ad1},
+	{0x2030, 0x2360},
+	{0x2032, 0x0009},
+	{0x2034, 0x5020},
+	{0x2036, 0x000b},
+	{0x2038, 0x0002},
+	{0x203a, 0x0044},
+	{0x203c, 0x0016},
+	{0x203e, 0x1792},
+	{0x2040, 0x7002},
+	{0x2042, 0x154f},
+	{0x2044, 0x00d5},
+	{0x2046, 0x000b},
+	{0x2048, 0x0019},
+	{0x204a, 0x1698},
+	{0x204c, 0x000e},
+	{0x204e, 0x099a},
+	{0x2050, 0x0058},
+	{0x2052, 0x7000},
+	{0x2054, 0x1799},
+	{0x2056, 0x0310},
+	{0x2058, 0x03c3},
+	{0x205a, 0x004c},
+	{0x205c, 0x064a},
+	{0x205e, 0x0001},
+	{0x2060, 0x0007},
+	{0x2062, 0x0bc7},
+	{0x2064, 0x0055},
+	{0x2066, 0x7000},
+	{0x2068, 0x1550},
+	{0x206a, 0x158a},
+	{0x206c, 0x0004},
+	{0x206e, 0x1488},
+	{0x2070, 0x7010},
+	{0x2072, 0x1508},
+	{0x2074, 0x0004},
+	{0x2076, 0x0016},
+	{0x2078, 0x03d5},
+	{0x207a, 0x0055},
+	{0x207c, 0x08ca},
+	{0x207e, 0x2019},
+	{0x2080, 0x0007},
+	{0x2082, 0x7057},
+	{0x2084, 0x0fc7},
+	{0x2086, 0x5041},
+	{0x2088, 0x12c8},
+	{0x208a, 0x5060},
+	{0x208c, 0x5080},
+	{0x208e, 0x2084},
+	{0x2090, 0x12c8},
+	{0x2092, 0x7800},
+	{0x2094, 0x0802},
+	{0x2096, 0x040f},
+	{0x2098, 0x1007},
+	{0x209a, 0x0803},
+	{0x209c, 0x080b},
+	{0x209e, 0x3803},
+	{0x20a0, 0x0807},
+	{0x20a2, 0x0404},
+	{0x20a4, 0x0400},
+	{0x20a6, 0xffff},
+	{0x20a8, 0xf0b2},
+	{0x20aa, 0xffef},
+	{0x20ac, 0x0a84},
+	{0x20ae, 0x1292},
+	{0x20b0, 0xc02e},
+	{0x20b2, 0x4130},
+	{0x23fe, 0xc056},
+	{0x3232, 0xfc0c},
+	{0x3236, 0xfc22},
+	{0x3248, 0xfca8},
+	{0x326a, 0x8302},
+	{0x326c, 0x830a},
+	{0x326e, 0x0000},
+	{0x32ca, 0xfc28},
+	{0x32cc, 0xc3bc},
+	{0x32ce, 0xc34c},
+	{0x32d0, 0xc35a},
+	{0x32d2, 0xc368},
+	{0x32d4, 0xc376},
+	{0x32d6, 0xc3c2},
+	{0x32d8, 0xc3e6},
+	{0x32da, 0x0003},
+	{0x32dc, 0x0003},
+	{0x32de, 0x00c7},
+	{0x32e0, 0x0031},
+	{0x32e2, 0x0031},
+	{0x32e4, 0x0031},
+	{0x32e6, 0xfc28},
+	{0x32e8, 0xc3bc},
+	{0x32ea, 0xc384},
+	{0x32ec, 0xc392},
+	{0x32ee, 0xc3a0},
+	{0x32f0, 0xc3ae},
+	{0x32f2, 0xc3c4},
+	{0x32f4, 0xc3e6},
+	{0x32f6, 0x0003},
+	{0x32f8, 0x0003},
+	{0x32fa, 0x00c7},
+	{0x32fc, 0x0031},
+	{0x32fe, 0x0031},
+	{0x3300, 0x0031},
+	{0x3302, 0x82ca},
+	{0x3304, 0xc164},
+	{0x3306, 0x82e6},
+	{0x3308, 0xc19c},
+	{0x330a, 0x001f},
+	{0x330c, 0x001a},
+	{0x330e, 0x0034},
+	{0x3310, 0x0000},
+	{0x3312, 0x0000},
+	{0x3314, 0xfc94},
+	{0x3316, 0xc3d8},
+	/* regs above are unknown */
+	{HI846_REG_MODE_SELECT,			0x0000},
+	{HI846_REG_UNKNOWN_0E04,		0x0012},
+	{HI846_REG_Y_ODD_INC_FOBP,		0x1111},
+	{HI846_REG_Y_ODD_INC_VACT,		0x1111},
+	{HI846_REG_UNKNOWN_0022,		0x0008},
+	{HI846_REG_Y_ADDR_START_VACT_H,		0x0040},
+	{HI846_REG_UNKNOWN_0028,		0x0017},
+	{HI846_REG_Y_ADDR_END_VACT_H,		0x09cf},
+	{HI846_REG_UNKNOWN_005C,		0x2101},
+	{HI846_REG_FLL,				0x09de},
+	{HI846_REG_LLP,				0x0ed8},
+	{HI846_REG_IMAGE_ORIENTATION,		0x0100},
+	{HI846_REG_BINNING_MODE,		0x0022},
+	{HI846_REG_HBIN_MODE,			0x0000},
+	{HI846_REG_UNKNOWN_0A24,		0x0000},
+	{HI846_REG_X_START_H,			0x0000},
+	{HI846_REG_X_OUTPUT_SIZE_H,		0x0cc0},
+	{HI846_REG_Y_OUTPUT_SIZE_H,		0x0990},
+	{HI846_REG_EXPOSURE,			0x09d8},
+	{HI846_REG_ANALOG_GAIN,			0x0000},
+	{HI846_REG_GROUPED_PARA_HOLD,		0x0000},
+	{HI846_REG_UNKNOWN_051E,		0x0000},
+	{HI846_REG_UNKNOWN_0200,		0x0400},
+	{HI846_REG_PEDESTAL_EN,			0x0c00},
+	{HI846_REG_UNKNOWN_0A0C,		0x0010},
+	{HI846_REG_UNKNOWN_0A1E,		0x0ccf},
+	{HI846_REG_UNKNOWN_0402,		0x0110},
+	{HI846_REG_UNKNOWN_0404,		0x00f4},
+	{HI846_REG_UNKNOWN_0408,		0x0000},
+	{HI846_REG_UNKNOWN_0410,		0x008d},
+	{HI846_REG_UNKNOWN_0412,		0x011a},
+	{HI846_REG_UNKNOWN_0414,		0x864c},
+	{HI846_REG_UNKNOWN_021C,		0x0003},
+	{HI846_REG_UNKNOWN_021E,		0x0235},
+	{HI846_REG_BLC_CTL0,			0x9150},
+	{HI846_REG_UNKNOWN_0C06,		0x0021},
+	{HI846_REG_UNKNOWN_0C10,		0x0040},
+	{HI846_REG_UNKNOWN_0C12,		0x0040},
+	{HI846_REG_UNKNOWN_0C14,		0x0040},
+	{HI846_REG_UNKNOWN_0C16,		0x0040},
+	{HI846_REG_FAST_STANDBY_MODE,		0x0100},
+	{HI846_REG_ISP_EN_H,			0x014a},
+	{HI846_REG_UNKNOWN_0418,		0x0000},
+	{HI846_REG_UNKNOWN_012A,		0x03b4},
+	{HI846_REG_X_ADDR_START_HACT_H,		0x0046},
+	{HI846_REG_X_ADDR_END_HACT_H,		0x0376},
+	{HI846_REG_UNKNOWN_0B02,		0xe04d},
+	{HI846_REG_UNKNOWN_0B10,		0x6821},
+	{HI846_REG_UNKNOWN_0B12,		0x0120},
+	{HI846_REG_UNKNOWN_0B14,		0x0001},
+	{HI846_REG_UNKNOWN_2008,		0x38fd},
+	{HI846_REG_UNKNOWN_326E,		0x0000},
+	{HI846_REG_UNKNOWN_0900,		0x0320},
+	{HI846_REG_MIPI_TX_OP_MODE,		0xc31a},
+	{HI846_REG_UNKNOWN_0914,		0xc109},
+	{HI846_REG_TCLK_PREPARE,		0x061a},
+	{HI846_REG_UNKNOWN_0918,		0x0306},
+	{HI846_REG_THS_ZERO,			0x0b09},
+	{HI846_REG_TCLK_POST,			0x0c07},
+	{HI846_REG_UNKNOWN_091E,		0x0a00},
+	{HI846_REG_UNKNOWN_090C,		0x042a},
+	{HI846_REG_UNKNOWN_090E,		0x006b},
+	{HI846_REG_UNKNOWN_0954,		0x0089},
+	{HI846_REG_UNKNOWN_0956,		0x0000},
+	{HI846_REG_UNKNOWN_0958,		0xca00},
+	{HI846_REG_UNKNOWN_095A,		0x9240},
+	{HI846_REG_UNKNOWN_0F08,		0x2f04},
+	{HI846_REG_UNKNOWN_0F30,		0x001f},
+	{HI846_REG_UNKNOWN_0F36,		0x001f},
+	{HI846_REG_UNKNOWN_0F04,		0x3a00},
+	{HI846_REG_PLL_CFG_RAMP1_H,		0x025a},
+	{HI846_REG_PLL_CFG_MIPI1_H,		0x025a},
+	{HI846_REG_PLL_CFG_MIPI2_H,		0x0024},
+	{HI846_REG_UNKNOWN_006A,		0x0100},
+	{HI846_REG_TG_ENABLE,			0x0100},
+};
+
+static const struct hi846_reg hi846_init_4lane[] = {
+	{0x2000, 0x987a},
+	{0x2002, 0x00ff},
+	{0x2004, 0x0047},
+	{0x2006, 0x3fff},
+	{0x2008, 0x3fff},
+	{0x200a, 0xc216},
+	{0x200c, 0x1292},
+	{0x200e, 0xc01a},
+	{0x2010, 0x403d},
+	{0x2012, 0x000e},
+	{0x2014, 0x403e},
+	{0x2016, 0x0b80},
+	{0x2018, 0x403f},
+	{0x201a, 0x82ae},
+	{0x201c, 0x1292},
+	{0x201e, 0xc00c},
+	{0x2020, 0x4130},
+	{0x2022, 0x43e2},
+	{0x2024, 0x0180},
+	{0x2026, 0x4130},
+	{0x2028, 0x7400},
+	{0x202a, 0x5000},
+	{0x202c, 0x0253},
+	{0x202e, 0x0ad1},
+	{0x2030, 0x2360},
+	{0x2032, 0x0009},
+	{0x2034, 0x5020},
+	{0x2036, 0x000b},
+	{0x2038, 0x0002},
+	{0x203a, 0x0044},
+	{0x203c, 0x0016},
+	{0x203e, 0x1792},
+	{0x2040, 0x7002},
+	{0x2042, 0x154f},
+	{0x2044, 0x00d5},
+	{0x2046, 0x000b},
+	{0x2048, 0x0019},
+	{0x204a, 0x1698},
+	{0x204c, 0x000e},
+	{0x204e, 0x099a},
+	{0x2050, 0x0058},
+	{0x2052, 0x7000},
+	{0x2054, 0x1799},
+	{0x2056, 0x0310},
+	{0x2058, 0x03c3},
+	{0x205a, 0x004c},
+	{0x205c, 0x064a},
+	{0x205e, 0x0001},
+	{0x2060, 0x0007},
+	{0x2062, 0x0bc7},
+	{0x2064, 0x0055},
+	{0x2066, 0x7000},
+	{0x2068, 0x1550},
+	{0x206a, 0x158a},
+	{0x206c, 0x0004},
+	{0x206e, 0x1488},
+	{0x2070, 0x7010},
+	{0x2072, 0x1508},
+	{0x2074, 0x0004},
+	{0x2076, 0x0016},
+	{0x2078, 0x03d5},
+	{0x207a, 0x0055},
+	{0x207c, 0x08ca},
+	{0x207e, 0x2019},
+	{0x2080, 0x0007},
+	{0x2082, 0x7057},
+	{0x2084, 0x0fc7},
+	{0x2086, 0x5041},
+	{0x2088, 0x12c8},
+	{0x208a, 0x5060},
+	{0x208c, 0x5080},
+	{0x208e, 0x2084},
+	{0x2090, 0x12c8},
+	{0x2092, 0x7800},
+	{0x2094, 0x0802},
+	{0x2096, 0x040f},
+	{0x2098, 0x1007},
+	{0x209a, 0x0803},
+	{0x209c, 0x080b},
+	{0x209e, 0x3803},
+	{0x20a0, 0x0807},
+	{0x20a2, 0x0404},
+	{0x20a4, 0x0400},
+	{0x20a6, 0xffff},
+	{0x20a8, 0xf0b2},
+	{0x20aa, 0xffef},
+	{0x20ac, 0x0a84},
+	{0x20ae, 0x1292},
+	{0x20b0, 0xc02e},
+	{0x20b2, 0x4130},
+	{0x20b4, 0xf0b2},
+	{0x20b6, 0xffbf},
+	{0x20b8, 0x2004},
+	{0x20ba, 0x403f},
+	{0x20bc, 0x00c3},
+	{0x20be, 0x4fe2},
+	{0x20c0, 0x8318},
+	{0x20c2, 0x43cf},
+	{0x20c4, 0x0000},
+	{0x20c6, 0x9382},
+	{0x20c8, 0xc314},
+	{0x20ca, 0x2003},
+	{0x20cc, 0x12b0},
+	{0x20ce, 0xcab0},
+	{0x20d0, 0x4130},
+	{0x20d2, 0x12b0},
+	{0x20d4, 0xc90a},
+	{0x20d6, 0x4130},
+	{0x20d8, 0x42d2},
+	{0x20da, 0x8318},
+	{0x20dc, 0x00c3},
+	{0x20de, 0x9382},
+	{0x20e0, 0xc314},
+	{0x20e2, 0x2009},
+	{0x20e4, 0x120b},
+	{0x20e6, 0x120a},
+	{0x20e8, 0x1209},
+	{0x20ea, 0x1208},
+	{0x20ec, 0x1207},
+	{0x20ee, 0x1206},
+	{0x20f0, 0x4030},
+	{0x20f2, 0xc15e},
+	{0x20f4, 0x4130},
+	{0x20f6, 0x1292},
+	{0x20f8, 0xc008},
+	{0x20fa, 0x4130},
+	{0x20fc, 0x42d2},
+	{0x20fe, 0x82a1},
+	{0x2100, 0x00c2},
+	{0x2102, 0x1292},
+	{0x2104, 0xc040},
+	{0x2106, 0x4130},
+	{0x2108, 0x1292},
+	{0x210a, 0xc006},
+	{0x210c, 0x42a2},
+	{0x210e, 0x7324},
+	{0x2110, 0x9382},
+	{0x2112, 0xc314},
+	{0x2114, 0x2011},
+	{0x2116, 0x425f},
+	{0x2118, 0x82a1},
+	{0x211a, 0xf25f},
+	{0x211c, 0x00c1},
+	{0x211e, 0xf35f},
+	{0x2120, 0x2406},
+	{0x2122, 0x425f},
+	{0x2124, 0x00c0},
+	{0x2126, 0xf37f},
+	{0x2128, 0x522f},
+	{0x212a, 0x4f82},
+	{0x212c, 0x7324},
+	{0x212e, 0x425f},
+	{0x2130, 0x82d4},
+	{0x2132, 0xf35f},
+	{0x2134, 0x4fc2},
+	{0x2136, 0x01b3},
+	{0x2138, 0x93c2},
+	{0x213a, 0x829f},
+	{0x213c, 0x2421},
+	{0x213e, 0x403e},
+	{0x2140, 0xfffe},
+	{0x2142, 0x40b2},
+	{0x2144, 0xec78},
+	{0x2146, 0x831c},
+	{0x2148, 0x40b2},
+	{0x214a, 0xec78},
+	{0x214c, 0x831e},
+	{0x214e, 0x40b2},
+	{0x2150, 0xec78},
+	{0x2152, 0x8320},
+	{0x2154, 0xb3d2},
+	{0x2156, 0x008c},
+	{0x2158, 0x2405},
+	{0x215a, 0x4e0f},
+	{0x215c, 0x503f},
+	{0x215e, 0xffd8},
+	{0x2160, 0x4f82},
+	{0x2162, 0x831c},
+	{0x2164, 0x90f2},
+	{0x2166, 0x0003},
+	{0x2168, 0x008c},
+	{0x216a, 0x2401},
+	{0x216c, 0x4130},
+	{0x216e, 0x421f},
+	{0x2170, 0x831c},
+	{0x2172, 0x5e0f},
+	{0x2174, 0x4f82},
+	{0x2176, 0x831e},
+	{0x2178, 0x5e0f},
+	{0x217a, 0x4f82},
+	{0x217c, 0x8320},
+	{0x217e, 0x3ff6},
+	{0x2180, 0x432e},
+	{0x2182, 0x3fdf},
+	{0x2184, 0x421f},
+	{0x2186, 0x7100},
+	{0x2188, 0x4f0e},
+	{0x218a, 0x503e},
+	{0x218c, 0xffd8},
+	{0x218e, 0x4e82},
+	{0x2190, 0x7a04},
+	{0x2192, 0x421e},
+	{0x2194, 0x831c},
+	{0x2196, 0x5f0e},
+	{0x2198, 0x4e82},
+	{0x219a, 0x7a06},
+	{0x219c, 0x0b00},
+	{0x219e, 0x7304},
+	{0x21a0, 0x0050},
+	{0x21a2, 0x40b2},
+	{0x21a4, 0xd081},
+	{0x21a6, 0x0b88},
+	{0x21a8, 0x421e},
+	{0x21aa, 0x831e},
+	{0x21ac, 0x5f0e},
+	{0x21ae, 0x4e82},
+	{0x21b0, 0x7a0e},
+	{0x21b2, 0x521f},
+	{0x21b4, 0x8320},
+	{0x21b6, 0x4f82},
+	{0x21b8, 0x7a10},
+	{0x21ba, 0x0b00},
+	{0x21bc, 0x7304},
+	{0x21be, 0x007a},
+	{0x21c0, 0x40b2},
+	{0x21c2, 0x0081},
+	{0x21c4, 0x0b88},
+	{0x21c6, 0x4392},
+	{0x21c8, 0x7a0a},
+	{0x21ca, 0x0800},
+	{0x21cc, 0x7a0c},
+	{0x21ce, 0x0b00},
+	{0x21d0, 0x7304},
+	{0x21d2, 0x022b},
+	{0x21d4, 0x40b2},
+	{0x21d6, 0xd081},
+	{0x21d8, 0x0b88},
+	{0x21da, 0x0b00},
+	{0x21dc, 0x7304},
+	{0x21de, 0x0255},
+	{0x21e0, 0x40b2},
+	{0x21e2, 0x0081},
+	{0x21e4, 0x0b88},
+	{0x21e6, 0x4130},
+	{0x23fe, 0xc056},
+	{0x3232, 0xfc0c},
+	{0x3236, 0xfc22},
+	{0x3238, 0xfcfc},
+	{0x323a, 0xfd84},
+	{0x323c, 0xfd08},
+	{0x3246, 0xfcd8},
+	{0x3248, 0xfca8},
+	{0x324e, 0xfcb4},
+	{0x326a, 0x8302},
+	{0x326c, 0x830a},
+	{0x326e, 0x0000},
+	{0x32ca, 0xfc28},
+	{0x32cc, 0xc3bc},
+	{0x32ce, 0xc34c},
+	{0x32d0, 0xc35a},
+	{0x32d2, 0xc368},
+	{0x32d4, 0xc376},
+	{0x32d6, 0xc3c2},
+	{0x32d8, 0xc3e6},
+	{0x32da, 0x0003},
+	{0x32dc, 0x0003},
+	{0x32de, 0x00c7},
+	{0x32e0, 0x0031},
+	{0x32e2, 0x0031},
+	{0x32e4, 0x0031},
+	{0x32e6, 0xfc28},
+	{0x32e8, 0xc3bc},
+	{0x32ea, 0xc384},
+	{0x32ec, 0xc392},
+	{0x32ee, 0xc3a0},
+	{0x32f0, 0xc3ae},
+	{0x32f2, 0xc3c4},
+	{0x32f4, 0xc3e6},
+	{0x32f6, 0x0003},
+	{0x32f8, 0x0003},
+	{0x32fa, 0x00c7},
+	{0x32fc, 0x0031},
+	{0x32fe, 0x0031},
+	{0x3300, 0x0031},
+	{0x3302, 0x82ca},
+	{0x3304, 0xc164},
+	{0x3306, 0x82e6},
+	{0x3308, 0xc19c},
+	{0x330a, 0x001f},
+	{0x330c, 0x001a},
+	{0x330e, 0x0034},
+	{0x3310, 0x0000},
+	{0x3312, 0x0000},
+	{0x3314, 0xfc94},
+	{0x3316, 0xc3d8},
+
+	{0x0a00, 0x0000},
+	{0x0e04, 0x0012},
+	{0x002e, 0x1111},
+	{0x0032, 0x1111},
+	{0x0022, 0x0008},
+	{0x0026, 0x0040},
+	{0x0028, 0x0017},
+	{0x002c, 0x09cf},
+	{0x005c, 0x2101},
+	{0x0006, 0x09de},
+	{0x0008, 0x0ed8},
+	{0x000e, 0x0100},
+	{0x000c, 0x0022},
+	{0x0a22, 0x0000},
+	{0x0a24, 0x0000},
+	{0x0804, 0x0000},
+	{0x0a12, 0x0cc0},
+	{0x0a14, 0x0990},
+	{0x0074, 0x09d8},
+	{0x0076, 0x0000},
+	{0x051e, 0x0000},
+	{0x0200, 0x0400},
+	{0x0a1a, 0x0c00},
+	{0x0a0c, 0x0010},
+	{0x0a1e, 0x0ccf},
+	{0x0402, 0x0110},
+	{0x0404, 0x00f4},
+	{0x0408, 0x0000},
+	{0x0410, 0x008d},
+	{0x0412, 0x011a},
+	{0x0414, 0x864c},
+	/* for OTP */
+	{0x021c, 0x0003},
+	{0x021e, 0x0235},
+	/* for OTP */
+	{0x0c00, 0x9950},
+	{0x0c06, 0x0021},
+	{0x0c10, 0x0040},
+	{0x0c12, 0x0040},
+	{0x0c14, 0x0040},
+	{0x0c16, 0x0040},
+	{0x0a02, 0x0100},
+	{0x0a04, 0x015a},
+	{0x0418, 0x0000},
+	{0x0128, 0x0028},
+	{0x012a, 0xffff},
+	{0x0120, 0x0046},
+	{0x0122, 0x0376},
+	{0x012c, 0x0020},
+	{0x012e, 0xffff},
+	{0x0124, 0x0040},
+	{0x0126, 0x0378},
+	{0x0746, 0x0050},
+	{0x0748, 0x01d5},
+	{0x074a, 0x022b},
+	{0x074c, 0x03b0},
+	{0x0756, 0x043f},
+	{0x0758, 0x3f1d},
+	{0x0b02, 0xe04d},
+	{0x0b10, 0x6821},
+	{0x0b12, 0x0120},
+	{0x0b14, 0x0001},
+	{0x2008, 0x38fd},
+	{0x326e, 0x0000},
+	{0x0900, 0x0300},
+	{0x0902, 0xc319},
+	{0x0914, 0xc109},
+	{0x0916, 0x061a},
+	{0x0918, 0x0407},
+	{0x091a, 0x0a0b},
+	{0x091c, 0x0e08},
+	{0x091e, 0x0a00},
+	{0x090c, 0x0427},
+	{0x090e, 0x0059},
+	{0x0954, 0x0089},
+	{0x0956, 0x0000},
+	{0x0958, 0xca80},
+	{0x095a, 0x9240},
+	{0x0f08, 0x2f04},
+	{0x0f30, 0x001f},
+	{0x0f36, 0x001f},
+	{0x0f04, 0x3a00},
+	{0x0f32, 0x025a},
+	{0x0f38, 0x025a},
+	{0x0f2a, 0x4124},
+	{0x006a, 0x0100},
+	{0x004c, 0x0100},
+	{0x0044, 0x0001},
+};
+
+static const struct hi846_reg mode_640x480_config[] = {
+	{HI846_REG_MODE_SELECT,			0x0000},
+	{HI846_REG_Y_ODD_INC_FOBP,		0x7711},
+	{HI846_REG_Y_ODD_INC_VACT,		0x7711},
+	{HI846_REG_Y_ADDR_START_VACT_H,		0x0148},
+	{HI846_REG_Y_ADDR_END_VACT_H,		0x08c7},
+	{HI846_REG_UNKNOWN_005C,		0x4404},
+	{HI846_REG_FLL,				0x0277},
+	{HI846_REG_LLP,				0x0ed8},
+	{HI846_REG_BINNING_MODE,		0x0322},
+	{HI846_REG_HBIN_MODE,			0x0200},
+	{HI846_REG_UNKNOWN_0A24,		0x0000},
+	{HI846_REG_X_START_H,			0x0058},
+	{HI846_REG_X_OUTPUT_SIZE_H,		0x0280},
+	{HI846_REG_Y_OUTPUT_SIZE_H,		0x01e0},
+
+	/* For OTP */
+	{HI846_REG_UNKNOWN_021C,		0x0003},
+	{HI846_REG_UNKNOWN_021E,		0x0235},
+
+	{HI846_REG_ISP_EN_H,			0x016a},
+	{HI846_REG_UNKNOWN_0418,		0x0210},
+	{HI846_REG_UNKNOWN_0B02,		0xe04d},
+	{HI846_REG_UNKNOWN_0B10,		0x7021},
+	{HI846_REG_UNKNOWN_0B12,		0x0120},
+	{HI846_REG_UNKNOWN_0B14,		0x0001},
+	{HI846_REG_UNKNOWN_2008,		0x38fd},
+	{HI846_REG_UNKNOWN_326E,		0x0000},
+};
+
+static const struct hi846_reg mode_640x480_mipi_2lane[] = {
+	{HI846_REG_UNKNOWN_0900,		0x0300},
+	{HI846_REG_MIPI_TX_OP_MODE,		0x4319},
+	{HI846_REG_UNKNOWN_0914,		0xc105},
+	{HI846_REG_TCLK_PREPARE,		0x030c},
+	{HI846_REG_UNKNOWN_0918,		0x0304},
+	{HI846_REG_THS_ZERO,			0x0708},
+	{HI846_REG_TCLK_POST,			0x0b04},
+	{HI846_REG_UNKNOWN_091E,		0x0500},
+	{HI846_REG_UNKNOWN_090C,		0x0208},
+	{HI846_REG_UNKNOWN_090E,		0x009a},
+	{HI846_REG_UNKNOWN_0954,		0x0089},
+	{HI846_REG_UNKNOWN_0956,		0x0000},
+	{HI846_REG_UNKNOWN_0958,		0xca80},
+	{HI846_REG_UNKNOWN_095A,		0x9240},
+	{HI846_REG_PLL_CFG_MIPI2_H,		0x4924},
+	{HI846_REG_TG_ENABLE,			0x0100},
+};
+
+static const struct hi846_reg mode_1280x720_config[] = {
+	{HI846_REG_MODE_SELECT,			0x0000},
+	{HI846_REG_Y_ODD_INC_FOBP,		0x3311},
+	{HI846_REG_Y_ODD_INC_VACT,		0x3311},
+	{HI846_REG_Y_ADDR_START_VACT_H,		0x0238},
+	{HI846_REG_Y_ADDR_END_VACT_H,		0x07d7},
+	{HI846_REG_UNKNOWN_005C,		0x4202},
+	{HI846_REG_FLL,				0x034a},
+	{HI846_REG_LLP,				0x0ed8},
+	{HI846_REG_BINNING_MODE,		0x0122},
+	{HI846_REG_HBIN_MODE,			0x0100},
+	{HI846_REG_UNKNOWN_0A24,		0x0000},
+	{HI846_REG_X_START_H,			0x00b0},
+	{HI846_REG_X_OUTPUT_SIZE_H,		0x0500},
+	{HI846_REG_Y_OUTPUT_SIZE_H,		0x02d0},
+	{HI846_REG_EXPOSURE,			0x0344},
+
+	/* For OTP */
+	{HI846_REG_UNKNOWN_021C,		0x0003},
+	{HI846_REG_UNKNOWN_021E,		0x0235},
+
+	{HI846_REG_ISP_EN_H,			0x016a},
+	{HI846_REG_UNKNOWN_0418,		0x0410},
+	{HI846_REG_UNKNOWN_0B02,		0xe04d},
+	{HI846_REG_UNKNOWN_0B10,		0x6c21},
+	{HI846_REG_UNKNOWN_0B12,		0x0120},
+	{HI846_REG_UNKNOWN_0B14,		0x0005},
+	{HI846_REG_UNKNOWN_2008,		0x38fd},
+	{HI846_REG_UNKNOWN_326E,		0x0000},
+};
+
+static const struct hi846_reg mode_1280x720_mipi_2lane[] = {
+	{HI846_REG_UNKNOWN_0900,		0x0300},
+	{HI846_REG_MIPI_TX_OP_MODE,		0x4319},
+	{HI846_REG_UNKNOWN_0914,		0xc109},
+	{HI846_REG_TCLK_PREPARE,		0x061a},
+	{HI846_REG_UNKNOWN_0918,		0x0407},
+	{HI846_REG_THS_ZERO,			0x0a0b},
+	{HI846_REG_TCLK_POST,			0x0e08},
+	{HI846_REG_UNKNOWN_091E,		0x0a00},
+	{HI846_REG_UNKNOWN_090C,		0x0427},
+	{HI846_REG_UNKNOWN_090E,		0x0145},
+	{HI846_REG_UNKNOWN_0954,		0x0089},
+	{HI846_REG_UNKNOWN_0956,		0x0000},
+	{HI846_REG_UNKNOWN_0958,		0xca80},
+	{HI846_REG_UNKNOWN_095A,		0x9240},
+	{HI846_REG_PLL_CFG_MIPI2_H,		0x4124},
+	{HI846_REG_TG_ENABLE,			0x0100},
+};
+
+static const struct hi846_reg mode_1280x720_mipi_4lane[] = {
+	/* 360Mbps */
+	{HI846_REG_UNKNOWN_0900,		0x0300},
+	{HI846_REG_MIPI_TX_OP_MODE,		0xc319},
+	{HI846_REG_UNKNOWN_0914,		0xc105},
+	{HI846_REG_TCLK_PREPARE,		0x030c},
+	{HI846_REG_UNKNOWN_0918,		0x0304},
+	{HI846_REG_THS_ZERO,			0x0708},
+	{HI846_REG_TCLK_POST,			0x0b04},
+	{HI846_REG_UNKNOWN_091E,		0x0500},
+	{HI846_REG_UNKNOWN_090C,		0x0208},
+	{HI846_REG_UNKNOWN_090E,		0x008a},
+	{HI846_REG_UNKNOWN_0954,		0x0089},
+	{HI846_REG_UNKNOWN_0956,		0x0000},
+	{HI846_REG_UNKNOWN_0958,		0xca80},
+	{HI846_REG_UNKNOWN_095A,		0x9240},
+	{HI846_REG_PLL_CFG_MIPI2_H,		0x4924},
+	{HI846_REG_TG_ENABLE,			0x0100},
+};
+
+static const struct hi846_reg mode_1632x1224_config[] = {
+	{HI846_REG_MODE_SELECT,			0x0000},
+	{HI846_REG_Y_ODD_INC_FOBP,		0x3311},
+	{HI846_REG_Y_ODD_INC_VACT,		0x3311},
+	{HI846_REG_Y_ADDR_START_VACT_H,		0x0040},
+	{HI846_REG_Y_ADDR_END_VACT_H,		0x09cf},
+	{HI846_REG_UNKNOWN_005C,		0x4202},
+	{HI846_REG_FLL,				0x09de},
+	{HI846_REG_LLP,				0x0ed8},
+	{HI846_REG_BINNING_MODE,		0x0122},
+	{HI846_REG_HBIN_MODE,			0x0100},
+	{HI846_REG_UNKNOWN_0A24,		0x0000},
+	{HI846_REG_X_START_H,			0x0000},
+	{HI846_REG_X_OUTPUT_SIZE_H,		0x0660},
+	{HI846_REG_Y_OUTPUT_SIZE_H,		0x04c8},
+	{HI846_REG_EXPOSURE,			0x09d8},
+
+	/* For OTP */
+	{HI846_REG_UNKNOWN_021C,		0x0003},
+	{HI846_REG_UNKNOWN_021E,		0x0235},
+
+	{HI846_REG_ISP_EN_H,			0x016a},
+	{HI846_REG_UNKNOWN_0418,		0x0000},
+	{HI846_REG_UNKNOWN_0B02,		0xe04d},
+	{HI846_REG_UNKNOWN_0B10,		0x6c21},
+	{HI846_REG_UNKNOWN_0B12,		0x0120},
+	{HI846_REG_UNKNOWN_0B14,		0x0005},
+	{HI846_REG_UNKNOWN_2008,		0x38fd},
+	{HI846_REG_UNKNOWN_326E,		0x0000},
+};
+
+static const struct hi846_reg mode_1632x1224_mipi_2lane[] = {
+	{HI846_REG_UNKNOWN_0900,		0x0300},
+	{HI846_REG_MIPI_TX_OP_MODE,		0x4319},
+	{HI846_REG_UNKNOWN_0914,		0xc109},
+	{HI846_REG_TCLK_PREPARE,		0x061a},
+	{HI846_REG_UNKNOWN_0918,		0x0407},
+	{HI846_REG_THS_ZERO,			0x0a0b},
+	{HI846_REG_TCLK_POST,			0x0e08},
+	{HI846_REG_UNKNOWN_091E,		0x0a00},
+	{HI846_REG_UNKNOWN_090C,		0x0427},
+	{HI846_REG_UNKNOWN_090E,		0x0069},
+	{HI846_REG_UNKNOWN_0954,		0x0089},
+	{HI846_REG_UNKNOWN_0956,		0x0000},
+	{HI846_REG_UNKNOWN_0958,		0xca80},
+	{HI846_REG_UNKNOWN_095A,		0x9240},
+	{HI846_REG_PLL_CFG_MIPI2_H,		0x4124},
+	{HI846_REG_TG_ENABLE,			0x0100},
+};
+
+static const struct hi846_reg mode_1632x1224_mipi_4lane[] = {
+	{HI846_REG_UNKNOWN_0900,		0x0300},
+	{HI846_REG_MIPI_TX_OP_MODE,		0xc319},
+	{HI846_REG_UNKNOWN_0914,		0xc105},
+	{HI846_REG_TCLK_PREPARE,		0x030c},
+	{HI846_REG_UNKNOWN_0918,		0x0304},
+	{HI846_REG_THS_ZERO,			0x0708},
+	{HI846_REG_TCLK_POST,			0x0b04},
+	{HI846_REG_UNKNOWN_091E,		0x0500},
+	{HI846_REG_UNKNOWN_090C,		0x0208},
+	{HI846_REG_UNKNOWN_090E,		0x001c},
+	{HI846_REG_UNKNOWN_0954,		0x0089},
+	{HI846_REG_UNKNOWN_0956,		0x0000},
+	{HI846_REG_UNKNOWN_0958,		0xca80},
+	{HI846_REG_UNKNOWN_095A,		0x9240},
+	{HI846_REG_PLL_CFG_MIPI2_H,		0x4924},
+	{HI846_REG_TG_ENABLE,			0x0100},
+};
+
+static const char * const hi846_test_pattern_menu[] = {
+	"Disabled",
+	"Solid Colour",
+	"100% Colour Bars",
+	"Fade To Grey Colour Bars",
+	"PN9",
+	"Gradient Horizontal",
+	"Gradient Vertical",
+	"Check Board",
+	"Slant Pattern",
+	"Resolution Pattern",
+};
+
+#define FREQ_INDEX_640	0
+#define FREQ_INDEX_1280	1
+static const s64 hi846_link_freqs[] = {
+	[FREQ_INDEX_640] = 80000000,
+	[FREQ_INDEX_1280] = 200000000,
+};
+
+static const struct hi846_reg_list hi846_init_regs_list_2lane = {
+	.num_of_regs = ARRAY_SIZE(hi846_init_2lane),
+	.regs = hi846_init_2lane,
+};
+
+static const struct hi846_reg_list hi846_init_regs_list_4lane = {
+	.num_of_regs = ARRAY_SIZE(hi846_init_4lane),
+	.regs = hi846_init_4lane,
+};
+
+static const struct hi846_mode supported_modes[] = {
+	{
+		.width = 640,
+		.height = 480,
+		.link_freq_index = FREQ_INDEX_640,
+		.fps = 120,
+		.frame_len = 631,
+		.llp = HI846_LINE_LENGTH,
+		.reg_list_config = {
+			.num_of_regs = ARRAY_SIZE(mode_640x480_config),
+			.regs = mode_640x480_config,
+		},
+		.reg_list_2lane = {
+			.num_of_regs = ARRAY_SIZE(mode_640x480_mipi_2lane),
+			.regs = mode_640x480_mipi_2lane,
+		},
+		.reg_list_4lane = {
+			.num_of_regs = 0,
+		},
+		.crop = {
+			.left = 0x58,
+			.top = 0x148,
+			.width = 640 * 4,
+			.height = 480 * 4,
+		},
+	},
+	{
+		.width = 1280,
+		.height = 720,
+		.link_freq_index = FREQ_INDEX_1280,
+		.fps = 90,
+		.frame_len = 842,
+		.llp = HI846_LINE_LENGTH,
+		.reg_list_config = {
+			.num_of_regs = ARRAY_SIZE(mode_1280x720_config),
+			.regs = mode_1280x720_config,
+		},
+		.reg_list_2lane = {
+			.num_of_regs = ARRAY_SIZE(mode_1280x720_mipi_2lane),
+			.regs = mode_1280x720_mipi_2lane,
+		},
+		.reg_list_4lane = {
+			.num_of_regs = ARRAY_SIZE(mode_1280x720_mipi_4lane),
+			.regs = mode_1280x720_mipi_4lane,
+		},
+		.crop = {
+			.left = 0xb0,
+			.top = 0x238,
+			.width = 1280 * 2,
+			.height = 720 * 2,
+		},
+	},
+	{
+		.width = 1632,
+		.height = 1224,
+		.link_freq_index = FREQ_INDEX_1280,
+		.fps = 30,
+		.frame_len = 2526,
+		.llp = HI846_LINE_LENGTH,
+		.reg_list_config = {
+			.num_of_regs = ARRAY_SIZE(mode_1632x1224_config),
+			.regs = mode_1632x1224_config,
+		},
+		.reg_list_2lane = {
+			.num_of_regs = ARRAY_SIZE(mode_1632x1224_mipi_2lane),
+			.regs = mode_1632x1224_mipi_2lane,
+		},
+		.reg_list_4lane = {
+			.num_of_regs = ARRAY_SIZE(mode_1632x1224_mipi_4lane),
+			.regs = mode_1632x1224_mipi_4lane,
+		},
+		.crop = {
+			.left = 0x0,
+			.top = 0x0,
+			.width = 1632 * 2,
+			.height = 1224 * 2,
+		},
+	}
+};
+
+struct hi846_datafmt {
+	u32 code;
+	enum v4l2_colorspace colorspace;
+};
+
+static const char * const hi846_supply_names[] = {
+	"vddio", /* Digital I/O (1.8V or 2.8V) */
+	"vdda", /* Analog (2.8V) */
+	"vddd", /* Digital Core (1.2V) */
+};
+
+#define HI846_NUM_SUPPLIES ARRAY_SIZE(hi846_supply_names)
+
+struct hi846 {
+	struct gpio_desc *rst_gpio;
+	struct gpio_desc *shutdown_gpio;
+	struct regulator_bulk_data supplies[HI846_NUM_SUPPLIES];
+	struct clk *clock;
+	const struct hi846_datafmt *fmt;
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct v4l2_ctrl_handler ctrl_handler;
+	u8 nr_lanes;
+
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *pixel_rate;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *exposure;
+
+	struct mutex mutex; /* protect cur_mode, streaming and chip access */
+	const struct hi846_mode *cur_mode;
+	bool streaming;
+};
+
+static inline struct hi846 *to_hi846(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct hi846, sd);
+}
+
+static const struct hi846_datafmt hi846_colour_fmts[] = {
+	{ HI846_MEDIA_BUS_FORMAT, V4L2_COLORSPACE_RAW },
+};
+
+static const struct hi846_datafmt *hi846_find_datafmt(u32 code)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(hi846_colour_fmts); i++)
+		if (hi846_colour_fmts[i].code == code)
+			return &hi846_colour_fmts[i];
+
+	return NULL;
+}
+
+static inline u8 hi846_get_link_freq_index(struct hi846 *hi846)
+{
+	return hi846->cur_mode->link_freq_index;
+}
+
+static u64 hi846_get_link_freq(struct hi846 *hi846)
+{
+	u8 index = hi846_get_link_freq_index(hi846);
+
+	return hi846_link_freqs[index];
+}
+
+static u64 hi846_calc_pixel_rate(struct hi846 *hi846)
+{
+	u64 link_freq = hi846_get_link_freq(hi846);
+	u64 pixel_rate = link_freq * 2 * hi846->nr_lanes;
+
+	do_div(pixel_rate, HI846_RGB_DEPTH);
+
+	return pixel_rate;
+}
+
+static int hi846_read_reg(struct hi846 *hi846, u16 reg, u8 *val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&hi846->sd);
+	struct i2c_msg msgs[2];
+	u8 addr_buf[2];
+	u8 data_buf[1] = {0};
+	int ret;
+
+	put_unaligned_be16(reg, addr_buf);
+	msgs[0].addr = client->addr;
+	msgs[0].flags = 0;
+	msgs[0].len = sizeof(addr_buf);
+	msgs[0].buf = addr_buf;
+	msgs[1].addr = client->addr;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].len = 1;
+	msgs[1].buf = data_buf;
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret != ARRAY_SIZE(msgs)) {
+		dev_err(&client->dev, "i2c read error: %d\n", ret);
+		return -EIO;
+	}
+
+	*val = data_buf[0];
+
+	return 0;
+}
+
+static int hi846_write_reg(struct hi846 *hi846, u16 reg, u8 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&hi846->sd);
+	u8 buf[3] = { reg >> 8, reg & 0xff, val };
+	struct i2c_msg msg[] = {
+		{ .addr = client->addr, .flags = 0,
+		  .len = ARRAY_SIZE(buf), .buf = buf },
+	};
+	int ret;
+
+	ret = i2c_transfer(client->adapter, msg, ARRAY_SIZE(msg));
+	if (ret != ARRAY_SIZE(msg)) {
+		dev_err(&client->dev, "i2c write error\n");
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static void hi846_write_reg_16(struct hi846 *hi846, u16 reg, u16 val, int *err)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&hi846->sd);
+	u8 buf[4];
+	int ret;
+
+	if (*err < 0)
+		return;
+
+	put_unaligned_be16(reg, buf);
+	put_unaligned_be16(val, buf + 2);
+	ret = i2c_master_send(client, buf, sizeof(buf));
+	if (ret != sizeof(buf)) {
+		dev_err(&client->dev, "i2c_master_send != %zu: %d\n",
+			sizeof(buf), ret);
+		*err = -EIO;
+	}
+}
+
+static int hi846_write_reg_list(struct hi846 *hi846,
+				const struct hi846_reg_list *r_list)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&hi846->sd);
+	unsigned int i;
+	int ret = 0;
+
+	for (i = 0; i < r_list->num_of_regs; i++) {
+		hi846_write_reg_16(hi846, r_list->regs[i].address,
+				   r_list->regs[i].val, &ret);
+		if (ret) {
+			dev_err_ratelimited(&client->dev,
+					    "failed to write reg 0x%4.4x: %d",
+					    r_list->regs[i].address, ret);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int hi846_update_digital_gain(struct hi846 *hi846, u16 d_gain)
+{
+	int ret = 0;
+
+	hi846_write_reg_16(hi846, HI846_REG_MWB_GR_GAIN_H, d_gain, &ret);
+	hi846_write_reg_16(hi846, HI846_REG_MWB_GB_GAIN_H, d_gain, &ret);
+	hi846_write_reg_16(hi846, HI846_REG_MWB_R_GAIN_H, d_gain, &ret);
+	hi846_write_reg_16(hi846, HI846_REG_MWB_B_GAIN_H, d_gain, &ret);
+
+	return ret;
+}
+
+static int hi846_test_pattern(struct hi846 *hi846, u32 pattern)
+{
+	int ret;
+	u8 val;
+
+	if (pattern) {
+		ret = hi846_read_reg(hi846, HI846_REG_ISP, &val);
+		if (ret)
+			return ret;
+
+		ret = hi846_write_reg(hi846, HI846_REG_ISP,
+				      val | HI846_REG_ISP_TPG_EN);
+		if (ret)
+			return ret;
+	}
+
+	return hi846_write_reg(hi846, HI846_REG_TEST_PATTERN, pattern);
+}
+
+static int hi846_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct hi846 *hi846 = container_of(ctrl->handler,
+					     struct hi846, ctrl_handler);
+	struct i2c_client *client = v4l2_get_subdevdata(&hi846->sd);
+	s64 exposure_max;
+	int ret = 0;
+	u32 shutter, frame_len;
+
+	/* Propagate change of current control to all related controls */
+	if (ctrl->id == V4L2_CID_VBLANK) {
+		/* Update max exposure while meeting expected vblanking */
+		exposure_max = hi846->cur_mode->height + ctrl->val -
+			       HI846_EXPOSURE_MAX_MARGIN;
+		__v4l2_ctrl_modify_range(hi846->exposure,
+					 hi846->exposure->minimum,
+					 exposure_max, hi846->exposure->step,
+					 exposure_max);
+	}
+
+	ret = pm_runtime_get_if_in_use(&client->dev);
+	if (!ret || ret == -EAGAIN)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_ANALOGUE_GAIN:
+		ret = hi846_write_reg(hi846, HI846_REG_ANALOG_GAIN, ctrl->val);
+		break;
+
+	case V4L2_CID_DIGITAL_GAIN:
+		ret = hi846_update_digital_gain(hi846, ctrl->val);
+		break;
+
+	case V4L2_CID_EXPOSURE:
+		shutter = ctrl->val;
+		frame_len = hi846->cur_mode->frame_len;
+
+		if (shutter > frame_len - 6) { /* margin */
+			frame_len = shutter + 6;
+			if (frame_len > 0xffff) { /* max frame len */
+				frame_len = 0xffff;
+			}
+		}
+
+		if (shutter < 6)
+			shutter = 6;
+		if (shutter > (0xffff - 6))
+			shutter = 0xffff - 6;
+
+		hi846_write_reg_16(hi846, HI846_REG_FLL, frame_len, &ret);
+		hi846_write_reg_16(hi846, HI846_REG_EXPOSURE, shutter, &ret);
+		break;
+
+	case V4L2_CID_VBLANK:
+		/* Update FLL that meets expected vertical blanking */
+		hi846_write_reg_16(hi846, HI846_REG_FLL,
+				   hi846->cur_mode->height + ctrl->val, &ret);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		ret = hi846_test_pattern(hi846, ctrl->val);
+		break;
+
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	pm_runtime_put(&client->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops hi846_ctrl_ops = {
+	.s_ctrl = hi846_set_ctrl,
+};
+
+static int hi846_init_controls(struct hi846 *hi846)
+{
+	struct v4l2_ctrl_handler *ctrl_hdlr;
+	s64 exposure_max, h_blank;
+	int ret;
+	struct i2c_client *client = v4l2_get_subdevdata(&hi846->sd);
+	struct v4l2_fwnode_device_properties props;
+
+	ctrl_hdlr = &hi846->ctrl_handler;
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 10);
+	if (ret)
+		return ret;
+
+	ctrl_hdlr->lock = &hi846->mutex;
+
+	hi846->link_freq =
+		v4l2_ctrl_new_int_menu(ctrl_hdlr, &hi846_ctrl_ops,
+				       V4L2_CID_LINK_FREQ,
+				       ARRAY_SIZE(hi846_link_freqs) - 1,
+				       0, hi846_link_freqs);
+	if (hi846->link_freq)
+		hi846->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	hi846->pixel_rate =
+		v4l2_ctrl_new_std(ctrl_hdlr, &hi846_ctrl_ops,
+				  V4L2_CID_PIXEL_RATE, 0,
+				  hi846_calc_pixel_rate(hi846), 1,
+				  hi846_calc_pixel_rate(hi846));
+	hi846->vblank = v4l2_ctrl_new_std(ctrl_hdlr, &hi846_ctrl_ops,
+					  V4L2_CID_VBLANK,
+					  hi846->cur_mode->frame_len -
+					  hi846->cur_mode->height,
+					  HI846_FLL_MAX -
+					  hi846->cur_mode->height, 1,
+					  hi846->cur_mode->frame_len -
+					  hi846->cur_mode->height);
+
+	h_blank = hi846->cur_mode->llp - hi846->cur_mode->width;
+
+	hi846->hblank = v4l2_ctrl_new_std(ctrl_hdlr, &hi846_ctrl_ops,
+					  V4L2_CID_HBLANK, h_blank, h_blank, 1,
+					  h_blank);
+	if (hi846->hblank)
+		hi846->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &hi846_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
+			  HI846_ANAL_GAIN_MIN, HI846_ANAL_GAIN_MAX,
+			  HI846_ANAL_GAIN_STEP, HI846_ANAL_GAIN_MIN);
+	v4l2_ctrl_new_std(ctrl_hdlr, &hi846_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
+			  HI846_DGTL_GAIN_MIN, HI846_DGTL_GAIN_MAX,
+			  HI846_DGTL_GAIN_STEP, HI846_DGTL_GAIN_DEFAULT);
+	exposure_max = hi846->cur_mode->frame_len - HI846_EXPOSURE_MAX_MARGIN;
+	hi846->exposure = v4l2_ctrl_new_std(ctrl_hdlr, &hi846_ctrl_ops,
+					    V4L2_CID_EXPOSURE,
+					    HI846_EXPOSURE_MIN, exposure_max,
+					    HI846_EXPOSURE_STEP,
+					    exposure_max);
+	v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &hi846_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(hi846_test_pattern_menu) - 1,
+				     0, 0, hi846_test_pattern_menu);
+	if (ctrl_hdlr->error) {
+		dev_err(&client->dev, "v4l ctrl handler error: %d\n",
+			ctrl_hdlr->error);
+		ret = ctrl_hdlr->error;
+		goto error;
+	}
+
+	ret = v4l2_fwnode_device_parse(&client->dev, &props);
+	if (ret)
+		goto error;
+
+	ret = v4l2_ctrl_new_fwnode_properties(ctrl_hdlr, &hi846_ctrl_ops,
+					      &props);
+	if (ret)
+		goto error;
+
+	hi846->sd.ctrl_handler = ctrl_hdlr;
+
+	return 0;
+
+error:
+	v4l2_ctrl_handler_free(ctrl_hdlr);
+	return ret;
+}
+
+static int hi846_set_video_mode(struct hi846 *hi846, int fps)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&hi846->sd);
+	u64 frame_length;
+	int ret = 0;
+	int dummy_lines;
+	u64 link_freq = hi846_get_link_freq(hi846);
+
+	dev_dbg(&client->dev, "%s: link freq: %llu\n", __func__,
+		hi846_get_link_freq(hi846));
+
+	do_div(link_freq, fps);
+	frame_length = link_freq;
+	do_div(frame_length, HI846_LINE_LENGTH);
+
+	dummy_lines = (frame_length > hi846->cur_mode->frame_len) ?
+			(frame_length - hi846->cur_mode->frame_len) : 0;
+
+	frame_length = hi846->cur_mode->frame_len + dummy_lines;
+
+	dev_dbg(&client->dev, "%s: frame length calculated: %llu\n", __func__,
+		frame_length);
+
+	hi846_write_reg_16(hi846, HI846_REG_FLL, frame_length & 0xFFFF, &ret);
+	hi846_write_reg_16(hi846, HI846_REG_LLP,
+			   HI846_LINE_LENGTH & 0xFFFF, &ret);
+
+	return ret;
+}
+
+static int hi846_start_streaming(struct hi846 *hi846)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&hi846->sd);
+	int ret = 0;
+	u8 val;
+
+	if (hi846->nr_lanes == 2)
+		ret = hi846_write_reg_list(hi846, &hi846_init_regs_list_2lane);
+	else
+		ret = hi846_write_reg_list(hi846, &hi846_init_regs_list_4lane);
+	if (ret) {
+		dev_err(&client->dev, "failed to set plls: %d\n", ret);
+		return ret;
+	}
+
+	ret = hi846_write_reg_list(hi846, &hi846->cur_mode->reg_list_config);
+	if (ret) {
+		dev_err(&client->dev, "failed to set mode: %d\n", ret);
+		return ret;
+	}
+
+	if (hi846->nr_lanes == 2)
+		ret = hi846_write_reg_list(hi846,
+					   &hi846->cur_mode->reg_list_2lane);
+	else
+		ret = hi846_write_reg_list(hi846,
+					   &hi846->cur_mode->reg_list_4lane);
+	if (ret) {
+		dev_err(&client->dev, "failed to set mipi mode: %d\n", ret);
+		return ret;
+	}
+
+	hi846_set_video_mode(hi846, hi846->cur_mode->fps);
+
+	ret = __v4l2_ctrl_handler_setup(hi846->sd.ctrl_handler);
+	if (ret)
+		return ret;
+
+	/*
+	 * Reading 0x0034 is purely done for debugging reasons: It is not
+	 * documented in the DS but only mentioned once:
+	 * "If 0x0034[2] bit is disabled , Visible pixel width and height is 0."
+	 * So even though that sounds like we won't see anything, we don't
+	 * know more about this, so in that case only inform the user but do
+	 * nothing more.
+	 */
+	ret = hi846_read_reg(hi846, 0x0034, &val);
+	if (ret)
+		return ret;
+	if (!(val & BIT(2)))
+		dev_info(&client->dev, "visible pixel width and height is 0\n");
+
+	ret = hi846_write_reg(hi846, HI846_REG_MODE_SELECT,
+			      HI846_MODE_STREAMING);
+	if (ret) {
+		dev_err(&client->dev, "failed to start stream");
+		return ret;
+	}
+
+	hi846->streaming = 1;
+
+	dev_dbg(&client->dev, "%s: started streaming successfully\n", __func__);
+
+	return ret;
+}
+
+static void hi846_stop_streaming(struct hi846 *hi846)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&hi846->sd);
+
+	if (hi846_write_reg(hi846, HI846_REG_MODE_SELECT, HI846_MODE_STANDBY))
+		dev_err(&client->dev, "failed to stop stream");
+
+	hi846->streaming = 0;
+}
+
+static int hi846_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct hi846 *hi846 = to_hi846(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	if (hi846->streaming == enable)
+		return 0;
+
+	mutex_lock(&hi846->mutex);
+
+	if (enable) {
+		ret = pm_runtime_get_sync(&client->dev);
+		if (ret < 0) {
+			pm_runtime_put_noidle(&client->dev);
+			goto out;
+		}
+
+		ret = hi846_start_streaming(hi846);
+	}
+
+	if (!enable || ret) {
+		hi846_stop_streaming(hi846);
+		pm_runtime_put(&client->dev);
+	}
+
+out:
+	mutex_unlock(&hi846->mutex);
+
+	return ret;
+}
+
+static int hi846_power_on(struct hi846 *hi846)
+{
+	int ret;
+
+	ret = regulator_bulk_enable(HI846_NUM_SUPPLIES, hi846->supplies);
+	if (ret < 0)
+		return ret;
+
+	ret = clk_prepare_enable(hi846->clock);
+	if (ret < 0)
+		goto err_reg;
+
+	if (hi846->shutdown_gpio)
+		gpiod_set_value_cansleep(hi846->shutdown_gpio, 0);
+
+	/* 30us = 2400 cycles at 80Mhz */
+	usleep_range(30, 60);
+	if (hi846->rst_gpio)
+		gpiod_set_value_cansleep(hi846->rst_gpio, 0);
+	usleep_range(30, 60);
+
+	return 0;
+
+err_reg:
+	regulator_bulk_disable(HI846_NUM_SUPPLIES, hi846->supplies);
+
+	return ret;
+}
+
+static int hi846_power_off(struct hi846 *hi846)
+{
+	if (hi846->rst_gpio)
+		gpiod_set_value_cansleep(hi846->rst_gpio, 1);
+
+	if (hi846->shutdown_gpio)
+		gpiod_set_value_cansleep(hi846->shutdown_gpio, 1);
+
+	clk_disable_unprepare(hi846->clock);
+	return regulator_bulk_disable(HI846_NUM_SUPPLIES, hi846->supplies);
+}
+
+static int __maybe_unused hi846_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct hi846 *hi846 = to_hi846(sd);
+
+	if (hi846->streaming)
+		hi846_stop_streaming(hi846);
+
+	return hi846_power_off(hi846);
+}
+
+static int __maybe_unused hi846_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct hi846 *hi846 = to_hi846(sd);
+	int ret;
+
+	ret = hi846_power_on(hi846);
+	if (ret)
+		return ret;
+
+	if (hi846->streaming) {
+		ret = hi846_start_streaming(hi846);
+		if (ret) {
+			dev_err(dev, "%s: start streaming failed: %d\n",
+				__func__, ret);
+			goto error;
+		}
+	}
+
+	return 0;
+
+error:
+	hi846_power_off(hi846);
+	return ret;
+}
+
+static int hi846_set_format(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_state *sd_state,
+			    struct v4l2_subdev_format *format)
+{
+	struct hi846 *hi846 = to_hi846(sd);
+	struct v4l2_mbus_framefmt *mf = &format->format;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	const struct hi846_datafmt *fmt = hi846_find_datafmt(mf->code);
+	u32 tgt_fps;
+	s32 vblank_def, h_blank;
+
+	if (!fmt) {
+		mf->code = hi846_colour_fmts[0].code;
+		mf->colorspace = hi846_colour_fmts[0].colorspace;
+		fmt = &hi846_colour_fmts[0];
+	}
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		*v4l2_subdev_get_try_format(sd, sd_state, format->pad) = *mf;
+		return 0;
+	}
+
+	if (hi846->nr_lanes == 2) {
+		if (!hi846->cur_mode->reg_list_2lane.num_of_regs) {
+			dev_err(&client->dev,
+				"this mode is not supported for 2 lanes\n");
+			return -EINVAL;
+		}
+	} else {
+		if (!hi846->cur_mode->reg_list_4lane.num_of_regs) {
+			dev_err(&client->dev,
+				"this mode is not supported for 4 lanes\n");
+			return -EINVAL;
+		}
+	}
+
+	mutex_lock(&hi846->mutex);
+
+	if (hi846->streaming) {
+		mutex_unlock(&hi846->mutex);
+		return -EBUSY;
+	}
+
+	hi846->fmt = fmt;
+
+	hi846->cur_mode =
+		v4l2_find_nearest_size(supported_modes,
+				       ARRAY_SIZE(supported_modes),
+				       width, height, mf->width, mf->height);
+	dev_dbg(&client->dev, "%s: found mode: %dx%d\n", __func__,
+		hi846->cur_mode->width, hi846->cur_mode->height);
+
+	tgt_fps = hi846->cur_mode->fps;
+	dev_dbg(&client->dev, "%s: target fps: %d\n", __func__, tgt_fps);
+
+	mf->width = hi846->cur_mode->width;
+	mf->height = hi846->cur_mode->height;
+	mf->code = HI846_MEDIA_BUS_FORMAT;
+	mf->field = V4L2_FIELD_NONE;
+
+	__v4l2_ctrl_s_ctrl(hi846->link_freq, hi846_get_link_freq_index(hi846));
+	__v4l2_ctrl_s_ctrl_int64(hi846->pixel_rate,
+				 hi846_calc_pixel_rate(hi846));
+
+	/* Update limits and set FPS to default */
+	vblank_def = hi846->cur_mode->frame_len - hi846->cur_mode->height;
+	__v4l2_ctrl_modify_range(hi846->vblank,
+				 hi846->cur_mode->frame_len -
+					hi846->cur_mode->height,
+				 HI846_FLL_MAX - hi846->cur_mode->height, 1,
+				 vblank_def);
+	__v4l2_ctrl_s_ctrl(hi846->vblank, vblank_def);
+
+	h_blank = hi846->cur_mode->llp - hi846->cur_mode->width;
+
+	__v4l2_ctrl_modify_range(hi846->hblank, h_blank, h_blank, 1,
+				 h_blank);
+
+	dev_dbg(&client->dev, "Set fmt w=%d h=%d code=0x%x colorspace=0x%x\n",
+		mf->width, mf->height,
+		fmt->code, fmt->colorspace);
+
+	mutex_unlock(&hi846->mutex);
+
+	return 0;
+}
+
+static int hi846_get_format(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_state *sd_state,
+			    struct v4l2_subdev_format *format)
+{
+	struct hi846 *hi846 = to_hi846(sd);
+	struct v4l2_mbus_framefmt *mf = &format->format;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		format->format = *v4l2_subdev_get_try_format(&hi846->sd,
+							sd_state,
+							format->pad);
+		return 0;
+	}
+
+	mutex_lock(&hi846->mutex);
+	mf->code        = HI846_MEDIA_BUS_FORMAT;
+	mf->colorspace  = V4L2_COLORSPACE_RAW;
+	mf->field       = V4L2_FIELD_NONE;
+	mf->width       = hi846->cur_mode->width;
+	mf->height      = hi846->cur_mode->height;
+	mutex_unlock(&hi846->mutex);
+	dev_dbg(&client->dev,
+		"Get format w=%d h=%d code=0x%x colorspace=0x%x\n",
+		mf->width, mf->height, mf->code, mf->colorspace);
+
+	return 0;
+}
+
+static int hi846_enum_mbus_code(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *sd_state,
+				struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->pad || code->index > 0)
+		return -EINVAL;
+
+	code->code = HI846_MEDIA_BUS_FORMAT;
+
+	return 0;
+}
+
+static int hi846_enum_frame_size(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (fse->pad || fse->index >= ARRAY_SIZE(supported_modes))
+		return -EINVAL;
+
+	if (fse->code != HI846_MEDIA_BUS_FORMAT) {
+		dev_err(&client->dev, "frame size enum not matching\n");
+		return -EINVAL;
+	}
+
+	fse->min_width = supported_modes[fse->index].width;
+	fse->max_width = supported_modes[fse->index].width;
+	fse->min_height = supported_modes[fse->index].height;
+	fse->max_height = supported_modes[fse->index].height;
+
+	dev_dbg(&client->dev, "%s: max width: %d max height: %d\n", __func__,
+		fse->max_width, fse->max_height);
+
+	return 0;
+}
+
+static int hi846_get_selection(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_state *sd_state,
+			       struct v4l2_subdev_selection *sel)
+{
+	struct hi846 *hi846 = to_hi846(sd);
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		mutex_lock(&hi846->mutex);
+		switch (sel->which) {
+		case V4L2_SUBDEV_FORMAT_TRY:
+			v4l2_subdev_get_try_crop(sd, sd_state, sel->pad);
+			break;
+		case V4L2_SUBDEV_FORMAT_ACTIVE:
+			sel->r = hi846->cur_mode->crop;
+			break;
+		}
+		mutex_unlock(&hi846->mutex);
+		return 0;
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_NATIVE_SIZE:
+		sel->r.top = 0;
+		sel->r.left = 0;
+		sel->r.width = 3264;
+		sel->r.height = 2448;
+		return 0;
+	default:
+		return -EINVAL;
+	}
+}
+
+static int hi846_init_cfg(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state)
+{
+	struct hi846 *hi846 = to_hi846(sd);
+	struct v4l2_mbus_framefmt *mf;
+
+	mf = v4l2_subdev_get_try_format(sd, sd_state, 0);
+
+	mutex_lock(&hi846->mutex);
+	mf->code        = HI846_MEDIA_BUS_FORMAT;
+	mf->colorspace  = V4L2_COLORSPACE_RAW;
+	mf->field       = V4L2_FIELD_NONE;
+	mf->width       = hi846->cur_mode->width;
+	mf->height      = hi846->cur_mode->height;
+	mutex_unlock(&hi846->mutex);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops hi846_video_ops = {
+	.s_stream = hi846_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops hi846_pad_ops = {
+	.init_cfg = hi846_init_cfg,
+	.enum_frame_size = hi846_enum_frame_size,
+	.enum_mbus_code = hi846_enum_mbus_code,
+	.set_fmt = hi846_set_format,
+	.get_fmt = hi846_get_format,
+	.get_selection = hi846_get_selection,
+};
+
+static const struct v4l2_subdev_ops hi846_subdev_ops = {
+	.video = &hi846_video_ops,
+	.pad = &hi846_pad_ops,
+};
+
+static const struct media_entity_operations hi846_subdev_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static int hi846_identify_module(struct hi846 *hi846)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&hi846->sd);
+	int ret;
+	u8 hi, lo;
+
+	ret = hi846_read_reg(hi846, HI846_REG_CHIP_ID_L, &lo);
+	if (ret)
+		return ret;
+
+	if (lo != HI846_CHIP_ID_L) {
+		dev_err(&client->dev, "wrong chip id low byte: %x", lo);
+		return -ENXIO;
+	}
+
+	ret = hi846_read_reg(hi846, HI846_REG_CHIP_ID_H, &hi);
+	if (ret)
+		return ret;
+
+	if (hi != HI846_CHIP_ID_H) {
+		dev_err(&client->dev, "wrong chip id high byte: %x", hi);
+		return -ENXIO;
+	}
+
+	dev_info(&client->dev, "chip id %02X %02X using %d mipi lanes\n",
+		 hi, lo, hi846->nr_lanes);
+
+	return 0;
+}
+
+static s64 hi846_check_link_freqs(struct hi846 *hi846,
+				  struct v4l2_fwnode_endpoint *ep)
+{
+	const s64 *freqs = hi846_link_freqs;
+	int freqs_count = ARRAY_SIZE(hi846_link_freqs);
+	int i, j;
+
+	for (i = 0; i < freqs_count; i++) {
+		for (j = 0; j < ep->nr_of_link_frequencies; j++)
+			if (freqs[i] == ep->link_frequencies[j])
+				break;
+		if (j == ep->nr_of_link_frequencies)
+			return freqs[i];
+	}
+
+	return 0;
+}
+
+static int hi846_parse_dt(struct hi846 *hi846, struct device *dev)
+{
+	struct fwnode_handle *ep;
+	struct fwnode_handle *fwnode = dev_fwnode(dev);
+	struct v4l2_fwnode_endpoint bus_cfg = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY
+	};
+	int ret;
+	s64 fq;
+
+	ep = fwnode_graph_get_next_endpoint(fwnode, NULL);
+	if (!ep) {
+		dev_err(dev, "unable to find endpoint node\n");
+		return -ENXIO;
+	}
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(ep, &bus_cfg);
+	fwnode_handle_put(ep);
+	if (ret) {
+		dev_err(dev, "failed to parse endpoint node: %d\n", ret);
+		return ret;
+	}
+
+	if (bus_cfg.bus.mipi_csi2.num_data_lanes != 2 &&
+	    bus_cfg.bus.mipi_csi2.num_data_lanes != 4) {
+		dev_err(dev, "number of CSI2 data lanes %d is not supported",
+			bus_cfg.bus.mipi_csi2.num_data_lanes);
+		ret = -EINVAL;
+		goto check_hwcfg_error;
+	}
+
+	hi846->nr_lanes = bus_cfg.bus.mipi_csi2.num_data_lanes;
+
+	if (!bus_cfg.nr_of_link_frequencies) {
+		dev_err(dev, "link-frequency property not found in DT\n");
+		ret = -EINVAL;
+		goto check_hwcfg_error;
+	}
+
+	/* Check that link frequences for all the modes are in device tree */
+	fq = hi846_check_link_freqs(hi846, &bus_cfg);
+	if (fq) {
+		dev_err(dev, "Link frequency of %lld is not supported\n", fq);
+		ret = -EINVAL;
+		goto check_hwcfg_error;
+	}
+
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+
+	hi846->rst_gpio = devm_gpiod_get_optional(dev, "reset", GPIOD_OUT_LOW);
+	if (IS_ERR(hi846->rst_gpio)) {
+		dev_err(dev, "failed to get reset gpio: %pe\n",
+			hi846->rst_gpio);
+		return PTR_ERR(hi846->rst_gpio);
+	}
+
+	hi846->shutdown_gpio = devm_gpiod_get_optional(dev, "shutdown",
+						       GPIOD_OUT_LOW);
+	if (IS_ERR(hi846->shutdown_gpio)) {
+		dev_err(dev, "failed to get shutdown gpio: %pe\n",
+			hi846->shutdown_gpio);
+		return PTR_ERR(hi846->shutdown_gpio);
+	}
+
+	return 0;
+
+check_hwcfg_error:
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+	return ret;
+}
+
+static int hi846_probe(struct i2c_client *client)
+{
+	struct hi846 *hi846;
+	int ret;
+	int i;
+	u32 mclk_freq;
+
+	hi846 = devm_kzalloc(&client->dev, sizeof(*hi846), GFP_KERNEL);
+	if (!hi846)
+		return -ENOMEM;
+
+	ret = hi846_parse_dt(hi846, &client->dev);
+	if (ret) {
+		dev_err(&client->dev, "failed to check HW configuration: %d",
+			ret);
+		return ret;
+	}
+
+	hi846->clock = devm_clk_get(&client->dev, NULL);
+	if (IS_ERR(hi846->clock)) {
+		dev_err(&client->dev, "failed to get clock: %pe\n",
+			hi846->clock);
+		return PTR_ERR(hi846->clock);
+	}
+
+	mclk_freq = clk_get_rate(hi846->clock);
+	if (mclk_freq != 25000000)
+		dev_warn(&client->dev,
+			 "External clock freq should be 25000000, not %u.\n",
+			 mclk_freq);
+
+	for (i = 0; i < HI846_NUM_SUPPLIES; i++)
+		hi846->supplies[i].supply = hi846_supply_names[i];
+
+	ret = devm_regulator_bulk_get(&client->dev, HI846_NUM_SUPPLIES,
+				      hi846->supplies);
+	if (ret < 0)
+		return ret;
+
+	v4l2_i2c_subdev_init(&hi846->sd, client, &hi846_subdev_ops);
+
+	mutex_init(&hi846->mutex);
+
+	ret = hi846_power_on(hi846);
+	if (ret)
+		goto err_mutex;
+
+	ret = hi846_identify_module(hi846);
+	if (ret)
+		goto err_power_off;
+
+	hi846->cur_mode = &supported_modes[0];
+
+	ret = hi846_init_controls(hi846);
+	if (ret) {
+		dev_err(&client->dev, "failed to init controls: %d", ret);
+		goto err_power_off;
+	}
+
+	hi846->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	hi846->sd.entity.ops = &hi846_subdev_entity_ops;
+	hi846->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	hi846->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&hi846->sd.entity, 1, &hi846->pad);
+	if (ret) {
+		dev_err(&client->dev, "failed to init entity pads: %d", ret);
+		goto err_v4l2_ctrl_handler_free;
+	}
+
+	ret = v4l2_async_register_subdev_sensor(&hi846->sd);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed to register V4L2 subdev: %d",
+			ret);
+		goto err_media_entity_cleanup;
+	}
+
+	pm_runtime_set_active(&client->dev);
+	pm_runtime_enable(&client->dev);
+	pm_runtime_idle(&client->dev);
+
+	return 0;
+
+err_media_entity_cleanup:
+	media_entity_cleanup(&hi846->sd.entity);
+
+err_v4l2_ctrl_handler_free:
+	v4l2_ctrl_handler_free(hi846->sd.ctrl_handler);
+
+err_power_off:
+	hi846_power_off(hi846);
+
+err_mutex:
+	mutex_destroy(&hi846->mutex);
+
+	return ret;
+}
+
+static void hi846_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct hi846 *hi846 = to_hi846(sd);
+
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+
+	pm_runtime_disable(&client->dev);
+	if (!pm_runtime_status_suspended(&client->dev))
+		hi846_suspend(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+
+	mutex_destroy(&hi846->mutex);
+}
+
+static const struct dev_pm_ops hi846_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend,
+				pm_runtime_force_resume)
+	SET_RUNTIME_PM_OPS(hi846_suspend, hi846_resume, NULL)
+};
+
+static const struct of_device_id hi846_of_match[] = {
+	{ .compatible = "hynix,hi846", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, hi846_of_match);
+
+static struct i2c_driver hi846_i2c_driver = {
+	.driver = {
+		.name = "hi846",
+		.pm = &hi846_pm_ops,
+		.of_match_table = hi846_of_match,
+	},
+	.probe = hi846_probe,
+	.remove = hi846_remove,
+};
+
+module_i2c_driver(hi846_i2c_driver);
+
+MODULE_AUTHOR("Angus Ainslie <angus@akkea.ca>");
+MODULE_AUTHOR("Martin Kepplinger <martin.kepplinger@puri.sm>");
+MODULE_DESCRIPTION("Hynix HI846 sensor driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/hi847.c b/drivers/media/i2c/hi847.c
new file mode 100644
index 000000000..32547d7a2
--- /dev/null
+++ b/drivers/media/i2c/hi847.c
@@ -0,0 +1,3010 @@
+// SPDX-License-Identifier: GPL-2.0
+// Copyright (c) 2022 Intel Corporation.
+
+#include <asm/unaligned.h>
+#include <linux/acpi.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+
+#define HI847_REG_VALUE_08BIT		1
+#define HI847_REG_VALUE_16BIT		2
+#define HI847_REG_VALUE_24BIT		3
+
+#define HI847_LINK_FREQ_400MHZ		400000000ULL
+#define HI847_LINK_FREQ_200MHZ		200000000ULL
+#define HI847_SCLK			72000000ULL
+#define HI847_MCLK			19200000
+#define HI847_DATA_LANES		4
+#define HI847_RGB_DEPTH			10
+
+#define HI847_REG_CHIP_ID		0x0716
+#define HI847_CHIP_ID			0x0847
+
+#define HI847_REG_MODE_SELECT		0x0B00
+#define HI847_MODE_STANDBY		0x0000
+#define HI847_MODE_STREAMING		0x0100
+
+#define HI847_REG_MODE_TG		0x027E
+#define HI847_REG_MODE_TG_ENABLE	0x0100
+#define HI847_REG_MODE_TG_DISABLE	0x0000
+
+/* vertical-timings from sensor */
+#define HI847_REG_FLL			0x020E
+#define HI847_FLL_30FPS			0x0B51
+#define HI847_FLL_30FPS_MIN		0x0B51
+#define HI847_FLL_60FPS			0x05A9
+#define HI847_FLL_60FPS_MIN		0x05A9
+#define HI847_FLL_MAX			0x7fff
+
+/* horizontal-timings from sensor */
+#define HI847_REG_LLP			0x0206
+
+/* Exposure controls from sensor */
+#define HI847_REG_EXPOSURE		0x020A
+#define HI847_EXPOSURE_MIN		4
+#define HI847_EXPOSURE_MAX_MARGIN	4
+#define HI847_EXPOSURE_STEP		1
+
+/* Analog gain controls from sensor */
+#define HI847_REG_ANALOG_GAIN		0x0212
+#define HI847_ANAL_GAIN_MIN		0
+#define HI847_ANAL_GAIN_MAX		240
+#define HI847_ANAL_GAIN_STEP		1
+
+/* Digital gain controls from sensor */
+#define HI847_REG_MWB_GR_GAIN		0x0214
+#define HI847_REG_MWB_GB_GAIN		0x0216
+#define HI847_REG_MWB_R_GAIN		0x0218
+#define HI847_REG_MWB_B_GAIN		0x021A
+#define HI847_DGTL_GAIN_MIN		1
+#define HI847_DGTL_GAIN_MAX		8191
+#define HI847_DGTL_GAIN_STEP		1
+#define HI847_DGTL_GAIN_DEFAULT		512
+
+/* Test Pattern Control */
+#define HI847_REG_ISP			0X0B04
+#define HI847_REG_ISP_TPG_EN		0x0001
+#define HI847_REG_TEST_PATTERN		0x0C0A
+
+/* Flip Mirror Controls from sensor */
+#define HI847_REG_MIRROR_FLIP	0x0202
+
+#define HI847_REG_FORMAT_X	0x0F04
+#define HI847_REG_FORMAT_Y	0x0F06
+
+enum {
+	HI847_LINK_FREQ_400MHZ_INDEX,
+	HI847_LINK_FREQ_200MHZ_INDEX,
+};
+
+struct hi847_reg {
+	u16 address;
+	u16 val;
+};
+
+struct hi847_reg_list {
+	u32 num_of_regs;
+	const struct hi847_reg *regs;
+};
+
+struct hi847_link_freq_config {
+	const struct hi847_reg_list reg_list;
+};
+
+struct hi847_mode {
+	/* Frame width in pixels */
+	u32 width;
+
+	/* Frame height in pixels */
+	u32 height;
+
+	/* Horizontal timining size */
+	u32 llp;
+
+	/* Default vertical timining size */
+	u32 fll_def;
+
+	/* Min vertical timining size */
+	u32 fll_min;
+
+	/* Link frequency needed for this resolution */
+	u32 link_freq_index;
+
+	/* Sensor register settings for this resolution */
+	const struct hi847_reg_list reg_list;
+};
+
+#define to_hi847(_sd) container_of(_sd, struct hi847, sd)
+
+//SENSOR_INITIALIZATION
+static const struct hi847_reg mipi_data_rate_lane_4[] = {
+	{0x0790, 0x0100},
+	{0x2000, 0x0000},
+	{0x2002, 0x0058},
+	{0x2006, 0x40B2},
+	{0x2008, 0xB05C},
+	{0x200A, 0x8446},
+	{0x200C, 0x40B2},
+	{0x200E, 0xB082},
+	{0x2010, 0x8450},
+	{0x2012, 0x40B2},
+	{0x2014, 0xB0AE},
+	{0x2016, 0x84C6},
+	{0x2018, 0x40B2},
+	{0x201A, 0xB11A},
+	{0x201C, 0x84BC},
+	{0x201E, 0x40B2},
+	{0x2020, 0xB34A},
+	{0x2022, 0x84B4},
+	{0x2024, 0x40B2},
+	{0x2026, 0xB386},
+	{0x2028, 0x84B0},
+	{0x202A, 0x40B2},
+	{0x202C, 0xB3B4},
+	{0x202E, 0x84B8},
+	{0x2030, 0x40B2},
+	{0x2032, 0xB0F4},
+	{0x2034, 0x8470},
+	{0x2036, 0x40B2},
+	{0x2038, 0xB3EA},
+	{0x203A, 0x847C},
+	{0x203C, 0x40B2},
+	{0x203E, 0xB658},
+	{0x2040, 0x8478},
+	{0x2042, 0x40B2},
+	{0x2044, 0xB67E},
+	{0x2046, 0x847E},
+	{0x2048, 0x40B2},
+	{0x204A, 0xB78E},
+	{0x204C, 0x843A},
+	{0x204E, 0x40B2},
+	{0x2050, 0xB980},
+	{0x2052, 0x845C},
+	{0x2054, 0x40B2},
+	{0x2056, 0xB9B0},
+	{0x2058, 0x845E},
+	{0x205A, 0x4130},
+	{0x205C, 0x1292},
+	{0x205E, 0xD016},
+	{0x2060, 0xB3D2},
+	{0x2062, 0x0B00},
+	{0x2064, 0x2002},
+	{0x2066, 0xD2E2},
+	{0x2068, 0x0381},
+	{0x206A, 0x93C2},
+	{0x206C, 0x0263},
+	{0x206E, 0x2001},
+	{0x2070, 0x4130},
+	{0x2072, 0x422D},
+	{0x2074, 0x403E},
+	{0x2076, 0x888E},
+	{0x2078, 0x403F},
+	{0x207A, 0x192A},
+	{0x207C, 0x1292},
+	{0x207E, 0x843E},
+	{0x2080, 0x3FF7},
+	{0x2082, 0x422D},
+	{0x2084, 0x403E},
+	{0x2086, 0x192A},
+	{0x2088, 0x403F},
+	{0x208A, 0x888E},
+	{0x208C, 0x1292},
+	{0x208E, 0x843E},
+	{0x2090, 0xB3D2},
+	{0x2092, 0x0267},
+	{0x2094, 0x2403},
+	{0x2096, 0xD0F2},
+	{0x2098, 0x0040},
+	{0x209A, 0x0381},
+	{0x209C, 0x90F2},
+	{0x209E, 0x0010},
+	{0x20A0, 0x0260},
+	{0x20A2, 0x2002},
+	{0x20A4, 0x1292},
+	{0x20A6, 0x84BC},
+	{0x20A8, 0x1292},
+	{0x20AA, 0xD020},
+	{0x20AC, 0x4130},
+	{0x20AE, 0x1292},
+	{0x20B0, 0x8470},
+	{0x20B2, 0x1292},
+	{0x20B4, 0x8452},
+	{0x20B6, 0x0900},
+	{0x20B8, 0x7118},
+	{0x20BA, 0x1292},
+	{0x20BC, 0x848E},
+	{0x20BE, 0x0900},
+	{0x20C0, 0x7112},
+	{0x20C2, 0x0800},
+	{0x20C4, 0x7A20},
+	{0x20C6, 0x4292},
+	{0x20C8, 0x87DE},
+	{0x20CA, 0x7334},
+	{0x20CC, 0x0F00},
+	{0x20CE, 0x7304},
+	{0x20D0, 0x421F},
+	{0x20D2, 0x8718},
+	{0x20D4, 0x1292},
+	{0x20D6, 0x846E},
+	{0x20D8, 0x1292},
+	{0x20DA, 0x8488},
+	{0x20DC, 0x0B00},
+	{0x20DE, 0x7114},
+	{0x20E0, 0x0002},
+	{0x20E2, 0x1292},
+	{0x20E4, 0x848C},
+	{0x20E6, 0x1292},
+	{0x20E8, 0x8454},
+	{0x20EA, 0x43C2},
+	{0x20EC, 0x86EE},
+	{0x20EE, 0x1292},
+	{0x20F0, 0x8444},
+	{0x20F2, 0x4130},
+	{0x20F4, 0x4392},
+	{0x20F6, 0x7360},
+	{0x20F8, 0xB3D2},
+	{0x20FA, 0x0B00},
+	{0x20FC, 0x2402},
+	{0x20FE, 0xC2E2},
+	{0x2100, 0x0381},
+	{0x2102, 0x0900},
+	{0x2104, 0x732C},
+	{0x2106, 0x4382},
+	{0x2108, 0x7360},
+	{0x210A, 0x422D},
+	{0x210C, 0x403E},
+	{0x210E, 0x87F0},
+	{0x2110, 0x403F},
+	{0x2112, 0x87E8},
+	{0x2114, 0x1292},
+	{0x2116, 0x843E},
+	{0x2118, 0x4130},
+	{0x211A, 0x120B},
+	{0x211C, 0x120A},
+	{0x211E, 0x4392},
+	{0x2120, 0x87FA},
+	{0x2122, 0x4392},
+	{0x2124, 0x760E},
+	{0x2126, 0x0900},
+	{0x2128, 0x760C},
+	{0x212A, 0x421B},
+	{0x212C, 0x760A},
+	{0x212E, 0x903B},
+	{0x2130, 0x0201},
+	{0x2132, 0x2408},
+	{0x2134, 0x903B},
+	{0x2136, 0x0102},
+	{0x2138, 0x2405},
+	{0x213A, 0x4292},
+	{0x213C, 0x030A},
+	{0x213E, 0x87F8},
+	{0x2140, 0x1292},
+	{0x2142, 0x849A},
+	{0x2144, 0x903B},
+	{0x2146, 0x0020},
+	{0x2148, 0x2010},
+	{0x214A, 0x403B},
+	{0x214C, 0x8498},
+	{0x214E, 0x422F},
+	{0x2150, 0x12AB},
+	{0x2152, 0x403F},
+	{0x2154, 0x0028},
+	{0x2156, 0x12AB},
+	{0x2158, 0x403B},
+	{0x215A, 0x84C4},
+	{0x215C, 0x407F},
+	{0x215E, 0xFFAA},
+	{0x2160, 0x12AB},
+	{0x2162, 0x407F},
+	{0x2164, 0x0055},
+	{0x2166, 0x12AB},
+	{0x2168, 0x3FDC},
+	{0x216A, 0x903B},
+	{0x216C, 0x0021},
+	{0x216E, 0x2890},
+	{0x2170, 0x903B},
+	{0x2172, 0x0100},
+	{0x2174, 0x200D},
+	{0x2176, 0x403F},
+	{0x2178, 0x0028},
+	{0x217A, 0x1292},
+	{0x217C, 0x8498},
+	{0x217E, 0x425F},
+	{0x2180, 0x0306},
+	{0x2182, 0x1292},
+	{0x2184, 0x84C4},
+	{0x2186, 0x4FC2},
+	{0x2188, 0x0318},
+	{0x218A, 0x0261},
+	{0x218C, 0x0000},
+	{0x218E, 0x3FC9},
+	{0x2190, 0x903B},
+	{0x2192, 0x0101},
+	{0x2194, 0x2858},
+	{0x2196, 0x903B},
+	{0x2198, 0x0200},
+	{0x219A, 0x2450},
+	{0x219C, 0x903B},
+	{0x219E, 0x0201},
+	{0x21A0, 0x2C47},
+	{0x21A2, 0x903B},
+	{0x21A4, 0x0102},
+	{0x21A6, 0x2041},
+	{0x21A8, 0x93E2},
+	{0x21AA, 0x0262},
+	{0x21AC, 0x240A},
+	{0x21AE, 0x425F},
+	{0x21B0, 0x0306},
+	{0x21B2, 0x1292},
+	{0x21B4, 0x84C4},
+	{0x21B6, 0x4F4E},
+	{0x21B8, 0x4EC2},
+	{0x21BA, 0x0318},
+	{0x21BC, 0x0260},
+	{0x21BE, 0x0000},
+	{0x21C0, 0x3FB0},
+	{0x21C2, 0x403A},
+	{0x21C4, 0x8030},
+	{0x21C6, 0x4382},
+	{0x21C8, 0x0326},
+	{0x21CA, 0x4382},
+	{0x21CC, 0x0328},
+	{0x21CE, 0x421B},
+	{0x21D0, 0x030C},
+	{0x21D2, 0x930B},
+	{0x21D4, 0x2420},
+	{0x21D6, 0x4A5F},
+	{0x21D8, 0x0001},
+	{0x21DA, 0x1292},
+	{0x21DC, 0x84C4},
+	{0x21DE, 0x4F4E},
+	{0x21E0, 0x4A5F},
+	{0x21E2, 0x0001},
+	{0x21E4, 0x9F0E},
+	{0x21E6, 0x2402},
+	{0x21E8, 0x5392},
+	{0x21EA, 0x0326},
+	{0x21EC, 0x4ECA},
+	{0x21EE, 0x0001},
+	{0x21F0, 0x533B},
+	{0x21F2, 0x2411},
+	{0x21F4, 0x4A6F},
+	{0x21F6, 0x1292},
+	{0x21F8, 0x84C4},
+	{0x21FA, 0x4F4E},
+	{0x21FC, 0x4A6F},
+	{0x21FE, 0x9F0E},
+	{0x2200, 0x2402},
+	{0x2202, 0x5392},
+	{0x2204, 0x0326},
+	{0x2206, 0x4ECA},
+	{0x2208, 0x0000},
+	{0x220A, 0x533B},
+	{0x220C, 0x532A},
+	{0x220E, 0x0260},
+	{0x2210, 0x0000},
+	{0x2212, 0x930B},
+	{0x2214, 0x23E0},
+	{0x2216, 0x40B2},
+	{0x2218, 0xAA55},
+	{0x221A, 0x0328},
+	{0x221C, 0xB0F2},
+	{0x221E, 0x0040},
+	{0x2220, 0x0381},
+	{0x2222, 0x277F},
+	{0x2224, 0xD3D2},
+	{0x2226, 0x0267},
+	{0x2228, 0x3F7C},
+	{0x222A, 0x0261},
+	{0x222C, 0x0000},
+	{0x222E, 0x3F79},
+	{0x2230, 0x903B},
+	{0x2232, 0x0201},
+	{0x2234, 0x23FA},
+	{0x2236, 0x1292},
+	{0x2238, 0x84C0},
+	{0x223A, 0x3F73},
+	{0x223C, 0x1292},
+	{0x223E, 0x84C0},
+	{0x2240, 0x0261},
+	{0x2242, 0x0000},
+	{0x2244, 0x3F6E},
+	{0x2246, 0x903B},
+	{0x2248, 0x0040},
+	{0x224A, 0x2018},
+	{0x224C, 0x422F},
+	{0x224E, 0x1292},
+	{0x2250, 0x8498},
+	{0x2252, 0x12B0},
+	{0x2254, 0xF0EA},
+	{0x2256, 0x907F},
+	{0x2258, 0xFFAA},
+	{0x225A, 0x240D},
+	{0x225C, 0x5392},
+	{0x225E, 0x0312},
+	{0x2260, 0x12B0},
+	{0x2262, 0xF0EA},
+	{0x2264, 0x907F},
+	{0x2266, 0x0055},
+	{0x2268, 0x2403},
+	{0x226A, 0x5392},
+	{0x226C, 0x0312},
+	{0x226E, 0x3F59},
+	{0x2270, 0x5392},
+	{0x2272, 0x0310},
+	{0x2274, 0x3F56},
+	{0x2276, 0x5392},
+	{0x2278, 0x0310},
+	{0x227A, 0x3FF2},
+	{0x227C, 0x903B},
+	{0x227E, 0x0080},
+	{0x2280, 0x23D4},
+	{0x2282, 0x4382},
+	{0x2284, 0x0312},
+	{0x2286, 0x4382},
+	{0x2288, 0x0310},
+	{0x228A, 0x0261},
+	{0x228C, 0x0000},
+	{0x228E, 0x3F49},
+	{0x2290, 0x932B},
+	{0x2292, 0x2005},
+	{0x2294, 0x403F},
+	{0x2296, 0x0028},
+	{0x2298, 0x1292},
+	{0x229A, 0x8498},
+	{0x229C, 0x3F42},
+	{0x229E, 0x903B},
+	{0x22A0, 0x0003},
+	{0x22A2, 0x284B},
+	{0x22A4, 0x923B},
+	{0x22A6, 0x2015},
+	{0x22A8, 0x403F},
+	{0x22AA, 0x0023},
+	{0x22AC, 0x1292},
+	{0x22AE, 0x8498},
+	{0x22B0, 0x421B},
+	{0x22B2, 0x87F8},
+	{0x22B4, 0x421F},
+	{0x22B6, 0x030C},
+	{0x22B8, 0x9F0B},
+	{0x22BA, 0x2F33},
+	{0x22BC, 0x1292},
+	{0x22BE, 0x84BA},
+	{0x22C0, 0x930F},
+	{0x22C2, 0x2004},
+	{0x22C4, 0x5392},
+	{0x22C6, 0x0312},
+	{0x22C8, 0x531B},
+	{0x22CA, 0x3FF4},
+	{0x22CC, 0x5392},
+	{0x22CE, 0x0310},
+	{0x22D0, 0x3FFB},
+	{0x22D2, 0x903B},
+	{0x22D4, 0x0009},
+	{0x22D6, 0x2818},
+	{0x22D8, 0x903B},
+	{0x22DA, 0x0010},
+	{0x22DC, 0x23A6},
+	{0x22DE, 0x403F},
+	{0x22E0, 0x0027},
+	{0x22E2, 0x1292},
+	{0x22E4, 0x8498},
+	{0x22E6, 0x421B},
+	{0x22E8, 0x87F8},
+	{0x22EA, 0x421F},
+	{0x22EC, 0x030C},
+	{0x22EE, 0x9F0B},
+	{0x22F0, 0x2F18},
+	{0x22F2, 0x1292},
+	{0x22F4, 0x84BA},
+	{0x22F6, 0x930F},
+	{0x22F8, 0x2004},
+	{0x22FA, 0x5392},
+	{0x22FC, 0x0312},
+	{0x22FE, 0x531B},
+	{0x2300, 0x3FF4},
+	{0x2302, 0x5392},
+	{0x2304, 0x0310},
+	{0x2306, 0x3FFB},
+	{0x2308, 0x922B},
+	{0x230A, 0x238F},
+	{0x230C, 0x421B},
+	{0x230E, 0x87F8},
+	{0x2310, 0x421F},
+	{0x2312, 0x030C},
+	{0x2314, 0x9F0B},
+	{0x2316, 0x2C0B},
+	{0x2318, 0x1292},
+	{0x231A, 0x84C2},
+	{0x231C, 0x934F},
+	{0x231E, 0x240A},
+	{0x2320, 0x5392},
+	{0x2322, 0x0312},
+	{0x2324, 0x531B},
+	{0x2326, 0x421F},
+	{0x2328, 0x030C},
+	{0x232A, 0x9F0B},
+	{0x232C, 0x2BF5},
+	{0x232E, 0x0261},
+	{0x2330, 0x0000},
+	{0x2332, 0x3EF7},
+	{0x2334, 0x5392},
+	{0x2336, 0x0310},
+	{0x2338, 0x3FF5},
+	{0x233A, 0x930B},
+	{0x233C, 0x277F},
+	{0x233E, 0x931B},
+	{0x2340, 0x277A},
+	{0x2342, 0x3F73},
+	{0x2344, 0x413A},
+	{0x2346, 0x413B},
+	{0x2348, 0x4130},
+	{0x234A, 0x4F0C},
+	{0x234C, 0x403F},
+	{0x234E, 0x0267},
+	{0x2350, 0xF0FF},
+	{0x2352, 0xFFDF},
+	{0x2354, 0x0000},
+	{0x2356, 0xF0FF},
+	{0x2358, 0xFFEF},
+	{0x235A, 0x0000},
+	{0x235C, 0x421D},
+	{0x235E, 0x84B0},
+	{0x2360, 0x403E},
+	{0x2362, 0x06F9},
+	{0x2364, 0x4C0F},
+	{0x2366, 0x1292},
+	{0x2368, 0x84AC},
+	{0x236A, 0x4F4E},
+	{0x236C, 0xB31E},
+	{0x236E, 0x2403},
+	{0x2370, 0xD0F2},
+	{0x2372, 0x0020},
+	{0x2374, 0x0267},
+	{0x2376, 0xB32E},
+	{0x2378, 0x2403},
+	{0x237A, 0xD0F2},
+	{0x237C, 0x0010},
+	{0x237E, 0x0267},
+	{0x2380, 0xC3E2},
+	{0x2382, 0x0267},
+	{0x2384, 0x4130},
+	{0x2386, 0x120B},
+	{0x2388, 0x120A},
+	{0x238A, 0x403A},
+	{0x238C, 0x1140},
+	{0x238E, 0x1292},
+	{0x2390, 0xD080},
+	{0x2392, 0x430B},
+	{0x2394, 0x4A0F},
+	{0x2396, 0x532A},
+	{0x2398, 0x1292},
+	{0x239A, 0x84A4},
+	{0x239C, 0x4F0E},
+	{0x239E, 0x430F},
+	{0x23A0, 0x5E82},
+	{0x23A2, 0x87FC},
+	{0x23A4, 0x6F82},
+	{0x23A6, 0x87FE},
+	{0x23A8, 0x531B},
+	{0x23AA, 0x923B},
+	{0x23AC, 0x2BF3},
+	{0x23AE, 0x413A},
+	{0x23B0, 0x413B},
+	{0x23B2, 0x4130},
+	{0x23B4, 0xF0F2},
+	{0x23B6, 0x007F},
+	{0x23B8, 0x0267},
+	{0x23BA, 0x421D},
+	{0x23BC, 0x84B6},
+	{0x23BE, 0x403E},
+	{0x23C0, 0x01F9},
+	{0x23C2, 0x1292},
+	{0x23C4, 0x84AC},
+	{0x23C6, 0x4F4E},
+	{0x23C8, 0xF35F},
+	{0x23CA, 0x2403},
+	{0x23CC, 0xD0F2},
+	{0x23CE, 0xFF80},
+	{0x23D0, 0x0267},
+	{0x23D2, 0xB36E},
+	{0x23D4, 0x2404},
+	{0x23D6, 0xD0F2},
+	{0x23D8, 0x0040},
+	{0x23DA, 0x0267},
+	{0x23DC, 0x3C03},
+	{0x23DE, 0xF0F2},
+	{0x23E0, 0xFFBF},
+	{0x23E2, 0x0267},
+	{0x23E4, 0xC2E2},
+	{0x23E6, 0x0267},
+	{0x23E8, 0x4130},
+	{0x23EA, 0x120B},
+	{0x23EC, 0x120A},
+	{0x23EE, 0x8231},
+	{0x23F0, 0x430B},
+	{0x23F2, 0x93C2},
+	{0x23F4, 0x0C0A},
+	{0x23F6, 0x2404},
+	{0x23F8, 0xB3D2},
+	{0x23FA, 0x0B05},
+	{0x23FC, 0x2401},
+	{0x23FE, 0x431B},
+	{0x2400, 0x422D},
+	{0x2402, 0x403E},
+	{0x2404, 0x192A},
+	{0x2406, 0x403F},
+	{0x2408, 0x888E},
+	{0x240A, 0x1292},
+	{0x240C, 0x843E},
+	{0x240E, 0x930B},
+	{0x2410, 0x20F4},
+	{0x2412, 0x93E2},
+	{0x2414, 0x0241},
+	{0x2416, 0x24EB},
+	{0x2418, 0x403A},
+	{0x241A, 0x0292},
+	{0x241C, 0x4AA2},
+	{0x241E, 0x0A00},
+	{0x2420, 0xB2E2},
+	{0x2422, 0x0361},
+	{0x2424, 0x2405},
+	{0x2426, 0x4A2F},
+	{0x2428, 0x1292},
+	{0x242A, 0x8474},
+	{0x242C, 0x4F82},
+	{0x242E, 0x0A1C},
+	{0x2430, 0x93C2},
+	{0x2432, 0x0360},
+	{0x2434, 0x34CD},
+	{0x2436, 0x430C},
+	{0x2438, 0x4C0F},
+	{0x243A, 0x5F0F},
+	{0x243C, 0x4F0D},
+	{0x243E, 0x510D},
+	{0x2440, 0x4F0E},
+	{0x2442, 0x5A0E},
+	{0x2444, 0x4E1E},
+	{0x2446, 0x0002},
+	{0x2448, 0x4F1F},
+	{0x244A, 0x192A},
+	{0x244C, 0x1202},
+	{0x244E, 0xC232},
+	{0x2450, 0x4303},
+	{0x2452, 0x4E82},
+	{0x2454, 0x0130},
+	{0x2456, 0x4F82},
+	{0x2458, 0x0138},
+	{0x245A, 0x421E},
+	{0x245C, 0x013A},
+	{0x245E, 0x421F},
+	{0x2460, 0x013C},
+	{0x2462, 0x4132},
+	{0x2464, 0x108E},
+	{0x2466, 0x108F},
+	{0x2468, 0xEF4E},
+	{0x246A, 0xEF0E},
+	{0x246C, 0xF37F},
+	{0x246E, 0xC312},
+	{0x2470, 0x100F},
+	{0x2472, 0x100E},
+	{0x2474, 0x4E8D},
+	{0x2476, 0x0000},
+	{0x2478, 0x531C},
+	{0x247A, 0x922C},
+	{0x247C, 0x2BDD},
+	{0x247E, 0xB3D2},
+	{0x2480, 0x1921},
+	{0x2482, 0x2403},
+	{0x2484, 0x410F},
+	{0x2486, 0x1292},
+	{0x2488, 0x847E},
+	{0x248A, 0x403B},
+	{0x248C, 0x843E},
+	{0x248E, 0x422D},
+	{0x2490, 0x410E},
+	{0x2492, 0x403F},
+	{0x2494, 0x1908},
+	{0x2496, 0x12AB},
+	{0x2498, 0x403D},
+	{0x249A, 0x0005},
+	{0x249C, 0x403E},
+	{0x249E, 0x0292},
+	{0x24A0, 0x403F},
+	{0x24A2, 0x86E4},
+	{0x24A4, 0x12AB},
+	{0x24A6, 0x421F},
+	{0x24A8, 0x060E},
+	{0x24AA, 0x9F82},
+	{0x24AC, 0x8720},
+	{0x24AE, 0x288D},
+	{0x24B0, 0x9382},
+	{0x24B2, 0x060E},
+	{0x24B4, 0x248A},
+	{0x24B6, 0x90BA},
+	{0x24B8, 0x0010},
+	{0x24BA, 0x0000},
+	{0x24BC, 0x2C0B},
+	{0x24BE, 0x93C2},
+	{0x24C0, 0x86EE},
+	{0x24C2, 0x2008},
+	{0x24C4, 0x403F},
+	{0x24C6, 0x06A7},
+	{0x24C8, 0xD0FF},
+	{0x24CA, 0x0007},
+	{0x24CC, 0x0000},
+	{0x24CE, 0xF0FF},
+	{0x24D0, 0xFFF8},
+	{0x24D2, 0x0000},
+	{0x24D4, 0x4392},
+	{0x24D6, 0x8720},
+	{0x24D8, 0x403F},
+	{0x24DA, 0x06A7},
+	{0x24DC, 0xD2EF},
+	{0x24DE, 0x0000},
+	{0x24E0, 0xC2EF},
+	{0x24E2, 0x0000},
+	{0x24E4, 0x93C2},
+	{0x24E6, 0x87D3},
+	{0x24E8, 0x2068},
+	{0x24EA, 0xB0F2},
+	{0x24EC, 0x0040},
+	{0x24EE, 0x0B05},
+	{0x24F0, 0x2461},
+	{0x24F2, 0xD3D2},
+	{0x24F4, 0x0410},
+	{0x24F6, 0xB3E2},
+	{0x24F8, 0x0381},
+	{0x24FA, 0x2089},
+	{0x24FC, 0x90B2},
+	{0x24FE, 0x0030},
+	{0x2500, 0x0A00},
+	{0x2502, 0x2C52},
+	{0x2504, 0x93C2},
+	{0x2506, 0x86EE},
+	{0x2508, 0x204F},
+	{0x250A, 0x430E},
+	{0x250C, 0x430C},
+	{0x250E, 0x4C0F},
+	{0x2510, 0x5F0F},
+	{0x2512, 0x5F0F},
+	{0x2514, 0x5F0F},
+	{0x2516, 0x4F1F},
+	{0x2518, 0x8668},
+	{0x251A, 0xF03F},
+	{0x251C, 0x07FF},
+	{0x251E, 0x903F},
+	{0x2520, 0x0400},
+	{0x2522, 0x343E},
+	{0x2524, 0x5F0E},
+	{0x2526, 0x531C},
+	{0x2528, 0x923C},
+	{0x252A, 0x2BF1},
+	{0x252C, 0x4E0F},
+	{0x252E, 0x930E},
+	{0x2530, 0x3834},
+	{0x2532, 0x110F},
+	{0x2534, 0x110F},
+	{0x2536, 0x110F},
+	{0x2538, 0x9382},
+	{0x253A, 0x86EE},
+	{0x253C, 0x2023},
+	{0x253E, 0x5F82},
+	{0x2540, 0x87D6},
+	{0x2542, 0x403B},
+	{0x2544, 0x87D6},
+	{0x2546, 0x4B2F},
+	{0x2548, 0x12B0},
+	{0x254A, 0xB624},
+	{0x254C, 0x4F8B},
+	{0x254E, 0x0000},
+	{0x2550, 0x430C},
+	{0x2552, 0x4C0D},
+	{0x2554, 0x5D0D},
+	{0x2556, 0x5D0D},
+	{0x2558, 0x5D0D},
+	{0x255A, 0x403A},
+	{0x255C, 0x87D8},
+	{0x255E, 0x421B},
+	{0x2560, 0x87D6},
+	{0x2562, 0x4B0F},
+	{0x2564, 0x8A2F},
+	{0x2566, 0x4F0E},
+	{0x2568, 0x4E0F},
+	{0x256A, 0x5F0F},
+	{0x256C, 0x7F0F},
+	{0x256E, 0xE33F},
+	{0x2570, 0x8E8D},
+	{0x2572, 0x8668},
+	{0x2574, 0x7F8D},
+	{0x2576, 0x866A},
+	{0x2578, 0x531C},
+	{0x257A, 0x923C},
+	{0x257C, 0x2BEA},
+	{0x257E, 0x4B8A},
+	{0x2580, 0x0000},
+	{0x2582, 0x3C45},
+	{0x2584, 0x9382},
+	{0x2586, 0x86F0},
+	{0x2588, 0x2005},
+	{0x258A, 0x4382},
+	{0x258C, 0x87D6},
+	{0x258E, 0x4382},
+	{0x2590, 0x87D8},
+	{0x2592, 0x3FD7},
+	{0x2594, 0x4F82},
+	{0x2596, 0x87D6},
+	{0x2598, 0x3FD4},
+	{0x259A, 0x503F},
+	{0x259C, 0x0007},
+	{0x259E, 0x3FC9},
+	{0x25A0, 0x5F0E},
+	{0x25A2, 0x503E},
+	{0x25A4, 0xF800},
+	{0x25A6, 0x3FBF},
+	{0x25A8, 0x430F},
+	{0x25AA, 0x12B0},
+	{0x25AC, 0xB624},
+	{0x25AE, 0x4382},
+	{0x25B0, 0x87D6},
+	{0x25B2, 0x3C2D},
+	{0x25B4, 0xC3D2},
+	{0x25B6, 0x0410},
+	{0x25B8, 0x3F9E},
+	{0x25BA, 0x430D},
+	{0x25BC, 0x403E},
+	{0x25BE, 0x0050},
+	{0x25C0, 0x403F},
+	{0x25C2, 0x85C8},
+	{0x25C4, 0x1292},
+	{0x25C6, 0x844E},
+	{0x25C8, 0x3F90},
+	{0x25CA, 0x5392},
+	{0x25CC, 0x8720},
+	{0x25CE, 0x3F84},
+	{0x25D0, 0x403B},
+	{0x25D2, 0x843E},
+	{0x25D4, 0x4A0F},
+	{0x25D6, 0x532F},
+	{0x25D8, 0x422D},
+	{0x25DA, 0x4F0E},
+	{0x25DC, 0x403F},
+	{0x25DE, 0x0E08},
+	{0x25E0, 0x12AB},
+	{0x25E2, 0x422D},
+	{0x25E4, 0x403E},
+	{0x25E6, 0x192A},
+	{0x25E8, 0x410F},
+	{0x25EA, 0x12AB},
+	{0x25EC, 0x3F48},
+	{0x25EE, 0x93C2},
+	{0x25F0, 0x86EE},
+	{0x25F2, 0x2312},
+	{0x25F4, 0x403A},
+	{0x25F6, 0x86E4},
+	{0x25F8, 0x3F11},
+	{0x25FA, 0x403D},
+	{0x25FC, 0x0200},
+	{0x25FE, 0x422E},
+	{0x2600, 0x403F},
+	{0x2602, 0x192A},
+	{0x2604, 0x1292},
+	{0x2606, 0x844E},
+	{0x2608, 0xC3D2},
+	{0x260A, 0x1921},
+	{0x260C, 0x3F02},
+	{0x260E, 0x422D},
+	{0x2610, 0x403E},
+	{0x2612, 0x888E},
+	{0x2614, 0x403F},
+	{0x2616, 0x192A},
+	{0x2618, 0x1292},
+	{0x261A, 0x843E},
+	{0x261C, 0x5231},
+	{0x261E, 0x413A},
+	{0x2620, 0x413B},
+	{0x2622, 0x4130},
+	{0x2624, 0x4382},
+	{0x2626, 0x052C},
+	{0x2628, 0x4F0D},
+	{0x262A, 0x930D},
+	{0x262C, 0x3402},
+	{0x262E, 0xE33D},
+	{0x2630, 0x531D},
+	{0x2632, 0xF03D},
+	{0x2634, 0x07F0},
+	{0x2636, 0x4D0E},
+	{0x2638, 0xC312},
+	{0x263A, 0x100E},
+	{0x263C, 0x110E},
+	{0x263E, 0x110E},
+	{0x2640, 0x110E},
+	{0x2642, 0x930F},
+	{0x2644, 0x3803},
+	{0x2646, 0x4EC2},
+	{0x2648, 0x052C},
+	{0x264A, 0x3C04},
+	{0x264C, 0x4EC2},
+	{0x264E, 0x052D},
+	{0x2650, 0xE33D},
+	{0x2652, 0x531D},
+	{0x2654, 0x4D0F},
+	{0x2656, 0x4130},
+	{0x2658, 0x1292},
+	{0x265A, 0xD048},
+	{0x265C, 0x93C2},
+	{0x265E, 0x86EE},
+	{0x2660, 0x200D},
+	{0x2662, 0xB0F2},
+	{0x2664, 0x0020},
+	{0x2666, 0x0381},
+	{0x2668, 0x2407},
+	{0x266A, 0x9292},
+	{0x266C, 0x8722},
+	{0x266E, 0x0384},
+	{0x2670, 0x2C03},
+	{0x2672, 0xD3D2},
+	{0x2674, 0x0649},
+	{0x2676, 0x4130},
+	{0x2678, 0xC3D2},
+	{0x267A, 0x0649},
+	{0x267C, 0x4130},
+	{0x267E, 0x120B},
+	{0x2680, 0x120A},
+	{0x2682, 0x1209},
+	{0x2684, 0x1208},
+	{0x2686, 0x1207},
+	{0x2688, 0x1206},
+	{0x268A, 0x1205},
+	{0x268C, 0x1204},
+	{0x268E, 0x8231},
+	{0x2690, 0x4F81},
+	{0x2692, 0x0000},
+	{0x2694, 0x4381},
+	{0x2696, 0x0002},
+	{0x2698, 0x4304},
+	{0x269A, 0x411C},
+	{0x269C, 0x0002},
+	{0x269E, 0x5C0C},
+	{0x26A0, 0x4C0F},
+	{0x26A2, 0x5F0F},
+	{0x26A4, 0x5F0F},
+	{0x26A6, 0x5F0F},
+	{0x26A8, 0x5F0F},
+	{0x26AA, 0x5F0F},
+	{0x26AC, 0x503F},
+	{0x26AE, 0x1980},
+	{0x26B0, 0x440D},
+	{0x26B2, 0x5D0D},
+	{0x26B4, 0x4D0E},
+	{0x26B6, 0x5F0E},
+	{0x26B8, 0x4E2E},
+	{0x26BA, 0x4D05},
+	{0x26BC, 0x5505},
+	{0x26BE, 0x5F05},
+	{0x26C0, 0x4516},
+	{0x26C2, 0x0008},
+	{0x26C4, 0x4517},
+	{0x26C6, 0x000A},
+	{0x26C8, 0x460A},
+	{0x26CA, 0x470B},
+	{0x26CC, 0xF30A},
+	{0x26CE, 0xF32B},
+	{0x26D0, 0x4A81},
+	{0x26D2, 0x0004},
+	{0x26D4, 0x4B81},
+	{0x26D6, 0x0006},
+	{0x26D8, 0xB03E},
+	{0x26DA, 0x2000},
+	{0x26DC, 0x2404},
+	{0x26DE, 0xF03E},
+	{0x26E0, 0x1FFF},
+	{0x26E2, 0xE33E},
+	{0x26E4, 0x531E},
+	{0x26E6, 0xF317},
+	{0x26E8, 0x503E},
+	{0x26EA, 0x2000},
+	{0x26EC, 0x4E0F},
+	{0x26EE, 0x5F0F},
+	{0x26F0, 0x7F0F},
+	{0x26F2, 0xE33F},
+	{0x26F4, 0x512C},
+	{0x26F6, 0x4C28},
+	{0x26F8, 0x4309},
+	{0x26FA, 0x4E0A},
+	{0x26FC, 0x4F0B},
+	{0x26FE, 0x480C},
+	{0x2700, 0x490D},
+	{0x2702, 0x1202},
+	{0x2704, 0xC232},
+	{0x2706, 0x12B0},
+	{0x2708, 0xFFC0},
+	{0x270A, 0x4132},
+	{0x270C, 0x108E},
+	{0x270E, 0x108F},
+	{0x2710, 0xEF4E},
+	{0x2712, 0xEF0E},
+	{0x2714, 0xF37F},
+	{0x2716, 0xC312},
+	{0x2718, 0x100F},
+	{0x271A, 0x100E},
+	{0x271C, 0x4E85},
+	{0x271E, 0x0018},
+	{0x2720, 0x4F85},
+	{0x2722, 0x001A},
+	{0x2724, 0x480A},
+	{0x2726, 0x490B},
+	{0x2728, 0x460C},
+	{0x272A, 0x470D},
+	{0x272C, 0x1202},
+	{0x272E, 0xC232},
+	{0x2730, 0x12B0},
+	{0x2732, 0xFFC0},
+	{0x2734, 0x4132},
+	{0x2736, 0x4E0C},
+	{0x2738, 0x4F0D},
+	{0x273A, 0x108C},
+	{0x273C, 0x108D},
+	{0x273E, 0xED4C},
+	{0x2740, 0xED0C},
+	{0x2742, 0xF37D},
+	{0x2744, 0xC312},
+	{0x2746, 0x100D},
+	{0x2748, 0x100C},
+	{0x274A, 0x411E},
+	{0x274C, 0x0004},
+	{0x274E, 0x411F},
+	{0x2750, 0x0006},
+	{0x2752, 0x5E0E},
+	{0x2754, 0x6F0F},
+	{0x2756, 0x5E0E},
+	{0x2758, 0x6F0F},
+	{0x275A, 0x5E0E},
+	{0x275C, 0x6F0F},
+	{0x275E, 0xDE0C},
+	{0x2760, 0xDF0D},
+	{0x2762, 0x4C85},
+	{0x2764, 0x002C},
+	{0x2766, 0x4D85},
+	{0x2768, 0x002E},
+	{0x276A, 0x5314},
+	{0x276C, 0x9224},
+	{0x276E, 0x2B95},
+	{0x2770, 0x5391},
+	{0x2772, 0x0002},
+	{0x2774, 0x92A1},
+	{0x2776, 0x0002},
+	{0x2778, 0x2B8F},
+	{0x277A, 0x5231},
+	{0x277C, 0x4134},
+	{0x277E, 0x4135},
+	{0x2780, 0x4136},
+	{0x2782, 0x4137},
+	{0x2784, 0x4138},
+	{0x2786, 0x4139},
+	{0x2788, 0x413A},
+	{0x278A, 0x413B},
+	{0x278C, 0x4130},
+	{0x278E, 0x120B},
+	{0x2790, 0x120A},
+	{0x2792, 0x1209},
+	{0x2794, 0x8031},
+	{0x2796, 0x000C},
+	{0x2798, 0x425F},
+	{0x279A, 0x0205},
+	{0x279C, 0xC312},
+	{0x279E, 0x104F},
+	{0x27A0, 0x114F},
+	{0x27A2, 0x114F},
+	{0x27A4, 0x114F},
+	{0x27A6, 0x114F},
+	{0x27A8, 0x114F},
+	{0x27AA, 0xF37F},
+	{0x27AC, 0x4F0B},
+	{0x27AE, 0xF31B},
+	{0x27B0, 0x5B0B},
+	{0x27B2, 0x5B0B},
+	{0x27B4, 0x5B0B},
+	{0x27B6, 0x503B},
+	{0x27B8, 0xD194},
+	{0x27BA, 0x4219},
+	{0x27BC, 0x0508},
+	{0x27BE, 0xF039},
+	{0x27C0, 0x2000},
+	{0x27C2, 0x4F0A},
+	{0x27C4, 0xC312},
+	{0x27C6, 0x100A},
+	{0x27C8, 0xE31A},
+	{0x27CA, 0x421F},
+	{0x27CC, 0x87DE},
+	{0x27CE, 0x503F},
+	{0x27D0, 0xFF60},
+	{0x27D2, 0x903F},
+	{0x27D4, 0x00C8},
+	{0x27D6, 0x2C02},
+	{0x27D8, 0x403F},
+	{0x27DA, 0x00C8},
+	{0x27DC, 0x4F82},
+	{0x27DE, 0x7322},
+	{0x27E0, 0xB3D2},
+	{0x27E2, 0x0381},
+	{0x27E4, 0x2009},
+	{0x27E6, 0x421F},
+	{0x27E8, 0x86F0},
+	{0x27EA, 0xD21F},
+	{0x27EC, 0x86EE},
+	{0x27EE, 0x930F},
+	{0x27F0, 0x24B9},
+	{0x27F2, 0x40F2},
+	{0x27F4, 0xFF80},
+	{0x27F6, 0x0619},
+	{0x27F8, 0x1292},
+	{0x27FA, 0xD00A},
+	{0x27FC, 0xB3D2},
+	{0x27FE, 0x0385},
+	{0x2800, 0x2405},
+	{0x2802, 0x421F},
+	{0x2804, 0x880A},
+	{0x2806, 0x4F92},
+	{0x2808, 0x0002},
+	{0x280A, 0x8714},
+	{0x280C, 0x430D},
+	{0x280E, 0x93C2},
+	{0x2810, 0x87D0},
+	{0x2812, 0x2003},
+	{0x2814, 0xB2F2},
+	{0x2816, 0x0360},
+	{0x2818, 0x2001},
+	{0x281A, 0x431D},
+	{0x281C, 0x425F},
+	{0x281E, 0x87D3},
+	{0x2820, 0xD25F},
+	{0x2822, 0x87D2},
+	{0x2824, 0xF37F},
+	{0x2826, 0x5F0F},
+	{0x2828, 0x425E},
+	{0x282A, 0x87CD},
+	{0x282C, 0xDE0F},
+	{0x282E, 0x5F0F},
+	{0x2830, 0x5B0F},
+	{0x2832, 0x4FA2},
+	{0x2834, 0x0402},
+	{0x2836, 0x930D},
+	{0x2838, 0x2007},
+	{0x283A, 0x930A},
+	{0x283C, 0x248E},
+	{0x283E, 0x4F5F},
+	{0x2840, 0x0001},
+	{0x2842, 0xF37F},
+	{0x2844, 0x4FC2},
+	{0x2846, 0x0403},
+	{0x2848, 0x93C2},
+	{0x284A, 0x87CD},
+	{0x284C, 0x2483},
+	{0x284E, 0xC2F2},
+	{0x2850, 0x0400},
+	{0x2852, 0xB2E2},
+	{0x2854, 0x0265},
+	{0x2856, 0x2407},
+	{0x2858, 0x421F},
+	{0x285A, 0x0508},
+	{0x285C, 0xF03F},
+	{0x285E, 0xFFDF},
+	{0x2860, 0xD90F},
+	{0x2862, 0x4F82},
+	{0x2864, 0x0508},
+	{0x2866, 0xB3D2},
+	{0x2868, 0x0383},
+	{0x286A, 0x2484},
+	{0x286C, 0x403F},
+	{0x286E, 0x0508},
+	{0x2870, 0x4FB1},
+	{0x2872, 0x0000},
+	{0x2874, 0x4FB1},
+	{0x2876, 0x0002},
+	{0x2878, 0x4FB1},
+	{0x287A, 0x0004},
+	{0x287C, 0x403F},
+	{0x287E, 0x0500},
+	{0x2880, 0x4FB1},
+	{0x2882, 0x0006},
+	{0x2884, 0x4FB1},
+	{0x2886, 0x0008},
+	{0x2888, 0x4FB1},
+	{0x288A, 0x000A},
+	{0x288C, 0xB3E2},
+	{0x288E, 0x0383},
+	{0x2890, 0x2412},
+	{0x2892, 0xC2E1},
+	{0x2894, 0x0002},
+	{0x2896, 0xB2E2},
+	{0x2898, 0x0383},
+	{0x289A, 0x434F},
+	{0x289C, 0x634F},
+	{0x289E, 0xF37F},
+	{0x28A0, 0x4F4E},
+	{0x28A2, 0x114E},
+	{0x28A4, 0x434E},
+	{0x28A6, 0x104E},
+	{0x28A8, 0x415F},
+	{0x28AA, 0x0007},
+	{0x28AC, 0xF07F},
+	{0x28AE, 0x007F},
+	{0x28B0, 0xDE4F},
+	{0x28B2, 0x4FC1},
+	{0x28B4, 0x0007},
+	{0x28B6, 0xB2F2},
+	{0x28B8, 0x0383},
+	{0x28BA, 0x2415},
+	{0x28BC, 0xF0F1},
+	{0x28BE, 0xFFBF},
+	{0x28C0, 0x0000},
+	{0x28C2, 0xB0F2},
+	{0x28C4, 0x0010},
+	{0x28C6, 0x0383},
+	{0x28C8, 0x434E},
+	{0x28CA, 0x634E},
+	{0x28CC, 0x5E4E},
+	{0x28CE, 0x5E4E},
+	{0x28D0, 0x5E4E},
+	{0x28D2, 0x5E4E},
+	{0x28D4, 0x5E4E},
+	{0x28D6, 0x5E4E},
+	{0x28D8, 0x415F},
+	{0x28DA, 0x0006},
+	{0x28DC, 0xF07F},
+	{0x28DE, 0xFFBF},
+	{0x28E0, 0xDE4F},
+	{0x28E2, 0x4FC1},
+	{0x28E4, 0x0006},
+	{0x28E6, 0xB0F2},
+	{0x28E8, 0x0020},
+	{0x28EA, 0x0383},
+	{0x28EC, 0x2410},
+	{0x28EE, 0xF0F1},
+	{0x28F0, 0xFFDF},
+	{0x28F2, 0x0002},
+	{0x28F4, 0xB0F2},
+	{0x28F6, 0x0040},
+	{0x28F8, 0x0383},
+	{0x28FA, 0x434E},
+	{0x28FC, 0x634E},
+	{0x28FE, 0x5E4E},
+	{0x2900, 0x5E4E},
+	{0x2902, 0x415F},
+	{0x2904, 0x0008},
+	{0x2906, 0xC26F},
+	{0x2908, 0xDE4F},
+	{0x290A, 0x4FC1},
+	{0x290C, 0x0008},
+	{0x290E, 0x93C2},
+	{0x2910, 0x0383},
+	{0x2912, 0x3412},
+	{0x2914, 0xF0F1},
+	{0x2916, 0xFFDF},
+	{0x2918, 0x0000},
+	{0x291A, 0x425E},
+	{0x291C, 0x0382},
+	{0x291E, 0xF35E},
+	{0x2920, 0x5E4E},
+	{0x2922, 0x5E4E},
+	{0x2924, 0x5E4E},
+	{0x2926, 0x5E4E},
+	{0x2928, 0x5E4E},
+	{0x292A, 0x415F},
+	{0x292C, 0x0006},
+	{0x292E, 0xF07F},
+	{0x2930, 0xFFDF},
+	{0x2932, 0xDE4F},
+	{0x2934, 0x4FC1},
+	{0x2936, 0x0006},
+	{0x2938, 0x410F},
+	{0x293A, 0x4FB2},
+	{0x293C, 0x0508},
+	{0x293E, 0x4FB2},
+	{0x2940, 0x050A},
+	{0x2942, 0x4FB2},
+	{0x2944, 0x050C},
+	{0x2946, 0x4FB2},
+	{0x2948, 0x0500},
+	{0x294A, 0x4FB2},
+	{0x294C, 0x0502},
+	{0x294E, 0x4FB2},
+	{0x2950, 0x0504},
+	{0x2952, 0x3C10},
+	{0x2954, 0xD2F2},
+	{0x2956, 0x0400},
+	{0x2958, 0x3F7C},
+	{0x295A, 0x4F6F},
+	{0x295C, 0xF37F},
+	{0x295E, 0x4FC2},
+	{0x2960, 0x0402},
+	{0x2962, 0x3F72},
+	{0x2964, 0x90F2},
+	{0x2966, 0x0011},
+	{0x2968, 0x0619},
+	{0x296A, 0x2B46},
+	{0x296C, 0x50F2},
+	{0x296E, 0xFFF0},
+	{0x2970, 0x0619},
+	{0x2972, 0x3F42},
+	{0x2974, 0x5031},
+	{0x2976, 0x000C},
+	{0x2978, 0x4139},
+	{0x297A, 0x413A},
+	{0x297C, 0x413B},
+	{0x297E, 0x4130},
+	{0x2980, 0x0900},
+	{0x2982, 0x7312},
+	{0x2984, 0x421F},
+	{0x2986, 0x0A08},
+	{0x2988, 0xF03F},
+	{0x298A, 0xF7FF},
+	{0x298C, 0x4F82},
+	{0x298E, 0x0A88},
+	{0x2990, 0x0900},
+	{0x2992, 0x7312},
+	{0x2994, 0x421F},
+	{0x2996, 0x0A0E},
+	{0x2998, 0xF03F},
+	{0x299A, 0x7FFF},
+	{0x299C, 0x4F82},
+	{0x299E, 0x0A8E},
+	{0x29A0, 0x0900},
+	{0x29A2, 0x7312},
+	{0x29A4, 0x421F},
+	{0x29A6, 0x0A1E},
+	{0x29A8, 0xC31F},
+	{0x29AA, 0x4F82},
+	{0x29AC, 0x0A9E},
+	{0x29AE, 0x4130},
+	{0x29B0, 0x4292},
+	{0x29B2, 0x0A08},
+	{0x29B4, 0x0A88},
+	{0x29B6, 0x0900},
+	{0x29B8, 0x7312},
+	{0x29BA, 0x4292},
+	{0x29BC, 0x0A0E},
+	{0x29BE, 0x0A8E},
+	{0x29C0, 0x0900},
+	{0x29C2, 0x7312},
+	{0x29C4, 0x4292},
+	{0x29C6, 0x0A1E},
+	{0x29C8, 0x0A9E},
+	{0x29CA, 0x4130},
+	{0x29CC, 0x7400},
+	{0x29CE, 0x8058},
+	{0x29D0, 0x1807},
+	{0x29D2, 0x00E0},
+	{0x29D4, 0x7002},
+	{0x29D6, 0x17C7},
+	{0x29D8, 0x0045},
+	{0x29DA, 0x0006},
+	{0x29DC, 0x17CC},
+	{0x29DE, 0x0015},
+	{0x29E0, 0x1512},
+	{0x29E2, 0x216F},
+	{0x29E4, 0x005B},
+	{0x29E6, 0x005D},
+	{0x29E8, 0x00DE},
+	{0x29EA, 0x00DD},
+	{0x29EC, 0x5023},
+	{0x29EE, 0x00DE},
+	{0x29F0, 0x005B},
+	{0x29F2, 0x0410},
+	{0x29F4, 0x0091},
+	{0x29F6, 0x0015},
+	{0x29F8, 0x0040},
+	{0x29FA, 0x7023},
+	{0x29FC, 0x1653},
+	{0x29FE, 0x0156},
+	{0x2A00, 0x0001},
+	{0x2A02, 0x2081},
+	{0x2A04, 0x7020},
+	{0x2A06, 0x2F99},
+	{0x2A08, 0x005C},
+	{0x2A0A, 0x0000},
+	{0x2A0C, 0x5040},
+	{0x2A0E, 0x0045},
+	{0x2A10, 0x213A},
+	{0x2A12, 0x0303},
+	{0x2A14, 0x0148},
+	{0x2A16, 0x0049},
+	{0x2A18, 0x0045},
+	{0x2A1A, 0x0046},
+	{0x2A1C, 0x05DD},
+	{0x2A1E, 0x00DE},
+	{0x2A20, 0x00DD},
+	{0x2A22, 0x00DC},
+	{0x2A24, 0x00DE},
+	{0x2A26, 0x04D6},
+	{0x2A28, 0x2014},
+	{0x2A2A, 0x2081},
+	{0x2A2C, 0x7087},
+	{0x2A2E, 0x2F99},
+	{0x2A30, 0x005C},
+	{0x2A32, 0x0002},
+	{0x2A34, 0x5060},
+	{0x2A36, 0x31C0},
+	{0x2A38, 0x2122},
+	{0x2A3A, 0x7800},
+	{0x2A3C, 0xC08C},
+	{0x2A3E, 0x0001},
+	{0x2A40, 0x9038},
+	{0x2A42, 0x59F7},
+	{0x2A44, 0x907A},
+	{0x2A46, 0x03D8},
+	{0x2A48, 0x8D90},
+	{0x2A4A, 0x01C0},
+	{0x2A4C, 0x7400},
+	{0x2A4E, 0x8058},
+	{0x2A50, 0x1807},
+	{0x2A52, 0x00E0},
+	{0x2A54, 0x7002},
+	{0x2A56, 0x17C7},
+	{0x2A58, 0x0045},
+	{0x2A5A, 0x0006},
+	{0x2A5C, 0x17CC},
+	{0x2A5E, 0x0015},
+	{0x2A60, 0x1512},
+	{0x2A62, 0x216F},
+	{0x2A64, 0x005B},
+	{0x2A66, 0x005D},
+	{0x2A68, 0x00DE},
+	{0x2A6A, 0x00DD},
+	{0x2A6C, 0x5023},
+	{0x2A6E, 0x00DE},
+	{0x2A70, 0x005B},
+	{0x2A72, 0x0410},
+	{0x2A74, 0x0091},
+	{0x2A76, 0x0015},
+	{0x2A78, 0x0040},
+	{0x2A7A, 0x7023},
+	{0x2A7C, 0x1653},
+	{0x2A7E, 0x0156},
+	{0x2A80, 0x0001},
+	{0x2A82, 0x2081},
+	{0x2A84, 0x7020},
+	{0x2A86, 0x2F99},
+	{0x2A88, 0x005C},
+	{0x2A8A, 0x0000},
+	{0x2A8C, 0x5040},
+	{0x2A8E, 0x0045},
+	{0x2A90, 0x213A},
+	{0x2A92, 0x0303},
+	{0x2A94, 0x0148},
+	{0x2A96, 0x0049},
+	{0x2A98, 0x0045},
+	{0x2A9A, 0x0046},
+	{0x2A9C, 0x05DD},
+	{0x2A9E, 0x00DE},
+	{0x2AA0, 0x00DD},
+	{0x2AA2, 0x00DC},
+	{0x2AA4, 0x00DE},
+	{0x2AA6, 0x0296},
+	{0x2AA8, 0x2014},
+	{0x2AAA, 0x2081},
+	{0x2AAC, 0x7087},
+	{0x2AAE, 0x2F99},
+	{0x2AB0, 0x005C},
+	{0x2AB2, 0x0002},
+	{0x2AB4, 0x5060},
+	{0x2AB6, 0x31C0},
+	{0x2AB8, 0x2122},
+	{0x2ABA, 0x7800},
+	{0x2ABC, 0xC08C},
+	{0x2ABE, 0x0001},
+	{0x2AC0, 0x9038},
+	{0x2AC2, 0x59F7},
+	{0x2AC4, 0x907A},
+	{0x2AC6, 0x03D8},
+	{0x2AC8, 0x8D90},
+	{0x2ACA, 0x01C0},
+	{0x2ACC, 0x7400},
+	{0x2ACE, 0x2002},
+	{0x2AD0, 0x70DF},
+	{0x2AD2, 0x2F21},
+	{0x2AD4, 0x04C1},
+	{0x2AD6, 0x0D80},
+	{0x2AD8, 0x7800},
+	{0x2ADA, 0x0041},
+	{0x2ADC, 0x7400},
+	{0x2ADE, 0x2004},
+	{0x2AE0, 0x70DF},
+	{0x2AE2, 0x2F21},
+	{0x2AE4, 0x04C2},
+	{0x2AE6, 0x0D80},
+	{0x2AE8, 0x7800},
+	{0x2AEA, 0x7400},
+	{0x2AEC, 0x2008},
+	{0x2AEE, 0x70DF},
+	{0x2AF0, 0x2F21},
+	{0x2AF2, 0x04C3},
+	{0x2AF4, 0x0D80},
+	{0x2AF6, 0x7800},
+	{0x2AF8, 0x7400},
+	{0x2AFA, 0x0004},
+	{0x2AFC, 0x70DF},
+	{0x2AFE, 0x2F22},
+	{0x2B00, 0x7008},
+	{0x2B02, 0x2F1F},
+	{0x2B04, 0x7021},
+	{0x2B06, 0x2F01},
+	{0x2B08, 0x7800},
+	{0x2B0A, 0x7400},
+	{0x2B0C, 0x0002},
+	{0x2B0E, 0x70DF},
+	{0x2B10, 0x3F5F},
+	{0x2B12, 0x703A},
+	{0x2B14, 0x2F01},
+	{0x2B16, 0x7800},
+	{0x2B18, 0x7400},
+	{0x2B1A, 0x2010},
+	{0x2B1C, 0x70DF},
+	{0x2B1E, 0x3F40},
+	{0x2B20, 0x700A},
+	{0x2B22, 0x0FC0},
+	{0x2B24, 0x7800},
+	{0x2B26, 0x7400},
+	{0x2B28, 0x2004},
+	{0x2B2A, 0x70DF},
+	{0x2B2C, 0x2F21},
+	{0x2B2E, 0x04C2},
+	{0x2B30, 0x0D80},
+	{0x2B32, 0x7800},
+	{0x2B34, 0x0041},
+	{0x2B36, 0x7400},
+	{0x2B38, 0x2002},
+	{0x2B3A, 0x70DF},
+	{0x2B3C, 0x2F22},
+	{0x2B3E, 0x04C1},
+	{0x2B40, 0x0D80},
+	{0x2B42, 0x7800},
+	{0x2B44, 0x7400},
+	{0x2B46, 0x0001},
+	{0x2B48, 0x70DF},
+	{0x2B4A, 0x3F5F},
+	{0x2B4C, 0x703A},
+	{0x2B4E, 0x2F01},
+	{0x2B50, 0x7800},
+	{0x2B52, 0x7400},
+	{0x2B54, 0x200A},
+	{0x2B56, 0x70DF},
+	{0x2B58, 0x3F40},
+	{0x2B5A, 0x700A},
+	{0x2B5C, 0x0FC0},
+	{0x2B5E, 0x7800},
+	{0x2B60, 0x7400},
+	{0x2B62, 0x2015},
+	{0x2B64, 0x70DF},
+	{0x2B66, 0x3F5F},
+	{0x2B68, 0x703A},
+	{0x2B6A, 0x2F01},
+	{0x2B6C, 0x7800},
+	{0x2B6E, 0x7400},
+	{0x2B70, 0x7800},
+	{0x2B72, 0x007F},
+	{0x2B74, 0x0000},
+	{0x2B76, 0xB9CC},
+	{0x2B78, 0x0000},
+	{0x2B7A, 0xB9CC},
+	{0x2B7C, 0xBA3C},
+	{0x2B7E, 0x0002},
+	{0x2B80, 0x0000},
+	{0x2B82, 0xBA4C},
+	{0x2B84, 0x0000},
+	{0x2B86, 0xBA4C},
+	{0x2B88, 0xBABC},
+	{0x2B8A, 0x0002},
+	{0x2B8C, 0x0063},
+	{0x2B8E, 0xBB26},
+	{0x2B90, 0x0063},
+	{0x2B92, 0xBB36},
+	{0x2B94, 0x0063},
+	{0x2B96, 0xBAEA},
+	{0x2B98, 0x0063},
+	{0x2B9A, 0xBAF8},
+	{0x2B9C, 0xBADA},
+	{0x2B9E, 0x0004},
+	{0x2BA0, 0x0063},
+	{0x2BA2, 0xBAEA},
+	{0x2BA4, 0x0063},
+	{0x2BA6, 0xBB18},
+	{0x2BA8, 0x0063},
+	{0x2BAA, 0xBB26},
+	{0x2BAC, 0x0063},
+	{0x2BAE, 0xBB44},
+	{0x2BB0, 0xBADA},
+	{0x2BB2, 0x0004},
+	{0x2BB4, 0x0063},
+	{0x2BB6, 0xBACC},
+	{0x2BB8, 0x0063},
+	{0x2BBA, 0xBADC},
+	{0x2BBC, 0x0063},
+	{0x2BBE, 0xBAEA},
+	{0x2BC0, 0x0063},
+	{0x2BC2, 0xBAF8},
+	{0x2BC4, 0xBADA},
+	{0x2BC6, 0x0004},
+	{0x2BC8, 0x0063},
+	{0x2BCA, 0xBAEA},
+	{0x2BCC, 0x0063},
+	{0x2BCE, 0xBB18},
+	{0x2BD0, 0x0063},
+	{0x2BD2, 0xBACC},
+	{0x2BD4, 0x0063},
+	{0x2BD6, 0xBB0A},
+	{0x2BD8, 0xBADA},
+	{0x2BDA, 0x0004},
+	{0x2BDC, 0x0063},
+	{0x2BDE, 0xBACC},
+	{0x2BE0, 0x0063},
+	{0x2BE2, 0xBADC},
+	{0x2BE4, 0x0063},
+	{0x2BE6, 0xBAEA},
+	{0x2BE8, 0x0063},
+	{0x2BEA, 0xBB18},
+	{0x2BEC, 0xBADA},
+	{0x2BEE, 0x0004},
+	{0x2BF0, 0xFFFF},
+	{0x2BF2, 0xBB6E},
+	{0x2BF4, 0x0000},
+	{0x2BF6, 0x0000},
+	{0x2BF8, 0x0000},
+	{0x2BFA, 0x0000},
+	{0x2BFC, 0x0000},
+	{0x2BFE, 0x0000},
+	{0x2C00, 0xBB72},
+	{0x2C02, 0x0001},
+	{0x2C04, 0x0063},
+	{0x2C06, 0xBB52},
+	{0x2C08, 0x0063},
+	{0x2C0A, 0xBB60},
+	{0x2C0C, 0x0000},
+	{0x2C0E, 0x0000},
+	{0x2C10, 0x0000},
+	{0x2C12, 0x0000},
+	{0x2C14, 0xBADA},
+	{0x2C16, 0x0002},
+	{0x2C18, 0x0066},
+	{0x2C1A, 0x0067},
+	{0x2C1C, 0x00AF},
+	{0x2C1E, 0x01CF},
+	{0x2C20, 0x0087},
+	{0x2C22, 0x0083},
+	{0x2C24, 0x011B},
+	{0x2C26, 0x035A},
+	{0x2C28, 0x00FA},
+	{0x2C2A, 0x00F2},
+	{0x2C2C, 0x00A6},
+	{0x2C2E, 0x00A4},
+	{0x2C30, 0xFFFF},
+	{0x2C32, 0x002C},
+	{0x2C34, 0x0058},
+	{0x2C36, 0x0000},
+	{0x2C38, 0x0000},
+	{0x2C3A, 0xBC18},
+	{0x2C3C, 0xBB74},
+	{0x2C3E, 0xBB80},
+	{0x2C40, 0xBC32},
+	{0x2C42, 0xBB8C},
+	{0x2C44, 0xBBA0},
+	{0x2C46, 0xBB8C},
+	{0x2C48, 0xBBA0},
+	{0x2C4A, 0xBC04},
+	{0x2C4C, 0xBC04},
+	{0x2C4E, 0xBBF0},
+	{0x2C50, 0xBBF0},
+	{0x2C52, 0xBBB4},
+	{0x2C54, 0xBBC8},
+	{0x2C56, 0xBBB4},
+	{0x2C58, 0xBBC8},
+	{0x2C5A, 0xBC04},
+	{0x2C5C, 0xBC04},
+	{0x2C5E, 0xBBF0},
+	{0x2C60, 0xBBF0},
+	{0x2C62, 0xBB8C},
+	{0x2C64, 0xBBA0},
+	{0x2C66, 0xBB8C},
+	{0x2C68, 0xBBA0},
+	{0x2C6A, 0xBC04},
+	{0x2C6C, 0xBC04},
+	{0x2C6E, 0xBBF0},
+	{0x2C70, 0xBBF0},
+	{0x2C72, 0xBBB4},
+	{0x2C74, 0xBBC8},
+	{0x2C76, 0xBBB4},
+	{0x2C78, 0xBBC8},
+	{0x2C7A, 0xBC04},
+	{0x2C7C, 0xBC04},
+	{0x2C7E, 0xBBF0},
+	{0x2C80, 0xBBF0},
+	{0x3800, 0x880E},
+	{0x3802, 0xBC62},
+	{0x3804, 0xBC40},
+	{0x3806, 0xD13E},
+	{0x3808, 0xBC42},
+	{0x380A, 0xBC3C},
+	{0x380C, 0x0000},
+	{0x380E, 0x0040},
+	{0x3810, 0x0040},
+	{0x3812, 0x0040},
+	{0x3814, 0x0043},
+	{0x3816, 0x0046},
+	{0x3818, 0x004B},
+	{0x381A, 0x004D},
+	{0x381C, 0x0051},
+	{0x381E, 0x0055},
+	{0x3820, 0x005A},
+	{0x3822, 0x005E},
+	{0x3824, 0x0062},
+	{0x3826, 0x0067},
+	{0x3828, 0x006C},
+	{0x382A, 0x0070},
+	{0x382C, 0x0078},
+	{0x382E, 0x0086},
+	{0x3830, 0x0090},
+	{0x3832, 0x0096},
+	{0x3834, 0x009D},
+	{0x3836, 0x00A5},
+	{0x3838, 0x00AD},
+	{0x383A, 0x00B4},
+	{0x383C, 0x00B9},
+	{0x383E, 0x00BE},
+	{0x3840, 0x00C3},
+	{0x3842, 0x00C8},
+	{0x3844, 0x00CD},
+	{0x3846, 0x00D2},
+	{0x3848, 0x00D7},
+	{0x384A, 0x00DC},
+	{0x384C, 0x00DC},
+	{0x384E, 0x0000},
+	{0x3850, 0x0000},
+	{0x3852, 0x0000},
+	{0x3854, 0x0000},
+	{0x3856, 0x0000},
+	{0x3858, 0x0000},
+	{0x385A, 0x0000},
+	{0x385C, 0x0000},
+	{0x385E, 0x0000},
+	{0x3860, 0x0000},
+	{0x3862, 0x0000},
+	{0x3864, 0x0000},
+	{0x3866, 0x0000},
+	{0x3868, 0x0000},
+	{0x386A, 0x0000},
+	{0x386C, 0x0000},
+	{0x386E, 0x0000},
+	{0x3870, 0x0000},
+	{0x3872, 0x0000},
+	{0x3874, 0x0000},
+	{0x3876, 0x0000},
+	{0x3878, 0x0000},
+	{0x387A, 0x0000},
+	{0x387C, 0x0000},
+	{0x387E, 0x0000},
+	{0x3880, 0x0000},
+	{0x3882, 0x0000},
+	{0x3884, 0x0000},
+	{0x3886, 0x0000},
+	{0x3888, 0x0000},
+	{0x388A, 0x0000},
+	{0x388C, 0x0000},
+	{0x026A, 0xFFFF},
+	{0x026C, 0x00FF},
+	{0x026E, 0x0000},
+	{0x0360, 0x1E8E},
+	{0x040E, 0x01EB},
+	{0x0600, 0x1130},
+	{0x0602, 0x3112},
+	{0x0604, 0x8048},
+	{0x0606, 0x00E9},
+	{0x067A, 0x0404},
+	{0x067C, 0x0404},
+	{0x06A8, 0x0240},
+	{0x06AA, 0x00CA},
+	{0x06AC, 0x0041},
+	{0x06B4, 0x3FFF},
+	{0x06DE, 0x0404},
+	{0x06E0, 0x0404},
+	{0x06E2, 0xFF00},
+	{0x06E4, 0x8333},
+	{0x06E6, 0x8333},
+	{0x06E8, 0x8333},
+	{0x06EA, 0x8333},
+	{0x052A, 0x0000},
+	{0x052C, 0x0000},
+	{0x0F06, 0x0002},
+	{0x0A04, 0xB4C5},
+	{0x0A06, 0xC400},
+	{0x0A08, 0x988A},
+	{0x0A0A, 0xA387},
+	{0x0A0E, 0xEEC0},
+	{0x0A12, 0x0000},
+	{0x0A18, 0x0010},
+	{0x0A1C, 0x0040},
+	{0x0A20, 0x0015},
+	{0x0C00, 0x0021},
+	{0x0C16, 0x0002},
+	{0x0708, 0x6FC0},
+	{0x070C, 0x0000},
+	{0x120C, 0x1428},
+	{0x121A, 0x0000},
+	{0x121C, 0x1896},
+	{0x121E, 0x0032},
+	{0x1220, 0x0000},
+	{0x1222, 0x96FF},
+	{0x1244, 0x0000},
+	{0x105C, 0x0F0B},
+	{0x1958, 0x0000},
+	{0x195A, 0x004C},
+	{0x195C, 0x0097},
+	{0x195E, 0x0221},
+	{0x1960, 0x03FE},
+	{0x1980, 0x00E0},
+	{0x1982, 0x0010},
+	{0x1984, 0x2018},
+	{0x1986, 0x0008},
+	{0x1988, 0x0000},
+	{0x198A, 0x0000},
+	{0x198C, 0x0880},
+	{0x198E, 0x0000},
+	{0x1990, 0x1A00},
+	{0x1992, 0x0000},
+	{0x1994, 0x2800},
+	{0x1996, 0x0002},
+	{0x1962, 0x0000},
+	{0x1964, 0x004C},
+	{0x1966, 0x0097},
+	{0x1968, 0x0221},
+	{0x196A, 0x03FE},
+	{0x19C0, 0x00E0},
+	{0x19C2, 0x0010},
+	{0x19C4, 0x2018},
+	{0x19C6, 0x0008},
+	{0x19C8, 0x0000},
+	{0x19CA, 0x0000},
+	{0x19CC, 0x0880},
+	{0x19CE, 0x0000},
+	{0x19D0, 0x1A00},
+	{0x19D2, 0x0000},
+	{0x19D4, 0x2800},
+	{0x19D6, 0x0002},
+	{0x196C, 0x0000},
+	{0x196E, 0x004C},
+	{0x1970, 0x0097},
+	{0x1972, 0x0221},
+	{0x1974, 0x03FE},
+	{0x1A00, 0x00E0},
+	{0x1A02, 0x0010},
+	{0x1A04, 0x2018},
+	{0x1A06, 0x0008},
+	{0x1A08, 0x0000},
+	{0x1A0A, 0x0000},
+	{0x1A0C, 0x0880},
+	{0x1A0E, 0x0000},
+	{0x1A10, 0x1A00},
+	{0x1A12, 0x0000},
+	{0x1A14, 0x2800},
+	{0x1A16, 0x0002},
+	{0x1976, 0x0000},
+	{0x1978, 0x004C},
+	{0x197A, 0x0097},
+	{0x197C, 0x0221},
+	{0x197E, 0x03FE},
+	{0x1A40, 0x00E0},
+	{0x1A42, 0x0010},
+	{0x1A44, 0x2018},
+	{0x1A46, 0x0008},
+	{0x1A48, 0x0000},
+	{0x1A4A, 0x0000},
+	{0x1A4C, 0x0880},
+	{0x1A4E, 0x0000},
+	{0x1A50, 0x1A00},
+	{0x1A52, 0x0000},
+	{0x1A54, 0x2800},
+	{0x1A56, 0x0002},
+	{0x192A, 0x0201},
+	{0x0384, 0x0001},
+	{0x027E, 0x0100},
+};
+
+static const struct hi847_reg mode_3264x2448_regs[] = {
+	{0x0B00, 0x0000},
+	{0x0204, 0x0000},
+	{0x0206, 0x033C},
+	{0x020A, 0x0B4D},
+	{0x020E, 0x0B51},
+	{0x0214, 0x0200},
+	{0x0216, 0x0200},
+	{0x0218, 0x0200},
+	{0x021A, 0x0200},
+	{0x0224, 0x002E},
+	{0x022A, 0x0017},
+	{0x022C, 0x0E1F},
+	{0x022E, 0x09C1},
+	{0x0234, 0x1111},
+	{0x0236, 0x1111},
+	{0x0238, 0x1111},
+	{0x023A, 0x1111},
+	{0x0250, 0x0000},
+	{0x0252, 0x0006},
+	{0x0254, 0x0000},
+	{0x0256, 0x0000},
+	{0x0258, 0x0000},
+	{0x025A, 0x0000},
+	{0x025C, 0x0000},
+	{0x025E, 0x0202},
+	{0x0268, 0x00CD},
+	{0x0440, 0x0002},
+	{0x0F00, 0x0000},
+	{0x0F04, 0x0008},
+	{0x0F06, 0x0002},
+	{0x0B02, 0x0100},
+	{0x0B04, 0x00DC},
+	{0x0B12, 0x0CC0},
+	{0x0B14, 0x0990},
+	{0x0B20, 0x0100},
+	{0x1100, 0x1100},
+	{0x1102, 0x0008},
+	{0x1108, 0x0202},
+	{0x1118, 0x0000},
+	{0x0A10, 0xB040},
+	{0x0C14, 0x0008},
+	{0x0C18, 0x0CC0},
+	{0x0C1A, 0x0990},
+	{0x0730, 0x0001},
+	{0x0732, 0x0000},
+	{0x0734, 0x0300},
+	{0x0736, 0x004B},
+	{0x0738, 0x0001},
+	{0x073C, 0x0900},
+	{0x0740, 0x0000},
+	{0x0742, 0x0000},
+	{0x0744, 0x0300},
+	{0x0746, 0x007D},
+	{0x0748, 0x0002},
+	{0x074A, 0x0900},
+	{0x074C, 0x0000},
+	{0x074E, 0x0100},
+	{0x0750, 0x0000},
+	{0x1200, 0x0946},
+	{0x1202, 0x1A00},
+	{0x120E, 0x6027},
+	{0x1210, 0x8027},
+	{0x1246, 0x0105},
+	{0x1000, 0x0300},
+	{0x1002, 0xC311},
+	{0x1004, 0x2BB0},
+	{0x1010, 0x087B},
+	{0x1012, 0x0040},
+	{0x1014, 0x0020},
+	{0x1016, 0x0020},
+	{0x101A, 0x0020},
+	{0x1020, 0xC107},
+	{0x1022, 0x081E},
+	{0x1024, 0x0509},
+	{0x1026, 0x0B0A},
+	{0x1028, 0x1409},
+	{0x102A, 0x0B05},
+	{0x102C, 0x1400},
+	{0x1038, 0x0000},
+	{0x103E, 0x0001},
+	{0x1040, 0x0000},
+	{0x1042, 0x0008},
+	{0x1044, 0x0120},
+	{0x1046, 0x01B0},
+	{0x1048, 0x0090},
+	{0x1066, 0x089C},
+	{0x1600, 0x0000},
+	{0x1608, 0x0028},
+	{0x160A, 0x0C80},
+	{0x160C, 0x001A},
+	{0x160E, 0x0960},
+	{0x0252, 0x0009},
+	{0x0202, 0x0000},
+};
+
+static const struct hi847_reg mode_1632x1224_regs[] = {
+	{0x0B00, 0x0000},
+	{0x0204, 0x0200},
+	{0x0206, 0x033C},
+	{0x020A, 0x05A5},
+	{0x020E, 0x05A9},
+	{0x0214, 0x0200},
+	{0x0216, 0x0200},
+	{0x0218, 0x0200},
+	{0x021A, 0x0200},
+	{0x0224, 0x002C},
+	{0x022A, 0x0015},
+	{0x022C, 0x0E2D},
+	{0x022E, 0x09C1},
+	{0x0234, 0x3311},
+	{0x0236, 0x3311},
+	{0x0238, 0x3311},
+	{0x023A, 0x2222},
+	{0x0250, 0x0000},
+	{0x0252, 0x0006},
+	{0x0254, 0x0000},
+	{0x0256, 0x0000},
+	{0x0258, 0x0000},
+	{0x025A, 0x0000},
+	{0x025C, 0x0000},
+	{0x025E, 0x0202},
+	{0x0268, 0x00CD},
+	{0x0440, 0x0002},
+	{0x0F00, 0x0400},
+	{0x0F04, 0x0004},
+	{0x0F06, 0x0002},
+	{0x0B02, 0x0100},
+	{0x0B04, 0x00FC},
+	{0x0B12, 0x0660},
+	{0x0B14, 0x04C8},
+	{0x0B20, 0x0200},
+	{0x1100, 0x1100},
+	{0x1102, 0x0008},
+	{0x1108, 0x0402},
+	{0x1118, 0x0000},
+	{0x0A10, 0xB060},
+	{0x0C14, 0x0008},
+	{0x0C18, 0x0CC0},
+	{0x0C1A, 0x04C8},
+	{0x0730, 0x0001},
+	{0x0732, 0x0000},
+	{0x0734, 0x0300},
+	{0x0736, 0x004B},
+	{0x0738, 0x0001},
+	{0x073C, 0x0900},
+	{0x0740, 0x0000},
+	{0x0742, 0x0000},
+	{0x0744, 0x0300},
+	{0x0746, 0x007D},
+	{0x0748, 0x0002},
+	{0x074A, 0x0900},
+	{0x074C, 0x0100},
+	{0x074E, 0x0100},
+	{0x0750, 0x0000},
+	{0x1200, 0x0946},
+	{0x1202, 0x1A00},
+	{0x120E, 0x6027},
+	{0x1210, 0x8027},
+	{0x1246, 0x0105},
+	{0x1000, 0x0300},
+	{0x1002, 0xC311},
+	{0x1004, 0x2BB0},
+	{0x1010, 0x042B},
+	{0x1012, 0x0012},
+	{0x1014, 0x0020},
+	{0x1016, 0x0020},
+	{0x101A, 0x0020},
+	{0x1020, 0xC103},
+	{0x1022, 0x040F},
+	{0x1024, 0x0304},
+	{0x1026, 0x0607},
+	{0x1028, 0x0D06},
+	{0x102A, 0x0605},
+	{0x102C, 0x0C00},
+	{0x1038, 0x0000},
+	{0x103E, 0x0101},
+	{0x1040, 0x0000},
+	{0x1042, 0x0008},
+	{0x1044, 0x0120},
+	{0x1046, 0x01B0},
+	{0x1048, 0x0090},
+	{0x1066, 0x043B},
+	{0x1600, 0x0400},
+	{0x1608, 0x0028},
+	{0x160A, 0x0C80},
+	{0x160C, 0x001A},
+	{0x160E, 0x0960},
+	{0x0252, 0x0009},
+	{0x0202, 0x0000},
+};
+
+static const char * const hi847_test_pattern_menu[] = {
+	"No Pattern",
+	"Solid Colour",
+	"100% Colour Bars",
+	"Fade To Grey Colour Bars",
+	"PN9",
+	"Horizontal Gradient Pattern",
+	"Vertical Gradient Pattern",
+	"Check Board",
+	"Slant Pattern",
+};
+
+static const s64 link_freq_menu_items[] = {
+	HI847_LINK_FREQ_400MHZ,
+	HI847_LINK_FREQ_200MHZ,
+};
+
+static const struct hi847_link_freq_config link_freq_configs[] = {
+	[HI847_LINK_FREQ_400MHZ_INDEX] = {
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mipi_data_rate_lane_4),
+			.regs = mipi_data_rate_lane_4,
+		}
+	},
+	[HI847_LINK_FREQ_200MHZ_INDEX] = {
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mipi_data_rate_lane_4),
+			.regs = mipi_data_rate_lane_4,
+		}
+	}
+};
+
+static const struct hi847_mode supported_modes[] = {
+	{
+		.width = 3264,
+		.height = 2448,
+		.fll_def = HI847_FLL_30FPS,
+		.fll_min = HI847_FLL_30FPS_MIN,
+		.llp = 0x033C,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_3264x2448_regs),
+			.regs = mode_3264x2448_regs,
+		},
+		.link_freq_index = HI847_LINK_FREQ_400MHZ_INDEX,
+	},
+	{
+		.width = 1632,
+		.height = 1224,
+		.fll_def = HI847_FLL_60FPS,
+		.fll_min = HI847_FLL_60FPS_MIN,
+		.llp = 0x033C,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1632x1224_regs),
+			.regs = mode_1632x1224_regs,
+		},
+		.link_freq_index = HI847_LINK_FREQ_200MHZ_INDEX,
+	}
+};
+
+struct hi847 {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct v4l2_ctrl_handler ctrl_handler;
+
+	/* V4L2 Controls */
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *pixel_rate;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *exposure;
+	struct v4l2_ctrl *vflip;
+	struct v4l2_ctrl *hflip;
+
+	/* Current mode */
+	const struct hi847_mode *cur_mode;
+
+	/* To serialize asynchronus callbacks */
+	struct mutex mutex;
+
+	/* Streaming on/off */
+	bool streaming;
+};
+
+static u64 to_pixel_rate(u32 f_index)
+{
+	u64 pixel_rate = link_freq_menu_items[f_index] * 2 * HI847_DATA_LANES;
+
+	do_div(pixel_rate, HI847_RGB_DEPTH);
+
+	return pixel_rate;
+}
+
+static int hi847_read_reg(struct hi847 *hi847, u16 reg, u16 len, u32 *val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&hi847->sd);
+	struct i2c_msg msgs[2];
+	u8 addr_buf[2];
+	u8 data_buf[4] = {0};
+	int ret;
+
+	if (len > 4)
+		return -EINVAL;
+
+	put_unaligned_be16(reg, addr_buf);
+	msgs[0].addr = client->addr;
+	msgs[0].flags = 0;
+	msgs[0].len = sizeof(addr_buf);
+	msgs[0].buf = addr_buf;
+	msgs[1].addr = client->addr;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].len = len;
+	msgs[1].buf = &data_buf[4 - len];
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret != ARRAY_SIZE(msgs))
+		return -EIO;
+
+	*val = get_unaligned_be32(data_buf);
+
+	return 0;
+}
+
+static int hi847_write_reg(struct hi847 *hi847, u16 reg, u16 len, u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&hi847->sd);
+	u8 buf[6];
+
+	if (len > 4)
+		return -EINVAL;
+
+	put_unaligned_be16(reg, buf);
+	put_unaligned_be32(val << 8 * (4 - len), buf + 2);
+	if (i2c_master_send(client, buf, len + 2) != len + 2)
+		return -EIO;
+
+	return 0;
+}
+
+static int hi847_write_reg_list(struct hi847 *hi847,
+				const struct hi847_reg_list *r_list)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&hi847->sd);
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < r_list->num_of_regs; i++) {
+		ret = hi847_write_reg(hi847, r_list->regs[i].address,
+				      HI847_REG_VALUE_16BIT,
+				      r_list->regs[i].val);
+		if (ret) {
+			dev_err_ratelimited(&client->dev,
+					    "failed to write reg 0x%4.4x. error = %d",
+					    r_list->regs[i].address, ret);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int hi847_update_digital_gain(struct hi847 *hi847, u32 d_gain)
+{
+	int ret;
+
+	ret = hi847_write_reg(hi847, HI847_REG_MWB_GR_GAIN,
+			      HI847_REG_VALUE_16BIT, d_gain);
+	if (ret)
+		return ret;
+
+	ret = hi847_write_reg(hi847, HI847_REG_MWB_GB_GAIN,
+			      HI847_REG_VALUE_16BIT, d_gain);
+	if (ret)
+		return ret;
+
+	ret = hi847_write_reg(hi847, HI847_REG_MWB_R_GAIN,
+			      HI847_REG_VALUE_16BIT, d_gain);
+	if (ret)
+		return ret;
+
+	return hi847_write_reg(hi847, HI847_REG_MWB_B_GAIN,
+			       HI847_REG_VALUE_16BIT, d_gain);
+}
+
+static int hi847_test_pattern(struct hi847 *hi847, u32 pattern)
+{
+	int ret;
+	u32 val;
+
+	if (pattern) {
+		ret = hi847_read_reg(hi847, HI847_REG_ISP,
+				     HI847_REG_VALUE_16BIT, &val);
+		if (ret)
+			return ret;
+
+		ret = hi847_write_reg(hi847, HI847_REG_ISP,
+				      HI847_REG_VALUE_16BIT,
+				      val | HI847_REG_ISP_TPG_EN);
+		if (ret)
+			return ret;
+	}
+
+	ret = hi847_read_reg(hi847, HI847_REG_TEST_PATTERN,
+			     HI847_REG_VALUE_16BIT, &val);
+	if (ret)
+		return ret;
+
+	return hi847_write_reg(hi847, HI847_REG_TEST_PATTERN,
+			       HI847_REG_VALUE_16BIT, val | pattern << 8);
+}
+
+static int hi847_grbg_shift(struct hi847 *hi847)
+{
+	int ret;
+	int hflip, vflip;
+
+	/* regs shift for full size */
+	static const u32 FORMAT_X_SHIFT_1[2][2] = {
+		{ 0x0008, 0x0007, },
+		{ 0x0008, 0x0007, },
+	};
+
+	static const u32 FORMAT_Y_SHIFT_1[2][2] = {
+		{ 0x0002, 0x0002, },
+		{ 0x0001, 0x0001, },
+	};
+
+	/* regs shift for binning size */
+	static const u32 FORMAT_X_SHIFT_2[2][2] = {
+		{ 0x0004, 0x0003, },
+		{ 0x0004, 0x0003, },
+	};
+
+	static const u32 FORMAT_Y_SHIFT_2[2][2] = {
+		{ 0x0002, 0x0002, },
+		{ 0x0001, 0x0001, },
+	};
+
+	hflip = hi847->hflip->val;
+	vflip = hi847->vflip->val;
+
+	if (hi847->cur_mode->width == 3264) {
+		ret = hi847_write_reg(hi847, HI847_REG_FORMAT_X,
+				      HI847_REG_VALUE_16BIT,
+				      FORMAT_X_SHIFT_1[vflip][hflip]);
+		if (ret)
+			return ret;
+
+		return hi847_write_reg(hi847, HI847_REG_FORMAT_Y,
+				       HI847_REG_VALUE_16BIT,
+				       FORMAT_Y_SHIFT_1[vflip][hflip]);
+	} else {
+		ret = hi847_write_reg(hi847, HI847_REG_FORMAT_X,
+				      HI847_REG_VALUE_16BIT,
+				      FORMAT_X_SHIFT_2[vflip][hflip]);
+		if (ret)
+			return ret;
+
+		return hi847_write_reg(hi847, HI847_REG_FORMAT_Y,
+				       HI847_REG_VALUE_16BIT,
+				       FORMAT_Y_SHIFT_2[vflip][hflip]);
+	}
+}
+
+static int hi847_set_ctrl_hflip(struct hi847 *hi847, u32 ctrl_val)
+{
+	int ret;
+	u32 val;
+
+	ret = hi847_read_reg(hi847, HI847_REG_MIRROR_FLIP,
+			     HI847_REG_VALUE_16BIT, &val);
+	if (ret)
+		return ret;
+
+	ret = hi847_grbg_shift(hi847);
+	if (ret)
+		return ret;
+
+	return hi847_write_reg(hi847, HI847_REG_MIRROR_FLIP,
+			       HI847_REG_VALUE_16BIT,
+			       ctrl_val ? val | BIT(8) : val & ~BIT(8));
+}
+
+static int hi847_set_ctrl_vflip(struct hi847 *hi847, u8 ctrl_val)
+{
+	int ret;
+	u32 val;
+
+	ret = hi847_read_reg(hi847, HI847_REG_MIRROR_FLIP,
+			     HI847_REG_VALUE_16BIT, &val);
+	if (ret)
+		return ret;
+
+	ret = hi847_grbg_shift(hi847);
+	if (ret)
+		return ret;
+
+	return hi847_write_reg(hi847, HI847_REG_MIRROR_FLIP,
+			       HI847_REG_VALUE_16BIT,
+			       ctrl_val ? val | BIT(9) : val & ~BIT(9));
+}
+
+static int hi847_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct hi847 *hi847 = container_of(ctrl->handler,
+					     struct hi847, ctrl_handler);
+	struct i2c_client *client = v4l2_get_subdevdata(&hi847->sd);
+	s64 exposure_max;
+	int ret = 0;
+
+	/* Propagate change of current control to all related controls */
+	if (ctrl->id == V4L2_CID_VBLANK) {
+		/* Update max exposure while meeting expected vblanking */
+		exposure_max = hi847->cur_mode->height + ctrl->val -
+			       HI847_EXPOSURE_MAX_MARGIN;
+		__v4l2_ctrl_modify_range(hi847->exposure,
+					 hi847->exposure->minimum,
+					 exposure_max, hi847->exposure->step,
+					 exposure_max);
+	}
+
+	/* V4L2 controls values will be applied only when power is already up */
+	if (!pm_runtime_get_if_in_use(&client->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_ANALOGUE_GAIN:
+		ret = hi847_write_reg(hi847, HI847_REG_ANALOG_GAIN,
+				      HI847_REG_VALUE_16BIT, ctrl->val);
+		break;
+
+	case V4L2_CID_DIGITAL_GAIN:
+		ret = hi847_update_digital_gain(hi847, ctrl->val);
+		break;
+
+	case V4L2_CID_EXPOSURE:
+		ret = hi847_write_reg(hi847, HI847_REG_EXPOSURE,
+				      HI847_REG_VALUE_16BIT, ctrl->val);
+		break;
+
+	case V4L2_CID_VBLANK:
+		/* Update FLL that meets expected vertical blanking */
+		ret = hi847_write_reg(hi847, HI847_REG_FLL,
+				      HI847_REG_VALUE_16BIT,
+				      hi847->cur_mode->height + ctrl->val);
+		break;
+
+	case V4L2_CID_TEST_PATTERN:
+		ret = hi847_test_pattern(hi847, ctrl->val);
+		break;
+
+	case V4L2_CID_HFLIP:
+		hi847_set_ctrl_hflip(hi847, ctrl->val);
+		break;
+
+	case V4L2_CID_VFLIP:
+		hi847_set_ctrl_vflip(hi847, ctrl->val);
+		break;
+
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	pm_runtime_put(&client->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops hi847_ctrl_ops = {
+	.s_ctrl = hi847_set_ctrl,
+};
+
+static int hi847_init_controls(struct hi847 *hi847)
+{
+	struct v4l2_ctrl_handler *ctrl_hdlr;
+	s64 exposure_max, h_blank;
+	int ret;
+
+	ctrl_hdlr = &hi847->ctrl_handler;
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 8);
+	if (ret)
+		return ret;
+
+	ctrl_hdlr->lock = &hi847->mutex;
+	hi847->link_freq = v4l2_ctrl_new_int_menu(ctrl_hdlr, &hi847_ctrl_ops,
+						  V4L2_CID_LINK_FREQ,
+					ARRAY_SIZE(link_freq_menu_items) - 1,
+					0, link_freq_menu_items);
+	if (hi847->link_freq)
+		hi847->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	hi847->pixel_rate = v4l2_ctrl_new_std
+			    (ctrl_hdlr, &hi847_ctrl_ops,
+			     V4L2_CID_PIXEL_RATE, 0,
+			     to_pixel_rate(HI847_LINK_FREQ_400MHZ_INDEX),
+			     1,
+			     to_pixel_rate(HI847_LINK_FREQ_400MHZ_INDEX));
+	hi847->vblank = v4l2_ctrl_new_std(ctrl_hdlr, &hi847_ctrl_ops,
+					  V4L2_CID_VBLANK,
+					  hi847->cur_mode->fll_min -
+					  hi847->cur_mode->height,
+					  HI847_FLL_MAX -
+					  hi847->cur_mode->height, 1,
+					  hi847->cur_mode->fll_def -
+					  hi847->cur_mode->height);
+
+	h_blank = hi847->cur_mode->llp - hi847->cur_mode->width;
+
+	hi847->hblank = v4l2_ctrl_new_std(ctrl_hdlr, &hi847_ctrl_ops,
+					  V4L2_CID_HBLANK, h_blank, h_blank, 1,
+					  h_blank);
+	if (hi847->hblank)
+		hi847->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &hi847_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
+			  HI847_ANAL_GAIN_MIN, HI847_ANAL_GAIN_MAX,
+			  HI847_ANAL_GAIN_STEP, HI847_ANAL_GAIN_MIN);
+	v4l2_ctrl_new_std(ctrl_hdlr, &hi847_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
+			  HI847_DGTL_GAIN_MIN, HI847_DGTL_GAIN_MAX,
+			  HI847_DGTL_GAIN_STEP, HI847_DGTL_GAIN_DEFAULT);
+	exposure_max = hi847->cur_mode->fll_def - HI847_EXPOSURE_MAX_MARGIN;
+	hi847->exposure = v4l2_ctrl_new_std(ctrl_hdlr, &hi847_ctrl_ops,
+					    V4L2_CID_EXPOSURE,
+					    HI847_EXPOSURE_MIN, exposure_max,
+					    HI847_EXPOSURE_STEP,
+					    exposure_max);
+	v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &hi847_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(hi847_test_pattern_menu) - 1,
+				     0, 0, hi847_test_pattern_menu);
+	hi847->hflip = v4l2_ctrl_new_std(ctrl_hdlr, &hi847_ctrl_ops,
+					 V4L2_CID_HFLIP, 0, 1, 1, 0);
+	hi847->vflip = v4l2_ctrl_new_std(ctrl_hdlr, &hi847_ctrl_ops,
+					 V4L2_CID_VFLIP, 0, 1, 1, 0);
+
+	if (ctrl_hdlr->error)
+		return ctrl_hdlr->error;
+
+	hi847->sd.ctrl_handler = ctrl_hdlr;
+
+	return 0;
+}
+
+static void hi847_assign_pad_format(const struct hi847_mode *mode,
+				    struct v4l2_mbus_framefmt *fmt)
+{
+	fmt->width = mode->width;
+	fmt->height = mode->height;
+	fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	fmt->field = V4L2_FIELD_NONE;
+}
+
+static int hi847_start_streaming(struct hi847 *hi847)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&hi847->sd);
+	const struct hi847_reg_list *reg_list;
+	int link_freq_index, ret;
+
+	link_freq_index = hi847->cur_mode->link_freq_index;
+	reg_list = &link_freq_configs[link_freq_index].reg_list;
+	ret = hi847_write_reg_list(hi847, reg_list);
+	if (ret) {
+		dev_err(&client->dev, "failed to set plls");
+		return ret;
+	}
+
+	reg_list = &hi847->cur_mode->reg_list;
+	ret = hi847_write_reg_list(hi847, reg_list);
+	if (ret) {
+		dev_err(&client->dev, "failed to set mode");
+		return ret;
+	}
+
+	ret = __v4l2_ctrl_handler_setup(hi847->sd.ctrl_handler);
+	if (ret)
+		return ret;
+
+	ret = hi847_write_reg(hi847, HI847_REG_MODE_TG,
+			      HI847_REG_VALUE_16BIT, HI847_REG_MODE_TG_ENABLE);
+
+	ret = hi847_write_reg(hi847, HI847_REG_MODE_SELECT,
+			      HI847_REG_VALUE_16BIT, HI847_MODE_STREAMING);
+
+	if (ret) {
+		dev_err(&client->dev, "failed to set stream");
+		return ret;
+	}
+
+	return 0;
+}
+
+static void hi847_stop_streaming(struct hi847 *hi847)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&hi847->sd);
+
+	if (hi847_write_reg(hi847, HI847_REG_MODE_TG,
+			    HI847_REG_VALUE_16BIT, HI847_REG_MODE_TG_DISABLE))
+		dev_err(&client->dev, "failed to set stream 0x%x",
+			HI847_REG_MODE_TG);
+
+	if (hi847_write_reg(hi847, HI847_REG_MODE_SELECT,
+			    HI847_REG_VALUE_16BIT, HI847_MODE_STANDBY))
+		dev_err(&client->dev, "failed to set stream 0x%x",
+		HI847_REG_MODE_SELECT);
+}
+
+static int hi847_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct hi847 *hi847 = to_hi847(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	if (hi847->streaming == enable)
+		return 0;
+
+	mutex_lock(&hi847->mutex);
+	if (enable) {
+		ret = pm_runtime_get_sync(&client->dev);
+		if (ret < 0) {
+			pm_runtime_put_noidle(&client->dev);
+			mutex_unlock(&hi847->mutex);
+			return ret;
+		}
+
+		ret = hi847_start_streaming(hi847);
+		if (ret) {
+			enable = 0;
+			hi847_stop_streaming(hi847);
+			pm_runtime_put(&client->dev);
+		}
+	} else {
+		hi847_stop_streaming(hi847);
+		pm_runtime_put(&client->dev);
+	}
+
+	hi847->streaming = enable;
+	mutex_unlock(&hi847->mutex);
+
+	return ret;
+}
+
+static int __maybe_unused hi847_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct hi847 *hi847 = to_hi847(sd);
+
+	mutex_lock(&hi847->mutex);
+	if (hi847->streaming)
+		hi847_stop_streaming(hi847);
+
+	mutex_unlock(&hi847->mutex);
+
+	return 0;
+}
+
+static int __maybe_unused hi847_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct hi847 *hi847 = to_hi847(sd);
+	int ret;
+
+	mutex_lock(&hi847->mutex);
+	if (hi847->streaming) {
+		ret = hi847_start_streaming(hi847);
+		if (ret)
+			goto error;
+	}
+
+	mutex_unlock(&hi847->mutex);
+
+	return 0;
+
+error:
+	hi847_stop_streaming(hi847);
+	hi847->streaming = 0;
+	mutex_unlock(&hi847->mutex);
+	return ret;
+}
+
+static int hi847_set_format(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_state *sd_state,
+			    struct v4l2_subdev_format *fmt)
+{
+	struct hi847 *hi847 = to_hi847(sd);
+	const struct hi847_mode *mode;
+	s32 vblank_def, h_blank;
+
+	mode = v4l2_find_nearest_size(supported_modes,
+				      ARRAY_SIZE(supported_modes), width,
+				      height, fmt->format.width,
+				      fmt->format.height);
+
+	mutex_lock(&hi847->mutex);
+	hi847_assign_pad_format(mode, &fmt->format);
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		*v4l2_subdev_get_try_format(sd, sd_state, fmt->pad) =
+			fmt->format;
+	} else {
+		hi847->cur_mode = mode;
+		__v4l2_ctrl_s_ctrl(hi847->link_freq, mode->link_freq_index);
+		__v4l2_ctrl_s_ctrl_int64(hi847->pixel_rate,
+					 to_pixel_rate(mode->link_freq_index));
+
+		/* Update limits and set FPS to default */
+		vblank_def = mode->fll_def - mode->height;
+		__v4l2_ctrl_modify_range(hi847->vblank,
+					 mode->fll_min - mode->height,
+					 HI847_FLL_MAX - mode->height, 1,
+					 vblank_def);
+		__v4l2_ctrl_s_ctrl(hi847->vblank, vblank_def);
+
+		h_blank = hi847->cur_mode->llp - hi847->cur_mode->width;
+
+		__v4l2_ctrl_modify_range(hi847->hblank, h_blank, h_blank, 1,
+					 h_blank);
+	}
+
+	mutex_unlock(&hi847->mutex);
+
+	return 0;
+}
+
+static int hi847_get_format(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_state *sd_state,
+			    struct v4l2_subdev_format *fmt)
+{
+	struct hi847 *hi847 = to_hi847(sd);
+
+	mutex_lock(&hi847->mutex);
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
+		fmt->format = *v4l2_subdev_get_try_format(&hi847->sd,
+							  sd_state,
+							  fmt->pad);
+	else
+		hi847_assign_pad_format(hi847->cur_mode, &fmt->format);
+
+	mutex_unlock(&hi847->mutex);
+
+	return 0;
+}
+
+static int hi847_enum_mbus_code(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *sd_state,
+				struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index > 0)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+
+	return 0;
+}
+
+static int hi847_enum_frame_size(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->index >= ARRAY_SIZE(supported_modes))
+		return -EINVAL;
+
+	if (fse->code != MEDIA_BUS_FMT_SGRBG10_1X10)
+		return -EINVAL;
+
+	fse->min_width = supported_modes[fse->index].width;
+	fse->max_width = fse->min_width;
+	fse->min_height = supported_modes[fse->index].height;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static int hi847_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct hi847 *hi847 = to_hi847(sd);
+
+	mutex_lock(&hi847->mutex);
+	hi847_assign_pad_format(&supported_modes[0],
+				v4l2_subdev_get_try_format(sd, fh->state, 0));
+	mutex_unlock(&hi847->mutex);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops hi847_video_ops = {
+	.s_stream = hi847_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops hi847_pad_ops = {
+	.set_fmt = hi847_set_format,
+	.get_fmt = hi847_get_format,
+	.enum_mbus_code = hi847_enum_mbus_code,
+	.enum_frame_size = hi847_enum_frame_size,
+};
+
+static const struct v4l2_subdev_ops hi847_subdev_ops = {
+	.video = &hi847_video_ops,
+	.pad = &hi847_pad_ops,
+};
+
+static const struct media_entity_operations hi847_subdev_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static const struct v4l2_subdev_internal_ops hi847_internal_ops = {
+	.open = hi847_open,
+};
+
+static int hi847_identify_module(struct hi847 *hi847)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&hi847->sd);
+	int ret;
+	u32 val;
+
+	ret = hi847_read_reg(hi847, HI847_REG_CHIP_ID,
+			     HI847_REG_VALUE_16BIT, &val);
+	if (ret)
+		return ret;
+
+	if (val != HI847_CHIP_ID) {
+		dev_err(&client->dev, "chip id mismatch: %x!=%x",
+			HI847_CHIP_ID, val);
+		return -ENXIO;
+	}
+
+	return 0;
+}
+
+static int hi847_check_hwcfg(struct device *dev)
+{
+	struct fwnode_handle *ep;
+	struct fwnode_handle *fwnode = dev_fwnode(dev);
+	struct v4l2_fwnode_endpoint bus_cfg = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY
+	};
+	u32 mclk;
+	int ret;
+	unsigned int i, j;
+
+	if (!fwnode)
+		return -ENXIO;
+
+	ret = fwnode_property_read_u32(fwnode, "clock-frequency", &mclk);
+	if (ret) {
+		dev_err(dev, "can't get clock frequency");
+		return ret;
+	}
+
+	if (mclk != HI847_MCLK) {
+		dev_err(dev, "external clock %d is not supported", mclk);
+		return -EINVAL;
+	}
+
+	ep = fwnode_graph_get_next_endpoint(fwnode, NULL);
+	if (!ep)
+		return -ENXIO;
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(ep, &bus_cfg);
+	fwnode_handle_put(ep);
+	if (ret)
+		return ret;
+
+	if (bus_cfg.bus.mipi_csi2.num_data_lanes != HI847_DATA_LANES) {
+		dev_err(dev, "number of CSI2 data lanes %d is not supported",
+			bus_cfg.bus.mipi_csi2.num_data_lanes);
+		ret = -EINVAL;
+		goto check_hwcfg_error;
+	}
+
+	if (!bus_cfg.nr_of_link_frequencies) {
+		dev_err(dev, "no link frequencies defined");
+		ret = -EINVAL;
+		goto check_hwcfg_error;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(link_freq_menu_items); i++) {
+		for (j = 0; j < bus_cfg.nr_of_link_frequencies; j++) {
+			if (link_freq_menu_items[i] ==
+				bus_cfg.link_frequencies[j])
+				break;
+		}
+
+		if (j == bus_cfg.nr_of_link_frequencies) {
+			dev_err(dev, "no link frequency %lld supported",
+				link_freq_menu_items[i]);
+			ret = -EINVAL;
+			goto check_hwcfg_error;
+		}
+	}
+
+check_hwcfg_error:
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+
+	return ret;
+}
+
+static void hi847_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct hi847 *hi847 = to_hi847(sd);
+
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+	pm_runtime_disable(&client->dev);
+	mutex_destroy(&hi847->mutex);
+}
+
+static int hi847_probe(struct i2c_client *client)
+{
+	struct hi847 *hi847;
+	int ret;
+
+	hi847 = devm_kzalloc(&client->dev, sizeof(*hi847), GFP_KERNEL);
+	if (!hi847)
+		return -ENOMEM;
+
+	ret = hi847_check_hwcfg(&client->dev);
+	if (ret) {
+		dev_err(&client->dev, "failed to get HW configuration: %d",
+			ret);
+		return ret;
+	}
+
+	v4l2_i2c_subdev_init(&hi847->sd, client, &hi847_subdev_ops);
+	ret = hi847_identify_module(hi847);
+	if (ret) {
+		dev_err(&client->dev, "failed to find sensor: %d", ret);
+		return ret;
+	}
+
+	mutex_init(&hi847->mutex);
+	hi847->cur_mode = &supported_modes[0];
+	ret = hi847_init_controls(hi847);
+	if (ret) {
+		dev_err(&client->dev, "failed to init controls: %d", ret);
+		goto probe_error_v4l2_ctrl_handler_free;
+	}
+
+	hi847->sd.internal_ops = &hi847_internal_ops;
+	hi847->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	hi847->sd.entity.ops = &hi847_subdev_entity_ops;
+	hi847->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	hi847->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&hi847->sd.entity, 1, &hi847->pad);
+	if (ret) {
+		dev_err(&client->dev, "failed to init entity pads: %d", ret);
+		goto probe_error_v4l2_ctrl_handler_free;
+	}
+
+	ret = v4l2_async_register_subdev_sensor(&hi847->sd);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed to register V4L2 subdev: %d",
+			ret);
+		goto probe_error_media_entity_cleanup;
+	}
+
+	pm_runtime_set_active(&client->dev);
+	pm_runtime_enable(&client->dev);
+	pm_runtime_idle(&client->dev);
+
+	return 0;
+
+probe_error_media_entity_cleanup:
+	media_entity_cleanup(&hi847->sd.entity);
+
+probe_error_v4l2_ctrl_handler_free:
+	v4l2_ctrl_handler_free(hi847->sd.ctrl_handler);
+	mutex_destroy(&hi847->mutex);
+
+	return ret;
+}
+
+static const struct dev_pm_ops hi847_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(hi847_suspend, hi847_resume)
+};
+
+#ifdef CONFIG_ACPI
+static const struct acpi_device_id hi847_acpi_ids[] = {
+	{"HYV0847"},
+	{}
+};
+
+MODULE_DEVICE_TABLE(acpi, hi847_acpi_ids);
+#endif
+
+static struct i2c_driver hi847_i2c_driver = {
+	.driver = {
+		.name = "hi847",
+		.pm = &hi847_pm_ops,
+		.acpi_match_table = ACPI_PTR(hi847_acpi_ids),
+	},
+	.probe = hi847_probe,
+	.remove = hi847_remove,
+};
+
+module_i2c_driver(hi847_i2c_driver);
+
+MODULE_AUTHOR("Shawn Tu");
+MODULE_DESCRIPTION("Hynix HI847 sensor driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/imx208.c b/drivers/media/i2c/imx208.c
new file mode 100644
index 000000000..ee5a28675
--- /dev/null
+++ b/drivers/media/i2c/imx208.c
@@ -0,0 +1,1114 @@
+// SPDX-License-Identifier: GPL-2.0
+// Copyright (C) 2021 Intel Corporation
+
+#include <linux/acpi.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <asm/unaligned.h>
+
+#define IMX208_REG_MODE_SELECT		0x0100
+#define IMX208_MODE_STANDBY		0x00
+#define IMX208_MODE_STREAMING		0x01
+
+/* Chip ID */
+#define IMX208_REG_CHIP_ID		0x0000
+#define IMX208_CHIP_ID			0x0208
+
+/* V_TIMING internal */
+#define IMX208_REG_VTS			0x0340
+#define IMX208_VTS_60FPS		0x0472
+#define IMX208_VTS_BINNING		0x0239
+#define IMX208_VTS_60FPS_MIN		0x0458
+#define IMX208_VTS_BINNING_MIN		0x0230
+#define IMX208_VTS_MAX			0xffff
+
+/* HBLANK control - read only */
+#define IMX208_PPL_384MHZ		2248
+#define IMX208_PPL_96MHZ		2248
+
+/* Exposure control */
+#define IMX208_REG_EXPOSURE		0x0202
+#define IMX208_EXPOSURE_MIN		4
+#define IMX208_EXPOSURE_STEP		1
+#define IMX208_EXPOSURE_DEFAULT		0x190
+#define IMX208_EXPOSURE_MAX		65535
+
+/* Analog gain control */
+#define IMX208_REG_ANALOG_GAIN		0x0204
+#define IMX208_ANA_GAIN_MIN		0
+#define IMX208_ANA_GAIN_MAX		0x00e0
+#define IMX208_ANA_GAIN_STEP		1
+#define IMX208_ANA_GAIN_DEFAULT		0x0
+
+/* Digital gain control */
+#define IMX208_REG_GR_DIGITAL_GAIN	0x020e
+#define IMX208_REG_R_DIGITAL_GAIN	0x0210
+#define IMX208_REG_B_DIGITAL_GAIN	0x0212
+#define IMX208_REG_GB_DIGITAL_GAIN	0x0214
+#define IMX208_DIGITAL_GAIN_SHIFT	8
+
+/* Orientation */
+#define IMX208_REG_ORIENTATION_CONTROL	0x0101
+
+/* Test Pattern Control */
+#define IMX208_REG_TEST_PATTERN_MODE	0x0600
+#define IMX208_TEST_PATTERN_DISABLE	0x0
+#define IMX208_TEST_PATTERN_SOLID_COLOR	0x1
+#define IMX208_TEST_PATTERN_COLOR_BARS	0x2
+#define IMX208_TEST_PATTERN_GREY_COLOR	0x3
+#define IMX208_TEST_PATTERN_PN9		0x4
+#define IMX208_TEST_PATTERN_FIX_1	0x100
+#define IMX208_TEST_PATTERN_FIX_2	0x101
+#define IMX208_TEST_PATTERN_FIX_3	0x102
+#define IMX208_TEST_PATTERN_FIX_4	0x103
+#define IMX208_TEST_PATTERN_FIX_5	0x104
+#define IMX208_TEST_PATTERN_FIX_6	0x105
+
+/* OTP Access */
+#define IMX208_OTP_BASE			0x3500
+#define IMX208_OTP_SIZE			40
+
+struct imx208_reg {
+	u16 address;
+	u8 val;
+};
+
+struct imx208_reg_list {
+	u32 num_of_regs;
+	const struct imx208_reg *regs;
+};
+
+/* Link frequency config */
+struct imx208_link_freq_config {
+	u32 pixels_per_line;
+
+	/* PLL registers for this link frequency */
+	struct imx208_reg_list reg_list;
+};
+
+/* Mode : resolution and related config&values */
+struct imx208_mode {
+	/* Frame width */
+	u32 width;
+	/* Frame height */
+	u32 height;
+
+	/* V-timing */
+	u32 vts_def;
+	u32 vts_min;
+
+	/* Index of Link frequency config to be used */
+	u32 link_freq_index;
+	/* Default register values */
+	struct imx208_reg_list reg_list;
+};
+
+static const struct imx208_reg pll_ctrl_reg[] = {
+	{0x0305, 0x02},
+	{0x0307, 0x50},
+	{0x303C, 0x3C},
+};
+
+static const struct imx208_reg mode_1936x1096_60fps_regs[] = {
+	{0x0340, 0x04},
+	{0x0341, 0x72},
+	{0x0342, 0x04},
+	{0x0343, 0x64},
+	{0x034C, 0x07},
+	{0x034D, 0x90},
+	{0x034E, 0x04},
+	{0x034F, 0x48},
+	{0x0381, 0x01},
+	{0x0383, 0x01},
+	{0x0385, 0x01},
+	{0x0387, 0x01},
+	{0x3048, 0x00},
+	{0x3050, 0x01},
+	{0x30D5, 0x00},
+	{0x3301, 0x00},
+	{0x3318, 0x62},
+	{0x0202, 0x01},
+	{0x0203, 0x90},
+	{0x0205, 0x00},
+};
+
+static const struct imx208_reg mode_968_548_60fps_regs[] = {
+	{0x0340, 0x02},
+	{0x0341, 0x39},
+	{0x0342, 0x08},
+	{0x0343, 0xC8},
+	{0x034C, 0x03},
+	{0x034D, 0xC8},
+	{0x034E, 0x02},
+	{0x034F, 0x24},
+	{0x0381, 0x01},
+	{0x0383, 0x03},
+	{0x0385, 0x01},
+	{0x0387, 0x03},
+	{0x3048, 0x01},
+	{0x3050, 0x02},
+	{0x30D5, 0x03},
+	{0x3301, 0x10},
+	{0x3318, 0x75},
+	{0x0202, 0x01},
+	{0x0203, 0x90},
+	{0x0205, 0x00},
+};
+
+static const s64 imx208_discrete_digital_gain[] = {
+	1, 2, 4, 8, 16,
+};
+
+static const char * const imx208_test_pattern_menu[] = {
+	"Disabled",
+	"Solid Color",
+	"100% Color Bar",
+	"Fade to Grey Color Bar",
+	"PN9",
+	"Fixed Pattern1",
+	"Fixed Pattern2",
+	"Fixed Pattern3",
+	"Fixed Pattern4",
+	"Fixed Pattern5",
+	"Fixed Pattern6"
+};
+
+static const int imx208_test_pattern_val[] = {
+	IMX208_TEST_PATTERN_DISABLE,
+	IMX208_TEST_PATTERN_SOLID_COLOR,
+	IMX208_TEST_PATTERN_COLOR_BARS,
+	IMX208_TEST_PATTERN_GREY_COLOR,
+	IMX208_TEST_PATTERN_PN9,
+	IMX208_TEST_PATTERN_FIX_1,
+	IMX208_TEST_PATTERN_FIX_2,
+	IMX208_TEST_PATTERN_FIX_3,
+	IMX208_TEST_PATTERN_FIX_4,
+	IMX208_TEST_PATTERN_FIX_5,
+	IMX208_TEST_PATTERN_FIX_6,
+};
+
+/* Configurations for supported link frequencies */
+#define IMX208_MHZ			(1000 * 1000ULL)
+#define IMX208_LINK_FREQ_384MHZ		(384ULL * IMX208_MHZ)
+#define IMX208_LINK_FREQ_96MHZ		(96ULL * IMX208_MHZ)
+
+#define IMX208_DATA_RATE_DOUBLE		2
+#define IMX208_NUM_OF_LANES		2
+#define IMX208_PIXEL_BITS		10
+
+enum {
+	IMX208_LINK_FREQ_384MHZ_INDEX,
+	IMX208_LINK_FREQ_96MHZ_INDEX,
+};
+
+/*
+ * pixel_rate = link_freq * data-rate * nr_of_lanes / bits_per_sample
+ * data rate => double data rate; number of lanes => 2; bits per pixel => 10
+ */
+static u64 link_freq_to_pixel_rate(u64 f)
+{
+	f *= IMX208_DATA_RATE_DOUBLE * IMX208_NUM_OF_LANES;
+	do_div(f, IMX208_PIXEL_BITS);
+
+	return f;
+}
+
+/* Menu items for LINK_FREQ V4L2 control */
+static const s64 link_freq_menu_items[] = {
+	[IMX208_LINK_FREQ_384MHZ_INDEX] = IMX208_LINK_FREQ_384MHZ,
+	[IMX208_LINK_FREQ_96MHZ_INDEX] = IMX208_LINK_FREQ_96MHZ,
+};
+
+/* Link frequency configs */
+static const struct imx208_link_freq_config link_freq_configs[] = {
+	[IMX208_LINK_FREQ_384MHZ_INDEX] = {
+		.pixels_per_line = IMX208_PPL_384MHZ,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(pll_ctrl_reg),
+			.regs = pll_ctrl_reg,
+		}
+	},
+	[IMX208_LINK_FREQ_96MHZ_INDEX] = {
+		.pixels_per_line = IMX208_PPL_96MHZ,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(pll_ctrl_reg),
+			.regs = pll_ctrl_reg,
+		}
+	},
+};
+
+/* Mode configs */
+static const struct imx208_mode supported_modes[] = {
+	{
+		.width = 1936,
+		.height = 1096,
+		.vts_def = IMX208_VTS_60FPS,
+		.vts_min = IMX208_VTS_60FPS_MIN,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1936x1096_60fps_regs),
+			.regs = mode_1936x1096_60fps_regs,
+		},
+		.link_freq_index = IMX208_LINK_FREQ_384MHZ_INDEX,
+	},
+	{
+		.width = 968,
+		.height = 548,
+		.vts_def = IMX208_VTS_BINNING,
+		.vts_min = IMX208_VTS_BINNING_MIN,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_968_548_60fps_regs),
+			.regs = mode_968_548_60fps_regs,
+		},
+		.link_freq_index = IMX208_LINK_FREQ_96MHZ_INDEX,
+	},
+};
+
+struct imx208 {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+
+	struct v4l2_ctrl_handler ctrl_handler;
+	/* V4L2 Controls */
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *pixel_rate;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *vflip;
+	struct v4l2_ctrl *hflip;
+
+	/* Current mode */
+	const struct imx208_mode *cur_mode;
+
+	/*
+	 * Mutex for serialized access:
+	 * Protect sensor set pad format and start/stop streaming safely.
+	 * Protect access to sensor v4l2 controls.
+	 */
+	struct mutex imx208_mx;
+
+	/* Streaming on/off */
+	bool streaming;
+
+	/* OTP data */
+	bool otp_read;
+	char otp_data[IMX208_OTP_SIZE];
+
+	/* True if the device has been identified */
+	bool identified;
+};
+
+static inline struct imx208 *to_imx208(struct v4l2_subdev *_sd)
+{
+	return container_of(_sd, struct imx208, sd);
+}
+
+/* Get bayer order based on flip setting. */
+static u32 imx208_get_format_code(struct imx208 *imx208)
+{
+	/*
+	 * Only one bayer order is supported.
+	 * It depends on the flip settings.
+	 */
+	static const u32 codes[2][2] = {
+		{ MEDIA_BUS_FMT_SRGGB10_1X10, MEDIA_BUS_FMT_SGRBG10_1X10, },
+		{ MEDIA_BUS_FMT_SGBRG10_1X10, MEDIA_BUS_FMT_SBGGR10_1X10, },
+	};
+
+	return codes[imx208->vflip->val][imx208->hflip->val];
+}
+
+/* Read registers up to 4 at a time */
+static int imx208_read_reg(struct imx208 *imx208, u16 reg, u32 len, u32 *val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx208->sd);
+	struct i2c_msg msgs[2];
+	u8 addr_buf[2] = { reg >> 8, reg & 0xff };
+	u8 data_buf[4] = { 0, };
+	int ret;
+
+	if (len > 4)
+		return -EINVAL;
+
+	/* Write register address */
+	msgs[0].addr = client->addr;
+	msgs[0].flags = 0;
+	msgs[0].len = ARRAY_SIZE(addr_buf);
+	msgs[0].buf = addr_buf;
+
+	/* Read data from register */
+	msgs[1].addr = client->addr;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].len = len;
+	msgs[1].buf = &data_buf[4 - len];
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret != ARRAY_SIZE(msgs))
+		return -EIO;
+
+	*val = get_unaligned_be32(data_buf);
+
+	return 0;
+}
+
+/* Write registers up to 4 at a time */
+static int imx208_write_reg(struct imx208 *imx208, u16 reg, u32 len, u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx208->sd);
+	u8 buf[6];
+
+	if (len > 4)
+		return -EINVAL;
+
+	put_unaligned_be16(reg, buf);
+	put_unaligned_be32(val << (8 * (4 - len)), buf + 2);
+	if (i2c_master_send(client, buf, len + 2) != len + 2)
+		return -EIO;
+
+	return 0;
+}
+
+/* Write a list of registers */
+static int imx208_write_regs(struct imx208 *imx208,
+			     const struct imx208_reg *regs, u32 len)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx208->sd);
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < len; i++) {
+		ret = imx208_write_reg(imx208, regs[i].address, 1,
+				       regs[i].val);
+		if (ret) {
+			dev_err_ratelimited(&client->dev,
+					    "Failed to write reg 0x%4.4x. error = %d\n",
+					    regs[i].address, ret);
+
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+/* Open sub-device */
+static int imx208_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_mbus_framefmt *try_fmt =
+		v4l2_subdev_get_try_format(sd, fh->state, 0);
+
+	/* Initialize try_fmt */
+	try_fmt->width = supported_modes[0].width;
+	try_fmt->height = supported_modes[0].height;
+	try_fmt->code = MEDIA_BUS_FMT_SRGGB10_1X10;
+	try_fmt->field = V4L2_FIELD_NONE;
+
+	return 0;
+}
+
+static int imx208_update_digital_gain(struct imx208 *imx208, u32 len, u32 val)
+{
+	int ret;
+
+	val = imx208_discrete_digital_gain[val] << IMX208_DIGITAL_GAIN_SHIFT;
+
+	ret = imx208_write_reg(imx208, IMX208_REG_GR_DIGITAL_GAIN, 2, val);
+	if (ret)
+		return ret;
+
+	ret = imx208_write_reg(imx208, IMX208_REG_GB_DIGITAL_GAIN, 2, val);
+	if (ret)
+		return ret;
+
+	ret = imx208_write_reg(imx208, IMX208_REG_R_DIGITAL_GAIN, 2, val);
+	if (ret)
+		return ret;
+
+	return imx208_write_reg(imx208, IMX208_REG_B_DIGITAL_GAIN, 2, val);
+}
+
+static int imx208_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct imx208 *imx208 =
+		container_of(ctrl->handler, struct imx208, ctrl_handler);
+	struct i2c_client *client = v4l2_get_subdevdata(&imx208->sd);
+	int ret;
+
+	/*
+	 * Applying V4L2 control value only happens
+	 * when power is up for streaming
+	 */
+	if (!pm_runtime_get_if_in_use(&client->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_ANALOGUE_GAIN:
+		ret = imx208_write_reg(imx208, IMX208_REG_ANALOG_GAIN,
+				       2, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE:
+		ret = imx208_write_reg(imx208, IMX208_REG_EXPOSURE,
+				       2, ctrl->val);
+		break;
+	case V4L2_CID_DIGITAL_GAIN:
+		ret = imx208_update_digital_gain(imx208, 2, ctrl->val);
+		break;
+	case V4L2_CID_VBLANK:
+		/* Update VTS that meets expected vertical blanking */
+		ret = imx208_write_reg(imx208, IMX208_REG_VTS, 2,
+				       imx208->cur_mode->height + ctrl->val);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		ret = imx208_write_reg(imx208, IMX208_REG_TEST_PATTERN_MODE,
+				       2, imx208_test_pattern_val[ctrl->val]);
+		break;
+	case V4L2_CID_HFLIP:
+	case V4L2_CID_VFLIP:
+		ret = imx208_write_reg(imx208, IMX208_REG_ORIENTATION_CONTROL,
+				       1,
+				       imx208->hflip->val |
+				       imx208->vflip->val << 1);
+		break;
+	default:
+		ret = -EINVAL;
+		dev_err(&client->dev,
+			"ctrl(id:0x%x,val:0x%x) is not handled\n",
+			ctrl->id, ctrl->val);
+		break;
+	}
+
+	pm_runtime_put(&client->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops imx208_ctrl_ops = {
+	.s_ctrl = imx208_set_ctrl,
+};
+
+static const struct v4l2_ctrl_config imx208_digital_gain_control = {
+	.ops = &imx208_ctrl_ops,
+	.id = V4L2_CID_DIGITAL_GAIN,
+	.name = "Digital Gain",
+	.type = V4L2_CTRL_TYPE_INTEGER_MENU,
+	.min = 0,
+	.max = ARRAY_SIZE(imx208_discrete_digital_gain) - 1,
+	.step = 0,
+	.def = 0,
+	.menu_skip_mask = 0,
+	.qmenu_int = imx208_discrete_digital_gain,
+};
+
+static int imx208_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct imx208 *imx208 = to_imx208(sd);
+
+	if (code->index > 0)
+		return -EINVAL;
+
+	code->code = imx208_get_format_code(imx208);
+
+	return 0;
+}
+
+static int imx208_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct imx208 *imx208 = to_imx208(sd);
+
+	if (fse->index >= ARRAY_SIZE(supported_modes))
+		return -EINVAL;
+
+	if (fse->code != imx208_get_format_code(imx208))
+		return -EINVAL;
+
+	fse->min_width = supported_modes[fse->index].width;
+	fse->max_width = fse->min_width;
+	fse->min_height = supported_modes[fse->index].height;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static void imx208_mode_to_pad_format(struct imx208 *imx208,
+				      const struct imx208_mode *mode,
+				      struct v4l2_subdev_format *fmt)
+{
+	fmt->format.width = mode->width;
+	fmt->format.height = mode->height;
+	fmt->format.code = imx208_get_format_code(imx208);
+	fmt->format.field = V4L2_FIELD_NONE;
+}
+
+static int __imx208_get_pad_format(struct imx208 *imx208,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_format *fmt)
+{
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
+		fmt->format = *v4l2_subdev_get_try_format(&imx208->sd,
+							  sd_state,
+							  fmt->pad);
+	else
+		imx208_mode_to_pad_format(imx208, imx208->cur_mode, fmt);
+
+	return 0;
+}
+
+static int imx208_get_pad_format(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_format *fmt)
+{
+	struct imx208 *imx208 = to_imx208(sd);
+	int ret;
+
+	mutex_lock(&imx208->imx208_mx);
+	ret = __imx208_get_pad_format(imx208, sd_state, fmt);
+	mutex_unlock(&imx208->imx208_mx);
+
+	return ret;
+}
+
+static int imx208_set_pad_format(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_format *fmt)
+{
+	struct imx208 *imx208 = to_imx208(sd);
+	const struct imx208_mode *mode;
+	s32 vblank_def;
+	s32 vblank_min;
+	s64 h_blank;
+	s64 pixel_rate;
+	s64 link_freq;
+
+	mutex_lock(&imx208->imx208_mx);
+
+	fmt->format.code = imx208_get_format_code(imx208);
+	mode = v4l2_find_nearest_size(supported_modes,
+				      ARRAY_SIZE(supported_modes), width, height,
+				      fmt->format.width, fmt->format.height);
+	imx208_mode_to_pad_format(imx208, mode, fmt);
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		*v4l2_subdev_get_try_format(sd, sd_state, fmt->pad) = fmt->format;
+	} else {
+		imx208->cur_mode = mode;
+		__v4l2_ctrl_s_ctrl(imx208->link_freq, mode->link_freq_index);
+		link_freq = link_freq_menu_items[mode->link_freq_index];
+		pixel_rate = link_freq_to_pixel_rate(link_freq);
+		__v4l2_ctrl_s_ctrl_int64(imx208->pixel_rate, pixel_rate);
+		/* Update limits and set FPS to default */
+		vblank_def = imx208->cur_mode->vts_def -
+			     imx208->cur_mode->height;
+		vblank_min = imx208->cur_mode->vts_min -
+			     imx208->cur_mode->height;
+		__v4l2_ctrl_modify_range(imx208->vblank, vblank_min,
+					 IMX208_VTS_MAX - imx208->cur_mode->height,
+					 1, vblank_def);
+		__v4l2_ctrl_s_ctrl(imx208->vblank, vblank_def);
+		h_blank =
+			link_freq_configs[mode->link_freq_index].pixels_per_line
+			 - imx208->cur_mode->width;
+		__v4l2_ctrl_modify_range(imx208->hblank, h_blank,
+					 h_blank, 1, h_blank);
+	}
+
+	mutex_unlock(&imx208->imx208_mx);
+
+	return 0;
+}
+
+static int imx208_identify_module(struct imx208 *imx208)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx208->sd);
+	int ret;
+	u32 val;
+
+	if (imx208->identified)
+		return 0;
+
+	ret = imx208_read_reg(imx208, IMX208_REG_CHIP_ID,
+			      2, &val);
+	if (ret) {
+		dev_err(&client->dev, "failed to read chip id %x\n",
+			IMX208_CHIP_ID);
+		return ret;
+	}
+
+	if (val != IMX208_CHIP_ID) {
+		dev_err(&client->dev, "chip id mismatch: %x!=%x\n",
+			IMX208_CHIP_ID, val);
+		return -EIO;
+	}
+
+	imx208->identified = true;
+
+	return 0;
+}
+
+/* Start streaming */
+static int imx208_start_streaming(struct imx208 *imx208)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx208->sd);
+	const struct imx208_reg_list *reg_list;
+	int ret, link_freq_index;
+
+	ret = imx208_identify_module(imx208);
+	if (ret)
+		return ret;
+
+	/* Setup PLL */
+	link_freq_index = imx208->cur_mode->link_freq_index;
+	reg_list = &link_freq_configs[link_freq_index].reg_list;
+	ret = imx208_write_regs(imx208, reg_list->regs, reg_list->num_of_regs);
+	if (ret) {
+		dev_err(&client->dev, "%s failed to set plls\n", __func__);
+		return ret;
+	}
+
+	/* Apply default values of current mode */
+	reg_list = &imx208->cur_mode->reg_list;
+	ret = imx208_write_regs(imx208, reg_list->regs, reg_list->num_of_regs);
+	if (ret) {
+		dev_err(&client->dev, "%s failed to set mode\n", __func__);
+		return ret;
+	}
+
+	/* Apply customized values from user */
+	ret =  __v4l2_ctrl_handler_setup(imx208->sd.ctrl_handler);
+	if (ret)
+		return ret;
+
+	/* set stream on register */
+	return imx208_write_reg(imx208, IMX208_REG_MODE_SELECT,
+				1, IMX208_MODE_STREAMING);
+}
+
+/* Stop streaming */
+static int imx208_stop_streaming(struct imx208 *imx208)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx208->sd);
+	int ret;
+
+	/* set stream off register */
+	ret = imx208_write_reg(imx208, IMX208_REG_MODE_SELECT,
+			       1, IMX208_MODE_STANDBY);
+	if (ret)
+		dev_err(&client->dev, "%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 imx208_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct imx208 *imx208 = to_imx208(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	mutex_lock(&imx208->imx208_mx);
+	if (imx208->streaming == enable) {
+		mutex_unlock(&imx208->imx208_mx);
+		return 0;
+	}
+
+	if (enable) {
+		ret = pm_runtime_get_sync(&client->dev);
+		if (ret < 0)
+			goto err_rpm_put;
+
+		/*
+		 * Apply default & customized values
+		 * and then start streaming.
+		 */
+		ret = imx208_start_streaming(imx208);
+		if (ret)
+			goto err_rpm_put;
+	} else {
+		imx208_stop_streaming(imx208);
+		pm_runtime_put(&client->dev);
+	}
+
+	imx208->streaming = enable;
+	mutex_unlock(&imx208->imx208_mx);
+
+	/* vflip and hflip cannot change during streaming */
+	v4l2_ctrl_grab(imx208->vflip, enable);
+	v4l2_ctrl_grab(imx208->hflip, enable);
+
+	return ret;
+
+err_rpm_put:
+	pm_runtime_put(&client->dev);
+	mutex_unlock(&imx208->imx208_mx);
+
+	return ret;
+}
+
+static int __maybe_unused imx208_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct imx208 *imx208 = to_imx208(sd);
+
+	if (imx208->streaming)
+		imx208_stop_streaming(imx208);
+
+	return 0;
+}
+
+static int __maybe_unused imx208_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct imx208 *imx208 = to_imx208(sd);
+	int ret;
+
+	if (imx208->streaming) {
+		ret = imx208_start_streaming(imx208);
+		if (ret)
+			goto error;
+	}
+
+	return 0;
+
+error:
+	imx208_stop_streaming(imx208);
+	imx208->streaming = 0;
+
+	return ret;
+}
+
+/* Verify chip ID */
+static const struct v4l2_subdev_video_ops imx208_video_ops = {
+	.s_stream = imx208_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops imx208_pad_ops = {
+	.enum_mbus_code = imx208_enum_mbus_code,
+	.get_fmt = imx208_get_pad_format,
+	.set_fmt = imx208_set_pad_format,
+	.enum_frame_size = imx208_enum_frame_size,
+};
+
+static const struct v4l2_subdev_ops imx208_subdev_ops = {
+	.video = &imx208_video_ops,
+	.pad = &imx208_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops imx208_internal_ops = {
+	.open = imx208_open,
+};
+
+static int imx208_read_otp(struct imx208 *imx208)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx208->sd);
+	struct i2c_msg msgs[2];
+	u8 addr_buf[2] = { IMX208_OTP_BASE >> 8, IMX208_OTP_BASE & 0xff };
+	int ret = 0;
+
+	mutex_lock(&imx208->imx208_mx);
+
+	if (imx208->otp_read)
+		goto out_unlock;
+
+	ret = pm_runtime_get_sync(&client->dev);
+	if (ret < 0) {
+		pm_runtime_put_noidle(&client->dev);
+		goto out_unlock;
+	}
+
+	ret = imx208_identify_module(imx208);
+	if (ret)
+		goto out_pm_put;
+
+	/* Write register address */
+	msgs[0].addr = client->addr;
+	msgs[0].flags = 0;
+	msgs[0].len = ARRAY_SIZE(addr_buf);
+	msgs[0].buf = addr_buf;
+
+	/* Read data from registers */
+	msgs[1].addr = client->addr;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].len = sizeof(imx208->otp_data);
+	msgs[1].buf = imx208->otp_data;
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret == ARRAY_SIZE(msgs)) {
+		imx208->otp_read = true;
+		ret = 0;
+	}
+
+out_pm_put:
+	pm_runtime_put(&client->dev);
+
+out_unlock:
+	mutex_unlock(&imx208->imx208_mx);
+
+	return ret;
+}
+
+static ssize_t otp_read(struct file *filp, struct kobject *kobj,
+			struct bin_attribute *bin_attr,
+			char *buf, loff_t off, size_t count)
+{
+	struct i2c_client *client = to_i2c_client(kobj_to_dev(kobj));
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct imx208 *imx208 = to_imx208(sd);
+	int ret;
+
+	ret = imx208_read_otp(imx208);
+	if (ret)
+		return ret;
+
+	memcpy(buf, &imx208->otp_data[off], count);
+	return count;
+}
+
+static const BIN_ATTR_RO(otp, IMX208_OTP_SIZE);
+
+/* Initialize control handlers */
+static int imx208_init_controls(struct imx208 *imx208)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx208->sd);
+	struct v4l2_ctrl_handler *ctrl_hdlr = &imx208->ctrl_handler;
+	s64 exposure_max;
+	s64 vblank_def;
+	s64 vblank_min;
+	s64 pixel_rate_min;
+	s64 pixel_rate_max;
+	int ret;
+
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 8);
+	if (ret)
+		return ret;
+
+	mutex_init(&imx208->imx208_mx);
+	ctrl_hdlr->lock = &imx208->imx208_mx;
+	imx208->link_freq =
+		v4l2_ctrl_new_int_menu(ctrl_hdlr,
+				       &imx208_ctrl_ops,
+				       V4L2_CID_LINK_FREQ,
+				       ARRAY_SIZE(link_freq_menu_items) - 1,
+				       0, link_freq_menu_items);
+
+	if (imx208->link_freq)
+		imx208->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	pixel_rate_max = link_freq_to_pixel_rate(link_freq_menu_items[0]);
+	pixel_rate_min =
+		link_freq_to_pixel_rate(link_freq_menu_items[ARRAY_SIZE(link_freq_menu_items) - 1]);
+	/* By default, PIXEL_RATE is read only */
+	imx208->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, &imx208_ctrl_ops,
+					       V4L2_CID_PIXEL_RATE,
+					       pixel_rate_min, pixel_rate_max,
+					       1, pixel_rate_max);
+
+	vblank_def = imx208->cur_mode->vts_def - imx208->cur_mode->height;
+	vblank_min = imx208->cur_mode->vts_min - imx208->cur_mode->height;
+	imx208->vblank =
+		v4l2_ctrl_new_std(ctrl_hdlr, &imx208_ctrl_ops, V4L2_CID_VBLANK,
+				  vblank_min,
+				  IMX208_VTS_MAX - imx208->cur_mode->height, 1,
+				  vblank_def);
+
+	imx208->hblank =
+		v4l2_ctrl_new_std(ctrl_hdlr, &imx208_ctrl_ops, V4L2_CID_HBLANK,
+				  IMX208_PPL_384MHZ - imx208->cur_mode->width,
+				  IMX208_PPL_384MHZ - imx208->cur_mode->width,
+				  1,
+				  IMX208_PPL_384MHZ - imx208->cur_mode->width);
+
+	if (imx208->hblank)
+		imx208->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	exposure_max = imx208->cur_mode->vts_def - 8;
+	v4l2_ctrl_new_std(ctrl_hdlr, &imx208_ctrl_ops, V4L2_CID_EXPOSURE,
+			  IMX208_EXPOSURE_MIN, exposure_max,
+			  IMX208_EXPOSURE_STEP, IMX208_EXPOSURE_DEFAULT);
+
+	imx208->hflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx208_ctrl_ops,
+					  V4L2_CID_HFLIP, 0, 1, 1, 0);
+	if (imx208->hflip)
+		imx208->hflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
+	imx208->vflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx208_ctrl_ops,
+					  V4L2_CID_VFLIP, 0, 1, 1, 0);
+	if (imx208->vflip)
+		imx208->vflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &imx208_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
+			  IMX208_ANA_GAIN_MIN, IMX208_ANA_GAIN_MAX,
+			  IMX208_ANA_GAIN_STEP, IMX208_ANA_GAIN_DEFAULT);
+
+	v4l2_ctrl_new_custom(ctrl_hdlr, &imx208_digital_gain_control, NULL);
+
+	v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &imx208_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(imx208_test_pattern_menu) - 1,
+				     0, 0, imx208_test_pattern_menu);
+
+	if (ctrl_hdlr->error) {
+		ret = ctrl_hdlr->error;
+		dev_err(&client->dev, "%s control init failed (%d)\n",
+			__func__, ret);
+		goto error;
+	}
+
+	imx208->sd.ctrl_handler = ctrl_hdlr;
+
+	return 0;
+
+error:
+	v4l2_ctrl_handler_free(ctrl_hdlr);
+	mutex_destroy(&imx208->imx208_mx);
+
+	return ret;
+}
+
+static void imx208_free_controls(struct imx208 *imx208)
+{
+	v4l2_ctrl_handler_free(imx208->sd.ctrl_handler);
+}
+
+static int imx208_probe(struct i2c_client *client)
+{
+	struct imx208 *imx208;
+	int ret;
+	bool full_power;
+	u32 val = 0;
+
+	device_property_read_u32(&client->dev, "clock-frequency", &val);
+	if (val != 19200000) {
+		dev_err(&client->dev,
+			"Unsupported clock-frequency %u. Expected 19200000.\n",
+			val);
+		return -EINVAL;
+	}
+
+	imx208 = devm_kzalloc(&client->dev, sizeof(*imx208), GFP_KERNEL);
+	if (!imx208)
+		return -ENOMEM;
+
+	/* Initialize subdev */
+	v4l2_i2c_subdev_init(&imx208->sd, client, &imx208_subdev_ops);
+
+	full_power = acpi_dev_state_d0(&client->dev);
+	if (full_power) {
+		/* Check module identity */
+		ret = imx208_identify_module(imx208);
+		if (ret) {
+			dev_err(&client->dev, "failed to find sensor: %d", ret);
+			goto error_probe;
+		}
+	}
+
+	/* Set default mode to max resolution */
+	imx208->cur_mode = &supported_modes[0];
+
+	ret = imx208_init_controls(imx208);
+	if (ret) {
+		dev_err(&client->dev, "failed to init controls: %d", ret);
+		goto error_probe;
+	}
+
+	/* Initialize subdev */
+	imx208->sd.internal_ops = &imx208_internal_ops;
+	imx208->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	imx208->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+	/* Initialize source pad */
+	imx208->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&imx208->sd.entity, 1, &imx208->pad);
+	if (ret) {
+		dev_err(&client->dev, "%s failed:%d\n", __func__, ret);
+		goto error_handler_free;
+	}
+
+	ret = v4l2_async_register_subdev_sensor(&imx208->sd);
+	if (ret < 0)
+		goto error_media_entity;
+
+	ret = device_create_bin_file(&client->dev, &bin_attr_otp);
+	if (ret) {
+		dev_err(&client->dev, "sysfs otp creation failed\n");
+		goto error_async_subdev;
+	}
+
+	/* Set the device's state to active if it's in D0 state. */
+	if (full_power)
+		pm_runtime_set_active(&client->dev);
+	pm_runtime_enable(&client->dev);
+	pm_runtime_idle(&client->dev);
+
+	return 0;
+
+error_async_subdev:
+	v4l2_async_unregister_subdev(&imx208->sd);
+
+error_media_entity:
+	media_entity_cleanup(&imx208->sd.entity);
+
+error_handler_free:
+	imx208_free_controls(imx208);
+
+error_probe:
+	mutex_destroy(&imx208->imx208_mx);
+
+	return ret;
+}
+
+static void imx208_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct imx208 *imx208 = to_imx208(sd);
+
+	device_remove_bin_file(&client->dev, &bin_attr_otp);
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	imx208_free_controls(imx208);
+
+	pm_runtime_disable(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+
+	mutex_destroy(&imx208->imx208_mx);
+}
+
+static const struct dev_pm_ops imx208_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(imx208_suspend, imx208_resume)
+};
+
+#ifdef CONFIG_ACPI
+static const struct acpi_device_id imx208_acpi_ids[] = {
+	{ "INT3478" },
+	{ /* sentinel */ }
+};
+
+MODULE_DEVICE_TABLE(acpi, imx208_acpi_ids);
+#endif
+
+static struct i2c_driver imx208_i2c_driver = {
+	.driver = {
+		.name = "imx208",
+		.pm = &imx208_pm_ops,
+		.acpi_match_table = ACPI_PTR(imx208_acpi_ids),
+	},
+	.probe = imx208_probe,
+	.remove = imx208_remove,
+	.flags = I2C_DRV_ACPI_WAIVE_D0_PROBE,
+};
+
+module_i2c_driver(imx208_i2c_driver);
+
+MODULE_AUTHOR("Yeh, Andy <andy.yeh@intel.com>");
+MODULE_AUTHOR("Chen, Ping-chung <ping-chung.chen@intel.com>");
+MODULE_AUTHOR("Shawn Tu");
+MODULE_DESCRIPTION("Sony IMX208 sensor driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/imx214.c b/drivers/media/i2c/imx214.c
new file mode 100644
index 000000000..2f9c8582f
--- /dev/null
+++ b/drivers/media/i2c/imx214.c
@@ -0,0 +1,1123 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * imx214.c - imx214 sensor driver
+ *
+ * Copyright 2018 Qtechnology A/S
+ *
+ * Ricardo Ribalda <ribalda@kernel.org>
+ */
+#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/regmap.h>
+#include <linux/regulator/consumer.h>
+#include <media/media-entity.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+#define IMX214_DEFAULT_CLK_FREQ	24000000
+#define IMX214_DEFAULT_LINK_FREQ 480000000
+#define IMX214_DEFAULT_PIXEL_RATE ((IMX214_DEFAULT_LINK_FREQ * 8LL) / 10)
+#define IMX214_FPS 30
+#define IMX214_MBUS_CODE MEDIA_BUS_FMT_SRGGB10_1X10
+
+static const char * const imx214_supply_name[] = {
+	"vdda",
+	"vddd",
+	"vdddo",
+};
+
+#define IMX214_NUM_SUPPLIES ARRAY_SIZE(imx214_supply_name)
+
+struct imx214 {
+	struct device *dev;
+	struct clk *xclk;
+	struct regmap *regmap;
+
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct v4l2_mbus_framefmt fmt;
+	struct v4l2_rect crop;
+
+	struct v4l2_ctrl_handler ctrls;
+	struct v4l2_ctrl *pixel_rate;
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *exposure;
+	struct v4l2_ctrl *unit_size;
+
+	struct regulator_bulk_data	supplies[IMX214_NUM_SUPPLIES];
+
+	struct gpio_desc *enable_gpio;
+
+	/*
+	 * Serialize control access, get/set format, get selection
+	 * and start streaming.
+	 */
+	struct mutex mutex;
+
+	bool streaming;
+};
+
+struct reg_8 {
+	u16 addr;
+	u8 val;
+};
+
+enum {
+	IMX214_TABLE_WAIT_MS = 0,
+	IMX214_TABLE_END,
+	IMX214_MAX_RETRIES,
+	IMX214_WAIT_MS
+};
+
+/*From imx214_mode_tbls.h*/
+static const struct reg_8 mode_4096x2304[] = {
+	{0x0114, 0x03},
+	{0x0220, 0x00},
+	{0x0221, 0x11},
+	{0x0222, 0x01},
+	{0x0340, 0x0C},
+	{0x0341, 0x7A},
+	{0x0342, 0x13},
+	{0x0343, 0x90},
+	{0x0344, 0x00},
+	{0x0345, 0x38},
+	{0x0346, 0x01},
+	{0x0347, 0x98},
+	{0x0348, 0x10},
+	{0x0349, 0x37},
+	{0x034A, 0x0A},
+	{0x034B, 0x97},
+	{0x0381, 0x01},
+	{0x0383, 0x01},
+	{0x0385, 0x01},
+	{0x0387, 0x01},
+	{0x0900, 0x00},
+	{0x0901, 0x00},
+	{0x0902, 0x00},
+	{0x3000, 0x35},
+	{0x3054, 0x01},
+	{0x305C, 0x11},
+
+	{0x0112, 0x0A},
+	{0x0113, 0x0A},
+	{0x034C, 0x10},
+	{0x034D, 0x00},
+	{0x034E, 0x09},
+	{0x034F, 0x00},
+	{0x0401, 0x00},
+	{0x0404, 0x00},
+	{0x0405, 0x10},
+	{0x0408, 0x00},
+	{0x0409, 0x00},
+	{0x040A, 0x00},
+	{0x040B, 0x00},
+	{0x040C, 0x10},
+	{0x040D, 0x00},
+	{0x040E, 0x09},
+	{0x040F, 0x00},
+
+	{0x0301, 0x05},
+	{0x0303, 0x02},
+	{0x0305, 0x03},
+	{0x0306, 0x00},
+	{0x0307, 0x96},
+	{0x0309, 0x0A},
+	{0x030B, 0x01},
+	{0x0310, 0x00},
+
+	{0x0820, 0x12},
+	{0x0821, 0xC0},
+	{0x0822, 0x00},
+	{0x0823, 0x00},
+
+	{0x3A03, 0x09},
+	{0x3A04, 0x50},
+	{0x3A05, 0x01},
+
+	{0x0B06, 0x01},
+	{0x30A2, 0x00},
+
+	{0x30B4, 0x00},
+
+	{0x3A02, 0xFF},
+
+	{0x3011, 0x00},
+	{0x3013, 0x01},
+
+	{0x0202, 0x0C},
+	{0x0203, 0x70},
+	{0x0224, 0x01},
+	{0x0225, 0xF4},
+
+	{0x0204, 0x00},
+	{0x0205, 0x00},
+	{0x020E, 0x01},
+	{0x020F, 0x00},
+	{0x0210, 0x01},
+	{0x0211, 0x00},
+	{0x0212, 0x01},
+	{0x0213, 0x00},
+	{0x0214, 0x01},
+	{0x0215, 0x00},
+	{0x0216, 0x00},
+	{0x0217, 0x00},
+
+	{0x4170, 0x00},
+	{0x4171, 0x10},
+	{0x4176, 0x00},
+	{0x4177, 0x3C},
+	{0xAE20, 0x04},
+	{0xAE21, 0x5C},
+
+	{IMX214_TABLE_WAIT_MS, 10},
+	{0x0138, 0x01},
+	{IMX214_TABLE_END, 0x00}
+};
+
+static const struct reg_8 mode_1920x1080[] = {
+	{0x0114, 0x03},
+	{0x0220, 0x00},
+	{0x0221, 0x11},
+	{0x0222, 0x01},
+	{0x0340, 0x0C},
+	{0x0341, 0x7A},
+	{0x0342, 0x13},
+	{0x0343, 0x90},
+	{0x0344, 0x04},
+	{0x0345, 0x78},
+	{0x0346, 0x03},
+	{0x0347, 0xFC},
+	{0x0348, 0x0B},
+	{0x0349, 0xF7},
+	{0x034A, 0x08},
+	{0x034B, 0x33},
+	{0x0381, 0x01},
+	{0x0383, 0x01},
+	{0x0385, 0x01},
+	{0x0387, 0x01},
+	{0x0900, 0x00},
+	{0x0901, 0x00},
+	{0x0902, 0x00},
+	{0x3000, 0x35},
+	{0x3054, 0x01},
+	{0x305C, 0x11},
+
+	{0x0112, 0x0A},
+	{0x0113, 0x0A},
+	{0x034C, 0x07},
+	{0x034D, 0x80},
+	{0x034E, 0x04},
+	{0x034F, 0x38},
+	{0x0401, 0x00},
+	{0x0404, 0x00},
+	{0x0405, 0x10},
+	{0x0408, 0x00},
+	{0x0409, 0x00},
+	{0x040A, 0x00},
+	{0x040B, 0x00},
+	{0x040C, 0x07},
+	{0x040D, 0x80},
+	{0x040E, 0x04},
+	{0x040F, 0x38},
+
+	{0x0301, 0x05},
+	{0x0303, 0x02},
+	{0x0305, 0x03},
+	{0x0306, 0x00},
+	{0x0307, 0x96},
+	{0x0309, 0x0A},
+	{0x030B, 0x01},
+	{0x0310, 0x00},
+
+	{0x0820, 0x12},
+	{0x0821, 0xC0},
+	{0x0822, 0x00},
+	{0x0823, 0x00},
+
+	{0x3A03, 0x04},
+	{0x3A04, 0xF8},
+	{0x3A05, 0x02},
+
+	{0x0B06, 0x01},
+	{0x30A2, 0x00},
+
+	{0x30B4, 0x00},
+
+	{0x3A02, 0xFF},
+
+	{0x3011, 0x00},
+	{0x3013, 0x01},
+
+	{0x0202, 0x0C},
+	{0x0203, 0x70},
+	{0x0224, 0x01},
+	{0x0225, 0xF4},
+
+	{0x0204, 0x00},
+	{0x0205, 0x00},
+	{0x020E, 0x01},
+	{0x020F, 0x00},
+	{0x0210, 0x01},
+	{0x0211, 0x00},
+	{0x0212, 0x01},
+	{0x0213, 0x00},
+	{0x0214, 0x01},
+	{0x0215, 0x00},
+	{0x0216, 0x00},
+	{0x0217, 0x00},
+
+	{0x4170, 0x00},
+	{0x4171, 0x10},
+	{0x4176, 0x00},
+	{0x4177, 0x3C},
+	{0xAE20, 0x04},
+	{0xAE21, 0x5C},
+
+	{IMX214_TABLE_WAIT_MS, 10},
+	{0x0138, 0x01},
+	{IMX214_TABLE_END, 0x00}
+};
+
+static const struct reg_8 mode_table_common[] = {
+	/* software reset */
+
+	/* software standby settings */
+	{0x0100, 0x00},
+
+	/* ATR setting */
+	{0x9300, 0x02},
+
+	/* external clock setting */
+	{0x0136, 0x18},
+	{0x0137, 0x00},
+
+	/* global setting */
+	/* basic config */
+	{0x0101, 0x00},
+	{0x0105, 0x01},
+	{0x0106, 0x01},
+	{0x4550, 0x02},
+	{0x4601, 0x00},
+	{0x4642, 0x05},
+	{0x6227, 0x11},
+	{0x6276, 0x00},
+	{0x900E, 0x06},
+	{0xA802, 0x90},
+	{0xA803, 0x11},
+	{0xA804, 0x62},
+	{0xA805, 0x77},
+	{0xA806, 0xAE},
+	{0xA807, 0x34},
+	{0xA808, 0xAE},
+	{0xA809, 0x35},
+	{0xA80A, 0x62},
+	{0xA80B, 0x83},
+	{0xAE33, 0x00},
+
+	/* analog setting */
+	{0x4174, 0x00},
+	{0x4175, 0x11},
+	{0x4612, 0x29},
+	{0x461B, 0x12},
+	{0x461F, 0x06},
+	{0x4635, 0x07},
+	{0x4637, 0x30},
+	{0x463F, 0x18},
+	{0x4641, 0x0D},
+	{0x465B, 0x12},
+	{0x465F, 0x11},
+	{0x4663, 0x11},
+	{0x4667, 0x0F},
+	{0x466F, 0x0F},
+	{0x470E, 0x09},
+	{0x4909, 0xAB},
+	{0x490B, 0x95},
+	{0x4915, 0x5D},
+	{0x4A5F, 0xFF},
+	{0x4A61, 0xFF},
+	{0x4A73, 0x62},
+	{0x4A85, 0x00},
+	{0x4A87, 0xFF},
+
+	/* embedded data */
+	{0x5041, 0x04},
+	{0x583C, 0x04},
+	{0x620E, 0x04},
+	{0x6EB2, 0x01},
+	{0x6EB3, 0x00},
+	{0x9300, 0x02},
+
+	/* imagequality */
+	/* HDR setting */
+	{0x3001, 0x07},
+	{0x6D12, 0x3F},
+	{0x6D13, 0xFF},
+	{0x9344, 0x03},
+	{0x9706, 0x10},
+	{0x9707, 0x03},
+	{0x9708, 0x03},
+	{0x9E04, 0x01},
+	{0x9E05, 0x00},
+	{0x9E0C, 0x01},
+	{0x9E0D, 0x02},
+	{0x9E24, 0x00},
+	{0x9E25, 0x8C},
+	{0x9E26, 0x00},
+	{0x9E27, 0x94},
+	{0x9E28, 0x00},
+	{0x9E29, 0x96},
+
+	/* CNR parameter setting */
+	{0x69DB, 0x01},
+
+	/* Moire reduction */
+	{0x6957, 0x01},
+
+	/* image enhancement */
+	{0x6987, 0x17},
+	{0x698A, 0x03},
+	{0x698B, 0x03},
+
+	/* white balanace */
+	{0x0B8E, 0x01},
+	{0x0B8F, 0x00},
+	{0x0B90, 0x01},
+	{0x0B91, 0x00},
+	{0x0B92, 0x01},
+	{0x0B93, 0x00},
+	{0x0B94, 0x01},
+	{0x0B95, 0x00},
+
+	/* ATR setting */
+	{0x6E50, 0x00},
+	{0x6E51, 0x32},
+	{0x9340, 0x00},
+	{0x9341, 0x3C},
+	{0x9342, 0x03},
+	{0x9343, 0xFF},
+	{IMX214_TABLE_END, 0x00}
+};
+
+/*
+ * Declare modes in order, from biggest
+ * to smallest height.
+ */
+static const struct imx214_mode {
+	u32 width;
+	u32 height;
+	const struct reg_8 *reg_table;
+} imx214_modes[] = {
+	{
+		.width = 4096,
+		.height = 2304,
+		.reg_table = mode_4096x2304,
+	},
+	{
+		.width = 1920,
+		.height = 1080,
+		.reg_table = mode_1920x1080,
+	},
+};
+
+static inline struct imx214 *to_imx214(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct imx214, sd);
+}
+
+static int __maybe_unused imx214_power_on(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct imx214 *imx214 = to_imx214(sd);
+	int ret;
+
+	ret = regulator_bulk_enable(IMX214_NUM_SUPPLIES, imx214->supplies);
+	if (ret < 0) {
+		dev_err(imx214->dev, "failed to enable regulators: %d\n", ret);
+		return ret;
+	}
+
+	usleep_range(2000, 3000);
+
+	ret = clk_prepare_enable(imx214->xclk);
+	if (ret < 0) {
+		regulator_bulk_disable(IMX214_NUM_SUPPLIES, imx214->supplies);
+		dev_err(imx214->dev, "clk prepare enable failed\n");
+		return ret;
+	}
+
+	gpiod_set_value_cansleep(imx214->enable_gpio, 1);
+	usleep_range(12000, 15000);
+
+	return 0;
+}
+
+static int __maybe_unused imx214_power_off(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct imx214 *imx214 = to_imx214(sd);
+
+	gpiod_set_value_cansleep(imx214->enable_gpio, 0);
+
+	clk_disable_unprepare(imx214->xclk);
+
+	regulator_bulk_disable(IMX214_NUM_SUPPLIES, imx214->supplies);
+	usleep_range(10, 20);
+
+	return 0;
+}
+
+static int imx214_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index > 0)
+		return -EINVAL;
+
+	code->code = IMX214_MBUS_CODE;
+
+	return 0;
+}
+
+static int imx214_enum_frame_size(struct v4l2_subdev *subdev,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->code != IMX214_MBUS_CODE)
+		return -EINVAL;
+
+	if (fse->index >= ARRAY_SIZE(imx214_modes))
+		return -EINVAL;
+
+	fse->min_width = fse->max_width = imx214_modes[fse->index].width;
+	fse->min_height = fse->max_height = imx214_modes[fse->index].height;
+
+	return 0;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int imx214_s_register(struct v4l2_subdev *subdev,
+			     const struct v4l2_dbg_register *reg)
+{
+	struct imx214 *imx214 = container_of(subdev, struct imx214, sd);
+
+	return regmap_write(imx214->regmap, reg->reg, reg->val);
+}
+
+static int imx214_g_register(struct v4l2_subdev *subdev,
+			     struct v4l2_dbg_register *reg)
+{
+	struct imx214 *imx214 = container_of(subdev, struct imx214, sd);
+	unsigned int aux;
+	int ret;
+
+	reg->size = 1;
+	ret = regmap_read(imx214->regmap, reg->reg, &aux);
+	reg->val = aux;
+
+	return ret;
+}
+#endif
+
+static const struct v4l2_subdev_core_ops imx214_core_ops = {
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = imx214_g_register,
+	.s_register = imx214_s_register,
+#endif
+};
+
+static struct v4l2_mbus_framefmt *
+__imx214_get_pad_format(struct imx214 *imx214,
+			struct v4l2_subdev_state *sd_state,
+			unsigned int pad,
+			enum v4l2_subdev_format_whence which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_format(&imx214->sd, sd_state, pad);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return &imx214->fmt;
+	default:
+		return NULL;
+	}
+}
+
+static int imx214_get_format(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *sd_state,
+			     struct v4l2_subdev_format *format)
+{
+	struct imx214 *imx214 = to_imx214(sd);
+
+	mutex_lock(&imx214->mutex);
+	format->format = *__imx214_get_pad_format(imx214, sd_state,
+						  format->pad,
+						  format->which);
+	mutex_unlock(&imx214->mutex);
+
+	return 0;
+}
+
+static struct v4l2_rect *
+__imx214_get_pad_crop(struct imx214 *imx214,
+		      struct v4l2_subdev_state *sd_state,
+		      unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_crop(&imx214->sd, sd_state, pad);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return &imx214->crop;
+	default:
+		return NULL;
+	}
+}
+
+static int imx214_set_format(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *sd_state,
+			     struct v4l2_subdev_format *format)
+{
+	struct imx214 *imx214 = to_imx214(sd);
+	struct v4l2_mbus_framefmt *__format;
+	struct v4l2_rect *__crop;
+	const struct imx214_mode *mode;
+
+	mutex_lock(&imx214->mutex);
+
+	__crop = __imx214_get_pad_crop(imx214, sd_state, format->pad,
+				       format->which);
+
+	mode = v4l2_find_nearest_size(imx214_modes,
+				      ARRAY_SIZE(imx214_modes), width, height,
+				      format->format.width,
+				      format->format.height);
+
+	__crop->width = mode->width;
+	__crop->height = mode->height;
+
+	__format = __imx214_get_pad_format(imx214, sd_state, format->pad,
+					   format->which);
+	__format->width = __crop->width;
+	__format->height = __crop->height;
+	__format->code = IMX214_MBUS_CODE;
+	__format->field = V4L2_FIELD_NONE;
+	__format->colorspace = V4L2_COLORSPACE_SRGB;
+	__format->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(__format->colorspace);
+	__format->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(true,
+				__format->colorspace, __format->ycbcr_enc);
+	__format->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(__format->colorspace);
+
+	format->format = *__format;
+
+	mutex_unlock(&imx214->mutex);
+
+	return 0;
+}
+
+static int imx214_get_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *sd_state,
+				struct v4l2_subdev_selection *sel)
+{
+	struct imx214 *imx214 = to_imx214(sd);
+
+	if (sel->target != V4L2_SEL_TGT_CROP)
+		return -EINVAL;
+
+	mutex_lock(&imx214->mutex);
+	sel->r = *__imx214_get_pad_crop(imx214, sd_state, sel->pad,
+					sel->which);
+	mutex_unlock(&imx214->mutex);
+	return 0;
+}
+
+static int imx214_entity_init_cfg(struct v4l2_subdev *subdev,
+				  struct v4l2_subdev_state *sd_state)
+{
+	struct v4l2_subdev_format fmt = { };
+
+	fmt.which = sd_state ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE;
+	fmt.format.width = imx214_modes[0].width;
+	fmt.format.height = imx214_modes[0].height;
+
+	imx214_set_format(subdev, sd_state, &fmt);
+
+	return 0;
+}
+
+static int imx214_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct imx214 *imx214 = container_of(ctrl->handler,
+					     struct imx214, ctrls);
+	u8 vals[2];
+	int ret;
+
+	/*
+	 * Applying V4L2 control value only happens
+	 * when power is up for streaming
+	 */
+	if (!pm_runtime_get_if_in_use(imx214->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE:
+		vals[1] = ctrl->val;
+		vals[0] = ctrl->val >> 8;
+		ret = regmap_bulk_write(imx214->regmap, 0x202, vals, 2);
+		if (ret < 0)
+			dev_err(imx214->dev, "Error %d\n", ret);
+		ret = 0;
+		break;
+
+	default:
+		ret = -EINVAL;
+	}
+
+	pm_runtime_put(imx214->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops imx214_ctrl_ops = {
+	.s_ctrl = imx214_set_ctrl,
+};
+
+#define MAX_CMD 4
+static int imx214_write_table(struct imx214 *imx214,
+			      const struct reg_8 table[])
+{
+	u8 vals[MAX_CMD];
+	int i;
+	int ret;
+
+	for (; table->addr != IMX214_TABLE_END ; table++) {
+		if (table->addr == IMX214_TABLE_WAIT_MS) {
+			usleep_range(table->val * 1000,
+				     table->val * 1000 + 500);
+			continue;
+		}
+
+		for (i = 0; i < MAX_CMD; i++) {
+			if (table[i].addr != (table[0].addr + i))
+				break;
+			vals[i] = table[i].val;
+		}
+
+		ret = regmap_bulk_write(imx214->regmap, table->addr, vals, i);
+
+		if (ret) {
+			dev_err(imx214->dev, "write_table error: %d\n", ret);
+			return ret;
+		}
+
+		table += i - 1;
+	}
+
+	return 0;
+}
+
+static int imx214_start_streaming(struct imx214 *imx214)
+{
+	const struct imx214_mode *mode;
+	int ret;
+
+	mutex_lock(&imx214->mutex);
+	ret = imx214_write_table(imx214, mode_table_common);
+	if (ret < 0) {
+		dev_err(imx214->dev, "could not sent common table %d\n", ret);
+		goto error;
+	}
+
+	mode = v4l2_find_nearest_size(imx214_modes,
+				ARRAY_SIZE(imx214_modes), width, height,
+				imx214->fmt.width, imx214->fmt.height);
+	ret = imx214_write_table(imx214, mode->reg_table);
+	if (ret < 0) {
+		dev_err(imx214->dev, "could not sent mode table %d\n", ret);
+		goto error;
+	}
+	ret = __v4l2_ctrl_handler_setup(&imx214->ctrls);
+	if (ret < 0) {
+		dev_err(imx214->dev, "could not sync v4l2 controls\n");
+		goto error;
+	}
+	ret = regmap_write(imx214->regmap, 0x100, 1);
+	if (ret < 0) {
+		dev_err(imx214->dev, "could not sent start table %d\n", ret);
+		goto error;
+	}
+
+	mutex_unlock(&imx214->mutex);
+	return 0;
+
+error:
+	mutex_unlock(&imx214->mutex);
+	return ret;
+}
+
+static int imx214_stop_streaming(struct imx214 *imx214)
+{
+	int ret;
+
+	ret = regmap_write(imx214->regmap, 0x100, 0);
+	if (ret < 0)
+		dev_err(imx214->dev, "could not sent stop table %d\n",	ret);
+
+	return ret;
+}
+
+static int imx214_s_stream(struct v4l2_subdev *subdev, int enable)
+{
+	struct imx214 *imx214 = to_imx214(subdev);
+	int ret;
+
+	if (imx214->streaming == enable)
+		return 0;
+
+	if (enable) {
+		ret = pm_runtime_resume_and_get(imx214->dev);
+		if (ret < 0)
+			return ret;
+
+		ret = imx214_start_streaming(imx214);
+		if (ret < 0)
+			goto err_rpm_put;
+	} else {
+		ret = imx214_stop_streaming(imx214);
+		if (ret < 0)
+			goto err_rpm_put;
+		pm_runtime_put(imx214->dev);
+	}
+
+	imx214->streaming = enable;
+	return 0;
+
+err_rpm_put:
+	pm_runtime_put(imx214->dev);
+	return ret;
+}
+
+static int imx214_g_frame_interval(struct v4l2_subdev *subdev,
+				   struct v4l2_subdev_frame_interval *fival)
+{
+	fival->interval.numerator = 1;
+	fival->interval.denominator = IMX214_FPS;
+
+	return 0;
+}
+
+static int imx214_enum_frame_interval(struct v4l2_subdev *subdev,
+				struct v4l2_subdev_state *sd_state,
+				struct v4l2_subdev_frame_interval_enum *fie)
+{
+	const struct imx214_mode *mode;
+
+	if (fie->index != 0)
+		return -EINVAL;
+
+	mode = v4l2_find_nearest_size(imx214_modes,
+				ARRAY_SIZE(imx214_modes), width, height,
+				fie->width, fie->height);
+
+	fie->code = IMX214_MBUS_CODE;
+	fie->width = mode->width;
+	fie->height = mode->height;
+	fie->interval.numerator = 1;
+	fie->interval.denominator = IMX214_FPS;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops imx214_video_ops = {
+	.s_stream = imx214_s_stream,
+	.g_frame_interval = imx214_g_frame_interval,
+	.s_frame_interval = imx214_g_frame_interval,
+};
+
+static const struct v4l2_subdev_pad_ops imx214_subdev_pad_ops = {
+	.enum_mbus_code = imx214_enum_mbus_code,
+	.enum_frame_size = imx214_enum_frame_size,
+	.enum_frame_interval = imx214_enum_frame_interval,
+	.get_fmt = imx214_get_format,
+	.set_fmt = imx214_set_format,
+	.get_selection = imx214_get_selection,
+	.init_cfg = imx214_entity_init_cfg,
+};
+
+static const struct v4l2_subdev_ops imx214_subdev_ops = {
+	.core = &imx214_core_ops,
+	.video = &imx214_video_ops,
+	.pad = &imx214_subdev_pad_ops,
+};
+
+static const struct regmap_config sensor_regmap_config = {
+	.reg_bits = 16,
+	.val_bits = 8,
+	.cache_type = REGCACHE_RBTREE,
+};
+
+static int imx214_get_regulators(struct device *dev, struct imx214 *imx214)
+{
+	unsigned int i;
+
+	for (i = 0; i < IMX214_NUM_SUPPLIES; i++)
+		imx214->supplies[i].supply = imx214_supply_name[i];
+
+	return devm_regulator_bulk_get(dev, IMX214_NUM_SUPPLIES,
+				       imx214->supplies);
+}
+
+static int imx214_parse_fwnode(struct device *dev)
+{
+	struct fwnode_handle *endpoint;
+	struct v4l2_fwnode_endpoint bus_cfg = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY,
+	};
+	unsigned int i;
+	int ret;
+
+	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, &bus_cfg);
+	if (ret) {
+		dev_err(dev, "parsing endpoint node failed\n");
+		goto done;
+	}
+
+	for (i = 0; i < bus_cfg.nr_of_link_frequencies; i++)
+		if (bus_cfg.link_frequencies[i] == IMX214_DEFAULT_LINK_FREQ)
+			break;
+
+	if (i == bus_cfg.nr_of_link_frequencies) {
+		dev_err(dev, "link-frequencies %d not supported, Please review your DT\n",
+			IMX214_DEFAULT_LINK_FREQ);
+		ret = -EINVAL;
+		goto done;
+	}
+
+done:
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+	fwnode_handle_put(endpoint);
+	return ret;
+}
+
+static int __maybe_unused imx214_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct imx214 *imx214 = to_imx214(sd);
+
+	if (imx214->streaming)
+		imx214_stop_streaming(imx214);
+
+	return 0;
+}
+
+static int __maybe_unused imx214_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct imx214 *imx214 = to_imx214(sd);
+	int ret;
+
+	if (imx214->streaming) {
+		ret = imx214_start_streaming(imx214);
+		if (ret)
+			goto error;
+	}
+
+	return 0;
+
+error:
+	imx214_stop_streaming(imx214);
+	imx214->streaming = 0;
+	return ret;
+}
+
+static int imx214_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct imx214 *imx214;
+	static const s64 link_freq[] = {
+		IMX214_DEFAULT_LINK_FREQ,
+	};
+	static const struct v4l2_area unit_size = {
+		.width = 1120,
+		.height = 1120,
+	};
+	int ret;
+
+	ret = imx214_parse_fwnode(dev);
+	if (ret)
+		return ret;
+
+	imx214 = devm_kzalloc(dev, sizeof(*imx214), GFP_KERNEL);
+	if (!imx214)
+		return -ENOMEM;
+
+	imx214->dev = dev;
+
+	imx214->xclk = devm_clk_get(dev, NULL);
+	if (IS_ERR(imx214->xclk)) {
+		dev_err(dev, "could not get xclk");
+		return PTR_ERR(imx214->xclk);
+	}
+
+	ret = clk_set_rate(imx214->xclk, IMX214_DEFAULT_CLK_FREQ);
+	if (ret) {
+		dev_err(dev, "could not set xclk frequency\n");
+		return ret;
+	}
+
+	ret = imx214_get_regulators(dev, imx214);
+	if (ret < 0) {
+		dev_err(dev, "cannot get regulators\n");
+		return ret;
+	}
+
+	imx214->enable_gpio = devm_gpiod_get(dev, "enable", GPIOD_OUT_LOW);
+	if (IS_ERR(imx214->enable_gpio)) {
+		dev_err(dev, "cannot get enable gpio\n");
+		return PTR_ERR(imx214->enable_gpio);
+	}
+
+	imx214->regmap = devm_regmap_init_i2c(client, &sensor_regmap_config);
+	if (IS_ERR(imx214->regmap)) {
+		dev_err(dev, "regmap init failed\n");
+		return PTR_ERR(imx214->regmap);
+	}
+
+	v4l2_i2c_subdev_init(&imx214->sd, client, &imx214_subdev_ops);
+
+	/*
+	 * Enable power initially, to avoid warnings
+	 * from clk_disable on power_off
+	 */
+	imx214_power_on(imx214->dev);
+
+	pm_runtime_set_active(imx214->dev);
+	pm_runtime_enable(imx214->dev);
+	pm_runtime_idle(imx214->dev);
+
+	v4l2_ctrl_handler_init(&imx214->ctrls, 3);
+
+	imx214->pixel_rate = v4l2_ctrl_new_std(&imx214->ctrls, NULL,
+					       V4L2_CID_PIXEL_RATE, 0,
+					       IMX214_DEFAULT_PIXEL_RATE, 1,
+					       IMX214_DEFAULT_PIXEL_RATE);
+	imx214->link_freq = v4l2_ctrl_new_int_menu(&imx214->ctrls, NULL,
+						   V4L2_CID_LINK_FREQ,
+						   ARRAY_SIZE(link_freq) - 1,
+						   0, link_freq);
+	if (imx214->link_freq)
+		imx214->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	/*
+	 * WARNING!
+	 * Values obtained reverse engineering blobs and/or devices.
+	 * Ranges and functionality might be wrong.
+	 *
+	 * Sony, please release some register set documentation for the
+	 * device.
+	 *
+	 * Yours sincerely, Ricardo.
+	 */
+	imx214->exposure = v4l2_ctrl_new_std(&imx214->ctrls, &imx214_ctrl_ops,
+					     V4L2_CID_EXPOSURE,
+					     0, 3184, 1, 0x0c70);
+
+	imx214->unit_size = v4l2_ctrl_new_std_compound(&imx214->ctrls,
+				NULL,
+				V4L2_CID_UNIT_CELL_SIZE,
+				v4l2_ctrl_ptr_create((void *)&unit_size));
+	ret = imx214->ctrls.error;
+	if (ret) {
+		dev_err(&client->dev, "%s control init failed (%d)\n",
+			__func__, ret);
+		goto free_ctrl;
+	}
+
+	imx214->sd.ctrl_handler = &imx214->ctrls;
+	mutex_init(&imx214->mutex);
+	imx214->ctrls.lock = &imx214->mutex;
+
+	imx214->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	imx214->pad.flags = MEDIA_PAD_FL_SOURCE;
+	imx214->sd.dev = &client->dev;
+	imx214->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+	ret = media_entity_pads_init(&imx214->sd.entity, 1, &imx214->pad);
+	if (ret < 0) {
+		dev_err(dev, "could not register media entity\n");
+		goto free_ctrl;
+	}
+
+	imx214_entity_init_cfg(&imx214->sd, NULL);
+
+	ret = v4l2_async_register_subdev_sensor(&imx214->sd);
+	if (ret < 0) {
+		dev_err(dev, "could not register v4l2 device\n");
+		goto free_entity;
+	}
+
+	return 0;
+
+free_entity:
+	media_entity_cleanup(&imx214->sd.entity);
+free_ctrl:
+	mutex_destroy(&imx214->mutex);
+	v4l2_ctrl_handler_free(&imx214->ctrls);
+	pm_runtime_disable(imx214->dev);
+
+	return ret;
+}
+
+static void imx214_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct imx214 *imx214 = to_imx214(sd);
+
+	v4l2_async_unregister_subdev(&imx214->sd);
+	media_entity_cleanup(&imx214->sd.entity);
+	v4l2_ctrl_handler_free(&imx214->ctrls);
+
+	pm_runtime_disable(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+
+	mutex_destroy(&imx214->mutex);
+}
+
+static const struct of_device_id imx214_of_match[] = {
+	{ .compatible = "sony,imx214" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, imx214_of_match);
+
+static const struct dev_pm_ops imx214_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(imx214_suspend, imx214_resume)
+	SET_RUNTIME_PM_OPS(imx214_power_off, imx214_power_on, NULL)
+};
+
+static struct i2c_driver imx214_i2c_driver = {
+	.driver = {
+		.of_match_table = imx214_of_match,
+		.pm = &imx214_pm_ops,
+		.name  = "imx214",
+	},
+	.probe = imx214_probe,
+	.remove = imx214_remove,
+};
+
+module_i2c_driver(imx214_i2c_driver);
+
+MODULE_DESCRIPTION("Sony IMX214 Camera driver");
+MODULE_AUTHOR("Ricardo Ribalda <ribalda@kernel.org>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/imx219.c b/drivers/media/i2c/imx219.c
new file mode 100644
index 000000000..3afa3f79c
--- /dev/null
+++ b/drivers/media/i2c/imx219.c
@@ -0,0 +1,1406 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * A V4L2 driver for Sony IMX219 cameras.
+ * Copyright (C) 2019, Raspberry Pi (Trading) Ltd
+ *
+ * Based on Sony imx258 camera driver
+ * Copyright (C) 2018 Intel Corporation
+ *
+ * DT / fwnode changes, and regulator / GPIO control taken from imx214 driver
+ * Copyright 2018 Qtechnology A/S
+ *
+ * Flip handling taken from the Sony IMX319 driver.
+ * Copyright (C) 2018 Intel Corporation
+ *
+ */
+
+#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-cci.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-mediabus.h>
+
+/* Chip ID */
+#define IMX219_REG_CHIP_ID		CCI_REG16(0x0000)
+#define IMX219_CHIP_ID			0x0219
+
+#define IMX219_REG_MODE_SELECT		CCI_REG8(0x0100)
+#define IMX219_MODE_STANDBY		0x00
+#define IMX219_MODE_STREAMING		0x01
+
+#define IMX219_REG_CSI_LANE_MODE	CCI_REG8(0x0114)
+#define IMX219_CSI_2_LANE_MODE		0x01
+#define IMX219_CSI_4_LANE_MODE		0x03
+
+#define IMX219_REG_DPHY_CTRL		CCI_REG8(0x0128)
+#define IMX219_DPHY_CTRL_TIMING_AUTO	0
+#define IMX219_DPHY_CTRL_TIMING_MANUAL	1
+
+#define IMX219_REG_EXCK_FREQ		CCI_REG16(0x012a)
+#define IMX219_EXCK_FREQ(n)		((n) * 256)		/* n expressed in MHz */
+
+/* Analog gain control */
+#define IMX219_REG_ANALOG_GAIN		CCI_REG8(0x0157)
+#define IMX219_ANA_GAIN_MIN		0
+#define IMX219_ANA_GAIN_MAX		232
+#define IMX219_ANA_GAIN_STEP		1
+#define IMX219_ANA_GAIN_DEFAULT		0x0
+
+/* Digital gain control */
+#define IMX219_REG_DIGITAL_GAIN		CCI_REG16(0x0158)
+#define IMX219_DGTL_GAIN_MIN		0x0100
+#define IMX219_DGTL_GAIN_MAX		0x0fff
+#define IMX219_DGTL_GAIN_DEFAULT	0x0100
+#define IMX219_DGTL_GAIN_STEP		1
+
+/* Exposure control */
+#define IMX219_REG_EXPOSURE		CCI_REG16(0x015a)
+#define IMX219_EXPOSURE_MIN		4
+#define IMX219_EXPOSURE_STEP		1
+#define IMX219_EXPOSURE_DEFAULT		0x640
+#define IMX219_EXPOSURE_MAX		65535
+
+/* V_TIMING internal */
+#define IMX219_REG_VTS			CCI_REG16(0x0160)
+#define IMX219_VTS_15FPS		0x0dc6
+#define IMX219_VTS_30FPS_1080P		0x06e3
+#define IMX219_VTS_30FPS_BINNED		0x06e3
+#define IMX219_VTS_30FPS_640x480	0x06e3
+#define IMX219_VTS_MAX			0xffff
+
+#define IMX219_VBLANK_MIN		4
+
+/*Frame Length Line*/
+#define IMX219_FLL_MIN			0x08a6
+#define IMX219_FLL_MAX			0xffff
+#define IMX219_FLL_STEP			1
+#define IMX219_FLL_DEFAULT		0x0c98
+
+/* HBLANK control - read only */
+#define IMX219_PPL_DEFAULT		3448
+
+#define IMX219_REG_LINE_LENGTH_A	CCI_REG16(0x0162)
+#define IMX219_REG_X_ADD_STA_A		CCI_REG16(0x0164)
+#define IMX219_REG_X_ADD_END_A		CCI_REG16(0x0166)
+#define IMX219_REG_Y_ADD_STA_A		CCI_REG16(0x0168)
+#define IMX219_REG_Y_ADD_END_A		CCI_REG16(0x016a)
+#define IMX219_REG_X_OUTPUT_SIZE	CCI_REG16(0x016c)
+#define IMX219_REG_Y_OUTPUT_SIZE	CCI_REG16(0x016e)
+#define IMX219_REG_X_ODD_INC_A		CCI_REG8(0x0170)
+#define IMX219_REG_Y_ODD_INC_A		CCI_REG8(0x0171)
+#define IMX219_REG_ORIENTATION		CCI_REG8(0x0172)
+
+/* Binning  Mode */
+#define IMX219_REG_BINNING_MODE		CCI_REG16(0x0174)
+#define IMX219_BINNING_NONE		0x0000
+#define IMX219_BINNING_2X2		0x0101
+#define IMX219_BINNING_2X2_ANALOG	0x0303
+
+#define IMX219_REG_CSI_DATA_FORMAT_A	CCI_REG16(0x018c)
+
+/* PLL Settings */
+#define IMX219_REG_VTPXCK_DIV		CCI_REG8(0x0301)
+#define IMX219_REG_VTSYCK_DIV		CCI_REG8(0x0303)
+#define IMX219_REG_PREPLLCK_VT_DIV	CCI_REG8(0x0304)
+#define IMX219_REG_PREPLLCK_OP_DIV	CCI_REG8(0x0305)
+#define IMX219_REG_PLL_VT_MPY		CCI_REG16(0x0306)
+#define IMX219_REG_OPPXCK_DIV		CCI_REG8(0x0309)
+#define IMX219_REG_OPSYCK_DIV		CCI_REG8(0x030b)
+#define IMX219_REG_PLL_OP_MPY		CCI_REG16(0x030c)
+
+/* Test Pattern Control */
+#define IMX219_REG_TEST_PATTERN		CCI_REG16(0x0600)
+#define IMX219_TEST_PATTERN_DISABLE	0
+#define IMX219_TEST_PATTERN_SOLID_COLOR	1
+#define IMX219_TEST_PATTERN_COLOR_BARS	2
+#define IMX219_TEST_PATTERN_GREY_COLOR	3
+#define IMX219_TEST_PATTERN_PN9		4
+
+/* Test pattern colour components */
+#define IMX219_REG_TESTP_RED		CCI_REG16(0x0602)
+#define IMX219_REG_TESTP_GREENR		CCI_REG16(0x0604)
+#define IMX219_REG_TESTP_BLUE		CCI_REG16(0x0606)
+#define IMX219_REG_TESTP_GREENB		CCI_REG16(0x0608)
+#define IMX219_TESTP_COLOUR_MIN		0
+#define IMX219_TESTP_COLOUR_MAX		0x03ff
+#define IMX219_TESTP_COLOUR_STEP	1
+#define IMX219_TESTP_RED_DEFAULT	IMX219_TESTP_COLOUR_MAX
+#define IMX219_TESTP_GREENR_DEFAULT	0
+#define IMX219_TESTP_BLUE_DEFAULT	0
+#define IMX219_TESTP_GREENB_DEFAULT	0
+
+#define IMX219_REG_TP_WINDOW_WIDTH	CCI_REG16(0x0624)
+#define IMX219_REG_TP_WINDOW_HEIGHT	CCI_REG16(0x0626)
+
+/* External clock frequency is 24.0M */
+#define IMX219_XCLK_FREQ		24000000
+
+/* Pixel rate is fixed for all the modes */
+#define IMX219_PIXEL_RATE		182400000
+#define IMX219_PIXEL_RATE_4LANE		280800000
+
+#define IMX219_DEFAULT_LINK_FREQ	456000000
+#define IMX219_DEFAULT_LINK_FREQ_4LANE	363000000
+
+/* IMX219 native and active pixel array size. */
+#define IMX219_NATIVE_WIDTH		3296U
+#define IMX219_NATIVE_HEIGHT		2480U
+#define IMX219_PIXEL_ARRAY_LEFT		8U
+#define IMX219_PIXEL_ARRAY_TOP		8U
+#define IMX219_PIXEL_ARRAY_WIDTH	3280U
+#define IMX219_PIXEL_ARRAY_HEIGHT	2464U
+
+struct imx219_reg_list {
+	unsigned int num_of_regs;
+	const struct cci_reg_sequence *regs;
+};
+
+/* Mode : resolution and related config&values */
+struct imx219_mode {
+	/* Frame width */
+	unsigned int width;
+	/* Frame height */
+	unsigned int height;
+
+	/* Analog crop rectangle. */
+	struct v4l2_rect crop;
+
+	/* V-timing */
+	unsigned int vts_def;
+
+	/* Default register values */
+	struct imx219_reg_list reg_list;
+
+	/* 2x2 binning is used */
+	bool binning;
+};
+
+static const struct cci_reg_sequence imx219_common_regs[] = {
+	{ IMX219_REG_MODE_SELECT, 0x00 },	/* Mode Select */
+
+	/* To Access Addresses 3000-5fff, send the following commands */
+	{ CCI_REG8(0x30eb), 0x0c },
+	{ CCI_REG8(0x30eb), 0x05 },
+	{ CCI_REG8(0x300a), 0xff },
+	{ CCI_REG8(0x300b), 0xff },
+	{ CCI_REG8(0x30eb), 0x05 },
+	{ CCI_REG8(0x30eb), 0x09 },
+
+	/* PLL Clock Table */
+	{ IMX219_REG_VTPXCK_DIV, 5 },
+	{ IMX219_REG_VTSYCK_DIV, 1 },
+	{ IMX219_REG_PREPLLCK_VT_DIV, 3 },	/* 0x03 = AUTO set */
+	{ IMX219_REG_PREPLLCK_OP_DIV, 3 },	/* 0x03 = AUTO set */
+	{ IMX219_REG_PLL_VT_MPY, 57 },
+	{ IMX219_REG_OPSYCK_DIV, 1 },
+	{ IMX219_REG_PLL_OP_MPY, 114 },
+
+	/* Undocumented registers */
+	{ CCI_REG8(0x455e), 0x00 },
+	{ CCI_REG8(0x471e), 0x4b },
+	{ CCI_REG8(0x4767), 0x0f },
+	{ CCI_REG8(0x4750), 0x14 },
+	{ CCI_REG8(0x4540), 0x00 },
+	{ CCI_REG8(0x47b4), 0x14 },
+	{ CCI_REG8(0x4713), 0x30 },
+	{ CCI_REG8(0x478b), 0x10 },
+	{ CCI_REG8(0x478f), 0x10 },
+	{ CCI_REG8(0x4793), 0x10 },
+	{ CCI_REG8(0x4797), 0x0e },
+	{ CCI_REG8(0x479b), 0x0e },
+
+	/* Frame Bank Register Group "A" */
+	{ IMX219_REG_LINE_LENGTH_A, 3448 },
+	{ IMX219_REG_X_ODD_INC_A, 1 },
+	{ IMX219_REG_Y_ODD_INC_A, 1 },
+
+	/* Output setup registers */
+	{ IMX219_REG_DPHY_CTRL, IMX219_DPHY_CTRL_TIMING_AUTO },
+	{ IMX219_REG_EXCK_FREQ, IMX219_EXCK_FREQ(IMX219_XCLK_FREQ / 1000000) },
+};
+
+/*
+ * Register sets lifted off the i2C interface from the Raspberry Pi firmware
+ * driver.
+ * 3280x2464 = mode 2, 1920x1080 = mode 1, 1640x1232 = mode 4, 640x480 = mode 7.
+ */
+static const struct cci_reg_sequence mode_3280x2464_regs[] = {
+	{ IMX219_REG_X_ADD_STA_A, 0 },
+	{ IMX219_REG_X_ADD_END_A, 3279 },
+	{ IMX219_REG_Y_ADD_STA_A, 0 },
+	{ IMX219_REG_Y_ADD_END_A, 2463 },
+	{ IMX219_REG_X_OUTPUT_SIZE, 3280 },
+	{ IMX219_REG_Y_OUTPUT_SIZE, 2464 },
+	{ IMX219_REG_TP_WINDOW_WIDTH, 3280 },
+	{ IMX219_REG_TP_WINDOW_HEIGHT, 2464 },
+};
+
+static const struct cci_reg_sequence mode_1920_1080_regs[] = {
+	{ IMX219_REG_X_ADD_STA_A, 680 },
+	{ IMX219_REG_X_ADD_END_A, 2599 },
+	{ IMX219_REG_Y_ADD_STA_A, 692 },
+	{ IMX219_REG_Y_ADD_END_A, 1771 },
+	{ IMX219_REG_X_OUTPUT_SIZE, 1920 },
+	{ IMX219_REG_Y_OUTPUT_SIZE, 1080 },
+	{ IMX219_REG_TP_WINDOW_WIDTH, 1920 },
+	{ IMX219_REG_TP_WINDOW_HEIGHT, 1080 },
+};
+
+static const struct cci_reg_sequence mode_1640_1232_regs[] = {
+	{ IMX219_REG_X_ADD_STA_A, 0 },
+	{ IMX219_REG_X_ADD_END_A, 3279 },
+	{ IMX219_REG_Y_ADD_STA_A, 0 },
+	{ IMX219_REG_Y_ADD_END_A, 2463 },
+	{ IMX219_REG_X_OUTPUT_SIZE, 1640 },
+	{ IMX219_REG_Y_OUTPUT_SIZE, 1232 },
+	{ IMX219_REG_TP_WINDOW_WIDTH, 1640 },
+	{ IMX219_REG_TP_WINDOW_HEIGHT, 1232 },
+};
+
+static const struct cci_reg_sequence mode_640_480_regs[] = {
+	{ IMX219_REG_X_ADD_STA_A, 1000 },
+	{ IMX219_REG_X_ADD_END_A, 2279 },
+	{ IMX219_REG_Y_ADD_STA_A, 752 },
+	{ IMX219_REG_Y_ADD_END_A, 1711 },
+	{ IMX219_REG_X_OUTPUT_SIZE, 640 },
+	{ IMX219_REG_Y_OUTPUT_SIZE, 480 },
+	{ IMX219_REG_TP_WINDOW_WIDTH, 1640 },
+	{ IMX219_REG_TP_WINDOW_HEIGHT, 1232 },
+};
+
+static const struct cci_reg_sequence raw8_framefmt_regs[] = {
+	{ IMX219_REG_CSI_DATA_FORMAT_A, 0x0808 },
+	{ IMX219_REG_OPPXCK_DIV, 8 },
+};
+
+static const struct cci_reg_sequence raw10_framefmt_regs[] = {
+	{ IMX219_REG_CSI_DATA_FORMAT_A, 0x0a0a },
+	{ IMX219_REG_OPPXCK_DIV, 10 },
+};
+
+static const s64 imx219_link_freq_menu[] = {
+	IMX219_DEFAULT_LINK_FREQ,
+};
+
+static const s64 imx219_link_freq_4lane_menu[] = {
+	IMX219_DEFAULT_LINK_FREQ_4LANE,
+};
+
+static const char * const imx219_test_pattern_menu[] = {
+	"Disabled",
+	"Color Bars",
+	"Solid Color",
+	"Grey Color Bars",
+	"PN9"
+};
+
+static const int imx219_test_pattern_val[] = {
+	IMX219_TEST_PATTERN_DISABLE,
+	IMX219_TEST_PATTERN_COLOR_BARS,
+	IMX219_TEST_PATTERN_SOLID_COLOR,
+	IMX219_TEST_PATTERN_GREY_COLOR,
+	IMX219_TEST_PATTERN_PN9,
+};
+
+/* regulator supplies */
+static const char * const imx219_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 */
+};
+
+#define IMX219_NUM_SUPPLIES ARRAY_SIZE(imx219_supply_name)
+
+/*
+ * The supported formats.
+ * This table MUST contain 4 entries per format, to cover the various flip
+ * combinations in the order
+ * - no flip
+ * - h flip
+ * - v flip
+ * - h&v flips
+ */
+static const u32 imx219_mbus_formats[] = {
+	MEDIA_BUS_FMT_SRGGB10_1X10,
+	MEDIA_BUS_FMT_SGRBG10_1X10,
+	MEDIA_BUS_FMT_SGBRG10_1X10,
+	MEDIA_BUS_FMT_SBGGR10_1X10,
+
+	MEDIA_BUS_FMT_SRGGB8_1X8,
+	MEDIA_BUS_FMT_SGRBG8_1X8,
+	MEDIA_BUS_FMT_SGBRG8_1X8,
+	MEDIA_BUS_FMT_SBGGR8_1X8,
+};
+
+/*
+ * Initialisation delay between XCLR low->high and the moment when the sensor
+ * can start capture (i.e. can leave software stanby) must be not less than:
+ *   t4 + max(t5, t6 + <time to initialize the sensor register over I2C>)
+ * where
+ *   t4 is fixed, and is max 200uS,
+ *   t5 is fixed, and is 6000uS,
+ *   t6 depends on the sensor external clock, and is max 32000 clock periods.
+ * As per sensor datasheet, the external clock must be from 6MHz to 27MHz.
+ * So for any acceptable external clock t6 is always within the range of
+ * 1185 to 5333 uS, and is always less than t5.
+ * For this reason this is always safe to wait (t4 + t5) = 6200 uS, then
+ * initialize the sensor over I2C, and then exit the software standby.
+ *
+ * This start-up time can be optimized a bit more, if we start the writes
+ * over I2C after (t4+t6), but before (t4+t5) expires. But then sensor
+ * initialization over I2C may complete before (t4+t5) expires, and we must
+ * ensure that capture is not started before (t4+t5).
+ *
+ * This delay doesn't account for the power supply startup time. If needed,
+ * this should be taken care of via the regulator framework. E.g. in the
+ * case of DT for regulator-fixed one should define the startup-delay-us
+ * property.
+ */
+#define IMX219_XCLR_MIN_DELAY_US	6200
+#define IMX219_XCLR_DELAY_RANGE_US	1000
+
+/* Mode configs */
+static const struct imx219_mode supported_modes[] = {
+	{
+		/* 8MPix 15fps mode */
+		.width = 3280,
+		.height = 2464,
+		.crop = {
+			.left = IMX219_PIXEL_ARRAY_LEFT,
+			.top = IMX219_PIXEL_ARRAY_TOP,
+			.width = 3280,
+			.height = 2464
+		},
+		.vts_def = IMX219_VTS_15FPS,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_3280x2464_regs),
+			.regs = mode_3280x2464_regs,
+		},
+		.binning = false,
+	},
+	{
+		/* 1080P 30fps cropped */
+		.width = 1920,
+		.height = 1080,
+		.crop = {
+			.left = 688,
+			.top = 700,
+			.width = 1920,
+			.height = 1080
+		},
+		.vts_def = IMX219_VTS_30FPS_1080P,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1920_1080_regs),
+			.regs = mode_1920_1080_regs,
+		},
+		.binning = false,
+	},
+	{
+		/* 2x2 binned 30fps mode */
+		.width = 1640,
+		.height = 1232,
+		.crop = {
+			.left = IMX219_PIXEL_ARRAY_LEFT,
+			.top = IMX219_PIXEL_ARRAY_TOP,
+			.width = 3280,
+			.height = 2464
+		},
+		.vts_def = IMX219_VTS_30FPS_BINNED,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1640_1232_regs),
+			.regs = mode_1640_1232_regs,
+		},
+		.binning = true,
+	},
+	{
+		/* 640x480 30fps mode */
+		.width = 640,
+		.height = 480,
+		.crop = {
+			.left = 1008,
+			.top = 760,
+			.width = 1280,
+			.height = 960
+		},
+		.vts_def = IMX219_VTS_30FPS_640x480,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_640_480_regs),
+			.regs = mode_640_480_regs,
+		},
+		.binning = true,
+	},
+};
+
+struct imx219 {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+
+	struct regmap *regmap;
+	struct clk *xclk; /* system clock to IMX219 */
+	u32 xclk_freq;
+
+	struct gpio_desc *reset_gpio;
+	struct regulator_bulk_data supplies[IMX219_NUM_SUPPLIES];
+
+	struct v4l2_ctrl_handler ctrl_handler;
+	/* V4L2 Controls */
+	struct v4l2_ctrl *pixel_rate;
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *exposure;
+	struct v4l2_ctrl *vflip;
+	struct v4l2_ctrl *hflip;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *hblank;
+
+	/* Current mode */
+	const struct imx219_mode *mode;
+
+	/* Streaming on/off */
+	bool streaming;
+
+	/* Two or Four lanes */
+	u8 lanes;
+};
+
+static inline struct imx219 *to_imx219(struct v4l2_subdev *_sd)
+{
+	return container_of(_sd, struct imx219, sd);
+}
+
+/* Get bayer order based on flip setting. */
+static u32 imx219_get_format_code(struct imx219 *imx219, u32 code)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(imx219_mbus_formats); i++)
+		if (imx219_mbus_formats[i] == code)
+			break;
+
+	if (i >= ARRAY_SIZE(imx219_mbus_formats))
+		i = 0;
+
+	i = (i & ~3) | (imx219->vflip->val ? 2 : 0) |
+	    (imx219->hflip->val ? 1 : 0);
+
+	return imx219_mbus_formats[i];
+}
+
+static int imx219_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct imx219 *imx219 =
+		container_of(ctrl->handler, struct imx219, ctrl_handler);
+	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+	int ret = 0;
+
+	if (ctrl->id == V4L2_CID_VBLANK) {
+		int exposure_max, exposure_def;
+
+		/* Update max exposure while meeting expected vblanking */
+		exposure_max = imx219->mode->height + ctrl->val - 4;
+		exposure_def = (exposure_max < IMX219_EXPOSURE_DEFAULT) ?
+			exposure_max : IMX219_EXPOSURE_DEFAULT;
+		__v4l2_ctrl_modify_range(imx219->exposure,
+					 imx219->exposure->minimum,
+					 exposure_max, imx219->exposure->step,
+					 exposure_def);
+	}
+
+	/*
+	 * Applying V4L2 control value only happens
+	 * when power is up for streaming
+	 */
+	if (pm_runtime_get_if_in_use(&client->dev) == 0)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_ANALOGUE_GAIN:
+		cci_write(imx219->regmap, IMX219_REG_ANALOG_GAIN,
+			  ctrl->val, &ret);
+		break;
+	case V4L2_CID_EXPOSURE:
+		cci_write(imx219->regmap, IMX219_REG_EXPOSURE,
+			  ctrl->val, &ret);
+		break;
+	case V4L2_CID_DIGITAL_GAIN:
+		cci_write(imx219->regmap, IMX219_REG_DIGITAL_GAIN,
+			  ctrl->val, &ret);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		cci_write(imx219->regmap, IMX219_REG_TEST_PATTERN,
+			  imx219_test_pattern_val[ctrl->val], &ret);
+		break;
+	case V4L2_CID_HFLIP:
+	case V4L2_CID_VFLIP:
+		cci_write(imx219->regmap, IMX219_REG_ORIENTATION,
+			  imx219->hflip->val | imx219->vflip->val << 1, &ret);
+		break;
+	case V4L2_CID_VBLANK:
+		cci_write(imx219->regmap, IMX219_REG_VTS,
+			  imx219->mode->height + ctrl->val, &ret);
+		break;
+	case V4L2_CID_TEST_PATTERN_RED:
+		cci_write(imx219->regmap, IMX219_REG_TESTP_RED,
+			  ctrl->val, &ret);
+		break;
+	case V4L2_CID_TEST_PATTERN_GREENR:
+		cci_write(imx219->regmap, IMX219_REG_TESTP_GREENR,
+			  ctrl->val, &ret);
+		break;
+	case V4L2_CID_TEST_PATTERN_BLUE:
+		cci_write(imx219->regmap, IMX219_REG_TESTP_BLUE,
+			  ctrl->val, &ret);
+		break;
+	case V4L2_CID_TEST_PATTERN_GREENB:
+		cci_write(imx219->regmap, IMX219_REG_TESTP_GREENB,
+			  ctrl->val, &ret);
+		break;
+	default:
+		dev_info(&client->dev,
+			 "ctrl(id:0x%x,val:0x%x) is not handled\n",
+			 ctrl->id, ctrl->val);
+		ret = -EINVAL;
+		break;
+	}
+
+	pm_runtime_put(&client->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops imx219_ctrl_ops = {
+	.s_ctrl = imx219_set_ctrl,
+};
+
+static void imx219_update_pad_format(struct imx219 *imx219,
+				     const struct imx219_mode *mode,
+				     struct v4l2_mbus_framefmt *fmt, u32 code)
+{
+	/* Bayer order varies with flips */
+	fmt->code = imx219_get_format_code(imx219, code);
+	fmt->width = mode->width;
+	fmt->height = mode->height;
+	fmt->field = V4L2_FIELD_NONE;
+	fmt->colorspace = V4L2_COLORSPACE_RAW;
+	fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
+	fmt->xfer_func = V4L2_XFER_FUNC_NONE;
+}
+
+static int imx219_init_cfg(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_state *state)
+{
+	struct imx219 *imx219 = to_imx219(sd);
+	struct v4l2_mbus_framefmt *format;
+	struct v4l2_rect *crop;
+
+	/* Initialize the format. */
+	format = v4l2_subdev_get_pad_format(sd, state, 0);
+	imx219_update_pad_format(imx219, &supported_modes[0], format,
+				 MEDIA_BUS_FMT_SRGGB10_1X10);
+
+	/* Initialize the crop rectangle. */
+	crop = v4l2_subdev_get_pad_crop(sd, state, 0);
+	crop->top = IMX219_PIXEL_ARRAY_TOP;
+	crop->left = IMX219_PIXEL_ARRAY_LEFT;
+	crop->width = IMX219_PIXEL_ARRAY_WIDTH;
+	crop->height = IMX219_PIXEL_ARRAY_HEIGHT;
+
+	return 0;
+}
+
+static int imx219_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct imx219 *imx219 = to_imx219(sd);
+
+	if (code->index >= (ARRAY_SIZE(imx219_mbus_formats) / 4))
+		return -EINVAL;
+
+	code->code = imx219_get_format_code(imx219, imx219_mbus_formats[code->index * 4]);
+
+	return 0;
+}
+
+static int imx219_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct imx219 *imx219 = to_imx219(sd);
+	u32 code;
+
+	if (fse->index >= ARRAY_SIZE(supported_modes))
+		return -EINVAL;
+
+	code = imx219_get_format_code(imx219, fse->code);
+	if (fse->code != code)
+		return -EINVAL;
+
+	fse->min_width = supported_modes[fse->index].width;
+	fse->max_width = fse->min_width;
+	fse->min_height = supported_modes[fse->index].height;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static int imx219_set_pad_format(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_format *fmt)
+{
+	struct imx219 *imx219 = to_imx219(sd);
+	const struct imx219_mode *mode;
+	int exposure_max, exposure_def, hblank;
+	struct v4l2_mbus_framefmt *format;
+	struct v4l2_rect *crop;
+
+	mode = v4l2_find_nearest_size(supported_modes,
+				      ARRAY_SIZE(supported_modes),
+				      width, height,
+				      fmt->format.width, fmt->format.height);
+
+	imx219_update_pad_format(imx219, mode, &fmt->format, fmt->format.code);
+
+	format = v4l2_subdev_get_pad_format(sd, sd_state, 0);
+	crop = v4l2_subdev_get_pad_crop(sd, sd_state, 0);
+
+	*format = fmt->format;
+	*crop = mode->crop;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		imx219->mode = mode;
+		/* Update limits and set FPS to default */
+		__v4l2_ctrl_modify_range(imx219->vblank, IMX219_VBLANK_MIN,
+					 IMX219_VTS_MAX - mode->height, 1,
+					 mode->vts_def - mode->height);
+		__v4l2_ctrl_s_ctrl(imx219->vblank,
+				   mode->vts_def - mode->height);
+		/* Update max exposure while meeting expected vblanking */
+		exposure_max = mode->vts_def - 4;
+		exposure_def = (exposure_max < IMX219_EXPOSURE_DEFAULT) ?
+			exposure_max : IMX219_EXPOSURE_DEFAULT;
+		__v4l2_ctrl_modify_range(imx219->exposure,
+					 imx219->exposure->minimum,
+					 exposure_max, imx219->exposure->step,
+					 exposure_def);
+		/*
+		 * Currently PPL is fixed to IMX219_PPL_DEFAULT, so hblank
+		 * depends on mode->width only, and is not changeble in any
+		 * way other than changing the mode.
+		 */
+		hblank = IMX219_PPL_DEFAULT - mode->width;
+		__v4l2_ctrl_modify_range(imx219->hblank, hblank, hblank, 1,
+					 hblank);
+	}
+
+	return 0;
+}
+
+static int imx219_set_framefmt(struct imx219 *imx219,
+			       const struct v4l2_mbus_framefmt *format)
+{
+	switch (format->code) {
+	case MEDIA_BUS_FMT_SRGGB8_1X8:
+	case MEDIA_BUS_FMT_SGRBG8_1X8:
+	case MEDIA_BUS_FMT_SGBRG8_1X8:
+	case MEDIA_BUS_FMT_SBGGR8_1X8:
+		return cci_multi_reg_write(imx219->regmap, raw8_framefmt_regs,
+					   ARRAY_SIZE(raw8_framefmt_regs), NULL);
+
+	case MEDIA_BUS_FMT_SRGGB10_1X10:
+	case MEDIA_BUS_FMT_SGRBG10_1X10:
+	case MEDIA_BUS_FMT_SGBRG10_1X10:
+	case MEDIA_BUS_FMT_SBGGR10_1X10:
+		return cci_multi_reg_write(imx219->regmap, raw10_framefmt_regs,
+					   ARRAY_SIZE(raw10_framefmt_regs), NULL);
+	}
+
+	return -EINVAL;
+}
+
+static int imx219_set_binning(struct imx219 *imx219,
+			      const struct v4l2_mbus_framefmt *format)
+{
+	if (!imx219->mode->binning)
+		return cci_write(imx219->regmap, IMX219_REG_BINNING_MODE,
+				 IMX219_BINNING_NONE, NULL);
+
+	switch (format->code) {
+	case MEDIA_BUS_FMT_SRGGB8_1X8:
+	case MEDIA_BUS_FMT_SGRBG8_1X8:
+	case MEDIA_BUS_FMT_SGBRG8_1X8:
+	case MEDIA_BUS_FMT_SBGGR8_1X8:
+		return cci_write(imx219->regmap, IMX219_REG_BINNING_MODE,
+				 IMX219_BINNING_2X2_ANALOG, NULL);
+
+	case MEDIA_BUS_FMT_SRGGB10_1X10:
+	case MEDIA_BUS_FMT_SGRBG10_1X10:
+	case MEDIA_BUS_FMT_SGBRG10_1X10:
+	case MEDIA_BUS_FMT_SBGGR10_1X10:
+		return cci_write(imx219->regmap, IMX219_REG_BINNING_MODE,
+				 IMX219_BINNING_2X2, NULL);
+	}
+
+	return -EINVAL;
+}
+
+static int imx219_get_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *sd_state,
+				struct v4l2_subdev_selection *sel)
+{
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP: {
+		sel->r = *v4l2_subdev_get_pad_crop(sd, sd_state, 0);
+		return 0;
+	}
+
+	case V4L2_SEL_TGT_NATIVE_SIZE:
+		sel->r.top = 0;
+		sel->r.left = 0;
+		sel->r.width = IMX219_NATIVE_WIDTH;
+		sel->r.height = IMX219_NATIVE_HEIGHT;
+
+		return 0;
+
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.top = IMX219_PIXEL_ARRAY_TOP;
+		sel->r.left = IMX219_PIXEL_ARRAY_LEFT;
+		sel->r.width = IMX219_PIXEL_ARRAY_WIDTH;
+		sel->r.height = IMX219_PIXEL_ARRAY_HEIGHT;
+
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int imx219_configure_lanes(struct imx219 *imx219)
+{
+	return cci_write(imx219->regmap, IMX219_REG_CSI_LANE_MODE,
+			 imx219->lanes == 2 ? IMX219_CSI_2_LANE_MODE :
+			 IMX219_CSI_4_LANE_MODE, NULL);
+};
+
+static int imx219_start_streaming(struct imx219 *imx219,
+				  struct v4l2_subdev_state *state)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+	const struct v4l2_mbus_framefmt *format;
+	const struct imx219_reg_list *reg_list;
+	int ret;
+
+	ret = pm_runtime_resume_and_get(&client->dev);
+	if (ret < 0)
+		return ret;
+
+	/* Send all registers that are common to all modes */
+	ret = cci_multi_reg_write(imx219->regmap, imx219_common_regs,
+				  ARRAY_SIZE(imx219_common_regs), NULL);
+	if (ret) {
+		dev_err(&client->dev, "%s failed to send mfg header\n", __func__);
+		goto err_rpm_put;
+	}
+
+	/* Configure two or four Lane mode */
+	ret = imx219_configure_lanes(imx219);
+	if (ret) {
+		dev_err(&client->dev, "%s failed to configure lanes\n", __func__);
+		goto err_rpm_put;
+	}
+
+	/* Apply default values of current mode */
+	reg_list = &imx219->mode->reg_list;
+	ret = cci_multi_reg_write(imx219->regmap, reg_list->regs,
+				  reg_list->num_of_regs, NULL);
+	if (ret) {
+		dev_err(&client->dev, "%s failed to set mode\n", __func__);
+		goto err_rpm_put;
+	}
+
+	format = v4l2_subdev_get_pad_format(&imx219->sd, state, 0);
+	ret = imx219_set_framefmt(imx219, format);
+	if (ret) {
+		dev_err(&client->dev, "%s failed to set frame format: %d\n",
+			__func__, ret);
+		goto err_rpm_put;
+	}
+
+	ret = imx219_set_binning(imx219, format);
+	if (ret) {
+		dev_err(&client->dev, "%s failed to set binning: %d\n",
+			__func__, ret);
+		goto err_rpm_put;
+	}
+
+	/* Apply customized values from user */
+	ret =  __v4l2_ctrl_handler_setup(imx219->sd.ctrl_handler);
+	if (ret)
+		goto err_rpm_put;
+
+	/* set stream on register */
+	ret = cci_write(imx219->regmap, IMX219_REG_MODE_SELECT,
+			IMX219_MODE_STREAMING, NULL);
+	if (ret)
+		goto err_rpm_put;
+
+	/* vflip and hflip cannot change during streaming */
+	__v4l2_ctrl_grab(imx219->vflip, true);
+	__v4l2_ctrl_grab(imx219->hflip, true);
+
+	return 0;
+
+err_rpm_put:
+	pm_runtime_put(&client->dev);
+	return ret;
+}
+
+static void imx219_stop_streaming(struct imx219 *imx219)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+	int ret;
+
+	/* set stream off register */
+	ret = cci_write(imx219->regmap, IMX219_REG_MODE_SELECT,
+			IMX219_MODE_STANDBY, NULL);
+	if (ret)
+		dev_err(&client->dev, "%s failed to set stream\n", __func__);
+
+	__v4l2_ctrl_grab(imx219->vflip, false);
+	__v4l2_ctrl_grab(imx219->hflip, false);
+
+	pm_runtime_put(&client->dev);
+}
+
+static int imx219_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct imx219 *imx219 = to_imx219(sd);
+	struct v4l2_subdev_state *state;
+	int ret = 0;
+
+	state = v4l2_subdev_lock_and_get_active_state(sd);
+
+	if (imx219->streaming == enable)
+		goto unlock;
+
+	if (enable) {
+		/*
+		 * Apply default & customized values
+		 * and then start streaming.
+		 */
+		ret = imx219_start_streaming(imx219, state);
+		if (ret)
+			goto unlock;
+	} else {
+		imx219_stop_streaming(imx219);
+	}
+
+	imx219->streaming = enable;
+
+unlock:
+	v4l2_subdev_unlock_state(state);
+	return ret;
+}
+
+/* Power/clock management functions */
+static int imx219_power_on(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct imx219 *imx219 = to_imx219(sd);
+	int ret;
+
+	ret = regulator_bulk_enable(IMX219_NUM_SUPPLIES,
+				    imx219->supplies);
+	if (ret) {
+		dev_err(dev, "%s: failed to enable regulators\n",
+			__func__);
+		return ret;
+	}
+
+	ret = clk_prepare_enable(imx219->xclk);
+	if (ret) {
+		dev_err(dev, "%s: failed to enable clock\n",
+			__func__);
+		goto reg_off;
+	}
+
+	gpiod_set_value_cansleep(imx219->reset_gpio, 1);
+	usleep_range(IMX219_XCLR_MIN_DELAY_US,
+		     IMX219_XCLR_MIN_DELAY_US + IMX219_XCLR_DELAY_RANGE_US);
+
+	return 0;
+
+reg_off:
+	regulator_bulk_disable(IMX219_NUM_SUPPLIES, imx219->supplies);
+
+	return ret;
+}
+
+static int imx219_power_off(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct imx219 *imx219 = to_imx219(sd);
+
+	gpiod_set_value_cansleep(imx219->reset_gpio, 0);
+	regulator_bulk_disable(IMX219_NUM_SUPPLIES, imx219->supplies);
+	clk_disable_unprepare(imx219->xclk);
+
+	return 0;
+}
+
+static int __maybe_unused imx219_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct imx219 *imx219 = to_imx219(sd);
+
+	if (imx219->streaming)
+		imx219_stop_streaming(imx219);
+
+	return 0;
+}
+
+static int __maybe_unused imx219_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct imx219 *imx219 = to_imx219(sd);
+	struct v4l2_subdev_state *state;
+	int ret;
+
+	if (imx219->streaming) {
+		state = v4l2_subdev_lock_and_get_active_state(sd);
+		ret = imx219_start_streaming(imx219, state);
+		v4l2_subdev_unlock_state(state);
+		if (ret)
+			goto error;
+	}
+
+	return 0;
+
+error:
+	imx219_stop_streaming(imx219);
+	imx219->streaming = false;
+
+	return ret;
+}
+
+static int imx219_get_regulators(struct imx219 *imx219)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+	unsigned int i;
+
+	for (i = 0; i < IMX219_NUM_SUPPLIES; i++)
+		imx219->supplies[i].supply = imx219_supply_name[i];
+
+	return devm_regulator_bulk_get(&client->dev,
+				       IMX219_NUM_SUPPLIES,
+				       imx219->supplies);
+}
+
+/* Verify chip ID */
+static int imx219_identify_module(struct imx219 *imx219)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+	int ret;
+	u64 val;
+
+	ret = cci_read(imx219->regmap, IMX219_REG_CHIP_ID, &val, NULL);
+	if (ret) {
+		dev_err(&client->dev, "failed to read chip id %x\n",
+			IMX219_CHIP_ID);
+		return ret;
+	}
+
+	if (val != IMX219_CHIP_ID) {
+		dev_err(&client->dev, "chip id mismatch: %x!=%llx\n",
+			IMX219_CHIP_ID, val);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops imx219_core_ops = {
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_video_ops imx219_video_ops = {
+	.s_stream = imx219_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops imx219_pad_ops = {
+	.init_cfg = imx219_init_cfg,
+	.enum_mbus_code = imx219_enum_mbus_code,
+	.get_fmt = v4l2_subdev_get_fmt,
+	.set_fmt = imx219_set_pad_format,
+	.get_selection = imx219_get_selection,
+	.enum_frame_size = imx219_enum_frame_size,
+};
+
+static const struct v4l2_subdev_ops imx219_subdev_ops = {
+	.core = &imx219_core_ops,
+	.video = &imx219_video_ops,
+	.pad = &imx219_pad_ops,
+};
+
+
+static unsigned long imx219_get_pixel_rate(struct imx219 *imx219)
+{
+	return (imx219->lanes == 2) ? IMX219_PIXEL_RATE : IMX219_PIXEL_RATE_4LANE;
+}
+
+/* Initialize control handlers */
+static int imx219_init_controls(struct imx219 *imx219)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+	struct v4l2_ctrl_handler *ctrl_hdlr;
+	unsigned int height = imx219->mode->height;
+	struct v4l2_fwnode_device_properties props;
+	int exposure_max, exposure_def, hblank;
+	int i, ret;
+
+	ctrl_hdlr = &imx219->ctrl_handler;
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 12);
+	if (ret)
+		return ret;
+
+	/* By default, PIXEL_RATE is read only */
+	imx219->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
+					       V4L2_CID_PIXEL_RATE,
+					       imx219_get_pixel_rate(imx219),
+					       imx219_get_pixel_rate(imx219), 1,
+					       imx219_get_pixel_rate(imx219));
+
+	imx219->link_freq =
+		v4l2_ctrl_new_int_menu(ctrl_hdlr, &imx219_ctrl_ops,
+				       V4L2_CID_LINK_FREQ,
+				       ARRAY_SIZE(imx219_link_freq_menu) - 1, 0,
+				       (imx219->lanes == 2) ? imx219_link_freq_menu :
+				       imx219_link_freq_4lane_menu);
+	if (imx219->link_freq)
+		imx219->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	/* Initial vblank/hblank/exposure parameters based on current mode */
+	imx219->vblank = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
+					   V4L2_CID_VBLANK, IMX219_VBLANK_MIN,
+					   IMX219_VTS_MAX - height, 1,
+					   imx219->mode->vts_def - height);
+	hblank = IMX219_PPL_DEFAULT - imx219->mode->width;
+	imx219->hblank = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
+					   V4L2_CID_HBLANK, hblank, hblank,
+					   1, hblank);
+	if (imx219->hblank)
+		imx219->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+	exposure_max = imx219->mode->vts_def - 4;
+	exposure_def = (exposure_max < IMX219_EXPOSURE_DEFAULT) ?
+		exposure_max : IMX219_EXPOSURE_DEFAULT;
+	imx219->exposure = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
+					     V4L2_CID_EXPOSURE,
+					     IMX219_EXPOSURE_MIN, exposure_max,
+					     IMX219_EXPOSURE_STEP,
+					     exposure_def);
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
+			  IMX219_ANA_GAIN_MIN, IMX219_ANA_GAIN_MAX,
+			  IMX219_ANA_GAIN_STEP, IMX219_ANA_GAIN_DEFAULT);
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
+			  IMX219_DGTL_GAIN_MIN, IMX219_DGTL_GAIN_MAX,
+			  IMX219_DGTL_GAIN_STEP, IMX219_DGTL_GAIN_DEFAULT);
+
+	imx219->hflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
+					  V4L2_CID_HFLIP, 0, 1, 1, 0);
+	if (imx219->hflip)
+		imx219->hflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
+
+	imx219->vflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
+					  V4L2_CID_VFLIP, 0, 1, 1, 0);
+	if (imx219->vflip)
+		imx219->vflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
+
+	v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &imx219_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(imx219_test_pattern_menu) - 1,
+				     0, 0, imx219_test_pattern_menu);
+	for (i = 0; i < 4; i++) {
+		/*
+		 * The assumption is that
+		 * V4L2_CID_TEST_PATTERN_GREENR == V4L2_CID_TEST_PATTERN_RED + 1
+		 * V4L2_CID_TEST_PATTERN_BLUE   == V4L2_CID_TEST_PATTERN_RED + 2
+		 * V4L2_CID_TEST_PATTERN_GREENB == V4L2_CID_TEST_PATTERN_RED + 3
+		 */
+		v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
+				  V4L2_CID_TEST_PATTERN_RED + i,
+				  IMX219_TESTP_COLOUR_MIN,
+				  IMX219_TESTP_COLOUR_MAX,
+				  IMX219_TESTP_COLOUR_STEP,
+				  IMX219_TESTP_COLOUR_MAX);
+		/* The "Solid color" pattern is white by default */
+	}
+
+	if (ctrl_hdlr->error) {
+		ret = ctrl_hdlr->error;
+		dev_err(&client->dev, "%s control init failed (%d)\n",
+			__func__, ret);
+		goto error;
+	}
+
+	ret = v4l2_fwnode_device_parse(&client->dev, &props);
+	if (ret)
+		goto error;
+
+	ret = v4l2_ctrl_new_fwnode_properties(ctrl_hdlr, &imx219_ctrl_ops,
+					      &props);
+	if (ret)
+		goto error;
+
+	imx219->sd.ctrl_handler = ctrl_hdlr;
+
+	return 0;
+
+error:
+	v4l2_ctrl_handler_free(ctrl_hdlr);
+
+	return ret;
+}
+
+static void imx219_free_controls(struct imx219 *imx219)
+{
+	v4l2_ctrl_handler_free(imx219->sd.ctrl_handler);
+}
+
+static int imx219_check_hwcfg(struct device *dev, struct imx219 *imx219)
+{
+	struct fwnode_handle *endpoint;
+	struct v4l2_fwnode_endpoint ep_cfg = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY
+	};
+	int ret = -EINVAL;
+
+	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;
+	}
+
+	/* Check the number of MIPI CSI2 data lanes */
+	if (ep_cfg.bus.mipi_csi2.num_data_lanes != 2 &&
+	    ep_cfg.bus.mipi_csi2.num_data_lanes != 4) {
+		dev_err(dev, "only 2 or 4 data lanes are currently supported\n");
+		goto error_out;
+	}
+	imx219->lanes = ep_cfg.bus.mipi_csi2.num_data_lanes;
+
+	/* Check the link frequency set in device tree */
+	if (!ep_cfg.nr_of_link_frequencies) {
+		dev_err(dev, "link-frequency property not found in DT\n");
+		goto error_out;
+	}
+
+	if (ep_cfg.nr_of_link_frequencies != 1 ||
+	   (ep_cfg.link_frequencies[0] != ((imx219->lanes == 2) ?
+	    IMX219_DEFAULT_LINK_FREQ : IMX219_DEFAULT_LINK_FREQ_4LANE))) {
+		dev_err(dev, "Link frequency not supported: %lld\n",
+			ep_cfg.link_frequencies[0]);
+		goto error_out;
+	}
+
+	ret = 0;
+
+error_out:
+	v4l2_fwnode_endpoint_free(&ep_cfg);
+	fwnode_handle_put(endpoint);
+
+	return ret;
+}
+
+static int imx219_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct imx219 *imx219;
+	int ret;
+
+	imx219 = devm_kzalloc(&client->dev, sizeof(*imx219), GFP_KERNEL);
+	if (!imx219)
+		return -ENOMEM;
+
+	v4l2_i2c_subdev_init(&imx219->sd, client, &imx219_subdev_ops);
+
+	/* Check the hardware configuration in device tree */
+	if (imx219_check_hwcfg(dev, imx219))
+		return -EINVAL;
+
+	imx219->regmap = devm_cci_regmap_init_i2c(client, 16);
+	if (IS_ERR(imx219->regmap)) {
+		ret = PTR_ERR(imx219->regmap);
+		dev_err(dev, "failed to initialize CCI: %d\n", ret);
+		return ret;
+	}
+
+	/* Get system clock (xclk) */
+	imx219->xclk = devm_clk_get(dev, NULL);
+	if (IS_ERR(imx219->xclk)) {
+		dev_err(dev, "failed to get xclk\n");
+		return PTR_ERR(imx219->xclk);
+	}
+
+	imx219->xclk_freq = clk_get_rate(imx219->xclk);
+	if (imx219->xclk_freq != IMX219_XCLK_FREQ) {
+		dev_err(dev, "xclk frequency not supported: %d Hz\n",
+			imx219->xclk_freq);
+		return -EINVAL;
+	}
+
+	ret = imx219_get_regulators(imx219);
+	if (ret) {
+		dev_err(dev, "failed to get regulators\n");
+		return ret;
+	}
+
+	/* Request optional enable pin */
+	imx219->reset_gpio = devm_gpiod_get_optional(dev, "reset",
+						     GPIOD_OUT_HIGH);
+
+	/*
+	 * The sensor must be powered for imx219_identify_module()
+	 * to be able to read the CHIP_ID register
+	 */
+	ret = imx219_power_on(dev);
+	if (ret)
+		return ret;
+
+	ret = imx219_identify_module(imx219);
+	if (ret)
+		goto error_power_off;
+
+	/* Set default mode to max resolution */
+	imx219->mode = &supported_modes[0];
+
+	/* sensor doesn't enter LP-11 state upon power up until and unless
+	 * streaming is started, so upon power up switch the modes to:
+	 * streaming -> standby
+	 */
+	ret = cci_write(imx219->regmap, IMX219_REG_MODE_SELECT,
+			IMX219_MODE_STREAMING, NULL);
+	if (ret < 0)
+		goto error_power_off;
+
+	usleep_range(100, 110);
+
+	/* put sensor back to standby mode */
+	ret = cci_write(imx219->regmap, IMX219_REG_MODE_SELECT,
+			IMX219_MODE_STANDBY, NULL);
+	if (ret < 0)
+		goto error_power_off;
+
+	usleep_range(100, 110);
+
+	ret = imx219_init_controls(imx219);
+	if (ret)
+		goto error_power_off;
+
+	/* Initialize subdev */
+	imx219->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+			    V4L2_SUBDEV_FL_HAS_EVENTS;
+	imx219->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+	/* Initialize source pad */
+	imx219->pad.flags = MEDIA_PAD_FL_SOURCE;
+
+	ret = media_entity_pads_init(&imx219->sd.entity, 1, &imx219->pad);
+	if (ret) {
+		dev_err(dev, "failed to init entity pads: %d\n", ret);
+		goto error_handler_free;
+	}
+
+	imx219->sd.state_lock = imx219->ctrl_handler.lock;
+	ret = v4l2_subdev_init_finalize(&imx219->sd);
+	if (ret < 0) {
+		dev_err(dev, "subdev init error: %d\n", ret);
+		goto error_media_entity;
+	}
+
+	ret = v4l2_async_register_subdev_sensor(&imx219->sd);
+	if (ret < 0) {
+		dev_err(dev, "failed to register sensor sub-device: %d\n", ret);
+		goto error_subdev_cleanup;
+	}
+
+	/* Enable runtime PM and turn off the device */
+	pm_runtime_set_active(dev);
+	pm_runtime_enable(dev);
+	pm_runtime_idle(dev);
+
+	return 0;
+
+error_subdev_cleanup:
+	v4l2_subdev_cleanup(&imx219->sd);
+
+error_media_entity:
+	media_entity_cleanup(&imx219->sd.entity);
+
+error_handler_free:
+	imx219_free_controls(imx219);
+
+error_power_off:
+	imx219_power_off(dev);
+
+	return ret;
+}
+
+static void imx219_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct imx219 *imx219 = to_imx219(sd);
+
+	v4l2_async_unregister_subdev(sd);
+	v4l2_subdev_cleanup(sd);
+	media_entity_cleanup(&sd->entity);
+	imx219_free_controls(imx219);
+
+	pm_runtime_disable(&client->dev);
+	if (!pm_runtime_status_suspended(&client->dev))
+		imx219_power_off(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+}
+
+static const struct of_device_id imx219_dt_ids[] = {
+	{ .compatible = "sony,imx219" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, imx219_dt_ids);
+
+static const struct dev_pm_ops imx219_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(imx219_suspend, imx219_resume)
+	SET_RUNTIME_PM_OPS(imx219_power_off, imx219_power_on, NULL)
+};
+
+static struct i2c_driver imx219_i2c_driver = {
+	.driver = {
+		.name = "imx219",
+		.of_match_table	= imx219_dt_ids,
+		.pm = &imx219_pm_ops,
+	},
+	.probe = imx219_probe,
+	.remove = imx219_remove,
+};
+
+module_i2c_driver(imx219_i2c_driver);
+
+MODULE_AUTHOR("Dave Stevenson <dave.stevenson@raspberrypi.com");
+MODULE_DESCRIPTION("Sony IMX219 sensor driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/imx258.c b/drivers/media/i2c/imx258.c
new file mode 100644
index 000000000..e196565e8
--- /dev/null
+++ b/drivers/media/i2c/imx258.c
@@ -0,0 +1,1408 @@
+// SPDX-License-Identifier: GPL-2.0
+// Copyright (C) 2018 Intel Corporation
+
+#include <linux/acpi.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+#include <asm/unaligned.h>
+
+#define IMX258_REG_VALUE_08BIT		1
+#define IMX258_REG_VALUE_16BIT		2
+
+#define IMX258_REG_MODE_SELECT		0x0100
+#define IMX258_MODE_STANDBY		0x00
+#define IMX258_MODE_STREAMING		0x01
+
+/* Chip ID */
+#define IMX258_REG_CHIP_ID		0x0016
+#define IMX258_CHIP_ID			0x0258
+
+/* V_TIMING internal */
+#define IMX258_VTS_30FPS		0x0c50
+#define IMX258_VTS_30FPS_2K		0x0638
+#define IMX258_VTS_30FPS_VGA		0x034c
+#define IMX258_VTS_MAX			0xffff
+
+/*Frame Length Line*/
+#define IMX258_FLL_MIN			0x08a6
+#define IMX258_FLL_MAX			0xffff
+#define IMX258_FLL_STEP			1
+#define IMX258_FLL_DEFAULT		0x0c98
+
+/* HBLANK control - read only */
+#define IMX258_PPL_DEFAULT		5352
+
+/* Exposure control */
+#define IMX258_REG_EXPOSURE		0x0202
+#define IMX258_EXPOSURE_MIN		4
+#define IMX258_EXPOSURE_STEP		1
+#define IMX258_EXPOSURE_DEFAULT		0x640
+#define IMX258_EXPOSURE_MAX		65535
+
+/* Analog gain control */
+#define IMX258_REG_ANALOG_GAIN		0x0204
+#define IMX258_ANA_GAIN_MIN		0
+#define IMX258_ANA_GAIN_MAX		480
+#define IMX258_ANA_GAIN_STEP		1
+#define IMX258_ANA_GAIN_DEFAULT		0x0
+
+/* Digital gain control */
+#define IMX258_REG_GR_DIGITAL_GAIN	0x020e
+#define IMX258_REG_R_DIGITAL_GAIN	0x0210
+#define IMX258_REG_B_DIGITAL_GAIN	0x0212
+#define IMX258_REG_GB_DIGITAL_GAIN	0x0214
+#define IMX258_DGTL_GAIN_MIN		0
+#define IMX258_DGTL_GAIN_MAX		4096	/* Max = 0xFFF */
+#define IMX258_DGTL_GAIN_DEFAULT	1024
+#define IMX258_DGTL_GAIN_STEP		1
+
+/* HDR control */
+#define IMX258_REG_HDR			0x0220
+#define IMX258_HDR_ON			BIT(0)
+#define IMX258_REG_HDR_RATIO		0x0222
+#define IMX258_HDR_RATIO_MIN		0
+#define IMX258_HDR_RATIO_MAX		5
+#define IMX258_HDR_RATIO_STEP		1
+#define IMX258_HDR_RATIO_DEFAULT	0x0
+
+/* Test Pattern Control */
+#define IMX258_REG_TEST_PATTERN		0x0600
+
+/* Orientation */
+#define REG_MIRROR_FLIP_CONTROL		0x0101
+#define REG_CONFIG_MIRROR_FLIP		0x03
+#define REG_CONFIG_FLIP_TEST_PATTERN	0x02
+
+/* Input clock frequency in Hz */
+#define IMX258_INPUT_CLOCK_FREQ		19200000
+
+struct imx258_reg {
+	u16 address;
+	u8 val;
+};
+
+struct imx258_reg_list {
+	u32 num_of_regs;
+	const struct imx258_reg *regs;
+};
+
+/* Link frequency config */
+struct imx258_link_freq_config {
+	u32 pixels_per_line;
+
+	/* PLL registers for this link frequency */
+	struct imx258_reg_list reg_list;
+};
+
+/* Mode : resolution and related config&values */
+struct imx258_mode {
+	/* Frame width */
+	u32 width;
+	/* Frame height */
+	u32 height;
+
+	/* V-timing */
+	u32 vts_def;
+	u32 vts_min;
+
+	/* Index of Link frequency config to be used */
+	u32 link_freq_index;
+	/* Default register values */
+	struct imx258_reg_list reg_list;
+};
+
+/* 4208x3118 needs 1267Mbps/lane, 4 lanes */
+static const struct imx258_reg mipi_data_rate_1267mbps[] = {
+	{ 0x0301, 0x05 },
+	{ 0x0303, 0x02 },
+	{ 0x0305, 0x03 },
+	{ 0x0306, 0x00 },
+	{ 0x0307, 0xC6 },
+	{ 0x0309, 0x0A },
+	{ 0x030B, 0x01 },
+	{ 0x030D, 0x02 },
+	{ 0x030E, 0x00 },
+	{ 0x030F, 0xD8 },
+	{ 0x0310, 0x00 },
+	{ 0x0820, 0x13 },
+	{ 0x0821, 0x4C },
+	{ 0x0822, 0xCC },
+	{ 0x0823, 0xCC },
+};
+
+static const struct imx258_reg mipi_data_rate_640mbps[] = {
+	{ 0x0301, 0x05 },
+	{ 0x0303, 0x02 },
+	{ 0x0305, 0x03 },
+	{ 0x0306, 0x00 },
+	{ 0x0307, 0x64 },
+	{ 0x0309, 0x0A },
+	{ 0x030B, 0x01 },
+	{ 0x030D, 0x02 },
+	{ 0x030E, 0x00 },
+	{ 0x030F, 0xD8 },
+	{ 0x0310, 0x00 },
+	{ 0x0820, 0x0A },
+	{ 0x0821, 0x00 },
+	{ 0x0822, 0x00 },
+	{ 0x0823, 0x00 },
+};
+
+static const struct imx258_reg mode_4208x3118_regs[] = {
+	{ 0x0136, 0x13 },
+	{ 0x0137, 0x33 },
+	{ 0x3051, 0x00 },
+	{ 0x3052, 0x00 },
+	{ 0x4E21, 0x14 },
+	{ 0x6B11, 0xCF },
+	{ 0x7FF0, 0x08 },
+	{ 0x7FF1, 0x0F },
+	{ 0x7FF2, 0x08 },
+	{ 0x7FF3, 0x1B },
+	{ 0x7FF4, 0x23 },
+	{ 0x7FF5, 0x60 },
+	{ 0x7FF6, 0x00 },
+	{ 0x7FF7, 0x01 },
+	{ 0x7FF8, 0x00 },
+	{ 0x7FF9, 0x78 },
+	{ 0x7FFA, 0x00 },
+	{ 0x7FFB, 0x00 },
+	{ 0x7FFC, 0x00 },
+	{ 0x7FFD, 0x00 },
+	{ 0x7FFE, 0x00 },
+	{ 0x7FFF, 0x03 },
+	{ 0x7F76, 0x03 },
+	{ 0x7F77, 0xFE },
+	{ 0x7FA8, 0x03 },
+	{ 0x7FA9, 0xFE },
+	{ 0x7B24, 0x81 },
+	{ 0x7B25, 0x00 },
+	{ 0x6564, 0x07 },
+	{ 0x6B0D, 0x41 },
+	{ 0x653D, 0x04 },
+	{ 0x6B05, 0x8C },
+	{ 0x6B06, 0xF9 },
+	{ 0x6B08, 0x65 },
+	{ 0x6B09, 0xFC },
+	{ 0x6B0A, 0xCF },
+	{ 0x6B0B, 0xD2 },
+	{ 0x6700, 0x0E },
+	{ 0x6707, 0x0E },
+	{ 0x9104, 0x00 },
+	{ 0x4648, 0x7F },
+	{ 0x7420, 0x00 },
+	{ 0x7421, 0x1C },
+	{ 0x7422, 0x00 },
+	{ 0x7423, 0xD7 },
+	{ 0x5F04, 0x00 },
+	{ 0x5F05, 0xED },
+	{ 0x0112, 0x0A },
+	{ 0x0113, 0x0A },
+	{ 0x0114, 0x03 },
+	{ 0x0342, 0x14 },
+	{ 0x0343, 0xE8 },
+	{ 0x0340, 0x0C },
+	{ 0x0341, 0x50 },
+	{ 0x0344, 0x00 },
+	{ 0x0345, 0x00 },
+	{ 0x0346, 0x00 },
+	{ 0x0347, 0x00 },
+	{ 0x0348, 0x10 },
+	{ 0x0349, 0x6F },
+	{ 0x034A, 0x0C },
+	{ 0x034B, 0x2E },
+	{ 0x0381, 0x01 },
+	{ 0x0383, 0x01 },
+	{ 0x0385, 0x01 },
+	{ 0x0387, 0x01 },
+	{ 0x0900, 0x00 },
+	{ 0x0901, 0x11 },
+	{ 0x0401, 0x00 },
+	{ 0x0404, 0x00 },
+	{ 0x0405, 0x10 },
+	{ 0x0408, 0x00 },
+	{ 0x0409, 0x00 },
+	{ 0x040A, 0x00 },
+	{ 0x040B, 0x00 },
+	{ 0x040C, 0x10 },
+	{ 0x040D, 0x70 },
+	{ 0x040E, 0x0C },
+	{ 0x040F, 0x30 },
+	{ 0x3038, 0x00 },
+	{ 0x303A, 0x00 },
+	{ 0x303B, 0x10 },
+	{ 0x300D, 0x00 },
+	{ 0x034C, 0x10 },
+	{ 0x034D, 0x70 },
+	{ 0x034E, 0x0C },
+	{ 0x034F, 0x30 },
+	{ 0x0350, 0x01 },
+	{ 0x0202, 0x0C },
+	{ 0x0203, 0x46 },
+	{ 0x0204, 0x00 },
+	{ 0x0205, 0x00 },
+	{ 0x020E, 0x01 },
+	{ 0x020F, 0x00 },
+	{ 0x0210, 0x01 },
+	{ 0x0211, 0x00 },
+	{ 0x0212, 0x01 },
+	{ 0x0213, 0x00 },
+	{ 0x0214, 0x01 },
+	{ 0x0215, 0x00 },
+	{ 0x7BCD, 0x00 },
+	{ 0x94DC, 0x20 },
+	{ 0x94DD, 0x20 },
+	{ 0x94DE, 0x20 },
+	{ 0x95DC, 0x20 },
+	{ 0x95DD, 0x20 },
+	{ 0x95DE, 0x20 },
+	{ 0x7FB0, 0x00 },
+	{ 0x9010, 0x3E },
+	{ 0x9419, 0x50 },
+	{ 0x941B, 0x50 },
+	{ 0x9519, 0x50 },
+	{ 0x951B, 0x50 },
+	{ 0x3030, 0x00 },
+	{ 0x3032, 0x00 },
+	{ 0x0220, 0x00 },
+};
+
+static const struct imx258_reg mode_2104_1560_regs[] = {
+	{ 0x0136, 0x13 },
+	{ 0x0137, 0x33 },
+	{ 0x3051, 0x00 },
+	{ 0x3052, 0x00 },
+	{ 0x4E21, 0x14 },
+	{ 0x6B11, 0xCF },
+	{ 0x7FF0, 0x08 },
+	{ 0x7FF1, 0x0F },
+	{ 0x7FF2, 0x08 },
+	{ 0x7FF3, 0x1B },
+	{ 0x7FF4, 0x23 },
+	{ 0x7FF5, 0x60 },
+	{ 0x7FF6, 0x00 },
+	{ 0x7FF7, 0x01 },
+	{ 0x7FF8, 0x00 },
+	{ 0x7FF9, 0x78 },
+	{ 0x7FFA, 0x00 },
+	{ 0x7FFB, 0x00 },
+	{ 0x7FFC, 0x00 },
+	{ 0x7FFD, 0x00 },
+	{ 0x7FFE, 0x00 },
+	{ 0x7FFF, 0x03 },
+	{ 0x7F76, 0x03 },
+	{ 0x7F77, 0xFE },
+	{ 0x7FA8, 0x03 },
+	{ 0x7FA9, 0xFE },
+	{ 0x7B24, 0x81 },
+	{ 0x7B25, 0x00 },
+	{ 0x6564, 0x07 },
+	{ 0x6B0D, 0x41 },
+	{ 0x653D, 0x04 },
+	{ 0x6B05, 0x8C },
+	{ 0x6B06, 0xF9 },
+	{ 0x6B08, 0x65 },
+	{ 0x6B09, 0xFC },
+	{ 0x6B0A, 0xCF },
+	{ 0x6B0B, 0xD2 },
+	{ 0x6700, 0x0E },
+	{ 0x6707, 0x0E },
+	{ 0x9104, 0x00 },
+	{ 0x4648, 0x7F },
+	{ 0x7420, 0x00 },
+	{ 0x7421, 0x1C },
+	{ 0x7422, 0x00 },
+	{ 0x7423, 0xD7 },
+	{ 0x5F04, 0x00 },
+	{ 0x5F05, 0xED },
+	{ 0x0112, 0x0A },
+	{ 0x0113, 0x0A },
+	{ 0x0114, 0x03 },
+	{ 0x0342, 0x14 },
+	{ 0x0343, 0xE8 },
+	{ 0x0340, 0x06 },
+	{ 0x0341, 0x38 },
+	{ 0x0344, 0x00 },
+	{ 0x0345, 0x00 },
+	{ 0x0346, 0x00 },
+	{ 0x0347, 0x00 },
+	{ 0x0348, 0x10 },
+	{ 0x0349, 0x6F },
+	{ 0x034A, 0x0C },
+	{ 0x034B, 0x2E },
+	{ 0x0381, 0x01 },
+	{ 0x0383, 0x01 },
+	{ 0x0385, 0x01 },
+	{ 0x0387, 0x01 },
+	{ 0x0900, 0x01 },
+	{ 0x0901, 0x12 },
+	{ 0x0401, 0x01 },
+	{ 0x0404, 0x00 },
+	{ 0x0405, 0x20 },
+	{ 0x0408, 0x00 },
+	{ 0x0409, 0x02 },
+	{ 0x040A, 0x00 },
+	{ 0x040B, 0x00 },
+	{ 0x040C, 0x10 },
+	{ 0x040D, 0x6A },
+	{ 0x040E, 0x06 },
+	{ 0x040F, 0x18 },
+	{ 0x3038, 0x00 },
+	{ 0x303A, 0x00 },
+	{ 0x303B, 0x10 },
+	{ 0x300D, 0x00 },
+	{ 0x034C, 0x08 },
+	{ 0x034D, 0x38 },
+	{ 0x034E, 0x06 },
+	{ 0x034F, 0x18 },
+	{ 0x0350, 0x01 },
+	{ 0x0202, 0x06 },
+	{ 0x0203, 0x2E },
+	{ 0x0204, 0x00 },
+	{ 0x0205, 0x00 },
+	{ 0x020E, 0x01 },
+	{ 0x020F, 0x00 },
+	{ 0x0210, 0x01 },
+	{ 0x0211, 0x00 },
+	{ 0x0212, 0x01 },
+	{ 0x0213, 0x00 },
+	{ 0x0214, 0x01 },
+	{ 0x0215, 0x00 },
+	{ 0x7BCD, 0x01 },
+	{ 0x94DC, 0x20 },
+	{ 0x94DD, 0x20 },
+	{ 0x94DE, 0x20 },
+	{ 0x95DC, 0x20 },
+	{ 0x95DD, 0x20 },
+	{ 0x95DE, 0x20 },
+	{ 0x7FB0, 0x00 },
+	{ 0x9010, 0x3E },
+	{ 0x9419, 0x50 },
+	{ 0x941B, 0x50 },
+	{ 0x9519, 0x50 },
+	{ 0x951B, 0x50 },
+	{ 0x3030, 0x00 },
+	{ 0x3032, 0x00 },
+	{ 0x0220, 0x00 },
+};
+
+static const struct imx258_reg mode_1048_780_regs[] = {
+	{ 0x0136, 0x13 },
+	{ 0x0137, 0x33 },
+	{ 0x3051, 0x00 },
+	{ 0x3052, 0x00 },
+	{ 0x4E21, 0x14 },
+	{ 0x6B11, 0xCF },
+	{ 0x7FF0, 0x08 },
+	{ 0x7FF1, 0x0F },
+	{ 0x7FF2, 0x08 },
+	{ 0x7FF3, 0x1B },
+	{ 0x7FF4, 0x23 },
+	{ 0x7FF5, 0x60 },
+	{ 0x7FF6, 0x00 },
+	{ 0x7FF7, 0x01 },
+	{ 0x7FF8, 0x00 },
+	{ 0x7FF9, 0x78 },
+	{ 0x7FFA, 0x00 },
+	{ 0x7FFB, 0x00 },
+	{ 0x7FFC, 0x00 },
+	{ 0x7FFD, 0x00 },
+	{ 0x7FFE, 0x00 },
+	{ 0x7FFF, 0x03 },
+	{ 0x7F76, 0x03 },
+	{ 0x7F77, 0xFE },
+	{ 0x7FA8, 0x03 },
+	{ 0x7FA9, 0xFE },
+	{ 0x7B24, 0x81 },
+	{ 0x7B25, 0x00 },
+	{ 0x6564, 0x07 },
+	{ 0x6B0D, 0x41 },
+	{ 0x653D, 0x04 },
+	{ 0x6B05, 0x8C },
+	{ 0x6B06, 0xF9 },
+	{ 0x6B08, 0x65 },
+	{ 0x6B09, 0xFC },
+	{ 0x6B0A, 0xCF },
+	{ 0x6B0B, 0xD2 },
+	{ 0x6700, 0x0E },
+	{ 0x6707, 0x0E },
+	{ 0x9104, 0x00 },
+	{ 0x4648, 0x7F },
+	{ 0x7420, 0x00 },
+	{ 0x7421, 0x1C },
+	{ 0x7422, 0x00 },
+	{ 0x7423, 0xD7 },
+	{ 0x5F04, 0x00 },
+	{ 0x5F05, 0xED },
+	{ 0x0112, 0x0A },
+	{ 0x0113, 0x0A },
+	{ 0x0114, 0x03 },
+	{ 0x0342, 0x14 },
+	{ 0x0343, 0xE8 },
+	{ 0x0340, 0x03 },
+	{ 0x0341, 0x4C },
+	{ 0x0344, 0x00 },
+	{ 0x0345, 0x00 },
+	{ 0x0346, 0x00 },
+	{ 0x0347, 0x00 },
+	{ 0x0348, 0x10 },
+	{ 0x0349, 0x6F },
+	{ 0x034A, 0x0C },
+	{ 0x034B, 0x2E },
+	{ 0x0381, 0x01 },
+	{ 0x0383, 0x01 },
+	{ 0x0385, 0x01 },
+	{ 0x0387, 0x01 },
+	{ 0x0900, 0x01 },
+	{ 0x0901, 0x14 },
+	{ 0x0401, 0x01 },
+	{ 0x0404, 0x00 },
+	{ 0x0405, 0x40 },
+	{ 0x0408, 0x00 },
+	{ 0x0409, 0x06 },
+	{ 0x040A, 0x00 },
+	{ 0x040B, 0x00 },
+	{ 0x040C, 0x10 },
+	{ 0x040D, 0x64 },
+	{ 0x040E, 0x03 },
+	{ 0x040F, 0x0C },
+	{ 0x3038, 0x00 },
+	{ 0x303A, 0x00 },
+	{ 0x303B, 0x10 },
+	{ 0x300D, 0x00 },
+	{ 0x034C, 0x04 },
+	{ 0x034D, 0x18 },
+	{ 0x034E, 0x03 },
+	{ 0x034F, 0x0C },
+	{ 0x0350, 0x01 },
+	{ 0x0202, 0x03 },
+	{ 0x0203, 0x42 },
+	{ 0x0204, 0x00 },
+	{ 0x0205, 0x00 },
+	{ 0x020E, 0x01 },
+	{ 0x020F, 0x00 },
+	{ 0x0210, 0x01 },
+	{ 0x0211, 0x00 },
+	{ 0x0212, 0x01 },
+	{ 0x0213, 0x00 },
+	{ 0x0214, 0x01 },
+	{ 0x0215, 0x00 },
+	{ 0x7BCD, 0x00 },
+	{ 0x94DC, 0x20 },
+	{ 0x94DD, 0x20 },
+	{ 0x94DE, 0x20 },
+	{ 0x95DC, 0x20 },
+	{ 0x95DD, 0x20 },
+	{ 0x95DE, 0x20 },
+	{ 0x7FB0, 0x00 },
+	{ 0x9010, 0x3E },
+	{ 0x9419, 0x50 },
+	{ 0x941B, 0x50 },
+	{ 0x9519, 0x50 },
+	{ 0x951B, 0x50 },
+	{ 0x3030, 0x00 },
+	{ 0x3032, 0x00 },
+	{ 0x0220, 0x00 },
+};
+
+static const char * const imx258_test_pattern_menu[] = {
+	"Disabled",
+	"Solid Colour",
+	"Eight Vertical Colour Bars",
+	"Colour Bars With Fade to Grey",
+	"Pseudorandom Sequence (PN9)",
+};
+
+/* Configurations for supported link frequencies */
+#define IMX258_LINK_FREQ_634MHZ	633600000ULL
+#define IMX258_LINK_FREQ_320MHZ	320000000ULL
+
+enum {
+	IMX258_LINK_FREQ_1267MBPS,
+	IMX258_LINK_FREQ_640MBPS,
+};
+
+/*
+ * pixel_rate = link_freq * data-rate * nr_of_lanes / bits_per_sample
+ * data rate => double data rate; number of lanes => 4; bits per pixel => 10
+ */
+static u64 link_freq_to_pixel_rate(u64 f)
+{
+	f *= 2 * 4;
+	do_div(f, 10);
+
+	return f;
+}
+
+/* Menu items for LINK_FREQ V4L2 control */
+static const s64 link_freq_menu_items[] = {
+	IMX258_LINK_FREQ_634MHZ,
+	IMX258_LINK_FREQ_320MHZ,
+};
+
+/* Link frequency configs */
+static const struct imx258_link_freq_config link_freq_configs[] = {
+	[IMX258_LINK_FREQ_1267MBPS] = {
+		.pixels_per_line = IMX258_PPL_DEFAULT,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mipi_data_rate_1267mbps),
+			.regs = mipi_data_rate_1267mbps,
+		}
+	},
+	[IMX258_LINK_FREQ_640MBPS] = {
+		.pixels_per_line = IMX258_PPL_DEFAULT,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mipi_data_rate_640mbps),
+			.regs = mipi_data_rate_640mbps,
+		}
+	},
+};
+
+/* Mode configs */
+static const struct imx258_mode supported_modes[] = {
+	{
+		.width = 4208,
+		.height = 3118,
+		.vts_def = IMX258_VTS_30FPS,
+		.vts_min = IMX258_VTS_30FPS,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_4208x3118_regs),
+			.regs = mode_4208x3118_regs,
+		},
+		.link_freq_index = IMX258_LINK_FREQ_1267MBPS,
+	},
+	{
+		.width = 2104,
+		.height = 1560,
+		.vts_def = IMX258_VTS_30FPS_2K,
+		.vts_min = IMX258_VTS_30FPS_2K,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_2104_1560_regs),
+			.regs = mode_2104_1560_regs,
+		},
+		.link_freq_index = IMX258_LINK_FREQ_640MBPS,
+	},
+	{
+		.width = 1048,
+		.height = 780,
+		.vts_def = IMX258_VTS_30FPS_VGA,
+		.vts_min = IMX258_VTS_30FPS_VGA,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1048_780_regs),
+			.regs = mode_1048_780_regs,
+		},
+		.link_freq_index = IMX258_LINK_FREQ_640MBPS,
+	},
+};
+
+struct imx258 {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+
+	struct v4l2_ctrl_handler ctrl_handler;
+	/* V4L2 Controls */
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *pixel_rate;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *exposure;
+
+	/* Current mode */
+	const struct imx258_mode *cur_mode;
+
+	/*
+	 * Mutex for serialized access:
+	 * Protect sensor module set pad format and start/stop streaming safely.
+	 */
+	struct mutex mutex;
+
+	/* Streaming on/off */
+	bool streaming;
+
+	struct clk *clk;
+};
+
+static inline struct imx258 *to_imx258(struct v4l2_subdev *_sd)
+{
+	return container_of(_sd, struct imx258, sd);
+}
+
+/* Read registers up to 2 at a time */
+static int imx258_read_reg(struct imx258 *imx258, u16 reg, u32 len, u32 *val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
+	struct i2c_msg msgs[2];
+	u8 addr_buf[2] = { reg >> 8, reg & 0xff };
+	u8 data_buf[4] = { 0, };
+	int ret;
+
+	if (len > 4)
+		return -EINVAL;
+
+	/* Write register address */
+	msgs[0].addr = client->addr;
+	msgs[0].flags = 0;
+	msgs[0].len = ARRAY_SIZE(addr_buf);
+	msgs[0].buf = addr_buf;
+
+	/* Read data from register */
+	msgs[1].addr = client->addr;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].len = len;
+	msgs[1].buf = &data_buf[4 - len];
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret != ARRAY_SIZE(msgs))
+		return -EIO;
+
+	*val = get_unaligned_be32(data_buf);
+
+	return 0;
+}
+
+/* Write registers up to 2 at a time */
+static int imx258_write_reg(struct imx258 *imx258, u16 reg, u32 len, u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
+	u8 buf[6];
+
+	if (len > 4)
+		return -EINVAL;
+
+	put_unaligned_be16(reg, buf);
+	put_unaligned_be32(val << (8 * (4 - len)), buf + 2);
+	if (i2c_master_send(client, buf, len + 2) != len + 2)
+		return -EIO;
+
+	return 0;
+}
+
+/* Write a list of registers */
+static int imx258_write_regs(struct imx258 *imx258,
+			     const struct imx258_reg *regs, u32 len)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < len; i++) {
+		ret = imx258_write_reg(imx258, regs[i].address, 1,
+					regs[i].val);
+		if (ret) {
+			dev_err_ratelimited(
+				&client->dev,
+				"Failed to write reg 0x%4.4x. error = %d\n",
+				regs[i].address, ret);
+
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+/* Open sub-device */
+static int imx258_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_mbus_framefmt *try_fmt =
+		v4l2_subdev_get_try_format(sd, fh->state, 0);
+
+	/* Initialize try_fmt */
+	try_fmt->width = supported_modes[0].width;
+	try_fmt->height = supported_modes[0].height;
+	try_fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	try_fmt->field = V4L2_FIELD_NONE;
+
+	return 0;
+}
+
+static int imx258_update_digital_gain(struct imx258 *imx258, u32 len, u32 val)
+{
+	int ret;
+
+	ret = imx258_write_reg(imx258, IMX258_REG_GR_DIGITAL_GAIN,
+				IMX258_REG_VALUE_16BIT,
+				val);
+	if (ret)
+		return ret;
+	ret = imx258_write_reg(imx258, IMX258_REG_GB_DIGITAL_GAIN,
+				IMX258_REG_VALUE_16BIT,
+				val);
+	if (ret)
+		return ret;
+	ret = imx258_write_reg(imx258, IMX258_REG_R_DIGITAL_GAIN,
+				IMX258_REG_VALUE_16BIT,
+				val);
+	if (ret)
+		return ret;
+	ret = imx258_write_reg(imx258, IMX258_REG_B_DIGITAL_GAIN,
+				IMX258_REG_VALUE_16BIT,
+				val);
+	if (ret)
+		return ret;
+	return 0;
+}
+
+static int imx258_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct imx258 *imx258 =
+		container_of(ctrl->handler, struct imx258, ctrl_handler);
+	struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
+	int ret = 0;
+
+	/*
+	 * Applying V4L2 control value only happens
+	 * when power is up for streaming
+	 */
+	if (pm_runtime_get_if_in_use(&client->dev) == 0)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_ANALOGUE_GAIN:
+		ret = imx258_write_reg(imx258, IMX258_REG_ANALOG_GAIN,
+				IMX258_REG_VALUE_16BIT,
+				ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE:
+		ret = imx258_write_reg(imx258, IMX258_REG_EXPOSURE,
+				IMX258_REG_VALUE_16BIT,
+				ctrl->val);
+		break;
+	case V4L2_CID_DIGITAL_GAIN:
+		ret = imx258_update_digital_gain(imx258, IMX258_REG_VALUE_16BIT,
+				ctrl->val);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		ret = imx258_write_reg(imx258, IMX258_REG_TEST_PATTERN,
+				IMX258_REG_VALUE_16BIT,
+				ctrl->val);
+		ret = imx258_write_reg(imx258, REG_MIRROR_FLIP_CONTROL,
+				IMX258_REG_VALUE_08BIT,
+				!ctrl->val ? REG_CONFIG_MIRROR_FLIP :
+				REG_CONFIG_FLIP_TEST_PATTERN);
+		break;
+	case V4L2_CID_WIDE_DYNAMIC_RANGE:
+		if (!ctrl->val) {
+			ret = imx258_write_reg(imx258, IMX258_REG_HDR,
+					       IMX258_REG_VALUE_08BIT,
+					       IMX258_HDR_RATIO_MIN);
+		} else {
+			ret = imx258_write_reg(imx258, IMX258_REG_HDR,
+					       IMX258_REG_VALUE_08BIT,
+					       IMX258_HDR_ON);
+			if (ret)
+				break;
+			ret = imx258_write_reg(imx258, IMX258_REG_HDR_RATIO,
+					       IMX258_REG_VALUE_08BIT,
+					       BIT(IMX258_HDR_RATIO_MAX));
+		}
+		break;
+	default:
+		dev_info(&client->dev,
+			 "ctrl(id:0x%x,val:0x%x) is not handled\n",
+			 ctrl->id, ctrl->val);
+		ret = -EINVAL;
+		break;
+	}
+
+	pm_runtime_put(&client->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops imx258_ctrl_ops = {
+	.s_ctrl = imx258_set_ctrl,
+};
+
+static int imx258_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	/* Only one bayer order(GRBG) is supported */
+	if (code->index > 0)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+
+	return 0;
+}
+
+static int imx258_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->index >= ARRAY_SIZE(supported_modes))
+		return -EINVAL;
+
+	if (fse->code != MEDIA_BUS_FMT_SGRBG10_1X10)
+		return -EINVAL;
+
+	fse->min_width = supported_modes[fse->index].width;
+	fse->max_width = fse->min_width;
+	fse->min_height = supported_modes[fse->index].height;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static void imx258_update_pad_format(const struct imx258_mode *mode,
+				     struct v4l2_subdev_format *fmt)
+{
+	fmt->format.width = mode->width;
+	fmt->format.height = mode->height;
+	fmt->format.code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	fmt->format.field = V4L2_FIELD_NONE;
+}
+
+static int __imx258_get_pad_format(struct imx258 *imx258,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_format *fmt)
+{
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
+		fmt->format = *v4l2_subdev_get_try_format(&imx258->sd,
+							  sd_state,
+							  fmt->pad);
+	else
+		imx258_update_pad_format(imx258->cur_mode, fmt);
+
+	return 0;
+}
+
+static int imx258_get_pad_format(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_format *fmt)
+{
+	struct imx258 *imx258 = to_imx258(sd);
+	int ret;
+
+	mutex_lock(&imx258->mutex);
+	ret = __imx258_get_pad_format(imx258, sd_state, fmt);
+	mutex_unlock(&imx258->mutex);
+
+	return ret;
+}
+
+static int imx258_set_pad_format(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_format *fmt)
+{
+	struct imx258 *imx258 = to_imx258(sd);
+	const struct imx258_mode *mode;
+	struct v4l2_mbus_framefmt *framefmt;
+	s32 vblank_def;
+	s32 vblank_min;
+	s64 h_blank;
+	s64 pixel_rate;
+	s64 link_freq;
+
+	mutex_lock(&imx258->mutex);
+
+	/* Only one raw bayer(GBRG) order is supported */
+	fmt->format.code = MEDIA_BUS_FMT_SGRBG10_1X10;
+
+	mode = v4l2_find_nearest_size(supported_modes,
+		ARRAY_SIZE(supported_modes), width, height,
+		fmt->format.width, fmt->format.height);
+	imx258_update_pad_format(mode, fmt);
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		framefmt = v4l2_subdev_get_try_format(sd, sd_state, fmt->pad);
+		*framefmt = fmt->format;
+	} else {
+		imx258->cur_mode = mode;
+		__v4l2_ctrl_s_ctrl(imx258->link_freq, mode->link_freq_index);
+
+		link_freq = link_freq_menu_items[mode->link_freq_index];
+		pixel_rate = link_freq_to_pixel_rate(link_freq);
+		__v4l2_ctrl_s_ctrl_int64(imx258->pixel_rate, pixel_rate);
+		/* Update limits and set FPS to default */
+		vblank_def = imx258->cur_mode->vts_def -
+			     imx258->cur_mode->height;
+		vblank_min = imx258->cur_mode->vts_min -
+			     imx258->cur_mode->height;
+		__v4l2_ctrl_modify_range(
+			imx258->vblank, vblank_min,
+			IMX258_VTS_MAX - imx258->cur_mode->height, 1,
+			vblank_def);
+		__v4l2_ctrl_s_ctrl(imx258->vblank, vblank_def);
+		h_blank =
+			link_freq_configs[mode->link_freq_index].pixels_per_line
+			 - imx258->cur_mode->width;
+		__v4l2_ctrl_modify_range(imx258->hblank, h_blank,
+					 h_blank, 1, h_blank);
+	}
+
+	mutex_unlock(&imx258->mutex);
+
+	return 0;
+}
+
+/* Start streaming */
+static int imx258_start_streaming(struct imx258 *imx258)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
+	const struct imx258_reg_list *reg_list;
+	int ret, link_freq_index;
+
+	/* Setup PLL */
+	link_freq_index = imx258->cur_mode->link_freq_index;
+	reg_list = &link_freq_configs[link_freq_index].reg_list;
+	ret = imx258_write_regs(imx258, reg_list->regs, reg_list->num_of_regs);
+	if (ret) {
+		dev_err(&client->dev, "%s failed to set plls\n", __func__);
+		return ret;
+	}
+
+	/* Apply default values of current mode */
+	reg_list = &imx258->cur_mode->reg_list;
+	ret = imx258_write_regs(imx258, reg_list->regs, reg_list->num_of_regs);
+	if (ret) {
+		dev_err(&client->dev, "%s failed to set mode\n", __func__);
+		return ret;
+	}
+
+	/* Set Orientation be 180 degree */
+	ret = imx258_write_reg(imx258, REG_MIRROR_FLIP_CONTROL,
+			       IMX258_REG_VALUE_08BIT, REG_CONFIG_MIRROR_FLIP);
+	if (ret) {
+		dev_err(&client->dev, "%s failed to set orientation\n",
+			__func__);
+		return ret;
+	}
+
+	/* Apply customized values from user */
+	ret =  __v4l2_ctrl_handler_setup(imx258->sd.ctrl_handler);
+	if (ret)
+		return ret;
+
+	/* set stream on register */
+	return imx258_write_reg(imx258, IMX258_REG_MODE_SELECT,
+				IMX258_REG_VALUE_08BIT,
+				IMX258_MODE_STREAMING);
+}
+
+/* Stop streaming */
+static int imx258_stop_streaming(struct imx258 *imx258)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
+	int ret;
+
+	/* set stream off register */
+	ret = imx258_write_reg(imx258, IMX258_REG_MODE_SELECT,
+		IMX258_REG_VALUE_08BIT, IMX258_MODE_STANDBY);
+	if (ret)
+		dev_err(&client->dev, "%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 imx258_power_on(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct imx258 *imx258 = to_imx258(sd);
+	int ret;
+
+	ret = clk_prepare_enable(imx258->clk);
+	if (ret)
+		dev_err(dev, "failed to enable clock\n");
+
+	return ret;
+}
+
+static int imx258_power_off(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct imx258 *imx258 = to_imx258(sd);
+
+	clk_disable_unprepare(imx258->clk);
+
+	return 0;
+}
+
+static int imx258_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct imx258 *imx258 = to_imx258(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	mutex_lock(&imx258->mutex);
+	if (imx258->streaming == enable) {
+		mutex_unlock(&imx258->mutex);
+		return 0;
+	}
+
+	if (enable) {
+		ret = pm_runtime_resume_and_get(&client->dev);
+		if (ret < 0)
+			goto err_unlock;
+
+		/*
+		 * Apply default & customized values
+		 * and then start streaming.
+		 */
+		ret = imx258_start_streaming(imx258);
+		if (ret)
+			goto err_rpm_put;
+	} else {
+		imx258_stop_streaming(imx258);
+		pm_runtime_put(&client->dev);
+	}
+
+	imx258->streaming = enable;
+	mutex_unlock(&imx258->mutex);
+
+	return ret;
+
+err_rpm_put:
+	pm_runtime_put(&client->dev);
+err_unlock:
+	mutex_unlock(&imx258->mutex);
+
+	return ret;
+}
+
+static int __maybe_unused imx258_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct imx258 *imx258 = to_imx258(sd);
+
+	if (imx258->streaming)
+		imx258_stop_streaming(imx258);
+
+	return 0;
+}
+
+static int __maybe_unused imx258_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct imx258 *imx258 = to_imx258(sd);
+	int ret;
+
+	if (imx258->streaming) {
+		ret = imx258_start_streaming(imx258);
+		if (ret)
+			goto error;
+	}
+
+	return 0;
+
+error:
+	imx258_stop_streaming(imx258);
+	imx258->streaming = 0;
+	return ret;
+}
+
+/* Verify chip ID */
+static int imx258_identify_module(struct imx258 *imx258)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
+	int ret;
+	u32 val;
+
+	ret = imx258_read_reg(imx258, IMX258_REG_CHIP_ID,
+			      IMX258_REG_VALUE_16BIT, &val);
+	if (ret) {
+		dev_err(&client->dev, "failed to read chip id %x\n",
+			IMX258_CHIP_ID);
+		return ret;
+	}
+
+	if (val != IMX258_CHIP_ID) {
+		dev_err(&client->dev, "chip id mismatch: %x!=%x\n",
+			IMX258_CHIP_ID, val);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops imx258_video_ops = {
+	.s_stream = imx258_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops imx258_pad_ops = {
+	.enum_mbus_code = imx258_enum_mbus_code,
+	.get_fmt = imx258_get_pad_format,
+	.set_fmt = imx258_set_pad_format,
+	.enum_frame_size = imx258_enum_frame_size,
+};
+
+static const struct v4l2_subdev_ops imx258_subdev_ops = {
+	.video = &imx258_video_ops,
+	.pad = &imx258_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops imx258_internal_ops = {
+	.open = imx258_open,
+};
+
+/* Initialize control handlers */
+static int imx258_init_controls(struct imx258 *imx258)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
+	struct v4l2_fwnode_device_properties props;
+	struct v4l2_ctrl_handler *ctrl_hdlr;
+	struct v4l2_ctrl *vflip, *hflip;
+	s64 vblank_def;
+	s64 vblank_min;
+	s64 pixel_rate_min;
+	s64 pixel_rate_max;
+	int ret;
+
+	ctrl_hdlr = &imx258->ctrl_handler;
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 13);
+	if (ret)
+		return ret;
+
+	mutex_init(&imx258->mutex);
+	ctrl_hdlr->lock = &imx258->mutex;
+	imx258->link_freq = v4l2_ctrl_new_int_menu(ctrl_hdlr,
+				&imx258_ctrl_ops,
+				V4L2_CID_LINK_FREQ,
+				ARRAY_SIZE(link_freq_menu_items) - 1,
+				0,
+				link_freq_menu_items);
+
+	if (imx258->link_freq)
+		imx258->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	/* The driver only supports one bayer order and flips by default. */
+	hflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx258_ctrl_ops,
+				  V4L2_CID_HFLIP, 1, 1, 1, 1);
+	if (hflip)
+		hflip->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	vflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx258_ctrl_ops,
+				  V4L2_CID_VFLIP, 1, 1, 1, 1);
+	if (vflip)
+		vflip->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	pixel_rate_max = link_freq_to_pixel_rate(link_freq_menu_items[0]);
+	pixel_rate_min = link_freq_to_pixel_rate(link_freq_menu_items[1]);
+	/* By default, PIXEL_RATE is read only */
+	imx258->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, &imx258_ctrl_ops,
+				V4L2_CID_PIXEL_RATE,
+				pixel_rate_min, pixel_rate_max,
+				1, pixel_rate_max);
+
+
+	vblank_def = imx258->cur_mode->vts_def - imx258->cur_mode->height;
+	vblank_min = imx258->cur_mode->vts_min - imx258->cur_mode->height;
+	imx258->vblank = v4l2_ctrl_new_std(
+				ctrl_hdlr, &imx258_ctrl_ops, V4L2_CID_VBLANK,
+				vblank_min,
+				IMX258_VTS_MAX - imx258->cur_mode->height, 1,
+				vblank_def);
+
+	if (imx258->vblank)
+		imx258->vblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	imx258->hblank = v4l2_ctrl_new_std(
+				ctrl_hdlr, &imx258_ctrl_ops, V4L2_CID_HBLANK,
+				IMX258_PPL_DEFAULT - imx258->cur_mode->width,
+				IMX258_PPL_DEFAULT - imx258->cur_mode->width,
+				1,
+				IMX258_PPL_DEFAULT - imx258->cur_mode->width);
+
+	if (imx258->hblank)
+		imx258->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	imx258->exposure = v4l2_ctrl_new_std(
+				ctrl_hdlr, &imx258_ctrl_ops,
+				V4L2_CID_EXPOSURE, IMX258_EXPOSURE_MIN,
+				IMX258_EXPOSURE_MAX, IMX258_EXPOSURE_STEP,
+				IMX258_EXPOSURE_DEFAULT);
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &imx258_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
+				IMX258_ANA_GAIN_MIN, IMX258_ANA_GAIN_MAX,
+				IMX258_ANA_GAIN_STEP, IMX258_ANA_GAIN_DEFAULT);
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &imx258_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
+				IMX258_DGTL_GAIN_MIN, IMX258_DGTL_GAIN_MAX,
+				IMX258_DGTL_GAIN_STEP,
+				IMX258_DGTL_GAIN_DEFAULT);
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &imx258_ctrl_ops, V4L2_CID_WIDE_DYNAMIC_RANGE,
+				0, 1, 1, IMX258_HDR_RATIO_DEFAULT);
+
+	v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &imx258_ctrl_ops,
+				V4L2_CID_TEST_PATTERN,
+				ARRAY_SIZE(imx258_test_pattern_menu) - 1,
+				0, 0, imx258_test_pattern_menu);
+
+	if (ctrl_hdlr->error) {
+		ret = ctrl_hdlr->error;
+		dev_err(&client->dev, "%s control init failed (%d)\n",
+				__func__, ret);
+		goto error;
+	}
+
+	ret = v4l2_fwnode_device_parse(&client->dev, &props);
+	if (ret)
+		goto error;
+
+	ret = v4l2_ctrl_new_fwnode_properties(ctrl_hdlr, &imx258_ctrl_ops,
+					      &props);
+	if (ret)
+		goto error;
+
+	imx258->sd.ctrl_handler = ctrl_hdlr;
+
+	return 0;
+
+error:
+	v4l2_ctrl_handler_free(ctrl_hdlr);
+	mutex_destroy(&imx258->mutex);
+
+	return ret;
+}
+
+static void imx258_free_controls(struct imx258 *imx258)
+{
+	v4l2_ctrl_handler_free(imx258->sd.ctrl_handler);
+	mutex_destroy(&imx258->mutex);
+}
+
+static int imx258_probe(struct i2c_client *client)
+{
+	struct imx258 *imx258;
+	int ret;
+	u32 val = 0;
+
+	imx258 = devm_kzalloc(&client->dev, sizeof(*imx258), GFP_KERNEL);
+	if (!imx258)
+		return -ENOMEM;
+
+	imx258->clk = devm_clk_get_optional(&client->dev, NULL);
+	if (IS_ERR(imx258->clk))
+		return dev_err_probe(&client->dev, PTR_ERR(imx258->clk),
+				     "error getting clock\n");
+	if (!imx258->clk) {
+		dev_dbg(&client->dev,
+			"no clock provided, using clock-frequency property\n");
+
+		device_property_read_u32(&client->dev, "clock-frequency", &val);
+	} else {
+		val = clk_get_rate(imx258->clk);
+	}
+	if (val != IMX258_INPUT_CLOCK_FREQ) {
+		dev_err(&client->dev, "input clock frequency not supported\n");
+		return -EINVAL;
+	}
+
+	/* Initialize subdev */
+	v4l2_i2c_subdev_init(&imx258->sd, client, &imx258_subdev_ops);
+
+	/* Will be powered off via pm_runtime_idle */
+	ret = imx258_power_on(&client->dev);
+	if (ret)
+		return ret;
+
+	/* Check module identity */
+	ret = imx258_identify_module(imx258);
+	if (ret)
+		goto error_identify;
+
+	/* Set default mode to max resolution */
+	imx258->cur_mode = &supported_modes[0];
+
+	ret = imx258_init_controls(imx258);
+	if (ret)
+		goto error_identify;
+
+	/* Initialize subdev */
+	imx258->sd.internal_ops = &imx258_internal_ops;
+	imx258->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	imx258->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+	/* Initialize source pad */
+	imx258->pad.flags = MEDIA_PAD_FL_SOURCE;
+
+	ret = media_entity_pads_init(&imx258->sd.entity, 1, &imx258->pad);
+	if (ret)
+		goto error_handler_free;
+
+	ret = v4l2_async_register_subdev_sensor(&imx258->sd);
+	if (ret < 0)
+		goto error_media_entity;
+
+	pm_runtime_set_active(&client->dev);
+	pm_runtime_enable(&client->dev);
+	pm_runtime_idle(&client->dev);
+
+	return 0;
+
+error_media_entity:
+	media_entity_cleanup(&imx258->sd.entity);
+
+error_handler_free:
+	imx258_free_controls(imx258);
+
+error_identify:
+	imx258_power_off(&client->dev);
+
+	return ret;
+}
+
+static void imx258_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct imx258 *imx258 = to_imx258(sd);
+
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	imx258_free_controls(imx258);
+
+	pm_runtime_disable(&client->dev);
+	if (!pm_runtime_status_suspended(&client->dev))
+		imx258_power_off(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+}
+
+static const struct dev_pm_ops imx258_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(imx258_suspend, imx258_resume)
+	SET_RUNTIME_PM_OPS(imx258_power_off, imx258_power_on, NULL)
+};
+
+#ifdef CONFIG_ACPI
+static const struct acpi_device_id imx258_acpi_ids[] = {
+	{ "SONY258A" },
+	{ /* sentinel */ }
+};
+
+MODULE_DEVICE_TABLE(acpi, imx258_acpi_ids);
+#endif
+
+static const struct of_device_id imx258_dt_ids[] = {
+	{ .compatible = "sony,imx258" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, imx258_dt_ids);
+
+static struct i2c_driver imx258_i2c_driver = {
+	.driver = {
+		.name = "imx258",
+		.pm = &imx258_pm_ops,
+		.acpi_match_table = ACPI_PTR(imx258_acpi_ids),
+		.of_match_table	= imx258_dt_ids,
+	},
+	.probe = imx258_probe,
+	.remove = imx258_remove,
+};
+
+module_i2c_driver(imx258_i2c_driver);
+
+MODULE_AUTHOR("Yeh, Andy <andy.yeh@intel.com>");
+MODULE_AUTHOR("Chiang, Alan");
+MODULE_AUTHOR("Chen, Jason");
+MODULE_DESCRIPTION("Sony IMX258 sensor driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/imx274.c b/drivers/media/i2c/imx274.c
new file mode 100644
index 000000000..f33b692e6
--- /dev/null
+++ b/drivers/media/i2c/imx274.c
@@ -0,0 +1,2180 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * imx274.c - IMX274 CMOS Image Sensor driver
+ *
+ * Copyright (C) 2017, Leopard Imaging, Inc.
+ *
+ * Leon Luo <leonl@leopardimaging.com>
+ * Edwin Zou <edwinz@leopardimaging.com>
+ * Luca Ceresoli <luca@lucaceresoli.net>
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/v4l2-mediabus.h>
+#include <linux/videodev2.h>
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+/*
+ * See "SHR, SVR Setting" in datasheet
+ */
+#define IMX274_DEFAULT_FRAME_LENGTH		(4550)
+#define IMX274_MAX_FRAME_LENGTH			(0x000fffff)
+
+/*
+ * See "Frame Rate Adjustment" in datasheet
+ */
+#define IMX274_PIXCLK_CONST1			(72000000)
+#define IMX274_PIXCLK_CONST2			(1000000)
+
+/*
+ * The input gain is shifted by IMX274_GAIN_SHIFT to get
+ * decimal number. The real gain is
+ * (float)input_gain_value / (1 << IMX274_GAIN_SHIFT)
+ */
+#define IMX274_GAIN_SHIFT			(8)
+#define IMX274_GAIN_SHIFT_MASK			((1 << IMX274_GAIN_SHIFT) - 1)
+
+/*
+ * See "Analog Gain" and "Digital Gain" in datasheet
+ * min gain is 1X
+ * max gain is calculated based on IMX274_GAIN_REG_MAX
+ */
+#define IMX274_GAIN_REG_MAX			(1957)
+#define IMX274_MIN_GAIN				(0x01 << IMX274_GAIN_SHIFT)
+#define IMX274_MAX_ANALOG_GAIN			((2048 << IMX274_GAIN_SHIFT)\
+					/ (2048 - IMX274_GAIN_REG_MAX))
+#define IMX274_MAX_DIGITAL_GAIN			(8)
+#define IMX274_DEF_GAIN				(20 << IMX274_GAIN_SHIFT)
+#define IMX274_GAIN_CONST			(2048) /* for gain formula */
+
+/*
+ * 1 line time in us = (HMAX / 72), minimal is 4 lines
+ */
+#define IMX274_MIN_EXPOSURE_TIME		(4 * 260 / 72)
+
+#define IMX274_MAX_WIDTH			(3840)
+#define IMX274_MAX_HEIGHT			(2160)
+#define IMX274_MAX_FRAME_RATE			(120)
+#define IMX274_MIN_FRAME_RATE			(5)
+#define IMX274_DEF_FRAME_RATE			(60)
+
+/*
+ * register SHR is limited to (SVR value + 1) x VMAX value - 4
+ */
+#define IMX274_SHR_LIMIT_CONST			(4)
+
+/*
+ * Min and max sensor reset delay (microseconds)
+ */
+#define IMX274_RESET_DELAY1			(2000)
+#define IMX274_RESET_DELAY2			(2200)
+
+/*
+ * shift and mask constants
+ */
+#define IMX274_SHIFT_8_BITS			(8)
+#define IMX274_SHIFT_16_BITS			(16)
+#define IMX274_MASK_LSB_2_BITS			(0x03)
+#define IMX274_MASK_LSB_3_BITS			(0x07)
+#define IMX274_MASK_LSB_4_BITS			(0x0f)
+#define IMX274_MASK_LSB_8_BITS			(0x00ff)
+
+#define DRIVER_NAME "IMX274"
+
+/*
+ * IMX274 register definitions
+ */
+#define IMX274_SHR_REG_MSB			0x300D /* SHR */
+#define IMX274_SHR_REG_LSB			0x300C /* SHR */
+#define IMX274_SVR_REG_MSB			0x300F /* SVR */
+#define IMX274_SVR_REG_LSB			0x300E /* SVR */
+#define IMX274_HTRIM_EN_REG			0x3037
+#define IMX274_HTRIM_START_REG_LSB		0x3038
+#define IMX274_HTRIM_START_REG_MSB		0x3039
+#define IMX274_HTRIM_END_REG_LSB		0x303A
+#define IMX274_HTRIM_END_REG_MSB		0x303B
+#define IMX274_VWIDCUTEN_REG			0x30DD
+#define IMX274_VWIDCUT_REG_LSB			0x30DE
+#define IMX274_VWIDCUT_REG_MSB			0x30DF
+#define IMX274_VWINPOS_REG_LSB			0x30E0
+#define IMX274_VWINPOS_REG_MSB			0x30E1
+#define IMX274_WRITE_VSIZE_REG_LSB		0x3130
+#define IMX274_WRITE_VSIZE_REG_MSB		0x3131
+#define IMX274_Y_OUT_SIZE_REG_LSB		0x3132
+#define IMX274_Y_OUT_SIZE_REG_MSB		0x3133
+#define IMX274_VMAX_REG_1			0x30FA /* VMAX, MSB */
+#define IMX274_VMAX_REG_2			0x30F9 /* VMAX */
+#define IMX274_VMAX_REG_3			0x30F8 /* VMAX, LSB */
+#define IMX274_HMAX_REG_MSB			0x30F7 /* HMAX */
+#define IMX274_HMAX_REG_LSB			0x30F6 /* HMAX */
+#define IMX274_ANALOG_GAIN_ADDR_LSB		0x300A /* ANALOG GAIN LSB */
+#define IMX274_ANALOG_GAIN_ADDR_MSB		0x300B /* ANALOG GAIN MSB */
+#define IMX274_DIGITAL_GAIN_REG			0x3012 /* Digital Gain */
+#define IMX274_VFLIP_REG			0x301A /* VERTICAL FLIP */
+#define IMX274_TEST_PATTERN_REG			0x303D /* TEST PATTERN */
+#define IMX274_STANDBY_REG			0x3000 /* STANDBY */
+
+#define IMX274_TABLE_WAIT_MS			0
+#define IMX274_TABLE_END			1
+
+/* regulator supplies */
+static const char * const imx274_supply_names[] = {
+	"vddl",  /* IF (1.2V) supply */
+	"vdig",  /* Digital Core (1.8V) supply */
+	"vana",  /* Analog (2.8V) supply */
+};
+
+#define IMX274_NUM_SUPPLIES ARRAY_SIZE(imx274_supply_names)
+
+/*
+ * imx274 I2C operation related structure
+ */
+struct reg_8 {
+	u16 addr;
+	u8 val;
+};
+
+static const struct regmap_config imx274_regmap_config = {
+	.reg_bits = 16,
+	.val_bits = 8,
+	.cache_type = REGCACHE_RBTREE,
+};
+
+/*
+ * Parameters for each imx274 readout mode.
+ *
+ * These are the values to configure the sensor in one of the
+ * implemented modes.
+ *
+ * @init_regs: registers to initialize the mode
+ * @wbin_ratio: width downscale factor (e.g. 3 for 1280; 3 = 3840/1280)
+ * @hbin_ratio: height downscale factor (e.g. 3 for 720; 3 = 2160/720)
+ * @min_frame_len: Minimum frame length for each mode (see "Frame Rate
+ *                 Adjustment (CSI-2)" in the datasheet)
+ * @min_SHR: Minimum SHR register value (see "Shutter Setting (CSI-2)" in the
+ *           datasheet)
+ * @max_fps: Maximum frames per second
+ * @nocpiop: Number of clocks per internal offset period (see "Integration Time
+ *           in Each Readout Drive Mode (CSI-2)" in the datasheet)
+ */
+struct imx274_mode {
+	const struct reg_8 *init_regs;
+	u8 wbin_ratio;
+	u8 hbin_ratio;
+	int min_frame_len;
+	int min_SHR;
+	int max_fps;
+	int nocpiop;
+};
+
+/*
+ * imx274 test pattern related structure
+ */
+enum {
+	TEST_PATTERN_DISABLED = 0,
+	TEST_PATTERN_ALL_000H,
+	TEST_PATTERN_ALL_FFFH,
+	TEST_PATTERN_ALL_555H,
+	TEST_PATTERN_ALL_AAAH,
+	TEST_PATTERN_VSP_5AH, /* VERTICAL STRIPE PATTERN 555H/AAAH */
+	TEST_PATTERN_VSP_A5H, /* VERTICAL STRIPE PATTERN AAAH/555H */
+	TEST_PATTERN_VSP_05H, /* VERTICAL STRIPE PATTERN 000H/555H */
+	TEST_PATTERN_VSP_50H, /* VERTICAL STRIPE PATTERN 555H/000H */
+	TEST_PATTERN_VSP_0FH, /* VERTICAL STRIPE PATTERN 000H/FFFH */
+	TEST_PATTERN_VSP_F0H, /* VERTICAL STRIPE PATTERN FFFH/000H */
+	TEST_PATTERN_H_COLOR_BARS,
+	TEST_PATTERN_V_COLOR_BARS,
+};
+
+static const char * const tp_qmenu[] = {
+	"Disabled",
+	"All 000h Pattern",
+	"All FFFh Pattern",
+	"All 555h Pattern",
+	"All AAAh Pattern",
+	"Vertical Stripe (555h / AAAh)",
+	"Vertical Stripe (AAAh / 555h)",
+	"Vertical Stripe (000h / 555h)",
+	"Vertical Stripe (555h / 000h)",
+	"Vertical Stripe (000h / FFFh)",
+	"Vertical Stripe (FFFh / 000h)",
+	"Vertical Color Bars",
+	"Horizontal Color Bars",
+};
+
+/*
+ * All-pixel scan mode (10-bit)
+ * imx274 mode1(refer to datasheet) register configuration with
+ * 3840x2160 resolution, raw10 data and mipi four lane output
+ */
+static const struct reg_8 imx274_mode1_3840x2160_raw10[] = {
+	{0x3004, 0x01},
+	{0x3005, 0x01},
+	{0x3006, 0x00},
+	{0x3007, 0xa2},
+
+	{0x3018, 0xA2}, /* output XVS, HVS */
+
+	{0x306B, 0x05},
+	{0x30E2, 0x01},
+
+	{0x30EE, 0x01},
+	{0x3342, 0x0A},
+	{0x3343, 0x00},
+	{0x3344, 0x16},
+	{0x3345, 0x00},
+	{0x33A6, 0x01},
+	{0x3528, 0x0E},
+	{0x3554, 0x1F},
+	{0x3555, 0x01},
+	{0x3556, 0x01},
+	{0x3557, 0x01},
+	{0x3558, 0x01},
+	{0x3559, 0x00},
+	{0x355A, 0x00},
+	{0x35BA, 0x0E},
+	{0x366A, 0x1B},
+	{0x366B, 0x1A},
+	{0x366C, 0x19},
+	{0x366D, 0x17},
+	{0x3A41, 0x08},
+
+	{IMX274_TABLE_END, 0x00}
+};
+
+/*
+ * Horizontal/vertical 2/2-line binning
+ * (Horizontal and vertical weightedbinning, 10-bit)
+ * imx274 mode3(refer to datasheet) register configuration with
+ * 1920x1080 resolution, raw10 data and mipi four lane output
+ */
+static const struct reg_8 imx274_mode3_1920x1080_raw10[] = {
+	{0x3004, 0x02},
+	{0x3005, 0x21},
+	{0x3006, 0x00},
+	{0x3007, 0xb1},
+
+	{0x3018, 0xA2}, /* output XVS, HVS */
+
+	{0x306B, 0x05},
+	{0x30E2, 0x02},
+
+	{0x30EE, 0x01},
+	{0x3342, 0x0A},
+	{0x3343, 0x00},
+	{0x3344, 0x1A},
+	{0x3345, 0x00},
+	{0x33A6, 0x01},
+	{0x3528, 0x0E},
+	{0x3554, 0x00},
+	{0x3555, 0x01},
+	{0x3556, 0x01},
+	{0x3557, 0x01},
+	{0x3558, 0x01},
+	{0x3559, 0x00},
+	{0x355A, 0x00},
+	{0x35BA, 0x0E},
+	{0x366A, 0x1B},
+	{0x366B, 0x1A},
+	{0x366C, 0x19},
+	{0x366D, 0x17},
+	{0x3A41, 0x08},
+
+	{IMX274_TABLE_END, 0x00}
+};
+
+/*
+ * Vertical 2/3 subsampling binning horizontal 3 binning
+ * imx274 mode5(refer to datasheet) register configuration with
+ * 1280x720 resolution, raw10 data and mipi four lane output
+ */
+static const struct reg_8 imx274_mode5_1280x720_raw10[] = {
+	{0x3004, 0x03},
+	{0x3005, 0x31},
+	{0x3006, 0x00},
+	{0x3007, 0xa9},
+
+	{0x3018, 0xA2}, /* output XVS, HVS */
+
+	{0x306B, 0x05},
+	{0x30E2, 0x03},
+
+	{0x30EE, 0x01},
+	{0x3342, 0x0A},
+	{0x3343, 0x00},
+	{0x3344, 0x1B},
+	{0x3345, 0x00},
+	{0x33A6, 0x01},
+	{0x3528, 0x0E},
+	{0x3554, 0x00},
+	{0x3555, 0x01},
+	{0x3556, 0x01},
+	{0x3557, 0x01},
+	{0x3558, 0x01},
+	{0x3559, 0x00},
+	{0x355A, 0x00},
+	{0x35BA, 0x0E},
+	{0x366A, 0x1B},
+	{0x366B, 0x19},
+	{0x366C, 0x17},
+	{0x366D, 0x17},
+	{0x3A41, 0x04},
+
+	{IMX274_TABLE_END, 0x00}
+};
+
+/*
+ * Vertical 2/8 subsampling horizontal 3 binning
+ * imx274 mode6(refer to datasheet) register configuration with
+ * 1280x540 resolution, raw10 data and mipi four lane output
+ */
+static const struct reg_8 imx274_mode6_1280x540_raw10[] = {
+	{0x3004, 0x04}, /* mode setting */
+	{0x3005, 0x31},
+	{0x3006, 0x00},
+	{0x3007, 0x02}, /* mode setting */
+
+	{0x3018, 0xA2}, /* output XVS, HVS */
+
+	{0x306B, 0x05},
+	{0x30E2, 0x04}, /* mode setting */
+
+	{0x30EE, 0x01},
+	{0x3342, 0x0A},
+	{0x3343, 0x00},
+	{0x3344, 0x16},
+	{0x3345, 0x00},
+	{0x33A6, 0x01},
+	{0x3528, 0x0E},
+	{0x3554, 0x1F},
+	{0x3555, 0x01},
+	{0x3556, 0x01},
+	{0x3557, 0x01},
+	{0x3558, 0x01},
+	{0x3559, 0x00},
+	{0x355A, 0x00},
+	{0x35BA, 0x0E},
+	{0x366A, 0x1B},
+	{0x366B, 0x1A},
+	{0x366C, 0x19},
+	{0x366D, 0x17},
+	{0x3A41, 0x04},
+
+	{IMX274_TABLE_END, 0x00}
+};
+
+/*
+ * imx274 first step register configuration for
+ * starting stream
+ */
+static const struct reg_8 imx274_start_1[] = {
+	{IMX274_STANDBY_REG, 0x12},
+
+	/* PLRD: clock settings */
+	{0x3120, 0xF0},
+	{0x3121, 0x00},
+	{0x3122, 0x02},
+	{0x3129, 0x9C},
+	{0x312A, 0x02},
+	{0x312D, 0x02},
+
+	{0x310B, 0x00},
+
+	/* PLSTMG */
+	{0x304C, 0x00}, /* PLSTMG01 */
+	{0x304D, 0x03},
+	{0x331C, 0x1A},
+	{0x331D, 0x00},
+	{0x3502, 0x02},
+	{0x3529, 0x0E},
+	{0x352A, 0x0E},
+	{0x352B, 0x0E},
+	{0x3538, 0x0E},
+	{0x3539, 0x0E},
+	{0x3553, 0x00},
+	{0x357D, 0x05},
+	{0x357F, 0x05},
+	{0x3581, 0x04},
+	{0x3583, 0x76},
+	{0x3587, 0x01},
+	{0x35BB, 0x0E},
+	{0x35BC, 0x0E},
+	{0x35BD, 0x0E},
+	{0x35BE, 0x0E},
+	{0x35BF, 0x0E},
+	{0x366E, 0x00},
+	{0x366F, 0x00},
+	{0x3670, 0x00},
+	{0x3671, 0x00},
+
+	/* PSMIPI */
+	{0x3304, 0x32}, /* PSMIPI1 */
+	{0x3305, 0x00},
+	{0x3306, 0x32},
+	{0x3307, 0x00},
+	{0x3590, 0x32},
+	{0x3591, 0x00},
+	{0x3686, 0x32},
+	{0x3687, 0x00},
+
+	{IMX274_TABLE_END, 0x00}
+};
+
+/*
+ * imx274 second step register configuration for
+ * starting stream
+ */
+static const struct reg_8 imx274_start_2[] = {
+	{IMX274_STANDBY_REG, 0x00},
+	{0x303E, 0x02}, /* SYS_MODE = 2 */
+	{IMX274_TABLE_END, 0x00}
+};
+
+/*
+ * imx274 third step register configuration for
+ * starting stream
+ */
+static const struct reg_8 imx274_start_3[] = {
+	{0x30F4, 0x00},
+	{0x3018, 0xA2}, /* XHS VHS OUTPUT */
+	{IMX274_TABLE_END, 0x00}
+};
+
+/*
+ * imx274 register configuration for stopping stream
+ */
+static const struct reg_8 imx274_stop[] = {
+	{IMX274_STANDBY_REG, 0x01},
+	{IMX274_TABLE_END, 0x00}
+};
+
+/*
+ * imx274 disable test pattern register configuration
+ */
+static const struct reg_8 imx274_tp_disabled[] = {
+	{0x303C, 0x00},
+	{0x377F, 0x00},
+	{0x3781, 0x00},
+	{0x370B, 0x00},
+	{IMX274_TABLE_END, 0x00}
+};
+
+/*
+ * imx274 test pattern register configuration
+ * reg 0x303D defines the test pattern modes
+ */
+static const struct reg_8 imx274_tp_regs[] = {
+	{0x303C, 0x11},
+	{0x370E, 0x01},
+	{0x377F, 0x01},
+	{0x3781, 0x01},
+	{0x370B, 0x11},
+	{IMX274_TABLE_END, 0x00}
+};
+
+/* nocpiop happens to be the same number for the implemented modes */
+static const struct imx274_mode imx274_modes[] = {
+	{
+		/* mode 1, 4K */
+		.wbin_ratio = 1, /* 3840 */
+		.hbin_ratio = 1, /* 2160 */
+		.init_regs = imx274_mode1_3840x2160_raw10,
+		.min_frame_len = 4550,
+		.min_SHR = 12,
+		.max_fps = 60,
+		.nocpiop = 112,
+	},
+	{
+		/* mode 3, 1080p */
+		.wbin_ratio = 2, /* 1920 */
+		.hbin_ratio = 2, /* 1080 */
+		.init_regs = imx274_mode3_1920x1080_raw10,
+		.min_frame_len = 2310,
+		.min_SHR = 8,
+		.max_fps = 120,
+		.nocpiop = 112,
+	},
+	{
+		/* mode 5, 720p */
+		.wbin_ratio = 3, /* 1280 */
+		.hbin_ratio = 3, /* 720 */
+		.init_regs = imx274_mode5_1280x720_raw10,
+		.min_frame_len = 2310,
+		.min_SHR = 8,
+		.max_fps = 120,
+		.nocpiop = 112,
+	},
+	{
+		/* mode 6, 540p */
+		.wbin_ratio = 3, /* 1280 */
+		.hbin_ratio = 4, /* 540 */
+		.init_regs = imx274_mode6_1280x540_raw10,
+		.min_frame_len = 2310,
+		.min_SHR = 4,
+		.max_fps = 120,
+		.nocpiop = 112,
+	},
+};
+
+/*
+ * struct imx274_ctrls - imx274 ctrl structure
+ * @handler: V4L2 ctrl handler structure
+ * @exposure: Pointer to expsure ctrl structure
+ * @gain: Pointer to gain ctrl structure
+ * @vflip: Pointer to vflip ctrl structure
+ * @test_pattern: Pointer to test pattern ctrl structure
+ */
+struct imx274_ctrls {
+	struct v4l2_ctrl_handler handler;
+	struct v4l2_ctrl *exposure;
+	struct v4l2_ctrl *gain;
+	struct v4l2_ctrl *vflip;
+	struct v4l2_ctrl *test_pattern;
+};
+
+/*
+ * struct stim274 - imx274 device structure
+ * @sd: V4L2 subdevice structure
+ * @pad: Media pad structure
+ * @client: Pointer to I2C client
+ * @ctrls: imx274 control structure
+ * @crop: rect to be captured
+ * @compose: compose rect, i.e. output resolution
+ * @format: V4L2 media bus frame format structure
+ *          (width and height are in sync with the compose rect)
+ * @frame_rate: V4L2 frame rate structure
+ * @regmap: Pointer to regmap structure
+ * @reset_gpio: Pointer to reset gpio
+ * @supplies: List of analog and digital supply regulators
+ * @inck: Pointer to sensor input clock
+ * @lock: Mutex structure
+ * @mode: Parameters for the selected readout mode
+ */
+struct stimx274 {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct i2c_client *client;
+	struct imx274_ctrls ctrls;
+	struct v4l2_rect crop;
+	struct v4l2_mbus_framefmt format;
+	struct v4l2_fract frame_interval;
+	struct regmap *regmap;
+	struct gpio_desc *reset_gpio;
+	struct regulator_bulk_data supplies[IMX274_NUM_SUPPLIES];
+	struct clk *inck;
+	struct mutex lock; /* mutex lock for operations */
+	const struct imx274_mode *mode;
+};
+
+#define IMX274_ROUND(dim, step, flags)			\
+	((flags) & V4L2_SEL_FLAG_GE			\
+	 ? roundup((dim), (step))			\
+	 : ((flags) & V4L2_SEL_FLAG_LE			\
+	    ? rounddown((dim), (step))			\
+	    : rounddown((dim) + (step) / 2, (step))))
+
+/*
+ * Function declaration
+ */
+static int imx274_set_gain(struct stimx274 *priv, struct v4l2_ctrl *ctrl);
+static int imx274_set_exposure(struct stimx274 *priv, int val);
+static int imx274_set_vflip(struct stimx274 *priv, int val);
+static int imx274_set_test_pattern(struct stimx274 *priv, int val);
+static int imx274_set_frame_interval(struct stimx274 *priv,
+				     struct v4l2_fract frame_interval);
+
+static inline void msleep_range(unsigned int delay_base)
+{
+	usleep_range(delay_base * 1000, delay_base * 1000 + 500);
+}
+
+/*
+ * v4l2_ctrl and v4l2_subdev related operations
+ */
+static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler,
+			     struct stimx274, ctrls.handler)->sd;
+}
+
+static inline struct stimx274 *to_imx274(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct stimx274, sd);
+}
+
+/*
+ * Writing a register table
+ *
+ * @priv: Pointer to device
+ * @table: Table containing register values (with optional delays)
+ *
+ * This is used to write register table into sensor's reg map.
+ *
+ * Return: 0 on success, errors otherwise
+ */
+static int imx274_write_table(struct stimx274 *priv, const struct reg_8 table[])
+{
+	struct regmap *regmap = priv->regmap;
+	int err = 0;
+	const struct reg_8 *next;
+	u8 val;
+
+	int range_start = -1;
+	int range_count = 0;
+	u8 range_vals[16];
+	int max_range_vals = ARRAY_SIZE(range_vals);
+
+	for (next = table;; next++) {
+		if ((next->addr != range_start + range_count) ||
+		    (next->addr == IMX274_TABLE_END) ||
+		    (next->addr == IMX274_TABLE_WAIT_MS) ||
+		    (range_count == max_range_vals)) {
+			if (range_count == 1)
+				err = regmap_write(regmap,
+						   range_start, range_vals[0]);
+			else if (range_count > 1)
+				err = regmap_bulk_write(regmap, range_start,
+							&range_vals[0],
+							range_count);
+			else
+				err = 0;
+
+			if (err)
+				return err;
+
+			range_start = -1;
+			range_count = 0;
+
+			/* Handle special address values */
+			if (next->addr == IMX274_TABLE_END)
+				break;
+
+			if (next->addr == IMX274_TABLE_WAIT_MS) {
+				msleep_range(next->val);
+				continue;
+			}
+		}
+
+		val = next->val;
+
+		if (range_start == -1)
+			range_start = next->addr;
+
+		range_vals[range_count++] = val;
+	}
+	return 0;
+}
+
+static inline int imx274_write_reg(struct stimx274 *priv, u16 addr, u8 val)
+{
+	int err;
+
+	err = regmap_write(priv->regmap, addr, val);
+	if (err)
+		dev_err(&priv->client->dev,
+			"%s : i2c write failed, %x = %x\n", __func__,
+			addr, val);
+	else
+		dev_dbg(&priv->client->dev,
+			"%s : addr 0x%x, val=0x%x\n", __func__,
+			addr, val);
+	return err;
+}
+
+/**
+ * imx274_read_mbreg - Read a multibyte register.
+ *
+ * Uses a bulk read where possible.
+ *
+ * @priv: Pointer to device structure
+ * @addr: Address of the LSB register.  Other registers must be
+ *        consecutive, least-to-most significant.
+ * @val: Pointer to store the register value (cpu endianness)
+ * @nbytes: Number of bytes to read (range: [1..3]).
+ *          Other bytes are zet to 0.
+ *
+ * Return: 0 on success, errors otherwise
+ */
+static int imx274_read_mbreg(struct stimx274 *priv, u16 addr, u32 *val,
+			     size_t nbytes)
+{
+	__le32 val_le = 0;
+	int err;
+
+	err = regmap_bulk_read(priv->regmap, addr, &val_le, nbytes);
+	if (err) {
+		dev_err(&priv->client->dev,
+			"%s : i2c bulk read failed, %x (%zu bytes)\n",
+			__func__, addr, nbytes);
+	} else {
+		*val = le32_to_cpu(val_le);
+		dev_dbg(&priv->client->dev,
+			"%s : addr 0x%x, val=0x%x (%zu bytes)\n",
+			__func__, addr, *val, nbytes);
+	}
+
+	return err;
+}
+
+/**
+ * imx274_write_mbreg - Write a multibyte register.
+ *
+ * Uses a bulk write where possible.
+ *
+ * @priv: Pointer to device structure
+ * @addr: Address of the LSB register.  Other registers must be
+ *        consecutive, least-to-most significant.
+ * @val: Value to be written to the register (cpu endianness)
+ * @nbytes: Number of bytes to write (range: [1..3])
+ */
+static int imx274_write_mbreg(struct stimx274 *priv, u16 addr, u32 val,
+			      size_t nbytes)
+{
+	__le32 val_le = cpu_to_le32(val);
+	int err;
+
+	err = regmap_bulk_write(priv->regmap, addr, &val_le, nbytes);
+	if (err)
+		dev_err(&priv->client->dev,
+			"%s : i2c bulk write failed, %x = %x (%zu bytes)\n",
+			__func__, addr, val, nbytes);
+	else
+		dev_dbg(&priv->client->dev,
+			"%s : addr 0x%x, val=0x%x (%zu bytes)\n",
+			__func__, addr, val, nbytes);
+	return err;
+}
+
+/*
+ * Set mode registers to start stream.
+ * @priv: Pointer to device structure
+ *
+ * Return: 0 on success, errors otherwise
+ */
+static int imx274_mode_regs(struct stimx274 *priv)
+{
+	int err = 0;
+
+	err = imx274_write_table(priv, imx274_start_1);
+	if (err)
+		return err;
+
+	err = imx274_write_table(priv, priv->mode->init_regs);
+
+	return err;
+}
+
+/*
+ * imx274_start_stream - Function for starting stream per mode index
+ * @priv: Pointer to device structure
+ *
+ * Return: 0 on success, errors otherwise
+ */
+static int imx274_start_stream(struct stimx274 *priv)
+{
+	int err = 0;
+
+	err = __v4l2_ctrl_handler_setup(&priv->ctrls.handler);
+	if (err) {
+		dev_err(&priv->client->dev, "Error %d setup controls\n", err);
+		return err;
+	}
+
+	/*
+	 * Refer to "Standby Cancel Sequence when using CSI-2" in
+	 * imx274 datasheet, it should wait 10ms or more here.
+	 * give it 1 extra ms for margin
+	 */
+	msleep_range(11);
+	err = imx274_write_table(priv, imx274_start_2);
+	if (err)
+		return err;
+
+	/*
+	 * Refer to "Standby Cancel Sequence when using CSI-2" in
+	 * imx274 datasheet, it should wait 7ms or more here.
+	 * give it 1 extra ms for margin
+	 */
+	msleep_range(8);
+	err = imx274_write_table(priv, imx274_start_3);
+	if (err)
+		return err;
+
+	return 0;
+}
+
+/*
+ * imx274_reset - Function called to reset the sensor
+ * @priv: Pointer to device structure
+ * @rst: Input value for determining the sensor's end state after reset
+ *
+ * Set the senor in reset and then
+ * if rst = 0, keep it in reset;
+ * if rst = 1, bring it out of reset.
+ *
+ */
+static void imx274_reset(struct stimx274 *priv, int rst)
+{
+	gpiod_set_value_cansleep(priv->reset_gpio, 0);
+	usleep_range(IMX274_RESET_DELAY1, IMX274_RESET_DELAY2);
+	gpiod_set_value_cansleep(priv->reset_gpio, !!rst);
+	usleep_range(IMX274_RESET_DELAY1, IMX274_RESET_DELAY2);
+}
+
+static int imx274_power_on(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct stimx274 *imx274 = to_imx274(sd);
+	int ret;
+
+	/* keep sensor in reset before power on */
+	imx274_reset(imx274, 0);
+
+	ret = clk_prepare_enable(imx274->inck);
+	if (ret) {
+		dev_err(&imx274->client->dev,
+			"Failed to enable input clock: %d\n", ret);
+		return ret;
+	}
+
+	ret = regulator_bulk_enable(IMX274_NUM_SUPPLIES, imx274->supplies);
+	if (ret) {
+		dev_err(&imx274->client->dev,
+			"Failed to enable regulators: %d\n", ret);
+		goto fail_reg;
+	}
+
+	udelay(2);
+	imx274_reset(imx274, 1);
+
+	return 0;
+
+fail_reg:
+	clk_disable_unprepare(imx274->inck);
+	return ret;
+}
+
+static int imx274_power_off(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct stimx274 *imx274 = to_imx274(sd);
+
+	imx274_reset(imx274, 0);
+
+	regulator_bulk_disable(IMX274_NUM_SUPPLIES, imx274->supplies);
+
+	clk_disable_unprepare(imx274->inck);
+
+	return 0;
+}
+
+static int imx274_regulators_get(struct device *dev, struct stimx274 *imx274)
+{
+	unsigned int i;
+
+	for (i = 0; i < IMX274_NUM_SUPPLIES; i++)
+		imx274->supplies[i].supply = imx274_supply_names[i];
+
+	return devm_regulator_bulk_get(dev, IMX274_NUM_SUPPLIES,
+					imx274->supplies);
+}
+
+/**
+ * imx274_s_ctrl - This is used to set the imx274 V4L2 controls
+ * @ctrl: V4L2 control to be set
+ *
+ * This function is used to set the V4L2 controls for the imx274 sensor.
+ *
+ * Return: 0 on success, errors otherwise
+ */
+static int imx274_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
+	struct stimx274 *imx274 = to_imx274(sd);
+	int ret = -EINVAL;
+
+	if (!pm_runtime_get_if_in_use(&imx274->client->dev))
+		return 0;
+
+	dev_dbg(&imx274->client->dev,
+		"%s : s_ctrl: %s, value: %d\n", __func__,
+		ctrl->name, ctrl->val);
+
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE:
+		dev_dbg(&imx274->client->dev,
+			"%s : set V4L2_CID_EXPOSURE\n", __func__);
+		ret = imx274_set_exposure(imx274, ctrl->val);
+		break;
+
+	case V4L2_CID_GAIN:
+		dev_dbg(&imx274->client->dev,
+			"%s : set V4L2_CID_GAIN\n", __func__);
+		ret = imx274_set_gain(imx274, ctrl);
+		break;
+
+	case V4L2_CID_VFLIP:
+		dev_dbg(&imx274->client->dev,
+			"%s : set V4L2_CID_VFLIP\n", __func__);
+		ret = imx274_set_vflip(imx274, ctrl->val);
+		break;
+
+	case V4L2_CID_TEST_PATTERN:
+		dev_dbg(&imx274->client->dev,
+			"%s : set V4L2_CID_TEST_PATTERN\n", __func__);
+		ret = imx274_set_test_pattern(imx274, ctrl->val);
+		break;
+	}
+
+	pm_runtime_put(&imx274->client->dev);
+
+	return ret;
+}
+
+static int imx274_binning_goodness(struct stimx274 *imx274,
+				   int w, int ask_w,
+				   int h, int ask_h, u32 flags)
+{
+	struct device *dev = &imx274->client->dev;
+	const int goodness = 100000;
+	int val = 0;
+
+	if (flags & V4L2_SEL_FLAG_GE) {
+		if (w < ask_w)
+			val -= goodness;
+		if (h < ask_h)
+			val -= goodness;
+	}
+
+	if (flags & V4L2_SEL_FLAG_LE) {
+		if (w > ask_w)
+			val -= goodness;
+		if (h > ask_h)
+			val -= goodness;
+	}
+
+	val -= abs(w - ask_w);
+	val -= abs(h - ask_h);
+
+	dev_dbg(dev, "%s: ask %dx%d, size %dx%d, goodness %d\n",
+		__func__, ask_w, ask_h, w, h, val);
+
+	return val;
+}
+
+/**
+ * __imx274_change_compose - Helper function to change binning and set both
+ *	compose and format.
+ *
+ * We have two entry points to change binning: set_fmt and
+ * set_selection(COMPOSE). Both have to compute the new output size
+ * and set it in both the compose rect and the frame format size. We
+ * also need to do the same things after setting cropping to restore
+ * 1:1 binning.
+ *
+ * This function contains the common code for these three cases, it
+ * has many arguments in order to accommodate the needs of all of
+ * them.
+ *
+ * Must be called with imx274->lock locked.
+ *
+ * @imx274: The device object
+ * @sd_state: The subdev state we are editing for TRY requests
+ * @which:  V4L2_SUBDEV_FORMAT_ACTIVE or V4L2_SUBDEV_FORMAT_TRY from the caller
+ * @width:  Input-output parameter: set to the desired width before
+ *          the call, contains the chosen value after returning successfully
+ * @height: Input-output parameter for height (see @width)
+ * @flags:  Selection flags from struct v4l2_subdev_selection, or 0 if not
+ *          available (when called from set_fmt)
+ */
+static int __imx274_change_compose(struct stimx274 *imx274,
+				   struct v4l2_subdev_state *sd_state,
+				   u32 which,
+				   u32 *width,
+				   u32 *height,
+				   u32 flags)
+{
+	struct device *dev = &imx274->client->dev;
+	const struct v4l2_rect *cur_crop;
+	struct v4l2_mbus_framefmt *tgt_fmt;
+	unsigned int i;
+	const struct imx274_mode *best_mode = &imx274_modes[0];
+	int best_goodness = INT_MIN;
+
+	if (which == V4L2_SUBDEV_FORMAT_TRY) {
+		cur_crop = &sd_state->pads->try_crop;
+		tgt_fmt = &sd_state->pads->try_fmt;
+	} else {
+		cur_crop = &imx274->crop;
+		tgt_fmt = &imx274->format;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(imx274_modes); i++) {
+		u8 wratio = imx274_modes[i].wbin_ratio;
+		u8 hratio = imx274_modes[i].hbin_ratio;
+
+		int goodness = imx274_binning_goodness(
+			imx274,
+			cur_crop->width / wratio, *width,
+			cur_crop->height / hratio, *height,
+			flags);
+
+		if (goodness >= best_goodness) {
+			best_goodness = goodness;
+			best_mode = &imx274_modes[i];
+		}
+	}
+
+	*width = cur_crop->width / best_mode->wbin_ratio;
+	*height = cur_crop->height / best_mode->hbin_ratio;
+
+	if (which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		imx274->mode = best_mode;
+
+	dev_dbg(dev, "%s: selected %ux%u binning\n",
+		__func__, best_mode->wbin_ratio, best_mode->hbin_ratio);
+
+	tgt_fmt->width = *width;
+	tgt_fmt->height = *height;
+	tgt_fmt->field = V4L2_FIELD_NONE;
+
+	return 0;
+}
+
+/**
+ * imx274_get_fmt - Get the pad format
+ * @sd: Pointer to V4L2 Sub device structure
+ * @sd_state: Pointer to sub device state structure
+ * @fmt: Pointer to pad level media bus format
+ *
+ * This function is used to get the pad format information.
+ *
+ * Return: 0 on success
+ */
+static int imx274_get_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct stimx274 *imx274 = to_imx274(sd);
+
+	mutex_lock(&imx274->lock);
+	fmt->format = imx274->format;
+	mutex_unlock(&imx274->lock);
+	return 0;
+}
+
+/**
+ * imx274_set_fmt - This is used to set the pad format
+ * @sd: Pointer to V4L2 Sub device structure
+ * @sd_state: Pointer to sub device state information structure
+ * @format: Pointer to pad level media bus format
+ *
+ * This function is used to set the pad format.
+ *
+ * Return: 0 on success
+ */
+static int imx274_set_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+	struct stimx274 *imx274 = to_imx274(sd);
+	int err = 0;
+
+	mutex_lock(&imx274->lock);
+
+	err = __imx274_change_compose(imx274, sd_state, format->which,
+				      &fmt->width, &fmt->height, 0);
+
+	if (err)
+		goto out;
+
+	/*
+	 * __imx274_change_compose already set width and height in the
+	 * applicable format, but we need to keep all other format
+	 * values, so do a full copy here
+	 */
+	fmt->field = V4L2_FIELD_NONE;
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+		sd_state->pads->try_fmt = *fmt;
+	else
+		imx274->format = *fmt;
+
+out:
+	mutex_unlock(&imx274->lock);
+
+	return err;
+}
+
+static int imx274_get_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *sd_state,
+				struct v4l2_subdev_selection *sel)
+{
+	struct stimx274 *imx274 = to_imx274(sd);
+	const struct v4l2_rect *src_crop;
+	const struct v4l2_mbus_framefmt *src_fmt;
+	int ret = 0;
+
+	if (sel->pad != 0)
+		return -EINVAL;
+
+	if (sel->target == V4L2_SEL_TGT_CROP_BOUNDS) {
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = IMX274_MAX_WIDTH;
+		sel->r.height = IMX274_MAX_HEIGHT;
+		return 0;
+	}
+
+	if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
+		src_crop = &sd_state->pads->try_crop;
+		src_fmt = &sd_state->pads->try_fmt;
+	} else {
+		src_crop = &imx274->crop;
+		src_fmt = &imx274->format;
+	}
+
+	mutex_lock(&imx274->lock);
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		sel->r = *src_crop;
+		break;
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+		sel->r.top = 0;
+		sel->r.left = 0;
+		sel->r.width = src_crop->width;
+		sel->r.height = src_crop->height;
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+		sel->r.top = 0;
+		sel->r.left = 0;
+		sel->r.width = src_fmt->width;
+		sel->r.height = src_fmt->height;
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	mutex_unlock(&imx274->lock);
+
+	return ret;
+}
+
+static int imx274_set_selection_crop(struct stimx274 *imx274,
+				     struct v4l2_subdev_state *sd_state,
+				     struct v4l2_subdev_selection *sel)
+{
+	struct v4l2_rect *tgt_crop;
+	struct v4l2_rect new_crop;
+	bool size_changed;
+
+	/*
+	 * h_step could be 12 or 24 depending on the binning. But we
+	 * won't know the binning until we choose the mode later in
+	 * __imx274_change_compose(). Thus let's be safe and use the
+	 * most conservative value in all cases.
+	 */
+	const u32 h_step = 24;
+
+	new_crop.width = min_t(u32,
+			       IMX274_ROUND(sel->r.width, h_step, sel->flags),
+			       IMX274_MAX_WIDTH);
+
+	/* Constraint: HTRIMMING_END - HTRIMMING_START >= 144 */
+	if (new_crop.width < 144)
+		new_crop.width = 144;
+
+	new_crop.left = min_t(u32,
+			      IMX274_ROUND(sel->r.left, h_step, 0),
+			      IMX274_MAX_WIDTH - new_crop.width);
+
+	new_crop.height = min_t(u32,
+				IMX274_ROUND(sel->r.height, 2, sel->flags),
+				IMX274_MAX_HEIGHT);
+
+	new_crop.top = min_t(u32, IMX274_ROUND(sel->r.top, 2, 0),
+			     IMX274_MAX_HEIGHT - new_crop.height);
+
+	sel->r = new_crop;
+
+	if (sel->which == V4L2_SUBDEV_FORMAT_TRY)
+		tgt_crop = &sd_state->pads->try_crop;
+	else
+		tgt_crop = &imx274->crop;
+
+	mutex_lock(&imx274->lock);
+
+	size_changed = (new_crop.width != tgt_crop->width ||
+			new_crop.height != tgt_crop->height);
+
+	/* __imx274_change_compose needs the new size in *tgt_crop */
+	*tgt_crop = new_crop;
+
+	/* if crop size changed then reset the output image size */
+	if (size_changed)
+		__imx274_change_compose(imx274, sd_state, sel->which,
+					&new_crop.width, &new_crop.height,
+					sel->flags);
+
+	mutex_unlock(&imx274->lock);
+
+	return 0;
+}
+
+static int imx274_set_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *sd_state,
+				struct v4l2_subdev_selection *sel)
+{
+	struct stimx274 *imx274 = to_imx274(sd);
+
+	if (sel->pad != 0)
+		return -EINVAL;
+
+	if (sel->target == V4L2_SEL_TGT_CROP)
+		return imx274_set_selection_crop(imx274, sd_state, sel);
+
+	if (sel->target == V4L2_SEL_TGT_COMPOSE) {
+		int err;
+
+		mutex_lock(&imx274->lock);
+		err =  __imx274_change_compose(imx274, sd_state, sel->which,
+					       &sel->r.width, &sel->r.height,
+					       sel->flags);
+		mutex_unlock(&imx274->lock);
+
+		/*
+		 * __imx274_change_compose already set width and
+		 * height in set->r, we still need to set top-left
+		 */
+		if (!err) {
+			sel->r.top = 0;
+			sel->r.left = 0;
+		}
+
+		return err;
+	}
+
+	return -EINVAL;
+}
+
+static int imx274_apply_trimming(struct stimx274 *imx274)
+{
+	u32 h_start;
+	u32 h_end;
+	u32 hmax;
+	u32 v_cut;
+	s32 v_pos;
+	u32 write_v_size;
+	u32 y_out_size;
+	int err;
+
+	h_start = imx274->crop.left + 12;
+	h_end = h_start + imx274->crop.width;
+
+	/* Use the minimum allowed value of HMAX */
+	/* Note: except in mode 1, (width / 16 + 23) is always < hmax_min */
+	/* Note: 260 is the minimum HMAX in all implemented modes */
+	hmax = max_t(u32, 260, (imx274->crop.width) / 16 + 23);
+
+	/* invert v_pos if VFLIP */
+	v_pos = imx274->ctrls.vflip->cur.val ?
+		(-imx274->crop.top / 2) : (imx274->crop.top / 2);
+	v_cut = (IMX274_MAX_HEIGHT - imx274->crop.height) / 2;
+	write_v_size = imx274->crop.height + 22;
+	y_out_size   = imx274->crop.height;
+
+	err = imx274_write_mbreg(imx274, IMX274_HMAX_REG_LSB, hmax, 2);
+	if (!err)
+		err = imx274_write_mbreg(imx274, IMX274_HTRIM_EN_REG, 1, 1);
+	if (!err)
+		err = imx274_write_mbreg(imx274, IMX274_HTRIM_START_REG_LSB,
+					 h_start, 2);
+	if (!err)
+		err = imx274_write_mbreg(imx274, IMX274_HTRIM_END_REG_LSB,
+					 h_end, 2);
+	if (!err)
+		err = imx274_write_mbreg(imx274, IMX274_VWIDCUTEN_REG, 1, 1);
+	if (!err)
+		err = imx274_write_mbreg(imx274, IMX274_VWIDCUT_REG_LSB,
+					 v_cut, 2);
+	if (!err)
+		err = imx274_write_mbreg(imx274, IMX274_VWINPOS_REG_LSB,
+					 v_pos, 2);
+	if (!err)
+		err = imx274_write_mbreg(imx274, IMX274_WRITE_VSIZE_REG_LSB,
+					 write_v_size, 2);
+	if (!err)
+		err = imx274_write_mbreg(imx274, IMX274_Y_OUT_SIZE_REG_LSB,
+					 y_out_size, 2);
+
+	return err;
+}
+
+/**
+ * imx274_g_frame_interval - Get the frame interval
+ * @sd: Pointer to V4L2 Sub device structure
+ * @fi: Pointer to V4l2 Sub device frame interval structure
+ *
+ * This function is used to get the frame interval.
+ *
+ * Return: 0 on success
+ */
+static int imx274_g_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *fi)
+{
+	struct stimx274 *imx274 = to_imx274(sd);
+
+	fi->interval = imx274->frame_interval;
+	dev_dbg(&imx274->client->dev, "%s frame rate = %d / %d\n",
+		__func__, imx274->frame_interval.numerator,
+		imx274->frame_interval.denominator);
+
+	return 0;
+}
+
+/**
+ * imx274_s_frame_interval - Set the frame interval
+ * @sd: Pointer to V4L2 Sub device structure
+ * @fi: Pointer to V4l2 Sub device frame interval structure
+ *
+ * This function is used to set the frame intervavl.
+ *
+ * Return: 0 on success
+ */
+static int imx274_s_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *fi)
+{
+	struct stimx274 *imx274 = to_imx274(sd);
+	struct v4l2_ctrl *ctrl = imx274->ctrls.exposure;
+	int min, max, def;
+	int ret;
+
+	ret = pm_runtime_resume_and_get(&imx274->client->dev);
+	if (ret < 0)
+		return ret;
+
+	mutex_lock(&imx274->lock);
+	ret = imx274_set_frame_interval(imx274, fi->interval);
+
+	if (!ret) {
+		fi->interval = imx274->frame_interval;
+
+		/*
+		 * exposure time range is decided by frame interval
+		 * need to update it after frame interval changes
+		 */
+		min = IMX274_MIN_EXPOSURE_TIME;
+		max = fi->interval.numerator * 1000000
+			/ fi->interval.denominator;
+		def = max;
+		ret = __v4l2_ctrl_modify_range(ctrl, min, max, 1, def);
+		if (ret) {
+			dev_err(&imx274->client->dev,
+				"Exposure ctrl range update failed\n");
+			goto unlock;
+		}
+
+		/* update exposure time accordingly */
+		imx274_set_exposure(imx274, ctrl->val);
+
+		dev_dbg(&imx274->client->dev, "set frame interval to %uus\n",
+			fi->interval.numerator * 1000000
+			/ fi->interval.denominator);
+	}
+
+unlock:
+	mutex_unlock(&imx274->lock);
+	pm_runtime_put(&imx274->client->dev);
+
+	return ret;
+}
+
+/**
+ * imx274_load_default - load default control values
+ * @priv: Pointer to device structure
+ *
+ * Return: 0 on success, errors otherwise
+ */
+static void imx274_load_default(struct stimx274 *priv)
+{
+	/* load default control values */
+	priv->frame_interval.numerator = 1;
+	priv->frame_interval.denominator = IMX274_DEF_FRAME_RATE;
+	priv->ctrls.exposure->val = 1000000 / IMX274_DEF_FRAME_RATE;
+	priv->ctrls.gain->val = IMX274_DEF_GAIN;
+	priv->ctrls.vflip->val = 0;
+	priv->ctrls.test_pattern->val = TEST_PATTERN_DISABLED;
+}
+
+/**
+ * imx274_s_stream - It is used to start/stop the streaming.
+ * @sd: V4L2 Sub device
+ * @on: Flag (True / False)
+ *
+ * This function controls the start or stop of streaming for the
+ * imx274 sensor.
+ *
+ * Return: 0 on success, errors otherwise
+ */
+static int imx274_s_stream(struct v4l2_subdev *sd, int on)
+{
+	struct stimx274 *imx274 = to_imx274(sd);
+	int ret = 0;
+
+	dev_dbg(&imx274->client->dev, "%s : %s, mode index = %td\n", __func__,
+		on ? "Stream Start" : "Stream Stop",
+		imx274->mode - &imx274_modes[0]);
+
+	mutex_lock(&imx274->lock);
+
+	if (on) {
+		ret = pm_runtime_resume_and_get(&imx274->client->dev);
+		if (ret < 0) {
+			mutex_unlock(&imx274->lock);
+			return ret;
+		}
+
+		/* load mode registers */
+		ret = imx274_mode_regs(imx274);
+		if (ret)
+			goto fail;
+
+		ret = imx274_apply_trimming(imx274);
+		if (ret)
+			goto fail;
+
+		/*
+		 * update frame rate & exposure. if the last mode is different,
+		 * HMAX could be changed. As the result, frame rate & exposure
+		 * are changed.
+		 * gain is not affected.
+		 */
+		ret = imx274_set_frame_interval(imx274,
+						imx274->frame_interval);
+		if (ret)
+			goto fail;
+
+		/* start stream */
+		ret = imx274_start_stream(imx274);
+		if (ret)
+			goto fail;
+	} else {
+		/* stop stream */
+		ret = imx274_write_table(imx274, imx274_stop);
+		if (ret)
+			goto fail;
+
+		pm_runtime_put(&imx274->client->dev);
+	}
+
+	mutex_unlock(&imx274->lock);
+	dev_dbg(&imx274->client->dev, "%s : Done\n", __func__);
+	return 0;
+
+fail:
+	pm_runtime_put(&imx274->client->dev);
+	mutex_unlock(&imx274->lock);
+	dev_err(&imx274->client->dev, "s_stream failed\n");
+	return ret;
+}
+
+/*
+ * imx274_get_frame_length - Function for obtaining current frame length
+ * @priv: Pointer to device structure
+ * @val: Pointer to obtained value
+ *
+ * frame_length = vmax x (svr + 1), in unit of hmax.
+ *
+ * Return: 0 on success
+ */
+static int imx274_get_frame_length(struct stimx274 *priv, u32 *val)
+{
+	int err;
+	u32 svr;
+	u32 vmax;
+
+	err = imx274_read_mbreg(priv, IMX274_SVR_REG_LSB, &svr, 2);
+	if (err)
+		goto fail;
+
+	err = imx274_read_mbreg(priv, IMX274_VMAX_REG_3, &vmax, 3);
+	if (err)
+		goto fail;
+
+	*val = vmax * (svr + 1);
+
+	return 0;
+
+fail:
+	dev_err(&priv->client->dev, "%s error = %d\n", __func__, err);
+	return err;
+}
+
+static int imx274_clamp_coarse_time(struct stimx274 *priv, u32 *val,
+				    u32 *frame_length)
+{
+	int err;
+
+	err = imx274_get_frame_length(priv, frame_length);
+	if (err)
+		return err;
+
+	if (*frame_length < priv->mode->min_frame_len)
+		*frame_length =  priv->mode->min_frame_len;
+
+	*val = *frame_length - *val; /* convert to raw shr */
+	if (*val > *frame_length - IMX274_SHR_LIMIT_CONST)
+		*val = *frame_length - IMX274_SHR_LIMIT_CONST;
+	else if (*val < priv->mode->min_SHR)
+		*val = priv->mode->min_SHR;
+
+	return 0;
+}
+
+/*
+ * imx274_set_digital gain - Function called when setting digital gain
+ * @priv: Pointer to device structure
+ * @dgain: Value of digital gain.
+ *
+ * Digital gain has only 4 steps: 1x, 2x, 4x, and 8x
+ *
+ * Return: 0 on success
+ */
+static int imx274_set_digital_gain(struct stimx274 *priv, u32 dgain)
+{
+	u8 reg_val;
+
+	reg_val = ffs(dgain);
+
+	if (reg_val)
+		reg_val--;
+
+	reg_val = clamp(reg_val, (u8)0, (u8)3);
+
+	return imx274_write_reg(priv, IMX274_DIGITAL_GAIN_REG,
+				reg_val & IMX274_MASK_LSB_4_BITS);
+}
+
+/*
+ * imx274_set_gain - Function called when setting gain
+ * @priv: Pointer to device structure
+ * @val: Value of gain. the real value = val << IMX274_GAIN_SHIFT;
+ * @ctrl: v4l2 control pointer
+ *
+ * Set the gain based on input value.
+ * The caller should hold the mutex lock imx274->lock if necessary
+ *
+ * Return: 0 on success
+ */
+static int imx274_set_gain(struct stimx274 *priv, struct v4l2_ctrl *ctrl)
+{
+	int err;
+	u32 gain, analog_gain, digital_gain, gain_reg;
+
+	gain = (u32)(ctrl->val);
+
+	dev_dbg(&priv->client->dev,
+		"%s : input gain = %d.%d\n", __func__,
+		gain >> IMX274_GAIN_SHIFT,
+		((gain & IMX274_GAIN_SHIFT_MASK) * 100) >> IMX274_GAIN_SHIFT);
+
+	if (gain > IMX274_MAX_DIGITAL_GAIN * IMX274_MAX_ANALOG_GAIN)
+		gain = IMX274_MAX_DIGITAL_GAIN * IMX274_MAX_ANALOG_GAIN;
+	else if (gain < IMX274_MIN_GAIN)
+		gain = IMX274_MIN_GAIN;
+
+	if (gain <= IMX274_MAX_ANALOG_GAIN)
+		digital_gain = 1;
+	else if (gain <= IMX274_MAX_ANALOG_GAIN * 2)
+		digital_gain = 2;
+	else if (gain <= IMX274_MAX_ANALOG_GAIN * 4)
+		digital_gain = 4;
+	else
+		digital_gain = IMX274_MAX_DIGITAL_GAIN;
+
+	analog_gain = gain / digital_gain;
+
+	dev_dbg(&priv->client->dev,
+		"%s : digital gain = %d, analog gain = %d.%d\n",
+		__func__, digital_gain, analog_gain >> IMX274_GAIN_SHIFT,
+		((analog_gain & IMX274_GAIN_SHIFT_MASK) * 100)
+		>> IMX274_GAIN_SHIFT);
+
+	err = imx274_set_digital_gain(priv, digital_gain);
+	if (err)
+		goto fail;
+
+	/* convert to register value, refer to imx274 datasheet */
+	gain_reg = (u32)IMX274_GAIN_CONST -
+		(IMX274_GAIN_CONST << IMX274_GAIN_SHIFT) / analog_gain;
+	if (gain_reg > IMX274_GAIN_REG_MAX)
+		gain_reg = IMX274_GAIN_REG_MAX;
+
+	err = imx274_write_mbreg(priv, IMX274_ANALOG_GAIN_ADDR_LSB, gain_reg,
+				 2);
+	if (err)
+		goto fail;
+
+	if (IMX274_GAIN_CONST - gain_reg == 0) {
+		err = -EINVAL;
+		goto fail;
+	}
+
+	/* convert register value back to gain value */
+	ctrl->val = (IMX274_GAIN_CONST << IMX274_GAIN_SHIFT)
+			/ (IMX274_GAIN_CONST - gain_reg) * digital_gain;
+
+	dev_dbg(&priv->client->dev,
+		"%s : GAIN control success, gain_reg = %d, new gain = %d\n",
+		__func__, gain_reg, ctrl->val);
+
+	return 0;
+
+fail:
+	dev_err(&priv->client->dev, "%s error = %d\n", __func__, err);
+	return err;
+}
+
+/*
+ * imx274_set_coarse_time - Function called when setting SHR value
+ * @priv: Pointer to device structure
+ * @val: Value for exposure time in number of line_length, or [HMAX]
+ *
+ * Set SHR value based on input value.
+ *
+ * Return: 0 on success
+ */
+static int imx274_set_coarse_time(struct stimx274 *priv, u32 *val)
+{
+	int err;
+	u32 coarse_time, frame_length;
+
+	coarse_time = *val;
+
+	/* convert exposure_time to appropriate SHR value */
+	err = imx274_clamp_coarse_time(priv, &coarse_time, &frame_length);
+	if (err)
+		goto fail;
+
+	err = imx274_write_mbreg(priv, IMX274_SHR_REG_LSB, coarse_time, 2);
+	if (err)
+		goto fail;
+
+	*val = frame_length - coarse_time;
+	return 0;
+
+fail:
+	dev_err(&priv->client->dev, "%s error = %d\n", __func__, err);
+	return err;
+}
+
+/*
+ * imx274_set_exposure - Function called when setting exposure time
+ * @priv: Pointer to device structure
+ * @val: Variable for exposure time, in the unit of micro-second
+ *
+ * Set exposure time based on input value.
+ * The caller should hold the mutex lock imx274->lock if necessary
+ *
+ * Return: 0 on success
+ */
+static int imx274_set_exposure(struct stimx274 *priv, int val)
+{
+	int err;
+	u32 hmax;
+	u32 coarse_time; /* exposure time in unit of line (HMAX)*/
+
+	dev_dbg(&priv->client->dev,
+		"%s : EXPOSURE control input = %d\n", __func__, val);
+
+	/* step 1: convert input exposure_time (val) into number of 1[HMAX] */
+
+	err = imx274_read_mbreg(priv, IMX274_HMAX_REG_LSB, &hmax, 2);
+	if (err)
+		goto fail;
+
+	if (hmax == 0) {
+		err = -EINVAL;
+		goto fail;
+	}
+
+	coarse_time = (IMX274_PIXCLK_CONST1 / IMX274_PIXCLK_CONST2 * val
+			- priv->mode->nocpiop) / hmax;
+
+	/* step 2: convert exposure_time into SHR value */
+
+	/* set SHR */
+	err = imx274_set_coarse_time(priv, &coarse_time);
+	if (err)
+		goto fail;
+
+	priv->ctrls.exposure->val =
+			(coarse_time * hmax + priv->mode->nocpiop)
+			/ (IMX274_PIXCLK_CONST1 / IMX274_PIXCLK_CONST2);
+
+	dev_dbg(&priv->client->dev,
+		"%s : EXPOSURE control success\n", __func__);
+	return 0;
+
+fail:
+	dev_err(&priv->client->dev, "%s error = %d\n", __func__, err);
+
+	return err;
+}
+
+/*
+ * imx274_set_vflip - Function called when setting vertical flip
+ * @priv: Pointer to device structure
+ * @val: Value for vflip setting
+ *
+ * Set vertical flip based on input value.
+ * val = 0: normal, no vertical flip
+ * val = 1: vertical flip enabled
+ * The caller should hold the mutex lock imx274->lock if necessary
+ *
+ * Return: 0 on success
+ */
+static int imx274_set_vflip(struct stimx274 *priv, int val)
+{
+	int err;
+
+	err = imx274_write_reg(priv, IMX274_VFLIP_REG, val);
+	if (err) {
+		dev_err(&priv->client->dev, "VFLIP control error\n");
+		return err;
+	}
+
+	dev_dbg(&priv->client->dev,
+		"%s : VFLIP control success\n", __func__);
+
+	return 0;
+}
+
+/*
+ * imx274_set_test_pattern - Function called when setting test pattern
+ * @priv: Pointer to device structure
+ * @val: Variable for test pattern
+ *
+ * Set to different test patterns based on input value.
+ *
+ * Return: 0 on success
+ */
+static int imx274_set_test_pattern(struct stimx274 *priv, int val)
+{
+	int err = 0;
+
+	if (val == TEST_PATTERN_DISABLED) {
+		err = imx274_write_table(priv, imx274_tp_disabled);
+	} else if (val <= TEST_PATTERN_V_COLOR_BARS) {
+		err = imx274_write_reg(priv, IMX274_TEST_PATTERN_REG, val - 1);
+		if (!err)
+			err = imx274_write_table(priv, imx274_tp_regs);
+	} else {
+		err = -EINVAL;
+	}
+
+	if (!err)
+		dev_dbg(&priv->client->dev,
+			"%s : TEST PATTERN control success\n", __func__);
+	else
+		dev_err(&priv->client->dev, "%s error = %d\n", __func__, err);
+
+	return err;
+}
+
+/*
+ * imx274_set_frame_length - Function called when setting frame length
+ * @priv: Pointer to device structure
+ * @val: Variable for frame length (= VMAX, i.e. vertical drive period length)
+ *
+ * Set frame length based on input value.
+ *
+ * Return: 0 on success
+ */
+static int imx274_set_frame_length(struct stimx274 *priv, u32 val)
+{
+	int err;
+	u32 frame_length;
+
+	dev_dbg(&priv->client->dev, "%s : input length = %d\n",
+		__func__, val);
+
+	frame_length = (u32)val;
+
+	err = imx274_write_mbreg(priv, IMX274_VMAX_REG_3, frame_length, 3);
+	if (err)
+		goto fail;
+
+	return 0;
+
+fail:
+	dev_err(&priv->client->dev, "%s error = %d\n", __func__, err);
+	return err;
+}
+
+/*
+ * imx274_set_frame_interval - Function called when setting frame interval
+ * @priv: Pointer to device structure
+ * @frame_interval: Variable for frame interval
+ *
+ * Change frame interval by updating VMAX value
+ * The caller should hold the mutex lock imx274->lock if necessary
+ *
+ * Return: 0 on success
+ */
+static int imx274_set_frame_interval(struct stimx274 *priv,
+				     struct v4l2_fract frame_interval)
+{
+	int err;
+	u32 frame_length, req_frame_rate;
+	u32 svr;
+	u32 hmax;
+
+	dev_dbg(&priv->client->dev, "%s: input frame interval = %d / %d",
+		__func__, frame_interval.numerator,
+		frame_interval.denominator);
+
+	if (frame_interval.numerator == 0 || frame_interval.denominator == 0) {
+		frame_interval.denominator = IMX274_DEF_FRAME_RATE;
+		frame_interval.numerator = 1;
+	}
+
+	req_frame_rate = (u32)(frame_interval.denominator
+				/ frame_interval.numerator);
+
+	/* boundary check */
+	if (req_frame_rate > priv->mode->max_fps) {
+		frame_interval.numerator = 1;
+		frame_interval.denominator = priv->mode->max_fps;
+	} else if (req_frame_rate < IMX274_MIN_FRAME_RATE) {
+		frame_interval.numerator = 1;
+		frame_interval.denominator = IMX274_MIN_FRAME_RATE;
+	}
+
+	/*
+	 * VMAX = 1/frame_rate x 72M / (SVR+1) / HMAX
+	 * frame_length (i.e. VMAX) = (frame_interval) x 72M /(SVR+1) / HMAX
+	 */
+
+	err = imx274_read_mbreg(priv, IMX274_SVR_REG_LSB, &svr, 2);
+	if (err)
+		goto fail;
+
+	dev_dbg(&priv->client->dev,
+		"%s : register SVR = %d\n", __func__, svr);
+
+	err = imx274_read_mbreg(priv, IMX274_HMAX_REG_LSB, &hmax, 2);
+	if (err)
+		goto fail;
+
+	dev_dbg(&priv->client->dev,
+		"%s : register HMAX = %d\n", __func__, hmax);
+
+	if (hmax == 0 || frame_interval.denominator == 0) {
+		err = -EINVAL;
+		goto fail;
+	}
+
+	frame_length = IMX274_PIXCLK_CONST1 / (svr + 1) / hmax
+					* frame_interval.numerator
+					/ frame_interval.denominator;
+
+	err = imx274_set_frame_length(priv, frame_length);
+	if (err)
+		goto fail;
+
+	priv->frame_interval = frame_interval;
+	return 0;
+
+fail:
+	dev_err(&priv->client->dev, "%s error = %d\n", __func__, err);
+	return err;
+}
+
+static int imx274_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index > 0)
+		return -EINVAL;
+
+	/* only supported format in the driver is Raw 10 bits SRGGB */
+	code->code = MEDIA_BUS_FMT_SRGGB10_1X10;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_pad_ops imx274_pad_ops = {
+	.enum_mbus_code = imx274_enum_mbus_code,
+	.get_fmt = imx274_get_fmt,
+	.set_fmt = imx274_set_fmt,
+	.get_selection = imx274_get_selection,
+	.set_selection = imx274_set_selection,
+};
+
+static const struct v4l2_subdev_video_ops imx274_video_ops = {
+	.g_frame_interval = imx274_g_frame_interval,
+	.s_frame_interval = imx274_s_frame_interval,
+	.s_stream = imx274_s_stream,
+};
+
+static const struct v4l2_subdev_ops imx274_subdev_ops = {
+	.pad = &imx274_pad_ops,
+	.video = &imx274_video_ops,
+};
+
+static const struct v4l2_ctrl_ops imx274_ctrl_ops = {
+	.s_ctrl	= imx274_s_ctrl,
+};
+
+static const struct of_device_id imx274_of_id_table[] = {
+	{ .compatible = "sony,imx274" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, imx274_of_id_table);
+
+static const struct i2c_device_id imx274_id[] = {
+	{ "IMX274", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, imx274_id);
+
+static int imx274_fwnode_parse(struct device *dev)
+{
+	struct fwnode_handle *endpoint;
+	/* Only CSI2 is supported */
+	struct v4l2_fwnode_endpoint ep = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY
+	};
+	int ret;
+
+	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_parse(endpoint, &ep);
+	fwnode_handle_put(endpoint);
+	if (ret == -ENXIO) {
+		dev_err(dev, "Unsupported bus type, should be CSI2\n");
+		return ret;
+	} else if (ret) {
+		dev_err(dev, "Parsing endpoint node failed %d\n", ret);
+		return ret;
+	}
+
+	/* Check number of data lanes, only 4 lanes supported */
+	if (ep.bus.mipi_csi2.num_data_lanes != 4) {
+		dev_err(dev, "Invalid data lanes: %d\n",
+			ep.bus.mipi_csi2.num_data_lanes);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int imx274_probe(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd;
+	struct stimx274 *imx274;
+	struct device *dev = &client->dev;
+	int ret;
+
+	/* initialize imx274 */
+	imx274 = devm_kzalloc(dev, sizeof(*imx274), GFP_KERNEL);
+	if (!imx274)
+		return -ENOMEM;
+
+	mutex_init(&imx274->lock);
+
+	ret = imx274_fwnode_parse(dev);
+	if (ret)
+		return ret;
+
+	imx274->inck = devm_clk_get_optional(dev, "inck");
+	if (IS_ERR(imx274->inck))
+		return PTR_ERR(imx274->inck);
+
+	ret = imx274_regulators_get(dev, imx274);
+	if (ret) {
+		dev_err(dev, "Failed to get power regulators, err: %d\n", ret);
+		return ret;
+	}
+
+	/* initialize format */
+	imx274->mode = &imx274_modes[0];
+	imx274->crop.width = IMX274_MAX_WIDTH;
+	imx274->crop.height = IMX274_MAX_HEIGHT;
+	imx274->format.width = imx274->crop.width / imx274->mode->wbin_ratio;
+	imx274->format.height = imx274->crop.height / imx274->mode->hbin_ratio;
+	imx274->format.field = V4L2_FIELD_NONE;
+	imx274->format.code = MEDIA_BUS_FMT_SRGGB10_1X10;
+	imx274->format.colorspace = V4L2_COLORSPACE_SRGB;
+	imx274->frame_interval.numerator = 1;
+	imx274->frame_interval.denominator = IMX274_DEF_FRAME_RATE;
+
+	/* initialize regmap */
+	imx274->regmap = devm_regmap_init_i2c(client, &imx274_regmap_config);
+	if (IS_ERR(imx274->regmap)) {
+		dev_err(dev,
+			"regmap init failed: %ld\n", PTR_ERR(imx274->regmap));
+		ret = -ENODEV;
+		goto err_regmap;
+	}
+
+	/* initialize subdevice */
+	imx274->client = client;
+	sd = &imx274->sd;
+	v4l2_i2c_subdev_init(sd, client, &imx274_subdev_ops);
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
+
+	/* initialize subdev media pad */
+	imx274->pad.flags = MEDIA_PAD_FL_SOURCE;
+	sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret = media_entity_pads_init(&sd->entity, 1, &imx274->pad);
+	if (ret < 0) {
+		dev_err(dev,
+			"%s : media entity init Failed %d\n", __func__, ret);
+		goto err_regmap;
+	}
+
+	/* initialize sensor reset gpio */
+	imx274->reset_gpio = devm_gpiod_get_optional(dev, "reset",
+						     GPIOD_OUT_HIGH);
+	if (IS_ERR(imx274->reset_gpio)) {
+		ret = dev_err_probe(dev, PTR_ERR(imx274->reset_gpio),
+				    "Reset GPIO not setup in DT\n");
+		goto err_me;
+	}
+
+	/* power on the sensor */
+	ret = imx274_power_on(dev);
+	if (ret < 0) {
+		dev_err(dev, "%s : imx274 power on failed\n", __func__);
+		goto err_me;
+	}
+
+	/* initialize controls */
+	ret = v4l2_ctrl_handler_init(&imx274->ctrls.handler, 4);
+	if (ret < 0) {
+		dev_err(dev, "%s : ctrl handler init Failed\n", __func__);
+		goto err_power_off;
+	}
+
+	imx274->ctrls.handler.lock = &imx274->lock;
+
+	/* add new controls */
+	imx274->ctrls.test_pattern = v4l2_ctrl_new_std_menu_items(
+		&imx274->ctrls.handler, &imx274_ctrl_ops,
+		V4L2_CID_TEST_PATTERN,
+		ARRAY_SIZE(tp_qmenu) - 1, 0, 0, tp_qmenu);
+
+	imx274->ctrls.gain = v4l2_ctrl_new_std(
+		&imx274->ctrls.handler,
+		&imx274_ctrl_ops,
+		V4L2_CID_GAIN, IMX274_MIN_GAIN,
+		IMX274_MAX_DIGITAL_GAIN * IMX274_MAX_ANALOG_GAIN, 1,
+		IMX274_DEF_GAIN);
+
+	imx274->ctrls.exposure = v4l2_ctrl_new_std(
+		&imx274->ctrls.handler,
+		&imx274_ctrl_ops,
+		V4L2_CID_EXPOSURE, IMX274_MIN_EXPOSURE_TIME,
+		1000000 / IMX274_DEF_FRAME_RATE, 1,
+		IMX274_MIN_EXPOSURE_TIME);
+
+	imx274->ctrls.vflip = v4l2_ctrl_new_std(
+		&imx274->ctrls.handler,
+		&imx274_ctrl_ops,
+		V4L2_CID_VFLIP, 0, 1, 1, 0);
+
+	imx274->sd.ctrl_handler = &imx274->ctrls.handler;
+	if (imx274->ctrls.handler.error) {
+		ret = imx274->ctrls.handler.error;
+		goto err_ctrls;
+	}
+
+	/* load default control values */
+	imx274_load_default(imx274);
+
+	/* register subdevice */
+	ret = v4l2_async_register_subdev(sd);
+	if (ret < 0) {
+		dev_err(dev, "%s : v4l2_async_register_subdev failed %d\n",
+			__func__, ret);
+		goto err_ctrls;
+	}
+
+	pm_runtime_set_active(dev);
+	pm_runtime_enable(dev);
+	pm_runtime_idle(dev);
+
+	dev_info(dev, "imx274 : imx274 probe success !\n");
+	return 0;
+
+err_ctrls:
+	v4l2_ctrl_handler_free(&imx274->ctrls.handler);
+err_power_off:
+	imx274_power_off(dev);
+err_me:
+	media_entity_cleanup(&sd->entity);
+err_regmap:
+	mutex_destroy(&imx274->lock);
+	return ret;
+}
+
+static void imx274_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct stimx274 *imx274 = to_imx274(sd);
+
+	pm_runtime_disable(&client->dev);
+	if (!pm_runtime_status_suspended(&client->dev))
+		imx274_power_off(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+
+	v4l2_async_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&imx274->ctrls.handler);
+
+	media_entity_cleanup(&sd->entity);
+	mutex_destroy(&imx274->lock);
+}
+
+static const struct dev_pm_ops imx274_pm_ops = {
+	SET_RUNTIME_PM_OPS(imx274_power_off, imx274_power_on, NULL)
+};
+
+static struct i2c_driver imx274_i2c_driver = {
+	.driver = {
+		.name	= DRIVER_NAME,
+		.pm = &imx274_pm_ops,
+		.of_match_table	= imx274_of_id_table,
+	},
+	.probe		= imx274_probe,
+	.remove		= imx274_remove,
+	.id_table	= imx274_id,
+};
+
+module_i2c_driver(imx274_i2c_driver);
+
+MODULE_AUTHOR("Leon Luo <leonl@leopardimaging.com>");
+MODULE_DESCRIPTION("IMX274 CMOS Image Sensor driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/imx290.c b/drivers/media/i2c/imx290.c
new file mode 100644
index 000000000..c6fea5837
--- /dev/null
+++ b/drivers/media/i2c/imx290.c
@@ -0,0 +1,1668 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Sony IMX290 CMOS Image Sensor Driver
+ *
+ * Copyright (C) 2019 FRAMOS GmbH.
+ *
+ * Copyright (C) 2019 Linaro Ltd.
+ * Author: Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/regulator/consumer.h>
+
+#include <asm/unaligned.h>
+
+#include <media/media-entity.h>
+#include <media/v4l2-cci.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+#define IMX290_STANDBY					CCI_REG8(0x3000)
+#define IMX290_REGHOLD					CCI_REG8(0x3001)
+#define IMX290_XMSTA					CCI_REG8(0x3002)
+#define IMX290_ADBIT					CCI_REG8(0x3005)
+#define IMX290_ADBIT_10BIT				(0 << 0)
+#define IMX290_ADBIT_12BIT				(1 << 0)
+#define IMX290_CTRL_07					CCI_REG8(0x3007)
+#define IMX290_VREVERSE					BIT(0)
+#define IMX290_HREVERSE					BIT(1)
+#define IMX290_WINMODE_1080P				(0 << 4)
+#define IMX290_WINMODE_720P				(1 << 4)
+#define IMX290_WINMODE_CROP				(4 << 4)
+#define IMX290_FR_FDG_SEL				CCI_REG8(0x3009)
+#define IMX290_BLKLEVEL					CCI_REG16_LE(0x300a)
+#define IMX290_GAIN					CCI_REG8(0x3014)
+#define IMX290_VMAX					CCI_REG24_LE(0x3018)
+#define IMX290_VMAX_MAX					0x3ffff
+#define IMX290_HMAX					CCI_REG16_LE(0x301c)
+#define IMX290_HMAX_MAX					0xffff
+#define IMX290_SHS1					CCI_REG24_LE(0x3020)
+#define IMX290_WINWV_OB					CCI_REG8(0x303a)
+#define IMX290_WINPV					CCI_REG16_LE(0x303c)
+#define IMX290_WINWV					CCI_REG16_LE(0x303e)
+#define IMX290_WINPH					CCI_REG16_LE(0x3040)
+#define IMX290_WINWH					CCI_REG16_LE(0x3042)
+#define IMX290_OUT_CTRL					CCI_REG8(0x3046)
+#define IMX290_ODBIT_10BIT				(0 << 0)
+#define IMX290_ODBIT_12BIT				(1 << 0)
+#define IMX290_OPORTSEL_PARALLEL			(0x0 << 4)
+#define IMX290_OPORTSEL_LVDS_2CH			(0xd << 4)
+#define IMX290_OPORTSEL_LVDS_4CH			(0xe << 4)
+#define IMX290_OPORTSEL_LVDS_8CH			(0xf << 4)
+#define IMX290_XSOUTSEL					CCI_REG8(0x304b)
+#define IMX290_XSOUTSEL_XVSOUTSEL_HIGH			(0 << 0)
+#define IMX290_XSOUTSEL_XVSOUTSEL_VSYNC			(2 << 0)
+#define IMX290_XSOUTSEL_XHSOUTSEL_HIGH			(0 << 2)
+#define IMX290_XSOUTSEL_XHSOUTSEL_HSYNC			(2 << 2)
+#define IMX290_INCKSEL1					CCI_REG8(0x305c)
+#define IMX290_INCKSEL2					CCI_REG8(0x305d)
+#define IMX290_INCKSEL3					CCI_REG8(0x305e)
+#define IMX290_INCKSEL4					CCI_REG8(0x305f)
+#define IMX290_PGCTRL					CCI_REG8(0x308c)
+#define IMX290_ADBIT1					CCI_REG8(0x3129)
+#define IMX290_ADBIT1_10BIT				0x1d
+#define IMX290_ADBIT1_12BIT				0x00
+#define IMX290_INCKSEL5					CCI_REG8(0x315e)
+#define IMX290_INCKSEL6					CCI_REG8(0x3164)
+#define IMX290_ADBIT2					CCI_REG8(0x317c)
+#define IMX290_ADBIT2_10BIT				0x12
+#define IMX290_ADBIT2_12BIT				0x00
+#define IMX290_CHIP_ID					CCI_REG16_LE(0x319a)
+#define IMX290_ADBIT3					CCI_REG8(0x31ec)
+#define IMX290_ADBIT3_10BIT				0x37
+#define IMX290_ADBIT3_12BIT				0x0e
+#define IMX290_REPETITION				CCI_REG8(0x3405)
+#define IMX290_PHY_LANE_NUM				CCI_REG8(0x3407)
+#define IMX290_OPB_SIZE_V				CCI_REG8(0x3414)
+#define IMX290_Y_OUT_SIZE				CCI_REG16_LE(0x3418)
+#define IMX290_CSI_DT_FMT				CCI_REG16_LE(0x3441)
+#define IMX290_CSI_DT_FMT_RAW10				0x0a0a
+#define IMX290_CSI_DT_FMT_RAW12				0x0c0c
+#define IMX290_CSI_LANE_MODE				CCI_REG8(0x3443)
+#define IMX290_EXTCK_FREQ				CCI_REG16_LE(0x3444)
+#define IMX290_TCLKPOST					CCI_REG16_LE(0x3446)
+#define IMX290_THSZERO					CCI_REG16_LE(0x3448)
+#define IMX290_THSPREPARE				CCI_REG16_LE(0x344a)
+#define IMX290_TCLKTRAIL				CCI_REG16_LE(0x344c)
+#define IMX290_THSTRAIL					CCI_REG16_LE(0x344e)
+#define IMX290_TCLKZERO					CCI_REG16_LE(0x3450)
+#define IMX290_TCLKPREPARE				CCI_REG16_LE(0x3452)
+#define IMX290_TLPX					CCI_REG16_LE(0x3454)
+#define IMX290_X_OUT_SIZE				CCI_REG16_LE(0x3472)
+#define IMX290_INCKSEL7					CCI_REG8(0x3480)
+
+#define IMX290_PGCTRL_REGEN				BIT(0)
+#define IMX290_PGCTRL_THRU				BIT(1)
+#define IMX290_PGCTRL_MODE(n)				((n) << 4)
+
+/* Number of lines by which exposure must be less than VMAX */
+#define IMX290_EXPOSURE_OFFSET				2
+
+#define IMX290_PIXEL_RATE				148500000
+
+/*
+ * The IMX290 pixel array is organized as follows:
+ *
+ *     +------------------------------------+
+ *     |           Optical Black            |     }  Vertical effective optical black (10)
+ * +---+------------------------------------+---+
+ * |   |                                    |   | }  Effective top margin (8)
+ * |   |   +----------------------------+   |   | \
+ * |   |   |                            |   |   |  |
+ * |   |   |                            |   |   |  |
+ * |   |   |                            |   |   |  |
+ * |   |   |    Recording Pixel Area    |   |   |  | Recommended height (1080)
+ * |   |   |                            |   |   |  |
+ * |   |   |                            |   |   |  |
+ * |   |   |                            |   |   |  |
+ * |   |   +----------------------------+   |   | /
+ * |   |                                    |   | }  Effective bottom margin (9)
+ * +---+------------------------------------+---+
+ *  <-> <-> <--------------------------> <-> <->
+ *                                            \----  Ignored right margin (4)
+ *                                        \--------  Effective right margin (9)
+ *                       \-------------------------  Recommended width (1920)
+ *       \-----------------------------------------  Effective left margin (8)
+ *   \---------------------------------------------  Ignored left margin (4)
+ *
+ * The optical black lines are output over CSI-2 with a separate data type.
+ *
+ * The pixel array is meant to have 1920x1080 usable pixels after image
+ * processing in an ISP. It has 8 (9) extra active pixels usable for color
+ * processing in the ISP on the top and left (bottom and right) sides of the
+ * image. In addition, 4 additional pixels are present on the left and right
+ * sides of the image, documented as "ignored area".
+ *
+ * As far as is understood, all pixels of the pixel array (ignored area, color
+ * processing margins and recording area) can be output by the sensor.
+ */
+
+#define IMX290_PIXEL_ARRAY_WIDTH			1945
+#define IMX290_PIXEL_ARRAY_HEIGHT			1097
+#define IMX920_PIXEL_ARRAY_MARGIN_LEFT			12
+#define IMX920_PIXEL_ARRAY_MARGIN_RIGHT			13
+#define IMX920_PIXEL_ARRAY_MARGIN_TOP			8
+#define IMX920_PIXEL_ARRAY_MARGIN_BOTTOM		9
+#define IMX290_PIXEL_ARRAY_RECORDING_WIDTH		1920
+#define IMX290_PIXEL_ARRAY_RECORDING_HEIGHT		1080
+
+/* Equivalent value for 16bpp */
+#define IMX290_BLACK_LEVEL_DEFAULT			3840
+
+#define IMX290_NUM_SUPPLIES				3
+
+enum imx290_colour_variant {
+	IMX290_VARIANT_COLOUR,
+	IMX290_VARIANT_MONO,
+	IMX290_VARIANT_MAX
+};
+
+enum imx290_model {
+	IMX290_MODEL_IMX290LQR,
+	IMX290_MODEL_IMX290LLR,
+	IMX290_MODEL_IMX327LQR,
+};
+
+struct imx290_model_info {
+	enum imx290_colour_variant colour_variant;
+	const struct cci_reg_sequence *init_regs;
+	size_t init_regs_num;
+	const char *name;
+};
+
+enum imx290_clk_freq {
+	IMX290_CLK_37_125,
+	IMX290_CLK_74_25,
+	IMX290_NUM_CLK
+};
+
+/*
+ * Clock configuration for registers INCKSEL1 to INCKSEL6.
+ */
+struct imx290_clk_cfg {
+	u8 incksel1;
+	u8 incksel2;
+	u8 incksel3;
+	u8 incksel4;
+	u8 incksel5;
+	u8 incksel6;
+};
+
+struct imx290_mode {
+	u32 width;
+	u32 height;
+	u32 hmax_min;
+	u32 vmax_min;
+	u8 link_freq_index;
+	u8 ctrl_07;
+
+	const struct cci_reg_sequence *data;
+	u32 data_size;
+
+	const struct imx290_clk_cfg *clk_cfg;
+};
+
+struct imx290_csi_cfg {
+	u16 repetition;
+	u16 tclkpost;
+	u16 thszero;
+	u16 thsprepare;
+	u16 tclktrail;
+	u16 thstrail;
+	u16 tclkzero;
+	u16 tclkprepare;
+	u16 tlpx;
+};
+
+struct imx290 {
+	struct device *dev;
+	struct clk *xclk;
+	struct regmap *regmap;
+	enum imx290_clk_freq xclk_idx;
+	u8 nlanes;
+	const struct imx290_model_info *model;
+
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+
+	const struct imx290_mode *current_mode;
+
+	struct regulator_bulk_data supplies[IMX290_NUM_SUPPLIES];
+	struct gpio_desc *rst_gpio;
+
+	struct v4l2_ctrl_handler ctrls;
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *exposure;
+	struct {
+		struct v4l2_ctrl *hflip;
+		struct v4l2_ctrl *vflip;
+	};
+};
+
+static inline struct imx290 *to_imx290(struct v4l2_subdev *_sd)
+{
+	return container_of(_sd, struct imx290, sd);
+}
+
+/* -----------------------------------------------------------------------------
+ * Modes and formats
+ */
+
+static const struct cci_reg_sequence imx290_global_init_settings[] = {
+	{ IMX290_WINWV_OB, 12 },
+	{ IMX290_WINPH, 0 },
+	{ IMX290_WINPV, 0 },
+	{ IMX290_WINWH, 1948 },
+	{ IMX290_WINWV, 1097 },
+	{ IMX290_XSOUTSEL, IMX290_XSOUTSEL_XVSOUTSEL_VSYNC |
+			   IMX290_XSOUTSEL_XHSOUTSEL_HSYNC },
+	{ CCI_REG8(0x3011), 0x02 },
+	{ CCI_REG8(0x3012), 0x64 },
+	{ CCI_REG8(0x3013), 0x00 },
+};
+
+static const struct cci_reg_sequence imx290_global_init_settings_290[] = {
+	{ CCI_REG8(0x300f), 0x00 },
+	{ CCI_REG8(0x3010), 0x21 },
+	{ CCI_REG8(0x3016), 0x09 },
+	{ CCI_REG8(0x3070), 0x02 },
+	{ CCI_REG8(0x3071), 0x11 },
+	{ CCI_REG8(0x309b), 0x10 },
+	{ CCI_REG8(0x309c), 0x22 },
+	{ CCI_REG8(0x30a2), 0x02 },
+	{ CCI_REG8(0x30a6), 0x20 },
+	{ CCI_REG8(0x30a8), 0x20 },
+	{ CCI_REG8(0x30aa), 0x20 },
+	{ CCI_REG8(0x30ac), 0x20 },
+	{ CCI_REG8(0x30b0), 0x43 },
+	{ CCI_REG8(0x3119), 0x9e },
+	{ CCI_REG8(0x311c), 0x1e },
+	{ CCI_REG8(0x311e), 0x08 },
+	{ CCI_REG8(0x3128), 0x05 },
+	{ CCI_REG8(0x313d), 0x83 },
+	{ CCI_REG8(0x3150), 0x03 },
+	{ CCI_REG8(0x317e), 0x00 },
+	{ CCI_REG8(0x32b8), 0x50 },
+	{ CCI_REG8(0x32b9), 0x10 },
+	{ CCI_REG8(0x32ba), 0x00 },
+	{ CCI_REG8(0x32bb), 0x04 },
+	{ CCI_REG8(0x32c8), 0x50 },
+	{ CCI_REG8(0x32c9), 0x10 },
+	{ CCI_REG8(0x32ca), 0x00 },
+	{ CCI_REG8(0x32cb), 0x04 },
+	{ CCI_REG8(0x332c), 0xd3 },
+	{ CCI_REG8(0x332d), 0x10 },
+	{ CCI_REG8(0x332e), 0x0d },
+	{ CCI_REG8(0x3358), 0x06 },
+	{ CCI_REG8(0x3359), 0xe1 },
+	{ CCI_REG8(0x335a), 0x11 },
+	{ CCI_REG8(0x3360), 0x1e },
+	{ CCI_REG8(0x3361), 0x61 },
+	{ CCI_REG8(0x3362), 0x10 },
+	{ CCI_REG8(0x33b0), 0x50 },
+	{ CCI_REG8(0x33b2), 0x1a },
+	{ CCI_REG8(0x33b3), 0x04 },
+};
+
+#define IMX290_NUM_CLK_REGS	2
+static const struct cci_reg_sequence xclk_regs[][IMX290_NUM_CLK_REGS] = {
+	[IMX290_CLK_37_125] = {
+		{ IMX290_EXTCK_FREQ, (37125 * 256) / 1000 },
+		{ IMX290_INCKSEL7, 0x49 },
+	},
+	[IMX290_CLK_74_25] = {
+		{ IMX290_EXTCK_FREQ, (74250 * 256) / 1000 },
+		{ IMX290_INCKSEL7, 0x92 },
+	},
+};
+
+static const struct cci_reg_sequence imx290_global_init_settings_327[] = {
+	{ CCI_REG8(0x309e), 0x4A },
+	{ CCI_REG8(0x309f), 0x4A },
+	{ CCI_REG8(0x313b), 0x61 },
+};
+
+static const struct cci_reg_sequence imx290_1080p_settings[] = {
+	/* mode settings */
+	{ IMX290_WINWV_OB, 12 },
+	{ IMX290_OPB_SIZE_V, 10 },
+	{ IMX290_X_OUT_SIZE, 1920 },
+	{ IMX290_Y_OUT_SIZE, 1080 },
+};
+
+static const struct cci_reg_sequence imx290_720p_settings[] = {
+	/* mode settings */
+	{ IMX290_WINWV_OB, 6 },
+	{ IMX290_OPB_SIZE_V, 4 },
+	{ IMX290_X_OUT_SIZE, 1280 },
+	{ IMX290_Y_OUT_SIZE, 720 },
+};
+
+static const struct cci_reg_sequence imx290_10bit_settings[] = {
+	{ IMX290_ADBIT, IMX290_ADBIT_10BIT },
+	{ IMX290_OUT_CTRL, IMX290_ODBIT_10BIT },
+	{ IMX290_ADBIT1, IMX290_ADBIT1_10BIT },
+	{ IMX290_ADBIT2, IMX290_ADBIT2_10BIT },
+	{ IMX290_ADBIT3, IMX290_ADBIT3_10BIT },
+	{ IMX290_CSI_DT_FMT, IMX290_CSI_DT_FMT_RAW10 },
+};
+
+static const struct cci_reg_sequence imx290_12bit_settings[] = {
+	{ IMX290_ADBIT, IMX290_ADBIT_12BIT },
+	{ IMX290_OUT_CTRL, IMX290_ODBIT_12BIT },
+	{ IMX290_ADBIT1, IMX290_ADBIT1_12BIT },
+	{ IMX290_ADBIT2, IMX290_ADBIT2_12BIT },
+	{ IMX290_ADBIT3, IMX290_ADBIT3_12BIT },
+	{ IMX290_CSI_DT_FMT, IMX290_CSI_DT_FMT_RAW12 },
+};
+
+static const struct imx290_csi_cfg imx290_csi_222_75mhz = {
+	/* 222.75MHz or 445.5Mbit/s per lane */
+	.repetition = 0x10,
+	.tclkpost = 87,
+	.thszero = 55,
+	.thsprepare = 31,
+	.tclktrail = 31,
+	.thstrail = 31,
+	.tclkzero = 119,
+	.tclkprepare = 31,
+	.tlpx = 23,
+};
+
+static const struct imx290_csi_cfg imx290_csi_445_5mhz = {
+	/* 445.5MHz or 891Mbit/s per lane */
+	.repetition = 0x00,
+	.tclkpost = 119,
+	.thszero = 103,
+	.thsprepare = 71,
+	.tclktrail = 55,
+	.thstrail = 63,
+	.tclkzero = 255,
+	.tclkprepare = 63,
+	.tlpx = 55,
+};
+
+static const struct imx290_csi_cfg imx290_csi_148_5mhz = {
+	/* 148.5MHz or 297Mbit/s per lane */
+	.repetition = 0x10,
+	.tclkpost = 79,
+	.thszero = 47,
+	.thsprepare = 23,
+	.tclktrail = 23,
+	.thstrail = 23,
+	.tclkzero = 87,
+	.tclkprepare = 23,
+	.tlpx = 23,
+};
+
+static const struct imx290_csi_cfg imx290_csi_297mhz = {
+	/* 297MHz or 594Mbit/s per lane */
+	.repetition = 0x00,
+	.tclkpost = 103,
+	.thszero = 87,
+	.thsprepare = 47,
+	.tclktrail = 39,
+	.thstrail = 47,
+	.tclkzero = 191,
+	.tclkprepare = 47,
+	.tlpx = 39,
+};
+
+/* supported link frequencies */
+#define FREQ_INDEX_1080P	0
+#define FREQ_INDEX_720P		1
+static const s64 imx290_link_freq_2lanes[] = {
+	[FREQ_INDEX_1080P] = 445500000,
+	[FREQ_INDEX_720P] = 297000000,
+};
+
+static const s64 imx290_link_freq_4lanes[] = {
+	[FREQ_INDEX_1080P] = 222750000,
+	[FREQ_INDEX_720P] = 148500000,
+};
+
+/*
+ * In this function and in the similar ones below We rely on imx290_probe()
+ * to ensure that nlanes is either 2 or 4.
+ */
+static inline const s64 *imx290_link_freqs_ptr(const struct imx290 *imx290)
+{
+	if (imx290->nlanes == 2)
+		return imx290_link_freq_2lanes;
+	else
+		return imx290_link_freq_4lanes;
+}
+
+static inline int imx290_link_freqs_num(const struct imx290 *imx290)
+{
+	if (imx290->nlanes == 2)
+		return ARRAY_SIZE(imx290_link_freq_2lanes);
+	else
+		return ARRAY_SIZE(imx290_link_freq_4lanes);
+}
+
+static const struct imx290_clk_cfg imx290_1080p_clock_config[] = {
+	[IMX290_CLK_37_125] = {
+		/* 37.125MHz clock config */
+		.incksel1 = 0x18,
+		.incksel2 = 0x03,
+		.incksel3 = 0x20,
+		.incksel4 = 0x01,
+		.incksel5 = 0x1a,
+		.incksel6 = 0x1a,
+	},
+	[IMX290_CLK_74_25] = {
+		/* 74.25MHz clock config */
+		.incksel1 = 0x0c,
+		.incksel2 = 0x03,
+		.incksel3 = 0x10,
+		.incksel4 = 0x01,
+		.incksel5 = 0x1b,
+		.incksel6 = 0x1b,
+	},
+};
+
+static const struct imx290_clk_cfg imx290_720p_clock_config[] = {
+	[IMX290_CLK_37_125] = {
+		/* 37.125MHz clock config */
+		.incksel1 = 0x20,
+		.incksel2 = 0x00,
+		.incksel3 = 0x20,
+		.incksel4 = 0x01,
+		.incksel5 = 0x1a,
+		.incksel6 = 0x1a,
+	},
+	[IMX290_CLK_74_25] = {
+		/* 74.25MHz clock config */
+		.incksel1 = 0x10,
+		.incksel2 = 0x00,
+		.incksel3 = 0x10,
+		.incksel4 = 0x01,
+		.incksel5 = 0x1b,
+		.incksel6 = 0x1b,
+	},
+};
+
+/* Mode configs */
+static const struct imx290_mode imx290_modes_2lanes[] = {
+	{
+		.width = 1920,
+		.height = 1080,
+		.hmax_min = 2200,
+		.vmax_min = 1125,
+		.link_freq_index = FREQ_INDEX_1080P,
+		.ctrl_07 = IMX290_WINMODE_1080P,
+		.data = imx290_1080p_settings,
+		.data_size = ARRAY_SIZE(imx290_1080p_settings),
+		.clk_cfg = imx290_1080p_clock_config,
+	},
+	{
+		.width = 1280,
+		.height = 720,
+		.hmax_min = 3300,
+		.vmax_min = 750,
+		.link_freq_index = FREQ_INDEX_720P,
+		.ctrl_07 = IMX290_WINMODE_720P,
+		.data = imx290_720p_settings,
+		.data_size = ARRAY_SIZE(imx290_720p_settings),
+		.clk_cfg = imx290_720p_clock_config,
+	},
+};
+
+static const struct imx290_mode imx290_modes_4lanes[] = {
+	{
+		.width = 1920,
+		.height = 1080,
+		.hmax_min = 2200,
+		.vmax_min = 1125,
+		.link_freq_index = FREQ_INDEX_1080P,
+		.ctrl_07 = IMX290_WINMODE_1080P,
+		.data = imx290_1080p_settings,
+		.data_size = ARRAY_SIZE(imx290_1080p_settings),
+		.clk_cfg = imx290_1080p_clock_config,
+	},
+	{
+		.width = 1280,
+		.height = 720,
+		.hmax_min = 3300,
+		.vmax_min = 750,
+		.link_freq_index = FREQ_INDEX_720P,
+		.ctrl_07 = IMX290_WINMODE_720P,
+		.data = imx290_720p_settings,
+		.data_size = ARRAY_SIZE(imx290_720p_settings),
+		.clk_cfg = imx290_720p_clock_config,
+	},
+};
+
+static inline const struct imx290_mode *imx290_modes_ptr(const struct imx290 *imx290)
+{
+	if (imx290->nlanes == 2)
+		return imx290_modes_2lanes;
+	else
+		return imx290_modes_4lanes;
+}
+
+static inline int imx290_modes_num(const struct imx290 *imx290)
+{
+	if (imx290->nlanes == 2)
+		return ARRAY_SIZE(imx290_modes_2lanes);
+	else
+		return ARRAY_SIZE(imx290_modes_4lanes);
+}
+
+struct imx290_format_info {
+	u32 code[IMX290_VARIANT_MAX];
+	u8 bpp;
+	const struct cci_reg_sequence *regs;
+	unsigned int num_regs;
+};
+
+static const struct imx290_format_info imx290_formats[] = {
+	{
+		.code = {
+			[IMX290_VARIANT_COLOUR] = MEDIA_BUS_FMT_SRGGB10_1X10,
+			[IMX290_VARIANT_MONO] = MEDIA_BUS_FMT_Y10_1X10
+		},
+		.bpp = 10,
+		.regs = imx290_10bit_settings,
+		.num_regs = ARRAY_SIZE(imx290_10bit_settings),
+	}, {
+		.code = {
+			[IMX290_VARIANT_COLOUR] = MEDIA_BUS_FMT_SRGGB12_1X12,
+			[IMX290_VARIANT_MONO] = MEDIA_BUS_FMT_Y12_1X12
+		},
+		.bpp = 12,
+		.regs = imx290_12bit_settings,
+		.num_regs = ARRAY_SIZE(imx290_12bit_settings),
+	}
+};
+
+static const struct imx290_format_info *
+imx290_format_info(const struct imx290 *imx290, u32 code)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(imx290_formats); ++i) {
+		const struct imx290_format_info *info = &imx290_formats[i];
+
+		if (info->code[imx290->model->colour_variant] == code)
+			return info;
+	}
+
+	return NULL;
+}
+
+static int imx290_set_register_array(struct imx290 *imx290,
+				     const struct cci_reg_sequence *settings,
+				     unsigned int num_settings)
+{
+	int ret;
+
+	ret = cci_multi_reg_write(imx290->regmap, settings, num_settings, NULL);
+	if (ret < 0)
+		return ret;
+
+	/* Provide 10ms settle time */
+	usleep_range(10000, 11000);
+
+	return 0;
+}
+
+static int imx290_set_clock(struct imx290 *imx290)
+{
+	const struct imx290_mode *mode = imx290->current_mode;
+	enum imx290_clk_freq clk_idx = imx290->xclk_idx;
+	const struct imx290_clk_cfg *clk_cfg = &mode->clk_cfg[clk_idx];
+	int ret;
+
+	ret = imx290_set_register_array(imx290, xclk_regs[clk_idx],
+					IMX290_NUM_CLK_REGS);
+
+	cci_write(imx290->regmap, IMX290_INCKSEL1, clk_cfg->incksel1, &ret);
+	cci_write(imx290->regmap, IMX290_INCKSEL2, clk_cfg->incksel2, &ret);
+	cci_write(imx290->regmap, IMX290_INCKSEL3, clk_cfg->incksel3, &ret);
+	cci_write(imx290->regmap, IMX290_INCKSEL4, clk_cfg->incksel4, &ret);
+	cci_write(imx290->regmap, IMX290_INCKSEL5, clk_cfg->incksel5, &ret);
+	cci_write(imx290->regmap, IMX290_INCKSEL6, clk_cfg->incksel6, &ret);
+
+	return ret;
+}
+
+static int imx290_set_data_lanes(struct imx290 *imx290)
+{
+	int ret = 0;
+
+	cci_write(imx290->regmap, IMX290_PHY_LANE_NUM, imx290->nlanes - 1,
+		  &ret);
+	cci_write(imx290->regmap, IMX290_CSI_LANE_MODE, imx290->nlanes - 1,
+		  &ret);
+	cci_write(imx290->regmap, IMX290_FR_FDG_SEL, 0x01, &ret);
+
+	return ret;
+}
+
+static int imx290_set_black_level(struct imx290 *imx290,
+				  const struct v4l2_mbus_framefmt *format,
+				  unsigned int black_level, int *err)
+{
+	unsigned int bpp = imx290_format_info(imx290, format->code)->bpp;
+
+	return cci_write(imx290->regmap, IMX290_BLKLEVEL,
+			 black_level >> (16 - bpp), err);
+}
+
+static int imx290_set_csi_config(struct imx290 *imx290)
+{
+	const s64 *link_freqs = imx290_link_freqs_ptr(imx290);
+	const struct imx290_csi_cfg *csi_cfg;
+	int ret = 0;
+
+	switch (link_freqs[imx290->current_mode->link_freq_index]) {
+	case 445500000:
+		csi_cfg = &imx290_csi_445_5mhz;
+		break;
+	case 297000000:
+		csi_cfg = &imx290_csi_297mhz;
+		break;
+	case 222750000:
+		csi_cfg = &imx290_csi_222_75mhz;
+		break;
+	case 148500000:
+		csi_cfg = &imx290_csi_148_5mhz;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	cci_write(imx290->regmap, IMX290_REPETITION, csi_cfg->repetition, &ret);
+	cci_write(imx290->regmap, IMX290_TCLKPOST, csi_cfg->tclkpost, &ret);
+	cci_write(imx290->regmap, IMX290_THSZERO, csi_cfg->thszero, &ret);
+	cci_write(imx290->regmap, IMX290_THSPREPARE, csi_cfg->thsprepare, &ret);
+	cci_write(imx290->regmap, IMX290_TCLKTRAIL, csi_cfg->tclktrail, &ret);
+	cci_write(imx290->regmap, IMX290_THSTRAIL, csi_cfg->thstrail, &ret);
+	cci_write(imx290->regmap, IMX290_TCLKZERO, csi_cfg->tclkzero, &ret);
+	cci_write(imx290->regmap, IMX290_TCLKPREPARE, csi_cfg->tclkprepare,
+		  &ret);
+	cci_write(imx290->regmap, IMX290_TLPX, csi_cfg->tlpx, &ret);
+
+	return ret;
+}
+
+static int imx290_setup_format(struct imx290 *imx290,
+			       const struct v4l2_mbus_framefmt *format)
+{
+	const struct imx290_format_info *info;
+	int ret;
+
+	info = imx290_format_info(imx290, format->code);
+
+	ret = imx290_set_register_array(imx290, info->regs, info->num_regs);
+	if (ret < 0) {
+		dev_err(imx290->dev, "Could not set format registers\n");
+		return ret;
+	}
+
+	return imx290_set_black_level(imx290, format,
+				      IMX290_BLACK_LEVEL_DEFAULT, &ret);
+}
+
+/* ----------------------------------------------------------------------------
+ * Controls
+ */
+static void imx290_exposure_update(struct imx290 *imx290,
+				   const struct imx290_mode *mode)
+{
+	unsigned int exposure_max;
+
+	exposure_max = imx290->vblank->val + mode->height -
+		       IMX290_EXPOSURE_OFFSET;
+	__v4l2_ctrl_modify_range(imx290->exposure, 1, exposure_max, 1,
+				 exposure_max);
+}
+
+static int imx290_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct imx290 *imx290 = container_of(ctrl->handler,
+					     struct imx290, ctrls);
+	const struct v4l2_mbus_framefmt *format;
+	struct v4l2_subdev_state *state;
+	int ret = 0, vmax;
+
+	/*
+	 * Return immediately for controls that don't need to be applied to the
+	 * device.
+	 */
+	if (ctrl->flags & V4L2_CTRL_FLAG_READ_ONLY)
+		return 0;
+
+	if (ctrl->id == V4L2_CID_VBLANK) {
+		/* Changing vblank changes the allowed range for exposure. */
+		imx290_exposure_update(imx290, imx290->current_mode);
+	}
+
+	/* V4L2 controls values will be applied only when power is already up */
+	if (!pm_runtime_get_if_in_use(imx290->dev))
+		return 0;
+
+	state = v4l2_subdev_get_locked_active_state(&imx290->sd);
+	format = v4l2_subdev_get_pad_format(&imx290->sd, state, 0);
+
+	switch (ctrl->id) {
+	case V4L2_CID_ANALOGUE_GAIN:
+		ret = cci_write(imx290->regmap, IMX290_GAIN, ctrl->val, NULL);
+		break;
+
+	case V4L2_CID_VBLANK:
+		ret = cci_write(imx290->regmap, IMX290_VMAX,
+				ctrl->val + imx290->current_mode->height, NULL);
+		/*
+		 * Due to the way that exposure is programmed in this sensor in
+		 * relation to VMAX, we have to reprogramme it whenever VMAX is
+		 * changed.
+		 * Update ctrl so that the V4L2_CID_EXPOSURE case can refer to
+		 * it.
+		 */
+		ctrl = imx290->exposure;
+		fallthrough;
+	case V4L2_CID_EXPOSURE:
+		vmax = imx290->vblank->val + imx290->current_mode->height;
+		ret = cci_write(imx290->regmap, IMX290_SHS1,
+				vmax - ctrl->val - 1, NULL);
+		break;
+
+	case V4L2_CID_TEST_PATTERN:
+		if (ctrl->val) {
+			imx290_set_black_level(imx290, format, 0, &ret);
+			usleep_range(10000, 11000);
+			cci_write(imx290->regmap, IMX290_PGCTRL,
+				  (u8)(IMX290_PGCTRL_REGEN |
+				       IMX290_PGCTRL_THRU |
+				       IMX290_PGCTRL_MODE(ctrl->val)), &ret);
+		} else {
+			cci_write(imx290->regmap, IMX290_PGCTRL, 0x00, &ret);
+			usleep_range(10000, 11000);
+			imx290_set_black_level(imx290, format,
+					       IMX290_BLACK_LEVEL_DEFAULT, &ret);
+		}
+		break;
+
+	case V4L2_CID_HBLANK:
+		ret = cci_write(imx290->regmap, IMX290_HMAX,
+				ctrl->val + imx290->current_mode->width, NULL);
+		break;
+
+	case V4L2_CID_HFLIP:
+	case V4L2_CID_VFLIP:
+	{
+		u32 reg;
+
+		reg = imx290->current_mode->ctrl_07;
+		if (imx290->hflip->val)
+			reg |= IMX290_HREVERSE;
+		if (imx290->vflip->val)
+			reg |= IMX290_VREVERSE;
+		ret = cci_write(imx290->regmap, IMX290_CTRL_07, reg, NULL);
+		break;
+	}
+
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	pm_runtime_mark_last_busy(imx290->dev);
+	pm_runtime_put_autosuspend(imx290->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops imx290_ctrl_ops = {
+	.s_ctrl = imx290_set_ctrl,
+};
+
+static const char * const imx290_test_pattern_menu[] = {
+	"Disabled",
+	"Sequence Pattern 1",
+	"Horizontal Color-bar Chart",
+	"Vertical Color-bar Chart",
+	"Sequence Pattern 2",
+	"Gradation Pattern 1",
+	"Gradation Pattern 2",
+	"000/555h Toggle Pattern",
+};
+
+static void imx290_ctrl_update(struct imx290 *imx290,
+			       const struct imx290_mode *mode)
+{
+	unsigned int hblank_min = mode->hmax_min - mode->width;
+	unsigned int hblank_max = IMX290_HMAX_MAX - mode->width;
+	unsigned int vblank_min = mode->vmax_min - mode->height;
+	unsigned int vblank_max = IMX290_VMAX_MAX - mode->height;
+
+	__v4l2_ctrl_s_ctrl(imx290->link_freq, mode->link_freq_index);
+
+	__v4l2_ctrl_modify_range(imx290->hblank, hblank_min, hblank_max, 1,
+				 hblank_min);
+	__v4l2_ctrl_modify_range(imx290->vblank, vblank_min, vblank_max, 1,
+				 vblank_min);
+}
+
+static int imx290_ctrl_init(struct imx290 *imx290)
+{
+	struct v4l2_fwnode_device_properties props;
+	int ret;
+
+	ret = v4l2_fwnode_device_parse(imx290->dev, &props);
+	if (ret < 0)
+		return ret;
+
+	v4l2_ctrl_handler_init(&imx290->ctrls, 11);
+
+	/*
+	 * The sensor has an analog gain and a digital gain, both controlled
+	 * through a single gain value, expressed in 0.3dB increments. Values
+	 * from 0.0dB (0) to 30.0dB (100) apply analog gain only, higher values
+	 * up to 72.0dB (240) add further digital gain. Limit the range to
+	 * analog gain only, support for digital gain can be added separately
+	 * if needed.
+	 *
+	 * The IMX327 and IMX462 are largely compatible with the IMX290, but
+	 * have an analog gain range of 0.0dB to 29.4dB and 42dB of digital
+	 * gain. When support for those sensors gets added to the driver, the
+	 * gain control should be adjusted accordingly.
+	 */
+	v4l2_ctrl_new_std(&imx290->ctrls, &imx290_ctrl_ops,
+			  V4L2_CID_ANALOGUE_GAIN, 0, 100, 1, 0);
+
+	/*
+	 * Correct range will be determined through imx290_ctrl_update setting
+	 * V4L2_CID_VBLANK.
+	 */
+	imx290->exposure = v4l2_ctrl_new_std(&imx290->ctrls, &imx290_ctrl_ops,
+					     V4L2_CID_EXPOSURE, 1, 65535, 1,
+					     65535);
+
+	/*
+	 * Set the link frequency, pixel rate, horizontal blanking and vertical
+	 * blanking to hardcoded values, they will be updated by
+	 * imx290_ctrl_update().
+	 */
+	imx290->link_freq =
+		v4l2_ctrl_new_int_menu(&imx290->ctrls, &imx290_ctrl_ops,
+				       V4L2_CID_LINK_FREQ,
+				       imx290_link_freqs_num(imx290) - 1, 0,
+				       imx290_link_freqs_ptr(imx290));
+	if (imx290->link_freq)
+		imx290->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	v4l2_ctrl_new_std(&imx290->ctrls, &imx290_ctrl_ops, V4L2_CID_PIXEL_RATE,
+			  IMX290_PIXEL_RATE, IMX290_PIXEL_RATE, 1,
+			  IMX290_PIXEL_RATE);
+
+	v4l2_ctrl_new_std_menu_items(&imx290->ctrls, &imx290_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(imx290_test_pattern_menu) - 1,
+				     0, 0, imx290_test_pattern_menu);
+
+	/*
+	 * Actual range will be set from imx290_ctrl_update later in the probe.
+	 */
+	imx290->hblank = v4l2_ctrl_new_std(&imx290->ctrls, &imx290_ctrl_ops,
+					   V4L2_CID_HBLANK, 1, 1, 1, 1);
+
+	imx290->vblank = v4l2_ctrl_new_std(&imx290->ctrls, &imx290_ctrl_ops,
+					   V4L2_CID_VBLANK, 1, 1, 1, 1);
+
+	imx290->hflip = v4l2_ctrl_new_std(&imx290->ctrls, &imx290_ctrl_ops,
+					  V4L2_CID_HFLIP, 0, 1, 1, 0);
+	imx290->vflip = v4l2_ctrl_new_std(&imx290->ctrls, &imx290_ctrl_ops,
+					  V4L2_CID_VFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_cluster(2, &imx290->hflip);
+
+	v4l2_ctrl_new_fwnode_properties(&imx290->ctrls, &imx290_ctrl_ops,
+					&props);
+
+	imx290->sd.ctrl_handler = &imx290->ctrls;
+
+	if (imx290->ctrls.error) {
+		ret = imx290->ctrls.error;
+		v4l2_ctrl_handler_free(&imx290->ctrls);
+		return ret;
+	}
+
+	return 0;
+}
+
+/* ----------------------------------------------------------------------------
+ * Subdev operations
+ */
+
+/* Start streaming */
+static int imx290_start_streaming(struct imx290 *imx290,
+				  struct v4l2_subdev_state *state)
+{
+	const struct v4l2_mbus_framefmt *format;
+	int ret;
+
+	/* Set init register settings */
+	ret = imx290_set_register_array(imx290, imx290_global_init_settings,
+					ARRAY_SIZE(imx290_global_init_settings));
+	if (ret < 0) {
+		dev_err(imx290->dev, "Could not set init registers\n");
+		return ret;
+	}
+
+	/* Set mdel specific init register settings */
+	ret = imx290_set_register_array(imx290, imx290->model->init_regs,
+					imx290->model->init_regs_num);
+	if (ret < 0) {
+		dev_err(imx290->dev, "Could not set model specific init registers\n");
+		return ret;
+	}
+
+	/* Set clock parameters based on mode and xclk */
+	ret = imx290_set_clock(imx290);
+	if (ret < 0) {
+		dev_err(imx290->dev, "Could not set clocks - %d\n", ret);
+		return ret;
+	}
+
+	/* Set data lane count */
+	ret = imx290_set_data_lanes(imx290);
+	if (ret < 0) {
+		dev_err(imx290->dev, "Could not set data lanes - %d\n", ret);
+		return ret;
+	}
+
+	ret = imx290_set_csi_config(imx290);
+	if (ret < 0) {
+		dev_err(imx290->dev, "Could not set csi cfg - %d\n", ret);
+		return ret;
+	}
+
+	/* Apply the register values related to current frame format */
+	format = v4l2_subdev_get_pad_format(&imx290->sd, state, 0);
+	ret = imx290_setup_format(imx290, format);
+	if (ret < 0) {
+		dev_err(imx290->dev, "Could not set frame format - %d\n", ret);
+		return ret;
+	}
+
+	/* Apply default values of current mode */
+	ret = imx290_set_register_array(imx290, imx290->current_mode->data,
+					imx290->current_mode->data_size);
+	if (ret < 0) {
+		dev_err(imx290->dev, "Could not set current mode - %d\n", ret);
+		return ret;
+	}
+
+	/* Apply customized values from user */
+	ret = __v4l2_ctrl_handler_setup(imx290->sd.ctrl_handler);
+	if (ret) {
+		dev_err(imx290->dev, "Could not sync v4l2 controls - %d\n", ret);
+		return ret;
+	}
+
+	cci_write(imx290->regmap, IMX290_STANDBY, 0x00, &ret);
+
+	msleep(30);
+
+	/* Start streaming */
+	return cci_write(imx290->regmap, IMX290_XMSTA, 0x00, &ret);
+}
+
+/* Stop streaming */
+static int imx290_stop_streaming(struct imx290 *imx290)
+{
+	int ret = 0;
+
+	cci_write(imx290->regmap, IMX290_STANDBY, 0x01, &ret);
+
+	msleep(30);
+
+	return cci_write(imx290->regmap, IMX290_XMSTA, 0x01, &ret);
+}
+
+static int imx290_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct imx290 *imx290 = to_imx290(sd);
+	struct v4l2_subdev_state *state;
+	int ret = 0;
+
+	state = v4l2_subdev_lock_and_get_active_state(sd);
+
+	if (enable) {
+		ret = pm_runtime_resume_and_get(imx290->dev);
+		if (ret < 0)
+			goto unlock;
+
+		ret = imx290_start_streaming(imx290, state);
+		if (ret) {
+			dev_err(imx290->dev, "Start stream failed\n");
+			pm_runtime_put_sync(imx290->dev);
+			goto unlock;
+		}
+	} else {
+		imx290_stop_streaming(imx290);
+		pm_runtime_mark_last_busy(imx290->dev);
+		pm_runtime_put_autosuspend(imx290->dev);
+	}
+
+	/*
+	 * vflip and hflip should not be changed during streaming as the sensor
+	 * will produce an invalid frame.
+	 */
+	__v4l2_ctrl_grab(imx290->vflip, enable);
+	__v4l2_ctrl_grab(imx290->hflip, enable);
+
+unlock:
+	v4l2_subdev_unlock_state(state);
+	return ret;
+}
+
+static int imx290_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	const struct imx290 *imx290 = to_imx290(sd);
+
+	if (code->index >= ARRAY_SIZE(imx290_formats))
+		return -EINVAL;
+
+	code->code = imx290_formats[code->index].code[imx290->model->colour_variant];
+
+	return 0;
+}
+
+static int imx290_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	const struct imx290 *imx290 = to_imx290(sd);
+	const struct imx290_mode *imx290_modes = imx290_modes_ptr(imx290);
+
+	if (!imx290_format_info(imx290, fse->code))
+		return -EINVAL;
+
+	if (fse->index >= imx290_modes_num(imx290))
+		return -EINVAL;
+
+	fse->min_width = imx290_modes[fse->index].width;
+	fse->max_width = imx290_modes[fse->index].width;
+	fse->min_height = imx290_modes[fse->index].height;
+	fse->max_height = imx290_modes[fse->index].height;
+
+	return 0;
+}
+
+static int imx290_set_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct imx290 *imx290 = to_imx290(sd);
+	const struct imx290_mode *mode;
+	struct v4l2_mbus_framefmt *format;
+
+	mode = v4l2_find_nearest_size(imx290_modes_ptr(imx290),
+				      imx290_modes_num(imx290), width, height,
+				      fmt->format.width, fmt->format.height);
+
+	fmt->format.width = mode->width;
+	fmt->format.height = mode->height;
+
+	if (!imx290_format_info(imx290, fmt->format.code))
+		fmt->format.code = imx290_formats[0].code[imx290->model->colour_variant];
+
+	fmt->format.field = V4L2_FIELD_NONE;
+	fmt->format.colorspace = V4L2_COLORSPACE_RAW;
+	fmt->format.ycbcr_enc = V4L2_YCBCR_ENC_601;
+	fmt->format.quantization = V4L2_QUANTIZATION_FULL_RANGE;
+	fmt->format.xfer_func = V4L2_XFER_FUNC_NONE;
+
+	format = v4l2_subdev_get_pad_format(sd, sd_state, 0);
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		imx290->current_mode = mode;
+
+		imx290_ctrl_update(imx290, mode);
+		imx290_exposure_update(imx290, mode);
+	}
+
+	*format = fmt->format;
+
+	return 0;
+}
+
+static int imx290_get_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *sd_state,
+				struct v4l2_subdev_selection *sel)
+{
+	struct imx290 *imx290 = to_imx290(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP: {
+		format = v4l2_subdev_get_pad_format(sd, sd_state, 0);
+
+		/*
+		 * The sensor moves the readout by 1 pixel based on flips to
+		 * keep the Bayer order the same.
+		 */
+		sel->r.top = IMX920_PIXEL_ARRAY_MARGIN_TOP
+			   + (IMX290_PIXEL_ARRAY_RECORDING_HEIGHT - format->height) / 2
+			   + imx290->vflip->val;
+		sel->r.left = IMX920_PIXEL_ARRAY_MARGIN_LEFT
+			    + (IMX290_PIXEL_ARRAY_RECORDING_WIDTH - format->width) / 2
+			    + imx290->hflip->val;
+		sel->r.width = format->width;
+		sel->r.height = format->height;
+
+		return 0;
+	}
+
+	case V4L2_SEL_TGT_NATIVE_SIZE:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.top = 0;
+		sel->r.left = 0;
+		sel->r.width = IMX290_PIXEL_ARRAY_WIDTH;
+		sel->r.height = IMX290_PIXEL_ARRAY_HEIGHT;
+
+		return 0;
+
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		sel->r.top = IMX920_PIXEL_ARRAY_MARGIN_TOP;
+		sel->r.left = IMX920_PIXEL_ARRAY_MARGIN_LEFT;
+		sel->r.width = IMX290_PIXEL_ARRAY_RECORDING_WIDTH;
+		sel->r.height = IMX290_PIXEL_ARRAY_RECORDING_HEIGHT;
+
+		return 0;
+
+	default:
+		return -EINVAL;
+	}
+}
+
+static int imx290_entity_init_cfg(struct v4l2_subdev *subdev,
+				  struct v4l2_subdev_state *sd_state)
+{
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+		.format = {
+			.width = 1920,
+			.height = 1080,
+		},
+	};
+
+	imx290_set_fmt(subdev, sd_state, &fmt);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops imx290_core_ops = {
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_video_ops imx290_video_ops = {
+	.s_stream = imx290_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops imx290_pad_ops = {
+	.init_cfg = imx290_entity_init_cfg,
+	.enum_mbus_code = imx290_enum_mbus_code,
+	.enum_frame_size = imx290_enum_frame_size,
+	.get_fmt = v4l2_subdev_get_fmt,
+	.set_fmt = imx290_set_fmt,
+	.get_selection = imx290_get_selection,
+};
+
+static const struct v4l2_subdev_ops imx290_subdev_ops = {
+	.core = &imx290_core_ops,
+	.video = &imx290_video_ops,
+	.pad = &imx290_pad_ops,
+};
+
+static const struct media_entity_operations imx290_subdev_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static int imx290_subdev_init(struct imx290 *imx290)
+{
+	struct i2c_client *client = to_i2c_client(imx290->dev);
+	struct v4l2_subdev_state *state;
+	int ret;
+
+	imx290->current_mode = &imx290_modes_ptr(imx290)[0];
+
+	v4l2_i2c_subdev_init(&imx290->sd, client, &imx290_subdev_ops);
+	imx290->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+			    V4L2_SUBDEV_FL_HAS_EVENTS;
+	imx290->sd.dev = imx290->dev;
+	imx290->sd.entity.ops = &imx290_subdev_entity_ops;
+	imx290->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+	imx290->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&imx290->sd.entity, 1, &imx290->pad);
+	if (ret < 0) {
+		dev_err(imx290->dev, "Could not register media entity\n");
+		return ret;
+	}
+
+	ret = imx290_ctrl_init(imx290);
+	if (ret < 0) {
+		dev_err(imx290->dev, "Control initialization error %d\n", ret);
+		goto err_media;
+	}
+
+	imx290->sd.state_lock = imx290->ctrls.lock;
+
+	ret = v4l2_subdev_init_finalize(&imx290->sd);
+	if (ret < 0) {
+		dev_err(imx290->dev, "subdev initialization error %d\n", ret);
+		goto err_ctrls;
+	}
+
+	state = v4l2_subdev_lock_and_get_active_state(&imx290->sd);
+	imx290_ctrl_update(imx290, imx290->current_mode);
+	v4l2_subdev_unlock_state(state);
+
+	return 0;
+
+err_ctrls:
+	v4l2_ctrl_handler_free(&imx290->ctrls);
+err_media:
+	media_entity_cleanup(&imx290->sd.entity);
+	return ret;
+}
+
+static void imx290_subdev_cleanup(struct imx290 *imx290)
+{
+	v4l2_subdev_cleanup(&imx290->sd);
+	media_entity_cleanup(&imx290->sd.entity);
+	v4l2_ctrl_handler_free(&imx290->ctrls);
+}
+
+/* ----------------------------------------------------------------------------
+ * Power management
+ */
+
+static int imx290_power_on(struct imx290 *imx290)
+{
+	int ret;
+
+	ret = clk_prepare_enable(imx290->xclk);
+	if (ret) {
+		dev_err(imx290->dev, "Failed to enable clock\n");
+		return ret;
+	}
+
+	ret = regulator_bulk_enable(ARRAY_SIZE(imx290->supplies),
+				    imx290->supplies);
+	if (ret) {
+		dev_err(imx290->dev, "Failed to enable regulators\n");
+		clk_disable_unprepare(imx290->xclk);
+		return ret;
+	}
+
+	usleep_range(1, 2);
+	gpiod_set_value_cansleep(imx290->rst_gpio, 0);
+	usleep_range(30000, 31000);
+
+	return 0;
+}
+
+static void imx290_power_off(struct imx290 *imx290)
+{
+	clk_disable_unprepare(imx290->xclk);
+	gpiod_set_value_cansleep(imx290->rst_gpio, 1);
+	regulator_bulk_disable(ARRAY_SIZE(imx290->supplies), imx290->supplies);
+}
+
+static int imx290_runtime_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct imx290 *imx290 = to_imx290(sd);
+
+	return imx290_power_on(imx290);
+}
+
+static int imx290_runtime_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct imx290 *imx290 = to_imx290(sd);
+
+	imx290_power_off(imx290);
+
+	return 0;
+}
+
+static const struct dev_pm_ops imx290_pm_ops = {
+	RUNTIME_PM_OPS(imx290_runtime_suspend, imx290_runtime_resume, NULL)
+};
+
+/* ----------------------------------------------------------------------------
+ * Probe & remove
+ */
+
+static const char * const imx290_supply_name[IMX290_NUM_SUPPLIES] = {
+	"vdda",
+	"vddd",
+	"vdddo",
+};
+
+static int imx290_get_regulators(struct device *dev, struct imx290 *imx290)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(imx290->supplies); i++)
+		imx290->supplies[i].supply = imx290_supply_name[i];
+
+	return devm_regulator_bulk_get(dev, ARRAY_SIZE(imx290->supplies),
+				       imx290->supplies);
+}
+
+static int imx290_init_clk(struct imx290 *imx290)
+{
+	u32 xclk_freq;
+	int ret;
+
+	ret = device_property_read_u32(imx290->dev, "clock-frequency",
+				       &xclk_freq);
+	if (ret) {
+		dev_err(imx290->dev, "Could not get xclk frequency\n");
+		return ret;
+	}
+
+	/* external clock must be 37.125 MHz or 74.25MHz */
+	switch (xclk_freq) {
+	case 37125000:
+		imx290->xclk_idx = IMX290_CLK_37_125;
+		break;
+	case 74250000:
+		imx290->xclk_idx = IMX290_CLK_74_25;
+		break;
+	default:
+		dev_err(imx290->dev, "External clock frequency %u is not supported\n",
+			xclk_freq);
+		return -EINVAL;
+	}
+
+	ret = clk_set_rate(imx290->xclk, xclk_freq);
+	if (ret) {
+		dev_err(imx290->dev, "Could not set xclk frequency\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+/*
+ * Returns 0 if all link frequencies used by the driver for the given number
+ * of MIPI data lanes are mentioned in the device tree, or the value of the
+ * first missing frequency otherwise.
+ */
+static s64 imx290_check_link_freqs(const struct imx290 *imx290,
+				   const struct v4l2_fwnode_endpoint *ep)
+{
+	int i, j;
+	const s64 *freqs = imx290_link_freqs_ptr(imx290);
+	int freqs_count = imx290_link_freqs_num(imx290);
+
+	for (i = 0; i < freqs_count; i++) {
+		for (j = 0; j < ep->nr_of_link_frequencies; j++)
+			if (freqs[i] == ep->link_frequencies[j])
+				break;
+		if (j == ep->nr_of_link_frequencies)
+			return freqs[i];
+	}
+	return 0;
+}
+
+static const struct imx290_model_info imx290_models[] = {
+	[IMX290_MODEL_IMX290LQR] = {
+		.colour_variant = IMX290_VARIANT_COLOUR,
+		.init_regs = imx290_global_init_settings_290,
+		.init_regs_num = ARRAY_SIZE(imx290_global_init_settings_290),
+		.name = "imx290",
+	},
+	[IMX290_MODEL_IMX290LLR] = {
+		.colour_variant = IMX290_VARIANT_MONO,
+		.init_regs = imx290_global_init_settings_290,
+		.init_regs_num = ARRAY_SIZE(imx290_global_init_settings_290),
+		.name = "imx290",
+	},
+	[IMX290_MODEL_IMX327LQR] = {
+		.colour_variant = IMX290_VARIANT_COLOUR,
+		.init_regs = imx290_global_init_settings_327,
+		.init_regs_num = ARRAY_SIZE(imx290_global_init_settings_327),
+		.name = "imx327",
+	},
+};
+
+static int imx290_parse_dt(struct imx290 *imx290)
+{
+	/* Only CSI2 is supported for now: */
+	struct v4l2_fwnode_endpoint ep = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY
+	};
+	struct fwnode_handle *endpoint;
+	int ret;
+	s64 fq;
+
+	imx290->model = of_device_get_match_data(imx290->dev);
+
+	endpoint = fwnode_graph_get_next_endpoint(dev_fwnode(imx290->dev), NULL);
+	if (!endpoint) {
+		dev_err(imx290->dev, "Endpoint node not found\n");
+		return -EINVAL;
+	}
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(endpoint, &ep);
+	fwnode_handle_put(endpoint);
+	if (ret == -ENXIO) {
+		dev_err(imx290->dev, "Unsupported bus type, should be CSI2\n");
+		goto done;
+	} else if (ret) {
+		dev_err(imx290->dev, "Parsing endpoint node failed\n");
+		goto done;
+	}
+
+	/* Get number of data lanes */
+	imx290->nlanes = ep.bus.mipi_csi2.num_data_lanes;
+	if (imx290->nlanes != 2 && imx290->nlanes != 4) {
+		dev_err(imx290->dev, "Invalid data lanes: %d\n", imx290->nlanes);
+		ret = -EINVAL;
+		goto done;
+	}
+
+	dev_dbg(imx290->dev, "Using %u data lanes\n", imx290->nlanes);
+
+	if (!ep.nr_of_link_frequencies) {
+		dev_err(imx290->dev, "link-frequency property not found in DT\n");
+		ret = -EINVAL;
+		goto done;
+	}
+
+	/* Check that link frequences for all the modes are in device tree */
+	fq = imx290_check_link_freqs(imx290, &ep);
+	if (fq) {
+		dev_err(imx290->dev, "Link frequency of %lld is not supported\n",
+			fq);
+		ret = -EINVAL;
+		goto done;
+	}
+
+	ret = 0;
+
+done:
+	v4l2_fwnode_endpoint_free(&ep);
+	return ret;
+}
+
+static int imx290_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct imx290 *imx290;
+	int ret;
+
+	imx290 = devm_kzalloc(dev, sizeof(*imx290), GFP_KERNEL);
+	if (!imx290)
+		return -ENOMEM;
+
+	imx290->dev = dev;
+	imx290->regmap = devm_cci_regmap_init_i2c(client, 16);
+	if (IS_ERR(imx290->regmap)) {
+		dev_err(dev, "Unable to initialize I2C\n");
+		return -ENODEV;
+	}
+
+	ret = imx290_parse_dt(imx290);
+	if (ret)
+		return ret;
+
+	/* Acquire resources. */
+	imx290->xclk = devm_clk_get(dev, "xclk");
+	if (IS_ERR(imx290->xclk))
+		return dev_err_probe(dev, PTR_ERR(imx290->xclk),
+				     "Could not get xclk\n");
+
+	ret = imx290_get_regulators(dev, imx290);
+	if (ret < 0)
+		return dev_err_probe(dev, ret, "Cannot get regulators\n");
+
+	imx290->rst_gpio = devm_gpiod_get_optional(dev, "reset",
+						   GPIOD_OUT_HIGH);
+	if (IS_ERR(imx290->rst_gpio))
+		return dev_err_probe(dev, PTR_ERR(imx290->rst_gpio),
+				     "Cannot get reset gpio\n");
+
+	/* Initialize external clock frequency. */
+	ret = imx290_init_clk(imx290);
+	if (ret)
+		return ret;
+
+	/*
+	 * Enable power management. The driver supports runtime PM, but needs to
+	 * work when runtime PM is disabled in the kernel. To that end, power
+	 * the sensor on manually here.
+	 */
+	ret = imx290_power_on(imx290);
+	if (ret < 0) {
+		dev_err(dev, "Could not power on the device\n");
+		return ret;
+	}
+
+	/*
+	 * Enable runtime PM with autosuspend. As the device has been powered
+	 * manually, mark it as active, and increase the usage count without
+	 * resuming the device.
+	 */
+	pm_runtime_set_active(dev);
+	pm_runtime_get_noresume(dev);
+	pm_runtime_enable(dev);
+	pm_runtime_set_autosuspend_delay(dev, 1000);
+	pm_runtime_use_autosuspend(dev);
+
+	/* Initialize the V4L2 subdev. */
+	ret = imx290_subdev_init(imx290);
+	if (ret)
+		goto err_pm;
+
+	v4l2_i2c_subdev_set_name(&imx290->sd, client,
+				 imx290->model->name, NULL);
+
+	/*
+	 * Finally, register the V4L2 subdev. This must be done after
+	 * initializing everything as the subdev can be used immediately after
+	 * being registered.
+	 */
+	ret = v4l2_async_register_subdev(&imx290->sd);
+	if (ret < 0) {
+		dev_err(dev, "Could not register v4l2 device\n");
+		goto err_subdev;
+	}
+
+	/*
+	 * Decrease the PM usage count. The device will get suspended after the
+	 * autosuspend delay, turning the power off.
+	 */
+	pm_runtime_mark_last_busy(dev);
+	pm_runtime_put_autosuspend(dev);
+
+	return 0;
+
+err_subdev:
+	imx290_subdev_cleanup(imx290);
+err_pm:
+	pm_runtime_disable(dev);
+	pm_runtime_put_noidle(dev);
+	imx290_power_off(imx290);
+	return ret;
+}
+
+static void imx290_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct imx290 *imx290 = to_imx290(sd);
+
+	v4l2_async_unregister_subdev(sd);
+	imx290_subdev_cleanup(imx290);
+
+	/*
+	 * Disable runtime PM. In case runtime PM is disabled in the kernel,
+	 * make sure to turn power off manually.
+	 */
+	pm_runtime_disable(imx290->dev);
+	if (!pm_runtime_status_suspended(imx290->dev))
+		imx290_power_off(imx290);
+	pm_runtime_set_suspended(imx290->dev);
+}
+
+static const struct of_device_id imx290_of_match[] = {
+	{
+		/* Deprecated - synonym for "sony,imx290lqr" */
+		.compatible = "sony,imx290",
+		.data = &imx290_models[IMX290_MODEL_IMX290LQR],
+	}, {
+		.compatible = "sony,imx290lqr",
+		.data = &imx290_models[IMX290_MODEL_IMX290LQR],
+	}, {
+		.compatible = "sony,imx290llr",
+		.data = &imx290_models[IMX290_MODEL_IMX290LLR],
+	}, {
+		.compatible = "sony,imx327lqr",
+		.data = &imx290_models[IMX290_MODEL_IMX327LQR],
+	},
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, imx290_of_match);
+
+static struct i2c_driver imx290_i2c_driver = {
+	.probe = imx290_probe,
+	.remove = imx290_remove,
+	.driver = {
+		.name = "imx290",
+		.pm = pm_ptr(&imx290_pm_ops),
+		.of_match_table = imx290_of_match,
+	},
+};
+
+module_i2c_driver(imx290_i2c_driver);
+
+MODULE_DESCRIPTION("Sony IMX290 CMOS Image Sensor Driver");
+MODULE_AUTHOR("FRAMOS GmbH");
+MODULE_AUTHOR("Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/imx296.c b/drivers/media/i2c/imx296.c
new file mode 100644
index 000000000..3b4539b62
--- /dev/null
+++ b/drivers/media/i2c/imx296.c
@@ -0,0 +1,1165 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Driver for IMX296 CMOS Image Sensor from Sony
+ *
+ * Copyright 2019 Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ */
+
+#include <linux/clk.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+#define IMX296_PIXEL_ARRAY_WIDTH			1456
+#define IMX296_PIXEL_ARRAY_HEIGHT			1088
+
+#define IMX296_REG_8BIT(n)				((1 << 16) | (n))
+#define IMX296_REG_16BIT(n)				((2 << 16) | (n))
+#define IMX296_REG_24BIT(n)				((3 << 16) | (n))
+#define IMX296_REG_SIZE_SHIFT				16
+#define IMX296_REG_ADDR_MASK				0xffff
+
+#define IMX296_CTRL00					IMX296_REG_8BIT(0x3000)
+#define IMX296_CTRL00_STANDBY				BIT(0)
+#define IMX296_CTRL08					IMX296_REG_8BIT(0x3008)
+#define IMX296_CTRL08_REGHOLD				BIT(0)
+#define IMX296_CTRL0A					IMX296_REG_8BIT(0x300a)
+#define IMX296_CTRL0A_XMSTA				BIT(0)
+#define IMX296_CTRL0B					IMX296_REG_8BIT(0x300b)
+#define IMX296_CTRL0B_TRIGEN				BIT(0)
+#define IMX296_CTRL0D					IMX296_REG_8BIT(0x300d)
+#define IMX296_CTRL0D_WINMODE_ALL			(0 << 0)
+#define IMX296_CTRL0D_WINMODE_FD_BINNING		(2 << 0)
+#define IMX296_CTRL0D_HADD_ON_BINNING			BIT(5)
+#define IMX296_CTRL0D_SAT_CNT				BIT(6)
+#define IMX296_CTRL0E					IMX296_REG_8BIT(0x300e)
+#define IMX296_CTRL0E_VREVERSE				BIT(0)
+#define IMX296_CTRL0E_HREVERSE				BIT(1)
+#define IMX296_VMAX					IMX296_REG_24BIT(0x3010)
+#define IMX296_HMAX					IMX296_REG_16BIT(0x3014)
+#define IMX296_TMDCTRL					IMX296_REG_8BIT(0x301d)
+#define IMX296_TMDCTRL_LATCH				BIT(0)
+#define IMX296_TMDOUT					IMX296_REG_16BIT(0x301e)
+#define IMX296_TMDOUT_MASK				0x3ff
+#define IMX296_WDSEL					IMX296_REG_8BIT(0x3021)
+#define IMX296_WDSEL_NORMAL				(0 << 0)
+#define IMX296_WDSEL_MULTI_2				(1 << 0)
+#define IMX296_WDSEL_MULTI_4				(3 << 0)
+#define IMX296_BLKLEVELAUTO				IMX296_REG_8BIT(0x3022)
+#define IMX296_BLKLEVELAUTO_ON				0x01
+#define IMX296_BLKLEVELAUTO_OFF				0xf0
+#define IMX296_SST					IMX296_REG_8BIT(0x3024)
+#define IMX296_SST_EN					BIT(0)
+#define IMX296_CTRLTOUT					IMX296_REG_8BIT(0x3026)
+#define IMX296_CTRLTOUT_TOUT1SEL_LOW			(0 << 0)
+#define IMX296_CTRLTOUT_TOUT1SEL_PULSE			(3 << 0)
+#define IMX296_CTRLTOUT_TOUT2SEL_LOW			(0 << 2)
+#define IMX296_CTRLTOUT_TOUT2SEL_PULSE			(3 << 2)
+#define IMX296_CTRLTRIG					IMX296_REG_8BIT(0x3029)
+#define IMX296_CTRLTRIG_TOUT1_SEL_LOW			(0 << 0)
+#define IMX296_CTRLTRIG_TOUT1_SEL_PULSE1		(1 << 0)
+#define IMX296_CTRLTRIG_TOUT2_SEL_LOW			(0 << 4)
+#define IMX296_CTRLTRIG_TOUT2_SEL_PULSE2		(2 << 4)
+#define IMX296_SYNCSEL					IMX296_REG_8BIT(0x3036)
+#define IMX296_SYNCSEL_NORMAL				0xc0
+#define IMX296_SYNCSEL_HIZ				0xf0
+#define IMX296_PULSE1					IMX296_REG_8BIT(0x306d)
+#define IMX296_PULSE1_EN_NOR				BIT(0)
+#define IMX296_PULSE1_EN_TRIG				BIT(1)
+#define IMX296_PULSE1_POL_HIGH				(0 << 2)
+#define IMX296_PULSE1_POL_LOW				(1 << 2)
+#define IMX296_PULSE1_UP				IMX296_REG_24BIT(0x3070)
+#define IMX296_PULSE1_DN				IMX296_REG_24BIT(0x3074)
+#define IMX296_PULSE2					IMX296_REG_8BIT(0x3079)
+#define IMX296_PULSE2_EN_NOR				BIT(0)
+#define IMX296_PULSE2_EN_TRIG				BIT(1)
+#define IMX296_PULSE2_POL_HIGH				(0 << 2)
+#define IMX296_PULSE2_POL_LOW				(1 << 2)
+#define IMX296_PULSE2_UP				IMX296_REG_24BIT(0x307c)
+#define IMX296_PULSE2_DN				IMX296_REG_24BIT(0x3080)
+#define IMX296_INCKSEL(n)				IMX296_REG_8BIT(0x3089 + (n))
+#define IMX296_SHS1					IMX296_REG_24BIT(0x308d)
+#define IMX296_SHS2					IMX296_REG_24BIT(0x3090)
+#define IMX296_SHS3					IMX296_REG_24BIT(0x3094)
+#define IMX296_SHS4					IMX296_REG_24BIT(0x3098)
+#define IMX296_VBLANKLP					IMX296_REG_8BIT(0x309c)
+#define IMX296_VBLANKLP_NORMAL				0x04
+#define IMX296_VBLANKLP_LOW_POWER			0x2c
+#define IMX296_EXP_CNT					IMX296_REG_8BIT(0x30a3)
+#define IMX296_EXP_CNT_RESET				BIT(0)
+#define IMX296_EXP_MAX					IMX296_REG_16BIT(0x30a6)
+#define IMX296_VINT					IMX296_REG_8BIT(0x30aa)
+#define IMX296_VINT_EN					BIT(0)
+#define IMX296_LOWLAGTRG				IMX296_REG_8BIT(0x30ae)
+#define IMX296_LOWLAGTRG_FAST				BIT(0)
+#define IMX296_I2CCTRL					IMX296_REG_8BIT(0x30ef)
+#define IMX296_I2CCTRL_I2CACKEN				BIT(0)
+
+#define IMX296_SENSOR_INFO				IMX296_REG_16BIT(0x3148)
+#define IMX296_SENSOR_INFO_MONO				BIT(15)
+#define IMX296_SENSOR_INFO_IMX296LQ			0x4a00
+#define IMX296_SENSOR_INFO_IMX296LL			0xca00
+#define IMX296_S_SHSA					IMX296_REG_16BIT(0x31ca)
+#define IMX296_S_SHSB					IMX296_REG_16BIT(0x31d2)
+/*
+ * Registers 0x31c8 to 0x31cd, 0x31d0 to 0x31d5, 0x31e2, 0x31e3, 0x31ea and
+ * 0x31eb are related to exposure mode but otherwise not documented.
+ */
+
+#define IMX296_GAINCTRL					IMX296_REG_8BIT(0x3200)
+#define IMX296_GAINCTRL_WD_GAIN_MODE_NORMAL		0x01
+#define IMX296_GAINCTRL_WD_GAIN_MODE_MULTI		0x41
+#define IMX296_GAIN					IMX296_REG_16BIT(0x3204)
+#define IMX296_GAIN_MIN					0
+#define IMX296_GAIN_MAX					480
+#define IMX296_GAIN1					IMX296_REG_16BIT(0x3208)
+#define IMX296_GAIN2					IMX296_REG_16BIT(0x320c)
+#define IMX296_GAIN3					IMX296_REG_16BIT(0x3210)
+#define IMX296_GAINDLY					IMX296_REG_8BIT(0x3212)
+#define IMX296_GAINDLY_NONE				0x08
+#define IMX296_GAINDLY_1FRAME				0x09
+#define IMX296_PGCTRL					IMX296_REG_8BIT(0x3238)
+#define IMX296_PGCTRL_REGEN				BIT(0)
+#define IMX296_PGCTRL_THRU				BIT(1)
+#define IMX296_PGCTRL_CLKEN				BIT(2)
+#define IMX296_PGCTRL_MODE(n)				((n) << 3)
+#define IMX296_PGHPOS					IMX296_REG_16BIT(0x3239)
+#define IMX296_PGVPOS					IMX296_REG_16BIT(0x323c)
+#define IMX296_PGHPSTEP					IMX296_REG_8BIT(0x323e)
+#define IMX296_PGVPSTEP					IMX296_REG_8BIT(0x323f)
+#define IMX296_PGHPNUM					IMX296_REG_8BIT(0x3240)
+#define IMX296_PGVPNUM					IMX296_REG_8BIT(0x3241)
+#define IMX296_PGDATA1					IMX296_REG_16BIT(0x3244)
+#define IMX296_PGDATA2					IMX296_REG_16BIT(0x3246)
+#define IMX296_PGHGSTEP					IMX296_REG_8BIT(0x3249)
+#define IMX296_BLKLEVEL					IMX296_REG_16BIT(0x3254)
+
+#define IMX296_FID0_ROI					IMX296_REG_8BIT(0x3300)
+#define IMX296_FID0_ROIH1ON				BIT(0)
+#define IMX296_FID0_ROIV1ON				BIT(1)
+#define IMX296_FID0_ROIPH1				IMX296_REG_16BIT(0x3310)
+#define IMX296_FID0_ROIPV1				IMX296_REG_16BIT(0x3312)
+#define IMX296_FID0_ROIWH1				IMX296_REG_16BIT(0x3314)
+#define IMX296_FID0_ROIWH1_MIN				80
+#define IMX296_FID0_ROIWV1				IMX296_REG_16BIT(0x3316)
+#define IMX296_FID0_ROIWV1_MIN				4
+
+#define IMX296_CM_HSST_STARTTMG				IMX296_REG_16BIT(0x4018)
+#define IMX296_CM_HSST_ENDTMG				IMX296_REG_16BIT(0x401a)
+#define IMX296_DA_HSST_STARTTMG				IMX296_REG_16BIT(0x404d)
+#define IMX296_DA_HSST_ENDTMG				IMX296_REG_16BIT(0x4050)
+#define IMX296_LM_HSST_STARTTMG				IMX296_REG_16BIT(0x4094)
+#define IMX296_LM_HSST_ENDTMG				IMX296_REG_16BIT(0x4096)
+#define IMX296_SST_SIEASTA1_SET				IMX296_REG_8BIT(0x40c9)
+#define IMX296_SST_SIEASTA1PRE_1U			IMX296_REG_16BIT(0x40cc)
+#define IMX296_SST_SIEASTA1PRE_1D			IMX296_REG_16BIT(0x40ce)
+#define IMX296_SST_SIEASTA1PRE_2U			IMX296_REG_16BIT(0x40d0)
+#define IMX296_SST_SIEASTA1PRE_2D			IMX296_REG_16BIT(0x40d2)
+#define IMX296_HSST					IMX296_REG_8BIT(0x40dc)
+#define IMX296_HSST_EN					BIT(2)
+
+#define IMX296_CKREQSEL					IMX296_REG_8BIT(0x4101)
+#define IMX296_CKREQSEL_HS				BIT(2)
+#define IMX296_GTTABLENUM				IMX296_REG_8BIT(0x4114)
+#define IMX296_CTRL418C					IMX296_REG_8BIT(0x418c)
+
+struct imx296_clk_params {
+	unsigned int freq;
+	u8 incksel[4];
+	u8 ctrl418c;
+};
+
+static const struct imx296_clk_params imx296_clk_params[] = {
+	{ 37125000, { 0x80, 0x0b, 0x80, 0x08 }, 116 },
+	{ 54000000, { 0xb0, 0x0f, 0xb0, 0x0c }, 168 },
+	{ 74250000, { 0x80, 0x0f, 0x80, 0x0c }, 232 },
+};
+
+static const char * const imx296_supply_names[] = {
+	"dvdd",
+	"ovdd",
+	"avdd",
+};
+
+struct imx296 {
+	struct device *dev;
+	struct clk *clk;
+	struct regulator_bulk_data supplies[ARRAY_SIZE(imx296_supply_names)];
+	struct gpio_desc *reset;
+	struct regmap *regmap;
+
+	const struct imx296_clk_params *clk_params;
+	bool mono;
+
+	bool streaming;
+
+	struct v4l2_subdev subdev;
+	struct media_pad pad;
+
+	struct v4l2_ctrl_handler ctrls;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *vblank;
+};
+
+static inline struct imx296 *to_imx296(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct imx296, subdev);
+}
+
+static int imx296_read(struct imx296 *sensor, u32 addr)
+{
+	u8 data[3] = { 0, 0, 0 };
+	int ret;
+
+	ret = regmap_raw_read(sensor->regmap, addr & IMX296_REG_ADDR_MASK, data,
+			      (addr >> IMX296_REG_SIZE_SHIFT) & 3);
+	if (ret < 0)
+		return ret;
+
+	return (data[2] << 16) | (data[1] << 8) | data[0];
+}
+
+static int imx296_write(struct imx296 *sensor, u32 addr, u32 value, int *err)
+{
+	u8 data[3] = { value & 0xff, (value >> 8) & 0xff, value >> 16 };
+	int ret;
+
+	if (err && *err)
+		return *err;
+
+	ret = regmap_raw_write(sensor->regmap, addr & IMX296_REG_ADDR_MASK,
+			       data, (addr >> IMX296_REG_SIZE_SHIFT) & 3);
+	if (ret < 0) {
+		dev_err(sensor->dev, "%u-bit write to 0x%04x failed: %d\n",
+			((addr >> IMX296_REG_SIZE_SHIFT) & 3) * 8,
+			addr & IMX296_REG_ADDR_MASK, ret);
+		if (err)
+			*err = ret;
+	}
+
+	return ret;
+}
+
+static int imx296_power_on(struct imx296 *sensor)
+{
+	int ret;
+
+	ret = regulator_bulk_enable(ARRAY_SIZE(sensor->supplies),
+				    sensor->supplies);
+	if (ret < 0)
+		return ret;
+
+	udelay(1);
+
+	ret = gpiod_direction_output(sensor->reset, 0);
+	if (ret < 0)
+		goto err_supply;
+
+	udelay(1);
+
+	ret = clk_prepare_enable(sensor->clk);
+	if (ret < 0)
+		goto err_reset;
+
+	/*
+	 * The documentation doesn't explicitly say how much time is required
+	 * after providing a clock and before starting I2C communication. It
+	 * mentions a delay of 20µs in 4-wire mode, but tests showed that a
+	 * delay of 100µs resulted in I2C communication failures, while 500µs
+	 * seems to be enough. Be conservative.
+	 */
+	usleep_range(1000, 2000);
+
+	return 0;
+
+err_reset:
+	gpiod_direction_output(sensor->reset, 1);
+err_supply:
+	regulator_bulk_disable(ARRAY_SIZE(sensor->supplies), sensor->supplies);
+	return ret;
+}
+
+static void imx296_power_off(struct imx296 *sensor)
+{
+	clk_disable_unprepare(sensor->clk);
+	gpiod_direction_output(sensor->reset, 1);
+	regulator_bulk_disable(ARRAY_SIZE(sensor->supplies), sensor->supplies);
+}
+
+/* -----------------------------------------------------------------------------
+ * Controls
+ */
+
+static const char * const imx296_test_pattern_menu[] = {
+	"Disabled",
+	"Multiple Pixels",
+	"Sequence 1",
+	"Sequence 2",
+	"Gradient",
+	"Row",
+	"Column",
+	"Cross",
+	"Stripe",
+	"Checks",
+};
+
+static int imx296_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct imx296 *sensor = container_of(ctrl->handler, struct imx296, ctrls);
+	const struct v4l2_mbus_framefmt *format;
+	struct v4l2_subdev_state *state;
+	unsigned int vmax;
+	int ret = 0;
+
+	if (!sensor->streaming)
+		return 0;
+
+	state = v4l2_subdev_get_locked_active_state(&sensor->subdev);
+	format = v4l2_subdev_get_pad_format(&sensor->subdev, state, 0);
+
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE:
+		/* Clamp the exposure value to VMAX. */
+		vmax = format->height + sensor->vblank->cur.val;
+		ctrl->val = min_t(int, ctrl->val, vmax);
+		imx296_write(sensor, IMX296_SHS1, vmax - ctrl->val, &ret);
+		break;
+
+	case V4L2_CID_ANALOGUE_GAIN:
+		imx296_write(sensor, IMX296_GAIN, ctrl->val, &ret);
+		break;
+
+	case V4L2_CID_VBLANK:
+		imx296_write(sensor, IMX296_VMAX, format->height + ctrl->val,
+			     &ret);
+		break;
+
+	case V4L2_CID_TEST_PATTERN:
+		if (ctrl->val) {
+			imx296_write(sensor, IMX296_PGHPOS, 8, &ret);
+			imx296_write(sensor, IMX296_PGVPOS, 8, &ret);
+			imx296_write(sensor, IMX296_PGHPSTEP, 8, &ret);
+			imx296_write(sensor, IMX296_PGVPSTEP, 8, &ret);
+			imx296_write(sensor, IMX296_PGHPNUM, 100, &ret);
+			imx296_write(sensor, IMX296_PGVPNUM, 100, &ret);
+			imx296_write(sensor, IMX296_PGDATA1, 0x300, &ret);
+			imx296_write(sensor, IMX296_PGDATA2, 0x100, &ret);
+			imx296_write(sensor, IMX296_PGHGSTEP, 0, &ret);
+			imx296_write(sensor, IMX296_BLKLEVEL, 0, &ret);
+			imx296_write(sensor, IMX296_BLKLEVELAUTO,
+				     IMX296_BLKLEVELAUTO_OFF, &ret);
+			imx296_write(sensor, IMX296_PGCTRL,
+				     IMX296_PGCTRL_REGEN |
+				     IMX296_PGCTRL_CLKEN |
+				     IMX296_PGCTRL_MODE(ctrl->val - 1), &ret);
+		} else {
+			imx296_write(sensor, IMX296_PGCTRL,
+				     IMX296_PGCTRL_CLKEN, &ret);
+			imx296_write(sensor, IMX296_BLKLEVEL, 0x3c, &ret);
+			imx296_write(sensor, IMX296_BLKLEVELAUTO,
+				     IMX296_BLKLEVELAUTO_ON, &ret);
+		}
+		break;
+
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops imx296_ctrl_ops = {
+	.s_ctrl = imx296_s_ctrl,
+};
+
+static int imx296_ctrls_init(struct imx296 *sensor)
+{
+	struct v4l2_fwnode_device_properties props;
+	unsigned int hblank;
+	int ret;
+
+	ret = v4l2_fwnode_device_parse(sensor->dev, &props);
+	if (ret < 0)
+		return ret;
+
+	v4l2_ctrl_handler_init(&sensor->ctrls, 9);
+
+	v4l2_ctrl_new_std(&sensor->ctrls, &imx296_ctrl_ops,
+			  V4L2_CID_EXPOSURE, 1, 1048575, 1, 1104);
+	v4l2_ctrl_new_std(&sensor->ctrls, &imx296_ctrl_ops,
+			  V4L2_CID_ANALOGUE_GAIN, IMX296_GAIN_MIN,
+			  IMX296_GAIN_MAX, 1, IMX296_GAIN_MIN);
+
+	/*
+	 * Horizontal blanking is controlled through the HMAX register, which
+	 * contains a line length in INCK clock units. The INCK frequency is
+	 * fixed to 74.25 MHz. The HMAX value is currently fixed to 1100,
+	 * convert it to a number of pixels based on the nominal pixel rate.
+	 */
+	hblank = 1100 * 1188000000ULL / 10 / 74250000
+	       - IMX296_PIXEL_ARRAY_WIDTH;
+	sensor->hblank = v4l2_ctrl_new_std(&sensor->ctrls, &imx296_ctrl_ops,
+					   V4L2_CID_HBLANK, hblank, hblank, 1,
+					   hblank);
+	if (sensor->hblank)
+		sensor->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	sensor->vblank = v4l2_ctrl_new_std(&sensor->ctrls, &imx296_ctrl_ops,
+					   V4L2_CID_VBLANK, 30,
+					   1048575 - IMX296_PIXEL_ARRAY_HEIGHT,
+					   1, 30);
+	/*
+	 * The sensor calculates the MIPI timings internally to achieve a bit
+	 * rate between 1122 and 1198 Mbps. The exact value is unfortunately not
+	 * reported, at least according to the documentation. Report a nominal
+	 * rate of 1188 Mbps as that is used by the datasheet in multiple
+	 * examples.
+	 */
+	v4l2_ctrl_new_std(&sensor->ctrls, NULL, V4L2_CID_PIXEL_RATE,
+			  1122000000 / 10, 1198000000 / 10, 1, 1188000000 / 10);
+	v4l2_ctrl_new_std_menu_items(&sensor->ctrls, &imx296_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(imx296_test_pattern_menu) - 1,
+				     0, 0, imx296_test_pattern_menu);
+
+	v4l2_ctrl_new_fwnode_properties(&sensor->ctrls, &imx296_ctrl_ops,
+					&props);
+
+	if (sensor->ctrls.error) {
+		dev_err(sensor->dev, "failed to add controls (%d)\n",
+			sensor->ctrls.error);
+		v4l2_ctrl_handler_free(&sensor->ctrls);
+		return sensor->ctrls.error;
+	}
+
+	sensor->subdev.ctrl_handler = &sensor->ctrls;
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdev Operations
+ */
+
+/*
+ * This table is extracted from vendor data that is entirely undocumented. The
+ * first register write is required to activate the CSI-2 output. The other
+ * entries may or may not be optional?
+ */
+static const struct {
+	unsigned int reg;
+	unsigned int value;
+} imx296_init_table[] = {
+	{ IMX296_REG_8BIT(0x3005), 0xf0 },
+	{ IMX296_REG_8BIT(0x309e), 0x04 },
+	{ IMX296_REG_8BIT(0x30a0), 0x04 },
+	{ IMX296_REG_8BIT(0x30a1), 0x3c },
+	{ IMX296_REG_8BIT(0x30a4), 0x5f },
+	{ IMX296_REG_8BIT(0x30a8), 0x91 },
+	{ IMX296_REG_8BIT(0x30ac), 0x28 },
+	{ IMX296_REG_8BIT(0x30af), 0x09 },
+	{ IMX296_REG_8BIT(0x30df), 0x00 },
+	{ IMX296_REG_8BIT(0x3165), 0x00 },
+	{ IMX296_REG_8BIT(0x3169), 0x10 },
+	{ IMX296_REG_8BIT(0x316a), 0x02 },
+	{ IMX296_REG_8BIT(0x31c8), 0xf3 },	/* Exposure-related */
+	{ IMX296_REG_8BIT(0x31d0), 0xf4 },	/* Exposure-related */
+	{ IMX296_REG_8BIT(0x321a), 0x00 },
+	{ IMX296_REG_8BIT(0x3226), 0x02 },
+	{ IMX296_REG_8BIT(0x3256), 0x01 },
+	{ IMX296_REG_8BIT(0x3541), 0x72 },
+	{ IMX296_REG_8BIT(0x3516), 0x77 },
+	{ IMX296_REG_8BIT(0x350b), 0x7f },
+	{ IMX296_REG_8BIT(0x3758), 0xa3 },
+	{ IMX296_REG_8BIT(0x3759), 0x00 },
+	{ IMX296_REG_8BIT(0x375a), 0x85 },
+	{ IMX296_REG_8BIT(0x375b), 0x00 },
+	{ IMX296_REG_8BIT(0x3832), 0xf5 },
+	{ IMX296_REG_8BIT(0x3833), 0x00 },
+	{ IMX296_REG_8BIT(0x38a2), 0xf6 },
+	{ IMX296_REG_8BIT(0x38a3), 0x00 },
+	{ IMX296_REG_8BIT(0x3a00), 0x80 },
+	{ IMX296_REG_8BIT(0x3d48), 0xa3 },
+	{ IMX296_REG_8BIT(0x3d49), 0x00 },
+	{ IMX296_REG_8BIT(0x3d4a), 0x85 },
+	{ IMX296_REG_8BIT(0x3d4b), 0x00 },
+	{ IMX296_REG_8BIT(0x400e), 0x58 },
+	{ IMX296_REG_8BIT(0x4014), 0x1c },
+	{ IMX296_REG_8BIT(0x4041), 0x2a },
+	{ IMX296_REG_8BIT(0x40a2), 0x06 },
+	{ IMX296_REG_8BIT(0x40c1), 0xf6 },
+	{ IMX296_REG_8BIT(0x40c7), 0x0f },
+	{ IMX296_REG_8BIT(0x40c8), 0x00 },
+	{ IMX296_REG_8BIT(0x4174), 0x00 },
+};
+
+static int imx296_setup(struct imx296 *sensor, struct v4l2_subdev_state *state)
+{
+	const struct v4l2_mbus_framefmt *format;
+	const struct v4l2_rect *crop;
+	unsigned int i;
+	int ret = 0;
+
+	format = v4l2_subdev_get_pad_format(&sensor->subdev, state, 0);
+	crop = v4l2_subdev_get_pad_crop(&sensor->subdev, state, 0);
+
+	for (i = 0; i < ARRAY_SIZE(imx296_init_table); ++i)
+		imx296_write(sensor, imx296_init_table[i].reg,
+			     imx296_init_table[i].value, &ret);
+
+	if (crop->width != IMX296_PIXEL_ARRAY_WIDTH ||
+	    crop->height != IMX296_PIXEL_ARRAY_HEIGHT) {
+		imx296_write(sensor, IMX296_FID0_ROI,
+			     IMX296_FID0_ROIH1ON | IMX296_FID0_ROIV1ON, &ret);
+		imx296_write(sensor, IMX296_FID0_ROIPH1, crop->left, &ret);
+		imx296_write(sensor, IMX296_FID0_ROIPV1, crop->top, &ret);
+		imx296_write(sensor, IMX296_FID0_ROIWH1, crop->width, &ret);
+		imx296_write(sensor, IMX296_FID0_ROIWV1, crop->height, &ret);
+	} else {
+		imx296_write(sensor, IMX296_FID0_ROI, 0, &ret);
+	}
+
+	imx296_write(sensor, IMX296_CTRL0D,
+		     (crop->width != format->width ?
+		      IMX296_CTRL0D_HADD_ON_BINNING : 0) |
+		     (crop->height != format->height ?
+		      IMX296_CTRL0D_WINMODE_FD_BINNING : 0),
+		     &ret);
+
+	/*
+	 * HMAX and VMAX configure horizontal and vertical blanking by
+	 * specifying the total line time and frame time respectively. The line
+	 * time is specified in operational clock units (which appears to be the
+	 * output of an internal PLL, fixed at 74.25 MHz regardless of the
+	 * exernal clock frequency), while the frame time is specified as a
+	 * number of lines.
+	 *
+	 * In the vertical direction the sensor outputs the following:
+	 *
+	 * - one line for the FS packet
+	 * - two lines of embedded data (DT 0x12)
+	 * - six null lines (DT 0x10)
+	 * - four lines of vertical effective optical black (DT 0x37)
+	 * - 8 to 1088 lines of active image data (RAW10, DT 0x2b)
+	 * - one line for the FE packet
+	 * - 16 or more lines of vertical blanking
+	 */
+	imx296_write(sensor, IMX296_HMAX, 1100, &ret);
+	imx296_write(sensor, IMX296_VMAX,
+		     format->height + sensor->vblank->cur.val, &ret);
+
+	for (i = 0; i < ARRAY_SIZE(sensor->clk_params->incksel); ++i)
+		imx296_write(sensor, IMX296_INCKSEL(i),
+			     sensor->clk_params->incksel[i], &ret);
+	imx296_write(sensor, IMX296_GTTABLENUM, 0xc5, &ret);
+	imx296_write(sensor, IMX296_CTRL418C, sensor->clk_params->ctrl418c,
+		     &ret);
+
+	imx296_write(sensor, IMX296_GAINDLY, IMX296_GAINDLY_NONE, &ret);
+	imx296_write(sensor, IMX296_BLKLEVEL, 0x03c, &ret);
+
+	return ret;
+}
+
+static int imx296_stream_on(struct imx296 *sensor)
+{
+	int ret = 0;
+
+	imx296_write(sensor, IMX296_CTRL00, 0, &ret);
+	usleep_range(2000, 5000);
+	imx296_write(sensor, IMX296_CTRL0A, 0, &ret);
+
+	return ret;
+}
+
+static int imx296_stream_off(struct imx296 *sensor)
+{
+	int ret = 0;
+
+	imx296_write(sensor, IMX296_CTRL0A, IMX296_CTRL0A_XMSTA, &ret);
+	imx296_write(sensor, IMX296_CTRL00, IMX296_CTRL00_STANDBY, &ret);
+
+	return ret;
+}
+
+static int imx296_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct imx296 *sensor = to_imx296(sd);
+	struct v4l2_subdev_state *state;
+	int ret;
+
+	state = v4l2_subdev_lock_and_get_active_state(sd);
+
+	if (!enable) {
+		ret = imx296_stream_off(sensor);
+
+		pm_runtime_mark_last_busy(sensor->dev);
+		pm_runtime_put_autosuspend(sensor->dev);
+
+		sensor->streaming = false;
+
+		goto unlock;
+	}
+
+	ret = pm_runtime_resume_and_get(sensor->dev);
+	if (ret < 0)
+		goto unlock;
+
+	ret = imx296_setup(sensor, state);
+	if (ret < 0)
+		goto err_pm;
+
+	/*
+	 * Set streaming to true to ensure __v4l2_ctrl_handler_setup() will set
+	 * the controls. The flag is reset to false further down if an error
+	 * occurs.
+	 */
+	sensor->streaming = true;
+
+	ret = __v4l2_ctrl_handler_setup(&sensor->ctrls);
+	if (ret < 0)
+		goto err_pm;
+
+	ret = imx296_stream_on(sensor);
+	if (ret)
+		goto err_pm;
+
+unlock:
+	v4l2_subdev_unlock_state(state);
+
+	return ret;
+
+err_pm:
+	/*
+	 * In case of error, turn the power off synchronously as the device
+	 * likely has no other chance to recover.
+	 */
+	pm_runtime_put_sync(sensor->dev);
+	sensor->streaming = false;
+
+	goto unlock;
+}
+
+static int imx296_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct imx296 *sensor = to_imx296(sd);
+
+	if (code->index != 0)
+		return -EINVAL;
+
+	code->code = sensor->mono ? MEDIA_BUS_FMT_Y10_1X10
+		   : MEDIA_BUS_FMT_SBGGR10_1X10;
+
+	return 0;
+}
+
+static int imx296_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	const struct v4l2_mbus_framefmt *format;
+
+	format = v4l2_subdev_get_pad_format(sd, state, fse->pad);
+
+	if (fse->index >= 2 || fse->code != format->code)
+		return -EINVAL;
+
+	fse->min_width = IMX296_PIXEL_ARRAY_WIDTH / (fse->index + 1);
+	fse->max_width = fse->min_width;
+	fse->min_height = IMX296_PIXEL_ARRAY_HEIGHT / (fse->index + 1);
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static int imx296_set_format(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *state,
+			     struct v4l2_subdev_format *fmt)
+{
+	struct imx296 *sensor = to_imx296(sd);
+	struct v4l2_mbus_framefmt *format;
+	struct v4l2_rect *crop;
+
+	crop = v4l2_subdev_get_pad_crop(sd, state, fmt->pad);
+	format = v4l2_subdev_get_pad_format(sd, state, fmt->pad);
+
+	/*
+	 * Binning is only allowed when cropping is disabled according to the
+	 * documentation. This should be double-checked.
+	 */
+	if (crop->width == IMX296_PIXEL_ARRAY_WIDTH &&
+	    crop->height == IMX296_PIXEL_ARRAY_HEIGHT) {
+		unsigned int width;
+		unsigned int height;
+		unsigned int hratio;
+		unsigned int vratio;
+
+		/* Clamp the width and height to avoid dividing by zero. */
+		width = clamp_t(unsigned int, fmt->format.width,
+				crop->width / 2, crop->width);
+		height = clamp_t(unsigned int, fmt->format.height,
+				 crop->height / 2, crop->height);
+
+		hratio = DIV_ROUND_CLOSEST(crop->width, width);
+		vratio = DIV_ROUND_CLOSEST(crop->height, height);
+
+		format->width = crop->width / hratio;
+		format->height = crop->height / vratio;
+	} else {
+		format->width = crop->width;
+		format->height = crop->height;
+	}
+
+	format->code = sensor->mono ? MEDIA_BUS_FMT_Y10_1X10
+		     : MEDIA_BUS_FMT_SBGGR10_1X10;
+	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;
+
+	fmt->format = *format;
+
+	return 0;
+}
+
+static int imx296_get_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *state,
+				struct v4l2_subdev_selection *sel)
+{
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		sel->r = *v4l2_subdev_get_pad_crop(sd, state, sel->pad);
+		break;
+
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_NATIVE_SIZE:
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = IMX296_PIXEL_ARRAY_WIDTH;
+		sel->r.height = IMX296_PIXEL_ARRAY_HEIGHT;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int imx296_set_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *state,
+				struct v4l2_subdev_selection *sel)
+{
+	struct v4l2_mbus_framefmt *format;
+	struct v4l2_rect *crop;
+	struct v4l2_rect rect;
+
+	if (sel->target != V4L2_SEL_TGT_CROP)
+		return -EINVAL;
+
+	/*
+	 * Clamp the crop rectangle boundaries and align them to a multiple of 4
+	 * pixels to satisfy hardware requirements.
+	 */
+	rect.left = clamp(ALIGN(sel->r.left, 4), 0,
+			  IMX296_PIXEL_ARRAY_WIDTH - IMX296_FID0_ROIWH1_MIN);
+	rect.top = clamp(ALIGN(sel->r.top, 4), 0,
+			 IMX296_PIXEL_ARRAY_HEIGHT - IMX296_FID0_ROIWV1_MIN);
+	rect.width = clamp_t(unsigned int, ALIGN(sel->r.width, 4),
+			     IMX296_FID0_ROIWH1_MIN, IMX296_PIXEL_ARRAY_WIDTH);
+	rect.height = clamp_t(unsigned int, ALIGN(sel->r.height, 4),
+			      IMX296_FID0_ROIWV1_MIN, IMX296_PIXEL_ARRAY_HEIGHT);
+
+	rect.width = min_t(unsigned int, rect.width,
+			   IMX296_PIXEL_ARRAY_WIDTH - rect.left);
+	rect.height = min_t(unsigned int, rect.height,
+			    IMX296_PIXEL_ARRAY_HEIGHT - rect.top);
+
+	crop = v4l2_subdev_get_pad_crop(sd, state, sel->pad);
+
+	if (rect.width != crop->width || rect.height != crop->height) {
+		/*
+		 * Reset the output image size if the crop rectangle size has
+		 * been modified.
+		 */
+		format = v4l2_subdev_get_pad_format(sd, state, sel->pad);
+		format->width = rect.width;
+		format->height = rect.height;
+	}
+
+	*crop = rect;
+	sel->r = rect;
+
+	return 0;
+}
+
+static int imx296_init_cfg(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_state *state)
+{
+	struct v4l2_subdev_selection sel = {
+		.target = V4L2_SEL_TGT_CROP,
+		.r.width = IMX296_PIXEL_ARRAY_WIDTH,
+		.r.height = IMX296_PIXEL_ARRAY_HEIGHT,
+	};
+	struct v4l2_subdev_format format = {
+		.format = {
+			.width = IMX296_PIXEL_ARRAY_WIDTH,
+			.height = IMX296_PIXEL_ARRAY_HEIGHT,
+		},
+	};
+
+	imx296_set_selection(sd, state, &sel);
+	imx296_set_format(sd, state, &format);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops imx296_subdev_video_ops = {
+	.s_stream = imx296_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops imx296_subdev_pad_ops = {
+	.enum_mbus_code = imx296_enum_mbus_code,
+	.enum_frame_size = imx296_enum_frame_size,
+	.get_fmt = v4l2_subdev_get_fmt,
+	.set_fmt = imx296_set_format,
+	.get_selection = imx296_get_selection,
+	.set_selection = imx296_set_selection,
+	.init_cfg = imx296_init_cfg,
+};
+
+static const struct v4l2_subdev_ops imx296_subdev_ops = {
+	.video = &imx296_subdev_video_ops,
+	.pad = &imx296_subdev_pad_ops,
+};
+
+static int imx296_subdev_init(struct imx296 *sensor)
+{
+	struct i2c_client *client = to_i2c_client(sensor->dev);
+	int ret;
+
+	v4l2_i2c_subdev_init(&sensor->subdev, client, &imx296_subdev_ops);
+
+	ret = imx296_ctrls_init(sensor);
+	if (ret < 0)
+		return ret;
+
+	sensor->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
+	sensor->subdev.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret = media_entity_pads_init(&sensor->subdev.entity, 1, &sensor->pad);
+	if (ret < 0) {
+		v4l2_ctrl_handler_free(&sensor->ctrls);
+		return ret;
+	}
+
+	sensor->subdev.state_lock = sensor->subdev.ctrl_handler->lock;
+
+	v4l2_subdev_init_finalize(&sensor->subdev);
+
+	return ret;
+}
+
+static void imx296_subdev_cleanup(struct imx296 *sensor)
+{
+	media_entity_cleanup(&sensor->subdev.entity);
+	v4l2_ctrl_handler_free(&sensor->ctrls);
+}
+
+/* -----------------------------------------------------------------------------
+ * Power management
+ */
+
+static int __maybe_unused imx296_runtime_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct imx296 *sensor = to_imx296(subdev);
+
+	return imx296_power_on(sensor);
+}
+
+static int __maybe_unused imx296_runtime_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct imx296 *sensor = to_imx296(subdev);
+
+	imx296_power_off(sensor);
+
+	return 0;
+}
+
+static const struct dev_pm_ops imx296_pm_ops = {
+	SET_RUNTIME_PM_OPS(imx296_runtime_suspend, imx296_runtime_resume, NULL)
+};
+
+/* -----------------------------------------------------------------------------
+ * Probe & Remove
+ */
+
+static int imx296_read_temperature(struct imx296 *sensor, int *temp)
+{
+	int tmdout;
+	int ret;
+
+	ret = imx296_write(sensor, IMX296_TMDCTRL, IMX296_TMDCTRL_LATCH, NULL);
+	if (ret < 0)
+		return ret;
+
+	tmdout = imx296_read(sensor, IMX296_TMDOUT);
+	if (tmdout < 0)
+		return tmdout;
+
+	tmdout &= IMX296_TMDOUT_MASK;
+
+	/* T(°C) = 246.312 - 0.304 * TMDOUT */;
+	*temp = 246312 - 304 * tmdout;
+
+	return imx296_write(sensor, IMX296_TMDCTRL, 0, NULL);
+}
+
+static int imx296_identify_model(struct imx296 *sensor)
+{
+	unsigned int model;
+	int temp = 0;
+	int ret;
+
+	model = (uintptr_t)of_device_get_match_data(sensor->dev);
+	if (model) {
+		dev_dbg(sensor->dev,
+			"sensor model auto-detection disabled, forcing 0x%04x\n",
+			model);
+		sensor->mono = model & IMX296_SENSOR_INFO_MONO;
+		return 0;
+	}
+
+	/*
+	 * While most registers can be read when the sensor is in standby, this
+	 * is not the case of the sensor info register :-(
+	 */
+	ret = imx296_write(sensor, IMX296_CTRL00, 0, NULL);
+	if (ret < 0) {
+		dev_err(sensor->dev,
+			"failed to get sensor out of standby (%d)\n", ret);
+		return ret;
+	}
+
+	ret = imx296_read(sensor, IMX296_SENSOR_INFO);
+	if (ret < 0) {
+		dev_err(sensor->dev, "failed to read sensor information (%d)\n",
+			ret);
+		goto done;
+	}
+
+	model = (ret >> 6) & 0x1ff;
+
+	switch (model) {
+	case 296:
+		sensor->mono = ret & IMX296_SENSOR_INFO_MONO;
+		break;
+	/*
+	 * The IMX297 seems to share features with the IMX296, it may be
+	 * possible to support it in the same driver.
+	 */
+	case 297:
+	default:
+		dev_err(sensor->dev, "invalid device model 0x%04x\n", ret);
+		ret = -ENODEV;
+		goto done;
+	}
+
+	ret = imx296_read_temperature(sensor, &temp);
+	if (ret < 0)
+		goto done;
+
+	dev_info(sensor->dev, "found IMX%u%s (%u.%uC)\n", model,
+		 sensor->mono ? "LL" : "LQ", temp / 1000, (temp / 100) % 10);
+
+done:
+	imx296_write(sensor, IMX296_CTRL00, IMX296_CTRL00_STANDBY, NULL);
+	return ret;
+}
+
+static const struct regmap_config imx296_regmap_config = {
+	.reg_bits = 16,
+	.val_bits = 8,
+
+	.wr_table = &(const struct regmap_access_table) {
+		.no_ranges = (const struct regmap_range[]) {
+			{
+				.range_min = IMX296_SENSOR_INFO & 0xffff,
+				.range_max = (IMX296_SENSOR_INFO & 0xffff) + 1,
+			},
+		},
+		.n_no_ranges = 1,
+	},
+};
+
+static int imx296_probe(struct i2c_client *client)
+{
+	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
+	unsigned long clk_rate;
+	struct imx296 *sensor;
+	unsigned int i;
+	int ret;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA)) {
+		dev_warn(&adapter->dev,
+			 "I2C-Adapter doesn't support I2C_FUNC_SMBUS_BYTE\n");
+		return -EIO;
+	}
+
+	sensor = devm_kzalloc(&client->dev, sizeof(*sensor), GFP_KERNEL);
+	if (!sensor)
+		return -ENOMEM;
+
+	sensor->dev = &client->dev;
+
+	/* Acquire resources. */
+	for (i = 0; i < ARRAY_SIZE(sensor->supplies); ++i)
+		sensor->supplies[i].supply = imx296_supply_names[i];
+
+	ret = devm_regulator_bulk_get(sensor->dev, ARRAY_SIZE(sensor->supplies),
+				      sensor->supplies);
+	if (ret) {
+		dev_err_probe(sensor->dev, ret, "failed to get supplies\n");
+		return ret;
+	}
+
+	sensor->reset = devm_gpiod_get_optional(sensor->dev, "reset",
+						GPIOD_OUT_HIGH);
+	if (IS_ERR(sensor->reset))
+		return dev_err_probe(sensor->dev, PTR_ERR(sensor->reset),
+				     "failed to get reset GPIO\n");
+
+	sensor->clk = devm_clk_get(sensor->dev, "inck");
+	if (IS_ERR(sensor->clk))
+		return dev_err_probe(sensor->dev, PTR_ERR(sensor->clk),
+				     "failed to get clock\n");
+
+	clk_rate = clk_get_rate(sensor->clk);
+	for (i = 0; i < ARRAY_SIZE(imx296_clk_params); ++i) {
+		if (clk_rate == imx296_clk_params[i].freq) {
+			sensor->clk_params = &imx296_clk_params[i];
+			break;
+		}
+	}
+
+	if (!sensor->clk_params) {
+		dev_err(sensor->dev, "unsupported clock rate %lu\n", clk_rate);
+		return -EINVAL;
+	}
+
+	sensor->regmap = devm_regmap_init_i2c(client, &imx296_regmap_config);
+	if (IS_ERR(sensor->regmap))
+		return PTR_ERR(sensor->regmap);
+
+	/*
+	 * Enable power management. The driver supports runtime PM, but needs to
+	 * work when runtime PM is disabled in the kernel. To that end, power
+	 * the sensor on manually here, identify it, and fully initialize it.
+	 */
+	ret = imx296_power_on(sensor);
+	if (ret < 0)
+		return ret;
+
+	ret = imx296_identify_model(sensor);
+	if (ret < 0)
+		goto err_power;
+
+	/* Initialize the V4L2 subdev. */
+	ret = imx296_subdev_init(sensor);
+	if (ret < 0)
+		goto err_power;
+
+	/*
+	 * Enable runtime PM. As the device has been powered manually, mark it
+	 * as active, and increase the usage count without resuming the device.
+	 */
+	pm_runtime_set_active(sensor->dev);
+	pm_runtime_get_noresume(sensor->dev);
+	pm_runtime_enable(sensor->dev);
+
+	/* Register the V4L2 subdev. */
+	ret = v4l2_async_register_subdev(&sensor->subdev);
+	if (ret < 0)
+		goto err_pm;
+
+	/*
+	 * Finally, enable autosuspend and decrease the usage count. The device
+	 * will get suspended after the autosuspend delay, turning the power
+	 * off.
+	 */
+	pm_runtime_set_autosuspend_delay(sensor->dev, 1000);
+	pm_runtime_use_autosuspend(sensor->dev);
+	pm_runtime_put_autosuspend(sensor->dev);
+
+	return 0;
+
+err_pm:
+	pm_runtime_disable(sensor->dev);
+	pm_runtime_put_noidle(sensor->dev);
+	imx296_subdev_cleanup(sensor);
+err_power:
+	imx296_power_off(sensor);
+	return ret;
+}
+
+static void imx296_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct imx296 *sensor = to_imx296(subdev);
+
+	v4l2_async_unregister_subdev(subdev);
+
+	imx296_subdev_cleanup(sensor);
+
+	/*
+	 * Disable runtime PM. In case runtime PM is disabled in the kernel,
+	 * make sure to turn power off manually.
+	 */
+	pm_runtime_disable(sensor->dev);
+	if (!pm_runtime_status_suspended(sensor->dev))
+		imx296_power_off(sensor);
+	pm_runtime_set_suspended(sensor->dev);
+}
+
+static const struct of_device_id imx296_of_match[] = {
+	{ .compatible = "sony,imx296", .data = NULL },
+	{ .compatible = "sony,imx296ll", .data = (void *)IMX296_SENSOR_INFO_IMX296LL },
+	{ .compatible = "sony,imx296lq", .data = (void *)IMX296_SENSOR_INFO_IMX296LQ },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, imx296_of_match);
+
+static struct i2c_driver imx296_i2c_driver = {
+	.driver = {
+		.of_match_table = imx296_of_match,
+		.name = "imx296",
+		.pm = &imx296_pm_ops
+	},
+	.probe = imx296_probe,
+	.remove = imx296_remove,
+};
+
+module_i2c_driver(imx296_i2c_driver);
+
+MODULE_DESCRIPTION("Sony IMX296 Camera driver");
+MODULE_AUTHOR("Laurent Pinchart <laurent.pinchart@ideasonboard.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/imx319.c b/drivers/media/i2c/imx319.c
new file mode 100644
index 000000000..52ebb096e
--- /dev/null
+++ b/drivers/media/i2c/imx319.c
@@ -0,0 +1,2572 @@
+// SPDX-License-Identifier: GPL-2.0
+// Copyright (C) 2018 Intel Corporation
+
+#include <asm/unaligned.h>
+#include <linux/acpi.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+
+#define IMX319_REG_MODE_SELECT		0x0100
+#define IMX319_MODE_STANDBY		0x00
+#define IMX319_MODE_STREAMING		0x01
+
+/* Chip ID */
+#define IMX319_REG_CHIP_ID		0x0016
+#define IMX319_CHIP_ID			0x0319
+
+/* V_TIMING internal */
+#define IMX319_REG_FLL			0x0340
+#define IMX319_FLL_MAX			0xffff
+
+/* Exposure control */
+#define IMX319_REG_EXPOSURE		0x0202
+#define IMX319_EXPOSURE_MIN		1
+#define IMX319_EXPOSURE_STEP		1
+#define IMX319_EXPOSURE_DEFAULT		0x04f6
+
+/*
+ *  the digital control register for all color control looks like:
+ *  +-----------------+------------------+
+ *  |      [7:0]      |       [15:8]     |
+ *  +-----------------+------------------+
+ *  |	  0x020f      |       0x020e     |
+ *  --------------------------------------
+ *  it is used to calculate the digital gain times value(integral + fractional)
+ *  the [15:8] bits is the fractional part and [7:0] bits is the integral
+ *  calculation equation is:
+ *      gain value (unit: times) = REG[15:8] + REG[7:0]/0x100
+ *  Only value in 0x0100 ~ 0x0FFF range is allowed.
+ *  Analog gain use 10 bits in the registers and allowed range is 0 ~ 960
+ */
+/* Analog gain control */
+#define IMX319_REG_ANALOG_GAIN		0x0204
+#define IMX319_ANA_GAIN_MIN		0
+#define IMX319_ANA_GAIN_MAX		960
+#define IMX319_ANA_GAIN_STEP		1
+#define IMX319_ANA_GAIN_DEFAULT		0
+
+/* Digital gain control */
+#define IMX319_REG_DPGA_USE_GLOBAL_GAIN	0x3ff9
+#define IMX319_REG_DIG_GAIN_GLOBAL	0x020e
+#define IMX319_DGTL_GAIN_MIN		256
+#define IMX319_DGTL_GAIN_MAX		4095
+#define IMX319_DGTL_GAIN_STEP		1
+#define IMX319_DGTL_GAIN_DEFAULT	256
+
+/* Test Pattern Control */
+#define IMX319_REG_TEST_PATTERN		0x0600
+#define IMX319_TEST_PATTERN_DISABLED		0
+#define IMX319_TEST_PATTERN_SOLID_COLOR		1
+#define IMX319_TEST_PATTERN_COLOR_BARS		2
+#define IMX319_TEST_PATTERN_GRAY_COLOR_BARS	3
+#define IMX319_TEST_PATTERN_PN9			4
+
+/* Flip Control */
+#define IMX319_REG_ORIENTATION		0x0101
+
+/* default link frequency and external clock */
+#define IMX319_LINK_FREQ_DEFAULT	482400000
+#define IMX319_EXT_CLK			19200000
+#define IMX319_LINK_FREQ_INDEX		0
+
+struct imx319_reg {
+	u16 address;
+	u8 val;
+};
+
+struct imx319_reg_list {
+	u32 num_of_regs;
+	const struct imx319_reg *regs;
+};
+
+/* Mode : resolution and related config&values */
+struct imx319_mode {
+	/* Frame width */
+	u32 width;
+	/* Frame height */
+	u32 height;
+
+	/* V-timing */
+	u32 fll_def;
+	u32 fll_min;
+
+	/* H-timing */
+	u32 llp;
+
+	/* index of link frequency */
+	u32 link_freq_index;
+
+	/* Default register values */
+	struct imx319_reg_list reg_list;
+};
+
+struct imx319_hwcfg {
+	u32 ext_clk;			/* sensor external clk */
+	s64 *link_freqs;		/* CSI-2 link frequencies */
+	unsigned int nr_of_link_freqs;
+};
+
+struct imx319 {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+
+	struct v4l2_ctrl_handler ctrl_handler;
+	/* V4L2 Controls */
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *pixel_rate;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *exposure;
+	struct v4l2_ctrl *vflip;
+	struct v4l2_ctrl *hflip;
+
+	/* Current mode */
+	const struct imx319_mode *cur_mode;
+
+	struct imx319_hwcfg *hwcfg;
+	s64 link_def_freq;	/* CSI-2 link default frequency */
+
+	/*
+	 * Mutex for serialized access:
+	 * Protect sensor set pad format and start/stop streaming safely.
+	 * Protect access to sensor v4l2 controls.
+	 */
+	struct mutex mutex;
+
+	/* Streaming on/off */
+	bool streaming;
+	/* True if the device has been identified */
+	bool identified;
+};
+
+static const struct imx319_reg imx319_global_regs[] = {
+	{ 0x0136, 0x13 },
+	{ 0x0137, 0x33 },
+	{ 0x3c7e, 0x05 },
+	{ 0x3c7f, 0x07 },
+	{ 0x4d39, 0x0b },
+	{ 0x4d41, 0x33 },
+	{ 0x4d43, 0x0c },
+	{ 0x4d49, 0x89 },
+	{ 0x4e05, 0x0b },
+	{ 0x4e0d, 0x33 },
+	{ 0x4e0f, 0x0c },
+	{ 0x4e15, 0x89 },
+	{ 0x4e49, 0x2a },
+	{ 0x4e51, 0x33 },
+	{ 0x4e53, 0x0c },
+	{ 0x4e59, 0x89 },
+	{ 0x5601, 0x4f },
+	{ 0x560b, 0x45 },
+	{ 0x562f, 0x0a },
+	{ 0x5643, 0x0a },
+	{ 0x5645, 0x0c },
+	{ 0x56ef, 0x51 },
+	{ 0x586f, 0x33 },
+	{ 0x5873, 0x89 },
+	{ 0x5905, 0x33 },
+	{ 0x5907, 0x89 },
+	{ 0x590d, 0x33 },
+	{ 0x590f, 0x89 },
+	{ 0x5915, 0x33 },
+	{ 0x5917, 0x89 },
+	{ 0x5969, 0x1c },
+	{ 0x596b, 0x72 },
+	{ 0x5971, 0x33 },
+	{ 0x5973, 0x89 },
+	{ 0x5975, 0x33 },
+	{ 0x5977, 0x89 },
+	{ 0x5979, 0x1c },
+	{ 0x597b, 0x72 },
+	{ 0x5985, 0x33 },
+	{ 0x5987, 0x89 },
+	{ 0x5999, 0x1c },
+	{ 0x599b, 0x72 },
+	{ 0x59a5, 0x33 },
+	{ 0x59a7, 0x89 },
+	{ 0x7485, 0x08 },
+	{ 0x7487, 0x0c },
+	{ 0x7489, 0xc7 },
+	{ 0x748b, 0x8b },
+	{ 0x9004, 0x09 },
+	{ 0x9200, 0x6a },
+	{ 0x9201, 0x22 },
+	{ 0x9202, 0x6a },
+	{ 0x9203, 0x23 },
+	{ 0x9204, 0x5f },
+	{ 0x9205, 0x23 },
+	{ 0x9206, 0x5f },
+	{ 0x9207, 0x24 },
+	{ 0x9208, 0x5f },
+	{ 0x9209, 0x26 },
+	{ 0x920a, 0x5f },
+	{ 0x920b, 0x27 },
+	{ 0x920c, 0x5f },
+	{ 0x920d, 0x29 },
+	{ 0x920e, 0x5f },
+	{ 0x920f, 0x2a },
+	{ 0x9210, 0x5f },
+	{ 0x9211, 0x2c },
+	{ 0xbc22, 0x1a },
+	{ 0xf01f, 0x04 },
+	{ 0xf021, 0x03 },
+	{ 0xf023, 0x02 },
+	{ 0xf03d, 0x05 },
+	{ 0xf03f, 0x03 },
+	{ 0xf041, 0x02 },
+	{ 0xf0af, 0x04 },
+	{ 0xf0b1, 0x03 },
+	{ 0xf0b3, 0x02 },
+	{ 0xf0cd, 0x05 },
+	{ 0xf0cf, 0x03 },
+	{ 0xf0d1, 0x02 },
+	{ 0xf13f, 0x04 },
+	{ 0xf141, 0x03 },
+	{ 0xf143, 0x02 },
+	{ 0xf15d, 0x05 },
+	{ 0xf15f, 0x03 },
+	{ 0xf161, 0x02 },
+	{ 0xf1cf, 0x04 },
+	{ 0xf1d1, 0x03 },
+	{ 0xf1d3, 0x02 },
+	{ 0xf1ed, 0x05 },
+	{ 0xf1ef, 0x03 },
+	{ 0xf1f1, 0x02 },
+	{ 0xf287, 0x04 },
+	{ 0xf289, 0x03 },
+	{ 0xf28b, 0x02 },
+	{ 0xf2a5, 0x05 },
+	{ 0xf2a7, 0x03 },
+	{ 0xf2a9, 0x02 },
+	{ 0xf2b7, 0x04 },
+	{ 0xf2b9, 0x03 },
+	{ 0xf2bb, 0x02 },
+	{ 0xf2d5, 0x05 },
+	{ 0xf2d7, 0x03 },
+	{ 0xf2d9, 0x02 },
+};
+
+static const struct imx319_reg_list imx319_global_setting = {
+	.num_of_regs = ARRAY_SIZE(imx319_global_regs),
+	.regs = imx319_global_regs,
+};
+
+static const struct imx319_reg mode_3264x2448_regs[] = {
+	{ 0x0112, 0x0a },
+	{ 0x0113, 0x0a },
+	{ 0x0114, 0x03 },
+	{ 0x0342, 0x0f },
+	{ 0x0343, 0x80 },
+	{ 0x0340, 0x0c },
+	{ 0x0341, 0xaa },
+	{ 0x0344, 0x00 },
+	{ 0x0345, 0x00 },
+	{ 0x0346, 0x00 },
+	{ 0x0347, 0x00 },
+	{ 0x0348, 0x0c },
+	{ 0x0349, 0xcf },
+	{ 0x034a, 0x09 },
+	{ 0x034b, 0x9f },
+	{ 0x0220, 0x00 },
+	{ 0x0221, 0x11 },
+	{ 0x0381, 0x01 },
+	{ 0x0383, 0x01 },
+	{ 0x0385, 0x01 },
+	{ 0x0387, 0x01 },
+	{ 0x0900, 0x00 },
+	{ 0x0901, 0x11 },
+	{ 0x0902, 0x0a },
+	{ 0x3140, 0x02 },
+	{ 0x3141, 0x00 },
+	{ 0x3f0d, 0x0a },
+	{ 0x3f14, 0x01 },
+	{ 0x3f3c, 0x01 },
+	{ 0x3f4d, 0x01 },
+	{ 0x3f4c, 0x01 },
+	{ 0x4254, 0x7f },
+	{ 0x0401, 0x00 },
+	{ 0x0404, 0x00 },
+	{ 0x0405, 0x10 },
+	{ 0x0408, 0x00 },
+	{ 0x0409, 0x08 },
+	{ 0x040a, 0x00 },
+	{ 0x040b, 0x08 },
+	{ 0x040c, 0x0c },
+	{ 0x040d, 0xc0 },
+	{ 0x040e, 0x09 },
+	{ 0x040f, 0x90 },
+	{ 0x034c, 0x0c },
+	{ 0x034d, 0xc0 },
+	{ 0x034e, 0x09 },
+	{ 0x034f, 0x90 },
+	{ 0x3261, 0x00 },
+	{ 0x3264, 0x00 },
+	{ 0x3265, 0x10 },
+	{ 0x0301, 0x05 },
+	{ 0x0303, 0x04 },
+	{ 0x0305, 0x04 },
+	{ 0x0306, 0x01 },
+	{ 0x0307, 0x92 },
+	{ 0x0309, 0x0a },
+	{ 0x030b, 0x02 },
+	{ 0x030d, 0x02 },
+	{ 0x030e, 0x00 },
+	{ 0x030f, 0xfa },
+	{ 0x0310, 0x00 },
+	{ 0x0820, 0x0f },
+	{ 0x0821, 0x13 },
+	{ 0x0822, 0x33 },
+	{ 0x0823, 0x33 },
+	{ 0x3e20, 0x01 },
+	{ 0x3e37, 0x00 },
+	{ 0x3e3b, 0x01 },
+	{ 0x38a3, 0x01 },
+	{ 0x38a8, 0x00 },
+	{ 0x38a9, 0x00 },
+	{ 0x38aa, 0x00 },
+	{ 0x38ab, 0x00 },
+	{ 0x3234, 0x00 },
+	{ 0x3fc1, 0x00 },
+	{ 0x3235, 0x00 },
+	{ 0x3802, 0x00 },
+	{ 0x3143, 0x04 },
+	{ 0x360a, 0x00 },
+	{ 0x0b00, 0x00 },
+	{ 0x0106, 0x00 },
+	{ 0x0b05, 0x01 },
+	{ 0x0b06, 0x01 },
+	{ 0x3230, 0x00 },
+	{ 0x3602, 0x01 },
+	{ 0x3607, 0x01 },
+	{ 0x3c00, 0x00 },
+	{ 0x3c01, 0x48 },
+	{ 0x3c02, 0xc8 },
+	{ 0x3c03, 0xaa },
+	{ 0x3c04, 0x91 },
+	{ 0x3c05, 0x54 },
+	{ 0x3c06, 0x26 },
+	{ 0x3c07, 0x20 },
+	{ 0x3c08, 0x51 },
+	{ 0x3d80, 0x00 },
+	{ 0x3f50, 0x00 },
+	{ 0x3f56, 0x00 },
+	{ 0x3f57, 0x30 },
+	{ 0x3f78, 0x01 },
+	{ 0x3f79, 0x18 },
+	{ 0x3f7c, 0x00 },
+	{ 0x3f7d, 0x00 },
+	{ 0x3fba, 0x00 },
+	{ 0x3fbb, 0x00 },
+	{ 0xa081, 0x00 },
+	{ 0xe014, 0x00 },
+	{ 0x0202, 0x0a },
+	{ 0x0203, 0x7a },
+	{ 0x0224, 0x01 },
+	{ 0x0225, 0xf4 },
+	{ 0x0204, 0x00 },
+	{ 0x0205, 0x00 },
+	{ 0x0216, 0x00 },
+	{ 0x0217, 0x00 },
+	{ 0x020e, 0x01 },
+	{ 0x020f, 0x00 },
+	{ 0x0210, 0x01 },
+	{ 0x0211, 0x00 },
+	{ 0x0212, 0x01 },
+	{ 0x0213, 0x00 },
+	{ 0x0214, 0x01 },
+	{ 0x0215, 0x00 },
+	{ 0x0218, 0x01 },
+	{ 0x0219, 0x00 },
+	{ 0x3614, 0x00 },
+	{ 0x3616, 0x0d },
+	{ 0x3617, 0x56 },
+	{ 0xb612, 0x20 },
+	{ 0xb613, 0x20 },
+	{ 0xb614, 0x20 },
+	{ 0xb615, 0x20 },
+	{ 0xb616, 0x0a },
+	{ 0xb617, 0x0a },
+	{ 0xb618, 0x20 },
+	{ 0xb619, 0x20 },
+	{ 0xb61a, 0x20 },
+	{ 0xb61b, 0x20 },
+	{ 0xb61c, 0x0a },
+	{ 0xb61d, 0x0a },
+	{ 0xb666, 0x30 },
+	{ 0xb667, 0x30 },
+	{ 0xb668, 0x30 },
+	{ 0xb669, 0x30 },
+	{ 0xb66a, 0x14 },
+	{ 0xb66b, 0x14 },
+	{ 0xb66c, 0x20 },
+	{ 0xb66d, 0x20 },
+	{ 0xb66e, 0x20 },
+	{ 0xb66f, 0x20 },
+	{ 0xb670, 0x10 },
+	{ 0xb671, 0x10 },
+	{ 0x3237, 0x00 },
+	{ 0x3900, 0x00 },
+	{ 0x3901, 0x00 },
+	{ 0x3902, 0x00 },
+	{ 0x3904, 0x00 },
+	{ 0x3905, 0x00 },
+	{ 0x3906, 0x00 },
+	{ 0x3907, 0x00 },
+	{ 0x3908, 0x00 },
+	{ 0x3909, 0x00 },
+	{ 0x3912, 0x00 },
+	{ 0x3930, 0x00 },
+	{ 0x3931, 0x00 },
+	{ 0x3933, 0x00 },
+	{ 0x3934, 0x00 },
+	{ 0x3935, 0x00 },
+	{ 0x3936, 0x00 },
+	{ 0x3937, 0x00 },
+	{ 0x30ac, 0x00 },
+};
+
+static const struct imx319_reg mode_3280x2464_regs[] = {
+	{ 0x0112, 0x0a },
+	{ 0x0113, 0x0a },
+	{ 0x0114, 0x03 },
+	{ 0x0342, 0x0f },
+	{ 0x0343, 0x80 },
+	{ 0x0340, 0x0c },
+	{ 0x0341, 0xaa },
+	{ 0x0344, 0x00 },
+	{ 0x0345, 0x00 },
+	{ 0x0346, 0x00 },
+	{ 0x0347, 0x00 },
+	{ 0x0348, 0x0c },
+	{ 0x0349, 0xcf },
+	{ 0x034a, 0x09 },
+	{ 0x034b, 0x9f },
+	{ 0x0220, 0x00 },
+	{ 0x0221, 0x11 },
+	{ 0x0381, 0x01 },
+	{ 0x0383, 0x01 },
+	{ 0x0385, 0x01 },
+	{ 0x0387, 0x01 },
+	{ 0x0900, 0x00 },
+	{ 0x0901, 0x11 },
+	{ 0x0902, 0x0a },
+	{ 0x3140, 0x02 },
+	{ 0x3141, 0x00 },
+	{ 0x3f0d, 0x0a },
+	{ 0x3f14, 0x01 },
+	{ 0x3f3c, 0x01 },
+	{ 0x3f4d, 0x01 },
+	{ 0x3f4c, 0x01 },
+	{ 0x4254, 0x7f },
+	{ 0x0401, 0x00 },
+	{ 0x0404, 0x00 },
+	{ 0x0405, 0x10 },
+	{ 0x0408, 0x00 },
+	{ 0x0409, 0x00 },
+	{ 0x040a, 0x00 },
+	{ 0x040b, 0x00 },
+	{ 0x040c, 0x0c },
+	{ 0x040d, 0xd0 },
+	{ 0x040e, 0x09 },
+	{ 0x040f, 0xa0 },
+	{ 0x034c, 0x0c },
+	{ 0x034d, 0xd0 },
+	{ 0x034e, 0x09 },
+	{ 0x034f, 0xa0 },
+	{ 0x3261, 0x00 },
+	{ 0x3264, 0x00 },
+	{ 0x3265, 0x10 },
+	{ 0x0301, 0x05 },
+	{ 0x0303, 0x04 },
+	{ 0x0305, 0x04 },
+	{ 0x0306, 0x01 },
+	{ 0x0307, 0x92 },
+	{ 0x0309, 0x0a },
+	{ 0x030b, 0x02 },
+	{ 0x030d, 0x02 },
+	{ 0x030e, 0x00 },
+	{ 0x030f, 0xfa },
+	{ 0x0310, 0x00 },
+	{ 0x0820, 0x0f },
+	{ 0x0821, 0x13 },
+	{ 0x0822, 0x33 },
+	{ 0x0823, 0x33 },
+	{ 0x3e20, 0x01 },
+	{ 0x3e37, 0x00 },
+	{ 0x3e3b, 0x01 },
+	{ 0x38a3, 0x01 },
+	{ 0x38a8, 0x00 },
+	{ 0x38a9, 0x00 },
+	{ 0x38aa, 0x00 },
+	{ 0x38ab, 0x00 },
+	{ 0x3234, 0x00 },
+	{ 0x3fc1, 0x00 },
+	{ 0x3235, 0x00 },
+	{ 0x3802, 0x00 },
+	{ 0x3143, 0x04 },
+	{ 0x360a, 0x00 },
+	{ 0x0b00, 0x00 },
+	{ 0x0106, 0x00 },
+	{ 0x0b05, 0x01 },
+	{ 0x0b06, 0x01 },
+	{ 0x3230, 0x00 },
+	{ 0x3602, 0x01 },
+	{ 0x3607, 0x01 },
+	{ 0x3c00, 0x00 },
+	{ 0x3c01, 0x48 },
+	{ 0x3c02, 0xc8 },
+	{ 0x3c03, 0xaa },
+	{ 0x3c04, 0x91 },
+	{ 0x3c05, 0x54 },
+	{ 0x3c06, 0x26 },
+	{ 0x3c07, 0x20 },
+	{ 0x3c08, 0x51 },
+	{ 0x3d80, 0x00 },
+	{ 0x3f50, 0x00 },
+	{ 0x3f56, 0x00 },
+	{ 0x3f57, 0x30 },
+	{ 0x3f78, 0x01 },
+	{ 0x3f79, 0x18 },
+	{ 0x3f7c, 0x00 },
+	{ 0x3f7d, 0x00 },
+	{ 0x3fba, 0x00 },
+	{ 0x3fbb, 0x00 },
+	{ 0xa081, 0x00 },
+	{ 0xe014, 0x00 },
+	{ 0x0202, 0x0a },
+	{ 0x0203, 0x7a },
+	{ 0x0224, 0x01 },
+	{ 0x0225, 0xf4 },
+	{ 0x0204, 0x00 },
+	{ 0x0205, 0x00 },
+	{ 0x0216, 0x00 },
+	{ 0x0217, 0x00 },
+	{ 0x020e, 0x01 },
+	{ 0x020f, 0x00 },
+	{ 0x0210, 0x01 },
+	{ 0x0211, 0x00 },
+	{ 0x0212, 0x01 },
+	{ 0x0213, 0x00 },
+	{ 0x0214, 0x01 },
+	{ 0x0215, 0x00 },
+	{ 0x0218, 0x01 },
+	{ 0x0219, 0x00 },
+	{ 0x3614, 0x00 },
+	{ 0x3616, 0x0d },
+	{ 0x3617, 0x56 },
+	{ 0xb612, 0x20 },
+	{ 0xb613, 0x20 },
+	{ 0xb614, 0x20 },
+	{ 0xb615, 0x20 },
+	{ 0xb616, 0x0a },
+	{ 0xb617, 0x0a },
+	{ 0xb618, 0x20 },
+	{ 0xb619, 0x20 },
+	{ 0xb61a, 0x20 },
+	{ 0xb61b, 0x20 },
+	{ 0xb61c, 0x0a },
+	{ 0xb61d, 0x0a },
+	{ 0xb666, 0x30 },
+	{ 0xb667, 0x30 },
+	{ 0xb668, 0x30 },
+	{ 0xb669, 0x30 },
+	{ 0xb66a, 0x14 },
+	{ 0xb66b, 0x14 },
+	{ 0xb66c, 0x20 },
+	{ 0xb66d, 0x20 },
+	{ 0xb66e, 0x20 },
+	{ 0xb66f, 0x20 },
+	{ 0xb670, 0x10 },
+	{ 0xb671, 0x10 },
+	{ 0x3237, 0x00 },
+	{ 0x3900, 0x00 },
+	{ 0x3901, 0x00 },
+	{ 0x3902, 0x00 },
+	{ 0x3904, 0x00 },
+	{ 0x3905, 0x00 },
+	{ 0x3906, 0x00 },
+	{ 0x3907, 0x00 },
+	{ 0x3908, 0x00 },
+	{ 0x3909, 0x00 },
+	{ 0x3912, 0x00 },
+	{ 0x3930, 0x00 },
+	{ 0x3931, 0x00 },
+	{ 0x3933, 0x00 },
+	{ 0x3934, 0x00 },
+	{ 0x3935, 0x00 },
+	{ 0x3936, 0x00 },
+	{ 0x3937, 0x00 },
+	{ 0x30ac, 0x00 },
+};
+
+static const struct imx319_reg mode_1936x1096_regs[] = {
+	{ 0x0112, 0x0a },
+	{ 0x0113, 0x0a },
+	{ 0x0114, 0x03 },
+	{ 0x0342, 0x0f },
+	{ 0x0343, 0x80 },
+	{ 0x0340, 0x0c },
+	{ 0x0341, 0xaa },
+	{ 0x0344, 0x00 },
+	{ 0x0345, 0x00 },
+	{ 0x0346, 0x02 },
+	{ 0x0347, 0xac },
+	{ 0x0348, 0x0c },
+	{ 0x0349, 0xcf },
+	{ 0x034a, 0x06 },
+	{ 0x034b, 0xf3 },
+	{ 0x0220, 0x00 },
+	{ 0x0221, 0x11 },
+	{ 0x0381, 0x01 },
+	{ 0x0383, 0x01 },
+	{ 0x0385, 0x01 },
+	{ 0x0387, 0x01 },
+	{ 0x0900, 0x00 },
+	{ 0x0901, 0x11 },
+	{ 0x0902, 0x0a },
+	{ 0x3140, 0x02 },
+	{ 0x3141, 0x00 },
+	{ 0x3f0d, 0x0a },
+	{ 0x3f14, 0x01 },
+	{ 0x3f3c, 0x01 },
+	{ 0x3f4d, 0x01 },
+	{ 0x3f4c, 0x01 },
+	{ 0x4254, 0x7f },
+	{ 0x0401, 0x00 },
+	{ 0x0404, 0x00 },
+	{ 0x0405, 0x10 },
+	{ 0x0408, 0x02 },
+	{ 0x0409, 0xa0 },
+	{ 0x040a, 0x00 },
+	{ 0x040b, 0x00 },
+	{ 0x040c, 0x07 },
+	{ 0x040d, 0x90 },
+	{ 0x040e, 0x04 },
+	{ 0x040f, 0x48 },
+	{ 0x034c, 0x07 },
+	{ 0x034d, 0x90 },
+	{ 0x034e, 0x04 },
+	{ 0x034f, 0x48 },
+	{ 0x3261, 0x00 },
+	{ 0x3264, 0x00 },
+	{ 0x3265, 0x10 },
+	{ 0x0301, 0x05 },
+	{ 0x0303, 0x04 },
+	{ 0x0305, 0x04 },
+	{ 0x0306, 0x01 },
+	{ 0x0307, 0x92 },
+	{ 0x0309, 0x0a },
+	{ 0x030b, 0x02 },
+	{ 0x030d, 0x02 },
+	{ 0x030e, 0x00 },
+	{ 0x030f, 0xfa },
+	{ 0x0310, 0x00 },
+	{ 0x0820, 0x0f },
+	{ 0x0821, 0x13 },
+	{ 0x0822, 0x33 },
+	{ 0x0823, 0x33 },
+	{ 0x3e20, 0x01 },
+	{ 0x3e37, 0x00 },
+	{ 0x3e3b, 0x01 },
+	{ 0x38a3, 0x01 },
+	{ 0x38a8, 0x00 },
+	{ 0x38a9, 0x00 },
+	{ 0x38aa, 0x00 },
+	{ 0x38ab, 0x00 },
+	{ 0x3234, 0x00 },
+	{ 0x3fc1, 0x00 },
+	{ 0x3235, 0x00 },
+	{ 0x3802, 0x00 },
+	{ 0x3143, 0x04 },
+	{ 0x360a, 0x00 },
+	{ 0x0b00, 0x00 },
+	{ 0x0106, 0x00 },
+	{ 0x0b05, 0x01 },
+	{ 0x0b06, 0x01 },
+	{ 0x3230, 0x00 },
+	{ 0x3602, 0x01 },
+	{ 0x3607, 0x01 },
+	{ 0x3c00, 0x00 },
+	{ 0x3c01, 0x48 },
+	{ 0x3c02, 0xc8 },
+	{ 0x3c03, 0xaa },
+	{ 0x3c04, 0x91 },
+	{ 0x3c05, 0x54 },
+	{ 0x3c06, 0x26 },
+	{ 0x3c07, 0x20 },
+	{ 0x3c08, 0x51 },
+	{ 0x3d80, 0x00 },
+	{ 0x3f50, 0x00 },
+	{ 0x3f56, 0x00 },
+	{ 0x3f57, 0x30 },
+	{ 0x3f78, 0x01 },
+	{ 0x3f79, 0x18 },
+	{ 0x3f7c, 0x00 },
+	{ 0x3f7d, 0x00 },
+	{ 0x3fba, 0x00 },
+	{ 0x3fbb, 0x00 },
+	{ 0xa081, 0x00 },
+	{ 0xe014, 0x00 },
+	{ 0x0202, 0x05 },
+	{ 0x0203, 0x34 },
+	{ 0x0224, 0x01 },
+	{ 0x0225, 0xf4 },
+	{ 0x0204, 0x00 },
+	{ 0x0205, 0x00 },
+	{ 0x0216, 0x00 },
+	{ 0x0217, 0x00 },
+	{ 0x020e, 0x01 },
+	{ 0x020f, 0x00 },
+	{ 0x0210, 0x01 },
+	{ 0x0211, 0x00 },
+	{ 0x0212, 0x01 },
+	{ 0x0213, 0x00 },
+	{ 0x0214, 0x01 },
+	{ 0x0215, 0x00 },
+	{ 0x0218, 0x01 },
+	{ 0x0219, 0x00 },
+	{ 0x3614, 0x00 },
+	{ 0x3616, 0x0d },
+	{ 0x3617, 0x56 },
+	{ 0xb612, 0x20 },
+	{ 0xb613, 0x20 },
+	{ 0xb614, 0x20 },
+	{ 0xb615, 0x20 },
+	{ 0xb616, 0x0a },
+	{ 0xb617, 0x0a },
+	{ 0xb618, 0x20 },
+	{ 0xb619, 0x20 },
+	{ 0xb61a, 0x20 },
+	{ 0xb61b, 0x20 },
+	{ 0xb61c, 0x0a },
+	{ 0xb61d, 0x0a },
+	{ 0xb666, 0x30 },
+	{ 0xb667, 0x30 },
+	{ 0xb668, 0x30 },
+	{ 0xb669, 0x30 },
+	{ 0xb66a, 0x14 },
+	{ 0xb66b, 0x14 },
+	{ 0xb66c, 0x20 },
+	{ 0xb66d, 0x20 },
+	{ 0xb66e, 0x20 },
+	{ 0xb66f, 0x20 },
+	{ 0xb670, 0x10 },
+	{ 0xb671, 0x10 },
+	{ 0x3237, 0x00 },
+	{ 0x3900, 0x00 },
+	{ 0x3901, 0x00 },
+	{ 0x3902, 0x00 },
+	{ 0x3904, 0x00 },
+	{ 0x3905, 0x00 },
+	{ 0x3906, 0x00 },
+	{ 0x3907, 0x00 },
+	{ 0x3908, 0x00 },
+	{ 0x3909, 0x00 },
+	{ 0x3912, 0x00 },
+	{ 0x3930, 0x00 },
+	{ 0x3931, 0x00 },
+	{ 0x3933, 0x00 },
+	{ 0x3934, 0x00 },
+	{ 0x3935, 0x00 },
+	{ 0x3936, 0x00 },
+	{ 0x3937, 0x00 },
+	{ 0x30ac, 0x00 },
+};
+
+static const struct imx319_reg mode_1920x1080_regs[] = {
+	{ 0x0112, 0x0a },
+	{ 0x0113, 0x0a },
+	{ 0x0114, 0x03 },
+	{ 0x0342, 0x0f },
+	{ 0x0343, 0x80 },
+	{ 0x0340, 0x0c },
+	{ 0x0341, 0xaa },
+	{ 0x0344, 0x00 },
+	{ 0x0345, 0x00 },
+	{ 0x0346, 0x02 },
+	{ 0x0347, 0xb4 },
+	{ 0x0348, 0x0c },
+	{ 0x0349, 0xcf },
+	{ 0x034a, 0x06 },
+	{ 0x034b, 0xeb },
+	{ 0x0220, 0x00 },
+	{ 0x0221, 0x11 },
+	{ 0x0381, 0x01 },
+	{ 0x0383, 0x01 },
+	{ 0x0385, 0x01 },
+	{ 0x0387, 0x01 },
+	{ 0x0900, 0x00 },
+	{ 0x0901, 0x11 },
+	{ 0x0902, 0x0a },
+	{ 0x3140, 0x02 },
+	{ 0x3141, 0x00 },
+	{ 0x3f0d, 0x0a },
+	{ 0x3f14, 0x01 },
+	{ 0x3f3c, 0x01 },
+	{ 0x3f4d, 0x01 },
+	{ 0x3f4c, 0x01 },
+	{ 0x4254, 0x7f },
+	{ 0x0401, 0x00 },
+	{ 0x0404, 0x00 },
+	{ 0x0405, 0x10 },
+	{ 0x0408, 0x02 },
+	{ 0x0409, 0xa8 },
+	{ 0x040a, 0x00 },
+	{ 0x040b, 0x00 },
+	{ 0x040c, 0x07 },
+	{ 0x040d, 0x80 },
+	{ 0x040e, 0x04 },
+	{ 0x040f, 0x38 },
+	{ 0x034c, 0x07 },
+	{ 0x034d, 0x80 },
+	{ 0x034e, 0x04 },
+	{ 0x034f, 0x38 },
+	{ 0x3261, 0x00 },
+	{ 0x3264, 0x00 },
+	{ 0x3265, 0x10 },
+	{ 0x0301, 0x05 },
+	{ 0x0303, 0x04 },
+	{ 0x0305, 0x04 },
+	{ 0x0306, 0x01 },
+	{ 0x0307, 0x92 },
+	{ 0x0309, 0x0a },
+	{ 0x030b, 0x02 },
+	{ 0x030d, 0x02 },
+	{ 0x030e, 0x00 },
+	{ 0x030f, 0xfa },
+	{ 0x0310, 0x00 },
+	{ 0x0820, 0x0f },
+	{ 0x0821, 0x13 },
+	{ 0x0822, 0x33 },
+	{ 0x0823, 0x33 },
+	{ 0x3e20, 0x01 },
+	{ 0x3e37, 0x00 },
+	{ 0x3e3b, 0x01 },
+	{ 0x38a3, 0x01 },
+	{ 0x38a8, 0x00 },
+	{ 0x38a9, 0x00 },
+	{ 0x38aa, 0x00 },
+	{ 0x38ab, 0x00 },
+	{ 0x3234, 0x00 },
+	{ 0x3fc1, 0x00 },
+	{ 0x3235, 0x00 },
+	{ 0x3802, 0x00 },
+	{ 0x3143, 0x04 },
+	{ 0x360a, 0x00 },
+	{ 0x0b00, 0x00 },
+	{ 0x0106, 0x00 },
+	{ 0x0b05, 0x01 },
+	{ 0x0b06, 0x01 },
+	{ 0x3230, 0x00 },
+	{ 0x3602, 0x01 },
+	{ 0x3607, 0x01 },
+	{ 0x3c00, 0x00 },
+	{ 0x3c01, 0x48 },
+	{ 0x3c02, 0xc8 },
+	{ 0x3c03, 0xaa },
+	{ 0x3c04, 0x91 },
+	{ 0x3c05, 0x54 },
+	{ 0x3c06, 0x26 },
+	{ 0x3c07, 0x20 },
+	{ 0x3c08, 0x51 },
+	{ 0x3d80, 0x00 },
+	{ 0x3f50, 0x00 },
+	{ 0x3f56, 0x00 },
+	{ 0x3f57, 0x30 },
+	{ 0x3f78, 0x01 },
+	{ 0x3f79, 0x18 },
+	{ 0x3f7c, 0x00 },
+	{ 0x3f7d, 0x00 },
+	{ 0x3fba, 0x00 },
+	{ 0x3fbb, 0x00 },
+	{ 0xa081, 0x00 },
+	{ 0xe014, 0x00 },
+	{ 0x0202, 0x05 },
+	{ 0x0203, 0x34 },
+	{ 0x0224, 0x01 },
+	{ 0x0225, 0xf4 },
+	{ 0x0204, 0x00 },
+	{ 0x0205, 0x00 },
+	{ 0x0216, 0x00 },
+	{ 0x0217, 0x00 },
+	{ 0x020e, 0x01 },
+	{ 0x020f, 0x00 },
+	{ 0x0210, 0x01 },
+	{ 0x0211, 0x00 },
+	{ 0x0212, 0x01 },
+	{ 0x0213, 0x00 },
+	{ 0x0214, 0x01 },
+	{ 0x0215, 0x00 },
+	{ 0x0218, 0x01 },
+	{ 0x0219, 0x00 },
+	{ 0x3614, 0x00 },
+	{ 0x3616, 0x0d },
+	{ 0x3617, 0x56 },
+	{ 0xb612, 0x20 },
+	{ 0xb613, 0x20 },
+	{ 0xb614, 0x20 },
+	{ 0xb615, 0x20 },
+	{ 0xb616, 0x0a },
+	{ 0xb617, 0x0a },
+	{ 0xb618, 0x20 },
+	{ 0xb619, 0x20 },
+	{ 0xb61a, 0x20 },
+	{ 0xb61b, 0x20 },
+	{ 0xb61c, 0x0a },
+	{ 0xb61d, 0x0a },
+	{ 0xb666, 0x30 },
+	{ 0xb667, 0x30 },
+	{ 0xb668, 0x30 },
+	{ 0xb669, 0x30 },
+	{ 0xb66a, 0x14 },
+	{ 0xb66b, 0x14 },
+	{ 0xb66c, 0x20 },
+	{ 0xb66d, 0x20 },
+	{ 0xb66e, 0x20 },
+	{ 0xb66f, 0x20 },
+	{ 0xb670, 0x10 },
+	{ 0xb671, 0x10 },
+	{ 0x3237, 0x00 },
+	{ 0x3900, 0x00 },
+	{ 0x3901, 0x00 },
+	{ 0x3902, 0x00 },
+	{ 0x3904, 0x00 },
+	{ 0x3905, 0x00 },
+	{ 0x3906, 0x00 },
+	{ 0x3907, 0x00 },
+	{ 0x3908, 0x00 },
+	{ 0x3909, 0x00 },
+	{ 0x3912, 0x00 },
+	{ 0x3930, 0x00 },
+	{ 0x3931, 0x00 },
+	{ 0x3933, 0x00 },
+	{ 0x3934, 0x00 },
+	{ 0x3935, 0x00 },
+	{ 0x3936, 0x00 },
+	{ 0x3937, 0x00 },
+	{ 0x30ac, 0x00 },
+};
+
+static const struct imx319_reg mode_1640x1232_regs[] = {
+	{ 0x0112, 0x0a },
+	{ 0x0113, 0x0a },
+	{ 0x0114, 0x03 },
+	{ 0x0342, 0x08 },
+	{ 0x0343, 0x20 },
+	{ 0x0340, 0x18 },
+	{ 0x0341, 0x2a },
+	{ 0x0344, 0x00 },
+	{ 0x0345, 0x00 },
+	{ 0x0346, 0x00 },
+	{ 0x0347, 0x00 },
+	{ 0x0348, 0x0c },
+	{ 0x0349, 0xcf },
+	{ 0x034a, 0x09 },
+	{ 0x034b, 0x9f },
+	{ 0x0220, 0x00 },
+	{ 0x0221, 0x11 },
+	{ 0x0381, 0x01 },
+	{ 0x0383, 0x01 },
+	{ 0x0385, 0x01 },
+	{ 0x0387, 0x01 },
+	{ 0x0900, 0x01 },
+	{ 0x0901, 0x22 },
+	{ 0x0902, 0x0a },
+	{ 0x3140, 0x02 },
+	{ 0x3141, 0x00 },
+	{ 0x3f0d, 0x0a },
+	{ 0x3f14, 0x01 },
+	{ 0x3f3c, 0x02 },
+	{ 0x3f4d, 0x01 },
+	{ 0x3f4c, 0x01 },
+	{ 0x4254, 0x7f },
+	{ 0x0401, 0x00 },
+	{ 0x0404, 0x00 },
+	{ 0x0405, 0x10 },
+	{ 0x0408, 0x00 },
+	{ 0x0409, 0x00 },
+	{ 0x040a, 0x00 },
+	{ 0x040b, 0x00 },
+	{ 0x040c, 0x06 },
+	{ 0x040d, 0x68 },
+	{ 0x040e, 0x04 },
+	{ 0x040f, 0xd0 },
+	{ 0x034c, 0x06 },
+	{ 0x034d, 0x68 },
+	{ 0x034e, 0x04 },
+	{ 0x034f, 0xd0 },
+	{ 0x3261, 0x00 },
+	{ 0x3264, 0x00 },
+	{ 0x3265, 0x10 },
+	{ 0x0301, 0x05 },
+	{ 0x0303, 0x04 },
+	{ 0x0305, 0x04 },
+	{ 0x0306, 0x01 },
+	{ 0x0307, 0x92 },
+	{ 0x0309, 0x0a },
+	{ 0x030b, 0x02 },
+	{ 0x030d, 0x02 },
+	{ 0x030e, 0x00 },
+	{ 0x030f, 0xfa },
+	{ 0x0310, 0x00 },
+	{ 0x0820, 0x0f },
+	{ 0x0821, 0x13 },
+	{ 0x0822, 0x33 },
+	{ 0x0823, 0x33 },
+	{ 0x3e20, 0x01 },
+	{ 0x3e37, 0x00 },
+	{ 0x3e3b, 0x01 },
+	{ 0x38a3, 0x01 },
+	{ 0x38a8, 0x00 },
+	{ 0x38a9, 0x00 },
+	{ 0x38aa, 0x00 },
+	{ 0x38ab, 0x00 },
+	{ 0x3234, 0x00 },
+	{ 0x3fc1, 0x00 },
+	{ 0x3235, 0x00 },
+	{ 0x3802, 0x00 },
+	{ 0x3143, 0x04 },
+	{ 0x360a, 0x00 },
+	{ 0x0b00, 0x00 },
+	{ 0x0106, 0x00 },
+	{ 0x0b05, 0x01 },
+	{ 0x0b06, 0x01 },
+	{ 0x3230, 0x00 },
+	{ 0x3602, 0x01 },
+	{ 0x3607, 0x01 },
+	{ 0x3c00, 0x00 },
+	{ 0x3c01, 0xba },
+	{ 0x3c02, 0xc8 },
+	{ 0x3c03, 0xaa },
+	{ 0x3c04, 0x91 },
+	{ 0x3c05, 0x54 },
+	{ 0x3c06, 0x26 },
+	{ 0x3c07, 0x20 },
+	{ 0x3c08, 0x51 },
+	{ 0x3d80, 0x00 },
+	{ 0x3f50, 0x00 },
+	{ 0x3f56, 0x00 },
+	{ 0x3f57, 0x30 },
+	{ 0x3f78, 0x00 },
+	{ 0x3f79, 0x34 },
+	{ 0x3f7c, 0x00 },
+	{ 0x3f7d, 0x00 },
+	{ 0x3fba, 0x00 },
+	{ 0x3fbb, 0x00 },
+	{ 0xa081, 0x04 },
+	{ 0xe014, 0x00 },
+	{ 0x0202, 0x04 },
+	{ 0x0203, 0xf6 },
+	{ 0x0224, 0x01 },
+	{ 0x0225, 0xf4 },
+	{ 0x0204, 0x00 },
+	{ 0x0205, 0x00 },
+	{ 0x0216, 0x00 },
+	{ 0x0217, 0x00 },
+	{ 0x020e, 0x01 },
+	{ 0x020f, 0x00 },
+	{ 0x0210, 0x01 },
+	{ 0x0211, 0x00 },
+	{ 0x0212, 0x01 },
+	{ 0x0213, 0x00 },
+	{ 0x0214, 0x01 },
+	{ 0x0215, 0x00 },
+	{ 0x0218, 0x01 },
+	{ 0x0219, 0x00 },
+	{ 0x3614, 0x00 },
+	{ 0x3616, 0x0d },
+	{ 0x3617, 0x56 },
+	{ 0xb612, 0x20 },
+	{ 0xb613, 0x20 },
+	{ 0xb614, 0x20 },
+	{ 0xb615, 0x20 },
+	{ 0xb616, 0x0a },
+	{ 0xb617, 0x0a },
+	{ 0xb618, 0x20 },
+	{ 0xb619, 0x20 },
+	{ 0xb61a, 0x20 },
+	{ 0xb61b, 0x20 },
+	{ 0xb61c, 0x0a },
+	{ 0xb61d, 0x0a },
+	{ 0xb666, 0x30 },
+	{ 0xb667, 0x30 },
+	{ 0xb668, 0x30 },
+	{ 0xb669, 0x30 },
+	{ 0xb66a, 0x14 },
+	{ 0xb66b, 0x14 },
+	{ 0xb66c, 0x20 },
+	{ 0xb66d, 0x20 },
+	{ 0xb66e, 0x20 },
+	{ 0xb66f, 0x20 },
+	{ 0xb670, 0x10 },
+	{ 0xb671, 0x10 },
+	{ 0x3237, 0x00 },
+	{ 0x3900, 0x00 },
+	{ 0x3901, 0x00 },
+	{ 0x3902, 0x00 },
+	{ 0x3904, 0x00 },
+	{ 0x3905, 0x00 },
+	{ 0x3906, 0x00 },
+	{ 0x3907, 0x00 },
+	{ 0x3908, 0x00 },
+	{ 0x3909, 0x00 },
+	{ 0x3912, 0x00 },
+	{ 0x3930, 0x00 },
+	{ 0x3931, 0x00 },
+	{ 0x3933, 0x00 },
+	{ 0x3934, 0x00 },
+	{ 0x3935, 0x00 },
+	{ 0x3936, 0x00 },
+	{ 0x3937, 0x00 },
+	{ 0x30ac, 0x00 },
+};
+
+static const struct imx319_reg mode_1640x922_regs[] = {
+	{ 0x0112, 0x0a },
+	{ 0x0113, 0x0a },
+	{ 0x0114, 0x03 },
+	{ 0x0342, 0x08 },
+	{ 0x0343, 0x20 },
+	{ 0x0340, 0x18 },
+	{ 0x0341, 0x2a },
+	{ 0x0344, 0x00 },
+	{ 0x0345, 0x00 },
+	{ 0x0346, 0x01 },
+	{ 0x0347, 0x30 },
+	{ 0x0348, 0x0c },
+	{ 0x0349, 0xcf },
+	{ 0x034a, 0x08 },
+	{ 0x034b, 0x6f },
+	{ 0x0220, 0x00 },
+	{ 0x0221, 0x11 },
+	{ 0x0381, 0x01 },
+	{ 0x0383, 0x01 },
+	{ 0x0385, 0x01 },
+	{ 0x0387, 0x01 },
+	{ 0x0900, 0x01 },
+	{ 0x0901, 0x22 },
+	{ 0x0902, 0x0a },
+	{ 0x3140, 0x02 },
+	{ 0x3141, 0x00 },
+	{ 0x3f0d, 0x0a },
+	{ 0x3f14, 0x01 },
+	{ 0x3f3c, 0x02 },
+	{ 0x3f4d, 0x01 },
+	{ 0x3f4c, 0x01 },
+	{ 0x4254, 0x7f },
+	{ 0x0401, 0x00 },
+	{ 0x0404, 0x00 },
+	{ 0x0405, 0x10 },
+	{ 0x0408, 0x00 },
+	{ 0x0409, 0x00 },
+	{ 0x040a, 0x00 },
+	{ 0x040b, 0x02 },
+	{ 0x040c, 0x06 },
+	{ 0x040d, 0x68 },
+	{ 0x040e, 0x03 },
+	{ 0x040f, 0x9a },
+	{ 0x034c, 0x06 },
+	{ 0x034d, 0x68 },
+	{ 0x034e, 0x03 },
+	{ 0x034f, 0x9a },
+	{ 0x3261, 0x00 },
+	{ 0x3264, 0x00 },
+	{ 0x3265, 0x10 },
+	{ 0x0301, 0x05 },
+	{ 0x0303, 0x04 },
+	{ 0x0305, 0x04 },
+	{ 0x0306, 0x01 },
+	{ 0x0307, 0x92 },
+	{ 0x0309, 0x0a },
+	{ 0x030b, 0x02 },
+	{ 0x030d, 0x02 },
+	{ 0x030e, 0x00 },
+	{ 0x030f, 0xfa },
+	{ 0x0310, 0x00 },
+	{ 0x0820, 0x0f },
+	{ 0x0821, 0x13 },
+	{ 0x0822, 0x33 },
+	{ 0x0823, 0x33 },
+	{ 0x3e20, 0x01 },
+	{ 0x3e37, 0x00 },
+	{ 0x3e3b, 0x01 },
+	{ 0x38a3, 0x01 },
+	{ 0x38a8, 0x00 },
+	{ 0x38a9, 0x00 },
+	{ 0x38aa, 0x00 },
+	{ 0x38ab, 0x00 },
+	{ 0x3234, 0x00 },
+	{ 0x3fc1, 0x00 },
+	{ 0x3235, 0x00 },
+	{ 0x3802, 0x00 },
+	{ 0x3143, 0x04 },
+	{ 0x360a, 0x00 },
+	{ 0x0b00, 0x00 },
+	{ 0x0106, 0x00 },
+	{ 0x0b05, 0x01 },
+	{ 0x0b06, 0x01 },
+	{ 0x3230, 0x00 },
+	{ 0x3602, 0x01 },
+	{ 0x3607, 0x01 },
+	{ 0x3c00, 0x00 },
+	{ 0x3c01, 0xba },
+	{ 0x3c02, 0xc8 },
+	{ 0x3c03, 0xaa },
+	{ 0x3c04, 0x91 },
+	{ 0x3c05, 0x54 },
+	{ 0x3c06, 0x26 },
+	{ 0x3c07, 0x20 },
+	{ 0x3c08, 0x51 },
+	{ 0x3d80, 0x00 },
+	{ 0x3f50, 0x00 },
+	{ 0x3f56, 0x00 },
+	{ 0x3f57, 0x30 },
+	{ 0x3f78, 0x00 },
+	{ 0x3f79, 0x34 },
+	{ 0x3f7c, 0x00 },
+	{ 0x3f7d, 0x00 },
+	{ 0x3fba, 0x00 },
+	{ 0x3fbb, 0x00 },
+	{ 0xa081, 0x04 },
+	{ 0xe014, 0x00 },
+	{ 0x0202, 0x04 },
+	{ 0x0203, 0xf6 },
+	{ 0x0224, 0x01 },
+	{ 0x0225, 0xf4 },
+	{ 0x0204, 0x00 },
+	{ 0x0205, 0x00 },
+	{ 0x0216, 0x00 },
+	{ 0x0217, 0x00 },
+	{ 0x020e, 0x01 },
+	{ 0x020f, 0x00 },
+	{ 0x0210, 0x01 },
+	{ 0x0211, 0x00 },
+	{ 0x0212, 0x01 },
+	{ 0x0213, 0x00 },
+	{ 0x0214, 0x01 },
+	{ 0x0215, 0x00 },
+	{ 0x0218, 0x01 },
+	{ 0x0219, 0x00 },
+	{ 0x3614, 0x00 },
+	{ 0x3616, 0x0d },
+	{ 0x3617, 0x56 },
+	{ 0xb612, 0x20 },
+	{ 0xb613, 0x20 },
+	{ 0xb614, 0x20 },
+	{ 0xb615, 0x20 },
+	{ 0xb616, 0x0a },
+	{ 0xb617, 0x0a },
+	{ 0xb618, 0x20 },
+	{ 0xb619, 0x20 },
+	{ 0xb61a, 0x20 },
+	{ 0xb61b, 0x20 },
+	{ 0xb61c, 0x0a },
+	{ 0xb61d, 0x0a },
+	{ 0xb666, 0x30 },
+	{ 0xb667, 0x30 },
+	{ 0xb668, 0x30 },
+	{ 0xb669, 0x30 },
+	{ 0xb66a, 0x14 },
+	{ 0xb66b, 0x14 },
+	{ 0xb66c, 0x20 },
+	{ 0xb66d, 0x20 },
+	{ 0xb66e, 0x20 },
+	{ 0xb66f, 0x20 },
+	{ 0xb670, 0x10 },
+	{ 0xb671, 0x10 },
+	{ 0x3237, 0x00 },
+	{ 0x3900, 0x00 },
+	{ 0x3901, 0x00 },
+	{ 0x3902, 0x00 },
+	{ 0x3904, 0x00 },
+	{ 0x3905, 0x00 },
+	{ 0x3906, 0x00 },
+	{ 0x3907, 0x00 },
+	{ 0x3908, 0x00 },
+	{ 0x3909, 0x00 },
+	{ 0x3912, 0x00 },
+	{ 0x3930, 0x00 },
+	{ 0x3931, 0x00 },
+	{ 0x3933, 0x00 },
+	{ 0x3934, 0x00 },
+	{ 0x3935, 0x00 },
+	{ 0x3936, 0x00 },
+	{ 0x3937, 0x00 },
+	{ 0x30ac, 0x00 },
+};
+
+static const struct imx319_reg mode_1296x736_regs[] = {
+	{ 0x0112, 0x0a },
+	{ 0x0113, 0x0a },
+	{ 0x0114, 0x03 },
+	{ 0x0342, 0x08 },
+	{ 0x0343, 0x20 },
+	{ 0x0340, 0x18 },
+	{ 0x0341, 0x2a },
+	{ 0x0344, 0x00 },
+	{ 0x0345, 0x00 },
+	{ 0x0346, 0x01 },
+	{ 0x0347, 0xf0 },
+	{ 0x0348, 0x0c },
+	{ 0x0349, 0xcf },
+	{ 0x034a, 0x07 },
+	{ 0x034b, 0xaf },
+	{ 0x0220, 0x00 },
+	{ 0x0221, 0x11 },
+	{ 0x0381, 0x01 },
+	{ 0x0383, 0x01 },
+	{ 0x0385, 0x01 },
+	{ 0x0387, 0x01 },
+	{ 0x0900, 0x01 },
+	{ 0x0901, 0x22 },
+	{ 0x0902, 0x0a },
+	{ 0x3140, 0x02 },
+	{ 0x3141, 0x00 },
+	{ 0x3f0d, 0x0a },
+	{ 0x3f14, 0x01 },
+	{ 0x3f3c, 0x02 },
+	{ 0x3f4d, 0x01 },
+	{ 0x3f4c, 0x01 },
+	{ 0x4254, 0x7f },
+	{ 0x0401, 0x00 },
+	{ 0x0404, 0x00 },
+	{ 0x0405, 0x10 },
+	{ 0x0408, 0x00 },
+	{ 0x0409, 0xac },
+	{ 0x040a, 0x00 },
+	{ 0x040b, 0x00 },
+	{ 0x040c, 0x05 },
+	{ 0x040d, 0x10 },
+	{ 0x040e, 0x02 },
+	{ 0x040f, 0xe0 },
+	{ 0x034c, 0x05 },
+	{ 0x034d, 0x10 },
+	{ 0x034e, 0x02 },
+	{ 0x034f, 0xe0 },
+	{ 0x3261, 0x00 },
+	{ 0x3264, 0x00 },
+	{ 0x3265, 0x10 },
+	{ 0x0301, 0x05 },
+	{ 0x0303, 0x04 },
+	{ 0x0305, 0x04 },
+	{ 0x0306, 0x01 },
+	{ 0x0307, 0x92 },
+	{ 0x0309, 0x0a },
+	{ 0x030b, 0x02 },
+	{ 0x030d, 0x02 },
+	{ 0x030e, 0x00 },
+	{ 0x030f, 0xfa },
+	{ 0x0310, 0x00 },
+	{ 0x0820, 0x0f },
+	{ 0x0821, 0x13 },
+	{ 0x0822, 0x33 },
+	{ 0x0823, 0x33 },
+	{ 0x3e20, 0x01 },
+	{ 0x3e37, 0x00 },
+	{ 0x3e3b, 0x01 },
+	{ 0x38a3, 0x01 },
+	{ 0x38a8, 0x00 },
+	{ 0x38a9, 0x00 },
+	{ 0x38aa, 0x00 },
+	{ 0x38ab, 0x00 },
+	{ 0x3234, 0x00 },
+	{ 0x3fc1, 0x00 },
+	{ 0x3235, 0x00 },
+	{ 0x3802, 0x00 },
+	{ 0x3143, 0x04 },
+	{ 0x360a, 0x00 },
+	{ 0x0b00, 0x00 },
+	{ 0x0106, 0x00 },
+	{ 0x0b05, 0x01 },
+	{ 0x0b06, 0x01 },
+	{ 0x3230, 0x00 },
+	{ 0x3602, 0x01 },
+	{ 0x3607, 0x01 },
+	{ 0x3c00, 0x00 },
+	{ 0x3c01, 0xba },
+	{ 0x3c02, 0xc8 },
+	{ 0x3c03, 0xaa },
+	{ 0x3c04, 0x91 },
+	{ 0x3c05, 0x54 },
+	{ 0x3c06, 0x26 },
+	{ 0x3c07, 0x20 },
+	{ 0x3c08, 0x51 },
+	{ 0x3d80, 0x00 },
+	{ 0x3f50, 0x00 },
+	{ 0x3f56, 0x00 },
+	{ 0x3f57, 0x30 },
+	{ 0x3f78, 0x00 },
+	{ 0x3f79, 0x34 },
+	{ 0x3f7c, 0x00 },
+	{ 0x3f7d, 0x00 },
+	{ 0x3fba, 0x00 },
+	{ 0x3fbb, 0x00 },
+	{ 0xa081, 0x04 },
+	{ 0xe014, 0x00 },
+	{ 0x0202, 0x04 },
+	{ 0x0203, 0xf6 },
+	{ 0x0224, 0x01 },
+	{ 0x0225, 0xf4 },
+	{ 0x0204, 0x00 },
+	{ 0x0205, 0x00 },
+	{ 0x0216, 0x00 },
+	{ 0x0217, 0x00 },
+	{ 0x020e, 0x01 },
+	{ 0x020f, 0x00 },
+	{ 0x0210, 0x01 },
+	{ 0x0211, 0x00 },
+	{ 0x0212, 0x01 },
+	{ 0x0213, 0x00 },
+	{ 0x0214, 0x01 },
+	{ 0x0215, 0x00 },
+	{ 0x0218, 0x01 },
+	{ 0x0219, 0x00 },
+	{ 0x3614, 0x00 },
+	{ 0x3616, 0x0d },
+	{ 0x3617, 0x56 },
+	{ 0xb612, 0x20 },
+	{ 0xb613, 0x20 },
+	{ 0xb614, 0x20 },
+	{ 0xb615, 0x20 },
+	{ 0xb616, 0x0a },
+	{ 0xb617, 0x0a },
+	{ 0xb618, 0x20 },
+	{ 0xb619, 0x20 },
+	{ 0xb61a, 0x20 },
+	{ 0xb61b, 0x20 },
+	{ 0xb61c, 0x0a },
+	{ 0xb61d, 0x0a },
+	{ 0xb666, 0x30 },
+	{ 0xb667, 0x30 },
+	{ 0xb668, 0x30 },
+	{ 0xb669, 0x30 },
+	{ 0xb66a, 0x14 },
+	{ 0xb66b, 0x14 },
+	{ 0xb66c, 0x20 },
+	{ 0xb66d, 0x20 },
+	{ 0xb66e, 0x20 },
+	{ 0xb66f, 0x20 },
+	{ 0xb670, 0x10 },
+	{ 0xb671, 0x10 },
+	{ 0x3237, 0x00 },
+	{ 0x3900, 0x00 },
+	{ 0x3901, 0x00 },
+	{ 0x3902, 0x00 },
+	{ 0x3904, 0x00 },
+	{ 0x3905, 0x00 },
+	{ 0x3906, 0x00 },
+	{ 0x3907, 0x00 },
+	{ 0x3908, 0x00 },
+	{ 0x3909, 0x00 },
+	{ 0x3912, 0x00 },
+	{ 0x3930, 0x00 },
+	{ 0x3931, 0x00 },
+	{ 0x3933, 0x00 },
+	{ 0x3934, 0x00 },
+	{ 0x3935, 0x00 },
+	{ 0x3936, 0x00 },
+	{ 0x3937, 0x00 },
+	{ 0x30ac, 0x00 },
+};
+
+static const struct imx319_reg mode_1280x720_regs[] = {
+	{ 0x0112, 0x0a },
+	{ 0x0113, 0x0a },
+	{ 0x0114, 0x03 },
+	{ 0x0342, 0x08 },
+	{ 0x0343, 0x20 },
+	{ 0x0340, 0x18 },
+	{ 0x0341, 0x2a },
+	{ 0x0344, 0x00 },
+	{ 0x0345, 0x00 },
+	{ 0x0346, 0x02 },
+	{ 0x0347, 0x00 },
+	{ 0x0348, 0x0c },
+	{ 0x0349, 0xcf },
+	{ 0x034a, 0x07 },
+	{ 0x034b, 0x9f },
+	{ 0x0220, 0x00 },
+	{ 0x0221, 0x11 },
+	{ 0x0381, 0x01 },
+	{ 0x0383, 0x01 },
+	{ 0x0385, 0x01 },
+	{ 0x0387, 0x01 },
+	{ 0x0900, 0x01 },
+	{ 0x0901, 0x22 },
+	{ 0x0902, 0x0a },
+	{ 0x3140, 0x02 },
+	{ 0x3141, 0x00 },
+	{ 0x3f0d, 0x0a },
+	{ 0x3f14, 0x01 },
+	{ 0x3f3c, 0x02 },
+	{ 0x3f4d, 0x01 },
+	{ 0x3f4c, 0x01 },
+	{ 0x4254, 0x7f },
+	{ 0x0401, 0x00 },
+	{ 0x0404, 0x00 },
+	{ 0x0405, 0x10 },
+	{ 0x0408, 0x00 },
+	{ 0x0409, 0xb4 },
+	{ 0x040a, 0x00 },
+	{ 0x040b, 0x00 },
+	{ 0x040c, 0x05 },
+	{ 0x040d, 0x00 },
+	{ 0x040e, 0x02 },
+	{ 0x040f, 0xd0 },
+	{ 0x034c, 0x05 },
+	{ 0x034d, 0x00 },
+	{ 0x034e, 0x02 },
+	{ 0x034f, 0xd0 },
+	{ 0x3261, 0x00 },
+	{ 0x3264, 0x00 },
+	{ 0x3265, 0x10 },
+	{ 0x0301, 0x05 },
+	{ 0x0303, 0x04 },
+	{ 0x0305, 0x04 },
+	{ 0x0306, 0x01 },
+	{ 0x0307, 0x92 },
+	{ 0x0309, 0x0a },
+	{ 0x030b, 0x02 },
+	{ 0x030d, 0x02 },
+	{ 0x030e, 0x00 },
+	{ 0x030f, 0xfa },
+	{ 0x0310, 0x00 },
+	{ 0x0820, 0x0f },
+	{ 0x0821, 0x13 },
+	{ 0x0822, 0x33 },
+	{ 0x0823, 0x33 },
+	{ 0x3e20, 0x01 },
+	{ 0x3e37, 0x00 },
+	{ 0x3e3b, 0x01 },
+	{ 0x38a3, 0x01 },
+	{ 0x38a8, 0x00 },
+	{ 0x38a9, 0x00 },
+	{ 0x38aa, 0x00 },
+	{ 0x38ab, 0x00 },
+	{ 0x3234, 0x00 },
+	{ 0x3fc1, 0x00 },
+	{ 0x3235, 0x00 },
+	{ 0x3802, 0x00 },
+	{ 0x3143, 0x04 },
+	{ 0x360a, 0x00 },
+	{ 0x0b00, 0x00 },
+	{ 0x0106, 0x00 },
+	{ 0x0b05, 0x01 },
+	{ 0x0b06, 0x01 },
+	{ 0x3230, 0x00 },
+	{ 0x3602, 0x01 },
+	{ 0x3607, 0x01 },
+	{ 0x3c00, 0x00 },
+	{ 0x3c01, 0xba },
+	{ 0x3c02, 0xc8 },
+	{ 0x3c03, 0xaa },
+	{ 0x3c04, 0x91 },
+	{ 0x3c05, 0x54 },
+	{ 0x3c06, 0x26 },
+	{ 0x3c07, 0x20 },
+	{ 0x3c08, 0x51 },
+	{ 0x3d80, 0x00 },
+	{ 0x3f50, 0x00 },
+	{ 0x3f56, 0x00 },
+	{ 0x3f57, 0x30 },
+	{ 0x3f78, 0x00 },
+	{ 0x3f79, 0x34 },
+	{ 0x3f7c, 0x00 },
+	{ 0x3f7d, 0x00 },
+	{ 0x3fba, 0x00 },
+	{ 0x3fbb, 0x00 },
+	{ 0xa081, 0x04 },
+	{ 0xe014, 0x00 },
+	{ 0x0202, 0x04 },
+	{ 0x0203, 0xf6 },
+	{ 0x0224, 0x01 },
+	{ 0x0225, 0xf4 },
+	{ 0x0204, 0x00 },
+	{ 0x0205, 0x00 },
+	{ 0x0216, 0x00 },
+	{ 0x0217, 0x00 },
+	{ 0x020e, 0x01 },
+	{ 0x020f, 0x00 },
+	{ 0x0210, 0x01 },
+	{ 0x0211, 0x00 },
+	{ 0x0212, 0x01 },
+	{ 0x0213, 0x00 },
+	{ 0x0214, 0x01 },
+	{ 0x0215, 0x00 },
+	{ 0x0218, 0x01 },
+	{ 0x0219, 0x00 },
+	{ 0x3614, 0x00 },
+	{ 0x3616, 0x0d },
+	{ 0x3617, 0x56 },
+	{ 0xb612, 0x20 },
+	{ 0xb613, 0x20 },
+	{ 0xb614, 0x20 },
+	{ 0xb615, 0x20 },
+	{ 0xb616, 0x0a },
+	{ 0xb617, 0x0a },
+	{ 0xb618, 0x20 },
+	{ 0xb619, 0x20 },
+	{ 0xb61a, 0x20 },
+	{ 0xb61b, 0x20 },
+	{ 0xb61c, 0x0a },
+	{ 0xb61d, 0x0a },
+	{ 0xb666, 0x30 },
+	{ 0xb667, 0x30 },
+	{ 0xb668, 0x30 },
+	{ 0xb669, 0x30 },
+	{ 0xb66a, 0x14 },
+	{ 0xb66b, 0x14 },
+	{ 0xb66c, 0x20 },
+	{ 0xb66d, 0x20 },
+	{ 0xb66e, 0x20 },
+	{ 0xb66f, 0x20 },
+	{ 0xb670, 0x10 },
+	{ 0xb671, 0x10 },
+	{ 0x3237, 0x00 },
+	{ 0x3900, 0x00 },
+	{ 0x3901, 0x00 },
+	{ 0x3902, 0x00 },
+	{ 0x3904, 0x00 },
+	{ 0x3905, 0x00 },
+	{ 0x3906, 0x00 },
+	{ 0x3907, 0x00 },
+	{ 0x3908, 0x00 },
+	{ 0x3909, 0x00 },
+	{ 0x3912, 0x00 },
+	{ 0x3930, 0x00 },
+	{ 0x3931, 0x00 },
+	{ 0x3933, 0x00 },
+	{ 0x3934, 0x00 },
+	{ 0x3935, 0x00 },
+	{ 0x3936, 0x00 },
+	{ 0x3937, 0x00 },
+	{ 0x30ac, 0x00 },
+};
+
+static const char * const imx319_test_pattern_menu[] = {
+	"Disabled",
+	"Solid Colour",
+	"Eight Vertical Colour Bars",
+	"Colour Bars With Fade to Grey",
+	"Pseudorandom Sequence (PN9)",
+};
+
+/* supported link frequencies */
+static const s64 link_freq_menu_items[] = {
+	IMX319_LINK_FREQ_DEFAULT,
+};
+
+/* Mode configs */
+static const struct imx319_mode supported_modes[] = {
+	{
+		.width = 3280,
+		.height = 2464,
+		.fll_def = 3242,
+		.fll_min = 3242,
+		.llp = 3968,
+		.link_freq_index = IMX319_LINK_FREQ_INDEX,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_3280x2464_regs),
+			.regs = mode_3280x2464_regs,
+		},
+	},
+	{
+		.width = 3264,
+		.height = 2448,
+		.fll_def = 3242,
+		.fll_min = 3242,
+		.llp = 3968,
+		.link_freq_index = IMX319_LINK_FREQ_INDEX,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_3264x2448_regs),
+			.regs = mode_3264x2448_regs,
+		},
+	},
+	{
+		.width = 1936,
+		.height = 1096,
+		.fll_def = 3242,
+		.fll_min = 3242,
+		.llp = 3968,
+		.link_freq_index = IMX319_LINK_FREQ_INDEX,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1936x1096_regs),
+			.regs = mode_1936x1096_regs,
+		},
+	},
+	{
+		.width = 1920,
+		.height = 1080,
+		.fll_def = 3242,
+		.fll_min = 3242,
+		.llp = 3968,
+		.link_freq_index = IMX319_LINK_FREQ_INDEX,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1920x1080_regs),
+			.regs = mode_1920x1080_regs,
+		},
+	},
+	{
+		.width = 1640,
+		.height = 1232,
+		.fll_def = 5146,
+		.fll_min = 5146,
+		.llp = 2500,
+		.link_freq_index = IMX319_LINK_FREQ_INDEX,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1640x1232_regs),
+			.regs = mode_1640x1232_regs,
+		},
+	},
+	{
+		.width = 1640,
+		.height = 922,
+		.fll_def = 5146,
+		.fll_min = 5146,
+		.llp = 2500,
+		.link_freq_index = IMX319_LINK_FREQ_INDEX,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1640x922_regs),
+			.regs = mode_1640x922_regs,
+		},
+	},
+	{
+		.width = 1296,
+		.height = 736,
+		.fll_def = 5146,
+		.fll_min = 5146,
+		.llp = 2500,
+		.link_freq_index = IMX319_LINK_FREQ_INDEX,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1296x736_regs),
+			.regs = mode_1296x736_regs,
+		},
+	},
+	{
+		.width = 1280,
+		.height = 720,
+		.fll_def = 5146,
+		.fll_min = 5146,
+		.llp = 2500,
+		.link_freq_index = IMX319_LINK_FREQ_INDEX,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1280x720_regs),
+			.regs = mode_1280x720_regs,
+		},
+	},
+};
+
+static inline struct imx319 *to_imx319(struct v4l2_subdev *_sd)
+{
+	return container_of(_sd, struct imx319, sd);
+}
+
+/* Get bayer order based on flip setting. */
+static u32 imx319_get_format_code(struct imx319 *imx319)
+{
+	/*
+	 * Only one bayer order is supported.
+	 * It depends on the flip settings.
+	 */
+	u32 code;
+	static const u32 codes[2][2] = {
+		{ MEDIA_BUS_FMT_SRGGB10_1X10, MEDIA_BUS_FMT_SGRBG10_1X10, },
+		{ MEDIA_BUS_FMT_SGBRG10_1X10, MEDIA_BUS_FMT_SBGGR10_1X10, },
+	};
+
+	lockdep_assert_held(&imx319->mutex);
+	code = codes[imx319->vflip->val][imx319->hflip->val];
+
+	return code;
+}
+
+/* Read registers up to 4 at a time */
+static int imx319_read_reg(struct imx319 *imx319, u16 reg, u32 len, u32 *val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx319->sd);
+	struct i2c_msg msgs[2];
+	u8 addr_buf[2];
+	u8 data_buf[4] = { 0 };
+	int ret;
+
+	if (len > 4)
+		return -EINVAL;
+
+	put_unaligned_be16(reg, addr_buf);
+	/* Write register address */
+	msgs[0].addr = client->addr;
+	msgs[0].flags = 0;
+	msgs[0].len = ARRAY_SIZE(addr_buf);
+	msgs[0].buf = addr_buf;
+
+	/* Read data from register */
+	msgs[1].addr = client->addr;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].len = len;
+	msgs[1].buf = &data_buf[4 - len];
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret != ARRAY_SIZE(msgs))
+		return -EIO;
+
+	*val = get_unaligned_be32(data_buf);
+
+	return 0;
+}
+
+/* Write registers up to 4 at a time */
+static int imx319_write_reg(struct imx319 *imx319, u16 reg, u32 len, u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx319->sd);
+	u8 buf[6];
+
+	if (len > 4)
+		return -EINVAL;
+
+	put_unaligned_be16(reg, buf);
+	put_unaligned_be32(val << (8 * (4 - len)), buf + 2);
+	if (i2c_master_send(client, buf, len + 2) != len + 2)
+		return -EIO;
+
+	return 0;
+}
+
+/* Write a list of registers */
+static int imx319_write_regs(struct imx319 *imx319,
+			     const struct imx319_reg *regs, u32 len)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx319->sd);
+	int ret;
+	u32 i;
+
+	for (i = 0; i < len; i++) {
+		ret = imx319_write_reg(imx319, regs[i].address, 1, regs[i].val);
+		if (ret) {
+			dev_err_ratelimited(&client->dev,
+					    "write reg 0x%4.4x return err %d",
+					    regs[i].address, ret);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+/* Open sub-device */
+static int imx319_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct imx319 *imx319 = to_imx319(sd);
+	struct v4l2_mbus_framefmt *try_fmt =
+		v4l2_subdev_get_try_format(sd, fh->state, 0);
+
+	mutex_lock(&imx319->mutex);
+
+	/* Initialize try_fmt */
+	try_fmt->width = imx319->cur_mode->width;
+	try_fmt->height = imx319->cur_mode->height;
+	try_fmt->code = imx319_get_format_code(imx319);
+	try_fmt->field = V4L2_FIELD_NONE;
+
+	mutex_unlock(&imx319->mutex);
+
+	return 0;
+}
+
+static int imx319_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct imx319 *imx319 = container_of(ctrl->handler,
+					     struct imx319, ctrl_handler);
+	struct i2c_client *client = v4l2_get_subdevdata(&imx319->sd);
+	s64 max;
+	int ret;
+
+	/* Propagate change of current control to all related controls */
+	switch (ctrl->id) {
+	case V4L2_CID_VBLANK:
+		/* Update max exposure while meeting expected vblanking */
+		max = imx319->cur_mode->height + ctrl->val - 18;
+		__v4l2_ctrl_modify_range(imx319->exposure,
+					 imx319->exposure->minimum,
+					 max, imx319->exposure->step, max);
+		break;
+	}
+
+	/*
+	 * Applying V4L2 control value only happens
+	 * when power is up for streaming
+	 */
+	if (!pm_runtime_get_if_in_use(&client->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_ANALOGUE_GAIN:
+		/* Analog gain = 1024/(1024 - ctrl->val) times */
+		ret = imx319_write_reg(imx319, IMX319_REG_ANALOG_GAIN, 2,
+				       ctrl->val);
+		break;
+	case V4L2_CID_DIGITAL_GAIN:
+		ret = imx319_write_reg(imx319, IMX319_REG_DIG_GAIN_GLOBAL, 2,
+				       ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE:
+		ret = imx319_write_reg(imx319, IMX319_REG_EXPOSURE, 2,
+				       ctrl->val);
+		break;
+	case V4L2_CID_VBLANK:
+		/* Update FLL that meets expected vertical blanking */
+		ret = imx319_write_reg(imx319, IMX319_REG_FLL, 2,
+				       imx319->cur_mode->height + ctrl->val);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		ret = imx319_write_reg(imx319, IMX319_REG_TEST_PATTERN,
+				       2, ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+	case V4L2_CID_VFLIP:
+		ret = imx319_write_reg(imx319, IMX319_REG_ORIENTATION, 1,
+				       imx319->hflip->val |
+				       imx319->vflip->val << 1);
+		break;
+	default:
+		ret = -EINVAL;
+		dev_info(&client->dev, "ctrl(id:0x%x,val:0x%x) is not handled",
+			 ctrl->id, ctrl->val);
+		break;
+	}
+
+	pm_runtime_put(&client->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops imx319_ctrl_ops = {
+	.s_ctrl = imx319_set_ctrl,
+};
+
+static int imx319_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct imx319 *imx319 = to_imx319(sd);
+
+	if (code->index > 0)
+		return -EINVAL;
+
+	mutex_lock(&imx319->mutex);
+	code->code = imx319_get_format_code(imx319);
+	mutex_unlock(&imx319->mutex);
+
+	return 0;
+}
+
+static int imx319_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct imx319 *imx319 = to_imx319(sd);
+
+	if (fse->index >= ARRAY_SIZE(supported_modes))
+		return -EINVAL;
+
+	mutex_lock(&imx319->mutex);
+	if (fse->code != imx319_get_format_code(imx319)) {
+		mutex_unlock(&imx319->mutex);
+		return -EINVAL;
+	}
+	mutex_unlock(&imx319->mutex);
+
+	fse->min_width = supported_modes[fse->index].width;
+	fse->max_width = fse->min_width;
+	fse->min_height = supported_modes[fse->index].height;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static void imx319_update_pad_format(struct imx319 *imx319,
+				     const struct imx319_mode *mode,
+				     struct v4l2_subdev_format *fmt)
+{
+	fmt->format.width = mode->width;
+	fmt->format.height = mode->height;
+	fmt->format.code = imx319_get_format_code(imx319);
+	fmt->format.field = V4L2_FIELD_NONE;
+}
+
+static int imx319_do_get_pad_format(struct imx319 *imx319,
+				    struct v4l2_subdev_state *sd_state,
+				    struct v4l2_subdev_format *fmt)
+{
+	struct v4l2_mbus_framefmt *framefmt;
+	struct v4l2_subdev *sd = &imx319->sd;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		framefmt = v4l2_subdev_get_try_format(sd, sd_state, fmt->pad);
+		fmt->format = *framefmt;
+	} else {
+		imx319_update_pad_format(imx319, imx319->cur_mode, fmt);
+	}
+
+	return 0;
+}
+
+static int imx319_get_pad_format(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_format *fmt)
+{
+	struct imx319 *imx319 = to_imx319(sd);
+	int ret;
+
+	mutex_lock(&imx319->mutex);
+	ret = imx319_do_get_pad_format(imx319, sd_state, fmt);
+	mutex_unlock(&imx319->mutex);
+
+	return ret;
+}
+
+static int
+imx319_set_pad_format(struct v4l2_subdev *sd,
+		      struct v4l2_subdev_state *sd_state,
+		      struct v4l2_subdev_format *fmt)
+{
+	struct imx319 *imx319 = to_imx319(sd);
+	const struct imx319_mode *mode;
+	struct v4l2_mbus_framefmt *framefmt;
+	s32 vblank_def;
+	s32 vblank_min;
+	s64 h_blank;
+	u64 pixel_rate;
+	u32 height;
+
+	mutex_lock(&imx319->mutex);
+
+	/*
+	 * Only one bayer order is supported.
+	 * It depends on the flip settings.
+	 */
+	fmt->format.code = imx319_get_format_code(imx319);
+
+	mode = v4l2_find_nearest_size(supported_modes,
+				      ARRAY_SIZE(supported_modes),
+				      width, height,
+				      fmt->format.width, fmt->format.height);
+	imx319_update_pad_format(imx319, mode, fmt);
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		framefmt = v4l2_subdev_get_try_format(sd, sd_state, fmt->pad);
+		*framefmt = fmt->format;
+	} else {
+		imx319->cur_mode = mode;
+		pixel_rate = imx319->link_def_freq * 2 * 4;
+		do_div(pixel_rate, 10);
+		__v4l2_ctrl_s_ctrl_int64(imx319->pixel_rate, pixel_rate);
+		/* Update limits and set FPS to default */
+		height = imx319->cur_mode->height;
+		vblank_def = imx319->cur_mode->fll_def - height;
+		vblank_min = imx319->cur_mode->fll_min - height;
+		height = IMX319_FLL_MAX - height;
+		__v4l2_ctrl_modify_range(imx319->vblank, vblank_min, height, 1,
+					 vblank_def);
+		__v4l2_ctrl_s_ctrl(imx319->vblank, vblank_def);
+		h_blank = mode->llp - imx319->cur_mode->width;
+		/*
+		 * Currently hblank is not changeable.
+		 * So FPS control is done only by vblank.
+		 */
+		__v4l2_ctrl_modify_range(imx319->hblank, h_blank,
+					 h_blank, 1, h_blank);
+	}
+
+	mutex_unlock(&imx319->mutex);
+
+	return 0;
+}
+
+/* Verify chip ID */
+static int imx319_identify_module(struct imx319 *imx319)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx319->sd);
+	int ret;
+	u32 val;
+
+	if (imx319->identified)
+		return 0;
+
+	ret = imx319_read_reg(imx319, IMX319_REG_CHIP_ID, 2, &val);
+	if (ret)
+		return ret;
+
+	if (val != IMX319_CHIP_ID) {
+		dev_err(&client->dev, "chip id mismatch: %x!=%x",
+			IMX319_CHIP_ID, val);
+		return -EIO;
+	}
+
+	imx319->identified = true;
+
+	return 0;
+}
+
+/* Start streaming */
+static int imx319_start_streaming(struct imx319 *imx319)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx319->sd);
+	const struct imx319_reg_list *reg_list;
+	int ret;
+
+	ret = imx319_identify_module(imx319);
+	if (ret)
+		return ret;
+
+	/* Global Setting */
+	reg_list = &imx319_global_setting;
+	ret = imx319_write_regs(imx319, reg_list->regs, reg_list->num_of_regs);
+	if (ret) {
+		dev_err(&client->dev, "failed to set global settings");
+		return ret;
+	}
+
+	/* Apply default values of current mode */
+	reg_list = &imx319->cur_mode->reg_list;
+	ret = imx319_write_regs(imx319, reg_list->regs, reg_list->num_of_regs);
+	if (ret) {
+		dev_err(&client->dev, "failed to set mode");
+		return ret;
+	}
+
+	/* set digital gain control to all color mode */
+	ret = imx319_write_reg(imx319, IMX319_REG_DPGA_USE_GLOBAL_GAIN, 1, 1);
+	if (ret)
+		return ret;
+
+	/* Apply customized values from user */
+	ret =  __v4l2_ctrl_handler_setup(imx319->sd.ctrl_handler);
+	if (ret)
+		return ret;
+
+	return imx319_write_reg(imx319, IMX319_REG_MODE_SELECT,
+				1, IMX319_MODE_STREAMING);
+}
+
+/* Stop streaming */
+static int imx319_stop_streaming(struct imx319 *imx319)
+{
+	return imx319_write_reg(imx319, IMX319_REG_MODE_SELECT,
+				1, IMX319_MODE_STANDBY);
+}
+
+static int imx319_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct imx319 *imx319 = to_imx319(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	mutex_lock(&imx319->mutex);
+	if (imx319->streaming == enable) {
+		mutex_unlock(&imx319->mutex);
+		return 0;
+	}
+
+	if (enable) {
+		ret = pm_runtime_resume_and_get(&client->dev);
+		if (ret < 0)
+			goto err_unlock;
+
+		/*
+		 * Apply default & customized values
+		 * and then start streaming.
+		 */
+		ret = imx319_start_streaming(imx319);
+		if (ret)
+			goto err_rpm_put;
+	} else {
+		imx319_stop_streaming(imx319);
+		pm_runtime_put(&client->dev);
+	}
+
+	imx319->streaming = enable;
+
+	/* vflip and hflip cannot change during streaming */
+	__v4l2_ctrl_grab(imx319->vflip, enable);
+	__v4l2_ctrl_grab(imx319->hflip, enable);
+
+	mutex_unlock(&imx319->mutex);
+
+	return ret;
+
+err_rpm_put:
+	pm_runtime_put(&client->dev);
+err_unlock:
+	mutex_unlock(&imx319->mutex);
+
+	return ret;
+}
+
+static int __maybe_unused imx319_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct imx319 *imx319 = to_imx319(sd);
+
+	if (imx319->streaming)
+		imx319_stop_streaming(imx319);
+
+	return 0;
+}
+
+static int __maybe_unused imx319_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct imx319 *imx319 = to_imx319(sd);
+	int ret;
+
+	if (imx319->streaming) {
+		ret = imx319_start_streaming(imx319);
+		if (ret)
+			goto error;
+	}
+
+	return 0;
+
+error:
+	imx319_stop_streaming(imx319);
+	imx319->streaming = 0;
+	return ret;
+}
+
+static const struct v4l2_subdev_core_ops imx319_subdev_core_ops = {
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_video_ops imx319_video_ops = {
+	.s_stream = imx319_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops imx319_pad_ops = {
+	.enum_mbus_code = imx319_enum_mbus_code,
+	.get_fmt = imx319_get_pad_format,
+	.set_fmt = imx319_set_pad_format,
+	.enum_frame_size = imx319_enum_frame_size,
+};
+
+static const struct v4l2_subdev_ops imx319_subdev_ops = {
+	.core = &imx319_subdev_core_ops,
+	.video = &imx319_video_ops,
+	.pad = &imx319_pad_ops,
+};
+
+static const struct media_entity_operations imx319_subdev_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static const struct v4l2_subdev_internal_ops imx319_internal_ops = {
+	.open = imx319_open,
+};
+
+/* Initialize control handlers */
+static int imx319_init_controls(struct imx319 *imx319)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx319->sd);
+	struct v4l2_ctrl_handler *ctrl_hdlr;
+	s64 exposure_max;
+	s64 vblank_def;
+	s64 vblank_min;
+	s64 hblank;
+	u64 pixel_rate;
+	const struct imx319_mode *mode;
+	u32 max;
+	int ret;
+
+	ctrl_hdlr = &imx319->ctrl_handler;
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 10);
+	if (ret)
+		return ret;
+
+	ctrl_hdlr->lock = &imx319->mutex;
+	max = ARRAY_SIZE(link_freq_menu_items) - 1;
+	imx319->link_freq = v4l2_ctrl_new_int_menu(ctrl_hdlr, &imx319_ctrl_ops,
+						   V4L2_CID_LINK_FREQ, max, 0,
+						   link_freq_menu_items);
+	if (imx319->link_freq)
+		imx319->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	/* pixel_rate = link_freq * 2 * nr_of_lanes / bits_per_sample */
+	pixel_rate = imx319->link_def_freq * 2 * 4;
+	do_div(pixel_rate, 10);
+	/* By default, PIXEL_RATE is read only */
+	imx319->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, &imx319_ctrl_ops,
+					       V4L2_CID_PIXEL_RATE, pixel_rate,
+					       pixel_rate, 1, pixel_rate);
+
+	/* Initial vblank/hblank/exposure parameters based on current mode */
+	mode = imx319->cur_mode;
+	vblank_def = mode->fll_def - mode->height;
+	vblank_min = mode->fll_min - mode->height;
+	imx319->vblank = v4l2_ctrl_new_std(ctrl_hdlr, &imx319_ctrl_ops,
+					   V4L2_CID_VBLANK, vblank_min,
+					   IMX319_FLL_MAX - mode->height,
+					   1, vblank_def);
+
+	hblank = mode->llp - mode->width;
+	imx319->hblank = v4l2_ctrl_new_std(ctrl_hdlr, &imx319_ctrl_ops,
+					   V4L2_CID_HBLANK, hblank, hblank,
+					   1, hblank);
+	if (imx319->hblank)
+		imx319->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	/* fll >= exposure time + adjust parameter (default value is 18) */
+	exposure_max = mode->fll_def - 18;
+	imx319->exposure = v4l2_ctrl_new_std(ctrl_hdlr, &imx319_ctrl_ops,
+					     V4L2_CID_EXPOSURE,
+					     IMX319_EXPOSURE_MIN, exposure_max,
+					     IMX319_EXPOSURE_STEP,
+					     IMX319_EXPOSURE_DEFAULT);
+
+	imx319->hflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx319_ctrl_ops,
+					  V4L2_CID_HFLIP, 0, 1, 1, 0);
+	if (imx319->hflip)
+		imx319->hflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
+	imx319->vflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx319_ctrl_ops,
+					  V4L2_CID_VFLIP, 0, 1, 1, 0);
+	if (imx319->vflip)
+		imx319->vflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &imx319_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
+			  IMX319_ANA_GAIN_MIN, IMX319_ANA_GAIN_MAX,
+			  IMX319_ANA_GAIN_STEP, IMX319_ANA_GAIN_DEFAULT);
+
+	/* Digital gain */
+	v4l2_ctrl_new_std(ctrl_hdlr, &imx319_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
+			  IMX319_DGTL_GAIN_MIN, IMX319_DGTL_GAIN_MAX,
+			  IMX319_DGTL_GAIN_STEP, IMX319_DGTL_GAIN_DEFAULT);
+
+	v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &imx319_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(imx319_test_pattern_menu) - 1,
+				     0, 0, imx319_test_pattern_menu);
+	if (ctrl_hdlr->error) {
+		ret = ctrl_hdlr->error;
+		dev_err(&client->dev, "control init failed: %d", ret);
+		goto error;
+	}
+
+	imx319->sd.ctrl_handler = ctrl_hdlr;
+
+	return 0;
+
+error:
+	v4l2_ctrl_handler_free(ctrl_hdlr);
+
+	return ret;
+}
+
+static struct imx319_hwcfg *imx319_get_hwcfg(struct device *dev)
+{
+	struct imx319_hwcfg *cfg;
+	struct v4l2_fwnode_endpoint bus_cfg = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY
+	};
+	struct fwnode_handle *ep;
+	struct fwnode_handle *fwnode = dev_fwnode(dev);
+	unsigned int i;
+	int ret;
+
+	if (!fwnode)
+		return NULL;
+
+	ep = fwnode_graph_get_next_endpoint(fwnode, NULL);
+	if (!ep)
+		return NULL;
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(ep, &bus_cfg);
+	if (ret)
+		goto out_err;
+
+	cfg = devm_kzalloc(dev, sizeof(*cfg), GFP_KERNEL);
+	if (!cfg)
+		goto out_err;
+
+	ret = fwnode_property_read_u32(dev_fwnode(dev), "clock-frequency",
+				       &cfg->ext_clk);
+	if (ret) {
+		dev_err(dev, "can't get clock frequency");
+		goto out_err;
+	}
+
+	dev_dbg(dev, "ext clk: %d", cfg->ext_clk);
+	if (cfg->ext_clk != IMX319_EXT_CLK) {
+		dev_err(dev, "external clock %d is not supported",
+			cfg->ext_clk);
+		goto out_err;
+	}
+
+	dev_dbg(dev, "num of link freqs: %d", bus_cfg.nr_of_link_frequencies);
+	if (!bus_cfg.nr_of_link_frequencies) {
+		dev_warn(dev, "no link frequencies defined");
+		goto out_err;
+	}
+
+	cfg->nr_of_link_freqs = bus_cfg.nr_of_link_frequencies;
+	cfg->link_freqs = devm_kcalloc(dev,
+				       bus_cfg.nr_of_link_frequencies + 1,
+				       sizeof(*cfg->link_freqs), GFP_KERNEL);
+	if (!cfg->link_freqs)
+		goto out_err;
+
+	for (i = 0; i < bus_cfg.nr_of_link_frequencies; i++) {
+		cfg->link_freqs[i] = bus_cfg.link_frequencies[i];
+		dev_dbg(dev, "link_freq[%d] = %lld", i, cfg->link_freqs[i]);
+	}
+
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+	fwnode_handle_put(ep);
+	return cfg;
+
+out_err:
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+	fwnode_handle_put(ep);
+	return NULL;
+}
+
+static int imx319_probe(struct i2c_client *client)
+{
+	struct imx319 *imx319;
+	bool full_power;
+	int ret;
+	u32 i;
+
+	imx319 = devm_kzalloc(&client->dev, sizeof(*imx319), GFP_KERNEL);
+	if (!imx319)
+		return -ENOMEM;
+
+	mutex_init(&imx319->mutex);
+
+	/* Initialize subdev */
+	v4l2_i2c_subdev_init(&imx319->sd, client, &imx319_subdev_ops);
+
+	full_power = acpi_dev_state_d0(&client->dev);
+	if (full_power) {
+		/* Check module identity */
+		ret = imx319_identify_module(imx319);
+		if (ret) {
+			dev_err(&client->dev, "failed to find sensor: %d", ret);
+			goto error_probe;
+		}
+	}
+
+	imx319->hwcfg = imx319_get_hwcfg(&client->dev);
+	if (!imx319->hwcfg) {
+		dev_err(&client->dev, "failed to get hwcfg");
+		ret = -ENODEV;
+		goto error_probe;
+	}
+
+	imx319->link_def_freq = link_freq_menu_items[IMX319_LINK_FREQ_INDEX];
+	for (i = 0; i < imx319->hwcfg->nr_of_link_freqs; i++) {
+		if (imx319->hwcfg->link_freqs[i] == imx319->link_def_freq) {
+			dev_dbg(&client->dev, "link freq index %d matched", i);
+			break;
+		}
+	}
+
+	if (i == imx319->hwcfg->nr_of_link_freqs) {
+		dev_err(&client->dev, "no link frequency supported");
+		ret = -EINVAL;
+		goto error_probe;
+	}
+
+	/* Set default mode to max resolution */
+	imx319->cur_mode = &supported_modes[0];
+
+	ret = imx319_init_controls(imx319);
+	if (ret) {
+		dev_err(&client->dev, "failed to init controls: %d", ret);
+		goto error_probe;
+	}
+
+	/* Initialize subdev */
+	imx319->sd.internal_ops = &imx319_internal_ops;
+	imx319->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+		V4L2_SUBDEV_FL_HAS_EVENTS;
+	imx319->sd.entity.ops = &imx319_subdev_entity_ops;
+	imx319->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+	/* Initialize source pad */
+	imx319->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&imx319->sd.entity, 1, &imx319->pad);
+	if (ret) {
+		dev_err(&client->dev, "failed to init entity pads: %d", ret);
+		goto error_handler_free;
+	}
+
+	ret = v4l2_async_register_subdev_sensor(&imx319->sd);
+	if (ret < 0)
+		goto error_media_entity;
+
+	/* Set the device's state to active if it's in D0 state. */
+	if (full_power)
+		pm_runtime_set_active(&client->dev);
+	pm_runtime_enable(&client->dev);
+	pm_runtime_idle(&client->dev);
+
+	return 0;
+
+error_media_entity:
+	media_entity_cleanup(&imx319->sd.entity);
+
+error_handler_free:
+	v4l2_ctrl_handler_free(imx319->sd.ctrl_handler);
+
+error_probe:
+	mutex_destroy(&imx319->mutex);
+
+	return ret;
+}
+
+static void imx319_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct imx319 *imx319 = to_imx319(sd);
+
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+
+	pm_runtime_disable(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+
+	mutex_destroy(&imx319->mutex);
+}
+
+static const struct dev_pm_ops imx319_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(imx319_suspend, imx319_resume)
+};
+
+static const struct acpi_device_id imx319_acpi_ids[] __maybe_unused = {
+	{ "SONY319A" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(acpi, imx319_acpi_ids);
+
+static struct i2c_driver imx319_i2c_driver = {
+	.driver = {
+		.name = "imx319",
+		.pm = &imx319_pm_ops,
+		.acpi_match_table = ACPI_PTR(imx319_acpi_ids),
+	},
+	.probe = imx319_probe,
+	.remove = imx319_remove,
+	.flags = I2C_DRV_ACPI_WAIVE_D0_PROBE,
+};
+module_i2c_driver(imx319_i2c_driver);
+
+MODULE_AUTHOR("Qiu, Tianshu <tian.shu.qiu@intel.com>");
+MODULE_AUTHOR("Rapolu, Chiranjeevi");
+MODULE_AUTHOR("Bingbu Cao <bingbu.cao@intel.com>");
+MODULE_AUTHOR("Yang, Hyungwoo");
+MODULE_DESCRIPTION("Sony imx319 sensor driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/imx334.c b/drivers/media/i2c/imx334.c
new file mode 100644
index 000000000..d722c9b7c
--- /dev/null
+++ b/drivers/media/i2c/imx334.c
@@ -0,0 +1,1431 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Sony imx334 sensor driver
+ *
+ * Copyright (C) 2021 Intel Corporation
+ */
+#include <asm/unaligned.h>
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+/* Streaming Mode */
+#define IMX334_REG_MODE_SELECT	0x3000
+#define IMX334_MODE_STANDBY	0x01
+#define IMX334_MODE_STREAMING	0x00
+
+/* Lines per frame */
+#define IMX334_REG_LPFR		0x3030
+
+/* Chip ID */
+#define IMX334_REG_ID		0x3044
+#define IMX334_ID		0x1e
+
+/* Exposure control */
+#define IMX334_REG_SHUTTER	0x3058
+#define IMX334_EXPOSURE_MIN	1
+#define IMX334_EXPOSURE_OFFSET	5
+#define IMX334_EXPOSURE_STEP	1
+#define IMX334_EXPOSURE_DEFAULT	0x0648
+
+/* Analog gain control */
+#define IMX334_REG_AGAIN	0x30e8
+#define IMX334_AGAIN_MIN	0
+#define IMX334_AGAIN_MAX	240
+#define IMX334_AGAIN_STEP	1
+#define IMX334_AGAIN_DEFAULT	0
+
+/* Group hold register */
+#define IMX334_REG_HOLD		0x3001
+
+/* Input clock rate */
+#define IMX334_INCLK_RATE	24000000
+
+/* CSI2 HW configuration */
+#define IMX334_LINK_FREQ_891M	891000000
+#define IMX334_LINK_FREQ_445M	445500000
+#define IMX334_NUM_DATA_LANES	4
+
+#define IMX334_REG_MIN		0x00
+#define IMX334_REG_MAX		0xfffff
+
+/**
+ * struct imx334_reg - imx334 sensor register
+ * @address: Register address
+ * @val: Register value
+ */
+struct imx334_reg {
+	u16 address;
+	u8 val;
+};
+
+/**
+ * struct imx334_reg_list - imx334 sensor register list
+ * @num_of_regs: Number of registers in the list
+ * @regs: Pointer to register list
+ */
+struct imx334_reg_list {
+	u32 num_of_regs;
+	const struct imx334_reg *regs;
+};
+
+/**
+ * struct imx334_mode - imx334 sensor mode structure
+ * @width: Frame width
+ * @height: Frame height
+ * @hblank: Horizontal blanking in lines
+ * @vblank: Vertical blanking in lines
+ * @vblank_min: Minimal vertical blanking in lines
+ * @vblank_max: Maximum vertical blanking in lines
+ * @pclk: Sensor pixel clock
+ * @link_freq_idx: Link frequency index
+ * @reg_list: Register list for sensor mode
+ */
+struct imx334_mode {
+	u32 width;
+	u32 height;
+	u32 hblank;
+	u32 vblank;
+	u32 vblank_min;
+	u32 vblank_max;
+	u64 pclk;
+	u32 link_freq_idx;
+	struct imx334_reg_list reg_list;
+};
+
+/**
+ * struct imx334 - imx334 sensor device structure
+ * @dev: Pointer to generic device
+ * @client: Pointer to i2c client
+ * @sd: V4L2 sub-device
+ * @pad: Media pad. Only one pad supported
+ * @reset_gpio: Sensor reset gpio
+ * @inclk: Sensor input clock
+ * @ctrl_handler: V4L2 control handler
+ * @link_freq_ctrl: Pointer to link frequency control
+ * @pclk_ctrl: Pointer to pixel clock control
+ * @hblank_ctrl: Pointer to horizontal blanking control
+ * @vblank_ctrl: Pointer to vertical blanking control
+ * @exp_ctrl: Pointer to exposure control
+ * @again_ctrl: Pointer to analog gain control
+ * @vblank: Vertical blanking in lines
+ * @cur_mode: Pointer to current selected sensor mode
+ * @mutex: Mutex for serializing sensor controls
+ * @menu_skip_mask: Menu skip mask for link_freq_ctrl
+ * @cur_code: current selected format code
+ * @streaming: Flag indicating streaming state
+ */
+struct imx334 {
+	struct device *dev;
+	struct i2c_client *client;
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct gpio_desc *reset_gpio;
+	struct clk *inclk;
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_ctrl *link_freq_ctrl;
+	struct v4l2_ctrl *pclk_ctrl;
+	struct v4l2_ctrl *hblank_ctrl;
+	struct v4l2_ctrl *vblank_ctrl;
+	struct {
+		struct v4l2_ctrl *exp_ctrl;
+		struct v4l2_ctrl *again_ctrl;
+	};
+	u32 vblank;
+	const struct imx334_mode *cur_mode;
+	struct mutex mutex;
+	unsigned long menu_skip_mask;
+	u32 cur_code;
+	bool streaming;
+};
+
+static const s64 link_freq[] = {
+	IMX334_LINK_FREQ_891M,
+	IMX334_LINK_FREQ_445M,
+};
+
+/* Sensor mode registers for 1920x1080@30fps */
+static const struct imx334_reg mode_1920x1080_regs[] = {
+	{0x3000, 0x01},
+	{0x3018, 0x04},
+	{0x3030, 0xca},
+	{0x3031, 0x08},
+	{0x3032, 0x00},
+	{0x3034, 0x4c},
+	{0x3035, 0x04},
+	{0x302c, 0xf0},
+	{0x302d, 0x03},
+	{0x302e, 0x80},
+	{0x302f, 0x07},
+	{0x3074, 0xcc},
+	{0x3075, 0x02},
+	{0x308e, 0xcd},
+	{0x308f, 0x02},
+	{0x3076, 0x38},
+	{0x3077, 0x04},
+	{0x3090, 0x38},
+	{0x3091, 0x04},
+	{0x3308, 0x38},
+	{0x3309, 0x04},
+	{0x30C6, 0x00},
+	{0x30c7, 0x00},
+	{0x30ce, 0x00},
+	{0x30cf, 0x00},
+	{0x30d8, 0x18},
+	{0x30d9, 0x0a},
+	{0x304c, 0x00},
+	{0x304e, 0x00},
+	{0x304f, 0x00},
+	{0x3050, 0x00},
+	{0x30b6, 0x00},
+	{0x30b7, 0x00},
+	{0x3116, 0x08},
+	{0x3117, 0x00},
+	{0x31a0, 0x20},
+	{0x31a1, 0x0f},
+	{0x300c, 0x3b},
+	{0x300d, 0x29},
+	{0x314c, 0x29},
+	{0x314d, 0x01},
+	{0x315a, 0x06},
+	{0x3168, 0xa0},
+	{0x316a, 0x7e},
+	{0x319e, 0x02},
+	{0x3199, 0x00},
+	{0x319d, 0x00},
+	{0x31dd, 0x03},
+	{0x3300, 0x00},
+	{0x341c, 0xff},
+	{0x341d, 0x01},
+	{0x3a01, 0x03},
+	{0x3a18, 0x7f},
+	{0x3a19, 0x00},
+	{0x3a1a, 0x37},
+	{0x3a1b, 0x00},
+	{0x3a1c, 0x37},
+	{0x3a1d, 0x00},
+	{0x3a1e, 0xf7},
+	{0x3a1f, 0x00},
+	{0x3a20, 0x3f},
+	{0x3a21, 0x00},
+	{0x3a20, 0x6f},
+	{0x3a21, 0x00},
+	{0x3a20, 0x3f},
+	{0x3a21, 0x00},
+	{0x3a20, 0x5f},
+	{0x3a21, 0x00},
+	{0x3a20, 0x2f},
+	{0x3a21, 0x00},
+	{0x3078, 0x02},
+	{0x3079, 0x00},
+	{0x307a, 0x00},
+	{0x307b, 0x00},
+	{0x3080, 0x02},
+	{0x3081, 0x00},
+	{0x3082, 0x00},
+	{0x3083, 0x00},
+	{0x3088, 0x02},
+	{0x3094, 0x00},
+	{0x3095, 0x00},
+	{0x3096, 0x00},
+	{0x309b, 0x02},
+	{0x309c, 0x00},
+	{0x309d, 0x00},
+	{0x309e, 0x00},
+	{0x30a4, 0x00},
+	{0x30a5, 0x00},
+	{0x3288, 0x21},
+	{0x328a, 0x02},
+	{0x3414, 0x05},
+	{0x3416, 0x18},
+	{0x35Ac, 0x0e},
+	{0x3648, 0x01},
+	{0x364a, 0x04},
+	{0x364c, 0x04},
+	{0x3678, 0x01},
+	{0x367c, 0x31},
+	{0x367e, 0x31},
+	{0x3708, 0x02},
+	{0x3714, 0x01},
+	{0x3715, 0x02},
+	{0x3716, 0x02},
+	{0x3717, 0x02},
+	{0x371c, 0x3d},
+	{0x371d, 0x3f},
+	{0x372c, 0x00},
+	{0x372d, 0x00},
+	{0x372e, 0x46},
+	{0x372f, 0x00},
+	{0x3730, 0x89},
+	{0x3731, 0x00},
+	{0x3732, 0x08},
+	{0x3733, 0x01},
+	{0x3734, 0xfe},
+	{0x3735, 0x05},
+	{0x375d, 0x00},
+	{0x375e, 0x00},
+	{0x375f, 0x61},
+	{0x3760, 0x06},
+	{0x3768, 0x1b},
+	{0x3769, 0x1b},
+	{0x376a, 0x1a},
+	{0x376b, 0x19},
+	{0x376c, 0x18},
+	{0x376d, 0x14},
+	{0x376e, 0x0f},
+	{0x3776, 0x00},
+	{0x3777, 0x00},
+	{0x3778, 0x46},
+	{0x3779, 0x00},
+	{0x377a, 0x08},
+	{0x377b, 0x01},
+	{0x377c, 0x45},
+	{0x377d, 0x01},
+	{0x377e, 0x23},
+	{0x377f, 0x02},
+	{0x3780, 0xd9},
+	{0x3781, 0x03},
+	{0x3782, 0xf5},
+	{0x3783, 0x06},
+	{0x3784, 0xa5},
+	{0x3788, 0x0f},
+	{0x378a, 0xd9},
+	{0x378b, 0x03},
+	{0x378c, 0xeb},
+	{0x378d, 0x05},
+	{0x378e, 0x87},
+	{0x378f, 0x06},
+	{0x3790, 0xf5},
+	{0x3792, 0x43},
+	{0x3794, 0x7a},
+	{0x3796, 0xa1},
+	{0x37b0, 0x37},
+	{0x3e04, 0x0e},
+	{0x30e8, 0x50},
+	{0x30e9, 0x00},
+	{0x3e04, 0x0e},
+	{0x3002, 0x00},
+};
+
+/* Sensor mode registers for 3840x2160@30fps */
+static const struct imx334_reg mode_3840x2160_regs[] = {
+	{0x3000, 0x01},
+	{0x3002, 0x00},
+	{0x3018, 0x04},
+	{0x37b0, 0x36},
+	{0x304c, 0x00},
+	{0x300c, 0x3b},
+	{0x300d, 0x2a},
+	{0x3034, 0x26},
+	{0x3035, 0x02},
+	{0x314c, 0x29},
+	{0x314d, 0x01},
+	{0x315a, 0x02},
+	{0x3168, 0xa0},
+	{0x316a, 0x7e},
+	{0x3288, 0x21},
+	{0x328a, 0x02},
+	{0x302c, 0x3c},
+	{0x302d, 0x00},
+	{0x302e, 0x00},
+	{0x302f, 0x0f},
+	{0x3076, 0x70},
+	{0x3077, 0x08},
+	{0x3090, 0x70},
+	{0x3091, 0x08},
+	{0x30d8, 0x20},
+	{0x30d9, 0x12},
+	{0x3308, 0x70},
+	{0x3309, 0x08},
+	{0x3414, 0x05},
+	{0x3416, 0x18},
+	{0x35ac, 0x0e},
+	{0x3648, 0x01},
+	{0x364a, 0x04},
+	{0x364c, 0x04},
+	{0x3678, 0x01},
+	{0x367c, 0x31},
+	{0x367e, 0x31},
+	{0x3708, 0x02},
+	{0x3714, 0x01},
+	{0x3715, 0x02},
+	{0x3716, 0x02},
+	{0x3717, 0x02},
+	{0x371c, 0x3d},
+	{0x371d, 0x3f},
+	{0x372c, 0x00},
+	{0x372d, 0x00},
+	{0x372e, 0x46},
+	{0x372f, 0x00},
+	{0x3730, 0x89},
+	{0x3731, 0x00},
+	{0x3732, 0x08},
+	{0x3733, 0x01},
+	{0x3734, 0xfe},
+	{0x3735, 0x05},
+	{0x375d, 0x00},
+	{0x375e, 0x00},
+	{0x375f, 0x61},
+	{0x3760, 0x06},
+	{0x3768, 0x1b},
+	{0x3769, 0x1b},
+	{0x376a, 0x1a},
+	{0x376b, 0x19},
+	{0x376c, 0x18},
+	{0x376d, 0x14},
+	{0x376e, 0x0f},
+	{0x3776, 0x00},
+	{0x3777, 0x00},
+	{0x3778, 0x46},
+	{0x3779, 0x00},
+	{0x377a, 0x08},
+	{0x377b, 0x01},
+	{0x377c, 0x45},
+	{0x377d, 0x01},
+	{0x377e, 0x23},
+	{0x377f, 0x02},
+	{0x3780, 0xd9},
+	{0x3781, 0x03},
+	{0x3782, 0xf5},
+	{0x3783, 0x06},
+	{0x3784, 0xa5},
+	{0x3788, 0x0f},
+	{0x378a, 0xd9},
+	{0x378b, 0x03},
+	{0x378c, 0xeb},
+	{0x378d, 0x05},
+	{0x378e, 0x87},
+	{0x378f, 0x06},
+	{0x3790, 0xf5},
+	{0x3792, 0x43},
+	{0x3794, 0x7a},
+	{0x3796, 0xa1},
+	{0x3e04, 0x0e},
+	{0x319e, 0x00},
+	{0x3a00, 0x01},
+	{0x3a18, 0xbf},
+	{0x3a19, 0x00},
+	{0x3a1a, 0x67},
+	{0x3a1b, 0x00},
+	{0x3a1c, 0x6f},
+	{0x3a1d, 0x00},
+	{0x3a1e, 0xd7},
+	{0x3a1f, 0x01},
+	{0x3a20, 0x6f},
+	{0x3a21, 0x00},
+	{0x3a22, 0xcf},
+	{0x3a23, 0x00},
+	{0x3a24, 0x6f},
+	{0x3a25, 0x00},
+	{0x3a26, 0xb7},
+	{0x3a27, 0x00},
+	{0x3a28, 0x5f},
+	{0x3a29, 0x00},
+};
+
+static const struct imx334_reg raw10_framefmt_regs[] = {
+	{0x3050, 0x00},
+	{0x319d, 0x00},
+	{0x341c, 0xff},
+	{0x341d, 0x01},
+};
+
+static const struct imx334_reg raw12_framefmt_regs[] = {
+	{0x3050, 0x01},
+	{0x319d, 0x01},
+	{0x341c, 0x47},
+	{0x341d, 0x00},
+};
+
+static const u32 imx334_mbus_codes[] = {
+	MEDIA_BUS_FMT_SRGGB12_1X12,
+	MEDIA_BUS_FMT_SRGGB10_1X10,
+};
+
+/* Supported sensor mode configurations */
+static const struct imx334_mode supported_modes[] = {
+	{
+		.width = 3840,
+		.height = 2160,
+		.hblank = 560,
+		.vblank = 2340,
+		.vblank_min = 90,
+		.vblank_max = 132840,
+		.pclk = 594000000,
+		.link_freq_idx = 0,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_3840x2160_regs),
+			.regs = mode_3840x2160_regs,
+		},
+	}, {
+		.width = 1920,
+		.height = 1080,
+		.hblank = 2480,
+		.vblank = 1170,
+		.vblank_min = 45,
+		.vblank_max = 132840,
+		.pclk = 297000000,
+		.link_freq_idx = 1,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1920x1080_regs),
+			.regs = mode_1920x1080_regs,
+		},
+	},
+};
+
+/**
+ * to_imx334() - imv334 V4L2 sub-device to imx334 device.
+ * @subdev: pointer to imx334 V4L2 sub-device
+ *
+ * Return: pointer to imx334 device
+ */
+static inline struct imx334 *to_imx334(struct v4l2_subdev *subdev)
+{
+	return container_of(subdev, struct imx334, sd);
+}
+
+/**
+ * imx334_read_reg() - Read registers.
+ * @imx334: pointer to imx334 device
+ * @reg: register address
+ * @len: length of bytes to read. Max supported bytes is 4
+ * @val: pointer to register value to be filled.
+ *
+ * Big endian register addresses with little endian values.
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx334_read_reg(struct imx334 *imx334, u16 reg, u32 len, u32 *val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx334->sd);
+	struct i2c_msg msgs[2] = {0};
+	u8 addr_buf[2] = {0};
+	u8 data_buf[4] = {0};
+	int ret;
+
+	if (WARN_ON(len > 4))
+		return -EINVAL;
+
+	put_unaligned_be16(reg, addr_buf);
+
+	/* Write register address */
+	msgs[0].addr = client->addr;
+	msgs[0].flags = 0;
+	msgs[0].len = ARRAY_SIZE(addr_buf);
+	msgs[0].buf = addr_buf;
+
+	/* Read data from register */
+	msgs[1].addr = client->addr;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].len = len;
+	msgs[1].buf = data_buf;
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret != ARRAY_SIZE(msgs))
+		return -EIO;
+
+	*val = get_unaligned_le32(data_buf);
+
+	return 0;
+}
+
+/**
+ * imx334_write_reg() - Write register
+ * @imx334: pointer to imx334 device
+ * @reg: register address
+ * @len: length of bytes. Max supported bytes is 4
+ * @val: register value
+ *
+ * Big endian register addresses with little endian values.
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx334_write_reg(struct imx334 *imx334, u16 reg, u32 len, u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx334->sd);
+	u8 buf[6] = {0};
+
+	if (WARN_ON(len > 4))
+		return -EINVAL;
+
+	put_unaligned_be16(reg, buf);
+	put_unaligned_le32(val, buf + 2);
+	if (i2c_master_send(client, buf, len + 2) != len + 2)
+		return -EIO;
+
+	return 0;
+}
+
+/**
+ * imx334_write_regs() - Write a list of registers
+ * @imx334: pointer to imx334 device
+ * @regs: list of registers to be written
+ * @len: length of registers array
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx334_write_regs(struct imx334 *imx334,
+			     const struct imx334_reg *regs, u32 len)
+{
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < len; i++) {
+		ret = imx334_write_reg(imx334, regs[i].address, 1, regs[i].val);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+/**
+ * imx334_update_controls() - Update control ranges based on streaming mode
+ * @imx334: pointer to imx334 device
+ * @mode: pointer to imx334_mode sensor mode
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx334_update_controls(struct imx334 *imx334,
+				  const struct imx334_mode *mode)
+{
+	int ret;
+
+	ret = __v4l2_ctrl_s_ctrl(imx334->link_freq_ctrl, mode->link_freq_idx);
+	if (ret)
+		return ret;
+
+	ret = __v4l2_ctrl_modify_range(imx334->pclk_ctrl, mode->pclk,
+				       mode->pclk, 1, mode->pclk);
+	if (ret)
+		return ret;
+
+	ret = __v4l2_ctrl_modify_range(imx334->hblank_ctrl, mode->hblank,
+				       mode->hblank, 1, mode->hblank);
+	if (ret)
+		return ret;
+
+	ret =  __v4l2_ctrl_modify_range(imx334->vblank_ctrl, mode->vblank_min,
+					mode->vblank_max, 1, mode->vblank);
+	if (ret)
+		return ret;
+
+	return __v4l2_ctrl_s_ctrl(imx334->vblank_ctrl, mode->vblank);
+}
+
+/**
+ * imx334_update_exp_gain() - Set updated exposure and gain
+ * @imx334: pointer to imx334 device
+ * @exposure: updated exposure value
+ * @gain: updated analog gain value
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx334_update_exp_gain(struct imx334 *imx334, u32 exposure, u32 gain)
+{
+	u32 lpfr, shutter;
+	int ret;
+
+	lpfr = imx334->vblank + imx334->cur_mode->height;
+	shutter = lpfr - exposure;
+
+	dev_dbg(imx334->dev, "Set long exp %u analog gain %u sh0 %u lpfr %u",
+		exposure, gain, shutter, lpfr);
+
+	ret = imx334_write_reg(imx334, IMX334_REG_HOLD, 1, 1);
+	if (ret)
+		return ret;
+
+	ret = imx334_write_reg(imx334, IMX334_REG_LPFR, 3, lpfr);
+	if (ret)
+		goto error_release_group_hold;
+
+	ret = imx334_write_reg(imx334, IMX334_REG_SHUTTER, 3, shutter);
+	if (ret)
+		goto error_release_group_hold;
+
+	ret = imx334_write_reg(imx334, IMX334_REG_AGAIN, 1, gain);
+
+error_release_group_hold:
+	imx334_write_reg(imx334, IMX334_REG_HOLD, 1, 0);
+
+	return ret;
+}
+
+/**
+ * imx334_set_ctrl() - Set subdevice control
+ * @ctrl: pointer to v4l2_ctrl structure
+ *
+ * Supported controls:
+ * - V4L2_CID_VBLANK
+ * - cluster controls:
+ *   - V4L2_CID_ANALOGUE_GAIN
+ *   - V4L2_CID_EXPOSURE
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx334_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct imx334 *imx334 =
+		container_of(ctrl->handler, struct imx334, ctrl_handler);
+	u32 analog_gain;
+	u32 exposure;
+	int ret;
+
+	switch (ctrl->id) {
+	case V4L2_CID_VBLANK:
+		imx334->vblank = imx334->vblank_ctrl->val;
+
+		dev_dbg(imx334->dev, "Received vblank %u, new lpfr %u",
+			imx334->vblank,
+			imx334->vblank + imx334->cur_mode->height);
+
+		ret = __v4l2_ctrl_modify_range(imx334->exp_ctrl,
+					       IMX334_EXPOSURE_MIN,
+					       imx334->vblank +
+					       imx334->cur_mode->height -
+					       IMX334_EXPOSURE_OFFSET,
+					       1, IMX334_EXPOSURE_DEFAULT);
+		break;
+	case V4L2_CID_EXPOSURE:
+
+		/* Set controls only if sensor is in power on state */
+		if (!pm_runtime_get_if_in_use(imx334->dev))
+			return 0;
+
+		exposure = ctrl->val;
+		analog_gain = imx334->again_ctrl->val;
+
+		dev_dbg(imx334->dev, "Received exp %u analog gain %u",
+			exposure, analog_gain);
+
+		ret = imx334_update_exp_gain(imx334, exposure, analog_gain);
+
+		pm_runtime_put(imx334->dev);
+
+		break;
+	case V4L2_CID_PIXEL_RATE:
+	case V4L2_CID_LINK_FREQ:
+	case V4L2_CID_HBLANK:
+		ret = 0;
+		break;
+	default:
+		dev_err(imx334->dev, "Invalid control %d", ctrl->id);
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+/* V4l2 subdevice control ops*/
+static const struct v4l2_ctrl_ops imx334_ctrl_ops = {
+	.s_ctrl = imx334_set_ctrl,
+};
+
+static int imx334_get_format_code(struct imx334 *imx334, u32 code)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(imx334_mbus_codes); i++) {
+		if (imx334_mbus_codes[i] == code)
+			return imx334_mbus_codes[i];
+	}
+
+	return imx334_mbus_codes[0];
+}
+
+/**
+ * imx334_enum_mbus_code() - Enumerate V4L2 sub-device mbus codes
+ * @sd: pointer to imx334 V4L2 sub-device structure
+ * @sd_state: V4L2 sub-device state
+ * @code: V4L2 sub-device code enumeration need to be filled
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx334_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index >= ARRAY_SIZE(imx334_mbus_codes))
+		return -EINVAL;
+
+	code->code = imx334_mbus_codes[code->index];
+
+	return 0;
+}
+
+/**
+ * imx334_enum_frame_size() - Enumerate V4L2 sub-device frame sizes
+ * @sd: pointer to imx334 V4L2 sub-device structure
+ * @sd_state: V4L2 sub-device state
+ * @fsize: V4L2 sub-device size enumeration need to be filled
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx334_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fsize)
+{
+	struct imx334 *imx334 = to_imx334(sd);
+	u32 code;
+
+	if (fsize->index >= ARRAY_SIZE(supported_modes))
+		return -EINVAL;
+
+	code = imx334_get_format_code(imx334, fsize->code);
+
+	if (fsize->code != code)
+		return -EINVAL;
+
+	fsize->min_width = supported_modes[fsize->index].width;
+	fsize->max_width = fsize->min_width;
+	fsize->min_height = supported_modes[fsize->index].height;
+	fsize->max_height = fsize->min_height;
+
+	return 0;
+}
+
+/**
+ * imx334_fill_pad_format() - Fill subdevice pad format
+ *                            from selected sensor mode
+ * @imx334: pointer to imx334 device
+ * @mode: pointer to imx334_mode sensor mode
+ * @fmt: V4L2 sub-device format need to be filled
+ */
+static void imx334_fill_pad_format(struct imx334 *imx334,
+				   const struct imx334_mode *mode,
+				   struct v4l2_subdev_format *fmt)
+{
+	fmt->format.width = mode->width;
+	fmt->format.height = mode->height;
+	fmt->format.field = V4L2_FIELD_NONE;
+	fmt->format.colorspace = V4L2_COLORSPACE_RAW;
+	fmt->format.ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+	fmt->format.quantization = V4L2_QUANTIZATION_DEFAULT;
+	fmt->format.xfer_func = V4L2_XFER_FUNC_NONE;
+}
+
+/**
+ * imx334_get_pad_format() - Get subdevice pad format
+ * @sd: pointer to imx334 V4L2 sub-device structure
+ * @sd_state: V4L2 sub-device state
+ * @fmt: V4L2 sub-device format need to be set
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx334_get_pad_format(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_format *fmt)
+{
+	struct imx334 *imx334 = to_imx334(sd);
+
+	mutex_lock(&imx334->mutex);
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		struct v4l2_mbus_framefmt *framefmt;
+
+		framefmt = v4l2_subdev_get_try_format(sd, sd_state, fmt->pad);
+		fmt->format = *framefmt;
+	} else {
+		fmt->format.code = imx334->cur_code;
+		imx334_fill_pad_format(imx334, imx334->cur_mode, fmt);
+	}
+
+	mutex_unlock(&imx334->mutex);
+
+	return 0;
+}
+
+/**
+ * imx334_set_pad_format() - Set subdevice pad format
+ * @sd: pointer to imx334 V4L2 sub-device structure
+ * @sd_state: V4L2 sub-device state
+ * @fmt: V4L2 sub-device format need to be set
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx334_set_pad_format(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_format *fmt)
+{
+	struct imx334 *imx334 = to_imx334(sd);
+	const struct imx334_mode *mode;
+	int ret = 0;
+
+	mutex_lock(&imx334->mutex);
+
+	mode = v4l2_find_nearest_size(supported_modes,
+				      ARRAY_SIZE(supported_modes),
+				      width, height,
+				      fmt->format.width, fmt->format.height);
+
+	imx334_fill_pad_format(imx334, mode, fmt);
+	fmt->format.code = imx334_get_format_code(imx334, fmt->format.code);
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		struct v4l2_mbus_framefmt *framefmt;
+
+		framefmt = v4l2_subdev_get_try_format(sd, sd_state, fmt->pad);
+		*framefmt = fmt->format;
+	} else if (imx334->cur_mode != mode || imx334->cur_code != fmt->format.code) {
+		imx334->cur_code = fmt->format.code;
+		ret = imx334_update_controls(imx334, mode);
+		if (!ret)
+			imx334->cur_mode = mode;
+	}
+
+	mutex_unlock(&imx334->mutex);
+
+	return ret;
+}
+
+/**
+ * imx334_init_pad_cfg() - Initialize sub-device pad configuration
+ * @sd: pointer to imx334 V4L2 sub-device structure
+ * @sd_state: V4L2 sub-device state
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx334_init_pad_cfg(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_state *sd_state)
+{
+	struct imx334 *imx334 = to_imx334(sd);
+	struct v4l2_subdev_format fmt = { 0 };
+
+	fmt.which = sd_state ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE;
+
+	mutex_lock(&imx334->mutex);
+
+	imx334_fill_pad_format(imx334, imx334->cur_mode, &fmt);
+
+	__v4l2_ctrl_modify_range(imx334->link_freq_ctrl, 0,
+				 __fls(imx334->menu_skip_mask),
+				 ~(imx334->menu_skip_mask),
+				 __ffs(imx334->menu_skip_mask));
+
+	mutex_unlock(&imx334->mutex);
+
+	return imx334_set_pad_format(sd, sd_state, &fmt);
+}
+
+static int imx334_set_framefmt(struct imx334 *imx334)
+{
+	switch (imx334->cur_code) {
+	case MEDIA_BUS_FMT_SRGGB10_1X10:
+		return imx334_write_regs(imx334, raw10_framefmt_regs,
+					 ARRAY_SIZE(raw10_framefmt_regs));
+
+	case MEDIA_BUS_FMT_SRGGB12_1X12:
+		return imx334_write_regs(imx334, raw12_framefmt_regs,
+					 ARRAY_SIZE(raw12_framefmt_regs));
+	}
+
+	return -EINVAL;
+}
+
+/**
+ * imx334_start_streaming() - Start sensor stream
+ * @imx334: pointer to imx334 device
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx334_start_streaming(struct imx334 *imx334)
+{
+	const struct imx334_reg_list *reg_list;
+	int ret;
+
+	/* Write sensor mode registers */
+	reg_list = &imx334->cur_mode->reg_list;
+	ret = imx334_write_regs(imx334, reg_list->regs,
+				reg_list->num_of_regs);
+	if (ret) {
+		dev_err(imx334->dev, "fail to write initial registers");
+		return ret;
+	}
+
+	ret = imx334_set_framefmt(imx334);
+	if (ret) {
+		dev_err(imx334->dev, "%s failed to set frame format: %d\n",
+			__func__, ret);
+		return ret;
+	}
+
+	/* Setup handler will write actual exposure and gain */
+	ret =  __v4l2_ctrl_handler_setup(imx334->sd.ctrl_handler);
+	if (ret) {
+		dev_err(imx334->dev, "fail to setup handler");
+		return ret;
+	}
+
+	/* Start streaming */
+	ret = imx334_write_reg(imx334, IMX334_REG_MODE_SELECT,
+			       1, IMX334_MODE_STREAMING);
+	if (ret) {
+		dev_err(imx334->dev, "fail to start streaming");
+		return ret;
+	}
+
+	return 0;
+}
+
+/**
+ * imx334_stop_streaming() - Stop sensor stream
+ * @imx334: pointer to imx334 device
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx334_stop_streaming(struct imx334 *imx334)
+{
+	return imx334_write_reg(imx334, IMX334_REG_MODE_SELECT,
+				1, IMX334_MODE_STANDBY);
+}
+
+/**
+ * imx334_set_stream() - Enable sensor streaming
+ * @sd: pointer to imx334 subdevice
+ * @enable: set to enable sensor streaming
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx334_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct imx334 *imx334 = to_imx334(sd);
+	int ret;
+
+	mutex_lock(&imx334->mutex);
+
+	if (imx334->streaming == enable) {
+		mutex_unlock(&imx334->mutex);
+		return 0;
+	}
+
+	if (enable) {
+		ret = pm_runtime_resume_and_get(imx334->dev);
+		if (ret < 0)
+			goto error_unlock;
+
+		ret = imx334_start_streaming(imx334);
+		if (ret)
+			goto error_power_off;
+	} else {
+		imx334_stop_streaming(imx334);
+		pm_runtime_put(imx334->dev);
+	}
+
+	imx334->streaming = enable;
+
+	mutex_unlock(&imx334->mutex);
+
+	return 0;
+
+error_power_off:
+	pm_runtime_put(imx334->dev);
+error_unlock:
+	mutex_unlock(&imx334->mutex);
+
+	return ret;
+}
+
+/**
+ * imx334_detect() - Detect imx334 sensor
+ * @imx334: pointer to imx334 device
+ *
+ * Return: 0 if successful, -EIO if sensor id does not match
+ */
+static int imx334_detect(struct imx334 *imx334)
+{
+	int ret;
+	u32 val;
+
+	ret = imx334_read_reg(imx334, IMX334_REG_ID, 2, &val);
+	if (ret)
+		return ret;
+
+	if (val != IMX334_ID) {
+		dev_err(imx334->dev, "chip id mismatch: %x!=%x",
+			IMX334_ID, val);
+		return -ENXIO;
+	}
+
+	return 0;
+}
+
+/**
+ * imx334_parse_hw_config() - Parse HW configuration and check if supported
+ * @imx334: pointer to imx334 device
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx334_parse_hw_config(struct imx334 *imx334)
+{
+	struct fwnode_handle *fwnode = dev_fwnode(imx334->dev);
+	struct v4l2_fwnode_endpoint bus_cfg = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY
+	};
+	struct fwnode_handle *ep;
+	unsigned long rate;
+	unsigned int i, j;
+	int ret;
+
+	if (!fwnode)
+		return -ENXIO;
+
+	/* Request optional reset pin */
+	imx334->reset_gpio = devm_gpiod_get_optional(imx334->dev, "reset",
+						     GPIOD_OUT_LOW);
+	if (IS_ERR(imx334->reset_gpio)) {
+		dev_err(imx334->dev, "failed to get reset gpio %ld",
+			PTR_ERR(imx334->reset_gpio));
+		return PTR_ERR(imx334->reset_gpio);
+	}
+
+	/* Get sensor input clock */
+	imx334->inclk = devm_clk_get(imx334->dev, NULL);
+	if (IS_ERR(imx334->inclk)) {
+		dev_err(imx334->dev, "could not get inclk");
+		return PTR_ERR(imx334->inclk);
+	}
+
+	rate = clk_get_rate(imx334->inclk);
+	if (rate != IMX334_INCLK_RATE) {
+		dev_err(imx334->dev, "inclk frequency mismatch");
+		return -EINVAL;
+	}
+
+	ep = fwnode_graph_get_next_endpoint(fwnode, NULL);
+	if (!ep)
+		return -ENXIO;
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(ep, &bus_cfg);
+	fwnode_handle_put(ep);
+	if (ret)
+		return ret;
+
+	if (bus_cfg.bus.mipi_csi2.num_data_lanes != IMX334_NUM_DATA_LANES) {
+		dev_err(imx334->dev,
+			"number of CSI2 data lanes %d is not supported",
+			bus_cfg.bus.mipi_csi2.num_data_lanes);
+		ret = -EINVAL;
+		goto done_endpoint_free;
+	}
+
+	if (!bus_cfg.nr_of_link_frequencies) {
+		dev_err(imx334->dev, "no link frequencies defined");
+		ret = -EINVAL;
+		goto done_endpoint_free;
+	}
+
+	for (i = 0; i < bus_cfg.nr_of_link_frequencies; i++) {
+		for (j = 0; j < ARRAY_SIZE(link_freq); j++) {
+			if (bus_cfg.link_frequencies[i] == link_freq[j]) {
+				set_bit(j, &imx334->menu_skip_mask);
+				break;
+			}
+		}
+
+		if (j == ARRAY_SIZE(link_freq)) {
+			ret = dev_err_probe(imx334->dev, -EINVAL,
+					    "no supported link freq found\n");
+			goto done_endpoint_free;
+		}
+	}
+
+done_endpoint_free:
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+
+	return ret;
+}
+
+/* V4l2 subdevice ops */
+static const struct v4l2_subdev_video_ops imx334_video_ops = {
+	.s_stream = imx334_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops imx334_pad_ops = {
+	.init_cfg = imx334_init_pad_cfg,
+	.enum_mbus_code = imx334_enum_mbus_code,
+	.enum_frame_size = imx334_enum_frame_size,
+	.get_fmt = imx334_get_pad_format,
+	.set_fmt = imx334_set_pad_format,
+};
+
+static const struct v4l2_subdev_ops imx334_subdev_ops = {
+	.video = &imx334_video_ops,
+	.pad = &imx334_pad_ops,
+};
+
+/**
+ * imx334_power_on() - Sensor power on sequence
+ * @dev: pointer to i2c device
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx334_power_on(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct imx334 *imx334 = to_imx334(sd);
+	int ret;
+
+	gpiod_set_value_cansleep(imx334->reset_gpio, 1);
+
+	ret = clk_prepare_enable(imx334->inclk);
+	if (ret) {
+		dev_err(imx334->dev, "fail to enable inclk");
+		goto error_reset;
+	}
+
+	usleep_range(18000, 20000);
+
+	return 0;
+
+error_reset:
+	gpiod_set_value_cansleep(imx334->reset_gpio, 0);
+
+	return ret;
+}
+
+/**
+ * imx334_power_off() - Sensor power off sequence
+ * @dev: pointer to i2c device
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx334_power_off(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct imx334 *imx334 = to_imx334(sd);
+
+	gpiod_set_value_cansleep(imx334->reset_gpio, 0);
+
+	clk_disable_unprepare(imx334->inclk);
+
+	return 0;
+}
+
+/**
+ * imx334_init_controls() - Initialize sensor subdevice controls
+ * @imx334: pointer to imx334 device
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx334_init_controls(struct imx334 *imx334)
+{
+	struct v4l2_ctrl_handler *ctrl_hdlr = &imx334->ctrl_handler;
+	const struct imx334_mode *mode = imx334->cur_mode;
+	u32 lpfr;
+	int ret;
+
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 6);
+	if (ret)
+		return ret;
+
+	/* Serialize controls with sensor device */
+	ctrl_hdlr->lock = &imx334->mutex;
+
+	/* Initialize exposure and gain */
+	lpfr = mode->vblank + mode->height;
+	imx334->exp_ctrl = v4l2_ctrl_new_std(ctrl_hdlr,
+					     &imx334_ctrl_ops,
+					     V4L2_CID_EXPOSURE,
+					     IMX334_EXPOSURE_MIN,
+					     lpfr - IMX334_EXPOSURE_OFFSET,
+					     IMX334_EXPOSURE_STEP,
+					     IMX334_EXPOSURE_DEFAULT);
+
+	imx334->again_ctrl = v4l2_ctrl_new_std(ctrl_hdlr,
+					       &imx334_ctrl_ops,
+					       V4L2_CID_ANALOGUE_GAIN,
+					       IMX334_AGAIN_MIN,
+					       IMX334_AGAIN_MAX,
+					       IMX334_AGAIN_STEP,
+					       IMX334_AGAIN_DEFAULT);
+
+	v4l2_ctrl_cluster(2, &imx334->exp_ctrl);
+
+	imx334->vblank_ctrl = v4l2_ctrl_new_std(ctrl_hdlr,
+						&imx334_ctrl_ops,
+						V4L2_CID_VBLANK,
+						mode->vblank_min,
+						mode->vblank_max,
+						1, mode->vblank);
+
+	/* Read only controls */
+	imx334->pclk_ctrl = v4l2_ctrl_new_std(ctrl_hdlr,
+					      &imx334_ctrl_ops,
+					      V4L2_CID_PIXEL_RATE,
+					      mode->pclk, mode->pclk,
+					      1, mode->pclk);
+
+	imx334->link_freq_ctrl = v4l2_ctrl_new_int_menu(ctrl_hdlr,
+							&imx334_ctrl_ops,
+							V4L2_CID_LINK_FREQ,
+							__fls(imx334->menu_skip_mask),
+							__ffs(imx334->menu_skip_mask),
+							link_freq);
+
+	if (imx334->link_freq_ctrl)
+		imx334->link_freq_ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	imx334->hblank_ctrl = v4l2_ctrl_new_std(ctrl_hdlr,
+						&imx334_ctrl_ops,
+						V4L2_CID_HBLANK,
+						IMX334_REG_MIN,
+						IMX334_REG_MAX,
+						1, mode->hblank);
+	if (imx334->hblank_ctrl)
+		imx334->hblank_ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	if (ctrl_hdlr->error) {
+		dev_err(imx334->dev, "control init failed: %d",
+			ctrl_hdlr->error);
+		v4l2_ctrl_handler_free(ctrl_hdlr);
+		return ctrl_hdlr->error;
+	}
+
+	imx334->sd.ctrl_handler = ctrl_hdlr;
+
+	return 0;
+}
+
+/**
+ * imx334_probe() - I2C client device binding
+ * @client: pointer to i2c client device
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx334_probe(struct i2c_client *client)
+{
+	struct imx334 *imx334;
+	int ret;
+
+	imx334 = devm_kzalloc(&client->dev, sizeof(*imx334), GFP_KERNEL);
+	if (!imx334)
+		return -ENOMEM;
+
+	imx334->dev = &client->dev;
+
+	/* Initialize subdev */
+	v4l2_i2c_subdev_init(&imx334->sd, client, &imx334_subdev_ops);
+
+	ret = imx334_parse_hw_config(imx334);
+	if (ret) {
+		dev_err(imx334->dev, "HW configuration is not supported");
+		return ret;
+	}
+
+	mutex_init(&imx334->mutex);
+
+	ret = imx334_power_on(imx334->dev);
+	if (ret) {
+		dev_err(imx334->dev, "failed to power-on the sensor");
+		goto error_mutex_destroy;
+	}
+
+	/* Check module identity */
+	ret = imx334_detect(imx334);
+	if (ret) {
+		dev_err(imx334->dev, "failed to find sensor: %d", ret);
+		goto error_power_off;
+	}
+
+	/* Set default mode to max resolution */
+	imx334->cur_mode = &supported_modes[__ffs(imx334->menu_skip_mask)];
+	imx334->cur_code = imx334_mbus_codes[0];
+	imx334->vblank = imx334->cur_mode->vblank;
+
+	ret = imx334_init_controls(imx334);
+	if (ret) {
+		dev_err(imx334->dev, "failed to init controls: %d", ret);
+		goto error_power_off;
+	}
+
+	/* Initialize subdev */
+	imx334->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	imx334->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+	/* Initialize source pad */
+	imx334->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&imx334->sd.entity, 1, &imx334->pad);
+	if (ret) {
+		dev_err(imx334->dev, "failed to init entity pads: %d", ret);
+		goto error_handler_free;
+	}
+
+	ret = v4l2_async_register_subdev_sensor(&imx334->sd);
+	if (ret < 0) {
+		dev_err(imx334->dev,
+			"failed to register async subdev: %d", ret);
+		goto error_media_entity;
+	}
+
+	pm_runtime_set_active(imx334->dev);
+	pm_runtime_enable(imx334->dev);
+	pm_runtime_idle(imx334->dev);
+
+	return 0;
+
+error_media_entity:
+	media_entity_cleanup(&imx334->sd.entity);
+error_handler_free:
+	v4l2_ctrl_handler_free(imx334->sd.ctrl_handler);
+error_power_off:
+	imx334_power_off(imx334->dev);
+error_mutex_destroy:
+	mutex_destroy(&imx334->mutex);
+
+	return ret;
+}
+
+/**
+ * imx334_remove() - I2C client device unbinding
+ * @client: pointer to I2C client device
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static void imx334_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct imx334 *imx334 = to_imx334(sd);
+
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+
+	pm_runtime_disable(&client->dev);
+	pm_runtime_suspended(&client->dev);
+
+	mutex_destroy(&imx334->mutex);
+}
+
+static const struct dev_pm_ops imx334_pm_ops = {
+	SET_RUNTIME_PM_OPS(imx334_power_off, imx334_power_on, NULL)
+};
+
+static const struct of_device_id imx334_of_match[] = {
+	{ .compatible = "sony,imx334" },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(of, imx334_of_match);
+
+static struct i2c_driver imx334_driver = {
+	.probe = imx334_probe,
+	.remove = imx334_remove,
+	.driver = {
+		.name = "imx334",
+		.pm = &imx334_pm_ops,
+		.of_match_table = imx334_of_match,
+	},
+};
+
+module_i2c_driver(imx334_driver);
+
+MODULE_DESCRIPTION("Sony imx334 sensor driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/imx335.c b/drivers/media/i2c/imx335.c
new file mode 100644
index 000000000..482a0b7f0
--- /dev/null
+++ b/drivers/media/i2c/imx335.c
@@ -0,0 +1,1127 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Sony imx335 Camera Sensor Driver
+ *
+ * Copyright (C) 2021 Intel Corporation
+ */
+#include <asm/unaligned.h>
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+/* Streaming Mode */
+#define IMX335_REG_MODE_SELECT	0x3000
+#define IMX335_MODE_STANDBY	0x01
+#define IMX335_MODE_STREAMING	0x00
+
+/* Lines per frame */
+#define IMX335_REG_LPFR		0x3030
+
+/* Chip ID */
+#define IMX335_REG_ID		0x3912
+#define IMX335_ID		0x00
+
+/* Exposure control */
+#define IMX335_REG_SHUTTER	0x3058
+#define IMX335_EXPOSURE_MIN	1
+#define IMX335_EXPOSURE_OFFSET	9
+#define IMX335_EXPOSURE_STEP	1
+#define IMX335_EXPOSURE_DEFAULT	0x0648
+
+/* Analog gain control */
+#define IMX335_REG_AGAIN	0x30e8
+#define IMX335_AGAIN_MIN	0
+#define IMX335_AGAIN_MAX	240
+#define IMX335_AGAIN_STEP	1
+#define IMX335_AGAIN_DEFAULT	0
+
+/* Group hold register */
+#define IMX335_REG_HOLD		0x3001
+
+/* Input clock rate */
+#define IMX335_INCLK_RATE	24000000
+
+/* CSI2 HW configuration */
+#define IMX335_LINK_FREQ	594000000
+#define IMX335_NUM_DATA_LANES	4
+
+#define IMX335_REG_MIN		0x00
+#define IMX335_REG_MAX		0xfffff
+
+/**
+ * struct imx335_reg - imx335 sensor register
+ * @address: Register address
+ * @val: Register value
+ */
+struct imx335_reg {
+	u16 address;
+	u8 val;
+};
+
+/**
+ * struct imx335_reg_list - imx335 sensor register list
+ * @num_of_regs: Number of registers in the list
+ * @regs: Pointer to register list
+ */
+struct imx335_reg_list {
+	u32 num_of_regs;
+	const struct imx335_reg *regs;
+};
+
+/**
+ * struct imx335_mode - imx335 sensor mode structure
+ * @width: Frame width
+ * @height: Frame height
+ * @code: Format code
+ * @hblank: Horizontal blanking in lines
+ * @vblank: Vertical blanking in lines
+ * @vblank_min: Minimum vertical blanking in lines
+ * @vblank_max: Maximum vertical blanking in lines
+ * @pclk: Sensor pixel clock
+ * @link_freq_idx: Link frequency index
+ * @reg_list: Register list for sensor mode
+ */
+struct imx335_mode {
+	u32 width;
+	u32 height;
+	u32 code;
+	u32 hblank;
+	u32 vblank;
+	u32 vblank_min;
+	u32 vblank_max;
+	u64 pclk;
+	u32 link_freq_idx;
+	struct imx335_reg_list reg_list;
+};
+
+/**
+ * struct imx335 - imx335 sensor device structure
+ * @dev: Pointer to generic device
+ * @client: Pointer to i2c client
+ * @sd: V4L2 sub-device
+ * @pad: Media pad. Only one pad supported
+ * @reset_gpio: Sensor reset gpio
+ * @inclk: Sensor input clock
+ * @ctrl_handler: V4L2 control handler
+ * @link_freq_ctrl: Pointer to link frequency control
+ * @pclk_ctrl: Pointer to pixel clock control
+ * @hblank_ctrl: Pointer to horizontal blanking control
+ * @vblank_ctrl: Pointer to vertical blanking control
+ * @exp_ctrl: Pointer to exposure control
+ * @again_ctrl: Pointer to analog gain control
+ * @vblank: Vertical blanking in lines
+ * @cur_mode: Pointer to current selected sensor mode
+ * @mutex: Mutex for serializing sensor controls
+ * @streaming: Flag indicating streaming state
+ */
+struct imx335 {
+	struct device *dev;
+	struct i2c_client *client;
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct gpio_desc *reset_gpio;
+	struct clk *inclk;
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_ctrl *link_freq_ctrl;
+	struct v4l2_ctrl *pclk_ctrl;
+	struct v4l2_ctrl *hblank_ctrl;
+	struct v4l2_ctrl *vblank_ctrl;
+	struct {
+		struct v4l2_ctrl *exp_ctrl;
+		struct v4l2_ctrl *again_ctrl;
+	};
+	u32 vblank;
+	const struct imx335_mode *cur_mode;
+	struct mutex mutex;
+	bool streaming;
+};
+
+static const s64 link_freq[] = {
+	IMX335_LINK_FREQ,
+};
+
+/* Sensor mode registers */
+static const struct imx335_reg mode_2592x1940_regs[] = {
+	{0x3000, 0x01},
+	{0x3002, 0x00},
+	{0x300c, 0x3b},
+	{0x300d, 0x2a},
+	{0x3018, 0x04},
+	{0x302c, 0x3c},
+	{0x302e, 0x20},
+	{0x3056, 0x94},
+	{0x3074, 0xc8},
+	{0x3076, 0x28},
+	{0x304c, 0x00},
+	{0x314c, 0xc6},
+	{0x315a, 0x02},
+	{0x3168, 0xa0},
+	{0x316a, 0x7e},
+	{0x31a1, 0x00},
+	{0x3288, 0x21},
+	{0x328a, 0x02},
+	{0x3414, 0x05},
+	{0x3416, 0x18},
+	{0x3648, 0x01},
+	{0x364a, 0x04},
+	{0x364c, 0x04},
+	{0x3678, 0x01},
+	{0x367c, 0x31},
+	{0x367e, 0x31},
+	{0x3706, 0x10},
+	{0x3708, 0x03},
+	{0x3714, 0x02},
+	{0x3715, 0x02},
+	{0x3716, 0x01},
+	{0x3717, 0x03},
+	{0x371c, 0x3d},
+	{0x371d, 0x3f},
+	{0x372c, 0x00},
+	{0x372d, 0x00},
+	{0x372e, 0x46},
+	{0x372f, 0x00},
+	{0x3730, 0x89},
+	{0x3731, 0x00},
+	{0x3732, 0x08},
+	{0x3733, 0x01},
+	{0x3734, 0xfe},
+	{0x3735, 0x05},
+	{0x3740, 0x02},
+	{0x375d, 0x00},
+	{0x375e, 0x00},
+	{0x375f, 0x11},
+	{0x3760, 0x01},
+	{0x3768, 0x1b},
+	{0x3769, 0x1b},
+	{0x376a, 0x1b},
+	{0x376b, 0x1b},
+	{0x376c, 0x1a},
+	{0x376d, 0x17},
+	{0x376e, 0x0f},
+	{0x3776, 0x00},
+	{0x3777, 0x00},
+	{0x3778, 0x46},
+	{0x3779, 0x00},
+	{0x377a, 0x89},
+	{0x377b, 0x00},
+	{0x377c, 0x08},
+	{0x377d, 0x01},
+	{0x377e, 0x23},
+	{0x377f, 0x02},
+	{0x3780, 0xd9},
+	{0x3781, 0x03},
+	{0x3782, 0xf5},
+	{0x3783, 0x06},
+	{0x3784, 0xa5},
+	{0x3788, 0x0f},
+	{0x378a, 0xd9},
+	{0x378b, 0x03},
+	{0x378c, 0xeb},
+	{0x378d, 0x05},
+	{0x378e, 0x87},
+	{0x378f, 0x06},
+	{0x3790, 0xf5},
+	{0x3792, 0x43},
+	{0x3794, 0x7a},
+	{0x3796, 0xa1},
+	{0x37b0, 0x36},
+	{0x3a00, 0x01},
+};
+
+/* Supported sensor mode configurations */
+static const struct imx335_mode supported_mode = {
+	.width = 2592,
+	.height = 1940,
+	.hblank = 342,
+	.vblank = 2560,
+	.vblank_min = 2560,
+	.vblank_max = 133060,
+	.pclk = 396000000,
+	.link_freq_idx = 0,
+	.code = MEDIA_BUS_FMT_SRGGB12_1X12,
+	.reg_list = {
+		.num_of_regs = ARRAY_SIZE(mode_2592x1940_regs),
+		.regs = mode_2592x1940_regs,
+	},
+};
+
+/**
+ * to_imx335() - imx335 V4L2 sub-device to imx335 device.
+ * @subdev: pointer to imx335 V4L2 sub-device
+ *
+ * Return: pointer to imx335 device
+ */
+static inline struct imx335 *to_imx335(struct v4l2_subdev *subdev)
+{
+	return container_of(subdev, struct imx335, sd);
+}
+
+/**
+ * imx335_read_reg() - Read registers.
+ * @imx335: pointer to imx335 device
+ * @reg: register address
+ * @len: length of bytes to read. Max supported bytes is 4
+ * @val: pointer to register value to be filled.
+ *
+ * Big endian register addresses with little endian values.
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx335_read_reg(struct imx335 *imx335, u16 reg, u32 len, u32 *val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx335->sd);
+	struct i2c_msg msgs[2] = {0};
+	u8 addr_buf[2] = {0};
+	u8 data_buf[4] = {0};
+	int ret;
+
+	if (WARN_ON(len > 4))
+		return -EINVAL;
+
+	put_unaligned_be16(reg, addr_buf);
+
+	/* Write register address */
+	msgs[0].addr = client->addr;
+	msgs[0].flags = 0;
+	msgs[0].len = ARRAY_SIZE(addr_buf);
+	msgs[0].buf = addr_buf;
+
+	/* Read data from register */
+	msgs[1].addr = client->addr;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].len = len;
+	msgs[1].buf = data_buf;
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret != ARRAY_SIZE(msgs))
+		return -EIO;
+
+	*val = get_unaligned_le32(data_buf);
+
+	return 0;
+}
+
+/**
+ * imx335_write_reg() - Write register
+ * @imx335: pointer to imx335 device
+ * @reg: register address
+ * @len: length of bytes. Max supported bytes is 4
+ * @val: register value
+ *
+ * Big endian register addresses with little endian values.
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx335_write_reg(struct imx335 *imx335, u16 reg, u32 len, u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx335->sd);
+	u8 buf[6] = {0};
+
+	if (WARN_ON(len > 4))
+		return -EINVAL;
+
+	put_unaligned_be16(reg, buf);
+	put_unaligned_le32(val, buf + 2);
+	if (i2c_master_send(client, buf, len + 2) != len + 2)
+		return -EIO;
+
+	return 0;
+}
+
+/**
+ * imx335_write_regs() - Write a list of registers
+ * @imx335: pointer to imx335 device
+ * @regs: list of registers to be written
+ * @len: length of registers array
+ *
+ * Return: 0 if successful. error code otherwise.
+ */
+static int imx335_write_regs(struct imx335 *imx335,
+			     const struct imx335_reg *regs, u32 len)
+{
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < len; i++) {
+		ret = imx335_write_reg(imx335, regs[i].address, 1, regs[i].val);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+/**
+ * imx335_update_controls() - Update control ranges based on streaming mode
+ * @imx335: pointer to imx335 device
+ * @mode: pointer to imx335_mode sensor mode
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx335_update_controls(struct imx335 *imx335,
+				  const struct imx335_mode *mode)
+{
+	int ret;
+
+	ret = __v4l2_ctrl_s_ctrl(imx335->link_freq_ctrl, mode->link_freq_idx);
+	if (ret)
+		return ret;
+
+	ret = __v4l2_ctrl_s_ctrl(imx335->hblank_ctrl, mode->hblank);
+	if (ret)
+		return ret;
+
+	return __v4l2_ctrl_modify_range(imx335->vblank_ctrl, mode->vblank_min,
+					mode->vblank_max, 1, mode->vblank);
+}
+
+/**
+ * imx335_update_exp_gain() - Set updated exposure and gain
+ * @imx335: pointer to imx335 device
+ * @exposure: updated exposure value
+ * @gain: updated analog gain value
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx335_update_exp_gain(struct imx335 *imx335, u32 exposure, u32 gain)
+{
+	u32 lpfr, shutter;
+	int ret;
+
+	lpfr = imx335->vblank + imx335->cur_mode->height;
+	shutter = lpfr - exposure;
+
+	dev_dbg(imx335->dev, "Set exp %u, analog gain %u, shutter %u, lpfr %u",
+		exposure, gain, shutter, lpfr);
+
+	ret = imx335_write_reg(imx335, IMX335_REG_HOLD, 1, 1);
+	if (ret)
+		return ret;
+
+	ret = imx335_write_reg(imx335, IMX335_REG_LPFR, 3, lpfr);
+	if (ret)
+		goto error_release_group_hold;
+
+	ret = imx335_write_reg(imx335, IMX335_REG_SHUTTER, 3, shutter);
+	if (ret)
+		goto error_release_group_hold;
+
+	ret = imx335_write_reg(imx335, IMX335_REG_AGAIN, 2, gain);
+
+error_release_group_hold:
+	imx335_write_reg(imx335, IMX335_REG_HOLD, 1, 0);
+
+	return ret;
+}
+
+/**
+ * imx335_set_ctrl() - Set subdevice control
+ * @ctrl: pointer to v4l2_ctrl structure
+ *
+ * Supported controls:
+ * - V4L2_CID_VBLANK
+ * - cluster controls:
+ *   - V4L2_CID_ANALOGUE_GAIN
+ *   - V4L2_CID_EXPOSURE
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx335_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct imx335 *imx335 =
+		container_of(ctrl->handler, struct imx335, ctrl_handler);
+	u32 analog_gain;
+	u32 exposure;
+	int ret;
+
+	switch (ctrl->id) {
+	case V4L2_CID_VBLANK:
+		imx335->vblank = imx335->vblank_ctrl->val;
+
+		dev_dbg(imx335->dev, "Received vblank %u, new lpfr %u",
+			imx335->vblank,
+			imx335->vblank + imx335->cur_mode->height);
+
+		ret = __v4l2_ctrl_modify_range(imx335->exp_ctrl,
+					       IMX335_EXPOSURE_MIN,
+					       imx335->vblank +
+					       imx335->cur_mode->height -
+					       IMX335_EXPOSURE_OFFSET,
+					       1, IMX335_EXPOSURE_DEFAULT);
+		break;
+	case V4L2_CID_EXPOSURE:
+		/* Set controls only if sensor is in power on state */
+		if (!pm_runtime_get_if_in_use(imx335->dev))
+			return 0;
+
+		exposure = ctrl->val;
+		analog_gain = imx335->again_ctrl->val;
+
+		dev_dbg(imx335->dev, "Received exp %u, analog gain %u",
+			exposure, analog_gain);
+
+		ret = imx335_update_exp_gain(imx335, exposure, analog_gain);
+
+		pm_runtime_put(imx335->dev);
+
+		break;
+	default:
+		dev_err(imx335->dev, "Invalid control %d", ctrl->id);
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+/* V4l2 subdevice control ops*/
+static const struct v4l2_ctrl_ops imx335_ctrl_ops = {
+	.s_ctrl = imx335_set_ctrl,
+};
+
+/**
+ * imx335_enum_mbus_code() - Enumerate V4L2 sub-device mbus codes
+ * @sd: pointer to imx335 V4L2 sub-device structure
+ * @sd_state: V4L2 sub-device configuration
+ * @code: V4L2 sub-device code enumeration need to be filled
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx335_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index > 0)
+		return -EINVAL;
+
+	code->code = supported_mode.code;
+
+	return 0;
+}
+
+/**
+ * imx335_enum_frame_size() - Enumerate V4L2 sub-device frame sizes
+ * @sd: pointer to imx335 V4L2 sub-device structure
+ * @sd_state: V4L2 sub-device configuration
+ * @fsize: V4L2 sub-device size enumeration need to be filled
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx335_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fsize)
+{
+	if (fsize->index > 0)
+		return -EINVAL;
+
+	if (fsize->code != supported_mode.code)
+		return -EINVAL;
+
+	fsize->min_width = supported_mode.width;
+	fsize->max_width = fsize->min_width;
+	fsize->min_height = supported_mode.height;
+	fsize->max_height = fsize->min_height;
+
+	return 0;
+}
+
+/**
+ * imx335_fill_pad_format() - Fill subdevice pad format
+ *                            from selected sensor mode
+ * @imx335: pointer to imx335 device
+ * @mode: pointer to imx335_mode sensor mode
+ * @fmt: V4L2 sub-device format need to be filled
+ */
+static void imx335_fill_pad_format(struct imx335 *imx335,
+				   const struct imx335_mode *mode,
+				   struct v4l2_subdev_format *fmt)
+{
+	fmt->format.width = mode->width;
+	fmt->format.height = mode->height;
+	fmt->format.code = mode->code;
+	fmt->format.field = V4L2_FIELD_NONE;
+	fmt->format.colorspace = V4L2_COLORSPACE_RAW;
+	fmt->format.ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+	fmt->format.quantization = V4L2_QUANTIZATION_DEFAULT;
+	fmt->format.xfer_func = V4L2_XFER_FUNC_NONE;
+}
+
+/**
+ * imx335_get_pad_format() - Get subdevice pad format
+ * @sd: pointer to imx335 V4L2 sub-device structure
+ * @sd_state: V4L2 sub-device configuration
+ * @fmt: V4L2 sub-device format need to be set
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx335_get_pad_format(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_format *fmt)
+{
+	struct imx335 *imx335 = to_imx335(sd);
+
+	mutex_lock(&imx335->mutex);
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		struct v4l2_mbus_framefmt *framefmt;
+
+		framefmt = v4l2_subdev_get_try_format(sd, sd_state, fmt->pad);
+		fmt->format = *framefmt;
+	} else {
+		imx335_fill_pad_format(imx335, imx335->cur_mode, fmt);
+	}
+
+	mutex_unlock(&imx335->mutex);
+
+	return 0;
+}
+
+/**
+ * imx335_set_pad_format() - Set subdevice pad format
+ * @sd: pointer to imx335 V4L2 sub-device structure
+ * @sd_state: V4L2 sub-device configuration
+ * @fmt: V4L2 sub-device format need to be set
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx335_set_pad_format(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_format *fmt)
+{
+	struct imx335 *imx335 = to_imx335(sd);
+	const struct imx335_mode *mode;
+	int ret = 0;
+
+	mutex_lock(&imx335->mutex);
+
+	mode = &supported_mode;
+	imx335_fill_pad_format(imx335, mode, fmt);
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		struct v4l2_mbus_framefmt *framefmt;
+
+		framefmt = v4l2_subdev_get_try_format(sd, sd_state, fmt->pad);
+		*framefmt = fmt->format;
+	} else {
+		ret = imx335_update_controls(imx335, mode);
+		if (!ret)
+			imx335->cur_mode = mode;
+	}
+
+	mutex_unlock(&imx335->mutex);
+
+	return ret;
+}
+
+/**
+ * imx335_init_pad_cfg() - Initialize sub-device pad configuration
+ * @sd: pointer to imx335 V4L2 sub-device structure
+ * @sd_state: V4L2 sub-device configuration
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx335_init_pad_cfg(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_state *sd_state)
+{
+	struct imx335 *imx335 = to_imx335(sd);
+	struct v4l2_subdev_format fmt = { 0 };
+
+	fmt.which = sd_state ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE;
+	imx335_fill_pad_format(imx335, &supported_mode, &fmt);
+
+	return imx335_set_pad_format(sd, sd_state, &fmt);
+}
+
+/**
+ * imx335_start_streaming() - Start sensor stream
+ * @imx335: pointer to imx335 device
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx335_start_streaming(struct imx335 *imx335)
+{
+	const struct imx335_reg_list *reg_list;
+	int ret;
+
+	/* Write sensor mode registers */
+	reg_list = &imx335->cur_mode->reg_list;
+	ret = imx335_write_regs(imx335, reg_list->regs,
+				reg_list->num_of_regs);
+	if (ret) {
+		dev_err(imx335->dev, "fail to write initial registers");
+		return ret;
+	}
+
+	/* Setup handler will write actual exposure and gain */
+	ret =  __v4l2_ctrl_handler_setup(imx335->sd.ctrl_handler);
+	if (ret) {
+		dev_err(imx335->dev, "fail to setup handler");
+		return ret;
+	}
+
+	/* Start streaming */
+	ret = imx335_write_reg(imx335, IMX335_REG_MODE_SELECT,
+			       1, IMX335_MODE_STREAMING);
+	if (ret) {
+		dev_err(imx335->dev, "fail to start streaming");
+		return ret;
+	}
+
+	/* Initial regulator stabilization period */
+	usleep_range(18000, 20000);
+
+	return 0;
+}
+
+/**
+ * imx335_stop_streaming() - Stop sensor stream
+ * @imx335: pointer to imx335 device
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx335_stop_streaming(struct imx335 *imx335)
+{
+	return imx335_write_reg(imx335, IMX335_REG_MODE_SELECT,
+				1, IMX335_MODE_STANDBY);
+}
+
+/**
+ * imx335_set_stream() - Enable sensor streaming
+ * @sd: pointer to imx335 subdevice
+ * @enable: set to enable sensor streaming
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx335_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct imx335 *imx335 = to_imx335(sd);
+	int ret;
+
+	mutex_lock(&imx335->mutex);
+
+	if (imx335->streaming == enable) {
+		mutex_unlock(&imx335->mutex);
+		return 0;
+	}
+
+	if (enable) {
+		ret = pm_runtime_resume_and_get(imx335->dev);
+		if (ret)
+			goto error_unlock;
+
+		ret = imx335_start_streaming(imx335);
+		if (ret)
+			goto error_power_off;
+	} else {
+		imx335_stop_streaming(imx335);
+		pm_runtime_put(imx335->dev);
+	}
+
+	imx335->streaming = enable;
+
+	mutex_unlock(&imx335->mutex);
+
+	return 0;
+
+error_power_off:
+	pm_runtime_put(imx335->dev);
+error_unlock:
+	mutex_unlock(&imx335->mutex);
+
+	return ret;
+}
+
+/**
+ * imx335_detect() - Detect imx335 sensor
+ * @imx335: pointer to imx335 device
+ *
+ * Return: 0 if successful, -EIO if sensor id does not match
+ */
+static int imx335_detect(struct imx335 *imx335)
+{
+	int ret;
+	u32 val;
+
+	ret = imx335_read_reg(imx335, IMX335_REG_ID, 2, &val);
+	if (ret)
+		return ret;
+
+	if (val != IMX335_ID) {
+		dev_err(imx335->dev, "chip id mismatch: %x!=%x",
+			IMX335_ID, val);
+		return -ENXIO;
+	}
+
+	return 0;
+}
+
+/**
+ * imx335_parse_hw_config() - Parse HW configuration and check if supported
+ * @imx335: pointer to imx335 device
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx335_parse_hw_config(struct imx335 *imx335)
+{
+	struct fwnode_handle *fwnode = dev_fwnode(imx335->dev);
+	struct v4l2_fwnode_endpoint bus_cfg = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY
+	};
+	struct fwnode_handle *ep;
+	unsigned long rate;
+	unsigned int i;
+	int ret;
+
+	if (!fwnode)
+		return -ENXIO;
+
+	/* Request optional reset pin */
+	imx335->reset_gpio = devm_gpiod_get_optional(imx335->dev, "reset",
+						     GPIOD_OUT_LOW);
+	if (IS_ERR(imx335->reset_gpio)) {
+		dev_err(imx335->dev, "failed to get reset gpio %ld",
+			PTR_ERR(imx335->reset_gpio));
+		return PTR_ERR(imx335->reset_gpio);
+	}
+
+	/* Get sensor input clock */
+	imx335->inclk = devm_clk_get(imx335->dev, NULL);
+	if (IS_ERR(imx335->inclk)) {
+		dev_err(imx335->dev, "could not get inclk");
+		return PTR_ERR(imx335->inclk);
+	}
+
+	rate = clk_get_rate(imx335->inclk);
+	if (rate != IMX335_INCLK_RATE) {
+		dev_err(imx335->dev, "inclk frequency mismatch");
+		return -EINVAL;
+	}
+
+	ep = fwnode_graph_get_next_endpoint(fwnode, NULL);
+	if (!ep)
+		return -ENXIO;
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(ep, &bus_cfg);
+	fwnode_handle_put(ep);
+	if (ret)
+		return ret;
+
+	if (bus_cfg.bus.mipi_csi2.num_data_lanes != IMX335_NUM_DATA_LANES) {
+		dev_err(imx335->dev,
+			"number of CSI2 data lanes %d is not supported",
+			bus_cfg.bus.mipi_csi2.num_data_lanes);
+		ret = -EINVAL;
+		goto done_endpoint_free;
+	}
+
+	if (!bus_cfg.nr_of_link_frequencies) {
+		dev_err(imx335->dev, "no link frequencies defined");
+		ret = -EINVAL;
+		goto done_endpoint_free;
+	}
+
+	for (i = 0; i < bus_cfg.nr_of_link_frequencies; i++)
+		if (bus_cfg.link_frequencies[i] == IMX335_LINK_FREQ)
+			goto done_endpoint_free;
+
+	ret = -EINVAL;
+
+done_endpoint_free:
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+
+	return ret;
+}
+
+/* V4l2 subdevice ops */
+static const struct v4l2_subdev_video_ops imx335_video_ops = {
+	.s_stream = imx335_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops imx335_pad_ops = {
+	.init_cfg = imx335_init_pad_cfg,
+	.enum_mbus_code = imx335_enum_mbus_code,
+	.enum_frame_size = imx335_enum_frame_size,
+	.get_fmt = imx335_get_pad_format,
+	.set_fmt = imx335_set_pad_format,
+};
+
+static const struct v4l2_subdev_ops imx335_subdev_ops = {
+	.video = &imx335_video_ops,
+	.pad = &imx335_pad_ops,
+};
+
+/**
+ * imx335_power_on() - Sensor power on sequence
+ * @dev: pointer to i2c device
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx335_power_on(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct imx335 *imx335 = to_imx335(sd);
+	int ret;
+
+	gpiod_set_value_cansleep(imx335->reset_gpio, 1);
+
+	ret = clk_prepare_enable(imx335->inclk);
+	if (ret) {
+		dev_err(imx335->dev, "fail to enable inclk");
+		goto error_reset;
+	}
+
+	usleep_range(20, 22);
+
+	return 0;
+
+error_reset:
+	gpiod_set_value_cansleep(imx335->reset_gpio, 0);
+
+	return ret;
+}
+
+/**
+ * imx335_power_off() - Sensor power off sequence
+ * @dev: pointer to i2c device
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx335_power_off(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct imx335 *imx335 = to_imx335(sd);
+
+	gpiod_set_value_cansleep(imx335->reset_gpio, 0);
+
+	clk_disable_unprepare(imx335->inclk);
+
+	return 0;
+}
+
+/**
+ * imx335_init_controls() - Initialize sensor subdevice controls
+ * @imx335: pointer to imx335 device
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx335_init_controls(struct imx335 *imx335)
+{
+	struct v4l2_ctrl_handler *ctrl_hdlr = &imx335->ctrl_handler;
+	const struct imx335_mode *mode = imx335->cur_mode;
+	u32 lpfr;
+	int ret;
+
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 6);
+	if (ret)
+		return ret;
+
+	/* Serialize controls with sensor device */
+	ctrl_hdlr->lock = &imx335->mutex;
+
+	/* Initialize exposure and gain */
+	lpfr = mode->vblank + mode->height;
+	imx335->exp_ctrl = v4l2_ctrl_new_std(ctrl_hdlr,
+					     &imx335_ctrl_ops,
+					     V4L2_CID_EXPOSURE,
+					     IMX335_EXPOSURE_MIN,
+					     lpfr - IMX335_EXPOSURE_OFFSET,
+					     IMX335_EXPOSURE_STEP,
+					     IMX335_EXPOSURE_DEFAULT);
+
+	imx335->again_ctrl = v4l2_ctrl_new_std(ctrl_hdlr,
+					       &imx335_ctrl_ops,
+					       V4L2_CID_ANALOGUE_GAIN,
+					       IMX335_AGAIN_MIN,
+					       IMX335_AGAIN_MAX,
+					       IMX335_AGAIN_STEP,
+					       IMX335_AGAIN_DEFAULT);
+
+	v4l2_ctrl_cluster(2, &imx335->exp_ctrl);
+
+	imx335->vblank_ctrl = v4l2_ctrl_new_std(ctrl_hdlr,
+						&imx335_ctrl_ops,
+						V4L2_CID_VBLANK,
+						mode->vblank_min,
+						mode->vblank_max,
+						1, mode->vblank);
+
+	/* Read only controls */
+	imx335->pclk_ctrl = v4l2_ctrl_new_std(ctrl_hdlr,
+					      &imx335_ctrl_ops,
+					      V4L2_CID_PIXEL_RATE,
+					      mode->pclk, mode->pclk,
+					      1, mode->pclk);
+
+	imx335->link_freq_ctrl = v4l2_ctrl_new_int_menu(ctrl_hdlr,
+							&imx335_ctrl_ops,
+							V4L2_CID_LINK_FREQ,
+							ARRAY_SIZE(link_freq) -
+							1,
+							mode->link_freq_idx,
+							link_freq);
+	if (imx335->link_freq_ctrl)
+		imx335->link_freq_ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	imx335->hblank_ctrl = v4l2_ctrl_new_std(ctrl_hdlr,
+						&imx335_ctrl_ops,
+						V4L2_CID_HBLANK,
+						IMX335_REG_MIN,
+						IMX335_REG_MAX,
+						1, mode->hblank);
+	if (imx335->hblank_ctrl)
+		imx335->hblank_ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	if (ctrl_hdlr->error) {
+		dev_err(imx335->dev, "control init failed: %d",
+			ctrl_hdlr->error);
+		v4l2_ctrl_handler_free(ctrl_hdlr);
+		return ctrl_hdlr->error;
+	}
+
+	imx335->sd.ctrl_handler = ctrl_hdlr;
+
+	return 0;
+}
+
+/**
+ * imx335_probe() - I2C client device binding
+ * @client: pointer to i2c client device
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx335_probe(struct i2c_client *client)
+{
+	struct imx335 *imx335;
+	int ret;
+
+	imx335 = devm_kzalloc(&client->dev, sizeof(*imx335), GFP_KERNEL);
+	if (!imx335)
+		return -ENOMEM;
+
+	imx335->dev = &client->dev;
+
+	/* Initialize subdev */
+	v4l2_i2c_subdev_init(&imx335->sd, client, &imx335_subdev_ops);
+
+	ret = imx335_parse_hw_config(imx335);
+	if (ret) {
+		dev_err(imx335->dev, "HW configuration is not supported");
+		return ret;
+	}
+
+	mutex_init(&imx335->mutex);
+
+	ret = imx335_power_on(imx335->dev);
+	if (ret) {
+		dev_err(imx335->dev, "failed to power-on the sensor");
+		goto error_mutex_destroy;
+	}
+
+	/* Check module identity */
+	ret = imx335_detect(imx335);
+	if (ret) {
+		dev_err(imx335->dev, "failed to find sensor: %d", ret);
+		goto error_power_off;
+	}
+
+	/* Set default mode to max resolution */
+	imx335->cur_mode = &supported_mode;
+	imx335->vblank = imx335->cur_mode->vblank;
+
+	ret = imx335_init_controls(imx335);
+	if (ret) {
+		dev_err(imx335->dev, "failed to init controls: %d", ret);
+		goto error_power_off;
+	}
+
+	/* Initialize subdev */
+	imx335->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	imx335->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+	/* Initialize source pad */
+	imx335->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&imx335->sd.entity, 1, &imx335->pad);
+	if (ret) {
+		dev_err(imx335->dev, "failed to init entity pads: %d", ret);
+		goto error_handler_free;
+	}
+
+	ret = v4l2_async_register_subdev_sensor(&imx335->sd);
+	if (ret < 0) {
+		dev_err(imx335->dev,
+			"failed to register async subdev: %d", ret);
+		goto error_media_entity;
+	}
+
+	pm_runtime_set_active(imx335->dev);
+	pm_runtime_enable(imx335->dev);
+	pm_runtime_idle(imx335->dev);
+
+	return 0;
+
+error_media_entity:
+	media_entity_cleanup(&imx335->sd.entity);
+error_handler_free:
+	v4l2_ctrl_handler_free(imx335->sd.ctrl_handler);
+error_power_off:
+	imx335_power_off(imx335->dev);
+error_mutex_destroy:
+	mutex_destroy(&imx335->mutex);
+
+	return ret;
+}
+
+/**
+ * imx335_remove() - I2C client device unbinding
+ * @client: pointer to I2C client device
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static void imx335_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct imx335 *imx335 = to_imx335(sd);
+
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+
+	pm_runtime_disable(&client->dev);
+	if (!pm_runtime_status_suspended(&client->dev))
+		imx335_power_off(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+
+	mutex_destroy(&imx335->mutex);
+}
+
+static const struct dev_pm_ops imx335_pm_ops = {
+	SET_RUNTIME_PM_OPS(imx335_power_off, imx335_power_on, NULL)
+};
+
+static const struct of_device_id imx335_of_match[] = {
+	{ .compatible = "sony,imx335" },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(of, imx335_of_match);
+
+static struct i2c_driver imx335_driver = {
+	.probe = imx335_probe,
+	.remove = imx335_remove,
+	.driver = {
+		.name = "imx335",
+		.pm = &imx335_pm_ops,
+		.of_match_table = imx335_of_match,
+	},
+};
+
+module_i2c_driver(imx335_driver);
+
+MODULE_DESCRIPTION("Sony imx335 sensor driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/imx355.c b/drivers/media/i2c/imx355.c
new file mode 100644
index 000000000..059a41b7e
--- /dev/null
+++ b/drivers/media/i2c/imx355.c
@@ -0,0 +1,1860 @@
+// SPDX-License-Identifier: GPL-2.0
+// Copyright (C) 2018 Intel Corporation
+
+#include <asm/unaligned.h>
+#include <linux/acpi.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+
+#define IMX355_REG_MODE_SELECT		0x0100
+#define IMX355_MODE_STANDBY		0x00
+#define IMX355_MODE_STREAMING		0x01
+
+/* Chip ID */
+#define IMX355_REG_CHIP_ID		0x0016
+#define IMX355_CHIP_ID			0x0355
+
+/* V_TIMING internal */
+#define IMX355_REG_FLL			0x0340
+#define IMX355_FLL_MAX			0xffff
+
+/* Exposure control */
+#define IMX355_REG_EXPOSURE		0x0202
+#define IMX355_EXPOSURE_MIN		1
+#define IMX355_EXPOSURE_STEP		1
+#define IMX355_EXPOSURE_DEFAULT		0x0282
+
+/* Analog gain control */
+#define IMX355_REG_ANALOG_GAIN		0x0204
+#define IMX355_ANA_GAIN_MIN		0
+#define IMX355_ANA_GAIN_MAX		960
+#define IMX355_ANA_GAIN_STEP		1
+#define IMX355_ANA_GAIN_DEFAULT		0
+
+/* Digital gain control */
+#define IMX355_REG_DPGA_USE_GLOBAL_GAIN	0x3070
+#define IMX355_REG_DIG_GAIN_GLOBAL	0x020e
+#define IMX355_DGTL_GAIN_MIN		256
+#define IMX355_DGTL_GAIN_MAX		4095
+#define IMX355_DGTL_GAIN_STEP		1
+#define IMX355_DGTL_GAIN_DEFAULT	256
+
+/* Test Pattern Control */
+#define IMX355_REG_TEST_PATTERN		0x0600
+#define IMX355_TEST_PATTERN_DISABLED		0
+#define IMX355_TEST_PATTERN_SOLID_COLOR		1
+#define IMX355_TEST_PATTERN_COLOR_BARS		2
+#define IMX355_TEST_PATTERN_GRAY_COLOR_BARS	3
+#define IMX355_TEST_PATTERN_PN9			4
+
+/* Flip Control */
+#define IMX355_REG_ORIENTATION		0x0101
+
+/* default link frequency and external clock */
+#define IMX355_LINK_FREQ_DEFAULT	360000000
+#define IMX355_EXT_CLK			19200000
+#define IMX355_LINK_FREQ_INDEX		0
+
+struct imx355_reg {
+	u16 address;
+	u8 val;
+};
+
+struct imx355_reg_list {
+	u32 num_of_regs;
+	const struct imx355_reg *regs;
+};
+
+/* Mode : resolution and related config&values */
+struct imx355_mode {
+	/* Frame width */
+	u32 width;
+	/* Frame height */
+	u32 height;
+
+	/* V-timing */
+	u32 fll_def;
+	u32 fll_min;
+
+	/* H-timing */
+	u32 llp;
+
+	/* index of link frequency */
+	u32 link_freq_index;
+
+	/* Default register values */
+	struct imx355_reg_list reg_list;
+};
+
+struct imx355_hwcfg {
+	u32 ext_clk;			/* sensor external clk */
+	s64 *link_freqs;		/* CSI-2 link frequencies */
+	unsigned int nr_of_link_freqs;
+};
+
+struct imx355 {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+
+	struct v4l2_ctrl_handler ctrl_handler;
+	/* V4L2 Controls */
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *pixel_rate;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *exposure;
+	struct v4l2_ctrl *vflip;
+	struct v4l2_ctrl *hflip;
+
+	/* Current mode */
+	const struct imx355_mode *cur_mode;
+
+	struct imx355_hwcfg *hwcfg;
+	s64 link_def_freq;	/* CSI-2 link default frequency */
+
+	/*
+	 * Mutex for serialized access:
+	 * Protect sensor set pad format and start/stop streaming safely.
+	 * Protect access to sensor v4l2 controls.
+	 */
+	struct mutex mutex;
+
+	/* Streaming on/off */
+	bool streaming;
+};
+
+static const struct imx355_reg imx355_global_regs[] = {
+	{ 0x0136, 0x13 },
+	{ 0x0137, 0x33 },
+	{ 0x304e, 0x03 },
+	{ 0x4348, 0x16 },
+	{ 0x4350, 0x19 },
+	{ 0x4408, 0x0a },
+	{ 0x440c, 0x0b },
+	{ 0x4411, 0x5f },
+	{ 0x4412, 0x2c },
+	{ 0x4623, 0x00 },
+	{ 0x462c, 0x0f },
+	{ 0x462d, 0x00 },
+	{ 0x462e, 0x00 },
+	{ 0x4684, 0x54 },
+	{ 0x480a, 0x07 },
+	{ 0x4908, 0x07 },
+	{ 0x4909, 0x07 },
+	{ 0x490d, 0x0a },
+	{ 0x491e, 0x0f },
+	{ 0x4921, 0x06 },
+	{ 0x4923, 0x28 },
+	{ 0x4924, 0x28 },
+	{ 0x4925, 0x29 },
+	{ 0x4926, 0x29 },
+	{ 0x4927, 0x1f },
+	{ 0x4928, 0x20 },
+	{ 0x4929, 0x20 },
+	{ 0x492a, 0x20 },
+	{ 0x492c, 0x05 },
+	{ 0x492d, 0x06 },
+	{ 0x492e, 0x06 },
+	{ 0x492f, 0x06 },
+	{ 0x4930, 0x03 },
+	{ 0x4931, 0x04 },
+	{ 0x4932, 0x04 },
+	{ 0x4933, 0x05 },
+	{ 0x595e, 0x01 },
+	{ 0x5963, 0x01 },
+	{ 0x3030, 0x01 },
+	{ 0x3031, 0x01 },
+	{ 0x3045, 0x01 },
+	{ 0x4010, 0x00 },
+	{ 0x4011, 0x00 },
+	{ 0x4012, 0x00 },
+	{ 0x4013, 0x01 },
+	{ 0x68a8, 0xfe },
+	{ 0x68a9, 0xff },
+	{ 0x6888, 0x00 },
+	{ 0x6889, 0x00 },
+	{ 0x68b0, 0x00 },
+	{ 0x3058, 0x00 },
+	{ 0x305a, 0x00 },
+};
+
+static const struct imx355_reg_list imx355_global_setting = {
+	.num_of_regs = ARRAY_SIZE(imx355_global_regs),
+	.regs = imx355_global_regs,
+};
+
+static const struct imx355_reg mode_3268x2448_regs[] = {
+	{ 0x0112, 0x0a },
+	{ 0x0113, 0x0a },
+	{ 0x0114, 0x03 },
+	{ 0x0342, 0x0e },
+	{ 0x0343, 0x58 },
+	{ 0x0340, 0x0a },
+	{ 0x0341, 0x37 },
+	{ 0x0344, 0x00 },
+	{ 0x0345, 0x08 },
+	{ 0x0346, 0x00 },
+	{ 0x0347, 0x08 },
+	{ 0x0348, 0x0c },
+	{ 0x0349, 0xcb },
+	{ 0x034a, 0x09 },
+	{ 0x034b, 0x97 },
+	{ 0x0220, 0x00 },
+	{ 0x0222, 0x01 },
+	{ 0x0900, 0x00 },
+	{ 0x0901, 0x11 },
+	{ 0x0902, 0x00 },
+	{ 0x034c, 0x0c },
+	{ 0x034d, 0xc4 },
+	{ 0x034e, 0x09 },
+	{ 0x034f, 0x90 },
+	{ 0x0301, 0x05 },
+	{ 0x0303, 0x01 },
+	{ 0x0305, 0x02 },
+	{ 0x0306, 0x00 },
+	{ 0x0307, 0x78 },
+	{ 0x030b, 0x01 },
+	{ 0x030d, 0x02 },
+	{ 0x030e, 0x00 },
+	{ 0x030f, 0x4b },
+	{ 0x0310, 0x00 },
+	{ 0x0700, 0x00 },
+	{ 0x0701, 0x10 },
+	{ 0x0820, 0x0b },
+	{ 0x0821, 0x40 },
+	{ 0x3088, 0x04 },
+	{ 0x6813, 0x02 },
+	{ 0x6835, 0x07 },
+	{ 0x6836, 0x01 },
+	{ 0x6837, 0x04 },
+	{ 0x684d, 0x07 },
+	{ 0x684e, 0x01 },
+	{ 0x684f, 0x04 },
+};
+
+static const struct imx355_reg mode_3264x2448_regs[] = {
+	{ 0x0112, 0x0a },
+	{ 0x0113, 0x0a },
+	{ 0x0114, 0x03 },
+	{ 0x0342, 0x0e },
+	{ 0x0343, 0x58 },
+	{ 0x0340, 0x0a },
+	{ 0x0341, 0x37 },
+	{ 0x0344, 0x00 },
+	{ 0x0345, 0x08 },
+	{ 0x0346, 0x00 },
+	{ 0x0347, 0x08 },
+	{ 0x0348, 0x0c },
+	{ 0x0349, 0xc7 },
+	{ 0x034a, 0x09 },
+	{ 0x034b, 0x97 },
+	{ 0x0220, 0x00 },
+	{ 0x0222, 0x01 },
+	{ 0x0900, 0x00 },
+	{ 0x0901, 0x11 },
+	{ 0x0902, 0x00 },
+	{ 0x034c, 0x0c },
+	{ 0x034d, 0xc0 },
+	{ 0x034e, 0x09 },
+	{ 0x034f, 0x90 },
+	{ 0x0301, 0x05 },
+	{ 0x0303, 0x01 },
+	{ 0x0305, 0x02 },
+	{ 0x0306, 0x00 },
+	{ 0x0307, 0x78 },
+	{ 0x030b, 0x01 },
+	{ 0x030d, 0x02 },
+	{ 0x030e, 0x00 },
+	{ 0x030f, 0x4b },
+	{ 0x0310, 0x00 },
+	{ 0x0700, 0x00 },
+	{ 0x0701, 0x10 },
+	{ 0x0820, 0x0b },
+	{ 0x0821, 0x40 },
+	{ 0x3088, 0x04 },
+	{ 0x6813, 0x02 },
+	{ 0x6835, 0x07 },
+	{ 0x6836, 0x01 },
+	{ 0x6837, 0x04 },
+	{ 0x684d, 0x07 },
+	{ 0x684e, 0x01 },
+	{ 0x684f, 0x04 },
+};
+
+static const struct imx355_reg mode_3280x2464_regs[] = {
+	{ 0x0112, 0x0a },
+	{ 0x0113, 0x0a },
+	{ 0x0114, 0x03 },
+	{ 0x0342, 0x0e },
+	{ 0x0343, 0x58 },
+	{ 0x0340, 0x0a },
+	{ 0x0341, 0x37 },
+	{ 0x0344, 0x00 },
+	{ 0x0345, 0x00 },
+	{ 0x0346, 0x00 },
+	{ 0x0347, 0x00 },
+	{ 0x0348, 0x0c },
+	{ 0x0349, 0xcf },
+	{ 0x034a, 0x09 },
+	{ 0x034b, 0x9f },
+	{ 0x0220, 0x00 },
+	{ 0x0222, 0x01 },
+	{ 0x0900, 0x00 },
+	{ 0x0901, 0x11 },
+	{ 0x0902, 0x00 },
+	{ 0x034c, 0x0c },
+	{ 0x034d, 0xd0 },
+	{ 0x034e, 0x09 },
+	{ 0x034f, 0xa0 },
+	{ 0x0301, 0x05 },
+	{ 0x0303, 0x01 },
+	{ 0x0305, 0x02 },
+	{ 0x0306, 0x00 },
+	{ 0x0307, 0x78 },
+	{ 0x030b, 0x01 },
+	{ 0x030d, 0x02 },
+	{ 0x030e, 0x00 },
+	{ 0x030f, 0x4b },
+	{ 0x0310, 0x00 },
+	{ 0x0700, 0x00 },
+	{ 0x0701, 0x10 },
+	{ 0x0820, 0x0b },
+	{ 0x0821, 0x40 },
+	{ 0x3088, 0x04 },
+	{ 0x6813, 0x02 },
+	{ 0x6835, 0x07 },
+	{ 0x6836, 0x01 },
+	{ 0x6837, 0x04 },
+	{ 0x684d, 0x07 },
+	{ 0x684e, 0x01 },
+	{ 0x684f, 0x04 },
+};
+
+static const struct imx355_reg mode_1940x1096_regs[] = {
+	{ 0x0112, 0x0a },
+	{ 0x0113, 0x0a },
+	{ 0x0114, 0x03 },
+	{ 0x0342, 0x0e },
+	{ 0x0343, 0x58 },
+	{ 0x0340, 0x05 },
+	{ 0x0341, 0x1a },
+	{ 0x0344, 0x02 },
+	{ 0x0345, 0xa0 },
+	{ 0x0346, 0x02 },
+	{ 0x0347, 0xac },
+	{ 0x0348, 0x0a },
+	{ 0x0349, 0x33 },
+	{ 0x034a, 0x06 },
+	{ 0x034b, 0xf3 },
+	{ 0x0220, 0x00 },
+	{ 0x0222, 0x01 },
+	{ 0x0900, 0x00 },
+	{ 0x0901, 0x11 },
+	{ 0x0902, 0x00 },
+	{ 0x034c, 0x07 },
+	{ 0x034d, 0x94 },
+	{ 0x034e, 0x04 },
+	{ 0x034f, 0x48 },
+	{ 0x0301, 0x05 },
+	{ 0x0303, 0x01 },
+	{ 0x0305, 0x02 },
+	{ 0x0306, 0x00 },
+	{ 0x0307, 0x78 },
+	{ 0x030b, 0x01 },
+	{ 0x030d, 0x02 },
+	{ 0x030e, 0x00 },
+	{ 0x030f, 0x4b },
+	{ 0x0310, 0x00 },
+	{ 0x0700, 0x00 },
+	{ 0x0701, 0x10 },
+	{ 0x0820, 0x0b },
+	{ 0x0821, 0x40 },
+	{ 0x3088, 0x04 },
+	{ 0x6813, 0x02 },
+	{ 0x6835, 0x07 },
+	{ 0x6836, 0x01 },
+	{ 0x6837, 0x04 },
+	{ 0x684d, 0x07 },
+	{ 0x684e, 0x01 },
+	{ 0x684f, 0x04 },
+};
+
+static const struct imx355_reg mode_1936x1096_regs[] = {
+	{ 0x0112, 0x0a },
+	{ 0x0113, 0x0a },
+	{ 0x0114, 0x03 },
+	{ 0x0342, 0x0e },
+	{ 0x0343, 0x58 },
+	{ 0x0340, 0x05 },
+	{ 0x0341, 0x1a },
+	{ 0x0344, 0x02 },
+	{ 0x0345, 0xa0 },
+	{ 0x0346, 0x02 },
+	{ 0x0347, 0xac },
+	{ 0x0348, 0x0a },
+	{ 0x0349, 0x2f },
+	{ 0x034a, 0x06 },
+	{ 0x034b, 0xf3 },
+	{ 0x0220, 0x00 },
+	{ 0x0222, 0x01 },
+	{ 0x0900, 0x00 },
+	{ 0x0901, 0x11 },
+	{ 0x0902, 0x00 },
+	{ 0x034c, 0x07 },
+	{ 0x034d, 0x90 },
+	{ 0x034e, 0x04 },
+	{ 0x034f, 0x48 },
+	{ 0x0301, 0x05 },
+	{ 0x0303, 0x01 },
+	{ 0x0305, 0x02 },
+	{ 0x0306, 0x00 },
+	{ 0x0307, 0x78 },
+	{ 0x030b, 0x01 },
+	{ 0x030d, 0x02 },
+	{ 0x030e, 0x00 },
+	{ 0x030f, 0x4b },
+	{ 0x0310, 0x00 },
+	{ 0x0700, 0x00 },
+	{ 0x0701, 0x10 },
+	{ 0x0820, 0x0b },
+	{ 0x0821, 0x40 },
+	{ 0x3088, 0x04 },
+	{ 0x6813, 0x02 },
+	{ 0x6835, 0x07 },
+	{ 0x6836, 0x01 },
+	{ 0x6837, 0x04 },
+	{ 0x684d, 0x07 },
+	{ 0x684e, 0x01 },
+	{ 0x684f, 0x04 },
+};
+
+static const struct imx355_reg mode_1924x1080_regs[] = {
+	{ 0x0112, 0x0a },
+	{ 0x0113, 0x0a },
+	{ 0x0114, 0x03 },
+	{ 0x0342, 0x0e },
+	{ 0x0343, 0x58 },
+	{ 0x0340, 0x05 },
+	{ 0x0341, 0x1a },
+	{ 0x0344, 0x02 },
+	{ 0x0345, 0xa8 },
+	{ 0x0346, 0x02 },
+	{ 0x0347, 0xb4 },
+	{ 0x0348, 0x0a },
+	{ 0x0349, 0x2b },
+	{ 0x034a, 0x06 },
+	{ 0x034b, 0xeb },
+	{ 0x0220, 0x00 },
+	{ 0x0222, 0x01 },
+	{ 0x0900, 0x00 },
+	{ 0x0901, 0x11 },
+	{ 0x0902, 0x00 },
+	{ 0x034c, 0x07 },
+	{ 0x034d, 0x84 },
+	{ 0x034e, 0x04 },
+	{ 0x034f, 0x38 },
+	{ 0x0301, 0x05 },
+	{ 0x0303, 0x01 },
+	{ 0x0305, 0x02 },
+	{ 0x0306, 0x00 },
+	{ 0x0307, 0x78 },
+	{ 0x030b, 0x01 },
+	{ 0x030d, 0x02 },
+	{ 0x030e, 0x00 },
+	{ 0x030f, 0x4b },
+	{ 0x0310, 0x00 },
+	{ 0x0700, 0x00 },
+	{ 0x0701, 0x10 },
+	{ 0x0820, 0x0b },
+	{ 0x0821, 0x40 },
+	{ 0x3088, 0x04 },
+	{ 0x6813, 0x02 },
+	{ 0x6835, 0x07 },
+	{ 0x6836, 0x01 },
+	{ 0x6837, 0x04 },
+	{ 0x684d, 0x07 },
+	{ 0x684e, 0x01 },
+	{ 0x684f, 0x04 },
+};
+
+static const struct imx355_reg mode_1920x1080_regs[] = {
+	{ 0x0112, 0x0a },
+	{ 0x0113, 0x0a },
+	{ 0x0114, 0x03 },
+	{ 0x0342, 0x0e },
+	{ 0x0343, 0x58 },
+	{ 0x0340, 0x05 },
+	{ 0x0341, 0x1a },
+	{ 0x0344, 0x02 },
+	{ 0x0345, 0xa8 },
+	{ 0x0346, 0x02 },
+	{ 0x0347, 0xb4 },
+	{ 0x0348, 0x0a },
+	{ 0x0349, 0x27 },
+	{ 0x034a, 0x06 },
+	{ 0x034b, 0xeb },
+	{ 0x0220, 0x00 },
+	{ 0x0222, 0x01 },
+	{ 0x0900, 0x00 },
+	{ 0x0901, 0x11 },
+	{ 0x0902, 0x00 },
+	{ 0x034c, 0x07 },
+	{ 0x034d, 0x80 },
+	{ 0x034e, 0x04 },
+	{ 0x034f, 0x38 },
+	{ 0x0301, 0x05 },
+	{ 0x0303, 0x01 },
+	{ 0x0305, 0x02 },
+	{ 0x0306, 0x00 },
+	{ 0x0307, 0x78 },
+	{ 0x030b, 0x01 },
+	{ 0x030d, 0x02 },
+	{ 0x030e, 0x00 },
+	{ 0x030f, 0x4b },
+	{ 0x0310, 0x00 },
+	{ 0x0700, 0x00 },
+	{ 0x0701, 0x10 },
+	{ 0x0820, 0x0b },
+	{ 0x0821, 0x40 },
+	{ 0x3088, 0x04 },
+	{ 0x6813, 0x02 },
+	{ 0x6835, 0x07 },
+	{ 0x6836, 0x01 },
+	{ 0x6837, 0x04 },
+	{ 0x684d, 0x07 },
+	{ 0x684e, 0x01 },
+	{ 0x684f, 0x04 },
+};
+
+static const struct imx355_reg mode_1640x1232_regs[] = {
+	{ 0x0112, 0x0a },
+	{ 0x0113, 0x0a },
+	{ 0x0114, 0x03 },
+	{ 0x0342, 0x07 },
+	{ 0x0343, 0x2c },
+	{ 0x0340, 0x05 },
+	{ 0x0341, 0x1a },
+	{ 0x0344, 0x00 },
+	{ 0x0345, 0x00 },
+	{ 0x0346, 0x00 },
+	{ 0x0347, 0x00 },
+	{ 0x0348, 0x0c },
+	{ 0x0349, 0xcf },
+	{ 0x034a, 0x09 },
+	{ 0x034b, 0x9f },
+	{ 0x0220, 0x00 },
+	{ 0x0222, 0x01 },
+	{ 0x0900, 0x01 },
+	{ 0x0901, 0x22 },
+	{ 0x0902, 0x00 },
+	{ 0x034c, 0x06 },
+	{ 0x034d, 0x68 },
+	{ 0x034e, 0x04 },
+	{ 0x034f, 0xd0 },
+	{ 0x0301, 0x05 },
+	{ 0x0303, 0x01 },
+	{ 0x0305, 0x02 },
+	{ 0x0306, 0x00 },
+	{ 0x0307, 0x78 },
+	{ 0x030b, 0x01 },
+	{ 0x030d, 0x02 },
+	{ 0x030e, 0x00 },
+	{ 0x030f, 0x4b },
+	{ 0x0310, 0x00 },
+	{ 0x0700, 0x00 },
+	{ 0x0701, 0x10 },
+	{ 0x0820, 0x0b },
+	{ 0x0821, 0x40 },
+	{ 0x3088, 0x04 },
+	{ 0x6813, 0x02 },
+	{ 0x6835, 0x07 },
+	{ 0x6836, 0x01 },
+	{ 0x6837, 0x04 },
+	{ 0x684d, 0x07 },
+	{ 0x684e, 0x01 },
+	{ 0x684f, 0x04 },
+};
+
+static const struct imx355_reg mode_1640x922_regs[] = {
+	{ 0x0112, 0x0a },
+	{ 0x0113, 0x0a },
+	{ 0x0114, 0x03 },
+	{ 0x0342, 0x07 },
+	{ 0x0343, 0x2c },
+	{ 0x0340, 0x05 },
+	{ 0x0341, 0x1a },
+	{ 0x0344, 0x00 },
+	{ 0x0345, 0x00 },
+	{ 0x0346, 0x01 },
+	{ 0x0347, 0x30 },
+	{ 0x0348, 0x0c },
+	{ 0x0349, 0xcf },
+	{ 0x034a, 0x08 },
+	{ 0x034b, 0x63 },
+	{ 0x0220, 0x00 },
+	{ 0x0222, 0x01 },
+	{ 0x0900, 0x01 },
+	{ 0x0901, 0x22 },
+	{ 0x0902, 0x00 },
+	{ 0x034c, 0x06 },
+	{ 0x034d, 0x68 },
+	{ 0x034e, 0x03 },
+	{ 0x034f, 0x9a },
+	{ 0x0301, 0x05 },
+	{ 0x0303, 0x01 },
+	{ 0x0305, 0x02 },
+	{ 0x0306, 0x00 },
+	{ 0x0307, 0x78 },
+	{ 0x030b, 0x01 },
+	{ 0x030d, 0x02 },
+	{ 0x030e, 0x00 },
+	{ 0x030f, 0x4b },
+	{ 0x0310, 0x00 },
+	{ 0x0700, 0x00 },
+	{ 0x0701, 0x10 },
+	{ 0x0820, 0x0b },
+	{ 0x0821, 0x40 },
+	{ 0x3088, 0x04 },
+	{ 0x6813, 0x02 },
+	{ 0x6835, 0x07 },
+	{ 0x6836, 0x01 },
+	{ 0x6837, 0x04 },
+	{ 0x684d, 0x07 },
+	{ 0x684e, 0x01 },
+	{ 0x684f, 0x04 },
+};
+
+static const struct imx355_reg mode_1300x736_regs[] = {
+	{ 0x0112, 0x0a },
+	{ 0x0113, 0x0a },
+	{ 0x0114, 0x03 },
+	{ 0x0342, 0x07 },
+	{ 0x0343, 0x2c },
+	{ 0x0340, 0x05 },
+	{ 0x0341, 0x1a },
+	{ 0x0344, 0x01 },
+	{ 0x0345, 0x58 },
+	{ 0x0346, 0x01 },
+	{ 0x0347, 0xf0 },
+	{ 0x0348, 0x0b },
+	{ 0x0349, 0x7f },
+	{ 0x034a, 0x07 },
+	{ 0x034b, 0xaf },
+	{ 0x0220, 0x00 },
+	{ 0x0222, 0x01 },
+	{ 0x0900, 0x01 },
+	{ 0x0901, 0x22 },
+	{ 0x0902, 0x00 },
+	{ 0x034c, 0x05 },
+	{ 0x034d, 0x14 },
+	{ 0x034e, 0x02 },
+	{ 0x034f, 0xe0 },
+	{ 0x0301, 0x05 },
+	{ 0x0303, 0x01 },
+	{ 0x0305, 0x02 },
+	{ 0x0306, 0x00 },
+	{ 0x0307, 0x78 },
+	{ 0x030b, 0x01 },
+	{ 0x030d, 0x02 },
+	{ 0x030e, 0x00 },
+	{ 0x030f, 0x4b },
+	{ 0x0310, 0x00 },
+	{ 0x0700, 0x00 },
+	{ 0x0701, 0x10 },
+	{ 0x0820, 0x0b },
+	{ 0x0821, 0x40 },
+	{ 0x3088, 0x04 },
+	{ 0x6813, 0x02 },
+	{ 0x6835, 0x07 },
+	{ 0x6836, 0x01 },
+	{ 0x6837, 0x04 },
+	{ 0x684d, 0x07 },
+	{ 0x684e, 0x01 },
+	{ 0x684f, 0x04 },
+};
+
+static const struct imx355_reg mode_1296x736_regs[] = {
+	{ 0x0112, 0x0a },
+	{ 0x0113, 0x0a },
+	{ 0x0114, 0x03 },
+	{ 0x0342, 0x07 },
+	{ 0x0343, 0x2c },
+	{ 0x0340, 0x05 },
+	{ 0x0341, 0x1a },
+	{ 0x0344, 0x01 },
+	{ 0x0345, 0x58 },
+	{ 0x0346, 0x01 },
+	{ 0x0347, 0xf0 },
+	{ 0x0348, 0x0b },
+	{ 0x0349, 0x77 },
+	{ 0x034a, 0x07 },
+	{ 0x034b, 0xaf },
+	{ 0x0220, 0x00 },
+	{ 0x0222, 0x01 },
+	{ 0x0900, 0x01 },
+	{ 0x0901, 0x22 },
+	{ 0x0902, 0x00 },
+	{ 0x034c, 0x05 },
+	{ 0x034d, 0x10 },
+	{ 0x034e, 0x02 },
+	{ 0x034f, 0xe0 },
+	{ 0x0301, 0x05 },
+	{ 0x0303, 0x01 },
+	{ 0x0305, 0x02 },
+	{ 0x0306, 0x00 },
+	{ 0x0307, 0x78 },
+	{ 0x030b, 0x01 },
+	{ 0x030d, 0x02 },
+	{ 0x030e, 0x00 },
+	{ 0x030f, 0x4b },
+	{ 0x0310, 0x00 },
+	{ 0x0700, 0x00 },
+	{ 0x0701, 0x10 },
+	{ 0x0820, 0x0b },
+	{ 0x0821, 0x40 },
+	{ 0x3088, 0x04 },
+	{ 0x6813, 0x02 },
+	{ 0x6835, 0x07 },
+	{ 0x6836, 0x01 },
+	{ 0x6837, 0x04 },
+	{ 0x684d, 0x07 },
+	{ 0x684e, 0x01 },
+	{ 0x684f, 0x04 },
+};
+
+static const struct imx355_reg mode_1284x720_regs[] = {
+	{ 0x0112, 0x0a },
+	{ 0x0113, 0x0a },
+	{ 0x0114, 0x03 },
+	{ 0x0342, 0x07 },
+	{ 0x0343, 0x2c },
+	{ 0x0340, 0x05 },
+	{ 0x0341, 0x1a },
+	{ 0x0344, 0x01 },
+	{ 0x0345, 0x68 },
+	{ 0x0346, 0x02 },
+	{ 0x0347, 0x00 },
+	{ 0x0348, 0x0b },
+	{ 0x0349, 0x6f },
+	{ 0x034a, 0x07 },
+	{ 0x034b, 0x9f },
+	{ 0x0220, 0x00 },
+	{ 0x0222, 0x01 },
+	{ 0x0900, 0x01 },
+	{ 0x0901, 0x22 },
+	{ 0x0902, 0x00 },
+	{ 0x034c, 0x05 },
+	{ 0x034d, 0x04 },
+	{ 0x034e, 0x02 },
+	{ 0x034f, 0xd0 },
+	{ 0x0301, 0x05 },
+	{ 0x0303, 0x01 },
+	{ 0x0305, 0x02 },
+	{ 0x0306, 0x00 },
+	{ 0x0307, 0x78 },
+	{ 0x030b, 0x01 },
+	{ 0x030d, 0x02 },
+	{ 0x030e, 0x00 },
+	{ 0x030f, 0x4b },
+	{ 0x0310, 0x00 },
+	{ 0x0700, 0x00 },
+	{ 0x0701, 0x10 },
+	{ 0x0820, 0x0b },
+	{ 0x0821, 0x40 },
+	{ 0x3088, 0x04 },
+	{ 0x6813, 0x02 },
+	{ 0x6835, 0x07 },
+	{ 0x6836, 0x01 },
+	{ 0x6837, 0x04 },
+	{ 0x684d, 0x07 },
+	{ 0x684e, 0x01 },
+	{ 0x684f, 0x04 },
+};
+
+static const struct imx355_reg mode_1280x720_regs[] = {
+	{ 0x0112, 0x0a },
+	{ 0x0113, 0x0a },
+	{ 0x0114, 0x03 },
+	{ 0x0342, 0x07 },
+	{ 0x0343, 0x2c },
+	{ 0x0340, 0x05 },
+	{ 0x0341, 0x1a },
+	{ 0x0344, 0x01 },
+	{ 0x0345, 0x68 },
+	{ 0x0346, 0x02 },
+	{ 0x0347, 0x00 },
+	{ 0x0348, 0x0b },
+	{ 0x0349, 0x67 },
+	{ 0x034a, 0x07 },
+	{ 0x034b, 0x9f },
+	{ 0x0220, 0x00 },
+	{ 0x0222, 0x01 },
+	{ 0x0900, 0x01 },
+	{ 0x0901, 0x22 },
+	{ 0x0902, 0x00 },
+	{ 0x034c, 0x05 },
+	{ 0x034d, 0x00 },
+	{ 0x034e, 0x02 },
+	{ 0x034f, 0xd0 },
+	{ 0x0301, 0x05 },
+	{ 0x0303, 0x01 },
+	{ 0x0305, 0x02 },
+	{ 0x0306, 0x00 },
+	{ 0x0307, 0x78 },
+	{ 0x030b, 0x01 },
+	{ 0x030d, 0x02 },
+	{ 0x030e, 0x00 },
+	{ 0x030f, 0x4b },
+	{ 0x0310, 0x00 },
+	{ 0x0700, 0x00 },
+	{ 0x0701, 0x10 },
+	{ 0x0820, 0x0b },
+	{ 0x0821, 0x40 },
+	{ 0x3088, 0x04 },
+	{ 0x6813, 0x02 },
+	{ 0x6835, 0x07 },
+	{ 0x6836, 0x01 },
+	{ 0x6837, 0x04 },
+	{ 0x684d, 0x07 },
+	{ 0x684e, 0x01 },
+	{ 0x684f, 0x04 },
+};
+
+static const struct imx355_reg mode_820x616_regs[] = {
+	{ 0x0112, 0x0a },
+	{ 0x0113, 0x0a },
+	{ 0x0114, 0x03 },
+	{ 0x0342, 0x0e },
+	{ 0x0343, 0x58 },
+	{ 0x0340, 0x02 },
+	{ 0x0341, 0x8c },
+	{ 0x0344, 0x00 },
+	{ 0x0345, 0x00 },
+	{ 0x0346, 0x00 },
+	{ 0x0347, 0x00 },
+	{ 0x0348, 0x0c },
+	{ 0x0349, 0xcf },
+	{ 0x034a, 0x09 },
+	{ 0x034b, 0x9f },
+	{ 0x0220, 0x00 },
+	{ 0x0222, 0x01 },
+	{ 0x0900, 0x01 },
+	{ 0x0901, 0x44 },
+	{ 0x0902, 0x00 },
+	{ 0x034c, 0x03 },
+	{ 0x034d, 0x34 },
+	{ 0x034e, 0x02 },
+	{ 0x034f, 0x68 },
+	{ 0x0301, 0x05 },
+	{ 0x0303, 0x01 },
+	{ 0x0305, 0x02 },
+	{ 0x0306, 0x00 },
+	{ 0x0307, 0x78 },
+	{ 0x030b, 0x01 },
+	{ 0x030d, 0x02 },
+	{ 0x030e, 0x00 },
+	{ 0x030f, 0x4b },
+	{ 0x0310, 0x00 },
+	{ 0x0700, 0x02 },
+	{ 0x0701, 0x78 },
+	{ 0x0820, 0x0b },
+	{ 0x0821, 0x40 },
+	{ 0x3088, 0x04 },
+	{ 0x6813, 0x02 },
+	{ 0x6835, 0x07 },
+	{ 0x6836, 0x01 },
+	{ 0x6837, 0x04 },
+	{ 0x684d, 0x07 },
+	{ 0x684e, 0x01 },
+	{ 0x684f, 0x04 },
+};
+
+static const char * const imx355_test_pattern_menu[] = {
+	"Disabled",
+	"Solid Colour",
+	"Eight Vertical Colour Bars",
+	"Colour Bars With Fade to Grey",
+	"Pseudorandom Sequence (PN9)",
+};
+
+/* supported link frequencies */
+static const s64 link_freq_menu_items[] = {
+	IMX355_LINK_FREQ_DEFAULT,
+};
+
+/* Mode configs */
+static const struct imx355_mode supported_modes[] = {
+	{
+		.width = 3280,
+		.height = 2464,
+		.fll_def = 2615,
+		.fll_min = 2615,
+		.llp = 3672,
+		.link_freq_index = IMX355_LINK_FREQ_INDEX,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_3280x2464_regs),
+			.regs = mode_3280x2464_regs,
+		},
+	},
+	{
+		.width = 3268,
+		.height = 2448,
+		.fll_def = 2615,
+		.fll_min = 2615,
+		.llp = 3672,
+		.link_freq_index = IMX355_LINK_FREQ_INDEX,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_3268x2448_regs),
+			.regs = mode_3268x2448_regs,
+		},
+	},
+	{
+		.width = 3264,
+		.height = 2448,
+		.fll_def = 2615,
+		.fll_min = 2615,
+		.llp = 3672,
+		.link_freq_index = IMX355_LINK_FREQ_INDEX,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_3264x2448_regs),
+			.regs = mode_3264x2448_regs,
+		},
+	},
+	{
+		.width = 1940,
+		.height = 1096,
+		.fll_def = 1306,
+		.fll_min = 1306,
+		.llp = 3672,
+		.link_freq_index = IMX355_LINK_FREQ_INDEX,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1940x1096_regs),
+			.regs = mode_1940x1096_regs,
+		},
+	},
+	{
+		.width = 1936,
+		.height = 1096,
+		.fll_def = 1306,
+		.fll_min = 1306,
+		.llp = 3672,
+		.link_freq_index = IMX355_LINK_FREQ_INDEX,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1936x1096_regs),
+			.regs = mode_1936x1096_regs,
+		},
+	},
+	{
+		.width = 1924,
+		.height = 1080,
+		.fll_def = 1306,
+		.fll_min = 1306,
+		.llp = 3672,
+		.link_freq_index = IMX355_LINK_FREQ_INDEX,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1924x1080_regs),
+			.regs = mode_1924x1080_regs,
+		},
+	},
+	{
+		.width = 1920,
+		.height = 1080,
+		.fll_def = 1306,
+		.fll_min = 1306,
+		.llp = 3672,
+		.link_freq_index = IMX355_LINK_FREQ_INDEX,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1920x1080_regs),
+			.regs = mode_1920x1080_regs,
+		},
+	},
+	{
+		.width = 1640,
+		.height = 1232,
+		.fll_def = 1306,
+		.fll_min = 1306,
+		.llp = 1836,
+		.link_freq_index = IMX355_LINK_FREQ_INDEX,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1640x1232_regs),
+			.regs = mode_1640x1232_regs,
+		},
+	},
+	{
+		.width = 1640,
+		.height = 922,
+		.fll_def = 1306,
+		.fll_min = 1306,
+		.llp = 1836,
+		.link_freq_index = IMX355_LINK_FREQ_INDEX,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1640x922_regs),
+			.regs = mode_1640x922_regs,
+		},
+	},
+	{
+		.width = 1300,
+		.height = 736,
+		.fll_def = 1306,
+		.fll_min = 1306,
+		.llp = 1836,
+		.link_freq_index = IMX355_LINK_FREQ_INDEX,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1300x736_regs),
+			.regs = mode_1300x736_regs,
+		},
+	},
+	{
+		.width = 1296,
+		.height = 736,
+		.fll_def = 1306,
+		.fll_min = 1306,
+		.llp = 1836,
+		.link_freq_index = IMX355_LINK_FREQ_INDEX,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1296x736_regs),
+			.regs = mode_1296x736_regs,
+		},
+	},
+	{
+		.width = 1284,
+		.height = 720,
+		.fll_def = 1306,
+		.fll_min = 1306,
+		.llp = 1836,
+		.link_freq_index = IMX355_LINK_FREQ_INDEX,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1284x720_regs),
+			.regs = mode_1284x720_regs,
+		},
+	},
+	{
+		.width = 1280,
+		.height = 720,
+		.fll_def = 1306,
+		.fll_min = 1306,
+		.llp = 1836,
+		.link_freq_index = IMX355_LINK_FREQ_INDEX,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1280x720_regs),
+			.regs = mode_1280x720_regs,
+		},
+	},
+	{
+		.width = 820,
+		.height = 616,
+		.fll_def = 652,
+		.fll_min = 652,
+		.llp = 3672,
+		.link_freq_index = IMX355_LINK_FREQ_INDEX,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_820x616_regs),
+			.regs = mode_820x616_regs,
+		},
+	},
+};
+
+static inline struct imx355 *to_imx355(struct v4l2_subdev *_sd)
+{
+	return container_of(_sd, struct imx355, sd);
+}
+
+/* Get bayer order based on flip setting. */
+static u32 imx355_get_format_code(struct imx355 *imx355)
+{
+	/*
+	 * Only one bayer order is supported.
+	 * It depends on the flip settings.
+	 */
+	u32 code;
+	static const u32 codes[2][2] = {
+		{ MEDIA_BUS_FMT_SRGGB10_1X10, MEDIA_BUS_FMT_SGRBG10_1X10, },
+		{ MEDIA_BUS_FMT_SGBRG10_1X10, MEDIA_BUS_FMT_SBGGR10_1X10, },
+	};
+
+	lockdep_assert_held(&imx355->mutex);
+	code = codes[imx355->vflip->val][imx355->hflip->val];
+
+	return code;
+}
+
+/* Read registers up to 4 at a time */
+static int imx355_read_reg(struct imx355 *imx355, u16 reg, u32 len, u32 *val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx355->sd);
+	struct i2c_msg msgs[2];
+	u8 addr_buf[2];
+	u8 data_buf[4] = { 0 };
+	int ret;
+
+	if (len > 4)
+		return -EINVAL;
+
+	put_unaligned_be16(reg, addr_buf);
+	/* Write register address */
+	msgs[0].addr = client->addr;
+	msgs[0].flags = 0;
+	msgs[0].len = ARRAY_SIZE(addr_buf);
+	msgs[0].buf = addr_buf;
+
+	/* Read data from register */
+	msgs[1].addr = client->addr;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].len = len;
+	msgs[1].buf = &data_buf[4 - len];
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret != ARRAY_SIZE(msgs))
+		return -EIO;
+
+	*val = get_unaligned_be32(data_buf);
+
+	return 0;
+}
+
+/* Write registers up to 4 at a time */
+static int imx355_write_reg(struct imx355 *imx355, u16 reg, u32 len, u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx355->sd);
+	u8 buf[6];
+
+	if (len > 4)
+		return -EINVAL;
+
+	put_unaligned_be16(reg, buf);
+	put_unaligned_be32(val << (8 * (4 - len)), buf + 2);
+	if (i2c_master_send(client, buf, len + 2) != len + 2)
+		return -EIO;
+
+	return 0;
+}
+
+/* Write a list of registers */
+static int imx355_write_regs(struct imx355 *imx355,
+			     const struct imx355_reg *regs, u32 len)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx355->sd);
+	int ret;
+	u32 i;
+
+	for (i = 0; i < len; i++) {
+		ret = imx355_write_reg(imx355, regs[i].address, 1, regs[i].val);
+		if (ret) {
+			dev_err_ratelimited(&client->dev,
+					    "write reg 0x%4.4x return err %d",
+					    regs[i].address, ret);
+
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+/* Open sub-device */
+static int imx355_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct imx355 *imx355 = to_imx355(sd);
+	struct v4l2_mbus_framefmt *try_fmt =
+		v4l2_subdev_get_try_format(sd, fh->state, 0);
+
+	mutex_lock(&imx355->mutex);
+
+	/* Initialize try_fmt */
+	try_fmt->width = imx355->cur_mode->width;
+	try_fmt->height = imx355->cur_mode->height;
+	try_fmt->code = imx355_get_format_code(imx355);
+	try_fmt->field = V4L2_FIELD_NONE;
+
+	mutex_unlock(&imx355->mutex);
+
+	return 0;
+}
+
+static int imx355_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct imx355 *imx355 = container_of(ctrl->handler,
+					     struct imx355, ctrl_handler);
+	struct i2c_client *client = v4l2_get_subdevdata(&imx355->sd);
+	s64 max;
+	int ret;
+
+	/* Propagate change of current control to all related controls */
+	switch (ctrl->id) {
+	case V4L2_CID_VBLANK:
+		/* Update max exposure while meeting expected vblanking */
+		max = imx355->cur_mode->height + ctrl->val - 10;
+		__v4l2_ctrl_modify_range(imx355->exposure,
+					 imx355->exposure->minimum,
+					 max, imx355->exposure->step, max);
+		break;
+	}
+
+	/*
+	 * Applying V4L2 control value only happens
+	 * when power is up for streaming
+	 */
+	if (!pm_runtime_get_if_in_use(&client->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_ANALOGUE_GAIN:
+		/* Analog gain = 1024/(1024 - ctrl->val) times */
+		ret = imx355_write_reg(imx355, IMX355_REG_ANALOG_GAIN, 2,
+				       ctrl->val);
+		break;
+	case V4L2_CID_DIGITAL_GAIN:
+		ret = imx355_write_reg(imx355, IMX355_REG_DIG_GAIN_GLOBAL, 2,
+				       ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE:
+		ret = imx355_write_reg(imx355, IMX355_REG_EXPOSURE, 2,
+				       ctrl->val);
+		break;
+	case V4L2_CID_VBLANK:
+		/* Update FLL that meets expected vertical blanking */
+		ret = imx355_write_reg(imx355, IMX355_REG_FLL, 2,
+				       imx355->cur_mode->height + ctrl->val);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		ret = imx355_write_reg(imx355, IMX355_REG_TEST_PATTERN,
+				       2, ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+	case V4L2_CID_VFLIP:
+		ret = imx355_write_reg(imx355, IMX355_REG_ORIENTATION, 1,
+				       imx355->hflip->val |
+				       imx355->vflip->val << 1);
+		break;
+	default:
+		ret = -EINVAL;
+		dev_info(&client->dev, "ctrl(id:0x%x,val:0x%x) is not handled",
+			 ctrl->id, ctrl->val);
+		break;
+	}
+
+	pm_runtime_put(&client->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops imx355_ctrl_ops = {
+	.s_ctrl = imx355_set_ctrl,
+};
+
+static int imx355_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct imx355 *imx355 = to_imx355(sd);
+
+	if (code->index > 0)
+		return -EINVAL;
+
+	mutex_lock(&imx355->mutex);
+	code->code = imx355_get_format_code(imx355);
+	mutex_unlock(&imx355->mutex);
+
+	return 0;
+}
+
+static int imx355_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct imx355 *imx355 = to_imx355(sd);
+
+	if (fse->index >= ARRAY_SIZE(supported_modes))
+		return -EINVAL;
+
+	mutex_lock(&imx355->mutex);
+	if (fse->code != imx355_get_format_code(imx355)) {
+		mutex_unlock(&imx355->mutex);
+		return -EINVAL;
+	}
+	mutex_unlock(&imx355->mutex);
+
+	fse->min_width = supported_modes[fse->index].width;
+	fse->max_width = fse->min_width;
+	fse->min_height = supported_modes[fse->index].height;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static void imx355_update_pad_format(struct imx355 *imx355,
+				     const struct imx355_mode *mode,
+				     struct v4l2_subdev_format *fmt)
+{
+	fmt->format.width = mode->width;
+	fmt->format.height = mode->height;
+	fmt->format.code = imx355_get_format_code(imx355);
+	fmt->format.field = V4L2_FIELD_NONE;
+}
+
+static int imx355_do_get_pad_format(struct imx355 *imx355,
+				    struct v4l2_subdev_state *sd_state,
+				    struct v4l2_subdev_format *fmt)
+{
+	struct v4l2_mbus_framefmt *framefmt;
+	struct v4l2_subdev *sd = &imx355->sd;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		framefmt = v4l2_subdev_get_try_format(sd, sd_state, fmt->pad);
+		fmt->format = *framefmt;
+	} else {
+		imx355_update_pad_format(imx355, imx355->cur_mode, fmt);
+	}
+
+	return 0;
+}
+
+static int imx355_get_pad_format(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_format *fmt)
+{
+	struct imx355 *imx355 = to_imx355(sd);
+	int ret;
+
+	mutex_lock(&imx355->mutex);
+	ret = imx355_do_get_pad_format(imx355, sd_state, fmt);
+	mutex_unlock(&imx355->mutex);
+
+	return ret;
+}
+
+static int
+imx355_set_pad_format(struct v4l2_subdev *sd,
+		      struct v4l2_subdev_state *sd_state,
+		      struct v4l2_subdev_format *fmt)
+{
+	struct imx355 *imx355 = to_imx355(sd);
+	const struct imx355_mode *mode;
+	struct v4l2_mbus_framefmt *framefmt;
+	s32 vblank_def;
+	s32 vblank_min;
+	s64 h_blank;
+	u64 pixel_rate;
+	u32 height;
+
+	mutex_lock(&imx355->mutex);
+
+	/*
+	 * Only one bayer order is supported.
+	 * It depends on the flip settings.
+	 */
+	fmt->format.code = imx355_get_format_code(imx355);
+
+	mode = v4l2_find_nearest_size(supported_modes,
+				      ARRAY_SIZE(supported_modes),
+				      width, height,
+				      fmt->format.width, fmt->format.height);
+	imx355_update_pad_format(imx355, mode, fmt);
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		framefmt = v4l2_subdev_get_try_format(sd, sd_state, fmt->pad);
+		*framefmt = fmt->format;
+	} else {
+		imx355->cur_mode = mode;
+		pixel_rate = imx355->link_def_freq * 2 * 4;
+		do_div(pixel_rate, 10);
+		__v4l2_ctrl_s_ctrl_int64(imx355->pixel_rate, pixel_rate);
+		/* Update limits and set FPS to default */
+		height = imx355->cur_mode->height;
+		vblank_def = imx355->cur_mode->fll_def - height;
+		vblank_min = imx355->cur_mode->fll_min - height;
+		height = IMX355_FLL_MAX - height;
+		__v4l2_ctrl_modify_range(imx355->vblank, vblank_min, height, 1,
+					 vblank_def);
+		__v4l2_ctrl_s_ctrl(imx355->vblank, vblank_def);
+		h_blank = mode->llp - imx355->cur_mode->width;
+		/*
+		 * Currently hblank is not changeable.
+		 * So FPS control is done only by vblank.
+		 */
+		__v4l2_ctrl_modify_range(imx355->hblank, h_blank,
+					 h_blank, 1, h_blank);
+	}
+
+	mutex_unlock(&imx355->mutex);
+
+	return 0;
+}
+
+/* Start streaming */
+static int imx355_start_streaming(struct imx355 *imx355)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx355->sd);
+	const struct imx355_reg_list *reg_list;
+	int ret;
+
+	/* Global Setting */
+	reg_list = &imx355_global_setting;
+	ret = imx355_write_regs(imx355, reg_list->regs, reg_list->num_of_regs);
+	if (ret) {
+		dev_err(&client->dev, "failed to set global settings");
+		return ret;
+	}
+
+	/* Apply default values of current mode */
+	reg_list = &imx355->cur_mode->reg_list;
+	ret = imx355_write_regs(imx355, reg_list->regs, reg_list->num_of_regs);
+	if (ret) {
+		dev_err(&client->dev, "failed to set mode");
+		return ret;
+	}
+
+	/* set digital gain control to all color mode */
+	ret = imx355_write_reg(imx355, IMX355_REG_DPGA_USE_GLOBAL_GAIN, 1, 1);
+	if (ret)
+		return ret;
+
+	/* Apply customized values from user */
+	ret =  __v4l2_ctrl_handler_setup(imx355->sd.ctrl_handler);
+	if (ret)
+		return ret;
+
+	return imx355_write_reg(imx355, IMX355_REG_MODE_SELECT,
+				1, IMX355_MODE_STREAMING);
+}
+
+/* Stop streaming */
+static int imx355_stop_streaming(struct imx355 *imx355)
+{
+	return imx355_write_reg(imx355, IMX355_REG_MODE_SELECT,
+				1, IMX355_MODE_STANDBY);
+}
+
+static int imx355_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct imx355 *imx355 = to_imx355(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	mutex_lock(&imx355->mutex);
+	if (imx355->streaming == enable) {
+		mutex_unlock(&imx355->mutex);
+		return 0;
+	}
+
+	if (enable) {
+		ret = pm_runtime_resume_and_get(&client->dev);
+		if (ret < 0)
+			goto err_unlock;
+
+		/*
+		 * Apply default & customized values
+		 * and then start streaming.
+		 */
+		ret = imx355_start_streaming(imx355);
+		if (ret)
+			goto err_rpm_put;
+	} else {
+		imx355_stop_streaming(imx355);
+		pm_runtime_put(&client->dev);
+	}
+
+	imx355->streaming = enable;
+
+	/* vflip and hflip cannot change during streaming */
+	__v4l2_ctrl_grab(imx355->vflip, enable);
+	__v4l2_ctrl_grab(imx355->hflip, enable);
+
+	mutex_unlock(&imx355->mutex);
+
+	return ret;
+
+err_rpm_put:
+	pm_runtime_put(&client->dev);
+err_unlock:
+	mutex_unlock(&imx355->mutex);
+
+	return ret;
+}
+
+static int __maybe_unused imx355_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct imx355 *imx355 = to_imx355(sd);
+
+	if (imx355->streaming)
+		imx355_stop_streaming(imx355);
+
+	return 0;
+}
+
+static int __maybe_unused imx355_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct imx355 *imx355 = to_imx355(sd);
+	int ret;
+
+	if (imx355->streaming) {
+		ret = imx355_start_streaming(imx355);
+		if (ret)
+			goto error;
+	}
+
+	return 0;
+
+error:
+	imx355_stop_streaming(imx355);
+	imx355->streaming = 0;
+	return ret;
+}
+
+/* Verify chip ID */
+static int imx355_identify_module(struct imx355 *imx355)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx355->sd);
+	int ret;
+	u32 val;
+
+	ret = imx355_read_reg(imx355, IMX355_REG_CHIP_ID, 2, &val);
+	if (ret)
+		return ret;
+
+	if (val != IMX355_CHIP_ID) {
+		dev_err(&client->dev, "chip id mismatch: %x!=%x",
+			IMX355_CHIP_ID, val);
+		return -EIO;
+	}
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops imx355_subdev_core_ops = {
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_video_ops imx355_video_ops = {
+	.s_stream = imx355_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops imx355_pad_ops = {
+	.enum_mbus_code = imx355_enum_mbus_code,
+	.get_fmt = imx355_get_pad_format,
+	.set_fmt = imx355_set_pad_format,
+	.enum_frame_size = imx355_enum_frame_size,
+};
+
+static const struct v4l2_subdev_ops imx355_subdev_ops = {
+	.core = &imx355_subdev_core_ops,
+	.video = &imx355_video_ops,
+	.pad = &imx355_pad_ops,
+};
+
+static const struct media_entity_operations imx355_subdev_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static const struct v4l2_subdev_internal_ops imx355_internal_ops = {
+	.open = imx355_open,
+};
+
+/* Initialize control handlers */
+static int imx355_init_controls(struct imx355 *imx355)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx355->sd);
+	struct v4l2_ctrl_handler *ctrl_hdlr;
+	s64 exposure_max;
+	s64 vblank_def;
+	s64 vblank_min;
+	s64 hblank;
+	u64 pixel_rate;
+	const struct imx355_mode *mode;
+	u32 max;
+	int ret;
+
+	ctrl_hdlr = &imx355->ctrl_handler;
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 10);
+	if (ret)
+		return ret;
+
+	ctrl_hdlr->lock = &imx355->mutex;
+	max = ARRAY_SIZE(link_freq_menu_items) - 1;
+	imx355->link_freq = v4l2_ctrl_new_int_menu(ctrl_hdlr, &imx355_ctrl_ops,
+						   V4L2_CID_LINK_FREQ, max, 0,
+						   link_freq_menu_items);
+	if (imx355->link_freq)
+		imx355->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	/* pixel_rate = link_freq * 2 * nr_of_lanes / bits_per_sample */
+	pixel_rate = imx355->link_def_freq * 2 * 4;
+	do_div(pixel_rate, 10);
+	/* By default, PIXEL_RATE is read only */
+	imx355->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, &imx355_ctrl_ops,
+					       V4L2_CID_PIXEL_RATE, pixel_rate,
+					       pixel_rate, 1, pixel_rate);
+
+	/* Initialize vblank/hblank/exposure parameters based on current mode */
+	mode = imx355->cur_mode;
+	vblank_def = mode->fll_def - mode->height;
+	vblank_min = mode->fll_min - mode->height;
+	imx355->vblank = v4l2_ctrl_new_std(ctrl_hdlr, &imx355_ctrl_ops,
+					   V4L2_CID_VBLANK, vblank_min,
+					   IMX355_FLL_MAX - mode->height,
+					   1, vblank_def);
+
+	hblank = mode->llp - mode->width;
+	imx355->hblank = v4l2_ctrl_new_std(ctrl_hdlr, &imx355_ctrl_ops,
+					   V4L2_CID_HBLANK, hblank, hblank,
+					   1, hblank);
+	if (imx355->hblank)
+		imx355->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	/* fll >= exposure time + adjust parameter (default value is 10) */
+	exposure_max = mode->fll_def - 10;
+	imx355->exposure = v4l2_ctrl_new_std(ctrl_hdlr, &imx355_ctrl_ops,
+					     V4L2_CID_EXPOSURE,
+					     IMX355_EXPOSURE_MIN, exposure_max,
+					     IMX355_EXPOSURE_STEP,
+					     IMX355_EXPOSURE_DEFAULT);
+
+	imx355->hflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx355_ctrl_ops,
+					  V4L2_CID_HFLIP, 0, 1, 1, 0);
+	if (imx355->hflip)
+		imx355->hflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
+	imx355->vflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx355_ctrl_ops,
+					  V4L2_CID_VFLIP, 0, 1, 1, 0);
+	if (imx355->vflip)
+		imx355->vflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &imx355_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
+			  IMX355_ANA_GAIN_MIN, IMX355_ANA_GAIN_MAX,
+			  IMX355_ANA_GAIN_STEP, IMX355_ANA_GAIN_DEFAULT);
+
+	/* Digital gain */
+	v4l2_ctrl_new_std(ctrl_hdlr, &imx355_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
+			  IMX355_DGTL_GAIN_MIN, IMX355_DGTL_GAIN_MAX,
+			  IMX355_DGTL_GAIN_STEP, IMX355_DGTL_GAIN_DEFAULT);
+
+	v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &imx355_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(imx355_test_pattern_menu) - 1,
+				     0, 0, imx355_test_pattern_menu);
+	if (ctrl_hdlr->error) {
+		ret = ctrl_hdlr->error;
+		dev_err(&client->dev, "control init failed: %d", ret);
+		goto error;
+	}
+
+	imx355->sd.ctrl_handler = ctrl_hdlr;
+
+	return 0;
+
+error:
+	v4l2_ctrl_handler_free(ctrl_hdlr);
+
+	return ret;
+}
+
+static struct imx355_hwcfg *imx355_get_hwcfg(struct device *dev)
+{
+	struct imx355_hwcfg *cfg;
+	struct v4l2_fwnode_endpoint bus_cfg = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY
+	};
+	struct fwnode_handle *ep;
+	struct fwnode_handle *fwnode = dev_fwnode(dev);
+	unsigned int i;
+	int ret;
+
+	if (!fwnode)
+		return NULL;
+
+	ep = fwnode_graph_get_next_endpoint(fwnode, NULL);
+	if (!ep)
+		return NULL;
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(ep, &bus_cfg);
+	if (ret)
+		goto out_err;
+
+	cfg = devm_kzalloc(dev, sizeof(*cfg), GFP_KERNEL);
+	if (!cfg)
+		goto out_err;
+
+	ret = fwnode_property_read_u32(dev_fwnode(dev), "clock-frequency",
+				       &cfg->ext_clk);
+	if (ret) {
+		dev_err(dev, "can't get clock frequency");
+		goto out_err;
+	}
+
+	dev_dbg(dev, "ext clk: %d", cfg->ext_clk);
+	if (cfg->ext_clk != IMX355_EXT_CLK) {
+		dev_err(dev, "external clock %d is not supported",
+			cfg->ext_clk);
+		goto out_err;
+	}
+
+	dev_dbg(dev, "num of link freqs: %d", bus_cfg.nr_of_link_frequencies);
+	if (!bus_cfg.nr_of_link_frequencies) {
+		dev_warn(dev, "no link frequencies defined");
+		goto out_err;
+	}
+
+	cfg->nr_of_link_freqs = bus_cfg.nr_of_link_frequencies;
+	cfg->link_freqs = devm_kcalloc(dev,
+				       bus_cfg.nr_of_link_frequencies + 1,
+				       sizeof(*cfg->link_freqs), GFP_KERNEL);
+	if (!cfg->link_freqs)
+		goto out_err;
+
+	for (i = 0; i < bus_cfg.nr_of_link_frequencies; i++) {
+		cfg->link_freqs[i] = bus_cfg.link_frequencies[i];
+		dev_dbg(dev, "link_freq[%d] = %lld", i, cfg->link_freqs[i]);
+	}
+
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+	fwnode_handle_put(ep);
+	return cfg;
+
+out_err:
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+	fwnode_handle_put(ep);
+	return NULL;
+}
+
+static int imx355_probe(struct i2c_client *client)
+{
+	struct imx355 *imx355;
+	int ret;
+	u32 i;
+
+	imx355 = devm_kzalloc(&client->dev, sizeof(*imx355), GFP_KERNEL);
+	if (!imx355)
+		return -ENOMEM;
+
+	mutex_init(&imx355->mutex);
+
+	/* Initialize subdev */
+	v4l2_i2c_subdev_init(&imx355->sd, client, &imx355_subdev_ops);
+
+	/* Check module identity */
+	ret = imx355_identify_module(imx355);
+	if (ret) {
+		dev_err(&client->dev, "failed to find sensor: %d", ret);
+		goto error_probe;
+	}
+
+	imx355->hwcfg = imx355_get_hwcfg(&client->dev);
+	if (!imx355->hwcfg) {
+		dev_err(&client->dev, "failed to get hwcfg");
+		ret = -ENODEV;
+		goto error_probe;
+	}
+
+	imx355->link_def_freq = link_freq_menu_items[IMX355_LINK_FREQ_INDEX];
+	for (i = 0; i < imx355->hwcfg->nr_of_link_freqs; i++) {
+		if (imx355->hwcfg->link_freqs[i] == imx355->link_def_freq) {
+			dev_dbg(&client->dev, "link freq index %d matched", i);
+			break;
+		}
+	}
+
+	if (i == imx355->hwcfg->nr_of_link_freqs) {
+		dev_err(&client->dev, "no link frequency supported");
+		ret = -EINVAL;
+		goto error_probe;
+	}
+
+	/* Set default mode to max resolution */
+	imx355->cur_mode = &supported_modes[0];
+
+	ret = imx355_init_controls(imx355);
+	if (ret) {
+		dev_err(&client->dev, "failed to init controls: %d", ret);
+		goto error_probe;
+	}
+
+	/* Initialize subdev */
+	imx355->sd.internal_ops = &imx355_internal_ops;
+	imx355->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+		V4L2_SUBDEV_FL_HAS_EVENTS;
+	imx355->sd.entity.ops = &imx355_subdev_entity_ops;
+	imx355->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+	/* Initialize source pad */
+	imx355->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&imx355->sd.entity, 1, &imx355->pad);
+	if (ret) {
+		dev_err(&client->dev, "failed to init entity pads: %d", ret);
+		goto error_handler_free;
+	}
+
+	/*
+	 * Device is already turned on by i2c-core with ACPI domain PM.
+	 * Enable runtime PM and turn off the device.
+	 */
+	pm_runtime_set_active(&client->dev);
+	pm_runtime_enable(&client->dev);
+	pm_runtime_idle(&client->dev);
+
+	ret = v4l2_async_register_subdev_sensor(&imx355->sd);
+	if (ret < 0)
+		goto error_media_entity_runtime_pm;
+
+	return 0;
+
+error_media_entity_runtime_pm:
+	pm_runtime_disable(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+	media_entity_cleanup(&imx355->sd.entity);
+
+error_handler_free:
+	v4l2_ctrl_handler_free(imx355->sd.ctrl_handler);
+
+error_probe:
+	mutex_destroy(&imx355->mutex);
+
+	return ret;
+}
+
+static void imx355_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct imx355 *imx355 = to_imx355(sd);
+
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+
+	pm_runtime_disable(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+
+	mutex_destroy(&imx355->mutex);
+}
+
+static const struct dev_pm_ops imx355_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(imx355_suspend, imx355_resume)
+};
+
+static const struct acpi_device_id imx355_acpi_ids[] __maybe_unused = {
+	{ "SONY355A" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(acpi, imx355_acpi_ids);
+
+static struct i2c_driver imx355_i2c_driver = {
+	.driver = {
+		.name = "imx355",
+		.pm = &imx355_pm_ops,
+		.acpi_match_table = ACPI_PTR(imx355_acpi_ids),
+	},
+	.probe = imx355_probe,
+	.remove = imx355_remove,
+};
+module_i2c_driver(imx355_i2c_driver);
+
+MODULE_AUTHOR("Qiu, Tianshu <tian.shu.qiu@intel.com>");
+MODULE_AUTHOR("Rapolu, Chiranjeevi");
+MODULE_AUTHOR("Bingbu Cao <bingbu.cao@intel.com>");
+MODULE_AUTHOR("Yang, Hyungwoo");
+MODULE_DESCRIPTION("Sony imx355 sensor driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/imx412.c b/drivers/media/i2c/imx412.c
new file mode 100644
index 000000000..c7e862ae4
--- /dev/null
+++ b/drivers/media/i2c/imx412.c
@@ -0,0 +1,1308 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Sony imx412 Camera Sensor Driver
+ *
+ * Copyright (C) 2021 Intel Corporation
+ */
+#include <asm/unaligned.h>
+
+#include <linux/clk.h>
+#include <linux/delay.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-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+/* Streaming Mode */
+#define IMX412_REG_MODE_SELECT	0x0100
+#define IMX412_MODE_STANDBY	0x00
+#define IMX412_MODE_STREAMING	0x01
+
+/* Lines per frame */
+#define IMX412_REG_LPFR		0x0340
+
+/* Chip ID */
+#define IMX412_REG_ID		0x0016
+#define IMX412_ID		0x577
+
+/* Exposure control */
+#define IMX412_REG_EXPOSURE_CIT	0x0202
+#define IMX412_EXPOSURE_MIN	8
+#define IMX412_EXPOSURE_OFFSET	22
+#define IMX412_EXPOSURE_STEP	1
+#define IMX412_EXPOSURE_DEFAULT	0x0648
+
+/* Analog gain control */
+#define IMX412_REG_AGAIN	0x0204
+#define IMX412_AGAIN_MIN	0
+#define IMX412_AGAIN_MAX	978
+#define IMX412_AGAIN_STEP	1
+#define IMX412_AGAIN_DEFAULT	0
+
+/* Group hold register */
+#define IMX412_REG_HOLD		0x0104
+
+/* Input clock rate */
+#define IMX412_INCLK_RATE	24000000
+
+/* CSI2 HW configuration */
+#define IMX412_LINK_FREQ	600000000
+#define IMX412_NUM_DATA_LANES	4
+
+#define IMX412_REG_MIN		0x00
+#define IMX412_REG_MAX		0xffff
+
+/**
+ * struct imx412_reg - imx412 sensor register
+ * @address: Register address
+ * @val: Register value
+ */
+struct imx412_reg {
+	u16 address;
+	u8 val;
+};
+
+/**
+ * struct imx412_reg_list - imx412 sensor register list
+ * @num_of_regs: Number of registers in the list
+ * @regs: Pointer to register list
+ */
+struct imx412_reg_list {
+	u32 num_of_regs;
+	const struct imx412_reg *regs;
+};
+
+/**
+ * struct imx412_mode - imx412 sensor mode structure
+ * @width: Frame width
+ * @height: Frame height
+ * @code: Format code
+ * @hblank: Horizontal blanking in lines
+ * @vblank: Vertical blanking in lines
+ * @vblank_min: Minimum vertical blanking in lines
+ * @vblank_max: Maximum vertical blanking in lines
+ * @pclk: Sensor pixel clock
+ * @link_freq_idx: Link frequency index
+ * @reg_list: Register list for sensor mode
+ */
+struct imx412_mode {
+	u32 width;
+	u32 height;
+	u32 code;
+	u32 hblank;
+	u32 vblank;
+	u32 vblank_min;
+	u32 vblank_max;
+	u64 pclk;
+	u32 link_freq_idx;
+	struct imx412_reg_list reg_list;
+};
+
+static const char * const imx412_supply_names[] = {
+	"dovdd",	/* Digital I/O power */
+	"avdd",		/* Analog power */
+	"dvdd",		/* Digital core power */
+};
+
+/**
+ * struct imx412 - imx412 sensor device structure
+ * @dev: Pointer to generic device
+ * @client: Pointer to i2c client
+ * @sd: V4L2 sub-device
+ * @pad: Media pad. Only one pad supported
+ * @reset_gpio: Sensor reset gpio
+ * @inclk: Sensor input clock
+ * @supplies: Regulator supplies
+ * @ctrl_handler: V4L2 control handler
+ * @link_freq_ctrl: Pointer to link frequency control
+ * @pclk_ctrl: Pointer to pixel clock control
+ * @hblank_ctrl: Pointer to horizontal blanking control
+ * @vblank_ctrl: Pointer to vertical blanking control
+ * @exp_ctrl: Pointer to exposure control
+ * @again_ctrl: Pointer to analog gain control
+ * @vblank: Vertical blanking in lines
+ * @cur_mode: Pointer to current selected sensor mode
+ * @mutex: Mutex for serializing sensor controls
+ * @streaming: Flag indicating streaming state
+ */
+struct imx412 {
+	struct device *dev;
+	struct i2c_client *client;
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct gpio_desc *reset_gpio;
+	struct clk *inclk;
+	struct regulator_bulk_data supplies[ARRAY_SIZE(imx412_supply_names)];
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_ctrl *link_freq_ctrl;
+	struct v4l2_ctrl *pclk_ctrl;
+	struct v4l2_ctrl *hblank_ctrl;
+	struct v4l2_ctrl *vblank_ctrl;
+	struct {
+		struct v4l2_ctrl *exp_ctrl;
+		struct v4l2_ctrl *again_ctrl;
+	};
+	u32 vblank;
+	const struct imx412_mode *cur_mode;
+	struct mutex mutex;
+	bool streaming;
+};
+
+static const s64 link_freq[] = {
+	IMX412_LINK_FREQ,
+};
+
+/* Sensor mode registers */
+static const struct imx412_reg mode_4056x3040_regs[] = {
+	{0x0136, 0x18},
+	{0x0137, 0x00},
+	{0x3c7e, 0x08},
+	{0x3c7f, 0x02},
+	{0x38a8, 0x1f},
+	{0x38a9, 0xff},
+	{0x38aa, 0x1f},
+	{0x38ab, 0xff},
+	{0x55d4, 0x00},
+	{0x55d5, 0x00},
+	{0x55d6, 0x07},
+	{0x55d7, 0xff},
+	{0x55e8, 0x07},
+	{0x55e9, 0xff},
+	{0x55ea, 0x00},
+	{0x55eb, 0x00},
+	{0x575c, 0x07},
+	{0x575d, 0xff},
+	{0x575e, 0x00},
+	{0x575f, 0x00},
+	{0x5764, 0x00},
+	{0x5765, 0x00},
+	{0x5766, 0x07},
+	{0x5767, 0xff},
+	{0x5974, 0x04},
+	{0x5975, 0x01},
+	{0x5f10, 0x09},
+	{0x5f11, 0x92},
+	{0x5f12, 0x32},
+	{0x5f13, 0x72},
+	{0x5f14, 0x16},
+	{0x5f15, 0xba},
+	{0x5f17, 0x13},
+	{0x5f18, 0x24},
+	{0x5f19, 0x60},
+	{0x5f1a, 0xe3},
+	{0x5f1b, 0xad},
+	{0x5f1c, 0x74},
+	{0x5f2d, 0x25},
+	{0x5f5c, 0xd0},
+	{0x6a22, 0x00},
+	{0x6a23, 0x1d},
+	{0x7ba8, 0x00},
+	{0x7ba9, 0x00},
+	{0x886b, 0x00},
+	{0x9002, 0x0a},
+	{0x9004, 0x1a},
+	{0x9214, 0x93},
+	{0x9215, 0x69},
+	{0x9216, 0x93},
+	{0x9217, 0x6b},
+	{0x9218, 0x93},
+	{0x9219, 0x6d},
+	{0x921a, 0x57},
+	{0x921b, 0x58},
+	{0x921c, 0x57},
+	{0x921d, 0x59},
+	{0x921e, 0x57},
+	{0x921f, 0x5a},
+	{0x9220, 0x57},
+	{0x9221, 0x5b},
+	{0x9222, 0x93},
+	{0x9223, 0x02},
+	{0x9224, 0x93},
+	{0x9225, 0x03},
+	{0x9226, 0x93},
+	{0x9227, 0x04},
+	{0x9228, 0x93},
+	{0x9229, 0x05},
+	{0x922a, 0x98},
+	{0x922b, 0x21},
+	{0x922c, 0xb2},
+	{0x922d, 0xdb},
+	{0x922e, 0xb2},
+	{0x922f, 0xdc},
+	{0x9230, 0xb2},
+	{0x9231, 0xdd},
+	{0x9232, 0xe2},
+	{0x9233, 0xe1},
+	{0x9234, 0xb2},
+	{0x9235, 0xe2},
+	{0x9236, 0xb2},
+	{0x9237, 0xe3},
+	{0x9238, 0xb7},
+	{0x9239, 0xb9},
+	{0x923a, 0xb7},
+	{0x923b, 0xbb},
+	{0x923c, 0xb7},
+	{0x923d, 0xbc},
+	{0x923e, 0xb7},
+	{0x923f, 0xc5},
+	{0x9240, 0xb7},
+	{0x9241, 0xc7},
+	{0x9242, 0xb7},
+	{0x9243, 0xc9},
+	{0x9244, 0x98},
+	{0x9245, 0x56},
+	{0x9246, 0x98},
+	{0x9247, 0x55},
+	{0x9380, 0x00},
+	{0x9381, 0x62},
+	{0x9382, 0x00},
+	{0x9383, 0x56},
+	{0x9384, 0x00},
+	{0x9385, 0x52},
+	{0x9388, 0x00},
+	{0x9389, 0x55},
+	{0x938a, 0x00},
+	{0x938b, 0x55},
+	{0x938c, 0x00},
+	{0x938d, 0x41},
+	{0x5078, 0x01},
+	{0x0112, 0x0a},
+	{0x0113, 0x0a},
+	{0x0114, 0x03},
+	{0x0342, 0x11},
+	{0x0343, 0xa0},
+	{0x0340, 0x0d},
+	{0x0341, 0xda},
+	{0x3210, 0x00},
+	{0x0344, 0x00},
+	{0x0345, 0x00},
+	{0x0346, 0x00},
+	{0x0347, 0x00},
+	{0x0348, 0x0f},
+	{0x0349, 0xd7},
+	{0x034a, 0x0b},
+	{0x034b, 0xdf},
+	{0x00e3, 0x00},
+	{0x00e4, 0x00},
+	{0x00e5, 0x01},
+	{0x00fc, 0x0a},
+	{0x00fd, 0x0a},
+	{0x00fe, 0x0a},
+	{0x00ff, 0x0a},
+	{0xe013, 0x00},
+	{0x0220, 0x00},
+	{0x0221, 0x11},
+	{0x0381, 0x01},
+	{0x0383, 0x01},
+	{0x0385, 0x01},
+	{0x0387, 0x01},
+	{0x0900, 0x00},
+	{0x0901, 0x11},
+	{0x0902, 0x00},
+	{0x3140, 0x02},
+	{0x3241, 0x11},
+	{0x3250, 0x03},
+	{0x3e10, 0x00},
+	{0x3e11, 0x00},
+	{0x3f0d, 0x00},
+	{0x3f42, 0x00},
+	{0x3f43, 0x00},
+	{0x0401, 0x00},
+	{0x0404, 0x00},
+	{0x0405, 0x10},
+	{0x0408, 0x00},
+	{0x0409, 0x00},
+	{0x040a, 0x00},
+	{0x040b, 0x00},
+	{0x040c, 0x0f},
+	{0x040d, 0xd8},
+	{0x040e, 0x0b},
+	{0x040f, 0xe0},
+	{0x034c, 0x0f},
+	{0x034d, 0xd8},
+	{0x034e, 0x0b},
+	{0x034f, 0xe0},
+	{0x0301, 0x05},
+	{0x0303, 0x02},
+	{0x0305, 0x04},
+	{0x0306, 0x00},
+	{0x0307, 0xc8},
+	{0x0309, 0x0a},
+	{0x030b, 0x01},
+	{0x030d, 0x02},
+	{0x030e, 0x01},
+	{0x030f, 0x5e},
+	{0x0310, 0x00},
+	{0x0820, 0x12},
+	{0x0821, 0xc0},
+	{0x0822, 0x00},
+	{0x0823, 0x00},
+	{0x3e20, 0x01},
+	{0x3e37, 0x00},
+	{0x3f50, 0x00},
+	{0x3f56, 0x00},
+	{0x3f57, 0xe2},
+	{0x3c0a, 0x5a},
+	{0x3c0b, 0x55},
+	{0x3c0c, 0x28},
+	{0x3c0d, 0x07},
+	{0x3c0e, 0xff},
+	{0x3c0f, 0x00},
+	{0x3c10, 0x00},
+	{0x3c11, 0x02},
+	{0x3c12, 0x00},
+	{0x3c13, 0x03},
+	{0x3c14, 0x00},
+	{0x3c15, 0x00},
+	{0x3c16, 0x0c},
+	{0x3c17, 0x0c},
+	{0x3c18, 0x0c},
+	{0x3c19, 0x0a},
+	{0x3c1a, 0x0a},
+	{0x3c1b, 0x0a},
+	{0x3c1c, 0x00},
+	{0x3c1d, 0x00},
+	{0x3c1e, 0x00},
+	{0x3c1f, 0x00},
+	{0x3c20, 0x00},
+	{0x3c21, 0x00},
+	{0x3c22, 0x3f},
+	{0x3c23, 0x0a},
+	{0x3e35, 0x01},
+	{0x3f4a, 0x03},
+	{0x3f4b, 0xbf},
+	{0x3f26, 0x00},
+	{0x0202, 0x0d},
+	{0x0203, 0xc4},
+	{0x0204, 0x00},
+	{0x0205, 0x00},
+	{0x020e, 0x01},
+	{0x020f, 0x00},
+	{0x0210, 0x01},
+	{0x0211, 0x00},
+	{0x0212, 0x01},
+	{0x0213, 0x00},
+	{0x0214, 0x01},
+	{0x0215, 0x00},
+	{0xbcf1, 0x00},
+};
+
+/* Supported sensor mode configurations */
+static const struct imx412_mode supported_mode = {
+	.width = 4056,
+	.height = 3040,
+	.hblank = 456,
+	.vblank = 506,
+	.vblank_min = 506,
+	.vblank_max = 32420,
+	.pclk = 480000000,
+	.link_freq_idx = 0,
+	.code = MEDIA_BUS_FMT_SRGGB10_1X10,
+	.reg_list = {
+		.num_of_regs = ARRAY_SIZE(mode_4056x3040_regs),
+		.regs = mode_4056x3040_regs,
+	},
+};
+
+/**
+ * to_imx412() - imx412 V4L2 sub-device to imx412 device.
+ * @subdev: pointer to imx412 V4L2 sub-device
+ *
+ * Return: pointer to imx412 device
+ */
+static inline struct imx412 *to_imx412(struct v4l2_subdev *subdev)
+{
+	return container_of(subdev, struct imx412, sd);
+}
+
+/**
+ * imx412_read_reg() - Read registers.
+ * @imx412: pointer to imx412 device
+ * @reg: register address
+ * @len: length of bytes to read. Max supported bytes is 4
+ * @val: pointer to register value to be filled.
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx412_read_reg(struct imx412 *imx412, u16 reg, u32 len, u32 *val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx412->sd);
+	struct i2c_msg msgs[2] = {0};
+	u8 addr_buf[2] = {0};
+	u8 data_buf[4] = {0};
+	int ret;
+
+	if (WARN_ON(len > 4))
+		return -EINVAL;
+
+	put_unaligned_be16(reg, addr_buf);
+
+	/* Write register address */
+	msgs[0].addr = client->addr;
+	msgs[0].flags = 0;
+	msgs[0].len = ARRAY_SIZE(addr_buf);
+	msgs[0].buf = addr_buf;
+
+	/* Read data from register */
+	msgs[1].addr = client->addr;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].len = len;
+	msgs[1].buf = &data_buf[4 - len];
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret != ARRAY_SIZE(msgs))
+		return -EIO;
+
+	*val = get_unaligned_be32(data_buf);
+
+	return 0;
+}
+
+/**
+ * imx412_write_reg() - Write register
+ * @imx412: pointer to imx412 device
+ * @reg: register address
+ * @len: length of bytes. Max supported bytes is 4
+ * @val: register value
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx412_write_reg(struct imx412 *imx412, u16 reg, u32 len, u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx412->sd);
+	u8 buf[6] = {0};
+
+	if (WARN_ON(len > 4))
+		return -EINVAL;
+
+	put_unaligned_be16(reg, buf);
+	put_unaligned_be32(val << (8 * (4 - len)), buf + 2);
+	if (i2c_master_send(client, buf, len + 2) != len + 2)
+		return -EIO;
+
+	return 0;
+}
+
+/**
+ * imx412_write_regs() - Write a list of registers
+ * @imx412: pointer to imx412 device
+ * @regs: list of registers to be written
+ * @len: length of registers array
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx412_write_regs(struct imx412 *imx412,
+			     const struct imx412_reg *regs, u32 len)
+{
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < len; i++) {
+		ret = imx412_write_reg(imx412, regs[i].address, 1, regs[i].val);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+/**
+ * imx412_update_controls() - Update control ranges based on streaming mode
+ * @imx412: pointer to imx412 device
+ * @mode: pointer to imx412_mode sensor mode
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx412_update_controls(struct imx412 *imx412,
+				  const struct imx412_mode *mode)
+{
+	int ret;
+
+	ret = __v4l2_ctrl_s_ctrl(imx412->link_freq_ctrl, mode->link_freq_idx);
+	if (ret)
+		return ret;
+
+	ret = __v4l2_ctrl_s_ctrl(imx412->hblank_ctrl, mode->hblank);
+	if (ret)
+		return ret;
+
+	return __v4l2_ctrl_modify_range(imx412->vblank_ctrl, mode->vblank_min,
+					mode->vblank_max, 1, mode->vblank);
+}
+
+/**
+ * imx412_update_exp_gain() - Set updated exposure and gain
+ * @imx412: pointer to imx412 device
+ * @exposure: updated exposure value
+ * @gain: updated analog gain value
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx412_update_exp_gain(struct imx412 *imx412, u32 exposure, u32 gain)
+{
+	u32 lpfr, shutter;
+	int ret;
+
+	lpfr = imx412->vblank + imx412->cur_mode->height;
+	shutter = lpfr - exposure;
+
+	dev_dbg(imx412->dev, "Set exp %u, analog gain %u, shutter %u, lpfr %u",
+		exposure, gain, shutter, lpfr);
+
+	ret = imx412_write_reg(imx412, IMX412_REG_HOLD, 1, 1);
+	if (ret)
+		return ret;
+
+	ret = imx412_write_reg(imx412, IMX412_REG_LPFR, 2, lpfr);
+	if (ret)
+		goto error_release_group_hold;
+
+	ret = imx412_write_reg(imx412, IMX412_REG_EXPOSURE_CIT, 2, shutter);
+	if (ret)
+		goto error_release_group_hold;
+
+	ret = imx412_write_reg(imx412, IMX412_REG_AGAIN, 2, gain);
+
+error_release_group_hold:
+	imx412_write_reg(imx412, IMX412_REG_HOLD, 1, 0);
+
+	return ret;
+}
+
+/**
+ * imx412_set_ctrl() - Set subdevice control
+ * @ctrl: pointer to v4l2_ctrl structure
+ *
+ * Supported controls:
+ * - V4L2_CID_VBLANK
+ * - cluster controls:
+ *   - V4L2_CID_ANALOGUE_GAIN
+ *   - V4L2_CID_EXPOSURE
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx412_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct imx412 *imx412 =
+		container_of(ctrl->handler, struct imx412, ctrl_handler);
+	u32 analog_gain;
+	u32 exposure;
+	int ret;
+
+	switch (ctrl->id) {
+	case V4L2_CID_VBLANK:
+		imx412->vblank = imx412->vblank_ctrl->val;
+
+		dev_dbg(imx412->dev, "Received vblank %u, new lpfr %u",
+			imx412->vblank,
+			imx412->vblank + imx412->cur_mode->height);
+
+		ret = __v4l2_ctrl_modify_range(imx412->exp_ctrl,
+					       IMX412_EXPOSURE_MIN,
+					       imx412->vblank +
+					       imx412->cur_mode->height -
+					       IMX412_EXPOSURE_OFFSET,
+					       1, IMX412_EXPOSURE_DEFAULT);
+		break;
+	case V4L2_CID_EXPOSURE:
+		/* Set controls only if sensor is in power on state */
+		if (!pm_runtime_get_if_in_use(imx412->dev))
+			return 0;
+
+		exposure = ctrl->val;
+		analog_gain = imx412->again_ctrl->val;
+
+		dev_dbg(imx412->dev, "Received exp %u, analog gain %u",
+			exposure, analog_gain);
+
+		ret = imx412_update_exp_gain(imx412, exposure, analog_gain);
+
+		pm_runtime_put(imx412->dev);
+
+		break;
+	default:
+		dev_err(imx412->dev, "Invalid control %d", ctrl->id);
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+/* V4l2 subdevice control ops*/
+static const struct v4l2_ctrl_ops imx412_ctrl_ops = {
+	.s_ctrl = imx412_set_ctrl,
+};
+
+/**
+ * imx412_enum_mbus_code() - Enumerate V4L2 sub-device mbus codes
+ * @sd: pointer to imx412 V4L2 sub-device structure
+ * @sd_state: V4L2 sub-device configuration
+ * @code: V4L2 sub-device code enumeration need to be filled
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx412_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index > 0)
+		return -EINVAL;
+
+	code->code = supported_mode.code;
+
+	return 0;
+}
+
+/**
+ * imx412_enum_frame_size() - Enumerate V4L2 sub-device frame sizes
+ * @sd: pointer to imx412 V4L2 sub-device structure
+ * @sd_state: V4L2 sub-device configuration
+ * @fsize: V4L2 sub-device size enumeration need to be filled
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx412_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fsize)
+{
+	if (fsize->index > 0)
+		return -EINVAL;
+
+	if (fsize->code != supported_mode.code)
+		return -EINVAL;
+
+	fsize->min_width = supported_mode.width;
+	fsize->max_width = fsize->min_width;
+	fsize->min_height = supported_mode.height;
+	fsize->max_height = fsize->min_height;
+
+	return 0;
+}
+
+/**
+ * imx412_fill_pad_format() - Fill subdevice pad format
+ *                            from selected sensor mode
+ * @imx412: pointer to imx412 device
+ * @mode: pointer to imx412_mode sensor mode
+ * @fmt: V4L2 sub-device format need to be filled
+ */
+static void imx412_fill_pad_format(struct imx412 *imx412,
+				   const struct imx412_mode *mode,
+				   struct v4l2_subdev_format *fmt)
+{
+	fmt->format.width = mode->width;
+	fmt->format.height = mode->height;
+	fmt->format.code = mode->code;
+	fmt->format.field = V4L2_FIELD_NONE;
+	fmt->format.colorspace = V4L2_COLORSPACE_RAW;
+	fmt->format.ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+	fmt->format.quantization = V4L2_QUANTIZATION_DEFAULT;
+	fmt->format.xfer_func = V4L2_XFER_FUNC_NONE;
+}
+
+/**
+ * imx412_get_pad_format() - Get subdevice pad format
+ * @sd: pointer to imx412 V4L2 sub-device structure
+ * @sd_state: V4L2 sub-device configuration
+ * @fmt: V4L2 sub-device format need to be set
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx412_get_pad_format(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_format *fmt)
+{
+	struct imx412 *imx412 = to_imx412(sd);
+
+	mutex_lock(&imx412->mutex);
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		struct v4l2_mbus_framefmt *framefmt;
+
+		framefmt = v4l2_subdev_get_try_format(sd, sd_state, fmt->pad);
+		fmt->format = *framefmt;
+	} else {
+		imx412_fill_pad_format(imx412, imx412->cur_mode, fmt);
+	}
+
+	mutex_unlock(&imx412->mutex);
+
+	return 0;
+}
+
+/**
+ * imx412_set_pad_format() - Set subdevice pad format
+ * @sd: pointer to imx412 V4L2 sub-device structure
+ * @sd_state: V4L2 sub-device configuration
+ * @fmt: V4L2 sub-device format need to be set
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx412_set_pad_format(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_format *fmt)
+{
+	struct imx412 *imx412 = to_imx412(sd);
+	const struct imx412_mode *mode;
+	int ret = 0;
+
+	mutex_lock(&imx412->mutex);
+
+	mode = &supported_mode;
+	imx412_fill_pad_format(imx412, mode, fmt);
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		struct v4l2_mbus_framefmt *framefmt;
+
+		framefmt = v4l2_subdev_get_try_format(sd, sd_state, fmt->pad);
+		*framefmt = fmt->format;
+	} else {
+		ret = imx412_update_controls(imx412, mode);
+		if (!ret)
+			imx412->cur_mode = mode;
+	}
+
+	mutex_unlock(&imx412->mutex);
+
+	return ret;
+}
+
+/**
+ * imx412_init_pad_cfg() - Initialize sub-device pad configuration
+ * @sd: pointer to imx412 V4L2 sub-device structure
+ * @sd_state: V4L2 sub-device configuration
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx412_init_pad_cfg(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_state *sd_state)
+{
+	struct imx412 *imx412 = to_imx412(sd);
+	struct v4l2_subdev_format fmt = { 0 };
+
+	fmt.which = sd_state ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE;
+	imx412_fill_pad_format(imx412, &supported_mode, &fmt);
+
+	return imx412_set_pad_format(sd, sd_state, &fmt);
+}
+
+/**
+ * imx412_start_streaming() - Start sensor stream
+ * @imx412: pointer to imx412 device
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx412_start_streaming(struct imx412 *imx412)
+{
+	const struct imx412_reg_list *reg_list;
+	int ret;
+
+	/* Write sensor mode registers */
+	reg_list = &imx412->cur_mode->reg_list;
+	ret = imx412_write_regs(imx412, reg_list->regs,
+				reg_list->num_of_regs);
+	if (ret) {
+		dev_err(imx412->dev, "fail to write initial registers");
+		return ret;
+	}
+
+	/* Setup handler will write actual exposure and gain */
+	ret =  __v4l2_ctrl_handler_setup(imx412->sd.ctrl_handler);
+	if (ret) {
+		dev_err(imx412->dev, "fail to setup handler");
+		return ret;
+	}
+
+	/* Delay is required before streaming*/
+	usleep_range(7400, 8000);
+
+	/* Start streaming */
+	ret = imx412_write_reg(imx412, IMX412_REG_MODE_SELECT,
+			       1, IMX412_MODE_STREAMING);
+	if (ret) {
+		dev_err(imx412->dev, "fail to start streaming");
+		return ret;
+	}
+
+	return 0;
+}
+
+/**
+ * imx412_stop_streaming() - Stop sensor stream
+ * @imx412: pointer to imx412 device
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx412_stop_streaming(struct imx412 *imx412)
+{
+	return imx412_write_reg(imx412, IMX412_REG_MODE_SELECT,
+				1, IMX412_MODE_STANDBY);
+}
+
+/**
+ * imx412_set_stream() - Enable sensor streaming
+ * @sd: pointer to imx412 subdevice
+ * @enable: set to enable sensor streaming
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx412_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct imx412 *imx412 = to_imx412(sd);
+	int ret;
+
+	mutex_lock(&imx412->mutex);
+
+	if (imx412->streaming == enable) {
+		mutex_unlock(&imx412->mutex);
+		return 0;
+	}
+
+	if (enable) {
+		ret = pm_runtime_resume_and_get(imx412->dev);
+		if (ret)
+			goto error_unlock;
+
+		ret = imx412_start_streaming(imx412);
+		if (ret)
+			goto error_power_off;
+	} else {
+		imx412_stop_streaming(imx412);
+		pm_runtime_put(imx412->dev);
+	}
+
+	imx412->streaming = enable;
+
+	mutex_unlock(&imx412->mutex);
+
+	return 0;
+
+error_power_off:
+	pm_runtime_put(imx412->dev);
+error_unlock:
+	mutex_unlock(&imx412->mutex);
+
+	return ret;
+}
+
+/**
+ * imx412_detect() - Detect imx412 sensor
+ * @imx412: pointer to imx412 device
+ *
+ * Return: 0 if successful, -EIO if sensor id does not match
+ */
+static int imx412_detect(struct imx412 *imx412)
+{
+	int ret;
+	u32 val;
+
+	ret = imx412_read_reg(imx412, IMX412_REG_ID, 2, &val);
+	if (ret)
+		return ret;
+
+	if (val != IMX412_ID) {
+		dev_err(imx412->dev, "chip id mismatch: %x!=%x",
+			IMX412_ID, val);
+		return -ENXIO;
+	}
+
+	return 0;
+}
+
+/**
+ * imx412_parse_hw_config() - Parse HW configuration and check if supported
+ * @imx412: pointer to imx412 device
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx412_parse_hw_config(struct imx412 *imx412)
+{
+	struct fwnode_handle *fwnode = dev_fwnode(imx412->dev);
+	struct v4l2_fwnode_endpoint bus_cfg = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY
+	};
+	struct fwnode_handle *ep;
+	unsigned long rate;
+	unsigned int i;
+	int ret;
+
+	if (!fwnode)
+		return -ENXIO;
+
+	/* Request optional reset pin */
+	imx412->reset_gpio = devm_gpiod_get_optional(imx412->dev, "reset",
+						     GPIOD_OUT_LOW);
+	if (IS_ERR(imx412->reset_gpio)) {
+		dev_err(imx412->dev, "failed to get reset gpio %ld",
+			PTR_ERR(imx412->reset_gpio));
+		return PTR_ERR(imx412->reset_gpio);
+	}
+
+	/* Get sensor input clock */
+	imx412->inclk = devm_clk_get(imx412->dev, NULL);
+	if (IS_ERR(imx412->inclk)) {
+		dev_err(imx412->dev, "could not get inclk");
+		return PTR_ERR(imx412->inclk);
+	}
+
+	rate = clk_get_rate(imx412->inclk);
+	if (rate != IMX412_INCLK_RATE) {
+		dev_err(imx412->dev, "inclk frequency mismatch");
+		return -EINVAL;
+	}
+
+	/* Get optional DT defined regulators */
+	for (i = 0; i < ARRAY_SIZE(imx412_supply_names); i++)
+		imx412->supplies[i].supply = imx412_supply_names[i];
+
+	ret = devm_regulator_bulk_get(imx412->dev,
+				      ARRAY_SIZE(imx412_supply_names),
+				      imx412->supplies);
+	if (ret)
+		return ret;
+
+	ep = fwnode_graph_get_next_endpoint(fwnode, NULL);
+	if (!ep)
+		return -ENXIO;
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(ep, &bus_cfg);
+	fwnode_handle_put(ep);
+	if (ret)
+		return ret;
+
+	if (bus_cfg.bus.mipi_csi2.num_data_lanes != IMX412_NUM_DATA_LANES) {
+		dev_err(imx412->dev,
+			"number of CSI2 data lanes %d is not supported",
+			bus_cfg.bus.mipi_csi2.num_data_lanes);
+		ret = -EINVAL;
+		goto done_endpoint_free;
+	}
+
+	if (!bus_cfg.nr_of_link_frequencies) {
+		dev_err(imx412->dev, "no link frequencies defined");
+		ret = -EINVAL;
+		goto done_endpoint_free;
+	}
+
+	for (i = 0; i < bus_cfg.nr_of_link_frequencies; i++)
+		if (bus_cfg.link_frequencies[i] == IMX412_LINK_FREQ)
+			goto done_endpoint_free;
+
+	ret = -EINVAL;
+
+done_endpoint_free:
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+
+	return ret;
+}
+
+/* V4l2 subdevice ops */
+static const struct v4l2_subdev_video_ops imx412_video_ops = {
+	.s_stream = imx412_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops imx412_pad_ops = {
+	.init_cfg = imx412_init_pad_cfg,
+	.enum_mbus_code = imx412_enum_mbus_code,
+	.enum_frame_size = imx412_enum_frame_size,
+	.get_fmt = imx412_get_pad_format,
+	.set_fmt = imx412_set_pad_format,
+};
+
+static const struct v4l2_subdev_ops imx412_subdev_ops = {
+	.video = &imx412_video_ops,
+	.pad = &imx412_pad_ops,
+};
+
+/**
+ * imx412_power_on() - Sensor power on sequence
+ * @dev: pointer to i2c device
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx412_power_on(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct imx412 *imx412 = to_imx412(sd);
+	int ret;
+
+	ret = regulator_bulk_enable(ARRAY_SIZE(imx412_supply_names),
+				    imx412->supplies);
+	if (ret < 0) {
+		dev_err(dev, "failed to enable regulators\n");
+		return ret;
+	}
+
+	gpiod_set_value_cansleep(imx412->reset_gpio, 0);
+
+	ret = clk_prepare_enable(imx412->inclk);
+	if (ret) {
+		dev_err(imx412->dev, "fail to enable inclk");
+		goto error_reset;
+	}
+
+	usleep_range(1000, 1200);
+
+	return 0;
+
+error_reset:
+	gpiod_set_value_cansleep(imx412->reset_gpio, 1);
+	regulator_bulk_disable(ARRAY_SIZE(imx412_supply_names),
+			       imx412->supplies);
+
+	return ret;
+}
+
+/**
+ * imx412_power_off() - Sensor power off sequence
+ * @dev: pointer to i2c device
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx412_power_off(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct imx412 *imx412 = to_imx412(sd);
+
+	clk_disable_unprepare(imx412->inclk);
+
+	gpiod_set_value_cansleep(imx412->reset_gpio, 1);
+
+	regulator_bulk_disable(ARRAY_SIZE(imx412_supply_names),
+			       imx412->supplies);
+
+	return 0;
+}
+
+/**
+ * imx412_init_controls() - Initialize sensor subdevice controls
+ * @imx412: pointer to imx412 device
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx412_init_controls(struct imx412 *imx412)
+{
+	struct v4l2_ctrl_handler *ctrl_hdlr = &imx412->ctrl_handler;
+	const struct imx412_mode *mode = imx412->cur_mode;
+	u32 lpfr;
+	int ret;
+
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 6);
+	if (ret)
+		return ret;
+
+	/* Serialize controls with sensor device */
+	ctrl_hdlr->lock = &imx412->mutex;
+
+	/* Initialize exposure and gain */
+	lpfr = mode->vblank + mode->height;
+	imx412->exp_ctrl = v4l2_ctrl_new_std(ctrl_hdlr,
+					     &imx412_ctrl_ops,
+					     V4L2_CID_EXPOSURE,
+					     IMX412_EXPOSURE_MIN,
+					     lpfr - IMX412_EXPOSURE_OFFSET,
+					     IMX412_EXPOSURE_STEP,
+					     IMX412_EXPOSURE_DEFAULT);
+
+	imx412->again_ctrl = v4l2_ctrl_new_std(ctrl_hdlr,
+					       &imx412_ctrl_ops,
+					       V4L2_CID_ANALOGUE_GAIN,
+					       IMX412_AGAIN_MIN,
+					       IMX412_AGAIN_MAX,
+					       IMX412_AGAIN_STEP,
+					       IMX412_AGAIN_DEFAULT);
+
+	v4l2_ctrl_cluster(2, &imx412->exp_ctrl);
+
+	imx412->vblank_ctrl = v4l2_ctrl_new_std(ctrl_hdlr,
+						&imx412_ctrl_ops,
+						V4L2_CID_VBLANK,
+						mode->vblank_min,
+						mode->vblank_max,
+						1, mode->vblank);
+
+	/* Read only controls */
+	imx412->pclk_ctrl = v4l2_ctrl_new_std(ctrl_hdlr,
+					      &imx412_ctrl_ops,
+					      V4L2_CID_PIXEL_RATE,
+					      mode->pclk, mode->pclk,
+					      1, mode->pclk);
+
+	imx412->link_freq_ctrl = v4l2_ctrl_new_int_menu(ctrl_hdlr,
+							&imx412_ctrl_ops,
+							V4L2_CID_LINK_FREQ,
+							ARRAY_SIZE(link_freq) -
+							1,
+							mode->link_freq_idx,
+							link_freq);
+	if (imx412->link_freq_ctrl)
+		imx412->link_freq_ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	imx412->hblank_ctrl = v4l2_ctrl_new_std(ctrl_hdlr,
+						&imx412_ctrl_ops,
+						V4L2_CID_HBLANK,
+						IMX412_REG_MIN,
+						IMX412_REG_MAX,
+						1, mode->hblank);
+	if (imx412->hblank_ctrl)
+		imx412->hblank_ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	if (ctrl_hdlr->error) {
+		dev_err(imx412->dev, "control init failed: %d",
+			ctrl_hdlr->error);
+		v4l2_ctrl_handler_free(ctrl_hdlr);
+		return ctrl_hdlr->error;
+	}
+
+	imx412->sd.ctrl_handler = ctrl_hdlr;
+
+	return 0;
+}
+
+/**
+ * imx412_probe() - I2C client device binding
+ * @client: pointer to i2c client device
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int imx412_probe(struct i2c_client *client)
+{
+	struct imx412 *imx412;
+	const char *name;
+	int ret;
+
+	imx412 = devm_kzalloc(&client->dev, sizeof(*imx412), GFP_KERNEL);
+	if (!imx412)
+		return -ENOMEM;
+
+	imx412->dev = &client->dev;
+	name = device_get_match_data(&client->dev);
+	if (!name)
+		return -ENODEV;
+
+	/* Initialize subdev */
+	v4l2_i2c_subdev_init(&imx412->sd, client, &imx412_subdev_ops);
+
+	ret = imx412_parse_hw_config(imx412);
+	if (ret) {
+		dev_err(imx412->dev, "HW configuration is not supported");
+		return ret;
+	}
+
+	mutex_init(&imx412->mutex);
+
+	ret = imx412_power_on(imx412->dev);
+	if (ret) {
+		dev_err(imx412->dev, "failed to power-on the sensor");
+		goto error_mutex_destroy;
+	}
+
+	/* Check module identity */
+	ret = imx412_detect(imx412);
+	if (ret) {
+		dev_err(imx412->dev, "failed to find sensor: %d", ret);
+		goto error_power_off;
+	}
+
+	/* Set default mode to max resolution */
+	imx412->cur_mode = &supported_mode;
+	imx412->vblank = imx412->cur_mode->vblank;
+
+	ret = imx412_init_controls(imx412);
+	if (ret) {
+		dev_err(imx412->dev, "failed to init controls: %d", ret);
+		goto error_power_off;
+	}
+
+	/* Initialize subdev */
+	imx412->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	imx412->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+	v4l2_i2c_subdev_set_name(&imx412->sd, client, name, NULL);
+
+	/* Initialize source pad */
+	imx412->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&imx412->sd.entity, 1, &imx412->pad);
+	if (ret) {
+		dev_err(imx412->dev, "failed to init entity pads: %d", ret);
+		goto error_handler_free;
+	}
+
+	ret = v4l2_async_register_subdev_sensor(&imx412->sd);
+	if (ret < 0) {
+		dev_err(imx412->dev,
+			"failed to register async subdev: %d", ret);
+		goto error_media_entity;
+	}
+
+	pm_runtime_set_active(imx412->dev);
+	pm_runtime_enable(imx412->dev);
+	pm_runtime_idle(imx412->dev);
+
+	return 0;
+
+error_media_entity:
+	media_entity_cleanup(&imx412->sd.entity);
+error_handler_free:
+	v4l2_ctrl_handler_free(imx412->sd.ctrl_handler);
+error_power_off:
+	imx412_power_off(imx412->dev);
+error_mutex_destroy:
+	mutex_destroy(&imx412->mutex);
+
+	return ret;
+}
+
+/**
+ * imx412_remove() - I2C client device unbinding
+ * @client: pointer to I2C client device
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static void imx412_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct imx412 *imx412 = to_imx412(sd);
+
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+
+	pm_runtime_disable(&client->dev);
+	if (!pm_runtime_status_suspended(&client->dev))
+		imx412_power_off(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+
+	mutex_destroy(&imx412->mutex);
+}
+
+static const struct dev_pm_ops imx412_pm_ops = {
+	SET_RUNTIME_PM_OPS(imx412_power_off, imx412_power_on, NULL)
+};
+
+static const struct of_device_id imx412_of_match[] = {
+	{ .compatible = "sony,imx412", .data = "imx412" },
+	{ .compatible = "sony,imx577", .data = "imx577" },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(of, imx412_of_match);
+
+static struct i2c_driver imx412_driver = {
+	.probe = imx412_probe,
+	.remove = imx412_remove,
+	.driver = {
+		.name = "imx412",
+		.pm = &imx412_pm_ops,
+		.of_match_table = imx412_of_match,
+	},
+};
+
+module_i2c_driver(imx412_driver);
+
+MODULE_DESCRIPTION("Sony imx412 sensor driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/imx415.c b/drivers/media/i2c/imx415.c
new file mode 100644
index 000000000..3f00172df
--- /dev/null
+++ b/drivers/media/i2c/imx415.c
@@ -0,0 +1,1300 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Driver for the Sony IMX415 CMOS Image Sensor.
+ *
+ * Copyright (C) 2023 WolfVision GmbH.
+ */
+
+#include <linux/clk.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+#define IMX415_PIXEL_ARRAY_TOP	  0
+#define IMX415_PIXEL_ARRAY_LEFT	  0
+#define IMX415_PIXEL_ARRAY_WIDTH  3864
+#define IMX415_PIXEL_ARRAY_HEIGHT 2192
+#define IMX415_PIXEL_ARRAY_VBLANK 58
+
+#define IMX415_NUM_CLK_PARAM_REGS 11
+
+#define IMX415_REG_8BIT(n)	  ((1 << 16) | (n))
+#define IMX415_REG_16BIT(n)	  ((2 << 16) | (n))
+#define IMX415_REG_24BIT(n)	  ((3 << 16) | (n))
+#define IMX415_REG_SIZE_SHIFT	  16
+#define IMX415_REG_ADDR_MASK	  0xffff
+
+#define IMX415_MODE		  IMX415_REG_8BIT(0x3000)
+#define IMX415_MODE_OPERATING	  (0)
+#define IMX415_MODE_STANDBY	  BIT(0)
+#define IMX415_REGHOLD		  IMX415_REG_8BIT(0x3001)
+#define IMX415_REGHOLD_INVALID	  (0)
+#define IMX415_REGHOLD_VALID	  BIT(0)
+#define IMX415_XMSTA		  IMX415_REG_8BIT(0x3002)
+#define IMX415_XMSTA_START	  (0)
+#define IMX415_XMSTA_STOP	  BIT(0)
+#define IMX415_BCWAIT_TIME	  IMX415_REG_16BIT(0x3008)
+#define IMX415_CPWAIT_TIME	  IMX415_REG_16BIT(0x300A)
+#define IMX415_WINMODE		  IMX415_REG_8BIT(0x301C)
+#define IMX415_ADDMODE		  IMX415_REG_8BIT(0x3022)
+#define IMX415_REVERSE		  IMX415_REG_8BIT(0x3030)
+#define IMX415_HREVERSE_SHIFT	  (0)
+#define IMX415_VREVERSE_SHIFT	  BIT(0)
+#define IMX415_ADBIT		  IMX415_REG_8BIT(0x3031)
+#define IMX415_MDBIT		  IMX415_REG_8BIT(0x3032)
+#define IMX415_SYS_MODE		  IMX415_REG_8BIT(0x3033)
+#define IMX415_OUTSEL		  IMX415_REG_8BIT(0x30C0)
+#define IMX415_DRV		  IMX415_REG_8BIT(0x30C1)
+#define IMX415_VMAX		  IMX415_REG_24BIT(0x3024)
+#define IMX415_HMAX		  IMX415_REG_16BIT(0x3028)
+#define IMX415_SHR0		  IMX415_REG_24BIT(0x3050)
+#define IMX415_GAIN_PCG_0	  IMX415_REG_16BIT(0x3090)
+#define IMX415_AGAIN_MIN	  0
+#define IMX415_AGAIN_MAX	  100
+#define IMX415_AGAIN_STEP	  1
+#define IMX415_BLKLEVEL		  IMX415_REG_16BIT(0x30E2)
+#define IMX415_BLKLEVEL_DEFAULT	  50
+#define IMX415_TPG_EN_DUOUT	  IMX415_REG_8BIT(0x30E4)
+#define IMX415_TPG_PATSEL_DUOUT	  IMX415_REG_8BIT(0x30E6)
+#define IMX415_TPG_COLORWIDTH	  IMX415_REG_8BIT(0x30E8)
+#define IMX415_TESTCLKEN_MIPI	  IMX415_REG_8BIT(0x3110)
+#define IMX415_INCKSEL1		  IMX415_REG_8BIT(0x3115)
+#define IMX415_INCKSEL2		  IMX415_REG_8BIT(0x3116)
+#define IMX415_INCKSEL3		  IMX415_REG_16BIT(0x3118)
+#define IMX415_INCKSEL4		  IMX415_REG_16BIT(0x311A)
+#define IMX415_INCKSEL5		  IMX415_REG_8BIT(0x311E)
+#define IMX415_DIG_CLP_MODE	  IMX415_REG_8BIT(0x32C8)
+#define IMX415_WRJ_OPEN		  IMX415_REG_8BIT(0x3390)
+#define IMX415_SENSOR_INFO	  IMX415_REG_16BIT(0x3F12)
+#define IMX415_SENSOR_INFO_MASK	  0xFFF
+#define IMX415_CHIP_ID		  0x514
+#define IMX415_LANEMODE		  IMX415_REG_16BIT(0x4001)
+#define IMX415_LANEMODE_2	  1
+#define IMX415_LANEMODE_4	  3
+#define IMX415_TXCLKESC_FREQ	  IMX415_REG_16BIT(0x4004)
+#define IMX415_INCKSEL6		  IMX415_REG_8BIT(0x400C)
+#define IMX415_TCLKPOST		  IMX415_REG_16BIT(0x4018)
+#define IMX415_TCLKPREPARE	  IMX415_REG_16BIT(0x401A)
+#define IMX415_TCLKTRAIL	  IMX415_REG_16BIT(0x401C)
+#define IMX415_TCLKZERO		  IMX415_REG_16BIT(0x401E)
+#define IMX415_THSPREPARE	  IMX415_REG_16BIT(0x4020)
+#define IMX415_THSZERO		  IMX415_REG_16BIT(0x4022)
+#define IMX415_THSTRAIL		  IMX415_REG_16BIT(0x4024)
+#define IMX415_THSEXIT		  IMX415_REG_16BIT(0x4026)
+#define IMX415_TLPX		  IMX415_REG_16BIT(0x4028)
+#define IMX415_INCKSEL7		  IMX415_REG_8BIT(0x4074)
+
+struct imx415_reg {
+	u32 address;
+	u32 val;
+};
+
+static const char *const imx415_supply_names[] = {
+	"dvdd",
+	"ovdd",
+	"avdd",
+};
+
+/*
+ * The IMX415 data sheet uses lane rates but v4l2 uses link frequency to
+ * describe MIPI CSI-2 speed. This driver uses lane rates wherever possible
+ * and converts them to link frequencies by a factor of two when needed.
+ */
+static const s64 link_freq_menu_items[] = {
+	594000000 / 2,	720000000 / 2,	891000000 / 2,
+	1440000000 / 2, 1485000000 / 2,
+};
+
+struct imx415_clk_params {
+	u64 lane_rate;
+	u64 inck;
+	struct imx415_reg regs[IMX415_NUM_CLK_PARAM_REGS];
+};
+
+/* INCK Settings - includes all lane rate and INCK dependent registers */
+static const struct imx415_clk_params imx415_clk_params[] = {
+	{
+		.lane_rate = 594000000,
+		.inck = 27000000,
+		.regs[0] = { IMX415_BCWAIT_TIME, 0x05D },
+		.regs[1] = { IMX415_CPWAIT_TIME, 0x042 },
+		.regs[2] = { IMX415_SYS_MODE, 0x7 },
+		.regs[3] = { IMX415_INCKSEL1, 0x00 },
+		.regs[4] = { IMX415_INCKSEL2, 0x23 },
+		.regs[5] = { IMX415_INCKSEL3, 0x084 },
+		.regs[6] = { IMX415_INCKSEL4, 0x0E7 },
+		.regs[7] = { IMX415_INCKSEL5, 0x23 },
+		.regs[8] = { IMX415_INCKSEL6, 0x0 },
+		.regs[9] = { IMX415_INCKSEL7, 0x1 },
+		.regs[10] = { IMX415_TXCLKESC_FREQ, 0x06C0 },
+	},
+	{
+		.lane_rate = 720000000,
+		.inck = 24000000,
+		.regs[0] = { IMX415_BCWAIT_TIME, 0x054 },
+		.regs[1] = { IMX415_CPWAIT_TIME, 0x03B },
+		.regs[2] = { IMX415_SYS_MODE, 0x9 },
+		.regs[3] = { IMX415_INCKSEL1, 0x00 },
+		.regs[4] = { IMX415_INCKSEL2, 0x23 },
+		.regs[5] = { IMX415_INCKSEL3, 0x0B4 },
+		.regs[6] = { IMX415_INCKSEL4, 0x0FC },
+		.regs[7] = { IMX415_INCKSEL5, 0x23 },
+		.regs[8] = { IMX415_INCKSEL6, 0x0 },
+		.regs[9] = { IMX415_INCKSEL7, 0x1 },
+		.regs[10] = { IMX415_TXCLKESC_FREQ, 0x0600 },
+	},
+	{
+		.lane_rate = 891000000,
+		.inck = 27000000,
+		.regs[0] = { IMX415_BCWAIT_TIME, 0x05D },
+		.regs[1] = { IMX415_CPWAIT_TIME, 0x042 },
+		.regs[2] = { IMX415_SYS_MODE, 0x5 },
+		.regs[3] = { IMX415_INCKSEL1, 0x00 },
+		.regs[4] = { IMX415_INCKSEL2, 0x23 },
+		.regs[5] = { IMX415_INCKSEL3, 0x0C6 },
+		.regs[6] = { IMX415_INCKSEL4, 0x0E7 },
+		.regs[7] = { IMX415_INCKSEL5, 0x23 },
+		.regs[8] = { IMX415_INCKSEL6, 0x0 },
+		.regs[9] = { IMX415_INCKSEL7, 0x1 },
+		.regs[10] = { IMX415_TXCLKESC_FREQ, 0x06C0 },
+	},
+	{
+		.lane_rate = 1440000000,
+		.inck = 24000000,
+		.regs[0] = { IMX415_BCWAIT_TIME, 0x054 },
+		.regs[1] = { IMX415_CPWAIT_TIME, 0x03B },
+		.regs[2] = { IMX415_SYS_MODE, 0x8 },
+		.regs[3] = { IMX415_INCKSEL1, 0x00 },
+		.regs[4] = { IMX415_INCKSEL2, 0x23 },
+		.regs[5] = { IMX415_INCKSEL3, 0x0B4 },
+		.regs[6] = { IMX415_INCKSEL4, 0x0FC },
+		.regs[7] = { IMX415_INCKSEL5, 0x23 },
+		.regs[8] = { IMX415_INCKSEL6, 0x1 },
+		.regs[9] = { IMX415_INCKSEL7, 0x0 },
+		.regs[10] = { IMX415_TXCLKESC_FREQ, 0x0600 },
+	},
+	{
+		.lane_rate = 1485000000,
+		.inck = 27000000,
+		.regs[0] = { IMX415_BCWAIT_TIME, 0x05D },
+		.regs[1] = { IMX415_CPWAIT_TIME, 0x042 },
+		.regs[2] = { IMX415_SYS_MODE, 0x8 },
+		.regs[3] = { IMX415_INCKSEL1, 0x00 },
+		.regs[4] = { IMX415_INCKSEL2, 0x23 },
+		.regs[5] = { IMX415_INCKSEL3, 0x0A5 },
+		.regs[6] = { IMX415_INCKSEL4, 0x0E7 },
+		.regs[7] = { IMX415_INCKSEL5, 0x23 },
+		.regs[8] = { IMX415_INCKSEL6, 0x1 },
+		.regs[9] = { IMX415_INCKSEL7, 0x0 },
+		.regs[10] = { IMX415_TXCLKESC_FREQ, 0x06C0 },
+	},
+};
+
+/* all-pixel 2-lane 720 Mbps 15.74 Hz mode */
+static const struct imx415_reg imx415_mode_2_720[] = {
+	{ IMX415_VMAX, 0x08CA },
+	{ IMX415_HMAX, 0x07F0 },
+	{ IMX415_LANEMODE, IMX415_LANEMODE_2 },
+	{ IMX415_TCLKPOST, 0x006F },
+	{ IMX415_TCLKPREPARE, 0x002F },
+	{ IMX415_TCLKTRAIL, 0x002F },
+	{ IMX415_TCLKZERO, 0x00BF },
+	{ IMX415_THSPREPARE, 0x002F },
+	{ IMX415_THSZERO, 0x0057 },
+	{ IMX415_THSTRAIL, 0x002F },
+	{ IMX415_THSEXIT, 0x004F },
+	{ IMX415_TLPX, 0x0027 },
+};
+
+/* all-pixel 2-lane 1440 Mbps 30.01 Hz mode */
+static const struct imx415_reg imx415_mode_2_1440[] = {
+	{ IMX415_VMAX, 0x08CA },
+	{ IMX415_HMAX, 0x042A },
+	{ IMX415_LANEMODE, IMX415_LANEMODE_2 },
+	{ IMX415_TCLKPOST, 0x009F },
+	{ IMX415_TCLKPREPARE, 0x0057 },
+	{ IMX415_TCLKTRAIL, 0x0057 },
+	{ IMX415_TCLKZERO, 0x0187 },
+	{ IMX415_THSPREPARE, 0x005F },
+	{ IMX415_THSZERO, 0x00A7 },
+	{ IMX415_THSTRAIL, 0x005F },
+	{ IMX415_THSEXIT, 0x0097 },
+	{ IMX415_TLPX, 0x004F },
+};
+
+/* all-pixel 4-lane 891 Mbps 30 Hz mode */
+static const struct imx415_reg imx415_mode_4_891[] = {
+	{ IMX415_VMAX, 0x08CA },
+	{ IMX415_HMAX, 0x044C },
+	{ IMX415_LANEMODE, IMX415_LANEMODE_4 },
+	{ IMX415_TCLKPOST, 0x007F },
+	{ IMX415_TCLKPREPARE, 0x0037 },
+	{ IMX415_TCLKTRAIL, 0x0037 },
+	{ IMX415_TCLKZERO, 0x00F7 },
+	{ IMX415_THSPREPARE, 0x003F },
+	{ IMX415_THSZERO, 0x006F },
+	{ IMX415_THSTRAIL, 0x003F },
+	{ IMX415_THSEXIT, 0x005F },
+	{ IMX415_TLPX, 0x002F },
+};
+
+struct imx415_mode_reg_list {
+	u32 num_of_regs;
+	const struct imx415_reg *regs;
+};
+
+/*
+ * Mode : number of lanes, lane rate and frame rate dependent settings
+ *
+ * pixel_rate and hmax_pix are needed to calculate hblank for the v4l2 ctrl
+ * interface. These values can not be found in the data sheet and should be
+ * treated as virtual values. Use following table when adding new modes.
+ *
+ * lane_rate  lanes    fps     hmax_pix   pixel_rate
+ *
+ *     594      2     10.000     4400       99000000
+ *     891      2     15.000     4400      148500000
+ *     720      2     15.748     4064      144000000
+ *    1782      2     30.000     4400      297000000
+ *    2079      2     30.000     4400      297000000
+ *    1440      2     30.019     4510      304615385
+ *
+ *     594      4     20.000     5500      247500000
+ *     594      4     25.000     4400      247500000
+ *     720      4     25.000     4400      247500000
+ *     720      4     30.019     4510      304615385
+ *     891      4     30.000     4400      297000000
+ *    1440      4     30.019     4510      304615385
+ *    1440      4     60.038     4510      609230769
+ *    1485      4     60.000     4400      594000000
+ *    1782      4     60.000     4400      594000000
+ *    2079      4     60.000     4400      594000000
+ *    2376      4     90.164     4392      891000000
+ */
+struct imx415_mode {
+	u64 lane_rate;
+	u32 lanes;
+	u32 hmax_pix;
+	u64 pixel_rate;
+	struct imx415_mode_reg_list reg_list;
+};
+
+/* mode configs */
+static const struct imx415_mode supported_modes[] = {
+	{
+		.lane_rate = 720000000,
+		.lanes = 2,
+		.hmax_pix = 4064,
+		.pixel_rate = 144000000,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(imx415_mode_2_720),
+			.regs = imx415_mode_2_720,
+		},
+	},
+	{
+		.lane_rate = 1440000000,
+		.lanes = 2,
+		.hmax_pix = 4510,
+		.pixel_rate = 304615385,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(imx415_mode_2_1440),
+			.regs = imx415_mode_2_1440,
+		},
+	},
+	{
+		.lane_rate = 891000000,
+		.lanes = 4,
+		.hmax_pix = 4400,
+		.pixel_rate = 297000000,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(imx415_mode_4_891),
+			.regs = imx415_mode_4_891,
+		},
+	},
+};
+
+static const struct regmap_config imx415_regmap_config = {
+	.reg_bits = 16,
+	.val_bits = 8,
+};
+
+static const char *const imx415_test_pattern_menu[] = {
+	"disabled",
+	"solid black",
+	"solid white",
+	"solid dark gray",
+	"solid light gray",
+	"stripes light/dark grey",
+	"stripes dark/light grey",
+	"stripes black/dark grey",
+	"stripes dark grey/black",
+	"stripes black/white",
+	"stripes white/black",
+	"horizontal color bar",
+	"vertical color bar",
+};
+
+struct imx415 {
+	struct device *dev;
+	struct clk *clk;
+	struct regulator_bulk_data supplies[ARRAY_SIZE(imx415_supply_names)];
+	struct gpio_desc *reset;
+	struct regmap *regmap;
+
+	const struct imx415_clk_params *clk_params;
+
+	bool streaming;
+
+	struct v4l2_subdev subdev;
+	struct media_pad pad;
+
+	struct v4l2_ctrl_handler ctrls;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *hflip;
+	struct v4l2_ctrl *vflip;
+
+	unsigned int cur_mode;
+	unsigned int num_data_lanes;
+};
+
+/*
+ * This table includes fixed register settings and a bunch of undocumented
+ * registers that have to be set to another value than default.
+ */
+static const struct imx415_reg imx415_init_table[] = {
+	/* use all-pixel readout mode, no flip */
+	{ IMX415_WINMODE, 0x00 },
+	{ IMX415_ADDMODE, 0x00 },
+	{ IMX415_REVERSE, 0x00 },
+	/* use RAW 10-bit mode */
+	{ IMX415_ADBIT, 0x00 },
+	{ IMX415_MDBIT, 0x00 },
+	/* output VSYNC on XVS and low on XHS */
+	{ IMX415_OUTSEL, 0x22 },
+	{ IMX415_DRV, 0x00 },
+
+	/* SONY magic registers */
+	{ IMX415_REG_8BIT(0x32D4), 0x21 },
+	{ IMX415_REG_8BIT(0x32EC), 0xA1 },
+	{ IMX415_REG_8BIT(0x3452), 0x7F },
+	{ IMX415_REG_8BIT(0x3453), 0x03 },
+	{ IMX415_REG_8BIT(0x358A), 0x04 },
+	{ IMX415_REG_8BIT(0x35A1), 0x02 },
+	{ IMX415_REG_8BIT(0x36BC), 0x0C },
+	{ IMX415_REG_8BIT(0x36CC), 0x53 },
+	{ IMX415_REG_8BIT(0x36CD), 0x00 },
+	{ IMX415_REG_8BIT(0x36CE), 0x3C },
+	{ IMX415_REG_8BIT(0x36D0), 0x8C },
+	{ IMX415_REG_8BIT(0x36D1), 0x00 },
+	{ IMX415_REG_8BIT(0x36D2), 0x71 },
+	{ IMX415_REG_8BIT(0x36D4), 0x3C },
+	{ IMX415_REG_8BIT(0x36D6), 0x53 },
+	{ IMX415_REG_8BIT(0x36D7), 0x00 },
+	{ IMX415_REG_8BIT(0x36D8), 0x71 },
+	{ IMX415_REG_8BIT(0x36DA), 0x8C },
+	{ IMX415_REG_8BIT(0x36DB), 0x00 },
+	{ IMX415_REG_8BIT(0x3724), 0x02 },
+	{ IMX415_REG_8BIT(0x3726), 0x02 },
+	{ IMX415_REG_8BIT(0x3732), 0x02 },
+	{ IMX415_REG_8BIT(0x3734), 0x03 },
+	{ IMX415_REG_8BIT(0x3736), 0x03 },
+	{ IMX415_REG_8BIT(0x3742), 0x03 },
+	{ IMX415_REG_8BIT(0x3862), 0xE0 },
+	{ IMX415_REG_8BIT(0x38CC), 0x30 },
+	{ IMX415_REG_8BIT(0x38CD), 0x2F },
+	{ IMX415_REG_8BIT(0x395C), 0x0C },
+	{ IMX415_REG_8BIT(0x3A42), 0xD1 },
+	{ IMX415_REG_8BIT(0x3A4C), 0x77 },
+	{ IMX415_REG_8BIT(0x3AE0), 0x02 },
+	{ IMX415_REG_8BIT(0x3AEC), 0x0C },
+	{ IMX415_REG_8BIT(0x3B00), 0x2E },
+	{ IMX415_REG_8BIT(0x3B06), 0x29 },
+	{ IMX415_REG_8BIT(0x3B98), 0x25 },
+	{ IMX415_REG_8BIT(0x3B99), 0x21 },
+	{ IMX415_REG_8BIT(0x3B9B), 0x13 },
+	{ IMX415_REG_8BIT(0x3B9C), 0x13 },
+	{ IMX415_REG_8BIT(0x3B9D), 0x13 },
+	{ IMX415_REG_8BIT(0x3B9E), 0x13 },
+	{ IMX415_REG_8BIT(0x3BA1), 0x00 },
+	{ IMX415_REG_8BIT(0x3BA2), 0x06 },
+	{ IMX415_REG_8BIT(0x3BA3), 0x0B },
+	{ IMX415_REG_8BIT(0x3BA4), 0x10 },
+	{ IMX415_REG_8BIT(0x3BA5), 0x14 },
+	{ IMX415_REG_8BIT(0x3BA6), 0x18 },
+	{ IMX415_REG_8BIT(0x3BA7), 0x1A },
+	{ IMX415_REG_8BIT(0x3BA8), 0x1A },
+	{ IMX415_REG_8BIT(0x3BA9), 0x1A },
+	{ IMX415_REG_8BIT(0x3BAC), 0xED },
+	{ IMX415_REG_8BIT(0x3BAD), 0x01 },
+	{ IMX415_REG_8BIT(0x3BAE), 0xF6 },
+	{ IMX415_REG_8BIT(0x3BAF), 0x02 },
+	{ IMX415_REG_8BIT(0x3BB0), 0xA2 },
+	{ IMX415_REG_8BIT(0x3BB1), 0x03 },
+	{ IMX415_REG_8BIT(0x3BB2), 0xE0 },
+	{ IMX415_REG_8BIT(0x3BB3), 0x03 },
+	{ IMX415_REG_8BIT(0x3BB4), 0xE0 },
+	{ IMX415_REG_8BIT(0x3BB5), 0x03 },
+	{ IMX415_REG_8BIT(0x3BB6), 0xE0 },
+	{ IMX415_REG_8BIT(0x3BB7), 0x03 },
+	{ IMX415_REG_8BIT(0x3BB8), 0xE0 },
+	{ IMX415_REG_8BIT(0x3BBA), 0xE0 },
+	{ IMX415_REG_8BIT(0x3BBC), 0xDA },
+	{ IMX415_REG_8BIT(0x3BBE), 0x88 },
+	{ IMX415_REG_8BIT(0x3BC0), 0x44 },
+	{ IMX415_REG_8BIT(0x3BC2), 0x7B },
+	{ IMX415_REG_8BIT(0x3BC4), 0xA2 },
+	{ IMX415_REG_8BIT(0x3BC8), 0xBD },
+	{ IMX415_REG_8BIT(0x3BCA), 0xBD },
+};
+
+static inline struct imx415 *to_imx415(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct imx415, subdev);
+}
+
+static int imx415_read(struct imx415 *sensor, u32 addr)
+{
+	u8 data[3] = { 0 };
+	int ret;
+
+	ret = regmap_raw_read(sensor->regmap, addr & IMX415_REG_ADDR_MASK, data,
+			      (addr >> IMX415_REG_SIZE_SHIFT) & 3);
+	if (ret < 0)
+		return ret;
+
+	return (data[2] << 16) | (data[1] << 8) | data[0];
+}
+
+static int imx415_write(struct imx415 *sensor, u32 addr, u32 value)
+{
+	u8 data[3] = { value & 0xff, (value >> 8) & 0xff, value >> 16 };
+	int ret;
+
+	ret = regmap_raw_write(sensor->regmap, addr & IMX415_REG_ADDR_MASK,
+			       data, (addr >> IMX415_REG_SIZE_SHIFT) & 3);
+	if (ret < 0)
+		dev_err_ratelimited(sensor->dev,
+				    "%u-bit write to 0x%04x failed: %d\n",
+				    ((addr >> IMX415_REG_SIZE_SHIFT) & 3) * 8,
+				    addr & IMX415_REG_ADDR_MASK, ret);
+
+	return 0;
+}
+
+static int imx415_set_testpattern(struct imx415 *sensor, int val)
+{
+	int ret;
+
+	if (val) {
+		ret = imx415_write(sensor, IMX415_BLKLEVEL, 0x00);
+		if (ret)
+			return ret;
+		ret = imx415_write(sensor, IMX415_TPG_EN_DUOUT, 0x01);
+		if (ret)
+			return ret;
+		ret = imx415_write(sensor, IMX415_TPG_PATSEL_DUOUT, val - 1);
+		if (ret)
+			return ret;
+		ret = imx415_write(sensor, IMX415_TPG_COLORWIDTH, 0x01);
+		if (ret)
+			return ret;
+		ret = imx415_write(sensor, IMX415_TESTCLKEN_MIPI, 0x20);
+		if (ret)
+			return ret;
+		ret = imx415_write(sensor, IMX415_DIG_CLP_MODE, 0x00);
+		if (ret)
+			return ret;
+		ret = imx415_write(sensor, IMX415_WRJ_OPEN, 0x00);
+	} else {
+		ret = imx415_write(sensor, IMX415_BLKLEVEL,
+				   IMX415_BLKLEVEL_DEFAULT);
+		if (ret)
+			return ret;
+		ret = imx415_write(sensor, IMX415_TPG_EN_DUOUT, 0x00);
+		if (ret)
+			return ret;
+		ret = imx415_write(sensor, IMX415_TESTCLKEN_MIPI, 0x00);
+		if (ret)
+			return ret;
+		ret = imx415_write(sensor, IMX415_DIG_CLP_MODE, 0x01);
+		if (ret)
+			return ret;
+		ret = imx415_write(sensor, IMX415_WRJ_OPEN, 0x01);
+	}
+	return 0;
+}
+
+static int imx415_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct imx415 *sensor = container_of(ctrl->handler, struct imx415,
+					     ctrls);
+	const struct v4l2_mbus_framefmt *format;
+	struct v4l2_subdev_state *state;
+	unsigned int vmax;
+	unsigned int flip;
+
+	if (!sensor->streaming)
+		return 0;
+
+	state = v4l2_subdev_get_locked_active_state(&sensor->subdev);
+	format = v4l2_subdev_get_pad_format(&sensor->subdev, state, 0);
+
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE:
+		/* clamp the exposure value to VMAX. */
+		vmax = format->height + sensor->vblank->cur.val;
+		ctrl->val = min_t(int, ctrl->val, vmax);
+		return imx415_write(sensor, IMX415_SHR0, vmax - ctrl->val);
+
+	case V4L2_CID_ANALOGUE_GAIN:
+		/* analogue gain in 0.3 dB step size */
+		return imx415_write(sensor, IMX415_GAIN_PCG_0, ctrl->val);
+
+	case V4L2_CID_HFLIP:
+	case V4L2_CID_VFLIP:
+		flip = (sensor->hflip->val << IMX415_HREVERSE_SHIFT) |
+		       (sensor->vflip->val << IMX415_VREVERSE_SHIFT);
+		return imx415_write(sensor, IMX415_REVERSE, flip);
+
+	case V4L2_CID_TEST_PATTERN:
+		return imx415_set_testpattern(sensor, ctrl->val);
+
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct v4l2_ctrl_ops imx415_ctrl_ops = {
+	.s_ctrl = imx415_s_ctrl,
+};
+
+static int imx415_ctrls_init(struct imx415 *sensor)
+{
+	struct v4l2_fwnode_device_properties props;
+	struct v4l2_ctrl *ctrl;
+	u64 pixel_rate = supported_modes[sensor->cur_mode].pixel_rate;
+	u64 lane_rate = supported_modes[sensor->cur_mode].lane_rate;
+	u32 exposure_max = IMX415_PIXEL_ARRAY_HEIGHT +
+			   IMX415_PIXEL_ARRAY_VBLANK - 8;
+	u32 hblank;
+	unsigned int i;
+	int ret;
+
+	ret = v4l2_fwnode_device_parse(sensor->dev, &props);
+	if (ret < 0)
+		return ret;
+
+	v4l2_ctrl_handler_init(&sensor->ctrls, 10);
+
+	for (i = 0; i < ARRAY_SIZE(link_freq_menu_items); ++i) {
+		if (lane_rate == link_freq_menu_items[i] * 2)
+			break;
+	}
+	if (i == ARRAY_SIZE(link_freq_menu_items)) {
+		return dev_err_probe(sensor->dev, -EINVAL,
+				     "lane rate %llu not supported\n",
+				     lane_rate);
+	}
+
+	ctrl = v4l2_ctrl_new_int_menu(&sensor->ctrls, &imx415_ctrl_ops,
+				      V4L2_CID_LINK_FREQ,
+				      ARRAY_SIZE(link_freq_menu_items) - 1, i,
+				      link_freq_menu_items);
+
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	v4l2_ctrl_new_std(&sensor->ctrls, &imx415_ctrl_ops, V4L2_CID_EXPOSURE,
+			  4, exposure_max, 1, exposure_max);
+
+	v4l2_ctrl_new_std(&sensor->ctrls, &imx415_ctrl_ops,
+			  V4L2_CID_ANALOGUE_GAIN, IMX415_AGAIN_MIN,
+			  IMX415_AGAIN_MAX, IMX415_AGAIN_STEP,
+			  IMX415_AGAIN_MIN);
+
+	hblank = supported_modes[sensor->cur_mode].hmax_pix -
+		 IMX415_PIXEL_ARRAY_WIDTH;
+	ctrl = v4l2_ctrl_new_std(&sensor->ctrls, &imx415_ctrl_ops,
+				 V4L2_CID_HBLANK, hblank, hblank, 1, hblank);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	sensor->vblank = v4l2_ctrl_new_std(&sensor->ctrls, &imx415_ctrl_ops,
+					   V4L2_CID_VBLANK,
+					   IMX415_PIXEL_ARRAY_VBLANK,
+					   IMX415_PIXEL_ARRAY_VBLANK, 1,
+					   IMX415_PIXEL_ARRAY_VBLANK);
+	if (sensor->vblank)
+		sensor->vblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	/*
+	 * The pixel rate used here is a virtual value and can be used for
+	 * calculating the frame rate together with hblank. It may not
+	 * necessarily be the physically correct pixel clock.
+	 */
+	v4l2_ctrl_new_std(&sensor->ctrls, NULL, V4L2_CID_PIXEL_RATE, pixel_rate,
+			  pixel_rate, 1, pixel_rate);
+
+	sensor->hflip = v4l2_ctrl_new_std(&sensor->ctrls, &imx415_ctrl_ops,
+					  V4L2_CID_HFLIP, 0, 1, 1, 0);
+	sensor->vflip = v4l2_ctrl_new_std(&sensor->ctrls, &imx415_ctrl_ops,
+					  V4L2_CID_VFLIP, 0, 1, 1, 0);
+
+	v4l2_ctrl_new_std_menu_items(&sensor->ctrls, &imx415_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(imx415_test_pattern_menu) - 1,
+				     0, 0, imx415_test_pattern_menu);
+
+	v4l2_ctrl_new_fwnode_properties(&sensor->ctrls, &imx415_ctrl_ops,
+					&props);
+
+	if (sensor->ctrls.error) {
+		dev_err_probe(sensor->dev, sensor->ctrls.error,
+			      "failed to add controls\n");
+		v4l2_ctrl_handler_free(&sensor->ctrls);
+		return sensor->ctrls.error;
+	}
+	sensor->subdev.ctrl_handler = &sensor->ctrls;
+
+	return 0;
+}
+
+static int imx415_set_mode(struct imx415 *sensor, int mode)
+{
+	const struct imx415_reg *reg;
+	unsigned int i;
+	int ret = 0;
+
+	if (mode >= ARRAY_SIZE(supported_modes)) {
+		dev_err(sensor->dev, "Mode %d not supported\n", mode);
+		return -EINVAL;
+	}
+
+	for (i = 0; i < supported_modes[mode].reg_list.num_of_regs; ++i) {
+		reg = &supported_modes[mode].reg_list.regs[i];
+		ret = imx415_write(sensor, reg->address, reg->val);
+		if (ret)
+			return ret;
+	}
+
+	for (i = 0; i < IMX415_NUM_CLK_PARAM_REGS; ++i) {
+		reg = &sensor->clk_params->regs[i];
+		ret = imx415_write(sensor, reg->address, reg->val);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int imx415_setup(struct imx415 *sensor, struct v4l2_subdev_state *state)
+{
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < ARRAY_SIZE(imx415_init_table); ++i) {
+		ret = imx415_write(sensor, imx415_init_table[i].address,
+				   imx415_init_table[i].val);
+		if (ret)
+			return ret;
+	}
+
+	return imx415_set_mode(sensor, sensor->cur_mode);
+}
+
+static int imx415_wakeup(struct imx415 *sensor)
+{
+	int ret;
+
+	ret = imx415_write(sensor, IMX415_MODE, IMX415_MODE_OPERATING);
+	if (ret)
+		return ret;
+
+	/*
+	 * According to the datasheet we have to wait at least 63 us after
+	 * leaving standby mode. But this doesn't work even after 30 ms.
+	 * So probably this should be 63 ms and therefore we wait for 80 ms.
+	 */
+	msleep(80);
+
+	return 0;
+}
+
+static int imx415_stream_on(struct imx415 *sensor)
+{
+	int ret;
+
+	ret = imx415_wakeup(sensor);
+	if (ret)
+		return ret;
+
+	return imx415_write(sensor, IMX415_XMSTA, IMX415_XMSTA_START);
+}
+
+static int imx415_stream_off(struct imx415 *sensor)
+{
+	int ret;
+
+	ret = imx415_write(sensor, IMX415_XMSTA, IMX415_XMSTA_STOP);
+	if (ret)
+		return ret;
+
+	return imx415_write(sensor, IMX415_MODE, IMX415_MODE_STANDBY);
+}
+
+static int imx415_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct imx415 *sensor = to_imx415(sd);
+	struct v4l2_subdev_state *state;
+	int ret;
+
+	state = v4l2_subdev_lock_and_get_active_state(sd);
+
+	if (!enable) {
+		ret = imx415_stream_off(sensor);
+
+		pm_runtime_mark_last_busy(sensor->dev);
+		pm_runtime_put_autosuspend(sensor->dev);
+
+		sensor->streaming = false;
+
+		goto unlock;
+	}
+
+	ret = pm_runtime_resume_and_get(sensor->dev);
+	if (ret < 0)
+		goto unlock;
+
+	ret = imx415_setup(sensor, state);
+	if (ret)
+		goto err_pm;
+
+	/*
+	 * Set streaming to true to ensure __v4l2_ctrl_handler_setup() will set
+	 * the controls. The flag is reset to false further down if an error
+	 * occurs.
+	 */
+	sensor->streaming = true;
+
+	ret = __v4l2_ctrl_handler_setup(&sensor->ctrls);
+	if (ret < 0)
+		goto err_pm;
+
+	ret = imx415_stream_on(sensor);
+	if (ret)
+		goto err_pm;
+
+	ret = 0;
+
+unlock:
+	v4l2_subdev_unlock_state(state);
+
+	return ret;
+
+err_pm:
+	/*
+	 * In case of error, turn the power off synchronously as the device
+	 * likely has no other chance to recover.
+	 */
+	pm_runtime_put_sync(sensor->dev);
+	sensor->streaming = false;
+
+	goto unlock;
+}
+
+static int imx415_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_SGBRG10_1X10;
+
+	return 0;
+}
+
+static int imx415_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	const struct v4l2_mbus_framefmt *format;
+
+	format = v4l2_subdev_get_pad_format(sd, state, fse->pad);
+
+	if (fse->index > 0 || fse->code != format->code)
+		return -EINVAL;
+
+	fse->min_width = IMX415_PIXEL_ARRAY_WIDTH;
+	fse->max_width = fse->min_width;
+	fse->min_height = IMX415_PIXEL_ARRAY_HEIGHT;
+	fse->max_height = fse->min_height;
+	return 0;
+}
+
+static int imx415_get_format(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *state,
+			     struct v4l2_subdev_format *fmt)
+{
+	fmt->format = *v4l2_subdev_get_pad_format(sd, state, fmt->pad);
+
+	return 0;
+}
+
+static int imx415_set_format(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *state,
+			     struct v4l2_subdev_format *fmt)
+{
+	struct v4l2_mbus_framefmt *format;
+
+	format = v4l2_subdev_get_pad_format(sd, state, fmt->pad);
+
+	format->width = fmt->format.width;
+	format->height = fmt->format.height;
+	format->code = MEDIA_BUS_FMT_SGBRG10_1X10;
+	format->field = V4L2_FIELD_NONE;
+	format->colorspace = V4L2_COLORSPACE_RAW;
+	format->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+	format->quantization = V4L2_QUANTIZATION_DEFAULT;
+	format->xfer_func = V4L2_XFER_FUNC_NONE;
+
+	fmt->format = *format;
+	return 0;
+}
+
+static int imx415_get_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *sd_state,
+				struct v4l2_subdev_selection *sel)
+{
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.top = IMX415_PIXEL_ARRAY_TOP;
+		sel->r.left = IMX415_PIXEL_ARRAY_LEFT;
+		sel->r.width = IMX415_PIXEL_ARRAY_WIDTH;
+		sel->r.height = IMX415_PIXEL_ARRAY_HEIGHT;
+
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int imx415_init_cfg(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_state *state)
+{
+	struct v4l2_subdev_format format = {
+		.format = {
+			.width = IMX415_PIXEL_ARRAY_WIDTH,
+			.height = IMX415_PIXEL_ARRAY_HEIGHT,
+		},
+	};
+
+	imx415_set_format(sd, state, &format);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops imx415_subdev_video_ops = {
+	.s_stream = imx415_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops imx415_subdev_pad_ops = {
+	.enum_mbus_code = imx415_enum_mbus_code,
+	.enum_frame_size = imx415_enum_frame_size,
+	.get_fmt = imx415_get_format,
+	.set_fmt = imx415_set_format,
+	.get_selection = imx415_get_selection,
+	.init_cfg = imx415_init_cfg,
+};
+
+static const struct v4l2_subdev_ops imx415_subdev_ops = {
+	.video = &imx415_subdev_video_ops,
+	.pad = &imx415_subdev_pad_ops,
+};
+
+static int imx415_subdev_init(struct imx415 *sensor)
+{
+	struct i2c_client *client = to_i2c_client(sensor->dev);
+	int ret;
+
+	v4l2_i2c_subdev_init(&sensor->subdev, client, &imx415_subdev_ops);
+
+	ret = imx415_ctrls_init(sensor);
+	if (ret)
+		return ret;
+
+	sensor->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+				V4L2_SUBDEV_FL_HAS_EVENTS;
+	sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
+	sensor->subdev.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret = media_entity_pads_init(&sensor->subdev.entity, 1, &sensor->pad);
+	if (ret < 0) {
+		v4l2_ctrl_handler_free(&sensor->ctrls);
+		return ret;
+	}
+
+	sensor->subdev.state_lock = sensor->subdev.ctrl_handler->lock;
+	v4l2_subdev_init_finalize(&sensor->subdev);
+
+	return 0;
+}
+
+static void imx415_subdev_cleanup(struct imx415 *sensor)
+{
+	media_entity_cleanup(&sensor->subdev.entity);
+	v4l2_ctrl_handler_free(&sensor->ctrls);
+}
+
+static int imx415_power_on(struct imx415 *sensor)
+{
+	int ret;
+
+	ret = regulator_bulk_enable(ARRAY_SIZE(sensor->supplies),
+				    sensor->supplies);
+	if (ret < 0)
+		return ret;
+
+	gpiod_set_value_cansleep(sensor->reset, 0);
+
+	udelay(1);
+
+	ret = clk_prepare_enable(sensor->clk);
+	if (ret < 0)
+		goto err_reset;
+
+	/*
+	 * Data sheet states that 20 us are required before communication start,
+	 * but this doesn't work in all cases. Use 100 us to be on the safe
+	 * side.
+	 */
+	usleep_range(100, 200);
+
+	return 0;
+
+err_reset:
+	gpiod_set_value_cansleep(sensor->reset, 1);
+	regulator_bulk_disable(ARRAY_SIZE(sensor->supplies), sensor->supplies);
+	return ret;
+}
+
+static void imx415_power_off(struct imx415 *sensor)
+{
+	clk_disable_unprepare(sensor->clk);
+	gpiod_set_value_cansleep(sensor->reset, 1);
+	regulator_bulk_disable(ARRAY_SIZE(sensor->supplies), sensor->supplies);
+}
+
+static int imx415_identify_model(struct imx415 *sensor)
+{
+	int model, ret;
+
+	/*
+	 * While most registers can be read when the sensor is in standby, this
+	 * is not the case of the sensor info register :-(
+	 */
+	ret = imx415_wakeup(sensor);
+	if (ret)
+		return dev_err_probe(sensor->dev, ret,
+				     "failed to get sensor out of standby\n");
+
+	ret = imx415_read(sensor, IMX415_SENSOR_INFO);
+	if (ret < 0) {
+		dev_err_probe(sensor->dev, ret,
+			      "failed to read sensor information\n");
+		goto done;
+	}
+
+	model = ret & IMX415_SENSOR_INFO_MASK;
+
+	switch (model) {
+	case IMX415_CHIP_ID:
+		dev_info(sensor->dev, "Detected IMX415 image sensor\n");
+		break;
+	default:
+		ret = dev_err_probe(sensor->dev, -ENODEV,
+				    "invalid device model 0x%04x\n", model);
+		goto done;
+	}
+
+	ret = 0;
+
+done:
+	imx415_write(sensor, IMX415_MODE, IMX415_MODE_STANDBY);
+	return ret;
+}
+
+static int imx415_check_inck(unsigned long inck, u64 link_frequency)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(imx415_clk_params); ++i) {
+		if ((imx415_clk_params[i].lane_rate == link_frequency * 2) &&
+		    imx415_clk_params[i].inck == inck)
+			break;
+	}
+
+	if (i == ARRAY_SIZE(imx415_clk_params))
+		return -EINVAL;
+	else
+		return 0;
+}
+
+static int imx415_parse_hw_config(struct imx415 *sensor)
+{
+	struct v4l2_fwnode_endpoint bus_cfg = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY,
+	};
+	struct fwnode_handle *ep;
+	u64 lane_rate;
+	unsigned long inck;
+	unsigned int i, j;
+	int ret;
+
+	for (i = 0; i < ARRAY_SIZE(sensor->supplies); ++i)
+		sensor->supplies[i].supply = imx415_supply_names[i];
+
+	ret = devm_regulator_bulk_get(sensor->dev, ARRAY_SIZE(sensor->supplies),
+				      sensor->supplies);
+	if (ret)
+		return dev_err_probe(sensor->dev, ret,
+				     "failed to get supplies\n");
+
+	sensor->reset = devm_gpiod_get_optional(sensor->dev, "reset",
+						GPIOD_OUT_HIGH);
+	if (IS_ERR(sensor->reset))
+		return dev_err_probe(sensor->dev, PTR_ERR(sensor->reset),
+				     "failed to get reset GPIO\n");
+
+	sensor->clk = devm_clk_get(sensor->dev, "inck");
+	if (IS_ERR(sensor->clk))
+		return dev_err_probe(sensor->dev, PTR_ERR(sensor->clk),
+				     "failed to get clock\n");
+
+	ep = fwnode_graph_get_next_endpoint(dev_fwnode(sensor->dev), NULL);
+	if (!ep)
+		return -ENXIO;
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(ep, &bus_cfg);
+	fwnode_handle_put(ep);
+	if (ret)
+		return ret;
+
+	switch (bus_cfg.bus.mipi_csi2.num_data_lanes) {
+	case 2:
+	case 4:
+		sensor->num_data_lanes = bus_cfg.bus.mipi_csi2.num_data_lanes;
+		break;
+	default:
+		ret = dev_err_probe(sensor->dev, -EINVAL,
+				    "invalid number of CSI2 data lanes %d\n",
+				    bus_cfg.bus.mipi_csi2.num_data_lanes);
+		goto done_endpoint_free;
+	}
+
+	if (!bus_cfg.nr_of_link_frequencies) {
+		ret = dev_err_probe(sensor->dev, -EINVAL,
+				    "no link frequencies defined");
+		goto done_endpoint_free;
+	}
+
+	/*
+	 * Check if there exists a sensor mode defined for current INCK,
+	 * number of lanes and given lane rates.
+	 */
+	inck = clk_get_rate(sensor->clk);
+	for (i = 0; i < bus_cfg.nr_of_link_frequencies; ++i) {
+		if (imx415_check_inck(inck, bus_cfg.link_frequencies[i])) {
+			dev_dbg(sensor->dev,
+				"INCK %lu Hz not supported for this link freq",
+				inck);
+			continue;
+		}
+
+		for (j = 0; j < ARRAY_SIZE(supported_modes); ++j) {
+			if (sensor->num_data_lanes != supported_modes[j].lanes)
+				continue;
+			if (bus_cfg.link_frequencies[i] * 2 !=
+			    supported_modes[j].lane_rate)
+				continue;
+			sensor->cur_mode = j;
+			break;
+		}
+		if (j < ARRAY_SIZE(supported_modes))
+			break;
+	}
+	if (i == bus_cfg.nr_of_link_frequencies) {
+		ret = dev_err_probe(sensor->dev, -EINVAL,
+				    "no valid sensor mode defined\n");
+		goto done_endpoint_free;
+	}
+
+	lane_rate = supported_modes[sensor->cur_mode].lane_rate;
+	for (i = 0; i < ARRAY_SIZE(imx415_clk_params); ++i) {
+		if (lane_rate == imx415_clk_params[i].lane_rate &&
+		    inck == imx415_clk_params[i].inck) {
+			sensor->clk_params = &imx415_clk_params[i];
+			break;
+		}
+	}
+	if (i == ARRAY_SIZE(imx415_clk_params)) {
+		ret = dev_err_probe(sensor->dev, -EINVAL,
+				    "Mode %d not supported\n",
+				    sensor->cur_mode);
+		goto done_endpoint_free;
+	}
+
+	ret = 0;
+	dev_dbg(sensor->dev, "clock: %lu Hz, lane_rate: %llu bps, lanes: %d\n",
+		inck, lane_rate, sensor->num_data_lanes);
+
+done_endpoint_free:
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+
+	return ret;
+}
+
+static int imx415_probe(struct i2c_client *client)
+{
+	struct imx415 *sensor;
+	int ret;
+
+	sensor = devm_kzalloc(&client->dev, sizeof(*sensor), GFP_KERNEL);
+	if (!sensor)
+		return -ENOMEM;
+
+	sensor->dev = &client->dev;
+
+	ret = imx415_parse_hw_config(sensor);
+	if (ret)
+		return ret;
+
+	sensor->regmap = devm_regmap_init_i2c(client, &imx415_regmap_config);
+	if (IS_ERR(sensor->regmap))
+		return PTR_ERR(sensor->regmap);
+
+	/*
+	 * Enable power management. The driver supports runtime PM, but needs to
+	 * work when runtime PM is disabled in the kernel. To that end, power
+	 * the sensor on manually here, identify it, and fully initialize it.
+	 */
+	ret = imx415_power_on(sensor);
+	if (ret)
+		return ret;
+
+	ret = imx415_identify_model(sensor);
+	if (ret)
+		goto err_power;
+
+	ret = imx415_subdev_init(sensor);
+	if (ret)
+		goto err_power;
+
+	/*
+	 * Enable runtime PM. As the device has been powered manually, mark it
+	 * as active, and increase the usage count without resuming the device.
+	 */
+	pm_runtime_set_active(sensor->dev);
+	pm_runtime_get_noresume(sensor->dev);
+	pm_runtime_enable(sensor->dev);
+
+	ret = v4l2_async_register_subdev_sensor(&sensor->subdev);
+	if (ret < 0)
+		goto err_pm;
+
+	/*
+	 * Finally, enable autosuspend and decrease the usage count. The device
+	 * will get suspended after the autosuspend delay, turning the power
+	 * off.
+	 */
+	pm_runtime_set_autosuspend_delay(sensor->dev, 1000);
+	pm_runtime_use_autosuspend(sensor->dev);
+	pm_runtime_put_autosuspend(sensor->dev);
+
+	return 0;
+
+err_pm:
+	pm_runtime_disable(sensor->dev);
+	pm_runtime_put_noidle(sensor->dev);
+	imx415_subdev_cleanup(sensor);
+err_power:
+	imx415_power_off(sensor);
+	return ret;
+}
+
+static void imx415_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct imx415 *sensor = to_imx415(subdev);
+
+	v4l2_async_unregister_subdev(subdev);
+
+	imx415_subdev_cleanup(sensor);
+
+	/*
+	 * Disable runtime PM. In case runtime PM is disabled in the kernel,
+	 * make sure to turn power off manually.
+	 */
+	pm_runtime_disable(sensor->dev);
+	if (!pm_runtime_status_suspended(sensor->dev))
+		imx415_power_off(sensor);
+	pm_runtime_set_suspended(sensor->dev);
+}
+
+static int imx415_runtime_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct imx415 *sensor = to_imx415(subdev);
+
+	return imx415_power_on(sensor);
+}
+
+static int imx415_runtime_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct imx415 *sensor = to_imx415(subdev);
+
+	imx415_power_off(sensor);
+
+	return 0;
+}
+
+static DEFINE_RUNTIME_DEV_PM_OPS(imx415_pm_ops, imx415_runtime_suspend,
+				 imx415_runtime_resume, NULL);
+
+static const struct of_device_id imx415_of_match[] = {
+	{ .compatible = "sony,imx415" },
+	{ /* sentinel */ }
+};
+
+MODULE_DEVICE_TABLE(of, imx415_of_match);
+
+static struct i2c_driver imx415_driver = {
+	.probe = imx415_probe,
+	.remove = imx415_remove,
+	.driver = {
+		.name = "imx415",
+		.of_match_table = imx415_of_match,
+		.pm = pm_ptr(&imx415_pm_ops),
+	},
+};
+
+module_i2c_driver(imx415_driver);
+
+MODULE_DESCRIPTION("Sony IMX415 image sensor driver");
+MODULE_AUTHOR("Gerald Loacker <gerald.loacker@wolfvision.net>");
+MODULE_AUTHOR("Michael Riesch <michael.riesch@wolfvision.net>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/ir-kbd-i2c.c b/drivers/media/i2c/ir-kbd-i2c.c
new file mode 100644
index 000000000..b37a2aaf8
--- /dev/null
+++ b/drivers/media/i2c/ir-kbd-i2c.c
@@ -0,0 +1,1002 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ *
+ * keyboard input driver for i2c IR remote controls
+ *
+ * Copyright (c) 2000-2003 Gerd Knorr <kraxel@bytesex.org>
+ * modified for PixelView (BT878P+W/FM) by
+ *      Michal Kochanowicz <mkochano@pld.org.pl>
+ *      Christoph Bartelmus <lirc@bartelmus.de>
+ * modified for KNC ONE TV Station/Anubis Typhoon TView Tuner by
+ *      Ulrich Mueller <ulrich.mueller42@web.de>
+ * modified for em2820 based USB TV tuners by
+ *      Markus Rechberger <mrechberger@gmail.com>
+ * modified for DViCO Fusion HDTV 5 RT GOLD by
+ *      Chaogui Zhang <czhang1974@gmail.com>
+ * modified for MSI TV@nywhere Plus by
+ *      Henry Wong <henry@stuffedcow.net>
+ *      Mark Schultz <n9xmj@yahoo.com>
+ *      Brian Rogers <brian_rogers@comcast.net>
+ * modified for AVerMedia Cardbus by
+ *      Oldrich Jedlicka <oldium.pro@seznam.cz>
+ * Zilog Transmitter portions/ideas were derived from GPLv2+ sources:
+ *  - drivers/char/pctv_zilogir.[ch] from Hauppauge Broadway product
+ *	Copyright 2011 Hauppauge Computer works
+ *  - drivers/staging/media/lirc/lirc_zilog.c
+ *	Copyright (c) 2000 Gerd Knorr <kraxel@goldbach.in-berlin.de>
+ *	Michal Kochanowicz <mkochano@pld.org.pl>
+ *	Christoph Bartelmus <lirc@bartelmus.de>
+ *	Ulrich Mueller <ulrich.mueller42@web.de>
+ *	Stefan Jahn <stefan@lkcc.org>
+ *	Jerome Brock <jbrock@users.sourceforge.net>
+ *	Thomas Reitmayr (treitmayr@yahoo.com)
+ *	Mark Weaver <mark@npsl.co.uk>
+ *	Jarod Wilson <jarod@redhat.com>
+ *	Copyright (C) 2011 Andy Walls <awalls@md.metrocast.net>
+ */
+
+#include <asm/unaligned.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/string.h>
+#include <linux/timer.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/workqueue.h>
+
+#include <media/rc-core.h>
+#include <media/i2c/ir-kbd-i2c.h>
+
+#define FLAG_TX		1
+#define FLAG_HDPVR	2
+
+static bool enable_hdpvr;
+module_param(enable_hdpvr, bool, 0644);
+
+static int get_key_haup_common(struct IR_i2c *ir, enum rc_proto *protocol,
+			       u32 *scancode, u8 *ptoggle, int size)
+{
+	unsigned char buf[6];
+	int start, range, toggle, dev, code, ircode, vendor;
+
+	/* poll IR chip */
+	if (size != i2c_master_recv(ir->c, buf, size))
+		return -EIO;
+
+	if (buf[0] & 0x80) {
+		int offset = (size == 6) ? 3 : 0;
+
+		/* split rc5 data block ... */
+		start  = (buf[offset] >> 7) &    1;
+		range  = (buf[offset] >> 6) &    1;
+		toggle = (buf[offset] >> 5) &    1;
+		dev    =  buf[offset]       & 0x1f;
+		code   = (buf[offset+1] >> 2) & 0x3f;
+
+		/* rc5 has two start bits
+		 * the first bit must be one
+		 * the second bit defines the command range:
+		 * 1 = 0-63, 0 = 64 - 127
+		 */
+		if (!start)
+			/* no key pressed */
+			return 0;
+
+		/* filter out invalid key presses */
+		ircode = (start << 12) | (toggle << 11) | (dev << 6) | code;
+		if ((ircode & 0x1fff) == 0x1fff)
+			return 0;
+
+		if (!range)
+			code += 64;
+
+		dev_dbg(&ir->rc->dev,
+			"ir hauppauge (rc5): s%d r%d t%d dev=%d code=%d\n",
+			start, range, toggle, dev, code);
+
+		*protocol = RC_PROTO_RC5;
+		*scancode = RC_SCANCODE_RC5(dev, code);
+		*ptoggle = toggle;
+
+		return 1;
+	} else if (size == 6 && (buf[0] & 0x40)) {
+		code = buf[4];
+		dev = buf[3];
+		vendor = get_unaligned_be16(buf + 1);
+
+		if (vendor == 0x800f) {
+			*ptoggle = (dev & 0x80) != 0;
+			*protocol = RC_PROTO_RC6_MCE;
+			dev &= 0x7f;
+			dev_dbg(&ir->rc->dev,
+				"ir hauppauge (rc6-mce): t%d vendor=%d dev=%d code=%d\n",
+				*ptoggle, vendor, dev, code);
+		} else {
+			*ptoggle = 0;
+			*protocol = RC_PROTO_RC6_6A_32;
+			dev_dbg(&ir->rc->dev,
+				"ir hauppauge (rc6-6a-32): vendor=%d dev=%d code=%d\n",
+				vendor, dev, code);
+		}
+
+		*scancode = RC_SCANCODE_RC6_6A(vendor, dev, code);
+
+		return 1;
+	}
+
+	return 0;
+}
+
+static int get_key_haup(struct IR_i2c *ir, enum rc_proto *protocol,
+			u32 *scancode, u8 *toggle)
+{
+	return get_key_haup_common(ir, protocol, scancode, toggle, 3);
+}
+
+static int get_key_haup_xvr(struct IR_i2c *ir, enum rc_proto *protocol,
+			    u32 *scancode, u8 *toggle)
+{
+	int ret;
+	unsigned char buf[1] = { 0 };
+
+	/*
+	 * This is the same apparent "are you ready?" poll command observed
+	 * watching Windows driver traffic and implemented in lirc_zilog. With
+	 * this added, we get far saner remote behavior with z8 chips on usb
+	 * connected devices, even with the default polling interval of 100ms.
+	 */
+	ret = i2c_master_send(ir->c, buf, 1);
+	if (ret != 1)
+		return (ret < 0) ? ret : -EINVAL;
+
+	return get_key_haup_common(ir, protocol, scancode, toggle, 6);
+}
+
+static int get_key_pixelview(struct IR_i2c *ir, enum rc_proto *protocol,
+			     u32 *scancode, u8 *toggle)
+{
+	int rc;
+	unsigned char b;
+
+	/* poll IR chip */
+	rc = i2c_master_recv(ir->c, &b, 1);
+	if (rc != 1) {
+		dev_dbg(&ir->rc->dev, "read error\n");
+		if (rc < 0)
+			return rc;
+		return -EIO;
+	}
+
+	*protocol = RC_PROTO_OTHER;
+	*scancode = b;
+	*toggle = 0;
+	return 1;
+}
+
+static int get_key_fusionhdtv(struct IR_i2c *ir, enum rc_proto *protocol,
+			      u32 *scancode, u8 *toggle)
+{
+	int rc;
+	unsigned char buf[4];
+
+	/* poll IR chip */
+	rc = i2c_master_recv(ir->c, buf, 4);
+	if (rc != 4) {
+		dev_dbg(&ir->rc->dev, "read error\n");
+		if (rc < 0)
+			return rc;
+		return -EIO;
+	}
+
+	if (buf[0] != 0 || buf[1] != 0 || buf[2] != 0 || buf[3] != 0)
+		dev_dbg(&ir->rc->dev, "%s: %*ph\n", __func__, 4, buf);
+
+	/* no key pressed or signal from other ir remote */
+	if(buf[0] != 0x1 ||  buf[1] != 0xfe)
+		return 0;
+
+	*protocol = RC_PROTO_UNKNOWN;
+	*scancode = buf[2];
+	*toggle = 0;
+	return 1;
+}
+
+static int get_key_knc1(struct IR_i2c *ir, enum rc_proto *protocol,
+			u32 *scancode, u8 *toggle)
+{
+	int rc;
+	unsigned char b;
+
+	/* poll IR chip */
+	rc = i2c_master_recv(ir->c, &b, 1);
+	if (rc != 1) {
+		dev_dbg(&ir->rc->dev, "read error\n");
+		if (rc < 0)
+			return rc;
+		return -EIO;
+	}
+
+	/* it seems that 0xFE indicates that a button is still hold
+	   down, while 0xff indicates that no button is hold
+	   down. 0xfe sequences are sometimes interrupted by 0xFF */
+
+	dev_dbg(&ir->rc->dev, "key %02x\n", b);
+
+	if (b == 0xff)
+		return 0;
+
+	if (b == 0xfe)
+		/* keep old data */
+		return 1;
+
+	*protocol = RC_PROTO_UNKNOWN;
+	*scancode = b;
+	*toggle = 0;
+	return 1;
+}
+
+static int get_key_geniatech(struct IR_i2c *ir, enum rc_proto *protocol,
+			     u32 *scancode, u8 *toggle)
+{
+	int i, rc;
+	unsigned char b;
+
+	/* poll IR chip */
+	for (i = 0; i < 4; i++) {
+		rc = i2c_master_recv(ir->c, &b, 1);
+		if (rc == 1)
+			break;
+		msleep(20);
+	}
+	if (rc != 1) {
+		dev_dbg(&ir->rc->dev, "read error\n");
+		if (rc < 0)
+			return rc;
+		return -EIO;
+	}
+
+	/* don't repeat the key */
+	if (ir->old == b)
+		return 0;
+	ir->old = b;
+
+	/* decode to RC5 */
+	b &= 0x7f;
+	b = (b - 1) / 2;
+
+	dev_dbg(&ir->rc->dev, "key %02x\n", b);
+
+	*protocol = RC_PROTO_RC5;
+	*scancode = b;
+	*toggle = ir->old >> 7;
+	return 1;
+}
+
+static int get_key_avermedia_cardbus(struct IR_i2c *ir, enum rc_proto *protocol,
+				     u32 *scancode, u8 *toggle)
+{
+	unsigned char subaddr, key, keygroup;
+	struct i2c_msg msg[] = { { .addr = ir->c->addr, .flags = 0,
+				   .buf = &subaddr, .len = 1},
+				 { .addr = ir->c->addr, .flags = I2C_M_RD,
+				  .buf = &key, .len = 1} };
+	subaddr = 0x0d;
+	if (2 != i2c_transfer(ir->c->adapter, msg, 2)) {
+		dev_dbg(&ir->rc->dev, "read error\n");
+		return -EIO;
+	}
+
+	if (key == 0xff)
+		return 0;
+
+	subaddr = 0x0b;
+	msg[1].buf = &keygroup;
+	if (2 != i2c_transfer(ir->c->adapter, msg, 2)) {
+		dev_dbg(&ir->rc->dev, "read error\n");
+		return -EIO;
+	}
+
+	if (keygroup == 0xff)
+		return 0;
+
+	dev_dbg(&ir->rc->dev, "read key 0x%02x/0x%02x\n", key, keygroup);
+	if (keygroup < 2 || keygroup > 4) {
+		dev_warn(&ir->rc->dev, "warning: invalid key group 0x%02x for key 0x%02x\n",
+			 keygroup, key);
+	}
+	key |= (keygroup & 1) << 6;
+
+	*protocol = RC_PROTO_UNKNOWN;
+	*scancode = key;
+	if (ir->c->addr == 0x41) /* AVerMedia EM78P153 */
+		*scancode |= keygroup << 8;
+	*toggle = 0;
+	return 1;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static int ir_key_poll(struct IR_i2c *ir)
+{
+	enum rc_proto protocol;
+	u32 scancode;
+	u8 toggle;
+	int rc;
+
+	dev_dbg(&ir->rc->dev, "%s\n", __func__);
+	rc = ir->get_key(ir, &protocol, &scancode, &toggle);
+	if (rc < 0) {
+		dev_warn(&ir->rc->dev, "error %d\n", rc);
+		return rc;
+	}
+
+	if (rc) {
+		dev_dbg(&ir->rc->dev, "%s: proto = 0x%04x, scancode = 0x%08x\n",
+			__func__, protocol, scancode);
+		rc_keydown(ir->rc, protocol, scancode, toggle);
+	}
+	return 0;
+}
+
+static void ir_work(struct work_struct *work)
+{
+	int rc;
+	struct IR_i2c *ir = container_of(work, struct IR_i2c, work.work);
+
+	/*
+	 * If the transmit code is holding the lock, skip polling for
+	 * IR, we'll get it to it next time round
+	 */
+	if (mutex_trylock(&ir->lock)) {
+		rc = ir_key_poll(ir);
+		mutex_unlock(&ir->lock);
+		if (rc == -ENODEV) {
+			rc_unregister_device(ir->rc);
+			ir->rc = NULL;
+			return;
+		}
+	}
+
+	schedule_delayed_work(&ir->work, msecs_to_jiffies(ir->polling_interval));
+}
+
+static int ir_open(struct rc_dev *dev)
+{
+	struct IR_i2c *ir = dev->priv;
+
+	schedule_delayed_work(&ir->work, 0);
+
+	return 0;
+}
+
+static void ir_close(struct rc_dev *dev)
+{
+	struct IR_i2c *ir = dev->priv;
+
+	cancel_delayed_work_sync(&ir->work);
+}
+
+/* Zilog Transmit Interface */
+#define XTAL_FREQ		18432000
+
+#define ZILOG_SEND		0x80
+#define ZILOG_UIR_END		0x40
+#define ZILOG_INIT_END		0x20
+#define ZILOG_LIR_END		0x10
+
+#define ZILOG_STATUS_OK		0x80
+#define ZILOG_STATUS_TX		0x40
+#define ZILOG_STATUS_SET	0x20
+
+/*
+ * As you can see here, very few different lengths of pulse and space
+ * can be encoded. This means that the hardware does not work well with
+ * recorded IR. It's best to work with generated IR, like from ir-ctl or
+ * the in-kernel encoders.
+ */
+struct code_block {
+	u8	length;
+	u16	pulse[7];	/* not aligned */
+	u8	carrier_pulse;
+	u8	carrier_space;
+	u16	space[8];	/* not aligned */
+	u8	codes[61];
+	u8	csum[2];
+} __packed;
+
+static int send_data_block(struct IR_i2c *ir, int cmd,
+			   struct code_block *code_block)
+{
+	int i, j, ret;
+	u8 buf[5], *p;
+
+	p = &code_block->length;
+	for (i = 0; p < code_block->csum; i++)
+		code_block->csum[i & 1] ^= *p++;
+
+	p = &code_block->length;
+
+	for (i = 0; i < sizeof(*code_block);) {
+		int tosend = sizeof(*code_block) - i;
+
+		if (tosend > 4)
+			tosend = 4;
+		buf[0] = i + 1;
+		for (j = 0; j < tosend; ++j)
+			buf[1 + j] = p[i + j];
+		dev_dbg(&ir->rc->dev, "%*ph", tosend + 1, buf);
+		ret = i2c_master_send(ir->tx_c, buf, tosend + 1);
+		if (ret != tosend + 1) {
+			dev_dbg(&ir->rc->dev,
+				"i2c_master_send failed with %d\n", ret);
+			return ret < 0 ? ret : -EIO;
+		}
+		i += tosend;
+	}
+
+	buf[0] = 0;
+	buf[1] = cmd;
+	ret = i2c_master_send(ir->tx_c, buf, 2);
+	if (ret != 2) {
+		dev_err(&ir->rc->dev, "i2c_master_send failed with %d\n", ret);
+		return ret < 0 ? ret : -EIO;
+	}
+
+	usleep_range(2000, 5000);
+
+	ret = i2c_master_send(ir->tx_c, buf, 1);
+	if (ret != 1) {
+		dev_err(&ir->rc->dev, "i2c_master_send failed with %d\n", ret);
+		return ret < 0 ? ret : -EIO;
+	}
+
+	return 0;
+}
+
+static int zilog_init(struct IR_i2c *ir)
+{
+	struct code_block code_block = { .length = sizeof(code_block) };
+	u8 buf[4];
+	int ret;
+
+	put_unaligned_be16(0x1000, &code_block.pulse[3]);
+
+	ret = send_data_block(ir, ZILOG_INIT_END, &code_block);
+	if (ret)
+		return ret;
+
+	ret = i2c_master_recv(ir->tx_c, buf, 4);
+	if (ret != 4) {
+		dev_err(&ir->c->dev, "failed to retrieve firmware version: %d\n",
+			ret);
+		return ret < 0 ? ret : -EIO;
+	}
+
+	dev_info(&ir->c->dev, "Zilog/Hauppauge IR blaster firmware version %d.%d.%d\n",
+		 buf[1], buf[2], buf[3]);
+
+	return 0;
+}
+
+/*
+ * If the last slot for pulse is the same as the current slot for pulse,
+ * then use slot no 7.
+ */
+static void copy_codes(u8 *dst, u8 *src, unsigned int count)
+{
+	u8 c, last = 0xff;
+
+	while (count--) {
+		c = *src++;
+		if ((c & 0xf0) == last) {
+			*dst++ = 0x70 | (c & 0xf);
+		} else {
+			*dst++ = c;
+			last = c & 0xf0;
+		}
+	}
+}
+
+/*
+ * When looking for repeats, we don't care about the trailing space. This
+ * is set to the shortest possible anyway.
+ */
+static int cmp_no_trail(u8 *a, u8 *b, unsigned int count)
+{
+	while (--count) {
+		if (*a++ != *b++)
+			return 1;
+	}
+
+	return (*a & 0xf0) - (*b & 0xf0);
+}
+
+static int find_slot(u16 *array, unsigned int size, u16 val)
+{
+	int i;
+
+	for (i = 0; i < size; i++) {
+		if (get_unaligned_be16(&array[i]) == val) {
+			return i;
+		} else if (!array[i]) {
+			put_unaligned_be16(val, &array[i]);
+			return i;
+		}
+	}
+
+	return -1;
+}
+
+static int zilog_ir_format(struct rc_dev *rcdev, unsigned int *txbuf,
+			   unsigned int count, struct code_block *code_block)
+{
+	struct IR_i2c *ir = rcdev->priv;
+	int rep, i, l, p = 0, s, c = 0;
+	bool repeating;
+	u8 codes[174];
+
+	code_block->carrier_pulse = DIV_ROUND_CLOSEST(
+			ir->duty_cycle * XTAL_FREQ / 1000, ir->carrier);
+	code_block->carrier_space = DIV_ROUND_CLOSEST(
+			(100 - ir->duty_cycle) * XTAL_FREQ / 1000, ir->carrier);
+
+	for (i = 0; i < count; i++) {
+		if (c >= ARRAY_SIZE(codes) - 1) {
+			dev_warn(&rcdev->dev, "IR too long, cannot transmit\n");
+			return -EINVAL;
+		}
+
+		/*
+		 * Lengths more than 142220us cannot be encoded; also
+		 * this checks for multiply overflow
+		 */
+		if (txbuf[i] > 142220)
+			return -EINVAL;
+
+		l = DIV_ROUND_CLOSEST((XTAL_FREQ / 1000) * txbuf[i], 40000);
+
+		if (i & 1) {
+			s = find_slot(code_block->space,
+				      ARRAY_SIZE(code_block->space), l);
+			if (s == -1) {
+				dev_warn(&rcdev->dev, "Too many different lengths spaces, cannot transmit");
+				return -EINVAL;
+			}
+
+			/* We have a pulse and space */
+			codes[c++] = (p << 4) | s;
+		} else {
+			p = find_slot(code_block->pulse,
+				      ARRAY_SIZE(code_block->pulse), l);
+			if (p == -1) {
+				dev_warn(&rcdev->dev, "Too many different lengths pulses, cannot transmit");
+				return -EINVAL;
+			}
+		}
+	}
+
+	/* We have to encode the trailing pulse. Find the shortest space */
+	s = 0;
+	for (i = 1; i < ARRAY_SIZE(code_block->space); i++) {
+		u16 d = get_unaligned_be16(&code_block->space[i]);
+
+		if (get_unaligned_be16(&code_block->space[s]) > d)
+			s = i;
+	}
+
+	codes[c++] = (p << 4) | s;
+
+	dev_dbg(&rcdev->dev, "generated %d codes\n", c);
+
+	/*
+	 * Are the last N codes (so pulse + space) repeating 3 times?
+	 * if so we can shorten the codes list and use code 0xc0 to repeat
+	 * them.
+	 */
+	repeating = false;
+
+	for (rep = c / 3; rep >= 1; rep--) {
+		if (!memcmp(&codes[c - rep * 3], &codes[c - rep * 2], rep) &&
+		    !cmp_no_trail(&codes[c - rep], &codes[c - rep * 2], rep)) {
+			repeating = true;
+			break;
+		}
+	}
+
+	if (repeating) {
+		/* first copy any leading non-repeating */
+		int leading = c - rep * 3;
+
+		if (leading >= ARRAY_SIZE(code_block->codes) - 3 - rep) {
+			dev_warn(&rcdev->dev, "IR too long, cannot transmit\n");
+			return -EINVAL;
+		}
+
+		dev_dbg(&rcdev->dev, "found trailing %d repeat\n", rep);
+		copy_codes(code_block->codes, codes, leading);
+		code_block->codes[leading] = 0x82;
+		copy_codes(code_block->codes + leading + 1, codes + leading,
+			   rep);
+		c = leading + 1 + rep;
+		code_block->codes[c++] = 0xc0;
+	} else {
+		if (c >= ARRAY_SIZE(code_block->codes) - 3) {
+			dev_warn(&rcdev->dev, "IR too long, cannot transmit\n");
+			return -EINVAL;
+		}
+
+		dev_dbg(&rcdev->dev, "found no trailing repeat\n");
+		code_block->codes[0] = 0x82;
+		copy_codes(code_block->codes + 1, codes, c);
+		c++;
+		code_block->codes[c++] = 0xc4;
+	}
+
+	while (c < ARRAY_SIZE(code_block->codes))
+		code_block->codes[c++] = 0x83;
+
+	return 0;
+}
+
+static int zilog_tx(struct rc_dev *rcdev, unsigned int *txbuf,
+		    unsigned int count)
+{
+	struct IR_i2c *ir = rcdev->priv;
+	struct code_block code_block = { .length = sizeof(code_block) };
+	u8 buf[2];
+	int ret, i;
+
+	ret = zilog_ir_format(rcdev, txbuf, count, &code_block);
+	if (ret)
+		return ret;
+
+	ret = mutex_lock_interruptible(&ir->lock);
+	if (ret)
+		return ret;
+
+	ret = send_data_block(ir, ZILOG_UIR_END, &code_block);
+	if (ret)
+		goto out_unlock;
+
+	ret = i2c_master_recv(ir->tx_c, buf, 1);
+	if (ret != 1) {
+		dev_err(&ir->rc->dev, "i2c_master_recv failed with %d\n", ret);
+		goto out_unlock;
+	}
+
+	dev_dbg(&ir->rc->dev, "code set status: %02x\n", buf[0]);
+
+	if (buf[0] != (ZILOG_STATUS_OK | ZILOG_STATUS_SET)) {
+		dev_err(&ir->rc->dev, "unexpected IR TX response %02x\n",
+			buf[0]);
+		ret = -EIO;
+		goto out_unlock;
+	}
+
+	buf[0] = 0x00;
+	buf[1] = ZILOG_SEND;
+
+	ret = i2c_master_send(ir->tx_c, buf, 2);
+	if (ret != 2) {
+		dev_err(&ir->rc->dev, "i2c_master_send failed with %d\n", ret);
+		if (ret >= 0)
+			ret = -EIO;
+		goto out_unlock;
+	}
+
+	dev_dbg(&ir->rc->dev, "send command sent\n");
+
+	/*
+	 * This bit NAKs until the device is ready, so we retry it
+	 * sleeping a bit each time.  This seems to be what the windows
+	 * driver does, approximately.
+	 * Try for up to 1s.
+	 */
+	for (i = 0; i < 20; ++i) {
+		set_current_state(TASK_UNINTERRUPTIBLE);
+		schedule_timeout(msecs_to_jiffies(50));
+		ret = i2c_master_send(ir->tx_c, buf, 1);
+		if (ret == 1)
+			break;
+		dev_dbg(&ir->rc->dev,
+			"NAK expected: i2c_master_send failed with %d (try %d)\n",
+			ret, i + 1);
+	}
+
+	if (ret != 1) {
+		dev_err(&ir->rc->dev,
+			"IR TX chip never got ready: last i2c_master_send failed with %d\n",
+			ret);
+		if (ret >= 0)
+			ret = -EIO;
+		goto out_unlock;
+	}
+
+	ret = i2c_master_recv(ir->tx_c, buf, 1);
+	if (ret != 1) {
+		dev_err(&ir->rc->dev, "i2c_master_recv failed with %d\n", ret);
+		ret = -EIO;
+		goto out_unlock;
+	} else if (buf[0] != ZILOG_STATUS_OK) {
+		dev_err(&ir->rc->dev, "unexpected IR TX response #2: %02x\n",
+			buf[0]);
+		ret = -EIO;
+		goto out_unlock;
+	}
+	dev_dbg(&ir->rc->dev, "transmit complete\n");
+
+	/* Oh good, it worked */
+	ret = count;
+out_unlock:
+	mutex_unlock(&ir->lock);
+
+	return ret;
+}
+
+static int zilog_tx_carrier(struct rc_dev *dev, u32 carrier)
+{
+	struct IR_i2c *ir = dev->priv;
+
+	if (carrier > 500000 || carrier < 20000)
+		return -EINVAL;
+
+	ir->carrier = carrier;
+
+	return 0;
+}
+
+static int zilog_tx_duty_cycle(struct rc_dev *dev, u32 duty_cycle)
+{
+	struct IR_i2c *ir = dev->priv;
+
+	ir->duty_cycle = duty_cycle;
+
+	return 0;
+}
+
+static int ir_probe(struct i2c_client *client)
+{
+	const struct i2c_device_id *id = i2c_client_get_device_id(client);
+	char *ir_codes = NULL;
+	const char *name = NULL;
+	u64 rc_proto = RC_PROTO_BIT_UNKNOWN;
+	struct IR_i2c *ir;
+	struct rc_dev *rc = NULL;
+	struct i2c_adapter *adap = client->adapter;
+	unsigned short addr = client->addr;
+	bool probe_tx = (id->driver_data & FLAG_TX) != 0;
+	int err;
+
+	if ((id->driver_data & FLAG_HDPVR) && !enable_hdpvr) {
+		dev_err(&client->dev, "IR for HDPVR is known to cause problems during recording, use enable_hdpvr modparam to enable\n");
+		return -ENODEV;
+	}
+
+	ir = devm_kzalloc(&client->dev, sizeof(*ir), GFP_KERNEL);
+	if (!ir)
+		return -ENOMEM;
+
+	ir->c = client;
+	ir->polling_interval = DEFAULT_POLLING_INTERVAL;
+	i2c_set_clientdata(client, ir);
+
+	switch(addr) {
+	case 0x64:
+		name        = "Pixelview";
+		ir->get_key = get_key_pixelview;
+		rc_proto    = RC_PROTO_BIT_OTHER;
+		ir_codes    = RC_MAP_EMPTY;
+		break;
+	case 0x18:
+	case 0x1f:
+	case 0x1a:
+		name        = "Hauppauge";
+		ir->get_key = get_key_haup;
+		rc_proto    = RC_PROTO_BIT_RC5;
+		ir_codes    = RC_MAP_HAUPPAUGE;
+		break;
+	case 0x30:
+		name        = "KNC One";
+		ir->get_key = get_key_knc1;
+		rc_proto    = RC_PROTO_BIT_OTHER;
+		ir_codes    = RC_MAP_EMPTY;
+		break;
+	case 0x33:
+		name        = "Geniatech";
+		ir->get_key = get_key_geniatech;
+		rc_proto    = RC_PROTO_BIT_RC5;
+		ir_codes    = RC_MAP_TOTAL_MEDIA_IN_HAND_02;
+		ir->old     = 0xfc;
+		break;
+	case 0x6b:
+		name        = "FusionHDTV";
+		ir->get_key = get_key_fusionhdtv;
+		rc_proto    = RC_PROTO_BIT_UNKNOWN;
+		ir_codes    = RC_MAP_FUSIONHDTV_MCE;
+		break;
+	case 0x40:
+		name        = "AVerMedia Cardbus remote";
+		ir->get_key = get_key_avermedia_cardbus;
+		rc_proto    = RC_PROTO_BIT_OTHER;
+		ir_codes    = RC_MAP_AVERMEDIA_CARDBUS;
+		break;
+	case 0x41:
+		name        = "AVerMedia EM78P153";
+		ir->get_key = get_key_avermedia_cardbus;
+		rc_proto    = RC_PROTO_BIT_OTHER;
+		/* RM-KV remote, seems to be same as RM-K6 */
+		ir_codes    = RC_MAP_AVERMEDIA_M733A_RM_K6;
+		break;
+	case 0x71:
+		name        = "Hauppauge/Zilog Z8";
+		ir->get_key = get_key_haup_xvr;
+		rc_proto    = RC_PROTO_BIT_RC5 | RC_PROTO_BIT_RC6_MCE |
+							RC_PROTO_BIT_RC6_6A_32;
+		ir_codes    = RC_MAP_HAUPPAUGE;
+		ir->polling_interval = 125;
+		probe_tx = true;
+		break;
+	}
+
+	/* Let the caller override settings */
+	if (client->dev.platform_data) {
+		const struct IR_i2c_init_data *init_data =
+						client->dev.platform_data;
+
+		ir_codes = init_data->ir_codes;
+		rc = init_data->rc_dev;
+
+		name = init_data->name;
+		if (init_data->type)
+			rc_proto = init_data->type;
+
+		if (init_data->polling_interval)
+			ir->polling_interval = init_data->polling_interval;
+
+		switch (init_data->internal_get_key_func) {
+		case IR_KBD_GET_KEY_CUSTOM:
+			/* The bridge driver provided us its own function */
+			ir->get_key = init_data->get_key;
+			break;
+		case IR_KBD_GET_KEY_PIXELVIEW:
+			ir->get_key = get_key_pixelview;
+			break;
+		case IR_KBD_GET_KEY_HAUP:
+			ir->get_key = get_key_haup;
+			break;
+		case IR_KBD_GET_KEY_KNC1:
+			ir->get_key = get_key_knc1;
+			break;
+		case IR_KBD_GET_KEY_GENIATECH:
+			ir->get_key = get_key_geniatech;
+			break;
+		case IR_KBD_GET_KEY_FUSIONHDTV:
+			ir->get_key = get_key_fusionhdtv;
+			break;
+		case IR_KBD_GET_KEY_HAUP_XVR:
+			ir->get_key = get_key_haup_xvr;
+			break;
+		case IR_KBD_GET_KEY_AVERMEDIA_CARDBUS:
+			ir->get_key = get_key_avermedia_cardbus;
+			break;
+		}
+	}
+
+	if (!rc) {
+		/*
+		 * If platform_data doesn't specify rc_dev, initialize it
+		 * internally
+		 */
+		rc = rc_allocate_device(RC_DRIVER_SCANCODE);
+		if (!rc)
+			return -ENOMEM;
+	}
+	ir->rc = rc;
+
+	/* Make sure we are all setup before going on */
+	if (!name || !ir->get_key || !rc_proto || !ir_codes) {
+		dev_warn(&client->dev, "Unsupported device at address 0x%02x\n",
+			 addr);
+		err = -ENODEV;
+		goto err_out_free;
+	}
+
+	ir->ir_codes = ir_codes;
+
+	snprintf(ir->phys, sizeof(ir->phys), "%s/%s", dev_name(&adap->dev),
+		 dev_name(&client->dev));
+
+	/*
+	 * Initialize input_dev fields
+	 * It doesn't make sense to allow overriding them via platform_data
+	 */
+	rc->input_id.bustype = BUS_I2C;
+	rc->input_phys       = ir->phys;
+	rc->device_name	     = name;
+	rc->dev.parent       = &client->dev;
+	rc->priv             = ir;
+	rc->open             = ir_open;
+	rc->close            = ir_close;
+
+	/*
+	 * Initialize the other fields of rc_dev
+	 */
+	rc->map_name       = ir->ir_codes;
+	rc->allowed_protocols = rc_proto;
+	if (!rc->driver_name)
+		rc->driver_name = KBUILD_MODNAME;
+
+	mutex_init(&ir->lock);
+
+	INIT_DELAYED_WORK(&ir->work, ir_work);
+
+	if (probe_tx) {
+		ir->tx_c = i2c_new_dummy_device(client->adapter, 0x70);
+		if (IS_ERR(ir->tx_c)) {
+			dev_err(&client->dev, "failed to setup tx i2c address");
+			err = PTR_ERR(ir->tx_c);
+			goto err_out_free;
+		} else if (!zilog_init(ir)) {
+			ir->carrier = 38000;
+			ir->duty_cycle = 40;
+			rc->tx_ir = zilog_tx;
+			rc->s_tx_carrier = zilog_tx_carrier;
+			rc->s_tx_duty_cycle = zilog_tx_duty_cycle;
+		}
+	}
+
+	err = rc_register_device(rc);
+	if (err)
+		goto err_out_free;
+
+	return 0;
+
+ err_out_free:
+	if (!IS_ERR(ir->tx_c))
+		i2c_unregister_device(ir->tx_c);
+
+	/* Only frees rc if it were allocated internally */
+	rc_free_device(rc);
+	return err;
+}
+
+static void ir_remove(struct i2c_client *client)
+{
+	struct IR_i2c *ir = i2c_get_clientdata(client);
+
+	cancel_delayed_work_sync(&ir->work);
+
+	i2c_unregister_device(ir->tx_c);
+
+	rc_unregister_device(ir->rc);
+}
+
+static const struct i2c_device_id ir_kbd_id[] = {
+	/* Generic entry for any IR receiver */
+	{ "ir_video", 0 },
+	/* IR device specific entries should be added here */
+	{ "ir_z8f0811_haup", FLAG_TX },
+	{ "ir_z8f0811_hdpvr", FLAG_TX | FLAG_HDPVR },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ir_kbd_id);
+
+static struct i2c_driver ir_kbd_driver = {
+	.driver = {
+		.name   = "ir-kbd-i2c",
+	},
+	.probe          = ir_probe,
+	.remove         = ir_remove,
+	.id_table       = ir_kbd_id,
+};
+
+module_i2c_driver(ir_kbd_driver);
+
+/* ----------------------------------------------------------------------- */
+
+MODULE_AUTHOR("Gerd Knorr, Michal Kochanowicz, Christoph Bartelmus, Ulrich Mueller");
+MODULE_DESCRIPTION("input driver for i2c IR remote controls");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/isl7998x.c b/drivers/media/i2c/isl7998x.c
new file mode 100644
index 000000000..73460688c
--- /dev/null
+++ b/drivers/media/i2c/isl7998x.c
@@ -0,0 +1,1626 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Intersil ISL7998x analog to MIPI CSI-2 or BT.656 decoder driver.
+ *
+ * Copyright (C) 2018-2019 Marek Vasut <marex@denx.de>
+ * Copyright (C) 2021 Michael Tretter <kernel@pengutronix.de>
+ */
+
+#include <linux/bitfield.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/of_graph.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+#include <linux/v4l2-mediabus.h>
+#include <linux/videodev2.h>
+
+#include <media/v4l2-async.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-ioctl.h>
+
+/*
+ * This control allows to activate and deactivate the test pattern on
+ * selected output channels.
+ * This value is ISL7998x specific.
+ */
+#define V4L2_CID_TEST_PATTERN_CHANNELS	(V4L2_CID_USER_ISL7998X_BASE + 0)
+
+/*
+ * This control allows to specify the color of the test pattern.
+ * This value is ISL7998x specific.
+ */
+#define V4L2_CID_TEST_PATTERN_COLOR	(V4L2_CID_USER_ISL7998X_BASE + 1)
+
+/*
+ * This control allows to specify the bar pattern in the test pattern.
+ * This value is ISL7998x specific.
+ */
+#define V4L2_CID_TEST_PATTERN_BARS	(V4L2_CID_USER_ISL7998X_BASE + 2)
+
+#define ISL7998X_INPUTS			4
+
+#define ISL7998X_REG(page, reg)		(((page) << 8) | (reg))
+
+#define ISL7998X_REG_PN_SIZE			256
+#define ISL7998X_REG_PN_BASE(n)			((n) * ISL7998X_REG_PN_SIZE)
+
+#define ISL7998X_REG_PX_DEC_PAGE(page)		ISL7998X_REG((page), 0xff)
+#define ISL7998X_REG_PX_DEC_PAGE_MASK		0xf
+#define ISL7998X_REG_P0_PRODUCT_ID_CODE		ISL7998X_REG(0, 0x00)
+#define ISL7998X_REG_P0_PRODUCT_REV_CODE	ISL7998X_REG(0, 0x01)
+#define ISL7998X_REG_P0_SW_RESET_CTL		ISL7998X_REG(0, 0x02)
+#define ISL7998X_REG_P0_IO_BUFFER_CTL		ISL7998X_REG(0, 0x03)
+#define ISL7998X_REG_P0_IO_BUFFER_CTL_1_1	ISL7998X_REG(0, 0x04)
+#define ISL7998X_REG_P0_IO_PAD_PULL_EN_CTL	ISL7998X_REG(0, 0x05)
+#define ISL7998X_REG_P0_IO_BUFFER_CTL_1_2	ISL7998X_REG(0, 0x06)
+#define ISL7998X_REG_P0_VIDEO_IN_CHAN_CTL	ISL7998X_REG(0, 0x07)
+#define ISL7998X_REG_P0_CLK_CTL_1		ISL7998X_REG(0, 0x08)
+#define ISL7998X_REG_P0_CLK_CTL_2		ISL7998X_REG(0, 0x09)
+#define ISL7998X_REG_P0_CLK_CTL_3		ISL7998X_REG(0, 0x0a)
+#define ISL7998X_REG_P0_CLK_CTL_4		ISL7998X_REG(0, 0x0b)
+#define ISL7998X_REG_P0_MPP1_SYNC_CTL		ISL7998X_REG(0, 0x0c)
+#define ISL7998X_REG_P0_MPP2_SYNC_CTL		ISL7998X_REG(0, 0x0d)
+#define ISL7998X_REG_P0_IRQ_SYNC_CTL		ISL7998X_REG(0, 0x0e)
+#define ISL7998X_REG_P0_INTERRUPT_STATUS	ISL7998X_REG(0, 0x10)
+#define ISL7998X_REG_P0_CHAN_1_IRQ		ISL7998X_REG(0, 0x11)
+#define ISL7998X_REG_P0_CHAN_2_IRQ		ISL7998X_REG(0, 0x12)
+#define ISL7998X_REG_P0_CHAN_3_IRQ		ISL7998X_REG(0, 0x13)
+#define ISL7998X_REG_P0_CHAN_4_IRQ		ISL7998X_REG(0, 0x14)
+#define ISL7998X_REG_P0_SHORT_DIAG_IRQ		ISL7998X_REG(0, 0x15)
+#define ISL7998X_REG_P0_CHAN_1_IRQ_EN		ISL7998X_REG(0, 0x16)
+#define ISL7998X_REG_P0_CHAN_2_IRQ_EN		ISL7998X_REG(0, 0x17)
+#define ISL7998X_REG_P0_CHAN_3_IRQ_EN		ISL7998X_REG(0, 0x18)
+#define ISL7998X_REG_P0_CHAN_4_IRQ_EN		ISL7998X_REG(0, 0x19)
+#define ISL7998X_REG_P0_SHORT_DIAG_IRQ_EN	ISL7998X_REG(0, 0x1a)
+#define ISL7998X_REG_P0_CHAN_1_STATUS		ISL7998X_REG(0, 0x1b)
+#define ISL7998X_REG_P0_CHAN_2_STATUS		ISL7998X_REG(0, 0x1c)
+#define ISL7998X_REG_P0_CHAN_3_STATUS		ISL7998X_REG(0, 0x1d)
+#define ISL7998X_REG_P0_CHAN_4_STATUS		ISL7998X_REG(0, 0x1e)
+#define ISL7998X_REG_P0_SHORT_DIAG_STATUS	ISL7998X_REG(0, 0x1f)
+#define ISL7998X_REG_P0_CLOCK_DELAY		ISL7998X_REG(0, 0x20)
+
+#define ISL7998X_REG_PX_DEC_INPUT_FMT(pg)	ISL7998X_REG((pg), 0x02)
+#define ISL7998X_REG_PX_DEC_STATUS_1(pg)	ISL7998X_REG((pg), 0x03)
+#define ISL7998X_REG_PX_DEC_STATUS_1_VDLOSS	BIT(7)
+#define ISL7998X_REG_PX_DEC_STATUS_1_HLOCK	BIT(6)
+#define ISL7998X_REG_PX_DEC_STATUS_1_VLOCK	BIT(3)
+#define ISL7998X_REG_PX_DEC_HS_DELAY_CTL(pg)	ISL7998X_REG((pg), 0x04)
+#define ISL7998X_REG_PX_DEC_ANCTL(pg)		ISL7998X_REG((pg), 0x06)
+#define ISL7998X_REG_PX_DEC_CROP_HI(pg)		ISL7998X_REG((pg), 0x07)
+#define ISL7998X_REG_PX_DEC_VDELAY_LO(pg)	ISL7998X_REG((pg), 0x08)
+#define ISL7998X_REG_PX_DEC_VACTIVE_LO(pg)	ISL7998X_REG((pg), 0x09)
+#define ISL7998X_REG_PX_DEC_HDELAY_LO(pg)	ISL7998X_REG((pg), 0x0a)
+#define ISL7998X_REG_PX_DEC_HACTIVE_LO(pg)	ISL7998X_REG((pg), 0x0b)
+#define ISL7998X_REG_PX_DEC_CNTRL1(pg)		ISL7998X_REG((pg), 0x0c)
+#define ISL7998X_REG_PX_DEC_CSC_CTL(pg)		ISL7998X_REG((pg), 0x0d)
+#define ISL7998X_REG_PX_DEC_BRIGHT(pg)		ISL7998X_REG((pg), 0x10)
+#define ISL7998X_REG_PX_DEC_CONTRAST(pg)	ISL7998X_REG((pg), 0x11)
+#define ISL7998X_REG_PX_DEC_SHARPNESS(pg)	ISL7998X_REG((pg), 0x12)
+#define ISL7998X_REG_PX_DEC_SAT_U(pg)		ISL7998X_REG((pg), 0x13)
+#define ISL7998X_REG_PX_DEC_SAT_V(pg)		ISL7998X_REG((pg), 0x14)
+#define ISL7998X_REG_PX_DEC_HUE(pg)		ISL7998X_REG((pg), 0x15)
+#define ISL7998X_REG_PX_DEC_VERT_PEAK(pg)	ISL7998X_REG((pg), 0x17)
+#define ISL7998X_REG_PX_DEC_CORING(pg)		ISL7998X_REG((pg), 0x18)
+#define ISL7998X_REG_PX_DEC_SDT(pg)		ISL7998X_REG((pg), 0x1c)
+#define ISL7998X_REG_PX_DEC_SDT_DET		BIT(7)
+#define ISL7998X_REG_PX_DEC_SDT_NOW		GENMASK(6, 4)
+#define ISL7998X_REG_PX_DEC_SDT_STANDARD	GENMASK(2, 0)
+#define ISL7998X_REG_PX_DEC_SDT_STANDARD_NTSC_M		0
+#define ISL7998X_REG_PX_DEC_SDT_STANDARD_PAL		1
+#define ISL7998X_REG_PX_DEC_SDT_STANDARD_SECAM		2
+#define ISL7998X_REG_PX_DEC_SDT_STANDARD_NTSC_443	3
+#define ISL7998X_REG_PX_DEC_SDT_STANDARD_PAL_M		4
+#define ISL7998X_REG_PX_DEC_SDT_STANDARD_PAL_CN		5
+#define ISL7998X_REG_PX_DEC_SDT_STANDARD_PAL_60		6
+#define ISL7998X_REG_PX_DEC_SDT_STANDARD_UNKNOWN	7
+#define ISL7998X_REG_PX_DEC_SDTR(pg)		ISL7998X_REG((pg), 0x1d)
+#define ISL7998X_REG_PX_DEC_SDTR_ATSTART	BIT(7)
+#define ISL7998X_REG_PX_DEC_CLMPG(pg)		ISL7998X_REG((pg), 0x20)
+#define ISL7998X_REG_PX_DEC_IAGC(pg)		ISL7998X_REG((pg), 0x21)
+#define ISL7998X_REG_PX_DEC_AGCGAIN(pg)		ISL7998X_REG((pg), 0x22)
+#define ISL7998X_REG_PX_DEC_PEAKWT(pg)		ISL7998X_REG((pg), 0x23)
+#define ISL7998X_REG_PX_DEC_CLMPL(pg)		ISL7998X_REG((pg), 0x24)
+#define ISL7998X_REG_PX_DEC_SYNCT(pg)		ISL7998X_REG((pg), 0x25)
+#define ISL7998X_REG_PX_DEC_MISSCNT(pg)		ISL7998X_REG((pg), 0x26)
+#define ISL7998X_REG_PX_DEC_PCLAMP(pg)		ISL7998X_REG((pg), 0x27)
+#define ISL7998X_REG_PX_DEC_VERT_CTL_1(pg)	ISL7998X_REG((pg), 0x28)
+#define ISL7998X_REG_PX_DEC_VERT_CTL_2(pg)	ISL7998X_REG((pg), 0x29)
+#define ISL7998X_REG_PX_DEC_CLR_KILL_LVL(pg)	ISL7998X_REG((pg), 0x2a)
+#define ISL7998X_REG_PX_DEC_COMB_FILTER_CTL(pg)	ISL7998X_REG((pg), 0x2b)
+#define ISL7998X_REG_PX_DEC_LUMA_DELAY(pg)	ISL7998X_REG((pg), 0x2c)
+#define ISL7998X_REG_PX_DEC_MISC1(pg)		ISL7998X_REG((pg), 0x2d)
+#define ISL7998X_REG_PX_DEC_MISC2(pg)		ISL7998X_REG((pg), 0x2e)
+#define ISL7998X_REG_PX_DEC_MISC3(pg)		ISL7998X_REG((pg), 0x2f)
+#define ISL7998X_REG_PX_DEC_MVSN(pg)		ISL7998X_REG((pg), 0x30)
+#define ISL7998X_REG_PX_DEC_CSTATUS2(pg)	ISL7998X_REG((pg), 0x31)
+#define ISL7998X_REG_PX_DEC_HFREF(pg)		ISL7998X_REG((pg), 0x32)
+#define ISL7998X_REG_PX_DEC_CLMD(pg)		ISL7998X_REG((pg), 0x33)
+#define ISL7998X_REG_PX_DEC_ID_DET_CTL(pg)	ISL7998X_REG((pg), 0x34)
+#define ISL7998X_REG_PX_DEC_CLCNTL(pg)		ISL7998X_REG((pg), 0x35)
+#define ISL7998X_REG_PX_DEC_DIFF_CLMP_CTL_1(pg)	ISL7998X_REG((pg), 0x36)
+#define ISL7998X_REG_PX_DEC_DIFF_CLMP_CTL_2(pg)	ISL7998X_REG((pg), 0x37)
+#define ISL7998X_REG_PX_DEC_DIFF_CLMP_CTL_3(pg)	ISL7998X_REG((pg), 0x38)
+#define ISL7998X_REG_PX_DEC_DIFF_CLMP_CTL_4(pg)	ISL7998X_REG((pg), 0x39)
+#define ISL7998X_REG_PX_DEC_SHORT_DET_CTL(pg)	ISL7998X_REG((pg), 0x3a)
+#define ISL7998X_REG_PX_DEC_SHORT_DET_CTL_1(pg)	ISL7998X_REG((pg), 0x3b)
+#define ISL7998X_REG_PX_DEC_AFE_TST_MUX_CTL(pg)	ISL7998X_REG((pg), 0x3c)
+#define ISL7998X_REG_PX_DEC_DATA_CONV(pg)	ISL7998X_REG((pg), 0x3d)
+#define ISL7998X_REG_PX_DEC_INTERNAL_TEST(pg)	ISL7998X_REG((pg), 0x3f)
+#define ISL7998X_REG_PX_DEC_H_DELAY_CTL(pg)	ISL7998X_REG((pg), 0x43)
+#define ISL7998X_REG_PX_DEC_H_DELAY_II_HI(pg)	ISL7998X_REG((pg), 0x44)
+#define ISL7998X_REG_PX_DEC_H_DELAY_II_LOW(pg)	ISL7998X_REG((pg), 0x45)
+
+#define ISL7998X_REG_PX_ACA_CTL_1(pg)		ISL7998X_REG((pg), 0x80)
+#define ISL7998X_REG_PX_ACA_GAIN_CTL(pg)	ISL7998X_REG((pg), 0x81)
+#define ISL7998X_REG_PX_ACA_Y_AVG_HI_LIMIT(pg)	ISL7998X_REG((pg), 0x82)
+#define ISL7998X_REG_PX_ACA_Y_AVG_LO_LIMIT(pg)	ISL7998X_REG((pg), 0x83)
+#define ISL7998X_REG_PX_ACA_Y_DET_THRESHOLD(pg)	ISL7998X_REG((pg), 0x84)
+#define ISL7998X_REG_PX_ACA_BLACK_LVL(pg)	ISL7998X_REG((pg), 0x85)
+#define ISL7998X_REG_PX_ACA_CENTER_LVL(pg)	ISL7998X_REG((pg), 0x86)
+#define ISL7998X_REG_PX_ACA_WHITE_LVL(pg)	ISL7998X_REG((pg), 0x87)
+#define ISL7998X_REG_PX_ACA_MEAN_OFF_LIMIT(pg)	ISL7998X_REG((pg), 0x88)
+#define ISL7998X_REG_PX_ACA_MEAN_OFF_UPGAIN(pg)	ISL7998X_REG((pg), 0x89)
+#define ISL7998X_REG_PX_ACA_MEAN_OFF_SLOPE(pg)	ISL7998X_REG((pg), 0x8a)
+#define ISL7998X_REG_PX_ACA_MEAN_OFF_DNGAIN(pg)	ISL7998X_REG((pg), 0x8b)
+#define ISL7998X_REG_PX_ACA_DELTA_CO_THRES(pg)	ISL7998X_REG((pg), 0x8c)
+#define ISL7998X_REG_PX_ACA_DELTA_SLOPE(pg)	ISL7998X_REG((pg), 0x8d)
+#define ISL7998X_REG_PX_ACA_LO_HI_AVG_THRES(pg)	ISL7998X_REG((pg), 0x8e)
+#define ISL7998X_REG_PX_ACA_LO_MAX_LVL_CTL(pg)	ISL7998X_REG((pg), 0x8f)
+#define ISL7998X_REG_PX_ACA_HI_MAX_LVL_CTL(pg)	ISL7998X_REG((pg), 0x90)
+#define ISL7998X_REG_PX_ACA_LO_UPGAIN_CTL(pg)	ISL7998X_REG((pg), 0x91)
+#define ISL7998X_REG_PX_ACA_LO_DNGAIN_CTL(pg)	ISL7998X_REG((pg), 0x92)
+#define ISL7998X_REG_PX_ACA_HI_UPGAIN_CTL(pg)	ISL7998X_REG((pg), 0x93)
+#define ISL7998X_REG_PX_ACA_HI_DNGAIN_CTL(pg)	ISL7998X_REG((pg), 0x94)
+#define ISL7998X_REG_PX_ACA_LOPASS_FLT_COEF(pg)	ISL7998X_REG((pg), 0x95)
+#define ISL7998X_REG_PX_ACA_PDF_INDEX(pg)	ISL7998X_REG((pg), 0x96)
+#define ISL7998X_REG_PX_ACA_HIST_WIN_H_STT(pg)	ISL7998X_REG((pg), 0x97)
+#define ISL7998X_REG_PX_ACA_HIST_WIN_H_SZ1(pg)	ISL7998X_REG((pg), 0x98)
+#define ISL7998X_REG_PX_ACA_HIST_WIN_H_SZ2(pg)	ISL7998X_REG((pg), 0x99)
+#define ISL7998X_REG_PX_ACA_HIST_WIN_V_STT(pg)	ISL7998X_REG((pg), 0x9a)
+#define ISL7998X_REG_PX_ACA_HIST_WIN_V_SZ1(pg)	ISL7998X_REG((pg), 0x9b)
+#define ISL7998X_REG_PX_ACA_HIST_WIN_V_SZ2(pg)	ISL7998X_REG((pg), 0x9c)
+#define ISL7998X_REG_PX_ACA_Y_AVG(pg)		ISL7998X_REG((pg), 0xa0)
+#define ISL7998X_REG_PX_ACA_Y_AVG_LIM(pg)	ISL7998X_REG((pg), 0xa1)
+#define ISL7998X_REG_PX_ACA_LO_AVG(pg)		ISL7998X_REG((pg), 0xa2)
+#define ISL7998X_REG_PX_ACA_HI_AVG(pg)		ISL7998X_REG((pg), 0xa3)
+#define ISL7998X_REG_PX_ACA_Y_MAX(pg)		ISL7998X_REG((pg), 0xa4)
+#define ISL7998X_REG_PX_ACA_Y_MIN(pg)		ISL7998X_REG((pg), 0xa5)
+#define ISL7998X_REG_PX_ACA_MOFFSET(pg)		ISL7998X_REG((pg), 0xa6)
+#define ISL7998X_REG_PX_ACA_LO_GAIN(pg)		ISL7998X_REG((pg), 0xa7)
+#define ISL7998X_REG_PX_ACA_HI_GAIN(pg)		ISL7998X_REG((pg), 0xa8)
+#define ISL7998X_REG_PX_ACA_LL_SLOPE(pg)	ISL7998X_REG((pg), 0xa9)
+#define ISL7998X_REG_PX_ACA_LH_SLOPE(pg)	ISL7998X_REG((pg), 0xaa)
+#define ISL7998X_REG_PX_ACA_HL_SLOPE(pg)	ISL7998X_REG((pg), 0xab)
+#define ISL7998X_REG_PX_ACA_HH_SLOPE(pg)	ISL7998X_REG((pg), 0xac)
+#define ISL7998X_REG_PX_ACA_X_LOW(pg)		ISL7998X_REG((pg), 0xad)
+#define ISL7998X_REG_PX_ACA_X_MEAN(pg)		ISL7998X_REG((pg), 0xae)
+#define ISL7998X_REG_PX_ACA_X_HIGH(pg)		ISL7998X_REG((pg), 0xaf)
+#define ISL7998X_REG_PX_ACA_Y_LOW(pg)		ISL7998X_REG((pg), 0xb0)
+#define ISL7998X_REG_PX_ACA_Y_MEAN(pg)		ISL7998X_REG((pg), 0xb1)
+#define ISL7998X_REG_PX_ACA_Y_HIGH(pg)		ISL7998X_REG((pg), 0xb2)
+#define ISL7998X_REG_PX_ACA_CTL_2(pg)		ISL7998X_REG((pg), 0xb3)
+#define ISL7998X_REG_PX_ACA_CTL_3(pg)		ISL7998X_REG((pg), 0xb4)
+#define ISL7998X_REG_PX_ACA_CTL_4(pg)		ISL7998X_REG((pg), 0xb5)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_HIST(pg)	ISL7998X_REG((pg), 0xc0)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_X_TL_H(pg)	ISL7998X_REG((pg), 0xc1)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_X_TL_L(pg)	ISL7998X_REG((pg), 0xc2)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_Y_TL_H(pg)	ISL7998X_REG((pg), 0xc3)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_Y_TL_L(pg)	ISL7998X_REG((pg), 0xc4)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_X_TR_H(pg)	ISL7998X_REG((pg), 0xc5)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_X_TR_L(pg)	ISL7998X_REG((pg), 0xc6)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_Y_TR_H(pg)	ISL7998X_REG((pg), 0xc7)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_Y_TR_L(pg)	ISL7998X_REG((pg), 0xc8)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_X_BL_H(pg)	ISL7998X_REG((pg), 0xc9)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_X_BL_L(pg)	ISL7998X_REG((pg), 0xca)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_Y_BL_H(pg)	ISL7998X_REG((pg), 0xcb)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_Y_BL_L(pg)	ISL7998X_REG((pg), 0xcc)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_X_BR_H(pg)	ISL7998X_REG((pg), 0xcd)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_X_BR_L(pg)	ISL7998X_REG((pg), 0xce)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_Y_BR_H(pg)	ISL7998X_REG((pg), 0xcf)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_Y_BR_L(pg)	ISL7998X_REG((pg), 0xd0)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_X_LM_H(pg)	ISL7998X_REG((pg), 0xd1)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_X_LM_L(pg)	ISL7998X_REG((pg), 0xd2)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_Y_LM_H(pg)	ISL7998X_REG((pg), 0xd3)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_Y_LM_L(pg)	ISL7998X_REG((pg), 0xd4)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_X_TM_H(pg)	ISL7998X_REG((pg), 0xd5)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_X_TM_L(pg)	ISL7998X_REG((pg), 0xd6)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_Y_TM_H(pg)	ISL7998X_REG((pg), 0xd7)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_Y_TM_L(pg)	ISL7998X_REG((pg), 0xd8)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_X_BM_H(pg)	ISL7998X_REG((pg), 0xd9)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_X_BM_L(pg)	ISL7998X_REG((pg), 0xda)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_Y_BM_H(pg)	ISL7998X_REG((pg), 0xdb)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_Y_BM_L(pg)	ISL7998X_REG((pg), 0xdc)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_X_RM_H(pg)	ISL7998X_REG((pg), 0xdd)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_X_RM_L(pg)	ISL7998X_REG((pg), 0xde)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_Y_RM_H(pg)	ISL7998X_REG((pg), 0xdf)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_Y_RM_L(pg)	ISL7998X_REG((pg), 0xe0)
+#define ISL7998X_REG_PX_ACA_HIST_DATA_LO(pg)	ISL7998X_REG((pg), 0xe1)
+#define ISL7998X_REG_PX_ACA_HIST_DATA_MID(pg)	ISL7998X_REG((pg), 0xe2)
+#define ISL7998X_REG_PX_ACA_HIST_DATA_HI(pg)	ISL7998X_REG((pg), 0xe3)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_Y_CLR(pg)	ISL7998X_REG((pg), 0xe4)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_CB_CLR(pg)	ISL7998X_REG((pg), 0xe5)
+#define ISL7998X_REG_PX_ACA_FLEX_WIN_CR_CLR(pg)	ISL7998X_REG((pg), 0xe6)
+#define ISL7998X_REG_PX_ACA_XFER_HIST_HOST(pg)	ISL7998X_REG((pg), 0xe7)
+
+#define ISL7998X_REG_P5_LI_ENGINE_CTL		ISL7998X_REG(5, 0x00)
+#define ISL7998X_REG_P5_LI_ENGINE_LINE_CTL	ISL7998X_REG(5, 0x01)
+#define ISL7998X_REG_P5_LI_ENGINE_PIC_WIDTH	ISL7998X_REG(5, 0x02)
+#define ISL7998X_REG_P5_LI_ENGINE_SYNC_CTL	ISL7998X_REG(5, 0x03)
+#define ISL7998X_REG_P5_LI_ENGINE_VC_ASSIGNMENT	ISL7998X_REG(5, 0x04)
+#define ISL7998X_REG_P5_LI_ENGINE_TYPE_CTL	ISL7998X_REG(5, 0x05)
+#define ISL7998X_REG_P5_LI_ENGINE_FIFO_CTL	ISL7998X_REG(5, 0x06)
+#define ISL7998X_REG_P5_MIPI_READ_START_CTL	ISL7998X_REG(5, 0x07)
+#define ISL7998X_REG_P5_PSEUDO_FRM_FIELD_CTL	ISL7998X_REG(5, 0x08)
+#define ISL7998X_REG_P5_ONE_FIELD_MODE_CTL	ISL7998X_REG(5, 0x09)
+#define ISL7998X_REG_P5_MIPI_INT_HW_TST_CTR	ISL7998X_REG(5, 0x0a)
+#define ISL7998X_REG_P5_TP_GEN_BAR_PATTERN	ISL7998X_REG(5, 0x0b)
+#define ISL7998X_REG_P5_MIPI_PCNT_PSFRM		ISL7998X_REG(5, 0x0c)
+#define ISL7998X_REG_P5_LI_ENGINE_TP_GEN_CTL	ISL7998X_REG(5, 0x0d)
+#define ISL7998X_REG_P5_MIPI_VBLANK_PSFRM	ISL7998X_REG(5, 0x0e)
+#define ISL7998X_REG_P5_LI_ENGINE_CTL_2		ISL7998X_REG(5, 0x0f)
+#define ISL7998X_REG_P5_MIPI_WCNT_1		ISL7998X_REG(5, 0x10)
+#define ISL7998X_REG_P5_MIPI_WCNT_2		ISL7998X_REG(5, 0x11)
+#define ISL7998X_REG_P5_MIPI_DPHY_TIMING_CTL_1	ISL7998X_REG(5, 0x12)
+#define ISL7998X_REG_P5_MIPI_DPHY_TIMING_CTL_2	ISL7998X_REG(5, 0x13)
+#define ISL7998X_REG_P5_MIPI_DPHY_TIMING_CTL_3	ISL7998X_REG(5, 0x14)
+#define ISL7998X_REG_P5_MIPI_DPHY_TIMING_CTL_4	ISL7998X_REG(5, 0x15)
+#define ISL7998X_REG_P5_MIPI_DPHY_TIMING_CTL_5	ISL7998X_REG(5, 0x16)
+#define ISL7998X_REG_P5_MIPI_DPHY_TIMING_CTL_6	ISL7998X_REG(5, 0x17)
+#define ISL7998X_REG_P5_MIPI_DPHY_PARAMS_1	ISL7998X_REG(5, 0x18)
+#define ISL7998X_REG_P5_MIPI_DPHY_SOT_PERIOD	ISL7998X_REG(5, 0x19)
+#define ISL7998X_REG_P5_MIPI_DPHY_EOT_PERIOD	ISL7998X_REG(5, 0x1a)
+#define ISL7998X_REG_P5_MIPI_DPHY_PARAMS_2	ISL7998X_REG(5, 0x1b)
+#define ISL7998X_REG_P5_MIPI_DPHY_TIMING_CTL_7	ISL7998X_REG(5, 0x1c)
+#define ISL7998X_REG_P5_MIPI_DPHY_TIMING_CTL_8	ISL7998X_REG(5, 0x1d)
+#define ISL7998X_REG_P5_MIPI_DPHY_TIMING_CTL_9	ISL7998X_REG(5, 0x1e)
+#define ISL7998X_REG_P5_MIPI_DPHY_TIMING_CTL_10	ISL7998X_REG(5, 0x1f)
+#define ISL7998X_REG_P5_TP_GEN_MIPI		ISL7998X_REG(5, 0x20)
+#define ISL7998X_REG_P5_ESC_MODE_TIME_CTL	ISL7998X_REG(5, 0x21)
+#define ISL7998X_REG_P5_AUTO_TEST_ERR_DET	ISL7998X_REG(5, 0x22)
+#define ISL7998X_REG_P5_MIPI_TIMING		ISL7998X_REG(5, 0x23)
+#define ISL7998X_REG_P5_PIC_HEIGHT_HIGH		ISL7998X_REG(5, 0x24)
+#define ISL7998X_REG_P5_PIC_HEIGHT_LOW		ISL7998X_REG(5, 0x25)
+#define ISL7998X_REG_P5_MIPI_SP_HS_TRL_CTL	ISL7998X_REG(5, 0x26)
+#define ISL7998X_REG_P5_FIFO_THRSH_CNT_1	ISL7998X_REG(5, 0x28)
+#define ISL7998X_REG_P5_FIFO_THRSH_CNT_2	ISL7998X_REG(5, 0x29)
+#define ISL7998X_REG_P5_TP_GEN_RND_SYNC_CTL_1	ISL7998X_REG(5, 0x2a)
+#define ISL7998X_REG_P5_TP_GEN_RND_SYNC_CTL_2	ISL7998X_REG(5, 0x2b)
+#define ISL7998X_REG_P5_PSF_FIELD_END_CTL_1	ISL7998X_REG(5, 0x2c)
+#define ISL7998X_REG_P5_PSF_FIELD_END_CTL_2	ISL7998X_REG(5, 0x2d)
+#define ISL7998X_REG_P5_PSF_FIELD_END_CTL_3	ISL7998X_REG(5, 0x2e)
+#define ISL7998X_REG_P5_PSF_FIELD_END_CTL_4	ISL7998X_REG(5, 0x2f)
+#define ISL7998X_REG_P5_MIPI_ANA_DATA_CTL_1	ISL7998X_REG(5, 0x30)
+#define ISL7998X_REG_P5_MIPI_ANA_DATA_CTL_2	ISL7998X_REG(5, 0x31)
+#define ISL7998X_REG_P5_MIPI_ANA_CLK_CTL	ISL7998X_REG(5, 0x32)
+#define ISL7998X_REG_P5_PLL_ANA_STATUS		ISL7998X_REG(5, 0x33)
+#define ISL7998X_REG_P5_PLL_ANA_MISC_CTL	ISL7998X_REG(5, 0x34)
+#define ISL7998X_REG_P5_MIPI_ANA		ISL7998X_REG(5, 0x35)
+#define ISL7998X_REG_P5_PLL_ANA			ISL7998X_REG(5, 0x36)
+#define ISL7998X_REG_P5_TOTAL_PF_LINE_CNT_1	ISL7998X_REG(5, 0x38)
+#define ISL7998X_REG_P5_TOTAL_PF_LINE_CNT_2	ISL7998X_REG(5, 0x39)
+#define ISL7998X_REG_P5_H_LINE_CNT_1		ISL7998X_REG(5, 0x3a)
+#define ISL7998X_REG_P5_H_LINE_CNT_2		ISL7998X_REG(5, 0x3b)
+#define ISL7998X_REG_P5_HIST_LINE_CNT_1		ISL7998X_REG(5, 0x3c)
+#define ISL7998X_REG_P5_HIST_LINE_CNT_2		ISL7998X_REG(5, 0x3d)
+
+static const struct reg_sequence isl7998x_init_seq_1[] = {
+	{ ISL7998X_REG_P0_SHORT_DIAG_IRQ_EN, 0xff },
+	{ ISL7998X_REG_PX_DEC_SDT(0x1), 0x00 },
+	{ ISL7998X_REG_PX_DEC_SHORT_DET_CTL_1(0x1), 0x03 },
+	{ ISL7998X_REG_PX_DEC_SDT(0x2), 0x00 },
+	{ ISL7998X_REG_PX_DEC_SHORT_DET_CTL_1(0x2), 0x03 },
+	{ ISL7998X_REG_PX_DEC_SDT(0x3), 0x00 },
+	{ ISL7998X_REG_PX_DEC_SHORT_DET_CTL_1(0x3), 0x03 },
+	{ ISL7998X_REG_PX_DEC_SDT(0x4), 0x00 },
+	{ ISL7998X_REG_PX_DEC_SHORT_DET_CTL_1(0x4), 0x03 },
+	{ ISL7998X_REG_P5_LI_ENGINE_CTL, 0x00 },
+	{ ISL7998X_REG_P0_SW_RESET_CTL, 0x1f, 10 },
+	{ ISL7998X_REG_P0_IO_BUFFER_CTL, 0x00 },
+	{ ISL7998X_REG_P0_MPP2_SYNC_CTL, 0xc9 },
+	{ ISL7998X_REG_P0_IRQ_SYNC_CTL, 0xc9 },
+	{ ISL7998X_REG_P0_CHAN_1_IRQ, 0x03 },
+	{ ISL7998X_REG_P0_CHAN_2_IRQ, 0x00 },
+	{ ISL7998X_REG_P0_CHAN_3_IRQ, 0x00 },
+	{ ISL7998X_REG_P0_CHAN_4_IRQ, 0x00 },
+	{ ISL7998X_REG_P5_LI_ENGINE_CTL, 0x02 },
+	{ ISL7998X_REG_P5_LI_ENGINE_LINE_CTL, 0x85 },
+	{ ISL7998X_REG_P5_LI_ENGINE_PIC_WIDTH, 0xa0 },
+	{ ISL7998X_REG_P5_LI_ENGINE_SYNC_CTL, 0x18 },
+	{ ISL7998X_REG_P5_LI_ENGINE_TYPE_CTL, 0x40 },
+	{ ISL7998X_REG_P5_LI_ENGINE_FIFO_CTL, 0x40 },
+	{ ISL7998X_REG_P5_MIPI_WCNT_1, 0x05 },
+	{ ISL7998X_REG_P5_MIPI_WCNT_2, 0xa0 },
+	{ ISL7998X_REG_P5_TP_GEN_MIPI, 0x00 },
+	{ ISL7998X_REG_P5_ESC_MODE_TIME_CTL, 0x0c },
+	{ ISL7998X_REG_P5_MIPI_SP_HS_TRL_CTL, 0x00 },
+	{ ISL7998X_REG_P5_TP_GEN_RND_SYNC_CTL_1, 0x00 },
+	{ ISL7998X_REG_P5_TP_GEN_RND_SYNC_CTL_2, 0x19 },
+	{ ISL7998X_REG_P5_PSF_FIELD_END_CTL_1, 0x18 },
+	{ ISL7998X_REG_P5_PSF_FIELD_END_CTL_2, 0xf1 },
+	{ ISL7998X_REG_P5_PSF_FIELD_END_CTL_3, 0x00 },
+	{ ISL7998X_REG_P5_PSF_FIELD_END_CTL_4, 0xf1 },
+	{ ISL7998X_REG_P5_MIPI_ANA_DATA_CTL_1, 0x00 },
+	{ ISL7998X_REG_P5_MIPI_ANA_DATA_CTL_2, 0x00 },
+	{ ISL7998X_REG_P5_MIPI_ANA_CLK_CTL, 0x00 },
+	{ ISL7998X_REG_P5_PLL_ANA_STATUS, 0xc0 },
+	{ ISL7998X_REG_P5_PLL_ANA_MISC_CTL, 0x18 },
+	{ ISL7998X_REG_P5_PLL_ANA, 0x00 },
+	{ ISL7998X_REG_P0_SW_RESET_CTL, 0x10, 10 },
+	/* Page 0xf means write to all of pages 1,2,3,4 */
+	{ ISL7998X_REG_PX_DEC_VDELAY_LO(0xf), 0x14 },
+	{ ISL7998X_REG_PX_DEC_MISC3(0xf), 0xe6 },
+	{ ISL7998X_REG_PX_DEC_CLMD(0xf), 0x85 },
+	{ ISL7998X_REG_PX_DEC_H_DELAY_II_LOW(0xf), 0x11 },
+	{ ISL7998X_REG_PX_ACA_XFER_HIST_HOST(0xf), 0x00 },
+	{ ISL7998X_REG_P0_CLK_CTL_1, 0x1f },
+	{ ISL7998X_REG_P0_CLK_CTL_2, 0x43 },
+	{ ISL7998X_REG_P0_CLK_CTL_3, 0x4f },
+};
+
+static const struct reg_sequence isl7998x_init_seq_2[] = {
+	{ ISL7998X_REG_P5_LI_ENGINE_SYNC_CTL, 0x10 },
+	{ ISL7998X_REG_P5_LI_ENGINE_VC_ASSIGNMENT, 0xe4 },
+	{ ISL7998X_REG_P5_LI_ENGINE_TYPE_CTL, 0x00 },
+	{ ISL7998X_REG_P5_LI_ENGINE_FIFO_CTL, 0x60 },
+	{ ISL7998X_REG_P5_MIPI_READ_START_CTL, 0x2b },
+	{ ISL7998X_REG_P5_PSEUDO_FRM_FIELD_CTL, 0x02 },
+	{ ISL7998X_REG_P5_ONE_FIELD_MODE_CTL, 0x00 },
+	{ ISL7998X_REG_P5_MIPI_INT_HW_TST_CTR, 0x62 },
+	{ ISL7998X_REG_P5_TP_GEN_BAR_PATTERN, 0x02 },
+	{ ISL7998X_REG_P5_MIPI_PCNT_PSFRM, 0x36 },
+	{ ISL7998X_REG_P5_LI_ENGINE_TP_GEN_CTL, 0x00 },
+	{ ISL7998X_REG_P5_MIPI_VBLANK_PSFRM, 0x6c },
+	{ ISL7998X_REG_P5_LI_ENGINE_CTL_2, 0x00 },
+	{ ISL7998X_REG_P5_MIPI_WCNT_1, 0x05 },
+	{ ISL7998X_REG_P5_MIPI_WCNT_2, 0xa0 },
+	{ ISL7998X_REG_P5_MIPI_DPHY_TIMING_CTL_1, 0x77 },
+	{ ISL7998X_REG_P5_MIPI_DPHY_TIMING_CTL_2, 0x17 },
+	{ ISL7998X_REG_P5_MIPI_DPHY_TIMING_CTL_3, 0x08 },
+	{ ISL7998X_REG_P5_MIPI_DPHY_TIMING_CTL_4, 0x38 },
+	{ ISL7998X_REG_P5_MIPI_DPHY_TIMING_CTL_5, 0x14 },
+	{ ISL7998X_REG_P5_MIPI_DPHY_TIMING_CTL_6, 0xf6 },
+	{ ISL7998X_REG_P5_MIPI_DPHY_PARAMS_1, 0x00 },
+	{ ISL7998X_REG_P5_MIPI_DPHY_SOT_PERIOD, 0x17 },
+	{ ISL7998X_REG_P5_MIPI_DPHY_EOT_PERIOD, 0x0a },
+	{ ISL7998X_REG_P5_MIPI_DPHY_PARAMS_2, 0x71 },
+	{ ISL7998X_REG_P5_MIPI_DPHY_TIMING_CTL_7, 0x7a },
+	{ ISL7998X_REG_P5_MIPI_DPHY_TIMING_CTL_8, 0x0f },
+	{ ISL7998X_REG_P5_MIPI_DPHY_TIMING_CTL_9, 0x8c },
+	{ ISL7998X_REG_P5_MIPI_SP_HS_TRL_CTL, 0x08 },
+	{ ISL7998X_REG_P5_FIFO_THRSH_CNT_1, 0x01 },
+	{ ISL7998X_REG_P5_FIFO_THRSH_CNT_2, 0x0e },
+	{ ISL7998X_REG_P5_TP_GEN_RND_SYNC_CTL_1, 0x00 },
+	{ ISL7998X_REG_P5_TP_GEN_RND_SYNC_CTL_2, 0x00 },
+	{ ISL7998X_REG_P5_TOTAL_PF_LINE_CNT_1, 0x03 },
+	{ ISL7998X_REG_P5_TOTAL_PF_LINE_CNT_2, 0xc0 },
+	{ ISL7998X_REG_P5_H_LINE_CNT_1, 0x06 },
+	{ ISL7998X_REG_P5_H_LINE_CNT_2, 0xb3 },
+	{ ISL7998X_REG_P5_HIST_LINE_CNT_1, 0x00 },
+	{ ISL7998X_REG_P5_HIST_LINE_CNT_2, 0xf1 },
+	{ ISL7998X_REG_P5_LI_ENGINE_FIFO_CTL, 0x00 },
+	{ ISL7998X_REG_P5_MIPI_ANA, 0x00 },
+	/*
+	 * Wait a bit after reset so that the chip can capture a frame
+	 * and update internal line counters.
+	 */
+	{ ISL7998X_REG_P0_SW_RESET_CTL, 0x00, 50 },
+};
+
+enum isl7998x_pads {
+	ISL7998X_PAD_OUT,
+	ISL7998X_PAD_VIN1,
+	ISL7998X_PAD_VIN2,
+	ISL7998X_PAD_VIN3,
+	ISL7998X_PAD_VIN4,
+	ISL7998X_NUM_PADS
+};
+
+struct isl7998x_datafmt {
+	u32			code;
+	enum v4l2_colorspace	colorspace;
+};
+
+static const struct isl7998x_datafmt isl7998x_colour_fmts[] = {
+	{ MEDIA_BUS_FMT_UYVY8_2X8, V4L2_COLORSPACE_SRGB },
+};
+
+/* Menu items for LINK_FREQ V4L2 control */
+static const s64 link_freq_menu_items[] = {
+	/* 1 channel, 1 lane or 2 channels, 2 lanes */
+	108000000,
+	/* 2 channels, 1 lane or 4 channels, 2 lanes */
+	216000000,
+	/* 4 channels, 1 lane */
+	432000000,
+};
+
+/* Menu items for TEST_PATTERN V4L2 control */
+static const char * const isl7998x_test_pattern_menu[] = {
+	"Disabled",
+	"Enabled",
+};
+
+static const char * const isl7998x_test_pattern_bars[] = {
+	"bbbbwb", "bbbwwb", "bbwbwb", "bbwwwb",
+};
+
+static const char * const isl7998x_test_pattern_colors[] = {
+	"Yellow", "Blue", "Green", "Pink",
+};
+
+struct isl7998x_mode {
+	unsigned int width;
+	unsigned int height;
+	enum v4l2_field field;
+};
+
+static const struct isl7998x_mode supported_modes[] = {
+	{
+		.width = 720,
+		.height = 576,
+		.field = V4L2_FIELD_SEQ_TB,
+	},
+	{
+		.width = 720,
+		.height = 480,
+		.field = V4L2_FIELD_SEQ_BT,
+	},
+};
+
+static const struct isl7998x_video_std {
+	const v4l2_std_id norm;
+	unsigned int id;
+	const struct isl7998x_mode *mode;
+} isl7998x_std_res[] = {
+	{ V4L2_STD_NTSC_443,
+	  ISL7998X_REG_PX_DEC_SDT_STANDARD_NTSC_443,
+	  &supported_modes[1] },
+	{ V4L2_STD_PAL_M,
+	  ISL7998X_REG_PX_DEC_SDT_STANDARD_PAL_M,
+	  &supported_modes[1] },
+	{ V4L2_STD_PAL_Nc,
+	  ISL7998X_REG_PX_DEC_SDT_STANDARD_PAL_CN,
+	  &supported_modes[0] },
+	{ V4L2_STD_PAL_N,
+	  ISL7998X_REG_PX_DEC_SDT_STANDARD_PAL,
+	  &supported_modes[0] },
+	{ V4L2_STD_PAL_60,
+	  ISL7998X_REG_PX_DEC_SDT_STANDARD_PAL_60,
+	  &supported_modes[1] },
+	{ V4L2_STD_NTSC,
+	  ISL7998X_REG_PX_DEC_SDT_STANDARD_NTSC_M,
+	  &supported_modes[1] },
+	{ V4L2_STD_PAL,
+	  ISL7998X_REG_PX_DEC_SDT_STANDARD_PAL,
+	  &supported_modes[0] },
+	{ V4L2_STD_SECAM,
+	  ISL7998X_REG_PX_DEC_SDT_STANDARD_SECAM,
+	  &supported_modes[0] },
+	{ V4L2_STD_UNKNOWN,
+	  ISL7998X_REG_PX_DEC_SDT_STANDARD_UNKNOWN,
+	  &supported_modes[1] },
+};
+
+struct isl7998x {
+	struct v4l2_subdev		subdev;
+	struct regmap			*regmap;
+	struct gpio_desc		*pd_gpio;
+	struct gpio_desc		*rstb_gpio;
+	unsigned int			nr_mipi_lanes;
+	u32				nr_inputs;
+
+	const struct isl7998x_datafmt	*fmt;
+	v4l2_std_id			norm;
+	struct media_pad		pads[ISL7998X_NUM_PADS];
+
+	int				enabled;
+
+	/* protect fmt, norm, enabled */
+	struct mutex			lock;
+
+	struct v4l2_ctrl_handler	ctrl_handler;
+	/* protect ctrl_handler */
+	struct mutex			ctrl_mutex;
+
+	/* V4L2 Controls */
+	struct v4l2_ctrl		*link_freq;
+	u8				test_pattern;
+	u8				test_pattern_bars;
+	u8				test_pattern_chans;
+	u8				test_pattern_color;
+};
+
+static struct isl7998x *sd_to_isl7998x(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct isl7998x, subdev);
+}
+
+static struct isl7998x *i2c_to_isl7998x(const struct i2c_client *client)
+{
+	return sd_to_isl7998x(i2c_get_clientdata(client));
+}
+
+static unsigned int isl7998x_norm_to_val(v4l2_std_id norm)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(isl7998x_std_res); i++)
+		if (isl7998x_std_res[i].norm & norm)
+			break;
+	if (i == ARRAY_SIZE(isl7998x_std_res))
+		return ISL7998X_REG_PX_DEC_SDT_STANDARD_UNKNOWN;
+
+	return isl7998x_std_res[i].id;
+}
+
+static const struct isl7998x_mode *isl7998x_norm_to_mode(v4l2_std_id norm)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(isl7998x_std_res); i++)
+		if (isl7998x_std_res[i].norm & norm)
+			break;
+	/* Use NTSC default resolution during standard detection */
+	if (i == ARRAY_SIZE(isl7998x_std_res))
+		return &supported_modes[1];
+
+	return isl7998x_std_res[i].mode;
+}
+
+static int isl7998x_get_nr_inputs(struct device_node *of_node)
+{
+	struct device_node *port;
+	unsigned int inputs = 0;
+	unsigned int i;
+
+	if (of_graph_get_endpoint_count(of_node) > ISL7998X_NUM_PADS)
+		return -EINVAL;
+
+	/*
+	 * The driver does not provide means to remap the input ports. It
+	 * always configures input ports to start from VID1. Ensure that the
+	 * device tree is correct.
+	 */
+	for (i = ISL7998X_PAD_VIN1; i <= ISL7998X_PAD_VIN4; i++) {
+		port = of_graph_get_port_by_id(of_node, i);
+		if (!port)
+			continue;
+
+		inputs |= BIT(i);
+		of_node_put(port);
+	}
+
+	switch (inputs) {
+	case BIT(ISL7998X_PAD_VIN1):
+		return 1;
+	case BIT(ISL7998X_PAD_VIN1) | BIT(ISL7998X_PAD_VIN2):
+		return 2;
+	case BIT(ISL7998X_PAD_VIN1) | BIT(ISL7998X_PAD_VIN2) |
+	     BIT(ISL7998X_PAD_VIN3) | BIT(ISL7998X_PAD_VIN4):
+		return 4;
+	default:
+		return -EINVAL;
+	}
+}
+
+static int isl7998x_wait_power_on(struct isl7998x *isl7998x)
+{
+	struct device *dev = isl7998x->subdev.dev;
+	u32 chip_id;
+	int ret;
+	int err;
+
+	ret = read_poll_timeout(regmap_read, err, !err, 2000, 20000, false,
+				isl7998x->regmap,
+				ISL7998X_REG_P0_PRODUCT_ID_CODE, &chip_id);
+	if (ret) {
+		dev_err(dev, "timeout while waiting for ISL7998X\n");
+		return ret;
+	}
+
+	dev_dbg(dev, "Found ISL799%x\n", chip_id);
+
+	return ret;
+}
+
+static int isl7998x_set_standard(struct isl7998x *isl7998x, v4l2_std_id norm)
+{
+	const struct isl7998x_mode *mode = isl7998x_norm_to_mode(norm);
+	unsigned int val = isl7998x_norm_to_val(norm);
+	unsigned int width = mode->width;
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < ISL7998X_INPUTS; i++) {
+		ret = regmap_write_bits(isl7998x->regmap,
+					ISL7998X_REG_PX_DEC_SDT(i + 1),
+					ISL7998X_REG_PX_DEC_SDT_STANDARD,
+					val);
+		if (ret)
+			return ret;
+	}
+
+	ret = regmap_write(isl7998x->regmap,
+			   ISL7998X_REG_P5_LI_ENGINE_LINE_CTL,
+			   0x20 | ((width >> 7) & 0x1f));
+	if (ret)
+		return ret;
+
+	ret = regmap_write(isl7998x->regmap,
+			   ISL7998X_REG_P5_LI_ENGINE_PIC_WIDTH,
+			   (width << 1) & 0xff);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int isl7998x_init(struct isl7998x *isl7998x)
+{
+	const unsigned int lanes = isl7998x->nr_mipi_lanes;
+	static const u32 isl7998x_video_in_chan_map[] = { 0x00, 0x11, 0x02, 0x02 };
+	const struct reg_sequence isl7998x_init_seq_custom[] = {
+		{ ISL7998X_REG_P0_VIDEO_IN_CHAN_CTL,
+		  isl7998x_video_in_chan_map[isl7998x->nr_inputs - 1] },
+		{ ISL7998X_REG_P0_CLK_CTL_4,
+		  (lanes == 1) ? 0x40 : 0x41 },
+		{ ISL7998X_REG_P5_LI_ENGINE_CTL,
+		  (lanes == 1) ? 0x01 : 0x02 },
+	};
+	struct device *dev = isl7998x->subdev.dev;
+	struct regmap *regmap = isl7998x->regmap;
+	int ret;
+
+	dev_dbg(dev, "configuring %d lanes for %d inputs (norm %s)\n",
+		isl7998x->nr_mipi_lanes, isl7998x->nr_inputs,
+		v4l2_norm_to_name(isl7998x->norm));
+
+	ret = regmap_register_patch(regmap, isl7998x_init_seq_1,
+				    ARRAY_SIZE(isl7998x_init_seq_1));
+	if (ret)
+		return ret;
+
+	mutex_lock(&isl7998x->lock);
+	ret = isl7998x_set_standard(isl7998x, isl7998x->norm);
+	mutex_unlock(&isl7998x->lock);
+	if (ret)
+		return ret;
+
+	ret = regmap_register_patch(regmap, isl7998x_init_seq_custom,
+				    ARRAY_SIZE(isl7998x_init_seq_custom));
+	if (ret)
+		return ret;
+
+	return regmap_register_patch(regmap, isl7998x_init_seq_2,
+				     ARRAY_SIZE(isl7998x_init_seq_2));
+}
+
+static int isl7998x_set_test_pattern(struct isl7998x *isl7998x)
+{
+	const struct reg_sequence isl7998x_init_seq_tpg_off[] = {
+		{ ISL7998X_REG_P5_LI_ENGINE_TP_GEN_CTL, 0 },
+		{ ISL7998X_REG_P5_LI_ENGINE_CTL_2, 0 }
+	};
+	const struct reg_sequence isl7998x_init_seq_tpg_on[] = {
+		{ ISL7998X_REG_P5_TP_GEN_BAR_PATTERN,
+		  isl7998x->test_pattern_bars << 6 },
+		{ ISL7998X_REG_P5_LI_ENGINE_CTL_2,
+		  isl7998x->norm & V4L2_STD_PAL ? BIT(2) : 0 },
+		{ ISL7998X_REG_P5_LI_ENGINE_TP_GEN_CTL,
+		  (isl7998x->test_pattern_chans << 4) |
+		  (isl7998x->test_pattern_color << 2) }
+	};
+	struct device *dev = isl7998x->subdev.dev;
+	struct regmap *regmap = isl7998x->regmap;
+	int ret;
+
+	if (pm_runtime_get_if_in_use(dev) <= 0)
+		return 0;
+
+	if (isl7998x->test_pattern != 0) {
+		dev_dbg(dev, "enabling test pattern: channels 0x%x, %s, %s\n",
+			isl7998x->test_pattern_chans,
+			isl7998x_test_pattern_bars[isl7998x->test_pattern_bars],
+			isl7998x_test_pattern_colors[isl7998x->test_pattern_color]);
+		ret = regmap_register_patch(regmap, isl7998x_init_seq_tpg_on,
+					    ARRAY_SIZE(isl7998x_init_seq_tpg_on));
+	} else {
+		ret = regmap_register_patch(regmap, isl7998x_init_seq_tpg_off,
+					    ARRAY_SIZE(isl7998x_init_seq_tpg_off));
+	}
+
+	pm_runtime_put(dev);
+
+	return ret;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int isl7998x_g_register(struct v4l2_subdev *sd,
+			       struct v4l2_dbg_register *reg)
+{
+	struct isl7998x *isl7998x = sd_to_isl7998x(sd);
+	int ret;
+	u32 val;
+
+	ret = regmap_read(isl7998x->regmap, reg->reg, &val);
+	if (ret)
+		return ret;
+
+	reg->size = 1;
+	reg->val = val;
+
+	return 0;
+}
+
+static int isl7998x_s_register(struct v4l2_subdev *sd,
+			       const struct v4l2_dbg_register *reg)
+{
+	struct isl7998x *isl7998x = sd_to_isl7998x(sd);
+
+	return regmap_write(isl7998x->regmap, reg->reg, reg->val);
+}
+#endif
+
+static int isl7998x_g_std(struct v4l2_subdev *sd, v4l2_std_id *norm)
+{
+	struct isl7998x *isl7998x = sd_to_isl7998x(sd);
+
+	mutex_lock(&isl7998x->lock);
+	*norm = isl7998x->norm;
+	mutex_unlock(&isl7998x->lock);
+
+	return 0;
+}
+
+static int isl7998x_s_std(struct v4l2_subdev *sd, v4l2_std_id norm)
+{
+	struct isl7998x *isl7998x = sd_to_isl7998x(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct device *dev = &client->dev;
+	int ret = 0;
+
+	mutex_lock(&isl7998x->lock);
+	if (isl7998x->enabled) {
+		ret = -EBUSY;
+		mutex_unlock(&isl7998x->lock);
+		return ret;
+	}
+	isl7998x->norm = norm;
+	mutex_unlock(&isl7998x->lock);
+
+	if (pm_runtime_get_if_in_use(dev) <= 0)
+		return ret;
+
+	ret = isl7998x_set_standard(isl7998x, norm);
+
+	pm_runtime_put(dev);
+
+	return ret;
+}
+
+static int isl7998x_querystd(struct v4l2_subdev *sd, v4l2_std_id *std)
+{
+	struct isl7998x *isl7998x = sd_to_isl7998x(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct device *dev = &client->dev;
+	unsigned int std_id[ISL7998X_INPUTS];
+	unsigned int i;
+	int ret;
+	u32 reg;
+
+	ret = pm_runtime_resume_and_get(dev);
+	if (ret)
+		return ret;
+
+	dev_dbg(dev, "starting video standard detection\n");
+
+	mutex_lock(&isl7998x->lock);
+	if (isl7998x->enabled) {
+		ret = -EBUSY;
+		goto out_unlock;
+	}
+
+	ret = isl7998x_set_standard(isl7998x, V4L2_STD_UNKNOWN);
+	if (ret)
+		goto out_unlock;
+
+	for (i = 0; i < ISL7998X_INPUTS; i++) {
+		ret = regmap_write(isl7998x->regmap,
+				   ISL7998X_REG_PX_DEC_SDTR(i + 1),
+				   ISL7998X_REG_PX_DEC_SDTR_ATSTART);
+		if (ret)
+			goto out_reset_std;
+	}
+
+	for (i = 0; i < ISL7998X_INPUTS; i++) {
+		ret = regmap_read_poll_timeout(isl7998x->regmap,
+					       ISL7998X_REG_PX_DEC_SDT(i + 1),
+					       reg,
+					       !(reg & ISL7998X_REG_PX_DEC_SDT_DET),
+					       2000, 500 * USEC_PER_MSEC);
+		if (ret)
+			goto out_reset_std;
+		std_id[i] = FIELD_GET(ISL7998X_REG_PX_DEC_SDT_NOW, reg);
+	}
+
+	/*
+	 * According to Renesas FAE, all input cameras must have the
+	 * same standard on this chip.
+	 */
+	for (i = 0; i < isl7998x->nr_inputs; i++) {
+		dev_dbg(dev, "input %d: detected %s\n",
+			i, v4l2_norm_to_name(isl7998x_std_res[std_id[i]].norm));
+		if (std_id[0] != std_id[i])
+			dev_warn(dev,
+				 "incompatible standards: %s on input %d (expected %s)\n",
+				 v4l2_norm_to_name(isl7998x_std_res[std_id[i]].norm), i,
+				 v4l2_norm_to_name(isl7998x_std_res[std_id[0]].norm));
+	}
+
+	*std = isl7998x_std_res[std_id[0]].norm;
+
+out_reset_std:
+	isl7998x_set_standard(isl7998x, isl7998x->norm);
+out_unlock:
+	mutex_unlock(&isl7998x->lock);
+	pm_runtime_put(dev);
+
+	return ret;
+}
+
+static int isl7998x_g_tvnorms(struct v4l2_subdev *sd, v4l2_std_id *std)
+{
+	*std = V4L2_STD_ALL;
+
+	return 0;
+}
+
+static int isl7998x_g_input_status(struct v4l2_subdev *sd, u32 *status)
+{
+	struct isl7998x *isl7998x = sd_to_isl7998x(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct device *dev = &client->dev;
+	unsigned int i;
+	int ret = 0;
+	u32 reg;
+
+	if (!pm_runtime_active(dev)) {
+		*status |= V4L2_IN_ST_NO_POWER;
+		return 0;
+	}
+
+	for (i = 0; i < isl7998x->nr_inputs; i++) {
+		ret = regmap_read(isl7998x->regmap,
+				  ISL7998X_REG_PX_DEC_STATUS_1(i + 1), &reg);
+		if (!ret) {
+			if (reg & ISL7998X_REG_PX_DEC_STATUS_1_VDLOSS)
+				*status |= V4L2_IN_ST_NO_SIGNAL;
+			if (!(reg & ISL7998X_REG_PX_DEC_STATUS_1_HLOCK))
+				*status |= V4L2_IN_ST_NO_H_LOCK;
+			if (!(reg & ISL7998X_REG_PX_DEC_STATUS_1_VLOCK))
+				*status |= V4L2_IN_ST_NO_V_LOCK;
+		}
+	}
+
+	return ret;
+}
+
+static int isl7998x_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct isl7998x *isl7998x = sd_to_isl7998x(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct device *dev = &client->dev;
+	int ret = 0;
+	u32 reg;
+
+	dev_dbg(dev, "stream %s\n", enable ? "ON" : "OFF");
+
+	mutex_lock(&isl7998x->lock);
+	if (isl7998x->enabled == enable)
+		goto out;
+	isl7998x->enabled = enable;
+
+	if (enable) {
+		ret = isl7998x_set_test_pattern(isl7998x);
+		if (ret)
+			goto out;
+	}
+
+	regmap_read(isl7998x->regmap,
+		    ISL7998X_REG_P5_LI_ENGINE_CTL, &reg);
+	if (enable)
+		reg &= ~BIT(7);
+	else
+		reg |= BIT(7);
+	ret = regmap_write(isl7998x->regmap,
+			   ISL7998X_REG_P5_LI_ENGINE_CTL, reg);
+
+out:
+	mutex_unlock(&isl7998x->lock);
+
+	return ret;
+}
+
+static int isl7998x_pre_streamon(struct v4l2_subdev *sd, u32 flags)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct device *dev = &client->dev;
+
+	return pm_runtime_resume_and_get(dev);
+}
+
+static int isl7998x_post_streamoff(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct device *dev = &client->dev;
+
+	pm_runtime_put(dev);
+
+	return 0;
+}
+
+static int isl7998x_enum_mbus_code(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index >= ARRAY_SIZE(isl7998x_colour_fmts))
+		return -EINVAL;
+
+	code->code = isl7998x_colour_fmts[code->index].code;
+
+	return 0;
+}
+
+static int isl7998x_enum_frame_size(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_state *sd_state,
+				    struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->index >= ARRAY_SIZE(supported_modes))
+		return -EINVAL;
+
+	if (fse->code != isl7998x_colour_fmts[0].code)
+		return -EINVAL;
+
+	fse->min_width = supported_modes[fse->index].width;
+	fse->max_width = fse->min_width;
+	fse->min_height = supported_modes[fse->index].height;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static int isl7998x_get_fmt(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_state *sd_state,
+			    struct v4l2_subdev_format *format)
+{
+	struct isl7998x *isl7998x = sd_to_isl7998x(sd);
+	struct v4l2_mbus_framefmt *mf = &format->format;
+	const struct isl7998x_mode *mode;
+
+	mutex_lock(&isl7998x->lock);
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		format->format = *v4l2_subdev_get_try_format(sd, sd_state,
+							     format->pad);
+		goto out;
+	}
+
+	mode = isl7998x_norm_to_mode(isl7998x->norm);
+
+	mf->width	= mode->width;
+	mf->height	= mode->height;
+	mf->code	= isl7998x->fmt->code;
+	mf->field	= mode->field;
+	mf->colorspace	= 0;
+
+out:
+	mutex_unlock(&isl7998x->lock);
+
+	return 0;
+}
+
+static int isl7998x_set_fmt(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_state *sd_state,
+			    struct v4l2_subdev_format *format)
+{
+	struct isl7998x *isl7998x = sd_to_isl7998x(sd);
+	struct v4l2_mbus_framefmt *mf = &format->format;
+	const struct isl7998x_mode *mode;
+
+	mutex_lock(&isl7998x->lock);
+
+	mode = isl7998x_norm_to_mode(isl7998x->norm);
+
+	mf->width = mode->width;
+	mf->height = mode->height;
+	mf->code = isl7998x->fmt->code;
+	mf->field = mode->field;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+		*v4l2_subdev_get_try_format(sd, sd_state, format->pad) = format->format;
+
+	mutex_unlock(&isl7998x->lock);
+
+	return 0;
+}
+
+static int isl7998x_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct isl7998x *isl7998x = container_of(ctrl->handler,
+						 struct isl7998x, ctrl_handler);
+	int ret = 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_TEST_PATTERN_BARS:
+		mutex_lock(&isl7998x->lock);
+		isl7998x->test_pattern_bars = ctrl->val & 0x3;
+		ret = isl7998x_set_test_pattern(isl7998x);
+		mutex_unlock(&isl7998x->lock);
+		break;
+	case V4L2_CID_TEST_PATTERN_CHANNELS:
+		mutex_lock(&isl7998x->lock);
+		isl7998x->test_pattern_chans = ctrl->val & 0xf;
+		ret = isl7998x_set_test_pattern(isl7998x);
+		mutex_unlock(&isl7998x->lock);
+		break;
+	case V4L2_CID_TEST_PATTERN_COLOR:
+		mutex_lock(&isl7998x->lock);
+		isl7998x->test_pattern_color = ctrl->val & 0x3;
+		ret = isl7998x_set_test_pattern(isl7998x);
+		mutex_unlock(&isl7998x->lock);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		mutex_lock(&isl7998x->lock);
+		isl7998x->test_pattern = ctrl->val;
+		ret = isl7998x_set_test_pattern(isl7998x);
+		mutex_unlock(&isl7998x->lock);
+		break;
+	}
+
+	return ret;
+}
+
+static const struct v4l2_subdev_core_ops isl7998x_subdev_core_ops = {
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register	= isl7998x_g_register,
+	.s_register	= isl7998x_s_register,
+#endif
+};
+
+static const struct v4l2_subdev_video_ops isl7998x_subdev_video_ops = {
+	.g_std		= isl7998x_g_std,
+	.s_std		= isl7998x_s_std,
+	.querystd	= isl7998x_querystd,
+	.g_tvnorms	= isl7998x_g_tvnorms,
+	.g_input_status	= isl7998x_g_input_status,
+	.s_stream	= isl7998x_s_stream,
+	.pre_streamon	= isl7998x_pre_streamon,
+	.post_streamoff	= isl7998x_post_streamoff,
+};
+
+static const struct v4l2_subdev_pad_ops isl7998x_subdev_pad_ops = {
+	.enum_mbus_code		= isl7998x_enum_mbus_code,
+	.enum_frame_size	= isl7998x_enum_frame_size,
+	.get_fmt		= isl7998x_get_fmt,
+	.set_fmt		= isl7998x_set_fmt,
+};
+
+static const struct v4l2_subdev_ops isl7998x_subdev_ops = {
+	.core		= &isl7998x_subdev_core_ops,
+	.video		= &isl7998x_subdev_video_ops,
+	.pad		= &isl7998x_subdev_pad_ops,
+};
+
+static const struct media_entity_operations isl7998x_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static const struct v4l2_ctrl_ops isl7998x_ctrl_ops = {
+	.s_ctrl			= isl7998x_set_ctrl,
+};
+
+static const struct v4l2_ctrl_config isl7998x_ctrls[] = {
+	{
+		.ops		= &isl7998x_ctrl_ops,
+		.id		= V4L2_CID_TEST_PATTERN_BARS,
+		.type		= V4L2_CTRL_TYPE_MENU,
+		.name		= "Test Pattern Bars",
+		.max		= ARRAY_SIZE(isl7998x_test_pattern_bars) - 1,
+		.def		= 0,
+		.qmenu		= isl7998x_test_pattern_bars,
+	}, {
+		.ops		= &isl7998x_ctrl_ops,
+		.id		= V4L2_CID_TEST_PATTERN_CHANNELS,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "Test Pattern Channels",
+		.min		= 0,
+		.max		= 0xf,
+		.step		= 1,
+		.def		= 0xf,
+		.flags		= 0,
+	}, {
+		.ops		= &isl7998x_ctrl_ops,
+		.id		= V4L2_CID_TEST_PATTERN_COLOR,
+		.type		= V4L2_CTRL_TYPE_MENU,
+		.name		= "Test Pattern Color",
+		.max		= ARRAY_SIZE(isl7998x_test_pattern_colors) - 1,
+		.def		= 0,
+		.qmenu		= isl7998x_test_pattern_colors,
+	},
+};
+
+#define ISL7998X_REG_DECODER_ACA_READABLE_RANGE(page)			\
+	/* Decoder range */						\
+	regmap_reg_range(ISL7998X_REG_PX_DEC_INPUT_FMT(page),		\
+			 ISL7998X_REG_PX_DEC_HS_DELAY_CTL(page)),	\
+	regmap_reg_range(ISL7998X_REG_PX_DEC_ANCTL(page),		\
+			 ISL7998X_REG_PX_DEC_CSC_CTL(page)),		\
+	regmap_reg_range(ISL7998X_REG_PX_DEC_BRIGHT(page),		\
+			 ISL7998X_REG_PX_DEC_HUE(page)),		\
+	regmap_reg_range(ISL7998X_REG_PX_DEC_VERT_PEAK(page),		\
+			 ISL7998X_REG_PX_DEC_CORING(page)),		\
+	regmap_reg_range(ISL7998X_REG_PX_DEC_SDT(page),			\
+			 ISL7998X_REG_PX_DEC_SDTR(page)),		\
+	regmap_reg_range(ISL7998X_REG_PX_DEC_CLMPG(page),		\
+			 ISL7998X_REG_PX_DEC_DATA_CONV(page)),		\
+	regmap_reg_range(ISL7998X_REG_PX_DEC_INTERNAL_TEST(page),	\
+			 ISL7998X_REG_PX_DEC_INTERNAL_TEST(page)),	\
+	regmap_reg_range(ISL7998X_REG_PX_DEC_H_DELAY_CTL(page),		\
+			 ISL7998X_REG_PX_DEC_H_DELAY_II_LOW(page)),	\
+	/* ACA range */							\
+	regmap_reg_range(ISL7998X_REG_PX_ACA_CTL_1(page),		\
+			 ISL7998X_REG_PX_ACA_HIST_WIN_V_SZ2(page)),	\
+	regmap_reg_range(ISL7998X_REG_PX_ACA_Y_AVG(page),		\
+			 ISL7998X_REG_PX_ACA_CTL_4(page)),		\
+	regmap_reg_range(ISL7998X_REG_PX_ACA_FLEX_WIN_HIST(page),	\
+			 ISL7998X_REG_PX_ACA_XFER_HIST_HOST(page)),	\
+	regmap_reg_range(ISL7998X_REG_PX_DEC_PAGE(page),		\
+			 ISL7998X_REG_PX_DEC_PAGE(page))
+
+#define ISL7998X_REG_DECODER_ACA_WRITEABLE_RANGE(page)			\
+	/* Decoder range */						\
+	regmap_reg_range(ISL7998X_REG_PX_DEC_INPUT_FMT(page),		\
+			 ISL7998X_REG_PX_DEC_INPUT_FMT(page)),		\
+	regmap_reg_range(ISL7998X_REG_PX_DEC_HS_DELAY_CTL(page),	\
+			 ISL7998X_REG_PX_DEC_HS_DELAY_CTL(page)),	\
+	regmap_reg_range(ISL7998X_REG_PX_DEC_ANCTL(page),		\
+			 ISL7998X_REG_PX_DEC_CSC_CTL(page)),		\
+	regmap_reg_range(ISL7998X_REG_PX_DEC_BRIGHT(page),		\
+			 ISL7998X_REG_PX_DEC_HUE(page)),		\
+	regmap_reg_range(ISL7998X_REG_PX_DEC_VERT_PEAK(page),		\
+			 ISL7998X_REG_PX_DEC_CORING(page)),		\
+	regmap_reg_range(ISL7998X_REG_PX_DEC_SDT(page),			\
+			 ISL7998X_REG_PX_DEC_SDTR(page)),		\
+	regmap_reg_range(ISL7998X_REG_PX_DEC_CLMPG(page),		\
+			 ISL7998X_REG_PX_DEC_MISC3(page)),		\
+	regmap_reg_range(ISL7998X_REG_PX_DEC_CLMD(page),		\
+			 ISL7998X_REG_PX_DEC_DATA_CONV(page)),		\
+	regmap_reg_range(ISL7998X_REG_PX_DEC_INTERNAL_TEST(page),	\
+			 ISL7998X_REG_PX_DEC_INTERNAL_TEST(page)),	\
+	regmap_reg_range(ISL7998X_REG_PX_DEC_H_DELAY_CTL(page),		\
+			 ISL7998X_REG_PX_DEC_H_DELAY_II_LOW(page)),	\
+	/* ACA range */							\
+	regmap_reg_range(ISL7998X_REG_PX_ACA_CTL_1(page),		\
+			 ISL7998X_REG_PX_ACA_HIST_WIN_V_SZ2(page)),	\
+	regmap_reg_range(ISL7998X_REG_PX_ACA_CTL_2(page),		\
+			 ISL7998X_REG_PX_ACA_CTL_4(page)),		\
+	regmap_reg_range(ISL7998X_REG_PX_ACA_FLEX_WIN_HIST(page),	\
+			 ISL7998X_REG_PX_ACA_HIST_DATA_LO(page)),	\
+	regmap_reg_range(ISL7998X_REG_PX_ACA_XFER_HIST_HOST(page),	\
+			 ISL7998X_REG_PX_ACA_XFER_HIST_HOST(page)),	\
+	regmap_reg_range(ISL7998X_REG_PX_DEC_PAGE(page),		\
+			 ISL7998X_REG_PX_DEC_PAGE(page))
+
+#define ISL7998X_REG_DECODER_ACA_VOLATILE_RANGE(page)			\
+	/* Decoder range */						\
+	regmap_reg_range(ISL7998X_REG_PX_DEC_STATUS_1(page),		\
+			 ISL7998X_REG_PX_DEC_STATUS_1(page)),		\
+	regmap_reg_range(ISL7998X_REG_PX_DEC_SDT(page),			\
+			 ISL7998X_REG_PX_DEC_SDT(page)),		\
+	regmap_reg_range(ISL7998X_REG_PX_DEC_MVSN(page),		\
+			 ISL7998X_REG_PX_DEC_HFREF(page)),		\
+	/* ACA range */							\
+	regmap_reg_range(ISL7998X_REG_PX_ACA_Y_AVG(page),		\
+			 ISL7998X_REG_PX_ACA_Y_HIGH(page)),		\
+	regmap_reg_range(ISL7998X_REG_PX_ACA_HIST_DATA_LO(page),	\
+			 ISL7998X_REG_PX_ACA_FLEX_WIN_CR_CLR(page))
+
+static const struct regmap_range isl7998x_readable_ranges[] = {
+	regmap_reg_range(ISL7998X_REG_P0_PRODUCT_ID_CODE,
+			 ISL7998X_REG_P0_IRQ_SYNC_CTL),
+	regmap_reg_range(ISL7998X_REG_P0_INTERRUPT_STATUS,
+			 ISL7998X_REG_P0_CLOCK_DELAY),
+	regmap_reg_range(ISL7998X_REG_PX_DEC_PAGE(0),
+			 ISL7998X_REG_PX_DEC_PAGE(0)),
+
+	ISL7998X_REG_DECODER_ACA_READABLE_RANGE(1),
+	ISL7998X_REG_DECODER_ACA_READABLE_RANGE(2),
+	ISL7998X_REG_DECODER_ACA_READABLE_RANGE(3),
+	ISL7998X_REG_DECODER_ACA_READABLE_RANGE(4),
+
+	regmap_reg_range(ISL7998X_REG_P5_LI_ENGINE_CTL,
+			 ISL7998X_REG_P5_MIPI_SP_HS_TRL_CTL),
+	regmap_reg_range(ISL7998X_REG_P5_FIFO_THRSH_CNT_1,
+			 ISL7998X_REG_P5_PLL_ANA),
+	regmap_reg_range(ISL7998X_REG_P5_TOTAL_PF_LINE_CNT_1,
+			 ISL7998X_REG_P5_HIST_LINE_CNT_2),
+	regmap_reg_range(ISL7998X_REG_PX_DEC_PAGE(5),
+			 ISL7998X_REG_PX_DEC_PAGE(5)),
+};
+
+static const struct regmap_range isl7998x_writeable_ranges[] = {
+	regmap_reg_range(ISL7998X_REG_P0_SW_RESET_CTL,
+			 ISL7998X_REG_P0_IRQ_SYNC_CTL),
+	regmap_reg_range(ISL7998X_REG_P0_CHAN_1_IRQ,
+			 ISL7998X_REG_P0_SHORT_DIAG_IRQ_EN),
+	regmap_reg_range(ISL7998X_REG_P0_CLOCK_DELAY,
+			 ISL7998X_REG_P0_CLOCK_DELAY),
+	regmap_reg_range(ISL7998X_REG_PX_DEC_PAGE(0),
+			 ISL7998X_REG_PX_DEC_PAGE(0)),
+
+	ISL7998X_REG_DECODER_ACA_WRITEABLE_RANGE(1),
+	ISL7998X_REG_DECODER_ACA_WRITEABLE_RANGE(2),
+	ISL7998X_REG_DECODER_ACA_WRITEABLE_RANGE(3),
+	ISL7998X_REG_DECODER_ACA_WRITEABLE_RANGE(4),
+
+	regmap_reg_range(ISL7998X_REG_P5_LI_ENGINE_CTL,
+			 ISL7998X_REG_P5_ESC_MODE_TIME_CTL),
+	regmap_reg_range(ISL7998X_REG_P5_MIPI_SP_HS_TRL_CTL,
+			 ISL7998X_REG_P5_PLL_ANA),
+	regmap_reg_range(ISL7998X_REG_P5_TOTAL_PF_LINE_CNT_1,
+			 ISL7998X_REG_P5_HIST_LINE_CNT_2),
+	regmap_reg_range(ISL7998X_REG_PX_DEC_PAGE(5),
+			 ISL7998X_REG_PX_DEC_PAGE(5)),
+
+	ISL7998X_REG_DECODER_ACA_WRITEABLE_RANGE(0xf),
+};
+
+static const struct regmap_range isl7998x_volatile_ranges[] = {
+	/* Product id code register is used to check availability */
+	regmap_reg_range(ISL7998X_REG_P0_PRODUCT_ID_CODE,
+			 ISL7998X_REG_P0_PRODUCT_ID_CODE),
+	regmap_reg_range(ISL7998X_REG_P0_MPP1_SYNC_CTL,
+			 ISL7998X_REG_P0_IRQ_SYNC_CTL),
+	regmap_reg_range(ISL7998X_REG_P0_INTERRUPT_STATUS,
+			 ISL7998X_REG_P0_INTERRUPT_STATUS),
+	regmap_reg_range(ISL7998X_REG_P0_CHAN_1_STATUS,
+			 ISL7998X_REG_P0_SHORT_DIAG_STATUS),
+
+	ISL7998X_REG_DECODER_ACA_VOLATILE_RANGE(1),
+	ISL7998X_REG_DECODER_ACA_VOLATILE_RANGE(2),
+	ISL7998X_REG_DECODER_ACA_VOLATILE_RANGE(3),
+	ISL7998X_REG_DECODER_ACA_VOLATILE_RANGE(4),
+
+	regmap_reg_range(ISL7998X_REG_P5_AUTO_TEST_ERR_DET,
+			 ISL7998X_REG_P5_PIC_HEIGHT_LOW),
+};
+
+static const struct regmap_access_table isl7998x_readable_table = {
+	.yes_ranges = isl7998x_readable_ranges,
+	.n_yes_ranges = ARRAY_SIZE(isl7998x_readable_ranges),
+};
+
+static const struct regmap_access_table isl7998x_writeable_table = {
+	.yes_ranges = isl7998x_writeable_ranges,
+	.n_yes_ranges = ARRAY_SIZE(isl7998x_writeable_ranges),
+};
+
+static const struct regmap_access_table isl7998x_volatile_table = {
+	.yes_ranges = isl7998x_volatile_ranges,
+	.n_yes_ranges = ARRAY_SIZE(isl7998x_volatile_ranges),
+};
+
+static const struct regmap_range_cfg isl7998x_ranges[] = {
+	{
+		.range_min	= ISL7998X_REG_PN_BASE(0),
+		.range_max	= ISL7998X_REG_PX_ACA_XFER_HIST_HOST(0xf),
+		.selector_reg	= ISL7998X_REG_PX_DEC_PAGE(0),
+		.selector_mask	= ISL7998X_REG_PX_DEC_PAGE_MASK,
+		.window_start	= 0,
+		.window_len	= 256,
+	}
+};
+
+static const struct regmap_config isl7998x_regmap = {
+	.reg_bits	= 8,
+	.val_bits	= 8,
+	.max_register	= ISL7998X_REG_PX_ACA_XFER_HIST_HOST(0xf),
+	.ranges		= isl7998x_ranges,
+	.num_ranges	= ARRAY_SIZE(isl7998x_ranges),
+	.rd_table	= &isl7998x_readable_table,
+	.wr_table	= &isl7998x_writeable_table,
+	.volatile_table	= &isl7998x_volatile_table,
+	.cache_type	= REGCACHE_RBTREE,
+};
+
+static int isl7998x_mc_init(struct isl7998x *isl7998x)
+{
+	unsigned int i;
+
+	isl7998x->subdev.entity.ops = &isl7998x_entity_ops;
+	isl7998x->subdev.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
+
+	isl7998x->pads[ISL7998X_PAD_OUT].flags = MEDIA_PAD_FL_SOURCE;
+	for (i = ISL7998X_PAD_VIN1; i < ISL7998X_NUM_PADS; i++)
+		isl7998x->pads[i].flags = MEDIA_PAD_FL_SINK;
+
+	return media_entity_pads_init(&isl7998x->subdev.entity,
+				      ISL7998X_NUM_PADS,
+				      isl7998x->pads);
+}
+
+static int get_link_freq_menu_index(unsigned int lanes,
+				    unsigned int inputs)
+{
+	int ret = -EINVAL;
+
+	switch (lanes) {
+	case 1:
+		if (inputs == 1)
+			ret = 0;
+		if (inputs == 2)
+			ret = 1;
+		if (inputs == 4)
+			ret = 2;
+		break;
+	case 2:
+		if (inputs == 2)
+			ret = 0;
+		if (inputs == 4)
+			ret = 1;
+		break;
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+static void isl7998x_remove_controls(struct isl7998x *isl7998x)
+{
+	v4l2_ctrl_handler_free(&isl7998x->ctrl_handler);
+	mutex_destroy(&isl7998x->ctrl_mutex);
+}
+
+static int isl7998x_init_controls(struct isl7998x *isl7998x)
+{
+	struct v4l2_subdev *sd = &isl7998x->subdev;
+	int link_freq_index;
+	unsigned int i;
+	int ret;
+
+	ret = v4l2_ctrl_handler_init(&isl7998x->ctrl_handler,
+				     2 + ARRAY_SIZE(isl7998x_ctrls));
+	if (ret)
+		return ret;
+
+	mutex_init(&isl7998x->ctrl_mutex);
+	isl7998x->ctrl_handler.lock = &isl7998x->ctrl_mutex;
+	link_freq_index = get_link_freq_menu_index(isl7998x->nr_mipi_lanes,
+						   isl7998x->nr_inputs);
+	if (link_freq_index < 0 ||
+	    link_freq_index >= ARRAY_SIZE(link_freq_menu_items)) {
+		dev_err(sd->dev,
+			"failed to find MIPI link freq: %d lanes, %d inputs\n",
+			isl7998x->nr_mipi_lanes, isl7998x->nr_inputs);
+		ret = -EINVAL;
+		goto err;
+	}
+
+	isl7998x->link_freq = v4l2_ctrl_new_int_menu(&isl7998x->ctrl_handler,
+						     &isl7998x_ctrl_ops,
+						     V4L2_CID_LINK_FREQ,
+						     ARRAY_SIZE(link_freq_menu_items) - 1,
+						     link_freq_index,
+						     link_freq_menu_items);
+	if (isl7998x->link_freq)
+		isl7998x->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	for (i = 0; i < ARRAY_SIZE(isl7998x_ctrls); i++)
+		v4l2_ctrl_new_custom(&isl7998x->ctrl_handler,
+				     &isl7998x_ctrls[i], NULL);
+
+	v4l2_ctrl_new_std_menu_items(&isl7998x->ctrl_handler,
+				     &isl7998x_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(isl7998x_test_pattern_menu) - 1,
+				     0, 0, isl7998x_test_pattern_menu);
+
+	ret = isl7998x->ctrl_handler.error;
+	if (ret)
+		goto err;
+
+	isl7998x->subdev.ctrl_handler = &isl7998x->ctrl_handler;
+	v4l2_ctrl_handler_setup(&isl7998x->ctrl_handler);
+
+	return 0;
+
+err:
+	isl7998x_remove_controls(isl7998x);
+
+	return ret;
+}
+
+static int isl7998x_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct v4l2_fwnode_endpoint endpoint = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY,
+	};
+	struct fwnode_handle *ep;
+	struct isl7998x *isl7998x;
+	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
+	int nr_inputs;
+	int ret;
+
+	ret = i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA);
+	if (!ret) {
+		dev_warn(&adapter->dev,
+			 "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
+		return -EIO;
+	}
+
+	isl7998x = devm_kzalloc(dev, sizeof(*isl7998x), GFP_KERNEL);
+	if (!isl7998x)
+		return -ENOMEM;
+
+	isl7998x->pd_gpio = devm_gpiod_get_optional(dev, "powerdown",
+						    GPIOD_OUT_HIGH);
+	if (IS_ERR(isl7998x->pd_gpio))
+		return dev_err_probe(dev, PTR_ERR(isl7998x->pd_gpio),
+				     "Failed to retrieve/request PD GPIO\n");
+
+	isl7998x->rstb_gpio = devm_gpiod_get_optional(dev, "reset",
+						      GPIOD_OUT_HIGH);
+	if (IS_ERR(isl7998x->rstb_gpio))
+		return dev_err_probe(dev, PTR_ERR(isl7998x->rstb_gpio),
+				     "Failed to retrieve/request RSTB GPIO\n");
+
+	isl7998x->regmap = devm_regmap_init_i2c(client, &isl7998x_regmap);
+	if (IS_ERR(isl7998x->regmap))
+		return dev_err_probe(dev, PTR_ERR(isl7998x->regmap),
+				     "Failed to allocate register map\n");
+
+	ep = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev),
+					     ISL7998X_PAD_OUT, 0, 0);
+	if (!ep)
+		return dev_err_probe(dev, -EINVAL, "Missing endpoint node\n");
+
+	ret = v4l2_fwnode_endpoint_parse(ep, &endpoint);
+	fwnode_handle_put(ep);
+	if (ret)
+		return dev_err_probe(dev, ret, "Failed to parse endpoint\n");
+
+	if (endpoint.bus.mipi_csi2.num_data_lanes == 0 ||
+	    endpoint.bus.mipi_csi2.num_data_lanes > 2)
+		return dev_err_probe(dev, -EINVAL,
+				     "Invalid number of MIPI lanes\n");
+
+	isl7998x->nr_mipi_lanes = endpoint.bus.mipi_csi2.num_data_lanes;
+
+	nr_inputs = isl7998x_get_nr_inputs(dev->of_node);
+	if (nr_inputs < 0)
+		return dev_err_probe(dev, nr_inputs,
+				     "Invalid number of input ports\n");
+	isl7998x->nr_inputs = nr_inputs;
+
+	v4l2_i2c_subdev_init(&isl7998x->subdev, client, &isl7998x_subdev_ops);
+	isl7998x->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	ret = isl7998x_mc_init(isl7998x);
+	if (ret < 0)
+		return ret;
+
+	isl7998x->fmt = &isl7998x_colour_fmts[0];
+	isl7998x->norm = V4L2_STD_NTSC;
+	isl7998x->enabled = 0;
+
+	mutex_init(&isl7998x->lock);
+
+	ret = isl7998x_init_controls(isl7998x);
+	if (ret)
+		goto err_entity_cleanup;
+
+	ret = v4l2_async_register_subdev(&isl7998x->subdev);
+	if (ret < 0)
+		goto err_controls_cleanup;
+
+	pm_runtime_enable(dev);
+
+	return 0;
+
+err_controls_cleanup:
+	isl7998x_remove_controls(isl7998x);
+err_entity_cleanup:
+	media_entity_cleanup(&isl7998x->subdev.entity);
+
+	return ret;
+}
+
+static void isl7998x_remove(struct i2c_client *client)
+{
+	struct isl7998x *isl7998x = i2c_to_isl7998x(client);
+
+	pm_runtime_disable(&client->dev);
+	v4l2_async_unregister_subdev(&isl7998x->subdev);
+	isl7998x_remove_controls(isl7998x);
+	media_entity_cleanup(&isl7998x->subdev.entity);
+}
+
+static const struct of_device_id isl7998x_of_match[] = {
+	{ .compatible = "isil,isl79987", },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, isl7998x_of_match);
+
+static const struct i2c_device_id isl7998x_id[] = {
+	{ "isl79987", 0 },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(i2c, isl7998x_id);
+
+static int __maybe_unused isl7998x_runtime_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct isl7998x *isl7998x = sd_to_isl7998x(sd);
+	int ret;
+
+	gpiod_set_value(isl7998x->rstb_gpio, 1);
+	gpiod_set_value(isl7998x->pd_gpio, 0);
+	gpiod_set_value(isl7998x->rstb_gpio, 0);
+
+	ret = isl7998x_wait_power_on(isl7998x);
+	if (ret)
+		goto err;
+
+	ret = isl7998x_init(isl7998x);
+	if (ret)
+		goto err;
+
+	return 0;
+
+err:
+	gpiod_set_value(isl7998x->pd_gpio, 1);
+
+	return ret;
+}
+
+static int __maybe_unused isl7998x_runtime_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct isl7998x *isl7998x = sd_to_isl7998x(sd);
+
+	gpiod_set_value(isl7998x->pd_gpio, 1);
+
+	return 0;
+}
+
+static const struct dev_pm_ops isl7998x_pm_ops = {
+	SET_RUNTIME_PM_OPS(isl7998x_runtime_suspend,
+			   isl7998x_runtime_resume,
+			   NULL)
+};
+
+static struct i2c_driver isl7998x_i2c_driver = {
+	.driver = {
+		.name = "isl7998x",
+		.of_match_table = isl7998x_of_match,
+		.pm = &isl7998x_pm_ops,
+	},
+	.probe		= isl7998x_probe,
+	.remove		= isl7998x_remove,
+	.id_table	= isl7998x_id,
+};
+
+module_i2c_driver(isl7998x_i2c_driver);
+
+MODULE_DESCRIPTION("Intersil ISL7998x Analog to MIPI CSI-2/BT656 decoder");
+MODULE_AUTHOR("Marek Vasut <marex@denx.de>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/ks0127.c b/drivers/media/i2c/ks0127.c
new file mode 100644
index 000000000..5c583f57e
--- /dev/null
+++ b/drivers/media/i2c/ks0127.c
@@ -0,0 +1,704 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Video Capture Driver (Video for Linux 1/2)
+ * for the Matrox Marvel G200,G400 and Rainbow Runner-G series
+ *
+ * This module is an interface to the KS0127 video decoder chip.
+ *
+ * Copyright (C) 1999  Ryan Drake <stiletto@mediaone.net>
+ *
+ *****************************************************************************
+ *
+ * Modified and extended by
+ *	Mike Bernson <mike@mlb.org>
+ *	Gerard v.d. Horst
+ *	Leon van Stuivenberg <l.vanstuivenberg@chello.nl>
+ *	Gernot Ziegler <gz@lysator.liu.se>
+ *
+ * Version History:
+ * V1.0 Ryan Drake	   Initial version by Ryan Drake
+ * V1.1 Gerard v.d. Horst  Added some debugoutput, reset the video-standard
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <linux/slab.h>
+#include <media/v4l2-device.h>
+#include "ks0127.h"
+
+MODULE_DESCRIPTION("KS0127 video decoder driver");
+MODULE_AUTHOR("Ryan Drake");
+MODULE_LICENSE("GPL");
+
+/* Addresses */
+#define I2C_KS0127_ADDON   0xD8
+#define I2C_KS0127_ONBOARD 0xDA
+
+
+/* ks0127 control registers */
+#define KS_STAT     0x00
+#define KS_CMDA     0x01
+#define KS_CMDB     0x02
+#define KS_CMDC     0x03
+#define KS_CMDD     0x04
+#define KS_HAVB     0x05
+#define KS_HAVE     0x06
+#define KS_HS1B     0x07
+#define KS_HS1E     0x08
+#define KS_HS2B     0x09
+#define KS_HS2E     0x0a
+#define KS_AGC      0x0b
+#define KS_HXTRA    0x0c
+#define KS_CDEM     0x0d
+#define KS_PORTAB   0x0e
+#define KS_LUMA     0x0f
+#define KS_CON      0x10
+#define KS_BRT      0x11
+#define KS_CHROMA   0x12
+#define KS_CHROMB   0x13
+#define KS_DEMOD    0x14
+#define KS_SAT      0x15
+#define KS_HUE      0x16
+#define KS_VERTIA   0x17
+#define KS_VERTIB   0x18
+#define KS_VERTIC   0x19
+#define KS_HSCLL    0x1a
+#define KS_HSCLH    0x1b
+#define KS_VSCLL    0x1c
+#define KS_VSCLH    0x1d
+#define KS_OFMTA    0x1e
+#define KS_OFMTB    0x1f
+#define KS_VBICTL   0x20
+#define KS_CCDAT2   0x21
+#define KS_CCDAT1   0x22
+#define KS_VBIL30   0x23
+#define KS_VBIL74   0x24
+#define KS_VBIL118  0x25
+#define KS_VBIL1512 0x26
+#define KS_TTFRAM   0x27
+#define KS_TESTA    0x28
+#define KS_UVOFFH   0x29
+#define KS_UVOFFL   0x2a
+#define KS_UGAIN    0x2b
+#define KS_VGAIN    0x2c
+#define KS_VAVB     0x2d
+#define KS_VAVE     0x2e
+#define KS_CTRACK   0x2f
+#define KS_POLCTL   0x30
+#define KS_REFCOD   0x31
+#define KS_INVALY   0x32
+#define KS_INVALU   0x33
+#define KS_INVALV   0x34
+#define KS_UNUSEY   0x35
+#define KS_UNUSEU   0x36
+#define KS_UNUSEV   0x37
+#define KS_USRSAV   0x38
+#define KS_USREAV   0x39
+#define KS_SHS1A    0x3a
+#define KS_SHS1B    0x3b
+#define KS_SHS1C    0x3c
+#define KS_CMDE     0x3d
+#define KS_VSDEL    0x3e
+#define KS_CMDF     0x3f
+#define KS_GAMMA0   0x40
+#define KS_GAMMA1   0x41
+#define KS_GAMMA2   0x42
+#define KS_GAMMA3   0x43
+#define KS_GAMMA4   0x44
+#define KS_GAMMA5   0x45
+#define KS_GAMMA6   0x46
+#define KS_GAMMA7   0x47
+#define KS_GAMMA8   0x48
+#define KS_GAMMA9   0x49
+#define KS_GAMMA10  0x4a
+#define KS_GAMMA11  0x4b
+#define KS_GAMMA12  0x4c
+#define KS_GAMMA13  0x4d
+#define KS_GAMMA14  0x4e
+#define KS_GAMMA15  0x4f
+#define KS_GAMMA16  0x50
+#define KS_GAMMA17  0x51
+#define KS_GAMMA18  0x52
+#define KS_GAMMA19  0x53
+#define KS_GAMMA20  0x54
+#define KS_GAMMA21  0x55
+#define KS_GAMMA22  0x56
+#define KS_GAMMA23  0x57
+#define KS_GAMMA24  0x58
+#define KS_GAMMA25  0x59
+#define KS_GAMMA26  0x5a
+#define KS_GAMMA27  0x5b
+#define KS_GAMMA28  0x5c
+#define KS_GAMMA29  0x5d
+#define KS_GAMMA30  0x5e
+#define KS_GAMMA31  0x5f
+#define KS_GAMMAD0  0x60
+#define KS_GAMMAD1  0x61
+#define KS_GAMMAD2  0x62
+#define KS_GAMMAD3  0x63
+#define KS_GAMMAD4  0x64
+#define KS_GAMMAD5  0x65
+#define KS_GAMMAD6  0x66
+#define KS_GAMMAD7  0x67
+#define KS_GAMMAD8  0x68
+#define KS_GAMMAD9  0x69
+#define KS_GAMMAD10 0x6a
+#define KS_GAMMAD11 0x6b
+#define KS_GAMMAD12 0x6c
+#define KS_GAMMAD13 0x6d
+#define KS_GAMMAD14 0x6e
+#define KS_GAMMAD15 0x6f
+#define KS_GAMMAD16 0x70
+#define KS_GAMMAD17 0x71
+#define KS_GAMMAD18 0x72
+#define KS_GAMMAD19 0x73
+#define KS_GAMMAD20 0x74
+#define KS_GAMMAD21 0x75
+#define KS_GAMMAD22 0x76
+#define KS_GAMMAD23 0x77
+#define KS_GAMMAD24 0x78
+#define KS_GAMMAD25 0x79
+#define KS_GAMMAD26 0x7a
+#define KS_GAMMAD27 0x7b
+#define KS_GAMMAD28 0x7c
+#define KS_GAMMAD29 0x7d
+#define KS_GAMMAD30 0x7e
+#define KS_GAMMAD31 0x7f
+
+
+/****************************************************************************
+* mga_dev : represents one ks0127 chip.
+****************************************************************************/
+
+struct adjust {
+	int	contrast;
+	int	bright;
+	int	hue;
+	int	ugain;
+	int	vgain;
+};
+
+struct ks0127 {
+	struct v4l2_subdev sd;
+	v4l2_std_id	norm;
+	u8		regs[256];
+};
+
+static inline struct ks0127 *to_ks0127(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct ks0127, sd);
+}
+
+
+static int debug; /* insmod parameter */
+
+module_param(debug, int, 0);
+MODULE_PARM_DESC(debug, "Debug output");
+
+static u8 reg_defaults[64];
+
+static void init_reg_defaults(void)
+{
+	static int initialized;
+	u8 *table = reg_defaults;
+
+	if (initialized)
+		return;
+	initialized = 1;
+
+	table[KS_CMDA]     = 0x2c;  /* VSE=0, CCIR 601, autodetect standard */
+	table[KS_CMDB]     = 0x12;  /* VALIGN=0, AGC control and input */
+	table[KS_CMDC]     = 0x00;  /* Test options */
+	/* clock & input select, write 1 to PORTA */
+	table[KS_CMDD]     = 0x01;
+	table[KS_HAVB]     = 0x00;  /* HAV Start Control */
+	table[KS_HAVE]     = 0x00;  /* HAV End Control */
+	table[KS_HS1B]     = 0x10;  /* HS1 Start Control */
+	table[KS_HS1E]     = 0x00;  /* HS1 End Control */
+	table[KS_HS2B]     = 0x00;  /* HS2 Start Control */
+	table[KS_HS2E]     = 0x00;  /* HS2 End Control */
+	table[KS_AGC]      = 0x53;  /* Manual setting for AGC */
+	table[KS_HXTRA]    = 0x00;  /* Extra Bits for HAV and HS1/2 */
+	table[KS_CDEM]     = 0x00;  /* Chroma Demodulation Control */
+	table[KS_PORTAB]   = 0x0f;  /* port B is input, port A output GPPORT */
+	table[KS_LUMA]     = 0x01;  /* Luma control */
+	table[KS_CON]      = 0x00;  /* Contrast Control */
+	table[KS_BRT]      = 0x00;  /* Brightness Control */
+	table[KS_CHROMA]   = 0x2a;  /* Chroma control A */
+	table[KS_CHROMB]   = 0x90;  /* Chroma control B */
+	table[KS_DEMOD]    = 0x00;  /* Chroma Demodulation Control & Status */
+	table[KS_SAT]      = 0x00;  /* Color Saturation Control*/
+	table[KS_HUE]      = 0x00;  /* Hue Control */
+	table[KS_VERTIA]   = 0x00;  /* Vertical Processing Control A */
+	/* Vertical Processing Control B, luma 1 line delayed */
+	table[KS_VERTIB]   = 0x12;
+	table[KS_VERTIC]   = 0x0b;  /* Vertical Processing Control C */
+	table[KS_HSCLL]    = 0x00;  /* Horizontal Scaling Ratio Low */
+	table[KS_HSCLH]    = 0x00;  /* Horizontal Scaling Ratio High */
+	table[KS_VSCLL]    = 0x00;  /* Vertical Scaling Ratio Low */
+	table[KS_VSCLH]    = 0x00;  /* Vertical Scaling Ratio High */
+	/* 16 bit YCbCr 4:2:2 output; I can't make the bt866 like 8 bit /Sam */
+	table[KS_OFMTA]    = 0x30;
+	table[KS_OFMTB]    = 0x00;  /* Output Control B */
+	/* VBI Decoder Control; 4bit fmt: avoid Y overflow */
+	table[KS_VBICTL]   = 0x5d;
+	table[KS_CCDAT2]   = 0x00;  /* Read Only register */
+	table[KS_CCDAT1]   = 0x00;  /* Read Only register */
+	table[KS_VBIL30]   = 0xa8;  /* VBI data decoding options */
+	table[KS_VBIL74]   = 0xaa;  /* VBI data decoding options */
+	table[KS_VBIL118]  = 0x2a;  /* VBI data decoding options */
+	table[KS_VBIL1512] = 0x00;  /* VBI data decoding options */
+	table[KS_TTFRAM]   = 0x00;  /* Teletext frame alignment pattern */
+	table[KS_TESTA]    = 0x00;  /* test register, shouldn't be written */
+	table[KS_UVOFFH]   = 0x00;  /* UV Offset Adjustment High */
+	table[KS_UVOFFL]   = 0x00;  /* UV Offset Adjustment Low */
+	table[KS_UGAIN]    = 0x00;  /* U Component Gain Adjustment */
+	table[KS_VGAIN]    = 0x00;  /* V Component Gain Adjustment */
+	table[KS_VAVB]     = 0x07;  /* VAV Begin */
+	table[KS_VAVE]     = 0x00;  /* VAV End */
+	table[KS_CTRACK]   = 0x00;  /* Chroma Tracking Control */
+	table[KS_POLCTL]   = 0x41;  /* Timing Signal Polarity Control */
+	table[KS_REFCOD]   = 0x80;  /* Reference Code Insertion Control */
+	table[KS_INVALY]   = 0x10;  /* Invalid Y Code */
+	table[KS_INVALU]   = 0x80;  /* Invalid U Code */
+	table[KS_INVALV]   = 0x80;  /* Invalid V Code */
+	table[KS_UNUSEY]   = 0x10;  /* Unused Y Code */
+	table[KS_UNUSEU]   = 0x80;  /* Unused U Code */
+	table[KS_UNUSEV]   = 0x80;  /* Unused V Code */
+	table[KS_USRSAV]   = 0x00;  /* reserved */
+	table[KS_USREAV]   = 0x00;  /* reserved */
+	table[KS_SHS1A]    = 0x00;  /* User Defined SHS1 A */
+	/* User Defined SHS1 B, ALT656=1 on 0127B */
+	table[KS_SHS1B]    = 0x80;
+	table[KS_SHS1C]    = 0x00;  /* User Defined SHS1 C */
+	table[KS_CMDE]     = 0x00;  /* Command Register E */
+	table[KS_VSDEL]    = 0x00;  /* VS Delay Control */
+	/* Command Register F, update -immediately- */
+	/* (there might come no vsync)*/
+	table[KS_CMDF]     = 0x02;
+}
+
+
+/* We need to manually read because of a bug in the KS0127 chip.
+ *
+ * An explanation from kayork@mail.utexas.edu:
+ *
+ * During I2C reads, the KS0127 only samples for a stop condition
+ * during the place where the acknowledge bit should be. Any standard
+ * I2C implementation (correctly) throws in another clock transition
+ * at the 9th bit, and the KS0127 will not recognize the stop condition
+ * and will continue to clock out data.
+ *
+ * So we have to do the read ourself.  Big deal.
+ *	   workaround in i2c-algo-bit
+ */
+
+
+static u8 ks0127_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	char val = 0;
+	struct i2c_msg msgs[] = {
+		{
+			.addr = client->addr,
+			.len = sizeof(reg),
+			.buf = &reg
+		},
+		{
+			.addr = client->addr,
+			.flags = I2C_M_RD | I2C_M_NO_RD_ACK,
+			.len = sizeof(val),
+			.buf = &val
+		}
+	};
+	int ret;
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret != ARRAY_SIZE(msgs))
+		v4l2_dbg(1, debug, sd, "read error\n");
+
+	return val;
+}
+
+
+static void ks0127_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ks0127 *ks = to_ks0127(sd);
+	char msg[] = { reg, val };
+
+	if (i2c_master_send(client, msg, sizeof(msg)) != sizeof(msg))
+		v4l2_dbg(1, debug, sd, "write error\n");
+
+	ks->regs[reg] = val;
+}
+
+
+/* generic bit-twiddling */
+static void ks0127_and_or(struct v4l2_subdev *sd, u8 reg, u8 and_v, u8 or_v)
+{
+	struct ks0127 *ks = to_ks0127(sd);
+
+	u8 val = ks->regs[reg];
+	val = (val & and_v) | or_v;
+	ks0127_write(sd, reg, val);
+}
+
+
+
+/****************************************************************************
+* ks0127 private api
+****************************************************************************/
+static void ks0127_init(struct v4l2_subdev *sd)
+{
+	u8 *table = reg_defaults;
+	int i;
+
+	v4l2_dbg(1, debug, sd, "reset\n");
+	msleep(1);
+
+	/* initialize all registers to known values */
+	/* (except STAT, 0x21, 0x22, TEST and 0x38,0x39) */
+
+	for (i = 1; i < 33; i++)
+		ks0127_write(sd, i, table[i]);
+
+	for (i = 35; i < 40; i++)
+		ks0127_write(sd, i, table[i]);
+
+	for (i = 41; i < 56; i++)
+		ks0127_write(sd, i, table[i]);
+
+	for (i = 58; i < 64; i++)
+		ks0127_write(sd, i, table[i]);
+
+
+	if ((ks0127_read(sd, KS_STAT) & 0x80) == 0) {
+		v4l2_dbg(1, debug, sd, "ks0122s found\n");
+		return;
+	}
+
+	switch (ks0127_read(sd, KS_CMDE) & 0x0f) {
+	case 0:
+		v4l2_dbg(1, debug, sd, "ks0127 found\n");
+		break;
+
+	case 9:
+		v4l2_dbg(1, debug, sd, "ks0127B Revision A found\n");
+		break;
+
+	default:
+		v4l2_dbg(1, debug, sd, "unknown revision\n");
+		break;
+	}
+}
+
+static int ks0127_s_routing(struct v4l2_subdev *sd,
+			    u32 input, u32 output, u32 config)
+{
+	struct ks0127 *ks = to_ks0127(sd);
+
+	switch (input) {
+	case KS_INPUT_COMPOSITE_1:
+	case KS_INPUT_COMPOSITE_2:
+	case KS_INPUT_COMPOSITE_3:
+	case KS_INPUT_COMPOSITE_4:
+	case KS_INPUT_COMPOSITE_5:
+	case KS_INPUT_COMPOSITE_6:
+		v4l2_dbg(1, debug, sd,
+			"s_routing %d: Composite\n", input);
+		/* autodetect 50/60 Hz */
+		ks0127_and_or(sd, KS_CMDA,   0xfc, 0x00);
+		/* VSE=0 */
+		ks0127_and_or(sd, KS_CMDA,   ~0x40, 0x00);
+		/* set input line */
+		ks0127_and_or(sd, KS_CMDB,   0xb0, input);
+		/* non-freerunning mode */
+		ks0127_and_or(sd, KS_CMDC,   0x70, 0x0a);
+		/* analog input */
+		ks0127_and_or(sd, KS_CMDD,   0x03, 0x00);
+		/* enable chroma demodulation */
+		ks0127_and_or(sd, KS_CTRACK, 0xcf, 0x00);
+		/* chroma trap, HYBWR=1 */
+		ks0127_and_or(sd, KS_LUMA,   0x00,
+			       (reg_defaults[KS_LUMA])|0x0c);
+		/* scaler fullbw, luma comb off */
+		ks0127_and_or(sd, KS_VERTIA, 0x08, 0x81);
+		/* manual chroma comb .25 .5 .25 */
+		ks0127_and_or(sd, KS_VERTIC, 0x0f, 0x90);
+
+		/* chroma path delay */
+		ks0127_and_or(sd, KS_CHROMB, 0x0f, 0x90);
+
+		ks0127_write(sd, KS_UGAIN, reg_defaults[KS_UGAIN]);
+		ks0127_write(sd, KS_VGAIN, reg_defaults[KS_VGAIN]);
+		ks0127_write(sd, KS_UVOFFH, reg_defaults[KS_UVOFFH]);
+		ks0127_write(sd, KS_UVOFFL, reg_defaults[KS_UVOFFL]);
+		break;
+
+	case KS_INPUT_SVIDEO_1:
+	case KS_INPUT_SVIDEO_2:
+	case KS_INPUT_SVIDEO_3:
+		v4l2_dbg(1, debug, sd,
+			"s_routing %d: S-Video\n", input);
+		/* autodetect 50/60 Hz */
+		ks0127_and_or(sd, KS_CMDA,   0xfc, 0x00);
+		/* VSE=0 */
+		ks0127_and_or(sd, KS_CMDA,   ~0x40, 0x00);
+		/* set input line */
+		ks0127_and_or(sd, KS_CMDB,   0xb0, input);
+		/* non-freerunning mode */
+		ks0127_and_or(sd, KS_CMDC,   0x70, 0x0a);
+		/* analog input */
+		ks0127_and_or(sd, KS_CMDD,   0x03, 0x00);
+		/* enable chroma demodulation */
+		ks0127_and_or(sd, KS_CTRACK, 0xcf, 0x00);
+		ks0127_and_or(sd, KS_LUMA, 0x00,
+			       reg_defaults[KS_LUMA]);
+		/* disable luma comb */
+		ks0127_and_or(sd, KS_VERTIA, 0x08,
+			       (reg_defaults[KS_VERTIA]&0xf0)|0x01);
+		ks0127_and_or(sd, KS_VERTIC, 0x0f,
+			       reg_defaults[KS_VERTIC]&0xf0);
+
+		ks0127_and_or(sd, KS_CHROMB, 0x0f,
+			       reg_defaults[KS_CHROMB]&0xf0);
+
+		ks0127_write(sd, KS_UGAIN, reg_defaults[KS_UGAIN]);
+		ks0127_write(sd, KS_VGAIN, reg_defaults[KS_VGAIN]);
+		ks0127_write(sd, KS_UVOFFH, reg_defaults[KS_UVOFFH]);
+		ks0127_write(sd, KS_UVOFFL, reg_defaults[KS_UVOFFL]);
+		break;
+
+	case KS_INPUT_YUV656:
+		v4l2_dbg(1, debug, sd, "s_routing 15: YUV656\n");
+		if (ks->norm & V4L2_STD_525_60)
+			/* force 60 Hz */
+			ks0127_and_or(sd, KS_CMDA,   0xfc, 0x03);
+		else
+			/* force 50 Hz */
+			ks0127_and_or(sd, KS_CMDA,   0xfc, 0x02);
+
+		ks0127_and_or(sd, KS_CMDA,   0xff, 0x40); /* VSE=1 */
+		/* set input line and VALIGN */
+		ks0127_and_or(sd, KS_CMDB,   0xb0, (input | 0x40));
+		/* freerunning mode, */
+		/* TSTGEN = 1 TSTGFR=11 TSTGPH=0 TSTGPK=0  VMEM=1*/
+		ks0127_and_or(sd, KS_CMDC,   0x70, 0x87);
+		/* digital input, SYNDIR = 0 INPSL=01 CLKDIR=0 EAV=0 */
+		ks0127_and_or(sd, KS_CMDD,   0x03, 0x08);
+		/* disable chroma demodulation */
+		ks0127_and_or(sd, KS_CTRACK, 0xcf, 0x30);
+		/* HYPK =01 CTRAP = 0 HYBWR=0 PED=1 RGBH=1 UNIT=1 */
+		ks0127_and_or(sd, KS_LUMA,   0x00, 0x71);
+		ks0127_and_or(sd, KS_VERTIC, 0x0f,
+			       reg_defaults[KS_VERTIC]&0xf0);
+
+		/* scaler fullbw, luma comb off */
+		ks0127_and_or(sd, KS_VERTIA, 0x08, 0x81);
+
+		ks0127_and_or(sd, KS_CHROMB, 0x0f,
+			       reg_defaults[KS_CHROMB]&0xf0);
+
+		ks0127_and_or(sd, KS_CON, 0x00, 0x00);
+		ks0127_and_or(sd, KS_BRT, 0x00, 32);	/* spec: 34 */
+			/* spec: 229 (e5) */
+		ks0127_and_or(sd, KS_SAT, 0x00, 0xe8);
+		ks0127_and_or(sd, KS_HUE, 0x00, 0);
+
+		ks0127_and_or(sd, KS_UGAIN, 0x00, 238);
+		ks0127_and_or(sd, KS_VGAIN, 0x00, 0x00);
+
+		/*UOFF:0x30, VOFF:0x30, TSTCGN=1 */
+		ks0127_and_or(sd, KS_UVOFFH, 0x00, 0x4f);
+		ks0127_and_or(sd, KS_UVOFFL, 0x00, 0x00);
+		break;
+
+	default:
+		v4l2_dbg(1, debug, sd,
+			"s_routing: Unknown input %d\n", input);
+		break;
+	}
+
+	/* hack: CDMLPF sometimes spontaneously switches on; */
+	/* force back off */
+	ks0127_write(sd, KS_DEMOD, reg_defaults[KS_DEMOD]);
+	return 0;
+}
+
+static int ks0127_s_std(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct ks0127 *ks = to_ks0127(sd);
+
+	/* Set to automatic SECAM/Fsc mode */
+	ks0127_and_or(sd, KS_DEMOD, 0xf0, 0x00);
+
+	ks->norm = std;
+	if (std & V4L2_STD_NTSC) {
+		v4l2_dbg(1, debug, sd,
+			"s_std: NTSC_M\n");
+		ks0127_and_or(sd, KS_CHROMA, 0x9f, 0x20);
+	} else if (std & V4L2_STD_PAL_N) {
+		v4l2_dbg(1, debug, sd,
+			"s_std: NTSC_N (fixme)\n");
+		ks0127_and_or(sd, KS_CHROMA, 0x9f, 0x40);
+	} else if (std & V4L2_STD_PAL) {
+		v4l2_dbg(1, debug, sd,
+			"s_std: PAL_N\n");
+		ks0127_and_or(sd, KS_CHROMA, 0x9f, 0x20);
+	} else if (std & V4L2_STD_PAL_M) {
+		v4l2_dbg(1, debug, sd,
+			"s_std: PAL_M (fixme)\n");
+		ks0127_and_or(sd, KS_CHROMA, 0x9f, 0x40);
+	} else if (std & V4L2_STD_SECAM) {
+		v4l2_dbg(1, debug, sd,
+			"s_std: SECAM\n");
+
+		/* set to secam autodetection */
+		ks0127_and_or(sd, KS_CHROMA, 0xdf, 0x20);
+		ks0127_and_or(sd, KS_DEMOD, 0xf0, 0x00);
+		schedule_timeout_interruptible(HZ/10+1);
+
+		/* did it autodetect? */
+		if (!(ks0127_read(sd, KS_DEMOD) & 0x40))
+			/* force to secam mode */
+			ks0127_and_or(sd, KS_DEMOD, 0xf0, 0x0f);
+	} else {
+		v4l2_dbg(1, debug, sd, "s_std: Unknown norm %llx\n",
+			       (unsigned long long)std);
+	}
+	return 0;
+}
+
+static int ks0127_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	v4l2_dbg(1, debug, sd, "s_stream(%d)\n", enable);
+	if (enable) {
+		/* All output pins on */
+		ks0127_and_or(sd, KS_OFMTA, 0xcf, 0x30);
+		/* Obey the OEN pin */
+		ks0127_and_or(sd, KS_CDEM, 0x7f, 0x00);
+	} else {
+		/* Video output pins off */
+		ks0127_and_or(sd, KS_OFMTA, 0xcf, 0x00);
+		/* Ignore the OEN pin */
+		ks0127_and_or(sd, KS_CDEM, 0x7f, 0x80);
+	}
+	return 0;
+}
+
+static int ks0127_status(struct v4l2_subdev *sd, u32 *pstatus, v4l2_std_id *pstd)
+{
+	int stat = V4L2_IN_ST_NO_SIGNAL;
+	u8 status;
+	v4l2_std_id std = pstd ? *pstd : V4L2_STD_ALL;
+
+	status = ks0127_read(sd, KS_STAT);
+	if (!(status & 0x20))		 /* NOVID not set */
+		stat = 0;
+	if (!(status & 0x01)) {		      /* CLOCK set */
+		stat |= V4L2_IN_ST_NO_COLOR;
+		std = V4L2_STD_UNKNOWN;
+	} else {
+		if ((status & 0x08))		   /* PALDET set */
+			std &= V4L2_STD_PAL;
+		else
+			std &= V4L2_STD_NTSC;
+	}
+	if ((status & 0x10))		   /* PALDET set */
+		std &= V4L2_STD_525_60;
+	else
+		std &= V4L2_STD_625_50;
+	if (pstd)
+		*pstd = std;
+	if (pstatus)
+		*pstatus = stat;
+	return 0;
+}
+
+static int ks0127_querystd(struct v4l2_subdev *sd, v4l2_std_id *std)
+{
+	v4l2_dbg(1, debug, sd, "querystd\n");
+	return ks0127_status(sd, NULL, std);
+}
+
+static int ks0127_g_input_status(struct v4l2_subdev *sd, u32 *status)
+{
+	v4l2_dbg(1, debug, sd, "g_input_status\n");
+	return ks0127_status(sd, status, NULL);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_video_ops ks0127_video_ops = {
+	.s_std = ks0127_s_std,
+	.s_routing = ks0127_s_routing,
+	.s_stream = ks0127_s_stream,
+	.querystd = ks0127_querystd,
+	.g_input_status = ks0127_g_input_status,
+};
+
+static const struct v4l2_subdev_ops ks0127_ops = {
+	.video = &ks0127_video_ops,
+};
+
+/* ----------------------------------------------------------------------- */
+
+
+static int ks0127_probe(struct i2c_client *client)
+{
+	struct ks0127 *ks;
+	struct v4l2_subdev *sd;
+
+	v4l_info(client, "%s chip found @ 0x%x (%s)\n",
+		client->addr == (I2C_KS0127_ADDON >> 1) ? "addon" : "on-board",
+		client->addr << 1, client->adapter->name);
+
+	ks = devm_kzalloc(&client->dev, sizeof(*ks), GFP_KERNEL);
+	if (ks == NULL)
+		return -ENOMEM;
+	sd = &ks->sd;
+	v4l2_i2c_subdev_init(sd, client, &ks0127_ops);
+
+	/* power up */
+	init_reg_defaults();
+	ks0127_write(sd, KS_CMDA, 0x2c);
+	mdelay(10);
+
+	/* reset the device */
+	ks0127_init(sd);
+	return 0;
+}
+
+static void ks0127_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	ks0127_write(sd, KS_OFMTA, 0x20); /* tristate */
+	ks0127_write(sd, KS_CMDA, 0x2c | 0x80); /* power down */
+}
+
+static const struct i2c_device_id ks0127_id[] = {
+	{ "ks0127", 0 },
+	{ "ks0127b", 0 },
+	{ "ks0122s", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ks0127_id);
+
+static struct i2c_driver ks0127_driver = {
+	.driver = {
+		.name	= "ks0127",
+	},
+	.probe		= ks0127_probe,
+	.remove		= ks0127_remove,
+	.id_table	= ks0127_id,
+};
+
+module_i2c_driver(ks0127_driver);
diff --git a/drivers/media/i2c/ks0127.h b/drivers/media/i2c/ks0127.h
new file mode 100644
index 000000000..333d1d1e9
--- /dev/null
+++ b/drivers/media/i2c/ks0127.h
@@ -0,0 +1,38 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+ * Video Capture Driver ( Video for Linux 1/2 )
+ * for the Matrox Marvel G200,G400 and Rainbow Runner-G series
+ *
+ * This module is an interface to the KS0127 video decoder chip.
+ *
+ * Copyright (C) 1999  Ryan Drake <stiletto@mediaone.net>
+ */
+
+#ifndef KS0127_H
+#define KS0127_H
+
+/* input channels */
+#define KS_INPUT_COMPOSITE_1    0
+#define KS_INPUT_COMPOSITE_2    1
+#define KS_INPUT_COMPOSITE_3    2
+#define KS_INPUT_COMPOSITE_4    4
+#define KS_INPUT_COMPOSITE_5    5
+#define KS_INPUT_COMPOSITE_6    6
+
+#define KS_INPUT_SVIDEO_1       8
+#define KS_INPUT_SVIDEO_2       9
+#define KS_INPUT_SVIDEO_3       10
+
+#define KS_INPUT_YUV656		15
+#define KS_INPUT_COUNT          10
+
+/* output channels */
+#define KS_OUTPUT_YUV656E       0
+#define KS_OUTPUT_EXV           1
+
+/* video standards */
+#define KS_STD_NTSC_N           112       /* 50 Hz NTSC */
+#define KS_STD_PAL_M            113       /* 60 Hz PAL  */
+
+#endif /* KS0127_H */
+
diff --git a/drivers/media/i2c/lm3560.c b/drivers/media/i2c/lm3560.c
new file mode 100644
index 000000000..05283ac68
--- /dev/null
+++ b/drivers/media/i2c/lm3560.c
@@ -0,0 +1,480 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * drivers/media/i2c/lm3560.c
+ * General device driver for TI lm3559, lm3560, FLASH LED Driver
+ *
+ * Copyright (C) 2013 Texas Instruments
+ *
+ * Contact: Daniel Jeong <gshark.jeong@gmail.com>
+ *			Ldd-Mlp <ldd-mlp@list.ti.com>
+ */
+
+#include <linux/delay.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/mutex.h>
+#include <linux/regmap.h>
+#include <linux/videodev2.h>
+#include <media/i2c/lm3560.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+
+/* registers definitions */
+#define REG_ENABLE		0x10
+#define REG_TORCH_BR	0xa0
+#define REG_FLASH_BR	0xb0
+#define REG_FLASH_TOUT	0xc0
+#define REG_FLAG		0xd0
+#define REG_CONFIG1		0xe0
+
+/* fault mask */
+#define FAULT_TIMEOUT	(1<<0)
+#define FAULT_OVERTEMP	(1<<1)
+#define FAULT_SHORT_CIRCUIT	(1<<2)
+
+enum led_enable {
+	MODE_SHDN = 0x0,
+	MODE_TORCH = 0x2,
+	MODE_FLASH = 0x3,
+};
+
+/**
+ * struct lm3560_flash
+ *
+ * @dev: pointer to &struct device
+ * @pdata: platform data
+ * @regmap: reg. map for i2c
+ * @lock: muxtex for serial access.
+ * @led_mode: V4L2 LED mode
+ * @ctrls_led: V4L2 controls
+ * @subdev_led: V4L2 subdev
+ */
+struct lm3560_flash {
+	struct device *dev;
+	struct lm3560_platform_data *pdata;
+	struct regmap *regmap;
+	struct mutex lock;
+
+	enum v4l2_flash_led_mode led_mode;
+	struct v4l2_ctrl_handler ctrls_led[LM3560_LED_MAX];
+	struct v4l2_subdev subdev_led[LM3560_LED_MAX];
+};
+
+#define to_lm3560_flash(_ctrl, _no)	\
+	container_of(_ctrl->handler, struct lm3560_flash, ctrls_led[_no])
+
+/* enable mode control */
+static int lm3560_mode_ctrl(struct lm3560_flash *flash)
+{
+	int rval = -EINVAL;
+
+	switch (flash->led_mode) {
+	case V4L2_FLASH_LED_MODE_NONE:
+		rval = regmap_update_bits(flash->regmap,
+					  REG_ENABLE, 0x03, MODE_SHDN);
+		break;
+	case V4L2_FLASH_LED_MODE_TORCH:
+		rval = regmap_update_bits(flash->regmap,
+					  REG_ENABLE, 0x03, MODE_TORCH);
+		break;
+	case V4L2_FLASH_LED_MODE_FLASH:
+		rval = regmap_update_bits(flash->regmap,
+					  REG_ENABLE, 0x03, MODE_FLASH);
+		break;
+	}
+	return rval;
+}
+
+/* led1/2 enable/disable */
+static int lm3560_enable_ctrl(struct lm3560_flash *flash,
+			      enum lm3560_led_id led_no, bool on)
+{
+	int rval;
+
+	if (led_no == LM3560_LED0) {
+		if (on)
+			rval = regmap_update_bits(flash->regmap,
+						  REG_ENABLE, 0x08, 0x08);
+		else
+			rval = regmap_update_bits(flash->regmap,
+						  REG_ENABLE, 0x08, 0x00);
+	} else {
+		if (on)
+			rval = regmap_update_bits(flash->regmap,
+						  REG_ENABLE, 0x10, 0x10);
+		else
+			rval = regmap_update_bits(flash->regmap,
+						  REG_ENABLE, 0x10, 0x00);
+	}
+	return rval;
+}
+
+/* torch1/2 brightness control */
+static int lm3560_torch_brt_ctrl(struct lm3560_flash *flash,
+				 enum lm3560_led_id led_no, unsigned int brt)
+{
+	int rval;
+	u8 br_bits;
+
+	if (brt < LM3560_TORCH_BRT_MIN)
+		return lm3560_enable_ctrl(flash, led_no, false);
+	else
+		rval = lm3560_enable_ctrl(flash, led_no, true);
+
+	br_bits = LM3560_TORCH_BRT_uA_TO_REG(brt);
+	if (led_no == LM3560_LED0)
+		rval = regmap_update_bits(flash->regmap,
+					  REG_TORCH_BR, 0x07, br_bits);
+	else
+		rval = regmap_update_bits(flash->regmap,
+					  REG_TORCH_BR, 0x38, br_bits << 3);
+
+	return rval;
+}
+
+/* flash1/2 brightness control */
+static int lm3560_flash_brt_ctrl(struct lm3560_flash *flash,
+				 enum lm3560_led_id led_no, unsigned int brt)
+{
+	int rval;
+	u8 br_bits;
+
+	if (brt < LM3560_FLASH_BRT_MIN)
+		return lm3560_enable_ctrl(flash, led_no, false);
+	else
+		rval = lm3560_enable_ctrl(flash, led_no, true);
+
+	br_bits = LM3560_FLASH_BRT_uA_TO_REG(brt);
+	if (led_no == LM3560_LED0)
+		rval = regmap_update_bits(flash->regmap,
+					  REG_FLASH_BR, 0x0f, br_bits);
+	else
+		rval = regmap_update_bits(flash->regmap,
+					  REG_FLASH_BR, 0xf0, br_bits << 4);
+
+	return rval;
+}
+
+/* v4l2 controls  */
+static int lm3560_get_ctrl(struct v4l2_ctrl *ctrl, enum lm3560_led_id led_no)
+{
+	struct lm3560_flash *flash = to_lm3560_flash(ctrl, led_no);
+	int rval = -EINVAL;
+
+	mutex_lock(&flash->lock);
+
+	if (ctrl->id == V4L2_CID_FLASH_FAULT) {
+		s32 fault = 0;
+		unsigned int reg_val;
+		rval = regmap_read(flash->regmap, REG_FLAG, &reg_val);
+		if (rval < 0)
+			goto out;
+		if (reg_val & FAULT_SHORT_CIRCUIT)
+			fault |= V4L2_FLASH_FAULT_SHORT_CIRCUIT;
+		if (reg_val & FAULT_OVERTEMP)
+			fault |= V4L2_FLASH_FAULT_OVER_TEMPERATURE;
+		if (reg_val & FAULT_TIMEOUT)
+			fault |= V4L2_FLASH_FAULT_TIMEOUT;
+		ctrl->cur.val = fault;
+	}
+
+out:
+	mutex_unlock(&flash->lock);
+	return rval;
+}
+
+static int lm3560_set_ctrl(struct v4l2_ctrl *ctrl, enum lm3560_led_id led_no)
+{
+	struct lm3560_flash *flash = to_lm3560_flash(ctrl, led_no);
+	u8 tout_bits;
+	int rval = -EINVAL;
+
+	mutex_lock(&flash->lock);
+
+	switch (ctrl->id) {
+	case V4L2_CID_FLASH_LED_MODE:
+		flash->led_mode = ctrl->val;
+		if (flash->led_mode != V4L2_FLASH_LED_MODE_FLASH)
+			rval = lm3560_mode_ctrl(flash);
+		break;
+
+	case V4L2_CID_FLASH_STROBE_SOURCE:
+		rval = regmap_update_bits(flash->regmap,
+					  REG_CONFIG1, 0x04, (ctrl->val) << 2);
+		if (rval < 0)
+			goto err_out;
+		break;
+
+	case V4L2_CID_FLASH_STROBE:
+		if (flash->led_mode != V4L2_FLASH_LED_MODE_FLASH) {
+			rval = -EBUSY;
+			goto err_out;
+		}
+		flash->led_mode = V4L2_FLASH_LED_MODE_FLASH;
+		rval = lm3560_mode_ctrl(flash);
+		break;
+
+	case V4L2_CID_FLASH_STROBE_STOP:
+		if (flash->led_mode != V4L2_FLASH_LED_MODE_FLASH) {
+			rval = -EBUSY;
+			goto err_out;
+		}
+		flash->led_mode = V4L2_FLASH_LED_MODE_NONE;
+		rval = lm3560_mode_ctrl(flash);
+		break;
+
+	case V4L2_CID_FLASH_TIMEOUT:
+		tout_bits = LM3560_FLASH_TOUT_ms_TO_REG(ctrl->val);
+		rval = regmap_update_bits(flash->regmap,
+					  REG_FLASH_TOUT, 0x1f, tout_bits);
+		break;
+
+	case V4L2_CID_FLASH_INTENSITY:
+		rval = lm3560_flash_brt_ctrl(flash, led_no, ctrl->val);
+		break;
+
+	case V4L2_CID_FLASH_TORCH_INTENSITY:
+		rval = lm3560_torch_brt_ctrl(flash, led_no, ctrl->val);
+		break;
+	}
+
+err_out:
+	mutex_unlock(&flash->lock);
+	return rval;
+}
+
+static int lm3560_led1_get_ctrl(struct v4l2_ctrl *ctrl)
+{
+	return lm3560_get_ctrl(ctrl, LM3560_LED1);
+}
+
+static int lm3560_led1_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	return lm3560_set_ctrl(ctrl, LM3560_LED1);
+}
+
+static int lm3560_led0_get_ctrl(struct v4l2_ctrl *ctrl)
+{
+	return lm3560_get_ctrl(ctrl, LM3560_LED0);
+}
+
+static int lm3560_led0_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	return lm3560_set_ctrl(ctrl, LM3560_LED0);
+}
+
+static const struct v4l2_ctrl_ops lm3560_led_ctrl_ops[LM3560_LED_MAX] = {
+	[LM3560_LED0] = {
+			 .g_volatile_ctrl = lm3560_led0_get_ctrl,
+			 .s_ctrl = lm3560_led0_set_ctrl,
+			 },
+	[LM3560_LED1] = {
+			 .g_volatile_ctrl = lm3560_led1_get_ctrl,
+			 .s_ctrl = lm3560_led1_set_ctrl,
+			 }
+};
+
+static int lm3560_init_controls(struct lm3560_flash *flash,
+				enum lm3560_led_id led_no)
+{
+	struct v4l2_ctrl *fault;
+	u32 max_flash_brt = flash->pdata->max_flash_brt[led_no];
+	u32 max_torch_brt = flash->pdata->max_torch_brt[led_no];
+	struct v4l2_ctrl_handler *hdl = &flash->ctrls_led[led_no];
+	const struct v4l2_ctrl_ops *ops = &lm3560_led_ctrl_ops[led_no];
+
+	v4l2_ctrl_handler_init(hdl, 8);
+
+	/* flash mode */
+	v4l2_ctrl_new_std_menu(hdl, ops, V4L2_CID_FLASH_LED_MODE,
+			       V4L2_FLASH_LED_MODE_TORCH, ~0x7,
+			       V4L2_FLASH_LED_MODE_NONE);
+	flash->led_mode = V4L2_FLASH_LED_MODE_NONE;
+
+	/* flash source */
+	v4l2_ctrl_new_std_menu(hdl, ops, V4L2_CID_FLASH_STROBE_SOURCE,
+			       0x1, ~0x3, V4L2_FLASH_STROBE_SOURCE_SOFTWARE);
+
+	/* flash strobe */
+	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_FLASH_STROBE, 0, 0, 0, 0);
+
+	/* flash strobe stop */
+	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_FLASH_STROBE_STOP, 0, 0, 0, 0);
+
+	/* flash strobe timeout */
+	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_FLASH_TIMEOUT,
+			  LM3560_FLASH_TOUT_MIN,
+			  flash->pdata->max_flash_timeout,
+			  LM3560_FLASH_TOUT_STEP,
+			  flash->pdata->max_flash_timeout);
+
+	/* flash brt */
+	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_FLASH_INTENSITY,
+			  LM3560_FLASH_BRT_MIN, max_flash_brt,
+			  LM3560_FLASH_BRT_STEP, max_flash_brt);
+
+	/* torch brt */
+	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_FLASH_TORCH_INTENSITY,
+			  LM3560_TORCH_BRT_MIN, max_torch_brt,
+			  LM3560_TORCH_BRT_STEP, max_torch_brt);
+
+	/* fault */
+	fault = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_FLASH_FAULT, 0,
+				  V4L2_FLASH_FAULT_OVER_VOLTAGE
+				  | V4L2_FLASH_FAULT_OVER_TEMPERATURE
+				  | V4L2_FLASH_FAULT_SHORT_CIRCUIT
+				  | V4L2_FLASH_FAULT_TIMEOUT, 0, 0);
+	if (fault != NULL)
+		fault->flags |= V4L2_CTRL_FLAG_VOLATILE;
+
+	if (hdl->error)
+		return hdl->error;
+
+	flash->subdev_led[led_no].ctrl_handler = hdl;
+	return 0;
+}
+
+/* initialize device */
+static const struct v4l2_subdev_ops lm3560_ops = {
+	.core = NULL,
+};
+
+static const struct regmap_config lm3560_regmap = {
+	.reg_bits = 8,
+	.val_bits = 8,
+	.max_register = 0xFF,
+};
+
+static int lm3560_subdev_init(struct lm3560_flash *flash,
+			      enum lm3560_led_id led_no, char *led_name)
+{
+	struct i2c_client *client = to_i2c_client(flash->dev);
+	int rval;
+
+	v4l2_i2c_subdev_init(&flash->subdev_led[led_no], client, &lm3560_ops);
+	flash->subdev_led[led_no].flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	strscpy(flash->subdev_led[led_no].name, led_name,
+		sizeof(flash->subdev_led[led_no].name));
+	rval = lm3560_init_controls(flash, led_no);
+	if (rval)
+		goto err_out;
+	rval = media_entity_pads_init(&flash->subdev_led[led_no].entity, 0, NULL);
+	if (rval < 0)
+		goto err_out;
+	flash->subdev_led[led_no].entity.function = MEDIA_ENT_F_FLASH;
+
+	return rval;
+
+err_out:
+	v4l2_ctrl_handler_free(&flash->ctrls_led[led_no]);
+	return rval;
+}
+
+static int lm3560_init_device(struct lm3560_flash *flash)
+{
+	int rval;
+	unsigned int reg_val;
+
+	/* set peak current */
+	rval = regmap_update_bits(flash->regmap,
+				  REG_FLASH_TOUT, 0x60, flash->pdata->peak);
+	if (rval < 0)
+		return rval;
+	/* output disable */
+	flash->led_mode = V4L2_FLASH_LED_MODE_NONE;
+	rval = lm3560_mode_ctrl(flash);
+	if (rval < 0)
+		return rval;
+	/* reset faults */
+	rval = regmap_read(flash->regmap, REG_FLAG, &reg_val);
+	return rval;
+}
+
+static int lm3560_probe(struct i2c_client *client)
+{
+	struct lm3560_flash *flash;
+	struct lm3560_platform_data *pdata = dev_get_platdata(&client->dev);
+	int rval;
+
+	flash = devm_kzalloc(&client->dev, sizeof(*flash), GFP_KERNEL);
+	if (flash == NULL)
+		return -ENOMEM;
+
+	flash->regmap = devm_regmap_init_i2c(client, &lm3560_regmap);
+	if (IS_ERR(flash->regmap)) {
+		rval = PTR_ERR(flash->regmap);
+		return rval;
+	}
+
+	/* if there is no platform data, use chip default value */
+	if (pdata == NULL) {
+		pdata = devm_kzalloc(&client->dev, sizeof(*pdata), GFP_KERNEL);
+		if (pdata == NULL)
+			return -ENODEV;
+		pdata->peak = LM3560_PEAK_3600mA;
+		pdata->max_flash_timeout = LM3560_FLASH_TOUT_MAX;
+		/* led 1 */
+		pdata->max_flash_brt[LM3560_LED0] = LM3560_FLASH_BRT_MAX;
+		pdata->max_torch_brt[LM3560_LED0] = LM3560_TORCH_BRT_MAX;
+		/* led 2 */
+		pdata->max_flash_brt[LM3560_LED1] = LM3560_FLASH_BRT_MAX;
+		pdata->max_torch_brt[LM3560_LED1] = LM3560_TORCH_BRT_MAX;
+	}
+	flash->pdata = pdata;
+	flash->dev = &client->dev;
+	mutex_init(&flash->lock);
+
+	rval = lm3560_subdev_init(flash, LM3560_LED0, "lm3560-led0");
+	if (rval < 0)
+		return rval;
+
+	rval = lm3560_subdev_init(flash, LM3560_LED1, "lm3560-led1");
+	if (rval < 0)
+		return rval;
+
+	rval = lm3560_init_device(flash);
+	if (rval < 0)
+		return rval;
+
+	i2c_set_clientdata(client, flash);
+
+	return 0;
+}
+
+static void lm3560_remove(struct i2c_client *client)
+{
+	struct lm3560_flash *flash = i2c_get_clientdata(client);
+	unsigned int i;
+
+	for (i = LM3560_LED0; i < LM3560_LED_MAX; i++) {
+		v4l2_device_unregister_subdev(&flash->subdev_led[i]);
+		v4l2_ctrl_handler_free(&flash->ctrls_led[i]);
+		media_entity_cleanup(&flash->subdev_led[i].entity);
+	}
+}
+
+static const struct i2c_device_id lm3560_id_table[] = {
+	{LM3559_NAME, 0},
+	{LM3560_NAME, 0},
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, lm3560_id_table);
+
+static struct i2c_driver lm3560_i2c_driver = {
+	.driver = {
+		   .name = LM3560_NAME,
+		   .pm = NULL,
+		   },
+	.probe = lm3560_probe,
+	.remove = lm3560_remove,
+	.id_table = lm3560_id_table,
+};
+
+module_i2c_driver(lm3560_i2c_driver);
+
+MODULE_AUTHOR("Daniel Jeong <gshark.jeong@gmail.com>");
+MODULE_AUTHOR("Ldd Mlp <ldd-mlp@list.ti.com>");
+MODULE_DESCRIPTION("Texas Instruments LM3560 LED flash driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/lm3646.c b/drivers/media/i2c/lm3646.c
new file mode 100644
index 000000000..fab3a7e05
--- /dev/null
+++ b/drivers/media/i2c/lm3646.c
@@ -0,0 +1,409 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * drivers/media/i2c/lm3646.c
+ * General device driver for TI lm3646, Dual FLASH LED Driver
+ *
+ * Copyright (C) 2014 Texas Instruments
+ *
+ * Contact: Daniel Jeong <gshark.jeong@gmail.com>
+ *			Ldd-Mlp <ldd-mlp@list.ti.com>
+ */
+
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/regmap.h>
+#include <linux/videodev2.h>
+#include <media/i2c/lm3646.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+
+/* registers definitions */
+#define REG_ENABLE		0x01
+#define REG_TORCH_BR	0x05
+#define REG_FLASH_BR	0x05
+#define REG_FLASH_TOUT	0x04
+#define REG_FLAG		0x08
+#define REG_STROBE_SRC	0x06
+#define REG_LED1_FLASH_BR 0x06
+#define REG_LED1_TORCH_BR 0x07
+
+#define MASK_ENABLE		0x03
+#define MASK_TORCH_BR	0x70
+#define MASK_FLASH_BR	0x0F
+#define MASK_FLASH_TOUT	0x07
+#define MASK_FLAG		0xFF
+#define MASK_STROBE_SRC	0x80
+
+/* Fault Mask */
+#define FAULT_TIMEOUT	(1<<0)
+#define FAULT_SHORT_CIRCUIT	(1<<1)
+#define FAULT_UVLO		(1<<2)
+#define FAULT_IVFM		(1<<3)
+#define FAULT_OCP		(1<<4)
+#define FAULT_OVERTEMP	(1<<5)
+#define FAULT_NTC_TRIP	(1<<6)
+#define FAULT_OVP		(1<<7)
+
+enum led_mode {
+	MODE_SHDN = 0x0,
+	MODE_TORCH = 0x2,
+	MODE_FLASH = 0x3,
+};
+
+/*
+ * struct lm3646_flash
+ *
+ * @pdata: platform data
+ * @regmap: reg. map for i2c
+ * @lock: muxtex for serial access.
+ * @led_mode: V4L2 LED mode
+ * @ctrls_led: V4L2 controls
+ * @subdev_led: V4L2 subdev
+ * @mode_reg : mode register value
+ */
+struct lm3646_flash {
+	struct device *dev;
+	struct lm3646_platform_data *pdata;
+	struct regmap *regmap;
+
+	struct v4l2_ctrl_handler ctrls_led;
+	struct v4l2_subdev subdev_led;
+
+	u8 mode_reg;
+};
+
+#define to_lm3646_flash(_ctrl)	\
+	container_of(_ctrl->handler, struct lm3646_flash, ctrls_led)
+
+/* enable mode control */
+static int lm3646_mode_ctrl(struct lm3646_flash *flash,
+			    enum v4l2_flash_led_mode led_mode)
+{
+	switch (led_mode) {
+	case V4L2_FLASH_LED_MODE_NONE:
+		return regmap_write(flash->regmap,
+				    REG_ENABLE, flash->mode_reg | MODE_SHDN);
+	case V4L2_FLASH_LED_MODE_TORCH:
+		return regmap_write(flash->regmap,
+				    REG_ENABLE, flash->mode_reg | MODE_TORCH);
+	case V4L2_FLASH_LED_MODE_FLASH:
+		return regmap_write(flash->regmap,
+				    REG_ENABLE, flash->mode_reg | MODE_FLASH);
+	}
+	return -EINVAL;
+}
+
+/* V4L2 controls  */
+static int lm3646_get_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct lm3646_flash *flash = to_lm3646_flash(ctrl);
+	unsigned int reg_val;
+	int rval;
+
+	if (ctrl->id != V4L2_CID_FLASH_FAULT)
+		return -EINVAL;
+
+	rval = regmap_read(flash->regmap, REG_FLAG, &reg_val);
+	if (rval < 0)
+		return rval;
+
+	ctrl->val = 0;
+	if (reg_val & FAULT_TIMEOUT)
+		ctrl->val |= V4L2_FLASH_FAULT_TIMEOUT;
+	if (reg_val & FAULT_SHORT_CIRCUIT)
+		ctrl->val |= V4L2_FLASH_FAULT_SHORT_CIRCUIT;
+	if (reg_val & FAULT_UVLO)
+		ctrl->val |= V4L2_FLASH_FAULT_UNDER_VOLTAGE;
+	if (reg_val & FAULT_IVFM)
+		ctrl->val |= V4L2_FLASH_FAULT_INPUT_VOLTAGE;
+	if (reg_val & FAULT_OCP)
+		ctrl->val |= V4L2_FLASH_FAULT_OVER_CURRENT;
+	if (reg_val & FAULT_OVERTEMP)
+		ctrl->val |= V4L2_FLASH_FAULT_OVER_TEMPERATURE;
+	if (reg_val & FAULT_NTC_TRIP)
+		ctrl->val |= V4L2_FLASH_FAULT_LED_OVER_TEMPERATURE;
+	if (reg_val & FAULT_OVP)
+		ctrl->val |= V4L2_FLASH_FAULT_OVER_VOLTAGE;
+
+	return 0;
+}
+
+static int lm3646_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct lm3646_flash *flash = to_lm3646_flash(ctrl);
+	unsigned int reg_val;
+	int rval;
+
+	switch (ctrl->id) {
+	case V4L2_CID_FLASH_LED_MODE:
+
+		if (ctrl->val != V4L2_FLASH_LED_MODE_FLASH)
+			return lm3646_mode_ctrl(flash, ctrl->val);
+		/* switch to SHDN mode before flash strobe on */
+		return lm3646_mode_ctrl(flash, V4L2_FLASH_LED_MODE_NONE);
+
+	case V4L2_CID_FLASH_STROBE_SOURCE:
+		return regmap_update_bits(flash->regmap,
+					  REG_STROBE_SRC, MASK_STROBE_SRC,
+					  (ctrl->val) << 7);
+
+	case V4L2_CID_FLASH_STROBE:
+
+		/* read and check current mode of chip to start flash */
+		rval = regmap_read(flash->regmap, REG_ENABLE, &reg_val);
+		if (rval < 0 || ((reg_val & MASK_ENABLE) != MODE_SHDN))
+			return rval;
+		/* flash on */
+		return lm3646_mode_ctrl(flash, V4L2_FLASH_LED_MODE_FLASH);
+
+	case V4L2_CID_FLASH_STROBE_STOP:
+
+		/*
+		 * flash mode will be turned automatically
+		 * from FLASH mode to SHDN mode after flash duration timeout
+		 * read and check current mode of chip to stop flash
+		 */
+		rval = regmap_read(flash->regmap, REG_ENABLE, &reg_val);
+		if (rval < 0)
+			return rval;
+		if ((reg_val & MASK_ENABLE) == MODE_FLASH)
+			return lm3646_mode_ctrl(flash,
+						V4L2_FLASH_LED_MODE_NONE);
+		return rval;
+
+	case V4L2_CID_FLASH_TIMEOUT:
+		return regmap_update_bits(flash->regmap,
+					  REG_FLASH_TOUT, MASK_FLASH_TOUT,
+					  LM3646_FLASH_TOUT_ms_TO_REG
+					  (ctrl->val));
+
+	case V4L2_CID_FLASH_INTENSITY:
+		return regmap_update_bits(flash->regmap,
+					  REG_FLASH_BR, MASK_FLASH_BR,
+					  LM3646_TOTAL_FLASH_BRT_uA_TO_REG
+					  (ctrl->val));
+
+	case V4L2_CID_FLASH_TORCH_INTENSITY:
+		return regmap_update_bits(flash->regmap,
+					  REG_TORCH_BR, MASK_TORCH_BR,
+					  LM3646_TOTAL_TORCH_BRT_uA_TO_REG
+					  (ctrl->val) << 4);
+	}
+
+	return -EINVAL;
+}
+
+static const struct v4l2_ctrl_ops lm3646_led_ctrl_ops = {
+	.g_volatile_ctrl = lm3646_get_ctrl,
+	.s_ctrl = lm3646_set_ctrl,
+};
+
+static int lm3646_init_controls(struct lm3646_flash *flash)
+{
+	struct v4l2_ctrl *fault;
+	struct v4l2_ctrl_handler *hdl = &flash->ctrls_led;
+	const struct v4l2_ctrl_ops *ops = &lm3646_led_ctrl_ops;
+
+	v4l2_ctrl_handler_init(hdl, 8);
+	/* flash mode */
+	v4l2_ctrl_new_std_menu(hdl, ops, V4L2_CID_FLASH_LED_MODE,
+			       V4L2_FLASH_LED_MODE_TORCH, ~0x7,
+			       V4L2_FLASH_LED_MODE_NONE);
+
+	/* flash source */
+	v4l2_ctrl_new_std_menu(hdl, ops, V4L2_CID_FLASH_STROBE_SOURCE,
+			       0x1, ~0x3, V4L2_FLASH_STROBE_SOURCE_SOFTWARE);
+
+	/* flash strobe */
+	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_FLASH_STROBE, 0, 0, 0, 0);
+	/* flash strobe stop */
+	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_FLASH_STROBE_STOP, 0, 0, 0, 0);
+
+	/* flash strobe timeout */
+	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_FLASH_TIMEOUT,
+			  LM3646_FLASH_TOUT_MIN,
+			  LM3646_FLASH_TOUT_MAX,
+			  LM3646_FLASH_TOUT_STEP, flash->pdata->flash_timeout);
+
+	/* max flash current */
+	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_FLASH_INTENSITY,
+			  LM3646_TOTAL_FLASH_BRT_MIN,
+			  LM3646_TOTAL_FLASH_BRT_MAX,
+			  LM3646_TOTAL_FLASH_BRT_STEP,
+			  LM3646_TOTAL_FLASH_BRT_MAX);
+
+	/* max torch current */
+	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_FLASH_TORCH_INTENSITY,
+			  LM3646_TOTAL_TORCH_BRT_MIN,
+			  LM3646_TOTAL_TORCH_BRT_MAX,
+			  LM3646_TOTAL_TORCH_BRT_STEP,
+			  LM3646_TOTAL_TORCH_BRT_MAX);
+
+	/* fault */
+	fault = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_FLASH_FAULT, 0,
+				  V4L2_FLASH_FAULT_OVER_VOLTAGE
+				  | V4L2_FLASH_FAULT_OVER_TEMPERATURE
+				  | V4L2_FLASH_FAULT_SHORT_CIRCUIT
+				  | V4L2_FLASH_FAULT_TIMEOUT, 0, 0);
+	if (fault != NULL)
+		fault->flags |= V4L2_CTRL_FLAG_VOLATILE;
+
+	if (hdl->error)
+		return hdl->error;
+
+	flash->subdev_led.ctrl_handler = hdl;
+	return 0;
+}
+
+/* initialize device */
+static const struct v4l2_subdev_ops lm3646_ops = {
+	.core = NULL,
+};
+
+static const struct regmap_config lm3646_regmap = {
+	.reg_bits = 8,
+	.val_bits = 8,
+	.max_register = 0xFF,
+};
+
+static int lm3646_subdev_init(struct lm3646_flash *flash)
+{
+	struct i2c_client *client = to_i2c_client(flash->dev);
+	int rval;
+
+	v4l2_i2c_subdev_init(&flash->subdev_led, client, &lm3646_ops);
+	flash->subdev_led.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	strscpy(flash->subdev_led.name, LM3646_NAME,
+		sizeof(flash->subdev_led.name));
+	rval = lm3646_init_controls(flash);
+	if (rval)
+		goto err_out;
+	rval = media_entity_pads_init(&flash->subdev_led.entity, 0, NULL);
+	if (rval < 0)
+		goto err_out;
+	flash->subdev_led.entity.function = MEDIA_ENT_F_FLASH;
+	return rval;
+
+err_out:
+	v4l2_ctrl_handler_free(&flash->ctrls_led);
+	return rval;
+}
+
+static int lm3646_init_device(struct lm3646_flash *flash)
+{
+	unsigned int reg_val;
+	int rval;
+
+	/* read the value of mode register to reduce redundant i2c accesses */
+	rval = regmap_read(flash->regmap, REG_ENABLE, &reg_val);
+	if (rval < 0)
+		return rval;
+	flash->mode_reg = reg_val & 0xfc;
+
+	/* output disable */
+	rval = lm3646_mode_ctrl(flash, V4L2_FLASH_LED_MODE_NONE);
+	if (rval < 0)
+		return rval;
+
+	/*
+	 * LED1 flash current setting
+	 * LED2 flash current = Total(Max) flash current - LED1 flash current
+	 */
+	rval = regmap_update_bits(flash->regmap,
+				  REG_LED1_FLASH_BR, 0x7F,
+				  LM3646_LED1_FLASH_BRT_uA_TO_REG
+				  (flash->pdata->led1_flash_brt));
+
+	if (rval < 0)
+		return rval;
+
+	/*
+	 * LED1 torch current setting
+	 * LED2 torch current = Total(Max) torch current - LED1 torch current
+	 */
+	rval = regmap_update_bits(flash->regmap,
+				  REG_LED1_TORCH_BR, 0x7F,
+				  LM3646_LED1_TORCH_BRT_uA_TO_REG
+				  (flash->pdata->led1_torch_brt));
+	if (rval < 0)
+		return rval;
+
+	/* Reset flag register */
+	return regmap_read(flash->regmap, REG_FLAG, &reg_val);
+}
+
+static int lm3646_probe(struct i2c_client *client)
+{
+	struct lm3646_flash *flash;
+	struct lm3646_platform_data *pdata = dev_get_platdata(&client->dev);
+	int rval;
+
+	flash = devm_kzalloc(&client->dev, sizeof(*flash), GFP_KERNEL);
+	if (flash == NULL)
+		return -ENOMEM;
+
+	flash->regmap = devm_regmap_init_i2c(client, &lm3646_regmap);
+	if (IS_ERR(flash->regmap))
+		return PTR_ERR(flash->regmap);
+
+	/* check device tree if there is no platform data */
+	if (pdata == NULL) {
+		pdata = devm_kzalloc(&client->dev,
+				     sizeof(struct lm3646_platform_data),
+				     GFP_KERNEL);
+		if (pdata == NULL)
+			return -ENOMEM;
+		/* use default data in case of no platform data */
+		pdata->flash_timeout = LM3646_FLASH_TOUT_MAX;
+		pdata->led1_torch_brt = LM3646_LED1_TORCH_BRT_MAX;
+		pdata->led1_flash_brt = LM3646_LED1_FLASH_BRT_MAX;
+	}
+	flash->pdata = pdata;
+	flash->dev = &client->dev;
+
+	rval = lm3646_subdev_init(flash);
+	if (rval < 0)
+		return rval;
+
+	rval = lm3646_init_device(flash);
+	if (rval < 0)
+		return rval;
+
+	i2c_set_clientdata(client, flash);
+
+	return 0;
+}
+
+static void lm3646_remove(struct i2c_client *client)
+{
+	struct lm3646_flash *flash = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(&flash->subdev_led);
+	v4l2_ctrl_handler_free(&flash->ctrls_led);
+	media_entity_cleanup(&flash->subdev_led.entity);
+}
+
+static const struct i2c_device_id lm3646_id_table[] = {
+	{LM3646_NAME, 0},
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, lm3646_id_table);
+
+static struct i2c_driver lm3646_i2c_driver = {
+	.driver = {
+		   .name = LM3646_NAME,
+		   },
+	.probe = lm3646_probe,
+	.remove = lm3646_remove,
+	.id_table = lm3646_id_table,
+};
+
+module_i2c_driver(lm3646_i2c_driver);
+
+MODULE_AUTHOR("Daniel Jeong <gshark.jeong@gmail.com>");
+MODULE_AUTHOR("Ldd Mlp <ldd-mlp@list.ti.com>");
+MODULE_DESCRIPTION("Texas Instruments LM3646 Dual Flash LED driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/m52790.c b/drivers/media/i2c/m52790.c
new file mode 100644
index 000000000..f8a69142a
--- /dev/null
+++ b/drivers/media/i2c/m52790.c
@@ -0,0 +1,180 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * m52790 i2c ivtv driver.
+ * Copyright (C) 2007  Hans Verkuil
+ *
+ * A/V source switching Mitsubishi M52790SP/FP
+ */
+
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <linux/ioctl.h>
+#include <linux/uaccess.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <media/i2c/m52790.h>
+#include <media/v4l2-device.h>
+
+MODULE_DESCRIPTION("i2c device driver for m52790 A/V switch");
+MODULE_AUTHOR("Hans Verkuil");
+MODULE_LICENSE("GPL");
+
+
+struct m52790_state {
+	struct v4l2_subdev sd;
+	u16 input;
+	u16 output;
+};
+
+static inline struct m52790_state *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct m52790_state, sd);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static int m52790_write(struct v4l2_subdev *sd)
+{
+	struct m52790_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	u8 sw1 = (state->input | state->output) & 0xff;
+	u8 sw2 = (state->input | state->output) >> 8;
+
+	return i2c_smbus_write_byte_data(client, sw1, sw2);
+}
+
+/* Note: audio and video are linked and cannot be switched separately.
+   So audio and video routing commands are identical for this chip.
+   In theory the video amplifier and audio modes could be handled
+   separately for the output, but that seems to be overkill right now.
+   The same holds for implementing an audio mute control, this is now
+   part of the audio output routing. The normal case is that another
+   chip takes care of the actual muting so making it part of the
+   output routing seems to be the right thing to do for now. */
+static int m52790_s_routing(struct v4l2_subdev *sd,
+			    u32 input, u32 output, u32 config)
+{
+	struct m52790_state *state = to_state(sd);
+
+	state->input = input;
+	state->output = output;
+	m52790_write(sd);
+	return 0;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int m52790_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+{
+	struct m52790_state *state = to_state(sd);
+
+	if (reg->reg != 0)
+		return -EINVAL;
+	reg->size = 1;
+	reg->val = state->input | state->output;
+	return 0;
+}
+
+static int m52790_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
+{
+	struct m52790_state *state = to_state(sd);
+
+	if (reg->reg != 0)
+		return -EINVAL;
+	state->input = reg->val & 0x0303;
+	state->output = reg->val & ~0x0303;
+	m52790_write(sd);
+	return 0;
+}
+#endif
+
+static int m52790_log_status(struct v4l2_subdev *sd)
+{
+	struct m52790_state *state = to_state(sd);
+
+	v4l2_info(sd, "Switch 1: %02x\n",
+			(state->input | state->output) & 0xff);
+	v4l2_info(sd, "Switch 2: %02x\n",
+			(state->input | state->output) >> 8);
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_core_ops m52790_core_ops = {
+	.log_status = m52790_log_status,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = m52790_g_register,
+	.s_register = m52790_s_register,
+#endif
+};
+
+static const struct v4l2_subdev_audio_ops m52790_audio_ops = {
+	.s_routing = m52790_s_routing,
+};
+
+static const struct v4l2_subdev_video_ops m52790_video_ops = {
+	.s_routing = m52790_s_routing,
+};
+
+static const struct v4l2_subdev_ops m52790_ops = {
+	.core = &m52790_core_ops,
+	.audio = &m52790_audio_ops,
+	.video = &m52790_video_ops,
+};
+
+/* ----------------------------------------------------------------------- */
+
+/* i2c implementation */
+
+static int m52790_probe(struct i2c_client *client)
+{
+	struct m52790_state *state;
+	struct v4l2_subdev *sd;
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	state = devm_kzalloc(&client->dev, sizeof(*state), GFP_KERNEL);
+	if (state == NULL)
+		return -ENOMEM;
+
+	sd = &state->sd;
+	v4l2_i2c_subdev_init(sd, client, &m52790_ops);
+	state->input = M52790_IN_TUNER;
+	state->output = M52790_OUT_STEREO;
+	m52790_write(sd);
+	return 0;
+}
+
+static void m52790_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id m52790_id[] = {
+	{ "m52790", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, m52790_id);
+
+static struct i2c_driver m52790_driver = {
+	.driver = {
+		.name	= "m52790",
+	},
+	.probe		= m52790_probe,
+	.remove		= m52790_remove,
+	.id_table	= m52790_id,
+};
+
+module_i2c_driver(m52790_driver);
diff --git a/drivers/media/i2c/max2175.c b/drivers/media/i2c/max2175.c
new file mode 100644
index 000000000..70c2a2948
--- /dev/null
+++ b/drivers/media/i2c/max2175.c
@@ -0,0 +1,1441 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Maxim Integrated MAX2175 RF to Bits tuner driver
+ *
+ * This driver & most of the hard coded values are based on the reference
+ * application delivered by Maxim for this device.
+ *
+ * Copyright (C) 2016 Maxim Integrated Products
+ * Copyright (C) 2017 Renesas Electronics Corporation
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/i2c.h>
+#include <linux/kernel.h>
+#include <linux/math64.h>
+#include <linux/max2175.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+
+#include "max2175.h"
+
+#define DRIVER_NAME "max2175"
+
+#define mxm_dbg(ctx, fmt, arg...) dev_dbg(&ctx->client->dev, fmt, ## arg)
+#define mxm_err(ctx, fmt, arg...) dev_err(&ctx->client->dev, fmt, ## arg)
+
+/* Rx mode */
+struct max2175_rxmode {
+	enum max2175_band band;		/* Associated band */
+	u32 freq;			/* Default freq in Hz */
+	u8 i2s_word_size;		/* Bit value */
+};
+
+/* Register map to define preset values */
+struct max2175_reg_map {
+	u8 idx;				/* Register index */
+	u8 val;				/* Register value */
+};
+
+static const struct max2175_rxmode eu_rx_modes[] = {
+	/* EU modes */
+	[MAX2175_EU_FM_1_2] = { MAX2175_BAND_FM, 98256000, 1 },
+	[MAX2175_DAB_1_2]   = { MAX2175_BAND_VHF, 182640000, 0 },
+};
+
+static const struct max2175_rxmode na_rx_modes[] = {
+	/* NA modes */
+	[MAX2175_NA_FM_1_0] = { MAX2175_BAND_FM, 98255520, 1 },
+	[MAX2175_NA_FM_2_0] = { MAX2175_BAND_FM, 98255520, 6 },
+};
+
+/*
+ * Preset values:
+ * Based on Maxim MAX2175 Register Table revision: 130p10
+ */
+static const u8 full_fm_eu_1p0[] = {
+	0x15, 0x04, 0xb8, 0xe3, 0x35, 0x18, 0x7c, 0x00,
+	0x00, 0x7d, 0x40, 0x08, 0x70, 0x7a, 0x88, 0x91,
+	0x61, 0x61, 0x61, 0x61, 0x5a, 0x0f, 0x34, 0x1c,
+	0x14, 0x88, 0x33, 0x02, 0x00, 0x09, 0x00, 0x65,
+	0x9f, 0x2b, 0x80, 0x00, 0x95, 0x05, 0x2c, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40,
+	0x4a, 0x08, 0xa8, 0x0e, 0x0e, 0x2f, 0x7e, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0xab, 0x5e, 0xa9,
+	0xae, 0xbb, 0x57, 0x18, 0x3b, 0x03, 0x3b, 0x64,
+	0x40, 0x60, 0x00, 0x2a, 0xbf, 0x3f, 0xff, 0x9f,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0a, 0x00,
+	0xff, 0xfc, 0xef, 0x1c, 0x40, 0x00, 0x00, 0x02,
+	0x00, 0x00, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0xac, 0x40, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x75, 0x00, 0x00,
+	0x00, 0x47, 0x00, 0x00, 0x11, 0x3f, 0x22, 0x00,
+	0xf1, 0x00, 0x41, 0x03, 0xb0, 0x00, 0x00, 0x00,
+	0x1b,
+};
+
+static const u8 full_fm_na_1p0[] = {
+	0x13, 0x08, 0x8d, 0xc0, 0x35, 0x18, 0x7d, 0x3f,
+	0x7d, 0x75, 0x40, 0x08, 0x70, 0x7a, 0x88, 0x91,
+	0x61, 0x61, 0x61, 0x61, 0x5c, 0x0f, 0x34, 0x1c,
+	0x14, 0x88, 0x33, 0x02, 0x00, 0x01, 0x00, 0x65,
+	0x9f, 0x2b, 0x80, 0x00, 0x95, 0x05, 0x2c, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40,
+	0x4a, 0x08, 0xa8, 0x0e, 0x0e, 0xaf, 0x7e, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0xab, 0x5e, 0xa9,
+	0xae, 0xbb, 0x57, 0x18, 0x3b, 0x03, 0x3b, 0x64,
+	0x40, 0x60, 0x00, 0x2a, 0xbf, 0x3f, 0xff, 0x9f,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0a, 0x00,
+	0xff, 0xfc, 0xef, 0x1c, 0x40, 0x00, 0x00, 0x02,
+	0x00, 0x00, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0xa6, 0x40, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x75, 0x00, 0x00,
+	0x00, 0x35, 0x00, 0x00, 0x11, 0x3f, 0x22, 0x00,
+	0xf1, 0x00, 0x41, 0x03, 0xb0, 0x00, 0x00, 0x00,
+	0x1b,
+};
+
+/* DAB1.2 settings */
+static const struct max2175_reg_map dab12_map[] = {
+	{ 0x01, 0x13 }, { 0x02, 0x0d }, { 0x03, 0x15 }, { 0x04, 0x55 },
+	{ 0x05, 0x0a }, { 0x06, 0xa0 }, { 0x07, 0x40 }, { 0x08, 0x00 },
+	{ 0x09, 0x00 }, { 0x0a, 0x7d }, { 0x0b, 0x4a }, { 0x0c, 0x28 },
+	{ 0x0e, 0x43 }, { 0x0f, 0xb5 }, { 0x10, 0x31 }, { 0x11, 0x9e },
+	{ 0x12, 0x68 }, { 0x13, 0x9e }, { 0x14, 0x68 }, { 0x15, 0x58 },
+	{ 0x16, 0x2f }, { 0x17, 0x3f }, { 0x18, 0x40 }, { 0x1a, 0x88 },
+	{ 0x1b, 0xaa }, { 0x1c, 0x9a }, { 0x1d, 0x00 }, { 0x1e, 0x00 },
+	{ 0x23, 0x80 }, { 0x24, 0x00 }, { 0x25, 0x00 }, { 0x26, 0x00 },
+	{ 0x27, 0x00 }, { 0x32, 0x08 }, { 0x33, 0xf8 }, { 0x36, 0x2d },
+	{ 0x37, 0x7e }, { 0x55, 0xaf }, { 0x56, 0x3f }, { 0x57, 0xf8 },
+	{ 0x58, 0x99 }, { 0x76, 0x00 }, { 0x77, 0x00 }, { 0x78, 0x02 },
+	{ 0x79, 0x40 }, { 0x82, 0x00 }, { 0x83, 0x00 }, { 0x85, 0x00 },
+	{ 0x86, 0x20 },
+};
+
+/* EU FM 1.2 settings */
+static const struct max2175_reg_map fmeu1p2_map[] = {
+	{ 0x01, 0x15 }, { 0x02, 0x04 }, { 0x03, 0xb8 }, { 0x04, 0xe3 },
+	{ 0x05, 0x35 }, { 0x06, 0x18 }, { 0x07, 0x7c }, { 0x08, 0x00 },
+	{ 0x09, 0x00 }, { 0x0a, 0x73 }, { 0x0b, 0x40 }, { 0x0c, 0x08 },
+	{ 0x0e, 0x7a }, { 0x0f, 0x88 }, { 0x10, 0x91 }, { 0x11, 0x61 },
+	{ 0x12, 0x61 }, { 0x13, 0x61 }, { 0x14, 0x61 }, { 0x15, 0x5a },
+	{ 0x16, 0x0f }, { 0x17, 0x34 }, { 0x18, 0x1c }, { 0x1a, 0x88 },
+	{ 0x1b, 0x33 }, { 0x1c, 0x02 }, { 0x1d, 0x00 }, { 0x1e, 0x01 },
+	{ 0x23, 0x80 }, { 0x24, 0x00 }, { 0x25, 0x95 }, { 0x26, 0x05 },
+	{ 0x27, 0x2c }, { 0x32, 0x08 }, { 0x33, 0xa8 }, { 0x36, 0x2f },
+	{ 0x37, 0x7e }, { 0x55, 0xbf }, { 0x56, 0x3f }, { 0x57, 0xff },
+	{ 0x58, 0x9f }, { 0x76, 0xac }, { 0x77, 0x40 }, { 0x78, 0x00 },
+	{ 0x79, 0x00 }, { 0x82, 0x47 }, { 0x83, 0x00 }, { 0x85, 0x11 },
+	{ 0x86, 0x3f },
+};
+
+/* FM NA 1.0 settings */
+static const struct max2175_reg_map fmna1p0_map[] = {
+	{ 0x01, 0x13 }, { 0x02, 0x08 }, { 0x03, 0x8d }, { 0x04, 0xc0 },
+	{ 0x05, 0x35 }, { 0x06, 0x18 }, { 0x07, 0x7d }, { 0x08, 0x3f },
+	{ 0x09, 0x7d }, { 0x0a, 0x75 }, { 0x0b, 0x40 }, { 0x0c, 0x08 },
+	{ 0x0e, 0x7a }, { 0x0f, 0x88 }, { 0x10, 0x91 }, { 0x11, 0x61 },
+	{ 0x12, 0x61 }, { 0x13, 0x61 }, { 0x14, 0x61 }, { 0x15, 0x5c },
+	{ 0x16, 0x0f }, { 0x17, 0x34 }, { 0x18, 0x1c }, { 0x1a, 0x88 },
+	{ 0x1b, 0x33 }, { 0x1c, 0x02 }, { 0x1d, 0x00 }, { 0x1e, 0x01 },
+	{ 0x23, 0x80 }, { 0x24, 0x00 }, { 0x25, 0x95 }, { 0x26, 0x05 },
+	{ 0x27, 0x2c }, { 0x32, 0x08 }, { 0x33, 0xa8 }, { 0x36, 0xaf },
+	{ 0x37, 0x7e }, { 0x55, 0xbf }, { 0x56, 0x3f }, { 0x57, 0xff },
+	{ 0x58, 0x9f }, { 0x76, 0xa6 }, { 0x77, 0x40 }, { 0x78, 0x00 },
+	{ 0x79, 0x00 }, { 0x82, 0x35 }, { 0x83, 0x00 }, { 0x85, 0x11 },
+	{ 0x86, 0x3f },
+};
+
+/* FM NA 2.0 settings */
+static const struct max2175_reg_map fmna2p0_map[] = {
+	{ 0x01, 0x13 }, { 0x02, 0x08 }, { 0x03, 0x8d }, { 0x04, 0xc0 },
+	{ 0x05, 0x35 }, { 0x06, 0x18 }, { 0x07, 0x7c }, { 0x08, 0x54 },
+	{ 0x09, 0xa7 }, { 0x0a, 0x55 }, { 0x0b, 0x42 }, { 0x0c, 0x48 },
+	{ 0x0e, 0x7a }, { 0x0f, 0x88 }, { 0x10, 0x91 }, { 0x11, 0x61 },
+	{ 0x12, 0x61 }, { 0x13, 0x61 }, { 0x14, 0x61 }, { 0x15, 0x5c },
+	{ 0x16, 0x0f }, { 0x17, 0x34 }, { 0x18, 0x1c }, { 0x1a, 0x88 },
+	{ 0x1b, 0x33 }, { 0x1c, 0x02 }, { 0x1d, 0x00 }, { 0x1e, 0x01 },
+	{ 0x23, 0x80 }, { 0x24, 0x00 }, { 0x25, 0x95 }, { 0x26, 0x05 },
+	{ 0x27, 0x2c }, { 0x32, 0x08 }, { 0x33, 0xa8 }, { 0x36, 0xaf },
+	{ 0x37, 0x7e }, { 0x55, 0xbf }, { 0x56, 0x3f }, { 0x57, 0xff },
+	{ 0x58, 0x9f }, { 0x76, 0xac }, { 0x77, 0xc0 }, { 0x78, 0x00 },
+	{ 0x79, 0x00 }, { 0x82, 0x6b }, { 0x83, 0x00 }, { 0x85, 0x11 },
+	{ 0x86, 0x3f },
+};
+
+static const u16 ch_coeff_dab1[] = {
+	0x001c, 0x0007, 0xffcd, 0x0056, 0xffa4, 0x0033, 0x0027, 0xff61,
+	0x010e, 0xfec0, 0x0106, 0xffb8, 0xff1c, 0x023c, 0xfcb2, 0x039b,
+	0xfd4e, 0x0055, 0x036a, 0xf7de, 0x0d21, 0xee72, 0x1499, 0x6a51,
+};
+
+static const u16 ch_coeff_fmeu[] = {
+	0x0000, 0xffff, 0x0001, 0x0002, 0xfffa, 0xffff, 0x0015, 0xffec,
+	0xffde, 0x0054, 0xfff9, 0xff52, 0x00b8, 0x00a2, 0xfe0a, 0x00af,
+	0x02e3, 0xfc14, 0xfe89, 0x089d, 0xfa2e, 0xf30f, 0x25be, 0x4eb6,
+};
+
+static const u16 eq_coeff_fmeu1_ra02_m6db[] = {
+	0x0040, 0xffc6, 0xfffa, 0x002c, 0x000d, 0xff90, 0x0037, 0x006e,
+	0xffc0, 0xff5b, 0x006a, 0x00f0, 0xff57, 0xfe94, 0x0112, 0x0252,
+	0xfe0c, 0xfc6a, 0x0385, 0x0553, 0xfa49, 0xf789, 0x0b91, 0x1a10,
+};
+
+static const u16 ch_coeff_fmna[] = {
+	0x0001, 0x0003, 0xfffe, 0xfff4, 0x0000, 0x001f, 0x000c, 0xffbc,
+	0xffd3, 0x007d, 0x0075, 0xff33, 0xff01, 0x0131, 0x01ef, 0xfe60,
+	0xfc7a, 0x020e, 0x0656, 0xfd94, 0xf395, 0x02ab, 0x2857, 0x3d3f,
+};
+
+static const u16 eq_coeff_fmna1_ra02_m6db[] = {
+	0xfff1, 0xffe1, 0xffef, 0x000e, 0x0030, 0x002f, 0xfff6, 0xffa7,
+	0xff9d, 0x000a, 0x00a2, 0x00b5, 0xffea, 0xfed9, 0xfec5, 0x003d,
+	0x0217, 0x021b, 0xff5a, 0xfc2b, 0xfcbd, 0x02c4, 0x0ac3, 0x0e85,
+};
+
+static const u8 adc_presets[2][23] = {
+	{
+		0x83, 0x00, 0xcf, 0xb4, 0x0f, 0x2c, 0x0c, 0x49,
+		0x00, 0x00, 0x00, 0x8c,	0x02, 0x02, 0x00, 0x04,
+		0xec, 0x82, 0x4b, 0xcc, 0x01, 0x88, 0x0c,
+	},
+	{
+		0x83, 0x00, 0xcf, 0xb4,	0x0f, 0x2c, 0x0c, 0x49,
+		0x00, 0x00, 0x00, 0x8c,	0x02, 0x20, 0x33, 0x8c,
+		0x57, 0xd7, 0x59, 0xb7,	0x65, 0x0e, 0x0c,
+	},
+};
+
+/* Tuner bands */
+static const struct v4l2_frequency_band eu_bands_rf = {
+	.tuner = 0,
+	.type = V4L2_TUNER_RF,
+	.index = 0,
+	.capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS,
+	.rangelow   = 65000000,
+	.rangehigh  = 240000000,
+};
+
+static const struct v4l2_frequency_band na_bands_rf = {
+	.tuner = 0,
+	.type = V4L2_TUNER_RF,
+	.index = 0,
+	.capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS,
+	.rangelow   = 65000000,
+	.rangehigh  = 108000000,
+};
+
+/* Regmap settings */
+static const struct regmap_range max2175_regmap_volatile_range[] = {
+	regmap_reg_range(0x30, 0x35),
+	regmap_reg_range(0x3a, 0x45),
+	regmap_reg_range(0x59, 0x5e),
+	regmap_reg_range(0x73, 0x75),
+};
+
+static const struct regmap_access_table max2175_volatile_regs = {
+	.yes_ranges = max2175_regmap_volatile_range,
+	.n_yes_ranges = ARRAY_SIZE(max2175_regmap_volatile_range),
+};
+
+static const struct reg_default max2175_reg_defaults[] = {
+	{ 0x00, 0x07},
+};
+
+static const struct regmap_config max2175_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+	.max_register = 0xff,
+	.reg_defaults = max2175_reg_defaults,
+	.num_reg_defaults = ARRAY_SIZE(max2175_reg_defaults),
+	.volatile_table = &max2175_volatile_regs,
+	.cache_type = REGCACHE_RBTREE,
+};
+
+struct max2175 {
+	struct v4l2_subdev sd;		/* Sub-device */
+	struct i2c_client *client;	/* I2C client */
+
+	/* Controls */
+	struct v4l2_ctrl_handler ctrl_hdl;
+	struct v4l2_ctrl *lna_gain;	/* LNA gain value */
+	struct v4l2_ctrl *if_gain;	/* I/F gain value */
+	struct v4l2_ctrl *pll_lock;	/* PLL lock */
+	struct v4l2_ctrl *i2s_en;	/* I2S output enable */
+	struct v4l2_ctrl *hsls;		/* High-side/Low-side polarity */
+	struct v4l2_ctrl *rx_mode;	/* Receive mode */
+
+	/* Regmap */
+	struct regmap *regmap;
+
+	/* Cached configuration */
+	u32 freq;			/* Tuned freq In Hz */
+	const struct max2175_rxmode *rx_modes;		/* EU or NA modes */
+	const struct v4l2_frequency_band *bands_rf;	/* EU or NA bands */
+
+	/* Device settings */
+	unsigned long xtal_freq;	/* Ref Oscillator freq in Hz */
+	u32 decim_ratio;
+	bool master;			/* Master/Slave */
+	bool am_hiz;			/* AM Hi-Z filter */
+
+	/* ROM values */
+	u8 rom_bbf_bw_am;
+	u8 rom_bbf_bw_fm;
+	u8 rom_bbf_bw_dab;
+
+	/* Driver private variables */
+	bool mode_resolved;		/* Flag to sanity check settings */
+};
+
+static inline struct max2175 *max2175_from_sd(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct max2175, sd);
+}
+
+static inline struct max2175 *max2175_from_ctrl_hdl(struct v4l2_ctrl_handler *h)
+{
+	return container_of(h, struct max2175, ctrl_hdl);
+}
+
+/* Get bitval of a given val */
+static inline u8 max2175_get_bitval(u8 val, u8 msb, u8 lsb)
+{
+	return (val & GENMASK(msb, lsb)) >> lsb;
+}
+
+/* Read/Write bit(s) on top of regmap */
+static int max2175_read(struct max2175 *ctx, u8 idx, u8 *val)
+{
+	u32 regval;
+	int ret;
+
+	ret = regmap_read(ctx->regmap, idx, &regval);
+	if (ret)
+		mxm_err(ctx, "read ret(%d): idx 0x%02x\n", ret, idx);
+	else
+		*val = regval;
+
+	return ret;
+}
+
+static int max2175_write(struct max2175 *ctx, u8 idx, u8 val)
+{
+	int ret;
+
+	ret = regmap_write(ctx->regmap, idx, val);
+	if (ret)
+		mxm_err(ctx, "write ret(%d): idx 0x%02x val 0x%02x\n",
+			ret, idx, val);
+
+	return ret;
+}
+
+static u8 max2175_read_bits(struct max2175 *ctx, u8 idx, u8 msb, u8 lsb)
+{
+	u8 val;
+
+	if (max2175_read(ctx, idx, &val))
+		return 0;
+
+	return max2175_get_bitval(val, msb, lsb);
+}
+
+static int max2175_write_bits(struct max2175 *ctx, u8 idx,
+			     u8 msb, u8 lsb, u8 newval)
+{
+	int ret = regmap_update_bits(ctx->regmap, idx, GENMASK(msb, lsb),
+				     newval << lsb);
+
+	if (ret)
+		mxm_err(ctx, "wbits ret(%d): idx 0x%02x\n", ret, idx);
+
+	return ret;
+}
+
+static int max2175_write_bit(struct max2175 *ctx, u8 idx, u8 bit, u8 newval)
+{
+	return max2175_write_bits(ctx, idx, bit, bit, newval);
+}
+
+/* Checks expected pattern every msec until timeout */
+static int max2175_poll_timeout(struct max2175 *ctx, u8 idx, u8 msb, u8 lsb,
+				u8 exp_bitval, u32 timeout_us)
+{
+	unsigned int val;
+
+	return regmap_read_poll_timeout(ctx->regmap, idx, val,
+			(max2175_get_bitval(val, msb, lsb) == exp_bitval),
+			1000, timeout_us);
+}
+
+static int max2175_poll_csm_ready(struct max2175 *ctx)
+{
+	int ret;
+
+	ret = max2175_poll_timeout(ctx, 69, 1, 1, 0, 50000);
+	if (ret)
+		mxm_err(ctx, "csm not ready\n");
+
+	return ret;
+}
+
+#define MAX2175_IS_BAND_AM(ctx)		\
+	(max2175_read_bits(ctx, 5, 1, 0) == MAX2175_BAND_AM)
+
+#define MAX2175_IS_BAND_VHF(ctx)	\
+	(max2175_read_bits(ctx, 5, 1, 0) == MAX2175_BAND_VHF)
+
+#define MAX2175_IS_FM_MODE(ctx)		\
+	(max2175_read_bits(ctx, 12, 5, 4) == 0)
+
+#define MAX2175_IS_FMHD_MODE(ctx)	\
+	(max2175_read_bits(ctx, 12, 5, 4) == 1)
+
+#define MAX2175_IS_DAB_MODE(ctx)	\
+	(max2175_read_bits(ctx, 12, 5, 4) == 2)
+
+static int max2175_band_from_freq(u32 freq)
+{
+	if (freq >= 144000 && freq <= 26100000)
+		return MAX2175_BAND_AM;
+	else if (freq >= 65000000 && freq <= 108000000)
+		return MAX2175_BAND_FM;
+
+	return MAX2175_BAND_VHF;
+}
+
+static void max2175_i2s_enable(struct max2175 *ctx, bool enable)
+{
+	if (enable)
+		/* Stuff bits are zeroed */
+		max2175_write_bits(ctx, 104, 3, 0, 2);
+	else
+		/* Keep SCK alive */
+		max2175_write_bits(ctx, 104, 3, 0, 9);
+	mxm_dbg(ctx, "i2s %sabled\n", enable ? "en" : "dis");
+}
+
+static void max2175_set_filter_coeffs(struct max2175 *ctx, u8 m_sel,
+				      u8 bank, const u16 *coeffs)
+{
+	unsigned int i;
+	u8 coeff_addr, upper_address = 24;
+
+	mxm_dbg(ctx, "set_filter_coeffs: m_sel %d bank %d\n", m_sel, bank);
+	max2175_write_bits(ctx, 114, 5, 4, m_sel);
+
+	if (m_sel == 2)
+		upper_address = 12;
+
+	for (i = 0; i < upper_address; i++) {
+		coeff_addr = i + bank * 24;
+		max2175_write(ctx, 115, coeffs[i] >> 8);
+		max2175_write(ctx, 116, coeffs[i]);
+		max2175_write(ctx, 117, coeff_addr | 1 << 7);
+	}
+	max2175_write_bit(ctx, 117, 7, 0);
+}
+
+static void max2175_load_fmeu_1p2(struct max2175 *ctx)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(fmeu1p2_map); i++)
+		max2175_write(ctx, fmeu1p2_map[i].idx, fmeu1p2_map[i].val);
+
+	ctx->decim_ratio = 36;
+
+	/* Load the Channel Filter Coefficients into channel filter bank #2 */
+	max2175_set_filter_coeffs(ctx, MAX2175_CH_MSEL, 0, ch_coeff_fmeu);
+	max2175_set_filter_coeffs(ctx, MAX2175_EQ_MSEL, 0,
+				  eq_coeff_fmeu1_ra02_m6db);
+}
+
+static void max2175_load_dab_1p2(struct max2175 *ctx)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(dab12_map); i++)
+		max2175_write(ctx, dab12_map[i].idx, dab12_map[i].val);
+
+	ctx->decim_ratio = 1;
+
+	/* Load the Channel Filter Coefficients into channel filter bank #2 */
+	max2175_set_filter_coeffs(ctx, MAX2175_CH_MSEL, 2, ch_coeff_dab1);
+}
+
+static void max2175_load_fmna_1p0(struct max2175 *ctx)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(fmna1p0_map); i++)
+		max2175_write(ctx, fmna1p0_map[i].idx, fmna1p0_map[i].val);
+}
+
+static void max2175_load_fmna_2p0(struct max2175 *ctx)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(fmna2p0_map); i++)
+		max2175_write(ctx, fmna2p0_map[i].idx, fmna2p0_map[i].val);
+}
+
+static void max2175_set_bbfilter(struct max2175 *ctx)
+{
+	if (MAX2175_IS_BAND_AM(ctx)) {
+		max2175_write_bits(ctx, 12, 3, 0, ctx->rom_bbf_bw_am);
+		mxm_dbg(ctx, "set_bbfilter AM: rom %d\n", ctx->rom_bbf_bw_am);
+	} else if (MAX2175_IS_DAB_MODE(ctx)) {
+		max2175_write_bits(ctx, 12, 3, 0, ctx->rom_bbf_bw_dab);
+		mxm_dbg(ctx, "set_bbfilter DAB: rom %d\n", ctx->rom_bbf_bw_dab);
+	} else {
+		max2175_write_bits(ctx, 12, 3, 0, ctx->rom_bbf_bw_fm);
+		mxm_dbg(ctx, "set_bbfilter FM: rom %d\n", ctx->rom_bbf_bw_fm);
+	}
+}
+
+static int max2175_set_csm_mode(struct max2175 *ctx,
+			  enum max2175_csm_mode new_mode)
+{
+	int ret = max2175_poll_csm_ready(ctx);
+
+	if (ret)
+		return ret;
+
+	max2175_write_bits(ctx, 0, 2, 0, new_mode);
+	mxm_dbg(ctx, "set csm new mode %d\n", new_mode);
+
+	/* Wait for a fixed settle down time depending on new mode */
+	switch (new_mode) {
+	case MAX2175_PRESET_TUNE:
+		usleep_range(51100, 51500);	/* 51.1ms */
+		break;
+	/*
+	 * Other mode switches need different sleep values depending on band &
+	 * mode
+	 */
+	default:
+		break;
+	}
+
+	return max2175_poll_csm_ready(ctx);
+}
+
+static int max2175_csm_action(struct max2175 *ctx,
+			      enum max2175_csm_mode action)
+{
+	int ret;
+
+	mxm_dbg(ctx, "csm_action: %d\n", action);
+
+	/* Other actions can be added in future when needed */
+	ret = max2175_set_csm_mode(ctx, MAX2175_LOAD_TO_BUFFER);
+	if (ret)
+		return ret;
+
+	return max2175_set_csm_mode(ctx, MAX2175_PRESET_TUNE);
+}
+
+static int max2175_set_lo_freq(struct max2175 *ctx, u32 lo_freq)
+{
+	u8 lo_mult, loband_bits = 0, vcodiv_bits = 0;
+	u32 int_desired, frac_desired;
+	enum max2175_band band;
+	int ret;
+
+	band = max2175_read_bits(ctx, 5, 1, 0);
+	switch (band) {
+	case MAX2175_BAND_AM:
+		lo_mult = 16;
+		break;
+	case MAX2175_BAND_FM:
+		if (lo_freq <= 74700000) {
+			lo_mult = 16;
+		} else if (lo_freq > 74700000 && lo_freq <= 110000000) {
+			loband_bits = 1;
+			lo_mult = 8;
+		} else {
+			loband_bits = 1;
+			vcodiv_bits = 3;
+			lo_mult = 8;
+		}
+		break;
+	case MAX2175_BAND_VHF:
+		if (lo_freq <= 210000000)
+			vcodiv_bits = 2;
+		else
+			vcodiv_bits = 1;
+
+		loband_bits = 2;
+		lo_mult = 4;
+		break;
+	default:
+		loband_bits = 3;
+		vcodiv_bits = 2;
+		lo_mult = 2;
+		break;
+	}
+
+	if (band == MAX2175_BAND_L)
+		lo_freq /= lo_mult;
+	else
+		lo_freq *= lo_mult;
+
+	int_desired = lo_freq / ctx->xtal_freq;
+	frac_desired = div64_ul((u64)(lo_freq % ctx->xtal_freq) << 20,
+				ctx->xtal_freq);
+
+	/* Check CSM is not busy */
+	ret = max2175_poll_csm_ready(ctx);
+	if (ret)
+		return ret;
+
+	mxm_dbg(ctx, "lo_mult %u int %u  frac %u\n",
+		lo_mult, int_desired, frac_desired);
+
+	/* Write the calculated values to the appropriate registers */
+	max2175_write(ctx, 1, int_desired);
+	max2175_write_bits(ctx, 2, 3, 0, (frac_desired >> 16) & 0xf);
+	max2175_write(ctx, 3, frac_desired >> 8);
+	max2175_write(ctx, 4, frac_desired);
+	max2175_write_bits(ctx, 5, 3, 2, loband_bits);
+	max2175_write_bits(ctx, 6, 7, 6, vcodiv_bits);
+
+	return ret;
+}
+
+/*
+ * Helper similar to DIV_ROUND_CLOSEST but an inline function that accepts s64
+ * dividend and s32 divisor
+ */
+static inline s64 max2175_round_closest(s64 dividend, s32 divisor)
+{
+	if ((dividend > 0 && divisor > 0) || (dividend < 0 && divisor < 0))
+		return div_s64(dividend + divisor / 2, divisor);
+
+	return div_s64(dividend - divisor / 2, divisor);
+}
+
+static int max2175_set_nco_freq(struct max2175 *ctx, s32 nco_freq)
+{
+	s32 clock_rate = ctx->xtal_freq / ctx->decim_ratio;
+	u32 nco_reg, abs_nco_freq = abs(nco_freq);
+	s64 nco_val_desired;
+	int ret;
+
+	if (abs_nco_freq < clock_rate / 2) {
+		nco_val_desired = 2 * nco_freq;
+	} else {
+		nco_val_desired = 2LL * (clock_rate - abs_nco_freq);
+		if (nco_freq < 0)
+			nco_val_desired = -nco_val_desired;
+	}
+
+	nco_reg = max2175_round_closest(nco_val_desired << 20, clock_rate);
+
+	if (nco_freq < 0)
+		nco_reg += 0x200000;
+
+	/* Check CSM is not busy */
+	ret = max2175_poll_csm_ready(ctx);
+	if (ret)
+		return ret;
+
+	mxm_dbg(ctx, "freq %d desired %lld reg %u\n",
+		nco_freq, nco_val_desired, nco_reg);
+
+	/* Write the calculated values to the appropriate registers */
+	max2175_write_bits(ctx, 7, 4, 0, (nco_reg >> 16) & 0x1f);
+	max2175_write(ctx, 8, nco_reg >> 8);
+	max2175_write(ctx, 9, nco_reg);
+
+	return ret;
+}
+
+static int max2175_set_rf_freq_non_am_bands(struct max2175 *ctx, u64 freq,
+					    u32 lo_pos)
+{
+	s64 adj_freq, low_if_freq;
+	int ret;
+
+	mxm_dbg(ctx, "rf_freq: non AM bands\n");
+
+	if (MAX2175_IS_FM_MODE(ctx))
+		low_if_freq = 128000;
+	else if (MAX2175_IS_FMHD_MODE(ctx))
+		low_if_freq = 228000;
+	else
+		return max2175_set_lo_freq(ctx, freq);
+
+	if (MAX2175_IS_BAND_VHF(ctx) == (lo_pos == MAX2175_LO_ABOVE_DESIRED))
+		adj_freq = freq + low_if_freq;
+	else
+		adj_freq = freq - low_if_freq;
+
+	ret = max2175_set_lo_freq(ctx, adj_freq);
+	if (ret)
+		return ret;
+
+	return max2175_set_nco_freq(ctx, -low_if_freq);
+}
+
+static int max2175_set_rf_freq(struct max2175 *ctx, u64 freq, u32 lo_pos)
+{
+	int ret;
+
+	if (MAX2175_IS_BAND_AM(ctx))
+		ret = max2175_set_nco_freq(ctx, freq);
+	else
+		ret = max2175_set_rf_freq_non_am_bands(ctx, freq, lo_pos);
+
+	mxm_dbg(ctx, "set_rf_freq: ret %d freq %llu\n", ret, freq);
+
+	return ret;
+}
+
+static int max2175_tune_rf_freq(struct max2175 *ctx, u64 freq, u32 hsls)
+{
+	int ret;
+
+	ret = max2175_set_rf_freq(ctx, freq, hsls);
+	if (ret)
+		return ret;
+
+	ret = max2175_csm_action(ctx, MAX2175_BUFFER_PLUS_PRESET_TUNE);
+	if (ret)
+		return ret;
+
+	mxm_dbg(ctx, "tune_rf_freq: old %u new %llu\n", ctx->freq, freq);
+	ctx->freq = freq;
+
+	return ret;
+}
+
+static void max2175_set_hsls(struct max2175 *ctx, u32 lo_pos)
+{
+	mxm_dbg(ctx, "set_hsls: lo_pos %u\n", lo_pos);
+
+	if ((lo_pos == MAX2175_LO_BELOW_DESIRED) == MAX2175_IS_BAND_VHF(ctx))
+		max2175_write_bit(ctx, 5, 4, 1);
+	else
+		max2175_write_bit(ctx, 5, 4, 0);
+}
+
+static void max2175_set_eu_rx_mode(struct max2175 *ctx, u32 rx_mode)
+{
+	switch (rx_mode) {
+	case MAX2175_EU_FM_1_2:
+		max2175_load_fmeu_1p2(ctx);
+		break;
+
+	case MAX2175_DAB_1_2:
+		max2175_load_dab_1p2(ctx);
+		break;
+	}
+	/* Master is the default setting */
+	if (!ctx->master)
+		max2175_write_bit(ctx, 30, 7, 1);
+}
+
+static void max2175_set_na_rx_mode(struct max2175 *ctx, u32 rx_mode)
+{
+	switch (rx_mode) {
+	case MAX2175_NA_FM_1_0:
+		max2175_load_fmna_1p0(ctx);
+		break;
+	case MAX2175_NA_FM_2_0:
+		max2175_load_fmna_2p0(ctx);
+		break;
+	}
+	/* Master is the default setting */
+	if (!ctx->master)
+		max2175_write_bit(ctx, 30, 7, 1);
+
+	ctx->decim_ratio = 27;
+
+	/* Load the Channel Filter Coefficients into channel filter bank #2 */
+	max2175_set_filter_coeffs(ctx, MAX2175_CH_MSEL, 0, ch_coeff_fmna);
+	max2175_set_filter_coeffs(ctx, MAX2175_EQ_MSEL, 0,
+				  eq_coeff_fmna1_ra02_m6db);
+}
+
+static int max2175_set_rx_mode(struct max2175 *ctx, u32 rx_mode)
+{
+	mxm_dbg(ctx, "set_rx_mode: %u am_hiz %u\n", rx_mode, ctx->am_hiz);
+	if (ctx->xtal_freq == MAX2175_EU_XTAL_FREQ)
+		max2175_set_eu_rx_mode(ctx, rx_mode);
+	else
+		max2175_set_na_rx_mode(ctx, rx_mode);
+
+	if (ctx->am_hiz) {
+		mxm_dbg(ctx, "setting AM HiZ related config\n");
+		max2175_write_bit(ctx, 50, 5, 1);
+		max2175_write_bit(ctx, 90, 7, 1);
+		max2175_write_bits(ctx, 73, 1, 0, 2);
+		max2175_write_bits(ctx, 80, 5, 0, 33);
+	}
+
+	/* Load BB filter trim values saved in ROM */
+	max2175_set_bbfilter(ctx);
+
+	/* Set HSLS */
+	max2175_set_hsls(ctx, ctx->hsls->cur.val);
+
+	/* Use i2s enable settings */
+	max2175_i2s_enable(ctx, ctx->i2s_en->cur.val);
+
+	ctx->mode_resolved = true;
+
+	return 0;
+}
+
+static int max2175_rx_mode_from_freq(struct max2175 *ctx, u32 freq, u32 *mode)
+{
+	unsigned int i;
+	int band = max2175_band_from_freq(freq);
+
+	/* Pick the first match always */
+	for (i = 0; i <= ctx->rx_mode->maximum; i++) {
+		if (ctx->rx_modes[i].band == band) {
+			*mode = i;
+			mxm_dbg(ctx, "rx_mode_from_freq: freq %u mode %d\n",
+				freq, *mode);
+			return 0;
+		}
+	}
+
+	return -EINVAL;
+}
+
+static bool max2175_freq_rx_mode_valid(struct max2175 *ctx,
+					 u32 mode, u32 freq)
+{
+	int band = max2175_band_from_freq(freq);
+
+	return (ctx->rx_modes[mode].band == band);
+}
+
+static void max2175_load_adc_presets(struct max2175 *ctx)
+{
+	unsigned int i, j;
+
+	for (i = 0; i < ARRAY_SIZE(adc_presets); i++)
+		for (j = 0; j < ARRAY_SIZE(adc_presets[0]); j++)
+			max2175_write(ctx, 146 + j + i * 55, adc_presets[i][j]);
+}
+
+static int max2175_init_power_manager(struct max2175 *ctx)
+{
+	int ret;
+
+	/* Execute on-chip power-up/calibration */
+	max2175_write_bit(ctx, 99, 2, 0);
+	usleep_range(1000, 1500);
+	max2175_write_bit(ctx, 99, 2, 1);
+
+	/* Wait for the power manager to finish. */
+	ret = max2175_poll_timeout(ctx, 69, 7, 7, 1, 50000);
+	if (ret)
+		mxm_err(ctx, "init pm failed\n");
+
+	return ret;
+}
+
+static int max2175_recalibrate_adc(struct max2175 *ctx)
+{
+	int ret;
+
+	/* ADC Re-calibration */
+	max2175_write(ctx, 150, 0xff);
+	max2175_write(ctx, 205, 0xff);
+	max2175_write(ctx, 147, 0x20);
+	max2175_write(ctx, 147, 0x00);
+	max2175_write(ctx, 202, 0x20);
+	max2175_write(ctx, 202, 0x00);
+
+	ret = max2175_poll_timeout(ctx, 69, 4, 3, 3, 50000);
+	if (ret)
+		mxm_err(ctx, "adc recalibration failed\n");
+
+	return ret;
+}
+
+static u8 max2175_read_rom(struct max2175 *ctx, u8 row)
+{
+	u8 data = 0;
+
+	max2175_write_bit(ctx, 56, 4, 0);
+	max2175_write_bits(ctx, 56, 3, 0, row);
+
+	usleep_range(2000, 2500);
+	max2175_read(ctx, 58, &data);
+
+	max2175_write_bits(ctx, 56, 3, 0, 0);
+
+	mxm_dbg(ctx, "read_rom: row %d data 0x%02x\n", row, data);
+
+	return data;
+}
+
+static void max2175_load_from_rom(struct max2175 *ctx)
+{
+	u8 data = 0;
+
+	data = max2175_read_rom(ctx, 0);
+	ctx->rom_bbf_bw_am = data & 0x0f;
+	max2175_write_bits(ctx, 81, 3, 0, data >> 4);
+
+	data = max2175_read_rom(ctx, 1);
+	ctx->rom_bbf_bw_fm = data & 0x0f;
+	ctx->rom_bbf_bw_dab = data >> 4;
+
+	data = max2175_read_rom(ctx, 2);
+	max2175_write_bits(ctx, 82, 4, 0, data & 0x1f);
+	max2175_write_bits(ctx, 82, 7, 5, data >> 5);
+
+	data = max2175_read_rom(ctx, 3);
+	if (ctx->am_hiz) {
+		data &= 0x0f;
+		data |= (max2175_read_rom(ctx, 7) & 0x40) >> 2;
+		if (!data)
+			data |= 2;
+	} else {
+		data = (data & 0xf0) >> 4;
+		data |= (max2175_read_rom(ctx, 7) & 0x80) >> 3;
+		if (!data)
+			data |= 30;
+	}
+	max2175_write_bits(ctx, 80, 5, 0, data + 31);
+
+	data = max2175_read_rom(ctx, 6);
+	max2175_write_bits(ctx, 81, 7, 6, data >> 6);
+}
+
+static void max2175_load_full_fm_eu_1p0(struct max2175 *ctx)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(full_fm_eu_1p0); i++)
+		max2175_write(ctx, i + 1, full_fm_eu_1p0[i]);
+
+	usleep_range(5000, 5500);
+	ctx->decim_ratio = 36;
+}
+
+static void max2175_load_full_fm_na_1p0(struct max2175 *ctx)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(full_fm_na_1p0); i++)
+		max2175_write(ctx, i + 1, full_fm_na_1p0[i]);
+
+	usleep_range(5000, 5500);
+	ctx->decim_ratio = 27;
+}
+
+static int max2175_core_init(struct max2175 *ctx, u32 refout_bits)
+{
+	int ret;
+
+	/* MAX2175 uses 36.864MHz clock for EU & 40.154MHz for NA region */
+	if (ctx->xtal_freq == MAX2175_EU_XTAL_FREQ)
+		max2175_load_full_fm_eu_1p0(ctx);
+	else
+		max2175_load_full_fm_na_1p0(ctx);
+
+	/* The default settings assume master */
+	if (!ctx->master)
+		max2175_write_bit(ctx, 30, 7, 1);
+
+	mxm_dbg(ctx, "refout_bits %u\n", refout_bits);
+
+	/* Set REFOUT */
+	max2175_write_bits(ctx, 56, 7, 5, refout_bits);
+
+	/* ADC Reset */
+	max2175_write_bit(ctx, 99, 1, 0);
+	usleep_range(1000, 1500);
+	max2175_write_bit(ctx, 99, 1, 1);
+
+	/* Load ADC preset values */
+	max2175_load_adc_presets(ctx);
+
+	/* Initialize the power management state machine */
+	ret = max2175_init_power_manager(ctx);
+	if (ret)
+		return ret;
+
+	/* Recalibrate ADC */
+	ret = max2175_recalibrate_adc(ctx);
+	if (ret)
+		return ret;
+
+	/* Load ROM values to appropriate registers */
+	max2175_load_from_rom(ctx);
+
+	if (ctx->xtal_freq == MAX2175_EU_XTAL_FREQ) {
+		/* Load FIR coefficients into bank 0 */
+		max2175_set_filter_coeffs(ctx, MAX2175_CH_MSEL, 0,
+					  ch_coeff_fmeu);
+		max2175_set_filter_coeffs(ctx, MAX2175_EQ_MSEL, 0,
+					  eq_coeff_fmeu1_ra02_m6db);
+	} else {
+		/* Load FIR coefficients into bank 0 */
+		max2175_set_filter_coeffs(ctx, MAX2175_CH_MSEL, 0,
+					  ch_coeff_fmna);
+		max2175_set_filter_coeffs(ctx, MAX2175_EQ_MSEL, 0,
+					  eq_coeff_fmna1_ra02_m6db);
+	}
+	mxm_dbg(ctx, "core initialized\n");
+
+	return 0;
+}
+
+static void max2175_s_ctrl_rx_mode(struct max2175 *ctx, u32 rx_mode)
+{
+	/* Load mode. Range check already done */
+	max2175_set_rx_mode(ctx, rx_mode);
+
+	mxm_dbg(ctx, "s_ctrl_rx_mode: %u curr freq %u\n", rx_mode, ctx->freq);
+
+	/* Check if current freq valid for mode & update */
+	if (max2175_freq_rx_mode_valid(ctx, rx_mode, ctx->freq))
+		max2175_tune_rf_freq(ctx, ctx->freq, ctx->hsls->cur.val);
+	else
+		/* Use default freq of mode if current freq is not valid */
+		max2175_tune_rf_freq(ctx, ctx->rx_modes[rx_mode].freq,
+				     ctx->hsls->cur.val);
+}
+
+static int max2175_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct max2175 *ctx = max2175_from_ctrl_hdl(ctrl->handler);
+
+	mxm_dbg(ctx, "s_ctrl: id 0x%x, val %u\n", ctrl->id, ctrl->val);
+	switch (ctrl->id) {
+	case V4L2_CID_MAX2175_I2S_ENABLE:
+		max2175_i2s_enable(ctx, ctrl->val);
+		break;
+	case V4L2_CID_MAX2175_HSLS:
+		max2175_set_hsls(ctx, ctrl->val);
+		break;
+	case V4L2_CID_MAX2175_RX_MODE:
+		max2175_s_ctrl_rx_mode(ctx, ctrl->val);
+		break;
+	}
+
+	return 0;
+}
+
+static u32 max2175_get_lna_gain(struct max2175 *ctx)
+{
+	enum max2175_band band = max2175_read_bits(ctx, 5, 1, 0);
+
+	switch (band) {
+	case MAX2175_BAND_AM:
+		return max2175_read_bits(ctx, 51, 3, 0);
+	case MAX2175_BAND_FM:
+		return max2175_read_bits(ctx, 50, 3, 0);
+	case MAX2175_BAND_VHF:
+		return max2175_read_bits(ctx, 52, 5, 0);
+	default:
+		return 0;
+	}
+}
+
+static int max2175_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct max2175 *ctx = max2175_from_ctrl_hdl(ctrl->handler);
+
+	switch (ctrl->id) {
+	case V4L2_CID_RF_TUNER_LNA_GAIN:
+		ctrl->val = max2175_get_lna_gain(ctx);
+		break;
+	case V4L2_CID_RF_TUNER_IF_GAIN:
+		ctrl->val = max2175_read_bits(ctx, 49, 4, 0);
+		break;
+	case V4L2_CID_RF_TUNER_PLL_LOCK:
+		ctrl->val = (max2175_read_bits(ctx, 60, 7, 6) == 3);
+		break;
+	}
+
+	return 0;
+};
+
+static int max2175_set_freq_and_mode(struct max2175 *ctx, u32 freq)
+{
+	u32 rx_mode;
+	int ret;
+
+	/* Get band from frequency */
+	ret = max2175_rx_mode_from_freq(ctx, freq, &rx_mode);
+	if (ret)
+		return ret;
+
+	mxm_dbg(ctx, "set_freq_and_mode: freq %u rx_mode %d\n", freq, rx_mode);
+
+	/* Load mode */
+	max2175_set_rx_mode(ctx, rx_mode);
+	ctx->rx_mode->cur.val = rx_mode;
+
+	/* Tune to the new freq given */
+	return max2175_tune_rf_freq(ctx, freq, ctx->hsls->cur.val);
+}
+
+static int max2175_s_frequency(struct v4l2_subdev *sd,
+			       const struct v4l2_frequency *vf)
+{
+	struct max2175 *ctx = max2175_from_sd(sd);
+	u32 freq;
+	int ret = 0;
+
+	mxm_dbg(ctx, "s_freq: new %u curr %u, mode_resolved %d\n",
+		vf->frequency, ctx->freq, ctx->mode_resolved);
+
+	if (vf->tuner != 0)
+		return -EINVAL;
+
+	freq = clamp(vf->frequency, ctx->bands_rf->rangelow,
+		     ctx->bands_rf->rangehigh);
+
+	/* Check new freq valid for rx_mode if already resolved */
+	if (ctx->mode_resolved &&
+	    max2175_freq_rx_mode_valid(ctx, ctx->rx_mode->cur.val, freq))
+		ret = max2175_tune_rf_freq(ctx, freq, ctx->hsls->cur.val);
+	else
+		/* Find default rx_mode for freq and tune to it */
+		ret = max2175_set_freq_and_mode(ctx, freq);
+
+	mxm_dbg(ctx, "s_freq: ret %d curr %u mode_resolved %d mode %u\n",
+		ret, ctx->freq, ctx->mode_resolved, ctx->rx_mode->cur.val);
+
+	return ret;
+}
+
+static int max2175_g_frequency(struct v4l2_subdev *sd,
+			       struct v4l2_frequency *vf)
+{
+	struct max2175 *ctx = max2175_from_sd(sd);
+
+	if (vf->tuner != 0)
+		return -EINVAL;
+
+	/* RF freq */
+	vf->type = V4L2_TUNER_RF;
+	vf->frequency = ctx->freq;
+
+	return 0;
+}
+
+static int max2175_enum_freq_bands(struct v4l2_subdev *sd,
+			    struct v4l2_frequency_band *band)
+{
+	struct max2175 *ctx = max2175_from_sd(sd);
+
+	if (band->tuner != 0 || band->index != 0)
+		return -EINVAL;
+
+	*band = *ctx->bands_rf;
+
+	return 0;
+}
+
+static int max2175_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
+{
+	struct max2175 *ctx = max2175_from_sd(sd);
+
+	if (vt->index > 0)
+		return -EINVAL;
+
+	strscpy(vt->name, "RF", sizeof(vt->name));
+	vt->type = V4L2_TUNER_RF;
+	vt->capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS;
+	vt->rangelow = ctx->bands_rf->rangelow;
+	vt->rangehigh = ctx->bands_rf->rangehigh;
+
+	return 0;
+}
+
+static int max2175_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *vt)
+{
+	/* Check tuner index is valid */
+	if (vt->index > 0)
+		return -EINVAL;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_tuner_ops max2175_tuner_ops = {
+	.s_frequency = max2175_s_frequency,
+	.g_frequency = max2175_g_frequency,
+	.enum_freq_bands = max2175_enum_freq_bands,
+	.g_tuner = max2175_g_tuner,
+	.s_tuner = max2175_s_tuner,
+};
+
+static const struct v4l2_subdev_ops max2175_ops = {
+	.tuner = &max2175_tuner_ops,
+};
+
+static const struct v4l2_ctrl_ops max2175_ctrl_ops = {
+	.s_ctrl = max2175_s_ctrl,
+	.g_volatile_ctrl = max2175_g_volatile_ctrl,
+};
+
+/*
+ * I2S output enable/disable configuration. This is a private control.
+ * Refer to Documentation/userspace-api/media/drivers/max2175.rst for more details.
+ */
+static const struct v4l2_ctrl_config max2175_i2s_en = {
+	.ops = &max2175_ctrl_ops,
+	.id = V4L2_CID_MAX2175_I2S_ENABLE,
+	.name = "I2S Enable",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.min = 0,
+	.max = 1,
+	.step = 1,
+	.def = 1,
+	.is_private = 1,
+};
+
+/*
+ * HSLS value control LO freq adjacent location configuration.
+ * Refer to Documentation/userspace-api/media/drivers/max2175.rst for more details.
+ */
+static const struct v4l2_ctrl_config max2175_hsls = {
+	.ops = &max2175_ctrl_ops,
+	.id = V4L2_CID_MAX2175_HSLS,
+	.name = "HSLS Above/Below Desired",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.min = 0,
+	.max = 1,
+	.step = 1,
+	.def = 1,
+};
+
+/*
+ * Rx modes below are a set of preset configurations that decides the tuner's
+ * sck and sample rate of transmission. They are separate for EU & NA regions.
+ * Refer to Documentation/userspace-api/media/drivers/max2175.rst for more details.
+ */
+static const char * const max2175_ctrl_eu_rx_modes[] = {
+	[MAX2175_EU_FM_1_2]	= "EU FM 1.2",
+	[MAX2175_DAB_1_2]	= "DAB 1.2",
+};
+
+static const char * const max2175_ctrl_na_rx_modes[] = {
+	[MAX2175_NA_FM_1_0]	= "NA FM 1.0",
+	[MAX2175_NA_FM_2_0]	= "NA FM 2.0",
+};
+
+static const struct v4l2_ctrl_config max2175_eu_rx_mode = {
+	.ops = &max2175_ctrl_ops,
+	.id = V4L2_CID_MAX2175_RX_MODE,
+	.name = "RX Mode",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.max = ARRAY_SIZE(max2175_ctrl_eu_rx_modes) - 1,
+	.def = 0,
+	.qmenu = max2175_ctrl_eu_rx_modes,
+};
+
+static const struct v4l2_ctrl_config max2175_na_rx_mode = {
+	.ops = &max2175_ctrl_ops,
+	.id = V4L2_CID_MAX2175_RX_MODE,
+	.name = "RX Mode",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.max = ARRAY_SIZE(max2175_ctrl_na_rx_modes) - 1,
+	.def = 0,
+	.qmenu = max2175_ctrl_na_rx_modes,
+};
+
+static int max2175_refout_load_to_bits(struct i2c_client *client, u32 load,
+				       u32 *bits)
+{
+	if (load <= 40)
+		*bits = load / 10;
+	else if (load >= 60 && load <= 70)
+		*bits = load / 10 - 1;
+	else
+		return -EINVAL;
+
+	return 0;
+}
+
+static int max2175_probe(struct i2c_client *client)
+{
+	bool master = true, am_hiz = false;
+	u32 refout_load, refout_bits = 0;	/* REFOUT disabled */
+	struct v4l2_ctrl_handler *hdl;
+	struct fwnode_handle *fwnode;
+	struct device_node *np;
+	struct v4l2_subdev *sd;
+	struct regmap *regmap;
+	struct max2175 *ctx;
+	struct clk *clk;
+	int ret;
+
+	/* Parse DT properties */
+	np = of_parse_phandle(client->dev.of_node, "maxim,master", 0);
+	if (np) {
+		master = false;			/* Slave tuner */
+		of_node_put(np);
+	}
+
+	fwnode = of_fwnode_handle(client->dev.of_node);
+	if (fwnode_property_present(fwnode, "maxim,am-hiz-filter"))
+		am_hiz = true;
+
+	if (!fwnode_property_read_u32(fwnode, "maxim,refout-load",
+				      &refout_load)) {
+		ret = max2175_refout_load_to_bits(client, refout_load,
+						  &refout_bits);
+		if (ret) {
+			dev_err(&client->dev, "invalid refout_load %u\n",
+				refout_load);
+			return -EINVAL;
+		}
+	}
+
+	clk = devm_clk_get(&client->dev, NULL);
+	if (IS_ERR(clk)) {
+		ret = PTR_ERR(clk);
+		dev_err(&client->dev, "cannot get clock %d\n", ret);
+		return ret;
+	}
+
+	regmap = devm_regmap_init_i2c(client, &max2175_regmap_config);
+	if (IS_ERR(regmap)) {
+		ret = PTR_ERR(regmap);
+		dev_err(&client->dev, "regmap init failed %d\n", ret);
+		return -ENODEV;
+	}
+
+	/* Alloc tuner context */
+	ctx = devm_kzalloc(&client->dev, sizeof(*ctx), GFP_KERNEL);
+	if (ctx == NULL)
+		return -ENOMEM;
+
+	sd = &ctx->sd;
+	ctx->master = master;
+	ctx->am_hiz = am_hiz;
+	ctx->mode_resolved = false;
+	ctx->regmap = regmap;
+	ctx->xtal_freq = clk_get_rate(clk);
+	dev_info(&client->dev, "xtal freq %luHz\n", ctx->xtal_freq);
+
+	v4l2_i2c_subdev_init(sd, client, &max2175_ops);
+	ctx->client = client;
+
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	/* Controls */
+	hdl = &ctx->ctrl_hdl;
+	ret = v4l2_ctrl_handler_init(hdl, 7);
+	if (ret)
+		return ret;
+
+	ctx->lna_gain = v4l2_ctrl_new_std(hdl, &max2175_ctrl_ops,
+					  V4L2_CID_RF_TUNER_LNA_GAIN,
+					  0, 63, 1, 0);
+	ctx->lna_gain->flags |= (V4L2_CTRL_FLAG_VOLATILE |
+				 V4L2_CTRL_FLAG_READ_ONLY);
+	ctx->if_gain = v4l2_ctrl_new_std(hdl, &max2175_ctrl_ops,
+					 V4L2_CID_RF_TUNER_IF_GAIN,
+					 0, 31, 1, 0);
+	ctx->if_gain->flags |= (V4L2_CTRL_FLAG_VOLATILE |
+				V4L2_CTRL_FLAG_READ_ONLY);
+	ctx->pll_lock = v4l2_ctrl_new_std(hdl, &max2175_ctrl_ops,
+					  V4L2_CID_RF_TUNER_PLL_LOCK,
+					  0, 1, 1, 0);
+	ctx->pll_lock->flags |= (V4L2_CTRL_FLAG_VOLATILE |
+				 V4L2_CTRL_FLAG_READ_ONLY);
+	ctx->i2s_en = v4l2_ctrl_new_custom(hdl, &max2175_i2s_en, NULL);
+	ctx->hsls = v4l2_ctrl_new_custom(hdl, &max2175_hsls, NULL);
+
+	if (ctx->xtal_freq == MAX2175_EU_XTAL_FREQ) {
+		ctx->rx_mode = v4l2_ctrl_new_custom(hdl,
+						    &max2175_eu_rx_mode, NULL);
+		ctx->rx_modes = eu_rx_modes;
+		ctx->bands_rf = &eu_bands_rf;
+	} else {
+		ctx->rx_mode = v4l2_ctrl_new_custom(hdl,
+						    &max2175_na_rx_mode, NULL);
+		ctx->rx_modes = na_rx_modes;
+		ctx->bands_rf = &na_bands_rf;
+	}
+	ctx->sd.ctrl_handler = &ctx->ctrl_hdl;
+
+	/* Set the defaults */
+	ctx->freq = ctx->bands_rf->rangelow;
+
+	/* Register subdev */
+	ret = v4l2_async_register_subdev(sd);
+	if (ret) {
+		dev_err(&client->dev, "register subdev failed\n");
+		goto err_reg;
+	}
+
+	/* Initialize device */
+	ret = max2175_core_init(ctx, refout_bits);
+	if (ret)
+		goto err_init;
+
+	ret = v4l2_ctrl_handler_setup(hdl);
+	if (ret)
+		goto err_init;
+
+	return 0;
+
+err_init:
+	v4l2_async_unregister_subdev(sd);
+err_reg:
+	v4l2_ctrl_handler_free(&ctx->ctrl_hdl);
+
+	return ret;
+}
+
+static void max2175_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct max2175 *ctx = max2175_from_sd(sd);
+
+	v4l2_ctrl_handler_free(&ctx->ctrl_hdl);
+	v4l2_async_unregister_subdev(sd);
+}
+
+static const struct i2c_device_id max2175_id[] = {
+	{ DRIVER_NAME, 0},
+	{},
+};
+MODULE_DEVICE_TABLE(i2c, max2175_id);
+
+static const struct of_device_id max2175_of_ids[] = {
+	{ .compatible = "maxim,max2175", },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, max2175_of_ids);
+
+static struct i2c_driver max2175_driver = {
+	.driver = {
+		.name	= DRIVER_NAME,
+		.of_match_table = max2175_of_ids,
+	},
+	.probe		= max2175_probe,
+	.remove		= max2175_remove,
+	.id_table	= max2175_id,
+};
+
+module_i2c_driver(max2175_driver);
+
+MODULE_DESCRIPTION("Maxim MAX2175 RF to Bits tuner driver");
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Ramesh Shanmugasundaram <ramesh.shanmugasundaram@bp.renesas.com>");
diff --git a/drivers/media/i2c/max2175.h b/drivers/media/i2c/max2175.h
new file mode 100644
index 000000000..4c722ea3e
--- /dev/null
+++ b/drivers/media/i2c/max2175.h
@@ -0,0 +1,101 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Maxim Integrated MAX2175 RF to Bits tuner driver
+ *
+ * This driver & most of the hard coded values are based on the reference
+ * application delivered by Maxim for this device.
+ *
+ * Copyright (C) 2016 Maxim Integrated Products
+ * Copyright (C) 2017 Renesas Electronics Corporation
+ */
+
+#ifndef __MAX2175_H__
+#define __MAX2175_H__
+
+#define MAX2175_EU_XTAL_FREQ	36864000	/* In Hz */
+#define MAX2175_NA_XTAL_FREQ	40186125	/* In Hz */
+
+enum max2175_region {
+	MAX2175_REGION_EU = 0,	/* Europe */
+	MAX2175_REGION_NA,	/* North America */
+};
+
+enum max2175_band {
+	MAX2175_BAND_AM = 0,
+	MAX2175_BAND_FM,
+	MAX2175_BAND_VHF,
+	MAX2175_BAND_L,
+};
+
+enum max2175_eu_mode {
+	/* EU modes */
+	MAX2175_EU_FM_1_2 = 0,
+	MAX2175_DAB_1_2,
+
+	/*
+	 * Other possible modes to add in future
+	 * MAX2175_DAB_1_0,
+	 * MAX2175_DAB_1_3,
+	 * MAX2175_EU_FM_2_2,
+	 * MAX2175_EU_FMHD_4_0,
+	 * MAX2175_EU_AM_1_0,
+	 * MAX2175_EU_AM_2_2,
+	 */
+};
+
+enum max2175_na_mode {
+	/* NA modes */
+	MAX2175_NA_FM_1_0 = 0,
+	MAX2175_NA_FM_2_0,
+
+	/*
+	 * Other possible modes to add in future
+	 * MAX2175_NA_FMHD_1_0,
+	 * MAX2175_NA_FMHD_1_2,
+	 * MAX2175_NA_AM_1_0,
+	 * MAX2175_NA_AM_1_2,
+	 */
+};
+
+/* Supported I2S modes */
+enum {
+	MAX2175_I2S_MODE0 = 0,
+	MAX2175_I2S_MODE1,
+	MAX2175_I2S_MODE2,
+	MAX2175_I2S_MODE3,
+	MAX2175_I2S_MODE4,
+};
+
+/* Coefficient table groups */
+enum {
+	MAX2175_CH_MSEL = 0,
+	MAX2175_EQ_MSEL,
+	MAX2175_AA_MSEL,
+};
+
+/* HSLS LO injection polarity */
+enum {
+	MAX2175_LO_BELOW_DESIRED = 0,
+	MAX2175_LO_ABOVE_DESIRED,
+};
+
+/* Channel FSM modes */
+enum max2175_csm_mode {
+	MAX2175_LOAD_TO_BUFFER = 0,
+	MAX2175_PRESET_TUNE,
+	MAX2175_SEARCH,
+	MAX2175_AF_UPDATE,
+	MAX2175_JUMP_FAST_TUNE,
+	MAX2175_CHECK,
+	MAX2175_LOAD_AND_SWAP,
+	MAX2175_END,
+	MAX2175_BUFFER_PLUS_PRESET_TUNE,
+	MAX2175_BUFFER_PLUS_SEARCH,
+	MAX2175_BUFFER_PLUS_AF_UPDATE,
+	MAX2175_BUFFER_PLUS_JUMP_FAST_TUNE,
+	MAX2175_BUFFER_PLUS_CHECK,
+	MAX2175_BUFFER_PLUS_LOAD_AND_SWAP,
+	MAX2175_NO_ACTION
+};
+
+#endif /* __MAX2175_H__ */
diff --git a/drivers/media/i2c/max9271.c b/drivers/media/i2c/max9271.c
new file mode 100644
index 000000000..ff86c8c4e
--- /dev/null
+++ b/drivers/media/i2c/max9271.c
@@ -0,0 +1,374 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) 2017-2020 Jacopo Mondi
+ * Copyright (C) 2017-2020 Kieran Bingham
+ * Copyright (C) 2017-2020 Laurent Pinchart
+ * Copyright (C) 2017-2020 Niklas Söderlund
+ * Copyright (C) 2016 Renesas Electronics Corporation
+ * Copyright (C) 2015 Cogent Embedded, Inc.
+ *
+ * This file exports functions to control the Maxim MAX9271 GMSL serializer
+ * chip. This is not a self-contained driver, as MAX9271 is usually embedded in
+ * camera modules with at least one image sensor and optional additional
+ * components, such as uController units or ISPs/DSPs.
+ *
+ * Drivers for the camera modules (i.e. rdacm20/21) are expected to use
+ * functions exported from this library driver to maximize code re-use.
+ */
+
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+
+#include "max9271.h"
+
+static int max9271_read(struct max9271_device *dev, u8 reg)
+{
+	int ret;
+
+	dev_dbg(&dev->client->dev, "%s(0x%02x)\n", __func__, reg);
+
+	ret = i2c_smbus_read_byte_data(dev->client, reg);
+	if (ret < 0)
+		dev_dbg(&dev->client->dev,
+			"%s: register 0x%02x read failed (%d)\n",
+			__func__, reg, ret);
+
+	return ret;
+}
+
+static int max9271_write(struct max9271_device *dev, u8 reg, u8 val)
+{
+	int ret;
+
+	dev_dbg(&dev->client->dev, "%s(0x%02x, 0x%02x)\n", __func__, reg, val);
+
+	ret = i2c_smbus_write_byte_data(dev->client, reg, val);
+	if (ret < 0)
+		dev_err(&dev->client->dev,
+			"%s: register 0x%02x write failed (%d)\n",
+			__func__, reg, ret);
+
+	return ret;
+}
+
+/*
+ * max9271_pclk_detect() - Detect valid pixel clock from image sensor
+ *
+ * Wait up to 10ms for a valid pixel clock.
+ *
+ * Returns 0 for success, < 0 for pixel clock not properly detected
+ */
+static int max9271_pclk_detect(struct max9271_device *dev)
+{
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < 100; i++) {
+		ret = max9271_read(dev, 0x15);
+		if (ret < 0)
+			return ret;
+
+		if (ret & MAX9271_PCLKDET)
+			return 0;
+
+		usleep_range(50, 100);
+	}
+
+	dev_err(&dev->client->dev, "Unable to detect valid pixel clock\n");
+
+	return -EIO;
+}
+
+void max9271_wake_up(struct max9271_device *dev)
+{
+	/*
+	 * Use the chip default address as this function has to be called
+	 * before any other one.
+	 */
+	dev->client->addr = MAX9271_DEFAULT_ADDR;
+	i2c_smbus_read_byte(dev->client);
+	usleep_range(5000, 8000);
+}
+EXPORT_SYMBOL_GPL(max9271_wake_up);
+
+int max9271_set_serial_link(struct max9271_device *dev, bool enable)
+{
+	int ret;
+	u8 val = MAX9271_REVCCEN | MAX9271_FWDCCEN;
+
+	if (enable) {
+		ret = max9271_pclk_detect(dev);
+		if (ret)
+			return ret;
+
+		val |= MAX9271_SEREN;
+	} else {
+		val |= MAX9271_CLINKEN;
+	}
+
+	/*
+	 * The serializer temporarily disables the reverse control channel for
+	 * 350µs after starting/stopping the forward serial link, but the
+	 * deserializer synchronization time isn't clearly documented.
+	 *
+	 * According to the serializer datasheet we should wait 3ms, while
+	 * according to the deserializer datasheet we should wait 5ms.
+	 *
+	 * Short delays here appear to show bit-errors in the writes following.
+	 * Therefore a conservative delay seems best here.
+	 */
+	ret = max9271_write(dev, 0x04, val);
+	if (ret < 0)
+		return ret;
+
+	usleep_range(5000, 8000);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(max9271_set_serial_link);
+
+int max9271_configure_i2c(struct max9271_device *dev, u8 i2c_config)
+{
+	int ret;
+
+	ret = max9271_write(dev, 0x0d, i2c_config);
+	if (ret < 0)
+		return ret;
+
+	/* The delay required after an I2C bus configuration change is not
+	 * characterized in the serializer manual. Sleep up to 5msec to
+	 * stay safe.
+	 */
+	usleep_range(3500, 5000);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(max9271_configure_i2c);
+
+int max9271_set_high_threshold(struct max9271_device *dev, bool enable)
+{
+	int ret;
+
+	ret = max9271_read(dev, 0x08);
+	if (ret < 0)
+		return ret;
+
+	/*
+	 * Enable or disable reverse channel high threshold to increase
+	 * immunity to power supply noise.
+	 */
+	ret = max9271_write(dev, 0x08, enable ? ret | BIT(0) : ret & ~BIT(0));
+	if (ret < 0)
+		return ret;
+
+	usleep_range(2000, 2500);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(max9271_set_high_threshold);
+
+int max9271_configure_gmsl_link(struct max9271_device *dev)
+{
+	int ret;
+
+	/*
+	 * Configure the GMSL link:
+	 *
+	 * - Double input mode, high data rate, 24-bit mode
+	 * - Latch input data on PCLKIN rising edge
+	 * - Enable HS/VS encoding
+	 * - 1-bit parity error detection
+	 *
+	 * TODO: Make the GMSL link configuration parametric.
+	 */
+	ret = max9271_write(dev, 0x07, MAX9271_DBL | MAX9271_HVEN |
+			    MAX9271_EDC_1BIT_PARITY);
+	if (ret < 0)
+		return ret;
+
+	usleep_range(5000, 8000);
+
+	/*
+	 * Adjust spread spectrum to +4% and auto-detect pixel clock
+	 * and serial link rate.
+	 */
+	ret = max9271_write(dev, 0x02,
+			    MAX9271_SPREAD_SPECT_4 | MAX9271_R02_RES |
+			    MAX9271_PCLK_AUTODETECT |
+			    MAX9271_SERIAL_AUTODETECT);
+	if (ret < 0)
+		return ret;
+
+	usleep_range(5000, 8000);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(max9271_configure_gmsl_link);
+
+int max9271_set_gpios(struct max9271_device *dev, u8 gpio_mask)
+{
+	int ret;
+
+	ret = max9271_read(dev, 0x0f);
+	if (ret < 0)
+		return 0;
+
+	ret |= gpio_mask;
+	ret = max9271_write(dev, 0x0f, ret);
+	if (ret < 0) {
+		dev_err(&dev->client->dev, "Failed to set gpio (%d)\n", ret);
+		return ret;
+	}
+
+	usleep_range(3500, 5000);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(max9271_set_gpios);
+
+int max9271_clear_gpios(struct max9271_device *dev, u8 gpio_mask)
+{
+	int ret;
+
+	ret = max9271_read(dev, 0x0f);
+	if (ret < 0)
+		return 0;
+
+	ret &= ~gpio_mask;
+	ret = max9271_write(dev, 0x0f, ret);
+	if (ret < 0) {
+		dev_err(&dev->client->dev, "Failed to clear gpio (%d)\n", ret);
+		return ret;
+	}
+
+	usleep_range(3500, 5000);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(max9271_clear_gpios);
+
+int max9271_enable_gpios(struct max9271_device *dev, u8 gpio_mask)
+{
+	int ret;
+
+	ret = max9271_read(dev, 0x0e);
+	if (ret < 0)
+		return 0;
+
+	/* BIT(0) reserved: GPO is always enabled. */
+	ret |= (gpio_mask & ~BIT(0));
+	ret = max9271_write(dev, 0x0e, ret);
+	if (ret < 0) {
+		dev_err(&dev->client->dev, "Failed to enable gpio (%d)\n", ret);
+		return ret;
+	}
+
+	usleep_range(3500, 5000);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(max9271_enable_gpios);
+
+int max9271_disable_gpios(struct max9271_device *dev, u8 gpio_mask)
+{
+	int ret;
+
+	ret = max9271_read(dev, 0x0e);
+	if (ret < 0)
+		return 0;
+
+	/* BIT(0) reserved: GPO cannot be disabled */
+	ret &= ~(gpio_mask | BIT(0));
+	ret = max9271_write(dev, 0x0e, ret);
+	if (ret < 0) {
+		dev_err(&dev->client->dev, "Failed to disable gpio (%d)\n", ret);
+		return ret;
+	}
+
+	usleep_range(3500, 5000);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(max9271_disable_gpios);
+
+int max9271_verify_id(struct max9271_device *dev)
+{
+	int ret;
+
+	ret = max9271_read(dev, 0x1e);
+	if (ret < 0) {
+		dev_err(&dev->client->dev, "MAX9271 ID read failed (%d)\n",
+			ret);
+		return ret;
+	}
+
+	if (ret != MAX9271_ID) {
+		dev_err(&dev->client->dev, "MAX9271 ID mismatch (0x%02x)\n",
+			ret);
+		return -ENXIO;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(max9271_verify_id);
+
+int max9271_set_address(struct max9271_device *dev, u8 addr)
+{
+	int ret;
+
+	ret = max9271_write(dev, 0x00, addr << 1);
+	if (ret < 0) {
+		dev_err(&dev->client->dev,
+			"MAX9271 I2C address change failed (%d)\n", ret);
+		return ret;
+	}
+	usleep_range(3500, 5000);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(max9271_set_address);
+
+int max9271_set_deserializer_address(struct max9271_device *dev, u8 addr)
+{
+	int ret;
+
+	ret = max9271_write(dev, 0x01, addr << 1);
+	if (ret < 0) {
+		dev_err(&dev->client->dev,
+			"MAX9271 deserializer address set failed (%d)\n", ret);
+		return ret;
+	}
+	usleep_range(3500, 5000);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(max9271_set_deserializer_address);
+
+int max9271_set_translation(struct max9271_device *dev, u8 source, u8 dest)
+{
+	int ret;
+
+	ret = max9271_write(dev, 0x09, source << 1);
+	if (ret < 0) {
+		dev_err(&dev->client->dev,
+			"MAX9271 I2C translation setup failed (%d)\n", ret);
+		return ret;
+	}
+	usleep_range(3500, 5000);
+
+	ret = max9271_write(dev, 0x0a, dest << 1);
+	if (ret < 0) {
+		dev_err(&dev->client->dev,
+			"MAX9271 I2C translation setup failed (%d)\n", ret);
+		return ret;
+	}
+	usleep_range(3500, 5000);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(max9271_set_translation);
+
+MODULE_DESCRIPTION("Maxim MAX9271 GMSL Serializer");
+MODULE_AUTHOR("Jacopo Mondi");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/max9271.h b/drivers/media/i2c/max9271.h
new file mode 100644
index 000000000..dc5e4e70b
--- /dev/null
+++ b/drivers/media/i2c/max9271.h
@@ -0,0 +1,233 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (C) 2017-2020 Jacopo Mondi
+ * Copyright (C) 2017-2020 Kieran Bingham
+ * Copyright (C) 2017-2020 Laurent Pinchart
+ * Copyright (C) 2017-2020 Niklas Söderlund
+ * Copyright (C) 2016 Renesas Electronics Corporation
+ * Copyright (C) 2015 Cogent Embedded, Inc.
+ */
+
+#include <linux/i2c.h>
+
+#define MAX9271_DEFAULT_ADDR	0x40
+
+/* Register 0x02 */
+#define MAX9271_SPREAD_SPECT_0		(0 << 5)
+#define MAX9271_SPREAD_SPECT_05		(1 << 5)
+#define MAX9271_SPREAD_SPECT_15		(2 << 5)
+#define MAX9271_SPREAD_SPECT_1		(5 << 5)
+#define MAX9271_SPREAD_SPECT_2		(3 << 5)
+#define MAX9271_SPREAD_SPECT_3		(6 << 5)
+#define MAX9271_SPREAD_SPECT_4		(7 << 5)
+#define MAX9271_R02_RES			BIT(4)
+#define MAX9271_PCLK_AUTODETECT		(3 << 2)
+#define MAX9271_SERIAL_AUTODETECT	(0x03)
+/* Register 0x04 */
+#define MAX9271_SEREN			BIT(7)
+#define MAX9271_CLINKEN			BIT(6)
+#define MAX9271_PRBSEN			BIT(5)
+#define MAX9271_SLEEP			BIT(4)
+#define MAX9271_INTTYPE_I2C		(0 << 2)
+#define MAX9271_INTTYPE_UART		(1 << 2)
+#define MAX9271_INTTYPE_NONE		(2 << 2)
+#define MAX9271_REVCCEN			BIT(1)
+#define MAX9271_FWDCCEN			BIT(0)
+/* Register 0x07 */
+#define MAX9271_DBL			BIT(7)
+#define MAX9271_DRS			BIT(6)
+#define MAX9271_BWS			BIT(5)
+#define MAX9271_ES			BIT(4)
+#define MAX9271_HVEN			BIT(2)
+#define MAX9271_EDC_1BIT_PARITY		(0 << 0)
+#define MAX9271_EDC_6BIT_CRC		(1 << 0)
+#define MAX9271_EDC_6BIT_HAMMING	(2 << 0)
+/* Register 0x08 */
+#define MAX9271_INVVS			BIT(7)
+#define MAX9271_INVHS			BIT(6)
+#define MAX9271_REV_LOGAIN		BIT(3)
+#define MAX9271_REV_HIVTH		BIT(0)
+/* Register 0x09 */
+#define MAX9271_ID			0x09
+/* Register 0x0d */
+#define MAX9271_I2CLOCACK		BIT(7)
+#define MAX9271_I2CSLVSH_1046NS_469NS	(3 << 5)
+#define MAX9271_I2CSLVSH_938NS_352NS	(2 << 5)
+#define MAX9271_I2CSLVSH_469NS_234NS	(1 << 5)
+#define MAX9271_I2CSLVSH_352NS_117NS	(0 << 5)
+#define MAX9271_I2CMSTBT_837KBPS	(7 << 2)
+#define MAX9271_I2CMSTBT_533KBPS	(6 << 2)
+#define MAX9271_I2CMSTBT_339KBPS	(5 << 2)
+#define MAX9271_I2CMSTBT_173KBPS	(4 << 2)
+#define MAX9271_I2CMSTBT_105KBPS	(3 << 2)
+#define MAX9271_I2CMSTBT_84KBPS		(2 << 2)
+#define MAX9271_I2CMSTBT_28KBPS		(1 << 2)
+#define MAX9271_I2CMSTBT_8KBPS		(0 << 2)
+#define MAX9271_I2CSLVTO_NONE		(3 << 0)
+#define MAX9271_I2CSLVTO_1024US		(2 << 0)
+#define MAX9271_I2CSLVTO_256US		(1 << 0)
+#define MAX9271_I2CSLVTO_64US		(0 << 0)
+/* Register 0x0f */
+#define MAX9271_GPIO5OUT		BIT(5)
+#define MAX9271_GPIO4OUT		BIT(4)
+#define MAX9271_GPIO3OUT		BIT(3)
+#define MAX9271_GPIO2OUT		BIT(2)
+#define MAX9271_GPIO1OUT		BIT(1)
+#define MAX9271_GPO			BIT(0)
+/* Register 0x15 */
+#define MAX9271_PCLKDET			BIT(0)
+
+/**
+ * struct max9271_device - max9271 device
+ * @client: The i2c client for the max9271 instance
+ */
+struct max9271_device {
+	struct i2c_client *client;
+};
+
+/**
+ * max9271_wake_up() - Wake up the serializer by issuing an i2c transaction
+ * @dev: The max9271 device
+ *
+ * This function shall be called before any other interaction with the
+ * serializer.
+ */
+void max9271_wake_up(struct max9271_device *dev);
+
+/**
+ * max9271_set_serial_link() - Enable/disable serial link
+ * @dev: The max9271 device
+ * @enable: Serial link enable/disable flag
+ *
+ * Return 0 on success or a negative error code on failure
+ */
+int max9271_set_serial_link(struct max9271_device *dev, bool enable);
+
+/**
+ * max9271_configure_i2c() - Configure I2C bus parameters
+ * @dev: The max9271 device
+ * @i2c_config: The I2C bus configuration bit mask
+ *
+ * Configure MAX9271 I2C interface. The bus configuration provided in the
+ * @i2c_config parameter shall be assembled using bit values defined by the
+ * MAX9271_I2C* macros.
+ *
+ * Return 0 on success or a negative error code on failure
+ */
+int max9271_configure_i2c(struct max9271_device *dev, u8 i2c_config);
+
+/**
+ * max9271_set_high_threshold() - Enable or disable reverse channel high
+ *				  threshold
+ * @dev: The max9271 device
+ * @enable: High threshold enable/disable flag
+ *
+ * Return 0 on success or a negative error code on failure
+ */
+int max9271_set_high_threshold(struct max9271_device *dev, bool enable);
+
+/**
+ * max9271_configure_gmsl_link() - Configure the GMSL link
+ * @dev: The max9271 device
+ *
+ * FIXME: the GMSL link configuration is currently hardcoded and performed
+ * by programming registers 0x04, 0x07 and 0x02.
+ *
+ * Return 0 on success or a negative error code on failure
+ */
+int max9271_configure_gmsl_link(struct max9271_device *dev);
+
+/**
+ * max9271_set_gpios() - Set gpio lines to physical high value
+ * @dev: The max9271 device
+ * @gpio_mask: The mask of gpio lines to set to high value
+ *
+ * The @gpio_mask parameter shall be assembled using the MAX9271_GP[IO|O]*
+ * bit values.
+ *
+ * Return 0 on success or a negative error code on failure
+ */
+int max9271_set_gpios(struct max9271_device *dev, u8 gpio_mask);
+
+/**
+ * max9271_clear_gpios() - Set gpio lines to physical low value
+ * @dev: The max9271 device
+ * @gpio_mask: The mask of gpio lines to set to low value
+ *
+ * The @gpio_mask parameter shall be assembled using the MAX9271_GP[IO|O]*
+ * bit values.
+ *
+ * Return 0 on success or a negative error code on failure
+ */
+int max9271_clear_gpios(struct max9271_device *dev, u8 gpio_mask);
+
+/**
+ * max9271_enable_gpios() - Enable gpio lines
+ * @dev: The max9271 device
+ * @gpio_mask: The mask of gpio lines to enable
+ *
+ * The @gpio_mask parameter shall be assembled using the MAX9271_GPIO*
+ * bit values. GPO line is always enabled by default.
+ *
+ * Return 0 on success or a negative error code on failure
+ */
+int max9271_enable_gpios(struct max9271_device *dev, u8 gpio_mask);
+
+/**
+ * max9271_disable_gpios() - Disable gpio lines
+ * @dev: The max9271 device
+ * @gpio_mask: The mask of gpio lines to disable
+ *
+ * The @gpio_mask parameter shall be assembled using the MAX9271_GPIO*
+ * bit values. GPO line is always enabled by default and cannot be disabled.
+ *
+ * Return 0 on success or a negative error code on failure
+ */
+int max9271_disable_gpios(struct max9271_device *dev, u8 gpio_mask);
+
+/**
+ * max9271_verify_id() - Read and verify MAX9271 id
+ * @dev: The max9271 device
+ *
+ * Return 0 on success or a negative error code on failure
+ */
+int max9271_verify_id(struct max9271_device *dev);
+
+/**
+ * max9271_set_address() - Program a new I2C address
+ * @dev: The max9271 device
+ * @addr: The new I2C address in 7-bit format
+ *
+ * This function only takes care of programming the new I2C address @addr to
+ * in the MAX9271 chip registers, it is responsiblity of the caller to set
+ * the i2c address client to the @addr value to be able to communicate with
+ * the MAX9271 chip using the I2C framework APIs after this function returns.
+ *
+ * Return 0 on success or a negative error code on failure
+ */
+int max9271_set_address(struct max9271_device *dev, u8 addr);
+
+/**
+ * max9271_set_deserializer_address() - Program the remote deserializer address
+ * @dev: The max9271 device
+ * @addr: The deserializer I2C address in 7-bit format
+ *
+ * Return 0 on success or a negative error code on failure
+ */
+int max9271_set_deserializer_address(struct max9271_device *dev, u8 addr);
+
+/**
+ * max9271_set_translation() - Program I2C address translation
+ * @dev: The max9271 device
+ * @source: The I2C source address
+ * @dest: The I2C destination address
+ *
+ * Program address translation from @source to @dest. This is required to
+ * communicate with local devices that do not support address reprogramming.
+ *
+ * TODO: The device supports translation of two address, this function currently
+ * supports a single one.
+ *
+ * Return 0 on success or a negative error code on failure
+ */
+int max9271_set_translation(struct max9271_device *dev, u8 source, u8 dest);
diff --git a/drivers/media/i2c/max9286.c b/drivers/media/i2c/max9286.c
new file mode 100644
index 000000000..fc1cf196e
--- /dev/null
+++ b/drivers/media/i2c/max9286.c
@@ -0,0 +1,1713 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Maxim MAX9286 GMSL Deserializer Driver
+ *
+ * Copyright (C) 2017-2019 Jacopo Mondi
+ * Copyright (C) 2017-2019 Kieran Bingham
+ * Copyright (C) 2017-2019 Laurent Pinchart
+ * Copyright (C) 2017-2019 Niklas Söderlund
+ * Copyright (C) 2016 Renesas Electronics Corporation
+ * Copyright (C) 2015 Cogent Embedded, Inc.
+ */
+
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/fwnode.h>
+#include <linux/gpio/consumer.h>
+#include <linux/gpio/driver.h>
+#include <linux/gpio/machine.h>
+#include <linux/i2c.h>
+#include <linux/i2c-mux.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/of_graph.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+
+#include <media/v4l2-async.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+/* Register 0x00 */
+#define MAX9286_MSTLINKSEL_AUTO		(7 << 5)
+#define MAX9286_MSTLINKSEL(n)		((n) << 5)
+#define MAX9286_EN_VS_GEN		BIT(4)
+#define MAX9286_LINKEN(n)		(1 << (n))
+/* Register 0x01 */
+#define MAX9286_FSYNCMODE_ECU		(3 << 6)
+#define MAX9286_FSYNCMODE_EXT		(2 << 6)
+#define MAX9286_FSYNCMODE_INT_OUT	(1 << 6)
+#define MAX9286_FSYNCMODE_INT_HIZ	(0 << 6)
+#define MAX9286_GPIEN			BIT(5)
+#define MAX9286_ENLMO_RSTFSYNC		BIT(2)
+#define MAX9286_FSYNCMETH_AUTO		(2 << 0)
+#define MAX9286_FSYNCMETH_SEMI_AUTO	(1 << 0)
+#define MAX9286_FSYNCMETH_MANUAL	(0 << 0)
+#define MAX9286_REG_FSYNC_PERIOD_L	0x06
+#define MAX9286_REG_FSYNC_PERIOD_M	0x07
+#define MAX9286_REG_FSYNC_PERIOD_H	0x08
+/* Register 0x0a */
+#define MAX9286_FWDCCEN(n)		(1 << ((n) + 4))
+#define MAX9286_REVCCEN(n)		(1 << (n))
+/* Register 0x0c */
+#define MAX9286_HVEN			BIT(7)
+#define MAX9286_EDC_6BIT_HAMMING	(2 << 5)
+#define MAX9286_EDC_6BIT_CRC		(1 << 5)
+#define MAX9286_EDC_1BIT_PARITY		(0 << 5)
+#define MAX9286_DESEL			BIT(4)
+#define MAX9286_INVVS			BIT(3)
+#define MAX9286_INVHS			BIT(2)
+#define MAX9286_HVSRC_D0		(2 << 0)
+#define MAX9286_HVSRC_D14		(1 << 0)
+#define MAX9286_HVSRC_D18		(0 << 0)
+/* Register 0x0f */
+#define MAX9286_0X0F_RESERVED		BIT(3)
+/* Register 0x12 */
+#define MAX9286_CSILANECNT(n)		(((n) - 1) << 6)
+#define MAX9286_CSIDBL			BIT(5)
+#define MAX9286_DBL			BIT(4)
+#define MAX9286_DATATYPE_USER_8BIT	(11 << 0)
+#define MAX9286_DATATYPE_USER_YUV_12BIT	(10 << 0)
+#define MAX9286_DATATYPE_USER_24BIT	(9 << 0)
+#define MAX9286_DATATYPE_RAW14		(8 << 0)
+#define MAX9286_DATATYPE_RAW12		(7 << 0)
+#define MAX9286_DATATYPE_RAW10		(6 << 0)
+#define MAX9286_DATATYPE_RAW8		(5 << 0)
+#define MAX9286_DATATYPE_YUV422_10BIT	(4 << 0)
+#define MAX9286_DATATYPE_YUV422_8BIT	(3 << 0)
+#define MAX9286_DATATYPE_RGB555		(2 << 0)
+#define MAX9286_DATATYPE_RGB565		(1 << 0)
+#define MAX9286_DATATYPE_RGB888		(0 << 0)
+/* Register 0x15 */
+#define MAX9286_CSI_IMAGE_TYP		BIT(7)
+#define MAX9286_VC(n)			((n) << 5)
+#define MAX9286_VCTYPE			BIT(4)
+#define MAX9286_CSIOUTEN		BIT(3)
+#define MAX9286_SWP_ENDIAN		BIT(2)
+#define MAX9286_EN_CCBSYB_CLK_STR	BIT(1)
+#define MAX9286_EN_GPI_CCBSYB		BIT(0)
+/* Register 0x1b */
+#define MAX9286_SWITCHIN(n)		(1 << ((n) + 4))
+#define MAX9286_ENEQ(n)			(1 << (n))
+/* Register 0x1c */
+#define MAX9286_HIGHIMM(n)		BIT((n) + 4)
+#define MAX9286_I2CSEL			BIT(2)
+#define MAX9286_HIBW			BIT(1)
+#define MAX9286_BWS			BIT(0)
+/* Register 0x27 */
+#define MAX9286_LOCKED			BIT(7)
+/* Register 0x31 */
+#define MAX9286_FSYNC_LOCKED		BIT(6)
+/* Register 0x34 */
+#define MAX9286_I2CLOCACK		BIT(7)
+#define MAX9286_I2CSLVSH_1046NS_469NS	(3 << 5)
+#define MAX9286_I2CSLVSH_938NS_352NS	(2 << 5)
+#define MAX9286_I2CSLVSH_469NS_234NS	(1 << 5)
+#define MAX9286_I2CSLVSH_352NS_117NS	(0 << 5)
+#define MAX9286_I2CMSTBT_837KBPS	(7 << 2)
+#define MAX9286_I2CMSTBT_533KBPS	(6 << 2)
+#define MAX9286_I2CMSTBT_339KBPS	(5 << 2)
+#define MAX9286_I2CMSTBT_173KBPS	(4 << 2)
+#define MAX9286_I2CMSTBT_105KBPS	(3 << 2)
+#define MAX9286_I2CMSTBT_84KBPS		(2 << 2)
+#define MAX9286_I2CMSTBT_28KBPS		(1 << 2)
+#define MAX9286_I2CMSTBT_8KBPS		(0 << 2)
+#define MAX9286_I2CSLVTO_NONE		(3 << 0)
+#define MAX9286_I2CSLVTO_1024US		(2 << 0)
+#define MAX9286_I2CSLVTO_256US		(1 << 0)
+#define MAX9286_I2CSLVTO_64US		(0 << 0)
+/* Register 0x3b */
+#define MAX9286_REV_TRF(n)		((n) << 4)
+#define MAX9286_REV_AMP(n)		((((n) - 30) / 10) << 1) /* in mV */
+#define MAX9286_REV_AMP_X		BIT(0)
+#define MAX9286_REV_AMP_HIGH		170
+/* Register 0x3f */
+#define MAX9286_EN_REV_CFG		BIT(6)
+#define MAX9286_REV_FLEN(n)		((n) - 20)
+/* Register 0x49 */
+#define MAX9286_VIDEO_DETECT_MASK	0x0f
+/* Register 0x69 */
+#define MAX9286_LFLTBMONMASKED		BIT(7)
+#define MAX9286_LOCKMONMASKED		BIT(6)
+#define MAX9286_AUTOCOMBACKEN		BIT(5)
+#define MAX9286_AUTOMASKEN		BIT(4)
+#define MAX9286_MASKLINK(n)		((n) << 0)
+
+/*
+ * The sink and source pads are created to match the OF graph port numbers so
+ * that their indexes can be used interchangeably.
+ */
+#define MAX9286_NUM_GMSL		4
+#define MAX9286_N_SINKS			4
+#define MAX9286_N_PADS			5
+#define MAX9286_SRC_PAD			4
+
+struct max9286_format_info {
+	u32 code;
+	u8 datatype;
+};
+
+struct max9286_i2c_speed {
+	u32 rate;
+	u8 mstbt;
+};
+
+struct max9286_source {
+	struct v4l2_subdev *sd;
+	struct fwnode_handle *fwnode;
+	struct regulator *regulator;
+};
+
+struct max9286_asd {
+	struct v4l2_async_connection base;
+	struct max9286_source *source;
+};
+
+static inline struct max9286_asd *
+to_max9286_asd(struct v4l2_async_connection *asd)
+{
+	return container_of(asd, struct max9286_asd, base);
+}
+
+struct max9286_priv {
+	struct i2c_client *client;
+	struct gpio_desc *gpiod_pwdn;
+	struct v4l2_subdev sd;
+	struct media_pad pads[MAX9286_N_PADS];
+	struct regulator *regulator;
+
+	struct gpio_chip gpio;
+	u8 gpio_state;
+
+	struct i2c_mux_core *mux;
+	unsigned int mux_channel;
+	bool mux_open;
+
+	/* The initial reverse control channel amplitude. */
+	u32 init_rev_chan_mv;
+	u32 rev_chan_mv;
+	u8 i2c_mstbt;
+	u32 bus_width;
+
+	bool use_gpio_poc;
+	u32 gpio_poc[2];
+
+	struct v4l2_ctrl_handler ctrls;
+	struct v4l2_ctrl *pixelrate_ctrl;
+	unsigned int pixelrate;
+
+	struct v4l2_mbus_framefmt fmt[MAX9286_N_SINKS];
+	struct v4l2_fract interval;
+
+	/* Protects controls and fmt structures */
+	struct mutex mutex;
+
+	unsigned int nsources;
+	unsigned int source_mask;
+	unsigned int route_mask;
+	unsigned int bound_sources;
+	unsigned int csi2_data_lanes;
+	struct max9286_source sources[MAX9286_NUM_GMSL];
+	struct v4l2_async_notifier notifier;
+};
+
+static struct max9286_source *next_source(struct max9286_priv *priv,
+					  struct max9286_source *source)
+{
+	if (!source)
+		source = &priv->sources[0];
+	else
+		source++;
+
+	for (; source < &priv->sources[MAX9286_NUM_GMSL]; source++) {
+		if (source->fwnode)
+			return source;
+	}
+
+	return NULL;
+}
+
+#define for_each_source(priv, source) \
+	for ((source) = NULL; ((source) = next_source((priv), (source))); )
+
+#define to_index(priv, source) ((source) - &(priv)->sources[0])
+
+static inline struct max9286_priv *sd_to_max9286(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct max9286_priv, sd);
+}
+
+static const struct max9286_format_info max9286_formats[] = {
+	{
+		.code = MEDIA_BUS_FMT_UYVY8_1X16,
+		.datatype = MAX9286_DATATYPE_YUV422_8BIT,
+	}, {
+		.code = MEDIA_BUS_FMT_VYUY8_1X16,
+		.datatype = MAX9286_DATATYPE_YUV422_8BIT,
+	}, {
+		.code = MEDIA_BUS_FMT_YUYV8_1X16,
+		.datatype = MAX9286_DATATYPE_YUV422_8BIT,
+	}, {
+		.code = MEDIA_BUS_FMT_YVYU8_1X16,
+		.datatype = MAX9286_DATATYPE_YUV422_8BIT,
+	}, {
+		.code = MEDIA_BUS_FMT_SBGGR12_1X12,
+		.datatype = MAX9286_DATATYPE_RAW12,
+	}, {
+		.code = MEDIA_BUS_FMT_SGBRG12_1X12,
+		.datatype = MAX9286_DATATYPE_RAW12,
+	}, {
+		.code = MEDIA_BUS_FMT_SGRBG12_1X12,
+		.datatype = MAX9286_DATATYPE_RAW12,
+	}, {
+		.code = MEDIA_BUS_FMT_SRGGB12_1X12,
+		.datatype = MAX9286_DATATYPE_RAW12,
+	},
+};
+
+static const struct max9286_i2c_speed max9286_i2c_speeds[] = {
+	{ .rate =   8470, .mstbt = MAX9286_I2CMSTBT_8KBPS },
+	{ .rate =  28300, .mstbt = MAX9286_I2CMSTBT_28KBPS },
+	{ .rate =  84700, .mstbt = MAX9286_I2CMSTBT_84KBPS },
+	{ .rate = 105000, .mstbt = MAX9286_I2CMSTBT_105KBPS },
+	{ .rate = 173000, .mstbt = MAX9286_I2CMSTBT_173KBPS },
+	{ .rate = 339000, .mstbt = MAX9286_I2CMSTBT_339KBPS },
+	{ .rate = 533000, .mstbt = MAX9286_I2CMSTBT_533KBPS },
+	{ .rate = 837000, .mstbt = MAX9286_I2CMSTBT_837KBPS },
+};
+
+/* -----------------------------------------------------------------------------
+ * I2C IO
+ */
+
+static int max9286_read(struct max9286_priv *priv, u8 reg)
+{
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(priv->client, reg);
+	if (ret < 0)
+		dev_err(&priv->client->dev,
+			"%s: register 0x%02x read failed (%d)\n",
+			__func__, reg, ret);
+
+	return ret;
+}
+
+static int max9286_write(struct max9286_priv *priv, u8 reg, u8 val)
+{
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(priv->client, reg, val);
+	if (ret < 0)
+		dev_err(&priv->client->dev,
+			"%s: register 0x%02x write failed (%d)\n",
+			__func__, reg, ret);
+
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * I2C Multiplexer
+ */
+
+static void max9286_i2c_mux_configure(struct max9286_priv *priv, u8 conf)
+{
+	max9286_write(priv, 0x0a, conf);
+
+	/*
+	 * We must sleep after any change to the forward or reverse channel
+	 * configuration.
+	 */
+	usleep_range(3000, 5000);
+}
+
+static void max9286_i2c_mux_open(struct max9286_priv *priv)
+{
+	/* Open all channels on the MAX9286 */
+	max9286_i2c_mux_configure(priv, 0xff);
+
+	priv->mux_open = true;
+}
+
+static void max9286_i2c_mux_close(struct max9286_priv *priv)
+{
+	/*
+	 * Ensure that both the forward and reverse channel are disabled on the
+	 * mux, and that the channel ID is invalidated to ensure we reconfigure
+	 * on the next max9286_i2c_mux_select() call.
+	 */
+	max9286_i2c_mux_configure(priv, 0x00);
+
+	priv->mux_open = false;
+	priv->mux_channel = -1;
+}
+
+static int max9286_i2c_mux_select(struct i2c_mux_core *muxc, u32 chan)
+{
+	struct max9286_priv *priv = i2c_mux_priv(muxc);
+
+	/* Channel select is disabled when configured in the opened state. */
+	if (priv->mux_open)
+		return 0;
+
+	if (priv->mux_channel == chan)
+		return 0;
+
+	priv->mux_channel = chan;
+
+	max9286_i2c_mux_configure(priv, MAX9286_FWDCCEN(chan) |
+					MAX9286_REVCCEN(chan));
+
+	return 0;
+}
+
+static int max9286_i2c_mux_init(struct max9286_priv *priv)
+{
+	struct max9286_source *source;
+	int ret;
+
+	if (!i2c_check_functionality(priv->client->adapter,
+				     I2C_FUNC_SMBUS_WRITE_BYTE_DATA))
+		return -ENODEV;
+
+	priv->mux = i2c_mux_alloc(priv->client->adapter, &priv->client->dev,
+				  priv->nsources, 0, I2C_MUX_LOCKED,
+				  max9286_i2c_mux_select, NULL);
+	if (!priv->mux)
+		return -ENOMEM;
+
+	priv->mux->priv = priv;
+
+	for_each_source(priv, source) {
+		unsigned int index = to_index(priv, source);
+
+		ret = i2c_mux_add_adapter(priv->mux, 0, index, 0);
+		if (ret < 0)
+			goto error;
+	}
+
+	return 0;
+
+error:
+	i2c_mux_del_adapters(priv->mux);
+	return ret;
+}
+
+static void max9286_configure_i2c(struct max9286_priv *priv, bool localack)
+{
+	u8 config = MAX9286_I2CSLVSH_469NS_234NS | MAX9286_I2CSLVTO_1024US |
+		    priv->i2c_mstbt;
+
+	if (localack)
+		config |= MAX9286_I2CLOCACK;
+
+	max9286_write(priv, 0x34, config);
+	usleep_range(3000, 5000);
+}
+
+static void max9286_reverse_channel_setup(struct max9286_priv *priv,
+					  unsigned int chan_amplitude)
+{
+	u8 chan_config;
+
+	if (priv->rev_chan_mv == chan_amplitude)
+		return;
+
+	priv->rev_chan_mv = chan_amplitude;
+
+	/* Reverse channel transmission time: default to 1. */
+	chan_config = MAX9286_REV_TRF(1);
+
+	/*
+	 * Reverse channel setup.
+	 *
+	 * - Enable custom reverse channel configuration (through register 0x3f)
+	 *   and set the first pulse length to 35 clock cycles.
+	 * - Adjust reverse channel amplitude: values > 130 are programmed
+	 *   using the additional +100mV REV_AMP_X boost flag
+	 */
+	max9286_write(priv, 0x3f, MAX9286_EN_REV_CFG | MAX9286_REV_FLEN(35));
+
+	if (chan_amplitude > 100) {
+		/* It is not possible to express values (100 < x < 130) */
+		chan_amplitude = max(30U, chan_amplitude - 100);
+		chan_config |= MAX9286_REV_AMP_X;
+	}
+	max9286_write(priv, 0x3b, chan_config | MAX9286_REV_AMP(chan_amplitude));
+	usleep_range(2000, 2500);
+}
+
+/*
+ * max9286_check_video_links() - Make sure video links are detected and locked
+ *
+ * Performs safety checks on video link status. Make sure they are detected
+ * and all enabled links are locked.
+ *
+ * Returns 0 for success, -EIO for errors.
+ */
+static int max9286_check_video_links(struct max9286_priv *priv)
+{
+	unsigned int i;
+	int ret;
+
+	/*
+	 * Make sure valid video links are detected.
+	 * The delay is not characterized in de-serializer manual, wait up
+	 * to 5 ms.
+	 */
+	for (i = 0; i < 10; i++) {
+		ret = max9286_read(priv, 0x49);
+		if (ret < 0)
+			return -EIO;
+
+		if ((ret & MAX9286_VIDEO_DETECT_MASK) == priv->source_mask)
+			break;
+
+		usleep_range(350, 500);
+	}
+
+	if (i == 10) {
+		dev_err(&priv->client->dev,
+			"Unable to detect video links: 0x%02x\n", ret);
+		return -EIO;
+	}
+
+	/* Make sure all enabled links are locked (4ms max). */
+	for (i = 0; i < 10; i++) {
+		ret = max9286_read(priv, 0x27);
+		if (ret < 0)
+			return -EIO;
+
+		if (ret & MAX9286_LOCKED)
+			break;
+
+		usleep_range(350, 450);
+	}
+
+	if (i == 10) {
+		dev_err(&priv->client->dev, "Not all enabled links locked\n");
+		return -EIO;
+	}
+
+	return 0;
+}
+
+/*
+ * max9286_check_config_link() - Detect and wait for configuration links
+ *
+ * Determine if the configuration channel is up and settled for a link.
+ *
+ * Returns 0 for success, -EIO for errors.
+ */
+static int max9286_check_config_link(struct max9286_priv *priv,
+				     unsigned int source_mask)
+{
+	unsigned int conflink_mask = (source_mask & 0x0f) << 4;
+	unsigned int i;
+	int ret;
+
+	/*
+	 * Make sure requested configuration links are detected.
+	 * The delay is not characterized in the chip manual: wait up
+	 * to 5 milliseconds.
+	 */
+	for (i = 0; i < 10; i++) {
+		ret = max9286_read(priv, 0x49);
+		if (ret < 0)
+			return -EIO;
+
+		ret &= 0xf0;
+		if (ret == conflink_mask)
+			break;
+
+		usleep_range(350, 500);
+	}
+
+	if (ret != conflink_mask) {
+		dev_err(&priv->client->dev,
+			"Unable to detect configuration links: 0x%02x expected 0x%02x\n",
+			ret, conflink_mask);
+		return -EIO;
+	}
+
+	dev_info(&priv->client->dev,
+		 "Successfully detected configuration links after %u loops: 0x%02x\n",
+		 i, conflink_mask);
+
+	return 0;
+}
+
+static void max9286_set_video_format(struct max9286_priv *priv,
+				     const struct v4l2_mbus_framefmt *format)
+{
+	const struct max9286_format_info *info = NULL;
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(max9286_formats); ++i) {
+		if (max9286_formats[i].code == format->code) {
+			info = &max9286_formats[i];
+			break;
+		}
+	}
+
+	if (WARN_ON(!info))
+		return;
+
+	/*
+	 * Video format setup: disable CSI output, set VC according to Link
+	 * number, enable I2C clock stretching when CCBSY is low, enable CCBSY
+	 * in external GPI-to-GPO mode.
+	 */
+	max9286_write(priv, 0x15, MAX9286_VCTYPE | MAX9286_EN_CCBSYB_CLK_STR |
+		      MAX9286_EN_GPI_CCBSYB);
+
+	/* Enable CSI-2 Lane D0-D3 only, DBL mode. */
+	max9286_write(priv, 0x12, MAX9286_CSIDBL | MAX9286_DBL |
+		      MAX9286_CSILANECNT(priv->csi2_data_lanes) |
+		      info->datatype);
+
+	/*
+	 * Enable HS/VS encoding, use HS as line valid source, use D14/15 for
+	 * HS/VS, invert VS.
+	 */
+	max9286_write(priv, 0x0c, MAX9286_HVEN | MAX9286_DESEL |
+		      MAX9286_INVVS | MAX9286_HVSRC_D14);
+}
+
+static void max9286_set_fsync_period(struct max9286_priv *priv)
+{
+	u32 fsync;
+
+	if (!priv->interval.numerator || !priv->interval.denominator) {
+		/*
+		 * Special case, a null interval enables automatic FRAMESYNC
+		 * mode. FRAMESYNC is taken from the slowest link.
+		 */
+		max9286_write(priv, 0x01, MAX9286_FSYNCMODE_INT_HIZ |
+			      MAX9286_FSYNCMETH_AUTO);
+		return;
+	}
+
+	/*
+	 * Manual FRAMESYNC
+	 *
+	 * The FRAMESYNC generator is configured with a period expressed as a
+	 * number of PCLK periods.
+	 */
+	fsync = div_u64((u64)priv->pixelrate * priv->interval.numerator,
+			priv->interval.denominator);
+
+	dev_dbg(&priv->client->dev, "fsync period %u (pclk %u)\n", fsync,
+		priv->pixelrate);
+
+	max9286_write(priv, 0x01, MAX9286_FSYNCMODE_INT_OUT |
+		      MAX9286_FSYNCMETH_MANUAL);
+
+	max9286_write(priv, 0x06, (fsync >> 0) & 0xff);
+	max9286_write(priv, 0x07, (fsync >> 8) & 0xff);
+	max9286_write(priv, 0x08, (fsync >> 16) & 0xff);
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdev
+ */
+
+static int max9286_set_pixelrate(struct max9286_priv *priv)
+{
+	struct max9286_source *source = NULL;
+	u64 pixelrate = 0;
+
+	for_each_source(priv, source) {
+		struct v4l2_ctrl *ctrl;
+		u64 source_rate = 0;
+
+		/* Pixel rate is mandatory to be reported by sources. */
+		ctrl = v4l2_ctrl_find(source->sd->ctrl_handler,
+				      V4L2_CID_PIXEL_RATE);
+		if (!ctrl) {
+			pixelrate = 0;
+			break;
+		}
+
+		/* All source must report the same pixel rate. */
+		source_rate = v4l2_ctrl_g_ctrl_int64(ctrl);
+		if (!pixelrate) {
+			pixelrate = source_rate;
+		} else if (pixelrate != source_rate) {
+			dev_err(&priv->client->dev,
+				"Unable to calculate pixel rate\n");
+			return -EINVAL;
+		}
+	}
+
+	if (!pixelrate) {
+		dev_err(&priv->client->dev,
+			"No pixel rate control available in sources\n");
+		return -EINVAL;
+	}
+
+	priv->pixelrate = pixelrate;
+
+	/*
+	 * The CSI-2 transmitter pixel rate is the single source rate multiplied
+	 * by the number of available sources.
+	 */
+	return v4l2_ctrl_s_ctrl_int64(priv->pixelrate_ctrl,
+				      pixelrate * priv->nsources);
+}
+
+static int max9286_notify_bound(struct v4l2_async_notifier *notifier,
+				struct v4l2_subdev *subdev,
+				struct v4l2_async_connection *asd)
+{
+	struct max9286_priv *priv = sd_to_max9286(notifier->sd);
+	struct max9286_source *source = to_max9286_asd(asd)->source;
+	unsigned int index = to_index(priv, source);
+	unsigned int src_pad;
+	int ret;
+
+	ret = media_entity_get_fwnode_pad(&subdev->entity,
+					  source->fwnode,
+					  MEDIA_PAD_FL_SOURCE);
+	if (ret < 0) {
+		dev_err(&priv->client->dev,
+			"Failed to find pad for %s\n", subdev->name);
+		return ret;
+	}
+
+	priv->bound_sources |= BIT(index);
+	source->sd = subdev;
+	src_pad = ret;
+
+	ret = media_create_pad_link(&source->sd->entity, src_pad,
+				    &priv->sd.entity, index,
+				    MEDIA_LNK_FL_ENABLED |
+				    MEDIA_LNK_FL_IMMUTABLE);
+	if (ret) {
+		dev_err(&priv->client->dev,
+			"Unable to link %s:%u -> %s:%u\n",
+			source->sd->name, src_pad, priv->sd.name, index);
+		return ret;
+	}
+
+	dev_dbg(&priv->client->dev, "Bound %s pad: %u on index %u\n",
+		subdev->name, src_pad, index);
+
+	/*
+	 * As we register a subdev notifiers we won't get a .complete() callback
+	 * here, so we have to use bound_sources to identify when all remote
+	 * serializers have probed.
+	 */
+	if (priv->bound_sources != priv->source_mask)
+		return 0;
+
+	/*
+	 * All enabled sources have probed and enabled their reverse control
+	 * channels:
+	 *
+	 * - Increase the reverse channel amplitude to compensate for the
+	 *   remote ends high threshold
+	 * - Verify all configuration links are properly detected
+	 * - Disable auto-ack as communication on the control channel are now
+	 *   stable.
+	 */
+	max9286_reverse_channel_setup(priv, MAX9286_REV_AMP_HIGH);
+	max9286_check_config_link(priv, priv->source_mask);
+	max9286_configure_i2c(priv, false);
+
+	return max9286_set_pixelrate(priv);
+}
+
+static void max9286_notify_unbind(struct v4l2_async_notifier *notifier,
+				  struct v4l2_subdev *subdev,
+				  struct v4l2_async_connection *asd)
+{
+	struct max9286_priv *priv = sd_to_max9286(notifier->sd);
+	struct max9286_source *source = to_max9286_asd(asd)->source;
+	unsigned int index = to_index(priv, source);
+
+	source->sd = NULL;
+	priv->bound_sources &= ~BIT(index);
+}
+
+static const struct v4l2_async_notifier_operations max9286_notify_ops = {
+	.bound = max9286_notify_bound,
+	.unbind = max9286_notify_unbind,
+};
+
+static int max9286_v4l2_notifier_register(struct max9286_priv *priv)
+{
+	struct device *dev = &priv->client->dev;
+	struct max9286_source *source = NULL;
+	int ret;
+
+	if (!priv->nsources)
+		return 0;
+
+	v4l2_async_subdev_nf_init(&priv->notifier, &priv->sd);
+
+	for_each_source(priv, source) {
+		unsigned int i = to_index(priv, source);
+		struct max9286_asd *mas;
+
+		mas = v4l2_async_nf_add_fwnode(&priv->notifier, source->fwnode,
+					       struct max9286_asd);
+		if (IS_ERR(mas)) {
+			dev_err(dev, "Failed to add subdev for source %u: %ld",
+				i, PTR_ERR(mas));
+			v4l2_async_nf_cleanup(&priv->notifier);
+			return PTR_ERR(mas);
+		}
+
+		mas->source = source;
+	}
+
+	priv->notifier.ops = &max9286_notify_ops;
+
+	ret = v4l2_async_nf_register(&priv->notifier);
+	if (ret) {
+		dev_err(dev, "Failed to register subdev_notifier");
+		v4l2_async_nf_cleanup(&priv->notifier);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void max9286_v4l2_notifier_unregister(struct max9286_priv *priv)
+{
+	if (!priv->nsources)
+		return;
+
+	v4l2_async_nf_unregister(&priv->notifier);
+	v4l2_async_nf_cleanup(&priv->notifier);
+}
+
+static int max9286_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct max9286_priv *priv = sd_to_max9286(sd);
+	struct max9286_source *source;
+	unsigned int i;
+	bool sync = false;
+	int ret;
+
+	if (enable) {
+		const struct v4l2_mbus_framefmt *format;
+
+		/*
+		 * Get the format from the first used sink pad, as all sink
+		 * formats must be identical.
+		 */
+		format = &priv->fmt[__ffs(priv->bound_sources)];
+
+		max9286_set_video_format(priv, format);
+		max9286_set_fsync_period(priv);
+
+		/*
+		 * The frame sync between cameras is transmitted across the
+		 * reverse channel as GPIO. We must open all channels while
+		 * streaming to allow this synchronisation signal to be shared.
+		 */
+		max9286_i2c_mux_open(priv);
+
+		/* Start all cameras. */
+		for_each_source(priv, source) {
+			ret = v4l2_subdev_call(source->sd, video, s_stream, 1);
+			if (ret)
+				return ret;
+		}
+
+		ret = max9286_check_video_links(priv);
+		if (ret)
+			return ret;
+
+		/*
+		 * Wait until frame synchronization is locked.
+		 *
+		 * Manual says frame sync locking should take ~6 VTS.
+		 * From practical experience at least 8 are required. Give
+		 * 12 complete frames time (~400ms at 30 fps) to achieve frame
+		 * locking before returning error.
+		 */
+		for (i = 0; i < 40; i++) {
+			if (max9286_read(priv, 0x31) & MAX9286_FSYNC_LOCKED) {
+				sync = true;
+				break;
+			}
+			usleep_range(9000, 11000);
+		}
+
+		if (!sync) {
+			dev_err(&priv->client->dev,
+				"Failed to get frame synchronization\n");
+			return -EXDEV; /* Invalid cross-device link */
+		}
+
+		/*
+		 * Configure the CSI-2 output to line interleaved mode (W x (N
+		 * x H), as opposed to the (N x W) x H mode that outputs the
+		 * images stitched side-by-side) and enable it.
+		 */
+		max9286_write(priv, 0x15, MAX9286_CSI_IMAGE_TYP | MAX9286_VCTYPE |
+			      MAX9286_CSIOUTEN | MAX9286_EN_CCBSYB_CLK_STR |
+			      MAX9286_EN_GPI_CCBSYB);
+	} else {
+		max9286_write(priv, 0x15, MAX9286_VCTYPE |
+			      MAX9286_EN_CCBSYB_CLK_STR |
+			      MAX9286_EN_GPI_CCBSYB);
+
+		/* Stop all cameras. */
+		for_each_source(priv, source)
+			v4l2_subdev_call(source->sd, video, s_stream, 0);
+
+		max9286_i2c_mux_close(priv);
+	}
+
+	return 0;
+}
+
+static int max9286_g_frame_interval(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_frame_interval *interval)
+{
+	struct max9286_priv *priv = sd_to_max9286(sd);
+
+	if (interval->pad != MAX9286_SRC_PAD)
+		return -EINVAL;
+
+	interval->interval = priv->interval;
+
+	return 0;
+}
+
+static int max9286_s_frame_interval(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_frame_interval *interval)
+{
+	struct max9286_priv *priv = sd_to_max9286(sd);
+
+	if (interval->pad != MAX9286_SRC_PAD)
+		return -EINVAL;
+
+	priv->interval = interval->interval;
+
+	return 0;
+}
+
+static int max9286_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->pad || code->index > 0)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_UYVY8_1X16;
+
+	return 0;
+}
+
+static struct v4l2_mbus_framefmt *
+max9286_get_pad_format(struct max9286_priv *priv,
+		       struct v4l2_subdev_state *sd_state,
+		       unsigned int pad, u32 which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_format(&priv->sd, sd_state, pad);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return &priv->fmt[pad];
+	default:
+		return NULL;
+	}
+}
+
+static int max9286_set_fmt(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_state *sd_state,
+			   struct v4l2_subdev_format *format)
+{
+	struct max9286_priv *priv = sd_to_max9286(sd);
+	struct v4l2_mbus_framefmt *cfg_fmt;
+	unsigned int i;
+
+	if (format->pad == MAX9286_SRC_PAD)
+		return -EINVAL;
+
+	/* Validate the format. */
+	for (i = 0; i < ARRAY_SIZE(max9286_formats); ++i) {
+		if (max9286_formats[i].code == format->format.code)
+			break;
+	}
+
+	if (i == ARRAY_SIZE(max9286_formats))
+		format->format.code = max9286_formats[0].code;
+
+	cfg_fmt = max9286_get_pad_format(priv, sd_state, format->pad,
+					 format->which);
+	if (!cfg_fmt)
+		return -EINVAL;
+
+	mutex_lock(&priv->mutex);
+	*cfg_fmt = format->format;
+	mutex_unlock(&priv->mutex);
+
+	return 0;
+}
+
+static int max9286_get_fmt(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_state *sd_state,
+			   struct v4l2_subdev_format *format)
+{
+	struct max9286_priv *priv = sd_to_max9286(sd);
+	struct v4l2_mbus_framefmt *cfg_fmt;
+	unsigned int pad = format->pad;
+
+	/*
+	 * Multiplexed Stream Support: Support link validation by returning the
+	 * format of the first bound link. All links must have the same format,
+	 * as we do not support mixing and matching of cameras connected to the
+	 * max9286.
+	 */
+	if (pad == MAX9286_SRC_PAD)
+		pad = __ffs(priv->bound_sources);
+
+	cfg_fmt = max9286_get_pad_format(priv, sd_state, pad, format->which);
+	if (!cfg_fmt)
+		return -EINVAL;
+
+	mutex_lock(&priv->mutex);
+	format->format = *cfg_fmt;
+	mutex_unlock(&priv->mutex);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops max9286_video_ops = {
+	.s_stream	= max9286_s_stream,
+	.g_frame_interval = max9286_g_frame_interval,
+	.s_frame_interval = max9286_s_frame_interval,
+};
+
+static const struct v4l2_subdev_pad_ops max9286_pad_ops = {
+	.enum_mbus_code = max9286_enum_mbus_code,
+	.get_fmt	= max9286_get_fmt,
+	.set_fmt	= max9286_set_fmt,
+};
+
+static const struct v4l2_subdev_ops max9286_subdev_ops = {
+	.video		= &max9286_video_ops,
+	.pad		= &max9286_pad_ops,
+};
+
+static const struct v4l2_mbus_framefmt max9286_default_format = {
+	.width		= 1280,
+	.height		= 800,
+	.code		= MEDIA_BUS_FMT_UYVY8_1X16,
+	.colorspace	= V4L2_COLORSPACE_SRGB,
+	.field		= V4L2_FIELD_NONE,
+	.ycbcr_enc	= V4L2_YCBCR_ENC_DEFAULT,
+	.quantization	= V4L2_QUANTIZATION_DEFAULT,
+	.xfer_func	= V4L2_XFER_FUNC_DEFAULT,
+};
+
+static void max9286_init_format(struct v4l2_mbus_framefmt *fmt)
+{
+	*fmt = max9286_default_format;
+}
+
+static int max9286_open(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_mbus_framefmt *format;
+	unsigned int i;
+
+	for (i = 0; i < MAX9286_N_SINKS; i++) {
+		format = v4l2_subdev_get_try_format(subdev, fh->state, i);
+		max9286_init_format(format);
+	}
+
+	return 0;
+}
+
+static const struct v4l2_subdev_internal_ops max9286_subdev_internal_ops = {
+	.open = max9286_open,
+};
+
+static const struct media_entity_operations max9286_media_ops = {
+	.link_validate = v4l2_subdev_link_validate
+};
+
+static int max9286_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	switch (ctrl->id) {
+	case V4L2_CID_PIXEL_RATE:
+		return 0;
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct v4l2_ctrl_ops max9286_ctrl_ops = {
+	.s_ctrl = max9286_s_ctrl,
+};
+
+static int max9286_v4l2_register(struct max9286_priv *priv)
+{
+	struct device *dev = &priv->client->dev;
+	int ret;
+	int i;
+
+	/* Register v4l2 async notifiers for connected Camera subdevices */
+	ret = max9286_v4l2_notifier_register(priv);
+	if (ret) {
+		dev_err(dev, "Unable to register V4L2 async notifiers\n");
+		return ret;
+	}
+
+	/* Configure V4L2 for the MAX9286 itself */
+
+	for (i = 0; i < MAX9286_N_SINKS; i++)
+		max9286_init_format(&priv->fmt[i]);
+
+	v4l2_i2c_subdev_init(&priv->sd, priv->client, &max9286_subdev_ops);
+	priv->sd.internal_ops = &max9286_subdev_internal_ops;
+	priv->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	v4l2_ctrl_handler_init(&priv->ctrls, 1);
+	priv->pixelrate_ctrl = v4l2_ctrl_new_std(&priv->ctrls,
+						 &max9286_ctrl_ops,
+						 V4L2_CID_PIXEL_RATE,
+						 1, INT_MAX, 1, 50000000);
+
+	priv->sd.ctrl_handler = &priv->ctrls;
+	ret = priv->ctrls.error;
+	if (ret)
+		goto err_async;
+
+	priv->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
+	priv->sd.entity.ops = &max9286_media_ops;
+
+	priv->pads[MAX9286_SRC_PAD].flags = MEDIA_PAD_FL_SOURCE;
+	for (i = 0; i < MAX9286_SRC_PAD; i++)
+		priv->pads[i].flags = MEDIA_PAD_FL_SINK;
+	ret = media_entity_pads_init(&priv->sd.entity, MAX9286_N_PADS,
+				     priv->pads);
+	if (ret)
+		goto err_async;
+
+	ret = v4l2_async_register_subdev(&priv->sd);
+	if (ret < 0) {
+		dev_err(dev, "Unable to register subdevice\n");
+		goto err_async;
+	}
+
+	return 0;
+
+err_async:
+	v4l2_ctrl_handler_free(&priv->ctrls);
+	max9286_v4l2_notifier_unregister(priv);
+
+	return ret;
+}
+
+static void max9286_v4l2_unregister(struct max9286_priv *priv)
+{
+	v4l2_ctrl_handler_free(&priv->ctrls);
+	v4l2_async_unregister_subdev(&priv->sd);
+	max9286_v4l2_notifier_unregister(priv);
+}
+
+/* -----------------------------------------------------------------------------
+ * Probe/Remove
+ */
+
+static int max9286_setup(struct max9286_priv *priv)
+{
+	/*
+	 * Link ordering values for all enabled links combinations. Orders must
+	 * be assigned sequentially from 0 to the number of enabled links
+	 * without leaving any hole for disabled links. We thus assign orders to
+	 * enabled links first, and use the remaining order values for disabled
+	 * links are all links must have a different order value;
+	 */
+	static const u8 link_order[] = {
+		(3 << 6) | (2 << 4) | (1 << 2) | (0 << 0), /* xxxx */
+		(3 << 6) | (2 << 4) | (1 << 2) | (0 << 0), /* xxx0 */
+		(3 << 6) | (2 << 4) | (0 << 2) | (1 << 0), /* xx0x */
+		(3 << 6) | (2 << 4) | (1 << 2) | (0 << 0), /* xx10 */
+		(3 << 6) | (0 << 4) | (2 << 2) | (1 << 0), /* x0xx */
+		(3 << 6) | (1 << 4) | (2 << 2) | (0 << 0), /* x1x0 */
+		(3 << 6) | (1 << 4) | (0 << 2) | (2 << 0), /* x10x */
+		(3 << 6) | (1 << 4) | (1 << 2) | (0 << 0), /* x210 */
+		(0 << 6) | (3 << 4) | (2 << 2) | (1 << 0), /* 0xxx */
+		(1 << 6) | (3 << 4) | (2 << 2) | (0 << 0), /* 1xx0 */
+		(1 << 6) | (3 << 4) | (0 << 2) | (2 << 0), /* 1x0x */
+		(2 << 6) | (3 << 4) | (1 << 2) | (0 << 0), /* 2x10 */
+		(1 << 6) | (0 << 4) | (3 << 2) | (2 << 0), /* 10xx */
+		(2 << 6) | (1 << 4) | (3 << 2) | (0 << 0), /* 21x0 */
+		(2 << 6) | (1 << 4) | (0 << 2) | (3 << 0), /* 210x */
+		(3 << 6) | (2 << 4) | (1 << 2) | (0 << 0), /* 3210 */
+	};
+	int cfg;
+
+	/*
+	 * Set the I2C bus speed.
+	 *
+	 * Enable I2C Local Acknowledge during the probe sequences of the camera
+	 * only. This should be disabled after the mux is initialised.
+	 */
+	max9286_configure_i2c(priv, true);
+	max9286_reverse_channel_setup(priv, priv->init_rev_chan_mv);
+
+	/*
+	 * Enable GMSL links, mask unused ones and autodetect link
+	 * used as CSI clock source.
+	 */
+	max9286_write(priv, 0x00, MAX9286_MSTLINKSEL_AUTO | priv->route_mask);
+	max9286_write(priv, 0x0b, link_order[priv->route_mask]);
+	max9286_write(priv, 0x69, (0xf & ~priv->route_mask));
+
+	max9286_set_video_format(priv, &max9286_default_format);
+	max9286_set_fsync_period(priv);
+
+	cfg = max9286_read(priv, 0x1c);
+	if (cfg < 0)
+		return cfg;
+
+	dev_dbg(&priv->client->dev, "power-up config: %s immunity, %u-bit bus\n",
+		cfg & MAX9286_HIGHIMM(0) ? "high" : "legacy",
+		cfg & MAX9286_BWS ? 32 : cfg & MAX9286_HIBW ? 27 : 24);
+
+	if (priv->bus_width) {
+		cfg &= ~(MAX9286_HIBW | MAX9286_BWS);
+
+		if (priv->bus_width == 27)
+			cfg |= MAX9286_HIBW;
+		else if (priv->bus_width == 32)
+			cfg |= MAX9286_BWS;
+
+		max9286_write(priv, 0x1c, cfg);
+	}
+
+	/*
+	 * The overlap window seems to provide additional validation by tracking
+	 * the delay between vsync and frame sync, generating an error if the
+	 * delay is bigger than the programmed window, though it's not yet clear
+	 * what value should be set.
+	 *
+	 * As it's an optional value and can be disabled, we do so by setting
+	 * a 0 overlap value.
+	 */
+	max9286_write(priv, 0x63, 0);
+	max9286_write(priv, 0x64, 0);
+
+	/*
+	 * Wait for 2ms to allow the link to resynchronize after the
+	 * configuration change.
+	 */
+	usleep_range(2000, 5000);
+
+	return 0;
+}
+
+static int max9286_gpio_set(struct max9286_priv *priv, unsigned int offset,
+			    int value)
+{
+	if (value)
+		priv->gpio_state |= BIT(offset);
+	else
+		priv->gpio_state &= ~BIT(offset);
+
+	return max9286_write(priv, 0x0f,
+			     MAX9286_0X0F_RESERVED | priv->gpio_state);
+}
+
+static void max9286_gpiochip_set(struct gpio_chip *chip,
+				 unsigned int offset, int value)
+{
+	struct max9286_priv *priv = gpiochip_get_data(chip);
+
+	max9286_gpio_set(priv, offset, value);
+}
+
+static int max9286_gpiochip_get(struct gpio_chip *chip, unsigned int offset)
+{
+	struct max9286_priv *priv = gpiochip_get_data(chip);
+
+	return priv->gpio_state & BIT(offset);
+}
+
+static int max9286_register_gpio(struct max9286_priv *priv)
+{
+	struct device *dev = &priv->client->dev;
+	struct gpio_chip *gpio = &priv->gpio;
+	int ret;
+
+	/* Configure the GPIO */
+	gpio->label = dev_name(dev);
+	gpio->parent = dev;
+	gpio->owner = THIS_MODULE;
+	gpio->ngpio = 2;
+	gpio->base = -1;
+	gpio->set = max9286_gpiochip_set;
+	gpio->get = max9286_gpiochip_get;
+	gpio->can_sleep = true;
+
+	ret = devm_gpiochip_add_data(dev, gpio, priv);
+	if (ret)
+		dev_err(dev, "Unable to create gpio_chip\n");
+
+	return ret;
+}
+
+static int max9286_parse_gpios(struct max9286_priv *priv)
+{
+	struct device *dev = &priv->client->dev;
+	int ret;
+
+	/*
+	 * Parse the "gpio-poc" vendor property. If the property is not
+	 * specified the camera power is controlled by a regulator.
+	 */
+	ret = of_property_read_u32_array(dev->of_node, "maxim,gpio-poc",
+					 priv->gpio_poc, 2);
+	if (ret == -EINVAL) {
+		/*
+		 * If gpio lines are not used for the camera power, register
+		 * a gpio controller for consumers.
+		 */
+		return max9286_register_gpio(priv);
+	}
+
+	/* If the property is specified make sure it is well formed. */
+	if (ret || priv->gpio_poc[0] > 1 ||
+	    (priv->gpio_poc[1] != GPIO_ACTIVE_HIGH &&
+	     priv->gpio_poc[1] != GPIO_ACTIVE_LOW)) {
+		dev_err(dev, "Invalid 'gpio-poc' property\n");
+		return -EINVAL;
+	}
+
+	priv->use_gpio_poc = true;
+	return 0;
+}
+
+static int max9286_poc_power_on(struct max9286_priv *priv)
+{
+	struct max9286_source *source;
+	unsigned int enabled = 0;
+	int ret;
+
+	/* Enable the global regulator if available. */
+	if (priv->regulator)
+		return regulator_enable(priv->regulator);
+
+	if (priv->use_gpio_poc)
+		return max9286_gpio_set(priv, priv->gpio_poc[0],
+					!priv->gpio_poc[1]);
+
+	/* Otherwise use the per-port regulators. */
+	for_each_source(priv, source) {
+		ret = regulator_enable(source->regulator);
+		if (ret < 0)
+			goto error;
+
+		enabled |= BIT(to_index(priv, source));
+	}
+
+	return 0;
+
+error:
+	for_each_source(priv, source) {
+		if (enabled & BIT(to_index(priv, source)))
+			regulator_disable(source->regulator);
+	}
+
+	return ret;
+}
+
+static int max9286_poc_power_off(struct max9286_priv *priv)
+{
+	struct max9286_source *source;
+	int ret = 0;
+
+	if (priv->regulator)
+		return regulator_disable(priv->regulator);
+
+	if (priv->use_gpio_poc)
+		return max9286_gpio_set(priv, priv->gpio_poc[0],
+					priv->gpio_poc[1]);
+
+	for_each_source(priv, source) {
+		int err;
+
+		err = regulator_disable(source->regulator);
+		if (!ret)
+			ret = err;
+	}
+
+	return ret;
+}
+
+static int max9286_poc_enable(struct max9286_priv *priv, bool enable)
+{
+	int ret;
+
+	if (enable)
+		ret = max9286_poc_power_on(priv);
+	else
+		ret = max9286_poc_power_off(priv);
+
+	if (ret < 0)
+		dev_err(&priv->client->dev, "Unable to turn power %s\n",
+			enable ? "on" : "off");
+
+	return ret;
+}
+
+static int max9286_init(struct max9286_priv *priv)
+{
+	struct i2c_client *client = priv->client;
+	int ret;
+
+	ret = max9286_poc_enable(priv, true);
+	if (ret)
+		return ret;
+
+	ret = max9286_setup(priv);
+	if (ret) {
+		dev_err(&client->dev, "Unable to setup max9286\n");
+		goto err_poc_disable;
+	}
+
+	/*
+	 * Register all V4L2 interactions for the MAX9286 and notifiers for
+	 * any subdevices connected.
+	 */
+	ret = max9286_v4l2_register(priv);
+	if (ret) {
+		dev_err(&client->dev, "Failed to register with V4L2\n");
+		goto err_poc_disable;
+	}
+
+	ret = max9286_i2c_mux_init(priv);
+	if (ret) {
+		dev_err(&client->dev, "Unable to initialize I2C multiplexer\n");
+		goto err_v4l2_register;
+	}
+
+	/* Leave the mux channels disabled until they are selected. */
+	max9286_i2c_mux_close(priv);
+
+	return 0;
+
+err_v4l2_register:
+	max9286_v4l2_unregister(priv);
+err_poc_disable:
+	max9286_poc_enable(priv, false);
+
+	return ret;
+}
+
+static void max9286_cleanup_dt(struct max9286_priv *priv)
+{
+	struct max9286_source *source;
+
+	for_each_source(priv, source) {
+		fwnode_handle_put(source->fwnode);
+		source->fwnode = NULL;
+	}
+}
+
+static int max9286_parse_dt(struct max9286_priv *priv)
+{
+	struct device *dev = &priv->client->dev;
+	struct device_node *i2c_mux;
+	struct device_node *node = NULL;
+	unsigned int i2c_mux_mask = 0;
+	u32 reverse_channel_microvolt;
+	u32 i2c_clk_freq = 105000;
+	unsigned int i;
+
+	/* Balance the of_node_put() performed by of_find_node_by_name(). */
+	of_node_get(dev->of_node);
+	i2c_mux = of_find_node_by_name(dev->of_node, "i2c-mux");
+	if (!i2c_mux) {
+		dev_err(dev, "Failed to find i2c-mux node\n");
+		return -EINVAL;
+	}
+
+	/* Identify which i2c-mux channels are enabled */
+	for_each_child_of_node(i2c_mux, node) {
+		u32 id = 0;
+
+		of_property_read_u32(node, "reg", &id);
+		if (id >= MAX9286_NUM_GMSL)
+			continue;
+
+		if (!of_device_is_available(node)) {
+			dev_dbg(dev, "Skipping disabled I2C bus port %u\n", id);
+			continue;
+		}
+
+		i2c_mux_mask |= BIT(id);
+	}
+	of_node_put(i2c_mux);
+
+	/* Parse the endpoints */
+	for_each_endpoint_of_node(dev->of_node, node) {
+		struct max9286_source *source;
+		struct of_endpoint ep;
+
+		of_graph_parse_endpoint(node, &ep);
+		dev_dbg(dev, "Endpoint %pOF on port %d",
+			ep.local_node, ep.port);
+
+		if (ep.port > MAX9286_NUM_GMSL) {
+			dev_err(dev, "Invalid endpoint %s on port %d",
+				of_node_full_name(ep.local_node), ep.port);
+			continue;
+		}
+
+		/* For the source endpoint just parse the bus configuration. */
+		if (ep.port == MAX9286_SRC_PAD) {
+			struct v4l2_fwnode_endpoint vep = {
+				.bus_type = V4L2_MBUS_CSI2_DPHY
+			};
+			int ret;
+
+			ret = v4l2_fwnode_endpoint_parse(
+					of_fwnode_handle(node), &vep);
+			if (ret) {
+				of_node_put(node);
+				return ret;
+			}
+
+			priv->csi2_data_lanes =
+				vep.bus.mipi_csi2.num_data_lanes;
+
+			continue;
+		}
+
+		/* Skip if the corresponding GMSL link is unavailable. */
+		if (!(i2c_mux_mask & BIT(ep.port)))
+			continue;
+
+		if (priv->sources[ep.port].fwnode) {
+			dev_err(dev,
+				"Multiple port endpoints are not supported: %d",
+				ep.port);
+
+			continue;
+		}
+
+		source = &priv->sources[ep.port];
+		source->fwnode = fwnode_graph_get_remote_endpoint(
+						of_fwnode_handle(node));
+		if (!source->fwnode) {
+			dev_err(dev,
+				"Endpoint %pOF has no remote endpoint connection\n",
+				ep.local_node);
+
+			continue;
+		}
+
+		priv->source_mask |= BIT(ep.port);
+		priv->nsources++;
+	}
+
+	of_property_read_u32(dev->of_node, "maxim,bus-width", &priv->bus_width);
+	switch (priv->bus_width) {
+	case 0:
+		/*
+		 * The property isn't specified in the device tree, the driver
+		 * will keep the default value selected by the BWS pin.
+		 */
+	case 24:
+	case 27:
+	case 32:
+		break;
+	default:
+		dev_err(dev, "Invalid %s value %u\n", "maxim,bus-width",
+			priv->bus_width);
+		return -EINVAL;
+	}
+
+	of_property_read_u32(dev->of_node, "maxim,i2c-remote-bus-hz",
+			     &i2c_clk_freq);
+	for (i = 0; i < ARRAY_SIZE(max9286_i2c_speeds); ++i) {
+		const struct max9286_i2c_speed *speed = &max9286_i2c_speeds[i];
+
+		if (speed->rate == i2c_clk_freq) {
+			priv->i2c_mstbt = speed->mstbt;
+			break;
+		}
+	}
+
+	if (i == ARRAY_SIZE(max9286_i2c_speeds)) {
+		dev_err(dev, "Invalid %s value %u\n", "maxim,i2c-remote-bus-hz",
+			i2c_clk_freq);
+		return -EINVAL;
+	}
+
+	/*
+	 * Parse the initial value of the reverse channel amplitude from
+	 * the firmware interface and convert it to millivolts.
+	 *
+	 * Default it to 170mV for backward compatibility with DTBs that do not
+	 * provide the property.
+	 */
+	if (of_property_read_u32(dev->of_node,
+				 "maxim,reverse-channel-microvolt",
+				 &reverse_channel_microvolt))
+		priv->init_rev_chan_mv = 170;
+	else
+		priv->init_rev_chan_mv = reverse_channel_microvolt / 1000U;
+
+	priv->route_mask = priv->source_mask;
+
+	return 0;
+}
+
+static int max9286_get_poc_supplies(struct max9286_priv *priv)
+{
+	struct device *dev = &priv->client->dev;
+	struct max9286_source *source;
+	int ret;
+
+	/* Start by getting the global regulator. */
+	priv->regulator = devm_regulator_get_optional(dev, "poc");
+	if (!IS_ERR(priv->regulator))
+		return 0;
+
+	if (PTR_ERR(priv->regulator) != -ENODEV)
+		return dev_err_probe(dev, PTR_ERR(priv->regulator),
+				     "Unable to get PoC regulator\n");
+
+	/* If there's no global regulator, get per-port regulators. */
+	dev_dbg(dev,
+		"No global PoC regulator, looking for per-port regulators\n");
+	priv->regulator = NULL;
+
+	for_each_source(priv, source) {
+		unsigned int index = to_index(priv, source);
+		char name[10];
+
+		snprintf(name, sizeof(name), "port%u-poc", index);
+		source->regulator = devm_regulator_get(dev, name);
+		if (IS_ERR(source->regulator)) {
+			ret = PTR_ERR(source->regulator);
+			dev_err_probe(dev, ret,
+				      "Unable to get port %u PoC regulator\n",
+				      index);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int max9286_probe(struct i2c_client *client)
+{
+	struct max9286_priv *priv;
+	int ret;
+
+	priv = devm_kzalloc(&client->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	mutex_init(&priv->mutex);
+
+	priv->client = client;
+
+	/* GPIO values default to high */
+	priv->gpio_state = BIT(0) | BIT(1);
+
+	ret = max9286_parse_dt(priv);
+	if (ret)
+		goto err_cleanup_dt;
+
+	priv->gpiod_pwdn = devm_gpiod_get_optional(&client->dev, "enable",
+						   GPIOD_OUT_HIGH);
+	if (IS_ERR(priv->gpiod_pwdn)) {
+		ret = PTR_ERR(priv->gpiod_pwdn);
+		goto err_cleanup_dt;
+	}
+
+	gpiod_set_consumer_name(priv->gpiod_pwdn, "max9286-pwdn");
+	gpiod_set_value_cansleep(priv->gpiod_pwdn, 1);
+
+	/* Wait at least 4ms before the I2C lines latch to the address */
+	if (priv->gpiod_pwdn)
+		usleep_range(4000, 5000);
+
+	/*
+	 * The MAX9286 starts by default with all ports enabled, we disable all
+	 * ports early to ensure that all channels are disabled if we error out
+	 * and keep the bus consistent.
+	 */
+	max9286_i2c_mux_close(priv);
+
+	/*
+	 * The MAX9286 initialises with auto-acknowledge enabled by default.
+	 * This can be invasive to other transactions on the same bus, so
+	 * disable it early. It will be enabled only as and when needed.
+	 */
+	max9286_configure_i2c(priv, false);
+
+	ret = max9286_parse_gpios(priv);
+	if (ret)
+		goto err_powerdown;
+
+	if (!priv->use_gpio_poc) {
+		ret = max9286_get_poc_supplies(priv);
+		if (ret)
+			goto err_cleanup_dt;
+	}
+
+	ret = max9286_init(priv);
+	if (ret < 0)
+		goto err_cleanup_dt;
+
+	return 0;
+
+err_powerdown:
+	gpiod_set_value_cansleep(priv->gpiod_pwdn, 0);
+err_cleanup_dt:
+	max9286_cleanup_dt(priv);
+
+	return ret;
+}
+
+static void max9286_remove(struct i2c_client *client)
+{
+	struct max9286_priv *priv = sd_to_max9286(i2c_get_clientdata(client));
+
+	i2c_mux_del_adapters(priv->mux);
+
+	max9286_v4l2_unregister(priv);
+
+	max9286_poc_enable(priv, false);
+
+	gpiod_set_value_cansleep(priv->gpiod_pwdn, 0);
+
+	max9286_cleanup_dt(priv);
+}
+
+static const struct of_device_id max9286_dt_ids[] = {
+	{ .compatible = "maxim,max9286" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, max9286_dt_ids);
+
+static struct i2c_driver max9286_i2c_driver = {
+	.driver	= {
+		.name		= "max9286",
+		.of_match_table	= max9286_dt_ids,
+	},
+	.probe		= max9286_probe,
+	.remove		= max9286_remove,
+};
+
+module_i2c_driver(max9286_i2c_driver);
+
+MODULE_DESCRIPTION("Maxim MAX9286 GMSL Deserializer Driver");
+MODULE_AUTHOR("Jacopo Mondi, Kieran Bingham, Laurent Pinchart, Niklas Söderlund, Vladimir Barinov");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/ml86v7667.c b/drivers/media/i2c/ml86v7667.c
new file mode 100644
index 000000000..5b72d4434
--- /dev/null
+++ b/drivers/media/i2c/ml86v7667.c
@@ -0,0 +1,445 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * OKI Semiconductor ML86V7667 video decoder driver
+ *
+ * Author: Vladimir Barinov <source@cogentembedded.com>
+ * Copyright (C) 2013 Cogent Embedded, Inc.
+ * Copyright (C) 2013 Renesas Solutions Corp.
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-ctrls.h>
+
+#define DRV_NAME "ml86v7667"
+
+/* Subaddresses */
+#define MRA_REG			0x00 /* Mode Register A */
+#define MRC_REG			0x02 /* Mode Register C */
+#define LUMC_REG		0x0C /* Luminance Control */
+#define CLC_REG			0x10 /* Contrast level control */
+#define SSEPL_REG		0x11 /* Sync separation level */
+#define CHRCA_REG		0x12 /* Chrominance Control A */
+#define ACCC_REG		0x14 /* ACC Loop filter & Chrominance control */
+#define ACCRC_REG		0x15 /* ACC Reference level control */
+#define HUE_REG			0x16 /* Hue control */
+#define ADC2_REG		0x1F /* ADC Register 2 */
+#define PLLR1_REG		0x20 /* PLL Register 1 */
+#define STATUS_REG		0x2C /* STATUS Register */
+
+/* Mode Register A register bits */
+#define MRA_OUTPUT_MODE_MASK	(3 << 6)
+#define MRA_ITUR_BT601		(1 << 6)
+#define MRA_ITUR_BT656		(0 << 6)
+#define MRA_INPUT_MODE_MASK	(7 << 3)
+#define MRA_PAL_BT601		(4 << 3)
+#define MRA_NTSC_BT601		(0 << 3)
+#define MRA_REGISTER_MODE	(1 << 0)
+
+/* Mode Register C register bits */
+#define MRC_AUTOSELECT		(1 << 7)
+
+/* Luminance Control register bits */
+#define LUMC_ONOFF_SHIFT	7
+#define LUMC_ONOFF_MASK		(1 << 7)
+
+/* Contrast level control register bits */
+#define CLC_CONTRAST_ONOFF	(1 << 7)
+#define CLC_CONTRAST_MASK	0x0F
+
+/* Sync separation level register bits */
+#define SSEPL_LUMINANCE_ONOFF	(1 << 7)
+#define SSEPL_LUMINANCE_MASK	0x7F
+
+/* Chrominance Control A register bits */
+#define CHRCA_MODE_SHIFT	6
+#define CHRCA_MODE_MASK		(1 << 6)
+
+/* ACC Loop filter & Chrominance control register bits */
+#define ACCC_CHROMA_CR_SHIFT	3
+#define ACCC_CHROMA_CR_MASK	(7 << 3)
+#define ACCC_CHROMA_CB_SHIFT	0
+#define ACCC_CHROMA_CB_MASK	(7 << 0)
+
+/* ACC Reference level control register bits */
+#define ACCRC_CHROMA_MASK	0xfc
+#define ACCRC_CHROMA_SHIFT	2
+
+/* ADC Register 2 register bits */
+#define ADC2_CLAMP_VOLTAGE_MASK	(7 << 1)
+#define ADC2_CLAMP_VOLTAGE(n)	((n & 7) << 1)
+
+/* PLL Register 1 register bits */
+#define PLLR1_FIXED_CLOCK	(1 << 7)
+
+/* STATUS Register register bits */
+#define STATUS_HLOCK_DETECT	(1 << 3)
+#define STATUS_NTSCPAL		(1 << 2)
+
+struct ml86v7667_priv {
+	struct v4l2_subdev		sd;
+	struct v4l2_ctrl_handler	hdl;
+	v4l2_std_id			std;
+};
+
+static inline struct ml86v7667_priv *to_ml86v7667(struct v4l2_subdev *subdev)
+{
+	return container_of(subdev, struct ml86v7667_priv, sd);
+}
+
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct ml86v7667_priv, hdl)->sd;
+}
+
+static int ml86v7667_mask_set(struct i2c_client *client, const u8 reg,
+			      const u8 mask, const u8 data)
+{
+	int val = i2c_smbus_read_byte_data(client, reg);
+	if (val < 0)
+		return val;
+
+	val = (val & ~mask) | (data & mask);
+	return i2c_smbus_write_byte_data(client, reg, val);
+}
+
+static int ml86v7667_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = -EINVAL;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		ret = ml86v7667_mask_set(client, SSEPL_REG,
+					 SSEPL_LUMINANCE_MASK, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		ret = ml86v7667_mask_set(client, CLC_REG,
+					 CLC_CONTRAST_MASK, ctrl->val);
+		break;
+	case V4L2_CID_CHROMA_GAIN:
+		ret = ml86v7667_mask_set(client, ACCRC_REG, ACCRC_CHROMA_MASK,
+					 ctrl->val << ACCRC_CHROMA_SHIFT);
+		break;
+	case V4L2_CID_HUE:
+		ret = ml86v7667_mask_set(client, HUE_REG, ~0, ctrl->val);
+		break;
+	case V4L2_CID_RED_BALANCE:
+		ret = ml86v7667_mask_set(client, ACCC_REG,
+					 ACCC_CHROMA_CR_MASK,
+					 ctrl->val << ACCC_CHROMA_CR_SHIFT);
+		break;
+	case V4L2_CID_BLUE_BALANCE:
+		ret = ml86v7667_mask_set(client, ACCC_REG,
+					 ACCC_CHROMA_CB_MASK,
+					 ctrl->val << ACCC_CHROMA_CB_SHIFT);
+		break;
+	case V4L2_CID_SHARPNESS:
+		ret = ml86v7667_mask_set(client, LUMC_REG,
+					 LUMC_ONOFF_MASK,
+					 ctrl->val << LUMC_ONOFF_SHIFT);
+		break;
+	case V4L2_CID_COLOR_KILLER:
+		ret = ml86v7667_mask_set(client, CHRCA_REG,
+					 CHRCA_MODE_MASK,
+					 ctrl->val << CHRCA_MODE_SHIFT);
+		break;
+	}
+
+	return ret;
+}
+
+static int ml86v7667_querystd(struct v4l2_subdev *sd, v4l2_std_id *std)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int status;
+
+	status = i2c_smbus_read_byte_data(client, STATUS_REG);
+	if (status < 0)
+		return status;
+
+	if (status & STATUS_HLOCK_DETECT)
+		*std &= status & STATUS_NTSCPAL ? V4L2_STD_625_50 : V4L2_STD_525_60;
+	else
+		*std = V4L2_STD_UNKNOWN;
+
+	return 0;
+}
+
+static int ml86v7667_g_input_status(struct v4l2_subdev *sd, u32 *status)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int status_reg;
+
+	status_reg = i2c_smbus_read_byte_data(client, STATUS_REG);
+	if (status_reg < 0)
+		return status_reg;
+
+	*status = status_reg & STATUS_HLOCK_DETECT ? 0 : V4L2_IN_ST_NO_SIGNAL;
+
+	return 0;
+}
+
+static int ml86v7667_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->pad || code->index > 0)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_YUYV8_2X8;
+
+	return 0;
+}
+
+static int ml86v7667_fill_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_format *format)
+{
+	struct ml86v7667_priv *priv = to_ml86v7667(sd);
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+
+	if (format->pad)
+		return -EINVAL;
+
+	fmt->code = MEDIA_BUS_FMT_YUYV8_2X8;
+	fmt->colorspace = V4L2_COLORSPACE_SMPTE170M;
+	/* The top field is always transferred first by the chip */
+	fmt->field = V4L2_FIELD_INTERLACED_TB;
+	fmt->width = 720;
+	fmt->height = priv->std & V4L2_STD_525_60 ? 480 : 576;
+
+	return 0;
+}
+
+static int ml86v7667_get_mbus_config(struct v4l2_subdev *sd,
+				     unsigned int pad,
+				     struct v4l2_mbus_config *cfg)
+{
+	cfg->type = V4L2_MBUS_BT656;
+	cfg->bus.parallel.flags = V4L2_MBUS_MASTER |
+				  V4L2_MBUS_PCLK_SAMPLE_RISING |
+				  V4L2_MBUS_DATA_ACTIVE_HIGH;
+
+	return 0;
+}
+
+static int ml86v7667_g_std(struct v4l2_subdev *sd, v4l2_std_id *std)
+{
+	struct ml86v7667_priv *priv = to_ml86v7667(sd);
+
+	*std = priv->std;
+
+	return 0;
+}
+
+static int ml86v7667_s_std(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct ml86v7667_priv *priv = to_ml86v7667(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(&priv->sd);
+	int ret;
+	u8 mode;
+
+	/* PAL/NTSC ITU-R BT.601 input mode */
+	mode = std & V4L2_STD_525_60 ? MRA_NTSC_BT601 : MRA_PAL_BT601;
+	ret = ml86v7667_mask_set(client, MRA_REG, MRA_INPUT_MODE_MASK, mode);
+	if (ret < 0)
+		return ret;
+
+	priv->std = std;
+
+	return 0;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int ml86v7667_g_register(struct v4l2_subdev *sd,
+				struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(client, (u8)reg->reg);
+	if (ret < 0)
+		return ret;
+
+	reg->val = ret;
+	reg->size = sizeof(u8);
+
+	return 0;
+}
+
+static int ml86v7667_s_register(struct v4l2_subdev *sd,
+				const struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return i2c_smbus_write_byte_data(client, (u8)reg->reg, (u8)reg->val);
+}
+#endif
+
+static const struct v4l2_ctrl_ops ml86v7667_ctrl_ops = {
+	.s_ctrl = ml86v7667_s_ctrl,
+};
+
+static const struct v4l2_subdev_video_ops ml86v7667_subdev_video_ops = {
+	.g_std = ml86v7667_g_std,
+	.s_std = ml86v7667_s_std,
+	.querystd = ml86v7667_querystd,
+	.g_input_status = ml86v7667_g_input_status,
+};
+
+static const struct v4l2_subdev_pad_ops ml86v7667_subdev_pad_ops = {
+	.enum_mbus_code = ml86v7667_enum_mbus_code,
+	.get_fmt = ml86v7667_fill_fmt,
+	.set_fmt = ml86v7667_fill_fmt,
+	.get_mbus_config = ml86v7667_get_mbus_config,
+};
+
+static const struct v4l2_subdev_core_ops ml86v7667_subdev_core_ops = {
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = ml86v7667_g_register,
+	.s_register = ml86v7667_s_register,
+#endif
+};
+
+static const struct v4l2_subdev_ops ml86v7667_subdev_ops = {
+	.core = &ml86v7667_subdev_core_ops,
+	.video = &ml86v7667_subdev_video_ops,
+	.pad = &ml86v7667_subdev_pad_ops,
+};
+
+static int ml86v7667_init(struct ml86v7667_priv *priv)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&priv->sd);
+	int val;
+	int ret;
+
+	/* BT.656-4 output mode, register mode */
+	ret = ml86v7667_mask_set(client, MRA_REG,
+				 MRA_OUTPUT_MODE_MASK | MRA_REGISTER_MODE,
+				 MRA_ITUR_BT656 | MRA_REGISTER_MODE);
+
+	/* PLL circuit fixed clock, 32MHz */
+	ret |= ml86v7667_mask_set(client, PLLR1_REG, PLLR1_FIXED_CLOCK,
+				  PLLR1_FIXED_CLOCK);
+
+	/* ADC2 clamping voltage maximum  */
+	ret |= ml86v7667_mask_set(client, ADC2_REG, ADC2_CLAMP_VOLTAGE_MASK,
+				  ADC2_CLAMP_VOLTAGE(7));
+
+	/* enable luminance function */
+	ret |= ml86v7667_mask_set(client, SSEPL_REG, SSEPL_LUMINANCE_ONOFF,
+				  SSEPL_LUMINANCE_ONOFF);
+
+	/* enable contrast function */
+	ret |= ml86v7667_mask_set(client, CLC_REG, CLC_CONTRAST_ONOFF, 0);
+
+	/*
+	 * PAL/NTSC autodetection is enabled after reset,
+	 * set the autodetected std in manual std mode and
+	 * disable autodetection
+	 */
+	val = i2c_smbus_read_byte_data(client, STATUS_REG);
+	if (val < 0)
+		return val;
+
+	priv->std = val & STATUS_NTSCPAL ? V4L2_STD_625_50 : V4L2_STD_525_60;
+	ret |= ml86v7667_mask_set(client, MRC_REG, MRC_AUTOSELECT, 0);
+
+	val = priv->std & V4L2_STD_525_60 ? MRA_NTSC_BT601 : MRA_PAL_BT601;
+	ret |= ml86v7667_mask_set(client, MRA_REG, MRA_INPUT_MODE_MASK, val);
+
+	return ret;
+}
+
+static int ml86v7667_probe(struct i2c_client *client)
+{
+	struct ml86v7667_priv *priv;
+	int ret;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+
+	priv = devm_kzalloc(&client->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	v4l2_i2c_subdev_init(&priv->sd, client, &ml86v7667_subdev_ops);
+
+	v4l2_ctrl_handler_init(&priv->hdl, 8);
+	v4l2_ctrl_new_std(&priv->hdl, &ml86v7667_ctrl_ops,
+			  V4L2_CID_BRIGHTNESS, -64, 63, 1, 0);
+	v4l2_ctrl_new_std(&priv->hdl, &ml86v7667_ctrl_ops,
+			  V4L2_CID_CONTRAST, -8, 7, 1, 0);
+	v4l2_ctrl_new_std(&priv->hdl, &ml86v7667_ctrl_ops,
+			  V4L2_CID_CHROMA_GAIN, -32, 31, 1, 0);
+	v4l2_ctrl_new_std(&priv->hdl, &ml86v7667_ctrl_ops,
+			  V4L2_CID_HUE, -128, 127, 1, 0);
+	v4l2_ctrl_new_std(&priv->hdl, &ml86v7667_ctrl_ops,
+			  V4L2_CID_RED_BALANCE, -4, 3, 1, 0);
+	v4l2_ctrl_new_std(&priv->hdl, &ml86v7667_ctrl_ops,
+			  V4L2_CID_BLUE_BALANCE, -4, 3, 1, 0);
+	v4l2_ctrl_new_std(&priv->hdl, &ml86v7667_ctrl_ops,
+			  V4L2_CID_SHARPNESS, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&priv->hdl, &ml86v7667_ctrl_ops,
+			  V4L2_CID_COLOR_KILLER, 0, 1, 1, 0);
+	priv->sd.ctrl_handler = &priv->hdl;
+
+	ret = priv->hdl.error;
+	if (ret)
+		goto cleanup;
+
+	v4l2_ctrl_handler_setup(&priv->hdl);
+
+	ret = ml86v7667_init(priv);
+	if (ret)
+		goto cleanup;
+
+	v4l_info(client, "chip found @ 0x%02x (%s)\n",
+		 client->addr, client->adapter->name);
+	return 0;
+
+cleanup:
+	v4l2_ctrl_handler_free(&priv->hdl);
+	v4l2_device_unregister_subdev(&priv->sd);
+	v4l_err(client, "failed to probe @ 0x%02x (%s)\n",
+		client->addr, client->adapter->name);
+	return ret;
+}
+
+static void ml86v7667_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ml86v7667_priv *priv = to_ml86v7667(sd);
+
+	v4l2_ctrl_handler_free(&priv->hdl);
+	v4l2_device_unregister_subdev(&priv->sd);
+}
+
+static const struct i2c_device_id ml86v7667_id[] = {
+	{DRV_NAME, 0},
+	{},
+};
+MODULE_DEVICE_TABLE(i2c, ml86v7667_id);
+
+static struct i2c_driver ml86v7667_i2c_driver = {
+	.driver = {
+		.name	= DRV_NAME,
+	},
+	.probe		= ml86v7667_probe,
+	.remove		= ml86v7667_remove,
+	.id_table	= ml86v7667_id,
+};
+
+module_i2c_driver(ml86v7667_i2c_driver);
+
+MODULE_DESCRIPTION("OKI Semiconductor ML86V7667 video decoder driver");
+MODULE_AUTHOR("Vladimir Barinov");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/msp3400-driver.c b/drivers/media/i2c/msp3400-driver.c
new file mode 100644
index 000000000..bec768014
--- /dev/null
+++ b/drivers/media/i2c/msp3400-driver.c
@@ -0,0 +1,900 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Programming the mspx4xx sound processor family
+ *
+ * (c) 1997-2001 Gerd Knorr <kraxel@bytesex.org>
+ *
+ * what works and what doesn't:
+ *
+ *  AM-Mono
+ *      Support for Hauppauge cards added (decoding handled by tuner) added by
+ *      Frederic Crozat <fcrozat@mail.dotcom.fr>
+ *
+ *  FM-Mono
+ *      should work. The stereo modes are backward compatible to FM-mono,
+ *      therefore FM-Mono should be always available.
+ *
+ *  FM-Stereo (B/G, used in germany)
+ *      should work, with autodetect
+ *
+ *  FM-Stereo (satellite)
+ *      should work, no autodetect (i.e. default is mono, but you can
+ *      switch to stereo -- untested)
+ *
+ *  NICAM (B/G, L , used in UK, Scandinavia, Spain and France)
+ *      should work, with autodetect. Support for NICAM was added by
+ *      Pekka Pietikainen <pp@netppl.fi>
+ *
+ * TODO:
+ *   - better SAT support
+ *
+ * 980623  Thomas Sailer (sailer@ife.ee.ethz.ch)
+ *         using soundcore instead of OSS
+ */
+
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/kthread.h>
+#include <linux/freezer.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/drv-intf/msp3400.h>
+#include <media/i2c/tvaudio.h>
+#include "msp3400-driver.h"
+
+/* ---------------------------------------------------------------------- */
+
+MODULE_DESCRIPTION("device driver for msp34xx TV sound processor");
+MODULE_AUTHOR("Gerd Knorr");
+MODULE_LICENSE("GPL");
+
+/* module parameters */
+static int opmode   = OPMODE_AUTO;
+int msp_debug;		 /* msp_debug output */
+bool msp_once;		 /* no continuous stereo monitoring */
+bool msp_amsound;	 /* hard-wire AM sound at 6.5 Hz (france),
+			    the autoscan seems work well only with FM... */
+int msp_standard = 1;    /* Override auto detect of audio msp_standard,
+			    if needed. */
+bool msp_dolby;
+
+int msp_stereo_thresh = 0x190; /* a2 threshold for stereo/bilingual
+					(msp34xxg only) 0x00a0-0x03c0 */
+
+/* read-only */
+module_param(opmode,           int, 0444);
+
+/* read-write */
+module_param_named(once, msp_once,                      bool, 0644);
+module_param_named(debug, msp_debug,                    int,  0644);
+module_param_named(stereo_threshold, msp_stereo_thresh, int,  0644);
+module_param_named(standard, msp_standard,              int,  0644);
+module_param_named(amsound, msp_amsound,                bool, 0644);
+module_param_named(dolby, msp_dolby,                    bool, 0644);
+
+MODULE_PARM_DESC(opmode, "Forces a MSP3400 opmode. 0=Manual, 1=Autodetect, 2=Autodetect and autoselect");
+MODULE_PARM_DESC(once, "No continuous stereo monitoring");
+MODULE_PARM_DESC(debug, "Enable debug messages [0-3]");
+MODULE_PARM_DESC(stereo_threshold, "Sets signal threshold to activate stereo");
+MODULE_PARM_DESC(standard, "Specify audio standard: 32 = NTSC, 64 = radio, Default: Autodetect");
+MODULE_PARM_DESC(amsound, "Hardwire AM sound at 6.5Hz (France), FM can autoscan");
+MODULE_PARM_DESC(dolby, "Activates Dolby processing");
+
+/* ---------------------------------------------------------------------- */
+
+/* control subaddress */
+#define I2C_MSP_CONTROL 0x00
+/* demodulator unit subaddress */
+#define I2C_MSP_DEM     0x10
+/* DSP unit subaddress */
+#define I2C_MSP_DSP     0x12
+
+
+/* ----------------------------------------------------------------------- */
+/* functions for talking to the MSP3400C Sound processor                   */
+
+int msp_reset(struct i2c_client *client)
+{
+	/* reset and read revision code */
+	static u8 reset_off[3] = { I2C_MSP_CONTROL, 0x80, 0x00 };
+	static u8 reset_on[3]  = { I2C_MSP_CONTROL, 0x00, 0x00 };
+	static u8 write[3]     = { I2C_MSP_DSP + 1, 0x00, 0x1e };
+	u8 read[2];
+	struct i2c_msg reset[2] = {
+		{
+			.addr = client->addr,
+			.flags = I2C_M_IGNORE_NAK,
+			.len = 3,
+			.buf = reset_off
+		},
+		{
+			.addr = client->addr,
+			.flags = I2C_M_IGNORE_NAK,
+			.len = 3,
+			.buf = reset_on
+		},
+	};
+	struct i2c_msg test[2] = {
+		{
+			.addr = client->addr,
+			.len = 3,
+			.buf = write
+		},
+		{
+			.addr = client->addr,
+			.flags = I2C_M_RD,
+			.len = 2,
+			.buf = read
+		},
+	};
+
+	dev_dbg_lvl(&client->dev, 3, msp_debug, "msp_reset\n");
+	if (i2c_transfer(client->adapter, &reset[0], 1) != 1 ||
+	    i2c_transfer(client->adapter, &reset[1], 1) != 1 ||
+	    i2c_transfer(client->adapter, test, 2) != 2) {
+		dev_err(&client->dev, "chip reset failed\n");
+		return -1;
+	}
+	return 0;
+}
+
+static int msp_read(struct i2c_client *client, int dev, int addr)
+{
+	int err, retval;
+	u8 write[3];
+	u8 read[2];
+	struct i2c_msg msgs[2] = {
+		{
+			.addr = client->addr,
+			.len = 3,
+			.buf = write
+		},
+		{
+			.addr = client->addr,
+			.flags = I2C_M_RD,
+			.len = 2,
+			.buf = read
+		}
+	};
+
+	write[0] = dev + 1;
+	write[1] = addr >> 8;
+	write[2] = addr & 0xff;
+
+	for (err = 0; err < 3; err++) {
+		if (i2c_transfer(client->adapter, msgs, 2) == 2)
+			break;
+		dev_warn(&client->dev, "I/O error #%d (read 0x%02x/0x%02x)\n", err,
+		       dev, addr);
+		schedule_timeout_interruptible(msecs_to_jiffies(10));
+	}
+	if (err == 3) {
+		dev_warn(&client->dev, "resetting chip, sound will go off.\n");
+		msp_reset(client);
+		return -1;
+	}
+	retval = read[0] << 8 | read[1];
+	dev_dbg_lvl(&client->dev, 3, msp_debug, "msp_read(0x%x, 0x%x): 0x%x\n",
+			dev, addr, retval);
+	return retval;
+}
+
+int msp_read_dem(struct i2c_client *client, int addr)
+{
+	return msp_read(client, I2C_MSP_DEM, addr);
+}
+
+int msp_read_dsp(struct i2c_client *client, int addr)
+{
+	return msp_read(client, I2C_MSP_DSP, addr);
+}
+
+static int msp_write(struct i2c_client *client, int dev, int addr, int val)
+{
+	int err;
+	u8 buffer[5];
+
+	buffer[0] = dev;
+	buffer[1] = addr >> 8;
+	buffer[2] = addr &  0xff;
+	buffer[3] = val  >> 8;
+	buffer[4] = val  &  0xff;
+
+	dev_dbg_lvl(&client->dev, 3, msp_debug, "msp_write(0x%x, 0x%x, 0x%x)\n",
+			dev, addr, val);
+	for (err = 0; err < 3; err++) {
+		if (i2c_master_send(client, buffer, 5) == 5)
+			break;
+		dev_warn(&client->dev, "I/O error #%d (write 0x%02x/0x%02x)\n", err,
+		       dev, addr);
+		schedule_timeout_interruptible(msecs_to_jiffies(10));
+	}
+	if (err == 3) {
+		dev_warn(&client->dev, "resetting chip, sound will go off.\n");
+		msp_reset(client);
+		return -1;
+	}
+	return 0;
+}
+
+int msp_write_dem(struct i2c_client *client, int addr, int val)
+{
+	return msp_write(client, I2C_MSP_DEM, addr, val);
+}
+
+int msp_write_dsp(struct i2c_client *client, int addr, int val)
+{
+	return msp_write(client, I2C_MSP_DSP, addr, val);
+}
+
+/* ----------------------------------------------------------------------- *
+ * bits  9  8  5 - SCART DSP input Select:
+ *       0  0  0 - SCART 1 to DSP input (reset position)
+ *       0  1  0 - MONO to DSP input
+ *       1  0  0 - SCART 2 to DSP input
+ *       1  1  1 - Mute DSP input
+ *
+ * bits 11 10  6 - SCART 1 Output Select:
+ *       0  0  0 - undefined (reset position)
+ *       0  1  0 - SCART 2 Input to SCART 1 Output (for devices with 2 SCARTS)
+ *       1  0  0 - MONO input to SCART 1 Output
+ *       1  1  0 - SCART 1 DA to SCART 1 Output
+ *       0  0  1 - SCART 2 DA to SCART 1 Output
+ *       0  1  1 - SCART 1 Input to SCART 1 Output
+ *       1  1  1 - Mute SCART 1 Output
+ *
+ * bits 13 12  7 - SCART 2 Output Select (for devices with 2 Output SCART):
+ *       0  0  0 - SCART 1 DA to SCART 2 Output (reset position)
+ *       0  1  0 - SCART 1 Input to SCART 2 Output
+ *       1  0  0 - MONO input to SCART 2 Output
+ *       0  0  1 - SCART 2 DA to SCART 2 Output
+ *       0  1  1 - SCART 2 Input to SCART 2 Output
+ *       1  1  0 - Mute SCART 2 Output
+ *
+ * Bits 4 to 0 should be zero.
+ * ----------------------------------------------------------------------- */
+
+static int scarts[3][9] = {
+	/* MASK   IN1     IN2     IN3     IN4     IN1_DA  IN2_DA  MONO    MUTE   */
+	/* SCART DSP Input select */
+	{ 0x0320, 0x0000, 0x0200, 0x0300, 0x0020, -1,     -1,     0x0100, 0x0320 },
+	/* SCART1 Output select */
+	{ 0x0c40, 0x0440, 0x0400, 0x0000, 0x0840, 0x0c00, 0x0040, 0x0800, 0x0c40 },
+	/* SCART2 Output select */
+	{ 0x3080, 0x1000, 0x1080, 0x2080, 0x3080, 0x0000, 0x0080, 0x2000, 0x3000 },
+};
+
+static char *scart_names[] = {
+	"in1", "in2", "in3", "in4", "in1 da", "in2 da", "mono", "mute"
+};
+
+void msp_set_scart(struct i2c_client *client, int in, int out)
+{
+	struct msp_state *state = to_state(i2c_get_clientdata(client));
+
+	state->in_scart = in;
+
+	if (in >= 0 && in <= 7 && out >= 0 && out <= 2) {
+		if (-1 == scarts[out][in + 1])
+			return;
+
+		state->acb &= ~scarts[out][0];
+		state->acb |=  scarts[out][in + 1];
+	} else
+		state->acb = 0xf60; /* Mute Input and SCART 1 Output */
+
+	dev_dbg_lvl(&client->dev, 1, msp_debug, "scart switch: %s => %d (ACB=0x%04x)\n",
+					scart_names[in], out, state->acb);
+	msp_write_dsp(client, 0x13, state->acb);
+
+	/* Sets I2S speed 0 = 1.024 Mbps, 1 = 2.048 Mbps */
+	if (state->has_i2s_conf)
+		msp_write_dem(client, 0x40, state->i2s_mode);
+}
+
+/* ------------------------------------------------------------------------ */
+
+static void msp_wake_thread(struct i2c_client *client)
+{
+	struct msp_state *state = to_state(i2c_get_clientdata(client));
+
+	if (NULL == state->kthread)
+		return;
+	state->watch_stereo = 0;
+	state->restart = 1;
+	wake_up_interruptible(&state->wq);
+}
+
+int msp_sleep(struct msp_state *state, int timeout)
+{
+	DECLARE_WAITQUEUE(wait, current);
+
+	add_wait_queue(&state->wq, &wait);
+	if (!kthread_should_stop()) {
+		if (timeout < 0) {
+			set_current_state(TASK_INTERRUPTIBLE);
+			schedule();
+		} else {
+			schedule_timeout_interruptible
+						(msecs_to_jiffies(timeout));
+		}
+	}
+
+	remove_wait_queue(&state->wq, &wait);
+	try_to_freeze();
+	return state->restart;
+}
+
+/* ------------------------------------------------------------------------ */
+
+static int msp_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct msp_state *state = ctrl_to_state(ctrl);
+	struct i2c_client *client = v4l2_get_subdevdata(&state->sd);
+	int val = ctrl->val;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUDIO_VOLUME: {
+		/* audio volume cluster */
+		int reallymuted = state->muted->val | state->scan_in_progress;
+
+		if (!reallymuted)
+			val = (val * 0x7f / 65535) << 8;
+
+		dev_dbg_lvl(&client->dev, 1, msp_debug, "mute=%s scanning=%s volume=%d\n",
+				state->muted->val ? "on" : "off",
+				state->scan_in_progress ? "yes" : "no",
+				state->volume->val);
+
+		msp_write_dsp(client, 0x0000, val);
+		msp_write_dsp(client, 0x0007, reallymuted ? 0x1 : (val | 0x1));
+		if (state->has_scart2_out_volume)
+			msp_write_dsp(client, 0x0040, reallymuted ? 0x1 : (val | 0x1));
+		if (state->has_headphones)
+			msp_write_dsp(client, 0x0006, val);
+		break;
+	}
+
+	case V4L2_CID_AUDIO_BASS:
+		val = ((val - 32768) * 0x60 / 65535) << 8;
+		msp_write_dsp(client, 0x0002, val);
+		if (state->has_headphones)
+			msp_write_dsp(client, 0x0031, val);
+		break;
+
+	case V4L2_CID_AUDIO_TREBLE:
+		val = ((val - 32768) * 0x60 / 65535) << 8;
+		msp_write_dsp(client, 0x0003, val);
+		if (state->has_headphones)
+			msp_write_dsp(client, 0x0032, val);
+		break;
+
+	case V4L2_CID_AUDIO_LOUDNESS:
+		val = val ? ((5 * 4) << 8) : 0;
+		msp_write_dsp(client, 0x0004, val);
+		if (state->has_headphones)
+			msp_write_dsp(client, 0x0033, val);
+		break;
+
+	case V4L2_CID_AUDIO_BALANCE:
+		val = (u8)((val / 256) - 128);
+		msp_write_dsp(client, 0x0001, val << 8);
+		if (state->has_headphones)
+			msp_write_dsp(client, 0x0030, val << 8);
+		break;
+
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+void msp_update_volume(struct msp_state *state)
+{
+	/* Force an update of the volume/mute cluster */
+	v4l2_ctrl_lock(state->volume);
+	state->volume->val = state->volume->cur.val;
+	state->muted->val = state->muted->cur.val;
+	msp_s_ctrl(state->volume);
+	v4l2_ctrl_unlock(state->volume);
+}
+
+/* --- v4l2 ioctls --- */
+static int msp_s_radio(struct v4l2_subdev *sd)
+{
+	struct msp_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (state->radio)
+		return 0;
+	state->radio = 1;
+	dev_dbg_lvl(&client->dev, 1, msp_debug, "switching to radio mode\n");
+	state->watch_stereo = 0;
+	switch (state->opmode) {
+	case OPMODE_MANUAL:
+		/* set msp3400 to FM radio mode */
+		msp3400c_set_mode(client, MSP_MODE_FM_RADIO);
+		msp3400c_set_carrier(client, MSP_CARRIER(10.7),
+				MSP_CARRIER(10.7));
+		msp_update_volume(state);
+		break;
+	case OPMODE_AUTODETECT:
+	case OPMODE_AUTOSELECT:
+		/* the thread will do for us */
+		msp_wake_thread(client);
+		break;
+	}
+	return 0;
+}
+
+static int msp_s_frequency(struct v4l2_subdev *sd, const struct v4l2_frequency *freq)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	/* new channel -- kick audio carrier scan */
+	msp_wake_thread(client);
+	return 0;
+}
+
+static int msp_querystd(struct v4l2_subdev *sd, v4l2_std_id *id)
+{
+	struct msp_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	*id &= state->detected_std;
+
+	dev_dbg_lvl(&client->dev, 2, msp_debug,
+		"detected standard: %s(0x%08Lx)\n",
+		msp_standard_std_name(state->std), state->detected_std);
+
+	return 0;
+}
+
+static int msp_s_std(struct v4l2_subdev *sd, v4l2_std_id id)
+{
+	struct msp_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int update = state->radio || state->v4l2_std != id;
+
+	state->v4l2_std = id;
+	state->radio = 0;
+	if (update)
+		msp_wake_thread(client);
+	return 0;
+}
+
+static int msp_s_routing(struct v4l2_subdev *sd,
+			 u32 input, u32 output, u32 config)
+{
+	struct msp_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int tuner = (input >> 3) & 1;
+	int sc_in = input & 0x7;
+	int sc1_out = output & 0xf;
+	int sc2_out = (output >> 4) & 0xf;
+	u16 val, reg;
+	int i;
+	int extern_input = 1;
+
+	if (state->route_in == input && state->route_out == output)
+		return 0;
+	state->route_in = input;
+	state->route_out = output;
+	/* check if the tuner input is used */
+	for (i = 0; i < 5; i++) {
+		if (((input >> (4 + i * 4)) & 0xf) == 0)
+			extern_input = 0;
+	}
+	state->mode = extern_input ? MSP_MODE_EXTERN : MSP_MODE_AM_DETECT;
+	state->rxsubchans = V4L2_TUNER_SUB_STEREO;
+	msp_set_scart(client, sc_in, 0);
+	msp_set_scart(client, sc1_out, 1);
+	msp_set_scart(client, sc2_out, 2);
+	msp_set_audmode(client);
+	reg = (state->opmode == OPMODE_AUTOSELECT) ? 0x30 : 0xbb;
+	val = msp_read_dem(client, reg);
+	msp_write_dem(client, reg, (val & ~0x100) | (tuner << 8));
+	/* wake thread when a new input is chosen */
+	msp_wake_thread(client);
+	return 0;
+}
+
+static int msp_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
+{
+	struct msp_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (vt->type != V4L2_TUNER_ANALOG_TV)
+		return 0;
+	if (!state->radio) {
+		if (state->opmode == OPMODE_AUTOSELECT)
+			msp_detect_stereo(client);
+		vt->rxsubchans = state->rxsubchans;
+	}
+	vt->audmode = state->audmode;
+	vt->capability |= V4L2_TUNER_CAP_STEREO |
+		V4L2_TUNER_CAP_LANG1 | V4L2_TUNER_CAP_LANG2;
+	return 0;
+}
+
+static int msp_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *vt)
+{
+	struct msp_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (state->radio)  /* TODO: add mono/stereo support for radio */
+		return 0;
+	if (state->audmode == vt->audmode)
+		return 0;
+	state->audmode = vt->audmode;
+	/* only set audmode */
+	msp_set_audmode(client);
+	return 0;
+}
+
+static int msp_s_i2s_clock_freq(struct v4l2_subdev *sd, u32 freq)
+{
+	struct msp_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	dev_dbg_lvl(&client->dev, 1, msp_debug, "Setting I2S speed to %d\n", freq);
+
+	switch (freq) {
+		case 1024000:
+			state->i2s_mode = 0;
+			break;
+		case 2048000:
+			state->i2s_mode = 1;
+			break;
+		default:
+			return -EINVAL;
+	}
+	return 0;
+}
+
+static int msp_log_status(struct v4l2_subdev *sd)
+{
+	struct msp_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	const char *p;
+	char prefix[V4L2_SUBDEV_NAME_SIZE + 20];
+
+	if (state->opmode == OPMODE_AUTOSELECT)
+		msp_detect_stereo(client);
+	dev_info(&client->dev, "%s rev1 = 0x%04x rev2 = 0x%04x\n",
+			client->name, state->rev1, state->rev2);
+	snprintf(prefix, sizeof(prefix), "%s: Audio:    ", sd->name);
+	v4l2_ctrl_handler_log_status(&state->hdl, prefix);
+	switch (state->mode) {
+		case MSP_MODE_AM_DETECT: p = "AM (for carrier detect)"; break;
+		case MSP_MODE_FM_RADIO: p = "FM Radio"; break;
+		case MSP_MODE_FM_TERRA: p = "Terrestrial FM-mono/stereo"; break;
+		case MSP_MODE_FM_SAT: p = "Satellite FM-mono"; break;
+		case MSP_MODE_FM_NICAM1: p = "NICAM/FM (B/G, D/K)"; break;
+		case MSP_MODE_FM_NICAM2: p = "NICAM/FM (I)"; break;
+		case MSP_MODE_AM_NICAM: p = "NICAM/AM (L)"; break;
+		case MSP_MODE_BTSC: p = "BTSC"; break;
+		case MSP_MODE_EXTERN: p = "External input"; break;
+		default: p = "unknown"; break;
+	}
+	if (state->mode == MSP_MODE_EXTERN) {
+		dev_info(&client->dev, "Mode:     %s\n", p);
+	} else if (state->opmode == OPMODE_MANUAL) {
+		dev_info(&client->dev, "Mode:     %s (%s%s)\n", p,
+				(state->rxsubchans & V4L2_TUNER_SUB_STEREO) ? "stereo" : "mono",
+				(state->rxsubchans & V4L2_TUNER_SUB_LANG2) ? ", dual" : "");
+	} else {
+		if (state->opmode == OPMODE_AUTODETECT)
+			dev_info(&client->dev, "Mode:     %s\n", p);
+		dev_info(&client->dev, "Standard: %s (%s%s)\n",
+				msp_standard_std_name(state->std),
+				(state->rxsubchans & V4L2_TUNER_SUB_STEREO) ? "stereo" : "mono",
+				(state->rxsubchans & V4L2_TUNER_SUB_LANG2) ? ", dual" : "");
+	}
+	dev_info(&client->dev, "Audmode:  0x%04x\n", state->audmode);
+	dev_info(&client->dev, "Routing:  0x%08x (input) 0x%08x (output)\n",
+			state->route_in, state->route_out);
+	dev_info(&client->dev, "ACB:      0x%04x\n", state->acb);
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int msp_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	dev_dbg_lvl(&client->dev, 1, msp_debug, "suspend\n");
+	msp_reset(client);
+	return 0;
+}
+
+static int msp_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	dev_dbg_lvl(&client->dev, 1, msp_debug, "resume\n");
+	msp_wake_thread(client);
+	return 0;
+}
+#endif
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_ctrl_ops msp_ctrl_ops = {
+	.s_ctrl = msp_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops msp_core_ops = {
+	.log_status = msp_log_status,
+};
+
+static const struct v4l2_subdev_video_ops msp_video_ops = {
+	.s_std = msp_s_std,
+	.querystd = msp_querystd,
+};
+
+static const struct v4l2_subdev_tuner_ops msp_tuner_ops = {
+	.s_frequency = msp_s_frequency,
+	.g_tuner = msp_g_tuner,
+	.s_tuner = msp_s_tuner,
+	.s_radio = msp_s_radio,
+};
+
+static const struct v4l2_subdev_audio_ops msp_audio_ops = {
+	.s_routing = msp_s_routing,
+	.s_i2s_clock_freq = msp_s_i2s_clock_freq,
+};
+
+static const struct v4l2_subdev_ops msp_ops = {
+	.core = &msp_core_ops,
+	.video = &msp_video_ops,
+	.tuner = &msp_tuner_ops,
+	.audio = &msp_audio_ops,
+};
+
+/* ----------------------------------------------------------------------- */
+
+
+static const char * const opmode_str[] = {
+	[OPMODE_MANUAL] = "manual",
+	[OPMODE_AUTODETECT] = "autodetect",
+	[OPMODE_AUTOSELECT] = "autodetect and autoselect",
+};
+
+static int msp_probe(struct i2c_client *client)
+{
+	const struct i2c_device_id *id = i2c_client_get_device_id(client);
+	struct msp_state *state;
+	struct v4l2_subdev *sd;
+	struct v4l2_ctrl_handler *hdl;
+	int (*thread_func)(void *data) = NULL;
+	int msp_hard;
+	int msp_family;
+	int msp_revision;
+	int msp_product, msp_prod_hi, msp_prod_lo;
+	int msp_rom;
+#if defined(CONFIG_MEDIA_CONTROLLER)
+	int ret;
+#endif
+
+	if (!id)
+		strscpy(client->name, "msp3400", sizeof(client->name));
+
+	if (msp_reset(client) == -1) {
+		dev_dbg_lvl(&client->dev, 1, msp_debug, "msp3400 not found\n");
+		return -ENODEV;
+	}
+
+	state = devm_kzalloc(&client->dev, sizeof(*state), GFP_KERNEL);
+	if (!state)
+		return -ENOMEM;
+
+	sd = &state->sd;
+	v4l2_i2c_subdev_init(sd, client, &msp_ops);
+
+#if defined(CONFIG_MEDIA_CONTROLLER)
+	state->pads[MSP3400_PAD_IF_INPUT].flags = MEDIA_PAD_FL_SINK;
+	state->pads[MSP3400_PAD_IF_INPUT].sig_type = PAD_SIGNAL_AUDIO;
+	state->pads[MSP3400_PAD_OUT].flags = MEDIA_PAD_FL_SOURCE;
+	state->pads[MSP3400_PAD_OUT].sig_type = PAD_SIGNAL_AUDIO;
+
+	sd->entity.function = MEDIA_ENT_F_IF_AUD_DECODER;
+
+	ret = media_entity_pads_init(&sd->entity, 2, state->pads);
+	if (ret < 0)
+		return ret;
+#endif
+
+	state->v4l2_std = V4L2_STD_NTSC;
+	state->detected_std = V4L2_STD_ALL;
+	state->audmode = V4L2_TUNER_MODE_STEREO;
+	state->input = -1;
+	state->i2s_mode = 0;
+	init_waitqueue_head(&state->wq);
+	/* These are the reset input/output positions */
+	state->route_in = MSP_INPUT_DEFAULT;
+	state->route_out = MSP_OUTPUT_DEFAULT;
+
+	state->rev1 = msp_read_dsp(client, 0x1e);
+	if (state->rev1 != -1)
+		state->rev2 = msp_read_dsp(client, 0x1f);
+	dev_dbg_lvl(&client->dev, 1, msp_debug, "rev1=0x%04x, rev2=0x%04x\n",
+			state->rev1, state->rev2);
+	if (state->rev1 == -1 || (state->rev1 == 0 && state->rev2 == 0)) {
+		dev_dbg_lvl(&client->dev, 1, msp_debug,
+				"not an msp3400 (cannot read chip version)\n");
+		return -ENODEV;
+	}
+
+	msp_family = ((state->rev1 >> 4) & 0x0f) + 3;
+	msp_product = (state->rev2 >> 8) & 0xff;
+	msp_prod_hi = msp_product / 10;
+	msp_prod_lo = msp_product % 10;
+	msp_revision = (state->rev1 & 0x0f) + '@';
+	msp_hard = ((state->rev1 >> 8) & 0xff) + '@';
+	msp_rom = state->rev2 & 0x1f;
+	/* Rev B=2, C=3, D=4, G=7 */
+	state->ident = msp_family * 10000 + 4000 + msp_product * 10 +
+			msp_revision - '@';
+
+	/* Has NICAM support: all mspx41x and mspx45x products have NICAM */
+	state->has_nicam =
+		msp_prod_hi == 1 || msp_prod_hi == 5;
+	/* Has radio support: was added with revision G */
+	state->has_radio =
+		msp_revision >= 'G';
+	/* Has headphones output: not for stripped down products */
+	state->has_headphones =
+		msp_prod_lo < 5;
+	/* Has scart2 input: not in stripped down products of the '3' family */
+	state->has_scart2 =
+		msp_family >= 4 || msp_prod_lo < 7;
+	/* Has scart3 input: not in stripped down products of the '3' family */
+	state->has_scart3 =
+		msp_family >= 4 || msp_prod_lo < 5;
+	/* Has scart4 input: not in pre D revisions, not in stripped D revs */
+	state->has_scart4 =
+		msp_family >= 4 || (msp_revision >= 'D' && msp_prod_lo < 5);
+	/* Has scart2 output: not in stripped down products of
+	 * the '3' family */
+	state->has_scart2_out =
+		msp_family >= 4 || msp_prod_lo < 5;
+	/* Has scart2 a volume control? Not in pre-D revisions. */
+	state->has_scart2_out_volume =
+		msp_revision > 'C' && state->has_scart2_out;
+	/* Has a configurable i2s out? */
+	state->has_i2s_conf =
+		msp_revision >= 'G' && msp_prod_lo < 7;
+	/* Has subwoofer output: not in pre-D revs and not in stripped down
+	 * products */
+	state->has_subwoofer =
+		msp_revision >= 'D' && msp_prod_lo < 5;
+	/* Has soundprocessing (bass/treble/balance/loudness/equalizer):
+	 *  not in stripped down products */
+	state->has_sound_processing =
+		msp_prod_lo < 7;
+	/* Has Virtual Dolby Surround: only in msp34x1 */
+	state->has_virtual_dolby_surround =
+		msp_revision == 'G' && msp_prod_lo == 1;
+	/* Has Virtual Dolby Surround & Dolby Pro Logic: only in msp34x2 */
+	state->has_dolby_pro_logic =
+		msp_revision == 'G' && msp_prod_lo == 2;
+	/* The msp343xG supports BTSC only and cannot do Automatic Standard
+	 * Detection. */
+	state->force_btsc =
+		msp_family == 3 && msp_revision == 'G' && msp_prod_hi == 3;
+
+	state->opmode = opmode;
+	if (state->opmode < OPMODE_MANUAL
+	    || state->opmode > OPMODE_AUTOSELECT) {
+		/* MSP revision G and up have both autodetect and autoselect */
+		if (msp_revision >= 'G')
+			state->opmode = OPMODE_AUTOSELECT;
+		/* MSP revision D and up have autodetect */
+		else if (msp_revision >= 'D')
+			state->opmode = OPMODE_AUTODETECT;
+		else
+			state->opmode = OPMODE_MANUAL;
+	}
+
+	hdl = &state->hdl;
+	v4l2_ctrl_handler_init(hdl, 6);
+	if (state->has_sound_processing) {
+		v4l2_ctrl_new_std(hdl, &msp_ctrl_ops,
+			V4L2_CID_AUDIO_BASS, 0, 65535, 65535 / 100, 32768);
+		v4l2_ctrl_new_std(hdl, &msp_ctrl_ops,
+			V4L2_CID_AUDIO_TREBLE, 0, 65535, 65535 / 100, 32768);
+		v4l2_ctrl_new_std(hdl, &msp_ctrl_ops,
+			V4L2_CID_AUDIO_LOUDNESS, 0, 1, 1, 0);
+	}
+	state->volume = v4l2_ctrl_new_std(hdl, &msp_ctrl_ops,
+			V4L2_CID_AUDIO_VOLUME, 0, 65535, 65535 / 100, 58880);
+	v4l2_ctrl_new_std(hdl, &msp_ctrl_ops,
+			V4L2_CID_AUDIO_BALANCE, 0, 65535, 65535 / 100, 32768);
+	state->muted = v4l2_ctrl_new_std(hdl, &msp_ctrl_ops,
+			V4L2_CID_AUDIO_MUTE, 0, 1, 1, 0);
+	sd->ctrl_handler = hdl;
+	if (hdl->error) {
+		int err = hdl->error;
+
+		v4l2_ctrl_handler_free(hdl);
+		return err;
+	}
+
+	v4l2_ctrl_cluster(2, &state->volume);
+	v4l2_ctrl_handler_setup(hdl);
+
+	dev_info(&client->dev,
+		 "MSP%d4%02d%c-%c%d found on %s: supports %s%s%s, mode is %s\n",
+		 msp_family, msp_product,
+		 msp_revision, msp_hard, msp_rom,
+		 client->adapter->name,
+		 (state->has_nicam) ? "nicam" : "",
+		 (state->has_nicam && state->has_radio) ? " and " : "",
+		 (state->has_radio) ? "radio" : "",
+		 opmode_str[state->opmode]);
+
+	/* version-specific initialization */
+	switch (state->opmode) {
+	case OPMODE_MANUAL:
+		thread_func = msp3400c_thread;
+		break;
+	case OPMODE_AUTODETECT:
+		thread_func = msp3410d_thread;
+		break;
+	case OPMODE_AUTOSELECT:
+		thread_func = msp34xxg_thread;
+		break;
+	}
+
+	/* startup control thread if needed */
+	if (thread_func) {
+		state->kthread = kthread_run(thread_func, client, "msp34xx");
+
+		if (IS_ERR(state->kthread))
+			dev_warn(&client->dev, "kernel_thread() failed\n");
+		msp_wake_thread(client);
+	}
+	return 0;
+}
+
+static void msp_remove(struct i2c_client *client)
+{
+	struct msp_state *state = to_state(i2c_get_clientdata(client));
+
+	v4l2_device_unregister_subdev(&state->sd);
+	/* shutdown control thread */
+	if (state->kthread) {
+		state->restart = 1;
+		kthread_stop(state->kthread);
+	}
+	msp_reset(client);
+
+	v4l2_ctrl_handler_free(&state->hdl);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct dev_pm_ops msp3400_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(msp_suspend, msp_resume)
+};
+
+static const struct i2c_device_id msp_id[] = {
+	{ "msp3400", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, msp_id);
+
+static struct i2c_driver msp_driver = {
+	.driver = {
+		.name	= "msp3400",
+		.pm	= &msp3400_pm_ops,
+	},
+	.probe		= msp_probe,
+	.remove		= msp_remove,
+	.id_table	= msp_id,
+};
+
+module_i2c_driver(msp_driver);
diff --git a/drivers/media/i2c/msp3400-driver.h b/drivers/media/i2c/msp3400-driver.h
new file mode 100644
index 000000000..2bb9d5ff1
--- /dev/null
+++ b/drivers/media/i2c/msp3400-driver.h
@@ -0,0 +1,149 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ */
+
+#ifndef MSP3400_DRIVER_H
+#define MSP3400_DRIVER_H
+
+#include <media/drv-intf/msp3400.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-mc.h>
+
+/* ---------------------------------------------------------------------- */
+
+/* This macro is allowed for *constants* only, gcc must calculate it
+   at compile time.  Remember -- no floats in kernel mode */
+#define MSP_CARRIER(freq) ((int)((float)(freq / 18.432) * (1 << 24)))
+
+#define MSP_MODE_AM_DETECT   0
+#define MSP_MODE_FM_RADIO    2
+#define MSP_MODE_FM_TERRA    3
+#define MSP_MODE_FM_SAT      4
+#define MSP_MODE_FM_NICAM1   5
+#define MSP_MODE_FM_NICAM2   6
+#define MSP_MODE_AM_NICAM    7
+#define MSP_MODE_BTSC        8
+#define MSP_MODE_EXTERN      9
+
+#define SCART_IN1     0
+#define SCART_IN2     1
+#define SCART_IN3     2
+#define SCART_IN4     3
+#define SCART_IN1_DA  4
+#define SCART_IN2_DA  5
+#define SCART_MONO    6
+#define SCART_MUTE    7
+
+#define SCART_DSP_IN  0
+#define SCART1_OUT    1
+#define SCART2_OUT    2
+
+#define OPMODE_AUTO       -1
+#define OPMODE_MANUAL      0
+#define OPMODE_AUTODETECT  1   /* use autodetect (>= msp3410 only) */
+#define OPMODE_AUTOSELECT  2   /* use autodetect & autoselect (>= msp34xxG)   */
+
+/* module parameters */
+extern int msp_debug;
+extern bool msp_once;
+extern bool msp_amsound;
+extern int msp_standard;
+extern bool msp_dolby;
+extern int msp_stereo_thresh;
+
+enum msp3400_pads {
+	MSP3400_PAD_IF_INPUT,
+	MSP3400_PAD_OUT,
+	MSP3400_NUM_PADS
+};
+
+struct msp_state {
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
+	int rev1, rev2;
+	int ident;
+	u8 has_nicam;
+	u8 has_radio;
+	u8 has_headphones;
+	u8 has_ntsc_jp_d_k3;
+	u8 has_scart2;
+	u8 has_scart3;
+	u8 has_scart4;
+	u8 has_scart2_out;
+	u8 has_scart2_out_volume;
+	u8 has_i2s_conf;
+	u8 has_subwoofer;
+	u8 has_sound_processing;
+	u8 has_virtual_dolby_surround;
+	u8 has_dolby_pro_logic;
+	u8 force_btsc;
+
+	int radio;
+	int opmode;
+	int std;
+	int mode;
+	v4l2_std_id v4l2_std, detected_std;
+	int nicam_on;
+	int acb;
+	int in_scart;
+	int i2s_mode;
+	int main, second;	/* sound carrier */
+	int input;
+	u32 route_in;
+	u32 route_out;
+
+	/* v4l2 */
+	int audmode;
+	int rxsubchans;
+
+	struct {
+		/* volume cluster */
+		struct v4l2_ctrl *volume;
+		struct v4l2_ctrl *muted;
+	};
+
+	int scan_in_progress;
+
+	/* thread */
+	struct task_struct   *kthread;
+	wait_queue_head_t    wq;
+	unsigned int         restart:1;
+	unsigned int         watch_stereo:1;
+
+#if IS_ENABLED(CONFIG_MEDIA_CONTROLLER)
+	struct media_pad pads[MSP3400_NUM_PADS];
+#endif
+};
+
+static inline struct msp_state *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct msp_state, sd);
+}
+
+static inline struct msp_state *ctrl_to_state(struct v4l2_ctrl *ctrl)
+{
+	return container_of(ctrl->handler, struct msp_state, hdl);
+}
+
+/* msp3400-driver.c */
+int msp_write_dem(struct i2c_client *client, int addr, int val);
+int msp_write_dsp(struct i2c_client *client, int addr, int val);
+int msp_read_dem(struct i2c_client *client, int addr);
+int msp_read_dsp(struct i2c_client *client, int addr);
+int msp_reset(struct i2c_client *client);
+void msp_set_scart(struct i2c_client *client, int in, int out);
+void msp_update_volume(struct msp_state *state);
+int msp_sleep(struct msp_state *state, int timeout);
+
+/* msp3400-kthreads.c */
+const char *msp_standard_std_name(int std);
+void msp_set_audmode(struct i2c_client *client);
+int msp_detect_stereo(struct i2c_client *client);
+int msp3400c_thread(void *data);
+int msp3410d_thread(void *data);
+int msp34xxg_thread(void *data);
+void msp3400c_set_mode(struct i2c_client *client, int mode);
+void msp3400c_set_carrier(struct i2c_client *client, int cdo1, int cdo2);
+
+#endif /* MSP3400_DRIVER_H */
diff --git a/drivers/media/i2c/msp3400-kthreads.c b/drivers/media/i2c/msp3400-kthreads.c
new file mode 100644
index 000000000..ecabc0e1d
--- /dev/null
+++ b/drivers/media/i2c/msp3400-kthreads.c
@@ -0,0 +1,1155 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Programming the mspx4xx sound processor family
+ *
+ * (c) 1997-2001 Gerd Knorr <kraxel@bytesex.org>
+ */
+
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/freezer.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-common.h>
+#include <media/drv-intf/msp3400.h>
+#include <linux/kthread.h>
+#include <linux/suspend.h>
+#include "msp3400-driver.h"
+
+/* this one uses the automatic sound standard detection of newer msp34xx
+   chip versions */
+static struct {
+	int retval;
+	int main, second;
+	char *name;
+	v4l2_std_id std;
+} msp_stdlist[] = {
+	{ 0x0000, 0, 0, "could not detect sound standard", V4L2_STD_ALL },
+	{ 0x0001, 0, 0, "autodetect start", V4L2_STD_ALL },
+	{ 0x0002, MSP_CARRIER(4.5), MSP_CARRIER(4.72),
+	  "4.5/4.72  M Dual FM-Stereo", V4L2_STD_MN },
+	{ 0x0003, MSP_CARRIER(5.5), MSP_CARRIER(5.7421875),
+	  "5.5/5.74  B/G Dual FM-Stereo", V4L2_STD_BG },
+	{ 0x0004, MSP_CARRIER(6.5), MSP_CARRIER(6.2578125),
+	  "6.5/6.25  D/K1 Dual FM-Stereo", V4L2_STD_DK },
+	{ 0x0005, MSP_CARRIER(6.5), MSP_CARRIER(6.7421875),
+	  "6.5/6.74  D/K2 Dual FM-Stereo", V4L2_STD_DK },
+	{ 0x0006, MSP_CARRIER(6.5), MSP_CARRIER(6.5),
+	  "6.5  D/K FM-Mono (HDEV3)", V4L2_STD_DK },
+	{ 0x0007, MSP_CARRIER(6.5), MSP_CARRIER(5.7421875),
+	  "6.5/5.74  D/K3 Dual FM-Stereo", V4L2_STD_DK },
+	{ 0x0008, MSP_CARRIER(5.5), MSP_CARRIER(5.85),
+	  "5.5/5.85  B/G NICAM FM", V4L2_STD_BG },
+	{ 0x0009, MSP_CARRIER(6.5), MSP_CARRIER(5.85),
+	  "6.5/5.85  L NICAM AM", V4L2_STD_L },
+	{ 0x000a, MSP_CARRIER(6.0), MSP_CARRIER(6.55),
+	  "6.0/6.55  I NICAM FM", V4L2_STD_PAL_I },
+	{ 0x000b, MSP_CARRIER(6.5), MSP_CARRIER(5.85),
+	  "6.5/5.85  D/K NICAM FM", V4L2_STD_DK },
+	{ 0x000c, MSP_CARRIER(6.5), MSP_CARRIER(5.85),
+	  "6.5/5.85  D/K NICAM FM (HDEV2)", V4L2_STD_DK },
+	{ 0x000d, MSP_CARRIER(6.5), MSP_CARRIER(5.85),
+	  "6.5/5.85  D/K NICAM FM (HDEV3)", V4L2_STD_DK },
+	{ 0x0020, MSP_CARRIER(4.5), MSP_CARRIER(4.5),
+	  "4.5  M BTSC-Stereo", V4L2_STD_MTS },
+	{ 0x0021, MSP_CARRIER(4.5), MSP_CARRIER(4.5),
+	  "4.5  M BTSC-Mono + SAP", V4L2_STD_MTS },
+	{ 0x0030, MSP_CARRIER(4.5), MSP_CARRIER(4.5),
+	  "4.5  M EIA-J Japan Stereo", V4L2_STD_NTSC_M_JP },
+	{ 0x0040, MSP_CARRIER(10.7), MSP_CARRIER(10.7),
+	  "10.7  FM-Stereo Radio", V4L2_STD_ALL },
+	{ 0x0050, MSP_CARRIER(6.5), MSP_CARRIER(6.5),
+	  "6.5  SAT-Mono", V4L2_STD_ALL },
+	{ 0x0051, MSP_CARRIER(7.02), MSP_CARRIER(7.20),
+	  "7.02/7.20  SAT-Stereo", V4L2_STD_ALL },
+	{ 0x0060, MSP_CARRIER(7.2), MSP_CARRIER(7.2),
+	  "7.2  SAT ADR", V4L2_STD_ALL },
+	{     -1, 0, 0, NULL, 0 }, /* EOF */
+};
+
+static struct msp3400c_init_data_dem {
+	int fir1[6];
+	int fir2[6];
+	int cdo1;
+	int cdo2;
+	int ad_cv;
+	int mode_reg;
+	int dsp_src;
+	int dsp_matrix;
+} msp3400c_init_data[] = {
+	{	/* AM (for carrier detect / msp3400) */
+		{75, 19, 36, 35, 39, 40},
+		{75, 19, 36, 35, 39, 40},
+		MSP_CARRIER(5.5), MSP_CARRIER(5.5),
+		0x00d0, 0x0500, 0x0020, 0x3000
+	}, {	/* AM (for carrier detect / msp3410) */
+		{-1, -1, -8, 2, 59, 126},
+		{-1, -1, -8, 2, 59, 126},
+		MSP_CARRIER(5.5), MSP_CARRIER(5.5),
+		0x00d0, 0x0100, 0x0020, 0x3000
+	}, {	/* FM Radio */
+		{-8, -8, 4, 6, 78, 107},
+		{-8, -8, 4, 6, 78, 107},
+		MSP_CARRIER(10.7), MSP_CARRIER(10.7),
+		0x00d0, 0x0480, 0x0020, 0x3000
+	}, {	/* Terrestrial FM-mono + FM-stereo */
+		{3, 18, 27, 48, 66, 72},
+		{3, 18, 27, 48, 66, 72},
+		MSP_CARRIER(5.5), MSP_CARRIER(5.5),
+		0x00d0, 0x0480, 0x0030, 0x3000
+	}, {	/* Sat FM-mono */
+		{ 1, 9, 14, 24, 33, 37},
+		{ 3, 18, 27, 48, 66, 72},
+		MSP_CARRIER(6.5), MSP_CARRIER(6.5),
+		0x00c6, 0x0480, 0x0000, 0x3000
+	}, {	/* NICAM/FM --  B/G (5.5/5.85), D/K (6.5/5.85) */
+		{-2, -8, -10, 10, 50, 86},
+		{3, 18, 27, 48, 66, 72},
+		MSP_CARRIER(5.5), MSP_CARRIER(5.5),
+		0x00d0, 0x0040, 0x0120, 0x3000
+	}, {	/* NICAM/FM -- I (6.0/6.552) */
+		{2, 4, -6, -4, 40, 94},
+		{3, 18, 27, 48, 66, 72},
+		MSP_CARRIER(6.0), MSP_CARRIER(6.0),
+		0x00d0, 0x0040, 0x0120, 0x3000
+	}, {	/* NICAM/AM -- L (6.5/5.85) */
+		{-2, -8, -10, 10, 50, 86},
+		{-4, -12, -9, 23, 79, 126},
+		MSP_CARRIER(6.5), MSP_CARRIER(6.5),
+		0x00c6, 0x0140, 0x0120, 0x7c00
+	},
+};
+
+struct msp3400c_carrier_detect {
+	int   cdo;
+	char *name;
+};
+
+static struct msp3400c_carrier_detect msp3400c_carrier_detect_main[] = {
+	/* main carrier */
+	{ MSP_CARRIER(4.5),        "4.5   NTSC"                   },
+	{ MSP_CARRIER(5.5),        "5.5   PAL B/G"                },
+	{ MSP_CARRIER(6.0),        "6.0   PAL I"                  },
+	{ MSP_CARRIER(6.5),        "6.5   PAL D/K + SAT + SECAM"  }
+};
+
+static struct msp3400c_carrier_detect msp3400c_carrier_detect_55[] = {
+	/* PAL B/G */
+	{ MSP_CARRIER(5.7421875),  "5.742 PAL B/G FM-stereo"     },
+	{ MSP_CARRIER(5.85),       "5.85  PAL B/G NICAM"         }
+};
+
+static struct msp3400c_carrier_detect msp3400c_carrier_detect_65[] = {
+	/* PAL SAT / SECAM */
+	{ MSP_CARRIER(5.85),       "5.85  PAL D/K + SECAM NICAM" },
+	{ MSP_CARRIER(6.2578125),  "6.25  PAL D/K1 FM-stereo" },
+	{ MSP_CARRIER(6.7421875),  "6.74  PAL D/K2 FM-stereo" },
+	{ MSP_CARRIER(7.02),       "7.02  PAL SAT FM-stereo s/b" },
+	{ MSP_CARRIER(7.20),       "7.20  PAL SAT FM-stereo s"   },
+	{ MSP_CARRIER(7.38),       "7.38  PAL SAT FM-stereo b"   },
+};
+
+/* ------------------------------------------------------------------------ */
+
+const char *msp_standard_std_name(int std)
+{
+	int i;
+
+	for (i = 0; msp_stdlist[i].name != NULL; i++)
+		if (msp_stdlist[i].retval == std)
+			return msp_stdlist[i].name;
+	return "unknown";
+}
+
+static v4l2_std_id msp_standard_std(int std)
+{
+	int i;
+
+	for (i = 0; msp_stdlist[i].name != NULL; i++)
+		if (msp_stdlist[i].retval == std)
+			return msp_stdlist[i].std;
+	return V4L2_STD_ALL;
+}
+
+static void msp_set_source(struct i2c_client *client, u16 src)
+{
+	struct msp_state *state = to_state(i2c_get_clientdata(client));
+
+	if (msp_dolby) {
+		msp_write_dsp(client, 0x0008, 0x0520); /* I2S1 */
+		msp_write_dsp(client, 0x0009, 0x0620); /* I2S2 */
+	} else {
+		msp_write_dsp(client, 0x0008, src);
+		msp_write_dsp(client, 0x0009, src);
+	}
+	msp_write_dsp(client, 0x000a, src);
+	msp_write_dsp(client, 0x000b, src);
+	msp_write_dsp(client, 0x000c, src);
+	if (state->has_scart2_out)
+		msp_write_dsp(client, 0x0041, src);
+}
+
+void msp3400c_set_carrier(struct i2c_client *client, int cdo1, int cdo2)
+{
+	msp_write_dem(client, 0x0093, cdo1 & 0xfff);
+	msp_write_dem(client, 0x009b, cdo1 >> 12);
+	msp_write_dem(client, 0x00a3, cdo2 & 0xfff);
+	msp_write_dem(client, 0x00ab, cdo2 >> 12);
+	msp_write_dem(client, 0x0056, 0); /* LOAD_REG_1/2 */
+}
+
+void msp3400c_set_mode(struct i2c_client *client, int mode)
+{
+	struct msp_state *state = to_state(i2c_get_clientdata(client));
+	struct msp3400c_init_data_dem *data = &msp3400c_init_data[mode];
+	int tuner = (state->route_in >> 3) & 1;
+	int i;
+
+	dev_dbg_lvl(&client->dev, 1, msp_debug, "set_mode: %d\n", mode);
+	state->mode = mode;
+	state->rxsubchans = V4L2_TUNER_SUB_MONO;
+
+	msp_write_dem(client, 0x00bb, data->ad_cv | (tuner ? 0x100 : 0));
+
+	for (i = 5; i >= 0; i--)               /* fir 1 */
+		msp_write_dem(client, 0x0001, data->fir1[i]);
+
+	msp_write_dem(client, 0x0005, 0x0004); /* fir 2 */
+	msp_write_dem(client, 0x0005, 0x0040);
+	msp_write_dem(client, 0x0005, 0x0000);
+	for (i = 5; i >= 0; i--)
+		msp_write_dem(client, 0x0005, data->fir2[i]);
+
+	msp_write_dem(client, 0x0083, data->mode_reg);
+
+	msp3400c_set_carrier(client, data->cdo1, data->cdo2);
+
+	msp_set_source(client, data->dsp_src);
+	/* set prescales */
+
+	/* volume prescale for SCART (AM mono input) */
+	msp_write_dsp(client, 0x000d, 0x1900);
+	msp_write_dsp(client, 0x000e, data->dsp_matrix);
+	if (state->has_nicam) /* nicam prescale */
+		msp_write_dsp(client, 0x0010, 0x5a00);
+}
+
+/* Set audio mode. Note that the pre-'G' models do not support BTSC+SAP,
+   nor do they support stereo BTSC. */
+static void msp3400c_set_audmode(struct i2c_client *client)
+{
+	static char *strmode[] = {
+		"mono", "stereo", "lang2", "lang1", "lang1+lang2"
+	};
+	struct msp_state *state = to_state(i2c_get_clientdata(client));
+	char *modestr = (state->audmode >= 0 && state->audmode < 5) ?
+		strmode[state->audmode] : "unknown";
+	int src = 0;	/* channel source: FM/AM, nicam or SCART */
+	int audmode = state->audmode;
+
+	if (state->opmode == OPMODE_AUTOSELECT) {
+		/* this method would break everything, let's make sure
+		 * it's never called
+		 */
+		dev_dbg_lvl(&client->dev, 1, msp_debug,
+			"set_audmode called with mode=%d instead of set_source (ignored)\n",
+			state->audmode);
+		return;
+	}
+
+	/* Note: for the C and D revs no NTSC stereo + SAP is possible as
+	   the hardware does not support SAP. So the rxsubchans combination
+	   of STEREO | LANG2 does not occur. */
+
+	if (state->mode != MSP_MODE_EXTERN) {
+		/* switch to mono if only mono is available */
+		if (state->rxsubchans == V4L2_TUNER_SUB_MONO)
+			audmode = V4L2_TUNER_MODE_MONO;
+		/* if bilingual */
+		else if (state->rxsubchans & V4L2_TUNER_SUB_LANG2) {
+			/* and mono or stereo, then fallback to lang1 */
+			if (audmode == V4L2_TUNER_MODE_MONO ||
+			    audmode == V4L2_TUNER_MODE_STEREO)
+				audmode = V4L2_TUNER_MODE_LANG1;
+		}
+		/* if stereo, and audmode is not mono, then switch to stereo */
+		else if (audmode != V4L2_TUNER_MODE_MONO)
+			audmode = V4L2_TUNER_MODE_STEREO;
+	}
+
+	/* switch demodulator */
+	switch (state->mode) {
+	case MSP_MODE_FM_TERRA:
+		dev_dbg_lvl(&client->dev, 1, msp_debug, "FM set_audmode: %s\n", modestr);
+		switch (audmode) {
+		case V4L2_TUNER_MODE_STEREO:
+			msp_write_dsp(client, 0x000e, 0x3001);
+			break;
+		case V4L2_TUNER_MODE_MONO:
+		case V4L2_TUNER_MODE_LANG1:
+		case V4L2_TUNER_MODE_LANG2:
+		case V4L2_TUNER_MODE_LANG1_LANG2:
+			msp_write_dsp(client, 0x000e, 0x3000);
+			break;
+		}
+		break;
+	case MSP_MODE_FM_SAT:
+		dev_dbg_lvl(&client->dev, 1, msp_debug, "SAT set_audmode: %s\n", modestr);
+		switch (audmode) {
+		case V4L2_TUNER_MODE_MONO:
+			msp3400c_set_carrier(client, MSP_CARRIER(6.5), MSP_CARRIER(6.5));
+			break;
+		case V4L2_TUNER_MODE_STEREO:
+		case V4L2_TUNER_MODE_LANG1_LANG2:
+			msp3400c_set_carrier(client, MSP_CARRIER(7.2), MSP_CARRIER(7.02));
+			break;
+		case V4L2_TUNER_MODE_LANG1:
+			msp3400c_set_carrier(client, MSP_CARRIER(7.38), MSP_CARRIER(7.02));
+			break;
+		case V4L2_TUNER_MODE_LANG2:
+			msp3400c_set_carrier(client, MSP_CARRIER(7.38), MSP_CARRIER(7.02));
+			break;
+		}
+		break;
+	case MSP_MODE_FM_NICAM1:
+	case MSP_MODE_FM_NICAM2:
+	case MSP_MODE_AM_NICAM:
+		dev_dbg_lvl(&client->dev, 1, msp_debug,
+			"NICAM set_audmode: %s\n", modestr);
+		if (state->nicam_on)
+			src = 0x0100;  /* NICAM */
+		break;
+	case MSP_MODE_BTSC:
+		dev_dbg_lvl(&client->dev, 1, msp_debug,
+			"BTSC set_audmode: %s\n", modestr);
+		break;
+	case MSP_MODE_EXTERN:
+		dev_dbg_lvl(&client->dev, 1, msp_debug,
+			"extern set_audmode: %s\n", modestr);
+		src = 0x0200;  /* SCART */
+		break;
+	case MSP_MODE_FM_RADIO:
+		dev_dbg_lvl(&client->dev, 1, msp_debug,
+			"FM-Radio set_audmode: %s\n", modestr);
+		break;
+	default:
+		dev_dbg_lvl(&client->dev, 1, msp_debug, "mono set_audmode\n");
+		return;
+	}
+
+	/* switch audio */
+	dev_dbg_lvl(&client->dev, 1, msp_debug, "set audmode %d\n", audmode);
+	switch (audmode) {
+	case V4L2_TUNER_MODE_STEREO:
+	case V4L2_TUNER_MODE_LANG1_LANG2:
+		src |= 0x0020;
+		break;
+	case V4L2_TUNER_MODE_MONO:
+		if (state->mode == MSP_MODE_AM_NICAM) {
+			dev_dbg_lvl(&client->dev, 1, msp_debug, "switching to AM mono\n");
+			/* AM mono decoding is handled by tuner, not MSP chip */
+			/* SCART switching control register */
+			msp_set_scart(client, SCART_MONO, 0);
+			src = 0x0200;
+			break;
+		}
+		if (state->rxsubchans & V4L2_TUNER_SUB_STEREO)
+			src = 0x0030;
+		break;
+	case V4L2_TUNER_MODE_LANG1:
+		break;
+	case V4L2_TUNER_MODE_LANG2:
+		src |= 0x0010;
+		break;
+	}
+	dev_dbg_lvl(&client->dev, 1, msp_debug,
+		"set_audmode final source/matrix = 0x%x\n", src);
+
+	msp_set_source(client, src);
+}
+
+static void msp3400c_print_mode(struct i2c_client *client)
+{
+	struct msp_state *state = to_state(i2c_get_clientdata(client));
+
+	if (state->main == state->second)
+		dev_dbg_lvl(&client->dev, 1, msp_debug,
+			"mono sound carrier: %d.%03d MHz\n",
+			state->main / 910000, (state->main / 910) % 1000);
+	else
+		dev_dbg_lvl(&client->dev, 1, msp_debug,
+			"main sound carrier: %d.%03d MHz\n",
+			state->main / 910000, (state->main / 910) % 1000);
+	if (state->mode == MSP_MODE_FM_NICAM1 || state->mode == MSP_MODE_FM_NICAM2)
+		dev_dbg_lvl(&client->dev, 1, msp_debug,
+			"NICAM/FM carrier  : %d.%03d MHz\n",
+			state->second / 910000, (state->second/910) % 1000);
+	if (state->mode == MSP_MODE_AM_NICAM)
+		dev_dbg_lvl(&client->dev, 1, msp_debug,
+			"NICAM/AM carrier  : %d.%03d MHz\n",
+			state->second / 910000, (state->second / 910) % 1000);
+	if (state->mode == MSP_MODE_FM_TERRA && state->main != state->second) {
+		dev_dbg_lvl(&client->dev, 1, msp_debug,
+			"FM-stereo carrier : %d.%03d MHz\n",
+			state->second / 910000, (state->second / 910) % 1000);
+	}
+}
+
+/* ----------------------------------------------------------------------- */
+
+static int msp3400c_detect_stereo(struct i2c_client *client)
+{
+	struct msp_state *state = to_state(i2c_get_clientdata(client));
+	int val;
+	int rxsubchans = state->rxsubchans;
+	int newnicam = state->nicam_on;
+	int update = 0;
+
+	switch (state->mode) {
+	case MSP_MODE_FM_TERRA:
+		val = msp_read_dsp(client, 0x18);
+		if (val > 32767)
+			val -= 65536;
+		dev_dbg_lvl(&client->dev, 2, msp_debug,
+			"stereo detect register: %d\n", val);
+		if (val > 8192) {
+			rxsubchans = V4L2_TUNER_SUB_STEREO;
+		} else if (val < -4096) {
+			rxsubchans = V4L2_TUNER_SUB_LANG1 | V4L2_TUNER_SUB_LANG2;
+		} else {
+			rxsubchans = V4L2_TUNER_SUB_MONO;
+		}
+		newnicam = 0;
+		break;
+	case MSP_MODE_FM_NICAM1:
+	case MSP_MODE_FM_NICAM2:
+	case MSP_MODE_AM_NICAM:
+		val = msp_read_dem(client, 0x23);
+		dev_dbg_lvl(&client->dev, 2, msp_debug, "nicam sync=%d, mode=%d\n",
+			val & 1, (val & 0x1e) >> 1);
+
+		if (val & 1) {
+			/* nicam synced */
+			switch ((val & 0x1e) >> 1)  {
+			case 0:
+			case 8:
+				rxsubchans = V4L2_TUNER_SUB_STEREO;
+				break;
+			case 1:
+			case 9:
+				rxsubchans = V4L2_TUNER_SUB_MONO;
+				break;
+			case 2:
+			case 10:
+				rxsubchans = V4L2_TUNER_SUB_LANG1 | V4L2_TUNER_SUB_LANG2;
+				break;
+			default:
+				rxsubchans = V4L2_TUNER_SUB_MONO;
+				break;
+			}
+			newnicam = 1;
+		} else {
+			newnicam = 0;
+			rxsubchans = V4L2_TUNER_SUB_MONO;
+		}
+		break;
+	}
+	if (rxsubchans != state->rxsubchans) {
+		update = 1;
+		dev_dbg_lvl(&client->dev, 1, msp_debug,
+			"watch: rxsubchans %02x => %02x\n",
+			state->rxsubchans, rxsubchans);
+		state->rxsubchans = rxsubchans;
+	}
+	if (newnicam != state->nicam_on) {
+		update = 1;
+		dev_dbg_lvl(&client->dev, 1, msp_debug, "watch: nicam %d => %d\n",
+			state->nicam_on, newnicam);
+		state->nicam_on = newnicam;
+	}
+	return update;
+}
+
+/*
+ * A kernel thread for msp3400 control -- we don't want to block the
+ * in the ioctl while doing the sound carrier & stereo detect
+ */
+/* stereo/multilang monitoring */
+static void watch_stereo(struct i2c_client *client)
+{
+	struct msp_state *state = to_state(i2c_get_clientdata(client));
+
+	if (msp_detect_stereo(client))
+		msp_set_audmode(client);
+
+	if (msp_once)
+		state->watch_stereo = 0;
+}
+
+int msp3400c_thread(void *data)
+{
+	struct i2c_client *client = data;
+	struct msp_state *state = to_state(i2c_get_clientdata(client));
+	struct msp3400c_carrier_detect *cd;
+	int count, max1, max2, val1, val2, val, i;
+
+	dev_dbg_lvl(&client->dev, 1, msp_debug, "msp3400 daemon started\n");
+	state->detected_std = V4L2_STD_ALL;
+	set_freezable();
+	for (;;) {
+		dev_dbg_lvl(&client->dev, 2, msp_debug, "msp3400 thread: sleep\n");
+		msp_sleep(state, -1);
+		dev_dbg_lvl(&client->dev, 2, msp_debug, "msp3400 thread: wakeup\n");
+
+restart:
+		dev_dbg_lvl(&client->dev, 2, msp_debug, "thread: restart scan\n");
+		state->restart = 0;
+		if (kthread_should_stop())
+			break;
+
+		if (state->radio || MSP_MODE_EXTERN == state->mode) {
+			/* no carrier scan, just unmute */
+			dev_dbg_lvl(&client->dev, 1, msp_debug,
+				"thread: no carrier scan\n");
+			state->scan_in_progress = 0;
+			msp_update_volume(state);
+			continue;
+		}
+
+		/* mute audio */
+		state->scan_in_progress = 1;
+		msp_update_volume(state);
+
+		msp3400c_set_mode(client, MSP_MODE_AM_DETECT);
+		val1 = val2 = 0;
+		max1 = max2 = -1;
+		state->watch_stereo = 0;
+		state->nicam_on = 0;
+
+		/* wait for tuner to settle down after a channel change */
+		if (msp_sleep(state, 200))
+			goto restart;
+
+		/* carrier detect pass #1 -- main carrier */
+		cd = msp3400c_carrier_detect_main;
+		count = ARRAY_SIZE(msp3400c_carrier_detect_main);
+
+		if (msp_amsound && (state->v4l2_std & V4L2_STD_SECAM)) {
+			/* autodetect doesn't work well with AM ... */
+			max1 = 3;
+			count = 0;
+			dev_dbg_lvl(&client->dev, 1, msp_debug, "AM sound override\n");
+		}
+
+		for (i = 0; i < count; i++) {
+			msp3400c_set_carrier(client, cd[i].cdo, cd[i].cdo);
+			if (msp_sleep(state, 100))
+				goto restart;
+			val = msp_read_dsp(client, 0x1b);
+			if (val > 32767)
+				val -= 65536;
+			if (val1 < val) {
+				val1 = val;
+				max1 = i;
+			}
+			dev_dbg_lvl(&client->dev, 1, msp_debug,
+				"carrier1 val: %5d / %s\n", val, cd[i].name);
+		}
+
+		/* carrier detect pass #2 -- second (stereo) carrier */
+		switch (max1) {
+		case 1: /* 5.5 */
+			cd = msp3400c_carrier_detect_55;
+			count = ARRAY_SIZE(msp3400c_carrier_detect_55);
+			break;
+		case 3: /* 6.5 */
+			cd = msp3400c_carrier_detect_65;
+			count = ARRAY_SIZE(msp3400c_carrier_detect_65);
+			break;
+		case 0: /* 4.5 */
+		case 2: /* 6.0 */
+		default:
+			cd = NULL;
+			count = 0;
+			break;
+		}
+
+		if (msp_amsound && (state->v4l2_std & V4L2_STD_SECAM)) {
+			/* autodetect doesn't work well with AM ... */
+			cd = NULL;
+			count = 0;
+			max2 = 0;
+		}
+		for (i = 0; i < count; i++) {
+			msp3400c_set_carrier(client, cd[i].cdo, cd[i].cdo);
+			if (msp_sleep(state, 100))
+				goto restart;
+			val = msp_read_dsp(client, 0x1b);
+			if (val > 32767)
+				val -= 65536;
+			if (val2 < val) {
+				val2 = val;
+				max2 = i;
+			}
+			dev_dbg_lvl(&client->dev, 1, msp_debug,
+				"carrier2 val: %5d / %s\n", val, cd[i].name);
+		}
+
+		/* program the msp3400 according to the results */
+		state->main = msp3400c_carrier_detect_main[max1].cdo;
+		switch (max1) {
+		case 1: /* 5.5 */
+			state->detected_std = V4L2_STD_BG | V4L2_STD_PAL_H;
+			if (max2 == 0) {
+				/* B/G FM-stereo */
+				state->second = msp3400c_carrier_detect_55[max2].cdo;
+				msp3400c_set_mode(client, MSP_MODE_FM_TERRA);
+				state->watch_stereo = 1;
+			} else if (max2 == 1 && state->has_nicam) {
+				/* B/G NICAM */
+				state->second = msp3400c_carrier_detect_55[max2].cdo;
+				msp3400c_set_mode(client, MSP_MODE_FM_NICAM1);
+				state->nicam_on = 1;
+				state->watch_stereo = 1;
+			} else {
+				goto no_second;
+			}
+			break;
+		case 2: /* 6.0 */
+			/* PAL I NICAM */
+			state->detected_std = V4L2_STD_PAL_I;
+			state->second = MSP_CARRIER(6.552);
+			msp3400c_set_mode(client, MSP_MODE_FM_NICAM2);
+			state->nicam_on = 1;
+			state->watch_stereo = 1;
+			break;
+		case 3: /* 6.5 */
+			if (max2 == 1 || max2 == 2) {
+				/* D/K FM-stereo */
+				state->second = msp3400c_carrier_detect_65[max2].cdo;
+				msp3400c_set_mode(client, MSP_MODE_FM_TERRA);
+				state->watch_stereo = 1;
+				state->detected_std = V4L2_STD_DK;
+			} else if (max2 == 0 && (state->v4l2_std & V4L2_STD_SECAM)) {
+				/* L NICAM or AM-mono */
+				state->second = msp3400c_carrier_detect_65[max2].cdo;
+				msp3400c_set_mode(client, MSP_MODE_AM_NICAM);
+				state->watch_stereo = 1;
+				state->detected_std = V4L2_STD_L;
+			} else if (max2 == 0 && state->has_nicam) {
+				/* D/K NICAM */
+				state->second = msp3400c_carrier_detect_65[max2].cdo;
+				msp3400c_set_mode(client, MSP_MODE_FM_NICAM1);
+				state->nicam_on = 1;
+				state->watch_stereo = 1;
+				state->detected_std = V4L2_STD_DK;
+			} else {
+				goto no_second;
+			}
+			break;
+		case 0: /* 4.5 */
+			state->detected_std = V4L2_STD_MN;
+			fallthrough;
+		default:
+no_second:
+			state->second = msp3400c_carrier_detect_main[max1].cdo;
+			msp3400c_set_mode(client, MSP_MODE_FM_TERRA);
+			break;
+		}
+		msp3400c_set_carrier(client, state->second, state->main);
+
+		/* unmute */
+		state->scan_in_progress = 0;
+		msp3400c_set_audmode(client);
+		msp_update_volume(state);
+
+		if (msp_debug)
+			msp3400c_print_mode(client);
+
+		/* monitor tv audio mode, the first time don't wait
+		   so long to get a quick stereo/bilingual result */
+		count = 3;
+		while (state->watch_stereo) {
+			if (msp_sleep(state, count ? 1000 : 5000))
+				goto restart;
+			if (count)
+				count--;
+			watch_stereo(client);
+		}
+	}
+	dev_dbg_lvl(&client->dev, 1, msp_debug, "thread: exit\n");
+	return 0;
+}
+
+
+int msp3410d_thread(void *data)
+{
+	struct i2c_client *client = data;
+	struct msp_state *state = to_state(i2c_get_clientdata(client));
+	int val, i, std, count;
+
+	dev_dbg_lvl(&client->dev, 1, msp_debug, "msp3410 daemon started\n");
+	state->detected_std = V4L2_STD_ALL;
+	set_freezable();
+	for (;;) {
+		dev_dbg_lvl(&client->dev, 2, msp_debug, "msp3410 thread: sleep\n");
+		msp_sleep(state, -1);
+		dev_dbg_lvl(&client->dev, 2, msp_debug, "msp3410 thread: wakeup\n");
+
+restart:
+		dev_dbg_lvl(&client->dev, 2, msp_debug, "thread: restart scan\n");
+		state->restart = 0;
+		if (kthread_should_stop())
+			break;
+
+		if (state->mode == MSP_MODE_EXTERN) {
+			/* no carrier scan needed, just unmute */
+			dev_dbg_lvl(&client->dev, 1, msp_debug,
+				"thread: no carrier scan\n");
+			state->scan_in_progress = 0;
+			msp_update_volume(state);
+			continue;
+		}
+
+		/* mute audio */
+		state->scan_in_progress = 1;
+		msp_update_volume(state);
+
+		/* start autodetect. Note: autodetect is not supported for
+		   NTSC-M and radio, hence we force the standard in those
+		   cases. */
+		if (state->radio)
+			std = 0x40;
+		else
+			std = (state->v4l2_std & V4L2_STD_NTSC) ? 0x20 : 1;
+		state->watch_stereo = 0;
+		state->nicam_on = 0;
+
+		/* wait for tuner to settle down after a channel change */
+		if (msp_sleep(state, 200))
+			goto restart;
+
+		if (msp_debug)
+			dev_dbg_lvl(&client->dev, 2, msp_debug,
+				"setting standard: %s (0x%04x)\n",
+				msp_standard_std_name(std), std);
+
+		if (std != 1) {
+			/* programmed some specific mode */
+			val = std;
+		} else {
+			/* triggered autodetect */
+			msp_write_dem(client, 0x20, std);
+			for (;;) {
+				if (msp_sleep(state, 100))
+					goto restart;
+
+				/* check results */
+				val = msp_read_dem(client, 0x7e);
+				if (val < 0x07ff)
+					break;
+				dev_dbg_lvl(&client->dev, 2, msp_debug,
+					"detection still in progress\n");
+			}
+		}
+		for (i = 0; msp_stdlist[i].name != NULL; i++)
+			if (msp_stdlist[i].retval == val)
+				break;
+		dev_dbg_lvl(&client->dev, 1, msp_debug, "current standard: %s (0x%04x)\n",
+			msp_standard_std_name(val), val);
+		state->main   = msp_stdlist[i].main;
+		state->second = msp_stdlist[i].second;
+		state->std = val;
+		state->rxsubchans = V4L2_TUNER_SUB_MONO;
+
+		if (msp_amsound && !state->radio &&
+		    (state->v4l2_std & V4L2_STD_SECAM) && (val != 0x0009)) {
+			/* autodetection has failed, let backup */
+			dev_dbg_lvl(&client->dev, 1, msp_debug, "autodetection failed, switching to backup standard: %s (0x%04x)\n",
+				msp_stdlist[8].name ?
+					msp_stdlist[8].name : "unknown", val);
+			state->std = val = 0x0009;
+			msp_write_dem(client, 0x20, val);
+		} else {
+			state->detected_std = msp_standard_std(state->std);
+		}
+
+		/* set stereo */
+		switch (val) {
+		case 0x0008: /* B/G NICAM */
+		case 0x000a: /* I NICAM */
+		case 0x000b: /* D/K NICAM */
+			if (val == 0x000a)
+				state->mode = MSP_MODE_FM_NICAM2;
+			else
+				state->mode = MSP_MODE_FM_NICAM1;
+			/* just turn on stereo */
+			state->nicam_on = 1;
+			state->watch_stereo = 1;
+			break;
+		case 0x0009:
+			state->mode = MSP_MODE_AM_NICAM;
+			state->nicam_on = 1;
+			state->watch_stereo = 1;
+			break;
+		case 0x0020: /* BTSC */
+			/* The pre-'G' models only have BTSC-mono */
+			state->mode = MSP_MODE_BTSC;
+			break;
+		case 0x0040: /* FM radio */
+			state->mode = MSP_MODE_FM_RADIO;
+			state->rxsubchans = V4L2_TUNER_SUB_STEREO;
+			/* not needed in theory if we have radio, but
+			   short programming enables carrier mute */
+			msp3400c_set_mode(client, MSP_MODE_FM_RADIO);
+			msp3400c_set_carrier(client, MSP_CARRIER(10.7),
+					    MSP_CARRIER(10.7));
+			break;
+		case 0x0002:
+		case 0x0003:
+		case 0x0004:
+		case 0x0005:
+			state->mode = MSP_MODE_FM_TERRA;
+			state->watch_stereo = 1;
+			break;
+		}
+
+		/* set various prescales */
+		msp_write_dsp(client, 0x0d, 0x1900); /* scart */
+		msp_write_dsp(client, 0x0e, 0x3000); /* FM */
+		if (state->has_nicam)
+			msp_write_dsp(client, 0x10, 0x5a00); /* nicam */
+
+		if (state->has_i2s_conf)
+			msp_write_dem(client, 0x40, state->i2s_mode);
+
+		/* unmute */
+		msp3400c_set_audmode(client);
+		state->scan_in_progress = 0;
+		msp_update_volume(state);
+
+		/* monitor tv audio mode, the first time don't wait
+		   so long to get a quick stereo/bilingual result */
+		count = 3;
+		while (state->watch_stereo) {
+			if (msp_sleep(state, count ? 1000 : 5000))
+				goto restart;
+			if (count)
+				count--;
+			watch_stereo(client);
+		}
+	}
+	dev_dbg_lvl(&client->dev, 1, msp_debug, "thread: exit\n");
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+/* msp34xxG + (autoselect no-thread)
+ * this one uses both automatic standard detection and automatic sound
+ * select which are available in the newer G versions
+ * struct msp: only norm, acb and source are really used in this mode
+ */
+
+static int msp34xxg_modus(struct i2c_client *client)
+{
+	struct msp_state *state = to_state(i2c_get_clientdata(client));
+
+	if (state->radio) {
+		dev_dbg_lvl(&client->dev, 1, msp_debug, "selected radio modus\n");
+		return 0x0001;
+	}
+	if (state->v4l2_std == V4L2_STD_NTSC_M_JP) {
+		dev_dbg_lvl(&client->dev, 1, msp_debug, "selected M (EIA-J) modus\n");
+		return 0x4001;
+	}
+	if (state->v4l2_std == V4L2_STD_NTSC_M_KR) {
+		dev_dbg_lvl(&client->dev, 1, msp_debug, "selected M (A2) modus\n");
+		return 0x0001;
+	}
+	if (state->v4l2_std == V4L2_STD_SECAM_L) {
+		dev_dbg_lvl(&client->dev, 1, msp_debug, "selected SECAM-L modus\n");
+		return 0x6001;
+	}
+	if (state->v4l2_std & V4L2_STD_MN) {
+		dev_dbg_lvl(&client->dev, 1, msp_debug, "selected M (BTSC) modus\n");
+		return 0x2001;
+	}
+	return 0x7001;
+}
+
+static void msp34xxg_set_source(struct i2c_client *client, u16 reg, int in)
+{
+	struct msp_state *state = to_state(i2c_get_clientdata(client));
+	int source, matrix;
+
+	switch (state->audmode) {
+	case V4L2_TUNER_MODE_MONO:
+		source = 0; /* mono only */
+		matrix = 0x30;
+		break;
+	case V4L2_TUNER_MODE_LANG2:
+		source = 4; /* stereo or B */
+		matrix = 0x10;
+		break;
+	case V4L2_TUNER_MODE_LANG1_LANG2:
+		source = 1; /* stereo or A|B */
+		matrix = 0x20;
+		break;
+	case V4L2_TUNER_MODE_LANG1:
+		source = 3; /* stereo or A */
+		matrix = 0x00;
+		break;
+	case V4L2_TUNER_MODE_STEREO:
+	default:
+		source = 3; /* stereo or A */
+		matrix = 0x20;
+		break;
+	}
+
+	if (in == MSP_DSP_IN_TUNER)
+		source = (source << 8) | 0x20;
+	/* the msp34x2g puts the MAIN_AVC, MAIN and AUX sources in 12, 13, 14
+	   instead of 11, 12, 13. So we add one for that msp version. */
+	else if (in >= MSP_DSP_IN_MAIN_AVC && state->has_dolby_pro_logic)
+		source = ((in + 1) << 8) | matrix;
+	else
+		source = (in << 8) | matrix;
+
+	dev_dbg_lvl(&client->dev, 1, msp_debug,
+		"set source to %d (0x%x) for output %02x\n", in, source, reg);
+	msp_write_dsp(client, reg, source);
+}
+
+static void msp34xxg_set_sources(struct i2c_client *client)
+{
+	struct msp_state *state = to_state(i2c_get_clientdata(client));
+	u32 in = state->route_in;
+
+	msp34xxg_set_source(client, 0x0008, (in >> 4) & 0xf);
+	/* quasi-peak detector is set to same input as the loudspeaker (MAIN) */
+	msp34xxg_set_source(client, 0x000c, (in >> 4) & 0xf);
+	msp34xxg_set_source(client, 0x0009, (in >> 8) & 0xf);
+	msp34xxg_set_source(client, 0x000a, (in >> 12) & 0xf);
+	if (state->has_scart2_out)
+		msp34xxg_set_source(client, 0x0041, (in >> 16) & 0xf);
+	msp34xxg_set_source(client, 0x000b, (in >> 20) & 0xf);
+}
+
+/* (re-)initialize the msp34xxg */
+static void msp34xxg_reset(struct i2c_client *client)
+{
+	struct msp_state *state = to_state(i2c_get_clientdata(client));
+	int tuner = (state->route_in >> 3) & 1;
+	int modus;
+
+	/* initialize std to 1 (autodetect) to signal that no standard is
+	   selected yet. */
+	state->std = 1;
+
+	msp_reset(client);
+
+	if (state->has_i2s_conf)
+		msp_write_dem(client, 0x40, state->i2s_mode);
+
+	/* step-by-step initialisation, as described in the manual */
+	modus = msp34xxg_modus(client);
+	modus |= tuner ? 0x100 : 0;
+	msp_write_dem(client, 0x30, modus);
+
+	/* write the dsps that may have an influence on
+	   standard/audio autodetection right now */
+	msp34xxg_set_sources(client);
+
+	msp_write_dsp(client, 0x0d, 0x1900); /* scart */
+	msp_write_dsp(client, 0x0e, 0x3000); /* FM */
+	if (state->has_nicam)
+		msp_write_dsp(client, 0x10, 0x5a00); /* nicam */
+
+	/* set identification threshold. Personally, I
+	 * I set it to a higher value than the default
+	 * of 0x190 to ignore noisy stereo signals.
+	 * this needs tuning. (recommended range 0x00a0-0x03c0)
+	 * 0x7f0 = forced mono mode
+	 *
+	 * a2 threshold for stereo/bilingual.
+	 * Note: this register is part of the Manual/Compatibility mode.
+	 * It is supported by all 'G'-family chips.
+	 */
+	msp_write_dem(client, 0x22, msp_stereo_thresh);
+}
+
+int msp34xxg_thread(void *data)
+{
+	struct i2c_client *client = data;
+	struct msp_state *state = to_state(i2c_get_clientdata(client));
+	int val, i;
+
+	dev_dbg_lvl(&client->dev, 1, msp_debug, "msp34xxg daemon started\n");
+	state->detected_std = V4L2_STD_ALL;
+	set_freezable();
+	for (;;) {
+		dev_dbg_lvl(&client->dev, 2, msp_debug, "msp34xxg thread: sleep\n");
+		msp_sleep(state, -1);
+		dev_dbg_lvl(&client->dev, 2, msp_debug, "msp34xxg thread: wakeup\n");
+
+restart:
+		dev_dbg_lvl(&client->dev, 1, msp_debug, "thread: restart scan\n");
+		state->restart = 0;
+		if (kthread_should_stop())
+			break;
+
+		if (state->mode == MSP_MODE_EXTERN) {
+			/* no carrier scan needed, just unmute */
+			dev_dbg_lvl(&client->dev, 1, msp_debug,
+				"thread: no carrier scan\n");
+			state->scan_in_progress = 0;
+			msp_update_volume(state);
+			continue;
+		}
+
+		/* setup the chip*/
+		msp34xxg_reset(client);
+		state->std = state->radio ? 0x40 :
+			(state->force_btsc && msp_standard == 1) ? 32 : msp_standard;
+		msp_write_dem(client, 0x20, state->std);
+		/* start autodetect */
+		if (state->std != 1)
+			goto unmute;
+
+		/* watch autodetect */
+		dev_dbg_lvl(&client->dev, 1, msp_debug,
+			"started autodetect, waiting for result\n");
+		for (i = 0; i < 10; i++) {
+			if (msp_sleep(state, 100))
+				goto restart;
+
+			/* check results */
+			val = msp_read_dem(client, 0x7e);
+			if (val < 0x07ff) {
+				state->std = val;
+				break;
+			}
+			dev_dbg_lvl(&client->dev, 2, msp_debug,
+				"detection still in progress\n");
+		}
+		if (state->std == 1) {
+			dev_dbg_lvl(&client->dev, 1, msp_debug,
+				"detection still in progress after 10 tries. giving up.\n");
+			continue;
+		}
+
+unmute:
+		dev_dbg_lvl(&client->dev, 1, msp_debug,
+			"detected standard: %s (0x%04x)\n",
+			msp_standard_std_name(state->std), state->std);
+		state->detected_std = msp_standard_std(state->std);
+
+		if (state->std == 9) {
+			/* AM NICAM mode */
+			msp_write_dsp(client, 0x0e, 0x7c00);
+		}
+
+		/* unmute: dispatch sound to scart output, set scart volume */
+		msp_update_volume(state);
+
+		/* restore ACB */
+		if (msp_write_dsp(client, 0x13, state->acb))
+			return -1;
+
+		/* the periodic stereo/SAP check is only relevant for
+		   the 0x20 standard (BTSC) */
+		if (state->std != 0x20)
+			continue;
+
+		state->watch_stereo = 1;
+
+		/* monitor tv audio mode, the first time don't wait
+		   in order to get a quick stereo/SAP update */
+		watch_stereo(client);
+		while (state->watch_stereo) {
+			watch_stereo(client);
+			if (msp_sleep(state, 5000))
+				goto restart;
+		}
+	}
+	dev_dbg_lvl(&client->dev, 1, msp_debug, "thread: exit\n");
+	return 0;
+}
+
+static int msp34xxg_detect_stereo(struct i2c_client *client)
+{
+	struct msp_state *state = to_state(i2c_get_clientdata(client));
+	int status = msp_read_dem(client, 0x0200);
+	int is_bilingual = status & 0x100;
+	int is_stereo = status & 0x40;
+	int oldrx = state->rxsubchans;
+
+	if (state->mode == MSP_MODE_EXTERN)
+		return 0;
+
+	state->rxsubchans = 0;
+	if (is_stereo)
+		state->rxsubchans = V4L2_TUNER_SUB_STEREO;
+	else
+		state->rxsubchans = V4L2_TUNER_SUB_MONO;
+	if (is_bilingual) {
+		if (state->std == 0x20)
+			state->rxsubchans |= V4L2_TUNER_SUB_SAP;
+		else
+			state->rxsubchans =
+				V4L2_TUNER_SUB_LANG1 | V4L2_TUNER_SUB_LANG2;
+	}
+	dev_dbg_lvl(&client->dev, 1, msp_debug,
+		"status=0x%x, stereo=%d, bilingual=%d -> rxsubchans=%d\n",
+		status, is_stereo, is_bilingual, state->rxsubchans);
+	return (oldrx != state->rxsubchans);
+}
+
+static void msp34xxg_set_audmode(struct i2c_client *client)
+{
+	struct msp_state *state = to_state(i2c_get_clientdata(client));
+
+	if (state->std == 0x20) {
+	       if ((state->rxsubchans & V4L2_TUNER_SUB_SAP) &&
+		   (state->audmode == V4L2_TUNER_MODE_LANG1_LANG2 ||
+		    state->audmode == V4L2_TUNER_MODE_LANG2)) {
+			msp_write_dem(client, 0x20, 0x21);
+	       } else {
+			msp_write_dem(client, 0x20, 0x20);
+	       }
+	}
+
+	msp34xxg_set_sources(client);
+}
+
+void msp_set_audmode(struct i2c_client *client)
+{
+	struct msp_state *state = to_state(i2c_get_clientdata(client));
+
+	switch (state->opmode) {
+	case OPMODE_MANUAL:
+	case OPMODE_AUTODETECT:
+		msp3400c_set_audmode(client);
+		break;
+	case OPMODE_AUTOSELECT:
+		msp34xxg_set_audmode(client);
+		break;
+	}
+}
+
+int msp_detect_stereo(struct i2c_client *client)
+{
+	struct msp_state *state  = to_state(i2c_get_clientdata(client));
+
+	switch (state->opmode) {
+	case OPMODE_MANUAL:
+	case OPMODE_AUTODETECT:
+		return msp3400c_detect_stereo(client);
+	case OPMODE_AUTOSELECT:
+		return msp34xxg_detect_stereo(client);
+	}
+	return 0;
+}
+
diff --git a/drivers/media/i2c/mt9m001.c b/drivers/media/i2c/mt9m001.c
new file mode 100644
index 000000000..ce9568e83
--- /dev/null
+++ b/drivers/media/i2c/mt9m001.c
@@ -0,0 +1,889 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Driver for MT9M001 CMOS Image Sensor from Micron
+ *
+ * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/log2.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-subdev.h>
+
+/*
+ * mt9m001 i2c address 0x5d
+ */
+
+/* mt9m001 selected register addresses */
+#define MT9M001_CHIP_VERSION		0x00
+#define MT9M001_ROW_START		0x01
+#define MT9M001_COLUMN_START		0x02
+#define MT9M001_WINDOW_HEIGHT		0x03
+#define MT9M001_WINDOW_WIDTH		0x04
+#define MT9M001_HORIZONTAL_BLANKING	0x05
+#define MT9M001_VERTICAL_BLANKING	0x06
+#define MT9M001_OUTPUT_CONTROL		0x07
+#define MT9M001_SHUTTER_WIDTH		0x09
+#define MT9M001_FRAME_RESTART		0x0b
+#define MT9M001_SHUTTER_DELAY		0x0c
+#define MT9M001_RESET			0x0d
+#define MT9M001_READ_OPTIONS1		0x1e
+#define MT9M001_READ_OPTIONS2		0x20
+#define MT9M001_GLOBAL_GAIN		0x35
+#define MT9M001_CHIP_ENABLE		0xF1
+
+#define MT9M001_MAX_WIDTH		1280
+#define MT9M001_MAX_HEIGHT		1024
+#define MT9M001_MIN_WIDTH		48
+#define MT9M001_MIN_HEIGHT		32
+#define MT9M001_COLUMN_SKIP		20
+#define MT9M001_ROW_SKIP		12
+#define MT9M001_DEFAULT_HBLANK		9
+#define MT9M001_DEFAULT_VBLANK		25
+
+/* MT9M001 has only one fixed colorspace per pixelcode */
+struct mt9m001_datafmt {
+	u32	code;
+	enum v4l2_colorspace		colorspace;
+};
+
+/* Find a data format by a pixel code in an array */
+static const struct mt9m001_datafmt *mt9m001_find_datafmt(
+	u32 code, const struct mt9m001_datafmt *fmt,
+	int n)
+{
+	int i;
+	for (i = 0; i < n; i++)
+		if (fmt[i].code == code)
+			return fmt + i;
+
+	return NULL;
+}
+
+static const struct mt9m001_datafmt mt9m001_colour_fmts[] = {
+	/*
+	 * Order important: first natively supported,
+	 * second supported with a GPIO extender
+	 */
+	{MEDIA_BUS_FMT_SBGGR10_1X10, V4L2_COLORSPACE_SRGB},
+	{MEDIA_BUS_FMT_SBGGR8_1X8, V4L2_COLORSPACE_SRGB},
+};
+
+static const struct mt9m001_datafmt mt9m001_monochrome_fmts[] = {
+	/* Order important - see above */
+	{MEDIA_BUS_FMT_Y10_1X10, V4L2_COLORSPACE_JPEG},
+	{MEDIA_BUS_FMT_Y8_1X8, V4L2_COLORSPACE_JPEG},
+};
+
+struct mt9m001 {
+	struct v4l2_subdev subdev;
+	struct v4l2_ctrl_handler hdl;
+	struct {
+		/* exposure/auto-exposure cluster */
+		struct v4l2_ctrl *autoexposure;
+		struct v4l2_ctrl *exposure;
+	};
+	bool streaming;
+	struct mutex mutex;
+	struct v4l2_rect rect;	/* Sensor window */
+	struct clk *clk;
+	struct gpio_desc *standby_gpio;
+	struct gpio_desc *reset_gpio;
+	const struct mt9m001_datafmt *fmt;
+	const struct mt9m001_datafmt *fmts;
+	int num_fmts;
+	unsigned int total_h;
+	unsigned short y_skip_top;	/* Lines to skip at the top */
+	struct media_pad pad;
+};
+
+static struct mt9m001 *to_mt9m001(const struct i2c_client *client)
+{
+	return container_of(i2c_get_clientdata(client), struct mt9m001, subdev);
+}
+
+static int reg_read(struct i2c_client *client, const u8 reg)
+{
+	return i2c_smbus_read_word_swapped(client, reg);
+}
+
+static int reg_write(struct i2c_client *client, const u8 reg,
+		     const u16 data)
+{
+	return i2c_smbus_write_word_swapped(client, reg, data);
+}
+
+static int reg_set(struct i2c_client *client, const u8 reg,
+		   const u16 data)
+{
+	int ret;
+
+	ret = reg_read(client, reg);
+	if (ret < 0)
+		return ret;
+	return reg_write(client, reg, ret | data);
+}
+
+static int reg_clear(struct i2c_client *client, const u8 reg,
+		     const u16 data)
+{
+	int ret;
+
+	ret = reg_read(client, reg);
+	if (ret < 0)
+		return ret;
+	return reg_write(client, reg, ret & ~data);
+}
+
+struct mt9m001_reg {
+	u8 reg;
+	u16 data;
+};
+
+static int multi_reg_write(struct i2c_client *client,
+			   const struct mt9m001_reg *regs, int num)
+{
+	int i;
+
+	for (i = 0; i < num; i++) {
+		int ret = reg_write(client, regs[i].reg, regs[i].data);
+
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int mt9m001_init(struct i2c_client *client)
+{
+	static const struct mt9m001_reg init_regs[] = {
+		/*
+		 * Issue a soft reset. This returns all registers to their
+		 * default values.
+		 */
+		{ MT9M001_RESET, 1 },
+		{ MT9M001_RESET, 0 },
+		/* Disable chip, synchronous option update */
+		{ MT9M001_OUTPUT_CONTROL, 0 }
+	};
+
+	dev_dbg(&client->dev, "%s\n", __func__);
+
+	return multi_reg_write(client, init_regs, ARRAY_SIZE(init_regs));
+}
+
+static int mt9m001_apply_selection(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9m001 *mt9m001 = to_mt9m001(client);
+	const struct mt9m001_reg regs[] = {
+		/* Blanking and start values - default... */
+		{ MT9M001_HORIZONTAL_BLANKING, MT9M001_DEFAULT_HBLANK },
+		{ MT9M001_VERTICAL_BLANKING, MT9M001_DEFAULT_VBLANK },
+		/*
+		 * The caller provides a supported format, as verified per
+		 * call to .set_fmt(FORMAT_TRY).
+		 */
+		{ MT9M001_COLUMN_START, mt9m001->rect.left },
+		{ MT9M001_ROW_START, mt9m001->rect.top },
+		{ MT9M001_WINDOW_WIDTH, mt9m001->rect.width - 1 },
+		{ MT9M001_WINDOW_HEIGHT,
+			mt9m001->rect.height + mt9m001->y_skip_top - 1 },
+	};
+
+	return multi_reg_write(client, regs, ARRAY_SIZE(regs));
+}
+
+static int mt9m001_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9m001 *mt9m001 = to_mt9m001(client);
+	int ret = 0;
+
+	mutex_lock(&mt9m001->mutex);
+
+	if (mt9m001->streaming == enable)
+		goto done;
+
+	if (enable) {
+		ret = pm_runtime_resume_and_get(&client->dev);
+		if (ret < 0)
+			goto unlock;
+
+		ret = mt9m001_apply_selection(sd);
+		if (ret)
+			goto put_unlock;
+
+		ret = __v4l2_ctrl_handler_setup(&mt9m001->hdl);
+		if (ret)
+			goto put_unlock;
+
+		/* Switch to master "normal" mode */
+		ret = reg_write(client, MT9M001_OUTPUT_CONTROL, 2);
+		if (ret < 0)
+			goto put_unlock;
+	} else {
+		/* Switch to master stop sensor readout */
+		reg_write(client, MT9M001_OUTPUT_CONTROL, 0);
+		pm_runtime_put(&client->dev);
+	}
+
+	mt9m001->streaming = enable;
+done:
+	mutex_unlock(&mt9m001->mutex);
+
+	return 0;
+
+put_unlock:
+	pm_runtime_put(&client->dev);
+unlock:
+	mutex_unlock(&mt9m001->mutex);
+
+	return ret;
+}
+
+static int mt9m001_set_selection(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_selection *sel)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9m001 *mt9m001 = to_mt9m001(client);
+	struct v4l2_rect rect = sel->r;
+
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE ||
+	    sel->target != V4L2_SEL_TGT_CROP)
+		return -EINVAL;
+
+	if (mt9m001->fmts == mt9m001_colour_fmts)
+		/*
+		 * Bayer format - even number of rows for simplicity,
+		 * but let the user play with the top row.
+		 */
+		rect.height = ALIGN(rect.height, 2);
+
+	/* Datasheet requirement: see register description */
+	rect.width = ALIGN(rect.width, 2);
+	rect.left = ALIGN(rect.left, 2);
+
+	rect.width = clamp_t(u32, rect.width, MT9M001_MIN_WIDTH,
+			MT9M001_MAX_WIDTH);
+	rect.left = clamp_t(u32, rect.left, MT9M001_COLUMN_SKIP,
+			MT9M001_COLUMN_SKIP + MT9M001_MAX_WIDTH - rect.width);
+
+	rect.height = clamp_t(u32, rect.height, MT9M001_MIN_HEIGHT,
+			MT9M001_MAX_HEIGHT);
+	rect.top = clamp_t(u32, rect.top, MT9M001_ROW_SKIP,
+			MT9M001_ROW_SKIP + MT9M001_MAX_HEIGHT - rect.height);
+
+	mt9m001->total_h = rect.height + mt9m001->y_skip_top +
+			   MT9M001_DEFAULT_VBLANK;
+
+	mt9m001->rect = rect;
+
+	return 0;
+}
+
+static int mt9m001_get_selection(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_selection *sel)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9m001 *mt9m001 = to_mt9m001(client);
+
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
+		return -EINVAL;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.left = MT9M001_COLUMN_SKIP;
+		sel->r.top = MT9M001_ROW_SKIP;
+		sel->r.width = MT9M001_MAX_WIDTH;
+		sel->r.height = MT9M001_MAX_HEIGHT;
+		return 0;
+	case V4L2_SEL_TGT_CROP:
+		sel->r = mt9m001->rect;
+		return 0;
+	default:
+		return -EINVAL;
+	}
+}
+
+static int mt9m001_get_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_format *format)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9m001 *mt9m001 = to_mt9m001(client);
+	struct v4l2_mbus_framefmt *mf = &format->format;
+
+	if (format->pad)
+		return -EINVAL;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		mf = v4l2_subdev_get_try_format(sd, sd_state, 0);
+		format->format = *mf;
+		return 0;
+	}
+
+	mf->width	= mt9m001->rect.width;
+	mf->height	= mt9m001->rect.height;
+	mf->code	= mt9m001->fmt->code;
+	mf->colorspace	= mt9m001->fmt->colorspace;
+	mf->field	= V4L2_FIELD_NONE;
+	mf->ycbcr_enc	= V4L2_YCBCR_ENC_DEFAULT;
+	mf->quantization = V4L2_QUANTIZATION_DEFAULT;
+	mf->xfer_func	= V4L2_XFER_FUNC_DEFAULT;
+
+	return 0;
+}
+
+static int mt9m001_s_fmt(struct v4l2_subdev *sd,
+			 const struct mt9m001_datafmt *fmt,
+			 struct v4l2_mbus_framefmt *mf)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9m001 *mt9m001 = to_mt9m001(client);
+	struct v4l2_subdev_selection sel = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.target = V4L2_SEL_TGT_CROP,
+		.r.left = mt9m001->rect.left,
+		.r.top = mt9m001->rect.top,
+		.r.width = mf->width,
+		.r.height = mf->height,
+	};
+	int ret;
+
+	/* No support for scaling so far, just crop. TODO: use skipping */
+	ret = mt9m001_set_selection(sd, NULL, &sel);
+	if (!ret) {
+		mf->width	= mt9m001->rect.width;
+		mf->height	= mt9m001->rect.height;
+		mt9m001->fmt	= fmt;
+		mf->colorspace	= fmt->colorspace;
+	}
+
+	return ret;
+}
+
+static int mt9m001_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *mf = &format->format;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9m001 *mt9m001 = to_mt9m001(client);
+	const struct mt9m001_datafmt *fmt;
+
+	if (format->pad)
+		return -EINVAL;
+
+	v4l_bound_align_image(&mf->width, MT9M001_MIN_WIDTH,
+		MT9M001_MAX_WIDTH, 1,
+		&mf->height, MT9M001_MIN_HEIGHT + mt9m001->y_skip_top,
+		MT9M001_MAX_HEIGHT + mt9m001->y_skip_top, 0, 0);
+
+	if (mt9m001->fmts == mt9m001_colour_fmts)
+		mf->height = ALIGN(mf->height - 1, 2);
+
+	fmt = mt9m001_find_datafmt(mf->code, mt9m001->fmts,
+				   mt9m001->num_fmts);
+	if (!fmt) {
+		fmt = mt9m001->fmt;
+		mf->code = fmt->code;
+	}
+
+	mf->colorspace	= fmt->colorspace;
+	mf->field	= V4L2_FIELD_NONE;
+	mf->ycbcr_enc	= V4L2_YCBCR_ENC_DEFAULT;
+	mf->quantization = V4L2_QUANTIZATION_DEFAULT;
+	mf->xfer_func	= V4L2_XFER_FUNC_DEFAULT;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		return mt9m001_s_fmt(sd, fmt, mf);
+	sd_state->pads->try_fmt = *mf;
+	return 0;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int mt9m001_g_register(struct v4l2_subdev *sd,
+			      struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (reg->reg > 0xff)
+		return -EINVAL;
+
+	reg->size = 2;
+	reg->val = reg_read(client, reg->reg);
+
+	if (reg->val > 0xffff)
+		return -EIO;
+
+	return 0;
+}
+
+static int mt9m001_s_register(struct v4l2_subdev *sd,
+			      const struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (reg->reg > 0xff)
+		return -EINVAL;
+
+	if (reg_write(client, reg->reg, reg->val) < 0)
+		return -EIO;
+
+	return 0;
+}
+#endif
+
+static int mt9m001_power_on(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct mt9m001 *mt9m001 = to_mt9m001(client);
+	int ret;
+
+	ret = clk_prepare_enable(mt9m001->clk);
+	if (ret)
+		return ret;
+
+	if (mt9m001->standby_gpio) {
+		gpiod_set_value_cansleep(mt9m001->standby_gpio, 0);
+		usleep_range(1000, 2000);
+	}
+
+	if (mt9m001->reset_gpio) {
+		gpiod_set_value_cansleep(mt9m001->reset_gpio, 1);
+		usleep_range(1000, 2000);
+		gpiod_set_value_cansleep(mt9m001->reset_gpio, 0);
+		usleep_range(1000, 2000);
+	}
+
+	return 0;
+}
+
+static int mt9m001_power_off(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct mt9m001 *mt9m001 = to_mt9m001(client);
+
+	gpiod_set_value_cansleep(mt9m001->standby_gpio, 1);
+	clk_disable_unprepare(mt9m001->clk);
+
+	return 0;
+}
+
+static int mt9m001_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct mt9m001 *mt9m001 = container_of(ctrl->handler,
+					       struct mt9m001, hdl);
+	s32 min, max;
+
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE_AUTO:
+		min = mt9m001->exposure->minimum;
+		max = mt9m001->exposure->maximum;
+		mt9m001->exposure->val =
+			(524 + (mt9m001->total_h - 1) * (max - min)) / 1048 + min;
+		break;
+	}
+	return 0;
+}
+
+static int mt9m001_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct mt9m001 *mt9m001 = container_of(ctrl->handler,
+					       struct mt9m001, hdl);
+	struct v4l2_subdev *sd = &mt9m001->subdev;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct v4l2_ctrl *exp = mt9m001->exposure;
+	int data;
+	int ret;
+
+	if (!pm_runtime_get_if_in_use(&client->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_VFLIP:
+		if (ctrl->val)
+			ret = reg_set(client, MT9M001_READ_OPTIONS2, 0x8000);
+		else
+			ret = reg_clear(client, MT9M001_READ_OPTIONS2, 0x8000);
+		break;
+
+	case V4L2_CID_GAIN:
+		/* See Datasheet Table 7, Gain settings. */
+		if (ctrl->val <= ctrl->default_value) {
+			/* Pack it into 0..1 step 0.125, register values 0..8 */
+			unsigned long range = ctrl->default_value - ctrl->minimum;
+			data = ((ctrl->val - (s32)ctrl->minimum) * 8 + range / 2) / range;
+
+			dev_dbg(&client->dev, "Setting gain %d\n", data);
+			ret = reg_write(client, MT9M001_GLOBAL_GAIN, data);
+		} else {
+			/* Pack it into 1.125..15 variable step, register values 9..67 */
+			/* We assume qctrl->maximum - qctrl->default_value - 1 > 0 */
+			unsigned long range = ctrl->maximum - ctrl->default_value - 1;
+			unsigned long gain = ((ctrl->val - (s32)ctrl->default_value - 1) *
+					       111 + range / 2) / range + 9;
+
+			if (gain <= 32)
+				data = gain;
+			else if (gain <= 64)
+				data = ((gain - 32) * 16 + 16) / 32 + 80;
+			else
+				data = ((gain - 64) * 7 + 28) / 56 + 96;
+
+			dev_dbg(&client->dev, "Setting gain from %d to %d\n",
+				 reg_read(client, MT9M001_GLOBAL_GAIN), data);
+			ret = reg_write(client, MT9M001_GLOBAL_GAIN, data);
+		}
+		break;
+
+	case V4L2_CID_EXPOSURE_AUTO:
+		if (ctrl->val == V4L2_EXPOSURE_MANUAL) {
+			unsigned long range = exp->maximum - exp->minimum;
+			unsigned long shutter = ((exp->val - (s32)exp->minimum) * 1048 +
+						 range / 2) / range + 1;
+
+			dev_dbg(&client->dev,
+				"Setting shutter width from %d to %lu\n",
+				reg_read(client, MT9M001_SHUTTER_WIDTH), shutter);
+			ret = reg_write(client, MT9M001_SHUTTER_WIDTH, shutter);
+		} else {
+			mt9m001->total_h = mt9m001->rect.height +
+				mt9m001->y_skip_top + MT9M001_DEFAULT_VBLANK;
+			ret = reg_write(client, MT9M001_SHUTTER_WIDTH,
+					mt9m001->total_h);
+		}
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	pm_runtime_put(&client->dev);
+
+	return ret;
+}
+
+/*
+ * Interface active, can use i2c. If it fails, it can indeed mean, that
+ * this wasn't our capture interface, so, we wait for the right one
+ */
+static int mt9m001_video_probe(struct i2c_client *client)
+{
+	struct mt9m001 *mt9m001 = to_mt9m001(client);
+	s32 data;
+	int ret;
+
+	/* Enable the chip */
+	data = reg_write(client, MT9M001_CHIP_ENABLE, 1);
+	dev_dbg(&client->dev, "write: %d\n", data);
+
+	/* Read out the chip version register */
+	data = reg_read(client, MT9M001_CHIP_VERSION);
+
+	/* must be 0x8411 or 0x8421 for colour sensor and 8431 for bw */
+	switch (data) {
+	case 0x8411:
+	case 0x8421:
+		mt9m001->fmts = mt9m001_colour_fmts;
+		mt9m001->num_fmts = ARRAY_SIZE(mt9m001_colour_fmts);
+		break;
+	case 0x8431:
+		mt9m001->fmts = mt9m001_monochrome_fmts;
+		mt9m001->num_fmts = ARRAY_SIZE(mt9m001_monochrome_fmts);
+		break;
+	default:
+		dev_err(&client->dev,
+			"No MT9M001 chip detected, register read %x\n", data);
+		ret = -ENODEV;
+		goto done;
+	}
+
+	mt9m001->fmt = &mt9m001->fmts[0];
+
+	dev_info(&client->dev, "Detected a MT9M001 chip ID %x (%s)\n", data,
+		 data == 0x8431 ? "C12STM" : "C12ST");
+
+	ret = mt9m001_init(client);
+	if (ret < 0) {
+		dev_err(&client->dev, "Failed to initialise the camera\n");
+		goto done;
+	}
+
+	/* mt9m001_init() has reset the chip, returning registers to defaults */
+	ret = v4l2_ctrl_handler_setup(&mt9m001->hdl);
+
+done:
+	return ret;
+}
+
+static int mt9m001_g_skip_top_lines(struct v4l2_subdev *sd, u32 *lines)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9m001 *mt9m001 = to_mt9m001(client);
+
+	*lines = mt9m001->y_skip_top;
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops mt9m001_ctrl_ops = {
+	.g_volatile_ctrl = mt9m001_g_volatile_ctrl,
+	.s_ctrl = mt9m001_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops mt9m001_subdev_core_ops = {
+	.log_status = v4l2_ctrl_subdev_log_status,
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register	= mt9m001_g_register,
+	.s_register	= mt9m001_s_register,
+#endif
+};
+
+static int mt9m001_init_cfg(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_state *sd_state)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9m001 *mt9m001 = to_mt9m001(client);
+	struct v4l2_mbus_framefmt *try_fmt =
+		v4l2_subdev_get_try_format(sd, sd_state, 0);
+
+	try_fmt->width		= MT9M001_MAX_WIDTH;
+	try_fmt->height		= MT9M001_MAX_HEIGHT;
+	try_fmt->code		= mt9m001->fmts[0].code;
+	try_fmt->colorspace	= mt9m001->fmts[0].colorspace;
+	try_fmt->field		= V4L2_FIELD_NONE;
+	try_fmt->ycbcr_enc	= V4L2_YCBCR_ENC_DEFAULT;
+	try_fmt->quantization	= V4L2_QUANTIZATION_DEFAULT;
+	try_fmt->xfer_func	= V4L2_XFER_FUNC_DEFAULT;
+
+	return 0;
+}
+
+static int mt9m001_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9m001 *mt9m001 = to_mt9m001(client);
+
+	if (code->pad || code->index >= mt9m001->num_fmts)
+		return -EINVAL;
+
+	code->code = mt9m001->fmts[code->index].code;
+	return 0;
+}
+
+static int mt9m001_get_mbus_config(struct v4l2_subdev *sd,
+				   unsigned int pad,
+				   struct v4l2_mbus_config *cfg)
+{
+	/* MT9M001 has all capture_format parameters fixed */
+	cfg->type = V4L2_MBUS_PARALLEL;
+	cfg->bus.parallel.flags = V4L2_MBUS_PCLK_SAMPLE_FALLING |
+				  V4L2_MBUS_HSYNC_ACTIVE_HIGH |
+				  V4L2_MBUS_VSYNC_ACTIVE_HIGH |
+				  V4L2_MBUS_DATA_ACTIVE_HIGH |
+				  V4L2_MBUS_MASTER;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops mt9m001_subdev_video_ops = {
+	.s_stream	= mt9m001_s_stream,
+};
+
+static const struct v4l2_subdev_sensor_ops mt9m001_subdev_sensor_ops = {
+	.g_skip_top_lines	= mt9m001_g_skip_top_lines,
+};
+
+static const struct v4l2_subdev_pad_ops mt9m001_subdev_pad_ops = {
+	.init_cfg	= mt9m001_init_cfg,
+	.enum_mbus_code = mt9m001_enum_mbus_code,
+	.get_selection	= mt9m001_get_selection,
+	.set_selection	= mt9m001_set_selection,
+	.get_fmt	= mt9m001_get_fmt,
+	.set_fmt	= mt9m001_set_fmt,
+	.get_mbus_config = mt9m001_get_mbus_config,
+};
+
+static const struct v4l2_subdev_ops mt9m001_subdev_ops = {
+	.core	= &mt9m001_subdev_core_ops,
+	.video	= &mt9m001_subdev_video_ops,
+	.sensor	= &mt9m001_subdev_sensor_ops,
+	.pad	= &mt9m001_subdev_pad_ops,
+};
+
+static int mt9m001_probe(struct i2c_client *client)
+{
+	struct mt9m001 *mt9m001;
+	struct i2c_adapter *adapter = client->adapter;
+	int ret;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
+		dev_warn(&adapter->dev,
+			 "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
+		return -EIO;
+	}
+
+	mt9m001 = devm_kzalloc(&client->dev, sizeof(*mt9m001), GFP_KERNEL);
+	if (!mt9m001)
+		return -ENOMEM;
+
+	mt9m001->clk = devm_clk_get(&client->dev, NULL);
+	if (IS_ERR(mt9m001->clk))
+		return PTR_ERR(mt9m001->clk);
+
+	mt9m001->standby_gpio = devm_gpiod_get_optional(&client->dev, "standby",
+							GPIOD_OUT_LOW);
+	if (IS_ERR(mt9m001->standby_gpio))
+		return PTR_ERR(mt9m001->standby_gpio);
+
+	mt9m001->reset_gpio = devm_gpiod_get_optional(&client->dev, "reset",
+						      GPIOD_OUT_LOW);
+	if (IS_ERR(mt9m001->reset_gpio))
+		return PTR_ERR(mt9m001->reset_gpio);
+
+	v4l2_i2c_subdev_init(&mt9m001->subdev, client, &mt9m001_subdev_ops);
+	mt9m001->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+				 V4L2_SUBDEV_FL_HAS_EVENTS;
+	v4l2_ctrl_handler_init(&mt9m001->hdl, 4);
+	v4l2_ctrl_new_std(&mt9m001->hdl, &mt9m001_ctrl_ops,
+			V4L2_CID_VFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&mt9m001->hdl, &mt9m001_ctrl_ops,
+			V4L2_CID_GAIN, 0, 127, 1, 64);
+	mt9m001->exposure = v4l2_ctrl_new_std(&mt9m001->hdl, &mt9m001_ctrl_ops,
+			V4L2_CID_EXPOSURE, 1, 255, 1, 255);
+	/*
+	 * Simulated autoexposure. If enabled, we calculate shutter width
+	 * ourselves in the driver based on vertical blanking and frame width
+	 */
+	mt9m001->autoexposure = v4l2_ctrl_new_std_menu(&mt9m001->hdl,
+			&mt9m001_ctrl_ops, V4L2_CID_EXPOSURE_AUTO, 1, 0,
+			V4L2_EXPOSURE_AUTO);
+	mt9m001->subdev.ctrl_handler = &mt9m001->hdl;
+	if (mt9m001->hdl.error)
+		return mt9m001->hdl.error;
+
+	v4l2_ctrl_auto_cluster(2, &mt9m001->autoexposure,
+					V4L2_EXPOSURE_MANUAL, true);
+
+	mutex_init(&mt9m001->mutex);
+	mt9m001->hdl.lock = &mt9m001->mutex;
+
+	/* Second stage probe - when a capture adapter is there */
+	mt9m001->y_skip_top	= 0;
+	mt9m001->rect.left	= MT9M001_COLUMN_SKIP;
+	mt9m001->rect.top	= MT9M001_ROW_SKIP;
+	mt9m001->rect.width	= MT9M001_MAX_WIDTH;
+	mt9m001->rect.height	= MT9M001_MAX_HEIGHT;
+
+	ret = mt9m001_power_on(&client->dev);
+	if (ret)
+		goto error_hdl_free;
+
+	pm_runtime_set_active(&client->dev);
+	pm_runtime_enable(&client->dev);
+
+	ret = mt9m001_video_probe(client);
+	if (ret)
+		goto error_power_off;
+
+	mt9m001->pad.flags = MEDIA_PAD_FL_SOURCE;
+	mt9m001->subdev.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret = media_entity_pads_init(&mt9m001->subdev.entity, 1, &mt9m001->pad);
+	if (ret)
+		goto error_power_off;
+
+	ret = v4l2_async_register_subdev(&mt9m001->subdev);
+	if (ret)
+		goto error_entity_cleanup;
+
+	pm_runtime_idle(&client->dev);
+
+	return 0;
+
+error_entity_cleanup:
+	media_entity_cleanup(&mt9m001->subdev.entity);
+error_power_off:
+	pm_runtime_disable(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+	mt9m001_power_off(&client->dev);
+
+error_hdl_free:
+	v4l2_ctrl_handler_free(&mt9m001->hdl);
+	mutex_destroy(&mt9m001->mutex);
+
+	return ret;
+}
+
+static void mt9m001_remove(struct i2c_client *client)
+{
+	struct mt9m001 *mt9m001 = to_mt9m001(client);
+
+	/*
+	 * As it increments RPM usage_count even on errors, we don't need to
+	 * check the returned code here.
+	 */
+	pm_runtime_get_sync(&client->dev);
+
+	v4l2_async_unregister_subdev(&mt9m001->subdev);
+	media_entity_cleanup(&mt9m001->subdev.entity);
+
+	pm_runtime_disable(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+	pm_runtime_put_noidle(&client->dev);
+	mt9m001_power_off(&client->dev);
+
+	v4l2_ctrl_handler_free(&mt9m001->hdl);
+	mutex_destroy(&mt9m001->mutex);
+}
+
+static const struct i2c_device_id mt9m001_id[] = {
+	{ "mt9m001", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, mt9m001_id);
+
+static const struct dev_pm_ops mt9m001_pm_ops = {
+	SET_RUNTIME_PM_OPS(mt9m001_power_off, mt9m001_power_on, NULL)
+};
+
+static const struct of_device_id mt9m001_of_match[] = {
+	{ .compatible = "onnn,mt9m001", },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, mt9m001_of_match);
+
+static struct i2c_driver mt9m001_i2c_driver = {
+	.driver = {
+		.name = "mt9m001",
+		.pm = &mt9m001_pm_ops,
+		.of_match_table = mt9m001_of_match,
+	},
+	.probe		= mt9m001_probe,
+	.remove		= mt9m001_remove,
+	.id_table	= mt9m001_id,
+};
+
+module_i2c_driver(mt9m001_i2c_driver);
+
+MODULE_DESCRIPTION("Micron MT9M001 Camera driver");
+MODULE_AUTHOR("Guennadi Liakhovetski <kernel@pengutronix.de>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/mt9m111.c b/drivers/media/i2c/mt9m111.c
new file mode 100644
index 000000000..df8d9c9e6
--- /dev/null
+++ b/drivers/media/i2c/mt9m111.c
@@ -0,0 +1,1396 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Driver for MT9M111/MT9M112/MT9M131 CMOS Image Sensor from Micron/Aptina
+ *
+ * Copyright (C) 2008, Robert Jarzmik <robert.jarzmik@free.fr>
+ */
+#include <linux/clk.h>
+#include <linux/videodev2.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/log2.h>
+#include <linux/delay.h>
+#include <linux/regulator/consumer.h>
+#include <linux/v4l2-mediabus.h>
+#include <linux/module.h>
+#include <linux/property.h>
+
+#include <media/v4l2-async.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+
+/*
+ * MT9M111, MT9M112 and MT9M131:
+ * i2c address is 0x48 or 0x5d (depending on SADDR pin)
+ * The platform has to define struct i2c_board_info objects and link to them
+ * from struct soc_camera_host_desc
+ */
+
+/*
+ * Sensor core register addresses (0x000..0x0ff)
+ */
+#define MT9M111_CHIP_VERSION		0x000
+#define MT9M111_ROW_START		0x001
+#define MT9M111_COLUMN_START		0x002
+#define MT9M111_WINDOW_HEIGHT		0x003
+#define MT9M111_WINDOW_WIDTH		0x004
+#define MT9M111_HORIZONTAL_BLANKING_B	0x005
+#define MT9M111_VERTICAL_BLANKING_B	0x006
+#define MT9M111_HORIZONTAL_BLANKING_A	0x007
+#define MT9M111_VERTICAL_BLANKING_A	0x008
+#define MT9M111_SHUTTER_WIDTH		0x009
+#define MT9M111_ROW_SPEED		0x00a
+#define MT9M111_EXTRA_DELAY		0x00b
+#define MT9M111_SHUTTER_DELAY		0x00c
+#define MT9M111_RESET			0x00d
+#define MT9M111_READ_MODE_B		0x020
+#define MT9M111_READ_MODE_A		0x021
+#define MT9M111_FLASH_CONTROL		0x023
+#define MT9M111_GREEN1_GAIN		0x02b
+#define MT9M111_BLUE_GAIN		0x02c
+#define MT9M111_RED_GAIN		0x02d
+#define MT9M111_GREEN2_GAIN		0x02e
+#define MT9M111_GLOBAL_GAIN		0x02f
+#define MT9M111_CONTEXT_CONTROL		0x0c8
+#define MT9M111_PAGE_MAP		0x0f0
+#define MT9M111_BYTE_WISE_ADDR		0x0f1
+
+#define MT9M111_RESET_SYNC_CHANGES	(1 << 15)
+#define MT9M111_RESET_RESTART_BAD_FRAME	(1 << 9)
+#define MT9M111_RESET_SHOW_BAD_FRAMES	(1 << 8)
+#define MT9M111_RESET_RESET_SOC		(1 << 5)
+#define MT9M111_RESET_OUTPUT_DISABLE	(1 << 4)
+#define MT9M111_RESET_CHIP_ENABLE	(1 << 3)
+#define MT9M111_RESET_ANALOG_STANDBY	(1 << 2)
+#define MT9M111_RESET_RESTART_FRAME	(1 << 1)
+#define MT9M111_RESET_RESET_MODE	(1 << 0)
+
+#define MT9M111_RM_FULL_POWER_RD	(0 << 10)
+#define MT9M111_RM_LOW_POWER_RD		(1 << 10)
+#define MT9M111_RM_COL_SKIP_4X		(1 << 5)
+#define MT9M111_RM_ROW_SKIP_4X		(1 << 4)
+#define MT9M111_RM_COL_SKIP_2X		(1 << 3)
+#define MT9M111_RM_ROW_SKIP_2X		(1 << 2)
+#define MT9M111_RMB_MIRROR_COLS		(1 << 1)
+#define MT9M111_RMB_MIRROR_ROWS		(1 << 0)
+#define MT9M111_CTXT_CTRL_RESTART	(1 << 15)
+#define MT9M111_CTXT_CTRL_DEFECTCOR_B	(1 << 12)
+#define MT9M111_CTXT_CTRL_RESIZE_B	(1 << 10)
+#define MT9M111_CTXT_CTRL_CTRL2_B	(1 << 9)
+#define MT9M111_CTXT_CTRL_GAMMA_B	(1 << 8)
+#define MT9M111_CTXT_CTRL_XENON_EN	(1 << 7)
+#define MT9M111_CTXT_CTRL_READ_MODE_B	(1 << 3)
+#define MT9M111_CTXT_CTRL_LED_FLASH_EN	(1 << 2)
+#define MT9M111_CTXT_CTRL_VBLANK_SEL_B	(1 << 1)
+#define MT9M111_CTXT_CTRL_HBLANK_SEL_B	(1 << 0)
+
+/*
+ * Colorpipe register addresses (0x100..0x1ff)
+ */
+#define MT9M111_OPER_MODE_CTRL		0x106
+#define MT9M111_OUTPUT_FORMAT_CTRL	0x108
+#define MT9M111_TPG_CTRL		0x148
+#define MT9M111_REDUCER_XZOOM_B		0x1a0
+#define MT9M111_REDUCER_XSIZE_B		0x1a1
+#define MT9M111_REDUCER_YZOOM_B		0x1a3
+#define MT9M111_REDUCER_YSIZE_B		0x1a4
+#define MT9M111_REDUCER_XZOOM_A		0x1a6
+#define MT9M111_REDUCER_XSIZE_A		0x1a7
+#define MT9M111_REDUCER_YZOOM_A		0x1a9
+#define MT9M111_REDUCER_YSIZE_A		0x1aa
+#define MT9M111_EFFECTS_MODE		0x1e2
+
+#define MT9M111_OUTPUT_FORMAT_CTRL2_A	0x13a
+#define MT9M111_OUTPUT_FORMAT_CTRL2_B	0x19b
+
+#define MT9M111_OPMODE_AUTOEXPO_EN	(1 << 14)
+#define MT9M111_OPMODE_AUTOWHITEBAL_EN	(1 << 1)
+#define MT9M111_OUTFMT_FLIP_BAYER_COL	(1 << 9)
+#define MT9M111_OUTFMT_FLIP_BAYER_ROW	(1 << 8)
+#define MT9M111_OUTFMT_PROCESSED_BAYER	(1 << 14)
+#define MT9M111_OUTFMT_BYPASS_IFP	(1 << 10)
+#define MT9M111_OUTFMT_INV_PIX_CLOCK	(1 << 9)
+#define MT9M111_OUTFMT_RGB		(1 << 8)
+#define MT9M111_OUTFMT_RGB565		(0 << 6)
+#define MT9M111_OUTFMT_RGB555		(1 << 6)
+#define MT9M111_OUTFMT_RGB444x		(2 << 6)
+#define MT9M111_OUTFMT_RGBx444		(3 << 6)
+#define MT9M111_OUTFMT_TST_RAMP_OFF	(0 << 4)
+#define MT9M111_OUTFMT_TST_RAMP_COL	(1 << 4)
+#define MT9M111_OUTFMT_TST_RAMP_ROW	(2 << 4)
+#define MT9M111_OUTFMT_TST_RAMP_FRAME	(3 << 4)
+#define MT9M111_OUTFMT_SHIFT_3_UP	(1 << 3)
+#define MT9M111_OUTFMT_AVG_CHROMA	(1 << 2)
+#define MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN	(1 << 1)
+#define MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B	(1 << 0)
+#define MT9M111_TPG_SEL_MASK		GENMASK(2, 0)
+#define MT9M111_EFFECTS_MODE_MASK	GENMASK(2, 0)
+#define MT9M111_RM_PWR_MASK		BIT(10)
+#define MT9M111_RM_SKIP2_MASK		GENMASK(3, 2)
+
+/*
+ * Camera control register addresses (0x200..0x2ff not implemented)
+ */
+
+#define reg_read(reg) mt9m111_reg_read(client, MT9M111_##reg)
+#define reg_write(reg, val) mt9m111_reg_write(client, MT9M111_##reg, (val))
+#define reg_set(reg, val) mt9m111_reg_set(client, MT9M111_##reg, (val))
+#define reg_clear(reg, val) mt9m111_reg_clear(client, MT9M111_##reg, (val))
+#define reg_mask(reg, val, mask) mt9m111_reg_mask(client, MT9M111_##reg, \
+		(val), (mask))
+
+#define MT9M111_MIN_DARK_ROWS	8
+#define MT9M111_MIN_DARK_COLS	26
+#define MT9M111_MAX_HEIGHT	1024
+#define MT9M111_MAX_WIDTH	1280
+
+struct mt9m111_context {
+	u16 read_mode;
+	u16 blanking_h;
+	u16 blanking_v;
+	u16 reducer_xzoom;
+	u16 reducer_yzoom;
+	u16 reducer_xsize;
+	u16 reducer_ysize;
+	u16 output_fmt_ctrl2;
+	u16 control;
+};
+
+static struct mt9m111_context context_a = {
+	.read_mode		= MT9M111_READ_MODE_A,
+	.blanking_h		= MT9M111_HORIZONTAL_BLANKING_A,
+	.blanking_v		= MT9M111_VERTICAL_BLANKING_A,
+	.reducer_xzoom		= MT9M111_REDUCER_XZOOM_A,
+	.reducer_yzoom		= MT9M111_REDUCER_YZOOM_A,
+	.reducer_xsize		= MT9M111_REDUCER_XSIZE_A,
+	.reducer_ysize		= MT9M111_REDUCER_YSIZE_A,
+	.output_fmt_ctrl2	= MT9M111_OUTPUT_FORMAT_CTRL2_A,
+	.control		= MT9M111_CTXT_CTRL_RESTART,
+};
+
+static struct mt9m111_context context_b = {
+	.read_mode		= MT9M111_READ_MODE_B,
+	.blanking_h		= MT9M111_HORIZONTAL_BLANKING_B,
+	.blanking_v		= MT9M111_VERTICAL_BLANKING_B,
+	.reducer_xzoom		= MT9M111_REDUCER_XZOOM_B,
+	.reducer_yzoom		= MT9M111_REDUCER_YZOOM_B,
+	.reducer_xsize		= MT9M111_REDUCER_XSIZE_B,
+	.reducer_ysize		= MT9M111_REDUCER_YSIZE_B,
+	.output_fmt_ctrl2	= MT9M111_OUTPUT_FORMAT_CTRL2_B,
+	.control		= MT9M111_CTXT_CTRL_RESTART |
+		MT9M111_CTXT_CTRL_DEFECTCOR_B | MT9M111_CTXT_CTRL_RESIZE_B |
+		MT9M111_CTXT_CTRL_CTRL2_B | MT9M111_CTXT_CTRL_GAMMA_B |
+		MT9M111_CTXT_CTRL_READ_MODE_B | MT9M111_CTXT_CTRL_VBLANK_SEL_B |
+		MT9M111_CTXT_CTRL_HBLANK_SEL_B,
+};
+
+/* MT9M111 has only one fixed colorspace per pixelcode */
+struct mt9m111_datafmt {
+	u32	code;
+	enum v4l2_colorspace		colorspace;
+};
+
+static const struct mt9m111_datafmt mt9m111_colour_fmts[] = {
+	{MEDIA_BUS_FMT_YUYV8_2X8, V4L2_COLORSPACE_SRGB},
+	{MEDIA_BUS_FMT_YVYU8_2X8, V4L2_COLORSPACE_SRGB},
+	{MEDIA_BUS_FMT_UYVY8_2X8, V4L2_COLORSPACE_SRGB},
+	{MEDIA_BUS_FMT_VYUY8_2X8, V4L2_COLORSPACE_SRGB},
+	{MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE, V4L2_COLORSPACE_SRGB},
+	{MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE, V4L2_COLORSPACE_SRGB},
+	{MEDIA_BUS_FMT_RGB565_2X8_LE, V4L2_COLORSPACE_SRGB},
+	{MEDIA_BUS_FMT_RGB565_2X8_BE, V4L2_COLORSPACE_SRGB},
+	{MEDIA_BUS_FMT_BGR565_2X8_LE, V4L2_COLORSPACE_SRGB},
+	{MEDIA_BUS_FMT_BGR565_2X8_BE, V4L2_COLORSPACE_SRGB},
+	{MEDIA_BUS_FMT_SBGGR8_1X8, V4L2_COLORSPACE_SRGB},
+	{MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE, V4L2_COLORSPACE_SRGB},
+};
+
+enum mt9m111_mode_id {
+	MT9M111_MODE_SXGA_8FPS,
+	MT9M111_MODE_SXGA_15FPS,
+	MT9M111_MODE_QSXGA_30FPS,
+	MT9M111_NUM_MODES,
+};
+
+struct mt9m111_mode_info {
+	unsigned int sensor_w;
+	unsigned int sensor_h;
+	unsigned int max_image_w;
+	unsigned int max_image_h;
+	unsigned int max_fps;
+	unsigned int reg_val;
+	unsigned int reg_mask;
+};
+
+struct mt9m111 {
+	struct v4l2_subdev subdev;
+	struct v4l2_ctrl_handler hdl;
+	struct v4l2_ctrl *gain;
+	struct mt9m111_context *ctx;
+	struct v4l2_rect rect;	/* cropping rectangle */
+	struct clk *clk;
+	unsigned int width;	/* output */
+	unsigned int height;	/* sizes */
+	struct v4l2_fract frame_interval;
+	const struct mt9m111_mode_info *current_mode;
+	struct mutex power_lock; /* lock to protect power_count */
+	int power_count;
+	const struct mt9m111_datafmt *fmt;
+	int lastpage;	/* PageMap cache value */
+	struct regulator *regulator;
+	bool is_streaming;
+	/* user point of view - 0: falling 1: rising edge */
+	unsigned int pclk_sample:1;
+#ifdef CONFIG_MEDIA_CONTROLLER
+	struct media_pad pad;
+#endif
+};
+
+static const struct mt9m111_mode_info mt9m111_mode_data[MT9M111_NUM_MODES] = {
+	[MT9M111_MODE_SXGA_8FPS] = {
+		.sensor_w = 1280,
+		.sensor_h = 1024,
+		.max_image_w = 1280,
+		.max_image_h = 1024,
+		.max_fps = 8,
+		.reg_val = MT9M111_RM_LOW_POWER_RD,
+		.reg_mask = MT9M111_RM_PWR_MASK | MT9M111_RM_SKIP2_MASK,
+	},
+	[MT9M111_MODE_SXGA_15FPS] = {
+		.sensor_w = 1280,
+		.sensor_h = 1024,
+		.max_image_w = 1280,
+		.max_image_h = 1024,
+		.max_fps = 15,
+		.reg_val = MT9M111_RM_FULL_POWER_RD,
+		.reg_mask = MT9M111_RM_PWR_MASK | MT9M111_RM_SKIP2_MASK,
+	},
+	[MT9M111_MODE_QSXGA_30FPS] = {
+		.sensor_w = 1280,
+		.sensor_h = 1024,
+		.max_image_w = 640,
+		.max_image_h = 512,
+		.max_fps = 30,
+		.reg_val = MT9M111_RM_LOW_POWER_RD | MT9M111_RM_COL_SKIP_2X |
+			   MT9M111_RM_ROW_SKIP_2X,
+		.reg_mask = MT9M111_RM_PWR_MASK | MT9M111_RM_SKIP2_MASK,
+	},
+};
+
+/* Find a data format by a pixel code */
+static const struct mt9m111_datafmt *mt9m111_find_datafmt(struct mt9m111 *mt9m111,
+						u32 code)
+{
+	int i;
+	for (i = 0; i < ARRAY_SIZE(mt9m111_colour_fmts); i++)
+		if (mt9m111_colour_fmts[i].code == code)
+			return mt9m111_colour_fmts + i;
+
+	return mt9m111->fmt;
+}
+
+static struct mt9m111 *to_mt9m111(const struct i2c_client *client)
+{
+	return container_of(i2c_get_clientdata(client), struct mt9m111, subdev);
+}
+
+static int reg_page_map_set(struct i2c_client *client, const u16 reg)
+{
+	int ret;
+	u16 page;
+	struct mt9m111 *mt9m111 = to_mt9m111(client);
+
+	page = (reg >> 8);
+	if (page == mt9m111->lastpage)
+		return 0;
+	if (page > 2)
+		return -EINVAL;
+
+	ret = i2c_smbus_write_word_swapped(client, MT9M111_PAGE_MAP, page);
+	if (!ret)
+		mt9m111->lastpage = page;
+	return ret;
+}
+
+static int mt9m111_reg_read(struct i2c_client *client, const u16 reg)
+{
+	int ret;
+
+	ret = reg_page_map_set(client, reg);
+	if (!ret)
+		ret = i2c_smbus_read_word_swapped(client, reg & 0xff);
+
+	dev_dbg(&client->dev, "read  reg.%03x -> %04x\n", reg, ret);
+	return ret;
+}
+
+static int mt9m111_reg_write(struct i2c_client *client, const u16 reg,
+			     const u16 data)
+{
+	int ret;
+
+	ret = reg_page_map_set(client, reg);
+	if (!ret)
+		ret = i2c_smbus_write_word_swapped(client, reg & 0xff, data);
+	dev_dbg(&client->dev, "write reg.%03x = %04x -> %d\n", reg, data, ret);
+	return ret;
+}
+
+static int mt9m111_reg_set(struct i2c_client *client, const u16 reg,
+			   const u16 data)
+{
+	int ret;
+
+	ret = mt9m111_reg_read(client, reg);
+	if (ret >= 0)
+		ret = mt9m111_reg_write(client, reg, ret | data);
+	return ret;
+}
+
+static int mt9m111_reg_clear(struct i2c_client *client, const u16 reg,
+			     const u16 data)
+{
+	int ret;
+
+	ret = mt9m111_reg_read(client, reg);
+	if (ret >= 0)
+		ret = mt9m111_reg_write(client, reg, ret & ~data);
+	return ret;
+}
+
+static int mt9m111_reg_mask(struct i2c_client *client, const u16 reg,
+			    const u16 data, const u16 mask)
+{
+	int ret;
+
+	ret = mt9m111_reg_read(client, reg);
+	if (ret >= 0)
+		ret = mt9m111_reg_write(client, reg, (ret & ~mask) | data);
+	return ret;
+}
+
+static int mt9m111_set_context(struct mt9m111 *mt9m111,
+			       struct mt9m111_context *ctx)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
+	return reg_write(CONTEXT_CONTROL, ctx->control);
+}
+
+static int mt9m111_setup_rect_ctx(struct mt9m111 *mt9m111,
+			struct mt9m111_context *ctx, struct v4l2_rect *rect,
+			unsigned int width, unsigned int height)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
+	int ret = mt9m111_reg_write(client, ctx->reducer_xzoom, rect->width);
+	if (!ret)
+		ret = mt9m111_reg_write(client, ctx->reducer_yzoom, rect->height);
+	if (!ret)
+		ret = mt9m111_reg_write(client, ctx->reducer_xsize, width);
+	if (!ret)
+		ret = mt9m111_reg_write(client, ctx->reducer_ysize, height);
+	return ret;
+}
+
+static int mt9m111_setup_geometry(struct mt9m111 *mt9m111, struct v4l2_rect *rect,
+			int width, int height, u32 code)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
+	int ret;
+
+	ret = reg_write(COLUMN_START, rect->left);
+	if (!ret)
+		ret = reg_write(ROW_START, rect->top);
+
+	if (!ret)
+		ret = reg_write(WINDOW_WIDTH, rect->width);
+	if (!ret)
+		ret = reg_write(WINDOW_HEIGHT, rect->height);
+
+	if (code != MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) {
+		/* IFP in use, down-scaling possible */
+		if (!ret)
+			ret = mt9m111_setup_rect_ctx(mt9m111, &context_b,
+						     rect, width, height);
+		if (!ret)
+			ret = mt9m111_setup_rect_ctx(mt9m111, &context_a,
+						     rect, width, height);
+	}
+
+	dev_dbg(&client->dev, "%s(%x): %ux%u@%u:%u -> %ux%u = %d\n",
+		__func__, code, rect->width, rect->height, rect->left, rect->top,
+		width, height, ret);
+
+	return ret;
+}
+
+static int mt9m111_enable(struct mt9m111 *mt9m111)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
+	return reg_write(RESET, MT9M111_RESET_CHIP_ENABLE);
+}
+
+static int mt9m111_reset(struct mt9m111 *mt9m111)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
+	int ret;
+
+	ret = reg_set(RESET, MT9M111_RESET_RESET_MODE);
+	if (!ret)
+		ret = reg_set(RESET, MT9M111_RESET_RESET_SOC);
+	if (!ret)
+		ret = reg_clear(RESET, MT9M111_RESET_RESET_MODE
+				| MT9M111_RESET_RESET_SOC);
+
+	return ret;
+}
+
+static int mt9m111_set_selection(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_selection *sel)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9m111 *mt9m111 = to_mt9m111(client);
+	struct v4l2_rect rect = sel->r;
+	int width, height;
+	int ret, align = 0;
+
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE ||
+	    sel->target != V4L2_SEL_TGT_CROP)
+		return -EINVAL;
+
+	if (mt9m111->fmt->code == MEDIA_BUS_FMT_SBGGR8_1X8 ||
+	    mt9m111->fmt->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) {
+		/* Bayer format - even size lengths */
+		align = 1;
+		/* Let the user play with the starting pixel */
+	}
+
+	/* FIXME: the datasheet doesn't specify minimum sizes */
+	v4l_bound_align_image(&rect.width, 2, MT9M111_MAX_WIDTH, align,
+			      &rect.height, 2, MT9M111_MAX_HEIGHT, align, 0);
+	rect.left = clamp(rect.left, MT9M111_MIN_DARK_COLS,
+			  MT9M111_MIN_DARK_COLS + MT9M111_MAX_WIDTH -
+			  (__s32)rect.width);
+	rect.top = clamp(rect.top, MT9M111_MIN_DARK_ROWS,
+			 MT9M111_MIN_DARK_ROWS + MT9M111_MAX_HEIGHT -
+			 (__s32)rect.height);
+
+	width = min(mt9m111->width, rect.width);
+	height = min(mt9m111->height, rect.height);
+
+	ret = mt9m111_setup_geometry(mt9m111, &rect, width, height, mt9m111->fmt->code);
+	if (!ret) {
+		mt9m111->rect = rect;
+		mt9m111->width = width;
+		mt9m111->height = height;
+	}
+
+	return ret;
+}
+
+static int mt9m111_get_selection(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_selection *sel)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9m111 *mt9m111 = to_mt9m111(client);
+
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
+		return -EINVAL;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.left = MT9M111_MIN_DARK_COLS;
+		sel->r.top = MT9M111_MIN_DARK_ROWS;
+		sel->r.width = MT9M111_MAX_WIDTH;
+		sel->r.height = MT9M111_MAX_HEIGHT;
+		return 0;
+	case V4L2_SEL_TGT_CROP:
+		sel->r = mt9m111->rect;
+		return 0;
+	default:
+		return -EINVAL;
+	}
+}
+
+static int mt9m111_get_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *mf = &format->format;
+	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
+
+	if (format->pad)
+		return -EINVAL;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
+		mf = v4l2_subdev_get_try_format(sd, sd_state, format->pad);
+		format->format = *mf;
+		return 0;
+#else
+		return -EINVAL;
+#endif
+	}
+
+	mf->width	= mt9m111->width;
+	mf->height	= mt9m111->height;
+	mf->code	= mt9m111->fmt->code;
+	mf->colorspace	= mt9m111->fmt->colorspace;
+	mf->field	= V4L2_FIELD_NONE;
+	mf->ycbcr_enc	= V4L2_YCBCR_ENC_DEFAULT;
+	mf->quantization	= V4L2_QUANTIZATION_DEFAULT;
+	mf->xfer_func	= V4L2_XFER_FUNC_DEFAULT;
+
+	return 0;
+}
+
+static int mt9m111_set_pixfmt(struct mt9m111 *mt9m111,
+			      u32 code)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
+	u16 data_outfmt2, mask_outfmt2 = MT9M111_OUTFMT_PROCESSED_BAYER |
+		MT9M111_OUTFMT_BYPASS_IFP | MT9M111_OUTFMT_RGB |
+		MT9M111_OUTFMT_RGB565 | MT9M111_OUTFMT_RGB555 |
+		MT9M111_OUTFMT_RGB444x | MT9M111_OUTFMT_RGBx444 |
+		MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
+		MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
+	int ret;
+
+	switch (code) {
+	case MEDIA_BUS_FMT_SBGGR8_1X8:
+		data_outfmt2 = MT9M111_OUTFMT_PROCESSED_BAYER |
+			MT9M111_OUTFMT_RGB;
+		break;
+	case MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE:
+		data_outfmt2 = MT9M111_OUTFMT_BYPASS_IFP | MT9M111_OUTFMT_RGB;
+		break;
+	case MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE:
+		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB555 |
+			MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
+		break;
+	case MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE:
+		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB555;
+		break;
+	case MEDIA_BUS_FMT_RGB565_2X8_LE:
+		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
+			MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
+		break;
+	case MEDIA_BUS_FMT_RGB565_2X8_BE:
+		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565;
+		break;
+	case MEDIA_BUS_FMT_BGR565_2X8_BE:
+		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
+			MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
+		break;
+	case MEDIA_BUS_FMT_BGR565_2X8_LE:
+		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
+			MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
+			MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
+		break;
+	case MEDIA_BUS_FMT_UYVY8_2X8:
+		data_outfmt2 = 0;
+		break;
+	case MEDIA_BUS_FMT_VYUY8_2X8:
+		data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
+		break;
+	case MEDIA_BUS_FMT_YUYV8_2X8:
+		data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
+		break;
+	case MEDIA_BUS_FMT_YVYU8_2X8:
+		data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
+			MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
+		break;
+	default:
+		dev_err(&client->dev, "Pixel format not handled: %x\n", code);
+		return -EINVAL;
+	}
+
+	/* receiver samples on falling edge, chip-hw default is rising */
+	if (mt9m111->pclk_sample == 0)
+		mask_outfmt2 |= MT9M111_OUTFMT_INV_PIX_CLOCK;
+
+	ret = mt9m111_reg_mask(client, context_a.output_fmt_ctrl2,
+			       data_outfmt2, mask_outfmt2);
+	if (!ret)
+		ret = mt9m111_reg_mask(client, context_b.output_fmt_ctrl2,
+				       data_outfmt2, mask_outfmt2);
+
+	return ret;
+}
+
+static int mt9m111_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *mf = &format->format;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
+	const struct mt9m111_datafmt *fmt;
+	struct v4l2_rect *rect = &mt9m111->rect;
+	bool bayer;
+	int ret;
+
+	if (mt9m111->is_streaming)
+		return -EBUSY;
+
+	if (format->pad)
+		return -EINVAL;
+
+	fmt = mt9m111_find_datafmt(mt9m111, mf->code);
+
+	bayer = fmt->code == MEDIA_BUS_FMT_SBGGR8_1X8 ||
+		fmt->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE;
+
+	/*
+	 * With Bayer format enforce even side lengths, but let the user play
+	 * with the starting pixel
+	 */
+	if (bayer) {
+		rect->width = ALIGN(rect->width, 2);
+		rect->height = ALIGN(rect->height, 2);
+	}
+
+	if (fmt->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) {
+		/* IFP bypass mode, no scaling */
+		mf->width = rect->width;
+		mf->height = rect->height;
+	} else {
+		/* No upscaling */
+		if (mf->width > rect->width)
+			mf->width = rect->width;
+		if (mf->height > rect->height)
+			mf->height = rect->height;
+	}
+
+	dev_dbg(&client->dev, "%s(): %ux%u, code=%x\n", __func__,
+		mf->width, mf->height, fmt->code);
+
+	mf->code = fmt->code;
+	mf->colorspace = fmt->colorspace;
+	mf->field	= V4L2_FIELD_NONE;
+	mf->ycbcr_enc	= V4L2_YCBCR_ENC_DEFAULT;
+	mf->quantization	= V4L2_QUANTIZATION_DEFAULT;
+	mf->xfer_func	= V4L2_XFER_FUNC_DEFAULT;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		sd_state->pads->try_fmt = *mf;
+		return 0;
+	}
+
+	ret = mt9m111_setup_geometry(mt9m111, rect, mf->width, mf->height, mf->code);
+	if (!ret)
+		ret = mt9m111_set_pixfmt(mt9m111, mf->code);
+	if (!ret) {
+		mt9m111->width	= mf->width;
+		mt9m111->height	= mf->height;
+		mt9m111->fmt	= fmt;
+	}
+
+	return ret;
+}
+
+static const struct mt9m111_mode_info *
+mt9m111_find_mode(struct mt9m111 *mt9m111, unsigned int req_fps,
+		  unsigned int width, unsigned int height)
+{
+	const struct mt9m111_mode_info *mode;
+	struct v4l2_rect *sensor_rect = &mt9m111->rect;
+	unsigned int gap, gap_best = (unsigned int) -1;
+	int i, best_gap_idx = MT9M111_MODE_SXGA_15FPS;
+	bool skip_30fps = false;
+
+	/*
+	 * The fps selection is based on the row, column skipping mechanism.
+	 * So ensure that the sensor window is set to default else the fps
+	 * aren't calculated correctly within the sensor hw.
+	 */
+	if (sensor_rect->width != MT9M111_MAX_WIDTH ||
+	    sensor_rect->height != MT9M111_MAX_HEIGHT) {
+		dev_info(mt9m111->subdev.dev,
+			 "Framerate selection is not supported for cropped "
+			 "images\n");
+		return NULL;
+	}
+
+	/* 30fps only supported for images not exceeding 640x512 */
+	if (width > MT9M111_MAX_WIDTH / 2 || height > MT9M111_MAX_HEIGHT / 2) {
+		dev_dbg(mt9m111->subdev.dev,
+			"Framerates > 15fps are supported only for images "
+			"not exceeding 640x512\n");
+		skip_30fps = true;
+	}
+
+	/* find best matched fps */
+	for (i = 0; i < MT9M111_NUM_MODES; i++) {
+		unsigned int fps = mt9m111_mode_data[i].max_fps;
+
+		if (fps == 30 && skip_30fps)
+			continue;
+
+		gap = abs(fps - req_fps);
+		if (gap < gap_best) {
+			best_gap_idx = i;
+			gap_best = gap;
+		}
+	}
+
+	/*
+	 * Use context a/b default timing values instead of calculate blanking
+	 * timing values.
+	 */
+	mode = &mt9m111_mode_data[best_gap_idx];
+	mt9m111->ctx = (best_gap_idx == MT9M111_MODE_QSXGA_30FPS) ? &context_a :
+								    &context_b;
+	return mode;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int mt9m111_g_register(struct v4l2_subdev *sd,
+			      struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int val;
+
+	if (reg->reg > 0x2ff)
+		return -EINVAL;
+
+	val = mt9m111_reg_read(client, reg->reg);
+	reg->size = 2;
+	reg->val = (u64)val;
+
+	if (reg->val > 0xffff)
+		return -EIO;
+
+	return 0;
+}
+
+static int mt9m111_s_register(struct v4l2_subdev *sd,
+			      const struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (reg->reg > 0x2ff)
+		return -EINVAL;
+
+	if (mt9m111_reg_write(client, reg->reg, reg->val) < 0)
+		return -EIO;
+
+	return 0;
+}
+#endif
+
+static int mt9m111_set_flip(struct mt9m111 *mt9m111, int flip, int mask)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
+	int ret;
+
+	if (flip)
+		ret = mt9m111_reg_set(client, mt9m111->ctx->read_mode, mask);
+	else
+		ret = mt9m111_reg_clear(client, mt9m111->ctx->read_mode, mask);
+
+	return ret;
+}
+
+static int mt9m111_get_global_gain(struct mt9m111 *mt9m111)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
+	int data;
+
+	data = reg_read(GLOBAL_GAIN);
+	if (data >= 0)
+		return (data & 0x2f) * (1 << ((data >> 10) & 1)) *
+			(1 << ((data >> 9) & 1));
+	return data;
+}
+
+static int mt9m111_set_global_gain(struct mt9m111 *mt9m111, int gain)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
+	u16 val;
+
+	if (gain > 63 * 2 * 2)
+		return -EINVAL;
+
+	if ((gain >= 64 * 2) && (gain < 63 * 2 * 2))
+		val = (1 << 10) | (1 << 9) | (gain / 4);
+	else if ((gain >= 64) && (gain < 64 * 2))
+		val = (1 << 9) | (gain / 2);
+	else
+		val = gain;
+
+	return reg_write(GLOBAL_GAIN, val);
+}
+
+static int mt9m111_set_autoexposure(struct mt9m111 *mt9m111, int val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
+
+	if (val == V4L2_EXPOSURE_AUTO)
+		return reg_set(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOEXPO_EN);
+	return reg_clear(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOEXPO_EN);
+}
+
+static int mt9m111_set_autowhitebalance(struct mt9m111 *mt9m111, int on)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
+
+	if (on)
+		return reg_set(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOWHITEBAL_EN);
+	return reg_clear(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOWHITEBAL_EN);
+}
+
+static const char * const mt9m111_test_pattern_menu[] = {
+	"Disabled",
+	"Vertical monochrome gradient",
+	"Flat color type 1",
+	"Flat color type 2",
+	"Flat color type 3",
+	"Flat color type 4",
+	"Flat color type 5",
+	"Color bar",
+};
+
+static int mt9m111_set_test_pattern(struct mt9m111 *mt9m111, int val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
+
+	return mt9m111_reg_mask(client, MT9M111_TPG_CTRL, val,
+				MT9M111_TPG_SEL_MASK);
+}
+
+static int mt9m111_set_colorfx(struct mt9m111 *mt9m111, int val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
+	static const struct v4l2_control colorfx[] = {
+		{ V4L2_COLORFX_NONE,		0 },
+		{ V4L2_COLORFX_BW,		1 },
+		{ V4L2_COLORFX_SEPIA,		2 },
+		{ V4L2_COLORFX_NEGATIVE,	3 },
+		{ V4L2_COLORFX_SOLARIZATION,	4 },
+	};
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(colorfx); i++) {
+		if (colorfx[i].id == val) {
+			return mt9m111_reg_mask(client, MT9M111_EFFECTS_MODE,
+						colorfx[i].value,
+						MT9M111_EFFECTS_MODE_MASK);
+		}
+	}
+
+	return -EINVAL;
+}
+
+static int mt9m111_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct mt9m111 *mt9m111 = container_of(ctrl->handler,
+					       struct mt9m111, hdl);
+
+	switch (ctrl->id) {
+	case V4L2_CID_VFLIP:
+		return mt9m111_set_flip(mt9m111, ctrl->val,
+					MT9M111_RMB_MIRROR_ROWS);
+	case V4L2_CID_HFLIP:
+		return mt9m111_set_flip(mt9m111, ctrl->val,
+					MT9M111_RMB_MIRROR_COLS);
+	case V4L2_CID_GAIN:
+		return mt9m111_set_global_gain(mt9m111, ctrl->val);
+	case V4L2_CID_EXPOSURE_AUTO:
+		return mt9m111_set_autoexposure(mt9m111, ctrl->val);
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		return mt9m111_set_autowhitebalance(mt9m111, ctrl->val);
+	case V4L2_CID_TEST_PATTERN:
+		return mt9m111_set_test_pattern(mt9m111, ctrl->val);
+	case V4L2_CID_COLORFX:
+		return mt9m111_set_colorfx(mt9m111, ctrl->val);
+	}
+
+	return -EINVAL;
+}
+
+static int mt9m111_suspend(struct mt9m111 *mt9m111)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
+	int ret;
+
+	v4l2_ctrl_s_ctrl(mt9m111->gain, mt9m111_get_global_gain(mt9m111));
+
+	ret = reg_set(RESET, MT9M111_RESET_RESET_MODE);
+	if (!ret)
+		ret = reg_set(RESET, MT9M111_RESET_RESET_SOC |
+			      MT9M111_RESET_OUTPUT_DISABLE |
+			      MT9M111_RESET_ANALOG_STANDBY);
+	if (!ret)
+		ret = reg_clear(RESET, MT9M111_RESET_CHIP_ENABLE);
+
+	return ret;
+}
+
+static void mt9m111_restore_state(struct mt9m111 *mt9m111)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
+
+	mt9m111_set_context(mt9m111, mt9m111->ctx);
+	mt9m111_set_pixfmt(mt9m111, mt9m111->fmt->code);
+	mt9m111_setup_geometry(mt9m111, &mt9m111->rect,
+			mt9m111->width, mt9m111->height, mt9m111->fmt->code);
+	v4l2_ctrl_handler_setup(&mt9m111->hdl);
+	mt9m111_reg_mask(client, mt9m111->ctx->read_mode,
+			 mt9m111->current_mode->reg_val,
+			 mt9m111->current_mode->reg_mask);
+}
+
+static int mt9m111_resume(struct mt9m111 *mt9m111)
+{
+	int ret = mt9m111_enable(mt9m111);
+	if (!ret)
+		ret = mt9m111_reset(mt9m111);
+	if (!ret)
+		mt9m111_restore_state(mt9m111);
+
+	return ret;
+}
+
+static int mt9m111_init(struct mt9m111 *mt9m111)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
+	int ret;
+
+	ret = mt9m111_enable(mt9m111);
+	if (!ret)
+		ret = mt9m111_reset(mt9m111);
+	if (!ret)
+		ret = mt9m111_set_context(mt9m111, mt9m111->ctx);
+	if (ret)
+		dev_err(&client->dev, "mt9m111 init failed: %d\n", ret);
+	return ret;
+}
+
+static int mt9m111_power_on(struct mt9m111 *mt9m111)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
+	int ret;
+
+	ret = clk_prepare_enable(mt9m111->clk);
+	if (ret < 0)
+		return ret;
+
+	ret = regulator_enable(mt9m111->regulator);
+	if (ret < 0)
+		goto out_clk_disable;
+
+	ret = mt9m111_resume(mt9m111);
+	if (ret < 0)
+		goto out_regulator_disable;
+
+	return 0;
+
+out_regulator_disable:
+	regulator_disable(mt9m111->regulator);
+
+out_clk_disable:
+	clk_disable_unprepare(mt9m111->clk);
+
+	dev_err(&client->dev, "Failed to resume the sensor: %d\n", ret);
+
+	return ret;
+}
+
+static void mt9m111_power_off(struct mt9m111 *mt9m111)
+{
+	mt9m111_suspend(mt9m111);
+	regulator_disable(mt9m111->regulator);
+	clk_disable_unprepare(mt9m111->clk);
+}
+
+static int mt9m111_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
+	int ret = 0;
+
+	mutex_lock(&mt9m111->power_lock);
+
+	/*
+	 * If the power count is modified from 0 to != 0 or from != 0 to 0,
+	 * update the power state.
+	 */
+	if (mt9m111->power_count == !on) {
+		if (on)
+			ret = mt9m111_power_on(mt9m111);
+		else
+			mt9m111_power_off(mt9m111);
+	}
+
+	if (!ret) {
+		/* Update the power count. */
+		mt9m111->power_count += on ? 1 : -1;
+		WARN_ON(mt9m111->power_count < 0);
+	}
+
+	mutex_unlock(&mt9m111->power_lock);
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops mt9m111_ctrl_ops = {
+	.s_ctrl = mt9m111_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops mt9m111_subdev_core_ops = {
+	.s_power	= mt9m111_s_power,
+	.log_status = v4l2_ctrl_subdev_log_status,
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register	= mt9m111_g_register,
+	.s_register	= mt9m111_s_register,
+#endif
+};
+
+static int mt9m111_g_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *fi)
+{
+	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
+
+	fi->interval = mt9m111->frame_interval;
+
+	return 0;
+}
+
+static int mt9m111_s_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *fi)
+{
+	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
+	const struct mt9m111_mode_info *mode;
+	struct v4l2_fract *fract = &fi->interval;
+	int fps;
+
+	if (mt9m111->is_streaming)
+		return -EBUSY;
+
+	if (fi->pad != 0)
+		return -EINVAL;
+
+	if (fract->numerator == 0) {
+		fract->denominator = 30;
+		fract->numerator = 1;
+	}
+
+	fps = DIV_ROUND_CLOSEST(fract->denominator, fract->numerator);
+
+	/* Find best fitting mode. Do not update the mode if no one was found. */
+	mode = mt9m111_find_mode(mt9m111, fps, mt9m111->width, mt9m111->height);
+	if (!mode)
+		return 0;
+
+	if (mode->max_fps != fps) {
+		fract->denominator = mode->max_fps;
+		fract->numerator = 1;
+	}
+
+	mt9m111->current_mode = mode;
+	mt9m111->frame_interval = fi->interval;
+
+	return 0;
+}
+
+static int mt9m111_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->pad || code->index >= ARRAY_SIZE(mt9m111_colour_fmts))
+		return -EINVAL;
+
+	code->code = mt9m111_colour_fmts[code->index].code;
+	return 0;
+}
+
+static int mt9m111_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
+
+	mt9m111->is_streaming = !!enable;
+	return 0;
+}
+
+static int mt9m111_init_cfg(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_state *sd_state)
+{
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
+	struct v4l2_mbus_framefmt *format =
+		v4l2_subdev_get_try_format(sd, sd_state, 0);
+
+	format->width	= MT9M111_MAX_WIDTH;
+	format->height	= MT9M111_MAX_HEIGHT;
+	format->code	= mt9m111_colour_fmts[0].code;
+	format->colorspace	= mt9m111_colour_fmts[0].colorspace;
+	format->field	= V4L2_FIELD_NONE;
+	format->ycbcr_enc	= V4L2_YCBCR_ENC_DEFAULT;
+	format->quantization	= V4L2_QUANTIZATION_DEFAULT;
+	format->xfer_func	= V4L2_XFER_FUNC_DEFAULT;
+#endif
+	return 0;
+}
+
+static int mt9m111_get_mbus_config(struct v4l2_subdev *sd,
+				   unsigned int pad,
+				   struct v4l2_mbus_config *cfg)
+{
+	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
+
+	cfg->type = V4L2_MBUS_PARALLEL;
+
+	cfg->bus.parallel.flags = V4L2_MBUS_MASTER |
+				  V4L2_MBUS_HSYNC_ACTIVE_HIGH |
+				  V4L2_MBUS_VSYNC_ACTIVE_HIGH |
+				  V4L2_MBUS_DATA_ACTIVE_HIGH;
+
+	cfg->bus.parallel.flags |= mt9m111->pclk_sample ?
+				   V4L2_MBUS_PCLK_SAMPLE_RISING :
+				   V4L2_MBUS_PCLK_SAMPLE_FALLING;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops mt9m111_subdev_video_ops = {
+	.s_stream	= mt9m111_s_stream,
+	.g_frame_interval = mt9m111_g_frame_interval,
+	.s_frame_interval = mt9m111_s_frame_interval,
+};
+
+static const struct v4l2_subdev_pad_ops mt9m111_subdev_pad_ops = {
+	.init_cfg	= mt9m111_init_cfg,
+	.enum_mbus_code = mt9m111_enum_mbus_code,
+	.get_selection	= mt9m111_get_selection,
+	.set_selection	= mt9m111_set_selection,
+	.get_fmt	= mt9m111_get_fmt,
+	.set_fmt	= mt9m111_set_fmt,
+	.get_mbus_config = mt9m111_get_mbus_config,
+};
+
+static const struct v4l2_subdev_ops mt9m111_subdev_ops = {
+	.core	= &mt9m111_subdev_core_ops,
+	.video	= &mt9m111_subdev_video_ops,
+	.pad	= &mt9m111_subdev_pad_ops,
+};
+
+/*
+ * Interface active, can use i2c. If it fails, it can indeed mean, that
+ * this wasn't our capture interface, so, we wait for the right one
+ */
+static int mt9m111_video_probe(struct i2c_client *client)
+{
+	struct mt9m111 *mt9m111 = to_mt9m111(client);
+	s32 data;
+	int ret;
+
+	ret = mt9m111_s_power(&mt9m111->subdev, 1);
+	if (ret < 0)
+		return ret;
+
+	data = reg_read(CHIP_VERSION);
+
+	switch (data) {
+	case 0x143a: /* MT9M111 or MT9M131 */
+		dev_info(&client->dev,
+			"Detected a MT9M111/MT9M131 chip ID %x\n", data);
+		break;
+	case 0x148c: /* MT9M112 */
+		dev_info(&client->dev, "Detected a MT9M112 chip ID %x\n", data);
+		break;
+	default:
+		dev_err(&client->dev,
+			"No MT9M111/MT9M112/MT9M131 chip detected register read %x\n",
+			data);
+		ret = -ENODEV;
+		goto done;
+	}
+
+	ret = mt9m111_init(mt9m111);
+	if (ret)
+		goto done;
+
+	ret = v4l2_ctrl_handler_setup(&mt9m111->hdl);
+
+done:
+	mt9m111_s_power(&mt9m111->subdev, 0);
+	return ret;
+}
+
+static int mt9m111_probe_fw(struct i2c_client *client, struct mt9m111 *mt9m111)
+{
+	struct v4l2_fwnode_endpoint bus_cfg = {
+		.bus_type = V4L2_MBUS_PARALLEL
+	};
+	struct fwnode_handle *np;
+	int ret;
+
+	np = fwnode_graph_get_next_endpoint(dev_fwnode(&client->dev), NULL);
+	if (!np)
+		return -EINVAL;
+
+	ret = v4l2_fwnode_endpoint_parse(np, &bus_cfg);
+	if (ret)
+		goto out_put_fw;
+
+	mt9m111->pclk_sample = !!(bus_cfg.bus.parallel.flags &
+				  V4L2_MBUS_PCLK_SAMPLE_RISING);
+
+out_put_fw:
+	fwnode_handle_put(np);
+	return ret;
+}
+
+static int mt9m111_probe(struct i2c_client *client)
+{
+	struct mt9m111 *mt9m111;
+	struct i2c_adapter *adapter = client->adapter;
+	int ret;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
+		dev_warn(&adapter->dev,
+			 "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
+		return -EIO;
+	}
+
+	mt9m111 = devm_kzalloc(&client->dev, sizeof(struct mt9m111), GFP_KERNEL);
+	if (!mt9m111)
+		return -ENOMEM;
+
+	if (dev_fwnode(&client->dev)) {
+		ret = mt9m111_probe_fw(client, mt9m111);
+		if (ret)
+			return ret;
+	}
+
+	mt9m111->clk = devm_clk_get(&client->dev, "mclk");
+	if (IS_ERR(mt9m111->clk))
+		return PTR_ERR(mt9m111->clk);
+
+	mt9m111->regulator = devm_regulator_get(&client->dev, "vdd");
+	if (IS_ERR(mt9m111->regulator)) {
+		dev_err(&client->dev, "regulator not found: %ld\n",
+			PTR_ERR(mt9m111->regulator));
+		return PTR_ERR(mt9m111->regulator);
+	}
+
+	/* Default HIGHPOWER context */
+	mt9m111->ctx = &context_b;
+
+	v4l2_i2c_subdev_init(&mt9m111->subdev, client, &mt9m111_subdev_ops);
+	mt9m111->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+				 V4L2_SUBDEV_FL_HAS_EVENTS;
+
+	v4l2_ctrl_handler_init(&mt9m111->hdl, 7);
+	v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
+			V4L2_CID_VFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
+			V4L2_CID_HFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
+			V4L2_CID_AUTO_WHITE_BALANCE, 0, 1, 1, 1);
+	mt9m111->gain = v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
+			V4L2_CID_GAIN, 0, 63 * 2 * 2, 1, 32);
+	v4l2_ctrl_new_std_menu(&mt9m111->hdl,
+			&mt9m111_ctrl_ops, V4L2_CID_EXPOSURE_AUTO, 1, 0,
+			V4L2_EXPOSURE_AUTO);
+	v4l2_ctrl_new_std_menu_items(&mt9m111->hdl,
+			&mt9m111_ctrl_ops, V4L2_CID_TEST_PATTERN,
+			ARRAY_SIZE(mt9m111_test_pattern_menu) - 1, 0, 0,
+			mt9m111_test_pattern_menu);
+	v4l2_ctrl_new_std_menu(&mt9m111->hdl, &mt9m111_ctrl_ops,
+			V4L2_CID_COLORFX, V4L2_COLORFX_SOLARIZATION,
+			~(BIT(V4L2_COLORFX_NONE) |
+				BIT(V4L2_COLORFX_BW) |
+				BIT(V4L2_COLORFX_SEPIA) |
+				BIT(V4L2_COLORFX_NEGATIVE) |
+				BIT(V4L2_COLORFX_SOLARIZATION)),
+			V4L2_COLORFX_NONE);
+	mt9m111->subdev.ctrl_handler = &mt9m111->hdl;
+	if (mt9m111->hdl.error) {
+		ret = mt9m111->hdl.error;
+		return ret;
+	}
+
+#ifdef CONFIG_MEDIA_CONTROLLER
+	mt9m111->pad.flags = MEDIA_PAD_FL_SOURCE;
+	mt9m111->subdev.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret = media_entity_pads_init(&mt9m111->subdev.entity, 1, &mt9m111->pad);
+	if (ret < 0)
+		goto out_hdlfree;
+#endif
+
+	mt9m111->current_mode = &mt9m111_mode_data[MT9M111_MODE_SXGA_15FPS];
+	mt9m111->frame_interval.numerator = 1;
+	mt9m111->frame_interval.denominator = mt9m111->current_mode->max_fps;
+
+	/* Second stage probe - when a capture adapter is there */
+	mt9m111->rect.left	= MT9M111_MIN_DARK_COLS;
+	mt9m111->rect.top	= MT9M111_MIN_DARK_ROWS;
+	mt9m111->rect.width	= MT9M111_MAX_WIDTH;
+	mt9m111->rect.height	= MT9M111_MAX_HEIGHT;
+	mt9m111->width		= mt9m111->rect.width;
+	mt9m111->height		= mt9m111->rect.height;
+	mt9m111->fmt		= &mt9m111_colour_fmts[0];
+	mt9m111->lastpage	= -1;
+	mutex_init(&mt9m111->power_lock);
+
+	ret = mt9m111_video_probe(client);
+	if (ret < 0)
+		goto out_entityclean;
+
+	mt9m111->subdev.dev = &client->dev;
+	ret = v4l2_async_register_subdev(&mt9m111->subdev);
+	if (ret < 0)
+		goto out_entityclean;
+
+	return 0;
+
+out_entityclean:
+#ifdef CONFIG_MEDIA_CONTROLLER
+	media_entity_cleanup(&mt9m111->subdev.entity);
+out_hdlfree:
+#endif
+	v4l2_ctrl_handler_free(&mt9m111->hdl);
+
+	return ret;
+}
+
+static void mt9m111_remove(struct i2c_client *client)
+{
+	struct mt9m111 *mt9m111 = to_mt9m111(client);
+
+	v4l2_async_unregister_subdev(&mt9m111->subdev);
+	media_entity_cleanup(&mt9m111->subdev.entity);
+	v4l2_ctrl_handler_free(&mt9m111->hdl);
+}
+static const struct of_device_id mt9m111_of_match[] = {
+	{ .compatible = "micron,mt9m111", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, mt9m111_of_match);
+
+static const struct i2c_device_id mt9m111_id[] = {
+	{ "mt9m111", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, mt9m111_id);
+
+static struct i2c_driver mt9m111_i2c_driver = {
+	.driver = {
+		.name = "mt9m111",
+		.of_match_table = mt9m111_of_match,
+	},
+	.probe		= mt9m111_probe,
+	.remove		= mt9m111_remove,
+	.id_table	= mt9m111_id,
+};
+
+module_i2c_driver(mt9m111_i2c_driver);
+
+MODULE_DESCRIPTION("Micron/Aptina MT9M111/MT9M112/MT9M131 Camera driver");
+MODULE_AUTHOR("Robert Jarzmik");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/mt9p031.c b/drivers/media/i2c/mt9p031.c
new file mode 100644
index 000000000..348f1e109
--- /dev/null
+++ b/drivers/media/i2c/mt9p031.c
@@ -0,0 +1,1260 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Driver for MT9P031 CMOS Image Sensor from Aptina
+ *
+ * Copyright (C) 2011, Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ * Copyright (C) 2011, Javier Martin <javier.martin@vista-silicon.com>
+ * Copyright (C) 2011, Guennadi Liakhovetski <g.liakhovetski@gmx.de>
+ *
+ * Based on the MT9V032 driver and Bastian Hecht's code.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/log2.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_graph.h>
+#include <linux/pm.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+
+#include <media/i2c/mt9p031.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+#include "aptina-pll.h"
+
+#define MT9P031_PIXEL_ARRAY_WIDTH			2752
+#define MT9P031_PIXEL_ARRAY_HEIGHT			2004
+
+#define MT9P031_CHIP_VERSION				0x00
+#define		MT9P031_CHIP_VERSION_VALUE		0x1801
+#define MT9P031_ROW_START				0x01
+#define		MT9P031_ROW_START_MIN			0
+#define		MT9P031_ROW_START_MAX			2004
+#define		MT9P031_ROW_START_DEF			54
+#define MT9P031_COLUMN_START				0x02
+#define		MT9P031_COLUMN_START_MIN		0
+#define		MT9P031_COLUMN_START_MAX		2750
+#define		MT9P031_COLUMN_START_DEF		16
+#define MT9P031_WINDOW_HEIGHT				0x03
+#define		MT9P031_WINDOW_HEIGHT_MIN		2
+#define		MT9P031_WINDOW_HEIGHT_MAX		2006
+#define		MT9P031_WINDOW_HEIGHT_DEF		1944
+#define MT9P031_WINDOW_WIDTH				0x04
+#define		MT9P031_WINDOW_WIDTH_MIN		2
+#define		MT9P031_WINDOW_WIDTH_MAX		2752
+#define		MT9P031_WINDOW_WIDTH_DEF		2592
+#define MT9P031_HORIZONTAL_BLANK			0x05
+#define		MT9P031_HORIZONTAL_BLANK_MIN		0
+#define		MT9P031_HORIZONTAL_BLANK_MAX		4095
+#define MT9P031_VERTICAL_BLANK				0x06
+#define		MT9P031_VERTICAL_BLANK_MIN		1
+#define		MT9P031_VERTICAL_BLANK_MAX		4096
+#define		MT9P031_VERTICAL_BLANK_DEF		26
+#define MT9P031_OUTPUT_CONTROL				0x07
+#define		MT9P031_OUTPUT_CONTROL_CEN		2
+#define		MT9P031_OUTPUT_CONTROL_SYN		1
+#define		MT9P031_OUTPUT_CONTROL_DEF		0x1f82
+#define MT9P031_SHUTTER_WIDTH_UPPER			0x08
+#define MT9P031_SHUTTER_WIDTH_LOWER			0x09
+#define		MT9P031_SHUTTER_WIDTH_MIN		1
+#define		MT9P031_SHUTTER_WIDTH_MAX		1048575
+#define		MT9P031_SHUTTER_WIDTH_DEF		1943
+#define	MT9P031_PLL_CONTROL				0x10
+#define		MT9P031_PLL_CONTROL_PWROFF		0x0050
+#define		MT9P031_PLL_CONTROL_PWRON		0x0051
+#define		MT9P031_PLL_CONTROL_USEPLL		0x0052
+#define	MT9P031_PLL_CONFIG_1				0x11
+#define	MT9P031_PLL_CONFIG_2				0x12
+#define MT9P031_PIXEL_CLOCK_CONTROL			0x0a
+#define		MT9P031_PIXEL_CLOCK_INVERT		BIT(15)
+#define		MT9P031_PIXEL_CLOCK_SHIFT(n)		((n) << 8)
+#define		MT9P031_PIXEL_CLOCK_DIVIDE(n)		((n) << 0)
+#define MT9P031_RESTART					0x0b
+#define		MT9P031_FRAME_PAUSE_RESTART		BIT(1)
+#define		MT9P031_FRAME_RESTART			BIT(0)
+#define MT9P031_SHUTTER_DELAY				0x0c
+#define MT9P031_RST					0x0d
+#define		MT9P031_RST_ENABLE			BIT(0)
+#define MT9P031_READ_MODE_1				0x1e
+#define MT9P031_READ_MODE_2				0x20
+#define		MT9P031_READ_MODE_2_ROW_MIR		BIT(15)
+#define		MT9P031_READ_MODE_2_COL_MIR		BIT(14)
+#define		MT9P031_READ_MODE_2_ROW_BLC		BIT(6)
+#define MT9P031_ROW_ADDRESS_MODE			0x22
+#define MT9P031_COLUMN_ADDRESS_MODE			0x23
+#define MT9P031_GLOBAL_GAIN				0x35
+#define		MT9P031_GLOBAL_GAIN_MIN			8
+#define		MT9P031_GLOBAL_GAIN_MAX			1024
+#define		MT9P031_GLOBAL_GAIN_DEF			8
+#define		MT9P031_GLOBAL_GAIN_MULT		BIT(6)
+#define MT9P031_ROW_BLACK_TARGET			0x49
+#define MT9P031_ROW_BLACK_DEF_OFFSET			0x4b
+#define MT9P031_GREEN1_OFFSET				0x60
+#define MT9P031_GREEN2_OFFSET				0x61
+#define MT9P031_BLACK_LEVEL_CALIBRATION			0x62
+#define		MT9P031_BLC_MANUAL_BLC			BIT(0)
+#define MT9P031_RED_OFFSET				0x63
+#define MT9P031_BLUE_OFFSET				0x64
+#define MT9P031_TEST_PATTERN				0xa0
+#define		MT9P031_TEST_PATTERN_SHIFT		3
+#define		MT9P031_TEST_PATTERN_ENABLE		BIT(0)
+#define MT9P031_TEST_PATTERN_GREEN			0xa1
+#define MT9P031_TEST_PATTERN_RED			0xa2
+#define MT9P031_TEST_PATTERN_BLUE			0xa3
+
+enum mt9p031_model {
+	MT9P031_MODEL_COLOR,
+	MT9P031_MODEL_MONOCHROME,
+};
+
+struct mt9p031 {
+	struct v4l2_subdev subdev;
+	struct media_pad pad;
+	struct v4l2_rect crop;  /* Sensor window */
+	struct v4l2_mbus_framefmt format;
+	struct mt9p031_platform_data *pdata;
+	struct mutex power_lock; /* lock to protect power_count */
+	int power_count;
+
+	struct clk *clk;
+	struct regulator_bulk_data regulators[3];
+
+	enum mt9p031_model model;
+	struct aptina_pll pll;
+	unsigned int clk_div;
+	bool use_pll;
+	struct gpio_desc *reset;
+
+	struct v4l2_ctrl_handler ctrls;
+	struct v4l2_ctrl *blc_auto;
+	struct v4l2_ctrl *blc_offset;
+
+	/* Registers cache */
+	u16 output_control;
+	u16 mode2;
+};
+
+static struct mt9p031 *to_mt9p031(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct mt9p031, subdev);
+}
+
+static int mt9p031_read(struct i2c_client *client, u8 reg)
+{
+	return i2c_smbus_read_word_swapped(client, reg);
+}
+
+static int mt9p031_write(struct i2c_client *client, u8 reg, u16 data)
+{
+	return i2c_smbus_write_word_swapped(client, reg, data);
+}
+
+static int mt9p031_set_output_control(struct mt9p031 *mt9p031, u16 clear,
+				      u16 set)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
+	u16 value = (mt9p031->output_control & ~clear) | set;
+	int ret;
+
+	ret = mt9p031_write(client, MT9P031_OUTPUT_CONTROL, value);
+	if (ret < 0)
+		return ret;
+
+	mt9p031->output_control = value;
+	return 0;
+}
+
+static int mt9p031_set_mode2(struct mt9p031 *mt9p031, u16 clear, u16 set)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
+	u16 value = (mt9p031->mode2 & ~clear) | set;
+	int ret;
+
+	ret = mt9p031_write(client, MT9P031_READ_MODE_2, value);
+	if (ret < 0)
+		return ret;
+
+	mt9p031->mode2 = value;
+	return 0;
+}
+
+static int mt9p031_reset(struct mt9p031 *mt9p031)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
+	int ret;
+
+	/* Disable chip output, synchronous option update */
+	ret = mt9p031_write(client, MT9P031_RST, MT9P031_RST_ENABLE);
+	if (ret < 0)
+		return ret;
+	ret = mt9p031_write(client, MT9P031_RST, 0);
+	if (ret < 0)
+		return ret;
+
+	ret = mt9p031_write(client, MT9P031_PIXEL_CLOCK_CONTROL,
+			    MT9P031_PIXEL_CLOCK_DIVIDE(mt9p031->clk_div));
+	if (ret < 0)
+		return ret;
+
+	return mt9p031_set_output_control(mt9p031, MT9P031_OUTPUT_CONTROL_CEN,
+					  0);
+}
+
+static int mt9p031_clk_setup(struct mt9p031 *mt9p031)
+{
+	static const struct aptina_pll_limits limits = {
+		.ext_clock_min = 6000000,
+		.ext_clock_max = 27000000,
+		.int_clock_min = 2000000,
+		.int_clock_max = 13500000,
+		.out_clock_min = 180000000,
+		.out_clock_max = 360000000,
+		.pix_clock_max = 96000000,
+		.n_min = 1,
+		.n_max = 64,
+		.m_min = 16,
+		.m_max = 255,
+		.p1_min = 1,
+		.p1_max = 128,
+	};
+
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
+	struct mt9p031_platform_data *pdata = mt9p031->pdata;
+	unsigned long ext_freq;
+	int ret;
+
+	mt9p031->clk = devm_clk_get(&client->dev, NULL);
+	if (IS_ERR(mt9p031->clk))
+		return PTR_ERR(mt9p031->clk);
+
+	ret = clk_set_rate(mt9p031->clk, pdata->ext_freq);
+	if (ret < 0)
+		return ret;
+
+	ext_freq = clk_get_rate(mt9p031->clk);
+
+	/* If the external clock frequency is out of bounds for the PLL use the
+	 * pixel clock divider only and disable the PLL.
+	 */
+	if (ext_freq > limits.ext_clock_max) {
+		unsigned int div;
+
+		div = DIV_ROUND_UP(ext_freq, pdata->target_freq);
+		div = roundup_pow_of_two(div) / 2;
+
+		mt9p031->clk_div = min_t(unsigned int, div, 64);
+		mt9p031->use_pll = false;
+
+		return 0;
+	}
+
+	mt9p031->pll.ext_clock = ext_freq;
+	mt9p031->pll.pix_clock = pdata->target_freq;
+	mt9p031->use_pll = true;
+
+	return aptina_pll_calculate(&client->dev, &limits, &mt9p031->pll);
+}
+
+static int mt9p031_pll_enable(struct mt9p031 *mt9p031)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
+	int ret;
+
+	if (!mt9p031->use_pll)
+		return 0;
+
+	ret = mt9p031_write(client, MT9P031_PLL_CONTROL,
+			    MT9P031_PLL_CONTROL_PWRON);
+	if (ret < 0)
+		return ret;
+
+	ret = mt9p031_write(client, MT9P031_PLL_CONFIG_1,
+			    (mt9p031->pll.m << 8) | (mt9p031->pll.n - 1));
+	if (ret < 0)
+		return ret;
+
+	ret = mt9p031_write(client, MT9P031_PLL_CONFIG_2, mt9p031->pll.p1 - 1);
+	if (ret < 0)
+		return ret;
+
+	usleep_range(1000, 2000);
+	ret = mt9p031_write(client, MT9P031_PLL_CONTROL,
+			    MT9P031_PLL_CONTROL_PWRON |
+			    MT9P031_PLL_CONTROL_USEPLL);
+	return ret;
+}
+
+static inline int mt9p031_pll_disable(struct mt9p031 *mt9p031)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
+
+	if (!mt9p031->use_pll)
+		return 0;
+
+	return mt9p031_write(client, MT9P031_PLL_CONTROL,
+			     MT9P031_PLL_CONTROL_PWROFF);
+}
+
+static int mt9p031_power_on(struct mt9p031 *mt9p031)
+{
+	unsigned long rate, delay;
+	int ret;
+
+	/* Ensure RESET_BAR is active */
+	if (mt9p031->reset) {
+		gpiod_set_value(mt9p031->reset, 1);
+		usleep_range(1000, 2000);
+	}
+
+	/* Bring up the supplies */
+	ret = regulator_bulk_enable(ARRAY_SIZE(mt9p031->regulators),
+				   mt9p031->regulators);
+	if (ret < 0)
+		return ret;
+
+	/* Enable clock */
+	if (mt9p031->clk) {
+		ret = clk_prepare_enable(mt9p031->clk);
+		if (ret) {
+			regulator_bulk_disable(ARRAY_SIZE(mt9p031->regulators),
+					       mt9p031->regulators);
+			return ret;
+		}
+	}
+
+	/* Now RESET_BAR must be high */
+	if (mt9p031->reset) {
+		gpiod_set_value(mt9p031->reset, 0);
+		/* Wait 850000 EXTCLK cycles before de-asserting reset. */
+		rate = clk_get_rate(mt9p031->clk);
+		if (!rate)
+			rate = 6000000;	/* Slowest supported clock, 6 MHz */
+		delay = DIV_ROUND_UP(850000 * 1000, rate);
+		msleep(delay);
+	}
+
+	return 0;
+}
+
+static void mt9p031_power_off(struct mt9p031 *mt9p031)
+{
+	if (mt9p031->reset) {
+		gpiod_set_value(mt9p031->reset, 1);
+		usleep_range(1000, 2000);
+	}
+
+	regulator_bulk_disable(ARRAY_SIZE(mt9p031->regulators),
+			       mt9p031->regulators);
+
+	clk_disable_unprepare(mt9p031->clk);
+}
+
+static int __mt9p031_set_power(struct mt9p031 *mt9p031, bool on)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
+	int ret;
+
+	if (!on) {
+		mt9p031_power_off(mt9p031);
+		return 0;
+	}
+
+	ret = mt9p031_power_on(mt9p031);
+	if (ret < 0)
+		return ret;
+
+	ret = mt9p031_reset(mt9p031);
+	if (ret < 0) {
+		dev_err(&client->dev, "Failed to reset the camera\n");
+		return ret;
+	}
+
+	/* Configure the pixel clock polarity */
+	if (mt9p031->pdata && mt9p031->pdata->pixclk_pol) {
+		ret = mt9p031_write(client, MT9P031_PIXEL_CLOCK_CONTROL,
+				MT9P031_PIXEL_CLOCK_INVERT);
+		if (ret < 0)
+			return ret;
+	}
+
+	return v4l2_ctrl_handler_setup(&mt9p031->ctrls);
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev video operations
+ */
+
+static int mt9p031_set_params(struct mt9p031 *mt9p031)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
+	struct v4l2_mbus_framefmt *format = &mt9p031->format;
+	const struct v4l2_rect *crop = &mt9p031->crop;
+	unsigned int hblank;
+	unsigned int vblank;
+	unsigned int xskip;
+	unsigned int yskip;
+	unsigned int xbin;
+	unsigned int ybin;
+	int ret;
+
+	/* Windows position and size.
+	 *
+	 * TODO: Make sure the start coordinates and window size match the
+	 * skipping, binning and mirroring (see description of registers 2 and 4
+	 * in table 13, and Binning section on page 41).
+	 */
+	ret = mt9p031_write(client, MT9P031_COLUMN_START, crop->left);
+	if (ret < 0)
+		return ret;
+	ret = mt9p031_write(client, MT9P031_ROW_START, crop->top);
+	if (ret < 0)
+		return ret;
+	ret = mt9p031_write(client, MT9P031_WINDOW_WIDTH, crop->width - 1);
+	if (ret < 0)
+		return ret;
+	ret = mt9p031_write(client, MT9P031_WINDOW_HEIGHT, crop->height - 1);
+	if (ret < 0)
+		return ret;
+
+	/* Row and column binning and skipping. Use the maximum binning value
+	 * compatible with the skipping settings.
+	 */
+	xskip = DIV_ROUND_CLOSEST(crop->width, format->width);
+	yskip = DIV_ROUND_CLOSEST(crop->height, format->height);
+	xbin = 1 << (ffs(xskip) - 1);
+	ybin = 1 << (ffs(yskip) - 1);
+
+	ret = mt9p031_write(client, MT9P031_COLUMN_ADDRESS_MODE,
+			    ((xbin - 1) << 4) | (xskip - 1));
+	if (ret < 0)
+		return ret;
+	ret = mt9p031_write(client, MT9P031_ROW_ADDRESS_MODE,
+			    ((ybin - 1) << 4) | (yskip - 1));
+	if (ret < 0)
+		return ret;
+
+	/* Blanking - use minimum value for horizontal blanking and default
+	 * value for vertical blanking.
+	 */
+	hblank = 346 * ybin + 64 + (80 >> min_t(unsigned int, xbin, 3));
+	vblank = MT9P031_VERTICAL_BLANK_DEF;
+
+	ret = mt9p031_write(client, MT9P031_HORIZONTAL_BLANK, hblank - 1);
+	if (ret < 0)
+		return ret;
+	ret = mt9p031_write(client, MT9P031_VERTICAL_BLANK, vblank - 1);
+	if (ret < 0)
+		return ret;
+
+	return ret;
+}
+
+static int mt9p031_s_stream(struct v4l2_subdev *subdev, int enable)
+{
+	struct mt9p031 *mt9p031 = to_mt9p031(subdev);
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
+	int val;
+	int ret;
+
+	if (!enable) {
+		/* enable pause restart */
+		val = MT9P031_FRAME_PAUSE_RESTART;
+		ret = mt9p031_write(client, MT9P031_RESTART, val);
+		if (ret < 0)
+			return ret;
+
+		/* enable restart + keep pause restart set */
+		val |= MT9P031_FRAME_RESTART;
+		ret = mt9p031_write(client, MT9P031_RESTART, val);
+		if (ret < 0)
+			return ret;
+
+		/* Stop sensor readout */
+		ret = mt9p031_set_output_control(mt9p031,
+						 MT9P031_OUTPUT_CONTROL_CEN, 0);
+		if (ret < 0)
+			return ret;
+
+		return mt9p031_pll_disable(mt9p031);
+	}
+
+	ret = mt9p031_set_params(mt9p031);
+	if (ret < 0)
+		return ret;
+
+	/* Switch to master "normal" mode */
+	ret = mt9p031_set_output_control(mt9p031, 0,
+					 MT9P031_OUTPUT_CONTROL_CEN);
+	if (ret < 0)
+		return ret;
+
+	/*
+	 * - clear pause restart
+	 * - don't clear restart as clearing restart manually can cause
+	 *   undefined behavior
+	 */
+	val = MT9P031_FRAME_RESTART;
+	ret = mt9p031_write(client, MT9P031_RESTART, val);
+	if (ret < 0)
+		return ret;
+
+	return mt9p031_pll_enable(mt9p031);
+}
+
+static int mt9p031_enum_mbus_code(struct v4l2_subdev *subdev,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct mt9p031 *mt9p031 = to_mt9p031(subdev);
+
+	if (code->pad || code->index)
+		return -EINVAL;
+
+	code->code = mt9p031->format.code;
+	return 0;
+}
+
+static int mt9p031_enum_frame_size(struct v4l2_subdev *subdev,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct mt9p031 *mt9p031 = to_mt9p031(subdev);
+
+	if (fse->index >= 8 || fse->code != mt9p031->format.code)
+		return -EINVAL;
+
+	fse->min_width = MT9P031_WINDOW_WIDTH_DEF
+		       / min_t(unsigned int, 7, fse->index + 1);
+	fse->max_width = fse->min_width;
+	fse->min_height = MT9P031_WINDOW_HEIGHT_DEF / (fse->index + 1);
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static struct v4l2_mbus_framefmt *
+__mt9p031_get_pad_format(struct mt9p031 *mt9p031,
+			 struct v4l2_subdev_state *sd_state,
+			 unsigned int pad, u32 which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_format(&mt9p031->subdev, sd_state,
+						  pad);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return &mt9p031->format;
+	default:
+		return NULL;
+	}
+}
+
+static struct v4l2_rect *
+__mt9p031_get_pad_crop(struct mt9p031 *mt9p031,
+		       struct v4l2_subdev_state *sd_state,
+		       unsigned int pad, u32 which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_crop(&mt9p031->subdev, sd_state,
+						pad);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return &mt9p031->crop;
+	default:
+		return NULL;
+	}
+}
+
+static int mt9p031_get_format(struct v4l2_subdev *subdev,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_format *fmt)
+{
+	struct mt9p031 *mt9p031 = to_mt9p031(subdev);
+
+	fmt->format = *__mt9p031_get_pad_format(mt9p031, sd_state, fmt->pad,
+						fmt->which);
+	return 0;
+}
+
+static int mt9p031_set_format(struct v4l2_subdev *subdev,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_format *format)
+{
+	struct mt9p031 *mt9p031 = to_mt9p031(subdev);
+	struct v4l2_mbus_framefmt *__format;
+	struct v4l2_rect *__crop;
+	unsigned int width;
+	unsigned int height;
+	unsigned int hratio;
+	unsigned int vratio;
+
+	__crop = __mt9p031_get_pad_crop(mt9p031, sd_state, format->pad,
+					format->which);
+
+	/* Clamp the width and height to avoid dividing by zero. */
+	width = clamp_t(unsigned int, ALIGN(format->format.width, 2),
+			max_t(unsigned int, __crop->width / 7,
+			      MT9P031_WINDOW_WIDTH_MIN),
+			__crop->width);
+	height = clamp_t(unsigned int, ALIGN(format->format.height, 2),
+			 max_t(unsigned int, __crop->height / 8,
+			       MT9P031_WINDOW_HEIGHT_MIN),
+			 __crop->height);
+
+	hratio = DIV_ROUND_CLOSEST(__crop->width, width);
+	vratio = DIV_ROUND_CLOSEST(__crop->height, height);
+
+	__format = __mt9p031_get_pad_format(mt9p031, sd_state, format->pad,
+					    format->which);
+	__format->width = __crop->width / hratio;
+	__format->height = __crop->height / vratio;
+
+	format->format = *__format;
+
+	return 0;
+}
+
+static int mt9p031_get_selection(struct v4l2_subdev *subdev,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_selection *sel)
+{
+	struct mt9p031 *mt9p031 = to_mt9p031(subdev);
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.left = MT9P031_COLUMN_START_MIN;
+		sel->r.top = MT9P031_ROW_START_MIN;
+		sel->r.width = MT9P031_WINDOW_WIDTH_MAX;
+		sel->r.height = MT9P031_WINDOW_HEIGHT_MAX;
+		return 0;
+
+	case V4L2_SEL_TGT_CROP:
+		sel->r = *__mt9p031_get_pad_crop(mt9p031, sd_state,
+						 sel->pad, sel->which);
+		return 0;
+
+	default:
+		return -EINVAL;
+	}
+}
+
+static int mt9p031_set_selection(struct v4l2_subdev *subdev,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_selection *sel)
+{
+	struct mt9p031 *mt9p031 = to_mt9p031(subdev);
+	struct v4l2_mbus_framefmt *__format;
+	struct v4l2_rect *__crop;
+	struct v4l2_rect rect;
+
+	if (sel->target != V4L2_SEL_TGT_CROP)
+		return -EINVAL;
+
+	/* Clamp the crop rectangle boundaries and align them to a multiple of 2
+	 * pixels to ensure a GRBG Bayer pattern.
+	 */
+	rect.left = clamp(ALIGN(sel->r.left, 2), MT9P031_COLUMN_START_MIN,
+			  MT9P031_COLUMN_START_MAX);
+	rect.top = clamp(ALIGN(sel->r.top, 2), MT9P031_ROW_START_MIN,
+			 MT9P031_ROW_START_MAX);
+	rect.width = clamp_t(unsigned int, ALIGN(sel->r.width, 2),
+			     MT9P031_WINDOW_WIDTH_MIN,
+			     MT9P031_WINDOW_WIDTH_MAX);
+	rect.height = clamp_t(unsigned int, ALIGN(sel->r.height, 2),
+			      MT9P031_WINDOW_HEIGHT_MIN,
+			      MT9P031_WINDOW_HEIGHT_MAX);
+
+	rect.width = min_t(unsigned int, rect.width,
+			   MT9P031_PIXEL_ARRAY_WIDTH - rect.left);
+	rect.height = min_t(unsigned int, rect.height,
+			    MT9P031_PIXEL_ARRAY_HEIGHT - rect.top);
+
+	__crop = __mt9p031_get_pad_crop(mt9p031, sd_state, sel->pad,
+					sel->which);
+
+	if (rect.width != __crop->width || rect.height != __crop->height) {
+		/* Reset the output image size if the crop rectangle size has
+		 * been modified.
+		 */
+		__format = __mt9p031_get_pad_format(mt9p031, sd_state,
+						    sel->pad,
+						    sel->which);
+		__format->width = rect.width;
+		__format->height = rect.height;
+	}
+
+	*__crop = rect;
+	sel->r = rect;
+
+	return 0;
+}
+
+static int mt9p031_init_cfg(struct v4l2_subdev *subdev,
+			    struct v4l2_subdev_state *sd_state)
+{
+	struct mt9p031 *mt9p031 = to_mt9p031(subdev);
+	struct v4l2_mbus_framefmt *format;
+	struct v4l2_rect *crop;
+	const int which = sd_state == NULL ? V4L2_SUBDEV_FORMAT_ACTIVE :
+					     V4L2_SUBDEV_FORMAT_TRY;
+
+	crop = __mt9p031_get_pad_crop(mt9p031, sd_state, 0, which);
+	crop->left = MT9P031_COLUMN_START_DEF;
+	crop->top = MT9P031_ROW_START_DEF;
+	crop->width = MT9P031_WINDOW_WIDTH_DEF;
+	crop->height = MT9P031_WINDOW_HEIGHT_DEF;
+
+	format = __mt9p031_get_pad_format(mt9p031, sd_state, 0, which);
+
+	if (mt9p031->model == MT9P031_MODEL_MONOCHROME)
+		format->code = MEDIA_BUS_FMT_Y12_1X12;
+	else
+		format->code = MEDIA_BUS_FMT_SGRBG12_1X12;
+
+	format->width = MT9P031_WINDOW_WIDTH_DEF;
+	format->height = MT9P031_WINDOW_HEIGHT_DEF;
+	format->field = V4L2_FIELD_NONE;
+	format->colorspace = V4L2_COLORSPACE_SRGB;
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev control operations
+ */
+
+#define V4L2_CID_BLC_AUTO		(V4L2_CID_USER_BASE | 0x1002)
+#define V4L2_CID_BLC_TARGET_LEVEL	(V4L2_CID_USER_BASE | 0x1003)
+#define V4L2_CID_BLC_ANALOG_OFFSET	(V4L2_CID_USER_BASE | 0x1004)
+#define V4L2_CID_BLC_DIGITAL_OFFSET	(V4L2_CID_USER_BASE | 0x1005)
+
+static int mt9p031_restore_blc(struct mt9p031 *mt9p031)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
+	int ret;
+
+	if (mt9p031->blc_auto->cur.val != 0) {
+		ret = mt9p031_set_mode2(mt9p031, 0,
+					MT9P031_READ_MODE_2_ROW_BLC);
+		if (ret < 0)
+			return ret;
+	}
+
+	if (mt9p031->blc_offset->cur.val != 0) {
+		ret = mt9p031_write(client, MT9P031_ROW_BLACK_TARGET,
+				    mt9p031->blc_offset->cur.val);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int mt9p031_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct mt9p031 *mt9p031 =
+			container_of(ctrl->handler, struct mt9p031, ctrls);
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
+	u16 data;
+	int ret;
+
+	if (ctrl->flags & V4L2_CTRL_FLAG_INACTIVE)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE:
+		ret = mt9p031_write(client, MT9P031_SHUTTER_WIDTH_UPPER,
+				    (ctrl->val >> 16) & 0xffff);
+		if (ret < 0)
+			return ret;
+
+		return mt9p031_write(client, MT9P031_SHUTTER_WIDTH_LOWER,
+				     ctrl->val & 0xffff);
+
+	case V4L2_CID_GAIN:
+		/* Gain is controlled by 2 analog stages and a digital stage.
+		 * Valid values for the 3 stages are
+		 *
+		 * Stage                Min     Max     Step
+		 * ------------------------------------------
+		 * First analog stage   x1      x2      1
+		 * Second analog stage  x1      x4      0.125
+		 * Digital stage        x1      x16     0.125
+		 *
+		 * To minimize noise, the gain stages should be used in the
+		 * second analog stage, first analog stage, digital stage order.
+		 * Gain from a previous stage should be pushed to its maximum
+		 * value before the next stage is used.
+		 */
+		if (ctrl->val <= 32) {
+			data = ctrl->val;
+		} else if (ctrl->val <= 64) {
+			ctrl->val &= ~1;
+			data = (1 << 6) | (ctrl->val >> 1);
+		} else {
+			ctrl->val &= ~7;
+			data = ((ctrl->val - 64) << 5) | (1 << 6) | 32;
+		}
+
+		return mt9p031_write(client, MT9P031_GLOBAL_GAIN, data);
+
+	case V4L2_CID_HFLIP:
+		if (ctrl->val)
+			return mt9p031_set_mode2(mt9p031,
+					0, MT9P031_READ_MODE_2_COL_MIR);
+		else
+			return mt9p031_set_mode2(mt9p031,
+					MT9P031_READ_MODE_2_COL_MIR, 0);
+
+	case V4L2_CID_VFLIP:
+		if (ctrl->val)
+			return mt9p031_set_mode2(mt9p031,
+					0, MT9P031_READ_MODE_2_ROW_MIR);
+		else
+			return mt9p031_set_mode2(mt9p031,
+					MT9P031_READ_MODE_2_ROW_MIR, 0);
+
+	case V4L2_CID_TEST_PATTERN:
+		/* The digital side of the Black Level Calibration function must
+		 * be disabled when generating a test pattern to avoid artifacts
+		 * in the image. Activate (deactivate) the BLC-related controls
+		 * when the test pattern is enabled (disabled).
+		 */
+		v4l2_ctrl_activate(mt9p031->blc_auto, ctrl->val == 0);
+		v4l2_ctrl_activate(mt9p031->blc_offset, ctrl->val == 0);
+
+		if (!ctrl->val) {
+			/* Restore the BLC settings. */
+			ret = mt9p031_restore_blc(mt9p031);
+			if (ret < 0)
+				return ret;
+
+			return mt9p031_write(client, MT9P031_TEST_PATTERN, 0);
+		}
+
+		ret = mt9p031_write(client, MT9P031_TEST_PATTERN_GREEN, 0x05a0);
+		if (ret < 0)
+			return ret;
+		ret = mt9p031_write(client, MT9P031_TEST_PATTERN_RED, 0x0a50);
+		if (ret < 0)
+			return ret;
+		ret = mt9p031_write(client, MT9P031_TEST_PATTERN_BLUE, 0x0aa0);
+		if (ret < 0)
+			return ret;
+
+		/* Disable digital BLC when generating a test pattern. */
+		ret = mt9p031_set_mode2(mt9p031, MT9P031_READ_MODE_2_ROW_BLC,
+					0);
+		if (ret < 0)
+			return ret;
+
+		ret = mt9p031_write(client, MT9P031_ROW_BLACK_DEF_OFFSET, 0);
+		if (ret < 0)
+			return ret;
+
+		return mt9p031_write(client, MT9P031_TEST_PATTERN,
+				((ctrl->val - 1) << MT9P031_TEST_PATTERN_SHIFT)
+				| MT9P031_TEST_PATTERN_ENABLE);
+
+	case V4L2_CID_BLC_AUTO:
+		ret = mt9p031_set_mode2(mt9p031,
+				ctrl->val ? 0 : MT9P031_READ_MODE_2_ROW_BLC,
+				ctrl->val ? MT9P031_READ_MODE_2_ROW_BLC : 0);
+		if (ret < 0)
+			return ret;
+
+		return mt9p031_write(client, MT9P031_BLACK_LEVEL_CALIBRATION,
+				     ctrl->val ? 0 : MT9P031_BLC_MANUAL_BLC);
+
+	case V4L2_CID_BLC_TARGET_LEVEL:
+		return mt9p031_write(client, MT9P031_ROW_BLACK_TARGET,
+				     ctrl->val);
+
+	case V4L2_CID_BLC_ANALOG_OFFSET:
+		data = ctrl->val & ((1 << 9) - 1);
+
+		ret = mt9p031_write(client, MT9P031_GREEN1_OFFSET, data);
+		if (ret < 0)
+			return ret;
+		ret = mt9p031_write(client, MT9P031_GREEN2_OFFSET, data);
+		if (ret < 0)
+			return ret;
+		ret = mt9p031_write(client, MT9P031_RED_OFFSET, data);
+		if (ret < 0)
+			return ret;
+		return mt9p031_write(client, MT9P031_BLUE_OFFSET, data);
+
+	case V4L2_CID_BLC_DIGITAL_OFFSET:
+		return mt9p031_write(client, MT9P031_ROW_BLACK_DEF_OFFSET,
+				     ctrl->val & ((1 << 12) - 1));
+	}
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops mt9p031_ctrl_ops = {
+	.s_ctrl = mt9p031_s_ctrl,
+};
+
+static const char * const mt9p031_test_pattern_menu[] = {
+	"Disabled",
+	"Color Field",
+	"Horizontal Gradient",
+	"Vertical Gradient",
+	"Diagonal Gradient",
+	"Classic Test Pattern",
+	"Walking 1s",
+	"Monochrome Horizontal Bars",
+	"Monochrome Vertical Bars",
+	"Vertical Color Bars",
+};
+
+static const struct v4l2_ctrl_config mt9p031_ctrls[] = {
+	{
+		.ops		= &mt9p031_ctrl_ops,
+		.id		= V4L2_CID_BLC_AUTO,
+		.type		= V4L2_CTRL_TYPE_BOOLEAN,
+		.name		= "BLC, Auto",
+		.min		= 0,
+		.max		= 1,
+		.step		= 1,
+		.def		= 1,
+		.flags		= 0,
+	}, {
+		.ops		= &mt9p031_ctrl_ops,
+		.id		= V4L2_CID_BLC_TARGET_LEVEL,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "BLC Target Level",
+		.min		= 0,
+		.max		= 4095,
+		.step		= 1,
+		.def		= 168,
+		.flags		= 0,
+	}, {
+		.ops		= &mt9p031_ctrl_ops,
+		.id		= V4L2_CID_BLC_ANALOG_OFFSET,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "BLC Analog Offset",
+		.min		= -255,
+		.max		= 255,
+		.step		= 1,
+		.def		= 32,
+		.flags		= 0,
+	}, {
+		.ops		= &mt9p031_ctrl_ops,
+		.id		= V4L2_CID_BLC_DIGITAL_OFFSET,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "BLC Digital Offset",
+		.min		= -2048,
+		.max		= 2047,
+		.step		= 1,
+		.def		= 40,
+		.flags		= 0,
+	}
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev core operations
+ */
+
+static int mt9p031_set_power(struct v4l2_subdev *subdev, int on)
+{
+	struct mt9p031 *mt9p031 = to_mt9p031(subdev);
+	int ret = 0;
+
+	mutex_lock(&mt9p031->power_lock);
+
+	/* If the power count is modified from 0 to != 0 or from != 0 to 0,
+	 * update the power state.
+	 */
+	if (mt9p031->power_count == !on) {
+		ret = __mt9p031_set_power(mt9p031, !!on);
+		if (ret < 0)
+			goto out;
+	}
+
+	/* Update the power count. */
+	mt9p031->power_count += on ? 1 : -1;
+	WARN_ON(mt9p031->power_count < 0);
+
+out:
+	mutex_unlock(&mt9p031->power_lock);
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev internal operations
+ */
+
+static int mt9p031_registered(struct v4l2_subdev *subdev)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
+	struct mt9p031 *mt9p031 = to_mt9p031(subdev);
+	s32 data;
+	int ret;
+
+	ret = mt9p031_power_on(mt9p031);
+	if (ret < 0) {
+		dev_err(&client->dev, "MT9P031 power up failed\n");
+		return ret;
+	}
+
+	/* Read out the chip version register */
+	data = mt9p031_read(client, MT9P031_CHIP_VERSION);
+	mt9p031_power_off(mt9p031);
+
+	if (data != MT9P031_CHIP_VERSION_VALUE) {
+		dev_err(&client->dev, "MT9P031 not detected, wrong version "
+			"0x%04x\n", data);
+		return -ENODEV;
+	}
+
+	dev_info(&client->dev, "MT9P031 detected at address 0x%02x\n",
+		 client->addr);
+
+	return 0;
+}
+
+static int mt9p031_open(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
+{
+	return mt9p031_set_power(subdev, 1);
+}
+
+static int mt9p031_close(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
+{
+	return mt9p031_set_power(subdev, 0);
+}
+
+static const struct v4l2_subdev_core_ops mt9p031_subdev_core_ops = {
+	.s_power        = mt9p031_set_power,
+};
+
+static const struct v4l2_subdev_video_ops mt9p031_subdev_video_ops = {
+	.s_stream       = mt9p031_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops mt9p031_subdev_pad_ops = {
+	.init_cfg = mt9p031_init_cfg,
+	.enum_mbus_code = mt9p031_enum_mbus_code,
+	.enum_frame_size = mt9p031_enum_frame_size,
+	.get_fmt = mt9p031_get_format,
+	.set_fmt = mt9p031_set_format,
+	.get_selection = mt9p031_get_selection,
+	.set_selection = mt9p031_set_selection,
+};
+
+static const struct v4l2_subdev_ops mt9p031_subdev_ops = {
+	.core   = &mt9p031_subdev_core_ops,
+	.video  = &mt9p031_subdev_video_ops,
+	.pad    = &mt9p031_subdev_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops mt9p031_subdev_internal_ops = {
+	.registered = mt9p031_registered,
+	.open = mt9p031_open,
+	.close = mt9p031_close,
+};
+
+/* -----------------------------------------------------------------------------
+ * Driver initialization and probing
+ */
+
+static struct mt9p031_platform_data *
+mt9p031_get_pdata(struct i2c_client *client)
+{
+	struct mt9p031_platform_data *pdata = NULL;
+	struct device_node *np;
+	struct v4l2_fwnode_endpoint endpoint = {
+		.bus_type = V4L2_MBUS_PARALLEL
+	};
+
+	if (!IS_ENABLED(CONFIG_OF) || !client->dev.of_node)
+		return client->dev.platform_data;
+
+	np = of_graph_get_next_endpoint(client->dev.of_node, NULL);
+	if (!np)
+		return NULL;
+
+	if (v4l2_fwnode_endpoint_parse(of_fwnode_handle(np), &endpoint) < 0)
+		goto done;
+
+	pdata = devm_kzalloc(&client->dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		goto done;
+
+	of_property_read_u32(np, "input-clock-frequency", &pdata->ext_freq);
+	of_property_read_u32(np, "pixel-clock-frequency", &pdata->target_freq);
+
+	pdata->pixclk_pol = !!(endpoint.bus.parallel.flags &
+			       V4L2_MBUS_PCLK_SAMPLE_RISING);
+
+done:
+	of_node_put(np);
+	return pdata;
+}
+
+static int mt9p031_probe(struct i2c_client *client)
+{
+	const struct i2c_device_id *did = i2c_client_get_device_id(client);
+	struct mt9p031_platform_data *pdata = mt9p031_get_pdata(client);
+	struct i2c_adapter *adapter = client->adapter;
+	struct mt9p031 *mt9p031;
+	unsigned int i;
+	int ret;
+
+	if (pdata == NULL) {
+		dev_err(&client->dev, "No platform data\n");
+		return -EINVAL;
+	}
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
+		dev_warn(&client->dev,
+			"I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
+		return -EIO;
+	}
+
+	mt9p031 = devm_kzalloc(&client->dev, sizeof(*mt9p031), GFP_KERNEL);
+	if (mt9p031 == NULL)
+		return -ENOMEM;
+
+	mt9p031->pdata = pdata;
+	mt9p031->output_control	= MT9P031_OUTPUT_CONTROL_DEF;
+	mt9p031->mode2 = MT9P031_READ_MODE_2_ROW_BLC;
+	mt9p031->model = did->driver_data;
+
+	mt9p031->regulators[0].supply = "vdd";
+	mt9p031->regulators[1].supply = "vdd_io";
+	mt9p031->regulators[2].supply = "vaa";
+
+	ret = devm_regulator_bulk_get(&client->dev, 3, mt9p031->regulators);
+	if (ret < 0) {
+		dev_err(&client->dev, "Unable to get regulators\n");
+		return ret;
+	}
+
+	mutex_init(&mt9p031->power_lock);
+
+	v4l2_ctrl_handler_init(&mt9p031->ctrls, ARRAY_SIZE(mt9p031_ctrls) + 6);
+
+	v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
+			  V4L2_CID_EXPOSURE, MT9P031_SHUTTER_WIDTH_MIN,
+			  MT9P031_SHUTTER_WIDTH_MAX, 1,
+			  MT9P031_SHUTTER_WIDTH_DEF);
+	v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
+			  V4L2_CID_GAIN, MT9P031_GLOBAL_GAIN_MIN,
+			  MT9P031_GLOBAL_GAIN_MAX, 1, MT9P031_GLOBAL_GAIN_DEF);
+	v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
+			  V4L2_CID_HFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
+			  V4L2_CID_VFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
+			  V4L2_CID_PIXEL_RATE, pdata->target_freq,
+			  pdata->target_freq, 1, pdata->target_freq);
+	v4l2_ctrl_new_std_menu_items(&mt9p031->ctrls, &mt9p031_ctrl_ops,
+			  V4L2_CID_TEST_PATTERN,
+			  ARRAY_SIZE(mt9p031_test_pattern_menu) - 1, 0,
+			  0, mt9p031_test_pattern_menu);
+
+	for (i = 0; i < ARRAY_SIZE(mt9p031_ctrls); ++i)
+		v4l2_ctrl_new_custom(&mt9p031->ctrls, &mt9p031_ctrls[i], NULL);
+
+	mt9p031->subdev.ctrl_handler = &mt9p031->ctrls;
+
+	if (mt9p031->ctrls.error) {
+		printk(KERN_INFO "%s: control initialization error %d\n",
+		       __func__, mt9p031->ctrls.error);
+		ret = mt9p031->ctrls.error;
+		goto done;
+	}
+
+	mt9p031->blc_auto = v4l2_ctrl_find(&mt9p031->ctrls, V4L2_CID_BLC_AUTO);
+	mt9p031->blc_offset = v4l2_ctrl_find(&mt9p031->ctrls,
+					     V4L2_CID_BLC_DIGITAL_OFFSET);
+
+	v4l2_i2c_subdev_init(&mt9p031->subdev, client, &mt9p031_subdev_ops);
+	mt9p031->subdev.internal_ops = &mt9p031_subdev_internal_ops;
+
+	mt9p031->subdev.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	mt9p031->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&mt9p031->subdev.entity, 1, &mt9p031->pad);
+	if (ret < 0)
+		goto done;
+
+	mt9p031->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	ret = mt9p031_init_cfg(&mt9p031->subdev, NULL);
+	if (ret)
+		goto done;
+
+	mt9p031->reset = devm_gpiod_get_optional(&client->dev, "reset",
+						 GPIOD_OUT_HIGH);
+
+	ret = mt9p031_clk_setup(mt9p031);
+	if (ret)
+		goto done;
+
+	ret = v4l2_async_register_subdev(&mt9p031->subdev);
+
+done:
+	if (ret < 0) {
+		v4l2_ctrl_handler_free(&mt9p031->ctrls);
+		media_entity_cleanup(&mt9p031->subdev.entity);
+		mutex_destroy(&mt9p031->power_lock);
+	}
+
+	return ret;
+}
+
+static void mt9p031_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct mt9p031 *mt9p031 = to_mt9p031(subdev);
+
+	v4l2_ctrl_handler_free(&mt9p031->ctrls);
+	v4l2_async_unregister_subdev(subdev);
+	media_entity_cleanup(&subdev->entity);
+	mutex_destroy(&mt9p031->power_lock);
+}
+
+static const struct i2c_device_id mt9p031_id[] = {
+	{ "mt9p006", MT9P031_MODEL_COLOR },
+	{ "mt9p031", MT9P031_MODEL_COLOR },
+	{ "mt9p031m", MT9P031_MODEL_MONOCHROME },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, mt9p031_id);
+
+#if IS_ENABLED(CONFIG_OF)
+static const struct of_device_id mt9p031_of_match[] = {
+	{ .compatible = "aptina,mt9p006", },
+	{ .compatible = "aptina,mt9p031", },
+	{ .compatible = "aptina,mt9p031m", },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, mt9p031_of_match);
+#endif
+
+static struct i2c_driver mt9p031_i2c_driver = {
+	.driver = {
+		.of_match_table = of_match_ptr(mt9p031_of_match),
+		.name = "mt9p031",
+	},
+	.probe          = mt9p031_probe,
+	.remove         = mt9p031_remove,
+	.id_table       = mt9p031_id,
+};
+
+module_i2c_driver(mt9p031_i2c_driver);
+
+MODULE_DESCRIPTION("Aptina MT9P031 Camera driver");
+MODULE_AUTHOR("Bastian Hecht <hechtb@gmail.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/mt9t112.c b/drivers/media/i2c/mt9t112.c
new file mode 100644
index 000000000..93f34b767
--- /dev/null
+++ b/drivers/media/i2c/mt9t112.c
@@ -0,0 +1,1131 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * mt9t112 Camera Driver
+ *
+ * Copyright (C) 2018 Jacopo Mondi <jacopo+renesas@jmondi.org>
+ *
+ * Copyright (C) 2009 Renesas Solutions Corp.
+ * Kuninori Morimoto <morimoto.kuninori@renesas.com>
+ *
+ * Based on ov772x driver, mt9m111 driver,
+ *
+ * Copyright (C) 2008 Kuninori Morimoto <morimoto.kuninori@renesas.com>
+ * Copyright (C) 2008, Robert Jarzmik <robert.jarzmik@free.fr>
+ * Copyright 2006-7 Jonathan Corbet <corbet@lwn.net>
+ * Copyright (C) 2008 Magnus Damm
+ * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
+ *
+ * TODO: This driver lacks support for frame rate control due to missing
+ *	 register level documentation and suitable hardware for testing.
+ *	 v4l-utils compliance tools will report errors.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/v4l2-mediabus.h>
+#include <linux/videodev2.h>
+
+#include <media/i2c/mt9t112.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-image-sizes.h>
+#include <media/v4l2-subdev.h>
+
+/* you can check PLL/clock info */
+/* #define EXT_CLOCK 24000000 */
+
+/************************************************************************
+ *			macro
+ ***********************************************************************/
+/*
+ * frame size
+ */
+#define MAX_WIDTH   2048
+#define MAX_HEIGHT  1536
+
+/*
+ * macro of read/write
+ */
+#define ECHECKER(ret, x)		\
+	do {				\
+		(ret) = (x);		\
+		if ((ret) < 0)		\
+			return (ret);	\
+	} while (0)
+
+#define mt9t112_reg_write(ret, client, a, b) \
+	ECHECKER(ret, __mt9t112_reg_write(client, a, b))
+#define mt9t112_mcu_write(ret, client, a, b) \
+	ECHECKER(ret, __mt9t112_mcu_write(client, a, b))
+
+#define mt9t112_reg_mask_set(ret, client, a, b, c) \
+	ECHECKER(ret, __mt9t112_reg_mask_set(client, a, b, c))
+#define mt9t112_mcu_mask_set(ret, client, a, b, c) \
+	ECHECKER(ret, __mt9t112_mcu_mask_set(client, a, b, c))
+
+#define mt9t112_reg_read(ret, client, a) \
+	ECHECKER(ret, __mt9t112_reg_read(client, a))
+
+/*
+ * Logical address
+ */
+#define _VAR(id, offset, base)	(base | (id & 0x1f) << 10 | (offset & 0x3ff))
+#define VAR(id, offset)  _VAR(id, offset, 0x0000)
+#define VAR8(id, offset) _VAR(id, offset, 0x8000)
+
+/************************************************************************
+ *			struct
+ ***********************************************************************/
+struct mt9t112_format {
+	u32 code;
+	enum v4l2_colorspace colorspace;
+	u16 fmt;
+	u16 order;
+};
+
+struct mt9t112_priv {
+	struct v4l2_subdev		 subdev;
+	struct mt9t112_platform_data	*info;
+	struct i2c_client		*client;
+	struct v4l2_rect		 frame;
+	struct clk			*clk;
+	struct gpio_desc		*standby_gpio;
+	const struct mt9t112_format	*format;
+	int				 num_formats;
+	bool				 init_done;
+};
+
+/************************************************************************
+ *			supported format
+ ***********************************************************************/
+
+static const struct mt9t112_format mt9t112_cfmts[] = {
+	{
+		.code		= MEDIA_BUS_FMT_UYVY8_2X8,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.fmt		= 1,
+		.order		= 0,
+	}, {
+		.code		= MEDIA_BUS_FMT_VYUY8_2X8,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.fmt		= 1,
+		.order		= 1,
+	}, {
+		.code		= MEDIA_BUS_FMT_YUYV8_2X8,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.fmt		= 1,
+		.order		= 2,
+	}, {
+		.code		= MEDIA_BUS_FMT_YVYU8_2X8,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.fmt		= 1,
+		.order		= 3,
+	}, {
+		.code		= MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.fmt		= 8,
+		.order		= 2,
+	}, {
+		.code		= MEDIA_BUS_FMT_RGB565_2X8_LE,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.fmt		= 4,
+		.order		= 2,
+	},
+};
+
+/************************************************************************
+ *			general function
+ ***********************************************************************/
+static struct mt9t112_priv *to_mt9t112(const struct i2c_client *client)
+{
+	return container_of(i2c_get_clientdata(client),
+			    struct mt9t112_priv,
+			    subdev);
+}
+
+static int __mt9t112_reg_read(const struct i2c_client *client, u16 command)
+{
+	struct i2c_msg msg[2];
+	u8 buf[2];
+	int ret;
+
+	command = swab16(command);
+
+	msg[0].addr  = client->addr;
+	msg[0].flags = 0;
+	msg[0].len   = 2;
+	msg[0].buf   = (u8 *)&command;
+
+	msg[1].addr  = client->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].len   = 2;
+	msg[1].buf   = buf;
+
+	/*
+	 * If return value of this function is < 0, it means error, else,
+	 * below 16bit is valid data.
+	 */
+	ret = i2c_transfer(client->adapter, msg, 2);
+	if (ret < 0)
+		return ret;
+
+	memcpy(&ret, buf, 2);
+
+	return swab16(ret);
+}
+
+static int __mt9t112_reg_write(const struct i2c_client *client,
+			       u16 command, u16 data)
+{
+	struct i2c_msg msg;
+	u8 buf[4];
+	int ret;
+
+	command = swab16(command);
+	data = swab16(data);
+
+	memcpy(buf + 0, &command, 2);
+	memcpy(buf + 2, &data,    2);
+
+	msg.addr  = client->addr;
+	msg.flags = 0;
+	msg.len   = 4;
+	msg.buf   = buf;
+
+	/*
+	 * i2c_transfer return message length, but this function should
+	 * return 0 if correct case.
+	 */
+	ret = i2c_transfer(client->adapter, &msg, 1);
+
+	return ret >= 0 ? 0 : ret;
+}
+
+static int __mt9t112_reg_mask_set(const struct i2c_client *client,
+				  u16  command, u16  mask, u16  set)
+{
+	int val = __mt9t112_reg_read(client, command);
+
+	if (val < 0)
+		return val;
+
+	val &= ~mask;
+	val |= set & mask;
+
+	return __mt9t112_reg_write(client, command, val);
+}
+
+/* mcu access */
+static int __mt9t112_mcu_read(const struct i2c_client *client, u16 command)
+{
+	int ret;
+
+	ret = __mt9t112_reg_write(client, 0x098E, command);
+	if (ret < 0)
+		return ret;
+
+	return __mt9t112_reg_read(client, 0x0990);
+}
+
+static int __mt9t112_mcu_write(const struct i2c_client *client,
+			       u16 command, u16 data)
+{
+	int ret;
+
+	ret = __mt9t112_reg_write(client, 0x098E, command);
+	if (ret < 0)
+		return ret;
+
+	return __mt9t112_reg_write(client, 0x0990, data);
+}
+
+static int __mt9t112_mcu_mask_set(const struct i2c_client *client,
+				  u16  command, u16  mask, u16  set)
+{
+	int val = __mt9t112_mcu_read(client, command);
+
+	if (val < 0)
+		return val;
+
+	val &= ~mask;
+	val |= set & mask;
+
+	return __mt9t112_mcu_write(client, command, val);
+}
+
+static int mt9t112_reset(const struct i2c_client *client)
+{
+	int ret;
+
+	mt9t112_reg_mask_set(ret, client, 0x001a, 0x0001, 0x0001);
+	usleep_range(1000, 5000);
+	mt9t112_reg_mask_set(ret, client, 0x001a, 0x0001, 0x0000);
+
+	return ret;
+}
+
+#ifndef EXT_CLOCK
+#define CLOCK_INFO(a, b)
+#else
+#define CLOCK_INFO(a, b) mt9t112_clock_info(a, b)
+static int mt9t112_clock_info(const struct i2c_client *client, u32 ext)
+{
+	int m, n, p1, p2, p3, p4, p5, p6, p7;
+	u32 vco, clk;
+	char *enable;
+
+	ext /= 1000; /* kbyte order */
+
+	mt9t112_reg_read(n, client, 0x0012);
+	p1 = n & 0x000f;
+	n = n >> 4;
+	p2 = n & 0x000f;
+	n = n >> 4;
+	p3 = n & 0x000f;
+
+	mt9t112_reg_read(n, client, 0x002a);
+	p4 = n & 0x000f;
+	n = n >> 4;
+	p5 = n & 0x000f;
+	n = n >> 4;
+	p6 = n & 0x000f;
+
+	mt9t112_reg_read(n, client, 0x002c);
+	p7 = n & 0x000f;
+
+	mt9t112_reg_read(n, client, 0x0010);
+	m = n & 0x00ff;
+	n = (n >> 8) & 0x003f;
+
+	enable = ((ext < 6000) || (ext > 54000)) ? "X" : "";
+	dev_dbg(&client->dev, "EXTCLK          : %10u K %s\n", ext, enable);
+
+	vco = 2 * m * ext / (n + 1);
+	enable = ((vco < 384000) || (vco > 768000)) ? "X" : "";
+	dev_dbg(&client->dev, "VCO             : %10u K %s\n", vco, enable);
+
+	clk = vco / (p1 + 1) / (p2 + 1);
+	enable = (clk > 96000) ? "X" : "";
+	dev_dbg(&client->dev, "PIXCLK          : %10u K %s\n", clk, enable);
+
+	clk = vco / (p3 + 1);
+	enable = (clk > 768000) ? "X" : "";
+	dev_dbg(&client->dev, "MIPICLK         : %10u K %s\n", clk, enable);
+
+	clk = vco / (p6 + 1);
+	enable = (clk > 96000) ? "X" : "";
+	dev_dbg(&client->dev, "MCU CLK         : %10u K %s\n", clk, enable);
+
+	clk = vco / (p5 + 1);
+	enable = (clk > 54000) ? "X" : "";
+	dev_dbg(&client->dev, "SOC CLK         : %10u K %s\n", clk, enable);
+
+	clk = vco / (p4 + 1);
+	enable = (clk > 70000) ? "X" : "";
+	dev_dbg(&client->dev, "Sensor CLK      : %10u K %s\n", clk, enable);
+
+	clk = vco / (p7 + 1);
+	dev_dbg(&client->dev, "External sensor : %10u K\n", clk);
+
+	clk = ext / (n + 1);
+	enable = ((clk < 2000) || (clk > 24000)) ? "X" : "";
+	dev_dbg(&client->dev, "PFD             : %10u K %s\n", clk, enable);
+
+	return 0;
+}
+#endif
+
+static int mt9t112_set_a_frame_size(const struct i2c_client *client,
+				    u16 width, u16 height)
+{
+	int ret;
+	u16 wstart = (MAX_WIDTH - width) / 2;
+	u16 hstart = (MAX_HEIGHT - height) / 2;
+
+	/* (Context A) Image Width/Height. */
+	mt9t112_mcu_write(ret, client, VAR(26, 0), width);
+	mt9t112_mcu_write(ret, client, VAR(26, 2), height);
+
+	/* (Context A) Output Width/Height. */
+	mt9t112_mcu_write(ret, client, VAR(18, 43), 8 + width);
+	mt9t112_mcu_write(ret, client, VAR(18, 45), 8 + height);
+
+	/* (Context A) Start Row/Column. */
+	mt9t112_mcu_write(ret, client, VAR(18, 2), 4 + hstart);
+	mt9t112_mcu_write(ret, client, VAR(18, 4), 4 + wstart);
+
+	/* (Context A) End Row/Column. */
+	mt9t112_mcu_write(ret, client, VAR(18, 6), 11 + height + hstart);
+	mt9t112_mcu_write(ret, client, VAR(18, 8), 11 + width  + wstart);
+
+	mt9t112_mcu_write(ret, client, VAR8(1, 0), 0x06);
+
+	return ret;
+}
+
+static int mt9t112_set_pll_dividers(const struct i2c_client *client,
+				    u8 m, u8 n, u8 p1, u8 p2, u8 p3, u8 p4,
+				    u8 p5, u8 p6, u8 p7)
+{
+	int ret;
+	u16 val;
+
+	/* N/M */
+	val = (n << 8) | (m << 0);
+	mt9t112_reg_mask_set(ret, client, 0x0010, 0x3fff, val);
+
+	/* P1/P2/P3 */
+	val = ((p3 & 0x0F) << 8) | ((p2 & 0x0F) << 4) | ((p1 & 0x0F) << 0);
+	mt9t112_reg_mask_set(ret, client, 0x0012, 0x0fff, val);
+
+	/* P4/P5/P6 */
+	val = (0x7 << 12) | ((p6 & 0x0F) <<  8) | ((p5 & 0x0F) <<  4) |
+	      ((p4 & 0x0F) <<  0);
+	mt9t112_reg_mask_set(ret, client, 0x002A, 0x7fff, val);
+
+	/* P7 */
+	val = (0x1 << 12) | ((p7 & 0x0F) <<  0);
+	mt9t112_reg_mask_set(ret, client, 0x002C, 0x100f, val);
+
+	return ret;
+}
+
+static int mt9t112_init_pll(const struct i2c_client *client)
+{
+	struct mt9t112_priv *priv = to_mt9t112(client);
+	int data, i, ret;
+
+	mt9t112_reg_mask_set(ret, client, 0x0014, 0x003, 0x0001);
+
+	/* PLL control: BYPASS PLL = 8517. */
+	mt9t112_reg_write(ret, client, 0x0014, 0x2145);
+
+	/* Replace these registers when new timing parameters are generated. */
+	mt9t112_set_pll_dividers(client,
+				 priv->info->divider.m, priv->info->divider.n,
+				 priv->info->divider.p1, priv->info->divider.p2,
+				 priv->info->divider.p3, priv->info->divider.p4,
+				 priv->info->divider.p5, priv->info->divider.p6,
+				 priv->info->divider.p7);
+
+	/*
+	 * TEST_BYPASS  on
+	 * PLL_ENABLE   on
+	 * SEL_LOCK_DET on
+	 * TEST_BYPASS  off
+	 */
+	mt9t112_reg_write(ret, client, 0x0014, 0x2525);
+	mt9t112_reg_write(ret, client, 0x0014, 0x2527);
+	mt9t112_reg_write(ret, client, 0x0014, 0x3427);
+	mt9t112_reg_write(ret, client, 0x0014, 0x3027);
+
+	mdelay(10);
+
+	/*
+	 * PLL_BYPASS off
+	 * Reference clock count
+	 * I2C Master Clock Divider
+	 */
+	mt9t112_reg_write(ret, client, 0x0014, 0x3046);
+	/* JPEG initialization workaround */
+	mt9t112_reg_write(ret, client, 0x0016, 0x0400);
+	mt9t112_reg_write(ret, client, 0x0022, 0x0190);
+	mt9t112_reg_write(ret, client, 0x3B84, 0x0212);
+
+	/* External sensor clock is PLL bypass. */
+	mt9t112_reg_write(ret, client, 0x002E, 0x0500);
+
+	mt9t112_reg_mask_set(ret, client, 0x0018, 0x0002, 0x0002);
+	mt9t112_reg_mask_set(ret, client, 0x3B82, 0x0004, 0x0004);
+
+	/* MCU disabled. */
+	mt9t112_reg_mask_set(ret, client, 0x0018, 0x0004, 0x0004);
+
+	/* Out of standby. */
+	mt9t112_reg_mask_set(ret, client, 0x0018, 0x0001, 0);
+
+	mdelay(50);
+
+	/*
+	 * Standby Workaround
+	 * Disable Secondary I2C Pads
+	 */
+	mt9t112_reg_write(ret, client, 0x0614, 0x0001);
+	mdelay(1);
+	mt9t112_reg_write(ret, client, 0x0614, 0x0001);
+	mdelay(1);
+	mt9t112_reg_write(ret, client, 0x0614, 0x0001);
+	mdelay(1);
+	mt9t112_reg_write(ret, client, 0x0614, 0x0001);
+	mdelay(1);
+	mt9t112_reg_write(ret, client, 0x0614, 0x0001);
+	mdelay(1);
+	mt9t112_reg_write(ret, client, 0x0614, 0x0001);
+	mdelay(1);
+
+	/* Poll to verify out of standby. Must Poll this bit. */
+	for (i = 0; i < 100; i++) {
+		mt9t112_reg_read(data, client, 0x0018);
+		if (!(data & 0x4000))
+			break;
+
+		mdelay(10);
+	}
+
+	return ret;
+}
+
+static int mt9t112_init_setting(const struct i2c_client *client)
+{
+	int ret;
+
+	/* Adaptive Output Clock (A) */
+	mt9t112_mcu_mask_set(ret, client, VAR(26, 160), 0x0040, 0x0000);
+
+	/* Read Mode (A) */
+	mt9t112_mcu_write(ret, client, VAR(18, 12), 0x0024);
+
+	/* Fine Correction (A) */
+	mt9t112_mcu_write(ret, client, VAR(18, 15), 0x00CC);
+
+	/* Fine IT Min (A) */
+	mt9t112_mcu_write(ret, client, VAR(18, 17), 0x01f1);
+
+	/* Fine IT Max Margin (A) */
+	mt9t112_mcu_write(ret, client, VAR(18, 19), 0x00fF);
+
+	/* Base Frame Lines (A) */
+	mt9t112_mcu_write(ret, client, VAR(18, 29), 0x032D);
+
+	/* Min Line Length (A) */
+	mt9t112_mcu_write(ret, client, VAR(18, 31), 0x073a);
+
+	/* Line Length (A) */
+	mt9t112_mcu_write(ret, client, VAR(18, 37), 0x07d0);
+
+	/* Adaptive Output Clock (B) */
+	mt9t112_mcu_mask_set(ret, client, VAR(27, 160), 0x0040, 0x0000);
+
+	/* Row Start (B) */
+	mt9t112_mcu_write(ret, client, VAR(18, 74), 0x004);
+
+	/* Column Start (B) */
+	mt9t112_mcu_write(ret, client, VAR(18, 76), 0x004);
+
+	/* Row End (B) */
+	mt9t112_mcu_write(ret, client, VAR(18, 78), 0x60B);
+
+	/* Column End (B) */
+	mt9t112_mcu_write(ret, client, VAR(18, 80), 0x80B);
+
+	/* Fine Correction (B) */
+	mt9t112_mcu_write(ret, client, VAR(18, 87), 0x008C);
+
+	/* Fine IT Min (B) */
+	mt9t112_mcu_write(ret, client, VAR(18, 89), 0x01F1);
+
+	/* Fine IT Max Margin (B) */
+	mt9t112_mcu_write(ret, client, VAR(18, 91), 0x00FF);
+
+	/* Base Frame Lines (B) */
+	mt9t112_mcu_write(ret, client, VAR(18, 101), 0x0668);
+
+	/* Min Line Length (B) */
+	mt9t112_mcu_write(ret, client, VAR(18, 103), 0x0AF0);
+
+	/* Line Length (B) */
+	mt9t112_mcu_write(ret, client, VAR(18, 109), 0x0AF0);
+
+	/*
+	 * Flicker Detection registers.
+	 * This section should be replaced whenever new timing file is
+	 * generated. All the following registers need to be replaced.
+	 * Following registers are generated from Register Wizard but user can
+	 * modify them. For detail see auto flicker detection tuning.
+	 */
+
+	/* FD_FDPERIOD_SELECT */
+	mt9t112_mcu_write(ret, client, VAR8(8, 5), 0x01);
+
+	/* PRI_B_CONFIG_FD_ALGO_RUN */
+	mt9t112_mcu_write(ret, client, VAR(27, 17), 0x0003);
+
+	/* PRI_A_CONFIG_FD_ALGO_RUN */
+	mt9t112_mcu_write(ret, client, VAR(26, 17), 0x0003);
+
+	/*
+	 * AFD range detection tuning registers.
+	 */
+
+	/* Search_f1_50 */
+	mt9t112_mcu_write(ret, client, VAR8(18, 165), 0x25);
+
+	/* Search_f2_50 */
+	mt9t112_mcu_write(ret, client, VAR8(18, 166), 0x28);
+
+	/* Search_f1_60 */
+	mt9t112_mcu_write(ret, client, VAR8(18, 167), 0x2C);
+
+	/* Search_f2_60 */
+	mt9t112_mcu_write(ret, client, VAR8(18, 168), 0x2F);
+
+	/* Period_50Hz (A) */
+	mt9t112_mcu_write(ret, client, VAR8(18, 68), 0xBA);
+
+	/* Secret register by Aptina. */
+	/* Period_50Hz (A MSB) */
+	mt9t112_mcu_write(ret, client, VAR8(18, 303), 0x00);
+
+	/* Period_60Hz (A) */
+	mt9t112_mcu_write(ret, client, VAR8(18, 69), 0x9B);
+
+	/* Secret register by Aptina. */
+	/* Period_60Hz (A MSB) */
+	mt9t112_mcu_write(ret, client, VAR8(18, 301), 0x00);
+
+	/* Period_50Hz (B) */
+	mt9t112_mcu_write(ret, client, VAR8(18, 140), 0x82);
+
+	/* Secret register by Aptina. */
+	/* Period_50Hz (B) MSB */
+	mt9t112_mcu_write(ret, client, VAR8(18, 304), 0x00);
+
+	/* Period_60Hz (B) */
+	mt9t112_mcu_write(ret, client, VAR8(18, 141), 0x6D);
+
+	/* Secret register by Aptina. */
+	/* Period_60Hz (B) MSB */
+	mt9t112_mcu_write(ret, client, VAR8(18, 302), 0x00);
+
+	/* FD Mode */
+	mt9t112_mcu_write(ret, client, VAR8(8, 2), 0x10);
+
+	/* Stat_min */
+	mt9t112_mcu_write(ret, client, VAR8(8, 9), 0x02);
+
+	/* Stat_max */
+	mt9t112_mcu_write(ret, client, VAR8(8, 10), 0x03);
+
+	/* Min_amplitude */
+	mt9t112_mcu_write(ret, client, VAR8(8, 12), 0x0A);
+
+	/* RX FIFO Watermark (A) */
+	mt9t112_mcu_write(ret, client, VAR(18, 70), 0x0014);
+
+	/* RX FIFO Watermark (B) */
+	mt9t112_mcu_write(ret, client, VAR(18, 142), 0x0014);
+
+	/* MCLK: 16MHz
+	 * PCLK: 73MHz
+	 * CorePixCLK: 36.5 MHz
+	 */
+	mt9t112_mcu_write(ret, client, VAR8(18, 0x0044), 133);
+	mt9t112_mcu_write(ret, client, VAR8(18, 0x0045), 110);
+	mt9t112_mcu_write(ret, client, VAR8(18, 0x008c), 130);
+	mt9t112_mcu_write(ret, client, VAR8(18, 0x008d), 108);
+
+	mt9t112_mcu_write(ret, client, VAR8(18, 0x00A5), 27);
+	mt9t112_mcu_write(ret, client, VAR8(18, 0x00a6), 30);
+	mt9t112_mcu_write(ret, client, VAR8(18, 0x00a7), 32);
+	mt9t112_mcu_write(ret, client, VAR8(18, 0x00a8), 35);
+
+	return ret;
+}
+
+static int mt9t112_auto_focus_setting(const struct i2c_client *client)
+{
+	int ret;
+
+	mt9t112_mcu_write(ret, client, VAR(12, 13),	0x000F);
+	mt9t112_mcu_write(ret, client, VAR(12, 23),	0x0F0F);
+	mt9t112_mcu_write(ret, client, VAR8(1, 0),	0x06);
+
+	mt9t112_reg_write(ret, client, 0x0614, 0x0000);
+
+	mt9t112_mcu_write(ret, client, VAR8(1, 0),	0x05);
+	mt9t112_mcu_write(ret, client, VAR8(12, 2),	0x02);
+	mt9t112_mcu_write(ret, client, VAR(12, 3),	0x0002);
+	mt9t112_mcu_write(ret, client, VAR(17, 3),	0x8001);
+	mt9t112_mcu_write(ret, client, VAR(17, 11),	0x0025);
+	mt9t112_mcu_write(ret, client, VAR(17, 13),	0x0193);
+	mt9t112_mcu_write(ret, client, VAR8(17, 33),	0x18);
+	mt9t112_mcu_write(ret, client, VAR8(1, 0),	0x05);
+
+	return ret;
+}
+
+static int mt9t112_auto_focus_trigger(const struct i2c_client *client)
+{
+	int ret;
+
+	mt9t112_mcu_write(ret, client, VAR8(12, 25), 0x01);
+
+	return ret;
+}
+
+static int mt9t112_init_camera(const struct i2c_client *client)
+{
+	int ret;
+
+	ECHECKER(ret, mt9t112_reset(client));
+	ECHECKER(ret, mt9t112_init_pll(client));
+	ECHECKER(ret, mt9t112_init_setting(client));
+	ECHECKER(ret, mt9t112_auto_focus_setting(client));
+
+	mt9t112_reg_mask_set(ret, client, 0x0018, 0x0004, 0);
+
+	/* Analog setting B.*/
+	mt9t112_reg_write(ret, client, 0x3084, 0x2409);
+	mt9t112_reg_write(ret, client, 0x3092, 0x0A49);
+	mt9t112_reg_write(ret, client, 0x3094, 0x4949);
+	mt9t112_reg_write(ret, client, 0x3096, 0x4950);
+
+	/*
+	 * Disable adaptive clock.
+	 * PRI_A_CONFIG_JPEG_OB_TX_CONTROL_VAR
+	 * PRI_B_CONFIG_JPEG_OB_TX_CONTROL_VAR
+	 */
+	mt9t112_mcu_write(ret, client, VAR(26, 160), 0x0A2E);
+	mt9t112_mcu_write(ret, client, VAR(27, 160), 0x0A2E);
+
+	/*
+	 * Configure Status in Status_before_length Format and enable header.
+	 * PRI_B_CONFIG_JPEG_OB_TX_CONTROL_VAR
+	 */
+	mt9t112_mcu_write(ret, client, VAR(27, 144), 0x0CB4);
+
+	/*
+	 * Enable JPEG in context B.
+	 * PRI_B_CONFIG_JPEG_OB_TX_CONTROL_VAR
+	 */
+	mt9t112_mcu_write(ret, client, VAR8(27, 142), 0x01);
+
+	/* Disable Dac_TXLO. */
+	mt9t112_reg_write(ret, client, 0x316C, 0x350F);
+
+	/* Set max slew rates. */
+	mt9t112_reg_write(ret, client, 0x1E, 0x777);
+
+	return ret;
+}
+
+/************************************************************************
+ *			v4l2_subdev_core_ops
+ ***********************************************************************/
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int mt9t112_g_register(struct v4l2_subdev *sd,
+			      struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int                ret;
+
+	reg->size = 2;
+	mt9t112_reg_read(ret, client, reg->reg);
+
+	reg->val = (__u64)ret;
+
+	return 0;
+}
+
+static int mt9t112_s_register(struct v4l2_subdev *sd,
+			      const struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+	mt9t112_reg_write(ret, client, reg->reg, reg->val);
+
+	return ret;
+}
+#endif
+
+static int mt9t112_power_on(struct mt9t112_priv *priv)
+{
+	int ret;
+
+	ret = clk_prepare_enable(priv->clk);
+	if (ret)
+		return ret;
+
+	if (priv->standby_gpio) {
+		gpiod_set_value(priv->standby_gpio, 0);
+		msleep(100);
+	}
+
+	return 0;
+}
+
+static int mt9t112_power_off(struct mt9t112_priv *priv)
+{
+	clk_disable_unprepare(priv->clk);
+	if (priv->standby_gpio) {
+		gpiod_set_value(priv->standby_gpio, 1);
+		msleep(100);
+	}
+
+	return 0;
+}
+
+static int mt9t112_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9t112_priv *priv = to_mt9t112(client);
+
+	return on ? mt9t112_power_on(priv) :
+		    mt9t112_power_off(priv);
+}
+
+static const struct v4l2_subdev_core_ops mt9t112_subdev_core_ops = {
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register	= mt9t112_g_register,
+	.s_register	= mt9t112_s_register,
+#endif
+	.s_power	= mt9t112_s_power,
+};
+
+/************************************************************************
+ *			v4l2_subdev_video_ops
+ **********************************************************************/
+static int mt9t112_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9t112_priv *priv = to_mt9t112(client);
+	int ret = 0;
+
+	if (!enable) {
+		/* FIXME
+		 *
+		 * If user selected large output size, and used it long time,
+		 * mt9t112 camera will be very warm.
+		 *
+		 * But current driver can not stop mt9t112 camera.
+		 * So, set small size here to solve this problem.
+		 */
+		mt9t112_set_a_frame_size(client, VGA_WIDTH, VGA_HEIGHT);
+		return ret;
+	}
+
+	if (!priv->init_done) {
+		u16 param = MT9T112_FLAG_PCLK_RISING_EDGE & priv->info->flags ?
+			    0x0001 : 0x0000;
+
+		ECHECKER(ret, mt9t112_init_camera(client));
+
+		/* Invert PCLK (Data sampled on falling edge of pixclk). */
+		mt9t112_reg_write(ret, client, 0x3C20, param);
+
+		mdelay(5);
+
+		priv->init_done = true;
+	}
+
+	mt9t112_mcu_write(ret, client, VAR(26, 7), priv->format->fmt);
+	mt9t112_mcu_write(ret, client, VAR(26, 9), priv->format->order);
+	mt9t112_mcu_write(ret, client, VAR8(1, 0), 0x06);
+
+	mt9t112_set_a_frame_size(client, priv->frame.width, priv->frame.height);
+
+	ECHECKER(ret, mt9t112_auto_focus_trigger(client));
+
+	dev_dbg(&client->dev, "format : %d\n", priv->format->code);
+	dev_dbg(&client->dev, "size   : %d x %d\n",
+		priv->frame.width,
+		priv->frame.height);
+
+	CLOCK_INFO(client, EXT_CLOCK);
+
+	return ret;
+}
+
+static int mt9t112_set_params(struct mt9t112_priv *priv,
+			      const struct v4l2_rect *rect,
+			      u32 code)
+{
+	int i;
+
+	/*
+	 * get color format
+	 */
+	for (i = 0; i < priv->num_formats; i++)
+		if (mt9t112_cfmts[i].code == code)
+			break;
+
+	if (i == priv->num_formats)
+		return -EINVAL;
+
+	priv->frame = *rect;
+
+	/*
+	 * frame size check
+	 */
+	v4l_bound_align_image(&priv->frame.width, 0, MAX_WIDTH, 0,
+			      &priv->frame.height, 0, MAX_HEIGHT, 0, 0);
+
+	priv->format = mt9t112_cfmts + i;
+
+	return 0;
+}
+
+static int mt9t112_get_selection(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_selection *sel)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9t112_priv *priv = to_mt9t112(client);
+
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
+		return -EINVAL;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = MAX_WIDTH;
+		sel->r.height = MAX_HEIGHT;
+		return 0;
+	case V4L2_SEL_TGT_CROP:
+		sel->r = priv->frame;
+		return 0;
+	default:
+		return -EINVAL;
+	}
+}
+
+static int mt9t112_set_selection(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_selection *sel)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9t112_priv *priv = to_mt9t112(client);
+	const struct v4l2_rect *rect = &sel->r;
+
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE ||
+	    sel->target != V4L2_SEL_TGT_CROP)
+		return -EINVAL;
+
+	return mt9t112_set_params(priv, rect, priv->format->code);
+}
+
+static int mt9t112_get_fmt(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_state *sd_state,
+			   struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *mf = &format->format;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9t112_priv *priv = to_mt9t112(client);
+
+	if (format->pad)
+		return -EINVAL;
+
+	mf->width	= priv->frame.width;
+	mf->height	= priv->frame.height;
+	mf->colorspace	= priv->format->colorspace;
+	mf->code	= priv->format->code;
+	mf->field	= V4L2_FIELD_NONE;
+
+	return 0;
+}
+
+static int mt9t112_s_fmt(struct v4l2_subdev *sd,
+			 struct v4l2_mbus_framefmt *mf)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9t112_priv *priv = to_mt9t112(client);
+	struct v4l2_rect rect = {
+		.width = mf->width,
+		.height = mf->height,
+		.left = priv->frame.left,
+		.top = priv->frame.top,
+	};
+	int ret;
+
+	ret = mt9t112_set_params(priv, &rect, mf->code);
+
+	if (!ret)
+		mf->colorspace = priv->format->colorspace;
+
+	return ret;
+}
+
+static int mt9t112_set_fmt(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_state *sd_state,
+			   struct v4l2_subdev_format *format)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *mf = &format->format;
+	struct mt9t112_priv *priv = to_mt9t112(client);
+	int i;
+
+	if (format->pad)
+		return -EINVAL;
+
+	for (i = 0; i < priv->num_formats; i++)
+		if (mt9t112_cfmts[i].code == mf->code)
+			break;
+
+	if (i == priv->num_formats) {
+		mf->code = MEDIA_BUS_FMT_UYVY8_2X8;
+		mf->colorspace = V4L2_COLORSPACE_JPEG;
+	} else {
+		mf->colorspace = mt9t112_cfmts[i].colorspace;
+	}
+
+	v4l_bound_align_image(&mf->width, 0, MAX_WIDTH, 0,
+			      &mf->height, 0, MAX_HEIGHT, 0, 0);
+
+	mf->field = V4L2_FIELD_NONE;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		return mt9t112_s_fmt(sd, mf);
+	sd_state->pads->try_fmt = *mf;
+
+	return 0;
+}
+
+static int mt9t112_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9t112_priv *priv = to_mt9t112(client);
+
+	if (code->pad || code->index >= priv->num_formats)
+		return -EINVAL;
+
+	code->code = mt9t112_cfmts[code->index].code;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops mt9t112_subdev_video_ops = {
+	.s_stream	= mt9t112_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops mt9t112_subdev_pad_ops = {
+	.enum_mbus_code	= mt9t112_enum_mbus_code,
+	.get_selection	= mt9t112_get_selection,
+	.set_selection	= mt9t112_set_selection,
+	.get_fmt	= mt9t112_get_fmt,
+	.set_fmt	= mt9t112_set_fmt,
+};
+
+/************************************************************************
+ *			i2c driver
+ ***********************************************************************/
+static const struct v4l2_subdev_ops mt9t112_subdev_ops = {
+	.core	= &mt9t112_subdev_core_ops,
+	.video	= &mt9t112_subdev_video_ops,
+	.pad	= &mt9t112_subdev_pad_ops,
+};
+
+static int mt9t112_camera_probe(struct i2c_client *client)
+{
+	struct mt9t112_priv *priv = to_mt9t112(client);
+	const char          *devname;
+	int                  chipid;
+	int		     ret;
+
+	ret = mt9t112_s_power(&priv->subdev, 1);
+	if (ret < 0)
+		return ret;
+
+	/* Check and show chip ID. */
+	mt9t112_reg_read(chipid, client, 0x0000);
+
+	switch (chipid) {
+	case 0x2680:
+		devname = "mt9t111";
+		priv->num_formats = 1;
+		break;
+	case 0x2682:
+		devname = "mt9t112";
+		priv->num_formats = ARRAY_SIZE(mt9t112_cfmts);
+		break;
+	default:
+		dev_err(&client->dev, "Product ID error %04x\n", chipid);
+		ret = -ENODEV;
+		goto done;
+	}
+
+	dev_info(&client->dev, "%s chip ID %04x\n", devname, chipid);
+
+done:
+	mt9t112_s_power(&priv->subdev, 0);
+
+	return ret;
+}
+
+static int mt9t112_probe(struct i2c_client *client)
+{
+	struct mt9t112_priv *priv;
+	int ret;
+
+	if (!client->dev.platform_data) {
+		dev_err(&client->dev, "mt9t112: missing platform data!\n");
+		return -EINVAL;
+	}
+
+	priv = devm_kzalloc(&client->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->info = client->dev.platform_data;
+	priv->init_done = false;
+
+	v4l2_i2c_subdev_init(&priv->subdev, client, &mt9t112_subdev_ops);
+
+	priv->clk = devm_clk_get(&client->dev, "extclk");
+	if (PTR_ERR(priv->clk) == -ENOENT) {
+		priv->clk = NULL;
+	} else if (IS_ERR(priv->clk)) {
+		dev_err(&client->dev, "Unable to get clock \"extclk\"\n");
+		return PTR_ERR(priv->clk);
+	}
+
+	priv->standby_gpio = devm_gpiod_get_optional(&client->dev, "standby",
+						     GPIOD_OUT_HIGH);
+	if (IS_ERR(priv->standby_gpio)) {
+		dev_err(&client->dev, "Unable to get gpio \"standby\"\n");
+		return PTR_ERR(priv->standby_gpio);
+	}
+
+	ret = mt9t112_camera_probe(client);
+	if (ret)
+		return ret;
+
+	return v4l2_async_register_subdev(&priv->subdev);
+}
+
+static void mt9t112_remove(struct i2c_client *client)
+{
+	struct mt9t112_priv *priv = to_mt9t112(client);
+
+	clk_disable_unprepare(priv->clk);
+	v4l2_async_unregister_subdev(&priv->subdev);
+}
+
+static const struct i2c_device_id mt9t112_id[] = {
+	{ "mt9t112", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, mt9t112_id);
+
+static struct i2c_driver mt9t112_i2c_driver = {
+	.driver = {
+		.name = "mt9t112",
+	},
+	.probe    = mt9t112_probe,
+	.remove   = mt9t112_remove,
+	.id_table = mt9t112_id,
+};
+
+module_i2c_driver(mt9t112_i2c_driver);
+
+MODULE_DESCRIPTION("V4L2 driver for MT9T111/MT9T112 camera sensor");
+MODULE_AUTHOR("Kuninori Morimoto");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/mt9v011.c b/drivers/media/i2c/mt9v011.c
new file mode 100644
index 000000000..774861ba7
--- /dev/null
+++ b/drivers/media/i2c/mt9v011.c
@@ -0,0 +1,593 @@
+// SPDX-License-Identifier: GPL-2.0
+//
+// mt9v011 -Micron 1/4-Inch VGA Digital Image Sensor
+//
+// Copyright (c) 2009 Mauro Carvalho Chehab <mchehab@kernel.org>
+
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <linux/delay.h>
+#include <linux/module.h>
+#include <asm/div64.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <media/i2c/mt9v011.h>
+
+MODULE_DESCRIPTION("Micron mt9v011 sensor driver");
+MODULE_AUTHOR("Mauro Carvalho Chehab");
+MODULE_LICENSE("GPL v2");
+
+static int debug;
+module_param(debug, int, 0);
+MODULE_PARM_DESC(debug, "Debug level (0-2)");
+
+#define R00_MT9V011_CHIP_VERSION	0x00
+#define R01_MT9V011_ROWSTART		0x01
+#define R02_MT9V011_COLSTART		0x02
+#define R03_MT9V011_HEIGHT		0x03
+#define R04_MT9V011_WIDTH		0x04
+#define R05_MT9V011_HBLANK		0x05
+#define R06_MT9V011_VBLANK		0x06
+#define R07_MT9V011_OUT_CTRL		0x07
+#define R09_MT9V011_SHUTTER_WIDTH	0x09
+#define R0A_MT9V011_CLK_SPEED		0x0a
+#define R0B_MT9V011_RESTART		0x0b
+#define R0C_MT9V011_SHUTTER_DELAY	0x0c
+#define R0D_MT9V011_RESET		0x0d
+#define R1E_MT9V011_DIGITAL_ZOOM	0x1e
+#define R20_MT9V011_READ_MODE		0x20
+#define R2B_MT9V011_GREEN_1_GAIN	0x2b
+#define R2C_MT9V011_BLUE_GAIN		0x2c
+#define R2D_MT9V011_RED_GAIN		0x2d
+#define R2E_MT9V011_GREEN_2_GAIN	0x2e
+#define R35_MT9V011_GLOBAL_GAIN		0x35
+#define RF1_MT9V011_CHIP_ENABLE		0xf1
+
+#define MT9V011_VERSION			0x8232
+#define MT9V011_REV_B_VERSION		0x8243
+
+struct mt9v011 {
+	struct v4l2_subdev sd;
+#ifdef CONFIG_MEDIA_CONTROLLER
+	struct media_pad pad;
+#endif
+	struct v4l2_ctrl_handler ctrls;
+	unsigned width, height;
+	unsigned xtal;
+	unsigned hflip:1;
+	unsigned vflip:1;
+
+	u16 global_gain, exposure;
+	s16 red_bal, blue_bal;
+};
+
+static inline struct mt9v011 *to_mt9v011(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct mt9v011, sd);
+}
+
+static int mt9v011_read(struct v4l2_subdev *sd, unsigned char addr)
+{
+	struct i2c_client *c = v4l2_get_subdevdata(sd);
+	__be16 buffer;
+	int rc, val;
+
+	rc = i2c_master_send(c, &addr, 1);
+	if (rc != 1)
+		v4l2_dbg(0, debug, sd,
+			 "i2c i/o error: rc == %d (should be 1)\n", rc);
+
+	msleep(10);
+
+	rc = i2c_master_recv(c, (char *)&buffer, 2);
+	if (rc != 2)
+		v4l2_dbg(0, debug, sd,
+			 "i2c i/o error: rc == %d (should be 2)\n", rc);
+
+	val = be16_to_cpu(buffer);
+
+	v4l2_dbg(2, debug, sd, "mt9v011: read 0x%02x = 0x%04x\n", addr, val);
+
+	return val;
+}
+
+static void mt9v011_write(struct v4l2_subdev *sd, unsigned char addr,
+				 u16 value)
+{
+	struct i2c_client *c = v4l2_get_subdevdata(sd);
+	unsigned char buffer[3];
+	int rc;
+
+	buffer[0] = addr;
+	buffer[1] = value >> 8;
+	buffer[2] = value & 0xff;
+
+	v4l2_dbg(2, debug, sd,
+		 "mt9v011: writing 0x%02x 0x%04x\n", buffer[0], value);
+	rc = i2c_master_send(c, buffer, 3);
+	if (rc != 3)
+		v4l2_dbg(0, debug, sd,
+			 "i2c i/o error: rc == %d (should be 3)\n", rc);
+}
+
+
+struct i2c_reg_value {
+	unsigned char reg;
+	u16           value;
+};
+
+/*
+ * Values used at the original driver
+ * Some values are marked as Reserved at the datasheet
+ */
+static const struct i2c_reg_value mt9v011_init_default[] = {
+		{ R0D_MT9V011_RESET, 0x0001 },
+		{ R0D_MT9V011_RESET, 0x0000 },
+
+		{ R0C_MT9V011_SHUTTER_DELAY, 0x0000 },
+		{ R09_MT9V011_SHUTTER_WIDTH, 0x1fc },
+
+		{ R0A_MT9V011_CLK_SPEED, 0x0000 },
+		{ R1E_MT9V011_DIGITAL_ZOOM,  0x0000 },
+
+		{ R07_MT9V011_OUT_CTRL, 0x0002 },	/* chip enable */
+};
+
+
+static u16 calc_mt9v011_gain(s16 lineargain)
+{
+
+	u16 digitalgain = 0;
+	u16 analogmult = 0;
+	u16 analoginit = 0;
+
+	if (lineargain < 0)
+		lineargain = 0;
+
+	/* recommended minimum */
+	lineargain += 0x0020;
+
+	if (lineargain > 2047)
+		lineargain = 2047;
+
+	if (lineargain > 1023) {
+		digitalgain = 3;
+		analogmult = 3;
+		analoginit = lineargain / 16;
+	} else if (lineargain > 511) {
+		digitalgain = 1;
+		analogmult = 3;
+		analoginit = lineargain / 8;
+	} else if (lineargain > 255) {
+		analogmult = 3;
+		analoginit = lineargain / 4;
+	} else if (lineargain > 127) {
+		analogmult = 1;
+		analoginit = lineargain / 2;
+	} else
+		analoginit = lineargain;
+
+	return analoginit + (analogmult << 7) + (digitalgain << 9);
+
+}
+
+static void set_balance(struct v4l2_subdev *sd)
+{
+	struct mt9v011 *core = to_mt9v011(sd);
+	u16 green_gain, blue_gain, red_gain;
+	u16 exposure;
+	s16 bal;
+
+	exposure = core->exposure;
+
+	green_gain = calc_mt9v011_gain(core->global_gain);
+
+	bal = core->global_gain;
+	bal += (core->blue_bal * core->global_gain / (1 << 7));
+	blue_gain = calc_mt9v011_gain(bal);
+
+	bal = core->global_gain;
+	bal += (core->red_bal * core->global_gain / (1 << 7));
+	red_gain = calc_mt9v011_gain(bal);
+
+	mt9v011_write(sd, R2B_MT9V011_GREEN_1_GAIN, green_gain);
+	mt9v011_write(sd, R2E_MT9V011_GREEN_2_GAIN, green_gain);
+	mt9v011_write(sd, R2C_MT9V011_BLUE_GAIN, blue_gain);
+	mt9v011_write(sd, R2D_MT9V011_RED_GAIN, red_gain);
+	mt9v011_write(sd, R09_MT9V011_SHUTTER_WIDTH, exposure);
+}
+
+static void calc_fps(struct v4l2_subdev *sd, u32 *numerator, u32 *denominator)
+{
+	struct mt9v011 *core = to_mt9v011(sd);
+	unsigned height, width, hblank, vblank, speed;
+	unsigned row_time, t_time;
+	u64 frames_per_ms;
+	unsigned tmp;
+
+	height = mt9v011_read(sd, R03_MT9V011_HEIGHT);
+	width = mt9v011_read(sd, R04_MT9V011_WIDTH);
+	hblank = mt9v011_read(sd, R05_MT9V011_HBLANK);
+	vblank = mt9v011_read(sd, R06_MT9V011_VBLANK);
+	speed = mt9v011_read(sd, R0A_MT9V011_CLK_SPEED);
+
+	row_time = (width + 113 + hblank) * (speed + 2);
+	t_time = row_time * (height + vblank + 1);
+
+	frames_per_ms = core->xtal * 1000l;
+	do_div(frames_per_ms, t_time);
+	tmp = frames_per_ms;
+
+	v4l2_dbg(1, debug, sd, "Programmed to %u.%03u fps (%d pixel clcks)\n",
+		tmp / 1000, tmp % 1000, t_time);
+
+	if (numerator && denominator) {
+		*numerator = 1000;
+		*denominator = (u32)frames_per_ms;
+	}
+}
+
+static u16 calc_speed(struct v4l2_subdev *sd, u32 numerator, u32 denominator)
+{
+	struct mt9v011 *core = to_mt9v011(sd);
+	unsigned height, width, hblank, vblank;
+	unsigned row_time, line_time;
+	u64 t_time, speed;
+
+	/* Avoid bogus calculus */
+	if (!numerator || !denominator)
+		return 0;
+
+	height = mt9v011_read(sd, R03_MT9V011_HEIGHT);
+	width = mt9v011_read(sd, R04_MT9V011_WIDTH);
+	hblank = mt9v011_read(sd, R05_MT9V011_HBLANK);
+	vblank = mt9v011_read(sd, R06_MT9V011_VBLANK);
+
+	row_time = width + 113 + hblank;
+	line_time = height + vblank + 1;
+
+	t_time = core->xtal * ((u64)numerator);
+	/* round to the closest value */
+	t_time += denominator / 2;
+	do_div(t_time, denominator);
+
+	speed = t_time;
+	do_div(speed, row_time * line_time);
+
+	/* Avoid having a negative value for speed */
+	if (speed < 2)
+		speed = 0;
+	else
+		speed -= 2;
+
+	/* Avoid speed overflow */
+	if (speed > 15)
+		return 15;
+
+	return (u16)speed;
+}
+
+static void set_res(struct v4l2_subdev *sd)
+{
+	struct mt9v011 *core = to_mt9v011(sd);
+	unsigned vstart, hstart;
+
+	/*
+	 * The mt9v011 doesn't have scaling. So, in order to select the desired
+	 * resolution, we're cropping at the middle of the sensor.
+	 * hblank and vblank should be adjusted, in order to warrant that
+	 * we'll preserve the line timings for 30 fps, no matter what resolution
+	 * is selected.
+	 * NOTE: datasheet says that width (and height) should be filled with
+	 * width-1. However, this doesn't work, since one pixel per line will
+	 * be missing.
+	 */
+
+	hstart = 20 + (640 - core->width) / 2;
+	mt9v011_write(sd, R02_MT9V011_COLSTART, hstart);
+	mt9v011_write(sd, R04_MT9V011_WIDTH, core->width);
+	mt9v011_write(sd, R05_MT9V011_HBLANK, 771 - core->width);
+
+	vstart = 8 + (480 - core->height) / 2;
+	mt9v011_write(sd, R01_MT9V011_ROWSTART, vstart);
+	mt9v011_write(sd, R03_MT9V011_HEIGHT, core->height);
+	mt9v011_write(sd, R06_MT9V011_VBLANK, 508 - core->height);
+
+	calc_fps(sd, NULL, NULL);
+};
+
+static void set_read_mode(struct v4l2_subdev *sd)
+{
+	struct mt9v011 *core = to_mt9v011(sd);
+	unsigned mode = 0x1000;
+
+	if (core->hflip)
+		mode |= 0x4000;
+
+	if (core->vflip)
+		mode |= 0x8000;
+
+	mt9v011_write(sd, R20_MT9V011_READ_MODE, mode);
+}
+
+static int mt9v011_reset(struct v4l2_subdev *sd, u32 val)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(mt9v011_init_default); i++)
+		mt9v011_write(sd, mt9v011_init_default[i].reg,
+			       mt9v011_init_default[i].value);
+
+	set_balance(sd);
+	set_res(sd);
+	set_read_mode(sd);
+
+	return 0;
+}
+
+static int mt9v011_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->pad || code->index > 0)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_SGRBG8_1X8;
+	return 0;
+}
+
+static int mt9v011_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+	struct mt9v011 *core = to_mt9v011(sd);
+
+	if (format->pad || fmt->code != MEDIA_BUS_FMT_SGRBG8_1X8)
+		return -EINVAL;
+
+	v4l_bound_align_image(&fmt->width, 48, 639, 1,
+			      &fmt->height, 32, 480, 1, 0);
+	fmt->field = V4L2_FIELD_NONE;
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		core->width = fmt->width;
+		core->height = fmt->height;
+
+		set_res(sd);
+	} else {
+		sd_state->pads->try_fmt = *fmt;
+	}
+
+	return 0;
+}
+
+static int mt9v011_g_frame_interval(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_frame_interval *ival)
+{
+	calc_fps(sd,
+		 &ival->interval.numerator,
+		 &ival->interval.denominator);
+
+	return 0;
+}
+
+static int mt9v011_s_frame_interval(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_frame_interval *ival)
+{
+	struct v4l2_fract *tpf = &ival->interval;
+	u16 speed;
+
+	speed = calc_speed(sd, tpf->numerator, tpf->denominator);
+
+	mt9v011_write(sd, R0A_MT9V011_CLK_SPEED, speed);
+	v4l2_dbg(1, debug, sd, "Setting speed to %d\n", speed);
+
+	/* Recalculate and update fps info */
+	calc_fps(sd, &tpf->numerator, &tpf->denominator);
+
+	return 0;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int mt9v011_g_register(struct v4l2_subdev *sd,
+			      struct v4l2_dbg_register *reg)
+{
+	reg->val = mt9v011_read(sd, reg->reg & 0xff);
+	reg->size = 2;
+
+	return 0;
+}
+
+static int mt9v011_s_register(struct v4l2_subdev *sd,
+			      const struct v4l2_dbg_register *reg)
+{
+	mt9v011_write(sd, reg->reg & 0xff, reg->val & 0xffff);
+
+	return 0;
+}
+#endif
+
+static int mt9v011_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct mt9v011 *core =
+		container_of(ctrl->handler, struct mt9v011, ctrls);
+	struct v4l2_subdev *sd = &core->sd;
+
+	switch (ctrl->id) {
+	case V4L2_CID_GAIN:
+		core->global_gain = ctrl->val;
+		break;
+	case V4L2_CID_EXPOSURE:
+		core->exposure = ctrl->val;
+		break;
+	case V4L2_CID_RED_BALANCE:
+		core->red_bal = ctrl->val;
+		break;
+	case V4L2_CID_BLUE_BALANCE:
+		core->blue_bal = ctrl->val;
+		break;
+	case V4L2_CID_HFLIP:
+		core->hflip = ctrl->val;
+		set_read_mode(sd);
+		return 0;
+	case V4L2_CID_VFLIP:
+		core->vflip = ctrl->val;
+		set_read_mode(sd);
+		return 0;
+	default:
+		return -EINVAL;
+	}
+
+	set_balance(sd);
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops mt9v011_ctrl_ops = {
+	.s_ctrl = mt9v011_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops mt9v011_core_ops = {
+	.reset = mt9v011_reset,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = mt9v011_g_register,
+	.s_register = mt9v011_s_register,
+#endif
+};
+
+static const struct v4l2_subdev_video_ops mt9v011_video_ops = {
+	.g_frame_interval = mt9v011_g_frame_interval,
+	.s_frame_interval = mt9v011_s_frame_interval,
+};
+
+static const struct v4l2_subdev_pad_ops mt9v011_pad_ops = {
+	.enum_mbus_code = mt9v011_enum_mbus_code,
+	.set_fmt = mt9v011_set_fmt,
+};
+
+static const struct v4l2_subdev_ops mt9v011_ops = {
+	.core  = &mt9v011_core_ops,
+	.video = &mt9v011_video_ops,
+	.pad   = &mt9v011_pad_ops,
+};
+
+
+/****************************************************************************
+			I2C Client & Driver
+ ****************************************************************************/
+
+static int mt9v011_probe(struct i2c_client *c)
+{
+	u16 version;
+	struct mt9v011 *core;
+	struct v4l2_subdev *sd;
+#ifdef CONFIG_MEDIA_CONTROLLER
+	int ret;
+#endif
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(c->adapter,
+	     I2C_FUNC_SMBUS_READ_BYTE | I2C_FUNC_SMBUS_WRITE_BYTE_DATA))
+		return -EIO;
+
+	core = devm_kzalloc(&c->dev, sizeof(struct mt9v011), GFP_KERNEL);
+	if (!core)
+		return -ENOMEM;
+
+	sd = &core->sd;
+	v4l2_i2c_subdev_init(sd, c, &mt9v011_ops);
+
+#ifdef CONFIG_MEDIA_CONTROLLER
+	core->pad.flags = MEDIA_PAD_FL_SOURCE;
+	sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+	ret = media_entity_pads_init(&sd->entity, 1, &core->pad);
+	if (ret < 0)
+		return ret;
+#endif
+
+	/* Check if the sensor is really a MT9V011 */
+	version = mt9v011_read(sd, R00_MT9V011_CHIP_VERSION);
+	if ((version != MT9V011_VERSION) &&
+	    (version != MT9V011_REV_B_VERSION)) {
+		v4l2_info(sd, "*** unknown micron chip detected (0x%04x).\n",
+			  version);
+		return -EINVAL;
+	}
+
+	v4l2_ctrl_handler_init(&core->ctrls, 5);
+	v4l2_ctrl_new_std(&core->ctrls, &mt9v011_ctrl_ops,
+			  V4L2_CID_GAIN, 0, (1 << 12) - 1 - 0x20, 1, 0x20);
+	v4l2_ctrl_new_std(&core->ctrls, &mt9v011_ctrl_ops,
+			  V4L2_CID_EXPOSURE, 0, 2047, 1, 0x01fc);
+	v4l2_ctrl_new_std(&core->ctrls, &mt9v011_ctrl_ops,
+			  V4L2_CID_RED_BALANCE, -(1 << 9), (1 << 9) - 1, 1, 0);
+	v4l2_ctrl_new_std(&core->ctrls, &mt9v011_ctrl_ops,
+			  V4L2_CID_BLUE_BALANCE, -(1 << 9), (1 << 9) - 1, 1, 0);
+	v4l2_ctrl_new_std(&core->ctrls, &mt9v011_ctrl_ops,
+			  V4L2_CID_HFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&core->ctrls, &mt9v011_ctrl_ops,
+			  V4L2_CID_VFLIP, 0, 1, 1, 0);
+
+	if (core->ctrls.error) {
+		int ret = core->ctrls.error;
+
+		v4l2_err(sd, "control initialization error %d\n", ret);
+		v4l2_ctrl_handler_free(&core->ctrls);
+		return ret;
+	}
+	core->sd.ctrl_handler = &core->ctrls;
+
+	core->global_gain = 0x0024;
+	core->exposure = 0x01fc;
+	core->width  = 640;
+	core->height = 480;
+	core->xtal = 27000000;	/* Hz */
+
+	if (c->dev.platform_data) {
+		struct mt9v011_platform_data *pdata = c->dev.platform_data;
+
+		core->xtal = pdata->xtal;
+		v4l2_dbg(1, debug, sd, "xtal set to %d.%03d MHz\n",
+			core->xtal / 1000000, (core->xtal / 1000) % 1000);
+	}
+
+	v4l_info(c, "chip found @ 0x%02x (%s - chip version 0x%04x)\n",
+		 c->addr << 1, c->adapter->name, version);
+
+	return 0;
+}
+
+static void mt9v011_remove(struct i2c_client *c)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(c);
+	struct mt9v011 *core = to_mt9v011(sd);
+
+	v4l2_dbg(1, debug, sd,
+		"mt9v011.c: removing mt9v011 adapter on address 0x%x\n",
+		c->addr << 1);
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&core->ctrls);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id mt9v011_id[] = {
+	{ "mt9v011", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, mt9v011_id);
+
+static struct i2c_driver mt9v011_driver = {
+	.driver = {
+		.name	= "mt9v011",
+	},
+	.probe		= mt9v011_probe,
+	.remove		= mt9v011_remove,
+	.id_table	= mt9v011_id,
+};
+
+module_i2c_driver(mt9v011_driver);
diff --git a/drivers/media/i2c/mt9v032.c b/drivers/media/i2c/mt9v032.c
new file mode 100644
index 000000000..00e7bc6e3
--- /dev/null
+++ b/drivers/media/i2c/mt9v032.c
@@ -0,0 +1,1308 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Driver for MT9V022, MT9V024, MT9V032, and MT9V034 CMOS Image Sensors
+ *
+ * Copyright (C) 2010, Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *
+ * Based on the MT9M001 driver,
+ *
+ * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/log2.h>
+#include <linux/mutex.h>
+#include <linux/of.h>
+#include <linux/of_graph.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <linux/v4l2-mediabus.h>
+#include <linux/module.h>
+
+#include <media/i2c/mt9v032.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+/* The first four rows are black rows. The active area spans 753x481 pixels. */
+#define MT9V032_PIXEL_ARRAY_HEIGHT			485
+#define MT9V032_PIXEL_ARRAY_WIDTH			753
+
+#define MT9V032_SYSCLK_FREQ_DEF				26600000
+
+#define MT9V032_CHIP_VERSION				0x00
+#define		MT9V032_CHIP_ID_REV1			0x1311
+#define		MT9V032_CHIP_ID_REV3			0x1313
+#define		MT9V034_CHIP_ID_REV1			0X1324
+#define MT9V032_COLUMN_START				0x01
+#define		MT9V032_COLUMN_START_MIN		1
+#define		MT9V032_COLUMN_START_DEF		1
+#define		MT9V032_COLUMN_START_MAX		752
+#define MT9V032_ROW_START				0x02
+#define		MT9V032_ROW_START_MIN			4
+#define		MT9V032_ROW_START_DEF			5
+#define		MT9V032_ROW_START_MAX			482
+#define MT9V032_WINDOW_HEIGHT				0x03
+#define		MT9V032_WINDOW_HEIGHT_MIN		1
+#define		MT9V032_WINDOW_HEIGHT_DEF		480
+#define		MT9V032_WINDOW_HEIGHT_MAX		480
+#define MT9V032_WINDOW_WIDTH				0x04
+#define		MT9V032_WINDOW_WIDTH_MIN		1
+#define		MT9V032_WINDOW_WIDTH_DEF		752
+#define		MT9V032_WINDOW_WIDTH_MAX		752
+#define MT9V032_HORIZONTAL_BLANKING			0x05
+#define		MT9V032_HORIZONTAL_BLANKING_MIN		43
+#define		MT9V034_HORIZONTAL_BLANKING_MIN		61
+#define		MT9V032_HORIZONTAL_BLANKING_DEF		94
+#define		MT9V032_HORIZONTAL_BLANKING_MAX		1023
+#define MT9V032_VERTICAL_BLANKING			0x06
+#define		MT9V032_VERTICAL_BLANKING_MIN		4
+#define		MT9V034_VERTICAL_BLANKING_MIN		2
+#define		MT9V032_VERTICAL_BLANKING_DEF		45
+#define		MT9V032_VERTICAL_BLANKING_MAX		3000
+#define		MT9V034_VERTICAL_BLANKING_MAX		32288
+#define MT9V032_CHIP_CONTROL				0x07
+#define		MT9V032_CHIP_CONTROL_MASTER_MODE	(1 << 3)
+#define		MT9V032_CHIP_CONTROL_DOUT_ENABLE	(1 << 7)
+#define		MT9V032_CHIP_CONTROL_SEQUENTIAL		(1 << 8)
+#define MT9V032_SHUTTER_WIDTH1				0x08
+#define MT9V032_SHUTTER_WIDTH2				0x09
+#define MT9V032_SHUTTER_WIDTH_CONTROL			0x0a
+#define MT9V032_TOTAL_SHUTTER_WIDTH			0x0b
+#define		MT9V032_TOTAL_SHUTTER_WIDTH_MIN		1
+#define		MT9V034_TOTAL_SHUTTER_WIDTH_MIN		0
+#define		MT9V032_TOTAL_SHUTTER_WIDTH_DEF		480
+#define		MT9V032_TOTAL_SHUTTER_WIDTH_MAX		32767
+#define		MT9V034_TOTAL_SHUTTER_WIDTH_MAX		32765
+#define MT9V032_RESET					0x0c
+#define MT9V032_READ_MODE				0x0d
+#define		MT9V032_READ_MODE_ROW_BIN_MASK		(3 << 0)
+#define		MT9V032_READ_MODE_ROW_BIN_SHIFT		0
+#define		MT9V032_READ_MODE_COLUMN_BIN_MASK	(3 << 2)
+#define		MT9V032_READ_MODE_COLUMN_BIN_SHIFT	2
+#define		MT9V032_READ_MODE_ROW_FLIP		(1 << 4)
+#define		MT9V032_READ_MODE_COLUMN_FLIP		(1 << 5)
+#define		MT9V032_READ_MODE_DARK_COLUMNS		(1 << 6)
+#define		MT9V032_READ_MODE_DARK_ROWS		(1 << 7)
+#define		MT9V032_READ_MODE_RESERVED		0x0300
+#define MT9V032_PIXEL_OPERATION_MODE			0x0f
+#define		MT9V034_PIXEL_OPERATION_MODE_HDR	(1 << 0)
+#define		MT9V034_PIXEL_OPERATION_MODE_COLOR	(1 << 1)
+#define		MT9V032_PIXEL_OPERATION_MODE_COLOR	(1 << 2)
+#define		MT9V032_PIXEL_OPERATION_MODE_HDR	(1 << 6)
+#define MT9V032_ANALOG_GAIN				0x35
+#define		MT9V032_ANALOG_GAIN_MIN			16
+#define		MT9V032_ANALOG_GAIN_DEF			16
+#define		MT9V032_ANALOG_GAIN_MAX			64
+#define MT9V032_MAX_ANALOG_GAIN				0x36
+#define		MT9V032_MAX_ANALOG_GAIN_MAX		127
+#define MT9V032_FRAME_DARK_AVERAGE			0x42
+#define MT9V032_DARK_AVG_THRESH				0x46
+#define		MT9V032_DARK_AVG_LOW_THRESH_MASK	(255 << 0)
+#define		MT9V032_DARK_AVG_LOW_THRESH_SHIFT	0
+#define		MT9V032_DARK_AVG_HIGH_THRESH_MASK	(255 << 8)
+#define		MT9V032_DARK_AVG_HIGH_THRESH_SHIFT	8
+#define MT9V032_ROW_NOISE_CORR_CONTROL			0x70
+#define		MT9V034_ROW_NOISE_CORR_ENABLE		(1 << 0)
+#define		MT9V034_ROW_NOISE_CORR_USE_BLK_AVG	(1 << 1)
+#define		MT9V032_ROW_NOISE_CORR_ENABLE		(1 << 5)
+#define		MT9V032_ROW_NOISE_CORR_USE_BLK_AVG	(1 << 7)
+#define MT9V032_PIXEL_CLOCK				0x74
+#define MT9V034_PIXEL_CLOCK				0x72
+#define		MT9V032_PIXEL_CLOCK_INV_LINE		(1 << 0)
+#define		MT9V032_PIXEL_CLOCK_INV_FRAME		(1 << 1)
+#define		MT9V032_PIXEL_CLOCK_XOR_LINE		(1 << 2)
+#define		MT9V032_PIXEL_CLOCK_CONT_LINE		(1 << 3)
+#define		MT9V032_PIXEL_CLOCK_INV_PXL_CLK		(1 << 4)
+#define MT9V032_TEST_PATTERN				0x7f
+#define		MT9V032_TEST_PATTERN_DATA_MASK		(1023 << 0)
+#define		MT9V032_TEST_PATTERN_DATA_SHIFT		0
+#define		MT9V032_TEST_PATTERN_USE_DATA		(1 << 10)
+#define		MT9V032_TEST_PATTERN_GRAY_MASK		(3 << 11)
+#define		MT9V032_TEST_PATTERN_GRAY_NONE		(0 << 11)
+#define		MT9V032_TEST_PATTERN_GRAY_VERTICAL	(1 << 11)
+#define		MT9V032_TEST_PATTERN_GRAY_HORIZONTAL	(2 << 11)
+#define		MT9V032_TEST_PATTERN_GRAY_DIAGONAL	(3 << 11)
+#define		MT9V032_TEST_PATTERN_ENABLE		(1 << 13)
+#define		MT9V032_TEST_PATTERN_FLIP		(1 << 14)
+#define MT9V032_AEGC_DESIRED_BIN			0xa5
+#define MT9V032_AEC_UPDATE_FREQUENCY			0xa6
+#define MT9V032_AEC_LPF					0xa8
+#define MT9V032_AGC_UPDATE_FREQUENCY			0xa9
+#define MT9V032_AGC_LPF					0xaa
+#define MT9V032_AEC_AGC_ENABLE				0xaf
+#define		MT9V032_AEC_ENABLE			(1 << 0)
+#define		MT9V032_AGC_ENABLE			(1 << 1)
+#define MT9V034_AEC_MAX_SHUTTER_WIDTH			0xad
+#define MT9V032_AEC_MAX_SHUTTER_WIDTH			0xbd
+#define MT9V032_THERMAL_INFO				0xc1
+
+enum mt9v032_model {
+	MT9V032_MODEL_V022_COLOR,	/* MT9V022IX7ATC */
+	MT9V032_MODEL_V022_MONO,	/* MT9V022IX7ATM */
+	MT9V032_MODEL_V024_COLOR,	/* MT9V024IA7XTC */
+	MT9V032_MODEL_V024_MONO,	/* MT9V024IA7XTM */
+	MT9V032_MODEL_V032_COLOR,	/* MT9V032C12STM */
+	MT9V032_MODEL_V032_MONO,	/* MT9V032C12STC */
+	MT9V032_MODEL_V034_COLOR,
+	MT9V032_MODEL_V034_MONO,
+};
+
+struct mt9v032_model_version {
+	unsigned int version;
+	const char *name;
+};
+
+struct mt9v032_model_data {
+	unsigned int min_row_time;
+	unsigned int min_hblank;
+	unsigned int min_vblank;
+	unsigned int max_vblank;
+	unsigned int min_shutter;
+	unsigned int max_shutter;
+	unsigned int pclk_reg;
+	unsigned int aec_max_shutter_reg;
+	const struct v4l2_ctrl_config * const aec_max_shutter_v4l2_ctrl;
+};
+
+struct mt9v032_model_info {
+	const struct mt9v032_model_data *data;
+	bool color;
+};
+
+static const struct mt9v032_model_version mt9v032_versions[] = {
+	{ MT9V032_CHIP_ID_REV1, "MT9V022/MT9V032 rev1/2" },
+	{ MT9V032_CHIP_ID_REV3, "MT9V022/MT9V032 rev3" },
+	{ MT9V034_CHIP_ID_REV1, "MT9V024/MT9V034 rev1" },
+};
+
+struct mt9v032 {
+	struct v4l2_subdev subdev;
+	struct media_pad pad;
+
+	struct v4l2_mbus_framefmt format;
+	struct v4l2_rect crop;
+	unsigned int hratio;
+	unsigned int vratio;
+
+	struct v4l2_ctrl_handler ctrls;
+	struct {
+		struct v4l2_ctrl *link_freq;
+		struct v4l2_ctrl *pixel_rate;
+	};
+
+	struct mutex power_lock;
+	int power_count;
+
+	struct regmap *regmap;
+	struct clk *clk;
+	struct gpio_desc *reset_gpio;
+	struct gpio_desc *standby_gpio;
+
+	struct mt9v032_platform_data *pdata;
+	const struct mt9v032_model_info *model;
+	const struct mt9v032_model_version *version;
+
+	u32 sysclk;
+	u16 aec_agc;
+	u16 hblank;
+	struct {
+		struct v4l2_ctrl *test_pattern;
+		struct v4l2_ctrl *test_pattern_color;
+	};
+};
+
+static struct mt9v032 *to_mt9v032(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct mt9v032, subdev);
+}
+
+static int
+mt9v032_update_aec_agc(struct mt9v032 *mt9v032, u16 which, int enable)
+{
+	struct regmap *map = mt9v032->regmap;
+	u16 value = mt9v032->aec_agc;
+	int ret;
+
+	if (enable)
+		value |= which;
+	else
+		value &= ~which;
+
+	ret = regmap_write(map, MT9V032_AEC_AGC_ENABLE, value);
+	if (ret < 0)
+		return ret;
+
+	mt9v032->aec_agc = value;
+	return 0;
+}
+
+static int
+mt9v032_update_hblank(struct mt9v032 *mt9v032)
+{
+	struct v4l2_rect *crop = &mt9v032->crop;
+	unsigned int min_hblank = mt9v032->model->data->min_hblank;
+	unsigned int hblank;
+
+	if (mt9v032->version->version == MT9V034_CHIP_ID_REV1)
+		min_hblank += (mt9v032->hratio - 1) * 10;
+	min_hblank = max_t(int, mt9v032->model->data->min_row_time - crop->width,
+			   min_hblank);
+	hblank = max_t(unsigned int, mt9v032->hblank, min_hblank);
+
+	return regmap_write(mt9v032->regmap, MT9V032_HORIZONTAL_BLANKING,
+			    hblank);
+}
+
+static int mt9v032_power_on(struct mt9v032 *mt9v032)
+{
+	struct regmap *map = mt9v032->regmap;
+	int ret;
+
+	gpiod_set_value_cansleep(mt9v032->reset_gpio, 1);
+
+	ret = clk_set_rate(mt9v032->clk, mt9v032->sysclk);
+	if (ret < 0)
+		return ret;
+
+	/* System clock has to be enabled before releasing the reset */
+	ret = clk_prepare_enable(mt9v032->clk);
+	if (ret)
+		return ret;
+
+	udelay(1);
+
+	if (mt9v032->reset_gpio) {
+		gpiod_set_value_cansleep(mt9v032->reset_gpio, 0);
+
+		/* After releasing reset we need to wait 10 clock cycles
+		 * before accessing the sensor over I2C. As the minimum SYSCLK
+		 * frequency is 13MHz, waiting 1µs will be enough in the worst
+		 * case.
+		 */
+		udelay(1);
+	}
+
+	/* Reset the chip and stop data read out */
+	ret = regmap_write(map, MT9V032_RESET, 1);
+	if (ret < 0)
+		goto err;
+
+	ret = regmap_write(map, MT9V032_RESET, 0);
+	if (ret < 0)
+		goto err;
+
+	ret = regmap_write(map, MT9V032_CHIP_CONTROL,
+			   MT9V032_CHIP_CONTROL_MASTER_MODE);
+	if (ret < 0)
+		goto err;
+
+	return 0;
+
+err:
+	clk_disable_unprepare(mt9v032->clk);
+	return ret;
+}
+
+static void mt9v032_power_off(struct mt9v032 *mt9v032)
+{
+	clk_disable_unprepare(mt9v032->clk);
+}
+
+static int __mt9v032_set_power(struct mt9v032 *mt9v032, bool on)
+{
+	struct regmap *map = mt9v032->regmap;
+	int ret;
+
+	if (!on) {
+		mt9v032_power_off(mt9v032);
+		return 0;
+	}
+
+	ret = mt9v032_power_on(mt9v032);
+	if (ret < 0)
+		return ret;
+
+	/* Configure the pixel clock polarity */
+	if (mt9v032->pdata && mt9v032->pdata->clk_pol) {
+		ret = regmap_write(map, mt9v032->model->data->pclk_reg,
+				MT9V032_PIXEL_CLOCK_INV_PXL_CLK);
+		if (ret < 0)
+			return ret;
+	}
+
+	/* Disable the noise correction algorithm and restore the controls. */
+	ret = regmap_write(map, MT9V032_ROW_NOISE_CORR_CONTROL, 0);
+	if (ret < 0)
+		return ret;
+
+	return v4l2_ctrl_handler_setup(&mt9v032->ctrls);
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev video operations
+ */
+
+static struct v4l2_mbus_framefmt *
+__mt9v032_get_pad_format(struct mt9v032 *mt9v032,
+			 struct v4l2_subdev_state *sd_state,
+			 unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_format(&mt9v032->subdev, sd_state,
+						  pad);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return &mt9v032->format;
+	default:
+		return NULL;
+	}
+}
+
+static struct v4l2_rect *
+__mt9v032_get_pad_crop(struct mt9v032 *mt9v032,
+		       struct v4l2_subdev_state *sd_state,
+		       unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_crop(&mt9v032->subdev, sd_state,
+						pad);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return &mt9v032->crop;
+	default:
+		return NULL;
+	}
+}
+
+static int mt9v032_s_stream(struct v4l2_subdev *subdev, int enable)
+{
+	const u16 mode = MT9V032_CHIP_CONTROL_DOUT_ENABLE
+		       | MT9V032_CHIP_CONTROL_SEQUENTIAL;
+	struct mt9v032 *mt9v032 = to_mt9v032(subdev);
+	struct v4l2_rect *crop = &mt9v032->crop;
+	struct regmap *map = mt9v032->regmap;
+	unsigned int hbin;
+	unsigned int vbin;
+	int ret;
+
+	if (!enable)
+		return regmap_update_bits(map, MT9V032_CHIP_CONTROL, mode, 0);
+
+	/* Configure the window size and row/column bin */
+	hbin = fls(mt9v032->hratio) - 1;
+	vbin = fls(mt9v032->vratio) - 1;
+	ret = regmap_update_bits(map, MT9V032_READ_MODE,
+				 ~MT9V032_READ_MODE_RESERVED,
+				 hbin << MT9V032_READ_MODE_COLUMN_BIN_SHIFT |
+				 vbin << MT9V032_READ_MODE_ROW_BIN_SHIFT);
+	if (ret < 0)
+		return ret;
+
+	ret = regmap_write(map, MT9V032_COLUMN_START, crop->left);
+	if (ret < 0)
+		return ret;
+
+	ret = regmap_write(map, MT9V032_ROW_START, crop->top);
+	if (ret < 0)
+		return ret;
+
+	ret = regmap_write(map, MT9V032_WINDOW_WIDTH, crop->width);
+	if (ret < 0)
+		return ret;
+
+	ret = regmap_write(map, MT9V032_WINDOW_HEIGHT, crop->height);
+	if (ret < 0)
+		return ret;
+
+	ret = mt9v032_update_hblank(mt9v032);
+	if (ret < 0)
+		return ret;
+
+	/* Switch to master "normal" mode */
+	return regmap_update_bits(map, MT9V032_CHIP_CONTROL, mode, mode);
+}
+
+static int mt9v032_enum_mbus_code(struct v4l2_subdev *subdev,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct mt9v032 *mt9v032 = to_mt9v032(subdev);
+
+	if (code->index > 0)
+		return -EINVAL;
+
+	code->code = mt9v032->format.code;
+	return 0;
+}
+
+static int mt9v032_enum_frame_size(struct v4l2_subdev *subdev,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct mt9v032 *mt9v032 = to_mt9v032(subdev);
+
+	if (fse->index >= 3)
+		return -EINVAL;
+	if (mt9v032->format.code != fse->code)
+		return -EINVAL;
+
+	fse->min_width = MT9V032_WINDOW_WIDTH_DEF / (1 << fse->index);
+	fse->max_width = fse->min_width;
+	fse->min_height = MT9V032_WINDOW_HEIGHT_DEF / (1 << fse->index);
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static int mt9v032_get_format(struct v4l2_subdev *subdev,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_format *format)
+{
+	struct mt9v032 *mt9v032 = to_mt9v032(subdev);
+
+	format->format = *__mt9v032_get_pad_format(mt9v032, sd_state,
+						   format->pad,
+						   format->which);
+	return 0;
+}
+
+static void mt9v032_configure_pixel_rate(struct mt9v032 *mt9v032)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9v032->subdev);
+	int ret;
+
+	ret = v4l2_ctrl_s_ctrl_int64(mt9v032->pixel_rate,
+				     mt9v032->sysclk / mt9v032->hratio);
+	if (ret < 0)
+		dev_warn(&client->dev, "failed to set pixel rate (%d)\n", ret);
+}
+
+static unsigned int mt9v032_calc_ratio(unsigned int input, unsigned int output)
+{
+	/* Compute the power-of-two binning factor closest to the input size to
+	 * output size ratio. Given that the output size is bounded by input/4
+	 * and input, a generic implementation would be an ineffective luxury.
+	 */
+	if (output * 3 > input * 2)
+		return 1;
+	if (output * 3 > input)
+		return 2;
+	return 4;
+}
+
+static int mt9v032_set_format(struct v4l2_subdev *subdev,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_format *format)
+{
+	struct mt9v032 *mt9v032 = to_mt9v032(subdev);
+	struct v4l2_mbus_framefmt *__format;
+	struct v4l2_rect *__crop;
+	unsigned int width;
+	unsigned int height;
+	unsigned int hratio;
+	unsigned int vratio;
+
+	__crop = __mt9v032_get_pad_crop(mt9v032, sd_state, format->pad,
+					format->which);
+
+	/* Clamp the width and height to avoid dividing by zero. */
+	width = clamp(ALIGN(format->format.width, 2),
+		      max_t(unsigned int, __crop->width / 4,
+			    MT9V032_WINDOW_WIDTH_MIN),
+		      __crop->width);
+	height = clamp(ALIGN(format->format.height, 2),
+		       max_t(unsigned int, __crop->height / 4,
+			     MT9V032_WINDOW_HEIGHT_MIN),
+		       __crop->height);
+
+	hratio = mt9v032_calc_ratio(__crop->width, width);
+	vratio = mt9v032_calc_ratio(__crop->height, height);
+
+	__format = __mt9v032_get_pad_format(mt9v032, sd_state, format->pad,
+					    format->which);
+	__format->width = __crop->width / hratio;
+	__format->height = __crop->height / vratio;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		mt9v032->hratio = hratio;
+		mt9v032->vratio = vratio;
+		mt9v032_configure_pixel_rate(mt9v032);
+	}
+
+	format->format = *__format;
+
+	return 0;
+}
+
+static int mt9v032_get_selection(struct v4l2_subdev *subdev,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_selection *sel)
+{
+	struct mt9v032 *mt9v032 = to_mt9v032(subdev);
+
+	if (sel->target != V4L2_SEL_TGT_CROP)
+		return -EINVAL;
+
+	sel->r = *__mt9v032_get_pad_crop(mt9v032, sd_state, sel->pad,
+					 sel->which);
+	return 0;
+}
+
+static int mt9v032_set_selection(struct v4l2_subdev *subdev,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_selection *sel)
+{
+	struct mt9v032 *mt9v032 = to_mt9v032(subdev);
+	struct v4l2_mbus_framefmt *__format;
+	struct v4l2_rect *__crop;
+	struct v4l2_rect rect;
+
+	if (sel->target != V4L2_SEL_TGT_CROP)
+		return -EINVAL;
+
+	/* Clamp the crop rectangle boundaries and align them to a non multiple
+	 * of 2 pixels to ensure a GRBG Bayer pattern.
+	 */
+	rect.left = clamp(ALIGN(sel->r.left + 1, 2) - 1,
+			  MT9V032_COLUMN_START_MIN,
+			  MT9V032_COLUMN_START_MAX);
+	rect.top = clamp(ALIGN(sel->r.top + 1, 2) - 1,
+			 MT9V032_ROW_START_MIN,
+			 MT9V032_ROW_START_MAX);
+	rect.width = clamp_t(unsigned int, ALIGN(sel->r.width, 2),
+			     MT9V032_WINDOW_WIDTH_MIN,
+			     MT9V032_WINDOW_WIDTH_MAX);
+	rect.height = clamp_t(unsigned int, ALIGN(sel->r.height, 2),
+			      MT9V032_WINDOW_HEIGHT_MIN,
+			      MT9V032_WINDOW_HEIGHT_MAX);
+
+	rect.width = min_t(unsigned int,
+			   rect.width, MT9V032_PIXEL_ARRAY_WIDTH - rect.left);
+	rect.height = min_t(unsigned int,
+			    rect.height, MT9V032_PIXEL_ARRAY_HEIGHT - rect.top);
+
+	__crop = __mt9v032_get_pad_crop(mt9v032, sd_state, sel->pad,
+					sel->which);
+
+	if (rect.width != __crop->width || rect.height != __crop->height) {
+		/* Reset the output image size if the crop rectangle size has
+		 * been modified.
+		 */
+		__format = __mt9v032_get_pad_format(mt9v032, sd_state,
+						    sel->pad,
+						    sel->which);
+		__format->width = rect.width;
+		__format->height = rect.height;
+		if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+			mt9v032->hratio = 1;
+			mt9v032->vratio = 1;
+			mt9v032_configure_pixel_rate(mt9v032);
+		}
+	}
+
+	*__crop = rect;
+	sel->r = rect;
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev control operations
+ */
+
+#define V4L2_CID_TEST_PATTERN_COLOR	(V4L2_CID_USER_BASE | 0x1001)
+/*
+ * Value between 1 and 64 to set the desired bin. This is effectively a measure
+ * of how bright the image is supposed to be. Both AGC and AEC try to reach
+ * this.
+ */
+#define V4L2_CID_AEGC_DESIRED_BIN	(V4L2_CID_USER_BASE | 0x1002)
+/*
+ * LPF is the low pass filter capability of the chip. Both AEC and AGC have
+ * this setting. This limits the speed in which AGC/AEC adjust their settings.
+ * Possible values are 0-2. 0 means no LPF. For 1 and 2 this equation is used:
+ *
+ * if |(calculated new exp - current exp)| > (current exp / 4)
+ *	next exp = calculated new exp
+ * else
+ *	next exp = current exp + ((calculated new exp - current exp) / 2^LPF)
+ */
+#define V4L2_CID_AEC_LPF		(V4L2_CID_USER_BASE | 0x1003)
+#define V4L2_CID_AGC_LPF		(V4L2_CID_USER_BASE | 0x1004)
+/*
+ * Value between 0 and 15. This is the number of frames being skipped before
+ * updating the auto exposure/gain.
+ */
+#define V4L2_CID_AEC_UPDATE_INTERVAL	(V4L2_CID_USER_BASE | 0x1005)
+#define V4L2_CID_AGC_UPDATE_INTERVAL	(V4L2_CID_USER_BASE | 0x1006)
+/*
+ * Maximum shutter width used for AEC.
+ */
+#define V4L2_CID_AEC_MAX_SHUTTER_WIDTH	(V4L2_CID_USER_BASE | 0x1007)
+
+static int mt9v032_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct mt9v032 *mt9v032 =
+			container_of(ctrl->handler, struct mt9v032, ctrls);
+	struct regmap *map = mt9v032->regmap;
+	u32 freq;
+	u16 data;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTOGAIN:
+		return mt9v032_update_aec_agc(mt9v032, MT9V032_AGC_ENABLE,
+					      ctrl->val);
+
+	case V4L2_CID_GAIN:
+		return regmap_write(map, MT9V032_ANALOG_GAIN, ctrl->val);
+
+	case V4L2_CID_EXPOSURE_AUTO:
+		return mt9v032_update_aec_agc(mt9v032, MT9V032_AEC_ENABLE,
+					      !ctrl->val);
+
+	case V4L2_CID_EXPOSURE:
+		return regmap_write(map, MT9V032_TOTAL_SHUTTER_WIDTH,
+				    ctrl->val);
+
+	case V4L2_CID_HBLANK:
+		mt9v032->hblank = ctrl->val;
+		return mt9v032_update_hblank(mt9v032);
+
+	case V4L2_CID_VBLANK:
+		return regmap_write(map, MT9V032_VERTICAL_BLANKING,
+				    ctrl->val);
+
+	case V4L2_CID_PIXEL_RATE:
+	case V4L2_CID_LINK_FREQ:
+		if (mt9v032->link_freq == NULL)
+			break;
+
+		freq = mt9v032->pdata->link_freqs[mt9v032->link_freq->val];
+		*mt9v032->pixel_rate->p_new.p_s64 = freq;
+		mt9v032->sysclk = freq;
+		break;
+
+	case V4L2_CID_TEST_PATTERN:
+		switch (mt9v032->test_pattern->val) {
+		case 0:
+			data = 0;
+			break;
+		case 1:
+			data = MT9V032_TEST_PATTERN_GRAY_VERTICAL
+			     | MT9V032_TEST_PATTERN_ENABLE;
+			break;
+		case 2:
+			data = MT9V032_TEST_PATTERN_GRAY_HORIZONTAL
+			     | MT9V032_TEST_PATTERN_ENABLE;
+			break;
+		case 3:
+			data = MT9V032_TEST_PATTERN_GRAY_DIAGONAL
+			     | MT9V032_TEST_PATTERN_ENABLE;
+			break;
+		default:
+			data = (mt9v032->test_pattern_color->val <<
+				MT9V032_TEST_PATTERN_DATA_SHIFT)
+			     | MT9V032_TEST_PATTERN_USE_DATA
+			     | MT9V032_TEST_PATTERN_ENABLE
+			     | MT9V032_TEST_PATTERN_FLIP;
+			break;
+		}
+		return regmap_write(map, MT9V032_TEST_PATTERN, data);
+
+	case V4L2_CID_AEGC_DESIRED_BIN:
+		return regmap_write(map, MT9V032_AEGC_DESIRED_BIN, ctrl->val);
+
+	case V4L2_CID_AEC_LPF:
+		return regmap_write(map, MT9V032_AEC_LPF, ctrl->val);
+
+	case V4L2_CID_AGC_LPF:
+		return regmap_write(map, MT9V032_AGC_LPF, ctrl->val);
+
+	case V4L2_CID_AEC_UPDATE_INTERVAL:
+		return regmap_write(map, MT9V032_AEC_UPDATE_FREQUENCY,
+				    ctrl->val);
+
+	case V4L2_CID_AGC_UPDATE_INTERVAL:
+		return regmap_write(map, MT9V032_AGC_UPDATE_FREQUENCY,
+				    ctrl->val);
+
+	case V4L2_CID_AEC_MAX_SHUTTER_WIDTH:
+		return regmap_write(map,
+				    mt9v032->model->data->aec_max_shutter_reg,
+				    ctrl->val);
+	}
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops mt9v032_ctrl_ops = {
+	.s_ctrl = mt9v032_s_ctrl,
+};
+
+static const char * const mt9v032_test_pattern_menu[] = {
+	"Disabled",
+	"Gray Vertical Shade",
+	"Gray Horizontal Shade",
+	"Gray Diagonal Shade",
+	"Plain",
+};
+
+static const struct v4l2_ctrl_config mt9v032_test_pattern_color = {
+	.ops		= &mt9v032_ctrl_ops,
+	.id		= V4L2_CID_TEST_PATTERN_COLOR,
+	.type		= V4L2_CTRL_TYPE_INTEGER,
+	.name		= "Test Pattern Color",
+	.min		= 0,
+	.max		= 1023,
+	.step		= 1,
+	.def		= 0,
+	.flags		= 0,
+};
+
+static const struct v4l2_ctrl_config mt9v032_aegc_controls[] = {
+	{
+		.ops		= &mt9v032_ctrl_ops,
+		.id		= V4L2_CID_AEGC_DESIRED_BIN,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "AEC/AGC Desired Bin",
+		.min		= 1,
+		.max		= 64,
+		.step		= 1,
+		.def		= 58,
+		.flags		= 0,
+	}, {
+		.ops		= &mt9v032_ctrl_ops,
+		.id		= V4L2_CID_AEC_LPF,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "AEC Low Pass Filter",
+		.min		= 0,
+		.max		= 2,
+		.step		= 1,
+		.def		= 0,
+		.flags		= 0,
+	}, {
+		.ops		= &mt9v032_ctrl_ops,
+		.id		= V4L2_CID_AGC_LPF,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "AGC Low Pass Filter",
+		.min		= 0,
+		.max		= 2,
+		.step		= 1,
+		.def		= 2,
+		.flags		= 0,
+	}, {
+		.ops		= &mt9v032_ctrl_ops,
+		.id		= V4L2_CID_AEC_UPDATE_INTERVAL,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "AEC Update Interval",
+		.min		= 0,
+		.max		= 16,
+		.step		= 1,
+		.def		= 2,
+		.flags		= 0,
+	}, {
+		.ops		= &mt9v032_ctrl_ops,
+		.id		= V4L2_CID_AGC_UPDATE_INTERVAL,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "AGC Update Interval",
+		.min		= 0,
+		.max		= 16,
+		.step		= 1,
+		.def		= 2,
+		.flags		= 0,
+	}
+};
+
+static const struct v4l2_ctrl_config mt9v032_aec_max_shutter_width = {
+	.ops		= &mt9v032_ctrl_ops,
+	.id		= V4L2_CID_AEC_MAX_SHUTTER_WIDTH,
+	.type		= V4L2_CTRL_TYPE_INTEGER,
+	.name		= "AEC Max Shutter Width",
+	.min		= 1,
+	.max		= 2047,
+	.step		= 1,
+	.def		= 480,
+	.flags		= 0,
+};
+
+static const struct v4l2_ctrl_config mt9v034_aec_max_shutter_width = {
+	.ops		= &mt9v032_ctrl_ops,
+	.id		= V4L2_CID_AEC_MAX_SHUTTER_WIDTH,
+	.type		= V4L2_CTRL_TYPE_INTEGER,
+	.name		= "AEC Max Shutter Width",
+	.min		= 1,
+	.max		= 32765,
+	.step		= 1,
+	.def		= 480,
+	.flags		= 0,
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev core operations
+ */
+
+static int mt9v032_set_power(struct v4l2_subdev *subdev, int on)
+{
+	struct mt9v032 *mt9v032 = to_mt9v032(subdev);
+	int ret = 0;
+
+	mutex_lock(&mt9v032->power_lock);
+
+	/* If the power count is modified from 0 to != 0 or from != 0 to 0,
+	 * update the power state.
+	 */
+	if (mt9v032->power_count == !on) {
+		ret = __mt9v032_set_power(mt9v032, !!on);
+		if (ret < 0)
+			goto done;
+	}
+
+	/* Update the power count. */
+	mt9v032->power_count += on ? 1 : -1;
+	WARN_ON(mt9v032->power_count < 0);
+
+done:
+	mutex_unlock(&mt9v032->power_lock);
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev internal operations
+ */
+
+static int mt9v032_registered(struct v4l2_subdev *subdev)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
+	struct mt9v032 *mt9v032 = to_mt9v032(subdev);
+	unsigned int i;
+	u32 version;
+	int ret;
+
+	dev_info(&client->dev, "Probing MT9V032 at address 0x%02x\n",
+			client->addr);
+
+	ret = mt9v032_power_on(mt9v032);
+	if (ret < 0) {
+		dev_err(&client->dev, "MT9V032 power up failed\n");
+		return ret;
+	}
+
+	/* Read and check the sensor version */
+	ret = regmap_read(mt9v032->regmap, MT9V032_CHIP_VERSION, &version);
+
+	mt9v032_power_off(mt9v032);
+
+	if (ret < 0) {
+		dev_err(&client->dev, "Failed reading chip version\n");
+		return ret;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(mt9v032_versions); ++i) {
+		if (mt9v032_versions[i].version == version) {
+			mt9v032->version = &mt9v032_versions[i];
+			break;
+		}
+	}
+
+	if (mt9v032->version == NULL) {
+		dev_err(&client->dev, "Unsupported chip version 0x%04x\n",
+			version);
+		return -ENODEV;
+	}
+
+	dev_info(&client->dev, "%s detected at address 0x%02x\n",
+		 mt9v032->version->name, client->addr);
+
+	mt9v032_configure_pixel_rate(mt9v032);
+
+	return ret;
+}
+
+static int mt9v032_open(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
+{
+	struct mt9v032 *mt9v032 = to_mt9v032(subdev);
+	struct v4l2_mbus_framefmt *format;
+	struct v4l2_rect *crop;
+
+	crop = v4l2_subdev_get_try_crop(subdev, fh->state, 0);
+	crop->left = MT9V032_COLUMN_START_DEF;
+	crop->top = MT9V032_ROW_START_DEF;
+	crop->width = MT9V032_WINDOW_WIDTH_DEF;
+	crop->height = MT9V032_WINDOW_HEIGHT_DEF;
+
+	format = v4l2_subdev_get_try_format(subdev, fh->state, 0);
+
+	if (mt9v032->model->color)
+		format->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	else
+		format->code = MEDIA_BUS_FMT_Y10_1X10;
+
+	format->width = MT9V032_WINDOW_WIDTH_DEF;
+	format->height = MT9V032_WINDOW_HEIGHT_DEF;
+	format->field = V4L2_FIELD_NONE;
+	format->colorspace = V4L2_COLORSPACE_SRGB;
+
+	return mt9v032_set_power(subdev, 1);
+}
+
+static int mt9v032_close(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
+{
+	return mt9v032_set_power(subdev, 0);
+}
+
+static const struct v4l2_subdev_core_ops mt9v032_subdev_core_ops = {
+	.s_power	= mt9v032_set_power,
+};
+
+static const struct v4l2_subdev_video_ops mt9v032_subdev_video_ops = {
+	.s_stream	= mt9v032_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops mt9v032_subdev_pad_ops = {
+	.enum_mbus_code = mt9v032_enum_mbus_code,
+	.enum_frame_size = mt9v032_enum_frame_size,
+	.get_fmt = mt9v032_get_format,
+	.set_fmt = mt9v032_set_format,
+	.get_selection = mt9v032_get_selection,
+	.set_selection = mt9v032_set_selection,
+};
+
+static const struct v4l2_subdev_ops mt9v032_subdev_ops = {
+	.core	= &mt9v032_subdev_core_ops,
+	.video	= &mt9v032_subdev_video_ops,
+	.pad	= &mt9v032_subdev_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops mt9v032_subdev_internal_ops = {
+	.registered = mt9v032_registered,
+	.open = mt9v032_open,
+	.close = mt9v032_close,
+};
+
+static const struct regmap_config mt9v032_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 16,
+	.max_register = 0xff,
+	.cache_type = REGCACHE_RBTREE,
+};
+
+/* -----------------------------------------------------------------------------
+ * Driver initialization and probing
+ */
+
+static struct mt9v032_platform_data *
+mt9v032_get_pdata(struct i2c_client *client)
+{
+	struct mt9v032_platform_data *pdata = NULL;
+	struct v4l2_fwnode_endpoint endpoint = { .bus_type = 0 };
+	struct device_node *np;
+	struct property *prop;
+
+	if (!IS_ENABLED(CONFIG_OF) || !client->dev.of_node)
+		return client->dev.platform_data;
+
+	np = of_graph_get_next_endpoint(client->dev.of_node, NULL);
+	if (!np)
+		return NULL;
+
+	if (v4l2_fwnode_endpoint_parse(of_fwnode_handle(np), &endpoint) < 0)
+		goto done;
+
+	pdata = devm_kzalloc(&client->dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		goto done;
+
+	prop = of_find_property(np, "link-frequencies", NULL);
+	if (prop) {
+		u64 *link_freqs;
+		size_t size = prop->length / sizeof(*link_freqs);
+
+		link_freqs = devm_kcalloc(&client->dev, size,
+					  sizeof(*link_freqs), GFP_KERNEL);
+		if (!link_freqs)
+			goto done;
+
+		if (of_property_read_u64_array(np, "link-frequencies",
+					       link_freqs, size) < 0)
+			goto done;
+
+		pdata->link_freqs = link_freqs;
+		pdata->link_def_freq = link_freqs[0];
+	}
+
+	pdata->clk_pol = !!(endpoint.bus.parallel.flags &
+			    V4L2_MBUS_PCLK_SAMPLE_RISING);
+
+done:
+	of_node_put(np);
+	return pdata;
+}
+
+static int mt9v032_probe(struct i2c_client *client)
+{
+	const struct i2c_device_id *did = i2c_client_get_device_id(client);
+	struct mt9v032_platform_data *pdata = mt9v032_get_pdata(client);
+	struct mt9v032 *mt9v032;
+	unsigned int i;
+	int ret;
+
+	mt9v032 = devm_kzalloc(&client->dev, sizeof(*mt9v032), GFP_KERNEL);
+	if (!mt9v032)
+		return -ENOMEM;
+
+	mt9v032->regmap = devm_regmap_init_i2c(client, &mt9v032_regmap_config);
+	if (IS_ERR(mt9v032->regmap))
+		return PTR_ERR(mt9v032->regmap);
+
+	mt9v032->clk = devm_clk_get(&client->dev, NULL);
+	if (IS_ERR(mt9v032->clk))
+		return PTR_ERR(mt9v032->clk);
+
+	mt9v032->reset_gpio = devm_gpiod_get_optional(&client->dev, "reset",
+						      GPIOD_OUT_HIGH);
+	if (IS_ERR(mt9v032->reset_gpio))
+		return PTR_ERR(mt9v032->reset_gpio);
+
+	mt9v032->standby_gpio = devm_gpiod_get_optional(&client->dev, "standby",
+							GPIOD_OUT_LOW);
+	if (IS_ERR(mt9v032->standby_gpio))
+		return PTR_ERR(mt9v032->standby_gpio);
+
+	mutex_init(&mt9v032->power_lock);
+	mt9v032->pdata = pdata;
+	mt9v032->model = (const void *)did->driver_data;
+
+	v4l2_ctrl_handler_init(&mt9v032->ctrls, 11 +
+			       ARRAY_SIZE(mt9v032_aegc_controls));
+
+	v4l2_ctrl_new_std(&mt9v032->ctrls, &mt9v032_ctrl_ops,
+			  V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+	v4l2_ctrl_new_std(&mt9v032->ctrls, &mt9v032_ctrl_ops,
+			  V4L2_CID_GAIN, MT9V032_ANALOG_GAIN_MIN,
+			  MT9V032_ANALOG_GAIN_MAX, 1, MT9V032_ANALOG_GAIN_DEF);
+	v4l2_ctrl_new_std_menu(&mt9v032->ctrls, &mt9v032_ctrl_ops,
+			       V4L2_CID_EXPOSURE_AUTO, V4L2_EXPOSURE_MANUAL, 0,
+			       V4L2_EXPOSURE_AUTO);
+	v4l2_ctrl_new_std(&mt9v032->ctrls, &mt9v032_ctrl_ops,
+			  V4L2_CID_EXPOSURE, mt9v032->model->data->min_shutter,
+			  mt9v032->model->data->max_shutter, 1,
+			  MT9V032_TOTAL_SHUTTER_WIDTH_DEF);
+	v4l2_ctrl_new_std(&mt9v032->ctrls, &mt9v032_ctrl_ops,
+			  V4L2_CID_HBLANK, mt9v032->model->data->min_hblank,
+			  MT9V032_HORIZONTAL_BLANKING_MAX, 1,
+			  MT9V032_HORIZONTAL_BLANKING_DEF);
+	v4l2_ctrl_new_std(&mt9v032->ctrls, &mt9v032_ctrl_ops,
+			  V4L2_CID_VBLANK, mt9v032->model->data->min_vblank,
+			  mt9v032->model->data->max_vblank, 1,
+			  MT9V032_VERTICAL_BLANKING_DEF);
+	mt9v032->test_pattern = v4l2_ctrl_new_std_menu_items(&mt9v032->ctrls,
+				&mt9v032_ctrl_ops, V4L2_CID_TEST_PATTERN,
+				ARRAY_SIZE(mt9v032_test_pattern_menu) - 1, 0, 0,
+				mt9v032_test_pattern_menu);
+	mt9v032->test_pattern_color = v4l2_ctrl_new_custom(&mt9v032->ctrls,
+				      &mt9v032_test_pattern_color, NULL);
+
+	v4l2_ctrl_new_custom(&mt9v032->ctrls,
+			     mt9v032->model->data->aec_max_shutter_v4l2_ctrl,
+			     NULL);
+	for (i = 0; i < ARRAY_SIZE(mt9v032_aegc_controls); ++i)
+		v4l2_ctrl_new_custom(&mt9v032->ctrls, &mt9v032_aegc_controls[i],
+				     NULL);
+
+	v4l2_ctrl_cluster(2, &mt9v032->test_pattern);
+
+	mt9v032->pixel_rate =
+		v4l2_ctrl_new_std(&mt9v032->ctrls, &mt9v032_ctrl_ops,
+				  V4L2_CID_PIXEL_RATE, 1, INT_MAX, 1, 1);
+
+	if (pdata && pdata->link_freqs) {
+		unsigned int def = 0;
+
+		for (i = 0; pdata->link_freqs[i]; ++i) {
+			if (pdata->link_freqs[i] == pdata->link_def_freq)
+				def = i;
+		}
+
+		mt9v032->link_freq =
+			v4l2_ctrl_new_int_menu(&mt9v032->ctrls,
+					       &mt9v032_ctrl_ops,
+					       V4L2_CID_LINK_FREQ, i - 1, def,
+					       pdata->link_freqs);
+		v4l2_ctrl_cluster(2, &mt9v032->link_freq);
+	}
+
+
+	mt9v032->subdev.ctrl_handler = &mt9v032->ctrls;
+
+	if (mt9v032->ctrls.error) {
+		dev_err(&client->dev, "control initialization error %d\n",
+			mt9v032->ctrls.error);
+		ret = mt9v032->ctrls.error;
+		goto err;
+	}
+
+	mt9v032->crop.left = MT9V032_COLUMN_START_DEF;
+	mt9v032->crop.top = MT9V032_ROW_START_DEF;
+	mt9v032->crop.width = MT9V032_WINDOW_WIDTH_DEF;
+	mt9v032->crop.height = MT9V032_WINDOW_HEIGHT_DEF;
+
+	if (mt9v032->model->color)
+		mt9v032->format.code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	else
+		mt9v032->format.code = MEDIA_BUS_FMT_Y10_1X10;
+
+	mt9v032->format.width = MT9V032_WINDOW_WIDTH_DEF;
+	mt9v032->format.height = MT9V032_WINDOW_HEIGHT_DEF;
+	mt9v032->format.field = V4L2_FIELD_NONE;
+	mt9v032->format.colorspace = V4L2_COLORSPACE_SRGB;
+
+	mt9v032->hratio = 1;
+	mt9v032->vratio = 1;
+
+	mt9v032->aec_agc = MT9V032_AEC_ENABLE | MT9V032_AGC_ENABLE;
+	mt9v032->hblank = MT9V032_HORIZONTAL_BLANKING_DEF;
+	mt9v032->sysclk = MT9V032_SYSCLK_FREQ_DEF;
+
+	v4l2_i2c_subdev_init(&mt9v032->subdev, client, &mt9v032_subdev_ops);
+	mt9v032->subdev.internal_ops = &mt9v032_subdev_internal_ops;
+	mt9v032->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	mt9v032->subdev.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	mt9v032->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&mt9v032->subdev.entity, 1, &mt9v032->pad);
+	if (ret < 0)
+		goto err;
+
+	mt9v032->subdev.dev = &client->dev;
+	ret = v4l2_async_register_subdev(&mt9v032->subdev);
+	if (ret < 0)
+		goto err;
+
+	return 0;
+
+err:
+	media_entity_cleanup(&mt9v032->subdev.entity);
+	v4l2_ctrl_handler_free(&mt9v032->ctrls);
+	return ret;
+}
+
+static void mt9v032_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct mt9v032 *mt9v032 = to_mt9v032(subdev);
+
+	v4l2_async_unregister_subdev(subdev);
+	v4l2_ctrl_handler_free(&mt9v032->ctrls);
+	media_entity_cleanup(&subdev->entity);
+}
+
+static const struct mt9v032_model_data mt9v032_model_data[] = {
+	{
+		/* MT9V022, MT9V032 revisions 1/2/3 */
+		.min_row_time = 660,
+		.min_hblank = MT9V032_HORIZONTAL_BLANKING_MIN,
+		.min_vblank = MT9V032_VERTICAL_BLANKING_MIN,
+		.max_vblank = MT9V032_VERTICAL_BLANKING_MAX,
+		.min_shutter = MT9V032_TOTAL_SHUTTER_WIDTH_MIN,
+		.max_shutter = MT9V032_TOTAL_SHUTTER_WIDTH_MAX,
+		.pclk_reg = MT9V032_PIXEL_CLOCK,
+		.aec_max_shutter_reg = MT9V032_AEC_MAX_SHUTTER_WIDTH,
+		.aec_max_shutter_v4l2_ctrl = &mt9v032_aec_max_shutter_width,
+	}, {
+		/* MT9V024, MT9V034 */
+		.min_row_time = 690,
+		.min_hblank = MT9V034_HORIZONTAL_BLANKING_MIN,
+		.min_vblank = MT9V034_VERTICAL_BLANKING_MIN,
+		.max_vblank = MT9V034_VERTICAL_BLANKING_MAX,
+		.min_shutter = MT9V034_TOTAL_SHUTTER_WIDTH_MIN,
+		.max_shutter = MT9V034_TOTAL_SHUTTER_WIDTH_MAX,
+		.pclk_reg = MT9V034_PIXEL_CLOCK,
+		.aec_max_shutter_reg = MT9V034_AEC_MAX_SHUTTER_WIDTH,
+		.aec_max_shutter_v4l2_ctrl = &mt9v034_aec_max_shutter_width,
+	},
+};
+
+static const struct mt9v032_model_info mt9v032_models[] = {
+	[MT9V032_MODEL_V022_COLOR] = {
+		.data = &mt9v032_model_data[0],
+		.color = true,
+	},
+	[MT9V032_MODEL_V022_MONO] = {
+		.data = &mt9v032_model_data[0],
+		.color = false,
+	},
+	[MT9V032_MODEL_V024_COLOR] = {
+		.data = &mt9v032_model_data[1],
+		.color = true,
+	},
+	[MT9V032_MODEL_V024_MONO] = {
+		.data = &mt9v032_model_data[1],
+		.color = false,
+	},
+	[MT9V032_MODEL_V032_COLOR] = {
+		.data = &mt9v032_model_data[0],
+		.color = true,
+	},
+	[MT9V032_MODEL_V032_MONO] = {
+		.data = &mt9v032_model_data[0],
+		.color = false,
+	},
+	[MT9V032_MODEL_V034_COLOR] = {
+		.data = &mt9v032_model_data[1],
+		.color = true,
+	},
+	[MT9V032_MODEL_V034_MONO] = {
+		.data = &mt9v032_model_data[1],
+		.color = false,
+	},
+};
+
+static const struct i2c_device_id mt9v032_id[] = {
+	{ "mt9v022", (kernel_ulong_t)&mt9v032_models[MT9V032_MODEL_V022_COLOR] },
+	{ "mt9v022m", (kernel_ulong_t)&mt9v032_models[MT9V032_MODEL_V022_MONO] },
+	{ "mt9v024", (kernel_ulong_t)&mt9v032_models[MT9V032_MODEL_V024_COLOR] },
+	{ "mt9v024m", (kernel_ulong_t)&mt9v032_models[MT9V032_MODEL_V024_MONO] },
+	{ "mt9v032", (kernel_ulong_t)&mt9v032_models[MT9V032_MODEL_V032_COLOR] },
+	{ "mt9v032m", (kernel_ulong_t)&mt9v032_models[MT9V032_MODEL_V032_MONO] },
+	{ "mt9v034", (kernel_ulong_t)&mt9v032_models[MT9V032_MODEL_V034_COLOR] },
+	{ "mt9v034m", (kernel_ulong_t)&mt9v032_models[MT9V032_MODEL_V034_MONO] },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, mt9v032_id);
+
+#if IS_ENABLED(CONFIG_OF)
+static const struct of_device_id mt9v032_of_match[] = {
+	{ .compatible = "aptina,mt9v022" },
+	{ .compatible = "aptina,mt9v022m" },
+	{ .compatible = "aptina,mt9v024" },
+	{ .compatible = "aptina,mt9v024m" },
+	{ .compatible = "aptina,mt9v032" },
+	{ .compatible = "aptina,mt9v032m" },
+	{ .compatible = "aptina,mt9v034" },
+	{ .compatible = "aptina,mt9v034m" },
+	{ /* Sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, mt9v032_of_match);
+#endif
+
+static struct i2c_driver mt9v032_driver = {
+	.driver = {
+		.name = "mt9v032",
+		.of_match_table = of_match_ptr(mt9v032_of_match),
+	},
+	.probe		= mt9v032_probe,
+	.remove		= mt9v032_remove,
+	.id_table	= mt9v032_id,
+};
+
+module_i2c_driver(mt9v032_driver);
+
+MODULE_DESCRIPTION("Aptina MT9V032 Camera driver");
+MODULE_AUTHOR("Laurent Pinchart <laurent.pinchart@ideasonboard.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/mt9v111.c b/drivers/media/i2c/mt9v111.c
new file mode 100644
index 000000000..1f7edc0f5
--- /dev/null
+++ b/drivers/media/i2c/mt9v111.c
@@ -0,0 +1,1276 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * V4L2 sensor driver for Aptina MT9V111 image sensor
+ * Copyright (C) 2018 Jacopo Mondi <jacopo@jmondi.org>
+ *
+ * Based on mt9v032 driver
+ * Copyright (C) 2010, Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
+ *
+ * Based on mt9v011 driver
+ * Copyright (c) 2009 Mauro Carvalho Chehab <mchehab@kernel.org>
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/of.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <linux/v4l2-mediabus.h>
+#include <linux/module.h>
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-image-sizes.h>
+#include <media/v4l2-subdev.h>
+
+/*
+ * MT9V111 is a 1/4-Inch CMOS digital image sensor with an integrated
+ * Image Flow Processing (IFP) engine and a sensor core loosely based on
+ * MT9V011.
+ *
+ * The IFP can produce several output image formats from the sensor core
+ * output. This driver currently supports only YUYV format permutations.
+ *
+ * The driver allows manual frame rate control through s_frame_interval subdev
+ * operation or V4L2_CID_V/HBLANK controls, but it is known that the
+ * auto-exposure algorithm might modify the programmed frame rate. While the
+ * driver initially programs the sensor with auto-exposure and
+ * auto-white-balancing enabled, it is possible to disable them and more
+ * precisely control the frame rate.
+ *
+ * While it seems possible to instruct the auto-exposure control algorithm to
+ * respect a programmed frame rate when adjusting the pixel integration time,
+ * registers controlling this feature are not documented in the public
+ * available sensor manual used to develop this driver (09005aef80e90084,
+ * MT9V111_1.fm - Rev. G 1/05 EN).
+ */
+
+#define MT9V111_CHIP_ID_HIGH				0x82
+#define MT9V111_CHIP_ID_LOW				0x3a
+
+#define MT9V111_R01_ADDR_SPACE				0x01
+#define MT9V111_R01_IFP					0x01
+#define MT9V111_R01_CORE				0x04
+
+#define MT9V111_IFP_R06_OPMODE_CTRL			0x06
+#define		MT9V111_IFP_R06_OPMODE_CTRL_AWB_EN	BIT(1)
+#define		MT9V111_IFP_R06_OPMODE_CTRL_AE_EN	BIT(14)
+#define MT9V111_IFP_R07_IFP_RESET			0x07
+#define		MT9V111_IFP_R07_IFP_RESET_MASK		BIT(0)
+#define MT9V111_IFP_R08_OUTFMT_CTRL			0x08
+#define		MT9V111_IFP_R08_OUTFMT_CTRL_FLICKER	BIT(11)
+#define		MT9V111_IFP_R08_OUTFMT_CTRL_PCLK	BIT(5)
+#define MT9V111_IFP_R3A_OUTFMT_CTRL2			0x3a
+#define		MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_CBCR	BIT(0)
+#define		MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_YC	BIT(1)
+#define		MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_MASK	GENMASK(2, 0)
+#define MT9V111_IFP_RA5_HPAN				0xa5
+#define MT9V111_IFP_RA6_HZOOM				0xa6
+#define MT9V111_IFP_RA7_HOUT				0xa7
+#define MT9V111_IFP_RA8_VPAN				0xa8
+#define MT9V111_IFP_RA9_VZOOM				0xa9
+#define MT9V111_IFP_RAA_VOUT				0xaa
+#define MT9V111_IFP_DECIMATION_MASK			GENMASK(9, 0)
+#define MT9V111_IFP_DECIMATION_FREEZE			BIT(15)
+
+#define MT9V111_CORE_R03_WIN_HEIGHT			0x03
+#define		MT9V111_CORE_R03_WIN_V_OFFS		2
+#define MT9V111_CORE_R04_WIN_WIDTH			0x04
+#define		MT9V111_CORE_R04_WIN_H_OFFS		114
+#define MT9V111_CORE_R05_HBLANK				0x05
+#define		MT9V111_CORE_R05_MIN_HBLANK		0x09
+#define		MT9V111_CORE_R05_MAX_HBLANK		GENMASK(9, 0)
+#define		MT9V111_CORE_R05_DEF_HBLANK		0x26
+#define MT9V111_CORE_R06_VBLANK				0x06
+#define		MT9V111_CORE_R06_MIN_VBLANK		0x03
+#define		MT9V111_CORE_R06_MAX_VBLANK		GENMASK(11, 0)
+#define		MT9V111_CORE_R06_DEF_VBLANK		0x04
+#define MT9V111_CORE_R07_OUT_CTRL			0x07
+#define		MT9V111_CORE_R07_OUT_CTRL_SAMPLE	BIT(4)
+#define MT9V111_CORE_R09_PIXEL_INT			0x09
+#define		MT9V111_CORE_R09_PIXEL_INT_MASK		GENMASK(11, 0)
+#define MT9V111_CORE_R0D_CORE_RESET			0x0d
+#define		MT9V111_CORE_R0D_CORE_RESET_MASK	BIT(0)
+#define MT9V111_CORE_RFF_CHIP_VER			0xff
+
+#define MT9V111_PIXEL_ARRAY_WIDTH			640
+#define MT9V111_PIXEL_ARRAY_HEIGHT			480
+
+#define MT9V111_MAX_CLKIN				27000000
+
+/* The default sensor configuration at startup time. */
+static const struct v4l2_mbus_framefmt mt9v111_def_fmt = {
+	.width		= 640,
+	.height		= 480,
+	.code		= MEDIA_BUS_FMT_UYVY8_2X8,
+	.field		= V4L2_FIELD_NONE,
+	.colorspace	= V4L2_COLORSPACE_SRGB,
+	.ycbcr_enc	= V4L2_YCBCR_ENC_601,
+	.quantization	= V4L2_QUANTIZATION_LIM_RANGE,
+	.xfer_func	= V4L2_XFER_FUNC_SRGB,
+};
+
+struct mt9v111_dev {
+	struct device *dev;
+	struct i2c_client *client;
+
+	u8 addr_space;
+
+	struct v4l2_subdev sd;
+#if IS_ENABLED(CONFIG_MEDIA_CONTROLLER)
+	struct media_pad pad;
+#endif
+
+	struct v4l2_ctrl *auto_awb;
+	struct v4l2_ctrl *auto_exp;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl_handler ctrls;
+
+	/* Output image format and sizes. */
+	struct v4l2_mbus_framefmt fmt;
+	unsigned int fps;
+
+	/* Protects power up/down sequences. */
+	struct mutex pwr_mutex;
+	int pwr_count;
+
+	/* Protects stream on/off sequences. */
+	struct mutex stream_mutex;
+	bool streaming;
+
+	/* Flags to mark HW settings as not yet applied. */
+	bool pending;
+
+	/* Clock provider and system clock frequency. */
+	struct clk *clk;
+	u32 sysclk;
+
+	struct gpio_desc *oe;
+	struct gpio_desc *standby;
+	struct gpio_desc *reset;
+};
+
+#define sd_to_mt9v111(__sd) container_of((__sd), struct mt9v111_dev, sd)
+
+/*
+ * mt9v111_mbus_fmt - List all media bus formats supported by the driver.
+ *
+ * Only list the media bus code here. The image sizes are freely configurable
+ * in the pixel array sizes range.
+ *
+ * The desired frame interval, in the supported frame interval range, is
+ * obtained by configuring blanking as the sensor does not have a PLL but
+ * only a fixed clock divider that generates the output pixel clock.
+ */
+static struct mt9v111_mbus_fmt {
+	u32	code;
+} mt9v111_formats[] = {
+	{
+		.code	= MEDIA_BUS_FMT_UYVY8_2X8,
+	},
+	{
+		.code	= MEDIA_BUS_FMT_YUYV8_2X8,
+	},
+	{
+		.code	= MEDIA_BUS_FMT_VYUY8_2X8,
+	},
+	{
+		.code	= MEDIA_BUS_FMT_YVYU8_2X8,
+	},
+};
+
+static u32 mt9v111_frame_intervals[] = {5, 10, 15, 20, 30};
+
+/*
+ * mt9v111_frame_sizes - List sensor's supported resolutions.
+ *
+ * Resolution generated through decimation in the IFP block from the
+ * full VGA pixel array.
+ */
+static struct v4l2_rect mt9v111_frame_sizes[] = {
+	{
+		.width	= 640,
+		.height	= 480,
+	},
+	{
+		.width	= 352,
+		.height	= 288
+	},
+	{
+		.width	= 320,
+		.height	= 240,
+	},
+	{
+		.width	= 176,
+		.height	= 144,
+	},
+	{
+		.width	= 160,
+		.height	= 120,
+	},
+};
+
+/* --- Device I/O access --- */
+
+static int __mt9v111_read(struct i2c_client *c, u8 reg, u16 *val)
+{
+	struct i2c_msg msg[2];
+	__be16 buf;
+	int ret;
+
+	msg[0].addr = c->addr;
+	msg[0].flags = 0;
+	msg[0].len = 1;
+	msg[0].buf = &reg;
+
+	msg[1].addr = c->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = 2;
+	msg[1].buf = (char *)&buf;
+
+	ret = i2c_transfer(c->adapter, msg, 2);
+	if (ret < 0) {
+		dev_err(&c->dev, "i2c read transfer error: %d\n", ret);
+		return ret;
+	}
+
+	*val = be16_to_cpu(buf);
+
+	dev_dbg(&c->dev, "%s: %x=%x\n", __func__, reg, *val);
+
+	return 0;
+}
+
+static int __mt9v111_write(struct i2c_client *c, u8 reg, u16 val)
+{
+	struct i2c_msg msg;
+	u8 buf[3] = { 0 };
+	int ret;
+
+	buf[0] = reg;
+	buf[1] = val >> 8;
+	buf[2] = val & 0xff;
+
+	msg.addr = c->addr;
+	msg.flags = 0;
+	msg.len = 3;
+	msg.buf = (char *)buf;
+
+	dev_dbg(&c->dev, "%s: %x = %x%x\n", __func__, reg, buf[1], buf[2]);
+
+	ret = i2c_transfer(c->adapter, &msg, 1);
+	if (ret < 0) {
+		dev_err(&c->dev, "i2c write transfer error: %d\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int __mt9v111_addr_space_select(struct i2c_client *c, u16 addr_space)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(c);
+	struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
+	u16 val;
+	int ret;
+
+	if (mt9v111->addr_space == addr_space)
+		return 0;
+
+	ret = __mt9v111_write(c, MT9V111_R01_ADDR_SPACE, addr_space);
+	if (ret)
+		return ret;
+
+	/* Verify address space has been updated */
+	ret = __mt9v111_read(c, MT9V111_R01_ADDR_SPACE, &val);
+	if (ret)
+		return ret;
+
+	if (val != addr_space)
+		return -EINVAL;
+
+	mt9v111->addr_space = addr_space;
+
+	return 0;
+}
+
+static int mt9v111_read(struct i2c_client *c, u8 addr_space, u8 reg, u16 *val)
+{
+	int ret;
+
+	/* Select register address space first. */
+	ret = __mt9v111_addr_space_select(c, addr_space);
+	if (ret)
+		return ret;
+
+	ret = __mt9v111_read(c, reg, val);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int mt9v111_write(struct i2c_client *c, u8 addr_space, u8 reg, u16 val)
+{
+	int ret;
+
+	/* Select register address space first. */
+	ret = __mt9v111_addr_space_select(c, addr_space);
+	if (ret)
+		return ret;
+
+	ret = __mt9v111_write(c, reg, val);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int mt9v111_update(struct i2c_client *c, u8 addr_space, u8 reg,
+			  u16 mask, u16 val)
+{
+	u16 current_val;
+	int ret;
+
+	/* Select register address space first. */
+	ret = __mt9v111_addr_space_select(c, addr_space);
+	if (ret)
+		return ret;
+
+	/* Read the current register value, then update it. */
+	ret = __mt9v111_read(c, reg, &current_val);
+	if (ret)
+		return ret;
+
+	current_val &= ~mask;
+	current_val |= (val & mask);
+	ret = __mt9v111_write(c, reg, current_val);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+/* --- Sensor HW operations --- */
+
+static int __mt9v111_power_on(struct v4l2_subdev *sd)
+{
+	struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
+	int ret;
+
+	ret = clk_prepare_enable(mt9v111->clk);
+	if (ret)
+		return ret;
+
+	clk_set_rate(mt9v111->clk, mt9v111->sysclk);
+
+	gpiod_set_value(mt9v111->standby, 0);
+	usleep_range(500, 1000);
+
+	gpiod_set_value(mt9v111->oe, 1);
+	usleep_range(500, 1000);
+
+	return 0;
+}
+
+static int __mt9v111_power_off(struct v4l2_subdev *sd)
+{
+	struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
+
+	gpiod_set_value(mt9v111->oe, 0);
+	usleep_range(500, 1000);
+
+	gpiod_set_value(mt9v111->standby, 1);
+	usleep_range(500, 1000);
+
+	clk_disable_unprepare(mt9v111->clk);
+
+	return 0;
+}
+
+static int __mt9v111_hw_reset(struct mt9v111_dev *mt9v111)
+{
+	if (!mt9v111->reset)
+		return -EINVAL;
+
+	gpiod_set_value(mt9v111->reset, 1);
+	usleep_range(500, 1000);
+
+	gpiod_set_value(mt9v111->reset, 0);
+	usleep_range(500, 1000);
+
+	return 0;
+}
+
+static int __mt9v111_sw_reset(struct mt9v111_dev *mt9v111)
+{
+	struct i2c_client *c = mt9v111->client;
+	int ret;
+
+	/* Software reset core and IFP blocks. */
+
+	ret = mt9v111_update(c, MT9V111_R01_CORE,
+			     MT9V111_CORE_R0D_CORE_RESET,
+			     MT9V111_CORE_R0D_CORE_RESET_MASK, 1);
+	if (ret)
+		return ret;
+	usleep_range(500, 1000);
+
+	ret = mt9v111_update(c, MT9V111_R01_CORE,
+			     MT9V111_CORE_R0D_CORE_RESET,
+			     MT9V111_CORE_R0D_CORE_RESET_MASK, 0);
+	if (ret)
+		return ret;
+	usleep_range(500, 1000);
+
+	ret = mt9v111_update(c, MT9V111_R01_IFP,
+			     MT9V111_IFP_R07_IFP_RESET,
+			     MT9V111_IFP_R07_IFP_RESET_MASK, 1);
+	if (ret)
+		return ret;
+	usleep_range(500, 1000);
+
+	ret = mt9v111_update(c, MT9V111_R01_IFP,
+			     MT9V111_IFP_R07_IFP_RESET,
+			     MT9V111_IFP_R07_IFP_RESET_MASK, 0);
+	if (ret)
+		return ret;
+	usleep_range(500, 1000);
+
+	return 0;
+}
+
+static int mt9v111_calc_frame_rate(struct mt9v111_dev *mt9v111,
+				   struct v4l2_fract *tpf)
+{
+	unsigned int fps = tpf->numerator ?
+			   tpf->denominator / tpf->numerator :
+			   tpf->denominator;
+	unsigned int best_diff;
+	unsigned int frm_cols;
+	unsigned int row_pclk;
+	unsigned int best_fps;
+	unsigned int pclk;
+	unsigned int diff;
+	unsigned int idx;
+	unsigned int hb;
+	unsigned int vb;
+	unsigned int i;
+	int ret;
+
+	/* Approximate to the closest supported frame interval. */
+	best_diff = ~0L;
+	for (i = 0, idx = 0; i < ARRAY_SIZE(mt9v111_frame_intervals); i++) {
+		diff = abs(fps - mt9v111_frame_intervals[i]);
+		if (diff < best_diff) {
+			idx = i;
+			best_diff = diff;
+		}
+	}
+	fps = mt9v111_frame_intervals[idx];
+
+	/*
+	 * The sensor does not provide a PLL circuitry and pixel clock is
+	 * generated dividing the master clock source by two.
+	 *
+	 * Trow = (W + Hblank + 114) * 2 * (1 / SYSCLK)
+	 * TFrame = Trow * (H + Vblank + 2)
+	 *
+	 * FPS = (SYSCLK / 2) / (Trow * (H + Vblank + 2))
+	 *
+	 * This boils down to tune H and V blanks to best approximate the
+	 * above equation.
+	 *
+	 * Test all available H/V blank values, until we reach the
+	 * desired frame rate.
+	 */
+	best_fps = vb = hb = 0;
+	pclk = DIV_ROUND_CLOSEST(mt9v111->sysclk, 2);
+	row_pclk = MT9V111_PIXEL_ARRAY_WIDTH + 7 + MT9V111_CORE_R04_WIN_H_OFFS;
+	frm_cols = MT9V111_PIXEL_ARRAY_HEIGHT + 7 + MT9V111_CORE_R03_WIN_V_OFFS;
+
+	best_diff = ~0L;
+	for (vb = MT9V111_CORE_R06_MIN_VBLANK;
+	     vb < MT9V111_CORE_R06_MAX_VBLANK; vb++) {
+		for (hb = MT9V111_CORE_R05_MIN_HBLANK;
+		     hb < MT9V111_CORE_R05_MAX_HBLANK; hb += 10) {
+			unsigned int t_frame = (row_pclk + hb) *
+					       (frm_cols + vb);
+			unsigned int t_fps = DIV_ROUND_CLOSEST(pclk, t_frame);
+
+			diff = abs(fps - t_fps);
+			if (diff < best_diff) {
+				best_diff = diff;
+				best_fps = t_fps;
+
+				if (diff == 0)
+					break;
+			}
+		}
+
+		if (diff == 0)
+			break;
+	}
+
+	ret = v4l2_ctrl_s_ctrl_int64(mt9v111->hblank, hb);
+	if (ret)
+		return ret;
+
+	ret = v4l2_ctrl_s_ctrl_int64(mt9v111->vblank, vb);
+	if (ret)
+		return ret;
+
+	tpf->numerator = 1;
+	tpf->denominator = best_fps;
+
+	return 0;
+}
+
+static int mt9v111_hw_config(struct mt9v111_dev *mt9v111)
+{
+	struct i2c_client *c = mt9v111->client;
+	unsigned int ret;
+	u16 outfmtctrl2;
+
+	/* Force device reset. */
+	ret = __mt9v111_hw_reset(mt9v111);
+	if (ret == -EINVAL)
+		ret = __mt9v111_sw_reset(mt9v111);
+	if (ret)
+		return ret;
+
+	/* Configure internal clock sample rate. */
+	ret = mt9v111->sysclk < DIV_ROUND_CLOSEST(MT9V111_MAX_CLKIN, 2) ?
+				mt9v111_update(c, MT9V111_R01_CORE,
+					MT9V111_CORE_R07_OUT_CTRL,
+					MT9V111_CORE_R07_OUT_CTRL_SAMPLE, 1) :
+				mt9v111_update(c, MT9V111_R01_CORE,
+					MT9V111_CORE_R07_OUT_CTRL,
+					MT9V111_CORE_R07_OUT_CTRL_SAMPLE, 0);
+	if (ret)
+		return ret;
+
+	/*
+	 * Configure output image format components ordering.
+	 *
+	 * TODO: IFP block can also output several RGB permutations, we only
+	 *	 support YUYV permutations at the moment.
+	 */
+	switch (mt9v111->fmt.code) {
+	case MEDIA_BUS_FMT_YUYV8_2X8:
+			outfmtctrl2 = MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_YC;
+			break;
+	case MEDIA_BUS_FMT_VYUY8_2X8:
+			outfmtctrl2 = MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_CBCR;
+			break;
+	case MEDIA_BUS_FMT_YVYU8_2X8:
+			outfmtctrl2 = MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_YC |
+				      MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_CBCR;
+			break;
+	case MEDIA_BUS_FMT_UYVY8_2X8:
+	default:
+			outfmtctrl2 = 0;
+			break;
+	}
+
+	ret = mt9v111_update(c, MT9V111_R01_IFP, MT9V111_IFP_R3A_OUTFMT_CTRL2,
+			     MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_MASK,
+			     outfmtctrl2);
+	if (ret)
+		return ret;
+
+	/*
+	 * Do not change default sensor's core configuration:
+	 * output the whole 640x480 pixel array, skip 18 columns and 6 rows.
+	 *
+	 * Instead, control the output image size through IFP block.
+	 *
+	 * TODO: No zoom&pan support. Currently we control the output image
+	 *	 size only through decimation, with no zoom support.
+	 */
+	ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RA5_HPAN,
+			    MT9V111_IFP_DECIMATION_FREEZE);
+	if (ret)
+		return ret;
+
+	ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RA8_VPAN,
+			    MT9V111_IFP_DECIMATION_FREEZE);
+	if (ret)
+		return ret;
+
+	ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RA6_HZOOM,
+			    MT9V111_IFP_DECIMATION_FREEZE |
+			    MT9V111_PIXEL_ARRAY_WIDTH);
+	if (ret)
+		return ret;
+
+	ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RA9_VZOOM,
+			    MT9V111_IFP_DECIMATION_FREEZE |
+			    MT9V111_PIXEL_ARRAY_HEIGHT);
+	if (ret)
+		return ret;
+
+	ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RA7_HOUT,
+			    MT9V111_IFP_DECIMATION_FREEZE |
+			    mt9v111->fmt.width);
+	if (ret)
+		return ret;
+
+	ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RAA_VOUT,
+			    mt9v111->fmt.height);
+	if (ret)
+		return ret;
+
+	/* Apply controls to set auto exp, auto awb and timings */
+	ret = v4l2_ctrl_handler_setup(&mt9v111->ctrls);
+	if (ret)
+		return ret;
+
+	/*
+	 * Set pixel integration time to the whole frame time.
+	 * This value controls the shutter delay when running with AE
+	 * disabled. If longer than frame time, it affects the output
+	 * frame rate.
+	 */
+	return mt9v111_write(c, MT9V111_R01_CORE, MT9V111_CORE_R09_PIXEL_INT,
+			     MT9V111_PIXEL_ARRAY_HEIGHT);
+}
+
+/* ---  V4L2 subdev operations --- */
+
+static int mt9v111_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
+	int pwr_count;
+	int ret = 0;
+
+	mutex_lock(&mt9v111->pwr_mutex);
+
+	/*
+	 * Make sure we're transitioning from 0 to 1, or viceversa,
+	 * before actually changing the power state.
+	 */
+	pwr_count = mt9v111->pwr_count;
+	pwr_count += on ? 1 : -1;
+	if (pwr_count == !!on) {
+		ret = on ? __mt9v111_power_on(sd) :
+			   __mt9v111_power_off(sd);
+		if (!ret)
+			/* All went well, updated power counter. */
+			mt9v111->pwr_count = pwr_count;
+
+		mutex_unlock(&mt9v111->pwr_mutex);
+
+		return ret;
+	}
+
+	/*
+	 * Update power counter to keep track of how many nested calls we
+	 * received.
+	 */
+	WARN_ON(pwr_count < 0 || pwr_count > 1);
+	mt9v111->pwr_count = pwr_count;
+
+	mutex_unlock(&mt9v111->pwr_mutex);
+
+	return ret;
+}
+
+static int mt9v111_s_stream(struct v4l2_subdev *subdev, int enable)
+{
+	struct mt9v111_dev *mt9v111 = sd_to_mt9v111(subdev);
+	int ret;
+
+	mutex_lock(&mt9v111->stream_mutex);
+
+	if (mt9v111->streaming == enable) {
+		mutex_unlock(&mt9v111->stream_mutex);
+		return 0;
+	}
+
+	ret = mt9v111_s_power(subdev, enable);
+	if (ret)
+		goto error_unlock;
+
+	if (enable && mt9v111->pending) {
+		ret = mt9v111_hw_config(mt9v111);
+		if (ret)
+			goto error_unlock;
+
+		/*
+		 * No need to update control here as far as only H/VBLANK are
+		 * supported and immediately programmed to registers in .s_ctrl
+		 */
+
+		mt9v111->pending = false;
+	}
+
+	mt9v111->streaming = enable ? true : false;
+	mutex_unlock(&mt9v111->stream_mutex);
+
+	return 0;
+
+error_unlock:
+	mutex_unlock(&mt9v111->stream_mutex);
+
+	return ret;
+}
+
+static int mt9v111_s_frame_interval(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_frame_interval *ival)
+{
+	struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
+	struct v4l2_fract *tpf = &ival->interval;
+	unsigned int fps = tpf->numerator ?
+			   tpf->denominator / tpf->numerator :
+			   tpf->denominator;
+	unsigned int max_fps;
+
+	if (!tpf->numerator)
+		tpf->numerator = 1;
+
+	mutex_lock(&mt9v111->stream_mutex);
+
+	if (mt9v111->streaming) {
+		mutex_unlock(&mt9v111->stream_mutex);
+		return -EBUSY;
+	}
+
+	if (mt9v111->fps == fps) {
+		mutex_unlock(&mt9v111->stream_mutex);
+		return 0;
+	}
+
+	/* Make sure frame rate/image sizes constraints are respected. */
+	if (mt9v111->fmt.width < QVGA_WIDTH &&
+	    mt9v111->fmt.height < QVGA_HEIGHT)
+		max_fps = 90;
+	else if (mt9v111->fmt.width < CIF_WIDTH &&
+		 mt9v111->fmt.height < CIF_HEIGHT)
+		max_fps = 60;
+	else
+		max_fps = mt9v111->sysclk <
+				DIV_ROUND_CLOSEST(MT9V111_MAX_CLKIN, 2) ? 15 :
+									  30;
+
+	if (fps > max_fps) {
+		mutex_unlock(&mt9v111->stream_mutex);
+		return -EINVAL;
+	}
+
+	mt9v111_calc_frame_rate(mt9v111, tpf);
+
+	mt9v111->fps = fps;
+	mt9v111->pending = true;
+
+	mutex_unlock(&mt9v111->stream_mutex);
+
+	return 0;
+}
+
+static int mt9v111_g_frame_interval(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_frame_interval *ival)
+{
+	struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
+	struct v4l2_fract *tpf = &ival->interval;
+
+	mutex_lock(&mt9v111->stream_mutex);
+
+	tpf->numerator = 1;
+	tpf->denominator = mt9v111->fps;
+
+	mutex_unlock(&mt9v111->stream_mutex);
+
+	return 0;
+}
+
+static struct v4l2_mbus_framefmt *__mt9v111_get_pad_format(
+					struct mt9v111_dev *mt9v111,
+					struct v4l2_subdev_state *sd_state,
+					unsigned int pad,
+					enum v4l2_subdev_format_whence which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+#if IS_ENABLED(CONFIG_VIDEO_V4L2_SUBDEV_API)
+		return v4l2_subdev_get_try_format(&mt9v111->sd, sd_state, pad);
+#else
+		return &sd_state->pads->try_fmt;
+#endif
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return &mt9v111->fmt;
+	default:
+		return NULL;
+	}
+}
+
+static int mt9v111_enum_mbus_code(struct v4l2_subdev *subdev,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->pad || code->index > ARRAY_SIZE(mt9v111_formats) - 1)
+		return -EINVAL;
+
+	code->code = mt9v111_formats[code->index].code;
+
+	return 0;
+}
+
+static int mt9v111_enum_frame_interval(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *sd_state,
+				struct v4l2_subdev_frame_interval_enum *fie)
+{
+	unsigned int i;
+
+	if (fie->pad || fie->index >= ARRAY_SIZE(mt9v111_frame_intervals))
+		return -EINVAL;
+
+	for (i = 0; i < ARRAY_SIZE(mt9v111_frame_sizes); i++)
+		if (fie->width == mt9v111_frame_sizes[i].width &&
+		    fie->height == mt9v111_frame_sizes[i].height)
+			break;
+
+	if (i == ARRAY_SIZE(mt9v111_frame_sizes))
+		return -EINVAL;
+
+	fie->interval.numerator = 1;
+	fie->interval.denominator = mt9v111_frame_intervals[fie->index];
+
+	return 0;
+}
+
+static int mt9v111_enum_frame_size(struct v4l2_subdev *subdev,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->pad || fse->index >= ARRAY_SIZE(mt9v111_frame_sizes))
+		return -EINVAL;
+
+	fse->min_width = mt9v111_frame_sizes[fse->index].width;
+	fse->max_width = mt9v111_frame_sizes[fse->index].width;
+	fse->min_height = mt9v111_frame_sizes[fse->index].height;
+	fse->max_height = mt9v111_frame_sizes[fse->index].height;
+
+	return 0;
+}
+
+static int mt9v111_get_format(struct v4l2_subdev *subdev,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_format *format)
+{
+	struct mt9v111_dev *mt9v111 = sd_to_mt9v111(subdev);
+
+	if (format->pad)
+		return -EINVAL;
+
+	mutex_lock(&mt9v111->stream_mutex);
+	format->format = *__mt9v111_get_pad_format(mt9v111, sd_state,
+						   format->pad,
+						   format->which);
+	mutex_unlock(&mt9v111->stream_mutex);
+
+	return 0;
+}
+
+static int mt9v111_set_format(struct v4l2_subdev *subdev,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_format *format)
+{
+	struct mt9v111_dev *mt9v111 = sd_to_mt9v111(subdev);
+	struct v4l2_mbus_framefmt new_fmt;
+	struct v4l2_mbus_framefmt *__fmt;
+	unsigned int best_fit = ~0L;
+	unsigned int idx = 0;
+	unsigned int i;
+
+	mutex_lock(&mt9v111->stream_mutex);
+	if (mt9v111->streaming) {
+		mutex_unlock(&mt9v111->stream_mutex);
+		return -EBUSY;
+	}
+
+	if (format->pad) {
+		mutex_unlock(&mt9v111->stream_mutex);
+		return -EINVAL;
+	}
+
+	/* Update mbus format code and sizes. */
+	for (i = 0; i < ARRAY_SIZE(mt9v111_formats); i++) {
+		if (format->format.code == mt9v111_formats[i].code) {
+			new_fmt.code = mt9v111_formats[i].code;
+			break;
+		}
+	}
+	if (i == ARRAY_SIZE(mt9v111_formats))
+		new_fmt.code = mt9v111_formats[0].code;
+
+	for (i = 0; i < ARRAY_SIZE(mt9v111_frame_sizes); i++) {
+		unsigned int fit = abs(mt9v111_frame_sizes[i].width -
+				       format->format.width) +
+				   abs(mt9v111_frame_sizes[i].height -
+				       format->format.height);
+		if (fit < best_fit) {
+			best_fit = fit;
+			idx = i;
+
+			if (fit == 0)
+				break;
+		}
+	}
+	new_fmt.width = mt9v111_frame_sizes[idx].width;
+	new_fmt.height = mt9v111_frame_sizes[idx].height;
+
+	/* Update the device (or pad) format if it has changed. */
+	__fmt = __mt9v111_get_pad_format(mt9v111, sd_state, format->pad,
+					 format->which);
+
+	/* Format hasn't changed, stop here. */
+	if (__fmt->code == new_fmt.code &&
+	    __fmt->width == new_fmt.width &&
+	    __fmt->height == new_fmt.height)
+		goto done;
+
+	/* Update the format and sizes, then  mark changes as pending. */
+	__fmt->code = new_fmt.code;
+	__fmt->width = new_fmt.width;
+	__fmt->height = new_fmt.height;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		mt9v111->pending = true;
+
+	dev_dbg(mt9v111->dev, "%s: mbus_code: %x - (%ux%u)\n",
+		__func__, __fmt->code, __fmt->width, __fmt->height);
+
+done:
+	format->format = *__fmt;
+
+	mutex_unlock(&mt9v111->stream_mutex);
+
+	return 0;
+}
+
+static int mt9v111_init_cfg(struct v4l2_subdev *subdev,
+			    struct v4l2_subdev_state *sd_state)
+{
+	sd_state->pads->try_fmt = mt9v111_def_fmt;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops mt9v111_core_ops = {
+	.s_power		= mt9v111_s_power,
+};
+
+static const struct v4l2_subdev_video_ops mt9v111_video_ops = {
+	.s_stream		= mt9v111_s_stream,
+	.s_frame_interval	= mt9v111_s_frame_interval,
+	.g_frame_interval	= mt9v111_g_frame_interval,
+};
+
+static const struct v4l2_subdev_pad_ops mt9v111_pad_ops = {
+	.init_cfg		= mt9v111_init_cfg,
+	.enum_mbus_code		= mt9v111_enum_mbus_code,
+	.enum_frame_size	= mt9v111_enum_frame_size,
+	.enum_frame_interval	= mt9v111_enum_frame_interval,
+	.get_fmt		= mt9v111_get_format,
+	.set_fmt		= mt9v111_set_format,
+};
+
+static const struct v4l2_subdev_ops mt9v111_ops = {
+	.core	= &mt9v111_core_ops,
+	.video	= &mt9v111_video_ops,
+	.pad	= &mt9v111_pad_ops,
+};
+
+#if IS_ENABLED(CONFIG_MEDIA_CONTROLLER)
+static const struct media_entity_operations mt9v111_subdev_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+#endif
+
+/* --- V4L2 ctrl --- */
+static int mt9v111_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct mt9v111_dev *mt9v111 = container_of(ctrl->handler,
+						   struct mt9v111_dev,
+						   ctrls);
+	int ret;
+
+	mutex_lock(&mt9v111->pwr_mutex);
+	/*
+	 * If sensor is powered down, just cache new control values,
+	 * no actual register access.
+	 */
+	if (!mt9v111->pwr_count) {
+		mt9v111->pending = true;
+		mutex_unlock(&mt9v111->pwr_mutex);
+		return 0;
+	}
+	mutex_unlock(&mt9v111->pwr_mutex);
+
+	/*
+	 * Flickering control gets disabled if both auto exp and auto awb
+	 * are disabled too. If any of the two is enabled, enable it.
+	 *
+	 * Disabling flickering when ae and awb are off allows a more precise
+	 * control of the programmed frame rate.
+	 */
+	if (mt9v111->auto_exp->is_new || mt9v111->auto_awb->is_new) {
+		if (mt9v111->auto_exp->val == V4L2_EXPOSURE_MANUAL &&
+		    mt9v111->auto_awb->val == V4L2_WHITE_BALANCE_MANUAL)
+			ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP,
+					     MT9V111_IFP_R08_OUTFMT_CTRL,
+					     MT9V111_IFP_R08_OUTFMT_CTRL_FLICKER,
+					     0);
+		else
+			ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP,
+					     MT9V111_IFP_R08_OUTFMT_CTRL,
+					     MT9V111_IFP_R08_OUTFMT_CTRL_FLICKER,
+					     1);
+		if (ret)
+			return ret;
+	}
+
+	ret = -EINVAL;
+	switch (ctrl->id) {
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP,
+				     MT9V111_IFP_R06_OPMODE_CTRL,
+				     MT9V111_IFP_R06_OPMODE_CTRL_AWB_EN,
+				     ctrl->val == V4L2_WHITE_BALANCE_AUTO ?
+				     MT9V111_IFP_R06_OPMODE_CTRL_AWB_EN : 0);
+		break;
+	case V4L2_CID_EXPOSURE_AUTO:
+		ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP,
+				     MT9V111_IFP_R06_OPMODE_CTRL,
+				     MT9V111_IFP_R06_OPMODE_CTRL_AE_EN,
+				     ctrl->val == V4L2_EXPOSURE_AUTO ?
+				     MT9V111_IFP_R06_OPMODE_CTRL_AE_EN : 0);
+		break;
+	case V4L2_CID_HBLANK:
+		ret = mt9v111_update(mt9v111->client, MT9V111_R01_CORE,
+				     MT9V111_CORE_R05_HBLANK,
+				     MT9V111_CORE_R05_MAX_HBLANK,
+				     mt9v111->hblank->val);
+		break;
+	case V4L2_CID_VBLANK:
+		ret = mt9v111_update(mt9v111->client, MT9V111_R01_CORE,
+				     MT9V111_CORE_R06_VBLANK,
+				     MT9V111_CORE_R06_MAX_VBLANK,
+				     mt9v111->vblank->val);
+		break;
+	}
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops mt9v111_ctrl_ops = {
+	.s_ctrl = mt9v111_s_ctrl,
+};
+
+static int mt9v111_chip_probe(struct mt9v111_dev *mt9v111)
+{
+	int ret;
+	u16 val;
+
+	ret = __mt9v111_power_on(&mt9v111->sd);
+	if (ret)
+		return ret;
+
+	ret = mt9v111_read(mt9v111->client, MT9V111_R01_CORE,
+			   MT9V111_CORE_RFF_CHIP_VER, &val);
+	if (ret)
+		goto power_off;
+
+	if ((val >> 8) != MT9V111_CHIP_ID_HIGH &&
+	    (val & 0xff) != MT9V111_CHIP_ID_LOW) {
+		dev_err(mt9v111->dev,
+			"Unable to identify MT9V111 chip: 0x%2x%2x\n",
+			val >> 8, val & 0xff);
+		ret = -EIO;
+		goto power_off;
+	}
+
+	dev_dbg(mt9v111->dev, "Chip identified: 0x%2x%2x\n",
+		val >> 8, val & 0xff);
+
+power_off:
+	__mt9v111_power_off(&mt9v111->sd);
+
+	return ret;
+}
+
+static int mt9v111_probe(struct i2c_client *client)
+{
+	struct mt9v111_dev *mt9v111;
+	struct v4l2_fract tpf;
+	int ret;
+
+	mt9v111 = devm_kzalloc(&client->dev, sizeof(*mt9v111), GFP_KERNEL);
+	if (!mt9v111)
+		return -ENOMEM;
+
+	mt9v111->dev = &client->dev;
+	mt9v111->client = client;
+
+	mt9v111->clk = devm_clk_get(&client->dev, NULL);
+	if (IS_ERR(mt9v111->clk))
+		return PTR_ERR(mt9v111->clk);
+
+	mt9v111->sysclk = clk_get_rate(mt9v111->clk);
+	if (mt9v111->sysclk > MT9V111_MAX_CLKIN)
+		return -EINVAL;
+
+	mt9v111->oe = devm_gpiod_get_optional(&client->dev, "enable",
+					      GPIOD_OUT_LOW);
+	if (IS_ERR(mt9v111->oe)) {
+		dev_err(&client->dev, "Unable to get GPIO \"enable\": %ld\n",
+			PTR_ERR(mt9v111->oe));
+		return PTR_ERR(mt9v111->oe);
+	}
+
+	mt9v111->standby = devm_gpiod_get_optional(&client->dev, "standby",
+						   GPIOD_OUT_HIGH);
+	if (IS_ERR(mt9v111->standby)) {
+		dev_err(&client->dev, "Unable to get GPIO \"standby\": %ld\n",
+			PTR_ERR(mt9v111->standby));
+		return PTR_ERR(mt9v111->standby);
+	}
+
+	mt9v111->reset = devm_gpiod_get_optional(&client->dev, "reset",
+						 GPIOD_OUT_LOW);
+	if (IS_ERR(mt9v111->reset)) {
+		dev_err(&client->dev, "Unable to get GPIO \"reset\": %ld\n",
+			PTR_ERR(mt9v111->reset));
+		return PTR_ERR(mt9v111->reset);
+	}
+
+	mutex_init(&mt9v111->pwr_mutex);
+	mutex_init(&mt9v111->stream_mutex);
+
+	v4l2_ctrl_handler_init(&mt9v111->ctrls, 5);
+
+	mt9v111->auto_awb = v4l2_ctrl_new_std(&mt9v111->ctrls,
+					      &mt9v111_ctrl_ops,
+					      V4L2_CID_AUTO_WHITE_BALANCE,
+					      0, 1, 1,
+					      V4L2_WHITE_BALANCE_AUTO);
+	mt9v111->auto_exp = v4l2_ctrl_new_std_menu(&mt9v111->ctrls,
+						   &mt9v111_ctrl_ops,
+						   V4L2_CID_EXPOSURE_AUTO,
+						   V4L2_EXPOSURE_MANUAL,
+						   0, V4L2_EXPOSURE_AUTO);
+	mt9v111->hblank = v4l2_ctrl_new_std(&mt9v111->ctrls, &mt9v111_ctrl_ops,
+					    V4L2_CID_HBLANK,
+					    MT9V111_CORE_R05_MIN_HBLANK,
+					    MT9V111_CORE_R05_MAX_HBLANK, 1,
+					    MT9V111_CORE_R05_DEF_HBLANK);
+	mt9v111->vblank = v4l2_ctrl_new_std(&mt9v111->ctrls, &mt9v111_ctrl_ops,
+					    V4L2_CID_VBLANK,
+					    MT9V111_CORE_R06_MIN_VBLANK,
+					    MT9V111_CORE_R06_MAX_VBLANK, 1,
+					    MT9V111_CORE_R06_DEF_VBLANK);
+
+	/* PIXEL_RATE is fixed: just expose it to user space. */
+	v4l2_ctrl_new_std(&mt9v111->ctrls, &mt9v111_ctrl_ops,
+			  V4L2_CID_PIXEL_RATE, 0,
+			  DIV_ROUND_CLOSEST(mt9v111->sysclk, 2), 1,
+			  DIV_ROUND_CLOSEST(mt9v111->sysclk, 2));
+
+	if (mt9v111->ctrls.error) {
+		ret = mt9v111->ctrls.error;
+		goto error_free_ctrls;
+	}
+	mt9v111->sd.ctrl_handler = &mt9v111->ctrls;
+
+	/* Start with default configuration: 640x480 UYVY. */
+	mt9v111->fmt	= mt9v111_def_fmt;
+
+	/* Re-calculate blankings for 640x480@15fps. */
+	mt9v111->fps		= 15;
+	tpf.numerator		= 1;
+	tpf.denominator		= mt9v111->fps;
+	mt9v111_calc_frame_rate(mt9v111, &tpf);
+
+	mt9v111->pwr_count	= 0;
+	mt9v111->addr_space	= MT9V111_R01_IFP;
+	mt9v111->pending	= true;
+
+	v4l2_i2c_subdev_init(&mt9v111->sd, client, &mt9v111_ops);
+
+#if IS_ENABLED(CONFIG_MEDIA_CONTROLLER)
+	mt9v111->sd.flags	|= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	mt9v111->sd.entity.ops	= &mt9v111_subdev_entity_ops;
+	mt9v111->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+	mt9v111->pad.flags	= MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&mt9v111->sd.entity, 1, &mt9v111->pad);
+	if (ret)
+		goto error_free_entity;
+#endif
+
+	ret = mt9v111_chip_probe(mt9v111);
+	if (ret)
+		goto error_free_entity;
+
+	ret = v4l2_async_register_subdev(&mt9v111->sd);
+	if (ret)
+		goto error_free_entity;
+
+	return 0;
+
+error_free_entity:
+#if IS_ENABLED(CONFIG_MEDIA_CONTROLLER)
+	media_entity_cleanup(&mt9v111->sd.entity);
+#endif
+
+error_free_ctrls:
+	v4l2_ctrl_handler_free(&mt9v111->ctrls);
+
+	mutex_destroy(&mt9v111->pwr_mutex);
+	mutex_destroy(&mt9v111->stream_mutex);
+
+	return ret;
+}
+
+static void mt9v111_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
+
+	v4l2_async_unregister_subdev(sd);
+
+#if IS_ENABLED(CONFIG_MEDIA_CONTROLLER)
+	media_entity_cleanup(&sd->entity);
+#endif
+
+	v4l2_ctrl_handler_free(&mt9v111->ctrls);
+
+	mutex_destroy(&mt9v111->pwr_mutex);
+	mutex_destroy(&mt9v111->stream_mutex);
+}
+
+static const struct of_device_id mt9v111_of_match[] = {
+	{ .compatible = "aptina,mt9v111", },
+	{ /* sentinel */ },
+};
+
+static struct i2c_driver mt9v111_driver = {
+	.driver = {
+		.name = "mt9v111",
+		.of_match_table = mt9v111_of_match,
+	},
+	.probe		= mt9v111_probe,
+	.remove		= mt9v111_remove,
+};
+
+module_i2c_driver(mt9v111_driver);
+
+MODULE_DESCRIPTION("V4L2 sensor driver for Aptina MT9V111");
+MODULE_AUTHOR("Jacopo Mondi <jacopo@jmondi.org>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/og01a1b.c b/drivers/media/i2c/og01a1b.c
new file mode 100644
index 000000000..365ce5684
--- /dev/null
+++ b/drivers/media/i2c/og01a1b.c
@@ -0,0 +1,1126 @@
+// SPDX-License-Identifier: GPL-2.0
+// Copyright (c) 2022 Intel Corporation.
+
+#include <asm/unaligned.h>
+#include <linux/acpi.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+
+#define OG01A1B_REG_VALUE_08BIT		1
+#define OG01A1B_REG_VALUE_16BIT		2
+#define OG01A1B_REG_VALUE_24BIT		3
+
+#define OG01A1B_LINK_FREQ_500MHZ	500000000ULL
+#define OG01A1B_SCLK			120000000LL
+#define OG01A1B_MCLK			19200000
+#define OG01A1B_DATA_LANES		2
+#define OG01A1B_RGB_DEPTH		10
+
+#define OG01A1B_REG_CHIP_ID		0x300a
+#define OG01A1B_CHIP_ID			0x470141
+
+#define OG01A1B_REG_MODE_SELECT		0x0100
+#define OG01A1B_MODE_STANDBY		0x00
+#define OG01A1B_MODE_STREAMING		0x01
+
+/* vertical-timings from sensor */
+#define OG01A1B_REG_VTS			0x380e
+#define OG01A1B_VTS_120FPS		0x0498
+#define OG01A1B_VTS_120FPS_MIN		0x0498
+#define OG01A1B_VTS_MAX			0x7fff
+
+/* horizontal-timings from sensor */
+#define OG01A1B_REG_HTS			0x380c
+
+/* Exposure controls from sensor */
+#define OG01A1B_REG_EXPOSURE		0x3501
+#define	OG01A1B_EXPOSURE_MIN		1
+#define OG01A1B_EXPOSURE_MAX_MARGIN	14
+#define	OG01A1B_EXPOSURE_STEP		1
+
+/* Analog gain controls from sensor */
+#define OG01A1B_REG_ANALOG_GAIN		0x3508
+#define	OG01A1B_ANAL_GAIN_MIN		16
+#define	OG01A1B_ANAL_GAIN_MAX		248 /* Max = 15.5x */
+#define	OG01A1B_ANAL_GAIN_STEP		1
+
+/* Digital gain controls from sensor */
+#define OG01A1B_REG_DIG_GAIN		0x350a
+#define OG01A1B_DGTL_GAIN_MIN		1024
+#define OG01A1B_DGTL_GAIN_MAX		16384 /* Max = 16x */
+#define OG01A1B_DGTL_GAIN_STEP		1
+#define OG01A1B_DGTL_GAIN_DEFAULT	1024
+
+/* Group Access */
+#define OG01A1B_REG_GROUP_ACCESS	0x3208
+#define OG01A1B_GROUP_HOLD_START	0x0
+#define OG01A1B_GROUP_HOLD_END		0x10
+#define OG01A1B_GROUP_HOLD_LAUNCH	0xa0
+
+/* Test Pattern Control */
+#define OG01A1B_REG_TEST_PATTERN	0x5100
+#define OG01A1B_TEST_PATTERN_ENABLE	BIT(7)
+#define OG01A1B_TEST_PATTERN_BAR_SHIFT	2
+
+#define to_og01a1b(_sd)			container_of(_sd, struct og01a1b, sd)
+
+enum {
+	OG01A1B_LINK_FREQ_1000MBPS,
+};
+
+struct og01a1b_reg {
+	u16 address;
+	u8 val;
+};
+
+struct og01a1b_reg_list {
+	u32 num_of_regs;
+	const struct og01a1b_reg *regs;
+};
+
+struct og01a1b_link_freq_config {
+	const struct og01a1b_reg_list reg_list;
+};
+
+struct og01a1b_mode {
+	/* Frame width in pixels */
+	u32 width;
+
+	/* Frame height in pixels */
+	u32 height;
+
+	/* Horizontal timining size */
+	u32 hts;
+
+	/* Default vertical timining size */
+	u32 vts_def;
+
+	/* Min vertical timining size */
+	u32 vts_min;
+
+	/* Link frequency needed for this resolution */
+	u32 link_freq_index;
+
+	/* Sensor register settings for this resolution */
+	const struct og01a1b_reg_list reg_list;
+};
+
+static const struct og01a1b_reg mipi_data_rate_1000mbps[] = {
+	{0x0103, 0x01},
+	{0x0303, 0x02},
+	{0x0304, 0x00},
+	{0x0305, 0xd2},
+	{0x0323, 0x02},
+	{0x0324, 0x01},
+	{0x0325, 0x77},
+};
+
+static const struct og01a1b_reg mode_1280x1024_regs[] = {
+	{0x0300, 0x0a},
+	{0x0301, 0x29},
+	{0x0302, 0x31},
+	{0x0303, 0x02},
+	{0x0304, 0x00},
+	{0x0305, 0xd2},
+	{0x0306, 0x00},
+	{0x0307, 0x01},
+	{0x0308, 0x02},
+	{0x0309, 0x00},
+	{0x0310, 0x00},
+	{0x0311, 0x00},
+	{0x0312, 0x07},
+	{0x0313, 0x00},
+	{0x0314, 0x00},
+	{0x0315, 0x00},
+	{0x0320, 0x02},
+	{0x0321, 0x01},
+	{0x0322, 0x01},
+	{0x0323, 0x02},
+	{0x0324, 0x01},
+	{0x0325, 0x77},
+	{0x0326, 0xce},
+	{0x0327, 0x04},
+	{0x0329, 0x02},
+	{0x032a, 0x04},
+	{0x032b, 0x04},
+	{0x032c, 0x02},
+	{0x032d, 0x01},
+	{0x032e, 0x00},
+	{0x300d, 0x02},
+	{0x300e, 0x04},
+	{0x3021, 0x08},
+	{0x301e, 0x03},
+	{0x3103, 0x00},
+	{0x3106, 0x08},
+	{0x3107, 0x40},
+	{0x3216, 0x01},
+	{0x3217, 0x00},
+	{0x3218, 0xc0},
+	{0x3219, 0x55},
+	{0x3500, 0x00},
+	{0x3501, 0x04},
+	{0x3502, 0x8a},
+	{0x3506, 0x01},
+	{0x3507, 0x72},
+	{0x3508, 0x01},
+	{0x3509, 0x00},
+	{0x350a, 0x01},
+	{0x350b, 0x00},
+	{0x350c, 0x00},
+	{0x3541, 0x00},
+	{0x3542, 0x40},
+	{0x3605, 0xe0},
+	{0x3606, 0x41},
+	{0x3614, 0x20},
+	{0x3620, 0x0b},
+	{0x3630, 0x07},
+	{0x3636, 0xa0},
+	{0x3637, 0xf9},
+	{0x3638, 0x09},
+	{0x3639, 0x38},
+	{0x363f, 0x09},
+	{0x3640, 0x17},
+	{0x3662, 0x04},
+	{0x3665, 0x80},
+	{0x3670, 0x68},
+	{0x3674, 0x00},
+	{0x3677, 0x3f},
+	{0x3679, 0x00},
+	{0x369f, 0x19},
+	{0x36a0, 0x03},
+	{0x36a2, 0x19},
+	{0x36a3, 0x03},
+	{0x370d, 0x66},
+	{0x370f, 0x00},
+	{0x3710, 0x03},
+	{0x3715, 0x03},
+	{0x3716, 0x03},
+	{0x3717, 0x06},
+	{0x3733, 0x00},
+	{0x3778, 0x00},
+	{0x37a8, 0x0f},
+	{0x37a9, 0x01},
+	{0x37aa, 0x07},
+	{0x37bd, 0x1c},
+	{0x37c1, 0x2f},
+	{0x37c3, 0x09},
+	{0x37c8, 0x1d},
+	{0x37ca, 0x30},
+	{0x37df, 0x00},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x00},
+	{0x3804, 0x05},
+	{0x3805, 0x0f},
+	{0x3806, 0x04},
+	{0x3807, 0x0f},
+	{0x3808, 0x05},
+	{0x3809, 0x00},
+	{0x380a, 0x04},
+	{0x380b, 0x00},
+	{0x380c, 0x03},
+	{0x380d, 0x50},
+	{0x380e, 0x04},
+	{0x380f, 0x98},
+	{0x3810, 0x00},
+	{0x3811, 0x08},
+	{0x3812, 0x00},
+	{0x3813, 0x08},
+	{0x3814, 0x11},
+	{0x3815, 0x11},
+	{0x3820, 0x40},
+	{0x3821, 0x04},
+	{0x3826, 0x00},
+	{0x3827, 0x00},
+	{0x382a, 0x08},
+	{0x382b, 0x52},
+	{0x382d, 0xba},
+	{0x383d, 0x14},
+	{0x384a, 0xa2},
+	{0x3866, 0x0e},
+	{0x3867, 0x07},
+	{0x3884, 0x00},
+	{0x3885, 0x08},
+	{0x3893, 0x68},
+	{0x3894, 0x2a},
+	{0x3898, 0x00},
+	{0x3899, 0x31},
+	{0x389a, 0x04},
+	{0x389b, 0x00},
+	{0x389c, 0x0b},
+	{0x389d, 0xad},
+	{0x389f, 0x08},
+	{0x38a0, 0x00},
+	{0x38a1, 0x00},
+	{0x38a8, 0x70},
+	{0x38ac, 0xea},
+	{0x38b2, 0x00},
+	{0x38b3, 0x08},
+	{0x38bc, 0x20},
+	{0x38c4, 0x0c},
+	{0x38c5, 0x3a},
+	{0x38c7, 0x3a},
+	{0x38e1, 0xc0},
+	{0x38ec, 0x3c},
+	{0x38f0, 0x09},
+	{0x38f1, 0x6f},
+	{0x38fe, 0x3c},
+	{0x391e, 0x00},
+	{0x391f, 0x00},
+	{0x3920, 0xa5},
+	{0x3921, 0x00},
+	{0x3922, 0x00},
+	{0x3923, 0x00},
+	{0x3924, 0x05},
+	{0x3925, 0x00},
+	{0x3926, 0x00},
+	{0x3927, 0x00},
+	{0x3928, 0x1a},
+	{0x3929, 0x01},
+	{0x392a, 0xb4},
+	{0x392b, 0x00},
+	{0x392c, 0x10},
+	{0x392f, 0x40},
+	{0x4000, 0xcf},
+	{0x4003, 0x40},
+	{0x4008, 0x00},
+	{0x4009, 0x07},
+	{0x400a, 0x02},
+	{0x400b, 0x54},
+	{0x400c, 0x00},
+	{0x400d, 0x07},
+	{0x4010, 0xc0},
+	{0x4012, 0x02},
+	{0x4014, 0x04},
+	{0x4015, 0x04},
+	{0x4017, 0x02},
+	{0x4042, 0x01},
+	{0x4306, 0x04},
+	{0x4307, 0x12},
+	{0x4509, 0x00},
+	{0x450b, 0x83},
+	{0x4604, 0x68},
+	{0x4608, 0x0a},
+	{0x4700, 0x06},
+	{0x4800, 0x64},
+	{0x481b, 0x3c},
+	{0x4825, 0x32},
+	{0x4833, 0x18},
+	{0x4837, 0x0f},
+	{0x4850, 0x40},
+	{0x4860, 0x00},
+	{0x4861, 0xec},
+	{0x4864, 0x00},
+	{0x4883, 0x00},
+	{0x4888, 0x90},
+	{0x4889, 0x05},
+	{0x488b, 0x04},
+	{0x4f00, 0x04},
+	{0x4f10, 0x04},
+	{0x4f21, 0x01},
+	{0x4f22, 0x40},
+	{0x4f23, 0x44},
+	{0x4f24, 0x51},
+	{0x4f25, 0x41},
+	{0x5000, 0x1f},
+	{0x500a, 0x00},
+	{0x5100, 0x00},
+	{0x5111, 0x20},
+	{0x3020, 0x20},
+	{0x3613, 0x03},
+	{0x38c9, 0x02},
+	{0x5304, 0x01},
+	{0x3620, 0x08},
+	{0x3639, 0x58},
+	{0x363a, 0x10},
+	{0x3674, 0x04},
+	{0x3780, 0xff},
+	{0x3781, 0xff},
+	{0x3782, 0x00},
+	{0x3783, 0x01},
+	{0x3798, 0xa3},
+	{0x37aa, 0x10},
+	{0x38a8, 0xf0},
+	{0x38c4, 0x09},
+	{0x38c5, 0xb0},
+	{0x38df, 0x80},
+	{0x38ff, 0x05},
+	{0x4010, 0xf1},
+	{0x4011, 0x70},
+	{0x3667, 0x80},
+	{0x4d00, 0x4a},
+	{0x4d01, 0x18},
+	{0x4d02, 0xbb},
+	{0x4d03, 0xde},
+	{0x4d04, 0x93},
+	{0x4d05, 0xff},
+	{0x4d09, 0x0a},
+	{0x37aa, 0x16},
+	{0x3606, 0x42},
+	{0x3605, 0x00},
+	{0x36a2, 0x17},
+	{0x300d, 0x0a},
+	{0x4d00, 0x4d},
+	{0x4d01, 0x95},
+	{0x3d8C, 0x70},
+	{0x3d8d, 0xE9},
+	{0x5300, 0x00},
+	{0x5301, 0x10},
+	{0x5302, 0x00},
+	{0x5303, 0xE3},
+	{0x3d88, 0x00},
+	{0x3d89, 0x10},
+	{0x3d8a, 0x00},
+	{0x3d8b, 0xE3},
+	{0x4f22, 0x00},
+};
+
+static const char * const og01a1b_test_pattern_menu[] = {
+	"Disabled",
+	"Standard Color Bar",
+	"Top-Bottom Darker Color Bar",
+	"Right-Left Darker Color Bar",
+	"Bottom-Top Darker Color Bar"
+};
+
+static const s64 link_freq_menu_items[] = {
+	OG01A1B_LINK_FREQ_500MHZ,
+};
+
+static const struct og01a1b_link_freq_config link_freq_configs[] = {
+	[OG01A1B_LINK_FREQ_1000MBPS] = {
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mipi_data_rate_1000mbps),
+			.regs = mipi_data_rate_1000mbps,
+		}
+	}
+};
+
+static const struct og01a1b_mode supported_modes[] = {
+	{
+		.width = 1280,
+		.height = 1024,
+		.hts = 848,
+		.vts_def = OG01A1B_VTS_120FPS,
+		.vts_min = OG01A1B_VTS_120FPS_MIN,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1280x1024_regs),
+			.regs = mode_1280x1024_regs,
+		},
+		.link_freq_index = OG01A1B_LINK_FREQ_1000MBPS,
+	},
+};
+
+struct og01a1b {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct v4l2_ctrl_handler ctrl_handler;
+
+	/* V4L2 Controls */
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *pixel_rate;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *exposure;
+
+	/* Current mode */
+	const struct og01a1b_mode *cur_mode;
+
+	/* To serialize asynchronus callbacks */
+	struct mutex mutex;
+
+	/* Streaming on/off */
+	bool streaming;
+};
+
+static u64 to_pixel_rate(u32 f_index)
+{
+	u64 pixel_rate = link_freq_menu_items[f_index] * 2 * OG01A1B_DATA_LANES;
+
+	do_div(pixel_rate, OG01A1B_RGB_DEPTH);
+
+	return pixel_rate;
+}
+
+static u64 to_pixels_per_line(u32 hts, u32 f_index)
+{
+	u64 ppl = hts * to_pixel_rate(f_index);
+
+	do_div(ppl, OG01A1B_SCLK);
+
+	return ppl;
+}
+
+static int og01a1b_read_reg(struct og01a1b *og01a1b, u16 reg, u16 len, u32 *val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&og01a1b->sd);
+	struct i2c_msg msgs[2];
+	u8 addr_buf[2];
+	u8 data_buf[4] = {0};
+	int ret;
+
+	if (len > 4)
+		return -EINVAL;
+
+	put_unaligned_be16(reg, addr_buf);
+	msgs[0].addr = client->addr;
+	msgs[0].flags = 0;
+	msgs[0].len = sizeof(addr_buf);
+	msgs[0].buf = addr_buf;
+	msgs[1].addr = client->addr;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].len = len;
+	msgs[1].buf = &data_buf[4 - len];
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret != ARRAY_SIZE(msgs))
+		return -EIO;
+
+	*val = get_unaligned_be32(data_buf);
+
+	return 0;
+}
+
+static int og01a1b_write_reg(struct og01a1b *og01a1b, u16 reg, u16 len, u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&og01a1b->sd);
+	u8 buf[6];
+
+	if (len > 4)
+		return -EINVAL;
+
+	put_unaligned_be16(reg, buf);
+	put_unaligned_be32(val << 8 * (4 - len), buf + 2);
+	if (i2c_master_send(client, buf, len + 2) != len + 2)
+		return -EIO;
+
+	return 0;
+}
+
+static int og01a1b_write_reg_list(struct og01a1b *og01a1b,
+				  const struct og01a1b_reg_list *r_list)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&og01a1b->sd);
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < r_list->num_of_regs; i++) {
+		ret = og01a1b_write_reg(og01a1b, r_list->regs[i].address, 1,
+					r_list->regs[i].val);
+		if (ret) {
+			dev_err_ratelimited(&client->dev,
+					    "failed to write reg 0x%4.4x. error = %d",
+					    r_list->regs[i].address, ret);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int og01a1b_test_pattern(struct og01a1b *og01a1b, u32 pattern)
+{
+	if (pattern)
+		pattern = (pattern - 1) << OG01A1B_TEST_PATTERN_BAR_SHIFT |
+			  OG01A1B_TEST_PATTERN_ENABLE;
+
+	return og01a1b_write_reg(og01a1b, OG01A1B_REG_TEST_PATTERN,
+				 OG01A1B_REG_VALUE_08BIT, pattern);
+}
+
+static int og01a1b_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct og01a1b *og01a1b = container_of(ctrl->handler,
+					       struct og01a1b, ctrl_handler);
+	struct i2c_client *client = v4l2_get_subdevdata(&og01a1b->sd);
+	s64 exposure_max;
+	int ret = 0;
+
+	/* Propagate change of current control to all related controls */
+	if (ctrl->id == V4L2_CID_VBLANK) {
+		/* Update max exposure while meeting expected vblanking */
+		exposure_max = og01a1b->cur_mode->height + ctrl->val -
+			       OG01A1B_EXPOSURE_MAX_MARGIN;
+		__v4l2_ctrl_modify_range(og01a1b->exposure,
+					 og01a1b->exposure->minimum,
+					 exposure_max, og01a1b->exposure->step,
+					 exposure_max);
+	}
+
+	/* V4L2 controls values will be applied only when power is already up */
+	if (!pm_runtime_get_if_in_use(&client->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_ANALOGUE_GAIN:
+		ret = og01a1b_write_reg(og01a1b, OG01A1B_REG_ANALOG_GAIN,
+					OG01A1B_REG_VALUE_16BIT,
+					ctrl->val << 4);
+		break;
+
+	case V4L2_CID_DIGITAL_GAIN:
+		ret = og01a1b_write_reg(og01a1b, OG01A1B_REG_DIG_GAIN,
+					OG01A1B_REG_VALUE_24BIT,
+					ctrl->val << 6);
+		break;
+
+	case V4L2_CID_EXPOSURE:
+		ret = og01a1b_write_reg(og01a1b, OG01A1B_REG_EXPOSURE,
+					OG01A1B_REG_VALUE_16BIT, ctrl->val);
+		break;
+
+	case V4L2_CID_VBLANK:
+		ret = og01a1b_write_reg(og01a1b, OG01A1B_REG_VTS,
+					OG01A1B_REG_VALUE_16BIT,
+					og01a1b->cur_mode->height + ctrl->val);
+		break;
+
+	case V4L2_CID_TEST_PATTERN:
+		ret = og01a1b_test_pattern(og01a1b, ctrl->val);
+		break;
+
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	pm_runtime_put(&client->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops og01a1b_ctrl_ops = {
+	.s_ctrl = og01a1b_set_ctrl,
+};
+
+static int og01a1b_init_controls(struct og01a1b *og01a1b)
+{
+	struct v4l2_ctrl_handler *ctrl_hdlr;
+	s64 exposure_max, h_blank;
+	int ret;
+
+	ctrl_hdlr = &og01a1b->ctrl_handler;
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 8);
+	if (ret)
+		return ret;
+
+	ctrl_hdlr->lock = &og01a1b->mutex;
+	og01a1b->link_freq = v4l2_ctrl_new_int_menu(ctrl_hdlr,
+						    &og01a1b_ctrl_ops,
+						    V4L2_CID_LINK_FREQ,
+						    ARRAY_SIZE
+						    (link_freq_menu_items) - 1,
+						    0, link_freq_menu_items);
+	if (og01a1b->link_freq)
+		og01a1b->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	og01a1b->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, &og01a1b_ctrl_ops,
+						V4L2_CID_PIXEL_RATE, 0,
+						to_pixel_rate
+						(OG01A1B_LINK_FREQ_1000MBPS),
+						1,
+						to_pixel_rate
+						(OG01A1B_LINK_FREQ_1000MBPS));
+	og01a1b->vblank = v4l2_ctrl_new_std(ctrl_hdlr, &og01a1b_ctrl_ops,
+					    V4L2_CID_VBLANK,
+					    og01a1b->cur_mode->vts_min -
+					    og01a1b->cur_mode->height,
+					    OG01A1B_VTS_MAX -
+					    og01a1b->cur_mode->height, 1,
+					    og01a1b->cur_mode->vts_def -
+					    og01a1b->cur_mode->height);
+	h_blank = to_pixels_per_line(og01a1b->cur_mode->hts,
+				     og01a1b->cur_mode->link_freq_index) -
+				     og01a1b->cur_mode->width;
+	og01a1b->hblank = v4l2_ctrl_new_std(ctrl_hdlr, &og01a1b_ctrl_ops,
+					    V4L2_CID_HBLANK, h_blank, h_blank,
+					    1, h_blank);
+	if (og01a1b->hblank)
+		og01a1b->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &og01a1b_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
+			  OG01A1B_ANAL_GAIN_MIN, OG01A1B_ANAL_GAIN_MAX,
+			  OG01A1B_ANAL_GAIN_STEP, OG01A1B_ANAL_GAIN_MIN);
+	v4l2_ctrl_new_std(ctrl_hdlr, &og01a1b_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
+			  OG01A1B_DGTL_GAIN_MIN, OG01A1B_DGTL_GAIN_MAX,
+			  OG01A1B_DGTL_GAIN_STEP, OG01A1B_DGTL_GAIN_DEFAULT);
+	exposure_max = (og01a1b->cur_mode->vts_def -
+			OG01A1B_EXPOSURE_MAX_MARGIN);
+	og01a1b->exposure = v4l2_ctrl_new_std(ctrl_hdlr, &og01a1b_ctrl_ops,
+					      V4L2_CID_EXPOSURE,
+					      OG01A1B_EXPOSURE_MIN,
+					      exposure_max,
+					      OG01A1B_EXPOSURE_STEP,
+					      exposure_max);
+	v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &og01a1b_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(og01a1b_test_pattern_menu) - 1,
+				     0, 0, og01a1b_test_pattern_menu);
+
+	if (ctrl_hdlr->error)
+		return ctrl_hdlr->error;
+
+	og01a1b->sd.ctrl_handler = ctrl_hdlr;
+
+	return 0;
+}
+
+static void og01a1b_update_pad_format(const struct og01a1b_mode *mode,
+				      struct v4l2_mbus_framefmt *fmt)
+{
+	fmt->width = mode->width;
+	fmt->height = mode->height;
+	fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	fmt->field = V4L2_FIELD_NONE;
+}
+
+static int og01a1b_start_streaming(struct og01a1b *og01a1b)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&og01a1b->sd);
+	const struct og01a1b_reg_list *reg_list;
+	int link_freq_index, ret;
+
+	link_freq_index = og01a1b->cur_mode->link_freq_index;
+	reg_list = &link_freq_configs[link_freq_index].reg_list;
+
+	ret = og01a1b_write_reg_list(og01a1b, reg_list);
+	if (ret) {
+		dev_err(&client->dev, "failed to set plls");
+		return ret;
+	}
+
+	reg_list = &og01a1b->cur_mode->reg_list;
+	ret = og01a1b_write_reg_list(og01a1b, reg_list);
+	if (ret) {
+		dev_err(&client->dev, "failed to set mode");
+		return ret;
+	}
+
+	ret = __v4l2_ctrl_handler_setup(og01a1b->sd.ctrl_handler);
+	if (ret)
+		return ret;
+
+	ret = og01a1b_write_reg(og01a1b, OG01A1B_REG_MODE_SELECT,
+				OG01A1B_REG_VALUE_08BIT,
+				OG01A1B_MODE_STREAMING);
+	if (ret) {
+		dev_err(&client->dev, "failed to set stream");
+		return ret;
+	}
+
+	return 0;
+}
+
+static void og01a1b_stop_streaming(struct og01a1b *og01a1b)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&og01a1b->sd);
+
+	if (og01a1b_write_reg(og01a1b, OG01A1B_REG_MODE_SELECT,
+			      OG01A1B_REG_VALUE_08BIT, OG01A1B_MODE_STANDBY))
+		dev_err(&client->dev, "failed to set stream");
+}
+
+static int og01a1b_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct og01a1b *og01a1b = to_og01a1b(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	if (og01a1b->streaming == enable)
+		return 0;
+
+	mutex_lock(&og01a1b->mutex);
+	if (enable) {
+		ret = pm_runtime_get_sync(&client->dev);
+		if (ret < 0) {
+			pm_runtime_put_noidle(&client->dev);
+			mutex_unlock(&og01a1b->mutex);
+			return ret;
+		}
+
+		ret = og01a1b_start_streaming(og01a1b);
+		if (ret) {
+			enable = 0;
+			og01a1b_stop_streaming(og01a1b);
+			pm_runtime_put(&client->dev);
+		}
+	} else {
+		og01a1b_stop_streaming(og01a1b);
+		pm_runtime_put(&client->dev);
+	}
+
+	og01a1b->streaming = enable;
+	mutex_unlock(&og01a1b->mutex);
+
+	return ret;
+}
+
+static int __maybe_unused og01a1b_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct og01a1b *og01a1b = to_og01a1b(sd);
+
+	mutex_lock(&og01a1b->mutex);
+	if (og01a1b->streaming)
+		og01a1b_stop_streaming(og01a1b);
+
+	mutex_unlock(&og01a1b->mutex);
+
+	return 0;
+}
+
+static int __maybe_unused og01a1b_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct og01a1b *og01a1b = to_og01a1b(sd);
+	int ret;
+
+	mutex_lock(&og01a1b->mutex);
+	if (og01a1b->streaming) {
+		ret = og01a1b_start_streaming(og01a1b);
+		if (ret) {
+			og01a1b->streaming = false;
+			og01a1b_stop_streaming(og01a1b);
+			mutex_unlock(&og01a1b->mutex);
+			return ret;
+		}
+	}
+
+	mutex_unlock(&og01a1b->mutex);
+
+	return 0;
+}
+
+static int og01a1b_set_format(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_format *fmt)
+{
+	struct og01a1b *og01a1b = to_og01a1b(sd);
+	const struct og01a1b_mode *mode;
+	s32 vblank_def, h_blank;
+
+	mode = v4l2_find_nearest_size(supported_modes,
+				      ARRAY_SIZE(supported_modes), width,
+				      height, fmt->format.width,
+				      fmt->format.height);
+
+	mutex_lock(&og01a1b->mutex);
+	og01a1b_update_pad_format(mode, &fmt->format);
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		*v4l2_subdev_get_try_format(sd, sd_state,
+					    fmt->pad) = fmt->format;
+	} else {
+		og01a1b->cur_mode = mode;
+		__v4l2_ctrl_s_ctrl(og01a1b->link_freq, mode->link_freq_index);
+		__v4l2_ctrl_s_ctrl_int64(og01a1b->pixel_rate,
+					 to_pixel_rate(mode->link_freq_index));
+
+		/* Update limits and set FPS to default */
+		vblank_def = mode->vts_def - mode->height;
+		__v4l2_ctrl_modify_range(og01a1b->vblank,
+					 mode->vts_min - mode->height,
+					 OG01A1B_VTS_MAX - mode->height, 1,
+					 vblank_def);
+		__v4l2_ctrl_s_ctrl(og01a1b->vblank, vblank_def);
+		h_blank = to_pixels_per_line(mode->hts, mode->link_freq_index) -
+			  mode->width;
+		__v4l2_ctrl_modify_range(og01a1b->hblank, h_blank, h_blank, 1,
+					 h_blank);
+	}
+
+	mutex_unlock(&og01a1b->mutex);
+
+	return 0;
+}
+
+static int og01a1b_get_format(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_format *fmt)
+{
+	struct og01a1b *og01a1b = to_og01a1b(sd);
+
+	mutex_lock(&og01a1b->mutex);
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
+		fmt->format = *v4l2_subdev_get_try_format(&og01a1b->sd,
+							  sd_state,
+							  fmt->pad);
+	else
+		og01a1b_update_pad_format(og01a1b->cur_mode, &fmt->format);
+
+	mutex_unlock(&og01a1b->mutex);
+
+	return 0;
+}
+
+static int og01a1b_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index > 0)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+
+	return 0;
+}
+
+static int og01a1b_enum_frame_size(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->index >= ARRAY_SIZE(supported_modes))
+		return -EINVAL;
+
+	if (fse->code != MEDIA_BUS_FMT_SGRBG10_1X10)
+		return -EINVAL;
+
+	fse->min_width = supported_modes[fse->index].width;
+	fse->max_width = fse->min_width;
+	fse->min_height = supported_modes[fse->index].height;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static int og01a1b_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct og01a1b *og01a1b = to_og01a1b(sd);
+
+	mutex_lock(&og01a1b->mutex);
+	og01a1b_update_pad_format(&supported_modes[0],
+				  v4l2_subdev_get_try_format(sd, fh->state, 0));
+	mutex_unlock(&og01a1b->mutex);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops og01a1b_video_ops = {
+	.s_stream = og01a1b_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops og01a1b_pad_ops = {
+	.set_fmt = og01a1b_set_format,
+	.get_fmt = og01a1b_get_format,
+	.enum_mbus_code = og01a1b_enum_mbus_code,
+	.enum_frame_size = og01a1b_enum_frame_size,
+};
+
+static const struct v4l2_subdev_ops og01a1b_subdev_ops = {
+	.video = &og01a1b_video_ops,
+	.pad = &og01a1b_pad_ops,
+};
+
+static const struct media_entity_operations og01a1b_subdev_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static const struct v4l2_subdev_internal_ops og01a1b_internal_ops = {
+	.open = og01a1b_open,
+};
+
+static int og01a1b_identify_module(struct og01a1b *og01a1b)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&og01a1b->sd);
+	int ret;
+	u32 val;
+
+	ret = og01a1b_read_reg(og01a1b, OG01A1B_REG_CHIP_ID,
+			       OG01A1B_REG_VALUE_24BIT, &val);
+	if (ret)
+		return ret;
+
+	if (val != OG01A1B_CHIP_ID) {
+		dev_err(&client->dev, "chip id mismatch: %x!=%x",
+			OG01A1B_CHIP_ID, val);
+		return -ENXIO;
+	}
+
+	return 0;
+}
+
+static int og01a1b_check_hwcfg(struct device *dev)
+{
+	struct fwnode_handle *ep;
+	struct fwnode_handle *fwnode = dev_fwnode(dev);
+	struct v4l2_fwnode_endpoint bus_cfg = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY
+	};
+	u32 mclk;
+	int ret;
+	unsigned int i, j;
+
+	if (!fwnode)
+		return -ENXIO;
+
+	ret = fwnode_property_read_u32(fwnode, "clock-frequency", &mclk);
+
+	if (ret) {
+		dev_err(dev, "can't get clock frequency");
+		return ret;
+	}
+
+	if (mclk != OG01A1B_MCLK) {
+		dev_err(dev, "external clock %d is not supported", mclk);
+		return -EINVAL;
+	}
+
+	ep = fwnode_graph_get_next_endpoint(fwnode, NULL);
+	if (!ep)
+		return -ENXIO;
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(ep, &bus_cfg);
+	fwnode_handle_put(ep);
+	if (ret)
+		return ret;
+
+	if (bus_cfg.bus.mipi_csi2.num_data_lanes != OG01A1B_DATA_LANES) {
+		dev_err(dev, "number of CSI2 data lanes %d is not supported",
+			bus_cfg.bus.mipi_csi2.num_data_lanes);
+		ret = -EINVAL;
+		goto check_hwcfg_error;
+	}
+
+	if (!bus_cfg.nr_of_link_frequencies) {
+		dev_err(dev, "no link frequencies defined");
+		ret = -EINVAL;
+		goto check_hwcfg_error;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(link_freq_menu_items); i++) {
+		for (j = 0; j < bus_cfg.nr_of_link_frequencies; j++) {
+			if (link_freq_menu_items[i] ==
+				bus_cfg.link_frequencies[j])
+				break;
+		}
+
+		if (j == bus_cfg.nr_of_link_frequencies) {
+			dev_err(dev, "no link frequency %lld supported",
+				link_freq_menu_items[i]);
+			ret = -EINVAL;
+			goto check_hwcfg_error;
+		}
+	}
+
+check_hwcfg_error:
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+
+	return ret;
+}
+
+static void og01a1b_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct og01a1b *og01a1b = to_og01a1b(sd);
+
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+	pm_runtime_disable(&client->dev);
+	mutex_destroy(&og01a1b->mutex);
+}
+
+static int og01a1b_probe(struct i2c_client *client)
+{
+	struct og01a1b *og01a1b;
+	int ret;
+
+	ret = og01a1b_check_hwcfg(&client->dev);
+	if (ret) {
+		dev_err(&client->dev, "failed to check HW configuration: %d",
+			ret);
+		return ret;
+	}
+
+	og01a1b = devm_kzalloc(&client->dev, sizeof(*og01a1b), GFP_KERNEL);
+	if (!og01a1b)
+		return -ENOMEM;
+
+	v4l2_i2c_subdev_init(&og01a1b->sd, client, &og01a1b_subdev_ops);
+	ret = og01a1b_identify_module(og01a1b);
+	if (ret) {
+		dev_err(&client->dev, "failed to find sensor: %d", ret);
+		return ret;
+	}
+
+	mutex_init(&og01a1b->mutex);
+	og01a1b->cur_mode = &supported_modes[0];
+	ret = og01a1b_init_controls(og01a1b);
+	if (ret) {
+		dev_err(&client->dev, "failed to init controls: %d", ret);
+		goto probe_error_v4l2_ctrl_handler_free;
+	}
+
+	og01a1b->sd.internal_ops = &og01a1b_internal_ops;
+	og01a1b->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	og01a1b->sd.entity.ops = &og01a1b_subdev_entity_ops;
+	og01a1b->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	og01a1b->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&og01a1b->sd.entity, 1, &og01a1b->pad);
+	if (ret) {
+		dev_err(&client->dev, "failed to init entity pads: %d", ret);
+		goto probe_error_v4l2_ctrl_handler_free;
+	}
+
+	ret = v4l2_async_register_subdev_sensor(&og01a1b->sd);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed to register V4L2 subdev: %d",
+			ret);
+		goto probe_error_media_entity_cleanup;
+	}
+
+	/*
+	 * Device is already turned on by i2c-core with ACPI domain PM.
+	 * Enable runtime PM and turn off the device.
+	 */
+	pm_runtime_set_active(&client->dev);
+	pm_runtime_enable(&client->dev);
+	pm_runtime_idle(&client->dev);
+
+	return 0;
+
+probe_error_media_entity_cleanup:
+	media_entity_cleanup(&og01a1b->sd.entity);
+
+probe_error_v4l2_ctrl_handler_free:
+	v4l2_ctrl_handler_free(og01a1b->sd.ctrl_handler);
+	mutex_destroy(&og01a1b->mutex);
+
+	return ret;
+}
+
+static const struct dev_pm_ops og01a1b_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(og01a1b_suspend, og01a1b_resume)
+};
+
+#ifdef CONFIG_ACPI
+static const struct acpi_device_id og01a1b_acpi_ids[] = {
+	{"OVTI01AC"},
+	{}
+};
+
+MODULE_DEVICE_TABLE(acpi, og01a1b_acpi_ids);
+#endif
+
+static struct i2c_driver og01a1b_i2c_driver = {
+	.driver = {
+		.name = "og01a1b",
+		.pm = &og01a1b_pm_ops,
+		.acpi_match_table = ACPI_PTR(og01a1b_acpi_ids),
+	},
+	.probe = og01a1b_probe,
+	.remove = og01a1b_remove,
+};
+
+module_i2c_driver(og01a1b_i2c_driver);
+
+MODULE_AUTHOR("Shawn Tu");
+MODULE_DESCRIPTION("OmniVision OG01A1B sensor driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/ov01a10.c b/drivers/media/i2c/ov01a10.c
new file mode 100644
index 000000000..9afe9bf50
--- /dev/null
+++ b/drivers/media/i2c/ov01a10.c
@@ -0,0 +1,1014 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2023 Intel Corporation.
+ */
+
+#include <asm/unaligned.h>
+
+#include <linux/acpi.h>
+#include <linux/bitfield.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+
+#define OV01A10_LINK_FREQ_400MHZ	400000000ULL
+#define OV01A10_SCLK			40000000LL
+#define OV01A10_DATA_LANES		1
+
+#define OV01A10_REG_CHIP_ID		0x300a
+#define OV01A10_CHIP_ID			0x560141
+
+#define OV01A10_REG_MODE_SELECT		0x0100
+#define OV01A10_MODE_STANDBY		0x00
+#define OV01A10_MODE_STREAMING		0x01
+
+/* pixel array */
+#define OV01A10_PIXEL_ARRAY_WIDTH	1296
+#define OV01A10_PIXEL_ARRAY_HEIGHT	816
+#define OV01A10_ACITVE_WIDTH		1280
+#define OV01A10_ACITVE_HEIGHT		800
+
+/* vertical and horizontal timings */
+#define OV01A10_REG_VTS			0x380e
+#define OV01A10_VTS_DEF			0x0380
+#define OV01A10_VTS_MIN			0x0380
+#define OV01A10_VTS_MAX			0xffff
+#define OV01A10_HTS_DEF			1488
+
+/* exposure controls */
+#define OV01A10_REG_EXPOSURE		0x3501
+#define OV01A10_EXPOSURE_MIN		4
+#define OV01A10_EXPOSURE_MAX_MARGIN	8
+#define OV01A10_EXPOSURE_STEP		1
+
+/* analog gain controls */
+#define OV01A10_REG_ANALOG_GAIN		0x3508
+#define OV01A10_ANAL_GAIN_MIN		0x100
+#define OV01A10_ANAL_GAIN_MAX		0xffff
+#define OV01A10_ANAL_GAIN_STEP		1
+
+/* digital gain controls */
+#define OV01A10_REG_DIGITAL_GAIN_B	0x350a
+#define OV01A10_REG_DIGITAL_GAIN_GB	0x3510
+#define OV01A10_REG_DIGITAL_GAIN_GR	0x3513
+#define OV01A10_REG_DIGITAL_GAIN_R	0x3516
+#define OV01A10_DGTL_GAIN_MIN		0
+#define OV01A10_DGTL_GAIN_MAX		0x3ffff
+#define OV01A10_DGTL_GAIN_STEP		1
+#define OV01A10_DGTL_GAIN_DEFAULT	1024
+
+/* test pattern control */
+#define OV01A10_REG_TEST_PATTERN	0x4503
+#define OV01A10_TEST_PATTERN_ENABLE	BIT(7)
+#define OV01A10_LINK_FREQ_400MHZ_INDEX	0
+
+/* flip and mirror control */
+#define OV01A10_REG_FORMAT1		0x3820
+#define OV01A10_VFLIP_MASK		BIT(4)
+#define OV01A10_HFLIP_MASK		BIT(3)
+
+/* window offset */
+#define OV01A10_REG_X_WIN		0x3811
+#define OV01A10_REG_Y_WIN		0x3813
+
+struct ov01a10_reg {
+	u16 address;
+	u8 val;
+};
+
+struct ov01a10_reg_list {
+	u32 num_of_regs;
+	const struct ov01a10_reg *regs;
+};
+
+struct ov01a10_link_freq_config {
+	const struct ov01a10_reg_list reg_list;
+};
+
+struct ov01a10_mode {
+	u32 width;
+	u32 height;
+	u32 hts;
+	u32 vts_def;
+	u32 vts_min;
+	u32 link_freq_index;
+
+	const struct ov01a10_reg_list reg_list;
+};
+
+static const struct ov01a10_reg mipi_data_rate_720mbps[] = {
+	{0x0103, 0x01},
+	{0x0302, 0x00},
+	{0x0303, 0x06},
+	{0x0304, 0x01},
+	{0x0305, 0xe0},
+	{0x0306, 0x00},
+	{0x0308, 0x01},
+	{0x0309, 0x00},
+	{0x030c, 0x01},
+	{0x0322, 0x01},
+	{0x0323, 0x06},
+	{0x0324, 0x01},
+	{0x0325, 0x68},
+};
+
+static const struct ov01a10_reg sensor_1280x800_setting[] = {
+	{0x3002, 0xa1},
+	{0x301e, 0xf0},
+	{0x3022, 0x01},
+	{0x3501, 0x03},
+	{0x3502, 0x78},
+	{0x3504, 0x0c},
+	{0x3508, 0x01},
+	{0x3509, 0x00},
+	{0x3601, 0xc0},
+	{0x3603, 0x71},
+	{0x3610, 0x68},
+	{0x3611, 0x86},
+	{0x3640, 0x10},
+	{0x3641, 0x80},
+	{0x3642, 0xdc},
+	{0x3646, 0x55},
+	{0x3647, 0x57},
+	{0x364b, 0x00},
+	{0x3653, 0x10},
+	{0x3655, 0x00},
+	{0x3656, 0x00},
+	{0x365f, 0x0f},
+	{0x3661, 0x45},
+	{0x3662, 0x24},
+	{0x3663, 0x11},
+	{0x3664, 0x07},
+	{0x3709, 0x34},
+	{0x370b, 0x6f},
+	{0x3714, 0x22},
+	{0x371b, 0x27},
+	{0x371c, 0x67},
+	{0x371d, 0xa7},
+	{0x371e, 0xe7},
+	{0x3730, 0x81},
+	{0x3733, 0x10},
+	{0x3734, 0x40},
+	{0x3737, 0x04},
+	{0x3739, 0x1c},
+	{0x3767, 0x00},
+	{0x376c, 0x81},
+	{0x3772, 0x14},
+	{0x37c2, 0x04},
+	{0x37d8, 0x03},
+	{0x37d9, 0x0c},
+	{0x37e0, 0x00},
+	{0x37e1, 0x08},
+	{0x37e2, 0x10},
+	{0x37e3, 0x04},
+	{0x37e4, 0x04},
+	{0x37e5, 0x03},
+	{0x37e6, 0x04},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x00},
+	{0x3804, 0x05},
+	{0x3805, 0x0f},
+	{0x3806, 0x03},
+	{0x3807, 0x2f},
+	{0x3808, 0x05},
+	{0x3809, 0x00},
+	{0x380a, 0x03},
+	{0x380b, 0x20},
+	{0x380c, 0x02},
+	{0x380d, 0xe8},
+	{0x380e, 0x03},
+	{0x380f, 0x80},
+	{0x3810, 0x00},
+	{0x3811, 0x08},
+	{0x3812, 0x00},
+	{0x3813, 0x08},
+	{0x3814, 0x01},
+	{0x3815, 0x01},
+	{0x3816, 0x01},
+	{0x3817, 0x01},
+	{0x3820, 0xa0},
+	{0x3822, 0x13},
+	{0x3832, 0x28},
+	{0x3833, 0x10},
+	{0x3b00, 0x00},
+	{0x3c80, 0x00},
+	{0x3c88, 0x02},
+	{0x3c8c, 0x07},
+	{0x3c8d, 0x40},
+	{0x3cc7, 0x80},
+	{0x4000, 0xc3},
+	{0x4001, 0xe0},
+	{0x4003, 0x40},
+	{0x4008, 0x02},
+	{0x4009, 0x19},
+	{0x400a, 0x01},
+	{0x400b, 0x6c},
+	{0x4011, 0x00},
+	{0x4041, 0x00},
+	{0x4300, 0xff},
+	{0x4301, 0x00},
+	{0x4302, 0x0f},
+	{0x4503, 0x00},
+	{0x4601, 0x50},
+	{0x4800, 0x64},
+	{0x481f, 0x34},
+	{0x4825, 0x33},
+	{0x4837, 0x11},
+	{0x4881, 0x40},
+	{0x4883, 0x01},
+	{0x4890, 0x00},
+	{0x4901, 0x00},
+	{0x4902, 0x00},
+	{0x4b00, 0x2a},
+	{0x4b0d, 0x00},
+	{0x450a, 0x04},
+	{0x450b, 0x00},
+	{0x5000, 0x65},
+	{0x5200, 0x18},
+	{0x5004, 0x00},
+	{0x5080, 0x40},
+	{0x0305, 0xf4},
+	{0x0325, 0xc2},
+};
+
+static const char * const ov01a10_test_pattern_menu[] = {
+	"Disabled",
+	"Color Bar",
+	"Top-Bottom Darker Color Bar",
+	"Right-Left Darker Color Bar",
+	"Color Bar type 4",
+};
+
+static const s64 link_freq_menu_items[] = {
+	OV01A10_LINK_FREQ_400MHZ,
+};
+
+static const struct ov01a10_link_freq_config link_freq_configs[] = {
+	[OV01A10_LINK_FREQ_400MHZ_INDEX] = {
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mipi_data_rate_720mbps),
+			.regs = mipi_data_rate_720mbps,
+		}
+	},
+};
+
+static const struct ov01a10_mode supported_modes[] = {
+	{
+		.width = OV01A10_ACITVE_WIDTH,
+		.height = OV01A10_ACITVE_HEIGHT,
+		.hts = OV01A10_HTS_DEF,
+		.vts_def = OV01A10_VTS_DEF,
+		.vts_min = OV01A10_VTS_MIN,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(sensor_1280x800_setting),
+			.regs = sensor_1280x800_setting,
+		},
+		.link_freq_index = OV01A10_LINK_FREQ_400MHZ_INDEX,
+	},
+};
+
+struct ov01a10 {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct v4l2_ctrl_handler ctrl_handler;
+
+	/* v4l2 controls */
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *pixel_rate;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *exposure;
+
+	const struct ov01a10_mode *cur_mode;
+
+	/* streaming state */
+	bool streaming;
+};
+
+static inline struct ov01a10 *to_ov01a10(struct v4l2_subdev *subdev)
+{
+	return container_of(subdev, struct ov01a10, sd);
+}
+
+static int ov01a10_read_reg(struct ov01a10 *ov01a10, u16 reg, u16 len, u32 *val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov01a10->sd);
+	struct i2c_msg msgs[2];
+	u8 addr_buf[2];
+	u8 data_buf[4] = {0};
+	int ret = 0;
+
+	if (len > sizeof(data_buf))
+		return -EINVAL;
+
+	put_unaligned_be16(reg, addr_buf);
+	msgs[0].addr = client->addr;
+	msgs[0].flags = 0;
+	msgs[0].len = sizeof(addr_buf);
+	msgs[0].buf = addr_buf;
+	msgs[1].addr = client->addr;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].len = len;
+	msgs[1].buf = &data_buf[sizeof(data_buf) - len];
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+
+	if (ret != ARRAY_SIZE(msgs))
+		return ret < 0 ? ret : -EIO;
+
+	*val = get_unaligned_be32(data_buf);
+
+	return 0;
+}
+
+static int ov01a10_write_reg(struct ov01a10 *ov01a10, u16 reg, u16 len, u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov01a10->sd);
+	u8 buf[6];
+	int ret = 0;
+
+	if (len > 4)
+		return -EINVAL;
+
+	put_unaligned_be16(reg, buf);
+	put_unaligned_be32(val << 8 * (4 - len), buf + 2);
+
+	ret = i2c_master_send(client, buf, len + 2);
+	if (ret != len + 2)
+		return ret < 0 ? ret : -EIO;
+
+	return 0;
+}
+
+static int ov01a10_write_reg_list(struct ov01a10 *ov01a10,
+				  const struct ov01a10_reg_list *r_list)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov01a10->sd);
+	unsigned int i;
+	int ret = 0;
+
+	for (i = 0; i < r_list->num_of_regs; i++) {
+		ret = ov01a10_write_reg(ov01a10, r_list->regs[i].address, 1,
+					r_list->regs[i].val);
+		if (ret) {
+			dev_err_ratelimited(&client->dev,
+					    "write reg 0x%4.4x err = %d\n",
+					    r_list->regs[i].address, ret);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int ov01a10_update_digital_gain(struct ov01a10 *ov01a10, u32 d_gain)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov01a10->sd);
+	u32 real = d_gain << 6;
+	int ret = 0;
+
+	ret = ov01a10_write_reg(ov01a10, OV01A10_REG_DIGITAL_GAIN_B, 3, real);
+	if (ret) {
+		dev_err(&client->dev, "failed to set DIGITAL_GAIN_B\n");
+		return ret;
+	}
+
+	ret = ov01a10_write_reg(ov01a10, OV01A10_REG_DIGITAL_GAIN_GB, 3, real);
+	if (ret) {
+		dev_err(&client->dev, "failed to set DIGITAL_GAIN_GB\n");
+		return ret;
+	}
+
+	ret = ov01a10_write_reg(ov01a10, OV01A10_REG_DIGITAL_GAIN_GR, 3, real);
+	if (ret) {
+		dev_err(&client->dev, "failed to set DIGITAL_GAIN_GR\n");
+		return ret;
+	}
+
+	ret = ov01a10_write_reg(ov01a10, OV01A10_REG_DIGITAL_GAIN_R, 3, real);
+	if (ret)
+		dev_err(&client->dev, "failed to set DIGITAL_GAIN_R\n");
+
+	return ret;
+}
+
+static int ov01a10_test_pattern(struct ov01a10 *ov01a10, u32 pattern)
+{
+	if (!pattern)
+		return 0;
+
+	pattern = (pattern - 1) | OV01A10_TEST_PATTERN_ENABLE;
+
+	return ov01a10_write_reg(ov01a10, OV01A10_REG_TEST_PATTERN, 1, pattern);
+}
+
+/* for vflip and hflip, use 0x9 as window offset to keep the bayer */
+static int ov01a10_set_hflip(struct ov01a10 *ov01a10, u32 hflip)
+{
+	int ret;
+	u32 val, offset;
+
+	offset = hflip ? 0x9 : 0x8;
+	ret = ov01a10_write_reg(ov01a10, OV01A10_REG_X_WIN, 1, offset);
+	if (ret)
+		return ret;
+
+	ret = ov01a10_read_reg(ov01a10, OV01A10_REG_FORMAT1, 1, &val);
+	if (ret)
+		return ret;
+
+	val = hflip ? val | FIELD_PREP(OV01A10_HFLIP_MASK, 0x1) :
+		val & ~OV01A10_HFLIP_MASK;
+
+	return ov01a10_write_reg(ov01a10, OV01A10_REG_FORMAT1, 1, val);
+}
+
+static int ov01a10_set_vflip(struct ov01a10 *ov01a10, u32 vflip)
+{
+	int ret;
+	u32 val, offset;
+
+	offset = vflip ? 0x9 : 0x8;
+	ret = ov01a10_write_reg(ov01a10, OV01A10_REG_Y_WIN, 1, offset);
+	if (ret)
+		return ret;
+
+	ret = ov01a10_read_reg(ov01a10, OV01A10_REG_FORMAT1, 1, &val);
+	if (ret)
+		return ret;
+
+	val = vflip ? val | FIELD_PREP(OV01A10_VFLIP_MASK, 0x1) :
+		val & ~OV01A10_VFLIP_MASK;
+
+	return ov01a10_write_reg(ov01a10, OV01A10_REG_FORMAT1, 1, val);
+}
+
+static int ov01a10_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov01a10 *ov01a10 = container_of(ctrl->handler,
+					       struct ov01a10, ctrl_handler);
+	struct i2c_client *client = v4l2_get_subdevdata(&ov01a10->sd);
+	s64 exposure_max;
+	int ret = 0;
+
+	if (ctrl->id == V4L2_CID_VBLANK) {
+		exposure_max = ov01a10->cur_mode->height + ctrl->val -
+			OV01A10_EXPOSURE_MAX_MARGIN;
+		__v4l2_ctrl_modify_range(ov01a10->exposure,
+					 ov01a10->exposure->minimum,
+					 exposure_max, ov01a10->exposure->step,
+					 exposure_max);
+	}
+
+	if (!pm_runtime_get_if_in_use(&client->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_ANALOGUE_GAIN:
+		ret = ov01a10_write_reg(ov01a10, OV01A10_REG_ANALOG_GAIN, 2,
+					ctrl->val);
+		break;
+
+	case V4L2_CID_DIGITAL_GAIN:
+		ret = ov01a10_update_digital_gain(ov01a10, ctrl->val);
+		break;
+
+	case V4L2_CID_EXPOSURE:
+		ret = ov01a10_write_reg(ov01a10, OV01A10_REG_EXPOSURE, 2,
+					ctrl->val);
+		break;
+
+	case V4L2_CID_VBLANK:
+		ret = ov01a10_write_reg(ov01a10, OV01A10_REG_VTS, 2,
+					ov01a10->cur_mode->height + ctrl->val);
+		break;
+
+	case V4L2_CID_TEST_PATTERN:
+		ret = ov01a10_test_pattern(ov01a10, ctrl->val);
+		break;
+
+	case V4L2_CID_HFLIP:
+		ov01a10_set_hflip(ov01a10, ctrl->val);
+		break;
+
+	case V4L2_CID_VFLIP:
+		ov01a10_set_vflip(ov01a10, ctrl->val);
+		break;
+
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	pm_runtime_put(&client->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ov01a10_ctrl_ops = {
+	.s_ctrl = ov01a10_set_ctrl,
+};
+
+static int ov01a10_init_controls(struct ov01a10 *ov01a10)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov01a10->sd);
+	struct v4l2_fwnode_device_properties props;
+	u32 vblank_min, vblank_max, vblank_default;
+	struct v4l2_ctrl_handler *ctrl_hdlr;
+	const struct ov01a10_mode *cur_mode;
+	s64 exposure_max, h_blank;
+	int ret = 0;
+	int size;
+
+	ret = v4l2_fwnode_device_parse(&client->dev, &props);
+	if (ret)
+		return ret;
+
+	ctrl_hdlr = &ov01a10->ctrl_handler;
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 12);
+	if (ret)
+		return ret;
+
+	cur_mode = ov01a10->cur_mode;
+	size = ARRAY_SIZE(link_freq_menu_items);
+
+	ov01a10->link_freq = v4l2_ctrl_new_int_menu(ctrl_hdlr,
+						    &ov01a10_ctrl_ops,
+						    V4L2_CID_LINK_FREQ,
+						    size - 1, 0,
+						    link_freq_menu_items);
+	if (ov01a10->link_freq)
+		ov01a10->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	ov01a10->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, &ov01a10_ctrl_ops,
+						V4L2_CID_PIXEL_RATE, 0,
+						OV01A10_SCLK, 1, OV01A10_SCLK);
+
+	vblank_min = cur_mode->vts_min - cur_mode->height;
+	vblank_max = OV01A10_VTS_MAX - cur_mode->height;
+	vblank_default = cur_mode->vts_def - cur_mode->height;
+	ov01a10->vblank = v4l2_ctrl_new_std(ctrl_hdlr, &ov01a10_ctrl_ops,
+					    V4L2_CID_VBLANK, vblank_min,
+					    vblank_max, 1, vblank_default);
+
+	h_blank = cur_mode->hts - cur_mode->width;
+	ov01a10->hblank = v4l2_ctrl_new_std(ctrl_hdlr, &ov01a10_ctrl_ops,
+					    V4L2_CID_HBLANK, h_blank, h_blank,
+					    1, h_blank);
+	if (ov01a10->hblank)
+		ov01a10->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov01a10_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
+			  OV01A10_ANAL_GAIN_MIN, OV01A10_ANAL_GAIN_MAX,
+			  OV01A10_ANAL_GAIN_STEP, OV01A10_ANAL_GAIN_MIN);
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov01a10_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
+			  OV01A10_DGTL_GAIN_MIN, OV01A10_DGTL_GAIN_MAX,
+			  OV01A10_DGTL_GAIN_STEP, OV01A10_DGTL_GAIN_DEFAULT);
+
+	exposure_max = cur_mode->vts_def - OV01A10_EXPOSURE_MAX_MARGIN;
+	ov01a10->exposure = v4l2_ctrl_new_std(ctrl_hdlr, &ov01a10_ctrl_ops,
+					      V4L2_CID_EXPOSURE,
+					      OV01A10_EXPOSURE_MIN,
+					      exposure_max,
+					      OV01A10_EXPOSURE_STEP,
+					      exposure_max);
+
+	v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &ov01a10_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(ov01a10_test_pattern_menu) - 1,
+				     0, 0, ov01a10_test_pattern_menu);
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov01a10_ctrl_ops, V4L2_CID_HFLIP,
+			  0, 1, 1, 0);
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov01a10_ctrl_ops, V4L2_CID_VFLIP,
+			  0, 1, 1, 0);
+
+	ret = v4l2_ctrl_new_fwnode_properties(ctrl_hdlr, &ov01a10_ctrl_ops,
+					      &props);
+	if (ret)
+		goto fail;
+
+	if (ctrl_hdlr->error) {
+		ret = ctrl_hdlr->error;
+		goto fail;
+	}
+
+	ov01a10->sd.ctrl_handler = ctrl_hdlr;
+
+	return 0;
+fail:
+	v4l2_ctrl_handler_free(ctrl_hdlr);
+
+	return ret;
+}
+
+static void ov01a10_update_pad_format(const struct ov01a10_mode *mode,
+				      struct v4l2_mbus_framefmt *fmt)
+{
+	fmt->width = mode->width;
+	fmt->height = mode->height;
+	fmt->code = MEDIA_BUS_FMT_SBGGR10_1X10;
+	fmt->field = V4L2_FIELD_NONE;
+	fmt->colorspace = V4L2_COLORSPACE_RAW;
+}
+
+static int ov01a10_start_streaming(struct ov01a10 *ov01a10)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov01a10->sd);
+	const struct ov01a10_reg_list *reg_list;
+	int link_freq_index;
+	int ret = 0;
+
+	link_freq_index = ov01a10->cur_mode->link_freq_index;
+	reg_list = &link_freq_configs[link_freq_index].reg_list;
+	ret = ov01a10_write_reg_list(ov01a10, reg_list);
+	if (ret) {
+		dev_err(&client->dev, "failed to set plls\n");
+		return ret;
+	}
+
+	reg_list = &ov01a10->cur_mode->reg_list;
+	ret = ov01a10_write_reg_list(ov01a10, reg_list);
+	if (ret) {
+		dev_err(&client->dev, "failed to set mode\n");
+		return ret;
+	}
+
+	ret = __v4l2_ctrl_handler_setup(ov01a10->sd.ctrl_handler);
+	if (ret)
+		return ret;
+
+	ret = ov01a10_write_reg(ov01a10, OV01A10_REG_MODE_SELECT, 1,
+				OV01A10_MODE_STREAMING);
+	if (ret)
+		dev_err(&client->dev, "failed to start streaming\n");
+
+	return ret;
+}
+
+static void ov01a10_stop_streaming(struct ov01a10 *ov01a10)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov01a10->sd);
+	int ret = 0;
+
+	ret = ov01a10_write_reg(ov01a10, OV01A10_REG_MODE_SELECT, 1,
+				OV01A10_MODE_STANDBY);
+	if (ret)
+		dev_err(&client->dev, "failed to stop streaming\n");
+}
+
+static int ov01a10_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct ov01a10 *ov01a10 = to_ov01a10(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct v4l2_subdev_state *state;
+	int ret = 0;
+
+	state = v4l2_subdev_lock_and_get_active_state(sd);
+	if (ov01a10->streaming == enable)
+		goto unlock;
+
+	if (enable) {
+		ret = pm_runtime_resume_and_get(&client->dev);
+		if (ret < 0)
+			goto unlock;
+
+		ret = ov01a10_start_streaming(ov01a10);
+		if (ret) {
+			pm_runtime_put(&client->dev);
+			goto unlock;
+		}
+
+		goto done;
+	}
+
+	ov01a10_stop_streaming(ov01a10);
+	pm_runtime_put(&client->dev);
+done:
+	ov01a10->streaming = enable;
+unlock:
+	v4l2_subdev_unlock_state(state);
+
+	return ret;
+}
+
+static int __maybe_unused ov01a10_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov01a10 *ov01a10 = to_ov01a10(sd);
+	struct v4l2_subdev_state *state;
+
+	state = v4l2_subdev_lock_and_get_active_state(sd);
+	if (ov01a10->streaming)
+		ov01a10_stop_streaming(ov01a10);
+
+	v4l2_subdev_unlock_state(state);
+
+	return 0;
+}
+
+static int __maybe_unused ov01a10_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov01a10 *ov01a10 = to_ov01a10(sd);
+	struct v4l2_subdev_state *state;
+	int ret = 0;
+
+	state = v4l2_subdev_lock_and_get_active_state(sd);
+	if (!ov01a10->streaming)
+		goto exit;
+
+	ret = ov01a10_start_streaming(ov01a10);
+	if (ret) {
+		ov01a10->streaming = false;
+		ov01a10_stop_streaming(ov01a10);
+	}
+
+exit:
+	v4l2_subdev_unlock_state(state);
+
+	return ret;
+}
+
+static int ov01a10_set_format(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_format *fmt)
+{
+	struct ov01a10 *ov01a10 = to_ov01a10(sd);
+	const struct ov01a10_mode *mode;
+	struct v4l2_mbus_framefmt *format;
+	s32 vblank_def, h_blank;
+
+	mode = v4l2_find_nearest_size(supported_modes,
+				      ARRAY_SIZE(supported_modes), width,
+				      height, fmt->format.width,
+				      fmt->format.height);
+
+	ov01a10_update_pad_format(mode, &fmt->format);
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		ov01a10->cur_mode = mode;
+		__v4l2_ctrl_s_ctrl(ov01a10->link_freq, mode->link_freq_index);
+		__v4l2_ctrl_s_ctrl_int64(ov01a10->pixel_rate, OV01A10_SCLK);
+
+		vblank_def = mode->vts_def - mode->height;
+		__v4l2_ctrl_modify_range(ov01a10->vblank,
+					 mode->vts_min - mode->height,
+					 OV01A10_VTS_MAX - mode->height, 1,
+					 vblank_def);
+		__v4l2_ctrl_s_ctrl(ov01a10->vblank, vblank_def);
+		h_blank = mode->hts - mode->width;
+		__v4l2_ctrl_modify_range(ov01a10->hblank, h_blank, h_blank, 1,
+					 h_blank);
+	}
+
+	format = v4l2_subdev_get_pad_format(sd, sd_state, fmt->stream);
+	*format = fmt->format;
+
+	return 0;
+}
+
+static int ov01a10_init_cfg(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_state *state)
+{
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+		.format = {
+			.width = OV01A10_ACITVE_WIDTH,
+			.height = OV01A10_ACITVE_HEIGHT,
+		},
+	};
+
+	ov01a10_set_format(sd, state, &fmt);
+
+	return 0;
+}
+
+static int ov01a10_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index > 0)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_SBGGR10_1X10;
+
+	return 0;
+}
+
+static int ov01a10_enum_frame_size(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->index >= ARRAY_SIZE(supported_modes) ||
+	    fse->code != MEDIA_BUS_FMT_SBGGR10_1X10)
+		return -EINVAL;
+
+	fse->min_width = supported_modes[fse->index].width;
+	fse->max_width = fse->min_width;
+	fse->min_height = supported_modes[fse->index].height;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static int ov01a10_get_selection(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *state,
+				 struct v4l2_subdev_selection *sel)
+{
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
+		return -EINVAL;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_NATIVE_SIZE:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.top = 0;
+		sel->r.left = 0;
+		sel->r.width = OV01A10_PIXEL_ARRAY_WIDTH;
+		sel->r.height = OV01A10_PIXEL_ARRAY_HEIGHT;
+		return 0;
+	case V4L2_SEL_TGT_CROP:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		sel->r.top = (OV01A10_PIXEL_ARRAY_HEIGHT -
+			      OV01A10_ACITVE_HEIGHT) / 2;
+		sel->r.left = (OV01A10_PIXEL_ARRAY_WIDTH -
+			       OV01A10_ACITVE_WIDTH) / 2;
+		sel->r.width = OV01A10_ACITVE_WIDTH;
+		sel->r.height = OV01A10_ACITVE_HEIGHT;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static const struct v4l2_subdev_core_ops ov01a10_core_ops = {
+	.log_status = v4l2_ctrl_subdev_log_status,
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_video_ops ov01a10_video_ops = {
+	.s_stream = ov01a10_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops ov01a10_pad_ops = {
+	.init_cfg = ov01a10_init_cfg,
+	.set_fmt = ov01a10_set_format,
+	.get_fmt = v4l2_subdev_get_fmt,
+	.get_selection = ov01a10_get_selection,
+	.enum_mbus_code = ov01a10_enum_mbus_code,
+	.enum_frame_size = ov01a10_enum_frame_size,
+};
+
+static const struct v4l2_subdev_ops ov01a10_subdev_ops = {
+	.core = &ov01a10_core_ops,
+	.video = &ov01a10_video_ops,
+	.pad = &ov01a10_pad_ops,
+};
+
+static const struct media_entity_operations ov01a10_subdev_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static int ov01a10_identify_module(struct ov01a10 *ov01a10)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov01a10->sd);
+	int ret;
+	u32 val;
+
+	ret = ov01a10_read_reg(ov01a10, OV01A10_REG_CHIP_ID, 3, &val);
+	if (ret)
+		return ret;
+
+	if (val != OV01A10_CHIP_ID) {
+		dev_err(&client->dev, "chip id mismatch: %x!=%x\n",
+			OV01A10_CHIP_ID, val);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static void ov01a10_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+
+	pm_runtime_disable(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+}
+
+static int ov01a10_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct ov01a10 *ov01a10;
+	int ret = 0;
+
+	ov01a10 = devm_kzalloc(dev, sizeof(*ov01a10), GFP_KERNEL);
+	if (!ov01a10)
+		return -ENOMEM;
+
+	v4l2_i2c_subdev_init(&ov01a10->sd, client, &ov01a10_subdev_ops);
+
+	ret = ov01a10_identify_module(ov01a10);
+	if (ret)
+		return dev_err_probe(dev, ret,
+				     "failed to find sensor\n");
+
+	ov01a10->cur_mode = &supported_modes[0];
+
+	ret = ov01a10_init_controls(ov01a10);
+	if (ret) {
+		dev_err(dev, "failed to init controls: %d\n", ret);
+		return ret;
+	}
+
+	ov01a10->sd.state_lock = ov01a10->ctrl_handler.lock;
+	ov01a10->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+		V4L2_SUBDEV_FL_HAS_EVENTS;
+	ov01a10->sd.entity.ops = &ov01a10_subdev_entity_ops;
+	ov01a10->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ov01a10->pad.flags = MEDIA_PAD_FL_SOURCE;
+
+	ret = media_entity_pads_init(&ov01a10->sd.entity, 1, &ov01a10->pad);
+	if (ret) {
+		dev_err(dev, "Failed to init entity pads: %d\n", ret);
+		goto err_handler_free;
+	}
+
+	ret = v4l2_subdev_init_finalize(&ov01a10->sd);
+	if (ret) {
+		dev_err(dev, "Failed to allocate subdev state: %d\n", ret);
+		goto err_media_entity_cleanup;
+	}
+
+	/*
+	 * Device is already turned on by i2c-core with ACPI domain PM.
+	 * Enable runtime PM and turn off the device.
+	 */
+	pm_runtime_set_active(&client->dev);
+	pm_runtime_enable(dev);
+	pm_runtime_idle(dev);
+
+	ret = v4l2_async_register_subdev_sensor(&ov01a10->sd);
+	if (ret < 0) {
+		dev_err(dev, "Failed to register subdev: %d\n", ret);
+		goto err_pm_disable;
+	}
+
+	return 0;
+
+err_pm_disable:
+	pm_runtime_disable(dev);
+	pm_runtime_set_suspended(&client->dev);
+
+err_media_entity_cleanup:
+	media_entity_cleanup(&ov01a10->sd.entity);
+
+err_handler_free:
+	v4l2_ctrl_handler_free(ov01a10->sd.ctrl_handler);
+
+	return ret;
+}
+
+static const struct dev_pm_ops ov01a10_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(ov01a10_suspend, ov01a10_resume)
+};
+
+#ifdef CONFIG_ACPI
+static const struct acpi_device_id ov01a10_acpi_ids[] = {
+	{ "OVTI01A0" },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(acpi, ov01a10_acpi_ids);
+#endif
+
+static struct i2c_driver ov01a10_i2c_driver = {
+	.driver = {
+		.name = "ov01a10",
+		.pm = &ov01a10_pm_ops,
+		.acpi_match_table = ACPI_PTR(ov01a10_acpi_ids),
+	},
+	.probe = ov01a10_probe,
+	.remove = ov01a10_remove,
+};
+
+module_i2c_driver(ov01a10_i2c_driver);
+
+MODULE_AUTHOR("Bingbu Cao <bingbu.cao@intel.com>");
+MODULE_AUTHOR("Wang Yating <yating.wang@intel.com>");
+MODULE_DESCRIPTION("OmniVision OV01A10 sensor driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/ov02a10.c b/drivers/media/i2c/ov02a10.c
new file mode 100644
index 000000000..741d977a7
--- /dev/null
+++ b/drivers/media/i2c/ov02a10.c
@@ -0,0 +1,1012 @@
+// SPDX-License-Identifier: GPL-2.0
+// Copyright (c) 2020 MediaTek Inc.
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/device.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 <linux/units.h>
+#include <media/media-entity.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+#define OV02A10_ID					0x2509
+#define OV02A10_ID_MASK					GENMASK(15, 0)
+
+#define OV02A10_REG_CHIP_ID				0x02
+
+/* Bit[1] vertical upside down */
+/* Bit[0] horizontal mirror */
+#define REG_MIRROR_FLIP_CONTROL				0x3f
+
+/* Orientation */
+#define REG_MIRROR_FLIP_ENABLE				0x03
+
+/* Bit[2:0] MIPI transmission speed select */
+#define TX_SPEED_AREA_SEL				0xa1
+#define OV02A10_MIPI_TX_SPEED_DEFAULT			0x04
+
+#define REG_PAGE_SWITCH					0xfd
+#define REG_GLOBAL_EFFECTIVE				0x01
+#define REG_ENABLE					BIT(0)
+
+#define REG_SC_CTRL_MODE				0xac
+#define SC_CTRL_MODE_STANDBY				0x00
+#define SC_CTRL_MODE_STREAMING				0x01
+
+/* Exposure control */
+#define OV02A10_EXP_SHIFT				8
+#define OV02A10_REG_EXPOSURE_H				0x03
+#define OV02A10_REG_EXPOSURE_L				0x04
+#define	OV02A10_EXPOSURE_MIN				4
+#define OV02A10_EXPOSURE_MAX_MARGIN			4
+#define	OV02A10_EXPOSURE_STEP				1
+
+/* Vblanking control */
+#define OV02A10_VTS_SHIFT				8
+#define OV02A10_REG_VTS_H				0x05
+#define OV02A10_REG_VTS_L				0x06
+#define OV02A10_VTS_MAX					0x209f
+#define OV02A10_BASE_LINES				1224
+
+/* Analog gain control */
+#define OV02A10_REG_GAIN				0x24
+#define OV02A10_GAIN_MIN				0x10
+#define OV02A10_GAIN_MAX				0xf8
+#define OV02A10_GAIN_STEP				0x01
+#define OV02A10_GAIN_DEFAULT				0x40
+
+/* Test pattern control */
+#define OV02A10_REG_TEST_PATTERN			0xb6
+
+#define OV02A10_LINK_FREQ_390MHZ			(390 * HZ_PER_MHZ)
+#define OV02A10_ECLK_FREQ				(24 * HZ_PER_MHZ)
+
+/* Number of lanes supported by this driver */
+#define OV02A10_DATA_LANES				1
+
+/* Bits per sample of sensor output */
+#define OV02A10_BITS_PER_SAMPLE				10
+
+static const char * const ov02a10_supply_names[] = {
+	"dovdd",	/* Digital I/O power */
+	"avdd",		/* Analog power */
+	"dvdd",		/* Digital core power */
+};
+
+struct ov02a10_reg {
+	u8 addr;
+	u8 val;
+};
+
+struct ov02a10_reg_list {
+	u32 num_of_regs;
+	const struct ov02a10_reg *regs;
+};
+
+struct ov02a10_mode {
+	u32 width;
+	u32 height;
+	u32 exp_def;
+	u32 hts_def;
+	u32 vts_def;
+	const struct ov02a10_reg_list reg_list;
+};
+
+struct ov02a10 {
+	u32 eclk_freq;
+	/* Indication of MIPI transmission speed select */
+	u32 mipi_clock_voltage;
+
+	struct clk *eclk;
+	struct gpio_desc *pd_gpio;
+	struct gpio_desc *rst_gpio;
+	struct regulator_bulk_data supplies[ARRAY_SIZE(ov02a10_supply_names)];
+
+	bool streaming;
+	bool upside_down;
+
+	/*
+	 * Serialize control access, get/set format, get selection
+	 * and start streaming.
+	 */
+	struct mutex mutex;
+	struct v4l2_subdev subdev;
+	struct media_pad pad;
+	struct v4l2_mbus_framefmt fmt;
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_ctrl *exposure;
+
+	const struct ov02a10_mode *cur_mode;
+};
+
+static inline struct ov02a10 *to_ov02a10(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct ov02a10, subdev);
+}
+
+/*
+ * eclk 24Mhz
+ * pclk 39Mhz
+ * linelength 934(0x3a6)
+ * framelength 1390(0x56E)
+ * grabwindow_width 1600
+ * grabwindow_height 1200
+ * max_framerate 30fps
+ * mipi_datarate per lane 780Mbps
+ */
+static const struct ov02a10_reg ov02a10_1600x1200_regs[] = {
+	{0xfd, 0x01},
+	{0xac, 0x00},
+	{0xfd, 0x00},
+	{0x2f, 0x29},
+	{0x34, 0x00},
+	{0x35, 0x21},
+	{0x30, 0x15},
+	{0x33, 0x01},
+	{0xfd, 0x01},
+	{0x44, 0x00},
+	{0x2a, 0x4c},
+	{0x2b, 0x1e},
+	{0x2c, 0x60},
+	{0x25, 0x11},
+	{0x03, 0x01},
+	{0x04, 0xae},
+	{0x09, 0x00},
+	{0x0a, 0x02},
+	{0x06, 0xa6},
+	{0x31, 0x00},
+	{0x24, 0x40},
+	{0x01, 0x01},
+	{0xfb, 0x73},
+	{0xfd, 0x01},
+	{0x16, 0x04},
+	{0x1c, 0x09},
+	{0x21, 0x42},
+	{0x12, 0x04},
+	{0x13, 0x10},
+	{0x11, 0x40},
+	{0x33, 0x81},
+	{0xd0, 0x00},
+	{0xd1, 0x01},
+	{0xd2, 0x00},
+	{0x50, 0x10},
+	{0x51, 0x23},
+	{0x52, 0x20},
+	{0x53, 0x10},
+	{0x54, 0x02},
+	{0x55, 0x20},
+	{0x56, 0x02},
+	{0x58, 0x48},
+	{0x5d, 0x15},
+	{0x5e, 0x05},
+	{0x66, 0x66},
+	{0x68, 0x68},
+	{0x6b, 0x00},
+	{0x6c, 0x00},
+	{0x6f, 0x40},
+	{0x70, 0x40},
+	{0x71, 0x0a},
+	{0x72, 0xf0},
+	{0x73, 0x10},
+	{0x75, 0x80},
+	{0x76, 0x10},
+	{0x84, 0x00},
+	{0x85, 0x10},
+	{0x86, 0x10},
+	{0x87, 0x00},
+	{0x8a, 0x22},
+	{0x8b, 0x22},
+	{0x19, 0xf1},
+	{0x29, 0x01},
+	{0xfd, 0x01},
+	{0x9d, 0x16},
+	{0xa0, 0x29},
+	{0xa1, 0x04},
+	{0xad, 0x62},
+	{0xae, 0x00},
+	{0xaf, 0x85},
+	{0xb1, 0x01},
+	{0x8e, 0x06},
+	{0x8f, 0x40},
+	{0x90, 0x04},
+	{0x91, 0xb0},
+	{0x45, 0x01},
+	{0x46, 0x00},
+	{0x47, 0x6c},
+	{0x48, 0x03},
+	{0x49, 0x8b},
+	{0x4a, 0x00},
+	{0x4b, 0x07},
+	{0x4c, 0x04},
+	{0x4d, 0xb7},
+	{0xf0, 0x40},
+	{0xf1, 0x40},
+	{0xf2, 0x40},
+	{0xf3, 0x40},
+	{0x3f, 0x00},
+	{0xfd, 0x01},
+	{0x05, 0x00},
+	{0x06, 0xa6},
+	{0xfd, 0x01},
+};
+
+static const char * const ov02a10_test_pattern_menu[] = {
+	"Disabled",
+	"Eight Vertical Colour Bars",
+};
+
+static const s64 link_freq_menu_items[] = {
+	OV02A10_LINK_FREQ_390MHZ,
+};
+
+static u64 to_pixel_rate(u32 f_index)
+{
+	u64 pixel_rate = link_freq_menu_items[f_index] * 2 * OV02A10_DATA_LANES;
+
+	do_div(pixel_rate, OV02A10_BITS_PER_SAMPLE);
+
+	return pixel_rate;
+}
+
+static const struct ov02a10_mode supported_modes[] = {
+	{
+		.width = 1600,
+		.height = 1200,
+		.exp_def = 0x01ae,
+		.hts_def = 0x03a6,
+		.vts_def = 0x056e,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(ov02a10_1600x1200_regs),
+			.regs = ov02a10_1600x1200_regs,
+		},
+	},
+};
+
+static int ov02a10_write_array(struct ov02a10 *ov02a10,
+			       const struct ov02a10_reg_list *r_list)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov02a10->subdev);
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < r_list->num_of_regs; i++) {
+		ret = i2c_smbus_write_byte_data(client, r_list->regs[i].addr,
+						r_list->regs[i].val);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+static void ov02a10_fill_fmt(const struct ov02a10_mode *mode,
+			     struct v4l2_mbus_framefmt *fmt)
+{
+	fmt->width = mode->width;
+	fmt->height = mode->height;
+	fmt->field = V4L2_FIELD_NONE;
+}
+
+static int ov02a10_set_fmt(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_state *sd_state,
+			   struct v4l2_subdev_format *fmt)
+{
+	struct ov02a10 *ov02a10 = to_ov02a10(sd);
+	struct v4l2_mbus_framefmt *mbus_fmt = &fmt->format;
+	struct v4l2_mbus_framefmt *frame_fmt;
+	int ret = 0;
+
+	mutex_lock(&ov02a10->mutex);
+
+	if (ov02a10->streaming && fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		ret = -EBUSY;
+		goto out_unlock;
+	}
+
+	/* Only one sensor mode supported */
+	mbus_fmt->code = ov02a10->fmt.code;
+	ov02a10_fill_fmt(ov02a10->cur_mode, mbus_fmt);
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
+		frame_fmt = v4l2_subdev_get_try_format(sd, sd_state, 0);
+	else
+		frame_fmt = &ov02a10->fmt;
+
+	*frame_fmt = *mbus_fmt;
+
+out_unlock:
+	mutex_unlock(&ov02a10->mutex);
+	return ret;
+}
+
+static int ov02a10_get_fmt(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_state *sd_state,
+			   struct v4l2_subdev_format *fmt)
+{
+	struct ov02a10 *ov02a10 = to_ov02a10(sd);
+	struct v4l2_mbus_framefmt *mbus_fmt = &fmt->format;
+
+	mutex_lock(&ov02a10->mutex);
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		fmt->format = *v4l2_subdev_get_try_format(sd, sd_state,
+							  fmt->pad);
+	} else {
+		fmt->format = ov02a10->fmt;
+		mbus_fmt->code = ov02a10->fmt.code;
+		ov02a10_fill_fmt(ov02a10->cur_mode, mbus_fmt);
+	}
+
+	mutex_unlock(&ov02a10->mutex);
+
+	return 0;
+}
+
+static int ov02a10_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct ov02a10 *ov02a10 = to_ov02a10(sd);
+
+	if (code->index != 0)
+		return -EINVAL;
+
+	code->code = ov02a10->fmt.code;
+
+	return 0;
+}
+
+static int ov02a10_enum_frame_sizes(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_state *sd_state,
+				    struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->index >= ARRAY_SIZE(supported_modes))
+		return -EINVAL;
+
+	fse->min_width  = supported_modes[fse->index].width;
+	fse->max_width  = supported_modes[fse->index].width;
+	fse->max_height = supported_modes[fse->index].height;
+	fse->min_height = supported_modes[fse->index].height;
+
+	return 0;
+}
+
+static int ov02a10_check_sensor_id(struct ov02a10 *ov02a10)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov02a10->subdev);
+	u16 chip_id;
+	int ret;
+
+	/* Validate the chip ID */
+	ret = i2c_smbus_read_word_swapped(client, OV02A10_REG_CHIP_ID);
+	if (ret < 0)
+		return ret;
+
+	chip_id = le16_to_cpu((__force __le16)ret);
+
+	if ((chip_id & OV02A10_ID_MASK) != OV02A10_ID) {
+		dev_err(&client->dev, "unexpected sensor id(0x%04x)\n", chip_id);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int ov02a10_power_on(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov02a10 *ov02a10 = to_ov02a10(sd);
+	int ret;
+
+	gpiod_set_value_cansleep(ov02a10->rst_gpio, 1);
+	gpiod_set_value_cansleep(ov02a10->pd_gpio, 1);
+
+	ret = clk_prepare_enable(ov02a10->eclk);
+	if (ret < 0) {
+		dev_err(dev, "failed to enable eclk\n");
+		return ret;
+	}
+
+	ret = regulator_bulk_enable(ARRAY_SIZE(ov02a10_supply_names),
+				    ov02a10->supplies);
+	if (ret < 0) {
+		dev_err(dev, "failed to enable regulators\n");
+		goto disable_clk;
+	}
+	usleep_range(5000, 6000);
+
+	gpiod_set_value_cansleep(ov02a10->pd_gpio, 0);
+	usleep_range(5000, 6000);
+
+	gpiod_set_value_cansleep(ov02a10->rst_gpio, 0);
+	usleep_range(5000, 6000);
+
+	ret = ov02a10_check_sensor_id(ov02a10);
+	if (ret)
+		goto disable_regulator;
+
+	return 0;
+
+disable_regulator:
+	regulator_bulk_disable(ARRAY_SIZE(ov02a10_supply_names),
+			       ov02a10->supplies);
+disable_clk:
+	clk_disable_unprepare(ov02a10->eclk);
+
+	return ret;
+}
+
+static int ov02a10_power_off(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov02a10 *ov02a10 = to_ov02a10(sd);
+
+	gpiod_set_value_cansleep(ov02a10->rst_gpio, 1);
+	clk_disable_unprepare(ov02a10->eclk);
+	gpiod_set_value_cansleep(ov02a10->pd_gpio, 1);
+	regulator_bulk_disable(ARRAY_SIZE(ov02a10_supply_names),
+			       ov02a10->supplies);
+
+	return 0;
+}
+
+static int __ov02a10_start_stream(struct ov02a10 *ov02a10)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov02a10->subdev);
+	const struct ov02a10_reg_list *reg_list;
+	int ret;
+
+	/* Apply default values of current mode */
+	reg_list = &ov02a10->cur_mode->reg_list;
+	ret = ov02a10_write_array(ov02a10, reg_list);
+	if (ret)
+		return ret;
+
+	/* Apply customized values from user */
+	ret = __v4l2_ctrl_handler_setup(ov02a10->subdev.ctrl_handler);
+	if (ret)
+		return ret;
+
+	/* Set orientation to 180 degree */
+	if (ov02a10->upside_down) {
+		ret = i2c_smbus_write_byte_data(client, REG_MIRROR_FLIP_CONTROL,
+						REG_MIRROR_FLIP_ENABLE);
+		if (ret < 0) {
+			dev_err(&client->dev, "failed to set orientation\n");
+			return ret;
+		}
+		ret = i2c_smbus_write_byte_data(client, REG_GLOBAL_EFFECTIVE,
+						REG_ENABLE);
+		if (ret < 0)
+			return ret;
+	}
+
+	/* Set MIPI TX speed according to DT property */
+	if (ov02a10->mipi_clock_voltage != OV02A10_MIPI_TX_SPEED_DEFAULT) {
+		ret = i2c_smbus_write_byte_data(client, TX_SPEED_AREA_SEL,
+						ov02a10->mipi_clock_voltage);
+		if (ret < 0)
+			return ret;
+	}
+
+	/* Set stream on register */
+	return i2c_smbus_write_byte_data(client, REG_SC_CTRL_MODE,
+					 SC_CTRL_MODE_STREAMING);
+}
+
+static int __ov02a10_stop_stream(struct ov02a10 *ov02a10)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov02a10->subdev);
+
+	return i2c_smbus_write_byte_data(client, REG_SC_CTRL_MODE,
+					 SC_CTRL_MODE_STANDBY);
+}
+
+static int ov02a10_entity_init_cfg(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *sd_state)
+{
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+		.format = {
+			.width = 1600,
+			.height = 1200,
+		}
+	};
+
+	ov02a10_set_fmt(sd, sd_state, &fmt);
+
+	return 0;
+}
+
+static int ov02a10_s_stream(struct v4l2_subdev *sd, int on)
+{
+	struct ov02a10 *ov02a10 = to_ov02a10(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(&ov02a10->subdev);
+	int ret;
+
+	mutex_lock(&ov02a10->mutex);
+
+	if (ov02a10->streaming == on) {
+		ret = 0;
+		goto unlock_and_return;
+	}
+
+	if (on) {
+		ret = pm_runtime_resume_and_get(&client->dev);
+		if (ret < 0)
+			goto unlock_and_return;
+
+		ret = __ov02a10_start_stream(ov02a10);
+		if (ret) {
+			__ov02a10_stop_stream(ov02a10);
+			ov02a10->streaming = !on;
+			goto err_rpm_put;
+		}
+	} else {
+		__ov02a10_stop_stream(ov02a10);
+		pm_runtime_put(&client->dev);
+	}
+
+	ov02a10->streaming = on;
+	mutex_unlock(&ov02a10->mutex);
+
+	return 0;
+
+err_rpm_put:
+	pm_runtime_put(&client->dev);
+unlock_and_return:
+	mutex_unlock(&ov02a10->mutex);
+
+	return ret;
+}
+
+static const struct dev_pm_ops ov02a10_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend,
+				pm_runtime_force_resume)
+	SET_RUNTIME_PM_OPS(ov02a10_power_off, ov02a10_power_on, NULL)
+};
+
+static int ov02a10_set_exposure(struct ov02a10 *ov02a10, int val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov02a10->subdev);
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(client, REG_PAGE_SWITCH, REG_ENABLE);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_write_byte_data(client, OV02A10_REG_EXPOSURE_H,
+					val >> OV02A10_EXP_SHIFT);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_write_byte_data(client, OV02A10_REG_EXPOSURE_L, val);
+	if (ret < 0)
+		return ret;
+
+	return i2c_smbus_write_byte_data(client, REG_GLOBAL_EFFECTIVE,
+					 REG_ENABLE);
+}
+
+static int ov02a10_set_gain(struct ov02a10 *ov02a10, int val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov02a10->subdev);
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(client, REG_PAGE_SWITCH, REG_ENABLE);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_write_byte_data(client, OV02A10_REG_GAIN, val);
+	if (ret < 0)
+		return ret;
+
+	return i2c_smbus_write_byte_data(client, REG_GLOBAL_EFFECTIVE,
+					 REG_ENABLE);
+}
+
+static int ov02a10_set_vblank(struct ov02a10 *ov02a10, int val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov02a10->subdev);
+	u32 vts = val + ov02a10->cur_mode->height - OV02A10_BASE_LINES;
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(client, REG_PAGE_SWITCH, REG_ENABLE);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_write_byte_data(client, OV02A10_REG_VTS_H,
+					vts >> OV02A10_VTS_SHIFT);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_write_byte_data(client, OV02A10_REG_VTS_L, vts);
+	if (ret < 0)
+		return ret;
+
+	return i2c_smbus_write_byte_data(client, REG_GLOBAL_EFFECTIVE,
+					 REG_ENABLE);
+}
+
+static int ov02a10_set_test_pattern(struct ov02a10 *ov02a10, int pattern)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov02a10->subdev);
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(client, REG_PAGE_SWITCH, REG_ENABLE);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_write_byte_data(client, OV02A10_REG_TEST_PATTERN,
+					pattern);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_write_byte_data(client, REG_GLOBAL_EFFECTIVE,
+					REG_ENABLE);
+	if (ret < 0)
+		return ret;
+
+	return i2c_smbus_write_byte_data(client, REG_SC_CTRL_MODE,
+					 SC_CTRL_MODE_STREAMING);
+}
+
+static int ov02a10_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov02a10 *ov02a10 = container_of(ctrl->handler,
+					       struct ov02a10, ctrl_handler);
+	struct i2c_client *client = v4l2_get_subdevdata(&ov02a10->subdev);
+	s64 max_expo;
+	int ret;
+
+	/* Propagate change of current control to all related controls */
+	if (ctrl->id == V4L2_CID_VBLANK) {
+		/* Update max exposure while meeting expected vblanking */
+		max_expo = ov02a10->cur_mode->height + ctrl->val -
+			   OV02A10_EXPOSURE_MAX_MARGIN;
+		__v4l2_ctrl_modify_range(ov02a10->exposure,
+					 ov02a10->exposure->minimum, max_expo,
+					 ov02a10->exposure->step,
+					 ov02a10->exposure->default_value);
+	}
+
+	/* V4L2 controls values will be applied only when power is already up */
+	if (!pm_runtime_get_if_in_use(&client->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE:
+		ret = ov02a10_set_exposure(ov02a10, ctrl->val);
+		break;
+	case V4L2_CID_ANALOGUE_GAIN:
+		ret = ov02a10_set_gain(ov02a10, ctrl->val);
+		break;
+	case V4L2_CID_VBLANK:
+		ret = ov02a10_set_vblank(ov02a10, ctrl->val);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		ret = ov02a10_set_test_pattern(ov02a10, ctrl->val);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	pm_runtime_put(&client->dev);
+
+	return ret;
+}
+
+static const struct v4l2_subdev_video_ops ov02a10_video_ops = {
+	.s_stream = ov02a10_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops ov02a10_pad_ops = {
+	.init_cfg = ov02a10_entity_init_cfg,
+	.enum_mbus_code = ov02a10_enum_mbus_code,
+	.enum_frame_size = ov02a10_enum_frame_sizes,
+	.get_fmt = ov02a10_get_fmt,
+	.set_fmt = ov02a10_set_fmt,
+};
+
+static const struct v4l2_subdev_ops ov02a10_subdev_ops = {
+	.video	= &ov02a10_video_ops,
+	.pad	= &ov02a10_pad_ops,
+};
+
+static const struct media_entity_operations ov02a10_subdev_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static const struct v4l2_ctrl_ops ov02a10_ctrl_ops = {
+	.s_ctrl = ov02a10_set_ctrl,
+};
+
+static int ov02a10_initialize_controls(struct ov02a10 *ov02a10)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov02a10->subdev);
+	const struct ov02a10_mode *mode;
+	struct v4l2_ctrl_handler *handler;
+	struct v4l2_ctrl *ctrl;
+	s64 exposure_max;
+	s64 vblank_def;
+	s64 pixel_rate;
+	s64 h_blank;
+	int ret;
+
+	handler = &ov02a10->ctrl_handler;
+	mode = ov02a10->cur_mode;
+	ret = v4l2_ctrl_handler_init(handler, 7);
+	if (ret)
+		return ret;
+
+	handler->lock = &ov02a10->mutex;
+
+	ctrl = v4l2_ctrl_new_int_menu(handler, NULL, V4L2_CID_LINK_FREQ, 0, 0,
+				      link_freq_menu_items);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	pixel_rate = to_pixel_rate(0);
+	v4l2_ctrl_new_std(handler, NULL, V4L2_CID_PIXEL_RATE, 0, pixel_rate, 1,
+			  pixel_rate);
+
+	h_blank = mode->hts_def - mode->width;
+	v4l2_ctrl_new_std(handler, NULL, V4L2_CID_HBLANK, h_blank, h_blank, 1,
+			  h_blank);
+
+	vblank_def = mode->vts_def - mode->height;
+	v4l2_ctrl_new_std(handler, &ov02a10_ctrl_ops, V4L2_CID_VBLANK,
+			  vblank_def, OV02A10_VTS_MAX - mode->height, 1,
+			  vblank_def);
+
+	exposure_max = mode->vts_def - 4;
+	ov02a10->exposure = v4l2_ctrl_new_std(handler, &ov02a10_ctrl_ops,
+					      V4L2_CID_EXPOSURE,
+					      OV02A10_EXPOSURE_MIN,
+					      exposure_max,
+					      OV02A10_EXPOSURE_STEP,
+					      mode->exp_def);
+
+	v4l2_ctrl_new_std(handler, &ov02a10_ctrl_ops,
+			  V4L2_CID_ANALOGUE_GAIN, OV02A10_GAIN_MIN,
+			  OV02A10_GAIN_MAX, OV02A10_GAIN_STEP,
+			  OV02A10_GAIN_DEFAULT);
+
+	v4l2_ctrl_new_std_menu_items(handler, &ov02a10_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(ov02a10_test_pattern_menu) - 1,
+				     0, 0, ov02a10_test_pattern_menu);
+
+	if (handler->error) {
+		ret = handler->error;
+		dev_err(&client->dev, "failed to init controls(%d)\n", ret);
+		goto err_free_handler;
+	}
+
+	ov02a10->subdev.ctrl_handler = handler;
+
+	return 0;
+
+err_free_handler:
+	v4l2_ctrl_handler_free(handler);
+
+	return ret;
+}
+
+static int ov02a10_check_hwcfg(struct device *dev, struct ov02a10 *ov02a10)
+{
+	struct fwnode_handle *ep;
+	struct fwnode_handle *fwnode = dev_fwnode(dev);
+	struct v4l2_fwnode_endpoint bus_cfg = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY,
+	};
+	unsigned int i, j;
+	u32 clk_volt;
+	int ret;
+
+	if (!fwnode)
+		return -EINVAL;
+
+	ep = fwnode_graph_get_next_endpoint(fwnode, NULL);
+	if (!ep)
+		return -ENXIO;
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(ep, &bus_cfg);
+	fwnode_handle_put(ep);
+	if (ret)
+		return ret;
+
+	/* Optional indication of MIPI clock voltage unit */
+	ret = fwnode_property_read_u32(ep, "ovti,mipi-clock-voltage",
+				       &clk_volt);
+
+	if (!ret)
+		ov02a10->mipi_clock_voltage = clk_volt;
+
+	for (i = 0; i < ARRAY_SIZE(link_freq_menu_items); i++) {
+		for (j = 0; j < bus_cfg.nr_of_link_frequencies; j++) {
+			if (link_freq_menu_items[i] ==
+				bus_cfg.link_frequencies[j])
+				break;
+		}
+
+		if (j == bus_cfg.nr_of_link_frequencies) {
+			dev_err(dev, "no link frequency %lld supported\n",
+				link_freq_menu_items[i]);
+			ret = -EINVAL;
+			break;
+		}
+	}
+
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+
+	return ret;
+}
+
+static int ov02a10_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct ov02a10 *ov02a10;
+	unsigned int i;
+	unsigned int rotation;
+	int ret;
+
+	ov02a10 = devm_kzalloc(dev, sizeof(*ov02a10), GFP_KERNEL);
+	if (!ov02a10)
+		return -ENOMEM;
+
+	ret = ov02a10_check_hwcfg(dev, ov02a10);
+	if (ret)
+		return dev_err_probe(dev, ret,
+				     "failed to check HW configuration\n");
+
+	v4l2_i2c_subdev_init(&ov02a10->subdev, client, &ov02a10_subdev_ops);
+
+	ov02a10->mipi_clock_voltage = OV02A10_MIPI_TX_SPEED_DEFAULT;
+	ov02a10->fmt.code = MEDIA_BUS_FMT_SBGGR10_1X10;
+
+	/* Optional indication of physical rotation of sensor */
+	rotation = 0;
+	device_property_read_u32(dev, "rotation", &rotation);
+	if (rotation == 180) {
+		ov02a10->upside_down = true;
+		ov02a10->fmt.code = MEDIA_BUS_FMT_SRGGB10_1X10;
+	}
+
+	ov02a10->eclk = devm_clk_get(dev, "eclk");
+	if (IS_ERR(ov02a10->eclk))
+		return dev_err_probe(dev, PTR_ERR(ov02a10->eclk),
+				     "failed to get eclk\n");
+
+	ret = device_property_read_u32(dev, "clock-frequency",
+				       &ov02a10->eclk_freq);
+	if (ret < 0)
+		return dev_err_probe(dev, ret,
+				     "failed to get eclk frequency\n");
+
+	ret = clk_set_rate(ov02a10->eclk, ov02a10->eclk_freq);
+	if (ret < 0)
+		return dev_err_probe(dev, ret,
+				     "failed to set eclk frequency (24MHz)\n");
+
+	if (clk_get_rate(ov02a10->eclk) != OV02A10_ECLK_FREQ)
+		dev_warn(dev, "eclk mismatched, mode is based on 24MHz\n");
+
+	ov02a10->pd_gpio = devm_gpiod_get(dev, "powerdown", GPIOD_OUT_HIGH);
+	if (IS_ERR(ov02a10->pd_gpio))
+		return dev_err_probe(dev, PTR_ERR(ov02a10->pd_gpio),
+				     "failed to get powerdown-gpios\n");
+
+	ov02a10->rst_gpio = devm_gpiod_get(dev, "reset", GPIOD_OUT_HIGH);
+	if (IS_ERR(ov02a10->rst_gpio))
+		return dev_err_probe(dev, PTR_ERR(ov02a10->rst_gpio),
+				     "failed to get reset-gpios\n");
+
+	for (i = 0; i < ARRAY_SIZE(ov02a10_supply_names); i++)
+		ov02a10->supplies[i].supply = ov02a10_supply_names[i];
+
+	ret = devm_regulator_bulk_get(dev, ARRAY_SIZE(ov02a10_supply_names),
+				      ov02a10->supplies);
+	if (ret)
+		return dev_err_probe(dev, ret, "failed to get regulators\n");
+
+	mutex_init(&ov02a10->mutex);
+
+	/* Set default mode */
+	ov02a10->cur_mode = &supported_modes[0];
+
+	ret = ov02a10_initialize_controls(ov02a10);
+	if (ret) {
+		dev_err_probe(dev, ret, "failed to initialize controls\n");
+		goto err_destroy_mutex;
+	}
+
+	/* Initialize subdev */
+	ov02a10->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	ov02a10->subdev.entity.ops = &ov02a10_subdev_entity_ops;
+	ov02a10->subdev.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ov02a10->pad.flags = MEDIA_PAD_FL_SOURCE;
+
+	ret = media_entity_pads_init(&ov02a10->subdev.entity, 1, &ov02a10->pad);
+	if (ret < 0) {
+		dev_err_probe(dev, ret, "failed to initialize entity pads\n");
+		goto err_free_handler;
+	}
+
+	pm_runtime_enable(dev);
+	if (!pm_runtime_enabled(dev)) {
+		ret = ov02a10_power_on(dev);
+		if (ret < 0) {
+			dev_err_probe(dev, ret, "failed to power on\n");
+			goto err_clean_entity;
+		}
+	}
+
+	ret = v4l2_async_register_subdev(&ov02a10->subdev);
+	if (ret) {
+		dev_err_probe(dev, ret, "failed to register V4L2 subdev\n");
+		goto err_power_off;
+	}
+
+	return 0;
+
+err_power_off:
+	if (pm_runtime_enabled(dev))
+		pm_runtime_disable(dev);
+	else
+		ov02a10_power_off(dev);
+err_clean_entity:
+	media_entity_cleanup(&ov02a10->subdev.entity);
+err_free_handler:
+	v4l2_ctrl_handler_free(ov02a10->subdev.ctrl_handler);
+err_destroy_mutex:
+	mutex_destroy(&ov02a10->mutex);
+
+	return ret;
+}
+
+static void ov02a10_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov02a10 *ov02a10 = to_ov02a10(sd);
+
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+	pm_runtime_disable(&client->dev);
+	if (!pm_runtime_status_suspended(&client->dev))
+		ov02a10_power_off(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+	mutex_destroy(&ov02a10->mutex);
+}
+
+static const struct of_device_id ov02a10_of_match[] = {
+	{ .compatible = "ovti,ov02a10" },
+	{}
+};
+MODULE_DEVICE_TABLE(of, ov02a10_of_match);
+
+static struct i2c_driver ov02a10_i2c_driver = {
+	.driver = {
+		.name = "ov02a10",
+		.pm = &ov02a10_pm_ops,
+		.of_match_table = ov02a10_of_match,
+	},
+	.probe		= ov02a10_probe,
+	.remove		= ov02a10_remove,
+};
+module_i2c_driver(ov02a10_i2c_driver);
+
+MODULE_AUTHOR("Dongchun Zhu <dongchun.zhu@mediatek.com>");
+MODULE_DESCRIPTION("OmniVision OV02A10 sensor driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/ov08d10.c b/drivers/media/i2c/ov08d10.c
new file mode 100644
index 000000000..7d55d4ca2
--- /dev/null
+++ b/drivers/media/i2c/ov08d10.c
@@ -0,0 +1,1531 @@
+// SPDX-License-Identifier: GPL-2.0
+// Copyright (c) 2022 Intel Corporation.
+
+#include <linux/acpi.h>
+#include <linux/clk.h>
+#include <linux/delay.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-fwnode.h>
+
+#define OV08D10_SCLK			144000000ULL
+#define OV08D10_XVCLK_19_2		19200000
+#define OV08D10_ROWCLK			36000
+#define OV08D10_DATA_LANES		2
+#define OV08D10_RGB_DEPTH		10
+
+#define OV08D10_REG_PAGE		0xfd
+#define OV08D10_REG_GLOBAL_EFFECTIVE		0x01
+#define OV08D10_REG_CHIP_ID_0		0x00
+#define OV08D10_REG_CHIP_ID_1		0x01
+#define OV08D10_ID_MASK			GENMASK(15, 0)
+#define OV08D10_CHIP_ID			0x5608
+
+#define OV08D10_REG_MODE_SELECT		0xa0
+#define OV08D10_MODE_STANDBY		0x00
+#define OV08D10_MODE_STREAMING		0x01
+
+/* vertical-timings from sensor */
+#define OV08D10_REG_VTS_H		0x05
+#define OV08D10_REG_VTS_L		0x06
+#define OV08D10_VTS_MAX			0x7fff
+
+/* Exposure controls from sensor */
+#define OV08D10_REG_EXPOSURE_H		0x02
+#define OV08D10_REG_EXPOSURE_M		0x03
+#define OV08D10_REG_EXPOSURE_L		0x04
+#define	OV08D10_EXPOSURE_MIN		6
+#define OV08D10_EXPOSURE_MAX_MARGIN	6
+#define	OV08D10_EXPOSURE_STEP		1
+
+/* Analog gain controls from sensor */
+#define OV08D10_REG_ANALOG_GAIN		0x24
+#define	OV08D10_ANAL_GAIN_MIN		128
+#define	OV08D10_ANAL_GAIN_MAX		2047
+#define	OV08D10_ANAL_GAIN_STEP		1
+
+/* Digital gain controls from sensor */
+#define OV08D10_REG_MWB_DGAIN_C		0x21
+#define OV08D10_REG_MWB_DGAIN_F		0x22
+#define OV08D10_DGTL_GAIN_MIN		0
+#define OV08D10_DGTL_GAIN_MAX		4095
+#define OV08D10_DGTL_GAIN_STEP		1
+#define OV08D10_DGTL_GAIN_DEFAULT	1024
+
+/* Test Pattern Control */
+#define OV08D10_REG_TEST_PATTERN		0x12
+#define OV08D10_TEST_PATTERN_ENABLE		0x01
+#define OV08D10_TEST_PATTERN_DISABLE		0x00
+
+/* Flip Mirror Controls from sensor */
+#define OV08D10_REG_FLIP_OPT			0x32
+#define OV08D10_REG_FLIP_MASK			0x3
+
+#define to_ov08d10(_sd)		container_of(_sd, struct ov08d10, sd)
+
+struct ov08d10_reg {
+	u8 address;
+	u8 val;
+};
+
+struct ov08d10_reg_list {
+	u32 num_of_regs;
+	const struct ov08d10_reg *regs;
+};
+
+struct ov08d10_link_freq_config {
+	const struct ov08d10_reg_list reg_list;
+};
+
+struct ov08d10_mode {
+	/* Frame width in pixels */
+	u32 width;
+
+	/* Frame height in pixels */
+	u32 height;
+
+	/* Horizontal timining size */
+	u32 hts;
+
+	/* Default vertical timining size */
+	u32 vts_def;
+
+	/* Min vertical timining size */
+	u32 vts_min;
+
+	/* Link frequency needed for this resolution */
+	u32 link_freq_index;
+
+	/* Sensor register settings for this resolution */
+	const struct ov08d10_reg_list reg_list;
+
+	/* Number of data lanes */
+	u8 data_lanes;
+};
+
+/* 3280x2460, 3264x2448 need 720Mbps/lane, 2 lanes */
+static const struct ov08d10_reg mipi_data_rate_720mbps[] = {
+	{0xfd, 0x00},
+	{0x11, 0x2a},
+	{0x14, 0x43},
+	{0x1a, 0x04},
+	{0x1b, 0xe1},
+	{0x1e, 0x13},
+	{0xb7, 0x02}
+};
+
+/* 1632x1224 needs 360Mbps/lane, 2 lanes */
+static const struct ov08d10_reg mipi_data_rate_360mbps[] = {
+	{0xfd, 0x00},
+	{0x1a, 0x04},
+	{0x1b, 0xe1},
+	{0x1d, 0x00},
+	{0x1c, 0x19},
+	{0x11, 0x2a},
+	{0x14, 0x54},
+	{0x1e, 0x13},
+	{0xb7, 0x02}
+};
+
+static const struct ov08d10_reg lane_2_mode_3280x2460[] = {
+	/* 3280x2460 resolution */
+	{0xfd, 0x01},
+	{0x12, 0x00},
+	{0x03, 0x12},
+	{0x04, 0x58},
+	{0x07, 0x05},
+	{0x21, 0x02},
+	{0x24, 0x30},
+	{0x33, 0x03},
+	{0x01, 0x03},
+	{0x19, 0x10},
+	{0x42, 0x55},
+	{0x43, 0x00},
+	{0x47, 0x07},
+	{0x48, 0x08},
+	{0xb2, 0x7f},
+	{0xb3, 0x7b},
+	{0xbd, 0x08},
+	{0xd2, 0x57},
+	{0xd3, 0x10},
+	{0xd4, 0x08},
+	{0xd5, 0x08},
+	{0xd6, 0x06},
+	{0xb1, 0x00},
+	{0xb4, 0x00},
+	{0xb7, 0x0a},
+	{0xbc, 0x44},
+	{0xbf, 0x48},
+	{0xc1, 0x10},
+	{0xc3, 0x24},
+	{0xc8, 0x03},
+	{0xc9, 0xf8},
+	{0xe1, 0x33},
+	{0xe2, 0xbb},
+	{0x51, 0x0c},
+	{0x52, 0x0a},
+	{0x57, 0x8c},
+	{0x59, 0x09},
+	{0x5a, 0x08},
+	{0x5e, 0x10},
+	{0x60, 0x02},
+	{0x6d, 0x5c},
+	{0x76, 0x16},
+	{0x7c, 0x11},
+	{0x90, 0x28},
+	{0x91, 0x16},
+	{0x92, 0x1c},
+	{0x93, 0x24},
+	{0x95, 0x48},
+	{0x9c, 0x06},
+	{0xca, 0x0c},
+	{0xce, 0x0d},
+	{0xfd, 0x01},
+	{0xc0, 0x00},
+	{0xdd, 0x18},
+	{0xde, 0x19},
+	{0xdf, 0x32},
+	{0xe0, 0x70},
+	{0xfd, 0x01},
+	{0xc2, 0x05},
+	{0xd7, 0x88},
+	{0xd8, 0x77},
+	{0xd9, 0x00},
+	{0xfd, 0x07},
+	{0x00, 0xf8},
+	{0x01, 0x2b},
+	{0x05, 0x40},
+	{0x08, 0x06},
+	{0x09, 0x11},
+	{0x28, 0x6f},
+	{0x2a, 0x20},
+	{0x2b, 0x05},
+	{0x5e, 0x10},
+	{0x52, 0x00},
+	{0x53, 0x7c},
+	{0x54, 0x00},
+	{0x55, 0x7c},
+	{0x56, 0x00},
+	{0x57, 0x7c},
+	{0x58, 0x00},
+	{0x59, 0x7c},
+	{0xfd, 0x02},
+	{0x9a, 0x30},
+	{0xa8, 0x02},
+	{0xfd, 0x02},
+	{0xa1, 0x01},
+	{0xa2, 0x09},
+	{0xa3, 0x9c},
+	{0xa5, 0x00},
+	{0xa6, 0x0c},
+	{0xa7, 0xd0},
+	{0xfd, 0x00},
+	{0x24, 0x01},
+	{0xc0, 0x16},
+	{0xc1, 0x08},
+	{0xc2, 0x30},
+	{0x8e, 0x0c},
+	{0x8f, 0xd0},
+	{0x90, 0x09},
+	{0x91, 0x9c},
+	{0xfd, 0x05},
+	{0x04, 0x40},
+	{0x07, 0x00},
+	{0x0d, 0x01},
+	{0x0f, 0x01},
+	{0x10, 0x00},
+	{0x11, 0x00},
+	{0x12, 0x0c},
+	{0x13, 0xcf},
+	{0x14, 0x00},
+	{0x15, 0x00},
+	{0xfd, 0x00},
+	{0x20, 0x0f},
+	{0xe7, 0x03},
+	{0xe7, 0x00}
+};
+
+static const struct ov08d10_reg lane_2_mode_3264x2448[] = {
+	/* 3264x2448 resolution */
+	{0xfd, 0x01},
+	{0x12, 0x00},
+	{0x03, 0x12},
+	{0x04, 0x58},
+	{0x07, 0x05},
+	{0x21, 0x02},
+	{0x24, 0x30},
+	{0x33, 0x03},
+	{0x01, 0x03},
+	{0x19, 0x10},
+	{0x42, 0x55},
+	{0x43, 0x00},
+	{0x47, 0x07},
+	{0x48, 0x08},
+	{0xb2, 0x7f},
+	{0xb3, 0x7b},
+	{0xbd, 0x08},
+	{0xd2, 0x57},
+	{0xd3, 0x10},
+	{0xd4, 0x08},
+	{0xd5, 0x08},
+	{0xd6, 0x06},
+	{0xb1, 0x00},
+	{0xb4, 0x00},
+	{0xb7, 0x0a},
+	{0xbc, 0x44},
+	{0xbf, 0x48},
+	{0xc1, 0x10},
+	{0xc3, 0x24},
+	{0xc8, 0x03},
+	{0xc9, 0xf8},
+	{0xe1, 0x33},
+	{0xe2, 0xbb},
+	{0x51, 0x0c},
+	{0x52, 0x0a},
+	{0x57, 0x8c},
+	{0x59, 0x09},
+	{0x5a, 0x08},
+	{0x5e, 0x10},
+	{0x60, 0x02},
+	{0x6d, 0x5c},
+	{0x76, 0x16},
+	{0x7c, 0x11},
+	{0x90, 0x28},
+	{0x91, 0x16},
+	{0x92, 0x1c},
+	{0x93, 0x24},
+	{0x95, 0x48},
+	{0x9c, 0x06},
+	{0xca, 0x0c},
+	{0xce, 0x0d},
+	{0xfd, 0x01},
+	{0xc0, 0x00},
+	{0xdd, 0x18},
+	{0xde, 0x19},
+	{0xdf, 0x32},
+	{0xe0, 0x70},
+	{0xfd, 0x01},
+	{0xc2, 0x05},
+	{0xd7, 0x88},
+	{0xd8, 0x77},
+	{0xd9, 0x00},
+	{0xfd, 0x07},
+	{0x00, 0xf8},
+	{0x01, 0x2b},
+	{0x05, 0x40},
+	{0x08, 0x06},
+	{0x09, 0x11},
+	{0x28, 0x6f},
+	{0x2a, 0x20},
+	{0x2b, 0x05},
+	{0x5e, 0x10},
+	{0x52, 0x00},
+	{0x53, 0x7c},
+	{0x54, 0x00},
+	{0x55, 0x7c},
+	{0x56, 0x00},
+	{0x57, 0x7c},
+	{0x58, 0x00},
+	{0x59, 0x7c},
+	{0xfd, 0x02},
+	{0x9a, 0x30},
+	{0xa8, 0x02},
+	{0xfd, 0x02},
+	{0xa1, 0x09},
+	{0xa2, 0x09},
+	{0xa3, 0x90},
+	{0xa5, 0x08},
+	{0xa6, 0x0c},
+	{0xa7, 0xc0},
+	{0xfd, 0x00},
+	{0x24, 0x01},
+	{0xc0, 0x16},
+	{0xc1, 0x08},
+	{0xc2, 0x30},
+	{0x8e, 0x0c},
+	{0x8f, 0xc0},
+	{0x90, 0x09},
+	{0x91, 0x90},
+	{0xfd, 0x05},
+	{0x04, 0x40},
+	{0x07, 0x00},
+	{0x0d, 0x01},
+	{0x0f, 0x01},
+	{0x10, 0x00},
+	{0x11, 0x00},
+	{0x12, 0x0c},
+	{0x13, 0xcf},
+	{0x14, 0x00},
+	{0x15, 0x00},
+	{0xfd, 0x00},
+	{0x20, 0x0f},
+	{0xe7, 0x03},
+	{0xe7, 0x00}
+};
+
+static const struct ov08d10_reg lane_2_mode_1632x1224[] = {
+	/* 1640x1232 resolution */
+	{0xfd, 0x01},
+	{0x1a, 0x0a},
+	{0x1b, 0x08},
+	{0x2a, 0x01},
+	{0x2b, 0x9a},
+	{0xfd, 0x01},
+	{0x12, 0x00},
+	{0x03, 0x05},
+	{0x04, 0xe2},
+	{0x07, 0x05},
+	{0x21, 0x02},
+	{0x24, 0x30},
+	{0x33, 0x03},
+	{0x31, 0x06},
+	{0x33, 0x03},
+	{0x01, 0x03},
+	{0x19, 0x10},
+	{0x42, 0x55},
+	{0x43, 0x00},
+	{0x47, 0x07},
+	{0x48, 0x08},
+	{0xb2, 0x7f},
+	{0xb3, 0x7b},
+	{0xbd, 0x08},
+	{0xd2, 0x57},
+	{0xd3, 0x10},
+	{0xd4, 0x08},
+	{0xd5, 0x08},
+	{0xd6, 0x06},
+	{0xb1, 0x00},
+	{0xb4, 0x00},
+	{0xb7, 0x0a},
+	{0xbc, 0x44},
+	{0xbf, 0x48},
+	{0xc1, 0x10},
+	{0xc3, 0x24},
+	{0xc8, 0x03},
+	{0xc9, 0xf8},
+	{0xe1, 0x33},
+	{0xe2, 0xbb},
+	{0x51, 0x0c},
+	{0x52, 0x0a},
+	{0x57, 0x8c},
+	{0x59, 0x09},
+	{0x5a, 0x08},
+	{0x5e, 0x10},
+	{0x60, 0x02},
+	{0x6d, 0x5c},
+	{0x76, 0x16},
+	{0x7c, 0x1a},
+	{0x90, 0x28},
+	{0x91, 0x16},
+	{0x92, 0x1c},
+	{0x93, 0x24},
+	{0x95, 0x48},
+	{0x9c, 0x06},
+	{0xca, 0x0c},
+	{0xce, 0x0d},
+	{0xfd, 0x01},
+	{0xc0, 0x00},
+	{0xdd, 0x18},
+	{0xde, 0x19},
+	{0xdf, 0x32},
+	{0xe0, 0x70},
+	{0xfd, 0x01},
+	{0xc2, 0x05},
+	{0xd7, 0x88},
+	{0xd8, 0x77},
+	{0xd9, 0x00},
+	{0xfd, 0x07},
+	{0x00, 0xf8},
+	{0x01, 0x2b},
+	{0x05, 0x40},
+	{0x08, 0x03},
+	{0x09, 0x08},
+	{0x28, 0x6f},
+	{0x2a, 0x20},
+	{0x2b, 0x05},
+	{0x2c, 0x01},
+	{0x50, 0x02},
+	{0x51, 0x03},
+	{0x5e, 0x00},
+	{0x52, 0x00},
+	{0x53, 0x7c},
+	{0x54, 0x00},
+	{0x55, 0x7c},
+	{0x56, 0x00},
+	{0x57, 0x7c},
+	{0x58, 0x00},
+	{0x59, 0x7c},
+	{0xfd, 0x02},
+	{0x9a, 0x30},
+	{0xa8, 0x02},
+	{0xfd, 0x02},
+	{0xa9, 0x04},
+	{0xaa, 0xd0},
+	{0xab, 0x06},
+	{0xac, 0x68},
+	{0xa1, 0x09},
+	{0xa2, 0x04},
+	{0xa3, 0xc8},
+	{0xa5, 0x04},
+	{0xa6, 0x06},
+	{0xa7, 0x60},
+	{0xfd, 0x05},
+	{0x06, 0x80},
+	{0x18, 0x06},
+	{0x19, 0x68},
+	{0xfd, 0x00},
+	{0x24, 0x01},
+	{0xc0, 0x16},
+	{0xc1, 0x08},
+	{0xc2, 0x30},
+	{0x8e, 0x06},
+	{0x8f, 0x60},
+	{0x90, 0x04},
+	{0x91, 0xc8},
+	{0x93, 0x0e},
+	{0x94, 0x77},
+	{0x95, 0x77},
+	{0x96, 0x10},
+	{0x98, 0x88},
+	{0x9c, 0x1a},
+	{0xfd, 0x05},
+	{0x04, 0x40},
+	{0x07, 0x99},
+	{0x0d, 0x03},
+	{0x0f, 0x03},
+	{0x10, 0x00},
+	{0x11, 0x00},
+	{0x12, 0x0c},
+	{0x13, 0xcf},
+	{0x14, 0x00},
+	{0x15, 0x00},
+	{0xfd, 0x00},
+	{0x20, 0x0f},
+	{0xe7, 0x03},
+	{0xe7, 0x00},
+};
+
+static const char * const ov08d10_test_pattern_menu[] = {
+	"Disabled",
+	"Standard Color Bar",
+};
+
+struct ov08d10 {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct v4l2_ctrl_handler ctrl_handler;
+
+	struct clk		*xvclk;
+
+	/* V4L2 Controls */
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *pixel_rate;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *vflip;
+	struct v4l2_ctrl *hflip;
+	struct v4l2_ctrl *exposure;
+
+	/* Current mode */
+	const struct ov08d10_mode *cur_mode;
+
+	/* To serialize asynchronus callbacks */
+	struct mutex mutex;
+
+	/* Streaming on/off */
+	bool streaming;
+
+	/* lanes index */
+	u8 nlanes;
+
+	const struct ov08d10_lane_cfg *priv_lane;
+	u8 modes_size;
+};
+
+struct ov08d10_lane_cfg {
+	const s64 link_freq_menu[2];
+	const struct ov08d10_link_freq_config link_freq_configs[2];
+	const struct ov08d10_mode sp_modes[3];
+};
+
+static const struct ov08d10_lane_cfg lane_cfg_2 = {
+	{
+		720000000,
+		360000000,
+	},
+	{{
+		.reg_list = {
+			.num_of_regs =
+				ARRAY_SIZE(mipi_data_rate_720mbps),
+			.regs = mipi_data_rate_720mbps,
+		}
+	},
+	{
+		.reg_list = {
+			.num_of_regs =
+				ARRAY_SIZE(mipi_data_rate_360mbps),
+			.regs = mipi_data_rate_360mbps,
+		}
+	}},
+	{{
+		.width = 3280,
+		.height = 2460,
+		.hts = 1840,
+		.vts_def = 2504,
+		.vts_min = 2504,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(lane_2_mode_3280x2460),
+			.regs = lane_2_mode_3280x2460,
+		},
+		.link_freq_index = 0,
+		.data_lanes = 2,
+	},
+	{
+		.width = 3264,
+		.height = 2448,
+		.hts = 1840,
+		.vts_def = 2504,
+		.vts_min = 2504,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(lane_2_mode_3264x2448),
+			.regs = lane_2_mode_3264x2448,
+		},
+		.link_freq_index = 0,
+		.data_lanes = 2,
+	},
+	{
+		.width = 1632,
+		.height = 1224,
+		.hts = 1912,
+		.vts_def = 3736,
+		.vts_min = 3736,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(lane_2_mode_1632x1224),
+			.regs = lane_2_mode_1632x1224,
+		},
+		.link_freq_index = 1,
+		.data_lanes = 2,
+	}}
+};
+
+static u32 ov08d10_get_format_code(struct ov08d10 *ov08d10)
+{
+	static const u32 codes[2][2] = {
+		{ MEDIA_BUS_FMT_SGRBG10_1X10, MEDIA_BUS_FMT_SRGGB10_1X10},
+		{ MEDIA_BUS_FMT_SBGGR10_1X10, MEDIA_BUS_FMT_SGBRG10_1X10},
+	};
+
+	return codes[ov08d10->vflip->val][ov08d10->hflip->val];
+}
+
+static unsigned int ov08d10_modes_num(const struct ov08d10 *ov08d10)
+{
+	unsigned int i, count = 0;
+
+	for (i = 0; i < ARRAY_SIZE(ov08d10->priv_lane->sp_modes); i++) {
+		if (ov08d10->priv_lane->sp_modes[i].width == 0)
+			break;
+		count++;
+	}
+
+	return count;
+}
+
+static u64 to_rate(const s64 *link_freq_menu,
+		   u32 f_index, u8 nlanes)
+{
+	u64 pixel_rate = link_freq_menu[f_index] * 2 * nlanes;
+
+	do_div(pixel_rate, OV08D10_RGB_DEPTH);
+
+	return pixel_rate;
+}
+
+static u64 to_pixels_per_line(const s64 *link_freq_menu, u32 hts,
+			      u32 f_index, u8 nlanes)
+{
+	u64 ppl = hts * to_rate(link_freq_menu, f_index, nlanes);
+
+	do_div(ppl, OV08D10_SCLK);
+
+	return ppl;
+}
+
+static int ov08d10_write_reg_list(struct ov08d10 *ov08d10,
+				  const struct ov08d10_reg_list *r_list)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov08d10->sd);
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < r_list->num_of_regs; i++) {
+		ret = i2c_smbus_write_byte_data(client, r_list->regs[i].address,
+						r_list->regs[i].val);
+		if (ret) {
+			dev_err_ratelimited(&client->dev,
+					    "failed to write reg 0x%2.2x. error = %d",
+					    r_list->regs[i].address, ret);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int ov08d10_update_analog_gain(struct ov08d10 *ov08d10, u32 a_gain)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov08d10->sd);
+	u8 val;
+	int ret;
+
+	val = ((a_gain >> 3) & 0xFF);
+	/* CIS control registers */
+	ret = i2c_smbus_write_byte_data(client, OV08D10_REG_PAGE, 0x01);
+	if (ret < 0)
+		return ret;
+
+	/* update AGAIN */
+	ret = i2c_smbus_write_byte_data(client, OV08D10_REG_ANALOG_GAIN, val);
+	if (ret < 0)
+		return ret;
+
+	return i2c_smbus_write_byte_data(client,
+					 OV08D10_REG_GLOBAL_EFFECTIVE, 0x01);
+}
+
+static int ov08d10_update_digital_gain(struct ov08d10 *ov08d10, u32 d_gain)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov08d10->sd);
+	u8 val;
+	int ret;
+
+	d_gain = (d_gain >> 1);
+	/* CIS control registers */
+	ret = i2c_smbus_write_byte_data(client, OV08D10_REG_PAGE, 0x01);
+	if (ret < 0)
+		return ret;
+
+	val = ((d_gain >> 8) & 0x3F);
+	/* update DGAIN */
+	ret = i2c_smbus_write_byte_data(client, OV08D10_REG_MWB_DGAIN_C, val);
+	if (ret < 0)
+		return ret;
+
+	val = d_gain & 0xFF;
+	ret = i2c_smbus_write_byte_data(client, OV08D10_REG_MWB_DGAIN_F, val);
+	if (ret < 0)
+		return ret;
+
+	return i2c_smbus_write_byte_data(client,
+					 OV08D10_REG_GLOBAL_EFFECTIVE, 0x01);
+}
+
+static int ov08d10_set_exposure(struct ov08d10 *ov08d10, u32 exposure)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov08d10->sd);
+	u8 val;
+	u8 hts_h, hts_l;
+	u32 hts, cur_vts, exp_cal;
+	int ret;
+
+	cur_vts = ov08d10->cur_mode->vts_def;
+	ret = i2c_smbus_write_byte_data(client, OV08D10_REG_PAGE, 0x01);
+	if (ret < 0)
+		return ret;
+
+	hts_h = i2c_smbus_read_byte_data(client, 0x37);
+	hts_l = i2c_smbus_read_byte_data(client, 0x38);
+	hts = ((hts_h << 8) | (hts_l));
+	exp_cal = 66 * OV08D10_ROWCLK / hts;
+	exposure = exposure * exp_cal / (cur_vts - OV08D10_EXPOSURE_MAX_MARGIN);
+	/* CIS control registers */
+	ret = i2c_smbus_write_byte_data(client, OV08D10_REG_PAGE, 0x01);
+	if (ret < 0)
+		return ret;
+
+	/* update exposure */
+	val = ((exposure >> 16) & 0xFF);
+	ret = i2c_smbus_write_byte_data(client, OV08D10_REG_EXPOSURE_H, val);
+	if (ret < 0)
+		return ret;
+
+	val = ((exposure >> 8) & 0xFF);
+	ret = i2c_smbus_write_byte_data(client, OV08D10_REG_EXPOSURE_M, val);
+	if (ret < 0)
+		return ret;
+
+	val = exposure & 0xFF;
+	ret = i2c_smbus_write_byte_data(client, OV08D10_REG_EXPOSURE_L, val);
+	if (ret < 0)
+		return ret;
+
+	return i2c_smbus_write_byte_data(client,
+					 OV08D10_REG_GLOBAL_EFFECTIVE, 0x01);
+}
+
+static int ov08d10_set_vblank(struct ov08d10 *ov08d10, u32 vblank)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov08d10->sd);
+	u8 val;
+	int ret;
+
+	/* CIS control registers */
+	ret = i2c_smbus_write_byte_data(client, OV08D10_REG_PAGE, 0x01);
+	if (ret < 0)
+		return ret;
+
+	val = ((vblank >> 8) & 0xFF);
+	/* update vblank */
+	ret = i2c_smbus_write_byte_data(client, OV08D10_REG_VTS_H, val);
+	if (ret < 0)
+		return ret;
+
+	val = vblank & 0xFF;
+	ret = i2c_smbus_write_byte_data(client, OV08D10_REG_VTS_L, val);
+	if (ret < 0)
+		return ret;
+
+	return i2c_smbus_write_byte_data(client,
+					 OV08D10_REG_GLOBAL_EFFECTIVE, 0x01);
+}
+
+static int ov08d10_test_pattern(struct ov08d10 *ov08d10, u32 pattern)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov08d10->sd);
+	u8 val;
+	int ret;
+
+	if (pattern)
+		val = OV08D10_TEST_PATTERN_ENABLE;
+	else
+		val = OV08D10_TEST_PATTERN_DISABLE;
+
+	/* CIS control registers */
+	ret = i2c_smbus_write_byte_data(client, OV08D10_REG_PAGE, 0x01);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_write_byte_data(client,
+					OV08D10_REG_TEST_PATTERN, val);
+	if (ret < 0)
+		return ret;
+
+	return i2c_smbus_write_byte_data(client,
+					 OV08D10_REG_GLOBAL_EFFECTIVE, 0x01);
+}
+
+static int ov08d10_set_ctrl_flip(struct ov08d10 *ov08d10, u32 ctrl_val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov08d10->sd);
+	u8 val;
+	int ret;
+
+	/* System control registers */
+	ret = i2c_smbus_write_byte_data(client, OV08D10_REG_PAGE, 0x01);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_read_byte_data(client, OV08D10_REG_FLIP_OPT);
+	if (ret < 0)
+		return ret;
+
+	val = ret | (ctrl_val & OV08D10_REG_FLIP_MASK);
+
+	ret = i2c_smbus_write_byte_data(client, OV08D10_REG_PAGE, 0x01);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_write_byte_data(client, OV08D10_REG_FLIP_OPT, val);
+
+	if (ret < 0)
+		return ret;
+
+	return i2c_smbus_write_byte_data(client,
+					 OV08D10_REG_GLOBAL_EFFECTIVE, 0x01);
+}
+
+static int ov08d10_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov08d10 *ov08d10 = container_of(ctrl->handler,
+					     struct ov08d10, ctrl_handler);
+	struct i2c_client *client = v4l2_get_subdevdata(&ov08d10->sd);
+	s64 exposure_max;
+	int ret;
+
+	/* Propagate change of current control to all related controls */
+	if (ctrl->id == V4L2_CID_VBLANK) {
+		/* Update max exposure while meeting expected vblanking */
+		exposure_max = ov08d10->cur_mode->height + ctrl->val -
+			       OV08D10_EXPOSURE_MAX_MARGIN;
+		__v4l2_ctrl_modify_range(ov08d10->exposure,
+					 ov08d10->exposure->minimum,
+					 exposure_max, ov08d10->exposure->step,
+					 exposure_max);
+	}
+
+	/* V4L2 controls values will be applied only when power is already up */
+	if (!pm_runtime_get_if_in_use(&client->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_ANALOGUE_GAIN:
+		ret = ov08d10_update_analog_gain(ov08d10, ctrl->val);
+		break;
+
+	case V4L2_CID_DIGITAL_GAIN:
+		ret = ov08d10_update_digital_gain(ov08d10, ctrl->val);
+		break;
+
+	case V4L2_CID_EXPOSURE:
+		ret = ov08d10_set_exposure(ov08d10, ctrl->val);
+		break;
+
+	case V4L2_CID_VBLANK:
+		ret = ov08d10_set_vblank(ov08d10, ctrl->val);
+		break;
+
+	case V4L2_CID_TEST_PATTERN:
+		ret = ov08d10_test_pattern(ov08d10, ctrl->val);
+		break;
+
+	case V4L2_CID_HFLIP:
+	case V4L2_CID_VFLIP:
+		ret = ov08d10_set_ctrl_flip(ov08d10,
+					    ov08d10->hflip->val |
+					    ov08d10->vflip->val << 1);
+		break;
+
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	pm_runtime_put(&client->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ov08d10_ctrl_ops = {
+	.s_ctrl = ov08d10_set_ctrl,
+};
+
+static int ov08d10_init_controls(struct ov08d10 *ov08d10)
+{
+	struct v4l2_ctrl_handler *ctrl_hdlr;
+	u8 link_freq_size;
+	s64 exposure_max;
+	s64 vblank_def;
+	s64 vblank_min;
+	s64 h_blank;
+	s64 pixel_rate_max;
+	const struct ov08d10_mode *mode;
+	int ret;
+
+	ctrl_hdlr = &ov08d10->ctrl_handler;
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 8);
+	if (ret)
+		return ret;
+
+	ctrl_hdlr->lock = &ov08d10->mutex;
+	link_freq_size = ARRAY_SIZE(ov08d10->priv_lane->link_freq_menu);
+	ov08d10->link_freq =
+		v4l2_ctrl_new_int_menu(ctrl_hdlr, &ov08d10_ctrl_ops,
+				       V4L2_CID_LINK_FREQ,
+				       link_freq_size - 1,
+				       0,
+				       ov08d10->priv_lane->link_freq_menu);
+	if (ov08d10->link_freq)
+		ov08d10->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	pixel_rate_max = to_rate(ov08d10->priv_lane->link_freq_menu, 0,
+				 ov08d10->cur_mode->data_lanes);
+	ov08d10->pixel_rate =
+		v4l2_ctrl_new_std(ctrl_hdlr, &ov08d10_ctrl_ops,
+				  V4L2_CID_PIXEL_RATE, 0, pixel_rate_max, 1,
+				  pixel_rate_max);
+
+	mode = ov08d10->cur_mode;
+	vblank_def = mode->vts_def - mode->height;
+	vblank_min = mode->vts_min - mode->height;
+	ov08d10->vblank =
+		v4l2_ctrl_new_std(ctrl_hdlr, &ov08d10_ctrl_ops,
+				  V4L2_CID_VBLANK, vblank_min,
+				  OV08D10_VTS_MAX - mode->height, 1,
+				  vblank_def);
+
+	h_blank = to_pixels_per_line(ov08d10->priv_lane->link_freq_menu,
+				     mode->hts, mode->link_freq_index,
+				     mode->data_lanes) -
+				     mode->width;
+	ov08d10->hblank = v4l2_ctrl_new_std(ctrl_hdlr, &ov08d10_ctrl_ops,
+					    V4L2_CID_HBLANK, h_blank, h_blank,
+					    1, h_blank);
+	if (ov08d10->hblank)
+		ov08d10->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov08d10_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
+			  OV08D10_ANAL_GAIN_MIN, OV08D10_ANAL_GAIN_MAX,
+			  OV08D10_ANAL_GAIN_STEP, OV08D10_ANAL_GAIN_MIN);
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov08d10_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
+			  OV08D10_DGTL_GAIN_MIN, OV08D10_DGTL_GAIN_MAX,
+			  OV08D10_DGTL_GAIN_STEP, OV08D10_DGTL_GAIN_DEFAULT);
+
+	exposure_max = mode->vts_def - OV08D10_EXPOSURE_MAX_MARGIN;
+	ov08d10->exposure = v4l2_ctrl_new_std(ctrl_hdlr, &ov08d10_ctrl_ops,
+					      V4L2_CID_EXPOSURE,
+					      OV08D10_EXPOSURE_MIN,
+					      exposure_max,
+					      OV08D10_EXPOSURE_STEP,
+					      exposure_max);
+
+	v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &ov08d10_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(ov08d10_test_pattern_menu) - 1,
+				     0, 0, ov08d10_test_pattern_menu);
+
+	ov08d10->hflip = v4l2_ctrl_new_std(ctrl_hdlr, &ov08d10_ctrl_ops,
+					   V4L2_CID_HFLIP, 0, 1, 1, 0);
+	if (ov08d10->hflip)
+		ov08d10->hflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
+	ov08d10->vflip = v4l2_ctrl_new_std(ctrl_hdlr, &ov08d10_ctrl_ops,
+					   V4L2_CID_VFLIP, 0, 1, 1, 0);
+	if (ov08d10->vflip)
+		ov08d10->vflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
+
+	if (ctrl_hdlr->error)
+		return ctrl_hdlr->error;
+
+	ov08d10->sd.ctrl_handler = ctrl_hdlr;
+
+	return 0;
+}
+
+static void ov08d10_update_pad_format(struct ov08d10 *ov08d10,
+				      const struct ov08d10_mode *mode,
+				      struct v4l2_mbus_framefmt *fmt)
+{
+	fmt->width = mode->width;
+	fmt->height = mode->height;
+	fmt->code = ov08d10_get_format_code(ov08d10);
+	fmt->field = V4L2_FIELD_NONE;
+}
+
+static int ov08d10_start_streaming(struct ov08d10 *ov08d10)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov08d10->sd);
+	const struct ov08d10_reg_list *reg_list;
+	int link_freq_index, ret;
+
+	link_freq_index = ov08d10->cur_mode->link_freq_index;
+	reg_list =
+	    &ov08d10->priv_lane->link_freq_configs[link_freq_index].reg_list;
+
+	/* soft reset */
+	ret = i2c_smbus_write_byte_data(client, OV08D10_REG_PAGE, 0x00);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed to reset sensor");
+		return ret;
+	}
+	ret = i2c_smbus_write_byte_data(client, 0x20, 0x0e);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed to reset sensor");
+		return ret;
+	}
+	usleep_range(3000, 4000);
+	ret = i2c_smbus_write_byte_data(client, 0x20, 0x0b);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed to reset sensor");
+		return ret;
+	}
+
+	/* update sensor setting */
+	ret = ov08d10_write_reg_list(ov08d10, reg_list);
+	if (ret) {
+		dev_err(&client->dev, "failed to set plls");
+		return ret;
+	}
+
+	reg_list = &ov08d10->cur_mode->reg_list;
+	ret = ov08d10_write_reg_list(ov08d10, reg_list);
+	if (ret) {
+		dev_err(&client->dev, "failed to set mode");
+		return ret;
+	}
+
+	ret = __v4l2_ctrl_handler_setup(ov08d10->sd.ctrl_handler);
+	if (ret)
+		return ret;
+
+	ret = i2c_smbus_write_byte_data(client, OV08D10_REG_PAGE, 0x00);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_write_byte_data(client, OV08D10_REG_MODE_SELECT,
+					OV08D10_MODE_STREAMING);
+	if (ret < 0)
+		return ret;
+
+	return i2c_smbus_write_byte_data(client, OV08D10_REG_PAGE, 0x01);
+}
+
+static void ov08d10_stop_streaming(struct ov08d10 *ov08d10)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov08d10->sd);
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(client, OV08D10_REG_PAGE, 0x00);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed to stop streaming");
+		return;
+	}
+	ret = i2c_smbus_write_byte_data(client, OV08D10_REG_MODE_SELECT,
+					OV08D10_MODE_STANDBY);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed to stop streaming");
+		return;
+	}
+
+	ret = i2c_smbus_write_byte_data(client, OV08D10_REG_PAGE, 0x01);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed to stop streaming");
+		return;
+	}
+}
+
+static int ov08d10_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct ov08d10 *ov08d10 = to_ov08d10(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	if (ov08d10->streaming == enable)
+		return 0;
+
+	mutex_lock(&ov08d10->mutex);
+	if (enable) {
+		ret = pm_runtime_resume_and_get(&client->dev);
+		if (ret < 0) {
+			mutex_unlock(&ov08d10->mutex);
+			return ret;
+		}
+
+		ret = ov08d10_start_streaming(ov08d10);
+		if (ret) {
+			enable = 0;
+			ov08d10_stop_streaming(ov08d10);
+			pm_runtime_put(&client->dev);
+		}
+	} else {
+		ov08d10_stop_streaming(ov08d10);
+		pm_runtime_put(&client->dev);
+	}
+
+	ov08d10->streaming = enable;
+
+	/* vflip and hflip cannot change during streaming */
+	__v4l2_ctrl_grab(ov08d10->vflip, enable);
+	__v4l2_ctrl_grab(ov08d10->hflip, enable);
+
+	mutex_unlock(&ov08d10->mutex);
+
+	return ret;
+}
+
+static int __maybe_unused ov08d10_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov08d10 *ov08d10 = to_ov08d10(sd);
+
+	mutex_lock(&ov08d10->mutex);
+	if (ov08d10->streaming)
+		ov08d10_stop_streaming(ov08d10);
+
+	mutex_unlock(&ov08d10->mutex);
+
+	return 0;
+}
+
+static int __maybe_unused ov08d10_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov08d10 *ov08d10 = to_ov08d10(sd);
+	int ret;
+
+	mutex_lock(&ov08d10->mutex);
+
+	if (ov08d10->streaming) {
+		ret = ov08d10_start_streaming(ov08d10);
+		if (ret) {
+			ov08d10->streaming = false;
+			ov08d10_stop_streaming(ov08d10);
+			mutex_unlock(&ov08d10->mutex);
+			return ret;
+		}
+	}
+
+	mutex_unlock(&ov08d10->mutex);
+
+	return 0;
+}
+
+static int ov08d10_set_format(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_format *fmt)
+{
+	struct ov08d10 *ov08d10 = to_ov08d10(sd);
+	const struct ov08d10_mode *mode;
+	s32 vblank_def, h_blank;
+	s64 pixel_rate;
+
+	mode = v4l2_find_nearest_size(ov08d10->priv_lane->sp_modes,
+				      ov08d10->modes_size,
+				      width, height, fmt->format.width,
+				      fmt->format.height);
+
+	mutex_lock(&ov08d10->mutex);
+	ov08d10_update_pad_format(ov08d10, mode, &fmt->format);
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		*v4l2_subdev_get_try_format(sd, sd_state, fmt->pad) =
+								fmt->format;
+	} else {
+		ov08d10->cur_mode = mode;
+		__v4l2_ctrl_s_ctrl(ov08d10->link_freq, mode->link_freq_index);
+		pixel_rate = to_rate(ov08d10->priv_lane->link_freq_menu,
+				     mode->link_freq_index,
+				     ov08d10->cur_mode->data_lanes);
+		__v4l2_ctrl_s_ctrl_int64(ov08d10->pixel_rate, pixel_rate);
+
+		/* Update limits and set FPS to default */
+		vblank_def = mode->vts_def - mode->height;
+		__v4l2_ctrl_modify_range(ov08d10->vblank,
+					 mode->vts_min - mode->height,
+					 OV08D10_VTS_MAX - mode->height, 1,
+					 vblank_def);
+		__v4l2_ctrl_s_ctrl(ov08d10->vblank, vblank_def);
+		h_blank = to_pixels_per_line(ov08d10->priv_lane->link_freq_menu,
+					     mode->hts,
+					     mode->link_freq_index,
+					     ov08d10->cur_mode->data_lanes)
+					     - mode->width;
+		__v4l2_ctrl_modify_range(ov08d10->hblank, h_blank, h_blank, 1,
+					 h_blank);
+	}
+
+	mutex_unlock(&ov08d10->mutex);
+
+	return 0;
+}
+
+static int ov08d10_get_format(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_format *fmt)
+{
+	struct ov08d10 *ov08d10 = to_ov08d10(sd);
+
+	mutex_lock(&ov08d10->mutex);
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
+		fmt->format = *v4l2_subdev_get_try_format(&ov08d10->sd,
+							  sd_state,
+							  fmt->pad);
+	else
+		ov08d10_update_pad_format(ov08d10, ov08d10->cur_mode,
+					  &fmt->format);
+
+	mutex_unlock(&ov08d10->mutex);
+
+	return 0;
+}
+
+static int ov08d10_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct ov08d10 *ov08d10 = to_ov08d10(sd);
+
+	if (code->index > 0)
+		return -EINVAL;
+
+	mutex_lock(&ov08d10->mutex);
+	code->code = ov08d10_get_format_code(ov08d10);
+	mutex_unlock(&ov08d10->mutex);
+
+	return 0;
+}
+
+static int ov08d10_enum_frame_size(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct ov08d10 *ov08d10 = to_ov08d10(sd);
+
+	if (fse->index >= ov08d10->modes_size)
+		return -EINVAL;
+
+	mutex_lock(&ov08d10->mutex);
+	if (fse->code != ov08d10_get_format_code(ov08d10)) {
+		mutex_unlock(&ov08d10->mutex);
+		return -EINVAL;
+	}
+	mutex_unlock(&ov08d10->mutex);
+
+	fse->min_width = ov08d10->priv_lane->sp_modes[fse->index].width;
+	fse->max_width = fse->min_width;
+	fse->min_height = ov08d10->priv_lane->sp_modes[fse->index].height;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static int ov08d10_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct ov08d10 *ov08d10 = to_ov08d10(sd);
+
+	mutex_lock(&ov08d10->mutex);
+	ov08d10_update_pad_format(ov08d10, &ov08d10->priv_lane->sp_modes[0],
+				  v4l2_subdev_get_try_format(sd, fh->state, 0));
+	mutex_unlock(&ov08d10->mutex);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops ov08d10_video_ops = {
+	.s_stream = ov08d10_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops ov08d10_pad_ops = {
+	.set_fmt = ov08d10_set_format,
+	.get_fmt = ov08d10_get_format,
+	.enum_mbus_code = ov08d10_enum_mbus_code,
+	.enum_frame_size = ov08d10_enum_frame_size,
+};
+
+static const struct v4l2_subdev_ops ov08d10_subdev_ops = {
+	.video = &ov08d10_video_ops,
+	.pad = &ov08d10_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops ov08d10_internal_ops = {
+	.open = ov08d10_open,
+};
+
+static int ov08d10_identify_module(struct ov08d10 *ov08d10)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov08d10->sd);
+	u32 val;
+	u16 chip_id;
+	int ret;
+
+	/* System control registers */
+	ret = i2c_smbus_write_byte_data(client, OV08D10_REG_PAGE, 0x00);
+	if (ret < 0)
+		return ret;
+
+	/* Validate the chip ID */
+	ret = i2c_smbus_read_byte_data(client, OV08D10_REG_CHIP_ID_0);
+	if (ret < 0)
+		return ret;
+
+	val = ret << 8;
+
+	ret = i2c_smbus_read_byte_data(client, OV08D10_REG_CHIP_ID_1);
+	if (ret < 0)
+		return ret;
+
+	chip_id = val | ret;
+
+	if ((chip_id & OV08D10_ID_MASK) != OV08D10_CHIP_ID) {
+		dev_err(&client->dev, "unexpected sensor id(0x%04x)\n",
+			chip_id);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int ov08d10_get_hwcfg(struct ov08d10 *ov08d10, struct device *dev)
+{
+	struct fwnode_handle *ep;
+	struct fwnode_handle *fwnode = dev_fwnode(dev);
+	struct v4l2_fwnode_endpoint bus_cfg = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY
+	};
+	u32 xvclk_rate;
+	unsigned int i, j;
+	int ret;
+
+	if (!fwnode)
+		return -ENXIO;
+
+	ret = fwnode_property_read_u32(fwnode, "clock-frequency", &xvclk_rate);
+	if (ret)
+		return ret;
+
+	if (xvclk_rate != OV08D10_XVCLK_19_2)
+		dev_warn(dev, "external clock rate %u is unsupported",
+			 xvclk_rate);
+
+	ep = fwnode_graph_get_next_endpoint(fwnode, NULL);
+	if (!ep)
+		return -ENXIO;
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(ep, &bus_cfg);
+	fwnode_handle_put(ep);
+	if (ret)
+		return ret;
+
+	/* Get number of data lanes */
+	if (bus_cfg.bus.mipi_csi2.num_data_lanes != 2) {
+		dev_err(dev, "number of CSI2 data lanes %d is not supported",
+			bus_cfg.bus.mipi_csi2.num_data_lanes);
+		ret = -EINVAL;
+		goto check_hwcfg_error;
+	}
+
+	dev_dbg(dev, "Using %u data lanes\n", ov08d10->cur_mode->data_lanes);
+
+	ov08d10->priv_lane = &lane_cfg_2;
+	ov08d10->modes_size = ov08d10_modes_num(ov08d10);
+
+	if (!bus_cfg.nr_of_link_frequencies) {
+		dev_err(dev, "no link frequencies defined");
+		ret = -EINVAL;
+		goto check_hwcfg_error;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(ov08d10->priv_lane->link_freq_menu); i++) {
+		for (j = 0; j < bus_cfg.nr_of_link_frequencies; j++) {
+			if (ov08d10->priv_lane->link_freq_menu[i] ==
+			    bus_cfg.link_frequencies[j])
+				break;
+		}
+
+		if (j == bus_cfg.nr_of_link_frequencies) {
+			dev_err(dev, "no link frequency %lld supported",
+				ov08d10->priv_lane->link_freq_menu[i]);
+			ret = -EINVAL;
+			goto check_hwcfg_error;
+		}
+	}
+
+check_hwcfg_error:
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+
+	return ret;
+}
+
+static void ov08d10_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov08d10 *ov08d10 = to_ov08d10(sd);
+
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+	pm_runtime_disable(&client->dev);
+	mutex_destroy(&ov08d10->mutex);
+}
+
+static int ov08d10_probe(struct i2c_client *client)
+{
+	struct ov08d10 *ov08d10;
+	int ret;
+
+	ov08d10 = devm_kzalloc(&client->dev, sizeof(*ov08d10), GFP_KERNEL);
+	if (!ov08d10)
+		return -ENOMEM;
+
+	ret = ov08d10_get_hwcfg(ov08d10, &client->dev);
+	if (ret) {
+		dev_err(&client->dev, "failed to get HW configuration: %d",
+			ret);
+		return ret;
+	}
+
+	v4l2_i2c_subdev_init(&ov08d10->sd, client, &ov08d10_subdev_ops);
+
+	ret = ov08d10_identify_module(ov08d10);
+	if (ret) {
+		dev_err(&client->dev, "failed to find sensor: %d", ret);
+		return ret;
+	}
+
+	mutex_init(&ov08d10->mutex);
+	ov08d10->cur_mode = &ov08d10->priv_lane->sp_modes[0];
+	ret = ov08d10_init_controls(ov08d10);
+	if (ret) {
+		dev_err(&client->dev, "failed to init controls: %d", ret);
+		goto probe_error_v4l2_ctrl_handler_free;
+	}
+
+	ov08d10->sd.internal_ops = &ov08d10_internal_ops;
+	ov08d10->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	ov08d10->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ov08d10->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&ov08d10->sd.entity, 1, &ov08d10->pad);
+	if (ret) {
+		dev_err(&client->dev, "failed to init entity pads: %d", ret);
+		goto probe_error_v4l2_ctrl_handler_free;
+	}
+
+	ret = v4l2_async_register_subdev_sensor(&ov08d10->sd);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed to register V4L2 subdev: %d",
+			ret);
+		goto probe_error_media_entity_cleanup;
+	}
+
+	/*
+	 * Device is already turned on by i2c-core with ACPI domain PM.
+	 * Enable runtime PM and turn off the device.
+	 */
+	pm_runtime_set_active(&client->dev);
+	pm_runtime_enable(&client->dev);
+	pm_runtime_idle(&client->dev);
+
+	return 0;
+
+probe_error_media_entity_cleanup:
+	media_entity_cleanup(&ov08d10->sd.entity);
+
+probe_error_v4l2_ctrl_handler_free:
+	v4l2_ctrl_handler_free(ov08d10->sd.ctrl_handler);
+	mutex_destroy(&ov08d10->mutex);
+
+	return ret;
+}
+
+static const struct dev_pm_ops ov08d10_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(ov08d10_suspend, ov08d10_resume)
+};
+
+#ifdef CONFIG_ACPI
+static const struct acpi_device_id ov08d10_acpi_ids[] = {
+	{ "OVTI08D1" },
+	{ /* sentinel */ }
+};
+
+MODULE_DEVICE_TABLE(acpi, ov08d10_acpi_ids);
+#endif
+
+static struct i2c_driver ov08d10_i2c_driver = {
+	.driver = {
+		.name = "ov08d10",
+		.pm = &ov08d10_pm_ops,
+		.acpi_match_table = ACPI_PTR(ov08d10_acpi_ids),
+	},
+	.probe = ov08d10_probe,
+	.remove = ov08d10_remove,
+};
+
+module_i2c_driver(ov08d10_i2c_driver);
+
+MODULE_AUTHOR("Su, Jimmy <jimmy.su@intel.com>");
+MODULE_DESCRIPTION("OmniVision ov08d10 sensor driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/ov08x40.c b/drivers/media/i2c/ov08x40.c
new file mode 100644
index 000000000..637da4df6
--- /dev/null
+++ b/drivers/media/i2c/ov08x40.c
@@ -0,0 +1,3325 @@
+// SPDX-License-Identifier: GPL-2.0
+// Copyright (c) 2022 Intel Corporation.
+
+#include <linux/acpi.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/pm_runtime.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+
+#define OV08X40_REG_VALUE_08BIT		1
+#define OV08X40_REG_VALUE_16BIT		2
+#define OV08X40_REG_VALUE_24BIT		3
+
+#define OV08X40_REG_MODE_SELECT		0x0100
+#define OV08X40_MODE_STANDBY		0x00
+#define OV08X40_MODE_STREAMING		0x01
+
+#define OV08X40_REG_AO_STANDBY		0x1000
+#define OV08X40_AO_STREAMING		0x04
+
+#define OV08X40_REG_MS_SELECT		0x1001
+#define OV08X40_MS_STANDBY			0x00
+#define OV08X40_MS_STREAMING		0x04
+
+#define OV08X40_REG_SOFTWARE_RST	0x0103
+#define OV08X40_SOFTWARE_RST		0x01
+
+/* Chip ID */
+#define OV08X40_REG_CHIP_ID		0x300a
+#define OV08X40_CHIP_ID			0x560858
+
+/* V_TIMING internal */
+#define OV08X40_REG_VTS			0x380e
+#define OV08X40_VTS_30FPS		0x1388
+#define OV08X40_VTS_BIN_30FPS		0x115c
+#define OV08X40_VTS_MAX			0x7fff
+
+/* H TIMING internal */
+#define OV08X40_REG_HTS			0x380c
+#define OV08X40_HTS_30FPS		0x0280
+
+/* Exposure control */
+#define OV08X40_REG_EXPOSURE		0x3500
+#define OV08X40_EXPOSURE_MAX_MARGIN 31
+#define OV08X40_EXPOSURE_MIN		1
+#define OV08X40_EXPOSURE_STEP		1
+#define OV08X40_EXPOSURE_DEFAULT	0x40
+
+/* Short Exposure control */
+#define OV08X40_REG_SHORT_EXPOSURE	0x3540
+
+/* Analog gain control */
+#define OV08X40_REG_ANALOG_GAIN		0x3508
+#define OV08X40_ANA_GAIN_MIN		0x80
+#define OV08X40_ANA_GAIN_MAX		0x07c0
+#define OV08X40_ANA_GAIN_STEP		1
+#define OV08X40_ANA_GAIN_DEFAULT	0x80
+
+/* Digital gain control */
+#define OV08X40_REG_DGTL_GAIN_H		0x350a
+#define OV08X40_REG_DGTL_GAIN_M		0x350b
+#define OV08X40_REG_DGTL_GAIN_L		0x350c
+
+#define OV08X40_DGTL_GAIN_MIN		1024	     /* Min = 1 X */
+#define OV08X40_DGTL_GAIN_MAX		(4096 - 1)   /* Max = 4 X */
+#define OV08X40_DGTL_GAIN_DEFAULT	2560	     /* Default gain = 2.5 X */
+#define OV08X40_DGTL_GAIN_STEP		1            /* Each step = 1/1024 */
+
+#define OV08X40_DGTL_GAIN_L_SHIFT	6
+#define OV08X40_DGTL_GAIN_L_MASK	0x3
+#define OV08X40_DGTL_GAIN_M_SHIFT	2
+#define OV08X40_DGTL_GAIN_M_MASK	0xff
+#define OV08X40_DGTL_GAIN_H_SHIFT	10
+#define OV08X40_DGTL_GAIN_H_MASK	0x1F
+
+/* Test Pattern Control */
+#define OV08X40_REG_TEST_PATTERN	0x50C1
+#define OV08X40_REG_ISP             0x5000
+#define OV08X40_REG_SHORT_TEST_PATTERN  0x53C1
+#define OV08X40_TEST_PATTERN_ENABLE	BIT(0)
+#define OV08X40_TEST_PATTERN_MASK	0xcf
+#define OV08X40_TEST_PATTERN_BAR_SHIFT	4
+
+/* Flip Control */
+#define OV08X40_REG_VFLIP		0x3820
+#define OV08X40_REG_MIRROR		0x3821
+
+/* Horizontal Window Offset */
+#define OV08X40_REG_H_WIN_OFFSET	0x3811
+
+/* Vertical Window Offset */
+#define OV08X40_REG_V_WIN_OFFSET	0x3813
+
+enum {
+	OV08X40_LINK_FREQ_400MHZ_INDEX,
+};
+
+struct ov08x40_reg {
+	u16 address;
+	u8 val;
+};
+
+struct ov08x40_reg_list {
+	u32 num_of_regs;
+	const struct ov08x40_reg *regs;
+};
+
+/* Link frequency config */
+struct ov08x40_link_freq_config {
+	/* registers for this link frequency */
+	struct ov08x40_reg_list reg_list;
+};
+
+/* Mode : resolution and related config&values */
+struct ov08x40_mode {
+	/* Frame width */
+	u32 width;
+	/* Frame height */
+	u32 height;
+
+	u32 lanes;
+	/* V-timing */
+	u32 vts_def;
+	u32 vts_min;
+
+	/* HTS */
+	u32 hts;
+
+	/* Index of Link frequency config to be used */
+	u32 link_freq_index;
+	/* Default register values */
+	struct ov08x40_reg_list reg_list;
+};
+
+static const struct ov08x40_reg mipi_data_rate_800mbps[] = {
+	{0x0103, 0x01},
+	{0x1000, 0x00},
+	{0x1601, 0xd0},
+	{0x1001, 0x04},
+	{0x5004, 0x53},
+	{0x5110, 0x00},
+	{0x5111, 0x14},
+	{0x5112, 0x01},
+	{0x5113, 0x7b},
+	{0x5114, 0x00},
+	{0x5152, 0xa3},
+	{0x5a52, 0x1f},
+	{0x5a1a, 0x0e},
+	{0x5a1b, 0x10},
+	{0x5a1f, 0x0e},
+	{0x5a27, 0x0e},
+	{0x6002, 0x2e},
+};
+
+static const struct ov08x40_reg mode_3856x2416_regs[] = {
+	{0x5000, 0x5d},
+	{0x5001, 0x20},
+	{0x5008, 0xb0},
+	{0x50c1, 0x00},
+	{0x53c1, 0x00},
+	{0x5f40, 0x00},
+	{0x5f41, 0x40},
+	{0x0300, 0x3a},
+	{0x0301, 0xc8},
+	{0x0302, 0x31},
+	{0x0303, 0x03},
+	{0x0304, 0x01},
+	{0x0305, 0xa1},
+	{0x0306, 0x04},
+	{0x0307, 0x01},
+	{0x0308, 0x03},
+	{0x0309, 0x03},
+	{0x0310, 0x0a},
+	{0x0311, 0x02},
+	{0x0312, 0x01},
+	{0x0313, 0x08},
+	{0x0314, 0x66},
+	{0x0315, 0x00},
+	{0x0316, 0x34},
+	{0x0320, 0x02},
+	{0x0321, 0x03},
+	{0x0323, 0x05},
+	{0x0324, 0x01},
+	{0x0325, 0xb8},
+	{0x0326, 0x4a},
+	{0x0327, 0x04},
+	{0x0329, 0x00},
+	{0x032a, 0x05},
+	{0x032b, 0x00},
+	{0x032c, 0x00},
+	{0x032d, 0x00},
+	{0x032e, 0x02},
+	{0x032f, 0xa0},
+	{0x0350, 0x00},
+	{0x0360, 0x01},
+	{0x1216, 0x60},
+	{0x1217, 0x5b},
+	{0x1218, 0x00},
+	{0x1220, 0x24},
+	{0x198a, 0x00},
+	{0x198b, 0x01},
+	{0x198e, 0x00},
+	{0x198f, 0x01},
+	{0x3009, 0x04},
+	{0x3012, 0x41},
+	{0x3015, 0x00},
+	{0x3016, 0xb0},
+	{0x3017, 0xf0},
+	{0x3018, 0xf0},
+	{0x3019, 0xd2},
+	{0x301a, 0xb0},
+	{0x301c, 0x81},
+	{0x301d, 0x02},
+	{0x301e, 0x80},
+	{0x3022, 0xf0},
+	{0x3025, 0x89},
+	{0x3030, 0x03},
+	{0x3044, 0xc2},
+	{0x3050, 0x35},
+	{0x3051, 0x60},
+	{0x3052, 0x25},
+	{0x3053, 0x00},
+	{0x3054, 0x00},
+	{0x3055, 0x02},
+	{0x3056, 0x80},
+	{0x3057, 0x80},
+	{0x3058, 0x80},
+	{0x3059, 0x00},
+	{0x3107, 0x86},
+	{0x3400, 0x1c},
+	{0x3401, 0x80},
+	{0x3402, 0x8c},
+	{0x3419, 0x13},
+	{0x341a, 0x89},
+	{0x341b, 0x30},
+	{0x3420, 0x00},
+	{0x3421, 0x00},
+	{0x3422, 0x00},
+	{0x3423, 0x00},
+	{0x3424, 0x00},
+	{0x3425, 0x00},
+	{0x3426, 0x00},
+	{0x3427, 0x00},
+	{0x3428, 0x0f},
+	{0x3429, 0x00},
+	{0x342a, 0x00},
+	{0x342b, 0x00},
+	{0x342c, 0x00},
+	{0x342d, 0x00},
+	{0x342e, 0x00},
+	{0x342f, 0x11},
+	{0x3430, 0x11},
+	{0x3431, 0x10},
+	{0x3432, 0x00},
+	{0x3433, 0x00},
+	{0x3434, 0x00},
+	{0x3435, 0x00},
+	{0x3436, 0x00},
+	{0x3437, 0x00},
+	{0x3442, 0x02},
+	{0x3443, 0x02},
+	{0x3444, 0x07},
+	{0x3450, 0x00},
+	{0x3451, 0x00},
+	{0x3452, 0x18},
+	{0x3453, 0x18},
+	{0x3454, 0x00},
+	{0x3455, 0x80},
+	{0x3456, 0x08},
+	{0x3500, 0x00},
+	{0x3501, 0x02},
+	{0x3502, 0x00},
+	{0x3504, 0x4c},
+	{0x3506, 0x30},
+	{0x3507, 0x00},
+	{0x3508, 0x01},
+	{0x3509, 0x00},
+	{0x350a, 0x01},
+	{0x350b, 0x00},
+	{0x350c, 0x00},
+	{0x3540, 0x00},
+	{0x3541, 0x01},
+	{0x3542, 0x00},
+	{0x3544, 0x4c},
+	{0x3546, 0x30},
+	{0x3547, 0x00},
+	{0x3548, 0x01},
+	{0x3549, 0x00},
+	{0x354a, 0x01},
+	{0x354b, 0x00},
+	{0x354c, 0x00},
+	{0x3688, 0x02},
+	{0x368a, 0x2e},
+	{0x368e, 0x71},
+	{0x3696, 0xd1},
+	{0x3699, 0x00},
+	{0x369a, 0x00},
+	{0x36a4, 0x00},
+	{0x36a6, 0x00},
+	{0x3711, 0x00},
+	{0x3712, 0x51},
+	{0x3713, 0x00},
+	{0x3714, 0x24},
+	{0x3716, 0x00},
+	{0x3718, 0x07},
+	{0x371a, 0x1c},
+	{0x371b, 0x00},
+	{0x3720, 0x08},
+	{0x3725, 0x32},
+	{0x3727, 0x05},
+	{0x3760, 0x02},
+	{0x3761, 0x17},
+	{0x3762, 0x02},
+	{0x3763, 0x02},
+	{0x3764, 0x02},
+	{0x3765, 0x2c},
+	{0x3766, 0x04},
+	{0x3767, 0x2c},
+	{0x3768, 0x02},
+	{0x3769, 0x00},
+	{0x376b, 0x20},
+	{0x376e, 0x03},
+	{0x37b0, 0x00},
+	{0x37b1, 0xab},
+	{0x37b2, 0x01},
+	{0x37b3, 0x82},
+	{0x37b4, 0x00},
+	{0x37b5, 0xe4},
+	{0x37b6, 0x01},
+	{0x37b7, 0xee},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x00},
+	{0x3804, 0x0f},
+	{0x3805, 0x1f},
+	{0x3806, 0x09},
+	{0x3807, 0x7f},
+	{0x3808, 0x0f},
+	{0x3809, 0x10},
+	{0x380a, 0x09},
+	{0x380b, 0x70},
+	{0x380c, 0x02},
+	{0x380d, 0x80},
+	{0x380e, 0x13},
+	{0x380f, 0x88},
+	{0x3810, 0x00},
+	{0x3811, 0x08},
+	{0x3812, 0x00},
+	{0x3813, 0x07},
+	{0x3814, 0x11},
+	{0x3815, 0x11},
+	{0x3820, 0x00},
+	{0x3821, 0x04},
+	{0x3822, 0x00},
+	{0x3823, 0x04},
+	{0x3828, 0x0f},
+	{0x382a, 0x80},
+	{0x382e, 0x41},
+	{0x3837, 0x08},
+	{0x383a, 0x81},
+	{0x383b, 0x81},
+	{0x383c, 0x11},
+	{0x383d, 0x11},
+	{0x383e, 0x00},
+	{0x383f, 0x38},
+	{0x3840, 0x00},
+	{0x3847, 0x00},
+	{0x384a, 0x00},
+	{0x384c, 0x02},
+	{0x384d, 0x80},
+	{0x3856, 0x50},
+	{0x3857, 0x30},
+	{0x3858, 0x80},
+	{0x3859, 0x40},
+	{0x3860, 0x00},
+	{0x3888, 0x00},
+	{0x3889, 0x00},
+	{0x388a, 0x00},
+	{0x388b, 0x00},
+	{0x388c, 0x00},
+	{0x388d, 0x00},
+	{0x388e, 0x00},
+	{0x388f, 0x00},
+	{0x3894, 0x00},
+	{0x3895, 0x00},
+	{0x3c84, 0x00},
+	{0x3d85, 0x8b},
+	{0x3daa, 0x80},
+	{0x3dab, 0x14},
+	{0x3dac, 0x80},
+	{0x3dad, 0xc8},
+	{0x3dae, 0x81},
+	{0x3daf, 0x7b},
+	{0x3f00, 0x10},
+	{0x3f01, 0x11},
+	{0x3f06, 0x0d},
+	{0x3f07, 0x0b},
+	{0x3f08, 0x0d},
+	{0x3f09, 0x0b},
+	{0x3f0a, 0x01},
+	{0x3f0b, 0x11},
+	{0x3f0c, 0x33},
+	{0x4001, 0x07},
+	{0x4007, 0x20},
+	{0x4008, 0x00},
+	{0x4009, 0x05},
+	{0x400a, 0x00},
+	{0x400b, 0x08},
+	{0x400c, 0x00},
+	{0x400d, 0x08},
+	{0x400e, 0x14},
+	{0x4010, 0xf4},
+	{0x4011, 0x03},
+	{0x4012, 0x55},
+	{0x4015, 0x00},
+	{0x4016, 0x2d},
+	{0x4017, 0x00},
+	{0x4018, 0x0f},
+	{0x401b, 0x08},
+	{0x401c, 0x00},
+	{0x401d, 0x10},
+	{0x401e, 0x02},
+	{0x401f, 0x00},
+	{0x4050, 0x06},
+	{0x4051, 0xff},
+	{0x4052, 0xff},
+	{0x4053, 0xff},
+	{0x4054, 0xff},
+	{0x4055, 0xff},
+	{0x4056, 0xff},
+	{0x4057, 0x7f},
+	{0x4058, 0x00},
+	{0x4059, 0x00},
+	{0x405a, 0x00},
+	{0x405b, 0x00},
+	{0x405c, 0x07},
+	{0x405d, 0xff},
+	{0x405e, 0x07},
+	{0x405f, 0xff},
+	{0x4080, 0x78},
+	{0x4081, 0x78},
+	{0x4082, 0x78},
+	{0x4083, 0x78},
+	{0x4019, 0x00},
+	{0x401a, 0x40},
+	{0x4020, 0x04},
+	{0x4021, 0x00},
+	{0x4022, 0x04},
+	{0x4023, 0x00},
+	{0x4024, 0x04},
+	{0x4025, 0x00},
+	{0x4026, 0x04},
+	{0x4027, 0x00},
+	{0x4030, 0x00},
+	{0x4031, 0x00},
+	{0x4032, 0x00},
+	{0x4033, 0x00},
+	{0x4034, 0x00},
+	{0x4035, 0x00},
+	{0x4036, 0x00},
+	{0x4037, 0x00},
+	{0x4040, 0x00},
+	{0x4041, 0x80},
+	{0x4042, 0x00},
+	{0x4043, 0x80},
+	{0x4044, 0x00},
+	{0x4045, 0x80},
+	{0x4046, 0x00},
+	{0x4047, 0x80},
+	{0x4060, 0x00},
+	{0x4061, 0x00},
+	{0x4062, 0x00},
+	{0x4063, 0x00},
+	{0x4064, 0x00},
+	{0x4065, 0x00},
+	{0x4066, 0x00},
+	{0x4067, 0x00},
+	{0x4068, 0x00},
+	{0x4069, 0x00},
+	{0x406a, 0x00},
+	{0x406b, 0x00},
+	{0x406c, 0x00},
+	{0x406d, 0x00},
+	{0x406e, 0x00},
+	{0x406f, 0x00},
+	{0x4070, 0x00},
+	{0x4071, 0x00},
+	{0x4072, 0x00},
+	{0x4073, 0x00},
+	{0x4074, 0x00},
+	{0x4075, 0x00},
+	{0x4076, 0x00},
+	{0x4077, 0x00},
+	{0x4078, 0x00},
+	{0x4079, 0x00},
+	{0x407a, 0x00},
+	{0x407b, 0x00},
+	{0x407c, 0x00},
+	{0x407d, 0x00},
+	{0x407e, 0x00},
+	{0x407f, 0x00},
+	{0x40e0, 0x00},
+	{0x40e1, 0x00},
+	{0x40e2, 0x00},
+	{0x40e3, 0x00},
+	{0x40e4, 0x00},
+	{0x40e5, 0x00},
+	{0x40e6, 0x00},
+	{0x40e7, 0x00},
+	{0x40e8, 0x00},
+	{0x40e9, 0x80},
+	{0x40ea, 0x00},
+	{0x40eb, 0x80},
+	{0x40ec, 0x00},
+	{0x40ed, 0x80},
+	{0x40ee, 0x00},
+	{0x40ef, 0x80},
+	{0x40f0, 0x02},
+	{0x40f1, 0x04},
+	{0x4300, 0x00},
+	{0x4301, 0x00},
+	{0x4302, 0x00},
+	{0x4303, 0x00},
+	{0x4304, 0x00},
+	{0x4305, 0x00},
+	{0x4306, 0x00},
+	{0x4307, 0x00},
+	{0x4308, 0x00},
+	{0x4309, 0x00},
+	{0x430a, 0x00},
+	{0x430b, 0xff},
+	{0x430c, 0xff},
+	{0x430d, 0x00},
+	{0x430e, 0x00},
+	{0x4315, 0x00},
+	{0x4316, 0x00},
+	{0x4317, 0x00},
+	{0x4318, 0x00},
+	{0x4319, 0x00},
+	{0x431a, 0x00},
+	{0x431b, 0x00},
+	{0x431c, 0x00},
+	{0x4500, 0x07},
+	{0x4501, 0x00},
+	{0x4502, 0x00},
+	{0x4503, 0x0f},
+	{0x4504, 0x80},
+	{0x4506, 0x01},
+	{0x4509, 0x05},
+	{0x450c, 0x00},
+	{0x450d, 0x20},
+	{0x450e, 0x00},
+	{0x450f, 0x00},
+	{0x4510, 0x00},
+	{0x4523, 0x00},
+	{0x4526, 0x00},
+	{0x4542, 0x00},
+	{0x4543, 0x00},
+	{0x4544, 0x00},
+	{0x4545, 0x00},
+	{0x4546, 0x00},
+	{0x4547, 0x10},
+	{0x4602, 0x00},
+	{0x4603, 0x15},
+	{0x460b, 0x07},
+	{0x4680, 0x11},
+	{0x4686, 0x00},
+	{0x4687, 0x00},
+	{0x4700, 0x00},
+	{0x4800, 0x64},
+	{0x4806, 0x40},
+	{0x480b, 0x10},
+	{0x480c, 0x80},
+	{0x480f, 0x32},
+	{0x4813, 0xe4},
+	{0x4837, 0x14},
+	{0x4850, 0x42},
+	{0x4884, 0x04},
+	{0x4c00, 0xf8},
+	{0x4c01, 0x44},
+	{0x4c03, 0x00},
+	{0x4d00, 0x00},
+	{0x4d01, 0x16},
+	{0x4d04, 0x10},
+	{0x4d05, 0x00},
+	{0x4d06, 0x0c},
+	{0x4d07, 0x00},
+	{0x3d84, 0x04},
+	{0x3680, 0xa4},
+	{0x3682, 0x80},
+	{0x3601, 0x40},
+	{0x3602, 0x90},
+	{0x3608, 0x0a},
+	{0x3938, 0x09},
+	{0x3a74, 0x84},
+	{0x3a99, 0x84},
+	{0x3ab9, 0xa6},
+	{0x3aba, 0xba},
+	{0x3b12, 0x84},
+	{0x3b14, 0xbb},
+	{0x3b15, 0xbf},
+	{0x3a29, 0x26},
+	{0x3a1f, 0x8a},
+	{0x3a22, 0x91},
+	{0x3a25, 0x96},
+	{0x3a28, 0xb4},
+	{0x3a2b, 0xba},
+	{0x3a2e, 0xbf},
+	{0x3a31, 0xc1},
+	{0x3a20, 0x00},
+	{0x3939, 0x9d},
+	{0x3902, 0x0e},
+	{0x3903, 0x0e},
+	{0x3904, 0x0e},
+	{0x3905, 0x0e},
+	{0x3906, 0x07},
+	{0x3907, 0x0d},
+	{0x3908, 0x11},
+	{0x3909, 0x12},
+	{0x360f, 0x99},
+	{0x390c, 0x33},
+	{0x390d, 0x66},
+	{0x390e, 0xaa},
+	{0x3911, 0x90},
+	{0x3913, 0x90},
+	{0x3915, 0x90},
+	{0x3917, 0x90},
+	{0x3b3f, 0x9d},
+	{0x3b45, 0x9d},
+	{0x3b1b, 0xc9},
+	{0x3b21, 0xc9},
+	{0x3440, 0xa4},
+	{0x3a23, 0x15},
+	{0x3a26, 0x1d},
+	{0x3a2c, 0x4a},
+	{0x3a2f, 0x18},
+	{0x3a32, 0x55},
+	{0x3b0a, 0x01},
+	{0x3b0b, 0x00},
+	{0x3b0e, 0x01},
+	{0x3b0f, 0x00},
+	{0x392c, 0x02},
+	{0x392d, 0x02},
+	{0x392e, 0x04},
+	{0x392f, 0x03},
+	{0x3930, 0x08},
+	{0x3931, 0x07},
+	{0x3932, 0x10},
+	{0x3933, 0x0c},
+	{0x3609, 0x08},
+	{0x3921, 0x0f},
+	{0x3928, 0x15},
+	{0x3929, 0x2a},
+	{0x392a, 0x54},
+	{0x392b, 0xa8},
+	{0x3426, 0x10},
+	{0x3407, 0x01},
+	{0x3404, 0x01},
+	{0x3500, 0x00},
+	{0x3501, 0x10},
+	{0x3502, 0x10},
+	{0x3508, 0x0f},
+	{0x3509, 0x80},
+	{0x5a80, 0x75},
+	{0x5a81, 0x75},
+	{0x5a82, 0x75},
+	{0x5a83, 0x75},
+	{0x5a84, 0x75},
+	{0x5a85, 0x75},
+	{0x5a86, 0x75},
+	{0x5a87, 0x75},
+	{0x5a88, 0x75},
+	{0x5a89, 0x75},
+	{0x5a8a, 0x75},
+	{0x5a8b, 0x75},
+	{0x5a8c, 0x75},
+	{0x5a8d, 0x75},
+	{0x5a8e, 0x75},
+	{0x5a8f, 0x75},
+	{0x5a90, 0x75},
+	{0x5a91, 0x75},
+	{0x5a92, 0x75},
+	{0x5a93, 0x75},
+	{0x5a94, 0x75},
+	{0x5a95, 0x75},
+	{0x5a96, 0x75},
+	{0x5a97, 0x75},
+	{0x5a98, 0x75},
+	{0x5a99, 0x75},
+	{0x5a9a, 0x75},
+	{0x5a9b, 0x75},
+	{0x5a9c, 0x75},
+	{0x5a9d, 0x75},
+	{0x5a9e, 0x75},
+	{0x5a9f, 0x75},
+	{0x5aa0, 0x75},
+	{0x5aa1, 0x75},
+	{0x5aa2, 0x75},
+	{0x5aa3, 0x75},
+	{0x5aa4, 0x75},
+	{0x5aa5, 0x75},
+	{0x5aa6, 0x75},
+	{0x5aa7, 0x75},
+	{0x5aa8, 0x75},
+	{0x5aa9, 0x75},
+	{0x5aaa, 0x75},
+	{0x5aab, 0x75},
+	{0x5aac, 0x75},
+	{0x5aad, 0x75},
+	{0x5aae, 0x75},
+	{0x5aaf, 0x75},
+	{0x5ab0, 0x75},
+	{0x5ab1, 0x75},
+	{0x5ab2, 0x75},
+	{0x5ab3, 0x75},
+	{0x5ab4, 0x75},
+	{0x5ab5, 0x75},
+	{0x5ab6, 0x75},
+	{0x5ab7, 0x75},
+	{0x5ab8, 0x75},
+	{0x5ab9, 0x75},
+	{0x5aba, 0x75},
+	{0x5abb, 0x75},
+	{0x5abc, 0x75},
+	{0x5abd, 0x75},
+	{0x5abe, 0x75},
+	{0x5abf, 0x75},
+	{0x5ac0, 0x75},
+	{0x5ac1, 0x75},
+	{0x5ac2, 0x75},
+	{0x5ac3, 0x75},
+	{0x5ac4, 0x75},
+	{0x5ac5, 0x75},
+	{0x5ac6, 0x75},
+	{0x5ac7, 0x75},
+	{0x5ac8, 0x75},
+	{0x5ac9, 0x75},
+	{0x5aca, 0x75},
+	{0x5acb, 0x75},
+	{0x5acc, 0x75},
+	{0x5acd, 0x75},
+	{0x5ace, 0x75},
+	{0x5acf, 0x75},
+	{0x5ad0, 0x75},
+	{0x5ad1, 0x75},
+	{0x5ad2, 0x75},
+	{0x5ad3, 0x75},
+	{0x5ad4, 0x75},
+	{0x5ad5, 0x75},
+	{0x5ad6, 0x75},
+	{0x5ad7, 0x75},
+	{0x5ad8, 0x75},
+	{0x5ad9, 0x75},
+	{0x5ada, 0x75},
+	{0x5adb, 0x75},
+	{0x5adc, 0x75},
+	{0x5add, 0x75},
+	{0x5ade, 0x75},
+	{0x5adf, 0x75},
+	{0x5ae0, 0x75},
+	{0x5ae1, 0x75},
+	{0x5ae2, 0x75},
+	{0x5ae3, 0x75},
+	{0x5ae4, 0x75},
+	{0x5ae5, 0x75},
+	{0x5ae6, 0x75},
+	{0x5ae7, 0x75},
+	{0x5ae8, 0x75},
+	{0x5ae9, 0x75},
+	{0x5aea, 0x75},
+	{0x5aeb, 0x75},
+	{0x5aec, 0x75},
+	{0x5aed, 0x75},
+	{0x5aee, 0x75},
+	{0x5aef, 0x75},
+	{0x5af0, 0x75},
+	{0x5af1, 0x75},
+	{0x5af2, 0x75},
+	{0x5af3, 0x75},
+	{0x5af4, 0x75},
+	{0x5af5, 0x75},
+	{0x5af6, 0x75},
+	{0x5af7, 0x75},
+	{0x5af8, 0x75},
+	{0x5af9, 0x75},
+	{0x5afa, 0x75},
+	{0x5afb, 0x75},
+	{0x5afc, 0x75},
+	{0x5afd, 0x75},
+	{0x5afe, 0x75},
+	{0x5aff, 0x75},
+	{0x5b00, 0x75},
+	{0x5b01, 0x75},
+	{0x5b02, 0x75},
+	{0x5b03, 0x75},
+	{0x5b04, 0x75},
+	{0x5b05, 0x75},
+	{0x5b06, 0x75},
+	{0x5b07, 0x75},
+	{0x5b08, 0x75},
+	{0x5b09, 0x75},
+	{0x5b0a, 0x75},
+	{0x5b0b, 0x75},
+	{0x5b0c, 0x75},
+	{0x5b0d, 0x75},
+	{0x5b0e, 0x75},
+	{0x5b0f, 0x75},
+	{0x5b10, 0x75},
+	{0x5b11, 0x75},
+	{0x5b12, 0x75},
+	{0x5b13, 0x75},
+	{0x5b14, 0x75},
+	{0x5b15, 0x75},
+	{0x5b16, 0x75},
+	{0x5b17, 0x75},
+	{0x5b18, 0x75},
+	{0x5b19, 0x75},
+	{0x5b1a, 0x75},
+	{0x5b1b, 0x75},
+	{0x5b1c, 0x75},
+	{0x5b1d, 0x75},
+	{0x5b1e, 0x75},
+	{0x5b1f, 0x75},
+	{0x5b20, 0x75},
+	{0x5b21, 0x75},
+	{0x5b22, 0x75},
+	{0x5b23, 0x75},
+	{0x5b24, 0x75},
+	{0x5b25, 0x75},
+	{0x5b26, 0x75},
+	{0x5b27, 0x75},
+	{0x5b28, 0x75},
+	{0x5b29, 0x75},
+	{0x5b2a, 0x75},
+	{0x5b2b, 0x75},
+	{0x5b2c, 0x75},
+	{0x5b2d, 0x75},
+	{0x5b2e, 0x75},
+	{0x5b2f, 0x75},
+	{0x5b30, 0x75},
+	{0x5b31, 0x75},
+	{0x5b32, 0x75},
+	{0x5b33, 0x75},
+	{0x5b34, 0x75},
+	{0x5b35, 0x75},
+	{0x5b36, 0x75},
+	{0x5b37, 0x75},
+	{0x5b38, 0x75},
+	{0x5b39, 0x75},
+	{0x5b3a, 0x75},
+	{0x5b3b, 0x75},
+	{0x5b3c, 0x75},
+	{0x5b3d, 0x75},
+	{0x5b3e, 0x75},
+	{0x5b3f, 0x75},
+	{0x5b40, 0x75},
+	{0x5b41, 0x75},
+	{0x5b42, 0x75},
+	{0x5b43, 0x75},
+	{0x5b44, 0x75},
+	{0x5b45, 0x75},
+	{0x5b46, 0x75},
+	{0x5b47, 0x75},
+	{0x5b48, 0x75},
+	{0x5b49, 0x75},
+	{0x5b4a, 0x75},
+	{0x5b4b, 0x75},
+	{0x5b4c, 0x75},
+	{0x5b4d, 0x75},
+	{0x5b4e, 0x75},
+	{0x5b4f, 0x75},
+	{0x5b50, 0x75},
+	{0x5b51, 0x75},
+	{0x5b52, 0x75},
+	{0x5b53, 0x75},
+	{0x5b54, 0x75},
+	{0x5b55, 0x75},
+	{0x5b56, 0x75},
+	{0x5b57, 0x75},
+	{0x5b58, 0x75},
+	{0x5b59, 0x75},
+	{0x5b5a, 0x75},
+	{0x5b5b, 0x75},
+	{0x5b5c, 0x75},
+	{0x5b5d, 0x75},
+	{0x5b5e, 0x75},
+	{0x5b5f, 0x75},
+	{0x5b60, 0x75},
+	{0x5b61, 0x75},
+	{0x5b62, 0x75},
+	{0x5b63, 0x75},
+	{0x5b64, 0x75},
+	{0x5b65, 0x75},
+	{0x5b66, 0x75},
+	{0x5b67, 0x75},
+	{0x5b68, 0x75},
+	{0x5b69, 0x75},
+	{0x5b6a, 0x75},
+	{0x5b6b, 0x75},
+	{0x5b6c, 0x75},
+	{0x5b6d, 0x75},
+	{0x5b6e, 0x75},
+	{0x5b6f, 0x75},
+	{0x5b70, 0x75},
+	{0x5b71, 0x75},
+	{0x5b72, 0x75},
+	{0x5b73, 0x75},
+	{0x5b74, 0x75},
+	{0x5b75, 0x75},
+	{0x5b76, 0x75},
+	{0x5b77, 0x75},
+	{0x5b78, 0x75},
+	{0x5b79, 0x75},
+	{0x5b7a, 0x75},
+	{0x5b7b, 0x75},
+	{0x5b7c, 0x75},
+	{0x5b7d, 0x75},
+	{0x5b7e, 0x75},
+	{0x5b7f, 0x75},
+	{0x5b80, 0x75},
+	{0x5b81, 0x75},
+	{0x5b82, 0x75},
+	{0x5b83, 0x75},
+	{0x5b84, 0x75},
+	{0x5b85, 0x75},
+	{0x5b86, 0x75},
+	{0x5b87, 0x75},
+	{0x5b88, 0x75},
+	{0x5b89, 0x75},
+	{0x5b8a, 0x75},
+	{0x5b8b, 0x75},
+	{0x5b8c, 0x75},
+	{0x5b8d, 0x75},
+	{0x5b8e, 0x75},
+	{0x5b8f, 0x75},
+	{0x5b90, 0x75},
+	{0x5b91, 0x75},
+	{0x5b92, 0x75},
+	{0x5b93, 0x75},
+	{0x5b94, 0x75},
+	{0x5b95, 0x75},
+	{0x5b96, 0x75},
+	{0x5b97, 0x75},
+	{0x5b98, 0x75},
+	{0x5b99, 0x75},
+	{0x5b9a, 0x75},
+	{0x5b9b, 0x75},
+	{0x5b9c, 0x75},
+	{0x5b9d, 0x75},
+	{0x5b9e, 0x75},
+	{0x5b9f, 0x75},
+	{0x5bc0, 0x75},
+	{0x5bc1, 0x75},
+	{0x5bc2, 0x75},
+	{0x5bc3, 0x75},
+	{0x5bc4, 0x75},
+	{0x5bc5, 0x75},
+	{0x5bc6, 0x75},
+	{0x5bc7, 0x75},
+	{0x5bc8, 0x75},
+	{0x5bc9, 0x75},
+	{0x5bca, 0x75},
+	{0x5bcb, 0x75},
+	{0x5bcc, 0x75},
+	{0x5bcd, 0x75},
+	{0x5bce, 0x75},
+	{0x5bcf, 0x75},
+	{0x5bd0, 0x75},
+	{0x5bd1, 0x75},
+	{0x5bd2, 0x75},
+	{0x5bd3, 0x75},
+	{0x5bd4, 0x75},
+	{0x5bd5, 0x75},
+	{0x5bd6, 0x75},
+	{0x5bd7, 0x75},
+	{0x5bd8, 0x75},
+	{0x5bd9, 0x75},
+	{0x5bda, 0x75},
+	{0x5bdb, 0x75},
+	{0x5bdc, 0x75},
+	{0x5bdd, 0x75},
+	{0x5bde, 0x75},
+	{0x5bdf, 0x75},
+	{0x5be0, 0x75},
+	{0x5be1, 0x75},
+	{0x5be2, 0x75},
+	{0x5be3, 0x75},
+	{0x5be4, 0x75},
+	{0x5be5, 0x75},
+	{0x5be6, 0x75},
+	{0x5be7, 0x75},
+	{0x5be8, 0x75},
+	{0x5be9, 0x75},
+	{0x5bea, 0x75},
+	{0x5beb, 0x75},
+	{0x5bec, 0x75},
+	{0x5bed, 0x75},
+	{0x5bee, 0x75},
+	{0x5bef, 0x75},
+	{0x5bf0, 0x75},
+	{0x5bf1, 0x75},
+	{0x5bf2, 0x75},
+	{0x5bf3, 0x75},
+	{0x5bf4, 0x75},
+	{0x5bf5, 0x75},
+	{0x5bf6, 0x75},
+	{0x5bf7, 0x75},
+	{0x5bf8, 0x75},
+	{0x5bf9, 0x75},
+	{0x5bfa, 0x75},
+	{0x5bfb, 0x75},
+	{0x5bfc, 0x75},
+	{0x5bfd, 0x75},
+	{0x5bfe, 0x75},
+	{0x5bff, 0x75},
+	{0x5c00, 0x75},
+	{0x5c01, 0x75},
+	{0x5c02, 0x75},
+	{0x5c03, 0x75},
+	{0x5c04, 0x75},
+	{0x5c05, 0x75},
+	{0x5c06, 0x75},
+	{0x5c07, 0x75},
+	{0x5c08, 0x75},
+	{0x5c09, 0x75},
+	{0x5c0a, 0x75},
+	{0x5c0b, 0x75},
+	{0x5c0c, 0x75},
+	{0x5c0d, 0x75},
+	{0x5c0e, 0x75},
+	{0x5c0f, 0x75},
+	{0x5c10, 0x75},
+	{0x5c11, 0x75},
+	{0x5c12, 0x75},
+	{0x5c13, 0x75},
+	{0x5c14, 0x75},
+	{0x5c15, 0x75},
+	{0x5c16, 0x75},
+	{0x5c17, 0x75},
+	{0x5c18, 0x75},
+	{0x5c19, 0x75},
+	{0x5c1a, 0x75},
+	{0x5c1b, 0x75},
+	{0x5c1c, 0x75},
+	{0x5c1d, 0x75},
+	{0x5c1e, 0x75},
+	{0x5c1f, 0x75},
+	{0x5c20, 0x75},
+	{0x5c21, 0x75},
+	{0x5c22, 0x75},
+	{0x5c23, 0x75},
+	{0x5c24, 0x75},
+	{0x5c25, 0x75},
+	{0x5c26, 0x75},
+	{0x5c27, 0x75},
+	{0x5c28, 0x75},
+	{0x5c29, 0x75},
+	{0x5c2a, 0x75},
+	{0x5c2b, 0x75},
+	{0x5c2c, 0x75},
+	{0x5c2d, 0x75},
+	{0x5c2e, 0x75},
+	{0x5c2f, 0x75},
+	{0x5c30, 0x75},
+	{0x5c31, 0x75},
+	{0x5c32, 0x75},
+	{0x5c33, 0x75},
+	{0x5c34, 0x75},
+	{0x5c35, 0x75},
+	{0x5c36, 0x75},
+	{0x5c37, 0x75},
+	{0x5c38, 0x75},
+	{0x5c39, 0x75},
+	{0x5c3a, 0x75},
+	{0x5c3b, 0x75},
+	{0x5c3c, 0x75},
+	{0x5c3d, 0x75},
+	{0x5c3e, 0x75},
+	{0x5c3f, 0x75},
+	{0x5c40, 0x75},
+	{0x5c41, 0x75},
+	{0x5c42, 0x75},
+	{0x5c43, 0x75},
+	{0x5c44, 0x75},
+	{0x5c45, 0x75},
+	{0x5c46, 0x75},
+	{0x5c47, 0x75},
+	{0x5c48, 0x75},
+	{0x5c49, 0x75},
+	{0x5c4a, 0x75},
+	{0x5c4b, 0x75},
+	{0x5c4c, 0x75},
+	{0x5c4d, 0x75},
+	{0x5c4e, 0x75},
+	{0x5c4f, 0x75},
+	{0x5c50, 0x75},
+	{0x5c51, 0x75},
+	{0x5c52, 0x75},
+	{0x5c53, 0x75},
+	{0x5c54, 0x75},
+	{0x5c55, 0x75},
+	{0x5c56, 0x75},
+	{0x5c57, 0x75},
+	{0x5c58, 0x75},
+	{0x5c59, 0x75},
+	{0x5c5a, 0x75},
+	{0x5c5b, 0x75},
+	{0x5c5c, 0x75},
+	{0x5c5d, 0x75},
+	{0x5c5e, 0x75},
+	{0x5c5f, 0x75},
+	{0x5c60, 0x75},
+	{0x5c61, 0x75},
+	{0x5c62, 0x75},
+	{0x5c63, 0x75},
+	{0x5c64, 0x75},
+	{0x5c65, 0x75},
+	{0x5c66, 0x75},
+	{0x5c67, 0x75},
+	{0x5c68, 0x75},
+	{0x5c69, 0x75},
+	{0x5c6a, 0x75},
+	{0x5c6b, 0x75},
+	{0x5c6c, 0x75},
+	{0x5c6d, 0x75},
+	{0x5c6e, 0x75},
+	{0x5c6f, 0x75},
+	{0x5c70, 0x75},
+	{0x5c71, 0x75},
+	{0x5c72, 0x75},
+	{0x5c73, 0x75},
+	{0x5c74, 0x75},
+	{0x5c75, 0x75},
+	{0x5c76, 0x75},
+	{0x5c77, 0x75},
+	{0x5c78, 0x75},
+	{0x5c79, 0x75},
+	{0x5c7a, 0x75},
+	{0x5c7b, 0x75},
+	{0x5c7c, 0x75},
+	{0x5c7d, 0x75},
+	{0x5c7e, 0x75},
+	{0x5c7f, 0x75},
+	{0x5c80, 0x75},
+	{0x5c81, 0x75},
+	{0x5c82, 0x75},
+	{0x5c83, 0x75},
+	{0x5c84, 0x75},
+	{0x5c85, 0x75},
+	{0x5c86, 0x75},
+	{0x5c87, 0x75},
+	{0x5c88, 0x75},
+	{0x5c89, 0x75},
+	{0x5c8a, 0x75},
+	{0x5c8b, 0x75},
+	{0x5c8c, 0x75},
+	{0x5c8d, 0x75},
+	{0x5c8e, 0x75},
+	{0x5c8f, 0x75},
+	{0x5c90, 0x75},
+	{0x5c91, 0x75},
+	{0x5c92, 0x75},
+	{0x5c93, 0x75},
+	{0x5c94, 0x75},
+	{0x5c95, 0x75},
+	{0x5c96, 0x75},
+	{0x5c97, 0x75},
+	{0x5c98, 0x75},
+	{0x5c99, 0x75},
+	{0x5c9a, 0x75},
+	{0x5c9b, 0x75},
+	{0x5c9c, 0x75},
+	{0x5c9d, 0x75},
+	{0x5c9e, 0x75},
+	{0x5c9f, 0x75},
+	{0x5ca0, 0x75},
+	{0x5ca1, 0x75},
+	{0x5ca2, 0x75},
+	{0x5ca3, 0x75},
+	{0x5ca4, 0x75},
+	{0x5ca5, 0x75},
+	{0x5ca6, 0x75},
+	{0x5ca7, 0x75},
+	{0x5ca8, 0x75},
+	{0x5ca9, 0x75},
+	{0x5caa, 0x75},
+	{0x5cab, 0x75},
+	{0x5cac, 0x75},
+	{0x5cad, 0x75},
+	{0x5cae, 0x75},
+	{0x5caf, 0x75},
+	{0x5cb0, 0x75},
+	{0x5cb1, 0x75},
+	{0x5cb2, 0x75},
+	{0x5cb3, 0x75},
+	{0x5cb4, 0x75},
+	{0x5cb5, 0x75},
+	{0x5cb6, 0x75},
+	{0x5cb7, 0x75},
+	{0x5cb8, 0x75},
+	{0x5cb9, 0x75},
+	{0x5cba, 0x75},
+	{0x5cbb, 0x75},
+	{0x5cbc, 0x75},
+	{0x5cbd, 0x75},
+	{0x5cbe, 0x75},
+	{0x5cbf, 0x75},
+	{0x5cc0, 0x75},
+	{0x5cc1, 0x75},
+	{0x5cc2, 0x75},
+	{0x5cc3, 0x75},
+	{0x5cc4, 0x75},
+	{0x5cc5, 0x75},
+	{0x5cc6, 0x75},
+	{0x5cc7, 0x75},
+	{0x5cc8, 0x75},
+	{0x5cc9, 0x75},
+	{0x5cca, 0x75},
+	{0x5ccb, 0x75},
+	{0x5ccc, 0x75},
+	{0x5ccd, 0x75},
+	{0x5cce, 0x75},
+	{0x5ccf, 0x75},
+	{0x5cd0, 0x75},
+	{0x5cd1, 0x75},
+	{0x5cd2, 0x75},
+	{0x5cd3, 0x75},
+	{0x5cd4, 0x75},
+	{0x5cd5, 0x75},
+	{0x5cd6, 0x75},
+	{0x5cd7, 0x75},
+	{0x5cd8, 0x75},
+	{0x5cd9, 0x75},
+	{0x5cda, 0x75},
+	{0x5cdb, 0x75},
+	{0x5cdc, 0x75},
+	{0x5cdd, 0x75},
+	{0x5cde, 0x75},
+	{0x5cdf, 0x75},
+	{0x5ce0, 0x75},
+	{0x5ce1, 0x75},
+	{0x5ce2, 0x75},
+	{0x5ce3, 0x75},
+	{0x5ce4, 0x75},
+	{0x5ce5, 0x75},
+	{0x5ce6, 0x75},
+	{0x5ce7, 0x75},
+	{0x5ce8, 0x75},
+	{0x5ce9, 0x75},
+	{0x5cea, 0x75},
+	{0x5ceb, 0x75},
+	{0x5cec, 0x75},
+	{0x5ced, 0x75},
+	{0x5cee, 0x75},
+	{0x5cef, 0x75},
+	{0x5cf0, 0x75},
+	{0x5cf1, 0x75},
+	{0x5cf2, 0x75},
+	{0x5cf3, 0x75},
+	{0x5cf4, 0x75},
+	{0x5cf5, 0x75},
+	{0x5cf6, 0x75},
+	{0x5cf7, 0x75},
+	{0x5cf8, 0x75},
+	{0x5cf9, 0x75},
+	{0x5cfa, 0x75},
+	{0x5cfb, 0x75},
+	{0x5cfc, 0x75},
+	{0x5cfd, 0x75},
+	{0x5cfe, 0x75},
+	{0x5cff, 0x75},
+	{0x5d00, 0x75},
+	{0x5d01, 0x75},
+	{0x5d02, 0x75},
+	{0x5d03, 0x75},
+	{0x5d04, 0x75},
+	{0x5d05, 0x75},
+	{0x5d06, 0x75},
+	{0x5d07, 0x75},
+	{0x5d08, 0x75},
+	{0x5d09, 0x75},
+	{0x5d0a, 0x75},
+	{0x5d0b, 0x75},
+	{0x5d0c, 0x75},
+	{0x5d0d, 0x75},
+	{0x5d0e, 0x75},
+	{0x5d0f, 0x75},
+	{0x5d10, 0x75},
+	{0x5d11, 0x75},
+	{0x5d12, 0x75},
+	{0x5d13, 0x75},
+	{0x5d14, 0x75},
+	{0x5d15, 0x75},
+	{0x5d16, 0x75},
+	{0x5d17, 0x75},
+	{0x5d18, 0x75},
+	{0x5d19, 0x75},
+	{0x5d1a, 0x75},
+	{0x5d1b, 0x75},
+	{0x5d1c, 0x75},
+	{0x5d1d, 0x75},
+	{0x5d1e, 0x75},
+	{0x5d1f, 0x75},
+	{0x5d20, 0x75},
+	{0x5d21, 0x75},
+	{0x5d22, 0x75},
+	{0x5d23, 0x75},
+	{0x5d24, 0x75},
+	{0x5d25, 0x75},
+	{0x5d26, 0x75},
+	{0x5d27, 0x75},
+	{0x5d28, 0x75},
+	{0x5d29, 0x75},
+	{0x5d2a, 0x75},
+	{0x5d2b, 0x75},
+	{0x5d2c, 0x75},
+	{0x5d2d, 0x75},
+	{0x5d2e, 0x75},
+	{0x5d2f, 0x75},
+	{0x5d30, 0x75},
+	{0x5d31, 0x75},
+	{0x5d32, 0x75},
+	{0x5d33, 0x75},
+	{0x5d34, 0x75},
+	{0x5d35, 0x75},
+	{0x5d36, 0x75},
+	{0x5d37, 0x75},
+	{0x5d38, 0x75},
+	{0x5d39, 0x75},
+	{0x5d3a, 0x75},
+	{0x5d3b, 0x75},
+	{0x5d3c, 0x75},
+	{0x5d3d, 0x75},
+	{0x5d3e, 0x75},
+	{0x5d3f, 0x75},
+	{0x5d40, 0x75},
+	{0x5d41, 0x75},
+	{0x5d42, 0x75},
+	{0x5d43, 0x75},
+	{0x5d44, 0x75},
+	{0x5d45, 0x75},
+	{0x5d46, 0x75},
+	{0x5d47, 0x75},
+	{0x5d48, 0x75},
+	{0x5d49, 0x75},
+	{0x5d4a, 0x75},
+	{0x5d4b, 0x75},
+	{0x5d4c, 0x75},
+	{0x5d4d, 0x75},
+	{0x5d4e, 0x75},
+	{0x5d4f, 0x75},
+	{0x5d50, 0x75},
+	{0x5d51, 0x75},
+	{0x5d52, 0x75},
+	{0x5d53, 0x75},
+	{0x5d54, 0x75},
+	{0x5d55, 0x75},
+	{0x5d56, 0x75},
+	{0x5d57, 0x75},
+	{0x5d58, 0x75},
+	{0x5d59, 0x75},
+	{0x5d5a, 0x75},
+	{0x5d5b, 0x75},
+	{0x5d5c, 0x75},
+	{0x5d5d, 0x75},
+	{0x5d5e, 0x75},
+	{0x5d5f, 0x75},
+	{0x5d60, 0x75},
+	{0x5d61, 0x75},
+	{0x5d62, 0x75},
+	{0x5d63, 0x75},
+	{0x5d64, 0x75},
+	{0x5d65, 0x75},
+	{0x5d66, 0x75},
+	{0x5d67, 0x75},
+	{0x5d68, 0x75},
+	{0x5d69, 0x75},
+	{0x5d6a, 0x75},
+	{0x5d6b, 0x75},
+	{0x5d6c, 0x75},
+	{0x5d6d, 0x75},
+	{0x5d6e, 0x75},
+	{0x5d6f, 0x75},
+	{0x5d70, 0x75},
+	{0x5d71, 0x75},
+	{0x5d72, 0x75},
+	{0x5d73, 0x75},
+	{0x5d74, 0x75},
+	{0x5d75, 0x75},
+	{0x5d76, 0x75},
+	{0x5d77, 0x75},
+	{0x5d78, 0x75},
+	{0x5d79, 0x75},
+	{0x5d7a, 0x75},
+	{0x5d7b, 0x75},
+	{0x5d7c, 0x75},
+	{0x5d7d, 0x75},
+	{0x5d7e, 0x75},
+	{0x5d7f, 0x75},
+	{0x5d80, 0x75},
+	{0x5d81, 0x75},
+	{0x5d82, 0x75},
+	{0x5d83, 0x75},
+	{0x5d84, 0x75},
+	{0x5d85, 0x75},
+	{0x5d86, 0x75},
+	{0x5d87, 0x75},
+	{0x5d88, 0x75},
+	{0x5d89, 0x75},
+	{0x5d8a, 0x75},
+	{0x5d8b, 0x75},
+	{0x5d8c, 0x75},
+	{0x5d8d, 0x75},
+	{0x5d8e, 0x75},
+	{0x5d8f, 0x75},
+	{0x5d90, 0x75},
+	{0x5d91, 0x75},
+	{0x5d92, 0x75},
+	{0x5d93, 0x75},
+	{0x5d94, 0x75},
+	{0x5d95, 0x75},
+	{0x5d96, 0x75},
+	{0x5d97, 0x75},
+	{0x5d98, 0x75},
+	{0x5d99, 0x75},
+	{0x5d9a, 0x75},
+	{0x5d9b, 0x75},
+	{0x5d9c, 0x75},
+	{0x5d9d, 0x75},
+	{0x5d9e, 0x75},
+	{0x5d9f, 0x75},
+	{0x5da0, 0x75},
+	{0x5da1, 0x75},
+	{0x5da2, 0x75},
+	{0x5da3, 0x75},
+	{0x5da4, 0x75},
+	{0x5da5, 0x75},
+	{0x5da6, 0x75},
+	{0x5da7, 0x75},
+	{0x5da8, 0x75},
+	{0x5da9, 0x75},
+	{0x5daa, 0x75},
+	{0x5dab, 0x75},
+	{0x5dac, 0x75},
+	{0x5dad, 0x75},
+	{0x5dae, 0x75},
+	{0x5daf, 0x75},
+	{0x5db0, 0x75},
+	{0x5db1, 0x75},
+	{0x5db2, 0x75},
+	{0x5db3, 0x75},
+	{0x5db4, 0x75},
+	{0x5db5, 0x75},
+	{0x5db6, 0x75},
+	{0x5db7, 0x75},
+	{0x5db8, 0x75},
+	{0x5db9, 0x75},
+	{0x5dba, 0x75},
+	{0x5dbb, 0x75},
+	{0x5dbc, 0x75},
+	{0x5dbd, 0x75},
+	{0x5dbe, 0x75},
+	{0x5dbf, 0x75},
+	{0x5dc0, 0x75},
+	{0x5dc1, 0x75},
+	{0x5dc2, 0x75},
+	{0x5dc3, 0x75},
+	{0x5dc4, 0x75},
+	{0x5dc5, 0x75},
+	{0x5dc6, 0x75},
+	{0x5dc7, 0x75},
+	{0x5dc8, 0x75},
+	{0x5dc9, 0x75},
+	{0x5dca, 0x75},
+	{0x5dcb, 0x75},
+	{0x5dcc, 0x75},
+	{0x5dcd, 0x75},
+	{0x5dce, 0x75},
+	{0x5dcf, 0x75},
+	{0x5dd0, 0x75},
+	{0x5dd1, 0x75},
+	{0x5dd2, 0x75},
+	{0x5dd3, 0x75},
+	{0x5dd4, 0x75},
+	{0x5dd5, 0x75},
+	{0x5dd6, 0x75},
+	{0x5dd7, 0x75},
+	{0x5dd8, 0x75},
+	{0x5dd9, 0x75},
+	{0x5dda, 0x75},
+	{0x5ddb, 0x75},
+	{0x5ddc, 0x75},
+	{0x5ddd, 0x75},
+	{0x5dde, 0x75},
+	{0x5ddf, 0x75},
+	{0x5de0, 0x75},
+	{0x5de1, 0x75},
+	{0x5de2, 0x75},
+	{0x5de3, 0x75},
+	{0x5de4, 0x75},
+	{0x5de5, 0x75},
+	{0x5de6, 0x75},
+	{0x5de7, 0x75},
+	{0x5de8, 0x75},
+	{0x5de9, 0x75},
+	{0x5dea, 0x75},
+	{0x5deb, 0x75},
+	{0x5dec, 0x75},
+	{0x5ded, 0x75},
+	{0x5dee, 0x75},
+	{0x5def, 0x75},
+	{0x5df0, 0x75},
+	{0x5df1, 0x75},
+	{0x5df2, 0x75},
+	{0x5df3, 0x75},
+	{0x5df4, 0x75},
+	{0x5df5, 0x75},
+	{0x5df6, 0x75},
+	{0x5df7, 0x75},
+	{0x5df8, 0x75},
+	{0x5df9, 0x75},
+	{0x5dfa, 0x75},
+	{0x5dfb, 0x75},
+	{0x5dfc, 0x75},
+	{0x5dfd, 0x75},
+	{0x5dfe, 0x75},
+	{0x5dff, 0x75},
+	{0x5e00, 0x75},
+	{0x5e01, 0x75},
+	{0x5e02, 0x75},
+	{0x5e03, 0x75},
+	{0x5e04, 0x75},
+	{0x5e05, 0x75},
+	{0x5e06, 0x75},
+	{0x5e07, 0x75},
+	{0x5e08, 0x75},
+	{0x5e09, 0x75},
+	{0x5e0a, 0x75},
+	{0x5e0b, 0x75},
+	{0x5e0c, 0x75},
+	{0x5e0d, 0x75},
+	{0x5e0e, 0x75},
+	{0x5e0f, 0x75},
+	{0x5e10, 0x75},
+	{0x5e11, 0x75},
+	{0x5e12, 0x75},
+	{0x5e13, 0x75},
+	{0x5e14, 0x75},
+	{0x5e15, 0x75},
+	{0x5e16, 0x75},
+	{0x5e17, 0x75},
+	{0x5e18, 0x75},
+	{0x5e19, 0x75},
+	{0x5e1a, 0x75},
+	{0x5e1b, 0x75},
+	{0x5e1c, 0x75},
+	{0x5e1d, 0x75},
+	{0x5e1e, 0x75},
+	{0x5e1f, 0x75},
+	{0x5e20, 0x75},
+	{0x5e21, 0x75},
+	{0x5e22, 0x75},
+	{0x5e23, 0x75},
+	{0x5e24, 0x75},
+	{0x5e25, 0x75},
+	{0x5e26, 0x75},
+	{0x5e27, 0x75},
+	{0x5e28, 0x75},
+	{0x5e29, 0x75},
+	{0x5e2a, 0x75},
+	{0x5e2b, 0x75},
+	{0x5e2c, 0x75},
+	{0x5e2d, 0x75},
+	{0x5e2e, 0x75},
+	{0x5e2f, 0x75},
+	{0x5e30, 0x75},
+	{0x5e31, 0x75},
+	{0x5e32, 0x75},
+	{0x5e33, 0x75},
+	{0x5e34, 0x75},
+	{0x5e35, 0x75},
+	{0x5e36, 0x75},
+	{0x5e37, 0x75},
+	{0x5e38, 0x75},
+	{0x5e39, 0x75},
+	{0x5e3a, 0x75},
+	{0x5e3b, 0x75},
+	{0x5e3c, 0x75},
+	{0x5e3d, 0x75},
+	{0x5e3e, 0x75},
+	{0x5e3f, 0x75},
+	{0x5e40, 0x75},
+	{0x5e41, 0x75},
+	{0x5e42, 0x75},
+	{0x5e43, 0x75},
+	{0x5e44, 0x75},
+	{0x5e45, 0x75},
+	{0x5e46, 0x75},
+	{0x5e47, 0x75},
+	{0x5e48, 0x75},
+	{0x5e49, 0x75},
+	{0x5e4a, 0x75},
+	{0x5e4b, 0x75},
+	{0x5e4c, 0x75},
+	{0x5e4d, 0x75},
+	{0x5e4e, 0x75},
+	{0x5e4f, 0x75},
+	{0x5e50, 0x75},
+	{0x5e51, 0x75},
+	{0x5e52, 0x75},
+	{0x5e53, 0x75},
+	{0x5e54, 0x75},
+	{0x5e55, 0x75},
+	{0x5e56, 0x75},
+	{0x5e57, 0x75},
+	{0x5e58, 0x75},
+	{0x5e59, 0x75},
+	{0x5e5a, 0x75},
+	{0x5e5b, 0x75},
+	{0x5e5c, 0x75},
+	{0x5e5d, 0x75},
+	{0x5e5e, 0x75},
+	{0x5e5f, 0x75},
+	{0x5e60, 0x75},
+	{0x5e61, 0x75},
+	{0x5e62, 0x75},
+	{0x5e63, 0x75},
+	{0x5e64, 0x75},
+	{0x5e65, 0x75},
+	{0x5e66, 0x75},
+	{0x5e67, 0x75},
+	{0x5e68, 0x75},
+	{0x5e69, 0x75},
+	{0x5e6a, 0x75},
+	{0x5e6b, 0x75},
+	{0x5e6c, 0x75},
+	{0x5e6d, 0x75},
+	{0x5e6e, 0x75},
+	{0x5e6f, 0x75},
+	{0x5e70, 0x75},
+	{0x5e71, 0x75},
+	{0x5e72, 0x75},
+	{0x5e73, 0x75},
+	{0x5e74, 0x75},
+	{0x5e75, 0x75},
+	{0x5e76, 0x75},
+	{0x5e77, 0x75},
+	{0x5e78, 0x75},
+	{0x5e79, 0x75},
+	{0x5e7a, 0x75},
+	{0x5e7b, 0x75},
+	{0x5e7c, 0x75},
+	{0x5e7d, 0x75},
+	{0x5e7e, 0x75},
+	{0x5e7f, 0x75},
+	{0x5e80, 0x75},
+	{0x5e81, 0x75},
+	{0x5e82, 0x75},
+	{0x5e83, 0x75},
+	{0x5e84, 0x75},
+	{0x5e85, 0x75},
+	{0x5e86, 0x75},
+	{0x5e87, 0x75},
+	{0x5e88, 0x75},
+	{0x5e89, 0x75},
+	{0x5e8a, 0x75},
+	{0x5e8b, 0x75},
+	{0x5e8c, 0x75},
+	{0x5e8d, 0x75},
+	{0x5e8e, 0x75},
+	{0x5e8f, 0x75},
+	{0x5e90, 0x75},
+	{0x5e91, 0x75},
+	{0x5e92, 0x75},
+	{0x5e93, 0x75},
+	{0x5e94, 0x75},
+	{0x5e95, 0x75},
+	{0x5e96, 0x75},
+	{0x5e97, 0x75},
+	{0x5e98, 0x75},
+	{0x5e99, 0x75},
+	{0x5e9a, 0x75},
+	{0x5e9b, 0x75},
+	{0x5e9c, 0x75},
+	{0x5e9d, 0x75},
+	{0x5e9e, 0x75},
+	{0x5e9f, 0x75},
+	{0x5ea0, 0x75},
+	{0x5ea1, 0x75},
+	{0x5ea2, 0x75},
+	{0x5ea3, 0x75},
+	{0x5ea4, 0x75},
+	{0x5ea5, 0x75},
+	{0x5ea6, 0x75},
+	{0x5ea7, 0x75},
+	{0x5ea8, 0x75},
+	{0x5ea9, 0x75},
+	{0x5eaa, 0x75},
+	{0x5eab, 0x75},
+	{0x5eac, 0x75},
+	{0x5ead, 0x75},
+	{0x5eae, 0x75},
+	{0x5eaf, 0x75},
+	{0x5eb0, 0x75},
+	{0x5eb1, 0x75},
+	{0x5eb2, 0x75},
+	{0x5eb3, 0x75},
+	{0x5eb4, 0x75},
+	{0x5eb5, 0x75},
+	{0x5eb6, 0x75},
+	{0x5eb7, 0x75},
+	{0x5eb8, 0x75},
+	{0x5eb9, 0x75},
+	{0x5eba, 0x75},
+	{0x5ebb, 0x75},
+	{0x5ebc, 0x75},
+	{0x5ebd, 0x75},
+	{0x5ebe, 0x75},
+	{0x5ebf, 0x75},
+	{0x5ec0, 0x75},
+	{0x5ec1, 0x75},
+	{0x5ec2, 0x75},
+	{0x5ec3, 0x75},
+	{0x5ec4, 0x75},
+	{0x5ec5, 0x75},
+	{0x5ec6, 0x75},
+	{0x5ec7, 0x75},
+	{0x5ec8, 0x75},
+	{0x5ec9, 0x75},
+	{0x5eca, 0x75},
+	{0x5ecb, 0x75},
+	{0x5ecc, 0x75},
+	{0x5ecd, 0x75},
+	{0x5ece, 0x75},
+	{0x5ecf, 0x75},
+	{0x5ed0, 0x75},
+	{0x5ed1, 0x75},
+	{0x5ed2, 0x75},
+	{0x5ed3, 0x75},
+	{0x5ed4, 0x75},
+	{0x5ed5, 0x75},
+	{0x5ed6, 0x75},
+	{0x5ed7, 0x75},
+	{0x5ed8, 0x75},
+	{0x5ed9, 0x75},
+	{0x5eda, 0x75},
+	{0x5edb, 0x75},
+	{0x5edc, 0x75},
+	{0x5edd, 0x75},
+	{0x5ede, 0x75},
+	{0x5edf, 0x75},
+	{0x5ee0, 0x75},
+	{0x5ee1, 0x75},
+	{0x5ee2, 0x75},
+	{0x5ee3, 0x75},
+	{0x5ee4, 0x75},
+	{0x5ee5, 0x75},
+	{0x5ee6, 0x75},
+	{0x5ee7, 0x75},
+	{0x5ee8, 0x75},
+	{0x5ee9, 0x75},
+	{0x5eea, 0x75},
+	{0x5eeb, 0x75},
+	{0x5eec, 0x75},
+	{0x5eed, 0x75},
+	{0x5eee, 0x75},
+	{0x5eef, 0x75},
+	{0x5ef0, 0x75},
+	{0x5ef1, 0x75},
+	{0x5ef2, 0x75},
+	{0x5ef3, 0x75},
+	{0x5ef4, 0x75},
+	{0x5ef5, 0x75},
+	{0x5ef6, 0x75},
+	{0x5ef7, 0x75},
+	{0x5ef8, 0x75},
+	{0x5ef9, 0x75},
+	{0x5efa, 0x75},
+	{0x5efb, 0x75},
+	{0x5efc, 0x75},
+	{0x5efd, 0x75},
+	{0x5efe, 0x75},
+	{0x5eff, 0x75},
+	{0x5f00, 0x75},
+	{0x5f01, 0x75},
+	{0x5f02, 0x75},
+	{0x5f03, 0x75},
+	{0x5f04, 0x75},
+	{0x5f05, 0x75},
+	{0x5f06, 0x75},
+	{0x5f07, 0x75},
+	{0x5f08, 0x75},
+	{0x5f09, 0x75},
+	{0x5f0a, 0x75},
+	{0x5f0b, 0x75},
+	{0x5f0c, 0x75},
+	{0x5f0d, 0x75},
+	{0x5f0e, 0x75},
+	{0x5f0f, 0x75},
+	{0x5f10, 0x75},
+	{0x5f11, 0x75},
+	{0x5f12, 0x75},
+	{0x5f13, 0x75},
+	{0x5f14, 0x75},
+	{0x5f15, 0x75},
+	{0x5f16, 0x75},
+	{0x5f17, 0x75},
+	{0x5f18, 0x75},
+	{0x5f19, 0x75},
+	{0x5f1a, 0x75},
+	{0x5f1b, 0x75},
+	{0x5f1c, 0x75},
+	{0x5f1d, 0x75},
+	{0x5f1e, 0x75},
+	{0x5f1f, 0x75},
+};
+
+static const struct ov08x40_reg mode_1928x1208_regs[] = {
+	{0x5000, 0x55},
+	{0x5001, 0x00},
+	{0x5008, 0xb0},
+	{0x50c1, 0x00},
+	{0x53c1, 0x00},
+	{0x5f40, 0x00},
+	{0x5f41, 0x40},
+	{0x0300, 0x3a},
+	{0x0301, 0xc8},
+	{0x0302, 0x31},
+	{0x0303, 0x03},
+	{0x0304, 0x01},
+	{0x0305, 0xa1},
+	{0x0306, 0x04},
+	{0x0307, 0x01},
+	{0x0308, 0x03},
+	{0x0309, 0x03},
+	{0x0310, 0x0a},
+	{0x0311, 0x02},
+	{0x0312, 0x01},
+	{0x0313, 0x08},
+	{0x0314, 0x66},
+	{0x0315, 0x00},
+	{0x0316, 0x34},
+	{0x0320, 0x02},
+	{0x0321, 0x03},
+	{0x0323, 0x05},
+	{0x0324, 0x01},
+	{0x0325, 0xb8},
+	{0x0326, 0x4a},
+	{0x0327, 0x04},
+	{0x0329, 0x00},
+	{0x032a, 0x05},
+	{0x032b, 0x00},
+	{0x032c, 0x00},
+	{0x032d, 0x00},
+	{0x032e, 0x02},
+	{0x032f, 0xa0},
+	{0x0350, 0x00},
+	{0x0360, 0x01},
+	{0x1216, 0x60},
+	{0x1217, 0x5b},
+	{0x1218, 0x00},
+	{0x1220, 0x24},
+	{0x198a, 0x00},
+	{0x198b, 0x01},
+	{0x198e, 0x00},
+	{0x198f, 0x01},
+	{0x3009, 0x04},
+	{0x3012, 0x41},
+	{0x3015, 0x00},
+	{0x3016, 0xb0},
+	{0x3017, 0xf0},
+	{0x3018, 0xf0},
+	{0x3019, 0xd2},
+	{0x301a, 0xb0},
+	{0x301c, 0x81},
+	{0x301d, 0x02},
+	{0x301e, 0x80},
+	{0x3022, 0xf0},
+	{0x3025, 0x89},
+	{0x3030, 0x03},
+	{0x3044, 0xc2},
+	{0x3050, 0x35},
+	{0x3051, 0x60},
+	{0x3052, 0x25},
+	{0x3053, 0x00},
+	{0x3054, 0x00},
+	{0x3055, 0x02},
+	{0x3056, 0x80},
+	{0x3057, 0x80},
+	{0x3058, 0x80},
+	{0x3059, 0x00},
+	{0x3107, 0x86},
+	{0x3400, 0x1c},
+	{0x3401, 0x80},
+	{0x3402, 0x8c},
+	{0x3419, 0x08},
+	{0x341a, 0xaf},
+	{0x341b, 0x30},
+	{0x3420, 0x00},
+	{0x3421, 0x00},
+	{0x3422, 0x00},
+	{0x3423, 0x00},
+	{0x3424, 0x00},
+	{0x3425, 0x00},
+	{0x3426, 0x00},
+	{0x3427, 0x00},
+	{0x3428, 0x0f},
+	{0x3429, 0x00},
+	{0x342a, 0x00},
+	{0x342b, 0x00},
+	{0x342c, 0x00},
+	{0x342d, 0x00},
+	{0x342e, 0x00},
+	{0x342f, 0x11},
+	{0x3430, 0x11},
+	{0x3431, 0x10},
+	{0x3432, 0x00},
+	{0x3433, 0x00},
+	{0x3434, 0x00},
+	{0x3435, 0x00},
+	{0x3436, 0x00},
+	{0x3437, 0x00},
+	{0x3442, 0x02},
+	{0x3443, 0x02},
+	{0x3444, 0x07},
+	{0x3450, 0x00},
+	{0x3451, 0x00},
+	{0x3452, 0x18},
+	{0x3453, 0x18},
+	{0x3454, 0x00},
+	{0x3455, 0x80},
+	{0x3456, 0x08},
+	{0x3500, 0x00},
+	{0x3501, 0x02},
+	{0x3502, 0x00},
+	{0x3504, 0x4c},
+	{0x3506, 0x30},
+	{0x3507, 0x00},
+	{0x3508, 0x01},
+	{0x3509, 0x00},
+	{0x350a, 0x01},
+	{0x350b, 0x00},
+	{0x350c, 0x00},
+	{0x3540, 0x00},
+	{0x3541, 0x01},
+	{0x3542, 0x00},
+	{0x3544, 0x4c},
+	{0x3546, 0x30},
+	{0x3547, 0x00},
+	{0x3548, 0x01},
+	{0x3549, 0x00},
+	{0x354a, 0x01},
+	{0x354b, 0x00},
+	{0x354c, 0x00},
+	{0x3688, 0x02},
+	{0x368a, 0x2e},
+	{0x368e, 0x71},
+	{0x3696, 0xd1},
+	{0x3699, 0x00},
+	{0x369a, 0x00},
+	{0x36a4, 0x00},
+	{0x36a6, 0x00},
+	{0x3711, 0x00},
+	{0x3712, 0x50},
+	{0x3713, 0x00},
+	{0x3714, 0x21},
+	{0x3716, 0x00},
+	{0x3718, 0x07},
+	{0x371a, 0x1c},
+	{0x371b, 0x00},
+	{0x3720, 0x08},
+	{0x3725, 0x32},
+	{0x3727, 0x05},
+	{0x3760, 0x02},
+	{0x3761, 0x28},
+	{0x3762, 0x02},
+	{0x3763, 0x02},
+	{0x3764, 0x02},
+	{0x3765, 0x2c},
+	{0x3766, 0x04},
+	{0x3767, 0x2c},
+	{0x3768, 0x02},
+	{0x3769, 0x00},
+	{0x376b, 0x20},
+	{0x376e, 0x07},
+	{0x37b0, 0x01},
+	{0x37b1, 0x0f},
+	{0x37b2, 0x01},
+	{0x37b3, 0xd6},
+	{0x37b4, 0x01},
+	{0x37b5, 0x48},
+	{0x37b6, 0x02},
+	{0x37b7, 0x40},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x00},
+	{0x3804, 0x0f},
+	{0x3805, 0x1f},
+	{0x3806, 0x09},
+	{0x3807, 0x7f},
+	{0x3808, 0x07},
+	{0x3809, 0x88},
+	{0x380a, 0x04},
+	{0x380b, 0xb8},
+	{0x380c, 0x02},
+	{0x380d, 0xd0},
+	{0x380e, 0x11},
+	{0x380f, 0x5c},
+	{0x3810, 0x00},
+	{0x3811, 0x04},
+	{0x3812, 0x00},
+	{0x3813, 0x03},
+	{0x3814, 0x11},
+	{0x3815, 0x11},
+	{0x3820, 0x02},
+	{0x3821, 0x14},
+	{0x3822, 0x00},
+	{0x3823, 0x04},
+	{0x3828, 0x0f},
+	{0x382a, 0x80},
+	{0x382e, 0x41},
+	{0x3837, 0x08},
+	{0x383a, 0x81},
+	{0x383b, 0x81},
+	{0x383c, 0x11},
+	{0x383d, 0x11},
+	{0x383e, 0x00},
+	{0x383f, 0x38},
+	{0x3840, 0x00},
+	{0x3847, 0x00},
+	{0x384a, 0x00},
+	{0x384c, 0x02},
+	{0x384d, 0xd0},
+	{0x3856, 0x50},
+	{0x3857, 0x30},
+	{0x3858, 0x80},
+	{0x3859, 0x40},
+	{0x3860, 0x00},
+	{0x3888, 0x00},
+	{0x3889, 0x00},
+	{0x388a, 0x00},
+	{0x388b, 0x00},
+	{0x388c, 0x00},
+	{0x388d, 0x00},
+	{0x388e, 0x00},
+	{0x388f, 0x00},
+	{0x3894, 0x00},
+	{0x3895, 0x00},
+	{0x3c84, 0x00},
+	{0x3d85, 0x8b},
+	{0x3daa, 0x80},
+	{0x3dab, 0x14},
+	{0x3dac, 0x80},
+	{0x3dad, 0xc8},
+	{0x3dae, 0x81},
+	{0x3daf, 0x7b},
+	{0x3f00, 0x10},
+	{0x3f01, 0x11},
+	{0x3f06, 0x0d},
+	{0x3f07, 0x0b},
+	{0x3f08, 0x0d},
+	{0x3f09, 0x0b},
+	{0x3f0a, 0x01},
+	{0x3f0b, 0x11},
+	{0x3f0c, 0x33},
+	{0x4001, 0x07},
+	{0x4007, 0x20},
+	{0x4008, 0x00},
+	{0x4009, 0x05},
+	{0x400a, 0x00},
+	{0x400b, 0x04},
+	{0x400c, 0x00},
+	{0x400d, 0x04},
+	{0x400e, 0x14},
+	{0x4010, 0xf4},
+	{0x4011, 0x03},
+	{0x4012, 0x55},
+	{0x4015, 0x00},
+	{0x4016, 0x27},
+	{0x4017, 0x00},
+	{0x4018, 0x0f},
+	{0x401b, 0x08},
+	{0x401c, 0x00},
+	{0x401d, 0x10},
+	{0x401e, 0x02},
+	{0x401f, 0x00},
+	{0x4050, 0x06},
+	{0x4051, 0xff},
+	{0x4052, 0xff},
+	{0x4053, 0xff},
+	{0x4054, 0xff},
+	{0x4055, 0xff},
+	{0x4056, 0xff},
+	{0x4057, 0x7f},
+	{0x4058, 0x00},
+	{0x4059, 0x00},
+	{0x405a, 0x00},
+	{0x405b, 0x00},
+	{0x405c, 0x07},
+	{0x405d, 0xff},
+	{0x405e, 0x07},
+	{0x405f, 0xff},
+	{0x4080, 0x78},
+	{0x4081, 0x78},
+	{0x4082, 0x78},
+	{0x4083, 0x78},
+	{0x4019, 0x00},
+	{0x401a, 0x40},
+	{0x4020, 0x04},
+	{0x4021, 0x00},
+	{0x4022, 0x04},
+	{0x4023, 0x00},
+	{0x4024, 0x04},
+	{0x4025, 0x00},
+	{0x4026, 0x04},
+	{0x4027, 0x00},
+	{0x4030, 0x00},
+	{0x4031, 0x00},
+	{0x4032, 0x00},
+	{0x4033, 0x00},
+	{0x4034, 0x00},
+	{0x4035, 0x00},
+	{0x4036, 0x00},
+	{0x4037, 0x00},
+	{0x4040, 0x00},
+	{0x4041, 0x80},
+	{0x4042, 0x00},
+	{0x4043, 0x80},
+	{0x4044, 0x00},
+	{0x4045, 0x80},
+	{0x4046, 0x00},
+	{0x4047, 0x80},
+	{0x4060, 0x00},
+	{0x4061, 0x00},
+	{0x4062, 0x00},
+	{0x4063, 0x00},
+	{0x4064, 0x00},
+	{0x4065, 0x00},
+	{0x4066, 0x00},
+	{0x4067, 0x00},
+	{0x4068, 0x00},
+	{0x4069, 0x00},
+	{0x406a, 0x00},
+	{0x406b, 0x00},
+	{0x406c, 0x00},
+	{0x406d, 0x00},
+	{0x406e, 0x00},
+	{0x406f, 0x00},
+	{0x4070, 0x00},
+	{0x4071, 0x00},
+	{0x4072, 0x00},
+	{0x4073, 0x00},
+	{0x4074, 0x00},
+	{0x4075, 0x00},
+	{0x4076, 0x00},
+	{0x4077, 0x00},
+	{0x4078, 0x00},
+	{0x4079, 0x00},
+	{0x407a, 0x00},
+	{0x407b, 0x00},
+	{0x407c, 0x00},
+	{0x407d, 0x00},
+	{0x407e, 0x00},
+	{0x407f, 0x00},
+	{0x40e0, 0x00},
+	{0x40e1, 0x00},
+	{0x40e2, 0x00},
+	{0x40e3, 0x00},
+	{0x40e4, 0x00},
+	{0x40e5, 0x00},
+	{0x40e6, 0x00},
+	{0x40e7, 0x00},
+	{0x40e8, 0x00},
+	{0x40e9, 0x80},
+	{0x40ea, 0x00},
+	{0x40eb, 0x80},
+	{0x40ec, 0x00},
+	{0x40ed, 0x80},
+	{0x40ee, 0x00},
+	{0x40ef, 0x80},
+	{0x40f0, 0x02},
+	{0x40f1, 0x04},
+	{0x4300, 0x00},
+	{0x4301, 0x00},
+	{0x4302, 0x00},
+	{0x4303, 0x00},
+	{0x4304, 0x00},
+	{0x4305, 0x00},
+	{0x4306, 0x00},
+	{0x4307, 0x00},
+	{0x4308, 0x00},
+	{0x4309, 0x00},
+	{0x430a, 0x00},
+	{0x430b, 0xff},
+	{0x430c, 0xff},
+	{0x430d, 0x00},
+	{0x430e, 0x00},
+	{0x4315, 0x00},
+	{0x4316, 0x00},
+	{0x4317, 0x00},
+	{0x4318, 0x00},
+	{0x4319, 0x00},
+	{0x431a, 0x00},
+	{0x431b, 0x00},
+	{0x431c, 0x00},
+	{0x4500, 0x07},
+	{0x4501, 0x10},
+	{0x4502, 0x00},
+	{0x4503, 0x0f},
+	{0x4504, 0x80},
+	{0x4506, 0x01},
+	{0x4509, 0x05},
+	{0x450c, 0x00},
+	{0x450d, 0x20},
+	{0x450e, 0x00},
+	{0x450f, 0x00},
+	{0x4510, 0x00},
+	{0x4523, 0x00},
+	{0x4526, 0x00},
+	{0x4542, 0x00},
+	{0x4543, 0x00},
+	{0x4544, 0x00},
+	{0x4545, 0x00},
+	{0x4546, 0x00},
+	{0x4547, 0x10},
+	{0x4602, 0x00},
+	{0x4603, 0x15},
+	{0x460b, 0x07},
+	{0x4680, 0x11},
+	{0x4686, 0x00},
+	{0x4687, 0x00},
+	{0x4700, 0x00},
+	{0x4800, 0x64},
+	{0x4806, 0x40},
+	{0x480b, 0x10},
+	{0x480c, 0x80},
+	{0x480f, 0x32},
+	{0x4813, 0xe4},
+	{0x4837, 0x14},
+	{0x4850, 0x42},
+	{0x4884, 0x04},
+	{0x4c00, 0xf8},
+	{0x4c01, 0x44},
+	{0x4c03, 0x00},
+	{0x4d00, 0x00},
+	{0x4d01, 0x16},
+	{0x4d04, 0x10},
+	{0x4d05, 0x00},
+	{0x4d06, 0x0c},
+	{0x4d07, 0x00},
+	{0x3d84, 0x04},
+	{0x3680, 0xa4},
+	{0x3682, 0x80},
+	{0x3601, 0x40},
+	{0x3602, 0x90},
+	{0x3608, 0x0a},
+	{0x3938, 0x09},
+	{0x3a74, 0x84},
+	{0x3a99, 0x84},
+	{0x3ab9, 0xa6},
+	{0x3aba, 0xba},
+	{0x3b12, 0x84},
+	{0x3b14, 0xbb},
+	{0x3b15, 0xbf},
+	{0x3a29, 0x26},
+	{0x3a1f, 0x8a},
+	{0x3a22, 0x91},
+	{0x3a25, 0x96},
+	{0x3a28, 0xb4},
+	{0x3a2b, 0xba},
+	{0x3a2e, 0xbf},
+	{0x3a31, 0xc1},
+	{0x3a20, 0x05},
+	{0x3939, 0x6b},
+	{0x3902, 0x10},
+	{0x3903, 0x10},
+	{0x3904, 0x10},
+	{0x3905, 0x10},
+	{0x3906, 0x01},
+	{0x3907, 0x0b},
+	{0x3908, 0x10},
+	{0x3909, 0x13},
+	{0x360f, 0x99},
+	{0x390b, 0x11},
+	{0x390c, 0x21},
+	{0x390d, 0x32},
+	{0x390e, 0x76},
+	{0x3911, 0x90},
+	{0x3913, 0x90},
+	{0x3b3f, 0x9d},
+	{0x3b45, 0x9d},
+	{0x3b1b, 0xc9},
+	{0x3b21, 0xc9},
+	{0x3a1a, 0x1c},
+	{0x3a23, 0x15},
+	{0x3a26, 0x17},
+	{0x3a2c, 0x50},
+	{0x3a2f, 0x18},
+	{0x3a32, 0x4f},
+	{0x3ace, 0x01},
+	{0x3ad2, 0x01},
+	{0x3ad6, 0x01},
+	{0x3ada, 0x01},
+	{0x3ade, 0x01},
+	{0x3ae2, 0x01},
+	{0x3aee, 0x01},
+	{0x3af2, 0x01},
+	{0x3af6, 0x01},
+	{0x3afa, 0x01},
+	{0x3afe, 0x01},
+	{0x3b02, 0x01},
+	{0x3b06, 0x01},
+	{0x3b0a, 0x01},
+	{0x3b0b, 0x00},
+	{0x3b0e, 0x01},
+	{0x3b0f, 0x00},
+	{0x392c, 0x02},
+	{0x392d, 0x01},
+	{0x392e, 0x04},
+	{0x392f, 0x03},
+	{0x3930, 0x09},
+	{0x3931, 0x07},
+	{0x3932, 0x10},
+	{0x3933, 0x0d},
+	{0x3609, 0x08},
+	{0x3921, 0x0f},
+	{0x3928, 0x15},
+	{0x3929, 0x2a},
+	{0x392a, 0x52},
+	{0x392b, 0xa3},
+	{0x340b, 0x1b},
+	{0x3426, 0x10},
+	{0x3407, 0x01},
+	{0x3404, 0x01},
+	{0x3500, 0x00},
+	{0x3501, 0x08},
+	{0x3502, 0x10},
+	{0x3508, 0x04},
+	{0x3509, 0x00},
+};
+
+static const char * const ov08x40_test_pattern_menu[] = {
+	"Disabled",
+	"Vertical Color Bar Type 1",
+	"Vertical Color Bar Type 2",
+	"Vertical Color Bar Type 3",
+	"Vertical Color Bar Type 4"
+};
+
+/* Configurations for supported link frequencies */
+#define OV08X40_LINK_FREQ_400MHZ	400000000ULL
+
+#define OV08X40_EXT_CLK			19200000
+#define OV08X40_DATA_LANES		4
+
+/*
+ * pixel_rate = link_freq * data-rate * nr_of_lanes / bits_per_sample
+ * data rate => double data rate; number of lanes => 4; bits per pixel => 10
+ */
+static u64 link_freq_to_pixel_rate(u64 f)
+{
+	f *= 2 * OV08X40_DATA_LANES;
+	do_div(f, 10);
+
+	return f;
+}
+
+/* Menu items for LINK_FREQ V4L2 control */
+static const s64 link_freq_menu_items[] = {
+	OV08X40_LINK_FREQ_400MHZ,
+};
+
+/* Link frequency configs */
+static const struct ov08x40_link_freq_config link_freq_configs[] = {
+	[OV08X40_LINK_FREQ_400MHZ_INDEX] = {
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mipi_data_rate_800mbps),
+			.regs = mipi_data_rate_800mbps,
+		}
+	},
+};
+
+/* Mode configs */
+static const struct ov08x40_mode supported_modes[] = {
+	{
+		.width = 3856,
+		.height = 2416,
+		.vts_def = OV08X40_VTS_30FPS,
+		.vts_min = OV08X40_VTS_30FPS,
+		.hts = 640,
+		.lanes = 4,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_3856x2416_regs),
+			.regs = mode_3856x2416_regs,
+		},
+		.link_freq_index = OV08X40_LINK_FREQ_400MHZ_INDEX,
+	},
+	{
+		.width = 1928,
+		.height = 1208,
+		.vts_def = OV08X40_VTS_BIN_30FPS,
+		.vts_min = OV08X40_VTS_BIN_30FPS,
+		.hts = 720,
+		.lanes = 4,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1928x1208_regs),
+			.regs = mode_1928x1208_regs,
+		},
+		.link_freq_index = OV08X40_LINK_FREQ_400MHZ_INDEX,
+	},
+};
+
+struct ov08x40 {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+
+	struct v4l2_ctrl_handler ctrl_handler;
+	/* V4L2 Controls */
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *pixel_rate;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *exposure;
+
+	/* Current mode */
+	const struct ov08x40_mode *cur_mode;
+
+	/* Mutex for serialized access */
+	struct mutex mutex;
+
+	/* Streaming on/off */
+	bool streaming;
+};
+
+#define to_ov08x40(_sd)	container_of(_sd, struct ov08x40, sd)
+
+/* Read registers up to 4 at a time */
+static int ov08x40_read_reg(struct ov08x40 *ov08x,
+			    u16 reg, u32 len, u32 *val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov08x->sd);
+	struct i2c_msg msgs[2];
+	u8 *data_be_p;
+	int ret;
+	__be32 data_be = 0;
+	__be16 reg_addr_be = cpu_to_be16(reg);
+
+	if (len > 4)
+		return -EINVAL;
+
+	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[4 - len];
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret != ARRAY_SIZE(msgs))
+		return -EIO;
+
+	*val = be32_to_cpu(data_be);
+
+	return 0;
+}
+
+/* Write registers up to 4 at a time */
+static int ov08x40_write_reg(struct ov08x40 *ov08x,
+			     u16 reg, u32 len, u32 __val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov08x->sd);
+	int buf_i, val_i;
+	u8 buf[6], *val_p;
+	__be32 val;
+
+	if (len > 4)
+		return -EINVAL;
+
+	buf[0] = reg >> 8;
+	buf[1] = reg & 0xff;
+
+	val = cpu_to_be32(__val);
+	val_p = (u8 *)&val;
+	buf_i = 2;
+	val_i = 4 - len;
+
+	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;
+}
+
+/* Write a list of registers */
+static int ov08x40_write_regs(struct ov08x40 *ov08x,
+			      const struct ov08x40_reg *regs, u32 len)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov08x->sd);
+	int ret;
+	u32 i;
+
+	for (i = 0; i < len; i++) {
+		ret = ov08x40_write_reg(ov08x, regs[i].address, 1,
+					regs[i].val);
+
+		if (ret) {
+			dev_err_ratelimited(&client->dev,
+					    "Failed to write reg 0x%4.4x. error = %d\n",
+					    regs[i].address, ret);
+
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int ov08x40_write_reg_list(struct ov08x40 *ov08x,
+				  const struct ov08x40_reg_list *r_list)
+{
+	return ov08x40_write_regs(ov08x, r_list->regs, r_list->num_of_regs);
+}
+
+static int ov08x40_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	const struct ov08x40_mode *default_mode = &supported_modes[0];
+	struct ov08x40 *ov08x = to_ov08x40(sd);
+	struct v4l2_mbus_framefmt *try_fmt =
+		v4l2_subdev_get_try_format(sd, fh->state, 0);
+
+	mutex_lock(&ov08x->mutex);
+
+	/* Initialize try_fmt */
+	try_fmt->width = default_mode->width;
+	try_fmt->height = default_mode->height;
+	try_fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	try_fmt->field = V4L2_FIELD_NONE;
+
+	/* No crop or compose */
+	mutex_unlock(&ov08x->mutex);
+
+	return 0;
+}
+
+static int ov08x40_update_digital_gain(struct ov08x40 *ov08x, u32 d_gain)
+{
+	int ret;
+	u32 val;
+
+	/*
+	 * 0x350C[1:0], 0x350B[7:0], 0x350A[4:0]
+	 */
+
+	val = (d_gain & OV08X40_DGTL_GAIN_L_MASK) << OV08X40_DGTL_GAIN_L_SHIFT;
+	ret = ov08x40_write_reg(ov08x, OV08X40_REG_DGTL_GAIN_L,
+				OV08X40_REG_VALUE_08BIT, val);
+	if (ret)
+		return ret;
+
+	val = (d_gain >> OV08X40_DGTL_GAIN_M_SHIFT) & OV08X40_DGTL_GAIN_M_MASK;
+	ret = ov08x40_write_reg(ov08x, OV08X40_REG_DGTL_GAIN_M,
+				OV08X40_REG_VALUE_08BIT, val);
+	if (ret)
+		return ret;
+
+	val = (d_gain >> OV08X40_DGTL_GAIN_H_SHIFT) & OV08X40_DGTL_GAIN_H_MASK;
+
+	return ov08x40_write_reg(ov08x, OV08X40_REG_DGTL_GAIN_H,
+				 OV08X40_REG_VALUE_08BIT, val);
+}
+
+static int ov08x40_enable_test_pattern(struct ov08x40 *ov08x, u32 pattern)
+{
+	int ret;
+	u32 val;
+
+	ret = ov08x40_read_reg(ov08x, OV08X40_REG_TEST_PATTERN,
+			       OV08X40_REG_VALUE_08BIT, &val);
+	if (ret)
+		return ret;
+
+	if (pattern) {
+		ret = ov08x40_read_reg(ov08x, OV08X40_REG_ISP,
+				       OV08X40_REG_VALUE_08BIT, &val);
+		if (ret)
+			return ret;
+
+		ret = ov08x40_write_reg(ov08x, OV08X40_REG_ISP,
+					OV08X40_REG_VALUE_08BIT,
+					val | BIT(1));
+		if (ret)
+			return ret;
+
+		ret = ov08x40_read_reg(ov08x, OV08X40_REG_SHORT_TEST_PATTERN,
+				       OV08X40_REG_VALUE_08BIT, &val);
+		if (ret)
+			return ret;
+
+		ret = ov08x40_write_reg(ov08x, OV08X40_REG_SHORT_TEST_PATTERN,
+					OV08X40_REG_VALUE_08BIT,
+					val | BIT(0));
+		if (ret)
+			return ret;
+
+		ret = ov08x40_read_reg(ov08x, OV08X40_REG_TEST_PATTERN,
+				       OV08X40_REG_VALUE_08BIT, &val);
+		if (ret)
+			return ret;
+
+		val &= OV08X40_TEST_PATTERN_MASK;
+		val |= ((pattern - 1) << OV08X40_TEST_PATTERN_BAR_SHIFT) |
+			OV08X40_TEST_PATTERN_ENABLE;
+	} else {
+		val &= ~OV08X40_TEST_PATTERN_ENABLE;
+	}
+
+	return ov08x40_write_reg(ov08x, OV08X40_REG_TEST_PATTERN,
+				 OV08X40_REG_VALUE_08BIT, val);
+}
+
+static int ov08x40_set_ctrl_hflip(struct ov08x40 *ov08x, u32 ctrl_val)
+{
+	int ret;
+	u32 val;
+
+	ret = ov08x40_read_reg(ov08x, OV08X40_REG_MIRROR,
+			       OV08X40_REG_VALUE_08BIT, &val);
+	if (ret)
+		return ret;
+
+	return ov08x40_write_reg(ov08x, OV08X40_REG_MIRROR,
+				 OV08X40_REG_VALUE_08BIT,
+				 ctrl_val ? val | BIT(2) : val & ~BIT(2));
+}
+
+static int ov08x40_set_ctrl_vflip(struct ov08x40 *ov08x, u32 ctrl_val)
+{
+	int ret;
+	u32 val;
+
+	ret = ov08x40_read_reg(ov08x, OV08X40_REG_VFLIP,
+			       OV08X40_REG_VALUE_08BIT, &val);
+	if (ret)
+		return ret;
+
+	return ov08x40_write_reg(ov08x, OV08X40_REG_VFLIP,
+				 OV08X40_REG_VALUE_08BIT,
+				 ctrl_val ? val | BIT(2) : val & ~BIT(2));
+}
+
+static int ov08x40_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov08x40 *ov08x = container_of(ctrl->handler,
+					     struct ov08x40, ctrl_handler);
+	struct i2c_client *client = v4l2_get_subdevdata(&ov08x->sd);
+	s64 max;
+	int ret = 0;
+
+	/* Propagate change of current control to all related controls */
+	switch (ctrl->id) {
+	case V4L2_CID_VBLANK:
+		/* Update max exposure while meeting expected vblanking */
+		max = ov08x->cur_mode->height + ctrl->val - OV08X40_EXPOSURE_MAX_MARGIN;
+		__v4l2_ctrl_modify_range(ov08x->exposure,
+					 ov08x->exposure->minimum,
+					 max, ov08x->exposure->step, max);
+		break;
+	}
+
+	/*
+	 * Applying V4L2 control value only happens
+	 * when power is up for streaming
+	 */
+	if (!pm_runtime_get_if_in_use(&client->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_ANALOGUE_GAIN:
+		ret = ov08x40_write_reg(ov08x, OV08X40_REG_ANALOG_GAIN,
+					OV08X40_REG_VALUE_16BIT,
+					ctrl->val << 1);
+		break;
+	case V4L2_CID_DIGITAL_GAIN:
+		ret = ov08x40_update_digital_gain(ov08x, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE:
+		ret = ov08x40_write_reg(ov08x, OV08X40_REG_EXPOSURE,
+					OV08X40_REG_VALUE_24BIT,
+					ctrl->val);
+		break;
+	case V4L2_CID_VBLANK:
+		ret = ov08x40_write_reg(ov08x, OV08X40_REG_VTS,
+					OV08X40_REG_VALUE_16BIT,
+					ov08x->cur_mode->height
+					+ ctrl->val);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		ret = ov08x40_enable_test_pattern(ov08x, ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+		ov08x40_set_ctrl_hflip(ov08x, ctrl->val);
+		break;
+	case V4L2_CID_VFLIP:
+		ov08x40_set_ctrl_vflip(ov08x, ctrl->val);
+		break;
+	default:
+		dev_info(&client->dev,
+			 "ctrl(id:0x%x,val:0x%x) is not handled\n",
+			 ctrl->id, ctrl->val);
+		break;
+	}
+
+	pm_runtime_put(&client->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ov08x40_ctrl_ops = {
+	.s_ctrl = ov08x40_set_ctrl,
+};
+
+static int ov08x40_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	/* Only one bayer order(GRBG) is supported */
+	if (code->index > 0)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+
+	return 0;
+}
+
+static int ov08x40_enum_frame_size(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->index >= ARRAY_SIZE(supported_modes))
+		return -EINVAL;
+
+	if (fse->code != MEDIA_BUS_FMT_SGRBG10_1X10)
+		return -EINVAL;
+
+	fse->min_width = supported_modes[fse->index].width;
+	fse->max_width = fse->min_width;
+	fse->min_height = supported_modes[fse->index].height;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static void ov08x40_update_pad_format(const struct ov08x40_mode *mode,
+				      struct v4l2_subdev_format *fmt)
+{
+	fmt->format.width = mode->width;
+	fmt->format.height = mode->height;
+	fmt->format.code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	fmt->format.field = V4L2_FIELD_NONE;
+}
+
+static int ov08x40_do_get_pad_format(struct ov08x40 *ov08x,
+				     struct v4l2_subdev_state *sd_state,
+				     struct v4l2_subdev_format *fmt)
+{
+	struct v4l2_mbus_framefmt *framefmt;
+	struct v4l2_subdev *sd = &ov08x->sd;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		framefmt = v4l2_subdev_get_try_format(sd, sd_state, fmt->pad);
+		fmt->format = *framefmt;
+	} else {
+		ov08x40_update_pad_format(ov08x->cur_mode, fmt);
+	}
+
+	return 0;
+}
+
+static int ov08x40_get_pad_format(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_format *fmt)
+{
+	struct ov08x40 *ov08x = to_ov08x40(sd);
+	int ret;
+
+	mutex_lock(&ov08x->mutex);
+	ret = ov08x40_do_get_pad_format(ov08x, sd_state, fmt);
+	mutex_unlock(&ov08x->mutex);
+
+	return ret;
+}
+
+static int
+ov08x40_set_pad_format(struct v4l2_subdev *sd,
+		       struct v4l2_subdev_state *sd_state,
+		       struct v4l2_subdev_format *fmt)
+{
+	struct ov08x40 *ov08x = to_ov08x40(sd);
+	const struct ov08x40_mode *mode;
+	struct v4l2_mbus_framefmt *framefmt;
+	s32 vblank_def;
+	s32 vblank_min;
+	s64 h_blank;
+	s64 pixel_rate;
+	s64 link_freq;
+
+	mutex_lock(&ov08x->mutex);
+
+	/* Only one raw bayer(GRBG) order is supported */
+	if (fmt->format.code != MEDIA_BUS_FMT_SGRBG10_1X10)
+		fmt->format.code = MEDIA_BUS_FMT_SGRBG10_1X10;
+
+	mode = v4l2_find_nearest_size(supported_modes,
+				      ARRAY_SIZE(supported_modes),
+				      width, height,
+				      fmt->format.width, fmt->format.height);
+	ov08x40_update_pad_format(mode, fmt);
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		framefmt = v4l2_subdev_get_try_format(sd, sd_state, fmt->pad);
+		*framefmt = fmt->format;
+	} else {
+		ov08x->cur_mode = mode;
+		__v4l2_ctrl_s_ctrl(ov08x->link_freq, mode->link_freq_index);
+		link_freq = link_freq_menu_items[mode->link_freq_index];
+		pixel_rate = link_freq_to_pixel_rate(link_freq);
+		__v4l2_ctrl_s_ctrl_int64(ov08x->pixel_rate, pixel_rate);
+
+		/* Update limits and set FPS to default */
+		vblank_def = ov08x->cur_mode->vts_def -
+			     ov08x->cur_mode->height;
+		vblank_min = ov08x->cur_mode->vts_min -
+			     ov08x->cur_mode->height;
+		__v4l2_ctrl_modify_range(ov08x->vblank, vblank_min,
+					 OV08X40_VTS_MAX
+					 - ov08x->cur_mode->height,
+					 1,
+					 vblank_def);
+		__v4l2_ctrl_s_ctrl(ov08x->vblank, vblank_def);
+		h_blank = ov08x->cur_mode->hts;
+		__v4l2_ctrl_modify_range(ov08x->hblank, h_blank,
+					 h_blank, 1, h_blank);
+	}
+
+	mutex_unlock(&ov08x->mutex);
+
+	return 0;
+}
+
+static int ov08x40_start_streaming(struct ov08x40 *ov08x)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov08x->sd);
+	const struct ov08x40_reg_list *reg_list;
+	int ret, link_freq_index;
+
+	/* Get out of from software reset */
+	ret = ov08x40_write_reg(ov08x, OV08X40_REG_SOFTWARE_RST,
+				OV08X40_REG_VALUE_08BIT, OV08X40_SOFTWARE_RST);
+	if (ret) {
+		dev_err(&client->dev, "%s failed to set powerup registers\n",
+			__func__);
+		return ret;
+	}
+
+	link_freq_index = ov08x->cur_mode->link_freq_index;
+	reg_list = &link_freq_configs[link_freq_index].reg_list;
+
+	ret = ov08x40_write_reg_list(ov08x, reg_list);
+	if (ret) {
+		dev_err(&client->dev, "%s failed to set plls\n", __func__);
+		return ret;
+	}
+
+	/* Apply default values of current mode */
+	reg_list = &ov08x->cur_mode->reg_list;
+	ret = ov08x40_write_reg_list(ov08x, reg_list);
+	if (ret) {
+		dev_err(&client->dev, "%s failed to set mode\n", __func__);
+		return ret;
+	}
+
+	/* Apply customized values from user */
+	ret =  __v4l2_ctrl_handler_setup(ov08x->sd.ctrl_handler);
+	if (ret)
+		return ret;
+
+	return ov08x40_write_reg(ov08x, OV08X40_REG_MODE_SELECT,
+				 OV08X40_REG_VALUE_08BIT,
+				 OV08X40_MODE_STREAMING);
+}
+
+/* Stop streaming */
+static int ov08x40_stop_streaming(struct ov08x40 *ov08x)
+{
+	return ov08x40_write_reg(ov08x, OV08X40_REG_MODE_SELECT,
+				 OV08X40_REG_VALUE_08BIT, OV08X40_MODE_STANDBY);
+}
+
+static int ov08x40_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct ov08x40 *ov08x = to_ov08x40(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	mutex_lock(&ov08x->mutex);
+	if (ov08x->streaming == enable) {
+		mutex_unlock(&ov08x->mutex);
+		return 0;
+	}
+
+	if (enable) {
+		ret = pm_runtime_resume_and_get(&client->dev);
+		if (ret < 0)
+			goto err_unlock;
+
+		/*
+		 * Apply default & customized values
+		 * and then start streaming.
+		 */
+		ret = ov08x40_start_streaming(ov08x);
+		if (ret)
+			goto err_rpm_put;
+	} else {
+		ov08x40_stop_streaming(ov08x);
+		pm_runtime_put(&client->dev);
+	}
+
+	ov08x->streaming = enable;
+	mutex_unlock(&ov08x->mutex);
+
+	return ret;
+
+err_rpm_put:
+	pm_runtime_put(&client->dev);
+err_unlock:
+	mutex_unlock(&ov08x->mutex);
+
+	return ret;
+}
+
+static int __maybe_unused ov08x40_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov08x40 *ov08x = to_ov08x40(sd);
+
+	if (ov08x->streaming)
+		ov08x40_stop_streaming(ov08x);
+
+	return 0;
+}
+
+static int __maybe_unused ov08x40_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov08x40 *ov08x = to_ov08x40(sd);
+	int ret;
+
+	if (ov08x->streaming) {
+		ret = ov08x40_start_streaming(ov08x);
+		if (ret)
+			goto error;
+	}
+
+	return 0;
+
+error:
+	ov08x40_stop_streaming(ov08x);
+	ov08x->streaming = false;
+	return ret;
+}
+
+/* Verify chip ID */
+static int ov08x40_identify_module(struct ov08x40 *ov08x)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov08x->sd);
+	int ret;
+	u32 val;
+
+	ret = ov08x40_read_reg(ov08x, OV08X40_REG_CHIP_ID,
+			       OV08X40_REG_VALUE_24BIT, &val);
+	if (ret)
+		return ret;
+
+	if (val != OV08X40_CHIP_ID) {
+		dev_err(&client->dev, "chip id mismatch: %x!=%x\n",
+			OV08X40_CHIP_ID, val);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops ov08x40_video_ops = {
+	.s_stream = ov08x40_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops ov08x40_pad_ops = {
+	.enum_mbus_code = ov08x40_enum_mbus_code,
+	.get_fmt = ov08x40_get_pad_format,
+	.set_fmt = ov08x40_set_pad_format,
+	.enum_frame_size = ov08x40_enum_frame_size,
+};
+
+static const struct v4l2_subdev_ops ov08x40_subdev_ops = {
+	.video = &ov08x40_video_ops,
+	.pad = &ov08x40_pad_ops,
+};
+
+static const struct media_entity_operations ov08x40_subdev_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static const struct v4l2_subdev_internal_ops ov08x40_internal_ops = {
+	.open = ov08x40_open,
+};
+
+static int ov08x40_init_controls(struct ov08x40 *ov08x)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov08x->sd);
+	struct v4l2_fwnode_device_properties props;
+	struct v4l2_ctrl_handler *ctrl_hdlr;
+	s64 exposure_max;
+	s64 vblank_def;
+	s64 vblank_min;
+	s64 hblank;
+	s64 pixel_rate_min;
+	s64 pixel_rate_max;
+	const struct ov08x40_mode *mode;
+	u32 max;
+	int ret;
+
+	ctrl_hdlr = &ov08x->ctrl_handler;
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 10);
+	if (ret)
+		return ret;
+
+	mutex_init(&ov08x->mutex);
+	ctrl_hdlr->lock = &ov08x->mutex;
+	max = ARRAY_SIZE(link_freq_menu_items) - 1;
+	ov08x->link_freq = v4l2_ctrl_new_int_menu(ctrl_hdlr,
+						  &ov08x40_ctrl_ops,
+						  V4L2_CID_LINK_FREQ,
+						  max,
+						  0,
+						  link_freq_menu_items);
+	if (ov08x->link_freq)
+		ov08x->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	pixel_rate_max = link_freq_to_pixel_rate(link_freq_menu_items[0]);
+	pixel_rate_min = 0;
+	/* By default, PIXEL_RATE is read only */
+	ov08x->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, &ov08x40_ctrl_ops,
+					      V4L2_CID_PIXEL_RATE,
+					      pixel_rate_min, pixel_rate_max,
+					      1, pixel_rate_max);
+
+	mode = ov08x->cur_mode;
+	vblank_def = mode->vts_def - mode->height;
+	vblank_min = mode->vts_min - mode->height;
+	ov08x->vblank = v4l2_ctrl_new_std(ctrl_hdlr, &ov08x40_ctrl_ops,
+					  V4L2_CID_VBLANK,
+					  vblank_min,
+					  OV08X40_VTS_MAX - mode->height, 1,
+					  vblank_def);
+
+	hblank = ov08x->cur_mode->hts;
+	ov08x->hblank = v4l2_ctrl_new_std(ctrl_hdlr, &ov08x40_ctrl_ops,
+					  V4L2_CID_HBLANK,
+					  hblank, hblank, 1, hblank);
+	if (ov08x->hblank)
+		ov08x->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	exposure_max = mode->vts_def - OV08X40_EXPOSURE_MAX_MARGIN;
+	ov08x->exposure = v4l2_ctrl_new_std(ctrl_hdlr, &ov08x40_ctrl_ops,
+					    V4L2_CID_EXPOSURE,
+					    OV08X40_EXPOSURE_MIN,
+					    exposure_max, OV08X40_EXPOSURE_STEP,
+					    exposure_max);
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov08x40_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
+			  OV08X40_ANA_GAIN_MIN, OV08X40_ANA_GAIN_MAX,
+			  OV08X40_ANA_GAIN_STEP, OV08X40_ANA_GAIN_DEFAULT);
+
+	/* Digital gain */
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov08x40_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
+			  OV08X40_DGTL_GAIN_MIN, OV08X40_DGTL_GAIN_MAX,
+			  OV08X40_DGTL_GAIN_STEP, OV08X40_DGTL_GAIN_DEFAULT);
+
+	v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &ov08x40_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(ov08x40_test_pattern_menu) - 1,
+				     0, 0, ov08x40_test_pattern_menu);
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov08x40_ctrl_ops,
+			  V4L2_CID_HFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov08x40_ctrl_ops,
+			  V4L2_CID_VFLIP, 0, 1, 1, 0);
+
+	if (ctrl_hdlr->error) {
+		ret = ctrl_hdlr->error;
+		dev_err(&client->dev, "%s control init failed (%d)\n",
+			__func__, ret);
+		goto error;
+	}
+
+	ret = v4l2_fwnode_device_parse(&client->dev, &props);
+	if (ret)
+		goto error;
+
+	ret = v4l2_ctrl_new_fwnode_properties(ctrl_hdlr, &ov08x40_ctrl_ops,
+					      &props);
+	if (ret)
+		goto error;
+
+	ov08x->sd.ctrl_handler = ctrl_hdlr;
+
+	return 0;
+
+error:
+	v4l2_ctrl_handler_free(ctrl_hdlr);
+	mutex_destroy(&ov08x->mutex);
+
+	return ret;
+}
+
+static void ov08x40_free_controls(struct ov08x40 *ov08x)
+{
+	v4l2_ctrl_handler_free(ov08x->sd.ctrl_handler);
+	mutex_destroy(&ov08x->mutex);
+}
+
+static int ov08x40_check_hwcfg(struct device *dev)
+{
+	struct v4l2_fwnode_endpoint bus_cfg = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY
+	};
+	struct fwnode_handle *ep;
+	struct fwnode_handle *fwnode = dev_fwnode(dev);
+	unsigned int i, j;
+	int ret;
+	u32 ext_clk;
+
+	if (!fwnode)
+		return -ENXIO;
+
+	ret = fwnode_property_read_u32(dev_fwnode(dev), "clock-frequency",
+				       &ext_clk);
+	if (ret) {
+		dev_err(dev, "can't get clock frequency");
+		return ret;
+	}
+
+	if (ext_clk != OV08X40_EXT_CLK) {
+		dev_err(dev, "external clock %d is not supported",
+			ext_clk);
+		return -EINVAL;
+	}
+
+	ep = fwnode_graph_get_next_endpoint(fwnode, NULL);
+	if (!ep)
+		return -ENXIO;
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(ep, &bus_cfg);
+	fwnode_handle_put(ep);
+	if (ret)
+		return ret;
+
+	if (bus_cfg.bus.mipi_csi2.num_data_lanes != OV08X40_DATA_LANES) {
+		dev_err(dev, "number of CSI2 data lanes %d is not supported",
+			bus_cfg.bus.mipi_csi2.num_data_lanes);
+		ret = -EINVAL;
+		goto out_err;
+	}
+
+	if (!bus_cfg.nr_of_link_frequencies) {
+		dev_err(dev, "no link frequencies defined");
+		ret = -EINVAL;
+		goto out_err;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(link_freq_menu_items); i++) {
+		for (j = 0; j < bus_cfg.nr_of_link_frequencies; j++) {
+			if (link_freq_menu_items[i] ==
+				bus_cfg.link_frequencies[j])
+				break;
+		}
+
+		if (j == bus_cfg.nr_of_link_frequencies) {
+			dev_err(dev, "no link frequency %lld supported",
+				link_freq_menu_items[i]);
+			ret = -EINVAL;
+			goto out_err;
+		}
+	}
+
+out_err:
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+
+	return ret;
+}
+
+static int ov08x40_probe(struct i2c_client *client)
+{
+	struct ov08x40 *ov08x;
+	int ret;
+
+	/* Check HW config */
+	ret = ov08x40_check_hwcfg(&client->dev);
+	if (ret) {
+		dev_err(&client->dev, "failed to check hwcfg: %d", ret);
+		return ret;
+	}
+
+	ov08x = devm_kzalloc(&client->dev, sizeof(*ov08x), GFP_KERNEL);
+	if (!ov08x)
+		return -ENOMEM;
+
+	/* Initialize subdev */
+	v4l2_i2c_subdev_init(&ov08x->sd, client, &ov08x40_subdev_ops);
+
+	/* Check module identity */
+	ret = ov08x40_identify_module(ov08x);
+	if (ret) {
+		dev_err(&client->dev, "failed to find sensor: %d\n", ret);
+		return ret;
+	}
+
+	/* Set default mode to max resolution */
+	ov08x->cur_mode = &supported_modes[0];
+
+	ret = ov08x40_init_controls(ov08x);
+	if (ret)
+		return ret;
+
+	/* Initialize subdev */
+	ov08x->sd.internal_ops = &ov08x40_internal_ops;
+	ov08x->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	ov08x->sd.entity.ops = &ov08x40_subdev_entity_ops;
+	ov08x->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+	/* Initialize source pad */
+	ov08x->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&ov08x->sd.entity, 1, &ov08x->pad);
+	if (ret) {
+		dev_err(&client->dev, "%s failed:%d\n", __func__, ret);
+		goto error_handler_free;
+	}
+
+	ret = v4l2_async_register_subdev_sensor(&ov08x->sd);
+	if (ret < 0)
+		goto error_media_entity;
+
+	/*
+	 * Device is already turned on by i2c-core with ACPI domain PM.
+	 * Enable runtime PM and turn off the device.
+	 */
+	pm_runtime_set_active(&client->dev);
+	pm_runtime_enable(&client->dev);
+	pm_runtime_idle(&client->dev);
+
+	return 0;
+
+error_media_entity:
+	media_entity_cleanup(&ov08x->sd.entity);
+
+error_handler_free:
+	ov08x40_free_controls(ov08x);
+
+	return ret;
+}
+
+static void ov08x40_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov08x40 *ov08x = to_ov08x40(sd);
+
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	ov08x40_free_controls(ov08x);
+
+	pm_runtime_disable(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+}
+
+static const struct dev_pm_ops ov08x40_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(ov08x40_suspend, ov08x40_resume)
+};
+
+#ifdef CONFIG_ACPI
+static const struct acpi_device_id ov08x40_acpi_ids[] = {
+	{"OVTI08F4"},
+	{ /* sentinel */ }
+};
+
+MODULE_DEVICE_TABLE(acpi, ov08x40_acpi_ids);
+#endif
+
+static struct i2c_driver ov08x40_i2c_driver = {
+	.driver = {
+		.name = "ov08x40",
+		.pm = &ov08x40_pm_ops,
+		.acpi_match_table = ACPI_PTR(ov08x40_acpi_ids),
+	},
+	.probe = ov08x40_probe,
+	.remove = ov08x40_remove,
+};
+
+module_i2c_driver(ov08x40_i2c_driver);
+
+MODULE_AUTHOR("Jason Chen <jason.z.chen@intel.com>");
+MODULE_AUTHOR("Shawn Tu");
+MODULE_DESCRIPTION("OmniVision OV08X40 sensor driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/ov13858.c b/drivers/media/i2c/ov13858.c
new file mode 100644
index 000000000..35652b362
--- /dev/null
+++ b/drivers/media/i2c/ov13858.c
@@ -0,0 +1,1820 @@
+// SPDX-License-Identifier: GPL-2.0
+// Copyright (c) 2017 Intel Corporation.
+
+#include <linux/acpi.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+
+#define OV13858_REG_VALUE_08BIT		1
+#define OV13858_REG_VALUE_16BIT		2
+#define OV13858_REG_VALUE_24BIT		3
+
+#define OV13858_REG_MODE_SELECT		0x0100
+#define OV13858_MODE_STANDBY		0x00
+#define OV13858_MODE_STREAMING		0x01
+
+#define OV13858_REG_SOFTWARE_RST	0x0103
+#define OV13858_SOFTWARE_RST		0x01
+
+/* PLL1 generates PCLK and MIPI_PHY_CLK */
+#define OV13858_REG_PLL1_CTRL_0		0x0300
+#define OV13858_REG_PLL1_CTRL_1		0x0301
+#define OV13858_REG_PLL1_CTRL_2		0x0302
+#define OV13858_REG_PLL1_CTRL_3		0x0303
+#define OV13858_REG_PLL1_CTRL_4		0x0304
+#define OV13858_REG_PLL1_CTRL_5		0x0305
+
+/* PLL2 generates DAC_CLK, SCLK and SRAM_CLK */
+#define OV13858_REG_PLL2_CTRL_B		0x030b
+#define OV13858_REG_PLL2_CTRL_C		0x030c
+#define OV13858_REG_PLL2_CTRL_D		0x030d
+#define OV13858_REG_PLL2_CTRL_E		0x030e
+#define OV13858_REG_PLL2_CTRL_F		0x030f
+#define OV13858_REG_PLL2_CTRL_12	0x0312
+#define OV13858_REG_MIPI_SC_CTRL0	0x3016
+#define OV13858_REG_MIPI_SC_CTRL1	0x3022
+
+/* Chip ID */
+#define OV13858_REG_CHIP_ID		0x300a
+#define OV13858_CHIP_ID			0x00d855
+
+/* V_TIMING internal */
+#define OV13858_REG_VTS			0x380e
+#define OV13858_VTS_30FPS		0x0c8e /* 30 fps */
+#define OV13858_VTS_60FPS		0x0648 /* 60 fps */
+#define OV13858_VTS_MAX			0x7fff
+
+/* HBLANK control - read only */
+#define OV13858_PPL_270MHZ		2244
+#define OV13858_PPL_540MHZ		4488
+
+/* Exposure control */
+#define OV13858_REG_EXPOSURE		0x3500
+#define OV13858_EXPOSURE_MIN		4
+#define OV13858_EXPOSURE_STEP		1
+#define OV13858_EXPOSURE_DEFAULT	0x640
+
+/* Analog gain control */
+#define OV13858_REG_ANALOG_GAIN		0x3508
+#define OV13858_ANA_GAIN_MIN		0
+#define OV13858_ANA_GAIN_MAX		0x1fff
+#define OV13858_ANA_GAIN_STEP		1
+#define OV13858_ANA_GAIN_DEFAULT	0x80
+
+/* Digital gain control */
+#define OV13858_REG_B_MWB_GAIN		0x5100
+#define OV13858_REG_G_MWB_GAIN		0x5102
+#define OV13858_REG_R_MWB_GAIN		0x5104
+#define OV13858_DGTL_GAIN_MIN		0
+#define OV13858_DGTL_GAIN_MAX		16384	/* Max = 16 X */
+#define OV13858_DGTL_GAIN_DEFAULT	1024	/* Default gain = 1 X */
+#define OV13858_DGTL_GAIN_STEP		1	/* Each step = 1/1024 */
+
+/* Test Pattern Control */
+#define OV13858_REG_TEST_PATTERN	0x4503
+#define OV13858_TEST_PATTERN_ENABLE	BIT(7)
+#define OV13858_TEST_PATTERN_MASK	0xfc
+
+/* Number of frames to skip */
+#define OV13858_NUM_OF_SKIP_FRAMES	2
+
+struct ov13858_reg {
+	u16 address;
+	u8 val;
+};
+
+struct ov13858_reg_list {
+	u32 num_of_regs;
+	const struct ov13858_reg *regs;
+};
+
+/* Link frequency config */
+struct ov13858_link_freq_config {
+	u32 pixels_per_line;
+
+	/* PLL registers for this link frequency */
+	struct ov13858_reg_list reg_list;
+};
+
+/* Mode : resolution and related config&values */
+struct ov13858_mode {
+	/* Frame width */
+	u32 width;
+	/* Frame height */
+	u32 height;
+
+	/* V-timing */
+	u32 vts_def;
+	u32 vts_min;
+
+	/* Index of Link frequency config to be used */
+	u32 link_freq_index;
+	/* Default register values */
+	struct ov13858_reg_list reg_list;
+};
+
+/* 4224x3136 needs 1080Mbps/lane, 4 lanes */
+static const struct ov13858_reg mipi_data_rate_1080mbps[] = {
+	/* PLL1 registers */
+	{OV13858_REG_PLL1_CTRL_0, 0x07},
+	{OV13858_REG_PLL1_CTRL_1, 0x01},
+	{OV13858_REG_PLL1_CTRL_2, 0xc2},
+	{OV13858_REG_PLL1_CTRL_3, 0x00},
+	{OV13858_REG_PLL1_CTRL_4, 0x00},
+	{OV13858_REG_PLL1_CTRL_5, 0x01},
+
+	/* PLL2 registers */
+	{OV13858_REG_PLL2_CTRL_B, 0x05},
+	{OV13858_REG_PLL2_CTRL_C, 0x01},
+	{OV13858_REG_PLL2_CTRL_D, 0x0e},
+	{OV13858_REG_PLL2_CTRL_E, 0x05},
+	{OV13858_REG_PLL2_CTRL_F, 0x01},
+	{OV13858_REG_PLL2_CTRL_12, 0x01},
+	{OV13858_REG_MIPI_SC_CTRL0, 0x72},
+	{OV13858_REG_MIPI_SC_CTRL1, 0x01},
+};
+
+/*
+ * 2112x1568, 2112x1188, 1056x784 need 540Mbps/lane,
+ * 4 lanes
+ */
+static const struct ov13858_reg mipi_data_rate_540mbps[] = {
+	/* PLL1 registers */
+	{OV13858_REG_PLL1_CTRL_0, 0x07},
+	{OV13858_REG_PLL1_CTRL_1, 0x01},
+	{OV13858_REG_PLL1_CTRL_2, 0xc2},
+	{OV13858_REG_PLL1_CTRL_3, 0x01},
+	{OV13858_REG_PLL1_CTRL_4, 0x00},
+	{OV13858_REG_PLL1_CTRL_5, 0x01},
+
+	/* PLL2 registers */
+	{OV13858_REG_PLL2_CTRL_B, 0x05},
+	{OV13858_REG_PLL2_CTRL_C, 0x01},
+	{OV13858_REG_PLL2_CTRL_D, 0x0e},
+	{OV13858_REG_PLL2_CTRL_E, 0x05},
+	{OV13858_REG_PLL2_CTRL_F, 0x01},
+	{OV13858_REG_PLL2_CTRL_12, 0x01},
+	{OV13858_REG_MIPI_SC_CTRL0, 0x72},
+	{OV13858_REG_MIPI_SC_CTRL1, 0x01},
+};
+
+static const struct ov13858_reg mode_4224x3136_regs[] = {
+	{0x3013, 0x32},
+	{0x301b, 0xf0},
+	{0x301f, 0xd0},
+	{0x3106, 0x15},
+	{0x3107, 0x23},
+	{0x350a, 0x00},
+	{0x350e, 0x00},
+	{0x3510, 0x00},
+	{0x3511, 0x02},
+	{0x3512, 0x00},
+	{0x3600, 0x2b},
+	{0x3601, 0x52},
+	{0x3602, 0x60},
+	{0x3612, 0x05},
+	{0x3613, 0xa4},
+	{0x3620, 0x80},
+	{0x3621, 0x10},
+	{0x3622, 0x30},
+	{0x3624, 0x1c},
+	{0x3640, 0x10},
+	{0x3641, 0x70},
+	{0x3660, 0x04},
+	{0x3661, 0x80},
+	{0x3662, 0x12},
+	{0x3664, 0x73},
+	{0x3665, 0xa7},
+	{0x366e, 0xff},
+	{0x366f, 0xf4},
+	{0x3674, 0x00},
+	{0x3679, 0x0c},
+	{0x367f, 0x01},
+	{0x3680, 0x0c},
+	{0x3681, 0x50},
+	{0x3682, 0x50},
+	{0x3683, 0xa9},
+	{0x3684, 0xa9},
+	{0x3709, 0x5f},
+	{0x3714, 0x24},
+	{0x371a, 0x3e},
+	{0x3737, 0x04},
+	{0x3738, 0xcc},
+	{0x3739, 0x12},
+	{0x373d, 0x26},
+	{0x3764, 0x20},
+	{0x3765, 0x20},
+	{0x37a1, 0x36},
+	{0x37a8, 0x3b},
+	{0x37ab, 0x31},
+	{0x37c2, 0x04},
+	{0x37c3, 0xf1},
+	{0x37c5, 0x00},
+	{0x37d8, 0x03},
+	{0x37d9, 0x0c},
+	{0x37da, 0xc2},
+	{0x37dc, 0x02},
+	{0x37e0, 0x00},
+	{0x37e1, 0x0a},
+	{0x37e2, 0x14},
+	{0x37e3, 0x04},
+	{0x37e4, 0x2a},
+	{0x37e5, 0x03},
+	{0x37e6, 0x04},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x08},
+	{0x3804, 0x10},
+	{0x3805, 0x9f},
+	{0x3806, 0x0c},
+	{0x3807, 0x57},
+	{0x3808, 0x10},
+	{0x3809, 0x80},
+	{0x380a, 0x0c},
+	{0x380b, 0x40},
+	{0x380c, 0x04},
+	{0x380d, 0x62},
+	{0x380e, 0x0c},
+	{0x380f, 0x8e},
+	{0x3811, 0x04},
+	{0x3813, 0x05},
+	{0x3814, 0x01},
+	{0x3815, 0x01},
+	{0x3816, 0x01},
+	{0x3817, 0x01},
+	{0x3820, 0xa8},
+	{0x3821, 0x00},
+	{0x3822, 0xc2},
+	{0x3823, 0x18},
+	{0x3826, 0x11},
+	{0x3827, 0x1c},
+	{0x3829, 0x03},
+	{0x3832, 0x00},
+	{0x3c80, 0x00},
+	{0x3c87, 0x01},
+	{0x3c8c, 0x19},
+	{0x3c8d, 0x1c},
+	{0x3c90, 0x00},
+	{0x3c91, 0x00},
+	{0x3c92, 0x00},
+	{0x3c93, 0x00},
+	{0x3c94, 0x40},
+	{0x3c95, 0x54},
+	{0x3c96, 0x34},
+	{0x3c97, 0x04},
+	{0x3c98, 0x00},
+	{0x3d8c, 0x73},
+	{0x3d8d, 0xc0},
+	{0x3f00, 0x0b},
+	{0x3f03, 0x00},
+	{0x4001, 0xe0},
+	{0x4008, 0x00},
+	{0x4009, 0x0f},
+	{0x4011, 0xf0},
+	{0x4017, 0x08},
+	{0x4050, 0x04},
+	{0x4051, 0x0b},
+	{0x4052, 0x00},
+	{0x4053, 0x80},
+	{0x4054, 0x00},
+	{0x4055, 0x80},
+	{0x4056, 0x00},
+	{0x4057, 0x80},
+	{0x4058, 0x00},
+	{0x4059, 0x80},
+	{0x405e, 0x20},
+	{0x4500, 0x07},
+	{0x4503, 0x00},
+	{0x450a, 0x04},
+	{0x4809, 0x04},
+	{0x480c, 0x12},
+	{0x481f, 0x30},
+	{0x4833, 0x10},
+	{0x4837, 0x0e},
+	{0x4902, 0x01},
+	{0x4d00, 0x03},
+	{0x4d01, 0xc9},
+	{0x4d02, 0xbc},
+	{0x4d03, 0xd7},
+	{0x4d04, 0xf0},
+	{0x4d05, 0xa2},
+	{0x5000, 0xfd},
+	{0x5001, 0x01},
+	{0x5040, 0x39},
+	{0x5041, 0x10},
+	{0x5042, 0x10},
+	{0x5043, 0x84},
+	{0x5044, 0x62},
+	{0x5180, 0x00},
+	{0x5181, 0x10},
+	{0x5182, 0x02},
+	{0x5183, 0x0f},
+	{0x5200, 0x1b},
+	{0x520b, 0x07},
+	{0x520c, 0x0f},
+	{0x5300, 0x04},
+	{0x5301, 0x0c},
+	{0x5302, 0x0c},
+	{0x5303, 0x0f},
+	{0x5304, 0x00},
+	{0x5305, 0x70},
+	{0x5306, 0x00},
+	{0x5307, 0x80},
+	{0x5308, 0x00},
+	{0x5309, 0xa5},
+	{0x530a, 0x00},
+	{0x530b, 0xd3},
+	{0x530c, 0x00},
+	{0x530d, 0xf0},
+	{0x530e, 0x01},
+	{0x530f, 0x10},
+	{0x5310, 0x01},
+	{0x5311, 0x20},
+	{0x5312, 0x01},
+	{0x5313, 0x20},
+	{0x5314, 0x01},
+	{0x5315, 0x20},
+	{0x5316, 0x08},
+	{0x5317, 0x08},
+	{0x5318, 0x10},
+	{0x5319, 0x88},
+	{0x531a, 0x88},
+	{0x531b, 0xa9},
+	{0x531c, 0xaa},
+	{0x531d, 0x0a},
+	{0x5405, 0x02},
+	{0x5406, 0x67},
+	{0x5407, 0x01},
+	{0x5408, 0x4a},
+};
+
+static const struct ov13858_reg mode_2112x1568_regs[] = {
+	{0x3013, 0x32},
+	{0x301b, 0xf0},
+	{0x301f, 0xd0},
+	{0x3106, 0x15},
+	{0x3107, 0x23},
+	{0x350a, 0x00},
+	{0x350e, 0x00},
+	{0x3510, 0x00},
+	{0x3511, 0x02},
+	{0x3512, 0x00},
+	{0x3600, 0x2b},
+	{0x3601, 0x52},
+	{0x3602, 0x60},
+	{0x3612, 0x05},
+	{0x3613, 0xa4},
+	{0x3620, 0x80},
+	{0x3621, 0x10},
+	{0x3622, 0x30},
+	{0x3624, 0x1c},
+	{0x3640, 0x10},
+	{0x3641, 0x70},
+	{0x3660, 0x04},
+	{0x3661, 0x80},
+	{0x3662, 0x10},
+	{0x3664, 0x73},
+	{0x3665, 0xa7},
+	{0x366e, 0xff},
+	{0x366f, 0xf4},
+	{0x3674, 0x00},
+	{0x3679, 0x0c},
+	{0x367f, 0x01},
+	{0x3680, 0x0c},
+	{0x3681, 0x50},
+	{0x3682, 0x50},
+	{0x3683, 0xa9},
+	{0x3684, 0xa9},
+	{0x3709, 0x5f},
+	{0x3714, 0x28},
+	{0x371a, 0x3e},
+	{0x3737, 0x08},
+	{0x3738, 0xcc},
+	{0x3739, 0x20},
+	{0x373d, 0x26},
+	{0x3764, 0x20},
+	{0x3765, 0x20},
+	{0x37a1, 0x36},
+	{0x37a8, 0x3b},
+	{0x37ab, 0x31},
+	{0x37c2, 0x14},
+	{0x37c3, 0xf1},
+	{0x37c5, 0x00},
+	{0x37d8, 0x03},
+	{0x37d9, 0x0c},
+	{0x37da, 0xc2},
+	{0x37dc, 0x02},
+	{0x37e0, 0x00},
+	{0x37e1, 0x0a},
+	{0x37e2, 0x14},
+	{0x37e3, 0x08},
+	{0x37e4, 0x38},
+	{0x37e5, 0x03},
+	{0x37e6, 0x08},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x00},
+	{0x3804, 0x10},
+	{0x3805, 0x9f},
+	{0x3806, 0x0c},
+	{0x3807, 0x5f},
+	{0x3808, 0x08},
+	{0x3809, 0x40},
+	{0x380a, 0x06},
+	{0x380b, 0x20},
+	{0x380c, 0x04},
+	{0x380d, 0x62},
+	{0x380e, 0x0c},
+	{0x380f, 0x8e},
+	{0x3811, 0x04},
+	{0x3813, 0x05},
+	{0x3814, 0x03},
+	{0x3815, 0x01},
+	{0x3816, 0x03},
+	{0x3817, 0x01},
+	{0x3820, 0xab},
+	{0x3821, 0x00},
+	{0x3822, 0xc2},
+	{0x3823, 0x18},
+	{0x3826, 0x04},
+	{0x3827, 0x90},
+	{0x3829, 0x07},
+	{0x3832, 0x00},
+	{0x3c80, 0x00},
+	{0x3c87, 0x01},
+	{0x3c8c, 0x19},
+	{0x3c8d, 0x1c},
+	{0x3c90, 0x00},
+	{0x3c91, 0x00},
+	{0x3c92, 0x00},
+	{0x3c93, 0x00},
+	{0x3c94, 0x40},
+	{0x3c95, 0x54},
+	{0x3c96, 0x34},
+	{0x3c97, 0x04},
+	{0x3c98, 0x00},
+	{0x3d8c, 0x73},
+	{0x3d8d, 0xc0},
+	{0x3f00, 0x0b},
+	{0x3f03, 0x00},
+	{0x4001, 0xe0},
+	{0x4008, 0x00},
+	{0x4009, 0x0d},
+	{0x4011, 0xf0},
+	{0x4017, 0x08},
+	{0x4050, 0x04},
+	{0x4051, 0x0b},
+	{0x4052, 0x00},
+	{0x4053, 0x80},
+	{0x4054, 0x00},
+	{0x4055, 0x80},
+	{0x4056, 0x00},
+	{0x4057, 0x80},
+	{0x4058, 0x00},
+	{0x4059, 0x80},
+	{0x405e, 0x20},
+	{0x4500, 0x07},
+	{0x4503, 0x00},
+	{0x450a, 0x04},
+	{0x4809, 0x04},
+	{0x480c, 0x12},
+	{0x481f, 0x30},
+	{0x4833, 0x10},
+	{0x4837, 0x1c},
+	{0x4902, 0x01},
+	{0x4d00, 0x03},
+	{0x4d01, 0xc9},
+	{0x4d02, 0xbc},
+	{0x4d03, 0xd7},
+	{0x4d04, 0xf0},
+	{0x4d05, 0xa2},
+	{0x5000, 0xfd},
+	{0x5001, 0x01},
+	{0x5040, 0x39},
+	{0x5041, 0x10},
+	{0x5042, 0x10},
+	{0x5043, 0x84},
+	{0x5044, 0x62},
+	{0x5180, 0x00},
+	{0x5181, 0x10},
+	{0x5182, 0x02},
+	{0x5183, 0x0f},
+	{0x5200, 0x1b},
+	{0x520b, 0x07},
+	{0x520c, 0x0f},
+	{0x5300, 0x04},
+	{0x5301, 0x0c},
+	{0x5302, 0x0c},
+	{0x5303, 0x0f},
+	{0x5304, 0x00},
+	{0x5305, 0x70},
+	{0x5306, 0x00},
+	{0x5307, 0x80},
+	{0x5308, 0x00},
+	{0x5309, 0xa5},
+	{0x530a, 0x00},
+	{0x530b, 0xd3},
+	{0x530c, 0x00},
+	{0x530d, 0xf0},
+	{0x530e, 0x01},
+	{0x530f, 0x10},
+	{0x5310, 0x01},
+	{0x5311, 0x20},
+	{0x5312, 0x01},
+	{0x5313, 0x20},
+	{0x5314, 0x01},
+	{0x5315, 0x20},
+	{0x5316, 0x08},
+	{0x5317, 0x08},
+	{0x5318, 0x10},
+	{0x5319, 0x88},
+	{0x531a, 0x88},
+	{0x531b, 0xa9},
+	{0x531c, 0xaa},
+	{0x531d, 0x0a},
+	{0x5405, 0x02},
+	{0x5406, 0x67},
+	{0x5407, 0x01},
+	{0x5408, 0x4a},
+};
+
+static const struct ov13858_reg mode_2112x1188_regs[] = {
+	{0x3013, 0x32},
+	{0x301b, 0xf0},
+	{0x301f, 0xd0},
+	{0x3106, 0x15},
+	{0x3107, 0x23},
+	{0x350a, 0x00},
+	{0x350e, 0x00},
+	{0x3510, 0x00},
+	{0x3511, 0x02},
+	{0x3512, 0x00},
+	{0x3600, 0x2b},
+	{0x3601, 0x52},
+	{0x3602, 0x60},
+	{0x3612, 0x05},
+	{0x3613, 0xa4},
+	{0x3620, 0x80},
+	{0x3621, 0x10},
+	{0x3622, 0x30},
+	{0x3624, 0x1c},
+	{0x3640, 0x10},
+	{0x3641, 0x70},
+	{0x3660, 0x04},
+	{0x3661, 0x80},
+	{0x3662, 0x10},
+	{0x3664, 0x73},
+	{0x3665, 0xa7},
+	{0x366e, 0xff},
+	{0x366f, 0xf4},
+	{0x3674, 0x00},
+	{0x3679, 0x0c},
+	{0x367f, 0x01},
+	{0x3680, 0x0c},
+	{0x3681, 0x50},
+	{0x3682, 0x50},
+	{0x3683, 0xa9},
+	{0x3684, 0xa9},
+	{0x3709, 0x5f},
+	{0x3714, 0x28},
+	{0x371a, 0x3e},
+	{0x3737, 0x08},
+	{0x3738, 0xcc},
+	{0x3739, 0x20},
+	{0x373d, 0x26},
+	{0x3764, 0x20},
+	{0x3765, 0x20},
+	{0x37a1, 0x36},
+	{0x37a8, 0x3b},
+	{0x37ab, 0x31},
+	{0x37c2, 0x14},
+	{0x37c3, 0xf1},
+	{0x37c5, 0x00},
+	{0x37d8, 0x03},
+	{0x37d9, 0x0c},
+	{0x37da, 0xc2},
+	{0x37dc, 0x02},
+	{0x37e0, 0x00},
+	{0x37e1, 0x0a},
+	{0x37e2, 0x14},
+	{0x37e3, 0x08},
+	{0x37e4, 0x38},
+	{0x37e5, 0x03},
+	{0x37e6, 0x08},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x01},
+	{0x3803, 0x84},
+	{0x3804, 0x10},
+	{0x3805, 0x9f},
+	{0x3806, 0x0a},
+	{0x3807, 0xd3},
+	{0x3808, 0x08},
+	{0x3809, 0x40},
+	{0x380a, 0x04},
+	{0x380b, 0xa4},
+	{0x380c, 0x04},
+	{0x380d, 0x62},
+	{0x380e, 0x0c},
+	{0x380f, 0x8e},
+	{0x3811, 0x08},
+	{0x3813, 0x03},
+	{0x3814, 0x03},
+	{0x3815, 0x01},
+	{0x3816, 0x03},
+	{0x3817, 0x01},
+	{0x3820, 0xab},
+	{0x3821, 0x00},
+	{0x3822, 0xc2},
+	{0x3823, 0x18},
+	{0x3826, 0x04},
+	{0x3827, 0x90},
+	{0x3829, 0x07},
+	{0x3832, 0x00},
+	{0x3c80, 0x00},
+	{0x3c87, 0x01},
+	{0x3c8c, 0x19},
+	{0x3c8d, 0x1c},
+	{0x3c90, 0x00},
+	{0x3c91, 0x00},
+	{0x3c92, 0x00},
+	{0x3c93, 0x00},
+	{0x3c94, 0x40},
+	{0x3c95, 0x54},
+	{0x3c96, 0x34},
+	{0x3c97, 0x04},
+	{0x3c98, 0x00},
+	{0x3d8c, 0x73},
+	{0x3d8d, 0xc0},
+	{0x3f00, 0x0b},
+	{0x3f03, 0x00},
+	{0x4001, 0xe0},
+	{0x4008, 0x00},
+	{0x4009, 0x0d},
+	{0x4011, 0xf0},
+	{0x4017, 0x08},
+	{0x4050, 0x04},
+	{0x4051, 0x0b},
+	{0x4052, 0x00},
+	{0x4053, 0x80},
+	{0x4054, 0x00},
+	{0x4055, 0x80},
+	{0x4056, 0x00},
+	{0x4057, 0x80},
+	{0x4058, 0x00},
+	{0x4059, 0x80},
+	{0x405e, 0x20},
+	{0x4500, 0x07},
+	{0x4503, 0x00},
+	{0x450a, 0x04},
+	{0x4809, 0x04},
+	{0x480c, 0x12},
+	{0x481f, 0x30},
+	{0x4833, 0x10},
+	{0x4837, 0x1c},
+	{0x4902, 0x01},
+	{0x4d00, 0x03},
+	{0x4d01, 0xc9},
+	{0x4d02, 0xbc},
+	{0x4d03, 0xd7},
+	{0x4d04, 0xf0},
+	{0x4d05, 0xa2},
+	{0x5000, 0xfd},
+	{0x5001, 0x01},
+	{0x5040, 0x39},
+	{0x5041, 0x10},
+	{0x5042, 0x10},
+	{0x5043, 0x84},
+	{0x5044, 0x62},
+	{0x5180, 0x00},
+	{0x5181, 0x10},
+	{0x5182, 0x02},
+	{0x5183, 0x0f},
+	{0x5200, 0x1b},
+	{0x520b, 0x07},
+	{0x520c, 0x0f},
+	{0x5300, 0x04},
+	{0x5301, 0x0c},
+	{0x5302, 0x0c},
+	{0x5303, 0x0f},
+	{0x5304, 0x00},
+	{0x5305, 0x70},
+	{0x5306, 0x00},
+	{0x5307, 0x80},
+	{0x5308, 0x00},
+	{0x5309, 0xa5},
+	{0x530a, 0x00},
+	{0x530b, 0xd3},
+	{0x530c, 0x00},
+	{0x530d, 0xf0},
+	{0x530e, 0x01},
+	{0x530f, 0x10},
+	{0x5310, 0x01},
+	{0x5311, 0x20},
+	{0x5312, 0x01},
+	{0x5313, 0x20},
+	{0x5314, 0x01},
+	{0x5315, 0x20},
+	{0x5316, 0x08},
+	{0x5317, 0x08},
+	{0x5318, 0x10},
+	{0x5319, 0x88},
+	{0x531a, 0x88},
+	{0x531b, 0xa9},
+	{0x531c, 0xaa},
+	{0x531d, 0x0a},
+	{0x5405, 0x02},
+	{0x5406, 0x67},
+	{0x5407, 0x01},
+	{0x5408, 0x4a},
+};
+
+static const struct ov13858_reg mode_1056x784_regs[] = {
+	{0x3013, 0x32},
+	{0x301b, 0xf0},
+	{0x301f, 0xd0},
+	{0x3106, 0x15},
+	{0x3107, 0x23},
+	{0x350a, 0x00},
+	{0x350e, 0x00},
+	{0x3510, 0x00},
+	{0x3511, 0x02},
+	{0x3512, 0x00},
+	{0x3600, 0x2b},
+	{0x3601, 0x52},
+	{0x3602, 0x60},
+	{0x3612, 0x05},
+	{0x3613, 0xa4},
+	{0x3620, 0x80},
+	{0x3621, 0x10},
+	{0x3622, 0x30},
+	{0x3624, 0x1c},
+	{0x3640, 0x10},
+	{0x3641, 0x70},
+	{0x3660, 0x04},
+	{0x3661, 0x80},
+	{0x3662, 0x08},
+	{0x3664, 0x73},
+	{0x3665, 0xa7},
+	{0x366e, 0xff},
+	{0x366f, 0xf4},
+	{0x3674, 0x00},
+	{0x3679, 0x0c},
+	{0x367f, 0x01},
+	{0x3680, 0x0c},
+	{0x3681, 0x50},
+	{0x3682, 0x50},
+	{0x3683, 0xa9},
+	{0x3684, 0xa9},
+	{0x3709, 0x5f},
+	{0x3714, 0x30},
+	{0x371a, 0x3e},
+	{0x3737, 0x08},
+	{0x3738, 0xcc},
+	{0x3739, 0x20},
+	{0x373d, 0x26},
+	{0x3764, 0x20},
+	{0x3765, 0x20},
+	{0x37a1, 0x36},
+	{0x37a8, 0x3b},
+	{0x37ab, 0x31},
+	{0x37c2, 0x2c},
+	{0x37c3, 0xf1},
+	{0x37c5, 0x00},
+	{0x37d8, 0x03},
+	{0x37d9, 0x06},
+	{0x37da, 0xc2},
+	{0x37dc, 0x02},
+	{0x37e0, 0x00},
+	{0x37e1, 0x0a},
+	{0x37e2, 0x14},
+	{0x37e3, 0x08},
+	{0x37e4, 0x36},
+	{0x37e5, 0x03},
+	{0x37e6, 0x08},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x00},
+	{0x3804, 0x10},
+	{0x3805, 0x9f},
+	{0x3806, 0x0c},
+	{0x3807, 0x5f},
+	{0x3808, 0x04},
+	{0x3809, 0x20},
+	{0x380a, 0x03},
+	{0x380b, 0x10},
+	{0x380c, 0x04},
+	{0x380d, 0x62},
+	{0x380e, 0x0c},
+	{0x380f, 0x8e},
+	{0x3811, 0x04},
+	{0x3813, 0x05},
+	{0x3814, 0x07},
+	{0x3815, 0x01},
+	{0x3816, 0x07},
+	{0x3817, 0x01},
+	{0x3820, 0xac},
+	{0x3821, 0x00},
+	{0x3822, 0xc2},
+	{0x3823, 0x18},
+	{0x3826, 0x04},
+	{0x3827, 0x48},
+	{0x3829, 0x03},
+	{0x3832, 0x00},
+	{0x3c80, 0x00},
+	{0x3c87, 0x01},
+	{0x3c8c, 0x19},
+	{0x3c8d, 0x1c},
+	{0x3c90, 0x00},
+	{0x3c91, 0x00},
+	{0x3c92, 0x00},
+	{0x3c93, 0x00},
+	{0x3c94, 0x40},
+	{0x3c95, 0x54},
+	{0x3c96, 0x34},
+	{0x3c97, 0x04},
+	{0x3c98, 0x00},
+	{0x3d8c, 0x73},
+	{0x3d8d, 0xc0},
+	{0x3f00, 0x0b},
+	{0x3f03, 0x00},
+	{0x4001, 0xe0},
+	{0x4008, 0x00},
+	{0x4009, 0x05},
+	{0x4011, 0xf0},
+	{0x4017, 0x08},
+	{0x4050, 0x02},
+	{0x4051, 0x05},
+	{0x4052, 0x00},
+	{0x4053, 0x80},
+	{0x4054, 0x00},
+	{0x4055, 0x80},
+	{0x4056, 0x00},
+	{0x4057, 0x80},
+	{0x4058, 0x00},
+	{0x4059, 0x80},
+	{0x405e, 0x20},
+	{0x4500, 0x07},
+	{0x4503, 0x00},
+	{0x450a, 0x04},
+	{0x4809, 0x04},
+	{0x480c, 0x12},
+	{0x481f, 0x30},
+	{0x4833, 0x10},
+	{0x4837, 0x1e},
+	{0x4902, 0x02},
+	{0x4d00, 0x03},
+	{0x4d01, 0xc9},
+	{0x4d02, 0xbc},
+	{0x4d03, 0xd7},
+	{0x4d04, 0xf0},
+	{0x4d05, 0xa2},
+	{0x5000, 0xfd},
+	{0x5001, 0x01},
+	{0x5040, 0x39},
+	{0x5041, 0x10},
+	{0x5042, 0x10},
+	{0x5043, 0x84},
+	{0x5044, 0x62},
+	{0x5180, 0x00},
+	{0x5181, 0x10},
+	{0x5182, 0x02},
+	{0x5183, 0x0f},
+	{0x5200, 0x1b},
+	{0x520b, 0x07},
+	{0x520c, 0x0f},
+	{0x5300, 0x04},
+	{0x5301, 0x0c},
+	{0x5302, 0x0c},
+	{0x5303, 0x0f},
+	{0x5304, 0x00},
+	{0x5305, 0x70},
+	{0x5306, 0x00},
+	{0x5307, 0x80},
+	{0x5308, 0x00},
+	{0x5309, 0xa5},
+	{0x530a, 0x00},
+	{0x530b, 0xd3},
+	{0x530c, 0x00},
+	{0x530d, 0xf0},
+	{0x530e, 0x01},
+	{0x530f, 0x10},
+	{0x5310, 0x01},
+	{0x5311, 0x20},
+	{0x5312, 0x01},
+	{0x5313, 0x20},
+	{0x5314, 0x01},
+	{0x5315, 0x20},
+	{0x5316, 0x08},
+	{0x5317, 0x08},
+	{0x5318, 0x10},
+	{0x5319, 0x88},
+	{0x531a, 0x88},
+	{0x531b, 0xa9},
+	{0x531c, 0xaa},
+	{0x531d, 0x0a},
+	{0x5405, 0x02},
+	{0x5406, 0x67},
+	{0x5407, 0x01},
+	{0x5408, 0x4a},
+};
+
+static const char * const ov13858_test_pattern_menu[] = {
+	"Disabled",
+	"Vertical Color Bar Type 1",
+	"Vertical Color Bar Type 2",
+	"Vertical Color Bar Type 3",
+	"Vertical Color Bar Type 4"
+};
+
+/* Configurations for supported link frequencies */
+#define OV13858_NUM_OF_LINK_FREQS	2
+#define OV13858_LINK_FREQ_540MHZ	540000000ULL
+#define OV13858_LINK_FREQ_270MHZ	270000000ULL
+#define OV13858_LINK_FREQ_INDEX_0	0
+#define OV13858_LINK_FREQ_INDEX_1	1
+
+/*
+ * pixel_rate = link_freq * data-rate * nr_of_lanes / bits_per_sample
+ * data rate => double data rate; number of lanes => 4; bits per pixel => 10
+ */
+static u64 link_freq_to_pixel_rate(u64 f)
+{
+	f *= 2 * 4;
+	do_div(f, 10);
+
+	return f;
+}
+
+/* Menu items for LINK_FREQ V4L2 control */
+static const s64 link_freq_menu_items[OV13858_NUM_OF_LINK_FREQS] = {
+	OV13858_LINK_FREQ_540MHZ,
+	OV13858_LINK_FREQ_270MHZ
+};
+
+/* Link frequency configs */
+static const struct ov13858_link_freq_config
+			link_freq_configs[OV13858_NUM_OF_LINK_FREQS] = {
+	{
+		.pixels_per_line = OV13858_PPL_540MHZ,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mipi_data_rate_1080mbps),
+			.regs = mipi_data_rate_1080mbps,
+		}
+	},
+	{
+		.pixels_per_line = OV13858_PPL_270MHZ,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mipi_data_rate_540mbps),
+			.regs = mipi_data_rate_540mbps,
+		}
+	}
+};
+
+/* Mode configs */
+static const struct ov13858_mode supported_modes[] = {
+	{
+		.width = 4224,
+		.height = 3136,
+		.vts_def = OV13858_VTS_30FPS,
+		.vts_min = OV13858_VTS_30FPS,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_4224x3136_regs),
+			.regs = mode_4224x3136_regs,
+		},
+		.link_freq_index = OV13858_LINK_FREQ_INDEX_0,
+	},
+	{
+		.width = 2112,
+		.height = 1568,
+		.vts_def = OV13858_VTS_30FPS,
+		.vts_min = 1608,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_2112x1568_regs),
+			.regs = mode_2112x1568_regs,
+		},
+		.link_freq_index = OV13858_LINK_FREQ_INDEX_1,
+	},
+	{
+		.width = 2112,
+		.height = 1188,
+		.vts_def = OV13858_VTS_30FPS,
+		.vts_min = 1608,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_2112x1188_regs),
+			.regs = mode_2112x1188_regs,
+		},
+		.link_freq_index = OV13858_LINK_FREQ_INDEX_1,
+	},
+	{
+		.width = 1056,
+		.height = 784,
+		.vts_def = OV13858_VTS_30FPS,
+		.vts_min = 804,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1056x784_regs),
+			.regs = mode_1056x784_regs,
+		},
+		.link_freq_index = OV13858_LINK_FREQ_INDEX_1,
+	}
+};
+
+struct ov13858 {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+
+	struct v4l2_ctrl_handler ctrl_handler;
+	/* V4L2 Controls */
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *pixel_rate;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *exposure;
+
+	/* Current mode */
+	const struct ov13858_mode *cur_mode;
+
+	/* Mutex for serialized access */
+	struct mutex mutex;
+
+	/* Streaming on/off */
+	bool streaming;
+};
+
+#define to_ov13858(_sd)	container_of(_sd, struct ov13858, sd)
+
+/* Read registers up to 4 at a time */
+static int ov13858_read_reg(struct ov13858 *ov13858, u16 reg, u32 len,
+			    u32 *val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov13858->sd);
+	struct i2c_msg msgs[2];
+	u8 *data_be_p;
+	int ret;
+	__be32 data_be = 0;
+	__be16 reg_addr_be = cpu_to_be16(reg);
+
+	if (len > 4)
+		return -EINVAL;
+
+	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[4 - len];
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret != ARRAY_SIZE(msgs))
+		return -EIO;
+
+	*val = be32_to_cpu(data_be);
+
+	return 0;
+}
+
+/* Write registers up to 4 at a time */
+static int ov13858_write_reg(struct ov13858 *ov13858, u16 reg, u32 len,
+			     u32 __val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov13858->sd);
+	int buf_i, val_i;
+	u8 buf[6], *val_p;
+	__be32 val;
+
+	if (len > 4)
+		return -EINVAL;
+
+	buf[0] = reg >> 8;
+	buf[1] = reg & 0xff;
+
+	val = cpu_to_be32(__val);
+	val_p = (u8 *)&val;
+	buf_i = 2;
+	val_i = 4 - len;
+
+	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;
+}
+
+/* Write a list of registers */
+static int ov13858_write_regs(struct ov13858 *ov13858,
+			      const struct ov13858_reg *regs, u32 len)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov13858->sd);
+	int ret;
+	u32 i;
+
+	for (i = 0; i < len; i++) {
+		ret = ov13858_write_reg(ov13858, regs[i].address, 1,
+					regs[i].val);
+		if (ret) {
+			dev_err_ratelimited(
+				&client->dev,
+				"Failed to write reg 0x%4.4x. error = %d\n",
+				regs[i].address, ret);
+
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int ov13858_write_reg_list(struct ov13858 *ov13858,
+				  const struct ov13858_reg_list *r_list)
+{
+	return ov13858_write_regs(ov13858, r_list->regs, r_list->num_of_regs);
+}
+
+/* Open sub-device */
+static int ov13858_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct ov13858 *ov13858 = to_ov13858(sd);
+	struct v4l2_mbus_framefmt *try_fmt = v4l2_subdev_get_try_format(sd,
+									fh->state,
+									0);
+
+	mutex_lock(&ov13858->mutex);
+
+	/* Initialize try_fmt */
+	try_fmt->width = ov13858->cur_mode->width;
+	try_fmt->height = ov13858->cur_mode->height;
+	try_fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	try_fmt->field = V4L2_FIELD_NONE;
+
+	/* No crop or compose */
+	mutex_unlock(&ov13858->mutex);
+
+	return 0;
+}
+
+static int ov13858_update_digital_gain(struct ov13858 *ov13858, u32 d_gain)
+{
+	int ret;
+
+	ret = ov13858_write_reg(ov13858, OV13858_REG_B_MWB_GAIN,
+				OV13858_REG_VALUE_16BIT, d_gain);
+	if (ret)
+		return ret;
+
+	ret = ov13858_write_reg(ov13858, OV13858_REG_G_MWB_GAIN,
+				OV13858_REG_VALUE_16BIT, d_gain);
+	if (ret)
+		return ret;
+
+	ret = ov13858_write_reg(ov13858, OV13858_REG_R_MWB_GAIN,
+				OV13858_REG_VALUE_16BIT, d_gain);
+
+	return ret;
+}
+
+static int ov13858_enable_test_pattern(struct ov13858 *ov13858, u32 pattern)
+{
+	int ret;
+	u32 val;
+
+	ret = ov13858_read_reg(ov13858, OV13858_REG_TEST_PATTERN,
+			       OV13858_REG_VALUE_08BIT, &val);
+	if (ret)
+		return ret;
+
+	if (pattern) {
+		val &= OV13858_TEST_PATTERN_MASK;
+		val |= (pattern - 1) | OV13858_TEST_PATTERN_ENABLE;
+	} else {
+		val &= ~OV13858_TEST_PATTERN_ENABLE;
+	}
+
+	return ov13858_write_reg(ov13858, OV13858_REG_TEST_PATTERN,
+				 OV13858_REG_VALUE_08BIT, val);
+}
+
+static int ov13858_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov13858 *ov13858 = container_of(ctrl->handler,
+					       struct ov13858, ctrl_handler);
+	struct i2c_client *client = v4l2_get_subdevdata(&ov13858->sd);
+	s64 max;
+	int ret;
+
+	/* Propagate change of current control to all related controls */
+	switch (ctrl->id) {
+	case V4L2_CID_VBLANK:
+		/* Update max exposure while meeting expected vblanking */
+		max = ov13858->cur_mode->height + ctrl->val - 8;
+		__v4l2_ctrl_modify_range(ov13858->exposure,
+					 ov13858->exposure->minimum,
+					 max, ov13858->exposure->step, max);
+		break;
+	}
+
+	/*
+	 * Applying V4L2 control value only happens
+	 * when power is up for streaming
+	 */
+	if (!pm_runtime_get_if_in_use(&client->dev))
+		return 0;
+
+	ret = 0;
+	switch (ctrl->id) {
+	case V4L2_CID_ANALOGUE_GAIN:
+		ret = ov13858_write_reg(ov13858, OV13858_REG_ANALOG_GAIN,
+					OV13858_REG_VALUE_16BIT, ctrl->val);
+		break;
+	case V4L2_CID_DIGITAL_GAIN:
+		ret = ov13858_update_digital_gain(ov13858, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE:
+		ret = ov13858_write_reg(ov13858, OV13858_REG_EXPOSURE,
+					OV13858_REG_VALUE_24BIT,
+					ctrl->val << 4);
+		break;
+	case V4L2_CID_VBLANK:
+		/* Update VTS that meets expected vertical blanking */
+		ret = ov13858_write_reg(ov13858, OV13858_REG_VTS,
+					OV13858_REG_VALUE_16BIT,
+					ov13858->cur_mode->height
+					  + ctrl->val);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		ret = ov13858_enable_test_pattern(ov13858, ctrl->val);
+		break;
+	default:
+		dev_info(&client->dev,
+			 "ctrl(id:0x%x,val:0x%x) is not handled\n",
+			 ctrl->id, ctrl->val);
+		break;
+	}
+
+	pm_runtime_put(&client->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ov13858_ctrl_ops = {
+	.s_ctrl = ov13858_set_ctrl,
+};
+
+static int ov13858_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	/* Only one bayer order(GRBG) is supported */
+	if (code->index > 0)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+
+	return 0;
+}
+
+static int ov13858_enum_frame_size(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->index >= ARRAY_SIZE(supported_modes))
+		return -EINVAL;
+
+	if (fse->code != MEDIA_BUS_FMT_SGRBG10_1X10)
+		return -EINVAL;
+
+	fse->min_width = supported_modes[fse->index].width;
+	fse->max_width = fse->min_width;
+	fse->min_height = supported_modes[fse->index].height;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static void ov13858_update_pad_format(const struct ov13858_mode *mode,
+				      struct v4l2_subdev_format *fmt)
+{
+	fmt->format.width = mode->width;
+	fmt->format.height = mode->height;
+	fmt->format.code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	fmt->format.field = V4L2_FIELD_NONE;
+}
+
+static int ov13858_do_get_pad_format(struct ov13858 *ov13858,
+				     struct v4l2_subdev_state *sd_state,
+				     struct v4l2_subdev_format *fmt)
+{
+	struct v4l2_mbus_framefmt *framefmt;
+	struct v4l2_subdev *sd = &ov13858->sd;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		framefmt = v4l2_subdev_get_try_format(sd, sd_state, fmt->pad);
+		fmt->format = *framefmt;
+	} else {
+		ov13858_update_pad_format(ov13858->cur_mode, fmt);
+	}
+
+	return 0;
+}
+
+static int ov13858_get_pad_format(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_format *fmt)
+{
+	struct ov13858 *ov13858 = to_ov13858(sd);
+	int ret;
+
+	mutex_lock(&ov13858->mutex);
+	ret = ov13858_do_get_pad_format(ov13858, sd_state, fmt);
+	mutex_unlock(&ov13858->mutex);
+
+	return ret;
+}
+
+static int
+ov13858_set_pad_format(struct v4l2_subdev *sd,
+		       struct v4l2_subdev_state *sd_state,
+		       struct v4l2_subdev_format *fmt)
+{
+	struct ov13858 *ov13858 = to_ov13858(sd);
+	const struct ov13858_mode *mode;
+	struct v4l2_mbus_framefmt *framefmt;
+	s32 vblank_def;
+	s32 vblank_min;
+	s64 h_blank;
+	s64 pixel_rate;
+	s64 link_freq;
+
+	mutex_lock(&ov13858->mutex);
+
+	/* Only one raw bayer(GRBG) order is supported */
+	if (fmt->format.code != MEDIA_BUS_FMT_SGRBG10_1X10)
+		fmt->format.code = MEDIA_BUS_FMT_SGRBG10_1X10;
+
+	mode = v4l2_find_nearest_size(supported_modes,
+				      ARRAY_SIZE(supported_modes),
+				      width, height,
+				      fmt->format.width, fmt->format.height);
+	ov13858_update_pad_format(mode, fmt);
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		framefmt = v4l2_subdev_get_try_format(sd, sd_state, fmt->pad);
+		*framefmt = fmt->format;
+	} else {
+		ov13858->cur_mode = mode;
+		__v4l2_ctrl_s_ctrl(ov13858->link_freq, mode->link_freq_index);
+		link_freq = link_freq_menu_items[mode->link_freq_index];
+		pixel_rate = link_freq_to_pixel_rate(link_freq);
+		__v4l2_ctrl_s_ctrl_int64(ov13858->pixel_rate, pixel_rate);
+
+		/* Update limits and set FPS to default */
+		vblank_def = ov13858->cur_mode->vts_def -
+			     ov13858->cur_mode->height;
+		vblank_min = ov13858->cur_mode->vts_min -
+			     ov13858->cur_mode->height;
+		__v4l2_ctrl_modify_range(
+			ov13858->vblank, vblank_min,
+			OV13858_VTS_MAX - ov13858->cur_mode->height, 1,
+			vblank_def);
+		__v4l2_ctrl_s_ctrl(ov13858->vblank, vblank_def);
+		h_blank =
+			link_freq_configs[mode->link_freq_index].pixels_per_line
+			 - ov13858->cur_mode->width;
+		__v4l2_ctrl_modify_range(ov13858->hblank, h_blank,
+					 h_blank, 1, h_blank);
+	}
+
+	mutex_unlock(&ov13858->mutex);
+
+	return 0;
+}
+
+static int ov13858_get_skip_frames(struct v4l2_subdev *sd, u32 *frames)
+{
+	*frames = OV13858_NUM_OF_SKIP_FRAMES;
+
+	return 0;
+}
+
+/* Start streaming */
+static int ov13858_start_streaming(struct ov13858 *ov13858)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov13858->sd);
+	const struct ov13858_reg_list *reg_list;
+	int ret, link_freq_index;
+
+	/* Get out of from software reset */
+	ret = ov13858_write_reg(ov13858, OV13858_REG_SOFTWARE_RST,
+				OV13858_REG_VALUE_08BIT, OV13858_SOFTWARE_RST);
+	if (ret) {
+		dev_err(&client->dev, "%s failed to set powerup registers\n",
+			__func__);
+		return ret;
+	}
+
+	/* Setup PLL */
+	link_freq_index = ov13858->cur_mode->link_freq_index;
+	reg_list = &link_freq_configs[link_freq_index].reg_list;
+	ret = ov13858_write_reg_list(ov13858, reg_list);
+	if (ret) {
+		dev_err(&client->dev, "%s failed to set plls\n", __func__);
+		return ret;
+	}
+
+	/* Apply default values of current mode */
+	reg_list = &ov13858->cur_mode->reg_list;
+	ret = ov13858_write_reg_list(ov13858, reg_list);
+	if (ret) {
+		dev_err(&client->dev, "%s failed to set mode\n", __func__);
+		return ret;
+	}
+
+	/* Apply customized values from user */
+	ret =  __v4l2_ctrl_handler_setup(ov13858->sd.ctrl_handler);
+	if (ret)
+		return ret;
+
+	return ov13858_write_reg(ov13858, OV13858_REG_MODE_SELECT,
+				 OV13858_REG_VALUE_08BIT,
+				 OV13858_MODE_STREAMING);
+}
+
+/* Stop streaming */
+static int ov13858_stop_streaming(struct ov13858 *ov13858)
+{
+	return ov13858_write_reg(ov13858, OV13858_REG_MODE_SELECT,
+				 OV13858_REG_VALUE_08BIT, OV13858_MODE_STANDBY);
+}
+
+static int ov13858_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct ov13858 *ov13858 = to_ov13858(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	mutex_lock(&ov13858->mutex);
+	if (ov13858->streaming == enable) {
+		mutex_unlock(&ov13858->mutex);
+		return 0;
+	}
+
+	if (enable) {
+		ret = pm_runtime_resume_and_get(&client->dev);
+		if (ret < 0)
+			goto err_unlock;
+
+		/*
+		 * Apply default & customized values
+		 * and then start streaming.
+		 */
+		ret = ov13858_start_streaming(ov13858);
+		if (ret)
+			goto err_rpm_put;
+	} else {
+		ov13858_stop_streaming(ov13858);
+		pm_runtime_put(&client->dev);
+	}
+
+	ov13858->streaming = enable;
+	mutex_unlock(&ov13858->mutex);
+
+	return ret;
+
+err_rpm_put:
+	pm_runtime_put(&client->dev);
+err_unlock:
+	mutex_unlock(&ov13858->mutex);
+
+	return ret;
+}
+
+static int __maybe_unused ov13858_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov13858 *ov13858 = to_ov13858(sd);
+
+	if (ov13858->streaming)
+		ov13858_stop_streaming(ov13858);
+
+	return 0;
+}
+
+static int __maybe_unused ov13858_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov13858 *ov13858 = to_ov13858(sd);
+	int ret;
+
+	if (ov13858->streaming) {
+		ret = ov13858_start_streaming(ov13858);
+		if (ret)
+			goto error;
+	}
+
+	return 0;
+
+error:
+	ov13858_stop_streaming(ov13858);
+	ov13858->streaming = false;
+	return ret;
+}
+
+/* Verify chip ID */
+static int ov13858_identify_module(struct ov13858 *ov13858)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov13858->sd);
+	int ret;
+	u32 val;
+
+	ret = ov13858_read_reg(ov13858, OV13858_REG_CHIP_ID,
+			       OV13858_REG_VALUE_24BIT, &val);
+	if (ret)
+		return ret;
+
+	if (val != OV13858_CHIP_ID) {
+		dev_err(&client->dev, "chip id mismatch: %x!=%x\n",
+			OV13858_CHIP_ID, val);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops ov13858_core_ops = {
+	.log_status = v4l2_ctrl_subdev_log_status,
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_video_ops ov13858_video_ops = {
+	.s_stream = ov13858_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops ov13858_pad_ops = {
+	.enum_mbus_code = ov13858_enum_mbus_code,
+	.get_fmt = ov13858_get_pad_format,
+	.set_fmt = ov13858_set_pad_format,
+	.enum_frame_size = ov13858_enum_frame_size,
+};
+
+static const struct v4l2_subdev_sensor_ops ov13858_sensor_ops = {
+	.g_skip_frames = ov13858_get_skip_frames,
+};
+
+static const struct v4l2_subdev_ops ov13858_subdev_ops = {
+	.core = &ov13858_core_ops,
+	.video = &ov13858_video_ops,
+	.pad = &ov13858_pad_ops,
+	.sensor = &ov13858_sensor_ops,
+};
+
+static const struct media_entity_operations ov13858_subdev_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static const struct v4l2_subdev_internal_ops ov13858_internal_ops = {
+	.open = ov13858_open,
+};
+
+/* Initialize control handlers */
+static int ov13858_init_controls(struct ov13858 *ov13858)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov13858->sd);
+	struct v4l2_fwnode_device_properties props;
+	struct v4l2_ctrl_handler *ctrl_hdlr;
+	s64 exposure_max;
+	s64 vblank_def;
+	s64 vblank_min;
+	s64 hblank;
+	s64 pixel_rate_min;
+	s64 pixel_rate_max;
+	const struct ov13858_mode *mode;
+	int ret;
+
+	ctrl_hdlr = &ov13858->ctrl_handler;
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 10);
+	if (ret)
+		return ret;
+
+	mutex_init(&ov13858->mutex);
+	ctrl_hdlr->lock = &ov13858->mutex;
+	ov13858->link_freq = v4l2_ctrl_new_int_menu(ctrl_hdlr,
+				&ov13858_ctrl_ops,
+				V4L2_CID_LINK_FREQ,
+				OV13858_NUM_OF_LINK_FREQS - 1,
+				0,
+				link_freq_menu_items);
+	if (ov13858->link_freq)
+		ov13858->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	pixel_rate_max = link_freq_to_pixel_rate(link_freq_menu_items[0]);
+	pixel_rate_min = link_freq_to_pixel_rate(link_freq_menu_items[1]);
+	/* By default, PIXEL_RATE is read only */
+	ov13858->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, &ov13858_ctrl_ops,
+						V4L2_CID_PIXEL_RATE,
+						pixel_rate_min, pixel_rate_max,
+						1, pixel_rate_max);
+
+	mode = ov13858->cur_mode;
+	vblank_def = mode->vts_def - mode->height;
+	vblank_min = mode->vts_min - mode->height;
+	ov13858->vblank = v4l2_ctrl_new_std(
+				ctrl_hdlr, &ov13858_ctrl_ops, V4L2_CID_VBLANK,
+				vblank_min, OV13858_VTS_MAX - mode->height, 1,
+				vblank_def);
+
+	hblank = link_freq_configs[mode->link_freq_index].pixels_per_line -
+		 mode->width;
+	ov13858->hblank = v4l2_ctrl_new_std(
+				ctrl_hdlr, &ov13858_ctrl_ops, V4L2_CID_HBLANK,
+				hblank, hblank, 1, hblank);
+	if (ov13858->hblank)
+		ov13858->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	exposure_max = mode->vts_def - 8;
+	ov13858->exposure = v4l2_ctrl_new_std(
+				ctrl_hdlr, &ov13858_ctrl_ops,
+				V4L2_CID_EXPOSURE, OV13858_EXPOSURE_MIN,
+				exposure_max, OV13858_EXPOSURE_STEP,
+				OV13858_EXPOSURE_DEFAULT);
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov13858_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
+			  OV13858_ANA_GAIN_MIN, OV13858_ANA_GAIN_MAX,
+			  OV13858_ANA_GAIN_STEP, OV13858_ANA_GAIN_DEFAULT);
+
+	/* Digital gain */
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov13858_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
+			  OV13858_DGTL_GAIN_MIN, OV13858_DGTL_GAIN_MAX,
+			  OV13858_DGTL_GAIN_STEP, OV13858_DGTL_GAIN_DEFAULT);
+
+	v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &ov13858_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(ov13858_test_pattern_menu) - 1,
+				     0, 0, ov13858_test_pattern_menu);
+	if (ctrl_hdlr->error) {
+		ret = ctrl_hdlr->error;
+		dev_err(&client->dev, "%s control init failed (%d)\n",
+			__func__, ret);
+		goto error;
+	}
+
+	ret = v4l2_fwnode_device_parse(&client->dev, &props);
+	if (ret)
+		goto error;
+
+	ret = v4l2_ctrl_new_fwnode_properties(ctrl_hdlr, &ov13858_ctrl_ops,
+					      &props);
+	if (ret)
+		goto error;
+
+	ov13858->sd.ctrl_handler = ctrl_hdlr;
+
+	return 0;
+
+error:
+	v4l2_ctrl_handler_free(ctrl_hdlr);
+	mutex_destroy(&ov13858->mutex);
+
+	return ret;
+}
+
+static void ov13858_free_controls(struct ov13858 *ov13858)
+{
+	v4l2_ctrl_handler_free(ov13858->sd.ctrl_handler);
+	mutex_destroy(&ov13858->mutex);
+}
+
+static int ov13858_probe(struct i2c_client *client)
+{
+	struct ov13858 *ov13858;
+	int ret;
+	u32 val = 0;
+
+	device_property_read_u32(&client->dev, "clock-frequency", &val);
+	if (val != 19200000)
+		return -EINVAL;
+
+	ov13858 = devm_kzalloc(&client->dev, sizeof(*ov13858), GFP_KERNEL);
+	if (!ov13858)
+		return -ENOMEM;
+
+	/* Initialize subdev */
+	v4l2_i2c_subdev_init(&ov13858->sd, client, &ov13858_subdev_ops);
+
+	/* Check module identity */
+	ret = ov13858_identify_module(ov13858);
+	if (ret) {
+		dev_err(&client->dev, "failed to find sensor: %d\n", ret);
+		return ret;
+	}
+
+	/* Set default mode to max resolution */
+	ov13858->cur_mode = &supported_modes[0];
+
+	ret = ov13858_init_controls(ov13858);
+	if (ret)
+		return ret;
+
+	/* Initialize subdev */
+	ov13858->sd.internal_ops = &ov13858_internal_ops;
+	ov13858->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+			     V4L2_SUBDEV_FL_HAS_EVENTS;
+	ov13858->sd.entity.ops = &ov13858_subdev_entity_ops;
+	ov13858->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+	/* Initialize source pad */
+	ov13858->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&ov13858->sd.entity, 1, &ov13858->pad);
+	if (ret) {
+		dev_err(&client->dev, "%s failed:%d\n", __func__, ret);
+		goto error_handler_free;
+	}
+
+	ret = v4l2_async_register_subdev_sensor(&ov13858->sd);
+	if (ret < 0)
+		goto error_media_entity;
+
+	/*
+	 * Device is already turned on by i2c-core with ACPI domain PM.
+	 * Enable runtime PM and turn off the device.
+	 */
+	pm_runtime_set_active(&client->dev);
+	pm_runtime_enable(&client->dev);
+	pm_runtime_idle(&client->dev);
+
+	return 0;
+
+error_media_entity:
+	media_entity_cleanup(&ov13858->sd.entity);
+
+error_handler_free:
+	ov13858_free_controls(ov13858);
+	dev_err(&client->dev, "%s failed:%d\n", __func__, ret);
+
+	return ret;
+}
+
+static void ov13858_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov13858 *ov13858 = to_ov13858(sd);
+
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	ov13858_free_controls(ov13858);
+
+	pm_runtime_disable(&client->dev);
+}
+
+static const struct i2c_device_id ov13858_id_table[] = {
+	{"ov13858", 0},
+	{},
+};
+
+MODULE_DEVICE_TABLE(i2c, ov13858_id_table);
+
+static const struct dev_pm_ops ov13858_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(ov13858_suspend, ov13858_resume)
+};
+
+#ifdef CONFIG_ACPI
+static const struct acpi_device_id ov13858_acpi_ids[] = {
+	{"OVTID858"},
+	{ /* sentinel */ }
+};
+
+MODULE_DEVICE_TABLE(acpi, ov13858_acpi_ids);
+#endif
+
+static struct i2c_driver ov13858_i2c_driver = {
+	.driver = {
+		.name = "ov13858",
+		.pm = &ov13858_pm_ops,
+		.acpi_match_table = ACPI_PTR(ov13858_acpi_ids),
+	},
+	.probe = ov13858_probe,
+	.remove = ov13858_remove,
+	.id_table = ov13858_id_table,
+};
+
+module_i2c_driver(ov13858_i2c_driver);
+
+MODULE_AUTHOR("Kan, Chris <chris.kan@intel.com>");
+MODULE_AUTHOR("Rapolu, Chiranjeevi");
+MODULE_AUTHOR("Yang, Hyungwoo");
+MODULE_DESCRIPTION("Omnivision ov13858 sensor driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/ov13b10.c b/drivers/media/i2c/ov13b10.c
new file mode 100644
index 000000000..124eaa4f9
--- /dev/null
+++ b/drivers/media/i2c/ov13b10.c
@@ -0,0 +1,1613 @@
+// SPDX-License-Identifier: GPL-2.0
+// Copyright (c) 2021 Intel Corporation.
+
+#include <linux/acpi.h>
+#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 <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+
+#define OV13B10_REG_VALUE_08BIT		1
+#define OV13B10_REG_VALUE_16BIT		2
+#define OV13B10_REG_VALUE_24BIT		3
+
+#define OV13B10_REG_MODE_SELECT		0x0100
+#define OV13B10_MODE_STANDBY		0x00
+#define OV13B10_MODE_STREAMING		0x01
+
+#define OV13B10_REG_SOFTWARE_RST	0x0103
+#define OV13B10_SOFTWARE_RST		0x01
+
+/* Chip ID */
+#define OV13B10_REG_CHIP_ID		0x300a
+#define OV13B10_CHIP_ID			0x560d42
+
+/* V_TIMING internal */
+#define OV13B10_REG_VTS			0x380e
+#define OV13B10_VTS_30FPS		0x0c7c
+#define OV13B10_VTS_60FPS		0x063e
+#define OV13B10_VTS_MAX			0x7fff
+
+/* HBLANK control - read only */
+#define OV13B10_PPL_560MHZ		4704
+
+/* Exposure control */
+#define OV13B10_REG_EXPOSURE		0x3500
+#define OV13B10_EXPOSURE_MIN		4
+#define OV13B10_EXPOSURE_STEP		1
+#define OV13B10_EXPOSURE_DEFAULT	0x40
+
+/* Analog gain control */
+#define OV13B10_REG_ANALOG_GAIN		0x3508
+#define OV13B10_ANA_GAIN_MIN		0x80
+#define OV13B10_ANA_GAIN_MAX		0x07c0
+#define OV13B10_ANA_GAIN_STEP		1
+#define OV13B10_ANA_GAIN_DEFAULT	0x80
+
+/* Digital gain control */
+#define OV13B10_REG_DGTL_GAIN_H		0x350a
+#define OV13B10_REG_DGTL_GAIN_M		0x350b
+#define OV13B10_REG_DGTL_GAIN_L		0x350c
+
+#define OV13B10_DGTL_GAIN_MIN		1024	     /* Min = 1 X */
+#define OV13B10_DGTL_GAIN_MAX		(4096 - 1)   /* Max = 4 X */
+#define OV13B10_DGTL_GAIN_DEFAULT	2560	     /* Default gain = 2.5 X */
+#define OV13B10_DGTL_GAIN_STEP		1	     /* Each step = 1/1024 */
+
+#define OV13B10_DGTL_GAIN_L_SHIFT	6
+#define OV13B10_DGTL_GAIN_L_MASK	0x3
+#define OV13B10_DGTL_GAIN_M_SHIFT	2
+#define OV13B10_DGTL_GAIN_M_MASK	0xff
+#define OV13B10_DGTL_GAIN_H_SHIFT	10
+#define OV13B10_DGTL_GAIN_H_MASK	0x3
+
+/* Test Pattern Control */
+#define OV13B10_REG_TEST_PATTERN	0x5080
+#define OV13B10_TEST_PATTERN_ENABLE	BIT(7)
+#define OV13B10_TEST_PATTERN_MASK	0xf3
+#define OV13B10_TEST_PATTERN_BAR_SHIFT	2
+
+/* Flip Control */
+#define OV13B10_REG_FORMAT1		0x3820
+#define OV13B10_REG_FORMAT2		0x3821
+
+/* Horizontal Window Offset */
+#define OV13B10_REG_H_WIN_OFFSET	0x3811
+
+/* Vertical Window Offset */
+#define OV13B10_REG_V_WIN_OFFSET	0x3813
+
+struct ov13b10_reg {
+	u16 address;
+	u8 val;
+};
+
+struct ov13b10_reg_list {
+	u32 num_of_regs;
+	const struct ov13b10_reg *regs;
+};
+
+/* Link frequency config */
+struct ov13b10_link_freq_config {
+	u32 pixels_per_line;
+
+	/* registers for this link frequency */
+	struct ov13b10_reg_list reg_list;
+};
+
+/* Mode : resolution and related config&values */
+struct ov13b10_mode {
+	/* Frame width */
+	u32 width;
+	/* Frame height */
+	u32 height;
+
+	/* V-timing */
+	u32 vts_def;
+	u32 vts_min;
+
+	/* Index of Link frequency config to be used */
+	u32 link_freq_index;
+	/* Default register values */
+	struct ov13b10_reg_list reg_list;
+};
+
+/* 4208x3120 needs 1120Mbps/lane, 4 lanes */
+static const struct ov13b10_reg mipi_data_rate_1120mbps[] = {
+	{0x0103, 0x01},
+	{0x0303, 0x04},
+	{0x0305, 0xaf},
+	{0x0321, 0x00},
+	{0x0323, 0x04},
+	{0x0324, 0x01},
+	{0x0325, 0xa4},
+	{0x0326, 0x81},
+	{0x0327, 0x04},
+	{0x3012, 0x07},
+	{0x3013, 0x32},
+	{0x3107, 0x23},
+	{0x3501, 0x0c},
+	{0x3502, 0x10},
+	{0x3504, 0x08},
+	{0x3508, 0x07},
+	{0x3509, 0xc0},
+	{0x3600, 0x16},
+	{0x3601, 0x54},
+	{0x3612, 0x4e},
+	{0x3620, 0x00},
+	{0x3621, 0x68},
+	{0x3622, 0x66},
+	{0x3623, 0x03},
+	{0x3662, 0x92},
+	{0x3666, 0xbb},
+	{0x3667, 0x44},
+	{0x366e, 0xff},
+	{0x366f, 0xf3},
+	{0x3675, 0x44},
+	{0x3676, 0x00},
+	{0x367f, 0xe9},
+	{0x3681, 0x32},
+	{0x3682, 0x1f},
+	{0x3683, 0x0b},
+	{0x3684, 0x0b},
+	{0x3704, 0x0f},
+	{0x3706, 0x40},
+	{0x3708, 0x3b},
+	{0x3709, 0x72},
+	{0x370b, 0xa2},
+	{0x3714, 0x24},
+	{0x371a, 0x3e},
+	{0x3725, 0x42},
+	{0x3739, 0x12},
+	{0x3767, 0x00},
+	{0x377a, 0x0d},
+	{0x3789, 0x18},
+	{0x3790, 0x40},
+	{0x3791, 0xa2},
+	{0x37c2, 0x04},
+	{0x37c3, 0xf1},
+	{0x37d9, 0x0c},
+	{0x37da, 0x02},
+	{0x37dc, 0x02},
+	{0x37e1, 0x04},
+	{0x37e2, 0x0a},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x08},
+	{0x3804, 0x10},
+	{0x3805, 0x8f},
+	{0x3806, 0x0c},
+	{0x3807, 0x47},
+	{0x3808, 0x10},
+	{0x3809, 0x70},
+	{0x380a, 0x0c},
+	{0x380b, 0x30},
+	{0x380c, 0x04},
+	{0x380d, 0x98},
+	{0x380e, 0x0c},
+	{0x380f, 0x7c},
+	{0x3811, 0x0f},
+	{0x3813, 0x09},
+	{0x3814, 0x01},
+	{0x3815, 0x01},
+	{0x3816, 0x01},
+	{0x3817, 0x01},
+	{0x381f, 0x08},
+	{0x3820, 0x88},
+	{0x3821, 0x00},
+	{0x3822, 0x14},
+	{0x382e, 0xe6},
+	{0x3c80, 0x00},
+	{0x3c87, 0x01},
+	{0x3c8c, 0x19},
+	{0x3c8d, 0x1c},
+	{0x3ca0, 0x00},
+	{0x3ca1, 0x00},
+	{0x3ca2, 0x00},
+	{0x3ca3, 0x00},
+	{0x3ca4, 0x50},
+	{0x3ca5, 0x11},
+	{0x3ca6, 0x01},
+	{0x3ca7, 0x00},
+	{0x3ca8, 0x00},
+	{0x4008, 0x02},
+	{0x4009, 0x0f},
+	{0x400a, 0x01},
+	{0x400b, 0x19},
+	{0x4011, 0x21},
+	{0x4017, 0x08},
+	{0x4019, 0x04},
+	{0x401a, 0x58},
+	{0x4032, 0x1e},
+	{0x4050, 0x02},
+	{0x4051, 0x09},
+	{0x405e, 0x00},
+	{0x4066, 0x02},
+	{0x4501, 0x00},
+	{0x4502, 0x10},
+	{0x4505, 0x00},
+	{0x4800, 0x64},
+	{0x481b, 0x3e},
+	{0x481f, 0x30},
+	{0x4825, 0x34},
+	{0x4837, 0x0e},
+	{0x484b, 0x01},
+	{0x4883, 0x02},
+	{0x5000, 0xff},
+	{0x5001, 0x0f},
+	{0x5045, 0x20},
+	{0x5046, 0x20},
+	{0x5047, 0xa4},
+	{0x5048, 0x20},
+	{0x5049, 0xa4},
+};
+
+static const struct ov13b10_reg mode_4208x3120_regs[] = {
+	{0x0305, 0xaf},
+	{0x3501, 0x0c},
+	{0x3662, 0x92},
+	{0x3714, 0x24},
+	{0x3739, 0x12},
+	{0x37c2, 0x04},
+	{0x37d9, 0x0c},
+	{0x37e2, 0x0a},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x08},
+	{0x3804, 0x10},
+	{0x3805, 0x8f},
+	{0x3806, 0x0c},
+	{0x3807, 0x47},
+	{0x3808, 0x10},
+	{0x3809, 0x70},
+	{0x380a, 0x0c},
+	{0x380b, 0x30},
+	{0x380c, 0x04},
+	{0x380d, 0x98},
+	{0x380e, 0x0c},
+	{0x380f, 0x7c},
+	{0x3810, 0x00},
+	{0x3811, 0x0f},
+	{0x3812, 0x00},
+	{0x3813, 0x09},
+	{0x3814, 0x01},
+	{0x3816, 0x01},
+	{0x3820, 0x88},
+	{0x3c8c, 0x19},
+	{0x4008, 0x02},
+	{0x4009, 0x0f},
+	{0x4050, 0x02},
+	{0x4051, 0x09},
+	{0x4501, 0x00},
+	{0x4505, 0x00},
+	{0x4837, 0x0e},
+	{0x5000, 0xff},
+	{0x5001, 0x0f},
+};
+
+static const struct ov13b10_reg mode_4160x3120_regs[] = {
+	{0x0305, 0xaf},
+	{0x3501, 0x0c},
+	{0x3662, 0x92},
+	{0x3714, 0x24},
+	{0x3739, 0x12},
+	{0x37c2, 0x04},
+	{0x37d9, 0x0c},
+	{0x37e2, 0x0a},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x08},
+	{0x3804, 0x10},
+	{0x3805, 0x8f},
+	{0x3806, 0x0c},
+	{0x3807, 0x47},
+	{0x3808, 0x10},
+	{0x3809, 0x40},
+	{0x380a, 0x0c},
+	{0x380b, 0x30},
+	{0x380c, 0x04},
+	{0x380d, 0x98},
+	{0x380e, 0x0c},
+	{0x380f, 0x7c},
+	{0x3810, 0x00},
+	{0x3811, 0x27},
+	{0x3812, 0x00},
+	{0x3813, 0x09},
+	{0x3814, 0x01},
+	{0x3816, 0x01},
+	{0x3820, 0x88},
+	{0x3c8c, 0x19},
+	{0x4008, 0x02},
+	{0x4009, 0x0f},
+	{0x4050, 0x02},
+	{0x4051, 0x09},
+	{0x4501, 0x00},
+	{0x4505, 0x00},
+	{0x4837, 0x0e},
+	{0x5000, 0xff},
+	{0x5001, 0x0f},
+};
+
+static const struct ov13b10_reg mode_4160x2340_regs[] = {
+	{0x0305, 0xaf},
+	{0x3501, 0x0c},
+	{0x3662, 0x92},
+	{0x3714, 0x24},
+	{0x3739, 0x12},
+	{0x37c2, 0x04},
+	{0x37d9, 0x0c},
+	{0x37e2, 0x0a},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x08},
+	{0x3804, 0x10},
+	{0x3805, 0x8f},
+	{0x3806, 0x0c},
+	{0x3807, 0x47},
+	{0x3808, 0x10},
+	{0x3809, 0x40},
+	{0x380a, 0x09},
+	{0x380b, 0x24},
+	{0x380c, 0x04},
+	{0x380d, 0x98},
+	{0x380e, 0x0c},
+	{0x380f, 0x7c},
+	{0x3810, 0x00},
+	{0x3811, 0x27},
+	{0x3812, 0x01},
+	{0x3813, 0x8f},
+	{0x3814, 0x01},
+	{0x3816, 0x01},
+	{0x3820, 0x88},
+	{0x3c8c, 0x19},
+	{0x4008, 0x02},
+	{0x4009, 0x0f},
+	{0x4050, 0x02},
+	{0x4051, 0x09},
+	{0x4501, 0x00},
+	{0x4505, 0x00},
+	{0x4837, 0x0e},
+	{0x5000, 0xff},
+	{0x5001, 0x0f},
+};
+
+static const struct ov13b10_reg mode_2104x1560_regs[] = {
+	{0x0305, 0xaf},
+	{0x3501, 0x06},
+	{0x3662, 0x88},
+	{0x3714, 0x28},
+	{0x3739, 0x10},
+	{0x37c2, 0x14},
+	{0x37d9, 0x06},
+	{0x37e2, 0x0c},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x08},
+	{0x3804, 0x10},
+	{0x3805, 0x8f},
+	{0x3806, 0x0c},
+	{0x3807, 0x47},
+	{0x3808, 0x08},
+	{0x3809, 0x38},
+	{0x380a, 0x06},
+	{0x380b, 0x18},
+	{0x380c, 0x04},
+	{0x380d, 0x98},
+	{0x380e, 0x06},
+	{0x380f, 0x3e},
+	{0x3810, 0x00},
+	{0x3811, 0x07},
+	{0x3812, 0x00},
+	{0x3813, 0x05},
+	{0x3814, 0x03},
+	{0x3816, 0x03},
+	{0x3820, 0x8b},
+	{0x3c8c, 0x18},
+	{0x4008, 0x00},
+	{0x4009, 0x05},
+	{0x4050, 0x00},
+	{0x4051, 0x05},
+	{0x4501, 0x08},
+	{0x4505, 0x00},
+	{0x4837, 0x0e},
+	{0x5000, 0xfd},
+	{0x5001, 0x0d},
+};
+
+static const struct ov13b10_reg mode_2080x1170_regs[] = {
+	{0x0305, 0xaf},
+	{0x3501, 0x06},
+	{0x3662, 0x88},
+	{0x3714, 0x28},
+	{0x3739, 0x10},
+	{0x37c2, 0x14},
+	{0x37d9, 0x06},
+	{0x37e2, 0x0c},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x08},
+	{0x3804, 0x10},
+	{0x3805, 0x8f},
+	{0x3806, 0x0c},
+	{0x3807, 0x47},
+	{0x3808, 0x08},
+	{0x3809, 0x20},
+	{0x380a, 0x04},
+	{0x380b, 0x92},
+	{0x380c, 0x04},
+	{0x380d, 0x98},
+	{0x380e, 0x06},
+	{0x380f, 0x3e},
+	{0x3810, 0x00},
+	{0x3811, 0x13},
+	{0x3812, 0x00},
+	{0x3813, 0xc9},
+	{0x3814, 0x03},
+	{0x3816, 0x03},
+	{0x3820, 0x8b},
+	{0x3c8c, 0x18},
+	{0x4008, 0x00},
+	{0x4009, 0x05},
+	{0x4050, 0x00},
+	{0x4051, 0x05},
+	{0x4501, 0x08},
+	{0x4505, 0x00},
+	{0x4837, 0x0e},
+	{0x5000, 0xfd},
+	{0x5001, 0x0d},
+};
+
+static const char * const ov13b10_test_pattern_menu[] = {
+	"Disabled",
+	"Vertical Color Bar Type 1",
+	"Vertical Color Bar Type 2",
+	"Vertical Color Bar Type 3",
+	"Vertical Color Bar Type 4"
+};
+
+/* Configurations for supported link frequencies */
+#define OV13B10_LINK_FREQ_560MHZ	560000000ULL
+#define OV13B10_LINK_FREQ_INDEX_0	0
+
+#define OV13B10_EXT_CLK			19200000
+#define OV13B10_DATA_LANES		4
+
+/*
+ * pixel_rate = link_freq * data-rate * nr_of_lanes / bits_per_sample
+ * data rate => double data rate; number of lanes => 4; bits per pixel => 10
+ */
+static u64 link_freq_to_pixel_rate(u64 f)
+{
+	f *= 2 * OV13B10_DATA_LANES;
+	do_div(f, 10);
+
+	return f;
+}
+
+/* Menu items for LINK_FREQ V4L2 control */
+static const s64 link_freq_menu_items[] = {
+	OV13B10_LINK_FREQ_560MHZ
+};
+
+/* Link frequency configs */
+static const struct ov13b10_link_freq_config
+			link_freq_configs[] = {
+	{
+		.pixels_per_line = OV13B10_PPL_560MHZ,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mipi_data_rate_1120mbps),
+			.regs = mipi_data_rate_1120mbps,
+		}
+	}
+};
+
+/* Mode configs */
+static const struct ov13b10_mode supported_modes[] = {
+	{
+		.width = 4208,
+		.height = 3120,
+		.vts_def = OV13B10_VTS_30FPS,
+		.vts_min = OV13B10_VTS_30FPS,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_4208x3120_regs),
+			.regs = mode_4208x3120_regs,
+		},
+		.link_freq_index = OV13B10_LINK_FREQ_INDEX_0,
+	},
+	{
+		.width = 4160,
+		.height = 3120,
+		.vts_def = OV13B10_VTS_30FPS,
+		.vts_min = OV13B10_VTS_30FPS,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_4160x3120_regs),
+			.regs = mode_4160x3120_regs,
+		},
+		.link_freq_index = OV13B10_LINK_FREQ_INDEX_0,
+	},
+	{
+		.width = 4160,
+		.height = 2340,
+		.vts_def = OV13B10_VTS_30FPS,
+		.vts_min = OV13B10_VTS_30FPS,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_4160x2340_regs),
+			.regs = mode_4160x2340_regs,
+		},
+		.link_freq_index = OV13B10_LINK_FREQ_INDEX_0,
+	},
+	{
+		.width = 2104,
+		.height = 1560,
+		.vts_def = OV13B10_VTS_60FPS,
+		.vts_min = OV13B10_VTS_60FPS,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_2104x1560_regs),
+			.regs = mode_2104x1560_regs,
+		},
+		.link_freq_index = OV13B10_LINK_FREQ_INDEX_0,
+	},
+	{
+		.width = 2080,
+		.height = 1170,
+		.vts_def = OV13B10_VTS_60FPS,
+		.vts_min = OV13B10_VTS_60FPS,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_2080x1170_regs),
+			.regs = mode_2080x1170_regs,
+		},
+		.link_freq_index = OV13B10_LINK_FREQ_INDEX_0,
+	}
+};
+
+struct ov13b10 {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+
+	struct v4l2_ctrl_handler ctrl_handler;
+
+	struct clk *img_clk;
+	struct regulator *avdd;
+	struct gpio_desc *reset;
+
+	/* V4L2 Controls */
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *pixel_rate;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *exposure;
+
+	/* Current mode */
+	const struct ov13b10_mode *cur_mode;
+
+	/* Mutex for serialized access */
+	struct mutex mutex;
+
+	/* Streaming on/off */
+	bool streaming;
+
+	/* True if the device has been identified */
+	bool identified;
+};
+
+#define to_ov13b10(_sd)	container_of(_sd, struct ov13b10, sd)
+
+/* Read registers up to 4 at a time */
+static int ov13b10_read_reg(struct ov13b10 *ov13b,
+			    u16 reg, u32 len, u32 *val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov13b->sd);
+	struct i2c_msg msgs[2];
+	u8 *data_be_p;
+	int ret;
+	__be32 data_be = 0;
+	__be16 reg_addr_be = cpu_to_be16(reg);
+
+	if (len > 4)
+		return -EINVAL;
+
+	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[4 - len];
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret != ARRAY_SIZE(msgs))
+		return -EIO;
+
+	*val = be32_to_cpu(data_be);
+
+	return 0;
+}
+
+/* Write registers up to 4 at a time */
+static int ov13b10_write_reg(struct ov13b10 *ov13b,
+			     u16 reg, u32 len, u32 __val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov13b->sd);
+	int buf_i, val_i;
+	u8 buf[6], *val_p;
+	__be32 val;
+
+	if (len > 4)
+		return -EINVAL;
+
+	buf[0] = reg >> 8;
+	buf[1] = reg & 0xff;
+
+	val = cpu_to_be32(__val);
+	val_p = (u8 *)&val;
+	buf_i = 2;
+	val_i = 4 - len;
+
+	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;
+}
+
+/* Write a list of registers */
+static int ov13b10_write_regs(struct ov13b10 *ov13b,
+			      const struct ov13b10_reg *regs, u32 len)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov13b->sd);
+	int ret;
+	u32 i;
+
+	for (i = 0; i < len; i++) {
+		ret = ov13b10_write_reg(ov13b, regs[i].address, 1,
+					regs[i].val);
+		if (ret) {
+			dev_err_ratelimited(&client->dev,
+					    "Failed to write reg 0x%4.4x. error = %d\n",
+					    regs[i].address, ret);
+
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int ov13b10_write_reg_list(struct ov13b10 *ov13b,
+				  const struct ov13b10_reg_list *r_list)
+{
+	return ov13b10_write_regs(ov13b, r_list->regs, r_list->num_of_regs);
+}
+
+/* Open sub-device */
+static int ov13b10_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	const struct ov13b10_mode *default_mode = &supported_modes[0];
+	struct ov13b10 *ov13b = to_ov13b10(sd);
+	struct v4l2_mbus_framefmt *try_fmt = v4l2_subdev_get_try_format(sd,
+									fh->state,
+									0);
+
+	mutex_lock(&ov13b->mutex);
+
+	/* Initialize try_fmt */
+	try_fmt->width = default_mode->width;
+	try_fmt->height = default_mode->height;
+	try_fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	try_fmt->field = V4L2_FIELD_NONE;
+
+	/* No crop or compose */
+	mutex_unlock(&ov13b->mutex);
+
+	return 0;
+}
+
+static int ov13b10_update_digital_gain(struct ov13b10 *ov13b, u32 d_gain)
+{
+	int ret;
+	u32 val;
+
+	/*
+	 * 0x350C[7:6], 0x350B[7:0], 0x350A[1:0]
+	 */
+
+	val = (d_gain & OV13B10_DGTL_GAIN_L_MASK) << OV13B10_DGTL_GAIN_L_SHIFT;
+	ret = ov13b10_write_reg(ov13b, OV13B10_REG_DGTL_GAIN_L,
+				OV13B10_REG_VALUE_08BIT, val);
+	if (ret)
+		return ret;
+
+	val = (d_gain >> OV13B10_DGTL_GAIN_M_SHIFT) & OV13B10_DGTL_GAIN_M_MASK;
+	ret = ov13b10_write_reg(ov13b, OV13B10_REG_DGTL_GAIN_M,
+				OV13B10_REG_VALUE_08BIT, val);
+	if (ret)
+		return ret;
+
+	val = (d_gain >> OV13B10_DGTL_GAIN_H_SHIFT) & OV13B10_DGTL_GAIN_H_MASK;
+	ret = ov13b10_write_reg(ov13b, OV13B10_REG_DGTL_GAIN_H,
+				OV13B10_REG_VALUE_08BIT, val);
+
+	return ret;
+}
+
+static int ov13b10_enable_test_pattern(struct ov13b10 *ov13b, u32 pattern)
+{
+	int ret;
+	u32 val;
+
+	ret = ov13b10_read_reg(ov13b, OV13B10_REG_TEST_PATTERN,
+			       OV13B10_REG_VALUE_08BIT, &val);
+	if (ret)
+		return ret;
+
+	if (pattern) {
+		val &= OV13B10_TEST_PATTERN_MASK;
+		val |= ((pattern - 1) << OV13B10_TEST_PATTERN_BAR_SHIFT) |
+		     OV13B10_TEST_PATTERN_ENABLE;
+	} else {
+		val &= ~OV13B10_TEST_PATTERN_ENABLE;
+	}
+
+	return ov13b10_write_reg(ov13b, OV13B10_REG_TEST_PATTERN,
+				 OV13B10_REG_VALUE_08BIT, val);
+}
+
+static int ov13b10_set_ctrl_hflip(struct ov13b10 *ov13b, u32 ctrl_val)
+{
+	int ret;
+	u32 val;
+
+	ret = ov13b10_read_reg(ov13b, OV13B10_REG_FORMAT1,
+			       OV13B10_REG_VALUE_08BIT, &val);
+	if (ret)
+		return ret;
+
+	ret = ov13b10_write_reg(ov13b, OV13B10_REG_FORMAT1,
+				OV13B10_REG_VALUE_08BIT,
+				ctrl_val ? val & ~BIT(3) : val);
+
+	if (ret)
+		return ret;
+
+	ret = ov13b10_read_reg(ov13b, OV13B10_REG_H_WIN_OFFSET,
+			       OV13B10_REG_VALUE_08BIT, &val);
+	if (ret)
+		return ret;
+
+	/*
+	 * Applying cropping offset to reverse the change of Bayer order
+	 * after mirroring image
+	 */
+	return ov13b10_write_reg(ov13b, OV13B10_REG_H_WIN_OFFSET,
+				 OV13B10_REG_VALUE_08BIT,
+				 ctrl_val ? ++val : val);
+}
+
+static int ov13b10_set_ctrl_vflip(struct ov13b10 *ov13b, u32 ctrl_val)
+{
+	int ret;
+	u32 val;
+
+	ret = ov13b10_read_reg(ov13b, OV13B10_REG_FORMAT1,
+			       OV13B10_REG_VALUE_08BIT, &val);
+	if (ret)
+		return ret;
+
+	ret = ov13b10_write_reg(ov13b, OV13B10_REG_FORMAT1,
+				OV13B10_REG_VALUE_08BIT,
+				ctrl_val ? val | BIT(4) | BIT(5)  : val);
+
+	if (ret)
+		return ret;
+
+	ret = ov13b10_read_reg(ov13b, OV13B10_REG_V_WIN_OFFSET,
+			       OV13B10_REG_VALUE_08BIT, &val);
+	if (ret)
+		return ret;
+
+	/*
+	 * Applying cropping offset to reverse the change of Bayer order
+	 * after flipping image
+	 */
+	return ov13b10_write_reg(ov13b, OV13B10_REG_V_WIN_OFFSET,
+				 OV13B10_REG_VALUE_08BIT,
+				 ctrl_val ? --val : val);
+}
+
+static int ov13b10_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov13b10 *ov13b = container_of(ctrl->handler,
+					     struct ov13b10, ctrl_handler);
+	struct i2c_client *client = v4l2_get_subdevdata(&ov13b->sd);
+	s64 max;
+	int ret;
+
+	/* Propagate change of current control to all related controls */
+	switch (ctrl->id) {
+	case V4L2_CID_VBLANK:
+		/* Update max exposure while meeting expected vblanking */
+		max = ov13b->cur_mode->height + ctrl->val - 8;
+		__v4l2_ctrl_modify_range(ov13b->exposure,
+					 ov13b->exposure->minimum,
+					 max, ov13b->exposure->step, max);
+		break;
+	}
+
+	/*
+	 * Applying V4L2 control value only happens
+	 * when power is up for streaming
+	 */
+	if (!pm_runtime_get_if_in_use(&client->dev))
+		return 0;
+
+	ret = 0;
+	switch (ctrl->id) {
+	case V4L2_CID_ANALOGUE_GAIN:
+		ret = ov13b10_write_reg(ov13b, OV13B10_REG_ANALOG_GAIN,
+					OV13B10_REG_VALUE_16BIT,
+					ctrl->val << 1);
+		break;
+	case V4L2_CID_DIGITAL_GAIN:
+		ret = ov13b10_update_digital_gain(ov13b, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE:
+		ret = ov13b10_write_reg(ov13b, OV13B10_REG_EXPOSURE,
+					OV13B10_REG_VALUE_24BIT,
+					ctrl->val);
+		break;
+	case V4L2_CID_VBLANK:
+		ret = ov13b10_write_reg(ov13b, OV13B10_REG_VTS,
+					OV13B10_REG_VALUE_16BIT,
+					ov13b->cur_mode->height
+					+ ctrl->val);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		ret = ov13b10_enable_test_pattern(ov13b, ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+		ov13b10_set_ctrl_hflip(ov13b, ctrl->val);
+		break;
+	case V4L2_CID_VFLIP:
+		ov13b10_set_ctrl_vflip(ov13b, ctrl->val);
+		break;
+	default:
+		dev_info(&client->dev,
+			 "ctrl(id:0x%x,val:0x%x) is not handled\n",
+			 ctrl->id, ctrl->val);
+		break;
+	}
+
+	pm_runtime_put(&client->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ov13b10_ctrl_ops = {
+	.s_ctrl = ov13b10_set_ctrl,
+};
+
+static int ov13b10_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	/* Only one bayer order(GRBG) is supported */
+	if (code->index > 0)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+
+	return 0;
+}
+
+static int ov13b10_enum_frame_size(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->index >= ARRAY_SIZE(supported_modes))
+		return -EINVAL;
+
+	if (fse->code != MEDIA_BUS_FMT_SGRBG10_1X10)
+		return -EINVAL;
+
+	fse->min_width = supported_modes[fse->index].width;
+	fse->max_width = fse->min_width;
+	fse->min_height = supported_modes[fse->index].height;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static void ov13b10_update_pad_format(const struct ov13b10_mode *mode,
+				      struct v4l2_subdev_format *fmt)
+{
+	fmt->format.width = mode->width;
+	fmt->format.height = mode->height;
+	fmt->format.code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	fmt->format.field = V4L2_FIELD_NONE;
+}
+
+static int ov13b10_do_get_pad_format(struct ov13b10 *ov13b,
+				     struct v4l2_subdev_state *sd_state,
+				     struct v4l2_subdev_format *fmt)
+{
+	struct v4l2_mbus_framefmt *framefmt;
+	struct v4l2_subdev *sd = &ov13b->sd;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		framefmt = v4l2_subdev_get_try_format(sd, sd_state, fmt->pad);
+		fmt->format = *framefmt;
+	} else {
+		ov13b10_update_pad_format(ov13b->cur_mode, fmt);
+	}
+
+	return 0;
+}
+
+static int ov13b10_get_pad_format(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_format *fmt)
+{
+	struct ov13b10 *ov13b = to_ov13b10(sd);
+	int ret;
+
+	mutex_lock(&ov13b->mutex);
+	ret = ov13b10_do_get_pad_format(ov13b, sd_state, fmt);
+	mutex_unlock(&ov13b->mutex);
+
+	return ret;
+}
+
+static int
+ov13b10_set_pad_format(struct v4l2_subdev *sd,
+		       struct v4l2_subdev_state *sd_state,
+		       struct v4l2_subdev_format *fmt)
+{
+	struct ov13b10 *ov13b = to_ov13b10(sd);
+	const struct ov13b10_mode *mode;
+	struct v4l2_mbus_framefmt *framefmt;
+	s32 vblank_def;
+	s32 vblank_min;
+	s64 h_blank;
+	s64 pixel_rate;
+	s64 link_freq;
+
+	mutex_lock(&ov13b->mutex);
+
+	/* Only one raw bayer(GRBG) order is supported */
+	if (fmt->format.code != MEDIA_BUS_FMT_SGRBG10_1X10)
+		fmt->format.code = MEDIA_BUS_FMT_SGRBG10_1X10;
+
+	mode = v4l2_find_nearest_size(supported_modes,
+				      ARRAY_SIZE(supported_modes),
+				      width, height,
+				      fmt->format.width, fmt->format.height);
+	ov13b10_update_pad_format(mode, fmt);
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		framefmt = v4l2_subdev_get_try_format(sd, sd_state, fmt->pad);
+		*framefmt = fmt->format;
+	} else {
+		ov13b->cur_mode = mode;
+		__v4l2_ctrl_s_ctrl(ov13b->link_freq, mode->link_freq_index);
+		link_freq = link_freq_menu_items[mode->link_freq_index];
+		pixel_rate = link_freq_to_pixel_rate(link_freq);
+		__v4l2_ctrl_s_ctrl_int64(ov13b->pixel_rate, pixel_rate);
+
+		/* Update limits and set FPS to default */
+		vblank_def = ov13b->cur_mode->vts_def -
+			     ov13b->cur_mode->height;
+		vblank_min = ov13b->cur_mode->vts_min -
+			     ov13b->cur_mode->height;
+		__v4l2_ctrl_modify_range(ov13b->vblank, vblank_min,
+					 OV13B10_VTS_MAX
+					 - ov13b->cur_mode->height,
+					 1,
+					 vblank_def);
+		__v4l2_ctrl_s_ctrl(ov13b->vblank, vblank_def);
+		h_blank =
+			link_freq_configs[mode->link_freq_index].pixels_per_line
+			 - ov13b->cur_mode->width;
+		__v4l2_ctrl_modify_range(ov13b->hblank, h_blank,
+					 h_blank, 1, h_blank);
+	}
+
+	mutex_unlock(&ov13b->mutex);
+
+	return 0;
+}
+
+/* Verify chip ID */
+static int ov13b10_identify_module(struct ov13b10 *ov13b)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov13b->sd);
+	int ret;
+	u32 val;
+
+	if (ov13b->identified)
+		return 0;
+
+	ret = ov13b10_read_reg(ov13b, OV13B10_REG_CHIP_ID,
+			       OV13B10_REG_VALUE_24BIT, &val);
+	if (ret)
+		return ret;
+
+	if (val != OV13B10_CHIP_ID) {
+		dev_err(&client->dev, "chip id mismatch: %x!=%x\n",
+			OV13B10_CHIP_ID, val);
+		return -EIO;
+	}
+
+	ov13b->identified = true;
+
+	return 0;
+}
+
+static int ov13b10_power_off(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov13b10 *ov13b10 = to_ov13b10(sd);
+
+	gpiod_set_value_cansleep(ov13b10->reset, 1);
+
+	if (ov13b10->avdd)
+		regulator_disable(ov13b10->avdd);
+
+	clk_disable_unprepare(ov13b10->img_clk);
+
+	return 0;
+}
+
+static int ov13b10_power_on(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov13b10 *ov13b10 = to_ov13b10(sd);
+	int ret;
+
+	ret = clk_prepare_enable(ov13b10->img_clk);
+	if (ret < 0) {
+		dev_err(dev, "failed to enable imaging clock: %d", ret);
+		return ret;
+	}
+
+	if (ov13b10->avdd) {
+		ret = regulator_enable(ov13b10->avdd);
+		if (ret < 0) {
+			dev_err(dev, "failed to enable avdd: %d", ret);
+			clk_disable_unprepare(ov13b10->img_clk);
+			return ret;
+		}
+	}
+
+	gpiod_set_value_cansleep(ov13b10->reset, 0);
+	/* 5ms to wait ready after XSHUTDN assert */
+	usleep_range(5000, 5500);
+
+	return 0;
+}
+
+static int ov13b10_start_streaming(struct ov13b10 *ov13b)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov13b->sd);
+	const struct ov13b10_reg_list *reg_list;
+	int ret, link_freq_index;
+
+	ret = ov13b10_identify_module(ov13b);
+	if (ret)
+		return ret;
+
+	/* Get out of from software reset */
+	ret = ov13b10_write_reg(ov13b, OV13B10_REG_SOFTWARE_RST,
+				OV13B10_REG_VALUE_08BIT, OV13B10_SOFTWARE_RST);
+	if (ret) {
+		dev_err(&client->dev, "%s failed to set powerup registers\n",
+			__func__);
+		return ret;
+	}
+
+	link_freq_index = ov13b->cur_mode->link_freq_index;
+	reg_list = &link_freq_configs[link_freq_index].reg_list;
+	ret = ov13b10_write_reg_list(ov13b, reg_list);
+	if (ret) {
+		dev_err(&client->dev, "%s failed to set plls\n", __func__);
+		return ret;
+	}
+
+	/* Apply default values of current mode */
+	reg_list = &ov13b->cur_mode->reg_list;
+	ret = ov13b10_write_reg_list(ov13b, reg_list);
+	if (ret) {
+		dev_err(&client->dev, "%s failed to set mode\n", __func__);
+		return ret;
+	}
+
+	/* Apply customized values from user */
+	ret =  __v4l2_ctrl_handler_setup(ov13b->sd.ctrl_handler);
+	if (ret)
+		return ret;
+
+	return ov13b10_write_reg(ov13b, OV13B10_REG_MODE_SELECT,
+				 OV13B10_REG_VALUE_08BIT,
+				 OV13B10_MODE_STREAMING);
+}
+
+/* Stop streaming */
+static int ov13b10_stop_streaming(struct ov13b10 *ov13b)
+{
+	return ov13b10_write_reg(ov13b, OV13B10_REG_MODE_SELECT,
+				 OV13B10_REG_VALUE_08BIT, OV13B10_MODE_STANDBY);
+}
+
+static int ov13b10_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct ov13b10 *ov13b = to_ov13b10(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	mutex_lock(&ov13b->mutex);
+	if (ov13b->streaming == enable) {
+		mutex_unlock(&ov13b->mutex);
+		return 0;
+	}
+
+	if (enable) {
+		ret = pm_runtime_resume_and_get(&client->dev);
+		if (ret < 0)
+			goto err_unlock;
+
+		/*
+		 * Apply default & customized values
+		 * and then start streaming.
+		 */
+		ret = ov13b10_start_streaming(ov13b);
+		if (ret)
+			goto err_rpm_put;
+	} else {
+		ov13b10_stop_streaming(ov13b);
+		pm_runtime_put(&client->dev);
+	}
+
+	ov13b->streaming = enable;
+	mutex_unlock(&ov13b->mutex);
+
+	return ret;
+
+err_rpm_put:
+	pm_runtime_put(&client->dev);
+err_unlock:
+	mutex_unlock(&ov13b->mutex);
+
+	return ret;
+}
+
+static int ov13b10_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov13b10 *ov13b = to_ov13b10(sd);
+
+	if (ov13b->streaming)
+		ov13b10_stop_streaming(ov13b);
+
+	ov13b10_power_off(dev);
+
+	return 0;
+}
+
+static int ov13b10_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov13b10 *ov13b = to_ov13b10(sd);
+	int ret;
+
+	ret = ov13b10_power_on(dev);
+	if (ret)
+		goto pm_fail;
+
+	if (ov13b->streaming) {
+		ret = ov13b10_start_streaming(ov13b);
+		if (ret)
+			goto stop_streaming;
+	}
+
+	return 0;
+
+stop_streaming:
+	ov13b10_stop_streaming(ov13b);
+	ov13b10_power_off(dev);
+pm_fail:
+	ov13b->streaming = false;
+
+	return ret;
+}
+
+static const struct v4l2_subdev_video_ops ov13b10_video_ops = {
+	.s_stream = ov13b10_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops ov13b10_pad_ops = {
+	.enum_mbus_code = ov13b10_enum_mbus_code,
+	.get_fmt = ov13b10_get_pad_format,
+	.set_fmt = ov13b10_set_pad_format,
+	.enum_frame_size = ov13b10_enum_frame_size,
+};
+
+static const struct v4l2_subdev_ops ov13b10_subdev_ops = {
+	.video = &ov13b10_video_ops,
+	.pad = &ov13b10_pad_ops,
+};
+
+static const struct media_entity_operations ov13b10_subdev_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static const struct v4l2_subdev_internal_ops ov13b10_internal_ops = {
+	.open = ov13b10_open,
+};
+
+/* Initialize control handlers */
+static int ov13b10_init_controls(struct ov13b10 *ov13b)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov13b->sd);
+	struct v4l2_fwnode_device_properties props;
+	struct v4l2_ctrl_handler *ctrl_hdlr;
+	s64 exposure_max;
+	s64 vblank_def;
+	s64 vblank_min;
+	s64 hblank;
+	s64 pixel_rate_min;
+	s64 pixel_rate_max;
+	const struct ov13b10_mode *mode;
+	u32 max;
+	int ret;
+
+	ctrl_hdlr = &ov13b->ctrl_handler;
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 10);
+	if (ret)
+		return ret;
+
+	mutex_init(&ov13b->mutex);
+	ctrl_hdlr->lock = &ov13b->mutex;
+	max = ARRAY_SIZE(link_freq_menu_items) - 1;
+	ov13b->link_freq = v4l2_ctrl_new_int_menu(ctrl_hdlr,
+						  &ov13b10_ctrl_ops,
+						  V4L2_CID_LINK_FREQ,
+						  max,
+						  0,
+						  link_freq_menu_items);
+	if (ov13b->link_freq)
+		ov13b->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	pixel_rate_max = link_freq_to_pixel_rate(link_freq_menu_items[0]);
+	pixel_rate_min = 0;
+	/* By default, PIXEL_RATE is read only */
+	ov13b->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, &ov13b10_ctrl_ops,
+					      V4L2_CID_PIXEL_RATE,
+					      pixel_rate_min, pixel_rate_max,
+					      1, pixel_rate_max);
+
+	mode = ov13b->cur_mode;
+	vblank_def = mode->vts_def - mode->height;
+	vblank_min = mode->vts_min - mode->height;
+	ov13b->vblank = v4l2_ctrl_new_std(ctrl_hdlr, &ov13b10_ctrl_ops,
+					  V4L2_CID_VBLANK,
+					  vblank_min,
+					  OV13B10_VTS_MAX - mode->height, 1,
+					  vblank_def);
+
+	hblank = link_freq_configs[mode->link_freq_index].pixels_per_line -
+		 mode->width;
+	ov13b->hblank = v4l2_ctrl_new_std(ctrl_hdlr, &ov13b10_ctrl_ops,
+					  V4L2_CID_HBLANK,
+					  hblank, hblank, 1, hblank);
+	if (ov13b->hblank)
+		ov13b->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	exposure_max = mode->vts_def - 8;
+	ov13b->exposure = v4l2_ctrl_new_std(ctrl_hdlr, &ov13b10_ctrl_ops,
+					    V4L2_CID_EXPOSURE,
+					    OV13B10_EXPOSURE_MIN,
+					    exposure_max, OV13B10_EXPOSURE_STEP,
+					    exposure_max);
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov13b10_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
+			  OV13B10_ANA_GAIN_MIN, OV13B10_ANA_GAIN_MAX,
+			  OV13B10_ANA_GAIN_STEP, OV13B10_ANA_GAIN_DEFAULT);
+
+	/* Digital gain */
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov13b10_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
+			  OV13B10_DGTL_GAIN_MIN, OV13B10_DGTL_GAIN_MAX,
+			  OV13B10_DGTL_GAIN_STEP, OV13B10_DGTL_GAIN_DEFAULT);
+
+	v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &ov13b10_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(ov13b10_test_pattern_menu) - 1,
+				     0, 0, ov13b10_test_pattern_menu);
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov13b10_ctrl_ops,
+			  V4L2_CID_HFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov13b10_ctrl_ops,
+			  V4L2_CID_VFLIP, 0, 1, 1, 0);
+
+	if (ctrl_hdlr->error) {
+		ret = ctrl_hdlr->error;
+		dev_err(&client->dev, "%s control init failed (%d)\n",
+			__func__, ret);
+		goto error;
+	}
+
+	ret = v4l2_fwnode_device_parse(&client->dev, &props);
+	if (ret)
+		goto error;
+
+	ret = v4l2_ctrl_new_fwnode_properties(ctrl_hdlr, &ov13b10_ctrl_ops,
+					      &props);
+	if (ret)
+		goto error;
+
+	ov13b->sd.ctrl_handler = ctrl_hdlr;
+
+	return 0;
+
+error:
+	v4l2_ctrl_handler_free(ctrl_hdlr);
+	mutex_destroy(&ov13b->mutex);
+
+	return ret;
+}
+
+static void ov13b10_free_controls(struct ov13b10 *ov13b)
+{
+	v4l2_ctrl_handler_free(ov13b->sd.ctrl_handler);
+	mutex_destroy(&ov13b->mutex);
+}
+
+static int ov13b10_get_pm_resources(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov13b10 *ov13b = to_ov13b10(sd);
+	int ret;
+
+	ov13b->reset = devm_gpiod_get_optional(dev, "reset", GPIOD_OUT_LOW);
+	if (IS_ERR(ov13b->reset))
+		return dev_err_probe(dev, PTR_ERR(ov13b->reset),
+				     "failed to get reset gpio\n");
+
+	ov13b->img_clk = devm_clk_get_optional(dev, NULL);
+	if (IS_ERR(ov13b->img_clk))
+		return dev_err_probe(dev, PTR_ERR(ov13b->img_clk),
+				     "failed to get imaging clock\n");
+
+	ov13b->avdd = devm_regulator_get_optional(dev, "avdd");
+	if (IS_ERR(ov13b->avdd)) {
+		ret = PTR_ERR(ov13b->avdd);
+		ov13b->avdd = NULL;
+		if (ret != -ENODEV)
+			return dev_err_probe(dev, ret,
+					     "failed to get avdd regulator\n");
+	}
+
+	return 0;
+}
+
+static int ov13b10_check_hwcfg(struct device *dev)
+{
+	struct v4l2_fwnode_endpoint bus_cfg = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY
+	};
+	struct fwnode_handle *ep;
+	struct fwnode_handle *fwnode = dev_fwnode(dev);
+	unsigned int i, j;
+	int ret;
+	u32 ext_clk;
+
+	if (!fwnode)
+		return -ENXIO;
+
+	ep = fwnode_graph_get_next_endpoint(fwnode, NULL);
+	if (!ep)
+		return -EPROBE_DEFER;
+
+	ret = fwnode_property_read_u32(dev_fwnode(dev), "clock-frequency",
+				       &ext_clk);
+	if (ret) {
+		dev_err(dev, "can't get clock frequency");
+		return ret;
+	}
+
+	if (ext_clk != OV13B10_EXT_CLK) {
+		dev_err(dev, "external clock %d is not supported",
+			ext_clk);
+		return -EINVAL;
+	}
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(ep, &bus_cfg);
+	fwnode_handle_put(ep);
+	if (ret)
+		return ret;
+
+	if (bus_cfg.bus.mipi_csi2.num_data_lanes != OV13B10_DATA_LANES) {
+		dev_err(dev, "number of CSI2 data lanes %d is not supported",
+			bus_cfg.bus.mipi_csi2.num_data_lanes);
+		ret = -EINVAL;
+		goto out_err;
+	}
+
+	if (!bus_cfg.nr_of_link_frequencies) {
+		dev_err(dev, "no link frequencies defined");
+		ret = -EINVAL;
+		goto out_err;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(link_freq_menu_items); i++) {
+		for (j = 0; j < bus_cfg.nr_of_link_frequencies; j++) {
+			if (link_freq_menu_items[i] ==
+				bus_cfg.link_frequencies[j])
+				break;
+		}
+
+		if (j == bus_cfg.nr_of_link_frequencies) {
+			dev_err(dev, "no link frequency %lld supported",
+				link_freq_menu_items[i]);
+			ret = -EINVAL;
+			goto out_err;
+		}
+	}
+
+out_err:
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+
+	return ret;
+}
+
+static int ov13b10_probe(struct i2c_client *client)
+{
+	struct ov13b10 *ov13b;
+	bool full_power;
+	int ret;
+
+	/* Check HW config */
+	ret = ov13b10_check_hwcfg(&client->dev);
+	if (ret) {
+		dev_err(&client->dev, "failed to check hwcfg: %d", ret);
+		return ret;
+	}
+
+	ov13b = devm_kzalloc(&client->dev, sizeof(*ov13b), GFP_KERNEL);
+	if (!ov13b)
+		return -ENOMEM;
+
+	/* Initialize subdev */
+	v4l2_i2c_subdev_init(&ov13b->sd, client, &ov13b10_subdev_ops);
+
+	ret = ov13b10_get_pm_resources(&client->dev);
+	if (ret)
+		return ret;
+
+	full_power = acpi_dev_state_d0(&client->dev);
+	if (full_power) {
+		ret = ov13b10_power_on(&client->dev);
+		if (ret) {
+			dev_err(&client->dev, "failed to power on\n");
+			return ret;
+		}
+
+		/* Check module identity */
+		ret = ov13b10_identify_module(ov13b);
+		if (ret) {
+			dev_err(&client->dev, "failed to find sensor: %d\n", ret);
+			goto error_power_off;
+		}
+	}
+
+	/* Set default mode to max resolution */
+	ov13b->cur_mode = &supported_modes[0];
+
+	ret = ov13b10_init_controls(ov13b);
+	if (ret)
+		goto error_power_off;
+
+	/* Initialize subdev */
+	ov13b->sd.internal_ops = &ov13b10_internal_ops;
+	ov13b->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	ov13b->sd.entity.ops = &ov13b10_subdev_entity_ops;
+	ov13b->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+	/* Initialize source pad */
+	ov13b->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&ov13b->sd.entity, 1, &ov13b->pad);
+	if (ret) {
+		dev_err(&client->dev, "%s failed:%d\n", __func__, ret);
+		goto error_handler_free;
+	}
+
+
+	/*
+	 * Device is already turned on by i2c-core with ACPI domain PM.
+	 * Enable runtime PM and turn off the device.
+	 */
+	/* Set the device's state to active if it's in D0 state. */
+	if (full_power)
+		pm_runtime_set_active(&client->dev);
+	pm_runtime_enable(&client->dev);
+	pm_runtime_idle(&client->dev);
+
+	ret = v4l2_async_register_subdev_sensor(&ov13b->sd);
+	if (ret < 0)
+		goto error_media_entity_runtime_pm;
+
+	return 0;
+
+error_media_entity_runtime_pm:
+	pm_runtime_disable(&client->dev);
+	if (full_power)
+		pm_runtime_set_suspended(&client->dev);
+	media_entity_cleanup(&ov13b->sd.entity);
+
+error_handler_free:
+	ov13b10_free_controls(ov13b);
+	dev_err(&client->dev, "%s failed:%d\n", __func__, ret);
+
+error_power_off:
+	ov13b10_power_off(&client->dev);
+
+	return ret;
+}
+
+static void ov13b10_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov13b10 *ov13b = to_ov13b10(sd);
+
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	ov13b10_free_controls(ov13b);
+
+	pm_runtime_disable(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+}
+
+static DEFINE_RUNTIME_DEV_PM_OPS(ov13b10_pm_ops, ov13b10_suspend,
+				 ov13b10_resume, NULL);
+
+#ifdef CONFIG_ACPI
+static const struct acpi_device_id ov13b10_acpi_ids[] = {
+	{"OVTIDB10"},
+	{"OVTI13B1"},
+	{ /* sentinel */ }
+};
+
+MODULE_DEVICE_TABLE(acpi, ov13b10_acpi_ids);
+#endif
+
+static struct i2c_driver ov13b10_i2c_driver = {
+	.driver = {
+		.name = "ov13b10",
+		.pm = pm_ptr(&ov13b10_pm_ops),
+		.acpi_match_table = ACPI_PTR(ov13b10_acpi_ids),
+	},
+	.probe = ov13b10_probe,
+	.remove = ov13b10_remove,
+	.flags = I2C_DRV_ACPI_WAIVE_D0_PROBE,
+};
+
+module_i2c_driver(ov13b10_i2c_driver);
+
+MODULE_AUTHOR("Kao, Arec <arec.kao@intel.com>");
+MODULE_DESCRIPTION("Omnivision ov13b10 sensor driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/ov2640.c b/drivers/media/i2c/ov2640.c
new file mode 100644
index 000000000..bb6c9863a
--- /dev/null
+++ b/drivers/media/i2c/ov2640.c
@@ -0,0 +1,1310 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * ov2640 Camera Driver
+ *
+ * Copyright (C) 2010 Alberto Panizzo <maramaopercheseimorto@gmail.com>
+ *
+ * Based on ov772x, ov9640 drivers and previous non merged implementations.
+ *
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2006, OmniVision
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/clk.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/v4l2-mediabus.h>
+#include <linux/videodev2.h>
+
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-image-sizes.h>
+
+#define VAL_SET(x, mask, rshift, lshift)  \
+		((((x) >> rshift) & mask) << lshift)
+/*
+ * DSP registers
+ * register offset for BANK_SEL == BANK_SEL_DSP
+ */
+#define R_BYPASS    0x05 /* Bypass DSP */
+#define   R_BYPASS_DSP_BYPAS    0x01 /* Bypass DSP, sensor out directly */
+#define   R_BYPASS_USE_DSP      0x00 /* Use the internal DSP */
+#define QS          0x44 /* Quantization Scale Factor */
+#define CTRLI       0x50
+#define   CTRLI_LP_DP           0x80
+#define   CTRLI_ROUND           0x40
+#define   CTRLI_V_DIV_SET(x)    VAL_SET(x, 0x3, 0, 3)
+#define   CTRLI_H_DIV_SET(x)    VAL_SET(x, 0x3, 0, 0)
+#define HSIZE       0x51 /* H_SIZE[7:0] (real/4) */
+#define   HSIZE_SET(x)          VAL_SET(x, 0xFF, 2, 0)
+#define VSIZE       0x52 /* V_SIZE[7:0] (real/4) */
+#define   VSIZE_SET(x)          VAL_SET(x, 0xFF, 2, 0)
+#define XOFFL       0x53 /* OFFSET_X[7:0] */
+#define   XOFFL_SET(x)          VAL_SET(x, 0xFF, 0, 0)
+#define YOFFL       0x54 /* OFFSET_Y[7:0] */
+#define   YOFFL_SET(x)          VAL_SET(x, 0xFF, 0, 0)
+#define VHYX        0x55 /* Offset and size completion */
+#define   VHYX_VSIZE_SET(x)     VAL_SET(x, 0x1, (8+2), 7)
+#define   VHYX_HSIZE_SET(x)     VAL_SET(x, 0x1, (8+2), 3)
+#define   VHYX_YOFF_SET(x)      VAL_SET(x, 0x3, 8, 4)
+#define   VHYX_XOFF_SET(x)      VAL_SET(x, 0x3, 8, 0)
+#define DPRP        0x56
+#define TEST        0x57 /* Horizontal size completion */
+#define   TEST_HSIZE_SET(x)     VAL_SET(x, 0x1, (9+2), 7)
+#define ZMOW        0x5A /* Zoom: Out Width  OUTW[7:0] (real/4) */
+#define   ZMOW_OUTW_SET(x)      VAL_SET(x, 0xFF, 2, 0)
+#define ZMOH        0x5B /* Zoom: Out Height OUTH[7:0] (real/4) */
+#define   ZMOH_OUTH_SET(x)      VAL_SET(x, 0xFF, 2, 0)
+#define ZMHH        0x5C /* Zoom: Speed and H&W completion */
+#define   ZMHH_ZSPEED_SET(x)    VAL_SET(x, 0x0F, 0, 4)
+#define   ZMHH_OUTH_SET(x)      VAL_SET(x, 0x1, (8+2), 2)
+#define   ZMHH_OUTW_SET(x)      VAL_SET(x, 0x3, (8+2), 0)
+#define BPADDR      0x7C /* SDE Indirect Register Access: Address */
+#define BPDATA      0x7D /* SDE Indirect Register Access: Data */
+#define CTRL2       0x86 /* DSP Module enable 2 */
+#define   CTRL2_DCW_EN          0x20
+#define   CTRL2_SDE_EN          0x10
+#define   CTRL2_UV_ADJ_EN       0x08
+#define   CTRL2_UV_AVG_EN       0x04
+#define   CTRL2_CMX_EN          0x01
+#define CTRL3       0x87 /* DSP Module enable 3 */
+#define   CTRL3_BPC_EN          0x80
+#define   CTRL3_WPC_EN          0x40
+#define SIZEL       0x8C /* Image Size Completion */
+#define   SIZEL_HSIZE8_11_SET(x) VAL_SET(x, 0x1, 11, 6)
+#define   SIZEL_HSIZE8_SET(x)    VAL_SET(x, 0x7, 0, 3)
+#define   SIZEL_VSIZE8_SET(x)    VAL_SET(x, 0x7, 0, 0)
+#define HSIZE8      0xC0 /* Image Horizontal Size HSIZE[10:3] */
+#define   HSIZE8_SET(x)         VAL_SET(x, 0xFF, 3, 0)
+#define VSIZE8      0xC1 /* Image Vertical Size VSIZE[10:3] */
+#define   VSIZE8_SET(x)         VAL_SET(x, 0xFF, 3, 0)
+#define CTRL0       0xC2 /* DSP Module enable 0 */
+#define   CTRL0_AEC_EN       0x80
+#define   CTRL0_AEC_SEL      0x40
+#define   CTRL0_STAT_SEL     0x20
+#define   CTRL0_VFIRST       0x10
+#define   CTRL0_YUV422       0x08
+#define   CTRL0_YUV_EN       0x04
+#define   CTRL0_RGB_EN       0x02
+#define   CTRL0_RAW_EN       0x01
+#define CTRL1       0xC3 /* DSP Module enable 1 */
+#define   CTRL1_CIP          0x80
+#define   CTRL1_DMY          0x40
+#define   CTRL1_RAW_GMA      0x20
+#define   CTRL1_DG           0x10
+#define   CTRL1_AWB          0x08
+#define   CTRL1_AWB_GAIN     0x04
+#define   CTRL1_LENC         0x02
+#define   CTRL1_PRE          0x01
+/*      REG 0xC7 (unknown name): affects Auto White Balance (AWB)
+ *	  AWB_OFF            0x40
+ *	  AWB_SIMPLE         0x10
+ *	  AWB_ON             0x00	(Advanced AWB ?) */
+#define R_DVP_SP    0xD3 /* DVP output speed control */
+#define   R_DVP_SP_AUTO_MODE 0x80
+#define   R_DVP_SP_DVP_MASK  0x3F /* DVP PCLK = sysclk (48)/[6:0] (YUV0);
+				   *          = sysclk (48)/(2*[6:0]) (RAW);*/
+#define IMAGE_MODE  0xDA /* Image Output Format Select */
+#define   IMAGE_MODE_Y8_DVP_EN   0x40
+#define   IMAGE_MODE_JPEG_EN     0x10
+#define   IMAGE_MODE_YUV422      0x00
+#define   IMAGE_MODE_RAW10       0x04 /* (DVP) */
+#define   IMAGE_MODE_RGB565      0x08
+#define   IMAGE_MODE_HREF_VSYNC  0x02 /* HREF timing select in DVP JPEG output
+				       * mode (0 for HREF is same as sensor) */
+#define   IMAGE_MODE_LBYTE_FIRST 0x01 /* Byte swap enable for DVP
+				       *    1: Low byte first UYVY (C2[4] =0)
+				       *        VYUY (C2[4] =1)
+				       *    0: High byte first YUYV (C2[4]=0)
+				       *        YVYU (C2[4] = 1) */
+#define RESET       0xE0 /* Reset */
+#define   RESET_MICROC       0x40
+#define   RESET_SCCB         0x20
+#define   RESET_JPEG         0x10
+#define   RESET_DVP          0x04
+#define   RESET_IPU          0x02
+#define   RESET_CIF          0x01
+#define REGED       0xED /* Register ED */
+#define   REGED_CLK_OUT_DIS  0x10
+#define MS_SP       0xF0 /* SCCB Master Speed */
+#define SS_ID       0xF7 /* SCCB Slave ID */
+#define SS_CTRL     0xF8 /* SCCB Slave Control */
+#define   SS_CTRL_ADD_AUTO_INC  0x20
+#define   SS_CTRL_EN            0x08
+#define   SS_CTRL_DELAY_CLK     0x04
+#define   SS_CTRL_ACC_EN        0x02
+#define   SS_CTRL_SEN_PASS_THR  0x01
+#define MC_BIST     0xF9 /* Microcontroller misc register */
+#define   MC_BIST_RESET           0x80 /* Microcontroller Reset */
+#define   MC_BIST_BOOT_ROM_SEL    0x40
+#define   MC_BIST_12KB_SEL        0x20
+#define   MC_BIST_12KB_MASK       0x30
+#define   MC_BIST_512KB_SEL       0x08
+#define   MC_BIST_512KB_MASK      0x0C
+#define   MC_BIST_BUSY_BIT_R      0x02
+#define   MC_BIST_MC_RES_ONE_SH_W 0x02
+#define   MC_BIST_LAUNCH          0x01
+#define BANK_SEL    0xFF /* Register Bank Select */
+#define   BANK_SEL_DSP     0x00
+#define   BANK_SEL_SENS    0x01
+
+/*
+ * Sensor registers
+ * register offset for BANK_SEL == BANK_SEL_SENS
+ */
+#define GAIN        0x00 /* AGC - Gain control gain setting */
+#define COM1        0x03 /* Common control 1 */
+#define   COM1_1_DUMMY_FR          0x40
+#define   COM1_3_DUMMY_FR          0x80
+#define   COM1_7_DUMMY_FR          0xC0
+#define   COM1_VWIN_LSB_UXGA       0x0F
+#define   COM1_VWIN_LSB_SVGA       0x0A
+#define   COM1_VWIN_LSB_CIF        0x06
+#define REG04       0x04 /* Register 04 */
+#define   REG04_DEF             0x20 /* Always set */
+#define   REG04_HFLIP_IMG       0x80 /* Horizontal mirror image ON/OFF */
+#define   REG04_VFLIP_IMG       0x40 /* Vertical flip image ON/OFF */
+#define   REG04_VREF_EN         0x10
+#define   REG04_HREF_EN         0x08
+#define   REG04_AEC_SET(x)      VAL_SET(x, 0x3, 0, 0)
+#define REG08       0x08 /* Frame Exposure One-pin Control Pre-charge Row Num */
+#define COM2        0x09 /* Common control 2 */
+#define   COM2_SOFT_SLEEP_MODE  0x10 /* Soft sleep mode */
+				     /* Output drive capability */
+#define   COM2_OCAP_Nx_SET(N)   (((N) - 1) & 0x03) /* N = [1x .. 4x] */
+#define PID         0x0A /* Product ID Number MSB */
+#define VER         0x0B /* Product ID Number LSB */
+#define COM3        0x0C /* Common control 3 */
+#define   COM3_BAND_50H        0x04 /* 0 For Banding at 60H */
+#define   COM3_BAND_AUTO       0x02 /* Auto Banding */
+#define   COM3_SING_FR_SNAPSH  0x01 /* 0 For enable live video output after the
+				     * snapshot sequence*/
+#define AEC         0x10 /* AEC[9:2] Exposure Value */
+#define CLKRC       0x11 /* Internal clock */
+#define   CLKRC_EN             0x80
+#define   CLKRC_DIV_SET(x)     (((x) - 1) & 0x1F) /* CLK = XVCLK/(x) */
+#define COM7        0x12 /* Common control 7 */
+#define   COM7_SRST            0x80 /* Initiates system reset. All registers are
+				     * set to factory default values after which
+				     * the chip resumes normal operation */
+#define   COM7_RES_UXGA        0x00 /* Resolution selectors for UXGA */
+#define   COM7_RES_SVGA        0x40 /* SVGA */
+#define   COM7_RES_CIF         0x20 /* CIF */
+#define   COM7_ZOOM_EN         0x04 /* Enable Zoom mode */
+#define   COM7_COLOR_BAR_TEST  0x02 /* Enable Color Bar Test Pattern */
+#define COM8        0x13 /* Common control 8 */
+#define   COM8_DEF             0xC0
+#define   COM8_BNDF_EN         0x20 /* Banding filter ON/OFF */
+#define   COM8_AGC_EN          0x04 /* AGC Auto/Manual control selection */
+#define   COM8_AEC_EN          0x01 /* Auto/Manual Exposure control */
+#define COM9        0x14 /* Common control 9
+			  * Automatic gain ceiling - maximum AGC value [7:5]*/
+#define   COM9_AGC_GAIN_2x     0x00 /* 000 :   2x */
+#define   COM9_AGC_GAIN_4x     0x20 /* 001 :   4x */
+#define   COM9_AGC_GAIN_8x     0x40 /* 010 :   8x */
+#define   COM9_AGC_GAIN_16x    0x60 /* 011 :  16x */
+#define   COM9_AGC_GAIN_32x    0x80 /* 100 :  32x */
+#define   COM9_AGC_GAIN_64x    0xA0 /* 101 :  64x */
+#define   COM9_AGC_GAIN_128x   0xC0 /* 110 : 128x */
+#define COM10       0x15 /* Common control 10 */
+#define   COM10_PCLK_HREF      0x20 /* PCLK output qualified by HREF */
+#define   COM10_PCLK_RISE      0x10 /* Data is updated at the rising edge of
+				     * PCLK (user can latch data at the next
+				     * falling edge of PCLK).
+				     * 0 otherwise. */
+#define   COM10_HREF_INV       0x08 /* Invert HREF polarity:
+				     * HREF negative for valid data*/
+#define   COM10_VSINC_INV      0x02 /* Invert VSYNC polarity */
+#define HSTART      0x17 /* Horizontal Window start MSB 8 bit */
+#define HEND        0x18 /* Horizontal Window end MSB 8 bit */
+#define VSTART      0x19 /* Vertical Window start MSB 8 bit */
+#define VEND        0x1A /* Vertical Window end MSB 8 bit */
+#define MIDH        0x1C /* Manufacturer ID byte - high */
+#define MIDL        0x1D /* Manufacturer ID byte - low  */
+#define AEW         0x24 /* AGC/AEC - Stable operating region (upper limit) */
+#define AEB         0x25 /* AGC/AEC - Stable operating region (lower limit) */
+#define VV          0x26 /* AGC/AEC Fast mode operating region */
+#define   VV_HIGH_TH_SET(x)      VAL_SET(x, 0xF, 0, 4)
+#define   VV_LOW_TH_SET(x)       VAL_SET(x, 0xF, 0, 0)
+#define REG2A       0x2A /* Dummy pixel insert MSB */
+#define FRARL       0x2B /* Dummy pixel insert LSB */
+#define ADDVFL      0x2D /* LSB of insert dummy lines in Vertical direction */
+#define ADDVFH      0x2E /* MSB of insert dummy lines in Vertical direction */
+#define YAVG        0x2F /* Y/G Channel Average value */
+#define REG32       0x32 /* Common Control 32 */
+#define   REG32_PCLK_DIV_2    0x80 /* PCLK freq divided by 2 */
+#define   REG32_PCLK_DIV_4    0xC0 /* PCLK freq divided by 4 */
+#define ARCOM2      0x34 /* Zoom: Horizontal start point */
+#define REG45       0x45 /* Register 45 */
+#define FLL         0x46 /* Frame Length Adjustment LSBs */
+#define FLH         0x47 /* Frame Length Adjustment MSBs */
+#define COM19       0x48 /* Zoom: Vertical start point */
+#define ZOOMS       0x49 /* Zoom: Vertical start point */
+#define COM22       0x4B /* Flash light control */
+#define COM25       0x4E /* For Banding operations */
+#define   COM25_50HZ_BANDING_AEC_MSBS_MASK      0xC0 /* 50Hz Bd. AEC 2 MSBs */
+#define   COM25_60HZ_BANDING_AEC_MSBS_MASK      0x30 /* 60Hz Bd. AEC 2 MSBs */
+#define   COM25_50HZ_BANDING_AEC_MSBS_SET(x)    VAL_SET(x, 0x3, 8, 6)
+#define   COM25_60HZ_BANDING_AEC_MSBS_SET(x)    VAL_SET(x, 0x3, 8, 4)
+#define BD50        0x4F /* 50Hz Banding AEC 8 LSBs */
+#define   BD50_50HZ_BANDING_AEC_LSBS_SET(x)     VAL_SET(x, 0xFF, 0, 0)
+#define BD60        0x50 /* 60Hz Banding AEC 8 LSBs */
+#define   BD60_60HZ_BANDING_AEC_LSBS_SET(x)     VAL_SET(x, 0xFF, 0, 0)
+#define REG5A       0x5A /* 50/60Hz Banding Maximum AEC Step */
+#define   BD50_MAX_AEC_STEP_MASK         0xF0 /* 50Hz Banding Max. AEC Step */
+#define   BD60_MAX_AEC_STEP_MASK         0x0F /* 60Hz Banding Max. AEC Step */
+#define   BD50_MAX_AEC_STEP_SET(x)       VAL_SET((x - 1), 0x0F, 0, 4)
+#define   BD60_MAX_AEC_STEP_SET(x)       VAL_SET((x - 1), 0x0F, 0, 0)
+#define REG5D       0x5D /* AVGsel[7:0],   16-zone average weight option */
+#define REG5E       0x5E /* AVGsel[15:8],  16-zone average weight option */
+#define REG5F       0x5F /* AVGsel[23:16], 16-zone average weight option */
+#define REG60       0x60 /* AVGsel[31:24], 16-zone average weight option */
+#define HISTO_LOW   0x61 /* Histogram Algorithm Low Level */
+#define HISTO_HIGH  0x62 /* Histogram Algorithm High Level */
+
+/*
+ * ID
+ */
+#define MANUFACTURER_ID	0x7FA2
+#define PID_OV2640	0x2642
+#define VERSION(pid, ver) ((pid << 8) | (ver & 0xFF))
+
+/*
+ * Struct
+ */
+struct regval_list {
+	u8 reg_num;
+	u8 value;
+};
+
+struct ov2640_win_size {
+	char				*name;
+	u32				width;
+	u32				height;
+	const struct regval_list	*regs;
+};
+
+
+struct ov2640_priv {
+	struct v4l2_subdev		subdev;
+#if defined(CONFIG_MEDIA_CONTROLLER)
+	struct media_pad pad;
+#endif
+	struct v4l2_ctrl_handler	hdl;
+	u32	cfmt_code;
+	struct clk			*clk;
+	const struct ov2640_win_size	*win;
+
+	struct gpio_desc *resetb_gpio;
+	struct gpio_desc *pwdn_gpio;
+
+	struct mutex lock; /* lock to protect streaming and power_count */
+	bool streaming;
+	int power_count;
+};
+
+/*
+ * Registers settings
+ */
+
+#define ENDMARKER { 0xff, 0xff }
+
+static const struct regval_list ov2640_init_regs[] = {
+	{ BANK_SEL, BANK_SEL_DSP },
+	{ 0x2c,   0xff },
+	{ 0x2e,   0xdf },
+	{ BANK_SEL, BANK_SEL_SENS },
+	{ 0x3c,   0x32 },
+	{ CLKRC,  CLKRC_DIV_SET(1) },
+	{ COM2,   COM2_OCAP_Nx_SET(3) },
+	{ REG04,  REG04_DEF | REG04_HREF_EN },
+	{ COM8,   COM8_DEF | COM8_BNDF_EN | COM8_AGC_EN | COM8_AEC_EN },
+	{ COM9,   COM9_AGC_GAIN_8x | 0x08},
+	{ 0x2c,   0x0c },
+	{ 0x33,   0x78 },
+	{ 0x3a,   0x33 },
+	{ 0x3b,   0xfb },
+	{ 0x3e,   0x00 },
+	{ 0x43,   0x11 },
+	{ 0x16,   0x10 },
+	{ 0x39,   0x02 },
+	{ 0x35,   0x88 },
+	{ 0x22,   0x0a },
+	{ 0x37,   0x40 },
+	{ 0x23,   0x00 },
+	{ ARCOM2, 0xa0 },
+	{ 0x06,   0x02 },
+	{ 0x06,   0x88 },
+	{ 0x07,   0xc0 },
+	{ 0x0d,   0xb7 },
+	{ 0x0e,   0x01 },
+	{ 0x4c,   0x00 },
+	{ 0x4a,   0x81 },
+	{ 0x21,   0x99 },
+	{ AEW,    0x40 },
+	{ AEB,    0x38 },
+	{ VV,     VV_HIGH_TH_SET(0x08) | VV_LOW_TH_SET(0x02) },
+	{ 0x5c,   0x00 },
+	{ 0x63,   0x00 },
+	{ FLL,    0x22 },
+	{ COM3,   0x38 | COM3_BAND_AUTO },
+	{ REG5D,  0x55 },
+	{ REG5E,  0x7d },
+	{ REG5F,  0x7d },
+	{ REG60,  0x55 },
+	{ HISTO_LOW,   0x70 },
+	{ HISTO_HIGH,  0x80 },
+	{ 0x7c,   0x05 },
+	{ 0x20,   0x80 },
+	{ 0x28,   0x30 },
+	{ 0x6c,   0x00 },
+	{ 0x6d,   0x80 },
+	{ 0x6e,   0x00 },
+	{ 0x70,   0x02 },
+	{ 0x71,   0x94 },
+	{ 0x73,   0xc1 },
+	{ 0x3d,   0x34 },
+	{ COM7,   COM7_RES_UXGA | COM7_ZOOM_EN },
+	{ REG5A,  BD50_MAX_AEC_STEP_SET(6)
+		   | BD60_MAX_AEC_STEP_SET(8) },		/* 0x57 */
+	{ COM25,  COM25_50HZ_BANDING_AEC_MSBS_SET(0x0bb)
+		   | COM25_60HZ_BANDING_AEC_MSBS_SET(0x09c) },	/* 0x00 */
+	{ BD50,   BD50_50HZ_BANDING_AEC_LSBS_SET(0x0bb) },	/* 0xbb */
+	{ BD60,   BD60_60HZ_BANDING_AEC_LSBS_SET(0x09c) },	/* 0x9c */
+	{ BANK_SEL,  BANK_SEL_DSP },
+	{ 0xe5,   0x7f },
+	{ MC_BIST,  MC_BIST_RESET | MC_BIST_BOOT_ROM_SEL },
+	{ 0x41,   0x24 },
+	{ RESET,  RESET_JPEG | RESET_DVP },
+	{ 0x76,   0xff },
+	{ 0x33,   0xa0 },
+	{ 0x42,   0x20 },
+	{ 0x43,   0x18 },
+	{ 0x4c,   0x00 },
+	{ CTRL3,  CTRL3_BPC_EN | CTRL3_WPC_EN | 0x10 },
+	{ 0x88,   0x3f },
+	{ 0xd7,   0x03 },
+	{ 0xd9,   0x10 },
+	{ R_DVP_SP,  R_DVP_SP_AUTO_MODE | 0x2 },
+	{ 0xc8,   0x08 },
+	{ 0xc9,   0x80 },
+	{ BPADDR, 0x00 },
+	{ BPDATA, 0x00 },
+	{ BPADDR, 0x03 },
+	{ BPDATA, 0x48 },
+	{ BPDATA, 0x48 },
+	{ BPADDR, 0x08 },
+	{ BPDATA, 0x20 },
+	{ BPDATA, 0x10 },
+	{ BPDATA, 0x0e },
+	{ 0x90,   0x00 },
+	{ 0x91,   0x0e },
+	{ 0x91,   0x1a },
+	{ 0x91,   0x31 },
+	{ 0x91,   0x5a },
+	{ 0x91,   0x69 },
+	{ 0x91,   0x75 },
+	{ 0x91,   0x7e },
+	{ 0x91,   0x88 },
+	{ 0x91,   0x8f },
+	{ 0x91,   0x96 },
+	{ 0x91,   0xa3 },
+	{ 0x91,   0xaf },
+	{ 0x91,   0xc4 },
+	{ 0x91,   0xd7 },
+	{ 0x91,   0xe8 },
+	{ 0x91,   0x20 },
+	{ 0x92,   0x00 },
+	{ 0x93,   0x06 },
+	{ 0x93,   0xe3 },
+	{ 0x93,   0x03 },
+	{ 0x93,   0x03 },
+	{ 0x93,   0x00 },
+	{ 0x93,   0x02 },
+	{ 0x93,   0x00 },
+	{ 0x93,   0x00 },
+	{ 0x93,   0x00 },
+	{ 0x93,   0x00 },
+	{ 0x93,   0x00 },
+	{ 0x93,   0x00 },
+	{ 0x93,   0x00 },
+	{ 0x96,   0x00 },
+	{ 0x97,   0x08 },
+	{ 0x97,   0x19 },
+	{ 0x97,   0x02 },
+	{ 0x97,   0x0c },
+	{ 0x97,   0x24 },
+	{ 0x97,   0x30 },
+	{ 0x97,   0x28 },
+	{ 0x97,   0x26 },
+	{ 0x97,   0x02 },
+	{ 0x97,   0x98 },
+	{ 0x97,   0x80 },
+	{ 0x97,   0x00 },
+	{ 0x97,   0x00 },
+	{ 0xa4,   0x00 },
+	{ 0xa8,   0x00 },
+	{ 0xc5,   0x11 },
+	{ 0xc6,   0x51 },
+	{ 0xbf,   0x80 },
+	{ 0xc7,   0x10 },	/* simple AWB */
+	{ 0xb6,   0x66 },
+	{ 0xb8,   0xA5 },
+	{ 0xb7,   0x64 },
+	{ 0xb9,   0x7C },
+	{ 0xb3,   0xaf },
+	{ 0xb4,   0x97 },
+	{ 0xb5,   0xFF },
+	{ 0xb0,   0xC5 },
+	{ 0xb1,   0x94 },
+	{ 0xb2,   0x0f },
+	{ 0xc4,   0x5c },
+	{ 0xa6,   0x00 },
+	{ 0xa7,   0x20 },
+	{ 0xa7,   0xd8 },
+	{ 0xa7,   0x1b },
+	{ 0xa7,   0x31 },
+	{ 0xa7,   0x00 },
+	{ 0xa7,   0x18 },
+	{ 0xa7,   0x20 },
+	{ 0xa7,   0xd8 },
+	{ 0xa7,   0x19 },
+	{ 0xa7,   0x31 },
+	{ 0xa7,   0x00 },
+	{ 0xa7,   0x18 },
+	{ 0xa7,   0x20 },
+	{ 0xa7,   0xd8 },
+	{ 0xa7,   0x19 },
+	{ 0xa7,   0x31 },
+	{ 0xa7,   0x00 },
+	{ 0xa7,   0x18 },
+	{ 0x7f,   0x00 },
+	{ 0xe5,   0x1f },
+	{ 0xe1,   0x77 },
+	{ 0xdd,   0x7f },
+	{ CTRL0,  CTRL0_YUV422 | CTRL0_YUV_EN | CTRL0_RGB_EN },
+	ENDMARKER,
+};
+
+/*
+ * Register settings for window size
+ * The preamble, setup the internal DSP to input an UXGA (1600x1200) image.
+ * Then the different zooming configurations will setup the output image size.
+ */
+static const struct regval_list ov2640_size_change_preamble_regs[] = {
+	{ BANK_SEL, BANK_SEL_DSP },
+	{ RESET, RESET_DVP },
+	{ SIZEL, SIZEL_HSIZE8_11_SET(UXGA_WIDTH) |
+		 SIZEL_HSIZE8_SET(UXGA_WIDTH) |
+		 SIZEL_VSIZE8_SET(UXGA_HEIGHT) },
+	{ HSIZE8, HSIZE8_SET(UXGA_WIDTH) },
+	{ VSIZE8, VSIZE8_SET(UXGA_HEIGHT) },
+	{ CTRL2, CTRL2_DCW_EN | CTRL2_SDE_EN |
+		 CTRL2_UV_AVG_EN | CTRL2_CMX_EN | CTRL2_UV_ADJ_EN },
+	{ HSIZE, HSIZE_SET(UXGA_WIDTH) },
+	{ VSIZE, VSIZE_SET(UXGA_HEIGHT) },
+	{ XOFFL, XOFFL_SET(0) },
+	{ YOFFL, YOFFL_SET(0) },
+	{ VHYX, VHYX_HSIZE_SET(UXGA_WIDTH) | VHYX_VSIZE_SET(UXGA_HEIGHT) |
+		VHYX_XOFF_SET(0) | VHYX_YOFF_SET(0)},
+	{ TEST, TEST_HSIZE_SET(UXGA_WIDTH) },
+	ENDMARKER,
+};
+
+#define PER_SIZE_REG_SEQ(x, y, v_div, h_div, pclk_div)	\
+	{ CTRLI, CTRLI_LP_DP | CTRLI_V_DIV_SET(v_div) |	\
+		 CTRLI_H_DIV_SET(h_div)},		\
+	{ ZMOW, ZMOW_OUTW_SET(x) },			\
+	{ ZMOH, ZMOH_OUTH_SET(y) },			\
+	{ ZMHH, ZMHH_OUTW_SET(x) | ZMHH_OUTH_SET(y) },	\
+	{ R_DVP_SP, pclk_div },				\
+	{ RESET, 0x00}
+
+static const struct regval_list ov2640_qcif_regs[] = {
+	PER_SIZE_REG_SEQ(QCIF_WIDTH, QCIF_HEIGHT, 3, 3, 4),
+	ENDMARKER,
+};
+
+static const struct regval_list ov2640_qvga_regs[] = {
+	PER_SIZE_REG_SEQ(QVGA_WIDTH, QVGA_HEIGHT, 2, 2, 4),
+	ENDMARKER,
+};
+
+static const struct regval_list ov2640_cif_regs[] = {
+	PER_SIZE_REG_SEQ(CIF_WIDTH, CIF_HEIGHT, 2, 2, 8),
+	ENDMARKER,
+};
+
+static const struct regval_list ov2640_vga_regs[] = {
+	PER_SIZE_REG_SEQ(VGA_WIDTH, VGA_HEIGHT, 0, 0, 2),
+	ENDMARKER,
+};
+
+static const struct regval_list ov2640_svga_regs[] = {
+	PER_SIZE_REG_SEQ(SVGA_WIDTH, SVGA_HEIGHT, 1, 1, 2),
+	ENDMARKER,
+};
+
+static const struct regval_list ov2640_xga_regs[] = {
+	PER_SIZE_REG_SEQ(XGA_WIDTH, XGA_HEIGHT, 0, 0, 2),
+	{ CTRLI,    0x00},
+	ENDMARKER,
+};
+
+static const struct regval_list ov2640_sxga_regs[] = {
+	PER_SIZE_REG_SEQ(SXGA_WIDTH, SXGA_HEIGHT, 0, 0, 2),
+	{ CTRLI,    0x00},
+	{ R_DVP_SP, 2 | R_DVP_SP_AUTO_MODE },
+	ENDMARKER,
+};
+
+static const struct regval_list ov2640_uxga_regs[] = {
+	PER_SIZE_REG_SEQ(UXGA_WIDTH, UXGA_HEIGHT, 0, 0, 0),
+	{ CTRLI,    0x00},
+	{ R_DVP_SP, 0 | R_DVP_SP_AUTO_MODE },
+	ENDMARKER,
+};
+
+#define OV2640_SIZE(n, w, h, r) \
+	{.name = n, .width = w , .height = h, .regs = r }
+
+static const struct ov2640_win_size ov2640_supported_win_sizes[] = {
+	OV2640_SIZE("QCIF", QCIF_WIDTH, QCIF_HEIGHT, ov2640_qcif_regs),
+	OV2640_SIZE("QVGA", QVGA_WIDTH, QVGA_HEIGHT, ov2640_qvga_regs),
+	OV2640_SIZE("CIF", CIF_WIDTH, CIF_HEIGHT, ov2640_cif_regs),
+	OV2640_SIZE("VGA", VGA_WIDTH, VGA_HEIGHT, ov2640_vga_regs),
+	OV2640_SIZE("SVGA", SVGA_WIDTH, SVGA_HEIGHT, ov2640_svga_regs),
+	OV2640_SIZE("XGA", XGA_WIDTH, XGA_HEIGHT, ov2640_xga_regs),
+	OV2640_SIZE("SXGA", SXGA_WIDTH, SXGA_HEIGHT, ov2640_sxga_regs),
+	OV2640_SIZE("UXGA", UXGA_WIDTH, UXGA_HEIGHT, ov2640_uxga_regs),
+};
+
+/*
+ * Register settings for pixel formats
+ */
+static const struct regval_list ov2640_format_change_preamble_regs[] = {
+	{ BANK_SEL, BANK_SEL_DSP },
+	{ R_BYPASS, R_BYPASS_USE_DSP },
+	ENDMARKER,
+};
+
+static const struct regval_list ov2640_yuyv_regs[] = {
+	{ IMAGE_MODE, IMAGE_MODE_YUV422 },
+	{ 0xd7, 0x03 },
+	{ 0x33, 0xa0 },
+	{ 0xe5, 0x1f },
+	{ 0xe1, 0x67 },
+	{ RESET,  0x00 },
+	{ R_BYPASS, R_BYPASS_USE_DSP },
+	ENDMARKER,
+};
+
+static const struct regval_list ov2640_uyvy_regs[] = {
+	{ IMAGE_MODE, IMAGE_MODE_LBYTE_FIRST | IMAGE_MODE_YUV422 },
+	{ 0xd7, 0x01 },
+	{ 0x33, 0xa0 },
+	{ 0xe1, 0x67 },
+	{ RESET,  0x00 },
+	{ R_BYPASS, R_BYPASS_USE_DSP },
+	ENDMARKER,
+};
+
+static const struct regval_list ov2640_rgb565_be_regs[] = {
+	{ IMAGE_MODE, IMAGE_MODE_RGB565 },
+	{ 0xd7, 0x03 },
+	{ RESET,  0x00 },
+	{ R_BYPASS, R_BYPASS_USE_DSP },
+	ENDMARKER,
+};
+
+static const struct regval_list ov2640_rgb565_le_regs[] = {
+	{ IMAGE_MODE, IMAGE_MODE_LBYTE_FIRST | IMAGE_MODE_RGB565 },
+	{ 0xd7, 0x03 },
+	{ RESET,  0x00 },
+	{ R_BYPASS, R_BYPASS_USE_DSP },
+	ENDMARKER,
+};
+
+static u32 ov2640_codes[] = {
+	MEDIA_BUS_FMT_YUYV8_2X8,
+	MEDIA_BUS_FMT_UYVY8_2X8,
+	MEDIA_BUS_FMT_YVYU8_2X8,
+	MEDIA_BUS_FMT_VYUY8_2X8,
+	MEDIA_BUS_FMT_RGB565_2X8_BE,
+	MEDIA_BUS_FMT_RGB565_2X8_LE,
+};
+
+/*
+ * General functions
+ */
+static struct ov2640_priv *to_ov2640(const struct i2c_client *client)
+{
+	return container_of(i2c_get_clientdata(client), struct ov2640_priv,
+			    subdev);
+}
+
+static int ov2640_write_array(struct i2c_client *client,
+			      const struct regval_list *vals)
+{
+	int ret;
+
+	while ((vals->reg_num != 0xff) || (vals->value != 0xff)) {
+		ret = i2c_smbus_write_byte_data(client,
+						vals->reg_num, vals->value);
+		dev_vdbg(&client->dev, "array: 0x%02x, 0x%02x",
+			 vals->reg_num, vals->value);
+
+		if (ret < 0)
+			return ret;
+		vals++;
+	}
+	return 0;
+}
+
+static int ov2640_mask_set(struct i2c_client *client,
+			   u8  reg, u8  mask, u8  set)
+{
+	s32 val = i2c_smbus_read_byte_data(client, reg);
+	if (val < 0)
+		return val;
+
+	val &= ~mask;
+	val |= set & mask;
+
+	dev_vdbg(&client->dev, "masks: 0x%02x, 0x%02x", reg, val);
+
+	return i2c_smbus_write_byte_data(client, reg, val);
+}
+
+static int ov2640_reset(struct i2c_client *client)
+{
+	int ret;
+	static const struct regval_list reset_seq[] = {
+		{BANK_SEL, BANK_SEL_SENS},
+		{COM7, COM7_SRST},
+		ENDMARKER,
+	};
+
+	ret = ov2640_write_array(client, reset_seq);
+	if (ret)
+		goto err;
+
+	msleep(5);
+err:
+	dev_dbg(&client->dev, "%s: (ret %d)", __func__, ret);
+	return ret;
+}
+
+static const char * const ov2640_test_pattern_menu[] = {
+	"Disabled",
+	"Eight Vertical Colour Bars",
+};
+
+/*
+ * functions
+ */
+static int ov2640_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd =
+		&container_of(ctrl->handler, struct ov2640_priv, hdl)->subdev;
+	struct i2c_client  *client = v4l2_get_subdevdata(sd);
+	struct ov2640_priv *priv = to_ov2640(client);
+	u8 val;
+	int ret;
+
+	/* v4l2_ctrl_lock() locks our own mutex */
+
+	/*
+	 * If the device is not powered up by the host driver, do not apply any
+	 * controls to H/W at this time. Instead the controls will be restored
+	 * when the streaming is started.
+	 */
+	if (!priv->power_count)
+		return 0;
+
+	ret = i2c_smbus_write_byte_data(client, BANK_SEL, BANK_SEL_SENS);
+	if (ret < 0)
+		return ret;
+
+	switch (ctrl->id) {
+	case V4L2_CID_VFLIP:
+		val = ctrl->val ? REG04_VFLIP_IMG | REG04_VREF_EN : 0x00;
+		return ov2640_mask_set(client, REG04,
+				       REG04_VFLIP_IMG | REG04_VREF_EN, val);
+		/* NOTE: REG04_VREF_EN: 1 line shift / even/odd line swap */
+	case V4L2_CID_HFLIP:
+		val = ctrl->val ? REG04_HFLIP_IMG : 0x00;
+		return ov2640_mask_set(client, REG04, REG04_HFLIP_IMG, val);
+	case V4L2_CID_TEST_PATTERN:
+		val = ctrl->val ? COM7_COLOR_BAR_TEST : 0x00;
+		return ov2640_mask_set(client, COM7, COM7_COLOR_BAR_TEST, val);
+	}
+
+	return -EINVAL;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int ov2640_g_register(struct v4l2_subdev *sd,
+			     struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+	reg->size = 1;
+	if (reg->reg > 0xff)
+		return -EINVAL;
+
+	ret = i2c_smbus_read_byte_data(client, reg->reg);
+	if (ret < 0)
+		return ret;
+
+	reg->val = ret;
+
+	return 0;
+}
+
+static int ov2640_s_register(struct v4l2_subdev *sd,
+			     const struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (reg->reg > 0xff ||
+	    reg->val > 0xff)
+		return -EINVAL;
+
+	return i2c_smbus_write_byte_data(client, reg->reg, reg->val);
+}
+#endif
+
+static void ov2640_set_power(struct ov2640_priv *priv, int on)
+{
+#ifdef CONFIG_GPIOLIB
+	if (priv->pwdn_gpio)
+		gpiod_direction_output(priv->pwdn_gpio, !on);
+	if (on && priv->resetb_gpio) {
+		/* Active the resetb pin to perform a reset pulse */
+		gpiod_direction_output(priv->resetb_gpio, 1);
+		usleep_range(3000, 5000);
+		gpiod_set_value(priv->resetb_gpio, 0);
+	}
+#endif
+}
+
+static int ov2640_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov2640_priv *priv = to_ov2640(client);
+
+	mutex_lock(&priv->lock);
+
+	/*
+	 * If the power count is modified from 0 to != 0 or from != 0 to 0,
+	 * update the power state.
+	 */
+	if (priv->power_count == !on)
+		ov2640_set_power(priv, on);
+	priv->power_count += on ? 1 : -1;
+	WARN_ON(priv->power_count < 0);
+	mutex_unlock(&priv->lock);
+
+	return 0;
+}
+
+/* Select the nearest higher resolution for capture */
+static const struct ov2640_win_size *ov2640_select_win(u32 width, u32 height)
+{
+	int i, default_size = ARRAY_SIZE(ov2640_supported_win_sizes) - 1;
+
+	for (i = 0; i < ARRAY_SIZE(ov2640_supported_win_sizes); i++) {
+		if (ov2640_supported_win_sizes[i].width  >= width &&
+		    ov2640_supported_win_sizes[i].height >= height)
+			return &ov2640_supported_win_sizes[i];
+	}
+
+	return &ov2640_supported_win_sizes[default_size];
+}
+
+static int ov2640_set_params(struct i2c_client *client,
+			     const struct ov2640_win_size *win, u32 code)
+{
+	const struct regval_list *selected_cfmt_regs;
+	u8 val;
+	int ret;
+
+	switch (code) {
+	case MEDIA_BUS_FMT_RGB565_2X8_BE:
+		dev_dbg(&client->dev, "%s: Selected cfmt RGB565 BE", __func__);
+		selected_cfmt_regs = ov2640_rgb565_be_regs;
+		break;
+	case MEDIA_BUS_FMT_RGB565_2X8_LE:
+		dev_dbg(&client->dev, "%s: Selected cfmt RGB565 LE", __func__);
+		selected_cfmt_regs = ov2640_rgb565_le_regs;
+		break;
+	case MEDIA_BUS_FMT_YUYV8_2X8:
+		dev_dbg(&client->dev, "%s: Selected cfmt YUYV (YUV422)", __func__);
+		selected_cfmt_regs = ov2640_yuyv_regs;
+		break;
+	case MEDIA_BUS_FMT_UYVY8_2X8:
+	default:
+		dev_dbg(&client->dev, "%s: Selected cfmt UYVY", __func__);
+		selected_cfmt_regs = ov2640_uyvy_regs;
+		break;
+	case MEDIA_BUS_FMT_YVYU8_2X8:
+		dev_dbg(&client->dev, "%s: Selected cfmt YVYU", __func__);
+		selected_cfmt_regs = ov2640_yuyv_regs;
+		break;
+	case MEDIA_BUS_FMT_VYUY8_2X8:
+		dev_dbg(&client->dev, "%s: Selected cfmt VYUY", __func__);
+		selected_cfmt_regs = ov2640_uyvy_regs;
+		break;
+	}
+
+	/* reset hardware */
+	ov2640_reset(client);
+
+	/* initialize the sensor with default data */
+	dev_dbg(&client->dev, "%s: Init default", __func__);
+	ret = ov2640_write_array(client, ov2640_init_regs);
+	if (ret < 0)
+		goto err;
+
+	/* select preamble */
+	dev_dbg(&client->dev, "%s: Set size to %s", __func__, win->name);
+	ret = ov2640_write_array(client, ov2640_size_change_preamble_regs);
+	if (ret < 0)
+		goto err;
+
+	/* set size win */
+	ret = ov2640_write_array(client, win->regs);
+	if (ret < 0)
+		goto err;
+
+	/* cfmt preamble */
+	dev_dbg(&client->dev, "%s: Set cfmt", __func__);
+	ret = ov2640_write_array(client, ov2640_format_change_preamble_regs);
+	if (ret < 0)
+		goto err;
+
+	/* set cfmt */
+	ret = ov2640_write_array(client, selected_cfmt_regs);
+	if (ret < 0)
+		goto err;
+	val = (code == MEDIA_BUS_FMT_YVYU8_2X8)
+	      || (code == MEDIA_BUS_FMT_VYUY8_2X8) ? CTRL0_VFIRST : 0x00;
+	ret = ov2640_mask_set(client, CTRL0, CTRL0_VFIRST, val);
+	if (ret < 0)
+		goto err;
+
+	return 0;
+
+err:
+	dev_err(&client->dev, "%s: Error %d", __func__, ret);
+	ov2640_reset(client);
+
+	return ret;
+}
+
+static int ov2640_get_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *mf = &format->format;
+	struct i2c_client  *client = v4l2_get_subdevdata(sd);
+	struct ov2640_priv *priv = to_ov2640(client);
+
+	if (format->pad)
+		return -EINVAL;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
+		mf = v4l2_subdev_get_try_format(sd, sd_state, 0);
+		format->format = *mf;
+		return 0;
+#else
+		return -EINVAL;
+#endif
+	}
+
+	mf->width	= priv->win->width;
+	mf->height	= priv->win->height;
+	mf->code	= priv->cfmt_code;
+	mf->colorspace	= V4L2_COLORSPACE_SRGB;
+	mf->field	= V4L2_FIELD_NONE;
+	mf->ycbcr_enc	= V4L2_YCBCR_ENC_DEFAULT;
+	mf->quantization = V4L2_QUANTIZATION_DEFAULT;
+	mf->xfer_func	= V4L2_XFER_FUNC_DEFAULT;
+
+	return 0;
+}
+
+static int ov2640_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *mf = &format->format;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov2640_priv *priv = to_ov2640(client);
+	const struct ov2640_win_size *win;
+	int ret = 0;
+
+	if (format->pad)
+		return -EINVAL;
+
+	mutex_lock(&priv->lock);
+
+	/* select suitable win */
+	win = ov2640_select_win(mf->width, mf->height);
+	mf->width	= win->width;
+	mf->height	= win->height;
+
+	mf->field	= V4L2_FIELD_NONE;
+	mf->colorspace	= V4L2_COLORSPACE_SRGB;
+	mf->ycbcr_enc	= V4L2_YCBCR_ENC_DEFAULT;
+	mf->quantization = V4L2_QUANTIZATION_DEFAULT;
+	mf->xfer_func	= V4L2_XFER_FUNC_DEFAULT;
+
+	switch (mf->code) {
+	case MEDIA_BUS_FMT_RGB565_2X8_BE:
+	case MEDIA_BUS_FMT_RGB565_2X8_LE:
+	case MEDIA_BUS_FMT_YUYV8_2X8:
+	case MEDIA_BUS_FMT_UYVY8_2X8:
+	case MEDIA_BUS_FMT_YVYU8_2X8:
+	case MEDIA_BUS_FMT_VYUY8_2X8:
+		break;
+	default:
+		mf->code = MEDIA_BUS_FMT_UYVY8_2X8;
+		break;
+	}
+
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		struct ov2640_priv *priv = to_ov2640(client);
+
+		if (priv->streaming) {
+			ret = -EBUSY;
+			goto out;
+		}
+		/* select win */
+		priv->win = win;
+		/* select format */
+		priv->cfmt_code = mf->code;
+	} else {
+		sd_state->pads->try_fmt = *mf;
+	}
+out:
+	mutex_unlock(&priv->lock);
+
+	return ret;
+}
+
+static int ov2640_init_cfg(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_state *sd_state)
+{
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
+	struct v4l2_mbus_framefmt *try_fmt =
+		v4l2_subdev_get_try_format(sd, sd_state, 0);
+	const struct ov2640_win_size *win =
+		ov2640_select_win(SVGA_WIDTH, SVGA_HEIGHT);
+
+	try_fmt->width = win->width;
+	try_fmt->height = win->height;
+	try_fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
+	try_fmt->colorspace = V4L2_COLORSPACE_SRGB;
+	try_fmt->field = V4L2_FIELD_NONE;
+	try_fmt->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+	try_fmt->quantization = V4L2_QUANTIZATION_DEFAULT;
+	try_fmt->xfer_func = V4L2_XFER_FUNC_DEFAULT;
+#endif
+	return 0;
+}
+
+static int ov2640_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->pad || code->index >= ARRAY_SIZE(ov2640_codes))
+		return -EINVAL;
+
+	code->code = ov2640_codes[code->index];
+	return 0;
+}
+
+static int ov2640_get_selection(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_selection *sel)
+{
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
+		return -EINVAL;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP:
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = UXGA_WIDTH;
+		sel->r.height = UXGA_HEIGHT;
+		return 0;
+	default:
+		return -EINVAL;
+	}
+}
+
+static int ov2640_s_stream(struct v4l2_subdev *sd, int on)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov2640_priv *priv = to_ov2640(client);
+	int ret = 0;
+
+	mutex_lock(&priv->lock);
+	if (priv->streaming == !on) {
+		if (on) {
+			ret = ov2640_set_params(client, priv->win,
+						priv->cfmt_code);
+			if (!ret)
+				ret = __v4l2_ctrl_handler_setup(&priv->hdl);
+		}
+	}
+	if (!ret)
+		priv->streaming = on;
+	mutex_unlock(&priv->lock);
+
+	return ret;
+}
+
+static int ov2640_video_probe(struct i2c_client *client)
+{
+	struct ov2640_priv *priv = to_ov2640(client);
+	u8 pid, ver, midh, midl;
+	const char *devname;
+	int ret;
+
+	ret = ov2640_s_power(&priv->subdev, 1);
+	if (ret < 0)
+		return ret;
+
+	/*
+	 * check and show product ID and manufacturer ID
+	 */
+	i2c_smbus_write_byte_data(client, BANK_SEL, BANK_SEL_SENS);
+	pid  = i2c_smbus_read_byte_data(client, PID);
+	ver  = i2c_smbus_read_byte_data(client, VER);
+	midh = i2c_smbus_read_byte_data(client, MIDH);
+	midl = i2c_smbus_read_byte_data(client, MIDL);
+
+	switch (VERSION(pid, ver)) {
+	case PID_OV2640:
+		devname     = "ov2640";
+		break;
+	default:
+		dev_err(&client->dev,
+			"Product ID error %x:%x\n", pid, ver);
+		ret = -ENODEV;
+		goto done;
+	}
+
+	dev_info(&client->dev,
+		 "%s Product ID %0x:%0x Manufacturer ID %x:%x\n",
+		 devname, pid, ver, midh, midl);
+
+done:
+	ov2640_s_power(&priv->subdev, 0);
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ov2640_ctrl_ops = {
+	.s_ctrl = ov2640_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops ov2640_subdev_core_ops = {
+	.log_status = v4l2_ctrl_subdev_log_status,
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register	= ov2640_g_register,
+	.s_register	= ov2640_s_register,
+#endif
+	.s_power	= ov2640_s_power,
+};
+
+static const struct v4l2_subdev_pad_ops ov2640_subdev_pad_ops = {
+	.init_cfg	= ov2640_init_cfg,
+	.enum_mbus_code = ov2640_enum_mbus_code,
+	.get_selection	= ov2640_get_selection,
+	.get_fmt	= ov2640_get_fmt,
+	.set_fmt	= ov2640_set_fmt,
+};
+
+static const struct v4l2_subdev_video_ops ov2640_subdev_video_ops = {
+	.s_stream = ov2640_s_stream,
+};
+
+static const struct v4l2_subdev_ops ov2640_subdev_ops = {
+	.core	= &ov2640_subdev_core_ops,
+	.pad	= &ov2640_subdev_pad_ops,
+	.video	= &ov2640_subdev_video_ops,
+};
+
+static int ov2640_probe_dt(struct i2c_client *client,
+		struct ov2640_priv *priv)
+{
+	int ret;
+
+	/* Request the reset GPIO deasserted */
+	priv->resetb_gpio = devm_gpiod_get_optional(&client->dev, "resetb",
+			GPIOD_OUT_LOW);
+
+	if (!priv->resetb_gpio)
+		dev_dbg(&client->dev, "resetb gpio is not assigned!\n");
+
+	ret = PTR_ERR_OR_ZERO(priv->resetb_gpio);
+	if (ret && ret != -ENOSYS) {
+		dev_dbg(&client->dev,
+			"Error %d while getting resetb gpio\n", ret);
+		return ret;
+	}
+
+	/* Request the power down GPIO asserted */
+	priv->pwdn_gpio = devm_gpiod_get_optional(&client->dev, "pwdn",
+			GPIOD_OUT_HIGH);
+
+	if (!priv->pwdn_gpio)
+		dev_dbg(&client->dev, "pwdn gpio is not assigned!\n");
+
+	ret = PTR_ERR_OR_ZERO(priv->pwdn_gpio);
+	if (ret && ret != -ENOSYS) {
+		dev_dbg(&client->dev,
+			"Error %d while getting pwdn gpio\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+/*
+ * i2c_driver functions
+ */
+static int ov2640_probe(struct i2c_client *client)
+{
+	struct ov2640_priv	*priv;
+	struct i2c_adapter	*adapter = client->adapter;
+	int			ret;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA)) {
+		dev_err(&adapter->dev,
+			"OV2640: I2C-Adapter doesn't support SMBUS\n");
+		return -EIO;
+	}
+
+	priv = devm_kzalloc(&client->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	if (client->dev.of_node) {
+		priv->clk = devm_clk_get(&client->dev, "xvclk");
+		if (IS_ERR(priv->clk))
+			return PTR_ERR(priv->clk);
+		ret = clk_prepare_enable(priv->clk);
+		if (ret)
+			return ret;
+	}
+
+	ret = ov2640_probe_dt(client, priv);
+	if (ret)
+		goto err_clk;
+
+	priv->win = ov2640_select_win(SVGA_WIDTH, SVGA_HEIGHT);
+	priv->cfmt_code = MEDIA_BUS_FMT_UYVY8_2X8;
+
+	v4l2_i2c_subdev_init(&priv->subdev, client, &ov2640_subdev_ops);
+	priv->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+			      V4L2_SUBDEV_FL_HAS_EVENTS;
+	mutex_init(&priv->lock);
+	v4l2_ctrl_handler_init(&priv->hdl, 3);
+	priv->hdl.lock = &priv->lock;
+	v4l2_ctrl_new_std(&priv->hdl, &ov2640_ctrl_ops,
+			V4L2_CID_VFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&priv->hdl, &ov2640_ctrl_ops,
+			V4L2_CID_HFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std_menu_items(&priv->hdl, &ov2640_ctrl_ops,
+			V4L2_CID_TEST_PATTERN,
+			ARRAY_SIZE(ov2640_test_pattern_menu) - 1, 0, 0,
+			ov2640_test_pattern_menu);
+	priv->subdev.ctrl_handler = &priv->hdl;
+	if (priv->hdl.error) {
+		ret = priv->hdl.error;
+		goto err_hdl;
+	}
+#if defined(CONFIG_MEDIA_CONTROLLER)
+	priv->pad.flags = MEDIA_PAD_FL_SOURCE;
+	priv->subdev.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret = media_entity_pads_init(&priv->subdev.entity, 1, &priv->pad);
+	if (ret < 0)
+		goto err_hdl;
+#endif
+
+	ret = ov2640_video_probe(client);
+	if (ret < 0)
+		goto err_videoprobe;
+
+	ret = v4l2_async_register_subdev(&priv->subdev);
+	if (ret < 0)
+		goto err_videoprobe;
+
+	dev_info(&adapter->dev, "OV2640 Probed\n");
+
+	return 0;
+
+err_videoprobe:
+	media_entity_cleanup(&priv->subdev.entity);
+err_hdl:
+	v4l2_ctrl_handler_free(&priv->hdl);
+	mutex_destroy(&priv->lock);
+err_clk:
+	clk_disable_unprepare(priv->clk);
+	return ret;
+}
+
+static void ov2640_remove(struct i2c_client *client)
+{
+	struct ov2640_priv       *priv = to_ov2640(client);
+
+	v4l2_async_unregister_subdev(&priv->subdev);
+	v4l2_ctrl_handler_free(&priv->hdl);
+	mutex_destroy(&priv->lock);
+	media_entity_cleanup(&priv->subdev.entity);
+	v4l2_device_unregister_subdev(&priv->subdev);
+	clk_disable_unprepare(priv->clk);
+}
+
+static const struct i2c_device_id ov2640_id[] = {
+	{ "ov2640", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ov2640_id);
+
+static const struct of_device_id ov2640_of_match[] = {
+	{.compatible = "ovti,ov2640", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, ov2640_of_match);
+
+static struct i2c_driver ov2640_i2c_driver = {
+	.driver = {
+		.name = "ov2640",
+		.of_match_table = ov2640_of_match,
+	},
+	.probe    = ov2640_probe,
+	.remove   = ov2640_remove,
+	.id_table = ov2640_id,
+};
+
+module_i2c_driver(ov2640_i2c_driver);
+
+MODULE_DESCRIPTION("Driver for Omni Vision 2640 sensor");
+MODULE_AUTHOR("Alberto Panizzo");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/ov2659.c b/drivers/media/i2c/ov2659.c
new file mode 100644
index 000000000..5429bd2eb
--- /dev/null
+++ b/drivers/media/i2c/ov2659.c
@@ -0,0 +1,1596 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Omnivision OV2659 CMOS Image Sensor driver
+ *
+ * Copyright (C) 2015 Texas Instruments, Inc.
+ *
+ * Benoit Parrot <bparrot@ti.com>
+ * Lad, Prabhakar <prabhakar.csengg@gmail.com>
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/of_graph.h>
+#include <linux/pm_runtime.h>
+
+#include <media/i2c/ov2659.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-image-sizes.h>
+#include <media/v4l2-subdev.h>
+
+#define DRIVER_NAME "ov2659"
+
+/*
+ * OV2659 register definitions
+ */
+#define REG_SOFTWARE_STANDBY		0x0100
+#define REG_SOFTWARE_RESET		0x0103
+#define REG_IO_CTRL00			0x3000
+#define REG_IO_CTRL01			0x3001
+#define REG_IO_CTRL02			0x3002
+#define REG_OUTPUT_VALUE00		0x3008
+#define REG_OUTPUT_VALUE01		0x3009
+#define REG_OUTPUT_VALUE02		0x300d
+#define REG_OUTPUT_SELECT00		0x300e
+#define REG_OUTPUT_SELECT01		0x300f
+#define REG_OUTPUT_SELECT02		0x3010
+#define REG_OUTPUT_DRIVE		0x3011
+#define REG_INPUT_READOUT00		0x302d
+#define REG_INPUT_READOUT01		0x302e
+#define REG_INPUT_READOUT02		0x302f
+
+#define REG_SC_PLL_CTRL0		0x3003
+#define REG_SC_PLL_CTRL1		0x3004
+#define REG_SC_PLL_CTRL2		0x3005
+#define REG_SC_PLL_CTRL3		0x3006
+#define REG_SC_CHIP_ID_H		0x300a
+#define REG_SC_CHIP_ID_L		0x300b
+#define REG_SC_PWC			0x3014
+#define REG_SC_CLKRST0			0x301a
+#define REG_SC_CLKRST1			0x301b
+#define REG_SC_CLKRST2			0x301c
+#define REG_SC_CLKRST3			0x301d
+#define REG_SC_SUB_ID			0x302a
+#define REG_SC_SCCB_ID			0x302b
+
+#define REG_GROUP_ADDRESS_00		0x3200
+#define REG_GROUP_ADDRESS_01		0x3201
+#define REG_GROUP_ADDRESS_02		0x3202
+#define REG_GROUP_ADDRESS_03		0x3203
+#define REG_GROUP_ACCESS		0x3208
+
+#define REG_AWB_R_GAIN_H		0x3400
+#define REG_AWB_R_GAIN_L		0x3401
+#define REG_AWB_G_GAIN_H		0x3402
+#define REG_AWB_G_GAIN_L		0x3403
+#define REG_AWB_B_GAIN_H		0x3404
+#define REG_AWB_B_GAIN_L		0x3405
+#define REG_AWB_MANUAL_CONTROL		0x3406
+
+#define REG_TIMING_HS_H			0x3800
+#define REG_TIMING_HS_L			0x3801
+#define REG_TIMING_VS_H			0x3802
+#define REG_TIMING_VS_L			0x3803
+#define REG_TIMING_HW_H			0x3804
+#define REG_TIMING_HW_L			0x3805
+#define REG_TIMING_VH_H			0x3806
+#define REG_TIMING_VH_L			0x3807
+#define REG_TIMING_DVPHO_H		0x3808
+#define REG_TIMING_DVPHO_L		0x3809
+#define REG_TIMING_DVPVO_H		0x380a
+#define REG_TIMING_DVPVO_L		0x380b
+#define REG_TIMING_HTS_H		0x380c
+#define REG_TIMING_HTS_L		0x380d
+#define REG_TIMING_VTS_H		0x380e
+#define REG_TIMING_VTS_L		0x380f
+#define REG_TIMING_HOFFS_H		0x3810
+#define REG_TIMING_HOFFS_L		0x3811
+#define REG_TIMING_VOFFS_H		0x3812
+#define REG_TIMING_VOFFS_L		0x3813
+#define REG_TIMING_XINC			0x3814
+#define REG_TIMING_YINC			0x3815
+#define REG_TIMING_VERT_FORMAT		0x3820
+#define REG_TIMING_HORIZ_FORMAT		0x3821
+
+#define REG_FORMAT_CTRL00		0x4300
+
+#define REG_VFIFO_READ_START_H		0x4608
+#define REG_VFIFO_READ_START_L		0x4609
+
+#define REG_DVP_CTRL02			0x4708
+
+#define REG_ISP_CTRL00			0x5000
+#define REG_ISP_CTRL01			0x5001
+#define REG_ISP_CTRL02			0x5002
+
+#define REG_LENC_RED_X0_H		0x500c
+#define REG_LENC_RED_X0_L		0x500d
+#define REG_LENC_RED_Y0_H		0x500e
+#define REG_LENC_RED_Y0_L		0x500f
+#define REG_LENC_RED_A1			0x5010
+#define REG_LENC_RED_B1			0x5011
+#define REG_LENC_RED_A2_B2		0x5012
+#define REG_LENC_GREEN_X0_H		0x5013
+#define REG_LENC_GREEN_X0_L		0x5014
+#define REG_LENC_GREEN_Y0_H		0x5015
+#define REG_LENC_GREEN_Y0_L		0x5016
+#define REG_LENC_GREEN_A1		0x5017
+#define REG_LENC_GREEN_B1		0x5018
+#define REG_LENC_GREEN_A2_B2		0x5019
+#define REG_LENC_BLUE_X0_H		0x501a
+#define REG_LENC_BLUE_X0_L		0x501b
+#define REG_LENC_BLUE_Y0_H		0x501c
+#define REG_LENC_BLUE_Y0_L		0x501d
+#define REG_LENC_BLUE_A1		0x501e
+#define REG_LENC_BLUE_B1		0x501f
+#define REG_LENC_BLUE_A2_B2		0x5020
+
+#define REG_AWB_CTRL00			0x5035
+#define REG_AWB_CTRL01			0x5036
+#define REG_AWB_CTRL02			0x5037
+#define REG_AWB_CTRL03			0x5038
+#define REG_AWB_CTRL04			0x5039
+#define REG_AWB_LOCAL_LIMIT		0x503a
+#define REG_AWB_CTRL12			0x5049
+#define REG_AWB_CTRL13			0x504a
+#define REG_AWB_CTRL14			0x504b
+
+#define REG_SHARPENMT_THRESH1		0x5064
+#define REG_SHARPENMT_THRESH2		0x5065
+#define REG_SHARPENMT_OFFSET1		0x5066
+#define REG_SHARPENMT_OFFSET2		0x5067
+#define REG_DENOISE_THRESH1		0x5068
+#define REG_DENOISE_THRESH2		0x5069
+#define REG_DENOISE_OFFSET1		0x506a
+#define REG_DENOISE_OFFSET2		0x506b
+#define REG_SHARPEN_THRESH1		0x506c
+#define REG_SHARPEN_THRESH2		0x506d
+#define REG_CIP_CTRL00			0x506e
+#define REG_CIP_CTRL01			0x506f
+
+#define REG_CMX_SIGN			0x5079
+#define REG_CMX_MISC_CTRL		0x507a
+
+#define REG_PRE_ISP_CTRL00		0x50a0
+#define TEST_PATTERN_ENABLE		BIT(7)
+#define VERTICAL_COLOR_BAR_MASK		0x53
+
+#define REG_NULL			0x0000	/* Array end token */
+
+#define OV265X_ID(_msb, _lsb)		((_msb) << 8 | (_lsb))
+#define OV2659_ID			0x2656
+
+struct sensor_register {
+	u16 addr;
+	u8 value;
+};
+
+struct ov2659_framesize {
+	u16 width;
+	u16 height;
+	u16 max_exp_lines;
+	const struct sensor_register *regs;
+};
+
+struct ov2659_pll_ctrl {
+	u8 ctrl1;
+	u8 ctrl2;
+	u8 ctrl3;
+};
+
+struct ov2659_pixfmt {
+	u32 code;
+	/* Output format Register Value (REG_FORMAT_CTRL00) */
+	struct sensor_register *format_ctrl_regs;
+};
+
+struct pll_ctrl_reg {
+	unsigned int div;
+	unsigned char reg;
+};
+
+struct ov2659 {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct v4l2_mbus_framefmt format;
+	unsigned int xvclk_frequency;
+	const struct ov2659_platform_data *pdata;
+	struct mutex lock;
+	struct i2c_client *client;
+	struct v4l2_ctrl_handler ctrls;
+	struct v4l2_ctrl *link_frequency;
+	struct clk *clk;
+	const struct ov2659_framesize *frame_size;
+	struct sensor_register *format_ctrl_regs;
+	struct ov2659_pll_ctrl pll;
+	int streaming;
+	/* used to control the sensor PWDN pin */
+	struct gpio_desc *pwdn_gpio;
+	/* used to control the sensor RESETB pin */
+	struct gpio_desc *resetb_gpio;
+};
+
+static const struct sensor_register ov2659_init_regs[] = {
+	{ REG_IO_CTRL00, 0x03 },
+	{ REG_IO_CTRL01, 0xff },
+	{ REG_IO_CTRL02, 0xe0 },
+	{ 0x3633, 0x3d },
+	{ 0x3620, 0x02 },
+	{ 0x3631, 0x11 },
+	{ 0x3612, 0x04 },
+	{ 0x3630, 0x20 },
+	{ 0x4702, 0x02 },
+	{ 0x370c, 0x34 },
+	{ REG_TIMING_HS_H, 0x00 },
+	{ REG_TIMING_HS_L, 0x00 },
+	{ REG_TIMING_VS_H, 0x00 },
+	{ REG_TIMING_VS_L, 0x00 },
+	{ REG_TIMING_HW_H, 0x06 },
+	{ REG_TIMING_HW_L, 0x5f },
+	{ REG_TIMING_VH_H, 0x04 },
+	{ REG_TIMING_VH_L, 0xb7 },
+	{ REG_TIMING_DVPHO_H, 0x03 },
+	{ REG_TIMING_DVPHO_L, 0x20 },
+	{ REG_TIMING_DVPVO_H, 0x02 },
+	{ REG_TIMING_DVPVO_L, 0x58 },
+	{ REG_TIMING_HTS_H, 0x05 },
+	{ REG_TIMING_HTS_L, 0x14 },
+	{ REG_TIMING_VTS_H, 0x02 },
+	{ REG_TIMING_VTS_L, 0x68 },
+	{ REG_TIMING_HOFFS_L, 0x08 },
+	{ REG_TIMING_VOFFS_L, 0x02 },
+	{ REG_TIMING_XINC, 0x31 },
+	{ REG_TIMING_YINC, 0x31 },
+	{ 0x3a02, 0x02 },
+	{ 0x3a03, 0x68 },
+	{ 0x3a08, 0x00 },
+	{ 0x3a09, 0x5c },
+	{ 0x3a0a, 0x00 },
+	{ 0x3a0b, 0x4d },
+	{ 0x3a0d, 0x08 },
+	{ 0x3a0e, 0x06 },
+	{ 0x3a14, 0x02 },
+	{ 0x3a15, 0x28 },
+	{ REG_DVP_CTRL02, 0x01 },
+	{ 0x3623, 0x00 },
+	{ 0x3634, 0x76 },
+	{ 0x3701, 0x44 },
+	{ 0x3702, 0x18 },
+	{ 0x3703, 0x24 },
+	{ 0x3704, 0x24 },
+	{ 0x3705, 0x0c },
+	{ REG_TIMING_VERT_FORMAT, 0x81 },
+	{ REG_TIMING_HORIZ_FORMAT, 0x01 },
+	{ 0x370a, 0x52 },
+	{ REG_VFIFO_READ_START_H, 0x00 },
+	{ REG_VFIFO_READ_START_L, 0x80 },
+	{ REG_FORMAT_CTRL00, 0x30 },
+	{ 0x5086, 0x02 },
+	{ REG_ISP_CTRL00, 0xfb },
+	{ REG_ISP_CTRL01, 0x1f },
+	{ REG_ISP_CTRL02, 0x00 },
+	{ 0x5025, 0x0e },
+	{ 0x5026, 0x18 },
+	{ 0x5027, 0x34 },
+	{ 0x5028, 0x4c },
+	{ 0x5029, 0x62 },
+	{ 0x502a, 0x74 },
+	{ 0x502b, 0x85 },
+	{ 0x502c, 0x92 },
+	{ 0x502d, 0x9e },
+	{ 0x502e, 0xb2 },
+	{ 0x502f, 0xc0 },
+	{ 0x5030, 0xcc },
+	{ 0x5031, 0xe0 },
+	{ 0x5032, 0xee },
+	{ 0x5033, 0xf6 },
+	{ 0x5034, 0x11 },
+	{ 0x5070, 0x1c },
+	{ 0x5071, 0x5b },
+	{ 0x5072, 0x05 },
+	{ 0x5073, 0x20 },
+	{ 0x5074, 0x94 },
+	{ 0x5075, 0xb4 },
+	{ 0x5076, 0xb4 },
+	{ 0x5077, 0xaf },
+	{ 0x5078, 0x05 },
+	{ REG_CMX_SIGN, 0x98 },
+	{ REG_CMX_MISC_CTRL, 0x21 },
+	{ REG_AWB_CTRL00, 0x6a },
+	{ REG_AWB_CTRL01, 0x11 },
+	{ REG_AWB_CTRL02, 0x92 },
+	{ REG_AWB_CTRL03, 0x21 },
+	{ REG_AWB_CTRL04, 0xe1 },
+	{ REG_AWB_LOCAL_LIMIT, 0x01 },
+	{ 0x503c, 0x05 },
+	{ 0x503d, 0x08 },
+	{ 0x503e, 0x08 },
+	{ 0x503f, 0x64 },
+	{ 0x5040, 0x58 },
+	{ 0x5041, 0x2a },
+	{ 0x5042, 0xc5 },
+	{ 0x5043, 0x2e },
+	{ 0x5044, 0x3a },
+	{ 0x5045, 0x3c },
+	{ 0x5046, 0x44 },
+	{ 0x5047, 0xf8 },
+	{ 0x5048, 0x08 },
+	{ REG_AWB_CTRL12, 0x70 },
+	{ REG_AWB_CTRL13, 0xf0 },
+	{ REG_AWB_CTRL14, 0xf0 },
+	{ REG_LENC_RED_X0_H, 0x03 },
+	{ REG_LENC_RED_X0_L, 0x20 },
+	{ REG_LENC_RED_Y0_H, 0x02 },
+	{ REG_LENC_RED_Y0_L, 0x5c },
+	{ REG_LENC_RED_A1, 0x48 },
+	{ REG_LENC_RED_B1, 0x00 },
+	{ REG_LENC_RED_A2_B2, 0x66 },
+	{ REG_LENC_GREEN_X0_H, 0x03 },
+	{ REG_LENC_GREEN_X0_L, 0x30 },
+	{ REG_LENC_GREEN_Y0_H, 0x02 },
+	{ REG_LENC_GREEN_Y0_L, 0x7c },
+	{ REG_LENC_GREEN_A1, 0x40 },
+	{ REG_LENC_GREEN_B1, 0x00 },
+	{ REG_LENC_GREEN_A2_B2, 0x66 },
+	{ REG_LENC_BLUE_X0_H, 0x03 },
+	{ REG_LENC_BLUE_X0_L, 0x10 },
+	{ REG_LENC_BLUE_Y0_H, 0x02 },
+	{ REG_LENC_BLUE_Y0_L, 0x7c },
+	{ REG_LENC_BLUE_A1, 0x3a },
+	{ REG_LENC_BLUE_B1, 0x00 },
+	{ REG_LENC_BLUE_A2_B2, 0x66 },
+	{ REG_CIP_CTRL00, 0x44 },
+	{ REG_SHARPENMT_THRESH1, 0x08 },
+	{ REG_SHARPENMT_THRESH2, 0x10 },
+	{ REG_SHARPENMT_OFFSET1, 0x12 },
+	{ REG_SHARPENMT_OFFSET2, 0x02 },
+	{ REG_SHARPEN_THRESH1, 0x08 },
+	{ REG_SHARPEN_THRESH2, 0x10 },
+	{ REG_CIP_CTRL01, 0xa6 },
+	{ REG_DENOISE_THRESH1, 0x08 },
+	{ REG_DENOISE_THRESH2, 0x10 },
+	{ REG_DENOISE_OFFSET1, 0x04 },
+	{ REG_DENOISE_OFFSET2, 0x12 },
+	{ 0x507e, 0x40 },
+	{ 0x507f, 0x20 },
+	{ 0x507b, 0x02 },
+	{ REG_CMX_MISC_CTRL, 0x01 },
+	{ 0x5084, 0x0c },
+	{ 0x5085, 0x3e },
+	{ 0x5005, 0x80 },
+	{ 0x3a0f, 0x30 },
+	{ 0x3a10, 0x28 },
+	{ 0x3a1b, 0x32 },
+	{ 0x3a1e, 0x26 },
+	{ 0x3a11, 0x60 },
+	{ 0x3a1f, 0x14 },
+	{ 0x5060, 0x69 },
+	{ 0x5061, 0x7d },
+	{ 0x5062, 0x7d },
+	{ 0x5063, 0x69 },
+	{ REG_NULL, 0x00 },
+};
+
+/* 1280X720 720p */
+static struct sensor_register ov2659_720p[] = {
+	{ REG_TIMING_HS_H, 0x00 },
+	{ REG_TIMING_HS_L, 0xa0 },
+	{ REG_TIMING_VS_H, 0x00 },
+	{ REG_TIMING_VS_L, 0xf0 },
+	{ REG_TIMING_HW_H, 0x05 },
+	{ REG_TIMING_HW_L, 0xbf },
+	{ REG_TIMING_VH_H, 0x03 },
+	{ REG_TIMING_VH_L, 0xcb },
+	{ REG_TIMING_DVPHO_H, 0x05 },
+	{ REG_TIMING_DVPHO_L, 0x00 },
+	{ REG_TIMING_DVPVO_H, 0x02 },
+	{ REG_TIMING_DVPVO_L, 0xd0 },
+	{ REG_TIMING_HTS_H, 0x06 },
+	{ REG_TIMING_HTS_L, 0x4c },
+	{ REG_TIMING_VTS_H, 0x02 },
+	{ REG_TIMING_VTS_L, 0xe8 },
+	{ REG_TIMING_HOFFS_L, 0x10 },
+	{ REG_TIMING_VOFFS_L, 0x06 },
+	{ REG_TIMING_XINC, 0x11 },
+	{ REG_TIMING_YINC, 0x11 },
+	{ REG_TIMING_VERT_FORMAT, 0x80 },
+	{ REG_TIMING_HORIZ_FORMAT, 0x00 },
+	{ 0x370a, 0x12 },
+	{ 0x3a03, 0xe8 },
+	{ 0x3a09, 0x6f },
+	{ 0x3a0b, 0x5d },
+	{ 0x3a15, 0x9a },
+	{ REG_VFIFO_READ_START_H, 0x00 },
+	{ REG_VFIFO_READ_START_L, 0x80 },
+	{ REG_ISP_CTRL02, 0x00 },
+	{ REG_NULL, 0x00 },
+};
+
+/* 1600X1200 UXGA */
+static struct sensor_register ov2659_uxga[] = {
+	{ REG_TIMING_HS_H, 0x00 },
+	{ REG_TIMING_HS_L, 0x00 },
+	{ REG_TIMING_VS_H, 0x00 },
+	{ REG_TIMING_VS_L, 0x00 },
+	{ REG_TIMING_HW_H, 0x06 },
+	{ REG_TIMING_HW_L, 0x5f },
+	{ REG_TIMING_VH_H, 0x04 },
+	{ REG_TIMING_VH_L, 0xbb },
+	{ REG_TIMING_DVPHO_H, 0x06 },
+	{ REG_TIMING_DVPHO_L, 0x40 },
+	{ REG_TIMING_DVPVO_H, 0x04 },
+	{ REG_TIMING_DVPVO_L, 0xb0 },
+	{ REG_TIMING_HTS_H, 0x07 },
+	{ REG_TIMING_HTS_L, 0x9f },
+	{ REG_TIMING_VTS_H, 0x04 },
+	{ REG_TIMING_VTS_L, 0xd0 },
+	{ REG_TIMING_HOFFS_L, 0x10 },
+	{ REG_TIMING_VOFFS_L, 0x06 },
+	{ REG_TIMING_XINC, 0x11 },
+	{ REG_TIMING_YINC, 0x11 },
+	{ 0x3a02, 0x04 },
+	{ 0x3a03, 0xd0 },
+	{ 0x3a08, 0x00 },
+	{ 0x3a09, 0xb8 },
+	{ 0x3a0a, 0x00 },
+	{ 0x3a0b, 0x9a },
+	{ 0x3a0d, 0x08 },
+	{ 0x3a0e, 0x06 },
+	{ 0x3a14, 0x04 },
+	{ 0x3a15, 0x50 },
+	{ 0x3623, 0x00 },
+	{ 0x3634, 0x44 },
+	{ 0x3701, 0x44 },
+	{ 0x3702, 0x30 },
+	{ 0x3703, 0x48 },
+	{ 0x3704, 0x48 },
+	{ 0x3705, 0x18 },
+	{ REG_TIMING_VERT_FORMAT, 0x80 },
+	{ REG_TIMING_HORIZ_FORMAT, 0x00 },
+	{ 0x370a, 0x12 },
+	{ REG_VFIFO_READ_START_H, 0x00 },
+	{ REG_VFIFO_READ_START_L, 0x80 },
+	{ REG_ISP_CTRL02, 0x00 },
+	{ REG_NULL, 0x00 },
+};
+
+/* 1280X1024 SXGA */
+static struct sensor_register ov2659_sxga[] = {
+	{ REG_TIMING_HS_H, 0x00 },
+	{ REG_TIMING_HS_L, 0x00 },
+	{ REG_TIMING_VS_H, 0x00 },
+	{ REG_TIMING_VS_L, 0x00 },
+	{ REG_TIMING_HW_H, 0x06 },
+	{ REG_TIMING_HW_L, 0x5f },
+	{ REG_TIMING_VH_H, 0x04 },
+	{ REG_TIMING_VH_L, 0xb7 },
+	{ REG_TIMING_DVPHO_H, 0x05 },
+	{ REG_TIMING_DVPHO_L, 0x00 },
+	{ REG_TIMING_DVPVO_H, 0x04 },
+	{ REG_TIMING_DVPVO_L, 0x00 },
+	{ REG_TIMING_HTS_H, 0x07 },
+	{ REG_TIMING_HTS_L, 0x9c },
+	{ REG_TIMING_VTS_H, 0x04 },
+	{ REG_TIMING_VTS_L, 0xd0 },
+	{ REG_TIMING_HOFFS_L, 0x10 },
+	{ REG_TIMING_VOFFS_L, 0x06 },
+	{ REG_TIMING_XINC, 0x11 },
+	{ REG_TIMING_YINC, 0x11 },
+	{ 0x3a02, 0x02 },
+	{ 0x3a03, 0x68 },
+	{ 0x3a08, 0x00 },
+	{ 0x3a09, 0x5c },
+	{ 0x3a0a, 0x00 },
+	{ 0x3a0b, 0x4d },
+	{ 0x3a0d, 0x08 },
+	{ 0x3a0e, 0x06 },
+	{ 0x3a14, 0x02 },
+	{ 0x3a15, 0x28 },
+	{ 0x3623, 0x00 },
+	{ 0x3634, 0x76 },
+	{ 0x3701, 0x44 },
+	{ 0x3702, 0x18 },
+	{ 0x3703, 0x24 },
+	{ 0x3704, 0x24 },
+	{ 0x3705, 0x0c },
+	{ REG_TIMING_VERT_FORMAT, 0x80 },
+	{ REG_TIMING_HORIZ_FORMAT, 0x00 },
+	{ 0x370a, 0x52 },
+	{ REG_VFIFO_READ_START_H, 0x00 },
+	{ REG_VFIFO_READ_START_L, 0x80 },
+	{ REG_ISP_CTRL02, 0x00 },
+	{ REG_NULL, 0x00 },
+};
+
+/* 1024X768 SXGA */
+static struct sensor_register ov2659_xga[] = {
+	{ REG_TIMING_HS_H, 0x00 },
+	{ REG_TIMING_HS_L, 0x00 },
+	{ REG_TIMING_VS_H, 0x00 },
+	{ REG_TIMING_VS_L, 0x00 },
+	{ REG_TIMING_HW_H, 0x06 },
+	{ REG_TIMING_HW_L, 0x5f },
+	{ REG_TIMING_VH_H, 0x04 },
+	{ REG_TIMING_VH_L, 0xb7 },
+	{ REG_TIMING_DVPHO_H, 0x04 },
+	{ REG_TIMING_DVPHO_L, 0x00 },
+	{ REG_TIMING_DVPVO_H, 0x03 },
+	{ REG_TIMING_DVPVO_L, 0x00 },
+	{ REG_TIMING_HTS_H, 0x07 },
+	{ REG_TIMING_HTS_L, 0x9c },
+	{ REG_TIMING_VTS_H, 0x04 },
+	{ REG_TIMING_VTS_L, 0xd0 },
+	{ REG_TIMING_HOFFS_L, 0x10 },
+	{ REG_TIMING_VOFFS_L, 0x06 },
+	{ REG_TIMING_XINC, 0x11 },
+	{ REG_TIMING_YINC, 0x11 },
+	{ 0x3a02, 0x02 },
+	{ 0x3a03, 0x68 },
+	{ 0x3a08, 0x00 },
+	{ 0x3a09, 0x5c },
+	{ 0x3a0a, 0x00 },
+	{ 0x3a0b, 0x4d },
+	{ 0x3a0d, 0x08 },
+	{ 0x3a0e, 0x06 },
+	{ 0x3a14, 0x02 },
+	{ 0x3a15, 0x28 },
+	{ 0x3623, 0x00 },
+	{ 0x3634, 0x76 },
+	{ 0x3701, 0x44 },
+	{ 0x3702, 0x18 },
+	{ 0x3703, 0x24 },
+	{ 0x3704, 0x24 },
+	{ 0x3705, 0x0c },
+	{ REG_TIMING_VERT_FORMAT, 0x80 },
+	{ REG_TIMING_HORIZ_FORMAT, 0x00 },
+	{ 0x370a, 0x52 },
+	{ REG_VFIFO_READ_START_H, 0x00 },
+	{ REG_VFIFO_READ_START_L, 0x80 },
+	{ REG_ISP_CTRL02, 0x00 },
+	{ REG_NULL, 0x00 },
+};
+
+/* 800X600 SVGA */
+static struct sensor_register ov2659_svga[] = {
+	{ REG_TIMING_HS_H, 0x00 },
+	{ REG_TIMING_HS_L, 0x00 },
+	{ REG_TIMING_VS_H, 0x00 },
+	{ REG_TIMING_VS_L, 0x00 },
+	{ REG_TIMING_HW_H, 0x06 },
+	{ REG_TIMING_HW_L, 0x5f },
+	{ REG_TIMING_VH_H, 0x04 },
+	{ REG_TIMING_VH_L, 0xb7 },
+	{ REG_TIMING_DVPHO_H, 0x03 },
+	{ REG_TIMING_DVPHO_L, 0x20 },
+	{ REG_TIMING_DVPVO_H, 0x02 },
+	{ REG_TIMING_DVPVO_L, 0x58 },
+	{ REG_TIMING_HTS_H, 0x05 },
+	{ REG_TIMING_HTS_L, 0x14 },
+	{ REG_TIMING_VTS_H, 0x02 },
+	{ REG_TIMING_VTS_L, 0x68 },
+	{ REG_TIMING_HOFFS_L, 0x08 },
+	{ REG_TIMING_VOFFS_L, 0x02 },
+	{ REG_TIMING_XINC, 0x31 },
+	{ REG_TIMING_YINC, 0x31 },
+	{ 0x3a02, 0x02 },
+	{ 0x3a03, 0x68 },
+	{ 0x3a08, 0x00 },
+	{ 0x3a09, 0x5c },
+	{ 0x3a0a, 0x00 },
+	{ 0x3a0b, 0x4d },
+	{ 0x3a0d, 0x08 },
+	{ 0x3a0e, 0x06 },
+	{ 0x3a14, 0x02 },
+	{ 0x3a15, 0x28 },
+	{ 0x3623, 0x00 },
+	{ 0x3634, 0x76 },
+	{ 0x3701, 0x44 },
+	{ 0x3702, 0x18 },
+	{ 0x3703, 0x24 },
+	{ 0x3704, 0x24 },
+	{ 0x3705, 0x0c },
+	{ REG_TIMING_VERT_FORMAT, 0x81 },
+	{ REG_TIMING_HORIZ_FORMAT, 0x01 },
+	{ 0x370a, 0x52 },
+	{ REG_VFIFO_READ_START_H, 0x00 },
+	{ REG_VFIFO_READ_START_L, 0x80 },
+	{ REG_ISP_CTRL02, 0x00 },
+	{ REG_NULL, 0x00 },
+};
+
+/* 640X480 VGA */
+static struct sensor_register ov2659_vga[] = {
+	{ REG_TIMING_HS_H, 0x00 },
+	{ REG_TIMING_HS_L, 0x00 },
+	{ REG_TIMING_VS_H, 0x00 },
+	{ REG_TIMING_VS_L, 0x00 },
+	{ REG_TIMING_HW_H, 0x06 },
+	{ REG_TIMING_HW_L, 0x5f },
+	{ REG_TIMING_VH_H, 0x04 },
+	{ REG_TIMING_VH_L, 0xb7 },
+	{ REG_TIMING_DVPHO_H, 0x02 },
+	{ REG_TIMING_DVPHO_L, 0x80 },
+	{ REG_TIMING_DVPVO_H, 0x01 },
+	{ REG_TIMING_DVPVO_L, 0xe0 },
+	{ REG_TIMING_HTS_H, 0x05 },
+	{ REG_TIMING_HTS_L, 0x14 },
+	{ REG_TIMING_VTS_H, 0x02 },
+	{ REG_TIMING_VTS_L, 0x68 },
+	{ REG_TIMING_HOFFS_L, 0x08 },
+	{ REG_TIMING_VOFFS_L, 0x02 },
+	{ REG_TIMING_XINC, 0x31 },
+	{ REG_TIMING_YINC, 0x31 },
+	{ 0x3a02, 0x02 },
+	{ 0x3a03, 0x68 },
+	{ 0x3a08, 0x00 },
+	{ 0x3a09, 0x5c },
+	{ 0x3a0a, 0x00 },
+	{ 0x3a0b, 0x4d },
+	{ 0x3a0d, 0x08 },
+	{ 0x3a0e, 0x06 },
+	{ 0x3a14, 0x02 },
+	{ 0x3a15, 0x28 },
+	{ 0x3623, 0x00 },
+	{ 0x3634, 0x76 },
+	{ 0x3701, 0x44 },
+	{ 0x3702, 0x18 },
+	{ 0x3703, 0x24 },
+	{ 0x3704, 0x24 },
+	{ 0x3705, 0x0c },
+	{ REG_TIMING_VERT_FORMAT, 0x81 },
+	{ REG_TIMING_HORIZ_FORMAT, 0x01 },
+	{ 0x370a, 0x52 },
+	{ REG_VFIFO_READ_START_H, 0x00 },
+	{ REG_VFIFO_READ_START_L, 0xa0 },
+	{ REG_ISP_CTRL02, 0x10 },
+	{ REG_NULL, 0x00 },
+};
+
+/* 320X240 QVGA */
+static  struct sensor_register ov2659_qvga[] = {
+	{ REG_TIMING_HS_H, 0x00 },
+	{ REG_TIMING_HS_L, 0x00 },
+	{ REG_TIMING_VS_H, 0x00 },
+	{ REG_TIMING_VS_L, 0x00 },
+	{ REG_TIMING_HW_H, 0x06 },
+	{ REG_TIMING_HW_L, 0x5f },
+	{ REG_TIMING_VH_H, 0x04 },
+	{ REG_TIMING_VH_L, 0xb7 },
+	{ REG_TIMING_DVPHO_H, 0x01 },
+	{ REG_TIMING_DVPHO_L, 0x40 },
+	{ REG_TIMING_DVPVO_H, 0x00 },
+	{ REG_TIMING_DVPVO_L, 0xf0 },
+	{ REG_TIMING_HTS_H, 0x05 },
+	{ REG_TIMING_HTS_L, 0x14 },
+	{ REG_TIMING_VTS_H, 0x02 },
+	{ REG_TIMING_VTS_L, 0x68 },
+	{ REG_TIMING_HOFFS_L, 0x08 },
+	{ REG_TIMING_VOFFS_L, 0x02 },
+	{ REG_TIMING_XINC, 0x31 },
+	{ REG_TIMING_YINC, 0x31 },
+	{ 0x3a02, 0x02 },
+	{ 0x3a03, 0x68 },
+	{ 0x3a08, 0x00 },
+	{ 0x3a09, 0x5c },
+	{ 0x3a0a, 0x00 },
+	{ 0x3a0b, 0x4d },
+	{ 0x3a0d, 0x08 },
+	{ 0x3a0e, 0x06 },
+	{ 0x3a14, 0x02 },
+	{ 0x3a15, 0x28 },
+	{ 0x3623, 0x00 },
+	{ 0x3634, 0x76 },
+	{ 0x3701, 0x44 },
+	{ 0x3702, 0x18 },
+	{ 0x3703, 0x24 },
+	{ 0x3704, 0x24 },
+	{ 0x3705, 0x0c },
+	{ REG_TIMING_VERT_FORMAT, 0x81 },
+	{ REG_TIMING_HORIZ_FORMAT, 0x01 },
+	{ 0x370a, 0x52 },
+	{ REG_VFIFO_READ_START_H, 0x00 },
+	{ REG_VFIFO_READ_START_L, 0xa0 },
+	{ REG_ISP_CTRL02, 0x10 },
+	{ REG_NULL, 0x00 },
+};
+
+static const struct pll_ctrl_reg ctrl3[] = {
+	{ 1, 0x00 },
+	{ 2, 0x02 },
+	{ 3, 0x03 },
+	{ 4, 0x06 },
+	{ 6, 0x0d },
+	{ 8, 0x0e },
+	{ 12, 0x0f },
+	{ 16, 0x12 },
+	{ 24, 0x13 },
+	{ 32, 0x16 },
+	{ 48, 0x1b },
+	{ 64, 0x1e },
+	{ 96, 0x1f },
+	{ 0, 0x00 },
+};
+
+static const struct pll_ctrl_reg ctrl1[] = {
+	{ 2, 0x10 },
+	{ 4, 0x20 },
+	{ 6, 0x30 },
+	{ 8, 0x40 },
+	{ 10, 0x50 },
+	{ 12, 0x60 },
+	{ 14, 0x70 },
+	{ 16, 0x80 },
+	{ 18, 0x90 },
+	{ 20, 0xa0 },
+	{ 22, 0xb0 },
+	{ 24, 0xc0 },
+	{ 26, 0xd0 },
+	{ 28, 0xe0 },
+	{ 30, 0xf0 },
+	{ 0, 0x00 },
+};
+
+static const struct ov2659_framesize ov2659_framesizes[] = {
+	{ /* QVGA */
+		.width		= 320,
+		.height		= 240,
+		.regs		= ov2659_qvga,
+		.max_exp_lines	= 248,
+	}, { /* VGA */
+		.width		= 640,
+		.height		= 480,
+		.regs		= ov2659_vga,
+		.max_exp_lines	= 498,
+	}, { /* SVGA */
+		.width		= 800,
+		.height		= 600,
+		.regs		= ov2659_svga,
+		.max_exp_lines	= 498,
+	}, { /* XGA */
+		.width		= 1024,
+		.height		= 768,
+		.regs		= ov2659_xga,
+		.max_exp_lines	= 498,
+	}, { /* 720P */
+		.width		= 1280,
+		.height		= 720,
+		.regs		= ov2659_720p,
+		.max_exp_lines	= 498,
+	}, { /* SXGA */
+		.width		= 1280,
+		.height		= 1024,
+		.regs		= ov2659_sxga,
+		.max_exp_lines	= 1048,
+	}, { /* UXGA */
+		.width		= 1600,
+		.height		= 1200,
+		.regs		= ov2659_uxga,
+		.max_exp_lines	= 498,
+	},
+};
+
+/* YUV422 YUYV*/
+static struct sensor_register ov2659_format_yuyv[] = {
+	{ REG_FORMAT_CTRL00, 0x30 },
+	{ REG_NULL, 0x0 },
+};
+
+/* YUV422 UYVY  */
+static struct sensor_register ov2659_format_uyvy[] = {
+	{ REG_FORMAT_CTRL00, 0x32 },
+	{ REG_NULL, 0x0 },
+};
+
+/* Raw Bayer BGGR */
+static struct sensor_register ov2659_format_bggr[] = {
+	{ REG_FORMAT_CTRL00, 0x00 },
+	{ REG_NULL, 0x0 },
+};
+
+/* RGB565 */
+static struct sensor_register ov2659_format_rgb565[] = {
+	{ REG_FORMAT_CTRL00, 0x60 },
+	{ REG_NULL, 0x0 },
+};
+
+static const struct ov2659_pixfmt ov2659_formats[] = {
+	{
+		.code = MEDIA_BUS_FMT_YUYV8_2X8,
+		.format_ctrl_regs = ov2659_format_yuyv,
+	}, {
+		.code = MEDIA_BUS_FMT_UYVY8_2X8,
+		.format_ctrl_regs = ov2659_format_uyvy,
+	}, {
+		.code = MEDIA_BUS_FMT_RGB565_2X8_BE,
+		.format_ctrl_regs = ov2659_format_rgb565,
+	}, {
+		.code = MEDIA_BUS_FMT_SBGGR8_1X8,
+		.format_ctrl_regs = ov2659_format_bggr,
+	},
+};
+
+static inline struct ov2659 *to_ov2659(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct ov2659, sd);
+}
+
+/* sensor register write */
+static int ov2659_write(struct i2c_client *client, u16 reg, u8 val)
+{
+	struct i2c_msg msg;
+	u8 buf[3];
+	int ret;
+
+	buf[0] = reg >> 8;
+	buf[1] = reg & 0xFF;
+	buf[2] = val;
+
+	msg.addr = client->addr;
+	msg.flags = client->flags;
+	msg.buf = buf;
+	msg.len = sizeof(buf);
+
+	ret = i2c_transfer(client->adapter, &msg, 1);
+	if (ret >= 0)
+		return 0;
+
+	dev_dbg(&client->dev,
+		"ov2659 write reg(0x%x val:0x%x) failed !\n", reg, val);
+
+	return ret;
+}
+
+/* sensor register read */
+static int ov2659_read(struct i2c_client *client, u16 reg, u8 *val)
+{
+	struct i2c_msg msg[2];
+	u8 buf[2];
+	int ret;
+
+	buf[0] = reg >> 8;
+	buf[1] = reg & 0xFF;
+
+	msg[0].addr = client->addr;
+	msg[0].flags = client->flags;
+	msg[0].buf = buf;
+	msg[0].len = sizeof(buf);
+
+	msg[1].addr = client->addr;
+	msg[1].flags = client->flags | I2C_M_RD;
+	msg[1].buf = buf;
+	msg[1].len = 1;
+
+	ret = i2c_transfer(client->adapter, msg, 2);
+	if (ret >= 0) {
+		*val = buf[0];
+		return 0;
+	}
+
+	dev_dbg(&client->dev,
+		"ov2659 read reg(0x%x val:0x%x) failed !\n", reg, *val);
+
+	return ret;
+}
+
+static int ov2659_write_array(struct i2c_client *client,
+			      const struct sensor_register *regs)
+{
+	int i, ret = 0;
+
+	for (i = 0; ret == 0 && regs[i].addr; i++)
+		ret = ov2659_write(client, regs[i].addr, regs[i].value);
+
+	return ret;
+}
+
+static void ov2659_pll_calc_params(struct ov2659 *ov2659)
+{
+	const struct ov2659_platform_data *pdata = ov2659->pdata;
+	u8 ctrl1_reg = 0, ctrl2_reg = 0, ctrl3_reg = 0;
+	struct i2c_client *client = ov2659->client;
+	unsigned int desired = pdata->link_frequency;
+	u32 prediv, postdiv, mult;
+	u32 bestdelta = -1;
+	u32 delta, actual;
+	int i, j;
+
+	for (i = 0; ctrl1[i].div != 0; i++) {
+		postdiv = ctrl1[i].div;
+		for (j = 0; ctrl3[j].div != 0; j++) {
+			prediv = ctrl3[j].div;
+			for (mult = 1; mult <= 63; mult++) {
+				actual  = ov2659->xvclk_frequency;
+				actual *= mult;
+				actual /= prediv;
+				actual /= postdiv;
+				delta = actual - desired;
+				delta = abs(delta);
+
+				if ((delta < bestdelta) || (bestdelta == -1)) {
+					bestdelta = delta;
+					ctrl1_reg = ctrl1[i].reg;
+					ctrl2_reg = mult;
+					ctrl3_reg = ctrl3[j].reg;
+				}
+			}
+		}
+	}
+
+	ov2659->pll.ctrl1 = ctrl1_reg;
+	ov2659->pll.ctrl2 = ctrl2_reg;
+	ov2659->pll.ctrl3 = ctrl3_reg;
+
+	dev_dbg(&client->dev,
+		"Actual reg config: ctrl1_reg: %02x ctrl2_reg: %02x ctrl3_reg: %02x\n",
+		ctrl1_reg, ctrl2_reg, ctrl3_reg);
+}
+
+static int ov2659_set_pixel_clock(struct ov2659 *ov2659)
+{
+	struct i2c_client *client = ov2659->client;
+	struct sensor_register pll_regs[] = {
+		{REG_SC_PLL_CTRL1, ov2659->pll.ctrl1},
+		{REG_SC_PLL_CTRL2, ov2659->pll.ctrl2},
+		{REG_SC_PLL_CTRL3, ov2659->pll.ctrl3},
+		{REG_NULL, 0x00},
+	};
+
+	dev_dbg(&client->dev, "%s\n", __func__);
+
+	return ov2659_write_array(client, pll_regs);
+};
+
+static void ov2659_get_default_format(struct v4l2_mbus_framefmt *format)
+{
+	format->width = ov2659_framesizes[2].width;
+	format->height = ov2659_framesizes[2].height;
+	format->colorspace = V4L2_COLORSPACE_SRGB;
+	format->code = ov2659_formats[0].code;
+	format->field = V4L2_FIELD_NONE;
+}
+
+static void ov2659_set_streaming(struct ov2659 *ov2659, int on)
+{
+	struct i2c_client *client = ov2659->client;
+	int ret;
+
+	on = !!on;
+
+	dev_dbg(&client->dev, "%s: on: %d\n", __func__, on);
+
+	ret = ov2659_write(client, REG_SOFTWARE_STANDBY, on);
+	if (ret)
+		dev_err(&client->dev, "ov2659 soft standby failed\n");
+}
+
+static int ov2659_init(struct v4l2_subdev *sd, u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return ov2659_write_array(client, ov2659_init_regs);
+}
+
+/*
+ * V4L2 subdev video and pad level operations
+ */
+
+static int ov2659_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	dev_dbg(&client->dev, "%s:\n", __func__);
+
+	if (code->index >= ARRAY_SIZE(ov2659_formats))
+		return -EINVAL;
+
+	code->code = ov2659_formats[code->index].code;
+
+	return 0;
+}
+
+static int ov2659_enum_frame_sizes(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int i = ARRAY_SIZE(ov2659_formats);
+
+	dev_dbg(&client->dev, "%s:\n", __func__);
+
+	if (fse->index >= ARRAY_SIZE(ov2659_framesizes))
+		return -EINVAL;
+
+	while (--i)
+		if (fse->code == ov2659_formats[i].code)
+			break;
+
+	fse->code = ov2659_formats[i].code;
+
+	fse->min_width  = ov2659_framesizes[fse->index].width;
+	fse->max_width  = fse->min_width;
+	fse->max_height = ov2659_framesizes[fse->index].height;
+	fse->min_height = fse->max_height;
+
+	return 0;
+}
+
+static int ov2659_get_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov2659 *ov2659 = to_ov2659(sd);
+
+	dev_dbg(&client->dev, "ov2659_get_fmt\n");
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
+		struct v4l2_mbus_framefmt *mf;
+
+		mf = v4l2_subdev_get_try_format(sd, sd_state, 0);
+		mutex_lock(&ov2659->lock);
+		fmt->format = *mf;
+		mutex_unlock(&ov2659->lock);
+		return 0;
+#else
+		return -EINVAL;
+#endif
+	}
+
+	mutex_lock(&ov2659->lock);
+	fmt->format = ov2659->format;
+	mutex_unlock(&ov2659->lock);
+
+	dev_dbg(&client->dev, "ov2659_get_fmt: %x %dx%d\n",
+		ov2659->format.code, ov2659->format.width,
+		ov2659->format.height);
+
+	return 0;
+}
+
+static void __ov2659_try_frame_size(struct v4l2_mbus_framefmt *mf,
+				    const struct ov2659_framesize **size)
+{
+	const struct ov2659_framesize *fsize = &ov2659_framesizes[0];
+	const struct ov2659_framesize *match = NULL;
+	int i = ARRAY_SIZE(ov2659_framesizes);
+	unsigned int min_err = UINT_MAX;
+
+	while (i--) {
+		int err = abs(fsize->width - mf->width)
+				+ abs(fsize->height - mf->height);
+		if ((err < min_err) && (fsize->regs[0].addr)) {
+			min_err = err;
+			match = fsize;
+		}
+		fsize++;
+	}
+
+	if (!match)
+		match = &ov2659_framesizes[2];
+
+	mf->width  = match->width;
+	mf->height = match->height;
+
+	if (size)
+		*size = match;
+}
+
+static int ov2659_set_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int index = ARRAY_SIZE(ov2659_formats);
+	struct v4l2_mbus_framefmt *mf = &fmt->format;
+	const struct ov2659_framesize *size = NULL;
+	struct ov2659 *ov2659 = to_ov2659(sd);
+	int ret = 0;
+
+	dev_dbg(&client->dev, "ov2659_set_fmt\n");
+
+	__ov2659_try_frame_size(mf, &size);
+
+	while (--index >= 0)
+		if (ov2659_formats[index].code == mf->code)
+			break;
+
+	if (index < 0) {
+		index = 0;
+		mf->code = ov2659_formats[index].code;
+	}
+
+	mf->colorspace = V4L2_COLORSPACE_SRGB;
+	mf->field = V4L2_FIELD_NONE;
+
+	mutex_lock(&ov2659->lock);
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
+		mf = v4l2_subdev_get_try_format(sd, sd_state, fmt->pad);
+		*mf = fmt->format;
+#endif
+	} else {
+		s64 val;
+
+		if (ov2659->streaming) {
+			mutex_unlock(&ov2659->lock);
+			return -EBUSY;
+		}
+
+		ov2659->frame_size = size;
+		ov2659->format = fmt->format;
+		ov2659->format_ctrl_regs =
+			ov2659_formats[index].format_ctrl_regs;
+
+		if (ov2659->format.code != MEDIA_BUS_FMT_SBGGR8_1X8)
+			val = ov2659->pdata->link_frequency / 2;
+		else
+			val = ov2659->pdata->link_frequency;
+
+		ret = v4l2_ctrl_s_ctrl_int64(ov2659->link_frequency, val);
+		if (ret < 0)
+			dev_warn(&client->dev,
+				 "failed to set link_frequency rate (%d)\n",
+				 ret);
+	}
+
+	mutex_unlock(&ov2659->lock);
+	return ret;
+}
+
+static int ov2659_set_frame_size(struct ov2659 *ov2659)
+{
+	struct i2c_client *client = ov2659->client;
+
+	dev_dbg(&client->dev, "%s\n", __func__);
+
+	return ov2659_write_array(ov2659->client, ov2659->frame_size->regs);
+}
+
+static int ov2659_set_format(struct ov2659 *ov2659)
+{
+	struct i2c_client *client = ov2659->client;
+
+	dev_dbg(&client->dev, "%s\n", __func__);
+
+	return ov2659_write_array(ov2659->client, ov2659->format_ctrl_regs);
+}
+
+static int ov2659_s_stream(struct v4l2_subdev *sd, int on)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov2659 *ov2659 = to_ov2659(sd);
+	int ret = 0;
+
+	dev_dbg(&client->dev, "%s: on: %d\n", __func__, on);
+
+	mutex_lock(&ov2659->lock);
+
+	on = !!on;
+
+	if (ov2659->streaming == on)
+		goto unlock;
+
+	if (!on) {
+		/* Stop Streaming Sequence */
+		ov2659_set_streaming(ov2659, 0);
+		ov2659->streaming = on;
+		pm_runtime_put(&client->dev);
+		goto unlock;
+	}
+
+	ret = pm_runtime_resume_and_get(&client->dev);
+	if (ret < 0)
+		goto unlock;
+
+	ret = ov2659_init(sd, 0);
+	if (!ret)
+		ret = ov2659_set_pixel_clock(ov2659);
+	if (!ret)
+		ret = ov2659_set_frame_size(ov2659);
+	if (!ret)
+		ret = ov2659_set_format(ov2659);
+	if (!ret) {
+		ov2659_set_streaming(ov2659, 1);
+		ov2659->streaming = on;
+	}
+
+unlock:
+	mutex_unlock(&ov2659->lock);
+	return ret;
+}
+
+static int ov2659_set_test_pattern(struct ov2659 *ov2659, int value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov2659->sd);
+	int ret;
+	u8 val;
+
+	ret = ov2659_read(client, REG_PRE_ISP_CTRL00, &val);
+	if (ret < 0)
+		return ret;
+
+	switch (value) {
+	case 0:
+		val &= ~TEST_PATTERN_ENABLE;
+		break;
+	case 1:
+		val &= VERTICAL_COLOR_BAR_MASK;
+		val |= TEST_PATTERN_ENABLE;
+		break;
+	}
+
+	return ov2659_write(client, REG_PRE_ISP_CTRL00, val);
+}
+
+static int ov2659_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov2659 *ov2659 =
+			container_of(ctrl->handler, struct ov2659, ctrls);
+	struct i2c_client *client = ov2659->client;
+
+	/* V4L2 controls values will be applied only when power is already up */
+	if (!pm_runtime_get_if_in_use(&client->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_TEST_PATTERN:
+		return ov2659_set_test_pattern(ov2659, ctrl->val);
+	}
+
+	pm_runtime_put(&client->dev);
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops ov2659_ctrl_ops = {
+	.s_ctrl = ov2659_s_ctrl,
+};
+
+static const char * const ov2659_test_pattern_menu[] = {
+	"Disabled",
+	"Vertical Color Bars",
+};
+
+static int ov2659_power_off(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov2659 *ov2659 = to_ov2659(sd);
+
+	dev_dbg(&client->dev, "%s:\n", __func__);
+
+	gpiod_set_value(ov2659->pwdn_gpio, 1);
+
+	clk_disable_unprepare(ov2659->clk);
+
+	return 0;
+}
+
+static int ov2659_power_on(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov2659 *ov2659 = to_ov2659(sd);
+	int ret;
+
+	dev_dbg(&client->dev, "%s:\n", __func__);
+
+	ret = clk_prepare_enable(ov2659->clk);
+	if (ret) {
+		dev_err(&client->dev, "%s: failed to enable clock\n",
+			__func__);
+		return ret;
+	}
+
+	gpiod_set_value(ov2659->pwdn_gpio, 0);
+
+	if (ov2659->resetb_gpio) {
+		gpiod_set_value(ov2659->resetb_gpio, 1);
+		usleep_range(500, 1000);
+		gpiod_set_value(ov2659->resetb_gpio, 0);
+		usleep_range(3000, 5000);
+	}
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev internal operations
+ */
+
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
+static int ov2659_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format =
+				v4l2_subdev_get_try_format(sd, fh->state, 0);
+
+	dev_dbg(&client->dev, "%s:\n", __func__);
+
+	ov2659_get_default_format(format);
+
+	return 0;
+}
+#endif
+
+static const struct v4l2_subdev_core_ops ov2659_subdev_core_ops = {
+	.log_status = v4l2_ctrl_subdev_log_status,
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_video_ops ov2659_subdev_video_ops = {
+	.s_stream = ov2659_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops ov2659_subdev_pad_ops = {
+	.enum_mbus_code = ov2659_enum_mbus_code,
+	.enum_frame_size = ov2659_enum_frame_sizes,
+	.get_fmt = ov2659_get_fmt,
+	.set_fmt = ov2659_set_fmt,
+};
+
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
+static const struct v4l2_subdev_ops ov2659_subdev_ops = {
+	.core  = &ov2659_subdev_core_ops,
+	.video = &ov2659_subdev_video_ops,
+	.pad   = &ov2659_subdev_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops ov2659_subdev_internal_ops = {
+	.open = ov2659_open,
+};
+#endif
+
+static int ov2659_detect(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u8 pid = 0;
+	u8 ver = 0;
+	int ret;
+
+	dev_dbg(&client->dev, "%s:\n", __func__);
+
+	ret = ov2659_write(client, REG_SOFTWARE_RESET, 0x01);
+	if (ret != 0) {
+		dev_err(&client->dev, "Sensor soft reset failed\n");
+		return -ENODEV;
+	}
+	usleep_range(1000, 2000);
+
+	/* Check sensor revision */
+	ret = ov2659_read(client, REG_SC_CHIP_ID_H, &pid);
+	if (!ret)
+		ret = ov2659_read(client, REG_SC_CHIP_ID_L, &ver);
+
+	if (!ret) {
+		unsigned short id;
+
+		id = OV265X_ID(pid, ver);
+		if (id != OV2659_ID) {
+			dev_err(&client->dev,
+				"Sensor detection failed (%04X)\n", id);
+			ret = -ENODEV;
+		} else {
+			dev_info(&client->dev, "Found OV%04X sensor\n", id);
+		}
+	}
+
+	return ret;
+}
+
+static struct ov2659_platform_data *
+ov2659_get_pdata(struct i2c_client *client)
+{
+	struct ov2659_platform_data *pdata;
+	struct v4l2_fwnode_endpoint bus_cfg = { .bus_type = 0 };
+	struct device_node *endpoint;
+	int ret;
+
+	if (!IS_ENABLED(CONFIG_OF) || !client->dev.of_node)
+		return client->dev.platform_data;
+
+	endpoint = of_graph_get_next_endpoint(client->dev.of_node, NULL);
+	if (!endpoint)
+		return NULL;
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(of_fwnode_handle(endpoint),
+					       &bus_cfg);
+	if (ret) {
+		pdata = NULL;
+		goto done;
+	}
+
+	pdata = devm_kzalloc(&client->dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		goto done;
+
+	if (!bus_cfg.nr_of_link_frequencies) {
+		dev_err(&client->dev,
+			"link-frequencies property not found or too many\n");
+		pdata = NULL;
+		goto done;
+	}
+
+	pdata->link_frequency = bus_cfg.link_frequencies[0];
+
+done:
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+	of_node_put(endpoint);
+	return pdata;
+}
+
+static int ov2659_probe(struct i2c_client *client)
+{
+	const struct ov2659_platform_data *pdata = ov2659_get_pdata(client);
+	struct v4l2_subdev *sd;
+	struct ov2659 *ov2659;
+	int ret;
+
+	if (!pdata) {
+		dev_err(&client->dev, "platform data not specified\n");
+		return -EINVAL;
+	}
+
+	ov2659 = devm_kzalloc(&client->dev, sizeof(*ov2659), GFP_KERNEL);
+	if (!ov2659)
+		return -ENOMEM;
+
+	ov2659->pdata = pdata;
+	ov2659->client = client;
+
+	ov2659->clk = devm_clk_get(&client->dev, "xvclk");
+	if (IS_ERR(ov2659->clk))
+		return PTR_ERR(ov2659->clk);
+
+	ov2659->xvclk_frequency = clk_get_rate(ov2659->clk);
+	if (ov2659->xvclk_frequency < 6000000 ||
+	    ov2659->xvclk_frequency > 27000000)
+		return -EINVAL;
+
+	/* Optional gpio don't fail if not present */
+	ov2659->pwdn_gpio = devm_gpiod_get_optional(&client->dev, "powerdown",
+						    GPIOD_OUT_LOW);
+	if (IS_ERR(ov2659->pwdn_gpio))
+		return PTR_ERR(ov2659->pwdn_gpio);
+
+	/* Optional gpio don't fail if not present */
+	ov2659->resetb_gpio = devm_gpiod_get_optional(&client->dev, "reset",
+						      GPIOD_OUT_HIGH);
+	if (IS_ERR(ov2659->resetb_gpio))
+		return PTR_ERR(ov2659->resetb_gpio);
+
+	v4l2_ctrl_handler_init(&ov2659->ctrls, 2);
+	ov2659->link_frequency =
+			v4l2_ctrl_new_std(&ov2659->ctrls, &ov2659_ctrl_ops,
+					  V4L2_CID_PIXEL_RATE,
+					  pdata->link_frequency / 2,
+					  pdata->link_frequency, 1,
+					  pdata->link_frequency);
+	v4l2_ctrl_new_std_menu_items(&ov2659->ctrls, &ov2659_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(ov2659_test_pattern_menu) - 1,
+				     0, 0, ov2659_test_pattern_menu);
+	ov2659->sd.ctrl_handler = &ov2659->ctrls;
+
+	if (ov2659->ctrls.error) {
+		dev_err(&client->dev, "%s: control initialization error %d\n",
+			__func__, ov2659->ctrls.error);
+		return  ov2659->ctrls.error;
+	}
+
+	sd = &ov2659->sd;
+	client->flags |= I2C_CLIENT_SCCB;
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
+	v4l2_i2c_subdev_init(sd, client, &ov2659_subdev_ops);
+
+	sd->internal_ops = &ov2659_subdev_internal_ops;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+		     V4L2_SUBDEV_FL_HAS_EVENTS;
+#endif
+
+#if defined(CONFIG_MEDIA_CONTROLLER)
+	ov2659->pad.flags = MEDIA_PAD_FL_SOURCE;
+	sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret = media_entity_pads_init(&sd->entity, 1, &ov2659->pad);
+	if (ret < 0) {
+		v4l2_ctrl_handler_free(&ov2659->ctrls);
+		return ret;
+	}
+#endif
+
+	mutex_init(&ov2659->lock);
+
+	ov2659_get_default_format(&ov2659->format);
+	ov2659->frame_size = &ov2659_framesizes[2];
+	ov2659->format_ctrl_regs = ov2659_formats[0].format_ctrl_regs;
+
+	ret = ov2659_power_on(&client->dev);
+	if (ret < 0)
+		goto error;
+
+	ret = ov2659_detect(sd);
+	if (ret < 0)
+		goto error;
+
+	/* Calculate the PLL register value needed */
+	ov2659_pll_calc_params(ov2659);
+
+	ret = v4l2_async_register_subdev(&ov2659->sd);
+	if (ret)
+		goto error;
+
+	dev_info(&client->dev, "%s sensor driver registered !!\n", sd->name);
+
+	pm_runtime_set_active(&client->dev);
+	pm_runtime_enable(&client->dev);
+	pm_runtime_idle(&client->dev);
+
+	return 0;
+
+error:
+	v4l2_ctrl_handler_free(&ov2659->ctrls);
+	ov2659_power_off(&client->dev);
+	media_entity_cleanup(&sd->entity);
+	mutex_destroy(&ov2659->lock);
+	return ret;
+}
+
+static void ov2659_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov2659 *ov2659 = to_ov2659(sd);
+
+	v4l2_ctrl_handler_free(&ov2659->ctrls);
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	mutex_destroy(&ov2659->lock);
+
+	pm_runtime_disable(&client->dev);
+	if (!pm_runtime_status_suspended(&client->dev))
+		ov2659_power_off(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+}
+
+static const struct dev_pm_ops ov2659_pm_ops = {
+	SET_RUNTIME_PM_OPS(ov2659_power_off, ov2659_power_on, NULL)
+};
+
+static const struct i2c_device_id ov2659_id[] = {
+	{ "ov2659", 0 },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(i2c, ov2659_id);
+
+#if IS_ENABLED(CONFIG_OF)
+static const struct of_device_id ov2659_of_match[] = {
+	{ .compatible = "ovti,ov2659", },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, ov2659_of_match);
+#endif
+
+static struct i2c_driver ov2659_i2c_driver = {
+	.driver = {
+		.name	= DRIVER_NAME,
+		.pm	= &ov2659_pm_ops,
+		.of_match_table = of_match_ptr(ov2659_of_match),
+	},
+	.probe		= ov2659_probe,
+	.remove		= ov2659_remove,
+	.id_table	= ov2659_id,
+};
+
+module_i2c_driver(ov2659_i2c_driver);
+
+MODULE_AUTHOR("Benoit Parrot <bparrot@ti.com>");
+MODULE_DESCRIPTION("OV2659 CMOS Image Sensor driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/ov2680.c b/drivers/media/i2c/ov2680.c
new file mode 100644
index 000000000..72bab0ff8
--- /dev/null
+++ b/drivers/media/i2c/ov2680.c
@@ -0,0 +1,1286 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Omnivision OV2680 CMOS Image Sensor driver
+ *
+ * Copyright (C) 2018 Linaro Ltd
+ *
+ * Based on OV5640 Sensor Driver
+ * Copyright (C) 2011-2013 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2014-2017 Mentor Graphics Inc.
+ *
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/mod_devicetable.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/regulator/consumer.h>
+
+#include <media/v4l2-cci.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+#define OV2680_CHIP_ID				0x2680
+
+#define OV2680_REG_STREAM_CTRL			CCI_REG8(0x0100)
+#define OV2680_REG_SOFT_RESET			CCI_REG8(0x0103)
+
+#define OV2680_REG_CHIP_ID			CCI_REG16(0x300a)
+#define OV2680_REG_SC_CMMN_SUB_ID		CCI_REG8(0x302a)
+#define OV2680_REG_PLL_MULTIPLIER		CCI_REG16(0x3081)
+
+#define OV2680_REG_EXPOSURE_PK			CCI_REG24(0x3500)
+#define OV2680_REG_R_MANUAL			CCI_REG8(0x3503)
+#define OV2680_REG_GAIN_PK			CCI_REG16(0x350a)
+
+#define OV2680_REG_SENSOR_CTRL_0A		CCI_REG8(0x370a)
+
+#define OV2680_REG_HORIZONTAL_START		CCI_REG16(0x3800)
+#define OV2680_REG_VERTICAL_START		CCI_REG16(0x3802)
+#define OV2680_REG_HORIZONTAL_END		CCI_REG16(0x3804)
+#define OV2680_REG_VERTICAL_END			CCI_REG16(0x3806)
+#define OV2680_REG_HORIZONTAL_OUTPUT_SIZE	CCI_REG16(0x3808)
+#define OV2680_REG_VERTICAL_OUTPUT_SIZE		CCI_REG16(0x380a)
+#define OV2680_REG_TIMING_HTS			CCI_REG16(0x380c)
+#define OV2680_REG_TIMING_VTS			CCI_REG16(0x380e)
+#define OV2680_REG_ISP_X_WIN			CCI_REG16(0x3810)
+#define OV2680_REG_ISP_Y_WIN			CCI_REG16(0x3812)
+#define OV2680_REG_X_INC			CCI_REG8(0x3814)
+#define OV2680_REG_Y_INC			CCI_REG8(0x3815)
+#define OV2680_REG_FORMAT1			CCI_REG8(0x3820)
+#define OV2680_REG_FORMAT2			CCI_REG8(0x3821)
+
+#define OV2680_REG_ISP_CTRL00			CCI_REG8(0x5080)
+
+#define OV2680_REG_X_WIN			CCI_REG16(0x5704)
+#define OV2680_REG_Y_WIN			CCI_REG16(0x5706)
+
+#define OV2680_FRAME_RATE			30
+
+#define OV2680_NATIVE_WIDTH			1616
+#define OV2680_NATIVE_HEIGHT			1216
+#define OV2680_NATIVE_START_LEFT		0
+#define OV2680_NATIVE_START_TOP			0
+#define OV2680_ACTIVE_WIDTH			1600
+#define OV2680_ACTIVE_HEIGHT			1200
+#define OV2680_ACTIVE_START_LEFT		8
+#define OV2680_ACTIVE_START_TOP			8
+#define OV2680_MIN_CROP_WIDTH			2
+#define OV2680_MIN_CROP_HEIGHT			2
+
+/* Fixed pre-div of 1/2 */
+#define OV2680_PLL_PREDIV0			2
+
+/* Pre-div configurable through reg 0x3080, left at its default of 0x02 : 1/2 */
+#define OV2680_PLL_PREDIV			2
+
+/* 66MHz pixel clock: 66MHz / 1704 * 1294 = 30fps */
+#define OV2680_PIXELS_PER_LINE			1704
+#define OV2680_LINES_PER_FRAME			1294
+
+/* If possible send 16 extra rows / lines to the ISP as padding */
+#define OV2680_END_MARGIN			16
+
+/* Max exposure time is VTS - 8 */
+#define OV2680_INTEGRATION_TIME_MARGIN		8
+
+#define OV2680_DEFAULT_WIDTH			800
+#define OV2680_DEFAULT_HEIGHT			600
+
+/* For enum_frame_size() full-size + binned-/quarter-size */
+#define OV2680_FRAME_SIZES			2
+
+static const char * const ov2680_supply_name[] = {
+	"DOVDD",
+	"DVDD",
+	"AVDD",
+};
+
+#define OV2680_NUM_SUPPLIES ARRAY_SIZE(ov2680_supply_name)
+
+enum {
+	OV2680_19_2_MHZ,
+	OV2680_24_MHZ,
+};
+
+static const unsigned long ov2680_xvclk_freqs[] = {
+	[OV2680_19_2_MHZ] = 19200000,
+	[OV2680_24_MHZ] = 24000000,
+};
+
+static const u8 ov2680_pll_multipliers[] = {
+	[OV2680_19_2_MHZ] = 69,
+	[OV2680_24_MHZ] = 55,
+};
+
+struct ov2680_ctrls {
+	struct v4l2_ctrl_handler handler;
+	struct v4l2_ctrl *exposure;
+	struct v4l2_ctrl *gain;
+	struct v4l2_ctrl *hflip;
+	struct v4l2_ctrl *vflip;
+	struct v4l2_ctrl *test_pattern;
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *pixel_rate;
+};
+
+struct ov2680_mode {
+	struct v4l2_rect		crop;
+	struct v4l2_mbus_framefmt	fmt;
+	struct v4l2_fract		frame_interval;
+	bool				binning;
+	u16				h_start;
+	u16				v_start;
+	u16				h_end;
+	u16				v_end;
+	u16				h_output_size;
+	u16				v_output_size;
+	u16				hts;
+	u16				vts;
+};
+
+struct ov2680_dev {
+	struct device			*dev;
+	struct regmap			*regmap;
+	struct v4l2_subdev		sd;
+
+	struct media_pad		pad;
+	struct clk			*xvclk;
+	u32				xvclk_freq;
+	u8				pll_mult;
+	s64				link_freq[1];
+	u64				pixel_rate;
+	struct regulator_bulk_data	supplies[OV2680_NUM_SUPPLIES];
+
+	struct gpio_desc		*pwdn_gpio;
+	struct mutex			lock; /* protect members */
+
+	bool				is_streaming;
+
+	struct ov2680_ctrls		ctrls;
+	struct ov2680_mode		mode;
+};
+
+static const struct v4l2_rect ov2680_default_crop = {
+	.left = OV2680_ACTIVE_START_LEFT,
+	.top = OV2680_ACTIVE_START_TOP,
+	.width = OV2680_ACTIVE_WIDTH,
+	.height = OV2680_ACTIVE_HEIGHT,
+};
+
+static const char * const test_pattern_menu[] = {
+	"Disabled",
+	"Color Bars",
+	"Random Data",
+	"Square",
+	"Black Image",
+};
+
+static const int ov2680_hv_flip_bayer_order[] = {
+	MEDIA_BUS_FMT_SBGGR10_1X10,
+	MEDIA_BUS_FMT_SGRBG10_1X10,
+	MEDIA_BUS_FMT_SGBRG10_1X10,
+	MEDIA_BUS_FMT_SRGGB10_1X10,
+};
+
+static const struct reg_sequence ov2680_global_setting[] = {
+	/* MIPI PHY, 0x10 -> 0x1c enable bp_c_hs_en_lat and bp_d_hs_en_lat */
+	{0x3016, 0x1c},
+
+	/* R MANUAL set exposure and gain to manual (hw does not do auto) */
+	{0x3503, 0x03},
+
+	/* Analog control register tweaks */
+	{0x3603, 0x39}, /* Reset value 0x99 */
+	{0x3604, 0x24}, /* Reset value 0x74 */
+	{0x3621, 0x37}, /* Reset value 0x44 */
+
+	/* Sensor control register tweaks */
+	{0x3701, 0x64}, /* Reset value 0x61 */
+	{0x3705, 0x3c}, /* Reset value 0x21 */
+	{0x370c, 0x50}, /* Reset value 0x10 */
+	{0x370d, 0xc0}, /* Reset value 0x00 */
+	{0x3718, 0x88}, /* Reset value 0x80 */
+
+	/* PSRAM tweaks */
+	{0x3781, 0x80}, /* Reset value 0x00 */
+	{0x3784, 0x0c}, /* Reset value 0x00, based on OV2680_R1A_AM10.ovt */
+	{0x3789, 0x60}, /* Reset value 0x50 */
+
+	/* BLC CTRL00 0x01 -> 0x81 set avg_weight to 8 */
+	{0x4000, 0x81},
+
+	/* Set black level compensation range to 0 - 3 (default 0 - 11) */
+	{0x4008, 0x00},
+	{0x4009, 0x03},
+
+	/* VFIFO R2 0x00 -> 0x02 set Frame reset enable */
+	{0x4602, 0x02},
+
+	/* MIPI ctrl CLK PREPARE MIN change from 0x26 (38) -> 0x36 (54) */
+	{0x481f, 0x36},
+
+	/* MIPI ctrl CLK LPX P MIN change from 0x32 (50) -> 0x36 (54) */
+	{0x4825, 0x36},
+
+	/* R ISP CTRL2 0x20 -> 0x30, set sof_sel bit */
+	{0x5002, 0x30},
+
+	/*
+	 * Window CONTROL 0x00 -> 0x01, enable manual window control,
+	 * this is necessary for full size flip and mirror support.
+	 */
+	{0x5708, 0x01},
+
+	/*
+	 * DPC CTRL0 0x14 -> 0x3e, set enable_tail, enable_3x3_cluster
+	 * and enable_general_tail bits based OV2680_R1A_AM10.ovt.
+	 */
+	{0x5780, 0x3e},
+
+	/* DPC MORE CONNECTION CASE THRE 0x0c (12) -> 0x02 (2) */
+	{0x5788, 0x02},
+
+	/* DPC GAIN LIST1 0x0f (15) -> 0x08 (8) */
+	{0x578e, 0x08},
+
+	/* DPC GAIN LIST2 0x3f (63) -> 0x0c (12) */
+	{0x578f, 0x0c},
+
+	/* DPC THRE RATIO 0x04 (4) -> 0x00 (0) */
+	{0x5792, 0x00},
+};
+
+static struct ov2680_dev *to_ov2680_dev(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct ov2680_dev, sd);
+}
+
+static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct ov2680_dev,
+			     ctrls.handler)->sd;
+}
+
+static void ov2680_power_up(struct ov2680_dev *sensor)
+{
+	if (!sensor->pwdn_gpio)
+		return;
+
+	gpiod_set_value(sensor->pwdn_gpio, 0);
+	usleep_range(5000, 10000);
+}
+
+static void ov2680_power_down(struct ov2680_dev *sensor)
+{
+	if (!sensor->pwdn_gpio)
+		return;
+
+	gpiod_set_value(sensor->pwdn_gpio, 1);
+	usleep_range(5000, 10000);
+}
+
+static void ov2680_set_bayer_order(struct ov2680_dev *sensor,
+				   struct v4l2_mbus_framefmt *fmt)
+{
+	int hv_flip = 0;
+
+	if (sensor->ctrls.vflip && sensor->ctrls.vflip->val)
+		hv_flip += 1;
+
+	if (sensor->ctrls.hflip && sensor->ctrls.hflip->val)
+		hv_flip += 2;
+
+	fmt->code = ov2680_hv_flip_bayer_order[hv_flip];
+}
+
+static struct v4l2_mbus_framefmt *
+__ov2680_get_pad_format(struct ov2680_dev *sensor,
+			struct v4l2_subdev_state *state,
+			unsigned int pad,
+			enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(&sensor->sd, state, pad);
+
+	return &sensor->mode.fmt;
+}
+
+static struct v4l2_rect *
+__ov2680_get_pad_crop(struct ov2680_dev *sensor,
+		      struct v4l2_subdev_state *state,
+		      unsigned int pad,
+		      enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_crop(&sensor->sd, state, pad);
+
+	return &sensor->mode.crop;
+}
+
+static void ov2680_fill_format(struct ov2680_dev *sensor,
+			       struct v4l2_mbus_framefmt *fmt,
+			       unsigned int width, unsigned int height)
+{
+	memset(fmt, 0, sizeof(*fmt));
+	fmt->width = width;
+	fmt->height = height;
+	fmt->field = V4L2_FIELD_NONE;
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+	ov2680_set_bayer_order(sensor, fmt);
+}
+
+static void ov2680_calc_mode(struct ov2680_dev *sensor)
+{
+	int width = sensor->mode.fmt.width;
+	int height = sensor->mode.fmt.height;
+	int orig_width = width;
+	int orig_height = height;
+
+	if (width  <= (sensor->mode.crop.width / 2) &&
+	    height <= (sensor->mode.crop.height / 2)) {
+		sensor->mode.binning = true;
+		width *= 2;
+		height *= 2;
+	} else {
+		sensor->mode.binning = false;
+	}
+
+	sensor->mode.h_start = (sensor->mode.crop.left +
+				(sensor->mode.crop.width - width) / 2) & ~1;
+	sensor->mode.v_start = (sensor->mode.crop.top +
+				(sensor->mode.crop.height - height) / 2) & ~1;
+	sensor->mode.h_end =
+		min(sensor->mode.h_start + width + OV2680_END_MARGIN - 1,
+		    OV2680_NATIVE_WIDTH - 1);
+	sensor->mode.v_end =
+		min(sensor->mode.v_start + height + OV2680_END_MARGIN - 1,
+		    OV2680_NATIVE_HEIGHT - 1);
+	sensor->mode.h_output_size = orig_width;
+	sensor->mode.v_output_size = orig_height;
+	sensor->mode.hts = OV2680_PIXELS_PER_LINE;
+	sensor->mode.vts = OV2680_LINES_PER_FRAME;
+}
+
+static int ov2680_set_mode(struct ov2680_dev *sensor)
+{
+	u8 sensor_ctrl_0a, inc, fmt1, fmt2;
+	int ret = 0;
+
+	if (sensor->mode.binning) {
+		sensor_ctrl_0a = 0x23;
+		inc = 0x31;
+		fmt1 = 0xc2;
+		fmt2 = 0x01;
+	} else {
+		sensor_ctrl_0a = 0x21;
+		inc = 0x11;
+		fmt1 = 0xc0;
+		fmt2 = 0x00;
+	}
+
+	cci_write(sensor->regmap, OV2680_REG_SENSOR_CTRL_0A,
+		  sensor_ctrl_0a, &ret);
+	cci_write(sensor->regmap, OV2680_REG_HORIZONTAL_START,
+		  sensor->mode.h_start, &ret);
+	cci_write(sensor->regmap, OV2680_REG_VERTICAL_START,
+		  sensor->mode.v_start, &ret);
+	cci_write(sensor->regmap, OV2680_REG_HORIZONTAL_END,
+		  sensor->mode.h_end, &ret);
+	cci_write(sensor->regmap, OV2680_REG_VERTICAL_END,
+		  sensor->mode.v_end, &ret);
+	cci_write(sensor->regmap, OV2680_REG_HORIZONTAL_OUTPUT_SIZE,
+		  sensor->mode.h_output_size, &ret);
+	cci_write(sensor->regmap, OV2680_REG_VERTICAL_OUTPUT_SIZE,
+		  sensor->mode.v_output_size, &ret);
+	cci_write(sensor->regmap, OV2680_REG_TIMING_HTS,
+		  sensor->mode.hts, &ret);
+	cci_write(sensor->regmap, OV2680_REG_TIMING_VTS,
+		  sensor->mode.vts, &ret);
+	cci_write(sensor->regmap, OV2680_REG_ISP_X_WIN, 0, &ret);
+	cci_write(sensor->regmap, OV2680_REG_ISP_Y_WIN, 0, &ret);
+	cci_write(sensor->regmap, OV2680_REG_X_INC, inc, &ret);
+	cci_write(sensor->regmap, OV2680_REG_Y_INC, inc, &ret);
+	cci_write(sensor->regmap, OV2680_REG_X_WIN,
+		  sensor->mode.h_output_size, &ret);
+	cci_write(sensor->regmap, OV2680_REG_Y_WIN,
+		  sensor->mode.v_output_size, &ret);
+	cci_write(sensor->regmap, OV2680_REG_FORMAT1, fmt1, &ret);
+	cci_write(sensor->regmap, OV2680_REG_FORMAT2, fmt2, &ret);
+
+	return ret;
+}
+
+static int ov2680_set_vflip(struct ov2680_dev *sensor, s32 val)
+{
+	int ret;
+
+	if (sensor->is_streaming)
+		return -EBUSY;
+
+	ret = cci_update_bits(sensor->regmap, OV2680_REG_FORMAT1,
+			      BIT(2), val ? BIT(2) : 0, NULL);
+	if (ret < 0)
+		return ret;
+
+	ov2680_set_bayer_order(sensor, &sensor->mode.fmt);
+	return 0;
+}
+
+static int ov2680_set_hflip(struct ov2680_dev *sensor, s32 val)
+{
+	int ret;
+
+	if (sensor->is_streaming)
+		return -EBUSY;
+
+	ret = cci_update_bits(sensor->regmap, OV2680_REG_FORMAT2,
+			      BIT(2), val ? BIT(2) : 0, NULL);
+	if (ret < 0)
+		return ret;
+
+	ov2680_set_bayer_order(sensor, &sensor->mode.fmt);
+	return 0;
+}
+
+static int ov2680_test_pattern_set(struct ov2680_dev *sensor, int value)
+{
+	int ret = 0;
+
+	if (!value)
+		return cci_update_bits(sensor->regmap, OV2680_REG_ISP_CTRL00,
+				       BIT(7), 0, NULL);
+
+	cci_update_bits(sensor->regmap, OV2680_REG_ISP_CTRL00,
+			0x03, value - 1, &ret);
+	cci_update_bits(sensor->regmap, OV2680_REG_ISP_CTRL00,
+			BIT(7), BIT(7), &ret);
+
+	return ret;
+}
+
+static int ov2680_gain_set(struct ov2680_dev *sensor, u32 gain)
+{
+	return cci_write(sensor->regmap, OV2680_REG_GAIN_PK, gain, NULL);
+}
+
+static int ov2680_exposure_set(struct ov2680_dev *sensor, u32 exp)
+{
+	return cci_write(sensor->regmap, OV2680_REG_EXPOSURE_PK, exp << 4,
+			 NULL);
+}
+
+static int ov2680_stream_enable(struct ov2680_dev *sensor)
+{
+	int ret;
+
+	ret = cci_write(sensor->regmap, OV2680_REG_PLL_MULTIPLIER,
+			sensor->pll_mult, NULL);
+	if (ret < 0)
+		return ret;
+
+	ret = regmap_multi_reg_write(sensor->regmap,
+				     ov2680_global_setting,
+				     ARRAY_SIZE(ov2680_global_setting));
+	if (ret < 0)
+		return ret;
+
+	ret = ov2680_set_mode(sensor);
+	if (ret < 0)
+		return ret;
+
+	/* Restore value of all ctrls */
+	ret = __v4l2_ctrl_handler_setup(&sensor->ctrls.handler);
+	if (ret < 0)
+		return ret;
+
+	return cci_write(sensor->regmap, OV2680_REG_STREAM_CTRL, 1, NULL);
+}
+
+static int ov2680_stream_disable(struct ov2680_dev *sensor)
+{
+	return cci_write(sensor->regmap, OV2680_REG_STREAM_CTRL, 0, NULL);
+}
+
+static int ov2680_power_off(struct ov2680_dev *sensor)
+{
+	clk_disable_unprepare(sensor->xvclk);
+	ov2680_power_down(sensor);
+	regulator_bulk_disable(OV2680_NUM_SUPPLIES, sensor->supplies);
+	return 0;
+}
+
+static int ov2680_power_on(struct ov2680_dev *sensor)
+{
+	int ret;
+
+	ret = regulator_bulk_enable(OV2680_NUM_SUPPLIES, sensor->supplies);
+	if (ret < 0) {
+		dev_err(sensor->dev, "failed to enable regulators: %d\n", ret);
+		return ret;
+	}
+
+	if (!sensor->pwdn_gpio) {
+		ret = cci_write(sensor->regmap, OV2680_REG_SOFT_RESET, 0x01,
+				NULL);
+		if (ret != 0) {
+			dev_err(sensor->dev, "sensor soft reset failed\n");
+			goto err_disable_regulators;
+		}
+		usleep_range(1000, 2000);
+	} else {
+		ov2680_power_down(sensor);
+		ov2680_power_up(sensor);
+	}
+
+	ret = clk_prepare_enable(sensor->xvclk);
+	if (ret < 0)
+		goto err_disable_regulators;
+
+	return 0;
+
+err_disable_regulators:
+	regulator_bulk_disable(OV2680_NUM_SUPPLIES, sensor->supplies);
+	return ret;
+}
+
+static int ov2680_s_g_frame_interval(struct v4l2_subdev *sd,
+				     struct v4l2_subdev_frame_interval *fi)
+{
+	struct ov2680_dev *sensor = to_ov2680_dev(sd);
+
+	mutex_lock(&sensor->lock);
+	fi->interval = sensor->mode.frame_interval;
+	mutex_unlock(&sensor->lock);
+
+	return 0;
+}
+
+static int ov2680_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct ov2680_dev *sensor = to_ov2680_dev(sd);
+	int ret = 0;
+
+	mutex_lock(&sensor->lock);
+
+	if (sensor->is_streaming == !!enable)
+		goto unlock;
+
+	if (enable) {
+		ret = pm_runtime_resume_and_get(sensor->sd.dev);
+		if (ret < 0)
+			goto unlock;
+
+		ret = ov2680_stream_enable(sensor);
+		if (ret < 0) {
+			pm_runtime_put(sensor->sd.dev);
+			goto unlock;
+		}
+	} else {
+		ret = ov2680_stream_disable(sensor);
+		pm_runtime_put(sensor->sd.dev);
+	}
+
+	sensor->is_streaming = !!enable;
+
+unlock:
+	mutex_unlock(&sensor->lock);
+
+	return ret;
+}
+
+static int ov2680_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct ov2680_dev *sensor = to_ov2680_dev(sd);
+
+	if (code->index != 0)
+		return -EINVAL;
+
+	code->code = sensor->mode.fmt.code;
+
+	return 0;
+}
+
+static int ov2680_get_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *format)
+{
+	struct ov2680_dev *sensor = to_ov2680_dev(sd);
+	struct v4l2_mbus_framefmt *fmt;
+
+	fmt = __ov2680_get_pad_format(sensor, sd_state, format->pad,
+				      format->which);
+
+	mutex_lock(&sensor->lock);
+	format->format = *fmt;
+	mutex_unlock(&sensor->lock);
+
+	return 0;
+}
+
+static int ov2680_set_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *format)
+{
+	struct ov2680_dev *sensor = to_ov2680_dev(sd);
+	struct v4l2_mbus_framefmt *try_fmt;
+	const struct v4l2_rect *crop;
+	unsigned int width, height;
+	int ret = 0;
+
+	crop = __ov2680_get_pad_crop(sensor, sd_state, format->pad,
+				     format->which);
+
+	/* Limit set_fmt max size to crop width / height */
+	width = clamp_val(ALIGN(format->format.width, 2),
+			  OV2680_MIN_CROP_WIDTH, crop->width);
+	height = clamp_val(ALIGN(format->format.height, 2),
+			   OV2680_MIN_CROP_HEIGHT, crop->height);
+
+	ov2680_fill_format(sensor, &format->format, width, height);
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		try_fmt = v4l2_subdev_get_try_format(sd, sd_state, 0);
+		*try_fmt = format->format;
+		return 0;
+	}
+
+	mutex_lock(&sensor->lock);
+
+	if (sensor->is_streaming) {
+		ret = -EBUSY;
+		goto unlock;
+	}
+
+	sensor->mode.fmt = format->format;
+	ov2680_calc_mode(sensor);
+
+unlock:
+	mutex_unlock(&sensor->lock);
+
+	return ret;
+}
+
+static int ov2680_get_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *state,
+				struct v4l2_subdev_selection *sel)
+{
+	struct ov2680_dev *sensor = to_ov2680_dev(sd);
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		mutex_lock(&sensor->lock);
+		sel->r = *__ov2680_get_pad_crop(sensor, state, sel->pad,
+						sel->which);
+		mutex_unlock(&sensor->lock);
+		break;
+	case V4L2_SEL_TGT_NATIVE_SIZE:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.top = 0;
+		sel->r.left = 0;
+		sel->r.width = OV2680_NATIVE_WIDTH;
+		sel->r.height = OV2680_NATIVE_HEIGHT;
+		break;
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		sel->r = ov2680_default_crop;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int ov2680_set_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *state,
+				struct v4l2_subdev_selection *sel)
+{
+	struct ov2680_dev *sensor = to_ov2680_dev(sd);
+	struct v4l2_mbus_framefmt *format;
+	struct v4l2_rect *crop;
+	struct v4l2_rect rect;
+
+	if (sel->target != V4L2_SEL_TGT_CROP)
+		return -EINVAL;
+
+	/*
+	 * Clamp the boundaries of the crop rectangle to the size of the sensor
+	 * pixel array. Align to multiples of 2 to ensure Bayer pattern isn't
+	 * disrupted.
+	 */
+	rect.left = clamp_val(ALIGN(sel->r.left, 2),
+			      OV2680_NATIVE_START_LEFT, OV2680_NATIVE_WIDTH);
+	rect.top = clamp_val(ALIGN(sel->r.top, 2),
+			     OV2680_NATIVE_START_TOP, OV2680_NATIVE_HEIGHT);
+	rect.width = clamp_val(ALIGN(sel->r.width, 2),
+			       OV2680_MIN_CROP_WIDTH, OV2680_NATIVE_WIDTH);
+	rect.height = clamp_val(ALIGN(sel->r.height, 2),
+				OV2680_MIN_CROP_HEIGHT, OV2680_NATIVE_HEIGHT);
+
+	/* Make sure the crop rectangle isn't outside the bounds of the array */
+	rect.width = min_t(unsigned int, rect.width,
+			   OV2680_NATIVE_WIDTH - rect.left);
+	rect.height = min_t(unsigned int, rect.height,
+			    OV2680_NATIVE_HEIGHT - rect.top);
+
+	crop = __ov2680_get_pad_crop(sensor, state, sel->pad, sel->which);
+
+	mutex_lock(&sensor->lock);
+	if (rect.width != crop->width || rect.height != crop->height) {
+		/*
+		 * Reset the output image size if the crop rectangle size has
+		 * been modified.
+		 */
+		format = __ov2680_get_pad_format(sensor, state, sel->pad,
+						 sel->which);
+		format->width = rect.width;
+		format->height = rect.height;
+	}
+
+	*crop = rect;
+	mutex_unlock(&sensor->lock);
+
+	sel->r = rect;
+
+	return 0;
+}
+
+static int ov2680_init_cfg(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_state *sd_state)
+{
+	struct ov2680_dev *sensor = to_ov2680_dev(sd);
+
+	sd_state->pads[0].try_crop = ov2680_default_crop;
+
+	ov2680_fill_format(sensor, &sd_state->pads[0].try_fmt,
+			   OV2680_DEFAULT_WIDTH, OV2680_DEFAULT_HEIGHT);
+	return 0;
+}
+
+static int ov2680_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct ov2680_dev *sensor = to_ov2680_dev(sd);
+	struct v4l2_rect *crop;
+
+	if (fse->index >= OV2680_FRAME_SIZES)
+		return -EINVAL;
+
+	crop = __ov2680_get_pad_crop(sensor, sd_state, fse->pad, fse->which);
+	if (!crop)
+		return -EINVAL;
+
+	fse->min_width = crop->width / (fse->index + 1);
+	fse->min_height = crop->height / (fse->index + 1);
+	fse->max_width = fse->min_width;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static bool ov2680_valid_frame_size(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_state *sd_state,
+				    struct v4l2_subdev_frame_interval_enum *fie)
+{
+	struct v4l2_subdev_frame_size_enum fse = {
+		.pad = fie->pad,
+		.which = fie->which,
+	};
+	int i;
+
+	for (i = 0; i < OV2680_FRAME_SIZES; i++) {
+		fse.index = i;
+
+		if (ov2680_enum_frame_size(sd, sd_state, &fse))
+			return false;
+
+		if (fie->width == fse.min_width &&
+		    fie->height == fse.min_height)
+			return true;
+	}
+
+	return false;
+}
+
+static int ov2680_enum_frame_interval(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_frame_interval_enum *fie)
+{
+	struct ov2680_dev *sensor = to_ov2680_dev(sd);
+
+	/* Only 1 framerate */
+	if (fie->index || !ov2680_valid_frame_size(sd, sd_state, fie))
+		return -EINVAL;
+
+	fie->interval = sensor->mode.frame_interval;
+
+	return 0;
+}
+
+static int ov2680_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
+	struct ov2680_dev *sensor = to_ov2680_dev(sd);
+	int ret;
+
+	/* Only apply changes to the controls if the device is powered up */
+	if (!pm_runtime_get_if_in_use(sensor->sd.dev)) {
+		ov2680_set_bayer_order(sensor, &sensor->mode.fmt);
+		return 0;
+	}
+
+	switch (ctrl->id) {
+	case V4L2_CID_ANALOGUE_GAIN:
+		ret = ov2680_gain_set(sensor, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE:
+		ret = ov2680_exposure_set(sensor, ctrl->val);
+		break;
+	case V4L2_CID_VFLIP:
+		ret = ov2680_set_vflip(sensor, ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+		ret = ov2680_set_hflip(sensor, ctrl->val);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		ret = ov2680_test_pattern_set(sensor, ctrl->val);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	pm_runtime_put(sensor->sd.dev);
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ov2680_ctrl_ops = {
+	.s_ctrl = ov2680_s_ctrl,
+};
+
+static const struct v4l2_subdev_video_ops ov2680_video_ops = {
+	.g_frame_interval	= ov2680_s_g_frame_interval,
+	.s_frame_interval	= ov2680_s_g_frame_interval,
+	.s_stream		= ov2680_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops ov2680_pad_ops = {
+	.init_cfg		= ov2680_init_cfg,
+	.enum_mbus_code		= ov2680_enum_mbus_code,
+	.enum_frame_size	= ov2680_enum_frame_size,
+	.enum_frame_interval	= ov2680_enum_frame_interval,
+	.get_fmt		= ov2680_get_fmt,
+	.set_fmt		= ov2680_set_fmt,
+	.get_selection		= ov2680_get_selection,
+	.set_selection		= ov2680_set_selection,
+};
+
+static const struct v4l2_subdev_ops ov2680_subdev_ops = {
+	.video	= &ov2680_video_ops,
+	.pad	= &ov2680_pad_ops,
+};
+
+static int ov2680_mode_init(struct ov2680_dev *sensor)
+{
+	/* set initial mode */
+	sensor->mode.crop = ov2680_default_crop;
+	ov2680_fill_format(sensor, &sensor->mode.fmt,
+			   OV2680_DEFAULT_WIDTH, OV2680_DEFAULT_HEIGHT);
+	ov2680_calc_mode(sensor);
+
+	sensor->mode.frame_interval.denominator = OV2680_FRAME_RATE;
+	sensor->mode.frame_interval.numerator = 1;
+
+	return 0;
+}
+
+static int ov2680_v4l2_register(struct ov2680_dev *sensor)
+{
+	struct i2c_client *client = to_i2c_client(sensor->dev);
+	const struct v4l2_ctrl_ops *ops = &ov2680_ctrl_ops;
+	struct ov2680_ctrls *ctrls = &sensor->ctrls;
+	struct v4l2_ctrl_handler *hdl = &ctrls->handler;
+	int exp_max = OV2680_LINES_PER_FRAME - OV2680_INTEGRATION_TIME_MARGIN;
+	int ret = 0;
+
+	v4l2_i2c_subdev_init(&sensor->sd, client, &ov2680_subdev_ops);
+
+	sensor->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
+	sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
+	sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+	ret = media_entity_pads_init(&sensor->sd.entity, 1, &sensor->pad);
+	if (ret < 0)
+		return ret;
+
+	v4l2_ctrl_handler_init(hdl, 5);
+
+	hdl->lock = &sensor->lock;
+
+	ctrls->vflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
+	ctrls->hflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
+
+	ctrls->test_pattern = v4l2_ctrl_new_std_menu_items(hdl,
+					&ov2680_ctrl_ops, V4L2_CID_TEST_PATTERN,
+					ARRAY_SIZE(test_pattern_menu) - 1,
+					0, 0, test_pattern_menu);
+
+	ctrls->exposure = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
+					    0, exp_max, 1, exp_max);
+
+	ctrls->gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_ANALOGUE_GAIN,
+					0, 1023, 1, 250);
+
+	ctrls->link_freq = v4l2_ctrl_new_int_menu(hdl, NULL, V4L2_CID_LINK_FREQ,
+						  0, 0, sensor->link_freq);
+	ctrls->pixel_rate = v4l2_ctrl_new_std(hdl, NULL, V4L2_CID_PIXEL_RATE,
+					      0, sensor->pixel_rate,
+					      1, sensor->pixel_rate);
+
+	if (hdl->error) {
+		ret = hdl->error;
+		goto cleanup_entity;
+	}
+
+	ctrls->vflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
+	ctrls->hflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
+	ctrls->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	sensor->sd.ctrl_handler = hdl;
+
+	ret = v4l2_async_register_subdev(&sensor->sd);
+	if (ret < 0)
+		goto cleanup_entity;
+
+	return 0;
+
+cleanup_entity:
+	media_entity_cleanup(&sensor->sd.entity);
+	v4l2_ctrl_handler_free(hdl);
+
+	return ret;
+}
+
+static int ov2680_get_regulators(struct ov2680_dev *sensor)
+{
+	int i;
+
+	for (i = 0; i < OV2680_NUM_SUPPLIES; i++)
+		sensor->supplies[i].supply = ov2680_supply_name[i];
+
+	return devm_regulator_bulk_get(sensor->dev,
+				       OV2680_NUM_SUPPLIES, sensor->supplies);
+}
+
+static int ov2680_check_id(struct ov2680_dev *sensor)
+{
+	u64 chip_id, rev;
+	int ret = 0;
+
+	cci_read(sensor->regmap, OV2680_REG_CHIP_ID, &chip_id, &ret);
+	cci_read(sensor->regmap, OV2680_REG_SC_CMMN_SUB_ID, &rev, &ret);
+	if (ret < 0) {
+		dev_err(sensor->dev, "failed to read chip id\n");
+		return ret;
+	}
+
+	if (chip_id != OV2680_CHIP_ID) {
+		dev_err(sensor->dev, "chip id: 0x%04llx does not match expected 0x%04x\n",
+			chip_id, OV2680_CHIP_ID);
+		return -ENODEV;
+	}
+
+	dev_info(sensor->dev, "sensor_revision id = 0x%llx, rev= %lld\n",
+		 chip_id, rev & 0x0f);
+
+	return 0;
+}
+
+static int ov2680_parse_dt(struct ov2680_dev *sensor)
+{
+	struct v4l2_fwnode_endpoint bus_cfg = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY,
+	};
+	struct device *dev = sensor->dev;
+	struct fwnode_handle *ep_fwnode;
+	struct gpio_desc *gpio;
+	unsigned int rate = 0;
+	int i, ret;
+
+	/*
+	 * Sometimes the fwnode graph is initialized by the bridge driver.
+	 * Bridge drivers doing this may also add GPIO mappings, wait for this.
+	 */
+	ep_fwnode = fwnode_graph_get_next_endpoint(dev_fwnode(dev), NULL);
+	if (!ep_fwnode)
+		return dev_err_probe(dev, -EPROBE_DEFER,
+				     "waiting for fwnode graph endpoint\n");
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(ep_fwnode, &bus_cfg);
+	fwnode_handle_put(ep_fwnode);
+	if (ret)
+		return ret;
+
+	/*
+	 * The pin we want is named XSHUTDN in the datasheet. Linux sensor
+	 * drivers have standardized on using "powerdown" as con-id name
+	 * for powerdown or shutdown pins. Older DTB files use "reset",
+	 * so fallback to that if there is no "powerdown" pin.
+	 */
+	gpio = devm_gpiod_get_optional(dev, "powerdown", GPIOD_OUT_HIGH);
+	if (!gpio)
+		gpio = devm_gpiod_get_optional(dev, "reset", GPIOD_OUT_HIGH);
+
+	ret = PTR_ERR_OR_ZERO(gpio);
+	if (ret < 0) {
+		dev_dbg(dev, "error while getting reset gpio: %d\n", ret);
+		goto out_free_bus_cfg;
+	}
+
+	sensor->pwdn_gpio = gpio;
+
+	sensor->xvclk = devm_clk_get_optional(dev, "xvclk");
+	if (IS_ERR(sensor->xvclk)) {
+		ret = dev_err_probe(dev, PTR_ERR(sensor->xvclk),
+				    "xvclk clock missing or invalid\n");
+		goto out_free_bus_cfg;
+	}
+
+	/*
+	 * We could have either a 24MHz or 19.2MHz clock rate from either DT or
+	 * ACPI... but we also need to support the weird IPU3 case which will
+	 * have an external clock AND a clock-frequency property. Check for the
+	 * clock-frequency property and if found, set that rate if we managed
+	 * to acquire a clock. This should cover the ACPI case. If the system
+	 * uses devicetree then the configured rate should already be set, so
+	 * we can just read it.
+	 */
+	ret = fwnode_property_read_u32(dev_fwnode(dev), "clock-frequency",
+				       &rate);
+	if (ret && !sensor->xvclk) {
+		dev_err_probe(dev, ret, "invalid clock config\n");
+		goto out_free_bus_cfg;
+	}
+
+	if (!ret && sensor->xvclk) {
+		ret = clk_set_rate(sensor->xvclk, rate);
+		if (ret) {
+			dev_err_probe(dev, ret, "failed to set clock rate\n");
+			goto out_free_bus_cfg;
+		}
+	}
+
+	sensor->xvclk_freq = rate ?: clk_get_rate(sensor->xvclk);
+
+	for (i = 0; i < ARRAY_SIZE(ov2680_xvclk_freqs); i++) {
+		if (sensor->xvclk_freq == ov2680_xvclk_freqs[i])
+			break;
+	}
+
+	if (i == ARRAY_SIZE(ov2680_xvclk_freqs)) {
+		ret = dev_err_probe(dev, -EINVAL,
+				    "unsupported xvclk frequency %d Hz\n",
+				    sensor->xvclk_freq);
+		goto out_free_bus_cfg;
+	}
+
+	sensor->pll_mult = ov2680_pll_multipliers[i];
+
+	sensor->link_freq[0] = sensor->xvclk_freq / OV2680_PLL_PREDIV0 /
+			       OV2680_PLL_PREDIV * sensor->pll_mult;
+
+	/* CSI-2 is double data rate, bus-format is 10 bpp */
+	sensor->pixel_rate = sensor->link_freq[0] * 2;
+	do_div(sensor->pixel_rate, 10);
+
+	/* Verify bus cfg */
+	if (bus_cfg.bus.mipi_csi2.num_data_lanes != 1) {
+		ret = dev_err_probe(dev, -EINVAL,
+				    "only a 1-lane CSI2 config is supported");
+		goto out_free_bus_cfg;
+	}
+
+	for (i = 0; i < bus_cfg.nr_of_link_frequencies; i++)
+		if (bus_cfg.link_frequencies[i] == sensor->link_freq[0])
+			break;
+
+	if (bus_cfg.nr_of_link_frequencies == 0 ||
+	    bus_cfg.nr_of_link_frequencies == i) {
+		ret = dev_err_probe(dev, -EINVAL,
+				    "supported link freq %lld not found\n",
+				    sensor->link_freq[0]);
+		goto out_free_bus_cfg;
+	}
+
+out_free_bus_cfg:
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+	return ret;
+}
+
+static int ov2680_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct ov2680_dev *sensor;
+	int ret;
+
+	sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
+	if (!sensor)
+		return -ENOMEM;
+
+	sensor->dev = &client->dev;
+
+	sensor->regmap = devm_cci_regmap_init_i2c(client, 16);
+	if (IS_ERR(sensor->regmap))
+		return PTR_ERR(sensor->regmap);
+
+	ret = ov2680_parse_dt(sensor);
+	if (ret < 0)
+		return ret;
+
+	ret = ov2680_mode_init(sensor);
+	if (ret < 0)
+		return ret;
+
+	ret = ov2680_get_regulators(sensor);
+	if (ret < 0) {
+		dev_err(dev, "failed to get regulators\n");
+		return ret;
+	}
+
+	mutex_init(&sensor->lock);
+
+	/*
+	 * Power up and verify the chip now, so that if runtime pm is
+	 * disabled the chip is left on and streaming will work.
+	 */
+	ret = ov2680_power_on(sensor);
+	if (ret < 0)
+		goto lock_destroy;
+
+	ret = ov2680_check_id(sensor);
+	if (ret < 0)
+		goto err_powerdown;
+
+	pm_runtime_set_active(&client->dev);
+	pm_runtime_get_noresume(&client->dev);
+	pm_runtime_enable(&client->dev);
+
+	ret = ov2680_v4l2_register(sensor);
+	if (ret < 0)
+		goto err_pm_runtime;
+
+	pm_runtime_set_autosuspend_delay(&client->dev, 1000);
+	pm_runtime_use_autosuspend(&client->dev);
+	pm_runtime_put_autosuspend(&client->dev);
+
+	return 0;
+
+err_pm_runtime:
+	pm_runtime_disable(&client->dev);
+	pm_runtime_put_noidle(&client->dev);
+err_powerdown:
+	ov2680_power_off(sensor);
+lock_destroy:
+	dev_err(dev, "ov2680 init fail: %d\n", ret);
+	mutex_destroy(&sensor->lock);
+
+	return ret;
+}
+
+static void ov2680_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov2680_dev *sensor = to_ov2680_dev(sd);
+
+	v4l2_async_unregister_subdev(&sensor->sd);
+	mutex_destroy(&sensor->lock);
+	media_entity_cleanup(&sensor->sd.entity);
+	v4l2_ctrl_handler_free(&sensor->ctrls.handler);
+
+	/*
+	 * Disable runtime PM. In case runtime PM is disabled in the kernel,
+	 * make sure to turn power off manually.
+	 */
+	pm_runtime_disable(&client->dev);
+	if (!pm_runtime_status_suspended(&client->dev))
+		ov2680_power_off(sensor);
+	pm_runtime_set_suspended(&client->dev);
+}
+
+static int ov2680_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov2680_dev *sensor = to_ov2680_dev(sd);
+
+	if (sensor->is_streaming)
+		ov2680_stream_disable(sensor);
+
+	return ov2680_power_off(sensor);
+}
+
+static int ov2680_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov2680_dev *sensor = to_ov2680_dev(sd);
+	int ret;
+
+	ret = ov2680_power_on(sensor);
+	if (ret < 0)
+		goto stream_disable;
+
+	if (sensor->is_streaming) {
+		ret = ov2680_stream_enable(sensor);
+		if (ret < 0)
+			goto stream_disable;
+	}
+
+	return 0;
+
+stream_disable:
+	ov2680_stream_disable(sensor);
+	sensor->is_streaming = false;
+
+	return ret;
+}
+
+static DEFINE_RUNTIME_DEV_PM_OPS(ov2680_pm_ops, ov2680_suspend, ov2680_resume,
+				 NULL);
+
+static const struct of_device_id ov2680_dt_ids[] = {
+	{ .compatible = "ovti,ov2680" },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, ov2680_dt_ids);
+
+static const struct acpi_device_id ov2680_acpi_ids[] = {
+	{ "OVTI2680" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(acpi, ov2680_acpi_ids);
+
+static struct i2c_driver ov2680_i2c_driver = {
+	.driver = {
+		.name  = "ov2680",
+		.pm = pm_sleep_ptr(&ov2680_pm_ops),
+		.of_match_table	= ov2680_dt_ids,
+		.acpi_match_table = ov2680_acpi_ids,
+	},
+	.probe		= ov2680_probe,
+	.remove		= ov2680_remove,
+};
+module_i2c_driver(ov2680_i2c_driver);
+
+MODULE_AUTHOR("Rui Miguel Silva <rui.silva@linaro.org>");
+MODULE_DESCRIPTION("OV2680 CMOS Image Sensor driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/ov2685.c b/drivers/media/i2c/ov2685.c
new file mode 100644
index 000000000..303793e1f
--- /dev/null
+++ b/drivers/media/i2c/ov2685.c
@@ -0,0 +1,913 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * ov2685 driver
+ *
+ * Copyright (C) 2017 Fuzhou Rockchip Electronics Co., Ltd.
+ */
+
+#include <linux/clk.h>
+#include <linux/device.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 <linux/sysfs.h>
+#include <media/media-entity.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+#define CHIP_ID				0x2685
+#define OV2685_REG_CHIP_ID		0x300a
+
+#define OV2685_XVCLK_FREQ		24000000
+
+#define REG_SC_CTRL_MODE		0x0100
+#define     SC_CTRL_MODE_STANDBY	0x0
+#define     SC_CTRL_MODE_STREAMING	BIT(0)
+
+#define OV2685_REG_EXPOSURE		0x3500
+#define	OV2685_EXPOSURE_MIN		4
+#define	OV2685_EXPOSURE_STEP		1
+
+#define OV2685_REG_VTS			0x380e
+#define OV2685_VTS_MAX			0x7fff
+
+#define OV2685_REG_GAIN			0x350a
+#define OV2685_GAIN_MIN			0
+#define OV2685_GAIN_MAX			0x07ff
+#define OV2685_GAIN_STEP		0x1
+#define OV2685_GAIN_DEFAULT		0x0036
+
+#define OV2685_REG_TEST_PATTERN		0x5080
+#define OV2685_TEST_PATTERN_DISABLED		0x00
+#define OV2685_TEST_PATTERN_COLOR_BAR		0x80
+#define OV2685_TEST_PATTERN_RANDOM		0x81
+#define OV2685_TEST_PATTERN_COLOR_BAR_FADE	0x88
+#define OV2685_TEST_PATTERN_BW_SQUARE		0x92
+#define OV2685_TEST_PATTERN_COLOR_SQUARE	0x82
+
+#define REG_NULL			0xFFFF
+
+#define OV2685_REG_VALUE_08BIT		1
+#define OV2685_REG_VALUE_16BIT		2
+#define OV2685_REG_VALUE_24BIT		3
+
+#define OV2685_NATIVE_WIDTH		1616
+#define OV2685_NATIVE_HEIGHT		1216
+
+#define OV2685_LANES			1
+#define OV2685_BITS_PER_SAMPLE		10
+
+static const char * const ov2685_supply_names[] = {
+	"avdd",		/* Analog power */
+	"dovdd",	/* Digital I/O power */
+	"dvdd",		/* Digital core power */
+};
+
+#define OV2685_NUM_SUPPLIES ARRAY_SIZE(ov2685_supply_names)
+
+struct regval {
+	u16 addr;
+	u8 val;
+};
+
+struct ov2685_mode {
+	u32 width;
+	u32 height;
+	u32 exp_def;
+	u32 hts_def;
+	u32 vts_def;
+	const struct v4l2_rect *analog_crop;
+	const struct regval *reg_list;
+};
+
+struct ov2685 {
+	struct i2c_client	*client;
+	struct clk		*xvclk;
+	struct gpio_desc	*reset_gpio;
+	struct regulator_bulk_data supplies[OV2685_NUM_SUPPLIES];
+
+	bool			streaming;
+	struct mutex		mutex;
+	struct v4l2_subdev	subdev;
+	struct media_pad	pad;
+	struct v4l2_ctrl	*anal_gain;
+	struct v4l2_ctrl	*exposure;
+	struct v4l2_ctrl	*hblank;
+	struct v4l2_ctrl	*vblank;
+	struct v4l2_ctrl	*test_pattern;
+	struct v4l2_ctrl_handler ctrl_handler;
+
+	const struct ov2685_mode *cur_mode;
+};
+
+#define to_ov2685(sd) container_of(sd, struct ov2685, subdev)
+
+/* PLL settings bases on 24M xvclk */
+static struct regval ov2685_1600x1200_regs[] = {
+	{0x0103, 0x01},
+	{0x0100, 0x00},
+	{0x3002, 0x00},
+	{0x3016, 0x1c},
+	{0x3018, 0x44},
+	{0x301d, 0xf0},
+	{0x3020, 0x00},
+	{0x3082, 0x37},
+	{0x3083, 0x03},
+	{0x3084, 0x09},
+	{0x3085, 0x04},
+	{0x3086, 0x00},
+	{0x3087, 0x00},
+	{0x3501, 0x4e},
+	{0x3502, 0xe0},
+	{0x3503, 0x27},
+	{0x350b, 0x36},
+	{0x3600, 0xb4},
+	{0x3603, 0x35},
+	{0x3604, 0x24},
+	{0x3605, 0x00},
+	{0x3620, 0x24},
+	{0x3621, 0x34},
+	{0x3622, 0x03},
+	{0x3628, 0x10},
+	{0x3705, 0x3c},
+	{0x370a, 0x21},
+	{0x370c, 0x50},
+	{0x370d, 0xc0},
+	{0x3717, 0x58},
+	{0x3718, 0x80},
+	{0x3720, 0x00},
+	{0x3721, 0x09},
+	{0x3722, 0x06},
+	{0x3723, 0x59},
+	{0x3738, 0x99},
+	{0x3781, 0x80},
+	{0x3784, 0x0c},
+	{0x3789, 0x60},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x00},
+	{0x3804, 0x06},
+	{0x3805, 0x4f},
+	{0x3806, 0x04},
+	{0x3807, 0xbf},
+	{0x3808, 0x06},
+	{0x3809, 0x40},
+	{0x380a, 0x04},
+	{0x380b, 0xb0},
+	{0x380c, 0x06},
+	{0x380d, 0xa4},
+	{0x380e, 0x05},
+	{0x380f, 0x0e},
+	{0x3810, 0x00},
+	{0x3811, 0x08},
+	{0x3812, 0x00},
+	{0x3813, 0x08},
+	{0x3814, 0x11},
+	{0x3815, 0x11},
+	{0x3819, 0x04},
+	{0x3820, 0xc0},
+	{0x3821, 0x00},
+	{0x3a06, 0x01},
+	{0x3a07, 0x84},
+	{0x3a08, 0x01},
+	{0x3a09, 0x43},
+	{0x3a0a, 0x24},
+	{0x3a0b, 0x60},
+	{0x3a0c, 0x28},
+	{0x3a0d, 0x60},
+	{0x3a0e, 0x04},
+	{0x3a0f, 0x8c},
+	{0x3a10, 0x05},
+	{0x3a11, 0x0c},
+	{0x4000, 0x81},
+	{0x4001, 0x40},
+	{0x4008, 0x02},
+	{0x4009, 0x09},
+	{0x4300, 0x00},
+	{0x430e, 0x00},
+	{0x4602, 0x02},
+	{0x481b, 0x40},
+	{0x481f, 0x40},
+	{0x4837, 0x18},
+	{0x5000, 0x1f},
+	{0x5001, 0x05},
+	{0x5002, 0x30},
+	{0x5003, 0x04},
+	{0x5004, 0x00},
+	{0x5005, 0x0c},
+	{0x5280, 0x15},
+	{0x5281, 0x06},
+	{0x5282, 0x06},
+	{0x5283, 0x08},
+	{0x5284, 0x1c},
+	{0x5285, 0x1c},
+	{0x5286, 0x20},
+	{0x5287, 0x10},
+	{REG_NULL, 0x00}
+};
+
+#define OV2685_LINK_FREQ_330MHZ		330000000
+static const s64 link_freq_menu_items[] = {
+	OV2685_LINK_FREQ_330MHZ
+};
+
+static const char * const ov2685_test_pattern_menu[] = {
+	"Disabled",
+	"Color Bar",
+	"Color Bar FADE",
+	"Random Data",
+	"Black White Square",
+	"Color Square"
+};
+
+static const int ov2685_test_pattern_val[] = {
+	OV2685_TEST_PATTERN_DISABLED,
+	OV2685_TEST_PATTERN_COLOR_BAR,
+	OV2685_TEST_PATTERN_COLOR_BAR_FADE,
+	OV2685_TEST_PATTERN_RANDOM,
+	OV2685_TEST_PATTERN_BW_SQUARE,
+	OV2685_TEST_PATTERN_COLOR_SQUARE,
+};
+
+static const struct v4l2_rect ov2685_analog_crop = {
+	.left	= 8,
+	.top	= 8,
+	.width	= 1600,
+	.height	= 1200,
+};
+
+static const struct ov2685_mode supported_modes[] = {
+	{
+		.width = 1600,
+		.height = 1200,
+		.exp_def = 0x04ee,
+		.hts_def = 0x06a4,
+		.vts_def = 0x050e,
+		.analog_crop = &ov2685_analog_crop,
+		.reg_list = ov2685_1600x1200_regs,
+	},
+};
+
+/* Write registers up to 4 at a time */
+static int ov2685_write_reg(struct i2c_client *client, u16 reg,
+			    u32 len, u32 val)
+{
+	u32 val_i, buf_i;
+	u8 buf[6];
+	u8 *val_p;
+	__be32 val_be;
+
+	if (len > 4)
+		return -EINVAL;
+
+	buf[0] = reg >> 8;
+	buf[1] = reg & 0xff;
+
+	val_be = cpu_to_be32(val);
+	val_p = (u8 *)&val_be;
+	buf_i = 2;
+	val_i = 4 - len;
+
+	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;
+}
+
+static int ov2685_write_array(struct i2c_client *client,
+			      const struct regval *regs)
+{
+	int ret = 0;
+	u32 i;
+
+	for (i = 0; ret == 0 && regs[i].addr != REG_NULL; i++)
+		ret = ov2685_write_reg(client, regs[i].addr,
+				       OV2685_REG_VALUE_08BIT, regs[i].val);
+
+	return ret;
+}
+
+/* Read registers up to 4 at a time */
+static int ov2685_read_reg(struct i2c_client *client, u16 reg,
+			   u32 len, u32 *val)
+{
+	struct i2c_msg msgs[2];
+	u8 *data_be_p;
+	__be32 data_be = 0;
+	__be16 reg_addr_be = cpu_to_be16(reg);
+	int ret;
+
+	if (len > 4)
+		return -EINVAL;
+
+	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[4 - len];
+
+	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 void ov2685_fill_fmt(const struct ov2685_mode *mode,
+			    struct v4l2_mbus_framefmt *fmt)
+{
+	fmt->code = MEDIA_BUS_FMT_SBGGR10_1X10;
+	fmt->width = mode->width;
+	fmt->height = mode->height;
+	fmt->field = V4L2_FIELD_NONE;
+}
+
+static int ov2685_set_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct ov2685 *ov2685 = to_ov2685(sd);
+	struct v4l2_mbus_framefmt *mbus_fmt = &fmt->format;
+
+	/* only one mode supported for now */
+	ov2685_fill_fmt(ov2685->cur_mode, mbus_fmt);
+
+	return 0;
+}
+
+static int ov2685_get_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct ov2685 *ov2685 = to_ov2685(sd);
+	struct v4l2_mbus_framefmt *mbus_fmt = &fmt->format;
+
+	ov2685_fill_fmt(ov2685->cur_mode, mbus_fmt);
+
+	return 0;
+}
+
+static int ov2685_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index >= ARRAY_SIZE(supported_modes))
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_SBGGR10_1X10;
+
+	return 0;
+}
+
+static int ov2685_enum_frame_sizes(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_frame_size_enum *fse)
+{
+	int index = fse->index;
+
+	if (index >= ARRAY_SIZE(supported_modes))
+		return -EINVAL;
+
+	fse->code = MEDIA_BUS_FMT_SBGGR10_1X10;
+
+	fse->min_width  = supported_modes[index].width;
+	fse->max_width  = supported_modes[index].width;
+	fse->max_height = supported_modes[index].height;
+	fse->min_height = supported_modes[index].height;
+
+	return 0;
+}
+
+static const struct v4l2_rect *
+__ov2685_get_pad_crop(struct ov2685 *ov2685,
+		      struct v4l2_subdev_state *state, unsigned int pad,
+		      enum v4l2_subdev_format_whence which)
+{
+	const struct ov2685_mode *mode = ov2685->cur_mode;
+
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_crop(&ov2685->subdev, state, pad);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return mode->analog_crop;
+	}
+
+	return NULL;
+}
+
+static int ov2685_get_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *sd_state,
+				struct v4l2_subdev_selection *sel)
+{
+	struct ov2685 *ov2685 = to_ov2685(sd);
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		mutex_lock(&ov2685->mutex);
+		sel->r = *__ov2685_get_pad_crop(ov2685, sd_state, sel->pad,
+				sel->which);
+		mutex_unlock(&ov2685->mutex);
+		break;
+	case V4L2_SEL_TGT_NATIVE_SIZE:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.top = 0;
+		sel->r.left = 0;
+		sel->r.width = OV2685_NATIVE_WIDTH;
+		sel->r.height = OV2685_NATIVE_HEIGHT;
+		break;
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		sel->r = ov2685_analog_crop;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* Calculate the delay in us by clock rate and clock cycles */
+static inline u32 ov2685_cal_delay(u32 cycles)
+{
+	return DIV_ROUND_UP(cycles, OV2685_XVCLK_FREQ / 1000 / 1000);
+}
+
+static int __ov2685_power_on(struct ov2685 *ov2685)
+{
+	int ret;
+	u32 delay_us;
+	struct device *dev = &ov2685->client->dev;
+
+	ret = clk_prepare_enable(ov2685->xvclk);
+	if (ret < 0) {
+		dev_err(dev, "Failed to enable xvclk\n");
+		return ret;
+	}
+
+	gpiod_set_value_cansleep(ov2685->reset_gpio, 1);
+
+	ret = regulator_bulk_enable(OV2685_NUM_SUPPLIES, ov2685->supplies);
+	if (ret < 0) {
+		dev_err(dev, "Failed to enable regulators\n");
+		goto disable_clk;
+	}
+
+	/* The minimum delay between power supplies and reset rising can be 0 */
+	gpiod_set_value_cansleep(ov2685->reset_gpio, 0);
+	/* 8192 xvclk cycles prior to the first SCCB transaction */
+	delay_us = ov2685_cal_delay(8192);
+	usleep_range(delay_us, delay_us * 2);
+
+	/* HACK: ov2685 would output messy data after reset(R0103),
+	 * writing register before .s_stream() as a workaround
+	 */
+	ret = ov2685_write_array(ov2685->client, ov2685->cur_mode->reg_list);
+	if (ret) {
+		dev_err(dev, "Failed to set regs for power on\n");
+		goto disable_supplies;
+	}
+
+	return 0;
+
+disable_supplies:
+	regulator_bulk_disable(OV2685_NUM_SUPPLIES, ov2685->supplies);
+disable_clk:
+	clk_disable_unprepare(ov2685->xvclk);
+
+	return ret;
+}
+
+static void __ov2685_power_off(struct ov2685 *ov2685)
+{
+	/* 512 xvclk cycles after the last SCCB transaction or MIPI frame end */
+	u32 delay_us = ov2685_cal_delay(512);
+
+	usleep_range(delay_us, delay_us * 2);
+	clk_disable_unprepare(ov2685->xvclk);
+	gpiod_set_value_cansleep(ov2685->reset_gpio, 1);
+	regulator_bulk_disable(OV2685_NUM_SUPPLIES, ov2685->supplies);
+}
+
+static int ov2685_s_stream(struct v4l2_subdev *sd, int on)
+{
+	struct ov2685 *ov2685 = to_ov2685(sd);
+	struct i2c_client *client = ov2685->client;
+	int ret = 0;
+
+	mutex_lock(&ov2685->mutex);
+
+	on = !!on;
+	if (on == ov2685->streaming)
+		goto unlock_and_return;
+
+	if (on) {
+		ret = pm_runtime_resume_and_get(&ov2685->client->dev);
+		if (ret < 0)
+			goto unlock_and_return;
+
+		ret = __v4l2_ctrl_handler_setup(&ov2685->ctrl_handler);
+		if (ret) {
+			pm_runtime_put(&client->dev);
+			goto unlock_and_return;
+		}
+		ret = ov2685_write_reg(client, REG_SC_CTRL_MODE,
+				OV2685_REG_VALUE_08BIT, SC_CTRL_MODE_STREAMING);
+		if (ret) {
+			pm_runtime_put(&client->dev);
+			goto unlock_and_return;
+		}
+	} else {
+		ov2685_write_reg(client, REG_SC_CTRL_MODE,
+				OV2685_REG_VALUE_08BIT, SC_CTRL_MODE_STANDBY);
+		pm_runtime_put(&ov2685->client->dev);
+	}
+
+	ov2685->streaming = on;
+
+unlock_and_return:
+	mutex_unlock(&ov2685->mutex);
+
+	return ret;
+}
+
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
+static int ov2685_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct ov2685 *ov2685 = to_ov2685(sd);
+	struct v4l2_mbus_framefmt *try_fmt;
+
+	mutex_lock(&ov2685->mutex);
+
+	try_fmt = v4l2_subdev_get_try_format(sd, fh->state, 0);
+	/* Initialize try_fmt */
+	ov2685_fill_fmt(&supported_modes[0], try_fmt);
+
+	mutex_unlock(&ov2685->mutex);
+
+	return 0;
+}
+#endif
+
+static int __maybe_unused ov2685_runtime_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov2685 *ov2685 = to_ov2685(sd);
+
+	return __ov2685_power_on(ov2685);
+}
+
+static int __maybe_unused ov2685_runtime_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov2685 *ov2685 = to_ov2685(sd);
+
+	__ov2685_power_off(ov2685);
+
+	return 0;
+}
+
+static const struct dev_pm_ops ov2685_pm_ops = {
+	SET_RUNTIME_PM_OPS(ov2685_runtime_suspend,
+			   ov2685_runtime_resume, NULL)
+};
+
+static int ov2685_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov2685 *ov2685 = container_of(ctrl->handler,
+					     struct ov2685, ctrl_handler);
+	struct i2c_client *client = ov2685->client;
+	s64 max_expo;
+	int ret;
+
+	/* Propagate change of current control to all related controls */
+	switch (ctrl->id) {
+	case V4L2_CID_VBLANK:
+		/* Update max exposure while meeting expected vblanking */
+		max_expo = ov2685->cur_mode->height + ctrl->val - 4;
+		__v4l2_ctrl_modify_range(ov2685->exposure,
+					 ov2685->exposure->minimum, max_expo,
+					 ov2685->exposure->step,
+					 ov2685->exposure->default_value);
+		break;
+	}
+
+	if (!pm_runtime_get_if_in_use(&client->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE:
+		ret = ov2685_write_reg(ov2685->client, OV2685_REG_EXPOSURE,
+				       OV2685_REG_VALUE_24BIT, ctrl->val << 4);
+		break;
+	case V4L2_CID_ANALOGUE_GAIN:
+		ret = ov2685_write_reg(ov2685->client, OV2685_REG_GAIN,
+				       OV2685_REG_VALUE_16BIT, ctrl->val);
+		break;
+	case V4L2_CID_VBLANK:
+		ret = ov2685_write_reg(ov2685->client, OV2685_REG_VTS,
+				       OV2685_REG_VALUE_16BIT,
+				       ctrl->val + ov2685->cur_mode->height);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		ret = ov2685_write_reg(ov2685->client, OV2685_REG_TEST_PATTERN,
+				       OV2685_REG_VALUE_08BIT,
+				       ov2685_test_pattern_val[ctrl->val]);
+		break;
+	default:
+		dev_warn(&client->dev, "%s Unhandled id:0x%x, val:0x%x\n",
+			 __func__, ctrl->id, ctrl->val);
+		ret = -EINVAL;
+		break;
+	}
+
+	pm_runtime_put(&client->dev);
+
+	return ret;
+}
+
+static const struct v4l2_subdev_video_ops ov2685_video_ops = {
+	.s_stream = ov2685_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops ov2685_pad_ops = {
+	.enum_mbus_code = ov2685_enum_mbus_code,
+	.enum_frame_size = ov2685_enum_frame_sizes,
+	.get_fmt = ov2685_get_fmt,
+	.set_fmt = ov2685_set_fmt,
+	.get_selection = ov2685_get_selection,
+	.set_selection = ov2685_get_selection,
+};
+
+static const struct v4l2_subdev_ops ov2685_subdev_ops = {
+	.video	= &ov2685_video_ops,
+	.pad	= &ov2685_pad_ops,
+};
+
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
+static const struct v4l2_subdev_internal_ops ov2685_internal_ops = {
+	.open = ov2685_open,
+};
+#endif
+
+static const struct v4l2_ctrl_ops ov2685_ctrl_ops = {
+	.s_ctrl = ov2685_set_ctrl,
+};
+
+static int ov2685_initialize_controls(struct ov2685 *ov2685)
+{
+	const struct ov2685_mode *mode;
+	struct v4l2_ctrl_handler *handler;
+	struct v4l2_ctrl *ctrl;
+	struct v4l2_fwnode_device_properties props;
+	u64 exposure_max;
+	u32 pixel_rate, h_blank;
+	int ret;
+
+	handler = &ov2685->ctrl_handler;
+	mode = ov2685->cur_mode;
+	ret = v4l2_ctrl_handler_init(handler, 10);
+	if (ret)
+		return ret;
+	handler->lock = &ov2685->mutex;
+
+	ctrl = v4l2_ctrl_new_int_menu(handler, NULL, V4L2_CID_LINK_FREQ,
+				      0, 0, link_freq_menu_items);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	pixel_rate = (link_freq_menu_items[0] * 2 * OV2685_LANES) /
+		     OV2685_BITS_PER_SAMPLE;
+	v4l2_ctrl_new_std(handler, NULL, V4L2_CID_PIXEL_RATE,
+			  0, pixel_rate, 1, pixel_rate);
+
+	h_blank = mode->hts_def - mode->width;
+	ov2685->hblank = v4l2_ctrl_new_std(handler, NULL, V4L2_CID_HBLANK,
+				h_blank, h_blank, 1, h_blank);
+	if (ov2685->hblank)
+		ov2685->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	ov2685->vblank = v4l2_ctrl_new_std(handler, &ov2685_ctrl_ops,
+				V4L2_CID_VBLANK, mode->vts_def - mode->height,
+				OV2685_VTS_MAX - mode->height, 1,
+				mode->vts_def - mode->height);
+
+	exposure_max = mode->vts_def - 4;
+	ov2685->exposure = v4l2_ctrl_new_std(handler, &ov2685_ctrl_ops,
+				V4L2_CID_EXPOSURE, OV2685_EXPOSURE_MIN,
+				exposure_max, OV2685_EXPOSURE_STEP,
+				mode->exp_def);
+
+	ov2685->anal_gain = v4l2_ctrl_new_std(handler, &ov2685_ctrl_ops,
+				V4L2_CID_ANALOGUE_GAIN, OV2685_GAIN_MIN,
+				OV2685_GAIN_MAX, OV2685_GAIN_STEP,
+				OV2685_GAIN_DEFAULT);
+
+	ov2685->test_pattern = v4l2_ctrl_new_std_menu_items(handler,
+				&ov2685_ctrl_ops, V4L2_CID_TEST_PATTERN,
+				ARRAY_SIZE(ov2685_test_pattern_menu) - 1,
+				0, 0, ov2685_test_pattern_menu);
+
+	/* set properties from fwnode (e.g. rotation, orientation) */
+	ret = v4l2_fwnode_device_parse(&ov2685->client->dev, &props);
+	if (ret)
+		goto err_free_handler;
+
+	ret = v4l2_ctrl_new_fwnode_properties(handler, &ov2685_ctrl_ops, &props);
+	if (ret)
+		goto err_free_handler;
+
+	if (handler->error) {
+		ret = handler->error;
+		dev_err(&ov2685->client->dev,
+			"Failed to init controls(%d)\n", ret);
+		goto err_free_handler;
+	}
+
+	ov2685->subdev.ctrl_handler = handler;
+
+	return 0;
+
+err_free_handler:
+	v4l2_ctrl_handler_free(handler);
+
+	return ret;
+}
+
+static int ov2685_check_sensor_id(struct ov2685 *ov2685,
+				  struct i2c_client *client)
+{
+	struct device *dev = &ov2685->client->dev;
+	int ret;
+	u32 id = 0;
+
+	ret = ov2685_read_reg(client, OV2685_REG_CHIP_ID,
+			      OV2685_REG_VALUE_16BIT, &id);
+	if (id != CHIP_ID) {
+		dev_err(dev, "Unexpected sensor id(%04x), ret(%d)\n", id, ret);
+		return ret;
+	}
+
+	dev_info(dev, "Detected OV%04x sensor\n", CHIP_ID);
+
+	return 0;
+}
+
+static int ov2685_configure_regulators(struct ov2685 *ov2685)
+{
+	int i;
+
+	for (i = 0; i < OV2685_NUM_SUPPLIES; i++)
+		ov2685->supplies[i].supply = ov2685_supply_names[i];
+
+	return devm_regulator_bulk_get(&ov2685->client->dev,
+				       OV2685_NUM_SUPPLIES,
+				       ov2685->supplies);
+}
+
+static int ov2685_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct ov2685 *ov2685;
+	int ret;
+
+	ov2685 = devm_kzalloc(dev, sizeof(*ov2685), GFP_KERNEL);
+	if (!ov2685)
+		return -ENOMEM;
+
+	ov2685->client = client;
+	ov2685->cur_mode = &supported_modes[0];
+
+	ov2685->xvclk = devm_clk_get(dev, "xvclk");
+	if (IS_ERR(ov2685->xvclk)) {
+		dev_err(dev, "Failed to get xvclk\n");
+		return -EINVAL;
+	}
+	ret = clk_set_rate(ov2685->xvclk, OV2685_XVCLK_FREQ);
+	if (ret < 0) {
+		dev_err(dev, "Failed to set xvclk rate (24MHz)\n");
+		return ret;
+	}
+	if (clk_get_rate(ov2685->xvclk) != OV2685_XVCLK_FREQ)
+		dev_warn(dev, "xvclk mismatched, modes are based on 24MHz\n");
+
+	ov2685->reset_gpio = devm_gpiod_get_optional(dev, "reset", GPIOD_OUT_LOW);
+	if (IS_ERR(ov2685->reset_gpio)) {
+		dev_err(dev, "Failed to get reset-gpios\n");
+		return -EINVAL;
+	}
+
+	ret = ov2685_configure_regulators(ov2685);
+	if (ret) {
+		dev_err(dev, "Failed to get power regulators\n");
+		return ret;
+	}
+
+	mutex_init(&ov2685->mutex);
+	v4l2_i2c_subdev_init(&ov2685->subdev, client, &ov2685_subdev_ops);
+	ret = ov2685_initialize_controls(ov2685);
+	if (ret)
+		goto err_destroy_mutex;
+
+	ret = __ov2685_power_on(ov2685);
+	if (ret)
+		goto err_free_handler;
+
+	ret = ov2685_check_sensor_id(ov2685, client);
+	if (ret)
+		goto err_power_off;
+
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
+	ov2685->subdev.internal_ops = &ov2685_internal_ops;
+	ov2685->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+#endif
+#if defined(CONFIG_MEDIA_CONTROLLER)
+	ov2685->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ov2685->subdev.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret = media_entity_pads_init(&ov2685->subdev.entity, 1, &ov2685->pad);
+	if (ret < 0)
+		goto err_power_off;
+#endif
+
+	ret = v4l2_async_register_subdev(&ov2685->subdev);
+	if (ret) {
+		dev_err(dev, "v4l2 async register subdev failed\n");
+		goto err_clean_entity;
+	}
+
+	pm_runtime_set_active(dev);
+	pm_runtime_enable(dev);
+	pm_runtime_idle(dev);
+
+	return 0;
+
+err_clean_entity:
+#if defined(CONFIG_MEDIA_CONTROLLER)
+	media_entity_cleanup(&ov2685->subdev.entity);
+#endif
+err_power_off:
+	__ov2685_power_off(ov2685);
+err_free_handler:
+	v4l2_ctrl_handler_free(&ov2685->ctrl_handler);
+err_destroy_mutex:
+	mutex_destroy(&ov2685->mutex);
+
+	return ret;
+}
+
+static void ov2685_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov2685 *ov2685 = to_ov2685(sd);
+
+	v4l2_async_unregister_subdev(sd);
+#if defined(CONFIG_MEDIA_CONTROLLER)
+	media_entity_cleanup(&sd->entity);
+#endif
+	v4l2_ctrl_handler_free(&ov2685->ctrl_handler);
+	mutex_destroy(&ov2685->mutex);
+
+	pm_runtime_disable(&client->dev);
+	if (!pm_runtime_status_suspended(&client->dev))
+		__ov2685_power_off(ov2685);
+	pm_runtime_set_suspended(&client->dev);
+}
+
+#if IS_ENABLED(CONFIG_OF)
+static const struct of_device_id ov2685_of_match[] = {
+	{ .compatible = "ovti,ov2685" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, ov2685_of_match);
+#endif
+
+static struct i2c_driver ov2685_i2c_driver = {
+	.driver = {
+		.name = "ov2685",
+		.pm = &ov2685_pm_ops,
+		.of_match_table = of_match_ptr(ov2685_of_match),
+	},
+	.probe		= ov2685_probe,
+	.remove		= ov2685_remove,
+};
+
+module_i2c_driver(ov2685_i2c_driver);
+
+MODULE_DESCRIPTION("OmniVision ov2685 sensor driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/ov2740.c b/drivers/media/i2c/ov2740.c
new file mode 100644
index 000000000..41d4f8547
--- /dev/null
+++ b/drivers/media/i2c/ov2740.c
@@ -0,0 +1,1229 @@
+// SPDX-License-Identifier: GPL-2.0
+// Copyright (c) 2020 Intel Corporation.
+
+#include <asm/unaligned.h>
+#include <linux/acpi.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <linux/nvmem-provider.h>
+#include <linux/regmap.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+
+#define OV2740_LINK_FREQ_360MHZ		360000000ULL
+#define OV2740_SCLK			72000000LL
+#define OV2740_MCLK			19200000
+#define OV2740_DATA_LANES		2
+#define OV2740_RGB_DEPTH		10
+
+#define OV2740_REG_CHIP_ID		0x300a
+#define OV2740_CHIP_ID			0x2740
+
+#define OV2740_REG_MODE_SELECT		0x0100
+#define OV2740_MODE_STANDBY		0x00
+#define OV2740_MODE_STREAMING		0x01
+
+/* vertical-timings from sensor */
+#define OV2740_REG_VTS			0x380e
+#define OV2740_VTS_DEF			0x088a
+#define OV2740_VTS_MIN			0x0460
+#define OV2740_VTS_MAX			0x7fff
+
+/* horizontal-timings from sensor */
+#define OV2740_REG_HTS			0x380c
+
+/* Exposure controls from sensor */
+#define OV2740_REG_EXPOSURE		0x3500
+#define OV2740_EXPOSURE_MIN		4
+#define OV2740_EXPOSURE_MAX_MARGIN	8
+#define OV2740_EXPOSURE_STEP		1
+
+/* Analog gain controls from sensor */
+#define OV2740_REG_ANALOG_GAIN		0x3508
+#define OV2740_ANAL_GAIN_MIN		128
+#define OV2740_ANAL_GAIN_MAX		1983
+#define OV2740_ANAL_GAIN_STEP		1
+
+/* Digital gain controls from sensor */
+#define OV2740_REG_MWB_R_GAIN		0x500a
+#define OV2740_REG_MWB_G_GAIN		0x500c
+#define OV2740_REG_MWB_B_GAIN		0x500e
+#define OV2740_DGTL_GAIN_MIN		1024
+#define OV2740_DGTL_GAIN_MAX		4095
+#define OV2740_DGTL_GAIN_STEP		1
+#define OV2740_DGTL_GAIN_DEFAULT	1024
+
+/* Test Pattern Control */
+#define OV2740_REG_TEST_PATTERN		0x5040
+#define OV2740_TEST_PATTERN_ENABLE	BIT(7)
+#define OV2740_TEST_PATTERN_BAR_SHIFT	2
+
+/* Group Access */
+#define OV2740_REG_GROUP_ACCESS		0x3208
+#define OV2740_GROUP_HOLD_START		0x0
+#define OV2740_GROUP_HOLD_END		0x10
+#define OV2740_GROUP_HOLD_LAUNCH	0xa0
+
+/* ISP CTRL00 */
+#define OV2740_REG_ISP_CTRL00		0x5000
+/* ISP CTRL01 */
+#define OV2740_REG_ISP_CTRL01		0x5001
+/* Customer Addresses: 0x7010 - 0x710F */
+#define CUSTOMER_USE_OTP_SIZE		0x100
+/* OTP registers from sensor */
+#define OV2740_REG_OTP_CUSTOMER		0x7010
+
+struct nvm_data {
+	struct nvmem_device *nvmem;
+	struct regmap *regmap;
+	char *nvm_buffer;
+};
+
+enum {
+	OV2740_LINK_FREQ_360MHZ_INDEX,
+};
+
+struct ov2740_reg {
+	u16 address;
+	u8 val;
+};
+
+struct ov2740_reg_list {
+	u32 num_of_regs;
+	const struct ov2740_reg *regs;
+};
+
+struct ov2740_link_freq_config {
+	const struct ov2740_reg_list reg_list;
+};
+
+struct ov2740_mode {
+	/* Frame width in pixels */
+	u32 width;
+
+	/* Frame height in pixels */
+	u32 height;
+
+	/* Horizontal timining size */
+	u32 hts;
+
+	/* Default vertical timining size */
+	u32 vts_def;
+
+	/* Min vertical timining size */
+	u32 vts_min;
+
+	/* Link frequency needed for this resolution */
+	u32 link_freq_index;
+
+	/* Sensor register settings for this resolution */
+	const struct ov2740_reg_list reg_list;
+};
+
+static const struct ov2740_reg mipi_data_rate_720mbps[] = {
+	{0x0103, 0x01},
+	{0x0302, 0x4b},
+	{0x030d, 0x4b},
+	{0x030e, 0x02},
+	{0x030a, 0x01},
+	{0x0312, 0x11},
+};
+
+static const struct ov2740_reg mode_1932x1092_regs[] = {
+	{0x3000, 0x00},
+	{0x3018, 0x32},
+	{0x3031, 0x0a},
+	{0x3080, 0x08},
+	{0x3083, 0xB4},
+	{0x3103, 0x00},
+	{0x3104, 0x01},
+	{0x3106, 0x01},
+	{0x3500, 0x00},
+	{0x3501, 0x44},
+	{0x3502, 0x40},
+	{0x3503, 0x88},
+	{0x3507, 0x00},
+	{0x3508, 0x00},
+	{0x3509, 0x80},
+	{0x350c, 0x00},
+	{0x350d, 0x80},
+	{0x3510, 0x00},
+	{0x3511, 0x00},
+	{0x3512, 0x20},
+	{0x3632, 0x00},
+	{0x3633, 0x10},
+	{0x3634, 0x10},
+	{0x3635, 0x10},
+	{0x3645, 0x13},
+	{0x3646, 0x81},
+	{0x3636, 0x10},
+	{0x3651, 0x0a},
+	{0x3656, 0x02},
+	{0x3659, 0x04},
+	{0x365a, 0xda},
+	{0x365b, 0xa2},
+	{0x365c, 0x04},
+	{0x365d, 0x1d},
+	{0x365e, 0x1a},
+	{0x3662, 0xd7},
+	{0x3667, 0x78},
+	{0x3669, 0x0a},
+	{0x366a, 0x92},
+	{0x3700, 0x54},
+	{0x3702, 0x10},
+	{0x3706, 0x42},
+	{0x3709, 0x30},
+	{0x370b, 0xc2},
+	{0x3714, 0x63},
+	{0x3715, 0x01},
+	{0x3716, 0x00},
+	{0x371a, 0x3e},
+	{0x3732, 0x0e},
+	{0x3733, 0x10},
+	{0x375f, 0x0e},
+	{0x3768, 0x30},
+	{0x3769, 0x44},
+	{0x376a, 0x22},
+	{0x377b, 0x20},
+	{0x377c, 0x00},
+	{0x377d, 0x0c},
+	{0x3798, 0x00},
+	{0x37a1, 0x55},
+	{0x37a8, 0x6d},
+	{0x37c2, 0x04},
+	{0x37c5, 0x00},
+	{0x37c8, 0x00},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x00},
+	{0x3804, 0x07},
+	{0x3805, 0x8f},
+	{0x3806, 0x04},
+	{0x3807, 0x47},
+	{0x3808, 0x07},
+	{0x3809, 0x88},
+	{0x380a, 0x04},
+	{0x380b, 0x40},
+	{0x380c, 0x04},
+	{0x380d, 0x38},
+	{0x380e, 0x04},
+	{0x380f, 0x60},
+	{0x3810, 0x00},
+	{0x3811, 0x04},
+	{0x3812, 0x00},
+	{0x3813, 0x04},
+	{0x3814, 0x01},
+	{0x3815, 0x01},
+	{0x3820, 0x80},
+	{0x3821, 0x46},
+	{0x3822, 0x84},
+	{0x3829, 0x00},
+	{0x382a, 0x01},
+	{0x382b, 0x01},
+	{0x3830, 0x04},
+	{0x3836, 0x01},
+	{0x3837, 0x08},
+	{0x3839, 0x01},
+	{0x383a, 0x00},
+	{0x383b, 0x08},
+	{0x383c, 0x00},
+	{0x3f0b, 0x00},
+	{0x4001, 0x20},
+	{0x4009, 0x07},
+	{0x4003, 0x10},
+	{0x4010, 0xe0},
+	{0x4016, 0x00},
+	{0x4017, 0x10},
+	{0x4044, 0x02},
+	{0x4304, 0x08},
+	{0x4307, 0x30},
+	{0x4320, 0x80},
+	{0x4322, 0x00},
+	{0x4323, 0x00},
+	{0x4324, 0x00},
+	{0x4325, 0x00},
+	{0x4326, 0x00},
+	{0x4327, 0x00},
+	{0x4328, 0x00},
+	{0x4329, 0x00},
+	{0x432c, 0x03},
+	{0x432d, 0x81},
+	{0x4501, 0x84},
+	{0x4502, 0x40},
+	{0x4503, 0x18},
+	{0x4504, 0x04},
+	{0x4508, 0x02},
+	{0x4601, 0x10},
+	{0x4800, 0x00},
+	{0x4816, 0x52},
+	{0x4837, 0x16},
+	{0x5000, 0x7f},
+	{0x5001, 0x00},
+	{0x5005, 0x38},
+	{0x501e, 0x0d},
+	{0x5040, 0x00},
+	{0x5901, 0x00},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x00},
+	{0x3804, 0x07},
+	{0x3805, 0x8f},
+	{0x3806, 0x04},
+	{0x3807, 0x47},
+	{0x3808, 0x07},
+	{0x3809, 0x8c},
+	{0x380a, 0x04},
+	{0x380b, 0x44},
+	{0x3810, 0x00},
+	{0x3811, 0x00},
+	{0x3812, 0x00},
+	{0x3813, 0x01},
+};
+
+static const char * const ov2740_test_pattern_menu[] = {
+	"Disabled",
+	"Color Bar",
+	"Top-Bottom Darker Color Bar",
+	"Right-Left Darker Color Bar",
+	"Bottom-Top Darker Color Bar",
+};
+
+static const s64 link_freq_menu_items[] = {
+	OV2740_LINK_FREQ_360MHZ,
+};
+
+static const struct ov2740_link_freq_config link_freq_configs[] = {
+	[OV2740_LINK_FREQ_360MHZ_INDEX] = {
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mipi_data_rate_720mbps),
+			.regs = mipi_data_rate_720mbps,
+		}
+	},
+};
+
+static const struct ov2740_mode supported_modes[] = {
+	{
+		.width = 1932,
+		.height = 1092,
+		.hts = 1080,
+		.vts_def = OV2740_VTS_DEF,
+		.vts_min = OV2740_VTS_MIN,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1932x1092_regs),
+			.regs = mode_1932x1092_regs,
+		},
+		.link_freq_index = OV2740_LINK_FREQ_360MHZ_INDEX,
+	},
+};
+
+struct ov2740 {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct v4l2_ctrl_handler ctrl_handler;
+
+	/* V4L2 Controls */
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *pixel_rate;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *exposure;
+
+	/* Current mode */
+	const struct ov2740_mode *cur_mode;
+
+	/* To serialize asynchronus callbacks */
+	struct mutex mutex;
+
+	/* Streaming on/off */
+	bool streaming;
+
+	/* NVM data inforamtion */
+	struct nvm_data *nvm;
+
+	/* True if the device has been identified */
+	bool identified;
+};
+
+static inline struct ov2740 *to_ov2740(struct v4l2_subdev *subdev)
+{
+	return container_of(subdev, struct ov2740, sd);
+}
+
+static u64 to_pixel_rate(u32 f_index)
+{
+	u64 pixel_rate = link_freq_menu_items[f_index] * 2 * OV2740_DATA_LANES;
+
+	do_div(pixel_rate, OV2740_RGB_DEPTH);
+
+	return pixel_rate;
+}
+
+static u64 to_pixels_per_line(u32 hts, u32 f_index)
+{
+	u64 ppl = hts * to_pixel_rate(f_index);
+
+	do_div(ppl, OV2740_SCLK);
+
+	return ppl;
+}
+
+static int ov2740_read_reg(struct ov2740 *ov2740, u16 reg, u16 len, u32 *val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov2740->sd);
+	struct i2c_msg msgs[2];
+	u8 addr_buf[2];
+	u8 data_buf[4] = {0};
+	int ret;
+
+	if (len > sizeof(data_buf))
+		return -EINVAL;
+
+	put_unaligned_be16(reg, addr_buf);
+	msgs[0].addr = client->addr;
+	msgs[0].flags = 0;
+	msgs[0].len = sizeof(addr_buf);
+	msgs[0].buf = addr_buf;
+	msgs[1].addr = client->addr;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].len = len;
+	msgs[1].buf = &data_buf[sizeof(data_buf) - len];
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret != ARRAY_SIZE(msgs))
+		return ret < 0 ? ret : -EIO;
+
+	*val = get_unaligned_be32(data_buf);
+
+	return 0;
+}
+
+static int ov2740_write_reg(struct ov2740 *ov2740, u16 reg, u16 len, u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov2740->sd);
+	u8 buf[6];
+	int ret;
+
+	if (len > 4)
+		return -EINVAL;
+
+	put_unaligned_be16(reg, buf);
+	put_unaligned_be32(val << 8 * (4 - len), buf + 2);
+
+	ret = i2c_master_send(client, buf, len + 2);
+	if (ret != len + 2)
+		return ret < 0 ? ret : -EIO;
+
+	return 0;
+}
+
+static int ov2740_write_reg_list(struct ov2740 *ov2740,
+				 const struct ov2740_reg_list *r_list)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov2740->sd);
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < r_list->num_of_regs; i++) {
+		ret = ov2740_write_reg(ov2740, r_list->regs[i].address, 1,
+				       r_list->regs[i].val);
+		if (ret) {
+			dev_err_ratelimited(&client->dev,
+					    "write reg 0x%4.4x return err = %d\n",
+					    r_list->regs[i].address, ret);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int ov2740_identify_module(struct ov2740 *ov2740)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov2740->sd);
+	int ret;
+	u32 val;
+
+	if (ov2740->identified)
+		return 0;
+
+	ret = ov2740_read_reg(ov2740, OV2740_REG_CHIP_ID, 3, &val);
+	if (ret)
+		return ret;
+
+	if (val != OV2740_CHIP_ID) {
+		dev_err(&client->dev, "chip id mismatch: %x != %x\n",
+			OV2740_CHIP_ID, val);
+		return -ENXIO;
+	}
+
+	ov2740->identified = true;
+
+	return 0;
+}
+
+static int ov2740_update_digital_gain(struct ov2740 *ov2740, u32 d_gain)
+{
+	int ret;
+
+	ret = ov2740_write_reg(ov2740, OV2740_REG_GROUP_ACCESS, 1,
+			       OV2740_GROUP_HOLD_START);
+	if (ret)
+		return ret;
+
+	ret = ov2740_write_reg(ov2740, OV2740_REG_MWB_R_GAIN, 2, d_gain);
+	if (ret)
+		return ret;
+
+	ret = ov2740_write_reg(ov2740, OV2740_REG_MWB_G_GAIN, 2, d_gain);
+	if (ret)
+		return ret;
+
+	ret = ov2740_write_reg(ov2740, OV2740_REG_MWB_B_GAIN, 2, d_gain);
+	if (ret)
+		return ret;
+
+	ret = ov2740_write_reg(ov2740, OV2740_REG_GROUP_ACCESS, 1,
+			       OV2740_GROUP_HOLD_END);
+	if (ret)
+		return ret;
+
+	ret = ov2740_write_reg(ov2740, OV2740_REG_GROUP_ACCESS, 1,
+			       OV2740_GROUP_HOLD_LAUNCH);
+	return ret;
+}
+
+static int ov2740_test_pattern(struct ov2740 *ov2740, u32 pattern)
+{
+	if (pattern)
+		pattern = (pattern - 1) << OV2740_TEST_PATTERN_BAR_SHIFT |
+			  OV2740_TEST_PATTERN_ENABLE;
+
+	return ov2740_write_reg(ov2740, OV2740_REG_TEST_PATTERN, 1, pattern);
+}
+
+static int ov2740_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov2740 *ov2740 = container_of(ctrl->handler,
+					     struct ov2740, ctrl_handler);
+	struct i2c_client *client = v4l2_get_subdevdata(&ov2740->sd);
+	s64 exposure_max;
+	int ret;
+
+	/* Propagate change of current control to all related controls */
+	if (ctrl->id == V4L2_CID_VBLANK) {
+		/* Update max exposure while meeting expected vblanking */
+		exposure_max = ov2740->cur_mode->height + ctrl->val -
+			       OV2740_EXPOSURE_MAX_MARGIN;
+		__v4l2_ctrl_modify_range(ov2740->exposure,
+					 ov2740->exposure->minimum,
+					 exposure_max, ov2740->exposure->step,
+					 exposure_max);
+	}
+
+	/* V4L2 controls values will be applied only when power is already up */
+	if (!pm_runtime_get_if_in_use(&client->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_ANALOGUE_GAIN:
+		ret = ov2740_write_reg(ov2740, OV2740_REG_ANALOG_GAIN, 2,
+				       ctrl->val);
+		break;
+
+	case V4L2_CID_DIGITAL_GAIN:
+		ret = ov2740_update_digital_gain(ov2740, ctrl->val);
+		break;
+
+	case V4L2_CID_EXPOSURE:
+		/* 4 least significant bits of expsoure are fractional part */
+		ret = ov2740_write_reg(ov2740, OV2740_REG_EXPOSURE, 3,
+				       ctrl->val << 4);
+		break;
+
+	case V4L2_CID_VBLANK:
+		ret = ov2740_write_reg(ov2740, OV2740_REG_VTS, 2,
+				       ov2740->cur_mode->height + ctrl->val);
+		break;
+
+	case V4L2_CID_TEST_PATTERN:
+		ret = ov2740_test_pattern(ov2740, ctrl->val);
+		break;
+
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	pm_runtime_put(&client->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ov2740_ctrl_ops = {
+	.s_ctrl = ov2740_set_ctrl,
+};
+
+static int ov2740_init_controls(struct ov2740 *ov2740)
+{
+	struct v4l2_ctrl_handler *ctrl_hdlr;
+	const struct ov2740_mode *cur_mode;
+	s64 exposure_max, h_blank, pixel_rate;
+	u32 vblank_min, vblank_max, vblank_default;
+	int size;
+	int ret;
+
+	ctrl_hdlr = &ov2740->ctrl_handler;
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 8);
+	if (ret)
+		return ret;
+
+	ctrl_hdlr->lock = &ov2740->mutex;
+	cur_mode = ov2740->cur_mode;
+	size = ARRAY_SIZE(link_freq_menu_items);
+
+	ov2740->link_freq = v4l2_ctrl_new_int_menu(ctrl_hdlr, &ov2740_ctrl_ops,
+						   V4L2_CID_LINK_FREQ,
+						   size - 1, 0,
+						   link_freq_menu_items);
+	if (ov2740->link_freq)
+		ov2740->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	pixel_rate = to_pixel_rate(OV2740_LINK_FREQ_360MHZ_INDEX);
+	ov2740->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, &ov2740_ctrl_ops,
+					       V4L2_CID_PIXEL_RATE, 0,
+					       pixel_rate, 1, pixel_rate);
+
+	vblank_min = cur_mode->vts_min - cur_mode->height;
+	vblank_max = OV2740_VTS_MAX - cur_mode->height;
+	vblank_default = cur_mode->vts_def - cur_mode->height;
+	ov2740->vblank = v4l2_ctrl_new_std(ctrl_hdlr, &ov2740_ctrl_ops,
+					   V4L2_CID_VBLANK, vblank_min,
+					   vblank_max, 1, vblank_default);
+
+	h_blank = to_pixels_per_line(cur_mode->hts, cur_mode->link_freq_index);
+	h_blank -= cur_mode->width;
+	ov2740->hblank = v4l2_ctrl_new_std(ctrl_hdlr, &ov2740_ctrl_ops,
+					   V4L2_CID_HBLANK, h_blank, h_blank, 1,
+					   h_blank);
+	if (ov2740->hblank)
+		ov2740->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov2740_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
+			  OV2740_ANAL_GAIN_MIN, OV2740_ANAL_GAIN_MAX,
+			  OV2740_ANAL_GAIN_STEP, OV2740_ANAL_GAIN_MIN);
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov2740_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
+			  OV2740_DGTL_GAIN_MIN, OV2740_DGTL_GAIN_MAX,
+			  OV2740_DGTL_GAIN_STEP, OV2740_DGTL_GAIN_DEFAULT);
+	exposure_max = cur_mode->vts_def - OV2740_EXPOSURE_MAX_MARGIN;
+	ov2740->exposure = v4l2_ctrl_new_std(ctrl_hdlr, &ov2740_ctrl_ops,
+					     V4L2_CID_EXPOSURE,
+					     OV2740_EXPOSURE_MIN, exposure_max,
+					     OV2740_EXPOSURE_STEP,
+					     exposure_max);
+	v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &ov2740_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(ov2740_test_pattern_menu) - 1,
+				     0, 0, ov2740_test_pattern_menu);
+	if (ctrl_hdlr->error) {
+		v4l2_ctrl_handler_free(ctrl_hdlr);
+		return ctrl_hdlr->error;
+	}
+
+	ov2740->sd.ctrl_handler = ctrl_hdlr;
+
+	return 0;
+}
+
+static void ov2740_update_pad_format(const struct ov2740_mode *mode,
+				     struct v4l2_mbus_framefmt *fmt)
+{
+	fmt->width = mode->width;
+	fmt->height = mode->height;
+	fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	fmt->field = V4L2_FIELD_NONE;
+}
+
+static int ov2740_load_otp_data(struct nvm_data *nvm)
+{
+	struct device *dev = regmap_get_device(nvm->regmap);
+	struct ov2740 *ov2740 = to_ov2740(dev_get_drvdata(dev));
+	u32 isp_ctrl00 = 0;
+	u32 isp_ctrl01 = 0;
+	int ret;
+
+	if (nvm->nvm_buffer)
+		return 0;
+
+	nvm->nvm_buffer = kzalloc(CUSTOMER_USE_OTP_SIZE, GFP_KERNEL);
+	if (!nvm->nvm_buffer)
+		return -ENOMEM;
+
+	ret = ov2740_read_reg(ov2740, OV2740_REG_ISP_CTRL00, 1, &isp_ctrl00);
+	if (ret) {
+		dev_err(dev, "failed to read ISP CTRL00\n");
+		goto err;
+	}
+
+	ret = ov2740_read_reg(ov2740, OV2740_REG_ISP_CTRL01, 1, &isp_ctrl01);
+	if (ret) {
+		dev_err(dev, "failed to read ISP CTRL01\n");
+		goto err;
+	}
+
+	/* Clear bit 5 of ISP CTRL00 */
+	ret = ov2740_write_reg(ov2740, OV2740_REG_ISP_CTRL00, 1,
+			       isp_ctrl00 & ~BIT(5));
+	if (ret) {
+		dev_err(dev, "failed to set ISP CTRL00\n");
+		goto err;
+	}
+
+	/* Clear bit 7 of ISP CTRL01 */
+	ret = ov2740_write_reg(ov2740, OV2740_REG_ISP_CTRL01, 1,
+			       isp_ctrl01 & ~BIT(7));
+	if (ret) {
+		dev_err(dev, "failed to set ISP CTRL01\n");
+		goto err;
+	}
+
+	ret = ov2740_write_reg(ov2740, OV2740_REG_MODE_SELECT, 1,
+			       OV2740_MODE_STREAMING);
+	if (ret) {
+		dev_err(dev, "failed to set streaming mode\n");
+		goto err;
+	}
+
+	/*
+	 * Users are not allowed to access OTP-related registers and memory
+	 * during the 20 ms period after streaming starts (0x100 = 0x01).
+	 */
+	msleep(20);
+
+	ret = regmap_bulk_read(nvm->regmap, OV2740_REG_OTP_CUSTOMER,
+			       nvm->nvm_buffer, CUSTOMER_USE_OTP_SIZE);
+	if (ret) {
+		dev_err(dev, "failed to read OTP data, ret %d\n", ret);
+		goto err;
+	}
+
+	ret = ov2740_write_reg(ov2740, OV2740_REG_MODE_SELECT, 1,
+			       OV2740_MODE_STANDBY);
+	if (ret) {
+		dev_err(dev, "failed to set streaming mode\n");
+		goto err;
+	}
+
+	ret = ov2740_write_reg(ov2740, OV2740_REG_ISP_CTRL01, 1, isp_ctrl01);
+	if (ret) {
+		dev_err(dev, "failed to set ISP CTRL01\n");
+		goto err;
+	}
+
+	ret = ov2740_write_reg(ov2740, OV2740_REG_ISP_CTRL00, 1, isp_ctrl00);
+	if (ret) {
+		dev_err(dev, "failed to set ISP CTRL00\n");
+		goto err;
+	}
+
+	return 0;
+err:
+	kfree(nvm->nvm_buffer);
+	nvm->nvm_buffer = NULL;
+
+	return ret;
+}
+
+static int ov2740_start_streaming(struct ov2740 *ov2740)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov2740->sd);
+	const struct ov2740_reg_list *reg_list;
+	int link_freq_index;
+	int ret;
+
+	ret = ov2740_identify_module(ov2740);
+	if (ret)
+		return ret;
+
+	if (ov2740->nvm)
+		ov2740_load_otp_data(ov2740->nvm);
+
+	link_freq_index = ov2740->cur_mode->link_freq_index;
+	reg_list = &link_freq_configs[link_freq_index].reg_list;
+	ret = ov2740_write_reg_list(ov2740, reg_list);
+	if (ret) {
+		dev_err(&client->dev, "failed to set plls\n");
+		return ret;
+	}
+
+	reg_list = &ov2740->cur_mode->reg_list;
+	ret = ov2740_write_reg_list(ov2740, reg_list);
+	if (ret) {
+		dev_err(&client->dev, "failed to set mode\n");
+		return ret;
+	}
+
+	ret = __v4l2_ctrl_handler_setup(ov2740->sd.ctrl_handler);
+	if (ret)
+		return ret;
+
+	ret = ov2740_write_reg(ov2740, OV2740_REG_MODE_SELECT, 1,
+			       OV2740_MODE_STREAMING);
+	if (ret)
+		dev_err(&client->dev, "failed to start streaming\n");
+
+	return ret;
+}
+
+static void ov2740_stop_streaming(struct ov2740 *ov2740)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov2740->sd);
+
+	if (ov2740_write_reg(ov2740, OV2740_REG_MODE_SELECT, 1,
+			     OV2740_MODE_STANDBY))
+		dev_err(&client->dev, "failed to stop streaming\n");
+}
+
+static int ov2740_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct ov2740 *ov2740 = to_ov2740(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	if (ov2740->streaming == enable)
+		return 0;
+
+	mutex_lock(&ov2740->mutex);
+	if (enable) {
+		ret = pm_runtime_resume_and_get(&client->dev);
+		if (ret < 0) {
+			mutex_unlock(&ov2740->mutex);
+			return ret;
+		}
+
+		ret = ov2740_start_streaming(ov2740);
+		if (ret) {
+			enable = 0;
+			ov2740_stop_streaming(ov2740);
+			pm_runtime_put(&client->dev);
+		}
+	} else {
+		ov2740_stop_streaming(ov2740);
+		pm_runtime_put(&client->dev);
+	}
+
+	ov2740->streaming = enable;
+	mutex_unlock(&ov2740->mutex);
+
+	return ret;
+}
+
+static int ov2740_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov2740 *ov2740 = to_ov2740(sd);
+
+	mutex_lock(&ov2740->mutex);
+	if (ov2740->streaming)
+		ov2740_stop_streaming(ov2740);
+
+	mutex_unlock(&ov2740->mutex);
+
+	return 0;
+}
+
+static int ov2740_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov2740 *ov2740 = to_ov2740(sd);
+	int ret = 0;
+
+	mutex_lock(&ov2740->mutex);
+	if (!ov2740->streaming)
+		goto exit;
+
+	ret = ov2740_start_streaming(ov2740);
+	if (ret) {
+		ov2740->streaming = false;
+		ov2740_stop_streaming(ov2740);
+	}
+
+exit:
+	mutex_unlock(&ov2740->mutex);
+	return ret;
+}
+
+static int ov2740_set_format(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *sd_state,
+			     struct v4l2_subdev_format *fmt)
+{
+	struct ov2740 *ov2740 = to_ov2740(sd);
+	const struct ov2740_mode *mode;
+	s32 vblank_def, h_blank;
+
+	mode = v4l2_find_nearest_size(supported_modes,
+				      ARRAY_SIZE(supported_modes), width,
+				      height, fmt->format.width,
+				      fmt->format.height);
+
+	mutex_lock(&ov2740->mutex);
+	ov2740_update_pad_format(mode, &fmt->format);
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		*v4l2_subdev_get_try_format(sd, sd_state, fmt->pad) = fmt->format;
+	} else {
+		ov2740->cur_mode = mode;
+		__v4l2_ctrl_s_ctrl(ov2740->link_freq, mode->link_freq_index);
+		__v4l2_ctrl_s_ctrl_int64(ov2740->pixel_rate,
+					 to_pixel_rate(mode->link_freq_index));
+
+		/* Update limits and set FPS to default */
+		vblank_def = mode->vts_def - mode->height;
+		__v4l2_ctrl_modify_range(ov2740->vblank,
+					 mode->vts_min - mode->height,
+					 OV2740_VTS_MAX - mode->height, 1,
+					 vblank_def);
+		__v4l2_ctrl_s_ctrl(ov2740->vblank, vblank_def);
+		h_blank = to_pixels_per_line(mode->hts, mode->link_freq_index) -
+			  mode->width;
+		__v4l2_ctrl_modify_range(ov2740->hblank, h_blank, h_blank, 1,
+					 h_blank);
+	}
+	mutex_unlock(&ov2740->mutex);
+
+	return 0;
+}
+
+static int ov2740_get_format(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *sd_state,
+			     struct v4l2_subdev_format *fmt)
+{
+	struct ov2740 *ov2740 = to_ov2740(sd);
+
+	mutex_lock(&ov2740->mutex);
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
+		fmt->format = *v4l2_subdev_get_try_format(&ov2740->sd,
+							  sd_state,
+							  fmt->pad);
+	else
+		ov2740_update_pad_format(ov2740->cur_mode, &fmt->format);
+
+	mutex_unlock(&ov2740->mutex);
+
+	return 0;
+}
+
+static int ov2740_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index > 0)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+
+	return 0;
+}
+
+static int ov2740_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->index >= ARRAY_SIZE(supported_modes))
+		return -EINVAL;
+
+	if (fse->code != MEDIA_BUS_FMT_SGRBG10_1X10)
+		return -EINVAL;
+
+	fse->min_width = supported_modes[fse->index].width;
+	fse->max_width = fse->min_width;
+	fse->min_height = supported_modes[fse->index].height;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static int ov2740_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct ov2740 *ov2740 = to_ov2740(sd);
+
+	mutex_lock(&ov2740->mutex);
+	ov2740_update_pad_format(&supported_modes[0],
+				 v4l2_subdev_get_try_format(sd, fh->state, 0));
+	mutex_unlock(&ov2740->mutex);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops ov2740_video_ops = {
+	.s_stream = ov2740_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops ov2740_pad_ops = {
+	.set_fmt = ov2740_set_format,
+	.get_fmt = ov2740_get_format,
+	.enum_mbus_code = ov2740_enum_mbus_code,
+	.enum_frame_size = ov2740_enum_frame_size,
+};
+
+static const struct v4l2_subdev_ops ov2740_subdev_ops = {
+	.video = &ov2740_video_ops,
+	.pad = &ov2740_pad_ops,
+};
+
+static const struct media_entity_operations ov2740_subdev_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static const struct v4l2_subdev_internal_ops ov2740_internal_ops = {
+	.open = ov2740_open,
+};
+
+static int ov2740_check_hwcfg(struct device *dev)
+{
+	struct fwnode_handle *ep;
+	struct fwnode_handle *fwnode = dev_fwnode(dev);
+	struct v4l2_fwnode_endpoint bus_cfg = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY
+	};
+	u32 mclk;
+	int ret;
+	unsigned int i, j;
+
+	ret = fwnode_property_read_u32(fwnode, "clock-frequency", &mclk);
+	if (ret)
+		return ret;
+
+	if (mclk != OV2740_MCLK)
+		return dev_err_probe(dev, -EINVAL,
+				     "external clock %d is not supported\n",
+				     mclk);
+
+	ep = fwnode_graph_get_next_endpoint(fwnode, NULL);
+	if (!ep)
+		return -ENXIO;
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(ep, &bus_cfg);
+	fwnode_handle_put(ep);
+	if (ret)
+		return ret;
+
+	if (bus_cfg.bus.mipi_csi2.num_data_lanes != OV2740_DATA_LANES) {
+		ret = dev_err_probe(dev, -EINVAL,
+				    "number of CSI2 data lanes %d is not supported\n",
+				    bus_cfg.bus.mipi_csi2.num_data_lanes);
+		goto check_hwcfg_error;
+	}
+
+	if (!bus_cfg.nr_of_link_frequencies) {
+		ret = dev_err_probe(dev, -EINVAL, "no link frequencies defined\n");
+		goto check_hwcfg_error;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(link_freq_menu_items); i++) {
+		for (j = 0; j < bus_cfg.nr_of_link_frequencies; j++) {
+			if (link_freq_menu_items[i] ==
+				bus_cfg.link_frequencies[j])
+				break;
+		}
+
+		if (j == bus_cfg.nr_of_link_frequencies) {
+			ret = dev_err_probe(dev, -EINVAL,
+					    "no link frequency %lld supported\n",
+					    link_freq_menu_items[i]);
+			goto check_hwcfg_error;
+		}
+	}
+
+check_hwcfg_error:
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+
+	return ret;
+}
+
+static void ov2740_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov2740 *ov2740 = to_ov2740(sd);
+
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+	pm_runtime_disable(&client->dev);
+	mutex_destroy(&ov2740->mutex);
+}
+
+static int ov2740_nvmem_read(void *priv, unsigned int off, void *val,
+			     size_t count)
+{
+	struct nvm_data *nvm = priv;
+	struct device *dev = regmap_get_device(nvm->regmap);
+	struct ov2740 *ov2740 = to_ov2740(dev_get_drvdata(dev));
+	int ret = 0;
+
+	mutex_lock(&ov2740->mutex);
+
+	if (nvm->nvm_buffer) {
+		memcpy(val, nvm->nvm_buffer + off, count);
+		goto exit;
+	}
+
+	ret = pm_runtime_resume_and_get(dev);
+	if (ret < 0) {
+		goto exit;
+	}
+
+	ret = ov2740_load_otp_data(nvm);
+	if (!ret)
+		memcpy(val, nvm->nvm_buffer + off, count);
+
+	pm_runtime_put(dev);
+exit:
+	mutex_unlock(&ov2740->mutex);
+	return ret;
+}
+
+static int ov2740_register_nvmem(struct i2c_client *client,
+				 struct ov2740 *ov2740)
+{
+	struct nvm_data *nvm;
+	struct regmap_config regmap_config = { };
+	struct nvmem_config nvmem_config = { };
+	struct regmap *regmap;
+	struct device *dev = &client->dev;
+
+	nvm = devm_kzalloc(dev, sizeof(*nvm), GFP_KERNEL);
+	if (!nvm)
+		return -ENOMEM;
+
+	regmap_config.val_bits = 8;
+	regmap_config.reg_bits = 16;
+	regmap_config.disable_locking = true;
+	regmap = devm_regmap_init_i2c(client, &regmap_config);
+	if (IS_ERR(regmap))
+		return PTR_ERR(regmap);
+
+	nvm->regmap = regmap;
+
+	nvmem_config.name = dev_name(dev);
+	nvmem_config.dev = dev;
+	nvmem_config.read_only = true;
+	nvmem_config.root_only = true;
+	nvmem_config.owner = THIS_MODULE;
+	nvmem_config.compat = true;
+	nvmem_config.base_dev = dev;
+	nvmem_config.reg_read = ov2740_nvmem_read;
+	nvmem_config.reg_write = NULL;
+	nvmem_config.priv = nvm;
+	nvmem_config.stride = 1;
+	nvmem_config.word_size = 1;
+	nvmem_config.size = CUSTOMER_USE_OTP_SIZE;
+
+	nvm->nvmem = devm_nvmem_register(dev, &nvmem_config);
+	if (IS_ERR(nvm->nvmem))
+		return PTR_ERR(nvm->nvmem);
+
+	ov2740->nvm = nvm;
+	return 0;
+}
+
+static int ov2740_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct ov2740 *ov2740;
+	bool full_power;
+	int ret;
+
+	ret = ov2740_check_hwcfg(&client->dev);
+	if (ret)
+		return dev_err_probe(dev, ret, "failed to check HW configuration\n");
+
+	ov2740 = devm_kzalloc(&client->dev, sizeof(*ov2740), GFP_KERNEL);
+	if (!ov2740)
+		return -ENOMEM;
+
+	v4l2_i2c_subdev_init(&ov2740->sd, client, &ov2740_subdev_ops);
+	full_power = acpi_dev_state_d0(&client->dev);
+	if (full_power) {
+		ret = ov2740_identify_module(ov2740);
+		if (ret)
+			return dev_err_probe(dev, ret, "failed to find sensor\n");
+	}
+
+	mutex_init(&ov2740->mutex);
+	ov2740->cur_mode = &supported_modes[0];
+	ret = ov2740_init_controls(ov2740);
+	if (ret) {
+		dev_err_probe(dev, ret, "failed to init controls\n");
+		goto probe_error_v4l2_ctrl_handler_free;
+	}
+
+	ov2740->sd.internal_ops = &ov2740_internal_ops;
+	ov2740->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	ov2740->sd.entity.ops = &ov2740_subdev_entity_ops;
+	ov2740->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ov2740->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&ov2740->sd.entity, 1, &ov2740->pad);
+	if (ret) {
+		dev_err_probe(dev, ret, "failed to init entity pads\n");
+		goto probe_error_v4l2_ctrl_handler_free;
+	}
+
+	ret = v4l2_async_register_subdev_sensor(&ov2740->sd);
+	if (ret < 0) {
+		dev_err_probe(dev, ret, "failed to register V4L2 subdev\n");
+		goto probe_error_media_entity_cleanup;
+	}
+
+	ret = ov2740_register_nvmem(client, ov2740);
+	if (ret)
+		dev_warn(&client->dev, "register nvmem failed, ret %d\n", ret);
+
+	/* Set the device's state to active if it's in D0 state. */
+	if (full_power)
+		pm_runtime_set_active(&client->dev);
+	pm_runtime_enable(&client->dev);
+	pm_runtime_idle(&client->dev);
+
+	return 0;
+
+probe_error_media_entity_cleanup:
+	media_entity_cleanup(&ov2740->sd.entity);
+
+probe_error_v4l2_ctrl_handler_free:
+	v4l2_ctrl_handler_free(ov2740->sd.ctrl_handler);
+	mutex_destroy(&ov2740->mutex);
+
+	return ret;
+}
+
+static DEFINE_SIMPLE_DEV_PM_OPS(ov2740_pm_ops, ov2740_suspend, ov2740_resume);
+
+static const struct acpi_device_id ov2740_acpi_ids[] = {
+	{"INT3474"},
+	{}
+};
+
+MODULE_DEVICE_TABLE(acpi, ov2740_acpi_ids);
+
+static struct i2c_driver ov2740_i2c_driver = {
+	.driver = {
+		.name = "ov2740",
+		.pm = pm_sleep_ptr(&ov2740_pm_ops),
+		.acpi_match_table = ov2740_acpi_ids,
+	},
+	.probe = ov2740_probe,
+	.remove = ov2740_remove,
+	.flags = I2C_DRV_ACPI_WAIVE_D0_PROBE,
+};
+
+module_i2c_driver(ov2740_i2c_driver);
+
+MODULE_AUTHOR("Qiu, Tianshu <tian.shu.qiu@intel.com>");
+MODULE_AUTHOR("Shawn Tu");
+MODULE_AUTHOR("Bingbu Cao <bingbu.cao@intel.com>");
+MODULE_DESCRIPTION("OmniVision OV2740 sensor driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/ov4689.c b/drivers/media/i2c/ov4689.c
new file mode 100644
index 000000000..fda217d2c
--- /dev/null
+++ b/drivers/media/i2c/ov4689.c
@@ -0,0 +1,1018 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * ov4689 driver
+ *
+ * Copyright (C) 2017 Fuzhou Rockchip Electronics Co., Ltd.
+ * Copyright (C) 2022 Mikhail Rudenko
+ */
+
+#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/media-entity.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-fwnode.h>
+
+#define CHIP_ID				0x004688
+#define OV4689_REG_CHIP_ID		0x300a
+
+#define OV4689_XVCLK_FREQ		24000000
+
+#define OV4689_REG_CTRL_MODE		0x0100
+#define OV4689_MODE_SW_STANDBY		0x0
+#define OV4689_MODE_STREAMING		BIT(0)
+
+#define OV4689_REG_EXPOSURE		0x3500
+#define OV4689_EXPOSURE_MIN		4
+#define OV4689_EXPOSURE_STEP		1
+#define OV4689_VTS_MAX			0x7fff
+
+#define OV4689_REG_GAIN_H		0x3508
+#define OV4689_REG_GAIN_L		0x3509
+#define OV4689_GAIN_H_MASK		0x07
+#define OV4689_GAIN_H_SHIFT		8
+#define OV4689_GAIN_L_MASK		0xff
+#define OV4689_GAIN_STEP		1
+#define OV4689_GAIN_DEFAULT		0x80
+
+#define OV4689_REG_TEST_PATTERN		0x5040
+#define OV4689_TEST_PATTERN_ENABLE	0x80
+#define OV4689_TEST_PATTERN_DISABLE	0x0
+
+#define OV4689_REG_VTS			0x380e
+
+#define REG_NULL			0xFFFF
+
+#define OV4689_REG_VALUE_08BIT		1
+#define OV4689_REG_VALUE_16BIT		2
+#define OV4689_REG_VALUE_24BIT		3
+
+#define OV4689_LANES			4
+
+static const char *const ov4689_supply_names[] = {
+	"avdd", /* Analog power */
+	"dovdd", /* Digital I/O power */
+	"dvdd", /* Digital core power */
+};
+
+struct regval {
+	u16 addr;
+	u8 val;
+};
+
+enum ov4689_mode_id {
+	OV4689_MODE_2688_1520 = 0,
+	OV4689_NUM_MODES,
+};
+
+struct ov4689_mode {
+	enum ov4689_mode_id id;
+	u32 width;
+	u32 height;
+	u32 max_fps;
+	u32 hts_def;
+	u32 vts_def;
+	u32 exp_def;
+	u32 pixel_rate;
+	u32 sensor_width;
+	u32 sensor_height;
+	u32 crop_top;
+	u32 crop_left;
+	const struct regval *reg_list;
+};
+
+struct ov4689 {
+	struct i2c_client *client;
+	struct clk *xvclk;
+	struct gpio_desc *reset_gpio;
+	struct gpio_desc *pwdn_gpio;
+	struct regulator_bulk_data supplies[ARRAY_SIZE(ov4689_supply_names)];
+
+	struct v4l2_subdev subdev;
+	struct media_pad pad;
+
+	u32 clock_rate;
+
+	struct mutex mutex; /* lock to protect streaming, ctrls and cur_mode */
+	bool streaming;
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_ctrl *exposure;
+
+	const struct ov4689_mode *cur_mode;
+};
+
+#define to_ov4689(sd) container_of(sd, struct ov4689, subdev)
+
+struct ov4689_gain_range {
+	u32 logical_min;
+	u32 logical_max;
+	u32 offset;
+	u32 divider;
+	u32 physical_min;
+	u32 physical_max;
+};
+
+/*
+ * Xclk 24Mhz
+ * max_framerate 30fps
+ * mipi_datarate per lane 1008Mbps
+ */
+static const struct regval ov4689_2688x1520_regs[] = {
+	{0x0103, 0x01}, {0x3638, 0x00}, {0x0300, 0x00},
+	{0x0302, 0x2a}, {0x0303, 0x00}, {0x0304, 0x03},
+	{0x030b, 0x00}, {0x030d, 0x1e}, {0x030e, 0x04},
+	{0x030f, 0x01}, {0x0312, 0x01}, {0x031e, 0x00},
+	{0x3000, 0x20}, {0x3002, 0x00}, {0x3018, 0x72},
+	{0x3020, 0x93}, {0x3021, 0x03}, {0x3022, 0x01},
+	{0x3031, 0x0a}, {0x303f, 0x0c}, {0x3305, 0xf1},
+	{0x3307, 0x04}, {0x3309, 0x29}, {0x3500, 0x00},
+	{0x3501, 0x60}, {0x3502, 0x00}, {0x3503, 0x04},
+	{0x3504, 0x00}, {0x3505, 0x00}, {0x3506, 0x00},
+	{0x3507, 0x00}, {0x3508, 0x00}, {0x3509, 0x80},
+	{0x350a, 0x00}, {0x350b, 0x00}, {0x350c, 0x00},
+	{0x350d, 0x00}, {0x350e, 0x00}, {0x350f, 0x80},
+	{0x3510, 0x00}, {0x3511, 0x00}, {0x3512, 0x00},
+	{0x3513, 0x00}, {0x3514, 0x00}, {0x3515, 0x80},
+	{0x3516, 0x00}, {0x3517, 0x00}, {0x3518, 0x00},
+	{0x3519, 0x00}, {0x351a, 0x00}, {0x351b, 0x80},
+	{0x351c, 0x00}, {0x351d, 0x00}, {0x351e, 0x00},
+	{0x351f, 0x00}, {0x3520, 0x00}, {0x3521, 0x80},
+	{0x3522, 0x08}, {0x3524, 0x08}, {0x3526, 0x08},
+	{0x3528, 0x08}, {0x352a, 0x08}, {0x3602, 0x00},
+	{0x3603, 0x40}, {0x3604, 0x02}, {0x3605, 0x00},
+	{0x3606, 0x00}, {0x3607, 0x00}, {0x3609, 0x12},
+	{0x360a, 0x40}, {0x360c, 0x08}, {0x360f, 0xe5},
+	{0x3608, 0x8f}, {0x3611, 0x00}, {0x3613, 0xf7},
+	{0x3616, 0x58}, {0x3619, 0x99}, {0x361b, 0x60},
+	{0x361c, 0x7a}, {0x361e, 0x79}, {0x361f, 0x02},
+	{0x3632, 0x00}, {0x3633, 0x10}, {0x3634, 0x10},
+	{0x3635, 0x10}, {0x3636, 0x15}, {0x3646, 0x86},
+	{0x364a, 0x0b}, {0x3700, 0x17}, {0x3701, 0x22},
+	{0x3703, 0x10}, {0x370a, 0x37}, {0x3705, 0x00},
+	{0x3706, 0x63}, {0x3709, 0x3c}, {0x370b, 0x01},
+	{0x370c, 0x30}, {0x3710, 0x24}, {0x3711, 0x0c},
+	{0x3716, 0x00}, {0x3720, 0x28}, {0x3729, 0x7b},
+	{0x372a, 0x84}, {0x372b, 0xbd}, {0x372c, 0xbc},
+	{0x372e, 0x52}, {0x373c, 0x0e}, {0x373e, 0x33},
+	{0x3743, 0x10}, {0x3744, 0x88}, {0x3745, 0xc0},
+	{0x374a, 0x43}, {0x374c, 0x00}, {0x374e, 0x23},
+	{0x3751, 0x7b}, {0x3752, 0x84}, {0x3753, 0xbd},
+	{0x3754, 0xbc}, {0x3756, 0x52}, {0x375c, 0x00},
+	{0x3760, 0x00}, {0x3761, 0x00}, {0x3762, 0x00},
+	{0x3763, 0x00}, {0x3764, 0x00}, {0x3767, 0x04},
+	{0x3768, 0x04}, {0x3769, 0x08}, {0x376a, 0x08},
+	{0x376b, 0x20}, {0x376c, 0x00}, {0x376d, 0x00},
+	{0x376e, 0x00}, {0x3773, 0x00}, {0x3774, 0x51},
+	{0x3776, 0xbd}, {0x3777, 0xbd}, {0x3781, 0x18},
+	{0x3783, 0x25}, {0x3798, 0x1b}, {0x3800, 0x00},
+	{0x3801, 0x08}, {0x3802, 0x00}, {0x3803, 0x04},
+	{0x3804, 0x0a}, {0x3805, 0x97}, {0x3806, 0x05},
+	{0x3807, 0xfb}, {0x3808, 0x0a}, {0x3809, 0x80},
+	{0x380a, 0x05}, {0x380b, 0xf0}, {0x380c, 0x0a},
+	{0x380d, 0x0e}, {0x380e, 0x06}, {0x380f, 0x12},
+	{0x3810, 0x00}, {0x3811, 0x08}, {0x3812, 0x00},
+	{0x3813, 0x04}, {0x3814, 0x01}, {0x3815, 0x01},
+	{0x3819, 0x01}, {0x3820, 0x00}, {0x3821, 0x06},
+	{0x3829, 0x00}, {0x382a, 0x01}, {0x382b, 0x01},
+	{0x382d, 0x7f}, {0x3830, 0x04}, {0x3836, 0x01},
+	{0x3837, 0x00}, {0x3841, 0x02}, {0x3846, 0x08},
+	{0x3847, 0x07}, {0x3d85, 0x36}, {0x3d8c, 0x71},
+	{0x3d8d, 0xcb}, {0x3f0a, 0x00}, {0x4000, 0xf1},
+	{0x4001, 0x40}, {0x4002, 0x04}, {0x4003, 0x14},
+	{0x400e, 0x00}, {0x4011, 0x00}, {0x401a, 0x00},
+	{0x401b, 0x00}, {0x401c, 0x00}, {0x401d, 0x00},
+	{0x401f, 0x00}, {0x4020, 0x00}, {0x4021, 0x10},
+	{0x4022, 0x07}, {0x4023, 0xcf}, {0x4024, 0x09},
+	{0x4025, 0x60}, {0x4026, 0x09}, {0x4027, 0x6f},
+	{0x4028, 0x00}, {0x4029, 0x02}, {0x402a, 0x06},
+	{0x402b, 0x04}, {0x402c, 0x02}, {0x402d, 0x02},
+	{0x402e, 0x0e}, {0x402f, 0x04}, {0x4302, 0xff},
+	{0x4303, 0xff}, {0x4304, 0x00}, {0x4305, 0x00},
+	{0x4306, 0x00}, {0x4308, 0x02}, {0x4500, 0x6c},
+	{0x4501, 0xc4}, {0x4502, 0x40}, {0x4503, 0x01},
+	{0x4601, 0xa7}, {0x4800, 0x04}, {0x4813, 0x08},
+	{0x481f, 0x40}, {0x4829, 0x78}, {0x4837, 0x10},
+	{0x4b00, 0x2a}, {0x4b0d, 0x00}, {0x4d00, 0x04},
+	{0x4d01, 0x42}, {0x4d02, 0xd1}, {0x4d03, 0x93},
+	{0x4d04, 0xf5}, {0x4d05, 0xc1}, {0x5000, 0xf3},
+	{0x5001, 0x11}, {0x5004, 0x00}, {0x500a, 0x00},
+	{0x500b, 0x00}, {0x5032, 0x00}, {0x5040, 0x00},
+	{0x5050, 0x0c}, {0x5500, 0x00}, {0x5501, 0x10},
+	{0x5502, 0x01}, {0x5503, 0x0f}, {0x8000, 0x00},
+	{0x8001, 0x00}, {0x8002, 0x00}, {0x8003, 0x00},
+	{0x8004, 0x00}, {0x8005, 0x00}, {0x8006, 0x00},
+	{0x8007, 0x00}, {0x8008, 0x00}, {0x3638, 0x00},
+	{REG_NULL, 0x00},
+};
+
+static const struct ov4689_mode supported_modes[] = {
+	{
+		.id = OV4689_MODE_2688_1520,
+		.width = 2688,
+		.height = 1520,
+		.sensor_width = 2720,
+		.sensor_height = 1536,
+		.crop_top = 8,
+		.crop_left = 16,
+		.max_fps = 30,
+		.exp_def = 1536,
+		.hts_def = 4 * 2574,
+		.vts_def = 1554,
+		.pixel_rate = 480000000,
+		.reg_list = ov4689_2688x1520_regs,
+	},
+};
+
+static const u64 link_freq_menu_items[] = { 504000000 };
+
+static const char *const ov4689_test_pattern_menu[] = {
+	"Disabled",
+	"Vertical Color Bar Type 1",
+	"Vertical Color Bar Type 2",
+	"Vertical Color Bar Type 3",
+	"Vertical Color Bar Type 4"
+};
+
+/*
+ * These coefficients are based on those used in Rockchip's camera
+ * engine, with minor tweaks for continuity.
+ */
+static const struct ov4689_gain_range ov4689_gain_ranges[] = {
+	{
+		.logical_min = 0,
+		.logical_max = 255,
+		.offset = 0,
+		.divider = 1,
+		.physical_min = 0,
+		.physical_max = 255,
+	},
+	{
+		.logical_min = 256,
+		.logical_max = 511,
+		.offset = 252,
+		.divider = 2,
+		.physical_min = 376,
+		.physical_max = 504,
+	},
+	{
+		.logical_min = 512,
+		.logical_max = 1023,
+		.offset = 758,
+		.divider = 4,
+		.physical_min = 884,
+		.physical_max = 1012,
+	},
+	{
+		.logical_min = 1024,
+		.logical_max = 2047,
+		.offset = 1788,
+		.divider = 8,
+		.physical_min = 1912,
+		.physical_max = 2047,
+	},
+};
+
+/* Write registers up to 4 at a time */
+static int ov4689_write_reg(struct i2c_client *client, u16 reg, u32 len,
+			    u32 val)
+{
+	u32 buf_i, val_i;
+	__be32 val_be;
+	u8 *val_p;
+	u8 buf[6];
+
+	if (len > 4)
+		return -EINVAL;
+
+	buf[0] = reg >> 8;
+	buf[1] = reg & 0xff;
+
+	val_be = cpu_to_be32(val);
+	val_p = (u8 *)&val_be;
+	buf_i = 2;
+	val_i = 4 - len;
+
+	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;
+}
+
+static int ov4689_write_array(struct i2c_client *client,
+			      const struct regval *regs)
+{
+	int ret = 0;
+	u32 i;
+
+	for (i = 0; ret == 0 && regs[i].addr != REG_NULL; i++)
+		ret = ov4689_write_reg(client, regs[i].addr,
+				       OV4689_REG_VALUE_08BIT, regs[i].val);
+
+	return ret;
+}
+
+/* Read registers up to 4 at a time */
+static int ov4689_read_reg(struct i2c_client *client, u16 reg, unsigned int len,
+			   u32 *val)
+{
+	__be16 reg_addr_be = cpu_to_be16(reg);
+	struct i2c_msg msgs[2];
+	__be32 data_be = 0;
+	u8 *data_be_p;
+	int ret;
+
+	if (len > 4 || !len)
+		return -EINVAL;
+
+	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[4 - len];
+
+	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 void ov4689_fill_fmt(const struct ov4689_mode *mode,
+			    struct v4l2_mbus_framefmt *fmt)
+{
+	fmt->code = MEDIA_BUS_FMT_SBGGR10_1X10;
+	fmt->width = mode->width;
+	fmt->height = mode->height;
+	fmt->field = V4L2_FIELD_NONE;
+}
+
+static int ov4689_set_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct v4l2_mbus_framefmt *mbus_fmt = &fmt->format;
+	struct ov4689 *ov4689 = to_ov4689(sd);
+
+	/* only one mode supported for now */
+	ov4689_fill_fmt(ov4689->cur_mode, mbus_fmt);
+
+	return 0;
+}
+
+static int ov4689_get_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct v4l2_mbus_framefmt *mbus_fmt = &fmt->format;
+	struct ov4689 *ov4689 = to_ov4689(sd);
+
+	/* only one mode supported for now */
+	ov4689_fill_fmt(ov4689->cur_mode, mbus_fmt);
+
+	return 0;
+}
+
+static int ov4689_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index != 0)
+		return -EINVAL;
+	code->code = MEDIA_BUS_FMT_SBGGR10_1X10;
+
+	return 0;
+}
+
+static int ov4689_enum_frame_sizes(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->index >= ARRAY_SIZE(supported_modes))
+		return -EINVAL;
+
+	if (fse->code != MEDIA_BUS_FMT_SBGGR10_1X10)
+		return -EINVAL;
+
+	fse->min_width = supported_modes[fse->index].width;
+	fse->max_width = supported_modes[fse->index].width;
+	fse->max_height = supported_modes[fse->index].height;
+	fse->min_height = supported_modes[fse->index].height;
+
+	return 0;
+}
+
+static int ov4689_enable_test_pattern(struct ov4689 *ov4689, u32 pattern)
+{
+	u32 val;
+
+	if (pattern)
+		val = (pattern - 1) | OV4689_TEST_PATTERN_ENABLE;
+	else
+		val = OV4689_TEST_PATTERN_DISABLE;
+
+	return ov4689_write_reg(ov4689->client, OV4689_REG_TEST_PATTERN,
+				OV4689_REG_VALUE_08BIT, val);
+}
+
+static int ov4689_get_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *state,
+				struct v4l2_subdev_selection *sel)
+{
+	const struct ov4689_mode *mode = to_ov4689(sd)->cur_mode;
+
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
+		return -EINVAL;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.top = 0;
+		sel->r.left = 0;
+		sel->r.width = mode->sensor_width;
+		sel->r.height = mode->sensor_height;
+		return 0;
+	case V4L2_SEL_TGT_CROP:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		sel->r.top = mode->crop_top;
+		sel->r.left = mode->crop_left;
+		sel->r.width = mode->width;
+		sel->r.height = mode->height;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int ov4689_s_stream(struct v4l2_subdev *sd, int on)
+{
+	struct ov4689 *ov4689 = to_ov4689(sd);
+	struct i2c_client *client = ov4689->client;
+	int ret = 0;
+
+	mutex_lock(&ov4689->mutex);
+
+	on = !!on;
+	if (on == ov4689->streaming)
+		goto unlock_and_return;
+
+	if (on) {
+		ret = pm_runtime_resume_and_get(&client->dev);
+		if (ret < 0)
+			goto unlock_and_return;
+
+		ret = ov4689_write_array(ov4689->client,
+					 ov4689->cur_mode->reg_list);
+		if (ret) {
+			pm_runtime_put(&client->dev);
+			goto unlock_and_return;
+		}
+
+		ret = __v4l2_ctrl_handler_setup(&ov4689->ctrl_handler);
+		if (ret) {
+			pm_runtime_put(&client->dev);
+			goto unlock_and_return;
+		}
+
+		ret = ov4689_write_reg(ov4689->client, OV4689_REG_CTRL_MODE,
+				       OV4689_REG_VALUE_08BIT,
+				       OV4689_MODE_STREAMING);
+		if (ret) {
+			pm_runtime_put(&client->dev);
+			goto unlock_and_return;
+		}
+	} else {
+		ov4689_write_reg(ov4689->client, OV4689_REG_CTRL_MODE,
+				 OV4689_REG_VALUE_08BIT,
+				 OV4689_MODE_SW_STANDBY);
+		pm_runtime_put(&client->dev);
+	}
+
+	ov4689->streaming = on;
+
+unlock_and_return:
+	mutex_unlock(&ov4689->mutex);
+
+	return ret;
+}
+
+/* Calculate the delay in us by clock rate and clock cycles */
+static inline u32 ov4689_cal_delay(struct ov4689 *ov4689, u32 cycles)
+{
+	return DIV_ROUND_UP(cycles * 1000,
+			    DIV_ROUND_UP(ov4689->clock_rate, 1000));
+}
+
+static int __maybe_unused ov4689_power_on(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov4689 *ov4689 = to_ov4689(sd);
+	u32 delay_us;
+	int ret;
+
+	ret = clk_prepare_enable(ov4689->xvclk);
+	if (ret < 0) {
+		dev_err(dev, "Failed to enable xvclk\n");
+		return ret;
+	}
+
+	gpiod_set_value_cansleep(ov4689->reset_gpio, 1);
+
+	ret = regulator_bulk_enable(ARRAY_SIZE(ov4689_supply_names),
+				    ov4689->supplies);
+	if (ret < 0) {
+		dev_err(dev, "Failed to enable regulators\n");
+		goto disable_clk;
+	}
+
+	gpiod_set_value_cansleep(ov4689->reset_gpio, 0);
+	usleep_range(500, 1000);
+	gpiod_set_value_cansleep(ov4689->pwdn_gpio, 0);
+
+	/* 8192 cycles prior to first SCCB transaction */
+	delay_us = ov4689_cal_delay(ov4689, 8192);
+	usleep_range(delay_us, delay_us * 2);
+
+	return 0;
+
+disable_clk:
+	clk_disable_unprepare(ov4689->xvclk);
+
+	return ret;
+}
+
+static int __maybe_unused ov4689_power_off(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov4689 *ov4689 = to_ov4689(sd);
+
+	gpiod_set_value_cansleep(ov4689->pwdn_gpio, 1);
+	clk_disable_unprepare(ov4689->xvclk);
+	gpiod_set_value_cansleep(ov4689->reset_gpio, 1);
+	regulator_bulk_disable(ARRAY_SIZE(ov4689_supply_names),
+			       ov4689->supplies);
+	return 0;
+}
+
+static int ov4689_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct ov4689 *ov4689 = to_ov4689(sd);
+	struct v4l2_mbus_framefmt *try_fmt;
+
+	mutex_lock(&ov4689->mutex);
+
+	try_fmt = v4l2_subdev_get_try_format(sd, fh->state, 0);
+	/* Initialize try_fmt */
+	ov4689_fill_fmt(&supported_modes[OV4689_MODE_2688_1520], try_fmt);
+
+	mutex_unlock(&ov4689->mutex);
+
+	return 0;
+}
+
+static const struct dev_pm_ops ov4689_pm_ops = {
+	SET_RUNTIME_PM_OPS(ov4689_power_off, ov4689_power_on, NULL)
+};
+
+static const struct v4l2_subdev_internal_ops ov4689_internal_ops = {
+	.open = ov4689_open,
+};
+
+static const struct v4l2_subdev_video_ops ov4689_video_ops = {
+	.s_stream = ov4689_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops ov4689_pad_ops = {
+	.enum_mbus_code = ov4689_enum_mbus_code,
+	.enum_frame_size = ov4689_enum_frame_sizes,
+	.get_fmt = ov4689_get_fmt,
+	.set_fmt = ov4689_set_fmt,
+	.get_selection = ov4689_get_selection,
+};
+
+static const struct v4l2_subdev_ops ov4689_subdev_ops = {
+	.video = &ov4689_video_ops,
+	.pad = &ov4689_pad_ops,
+};
+
+/*
+ * Map userspace (logical) gain to sensor (physical) gain using
+ * ov4689_gain_ranges table.
+ */
+static int ov4689_map_gain(struct ov4689 *ov4689, int logical_gain, int *result)
+{
+	const struct device *dev = &ov4689->client->dev;
+	const struct ov4689_gain_range *range;
+	unsigned int n;
+
+	for (n = 0; n < ARRAY_SIZE(ov4689_gain_ranges); n++) {
+		if (logical_gain >= ov4689_gain_ranges[n].logical_min &&
+		    logical_gain <= ov4689_gain_ranges[n].logical_max)
+			break;
+	}
+
+	if (n == ARRAY_SIZE(ov4689_gain_ranges)) {
+		dev_warn_ratelimited(dev, "no mapping found for gain %d\n",
+				     logical_gain);
+		return -EINVAL;
+	}
+
+	range = &ov4689_gain_ranges[n];
+
+	*result = clamp(range->offset + (logical_gain) / range->divider,
+			range->physical_min, range->physical_max);
+	return 0;
+}
+
+static int ov4689_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov4689 *ov4689 =
+		container_of(ctrl->handler, struct ov4689, ctrl_handler);
+	struct i2c_client *client = ov4689->client;
+	int sensor_gain;
+	s64 max_expo;
+	int ret;
+
+	/* Propagate change of current control to all related controls */
+	switch (ctrl->id) {
+	case V4L2_CID_VBLANK:
+		/* Update max exposure while meeting expected vblanking */
+		max_expo = ov4689->cur_mode->height + ctrl->val - 4;
+		__v4l2_ctrl_modify_range(ov4689->exposure,
+					 ov4689->exposure->minimum, max_expo,
+					 ov4689->exposure->step,
+					 ov4689->exposure->default_value);
+		break;
+	}
+
+	if (!pm_runtime_get_if_in_use(&client->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE:
+		/* 4 least significant bits of expsoure are fractional part */
+		ret = ov4689_write_reg(ov4689->client, OV4689_REG_EXPOSURE,
+				       OV4689_REG_VALUE_24BIT, ctrl->val << 4);
+		break;
+	case V4L2_CID_ANALOGUE_GAIN:
+		ret = ov4689_map_gain(ov4689, ctrl->val, &sensor_gain);
+
+		ret = ret ?:
+			ov4689_write_reg(ov4689->client, OV4689_REG_GAIN_H,
+					 OV4689_REG_VALUE_08BIT,
+					 (sensor_gain >> OV4689_GAIN_H_SHIFT) &
+					 OV4689_GAIN_H_MASK);
+		ret = ret ?:
+			ov4689_write_reg(ov4689->client, OV4689_REG_GAIN_L,
+					 OV4689_REG_VALUE_08BIT,
+					 sensor_gain & OV4689_GAIN_L_MASK);
+		break;
+	case V4L2_CID_VBLANK:
+		ret = ov4689_write_reg(ov4689->client, OV4689_REG_VTS,
+				       OV4689_REG_VALUE_16BIT,
+				       ctrl->val + ov4689->cur_mode->height);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		ret = ov4689_enable_test_pattern(ov4689, ctrl->val);
+		break;
+	default:
+		dev_warn(&client->dev, "%s Unhandled id:0x%x, val:0x%x\n",
+			 __func__, ctrl->id, ctrl->val);
+		ret = -EINVAL;
+		break;
+	}
+
+	pm_runtime_put(&client->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ov4689_ctrl_ops = {
+	.s_ctrl = ov4689_set_ctrl,
+};
+
+static int ov4689_initialize_controls(struct ov4689 *ov4689)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov4689->subdev);
+	struct v4l2_fwnode_device_properties props;
+	struct v4l2_ctrl_handler *handler;
+	const struct ov4689_mode *mode;
+	s64 exposure_max, vblank_def;
+	struct v4l2_ctrl *ctrl;
+	s64 h_blank_def;
+	int ret;
+
+	handler = &ov4689->ctrl_handler;
+	mode = ov4689->cur_mode;
+	ret = v4l2_ctrl_handler_init(handler, 10);
+	if (ret)
+		return ret;
+	handler->lock = &ov4689->mutex;
+
+	ctrl = v4l2_ctrl_new_int_menu(handler, NULL, V4L2_CID_LINK_FREQ, 0, 0,
+				      link_freq_menu_items);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	v4l2_ctrl_new_std(handler, NULL, V4L2_CID_PIXEL_RATE, 0,
+			  mode->pixel_rate, 1, mode->pixel_rate);
+
+	h_blank_def = mode->hts_def - mode->width;
+	ctrl = v4l2_ctrl_new_std(handler, NULL, V4L2_CID_HBLANK, h_blank_def,
+				 h_blank_def, 1, h_blank_def);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	vblank_def = mode->vts_def - mode->height;
+	v4l2_ctrl_new_std(handler, &ov4689_ctrl_ops, V4L2_CID_VBLANK,
+			  vblank_def, OV4689_VTS_MAX - mode->height, 1,
+			  vblank_def);
+
+	exposure_max = mode->vts_def - 4;
+	ov4689->exposure =
+		v4l2_ctrl_new_std(handler, &ov4689_ctrl_ops, V4L2_CID_EXPOSURE,
+				  OV4689_EXPOSURE_MIN, exposure_max,
+				  OV4689_EXPOSURE_STEP, mode->exp_def);
+
+	v4l2_ctrl_new_std(handler, &ov4689_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
+			  ov4689_gain_ranges[0].logical_min,
+			  ov4689_gain_ranges[ARRAY_SIZE(ov4689_gain_ranges) - 1]
+				  .logical_max,
+			  OV4689_GAIN_STEP, OV4689_GAIN_DEFAULT);
+
+	v4l2_ctrl_new_std_menu_items(handler, &ov4689_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(ov4689_test_pattern_menu) - 1,
+				     0, 0, ov4689_test_pattern_menu);
+
+	if (handler->error) {
+		ret = handler->error;
+		dev_err(&ov4689->client->dev, "Failed to init controls(%d)\n",
+			ret);
+		goto err_free_handler;
+	}
+
+	ret = v4l2_fwnode_device_parse(&client->dev, &props);
+	if (ret)
+		goto err_free_handler;
+
+	ret = v4l2_ctrl_new_fwnode_properties(handler, &ov4689_ctrl_ops,
+					      &props);
+	if (ret)
+		goto err_free_handler;
+
+	ov4689->subdev.ctrl_handler = handler;
+
+	return 0;
+
+err_free_handler:
+	v4l2_ctrl_handler_free(handler);
+
+	return ret;
+}
+
+static int ov4689_check_sensor_id(struct ov4689 *ov4689,
+				  struct i2c_client *client)
+{
+	struct device *dev = &ov4689->client->dev;
+	u32 id = 0;
+	int ret;
+
+	ret = ov4689_read_reg(client, OV4689_REG_CHIP_ID,
+			      OV4689_REG_VALUE_16BIT, &id);
+	if (ret) {
+		dev_err(dev, "Cannot read sensor ID\n");
+		return ret;
+	}
+
+	if (id != CHIP_ID) {
+		dev_err(dev, "Unexpected sensor ID %06x, expected %06x\n",
+			id, CHIP_ID);
+		return -ENODEV;
+	}
+
+	dev_info(dev, "Detected OV%06x sensor\n", CHIP_ID);
+
+	return 0;
+}
+
+static int ov4689_configure_regulators(struct ov4689 *ov4689)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(ov4689_supply_names); i++)
+		ov4689->supplies[i].supply = ov4689_supply_names[i];
+
+	return devm_regulator_bulk_get(&ov4689->client->dev,
+				       ARRAY_SIZE(ov4689_supply_names),
+				       ov4689->supplies);
+}
+
+static u64 ov4689_check_link_frequency(struct v4l2_fwnode_endpoint *ep)
+{
+	const u64 *freqs = link_freq_menu_items;
+	unsigned int i, j;
+
+	for (i = 0; i < ARRAY_SIZE(link_freq_menu_items); i++) {
+		for (j = 0; j < ep->nr_of_link_frequencies; j++)
+			if (freqs[i] == ep->link_frequencies[j])
+				return freqs[i];
+	}
+
+	return 0;
+}
+
+static int ov4689_check_hwcfg(struct device *dev)
+{
+	struct fwnode_handle *fwnode = dev_fwnode(dev);
+	struct v4l2_fwnode_endpoint bus_cfg = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY,
+	};
+	struct fwnode_handle *endpoint;
+	int ret;
+
+	endpoint = fwnode_graph_get_next_endpoint(fwnode, NULL);
+	if (!endpoint)
+		return -EINVAL;
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(endpoint, &bus_cfg);
+	fwnode_handle_put(endpoint);
+	if (ret)
+		return ret;
+
+	if (bus_cfg.bus.mipi_csi2.num_data_lanes != OV4689_LANES) {
+		dev_err(dev, "Only a 4-lane CSI2 config is supported");
+		ret = -EINVAL;
+		goto out_free_bus_cfg;
+	}
+
+	if (!ov4689_check_link_frequency(&bus_cfg)) {
+		dev_err(dev, "No supported link frequency found\n");
+		ret = -EINVAL;
+	}
+
+out_free_bus_cfg:
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+
+	return ret;
+}
+
+static int ov4689_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct v4l2_subdev *sd;
+	struct ov4689 *ov4689;
+	int ret;
+
+	ret = ov4689_check_hwcfg(dev);
+	if (ret)
+		return ret;
+
+	ov4689 = devm_kzalloc(dev, sizeof(*ov4689), GFP_KERNEL);
+	if (!ov4689)
+		return -ENOMEM;
+
+	ov4689->client = client;
+	ov4689->cur_mode = &supported_modes[OV4689_MODE_2688_1520];
+
+	ov4689->xvclk = devm_clk_get_optional(dev, NULL);
+	if (IS_ERR(ov4689->xvclk))
+		return dev_err_probe(dev, PTR_ERR(ov4689->xvclk),
+				     "Failed to get external clock\n");
+
+	if (!ov4689->xvclk) {
+		dev_dbg(dev,
+			"No clock provided, using clock-frequency property\n");
+		device_property_read_u32(dev, "clock-frequency",
+					 &ov4689->clock_rate);
+	} else {
+		ov4689->clock_rate = clk_get_rate(ov4689->xvclk);
+	}
+
+	if (ov4689->clock_rate != OV4689_XVCLK_FREQ) {
+		dev_err(dev,
+			"External clock rate mismatch: got %d Hz, expected %d Hz\n",
+			ov4689->clock_rate, OV4689_XVCLK_FREQ);
+		return -EINVAL;
+	}
+
+	ov4689->reset_gpio = devm_gpiod_get_optional(dev, "reset",
+						     GPIOD_OUT_LOW);
+	if (IS_ERR(ov4689->reset_gpio)) {
+		dev_err(dev, "Failed to get reset-gpios\n");
+		return PTR_ERR(ov4689->reset_gpio);
+	}
+
+	ov4689->pwdn_gpio = devm_gpiod_get_optional(dev, "pwdn", GPIOD_OUT_LOW);
+	if (IS_ERR(ov4689->pwdn_gpio)) {
+		dev_err(dev, "Failed to get pwdn-gpios\n");
+		return PTR_ERR(ov4689->pwdn_gpio);
+	}
+
+	ret = ov4689_configure_regulators(ov4689);
+	if (ret)
+		return dev_err_probe(dev, ret,
+				     "Failed to get power regulators\n");
+
+	mutex_init(&ov4689->mutex);
+
+	sd = &ov4689->subdev;
+	v4l2_i2c_subdev_init(sd, client, &ov4689_subdev_ops);
+	ret = ov4689_initialize_controls(ov4689);
+	if (ret)
+		goto err_destroy_mutex;
+
+	ret = ov4689_power_on(dev);
+	if (ret)
+		goto err_free_handler;
+
+	ret = ov4689_check_sensor_id(ov4689, client);
+	if (ret)
+		goto err_power_off;
+
+	sd->internal_ops = &ov4689_internal_ops;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	ov4689->pad.flags = MEDIA_PAD_FL_SOURCE;
+	sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret = media_entity_pads_init(&sd->entity, 1, &ov4689->pad);
+	if (ret < 0)
+		goto err_power_off;
+
+	ret = v4l2_async_register_subdev_sensor(sd);
+	if (ret) {
+		dev_err(dev, "v4l2 async register subdev failed\n");
+		goto err_clean_entity;
+	}
+
+	pm_runtime_set_active(dev);
+	pm_runtime_enable(dev);
+	pm_runtime_idle(dev);
+
+	return 0;
+
+err_clean_entity:
+	media_entity_cleanup(&sd->entity);
+err_power_off:
+	ov4689_power_off(dev);
+err_free_handler:
+	v4l2_ctrl_handler_free(&ov4689->ctrl_handler);
+err_destroy_mutex:
+	mutex_destroy(&ov4689->mutex);
+
+	return ret;
+}
+
+static void ov4689_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov4689 *ov4689 = to_ov4689(sd);
+
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+
+	v4l2_ctrl_handler_free(&ov4689->ctrl_handler);
+	mutex_destroy(&ov4689->mutex);
+
+	pm_runtime_disable(&client->dev);
+	if (!pm_runtime_status_suspended(&client->dev))
+		ov4689_power_off(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+}
+
+static const struct of_device_id ov4689_of_match[] = {
+	{ .compatible = "ovti,ov4689" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, ov4689_of_match);
+
+static struct i2c_driver ov4689_i2c_driver = {
+	.driver = {
+		.name = "ov4689",
+		.pm = &ov4689_pm_ops,
+		.of_match_table = ov4689_of_match,
+	},
+	.probe = ov4689_probe,
+	.remove	= ov4689_remove,
+};
+
+module_i2c_driver(ov4689_i2c_driver);
+
+MODULE_DESCRIPTION("OmniVision ov4689 sensor driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/ov5640.c b/drivers/media/i2c/ov5640.c
new file mode 100644
index 000000000..40532f7bc
--- /dev/null
+++ b/drivers/media/i2c/ov5640.c
@@ -0,0 +1,4012 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2011-2013 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2014-2017 Mentor Graphics Inc.
+ */
+
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/clkdev.h>
+#include <linux/ctype.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/mod_devicetable.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+/* min/typical/max system clock (xclk) frequencies */
+#define OV5640_XCLK_MIN  6000000
+#define OV5640_XCLK_MAX 54000000
+
+#define OV5640_NATIVE_WIDTH		2624
+#define OV5640_NATIVE_HEIGHT		1964
+#define OV5640_PIXEL_ARRAY_TOP		14
+#define OV5640_PIXEL_ARRAY_LEFT		16
+#define OV5640_PIXEL_ARRAY_WIDTH	2592
+#define OV5640_PIXEL_ARRAY_HEIGHT	1944
+
+/* FIXME: not documented. */
+#define OV5640_MIN_VBLANK	24
+#define OV5640_MAX_VTS		3375
+
+#define OV5640_DEFAULT_SLAVE_ID 0x3c
+
+#define OV5640_LINK_RATE_MAX		490000000U
+
+#define OV5640_REG_SYS_RESET02		0x3002
+#define OV5640_REG_SYS_CLOCK_ENABLE02	0x3006
+#define OV5640_REG_SYS_CTRL0		0x3008
+#define OV5640_REG_SYS_CTRL0_SW_PWDN	0x42
+#define OV5640_REG_SYS_CTRL0_SW_PWUP	0x02
+#define OV5640_REG_SYS_CTRL0_SW_RST	0x82
+#define OV5640_REG_CHIP_ID		0x300a
+#define OV5640_REG_IO_MIPI_CTRL00	0x300e
+#define OV5640_REG_PAD_OUTPUT_ENABLE01	0x3017
+#define OV5640_REG_PAD_OUTPUT_ENABLE02	0x3018
+#define OV5640_REG_PAD_OUTPUT00		0x3019
+#define OV5640_REG_SYSTEM_CONTROL1	0x302e
+#define OV5640_REG_SC_PLL_CTRL0		0x3034
+#define OV5640_REG_SC_PLL_CTRL1		0x3035
+#define OV5640_REG_SC_PLL_CTRL2		0x3036
+#define OV5640_REG_SC_PLL_CTRL3		0x3037
+#define OV5640_REG_SLAVE_ID		0x3100
+#define OV5640_REG_SCCB_SYS_CTRL1	0x3103
+#define OV5640_REG_SYS_ROOT_DIVIDER	0x3108
+#define OV5640_REG_AWB_R_GAIN		0x3400
+#define OV5640_REG_AWB_G_GAIN		0x3402
+#define OV5640_REG_AWB_B_GAIN		0x3404
+#define OV5640_REG_AWB_MANUAL_CTRL	0x3406
+#define OV5640_REG_AEC_PK_EXPOSURE_HI	0x3500
+#define OV5640_REG_AEC_PK_EXPOSURE_MED	0x3501
+#define OV5640_REG_AEC_PK_EXPOSURE_LO	0x3502
+#define OV5640_REG_AEC_PK_MANUAL	0x3503
+#define OV5640_REG_AEC_PK_REAL_GAIN	0x350a
+#define OV5640_REG_AEC_PK_VTS		0x350c
+#define OV5640_REG_TIMING_HS		0x3800
+#define OV5640_REG_TIMING_VS		0x3802
+#define OV5640_REG_TIMING_HW		0x3804
+#define OV5640_REG_TIMING_VH		0x3806
+#define OV5640_REG_TIMING_DVPHO		0x3808
+#define OV5640_REG_TIMING_DVPVO		0x380a
+#define OV5640_REG_TIMING_HTS		0x380c
+#define OV5640_REG_TIMING_VTS		0x380e
+#define OV5640_REG_TIMING_HOFFS		0x3810
+#define OV5640_REG_TIMING_VOFFS		0x3812
+#define OV5640_REG_TIMING_TC_REG20	0x3820
+#define OV5640_REG_TIMING_TC_REG21	0x3821
+#define OV5640_REG_AEC_CTRL00		0x3a00
+#define OV5640_REG_AEC_B50_STEP		0x3a08
+#define OV5640_REG_AEC_B60_STEP		0x3a0a
+#define OV5640_REG_AEC_CTRL0D		0x3a0d
+#define OV5640_REG_AEC_CTRL0E		0x3a0e
+#define OV5640_REG_AEC_CTRL0F		0x3a0f
+#define OV5640_REG_AEC_CTRL10		0x3a10
+#define OV5640_REG_AEC_CTRL11		0x3a11
+#define OV5640_REG_AEC_CTRL1B		0x3a1b
+#define OV5640_REG_AEC_CTRL1E		0x3a1e
+#define OV5640_REG_AEC_CTRL1F		0x3a1f
+#define OV5640_REG_HZ5060_CTRL00	0x3c00
+#define OV5640_REG_HZ5060_CTRL01	0x3c01
+#define OV5640_REG_SIGMADELTA_CTRL0C	0x3c0c
+#define OV5640_REG_FRAME_CTRL01		0x4202
+#define OV5640_REG_FORMAT_CONTROL00	0x4300
+#define OV5640_REG_VFIFO_HSIZE		0x4602
+#define OV5640_REG_VFIFO_VSIZE		0x4604
+#define OV5640_REG_JPG_MODE_SELECT	0x4713
+#define OV5640_REG_CCIR656_CTRL00	0x4730
+#define OV5640_REG_POLARITY_CTRL00	0x4740
+#define OV5640_REG_MIPI_CTRL00		0x4800
+#define OV5640_REG_DEBUG_MODE		0x4814
+#define OV5640_REG_PCLK_PERIOD		0x4837
+#define OV5640_REG_ISP_FORMAT_MUX_CTRL	0x501f
+#define OV5640_REG_PRE_ISP_TEST_SET1	0x503d
+#define OV5640_REG_SDE_CTRL0		0x5580
+#define OV5640_REG_SDE_CTRL1		0x5581
+#define OV5640_REG_SDE_CTRL3		0x5583
+#define OV5640_REG_SDE_CTRL4		0x5584
+#define OV5640_REG_SDE_CTRL5		0x5585
+#define OV5640_REG_AVG_READOUT		0x56a1
+
+enum ov5640_mode_id {
+	OV5640_MODE_QQVGA_160_120 = 0,
+	OV5640_MODE_QCIF_176_144,
+	OV5640_MODE_QVGA_320_240,
+	OV5640_MODE_VGA_640_480,
+	OV5640_MODE_NTSC_720_480,
+	OV5640_MODE_PAL_720_576,
+	OV5640_MODE_XGA_1024_768,
+	OV5640_MODE_720P_1280_720,
+	OV5640_MODE_1080P_1920_1080,
+	OV5640_MODE_QSXGA_2592_1944,
+	OV5640_NUM_MODES,
+};
+
+enum ov5640_frame_rate {
+	OV5640_15_FPS = 0,
+	OV5640_30_FPS,
+	OV5640_60_FPS,
+	OV5640_NUM_FRAMERATES,
+};
+
+enum ov5640_pixel_rate_id {
+	OV5640_PIXEL_RATE_168M,
+	OV5640_PIXEL_RATE_148M,
+	OV5640_PIXEL_RATE_124M,
+	OV5640_PIXEL_RATE_96M,
+	OV5640_PIXEL_RATE_48M,
+	OV5640_NUM_PIXEL_RATES,
+};
+
+/*
+ * The chip manual suggests 24/48/96/192 MHz pixel clocks.
+ *
+ * 192MHz exceeds the sysclk limits; use 168MHz as maximum pixel rate for
+ * full resolution mode @15 FPS.
+ */
+static const u32 ov5640_pixel_rates[] = {
+	[OV5640_PIXEL_RATE_168M] = 168000000,
+	[OV5640_PIXEL_RATE_148M] = 148000000,
+	[OV5640_PIXEL_RATE_124M] = 124000000,
+	[OV5640_PIXEL_RATE_96M] = 96000000,
+	[OV5640_PIXEL_RATE_48M] = 48000000,
+};
+
+/*
+ * MIPI CSI-2 link frequencies.
+ *
+ * Derived from the above defined pixel rate for bpp = (8, 16, 24) and
+ * data_lanes = (1, 2)
+ *
+ * link_freq = (pixel_rate * bpp) / (2 * data_lanes)
+ */
+static const s64 ov5640_csi2_link_freqs[] = {
+	992000000, 888000000, 768000000, 744000000, 672000000, 672000000,
+	592000000, 592000000, 576000000, 576000000, 496000000, 496000000,
+	384000000, 384000000, 384000000, 336000000, 296000000, 288000000,
+	248000000, 192000000, 192000000, 192000000, 96000000,
+};
+
+/* Link freq for default mode: UYVY 16 bpp, 2 data lanes. */
+#define OV5640_DEFAULT_LINK_FREQ	13
+
+enum ov5640_format_mux {
+	OV5640_FMT_MUX_YUV422 = 0,
+	OV5640_FMT_MUX_RGB,
+	OV5640_FMT_MUX_DITHER,
+	OV5640_FMT_MUX_RAW_DPC,
+	OV5640_FMT_MUX_SNR_RAW,
+	OV5640_FMT_MUX_RAW_CIP,
+};
+
+struct ov5640_pixfmt {
+	u32 code;
+	u32 colorspace;
+	u8 bpp;
+	u8 ctrl00;
+	enum ov5640_format_mux mux;
+};
+
+static const struct ov5640_pixfmt ov5640_dvp_formats[] = {
+	{
+		/* YUV422, YUYV */
+		.code		= MEDIA_BUS_FMT_JPEG_1X8,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+		.bpp		= 16,
+		.ctrl00		= 0x30,
+		.mux		= OV5640_FMT_MUX_YUV422,
+	}, {
+		/* YUV422, UYVY */
+		.code		= MEDIA_BUS_FMT_UYVY8_2X8,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.bpp		= 16,
+		.ctrl00		= 0x3f,
+		.mux		= OV5640_FMT_MUX_YUV422,
+	}, {
+		/* YUV422, YUYV */
+		.code		= MEDIA_BUS_FMT_YUYV8_2X8,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.bpp		= 16,
+		.ctrl00		= 0x30,
+		.mux		= OV5640_FMT_MUX_YUV422,
+	}, {
+		/* RGB565 {g[2:0],b[4:0]},{r[4:0],g[5:3]} */
+		.code		= MEDIA_BUS_FMT_RGB565_2X8_LE,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.bpp		= 16,
+		.ctrl00		= 0x6f,
+		.mux		= OV5640_FMT_MUX_RGB,
+	}, {
+		/* RGB565 {r[4:0],g[5:3]},{g[2:0],b[4:0]} */
+		.code		= MEDIA_BUS_FMT_RGB565_2X8_BE,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.bpp		= 16,
+		.ctrl00		= 0x61,
+		.mux		= OV5640_FMT_MUX_RGB,
+	}, {
+		/* Raw, BGBG... / GRGR... */
+		.code		= MEDIA_BUS_FMT_SBGGR8_1X8,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.bpp		= 8,
+		.ctrl00		= 0x00,
+		.mux		= OV5640_FMT_MUX_RAW_DPC,
+	}, {
+		/* Raw bayer, GBGB... / RGRG... */
+		.code		= MEDIA_BUS_FMT_SGBRG8_1X8,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.bpp		= 8,
+		.ctrl00		= 0x01,
+		.mux		= OV5640_FMT_MUX_RAW_DPC,
+	}, {
+		/* Raw bayer, GRGR... / BGBG... */
+		.code		= MEDIA_BUS_FMT_SGRBG8_1X8,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.bpp		= 8,
+		.ctrl00		= 0x02,
+		.mux		= OV5640_FMT_MUX_RAW_DPC,
+	}, {
+		/* Raw bayer, RGRG... / GBGB... */
+		.code		= MEDIA_BUS_FMT_SRGGB8_1X8,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.bpp		= 8,
+		.ctrl00		= 0x03,
+		.mux		= OV5640_FMT_MUX_RAW_DPC,
+	},
+	{ /* sentinel */ }
+};
+
+static const struct ov5640_pixfmt ov5640_csi2_formats[] = {
+	{
+		/* YUV422, YUYV */
+		.code		= MEDIA_BUS_FMT_JPEG_1X8,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+		.bpp		= 16,
+		.ctrl00		= 0x30,
+		.mux		= OV5640_FMT_MUX_YUV422,
+	}, {
+		/* YUV422, UYVY */
+		.code		= MEDIA_BUS_FMT_UYVY8_1X16,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.bpp		= 16,
+		.ctrl00		= 0x3f,
+		.mux		= OV5640_FMT_MUX_YUV422,
+	}, {
+		/* YUV422, YUYV */
+		.code		= MEDIA_BUS_FMT_YUYV8_1X16,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.bpp		= 16,
+		.ctrl00		= 0x30,
+		.mux		= OV5640_FMT_MUX_YUV422,
+	}, {
+		/* RGB565 {g[2:0],b[4:0]},{r[4:0],g[5:3]} */
+		.code		= MEDIA_BUS_FMT_RGB565_1X16,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.bpp		= 16,
+		.ctrl00		= 0x6f,
+		.mux		= OV5640_FMT_MUX_RGB,
+	}, {
+		/* BGR888: RGB */
+		.code		= MEDIA_BUS_FMT_BGR888_1X24,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.bpp		= 24,
+		.ctrl00		= 0x23,
+		.mux		= OV5640_FMT_MUX_RGB,
+	}, {
+		/* Raw, BGBG... / GRGR... */
+		.code		= MEDIA_BUS_FMT_SBGGR8_1X8,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.bpp		= 8,
+		.ctrl00		= 0x00,
+		.mux		= OV5640_FMT_MUX_RAW_DPC,
+	}, {
+		/* Raw bayer, GBGB... / RGRG... */
+		.code		= MEDIA_BUS_FMT_SGBRG8_1X8,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.bpp		= 8,
+		.ctrl00		= 0x01,
+		.mux		= OV5640_FMT_MUX_RAW_DPC,
+	}, {
+		/* Raw bayer, GRGR... / BGBG... */
+		.code		= MEDIA_BUS_FMT_SGRBG8_1X8,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.bpp		= 8,
+		.ctrl00		= 0x02,
+		.mux		= OV5640_FMT_MUX_RAW_DPC,
+	}, {
+		/* Raw bayer, RGRG... / GBGB... */
+		.code		= MEDIA_BUS_FMT_SRGGB8_1X8,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.bpp		= 8,
+		.ctrl00		= 0x03,
+		.mux		= OV5640_FMT_MUX_RAW_DPC,
+	},
+	{ /* sentinel */ }
+};
+
+/*
+ * FIXME: remove this when a subdev API becomes available
+ * to set the MIPI CSI-2 virtual channel.
+ */
+static unsigned int virtual_channel;
+module_param(virtual_channel, uint, 0444);
+MODULE_PARM_DESC(virtual_channel,
+		 "MIPI CSI-2 virtual channel (0..3), default 0");
+
+static const int ov5640_framerates[] = {
+	[OV5640_15_FPS] = 15,
+	[OV5640_30_FPS] = 30,
+	[OV5640_60_FPS] = 60,
+};
+
+/* regulator supplies */
+static const char * const ov5640_supply_name[] = {
+	"DOVDD", /* Digital I/O (1.8V) supply */
+	"AVDD",  /* Analog (2.8V) supply */
+	"DVDD",  /* Digital Core (1.5V) supply */
+};
+
+#define OV5640_NUM_SUPPLIES ARRAY_SIZE(ov5640_supply_name)
+
+/*
+ * Image size under 1280 * 960 are SUBSAMPLING
+ * Image size upper 1280 * 960 are SCALING
+ */
+enum ov5640_downsize_mode {
+	SUBSAMPLING,
+	SCALING,
+};
+
+struct reg_value {
+	u16 reg_addr;
+	u8 val;
+	u8 mask;
+	u32 delay_ms;
+};
+
+struct ov5640_timings {
+	/* Analog crop rectangle. */
+	struct v4l2_rect analog_crop;
+	/* Visibile crop: from analog crop top-left corner. */
+	struct v4l2_rect crop;
+	/* Total pixels per line: width + fixed hblank. */
+	u32 htot;
+	/* Default vertical blanking: frame height = height + vblank. */
+	u32 vblank_def;
+};
+
+struct ov5640_mode_info {
+	enum ov5640_mode_id id;
+	enum ov5640_downsize_mode dn_mode;
+	enum ov5640_pixel_rate_id pixel_rate;
+
+	unsigned int width;
+	unsigned int height;
+
+	struct ov5640_timings dvp_timings;
+	struct ov5640_timings csi2_timings;
+
+	const struct reg_value *reg_data;
+	u32 reg_data_size;
+
+	/* Used by s_frame_interval only. */
+	u32 max_fps;
+	u32 def_fps;
+};
+
+struct ov5640_ctrls {
+	struct v4l2_ctrl_handler handler;
+	struct v4l2_ctrl *pixel_rate;
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *vblank;
+	struct {
+		struct v4l2_ctrl *auto_exp;
+		struct v4l2_ctrl *exposure;
+	};
+	struct {
+		struct v4l2_ctrl *auto_wb;
+		struct v4l2_ctrl *blue_balance;
+		struct v4l2_ctrl *red_balance;
+	};
+	struct {
+		struct v4l2_ctrl *auto_gain;
+		struct v4l2_ctrl *gain;
+	};
+	struct v4l2_ctrl *brightness;
+	struct v4l2_ctrl *light_freq;
+	struct v4l2_ctrl *saturation;
+	struct v4l2_ctrl *contrast;
+	struct v4l2_ctrl *hue;
+	struct v4l2_ctrl *test_pattern;
+	struct v4l2_ctrl *hflip;
+	struct v4l2_ctrl *vflip;
+};
+
+struct ov5640_dev {
+	struct i2c_client *i2c_client;
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct v4l2_fwnode_endpoint ep; /* the parsed DT endpoint info */
+	struct clk *xclk; /* system clock to OV5640 */
+	u32 xclk_freq;
+
+	struct regulator_bulk_data supplies[OV5640_NUM_SUPPLIES];
+	struct gpio_desc *reset_gpio;
+	struct gpio_desc *pwdn_gpio;
+	bool   upside_down;
+
+	/* lock to protect all members below */
+	struct mutex lock;
+
+	struct v4l2_mbus_framefmt fmt;
+	bool pending_fmt_change;
+
+	const struct ov5640_mode_info *current_mode;
+	const struct ov5640_mode_info *last_mode;
+	enum ov5640_frame_rate current_fr;
+	struct v4l2_fract frame_interval;
+	s64 current_link_freq;
+
+	struct ov5640_ctrls ctrls;
+
+	u32 prev_sysclk, prev_hts;
+	u32 ae_low, ae_high, ae_target;
+
+	bool pending_mode_change;
+	bool streaming;
+};
+
+static inline struct ov5640_dev *to_ov5640_dev(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct ov5640_dev, sd);
+}
+
+static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct ov5640_dev,
+			     ctrls.handler)->sd;
+}
+
+static inline bool ov5640_is_csi2(const struct ov5640_dev *sensor)
+{
+	return sensor->ep.bus_type == V4L2_MBUS_CSI2_DPHY;
+}
+
+static inline const struct ov5640_pixfmt *
+ov5640_formats(struct ov5640_dev *sensor)
+{
+	return ov5640_is_csi2(sensor) ? ov5640_csi2_formats
+				      : ov5640_dvp_formats;
+}
+
+static const struct ov5640_pixfmt *
+ov5640_code_to_pixfmt(struct ov5640_dev *sensor, u32 code)
+{
+	const struct ov5640_pixfmt *formats = ov5640_formats(sensor);
+	unsigned int i;
+
+	for (i = 0; formats[i].code; ++i) {
+		if (formats[i].code == code)
+			return &formats[i];
+	}
+
+	return &formats[0];
+}
+
+static u32 ov5640_code_to_bpp(struct ov5640_dev *sensor, u32 code)
+{
+	const struct ov5640_pixfmt *format = ov5640_code_to_pixfmt(sensor,
+								   code);
+
+	return format->bpp;
+}
+
+/*
+ * FIXME: all of these register tables are likely filled with
+ * entries that set the register to their power-on default values,
+ * and which are otherwise not touched by this driver. Those entries
+ * should be identified and removed to speed register load time
+ * over i2c.
+ */
+/* YUV422 UYVY VGA@30fps */
+
+static const struct v4l2_mbus_framefmt ov5640_csi2_default_fmt = {
+	.code = MEDIA_BUS_FMT_UYVY8_1X16,
+	.width = 640,
+	.height = 480,
+	.colorspace = V4L2_COLORSPACE_SRGB,
+	.ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(V4L2_COLORSPACE_SRGB),
+	.quantization = V4L2_QUANTIZATION_FULL_RANGE,
+	.xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(V4L2_COLORSPACE_SRGB),
+	.field = V4L2_FIELD_NONE,
+};
+
+static const struct v4l2_mbus_framefmt ov5640_dvp_default_fmt = {
+	.code = MEDIA_BUS_FMT_UYVY8_2X8,
+	.width = 640,
+	.height = 480,
+	.colorspace = V4L2_COLORSPACE_SRGB,
+	.ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(V4L2_COLORSPACE_SRGB),
+	.quantization = V4L2_QUANTIZATION_FULL_RANGE,
+	.xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(V4L2_COLORSPACE_SRGB),
+	.field = V4L2_FIELD_NONE,
+};
+
+static const struct reg_value ov5640_init_setting[] = {
+	{0x3103, 0x11, 0, 0},
+	{0x3103, 0x03, 0, 0}, {0x3630, 0x36, 0, 0},
+	{0x3631, 0x0e, 0, 0}, {0x3632, 0xe2, 0, 0}, {0x3633, 0x12, 0, 0},
+	{0x3621, 0xe0, 0, 0}, {0x3704, 0xa0, 0, 0}, {0x3703, 0x5a, 0, 0},
+	{0x3715, 0x78, 0, 0}, {0x3717, 0x01, 0, 0}, {0x370b, 0x60, 0, 0},
+	{0x3705, 0x1a, 0, 0}, {0x3905, 0x02, 0, 0}, {0x3906, 0x10, 0, 0},
+	{0x3901, 0x0a, 0, 0}, {0x3731, 0x12, 0, 0}, {0x3600, 0x08, 0, 0},
+	{0x3601, 0x33, 0, 0}, {0x302d, 0x60, 0, 0}, {0x3620, 0x52, 0, 0},
+	{0x371b, 0x20, 0, 0}, {0x471c, 0x50, 0, 0}, {0x3a13, 0x43, 0, 0},
+	{0x3a18, 0x00, 0, 0}, {0x3a19, 0xf8, 0, 0}, {0x3635, 0x13, 0, 0},
+	{0x3636, 0x03, 0, 0}, {0x3634, 0x40, 0, 0}, {0x3622, 0x01, 0, 0},
+	{0x3c01, 0xa4, 0, 0}, {0x3c04, 0x28, 0, 0}, {0x3c05, 0x98, 0, 0},
+	{0x3c06, 0x00, 0, 0}, {0x3c07, 0x08, 0, 0}, {0x3c08, 0x00, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3820, 0x41, 0, 0}, {0x3821, 0x07, 0, 0}, {0x3814, 0x31, 0, 0},
+	{0x3815, 0x31, 0, 0},
+	{0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
+	{0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x3000, 0x00, 0, 0},
+	{0x3002, 0x1c, 0, 0}, {0x3004, 0xff, 0, 0}, {0x3006, 0xc3, 0, 0},
+	{0x302e, 0x08, 0, 0}, {0x4300, 0x3f, 0, 0},
+	{0x501f, 0x00, 0, 0}, {0x440e, 0x00, 0, 0}, {0x4837, 0x0a, 0, 0},
+	{0x5000, 0xa7, 0, 0}, {0x5001, 0xa3, 0, 0}, {0x5180, 0xff, 0, 0},
+	{0x5181, 0xf2, 0, 0}, {0x5182, 0x00, 0, 0}, {0x5183, 0x14, 0, 0},
+	{0x5184, 0x25, 0, 0}, {0x5185, 0x24, 0, 0}, {0x5186, 0x09, 0, 0},
+	{0x5187, 0x09, 0, 0}, {0x5188, 0x09, 0, 0}, {0x5189, 0x88, 0, 0},
+	{0x518a, 0x54, 0, 0}, {0x518b, 0xee, 0, 0}, {0x518c, 0xb2, 0, 0},
+	{0x518d, 0x50, 0, 0}, {0x518e, 0x34, 0, 0}, {0x518f, 0x6b, 0, 0},
+	{0x5190, 0x46, 0, 0}, {0x5191, 0xf8, 0, 0}, {0x5192, 0x04, 0, 0},
+	{0x5193, 0x70, 0, 0}, {0x5194, 0xf0, 0, 0}, {0x5195, 0xf0, 0, 0},
+	{0x5196, 0x03, 0, 0}, {0x5197, 0x01, 0, 0}, {0x5198, 0x04, 0, 0},
+	{0x5199, 0x6c, 0, 0}, {0x519a, 0x04, 0, 0}, {0x519b, 0x00, 0, 0},
+	{0x519c, 0x09, 0, 0}, {0x519d, 0x2b, 0, 0}, {0x519e, 0x38, 0, 0},
+	{0x5381, 0x1e, 0, 0}, {0x5382, 0x5b, 0, 0}, {0x5383, 0x08, 0, 0},
+	{0x5384, 0x0a, 0, 0}, {0x5385, 0x7e, 0, 0}, {0x5386, 0x88, 0, 0},
+	{0x5387, 0x7c, 0, 0}, {0x5388, 0x6c, 0, 0}, {0x5389, 0x10, 0, 0},
+	{0x538a, 0x01, 0, 0}, {0x538b, 0x98, 0, 0}, {0x5300, 0x08, 0, 0},
+	{0x5301, 0x30, 0, 0}, {0x5302, 0x10, 0, 0}, {0x5303, 0x00, 0, 0},
+	{0x5304, 0x08, 0, 0}, {0x5305, 0x30, 0, 0}, {0x5306, 0x08, 0, 0},
+	{0x5307, 0x16, 0, 0}, {0x5309, 0x08, 0, 0}, {0x530a, 0x30, 0, 0},
+	{0x530b, 0x04, 0, 0}, {0x530c, 0x06, 0, 0}, {0x5480, 0x01, 0, 0},
+	{0x5481, 0x08, 0, 0}, {0x5482, 0x14, 0, 0}, {0x5483, 0x28, 0, 0},
+	{0x5484, 0x51, 0, 0}, {0x5485, 0x65, 0, 0}, {0x5486, 0x71, 0, 0},
+	{0x5487, 0x7d, 0, 0}, {0x5488, 0x87, 0, 0}, {0x5489, 0x91, 0, 0},
+	{0x548a, 0x9a, 0, 0}, {0x548b, 0xaa, 0, 0}, {0x548c, 0xb8, 0, 0},
+	{0x548d, 0xcd, 0, 0}, {0x548e, 0xdd, 0, 0}, {0x548f, 0xea, 0, 0},
+	{0x5490, 0x1d, 0, 0}, {0x5580, 0x02, 0, 0}, {0x5583, 0x40, 0, 0},
+	{0x5584, 0x10, 0, 0}, {0x5589, 0x10, 0, 0}, {0x558a, 0x00, 0, 0},
+	{0x558b, 0xf8, 0, 0}, {0x5800, 0x23, 0, 0}, {0x5801, 0x14, 0, 0},
+	{0x5802, 0x0f, 0, 0}, {0x5803, 0x0f, 0, 0}, {0x5804, 0x12, 0, 0},
+	{0x5805, 0x26, 0, 0}, {0x5806, 0x0c, 0, 0}, {0x5807, 0x08, 0, 0},
+	{0x5808, 0x05, 0, 0}, {0x5809, 0x05, 0, 0}, {0x580a, 0x08, 0, 0},
+	{0x580b, 0x0d, 0, 0}, {0x580c, 0x08, 0, 0}, {0x580d, 0x03, 0, 0},
+	{0x580e, 0x00, 0, 0}, {0x580f, 0x00, 0, 0}, {0x5810, 0x03, 0, 0},
+	{0x5811, 0x09, 0, 0}, {0x5812, 0x07, 0, 0}, {0x5813, 0x03, 0, 0},
+	{0x5814, 0x00, 0, 0}, {0x5815, 0x01, 0, 0}, {0x5816, 0x03, 0, 0},
+	{0x5817, 0x08, 0, 0}, {0x5818, 0x0d, 0, 0}, {0x5819, 0x08, 0, 0},
+	{0x581a, 0x05, 0, 0}, {0x581b, 0x06, 0, 0}, {0x581c, 0x08, 0, 0},
+	{0x581d, 0x0e, 0, 0}, {0x581e, 0x29, 0, 0}, {0x581f, 0x17, 0, 0},
+	{0x5820, 0x11, 0, 0}, {0x5821, 0x11, 0, 0}, {0x5822, 0x15, 0, 0},
+	{0x5823, 0x28, 0, 0}, {0x5824, 0x46, 0, 0}, {0x5825, 0x26, 0, 0},
+	{0x5826, 0x08, 0, 0}, {0x5827, 0x26, 0, 0}, {0x5828, 0x64, 0, 0},
+	{0x5829, 0x26, 0, 0}, {0x582a, 0x24, 0, 0}, {0x582b, 0x22, 0, 0},
+	{0x582c, 0x24, 0, 0}, {0x582d, 0x24, 0, 0}, {0x582e, 0x06, 0, 0},
+	{0x582f, 0x22, 0, 0}, {0x5830, 0x40, 0, 0}, {0x5831, 0x42, 0, 0},
+	{0x5832, 0x24, 0, 0}, {0x5833, 0x26, 0, 0}, {0x5834, 0x24, 0, 0},
+	{0x5835, 0x22, 0, 0}, {0x5836, 0x22, 0, 0}, {0x5837, 0x26, 0, 0},
+	{0x5838, 0x44, 0, 0}, {0x5839, 0x24, 0, 0}, {0x583a, 0x26, 0, 0},
+	{0x583b, 0x28, 0, 0}, {0x583c, 0x42, 0, 0}, {0x583d, 0xce, 0, 0},
+	{0x5025, 0x00, 0, 0}, {0x3a0f, 0x30, 0, 0}, {0x3a10, 0x28, 0, 0},
+	{0x3a1b, 0x30, 0, 0}, {0x3a1e, 0x26, 0, 0}, {0x3a11, 0x60, 0, 0},
+	{0x3a1f, 0x14, 0, 0}, {0x3008, 0x02, 0, 0}, {0x3c00, 0x04, 0, 300},
+};
+
+static const struct reg_value ov5640_setting_low_res[] = {
+	{0x3c07, 0x08, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3814, 0x31, 0, 0},
+	{0x3815, 0x31, 0, 0},
+	{0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
+	{0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+	{0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
+};
+
+static const struct reg_value ov5640_setting_720P_1280_720[] = {
+	{0x3c07, 0x07, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3814, 0x31, 0, 0},
+	{0x3815, 0x31, 0, 0},
+	{0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x02, 0, 0},
+	{0x3a03, 0xe4, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0xbc, 0, 0},
+	{0x3a0a, 0x01, 0, 0}, {0x3a0b, 0x72, 0, 0}, {0x3a0e, 0x01, 0, 0},
+	{0x3a0d, 0x02, 0, 0}, {0x3a14, 0x02, 0, 0}, {0x3a15, 0xe4, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x37, 0, 0}, {0x460c, 0x20, 0, 0},
+	{0x3824, 0x04, 0, 0}, {0x5001, 0x83, 0, 0},
+};
+
+static const struct reg_value ov5640_setting_1080P_1920_1080[] = {
+	{0x3c07, 0x08, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3814, 0x11, 0, 0},
+	{0x3815, 0x11, 0, 0},
+	{0x3618, 0x04, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x21, 0, 0},
+	{0x3709, 0x12, 0, 0}, {0x370c, 0x00, 0, 0}, {0x3a02, 0x03, 0, 0},
+	{0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x06, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+	{0x3824, 0x02, 0, 0}, {0x5001, 0x83, 0, 0},
+	{0x3c07, 0x07, 0, 0}, {0x3c08, 0x00, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3612, 0x2b, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3a02, 0x04, 0, 0}, {0x3a03, 0x60, 0, 0}, {0x3a08, 0x01, 0, 0},
+	{0x3a09, 0x50, 0, 0}, {0x3a0a, 0x01, 0, 0}, {0x3a0b, 0x18, 0, 0},
+	{0x3a0e, 0x03, 0, 0}, {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x04, 0, 0},
+	{0x3a15, 0x60, 0, 0}, {0x4407, 0x04, 0, 0},
+	{0x460b, 0x37, 0, 0}, {0x460c, 0x20, 0, 0}, {0x3824, 0x04, 0, 0},
+	{0x4005, 0x1a, 0, 0},
+};
+
+static const struct reg_value ov5640_setting_QSXGA_2592_1944[] = {
+	{0x3c07, 0x08, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3814, 0x11, 0, 0},
+	{0x3815, 0x11, 0, 0},
+	{0x3618, 0x04, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x21, 0, 0},
+	{0x3709, 0x12, 0, 0}, {0x370c, 0x00, 0, 0}, {0x3a02, 0x03, 0, 0},
+	{0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x06, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+	{0x3824, 0x02, 0, 0}, {0x5001, 0x83, 0, 70},
+};
+
+static const struct ov5640_mode_info ov5640_mode_data[OV5640_NUM_MODES] = {
+	{
+		/* 160x120 */
+		.id		= OV5640_MODE_QQVGA_160_120,
+		.dn_mode	= SUBSAMPLING,
+		.pixel_rate	= OV5640_PIXEL_RATE_48M,
+		.width		= 160,
+		.height		= 120,
+		.dvp_timings = {
+			.analog_crop = {
+				.left	= 0,
+				.top	= 4,
+				.width	= 2624,
+				.height	= 1944,
+			},
+			.crop = {
+				.left	= 16,
+				.top	= 6,
+				.width	= 160,
+				.height	= 120,
+			},
+			.htot		= 1896,
+			.vblank_def	= 864,
+		},
+		.csi2_timings = {
+			/* Feed the full valid pixel array to the ISP. */
+			.analog_crop = {
+				.left	= OV5640_PIXEL_ARRAY_LEFT,
+				.top	= OV5640_PIXEL_ARRAY_TOP,
+				.width	= OV5640_PIXEL_ARRAY_WIDTH,
+				.height	= OV5640_PIXEL_ARRAY_HEIGHT,
+			},
+			/* Maintain a minimum processing margin. */
+			.crop = {
+				.left	= 2,
+				.top	= 4,
+				.width	= 160,
+				.height	= 120,
+			},
+			.htot		= 1600,
+			.vblank_def	= 878,
+		},
+		.reg_data	= ov5640_setting_low_res,
+		.reg_data_size	= ARRAY_SIZE(ov5640_setting_low_res),
+		.max_fps	= OV5640_30_FPS,
+		.def_fps	= OV5640_30_FPS
+	}, {
+		/* 176x144 */
+		.id		= OV5640_MODE_QCIF_176_144,
+		.dn_mode	= SUBSAMPLING,
+		.pixel_rate	= OV5640_PIXEL_RATE_48M,
+		.width		= 176,
+		.height		= 144,
+		.dvp_timings = {
+			.analog_crop = {
+				.left	= 0,
+				.top	= 4,
+				.width	= 2624,
+				.height	= 1944,
+			},
+			.crop = {
+				.left	= 16,
+				.top	= 6,
+				.width	= 176,
+				.height	= 144,
+			},
+			.htot		= 1896,
+			.vblank_def	= 840,
+		},
+		.csi2_timings = {
+			/* Feed the full valid pixel array to the ISP. */
+			.analog_crop = {
+				.left	= OV5640_PIXEL_ARRAY_LEFT,
+				.top	= OV5640_PIXEL_ARRAY_TOP,
+				.width	= OV5640_PIXEL_ARRAY_WIDTH,
+				.height	= OV5640_PIXEL_ARRAY_HEIGHT,
+			},
+			/* Maintain a minimum processing margin. */
+			.crop = {
+				.left	= 2,
+				.top	= 4,
+				.width	= 176,
+				.height	= 144,
+			},
+			.htot		= 1600,
+			.vblank_def	= 854,
+		},
+		.reg_data	= ov5640_setting_low_res,
+		.reg_data_size	= ARRAY_SIZE(ov5640_setting_low_res),
+		.max_fps	= OV5640_30_FPS,
+		.def_fps	= OV5640_30_FPS
+	}, {
+		/* 320x240 */
+		.id		= OV5640_MODE_QVGA_320_240,
+		.dn_mode	= SUBSAMPLING,
+		.width		= 320,
+		.height		= 240,
+		.pixel_rate	= OV5640_PIXEL_RATE_48M,
+		.dvp_timings = {
+			.analog_crop = {
+				.left	= 0,
+				.top	= 4,
+				.width	= 2624,
+				.height	= 1944,
+			},
+			.crop = {
+				.left	= 16,
+				.top	= 6,
+				.width	= 320,
+				.height	= 240,
+			},
+			.htot		= 1896,
+			.vblank_def	= 744,
+		},
+		.csi2_timings = {
+			/* Feed the full valid pixel array to the ISP. */
+			.analog_crop = {
+				.left	= OV5640_PIXEL_ARRAY_LEFT,
+				.top	= OV5640_PIXEL_ARRAY_TOP,
+				.width	= OV5640_PIXEL_ARRAY_WIDTH,
+				.height	= OV5640_PIXEL_ARRAY_HEIGHT,
+			},
+			/* Maintain a minimum processing margin. */
+			.crop = {
+				.left	= 2,
+				.top	= 4,
+				.width	= 320,
+				.height	= 240,
+			},
+			.htot		= 1600,
+			.vblank_def	= 760,
+		},
+		.reg_data	= ov5640_setting_low_res,
+		.reg_data_size	= ARRAY_SIZE(ov5640_setting_low_res),
+		.max_fps	= OV5640_30_FPS,
+		.def_fps	= OV5640_30_FPS
+	}, {
+		/* 640x480 */
+		.id		= OV5640_MODE_VGA_640_480,
+		.dn_mode	= SUBSAMPLING,
+		.pixel_rate	= OV5640_PIXEL_RATE_48M,
+		.width		= 640,
+		.height		= 480,
+		.dvp_timings = {
+			.analog_crop = {
+				.left	= 0,
+				.top	= 4,
+				.width	= 2624,
+				.height	= 1944,
+			},
+			.crop = {
+				.left	= 16,
+				.top	= 6,
+				.width	= 640,
+				.height	= 480,
+			},
+			.htot		= 1896,
+			.vblank_def	= 600,
+		},
+		.csi2_timings = {
+			/* Feed the full valid pixel array to the ISP. */
+			.analog_crop = {
+				.left	= OV5640_PIXEL_ARRAY_LEFT,
+				.top	= OV5640_PIXEL_ARRAY_TOP,
+				.width	= OV5640_PIXEL_ARRAY_WIDTH,
+				.height	= OV5640_PIXEL_ARRAY_HEIGHT,
+			},
+			/* Maintain a minimum processing margin. */
+			.crop = {
+				.left	= 2,
+				.top	= 4,
+				.width	= 640,
+				.height	= 480,
+			},
+			.htot		= 1600,
+			.vblank_def	= 520,
+		},
+		.reg_data	= ov5640_setting_low_res,
+		.reg_data_size	= ARRAY_SIZE(ov5640_setting_low_res),
+		.max_fps	= OV5640_60_FPS,
+		.def_fps	= OV5640_30_FPS
+	}, {
+		/* 720x480 */
+		.id		= OV5640_MODE_NTSC_720_480,
+		.dn_mode	= SUBSAMPLING,
+		.width		= 720,
+		.height		= 480,
+		.pixel_rate	= OV5640_PIXEL_RATE_96M,
+		.dvp_timings = {
+			.analog_crop = {
+				.left	= 0,
+				.top	= 4,
+				.width	= 2624,
+				.height	= 1944,
+			},
+			.crop = {
+				.left	= 56,
+				.top	= 60,
+				.width	= 720,
+				.height	= 480,
+			},
+			.htot		= 1896,
+			.vblank_def	= 504,
+		},
+		.csi2_timings = {
+			/* Feed the full valid pixel array to the ISP. */
+			.analog_crop = {
+				.left	= OV5640_PIXEL_ARRAY_LEFT,
+				.top	= OV5640_PIXEL_ARRAY_TOP,
+				.width	= OV5640_PIXEL_ARRAY_WIDTH,
+				.height	= OV5640_PIXEL_ARRAY_HEIGHT,
+			},
+			.crop = {
+				.left	= 56,
+				.top	= 60,
+				.width	= 720,
+				.height	= 480,
+			},
+			.htot		= 1896,
+			.vblank_def	= 1206,
+		},
+		.reg_data	= ov5640_setting_low_res,
+		.reg_data_size	= ARRAY_SIZE(ov5640_setting_low_res),
+		.max_fps	= OV5640_30_FPS,
+		.def_fps	= OV5640_30_FPS
+	}, {
+		/* 720x576 */
+		.id		= OV5640_MODE_PAL_720_576,
+		.dn_mode	= SUBSAMPLING,
+		.width		= 720,
+		.height		= 576,
+		.pixel_rate	= OV5640_PIXEL_RATE_96M,
+		.dvp_timings = {
+			.analog_crop = {
+				.left	= 0,
+				.top	= 4,
+				.width	= 2624,
+				.height	= 1944,
+			},
+			.crop = {
+				.left	= 56,
+				.top	= 6,
+				.width	= 720,
+				.height	= 576,
+			},
+			.htot		= 1896,
+			.vblank_def	= 408,
+		},
+		.csi2_timings = {
+			/* Feed the full valid pixel array to the ISP. */
+			.analog_crop = {
+				.left	= OV5640_PIXEL_ARRAY_LEFT,
+				.top	= OV5640_PIXEL_ARRAY_TOP,
+				.width	= OV5640_PIXEL_ARRAY_WIDTH,
+				.height	= OV5640_PIXEL_ARRAY_HEIGHT,
+			},
+			.crop = {
+				.left	= 56,
+				.top	= 6,
+				.width	= 720,
+				.height	= 576,
+			},
+			.htot		= 1896,
+			.vblank_def	= 1110,
+		},
+		.reg_data	= ov5640_setting_low_res,
+		.reg_data_size	= ARRAY_SIZE(ov5640_setting_low_res),
+		.max_fps	= OV5640_30_FPS,
+		.def_fps	= OV5640_30_FPS
+	}, {
+		/* 1024x768 */
+		.id		= OV5640_MODE_XGA_1024_768,
+		.dn_mode	= SUBSAMPLING,
+		.pixel_rate	= OV5640_PIXEL_RATE_96M,
+		.width		= 1024,
+		.height		= 768,
+		.dvp_timings = {
+			.analog_crop = {
+				.left	= 0,
+				.top	= 4,
+				.width	= 2624,
+				.height	= 1944,
+			},
+			.crop = {
+				.left	= 16,
+				.top	= 6,
+				.width	= 1024,
+				.height	= 768,
+			},
+			.htot		= 1896,
+			.vblank_def	= 312,
+		},
+		.csi2_timings = {
+			.analog_crop = {
+				.left	= 0,
+				.top	= 4,
+				.width	= OV5640_NATIVE_WIDTH,
+				.height	= OV5640_PIXEL_ARRAY_HEIGHT,
+			},
+			.crop = {
+				.left	= 16,
+				.top	= 6,
+				.width	= 1024,
+				.height	= 768,
+			},
+			.htot		= 1896,
+			.vblank_def	= 918,
+		},
+		.reg_data	= ov5640_setting_low_res,
+		.reg_data_size	= ARRAY_SIZE(ov5640_setting_low_res),
+		.max_fps	= OV5640_30_FPS,
+		.def_fps	= OV5640_30_FPS
+	}, {
+		/* 1280x720 */
+		.id		= OV5640_MODE_720P_1280_720,
+		.dn_mode	= SUBSAMPLING,
+		.pixel_rate	= OV5640_PIXEL_RATE_124M,
+		.width		= 1280,
+		.height		= 720,
+		.dvp_timings = {
+			.analog_crop = {
+				.left	= 0,
+				.top	= 250,
+				.width	= 2624,
+				.height	= 1456,
+			},
+			.crop = {
+				.left	= 16,
+				.top	= 4,
+				.width	= 1280,
+				.height	= 720,
+			},
+			.htot		= 1892,
+			.vblank_def	= 20,
+		},
+		.csi2_timings = {
+			.analog_crop = {
+				.left	= 0,
+				.top	= 250,
+				.width	= 2624,
+				.height	= 1456,
+			},
+			.crop = {
+				.left	= 16,
+				.top	= 4,
+				.width	= 1280,
+				.height	= 720,
+			},
+			.htot		= 1600,
+			.vblank_def	= 560,
+		},
+		.reg_data	= ov5640_setting_720P_1280_720,
+		.reg_data_size	= ARRAY_SIZE(ov5640_setting_720P_1280_720),
+		.max_fps	= OV5640_30_FPS,
+		.def_fps	= OV5640_30_FPS
+	}, {
+		/* 1920x1080 */
+		.id		= OV5640_MODE_1080P_1920_1080,
+		.dn_mode	= SCALING,
+		.pixel_rate	= OV5640_PIXEL_RATE_148M,
+		.width		= 1920,
+		.height		= 1080,
+		.dvp_timings = {
+			.analog_crop = {
+				.left	= 336,
+				.top	= 434,
+				.width	= 1952,
+				.height	= 1088,
+			},
+			.crop = {
+				.left	= 16,
+				.top	= 4,
+				.width	= 1920,
+				.height	= 1080,
+			},
+			.htot		= 2500,
+			.vblank_def	= 40,
+		},
+		.csi2_timings = {
+			/* Crop the full valid pixel array in the center. */
+			.analog_crop = {
+				.left	= 336,
+				.top	= 434,
+				.width	= 1952,
+				.height	= 1088,
+			},
+			/* Maintain a larger processing margins. */
+			.crop = {
+				.left	= 16,
+				.top	= 4,
+				.width	= 1920,
+				.height	= 1080,
+			},
+			.htot		= 2234,
+			.vblank_def	= 24,
+		},
+		.reg_data	= ov5640_setting_1080P_1920_1080,
+		.reg_data_size	= ARRAY_SIZE(ov5640_setting_1080P_1920_1080),
+		.max_fps	= OV5640_30_FPS,
+		.def_fps	= OV5640_30_FPS
+	}, {
+		/* 2592x1944 */
+		.id		= OV5640_MODE_QSXGA_2592_1944,
+		.dn_mode	= SCALING,
+		.pixel_rate	= OV5640_PIXEL_RATE_168M,
+		.width		= OV5640_PIXEL_ARRAY_WIDTH,
+		.height		= OV5640_PIXEL_ARRAY_HEIGHT,
+		.dvp_timings = {
+			.analog_crop = {
+				.left	= 0,
+				.top	= 0,
+				.width	= 2624,
+				.height	= 1952,
+			},
+			.crop = {
+				.left	= 16,
+				.top	= 4,
+				.width	= 2592,
+				.height	= 1944,
+			},
+			.htot		= 2844,
+			.vblank_def	= 24,
+		},
+		.csi2_timings = {
+			/* Give more processing margin to full resolution. */
+			.analog_crop = {
+				.left	= 0,
+				.top	= 0,
+				.width	= OV5640_NATIVE_WIDTH,
+				.height	= 1952,
+			},
+			.crop = {
+				.left	= 16,
+				.top	= 4,
+				.width	= 2592,
+				.height	= 1944,
+			},
+			.htot		= 2844,
+			.vblank_def	= 24,
+		},
+		.reg_data	= ov5640_setting_QSXGA_2592_1944,
+		.reg_data_size	= ARRAY_SIZE(ov5640_setting_QSXGA_2592_1944),
+		.max_fps	= OV5640_15_FPS,
+		.def_fps	= OV5640_15_FPS
+	},
+};
+
+static const struct ov5640_timings *
+ov5640_timings(const struct ov5640_dev *sensor,
+	       const struct ov5640_mode_info *mode)
+{
+	if (ov5640_is_csi2(sensor))
+		return &mode->csi2_timings;
+
+	return &mode->dvp_timings;
+}
+
+static int ov5640_init_slave_id(struct ov5640_dev *sensor)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	struct i2c_msg msg;
+	u8 buf[3];
+	int ret;
+
+	if (client->addr == OV5640_DEFAULT_SLAVE_ID)
+		return 0;
+
+	buf[0] = OV5640_REG_SLAVE_ID >> 8;
+	buf[1] = OV5640_REG_SLAVE_ID & 0xff;
+	buf[2] = client->addr << 1;
+
+	msg.addr = OV5640_DEFAULT_SLAVE_ID;
+	msg.flags = 0;
+	msg.buf = buf;
+	msg.len = sizeof(buf);
+
+	ret = i2c_transfer(client->adapter, &msg, 1);
+	if (ret < 0) {
+		dev_err(&client->dev, "%s: failed with %d\n", __func__, ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ov5640_write_reg(struct ov5640_dev *sensor, u16 reg, u8 val)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	struct i2c_msg msg;
+	u8 buf[3];
+	int ret;
+
+	buf[0] = reg >> 8;
+	buf[1] = reg & 0xff;
+	buf[2] = val;
+
+	msg.addr = client->addr;
+	msg.flags = client->flags;
+	msg.buf = buf;
+	msg.len = sizeof(buf);
+
+	ret = i2c_transfer(client->adapter, &msg, 1);
+	if (ret < 0) {
+		dev_err(&client->dev, "%s: error: reg=%x, val=%x\n",
+			__func__, reg, val);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ov5640_read_reg(struct ov5640_dev *sensor, u16 reg, u8 *val)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	struct i2c_msg msg[2];
+	u8 buf[2];
+	int ret;
+
+	buf[0] = reg >> 8;
+	buf[1] = reg & 0xff;
+
+	msg[0].addr = client->addr;
+	msg[0].flags = client->flags;
+	msg[0].buf = buf;
+	msg[0].len = sizeof(buf);
+
+	msg[1].addr = client->addr;
+	msg[1].flags = client->flags | I2C_M_RD;
+	msg[1].buf = buf;
+	msg[1].len = 1;
+
+	ret = i2c_transfer(client->adapter, msg, 2);
+	if (ret < 0) {
+		dev_err(&client->dev, "%s: error: reg=%x\n",
+			__func__, reg);
+		return ret;
+	}
+
+	*val = buf[0];
+	return 0;
+}
+
+static int ov5640_read_reg16(struct ov5640_dev *sensor, u16 reg, u16 *val)
+{
+	u8 hi, lo;
+	int ret;
+
+	ret = ov5640_read_reg(sensor, reg, &hi);
+	if (ret)
+		return ret;
+	ret = ov5640_read_reg(sensor, reg + 1, &lo);
+	if (ret)
+		return ret;
+
+	*val = ((u16)hi << 8) | (u16)lo;
+	return 0;
+}
+
+static int ov5640_write_reg16(struct ov5640_dev *sensor, u16 reg, u16 val)
+{
+	int ret;
+
+	ret = ov5640_write_reg(sensor, reg, val >> 8);
+	if (ret)
+		return ret;
+
+	return ov5640_write_reg(sensor, reg + 1, val & 0xff);
+}
+
+static int ov5640_mod_reg(struct ov5640_dev *sensor, u16 reg,
+			  u8 mask, u8 val)
+{
+	u8 readval;
+	int ret;
+
+	ret = ov5640_read_reg(sensor, reg, &readval);
+	if (ret)
+		return ret;
+
+	readval &= ~mask;
+	val &= mask;
+	val |= readval;
+
+	return ov5640_write_reg(sensor, reg, val);
+}
+
+/*
+ * After trying the various combinations, reading various
+ * documentations spread around the net, and from the various
+ * feedback, the clock tree is probably as follows:
+ *
+ *   +--------------+
+ *   |  Ext. Clock  |
+ *   +-+------------+
+ *     |  +----------+
+ *     +->|   PLL1   | - reg 0x3036, for the multiplier
+ *        +-+--------+ - reg 0x3037, bits 0-3 for the pre-divider
+ *          |  +--------------+
+ *          +->| System Clock |  - reg 0x3035, bits 4-7
+ *             +-+------------+
+ *               |  +--------------+
+ *               +->| MIPI Divider | - reg 0x3035, bits 0-3
+ *               |  +-+------------+
+ *               |    +----------------> MIPI SCLK
+ *               |    +  +-----+
+ *               |    +->| / 2 |-------> MIPI BIT CLK
+ *               |       +-----+
+ *               |  +--------------+
+ *               +->| PLL Root Div | - reg 0x3037, bit 4
+ *                  +-+------------+
+ *                    |  +---------+
+ *                    +->| Bit Div | - reg 0x3034, bits 0-3
+ *                       +-+-------+
+ *                         |  +-------------+
+ *                         +->| SCLK Div    | - reg 0x3108, bits 0-1
+ *                         |  +-+-----------+
+ *                         |    +---------------> SCLK
+ *                         |  +-------------+
+ *                         +->| SCLK 2X Div | - reg 0x3108, bits 2-3
+ *                         |  +-+-----------+
+ *                         |    +---------------> SCLK 2X
+ *                         |  +-------------+
+ *                         +->| PCLK Div    | - reg 0x3108, bits 4-5
+ *                            ++------------+
+ *                             +  +-----------+
+ *                             +->|   P_DIV   | - reg 0x3035, bits 0-3
+ *                                +-----+-----+
+ *                                       +------------> PCLK
+ *
+ * There seems to be also constraints:
+ *  - the PLL pre-divider output rate should be in the 4-27MHz range
+ *  - the PLL multiplier output rate should be in the 500-1000MHz range
+ *  - PCLK >= SCLK * 2 in YUV, >= SCLK in Raw or JPEG
+ */
+
+/*
+ * This is supposed to be ranging from 1 to 8, but the value is always
+ * set to 3 in the vendor kernels.
+ */
+#define OV5640_PLL_PREDIV	3
+
+#define OV5640_PLL_MULT_MIN	4
+#define OV5640_PLL_MULT_MAX	252
+
+/*
+ * This is supposed to be ranging from 1 to 16, but the value is
+ * always set to either 1 or 2 in the vendor kernels.
+ */
+#define OV5640_SYSDIV_MIN	1
+#define OV5640_SYSDIV_MAX	16
+
+/*
+ * This is supposed to be ranging from 1 to 2, but the value is always
+ * set to 2 in the vendor kernels.
+ */
+#define OV5640_PLL_ROOT_DIV			2
+#define OV5640_PLL_CTRL3_PLL_ROOT_DIV_2		BIT(4)
+
+/*
+ * We only supports 8-bit formats at the moment
+ */
+#define OV5640_BIT_DIV				2
+#define OV5640_PLL_CTRL0_MIPI_MODE_8BIT		0x08
+
+/*
+ * This is supposed to be ranging from 1 to 8, but the value is always
+ * set to 2 in the vendor kernels.
+ */
+#define OV5640_SCLK_ROOT_DIV	2
+
+/*
+ * This is hardcoded so that the consistency is maintained between SCLK and
+ * SCLK 2x.
+ */
+#define OV5640_SCLK2X_ROOT_DIV (OV5640_SCLK_ROOT_DIV / 2)
+
+/*
+ * This is supposed to be ranging from 1 to 8, but the value is always
+ * set to 1 in the vendor kernels.
+ */
+#define OV5640_PCLK_ROOT_DIV			1
+#define OV5640_PLL_SYS_ROOT_DIVIDER_BYPASS	0x00
+
+static unsigned long ov5640_compute_sys_clk(struct ov5640_dev *sensor,
+					    u8 pll_prediv, u8 pll_mult,
+					    u8 sysdiv)
+{
+	unsigned long sysclk = sensor->xclk_freq / pll_prediv * pll_mult;
+
+	/* PLL1 output cannot exceed 1GHz. */
+	if (sysclk / 1000000 > 1000)
+		return 0;
+
+	return sysclk / sysdiv;
+}
+
+static unsigned long ov5640_calc_sys_clk(struct ov5640_dev *sensor,
+					 unsigned long rate,
+					 u8 *pll_prediv, u8 *pll_mult,
+					 u8 *sysdiv)
+{
+	unsigned long best = ~0;
+	u8 best_sysdiv = 1, best_mult = 1;
+	u8 _sysdiv, _pll_mult;
+
+	for (_sysdiv = OV5640_SYSDIV_MIN;
+	     _sysdiv <= OV5640_SYSDIV_MAX;
+	     _sysdiv++) {
+		for (_pll_mult = OV5640_PLL_MULT_MIN;
+		     _pll_mult <= OV5640_PLL_MULT_MAX;
+		     _pll_mult++) {
+			unsigned long _rate;
+
+			/*
+			 * The PLL multiplier cannot be odd if above
+			 * 127.
+			 */
+			if (_pll_mult > 127 && (_pll_mult % 2))
+				continue;
+
+			_rate = ov5640_compute_sys_clk(sensor,
+						       OV5640_PLL_PREDIV,
+						       _pll_mult, _sysdiv);
+
+			/*
+			 * We have reached the maximum allowed PLL1 output,
+			 * increase sysdiv.
+			 */
+			if (!_rate)
+				break;
+
+			/*
+			 * Prefer rates above the expected clock rate than
+			 * below, even if that means being less precise.
+			 */
+			if (_rate < rate)
+				continue;
+
+			if (abs(rate - _rate) < abs(rate - best)) {
+				best = _rate;
+				best_sysdiv = _sysdiv;
+				best_mult = _pll_mult;
+			}
+
+			if (_rate == rate)
+				goto out;
+		}
+	}
+
+out:
+	*sysdiv = best_sysdiv;
+	*pll_prediv = OV5640_PLL_PREDIV;
+	*pll_mult = best_mult;
+
+	return best;
+}
+
+/*
+ * ov5640_set_mipi_pclk() - Calculate the clock tree configuration values
+ *			    for the MIPI CSI-2 output.
+ */
+static int ov5640_set_mipi_pclk(struct ov5640_dev *sensor)
+{
+	u8 bit_div, mipi_div, pclk_div, sclk_div, sclk2x_div, root_div;
+	u8 prediv, mult, sysdiv;
+	unsigned long link_freq;
+	unsigned long sysclk;
+	u8 pclk_period;
+	u32 sample_rate;
+	u32 num_lanes;
+	int ret;
+
+	/* Use the link freq computed at ov5640_update_pixel_rate() time. */
+	link_freq = sensor->current_link_freq;
+
+	/*
+	 * - mipi_div - Additional divider for the MIPI lane clock.
+	 *
+	 * Higher link frequencies would make sysclk > 1GHz.
+	 * Keep the sysclk low and do not divide in the MIPI domain.
+	 */
+	if (link_freq > OV5640_LINK_RATE_MAX)
+		mipi_div = 1;
+	else
+		mipi_div = 2;
+
+	sysclk = link_freq * mipi_div;
+	ov5640_calc_sys_clk(sensor, sysclk, &prediv, &mult, &sysdiv);
+
+	/*
+	 * Adjust PLL parameters to maintain the MIPI_SCLK-to-PCLK ratio.
+	 *
+	 * - root_div = 2 (fixed)
+	 * - bit_div : MIPI 8-bit = 2; MIPI 10-bit = 2.5
+	 * - pclk_div = 1 (fixed)
+	 * - p_div  = (2 lanes ? mipi_div : 2 * mipi_div)
+	 *
+	 * This results in the following MIPI_SCLK depending on the number
+	 * of lanes:
+	 *
+	 * - 2 lanes: MIPI_SCLK = (4 or 5) * PCLK
+	 * - 1 lanes: MIPI_SCLK = (8 or 10) * PCLK
+	 */
+	root_div = OV5640_PLL_CTRL3_PLL_ROOT_DIV_2;
+	bit_div =  OV5640_PLL_CTRL0_MIPI_MODE_8BIT;
+	pclk_div = ilog2(OV5640_PCLK_ROOT_DIV);
+
+	/*
+	 * Scaler clock:
+	 * - YUV: PCLK >= 2 * SCLK
+	 * - RAW or JPEG: PCLK >= SCLK
+	 * - sclk2x_div = sclk_div / 2
+	 */
+	sclk_div = ilog2(OV5640_SCLK_ROOT_DIV);
+	sclk2x_div = ilog2(OV5640_SCLK2X_ROOT_DIV);
+
+	/*
+	 * Set the pixel clock period expressed in ns with 1-bit decimal
+	 * (0x01=0.5ns).
+	 *
+	 * The register is very briefly documented. In the OV5645 datasheet it
+	 * is described as (2 * pclk period), and from testing it seems the
+	 * actual definition is 2 * 8-bit sample period.
+	 *
+	 * 2 * sample_period = (mipi_clk * 2 * num_lanes / bpp) * (bpp / 8) / 2
+	 */
+	num_lanes = sensor->ep.bus.mipi_csi2.num_data_lanes;
+	sample_rate = (link_freq * mipi_div * num_lanes * 2) / 16;
+	pclk_period = 2000000000UL / sample_rate;
+
+	/* Program the clock tree registers. */
+	ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL0, 0x0f, bit_div);
+	if (ret)
+		return ret;
+
+	ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL1, 0xff,
+			     (sysdiv << 4) | mipi_div);
+	if (ret)
+		return ret;
+
+	ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL2, 0xff, mult);
+	if (ret)
+		return ret;
+
+	ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL3, 0x1f,
+			     root_div | prediv);
+	if (ret)
+		return ret;
+
+	ret = ov5640_mod_reg(sensor, OV5640_REG_SYS_ROOT_DIVIDER, 0x3f,
+			     (pclk_div << 4) | (sclk2x_div << 2) | sclk_div);
+	if (ret)
+		return ret;
+
+	return ov5640_write_reg(sensor, OV5640_REG_PCLK_PERIOD, pclk_period);
+}
+
+static u32 ov5640_calc_pixel_rate(struct ov5640_dev *sensor)
+{
+	const struct ov5640_mode_info *mode = sensor->current_mode;
+	const struct ov5640_timings *timings = &mode->dvp_timings;
+	u32 rate;
+
+	rate = timings->htot * (timings->crop.height + timings->vblank_def);
+	rate *= ov5640_framerates[sensor->current_fr];
+
+	return rate;
+}
+
+static unsigned long ov5640_calc_pclk(struct ov5640_dev *sensor,
+				      unsigned long rate,
+				      u8 *pll_prediv, u8 *pll_mult, u8 *sysdiv,
+				      u8 *pll_rdiv, u8 *bit_div, u8 *pclk_div)
+{
+	unsigned long _rate = rate * OV5640_PLL_ROOT_DIV * OV5640_BIT_DIV *
+				OV5640_PCLK_ROOT_DIV;
+
+	_rate = ov5640_calc_sys_clk(sensor, _rate, pll_prediv, pll_mult,
+				    sysdiv);
+	*pll_rdiv = OV5640_PLL_ROOT_DIV;
+	*bit_div = OV5640_BIT_DIV;
+	*pclk_div = OV5640_PCLK_ROOT_DIV;
+
+	return _rate / *pll_rdiv / *bit_div / *pclk_div;
+}
+
+static int ov5640_set_dvp_pclk(struct ov5640_dev *sensor)
+{
+	u8 prediv, mult, sysdiv, pll_rdiv, bit_div, pclk_div;
+	u32 rate;
+	int ret;
+
+	rate = ov5640_calc_pixel_rate(sensor);
+	rate *= ov5640_code_to_bpp(sensor, sensor->fmt.code);
+	rate /= sensor->ep.bus.parallel.bus_width;
+
+	ov5640_calc_pclk(sensor, rate, &prediv, &mult, &sysdiv, &pll_rdiv,
+			 &bit_div, &pclk_div);
+
+	if (bit_div == 2)
+		bit_div = 8;
+
+	ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL0,
+			     0x0f, bit_div);
+	if (ret)
+		return ret;
+
+	/*
+	 * We need to set sysdiv according to the clock, and to clear
+	 * the MIPI divider.
+	 */
+	ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL1,
+			     0xff, sysdiv << 4);
+	if (ret)
+		return ret;
+
+	ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL2,
+			     0xff, mult);
+	if (ret)
+		return ret;
+
+	ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL3,
+			     0x1f, prediv | ((pll_rdiv - 1) << 4));
+	if (ret)
+		return ret;
+
+	return ov5640_mod_reg(sensor, OV5640_REG_SYS_ROOT_DIVIDER, 0x30,
+			      (ilog2(pclk_div) << 4));
+}
+
+/* set JPEG framing sizes */
+static int ov5640_set_jpeg_timings(struct ov5640_dev *sensor,
+				   const struct ov5640_mode_info *mode)
+{
+	int ret;
+
+	/*
+	 * compression mode 3 timing
+	 *
+	 * Data is transmitted with programmable width (VFIFO_HSIZE).
+	 * No padding done. Last line may have less data. Varying
+	 * number of lines per frame, depending on amount of data.
+	 */
+	ret = ov5640_mod_reg(sensor, OV5640_REG_JPG_MODE_SELECT, 0x7, 0x3);
+	if (ret < 0)
+		return ret;
+
+	ret = ov5640_write_reg16(sensor, OV5640_REG_VFIFO_HSIZE, mode->width);
+	if (ret < 0)
+		return ret;
+
+	return ov5640_write_reg16(sensor, OV5640_REG_VFIFO_VSIZE, mode->height);
+}
+
+/* download ov5640 settings to sensor through i2c */
+static int ov5640_set_timings(struct ov5640_dev *sensor,
+			      const struct ov5640_mode_info *mode)
+{
+	const struct ov5640_timings *timings;
+	const struct v4l2_rect *analog_crop;
+	const struct v4l2_rect *crop;
+	int ret;
+
+	if (sensor->fmt.code == MEDIA_BUS_FMT_JPEG_1X8) {
+		ret = ov5640_set_jpeg_timings(sensor, mode);
+		if (ret < 0)
+			return ret;
+	}
+
+	timings = ov5640_timings(sensor, mode);
+	analog_crop = &timings->analog_crop;
+	crop = &timings->crop;
+
+	ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_HS,
+				 analog_crop->left);
+	if (ret < 0)
+		return ret;
+
+	ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_VS,
+				 analog_crop->top);
+	if (ret < 0)
+		return ret;
+
+	ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_HW,
+				 analog_crop->left + analog_crop->width - 1);
+	if (ret < 0)
+		return ret;
+
+	ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_VH,
+				 analog_crop->top + analog_crop->height - 1);
+	if (ret < 0)
+		return ret;
+
+	ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_HOFFS, crop->left);
+	if (ret < 0)
+		return ret;
+
+	ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_VOFFS, crop->top);
+	if (ret < 0)
+		return ret;
+
+	ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_DVPHO, mode->width);
+	if (ret < 0)
+		return ret;
+
+	ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_DVPVO, mode->height);
+	if (ret < 0)
+		return ret;
+
+	ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_HTS, timings->htot);
+	if (ret < 0)
+		return ret;
+
+	ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_VTS,
+				 mode->height + timings->vblank_def);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+static void ov5640_load_regs(struct ov5640_dev *sensor,
+			     const struct reg_value *regs, unsigned int regnum)
+{
+	unsigned int i;
+	u32 delay_ms;
+	u16 reg_addr;
+	u8 mask, val;
+	int ret = 0;
+
+	for (i = 0; i < regnum; ++i, ++regs) {
+		delay_ms = regs->delay_ms;
+		reg_addr = regs->reg_addr;
+		val = regs->val;
+		mask = regs->mask;
+
+		/* remain in power down mode for DVP */
+		if (regs->reg_addr == OV5640_REG_SYS_CTRL0 &&
+		    val == OV5640_REG_SYS_CTRL0_SW_PWUP &&
+		    !ov5640_is_csi2(sensor))
+			continue;
+
+		if (mask)
+			ret = ov5640_mod_reg(sensor, reg_addr, mask, val);
+		else
+			ret = ov5640_write_reg(sensor, reg_addr, val);
+		if (ret)
+			break;
+
+		if (delay_ms)
+			usleep_range(1000 * delay_ms, 1000 * delay_ms + 100);
+	}
+}
+
+static int ov5640_set_autoexposure(struct ov5640_dev *sensor, bool on)
+{
+	return ov5640_mod_reg(sensor, OV5640_REG_AEC_PK_MANUAL,
+			      BIT(0), on ? 0 : BIT(0));
+}
+
+/* read exposure, in number of line periods */
+static int ov5640_get_exposure(struct ov5640_dev *sensor)
+{
+	int exp, ret;
+	u8 temp;
+
+	ret = ov5640_read_reg(sensor, OV5640_REG_AEC_PK_EXPOSURE_HI, &temp);
+	if (ret)
+		return ret;
+	exp = ((int)temp & 0x0f) << 16;
+	ret = ov5640_read_reg(sensor, OV5640_REG_AEC_PK_EXPOSURE_MED, &temp);
+	if (ret)
+		return ret;
+	exp |= ((int)temp << 8);
+	ret = ov5640_read_reg(sensor, OV5640_REG_AEC_PK_EXPOSURE_LO, &temp);
+	if (ret)
+		return ret;
+	exp |= (int)temp;
+
+	return exp >> 4;
+}
+
+/* write exposure, given number of line periods */
+static int ov5640_set_exposure(struct ov5640_dev *sensor, u32 exposure)
+{
+	int ret;
+
+	exposure <<= 4;
+
+	ret = ov5640_write_reg(sensor,
+			       OV5640_REG_AEC_PK_EXPOSURE_LO,
+			       exposure & 0xff);
+	if (ret)
+		return ret;
+	ret = ov5640_write_reg(sensor,
+			       OV5640_REG_AEC_PK_EXPOSURE_MED,
+			       (exposure >> 8) & 0xff);
+	if (ret)
+		return ret;
+	return ov5640_write_reg(sensor,
+				OV5640_REG_AEC_PK_EXPOSURE_HI,
+				(exposure >> 16) & 0x0f);
+}
+
+static int ov5640_get_gain(struct ov5640_dev *sensor)
+{
+	u16 gain;
+	int ret;
+
+	ret = ov5640_read_reg16(sensor, OV5640_REG_AEC_PK_REAL_GAIN, &gain);
+	if (ret)
+		return ret;
+
+	return gain & 0x3ff;
+}
+
+static int ov5640_set_gain(struct ov5640_dev *sensor, int gain)
+{
+	return ov5640_write_reg16(sensor, OV5640_REG_AEC_PK_REAL_GAIN,
+				  (u16)gain & 0x3ff);
+}
+
+static int ov5640_set_autogain(struct ov5640_dev *sensor, bool on)
+{
+	return ov5640_mod_reg(sensor, OV5640_REG_AEC_PK_MANUAL,
+			      BIT(1), on ? 0 : BIT(1));
+}
+
+static int ov5640_set_stream_dvp(struct ov5640_dev *sensor, bool on)
+{
+	return ov5640_write_reg(sensor, OV5640_REG_SYS_CTRL0, on ?
+				OV5640_REG_SYS_CTRL0_SW_PWUP :
+				OV5640_REG_SYS_CTRL0_SW_PWDN);
+}
+
+static int ov5640_set_stream_mipi(struct ov5640_dev *sensor, bool on)
+{
+	int ret;
+
+	/*
+	 * Enable/disable the MIPI interface
+	 *
+	 * 0x300e = on ? 0x45 : 0x40
+	 *
+	 * FIXME: the sensor manual (version 2.03) reports
+	 * [7:5] = 000  : 1 data lane mode
+	 * [7:5] = 001  : 2 data lanes mode
+	 * But this settings do not work, while the following ones
+	 * have been validated for 2 data lanes mode.
+	 *
+	 * [7:5] = 010	: 2 data lanes mode
+	 * [4] = 0	: Power up MIPI HS Tx
+	 * [3] = 0	: Power up MIPI LS Rx
+	 * [2] = 1/0	: MIPI interface enable/disable
+	 * [1:0] = 01/00: FIXME: 'debug'
+	 */
+	ret = ov5640_write_reg(sensor, OV5640_REG_IO_MIPI_CTRL00,
+			       on ? 0x45 : 0x40);
+	if (ret)
+		return ret;
+
+	return ov5640_write_reg(sensor, OV5640_REG_FRAME_CTRL01,
+				on ? 0x00 : 0x0f);
+}
+
+static int ov5640_get_sysclk(struct ov5640_dev *sensor)
+{
+	 /* calculate sysclk */
+	u32 xvclk = sensor->xclk_freq / 10000;
+	u32 multiplier, prediv, VCO, sysdiv, pll_rdiv;
+	u32 sclk_rdiv_map[] = {1, 2, 4, 8};
+	u32 bit_div2x = 1, sclk_rdiv, sysclk;
+	u8 temp1, temp2;
+	int ret;
+
+	ret = ov5640_read_reg(sensor, OV5640_REG_SC_PLL_CTRL0, &temp1);
+	if (ret)
+		return ret;
+	temp2 = temp1 & 0x0f;
+	if (temp2 == 8 || temp2 == 10)
+		bit_div2x = temp2 / 2;
+
+	ret = ov5640_read_reg(sensor, OV5640_REG_SC_PLL_CTRL1, &temp1);
+	if (ret)
+		return ret;
+	sysdiv = temp1 >> 4;
+	if (sysdiv == 0)
+		sysdiv = 16;
+
+	ret = ov5640_read_reg(sensor, OV5640_REG_SC_PLL_CTRL2, &temp1);
+	if (ret)
+		return ret;
+	multiplier = temp1;
+
+	ret = ov5640_read_reg(sensor, OV5640_REG_SC_PLL_CTRL3, &temp1);
+	if (ret)
+		return ret;
+	prediv = temp1 & 0x0f;
+	pll_rdiv = ((temp1 >> 4) & 0x01) + 1;
+
+	ret = ov5640_read_reg(sensor, OV5640_REG_SYS_ROOT_DIVIDER, &temp1);
+	if (ret)
+		return ret;
+	temp2 = temp1 & 0x03;
+	sclk_rdiv = sclk_rdiv_map[temp2];
+
+	if (!prediv || !sysdiv || !pll_rdiv || !bit_div2x)
+		return -EINVAL;
+
+	VCO = xvclk * multiplier / prediv;
+
+	sysclk = VCO / sysdiv / pll_rdiv * 2 / bit_div2x / sclk_rdiv;
+
+	return sysclk;
+}
+
+static int ov5640_set_night_mode(struct ov5640_dev *sensor)
+{
+	 /* read HTS from register settings */
+	u8 mode;
+	int ret;
+
+	ret = ov5640_read_reg(sensor, OV5640_REG_AEC_CTRL00, &mode);
+	if (ret)
+		return ret;
+	mode &= 0xfb;
+	return ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL00, mode);
+}
+
+static int ov5640_get_hts(struct ov5640_dev *sensor)
+{
+	/* read HTS from register settings */
+	u16 hts;
+	int ret;
+
+	ret = ov5640_read_reg16(sensor, OV5640_REG_TIMING_HTS, &hts);
+	if (ret)
+		return ret;
+	return hts;
+}
+
+static int ov5640_get_vts(struct ov5640_dev *sensor)
+{
+	u16 vts;
+	int ret;
+
+	ret = ov5640_read_reg16(sensor, OV5640_REG_TIMING_VTS, &vts);
+	if (ret)
+		return ret;
+	return vts;
+}
+
+static int ov5640_set_vts(struct ov5640_dev *sensor, int vts)
+{
+	return ov5640_write_reg16(sensor, OV5640_REG_TIMING_VTS, vts);
+}
+
+static int ov5640_get_light_freq(struct ov5640_dev *sensor)
+{
+	/* get banding filter value */
+	int ret, light_freq = 0;
+	u8 temp, temp1;
+
+	ret = ov5640_read_reg(sensor, OV5640_REG_HZ5060_CTRL01, &temp);
+	if (ret)
+		return ret;
+
+	if (temp & 0x80) {
+		/* manual */
+		ret = ov5640_read_reg(sensor, OV5640_REG_HZ5060_CTRL00,
+				      &temp1);
+		if (ret)
+			return ret;
+		if (temp1 & 0x04) {
+			/* 50Hz */
+			light_freq = 50;
+		} else {
+			/* 60Hz */
+			light_freq = 60;
+		}
+	} else {
+		/* auto */
+		ret = ov5640_read_reg(sensor, OV5640_REG_SIGMADELTA_CTRL0C,
+				      &temp1);
+		if (ret)
+			return ret;
+
+		if (temp1 & 0x01) {
+			/* 50Hz */
+			light_freq = 50;
+		} else {
+			/* 60Hz */
+		}
+	}
+
+	return light_freq;
+}
+
+static int ov5640_set_bandingfilter(struct ov5640_dev *sensor)
+{
+	u32 band_step60, max_band60, band_step50, max_band50, prev_vts;
+	int ret;
+
+	/* read preview PCLK */
+	ret = ov5640_get_sysclk(sensor);
+	if (ret < 0)
+		return ret;
+	if (ret == 0)
+		return -EINVAL;
+	sensor->prev_sysclk = ret;
+	/* read preview HTS */
+	ret = ov5640_get_hts(sensor);
+	if (ret < 0)
+		return ret;
+	if (ret == 0)
+		return -EINVAL;
+	sensor->prev_hts = ret;
+
+	/* read preview VTS */
+	ret = ov5640_get_vts(sensor);
+	if (ret < 0)
+		return ret;
+	prev_vts = ret;
+
+	/* calculate banding filter */
+	/* 60Hz */
+	band_step60 = sensor->prev_sysclk * 100 / sensor->prev_hts * 100 / 120;
+	ret = ov5640_write_reg16(sensor, OV5640_REG_AEC_B60_STEP, band_step60);
+	if (ret)
+		return ret;
+	if (!band_step60)
+		return -EINVAL;
+	max_band60 = (int)((prev_vts - 4) / band_step60);
+	ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL0D, max_band60);
+	if (ret)
+		return ret;
+
+	/* 50Hz */
+	band_step50 = sensor->prev_sysclk * 100 / sensor->prev_hts;
+	ret = ov5640_write_reg16(sensor, OV5640_REG_AEC_B50_STEP, band_step50);
+	if (ret)
+		return ret;
+	if (!band_step50)
+		return -EINVAL;
+	max_band50 = (int)((prev_vts - 4) / band_step50);
+	return ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL0E, max_band50);
+}
+
+static int ov5640_set_ae_target(struct ov5640_dev *sensor, int target)
+{
+	/* stable in high */
+	u32 fast_high, fast_low;
+	int ret;
+
+	sensor->ae_low = target * 23 / 25;	/* 0.92 */
+	sensor->ae_high = target * 27 / 25;	/* 1.08 */
+
+	fast_high = sensor->ae_high << 1;
+	if (fast_high > 255)
+		fast_high = 255;
+
+	fast_low = sensor->ae_low >> 1;
+
+	ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL0F, sensor->ae_high);
+	if (ret)
+		return ret;
+	ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL10, sensor->ae_low);
+	if (ret)
+		return ret;
+	ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL1B, sensor->ae_high);
+	if (ret)
+		return ret;
+	ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL1E, sensor->ae_low);
+	if (ret)
+		return ret;
+	ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL11, fast_high);
+	if (ret)
+		return ret;
+	return ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL1F, fast_low);
+}
+
+static int ov5640_get_binning(struct ov5640_dev *sensor)
+{
+	u8 temp;
+	int ret;
+
+	ret = ov5640_read_reg(sensor, OV5640_REG_TIMING_TC_REG21, &temp);
+	if (ret)
+		return ret;
+
+	return temp & BIT(0);
+}
+
+static int ov5640_set_binning(struct ov5640_dev *sensor, bool enable)
+{
+	int ret;
+
+	/*
+	 * TIMING TC REG21:
+	 * - [0]:	Horizontal binning enable
+	 */
+	ret = ov5640_mod_reg(sensor, OV5640_REG_TIMING_TC_REG21,
+			     BIT(0), enable ? BIT(0) : 0);
+	if (ret)
+		return ret;
+	/*
+	 * TIMING TC REG20:
+	 * - [0]:	Undocumented, but hardcoded init sequences
+	 *		are always setting REG21/REG20 bit 0 to same value...
+	 */
+	return ov5640_mod_reg(sensor, OV5640_REG_TIMING_TC_REG20,
+			      BIT(0), enable ? BIT(0) : 0);
+}
+
+static int ov5640_set_virtual_channel(struct ov5640_dev *sensor)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	u8 temp, channel = virtual_channel;
+	int ret;
+
+	if (channel > 3) {
+		dev_err(&client->dev,
+			"%s: wrong virtual_channel parameter, expected (0..3), got %d\n",
+			__func__, channel);
+		return -EINVAL;
+	}
+
+	ret = ov5640_read_reg(sensor, OV5640_REG_DEBUG_MODE, &temp);
+	if (ret)
+		return ret;
+	temp &= ~(3 << 6);
+	temp |= (channel << 6);
+	return ov5640_write_reg(sensor, OV5640_REG_DEBUG_MODE, temp);
+}
+
+static const struct ov5640_mode_info *
+ov5640_find_mode(struct ov5640_dev *sensor, int width, int height, bool nearest)
+{
+	const struct ov5640_mode_info *mode;
+
+	mode = v4l2_find_nearest_size(ov5640_mode_data,
+				      ARRAY_SIZE(ov5640_mode_data),
+				      width, height, width, height);
+
+	if (!mode ||
+	    (!nearest &&
+	     (mode->width != width || mode->height != height)))
+		return NULL;
+
+	return mode;
+}
+
+/*
+ * sensor changes between scaling and subsampling, go through
+ * exposure calculation
+ */
+static int ov5640_set_mode_exposure_calc(struct ov5640_dev *sensor,
+					 const struct ov5640_mode_info *mode)
+{
+	u32 prev_shutter, prev_gain16;
+	u32 cap_shutter, cap_gain16;
+	u32 cap_sysclk, cap_hts, cap_vts;
+	u32 light_freq, cap_bandfilt, cap_maxband;
+	u32 cap_gain16_shutter;
+	u8 average;
+	int ret;
+
+	if (!mode->reg_data)
+		return -EINVAL;
+
+	/* read preview shutter */
+	ret = ov5640_get_exposure(sensor);
+	if (ret < 0)
+		return ret;
+	prev_shutter = ret;
+	ret = ov5640_get_binning(sensor);
+	if (ret < 0)
+		return ret;
+	if (ret && mode->id != OV5640_MODE_720P_1280_720 &&
+	    mode->id != OV5640_MODE_1080P_1920_1080)
+		prev_shutter *= 2;
+
+	/* read preview gain */
+	ret = ov5640_get_gain(sensor);
+	if (ret < 0)
+		return ret;
+	prev_gain16 = ret;
+
+	/* get average */
+	ret = ov5640_read_reg(sensor, OV5640_REG_AVG_READOUT, &average);
+	if (ret)
+		return ret;
+
+	/* turn off night mode for capture */
+	ret = ov5640_set_night_mode(sensor);
+	if (ret < 0)
+		return ret;
+
+	/* Write capture setting */
+	ov5640_load_regs(sensor, mode->reg_data, mode->reg_data_size);
+	ret = ov5640_set_timings(sensor, mode);
+	if (ret < 0)
+		return ret;
+
+	/* read capture VTS */
+	ret = ov5640_get_vts(sensor);
+	if (ret < 0)
+		return ret;
+	cap_vts = ret;
+	ret = ov5640_get_hts(sensor);
+	if (ret < 0)
+		return ret;
+	if (ret == 0)
+		return -EINVAL;
+	cap_hts = ret;
+
+	ret = ov5640_get_sysclk(sensor);
+	if (ret < 0)
+		return ret;
+	if (ret == 0)
+		return -EINVAL;
+	cap_sysclk = ret;
+
+	/* calculate capture banding filter */
+	ret = ov5640_get_light_freq(sensor);
+	if (ret < 0)
+		return ret;
+	light_freq = ret;
+
+	if (light_freq == 60) {
+		/* 60Hz */
+		cap_bandfilt = cap_sysclk * 100 / cap_hts * 100 / 120;
+	} else {
+		/* 50Hz */
+		cap_bandfilt = cap_sysclk * 100 / cap_hts;
+	}
+
+	if (!sensor->prev_sysclk) {
+		ret = ov5640_get_sysclk(sensor);
+		if (ret < 0)
+			return ret;
+		if (ret == 0)
+			return -EINVAL;
+		sensor->prev_sysclk = ret;
+	}
+
+	if (!cap_bandfilt)
+		return -EINVAL;
+
+	cap_maxband = (int)((cap_vts - 4) / cap_bandfilt);
+
+	/* calculate capture shutter/gain16 */
+	if (average > sensor->ae_low && average < sensor->ae_high) {
+		/* in stable range */
+		cap_gain16_shutter =
+			prev_gain16 * prev_shutter *
+			cap_sysclk / sensor->prev_sysclk *
+			sensor->prev_hts / cap_hts *
+			sensor->ae_target / average;
+	} else {
+		cap_gain16_shutter =
+			prev_gain16 * prev_shutter *
+			cap_sysclk / sensor->prev_sysclk *
+			sensor->prev_hts / cap_hts;
+	}
+
+	/* gain to shutter */
+	if (cap_gain16_shutter < (cap_bandfilt * 16)) {
+		/* shutter < 1/100 */
+		cap_shutter = cap_gain16_shutter / 16;
+		if (cap_shutter < 1)
+			cap_shutter = 1;
+
+		cap_gain16 = cap_gain16_shutter / cap_shutter;
+		if (cap_gain16 < 16)
+			cap_gain16 = 16;
+	} else {
+		if (cap_gain16_shutter > (cap_bandfilt * cap_maxband * 16)) {
+			/* exposure reach max */
+			cap_shutter = cap_bandfilt * cap_maxband;
+			if (!cap_shutter)
+				return -EINVAL;
+
+			cap_gain16 = cap_gain16_shutter / cap_shutter;
+		} else {
+			/* 1/100 < (cap_shutter = n/100) =< max */
+			cap_shutter =
+				((int)(cap_gain16_shutter / 16 / cap_bandfilt))
+				* cap_bandfilt;
+			if (!cap_shutter)
+				return -EINVAL;
+
+			cap_gain16 = cap_gain16_shutter / cap_shutter;
+		}
+	}
+
+	/* set capture gain */
+	ret = ov5640_set_gain(sensor, cap_gain16);
+	if (ret)
+		return ret;
+
+	/* write capture shutter */
+	if (cap_shutter > (cap_vts - 4)) {
+		cap_vts = cap_shutter + 4;
+		ret = ov5640_set_vts(sensor, cap_vts);
+		if (ret < 0)
+			return ret;
+	}
+
+	/* set exposure */
+	return ov5640_set_exposure(sensor, cap_shutter);
+}
+
+/*
+ * if sensor changes inside scaling or subsampling
+ * change mode directly
+ */
+static int ov5640_set_mode_direct(struct ov5640_dev *sensor,
+				  const struct ov5640_mode_info *mode)
+{
+	if (!mode->reg_data)
+		return -EINVAL;
+
+	/* Write capture setting */
+	ov5640_load_regs(sensor, mode->reg_data, mode->reg_data_size);
+	return ov5640_set_timings(sensor, mode);
+}
+
+static int ov5640_set_mode(struct ov5640_dev *sensor)
+{
+	const struct ov5640_mode_info *mode = sensor->current_mode;
+	const struct ov5640_mode_info *orig_mode = sensor->last_mode;
+	enum ov5640_downsize_mode dn_mode, orig_dn_mode;
+	bool auto_gain = sensor->ctrls.auto_gain->val == 1;
+	bool auto_exp =  sensor->ctrls.auto_exp->val == V4L2_EXPOSURE_AUTO;
+	int ret;
+
+	dn_mode = mode->dn_mode;
+	orig_dn_mode = orig_mode->dn_mode;
+
+	/* auto gain and exposure must be turned off when changing modes */
+	if (auto_gain) {
+		ret = ov5640_set_autogain(sensor, false);
+		if (ret)
+			return ret;
+	}
+
+	if (auto_exp) {
+		ret = ov5640_set_autoexposure(sensor, false);
+		if (ret)
+			goto restore_auto_gain;
+	}
+
+	if (ov5640_is_csi2(sensor))
+		ret = ov5640_set_mipi_pclk(sensor);
+	else
+		ret = ov5640_set_dvp_pclk(sensor);
+	if (ret < 0)
+		return 0;
+
+	if ((dn_mode == SUBSAMPLING && orig_dn_mode == SCALING) ||
+	    (dn_mode == SCALING && orig_dn_mode == SUBSAMPLING)) {
+		/*
+		 * change between subsampling and scaling
+		 * go through exposure calculation
+		 */
+		ret = ov5640_set_mode_exposure_calc(sensor, mode);
+	} else {
+		/*
+		 * change inside subsampling or scaling
+		 * download firmware directly
+		 */
+		ret = ov5640_set_mode_direct(sensor, mode);
+	}
+	if (ret < 0)
+		goto restore_auto_exp_gain;
+
+	/* restore auto gain and exposure */
+	if (auto_gain)
+		ov5640_set_autogain(sensor, true);
+	if (auto_exp)
+		ov5640_set_autoexposure(sensor, true);
+
+	ret = ov5640_set_binning(sensor, dn_mode != SCALING);
+	if (ret < 0)
+		return ret;
+	ret = ov5640_set_ae_target(sensor, sensor->ae_target);
+	if (ret < 0)
+		return ret;
+	ret = ov5640_get_light_freq(sensor);
+	if (ret < 0)
+		return ret;
+	ret = ov5640_set_bandingfilter(sensor);
+	if (ret < 0)
+		return ret;
+	ret = ov5640_set_virtual_channel(sensor);
+	if (ret < 0)
+		return ret;
+
+	sensor->pending_mode_change = false;
+	sensor->last_mode = mode;
+
+	return 0;
+
+restore_auto_exp_gain:
+	if (auto_exp)
+		ov5640_set_autoexposure(sensor, true);
+restore_auto_gain:
+	if (auto_gain)
+		ov5640_set_autogain(sensor, true);
+
+	return ret;
+}
+
+static int ov5640_set_framefmt(struct ov5640_dev *sensor,
+			       struct v4l2_mbus_framefmt *format);
+
+/* restore the last set video mode after chip power-on */
+static int ov5640_restore_mode(struct ov5640_dev *sensor)
+{
+	int ret;
+
+	/* first load the initial register values */
+	ov5640_load_regs(sensor, ov5640_init_setting,
+			 ARRAY_SIZE(ov5640_init_setting));
+
+	ret = ov5640_mod_reg(sensor, OV5640_REG_SYS_ROOT_DIVIDER, 0x3f,
+			     (ilog2(OV5640_SCLK2X_ROOT_DIV) << 2) |
+			     ilog2(OV5640_SCLK_ROOT_DIV));
+	if (ret)
+		return ret;
+
+	/* now restore the last capture mode */
+	ret = ov5640_set_mode(sensor);
+	if (ret < 0)
+		return ret;
+
+	return ov5640_set_framefmt(sensor, &sensor->fmt);
+}
+
+static void ov5640_power(struct ov5640_dev *sensor, bool enable)
+{
+	gpiod_set_value_cansleep(sensor->pwdn_gpio, enable ? 0 : 1);
+}
+
+/*
+ * From section 2.7 power up sequence:
+ * t0 + t1 + t2 >= 5ms	Delay from DOVDD stable to PWDN pull down
+ * t3 >= 1ms		Delay from PWDN pull down to RESETB pull up
+ * t4 >= 20ms		Delay from RESETB pull up to SCCB (i2c) stable
+ *
+ * Some modules don't expose RESETB/PWDN pins directly, instead providing a
+ * "PWUP" GPIO which is wired through appropriate delays and inverters to the
+ * pins.
+ *
+ * In such cases, this gpio should be mapped to pwdn_gpio in the driver, and we
+ * should still toggle the pwdn_gpio below with the appropriate delays, while
+ * the calls to reset_gpio will be ignored.
+ */
+static void ov5640_powerup_sequence(struct ov5640_dev *sensor)
+{
+	if (sensor->pwdn_gpio) {
+		gpiod_set_value_cansleep(sensor->reset_gpio, 1);
+
+		/* camera power cycle */
+		ov5640_power(sensor, false);
+		usleep_range(5000, 10000);	/* t2 */
+		ov5640_power(sensor, true);
+		usleep_range(1000, 2000);	/* t3 */
+
+		gpiod_set_value_cansleep(sensor->reset_gpio, 0);
+	} else {
+		/* software reset */
+		ov5640_write_reg(sensor, OV5640_REG_SYS_CTRL0,
+				 OV5640_REG_SYS_CTRL0_SW_RST);
+	}
+	usleep_range(20000, 25000);	/* t4 */
+
+	/*
+	 * software standby: allows registers programming;
+	 * exit at restore_mode() for CSI, s_stream(1) for DVP
+	 */
+	ov5640_write_reg(sensor, OV5640_REG_SYS_CTRL0,
+			 OV5640_REG_SYS_CTRL0_SW_PWDN);
+}
+
+static int ov5640_set_power_on(struct ov5640_dev *sensor)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	int ret;
+
+	ret = clk_prepare_enable(sensor->xclk);
+	if (ret) {
+		dev_err(&client->dev, "%s: failed to enable clock\n",
+			__func__);
+		return ret;
+	}
+
+	ret = regulator_bulk_enable(OV5640_NUM_SUPPLIES,
+				    sensor->supplies);
+	if (ret) {
+		dev_err(&client->dev, "%s: failed to enable regulators\n",
+			__func__);
+		goto xclk_off;
+	}
+
+	ov5640_powerup_sequence(sensor);
+
+	ret = ov5640_init_slave_id(sensor);
+	if (ret)
+		goto power_off;
+
+	return 0;
+
+power_off:
+	ov5640_power(sensor, false);
+	regulator_bulk_disable(OV5640_NUM_SUPPLIES, sensor->supplies);
+xclk_off:
+	clk_disable_unprepare(sensor->xclk);
+	return ret;
+}
+
+static void ov5640_set_power_off(struct ov5640_dev *sensor)
+{
+	ov5640_power(sensor, false);
+	regulator_bulk_disable(OV5640_NUM_SUPPLIES, sensor->supplies);
+	clk_disable_unprepare(sensor->xclk);
+}
+
+static int ov5640_set_power_mipi(struct ov5640_dev *sensor, bool on)
+{
+	int ret;
+
+	if (!on) {
+		/* Reset MIPI bus settings to their default values. */
+		ov5640_write_reg(sensor, OV5640_REG_IO_MIPI_CTRL00, 0x58);
+		ov5640_write_reg(sensor, OV5640_REG_MIPI_CTRL00, 0x04);
+		ov5640_write_reg(sensor, OV5640_REG_PAD_OUTPUT00, 0x00);
+		return 0;
+	}
+
+	/*
+	 * Power up MIPI HS Tx and LS Rx; 2 data lanes mode
+	 *
+	 * 0x300e = 0x40
+	 * [7:5] = 010	: 2 data lanes mode (see FIXME note in
+	 *		  "ov5640_set_stream_mipi()")
+	 * [4] = 0	: Power up MIPI HS Tx
+	 * [3] = 0	: Power up MIPI LS Rx
+	 * [2] = 1	: MIPI interface enabled
+	 */
+	ret = ov5640_write_reg(sensor, OV5640_REG_IO_MIPI_CTRL00, 0x44);
+	if (ret)
+		return ret;
+
+	/*
+	 * Gate clock and set LP11 in 'no packets mode' (idle)
+	 *
+	 * 0x4800 = 0x24
+	 * [5] = 1	: Gate clock when 'no packets'
+	 * [2] = 1	: MIPI bus in LP11 when 'no packets'
+	 */
+	ret = ov5640_write_reg(sensor, OV5640_REG_MIPI_CTRL00, 0x24);
+	if (ret)
+		return ret;
+
+	/*
+	 * Set data lanes and clock in LP11 when 'sleeping'
+	 *
+	 * 0x3019 = 0x70
+	 * [6] = 1	: MIPI data lane 2 in LP11 when 'sleeping'
+	 * [5] = 1	: MIPI data lane 1 in LP11 when 'sleeping'
+	 * [4] = 1	: MIPI clock lane in LP11 when 'sleeping'
+	 */
+	ret = ov5640_write_reg(sensor, OV5640_REG_PAD_OUTPUT00, 0x70);
+	if (ret)
+		return ret;
+
+	/* Give lanes some time to coax into LP11 state. */
+	usleep_range(500, 1000);
+
+	return 0;
+}
+
+static int ov5640_set_power_dvp(struct ov5640_dev *sensor, bool on)
+{
+	unsigned int flags = sensor->ep.bus.parallel.flags;
+	bool bt656 = sensor->ep.bus_type == V4L2_MBUS_BT656;
+	u8 polarities = 0;
+	int ret;
+
+	if (!on) {
+		/* Reset settings to their default values. */
+		ov5640_write_reg(sensor, OV5640_REG_CCIR656_CTRL00, 0x00);
+		ov5640_write_reg(sensor, OV5640_REG_IO_MIPI_CTRL00, 0x58);
+		ov5640_write_reg(sensor, OV5640_REG_POLARITY_CTRL00, 0x20);
+		ov5640_write_reg(sensor, OV5640_REG_PAD_OUTPUT_ENABLE01, 0x00);
+		ov5640_write_reg(sensor, OV5640_REG_PAD_OUTPUT_ENABLE02, 0x00);
+		return 0;
+	}
+
+	/*
+	 * Note about parallel port configuration.
+	 *
+	 * When configured in parallel mode, the OV5640 will
+	 * output 10 bits data on DVP data lines [9:0].
+	 * If only 8 bits data are wanted, the 8 bits data lines
+	 * of the camera interface must be physically connected
+	 * on the DVP data lines [9:2].
+	 *
+	 * Control lines polarity can be configured through
+	 * devicetree endpoint control lines properties.
+	 * If no endpoint control lines properties are set,
+	 * polarity will be as below:
+	 * - VSYNC:	active high
+	 * - HREF:	active low
+	 * - PCLK:	active low
+	 *
+	 * VSYNC & HREF are not configured if BT656 bus mode is selected
+	 */
+
+	/*
+	 * BT656 embedded synchronization configuration
+	 *
+	 * CCIR656 CTRL00
+	 * - [7]:	SYNC code selection (0: auto generate sync code,
+	 *		1: sync code from regs 0x4732-0x4735)
+	 * - [6]:	f value in CCIR656 SYNC code when fixed f value
+	 * - [5]:	Fixed f value
+	 * - [4:3]:	Blank toggle data options (00: data=1'h040/1'h200,
+	 *		01: data from regs 0x4736-0x4738, 10: always keep 0)
+	 * - [1]:	Clip data disable
+	 * - [0]:	CCIR656 mode enable
+	 *
+	 * Default CCIR656 SAV/EAV mode with default codes
+	 * SAV=0xff000080 & EAV=0xff00009d is enabled here with settings:
+	 * - CCIR656 mode enable
+	 * - auto generation of sync codes
+	 * - blank toggle data 1'h040/1'h200
+	 * - clip reserved data (0x00 & 0xff changed to 0x01 & 0xfe)
+	 */
+	ret = ov5640_write_reg(sensor, OV5640_REG_CCIR656_CTRL00,
+			       bt656 ? 0x01 : 0x00);
+	if (ret)
+		return ret;
+
+	/*
+	 * configure parallel port control lines polarity
+	 *
+	 * POLARITY CTRL0
+	 * - [5]:	PCLK polarity (0: active low, 1: active high)
+	 * - [1]:	HREF polarity (0: active low, 1: active high)
+	 * - [0]:	VSYNC polarity (mismatch here between
+	 *		datasheet and hardware, 0 is active high
+	 *		and 1 is active low...)
+	 */
+	if (!bt656) {
+		if (flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
+			polarities |= BIT(1);
+		if (flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
+			polarities |= BIT(0);
+	}
+	if (flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
+		polarities |= BIT(5);
+
+	ret = ov5640_write_reg(sensor, OV5640_REG_POLARITY_CTRL00, polarities);
+	if (ret)
+		return ret;
+
+	/*
+	 * powerdown MIPI TX/RX PHY & enable DVP
+	 *
+	 * MIPI CONTROL 00
+	 * [4] = 1	: Power down MIPI HS Tx
+	 * [3] = 1	: Power down MIPI LS Rx
+	 * [2] = 0	: DVP enable (MIPI disable)
+	 */
+	ret = ov5640_write_reg(sensor, OV5640_REG_IO_MIPI_CTRL00, 0x18);
+	if (ret)
+		return ret;
+
+	/*
+	 * enable VSYNC/HREF/PCLK DVP control lines
+	 * & D[9:6] DVP data lines
+	 *
+	 * PAD OUTPUT ENABLE 01
+	 * - 6:		VSYNC output enable
+	 * - 5:		HREF output enable
+	 * - 4:		PCLK output enable
+	 * - [3:0]:	D[9:6] output enable
+	 */
+	ret = ov5640_write_reg(sensor, OV5640_REG_PAD_OUTPUT_ENABLE01,
+			       bt656 ? 0x1f : 0x7f);
+	if (ret)
+		return ret;
+
+	/*
+	 * enable D[5:0] DVP data lines
+	 *
+	 * PAD OUTPUT ENABLE 02
+	 * - [7:2]:	D[5:0] output enable
+	 */
+	return ov5640_write_reg(sensor, OV5640_REG_PAD_OUTPUT_ENABLE02, 0xfc);
+}
+
+static int ov5640_set_power(struct ov5640_dev *sensor, bool on)
+{
+	int ret = 0;
+
+	if (on) {
+		ret = ov5640_set_power_on(sensor);
+		if (ret)
+			return ret;
+
+		ret = ov5640_restore_mode(sensor);
+		if (ret)
+			goto power_off;
+	}
+
+	if (sensor->ep.bus_type == V4L2_MBUS_CSI2_DPHY)
+		ret = ov5640_set_power_mipi(sensor, on);
+	else
+		ret = ov5640_set_power_dvp(sensor, on);
+	if (ret)
+		goto power_off;
+
+	if (!on)
+		ov5640_set_power_off(sensor);
+
+	return 0;
+
+power_off:
+	ov5640_set_power_off(sensor);
+	return ret;
+}
+
+static int ov5640_sensor_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov5640_dev *ov5640 = to_ov5640_dev(sd);
+
+	return ov5640_set_power(ov5640, false);
+}
+
+static int ov5640_sensor_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov5640_dev *ov5640 = to_ov5640_dev(sd);
+
+	return ov5640_set_power(ov5640, true);
+}
+
+/* --------------- Subdev Operations --------------- */
+
+static int ov5640_try_frame_interval(struct ov5640_dev *sensor,
+				     struct v4l2_fract *fi,
+				     const struct ov5640_mode_info *mode_info)
+{
+	const struct ov5640_mode_info *mode = mode_info;
+	enum ov5640_frame_rate rate = OV5640_15_FPS;
+	int minfps, maxfps, best_fps, fps;
+	int i;
+
+	minfps = ov5640_framerates[OV5640_15_FPS];
+	maxfps = ov5640_framerates[mode->max_fps];
+
+	if (fi->numerator == 0) {
+		fi->denominator = maxfps;
+		fi->numerator = 1;
+		rate = mode->max_fps;
+		goto find_mode;
+	}
+
+	fps = clamp_val(DIV_ROUND_CLOSEST(fi->denominator, fi->numerator),
+			minfps, maxfps);
+
+	best_fps = minfps;
+	for (i = 0; i < ARRAY_SIZE(ov5640_framerates); i++) {
+		int curr_fps = ov5640_framerates[i];
+
+		if (abs(curr_fps - fps) < abs(best_fps - fps)) {
+			best_fps = curr_fps;
+			rate = i;
+		}
+	}
+
+	fi->numerator = 1;
+	fi->denominator = best_fps;
+
+find_mode:
+	mode = ov5640_find_mode(sensor, mode->width, mode->height, false);
+	return mode ? rate : -EINVAL;
+}
+
+static int ov5640_get_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *format)
+{
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+	struct v4l2_mbus_framefmt *fmt;
+
+	if (format->pad != 0)
+		return -EINVAL;
+
+	mutex_lock(&sensor->lock);
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+		fmt = v4l2_subdev_get_try_format(&sensor->sd, sd_state,
+						 format->pad);
+	else
+		fmt = &sensor->fmt;
+
+	format->format = *fmt;
+
+	mutex_unlock(&sensor->lock);
+
+	return 0;
+}
+
+static int ov5640_try_fmt_internal(struct v4l2_subdev *sd,
+				   struct v4l2_mbus_framefmt *fmt,
+				   const struct ov5640_mode_info **new_mode)
+{
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+	const struct ov5640_mode_info *mode;
+	const struct ov5640_pixfmt *pixfmt;
+	unsigned int bpp;
+
+	mode = ov5640_find_mode(sensor, fmt->width, fmt->height, true);
+	if (!mode)
+		return -EINVAL;
+
+	pixfmt = ov5640_code_to_pixfmt(sensor, fmt->code);
+	bpp = pixfmt->bpp;
+
+	/*
+	 * Adjust mode according to bpp:
+	 * - 8bpp modes work for resolution >= 1280x720
+	 * - 24bpp modes work resolution < 1280x720
+	 */
+	if (bpp == 8 && mode->width < 1280)
+		mode = &ov5640_mode_data[OV5640_MODE_720P_1280_720];
+	else if (bpp == 24 && mode->width > 1024)
+		mode = &ov5640_mode_data[OV5640_MODE_XGA_1024_768];
+
+	fmt->width = mode->width;
+	fmt->height = mode->height;
+
+	if (new_mode)
+		*new_mode = mode;
+
+	fmt->code = pixfmt->code;
+	fmt->colorspace = pixfmt->colorspace;
+	fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(fmt->colorspace);
+	fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
+	fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(fmt->colorspace);
+
+	return 0;
+}
+
+static void __v4l2_ctrl_vblank_update(struct ov5640_dev *sensor, u32 vblank)
+{
+	const struct ov5640_mode_info *mode = sensor->current_mode;
+
+	__v4l2_ctrl_modify_range(sensor->ctrls.vblank, OV5640_MIN_VBLANK,
+				 OV5640_MAX_VTS - mode->height, 1, vblank);
+
+	__v4l2_ctrl_s_ctrl(sensor->ctrls.vblank, vblank);
+}
+
+static int ov5640_update_pixel_rate(struct ov5640_dev *sensor)
+{
+	const struct ov5640_mode_info *mode = sensor->current_mode;
+	enum ov5640_pixel_rate_id pixel_rate_id = mode->pixel_rate;
+	struct v4l2_mbus_framefmt *fmt = &sensor->fmt;
+	const struct ov5640_timings *timings = ov5640_timings(sensor, mode);
+	s32 exposure_val, exposure_max;
+	unsigned int hblank;
+	unsigned int i = 0;
+	u32 pixel_rate;
+	s64 link_freq;
+	u32 num_lanes;
+	u32 vblank;
+	u32 bpp;
+
+	/*
+	 * Update the pixel rate control value.
+	 *
+	 * For DVP mode, maintain the pixel rate calculation using fixed FPS.
+	 */
+	if (!ov5640_is_csi2(sensor)) {
+		__v4l2_ctrl_s_ctrl_int64(sensor->ctrls.pixel_rate,
+					 ov5640_calc_pixel_rate(sensor));
+
+		__v4l2_ctrl_vblank_update(sensor, timings->vblank_def);
+
+		return 0;
+	}
+
+	/*
+	 * The MIPI CSI-2 link frequency should comply with the CSI-2
+	 * specification and be lower than 1GHz.
+	 *
+	 * Start from the suggested pixel_rate for the current mode and
+	 * progressively slow it down if it exceeds 1GHz.
+	 */
+	num_lanes = sensor->ep.bus.mipi_csi2.num_data_lanes;
+	bpp = ov5640_code_to_bpp(sensor, fmt->code);
+	do {
+		pixel_rate = ov5640_pixel_rates[pixel_rate_id];
+		link_freq = pixel_rate * bpp / (2 * num_lanes);
+	} while (link_freq >= 1000000000U &&
+		 ++pixel_rate_id < OV5640_NUM_PIXEL_RATES);
+
+	sensor->current_link_freq = link_freq;
+
+	/*
+	 * Higher link rates require the clock tree to be programmed with
+	 * 'mipi_div' = 1; this has the effect of halving the actual output
+	 * pixel rate in the MIPI domain.
+	 *
+	 * Adjust the pixel rate and link frequency control value to report it
+	 * correctly to userspace.
+	 */
+	if (link_freq > OV5640_LINK_RATE_MAX) {
+		pixel_rate /= 2;
+		link_freq /= 2;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(ov5640_csi2_link_freqs); ++i) {
+		if (ov5640_csi2_link_freqs[i] == link_freq)
+			break;
+	}
+	WARN_ON(i == ARRAY_SIZE(ov5640_csi2_link_freqs));
+
+	__v4l2_ctrl_s_ctrl_int64(sensor->ctrls.pixel_rate, pixel_rate);
+	__v4l2_ctrl_s_ctrl(sensor->ctrls.link_freq, i);
+
+	hblank = timings->htot - mode->width;
+	__v4l2_ctrl_modify_range(sensor->ctrls.hblank,
+				 hblank, hblank, 1, hblank);
+
+	vblank = timings->vblank_def;
+	__v4l2_ctrl_vblank_update(sensor, vblank);
+
+	exposure_max = timings->crop.height + vblank - 4;
+	exposure_val = clamp_t(s32, sensor->ctrls.exposure->val,
+			       sensor->ctrls.exposure->minimum,
+			       exposure_max);
+
+	__v4l2_ctrl_modify_range(sensor->ctrls.exposure,
+				 sensor->ctrls.exposure->minimum,
+				 exposure_max, 1, exposure_val);
+
+	return 0;
+}
+
+static int ov5640_set_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *format)
+{
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+	const struct ov5640_mode_info *new_mode;
+	struct v4l2_mbus_framefmt *mbus_fmt = &format->format;
+	int ret;
+
+	if (format->pad != 0)
+		return -EINVAL;
+
+	mutex_lock(&sensor->lock);
+
+	if (sensor->streaming) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	ret = ov5640_try_fmt_internal(sd, mbus_fmt, &new_mode);
+	if (ret)
+		goto out;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		*v4l2_subdev_get_try_format(sd, sd_state, 0) = *mbus_fmt;
+		goto out;
+	}
+
+	if (new_mode != sensor->current_mode) {
+		sensor->current_fr = new_mode->def_fps;
+		sensor->current_mode = new_mode;
+		sensor->pending_mode_change = true;
+	}
+	if (mbus_fmt->code != sensor->fmt.code)
+		sensor->pending_fmt_change = true;
+
+	/* update format even if code is unchanged, resolution might change */
+	sensor->fmt = *mbus_fmt;
+
+	ov5640_update_pixel_rate(sensor);
+
+out:
+	mutex_unlock(&sensor->lock);
+	return ret;
+}
+
+static int ov5640_get_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *sd_state,
+				struct v4l2_subdev_selection *sel)
+{
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+	const struct ov5640_mode_info *mode = sensor->current_mode;
+	const struct ov5640_timings *timings;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP: {
+		mutex_lock(&sensor->lock);
+		timings = ov5640_timings(sensor, mode);
+		sel->r = timings->analog_crop;
+		mutex_unlock(&sensor->lock);
+
+		return 0;
+	}
+
+	case V4L2_SEL_TGT_NATIVE_SIZE:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.top = 0;
+		sel->r.left = 0;
+		sel->r.width = OV5640_NATIVE_WIDTH;
+		sel->r.height = OV5640_NATIVE_HEIGHT;
+
+		return 0;
+
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		sel->r.top = OV5640_PIXEL_ARRAY_TOP;
+		sel->r.left = OV5640_PIXEL_ARRAY_LEFT;
+		sel->r.width = OV5640_PIXEL_ARRAY_WIDTH;
+		sel->r.height = OV5640_PIXEL_ARRAY_HEIGHT;
+
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int ov5640_set_framefmt(struct ov5640_dev *sensor,
+			       struct v4l2_mbus_framefmt *format)
+{
+	bool is_jpeg = format->code == MEDIA_BUS_FMT_JPEG_1X8;
+	const struct ov5640_pixfmt *pixfmt;
+	int ret = 0;
+
+	pixfmt = ov5640_code_to_pixfmt(sensor, format->code);
+
+	/* FORMAT CONTROL00: YUV and RGB formatting */
+	ret = ov5640_write_reg(sensor, OV5640_REG_FORMAT_CONTROL00,
+			       pixfmt->ctrl00);
+	if (ret)
+		return ret;
+
+	/* FORMAT MUX CONTROL: ISP YUV or RGB */
+	ret = ov5640_write_reg(sensor, OV5640_REG_ISP_FORMAT_MUX_CTRL,
+			       pixfmt->mux);
+	if (ret)
+		return ret;
+
+	/*
+	 * TIMING TC REG21:
+	 * - [5]:	JPEG enable
+	 */
+	ret = ov5640_mod_reg(sensor, OV5640_REG_TIMING_TC_REG21,
+			     BIT(5), is_jpeg ? BIT(5) : 0);
+	if (ret)
+		return ret;
+
+	/*
+	 * SYSTEM RESET02:
+	 * - [4]:	Reset JFIFO
+	 * - [3]:	Reset SFIFO
+	 * - [2]:	Reset JPEG
+	 */
+	ret = ov5640_mod_reg(sensor, OV5640_REG_SYS_RESET02,
+			     BIT(4) | BIT(3) | BIT(2),
+			     is_jpeg ? 0 : (BIT(4) | BIT(3) | BIT(2)));
+	if (ret)
+		return ret;
+
+	/*
+	 * CLOCK ENABLE02:
+	 * - [5]:	Enable JPEG 2x clock
+	 * - [3]:	Enable JPEG clock
+	 */
+	return ov5640_mod_reg(sensor, OV5640_REG_SYS_CLOCK_ENABLE02,
+			      BIT(5) | BIT(3),
+			      is_jpeg ? (BIT(5) | BIT(3)) : 0);
+}
+
+/*
+ * Sensor Controls.
+ */
+
+static int ov5640_set_ctrl_hue(struct ov5640_dev *sensor, int value)
+{
+	int ret;
+
+	if (value) {
+		ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0,
+				     BIT(0), BIT(0));
+		if (ret)
+			return ret;
+		ret = ov5640_write_reg16(sensor, OV5640_REG_SDE_CTRL1, value);
+	} else {
+		ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0, BIT(0), 0);
+	}
+
+	return ret;
+}
+
+static int ov5640_set_ctrl_contrast(struct ov5640_dev *sensor, int value)
+{
+	int ret;
+
+	if (value) {
+		ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0,
+				     BIT(2), BIT(2));
+		if (ret)
+			return ret;
+		ret = ov5640_write_reg(sensor, OV5640_REG_SDE_CTRL5,
+				       value & 0xff);
+	} else {
+		ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0, BIT(2), 0);
+	}
+
+	return ret;
+}
+
+static int ov5640_set_ctrl_saturation(struct ov5640_dev *sensor, int value)
+{
+	int ret;
+
+	if (value) {
+		ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0,
+				     BIT(1), BIT(1));
+		if (ret)
+			return ret;
+		ret = ov5640_write_reg(sensor, OV5640_REG_SDE_CTRL3,
+				       value & 0xff);
+		if (ret)
+			return ret;
+		ret = ov5640_write_reg(sensor, OV5640_REG_SDE_CTRL4,
+				       value & 0xff);
+	} else {
+		ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0, BIT(1), 0);
+	}
+
+	return ret;
+}
+
+static int ov5640_set_ctrl_white_balance(struct ov5640_dev *sensor, int awb)
+{
+	int ret;
+
+	ret = ov5640_mod_reg(sensor, OV5640_REG_AWB_MANUAL_CTRL,
+			     BIT(0), awb ? 0 : 1);
+	if (ret)
+		return ret;
+
+	if (!awb) {
+		u16 red = (u16)sensor->ctrls.red_balance->val;
+		u16 blue = (u16)sensor->ctrls.blue_balance->val;
+
+		ret = ov5640_write_reg16(sensor, OV5640_REG_AWB_R_GAIN, red);
+		if (ret)
+			return ret;
+		ret = ov5640_write_reg16(sensor, OV5640_REG_AWB_B_GAIN, blue);
+	}
+
+	return ret;
+}
+
+static int ov5640_set_ctrl_exposure(struct ov5640_dev *sensor,
+				    enum v4l2_exposure_auto_type auto_exposure)
+{
+	struct ov5640_ctrls *ctrls = &sensor->ctrls;
+	bool auto_exp = (auto_exposure == V4L2_EXPOSURE_AUTO);
+	int ret = 0;
+
+	if (ctrls->auto_exp->is_new) {
+		ret = ov5640_set_autoexposure(sensor, auto_exp);
+		if (ret)
+			return ret;
+	}
+
+	if (!auto_exp && ctrls->exposure->is_new) {
+		u16 max_exp;
+
+		ret = ov5640_read_reg16(sensor, OV5640_REG_AEC_PK_VTS,
+					&max_exp);
+		if (ret)
+			return ret;
+		ret = ov5640_get_vts(sensor);
+		if (ret < 0)
+			return ret;
+		max_exp += ret;
+		ret = 0;
+
+		if (ctrls->exposure->val < max_exp)
+			ret = ov5640_set_exposure(sensor, ctrls->exposure->val);
+	}
+
+	return ret;
+}
+
+static int ov5640_set_ctrl_gain(struct ov5640_dev *sensor, bool auto_gain)
+{
+	struct ov5640_ctrls *ctrls = &sensor->ctrls;
+	int ret = 0;
+
+	if (ctrls->auto_gain->is_new) {
+		ret = ov5640_set_autogain(sensor, auto_gain);
+		if (ret)
+			return ret;
+	}
+
+	if (!auto_gain && ctrls->gain->is_new)
+		ret = ov5640_set_gain(sensor, ctrls->gain->val);
+
+	return ret;
+}
+
+static const char * const test_pattern_menu[] = {
+	"Disabled",
+	"Color bars",
+	"Color bars w/ rolling bar",
+	"Color squares",
+	"Color squares w/ rolling bar",
+};
+
+#define OV5640_TEST_ENABLE		BIT(7)
+#define OV5640_TEST_ROLLING		BIT(6)	/* rolling horizontal bar */
+#define OV5640_TEST_TRANSPARENT		BIT(5)
+#define OV5640_TEST_SQUARE_BW		BIT(4)	/* black & white squares */
+#define OV5640_TEST_BAR_STANDARD	(0 << 2)
+#define OV5640_TEST_BAR_VERT_CHANGE_1	(1 << 2)
+#define OV5640_TEST_BAR_HOR_CHANGE	(2 << 2)
+#define OV5640_TEST_BAR_VERT_CHANGE_2	(3 << 2)
+#define OV5640_TEST_BAR			(0 << 0)
+#define OV5640_TEST_RANDOM		(1 << 0)
+#define OV5640_TEST_SQUARE		(2 << 0)
+#define OV5640_TEST_BLACK		(3 << 0)
+
+static const u8 test_pattern_val[] = {
+	0,
+	OV5640_TEST_ENABLE | OV5640_TEST_BAR_VERT_CHANGE_1 |
+		OV5640_TEST_BAR,
+	OV5640_TEST_ENABLE | OV5640_TEST_ROLLING |
+		OV5640_TEST_BAR_VERT_CHANGE_1 | OV5640_TEST_BAR,
+	OV5640_TEST_ENABLE | OV5640_TEST_SQUARE,
+	OV5640_TEST_ENABLE | OV5640_TEST_ROLLING | OV5640_TEST_SQUARE,
+};
+
+static int ov5640_set_ctrl_test_pattern(struct ov5640_dev *sensor, int value)
+{
+	return ov5640_write_reg(sensor, OV5640_REG_PRE_ISP_TEST_SET1,
+				test_pattern_val[value]);
+}
+
+static int ov5640_set_ctrl_light_freq(struct ov5640_dev *sensor, int value)
+{
+	int ret;
+
+	ret = ov5640_mod_reg(sensor, OV5640_REG_HZ5060_CTRL01, BIT(7),
+			     (value == V4L2_CID_POWER_LINE_FREQUENCY_AUTO) ?
+			     0 : BIT(7));
+	if (ret)
+		return ret;
+
+	return ov5640_mod_reg(sensor, OV5640_REG_HZ5060_CTRL00, BIT(2),
+			      (value == V4L2_CID_POWER_LINE_FREQUENCY_50HZ) ?
+			      BIT(2) : 0);
+}
+
+static int ov5640_set_ctrl_hflip(struct ov5640_dev *sensor, int value)
+{
+	/*
+	 * If sensor is mounted upside down, mirror logic is inversed.
+	 *
+	 * Sensor is a BSI (Back Side Illuminated) one,
+	 * so image captured is physically mirrored.
+	 * This is why mirror logic is inversed in
+	 * order to cancel this mirror effect.
+	 */
+
+	/*
+	 * TIMING TC REG21:
+	 * - [2]:	ISP mirror
+	 * - [1]:	Sensor mirror
+	 */
+	return ov5640_mod_reg(sensor, OV5640_REG_TIMING_TC_REG21,
+			      BIT(2) | BIT(1),
+			      (!(value ^ sensor->upside_down)) ?
+			      (BIT(2) | BIT(1)) : 0);
+}
+
+static int ov5640_set_ctrl_vflip(struct ov5640_dev *sensor, int value)
+{
+	/* If sensor is mounted upside down, flip logic is inversed */
+
+	/*
+	 * TIMING TC REG20:
+	 * - [2]:	ISP vflip
+	 * - [1]:	Sensor vflip
+	 */
+	return ov5640_mod_reg(sensor, OV5640_REG_TIMING_TC_REG20,
+			      BIT(2) | BIT(1),
+			      (value ^ sensor->upside_down) ?
+			      (BIT(2) | BIT(1)) : 0);
+}
+
+static int ov5640_set_ctrl_vblank(struct ov5640_dev *sensor, int value)
+{
+	const struct ov5640_mode_info *mode = sensor->current_mode;
+
+	/* Update the VTOT timing register value. */
+	return ov5640_write_reg16(sensor, OV5640_REG_TIMING_VTS,
+				  mode->height + value);
+}
+
+static int ov5640_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+	int val;
+
+	/* v4l2_ctrl_lock() locks our own mutex */
+
+	if (!pm_runtime_get_if_in_use(&sensor->i2c_client->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTOGAIN:
+		val = ov5640_get_gain(sensor);
+		if (val < 0)
+			return val;
+		sensor->ctrls.gain->val = val;
+		break;
+	case V4L2_CID_EXPOSURE_AUTO:
+		val = ov5640_get_exposure(sensor);
+		if (val < 0)
+			return val;
+		sensor->ctrls.exposure->val = val;
+		break;
+	}
+
+	pm_runtime_mark_last_busy(&sensor->i2c_client->dev);
+	pm_runtime_put_autosuspend(&sensor->i2c_client->dev);
+
+	return 0;
+}
+
+static int ov5640_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+	const struct ov5640_mode_info *mode = sensor->current_mode;
+	const struct ov5640_timings *timings;
+	unsigned int exp_max;
+	int ret;
+
+	/* v4l2_ctrl_lock() locks our own mutex */
+
+	switch (ctrl->id) {
+	case V4L2_CID_VBLANK:
+		/* Update the exposure range to the newly programmed vblank. */
+		timings = ov5640_timings(sensor, mode);
+		exp_max = mode->height + ctrl->val - 4;
+		__v4l2_ctrl_modify_range(sensor->ctrls.exposure,
+					 sensor->ctrls.exposure->minimum,
+					 exp_max, sensor->ctrls.exposure->step,
+					 timings->vblank_def);
+		break;
+	}
+
+	/*
+	 * If the device is not powered up by the host driver do
+	 * not apply any controls to H/W at this time. Instead
+	 * the controls will be restored at start streaming time.
+	 */
+	if (!pm_runtime_get_if_in_use(&sensor->i2c_client->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTOGAIN:
+		ret = ov5640_set_ctrl_gain(sensor, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE_AUTO:
+		ret = ov5640_set_ctrl_exposure(sensor, ctrl->val);
+		break;
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		ret = ov5640_set_ctrl_white_balance(sensor, ctrl->val);
+		break;
+	case V4L2_CID_HUE:
+		ret = ov5640_set_ctrl_hue(sensor, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		ret = ov5640_set_ctrl_contrast(sensor, ctrl->val);
+		break;
+	case V4L2_CID_SATURATION:
+		ret = ov5640_set_ctrl_saturation(sensor, ctrl->val);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		ret = ov5640_set_ctrl_test_pattern(sensor, ctrl->val);
+		break;
+	case V4L2_CID_POWER_LINE_FREQUENCY:
+		ret = ov5640_set_ctrl_light_freq(sensor, ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+		ret = ov5640_set_ctrl_hflip(sensor, ctrl->val);
+		break;
+	case V4L2_CID_VFLIP:
+		ret = ov5640_set_ctrl_vflip(sensor, ctrl->val);
+		break;
+	case V4L2_CID_VBLANK:
+		ret = ov5640_set_ctrl_vblank(sensor, ctrl->val);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	pm_runtime_mark_last_busy(&sensor->i2c_client->dev);
+	pm_runtime_put_autosuspend(&sensor->i2c_client->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ov5640_ctrl_ops = {
+	.g_volatile_ctrl = ov5640_g_volatile_ctrl,
+	.s_ctrl = ov5640_s_ctrl,
+};
+
+static int ov5640_init_controls(struct ov5640_dev *sensor)
+{
+	const struct ov5640_mode_info *mode = sensor->current_mode;
+	const struct v4l2_ctrl_ops *ops = &ov5640_ctrl_ops;
+	struct ov5640_ctrls *ctrls = &sensor->ctrls;
+	struct v4l2_ctrl_handler *hdl = &ctrls->handler;
+	struct v4l2_fwnode_device_properties props;
+	const struct ov5640_timings *timings;
+	unsigned int max_vblank;
+	unsigned int hblank;
+	int ret;
+
+	v4l2_ctrl_handler_init(hdl, 32);
+
+	/* we can use our own mutex for the ctrl lock */
+	hdl->lock = &sensor->lock;
+
+	/* Clock related controls */
+	ctrls->pixel_rate = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_PIXEL_RATE,
+			      ov5640_pixel_rates[OV5640_NUM_PIXEL_RATES - 1],
+			      ov5640_pixel_rates[0], 1,
+			      ov5640_pixel_rates[mode->pixel_rate]);
+
+	ctrls->link_freq = v4l2_ctrl_new_int_menu(hdl, ops,
+					V4L2_CID_LINK_FREQ,
+					ARRAY_SIZE(ov5640_csi2_link_freqs) - 1,
+					OV5640_DEFAULT_LINK_FREQ,
+					ov5640_csi2_link_freqs);
+
+	timings = ov5640_timings(sensor, mode);
+	hblank = timings->htot - mode->width;
+	ctrls->hblank = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HBLANK, hblank,
+					  hblank, 1, hblank);
+
+	max_vblank = OV5640_MAX_VTS - mode->height;
+	ctrls->vblank = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VBLANK,
+					  OV5640_MIN_VBLANK, max_vblank,
+					  1, timings->vblank_def);
+
+	/* Auto/manual white balance */
+	ctrls->auto_wb = v4l2_ctrl_new_std(hdl, ops,
+					   V4L2_CID_AUTO_WHITE_BALANCE,
+					   0, 1, 1, 1);
+	ctrls->blue_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BLUE_BALANCE,
+						0, 4095, 1, 0);
+	ctrls->red_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_RED_BALANCE,
+					       0, 4095, 1, 0);
+	/* Auto/manual exposure */
+	ctrls->auto_exp = v4l2_ctrl_new_std_menu(hdl, ops,
+						 V4L2_CID_EXPOSURE_AUTO,
+						 V4L2_EXPOSURE_MANUAL, 0,
+						 V4L2_EXPOSURE_AUTO);
+	ctrls->exposure = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
+					    0, 65535, 1, 0);
+	/* Auto/manual gain */
+	ctrls->auto_gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_AUTOGAIN,
+					     0, 1, 1, 1);
+	ctrls->gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_ANALOGUE_GAIN,
+					0, 1023, 1, 0);
+
+	ctrls->saturation = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SATURATION,
+					      0, 255, 1, 64);
+	ctrls->hue = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HUE,
+				       0, 359, 1, 0);
+	ctrls->contrast = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_CONTRAST,
+					    0, 255, 1, 0);
+	ctrls->test_pattern =
+		v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
+					     ARRAY_SIZE(test_pattern_menu) - 1,
+					     0, 0, test_pattern_menu);
+	ctrls->hflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP,
+					 0, 1, 1, 0);
+	ctrls->vflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP,
+					 0, 1, 1, 0);
+
+	ctrls->light_freq =
+		v4l2_ctrl_new_std_menu(hdl, ops,
+				       V4L2_CID_POWER_LINE_FREQUENCY,
+				       V4L2_CID_POWER_LINE_FREQUENCY_AUTO, 0,
+				       V4L2_CID_POWER_LINE_FREQUENCY_50HZ);
+
+	if (hdl->error) {
+		ret = hdl->error;
+		goto free_ctrls;
+	}
+
+	ret = v4l2_fwnode_device_parse(&sensor->i2c_client->dev, &props);
+	if (ret)
+		goto free_ctrls;
+
+	if (props.rotation == 180)
+		sensor->upside_down = true;
+
+	ret = v4l2_ctrl_new_fwnode_properties(hdl, ops, &props);
+	if (ret)
+		goto free_ctrls;
+
+	ctrls->pixel_rate->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+	ctrls->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+	ctrls->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+	ctrls->gain->flags |= V4L2_CTRL_FLAG_VOLATILE;
+	ctrls->exposure->flags |= V4L2_CTRL_FLAG_VOLATILE;
+
+	v4l2_ctrl_auto_cluster(3, &ctrls->auto_wb, 0, false);
+	v4l2_ctrl_auto_cluster(2, &ctrls->auto_gain, 0, true);
+	v4l2_ctrl_auto_cluster(2, &ctrls->auto_exp, 1, true);
+
+	sensor->sd.ctrl_handler = hdl;
+	return 0;
+
+free_ctrls:
+	v4l2_ctrl_handler_free(hdl);
+	return ret;
+}
+
+static int ov5640_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+	u32 bpp = ov5640_code_to_bpp(sensor, fse->code);
+	unsigned int index = fse->index;
+
+	if (fse->pad != 0)
+		return -EINVAL;
+	if (!bpp)
+		return -EINVAL;
+
+	/* Only low-resolution modes are supported for 24bpp formats. */
+	if (bpp == 24 && index >= OV5640_MODE_720P_1280_720)
+		return -EINVAL;
+
+	/* FIXME: Low resolution modes don't work in 8bpp formats. */
+	if (bpp == 8)
+		index += OV5640_MODE_720P_1280_720;
+
+	if (index >= OV5640_NUM_MODES)
+		return -EINVAL;
+
+	fse->min_width = ov5640_mode_data[index].width;
+	fse->max_width = fse->min_width;
+	fse->min_height = ov5640_mode_data[index].height;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static int ov5640_enum_frame_interval(
+	struct v4l2_subdev *sd,
+	struct v4l2_subdev_state *sd_state,
+	struct v4l2_subdev_frame_interval_enum *fie)
+{
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+	const struct ov5640_mode_info *mode;
+	struct v4l2_fract tpf;
+	int ret;
+
+	if (fie->pad != 0)
+		return -EINVAL;
+	if (fie->index >= OV5640_NUM_FRAMERATES)
+		return -EINVAL;
+
+	mode = ov5640_find_mode(sensor, fie->width, fie->height, false);
+	if (!mode)
+		return -EINVAL;
+
+	tpf.numerator = 1;
+	tpf.denominator = ov5640_framerates[fie->index];
+
+	ret = ov5640_try_frame_interval(sensor, &tpf, mode);
+	if (ret < 0)
+		return -EINVAL;
+
+	fie->interval = tpf;
+	return 0;
+}
+
+static int ov5640_g_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *fi)
+{
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+
+	mutex_lock(&sensor->lock);
+	fi->interval = sensor->frame_interval;
+	mutex_unlock(&sensor->lock);
+
+	return 0;
+}
+
+static int ov5640_s_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *fi)
+{
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+	const struct ov5640_mode_info *mode;
+	int frame_rate, ret = 0;
+
+	if (fi->pad != 0)
+		return -EINVAL;
+
+	mutex_lock(&sensor->lock);
+
+	if (sensor->streaming) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	mode = sensor->current_mode;
+
+	frame_rate = ov5640_try_frame_interval(sensor, &fi->interval, mode);
+	if (frame_rate < 0) {
+		/* Always return a valid frame interval value */
+		fi->interval = sensor->frame_interval;
+		goto out;
+	}
+
+	mode = ov5640_find_mode(sensor, mode->width, mode->height, true);
+	if (!mode) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (ov5640_framerates[frame_rate] > ov5640_framerates[mode->max_fps]) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (mode != sensor->current_mode ||
+	    frame_rate != sensor->current_fr) {
+		sensor->current_fr = frame_rate;
+		sensor->frame_interval = fi->interval;
+		sensor->current_mode = mode;
+		sensor->pending_mode_change = true;
+
+		ov5640_update_pixel_rate(sensor);
+	}
+out:
+	mutex_unlock(&sensor->lock);
+	return ret;
+}
+
+static int ov5640_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+	const struct ov5640_pixfmt *formats;
+	unsigned int num_formats;
+
+	if (ov5640_is_csi2(sensor)) {
+		formats = ov5640_csi2_formats;
+		num_formats = ARRAY_SIZE(ov5640_csi2_formats) - 1;
+	} else {
+		formats = ov5640_dvp_formats;
+		num_formats = ARRAY_SIZE(ov5640_dvp_formats) - 1;
+	}
+
+	if (code->index >= num_formats)
+		return -EINVAL;
+
+	code->code = formats[code->index].code;
+
+	return 0;
+}
+
+static int ov5640_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+	int ret = 0;
+
+	if (enable) {
+		ret = pm_runtime_resume_and_get(&sensor->i2c_client->dev);
+		if (ret < 0)
+			return ret;
+
+		ret = v4l2_ctrl_handler_setup(&sensor->ctrls.handler);
+		if (ret) {
+			pm_runtime_put(&sensor->i2c_client->dev);
+			return ret;
+		}
+	}
+
+	mutex_lock(&sensor->lock);
+
+	if (sensor->streaming == !enable) {
+		if (enable && sensor->pending_mode_change) {
+			ret = ov5640_set_mode(sensor);
+			if (ret)
+				goto out;
+		}
+
+		if (enable && sensor->pending_fmt_change) {
+			ret = ov5640_set_framefmt(sensor, &sensor->fmt);
+			if (ret)
+				goto out;
+			sensor->pending_fmt_change = false;
+		}
+
+		if (ov5640_is_csi2(sensor))
+			ret = ov5640_set_stream_mipi(sensor, enable);
+		else
+			ret = ov5640_set_stream_dvp(sensor, enable);
+
+		if (!ret)
+			sensor->streaming = enable;
+	}
+
+out:
+	mutex_unlock(&sensor->lock);
+
+	if (!enable || ret) {
+		pm_runtime_mark_last_busy(&sensor->i2c_client->dev);
+		pm_runtime_put_autosuspend(&sensor->i2c_client->dev);
+	}
+
+	return ret;
+}
+
+static int ov5640_init_cfg(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_state *state)
+{
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+	struct v4l2_mbus_framefmt *fmt =
+				v4l2_subdev_get_try_format(sd, state, 0);
+	struct v4l2_rect *crop = v4l2_subdev_get_try_crop(sd, state, 0);
+
+	*fmt = ov5640_is_csi2(sensor) ? ov5640_csi2_default_fmt :
+					ov5640_dvp_default_fmt;
+
+	crop->left = OV5640_PIXEL_ARRAY_LEFT;
+	crop->top = OV5640_PIXEL_ARRAY_TOP;
+	crop->width = OV5640_PIXEL_ARRAY_WIDTH;
+	crop->height = OV5640_PIXEL_ARRAY_HEIGHT;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops ov5640_core_ops = {
+	.log_status = v4l2_ctrl_subdev_log_status,
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_video_ops ov5640_video_ops = {
+	.g_frame_interval = ov5640_g_frame_interval,
+	.s_frame_interval = ov5640_s_frame_interval,
+	.s_stream = ov5640_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops ov5640_pad_ops = {
+	.init_cfg = ov5640_init_cfg,
+	.enum_mbus_code = ov5640_enum_mbus_code,
+	.get_fmt = ov5640_get_fmt,
+	.set_fmt = ov5640_set_fmt,
+	.get_selection = ov5640_get_selection,
+	.enum_frame_size = ov5640_enum_frame_size,
+	.enum_frame_interval = ov5640_enum_frame_interval,
+};
+
+static const struct v4l2_subdev_ops ov5640_subdev_ops = {
+	.core = &ov5640_core_ops,
+	.video = &ov5640_video_ops,
+	.pad = &ov5640_pad_ops,
+};
+
+static int ov5640_get_regulators(struct ov5640_dev *sensor)
+{
+	int i;
+
+	for (i = 0; i < OV5640_NUM_SUPPLIES; i++)
+		sensor->supplies[i].supply = ov5640_supply_name[i];
+
+	return devm_regulator_bulk_get(&sensor->i2c_client->dev,
+				       OV5640_NUM_SUPPLIES,
+				       sensor->supplies);
+}
+
+static int ov5640_check_chip_id(struct ov5640_dev *sensor)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	int ret = 0;
+	u16 chip_id;
+
+	ret = ov5640_read_reg16(sensor, OV5640_REG_CHIP_ID, &chip_id);
+	if (ret) {
+		dev_err(&client->dev, "%s: failed to read chip identifier\n",
+			__func__);
+		return ret;
+	}
+
+	if (chip_id != 0x5640) {
+		dev_err(&client->dev, "%s: wrong chip identifier, expected 0x5640, got 0x%x\n",
+			__func__, chip_id);
+		return -ENXIO;
+	}
+
+	return 0;
+}
+
+static int ov5640_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct fwnode_handle *endpoint;
+	struct ov5640_dev *sensor;
+	int ret;
+
+	sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
+	if (!sensor)
+		return -ENOMEM;
+
+	sensor->i2c_client = client;
+
+	/*
+	 * default init sequence initialize sensor to
+	 * YUV422 UYVY VGA(30FPS in parallel mode, 60 in MIPI CSI-2 mode)
+	 */
+	sensor->frame_interval.numerator = 1;
+	sensor->frame_interval.denominator = ov5640_framerates[OV5640_30_FPS];
+	sensor->current_fr = OV5640_30_FPS;
+	sensor->current_mode =
+		&ov5640_mode_data[OV5640_MODE_VGA_640_480];
+	sensor->last_mode = sensor->current_mode;
+	sensor->current_link_freq =
+		ov5640_csi2_link_freqs[OV5640_DEFAULT_LINK_FREQ];
+
+	sensor->ae_target = 52;
+
+	endpoint = fwnode_graph_get_next_endpoint(dev_fwnode(&client->dev),
+						  NULL);
+	if (!endpoint) {
+		dev_err(dev, "endpoint node not found\n");
+		return -EINVAL;
+	}
+
+	ret = v4l2_fwnode_endpoint_parse(endpoint, &sensor->ep);
+	fwnode_handle_put(endpoint);
+	if (ret) {
+		dev_err(dev, "Could not parse endpoint\n");
+		return ret;
+	}
+
+	if (sensor->ep.bus_type != V4L2_MBUS_PARALLEL &&
+	    sensor->ep.bus_type != V4L2_MBUS_CSI2_DPHY &&
+	    sensor->ep.bus_type != V4L2_MBUS_BT656) {
+		dev_err(dev, "Unsupported bus type %d\n", sensor->ep.bus_type);
+		return -EINVAL;
+	}
+
+	sensor->fmt = ov5640_is_csi2(sensor) ? ov5640_csi2_default_fmt :
+					       ov5640_dvp_default_fmt;
+
+	/* get system clock (xclk) */
+	sensor->xclk = devm_clk_get(dev, "xclk");
+	if (IS_ERR(sensor->xclk)) {
+		dev_err(dev, "failed to get xclk\n");
+		return PTR_ERR(sensor->xclk);
+	}
+
+	sensor->xclk_freq = clk_get_rate(sensor->xclk);
+	if (sensor->xclk_freq < OV5640_XCLK_MIN ||
+	    sensor->xclk_freq > OV5640_XCLK_MAX) {
+		dev_err(dev, "xclk frequency out of range: %d Hz\n",
+			sensor->xclk_freq);
+		return -EINVAL;
+	}
+
+	/* request optional power down pin */
+	sensor->pwdn_gpio = devm_gpiod_get_optional(dev, "powerdown",
+						    GPIOD_OUT_HIGH);
+	if (IS_ERR(sensor->pwdn_gpio))
+		return PTR_ERR(sensor->pwdn_gpio);
+
+	/* request optional reset pin */
+	sensor->reset_gpio = devm_gpiod_get_optional(dev, "reset",
+						     GPIOD_OUT_HIGH);
+	if (IS_ERR(sensor->reset_gpio))
+		return PTR_ERR(sensor->reset_gpio);
+
+	v4l2_i2c_subdev_init(&sensor->sd, client, &ov5640_subdev_ops);
+
+	sensor->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+			    V4L2_SUBDEV_FL_HAS_EVENTS;
+	sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
+	sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret = media_entity_pads_init(&sensor->sd.entity, 1, &sensor->pad);
+	if (ret)
+		return ret;
+
+	ret = ov5640_get_regulators(sensor);
+	if (ret)
+		goto entity_cleanup;
+
+	mutex_init(&sensor->lock);
+
+	ret = ov5640_init_controls(sensor);
+	if (ret)
+		goto entity_cleanup;
+
+	ret = ov5640_sensor_resume(dev);
+	if (ret) {
+		dev_err(dev, "failed to power on\n");
+		goto free_ctrls;
+	}
+
+	pm_runtime_set_active(dev);
+	pm_runtime_get_noresume(dev);
+	pm_runtime_enable(dev);
+
+	ret = ov5640_check_chip_id(sensor);
+	if (ret)
+		goto err_pm_runtime;
+
+	ret = v4l2_async_register_subdev_sensor(&sensor->sd);
+	if (ret)
+		goto err_pm_runtime;
+
+	pm_runtime_set_autosuspend_delay(dev, 1000);
+	pm_runtime_use_autosuspend(dev);
+	pm_runtime_mark_last_busy(dev);
+	pm_runtime_put_autosuspend(dev);
+
+	return 0;
+
+err_pm_runtime:
+	pm_runtime_put_noidle(dev);
+	pm_runtime_disable(dev);
+	ov5640_sensor_suspend(dev);
+free_ctrls:
+	v4l2_ctrl_handler_free(&sensor->ctrls.handler);
+entity_cleanup:
+	media_entity_cleanup(&sensor->sd.entity);
+	mutex_destroy(&sensor->lock);
+	return ret;
+}
+
+static void ov5640_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+	struct device *dev = &client->dev;
+
+	pm_runtime_disable(dev);
+	if (!pm_runtime_status_suspended(dev))
+		ov5640_sensor_suspend(dev);
+	pm_runtime_set_suspended(dev);
+
+	v4l2_async_unregister_subdev(&sensor->sd);
+	media_entity_cleanup(&sensor->sd.entity);
+	v4l2_ctrl_handler_free(&sensor->ctrls.handler);
+	mutex_destroy(&sensor->lock);
+}
+
+static const struct dev_pm_ops ov5640_pm_ops = {
+	SET_RUNTIME_PM_OPS(ov5640_sensor_suspend, ov5640_sensor_resume, NULL)
+};
+
+static const struct i2c_device_id ov5640_id[] = {
+	{"ov5640", 0},
+	{},
+};
+MODULE_DEVICE_TABLE(i2c, ov5640_id);
+
+static const struct of_device_id ov5640_dt_ids[] = {
+	{ .compatible = "ovti,ov5640" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, ov5640_dt_ids);
+
+static struct i2c_driver ov5640_i2c_driver = {
+	.driver = {
+		.name  = "ov5640",
+		.of_match_table	= ov5640_dt_ids,
+		.pm = &ov5640_pm_ops,
+	},
+	.id_table = ov5640_id,
+	.probe    = ov5640_probe,
+	.remove   = ov5640_remove,
+};
+
+module_i2c_driver(ov5640_i2c_driver);
+
+MODULE_DESCRIPTION("OV5640 MIPI Camera Subdev Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/ov5645.c b/drivers/media/i2c/ov5645.c
new file mode 100644
index 000000000..a70db7e60
--- /dev/null
+++ b/drivers/media/i2c/ov5645.c
@@ -0,0 +1,1298 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Driver for the OV5645 camera sensor.
+ *
+ * Copyright (c) 2011-2015, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2015 By Tech Design S.L. All Rights Reserved.
+ * Copyright (C) 2012-2013 Freescale Semiconductor, Inc. All Rights Reserved.
+ *
+ * Based on:
+ * - the OV5645 driver from QC msm-3.10 kernel on codeaurora.org:
+ *   https://us.codeaurora.org/cgit/quic/la/kernel/msm-3.10/tree/drivers/
+ *       media/platform/msm/camera_v2/sensor/ov5645.c?h=LA.BR.1.2.4_rb1.41
+ * - the OV5640 driver posted on linux-media:
+ *   https://www.mail-archive.com/linux-media%40vger.kernel.org/msg92671.html
+ */
+
+#include <linux/bitops.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_graph.h>
+#include <linux/pm_runtime.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+#define OV5645_SYSTEM_CTRL0		0x3008
+#define		OV5645_SYSTEM_CTRL0_START	0x02
+#define		OV5645_SYSTEM_CTRL0_STOP	0x42
+#define OV5645_CHIP_ID_HIGH		0x300a
+#define		OV5645_CHIP_ID_HIGH_BYTE	0x56
+#define OV5645_CHIP_ID_LOW		0x300b
+#define		OV5645_CHIP_ID_LOW_BYTE		0x45
+#define OV5645_IO_MIPI_CTRL00		0x300e
+#define OV5645_PAD_OUTPUT00		0x3019
+#define OV5645_AWB_MANUAL_CONTROL	0x3406
+#define		OV5645_AWB_MANUAL_ENABLE	BIT(0)
+#define OV5645_AEC_PK_MANUAL		0x3503
+#define		OV5645_AEC_MANUAL_ENABLE	BIT(0)
+#define		OV5645_AGC_MANUAL_ENABLE	BIT(1)
+#define OV5645_TIMING_TC_REG20		0x3820
+#define		OV5645_SENSOR_VFLIP		BIT(1)
+#define		OV5645_ISP_VFLIP		BIT(2)
+#define OV5645_TIMING_TC_REG21		0x3821
+#define		OV5645_SENSOR_MIRROR		BIT(1)
+#define OV5645_MIPI_CTRL00		0x4800
+#define OV5645_PRE_ISP_TEST_SETTING_1	0x503d
+#define		OV5645_TEST_PATTERN_MASK	0x3
+#define		OV5645_SET_TEST_PATTERN(x)	((x) & OV5645_TEST_PATTERN_MASK)
+#define		OV5645_TEST_PATTERN_ENABLE	BIT(7)
+#define OV5645_SDE_SAT_U		0x5583
+#define OV5645_SDE_SAT_V		0x5584
+
+/* regulator supplies */
+static const char * const ov5645_supply_name[] = {
+	"vdddo", /* Digital I/O (1.8V) supply */
+	"vdda",  /* Analog (2.8V) supply */
+	"vddd",  /* Digital Core (1.5V) supply */
+};
+
+#define OV5645_NUM_SUPPLIES ARRAY_SIZE(ov5645_supply_name)
+
+struct reg_value {
+	u16 reg;
+	u8 val;
+};
+
+struct ov5645_mode_info {
+	u32 width;
+	u32 height;
+	const struct reg_value *data;
+	u32 data_size;
+	u32 pixel_clock;
+	u32 link_freq;
+};
+
+struct ov5645 {
+	struct i2c_client *i2c_client;
+	struct device *dev;
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct v4l2_fwnode_endpoint ep;
+	struct v4l2_mbus_framefmt fmt;
+	struct v4l2_rect crop;
+	struct clk *xclk;
+
+	struct regulator_bulk_data supplies[OV5645_NUM_SUPPLIES];
+
+	const struct ov5645_mode_info *current_mode;
+
+	struct v4l2_ctrl_handler ctrls;
+	struct v4l2_ctrl *pixel_clock;
+	struct v4l2_ctrl *link_freq;
+
+	/* Cached register values */
+	u8 aec_pk_manual;
+	u8 timing_tc_reg20;
+	u8 timing_tc_reg21;
+
+	struct mutex power_lock; /* lock to protect power state */
+
+	struct gpio_desc *enable_gpio;
+	struct gpio_desc *rst_gpio;
+};
+
+static inline struct ov5645 *to_ov5645(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct ov5645, sd);
+}
+
+static const struct reg_value ov5645_global_init_setting[] = {
+	{ 0x3103, 0x11 },
+	{ 0x3008, 0x82 },
+	{ 0x3008, 0x42 },
+	{ 0x3103, 0x03 },
+	{ 0x3503, 0x07 },
+	{ 0x3002, 0x1c },
+	{ 0x3006, 0xc3 },
+	{ 0x3017, 0x00 },
+	{ 0x3018, 0x00 },
+	{ 0x302e, 0x0b },
+	{ 0x3037, 0x13 },
+	{ 0x3108, 0x01 },
+	{ 0x3611, 0x06 },
+	{ 0x3500, 0x00 },
+	{ 0x3501, 0x01 },
+	{ 0x3502, 0x00 },
+	{ 0x350a, 0x00 },
+	{ 0x350b, 0x3f },
+	{ 0x3620, 0x33 },
+	{ 0x3621, 0xe0 },
+	{ 0x3622, 0x01 },
+	{ 0x3630, 0x2e },
+	{ 0x3631, 0x00 },
+	{ 0x3632, 0x32 },
+	{ 0x3633, 0x52 },
+	{ 0x3634, 0x70 },
+	{ 0x3635, 0x13 },
+	{ 0x3636, 0x03 },
+	{ 0x3703, 0x5a },
+	{ 0x3704, 0xa0 },
+	{ 0x3705, 0x1a },
+	{ 0x3709, 0x12 },
+	{ 0x370b, 0x61 },
+	{ 0x370f, 0x10 },
+	{ 0x3715, 0x78 },
+	{ 0x3717, 0x01 },
+	{ 0x371b, 0x20 },
+	{ 0x3731, 0x12 },
+	{ 0x3901, 0x0a },
+	{ 0x3905, 0x02 },
+	{ 0x3906, 0x10 },
+	{ 0x3719, 0x86 },
+	{ 0x3810, 0x00 },
+	{ 0x3811, 0x10 },
+	{ 0x3812, 0x00 },
+	{ 0x3821, 0x01 },
+	{ 0x3824, 0x01 },
+	{ 0x3826, 0x03 },
+	{ 0x3828, 0x08 },
+	{ 0x3a19, 0xf8 },
+	{ 0x3c01, 0x34 },
+	{ 0x3c04, 0x28 },
+	{ 0x3c05, 0x98 },
+	{ 0x3c07, 0x07 },
+	{ 0x3c09, 0xc2 },
+	{ 0x3c0a, 0x9c },
+	{ 0x3c0b, 0x40 },
+	{ 0x3c01, 0x34 },
+	{ 0x4001, 0x02 },
+	{ 0x4514, 0x00 },
+	{ 0x4520, 0xb0 },
+	{ 0x460b, 0x37 },
+	{ 0x460c, 0x20 },
+	{ 0x4818, 0x01 },
+	{ 0x481d, 0xf0 },
+	{ 0x481f, 0x50 },
+	{ 0x4823, 0x70 },
+	{ 0x4831, 0x14 },
+	{ 0x5000, 0xa7 },
+	{ 0x5001, 0x83 },
+	{ 0x501d, 0x00 },
+	{ 0x501f, 0x00 },
+	{ 0x503d, 0x00 },
+	{ 0x505c, 0x30 },
+	{ 0x5181, 0x59 },
+	{ 0x5183, 0x00 },
+	{ 0x5191, 0xf0 },
+	{ 0x5192, 0x03 },
+	{ 0x5684, 0x10 },
+	{ 0x5685, 0xa0 },
+	{ 0x5686, 0x0c },
+	{ 0x5687, 0x78 },
+	{ 0x5a00, 0x08 },
+	{ 0x5a21, 0x00 },
+	{ 0x5a24, 0x00 },
+	{ 0x3008, 0x02 },
+	{ 0x3503, 0x00 },
+	{ 0x5180, 0xff },
+	{ 0x5181, 0xf2 },
+	{ 0x5182, 0x00 },
+	{ 0x5183, 0x14 },
+	{ 0x5184, 0x25 },
+	{ 0x5185, 0x24 },
+	{ 0x5186, 0x09 },
+	{ 0x5187, 0x09 },
+	{ 0x5188, 0x0a },
+	{ 0x5189, 0x75 },
+	{ 0x518a, 0x52 },
+	{ 0x518b, 0xea },
+	{ 0x518c, 0xa8 },
+	{ 0x518d, 0x42 },
+	{ 0x518e, 0x38 },
+	{ 0x518f, 0x56 },
+	{ 0x5190, 0x42 },
+	{ 0x5191, 0xf8 },
+	{ 0x5192, 0x04 },
+	{ 0x5193, 0x70 },
+	{ 0x5194, 0xf0 },
+	{ 0x5195, 0xf0 },
+	{ 0x5196, 0x03 },
+	{ 0x5197, 0x01 },
+	{ 0x5198, 0x04 },
+	{ 0x5199, 0x12 },
+	{ 0x519a, 0x04 },
+	{ 0x519b, 0x00 },
+	{ 0x519c, 0x06 },
+	{ 0x519d, 0x82 },
+	{ 0x519e, 0x38 },
+	{ 0x5381, 0x1e },
+	{ 0x5382, 0x5b },
+	{ 0x5383, 0x08 },
+	{ 0x5384, 0x0a },
+	{ 0x5385, 0x7e },
+	{ 0x5386, 0x88 },
+	{ 0x5387, 0x7c },
+	{ 0x5388, 0x6c },
+	{ 0x5389, 0x10 },
+	{ 0x538a, 0x01 },
+	{ 0x538b, 0x98 },
+	{ 0x5300, 0x08 },
+	{ 0x5301, 0x30 },
+	{ 0x5302, 0x10 },
+	{ 0x5303, 0x00 },
+	{ 0x5304, 0x08 },
+	{ 0x5305, 0x30 },
+	{ 0x5306, 0x08 },
+	{ 0x5307, 0x16 },
+	{ 0x5309, 0x08 },
+	{ 0x530a, 0x30 },
+	{ 0x530b, 0x04 },
+	{ 0x530c, 0x06 },
+	{ 0x5480, 0x01 },
+	{ 0x5481, 0x08 },
+	{ 0x5482, 0x14 },
+	{ 0x5483, 0x28 },
+	{ 0x5484, 0x51 },
+	{ 0x5485, 0x65 },
+	{ 0x5486, 0x71 },
+	{ 0x5487, 0x7d },
+	{ 0x5488, 0x87 },
+	{ 0x5489, 0x91 },
+	{ 0x548a, 0x9a },
+	{ 0x548b, 0xaa },
+	{ 0x548c, 0xb8 },
+	{ 0x548d, 0xcd },
+	{ 0x548e, 0xdd },
+	{ 0x548f, 0xea },
+	{ 0x5490, 0x1d },
+	{ 0x5580, 0x02 },
+	{ 0x5583, 0x40 },
+	{ 0x5584, 0x10 },
+	{ 0x5589, 0x10 },
+	{ 0x558a, 0x00 },
+	{ 0x558b, 0xf8 },
+	{ 0x5800, 0x3f },
+	{ 0x5801, 0x16 },
+	{ 0x5802, 0x0e },
+	{ 0x5803, 0x0d },
+	{ 0x5804, 0x17 },
+	{ 0x5805, 0x3f },
+	{ 0x5806, 0x0b },
+	{ 0x5807, 0x06 },
+	{ 0x5808, 0x04 },
+	{ 0x5809, 0x04 },
+	{ 0x580a, 0x06 },
+	{ 0x580b, 0x0b },
+	{ 0x580c, 0x09 },
+	{ 0x580d, 0x03 },
+	{ 0x580e, 0x00 },
+	{ 0x580f, 0x00 },
+	{ 0x5810, 0x03 },
+	{ 0x5811, 0x08 },
+	{ 0x5812, 0x0a },
+	{ 0x5813, 0x03 },
+	{ 0x5814, 0x00 },
+	{ 0x5815, 0x00 },
+	{ 0x5816, 0x04 },
+	{ 0x5817, 0x09 },
+	{ 0x5818, 0x0f },
+	{ 0x5819, 0x08 },
+	{ 0x581a, 0x06 },
+	{ 0x581b, 0x06 },
+	{ 0x581c, 0x08 },
+	{ 0x581d, 0x0c },
+	{ 0x581e, 0x3f },
+	{ 0x581f, 0x1e },
+	{ 0x5820, 0x12 },
+	{ 0x5821, 0x13 },
+	{ 0x5822, 0x21 },
+	{ 0x5823, 0x3f },
+	{ 0x5824, 0x68 },
+	{ 0x5825, 0x28 },
+	{ 0x5826, 0x2c },
+	{ 0x5827, 0x28 },
+	{ 0x5828, 0x08 },
+	{ 0x5829, 0x48 },
+	{ 0x582a, 0x64 },
+	{ 0x582b, 0x62 },
+	{ 0x582c, 0x64 },
+	{ 0x582d, 0x28 },
+	{ 0x582e, 0x46 },
+	{ 0x582f, 0x62 },
+	{ 0x5830, 0x60 },
+	{ 0x5831, 0x62 },
+	{ 0x5832, 0x26 },
+	{ 0x5833, 0x48 },
+	{ 0x5834, 0x66 },
+	{ 0x5835, 0x44 },
+	{ 0x5836, 0x64 },
+	{ 0x5837, 0x28 },
+	{ 0x5838, 0x66 },
+	{ 0x5839, 0x48 },
+	{ 0x583a, 0x2c },
+	{ 0x583b, 0x28 },
+	{ 0x583c, 0x26 },
+	{ 0x583d, 0xae },
+	{ 0x5025, 0x00 },
+	{ 0x3a0f, 0x30 },
+	{ 0x3a10, 0x28 },
+	{ 0x3a1b, 0x30 },
+	{ 0x3a1e, 0x26 },
+	{ 0x3a11, 0x60 },
+	{ 0x3a1f, 0x14 },
+	{ 0x0601, 0x02 },
+	{ 0x3008, 0x42 },
+	{ 0x3008, 0x02 },
+	{ OV5645_IO_MIPI_CTRL00, 0x40 },
+	{ OV5645_MIPI_CTRL00, 0x24 },
+	{ OV5645_PAD_OUTPUT00, 0x70 }
+};
+
+static const struct reg_value ov5645_setting_sxga[] = {
+	{ 0x3612, 0xa9 },
+	{ 0x3614, 0x50 },
+	{ 0x3618, 0x00 },
+	{ 0x3034, 0x18 },
+	{ 0x3035, 0x21 },
+	{ 0x3036, 0x70 },
+	{ 0x3600, 0x09 },
+	{ 0x3601, 0x43 },
+	{ 0x3708, 0x66 },
+	{ 0x370c, 0xc3 },
+	{ 0x3800, 0x00 },
+	{ 0x3801, 0x00 },
+	{ 0x3802, 0x00 },
+	{ 0x3803, 0x06 },
+	{ 0x3804, 0x0a },
+	{ 0x3805, 0x3f },
+	{ 0x3806, 0x07 },
+	{ 0x3807, 0x9d },
+	{ 0x3808, 0x05 },
+	{ 0x3809, 0x00 },
+	{ 0x380a, 0x03 },
+	{ 0x380b, 0xc0 },
+	{ 0x380c, 0x07 },
+	{ 0x380d, 0x68 },
+	{ 0x380e, 0x03 },
+	{ 0x380f, 0xd8 },
+	{ 0x3813, 0x06 },
+	{ 0x3814, 0x31 },
+	{ 0x3815, 0x31 },
+	{ 0x3820, 0x47 },
+	{ 0x3a02, 0x03 },
+	{ 0x3a03, 0xd8 },
+	{ 0x3a08, 0x01 },
+	{ 0x3a09, 0xf8 },
+	{ 0x3a0a, 0x01 },
+	{ 0x3a0b, 0xa4 },
+	{ 0x3a0e, 0x02 },
+	{ 0x3a0d, 0x02 },
+	{ 0x3a14, 0x03 },
+	{ 0x3a15, 0xd8 },
+	{ 0x3a18, 0x00 },
+	{ 0x4004, 0x02 },
+	{ 0x4005, 0x18 },
+	{ 0x4300, 0x32 },
+	{ 0x4202, 0x00 }
+};
+
+static const struct reg_value ov5645_setting_1080p[] = {
+	{ 0x3612, 0xab },
+	{ 0x3614, 0x50 },
+	{ 0x3618, 0x04 },
+	{ 0x3034, 0x18 },
+	{ 0x3035, 0x11 },
+	{ 0x3036, 0x54 },
+	{ 0x3600, 0x08 },
+	{ 0x3601, 0x33 },
+	{ 0x3708, 0x63 },
+	{ 0x370c, 0xc0 },
+	{ 0x3800, 0x01 },
+	{ 0x3801, 0x50 },
+	{ 0x3802, 0x01 },
+	{ 0x3803, 0xb2 },
+	{ 0x3804, 0x08 },
+	{ 0x3805, 0xef },
+	{ 0x3806, 0x05 },
+	{ 0x3807, 0xf1 },
+	{ 0x3808, 0x07 },
+	{ 0x3809, 0x80 },
+	{ 0x380a, 0x04 },
+	{ 0x380b, 0x38 },
+	{ 0x380c, 0x09 },
+	{ 0x380d, 0xc4 },
+	{ 0x380e, 0x04 },
+	{ 0x380f, 0x60 },
+	{ 0x3813, 0x04 },
+	{ 0x3814, 0x11 },
+	{ 0x3815, 0x11 },
+	{ 0x3820, 0x47 },
+	{ 0x4514, 0x88 },
+	{ 0x3a02, 0x04 },
+	{ 0x3a03, 0x60 },
+	{ 0x3a08, 0x01 },
+	{ 0x3a09, 0x50 },
+	{ 0x3a0a, 0x01 },
+	{ 0x3a0b, 0x18 },
+	{ 0x3a0e, 0x03 },
+	{ 0x3a0d, 0x04 },
+	{ 0x3a14, 0x04 },
+	{ 0x3a15, 0x60 },
+	{ 0x3a18, 0x00 },
+	{ 0x4004, 0x06 },
+	{ 0x4005, 0x18 },
+	{ 0x4300, 0x32 },
+	{ 0x4202, 0x00 },
+	{ 0x4837, 0x0b }
+};
+
+static const struct reg_value ov5645_setting_full[] = {
+	{ 0x3612, 0xab },
+	{ 0x3614, 0x50 },
+	{ 0x3618, 0x04 },
+	{ 0x3034, 0x18 },
+	{ 0x3035, 0x11 },
+	{ 0x3036, 0x54 },
+	{ 0x3600, 0x08 },
+	{ 0x3601, 0x33 },
+	{ 0x3708, 0x63 },
+	{ 0x370c, 0xc0 },
+	{ 0x3800, 0x00 },
+	{ 0x3801, 0x00 },
+	{ 0x3802, 0x00 },
+	{ 0x3803, 0x00 },
+	{ 0x3804, 0x0a },
+	{ 0x3805, 0x3f },
+	{ 0x3806, 0x07 },
+	{ 0x3807, 0x9f },
+	{ 0x3808, 0x0a },
+	{ 0x3809, 0x20 },
+	{ 0x380a, 0x07 },
+	{ 0x380b, 0x98 },
+	{ 0x380c, 0x0b },
+	{ 0x380d, 0x1c },
+	{ 0x380e, 0x07 },
+	{ 0x380f, 0xb0 },
+	{ 0x3813, 0x06 },
+	{ 0x3814, 0x11 },
+	{ 0x3815, 0x11 },
+	{ 0x3820, 0x47 },
+	{ 0x4514, 0x88 },
+	{ 0x3a02, 0x07 },
+	{ 0x3a03, 0xb0 },
+	{ 0x3a08, 0x01 },
+	{ 0x3a09, 0x27 },
+	{ 0x3a0a, 0x00 },
+	{ 0x3a0b, 0xf6 },
+	{ 0x3a0e, 0x06 },
+	{ 0x3a0d, 0x08 },
+	{ 0x3a14, 0x07 },
+	{ 0x3a15, 0xb0 },
+	{ 0x3a18, 0x01 },
+	{ 0x4004, 0x06 },
+	{ 0x4005, 0x18 },
+	{ 0x4300, 0x32 },
+	{ 0x4837, 0x0b },
+	{ 0x4202, 0x00 }
+};
+
+static const s64 link_freq[] = {
+	224000000,
+	336000000
+};
+
+static const struct ov5645_mode_info ov5645_mode_info_data[] = {
+	{
+		.width = 1280,
+		.height = 960,
+		.data = ov5645_setting_sxga,
+		.data_size = ARRAY_SIZE(ov5645_setting_sxga),
+		.pixel_clock = 112000000,
+		.link_freq = 0 /* an index in link_freq[] */
+	},
+	{
+		.width = 1920,
+		.height = 1080,
+		.data = ov5645_setting_1080p,
+		.data_size = ARRAY_SIZE(ov5645_setting_1080p),
+		.pixel_clock = 168000000,
+		.link_freq = 1 /* an index in link_freq[] */
+	},
+	{
+		.width = 2592,
+		.height = 1944,
+		.data = ov5645_setting_full,
+		.data_size = ARRAY_SIZE(ov5645_setting_full),
+		.pixel_clock = 168000000,
+		.link_freq = 1 /* an index in link_freq[] */
+	},
+};
+
+static int ov5645_write_reg(struct ov5645 *ov5645, u16 reg, u8 val)
+{
+	u8 regbuf[3];
+	int ret;
+
+	regbuf[0] = reg >> 8;
+	regbuf[1] = reg & 0xff;
+	regbuf[2] = val;
+
+	ret = i2c_master_send(ov5645->i2c_client, regbuf, 3);
+	if (ret < 0) {
+		dev_err(ov5645->dev, "%s: write reg error %d: reg=%x, val=%x\n",
+			__func__, ret, reg, val);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ov5645_read_reg(struct ov5645 *ov5645, u16 reg, u8 *val)
+{
+	u8 regbuf[2];
+	int ret;
+
+	regbuf[0] = reg >> 8;
+	regbuf[1] = reg & 0xff;
+
+	ret = i2c_master_send(ov5645->i2c_client, regbuf, 2);
+	if (ret < 0) {
+		dev_err(ov5645->dev, "%s: write reg error %d: reg=%x\n",
+			__func__, ret, reg);
+		return ret;
+	}
+
+	ret = i2c_master_recv(ov5645->i2c_client, val, 1);
+	if (ret < 0) {
+		dev_err(ov5645->dev, "%s: read reg error %d: reg=%x\n",
+			__func__, ret, reg);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ov5645_set_aec_mode(struct ov5645 *ov5645, u32 mode)
+{
+	u8 val = ov5645->aec_pk_manual;
+	int ret;
+
+	if (mode == V4L2_EXPOSURE_AUTO)
+		val &= ~OV5645_AEC_MANUAL_ENABLE;
+	else /* V4L2_EXPOSURE_MANUAL */
+		val |= OV5645_AEC_MANUAL_ENABLE;
+
+	ret = ov5645_write_reg(ov5645, OV5645_AEC_PK_MANUAL, val);
+	if (!ret)
+		ov5645->aec_pk_manual = val;
+
+	return ret;
+}
+
+static int ov5645_set_agc_mode(struct ov5645 *ov5645, u32 enable)
+{
+	u8 val = ov5645->aec_pk_manual;
+	int ret;
+
+	if (enable)
+		val &= ~OV5645_AGC_MANUAL_ENABLE;
+	else
+		val |= OV5645_AGC_MANUAL_ENABLE;
+
+	ret = ov5645_write_reg(ov5645, OV5645_AEC_PK_MANUAL, val);
+	if (!ret)
+		ov5645->aec_pk_manual = val;
+
+	return ret;
+}
+
+static int ov5645_set_register_array(struct ov5645 *ov5645,
+				     const struct reg_value *settings,
+				     unsigned int num_settings)
+{
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < num_settings; ++i, ++settings) {
+		ret = ov5645_write_reg(ov5645, settings->reg, settings->val);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int ov5645_set_power_off(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov5645 *ov5645 = to_ov5645(sd);
+
+	ov5645_write_reg(ov5645, OV5645_IO_MIPI_CTRL00, 0x58);
+	gpiod_set_value_cansleep(ov5645->rst_gpio, 1);
+	gpiod_set_value_cansleep(ov5645->enable_gpio, 0);
+	clk_disable_unprepare(ov5645->xclk);
+	regulator_bulk_disable(OV5645_NUM_SUPPLIES, ov5645->supplies);
+
+	return 0;
+}
+
+static int ov5645_set_power_on(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov5645 *ov5645 = to_ov5645(sd);
+	int ret;
+
+	ret = regulator_bulk_enable(OV5645_NUM_SUPPLIES, ov5645->supplies);
+	if (ret < 0)
+		return ret;
+
+	ret = clk_prepare_enable(ov5645->xclk);
+	if (ret < 0) {
+		dev_err(ov5645->dev, "clk prepare enable failed\n");
+		regulator_bulk_disable(OV5645_NUM_SUPPLIES, ov5645->supplies);
+		return ret;
+	}
+
+	usleep_range(5000, 15000);
+	gpiod_set_value_cansleep(ov5645->enable_gpio, 1);
+
+	usleep_range(1000, 2000);
+	gpiod_set_value_cansleep(ov5645->rst_gpio, 0);
+
+	msleep(20);
+
+	ret = ov5645_set_register_array(ov5645, ov5645_global_init_setting,
+					ARRAY_SIZE(ov5645_global_init_setting));
+	if (ret < 0) {
+		dev_err(ov5645->dev, "could not set init registers\n");
+		goto exit;
+	}
+
+	usleep_range(500, 1000);
+
+	return 0;
+
+exit:
+	ov5645_set_power_off(dev);
+	return ret;
+}
+
+static int ov5645_set_saturation(struct ov5645 *ov5645, s32 value)
+{
+	u32 reg_value = (value * 0x10) + 0x40;
+	int ret;
+
+	ret = ov5645_write_reg(ov5645, OV5645_SDE_SAT_U, reg_value);
+	if (ret < 0)
+		return ret;
+
+	return ov5645_write_reg(ov5645, OV5645_SDE_SAT_V, reg_value);
+}
+
+static int ov5645_set_hflip(struct ov5645 *ov5645, s32 value)
+{
+	u8 val = ov5645->timing_tc_reg21;
+	int ret;
+
+	if (value == 0)
+		val &= ~(OV5645_SENSOR_MIRROR);
+	else
+		val |= (OV5645_SENSOR_MIRROR);
+
+	ret = ov5645_write_reg(ov5645, OV5645_TIMING_TC_REG21, val);
+	if (!ret)
+		ov5645->timing_tc_reg21 = val;
+
+	return ret;
+}
+
+static int ov5645_set_vflip(struct ov5645 *ov5645, s32 value)
+{
+	u8 val = ov5645->timing_tc_reg20;
+	int ret;
+
+	if (value == 0)
+		val |= (OV5645_SENSOR_VFLIP | OV5645_ISP_VFLIP);
+	else
+		val &= ~(OV5645_SENSOR_VFLIP | OV5645_ISP_VFLIP);
+
+	ret = ov5645_write_reg(ov5645, OV5645_TIMING_TC_REG20, val);
+	if (!ret)
+		ov5645->timing_tc_reg20 = val;
+
+	return ret;
+}
+
+static int ov5645_set_test_pattern(struct ov5645 *ov5645, s32 value)
+{
+	u8 val = 0;
+
+	if (value) {
+		val = OV5645_SET_TEST_PATTERN(value - 1);
+		val |= OV5645_TEST_PATTERN_ENABLE;
+	}
+
+	return ov5645_write_reg(ov5645, OV5645_PRE_ISP_TEST_SETTING_1, val);
+}
+
+static const char * const ov5645_test_pattern_menu[] = {
+	"Disabled",
+	"Vertical Color Bars",
+	"Pseudo-Random Data",
+	"Color Square",
+	"Black Image",
+};
+
+static int ov5645_set_awb(struct ov5645 *ov5645, s32 enable_auto)
+{
+	u8 val = 0;
+
+	if (!enable_auto)
+		val = OV5645_AWB_MANUAL_ENABLE;
+
+	return ov5645_write_reg(ov5645, OV5645_AWB_MANUAL_CONTROL, val);
+}
+
+static int ov5645_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov5645 *ov5645 = container_of(ctrl->handler,
+					     struct ov5645, ctrls);
+	int ret;
+
+	mutex_lock(&ov5645->power_lock);
+	if (!pm_runtime_get_if_in_use(ov5645->dev)) {
+		mutex_unlock(&ov5645->power_lock);
+		return 0;
+	}
+
+	switch (ctrl->id) {
+	case V4L2_CID_SATURATION:
+		ret = ov5645_set_saturation(ov5645, ctrl->val);
+		break;
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		ret = ov5645_set_awb(ov5645, ctrl->val);
+		break;
+	case V4L2_CID_AUTOGAIN:
+		ret = ov5645_set_agc_mode(ov5645, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE_AUTO:
+		ret = ov5645_set_aec_mode(ov5645, ctrl->val);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		ret = ov5645_set_test_pattern(ov5645, ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+		ret = ov5645_set_hflip(ov5645, ctrl->val);
+		break;
+	case V4L2_CID_VFLIP:
+		ret = ov5645_set_vflip(ov5645, ctrl->val);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	pm_runtime_mark_last_busy(ov5645->dev);
+	pm_runtime_put_autosuspend(ov5645->dev);
+	mutex_unlock(&ov5645->power_lock);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ov5645_ctrl_ops = {
+	.s_ctrl = ov5645_s_ctrl,
+};
+
+static int ov5645_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index > 0)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_UYVY8_1X16;
+
+	return 0;
+}
+
+static int ov5645_enum_frame_size(struct v4l2_subdev *subdev,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->code != MEDIA_BUS_FMT_UYVY8_1X16)
+		return -EINVAL;
+
+	if (fse->index >= ARRAY_SIZE(ov5645_mode_info_data))
+		return -EINVAL;
+
+	fse->min_width = ov5645_mode_info_data[fse->index].width;
+	fse->max_width = ov5645_mode_info_data[fse->index].width;
+	fse->min_height = ov5645_mode_info_data[fse->index].height;
+	fse->max_height = ov5645_mode_info_data[fse->index].height;
+
+	return 0;
+}
+
+static struct v4l2_mbus_framefmt *
+__ov5645_get_pad_format(struct ov5645 *ov5645,
+			struct v4l2_subdev_state *sd_state,
+			unsigned int pad,
+			enum v4l2_subdev_format_whence which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_format(&ov5645->sd, sd_state, pad);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return &ov5645->fmt;
+	default:
+		return NULL;
+	}
+}
+
+static int ov5645_get_format(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *sd_state,
+			     struct v4l2_subdev_format *format)
+{
+	struct ov5645 *ov5645 = to_ov5645(sd);
+
+	format->format = *__ov5645_get_pad_format(ov5645, sd_state,
+						  format->pad,
+						  format->which);
+	return 0;
+}
+
+static struct v4l2_rect *
+__ov5645_get_pad_crop(struct ov5645 *ov5645,
+		      struct v4l2_subdev_state *sd_state,
+		      unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_crop(&ov5645->sd, sd_state, pad);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return &ov5645->crop;
+	default:
+		return NULL;
+	}
+}
+
+static int ov5645_set_format(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *sd_state,
+			     struct v4l2_subdev_format *format)
+{
+	struct ov5645 *ov5645 = to_ov5645(sd);
+	struct v4l2_mbus_framefmt *__format;
+	struct v4l2_rect *__crop;
+	const struct ov5645_mode_info *new_mode;
+	int ret;
+
+	__crop = __ov5645_get_pad_crop(ov5645, sd_state, format->pad,
+				       format->which);
+
+	new_mode = v4l2_find_nearest_size(ov5645_mode_info_data,
+			       ARRAY_SIZE(ov5645_mode_info_data),
+			       width, height,
+			       format->format.width, format->format.height);
+
+	__crop->width = new_mode->width;
+	__crop->height = new_mode->height;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		ret = v4l2_ctrl_s_ctrl_int64(ov5645->pixel_clock,
+					     new_mode->pixel_clock);
+		if (ret < 0)
+			return ret;
+
+		ret = v4l2_ctrl_s_ctrl(ov5645->link_freq,
+				       new_mode->link_freq);
+		if (ret < 0)
+			return ret;
+
+		ov5645->current_mode = new_mode;
+	}
+
+	__format = __ov5645_get_pad_format(ov5645, sd_state, format->pad,
+					   format->which);
+	__format->width = __crop->width;
+	__format->height = __crop->height;
+	__format->code = MEDIA_BUS_FMT_UYVY8_1X16;
+	__format->field = V4L2_FIELD_NONE;
+	__format->colorspace = V4L2_COLORSPACE_SRGB;
+
+	format->format = *__format;
+
+	return 0;
+}
+
+static int ov5645_entity_init_cfg(struct v4l2_subdev *subdev,
+				  struct v4l2_subdev_state *sd_state)
+{
+	struct v4l2_subdev_format fmt = { 0 };
+
+	fmt.which = sd_state ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE;
+	fmt.format.width = 1920;
+	fmt.format.height = 1080;
+
+	ov5645_set_format(subdev, sd_state, &fmt);
+
+	return 0;
+}
+
+static int ov5645_get_selection(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_state *sd_state,
+			   struct v4l2_subdev_selection *sel)
+{
+	struct ov5645 *ov5645 = to_ov5645(sd);
+
+	if (sel->target != V4L2_SEL_TGT_CROP)
+		return -EINVAL;
+
+	sel->r = *__ov5645_get_pad_crop(ov5645, sd_state, sel->pad,
+					sel->which);
+	return 0;
+}
+
+static int ov5645_s_stream(struct v4l2_subdev *subdev, int enable)
+{
+	struct ov5645 *ov5645 = to_ov5645(subdev);
+	int ret;
+
+	if (enable) {
+		ret = pm_runtime_resume_and_get(ov5645->dev);
+		if (ret < 0)
+			return ret;
+
+		ret = ov5645_set_register_array(ov5645,
+					ov5645->current_mode->data,
+					ov5645->current_mode->data_size);
+		if (ret < 0) {
+			dev_err(ov5645->dev, "could not set mode %dx%d\n",
+				ov5645->current_mode->width,
+				ov5645->current_mode->height);
+			goto err_rpm_put;
+		}
+		ret = v4l2_ctrl_handler_setup(&ov5645->ctrls);
+		if (ret < 0) {
+			dev_err(ov5645->dev, "could not sync v4l2 controls\n");
+			goto err_rpm_put;
+		}
+
+		ret = ov5645_write_reg(ov5645, OV5645_IO_MIPI_CTRL00, 0x45);
+		if (ret < 0)
+			goto err_rpm_put;
+
+		ret = ov5645_write_reg(ov5645, OV5645_SYSTEM_CTRL0,
+				       OV5645_SYSTEM_CTRL0_START);
+		if (ret < 0)
+			goto err_rpm_put;
+	} else {
+		ret = ov5645_write_reg(ov5645, OV5645_IO_MIPI_CTRL00, 0x40);
+		if (ret < 0)
+			goto stream_off_rpm_put;
+
+		ret = ov5645_write_reg(ov5645, OV5645_SYSTEM_CTRL0,
+				       OV5645_SYSTEM_CTRL0_STOP);
+
+		goto stream_off_rpm_put;
+	}
+
+	return 0;
+
+err_rpm_put:
+	pm_runtime_put_sync(ov5645->dev);
+	return ret;
+
+stream_off_rpm_put:
+	pm_runtime_mark_last_busy(ov5645->dev);
+	pm_runtime_put_autosuspend(ov5645->dev);
+	return ret;
+}
+
+static const struct v4l2_subdev_video_ops ov5645_video_ops = {
+	.s_stream = ov5645_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops ov5645_subdev_pad_ops = {
+	.init_cfg = ov5645_entity_init_cfg,
+	.enum_mbus_code = ov5645_enum_mbus_code,
+	.enum_frame_size = ov5645_enum_frame_size,
+	.get_fmt = ov5645_get_format,
+	.set_fmt = ov5645_set_format,
+	.get_selection = ov5645_get_selection,
+};
+
+static const struct v4l2_subdev_ops ov5645_subdev_ops = {
+	.video = &ov5645_video_ops,
+	.pad = &ov5645_subdev_pad_ops,
+};
+
+static int ov5645_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct device_node *endpoint;
+	struct ov5645 *ov5645;
+	u8 chip_id_high, chip_id_low;
+	unsigned int i;
+	u32 xclk_freq;
+	int ret;
+
+	ov5645 = devm_kzalloc(dev, sizeof(struct ov5645), GFP_KERNEL);
+	if (!ov5645)
+		return -ENOMEM;
+
+	ov5645->i2c_client = client;
+	ov5645->dev = dev;
+
+	endpoint = of_graph_get_next_endpoint(dev->of_node, NULL);
+	if (!endpoint) {
+		dev_err(dev, "endpoint node not found\n");
+		return -EINVAL;
+	}
+
+	ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(endpoint),
+					 &ov5645->ep);
+
+	of_node_put(endpoint);
+
+	if (ret < 0) {
+		dev_err(dev, "parsing endpoint node failed\n");
+		return ret;
+	}
+
+	if (ov5645->ep.bus_type != V4L2_MBUS_CSI2_DPHY) {
+		dev_err(dev, "invalid bus type, must be CSI2\n");
+		return -EINVAL;
+	}
+
+	/* get system clock (xclk) */
+	ov5645->xclk = devm_clk_get(dev, NULL);
+	if (IS_ERR(ov5645->xclk)) {
+		dev_err(dev, "could not get xclk");
+		return PTR_ERR(ov5645->xclk);
+	}
+
+	ret = of_property_read_u32(dev->of_node, "clock-frequency", &xclk_freq);
+	if (ret) {
+		dev_err(dev, "could not get xclk frequency\n");
+		return ret;
+	}
+
+	/* external clock must be 24MHz, allow 1% tolerance */
+	if (xclk_freq < 23760000 || xclk_freq > 24240000) {
+		dev_err(dev, "external clock frequency %u is not supported\n",
+			xclk_freq);
+		return -EINVAL;
+	}
+
+	ret = clk_set_rate(ov5645->xclk, xclk_freq);
+	if (ret) {
+		dev_err(dev, "could not set xclk frequency\n");
+		return ret;
+	}
+
+	for (i = 0; i < OV5645_NUM_SUPPLIES; i++)
+		ov5645->supplies[i].supply = ov5645_supply_name[i];
+
+	ret = devm_regulator_bulk_get(dev, OV5645_NUM_SUPPLIES,
+				      ov5645->supplies);
+	if (ret < 0)
+		return ret;
+
+	ov5645->enable_gpio = devm_gpiod_get(dev, "enable", GPIOD_OUT_HIGH);
+	if (IS_ERR(ov5645->enable_gpio)) {
+		dev_err(dev, "cannot get enable gpio\n");
+		return PTR_ERR(ov5645->enable_gpio);
+	}
+
+	ov5645->rst_gpio = devm_gpiod_get(dev, "reset", GPIOD_OUT_HIGH);
+	if (IS_ERR(ov5645->rst_gpio)) {
+		dev_err(dev, "cannot get reset gpio\n");
+		return PTR_ERR(ov5645->rst_gpio);
+	}
+
+	mutex_init(&ov5645->power_lock);
+
+	v4l2_ctrl_handler_init(&ov5645->ctrls, 9);
+	v4l2_ctrl_new_std(&ov5645->ctrls, &ov5645_ctrl_ops,
+			  V4L2_CID_SATURATION, -4, 4, 1, 0);
+	v4l2_ctrl_new_std(&ov5645->ctrls, &ov5645_ctrl_ops,
+			  V4L2_CID_HFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&ov5645->ctrls, &ov5645_ctrl_ops,
+			  V4L2_CID_VFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&ov5645->ctrls, &ov5645_ctrl_ops,
+			  V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+	v4l2_ctrl_new_std(&ov5645->ctrls, &ov5645_ctrl_ops,
+			  V4L2_CID_AUTO_WHITE_BALANCE, 0, 1, 1, 1);
+	v4l2_ctrl_new_std_menu(&ov5645->ctrls, &ov5645_ctrl_ops,
+			       V4L2_CID_EXPOSURE_AUTO, V4L2_EXPOSURE_MANUAL,
+			       0, V4L2_EXPOSURE_AUTO);
+	v4l2_ctrl_new_std_menu_items(&ov5645->ctrls, &ov5645_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(ov5645_test_pattern_menu) - 1,
+				     0, 0, ov5645_test_pattern_menu);
+	ov5645->pixel_clock = v4l2_ctrl_new_std(&ov5645->ctrls,
+						&ov5645_ctrl_ops,
+						V4L2_CID_PIXEL_RATE,
+						1, INT_MAX, 1, 1);
+	ov5645->link_freq = v4l2_ctrl_new_int_menu(&ov5645->ctrls,
+						   &ov5645_ctrl_ops,
+						   V4L2_CID_LINK_FREQ,
+						   ARRAY_SIZE(link_freq) - 1,
+						   0, link_freq);
+	if (ov5645->link_freq)
+		ov5645->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	ov5645->sd.ctrl_handler = &ov5645->ctrls;
+
+	if (ov5645->ctrls.error) {
+		dev_err(dev, "%s: control initialization error %d\n",
+		       __func__, ov5645->ctrls.error);
+		ret = ov5645->ctrls.error;
+		goto free_ctrl;
+	}
+
+	v4l2_i2c_subdev_init(&ov5645->sd, client, &ov5645_subdev_ops);
+	ov5645->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	ov5645->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ov5645->sd.dev = &client->dev;
+	ov5645->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+	ret = media_entity_pads_init(&ov5645->sd.entity, 1, &ov5645->pad);
+	if (ret < 0) {
+		dev_err(dev, "could not register media entity\n");
+		goto free_ctrl;
+	}
+
+	ret = ov5645_set_power_on(dev);
+	if (ret)
+		goto free_entity;
+
+	ret = ov5645_read_reg(ov5645, OV5645_CHIP_ID_HIGH, &chip_id_high);
+	if (ret < 0 || chip_id_high != OV5645_CHIP_ID_HIGH_BYTE) {
+		dev_err(dev, "could not read ID high\n");
+		ret = -ENODEV;
+		goto power_down;
+	}
+	ret = ov5645_read_reg(ov5645, OV5645_CHIP_ID_LOW, &chip_id_low);
+	if (ret < 0 || chip_id_low != OV5645_CHIP_ID_LOW_BYTE) {
+		dev_err(dev, "could not read ID low\n");
+		ret = -ENODEV;
+		goto power_down;
+	}
+
+	dev_info(dev, "OV5645 detected at address 0x%02x\n", client->addr);
+
+	ret = ov5645_read_reg(ov5645, OV5645_AEC_PK_MANUAL,
+			      &ov5645->aec_pk_manual);
+	if (ret < 0) {
+		dev_err(dev, "could not read AEC/AGC mode\n");
+		ret = -ENODEV;
+		goto power_down;
+	}
+
+	ret = ov5645_read_reg(ov5645, OV5645_TIMING_TC_REG20,
+			      &ov5645->timing_tc_reg20);
+	if (ret < 0) {
+		dev_err(dev, "could not read vflip value\n");
+		ret = -ENODEV;
+		goto power_down;
+	}
+
+	ret = ov5645_read_reg(ov5645, OV5645_TIMING_TC_REG21,
+			      &ov5645->timing_tc_reg21);
+	if (ret < 0) {
+		dev_err(dev, "could not read hflip value\n");
+		ret = -ENODEV;
+		goto power_down;
+	}
+
+	pm_runtime_set_active(dev);
+	pm_runtime_get_noresume(dev);
+	pm_runtime_enable(dev);
+
+	ov5645_entity_init_cfg(&ov5645->sd, NULL);
+
+	ret = v4l2_async_register_subdev(&ov5645->sd);
+	if (ret < 0) {
+		dev_err(dev, "could not register v4l2 device\n");
+		goto err_pm_runtime;
+	}
+
+	pm_runtime_set_autosuspend_delay(dev, 1000);
+	pm_runtime_use_autosuspend(dev);
+	pm_runtime_mark_last_busy(dev);
+	pm_runtime_put_autosuspend(dev);
+
+	return 0;
+
+err_pm_runtime:
+	pm_runtime_disable(dev);
+	pm_runtime_put_noidle(dev);
+power_down:
+	ov5645_set_power_off(dev);
+free_entity:
+	media_entity_cleanup(&ov5645->sd.entity);
+free_ctrl:
+	v4l2_ctrl_handler_free(&ov5645->ctrls);
+	mutex_destroy(&ov5645->power_lock);
+
+	return ret;
+}
+
+static void ov5645_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov5645 *ov5645 = to_ov5645(sd);
+
+	v4l2_async_unregister_subdev(&ov5645->sd);
+	media_entity_cleanup(&ov5645->sd.entity);
+	v4l2_ctrl_handler_free(&ov5645->ctrls);
+	pm_runtime_disable(ov5645->dev);
+	if (!pm_runtime_status_suspended(ov5645->dev))
+		ov5645_set_power_off(ov5645->dev);
+	pm_runtime_set_suspended(ov5645->dev);
+	mutex_destroy(&ov5645->power_lock);
+}
+
+static const struct i2c_device_id ov5645_id[] = {
+	{ "ov5645", 0 },
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, ov5645_id);
+
+static const struct of_device_id ov5645_of_match[] = {
+	{ .compatible = "ovti,ov5645" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, ov5645_of_match);
+
+static const struct dev_pm_ops ov5645_pm_ops = {
+	SET_RUNTIME_PM_OPS(ov5645_set_power_off, ov5645_set_power_on, NULL)
+};
+
+static struct i2c_driver ov5645_i2c_driver = {
+	.driver = {
+		.of_match_table = ov5645_of_match,
+		.name  = "ov5645",
+		.pm = &ov5645_pm_ops,
+	},
+	.probe = ov5645_probe,
+	.remove = ov5645_remove,
+	.id_table = ov5645_id,
+};
+
+module_i2c_driver(ov5645_i2c_driver);
+
+MODULE_DESCRIPTION("Omnivision OV5645 Camera Driver");
+MODULE_AUTHOR("Todor Tomov <todor.tomov@linaro.org>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/ov5647.c b/drivers/media/i2c/ov5647.c
new file mode 100644
index 000000000..8de398423
--- /dev/null
+++ b/drivers/media/i2c/ov5647.c
@@ -0,0 +1,1527 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * A V4L2 driver for OmniVision OV5647 cameras.
+ *
+ * Based on Samsung S5K6AAFX SXGA 1/6" 1.3M CMOS Image Sensor driver
+ * Copyright (C) 2011 Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * Based on Omnivision OV7670 Camera Driver
+ * Copyright (C) 2006-7 Jonathan Corbet <corbet@lwn.net>
+ *
+ * Copyright (C) 2016, Synopsys, Inc.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of_graph.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-image-sizes.h>
+#include <media/v4l2-mediabus.h>
+
+/*
+ * From the datasheet, "20ms after PWDN goes low or 20ms after RESETB goes
+ * high if reset is inserted after PWDN goes high, host can access sensor's
+ * SCCB to initialize sensor."
+ */
+#define PWDN_ACTIVE_DELAY_MS	20
+
+#define MIPI_CTRL00_CLOCK_LANE_GATE		BIT(5)
+#define MIPI_CTRL00_LINE_SYNC_ENABLE		BIT(4)
+#define MIPI_CTRL00_BUS_IDLE			BIT(2)
+#define MIPI_CTRL00_CLOCK_LANE_DISABLE		BIT(0)
+
+#define OV5647_SW_STANDBY		0x0100
+#define OV5647_SW_RESET			0x0103
+#define OV5647_REG_CHIPID_H		0x300a
+#define OV5647_REG_CHIPID_L		0x300b
+#define OV5640_REG_PAD_OUT		0x300d
+#define OV5647_REG_EXP_HI		0x3500
+#define OV5647_REG_EXP_MID		0x3501
+#define OV5647_REG_EXP_LO		0x3502
+#define OV5647_REG_AEC_AGC		0x3503
+#define OV5647_REG_GAIN_HI		0x350a
+#define OV5647_REG_GAIN_LO		0x350b
+#define OV5647_REG_VTS_HI		0x380e
+#define OV5647_REG_VTS_LO		0x380f
+#define OV5647_REG_FRAME_OFF_NUMBER	0x4202
+#define OV5647_REG_MIPI_CTRL00		0x4800
+#define OV5647_REG_MIPI_CTRL14		0x4814
+#define OV5647_REG_AWB			0x5001
+#define OV5647_REG_ISPCTRL3D		0x503d
+
+#define REG_TERM 0xfffe
+#define VAL_TERM 0xfe
+#define REG_DLY  0xffff
+
+/* OV5647 native and active pixel array size */
+#define OV5647_NATIVE_WIDTH		2624U
+#define OV5647_NATIVE_HEIGHT		1956U
+
+#define OV5647_PIXEL_ARRAY_LEFT		16U
+#define OV5647_PIXEL_ARRAY_TOP		16U
+#define OV5647_PIXEL_ARRAY_WIDTH	2592U
+#define OV5647_PIXEL_ARRAY_HEIGHT	1944U
+
+#define OV5647_VBLANK_MIN		4
+#define OV5647_VTS_MAX			32767
+
+#define OV5647_EXPOSURE_MIN		4
+#define OV5647_EXPOSURE_STEP		1
+#define OV5647_EXPOSURE_DEFAULT		1000
+#define OV5647_EXPOSURE_MAX		65535
+
+struct regval_list {
+	u16 addr;
+	u8 data;
+};
+
+struct ov5647_mode {
+	struct v4l2_mbus_framefmt	format;
+	struct v4l2_rect		crop;
+	u64				pixel_rate;
+	int				hts;
+	int				vts;
+	const struct regval_list	*reg_list;
+	unsigned int			num_regs;
+};
+
+struct ov5647 {
+	struct v4l2_subdev		sd;
+	struct media_pad		pad;
+	struct mutex			lock;
+	struct clk			*xclk;
+	struct gpio_desc		*pwdn;
+	bool				clock_ncont;
+	struct v4l2_ctrl_handler	ctrls;
+	const struct ov5647_mode	*mode;
+	struct v4l2_ctrl		*pixel_rate;
+	struct v4l2_ctrl		*hblank;
+	struct v4l2_ctrl		*vblank;
+	struct v4l2_ctrl		*exposure;
+	bool				streaming;
+};
+
+static inline struct ov5647 *to_sensor(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct ov5647, sd);
+}
+
+static const char * const ov5647_test_pattern_menu[] = {
+	"Disabled",
+	"Color Bars",
+	"Color Squares",
+	"Random Data",
+};
+
+static const u8 ov5647_test_pattern_val[] = {
+	0x00,	/* Disabled */
+	0x80,	/* Color Bars */
+	0x82,	/* Color Squares */
+	0x81,	/* Random Data */
+};
+
+static const struct regval_list sensor_oe_disable_regs[] = {
+	{0x3000, 0x00},
+	{0x3001, 0x00},
+	{0x3002, 0x00},
+};
+
+static const struct regval_list sensor_oe_enable_regs[] = {
+	{0x3000, 0x0f},
+	{0x3001, 0xff},
+	{0x3002, 0xe4},
+};
+
+static struct regval_list ov5647_2592x1944_10bpp[] = {
+	{0x0100, 0x00},
+	{0x0103, 0x01},
+	{0x3034, 0x1a},
+	{0x3035, 0x21},
+	{0x3036, 0x69},
+	{0x303c, 0x11},
+	{0x3106, 0xf5},
+	{0x3821, 0x06},
+	{0x3820, 0x00},
+	{0x3827, 0xec},
+	{0x370c, 0x03},
+	{0x3612, 0x5b},
+	{0x3618, 0x04},
+	{0x5000, 0x06},
+	{0x5002, 0x41},
+	{0x5003, 0x08},
+	{0x5a00, 0x08},
+	{0x3000, 0x00},
+	{0x3001, 0x00},
+	{0x3002, 0x00},
+	{0x3016, 0x08},
+	{0x3017, 0xe0},
+	{0x3018, 0x44},
+	{0x301c, 0xf8},
+	{0x301d, 0xf0},
+	{0x3a18, 0x00},
+	{0x3a19, 0xf8},
+	{0x3c01, 0x80},
+	{0x3b07, 0x0c},
+	{0x380c, 0x0b},
+	{0x380d, 0x1c},
+	{0x3814, 0x11},
+	{0x3815, 0x11},
+	{0x3708, 0x64},
+	{0x3709, 0x12},
+	{0x3808, 0x0a},
+	{0x3809, 0x20},
+	{0x380a, 0x07},
+	{0x380b, 0x98},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x00},
+	{0x3804, 0x0a},
+	{0x3805, 0x3f},
+	{0x3806, 0x07},
+	{0x3807, 0xa3},
+	{0x3811, 0x10},
+	{0x3813, 0x06},
+	{0x3630, 0x2e},
+	{0x3632, 0xe2},
+	{0x3633, 0x23},
+	{0x3634, 0x44},
+	{0x3636, 0x06},
+	{0x3620, 0x64},
+	{0x3621, 0xe0},
+	{0x3600, 0x37},
+	{0x3704, 0xa0},
+	{0x3703, 0x5a},
+	{0x3715, 0x78},
+	{0x3717, 0x01},
+	{0x3731, 0x02},
+	{0x370b, 0x60},
+	{0x3705, 0x1a},
+	{0x3f05, 0x02},
+	{0x3f06, 0x10},
+	{0x3f01, 0x0a},
+	{0x3a08, 0x01},
+	{0x3a09, 0x28},
+	{0x3a0a, 0x00},
+	{0x3a0b, 0xf6},
+	{0x3a0d, 0x08},
+	{0x3a0e, 0x06},
+	{0x3a0f, 0x58},
+	{0x3a10, 0x50},
+	{0x3a1b, 0x58},
+	{0x3a1e, 0x50},
+	{0x3a11, 0x60},
+	{0x3a1f, 0x28},
+	{0x4001, 0x02},
+	{0x4004, 0x04},
+	{0x4000, 0x09},
+	{0x4837, 0x19},
+	{0x4800, 0x24},
+	{0x3503, 0x03},
+	{0x0100, 0x01},
+};
+
+static struct regval_list ov5647_1080p30_10bpp[] = {
+	{0x0100, 0x00},
+	{0x0103, 0x01},
+	{0x3034, 0x1a},
+	{0x3035, 0x21},
+	{0x3036, 0x62},
+	{0x303c, 0x11},
+	{0x3106, 0xf5},
+	{0x3821, 0x06},
+	{0x3820, 0x00},
+	{0x3827, 0xec},
+	{0x370c, 0x03},
+	{0x3612, 0x5b},
+	{0x3618, 0x04},
+	{0x5000, 0x06},
+	{0x5002, 0x41},
+	{0x5003, 0x08},
+	{0x5a00, 0x08},
+	{0x3000, 0x00},
+	{0x3001, 0x00},
+	{0x3002, 0x00},
+	{0x3016, 0x08},
+	{0x3017, 0xe0},
+	{0x3018, 0x44},
+	{0x301c, 0xf8},
+	{0x301d, 0xf0},
+	{0x3a18, 0x00},
+	{0x3a19, 0xf8},
+	{0x3c01, 0x80},
+	{0x3b07, 0x0c},
+	{0x380c, 0x09},
+	{0x380d, 0x70},
+	{0x3814, 0x11},
+	{0x3815, 0x11},
+	{0x3708, 0x64},
+	{0x3709, 0x12},
+	{0x3808, 0x07},
+	{0x3809, 0x80},
+	{0x380a, 0x04},
+	{0x380b, 0x38},
+	{0x3800, 0x01},
+	{0x3801, 0x5c},
+	{0x3802, 0x01},
+	{0x3803, 0xb2},
+	{0x3804, 0x08},
+	{0x3805, 0xe3},
+	{0x3806, 0x05},
+	{0x3807, 0xf1},
+	{0x3811, 0x04},
+	{0x3813, 0x02},
+	{0x3630, 0x2e},
+	{0x3632, 0xe2},
+	{0x3633, 0x23},
+	{0x3634, 0x44},
+	{0x3636, 0x06},
+	{0x3620, 0x64},
+	{0x3621, 0xe0},
+	{0x3600, 0x37},
+	{0x3704, 0xa0},
+	{0x3703, 0x5a},
+	{0x3715, 0x78},
+	{0x3717, 0x01},
+	{0x3731, 0x02},
+	{0x370b, 0x60},
+	{0x3705, 0x1a},
+	{0x3f05, 0x02},
+	{0x3f06, 0x10},
+	{0x3f01, 0x0a},
+	{0x3a08, 0x01},
+	{0x3a09, 0x4b},
+	{0x3a0a, 0x01},
+	{0x3a0b, 0x13},
+	{0x3a0d, 0x04},
+	{0x3a0e, 0x03},
+	{0x3a0f, 0x58},
+	{0x3a10, 0x50},
+	{0x3a1b, 0x58},
+	{0x3a1e, 0x50},
+	{0x3a11, 0x60},
+	{0x3a1f, 0x28},
+	{0x4001, 0x02},
+	{0x4004, 0x04},
+	{0x4000, 0x09},
+	{0x4837, 0x19},
+	{0x4800, 0x34},
+	{0x3503, 0x03},
+	{0x0100, 0x01},
+};
+
+static struct regval_list ov5647_2x2binned_10bpp[] = {
+	{0x0100, 0x00},
+	{0x0103, 0x01},
+	{0x3034, 0x1a},
+	{0x3035, 0x21},
+	{0x3036, 0x62},
+	{0x303c, 0x11},
+	{0x3106, 0xf5},
+	{0x3827, 0xec},
+	{0x370c, 0x03},
+	{0x3612, 0x59},
+	{0x3618, 0x00},
+	{0x5000, 0x06},
+	{0x5002, 0x41},
+	{0x5003, 0x08},
+	{0x5a00, 0x08},
+	{0x3000, 0x00},
+	{0x3001, 0x00},
+	{0x3002, 0x00},
+	{0x3016, 0x08},
+	{0x3017, 0xe0},
+	{0x3018, 0x44},
+	{0x301c, 0xf8},
+	{0x301d, 0xf0},
+	{0x3a18, 0x00},
+	{0x3a19, 0xf8},
+	{0x3c01, 0x80},
+	{0x3b07, 0x0c},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x00},
+	{0x3804, 0x0a},
+	{0x3805, 0x3f},
+	{0x3806, 0x07},
+	{0x3807, 0xa3},
+	{0x3808, 0x05},
+	{0x3809, 0x10},
+	{0x380a, 0x03},
+	{0x380b, 0xcc},
+	{0x380c, 0x07},
+	{0x380d, 0x68},
+	{0x3811, 0x0c},
+	{0x3813, 0x06},
+	{0x3814, 0x31},
+	{0x3815, 0x31},
+	{0x3630, 0x2e},
+	{0x3632, 0xe2},
+	{0x3633, 0x23},
+	{0x3634, 0x44},
+	{0x3636, 0x06},
+	{0x3620, 0x64},
+	{0x3621, 0xe0},
+	{0x3600, 0x37},
+	{0x3704, 0xa0},
+	{0x3703, 0x5a},
+	{0x3715, 0x78},
+	{0x3717, 0x01},
+	{0x3731, 0x02},
+	{0x370b, 0x60},
+	{0x3705, 0x1a},
+	{0x3f05, 0x02},
+	{0x3f06, 0x10},
+	{0x3f01, 0x0a},
+	{0x3a08, 0x01},
+	{0x3a09, 0x28},
+	{0x3a0a, 0x00},
+	{0x3a0b, 0xf6},
+	{0x3a0d, 0x08},
+	{0x3a0e, 0x06},
+	{0x3a0f, 0x58},
+	{0x3a10, 0x50},
+	{0x3a1b, 0x58},
+	{0x3a1e, 0x50},
+	{0x3a11, 0x60},
+	{0x3a1f, 0x28},
+	{0x4001, 0x02},
+	{0x4004, 0x04},
+	{0x4000, 0x09},
+	{0x4837, 0x16},
+	{0x4800, 0x24},
+	{0x3503, 0x03},
+	{0x3820, 0x41},
+	{0x3821, 0x07},
+	{0x350a, 0x00},
+	{0x350b, 0x10},
+	{0x3500, 0x00},
+	{0x3501, 0x1a},
+	{0x3502, 0xf0},
+	{0x3212, 0xa0},
+	{0x0100, 0x01},
+};
+
+static struct regval_list ov5647_640x480_10bpp[] = {
+	{0x0100, 0x00},
+	{0x0103, 0x01},
+	{0x3035, 0x11},
+	{0x3036, 0x46},
+	{0x303c, 0x11},
+	{0x3821, 0x07},
+	{0x3820, 0x41},
+	{0x370c, 0x03},
+	{0x3612, 0x59},
+	{0x3618, 0x00},
+	{0x5000, 0x06},
+	{0x5003, 0x08},
+	{0x5a00, 0x08},
+	{0x3000, 0xff},
+	{0x3001, 0xff},
+	{0x3002, 0xff},
+	{0x301d, 0xf0},
+	{0x3a18, 0x00},
+	{0x3a19, 0xf8},
+	{0x3c01, 0x80},
+	{0x3b07, 0x0c},
+	{0x380c, 0x07},
+	{0x380d, 0x3c},
+	{0x3814, 0x35},
+	{0x3815, 0x35},
+	{0x3708, 0x64},
+	{0x3709, 0x52},
+	{0x3808, 0x02},
+	{0x3809, 0x80},
+	{0x380a, 0x01},
+	{0x380b, 0xe0},
+	{0x3800, 0x00},
+	{0x3801, 0x10},
+	{0x3802, 0x00},
+	{0x3803, 0x00},
+	{0x3804, 0x0a},
+	{0x3805, 0x2f},
+	{0x3806, 0x07},
+	{0x3807, 0x9f},
+	{0x3630, 0x2e},
+	{0x3632, 0xe2},
+	{0x3633, 0x23},
+	{0x3634, 0x44},
+	{0x3620, 0x64},
+	{0x3621, 0xe0},
+	{0x3600, 0x37},
+	{0x3704, 0xa0},
+	{0x3703, 0x5a},
+	{0x3715, 0x78},
+	{0x3717, 0x01},
+	{0x3731, 0x02},
+	{0x370b, 0x60},
+	{0x3705, 0x1a},
+	{0x3f05, 0x02},
+	{0x3f06, 0x10},
+	{0x3f01, 0x0a},
+	{0x3a08, 0x01},
+	{0x3a09, 0x2e},
+	{0x3a0a, 0x00},
+	{0x3a0b, 0xfb},
+	{0x3a0d, 0x02},
+	{0x3a0e, 0x01},
+	{0x3a0f, 0x58},
+	{0x3a10, 0x50},
+	{0x3a1b, 0x58},
+	{0x3a1e, 0x50},
+	{0x3a11, 0x60},
+	{0x3a1f, 0x28},
+	{0x4001, 0x02},
+	{0x4004, 0x02},
+	{0x4000, 0x09},
+	{0x3000, 0x00},
+	{0x3001, 0x00},
+	{0x3002, 0x00},
+	{0x3017, 0xe0},
+	{0x301c, 0xfc},
+	{0x3636, 0x06},
+	{0x3016, 0x08},
+	{0x3827, 0xec},
+	{0x3018, 0x44},
+	{0x3035, 0x21},
+	{0x3106, 0xf5},
+	{0x3034, 0x1a},
+	{0x301c, 0xf8},
+	{0x4800, 0x34},
+	{0x3503, 0x03},
+	{0x0100, 0x01},
+};
+
+static const struct ov5647_mode ov5647_modes[] = {
+	/* 2592x1944 full resolution full FOV 10-bit mode. */
+	{
+		.format = {
+			.code		= MEDIA_BUS_FMT_SBGGR10_1X10,
+			.colorspace	= V4L2_COLORSPACE_SRGB,
+			.field		= V4L2_FIELD_NONE,
+			.width		= 2592,
+			.height		= 1944
+		},
+		.crop = {
+			.left		= OV5647_PIXEL_ARRAY_LEFT,
+			.top		= OV5647_PIXEL_ARRAY_TOP,
+			.width		= 2592,
+			.height		= 1944
+		},
+		.pixel_rate	= 87500000,
+		.hts		= 2844,
+		.vts		= 0x7b0,
+		.reg_list	= ov5647_2592x1944_10bpp,
+		.num_regs	= ARRAY_SIZE(ov5647_2592x1944_10bpp)
+	},
+	/* 1080p30 10-bit mode. Full resolution centre-cropped down to 1080p. */
+	{
+		.format = {
+			.code		= MEDIA_BUS_FMT_SBGGR10_1X10,
+			.colorspace	= V4L2_COLORSPACE_SRGB,
+			.field		= V4L2_FIELD_NONE,
+			.width		= 1920,
+			.height		= 1080
+		},
+		.crop = {
+			.left		= 348 + OV5647_PIXEL_ARRAY_LEFT,
+			.top		= 434 + OV5647_PIXEL_ARRAY_TOP,
+			.width		= 1928,
+			.height		= 1080,
+		},
+		.pixel_rate	= 81666700,
+		.hts		= 2416,
+		.vts		= 0x450,
+		.reg_list	= ov5647_1080p30_10bpp,
+		.num_regs	= ARRAY_SIZE(ov5647_1080p30_10bpp)
+	},
+	/* 2x2 binned full FOV 10-bit mode. */
+	{
+		.format = {
+			.code		= MEDIA_BUS_FMT_SBGGR10_1X10,
+			.colorspace	= V4L2_COLORSPACE_SRGB,
+			.field		= V4L2_FIELD_NONE,
+			.width		= 1296,
+			.height		= 972
+		},
+		.crop = {
+			.left		= OV5647_PIXEL_ARRAY_LEFT,
+			.top		= OV5647_PIXEL_ARRAY_TOP,
+			.width		= 2592,
+			.height		= 1944,
+		},
+		.pixel_rate	= 81666700,
+		.hts		= 1896,
+		.vts		= 0x59b,
+		.reg_list	= ov5647_2x2binned_10bpp,
+		.num_regs	= ARRAY_SIZE(ov5647_2x2binned_10bpp)
+	},
+	/* 10-bit VGA full FOV 60fps. 2x2 binned and subsampled down to VGA. */
+	{
+		.format = {
+			.code		= MEDIA_BUS_FMT_SBGGR10_1X10,
+			.colorspace	= V4L2_COLORSPACE_SRGB,
+			.field		= V4L2_FIELD_NONE,
+			.width		= 640,
+			.height		= 480
+		},
+		.crop = {
+			.left		= 16 + OV5647_PIXEL_ARRAY_LEFT,
+			.top		= OV5647_PIXEL_ARRAY_TOP,
+			.width		= 2560,
+			.height		= 1920,
+		},
+		.pixel_rate	= 55000000,
+		.hts		= 1852,
+		.vts		= 0x1f8,
+		.reg_list	= ov5647_640x480_10bpp,
+		.num_regs	= ARRAY_SIZE(ov5647_640x480_10bpp)
+	},
+};
+
+/* Default sensor mode is 2x2 binned 640x480 SBGGR10_1X10. */
+#define OV5647_DEFAULT_MODE	(&ov5647_modes[3])
+#define OV5647_DEFAULT_FORMAT	(ov5647_modes[3].format)
+
+static int ov5647_write16(struct v4l2_subdev *sd, u16 reg, u16 val)
+{
+	unsigned char data[4] = { reg >> 8, reg & 0xff, val >> 8, val & 0xff};
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+	ret = i2c_master_send(client, data, 4);
+	if (ret < 0) {
+		dev_dbg(&client->dev, "%s: i2c write error, reg: %x\n",
+			__func__, reg);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ov5647_write(struct v4l2_subdev *sd, u16 reg, u8 val)
+{
+	unsigned char data[3] = { reg >> 8, reg & 0xff, val};
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+	ret = i2c_master_send(client, data, 3);
+	if (ret < 0) {
+		dev_dbg(&client->dev, "%s: i2c write error, reg: %x\n",
+				__func__, reg);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ov5647_read(struct v4l2_subdev *sd, u16 reg, u8 *val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u8 buf[2] = { reg >> 8, reg & 0xff };
+	struct i2c_msg msg[2];
+	int ret;
+
+	msg[0].addr = client->addr;
+	msg[0].flags = client->flags;
+	msg[0].buf = buf;
+	msg[0].len = sizeof(buf);
+
+	msg[1].addr = client->addr;
+	msg[1].flags = client->flags | I2C_M_RD;
+	msg[1].buf = buf;
+	msg[1].len = 1;
+
+	ret = i2c_transfer(client->adapter, msg, 2);
+	if (ret != 2) {
+		dev_err(&client->dev, "%s: i2c read error, reg: %x = %d\n",
+			__func__, reg, ret);
+		return ret >= 0 ? -EINVAL : ret;
+	}
+
+	*val = buf[0];
+
+	return 0;
+}
+
+static int ov5647_write_array(struct v4l2_subdev *sd,
+			      const struct regval_list *regs, int array_size)
+{
+	int i, ret;
+
+	for (i = 0; i < array_size; i++) {
+		ret = ov5647_write(sd, regs[i].addr, regs[i].data);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int ov5647_set_virtual_channel(struct v4l2_subdev *sd, int channel)
+{
+	u8 channel_id;
+	int ret;
+
+	ret = ov5647_read(sd, OV5647_REG_MIPI_CTRL14, &channel_id);
+	if (ret < 0)
+		return ret;
+
+	channel_id &= ~(3 << 6);
+
+	return ov5647_write(sd, OV5647_REG_MIPI_CTRL14,
+			    channel_id | (channel << 6));
+}
+
+static int ov5647_set_mode(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov5647 *sensor = to_sensor(sd);
+	u8 resetval, rdval;
+	int ret;
+
+	ret = ov5647_read(sd, OV5647_SW_STANDBY, &rdval);
+	if (ret < 0)
+		return ret;
+
+	ret = ov5647_write_array(sd, sensor->mode->reg_list,
+				 sensor->mode->num_regs);
+	if (ret < 0) {
+		dev_err(&client->dev, "write sensor default regs error\n");
+		return ret;
+	}
+
+	ret = ov5647_set_virtual_channel(sd, 0);
+	if (ret < 0)
+		return ret;
+
+	ret = ov5647_read(sd, OV5647_SW_STANDBY, &resetval);
+	if (ret < 0)
+		return ret;
+
+	if (!(resetval & 0x01)) {
+		dev_err(&client->dev, "Device was in SW standby");
+		ret = ov5647_write(sd, OV5647_SW_STANDBY, 0x01);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int ov5647_stream_on(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov5647 *sensor = to_sensor(sd);
+	u8 val = MIPI_CTRL00_BUS_IDLE;
+	int ret;
+
+	ret = ov5647_set_mode(sd);
+	if (ret) {
+		dev_err(&client->dev, "Failed to program sensor mode: %d\n", ret);
+		return ret;
+	}
+
+	/* Apply customized values from user when stream starts. */
+	ret =  __v4l2_ctrl_handler_setup(sd->ctrl_handler);
+	if (ret)
+		return ret;
+
+	if (sensor->clock_ncont)
+		val |= MIPI_CTRL00_CLOCK_LANE_GATE |
+		       MIPI_CTRL00_LINE_SYNC_ENABLE;
+
+	ret = ov5647_write(sd, OV5647_REG_MIPI_CTRL00, val);
+	if (ret < 0)
+		return ret;
+
+	ret = ov5647_write(sd, OV5647_REG_FRAME_OFF_NUMBER, 0x00);
+	if (ret < 0)
+		return ret;
+
+	return ov5647_write(sd, OV5640_REG_PAD_OUT, 0x00);
+}
+
+static int ov5647_stream_off(struct v4l2_subdev *sd)
+{
+	int ret;
+
+	ret = ov5647_write(sd, OV5647_REG_MIPI_CTRL00,
+			   MIPI_CTRL00_CLOCK_LANE_GATE | MIPI_CTRL00_BUS_IDLE |
+			   MIPI_CTRL00_CLOCK_LANE_DISABLE);
+	if (ret < 0)
+		return ret;
+
+	ret = ov5647_write(sd, OV5647_REG_FRAME_OFF_NUMBER, 0x0f);
+	if (ret < 0)
+		return ret;
+
+	return ov5647_write(sd, OV5640_REG_PAD_OUT, 0x01);
+}
+
+static int ov5647_power_on(struct device *dev)
+{
+	struct ov5647 *sensor = dev_get_drvdata(dev);
+	int ret;
+
+	dev_dbg(dev, "OV5647 power on\n");
+
+	if (sensor->pwdn) {
+		gpiod_set_value_cansleep(sensor->pwdn, 0);
+		msleep(PWDN_ACTIVE_DELAY_MS);
+	}
+
+	ret = clk_prepare_enable(sensor->xclk);
+	if (ret < 0) {
+		dev_err(dev, "clk prepare enable failed\n");
+		goto error_pwdn;
+	}
+
+	ret = ov5647_write_array(&sensor->sd, sensor_oe_enable_regs,
+				 ARRAY_SIZE(sensor_oe_enable_regs));
+	if (ret < 0) {
+		dev_err(dev, "write sensor_oe_enable_regs error\n");
+		goto error_clk_disable;
+	}
+
+	/* Stream off to coax lanes into LP-11 state. */
+	ret = ov5647_stream_off(&sensor->sd);
+	if (ret < 0) {
+		dev_err(dev, "camera not available, check power\n");
+		goto error_clk_disable;
+	}
+
+	return 0;
+
+error_clk_disable:
+	clk_disable_unprepare(sensor->xclk);
+error_pwdn:
+	gpiod_set_value_cansleep(sensor->pwdn, 1);
+
+	return ret;
+}
+
+static int ov5647_power_off(struct device *dev)
+{
+	struct ov5647 *sensor = dev_get_drvdata(dev);
+	u8 rdval;
+	int ret;
+
+	dev_dbg(dev, "OV5647 power off\n");
+
+	ret = ov5647_write_array(&sensor->sd, sensor_oe_disable_regs,
+				 ARRAY_SIZE(sensor_oe_disable_regs));
+	if (ret < 0)
+		dev_dbg(dev, "disable oe failed\n");
+
+	/* Enter software standby */
+	ret = ov5647_read(&sensor->sd, OV5647_SW_STANDBY, &rdval);
+	if (ret < 0)
+		dev_dbg(dev, "software standby failed\n");
+
+	rdval &= ~0x01;
+	ret = ov5647_write(&sensor->sd, OV5647_SW_STANDBY, rdval);
+	if (ret < 0)
+		dev_dbg(dev, "software standby failed\n");
+
+	clk_disable_unprepare(sensor->xclk);
+	gpiod_set_value_cansleep(sensor->pwdn, 1);
+
+	return 0;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int ov5647_sensor_get_register(struct v4l2_subdev *sd,
+				      struct v4l2_dbg_register *reg)
+{
+	int ret;
+	u8 val;
+
+	ret = ov5647_read(sd, reg->reg & 0xff, &val);
+	if (ret < 0)
+		return ret;
+
+	reg->val = val;
+	reg->size = 1;
+
+	return 0;
+}
+
+static int ov5647_sensor_set_register(struct v4l2_subdev *sd,
+				      const struct v4l2_dbg_register *reg)
+{
+	return ov5647_write(sd, reg->reg & 0xff, reg->val & 0xff);
+}
+#endif
+
+/* Subdev core operations registration */
+static const struct v4l2_subdev_core_ops ov5647_subdev_core_ops = {
+	.subscribe_event	= v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event	= v4l2_event_subdev_unsubscribe,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register		= ov5647_sensor_get_register,
+	.s_register		= ov5647_sensor_set_register,
+#endif
+};
+
+static const struct v4l2_rect *
+__ov5647_get_pad_crop(struct ov5647 *ov5647,
+		      struct v4l2_subdev_state *sd_state,
+		      unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_crop(&ov5647->sd, sd_state, pad);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return &ov5647->mode->crop;
+	}
+
+	return NULL;
+}
+
+static int ov5647_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov5647 *sensor = to_sensor(sd);
+	int ret;
+
+	mutex_lock(&sensor->lock);
+	if (sensor->streaming == enable) {
+		mutex_unlock(&sensor->lock);
+		return 0;
+	}
+
+	if (enable) {
+		ret = pm_runtime_resume_and_get(&client->dev);
+		if (ret < 0)
+			goto error_unlock;
+
+		ret = ov5647_stream_on(sd);
+		if (ret < 0) {
+			dev_err(&client->dev, "stream start failed: %d\n", ret);
+			goto error_pm;
+		}
+	} else {
+		ret = ov5647_stream_off(sd);
+		if (ret < 0) {
+			dev_err(&client->dev, "stream stop failed: %d\n", ret);
+			goto error_pm;
+		}
+		pm_runtime_put(&client->dev);
+	}
+
+	sensor->streaming = enable;
+	mutex_unlock(&sensor->lock);
+
+	return 0;
+
+error_pm:
+	pm_runtime_put(&client->dev);
+error_unlock:
+	mutex_unlock(&sensor->lock);
+
+	return ret;
+}
+
+static const struct v4l2_subdev_video_ops ov5647_subdev_video_ops = {
+	.s_stream =		ov5647_s_stream,
+};
+
+static int ov5647_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index > 0)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_SBGGR10_1X10;
+
+	return 0;
+}
+
+static int ov5647_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	const struct v4l2_mbus_framefmt *fmt;
+
+	if (fse->code != MEDIA_BUS_FMT_SBGGR10_1X10 ||
+	    fse->index >= ARRAY_SIZE(ov5647_modes))
+		return -EINVAL;
+
+	fmt = &ov5647_modes[fse->index].format;
+	fse->min_width = fmt->width;
+	fse->max_width = fmt->width;
+	fse->min_height = fmt->height;
+	fse->max_height = fmt->height;
+
+	return 0;
+}
+
+static int ov5647_get_pad_fmt(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+	const struct v4l2_mbus_framefmt *sensor_format;
+	struct ov5647 *sensor = to_sensor(sd);
+
+	mutex_lock(&sensor->lock);
+	switch (format->which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		sensor_format = v4l2_subdev_get_try_format(sd, sd_state,
+							   format->pad);
+		break;
+	default:
+		sensor_format = &sensor->mode->format;
+		break;
+	}
+
+	*fmt = *sensor_format;
+	mutex_unlock(&sensor->lock);
+
+	return 0;
+}
+
+static int ov5647_set_pad_fmt(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+	struct ov5647 *sensor = to_sensor(sd);
+	const struct ov5647_mode *mode;
+
+	mode = v4l2_find_nearest_size(ov5647_modes, ARRAY_SIZE(ov5647_modes),
+				      format.width, format.height,
+				      fmt->width, fmt->height);
+
+	/* Update the sensor mode and apply at it at streamon time. */
+	mutex_lock(&sensor->lock);
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		*v4l2_subdev_get_try_format(sd, sd_state, format->pad) = mode->format;
+	} else {
+		int exposure_max, exposure_def;
+		int hblank, vblank;
+
+		sensor->mode = mode;
+		__v4l2_ctrl_modify_range(sensor->pixel_rate, mode->pixel_rate,
+					 mode->pixel_rate, 1, mode->pixel_rate);
+
+		hblank = mode->hts - mode->format.width;
+		__v4l2_ctrl_modify_range(sensor->hblank, hblank, hblank, 1,
+					 hblank);
+
+		vblank = mode->vts - mode->format.height;
+		__v4l2_ctrl_modify_range(sensor->vblank, OV5647_VBLANK_MIN,
+					 OV5647_VTS_MAX - mode->format.height,
+					 1, vblank);
+		__v4l2_ctrl_s_ctrl(sensor->vblank, vblank);
+
+		exposure_max = mode->vts - 4;
+		exposure_def = min(exposure_max, OV5647_EXPOSURE_DEFAULT);
+		__v4l2_ctrl_modify_range(sensor->exposure,
+					 sensor->exposure->minimum,
+					 exposure_max, sensor->exposure->step,
+					 exposure_def);
+	}
+	*fmt = mode->format;
+	mutex_unlock(&sensor->lock);
+
+	return 0;
+}
+
+static int ov5647_get_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *sd_state,
+				struct v4l2_subdev_selection *sel)
+{
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP: {
+		struct ov5647 *sensor = to_sensor(sd);
+
+		mutex_lock(&sensor->lock);
+		sel->r = *__ov5647_get_pad_crop(sensor, sd_state, sel->pad,
+						sel->which);
+		mutex_unlock(&sensor->lock);
+
+		return 0;
+	}
+
+	case V4L2_SEL_TGT_NATIVE_SIZE:
+		sel->r.top = 0;
+		sel->r.left = 0;
+		sel->r.width = OV5647_NATIVE_WIDTH;
+		sel->r.height = OV5647_NATIVE_HEIGHT;
+
+		return 0;
+
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.top = OV5647_PIXEL_ARRAY_TOP;
+		sel->r.left = OV5647_PIXEL_ARRAY_LEFT;
+		sel->r.width = OV5647_PIXEL_ARRAY_WIDTH;
+		sel->r.height = OV5647_PIXEL_ARRAY_HEIGHT;
+
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static const struct v4l2_subdev_pad_ops ov5647_subdev_pad_ops = {
+	.enum_mbus_code		= ov5647_enum_mbus_code,
+	.enum_frame_size	= ov5647_enum_frame_size,
+	.set_fmt		= ov5647_set_pad_fmt,
+	.get_fmt		= ov5647_get_pad_fmt,
+	.get_selection		= ov5647_get_selection,
+};
+
+static const struct v4l2_subdev_ops ov5647_subdev_ops = {
+	.core		= &ov5647_subdev_core_ops,
+	.video		= &ov5647_subdev_video_ops,
+	.pad		= &ov5647_subdev_pad_ops,
+};
+
+static int ov5647_detect(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u8 read;
+	int ret;
+
+	ret = ov5647_write(sd, OV5647_SW_RESET, 0x01);
+	if (ret < 0)
+		return ret;
+
+	ret = ov5647_read(sd, OV5647_REG_CHIPID_H, &read);
+	if (ret < 0)
+		return ret;
+
+	if (read != 0x56) {
+		dev_err(&client->dev, "ID High expected 0x56 got %x", read);
+		return -ENODEV;
+	}
+
+	ret = ov5647_read(sd, OV5647_REG_CHIPID_L, &read);
+	if (ret < 0)
+		return ret;
+
+	if (read != 0x47) {
+		dev_err(&client->dev, "ID Low expected 0x47 got %x", read);
+		return -ENODEV;
+	}
+
+	return ov5647_write(sd, OV5647_SW_RESET, 0x00);
+}
+
+static int ov5647_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_mbus_framefmt *format =
+				v4l2_subdev_get_try_format(sd, fh->state, 0);
+	struct v4l2_rect *crop = v4l2_subdev_get_try_crop(sd, fh->state, 0);
+
+	crop->left = OV5647_PIXEL_ARRAY_LEFT;
+	crop->top = OV5647_PIXEL_ARRAY_TOP;
+	crop->width = OV5647_PIXEL_ARRAY_WIDTH;
+	crop->height = OV5647_PIXEL_ARRAY_HEIGHT;
+
+	*format = OV5647_DEFAULT_FORMAT;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_internal_ops ov5647_subdev_internal_ops = {
+	.open = ov5647_open,
+};
+
+static int ov5647_s_auto_white_balance(struct v4l2_subdev *sd, u32 val)
+{
+	return ov5647_write(sd, OV5647_REG_AWB, val ? 1 : 0);
+}
+
+static int ov5647_s_autogain(struct v4l2_subdev *sd, u32 val)
+{
+	int ret;
+	u8 reg;
+
+	/* Non-zero turns on AGC by clearing bit 1.*/
+	ret = ov5647_read(sd, OV5647_REG_AEC_AGC, &reg);
+	if (ret)
+		return ret;
+
+	return ov5647_write(sd, OV5647_REG_AEC_AGC, val ? reg & ~BIT(1)
+							: reg | BIT(1));
+}
+
+static int ov5647_s_exposure_auto(struct v4l2_subdev *sd, u32 val)
+{
+	int ret;
+	u8 reg;
+
+	/*
+	 * Everything except V4L2_EXPOSURE_MANUAL turns on AEC by
+	 * clearing bit 0.
+	 */
+	ret = ov5647_read(sd, OV5647_REG_AEC_AGC, &reg);
+	if (ret)
+		return ret;
+
+	return ov5647_write(sd, OV5647_REG_AEC_AGC,
+			    val == V4L2_EXPOSURE_MANUAL ? reg | BIT(0)
+							: reg & ~BIT(0));
+}
+
+static int ov5647_s_analogue_gain(struct v4l2_subdev *sd, u32 val)
+{
+	int ret;
+
+	/* 10 bits of gain, 2 in the high register. */
+	ret = ov5647_write(sd, OV5647_REG_GAIN_HI, (val >> 8) & 3);
+	if (ret)
+		return ret;
+
+	return ov5647_write(sd, OV5647_REG_GAIN_LO, val & 0xff);
+}
+
+static int ov5647_s_exposure(struct v4l2_subdev *sd, u32 val)
+{
+	int ret;
+
+	/*
+	 * Sensor has 20 bits, but the bottom 4 bits are fractions of a line
+	 * which we leave as zero (and don't receive in "val").
+	 */
+	ret = ov5647_write(sd, OV5647_REG_EXP_HI, (val >> 12) & 0xf);
+	if (ret)
+		return ret;
+
+	ret = ov5647_write(sd, OV5647_REG_EXP_MID, (val >> 4) & 0xff);
+	if (ret)
+		return ret;
+
+	return ov5647_write(sd, OV5647_REG_EXP_LO, (val & 0xf) << 4);
+}
+
+static int ov5647_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov5647 *sensor = container_of(ctrl->handler,
+					    struct ov5647, ctrls);
+	struct v4l2_subdev *sd = &sensor->sd;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+
+	/* v4l2_ctrl_lock() locks our own mutex */
+
+	if (ctrl->id == V4L2_CID_VBLANK) {
+		int exposure_max, exposure_def;
+
+		/* Update max exposure while meeting expected vblanking */
+		exposure_max = sensor->mode->format.height + ctrl->val - 4;
+		exposure_def = min(exposure_max, OV5647_EXPOSURE_DEFAULT);
+		__v4l2_ctrl_modify_range(sensor->exposure,
+					 sensor->exposure->minimum,
+					 exposure_max, sensor->exposure->step,
+					 exposure_def);
+	}
+
+	/*
+	 * If the device is not powered up do not apply any controls
+	 * to H/W at this time. Instead the controls will be restored
+	 * at s_stream(1) time.
+	 */
+	if (pm_runtime_get_if_in_use(&client->dev) == 0)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		ret = ov5647_s_auto_white_balance(sd, ctrl->val);
+		break;
+	case V4L2_CID_AUTOGAIN:
+		ret = ov5647_s_autogain(sd, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE_AUTO:
+		ret = ov5647_s_exposure_auto(sd, ctrl->val);
+		break;
+	case V4L2_CID_ANALOGUE_GAIN:
+		ret =  ov5647_s_analogue_gain(sd, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE:
+		ret = ov5647_s_exposure(sd, ctrl->val);
+		break;
+	case V4L2_CID_VBLANK:
+		ret = ov5647_write16(sd, OV5647_REG_VTS_HI,
+				     sensor->mode->format.height + ctrl->val);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		ret = ov5647_write(sd, OV5647_REG_ISPCTRL3D,
+				   ov5647_test_pattern_val[ctrl->val]);
+		break;
+
+	/* Read-only, but we adjust it based on mode. */
+	case V4L2_CID_PIXEL_RATE:
+	case V4L2_CID_HBLANK:
+		/* Read-only, but we adjust it based on mode. */
+		break;
+
+	default:
+		dev_info(&client->dev,
+			 "Control (id:0x%x, val:0x%x) not supported\n",
+			 ctrl->id, ctrl->val);
+		return -EINVAL;
+	}
+
+	pm_runtime_put(&client->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ov5647_ctrl_ops = {
+	.s_ctrl = ov5647_s_ctrl,
+};
+
+static int ov5647_init_controls(struct ov5647 *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->sd);
+	int hblank, exposure_max, exposure_def;
+
+	v4l2_ctrl_handler_init(&sensor->ctrls, 9);
+
+	v4l2_ctrl_new_std(&sensor->ctrls, &ov5647_ctrl_ops,
+			  V4L2_CID_AUTOGAIN, 0, 1, 1, 0);
+
+	v4l2_ctrl_new_std(&sensor->ctrls, &ov5647_ctrl_ops,
+			  V4L2_CID_AUTO_WHITE_BALANCE, 0, 1, 1, 0);
+
+	v4l2_ctrl_new_std_menu(&sensor->ctrls, &ov5647_ctrl_ops,
+			       V4L2_CID_EXPOSURE_AUTO, V4L2_EXPOSURE_MANUAL,
+			       0, V4L2_EXPOSURE_MANUAL);
+
+	exposure_max = sensor->mode->vts - 4;
+	exposure_def = min(exposure_max, OV5647_EXPOSURE_DEFAULT);
+	sensor->exposure = v4l2_ctrl_new_std(&sensor->ctrls, &ov5647_ctrl_ops,
+					     V4L2_CID_EXPOSURE,
+					     OV5647_EXPOSURE_MIN,
+					     exposure_max, OV5647_EXPOSURE_STEP,
+					     exposure_def);
+
+	/* min: 16 = 1.0x; max (10 bits); default: 32 = 2.0x. */
+	v4l2_ctrl_new_std(&sensor->ctrls, &ov5647_ctrl_ops,
+			  V4L2_CID_ANALOGUE_GAIN, 16, 1023, 1, 32);
+
+	/* By default, PIXEL_RATE is read only, but it does change per mode */
+	sensor->pixel_rate = v4l2_ctrl_new_std(&sensor->ctrls, &ov5647_ctrl_ops,
+					       V4L2_CID_PIXEL_RATE,
+					       sensor->mode->pixel_rate,
+					       sensor->mode->pixel_rate, 1,
+					       sensor->mode->pixel_rate);
+
+	/* By default, HBLANK is read only, but it does change per mode. */
+	hblank = sensor->mode->hts - sensor->mode->format.width;
+	sensor->hblank = v4l2_ctrl_new_std(&sensor->ctrls, &ov5647_ctrl_ops,
+					   V4L2_CID_HBLANK, hblank, hblank, 1,
+					   hblank);
+
+	sensor->vblank = v4l2_ctrl_new_std(&sensor->ctrls, &ov5647_ctrl_ops,
+					   V4L2_CID_VBLANK, OV5647_VBLANK_MIN,
+					   OV5647_VTS_MAX -
+					   sensor->mode->format.height, 1,
+					   sensor->mode->vts -
+					   sensor->mode->format.height);
+
+	v4l2_ctrl_new_std_menu_items(&sensor->ctrls, &ov5647_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(ov5647_test_pattern_menu) - 1,
+				     0, 0, ov5647_test_pattern_menu);
+
+	if (sensor->ctrls.error)
+		goto handler_free;
+
+	sensor->pixel_rate->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+	sensor->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+	sensor->sd.ctrl_handler = &sensor->ctrls;
+
+	return 0;
+
+handler_free:
+	dev_err(&client->dev, "%s Controls initialization failed (%d)\n",
+		__func__, sensor->ctrls.error);
+	v4l2_ctrl_handler_free(&sensor->ctrls);
+
+	return sensor->ctrls.error;
+}
+
+static int ov5647_parse_dt(struct ov5647 *sensor, struct device_node *np)
+{
+	struct v4l2_fwnode_endpoint bus_cfg = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY,
+	};
+	struct device_node *ep;
+	int ret;
+
+	ep = of_graph_get_next_endpoint(np, NULL);
+	if (!ep)
+		return -EINVAL;
+
+	ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(ep), &bus_cfg);
+	if (ret)
+		goto out;
+
+	sensor->clock_ncont = bus_cfg.bus.mipi_csi2.flags &
+			      V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK;
+
+out:
+	of_node_put(ep);
+
+	return ret;
+}
+
+static int ov5647_probe(struct i2c_client *client)
+{
+	struct device_node *np = client->dev.of_node;
+	struct device *dev = &client->dev;
+	struct ov5647 *sensor;
+	struct v4l2_subdev *sd;
+	u32 xclk_freq;
+	int ret;
+
+	sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
+	if (!sensor)
+		return -ENOMEM;
+
+	if (IS_ENABLED(CONFIG_OF) && np) {
+		ret = ov5647_parse_dt(sensor, np);
+		if (ret) {
+			dev_err(dev, "DT parsing error: %d\n", ret);
+			return ret;
+		}
+	}
+
+	sensor->xclk = devm_clk_get(dev, NULL);
+	if (IS_ERR(sensor->xclk)) {
+		dev_err(dev, "could not get xclk");
+		return PTR_ERR(sensor->xclk);
+	}
+
+	xclk_freq = clk_get_rate(sensor->xclk);
+	if (xclk_freq != 25000000) {
+		dev_err(dev, "Unsupported clock frequency: %u\n", xclk_freq);
+		return -EINVAL;
+	}
+
+	/* Request the power down GPIO asserted. */
+	sensor->pwdn = devm_gpiod_get_optional(dev, "pwdn", GPIOD_OUT_HIGH);
+	if (IS_ERR(sensor->pwdn)) {
+		dev_err(dev, "Failed to get 'pwdn' gpio\n");
+		return -EINVAL;
+	}
+
+	mutex_init(&sensor->lock);
+
+	sensor->mode = OV5647_DEFAULT_MODE;
+
+	ret = ov5647_init_controls(sensor);
+	if (ret)
+		goto mutex_destroy;
+
+	sd = &sensor->sd;
+	v4l2_i2c_subdev_init(sd, client, &ov5647_subdev_ops);
+	sd->internal_ops = &ov5647_subdev_internal_ops;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
+
+	sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
+	sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret = media_entity_pads_init(&sd->entity, 1, &sensor->pad);
+	if (ret < 0)
+		goto ctrl_handler_free;
+
+	ret = ov5647_power_on(dev);
+	if (ret)
+		goto entity_cleanup;
+
+	ret = ov5647_detect(sd);
+	if (ret < 0)
+		goto power_off;
+
+	ret = v4l2_async_register_subdev(sd);
+	if (ret < 0)
+		goto power_off;
+
+	/* Enable runtime PM and turn off the device */
+	pm_runtime_set_active(dev);
+	pm_runtime_enable(dev);
+	pm_runtime_idle(dev);
+
+	dev_dbg(dev, "OmniVision OV5647 camera driver probed\n");
+
+	return 0;
+
+power_off:
+	ov5647_power_off(dev);
+entity_cleanup:
+	media_entity_cleanup(&sd->entity);
+ctrl_handler_free:
+	v4l2_ctrl_handler_free(&sensor->ctrls);
+mutex_destroy:
+	mutex_destroy(&sensor->lock);
+
+	return ret;
+}
+
+static void ov5647_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov5647 *sensor = to_sensor(sd);
+
+	v4l2_async_unregister_subdev(&sensor->sd);
+	media_entity_cleanup(&sensor->sd.entity);
+	v4l2_ctrl_handler_free(&sensor->ctrls);
+	v4l2_device_unregister_subdev(sd);
+	pm_runtime_disable(&client->dev);
+	mutex_destroy(&sensor->lock);
+}
+
+static const struct dev_pm_ops ov5647_pm_ops = {
+	SET_RUNTIME_PM_OPS(ov5647_power_off, ov5647_power_on, NULL)
+};
+
+static const struct i2c_device_id ov5647_id[] = {
+	{ "ov5647", 0 },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(i2c, ov5647_id);
+
+#if IS_ENABLED(CONFIG_OF)
+static const struct of_device_id ov5647_of_match[] = {
+	{ .compatible = "ovti,ov5647" },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, ov5647_of_match);
+#endif
+
+static struct i2c_driver ov5647_driver = {
+	.driver = {
+		.of_match_table = of_match_ptr(ov5647_of_match),
+		.name	= "ov5647",
+		.pm	= &ov5647_pm_ops,
+	},
+	.probe		= ov5647_probe,
+	.remove		= ov5647_remove,
+	.id_table	= ov5647_id,
+};
+
+module_i2c_driver(ov5647_driver);
+
+MODULE_AUTHOR("Ramiro Oliveira <roliveir@synopsys.com>");
+MODULE_DESCRIPTION("A low-level driver for OmniVision ov5647 sensors");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/ov5648.c b/drivers/media/i2c/ov5648.c
new file mode 100644
index 000000000..aa10eb4e3
--- /dev/null
+++ b/drivers/media/i2c/ov5648.c
@@ -0,0 +1,2627 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2020 Bootlin
+ * Author: Paul Kocialkowski <paul.kocialkowski@bootlin.com>
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/of_graph.h>
+#include <linux/pm_runtime.h>
+#include <linux/regulator/consumer.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-image-sizes.h>
+#include <media/v4l2-mediabus.h>
+
+/* Clock rate */
+
+#define OV5648_XVCLK_RATE			24000000
+
+/* Register definitions */
+
+/* System */
+
+#define OV5648_SW_STANDBY_REG			0x100
+#define OV5648_SW_STANDBY_STREAM_ON		BIT(0)
+
+#define OV5648_SW_RESET_REG			0x103
+#define OV5648_SW_RESET_RESET			BIT(0)
+
+#define OV5648_PAD_OEN0_REG			0x3000
+#define OV5648_PAD_OEN1_REG			0x3001
+#define OV5648_PAD_OEN2_REG			0x3002
+#define OV5648_PAD_OUT0_REG			0x3008
+#define OV5648_PAD_OUT1_REG			0x3009
+
+#define OV5648_CHIP_ID_H_REG			0x300a
+#define OV5648_CHIP_ID_H_VALUE			0x56
+#define OV5648_CHIP_ID_L_REG			0x300b
+#define OV5648_CHIP_ID_L_VALUE			0x48
+
+#define OV5648_PAD_OUT2_REG			0x300d
+#define OV5648_PAD_SEL0_REG			0x300e
+#define OV5648_PAD_SEL1_REG			0x300f
+#define OV5648_PAD_SEL2_REG			0x3010
+#define OV5648_PAD_PK_REG			0x3011
+#define OV5648_PAD_PK_PD_DATO_EN		BIT(7)
+#define OV5648_PAD_PK_DRIVE_STRENGTH_1X		(0 << 5)
+#define OV5648_PAD_PK_DRIVE_STRENGTH_2X		(2 << 5)
+#define OV5648_PAD_PK_FREX_N			BIT(1)
+
+#define OV5648_A_PWC_PK_O0_REG			0x3013
+#define OV5648_A_PWC_PK_O0_BP_REGULATOR_N	BIT(3)
+#define OV5648_A_PWC_PK_O1_REG			0x3014
+
+#define OV5648_MIPI_PHY0_REG			0x3016
+#define OV5648_MIPI_PHY1_REG			0x3017
+#define OV5648_MIPI_SC_CTRL0_REG		0x3018
+#define OV5648_MIPI_SC_CTRL0_MIPI_LANES(v)	(((v) << 5) & GENMASK(7, 5))
+#define OV5648_MIPI_SC_CTRL0_PHY_HS_TX_PD	BIT(4)
+#define OV5648_MIPI_SC_CTRL0_PHY_LP_RX_PD	BIT(3)
+#define OV5648_MIPI_SC_CTRL0_MIPI_EN		BIT(2)
+#define OV5648_MIPI_SC_CTRL0_MIPI_SUSP		BIT(1)
+#define OV5648_MIPI_SC_CTRL0_LANE_DIS_OP	BIT(0)
+#define OV5648_MIPI_SC_CTRL1_REG		0x3019
+#define OV5648_MISC_CTRL0_REG			0x3021
+#define OV5648_MIPI_SC_CTRL2_REG		0x3022
+#define OV5648_SUB_ID_REG			0x302a
+
+#define OV5648_PLL_CTRL0_REG			0x3034
+#define OV5648_PLL_CTRL0_PLL_CHARGE_PUMP(v)	(((v) << 4) & GENMASK(6, 4))
+#define OV5648_PLL_CTRL0_BITS(v)		((v) & GENMASK(3, 0))
+#define OV5648_PLL_CTRL1_REG			0x3035
+#define OV5648_PLL_CTRL1_SYS_DIV(v)		(((v) << 4) & GENMASK(7, 4))
+#define OV5648_PLL_CTRL1_MIPI_DIV(v)		((v) & GENMASK(3, 0))
+#define OV5648_PLL_MUL_REG			0x3036
+#define OV5648_PLL_MUL(v)			((v) & GENMASK(7, 0))
+#define OV5648_PLL_DIV_REG			0x3037
+#define OV5648_PLL_DIV_ROOT_DIV(v)		((((v) - 1) << 4) & BIT(4))
+#define OV5648_PLL_DIV_PLL_PRE_DIV(v)		((v) & GENMASK(3, 0))
+#define OV5648_PLL_DEBUG_REG			0x3038
+#define OV5648_PLL_BYPASS_REG			0x3039
+
+#define OV5648_PLLS_BYPASS_REG			0x303a
+#define OV5648_PLLS_MUL_REG			0x303b
+#define OV5648_PLLS_MUL(v)			((v) & GENMASK(4, 0))
+#define OV5648_PLLS_CTRL_REG			0x303c
+#define OV5648_PLLS_CTRL_PLL_CHARGE_PUMP(v)	(((v) << 4) & GENMASK(6, 4))
+#define OV5648_PLLS_CTRL_SYS_DIV(v)		((v) & GENMASK(3, 0))
+#define OV5648_PLLS_DIV_REG			0x303d
+#define OV5648_PLLS_DIV_PLLS_PRE_DIV(v)		(((v) << 4) & GENMASK(5, 4))
+#define OV5648_PLLS_DIV_PLLS_DIV_R(v)		((((v) - 1) << 2) & BIT(2))
+#define OV5648_PLLS_DIV_PLLS_SEL_DIV(v)		((v) & GENMASK(1, 0))
+
+#define OV5648_SRB_CTRL_REG			0x3106
+#define OV5648_SRB_CTRL_SCLK_DIV(v)		(((v) << 2) & GENMASK(3, 2))
+#define OV5648_SRB_CTRL_RESET_ARBITER_EN	BIT(1)
+#define OV5648_SRB_CTRL_SCLK_ARBITER_EN		BIT(0)
+
+/* Group Hold */
+
+#define OV5648_GROUP_ADR0_REG			0x3200
+#define OV5648_GROUP_ADR1_REG			0x3201
+#define OV5648_GROUP_ADR2_REG			0x3202
+#define OV5648_GROUP_ADR3_REG			0x3203
+#define OV5648_GROUP_LEN0_REG			0x3204
+#define OV5648_GROUP_LEN1_REG			0x3205
+#define OV5648_GROUP_LEN2_REG			0x3206
+#define OV5648_GROUP_LEN3_REG			0x3207
+#define OV5648_GROUP_ACCESS_REG			0x3208
+
+/* Exposure/gain/banding */
+
+#define OV5648_EXPOSURE_CTRL_HH_REG		0x3500
+#define OV5648_EXPOSURE_CTRL_HH(v)		(((v) & GENMASK(19, 16)) >> 16)
+#define OV5648_EXPOSURE_CTRL_HH_VALUE(v)	(((v) << 16) & GENMASK(19, 16))
+#define OV5648_EXPOSURE_CTRL_H_REG		0x3501
+#define OV5648_EXPOSURE_CTRL_H(v)		(((v) & GENMASK(15, 8)) >> 8)
+#define OV5648_EXPOSURE_CTRL_H_VALUE(v)		(((v) << 8) & GENMASK(15, 8))
+#define OV5648_EXPOSURE_CTRL_L_REG		0x3502
+#define OV5648_EXPOSURE_CTRL_L(v)		((v) & GENMASK(7, 0))
+#define OV5648_EXPOSURE_CTRL_L_VALUE(v)		((v) & GENMASK(7, 0))
+#define OV5648_MANUAL_CTRL_REG			0x3503
+#define OV5648_MANUAL_CTRL_FRAME_DELAY(v)	(((v) << 4) & GENMASK(5, 4))
+#define OV5648_MANUAL_CTRL_AGC_MANUAL_EN	BIT(1)
+#define OV5648_MANUAL_CTRL_AEC_MANUAL_EN	BIT(0)
+#define OV5648_GAIN_CTRL_H_REG			0x350a
+#define OV5648_GAIN_CTRL_H(v)			(((v) & GENMASK(9, 8)) >> 8)
+#define OV5648_GAIN_CTRL_H_VALUE(v)		(((v) << 8) & GENMASK(9, 8))
+#define OV5648_GAIN_CTRL_L_REG			0x350b
+#define OV5648_GAIN_CTRL_L(v)			((v) & GENMASK(7, 0))
+#define OV5648_GAIN_CTRL_L_VALUE(v)		((v) & GENMASK(7, 0))
+
+#define OV5648_ANALOG_CTRL0_REG_BASE		0x3600
+#define OV5648_ANALOG_CTRL1_REG_BASE		0x3700
+
+#define OV5648_AEC_CTRL0_REG			0x3a00
+#define OV5648_AEC_CTRL0_DEBUG			BIT(6)
+#define OV5648_AEC_CTRL0_DEBAND_EN		BIT(5)
+#define OV5648_AEC_CTRL0_DEBAND_LOW_LIMIT_EN	BIT(4)
+#define OV5648_AEC_CTRL0_START_SEL_EN		BIT(3)
+#define OV5648_AEC_CTRL0_NIGHT_MODE_EN		BIT(2)
+#define OV5648_AEC_CTRL0_FREEZE_EN		BIT(0)
+#define OV5648_EXPOSURE_MIN_REG			0x3a01
+#define OV5648_EXPOSURE_MAX_60_H_REG		0x3a02
+#define OV5648_EXPOSURE_MAX_60_L_REG		0x3a03
+#define OV5648_AEC_CTRL5_REG			0x3a05
+#define OV5648_AEC_CTRL6_REG			0x3a06
+#define OV5648_AEC_CTRL7_REG			0x3a07
+#define OV5648_BANDING_STEP_50_H_REG		0x3a08
+#define OV5648_BANDING_STEP_50_L_REG		0x3a09
+#define OV5648_BANDING_STEP_60_H_REG		0x3a0a
+#define OV5648_BANDING_STEP_60_L_REG		0x3a0b
+#define OV5648_AEC_CTRLC_REG			0x3a0c
+#define OV5648_BANDING_MAX_60_REG		0x3a0d
+#define OV5648_BANDING_MAX_50_REG		0x3a0e
+#define OV5648_WPT_REG				0x3a0f
+#define OV5648_BPT_REG				0x3a10
+#define OV5648_VPT_HIGH_REG			0x3a11
+#define OV5648_AVG_MANUAL_REG			0x3a12
+#define OV5648_PRE_GAIN_REG			0x3a13
+#define OV5648_EXPOSURE_MAX_50_H_REG		0x3a14
+#define OV5648_EXPOSURE_MAX_50_L_REG		0x3a15
+#define OV5648_GAIN_BASE_NIGHT_REG		0x3a17
+#define OV5648_AEC_GAIN_CEILING_H_REG		0x3a18
+#define OV5648_AEC_GAIN_CEILING_L_REG		0x3a19
+#define OV5648_DIFF_MAX_REG			0x3a1a
+#define OV5648_WPT2_REG				0x3a1b
+#define OV5648_LED_ADD_ROW_H_REG		0x3a1c
+#define OV5648_LED_ADD_ROW_L_REG		0x3a1d
+#define OV5648_BPT2_REG				0x3a1e
+#define OV5648_VPT_LOW_REG			0x3a1f
+#define OV5648_AEC_CTRL20_REG			0x3a20
+#define OV5648_AEC_CTRL21_REG			0x3a21
+
+#define OV5648_AVG_START_X_H_REG		0x5680
+#define OV5648_AVG_START_X_L_REG		0x5681
+#define OV5648_AVG_START_Y_H_REG		0x5682
+#define OV5648_AVG_START_Y_L_REG		0x5683
+#define OV5648_AVG_WINDOW_X_H_REG		0x5684
+#define OV5648_AVG_WINDOW_X_L_REG		0x5685
+#define OV5648_AVG_WINDOW_Y_H_REG		0x5686
+#define OV5648_AVG_WINDOW_Y_L_REG		0x5687
+#define OV5648_AVG_WEIGHT00_REG			0x5688
+#define OV5648_AVG_WEIGHT01_REG			0x5689
+#define OV5648_AVG_WEIGHT02_REG			0x568a
+#define OV5648_AVG_WEIGHT03_REG			0x568b
+#define OV5648_AVG_WEIGHT04_REG			0x568c
+#define OV5648_AVG_WEIGHT05_REG			0x568d
+#define OV5648_AVG_WEIGHT06_REG			0x568e
+#define OV5648_AVG_WEIGHT07_REG			0x568f
+#define OV5648_AVG_CTRL10_REG			0x5690
+#define OV5648_AVG_WEIGHT_SUM_REG		0x5691
+#define OV5648_AVG_READOUT_REG			0x5693
+
+#define OV5648_DIG_CTRL0_REG			0x5a00
+#define OV5648_DIG_COMP_MAN_H_REG		0x5a02
+#define OV5648_DIG_COMP_MAN_L_REG		0x5a03
+
+#define OV5648_GAINC_MAN_H_REG			0x5a20
+#define OV5648_GAINC_MAN_L_REG			0x5a21
+#define OV5648_GAINC_DGC_MAN_H_REG		0x5a22
+#define OV5648_GAINC_DGC_MAN_L_REG		0x5a23
+#define OV5648_GAINC_CTRL0_REG			0x5a24
+
+#define OV5648_GAINF_ANA_NUM_REG		0x5a40
+#define OV5648_GAINF_DIG_GAIN_REG		0x5a41
+
+/* Timing */
+
+#define OV5648_CROP_START_X_H_REG		0x3800
+#define OV5648_CROP_START_X_H(v)		(((v) & GENMASK(11, 8)) >> 8)
+#define OV5648_CROP_START_X_L_REG		0x3801
+#define OV5648_CROP_START_X_L(v)		((v) & GENMASK(7, 0))
+#define OV5648_CROP_START_Y_H_REG		0x3802
+#define OV5648_CROP_START_Y_H(v)		(((v) & GENMASK(11, 8)) >> 8)
+#define OV5648_CROP_START_Y_L_REG		0x3803
+#define OV5648_CROP_START_Y_L(v)		((v) & GENMASK(7, 0))
+#define OV5648_CROP_END_X_H_REG			0x3804
+#define OV5648_CROP_END_X_H(v)			(((v) & GENMASK(11, 8)) >> 8)
+#define OV5648_CROP_END_X_L_REG			0x3805
+#define OV5648_CROP_END_X_L(v)			((v) & GENMASK(7, 0))
+#define OV5648_CROP_END_Y_H_REG			0x3806
+#define OV5648_CROP_END_Y_H(v)			(((v) & GENMASK(11, 8)) >> 8)
+#define OV5648_CROP_END_Y_L_REG			0x3807
+#define OV5648_CROP_END_Y_L(v)			((v) & GENMASK(7, 0))
+#define OV5648_OUTPUT_SIZE_X_H_REG		0x3808
+#define OV5648_OUTPUT_SIZE_X_H(v)		(((v) & GENMASK(11, 8)) >> 8)
+#define OV5648_OUTPUT_SIZE_X_L_REG		0x3809
+#define OV5648_OUTPUT_SIZE_X_L(v)		((v) & GENMASK(7, 0))
+#define OV5648_OUTPUT_SIZE_Y_H_REG		0x380a
+#define OV5648_OUTPUT_SIZE_Y_H(v)		(((v) & GENMASK(11, 8)) >> 8)
+#define OV5648_OUTPUT_SIZE_Y_L_REG		0x380b
+#define OV5648_OUTPUT_SIZE_Y_L(v)		((v) & GENMASK(7, 0))
+#define OV5648_HTS_H_REG			0x380c
+#define OV5648_HTS_H(v)				(((v) & GENMASK(12, 8)) >> 8)
+#define OV5648_HTS_L_REG			0x380d
+#define OV5648_HTS_L(v)				((v) & GENMASK(7, 0))
+#define OV5648_VTS_H_REG			0x380e
+#define OV5648_VTS_H(v)				(((v) & GENMASK(15, 8)) >> 8)
+#define OV5648_VTS_L_REG			0x380f
+#define OV5648_VTS_L(v)				((v) & GENMASK(7, 0))
+#define OV5648_OFFSET_X_H_REG			0x3810
+#define OV5648_OFFSET_X_H(v)			(((v) & GENMASK(11, 8)) >> 8)
+#define OV5648_OFFSET_X_L_REG			0x3811
+#define OV5648_OFFSET_X_L(v)			((v) & GENMASK(7, 0))
+#define OV5648_OFFSET_Y_H_REG			0x3812
+#define OV5648_OFFSET_Y_H(v)			(((v) & GENMASK(11, 8)) >> 8)
+#define OV5648_OFFSET_Y_L_REG			0x3813
+#define OV5648_OFFSET_Y_L(v)			((v) & GENMASK(7, 0))
+#define OV5648_SUB_INC_X_REG			0x3814
+#define OV5648_SUB_INC_X_ODD(v)			(((v) << 4) & GENMASK(7, 4))
+#define OV5648_SUB_INC_X_EVEN(v)		((v) & GENMASK(3, 0))
+#define OV5648_SUB_INC_Y_REG			0x3815
+#define OV5648_SUB_INC_Y_ODD(v)			(((v) << 4) & GENMASK(7, 4))
+#define OV5648_SUB_INC_Y_EVEN(v)		((v) & GENMASK(3, 0))
+#define OV5648_HSYNCST_H_REG			0x3816
+#define OV5648_HSYNCST_H(v)			(((v) >> 8) & 0xf)
+#define OV5648_HSYNCST_L_REG			0x3817
+#define OV5648_HSYNCST_L(v)			((v) & GENMASK(7, 0))
+#define OV5648_HSYNCW_H_REG			0x3818
+#define OV5648_HSYNCW_H(v)			(((v) >> 8) & 0xf)
+#define OV5648_HSYNCW_L_REG			0x3819
+#define OV5648_HSYNCW_L(v)			((v) & GENMASK(7, 0))
+
+#define OV5648_TC20_REG				0x3820
+#define OV5648_TC20_DEBUG			BIT(6)
+#define OV5648_TC20_FLIP_VERT_ISP_EN		BIT(2)
+#define OV5648_TC20_FLIP_VERT_SENSOR_EN		BIT(1)
+#define OV5648_TC20_BINNING_VERT_EN		BIT(0)
+#define OV5648_TC21_REG				0x3821
+#define OV5648_TC21_FLIP_HORZ_ISP_EN		BIT(2)
+#define OV5648_TC21_FLIP_HORZ_SENSOR_EN		BIT(1)
+#define OV5648_TC21_BINNING_HORZ_EN		BIT(0)
+
+/* Strobe/exposure */
+
+#define OV5648_STROBE_REG			0x3b00
+#define OV5648_FREX_EXP_HH_REG			0x3b01
+#define OV5648_SHUTTER_DLY_H_REG		0x3b02
+#define OV5648_SHUTTER_DLY_L_REG		0x3b03
+#define OV5648_FREX_EXP_H_REG			0x3b04
+#define OV5648_FREX_EXP_L_REG			0x3b05
+#define OV5648_FREX_CTRL_REG			0x3b06
+#define OV5648_FREX_MODE_SEL_REG		0x3b07
+#define OV5648_FREX_MODE_SEL_FREX_SA1		BIT(4)
+#define OV5648_FREX_MODE_SEL_FX1_FM_EN		BIT(3)
+#define OV5648_FREX_MODE_SEL_FREX_INV		BIT(2)
+#define OV5648_FREX_MODE_SEL_MODE1		0x0
+#define OV5648_FREX_MODE_SEL_MODE2		0x1
+#define OV5648_FREX_MODE_SEL_ROLLING		0x2
+#define OV5648_FREX_EXP_REQ_REG			0x3b08
+#define OV5648_FREX_SHUTTER_DLY_REG		0x3b09
+#define OV5648_FREX_RST_LEN_REG			0x3b0a
+#define OV5648_STROBE_WIDTH_HH_REG		0x3b0b
+#define OV5648_STROBE_WIDTH_H_REG		0x3b0c
+
+/* OTP */
+
+#define OV5648_OTP_DATA_REG_BASE		0x3d00
+#define OV5648_OTP_PROGRAM_CTRL_REG		0x3d80
+#define OV5648_OTP_LOAD_CTRL_REG		0x3d81
+
+/* PSRAM */
+
+#define OV5648_PSRAM_CTRL1_REG			0x3f01
+#define OV5648_PSRAM_CTRLF_REG			0x3f0f
+
+/* Black Level */
+
+#define OV5648_BLC_CTRL0_REG			0x4000
+#define OV5648_BLC_CTRL1_REG			0x4001
+#define OV5648_BLC_CTRL1_START_LINE(v)		((v) & GENMASK(5, 0))
+#define OV5648_BLC_CTRL2_REG			0x4002
+#define OV5648_BLC_CTRL2_AUTO_EN		BIT(6)
+#define OV5648_BLC_CTRL2_RESET_FRAME_NUM(v)	((v) & GENMASK(5, 0))
+#define OV5648_BLC_CTRL3_REG			0x4003
+#define OV5648_BLC_LINE_NUM_REG			0x4004
+#define OV5648_BLC_LINE_NUM(v)			((v) & GENMASK(7, 0))
+#define OV5648_BLC_CTRL5_REG			0x4005
+#define OV5648_BLC_CTRL5_UPDATE_EN		BIT(1)
+#define OV5648_BLC_LEVEL_REG			0x4009
+
+/* Frame */
+
+#define OV5648_FRAME_CTRL_REG			0x4200
+#define OV5648_FRAME_ON_NUM_REG			0x4201
+#define OV5648_FRAME_OFF_NUM_REG		0x4202
+
+/* MIPI CSI-2 */
+
+#define OV5648_MIPI_CTRL0_REG			0x4800
+#define OV5648_MIPI_CTRL0_CLK_LANE_AUTOGATE	BIT(5)
+#define OV5648_MIPI_CTRL0_LANE_SYNC_EN		BIT(4)
+#define OV5648_MIPI_CTRL0_LANE_SELECT_LANE1	0
+#define OV5648_MIPI_CTRL0_LANE_SELECT_LANE2	BIT(3)
+#define OV5648_MIPI_CTRL0_IDLE_LP00		0
+#define OV5648_MIPI_CTRL0_IDLE_LP11		BIT(2)
+
+#define OV5648_MIPI_CTRL1_REG			0x4801
+#define OV5648_MIPI_CTRL2_REG			0x4802
+#define OV5648_MIPI_CTRL3_REG			0x4803
+#define OV5648_MIPI_CTRL4_REG			0x4804
+#define OV5648_MIPI_CTRL5_REG			0x4805
+#define OV5648_MIPI_MAX_FRAME_COUNT_H_REG	0x4810
+#define OV5648_MIPI_MAX_FRAME_COUNT_L_REG	0x4811
+#define OV5648_MIPI_CTRL14_REG			0x4814
+#define OV5648_MIPI_DT_SPKT_REG			0x4815
+#define OV5648_MIPI_HS_ZERO_MIN_H_REG		0x4818
+#define OV5648_MIPI_HS_ZERO_MIN_L_REG		0x4819
+#define OV5648_MIPI_HS_TRAIN_MIN_H_REG		0x481a
+#define OV5648_MIPI_HS_TRAIN_MIN_L_REG		0x481b
+#define OV5648_MIPI_CLK_ZERO_MIN_H_REG		0x481c
+#define OV5648_MIPI_CLK_ZERO_MIN_L_REG		0x481d
+#define OV5648_MIPI_CLK_PREPARE_MIN_H_REG	0x481e
+#define OV5648_MIPI_CLK_PREPARE_MIN_L_REG	0x481f
+#define OV5648_MIPI_CLK_POST_MIN_H_REG		0x4820
+#define OV5648_MIPI_CLK_POST_MIN_L_REG		0x4821
+#define OV5648_MIPI_CLK_TRAIL_MIN_H_REG		0x4822
+#define OV5648_MIPI_CLK_TRAIL_MIN_L_REG		0x4823
+#define OV5648_MIPI_LPX_P_MIN_H_REG		0x4824
+#define OV5648_MIPI_LPX_P_MIN_L_REG		0x4825
+#define OV5648_MIPI_HS_PREPARE_MIN_H_REG	0x4826
+#define OV5648_MIPI_HS_PREPARE_MIN_L_REG	0x4827
+#define OV5648_MIPI_HS_EXIT_MIN_H_REG		0x4828
+#define OV5648_MIPI_HS_EXIT_MIN_L_REG		0x4829
+#define OV5648_MIPI_HS_ZERO_MIN_UI_REG		0x482a
+#define OV5648_MIPI_HS_TRAIL_MIN_UI_REG		0x482b
+#define OV5648_MIPI_CLK_ZERO_MIN_UI_REG		0x482c
+#define OV5648_MIPI_CLK_PREPARE_MIN_UI_REG	0x482d
+#define OV5648_MIPI_CLK_POST_MIN_UI_REG		0x482e
+#define OV5648_MIPI_CLK_TRAIL_MIN_UI_REG	0x482f
+#define OV5648_MIPI_LPX_P_MIN_UI_REG		0x4830
+#define OV5648_MIPI_HS_PREPARE_MIN_UI_REG	0x4831
+#define OV5648_MIPI_HS_EXIT_MIN_UI_REG		0x4832
+#define OV5648_MIPI_REG_MIN_H_REG		0x4833
+#define OV5648_MIPI_REG_MIN_L_REG		0x4834
+#define OV5648_MIPI_REG_MAX_H_REG		0x4835
+#define OV5648_MIPI_REG_MAX_L_REG		0x4836
+#define OV5648_MIPI_PCLK_PERIOD_REG		0x4837
+#define OV5648_MIPI_WKUP_DLY_REG		0x4838
+#define OV5648_MIPI_LP_GPIO_REG			0x483b
+#define OV5648_MIPI_SNR_PCLK_DIV_REG		0x4843
+
+/* ISP */
+
+#define OV5648_ISP_CTRL0_REG			0x5000
+#define OV5648_ISP_CTRL0_BLACK_CORRECT_EN	BIT(2)
+#define OV5648_ISP_CTRL0_WHITE_CORRECT_EN	BIT(1)
+#define OV5648_ISP_CTRL1_REG			0x5001
+#define OV5648_ISP_CTRL1_AWB_EN			BIT(0)
+#define OV5648_ISP_CTRL2_REG			0x5002
+#define OV5648_ISP_CTRL2_WIN_EN			BIT(6)
+#define OV5648_ISP_CTRL2_OTP_EN			BIT(1)
+#define OV5648_ISP_CTRL2_AWB_GAIN_EN		BIT(0)
+#define OV5648_ISP_CTRL3_REG			0x5003
+#define OV5648_ISP_CTRL3_BUF_EN			BIT(3)
+#define OV5648_ISP_CTRL3_BIN_MAN_SET		BIT(2)
+#define OV5648_ISP_CTRL3_BIN_AUTO_EN		BIT(1)
+#define OV5648_ISP_CTRL4_REG			0x5004
+#define OV5648_ISP_CTRL5_REG			0x5005
+#define OV5648_ISP_CTRL6_REG			0x5006
+#define OV5648_ISP_CTRL7_REG			0x5007
+#define OV5648_ISP_MAN_OFFSET_X_H_REG		0x5008
+#define OV5648_ISP_MAN_OFFSET_X_L_REG		0x5009
+#define OV5648_ISP_MAN_OFFSET_Y_H_REG		0x500a
+#define OV5648_ISP_MAN_OFFSET_Y_L_REG		0x500b
+#define OV5648_ISP_MAN_WIN_OFFSET_X_H_REG	0x500c
+#define OV5648_ISP_MAN_WIN_OFFSET_X_L_REG	0x500d
+#define OV5648_ISP_MAN_WIN_OFFSET_Y_H_REG	0x500e
+#define OV5648_ISP_MAN_WIN_OFFSET_Y_L_REG	0x500f
+#define OV5648_ISP_MAN_WIN_OUTPUT_X_H_REG	0x5010
+#define OV5648_ISP_MAN_WIN_OUTPUT_X_L_REG	0x5011
+#define OV5648_ISP_MAN_WIN_OUTPUT_Y_H_REG	0x5012
+#define OV5648_ISP_MAN_WIN_OUTPUT_Y_L_REG	0x5013
+#define OV5648_ISP_MAN_INPUT_X_H_REG		0x5014
+#define OV5648_ISP_MAN_INPUT_X_L_REG		0x5015
+#define OV5648_ISP_MAN_INPUT_Y_H_REG		0x5016
+#define OV5648_ISP_MAN_INPUT_Y_L_REG		0x5017
+#define OV5648_ISP_CTRL18_REG			0x5018
+#define OV5648_ISP_CTRL19_REG			0x5019
+#define OV5648_ISP_CTRL1A_REG			0x501a
+#define OV5648_ISP_CTRL1D_REG			0x501d
+#define OV5648_ISP_CTRL1F_REG			0x501f
+#define OV5648_ISP_CTRL1F_OUTPUT_EN		3
+#define OV5648_ISP_CTRL25_REG			0x5025
+
+#define OV5648_ISP_CTRL3D_REG			0x503d
+#define OV5648_ISP_CTRL3D_PATTERN_EN		BIT(7)
+#define OV5648_ISP_CTRL3D_ROLLING_BAR_EN	BIT(6)
+#define OV5648_ISP_CTRL3D_TRANSPARENT_MODE	BIT(5)
+#define OV5648_ISP_CTRL3D_SQUARES_BW_MODE	BIT(4)
+#define OV5648_ISP_CTRL3D_PATTERN_COLOR_BARS	0
+#define OV5648_ISP_CTRL3D_PATTERN_RANDOM_DATA	1
+#define OV5648_ISP_CTRL3D_PATTERN_COLOR_SQUARES	2
+#define OV5648_ISP_CTRL3D_PATTERN_INPUT		3
+
+#define OV5648_ISP_CTRL3E_REG			0x503e
+#define OV5648_ISP_CTRL4B_REG			0x504b
+#define OV5648_ISP_CTRL4B_POST_BIN_H_EN		BIT(5)
+#define OV5648_ISP_CTRL4B_POST_BIN_V_EN		BIT(4)
+#define OV5648_ISP_CTRL4C_REG			0x504c
+#define OV5648_ISP_CTRL57_REG			0x5057
+#define OV5648_ISP_CTRL58_REG			0x5058
+#define OV5648_ISP_CTRL59_REG			0x5059
+
+#define OV5648_ISP_WINDOW_START_X_H_REG		0x5980
+#define OV5648_ISP_WINDOW_START_X_L_REG		0x5981
+#define OV5648_ISP_WINDOW_START_Y_H_REG		0x5982
+#define OV5648_ISP_WINDOW_START_Y_L_REG		0x5983
+#define OV5648_ISP_WINDOW_WIN_X_H_REG		0x5984
+#define OV5648_ISP_WINDOW_WIN_X_L_REG		0x5985
+#define OV5648_ISP_WINDOW_WIN_Y_H_REG		0x5986
+#define OV5648_ISP_WINDOW_WIN_Y_L_REG		0x5987
+#define OV5648_ISP_WINDOW_MAN_REG		0x5988
+
+/* White Balance */
+
+#define OV5648_AWB_CTRL_REG			0x5180
+#define OV5648_AWB_CTRL_FAST_AWB		BIT(6)
+#define OV5648_AWB_CTRL_GAIN_FREEZE_EN		BIT(5)
+#define OV5648_AWB_CTRL_SUM_FREEZE_EN		BIT(4)
+#define OV5648_AWB_CTRL_GAIN_MANUAL_EN		BIT(3)
+
+#define OV5648_AWB_DELTA_REG			0x5181
+#define OV5648_AWB_STABLE_RANGE_REG		0x5182
+#define OV5648_AWB_STABLE_RANGE_WIDE_REG	0x5183
+#define OV5648_HSIZE_MAN_REG			0x5185
+
+#define OV5648_GAIN_RED_MAN_H_REG		0x5186
+#define OV5648_GAIN_RED_MAN_H(v)		(((v) & GENMASK(11, 8)) >> 8)
+#define OV5648_GAIN_RED_MAN_L_REG		0x5187
+#define OV5648_GAIN_RED_MAN_L(v)		((v) & GENMASK(7, 0))
+#define OV5648_GAIN_GREEN_MAN_H_REG		0x5188
+#define OV5648_GAIN_GREEN_MAN_H(v)		(((v) & GENMASK(11, 8)) >> 8)
+#define OV5648_GAIN_GREEN_MAN_L_REG		0x5189
+#define OV5648_GAIN_GREEN_MAN_L(v)		((v) & GENMASK(7, 0))
+#define OV5648_GAIN_BLUE_MAN_H_REG		0x518a
+#define OV5648_GAIN_BLUE_MAN_H(v)		(((v) & GENMASK(11, 8)) >> 8)
+#define OV5648_GAIN_BLUE_MAN_L_REG		0x518b
+#define OV5648_GAIN_BLUE_MAN_L(v)		((v) & GENMASK(7, 0))
+#define OV5648_GAIN_RED_LIMIT_REG		0x518c
+#define OV5648_GAIN_GREEN_LIMIT_REG		0x518d
+#define OV5648_GAIN_BLUE_LIMIT_REG		0x518e
+#define OV5648_AWB_FRAME_COUNT_REG		0x518f
+#define OV5648_AWB_BASE_MAN_REG			0x51df
+
+/* Macros */
+
+#define ov5648_subdev_sensor(s) \
+	container_of(s, struct ov5648_sensor, subdev)
+
+#define ov5648_ctrl_subdev(c) \
+	(&container_of((c)->handler, struct ov5648_sensor, \
+		       ctrls.handler)->subdev)
+
+/* Data structures */
+
+struct ov5648_register_value {
+	u16 address;
+	u8 value;
+	unsigned int delay_ms;
+};
+
+/*
+ * PLL1 Clock Tree:
+ *
+ * +-< XVCLK
+ * |
+ * +-+ pll_pre_div (0x3037 [3:0], special values: 5: 1.5, 7: 2.5)
+ *   |
+ *   +-+ pll_mul (0x3036 [7:0])
+ *     |
+ *     +-+ sys_div (0x3035 [7:4])
+ *       |
+ *       +-+ mipi_div (0x3035 [3:0])
+ *       | |
+ *       | +-> MIPI_SCLK
+ *       | |
+ *       | +-+ mipi_phy_div (2)
+ *       |   |
+ *       |   +-> MIPI_CLK
+ *       |
+ *       +-+ root_div (0x3037 [4])
+ *         |
+ *         +-+ bit_div (0x3034 [3:0], 8 bits: 2, 10 bits: 2.5, other: 1)
+ *           |
+ *           +-+ sclk_div (0x3106 [3:2])
+ *             |
+ *             +-> SCLK
+ *             |
+ *             +-+ mipi_div (0x3035, 1: PCLK = SCLK)
+ *               |
+ *               +-> PCLK
+ */
+
+struct ov5648_pll1_config {
+	unsigned int pll_pre_div;
+	unsigned int pll_mul;
+	unsigned int sys_div;
+	unsigned int root_div;
+	unsigned int sclk_div;
+	unsigned int mipi_div;
+};
+
+/*
+ * PLL2 Clock Tree:
+ *
+ * +-< XVCLK
+ * |
+ * +-+ plls_pre_div (0x303d [5:4], special values: 0: 1, 1: 1.5)
+ *   |
+ *   +-+ plls_div_r (0x303d [2])
+ *     |
+ *     +-+ plls_mul (0x303b [4:0])
+ *       |
+ *       +-+ sys_div (0x303c [3:0])
+ *         |
+ *         +-+ sel_div (0x303d [1:0], special values: 0: 1, 3: 2.5)
+ *           |
+ *           +-> ADCLK
+ */
+
+struct ov5648_pll2_config {
+	unsigned int plls_pre_div;
+	unsigned int plls_div_r;
+	unsigned int plls_mul;
+	unsigned int sys_div;
+	unsigned int sel_div;
+};
+
+/*
+ * General formulas for (array-centered) mode calculation:
+ * - photo_array_width = 2624
+ * - crop_start_x = (photo_array_width - output_size_x) / 2
+ * - crop_end_x = crop_start_x + offset_x + output_size_x - 1
+ *
+ * - photo_array_height = 1956
+ * - crop_start_y = (photo_array_height - output_size_y) / 2
+ * - crop_end_y = crop_start_y + offset_y + output_size_y - 1
+ */
+
+struct ov5648_mode {
+	unsigned int crop_start_x;
+	unsigned int offset_x;
+	unsigned int output_size_x;
+	unsigned int crop_end_x;
+	unsigned int hts;
+
+	unsigned int crop_start_y;
+	unsigned int offset_y;
+	unsigned int output_size_y;
+	unsigned int crop_end_y;
+	unsigned int vts;
+
+	bool binning_x;
+	bool binning_y;
+
+	unsigned int inc_x_odd;
+	unsigned int inc_x_even;
+	unsigned int inc_y_odd;
+	unsigned int inc_y_even;
+
+	/* 8-bit frame interval followed by 10-bit frame interval. */
+	struct v4l2_fract frame_interval[2];
+
+	/* 8-bit config followed by 10-bit config. */
+	const struct ov5648_pll1_config *pll1_config[2];
+	const struct ov5648_pll2_config *pll2_config;
+
+	const struct ov5648_register_value *register_values;
+	unsigned int register_values_count;
+};
+
+struct ov5648_state {
+	const struct ov5648_mode *mode;
+	u32 mbus_code;
+
+	bool streaming;
+};
+
+struct ov5648_ctrls {
+	struct v4l2_ctrl *exposure_auto;
+	struct v4l2_ctrl *exposure;
+
+	struct v4l2_ctrl *gain_auto;
+	struct v4l2_ctrl *gain;
+
+	struct v4l2_ctrl *white_balance_auto;
+	struct v4l2_ctrl *red_balance;
+	struct v4l2_ctrl *blue_balance;
+
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *pixel_rate;
+
+	struct v4l2_ctrl_handler handler;
+};
+
+struct ov5648_sensor {
+	struct device *dev;
+	struct i2c_client *i2c_client;
+	struct gpio_desc *reset;
+	struct gpio_desc *powerdown;
+	struct regulator *avdd;
+	struct regulator *dvdd;
+	struct regulator *dovdd;
+	struct clk *xvclk;
+
+	struct v4l2_fwnode_endpoint endpoint;
+	struct v4l2_subdev subdev;
+	struct media_pad pad;
+
+	struct mutex mutex;
+
+	struct ov5648_state state;
+	struct ov5648_ctrls ctrls;
+};
+
+/* Static definitions */
+
+/*
+ * XVCLK = 24 MHz
+ * SCLK  = 84 MHz
+ * PCLK  = 84 MHz
+ */
+static const struct ov5648_pll1_config ov5648_pll1_config_native_8_bits = {
+	.pll_pre_div	= 3,
+	.pll_mul	= 84,
+	.sys_div	= 2,
+	.root_div	= 1,
+	.sclk_div	= 1,
+	.mipi_div	= 1,
+};
+
+/*
+ * XVCLK = 24 MHz
+ * SCLK  = 84 MHz
+ * PCLK  = 84 MHz
+ */
+static const struct ov5648_pll1_config ov5648_pll1_config_native_10_bits = {
+	.pll_pre_div	= 3,
+	.pll_mul	= 105,
+	.sys_div	= 2,
+	.root_div	= 1,
+	.sclk_div	= 1,
+	.mipi_div	= 1,
+};
+
+/*
+ * XVCLK = 24 MHz
+ * ADCLK = 200 MHz
+ */
+static const struct ov5648_pll2_config ov5648_pll2_config_native = {
+	.plls_pre_div	= 3,
+	.plls_div_r	= 1,
+	.plls_mul	= 25,
+	.sys_div	= 1,
+	.sel_div	= 1,
+};
+
+static const struct ov5648_mode ov5648_modes[] = {
+	/* 2592x1944 */
+	{
+		/* Horizontal */
+		.crop_start_x	= 16,
+		.offset_x	= 0,
+		.output_size_x	= 2592,
+		.crop_end_x	= 2607,
+		.hts		= 2816,
+
+		/* Vertical */
+		.crop_start_y	= 6,
+		.offset_y	= 0,
+		.output_size_y	= 1944,
+		.crop_end_y	= 1949,
+		.vts		= 1984,
+
+		/* Subsample increase */
+		.inc_x_odd	= 1,
+		.inc_x_even	= 1,
+		.inc_y_odd	= 1,
+		.inc_y_even	= 1,
+
+		/* Frame Interval */
+		.frame_interval	= {
+			{ 1,	15 },
+			{ 1,	15 },
+		},
+
+		/* PLL */
+		.pll1_config	= {
+			&ov5648_pll1_config_native_8_bits,
+			&ov5648_pll1_config_native_10_bits,
+		},
+		.pll2_config	= &ov5648_pll2_config_native,
+	},
+	/* 1600x1200 (UXGA) */
+	{
+		/* Horizontal */
+		.crop_start_x	= 512,
+		.offset_x	= 0,
+		.output_size_x	= 1600,
+		.crop_end_x	= 2111,
+		.hts		= 2816,
+
+		/* Vertical */
+		.crop_start_y	= 378,
+		.offset_y	= 0,
+		.output_size_y	= 1200,
+		.crop_end_y	= 1577,
+		.vts		= 1984,
+
+		/* Subsample increase */
+		.inc_x_odd	= 1,
+		.inc_x_even	= 1,
+		.inc_y_odd	= 1,
+		.inc_y_even	= 1,
+
+		/* Frame Interval */
+		.frame_interval	= {
+			{ 1,	15 },
+			{ 1,	15 },
+		},
+
+		/* PLL */
+		.pll1_config	= {
+			&ov5648_pll1_config_native_8_bits,
+			&ov5648_pll1_config_native_10_bits,
+		},
+		.pll2_config	= &ov5648_pll2_config_native,
+	},
+	/* 1920x1080 (Full HD) */
+	{
+		/* Horizontal */
+		.crop_start_x	= 352,
+		.offset_x	= 0,
+		.output_size_x	= 1920,
+		.crop_end_x	= 2271,
+		.hts		= 2816,
+
+		/* Vertical */
+		.crop_start_y	= 438,
+		.offset_y	= 0,
+		.output_size_y	= 1080,
+		.crop_end_y	= 1517,
+		.vts		= 1984,
+
+		/* Subsample increase */
+		.inc_x_odd	= 1,
+		.inc_x_even	= 1,
+		.inc_y_odd	= 1,
+		.inc_y_even	= 1,
+
+		/* Frame Interval */
+		.frame_interval	= {
+			{ 1,	15 },
+			{ 1,	15 },
+		},
+
+		/* PLL */
+		.pll1_config	= {
+			&ov5648_pll1_config_native_8_bits,
+			&ov5648_pll1_config_native_10_bits,
+		},
+		.pll2_config	= &ov5648_pll2_config_native,
+	},
+	/* 1280x960 */
+	{
+		/* Horizontal */
+		.crop_start_x	= 16,
+		.offset_x	= 8,
+		.output_size_x	= 1280,
+		.crop_end_x	= 2607,
+		.hts		= 1912,
+
+		/* Vertical */
+		.crop_start_y	= 6,
+		.offset_y	= 6,
+		.output_size_y	= 960,
+		.crop_end_y	= 1949,
+		.vts		= 1496,
+
+		/* Binning */
+		.binning_x	= true,
+
+		/* Subsample increase */
+		.inc_x_odd	= 3,
+		.inc_x_even	= 1,
+		.inc_y_odd	= 3,
+		.inc_y_even	= 1,
+
+		/* Frame Interval */
+		.frame_interval	= {
+			{ 1,	30 },
+			{ 1,	30 },
+		},
+
+		/* PLL */
+		.pll1_config	= {
+			&ov5648_pll1_config_native_8_bits,
+			&ov5648_pll1_config_native_10_bits,
+		},
+		.pll2_config	= &ov5648_pll2_config_native,
+	},
+	/* 1280x720 (HD) */
+	{
+		/* Horizontal */
+		.crop_start_x	= 16,
+		.offset_x	= 8,
+		.output_size_x	= 1280,
+		.crop_end_x	= 2607,
+		.hts		= 1912,
+
+		/* Vertical */
+		.crop_start_y	= 254,
+		.offset_y	= 2,
+		.output_size_y	= 720,
+		.crop_end_y	= 1701,
+		.vts		= 1496,
+
+		/* Binning */
+		.binning_x	= true,
+
+		/* Subsample increase */
+		.inc_x_odd	= 3,
+		.inc_x_even	= 1,
+		.inc_y_odd	= 3,
+		.inc_y_even	= 1,
+
+		/* Frame Interval */
+		.frame_interval	= {
+			{ 1,	30 },
+			{ 1,	30 },
+		},
+
+		/* PLL */
+		.pll1_config	= {
+			&ov5648_pll1_config_native_8_bits,
+			&ov5648_pll1_config_native_10_bits,
+		},
+		.pll2_config	= &ov5648_pll2_config_native,
+	},
+	/* 640x480 (VGA) */
+	{
+		/* Horizontal */
+		.crop_start_x	= 0,
+		.offset_x	= 8,
+		.output_size_x	= 640,
+		.crop_end_x	= 2623,
+		.hts		= 1896,
+
+		/* Vertical */
+		.crop_start_y	= 0,
+		.offset_y	= 2,
+		.output_size_y	= 480,
+		.crop_end_y	= 1953,
+		.vts		= 984,
+
+		/* Binning */
+		.binning_x	= true,
+
+		/* Subsample increase */
+		.inc_x_odd	= 7,
+		.inc_x_even	= 1,
+		.inc_y_odd	= 7,
+		.inc_y_even	= 1,
+
+		/* Frame Interval */
+		.frame_interval	= {
+			{ 1,	30 },
+			{ 1,	30 },
+		},
+
+		/* PLL */
+		.pll1_config	= {
+			&ov5648_pll1_config_native_8_bits,
+			&ov5648_pll1_config_native_10_bits,
+		},
+		.pll2_config	= &ov5648_pll2_config_native,
+	},
+};
+
+static const u32 ov5648_mbus_codes[] = {
+	MEDIA_BUS_FMT_SBGGR8_1X8,
+	MEDIA_BUS_FMT_SBGGR10_1X10,
+};
+
+static const struct ov5648_register_value ov5648_init_sequence[] = {
+	/* PSRAM */
+	{ OV5648_PSRAM_CTRL1_REG, 0x0d },
+	{ OV5648_PSRAM_CTRLF_REG, 0xf5 },
+};
+
+static const s64 ov5648_link_freq_menu[] = {
+	210000000,
+	168000000,
+};
+
+static const char *const ov5648_test_pattern_menu[] = {
+	"Disabled",
+	"Random data",
+	"Color bars",
+	"Color bars with rolling bar",
+	"Color squares",
+	"Color squares with rolling bar"
+};
+
+static const u8 ov5648_test_pattern_bits[] = {
+	0,
+	OV5648_ISP_CTRL3D_PATTERN_EN | OV5648_ISP_CTRL3D_PATTERN_RANDOM_DATA,
+	OV5648_ISP_CTRL3D_PATTERN_EN | OV5648_ISP_CTRL3D_PATTERN_COLOR_BARS,
+	OV5648_ISP_CTRL3D_PATTERN_EN | OV5648_ISP_CTRL3D_ROLLING_BAR_EN |
+	OV5648_ISP_CTRL3D_PATTERN_COLOR_BARS,
+	OV5648_ISP_CTRL3D_PATTERN_EN | OV5648_ISP_CTRL3D_PATTERN_COLOR_SQUARES,
+	OV5648_ISP_CTRL3D_PATTERN_EN | OV5648_ISP_CTRL3D_ROLLING_BAR_EN |
+	OV5648_ISP_CTRL3D_PATTERN_COLOR_SQUARES,
+};
+
+/* Input/Output */
+
+static int ov5648_read(struct ov5648_sensor *sensor, u16 address, u8 *value)
+{
+	unsigned char data[2] = { address >> 8, address & 0xff };
+	struct i2c_client *client = sensor->i2c_client;
+	int ret;
+
+	ret = i2c_master_send(client, data, sizeof(data));
+	if (ret < 0) {
+		dev_dbg(&client->dev, "i2c send error at address %#04x\n",
+			address);
+		return ret;
+	}
+
+	ret = i2c_master_recv(client, value, 1);
+	if (ret < 0) {
+		dev_dbg(&client->dev, "i2c recv error at address %#04x\n",
+			address);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ov5648_write(struct ov5648_sensor *sensor, u16 address, u8 value)
+{
+	unsigned char data[3] = { address >> 8, address & 0xff, value };
+	struct i2c_client *client = sensor->i2c_client;
+	int ret;
+
+	ret = i2c_master_send(client, data, sizeof(data));
+	if (ret < 0) {
+		dev_dbg(&client->dev, "i2c send error at address %#04x\n",
+			address);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ov5648_write_sequence(struct ov5648_sensor *sensor,
+				 const struct ov5648_register_value *sequence,
+				 unsigned int sequence_count)
+{
+	unsigned int i;
+	int ret = 0;
+
+	for (i = 0; i < sequence_count; i++) {
+		ret = ov5648_write(sensor, sequence[i].address,
+				   sequence[i].value);
+		if (ret)
+			break;
+
+		if (sequence[i].delay_ms)
+			msleep(sequence[i].delay_ms);
+	}
+
+	return ret;
+}
+
+static int ov5648_update_bits(struct ov5648_sensor *sensor, u16 address,
+			      u8 mask, u8 bits)
+{
+	u8 value = 0;
+	int ret;
+
+	ret = ov5648_read(sensor, address, &value);
+	if (ret)
+		return ret;
+
+	value &= ~mask;
+	value |= bits;
+
+	ret = ov5648_write(sensor, address, value);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+/* Sensor */
+
+static int ov5648_sw_reset(struct ov5648_sensor *sensor)
+{
+	return ov5648_write(sensor, OV5648_SW_RESET_REG, OV5648_SW_RESET_RESET);
+}
+
+static int ov5648_sw_standby(struct ov5648_sensor *sensor, int standby)
+{
+	u8 value = 0;
+
+	if (!standby)
+		value = OV5648_SW_STANDBY_STREAM_ON;
+
+	return ov5648_write(sensor, OV5648_SW_STANDBY_REG, value);
+}
+
+static int ov5648_chip_id_check(struct ov5648_sensor *sensor)
+{
+	u16 regs[] = { OV5648_CHIP_ID_H_REG, OV5648_CHIP_ID_L_REG };
+	u8 values[] = { OV5648_CHIP_ID_H_VALUE, OV5648_CHIP_ID_L_VALUE };
+	unsigned int i;
+	u8 value;
+	int ret;
+
+	for (i = 0; i < ARRAY_SIZE(regs); i++) {
+		ret = ov5648_read(sensor, regs[i], &value);
+		if (ret < 0)
+			return ret;
+
+		if (value != values[i]) {
+			dev_err(sensor->dev,
+				"chip id value mismatch: %#x instead of %#x\n",
+				value, values[i]);
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+static int ov5648_avdd_internal_power(struct ov5648_sensor *sensor, int on)
+{
+	return ov5648_write(sensor, OV5648_A_PWC_PK_O0_REG,
+			    on ? 0 : OV5648_A_PWC_PK_O0_BP_REGULATOR_N);
+}
+
+static int ov5648_pad_configure(struct ov5648_sensor *sensor)
+{
+	int ret;
+
+	/* Configure pads as input. */
+
+	ret = ov5648_write(sensor, OV5648_PAD_OEN1_REG, 0);
+	if (ret)
+		return ret;
+
+	ret = ov5648_write(sensor, OV5648_PAD_OEN2_REG, 0);
+	if (ret)
+		return ret;
+
+	/* Disable FREX pin. */
+
+	return ov5648_write(sensor, OV5648_PAD_PK_REG,
+			    OV5648_PAD_PK_DRIVE_STRENGTH_1X |
+			    OV5648_PAD_PK_FREX_N);
+}
+
+static int ov5648_mipi_configure(struct ov5648_sensor *sensor)
+{
+	struct v4l2_mbus_config_mipi_csi2 *bus_mipi_csi2 =
+		&sensor->endpoint.bus.mipi_csi2;
+	unsigned int lanes_count = bus_mipi_csi2->num_data_lanes;
+	int ret;
+
+	ret = ov5648_write(sensor, OV5648_MIPI_CTRL0_REG,
+			   OV5648_MIPI_CTRL0_CLK_LANE_AUTOGATE |
+			   OV5648_MIPI_CTRL0_LANE_SELECT_LANE1 |
+			   OV5648_MIPI_CTRL0_IDLE_LP11);
+	if (ret)
+		return ret;
+
+	return ov5648_write(sensor, OV5648_MIPI_SC_CTRL0_REG,
+			    OV5648_MIPI_SC_CTRL0_MIPI_LANES(lanes_count) |
+			    OV5648_MIPI_SC_CTRL0_PHY_LP_RX_PD |
+			    OV5648_MIPI_SC_CTRL0_MIPI_EN);
+}
+
+static int ov5648_black_level_configure(struct ov5648_sensor *sensor)
+{
+	int ret;
+
+	/* Up to 6 lines are available for black level calibration. */
+
+	ret = ov5648_write(sensor, OV5648_BLC_CTRL1_REG,
+			   OV5648_BLC_CTRL1_START_LINE(2));
+	if (ret)
+		return ret;
+
+	ret = ov5648_write(sensor, OV5648_BLC_CTRL2_REG,
+			   OV5648_BLC_CTRL2_AUTO_EN |
+			   OV5648_BLC_CTRL2_RESET_FRAME_NUM(5));
+	if (ret)
+		return ret;
+
+	ret = ov5648_write(sensor, OV5648_BLC_LINE_NUM_REG,
+			   OV5648_BLC_LINE_NUM(4));
+	if (ret)
+		return ret;
+
+	return ov5648_update_bits(sensor, OV5648_BLC_CTRL5_REG,
+				  OV5648_BLC_CTRL5_UPDATE_EN,
+				  OV5648_BLC_CTRL5_UPDATE_EN);
+}
+
+static int ov5648_isp_configure(struct ov5648_sensor *sensor)
+{
+	u8 bits;
+	int ret;
+
+	/* Enable black and white level correction. */
+	bits = OV5648_ISP_CTRL0_BLACK_CORRECT_EN |
+	       OV5648_ISP_CTRL0_WHITE_CORRECT_EN;
+
+	ret = ov5648_update_bits(sensor, OV5648_ISP_CTRL0_REG, bits, bits);
+	if (ret)
+		return ret;
+
+	/* Enable AWB. */
+	ret = ov5648_write(sensor, OV5648_ISP_CTRL1_REG,
+			   OV5648_ISP_CTRL1_AWB_EN);
+	if (ret)
+		return ret;
+
+	/* Enable AWB gain and windowing. */
+	ret = ov5648_write(sensor, OV5648_ISP_CTRL2_REG,
+			   OV5648_ISP_CTRL2_WIN_EN |
+			   OV5648_ISP_CTRL2_AWB_GAIN_EN);
+	if (ret)
+		return ret;
+
+	/* Enable buffering and auto-binning. */
+	ret = ov5648_write(sensor, OV5648_ISP_CTRL3_REG,
+			   OV5648_ISP_CTRL3_BUF_EN |
+			   OV5648_ISP_CTRL3_BIN_AUTO_EN);
+	if (ret)
+		return ret;
+
+	ret = ov5648_write(sensor, OV5648_ISP_CTRL4_REG, 0);
+	if (ret)
+		return ret;
+
+	ret = ov5648_write(sensor, OV5648_ISP_CTRL1F_REG,
+			   OV5648_ISP_CTRL1F_OUTPUT_EN);
+	if (ret)
+		return ret;
+
+	/* Enable post-binning filters. */
+	ret = ov5648_write(sensor, OV5648_ISP_CTRL4B_REG,
+			   OV5648_ISP_CTRL4B_POST_BIN_H_EN |
+			   OV5648_ISP_CTRL4B_POST_BIN_V_EN);
+	if (ret)
+		return ret;
+
+	/* Disable debanding and night mode. Debug bit seems necessary. */
+	ret = ov5648_write(sensor, OV5648_AEC_CTRL0_REG,
+			   OV5648_AEC_CTRL0_DEBUG |
+			   OV5648_AEC_CTRL0_START_SEL_EN);
+	if (ret)
+		return ret;
+
+	return ov5648_write(sensor, OV5648_MANUAL_CTRL_REG,
+			    OV5648_MANUAL_CTRL_FRAME_DELAY(1));
+}
+
+static unsigned long ov5648_mode_pll1_rate(struct ov5648_sensor *sensor,
+					   const struct ov5648_pll1_config *config)
+{
+	unsigned long xvclk_rate;
+	unsigned long pll1_rate;
+
+	xvclk_rate = clk_get_rate(sensor->xvclk);
+	pll1_rate = xvclk_rate * config->pll_mul;
+
+	switch (config->pll_pre_div) {
+	case 5:
+		pll1_rate *= 3;
+		pll1_rate /= 2;
+		break;
+	case 7:
+		pll1_rate *= 5;
+		pll1_rate /= 2;
+		break;
+	default:
+		pll1_rate /= config->pll_pre_div;
+		break;
+	}
+
+	return pll1_rate;
+}
+
+static int ov5648_mode_pll1_configure(struct ov5648_sensor *sensor,
+				      const struct ov5648_mode *mode,
+				      u32 mbus_code)
+{
+	const struct ov5648_pll1_config *config;
+	u8 value;
+	int ret;
+
+	value = OV5648_PLL_CTRL0_PLL_CHARGE_PUMP(1);
+
+	switch (mbus_code) {
+	case MEDIA_BUS_FMT_SBGGR8_1X8:
+		config = mode->pll1_config[0];
+		value |= OV5648_PLL_CTRL0_BITS(8);
+		break;
+	case MEDIA_BUS_FMT_SBGGR10_1X10:
+		config = mode->pll1_config[1];
+		value |= OV5648_PLL_CTRL0_BITS(10);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	ret = ov5648_write(sensor, OV5648_PLL_CTRL0_REG, value);
+	if (ret)
+		return ret;
+
+	ret = ov5648_write(sensor, OV5648_PLL_DIV_REG,
+			   OV5648_PLL_DIV_ROOT_DIV(config->root_div) |
+			   OV5648_PLL_DIV_PLL_PRE_DIV(config->pll_pre_div));
+	if (ret)
+		return ret;
+
+	ret = ov5648_write(sensor, OV5648_PLL_MUL_REG,
+			   OV5648_PLL_MUL(config->pll_mul));
+	if (ret)
+		return ret;
+
+	ret = ov5648_write(sensor, OV5648_PLL_CTRL1_REG,
+			   OV5648_PLL_CTRL1_SYS_DIV(config->sys_div) |
+			   OV5648_PLL_CTRL1_MIPI_DIV(config->mipi_div));
+	if (ret)
+		return ret;
+
+	return ov5648_write(sensor, OV5648_SRB_CTRL_REG,
+			    OV5648_SRB_CTRL_SCLK_DIV(config->sclk_div) |
+			    OV5648_SRB_CTRL_SCLK_ARBITER_EN);
+}
+
+static int ov5648_mode_pll2_configure(struct ov5648_sensor *sensor,
+				      const struct ov5648_mode *mode)
+{
+	const struct ov5648_pll2_config *config = mode->pll2_config;
+	int ret;
+
+	ret = ov5648_write(sensor, OV5648_PLLS_DIV_REG,
+			   OV5648_PLLS_DIV_PLLS_PRE_DIV(config->plls_pre_div) |
+			   OV5648_PLLS_DIV_PLLS_DIV_R(config->plls_div_r) |
+			   OV5648_PLLS_DIV_PLLS_SEL_DIV(config->sel_div));
+	if (ret)
+		return ret;
+
+	ret = ov5648_write(sensor, OV5648_PLLS_MUL_REG,
+			   OV5648_PLLS_MUL(config->plls_mul));
+	if (ret)
+		return ret;
+
+	return ov5648_write(sensor, OV5648_PLLS_CTRL_REG,
+			    OV5648_PLLS_CTRL_PLL_CHARGE_PUMP(1) |
+			    OV5648_PLLS_CTRL_SYS_DIV(config->sys_div));
+}
+
+static int ov5648_mode_configure(struct ov5648_sensor *sensor,
+				 const struct ov5648_mode *mode, u32 mbus_code)
+{
+	int ret;
+
+	/* Crop Start X */
+
+	ret = ov5648_write(sensor, OV5648_CROP_START_X_H_REG,
+			   OV5648_CROP_START_X_H(mode->crop_start_x));
+	if (ret)
+		return ret;
+
+	ret = ov5648_write(sensor, OV5648_CROP_START_X_L_REG,
+			   OV5648_CROP_START_X_L(mode->crop_start_x));
+	if (ret)
+		return ret;
+
+	/* Offset X */
+
+	ret = ov5648_write(sensor, OV5648_OFFSET_X_H_REG,
+			   OV5648_OFFSET_X_H(mode->offset_x));
+	if (ret)
+		return ret;
+
+	ret = ov5648_write(sensor, OV5648_OFFSET_X_L_REG,
+			   OV5648_OFFSET_X_L(mode->offset_x));
+	if (ret)
+		return ret;
+
+	/* Output Size X */
+
+	ret = ov5648_write(sensor, OV5648_OUTPUT_SIZE_X_H_REG,
+			   OV5648_OUTPUT_SIZE_X_H(mode->output_size_x));
+	if (ret)
+		return ret;
+
+	ret = ov5648_write(sensor, OV5648_OUTPUT_SIZE_X_L_REG,
+			   OV5648_OUTPUT_SIZE_X_L(mode->output_size_x));
+	if (ret)
+		return ret;
+
+	/* Crop End X */
+
+	ret = ov5648_write(sensor, OV5648_CROP_END_X_H_REG,
+			   OV5648_CROP_END_X_H(mode->crop_end_x));
+	if (ret)
+		return ret;
+
+	ret = ov5648_write(sensor, OV5648_CROP_END_X_L_REG,
+			   OV5648_CROP_END_X_L(mode->crop_end_x));
+	if (ret)
+		return ret;
+
+	/* Horizontal Total Size */
+
+	ret = ov5648_write(sensor, OV5648_HTS_H_REG, OV5648_HTS_H(mode->hts));
+	if (ret)
+		return ret;
+
+	ret = ov5648_write(sensor, OV5648_HTS_L_REG, OV5648_HTS_L(mode->hts));
+	if (ret)
+		return ret;
+
+	/* Crop Start Y */
+
+	ret = ov5648_write(sensor, OV5648_CROP_START_Y_H_REG,
+			   OV5648_CROP_START_Y_H(mode->crop_start_y));
+	if (ret)
+		return ret;
+
+	ret = ov5648_write(sensor, OV5648_CROP_START_Y_L_REG,
+			   OV5648_CROP_START_Y_L(mode->crop_start_y));
+	if (ret)
+		return ret;
+
+	/* Offset Y */
+
+	ret = ov5648_write(sensor, OV5648_OFFSET_Y_H_REG,
+			   OV5648_OFFSET_Y_H(mode->offset_y));
+	if (ret)
+		return ret;
+
+	ret = ov5648_write(sensor, OV5648_OFFSET_Y_L_REG,
+			   OV5648_OFFSET_Y_L(mode->offset_y));
+	if (ret)
+		return ret;
+
+	/* Output Size Y */
+
+	ret = ov5648_write(sensor, OV5648_OUTPUT_SIZE_Y_H_REG,
+			   OV5648_OUTPUT_SIZE_Y_H(mode->output_size_y));
+	if (ret)
+		return ret;
+
+	ret = ov5648_write(sensor, OV5648_OUTPUT_SIZE_Y_L_REG,
+			   OV5648_OUTPUT_SIZE_Y_L(mode->output_size_y));
+	if (ret)
+		return ret;
+
+	/* Crop End Y */
+
+	ret = ov5648_write(sensor, OV5648_CROP_END_Y_H_REG,
+			   OV5648_CROP_END_Y_H(mode->crop_end_y));
+	if (ret)
+		return ret;
+
+	ret = ov5648_write(sensor, OV5648_CROP_END_Y_L_REG,
+			   OV5648_CROP_END_Y_L(mode->crop_end_y));
+	if (ret)
+		return ret;
+
+	/* Vertical Total Size */
+
+	ret = ov5648_write(sensor, OV5648_VTS_H_REG, OV5648_VTS_H(mode->vts));
+	if (ret)
+		return ret;
+
+	ret = ov5648_write(sensor, OV5648_VTS_L_REG, OV5648_VTS_L(mode->vts));
+	if (ret)
+		return ret;
+
+	/* Flip/Mirror/Binning */
+
+	/*
+	 * A debug bit is enabled by default and needs to be cleared for
+	 * subsampling to work.
+	 */
+	ret = ov5648_update_bits(sensor, OV5648_TC20_REG,
+				 OV5648_TC20_DEBUG |
+				 OV5648_TC20_BINNING_VERT_EN,
+				 mode->binning_y ? OV5648_TC20_BINNING_VERT_EN :
+				 0);
+	if (ret)
+		return ret;
+
+	ret = ov5648_update_bits(sensor, OV5648_TC21_REG,
+				 OV5648_TC21_BINNING_HORZ_EN,
+				 mode->binning_x ? OV5648_TC21_BINNING_HORZ_EN :
+				 0);
+	if (ret)
+		return ret;
+
+	ret = ov5648_write(sensor, OV5648_SUB_INC_X_REG,
+			   OV5648_SUB_INC_X_ODD(mode->inc_x_odd) |
+			   OV5648_SUB_INC_X_EVEN(mode->inc_x_even));
+	if (ret)
+		return ret;
+
+	ret = ov5648_write(sensor, OV5648_SUB_INC_Y_REG,
+			   OV5648_SUB_INC_Y_ODD(mode->inc_y_odd) |
+			   OV5648_SUB_INC_Y_EVEN(mode->inc_y_even));
+	if (ret)
+		return ret;
+
+	/* PLLs */
+
+	ret = ov5648_mode_pll1_configure(sensor, mode, mbus_code);
+	if (ret)
+		return ret;
+
+	ret = ov5648_mode_pll2_configure(sensor, mode);
+	if (ret)
+		return ret;
+
+	/* Extra registers */
+
+	if (mode->register_values) {
+		ret = ov5648_write_sequence(sensor, mode->register_values,
+					    mode->register_values_count);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static unsigned long ov5648_mode_mipi_clk_rate(struct ov5648_sensor *sensor,
+					       const struct ov5648_mode *mode,
+					       u32 mbus_code)
+{
+	const struct ov5648_pll1_config *config;
+	unsigned long pll1_rate;
+
+	switch (mbus_code) {
+	case MEDIA_BUS_FMT_SBGGR8_1X8:
+		config = mode->pll1_config[0];
+		break;
+	case MEDIA_BUS_FMT_SBGGR10_1X10:
+		config = mode->pll1_config[1];
+		break;
+	default:
+		return 0;
+	}
+
+	pll1_rate = ov5648_mode_pll1_rate(sensor, config);
+
+	return pll1_rate / config->sys_div / config->mipi_div / 2;
+}
+
+/* Exposure */
+
+static int ov5648_exposure_auto_configure(struct ov5648_sensor *sensor,
+					  bool enable)
+{
+	return ov5648_update_bits(sensor, OV5648_MANUAL_CTRL_REG,
+				  OV5648_MANUAL_CTRL_AEC_MANUAL_EN,
+				  enable ? 0 : OV5648_MANUAL_CTRL_AEC_MANUAL_EN);
+}
+
+static int ov5648_exposure_configure(struct ov5648_sensor *sensor, u32 exposure)
+{
+	struct ov5648_ctrls *ctrls = &sensor->ctrls;
+	int ret;
+
+	if (ctrls->exposure_auto->val != V4L2_EXPOSURE_MANUAL)
+		return -EINVAL;
+
+	ret = ov5648_write(sensor, OV5648_EXPOSURE_CTRL_HH_REG,
+			   OV5648_EXPOSURE_CTRL_HH(exposure));
+	if (ret)
+		return ret;
+
+	ret = ov5648_write(sensor, OV5648_EXPOSURE_CTRL_H_REG,
+			   OV5648_EXPOSURE_CTRL_H(exposure));
+	if (ret)
+		return ret;
+
+	return ov5648_write(sensor, OV5648_EXPOSURE_CTRL_L_REG,
+			    OV5648_EXPOSURE_CTRL_L(exposure));
+}
+
+static int ov5648_exposure_value(struct ov5648_sensor *sensor,
+				 u32 *exposure)
+{
+	u8 exposure_hh = 0, exposure_h = 0, exposure_l = 0;
+	int ret;
+
+	ret = ov5648_read(sensor, OV5648_EXPOSURE_CTRL_HH_REG, &exposure_hh);
+	if (ret)
+		return ret;
+
+	ret = ov5648_read(sensor, OV5648_EXPOSURE_CTRL_H_REG, &exposure_h);
+	if (ret)
+		return ret;
+
+	ret = ov5648_read(sensor, OV5648_EXPOSURE_CTRL_L_REG, &exposure_l);
+	if (ret)
+		return ret;
+
+	*exposure = OV5648_EXPOSURE_CTRL_HH_VALUE((u32)exposure_hh) |
+		    OV5648_EXPOSURE_CTRL_H_VALUE((u32)exposure_h) |
+		    OV5648_EXPOSURE_CTRL_L_VALUE((u32)exposure_l);
+
+	return 0;
+}
+
+/* Gain */
+
+static int ov5648_gain_auto_configure(struct ov5648_sensor *sensor, bool enable)
+{
+	return ov5648_update_bits(sensor, OV5648_MANUAL_CTRL_REG,
+				  OV5648_MANUAL_CTRL_AGC_MANUAL_EN,
+				  enable ? 0 : OV5648_MANUAL_CTRL_AGC_MANUAL_EN);
+}
+
+static int ov5648_gain_configure(struct ov5648_sensor *sensor, u32 gain)
+{
+	struct ov5648_ctrls *ctrls = &sensor->ctrls;
+	int ret;
+
+	if (ctrls->gain_auto->val)
+		return -EINVAL;
+
+	ret = ov5648_write(sensor, OV5648_GAIN_CTRL_H_REG,
+			   OV5648_GAIN_CTRL_H(gain));
+	if (ret)
+		return ret;
+
+	return ov5648_write(sensor, OV5648_GAIN_CTRL_L_REG,
+			    OV5648_GAIN_CTRL_L(gain));
+}
+
+static int ov5648_gain_value(struct ov5648_sensor *sensor, u32 *gain)
+{
+	u8 gain_h = 0, gain_l = 0;
+	int ret;
+
+	ret = ov5648_read(sensor, OV5648_GAIN_CTRL_H_REG, &gain_h);
+	if (ret)
+		return ret;
+
+	ret = ov5648_read(sensor, OV5648_GAIN_CTRL_L_REG, &gain_l);
+	if (ret)
+		return ret;
+
+	*gain = OV5648_GAIN_CTRL_H_VALUE((u32)gain_h) |
+		OV5648_GAIN_CTRL_L_VALUE((u32)gain_l);
+
+	return 0;
+}
+
+/* White Balance */
+
+static int ov5648_white_balance_auto_configure(struct ov5648_sensor *sensor,
+					       bool enable)
+{
+	return ov5648_write(sensor, OV5648_AWB_CTRL_REG,
+			    enable ? 0 : OV5648_AWB_CTRL_GAIN_MANUAL_EN);
+}
+
+static int ov5648_white_balance_configure(struct ov5648_sensor *sensor,
+					  u32 red_balance, u32 blue_balance)
+{
+	struct ov5648_ctrls *ctrls = &sensor->ctrls;
+	int ret;
+
+	if (ctrls->white_balance_auto->val)
+		return -EINVAL;
+
+	ret = ov5648_write(sensor, OV5648_GAIN_RED_MAN_H_REG,
+			   OV5648_GAIN_RED_MAN_H(red_balance));
+	if (ret)
+		return ret;
+
+	ret = ov5648_write(sensor, OV5648_GAIN_RED_MAN_L_REG,
+			   OV5648_GAIN_RED_MAN_L(red_balance));
+	if (ret)
+		return ret;
+
+	ret = ov5648_write(sensor, OV5648_GAIN_BLUE_MAN_H_REG,
+			   OV5648_GAIN_BLUE_MAN_H(blue_balance));
+	if (ret)
+		return ret;
+
+	return ov5648_write(sensor, OV5648_GAIN_BLUE_MAN_L_REG,
+			    OV5648_GAIN_BLUE_MAN_L(blue_balance));
+}
+
+/* Flip */
+
+static int ov5648_flip_vert_configure(struct ov5648_sensor *sensor, bool enable)
+{
+	u8 bits = OV5648_TC20_FLIP_VERT_ISP_EN |
+		  OV5648_TC20_FLIP_VERT_SENSOR_EN;
+
+	return ov5648_update_bits(sensor, OV5648_TC20_REG, bits,
+				  enable ? bits : 0);
+}
+
+static int ov5648_flip_horz_configure(struct ov5648_sensor *sensor, bool enable)
+{
+	u8 bits = OV5648_TC21_FLIP_HORZ_ISP_EN |
+		  OV5648_TC21_FLIP_HORZ_SENSOR_EN;
+
+	return ov5648_update_bits(sensor, OV5648_TC21_REG, bits,
+				  enable ? bits : 0);
+}
+
+/* Test Pattern */
+
+static int ov5648_test_pattern_configure(struct ov5648_sensor *sensor,
+					 unsigned int index)
+{
+	if (index >= ARRAY_SIZE(ov5648_test_pattern_bits))
+		return -EINVAL;
+
+	return ov5648_write(sensor, OV5648_ISP_CTRL3D_REG,
+			    ov5648_test_pattern_bits[index]);
+}
+
+/* State */
+
+static int ov5648_state_mipi_configure(struct ov5648_sensor *sensor,
+				       const struct ov5648_mode *mode,
+				       u32 mbus_code)
+{
+	struct ov5648_ctrls *ctrls = &sensor->ctrls;
+	struct v4l2_mbus_config_mipi_csi2 *bus_mipi_csi2 =
+		&sensor->endpoint.bus.mipi_csi2;
+	unsigned long mipi_clk_rate;
+	unsigned int bits_per_sample;
+	unsigned int lanes_count;
+	unsigned int i, j;
+	s64 mipi_pixel_rate;
+
+	mipi_clk_rate = ov5648_mode_mipi_clk_rate(sensor, mode, mbus_code);
+	if (!mipi_clk_rate)
+		return -EINVAL;
+
+	for (i = 0; i < ARRAY_SIZE(ov5648_link_freq_menu); i++) {
+		s64 freq = ov5648_link_freq_menu[i];
+
+		if (freq == mipi_clk_rate)
+			break;
+	}
+
+	for (j = 0; j < sensor->endpoint.nr_of_link_frequencies; j++) {
+		u64 freq = sensor->endpoint.link_frequencies[j];
+
+		if (freq == mipi_clk_rate)
+			break;
+	}
+
+	if (i == ARRAY_SIZE(ov5648_link_freq_menu)) {
+		dev_err(sensor->dev,
+			"failed to find %lu clk rate in link freq\n",
+			mipi_clk_rate);
+	} else if (j == sensor->endpoint.nr_of_link_frequencies) {
+		dev_err(sensor->dev,
+			"failed to find %lu clk rate in endpoint link-frequencies\n",
+			mipi_clk_rate);
+	} else {
+		__v4l2_ctrl_s_ctrl(ctrls->link_freq, i);
+	}
+
+	switch (mbus_code) {
+	case MEDIA_BUS_FMT_SBGGR8_1X8:
+		bits_per_sample = 8;
+		break;
+	case MEDIA_BUS_FMT_SBGGR10_1X10:
+		bits_per_sample = 10;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	lanes_count = bus_mipi_csi2->num_data_lanes;
+	mipi_pixel_rate = mipi_clk_rate * 2 * lanes_count / bits_per_sample;
+
+	__v4l2_ctrl_s_ctrl_int64(ctrls->pixel_rate, mipi_pixel_rate);
+
+	return 0;
+}
+
+static int ov5648_state_configure(struct ov5648_sensor *sensor,
+				  const struct ov5648_mode *mode,
+				  u32 mbus_code)
+{
+	int ret;
+
+	if (sensor->state.streaming)
+		return -EBUSY;
+
+	/* State will be configured at first power on otherwise. */
+	if (pm_runtime_enabled(sensor->dev) &&
+	    !pm_runtime_suspended(sensor->dev)) {
+		ret = ov5648_mode_configure(sensor, mode, mbus_code);
+		if (ret)
+			return ret;
+	}
+
+	ret = ov5648_state_mipi_configure(sensor, mode, mbus_code);
+	if (ret)
+		return ret;
+
+	sensor->state.mode = mode;
+	sensor->state.mbus_code = mbus_code;
+
+	return 0;
+}
+
+static int ov5648_state_init(struct ov5648_sensor *sensor)
+{
+	int ret;
+
+	mutex_lock(&sensor->mutex);
+	ret = ov5648_state_configure(sensor, &ov5648_modes[0],
+				     ov5648_mbus_codes[0]);
+	mutex_unlock(&sensor->mutex);
+
+	return ret;
+}
+
+/* Sensor Base */
+
+static int ov5648_sensor_init(struct ov5648_sensor *sensor)
+{
+	int ret;
+
+	ret = ov5648_sw_reset(sensor);
+	if (ret) {
+		dev_err(sensor->dev, "failed to perform sw reset\n");
+		return ret;
+	}
+
+	ret = ov5648_sw_standby(sensor, 1);
+	if (ret) {
+		dev_err(sensor->dev, "failed to set sensor standby\n");
+		return ret;
+	}
+
+	ret = ov5648_chip_id_check(sensor);
+	if (ret) {
+		dev_err(sensor->dev, "failed to check sensor chip id\n");
+		return ret;
+	}
+
+	ret = ov5648_avdd_internal_power(sensor, !sensor->avdd);
+	if (ret) {
+		dev_err(sensor->dev, "failed to set internal avdd power\n");
+		return ret;
+	}
+
+	ret = ov5648_write_sequence(sensor, ov5648_init_sequence,
+				    ARRAY_SIZE(ov5648_init_sequence));
+	if (ret) {
+		dev_err(sensor->dev, "failed to write init sequence\n");
+		return ret;
+	}
+
+	ret = ov5648_pad_configure(sensor);
+	if (ret) {
+		dev_err(sensor->dev, "failed to configure pad\n");
+		return ret;
+	}
+
+	ret = ov5648_mipi_configure(sensor);
+	if (ret) {
+		dev_err(sensor->dev, "failed to configure MIPI\n");
+		return ret;
+	}
+
+	ret = ov5648_isp_configure(sensor);
+	if (ret) {
+		dev_err(sensor->dev, "failed to configure ISP\n");
+		return ret;
+	}
+
+	ret = ov5648_black_level_configure(sensor);
+	if (ret) {
+		dev_err(sensor->dev, "failed to configure black level\n");
+		return ret;
+	}
+
+	/* Configure current mode. */
+	ret = ov5648_state_configure(sensor, sensor->state.mode,
+				     sensor->state.mbus_code);
+	if (ret) {
+		dev_err(sensor->dev, "failed to configure state\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ov5648_sensor_power(struct ov5648_sensor *sensor, bool on)
+{
+	/* Keep initialized to zero for disable label. */
+	int ret = 0;
+
+	/*
+	 * General notes about the power sequence:
+	 * - power-down GPIO must be active (low) during power-on;
+	 * - reset GPIO state does not matter during power-on;
+	 * - XVCLK must be provided 1 ms before register access;
+	 * - 10 ms are needed between power-down deassert and register access.
+	 */
+
+	/* Note that regulator-and-GPIO-based power is untested. */
+	if (on) {
+		gpiod_set_value_cansleep(sensor->reset, 1);
+		gpiod_set_value_cansleep(sensor->powerdown, 1);
+
+		ret = regulator_enable(sensor->dovdd);
+		if (ret) {
+			dev_err(sensor->dev,
+				"failed to enable DOVDD regulator\n");
+			goto disable;
+		}
+
+		if (sensor->avdd) {
+			ret = regulator_enable(sensor->avdd);
+			if (ret) {
+				dev_err(sensor->dev,
+					"failed to enable AVDD regulator\n");
+				goto disable;
+			}
+		}
+
+		ret = regulator_enable(sensor->dvdd);
+		if (ret) {
+			dev_err(sensor->dev,
+				"failed to enable DVDD regulator\n");
+			goto disable;
+		}
+
+		/* According to OV5648 power up diagram. */
+		usleep_range(5000, 10000);
+
+		ret = clk_prepare_enable(sensor->xvclk);
+		if (ret) {
+			dev_err(sensor->dev, "failed to enable XVCLK clock\n");
+			goto disable;
+		}
+
+		gpiod_set_value_cansleep(sensor->reset, 0);
+		gpiod_set_value_cansleep(sensor->powerdown, 0);
+
+		usleep_range(20000, 25000);
+	} else {
+disable:
+		gpiod_set_value_cansleep(sensor->powerdown, 1);
+		gpiod_set_value_cansleep(sensor->reset, 1);
+
+		clk_disable_unprepare(sensor->xvclk);
+
+		regulator_disable(sensor->dvdd);
+
+		if (sensor->avdd)
+			regulator_disable(sensor->avdd);
+
+		regulator_disable(sensor->dovdd);
+	}
+
+	return ret;
+}
+
+/* Controls */
+
+static int ov5648_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *subdev = ov5648_ctrl_subdev(ctrl);
+	struct ov5648_sensor *sensor = ov5648_subdev_sensor(subdev);
+	struct ov5648_ctrls *ctrls = &sensor->ctrls;
+	int ret;
+
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE_AUTO:
+		ret = ov5648_exposure_value(sensor, &ctrls->exposure->val);
+		if (ret)
+			return ret;
+		break;
+	case V4L2_CID_AUTOGAIN:
+		ret = ov5648_gain_value(sensor, &ctrls->gain->val);
+		if (ret)
+			return ret;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int ov5648_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *subdev = ov5648_ctrl_subdev(ctrl);
+	struct ov5648_sensor *sensor = ov5648_subdev_sensor(subdev);
+	struct ov5648_ctrls *ctrls = &sensor->ctrls;
+	unsigned int index;
+	bool enable;
+	int ret;
+
+	/* Wait for the sensor to be on before setting controls. */
+	if (pm_runtime_suspended(sensor->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE_AUTO:
+		enable = ctrl->val == V4L2_EXPOSURE_AUTO;
+
+		ret = ov5648_exposure_auto_configure(sensor, enable);
+		if (ret)
+			return ret;
+
+		if (!enable && ctrls->exposure->is_new) {
+			ret = ov5648_exposure_configure(sensor,
+							ctrls->exposure->val);
+			if (ret)
+				return ret;
+		}
+		break;
+	case V4L2_CID_AUTOGAIN:
+		enable = !!ctrl->val;
+
+		ret = ov5648_gain_auto_configure(sensor, enable);
+		if (ret)
+			return ret;
+
+		if (!enable) {
+			ret = ov5648_gain_configure(sensor, ctrls->gain->val);
+			if (ret)
+				return ret;
+		}
+		break;
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		enable = !!ctrl->val;
+
+		ret = ov5648_white_balance_auto_configure(sensor, enable);
+		if (ret)
+			return ret;
+
+		if (!enable) {
+			ret = ov5648_white_balance_configure(sensor,
+							     ctrls->red_balance->val,
+							     ctrls->blue_balance->val);
+			if (ret)
+				return ret;
+		}
+		break;
+	case V4L2_CID_HFLIP:
+		enable = !!ctrl->val;
+		return ov5648_flip_horz_configure(sensor, enable);
+	case V4L2_CID_VFLIP:
+		enable = !!ctrl->val;
+		return ov5648_flip_vert_configure(sensor, enable);
+	case V4L2_CID_TEST_PATTERN:
+		index = (unsigned int)ctrl->val;
+		return ov5648_test_pattern_configure(sensor, index);
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops ov5648_ctrl_ops = {
+	.g_volatile_ctrl	= ov5648_g_volatile_ctrl,
+	.s_ctrl			= ov5648_s_ctrl,
+};
+
+static int ov5648_ctrls_init(struct ov5648_sensor *sensor)
+{
+	struct ov5648_ctrls *ctrls = &sensor->ctrls;
+	struct v4l2_ctrl_handler *handler = &ctrls->handler;
+	const struct v4l2_ctrl_ops *ops = &ov5648_ctrl_ops;
+	int ret;
+
+	v4l2_ctrl_handler_init(handler, 32);
+
+	/* Use our mutex for ctrl locking. */
+	handler->lock = &sensor->mutex;
+
+	/* Exposure */
+
+	ctrls->exposure_auto = v4l2_ctrl_new_std_menu(handler, ops,
+						      V4L2_CID_EXPOSURE_AUTO,
+						      V4L2_EXPOSURE_MANUAL, 0,
+						      V4L2_EXPOSURE_AUTO);
+
+	ctrls->exposure = v4l2_ctrl_new_std(handler, ops, V4L2_CID_EXPOSURE,
+					    16, 1048575, 16, 512);
+
+	v4l2_ctrl_auto_cluster(2, &ctrls->exposure_auto, 1, true);
+
+	/* Gain */
+
+	ctrls->gain_auto =
+		v4l2_ctrl_new_std(handler, ops, V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+
+	ctrls->gain = v4l2_ctrl_new_std(handler, ops, V4L2_CID_GAIN, 16, 1023,
+					16, 16);
+
+	v4l2_ctrl_auto_cluster(2, &ctrls->gain_auto, 0, true);
+
+	/* White Balance */
+
+	ctrls->white_balance_auto =
+		v4l2_ctrl_new_std(handler, ops, V4L2_CID_AUTO_WHITE_BALANCE, 0,
+				  1, 1, 1);
+
+	ctrls->red_balance = v4l2_ctrl_new_std(handler, ops,
+					       V4L2_CID_RED_BALANCE, 0, 4095,
+					       1, 1024);
+
+	ctrls->blue_balance = v4l2_ctrl_new_std(handler, ops,
+						V4L2_CID_BLUE_BALANCE, 0, 4095,
+						1, 1024);
+
+	v4l2_ctrl_auto_cluster(3, &ctrls->white_balance_auto, 0, false);
+
+	/* Flip */
+
+	v4l2_ctrl_new_std(handler, ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(handler, ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
+
+	/* Test Pattern */
+
+	v4l2_ctrl_new_std_menu_items(handler, ops, V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(ov5648_test_pattern_menu) - 1,
+				     0, 0, ov5648_test_pattern_menu);
+
+	/* MIPI CSI-2 */
+
+	ctrls->link_freq =
+		v4l2_ctrl_new_int_menu(handler, NULL, V4L2_CID_LINK_FREQ,
+				       ARRAY_SIZE(ov5648_link_freq_menu) - 1,
+				       0, ov5648_link_freq_menu);
+
+	ctrls->pixel_rate =
+		v4l2_ctrl_new_std(handler, NULL, V4L2_CID_PIXEL_RATE, 1,
+				  INT_MAX, 1, 1);
+
+	if (handler->error) {
+		ret = handler->error;
+		goto error_ctrls;
+	}
+
+	ctrls->exposure->flags |= V4L2_CTRL_FLAG_VOLATILE;
+	ctrls->gain->flags |= V4L2_CTRL_FLAG_VOLATILE;
+
+	ctrls->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+	ctrls->pixel_rate->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	sensor->subdev.ctrl_handler = handler;
+
+	return 0;
+
+error_ctrls:
+	v4l2_ctrl_handler_free(handler);
+
+	return ret;
+}
+
+/* Subdev Video Operations */
+
+static int ov5648_s_stream(struct v4l2_subdev *subdev, int enable)
+{
+	struct ov5648_sensor *sensor = ov5648_subdev_sensor(subdev);
+	struct ov5648_state *state = &sensor->state;
+	int ret;
+
+	if (enable) {
+		ret = pm_runtime_resume_and_get(sensor->dev);
+		if (ret < 0)
+			return ret;
+	}
+
+	mutex_lock(&sensor->mutex);
+	ret = ov5648_sw_standby(sensor, !enable);
+	mutex_unlock(&sensor->mutex);
+
+	if (ret)
+		return ret;
+
+	state->streaming = !!enable;
+
+	if (!enable)
+		pm_runtime_put(sensor->dev);
+
+	return 0;
+}
+
+static int ov5648_g_frame_interval(struct v4l2_subdev *subdev,
+				   struct v4l2_subdev_frame_interval *interval)
+{
+	struct ov5648_sensor *sensor = ov5648_subdev_sensor(subdev);
+	const struct ov5648_mode *mode;
+	int ret = 0;
+
+	mutex_lock(&sensor->mutex);
+
+	mode = sensor->state.mode;
+
+	switch (sensor->state.mbus_code) {
+	case MEDIA_BUS_FMT_SBGGR8_1X8:
+		interval->interval = mode->frame_interval[0];
+		break;
+	case MEDIA_BUS_FMT_SBGGR10_1X10:
+		interval->interval = mode->frame_interval[1];
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	mutex_unlock(&sensor->mutex);
+
+	return ret;
+}
+
+static const struct v4l2_subdev_video_ops ov5648_subdev_video_ops = {
+	.s_stream		= ov5648_s_stream,
+	.g_frame_interval	= ov5648_g_frame_interval,
+	.s_frame_interval	= ov5648_g_frame_interval,
+};
+
+/* Subdev Pad Operations */
+
+static int ov5648_enum_mbus_code(struct v4l2_subdev *subdev,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code_enum)
+{
+	if (code_enum->index >= ARRAY_SIZE(ov5648_mbus_codes))
+		return -EINVAL;
+
+	code_enum->code = ov5648_mbus_codes[code_enum->index];
+
+	return 0;
+}
+
+static void ov5648_mbus_format_fill(struct v4l2_mbus_framefmt *mbus_format,
+				    u32 mbus_code,
+				    const struct ov5648_mode *mode)
+{
+	mbus_format->width = mode->output_size_x;
+	mbus_format->height = mode->output_size_y;
+	mbus_format->code = mbus_code;
+
+	mbus_format->field = V4L2_FIELD_NONE;
+	mbus_format->colorspace = V4L2_COLORSPACE_RAW;
+	mbus_format->ycbcr_enc =
+		V4L2_MAP_YCBCR_ENC_DEFAULT(mbus_format->colorspace);
+	mbus_format->quantization = V4L2_QUANTIZATION_FULL_RANGE;
+	mbus_format->xfer_func =
+		V4L2_MAP_XFER_FUNC_DEFAULT(mbus_format->colorspace);
+}
+
+static int ov5648_get_fmt(struct v4l2_subdev *subdev,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *format)
+{
+	struct ov5648_sensor *sensor = ov5648_subdev_sensor(subdev);
+	struct v4l2_mbus_framefmt *mbus_format = &format->format;
+
+	mutex_lock(&sensor->mutex);
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+		*mbus_format = *v4l2_subdev_get_try_format(subdev, sd_state,
+							   format->pad);
+	else
+		ov5648_mbus_format_fill(mbus_format, sensor->state.mbus_code,
+					sensor->state.mode);
+
+	mutex_unlock(&sensor->mutex);
+
+	return 0;
+}
+
+static int ov5648_set_fmt(struct v4l2_subdev *subdev,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *format)
+{
+	struct ov5648_sensor *sensor = ov5648_subdev_sensor(subdev);
+	struct v4l2_mbus_framefmt *mbus_format = &format->format;
+	const struct ov5648_mode *mode;
+	u32 mbus_code = 0;
+	unsigned int index;
+	int ret = 0;
+
+	mutex_lock(&sensor->mutex);
+
+	if (sensor->state.streaming) {
+		ret = -EBUSY;
+		goto complete;
+	}
+
+	/* Try to find requested mbus code. */
+	for (index = 0; index < ARRAY_SIZE(ov5648_mbus_codes); index++) {
+		if (ov5648_mbus_codes[index] == mbus_format->code) {
+			mbus_code = mbus_format->code;
+			break;
+		}
+	}
+
+	/* Fallback to default. */
+	if (!mbus_code)
+		mbus_code = ov5648_mbus_codes[0];
+
+	/* Find the mode with nearest dimensions. */
+	mode = v4l2_find_nearest_size(ov5648_modes, ARRAY_SIZE(ov5648_modes),
+				      output_size_x, output_size_y,
+				      mbus_format->width, mbus_format->height);
+	if (!mode) {
+		ret = -EINVAL;
+		goto complete;
+	}
+
+	ov5648_mbus_format_fill(mbus_format, mbus_code, mode);
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+		*v4l2_subdev_get_try_format(subdev, sd_state, format->pad) =
+			*mbus_format;
+	else if (sensor->state.mode != mode ||
+		 sensor->state.mbus_code != mbus_code)
+		ret = ov5648_state_configure(sensor, mode, mbus_code);
+
+complete:
+	mutex_unlock(&sensor->mutex);
+
+	return ret;
+}
+
+static int ov5648_enum_frame_size(struct v4l2_subdev *subdev,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *size_enum)
+{
+	const struct ov5648_mode *mode;
+
+	if (size_enum->index >= ARRAY_SIZE(ov5648_modes))
+		return -EINVAL;
+
+	mode = &ov5648_modes[size_enum->index];
+
+	size_enum->min_width = size_enum->max_width = mode->output_size_x;
+	size_enum->min_height = size_enum->max_height = mode->output_size_y;
+
+	return 0;
+}
+
+static int ov5648_enum_frame_interval(struct v4l2_subdev *subdev,
+				      struct v4l2_subdev_state *sd_state,
+				      struct v4l2_subdev_frame_interval_enum *interval_enum)
+{
+	const struct ov5648_mode *mode = NULL;
+	unsigned int mode_index;
+	unsigned int interval_index;
+
+	if (interval_enum->index > 0)
+		return -EINVAL;
+
+	/*
+	 * Multiple modes with the same dimensions may have different frame
+	 * intervals, so look up each relevant mode.
+	 */
+	for (mode_index = 0, interval_index = 0;
+	     mode_index < ARRAY_SIZE(ov5648_modes); mode_index++) {
+		mode = &ov5648_modes[mode_index];
+
+		if (mode->output_size_x == interval_enum->width &&
+		    mode->output_size_y == interval_enum->height) {
+			if (interval_index == interval_enum->index)
+				break;
+
+			interval_index++;
+		}
+	}
+
+	if (mode_index == ARRAY_SIZE(ov5648_modes))
+		return -EINVAL;
+
+	switch (interval_enum->code) {
+	case MEDIA_BUS_FMT_SBGGR8_1X8:
+		interval_enum->interval = mode->frame_interval[0];
+		break;
+	case MEDIA_BUS_FMT_SBGGR10_1X10:
+		interval_enum->interval = mode->frame_interval[1];
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_subdev_pad_ops ov5648_subdev_pad_ops = {
+	.enum_mbus_code		= ov5648_enum_mbus_code,
+	.get_fmt		= ov5648_get_fmt,
+	.set_fmt		= ov5648_set_fmt,
+	.enum_frame_size	= ov5648_enum_frame_size,
+	.enum_frame_interval	= ov5648_enum_frame_interval,
+};
+
+static const struct v4l2_subdev_ops ov5648_subdev_ops = {
+	.video		= &ov5648_subdev_video_ops,
+	.pad		= &ov5648_subdev_pad_ops,
+};
+
+static int ov5648_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct ov5648_sensor *sensor = ov5648_subdev_sensor(subdev);
+	struct ov5648_state *state = &sensor->state;
+	int ret = 0;
+
+	mutex_lock(&sensor->mutex);
+
+	if (state->streaming) {
+		ret = ov5648_sw_standby(sensor, true);
+		if (ret)
+			goto complete;
+	}
+
+	ret = ov5648_sensor_power(sensor, false);
+	if (ret)
+		ov5648_sw_standby(sensor, false);
+
+complete:
+	mutex_unlock(&sensor->mutex);
+
+	return ret;
+}
+
+static int ov5648_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct ov5648_sensor *sensor = ov5648_subdev_sensor(subdev);
+	struct ov5648_state *state = &sensor->state;
+	int ret = 0;
+
+	mutex_lock(&sensor->mutex);
+
+	ret = ov5648_sensor_power(sensor, true);
+	if (ret)
+		goto complete;
+
+	ret = ov5648_sensor_init(sensor);
+	if (ret)
+		goto error_power;
+
+	ret = __v4l2_ctrl_handler_setup(&sensor->ctrls.handler);
+	if (ret)
+		goto error_power;
+
+	if (state->streaming) {
+		ret = ov5648_sw_standby(sensor, false);
+		if (ret)
+			goto error_power;
+	}
+
+	goto complete;
+
+error_power:
+	ov5648_sensor_power(sensor, false);
+
+complete:
+	mutex_unlock(&sensor->mutex);
+
+	return ret;
+}
+
+static int ov5648_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct fwnode_handle *handle;
+	struct ov5648_sensor *sensor;
+	struct v4l2_subdev *subdev;
+	struct media_pad *pad;
+	unsigned long rate;
+	int ret;
+
+	sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
+	if (!sensor)
+		return -ENOMEM;
+
+	sensor->dev = dev;
+	sensor->i2c_client = client;
+
+	/* Graph Endpoint */
+
+	handle = fwnode_graph_get_next_endpoint(dev_fwnode(dev), NULL);
+	if (!handle) {
+		dev_err(dev, "unable to find endpoint node\n");
+		return -EINVAL;
+	}
+
+	sensor->endpoint.bus_type = V4L2_MBUS_CSI2_DPHY;
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(handle, &sensor->endpoint);
+	fwnode_handle_put(handle);
+	if (ret) {
+		dev_err(dev, "failed to parse endpoint node\n");
+		return ret;
+	}
+
+	/* GPIOs */
+
+	sensor->powerdown = devm_gpiod_get_optional(dev, "powerdown",
+						    GPIOD_OUT_HIGH);
+	if (IS_ERR(sensor->powerdown)) {
+		ret = PTR_ERR(sensor->powerdown);
+		goto error_endpoint;
+	}
+
+	sensor->reset = devm_gpiod_get_optional(dev, "reset", GPIOD_OUT_HIGH);
+	if (IS_ERR(sensor->reset)) {
+		ret = PTR_ERR(sensor->reset);
+		goto error_endpoint;
+	}
+
+	/* Regulators */
+
+	/* DVDD: digital core */
+	sensor->dvdd = devm_regulator_get(dev, "dvdd");
+	if (IS_ERR(sensor->dvdd)) {
+		dev_err(dev, "cannot get DVDD (digital core) regulator\n");
+		ret = PTR_ERR(sensor->dvdd);
+		goto error_endpoint;
+	}
+
+	/* DOVDD: digital I/O */
+	sensor->dovdd = devm_regulator_get(dev, "dovdd");
+	if (IS_ERR(sensor->dovdd)) {
+		dev_err(dev, "cannot get DOVDD (digital I/O) regulator\n");
+		ret = PTR_ERR(sensor->dovdd);
+		goto error_endpoint;
+	}
+
+	/* AVDD: analog */
+	sensor->avdd = devm_regulator_get_optional(dev, "avdd");
+	if (IS_ERR(sensor->avdd)) {
+		dev_info(dev, "no AVDD regulator provided, using internal\n");
+		sensor->avdd = NULL;
+	}
+
+	/* External Clock */
+
+	sensor->xvclk = devm_clk_get(dev, NULL);
+	if (IS_ERR(sensor->xvclk)) {
+		dev_err(dev, "failed to get external clock\n");
+		ret = PTR_ERR(sensor->xvclk);
+		goto error_endpoint;
+	}
+
+	rate = clk_get_rate(sensor->xvclk);
+	if (rate != OV5648_XVCLK_RATE) {
+		dev_err(dev, "clock rate %lu Hz is unsupported\n", rate);
+		ret = -EINVAL;
+		goto error_endpoint;
+	}
+
+	/* Subdev, entity and pad */
+
+	subdev = &sensor->subdev;
+	v4l2_i2c_subdev_init(subdev, client, &ov5648_subdev_ops);
+
+	subdev->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	subdev->entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+	pad = &sensor->pad;
+	pad->flags = MEDIA_PAD_FL_SOURCE;
+
+	ret = media_entity_pads_init(&subdev->entity, 1, pad);
+	if (ret)
+		goto error_entity;
+
+	/* Mutex */
+
+	mutex_init(&sensor->mutex);
+
+	/* Sensor */
+
+	ret = ov5648_ctrls_init(sensor);
+	if (ret)
+		goto error_mutex;
+
+	ret = ov5648_state_init(sensor);
+	if (ret)
+		goto error_ctrls;
+
+	/* Runtime PM */
+
+	pm_runtime_enable(sensor->dev);
+	pm_runtime_set_suspended(sensor->dev);
+
+	/* V4L2 subdev register */
+
+	ret = v4l2_async_register_subdev_sensor(subdev);
+	if (ret)
+		goto error_pm;
+
+	return 0;
+
+error_pm:
+	pm_runtime_disable(sensor->dev);
+
+error_ctrls:
+	v4l2_ctrl_handler_free(&sensor->ctrls.handler);
+
+error_mutex:
+	mutex_destroy(&sensor->mutex);
+
+error_entity:
+	media_entity_cleanup(&sensor->subdev.entity);
+
+error_endpoint:
+	v4l2_fwnode_endpoint_free(&sensor->endpoint);
+
+	return ret;
+}
+
+static void ov5648_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct ov5648_sensor *sensor = ov5648_subdev_sensor(subdev);
+
+	v4l2_async_unregister_subdev(subdev);
+	pm_runtime_disable(sensor->dev);
+	v4l2_ctrl_handler_free(&sensor->ctrls.handler);
+	mutex_destroy(&sensor->mutex);
+	media_entity_cleanup(&subdev->entity);
+	v4l2_fwnode_endpoint_free(&sensor->endpoint);
+}
+
+static const struct dev_pm_ops ov5648_pm_ops = {
+	SET_RUNTIME_PM_OPS(ov5648_suspend, ov5648_resume, NULL)
+};
+
+static const struct of_device_id ov5648_of_match[] = {
+	{ .compatible = "ovti,ov5648" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, ov5648_of_match);
+
+static struct i2c_driver ov5648_driver = {
+	.driver = {
+		.name = "ov5648",
+		.of_match_table = ov5648_of_match,
+		.pm = &ov5648_pm_ops,
+	},
+	.probe = ov5648_probe,
+	.remove = ov5648_remove,
+};
+
+module_i2c_driver(ov5648_driver);
+
+MODULE_AUTHOR("Paul Kocialkowski <paul.kocialkowski@bootlin.com>");
+MODULE_DESCRIPTION("V4L2 driver for the OmniVision OV5648 image sensor");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/ov5670.c b/drivers/media/i2c/ov5670.c
new file mode 100644
index 000000000..29e773a99
--- /dev/null
+++ b/drivers/media/i2c/ov5670.c
@@ -0,0 +1,2866 @@
+// SPDX-License-Identifier: GPL-2.0
+// Copyright (c) 2017 Intel Corporation.
+
+#include <asm/unaligned.h>
+#include <linux/acpi.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/mod_devicetable.h>
+#include <linux/module.h>
+#include <linux/of.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>
+
+#define OV5670_XVCLK_FREQ		19200000
+
+#define OV5670_REG_CHIP_ID		0x300a
+#define OV5670_CHIP_ID			0x005670
+
+#define OV5670_REG_MODE_SELECT		0x0100
+#define OV5670_MODE_STANDBY		0x00
+#define OV5670_MODE_STREAMING		0x01
+
+#define OV5670_REG_SOFTWARE_RST		0x0103
+#define OV5670_SOFTWARE_RST		0x01
+
+#define OV5670_MIPI_SC_CTRL0_REG		0x3018
+#define OV5670_MIPI_SC_CTRL0_LANES(v)		((((v) - 1) << 5) & \
+						 GENMASK(7, 5))
+#define OV5670_MIPI_SC_CTRL0_MIPI_EN		BIT(4)
+#define OV5670_MIPI_SC_CTRL0_RESERVED		BIT(1)
+
+/* vertical-timings from sensor */
+#define OV5670_REG_VTS			0x380e
+#define OV5670_VTS_30FPS		0x0808 /* default for 30 fps */
+#define OV5670_VTS_MAX			0xffff
+
+/* horizontal-timings from sensor */
+#define OV5670_REG_HTS			0x380c
+
+/*
+ * Pixels-per-line(PPL) = Time-per-line * pixel-rate
+ * In OV5670, Time-per-line = HTS/SCLK.
+ * HTS is fixed for all resolutions, not recommended to change.
+ */
+#define OV5670_FIXED_PPL		2724	/* Pixels per line */
+
+/* Exposure controls from sensor */
+#define OV5670_REG_EXPOSURE		0x3500
+#define	OV5670_EXPOSURE_MIN		4
+#define	OV5670_EXPOSURE_STEP		1
+
+/* Analog gain controls from sensor */
+#define OV5670_REG_ANALOG_GAIN		0x3508
+#define	ANALOG_GAIN_MIN			0
+#define	ANALOG_GAIN_MAX			8191
+#define	ANALOG_GAIN_STEP		1
+#define	ANALOG_GAIN_DEFAULT		128
+
+/* Digital gain controls from sensor */
+#define OV5670_REG_R_DGTL_GAIN		0x5032
+#define OV5670_REG_G_DGTL_GAIN		0x5034
+#define OV5670_REG_B_DGTL_GAIN		0x5036
+#define OV5670_DGTL_GAIN_MIN		0
+#define OV5670_DGTL_GAIN_MAX		4095
+#define OV5670_DGTL_GAIN_STEP		1
+#define OV5670_DGTL_GAIN_DEFAULT	1024
+
+/* Test Pattern Control */
+#define OV5670_REG_TEST_PATTERN		0x4303
+#define OV5670_TEST_PATTERN_ENABLE	BIT(3)
+#define OV5670_REG_TEST_PATTERN_CTRL	0x4320
+
+#define OV5670_REG_VALUE_08BIT		1
+#define OV5670_REG_VALUE_16BIT		2
+#define OV5670_REG_VALUE_24BIT		3
+
+/* Pixel Array */
+#define OV5670_NATIVE_WIDTH		2624
+#define OV5670_NATIVE_HEIGHT		1980
+
+/* Initial number of frames to skip to avoid possible garbage */
+#define OV5670_NUM_OF_SKIP_FRAMES	2
+
+struct ov5670_reg {
+	u16 address;
+	u8 val;
+};
+
+struct ov5670_reg_list {
+	u32 num_of_regs;
+	const struct ov5670_reg *regs;
+};
+
+struct ov5670_link_freq_config {
+	const struct ov5670_reg_list reg_list;
+};
+
+static const char * const ov5670_supply_names[] = {
+	"avdd",		/* Analog power */
+	"dvdd",		/* Digital power */
+	"dovdd",	/* Digital output power */
+};
+
+#define OV5670_NUM_SUPPLIES ARRAY_SIZE(ov5670_supply_names)
+
+struct ov5670_mode {
+	/* Frame width in pixels */
+	u32 width;
+
+	/* Frame height in pixels */
+	u32 height;
+
+	/* Default vertical timining size */
+	u32 vts_def;
+
+	/* Min vertical timining size */
+	u32 vts_min;
+
+	/* Link frequency needed for this resolution */
+	u32 link_freq_index;
+
+	/* Analog crop rectangle */
+	const struct v4l2_rect *analog_crop;
+
+	/* Sensor register settings for this resolution */
+	const struct ov5670_reg_list reg_list;
+};
+
+/*
+ * All the modes supported by the driver are obtained by subsampling the
+ * full pixel array. The below values are reflected in registers from
+ * 0x3800-0x3807 in the modes register-value tables.
+ */
+static const struct v4l2_rect ov5670_analog_crop = {
+	.left	= 12,
+	.top	= 4,
+	.width	= 2600,
+	.height	= 1952,
+};
+
+static const struct ov5670_reg mipi_data_rate_840mbps[] = {
+	{0x0300, 0x04},
+	{0x0301, 0x00},
+	{0x0302, 0x84},
+	{0x0303, 0x00},
+	{0x0304, 0x03},
+	{0x0305, 0x01},
+	{0x0306, 0x01},
+	{0x030a, 0x00},
+	{0x030b, 0x00},
+	{0x030c, 0x00},
+	{0x030d, 0x26},
+	{0x030e, 0x00},
+	{0x030f, 0x06},
+	{0x0312, 0x01},
+	{0x3031, 0x0a},
+};
+
+static const struct ov5670_reg mode_2592x1944_regs[] = {
+	{0x3000, 0x00},
+	{0x3002, 0x21},
+	{0x3005, 0xf0},
+	{0x3007, 0x00},
+	{0x3015, 0x0f},
+	{0x301a, 0xf0},
+	{0x301b, 0xf0},
+	{0x301c, 0xf0},
+	{0x301d, 0xf0},
+	{0x301e, 0xf0},
+	{0x3030, 0x00},
+	{0x3031, 0x0a},
+	{0x303c, 0xff},
+	{0x303e, 0xff},
+	{0x3040, 0xf0},
+	{0x3041, 0x00},
+	{0x3042, 0xf0},
+	{0x3106, 0x11},
+	{0x3500, 0x00},
+	{0x3501, 0x80},
+	{0x3502, 0x00},
+	{0x3503, 0x04},
+	{0x3504, 0x03},
+	{0x3505, 0x83},
+	{0x3508, 0x04},
+	{0x3509, 0x00},
+	{0x350e, 0x04},
+	{0x350f, 0x00},
+	{0x3510, 0x00},
+	{0x3511, 0x02},
+	{0x3512, 0x00},
+	{0x3601, 0xc8},
+	{0x3610, 0x88},
+	{0x3612, 0x48},
+	{0x3614, 0x5b},
+	{0x3615, 0x96},
+	{0x3621, 0xd0},
+	{0x3622, 0x00},
+	{0x3623, 0x00},
+	{0x3633, 0x13},
+	{0x3634, 0x13},
+	{0x3635, 0x13},
+	{0x3636, 0x13},
+	{0x3645, 0x13},
+	{0x3646, 0x82},
+	{0x3650, 0x00},
+	{0x3652, 0xff},
+	{0x3655, 0x20},
+	{0x3656, 0xff},
+	{0x365a, 0xff},
+	{0x365e, 0xff},
+	{0x3668, 0x00},
+	{0x366a, 0x07},
+	{0x366e, 0x10},
+	{0x366d, 0x00},
+	{0x366f, 0x80},
+	{0x3700, 0x28},
+	{0x3701, 0x10},
+	{0x3702, 0x3a},
+	{0x3703, 0x19},
+	{0x3704, 0x10},
+	{0x3705, 0x00},
+	{0x3706, 0x66},
+	{0x3707, 0x08},
+	{0x3708, 0x34},
+	{0x3709, 0x40},
+	{0x370a, 0x01},
+	{0x370b, 0x1b},
+	{0x3714, 0x24},
+	{0x371a, 0x3e},
+	{0x3733, 0x00},
+	{0x3734, 0x00},
+	{0x373a, 0x05},
+	{0x373b, 0x06},
+	{0x373c, 0x0a},
+	{0x373f, 0xa0},
+	{0x3755, 0x00},
+	{0x3758, 0x00},
+	{0x375b, 0x0e},
+	{0x3766, 0x5f},
+	{0x3768, 0x00},
+	{0x3769, 0x22},
+	{0x3773, 0x08},
+	{0x3774, 0x1f},
+	{0x3776, 0x06},
+	{0x37a0, 0x88},
+	{0x37a1, 0x5c},
+	{0x37a7, 0x88},
+	{0x37a8, 0x70},
+	{0x37aa, 0x88},
+	{0x37ab, 0x48},
+	{0x37b3, 0x66},
+	{0x37c2, 0x04},
+	{0x37c5, 0x00},
+	{0x37c8, 0x00},
+	{0x3800, 0x00},
+	{0x3801, 0x0c},
+	{0x3802, 0x00},
+	{0x3803, 0x04},
+	{0x3804, 0x0a},
+	{0x3805, 0x33},
+	{0x3806, 0x07},
+	{0x3807, 0xa3},
+	{0x3808, 0x0a},
+	{0x3809, 0x20},
+	{0x380a, 0x07},
+	{0x380b, 0x98},
+	{0x380c, 0x06},
+	{0x380d, 0x90},
+	{0x380e, 0x08},
+	{0x380f, 0x08},
+	{0x3811, 0x04},
+	{0x3813, 0x02},
+	{0x3814, 0x01},
+	{0x3815, 0x01},
+	{0x3816, 0x00},
+	{0x3817, 0x00},
+	{0x3818, 0x00},
+	{0x3819, 0x00},
+	{0x3820, 0x84},
+	{0x3821, 0x46},
+	{0x3822, 0x48},
+	{0x3826, 0x00},
+	{0x3827, 0x08},
+	{0x382a, 0x01},
+	{0x382b, 0x01},
+	{0x3830, 0x08},
+	{0x3836, 0x02},
+	{0x3837, 0x00},
+	{0x3838, 0x10},
+	{0x3841, 0xff},
+	{0x3846, 0x48},
+	{0x3861, 0x00},
+	{0x3862, 0x04},
+	{0x3863, 0x06},
+	{0x3a11, 0x01},
+	{0x3a12, 0x78},
+	{0x3b00, 0x00},
+	{0x3b02, 0x00},
+	{0x3b03, 0x00},
+	{0x3b04, 0x00},
+	{0x3b05, 0x00},
+	{0x3c00, 0x89},
+	{0x3c01, 0xab},
+	{0x3c02, 0x01},
+	{0x3c03, 0x00},
+	{0x3c04, 0x00},
+	{0x3c05, 0x03},
+	{0x3c06, 0x00},
+	{0x3c07, 0x05},
+	{0x3c0c, 0x00},
+	{0x3c0d, 0x00},
+	{0x3c0e, 0x00},
+	{0x3c0f, 0x00},
+	{0x3c40, 0x00},
+	{0x3c41, 0xa3},
+	{0x3c43, 0x7d},
+	{0x3c45, 0xd7},
+	{0x3c47, 0xfc},
+	{0x3c50, 0x05},
+	{0x3c52, 0xaa},
+	{0x3c54, 0x71},
+	{0x3c56, 0x80},
+	{0x3d85, 0x17},
+	{0x3f03, 0x00},
+	{0x3f0a, 0x00},
+	{0x3f0b, 0x00},
+	{0x4001, 0x60},
+	{0x4009, 0x0d},
+	{0x4020, 0x00},
+	{0x4021, 0x00},
+	{0x4022, 0x00},
+	{0x4023, 0x00},
+	{0x4024, 0x00},
+	{0x4025, 0x00},
+	{0x4026, 0x00},
+	{0x4027, 0x00},
+	{0x4028, 0x00},
+	{0x4029, 0x00},
+	{0x402a, 0x00},
+	{0x402b, 0x00},
+	{0x402c, 0x00},
+	{0x402d, 0x00},
+	{0x402e, 0x00},
+	{0x402f, 0x00},
+	{0x4040, 0x00},
+	{0x4041, 0x03},
+	{0x4042, 0x00},
+	{0x4043, 0x7A},
+	{0x4044, 0x00},
+	{0x4045, 0x7A},
+	{0x4046, 0x00},
+	{0x4047, 0x7A},
+	{0x4048, 0x00},
+	{0x4049, 0x7A},
+	{0x4307, 0x30},
+	{0x4500, 0x58},
+	{0x4501, 0x04},
+	{0x4502, 0x40},
+	{0x4503, 0x10},
+	{0x4508, 0xaa},
+	{0x4509, 0xaa},
+	{0x450a, 0x00},
+	{0x450b, 0x00},
+	{0x4600, 0x01},
+	{0x4601, 0x03},
+	{0x4700, 0xa4},
+	{0x4800, 0x4c},
+	{0x4816, 0x53},
+	{0x481f, 0x40},
+	{0x4837, 0x13},
+	{0x5000, 0x56},
+	{0x5001, 0x01},
+	{0x5002, 0x28},
+	{0x5004, 0x0c},
+	{0x5006, 0x0c},
+	{0x5007, 0xe0},
+	{0x5008, 0x01},
+	{0x5009, 0xb0},
+	{0x5901, 0x00},
+	{0x5a01, 0x00},
+	{0x5a03, 0x00},
+	{0x5a04, 0x0c},
+	{0x5a05, 0xe0},
+	{0x5a06, 0x09},
+	{0x5a07, 0xb0},
+	{0x5a08, 0x06},
+	{0x5e00, 0x00},
+	{0x3734, 0x40},
+	{0x5b00, 0x01},
+	{0x5b01, 0x10},
+	{0x5b02, 0x01},
+	{0x5b03, 0xdb},
+	{0x3d8c, 0x71},
+	{0x3d8d, 0xea},
+	{0x4017, 0x08},
+	{0x3618, 0x2a},
+	{0x5780, 0x3e},
+	{0x5781, 0x0f},
+	{0x5782, 0x44},
+	{0x5783, 0x02},
+	{0x5784, 0x01},
+	{0x5785, 0x01},
+	{0x5786, 0x00},
+	{0x5787, 0x04},
+	{0x5788, 0x02},
+	{0x5789, 0x0f},
+	{0x578a, 0xfd},
+	{0x578b, 0xf5},
+	{0x578c, 0xf5},
+	{0x578d, 0x03},
+	{0x578e, 0x08},
+	{0x578f, 0x0c},
+	{0x5790, 0x08},
+	{0x5791, 0x06},
+	{0x5792, 0x00},
+	{0x5793, 0x52},
+	{0x5794, 0xa3},
+	{0x3503, 0x00},
+	{0x5045, 0x05},
+	{0x4003, 0x40},
+	{0x5048, 0x40}
+};
+
+static const struct ov5670_reg mode_1296x972_regs[] = {
+	{0x3000, 0x00},
+	{0x3002, 0x21},
+	{0x3005, 0xf0},
+	{0x3007, 0x00},
+	{0x3015, 0x0f},
+	{0x301a, 0xf0},
+	{0x301b, 0xf0},
+	{0x301c, 0xf0},
+	{0x301d, 0xf0},
+	{0x301e, 0xf0},
+	{0x3030, 0x00},
+	{0x3031, 0x0a},
+	{0x303c, 0xff},
+	{0x303e, 0xff},
+	{0x3040, 0xf0},
+	{0x3041, 0x00},
+	{0x3042, 0xf0},
+	{0x3106, 0x11},
+	{0x3500, 0x00},
+	{0x3501, 0x80},
+	{0x3502, 0x00},
+	{0x3503, 0x04},
+	{0x3504, 0x03},
+	{0x3505, 0x83},
+	{0x3508, 0x07},
+	{0x3509, 0x80},
+	{0x350e, 0x04},
+	{0x350f, 0x00},
+	{0x3510, 0x00},
+	{0x3511, 0x02},
+	{0x3512, 0x00},
+	{0x3601, 0xc8},
+	{0x3610, 0x88},
+	{0x3612, 0x48},
+	{0x3614, 0x5b},
+	{0x3615, 0x96},
+	{0x3621, 0xd0},
+	{0x3622, 0x00},
+	{0x3623, 0x00},
+	{0x3633, 0x13},
+	{0x3634, 0x13},
+	{0x3635, 0x13},
+	{0x3636, 0x13},
+	{0x3645, 0x13},
+	{0x3646, 0x82},
+	{0x3650, 0x00},
+	{0x3652, 0xff},
+	{0x3655, 0x20},
+	{0x3656, 0xff},
+	{0x365a, 0xff},
+	{0x365e, 0xff},
+	{0x3668, 0x00},
+	{0x366a, 0x07},
+	{0x366e, 0x08},
+	{0x366d, 0x00},
+	{0x366f, 0x80},
+	{0x3700, 0x28},
+	{0x3701, 0x10},
+	{0x3702, 0x3a},
+	{0x3703, 0x19},
+	{0x3704, 0x10},
+	{0x3705, 0x00},
+	{0x3706, 0x66},
+	{0x3707, 0x08},
+	{0x3708, 0x34},
+	{0x3709, 0x40},
+	{0x370a, 0x01},
+	{0x370b, 0x1b},
+	{0x3714, 0x24},
+	{0x371a, 0x3e},
+	{0x3733, 0x00},
+	{0x3734, 0x00},
+	{0x373a, 0x05},
+	{0x373b, 0x06},
+	{0x373c, 0x0a},
+	{0x373f, 0xa0},
+	{0x3755, 0x00},
+	{0x3758, 0x00},
+	{0x375b, 0x0e},
+	{0x3766, 0x5f},
+	{0x3768, 0x00},
+	{0x3769, 0x22},
+	{0x3773, 0x08},
+	{0x3774, 0x1f},
+	{0x3776, 0x06},
+	{0x37a0, 0x88},
+	{0x37a1, 0x5c},
+	{0x37a7, 0x88},
+	{0x37a8, 0x70},
+	{0x37aa, 0x88},
+	{0x37ab, 0x48},
+	{0x37b3, 0x66},
+	{0x37c2, 0x04},
+	{0x37c5, 0x00},
+	{0x37c8, 0x00},
+	{0x3800, 0x00},
+	{0x3801, 0x0c},
+	{0x3802, 0x00},
+	{0x3803, 0x04},
+	{0x3804, 0x0a},
+	{0x3805, 0x33},
+	{0x3806, 0x07},
+	{0x3807, 0xa3},
+	{0x3808, 0x05},
+	{0x3809, 0x10},
+	{0x380a, 0x03},
+	{0x380b, 0xcc},
+	{0x380c, 0x06},
+	{0x380d, 0x90},
+	{0x380e, 0x08},
+	{0x380f, 0x08},
+	{0x3811, 0x04},
+	{0x3813, 0x04},
+	{0x3814, 0x03},
+	{0x3815, 0x01},
+	{0x3816, 0x00},
+	{0x3817, 0x00},
+	{0x3818, 0x00},
+	{0x3819, 0x00},
+	{0x3820, 0x94},
+	{0x3821, 0x47},
+	{0x3822, 0x48},
+	{0x3826, 0x00},
+	{0x3827, 0x08},
+	{0x382a, 0x03},
+	{0x382b, 0x01},
+	{0x3830, 0x08},
+	{0x3836, 0x02},
+	{0x3837, 0x00},
+	{0x3838, 0x10},
+	{0x3841, 0xff},
+	{0x3846, 0x48},
+	{0x3861, 0x00},
+	{0x3862, 0x04},
+	{0x3863, 0x06},
+	{0x3a11, 0x01},
+	{0x3a12, 0x78},
+	{0x3b00, 0x00},
+	{0x3b02, 0x00},
+	{0x3b03, 0x00},
+	{0x3b04, 0x00},
+	{0x3b05, 0x00},
+	{0x3c00, 0x89},
+	{0x3c01, 0xab},
+	{0x3c02, 0x01},
+	{0x3c03, 0x00},
+	{0x3c04, 0x00},
+	{0x3c05, 0x03},
+	{0x3c06, 0x00},
+	{0x3c07, 0x05},
+	{0x3c0c, 0x00},
+	{0x3c0d, 0x00},
+	{0x3c0e, 0x00},
+	{0x3c0f, 0x00},
+	{0x3c40, 0x00},
+	{0x3c41, 0xa3},
+	{0x3c43, 0x7d},
+	{0x3c45, 0xd7},
+	{0x3c47, 0xfc},
+	{0x3c50, 0x05},
+	{0x3c52, 0xaa},
+	{0x3c54, 0x71},
+	{0x3c56, 0x80},
+	{0x3d85, 0x17},
+	{0x3f03, 0x00},
+	{0x3f0a, 0x00},
+	{0x3f0b, 0x00},
+	{0x4001, 0x60},
+	{0x4009, 0x05},
+	{0x4020, 0x00},
+	{0x4021, 0x00},
+	{0x4022, 0x00},
+	{0x4023, 0x00},
+	{0x4024, 0x00},
+	{0x4025, 0x00},
+	{0x4026, 0x00},
+	{0x4027, 0x00},
+	{0x4028, 0x00},
+	{0x4029, 0x00},
+	{0x402a, 0x00},
+	{0x402b, 0x00},
+	{0x402c, 0x00},
+	{0x402d, 0x00},
+	{0x402e, 0x00},
+	{0x402f, 0x00},
+	{0x4040, 0x00},
+	{0x4041, 0x03},
+	{0x4042, 0x00},
+	{0x4043, 0x7A},
+	{0x4044, 0x00},
+	{0x4045, 0x7A},
+	{0x4046, 0x00},
+	{0x4047, 0x7A},
+	{0x4048, 0x00},
+	{0x4049, 0x7A},
+	{0x4307, 0x30},
+	{0x4500, 0x58},
+	{0x4501, 0x04},
+	{0x4502, 0x48},
+	{0x4503, 0x10},
+	{0x4508, 0x55},
+	{0x4509, 0x55},
+	{0x450a, 0x00},
+	{0x450b, 0x00},
+	{0x4600, 0x00},
+	{0x4601, 0x81},
+	{0x4700, 0xa4},
+	{0x4800, 0x4c},
+	{0x4816, 0x53},
+	{0x481f, 0x40},
+	{0x4837, 0x13},
+	{0x5000, 0x56},
+	{0x5001, 0x01},
+	{0x5002, 0x28},
+	{0x5004, 0x0c},
+	{0x5006, 0x0c},
+	{0x5007, 0xe0},
+	{0x5008, 0x01},
+	{0x5009, 0xb0},
+	{0x5901, 0x00},
+	{0x5a01, 0x00},
+	{0x5a03, 0x00},
+	{0x5a04, 0x0c},
+	{0x5a05, 0xe0},
+	{0x5a06, 0x09},
+	{0x5a07, 0xb0},
+	{0x5a08, 0x06},
+	{0x5e00, 0x00},
+	{0x3734, 0x40},
+	{0x5b00, 0x01},
+	{0x5b01, 0x10},
+	{0x5b02, 0x01},
+	{0x5b03, 0xdb},
+	{0x3d8c, 0x71},
+	{0x3d8d, 0xea},
+	{0x4017, 0x10},
+	{0x3618, 0x2a},
+	{0x5780, 0x3e},
+	{0x5781, 0x0f},
+	{0x5782, 0x44},
+	{0x5783, 0x02},
+	{0x5784, 0x01},
+	{0x5785, 0x01},
+	{0x5786, 0x00},
+	{0x5787, 0x04},
+	{0x5788, 0x02},
+	{0x5789, 0x0f},
+	{0x578a, 0xfd},
+	{0x578b, 0xf5},
+	{0x578c, 0xf5},
+	{0x578d, 0x03},
+	{0x578e, 0x08},
+	{0x578f, 0x0c},
+	{0x5790, 0x08},
+	{0x5791, 0x04},
+	{0x5792, 0x00},
+	{0x5793, 0x52},
+	{0x5794, 0xa3},
+	{0x3503, 0x00},
+	{0x5045, 0x05},
+	{0x4003, 0x40},
+	{0x5048, 0x40}
+};
+
+static const struct ov5670_reg mode_648x486_regs[] = {
+	{0x3000, 0x00},
+	{0x3002, 0x21},
+	{0x3005, 0xf0},
+	{0x3007, 0x00},
+	{0x3015, 0x0f},
+	{0x301a, 0xf0},
+	{0x301b, 0xf0},
+	{0x301c, 0xf0},
+	{0x301d, 0xf0},
+	{0x301e, 0xf0},
+	{0x3030, 0x00},
+	{0x3031, 0x0a},
+	{0x303c, 0xff},
+	{0x303e, 0xff},
+	{0x3040, 0xf0},
+	{0x3041, 0x00},
+	{0x3042, 0xf0},
+	{0x3106, 0x11},
+	{0x3500, 0x00},
+	{0x3501, 0x80},
+	{0x3502, 0x00},
+	{0x3503, 0x04},
+	{0x3504, 0x03},
+	{0x3505, 0x83},
+	{0x3508, 0x04},
+	{0x3509, 0x00},
+	{0x350e, 0x04},
+	{0x350f, 0x00},
+	{0x3510, 0x00},
+	{0x3511, 0x02},
+	{0x3512, 0x00},
+	{0x3601, 0xc8},
+	{0x3610, 0x88},
+	{0x3612, 0x48},
+	{0x3614, 0x5b},
+	{0x3615, 0x96},
+	{0x3621, 0xd0},
+	{0x3622, 0x00},
+	{0x3623, 0x04},
+	{0x3633, 0x13},
+	{0x3634, 0x13},
+	{0x3635, 0x13},
+	{0x3636, 0x13},
+	{0x3645, 0x13},
+	{0x3646, 0x82},
+	{0x3650, 0x00},
+	{0x3652, 0xff},
+	{0x3655, 0x20},
+	{0x3656, 0xff},
+	{0x365a, 0xff},
+	{0x365e, 0xff},
+	{0x3668, 0x00},
+	{0x366a, 0x07},
+	{0x366e, 0x08},
+	{0x366d, 0x00},
+	{0x366f, 0x80},
+	{0x3700, 0x28},
+	{0x3701, 0x10},
+	{0x3702, 0x3a},
+	{0x3703, 0x19},
+	{0x3704, 0x10},
+	{0x3705, 0x00},
+	{0x3706, 0x66},
+	{0x3707, 0x08},
+	{0x3708, 0x34},
+	{0x3709, 0x40},
+	{0x370a, 0x01},
+	{0x370b, 0x1b},
+	{0x3714, 0x24},
+	{0x371a, 0x3e},
+	{0x3733, 0x00},
+	{0x3734, 0x00},
+	{0x373a, 0x05},
+	{0x373b, 0x06},
+	{0x373c, 0x0a},
+	{0x373f, 0xa0},
+	{0x3755, 0x00},
+	{0x3758, 0x00},
+	{0x375b, 0x0e},
+	{0x3766, 0x5f},
+	{0x3768, 0x00},
+	{0x3769, 0x22},
+	{0x3773, 0x08},
+	{0x3774, 0x1f},
+	{0x3776, 0x06},
+	{0x37a0, 0x88},
+	{0x37a1, 0x5c},
+	{0x37a7, 0x88},
+	{0x37a8, 0x70},
+	{0x37aa, 0x88},
+	{0x37ab, 0x48},
+	{0x37b3, 0x66},
+	{0x37c2, 0x04},
+	{0x37c5, 0x00},
+	{0x37c8, 0x00},
+	{0x3800, 0x00},
+	{0x3801, 0x0c},
+	{0x3802, 0x00},
+	{0x3803, 0x04},
+	{0x3804, 0x0a},
+	{0x3805, 0x33},
+	{0x3806, 0x07},
+	{0x3807, 0xa3},
+	{0x3808, 0x02},
+	{0x3809, 0x88},
+	{0x380a, 0x01},
+	{0x380b, 0xe6},
+	{0x380c, 0x06},
+	{0x380d, 0x90},
+	{0x380e, 0x08},
+	{0x380f, 0x08},
+	{0x3811, 0x04},
+	{0x3813, 0x02},
+	{0x3814, 0x07},
+	{0x3815, 0x01},
+	{0x3816, 0x00},
+	{0x3817, 0x00},
+	{0x3818, 0x00},
+	{0x3819, 0x00},
+	{0x3820, 0x94},
+	{0x3821, 0xc6},
+	{0x3822, 0x48},
+	{0x3826, 0x00},
+	{0x3827, 0x08},
+	{0x382a, 0x07},
+	{0x382b, 0x01},
+	{0x3830, 0x08},
+	{0x3836, 0x02},
+	{0x3837, 0x00},
+	{0x3838, 0x10},
+	{0x3841, 0xff},
+	{0x3846, 0x48},
+	{0x3861, 0x00},
+	{0x3862, 0x04},
+	{0x3863, 0x06},
+	{0x3a11, 0x01},
+	{0x3a12, 0x78},
+	{0x3b00, 0x00},
+	{0x3b02, 0x00},
+	{0x3b03, 0x00},
+	{0x3b04, 0x00},
+	{0x3b05, 0x00},
+	{0x3c00, 0x89},
+	{0x3c01, 0xab},
+	{0x3c02, 0x01},
+	{0x3c03, 0x00},
+	{0x3c04, 0x00},
+	{0x3c05, 0x03},
+	{0x3c06, 0x00},
+	{0x3c07, 0x05},
+	{0x3c0c, 0x00},
+	{0x3c0d, 0x00},
+	{0x3c0e, 0x00},
+	{0x3c0f, 0x00},
+	{0x3c40, 0x00},
+	{0x3c41, 0xa3},
+	{0x3c43, 0x7d},
+	{0x3c45, 0xd7},
+	{0x3c47, 0xfc},
+	{0x3c50, 0x05},
+	{0x3c52, 0xaa},
+	{0x3c54, 0x71},
+	{0x3c56, 0x80},
+	{0x3d85, 0x17},
+	{0x3f03, 0x00},
+	{0x3f0a, 0x00},
+	{0x3f0b, 0x00},
+	{0x4001, 0x60},
+	{0x4009, 0x05},
+	{0x4020, 0x00},
+	{0x4021, 0x00},
+	{0x4022, 0x00},
+	{0x4023, 0x00},
+	{0x4024, 0x00},
+	{0x4025, 0x00},
+	{0x4026, 0x00},
+	{0x4027, 0x00},
+	{0x4028, 0x00},
+	{0x4029, 0x00},
+	{0x402a, 0x00},
+	{0x402b, 0x00},
+	{0x402c, 0x00},
+	{0x402d, 0x00},
+	{0x402e, 0x00},
+	{0x402f, 0x00},
+	{0x4040, 0x00},
+	{0x4041, 0x03},
+	{0x4042, 0x00},
+	{0x4043, 0x7A},
+	{0x4044, 0x00},
+	{0x4045, 0x7A},
+	{0x4046, 0x00},
+	{0x4047, 0x7A},
+	{0x4048, 0x00},
+	{0x4049, 0x7A},
+	{0x4307, 0x30},
+	{0x4500, 0x58},
+	{0x4501, 0x04},
+	{0x4502, 0x40},
+	{0x4503, 0x10},
+	{0x4508, 0x55},
+	{0x4509, 0x55},
+	{0x450a, 0x02},
+	{0x450b, 0x00},
+	{0x4600, 0x00},
+	{0x4601, 0x40},
+	{0x4700, 0xa4},
+	{0x4800, 0x4c},
+	{0x4816, 0x53},
+	{0x481f, 0x40},
+	{0x4837, 0x13},
+	{0x5000, 0x56},
+	{0x5001, 0x01},
+	{0x5002, 0x28},
+	{0x5004, 0x0c},
+	{0x5006, 0x0c},
+	{0x5007, 0xe0},
+	{0x5008, 0x01},
+	{0x5009, 0xb0},
+	{0x5901, 0x00},
+	{0x5a01, 0x00},
+	{0x5a03, 0x00},
+	{0x5a04, 0x0c},
+	{0x5a05, 0xe0},
+	{0x5a06, 0x09},
+	{0x5a07, 0xb0},
+	{0x5a08, 0x06},
+	{0x5e00, 0x00},
+	{0x3734, 0x40},
+	{0x5b00, 0x01},
+	{0x5b01, 0x10},
+	{0x5b02, 0x01},
+	{0x5b03, 0xdb},
+	{0x3d8c, 0x71},
+	{0x3d8d, 0xea},
+	{0x4017, 0x10},
+	{0x3618, 0x2a},
+	{0x5780, 0x3e},
+	{0x5781, 0x0f},
+	{0x5782, 0x44},
+	{0x5783, 0x02},
+	{0x5784, 0x01},
+	{0x5785, 0x01},
+	{0x5786, 0x00},
+	{0x5787, 0x04},
+	{0x5788, 0x02},
+	{0x5789, 0x0f},
+	{0x578a, 0xfd},
+	{0x578b, 0xf5},
+	{0x578c, 0xf5},
+	{0x578d, 0x03},
+	{0x578e, 0x08},
+	{0x578f, 0x0c},
+	{0x5790, 0x08},
+	{0x5791, 0x06},
+	{0x5792, 0x00},
+	{0x5793, 0x52},
+	{0x5794, 0xa3},
+	{0x3503, 0x00},
+	{0x5045, 0x05},
+	{0x4003, 0x40},
+	{0x5048, 0x40}
+};
+
+static const struct ov5670_reg mode_2560x1440_regs[] = {
+	{0x3000, 0x00},
+	{0x3002, 0x21},
+	{0x3005, 0xf0},
+	{0x3007, 0x00},
+	{0x3015, 0x0f},
+	{0x301a, 0xf0},
+	{0x301b, 0xf0},
+	{0x301c, 0xf0},
+	{0x301d, 0xf0},
+	{0x301e, 0xf0},
+	{0x3030, 0x00},
+	{0x3031, 0x0a},
+	{0x303c, 0xff},
+	{0x303e, 0xff},
+	{0x3040, 0xf0},
+	{0x3041, 0x00},
+	{0x3042, 0xf0},
+	{0x3106, 0x11},
+	{0x3500, 0x00},
+	{0x3501, 0x80},
+	{0x3502, 0x00},
+	{0x3503, 0x04},
+	{0x3504, 0x03},
+	{0x3505, 0x83},
+	{0x3508, 0x04},
+	{0x3509, 0x00},
+	{0x350e, 0x04},
+	{0x350f, 0x00},
+	{0x3510, 0x00},
+	{0x3511, 0x02},
+	{0x3512, 0x00},
+	{0x3601, 0xc8},
+	{0x3610, 0x88},
+	{0x3612, 0x48},
+	{0x3614, 0x5b},
+	{0x3615, 0x96},
+	{0x3621, 0xd0},
+	{0x3622, 0x00},
+	{0x3623, 0x00},
+	{0x3633, 0x13},
+	{0x3634, 0x13},
+	{0x3635, 0x13},
+	{0x3636, 0x13},
+	{0x3645, 0x13},
+	{0x3646, 0x82},
+	{0x3650, 0x00},
+	{0x3652, 0xff},
+	{0x3655, 0x20},
+	{0x3656, 0xff},
+	{0x365a, 0xff},
+	{0x365e, 0xff},
+	{0x3668, 0x00},
+	{0x366a, 0x07},
+	{0x366e, 0x10},
+	{0x366d, 0x00},
+	{0x366f, 0x80},
+	{0x3700, 0x28},
+	{0x3701, 0x10},
+	{0x3702, 0x3a},
+	{0x3703, 0x19},
+	{0x3704, 0x10},
+	{0x3705, 0x00},
+	{0x3706, 0x66},
+	{0x3707, 0x08},
+	{0x3708, 0x34},
+	{0x3709, 0x40},
+	{0x370a, 0x01},
+	{0x370b, 0x1b},
+	{0x3714, 0x24},
+	{0x371a, 0x3e},
+	{0x3733, 0x00},
+	{0x3734, 0x00},
+	{0x373a, 0x05},
+	{0x373b, 0x06},
+	{0x373c, 0x0a},
+	{0x373f, 0xa0},
+	{0x3755, 0x00},
+	{0x3758, 0x00},
+	{0x375b, 0x0e},
+	{0x3766, 0x5f},
+	{0x3768, 0x00},
+	{0x3769, 0x22},
+	{0x3773, 0x08},
+	{0x3774, 0x1f},
+	{0x3776, 0x06},
+	{0x37a0, 0x88},
+	{0x37a1, 0x5c},
+	{0x37a7, 0x88},
+	{0x37a8, 0x70},
+	{0x37aa, 0x88},
+	{0x37ab, 0x48},
+	{0x37b3, 0x66},
+	{0x37c2, 0x04},
+	{0x37c5, 0x00},
+	{0x37c8, 0x00},
+	{0x3800, 0x00},
+	{0x3801, 0x0c},
+	{0x3802, 0x00},
+	{0x3803, 0x04},
+	{0x3804, 0x0a},
+	{0x3805, 0x33},
+	{0x3806, 0x07},
+	{0x3807, 0xa3},
+	{0x3808, 0x0a},
+	{0x3809, 0x00},
+	{0x380a, 0x05},
+	{0x380b, 0xa0},
+	{0x380c, 0x06},
+	{0x380d, 0x90},
+	{0x380e, 0x08},
+	{0x380f, 0x08},
+	{0x3811, 0x04},
+	{0x3813, 0x02},
+	{0x3814, 0x01},
+	{0x3815, 0x01},
+	{0x3816, 0x00},
+	{0x3817, 0x00},
+	{0x3818, 0x00},
+	{0x3819, 0x00},
+	{0x3820, 0x84},
+	{0x3821, 0x46},
+	{0x3822, 0x48},
+	{0x3826, 0x00},
+	{0x3827, 0x08},
+	{0x382a, 0x01},
+	{0x382b, 0x01},
+	{0x3830, 0x08},
+	{0x3836, 0x02},
+	{0x3837, 0x00},
+	{0x3838, 0x10},
+	{0x3841, 0xff},
+	{0x3846, 0x48},
+	{0x3861, 0x00},
+	{0x3862, 0x04},
+	{0x3863, 0x06},
+	{0x3a11, 0x01},
+	{0x3a12, 0x78},
+	{0x3b00, 0x00},
+	{0x3b02, 0x00},
+	{0x3b03, 0x00},
+	{0x3b04, 0x00},
+	{0x3b05, 0x00},
+	{0x3c00, 0x89},
+	{0x3c01, 0xab},
+	{0x3c02, 0x01},
+	{0x3c03, 0x00},
+	{0x3c04, 0x00},
+	{0x3c05, 0x03},
+	{0x3c06, 0x00},
+	{0x3c07, 0x05},
+	{0x3c0c, 0x00},
+	{0x3c0d, 0x00},
+	{0x3c0e, 0x00},
+	{0x3c0f, 0x00},
+	{0x3c40, 0x00},
+	{0x3c41, 0xa3},
+	{0x3c43, 0x7d},
+	{0x3c45, 0xd7},
+	{0x3c47, 0xfc},
+	{0x3c50, 0x05},
+	{0x3c52, 0xaa},
+	{0x3c54, 0x71},
+	{0x3c56, 0x80},
+	{0x3d85, 0x17},
+	{0x3f03, 0x00},
+	{0x3f0a, 0x00},
+	{0x3f0b, 0x00},
+	{0x4001, 0x60},
+	{0x4009, 0x0d},
+	{0x4020, 0x00},
+	{0x4021, 0x00},
+	{0x4022, 0x00},
+	{0x4023, 0x00},
+	{0x4024, 0x00},
+	{0x4025, 0x00},
+	{0x4026, 0x00},
+	{0x4027, 0x00},
+	{0x4028, 0x00},
+	{0x4029, 0x00},
+	{0x402a, 0x00},
+	{0x402b, 0x00},
+	{0x402c, 0x00},
+	{0x402d, 0x00},
+	{0x402e, 0x00},
+	{0x402f, 0x00},
+	{0x4040, 0x00},
+	{0x4041, 0x03},
+	{0x4042, 0x00},
+	{0x4043, 0x7A},
+	{0x4044, 0x00},
+	{0x4045, 0x7A},
+	{0x4046, 0x00},
+	{0x4047, 0x7A},
+	{0x4048, 0x00},
+	{0x4049, 0x7A},
+	{0x4307, 0x30},
+	{0x4500, 0x58},
+	{0x4501, 0x04},
+	{0x4502, 0x40},
+	{0x4503, 0x10},
+	{0x4508, 0xaa},
+	{0x4509, 0xaa},
+	{0x450a, 0x00},
+	{0x450b, 0x00},
+	{0x4600, 0x01},
+	{0x4601, 0x00},
+	{0x4700, 0xa4},
+	{0x4800, 0x4c},
+	{0x4816, 0x53},
+	{0x481f, 0x40},
+	{0x4837, 0x13},
+	{0x5000, 0x56},
+	{0x5001, 0x01},
+	{0x5002, 0x28},
+	{0x5004, 0x0c},
+	{0x5006, 0x0c},
+	{0x5007, 0xe0},
+	{0x5008, 0x01},
+	{0x5009, 0xb0},
+	{0x5901, 0x00},
+	{0x5a01, 0x00},
+	{0x5a03, 0x00},
+	{0x5a04, 0x0c},
+	{0x5a05, 0xe0},
+	{0x5a06, 0x09},
+	{0x5a07, 0xb0},
+	{0x5a08, 0x06},
+	{0x5e00, 0x00},
+	{0x3734, 0x40},
+	{0x5b00, 0x01},
+	{0x5b01, 0x10},
+	{0x5b02, 0x01},
+	{0x5b03, 0xdb},
+	{0x3d8c, 0x71},
+	{0x3d8d, 0xea},
+	{0x4017, 0x08},
+	{0x3618, 0x2a},
+	{0x5780, 0x3e},
+	{0x5781, 0x0f},
+	{0x5782, 0x44},
+	{0x5783, 0x02},
+	{0x5784, 0x01},
+	{0x5785, 0x01},
+	{0x5786, 0x00},
+	{0x5787, 0x04},
+	{0x5788, 0x02},
+	{0x5789, 0x0f},
+	{0x578a, 0xfd},
+	{0x578b, 0xf5},
+	{0x578c, 0xf5},
+	{0x578d, 0x03},
+	{0x578e, 0x08},
+	{0x578f, 0x0c},
+	{0x5790, 0x08},
+	{0x5791, 0x06},
+	{0x5792, 0x00},
+	{0x5793, 0x52},
+	{0x5794, 0xa3},
+	{0x5045, 0x05},
+	{0x4003, 0x40},
+	{0x5048, 0x40}
+};
+
+static const struct ov5670_reg mode_1280x720_regs[] = {
+	{0x3000, 0x00},
+	{0x3002, 0x21},
+	{0x3005, 0xf0},
+	{0x3007, 0x00},
+	{0x3015, 0x0f},
+	{0x301a, 0xf0},
+	{0x301b, 0xf0},
+	{0x301c, 0xf0},
+	{0x301d, 0xf0},
+	{0x301e, 0xf0},
+	{0x3030, 0x00},
+	{0x3031, 0x0a},
+	{0x303c, 0xff},
+	{0x303e, 0xff},
+	{0x3040, 0xf0},
+	{0x3041, 0x00},
+	{0x3042, 0xf0},
+	{0x3106, 0x11},
+	{0x3500, 0x00},
+	{0x3501, 0x80},
+	{0x3502, 0x00},
+	{0x3503, 0x04},
+	{0x3504, 0x03},
+	{0x3505, 0x83},
+	{0x3508, 0x04},
+	{0x3509, 0x00},
+	{0x350e, 0x04},
+	{0x350f, 0x00},
+	{0x3510, 0x00},
+	{0x3511, 0x02},
+	{0x3512, 0x00},
+	{0x3601, 0xc8},
+	{0x3610, 0x88},
+	{0x3612, 0x48},
+	{0x3614, 0x5b},
+	{0x3615, 0x96},
+	{0x3621, 0xd0},
+	{0x3622, 0x00},
+	{0x3623, 0x00},
+	{0x3633, 0x13},
+	{0x3634, 0x13},
+	{0x3635, 0x13},
+	{0x3636, 0x13},
+	{0x3645, 0x13},
+	{0x3646, 0x82},
+	{0x3650, 0x00},
+	{0x3652, 0xff},
+	{0x3655, 0x20},
+	{0x3656, 0xff},
+	{0x365a, 0xff},
+	{0x365e, 0xff},
+	{0x3668, 0x00},
+	{0x366a, 0x07},
+	{0x366e, 0x08},
+	{0x366d, 0x00},
+	{0x366f, 0x80},
+	{0x3700, 0x28},
+	{0x3701, 0x10},
+	{0x3702, 0x3a},
+	{0x3703, 0x19},
+	{0x3704, 0x10},
+	{0x3705, 0x00},
+	{0x3706, 0x66},
+	{0x3707, 0x08},
+	{0x3708, 0x34},
+	{0x3709, 0x40},
+	{0x370a, 0x01},
+	{0x370b, 0x1b},
+	{0x3714, 0x24},
+	{0x371a, 0x3e},
+	{0x3733, 0x00},
+	{0x3734, 0x00},
+	{0x373a, 0x05},
+	{0x373b, 0x06},
+	{0x373c, 0x0a},
+	{0x373f, 0xa0},
+	{0x3755, 0x00},
+	{0x3758, 0x00},
+	{0x375b, 0x0e},
+	{0x3766, 0x5f},
+	{0x3768, 0x00},
+	{0x3769, 0x22},
+	{0x3773, 0x08},
+	{0x3774, 0x1f},
+	{0x3776, 0x06},
+	{0x37a0, 0x88},
+	{0x37a1, 0x5c},
+	{0x37a7, 0x88},
+	{0x37a8, 0x70},
+	{0x37aa, 0x88},
+	{0x37ab, 0x48},
+	{0x37b3, 0x66},
+	{0x37c2, 0x04},
+	{0x37c5, 0x00},
+	{0x37c8, 0x00},
+	{0x3800, 0x00},
+	{0x3801, 0x0c},
+	{0x3802, 0x00},
+	{0x3803, 0x04},
+	{0x3804, 0x0a},
+	{0x3805, 0x33},
+	{0x3806, 0x07},
+	{0x3807, 0xa3},
+	{0x3808, 0x05},
+	{0x3809, 0x00},
+	{0x380a, 0x02},
+	{0x380b, 0xd0},
+	{0x380c, 0x06},
+	{0x380d, 0x90},
+	{0x380e, 0x08},
+	{0x380f, 0x08},
+	{0x3811, 0x04},
+	{0x3813, 0x02},
+	{0x3814, 0x03},
+	{0x3815, 0x01},
+	{0x3816, 0x00},
+	{0x3817, 0x00},
+	{0x3818, 0x00},
+	{0x3819, 0x00},
+	{0x3820, 0x94},
+	{0x3821, 0x47},
+	{0x3822, 0x48},
+	{0x3826, 0x00},
+	{0x3827, 0x08},
+	{0x382a, 0x03},
+	{0x382b, 0x01},
+	{0x3830, 0x08},
+	{0x3836, 0x02},
+	{0x3837, 0x00},
+	{0x3838, 0x10},
+	{0x3841, 0xff},
+	{0x3846, 0x48},
+	{0x3861, 0x00},
+	{0x3862, 0x04},
+	{0x3863, 0x06},
+	{0x3a11, 0x01},
+	{0x3a12, 0x78},
+	{0x3b00, 0x00},
+	{0x3b02, 0x00},
+	{0x3b03, 0x00},
+	{0x3b04, 0x00},
+	{0x3b05, 0x00},
+	{0x3c00, 0x89},
+	{0x3c01, 0xab},
+	{0x3c02, 0x01},
+	{0x3c03, 0x00},
+	{0x3c04, 0x00},
+	{0x3c05, 0x03},
+	{0x3c06, 0x00},
+	{0x3c07, 0x05},
+	{0x3c0c, 0x00},
+	{0x3c0d, 0x00},
+	{0x3c0e, 0x00},
+	{0x3c0f, 0x00},
+	{0x3c40, 0x00},
+	{0x3c41, 0xa3},
+	{0x3c43, 0x7d},
+	{0x3c45, 0xd7},
+	{0x3c47, 0xfc},
+	{0x3c50, 0x05},
+	{0x3c52, 0xaa},
+	{0x3c54, 0x71},
+	{0x3c56, 0x80},
+	{0x3d85, 0x17},
+	{0x3f03, 0x00},
+	{0x3f0a, 0x00},
+	{0x3f0b, 0x00},
+	{0x4001, 0x60},
+	{0x4009, 0x05},
+	{0x4020, 0x00},
+	{0x4021, 0x00},
+	{0x4022, 0x00},
+	{0x4023, 0x00},
+	{0x4024, 0x00},
+	{0x4025, 0x00},
+	{0x4026, 0x00},
+	{0x4027, 0x00},
+	{0x4028, 0x00},
+	{0x4029, 0x00},
+	{0x402a, 0x00},
+	{0x402b, 0x00},
+	{0x402c, 0x00},
+	{0x402d, 0x00},
+	{0x402e, 0x00},
+	{0x402f, 0x00},
+	{0x4040, 0x00},
+	{0x4041, 0x03},
+	{0x4042, 0x00},
+	{0x4043, 0x7A},
+	{0x4044, 0x00},
+	{0x4045, 0x7A},
+	{0x4046, 0x00},
+	{0x4047, 0x7A},
+	{0x4048, 0x00},
+	{0x4049, 0x7A},
+	{0x4307, 0x30},
+	{0x4500, 0x58},
+	{0x4501, 0x04},
+	{0x4502, 0x48},
+	{0x4503, 0x10},
+	{0x4508, 0x55},
+	{0x4509, 0x55},
+	{0x450a, 0x00},
+	{0x450b, 0x00},
+	{0x4600, 0x00},
+	{0x4601, 0x80},
+	{0x4700, 0xa4},
+	{0x4800, 0x4c},
+	{0x4816, 0x53},
+	{0x481f, 0x40},
+	{0x4837, 0x13},
+	{0x5000, 0x56},
+	{0x5001, 0x01},
+	{0x5002, 0x28},
+	{0x5004, 0x0c},
+	{0x5006, 0x0c},
+	{0x5007, 0xe0},
+	{0x5008, 0x01},
+	{0x5009, 0xb0},
+	{0x5901, 0x00},
+	{0x5a01, 0x00},
+	{0x5a03, 0x00},
+	{0x5a04, 0x0c},
+	{0x5a05, 0xe0},
+	{0x5a06, 0x09},
+	{0x5a07, 0xb0},
+	{0x5a08, 0x06},
+	{0x5e00, 0x00},
+	{0x3734, 0x40},
+	{0x5b00, 0x01},
+	{0x5b01, 0x10},
+	{0x5b02, 0x01},
+	{0x5b03, 0xdb},
+	{0x3d8c, 0x71},
+	{0x3d8d, 0xea},
+	{0x4017, 0x10},
+	{0x3618, 0x2a},
+	{0x5780, 0x3e},
+	{0x5781, 0x0f},
+	{0x5782, 0x44},
+	{0x5783, 0x02},
+	{0x5784, 0x01},
+	{0x5785, 0x01},
+	{0x5786, 0x00},
+	{0x5787, 0x04},
+	{0x5788, 0x02},
+	{0x5789, 0x0f},
+	{0x578a, 0xfd},
+	{0x578b, 0xf5},
+	{0x578c, 0xf5},
+	{0x578d, 0x03},
+	{0x578e, 0x08},
+	{0x578f, 0x0c},
+	{0x5790, 0x08},
+	{0x5791, 0x06},
+	{0x5792, 0x00},
+	{0x5793, 0x52},
+	{0x5794, 0xa3},
+	{0x3503, 0x00},
+	{0x5045, 0x05},
+	{0x4003, 0x40},
+	{0x5048, 0x40}
+};
+
+static const struct ov5670_reg mode_640x360_regs[] = {
+	{0x3000, 0x00},
+	{0x3002, 0x21},
+	{0x3005, 0xf0},
+	{0x3007, 0x00},
+	{0x3015, 0x0f},
+	{0x301a, 0xf0},
+	{0x301b, 0xf0},
+	{0x301c, 0xf0},
+	{0x301d, 0xf0},
+	{0x301e, 0xf0},
+	{0x3030, 0x00},
+	{0x3031, 0x0a},
+	{0x303c, 0xff},
+	{0x303e, 0xff},
+	{0x3040, 0xf0},
+	{0x3041, 0x00},
+	{0x3042, 0xf0},
+	{0x3106, 0x11},
+	{0x3500, 0x00},
+	{0x3501, 0x80},
+	{0x3502, 0x00},
+	{0x3503, 0x04},
+	{0x3504, 0x03},
+	{0x3505, 0x83},
+	{0x3508, 0x04},
+	{0x3509, 0x00},
+	{0x350e, 0x04},
+	{0x350f, 0x00},
+	{0x3510, 0x00},
+	{0x3511, 0x02},
+	{0x3512, 0x00},
+	{0x3601, 0xc8},
+	{0x3610, 0x88},
+	{0x3612, 0x48},
+	{0x3614, 0x5b},
+	{0x3615, 0x96},
+	{0x3621, 0xd0},
+	{0x3622, 0x00},
+	{0x3623, 0x04},
+	{0x3633, 0x13},
+	{0x3634, 0x13},
+	{0x3635, 0x13},
+	{0x3636, 0x13},
+	{0x3645, 0x13},
+	{0x3646, 0x82},
+	{0x3650, 0x00},
+	{0x3652, 0xff},
+	{0x3655, 0x20},
+	{0x3656, 0xff},
+	{0x365a, 0xff},
+	{0x365e, 0xff},
+	{0x3668, 0x00},
+	{0x366a, 0x07},
+	{0x366e, 0x08},
+	{0x366d, 0x00},
+	{0x366f, 0x80},
+	{0x3700, 0x28},
+	{0x3701, 0x10},
+	{0x3702, 0x3a},
+	{0x3703, 0x19},
+	{0x3704, 0x10},
+	{0x3705, 0x00},
+	{0x3706, 0x66},
+	{0x3707, 0x08},
+	{0x3708, 0x34},
+	{0x3709, 0x40},
+	{0x370a, 0x01},
+	{0x370b, 0x1b},
+	{0x3714, 0x24},
+	{0x371a, 0x3e},
+	{0x3733, 0x00},
+	{0x3734, 0x00},
+	{0x373a, 0x05},
+	{0x373b, 0x06},
+	{0x373c, 0x0a},
+	{0x373f, 0xa0},
+	{0x3755, 0x00},
+	{0x3758, 0x00},
+	{0x375b, 0x0e},
+	{0x3766, 0x5f},
+	{0x3768, 0x00},
+	{0x3769, 0x22},
+	{0x3773, 0x08},
+	{0x3774, 0x1f},
+	{0x3776, 0x06},
+	{0x37a0, 0x88},
+	{0x37a1, 0x5c},
+	{0x37a7, 0x88},
+	{0x37a8, 0x70},
+	{0x37aa, 0x88},
+	{0x37ab, 0x48},
+	{0x37b3, 0x66},
+	{0x37c2, 0x04},
+	{0x37c5, 0x00},
+	{0x37c8, 0x00},
+	{0x3800, 0x00},
+	{0x3801, 0x0c},
+	{0x3802, 0x00},
+	{0x3803, 0x04},
+	{0x3804, 0x0a},
+	{0x3805, 0x33},
+	{0x3806, 0x07},
+	{0x3807, 0xa3},
+	{0x3808, 0x02},
+	{0x3809, 0x80},
+	{0x380a, 0x01},
+	{0x380b, 0x68},
+	{0x380c, 0x06},
+	{0x380d, 0x90},
+	{0x380e, 0x08},
+	{0x380f, 0x08},
+	{0x3811, 0x04},
+	{0x3813, 0x02},
+	{0x3814, 0x07},
+	{0x3815, 0x01},
+	{0x3816, 0x00},
+	{0x3817, 0x00},
+	{0x3818, 0x00},
+	{0x3819, 0x00},
+	{0x3820, 0x94},
+	{0x3821, 0xc6},
+	{0x3822, 0x48},
+	{0x3826, 0x00},
+	{0x3827, 0x08},
+	{0x382a, 0x07},
+	{0x382b, 0x01},
+	{0x3830, 0x08},
+	{0x3836, 0x02},
+	{0x3837, 0x00},
+	{0x3838, 0x10},
+	{0x3841, 0xff},
+	{0x3846, 0x48},
+	{0x3861, 0x00},
+	{0x3862, 0x04},
+	{0x3863, 0x06},
+	{0x3a11, 0x01},
+	{0x3a12, 0x78},
+	{0x3b00, 0x00},
+	{0x3b02, 0x00},
+	{0x3b03, 0x00},
+	{0x3b04, 0x00},
+	{0x3b05, 0x00},
+	{0x3c00, 0x89},
+	{0x3c01, 0xab},
+	{0x3c02, 0x01},
+	{0x3c03, 0x00},
+	{0x3c04, 0x00},
+	{0x3c05, 0x03},
+	{0x3c06, 0x00},
+	{0x3c07, 0x05},
+	{0x3c0c, 0x00},
+	{0x3c0d, 0x00},
+	{0x3c0e, 0x00},
+	{0x3c0f, 0x00},
+	{0x3c40, 0x00},
+	{0x3c41, 0xa3},
+	{0x3c43, 0x7d},
+	{0x3c45, 0xd7},
+	{0x3c47, 0xfc},
+	{0x3c50, 0x05},
+	{0x3c52, 0xaa},
+	{0x3c54, 0x71},
+	{0x3c56, 0x80},
+	{0x3d85, 0x17},
+	{0x3f03, 0x00},
+	{0x3f0a, 0x00},
+	{0x3f0b, 0x00},
+	{0x4001, 0x60},
+	{0x4009, 0x05},
+	{0x4020, 0x00},
+	{0x4021, 0x00},
+	{0x4022, 0x00},
+	{0x4023, 0x00},
+	{0x4024, 0x00},
+	{0x4025, 0x00},
+	{0x4026, 0x00},
+	{0x4027, 0x00},
+	{0x4028, 0x00},
+	{0x4029, 0x00},
+	{0x402a, 0x00},
+	{0x402b, 0x00},
+	{0x402c, 0x00},
+	{0x402d, 0x00},
+	{0x402e, 0x00},
+	{0x402f, 0x00},
+	{0x4040, 0x00},
+	{0x4041, 0x03},
+	{0x4042, 0x00},
+	{0x4043, 0x7A},
+	{0x4044, 0x00},
+	{0x4045, 0x7A},
+	{0x4046, 0x00},
+	{0x4047, 0x7A},
+	{0x4048, 0x00},
+	{0x4049, 0x7A},
+	{0x4307, 0x30},
+	{0x4500, 0x58},
+	{0x4501, 0x04},
+	{0x4502, 0x40},
+	{0x4503, 0x10},
+	{0x4508, 0x55},
+	{0x4509, 0x55},
+	{0x450a, 0x02},
+	{0x450b, 0x00},
+	{0x4600, 0x00},
+	{0x4601, 0x40},
+	{0x4700, 0xa4},
+	{0x4800, 0x4c},
+	{0x4816, 0x53},
+	{0x481f, 0x40},
+	{0x4837, 0x13},
+	{0x5000, 0x56},
+	{0x5001, 0x01},
+	{0x5002, 0x28},
+	{0x5004, 0x0c},
+	{0x5006, 0x0c},
+	{0x5007, 0xe0},
+	{0x5008, 0x01},
+	{0x5009, 0xb0},
+	{0x5901, 0x00},
+	{0x5a01, 0x00},
+	{0x5a03, 0x00},
+	{0x5a04, 0x0c},
+	{0x5a05, 0xe0},
+	{0x5a06, 0x09},
+	{0x5a07, 0xb0},
+	{0x5a08, 0x06},
+	{0x5e00, 0x00},
+	{0x3734, 0x40},
+	{0x5b00, 0x01},
+	{0x5b01, 0x10},
+	{0x5b02, 0x01},
+	{0x5b03, 0xdb},
+	{0x3d8c, 0x71},
+	{0x3d8d, 0xea},
+	{0x4017, 0x10},
+	{0x3618, 0x2a},
+	{0x5780, 0x3e},
+	{0x5781, 0x0f},
+	{0x5782, 0x44},
+	{0x5783, 0x02},
+	{0x5784, 0x01},
+	{0x5785, 0x01},
+	{0x5786, 0x00},
+	{0x5787, 0x04},
+	{0x5788, 0x02},
+	{0x5789, 0x0f},
+	{0x578a, 0xfd},
+	{0x578b, 0xf5},
+	{0x578c, 0xf5},
+	{0x578d, 0x03},
+	{0x578e, 0x08},
+	{0x578f, 0x0c},
+	{0x5790, 0x08},
+	{0x5791, 0x06},
+	{0x5792, 0x00},
+	{0x5793, 0x52},
+	{0x5794, 0xa3},
+	{0x3503, 0x00},
+	{0x5045, 0x05},
+	{0x4003, 0x40},
+	{0x5048, 0x40}
+};
+
+static const char * const ov5670_test_pattern_menu[] = {
+	"Disabled",
+	"Vertical Color Bar Type 1",
+};
+
+/* Supported link frequencies */
+#define OV5670_LINK_FREQ_422MHZ		422400000
+#define OV5670_LINK_FREQ_422MHZ_INDEX	0
+static const struct ov5670_link_freq_config link_freq_configs[] = {
+	{
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mipi_data_rate_840mbps),
+			.regs = mipi_data_rate_840mbps,
+		}
+	}
+};
+
+static const s64 link_freq_menu_items[] = {
+	OV5670_LINK_FREQ_422MHZ
+};
+
+/*
+ * OV5670 sensor supports following resolutions with full FOV:
+ * 4:3  ==> {2592x1944, 1296x972, 648x486}
+ * 16:9 ==> {2560x1440, 1280x720, 640x360}
+ */
+static const struct ov5670_mode supported_modes[] = {
+	{
+		.width = 2592,
+		.height = 1944,
+		.vts_def = OV5670_VTS_30FPS,
+		.vts_min = OV5670_VTS_30FPS,
+		.link_freq_index = OV5670_LINK_FREQ_422MHZ_INDEX,
+		.analog_crop = &ov5670_analog_crop,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_2592x1944_regs),
+			.regs = mode_2592x1944_regs,
+		},
+	},
+	{
+		.width = 1296,
+		.height = 972,
+		.vts_def = OV5670_VTS_30FPS,
+		.vts_min = 996,
+		.link_freq_index = OV5670_LINK_FREQ_422MHZ_INDEX,
+		.analog_crop = &ov5670_analog_crop,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1296x972_regs),
+			.regs = mode_1296x972_regs,
+		},
+	},
+	{
+		.width = 648,
+		.height = 486,
+		.vts_def = OV5670_VTS_30FPS,
+		.vts_min = 516,
+		.link_freq_index = OV5670_LINK_FREQ_422MHZ_INDEX,
+		.analog_crop = &ov5670_analog_crop,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_648x486_regs),
+			.regs = mode_648x486_regs,
+		},
+	},
+	{
+		.width = 2560,
+		.height = 1440,
+		.vts_def = OV5670_VTS_30FPS,
+		.vts_min = OV5670_VTS_30FPS,
+		.link_freq_index = OV5670_LINK_FREQ_422MHZ_INDEX,
+		.analog_crop = &ov5670_analog_crop,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_2560x1440_regs),
+			.regs = mode_2560x1440_regs,
+		},
+	},
+	{
+		.width = 1280,
+		.height = 720,
+		.vts_def = OV5670_VTS_30FPS,
+		.vts_min = 1020,
+
+		.link_freq_index = OV5670_LINK_FREQ_422MHZ_INDEX,
+		.analog_crop = &ov5670_analog_crop,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1280x720_regs),
+			.regs = mode_1280x720_regs,
+		},
+	},
+	{
+		.width = 640,
+		.height = 360,
+		.vts_def = OV5670_VTS_30FPS,
+		.vts_min = 510,
+		.link_freq_index = OV5670_LINK_FREQ_422MHZ_INDEX,
+		.analog_crop = &ov5670_analog_crop,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_640x360_regs),
+			.regs = mode_640x360_regs,
+		},
+	}
+};
+
+struct ov5670 {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct v4l2_fwnode_endpoint endpoint;
+
+	struct v4l2_ctrl_handler ctrl_handler;
+	/* V4L2 Controls */
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *pixel_rate;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *exposure;
+
+	/* Current mode */
+	const struct ov5670_mode *cur_mode;
+
+	/* xvclk input clock */
+	struct clk *xvclk;
+
+	/* Regulators */
+	struct regulator_bulk_data supplies[OV5670_NUM_SUPPLIES];
+
+	/* Power-down and reset gpios. */
+	struct gpio_desc *pwdn_gpio; /* PWDNB pin. */
+	struct gpio_desc *reset_gpio; /* XSHUTDOWN pin. */
+
+	/* To serialize asynchronus callbacks */
+	struct mutex mutex;
+
+	/* Streaming on/off */
+	bool streaming;
+	/* True if the device has been identified */
+	bool identified;
+};
+
+#define to_ov5670(_sd)	container_of(_sd, struct ov5670, sd)
+
+/* Read registers up to 4 at a time */
+static int ov5670_read_reg(struct ov5670 *ov5670, u16 reg, unsigned int len,
+			   u32 *val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov5670->sd);
+	struct i2c_msg msgs[2];
+	u8 *data_be_p;
+	__be32 data_be = 0;
+	__be16 reg_addr_be = cpu_to_be16(reg);
+	int ret;
+
+	if (len > 4)
+		return -EINVAL;
+
+	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[4 - len];
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret != ARRAY_SIZE(msgs))
+		return -EIO;
+
+	*val = be32_to_cpu(data_be);
+
+	return 0;
+}
+
+/* Write registers up to 4 at a time */
+static int ov5670_write_reg(struct ov5670 *ov5670, u16 reg, unsigned int len,
+			    u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov5670->sd);
+	int buf_i;
+	int val_i;
+	u8 buf[6];
+	u8 *val_p;
+	__be32 tmp;
+
+	if (len > 4)
+		return -EINVAL;
+
+	buf[0] = reg >> 8;
+	buf[1] = reg & 0xff;
+
+	tmp = cpu_to_be32(val);
+	val_p = (u8 *)&tmp;
+	buf_i = 2;
+	val_i = 4 - len;
+
+	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;
+}
+
+/* Write a list of registers */
+static int ov5670_write_regs(struct ov5670 *ov5670,
+			     const struct ov5670_reg *regs, unsigned int len)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov5670->sd);
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < len; i++) {
+		ret = ov5670_write_reg(ov5670, regs[i].address, 1, regs[i].val);
+		if (ret) {
+			dev_err_ratelimited(
+				&client->dev,
+				"Failed to write reg 0x%4.4x. error = %d\n",
+				regs[i].address, ret);
+
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int ov5670_write_reg_list(struct ov5670 *ov5670,
+				 const struct ov5670_reg_list *r_list)
+{
+	return ov5670_write_regs(ov5670, r_list->regs, r_list->num_of_regs);
+}
+
+static int ov5670_update_digital_gain(struct ov5670 *ov5670, u32 d_gain)
+{
+	int ret;
+
+	ret = ov5670_write_reg(ov5670, OV5670_REG_R_DGTL_GAIN,
+			       OV5670_REG_VALUE_16BIT, d_gain);
+	if (ret)
+		return ret;
+
+	ret = ov5670_write_reg(ov5670, OV5670_REG_G_DGTL_GAIN,
+			       OV5670_REG_VALUE_16BIT, d_gain);
+	if (ret)
+		return ret;
+
+	return ov5670_write_reg(ov5670, OV5670_REG_B_DGTL_GAIN,
+				OV5670_REG_VALUE_16BIT, d_gain);
+}
+
+static int ov5670_enable_test_pattern(struct ov5670 *ov5670, u32 pattern)
+{
+	u32 val;
+	int ret;
+
+	/* Set the bayer order that we support */
+	ret = ov5670_write_reg(ov5670, OV5670_REG_TEST_PATTERN_CTRL,
+			       OV5670_REG_VALUE_08BIT, 0);
+	if (ret)
+		return ret;
+
+	ret = ov5670_read_reg(ov5670, OV5670_REG_TEST_PATTERN,
+			      OV5670_REG_VALUE_08BIT, &val);
+	if (ret)
+		return ret;
+
+	if (pattern)
+		val |= OV5670_TEST_PATTERN_ENABLE;
+	else
+		val &= ~OV5670_TEST_PATTERN_ENABLE;
+
+	return ov5670_write_reg(ov5670, OV5670_REG_TEST_PATTERN,
+				OV5670_REG_VALUE_08BIT, val);
+}
+
+/* Initialize control handlers */
+static int ov5670_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov5670 *ov5670 = container_of(ctrl->handler,
+					     struct ov5670, ctrl_handler);
+	struct i2c_client *client = v4l2_get_subdevdata(&ov5670->sd);
+	s64 max;
+	int ret;
+
+	/* Propagate change of current control to all related controls */
+	switch (ctrl->id) {
+	case V4L2_CID_VBLANK:
+		/* Update max exposure while meeting expected vblanking */
+		max = ov5670->cur_mode->height + ctrl->val - 8;
+		__v4l2_ctrl_modify_range(ov5670->exposure,
+					 ov5670->exposure->minimum, max,
+					 ov5670->exposure->step, max);
+		break;
+	}
+
+	/* V4L2 controls values will be applied only when power is already up */
+	if (!pm_runtime_get_if_in_use(&client->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_ANALOGUE_GAIN:
+		ret = ov5670_write_reg(ov5670, OV5670_REG_ANALOG_GAIN,
+				       OV5670_REG_VALUE_16BIT, ctrl->val);
+		break;
+	case V4L2_CID_DIGITAL_GAIN:
+		ret = ov5670_update_digital_gain(ov5670, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE:
+		/* 4 least significant bits of expsoure are fractional part */
+		ret = ov5670_write_reg(ov5670, OV5670_REG_EXPOSURE,
+				       OV5670_REG_VALUE_24BIT, ctrl->val << 4);
+		break;
+	case V4L2_CID_VBLANK:
+		/* Update VTS that meets expected vertical blanking */
+		ret = ov5670_write_reg(ov5670, OV5670_REG_VTS,
+				       OV5670_REG_VALUE_16BIT,
+				       ov5670->cur_mode->height + ctrl->val);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		ret = ov5670_enable_test_pattern(ov5670, ctrl->val);
+		break;
+	case V4L2_CID_HBLANK:
+	case V4L2_CID_LINK_FREQ:
+	case V4L2_CID_PIXEL_RATE:
+		ret = 0;
+		break;
+	default:
+		ret = -EINVAL;
+		dev_info(&client->dev, "%s Unhandled id:0x%x, val:0x%x\n",
+			 __func__, ctrl->id, ctrl->val);
+		break;
+	}
+
+	pm_runtime_put(&client->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ov5670_ctrl_ops = {
+	.s_ctrl = ov5670_set_ctrl,
+};
+
+/* Initialize control handlers */
+static int ov5670_init_controls(struct ov5670 *ov5670)
+{
+	struct v4l2_mbus_config_mipi_csi2 *bus_mipi_csi2 =
+		&ov5670->endpoint.bus.mipi_csi2;
+	struct i2c_client *client = v4l2_get_subdevdata(&ov5670->sd);
+	struct v4l2_fwnode_device_properties props;
+	struct v4l2_ctrl_handler *ctrl_hdlr;
+	unsigned int lanes_count;
+	s64 mipi_pixel_rate;
+	s64 vblank_max;
+	s64 vblank_def;
+	s64 vblank_min;
+	s64 exposure_max;
+	int ret;
+
+	ctrl_hdlr = &ov5670->ctrl_handler;
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 10);
+	if (ret)
+		return ret;
+
+	ctrl_hdlr->lock = &ov5670->mutex;
+	ov5670->link_freq = v4l2_ctrl_new_int_menu(ctrl_hdlr,
+						   &ov5670_ctrl_ops,
+						   V4L2_CID_LINK_FREQ,
+						   0, 0, link_freq_menu_items);
+	if (ov5670->link_freq)
+		ov5670->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	/* By default, V4L2_CID_PIXEL_RATE is read only */
+	lanes_count = bus_mipi_csi2->num_data_lanes;
+	mipi_pixel_rate = OV5670_LINK_FREQ_422MHZ * 2 * lanes_count / 10;
+
+	ov5670->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, &ov5670_ctrl_ops,
+					       V4L2_CID_PIXEL_RATE,
+					       mipi_pixel_rate,
+					       mipi_pixel_rate,
+					       1,
+					       mipi_pixel_rate);
+
+	vblank_max = OV5670_VTS_MAX - ov5670->cur_mode->height;
+	vblank_def = ov5670->cur_mode->vts_def - ov5670->cur_mode->height;
+	vblank_min = ov5670->cur_mode->vts_min - ov5670->cur_mode->height;
+	ov5670->vblank = v4l2_ctrl_new_std(ctrl_hdlr, &ov5670_ctrl_ops,
+					   V4L2_CID_VBLANK, vblank_min,
+					   vblank_max, 1, vblank_def);
+
+	ov5670->hblank = v4l2_ctrl_new_std(
+				ctrl_hdlr, &ov5670_ctrl_ops, V4L2_CID_HBLANK,
+				OV5670_FIXED_PPL - ov5670->cur_mode->width,
+				OV5670_FIXED_PPL - ov5670->cur_mode->width, 1,
+				OV5670_FIXED_PPL - ov5670->cur_mode->width);
+	if (ov5670->hblank)
+		ov5670->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	/* Get min, max, step, default from sensor */
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov5670_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
+			  ANALOG_GAIN_MIN, ANALOG_GAIN_MAX, ANALOG_GAIN_STEP,
+			  ANALOG_GAIN_DEFAULT);
+
+	/* Digital gain */
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov5670_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
+			  OV5670_DGTL_GAIN_MIN, OV5670_DGTL_GAIN_MAX,
+			  OV5670_DGTL_GAIN_STEP, OV5670_DGTL_GAIN_DEFAULT);
+
+	/* Get min, max, step, default from sensor */
+	exposure_max = ov5670->cur_mode->vts_def - 8;
+	ov5670->exposure = v4l2_ctrl_new_std(ctrl_hdlr, &ov5670_ctrl_ops,
+					     V4L2_CID_EXPOSURE,
+					     OV5670_EXPOSURE_MIN,
+					     exposure_max, OV5670_EXPOSURE_STEP,
+					     exposure_max);
+
+	v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &ov5670_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(ov5670_test_pattern_menu) - 1,
+				     0, 0, ov5670_test_pattern_menu);
+
+	if (ctrl_hdlr->error) {
+		ret = ctrl_hdlr->error;
+		goto error;
+	}
+
+	ret = v4l2_fwnode_device_parse(&client->dev, &props);
+	if (ret)
+		goto error;
+
+	ret = v4l2_ctrl_new_fwnode_properties(ctrl_hdlr, &ov5670_ctrl_ops,
+					      &props);
+	if (ret)
+		goto error;
+
+	ov5670->sd.ctrl_handler = ctrl_hdlr;
+
+	return 0;
+
+error:
+	v4l2_ctrl_handler_free(ctrl_hdlr);
+
+	return ret;
+}
+
+static int ov5670_init_cfg(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_state *state)
+{
+	struct v4l2_mbus_framefmt *fmt =
+				v4l2_subdev_get_try_format(sd, state, 0);
+	const struct ov5670_mode *default_mode = &supported_modes[0];
+	struct v4l2_rect *crop = v4l2_subdev_get_try_crop(sd, state, 0);
+
+	fmt->width = default_mode->width;
+	fmt->height = default_mode->height;
+	fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	fmt->field = V4L2_FIELD_NONE;
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+	fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(V4L2_COLORSPACE_SRGB);
+	fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
+	fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(V4L2_COLORSPACE_SRGB);
+
+	*crop = *default_mode->analog_crop;
+
+	return 0;
+}
+
+static int ov5670_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	/* Only one bayer order GRBG is supported */
+	if (code->index > 0)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+
+	return 0;
+}
+
+static int ov5670_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->index >= ARRAY_SIZE(supported_modes))
+		return -EINVAL;
+
+	if (fse->code != MEDIA_BUS_FMT_SGRBG10_1X10)
+		return -EINVAL;
+
+	fse->min_width = supported_modes[fse->index].width;
+	fse->max_width = fse->min_width;
+	fse->min_height = supported_modes[fse->index].height;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static void ov5670_update_pad_format(const struct ov5670_mode *mode,
+				     struct v4l2_subdev_format *fmt)
+{
+	fmt->format.width = mode->width;
+	fmt->format.height = mode->height;
+	fmt->format.code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	fmt->format.field = V4L2_FIELD_NONE;
+}
+
+static int ov5670_do_get_pad_format(struct ov5670 *ov5670,
+				    struct v4l2_subdev_state *sd_state,
+				    struct v4l2_subdev_format *fmt)
+{
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
+		fmt->format = *v4l2_subdev_get_try_format(&ov5670->sd,
+							  sd_state,
+							  fmt->pad);
+	else
+		ov5670_update_pad_format(ov5670->cur_mode, fmt);
+
+	return 0;
+}
+
+static int ov5670_get_pad_format(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_format *fmt)
+{
+	struct ov5670 *ov5670 = to_ov5670(sd);
+	int ret;
+
+	mutex_lock(&ov5670->mutex);
+	ret = ov5670_do_get_pad_format(ov5670, sd_state, fmt);
+	mutex_unlock(&ov5670->mutex);
+
+	return ret;
+}
+
+static int ov5670_set_pad_format(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_format *fmt)
+{
+	struct ov5670 *ov5670 = to_ov5670(sd);
+	struct v4l2_mbus_config_mipi_csi2 *bus_mipi_csi2 =
+		&ov5670->endpoint.bus.mipi_csi2;
+	const struct ov5670_mode *mode;
+	unsigned int lanes_count;
+	s64 mipi_pixel_rate;
+	s32 vblank_def;
+	s64 link_freq;
+	s32 h_blank;
+
+	mutex_lock(&ov5670->mutex);
+
+	fmt->format.code = MEDIA_BUS_FMT_SGRBG10_1X10;
+
+	mode = v4l2_find_nearest_size(supported_modes,
+				      ARRAY_SIZE(supported_modes),
+				      width, height,
+				      fmt->format.width, fmt->format.height);
+	ov5670_update_pad_format(mode, fmt);
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		*v4l2_subdev_get_try_format(sd, sd_state, fmt->pad) = fmt->format;
+	} else {
+		ov5670->cur_mode = mode;
+		__v4l2_ctrl_s_ctrl(ov5670->link_freq, mode->link_freq_index);
+
+		lanes_count = bus_mipi_csi2->num_data_lanes;
+		link_freq = link_freq_menu_items[mode->link_freq_index];
+		/* pixel_rate = link_freq * 2 * nr_of_lanes / bits_per_sample */
+		mipi_pixel_rate = div_s64(link_freq * 2 * lanes_count, 10);
+		__v4l2_ctrl_s_ctrl_int64(
+			ov5670->pixel_rate,
+			mipi_pixel_rate);
+		/* Update limits and set FPS to default */
+		vblank_def = ov5670->cur_mode->vts_def -
+			     ov5670->cur_mode->height;
+		__v4l2_ctrl_modify_range(
+			ov5670->vblank,
+			ov5670->cur_mode->vts_min - ov5670->cur_mode->height,
+			OV5670_VTS_MAX - ov5670->cur_mode->height, 1,
+			vblank_def);
+		__v4l2_ctrl_s_ctrl(ov5670->vblank, vblank_def);
+		h_blank = OV5670_FIXED_PPL - ov5670->cur_mode->width;
+		__v4l2_ctrl_modify_range(ov5670->hblank, h_blank, h_blank, 1,
+					 h_blank);
+	}
+
+	mutex_unlock(&ov5670->mutex);
+
+	return 0;
+}
+
+static int ov5670_get_skip_frames(struct v4l2_subdev *sd, u32 *frames)
+{
+	*frames = OV5670_NUM_OF_SKIP_FRAMES;
+
+	return 0;
+}
+
+/* Verify chip ID */
+static int ov5670_identify_module(struct ov5670 *ov5670)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov5670->sd);
+	int ret;
+	u32 val;
+
+	if (ov5670->identified)
+		return 0;
+
+	ret = ov5670_read_reg(ov5670, OV5670_REG_CHIP_ID,
+			      OV5670_REG_VALUE_24BIT, &val);
+	if (ret)
+		return ret;
+
+	if (val != OV5670_CHIP_ID) {
+		dev_err(&client->dev, "chip id mismatch: %x!=%x\n",
+			OV5670_CHIP_ID, val);
+		return -ENXIO;
+	}
+
+	ov5670->identified = true;
+
+	return 0;
+}
+
+static int ov5670_mipi_configure(struct ov5670 *ov5670)
+{
+	struct v4l2_mbus_config_mipi_csi2 *bus_mipi_csi2 =
+		&ov5670->endpoint.bus.mipi_csi2;
+	unsigned int lanes_count = bus_mipi_csi2->num_data_lanes;
+
+	return ov5670_write_reg(ov5670, OV5670_MIPI_SC_CTRL0_REG,
+				OV5670_REG_VALUE_08BIT,
+				OV5670_MIPI_SC_CTRL0_LANES(lanes_count) |
+				OV5670_MIPI_SC_CTRL0_MIPI_EN |
+				OV5670_MIPI_SC_CTRL0_RESERVED);
+}
+
+/* Prepare streaming by writing default values and customized values */
+static int ov5670_start_streaming(struct ov5670 *ov5670)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov5670->sd);
+	const struct ov5670_reg_list *reg_list;
+	int link_freq_index;
+	int ret;
+
+	ret = ov5670_identify_module(ov5670);
+	if (ret)
+		return ret;
+
+	/* Get out of from software reset */
+	ret = ov5670_write_reg(ov5670, OV5670_REG_SOFTWARE_RST,
+			       OV5670_REG_VALUE_08BIT, OV5670_SOFTWARE_RST);
+	if (ret) {
+		dev_err(&client->dev, "%s failed to set powerup registers\n",
+			__func__);
+		return ret;
+	}
+
+	/* Setup PLL */
+	link_freq_index = ov5670->cur_mode->link_freq_index;
+	reg_list = &link_freq_configs[link_freq_index].reg_list;
+	ret = ov5670_write_reg_list(ov5670, reg_list);
+	if (ret) {
+		dev_err(&client->dev, "%s failed to set plls\n", __func__);
+		return ret;
+	}
+
+	/* Apply default values of current mode */
+	reg_list = &ov5670->cur_mode->reg_list;
+	ret = ov5670_write_reg_list(ov5670, reg_list);
+	if (ret) {
+		dev_err(&client->dev, "%s failed to set mode\n", __func__);
+		return ret;
+	}
+
+	ret = ov5670_mipi_configure(ov5670);
+	if (ret) {
+		dev_err(&client->dev, "%s failed to configure MIPI\n", __func__);
+		return ret;
+	}
+
+	ret = __v4l2_ctrl_handler_setup(ov5670->sd.ctrl_handler);
+	if (ret)
+		return ret;
+
+	/* Write stream on list */
+	ret = ov5670_write_reg(ov5670, OV5670_REG_MODE_SELECT,
+			       OV5670_REG_VALUE_08BIT, OV5670_MODE_STREAMING);
+	if (ret) {
+		dev_err(&client->dev, "%s failed to set stream\n", __func__);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ov5670_stop_streaming(struct ov5670 *ov5670)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov5670->sd);
+	int ret;
+
+	ret = ov5670_write_reg(ov5670, OV5670_REG_MODE_SELECT,
+			       OV5670_REG_VALUE_08BIT, OV5670_MODE_STANDBY);
+	if (ret)
+		dev_err(&client->dev, "%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 ov5670_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct ov5670 *ov5670 = to_ov5670(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	mutex_lock(&ov5670->mutex);
+	if (ov5670->streaming == enable)
+		goto unlock_and_return;
+
+	if (enable) {
+		ret = pm_runtime_resume_and_get(&client->dev);
+		if (ret < 0)
+			goto unlock_and_return;
+
+		ret = ov5670_start_streaming(ov5670);
+		if (ret)
+			goto error;
+	} else {
+		ret = ov5670_stop_streaming(ov5670);
+		pm_runtime_put(&client->dev);
+	}
+	ov5670->streaming = enable;
+	goto unlock_and_return;
+
+error:
+	pm_runtime_put(&client->dev);
+
+unlock_and_return:
+	mutex_unlock(&ov5670->mutex);
+
+	return ret;
+}
+
+static int __maybe_unused ov5670_runtime_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov5670 *ov5670 = to_ov5670(sd);
+	unsigned long delay_us;
+	int ret;
+
+	ret = clk_prepare_enable(ov5670->xvclk);
+	if (ret)
+		return ret;
+
+	ret = regulator_bulk_enable(OV5670_NUM_SUPPLIES, ov5670->supplies);
+	if (ret) {
+		clk_disable_unprepare(ov5670->xvclk);
+		return ret;
+	}
+
+	gpiod_set_value_cansleep(ov5670->pwdn_gpio, 0);
+	gpiod_set_value_cansleep(ov5670->reset_gpio, 0);
+
+	/* 8192 * 2 clock pulses before the first SCCB transaction. */
+	delay_us = DIV_ROUND_UP(8192 * 2 * 1000,
+				DIV_ROUND_UP(OV5670_XVCLK_FREQ, 1000));
+	fsleep(delay_us);
+
+	return 0;
+}
+
+static int __maybe_unused ov5670_runtime_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov5670 *ov5670 = to_ov5670(sd);
+
+	gpiod_set_value_cansleep(ov5670->reset_gpio, 1);
+	gpiod_set_value_cansleep(ov5670->pwdn_gpio, 1);
+	regulator_bulk_disable(OV5670_NUM_SUPPLIES, ov5670->supplies);
+	clk_disable_unprepare(ov5670->xvclk);
+
+	return 0;
+}
+
+static int __maybe_unused ov5670_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov5670 *ov5670 = to_ov5670(sd);
+
+	if (ov5670->streaming)
+		ov5670_stop_streaming(ov5670);
+
+	return 0;
+}
+
+static int __maybe_unused ov5670_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov5670 *ov5670 = to_ov5670(sd);
+	int ret;
+
+	if (ov5670->streaming) {
+		ret = ov5670_start_streaming(ov5670);
+		if (ret) {
+			ov5670_stop_streaming(ov5670);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops ov5670_core_ops = {
+	.log_status = v4l2_ctrl_subdev_log_status,
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_rect *
+__ov5670_get_pad_crop(struct ov5670 *sensor, struct v4l2_subdev_state *state,
+		      unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+	const struct ov5670_mode *mode = sensor->cur_mode;
+
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_crop(&sensor->sd, state, pad);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return mode->analog_crop;
+	}
+
+	return NULL;
+}
+
+static int ov5670_get_selection(struct v4l2_subdev *subdev,
+				struct v4l2_subdev_state *state,
+				struct v4l2_subdev_selection *sel)
+{
+	struct ov5670 *sensor = to_ov5670(subdev);
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		mutex_lock(&sensor->mutex);
+		sel->r = *__ov5670_get_pad_crop(sensor, state, sel->pad,
+						sel->which);
+		mutex_unlock(&sensor->mutex);
+		break;
+	case V4L2_SEL_TGT_NATIVE_SIZE:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.top = 0;
+		sel->r.left = 0;
+		sel->r.width = OV5670_NATIVE_WIDTH;
+		sel->r.height = OV5670_NATIVE_HEIGHT;
+		break;
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		sel->r = ov5670_analog_crop;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops ov5670_video_ops = {
+	.s_stream = ov5670_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops ov5670_pad_ops = {
+	.init_cfg = ov5670_init_cfg,
+	.enum_mbus_code = ov5670_enum_mbus_code,
+	.get_fmt = ov5670_get_pad_format,
+	.set_fmt = ov5670_set_pad_format,
+	.enum_frame_size = ov5670_enum_frame_size,
+	.get_selection = ov5670_get_selection,
+	.set_selection = ov5670_get_selection,
+};
+
+static const struct v4l2_subdev_sensor_ops ov5670_sensor_ops = {
+	.g_skip_frames = ov5670_get_skip_frames,
+};
+
+static const struct v4l2_subdev_ops ov5670_subdev_ops = {
+	.core = &ov5670_core_ops,
+	.video = &ov5670_video_ops,
+	.pad = &ov5670_pad_ops,
+	.sensor = &ov5670_sensor_ops,
+};
+
+static const struct media_entity_operations ov5670_subdev_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static int ov5670_regulators_probe(struct ov5670 *ov5670)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov5670->sd);
+	unsigned int i;
+
+	for (i = 0; i < OV5670_NUM_SUPPLIES; i++)
+		ov5670->supplies[i].supply = ov5670_supply_names[i];
+
+	return devm_regulator_bulk_get(&client->dev, OV5670_NUM_SUPPLIES,
+				       ov5670->supplies);
+}
+
+static int ov5670_gpio_probe(struct ov5670 *ov5670)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov5670->sd);
+
+	ov5670->pwdn_gpio = devm_gpiod_get_optional(&client->dev, "powerdown",
+						    GPIOD_OUT_LOW);
+	if (IS_ERR(ov5670->pwdn_gpio))
+		return PTR_ERR(ov5670->pwdn_gpio);
+
+	ov5670->reset_gpio = devm_gpiod_get_optional(&client->dev, "reset",
+						     GPIOD_OUT_LOW);
+	if (IS_ERR(ov5670->reset_gpio))
+		return PTR_ERR(ov5670->reset_gpio);
+
+	return 0;
+}
+
+static int ov5670_probe(struct i2c_client *client)
+{
+	struct fwnode_handle *handle;
+	struct ov5670 *ov5670;
+	u32 input_clk = 0;
+	bool full_power;
+	int ret;
+
+	ov5670 = devm_kzalloc(&client->dev, sizeof(*ov5670), GFP_KERNEL);
+	if (!ov5670)
+		return -ENOMEM;
+
+	ov5670->xvclk = devm_clk_get_optional(&client->dev, NULL);
+	if (!IS_ERR_OR_NULL(ov5670->xvclk))
+		input_clk = clk_get_rate(ov5670->xvclk);
+	else if (!ov5670->xvclk || PTR_ERR(ov5670->xvclk) == -ENOENT)
+		device_property_read_u32(&client->dev, "clock-frequency",
+					 &input_clk);
+	else
+		return dev_err_probe(&client->dev, PTR_ERR(ov5670->xvclk),
+				     "error getting clock\n");
+
+	if (input_clk != OV5670_XVCLK_FREQ) {
+		dev_err(&client->dev,
+			"Unsupported clock frequency %u\n", input_clk);
+		return -EINVAL;
+	}
+
+	/* Initialize subdev */
+	v4l2_i2c_subdev_init(&ov5670->sd, client, &ov5670_subdev_ops);
+
+	ret = ov5670_regulators_probe(ov5670);
+	if (ret)
+		return dev_err_probe(&client->dev, ret, "Regulators probe failed\n");
+
+	ret = ov5670_gpio_probe(ov5670);
+	if (ret)
+		return dev_err_probe(&client->dev, ret, "GPIO probe failed\n");
+
+	/* Graph Endpoint */
+	handle = fwnode_graph_get_next_endpoint(dev_fwnode(&client->dev), NULL);
+	if (!handle)
+		return dev_err_probe(&client->dev, -ENXIO, "Endpoint for node get failed\n");
+
+	ov5670->endpoint.bus_type = V4L2_MBUS_CSI2_DPHY;
+	ov5670->endpoint.bus.mipi_csi2.num_data_lanes = 2;
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(handle, &ov5670->endpoint);
+	fwnode_handle_put(handle);
+	if (ret)
+		return dev_err_probe(&client->dev, ret, "Endpoint parse failed\n");
+
+	full_power = acpi_dev_state_d0(&client->dev);
+	if (full_power) {
+		ret = ov5670_runtime_resume(&client->dev);
+		if (ret) {
+			dev_err_probe(&client->dev, ret, "Power up failed\n");
+			goto error_endpoint;
+		}
+
+		/* Check module identity */
+		ret = ov5670_identify_module(ov5670);
+		if (ret) {
+			dev_err_probe(&client->dev, ret, "ov5670_identify_module() error\n");
+			goto error_power_off;
+		}
+	}
+
+	mutex_init(&ov5670->mutex);
+
+	/* Set default mode to max resolution */
+	ov5670->cur_mode = &supported_modes[0];
+
+	ret = ov5670_init_controls(ov5670);
+	if (ret) {
+		dev_err_probe(&client->dev, ret, "ov5670_init_controls() error\n");
+		goto error_mutex_destroy;
+	}
+
+	ov5670->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+			    V4L2_SUBDEV_FL_HAS_EVENTS;
+	ov5670->sd.entity.ops = &ov5670_subdev_entity_ops;
+	ov5670->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+	/* Source pad initialization */
+	ov5670->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&ov5670->sd.entity, 1, &ov5670->pad);
+	if (ret) {
+		dev_err_probe(&client->dev, ret, "media_entity_pads_init() error\n");
+		goto error_handler_free;
+	}
+
+	ov5670->streaming = false;
+
+	/* Set the device's state to active if it's in D0 state. */
+	if (full_power)
+		pm_runtime_set_active(&client->dev);
+	pm_runtime_enable(&client->dev);
+
+	/* Async register for subdev */
+	ret = v4l2_async_register_subdev_sensor(&ov5670->sd);
+	if (ret < 0) {
+		dev_err_probe(&client->dev, ret, "v4l2_async_register_subdev() error\n");
+		goto error_pm_disable;
+	}
+
+	pm_runtime_idle(&client->dev);
+
+	return 0;
+
+error_pm_disable:
+	pm_runtime_disable(&client->dev);
+
+	media_entity_cleanup(&ov5670->sd.entity);
+
+error_handler_free:
+	v4l2_ctrl_handler_free(ov5670->sd.ctrl_handler);
+
+error_mutex_destroy:
+	mutex_destroy(&ov5670->mutex);
+
+error_power_off:
+	if (full_power)
+		ov5670_runtime_suspend(&client->dev);
+
+error_endpoint:
+	v4l2_fwnode_endpoint_free(&ov5670->endpoint);
+
+	return ret;
+}
+
+static void ov5670_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov5670 *ov5670 = to_ov5670(sd);
+
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+	mutex_destroy(&ov5670->mutex);
+
+	pm_runtime_disable(&client->dev);
+	ov5670_runtime_suspend(&client->dev);
+
+	v4l2_fwnode_endpoint_free(&ov5670->endpoint);
+}
+
+static const struct dev_pm_ops ov5670_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(ov5670_suspend, ov5670_resume)
+	SET_RUNTIME_PM_OPS(ov5670_runtime_suspend, ov5670_runtime_resume, NULL)
+};
+
+#ifdef CONFIG_ACPI
+static const struct acpi_device_id ov5670_acpi_ids[] = {
+	{ "INT3479" },
+	{ /* sentinel */ }
+};
+
+MODULE_DEVICE_TABLE(acpi, ov5670_acpi_ids);
+#endif
+
+static const struct of_device_id ov5670_of_ids[] = {
+	{ .compatible = "ovti,ov5670" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, ov5670_of_ids);
+
+static struct i2c_driver ov5670_i2c_driver = {
+	.driver = {
+		.name = "ov5670",
+		.pm = &ov5670_pm_ops,
+		.acpi_match_table = ACPI_PTR(ov5670_acpi_ids),
+		.of_match_table = ov5670_of_ids,
+	},
+	.probe = ov5670_probe,
+	.remove = ov5670_remove,
+	.flags = I2C_DRV_ACPI_WAIVE_D0_PROBE,
+};
+
+module_i2c_driver(ov5670_i2c_driver);
+
+MODULE_AUTHOR("Rapolu, Chiranjeevi");
+MODULE_AUTHOR("Yang, Hyungwoo");
+MODULE_DESCRIPTION("Omnivision ov5670 sensor driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/ov5675.c b/drivers/media/i2c/ov5675.c
new file mode 100644
index 000000000..d5a2a5f82
--- /dev/null
+++ b/drivers/media/i2c/ov5675.c
@@ -0,0 +1,1447 @@
+// SPDX-License-Identifier: GPL-2.0
+// Copyright (c) 2019 Intel Corporation.
+
+#include <asm/unaligned.h>
+#include <linux/acpi.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/mod_devicetable.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-fwnode.h>
+
+#define OV5675_REG_VALUE_08BIT		1
+#define OV5675_REG_VALUE_16BIT		2
+#define OV5675_REG_VALUE_24BIT		3
+
+#define OV5675_LINK_FREQ_450MHZ		450000000ULL
+#define OV5675_SCLK			90000000LL
+#define OV5675_XVCLK_19_2		19200000
+#define OV5675_DATA_LANES		2
+#define OV5675_RGB_DEPTH		10
+
+#define OV5675_REG_CHIP_ID		0x300a
+#define OV5675_CHIP_ID			0x5675
+
+#define OV5675_REG_MODE_SELECT		0x0100
+#define OV5675_MODE_STANDBY		0x00
+#define OV5675_MODE_STREAMING		0x01
+
+/* vertical-timings from sensor */
+#define OV5675_REG_VTS			0x380e
+#define OV5675_VTS_30FPS		0x07e4
+#define OV5675_VTS_30FPS_MIN		0x07e4
+#define OV5675_VTS_MAX			0x7fff
+
+/* horizontal-timings from sensor */
+#define OV5675_REG_HTS			0x380c
+
+/* Exposure controls from sensor */
+#define OV5675_REG_EXPOSURE		0x3500
+#define	OV5675_EXPOSURE_MIN		4
+#define OV5675_EXPOSURE_MAX_MARGIN	4
+#define	OV5675_EXPOSURE_STEP		1
+
+/* Analog gain controls from sensor */
+#define OV5675_REG_ANALOG_GAIN		0x3508
+#define	OV5675_ANAL_GAIN_MIN		128
+#define	OV5675_ANAL_GAIN_MAX		2047
+#define	OV5675_ANAL_GAIN_STEP		1
+
+/* Digital gain controls from sensor */
+#define OV5675_REG_DIGITAL_GAIN		0x350a
+#define OV5675_REG_MWB_R_GAIN		0x5019
+#define OV5675_REG_MWB_G_GAIN		0x501b
+#define OV5675_REG_MWB_B_GAIN		0x501d
+#define OV5675_DGTL_GAIN_MIN		1024
+#define OV5675_DGTL_GAIN_MAX		4095
+#define OV5675_DGTL_GAIN_STEP		1
+#define OV5675_DGTL_GAIN_DEFAULT	1024
+
+/* Group Access */
+#define OV5675_REG_GROUP_ACCESS		0x3208
+#define OV5675_GROUP_HOLD_START		0x0
+#define OV5675_GROUP_HOLD_END		0x10
+#define OV5675_GROUP_HOLD_LAUNCH	0xa0
+
+/* Test Pattern Control */
+#define OV5675_REG_TEST_PATTERN		0x4503
+#define OV5675_TEST_PATTERN_ENABLE	BIT(7)
+#define OV5675_TEST_PATTERN_BAR_SHIFT	2
+
+/* Flip Mirror Controls from sensor */
+#define OV5675_REG_FORMAT1		0x3820
+#define OV5675_REG_FORMAT2		0x373d
+
+#define to_ov5675(_sd)			container_of(_sd, struct ov5675, sd)
+
+static const char * const ov5675_supply_names[] = {
+	"avdd",		/* Analog power */
+	"dovdd",	/* Digital I/O power */
+	"dvdd",		/* Digital core power */
+};
+
+#define OV5675_NUM_SUPPLIES	ARRAY_SIZE(ov5675_supply_names)
+
+enum {
+	OV5675_LINK_FREQ_900MBPS,
+};
+
+struct ov5675_reg {
+	u16 address;
+	u8 val;
+};
+
+struct ov5675_reg_list {
+	u32 num_of_regs;
+	const struct ov5675_reg *regs;
+};
+
+struct ov5675_link_freq_config {
+	const struct ov5675_reg_list reg_list;
+};
+
+struct ov5675_mode {
+	/* Frame width in pixels */
+	u32 width;
+
+	/* Frame height in pixels */
+	u32 height;
+
+	/* Horizontal timining size */
+	u32 hts;
+
+	/* Default vertical timining size */
+	u32 vts_def;
+
+	/* Min vertical timining size */
+	u32 vts_min;
+
+	/* Link frequency needed for this resolution */
+	u32 link_freq_index;
+
+	/* Sensor register settings for this resolution */
+	const struct ov5675_reg_list reg_list;
+};
+
+static const struct ov5675_reg mipi_data_rate_900mbps[] = {
+	{0x0103, 0x01},
+	{0x0100, 0x00},
+	{0x0300, 0x04},
+	{0x0302, 0x8d},
+	{0x0303, 0x00},
+	{0x030d, 0x26},
+};
+
+static const struct ov5675_reg mode_2592x1944_regs[] = {
+	{0x3002, 0x21},
+	{0x3107, 0x23},
+	{0x3501, 0x20},
+	{0x3503, 0x0c},
+	{0x3508, 0x03},
+	{0x3509, 0x00},
+	{0x3600, 0x66},
+	{0x3602, 0x30},
+	{0x3610, 0xa5},
+	{0x3612, 0x93},
+	{0x3620, 0x80},
+	{0x3642, 0x0e},
+	{0x3661, 0x00},
+	{0x3662, 0x10},
+	{0x3664, 0xf3},
+	{0x3665, 0x9e},
+	{0x3667, 0xa5},
+	{0x366e, 0x55},
+	{0x366f, 0x55},
+	{0x3670, 0x11},
+	{0x3671, 0x11},
+	{0x3672, 0x11},
+	{0x3673, 0x11},
+	{0x3714, 0x24},
+	{0x371a, 0x3e},
+	{0x3733, 0x10},
+	{0x3734, 0x00},
+	{0x373d, 0x24},
+	{0x3764, 0x20},
+	{0x3765, 0x20},
+	{0x3766, 0x12},
+	{0x37a1, 0x14},
+	{0x37a8, 0x1c},
+	{0x37ab, 0x0f},
+	{0x37c2, 0x04},
+	{0x37cb, 0x00},
+	{0x37cc, 0x00},
+	{0x37cd, 0x00},
+	{0x37ce, 0x00},
+	{0x37d8, 0x02},
+	{0x37d9, 0x08},
+	{0x37dc, 0x04},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x04},
+	{0x3804, 0x0a},
+	{0x3805, 0x3f},
+	{0x3806, 0x07},
+	{0x3807, 0xb3},
+	{0x3808, 0x0a},
+	{0x3809, 0x20},
+	{0x380a, 0x07},
+	{0x380b, 0x98},
+	{0x380c, 0x02},
+	{0x380d, 0xee},
+	{0x380e, 0x07},
+	{0x380f, 0xe4},
+	{0x3811, 0x10},
+	{0x3813, 0x0d},
+	{0x3814, 0x01},
+	{0x3815, 0x01},
+	{0x3816, 0x01},
+	{0x3817, 0x01},
+	{0x381e, 0x02},
+	{0x3820, 0x88},
+	{0x3821, 0x01},
+	{0x3832, 0x04},
+	{0x3c80, 0x01},
+	{0x3c82, 0x00},
+	{0x3c83, 0xc8},
+	{0x3c8c, 0x0f},
+	{0x3c8d, 0xa0},
+	{0x3c90, 0x07},
+	{0x3c91, 0x00},
+	{0x3c92, 0x00},
+	{0x3c93, 0x00},
+	{0x3c94, 0xd0},
+	{0x3c95, 0x50},
+	{0x3c96, 0x35},
+	{0x3c97, 0x00},
+	{0x4001, 0xe0},
+	{0x4008, 0x02},
+	{0x4009, 0x0d},
+	{0x400f, 0x80},
+	{0x4013, 0x02},
+	{0x4040, 0x00},
+	{0x4041, 0x07},
+	{0x404c, 0x50},
+	{0x404e, 0x20},
+	{0x4500, 0x06},
+	{0x4503, 0x00},
+	{0x450a, 0x04},
+	{0x4809, 0x04},
+	{0x480c, 0x12},
+	{0x4819, 0x70},
+	{0x4825, 0x32},
+	{0x4826, 0x32},
+	{0x482a, 0x06},
+	{0x4833, 0x08},
+	{0x4837, 0x0d},
+	{0x5000, 0x77},
+	{0x5b00, 0x01},
+	{0x5b01, 0x10},
+	{0x5b02, 0x01},
+	{0x5b03, 0xdb},
+	{0x5b05, 0x6c},
+	{0x5e10, 0xfc},
+	{0x3500, 0x00},
+	{0x3501, 0x3E},
+	{0x3502, 0x60},
+	{0x3503, 0x08},
+	{0x3508, 0x04},
+	{0x3509, 0x00},
+	{0x3832, 0x48},
+	{0x5780, 0x3e},
+	{0x5781, 0x0f},
+	{0x5782, 0x44},
+	{0x5783, 0x02},
+	{0x5784, 0x01},
+	{0x5785, 0x01},
+	{0x5786, 0x00},
+	{0x5787, 0x04},
+	{0x5788, 0x02},
+	{0x5789, 0x0f},
+	{0x578a, 0xfd},
+	{0x578b, 0xf5},
+	{0x578c, 0xf5},
+	{0x578d, 0x03},
+	{0x578e, 0x08},
+	{0x578f, 0x0c},
+	{0x5790, 0x08},
+	{0x5791, 0x06},
+	{0x5792, 0x00},
+	{0x5793, 0x52},
+	{0x5794, 0xa3},
+	{0x4003, 0x40},
+	{0x3107, 0x01},
+	{0x3c80, 0x08},
+	{0x3c83, 0xb1},
+	{0x3c8c, 0x10},
+	{0x3c8d, 0x00},
+	{0x3c90, 0x00},
+	{0x3c94, 0x00},
+	{0x3c95, 0x00},
+	{0x3c96, 0x00},
+	{0x37cb, 0x09},
+	{0x37cc, 0x15},
+	{0x37cd, 0x1f},
+	{0x37ce, 0x1f},
+};
+
+static const struct ov5675_reg mode_1296x972_regs[] = {
+	{0x3002, 0x21},
+	{0x3107, 0x23},
+	{0x3501, 0x20},
+	{0x3503, 0x0c},
+	{0x3508, 0x03},
+	{0x3509, 0x00},
+	{0x3600, 0x66},
+	{0x3602, 0x30},
+	{0x3610, 0xa5},
+	{0x3612, 0x93},
+	{0x3620, 0x80},
+	{0x3642, 0x0e},
+	{0x3661, 0x00},
+	{0x3662, 0x08},
+	{0x3664, 0xf3},
+	{0x3665, 0x9e},
+	{0x3667, 0xa5},
+	{0x366e, 0x55},
+	{0x366f, 0x55},
+	{0x3670, 0x11},
+	{0x3671, 0x11},
+	{0x3672, 0x11},
+	{0x3673, 0x11},
+	{0x3714, 0x28},
+	{0x371a, 0x3e},
+	{0x3733, 0x10},
+	{0x3734, 0x00},
+	{0x373d, 0x24},
+	{0x3764, 0x20},
+	{0x3765, 0x20},
+	{0x3766, 0x12},
+	{0x37a1, 0x14},
+	{0x37a8, 0x1c},
+	{0x37ab, 0x0f},
+	{0x37c2, 0x14},
+	{0x37cb, 0x00},
+	{0x37cc, 0x00},
+	{0x37cd, 0x00},
+	{0x37ce, 0x00},
+	{0x37d8, 0x02},
+	{0x37d9, 0x04},
+	{0x37dc, 0x04},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x00},
+	{0x3804, 0x0a},
+	{0x3805, 0x3f},
+	{0x3806, 0x07},
+	{0x3807, 0xb7},
+	{0x3808, 0x05},
+	{0x3809, 0x10},
+	{0x380a, 0x03},
+	{0x380b, 0xcc},
+	{0x380c, 0x02},
+	{0x380d, 0xee},
+	{0x380e, 0x07},
+	{0x380f, 0xd0},
+	{0x3811, 0x08},
+	{0x3813, 0x0d},
+	{0x3814, 0x03},
+	{0x3815, 0x01},
+	{0x3816, 0x03},
+	{0x3817, 0x01},
+	{0x381e, 0x02},
+	{0x3820, 0x8b},
+	{0x3821, 0x01},
+	{0x3832, 0x04},
+	{0x3c80, 0x01},
+	{0x3c82, 0x00},
+	{0x3c83, 0xc8},
+	{0x3c8c, 0x0f},
+	{0x3c8d, 0xa0},
+	{0x3c90, 0x07},
+	{0x3c91, 0x00},
+	{0x3c92, 0x00},
+	{0x3c93, 0x00},
+	{0x3c94, 0xd0},
+	{0x3c95, 0x50},
+	{0x3c96, 0x35},
+	{0x3c97, 0x00},
+	{0x4001, 0xe0},
+	{0x4008, 0x00},
+	{0x4009, 0x07},
+	{0x400f, 0x80},
+	{0x4013, 0x02},
+	{0x4040, 0x00},
+	{0x4041, 0x03},
+	{0x404c, 0x50},
+	{0x404e, 0x20},
+	{0x4500, 0x06},
+	{0x4503, 0x00},
+	{0x450a, 0x04},
+	{0x4809, 0x04},
+	{0x480c, 0x12},
+	{0x4819, 0x70},
+	{0x4825, 0x32},
+	{0x4826, 0x32},
+	{0x482a, 0x06},
+	{0x4833, 0x08},
+	{0x4837, 0x0d},
+	{0x5000, 0x77},
+	{0x5b00, 0x01},
+	{0x5b01, 0x10},
+	{0x5b02, 0x01},
+	{0x5b03, 0xdb},
+	{0x5b05, 0x6c},
+	{0x5e10, 0xfc},
+	{0x3500, 0x00},
+	{0x3501, 0x1F},
+	{0x3502, 0x20},
+	{0x3503, 0x08},
+	{0x3508, 0x04},
+	{0x3509, 0x00},
+	{0x3832, 0x48},
+	{0x5780, 0x3e},
+	{0x5781, 0x0f},
+	{0x5782, 0x44},
+	{0x5783, 0x02},
+	{0x5784, 0x01},
+	{0x5785, 0x01},
+	{0x5786, 0x00},
+	{0x5787, 0x04},
+	{0x5788, 0x02},
+	{0x5789, 0x0f},
+	{0x578a, 0xfd},
+	{0x578b, 0xf5},
+	{0x578c, 0xf5},
+	{0x578d, 0x03},
+	{0x578e, 0x08},
+	{0x578f, 0x0c},
+	{0x5790, 0x08},
+	{0x5791, 0x06},
+	{0x5792, 0x00},
+	{0x5793, 0x52},
+	{0x5794, 0xa3},
+	{0x4003, 0x40},
+	{0x3107, 0x01},
+	{0x3c80, 0x08},
+	{0x3c83, 0xb1},
+	{0x3c8c, 0x10},
+	{0x3c8d, 0x00},
+	{0x3c90, 0x00},
+	{0x3c94, 0x00},
+	{0x3c95, 0x00},
+	{0x3c96, 0x00},
+	{0x37cb, 0x09},
+	{0x37cc, 0x15},
+	{0x37cd, 0x1f},
+	{0x37ce, 0x1f},
+};
+
+static const char * const ov5675_test_pattern_menu[] = {
+	"Disabled",
+	"Standard Color Bar",
+	"Top-Bottom Darker Color Bar",
+	"Right-Left Darker Color Bar",
+	"Bottom-Top Darker Color Bar"
+};
+
+static const s64 link_freq_menu_items[] = {
+	OV5675_LINK_FREQ_450MHZ,
+};
+
+static const struct ov5675_link_freq_config link_freq_configs[] = {
+	[OV5675_LINK_FREQ_900MBPS] = {
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mipi_data_rate_900mbps),
+			.regs = mipi_data_rate_900mbps,
+		}
+	}
+};
+
+static const struct ov5675_mode supported_modes[] = {
+	{
+		.width = 2592,
+		.height = 1944,
+		.hts = 1500,
+		.vts_def = OV5675_VTS_30FPS,
+		.vts_min = OV5675_VTS_30FPS_MIN,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_2592x1944_regs),
+			.regs = mode_2592x1944_regs,
+		},
+		.link_freq_index = OV5675_LINK_FREQ_900MBPS,
+	},
+	{
+		.width = 1296,
+		.height = 972,
+		.hts = 1500,
+		.vts_def = OV5675_VTS_30FPS,
+		.vts_min = OV5675_VTS_30FPS_MIN,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1296x972_regs),
+			.regs = mode_1296x972_regs,
+		},
+		.link_freq_index = OV5675_LINK_FREQ_900MBPS,
+	}
+};
+
+struct ov5675 {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct clk *xvclk;
+	struct gpio_desc *reset_gpio;
+	struct regulator_bulk_data supplies[OV5675_NUM_SUPPLIES];
+
+	/* V4L2 Controls */
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *pixel_rate;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *exposure;
+
+	/* Current mode */
+	const struct ov5675_mode *cur_mode;
+
+	/* To serialize asynchronus callbacks */
+	struct mutex mutex;
+
+	/* Streaming on/off */
+	bool streaming;
+
+	/* True if the device has been identified */
+	bool identified;
+};
+
+static u64 to_pixel_rate(u32 f_index)
+{
+	u64 pixel_rate = link_freq_menu_items[f_index] * 2 * OV5675_DATA_LANES;
+
+	do_div(pixel_rate, OV5675_RGB_DEPTH);
+
+	return pixel_rate;
+}
+
+static u64 to_pixels_per_line(u32 hts, u32 f_index)
+{
+	u64 ppl = hts * to_pixel_rate(f_index);
+
+	do_div(ppl, OV5675_SCLK);
+
+	return ppl;
+}
+
+static int ov5675_read_reg(struct ov5675 *ov5675, u16 reg, u16 len, u32 *val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov5675->sd);
+	struct i2c_msg msgs[2];
+	u8 addr_buf[2];
+	u8 data_buf[4] = {0};
+	int ret;
+
+	if (len > 4)
+		return -EINVAL;
+
+	put_unaligned_be16(reg, addr_buf);
+	msgs[0].addr = client->addr;
+	msgs[0].flags = 0;
+	msgs[0].len = sizeof(addr_buf);
+	msgs[0].buf = addr_buf;
+	msgs[1].addr = client->addr;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].len = len;
+	msgs[1].buf = &data_buf[4 - len];
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret != ARRAY_SIZE(msgs))
+		return -EIO;
+
+	*val = get_unaligned_be32(data_buf);
+
+	return 0;
+}
+
+static int ov5675_write_reg(struct ov5675 *ov5675, u16 reg, u16 len, u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov5675->sd);
+	u8 buf[6];
+
+	if (len > 4)
+		return -EINVAL;
+
+	put_unaligned_be16(reg, buf);
+	put_unaligned_be32(val << 8 * (4 - len), buf + 2);
+	if (i2c_master_send(client, buf, len + 2) != len + 2)
+		return -EIO;
+
+	return 0;
+}
+
+static int ov5675_write_reg_list(struct ov5675 *ov5675,
+				 const struct ov5675_reg_list *r_list)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov5675->sd);
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < r_list->num_of_regs; i++) {
+		ret = ov5675_write_reg(ov5675, r_list->regs[i].address, 1,
+				       r_list->regs[i].val);
+		if (ret) {
+			dev_err_ratelimited(&client->dev,
+				    "failed to write reg 0x%4.4x. error = %d",
+				    r_list->regs[i].address, ret);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int ov5675_update_digital_gain(struct ov5675 *ov5675, u32 d_gain)
+{
+	int ret;
+
+	ret = ov5675_write_reg(ov5675, OV5675_REG_GROUP_ACCESS,
+			       OV5675_REG_VALUE_08BIT,
+			       OV5675_GROUP_HOLD_START);
+	if (ret)
+		return ret;
+
+	ret = ov5675_write_reg(ov5675, OV5675_REG_MWB_R_GAIN,
+			       OV5675_REG_VALUE_16BIT, d_gain);
+	if (ret)
+		return ret;
+
+	ret = ov5675_write_reg(ov5675, OV5675_REG_MWB_G_GAIN,
+			       OV5675_REG_VALUE_16BIT, d_gain);
+	if (ret)
+		return ret;
+
+	ret = ov5675_write_reg(ov5675, OV5675_REG_MWB_B_GAIN,
+			       OV5675_REG_VALUE_16BIT, d_gain);
+	if (ret)
+		return ret;
+
+	ret = ov5675_write_reg(ov5675, OV5675_REG_GROUP_ACCESS,
+			       OV5675_REG_VALUE_08BIT,
+			       OV5675_GROUP_HOLD_END);
+	if (ret)
+		return ret;
+
+	ret = ov5675_write_reg(ov5675, OV5675_REG_GROUP_ACCESS,
+			       OV5675_REG_VALUE_08BIT,
+			       OV5675_GROUP_HOLD_LAUNCH);
+	return ret;
+}
+
+static int ov5675_test_pattern(struct ov5675 *ov5675, u32 pattern)
+{
+	if (pattern)
+		pattern = (pattern - 1) << OV5675_TEST_PATTERN_BAR_SHIFT |
+			  OV5675_TEST_PATTERN_ENABLE;
+
+	return ov5675_write_reg(ov5675, OV5675_REG_TEST_PATTERN,
+				OV5675_REG_VALUE_08BIT, pattern);
+}
+
+/*
+ * OV5675 supports keeping the pixel order by mirror and flip function
+ * The Bayer order isn't affected by the flip controls
+ */
+static int ov5675_set_ctrl_hflip(struct ov5675 *ov5675, u32 ctrl_val)
+{
+	int ret;
+	u32 val;
+
+	ret = ov5675_read_reg(ov5675, OV5675_REG_FORMAT1,
+			      OV5675_REG_VALUE_08BIT, &val);
+	if (ret)
+		return ret;
+
+	return ov5675_write_reg(ov5675, OV5675_REG_FORMAT1,
+				OV5675_REG_VALUE_08BIT,
+				ctrl_val ? val & ~BIT(3) : val | BIT(3));
+}
+
+static int ov5675_set_ctrl_vflip(struct ov5675 *ov5675, u8 ctrl_val)
+{
+	int ret;
+	u32 val;
+
+	ret = ov5675_read_reg(ov5675, OV5675_REG_FORMAT1,
+			      OV5675_REG_VALUE_08BIT, &val);
+	if (ret)
+		return ret;
+
+	ret = ov5675_write_reg(ov5675, OV5675_REG_FORMAT1,
+			       OV5675_REG_VALUE_08BIT,
+			       ctrl_val ? val | BIT(4) | BIT(5)  : val & ~BIT(4) & ~BIT(5));
+
+	if (ret)
+		return ret;
+
+	ret = ov5675_read_reg(ov5675, OV5675_REG_FORMAT2,
+			      OV5675_REG_VALUE_08BIT, &val);
+
+	if (ret)
+		return ret;
+
+	return ov5675_write_reg(ov5675, OV5675_REG_FORMAT2,
+				OV5675_REG_VALUE_08BIT,
+				ctrl_val ? val | BIT(1) : val & ~BIT(1));
+}
+
+static int ov5675_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov5675 *ov5675 = container_of(ctrl->handler,
+					     struct ov5675, ctrl_handler);
+	struct i2c_client *client = v4l2_get_subdevdata(&ov5675->sd);
+	s64 exposure_max;
+	int ret = 0;
+
+	/* Propagate change of current control to all related controls */
+	if (ctrl->id == V4L2_CID_VBLANK) {
+		/* Update max exposure while meeting expected vblanking */
+		exposure_max = ov5675->cur_mode->height + ctrl->val -
+			OV5675_EXPOSURE_MAX_MARGIN;
+		__v4l2_ctrl_modify_range(ov5675->exposure,
+					 ov5675->exposure->minimum,
+					 exposure_max, ov5675->exposure->step,
+					 exposure_max);
+	}
+
+	/* V4L2 controls values will be applied only when power is already up */
+	if (!pm_runtime_get_if_in_use(&client->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_ANALOGUE_GAIN:
+		ret = ov5675_write_reg(ov5675, OV5675_REG_ANALOG_GAIN,
+				       OV5675_REG_VALUE_16BIT, ctrl->val);
+		break;
+
+	case V4L2_CID_DIGITAL_GAIN:
+		ret = ov5675_update_digital_gain(ov5675, ctrl->val);
+		break;
+
+	case V4L2_CID_EXPOSURE:
+		/* 4 least significant bits of expsoure are fractional part
+		 * val = val << 4
+		 * for ov5675, the unit of exposure is differnt from other
+		 * OmniVision sensors, its exposure value is twice of the
+		 * register value, the exposure should be divided by 2 before
+		 * set register, e.g. val << 3.
+		 */
+		ret = ov5675_write_reg(ov5675, OV5675_REG_EXPOSURE,
+				       OV5675_REG_VALUE_24BIT, ctrl->val << 3);
+		break;
+
+	case V4L2_CID_VBLANK:
+		ret = ov5675_write_reg(ov5675, OV5675_REG_VTS,
+				       OV5675_REG_VALUE_16BIT,
+				       ov5675->cur_mode->height + ctrl->val +
+				       10);
+		break;
+
+	case V4L2_CID_TEST_PATTERN:
+		ret = ov5675_test_pattern(ov5675, ctrl->val);
+		break;
+
+	case V4L2_CID_HFLIP:
+		ov5675_set_ctrl_hflip(ov5675, ctrl->val);
+		break;
+
+	case V4L2_CID_VFLIP:
+		ov5675_set_ctrl_vflip(ov5675, ctrl->val);
+		break;
+
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	pm_runtime_put(&client->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ov5675_ctrl_ops = {
+	.s_ctrl = ov5675_set_ctrl,
+};
+
+static int ov5675_init_controls(struct ov5675 *ov5675)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov5675->sd);
+	struct v4l2_fwnode_device_properties props;
+	struct v4l2_ctrl_handler *ctrl_hdlr;
+	s64 exposure_max, h_blank;
+	int ret;
+
+	ctrl_hdlr = &ov5675->ctrl_handler;
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 10);
+	if (ret)
+		return ret;
+
+	ctrl_hdlr->lock = &ov5675->mutex;
+	ov5675->link_freq = v4l2_ctrl_new_int_menu(ctrl_hdlr, &ov5675_ctrl_ops,
+					   V4L2_CID_LINK_FREQ,
+					   ARRAY_SIZE(link_freq_menu_items) - 1,
+					   0, link_freq_menu_items);
+	if (ov5675->link_freq)
+		ov5675->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	ov5675->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, &ov5675_ctrl_ops,
+				       V4L2_CID_PIXEL_RATE, 0,
+				       to_pixel_rate(OV5675_LINK_FREQ_900MBPS),
+				       1,
+				       to_pixel_rate(OV5675_LINK_FREQ_900MBPS));
+	ov5675->vblank = v4l2_ctrl_new_std(ctrl_hdlr, &ov5675_ctrl_ops,
+			  V4L2_CID_VBLANK,
+			  ov5675->cur_mode->vts_min - ov5675->cur_mode->height,
+			  OV5675_VTS_MAX - ov5675->cur_mode->height, 1,
+			  ov5675->cur_mode->vts_def - ov5675->cur_mode->height);
+	h_blank = to_pixels_per_line(ov5675->cur_mode->hts,
+		  ov5675->cur_mode->link_freq_index) - ov5675->cur_mode->width;
+	ov5675->hblank = v4l2_ctrl_new_std(ctrl_hdlr, &ov5675_ctrl_ops,
+					   V4L2_CID_HBLANK, h_blank, h_blank, 1,
+					   h_blank);
+	if (ov5675->hblank)
+		ov5675->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov5675_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
+			  OV5675_ANAL_GAIN_MIN, OV5675_ANAL_GAIN_MAX,
+			  OV5675_ANAL_GAIN_STEP, OV5675_ANAL_GAIN_MIN);
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov5675_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
+			  OV5675_DGTL_GAIN_MIN, OV5675_DGTL_GAIN_MAX,
+			  OV5675_DGTL_GAIN_STEP, OV5675_DGTL_GAIN_DEFAULT);
+	exposure_max = (ov5675->cur_mode->vts_def - OV5675_EXPOSURE_MAX_MARGIN);
+	ov5675->exposure = v4l2_ctrl_new_std(ctrl_hdlr, &ov5675_ctrl_ops,
+					     V4L2_CID_EXPOSURE,
+					     OV5675_EXPOSURE_MIN, exposure_max,
+					     OV5675_EXPOSURE_STEP,
+					     exposure_max);
+	v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &ov5675_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(ov5675_test_pattern_menu) - 1,
+				     0, 0, ov5675_test_pattern_menu);
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov5675_ctrl_ops,
+			  V4L2_CID_HFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov5675_ctrl_ops,
+			  V4L2_CID_VFLIP, 0, 1, 1, 0);
+
+	if (ctrl_hdlr->error) {
+		v4l2_ctrl_handler_free(ctrl_hdlr);
+		return ctrl_hdlr->error;
+	}
+
+	ret = v4l2_fwnode_device_parse(&client->dev, &props);
+	if (ret)
+		goto error;
+
+	ret = v4l2_ctrl_new_fwnode_properties(ctrl_hdlr, &ov5675_ctrl_ops,
+					      &props);
+	if (ret)
+		goto error;
+
+	ov5675->sd.ctrl_handler = ctrl_hdlr;
+
+	return 0;
+
+error:
+	v4l2_ctrl_handler_free(ctrl_hdlr);
+
+	return ret;
+}
+
+static void ov5675_update_pad_format(const struct ov5675_mode *mode,
+				     struct v4l2_mbus_framefmt *fmt)
+{
+	fmt->width = mode->width;
+	fmt->height = mode->height;
+	fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	fmt->field = V4L2_FIELD_NONE;
+}
+
+static int ov5675_identify_module(struct ov5675 *ov5675)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov5675->sd);
+	int ret;
+	u32 val;
+
+	if (ov5675->identified)
+		return 0;
+
+	ret = ov5675_read_reg(ov5675, OV5675_REG_CHIP_ID,
+			      OV5675_REG_VALUE_24BIT, &val);
+	if (ret)
+		return ret;
+
+	if (val != OV5675_CHIP_ID) {
+		dev_err(&client->dev, "chip id mismatch: %x!=%x",
+			OV5675_CHIP_ID, val);
+		return -ENXIO;
+	}
+
+	ov5675->identified = true;
+
+	return 0;
+}
+
+static int ov5675_start_streaming(struct ov5675 *ov5675)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov5675->sd);
+	const struct ov5675_reg_list *reg_list;
+	int link_freq_index, ret;
+
+	ret = ov5675_identify_module(ov5675);
+	if (ret)
+		return ret;
+
+	link_freq_index = ov5675->cur_mode->link_freq_index;
+	reg_list = &link_freq_configs[link_freq_index].reg_list;
+	ret = ov5675_write_reg_list(ov5675, reg_list);
+	if (ret) {
+		dev_err(&client->dev, "failed to set plls");
+		return ret;
+	}
+
+	reg_list = &ov5675->cur_mode->reg_list;
+	ret = ov5675_write_reg_list(ov5675, reg_list);
+	if (ret) {
+		dev_err(&client->dev, "failed to set mode");
+		return ret;
+	}
+
+	ret = __v4l2_ctrl_handler_setup(ov5675->sd.ctrl_handler);
+	if (ret)
+		return ret;
+
+	ret = ov5675_write_reg(ov5675, OV5675_REG_MODE_SELECT,
+			       OV5675_REG_VALUE_08BIT, OV5675_MODE_STREAMING);
+	if (ret) {
+		dev_err(&client->dev, "failed to set stream");
+		return ret;
+	}
+
+	return 0;
+}
+
+static void ov5675_stop_streaming(struct ov5675 *ov5675)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov5675->sd);
+
+	if (ov5675_write_reg(ov5675, OV5675_REG_MODE_SELECT,
+			     OV5675_REG_VALUE_08BIT, OV5675_MODE_STANDBY))
+		dev_err(&client->dev, "failed to set stream");
+}
+
+static int ov5675_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct ov5675 *ov5675 = to_ov5675(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	if (ov5675->streaming == enable)
+		return 0;
+
+	mutex_lock(&ov5675->mutex);
+	if (enable) {
+		ret = pm_runtime_resume_and_get(&client->dev);
+		if (ret < 0) {
+			mutex_unlock(&ov5675->mutex);
+			return ret;
+		}
+
+		ret = ov5675_start_streaming(ov5675);
+		if (ret) {
+			enable = 0;
+			ov5675_stop_streaming(ov5675);
+			pm_runtime_put(&client->dev);
+		}
+	} else {
+		ov5675_stop_streaming(ov5675);
+		pm_runtime_put(&client->dev);
+	}
+
+	ov5675->streaming = enable;
+	mutex_unlock(&ov5675->mutex);
+
+	return ret;
+}
+
+static int ov5675_power_off(struct device *dev)
+{
+	/* 512 xvclk cycles after the last SCCB transation or MIPI frame end */
+	u32 delay_us = DIV_ROUND_UP(512, OV5675_XVCLK_19_2 / 1000 / 1000);
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov5675 *ov5675 = to_ov5675(sd);
+
+	usleep_range(delay_us, delay_us * 2);
+
+	clk_disable_unprepare(ov5675->xvclk);
+	gpiod_set_value_cansleep(ov5675->reset_gpio, 1);
+	regulator_bulk_disable(OV5675_NUM_SUPPLIES, ov5675->supplies);
+
+	return 0;
+}
+
+static int ov5675_power_on(struct device *dev)
+{
+	u32 delay_us = DIV_ROUND_UP(8192, OV5675_XVCLK_19_2 / 1000 / 1000);
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov5675 *ov5675 = to_ov5675(sd);
+	int ret;
+
+	ret = clk_prepare_enable(ov5675->xvclk);
+	if (ret < 0) {
+		dev_err(dev, "failed to enable xvclk: %d\n", ret);
+		return ret;
+	}
+
+	gpiod_set_value_cansleep(ov5675->reset_gpio, 1);
+
+	ret = regulator_bulk_enable(OV5675_NUM_SUPPLIES, ov5675->supplies);
+	if (ret) {
+		clk_disable_unprepare(ov5675->xvclk);
+		return ret;
+	}
+
+	/* Reset pulse should be at least 2ms and reset gpio released only once
+	 * regulators are stable.
+	 */
+	usleep_range(2000, 2200);
+
+	gpiod_set_value_cansleep(ov5675->reset_gpio, 0);
+
+	/* 8192 xvclk cycles prior to the first SCCB transation */
+	usleep_range(delay_us, delay_us * 2);
+
+	return 0;
+}
+
+static int __maybe_unused ov5675_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov5675 *ov5675 = to_ov5675(sd);
+
+	mutex_lock(&ov5675->mutex);
+	if (ov5675->streaming)
+		ov5675_stop_streaming(ov5675);
+
+	mutex_unlock(&ov5675->mutex);
+
+	return 0;
+}
+
+static int __maybe_unused ov5675_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov5675 *ov5675 = to_ov5675(sd);
+	int ret;
+
+	mutex_lock(&ov5675->mutex);
+	if (ov5675->streaming) {
+		ret = ov5675_start_streaming(ov5675);
+		if (ret) {
+			ov5675->streaming = false;
+			ov5675_stop_streaming(ov5675);
+			mutex_unlock(&ov5675->mutex);
+			return ret;
+		}
+	}
+
+	mutex_unlock(&ov5675->mutex);
+
+	return 0;
+}
+
+static int ov5675_set_format(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *sd_state,
+			     struct v4l2_subdev_format *fmt)
+{
+	struct ov5675 *ov5675 = to_ov5675(sd);
+	const struct ov5675_mode *mode;
+	s32 vblank_def, h_blank;
+
+	mode = v4l2_find_nearest_size(supported_modes,
+				      ARRAY_SIZE(supported_modes), width,
+				      height, fmt->format.width,
+				      fmt->format.height);
+
+	mutex_lock(&ov5675->mutex);
+	ov5675_update_pad_format(mode, &fmt->format);
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		*v4l2_subdev_get_try_format(sd, sd_state, fmt->pad) = fmt->format;
+	} else {
+		ov5675->cur_mode = mode;
+		__v4l2_ctrl_s_ctrl(ov5675->link_freq, mode->link_freq_index);
+		__v4l2_ctrl_s_ctrl_int64(ov5675->pixel_rate,
+					 to_pixel_rate(mode->link_freq_index));
+
+		/* Update limits and set FPS to default */
+		vblank_def = mode->vts_def - mode->height;
+		__v4l2_ctrl_modify_range(ov5675->vblank,
+					 mode->vts_min - mode->height,
+					 OV5675_VTS_MAX - mode->height, 1,
+					 vblank_def);
+		__v4l2_ctrl_s_ctrl(ov5675->vblank, vblank_def);
+		h_blank = to_pixels_per_line(mode->hts, mode->link_freq_index) -
+			  mode->width;
+		__v4l2_ctrl_modify_range(ov5675->hblank, h_blank, h_blank, 1,
+					 h_blank);
+	}
+
+	mutex_unlock(&ov5675->mutex);
+
+	return 0;
+}
+
+static int ov5675_get_format(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *sd_state,
+			     struct v4l2_subdev_format *fmt)
+{
+	struct ov5675 *ov5675 = to_ov5675(sd);
+
+	mutex_lock(&ov5675->mutex);
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
+		fmt->format = *v4l2_subdev_get_try_format(&ov5675->sd,
+							  sd_state,
+							  fmt->pad);
+	else
+		ov5675_update_pad_format(ov5675->cur_mode, &fmt->format);
+
+	mutex_unlock(&ov5675->mutex);
+
+	return 0;
+}
+
+static int ov5675_get_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *state,
+				struct v4l2_subdev_selection *sel)
+{
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
+		return -EINVAL;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.top = 0;
+		sel->r.left = 0;
+		sel->r.width = 2624;
+		sel->r.height = 2000;
+		return 0;
+	case V4L2_SEL_TGT_CROP:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		sel->r.top = 16;
+		sel->r.left = 16;
+		sel->r.width = 2592;
+		sel->r.height = 1944;
+		return 0;
+	}
+	return -EINVAL;
+}
+
+static int ov5675_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index > 0)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+
+	return 0;
+}
+
+static int ov5675_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->index >= ARRAY_SIZE(supported_modes))
+		return -EINVAL;
+
+	if (fse->code != MEDIA_BUS_FMT_SGRBG10_1X10)
+		return -EINVAL;
+
+	fse->min_width = supported_modes[fse->index].width;
+	fse->max_width = fse->min_width;
+	fse->min_height = supported_modes[fse->index].height;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static int ov5675_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct ov5675 *ov5675 = to_ov5675(sd);
+
+	mutex_lock(&ov5675->mutex);
+	ov5675_update_pad_format(&supported_modes[0],
+				 v4l2_subdev_get_try_format(sd, fh->state, 0));
+	mutex_unlock(&ov5675->mutex);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops ov5675_video_ops = {
+	.s_stream = ov5675_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops ov5675_pad_ops = {
+	.set_fmt = ov5675_set_format,
+	.get_fmt = ov5675_get_format,
+	.get_selection = ov5675_get_selection,
+	.enum_mbus_code = ov5675_enum_mbus_code,
+	.enum_frame_size = ov5675_enum_frame_size,
+};
+
+static const struct v4l2_subdev_ops ov5675_subdev_ops = {
+	.video = &ov5675_video_ops,
+	.pad = &ov5675_pad_ops,
+};
+
+static const struct media_entity_operations ov5675_subdev_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static const struct v4l2_subdev_internal_ops ov5675_internal_ops = {
+	.open = ov5675_open,
+};
+
+static int ov5675_get_hwcfg(struct ov5675 *ov5675, struct device *dev)
+{
+	struct fwnode_handle *ep;
+	struct fwnode_handle *fwnode = dev_fwnode(dev);
+	struct v4l2_fwnode_endpoint bus_cfg = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY
+	};
+	u32 xvclk_rate;
+	int ret;
+	unsigned int i, j;
+
+	if (!fwnode)
+		return -ENXIO;
+
+	ov5675->xvclk = devm_clk_get_optional(dev, NULL);
+	if (IS_ERR(ov5675->xvclk))
+		return dev_err_probe(dev, PTR_ERR(ov5675->xvclk),
+				     "failed to get xvclk: %ld\n",
+				     PTR_ERR(ov5675->xvclk));
+
+	if (ov5675->xvclk) {
+		xvclk_rate = clk_get_rate(ov5675->xvclk);
+	} else {
+		ret = fwnode_property_read_u32(fwnode, "clock-frequency",
+					       &xvclk_rate);
+
+		if (ret) {
+			dev_err(dev, "can't get clock frequency");
+			return ret;
+		}
+	}
+
+	if (xvclk_rate != OV5675_XVCLK_19_2) {
+		dev_err(dev, "external clock rate %u is unsupported",
+			xvclk_rate);
+		return -EINVAL;
+	}
+
+	ov5675->reset_gpio = devm_gpiod_get_optional(dev, "reset",
+						     GPIOD_OUT_HIGH);
+	if (IS_ERR(ov5675->reset_gpio)) {
+		ret = PTR_ERR(ov5675->reset_gpio);
+		dev_err(dev, "failed to get reset-gpios: %d\n", ret);
+		return ret;
+	}
+
+	for (i = 0; i < OV5675_NUM_SUPPLIES; i++)
+		ov5675->supplies[i].supply = ov5675_supply_names[i];
+
+	ret = devm_regulator_bulk_get(dev, OV5675_NUM_SUPPLIES,
+				      ov5675->supplies);
+	if (ret)
+		return ret;
+
+	ep = fwnode_graph_get_next_endpoint(fwnode, NULL);
+	if (!ep)
+		return -ENXIO;
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(ep, &bus_cfg);
+	fwnode_handle_put(ep);
+	if (ret)
+		return ret;
+
+	if (bus_cfg.bus.mipi_csi2.num_data_lanes != OV5675_DATA_LANES) {
+		dev_err(dev, "number of CSI2 data lanes %d is not supported",
+			bus_cfg.bus.mipi_csi2.num_data_lanes);
+		ret = -EINVAL;
+		goto check_hwcfg_error;
+	}
+
+	if (!bus_cfg.nr_of_link_frequencies) {
+		dev_err(dev, "no link frequencies defined");
+		ret = -EINVAL;
+		goto check_hwcfg_error;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(link_freq_menu_items); i++) {
+		for (j = 0; j < bus_cfg.nr_of_link_frequencies; j++) {
+			if (link_freq_menu_items[i] ==
+				bus_cfg.link_frequencies[j])
+				break;
+		}
+
+		if (j == bus_cfg.nr_of_link_frequencies) {
+			dev_err(dev, "no link frequency %lld supported",
+				link_freq_menu_items[i]);
+			ret = -EINVAL;
+			goto check_hwcfg_error;
+		}
+	}
+
+check_hwcfg_error:
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+
+	return ret;
+}
+
+static void ov5675_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov5675 *ov5675 = to_ov5675(sd);
+
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+	pm_runtime_disable(&client->dev);
+	mutex_destroy(&ov5675->mutex);
+
+	if (!pm_runtime_status_suspended(&client->dev))
+		ov5675_power_off(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+}
+
+static int ov5675_probe(struct i2c_client *client)
+{
+	struct ov5675 *ov5675;
+	bool full_power;
+	int ret;
+
+	ov5675 = devm_kzalloc(&client->dev, sizeof(*ov5675), GFP_KERNEL);
+	if (!ov5675)
+		return -ENOMEM;
+
+	ret = ov5675_get_hwcfg(ov5675, &client->dev);
+	if (ret) {
+		dev_err(&client->dev, "failed to get HW configuration: %d",
+			ret);
+		return ret;
+	}
+
+	v4l2_i2c_subdev_init(&ov5675->sd, client, &ov5675_subdev_ops);
+
+	ret = ov5675_power_on(&client->dev);
+	if (ret) {
+		dev_err(&client->dev, "failed to power on: %d\n", ret);
+		return ret;
+	}
+
+	full_power = acpi_dev_state_d0(&client->dev);
+	if (full_power) {
+		ret = ov5675_identify_module(ov5675);
+		if (ret) {
+			dev_err(&client->dev, "failed to find sensor: %d", ret);
+			goto probe_power_off;
+		}
+	}
+
+	mutex_init(&ov5675->mutex);
+	ov5675->cur_mode = &supported_modes[0];
+	ret = ov5675_init_controls(ov5675);
+	if (ret) {
+		dev_err(&client->dev, "failed to init controls: %d", ret);
+		goto probe_error_v4l2_ctrl_handler_free;
+	}
+
+	ov5675->sd.internal_ops = &ov5675_internal_ops;
+	ov5675->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	ov5675->sd.entity.ops = &ov5675_subdev_entity_ops;
+	ov5675->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ov5675->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&ov5675->sd.entity, 1, &ov5675->pad);
+	if (ret) {
+		dev_err(&client->dev, "failed to init entity pads: %d", ret);
+		goto probe_error_v4l2_ctrl_handler_free;
+	}
+
+	ret = v4l2_async_register_subdev_sensor(&ov5675->sd);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed to register V4L2 subdev: %d",
+			ret);
+		goto probe_error_media_entity_cleanup;
+	}
+
+	/* Set the device's state to active if it's in D0 state. */
+	if (full_power)
+		pm_runtime_set_active(&client->dev);
+	pm_runtime_enable(&client->dev);
+	pm_runtime_idle(&client->dev);
+
+	return 0;
+
+probe_error_media_entity_cleanup:
+	media_entity_cleanup(&ov5675->sd.entity);
+
+probe_error_v4l2_ctrl_handler_free:
+	v4l2_ctrl_handler_free(ov5675->sd.ctrl_handler);
+	mutex_destroy(&ov5675->mutex);
+probe_power_off:
+	ov5675_power_off(&client->dev);
+
+	return ret;
+}
+
+static const struct dev_pm_ops ov5675_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(ov5675_suspend, ov5675_resume)
+	SET_RUNTIME_PM_OPS(ov5675_power_off, ov5675_power_on, NULL)
+};
+
+#ifdef CONFIG_ACPI
+static const struct acpi_device_id ov5675_acpi_ids[] = {
+	{"OVTI5675"},
+	{}
+};
+
+MODULE_DEVICE_TABLE(acpi, ov5675_acpi_ids);
+#endif
+
+static const struct of_device_id ov5675_of_match[] = {
+	{ .compatible = "ovti,ov5675", },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, ov5675_of_match);
+
+static struct i2c_driver ov5675_i2c_driver = {
+	.driver = {
+		.name = "ov5675",
+		.pm = &ov5675_pm_ops,
+		.acpi_match_table = ACPI_PTR(ov5675_acpi_ids),
+		.of_match_table = ov5675_of_match,
+	},
+	.probe = ov5675_probe,
+	.remove = ov5675_remove,
+	.flags = I2C_DRV_ACPI_WAIVE_D0_PROBE,
+};
+
+module_i2c_driver(ov5675_i2c_driver);
+
+MODULE_AUTHOR("Shawn Tu");
+MODULE_DESCRIPTION("OmniVision OV5675 sensor driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/ov5693.c b/drivers/media/i2c/ov5693.c
new file mode 100644
index 000000000..488ee6d9d
--- /dev/null
+++ b/drivers/media/i2c/ov5693.c
@@ -0,0 +1,1434 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2013 Intel Corporation. All Rights Reserved.
+ *
+ * Adapted from the atomisp-ov5693 driver, with contributions from:
+ *
+ * Daniel Scally
+ * Jean-Michel Hautbois
+ * Fabian Wuthrich
+ * Tsuchiya Yuto
+ * Jordan Hand
+ * Jake Day
+ */
+
+#include <linux/acpi.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+
+#include <media/v4l2-cci.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+
+/* System Control */
+#define OV5693_SW_RESET_REG			CCI_REG8(0x0103)
+#define OV5693_SW_STREAM_REG			CCI_REG8(0x0100)
+#define OV5693_START_STREAMING			0x01
+#define OV5693_STOP_STREAMING			0x00
+#define OV5693_SW_RESET				0x01
+
+#define OV5693_REG_CHIP_ID			CCI_REG16(0x300a)
+/* Yes, this is right. The datasheet for the OV5693 gives its ID as 0x5690 */
+#define OV5693_CHIP_ID				0x5690
+
+/* Exposure */
+#define OV5693_EXPOSURE_CTRL_REG		CCI_REG24(0x3500)
+#define OV5693_EXPOSURE_CTRL_MASK		GENMASK(19, 4)
+#define OV5693_INTEGRATION_TIME_MARGIN		8
+#define OV5693_EXPOSURE_MIN			1
+#define OV5693_EXPOSURE_STEP			1
+
+/* Analogue Gain */
+#define OV5693_GAIN_CTRL_REG			CCI_REG16(0x350a)
+#define OV5693_GAIN_CTRL_MASK			GENMASK(10, 4)
+#define OV5693_GAIN_MIN				1
+#define OV5693_GAIN_MAX				127
+#define OV5693_GAIN_DEF				8
+#define OV5693_GAIN_STEP			1
+
+/* Digital Gain */
+#define OV5693_MWB_RED_GAIN_REG			CCI_REG16(0x3400)
+#define OV5693_MWB_GREEN_GAIN_REG		CCI_REG16(0x3402)
+#define OV5693_MWB_BLUE_GAIN_REG		CCI_REG16(0x3404)
+#define OV5693_MWB_GAIN_MASK			GENMASK(11, 0)
+#define OV5693_MWB_GAIN_MAX			0x0fff
+#define OV5693_DIGITAL_GAIN_MIN			1
+#define OV5693_DIGITAL_GAIN_MAX			4095
+#define OV5693_DIGITAL_GAIN_DEF			1024
+#define OV5693_DIGITAL_GAIN_STEP		1
+
+/* Timing and Format */
+#define OV5693_CROP_START_X_REG			CCI_REG16(0x3800)
+#define OV5693_CROP_START_Y_REG			CCI_REG16(0x3802)
+#define OV5693_CROP_END_X_REG			CCI_REG16(0x3804)
+#define OV5693_CROP_END_Y_REG			CCI_REG16(0x3806)
+#define OV5693_OUTPUT_SIZE_X_REG		CCI_REG16(0x3808)
+#define OV5693_OUTPUT_SIZE_Y_REG		CCI_REG16(0x380a)
+
+#define OV5693_TIMING_HTS_REG			CCI_REG16(0x380c)
+#define OV5693_FIXED_PPL			2688U
+#define OV5693_TIMING_VTS_REG			CCI_REG16(0x380e)
+#define OV5693_TIMING_MAX_VTS			0xffff
+#define OV5693_TIMING_MIN_VTS			0x04
+
+#define OV5693_OFFSET_START_X_REG		CCI_REG16(0x3810)
+#define OV5693_OFFSET_START_Y_REG		CCI_REG16(0x3812)
+
+#define OV5693_SUB_INC_X_REG			CCI_REG8(0x3814)
+#define OV5693_SUB_INC_Y_REG			CCI_REG8(0x3815)
+
+#define OV5693_FORMAT1_REG			CCI_REG8(0x3820)
+#define OV5693_FORMAT1_FLIP_VERT_ISP_EN		BIT(6)
+#define OV5693_FORMAT1_FLIP_VERT_SENSOR_EN	BIT(1)
+#define OV5693_FORMAT1_VBIN_EN			BIT(0)
+#define OV5693_FORMAT2_REG			CCI_REG8(0x3821)
+#define OV5693_FORMAT2_HDR_EN			BIT(7)
+#define OV5693_FORMAT2_FLIP_HORZ_ISP_EN		BIT(2)
+#define OV5693_FORMAT2_FLIP_HORZ_SENSOR_EN	BIT(1)
+#define OV5693_FORMAT2_HBIN_EN			BIT(0)
+
+#define OV5693_ISP_CTRL2_REG			CCI_REG8(0x5002)
+#define OV5693_ISP_SCALE_ENABLE			BIT(7)
+
+/* Pixel Array */
+#define OV5693_NATIVE_WIDTH			2624
+#define OV5693_NATIVE_HEIGHT			1956
+#define OV5693_NATIVE_START_LEFT		0
+#define OV5693_NATIVE_START_TOP			0
+#define OV5693_ACTIVE_WIDTH			2592
+#define OV5693_ACTIVE_HEIGHT			1944
+#define OV5693_ACTIVE_START_LEFT		16
+#define OV5693_ACTIVE_START_TOP			6
+#define OV5693_MIN_CROP_WIDTH			2
+#define OV5693_MIN_CROP_HEIGHT			2
+
+/* Test Pattern */
+#define OV5693_TEST_PATTERN_REG			CCI_REG8(0x5e00)
+#define OV5693_TEST_PATTERN_ENABLE		BIT(7)
+#define OV5693_TEST_PATTERN_ROLLING		BIT(6)
+#define OV5693_TEST_PATTERN_RANDOM		0x01
+#define OV5693_TEST_PATTERN_BARS		0x00
+
+/* System Frequencies */
+#define OV5693_XVCLK_FREQ			19200000
+#define OV5693_LINK_FREQ_419_2MHZ		419200000
+#define OV5693_PIXEL_RATE			167680000
+
+#define to_ov5693_sensor(x) container_of(x, struct ov5693_device, sd)
+
+static const char * const ov5693_supply_names[] = {
+	"avdd",		/* Analog power */
+	"dovdd",	/* Digital I/O power */
+	"dvdd",		/* Digital circuit power */
+};
+
+#define OV5693_NUM_SUPPLIES	ARRAY_SIZE(ov5693_supply_names)
+
+struct ov5693_device {
+	struct device *dev;
+	struct regmap *regmap;
+
+	/* Protect against concurrent changes to controls */
+	struct mutex lock;
+
+	struct gpio_desc *reset;
+	struct gpio_desc *powerdown;
+	struct gpio_desc *privacy_led;
+	struct regulator_bulk_data supplies[OV5693_NUM_SUPPLIES];
+	struct clk *xvclk;
+
+	struct ov5693_mode {
+		struct v4l2_rect crop;
+		struct v4l2_mbus_framefmt format;
+		bool binning_x;
+		bool binning_y;
+		unsigned int inc_x_odd;
+		unsigned int inc_y_odd;
+		unsigned int vts;
+	} mode;
+	bool streaming;
+
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+
+	struct ov5693_v4l2_ctrls {
+		struct v4l2_ctrl_handler handler;
+		struct v4l2_ctrl *link_freq;
+		struct v4l2_ctrl *pixel_rate;
+		struct v4l2_ctrl *exposure;
+		struct v4l2_ctrl *analogue_gain;
+		struct v4l2_ctrl *digital_gain;
+		struct v4l2_ctrl *hflip;
+		struct v4l2_ctrl *vflip;
+		struct v4l2_ctrl *hblank;
+		struct v4l2_ctrl *vblank;
+		struct v4l2_ctrl *test_pattern;
+	} ctrls;
+};
+
+static const struct cci_reg_sequence ov5693_global_regs[] = {
+	{CCI_REG8(0x3016), 0xf0},
+	{CCI_REG8(0x3017), 0xf0},
+	{CCI_REG8(0x3018), 0xf0},
+	{CCI_REG8(0x3022), 0x01},
+	{CCI_REG8(0x3028), 0x44},
+	{CCI_REG8(0x3098), 0x02},
+	{CCI_REG8(0x3099), 0x19},
+	{CCI_REG8(0x309a), 0x02},
+	{CCI_REG8(0x309b), 0x01},
+	{CCI_REG8(0x309c), 0x00},
+	{CCI_REG8(0x30a0), 0xd2},
+	{CCI_REG8(0x30a2), 0x01},
+	{CCI_REG8(0x30b2), 0x00},
+	{CCI_REG8(0x30b3), 0x83},
+	{CCI_REG8(0x30b4), 0x03},
+	{CCI_REG8(0x30b5), 0x04},
+	{CCI_REG8(0x30b6), 0x01},
+	{CCI_REG8(0x3080), 0x01},
+	{CCI_REG8(0x3104), 0x21},
+	{CCI_REG8(0x3106), 0x00},
+	{CCI_REG8(0x3406), 0x01},
+	{CCI_REG8(0x3503), 0x07},
+	{CCI_REG8(0x350b), 0x40},
+	{CCI_REG8(0x3601), 0x0a},
+	{CCI_REG8(0x3602), 0x38},
+	{CCI_REG8(0x3612), 0x80},
+	{CCI_REG8(0x3620), 0x54},
+	{CCI_REG8(0x3621), 0xc7},
+	{CCI_REG8(0x3622), 0x0f},
+	{CCI_REG8(0x3625), 0x10},
+	{CCI_REG8(0x3630), 0x55},
+	{CCI_REG8(0x3631), 0xf4},
+	{CCI_REG8(0x3632), 0x00},
+	{CCI_REG8(0x3633), 0x34},
+	{CCI_REG8(0x3634), 0x02},
+	{CCI_REG8(0x364d), 0x0d},
+	{CCI_REG8(0x364f), 0xdd},
+	{CCI_REG8(0x3660), 0x04},
+	{CCI_REG8(0x3662), 0x10},
+	{CCI_REG8(0x3663), 0xf1},
+	{CCI_REG8(0x3665), 0x00},
+	{CCI_REG8(0x3666), 0x20},
+	{CCI_REG8(0x3667), 0x00},
+	{CCI_REG8(0x366a), 0x80},
+	{CCI_REG8(0x3680), 0xe0},
+	{CCI_REG8(0x3681), 0x00},
+	{CCI_REG8(0x3700), 0x42},
+	{CCI_REG8(0x3701), 0x14},
+	{CCI_REG8(0x3702), 0xa0},
+	{CCI_REG8(0x3703), 0xd8},
+	{CCI_REG8(0x3704), 0x78},
+	{CCI_REG8(0x3705), 0x02},
+	{CCI_REG8(0x370a), 0x00},
+	{CCI_REG8(0x370b), 0x20},
+	{CCI_REG8(0x370c), 0x0c},
+	{CCI_REG8(0x370d), 0x11},
+	{CCI_REG8(0x370e), 0x00},
+	{CCI_REG8(0x370f), 0x40},
+	{CCI_REG8(0x3710), 0x00},
+	{CCI_REG8(0x371a), 0x1c},
+	{CCI_REG8(0x371b), 0x05},
+	{CCI_REG8(0x371c), 0x01},
+	{CCI_REG8(0x371e), 0xa1},
+	{CCI_REG8(0x371f), 0x0c},
+	{CCI_REG8(0x3721), 0x00},
+	{CCI_REG8(0x3724), 0x10},
+	{CCI_REG8(0x3726), 0x00},
+	{CCI_REG8(0x372a), 0x01},
+	{CCI_REG8(0x3730), 0x10},
+	{CCI_REG8(0x3738), 0x22},
+	{CCI_REG8(0x3739), 0xe5},
+	{CCI_REG8(0x373a), 0x50},
+	{CCI_REG8(0x373b), 0x02},
+	{CCI_REG8(0x373c), 0x41},
+	{CCI_REG8(0x373f), 0x02},
+	{CCI_REG8(0x3740), 0x42},
+	{CCI_REG8(0x3741), 0x02},
+	{CCI_REG8(0x3742), 0x18},
+	{CCI_REG8(0x3743), 0x01},
+	{CCI_REG8(0x3744), 0x02},
+	{CCI_REG8(0x3747), 0x10},
+	{CCI_REG8(0x374c), 0x04},
+	{CCI_REG8(0x3751), 0xf0},
+	{CCI_REG8(0x3752), 0x00},
+	{CCI_REG8(0x3753), 0x00},
+	{CCI_REG8(0x3754), 0xc0},
+	{CCI_REG8(0x3755), 0x00},
+	{CCI_REG8(0x3756), 0x1a},
+	{CCI_REG8(0x3758), 0x00},
+	{CCI_REG8(0x3759), 0x0f},
+	{CCI_REG8(0x376b), 0x44},
+	{CCI_REG8(0x375c), 0x04},
+	{CCI_REG8(0x3774), 0x10},
+	{CCI_REG8(0x3776), 0x00},
+	{CCI_REG8(0x377f), 0x08},
+	{CCI_REG8(0x3780), 0x22},
+	{CCI_REG8(0x3781), 0x0c},
+	{CCI_REG8(0x3784), 0x2c},
+	{CCI_REG8(0x3785), 0x1e},
+	{CCI_REG8(0x378f), 0xf5},
+	{CCI_REG8(0x3791), 0xb0},
+	{CCI_REG8(0x3795), 0x00},
+	{CCI_REG8(0x3796), 0x64},
+	{CCI_REG8(0x3797), 0x11},
+	{CCI_REG8(0x3798), 0x30},
+	{CCI_REG8(0x3799), 0x41},
+	{CCI_REG8(0x379a), 0x07},
+	{CCI_REG8(0x379b), 0xb0},
+	{CCI_REG8(0x379c), 0x0c},
+	{CCI_REG8(0x3a04), 0x06},
+	{CCI_REG8(0x3a05), 0x14},
+	{CCI_REG8(0x3e07), 0x20},
+	{CCI_REG8(0x4000), 0x08},
+	{CCI_REG8(0x4001), 0x04},
+	{CCI_REG8(0x4004), 0x08},
+	{CCI_REG8(0x4006), 0x20},
+	{CCI_REG8(0x4008), 0x24},
+	{CCI_REG8(0x4009), 0x10},
+	{CCI_REG8(0x4058), 0x00},
+	{CCI_REG8(0x4101), 0xb2},
+	{CCI_REG8(0x4307), 0x31},
+	{CCI_REG8(0x4511), 0x05},
+	{CCI_REG8(0x4512), 0x01},
+	{CCI_REG8(0x481f), 0x30},
+	{CCI_REG8(0x4826), 0x2c},
+	{CCI_REG8(0x4d02), 0xfd},
+	{CCI_REG8(0x4d03), 0xf5},
+	{CCI_REG8(0x4d04), 0x0c},
+	{CCI_REG8(0x4d05), 0xcc},
+	{CCI_REG8(0x4837), 0x0a},
+	{CCI_REG8(0x5003), 0x20},
+	{CCI_REG8(0x5013), 0x00},
+	{CCI_REG8(0x5842), 0x01},
+	{CCI_REG8(0x5843), 0x2b},
+	{CCI_REG8(0x5844), 0x01},
+	{CCI_REG8(0x5845), 0x92},
+	{CCI_REG8(0x5846), 0x01},
+	{CCI_REG8(0x5847), 0x8f},
+	{CCI_REG8(0x5848), 0x01},
+	{CCI_REG8(0x5849), 0x0c},
+	{CCI_REG8(0x5e10), 0x0c},
+	{CCI_REG8(0x3820), 0x00},
+	{CCI_REG8(0x3821), 0x1e},
+	{CCI_REG8(0x5041), 0x14}
+};
+
+static const struct v4l2_rect ov5693_default_crop = {
+	.left = OV5693_ACTIVE_START_LEFT,
+	.top = OV5693_ACTIVE_START_TOP,
+	.width = OV5693_ACTIVE_WIDTH,
+	.height = OV5693_ACTIVE_HEIGHT,
+};
+
+static const struct v4l2_mbus_framefmt ov5693_default_fmt = {
+	.width = OV5693_ACTIVE_WIDTH,
+	.height = OV5693_ACTIVE_HEIGHT,
+	.code = MEDIA_BUS_FMT_SBGGR10_1X10,
+};
+
+static const s64 link_freq_menu_items[] = {
+	OV5693_LINK_FREQ_419_2MHZ
+};
+
+static const char * const ov5693_test_pattern_menu[] = {
+	"Disabled",
+	"Random Data",
+	"Colour Bars",
+	"Colour Bars with Rolling Bar"
+};
+
+static const u8 ov5693_test_pattern_bits[] = {
+	0,
+	OV5693_TEST_PATTERN_ENABLE | OV5693_TEST_PATTERN_RANDOM,
+	OV5693_TEST_PATTERN_ENABLE | OV5693_TEST_PATTERN_BARS,
+	OV5693_TEST_PATTERN_ENABLE | OV5693_TEST_PATTERN_BARS |
+	OV5693_TEST_PATTERN_ROLLING,
+};
+
+/* V4L2 Controls Functions */
+
+static int ov5693_flip_vert_configure(struct ov5693_device *ov5693,
+				      bool enable)
+{
+	u8 bits = OV5693_FORMAT1_FLIP_VERT_ISP_EN |
+		  OV5693_FORMAT1_FLIP_VERT_SENSOR_EN;
+	int ret;
+
+	ret = cci_update_bits(ov5693->regmap, OV5693_FORMAT1_REG, bits,
+			      enable ? bits : 0, NULL);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int ov5693_flip_horz_configure(struct ov5693_device *ov5693,
+				      bool enable)
+{
+	u8 bits = OV5693_FORMAT2_FLIP_HORZ_ISP_EN |
+		  OV5693_FORMAT2_FLIP_HORZ_SENSOR_EN;
+	int ret;
+
+	ret = cci_update_bits(ov5693->regmap, OV5693_FORMAT2_REG, bits,
+			      enable ? bits : 0, NULL);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int ov5693_get_exposure(struct ov5693_device *ov5693, s32 *value)
+{
+	u64 exposure;
+	int ret;
+
+	ret = cci_read(ov5693->regmap, OV5693_EXPOSURE_CTRL_REG, &exposure,
+		       NULL);
+	if (ret)
+		return ret;
+
+	/* The lowest 4 bits are unsupported fractional bits */
+	*value = exposure >> 4;
+
+	return 0;
+}
+
+static int ov5693_exposure_configure(struct ov5693_device *ov5693,
+				     u32 exposure)
+{
+	int ret = 0;
+
+	exposure = (exposure << 4) & OV5693_EXPOSURE_CTRL_MASK;
+
+	cci_write(ov5693->regmap, OV5693_EXPOSURE_CTRL_REG, exposure, &ret);
+
+	return ret;
+}
+
+static int ov5693_get_gain(struct ov5693_device *ov5693, u32 *gain)
+{
+	u64 value;
+	int ret;
+
+	ret = cci_read(ov5693->regmap, OV5693_GAIN_CTRL_REG, &value, NULL);
+	if (ret)
+		return ret;
+
+	/* As with exposure, the lowest 4 bits are fractional bits. */
+	*gain = value >> 4;
+
+	return ret;
+}
+
+static int ov5693_digital_gain_configure(struct ov5693_device *ov5693,
+					 u32 gain)
+{
+	int ret = 0;
+
+	gain &= OV5693_MWB_GAIN_MASK;
+
+	cci_write(ov5693->regmap, OV5693_MWB_RED_GAIN_REG, gain, &ret);
+	cci_write(ov5693->regmap, OV5693_MWB_GREEN_GAIN_REG, gain, &ret);
+	cci_write(ov5693->regmap, OV5693_MWB_BLUE_GAIN_REG, gain, &ret);
+
+	return ret;
+}
+
+static int ov5693_analog_gain_configure(struct ov5693_device *ov5693, u32 gain)
+{
+	int ret = 0;
+
+	gain = (gain << 4) & OV5693_GAIN_CTRL_MASK;
+
+	cci_write(ov5693->regmap, OV5693_GAIN_CTRL_REG, gain, &ret);
+
+	return ret;
+}
+
+static int ov5693_vts_configure(struct ov5693_device *ov5693, u32 vblank)
+{
+	u16 vts = ov5693->mode.format.height + vblank;
+	int ret = 0;
+
+	cci_write(ov5693->regmap, OV5693_TIMING_VTS_REG, vts, &ret);
+
+	return ret;
+}
+
+static int ov5693_test_pattern_configure(struct ov5693_device *ov5693, u32 idx)
+{
+	int ret = 0;
+
+	cci_write(ov5693->regmap, OV5693_TEST_PATTERN_REG,
+		  ov5693_test_pattern_bits[idx], &ret);
+
+	return ret;
+}
+
+static int ov5693_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov5693_device *ov5693 =
+	    container_of(ctrl->handler, struct ov5693_device, ctrls.handler);
+	int ret = 0;
+
+	/* If VBLANK is altered we need to update exposure to compensate */
+	if (ctrl->id == V4L2_CID_VBLANK) {
+		int exposure_max;
+
+		exposure_max = ov5693->mode.format.height + ctrl->val -
+			       OV5693_INTEGRATION_TIME_MARGIN;
+		__v4l2_ctrl_modify_range(ov5693->ctrls.exposure,
+					 ov5693->ctrls.exposure->minimum,
+					 exposure_max,
+					 ov5693->ctrls.exposure->step,
+					 min(ov5693->ctrls.exposure->val,
+					     exposure_max));
+	}
+
+	/* Only apply changes to the controls if the device is powered up */
+	if (!pm_runtime_get_if_in_use(ov5693->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE:
+		ret = ov5693_exposure_configure(ov5693, ctrl->val);
+		break;
+	case V4L2_CID_ANALOGUE_GAIN:
+		ret = ov5693_analog_gain_configure(ov5693, ctrl->val);
+		break;
+	case V4L2_CID_DIGITAL_GAIN:
+		ret = ov5693_digital_gain_configure(ov5693, ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+		ret = ov5693_flip_horz_configure(ov5693, !!ctrl->val);
+		break;
+	case V4L2_CID_VFLIP:
+		ret = ov5693_flip_vert_configure(ov5693, !!ctrl->val);
+		break;
+	case V4L2_CID_VBLANK:
+		ret = ov5693_vts_configure(ov5693, ctrl->val);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		ret = ov5693_test_pattern_configure(ov5693, ctrl->val);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	pm_runtime_put(ov5693->dev);
+
+	return ret;
+}
+
+static int ov5693_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov5693_device *ov5693 = container_of(ctrl->handler,
+						    struct ov5693_device,
+						    ctrls.handler);
+
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE_ABSOLUTE:
+		return ov5693_get_exposure(ov5693, &ctrl->val);
+	case V4L2_CID_AUTOGAIN:
+		return ov5693_get_gain(ov5693, &ctrl->val);
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct v4l2_ctrl_ops ov5693_ctrl_ops = {
+	.s_ctrl = ov5693_s_ctrl,
+	.g_volatile_ctrl = ov5693_g_volatile_ctrl
+};
+
+/* System Control Functions */
+
+static int ov5693_mode_configure(struct ov5693_device *ov5693)
+{
+	const struct ov5693_mode *mode = &ov5693->mode;
+	int ret = 0;
+
+	/* Crop Start X */
+	cci_write(ov5693->regmap, OV5693_CROP_START_X_REG, mode->crop.left,
+		  &ret);
+
+	/* Offset X */
+	cci_write(ov5693->regmap, OV5693_OFFSET_START_X_REG, 0, &ret);
+
+	/* Output Size X */
+	cci_write(ov5693->regmap, OV5693_OUTPUT_SIZE_X_REG, mode->format.width,
+		  &ret);
+
+	/* Crop End X */
+	cci_write(ov5693->regmap, OV5693_CROP_END_X_REG,
+		  mode->crop.left + mode->crop.width, &ret);
+
+	/* Horizontal Total Size */
+	cci_write(ov5693->regmap, OV5693_TIMING_HTS_REG, OV5693_FIXED_PPL,
+		  &ret);
+
+	/* Crop Start Y */
+	cci_write(ov5693->regmap, OV5693_CROP_START_Y_REG, mode->crop.top,
+		  &ret);
+
+	/* Offset Y */
+	cci_write(ov5693->regmap, OV5693_OFFSET_START_Y_REG, 0, &ret);
+
+	/* Output Size Y */
+	cci_write(ov5693->regmap, OV5693_OUTPUT_SIZE_Y_REG, mode->format.height,
+		  &ret);
+
+	/* Crop End Y */
+	cci_write(ov5693->regmap, OV5693_CROP_END_Y_REG,
+		  mode->crop.top + mode->crop.height, &ret);
+
+	/* Subsample X increase */
+	cci_write(ov5693->regmap, OV5693_SUB_INC_X_REG,
+		  ((mode->inc_x_odd << 4) & 0xf0) | 0x01, &ret);
+	/* Subsample Y increase */
+	cci_write(ov5693->regmap, OV5693_SUB_INC_Y_REG,
+		  ((mode->inc_y_odd << 4) & 0xf0) | 0x01, &ret);
+
+	/* Binning */
+	cci_update_bits(ov5693->regmap, OV5693_FORMAT1_REG,
+			OV5693_FORMAT1_VBIN_EN,
+			mode->binning_y ? OV5693_FORMAT1_VBIN_EN : 0, &ret);
+
+	cci_update_bits(ov5693->regmap, OV5693_FORMAT2_REG,
+			OV5693_FORMAT2_HBIN_EN,
+			mode->binning_x ? OV5693_FORMAT2_HBIN_EN : 0, &ret);
+
+	return ret;
+}
+
+static int ov5693_enable_streaming(struct ov5693_device *ov5693, bool enable)
+{
+	int ret = 0;
+
+	cci_write(ov5693->regmap, OV5693_SW_STREAM_REG,
+		  enable ? OV5693_START_STREAMING : OV5693_STOP_STREAMING,
+		  &ret);
+
+	return ret;
+}
+
+static int ov5693_sw_reset(struct ov5693_device *ov5693)
+{
+	int ret = 0;
+
+	cci_write(ov5693->regmap, OV5693_SW_RESET_REG, OV5693_SW_RESET, &ret);
+
+	return ret;
+}
+
+static int ov5693_sensor_init(struct ov5693_device *ov5693)
+{
+	int ret;
+
+	ret = ov5693_sw_reset(ov5693);
+	if (ret)
+		return dev_err_probe(ov5693->dev, ret,
+				     "software reset error\n");
+
+	ret = cci_multi_reg_write(ov5693->regmap, ov5693_global_regs,
+				  ARRAY_SIZE(ov5693_global_regs), NULL);
+	if (ret)
+		return dev_err_probe(ov5693->dev, ret,
+				     "global settings error\n");
+
+	ret = ov5693_mode_configure(ov5693);
+	if (ret)
+		return dev_err_probe(ov5693->dev, ret,
+				     "mode configure error\n");
+
+	ret = ov5693_enable_streaming(ov5693, false);
+	if (ret)
+		dev_err(ov5693->dev, "stop streaming error\n");
+
+	return ret;
+}
+
+static void ov5693_sensor_powerdown(struct ov5693_device *ov5693)
+{
+	gpiod_set_value_cansleep(ov5693->privacy_led, 0);
+	gpiod_set_value_cansleep(ov5693->reset, 1);
+	gpiod_set_value_cansleep(ov5693->powerdown, 1);
+
+	regulator_bulk_disable(OV5693_NUM_SUPPLIES, ov5693->supplies);
+
+	clk_disable_unprepare(ov5693->xvclk);
+}
+
+static int ov5693_sensor_powerup(struct ov5693_device *ov5693)
+{
+	int ret;
+
+	gpiod_set_value_cansleep(ov5693->reset, 1);
+	gpiod_set_value_cansleep(ov5693->powerdown, 1);
+
+	ret = clk_prepare_enable(ov5693->xvclk);
+	if (ret) {
+		dev_err(ov5693->dev, "Failed to enable clk\n");
+		goto fail_power;
+	}
+
+	ret = regulator_bulk_enable(OV5693_NUM_SUPPLIES, ov5693->supplies);
+	if (ret) {
+		dev_err(ov5693->dev, "Failed to enable regulators\n");
+		goto fail_power;
+	}
+
+	gpiod_set_value_cansleep(ov5693->powerdown, 0);
+	gpiod_set_value_cansleep(ov5693->reset, 0);
+	gpiod_set_value_cansleep(ov5693->privacy_led, 1);
+
+	usleep_range(5000, 7500);
+
+	return 0;
+
+fail_power:
+	ov5693_sensor_powerdown(ov5693);
+	return ret;
+}
+
+static int __maybe_unused ov5693_sensor_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov5693_device *ov5693 = to_ov5693_sensor(sd);
+
+	ov5693_sensor_powerdown(ov5693);
+
+	return 0;
+}
+
+static int __maybe_unused ov5693_sensor_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov5693_device *ov5693 = to_ov5693_sensor(sd);
+	int ret;
+
+	mutex_lock(&ov5693->lock);
+
+	ret = ov5693_sensor_powerup(ov5693);
+	if (ret)
+		goto out_unlock;
+
+	ret = ov5693_sensor_init(ov5693);
+	if (ret) {
+		dev_err(dev, "ov5693 sensor init failure\n");
+		goto err_power;
+	}
+
+	goto out_unlock;
+
+err_power:
+	ov5693_sensor_powerdown(ov5693);
+out_unlock:
+	mutex_unlock(&ov5693->lock);
+	return ret;
+}
+
+static int ov5693_detect(struct ov5693_device *ov5693)
+{
+	int ret;
+	u64 id;
+
+	ret = cci_read(ov5693->regmap, OV5693_REG_CHIP_ID, &id, NULL);
+	if (ret)
+		return ret;
+
+	if (id != OV5693_CHIP_ID)
+		return dev_err_probe(ov5693->dev, -ENODEV,
+				     "sensor ID mismatch. Got 0x%04llx\n", id);
+
+	return 0;
+}
+
+/* V4L2 Framework callbacks */
+
+static unsigned int __ov5693_calc_vts(u32 height)
+{
+	/*
+	 * We need to set a sensible default VTS for whatever format height we
+	 * happen to be given from set_fmt(). This function just targets
+	 * an even multiple of 30fps.
+	 */
+
+	unsigned int tgt_fps;
+
+	tgt_fps = rounddown(OV5693_PIXEL_RATE / OV5693_FIXED_PPL / height, 30);
+
+	return ALIGN_DOWN(OV5693_PIXEL_RATE / OV5693_FIXED_PPL / tgt_fps, 2);
+}
+
+static struct v4l2_mbus_framefmt *
+__ov5693_get_pad_format(struct ov5693_device *ov5693,
+			struct v4l2_subdev_state *state,
+			unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_format(&ov5693->sd, state, pad);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return &ov5693->mode.format;
+	default:
+		return NULL;
+	}
+}
+
+static struct v4l2_rect *
+__ov5693_get_pad_crop(struct ov5693_device *ov5693,
+		      struct v4l2_subdev_state *state,
+		      unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_crop(&ov5693->sd, state, pad);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return &ov5693->mode.crop;
+	}
+
+	return NULL;
+}
+
+static int ov5693_get_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *state,
+			  struct v4l2_subdev_format *format)
+{
+	struct ov5693_device *ov5693 = to_ov5693_sensor(sd);
+
+	format->format = ov5693->mode.format;
+
+	return 0;
+}
+
+static int ov5693_set_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *state,
+			  struct v4l2_subdev_format *format)
+{
+	struct ov5693_device *ov5693 = to_ov5693_sensor(sd);
+	const struct v4l2_rect *crop;
+	struct v4l2_mbus_framefmt *fmt;
+	unsigned int hratio, vratio;
+	unsigned int width, height;
+	unsigned int hblank;
+	int exposure_max;
+
+	crop = __ov5693_get_pad_crop(ov5693, state, format->pad, format->which);
+
+	/*
+	 * Align to two to simplify the binning calculations below, and clamp
+	 * the requested format at the crop rectangle
+	 */
+	width = clamp_t(unsigned int, ALIGN(format->format.width, 2),
+			OV5693_MIN_CROP_WIDTH, crop->width);
+	height = clamp_t(unsigned int, ALIGN(format->format.height, 2),
+			 OV5693_MIN_CROP_HEIGHT, crop->height);
+
+	/*
+	 * We can only support setting either the dimensions of the crop rect
+	 * or those dimensions binned (separately) by a factor of two.
+	 */
+	hratio = clamp_t(unsigned int,
+			 DIV_ROUND_CLOSEST(crop->width, width), 1, 2);
+	vratio = clamp_t(unsigned int,
+			 DIV_ROUND_CLOSEST(crop->height, height), 1, 2);
+
+	fmt = __ov5693_get_pad_format(ov5693, state, format->pad,
+				      format->which);
+
+	fmt->width = crop->width / hratio;
+	fmt->height = crop->height / vratio;
+	fmt->code = MEDIA_BUS_FMT_SBGGR10_1X10;
+
+	format->format = *fmt;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+		return 0;
+
+	mutex_lock(&ov5693->lock);
+
+	ov5693->mode.binning_x = hratio > 1;
+	ov5693->mode.inc_x_odd = hratio > 1 ? 3 : 1;
+	ov5693->mode.binning_y = vratio > 1;
+	ov5693->mode.inc_y_odd = vratio > 1 ? 3 : 1;
+
+	ov5693->mode.vts = __ov5693_calc_vts(fmt->height);
+
+	__v4l2_ctrl_modify_range(ov5693->ctrls.vblank,
+				 OV5693_TIMING_MIN_VTS,
+				 OV5693_TIMING_MAX_VTS - fmt->height,
+				 1, ov5693->mode.vts - fmt->height);
+	__v4l2_ctrl_s_ctrl(ov5693->ctrls.vblank,
+			   ov5693->mode.vts - fmt->height);
+
+	hblank = OV5693_FIXED_PPL - fmt->width;
+	__v4l2_ctrl_modify_range(ov5693->ctrls.hblank, hblank, hblank, 1,
+				 hblank);
+
+	exposure_max = ov5693->mode.vts - OV5693_INTEGRATION_TIME_MARGIN;
+	__v4l2_ctrl_modify_range(ov5693->ctrls.exposure,
+				 ov5693->ctrls.exposure->minimum, exposure_max,
+				 ov5693->ctrls.exposure->step,
+				 min(ov5693->ctrls.exposure->val,
+				     exposure_max));
+
+	mutex_unlock(&ov5693->lock);
+	return 0;
+}
+
+static int ov5693_get_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *state,
+				struct v4l2_subdev_selection *sel)
+{
+	struct ov5693_device *ov5693 = to_ov5693_sensor(sd);
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		mutex_lock(&ov5693->lock);
+		sel->r = *__ov5693_get_pad_crop(ov5693, state, sel->pad,
+						sel->which);
+		mutex_unlock(&ov5693->lock);
+		break;
+	case V4L2_SEL_TGT_NATIVE_SIZE:
+		sel->r.top = 0;
+		sel->r.left = 0;
+		sel->r.width = OV5693_NATIVE_WIDTH;
+		sel->r.height = OV5693_NATIVE_HEIGHT;
+		break;
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		sel->r.top = OV5693_ACTIVE_START_TOP;
+		sel->r.left = OV5693_ACTIVE_START_LEFT;
+		sel->r.width = OV5693_ACTIVE_WIDTH;
+		sel->r.height = OV5693_ACTIVE_HEIGHT;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int ov5693_set_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *state,
+				struct v4l2_subdev_selection *sel)
+{
+	struct ov5693_device *ov5693 = to_ov5693_sensor(sd);
+	struct v4l2_mbus_framefmt *format;
+	struct v4l2_rect *__crop;
+	struct v4l2_rect rect;
+
+	if (sel->target != V4L2_SEL_TGT_CROP)
+		return -EINVAL;
+
+	/*
+	 * Clamp the boundaries of the crop rectangle to the size of the sensor
+	 * pixel array. Align to multiples of 2 to ensure Bayer pattern isn't
+	 * disrupted.
+	 */
+	rect.left = clamp(ALIGN(sel->r.left, 2), OV5693_NATIVE_START_LEFT,
+			  OV5693_NATIVE_WIDTH);
+	rect.top = clamp(ALIGN(sel->r.top, 2), OV5693_NATIVE_START_TOP,
+			 OV5693_NATIVE_HEIGHT);
+	rect.width = clamp_t(unsigned int, ALIGN(sel->r.width, 2),
+			     OV5693_MIN_CROP_WIDTH, OV5693_NATIVE_WIDTH);
+	rect.height = clamp_t(unsigned int, ALIGN(sel->r.height, 2),
+			      OV5693_MIN_CROP_HEIGHT, OV5693_NATIVE_HEIGHT);
+
+	/* Make sure the crop rectangle isn't outside the bounds of the array */
+	rect.width = min_t(unsigned int, rect.width,
+			   OV5693_NATIVE_WIDTH - rect.left);
+	rect.height = min_t(unsigned int, rect.height,
+			    OV5693_NATIVE_HEIGHT - rect.top);
+
+	__crop = __ov5693_get_pad_crop(ov5693, state, sel->pad, sel->which);
+
+	if (rect.width != __crop->width || rect.height != __crop->height) {
+		/*
+		 * Reset the output image size if the crop rectangle size has
+		 * been modified.
+		 */
+		format = __ov5693_get_pad_format(ov5693, state, sel->pad,
+						 sel->which);
+		format->width = rect.width;
+		format->height = rect.height;
+	}
+
+	*__crop = rect;
+	sel->r = rect;
+
+	return 0;
+}
+
+static int ov5693_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct ov5693_device *ov5693 = to_ov5693_sensor(sd);
+	int ret;
+
+	if (enable) {
+		ret = pm_runtime_get_sync(ov5693->dev);
+		if (ret < 0)
+			goto err_power_down;
+
+		mutex_lock(&ov5693->lock);
+		ret = __v4l2_ctrl_handler_setup(&ov5693->ctrls.handler);
+		if (ret) {
+			mutex_unlock(&ov5693->lock);
+			goto err_power_down;
+		}
+
+		ret = ov5693_enable_streaming(ov5693, true);
+		mutex_unlock(&ov5693->lock);
+	} else {
+		mutex_lock(&ov5693->lock);
+		ret = ov5693_enable_streaming(ov5693, false);
+		mutex_unlock(&ov5693->lock);
+	}
+	if (ret)
+		goto err_power_down;
+
+	ov5693->streaming = !!enable;
+
+	if (!enable)
+		pm_runtime_put(ov5693->dev);
+
+	return 0;
+err_power_down:
+	pm_runtime_put_noidle(ov5693->dev);
+	return ret;
+}
+
+static int ov5693_g_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *interval)
+{
+	struct ov5693_device *ov5693 = to_ov5693_sensor(sd);
+	unsigned int framesize = OV5693_FIXED_PPL * (ov5693->mode.format.height +
+				 ov5693->ctrls.vblank->val);
+	unsigned int fps = DIV_ROUND_CLOSEST(OV5693_PIXEL_RATE, framesize);
+
+	interval->interval.numerator = 1;
+	interval->interval.denominator = fps;
+
+	return 0;
+}
+
+static int ov5693_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	/* Only a single mbus format is supported */
+	if (code->index > 0)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_SBGGR10_1X10;
+	return 0;
+}
+
+static int ov5693_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct ov5693_device *ov5693 = to_ov5693_sensor(sd);
+	struct v4l2_rect *__crop;
+
+	if (fse->index > 1 || fse->code != MEDIA_BUS_FMT_SBGGR10_1X10)
+		return -EINVAL;
+
+	__crop = __ov5693_get_pad_crop(ov5693, state, fse->pad, fse->which);
+	if (!__crop)
+		return -EINVAL;
+
+	fse->min_width = __crop->width / (fse->index + 1);
+	fse->min_height = __crop->height / (fse->index + 1);
+	fse->max_width = fse->min_width;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops ov5693_video_ops = {
+	.s_stream = ov5693_s_stream,
+	.g_frame_interval = ov5693_g_frame_interval,
+};
+
+static const struct v4l2_subdev_pad_ops ov5693_pad_ops = {
+	.enum_mbus_code = ov5693_enum_mbus_code,
+	.enum_frame_size = ov5693_enum_frame_size,
+	.get_fmt = ov5693_get_fmt,
+	.set_fmt = ov5693_set_fmt,
+	.get_selection = ov5693_get_selection,
+	.set_selection = ov5693_set_selection,
+};
+
+static const struct v4l2_subdev_ops ov5693_ops = {
+	.video = &ov5693_video_ops,
+	.pad = &ov5693_pad_ops,
+};
+
+/* Sensor and Driver Configuration Functions */
+
+static int ov5693_init_controls(struct ov5693_device *ov5693)
+{
+	const struct v4l2_ctrl_ops *ops = &ov5693_ctrl_ops;
+	struct ov5693_v4l2_ctrls *ctrls = &ov5693->ctrls;
+	struct v4l2_fwnode_device_properties props;
+	int vblank_max, vblank_def;
+	int exposure_max;
+	int hblank;
+	int ret;
+
+	ret = v4l2_ctrl_handler_init(&ctrls->handler, 12);
+	if (ret)
+		return ret;
+
+	/* link freq */
+	ctrls->link_freq = v4l2_ctrl_new_int_menu(&ctrls->handler,
+						  NULL, V4L2_CID_LINK_FREQ,
+						  0, 0, link_freq_menu_items);
+	if (ctrls->link_freq)
+		ctrls->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	/* pixel rate */
+	ctrls->pixel_rate = v4l2_ctrl_new_std(&ctrls->handler, NULL,
+					      V4L2_CID_PIXEL_RATE, 0,
+					      OV5693_PIXEL_RATE, 1,
+					      OV5693_PIXEL_RATE);
+
+	/* Exposure */
+	exposure_max = ov5693->mode.vts - OV5693_INTEGRATION_TIME_MARGIN;
+	ctrls->exposure = v4l2_ctrl_new_std(&ctrls->handler, ops,
+					    V4L2_CID_EXPOSURE,
+					    OV5693_EXPOSURE_MIN, exposure_max,
+					    OV5693_EXPOSURE_STEP, exposure_max);
+
+	/* Gain */
+	ctrls->analogue_gain = v4l2_ctrl_new_std(&ctrls->handler,
+						 ops, V4L2_CID_ANALOGUE_GAIN,
+						 OV5693_GAIN_MIN,
+						 OV5693_GAIN_MAX,
+						 OV5693_GAIN_STEP,
+						 OV5693_GAIN_DEF);
+
+	ctrls->digital_gain = v4l2_ctrl_new_std(&ctrls->handler, ops,
+						V4L2_CID_DIGITAL_GAIN,
+						OV5693_DIGITAL_GAIN_MIN,
+						OV5693_DIGITAL_GAIN_MAX,
+						OV5693_DIGITAL_GAIN_STEP,
+						OV5693_DIGITAL_GAIN_DEF);
+
+	/* Flip */
+	ctrls->hflip = v4l2_ctrl_new_std(&ctrls->handler, ops,
+					 V4L2_CID_HFLIP, 0, 1, 1, 0);
+
+	ctrls->vflip = v4l2_ctrl_new_std(&ctrls->handler, ops,
+					 V4L2_CID_VFLIP, 0, 1, 1, 0);
+
+	hblank = OV5693_FIXED_PPL - ov5693->mode.format.width;
+	ctrls->hblank = v4l2_ctrl_new_std(&ctrls->handler, ops,
+					  V4L2_CID_HBLANK, hblank,
+					  hblank, 1, hblank);
+
+	if (ctrls->hblank)
+		ctrls->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	vblank_max = OV5693_TIMING_MAX_VTS - ov5693->mode.format.height;
+	vblank_def = ov5693->mode.vts - ov5693->mode.format.height;
+	ctrls->vblank = v4l2_ctrl_new_std(&ctrls->handler, ops,
+					  V4L2_CID_VBLANK,
+					  OV5693_TIMING_MIN_VTS,
+					  vblank_max, 1, vblank_def);
+
+	ctrls->test_pattern = v4l2_ctrl_new_std_menu_items(
+					&ctrls->handler, ops,
+					V4L2_CID_TEST_PATTERN,
+					ARRAY_SIZE(ov5693_test_pattern_menu) - 1,
+					0, 0, ov5693_test_pattern_menu);
+
+	if (ctrls->handler.error) {
+		dev_err(ov5693->dev, "Error initialising v4l2 ctrls\n");
+		ret = ctrls->handler.error;
+		goto err_free_handler;
+	}
+
+	/* set properties from fwnode (e.g. rotation, orientation) */
+	ret = v4l2_fwnode_device_parse(ov5693->dev, &props);
+	if (ret)
+		goto err_free_handler;
+
+	ret = v4l2_ctrl_new_fwnode_properties(&ctrls->handler, ops,
+					      &props);
+	if (ret)
+		goto err_free_handler;
+
+	/* Use same lock for controls as for everything else. */
+	ctrls->handler.lock = &ov5693->lock;
+	ov5693->sd.ctrl_handler = &ctrls->handler;
+
+	return 0;
+
+err_free_handler:
+	v4l2_ctrl_handler_free(&ctrls->handler);
+	return ret;
+}
+
+static int ov5693_configure_gpios(struct ov5693_device *ov5693)
+{
+	ov5693->reset = devm_gpiod_get_optional(ov5693->dev, "reset",
+						GPIOD_OUT_HIGH);
+	if (IS_ERR(ov5693->reset)) {
+		dev_err(ov5693->dev, "Error fetching reset GPIO\n");
+		return PTR_ERR(ov5693->reset);
+	}
+
+	ov5693->powerdown = devm_gpiod_get_optional(ov5693->dev, "powerdown",
+						    GPIOD_OUT_HIGH);
+	if (IS_ERR(ov5693->powerdown)) {
+		dev_err(ov5693->dev, "Error fetching powerdown GPIO\n");
+		return PTR_ERR(ov5693->powerdown);
+	}
+
+	ov5693->privacy_led = devm_gpiod_get_optional(ov5693->dev, "privacy-led",
+						      GPIOD_OUT_LOW);
+	if (IS_ERR(ov5693->privacy_led)) {
+		dev_err(ov5693->dev, "Error fetching privacy-led GPIO\n");
+		return PTR_ERR(ov5693->privacy_led);
+	}
+
+	return 0;
+}
+
+static int ov5693_get_regulators(struct ov5693_device *ov5693)
+{
+	unsigned int i;
+
+	for (i = 0; i < OV5693_NUM_SUPPLIES; i++)
+		ov5693->supplies[i].supply = ov5693_supply_names[i];
+
+	return devm_regulator_bulk_get(ov5693->dev, OV5693_NUM_SUPPLIES,
+				       ov5693->supplies);
+}
+
+static int ov5693_check_hwcfg(struct ov5693_device *ov5693)
+{
+	struct fwnode_handle *fwnode = dev_fwnode(ov5693->dev);
+	struct v4l2_fwnode_endpoint bus_cfg = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY,
+	};
+	struct fwnode_handle *endpoint;
+	unsigned int i;
+	int ret;
+
+	endpoint = fwnode_graph_get_next_endpoint(fwnode, NULL);
+	if (!endpoint)
+		return -EPROBE_DEFER; /* Could be provided by cio2-bridge */
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(endpoint, &bus_cfg);
+	fwnode_handle_put(endpoint);
+	if (ret)
+		return ret;
+
+	if (bus_cfg.bus.mipi_csi2.num_data_lanes != 2) {
+		dev_err(ov5693->dev, "only a 2-lane CSI2 config is supported");
+		ret = -EINVAL;
+		goto out_free_bus_cfg;
+	}
+
+	if (!bus_cfg.nr_of_link_frequencies) {
+		dev_err(ov5693->dev, "no link frequencies defined\n");
+		ret = -EINVAL;
+		goto out_free_bus_cfg;
+	}
+
+	for (i = 0; i < bus_cfg.nr_of_link_frequencies; i++)
+		if (bus_cfg.link_frequencies[i] == OV5693_LINK_FREQ_419_2MHZ)
+			break;
+
+	if (i == bus_cfg.nr_of_link_frequencies) {
+		dev_err(ov5693->dev, "supported link freq %ull not found\n",
+			OV5693_LINK_FREQ_419_2MHZ);
+		ret = -EINVAL;
+		goto out_free_bus_cfg;
+	}
+
+out_free_bus_cfg:
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+
+	return ret;
+}
+
+static int ov5693_probe(struct i2c_client *client)
+{
+	struct ov5693_device *ov5693;
+	u32 xvclk_rate;
+	int ret = 0;
+
+	ov5693 = devm_kzalloc(&client->dev, sizeof(*ov5693), GFP_KERNEL);
+	if (!ov5693)
+		return -ENOMEM;
+
+	ov5693->dev = &client->dev;
+
+	ov5693->regmap = devm_cci_regmap_init_i2c(client, 16);
+	if (IS_ERR(ov5693->regmap))
+		return PTR_ERR(ov5693->regmap);
+
+	ret = ov5693_check_hwcfg(ov5693);
+	if (ret)
+		return ret;
+
+	mutex_init(&ov5693->lock);
+
+	v4l2_i2c_subdev_init(&ov5693->sd, client, &ov5693_ops);
+
+	ov5693->xvclk = devm_clk_get_optional(&client->dev, "xvclk");
+	if (IS_ERR(ov5693->xvclk))
+		return dev_err_probe(&client->dev, PTR_ERR(ov5693->xvclk),
+				     "failed to get xvclk: %ld\n",
+				     PTR_ERR(ov5693->xvclk));
+
+	if (ov5693->xvclk) {
+		xvclk_rate = clk_get_rate(ov5693->xvclk);
+	} else {
+		ret = fwnode_property_read_u32(dev_fwnode(&client->dev),
+				     "clock-frequency",
+				     &xvclk_rate);
+
+		if (ret) {
+			dev_err(&client->dev, "can't get clock frequency");
+			return ret;
+		}
+	}
+
+	if (xvclk_rate != OV5693_XVCLK_FREQ)
+		dev_warn(&client->dev, "Found clk freq %u, expected %u\n",
+			 xvclk_rate, OV5693_XVCLK_FREQ);
+
+	ret = ov5693_configure_gpios(ov5693);
+	if (ret)
+		return ret;
+
+	ret = ov5693_get_regulators(ov5693);
+	if (ret)
+		return dev_err_probe(&client->dev, ret,
+				     "Error fetching regulators\n");
+
+	ov5693->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	ov5693->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ov5693->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+	ov5693->mode.crop = ov5693_default_crop;
+	ov5693->mode.format = ov5693_default_fmt;
+	ov5693->mode.vts = __ov5693_calc_vts(ov5693->mode.format.height);
+
+	ret = ov5693_init_controls(ov5693);
+	if (ret)
+		return ret;
+
+	ret = media_entity_pads_init(&ov5693->sd.entity, 1, &ov5693->pad);
+	if (ret)
+		goto err_ctrl_handler_free;
+
+	/*
+	 * We need the driver to work in the event that pm runtime is disable in
+	 * the kernel, so power up and verify the chip now. In the event that
+	 * runtime pm is disabled this will leave the chip on, so that streaming
+	 * will work.
+	 */
+
+	ret = ov5693_sensor_powerup(ov5693);
+	if (ret)
+		goto err_media_entity_cleanup;
+
+	ret = ov5693_detect(ov5693);
+	if (ret)
+		goto err_powerdown;
+
+	pm_runtime_set_active(&client->dev);
+	pm_runtime_get_noresume(&client->dev);
+	pm_runtime_enable(&client->dev);
+
+	ret = v4l2_async_register_subdev_sensor(&ov5693->sd);
+	if (ret) {
+		dev_err(&client->dev, "failed to register V4L2 subdev: %d",
+			ret);
+		goto err_pm_runtime;
+	}
+
+	pm_runtime_set_autosuspend_delay(&client->dev, 1000);
+	pm_runtime_use_autosuspend(&client->dev);
+	pm_runtime_put_autosuspend(&client->dev);
+
+	return ret;
+
+err_pm_runtime:
+	pm_runtime_disable(&client->dev);
+	pm_runtime_put_noidle(&client->dev);
+err_powerdown:
+	ov5693_sensor_powerdown(ov5693);
+err_media_entity_cleanup:
+	media_entity_cleanup(&ov5693->sd.entity);
+err_ctrl_handler_free:
+	v4l2_ctrl_handler_free(&ov5693->ctrls.handler);
+
+	return ret;
+}
+
+static void ov5693_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov5693_device *ov5693 = to_ov5693_sensor(sd);
+
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&ov5693->sd.entity);
+	v4l2_ctrl_handler_free(&ov5693->ctrls.handler);
+	mutex_destroy(&ov5693->lock);
+
+	/*
+	 * Disable runtime PM. In case runtime PM is disabled in the kernel,
+	 * make sure to turn power off manually.
+	 */
+	pm_runtime_disable(&client->dev);
+	if (!pm_runtime_status_suspended(&client->dev))
+		ov5693_sensor_powerdown(ov5693);
+	pm_runtime_set_suspended(&client->dev);
+}
+
+static const struct dev_pm_ops ov5693_pm_ops = {
+	SET_RUNTIME_PM_OPS(ov5693_sensor_suspend, ov5693_sensor_resume, NULL)
+};
+
+static const struct acpi_device_id ov5693_acpi_match[] = {
+	{"INT33BE"},
+	{},
+};
+MODULE_DEVICE_TABLE(acpi, ov5693_acpi_match);
+
+static const struct of_device_id ov5693_of_match[] = {
+	{ .compatible = "ovti,ov5693", },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, ov5693_of_match);
+
+static struct i2c_driver ov5693_driver = {
+	.driver = {
+		.name = "ov5693",
+		.acpi_match_table = ov5693_acpi_match,
+		.of_match_table = ov5693_of_match,
+		.pm = &ov5693_pm_ops,
+	},
+	.probe = ov5693_probe,
+	.remove = ov5693_remove,
+};
+module_i2c_driver(ov5693_driver);
+
+MODULE_DESCRIPTION("A low-level driver for OmniVision 5693 sensors");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/ov5695.c b/drivers/media/i2c/ov5695.c
new file mode 100644
index 000000000..3023b7254
--- /dev/null
+++ b/drivers/media/i2c/ov5695.c
@@ -0,0 +1,1402 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * ov5695 driver
+ *
+ * Copyright (C) 2017 Fuzhou Rockchip Electronics Co., Ltd.
+ */
+
+#include <linux/clk.h>
+#include <linux/device.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 <linux/sysfs.h>
+#include <media/media-entity.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-subdev.h>
+
+#ifndef V4L2_CID_DIGITAL_GAIN
+#define V4L2_CID_DIGITAL_GAIN		V4L2_CID_GAIN
+#endif
+
+/* 45Mhz * 4 Binning */
+#define OV5695_PIXEL_RATE		(45 * 1000 * 1000 * 4)
+#define OV5695_XVCLK_FREQ		24000000
+
+#define CHIP_ID				0x005695
+#define OV5695_REG_CHIP_ID		0x300a
+
+#define OV5695_REG_CTRL_MODE		0x0100
+#define OV5695_MODE_SW_STANDBY		0x0
+#define OV5695_MODE_STREAMING		BIT(0)
+
+#define OV5695_REG_EXPOSURE		0x3500
+#define	OV5695_EXPOSURE_MIN		4
+#define	OV5695_EXPOSURE_STEP		1
+#define OV5695_VTS_MAX			0x7fff
+
+#define OV5695_REG_ANALOG_GAIN		0x3509
+#define	ANALOG_GAIN_MIN			0x10
+#define	ANALOG_GAIN_MAX			0xf8
+#define	ANALOG_GAIN_STEP		1
+#define	ANALOG_GAIN_DEFAULT		0xf8
+
+#define OV5695_REG_DIGI_GAIN_H		0x350a
+#define OV5695_REG_DIGI_GAIN_L		0x350b
+#define OV5695_DIGI_GAIN_L_MASK		0x3f
+#define OV5695_DIGI_GAIN_H_SHIFT	6
+#define OV5695_DIGI_GAIN_MIN		0
+#define OV5695_DIGI_GAIN_MAX		(0x4000 - 1)
+#define OV5695_DIGI_GAIN_STEP		1
+#define OV5695_DIGI_GAIN_DEFAULT	1024
+
+#define OV5695_REG_TEST_PATTERN		0x4503
+#define	OV5695_TEST_PATTERN_ENABLE	0x80
+#define	OV5695_TEST_PATTERN_DISABLE	0x0
+
+#define OV5695_REG_VTS			0x380e
+
+#define REG_NULL			0xFFFF
+
+#define OV5695_REG_VALUE_08BIT		1
+#define OV5695_REG_VALUE_16BIT		2
+#define OV5695_REG_VALUE_24BIT		3
+
+#define OV5695_LANES			2
+#define OV5695_BITS_PER_SAMPLE		10
+
+static const char * const ov5695_supply_names[] = {
+	"avdd",		/* Analog power */
+	"dovdd",	/* Digital I/O power */
+	"dvdd",		/* Digital core power */
+};
+
+#define OV5695_NUM_SUPPLIES ARRAY_SIZE(ov5695_supply_names)
+
+struct regval {
+	u16 addr;
+	u8 val;
+};
+
+struct ov5695_mode {
+	u32 width;
+	u32 height;
+	u32 max_fps;
+	u32 hts_def;
+	u32 vts_def;
+	u32 exp_def;
+	const struct regval *reg_list;
+};
+
+struct ov5695 {
+	struct i2c_client	*client;
+	struct clk		*xvclk;
+	struct gpio_desc	*reset_gpio;
+	struct regulator_bulk_data supplies[OV5695_NUM_SUPPLIES];
+
+	struct v4l2_subdev	subdev;
+	struct media_pad	pad;
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_ctrl	*exposure;
+	struct v4l2_ctrl	*anal_gain;
+	struct v4l2_ctrl	*digi_gain;
+	struct v4l2_ctrl	*hblank;
+	struct v4l2_ctrl	*vblank;
+	struct v4l2_ctrl	*test_pattern;
+	struct mutex		mutex;
+	bool			streaming;
+	const struct ov5695_mode *cur_mode;
+};
+
+#define to_ov5695(sd) container_of(sd, struct ov5695, subdev)
+
+/*
+ * Xclk 24Mhz
+ * Pclk 45Mhz
+ * linelength 672(0x2a0)
+ * framelength 2232(0x8b8)
+ * grabwindow_width 1296
+ * grabwindow_height 972
+ * max_framerate 30fps
+ * mipi_datarate per lane 840Mbps
+ */
+static const struct regval ov5695_global_regs[] = {
+	{0x0103, 0x01},
+	{0x0100, 0x00},
+	{0x0300, 0x04},
+	{0x0301, 0x00},
+	{0x0302, 0x69},
+	{0x0303, 0x00},
+	{0x0304, 0x00},
+	{0x0305, 0x01},
+	{0x0307, 0x00},
+	{0x030b, 0x00},
+	{0x030c, 0x00},
+	{0x030d, 0x1e},
+	{0x030e, 0x04},
+	{0x030f, 0x03},
+	{0x0312, 0x01},
+	{0x3000, 0x00},
+	{0x3002, 0xa1},
+	{0x3008, 0x00},
+	{0x3010, 0x00},
+	{0x3022, 0x51},
+	{0x3106, 0x15},
+	{0x3107, 0x01},
+	{0x3108, 0x05},
+	{0x3500, 0x00},
+	{0x3501, 0x45},
+	{0x3502, 0x00},
+	{0x3503, 0x08},
+	{0x3504, 0x03},
+	{0x3505, 0x8c},
+	{0x3507, 0x03},
+	{0x3508, 0x00},
+	{0x3509, 0x10},
+	{0x350c, 0x00},
+	{0x350d, 0x80},
+	{0x3510, 0x00},
+	{0x3511, 0x02},
+	{0x3512, 0x00},
+	{0x3601, 0x55},
+	{0x3602, 0x58},
+	{0x3614, 0x30},
+	{0x3615, 0x77},
+	{0x3621, 0x08},
+	{0x3624, 0x40},
+	{0x3633, 0x0c},
+	{0x3634, 0x0c},
+	{0x3635, 0x0c},
+	{0x3636, 0x0c},
+	{0x3638, 0x00},
+	{0x3639, 0x00},
+	{0x363a, 0x00},
+	{0x363b, 0x00},
+	{0x363c, 0xff},
+	{0x363d, 0xfa},
+	{0x3650, 0x44},
+	{0x3651, 0x44},
+	{0x3652, 0x44},
+	{0x3653, 0x44},
+	{0x3654, 0x44},
+	{0x3655, 0x44},
+	{0x3656, 0x44},
+	{0x3657, 0x44},
+	{0x3660, 0x00},
+	{0x3661, 0x00},
+	{0x3662, 0x00},
+	{0x366a, 0x00},
+	{0x366e, 0x0c},
+	{0x3673, 0x04},
+	{0x3700, 0x14},
+	{0x3703, 0x0c},
+	{0x3715, 0x01},
+	{0x3733, 0x10},
+	{0x3734, 0x40},
+	{0x373f, 0xa0},
+	{0x3765, 0x20},
+	{0x37a1, 0x1d},
+	{0x37a8, 0x26},
+	{0x37ab, 0x14},
+	{0x37c2, 0x04},
+	{0x37cb, 0x09},
+	{0x37cc, 0x13},
+	{0x37cd, 0x1f},
+	{0x37ce, 0x1f},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x00},
+	{0x3804, 0x0a},
+	{0x3805, 0x3f},
+	{0x3806, 0x07},
+	{0x3807, 0xaf},
+	{0x3808, 0x05},
+	{0x3809, 0x10},
+	{0x380a, 0x03},
+	{0x380b, 0xcc},
+	{0x380c, 0x02},
+	{0x380d, 0xa0},
+	{0x380e, 0x08},
+	{0x380f, 0xb8},
+	{0x3810, 0x00},
+	{0x3811, 0x06},
+	{0x3812, 0x00},
+	{0x3813, 0x06},
+	{0x3814, 0x03},
+	{0x3815, 0x01},
+	{0x3816, 0x03},
+	{0x3817, 0x01},
+	{0x3818, 0x00},
+	{0x3819, 0x00},
+	{0x381a, 0x00},
+	{0x381b, 0x01},
+	{0x3820, 0x8b},
+	{0x3821, 0x01},
+	{0x3c80, 0x08},
+	{0x3c82, 0x00},
+	{0x3c83, 0x00},
+	{0x3c88, 0x00},
+	{0x3d85, 0x14},
+	{0x3f02, 0x08},
+	{0x3f03, 0x10},
+	{0x4008, 0x02},
+	{0x4009, 0x09},
+	{0x404e, 0x20},
+	{0x4501, 0x00},
+	{0x4502, 0x10},
+	{0x4800, 0x00},
+	{0x481f, 0x2a},
+	{0x4837, 0x13},
+	{0x5000, 0x17},
+	{0x5780, 0x3e},
+	{0x5781, 0x0f},
+	{0x5782, 0x44},
+	{0x5783, 0x02},
+	{0x5784, 0x01},
+	{0x5785, 0x01},
+	{0x5786, 0x00},
+	{0x5787, 0x04},
+	{0x5788, 0x02},
+	{0x5789, 0x0f},
+	{0x578a, 0xfd},
+	{0x578b, 0xf5},
+	{0x578c, 0xf5},
+	{0x578d, 0x03},
+	{0x578e, 0x08},
+	{0x578f, 0x0c},
+	{0x5790, 0x08},
+	{0x5791, 0x06},
+	{0x5792, 0x00},
+	{0x5793, 0x52},
+	{0x5794, 0xa3},
+	{0x5b00, 0x00},
+	{0x5b01, 0x1c},
+	{0x5b02, 0x00},
+	{0x5b03, 0x7f},
+	{0x5b05, 0x6c},
+	{0x5e10, 0xfc},
+	{0x4010, 0xf1},
+	{0x3503, 0x08},
+	{0x3505, 0x8c},
+	{0x3507, 0x03},
+	{0x3508, 0x00},
+	{0x3509, 0xf8},
+	{REG_NULL, 0x00},
+};
+
+/*
+ * Xclk 24Mhz
+ * Pclk 45Mhz
+ * linelength 740(0x2e4)
+ * framelength 2024(0x7e8)
+ * grabwindow_width 2592
+ * grabwindow_height 1944
+ * max_framerate 30fps
+ * mipi_datarate per lane 840Mbps
+ */
+static const struct regval ov5695_2592x1944_regs[] = {
+	{0x3501, 0x7e},
+	{0x366e, 0x18},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x04},
+	{0x3804, 0x0a},
+	{0x3805, 0x3f},
+	{0x3806, 0x07},
+	{0x3807, 0xab},
+	{0x3808, 0x0a},
+	{0x3809, 0x20},
+	{0x380a, 0x07},
+	{0x380b, 0x98},
+	{0x380c, 0x02},
+	{0x380d, 0xe4},
+	{0x380e, 0x07},
+	{0x380f, 0xe8},
+	{0x3811, 0x06},
+	{0x3813, 0x08},
+	{0x3814, 0x01},
+	{0x3816, 0x01},
+	{0x3817, 0x01},
+	{0x3820, 0x88},
+	{0x3821, 0x00},
+	{0x4501, 0x00},
+	{0x4008, 0x04},
+	{0x4009, 0x13},
+	{REG_NULL, 0x00},
+};
+
+/*
+ * Xclk 24Mhz
+ * Pclk 45Mhz
+ * linelength 672(0x2a0)
+ * framelength 2232(0x8b8)
+ * grabwindow_width 1920
+ * grabwindow_height 1080
+ * max_framerate 30fps
+ * mipi_datarate per lane 840Mbps
+ */
+static const struct regval ov5695_1920x1080_regs[] = {
+	{0x3501, 0x45},
+	{0x366e, 0x18},
+	{0x3800, 0x01},
+	{0x3801, 0x50},
+	{0x3802, 0x01},
+	{0x3803, 0xb8},
+	{0x3804, 0x08},
+	{0x3805, 0xef},
+	{0x3806, 0x05},
+	{0x3807, 0xf7},
+	{0x3808, 0x07},
+	{0x3809, 0x80},
+	{0x380a, 0x04},
+	{0x380b, 0x38},
+	{0x380c, 0x02},
+	{0x380d, 0xa0},
+	{0x380e, 0x08},
+	{0x380f, 0xb8},
+	{0x3811, 0x06},
+	{0x3813, 0x04},
+	{0x3814, 0x01},
+	{0x3816, 0x01},
+	{0x3817, 0x01},
+	{0x3820, 0x88},
+	{0x3821, 0x00},
+	{0x4501, 0x00},
+	{0x4008, 0x04},
+	{0x4009, 0x13},
+	{REG_NULL, 0x00}
+};
+
+/*
+ * Xclk 24Mhz
+ * Pclk 45Mhz
+ * linelength 740(0x02e4)
+ * framelength 1012(0x03f4)
+ * grabwindow_width 1296
+ * grabwindow_height 972
+ * max_framerate 60fps
+ * mipi_datarate per lane 840Mbps
+ */
+static const struct regval ov5695_1296x972_regs[] = {
+	{0x0103, 0x01},
+	{0x0100, 0x00},
+	{0x0300, 0x04},
+	{0x0301, 0x00},
+	{0x0302, 0x69},
+	{0x0303, 0x00},
+	{0x0304, 0x00},
+	{0x0305, 0x01},
+	{0x0307, 0x00},
+	{0x030b, 0x00},
+	{0x030c, 0x00},
+	{0x030d, 0x1e},
+	{0x030e, 0x04},
+	{0x030f, 0x03},
+	{0x0312, 0x01},
+	{0x3000, 0x00},
+	{0x3002, 0xa1},
+	{0x3008, 0x00},
+	{0x3010, 0x00},
+	{0x3016, 0x32},
+	{0x3022, 0x51},
+	{0x3106, 0x15},
+	{0x3107, 0x01},
+	{0x3108, 0x05},
+	{0x3500, 0x00},
+	{0x3501, 0x3e},
+	{0x3502, 0x00},
+	{0x3503, 0x08},
+	{0x3504, 0x03},
+	{0x3505, 0x8c},
+	{0x3507, 0x03},
+	{0x3508, 0x00},
+	{0x3509, 0x10},
+	{0x350c, 0x00},
+	{0x350d, 0x80},
+	{0x3510, 0x00},
+	{0x3511, 0x02},
+	{0x3512, 0x00},
+	{0x3601, 0x55},
+	{0x3602, 0x58},
+	{0x3611, 0x58},
+	{0x3614, 0x30},
+	{0x3615, 0x77},
+	{0x3621, 0x08},
+	{0x3624, 0x40},
+	{0x3633, 0x0c},
+	{0x3634, 0x0c},
+	{0x3635, 0x0c},
+	{0x3636, 0x0c},
+	{0x3638, 0x00},
+	{0x3639, 0x00},
+	{0x363a, 0x00},
+	{0x363b, 0x00},
+	{0x363c, 0xff},
+	{0x363d, 0xfa},
+	{0x3650, 0x44},
+	{0x3651, 0x44},
+	{0x3652, 0x44},
+	{0x3653, 0x44},
+	{0x3654, 0x44},
+	{0x3655, 0x44},
+	{0x3656, 0x44},
+	{0x3657, 0x44},
+	{0x3660, 0x00},
+	{0x3661, 0x00},
+	{0x3662, 0x00},
+	{0x366a, 0x00},
+	{0x366e, 0x0c},
+	{0x3673, 0x04},
+	{0x3700, 0x14},
+	{0x3703, 0x0c},
+	{0x3706, 0x24},
+	{0x3714, 0x27},
+	{0x3715, 0x01},
+	{0x3716, 0x00},
+	{0x3717, 0x02},
+	{0x3733, 0x10},
+	{0x3734, 0x40},
+	{0x373f, 0xa0},
+	{0x3765, 0x20},
+	{0x37a1, 0x1d},
+	{0x37a8, 0x26},
+	{0x37ab, 0x14},
+	{0x37c2, 0x04},
+	{0x37c3, 0xf0},
+	{0x37cb, 0x09},
+	{0x37cc, 0x13},
+	{0x37cd, 0x1f},
+	{0x37ce, 0x1f},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x00},
+	{0x3804, 0x0a},
+	{0x3805, 0x3f},
+	{0x3806, 0x07},
+	{0x3807, 0xaf},
+	{0x3808, 0x05},
+	{0x3809, 0x10},
+	{0x380a, 0x03},
+	{0x380b, 0xcc},
+	{0x380c, 0x02},
+	{0x380d, 0xe4},
+	{0x380e, 0x03},
+	{0x380f, 0xf4},
+	{0x3810, 0x00},
+	{0x3811, 0x00},
+	{0x3812, 0x00},
+	{0x3813, 0x06},
+	{0x3814, 0x03},
+	{0x3815, 0x01},
+	{0x3816, 0x03},
+	{0x3817, 0x01},
+	{0x3818, 0x00},
+	{0x3819, 0x00},
+	{0x381a, 0x00},
+	{0x381b, 0x01},
+	{0x3820, 0x8b},
+	{0x3821, 0x01},
+	{0x3c80, 0x08},
+	{0x3c82, 0x00},
+	{0x3c83, 0x00},
+	{0x3c88, 0x00},
+	{0x3d85, 0x14},
+	{0x3f02, 0x08},
+	{0x3f03, 0x10},
+	{0x4008, 0x02},
+	{0x4009, 0x09},
+	{0x404e, 0x20},
+	{0x4501, 0x00},
+	{0x4502, 0x10},
+	{0x4800, 0x00},
+	{0x481f, 0x2a},
+	{0x4837, 0x13},
+	{0x5000, 0x13},
+	{0x5780, 0x3e},
+	{0x5781, 0x0f},
+	{0x5782, 0x44},
+	{0x5783, 0x02},
+	{0x5784, 0x01},
+	{0x5785, 0x01},
+	{0x5786, 0x00},
+	{0x5787, 0x04},
+	{0x5788, 0x02},
+	{0x5789, 0x0f},
+	{0x578a, 0xfd},
+	{0x578b, 0xf5},
+	{0x578c, 0xf5},
+	{0x578d, 0x03},
+	{0x578e, 0x08},
+	{0x578f, 0x0c},
+	{0x5790, 0x08},
+	{0x5791, 0x06},
+	{0x5792, 0x00},
+	{0x5793, 0x52},
+	{0x5794, 0xa3},
+	{0x5b00, 0x00},
+	{0x5b01, 0x1c},
+	{0x5b02, 0x00},
+	{0x5b03, 0x7f},
+	{0x5b05, 0x6c},
+	{0x5e10, 0xfc},
+	{0x4010, 0xf1},
+	{0x3503, 0x08},
+	{0x3505, 0x8c},
+	{0x3507, 0x03},
+	{0x3508, 0x00},
+	{0x3509, 0xf8},
+	{0x0100, 0x01},
+	{REG_NULL, 0x00}
+};
+
+/*
+ * Xclk 24Mhz
+ * Pclk 45Mhz
+ * linelength 672(0x2a0)
+ * framelength 2232(0x8b8)
+ * grabwindow_width 1280
+ * grabwindow_height 720
+ * max_framerate 30fps
+ * mipi_datarate per lane 840Mbps
+ */
+static const struct regval ov5695_1280x720_regs[] = {
+	{0x3501, 0x45},
+	{0x366e, 0x0c},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x01},
+	{0x3803, 0x00},
+	{0x3804, 0x0a},
+	{0x3805, 0x3f},
+	{0x3806, 0x06},
+	{0x3807, 0xaf},
+	{0x3808, 0x05},
+	{0x3809, 0x00},
+	{0x380a, 0x02},
+	{0x380b, 0xd0},
+	{0x380c, 0x02},
+	{0x380d, 0xa0},
+	{0x380e, 0x08},
+	{0x380f, 0xb8},
+	{0x3811, 0x06},
+	{0x3813, 0x02},
+	{0x3814, 0x03},
+	{0x3816, 0x03},
+	{0x3817, 0x01},
+	{0x3820, 0x8b},
+	{0x3821, 0x01},
+	{0x4501, 0x00},
+	{0x4008, 0x02},
+	{0x4009, 0x09},
+	{REG_NULL, 0x00}
+};
+
+/*
+ * Xclk 24Mhz
+ * Pclk 45Mhz
+ * linelength 672(0x2a0)
+ * framelength 558(0x22e)
+ * grabwindow_width 640
+ * grabwindow_height 480
+ * max_framerate 120fps
+ * mipi_datarate per lane 840Mbps
+ */
+static const struct regval ov5695_640x480_regs[] = {
+	{0x3501, 0x22},
+	{0x366e, 0x0c},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x08},
+	{0x3804, 0x0a},
+	{0x3805, 0x3f},
+	{0x3806, 0x07},
+	{0x3807, 0xa7},
+	{0x3808, 0x02},
+	{0x3809, 0x80},
+	{0x380a, 0x01},
+	{0x380b, 0xe0},
+	{0x380c, 0x02},
+	{0x380d, 0xa0},
+	{0x380e, 0x02},
+	{0x380f, 0x2e},
+	{0x3811, 0x06},
+	{0x3813, 0x04},
+	{0x3814, 0x07},
+	{0x3816, 0x05},
+	{0x3817, 0x03},
+	{0x3820, 0x8d},
+	{0x3821, 0x01},
+	{0x4501, 0x00},
+	{0x4008, 0x02},
+	{0x4009, 0x09},
+	{REG_NULL, 0x00}
+};
+
+static const struct ov5695_mode supported_modes[] = {
+	{
+		.width = 2592,
+		.height = 1944,
+		.max_fps = 30,
+		.exp_def = 0x0450,
+		.hts_def = 0x02e4 * 4,
+		.vts_def = 0x07e8,
+		.reg_list = ov5695_2592x1944_regs,
+	},
+	{
+		.width = 1920,
+		.height = 1080,
+		.max_fps = 30,
+		.exp_def = 0x0450,
+		.hts_def = 0x02a0 * 4,
+		.vts_def = 0x08b8,
+		.reg_list = ov5695_1920x1080_regs,
+	},
+	{
+		.width = 1296,
+		.height = 972,
+		.max_fps = 60,
+		.exp_def = 0x03e0,
+		.hts_def = 0x02e4 * 4,
+		.vts_def = 0x03f4,
+		.reg_list = ov5695_1296x972_regs,
+	},
+	{
+		.width = 1280,
+		.height = 720,
+		.max_fps = 30,
+		.exp_def = 0x0450,
+		.hts_def = 0x02a0 * 4,
+		.vts_def = 0x08b8,
+		.reg_list = ov5695_1280x720_regs,
+	},
+	{
+		.width = 640,
+		.height = 480,
+		.max_fps = 120,
+		.exp_def = 0x0450,
+		.hts_def = 0x02a0 * 4,
+		.vts_def = 0x022e,
+		.reg_list = ov5695_640x480_regs,
+	},
+};
+
+#define OV5695_LINK_FREQ_420MHZ		420000000
+static const s64 link_freq_menu_items[] = {
+	OV5695_LINK_FREQ_420MHZ
+};
+
+static const char * const ov5695_test_pattern_menu[] = {
+	"Disabled",
+	"Vertical Color Bar Type 1",
+	"Vertical Color Bar Type 2",
+	"Vertical Color Bar Type 3",
+	"Vertical Color Bar Type 4"
+};
+
+/* Write registers up to 4 at a time */
+static int ov5695_write_reg(struct i2c_client *client, u16 reg,
+			    u32 len, u32 val)
+{
+	u32 buf_i, val_i;
+	u8 buf[6];
+	u8 *val_p;
+	__be32 val_be;
+
+	if (len > 4)
+		return -EINVAL;
+
+	buf[0] = reg >> 8;
+	buf[1] = reg & 0xff;
+
+	val_be = cpu_to_be32(val);
+	val_p = (u8 *)&val_be;
+	buf_i = 2;
+	val_i = 4 - len;
+
+	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;
+}
+
+static int ov5695_write_array(struct i2c_client *client,
+			      const struct regval *regs)
+{
+	u32 i;
+	int ret = 0;
+
+	for (i = 0; ret == 0 && regs[i].addr != REG_NULL; i++)
+		ret = ov5695_write_reg(client, regs[i].addr,
+				       OV5695_REG_VALUE_08BIT, regs[i].val);
+
+	return ret;
+}
+
+/* Read registers up to 4 at a time */
+static int ov5695_read_reg(struct i2c_client *client, u16 reg, unsigned int len,
+			   u32 *val)
+{
+	struct i2c_msg msgs[2];
+	u8 *data_be_p;
+	__be32 data_be = 0;
+	__be16 reg_addr_be = cpu_to_be16(reg);
+	int ret;
+
+	if (len > 4)
+		return -EINVAL;
+
+	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[4 - len];
+
+	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 ov5695_get_reso_dist(const struct ov5695_mode *mode,
+				struct v4l2_mbus_framefmt *framefmt)
+{
+	return abs(mode->width - framefmt->width) +
+	       abs(mode->height - framefmt->height);
+}
+
+static const struct ov5695_mode *
+ov5695_find_best_fit(struct v4l2_subdev_format *fmt)
+{
+	struct v4l2_mbus_framefmt *framefmt = &fmt->format;
+	int dist;
+	int cur_best_fit = 0;
+	int cur_best_fit_dist = -1;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(supported_modes); i++) {
+		dist = ov5695_get_reso_dist(&supported_modes[i], framefmt);
+		if (cur_best_fit_dist == -1 || dist < cur_best_fit_dist) {
+			cur_best_fit_dist = dist;
+			cur_best_fit = i;
+		}
+	}
+
+	return &supported_modes[cur_best_fit];
+}
+
+static int ov5695_set_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct ov5695 *ov5695 = to_ov5695(sd);
+	const struct ov5695_mode *mode;
+	s64 h_blank, vblank_def;
+
+	mutex_lock(&ov5695->mutex);
+
+	mode = ov5695_find_best_fit(fmt);
+	fmt->format.code = MEDIA_BUS_FMT_SBGGR10_1X10;
+	fmt->format.width = mode->width;
+	fmt->format.height = mode->height;
+	fmt->format.field = V4L2_FIELD_NONE;
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
+		*v4l2_subdev_get_try_format(sd, sd_state, fmt->pad) = fmt->format;
+#endif
+	} else {
+		ov5695->cur_mode = mode;
+		h_blank = mode->hts_def - mode->width;
+		__v4l2_ctrl_modify_range(ov5695->hblank, h_blank,
+					 h_blank, 1, h_blank);
+		vblank_def = mode->vts_def - mode->height;
+		__v4l2_ctrl_modify_range(ov5695->vblank, vblank_def,
+					 OV5695_VTS_MAX - mode->height,
+					 1, vblank_def);
+	}
+
+	mutex_unlock(&ov5695->mutex);
+
+	return 0;
+}
+
+static int ov5695_get_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct ov5695 *ov5695 = to_ov5695(sd);
+	const struct ov5695_mode *mode = ov5695->cur_mode;
+
+	mutex_lock(&ov5695->mutex);
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
+		fmt->format = *v4l2_subdev_get_try_format(sd, sd_state,
+							  fmt->pad);
+#else
+		mutex_unlock(&ov5695->mutex);
+		return -EINVAL;
+#endif
+	} else {
+		fmt->format.width = mode->width;
+		fmt->format.height = mode->height;
+		fmt->format.code = MEDIA_BUS_FMT_SBGGR10_1X10;
+		fmt->format.field = V4L2_FIELD_NONE;
+	}
+	mutex_unlock(&ov5695->mutex);
+
+	return 0;
+}
+
+static int ov5695_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index != 0)
+		return -EINVAL;
+	code->code = MEDIA_BUS_FMT_SBGGR10_1X10;
+
+	return 0;
+}
+
+static int ov5695_enum_frame_sizes(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->index >= ARRAY_SIZE(supported_modes))
+		return -EINVAL;
+
+	if (fse->code != MEDIA_BUS_FMT_SBGGR10_1X10)
+		return -EINVAL;
+
+	fse->min_width  = supported_modes[fse->index].width;
+	fse->max_width  = supported_modes[fse->index].width;
+	fse->max_height = supported_modes[fse->index].height;
+	fse->min_height = supported_modes[fse->index].height;
+
+	return 0;
+}
+
+static int ov5695_enable_test_pattern(struct ov5695 *ov5695, u32 pattern)
+{
+	u32 val;
+
+	if (pattern)
+		val = (pattern - 1) | OV5695_TEST_PATTERN_ENABLE;
+	else
+		val = OV5695_TEST_PATTERN_DISABLE;
+
+	return ov5695_write_reg(ov5695->client, OV5695_REG_TEST_PATTERN,
+				OV5695_REG_VALUE_08BIT, val);
+}
+
+static int __ov5695_start_stream(struct ov5695 *ov5695)
+{
+	int ret;
+
+	ret = ov5695_write_array(ov5695->client, ov5695_global_regs);
+	if (ret)
+		return ret;
+	ret = ov5695_write_array(ov5695->client, ov5695->cur_mode->reg_list);
+	if (ret)
+		return ret;
+
+	/* In case these controls are set before streaming */
+	ret = __v4l2_ctrl_handler_setup(&ov5695->ctrl_handler);
+	if (ret)
+		return ret;
+
+	return ov5695_write_reg(ov5695->client, OV5695_REG_CTRL_MODE,
+				OV5695_REG_VALUE_08BIT, OV5695_MODE_STREAMING);
+}
+
+static int __ov5695_stop_stream(struct ov5695 *ov5695)
+{
+	return ov5695_write_reg(ov5695->client, OV5695_REG_CTRL_MODE,
+				OV5695_REG_VALUE_08BIT, OV5695_MODE_SW_STANDBY);
+}
+
+static int ov5695_s_stream(struct v4l2_subdev *sd, int on)
+{
+	struct ov5695 *ov5695 = to_ov5695(sd);
+	struct i2c_client *client = ov5695->client;
+	int ret = 0;
+
+	mutex_lock(&ov5695->mutex);
+	on = !!on;
+	if (on == ov5695->streaming)
+		goto unlock_and_return;
+
+	if (on) {
+		ret = pm_runtime_resume_and_get(&client->dev);
+		if (ret < 0)
+			goto unlock_and_return;
+
+		ret = __ov5695_start_stream(ov5695);
+		if (ret) {
+			v4l2_err(sd, "start stream failed while write regs\n");
+			pm_runtime_put(&client->dev);
+			goto unlock_and_return;
+		}
+	} else {
+		__ov5695_stop_stream(ov5695);
+		pm_runtime_put(&client->dev);
+	}
+
+	ov5695->streaming = on;
+
+unlock_and_return:
+	mutex_unlock(&ov5695->mutex);
+
+	return ret;
+}
+
+static int __ov5695_power_on(struct ov5695 *ov5695)
+{
+	int i, ret;
+	struct device *dev = &ov5695->client->dev;
+
+	ret = clk_prepare_enable(ov5695->xvclk);
+	if (ret < 0) {
+		dev_err(dev, "Failed to enable xvclk\n");
+		return ret;
+	}
+
+	gpiod_set_value_cansleep(ov5695->reset_gpio, 1);
+
+	/*
+	 * The hardware requires the regulators to be powered on in order,
+	 * so enable them one by one.
+	 */
+	for (i = 0; i < OV5695_NUM_SUPPLIES; i++) {
+		ret = regulator_enable(ov5695->supplies[i].consumer);
+		if (ret) {
+			dev_err(dev, "Failed to enable %s: %d\n",
+				ov5695->supplies[i].supply, ret);
+			goto disable_reg_clk;
+		}
+	}
+
+	gpiod_set_value_cansleep(ov5695->reset_gpio, 0);
+
+	usleep_range(1000, 1200);
+
+	return 0;
+
+disable_reg_clk:
+	for (--i; i >= 0; i--)
+		regulator_disable(ov5695->supplies[i].consumer);
+	clk_disable_unprepare(ov5695->xvclk);
+
+	return ret;
+}
+
+static void __ov5695_power_off(struct ov5695 *ov5695)
+{
+	struct device *dev = &ov5695->client->dev;
+	int i, ret;
+
+	clk_disable_unprepare(ov5695->xvclk);
+	gpiod_set_value_cansleep(ov5695->reset_gpio, 1);
+
+	/*
+	 * The hardware requires the regulators to be powered off in order,
+	 * so disable them one by one.
+	 */
+	for (i = OV5695_NUM_SUPPLIES - 1; i >= 0; i--) {
+		ret = regulator_disable(ov5695->supplies[i].consumer);
+		if (ret)
+			dev_err(dev, "Failed to disable %s: %d\n",
+				ov5695->supplies[i].supply, ret);
+	}
+}
+
+static int __maybe_unused ov5695_runtime_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov5695 *ov5695 = to_ov5695(sd);
+
+	return __ov5695_power_on(ov5695);
+}
+
+static int __maybe_unused ov5695_runtime_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov5695 *ov5695 = to_ov5695(sd);
+
+	__ov5695_power_off(ov5695);
+
+	return 0;
+}
+
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
+static int ov5695_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct ov5695 *ov5695 = to_ov5695(sd);
+	struct v4l2_mbus_framefmt *try_fmt =
+				v4l2_subdev_get_try_format(sd, fh->state, 0);
+	const struct ov5695_mode *def_mode = &supported_modes[0];
+
+	mutex_lock(&ov5695->mutex);
+	/* Initialize try_fmt */
+	try_fmt->width = def_mode->width;
+	try_fmt->height = def_mode->height;
+	try_fmt->code = MEDIA_BUS_FMT_SBGGR10_1X10;
+	try_fmt->field = V4L2_FIELD_NONE;
+
+	mutex_unlock(&ov5695->mutex);
+	/* No crop or compose */
+
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops ov5695_pm_ops = {
+	SET_RUNTIME_PM_OPS(ov5695_runtime_suspend,
+			   ov5695_runtime_resume, NULL)
+};
+
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
+static const struct v4l2_subdev_internal_ops ov5695_internal_ops = {
+	.open = ov5695_open,
+};
+#endif
+
+static const struct v4l2_subdev_video_ops ov5695_video_ops = {
+	.s_stream = ov5695_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops ov5695_pad_ops = {
+	.enum_mbus_code = ov5695_enum_mbus_code,
+	.enum_frame_size = ov5695_enum_frame_sizes,
+	.get_fmt = ov5695_get_fmt,
+	.set_fmt = ov5695_set_fmt,
+};
+
+static const struct v4l2_subdev_ops ov5695_subdev_ops = {
+	.video	= &ov5695_video_ops,
+	.pad	= &ov5695_pad_ops,
+};
+
+static int ov5695_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov5695 *ov5695 = container_of(ctrl->handler,
+					     struct ov5695, ctrl_handler);
+	struct i2c_client *client = ov5695->client;
+	s64 max;
+	int ret = 0;
+
+	/* Propagate change of current control to all related controls */
+	switch (ctrl->id) {
+	case V4L2_CID_VBLANK:
+		/* Update max exposure while meeting expected vblanking */
+		max = ov5695->cur_mode->height + ctrl->val - 4;
+		__v4l2_ctrl_modify_range(ov5695->exposure,
+					 ov5695->exposure->minimum, max,
+					 ov5695->exposure->step,
+					 ov5695->exposure->default_value);
+		break;
+	}
+
+	if (!pm_runtime_get_if_in_use(&client->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE:
+		/* 4 least significant bits of exposure are fractional part */
+		ret = ov5695_write_reg(ov5695->client, OV5695_REG_EXPOSURE,
+				       OV5695_REG_VALUE_24BIT, ctrl->val << 4);
+		break;
+	case V4L2_CID_ANALOGUE_GAIN:
+		ret = ov5695_write_reg(ov5695->client, OV5695_REG_ANALOG_GAIN,
+				       OV5695_REG_VALUE_08BIT, ctrl->val);
+		break;
+	case V4L2_CID_DIGITAL_GAIN:
+		ret = ov5695_write_reg(ov5695->client, OV5695_REG_DIGI_GAIN_L,
+				       OV5695_REG_VALUE_08BIT,
+				       ctrl->val & OV5695_DIGI_GAIN_L_MASK);
+		ret = ov5695_write_reg(ov5695->client, OV5695_REG_DIGI_GAIN_H,
+				       OV5695_REG_VALUE_08BIT,
+				       ctrl->val >> OV5695_DIGI_GAIN_H_SHIFT);
+		break;
+	case V4L2_CID_VBLANK:
+		ret = ov5695_write_reg(ov5695->client, OV5695_REG_VTS,
+				       OV5695_REG_VALUE_16BIT,
+				       ctrl->val + ov5695->cur_mode->height);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		ret = ov5695_enable_test_pattern(ov5695, ctrl->val);
+		break;
+	default:
+		dev_warn(&client->dev, "%s Unhandled id:0x%x, val:0x%x\n",
+			 __func__, ctrl->id, ctrl->val);
+		break;
+	}
+
+	pm_runtime_put(&client->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ov5695_ctrl_ops = {
+	.s_ctrl = ov5695_set_ctrl,
+};
+
+static int ov5695_initialize_controls(struct ov5695 *ov5695)
+{
+	const struct ov5695_mode *mode;
+	struct v4l2_ctrl_handler *handler;
+	struct v4l2_ctrl *ctrl;
+	s64 exposure_max, vblank_def;
+	u32 h_blank;
+	int ret;
+
+	handler = &ov5695->ctrl_handler;
+	mode = ov5695->cur_mode;
+	ret = v4l2_ctrl_handler_init(handler, 8);
+	if (ret)
+		return ret;
+	handler->lock = &ov5695->mutex;
+
+	ctrl = v4l2_ctrl_new_int_menu(handler, NULL, V4L2_CID_LINK_FREQ,
+				      0, 0, link_freq_menu_items);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	v4l2_ctrl_new_std(handler, NULL, V4L2_CID_PIXEL_RATE,
+			  0, OV5695_PIXEL_RATE, 1, OV5695_PIXEL_RATE);
+
+	h_blank = mode->hts_def - mode->width;
+	ov5695->hblank = v4l2_ctrl_new_std(handler, NULL, V4L2_CID_HBLANK,
+				h_blank, h_blank, 1, h_blank);
+	if (ov5695->hblank)
+		ov5695->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	vblank_def = mode->vts_def - mode->height;
+	ov5695->vblank = v4l2_ctrl_new_std(handler, &ov5695_ctrl_ops,
+				V4L2_CID_VBLANK, vblank_def,
+				OV5695_VTS_MAX - mode->height,
+				1, vblank_def);
+
+	exposure_max = mode->vts_def - 4;
+	ov5695->exposure = v4l2_ctrl_new_std(handler, &ov5695_ctrl_ops,
+				V4L2_CID_EXPOSURE, OV5695_EXPOSURE_MIN,
+				exposure_max, OV5695_EXPOSURE_STEP,
+				mode->exp_def);
+
+	ov5695->anal_gain = v4l2_ctrl_new_std(handler, &ov5695_ctrl_ops,
+				V4L2_CID_ANALOGUE_GAIN, ANALOG_GAIN_MIN,
+				ANALOG_GAIN_MAX, ANALOG_GAIN_STEP,
+				ANALOG_GAIN_DEFAULT);
+
+	/* Digital gain */
+	ov5695->digi_gain = v4l2_ctrl_new_std(handler, &ov5695_ctrl_ops,
+				V4L2_CID_DIGITAL_GAIN, OV5695_DIGI_GAIN_MIN,
+				OV5695_DIGI_GAIN_MAX, OV5695_DIGI_GAIN_STEP,
+				OV5695_DIGI_GAIN_DEFAULT);
+
+	ov5695->test_pattern = v4l2_ctrl_new_std_menu_items(handler,
+				&ov5695_ctrl_ops, V4L2_CID_TEST_PATTERN,
+				ARRAY_SIZE(ov5695_test_pattern_menu) - 1,
+				0, 0, ov5695_test_pattern_menu);
+
+	if (handler->error) {
+		ret = handler->error;
+		dev_err(&ov5695->client->dev,
+			"Failed to init controls(%d)\n", ret);
+		goto err_free_handler;
+	}
+
+	ov5695->subdev.ctrl_handler = handler;
+
+	return 0;
+
+err_free_handler:
+	v4l2_ctrl_handler_free(handler);
+
+	return ret;
+}
+
+static int ov5695_check_sensor_id(struct ov5695 *ov5695,
+				  struct i2c_client *client)
+{
+	struct device *dev = &ov5695->client->dev;
+	u32 id = 0;
+	int ret;
+
+	ret = ov5695_read_reg(client, OV5695_REG_CHIP_ID,
+			      OV5695_REG_VALUE_24BIT, &id);
+	if (id != CHIP_ID) {
+		dev_err(dev, "Unexpected sensor id(%06x), ret(%d)\n", id, ret);
+		return ret;
+	}
+
+	dev_info(dev, "Detected OV%06x sensor\n", CHIP_ID);
+
+	return 0;
+}
+
+static int ov5695_configure_regulators(struct ov5695 *ov5695)
+{
+	int i;
+
+	for (i = 0; i < OV5695_NUM_SUPPLIES; i++)
+		ov5695->supplies[i].supply = ov5695_supply_names[i];
+
+	return devm_regulator_bulk_get(&ov5695->client->dev,
+				       OV5695_NUM_SUPPLIES,
+				       ov5695->supplies);
+}
+
+static int ov5695_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct ov5695 *ov5695;
+	struct v4l2_subdev *sd;
+	int ret;
+
+	ov5695 = devm_kzalloc(dev, sizeof(*ov5695), GFP_KERNEL);
+	if (!ov5695)
+		return -ENOMEM;
+
+	ov5695->client = client;
+	ov5695->cur_mode = &supported_modes[0];
+
+	ov5695->xvclk = devm_clk_get(dev, "xvclk");
+	if (IS_ERR(ov5695->xvclk)) {
+		dev_err(dev, "Failed to get xvclk\n");
+		return -EINVAL;
+	}
+	ret = clk_set_rate(ov5695->xvclk, OV5695_XVCLK_FREQ);
+	if (ret < 0) {
+		dev_err(dev, "Failed to set xvclk rate (24MHz)\n");
+		return ret;
+	}
+	if (clk_get_rate(ov5695->xvclk) != OV5695_XVCLK_FREQ)
+		dev_warn(dev, "xvclk mismatched, modes are based on 24MHz\n");
+
+	ov5695->reset_gpio = devm_gpiod_get(dev, "reset", GPIOD_OUT_HIGH);
+	if (IS_ERR(ov5695->reset_gpio)) {
+		dev_err(dev, "Failed to get reset-gpios\n");
+		return -EINVAL;
+	}
+
+	ret = ov5695_configure_regulators(ov5695);
+	if (ret) {
+		dev_err(dev, "Failed to get power regulators\n");
+		return ret;
+	}
+
+	mutex_init(&ov5695->mutex);
+
+	sd = &ov5695->subdev;
+	v4l2_i2c_subdev_init(sd, client, &ov5695_subdev_ops);
+	ret = ov5695_initialize_controls(ov5695);
+	if (ret)
+		goto err_destroy_mutex;
+
+	ret = __ov5695_power_on(ov5695);
+	if (ret)
+		goto err_free_handler;
+
+	ret = ov5695_check_sensor_id(ov5695, client);
+	if (ret)
+		goto err_power_off;
+
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
+	sd->internal_ops = &ov5695_internal_ops;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+#endif
+#if defined(CONFIG_MEDIA_CONTROLLER)
+	ov5695->pad.flags = MEDIA_PAD_FL_SOURCE;
+	sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret = media_entity_pads_init(&sd->entity, 1, &ov5695->pad);
+	if (ret < 0)
+		goto err_power_off;
+#endif
+
+	ret = v4l2_async_register_subdev_sensor(sd);
+	if (ret) {
+		dev_err(dev, "v4l2 async register subdev failed\n");
+		goto err_clean_entity;
+	}
+
+	pm_runtime_set_active(dev);
+	pm_runtime_enable(dev);
+	pm_runtime_idle(dev);
+
+	return 0;
+
+err_clean_entity:
+#if defined(CONFIG_MEDIA_CONTROLLER)
+	media_entity_cleanup(&sd->entity);
+#endif
+err_power_off:
+	__ov5695_power_off(ov5695);
+err_free_handler:
+	v4l2_ctrl_handler_free(&ov5695->ctrl_handler);
+err_destroy_mutex:
+	mutex_destroy(&ov5695->mutex);
+
+	return ret;
+}
+
+static void ov5695_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov5695 *ov5695 = to_ov5695(sd);
+
+	v4l2_async_unregister_subdev(sd);
+#if defined(CONFIG_MEDIA_CONTROLLER)
+	media_entity_cleanup(&sd->entity);
+#endif
+	v4l2_ctrl_handler_free(&ov5695->ctrl_handler);
+	mutex_destroy(&ov5695->mutex);
+
+	pm_runtime_disable(&client->dev);
+	if (!pm_runtime_status_suspended(&client->dev))
+		__ov5695_power_off(ov5695);
+	pm_runtime_set_suspended(&client->dev);
+}
+
+#if IS_ENABLED(CONFIG_OF)
+static const struct of_device_id ov5695_of_match[] = {
+	{ .compatible = "ovti,ov5695" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, ov5695_of_match);
+#endif
+
+static struct i2c_driver ov5695_i2c_driver = {
+	.driver = {
+		.name = "ov5695",
+		.pm = &ov5695_pm_ops,
+		.of_match_table = of_match_ptr(ov5695_of_match),
+	},
+	.probe		= ov5695_probe,
+	.remove		= ov5695_remove,
+};
+
+module_i2c_driver(ov5695_i2c_driver);
+
+MODULE_DESCRIPTION("OmniVision ov5695 sensor driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/ov6650.c b/drivers/media/i2c/ov6650.c
new file mode 100644
index 000000000..1ad07935f
--- /dev/null
+++ b/drivers/media/i2c/ov6650.c
@@ -0,0 +1,1125 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * V4L2 subdevice driver for OmniVision OV6650 Camera Sensor
+ *
+ * Copyright (C) 2010 Janusz Krzysztofik <jkrzyszt@tis.icnet.pl>
+ *
+ * Based on OmniVision OV96xx Camera Driver
+ * Copyright (C) 2009 Marek Vasut <marek.vasut@gmail.com>
+ *
+ * Based on ov772x camera driver:
+ * Copyright (C) 2008 Renesas Solutions Corp.
+ * Kuninori Morimoto <morimoto.kuninori@renesas.com>
+ *
+ * Based on ov7670 and soc_camera_platform driver,
+ * Copyright 2006-7 Jonathan Corbet <corbet@lwn.net>
+ * Copyright (C) 2008 Magnus Damm
+ * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
+ *
+ * Hardware specific bits initially based on former work by Matt Callow
+ * drivers/media/video/omap/sensor_ov6650.c
+ * Copyright (C) 2006 Matt Callow
+ */
+
+#include <linux/bitops.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/v4l2-mediabus.h>
+#include <linux/module.h>
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+
+/* Register definitions */
+#define REG_GAIN		0x00	/* range 00 - 3F */
+#define REG_BLUE		0x01
+#define REG_RED			0x02
+#define REG_SAT			0x03	/* [7:4] saturation [0:3] reserved */
+#define REG_HUE			0x04	/* [7:6] rsrvd [5] hue en [4:0] hue */
+
+#define REG_BRT			0x06
+
+#define REG_PIDH		0x0a
+#define REG_PIDL		0x0b
+
+#define REG_AECH		0x10
+#define REG_CLKRC		0x11	/* Data Format and Internal Clock */
+					/* [7:6] Input system clock (MHz)*/
+					/*   00=8, 01=12, 10=16, 11=24 */
+					/* [5:0]: Internal Clock Pre-Scaler */
+#define REG_COMA		0x12	/* [7] Reset */
+#define REG_COMB		0x13
+#define REG_COMC		0x14
+#define REG_COMD		0x15
+#define REG_COML		0x16
+#define REG_HSTRT		0x17
+#define REG_HSTOP		0x18
+#define REG_VSTRT		0x19
+#define REG_VSTOP		0x1a
+#define REG_PSHFT		0x1b
+#define REG_MIDH		0x1c
+#define REG_MIDL		0x1d
+#define REG_HSYNS		0x1e
+#define REG_HSYNE		0x1f
+#define REG_COME		0x20
+#define REG_YOFF		0x21
+#define REG_UOFF		0x22
+#define REG_VOFF		0x23
+#define REG_AEW			0x24
+#define REG_AEB			0x25
+#define REG_COMF		0x26
+#define REG_COMG		0x27
+#define REG_COMH		0x28
+#define REG_COMI		0x29
+
+#define REG_FRARL		0x2b
+#define REG_COMJ		0x2c
+#define REG_COMK		0x2d
+#define REG_AVGY		0x2e
+#define REG_REF0		0x2f
+#define REG_REF1		0x30
+#define REG_REF2		0x31
+#define REG_FRAJH		0x32
+#define REG_FRAJL		0x33
+#define REG_FACT		0x34
+#define REG_L1AEC		0x35
+#define REG_AVGU		0x36
+#define REG_AVGV		0x37
+
+#define REG_SPCB		0x60
+#define REG_SPCC		0x61
+#define REG_GAM1		0x62
+#define REG_GAM2		0x63
+#define REG_GAM3		0x64
+#define REG_SPCD		0x65
+
+#define REG_SPCE		0x68
+#define REG_ADCL		0x69
+
+#define REG_RMCO		0x6c
+#define REG_GMCO		0x6d
+#define REG_BMCO		0x6e
+
+
+/* Register bits, values, etc. */
+#define OV6650_PIDH		0x66	/* high byte of product ID number */
+#define OV6650_PIDL		0x50	/* low byte of product ID number */
+#define OV6650_MIDH		0x7F	/* high byte of mfg ID */
+#define OV6650_MIDL		0xA2	/* low byte of mfg ID */
+
+#define DEF_GAIN		0x00
+#define DEF_BLUE		0x80
+#define DEF_RED			0x80
+
+#define SAT_SHIFT		4
+#define SAT_MASK		(0xf << SAT_SHIFT)
+#define SET_SAT(x)		(((x) << SAT_SHIFT) & SAT_MASK)
+
+#define HUE_EN			BIT(5)
+#define HUE_MASK		0x1f
+#define DEF_HUE			0x10
+#define SET_HUE(x)		(HUE_EN | ((x) & HUE_MASK))
+
+#define DEF_AECH		0x4D
+
+#define CLKRC_8MHz		0x00
+#define CLKRC_12MHz		0x40
+#define CLKRC_16MHz		0x80
+#define CLKRC_24MHz		0xc0
+#define CLKRC_DIV_MASK		0x3f
+#define GET_CLKRC_DIV(x)	(((x) & CLKRC_DIV_MASK) + 1)
+#define DEF_CLKRC		0x00
+
+#define COMA_RESET		BIT(7)
+#define COMA_QCIF		BIT(5)
+#define COMA_RAW_RGB		BIT(4)
+#define COMA_RGB		BIT(3)
+#define COMA_BW			BIT(2)
+#define COMA_WORD_SWAP		BIT(1)
+#define COMA_BYTE_SWAP		BIT(0)
+#define DEF_COMA		0x00
+
+#define COMB_FLIP_V		BIT(7)
+#define COMB_FLIP_H		BIT(5)
+#define COMB_BAND_FILTER	BIT(4)
+#define COMB_AWB		BIT(2)
+#define COMB_AGC		BIT(1)
+#define COMB_AEC		BIT(0)
+#define DEF_COMB		0x5f
+
+#define COML_ONE_CHANNEL	BIT(7)
+
+#define DEF_HSTRT		0x24
+#define DEF_HSTOP		0xd4
+#define DEF_VSTRT		0x04
+#define DEF_VSTOP		0x94
+
+#define COMF_HREF_LOW		BIT(4)
+
+#define COMJ_PCLK_RISING	BIT(4)
+#define COMJ_VSYNC_HIGH		BIT(0)
+
+/* supported resolutions */
+#define W_QCIF			(DEF_HSTOP - DEF_HSTRT)
+#define W_CIF			(W_QCIF << 1)
+#define H_QCIF			(DEF_VSTOP - DEF_VSTRT)
+#define H_CIF			(H_QCIF << 1)
+
+#define FRAME_RATE_MAX		30
+
+
+struct ov6650_reg {
+	u8	reg;
+	u8	val;
+};
+
+struct ov6650 {
+	struct v4l2_subdev	subdev;
+	struct v4l2_ctrl_handler hdl;
+	struct {
+		/* exposure/autoexposure cluster */
+		struct v4l2_ctrl *autoexposure;
+		struct v4l2_ctrl *exposure;
+	};
+	struct {
+		/* gain/autogain cluster */
+		struct v4l2_ctrl *autogain;
+		struct v4l2_ctrl *gain;
+	};
+	struct {
+		/* blue/red/autowhitebalance cluster */
+		struct v4l2_ctrl *autowb;
+		struct v4l2_ctrl *blue;
+		struct v4l2_ctrl *red;
+	};
+	struct clk		*clk;
+	bool			half_scale;	/* scale down output by 2 */
+	struct v4l2_rect	rect;		/* sensor cropping window */
+	struct v4l2_fract	tpf;		/* as requested with s_frame_interval */
+	u32 code;
+};
+
+struct ov6650_xclk {
+	unsigned long	rate;
+	u8		clkrc;
+};
+
+static const struct ov6650_xclk ov6650_xclk[] = {
+{
+	.rate	= 8000000,
+	.clkrc	= CLKRC_8MHz,
+},
+{
+	.rate	= 12000000,
+	.clkrc	= CLKRC_12MHz,
+},
+{
+	.rate	= 16000000,
+	.clkrc	= CLKRC_16MHz,
+},
+{
+	.rate	= 24000000,
+	.clkrc	= CLKRC_24MHz,
+},
+};
+
+static u32 ov6650_codes[] = {
+	MEDIA_BUS_FMT_YUYV8_2X8,
+	MEDIA_BUS_FMT_UYVY8_2X8,
+	MEDIA_BUS_FMT_YVYU8_2X8,
+	MEDIA_BUS_FMT_VYUY8_2X8,
+	MEDIA_BUS_FMT_SBGGR8_1X8,
+	MEDIA_BUS_FMT_Y8_1X8,
+};
+
+static const struct v4l2_mbus_framefmt ov6650_def_fmt = {
+	.width		= W_CIF,
+	.height		= H_CIF,
+	.code		= MEDIA_BUS_FMT_SBGGR8_1X8,
+	.colorspace	= V4L2_COLORSPACE_SRGB,
+	.field		= V4L2_FIELD_NONE,
+	.ycbcr_enc	= V4L2_YCBCR_ENC_DEFAULT,
+	.quantization	= V4L2_QUANTIZATION_DEFAULT,
+	.xfer_func	= V4L2_XFER_FUNC_DEFAULT,
+};
+
+/* read a register */
+static int ov6650_reg_read(struct i2c_client *client, u8 reg, u8 *val)
+{
+	int ret;
+	u8 data = reg;
+	struct i2c_msg msg = {
+		.addr	= client->addr,
+		.flags	= 0,
+		.len	= 1,
+		.buf	= &data,
+	};
+
+	ret = i2c_transfer(client->adapter, &msg, 1);
+	if (ret < 0)
+		goto err;
+
+	msg.flags = I2C_M_RD;
+	ret = i2c_transfer(client->adapter, &msg, 1);
+	if (ret < 0)
+		goto err;
+
+	*val = data;
+	return 0;
+
+err:
+	dev_err(&client->dev, "Failed reading register 0x%02x!\n", reg);
+	return ret;
+}
+
+/* write a register */
+static int ov6650_reg_write(struct i2c_client *client, u8 reg, u8 val)
+{
+	int ret;
+	unsigned char data[2] = { reg, val };
+	struct i2c_msg msg = {
+		.addr	= client->addr,
+		.flags	= 0,
+		.len	= 2,
+		.buf	= data,
+	};
+
+	ret = i2c_transfer(client->adapter, &msg, 1);
+	udelay(100);
+
+	if (ret < 0) {
+		dev_err(&client->dev, "Failed writing register 0x%02x!\n", reg);
+		return ret;
+	}
+	return 0;
+}
+
+
+/* Read a register, alter its bits, write it back */
+static int ov6650_reg_rmw(struct i2c_client *client, u8 reg, u8 set, u8 mask)
+{
+	u8 val;
+	int ret;
+
+	ret = ov6650_reg_read(client, reg, &val);
+	if (ret) {
+		dev_err(&client->dev,
+			"[Read]-Modify-Write of register 0x%02x failed!\n",
+			reg);
+		return ret;
+	}
+
+	val &= ~mask;
+	val |= set;
+
+	ret = ov6650_reg_write(client, reg, val);
+	if (ret)
+		dev_err(&client->dev,
+			"Read-Modify-[Write] of register 0x%02x failed!\n",
+			reg);
+
+	return ret;
+}
+
+static struct ov6650 *to_ov6650(const struct i2c_client *client)
+{
+	return container_of(i2c_get_clientdata(client), struct ov6650, subdev);
+}
+
+/* Start/Stop streaming from the device */
+static int ov6650_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	return 0;
+}
+
+/* Get status of additional camera capabilities */
+static int ov6550_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov6650 *priv = container_of(ctrl->handler, struct ov6650, hdl);
+	struct v4l2_subdev *sd = &priv->subdev;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	uint8_t reg, reg2;
+	int ret;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTOGAIN:
+		ret = ov6650_reg_read(client, REG_GAIN, &reg);
+		if (!ret)
+			priv->gain->val = reg;
+		return ret;
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		ret = ov6650_reg_read(client, REG_BLUE, &reg);
+		if (!ret)
+			ret = ov6650_reg_read(client, REG_RED, &reg2);
+		if (!ret) {
+			priv->blue->val = reg;
+			priv->red->val = reg2;
+		}
+		return ret;
+	case V4L2_CID_EXPOSURE_AUTO:
+		ret = ov6650_reg_read(client, REG_AECH, &reg);
+		if (!ret)
+			priv->exposure->val = reg;
+		return ret;
+	}
+	return -EINVAL;
+}
+
+/* Set status of additional camera capabilities */
+static int ov6550_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov6650 *priv = container_of(ctrl->handler, struct ov6650, hdl);
+	struct v4l2_subdev *sd = &priv->subdev;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTOGAIN:
+		ret = ov6650_reg_rmw(client, REG_COMB,
+				ctrl->val ? COMB_AGC : 0, COMB_AGC);
+		if (!ret && !ctrl->val)
+			ret = ov6650_reg_write(client, REG_GAIN, priv->gain->val);
+		return ret;
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		ret = ov6650_reg_rmw(client, REG_COMB,
+				ctrl->val ? COMB_AWB : 0, COMB_AWB);
+		if (!ret && !ctrl->val) {
+			ret = ov6650_reg_write(client, REG_BLUE, priv->blue->val);
+			if (!ret)
+				ret = ov6650_reg_write(client, REG_RED,
+							priv->red->val);
+		}
+		return ret;
+	case V4L2_CID_SATURATION:
+		return ov6650_reg_rmw(client, REG_SAT, SET_SAT(ctrl->val),
+				SAT_MASK);
+	case V4L2_CID_HUE:
+		return ov6650_reg_rmw(client, REG_HUE, SET_HUE(ctrl->val),
+				HUE_MASK);
+	case V4L2_CID_BRIGHTNESS:
+		return ov6650_reg_write(client, REG_BRT, ctrl->val);
+	case V4L2_CID_EXPOSURE_AUTO:
+		ret = ov6650_reg_rmw(client, REG_COMB, ctrl->val ==
+				V4L2_EXPOSURE_AUTO ? COMB_AEC : 0, COMB_AEC);
+		if (!ret && ctrl->val == V4L2_EXPOSURE_MANUAL)
+			ret = ov6650_reg_write(client, REG_AECH,
+						priv->exposure->val);
+		return ret;
+	case V4L2_CID_GAMMA:
+		return ov6650_reg_write(client, REG_GAM1, ctrl->val);
+	case V4L2_CID_VFLIP:
+		return ov6650_reg_rmw(client, REG_COMB,
+				ctrl->val ? COMB_FLIP_V : 0, COMB_FLIP_V);
+	case V4L2_CID_HFLIP:
+		return ov6650_reg_rmw(client, REG_COMB,
+				ctrl->val ? COMB_FLIP_H : 0, COMB_FLIP_H);
+	}
+
+	return -EINVAL;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int ov6650_get_register(struct v4l2_subdev *sd,
+				struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+	u8 val;
+
+	if (reg->reg & ~0xff)
+		return -EINVAL;
+
+	reg->size = 1;
+
+	ret = ov6650_reg_read(client, reg->reg, &val);
+	if (!ret)
+		reg->val = (__u64)val;
+
+	return ret;
+}
+
+static int ov6650_set_register(struct v4l2_subdev *sd,
+				const struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (reg->reg & ~0xff || reg->val & ~0xff)
+		return -EINVAL;
+
+	return ov6650_reg_write(client, reg->reg, reg->val);
+}
+#endif
+
+static int ov6650_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov6650 *priv = to_ov6650(client);
+	int ret = 0;
+
+	if (on)
+		ret = clk_prepare_enable(priv->clk);
+	else
+		clk_disable_unprepare(priv->clk);
+
+	return ret;
+}
+
+static int ov6650_get_selection(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_selection *sel)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov6650 *priv = to_ov6650(client);
+	struct v4l2_rect *rect;
+
+	if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
+		/* pre-select try crop rectangle */
+		rect = &sd_state->pads->try_crop;
+
+	} else {
+		/* pre-select active crop rectangle */
+		rect = &priv->rect;
+	}
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.left = DEF_HSTRT << 1;
+		sel->r.top = DEF_VSTRT << 1;
+		sel->r.width = W_CIF;
+		sel->r.height = H_CIF;
+		return 0;
+
+	case V4L2_SEL_TGT_CROP:
+		/* use selected crop rectangle */
+		sel->r = *rect;
+		return 0;
+
+	default:
+		return -EINVAL;
+	}
+}
+
+static bool is_unscaled_ok(int width, int height, struct v4l2_rect *rect)
+{
+	return width > rect->width >> 1 || height > rect->height >> 1;
+}
+
+static void ov6650_bind_align_crop_rectangle(struct v4l2_rect *rect)
+{
+	v4l_bound_align_image(&rect->width, 2, W_CIF, 1,
+			      &rect->height, 2, H_CIF, 1, 0);
+	v4l_bound_align_image(&rect->left, DEF_HSTRT << 1,
+			      (DEF_HSTRT << 1) + W_CIF - (__s32)rect->width, 1,
+			      &rect->top, DEF_VSTRT << 1,
+			      (DEF_VSTRT << 1) + H_CIF - (__s32)rect->height,
+			      1, 0);
+}
+
+static int ov6650_set_selection(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_selection *sel)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov6650 *priv = to_ov6650(client);
+	int ret;
+
+	if (sel->target != V4L2_SEL_TGT_CROP)
+		return -EINVAL;
+
+	ov6650_bind_align_crop_rectangle(&sel->r);
+
+	if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
+		struct v4l2_rect *crop = &sd_state->pads->try_crop;
+		struct v4l2_mbus_framefmt *mf = &sd_state->pads->try_fmt;
+		/* detect current pad config scaling factor */
+		bool half_scale = !is_unscaled_ok(mf->width, mf->height, crop);
+
+		/* store new crop rectangle */
+		*crop = sel->r;
+
+		/* adjust frame size */
+		mf->width = crop->width >> half_scale;
+		mf->height = crop->height >> half_scale;
+
+		return 0;
+	}
+
+	/* V4L2_SUBDEV_FORMAT_ACTIVE */
+
+	/* apply new crop rectangle */
+	ret = ov6650_reg_write(client, REG_HSTRT, sel->r.left >> 1);
+	if (!ret) {
+		priv->rect.width += priv->rect.left - sel->r.left;
+		priv->rect.left = sel->r.left;
+		ret = ov6650_reg_write(client, REG_HSTOP,
+				       (sel->r.left + sel->r.width) >> 1);
+	}
+	if (!ret) {
+		priv->rect.width = sel->r.width;
+		ret = ov6650_reg_write(client, REG_VSTRT, sel->r.top >> 1);
+	}
+	if (!ret) {
+		priv->rect.height += priv->rect.top - sel->r.top;
+		priv->rect.top = sel->r.top;
+		ret = ov6650_reg_write(client, REG_VSTOP,
+				       (sel->r.top + sel->r.height) >> 1);
+	}
+	if (!ret)
+		priv->rect.height = sel->r.height;
+
+	return ret;
+}
+
+static int ov6650_get_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *mf = &format->format;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov6650 *priv = to_ov6650(client);
+
+	if (format->pad)
+		return -EINVAL;
+
+	/* initialize response with default media bus frame format */
+	*mf = ov6650_def_fmt;
+
+	/* update media bus format code and frame size */
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		mf->width = sd_state->pads->try_fmt.width;
+		mf->height = sd_state->pads->try_fmt.height;
+		mf->code = sd_state->pads->try_fmt.code;
+
+	} else {
+		mf->width = priv->rect.width >> priv->half_scale;
+		mf->height = priv->rect.height >> priv->half_scale;
+		mf->code = priv->code;
+	}
+	return 0;
+}
+
+#define to_clkrc(div)	((div) - 1)
+
+/* set the format we will capture in */
+static int ov6650_s_fmt(struct v4l2_subdev *sd, u32 code, bool half_scale)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov6650 *priv = to_ov6650(client);
+	u8 coma_set = 0, coma_mask = 0, coml_set, coml_mask;
+	int ret;
+
+	/* select color matrix configuration for given color encoding */
+	switch (code) {
+	case MEDIA_BUS_FMT_Y8_1X8:
+		dev_dbg(&client->dev, "pixel format GREY8_1X8\n");
+		coma_mask |= COMA_RGB | COMA_WORD_SWAP | COMA_BYTE_SWAP;
+		coma_set |= COMA_BW;
+		break;
+	case MEDIA_BUS_FMT_YUYV8_2X8:
+		dev_dbg(&client->dev, "pixel format YUYV8_2X8_LE\n");
+		coma_mask |= COMA_RGB | COMA_BW | COMA_BYTE_SWAP;
+		coma_set |= COMA_WORD_SWAP;
+		break;
+	case MEDIA_BUS_FMT_YVYU8_2X8:
+		dev_dbg(&client->dev, "pixel format YVYU8_2X8_LE (untested)\n");
+		coma_mask |= COMA_RGB | COMA_BW | COMA_WORD_SWAP |
+				COMA_BYTE_SWAP;
+		break;
+	case MEDIA_BUS_FMT_UYVY8_2X8:
+		dev_dbg(&client->dev, "pixel format YUYV8_2X8_BE\n");
+		if (half_scale) {
+			coma_mask |= COMA_RGB | COMA_BW | COMA_WORD_SWAP;
+			coma_set |= COMA_BYTE_SWAP;
+		} else {
+			coma_mask |= COMA_RGB | COMA_BW;
+			coma_set |= COMA_BYTE_SWAP | COMA_WORD_SWAP;
+		}
+		break;
+	case MEDIA_BUS_FMT_VYUY8_2X8:
+		dev_dbg(&client->dev, "pixel format YVYU8_2X8_BE (untested)\n");
+		if (half_scale) {
+			coma_mask |= COMA_RGB | COMA_BW;
+			coma_set |= COMA_BYTE_SWAP | COMA_WORD_SWAP;
+		} else {
+			coma_mask |= COMA_RGB | COMA_BW | COMA_WORD_SWAP;
+			coma_set |= COMA_BYTE_SWAP;
+		}
+		break;
+	case MEDIA_BUS_FMT_SBGGR8_1X8:
+		dev_dbg(&client->dev, "pixel format SBGGR8_1X8 (untested)\n");
+		coma_mask |= COMA_BW | COMA_BYTE_SWAP | COMA_WORD_SWAP;
+		coma_set |= COMA_RAW_RGB | COMA_RGB;
+		break;
+	default:
+		dev_err(&client->dev, "Pixel format not handled: 0x%x\n", code);
+		return -EINVAL;
+	}
+
+	if (code == MEDIA_BUS_FMT_Y8_1X8 ||
+			code == MEDIA_BUS_FMT_SBGGR8_1X8) {
+		coml_mask = COML_ONE_CHANNEL;
+		coml_set = 0;
+	} else {
+		coml_mask = 0;
+		coml_set = COML_ONE_CHANNEL;
+	}
+
+	if (half_scale) {
+		dev_dbg(&client->dev, "max resolution: QCIF\n");
+		coma_set |= COMA_QCIF;
+	} else {
+		dev_dbg(&client->dev, "max resolution: CIF\n");
+		coma_mask |= COMA_QCIF;
+	}
+
+	ret = ov6650_reg_rmw(client, REG_COMA, coma_set, coma_mask);
+	if (!ret) {
+		priv->half_scale = half_scale;
+
+		ret = ov6650_reg_rmw(client, REG_COML, coml_set, coml_mask);
+	}
+	if (!ret)
+		priv->code = code;
+
+	return ret;
+}
+
+static int ov6650_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *mf = &format->format;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov6650 *priv = to_ov6650(client);
+	struct v4l2_rect *crop;
+	bool half_scale;
+
+	if (format->pad)
+		return -EINVAL;
+
+	switch (mf->code) {
+	case MEDIA_BUS_FMT_Y10_1X10:
+		mf->code = MEDIA_BUS_FMT_Y8_1X8;
+		fallthrough;
+	case MEDIA_BUS_FMT_Y8_1X8:
+	case MEDIA_BUS_FMT_YVYU8_2X8:
+	case MEDIA_BUS_FMT_YUYV8_2X8:
+	case MEDIA_BUS_FMT_VYUY8_2X8:
+	case MEDIA_BUS_FMT_UYVY8_2X8:
+		break;
+	default:
+		mf->code = MEDIA_BUS_FMT_SBGGR8_1X8;
+		fallthrough;
+	case MEDIA_BUS_FMT_SBGGR8_1X8:
+		break;
+	}
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+		crop = &sd_state->pads->try_crop;
+	else
+		crop = &priv->rect;
+
+	half_scale = !is_unscaled_ok(mf->width, mf->height, crop);
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		/* store new mbus frame format code and size in pad config */
+		sd_state->pads->try_fmt.width = crop->width >> half_scale;
+		sd_state->pads->try_fmt.height = crop->height >> half_scale;
+		sd_state->pads->try_fmt.code = mf->code;
+
+		/* return default mbus frame format updated with pad config */
+		*mf = ov6650_def_fmt;
+		mf->width = sd_state->pads->try_fmt.width;
+		mf->height = sd_state->pads->try_fmt.height;
+		mf->code = sd_state->pads->try_fmt.code;
+
+	} else {
+		int ret = 0;
+
+		/* apply new media bus frame format and scaling if changed */
+		if (mf->code != priv->code || half_scale != priv->half_scale)
+			ret = ov6650_s_fmt(sd, mf->code, half_scale);
+		if (ret)
+			return ret;
+
+		/* return default format updated with active size and code */
+		*mf = ov6650_def_fmt;
+		mf->width = priv->rect.width >> priv->half_scale;
+		mf->height = priv->rect.height >> priv->half_scale;
+		mf->code = priv->code;
+	}
+	return 0;
+}
+
+static int ov6650_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->pad || code->index >= ARRAY_SIZE(ov6650_codes))
+		return -EINVAL;
+
+	code->code = ov6650_codes[code->index];
+	return 0;
+}
+
+static int ov6650_enum_frame_interval(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_state *sd_state,
+				    struct v4l2_subdev_frame_interval_enum *fie)
+{
+	int i;
+
+	/* enumerate supported frame intervals not exceeding 1 second */
+	if (fie->index > CLKRC_DIV_MASK ||
+	    GET_CLKRC_DIV(fie->index) > FRAME_RATE_MAX)
+		return -EINVAL;
+
+	for (i = 0; i < ARRAY_SIZE(ov6650_codes); i++)
+		if (fie->code == ov6650_codes[i])
+			break;
+	if (i == ARRAY_SIZE(ov6650_codes))
+		return -EINVAL;
+
+	if (!fie->width || fie->width > W_CIF ||
+	    !fie->height || fie->height > H_CIF)
+		return -EINVAL;
+
+	fie->interval.numerator = GET_CLKRC_DIV(fie->index);
+	fie->interval.denominator = FRAME_RATE_MAX;
+
+	return 0;
+}
+
+static int ov6650_g_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *ival)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov6650 *priv = to_ov6650(client);
+
+	ival->interval = priv->tpf;
+
+	dev_dbg(&client->dev, "Frame interval: %u/%u s\n",
+		ival->interval.numerator, ival->interval.denominator);
+
+	return 0;
+}
+
+static int ov6650_s_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *ival)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov6650 *priv = to_ov6650(client);
+	struct v4l2_fract *tpf = &ival->interval;
+	int div, ret;
+
+	if (tpf->numerator == 0 || tpf->denominator == 0)
+		div = 1;  /* Reset to full rate */
+	else
+		div = (tpf->numerator * FRAME_RATE_MAX) / tpf->denominator;
+
+	if (div == 0)
+		div = 1;
+	else if (div > GET_CLKRC_DIV(CLKRC_DIV_MASK))
+		div = GET_CLKRC_DIV(CLKRC_DIV_MASK);
+
+	ret = ov6650_reg_rmw(client, REG_CLKRC, to_clkrc(div), CLKRC_DIV_MASK);
+	if (!ret) {
+		priv->tpf.numerator = div;
+		priv->tpf.denominator = FRAME_RATE_MAX;
+
+		*tpf = priv->tpf;
+	}
+
+	return ret;
+}
+
+/* Soft reset the camera. This has nothing to do with the RESET pin! */
+static int ov6650_reset(struct i2c_client *client)
+{
+	int ret;
+
+	dev_dbg(&client->dev, "reset\n");
+
+	ret = ov6650_reg_rmw(client, REG_COMA, COMA_RESET, 0);
+	if (ret)
+		dev_err(&client->dev,
+			"An error occurred while entering soft reset!\n");
+
+	return ret;
+}
+
+/* program default register values */
+static int ov6650_prog_dflt(struct i2c_client *client, u8 clkrc)
+{
+	int ret;
+
+	dev_dbg(&client->dev, "initializing\n");
+
+	ret = ov6650_reg_write(client, REG_COMA, 0);	/* ~COMA_RESET */
+	if (!ret)
+		ret = ov6650_reg_write(client, REG_CLKRC, clkrc);
+	if (!ret)
+		ret = ov6650_reg_rmw(client, REG_COMB, 0, COMB_BAND_FILTER);
+
+	return ret;
+}
+
+static int ov6650_video_probe(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov6650 *priv = to_ov6650(client);
+	const struct ov6650_xclk *xclk = NULL;
+	unsigned long rate;
+	u8 pidh, pidl, midh, midl;
+	int i, ret = 0;
+
+	priv->clk = devm_clk_get(&client->dev, NULL);
+	if (IS_ERR(priv->clk)) {
+		ret = PTR_ERR(priv->clk);
+		dev_err(&client->dev, "clk request err: %d\n", ret);
+		return ret;
+	}
+
+	rate = clk_get_rate(priv->clk);
+	for (i = 0; rate && i < ARRAY_SIZE(ov6650_xclk); i++) {
+		if (rate != ov6650_xclk[i].rate)
+			continue;
+
+		xclk = &ov6650_xclk[i];
+		dev_info(&client->dev, "using host default clock rate %lukHz\n",
+			 rate / 1000);
+		break;
+	}
+	for (i = 0; !xclk && i < ARRAY_SIZE(ov6650_xclk); i++) {
+		ret = clk_set_rate(priv->clk, ov6650_xclk[i].rate);
+		if (ret || clk_get_rate(priv->clk) != ov6650_xclk[i].rate)
+			continue;
+
+		xclk = &ov6650_xclk[i];
+		dev_info(&client->dev, "using negotiated clock rate %lukHz\n",
+			 xclk->rate / 1000);
+		break;
+	}
+	if (!xclk) {
+		dev_err(&client->dev, "unable to get supported clock rate\n");
+		if (!ret)
+			ret = -EINVAL;
+		return ret;
+	}
+
+	ret = ov6650_s_power(sd, 1);
+	if (ret < 0)
+		return ret;
+
+	msleep(20);
+
+	/*
+	 * check and show product ID and manufacturer ID
+	 */
+	ret = ov6650_reg_read(client, REG_PIDH, &pidh);
+	if (!ret)
+		ret = ov6650_reg_read(client, REG_PIDL, &pidl);
+	if (!ret)
+		ret = ov6650_reg_read(client, REG_MIDH, &midh);
+	if (!ret)
+		ret = ov6650_reg_read(client, REG_MIDL, &midl);
+
+	if (ret)
+		goto done;
+
+	if ((pidh != OV6650_PIDH) || (pidl != OV6650_PIDL)) {
+		dev_err(&client->dev, "Product ID error 0x%02x:0x%02x\n",
+				pidh, pidl);
+		ret = -ENODEV;
+		goto done;
+	}
+
+	dev_info(&client->dev,
+		"ov6650 Product ID 0x%02x:0x%02x Manufacturer ID 0x%02x:0x%02x\n",
+		pidh, pidl, midh, midl);
+
+	ret = ov6650_reset(client);
+	if (!ret)
+		ret = ov6650_prog_dflt(client, xclk->clkrc);
+	if (!ret) {
+		/* driver default frame format, no scaling */
+		ret = ov6650_s_fmt(sd, ov6650_def_fmt.code, false);
+	}
+	if (!ret)
+		ret = v4l2_ctrl_handler_setup(&priv->hdl);
+
+done:
+	ov6650_s_power(sd, 0);
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ov6550_ctrl_ops = {
+	.g_volatile_ctrl = ov6550_g_volatile_ctrl,
+	.s_ctrl = ov6550_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops ov6650_core_ops = {
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register		= ov6650_get_register,
+	.s_register		= ov6650_set_register,
+#endif
+	.s_power		= ov6650_s_power,
+};
+
+/* Request bus settings on camera side */
+static int ov6650_get_mbus_config(struct v4l2_subdev *sd,
+				  unsigned int pad,
+				  struct v4l2_mbus_config *cfg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u8 comj, comf;
+	int ret;
+
+	ret = ov6650_reg_read(client, REG_COMJ, &comj);
+	if (ret)
+		return ret;
+
+	ret = ov6650_reg_read(client, REG_COMF, &comf);
+	if (ret)
+		return ret;
+
+	cfg->type = V4L2_MBUS_PARALLEL;
+
+	cfg->bus.parallel.flags = V4L2_MBUS_MASTER | V4L2_MBUS_DATA_ACTIVE_HIGH
+		| ((comj & COMJ_VSYNC_HIGH)  ? V4L2_MBUS_VSYNC_ACTIVE_HIGH
+					     : V4L2_MBUS_VSYNC_ACTIVE_LOW)
+		| ((comf & COMF_HREF_LOW)    ? V4L2_MBUS_HSYNC_ACTIVE_LOW
+					     : V4L2_MBUS_HSYNC_ACTIVE_HIGH)
+		| ((comj & COMJ_PCLK_RISING) ? V4L2_MBUS_PCLK_SAMPLE_RISING
+					     : V4L2_MBUS_PCLK_SAMPLE_FALLING);
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops ov6650_video_ops = {
+	.s_stream	= ov6650_s_stream,
+	.g_frame_interval = ov6650_g_frame_interval,
+	.s_frame_interval = ov6650_s_frame_interval,
+};
+
+static const struct v4l2_subdev_pad_ops ov6650_pad_ops = {
+	.enum_mbus_code		= ov6650_enum_mbus_code,
+	.enum_frame_interval	= ov6650_enum_frame_interval,
+	.get_selection		= ov6650_get_selection,
+	.set_selection		= ov6650_set_selection,
+	.get_fmt		= ov6650_get_fmt,
+	.set_fmt		= ov6650_set_fmt,
+	.get_mbus_config	= ov6650_get_mbus_config,
+};
+
+static const struct v4l2_subdev_ops ov6650_subdev_ops = {
+	.core	= &ov6650_core_ops,
+	.video	= &ov6650_video_ops,
+	.pad	= &ov6650_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops ov6650_internal_ops = {
+	.registered = ov6650_video_probe,
+};
+
+/*
+ * i2c_driver function
+ */
+static int ov6650_probe(struct i2c_client *client)
+{
+	struct ov6650 *priv;
+	int ret;
+
+	priv = devm_kzalloc(&client->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	v4l2_i2c_subdev_init(&priv->subdev, client, &ov6650_subdev_ops);
+	v4l2_ctrl_handler_init(&priv->hdl, 13);
+	v4l2_ctrl_new_std(&priv->hdl, &ov6550_ctrl_ops,
+			V4L2_CID_VFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&priv->hdl, &ov6550_ctrl_ops,
+			V4L2_CID_HFLIP, 0, 1, 1, 0);
+	priv->autogain = v4l2_ctrl_new_std(&priv->hdl, &ov6550_ctrl_ops,
+			V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+	priv->gain = v4l2_ctrl_new_std(&priv->hdl, &ov6550_ctrl_ops,
+			V4L2_CID_GAIN, 0, 0x3f, 1, DEF_GAIN);
+	priv->autowb = v4l2_ctrl_new_std(&priv->hdl, &ov6550_ctrl_ops,
+			V4L2_CID_AUTO_WHITE_BALANCE, 0, 1, 1, 1);
+	priv->blue = v4l2_ctrl_new_std(&priv->hdl, &ov6550_ctrl_ops,
+			V4L2_CID_BLUE_BALANCE, 0, 0xff, 1, DEF_BLUE);
+	priv->red = v4l2_ctrl_new_std(&priv->hdl, &ov6550_ctrl_ops,
+			V4L2_CID_RED_BALANCE, 0, 0xff, 1, DEF_RED);
+	v4l2_ctrl_new_std(&priv->hdl, &ov6550_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 0xf, 1, 0x8);
+	v4l2_ctrl_new_std(&priv->hdl, &ov6550_ctrl_ops,
+			V4L2_CID_HUE, 0, HUE_MASK, 1, DEF_HUE);
+	v4l2_ctrl_new_std(&priv->hdl, &ov6550_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 0xff, 1, 0x80);
+	priv->autoexposure = v4l2_ctrl_new_std_menu(&priv->hdl,
+			&ov6550_ctrl_ops, V4L2_CID_EXPOSURE_AUTO,
+			V4L2_EXPOSURE_MANUAL, 0, V4L2_EXPOSURE_AUTO);
+	priv->exposure = v4l2_ctrl_new_std(&priv->hdl, &ov6550_ctrl_ops,
+			V4L2_CID_EXPOSURE, 0, 0xff, 1, DEF_AECH);
+	v4l2_ctrl_new_std(&priv->hdl, &ov6550_ctrl_ops,
+			V4L2_CID_GAMMA, 0, 0xff, 1, 0x12);
+
+	priv->subdev.ctrl_handler = &priv->hdl;
+	if (priv->hdl.error) {
+		ret = priv->hdl.error;
+		goto ectlhdlfree;
+	}
+
+	v4l2_ctrl_auto_cluster(2, &priv->autogain, 0, true);
+	v4l2_ctrl_auto_cluster(3, &priv->autowb, 0, true);
+	v4l2_ctrl_auto_cluster(2, &priv->autoexposure,
+				V4L2_EXPOSURE_MANUAL, true);
+
+	priv->rect.left	  = DEF_HSTRT << 1;
+	priv->rect.top	  = DEF_VSTRT << 1;
+	priv->rect.width  = W_CIF;
+	priv->rect.height = H_CIF;
+
+	/* Hardware default frame interval */
+	priv->tpf.numerator   = GET_CLKRC_DIV(DEF_CLKRC);
+	priv->tpf.denominator = FRAME_RATE_MAX;
+
+	priv->subdev.internal_ops = &ov6650_internal_ops;
+
+	ret = v4l2_async_register_subdev(&priv->subdev);
+	if (!ret)
+		return 0;
+ectlhdlfree:
+	v4l2_ctrl_handler_free(&priv->hdl);
+
+	return ret;
+}
+
+static void ov6650_remove(struct i2c_client *client)
+{
+	struct ov6650 *priv = to_ov6650(client);
+
+	v4l2_async_unregister_subdev(&priv->subdev);
+	v4l2_ctrl_handler_free(&priv->hdl);
+}
+
+static const struct i2c_device_id ov6650_id[] = {
+	{ "ov6650", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ov6650_id);
+
+static struct i2c_driver ov6650_i2c_driver = {
+	.driver = {
+		.name = "ov6650",
+	},
+	.probe    = ov6650_probe,
+	.remove   = ov6650_remove,
+	.id_table = ov6650_id,
+};
+
+module_i2c_driver(ov6650_i2c_driver);
+
+MODULE_DESCRIPTION("V4L2 subdevice driver for OmniVision OV6650 camera sensor");
+MODULE_AUTHOR("Janusz Krzysztofik <jmkrzyszt@gmail.com");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/ov7251.c b/drivers/media/i2c/ov7251.c
new file mode 100644
index 000000000..675fb37a6
--- /dev/null
+++ b/drivers/media/i2c/ov7251.c
@@ -0,0 +1,1817 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Driver for the OV7251 camera sensor.
+ *
+ * Copyright (c) 2017-2018, The Linux Foundation. All rights reserved.
+ * Copyright (c) 2017-2018, Linaro Ltd.
+ */
+
+#include <linux/bitops.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/mod_devicetable.h>
+#include <linux/pm_runtime.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+#define OV7251_SC_MODE_SELECT		0x0100
+#define OV7251_SC_MODE_SELECT_SW_STANDBY	0x0
+#define OV7251_SC_MODE_SELECT_STREAMING		0x1
+
+#define OV7251_CHIP_ID_HIGH		0x300a
+#define OV7251_CHIP_ID_HIGH_BYTE	0x77
+#define OV7251_CHIP_ID_LOW		0x300b
+#define OV7251_CHIP_ID_LOW_BYTE		0x50
+#define OV7251_SC_GP_IO_IN1		0x3029
+#define OV7251_AEC_EXPO_0		0x3500
+#define OV7251_AEC_EXPO_1		0x3501
+#define OV7251_AEC_EXPO_2		0x3502
+#define OV7251_AEC_AGC_ADJ_0		0x350a
+#define OV7251_AEC_AGC_ADJ_1		0x350b
+#define OV7251_TIMING_FORMAT1		0x3820
+#define OV7251_TIMING_FORMAT1_VFLIP	BIT(2)
+#define OV7251_TIMING_FORMAT2		0x3821
+#define OV7251_TIMING_FORMAT2_MIRROR	BIT(2)
+#define OV7251_PRE_ISP_00		0x5e00
+#define OV7251_PRE_ISP_00_TEST_PATTERN	BIT(7)
+#define OV7251_PLL1_PRE_DIV_REG		0x30b4
+#define OV7251_PLL1_MULT_REG		0x30b3
+#define OV7251_PLL1_DIVIDER_REG		0x30b1
+#define OV7251_PLL1_PIX_DIV_REG		0x30b0
+#define OV7251_PLL1_MIPI_DIV_REG	0x30b5
+#define OV7251_PLL2_PRE_DIV_REG		0x3098
+#define OV7251_PLL2_MULT_REG		0x3099
+#define OV7251_PLL2_DIVIDER_REG		0x309d
+#define OV7251_PLL2_SYS_DIV_REG		0x309a
+#define OV7251_PLL2_ADC_DIV_REG		0x309b
+
+#define OV7251_NATIVE_WIDTH		656
+#define OV7251_NATIVE_HEIGHT		496
+#define OV7251_ACTIVE_START_LEFT	4
+#define OV7251_ACTIVE_START_TOP		4
+#define OV7251_ACTIVE_WIDTH		648
+#define OV7251_ACTIVE_HEIGHT		488
+
+#define OV7251_FIXED_PPL		928
+#define OV7251_TIMING_VTS_REG		0x380e
+#define OV7251_TIMING_MIN_VTS		1
+#define OV7251_TIMING_MAX_VTS		0xffff
+#define OV7251_INTEGRATION_MARGIN	20
+
+struct reg_value {
+	u16 reg;
+	u8 val;
+};
+
+struct ov7251_mode_info {
+	u32 width;
+	u32 height;
+	u32 vts;
+	const struct reg_value *data;
+	u32 data_size;
+	u32 pixel_clock;
+	u32 link_freq;
+	u16 exposure_max;
+	u16 exposure_def;
+	struct v4l2_fract timeperframe;
+};
+
+struct ov7251_pll1_cfg {
+	unsigned int pre_div;
+	unsigned int mult;
+	unsigned int div;
+	unsigned int pix_div;
+	unsigned int mipi_div;
+};
+
+struct ov7251_pll2_cfg {
+	unsigned int pre_div;
+	unsigned int mult;
+	unsigned int div;
+	unsigned int sys_div;
+	unsigned int adc_div;
+};
+
+/*
+ * Rubbish ordering, but only PLL1 needs to have a separate configuration per
+ * link frequency and the array member needs to be last.
+ */
+struct ov7251_pll_cfgs {
+	const struct ov7251_pll2_cfg *pll2;
+	const struct ov7251_pll1_cfg *pll1[];
+};
+
+enum xclk_rate {
+	OV7251_19_2_MHZ,
+	OV7251_24_MHZ,
+	OV7251_NUM_SUPPORTED_RATES
+};
+
+enum supported_link_freqs {
+	OV7251_LINK_FREQ_240_MHZ,
+	OV7251_LINK_FREQ_319_2_MHZ,
+	OV7251_NUM_SUPPORTED_LINK_FREQS
+};
+
+struct ov7251 {
+	struct i2c_client *i2c_client;
+	struct device *dev;
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct v4l2_fwnode_endpoint ep;
+	struct v4l2_mbus_framefmt fmt;
+	struct v4l2_rect crop;
+	struct clk *xclk;
+	u32 xclk_freq;
+
+	struct regulator *io_regulator;
+	struct regulator *core_regulator;
+	struct regulator *analog_regulator;
+
+	const struct ov7251_pll_cfgs *pll_cfgs;
+	enum supported_link_freqs link_freq_idx;
+	const struct ov7251_mode_info *current_mode;
+
+	struct v4l2_ctrl_handler ctrls;
+	struct v4l2_ctrl *pixel_clock;
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *exposure;
+	struct v4l2_ctrl *gain;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *vblank;
+
+	/* Cached register values */
+	u8 aec_pk_manual;
+	u8 pre_isp_00;
+	u8 timing_format1;
+	u8 timing_format2;
+
+	struct mutex lock; /* lock to protect power state, ctrls and mode */
+	bool power_on;
+
+	struct gpio_desc *enable_gpio;
+};
+
+static inline struct ov7251 *to_ov7251(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct ov7251, sd);
+}
+
+static const struct ov7251_pll1_cfg ov7251_pll1_cfg_19_2_mhz_240_mhz = {
+	.pre_div = 0x03,
+	.mult = 0x4b,
+	.div = 0x01,
+	.pix_div = 0x0a,
+	.mipi_div = 0x05,
+};
+
+static const struct ov7251_pll1_cfg ov7251_pll1_cfg_19_2_mhz_319_2_mhz = {
+	.pre_div = 0x01,
+	.mult = 0x85,
+	.div = 0x04,
+	.pix_div = 0x0a,
+	.mipi_div = 0x05,
+};
+
+static const struct ov7251_pll1_cfg ov7251_pll1_cfg_24_mhz_240_mhz = {
+	.pre_div = 0x03,
+	.mult = 0x64,
+	.div = 0x01,
+	.pix_div = 0x0a,
+	.mipi_div = 0x05,
+};
+
+static const struct ov7251_pll1_cfg ov7251_pll1_cfg_24_mhz_319_2_mhz = {
+	.pre_div = 0x05,
+	.mult = 0x85,
+	.div = 0x02,
+	.pix_div = 0x0a,
+	.mipi_div = 0x05,
+};
+
+static const struct ov7251_pll2_cfg ov7251_pll2_cfg_19_2_mhz = {
+	.pre_div = 0x04,
+	.mult = 0x32,
+	.div = 0x00,
+	.sys_div = 0x05,
+	.adc_div = 0x04,
+};
+
+static const struct ov7251_pll2_cfg ov7251_pll2_cfg_24_mhz = {
+	.pre_div = 0x04,
+	.mult = 0x28,
+	.div = 0x00,
+	.sys_div = 0x05,
+	.adc_div = 0x04,
+};
+
+static const struct ov7251_pll_cfgs ov7251_pll_cfgs_19_2_mhz = {
+	.pll2 = &ov7251_pll2_cfg_19_2_mhz,
+	.pll1 = {
+		[OV7251_LINK_FREQ_240_MHZ] = &ov7251_pll1_cfg_19_2_mhz_240_mhz,
+		[OV7251_LINK_FREQ_319_2_MHZ] = &ov7251_pll1_cfg_19_2_mhz_319_2_mhz,
+	},
+};
+
+static const struct ov7251_pll_cfgs ov7251_pll_cfgs_24_mhz = {
+	.pll2 = &ov7251_pll2_cfg_24_mhz,
+	.pll1 = {
+		[OV7251_LINK_FREQ_240_MHZ] = &ov7251_pll1_cfg_24_mhz_240_mhz,
+		[OV7251_LINK_FREQ_319_2_MHZ] = &ov7251_pll1_cfg_24_mhz_319_2_mhz,
+	},
+};
+
+static const struct ov7251_pll_cfgs *ov7251_pll_cfgs[] = {
+	[OV7251_19_2_MHZ] = &ov7251_pll_cfgs_19_2_mhz,
+	[OV7251_24_MHZ] = &ov7251_pll_cfgs_24_mhz,
+};
+
+static const struct reg_value ov7251_global_init_setting[] = {
+	{ 0x0103, 0x01 },
+	{ 0x303b, 0x02 },
+};
+
+static const struct reg_value ov7251_setting_vga_30fps[] = {
+	{ 0x3005, 0x00 },
+	{ 0x3012, 0xc0 },
+	{ 0x3013, 0xd2 },
+	{ 0x3014, 0x04 },
+	{ 0x3016, 0xf0 },
+	{ 0x3017, 0xf0 },
+	{ 0x3018, 0xf0 },
+	{ 0x301a, 0xf0 },
+	{ 0x301b, 0xf0 },
+	{ 0x301c, 0xf0 },
+	{ 0x3023, 0x05 },
+	{ 0x3037, 0xf0 },
+	{ 0x3106, 0xda },
+	{ 0x3503, 0x07 },
+	{ 0x3509, 0x10 },
+	{ 0x3600, 0x1c },
+	{ 0x3602, 0x62 },
+	{ 0x3620, 0xb7 },
+	{ 0x3622, 0x04 },
+	{ 0x3626, 0x21 },
+	{ 0x3627, 0x30 },
+	{ 0x3630, 0x44 },
+	{ 0x3631, 0x35 },
+	{ 0x3634, 0x60 },
+	{ 0x3636, 0x00 },
+	{ 0x3662, 0x01 },
+	{ 0x3663, 0x70 },
+	{ 0x3664, 0x50 },
+	{ 0x3666, 0x0a },
+	{ 0x3669, 0x1a },
+	{ 0x366a, 0x00 },
+	{ 0x366b, 0x50 },
+	{ 0x3673, 0x01 },
+	{ 0x3674, 0xff },
+	{ 0x3675, 0x03 },
+	{ 0x3705, 0xc1 },
+	{ 0x3709, 0x40 },
+	{ 0x373c, 0x08 },
+	{ 0x3742, 0x00 },
+	{ 0x3757, 0xb3 },
+	{ 0x3788, 0x00 },
+	{ 0x37a8, 0x01 },
+	{ 0x37a9, 0xc0 },
+	{ 0x3800, 0x00 },
+	{ 0x3801, 0x04 },
+	{ 0x3802, 0x00 },
+	{ 0x3803, 0x04 },
+	{ 0x3804, 0x02 },
+	{ 0x3805, 0x8b },
+	{ 0x3806, 0x01 },
+	{ 0x3807, 0xeb },
+	{ 0x3808, 0x02 }, /* width high */
+	{ 0x3809, 0x80 }, /* width low */
+	{ 0x380a, 0x01 }, /* height high */
+	{ 0x380b, 0xe0 }, /* height low */
+	{ 0x380c, 0x03 }, /* total horiz timing high */
+	{ 0x380d, 0xa0 }, /* total horiz timing low */
+	{ 0x380e, 0x06 }, /* total vertical timing high */
+	{ 0x380f, 0xbc }, /* total vertical timing low */
+	{ 0x3810, 0x00 },
+	{ 0x3811, 0x04 },
+	{ 0x3812, 0x00 },
+	{ 0x3813, 0x05 },
+	{ 0x3814, 0x11 },
+	{ 0x3815, 0x11 },
+	{ 0x3820, 0x40 },
+	{ 0x3821, 0x00 },
+	{ 0x382f, 0x0e },
+	{ 0x3832, 0x00 },
+	{ 0x3833, 0x05 },
+	{ 0x3834, 0x00 },
+	{ 0x3835, 0x0c },
+	{ 0x3837, 0x00 },
+	{ 0x3b80, 0x00 },
+	{ 0x3b81, 0xa5 },
+	{ 0x3b82, 0x10 },
+	{ 0x3b83, 0x00 },
+	{ 0x3b84, 0x08 },
+	{ 0x3b85, 0x00 },
+	{ 0x3b86, 0x01 },
+	{ 0x3b87, 0x00 },
+	{ 0x3b88, 0x00 },
+	{ 0x3b89, 0x00 },
+	{ 0x3b8a, 0x00 },
+	{ 0x3b8b, 0x05 },
+	{ 0x3b8c, 0x00 },
+	{ 0x3b8d, 0x00 },
+	{ 0x3b8e, 0x00 },
+	{ 0x3b8f, 0x1a },
+	{ 0x3b94, 0x05 },
+	{ 0x3b95, 0xf2 },
+	{ 0x3b96, 0x40 },
+	{ 0x3c00, 0x89 },
+	{ 0x3c01, 0x63 },
+	{ 0x3c02, 0x01 },
+	{ 0x3c03, 0x00 },
+	{ 0x3c04, 0x00 },
+	{ 0x3c05, 0x03 },
+	{ 0x3c06, 0x00 },
+	{ 0x3c07, 0x06 },
+	{ 0x3c0c, 0x01 },
+	{ 0x3c0d, 0xd0 },
+	{ 0x3c0e, 0x02 },
+	{ 0x3c0f, 0x0a },
+	{ 0x4001, 0x42 },
+	{ 0x4004, 0x04 },
+	{ 0x4005, 0x00 },
+	{ 0x404e, 0x01 },
+	{ 0x4300, 0xff },
+	{ 0x4301, 0x00 },
+	{ 0x4315, 0x00 },
+	{ 0x4501, 0x48 },
+	{ 0x4600, 0x00 },
+	{ 0x4601, 0x4e },
+	{ 0x4801, 0x0f },
+	{ 0x4806, 0x0f },
+	{ 0x4819, 0xaa },
+	{ 0x4823, 0x3e },
+	{ 0x4837, 0x19 },
+	{ 0x4a0d, 0x00 },
+	{ 0x4a47, 0x7f },
+	{ 0x4a49, 0xf0 },
+	{ 0x4a4b, 0x30 },
+	{ 0x5000, 0x85 },
+	{ 0x5001, 0x80 },
+};
+
+static const struct reg_value ov7251_setting_vga_60fps[] = {
+	{ 0x3005, 0x00 },
+	{ 0x3012, 0xc0 },
+	{ 0x3013, 0xd2 },
+	{ 0x3014, 0x04 },
+	{ 0x3016, 0x10 },
+	{ 0x3017, 0x00 },
+	{ 0x3018, 0x00 },
+	{ 0x301a, 0x00 },
+	{ 0x301b, 0x00 },
+	{ 0x301c, 0x00 },
+	{ 0x3023, 0x05 },
+	{ 0x3037, 0xf0 },
+	{ 0x3106, 0xda },
+	{ 0x3503, 0x07 },
+	{ 0x3509, 0x10 },
+	{ 0x3600, 0x1c },
+	{ 0x3602, 0x62 },
+	{ 0x3620, 0xb7 },
+	{ 0x3622, 0x04 },
+	{ 0x3626, 0x21 },
+	{ 0x3627, 0x30 },
+	{ 0x3630, 0x44 },
+	{ 0x3631, 0x35 },
+	{ 0x3634, 0x60 },
+	{ 0x3636, 0x00 },
+	{ 0x3662, 0x01 },
+	{ 0x3663, 0x70 },
+	{ 0x3664, 0x50 },
+	{ 0x3666, 0x0a },
+	{ 0x3669, 0x1a },
+	{ 0x366a, 0x00 },
+	{ 0x366b, 0x50 },
+	{ 0x3673, 0x01 },
+	{ 0x3674, 0xff },
+	{ 0x3675, 0x03 },
+	{ 0x3705, 0xc1 },
+	{ 0x3709, 0x40 },
+	{ 0x373c, 0x08 },
+	{ 0x3742, 0x00 },
+	{ 0x3757, 0xb3 },
+	{ 0x3788, 0x00 },
+	{ 0x37a8, 0x01 },
+	{ 0x37a9, 0xc0 },
+	{ 0x3800, 0x00 },
+	{ 0x3801, 0x04 },
+	{ 0x3802, 0x00 },
+	{ 0x3803, 0x04 },
+	{ 0x3804, 0x02 },
+	{ 0x3805, 0x8b },
+	{ 0x3806, 0x01 },
+	{ 0x3807, 0xeb },
+	{ 0x3808, 0x02 }, /* width high */
+	{ 0x3809, 0x80 }, /* width low */
+	{ 0x380a, 0x01 }, /* height high */
+	{ 0x380b, 0xe0 }, /* height low */
+	{ 0x380c, 0x03 }, /* total horiz timing high */
+	{ 0x380d, 0xa0 }, /* total horiz timing low */
+	{ 0x380e, 0x03 }, /* total vertical timing high */
+	{ 0x380f, 0x5c }, /* total vertical timing low */
+	{ 0x3810, 0x00 },
+	{ 0x3811, 0x04 },
+	{ 0x3812, 0x00 },
+	{ 0x3813, 0x05 },
+	{ 0x3814, 0x11 },
+	{ 0x3815, 0x11 },
+	{ 0x3820, 0x40 },
+	{ 0x3821, 0x00 },
+	{ 0x382f, 0x0e },
+	{ 0x3832, 0x00 },
+	{ 0x3833, 0x05 },
+	{ 0x3834, 0x00 },
+	{ 0x3835, 0x0c },
+	{ 0x3837, 0x00 },
+	{ 0x3b80, 0x00 },
+	{ 0x3b81, 0xa5 },
+	{ 0x3b82, 0x10 },
+	{ 0x3b83, 0x00 },
+	{ 0x3b84, 0x08 },
+	{ 0x3b85, 0x00 },
+	{ 0x3b86, 0x01 },
+	{ 0x3b87, 0x00 },
+	{ 0x3b88, 0x00 },
+	{ 0x3b89, 0x00 },
+	{ 0x3b8a, 0x00 },
+	{ 0x3b8b, 0x05 },
+	{ 0x3b8c, 0x00 },
+	{ 0x3b8d, 0x00 },
+	{ 0x3b8e, 0x00 },
+	{ 0x3b8f, 0x1a },
+	{ 0x3b94, 0x05 },
+	{ 0x3b95, 0xf2 },
+	{ 0x3b96, 0x40 },
+	{ 0x3c00, 0x89 },
+	{ 0x3c01, 0x63 },
+	{ 0x3c02, 0x01 },
+	{ 0x3c03, 0x00 },
+	{ 0x3c04, 0x00 },
+	{ 0x3c05, 0x03 },
+	{ 0x3c06, 0x00 },
+	{ 0x3c07, 0x06 },
+	{ 0x3c0c, 0x01 },
+	{ 0x3c0d, 0xd0 },
+	{ 0x3c0e, 0x02 },
+	{ 0x3c0f, 0x0a },
+	{ 0x4001, 0x42 },
+	{ 0x4004, 0x04 },
+	{ 0x4005, 0x00 },
+	{ 0x404e, 0x01 },
+	{ 0x4300, 0xff },
+	{ 0x4301, 0x00 },
+	{ 0x4315, 0x00 },
+	{ 0x4501, 0x48 },
+	{ 0x4600, 0x00 },
+	{ 0x4601, 0x4e },
+	{ 0x4801, 0x0f },
+	{ 0x4806, 0x0f },
+	{ 0x4819, 0xaa },
+	{ 0x4823, 0x3e },
+	{ 0x4837, 0x19 },
+	{ 0x4a0d, 0x00 },
+	{ 0x4a47, 0x7f },
+	{ 0x4a49, 0xf0 },
+	{ 0x4a4b, 0x30 },
+	{ 0x5000, 0x85 },
+	{ 0x5001, 0x80 },
+};
+
+static const struct reg_value ov7251_setting_vga_90fps[] = {
+	{ 0x3005, 0x00 },
+	{ 0x3012, 0xc0 },
+	{ 0x3013, 0xd2 },
+	{ 0x3014, 0x04 },
+	{ 0x3016, 0x10 },
+	{ 0x3017, 0x00 },
+	{ 0x3018, 0x00 },
+	{ 0x301a, 0x00 },
+	{ 0x301b, 0x00 },
+	{ 0x301c, 0x00 },
+	{ 0x3023, 0x05 },
+	{ 0x3037, 0xf0 },
+	{ 0x3106, 0xda },
+	{ 0x3503, 0x07 },
+	{ 0x3509, 0x10 },
+	{ 0x3600, 0x1c },
+	{ 0x3602, 0x62 },
+	{ 0x3620, 0xb7 },
+	{ 0x3622, 0x04 },
+	{ 0x3626, 0x21 },
+	{ 0x3627, 0x30 },
+	{ 0x3630, 0x44 },
+	{ 0x3631, 0x35 },
+	{ 0x3634, 0x60 },
+	{ 0x3636, 0x00 },
+	{ 0x3662, 0x01 },
+	{ 0x3663, 0x70 },
+	{ 0x3664, 0x50 },
+	{ 0x3666, 0x0a },
+	{ 0x3669, 0x1a },
+	{ 0x366a, 0x00 },
+	{ 0x366b, 0x50 },
+	{ 0x3673, 0x01 },
+	{ 0x3674, 0xff },
+	{ 0x3675, 0x03 },
+	{ 0x3705, 0xc1 },
+	{ 0x3709, 0x40 },
+	{ 0x373c, 0x08 },
+	{ 0x3742, 0x00 },
+	{ 0x3757, 0xb3 },
+	{ 0x3788, 0x00 },
+	{ 0x37a8, 0x01 },
+	{ 0x37a9, 0xc0 },
+	{ 0x3800, 0x00 },
+	{ 0x3801, 0x04 },
+	{ 0x3802, 0x00 },
+	{ 0x3803, 0x04 },
+	{ 0x3804, 0x02 },
+	{ 0x3805, 0x8b },
+	{ 0x3806, 0x01 },
+	{ 0x3807, 0xeb },
+	{ 0x3808, 0x02 }, /* width high */
+	{ 0x3809, 0x80 }, /* width low */
+	{ 0x380a, 0x01 }, /* height high */
+	{ 0x380b, 0xe0 }, /* height low */
+	{ 0x380c, 0x03 }, /* total horiz timing high */
+	{ 0x380d, 0xa0 }, /* total horiz timing low */
+	{ 0x380e, 0x02 }, /* total vertical timing high */
+	{ 0x380f, 0x3c }, /* total vertical timing low */
+	{ 0x3810, 0x00 },
+	{ 0x3811, 0x04 },
+	{ 0x3812, 0x00 },
+	{ 0x3813, 0x05 },
+	{ 0x3814, 0x11 },
+	{ 0x3815, 0x11 },
+	{ 0x3820, 0x40 },
+	{ 0x3821, 0x00 },
+	{ 0x382f, 0x0e },
+	{ 0x3832, 0x00 },
+	{ 0x3833, 0x05 },
+	{ 0x3834, 0x00 },
+	{ 0x3835, 0x0c },
+	{ 0x3837, 0x00 },
+	{ 0x3b80, 0x00 },
+	{ 0x3b81, 0xa5 },
+	{ 0x3b82, 0x10 },
+	{ 0x3b83, 0x00 },
+	{ 0x3b84, 0x08 },
+	{ 0x3b85, 0x00 },
+	{ 0x3b86, 0x01 },
+	{ 0x3b87, 0x00 },
+	{ 0x3b88, 0x00 },
+	{ 0x3b89, 0x00 },
+	{ 0x3b8a, 0x00 },
+	{ 0x3b8b, 0x05 },
+	{ 0x3b8c, 0x00 },
+	{ 0x3b8d, 0x00 },
+	{ 0x3b8e, 0x00 },
+	{ 0x3b8f, 0x1a },
+	{ 0x3b94, 0x05 },
+	{ 0x3b95, 0xf2 },
+	{ 0x3b96, 0x40 },
+	{ 0x3c00, 0x89 },
+	{ 0x3c01, 0x63 },
+	{ 0x3c02, 0x01 },
+	{ 0x3c03, 0x00 },
+	{ 0x3c04, 0x00 },
+	{ 0x3c05, 0x03 },
+	{ 0x3c06, 0x00 },
+	{ 0x3c07, 0x06 },
+	{ 0x3c0c, 0x01 },
+	{ 0x3c0d, 0xd0 },
+	{ 0x3c0e, 0x02 },
+	{ 0x3c0f, 0x0a },
+	{ 0x4001, 0x42 },
+	{ 0x4004, 0x04 },
+	{ 0x4005, 0x00 },
+	{ 0x404e, 0x01 },
+	{ 0x4300, 0xff },
+	{ 0x4301, 0x00 },
+	{ 0x4315, 0x00 },
+	{ 0x4501, 0x48 },
+	{ 0x4600, 0x00 },
+	{ 0x4601, 0x4e },
+	{ 0x4801, 0x0f },
+	{ 0x4806, 0x0f },
+	{ 0x4819, 0xaa },
+	{ 0x4823, 0x3e },
+	{ 0x4837, 0x19 },
+	{ 0x4a0d, 0x00 },
+	{ 0x4a47, 0x7f },
+	{ 0x4a49, 0xf0 },
+	{ 0x4a4b, 0x30 },
+	{ 0x5000, 0x85 },
+	{ 0x5001, 0x80 },
+};
+
+static const unsigned long supported_xclk_rates[] = {
+	[OV7251_19_2_MHZ] = 19200000,
+	[OV7251_24_MHZ] = 24000000,
+};
+
+static const s64 link_freq[] = {
+	[OV7251_LINK_FREQ_240_MHZ] = 240000000,
+	[OV7251_LINK_FREQ_319_2_MHZ] = 319200000,
+};
+
+static const s64 pixel_rates[] = {
+	[OV7251_LINK_FREQ_240_MHZ] = 48000000,
+	[OV7251_LINK_FREQ_319_2_MHZ] = 63840000,
+};
+
+static const struct ov7251_mode_info ov7251_mode_info_data[] = {
+	{
+		.width = 640,
+		.height = 480,
+		.vts = 1724,
+		.data = ov7251_setting_vga_30fps,
+		.data_size = ARRAY_SIZE(ov7251_setting_vga_30fps),
+		.exposure_max = 1704,
+		.exposure_def = 504,
+		.timeperframe = {
+			.numerator = 100,
+			.denominator = 3000
+		}
+	},
+	{
+		.width = 640,
+		.height = 480,
+		.vts = 860,
+		.data = ov7251_setting_vga_60fps,
+		.data_size = ARRAY_SIZE(ov7251_setting_vga_60fps),
+		.exposure_max = 840,
+		.exposure_def = 504,
+		.timeperframe = {
+			.numerator = 100,
+			.denominator = 6014
+		}
+	},
+	{
+		.width = 640,
+		.height = 480,
+		.vts = 572,
+		.data = ov7251_setting_vga_90fps,
+		.data_size = ARRAY_SIZE(ov7251_setting_vga_90fps),
+		.exposure_max = 552,
+		.exposure_def = 504,
+		.timeperframe = {
+			.numerator = 100,
+			.denominator = 9043
+		}
+	},
+};
+
+static int ov7251_regulators_enable(struct ov7251 *ov7251)
+{
+	int ret;
+
+	/* OV7251 power up sequence requires core regulator
+	 * to be enabled not earlier than io regulator
+	 */
+
+	ret = regulator_enable(ov7251->io_regulator);
+	if (ret < 0) {
+		dev_err(ov7251->dev, "set io voltage failed\n");
+		return ret;
+	}
+
+	ret = regulator_enable(ov7251->analog_regulator);
+	if (ret) {
+		dev_err(ov7251->dev, "set analog voltage failed\n");
+		goto err_disable_io;
+	}
+
+	ret = regulator_enable(ov7251->core_regulator);
+	if (ret) {
+		dev_err(ov7251->dev, "set core voltage failed\n");
+		goto err_disable_analog;
+	}
+
+	return 0;
+
+err_disable_analog:
+	regulator_disable(ov7251->analog_regulator);
+
+err_disable_io:
+	regulator_disable(ov7251->io_regulator);
+
+	return ret;
+}
+
+static void ov7251_regulators_disable(struct ov7251 *ov7251)
+{
+	int ret;
+
+	ret = regulator_disable(ov7251->core_regulator);
+	if (ret < 0)
+		dev_err(ov7251->dev, "core regulator disable failed\n");
+
+	ret = regulator_disable(ov7251->analog_regulator);
+	if (ret < 0)
+		dev_err(ov7251->dev, "analog regulator disable failed\n");
+
+	ret = regulator_disable(ov7251->io_regulator);
+	if (ret < 0)
+		dev_err(ov7251->dev, "io regulator disable failed\n");
+}
+
+static int ov7251_write_reg(struct ov7251 *ov7251, u16 reg, u8 val)
+{
+	u8 regbuf[3];
+	int ret;
+
+	regbuf[0] = reg >> 8;
+	regbuf[1] = reg & 0xff;
+	regbuf[2] = val;
+
+	ret = i2c_master_send(ov7251->i2c_client, regbuf, 3);
+	if (ret < 0) {
+		dev_err(ov7251->dev, "%s: write reg error %d: reg=%x, val=%x\n",
+			__func__, ret, reg, val);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ov7251_write_seq_regs(struct ov7251 *ov7251, u16 reg, u8 *val,
+				 u8 num)
+{
+	u8 regbuf[5];
+	u8 nregbuf = sizeof(reg) + num * sizeof(*val);
+	int ret = 0;
+
+	if (nregbuf > sizeof(regbuf))
+		return -EINVAL;
+
+	regbuf[0] = reg >> 8;
+	regbuf[1] = reg & 0xff;
+
+	memcpy(regbuf + 2, val, num);
+
+	ret = i2c_master_send(ov7251->i2c_client, regbuf, nregbuf);
+	if (ret < 0) {
+		dev_err(ov7251->dev,
+			"%s: write seq regs error %d: first reg=%x\n",
+			__func__, ret, reg);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ov7251_read_reg(struct ov7251 *ov7251, u16 reg, u8 *val)
+{
+	u8 regbuf[2];
+	int ret;
+
+	regbuf[0] = reg >> 8;
+	regbuf[1] = reg & 0xff;
+
+	ret = i2c_master_send(ov7251->i2c_client, regbuf, 2);
+	if (ret < 0) {
+		dev_err(ov7251->dev, "%s: write reg error %d: reg=%x\n",
+			__func__, ret, reg);
+		return ret;
+	}
+
+	ret = i2c_master_recv(ov7251->i2c_client, val, 1);
+	if (ret < 0) {
+		dev_err(ov7251->dev, "%s: read reg error %d: reg=%x\n",
+			__func__, ret, reg);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ov7251_pll_configure(struct ov7251 *ov7251)
+{
+	const struct ov7251_pll_cfgs *configs;
+	int ret;
+
+	configs = ov7251->pll_cfgs;
+
+	ret = ov7251_write_reg(ov7251, OV7251_PLL1_PRE_DIV_REG,
+			       configs->pll1[ov7251->link_freq_idx]->pre_div);
+	if (ret < 0)
+		return ret;
+
+	ret = ov7251_write_reg(ov7251, OV7251_PLL1_MULT_REG,
+			       configs->pll1[ov7251->link_freq_idx]->mult);
+	if (ret < 0)
+		return ret;
+	ret = ov7251_write_reg(ov7251, OV7251_PLL1_DIVIDER_REG,
+			       configs->pll1[ov7251->link_freq_idx]->div);
+	if (ret < 0)
+		return ret;
+
+	ret = ov7251_write_reg(ov7251, OV7251_PLL1_PIX_DIV_REG,
+			       configs->pll1[ov7251->link_freq_idx]->pix_div);
+	if (ret < 0)
+		return ret;
+
+	ret = ov7251_write_reg(ov7251, OV7251_PLL1_MIPI_DIV_REG,
+			       configs->pll1[ov7251->link_freq_idx]->mipi_div);
+	if (ret < 0)
+		return ret;
+
+	ret = ov7251_write_reg(ov7251, OV7251_PLL2_PRE_DIV_REG,
+			       configs->pll2->pre_div);
+	if (ret < 0)
+		return ret;
+
+	ret = ov7251_write_reg(ov7251, OV7251_PLL2_MULT_REG,
+			       configs->pll2->mult);
+	if (ret < 0)
+		return ret;
+
+	ret = ov7251_write_reg(ov7251, OV7251_PLL2_DIVIDER_REG,
+			       configs->pll2->div);
+	if (ret < 0)
+		return ret;
+
+	ret = ov7251_write_reg(ov7251, OV7251_PLL2_SYS_DIV_REG,
+			       configs->pll2->sys_div);
+	if (ret < 0)
+		return ret;
+
+	ret = ov7251_write_reg(ov7251, OV7251_PLL2_ADC_DIV_REG,
+			       configs->pll2->adc_div);
+
+	return ret;
+}
+
+static int ov7251_set_exposure(struct ov7251 *ov7251, s32 exposure)
+{
+	u16 reg;
+	u8 val[3];
+
+	reg = OV7251_AEC_EXPO_0;
+	val[0] = (exposure & 0xf000) >> 12; /* goes to OV7251_AEC_EXPO_0 */
+	val[1] = (exposure & 0x0ff0) >> 4;  /* goes to OV7251_AEC_EXPO_1 */
+	val[2] = (exposure & 0x000f) << 4;  /* goes to OV7251_AEC_EXPO_2 */
+
+	return ov7251_write_seq_regs(ov7251, reg, val, 3);
+}
+
+static int ov7251_set_gain(struct ov7251 *ov7251, s32 gain)
+{
+	u16 reg;
+	u8 val[2];
+
+	reg = OV7251_AEC_AGC_ADJ_0;
+	val[0] = (gain & 0x0300) >> 8; /* goes to OV7251_AEC_AGC_ADJ_0 */
+	val[1] = gain & 0xff;          /* goes to OV7251_AEC_AGC_ADJ_1 */
+
+	return ov7251_write_seq_regs(ov7251, reg, val, 2);
+}
+
+static int ov7251_set_register_array(struct ov7251 *ov7251,
+				     const struct reg_value *settings,
+				     unsigned int num_settings)
+{
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < num_settings; ++i, ++settings) {
+		ret = ov7251_write_reg(ov7251, settings->reg, settings->val);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int ov7251_set_power_on(struct device *dev)
+{
+	struct i2c_client *client = container_of(dev, struct i2c_client, dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov7251 *ov7251 = to_ov7251(sd);
+	int ret;
+	u32 wait_us;
+
+	ret = ov7251_regulators_enable(ov7251);
+	if (ret < 0)
+		return ret;
+
+	ret = clk_prepare_enable(ov7251->xclk);
+	if (ret < 0) {
+		dev_err(ov7251->dev, "clk prepare enable failed\n");
+		ov7251_regulators_disable(ov7251);
+		return ret;
+	}
+
+	gpiod_set_value_cansleep(ov7251->enable_gpio, 1);
+
+	/* wait at least 65536 external clock cycles */
+	wait_us = DIV_ROUND_UP(65536 * 1000,
+			       DIV_ROUND_UP(ov7251->xclk_freq, 1000));
+	usleep_range(wait_us, wait_us + 1000);
+
+	ret = ov7251_set_register_array(ov7251,
+					ov7251_global_init_setting,
+					ARRAY_SIZE(ov7251_global_init_setting));
+	if (ret < 0) {
+		dev_err(ov7251->dev, "error during global init\n");
+		gpiod_set_value_cansleep(ov7251->enable_gpio, 0);
+		clk_disable_unprepare(ov7251->xclk);
+		ov7251_regulators_disable(ov7251);
+		return ret;
+	}
+
+	return ret;
+}
+
+static int ov7251_set_power_off(struct device *dev)
+{
+	struct i2c_client *client = container_of(dev, struct i2c_client, dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov7251 *ov7251 = to_ov7251(sd);
+
+	clk_disable_unprepare(ov7251->xclk);
+	gpiod_set_value_cansleep(ov7251->enable_gpio, 0);
+	ov7251_regulators_disable(ov7251);
+
+	return 0;
+}
+
+static int ov7251_set_hflip(struct ov7251 *ov7251, s32 value)
+{
+	u8 val = ov7251->timing_format2;
+	int ret;
+
+	if (value)
+		val |= OV7251_TIMING_FORMAT2_MIRROR;
+	else
+		val &= ~OV7251_TIMING_FORMAT2_MIRROR;
+
+	ret = ov7251_write_reg(ov7251, OV7251_TIMING_FORMAT2, val);
+	if (!ret)
+		ov7251->timing_format2 = val;
+
+	return ret;
+}
+
+static int ov7251_set_vflip(struct ov7251 *ov7251, s32 value)
+{
+	u8 val = ov7251->timing_format1;
+	int ret;
+
+	if (value)
+		val |= OV7251_TIMING_FORMAT1_VFLIP;
+	else
+		val &= ~OV7251_TIMING_FORMAT1_VFLIP;
+
+	ret = ov7251_write_reg(ov7251, OV7251_TIMING_FORMAT1, val);
+	if (!ret)
+		ov7251->timing_format1 = val;
+
+	return ret;
+}
+
+static int ov7251_set_test_pattern(struct ov7251 *ov7251, s32 value)
+{
+	u8 val = ov7251->pre_isp_00;
+	int ret;
+
+	if (value)
+		val |= OV7251_PRE_ISP_00_TEST_PATTERN;
+	else
+		val &= ~OV7251_PRE_ISP_00_TEST_PATTERN;
+
+	ret = ov7251_write_reg(ov7251, OV7251_PRE_ISP_00, val);
+	if (!ret)
+		ov7251->pre_isp_00 = val;
+
+	return ret;
+}
+
+static const char * const ov7251_test_pattern_menu[] = {
+	"Disabled",
+	"Vertical Pattern Bars",
+};
+
+static int ov7251_vts_configure(struct ov7251 *ov7251, s32 vblank)
+{
+	u8 vts[2];
+
+	vts[0] = ((ov7251->current_mode->height + vblank) & 0xff00) >> 8;
+	vts[1] = ((ov7251->current_mode->height + vblank) & 0x00ff);
+
+	return ov7251_write_seq_regs(ov7251, OV7251_TIMING_VTS_REG, vts, 2);
+}
+
+static int ov7251_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov7251 *ov7251 = container_of(ctrl->handler,
+					     struct ov7251, ctrls);
+	int ret;
+
+	/* If VBLANK is altered we need to update exposure to compensate */
+	if (ctrl->id == V4L2_CID_VBLANK) {
+		int exposure_max;
+
+		exposure_max = ov7251->current_mode->height + ctrl->val -
+			       OV7251_INTEGRATION_MARGIN;
+		__v4l2_ctrl_modify_range(ov7251->exposure,
+					 ov7251->exposure->minimum,
+					 exposure_max,
+					 ov7251->exposure->step,
+					 min(ov7251->exposure->val,
+					     exposure_max));
+	}
+
+	/* v4l2_ctrl_lock() locks our mutex */
+
+	if (!pm_runtime_get_if_in_use(ov7251->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE:
+		ret = ov7251_set_exposure(ov7251, ctrl->val);
+		break;
+	case V4L2_CID_GAIN:
+		ret = ov7251_set_gain(ov7251, ctrl->val);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		ret = ov7251_set_test_pattern(ov7251, ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+		ret = ov7251_set_hflip(ov7251, ctrl->val);
+		break;
+	case V4L2_CID_VFLIP:
+		ret = ov7251_set_vflip(ov7251, ctrl->val);
+		break;
+	case V4L2_CID_VBLANK:
+		ret = ov7251_vts_configure(ov7251, ctrl->val);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	pm_runtime_put(ov7251->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ov7251_ctrl_ops = {
+	.s_ctrl = ov7251_s_ctrl,
+};
+
+static int ov7251_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index > 0)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_Y10_1X10;
+
+	return 0;
+}
+
+static int ov7251_enum_frame_size(struct v4l2_subdev *subdev,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->code != MEDIA_BUS_FMT_Y10_1X10)
+		return -EINVAL;
+
+	if (fse->index >= ARRAY_SIZE(ov7251_mode_info_data))
+		return -EINVAL;
+
+	fse->min_width = ov7251_mode_info_data[fse->index].width;
+	fse->max_width = ov7251_mode_info_data[fse->index].width;
+	fse->min_height = ov7251_mode_info_data[fse->index].height;
+	fse->max_height = ov7251_mode_info_data[fse->index].height;
+
+	return 0;
+}
+
+static int ov7251_enum_frame_ival(struct v4l2_subdev *subdev,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_interval_enum *fie)
+{
+	unsigned int index = fie->index;
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(ov7251_mode_info_data); i++) {
+		if (fie->width != ov7251_mode_info_data[i].width ||
+		    fie->height != ov7251_mode_info_data[i].height)
+			continue;
+
+		if (index-- == 0) {
+			fie->interval = ov7251_mode_info_data[i].timeperframe;
+			return 0;
+		}
+	}
+
+	return -EINVAL;
+}
+
+static struct v4l2_mbus_framefmt *
+__ov7251_get_pad_format(struct ov7251 *ov7251,
+			struct v4l2_subdev_state *sd_state,
+			unsigned int pad,
+			enum v4l2_subdev_format_whence which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_format(&ov7251->sd, sd_state, pad);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return &ov7251->fmt;
+	default:
+		return NULL;
+	}
+}
+
+static int ov7251_get_format(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *sd_state,
+			     struct v4l2_subdev_format *format)
+{
+	struct ov7251 *ov7251 = to_ov7251(sd);
+
+	mutex_lock(&ov7251->lock);
+	format->format = *__ov7251_get_pad_format(ov7251, sd_state,
+						  format->pad,
+						  format->which);
+	mutex_unlock(&ov7251->lock);
+
+	return 0;
+}
+
+static struct v4l2_rect *
+__ov7251_get_pad_crop(struct ov7251 *ov7251,
+		      struct v4l2_subdev_state *sd_state,
+		      unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_crop(&ov7251->sd, sd_state, pad);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return &ov7251->crop;
+	default:
+		return NULL;
+	}
+}
+
+static inline u32 avg_fps(const struct v4l2_fract *t)
+{
+	return (t->denominator + (t->numerator >> 1)) / t->numerator;
+}
+
+static const struct ov7251_mode_info *
+ov7251_find_mode_by_ival(struct ov7251 *ov7251, struct v4l2_fract *timeperframe)
+{
+	const struct ov7251_mode_info *mode = ov7251->current_mode;
+	unsigned int fps_req = avg_fps(timeperframe);
+	unsigned int max_dist_match = (unsigned int) -1;
+	unsigned int i, n = 0;
+
+	for (i = 0; i < ARRAY_SIZE(ov7251_mode_info_data); i++) {
+		unsigned int dist;
+		unsigned int fps_tmp;
+
+		if (mode->width != ov7251_mode_info_data[i].width ||
+		    mode->height != ov7251_mode_info_data[i].height)
+			continue;
+
+		fps_tmp = avg_fps(&ov7251_mode_info_data[i].timeperframe);
+
+		dist = abs(fps_req - fps_tmp);
+
+		if (dist < max_dist_match) {
+			n = i;
+			max_dist_match = dist;
+		}
+	}
+
+	return &ov7251_mode_info_data[n];
+}
+
+static int ov7251_set_format(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *sd_state,
+			     struct v4l2_subdev_format *format)
+{
+	struct ov7251 *ov7251 = to_ov7251(sd);
+	struct v4l2_mbus_framefmt *__format;
+	int vblank_max, vblank_def;
+	struct v4l2_rect *__crop;
+	const struct ov7251_mode_info *new_mode;
+	int ret = 0;
+
+	mutex_lock(&ov7251->lock);
+
+	__crop = __ov7251_get_pad_crop(ov7251, sd_state, format->pad,
+				       format->which);
+
+	new_mode = v4l2_find_nearest_size(ov7251_mode_info_data,
+				ARRAY_SIZE(ov7251_mode_info_data),
+				width, height,
+				format->format.width, format->format.height);
+
+	__crop->width = new_mode->width;
+	__crop->height = new_mode->height;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		ret = __v4l2_ctrl_modify_range(ov7251->exposure,
+					       1, new_mode->exposure_max,
+					       1, new_mode->exposure_def);
+		if (ret < 0)
+			goto exit;
+
+		ret = __v4l2_ctrl_s_ctrl(ov7251->exposure,
+					 new_mode->exposure_def);
+		if (ret < 0)
+			goto exit;
+
+		ret = __v4l2_ctrl_s_ctrl(ov7251->gain, 16);
+		if (ret < 0)
+			goto exit;
+
+		vblank_max = OV7251_TIMING_MAX_VTS - new_mode->height;
+		vblank_def = new_mode->vts - new_mode->height;
+		ret = __v4l2_ctrl_modify_range(ov7251->vblank,
+					       OV7251_TIMING_MIN_VTS,
+					       vblank_max, 1, vblank_def);
+		if (ret < 0)
+			goto exit;
+
+		ov7251->current_mode = new_mode;
+	}
+
+	__format = __ov7251_get_pad_format(ov7251, sd_state, format->pad,
+					   format->which);
+	__format->width = __crop->width;
+	__format->height = __crop->height;
+	__format->code = MEDIA_BUS_FMT_Y10_1X10;
+	__format->field = V4L2_FIELD_NONE;
+	__format->colorspace = V4L2_COLORSPACE_SRGB;
+	__format->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(__format->colorspace);
+	__format->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(true,
+				__format->colorspace, __format->ycbcr_enc);
+	__format->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(__format->colorspace);
+
+	format->format = *__format;
+
+exit:
+	mutex_unlock(&ov7251->lock);
+
+	return ret;
+}
+
+static int ov7251_entity_init_cfg(struct v4l2_subdev *subdev,
+				  struct v4l2_subdev_state *sd_state)
+{
+	struct v4l2_subdev_format fmt = {
+		.which = sd_state ? V4L2_SUBDEV_FORMAT_TRY
+		: V4L2_SUBDEV_FORMAT_ACTIVE,
+		.format = {
+			.width = 640,
+			.height = 480
+		}
+	};
+
+	ov7251_set_format(subdev, sd_state, &fmt);
+
+	return 0;
+}
+
+static int ov7251_get_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *sd_state,
+				struct v4l2_subdev_selection *sel)
+{
+	struct ov7251 *ov7251 = to_ov7251(sd);
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP:
+	mutex_lock(&ov7251->lock);
+		sel->r = *__ov7251_get_pad_crop(ov7251, sd_state, sel->pad,
+						sel->which);
+		mutex_unlock(&ov7251->lock);
+		break;
+	case V4L2_SEL_TGT_NATIVE_SIZE:
+		sel->r.top = 0;
+		sel->r.left = 0;
+		sel->r.width = OV7251_NATIVE_WIDTH;
+		sel->r.height = OV7251_NATIVE_HEIGHT;
+		break;
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.top = OV7251_ACTIVE_START_TOP;
+		sel->r.left = OV7251_ACTIVE_START_LEFT;
+		sel->r.width = OV7251_ACTIVE_WIDTH;
+		sel->r.height = OV7251_ACTIVE_HEIGHT;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int ov7251_s_stream(struct v4l2_subdev *subdev, int enable)
+{
+	struct ov7251 *ov7251 = to_ov7251(subdev);
+	int ret;
+
+	mutex_lock(&ov7251->lock);
+
+	if (enable) {
+		ret = pm_runtime_get_sync(ov7251->dev);
+		if (ret < 0)
+			goto err_power_down;
+
+		ret = ov7251_pll_configure(ov7251);
+		if (ret) {
+			dev_err(ov7251->dev, "error configuring PLLs\n");
+			goto err_power_down;
+		}
+
+		ret = ov7251_set_register_array(ov7251,
+					ov7251->current_mode->data,
+					ov7251->current_mode->data_size);
+		if (ret < 0) {
+			dev_err(ov7251->dev, "could not set mode %dx%d\n",
+				ov7251->current_mode->width,
+				ov7251->current_mode->height);
+			goto err_power_down;
+		}
+		ret = __v4l2_ctrl_handler_setup(&ov7251->ctrls);
+		if (ret < 0) {
+			dev_err(ov7251->dev, "could not sync v4l2 controls\n");
+			goto err_power_down;
+		}
+		ret = ov7251_write_reg(ov7251, OV7251_SC_MODE_SELECT,
+				       OV7251_SC_MODE_SELECT_STREAMING);
+		if (ret)
+			goto err_power_down;
+	} else {
+		ret = ov7251_write_reg(ov7251, OV7251_SC_MODE_SELECT,
+				       OV7251_SC_MODE_SELECT_SW_STANDBY);
+		pm_runtime_put(ov7251->dev);
+	}
+
+	mutex_unlock(&ov7251->lock);
+	return ret;
+
+err_power_down:
+	pm_runtime_put(ov7251->dev);
+	mutex_unlock(&ov7251->lock);
+	return ret;
+}
+
+static int ov7251_get_frame_interval(struct v4l2_subdev *subdev,
+				     struct v4l2_subdev_frame_interval *fi)
+{
+	struct ov7251 *ov7251 = to_ov7251(subdev);
+
+	mutex_lock(&ov7251->lock);
+	fi->interval = ov7251->current_mode->timeperframe;
+	mutex_unlock(&ov7251->lock);
+
+	return 0;
+}
+
+static int ov7251_set_frame_interval(struct v4l2_subdev *subdev,
+				     struct v4l2_subdev_frame_interval *fi)
+{
+	struct ov7251 *ov7251 = to_ov7251(subdev);
+	const struct ov7251_mode_info *new_mode;
+	int ret = 0;
+
+	mutex_lock(&ov7251->lock);
+	new_mode = ov7251_find_mode_by_ival(ov7251, &fi->interval);
+
+	if (new_mode != ov7251->current_mode) {
+		ret = __v4l2_ctrl_modify_range(ov7251->exposure,
+					       1, new_mode->exposure_max,
+					       1, new_mode->exposure_def);
+		if (ret < 0)
+			goto exit;
+
+		ret = __v4l2_ctrl_s_ctrl(ov7251->exposure,
+					 new_mode->exposure_def);
+		if (ret < 0)
+			goto exit;
+
+		ret = __v4l2_ctrl_s_ctrl(ov7251->gain, 16);
+		if (ret < 0)
+			goto exit;
+
+		ov7251->current_mode = new_mode;
+	}
+
+	fi->interval = ov7251->current_mode->timeperframe;
+
+exit:
+	mutex_unlock(&ov7251->lock);
+
+	return ret;
+}
+
+static const struct v4l2_subdev_video_ops ov7251_video_ops = {
+	.s_stream = ov7251_s_stream,
+	.g_frame_interval = ov7251_get_frame_interval,
+	.s_frame_interval = ov7251_set_frame_interval,
+};
+
+static const struct v4l2_subdev_pad_ops ov7251_subdev_pad_ops = {
+	.init_cfg = ov7251_entity_init_cfg,
+	.enum_mbus_code = ov7251_enum_mbus_code,
+	.enum_frame_size = ov7251_enum_frame_size,
+	.enum_frame_interval = ov7251_enum_frame_ival,
+	.get_fmt = ov7251_get_format,
+	.set_fmt = ov7251_set_format,
+	.get_selection = ov7251_get_selection,
+};
+
+static const struct v4l2_subdev_ops ov7251_subdev_ops = {
+	.video = &ov7251_video_ops,
+	.pad = &ov7251_subdev_pad_ops,
+};
+
+static int ov7251_check_hwcfg(struct ov7251 *ov7251)
+{
+	struct fwnode_handle *fwnode = dev_fwnode(ov7251->dev);
+	struct v4l2_fwnode_endpoint bus_cfg = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY,
+	};
+	struct fwnode_handle *endpoint;
+	unsigned int i, j;
+	int ret;
+
+	endpoint = fwnode_graph_get_next_endpoint(fwnode, NULL);
+	if (!endpoint)
+		return -EPROBE_DEFER; /* could be provided by cio2-bridge */
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(endpoint, &bus_cfg);
+	fwnode_handle_put(endpoint);
+	if (ret)
+		return dev_err_probe(ov7251->dev, ret,
+				     "parsing endpoint node failed\n");
+
+	if (!bus_cfg.nr_of_link_frequencies) {
+		ret = dev_err_probe(ov7251->dev, -EINVAL,
+				    "no link frequencies defined\n");
+		goto out_free_bus_cfg;
+	}
+
+	for (i = 0; i < bus_cfg.nr_of_link_frequencies; i++) {
+		for (j = 0; j < ARRAY_SIZE(link_freq); j++)
+			if (bus_cfg.link_frequencies[i] == link_freq[j])
+				break;
+
+		if (j < ARRAY_SIZE(link_freq))
+			break;
+	}
+
+	if (i == bus_cfg.nr_of_link_frequencies) {
+		ret = dev_err_probe(ov7251->dev, -EINVAL,
+				    "no supported link freq found\n");
+		goto out_free_bus_cfg;
+	}
+
+	ov7251->link_freq_idx = i;
+
+out_free_bus_cfg:
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+
+	return ret;
+}
+
+static int ov7251_detect_chip(struct ov7251 *ov7251)
+{
+	u8 chip_id_high, chip_id_low, chip_rev;
+	int ret;
+
+	ret = ov7251_read_reg(ov7251, OV7251_CHIP_ID_HIGH, &chip_id_high);
+	if (ret < 0 || chip_id_high != OV7251_CHIP_ID_HIGH_BYTE)
+		return dev_err_probe(ov7251->dev, -ENODEV,
+				     "could not read ID high\n");
+
+	ret = ov7251_read_reg(ov7251, OV7251_CHIP_ID_LOW, &chip_id_low);
+	if (ret < 0 || chip_id_low != OV7251_CHIP_ID_LOW_BYTE)
+		return dev_err_probe(ov7251->dev, -ENODEV,
+				     "could not read ID low\n");
+
+	ret = ov7251_read_reg(ov7251, OV7251_SC_GP_IO_IN1, &chip_rev);
+	if (ret < 0)
+		return dev_err_probe(ov7251->dev, -ENODEV,
+				     "could not read revision\n");
+	chip_rev >>= 4;
+
+	dev_info(ov7251->dev,
+		 "OV7251 revision %x (%s) detected at address 0x%02x\n",
+		 chip_rev,
+		 chip_rev == 0x4 ? "1A / 1B" :
+		 chip_rev == 0x5 ? "1C / 1D" :
+		 chip_rev == 0x6 ? "1E" :
+		 chip_rev == 0x7 ? "1F" : "unknown",
+		 ov7251->i2c_client->addr);
+
+	return 0;
+}
+
+static int ov7251_init_ctrls(struct ov7251 *ov7251)
+{
+	int vblank_max, vblank_def;
+	s64 pixel_rate;
+	int hblank;
+
+	v4l2_ctrl_handler_init(&ov7251->ctrls, 7);
+	ov7251->ctrls.lock = &ov7251->lock;
+
+	v4l2_ctrl_new_std(&ov7251->ctrls, &ov7251_ctrl_ops,
+			  V4L2_CID_HFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&ov7251->ctrls, &ov7251_ctrl_ops,
+			  V4L2_CID_VFLIP, 0, 1, 1, 0);
+	ov7251->exposure = v4l2_ctrl_new_std(&ov7251->ctrls, &ov7251_ctrl_ops,
+					     V4L2_CID_EXPOSURE, 1, 32, 1, 32);
+	ov7251->gain = v4l2_ctrl_new_std(&ov7251->ctrls, &ov7251_ctrl_ops,
+					 V4L2_CID_GAIN, 16, 1023, 1, 16);
+	v4l2_ctrl_new_std_menu_items(&ov7251->ctrls, &ov7251_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(ov7251_test_pattern_menu) - 1,
+				     0, 0, ov7251_test_pattern_menu);
+
+	pixel_rate = pixel_rates[ov7251->link_freq_idx];
+	ov7251->pixel_clock = v4l2_ctrl_new_std(&ov7251->ctrls,
+						&ov7251_ctrl_ops,
+						V4L2_CID_PIXEL_RATE,
+						pixel_rate, INT_MAX,
+						pixel_rate, pixel_rate);
+	ov7251->link_freq = v4l2_ctrl_new_int_menu(&ov7251->ctrls,
+						   &ov7251_ctrl_ops,
+						   V4L2_CID_LINK_FREQ,
+						   ARRAY_SIZE(link_freq) - 1,
+						   ov7251->link_freq_idx,
+						   link_freq);
+	if (ov7251->link_freq)
+		ov7251->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+	if (ov7251->pixel_clock)
+		ov7251->pixel_clock->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	hblank = OV7251_FIXED_PPL - ov7251->current_mode->width;
+	ov7251->hblank = v4l2_ctrl_new_std(&ov7251->ctrls, &ov7251_ctrl_ops,
+					   V4L2_CID_HBLANK, hblank, hblank, 1,
+					   hblank);
+	if (ov7251->hblank)
+		ov7251->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	vblank_max = OV7251_TIMING_MAX_VTS - ov7251->current_mode->height;
+	vblank_def = ov7251->current_mode->vts - ov7251->current_mode->height;
+	ov7251->vblank = v4l2_ctrl_new_std(&ov7251->ctrls, &ov7251_ctrl_ops,
+					   V4L2_CID_VBLANK,
+					   OV7251_TIMING_MIN_VTS, vblank_max, 1,
+					   vblank_def);
+
+	ov7251->sd.ctrl_handler = &ov7251->ctrls;
+
+	if (ov7251->ctrls.error) {
+		v4l2_ctrl_handler_free(&ov7251->ctrls);
+		return ov7251->ctrls.error;
+	}
+
+	return 0;
+}
+
+static int ov7251_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct ov7251 *ov7251;
+	unsigned int rate = 0, clk_rate = 0;
+	int ret;
+	int i;
+
+	ov7251 = devm_kzalloc(dev, sizeof(struct ov7251), GFP_KERNEL);
+	if (!ov7251)
+		return -ENOMEM;
+
+	ov7251->i2c_client = client;
+	ov7251->dev = dev;
+
+	ret = ov7251_check_hwcfg(ov7251);
+	if (ret)
+		return ret;
+
+	/* get system clock (xclk) */
+	ov7251->xclk = devm_clk_get_optional(dev, NULL);
+	if (IS_ERR(ov7251->xclk))
+		return dev_err_probe(dev, PTR_ERR(ov7251->xclk),
+				     "could not get xclk");
+
+	/*
+	 * We could have either a 24MHz or 19.2MHz clock rate from either DT or
+	 * ACPI. We also need to support the IPU3 case which will have both an
+	 * external clock AND a clock-frequency property.
+	 */
+	ret = fwnode_property_read_u32(dev_fwnode(dev), "clock-frequency",
+				       &rate);
+	if (ret && !ov7251->xclk)
+		return dev_err_probe(dev, ret, "invalid clock config\n");
+
+	clk_rate = clk_get_rate(ov7251->xclk);
+	ov7251->xclk_freq = clk_rate ? clk_rate : rate;
+
+	if (ov7251->xclk_freq == 0)
+		return dev_err_probe(dev, -EINVAL, "invalid clock frequency\n");
+
+	if (!ret && ov7251->xclk) {
+		ret = clk_set_rate(ov7251->xclk, rate);
+		if (ret)
+			return dev_err_probe(dev, ret,
+					     "failed to set clock rate\n");
+	}
+
+	for (i = 0; i < ARRAY_SIZE(supported_xclk_rates); i++)
+		if (ov7251->xclk_freq == supported_xclk_rates[i])
+			break;
+
+	if (i == ARRAY_SIZE(supported_xclk_rates))
+		return dev_err_probe(dev, -EINVAL,
+				     "clock rate %u Hz is unsupported\n",
+				     ov7251->xclk_freq);
+
+	ov7251->pll_cfgs = ov7251_pll_cfgs[i];
+
+	ov7251->io_regulator = devm_regulator_get(dev, "vdddo");
+	if (IS_ERR(ov7251->io_regulator)) {
+		dev_err(dev, "cannot get io regulator\n");
+		return PTR_ERR(ov7251->io_regulator);
+	}
+
+	ov7251->core_regulator = devm_regulator_get(dev, "vddd");
+	if (IS_ERR(ov7251->core_regulator)) {
+		dev_err(dev, "cannot get core regulator\n");
+		return PTR_ERR(ov7251->core_regulator);
+	}
+
+	ov7251->analog_regulator = devm_regulator_get(dev, "vdda");
+	if (IS_ERR(ov7251->analog_regulator)) {
+		dev_err(dev, "cannot get analog regulator\n");
+		return PTR_ERR(ov7251->analog_regulator);
+	}
+
+	ov7251->enable_gpio = devm_gpiod_get(dev, "enable", GPIOD_OUT_HIGH);
+	if (IS_ERR(ov7251->enable_gpio)) {
+		dev_err(dev, "cannot get enable gpio\n");
+		return PTR_ERR(ov7251->enable_gpio);
+	}
+
+	mutex_init(&ov7251->lock);
+
+	ov7251->current_mode = &ov7251_mode_info_data[0];
+	ret = ov7251_init_ctrls(ov7251);
+	if (ret) {
+		dev_err_probe(dev, ret, "error during v4l2 ctrl init\n");
+		goto destroy_mutex;
+	}
+
+	v4l2_i2c_subdev_init(&ov7251->sd, client, &ov7251_subdev_ops);
+	ov7251->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	ov7251->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ov7251->sd.dev = &client->dev;
+	ov7251->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+	ret = media_entity_pads_init(&ov7251->sd.entity, 1, &ov7251->pad);
+	if (ret < 0) {
+		dev_err(dev, "could not register media entity\n");
+		goto free_ctrl;
+	}
+
+	ret = ov7251_set_power_on(ov7251->dev);
+	if (ret)
+		goto free_entity;
+
+	ret = ov7251_detect_chip(ov7251);
+	if (ret)
+		goto power_down;
+
+	pm_runtime_set_active(&client->dev);
+	pm_runtime_get_noresume(&client->dev);
+	pm_runtime_enable(&client->dev);
+
+	ret = ov7251_read_reg(ov7251, OV7251_PRE_ISP_00,
+			      &ov7251->pre_isp_00);
+	if (ret < 0) {
+		dev_err(dev, "could not read test pattern value\n");
+		ret = -ENODEV;
+		goto err_pm_runtime;
+	}
+
+	ret = ov7251_read_reg(ov7251, OV7251_TIMING_FORMAT1,
+			      &ov7251->timing_format1);
+	if (ret < 0) {
+		dev_err(dev, "could not read vflip value\n");
+		ret = -ENODEV;
+		goto err_pm_runtime;
+	}
+
+	ret = ov7251_read_reg(ov7251, OV7251_TIMING_FORMAT2,
+			      &ov7251->timing_format2);
+	if (ret < 0) {
+		dev_err(dev, "could not read hflip value\n");
+		ret = -ENODEV;
+		goto err_pm_runtime;
+	}
+
+	pm_runtime_set_autosuspend_delay(&client->dev, 1000);
+	pm_runtime_use_autosuspend(&client->dev);
+	pm_runtime_put_autosuspend(&client->dev);
+
+	ret = v4l2_async_register_subdev(&ov7251->sd);
+	if (ret < 0) {
+		dev_err(dev, "could not register v4l2 device\n");
+		goto free_entity;
+	}
+
+	ov7251_entity_init_cfg(&ov7251->sd, NULL);
+
+	return 0;
+
+err_pm_runtime:
+	pm_runtime_disable(ov7251->dev);
+	pm_runtime_put_noidle(ov7251->dev);
+power_down:
+	ov7251_set_power_off(ov7251->dev);
+free_entity:
+	media_entity_cleanup(&ov7251->sd.entity);
+free_ctrl:
+	v4l2_ctrl_handler_free(&ov7251->ctrls);
+destroy_mutex:
+	mutex_destroy(&ov7251->lock);
+
+	return ret;
+}
+
+static void ov7251_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov7251 *ov7251 = to_ov7251(sd);
+
+	v4l2_async_unregister_subdev(&ov7251->sd);
+	media_entity_cleanup(&ov7251->sd.entity);
+	v4l2_ctrl_handler_free(&ov7251->ctrls);
+	mutex_destroy(&ov7251->lock);
+
+	pm_runtime_disable(ov7251->dev);
+	if (!pm_runtime_status_suspended(ov7251->dev))
+		ov7251_set_power_off(ov7251->dev);
+	pm_runtime_set_suspended(ov7251->dev);
+}
+
+static const struct dev_pm_ops ov7251_pm_ops = {
+	SET_RUNTIME_PM_OPS(ov7251_set_power_off, ov7251_set_power_on, NULL)
+};
+
+static const struct of_device_id ov7251_of_match[] = {
+	{ .compatible = "ovti,ov7251" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, ov7251_of_match);
+
+static const struct acpi_device_id ov7251_acpi_match[] = {
+	{ "INT347E" },
+	{ }
+};
+MODULE_DEVICE_TABLE(acpi, ov7251_acpi_match);
+
+static struct i2c_driver ov7251_i2c_driver = {
+	.driver = {
+		.of_match_table = ov7251_of_match,
+		.acpi_match_table = ov7251_acpi_match,
+		.name  = "ov7251",
+		.pm = &ov7251_pm_ops,
+	},
+	.probe = ov7251_probe,
+	.remove = ov7251_remove,
+};
+
+module_i2c_driver(ov7251_i2c_driver);
+
+MODULE_DESCRIPTION("Omnivision OV7251 Camera Driver");
+MODULE_AUTHOR("Todor Tomov <todor.tomov@linaro.org>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/ov7640.c b/drivers/media/i2c/ov7640.c
new file mode 100644
index 000000000..293f5f404
--- /dev/null
+++ b/drivers/media/i2c/ov7640.c
@@ -0,0 +1,93 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2005-2006 Micronas USA Inc.
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <linux/slab.h>
+
+MODULE_DESCRIPTION("OmniVision ov7640 sensor driver");
+MODULE_LICENSE("GPL v2");
+
+struct reg_val {
+	u8 reg;
+	u8 val;
+};
+
+static const struct reg_val regval_init[] = {
+	{0x12, 0x80},
+	{0x12, 0x54},
+	{0x14, 0x24},
+	{0x15, 0x01},
+	{0x28, 0x20},
+	{0x75, 0x82},
+};
+
+static int write_regs(struct i2c_client *client,
+		const struct reg_val *rv, int len)
+{
+	while (--len >= 0) {
+		if (i2c_smbus_write_byte_data(client, rv->reg, rv->val) < 0)
+			return -1;
+		rv++;
+	}
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_ops ov7640_ops;
+
+static int ov7640_probe(struct i2c_client *client)
+{
+	struct i2c_adapter *adapter = client->adapter;
+	struct v4l2_subdev *sd;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -ENODEV;
+
+	sd = devm_kzalloc(&client->dev, sizeof(*sd), GFP_KERNEL);
+	if (sd == NULL)
+		return -ENOMEM;
+	v4l2_i2c_subdev_init(sd, client, &ov7640_ops);
+
+	client->flags = I2C_CLIENT_SCCB;
+
+	v4l_info(client, "chip found @ 0x%02x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	if (write_regs(client, regval_init, ARRAY_SIZE(regval_init)) < 0) {
+		v4l_err(client, "error initializing OV7640\n");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+
+static void ov7640_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+}
+
+static const struct i2c_device_id ov7640_id[] = {
+	{ "ov7640", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ov7640_id);
+
+static struct i2c_driver ov7640_driver = {
+	.driver = {
+		.name	= "ov7640",
+	},
+	.probe = ov7640_probe,
+	.remove = ov7640_remove,
+	.id_table = ov7640_id,
+};
+module_i2c_driver(ov7640_driver);
diff --git a/drivers/media/i2c/ov7670.c b/drivers/media/i2c/ov7670.c
new file mode 100644
index 000000000..2f55491ef
--- /dev/null
+++ b/drivers/media/i2c/ov7670.c
@@ -0,0 +1,2041 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * A V4L2 driver for OmniVision OV7670 cameras.
+ *
+ * Copyright 2006 One Laptop Per Child Association, Inc.  Written
+ * by Jonathan Corbet with substantial inspiration from Mark
+ * McClelland's ovcamchip code.
+ *
+ * Copyright 2006-7 Jonathan Corbet <corbet@lwn.net>
+ */
+#include <linux/clk.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <linux/videodev2.h>
+#include <linux/gpio/consumer.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-mediabus.h>
+#include <media/v4l2-image-sizes.h>
+#include <media/i2c/ov7670.h>
+
+MODULE_AUTHOR("Jonathan Corbet <corbet@lwn.net>");
+MODULE_DESCRIPTION("A low-level driver for OmniVision ov7670 sensors");
+MODULE_LICENSE("GPL");
+
+static bool debug;
+module_param(debug, bool, 0644);
+MODULE_PARM_DESC(debug, "Debug level (0-1)");
+
+/*
+ * The 7670 sits on i2c with ID 0x42
+ */
+#define OV7670_I2C_ADDR 0x42
+
+#define PLL_FACTOR	4
+
+/* Registers */
+#define REG_GAIN	0x00	/* Gain lower 8 bits (rest in vref) */
+#define REG_BLUE	0x01	/* blue gain */
+#define REG_RED		0x02	/* red gain */
+#define REG_VREF	0x03	/* Pieces of GAIN, VSTART, VSTOP */
+#define REG_COM1	0x04	/* Control 1 */
+#define  COM1_CCIR656	  0x40  /* CCIR656 enable */
+#define REG_BAVE	0x05	/* U/B Average level */
+#define REG_GbAVE	0x06	/* Y/Gb Average level */
+#define REG_AECHH	0x07	/* AEC MS 5 bits */
+#define REG_RAVE	0x08	/* V/R Average level */
+#define REG_COM2	0x09	/* Control 2 */
+#define  COM2_SSLEEP	  0x10	/* Soft sleep mode */
+#define REG_PID		0x0a	/* Product ID MSB */
+#define REG_VER		0x0b	/* Product ID LSB */
+#define REG_COM3	0x0c	/* Control 3 */
+#define  COM3_SWAP	  0x40	  /* Byte swap */
+#define  COM3_SCALEEN	  0x08	  /* Enable scaling */
+#define  COM3_DCWEN	  0x04	  /* Enable downsamp/crop/window */
+#define REG_COM4	0x0d	/* Control 4 */
+#define REG_COM5	0x0e	/* All "reserved" */
+#define REG_COM6	0x0f	/* Control 6 */
+#define REG_AECH	0x10	/* More bits of AEC value */
+#define REG_CLKRC	0x11	/* Clocl control */
+#define   CLK_EXT	  0x40	  /* Use external clock directly */
+#define   CLK_SCALE	  0x3f	  /* Mask for internal clock scale */
+#define REG_COM7	0x12	/* Control 7 */
+#define   COM7_RESET	  0x80	  /* Register reset */
+#define   COM7_FMT_MASK	  0x38
+#define   COM7_FMT_VGA	  0x00
+#define	  COM7_FMT_CIF	  0x20	  /* CIF format */
+#define   COM7_FMT_QVGA	  0x10	  /* QVGA format */
+#define   COM7_FMT_QCIF	  0x08	  /* QCIF format */
+#define	  COM7_RGB	  0x04	  /* bits 0 and 2 - RGB format */
+#define	  COM7_YUV	  0x00	  /* YUV */
+#define	  COM7_BAYER	  0x01	  /* Bayer format */
+#define	  COM7_PBAYER	  0x05	  /* "Processed bayer" */
+#define REG_COM8	0x13	/* Control 8 */
+#define   COM8_FASTAEC	  0x80	  /* Enable fast AGC/AEC */
+#define   COM8_AECSTEP	  0x40	  /* Unlimited AEC step size */
+#define   COM8_BFILT	  0x20	  /* Band filter enable */
+#define   COM8_AGC	  0x04	  /* Auto gain enable */
+#define   COM8_AWB	  0x02	  /* White balance enable */
+#define   COM8_AEC	  0x01	  /* Auto exposure enable */
+#define REG_COM9	0x14	/* Control 9  - gain ceiling */
+#define REG_COM10	0x15	/* Control 10 */
+#define   COM10_HSYNC	  0x40	  /* HSYNC instead of HREF */
+#define   COM10_PCLK_HB	  0x20	  /* Suppress PCLK on horiz blank */
+#define   COM10_HREF_REV  0x08	  /* Reverse HREF */
+#define   COM10_VS_LEAD	  0x04	  /* VSYNC on clock leading edge */
+#define   COM10_VS_NEG	  0x02	  /* VSYNC negative */
+#define   COM10_HS_NEG	  0x01	  /* HSYNC negative */
+#define REG_HSTART	0x17	/* Horiz start high bits */
+#define REG_HSTOP	0x18	/* Horiz stop high bits */
+#define REG_VSTART	0x19	/* Vert start high bits */
+#define REG_VSTOP	0x1a	/* Vert stop high bits */
+#define REG_PSHFT	0x1b	/* Pixel delay after HREF */
+#define REG_MIDH	0x1c	/* Manuf. ID high */
+#define REG_MIDL	0x1d	/* Manuf. ID low */
+#define REG_MVFP	0x1e	/* Mirror / vflip */
+#define   MVFP_MIRROR	  0x20	  /* Mirror image */
+#define   MVFP_FLIP	  0x10	  /* Vertical flip */
+
+#define REG_AEW		0x24	/* AGC upper limit */
+#define REG_AEB		0x25	/* AGC lower limit */
+#define REG_VPT		0x26	/* AGC/AEC fast mode op region */
+#define REG_HSYST	0x30	/* HSYNC rising edge delay */
+#define REG_HSYEN	0x31	/* HSYNC falling edge delay */
+#define REG_HREF	0x32	/* HREF pieces */
+#define REG_TSLB	0x3a	/* lots of stuff */
+#define   TSLB_YLAST	  0x04	  /* UYVY or VYUY - see com13 */
+#define REG_COM11	0x3b	/* Control 11 */
+#define   COM11_NIGHT	  0x80	  /* NIght mode enable */
+#define   COM11_NMFR	  0x60	  /* Two bit NM frame rate */
+#define   COM11_HZAUTO	  0x10	  /* Auto detect 50/60 Hz */
+#define	  COM11_50HZ	  0x08	  /* Manual 50Hz select */
+#define   COM11_EXP	  0x02
+#define REG_COM12	0x3c	/* Control 12 */
+#define   COM12_HREF	  0x80	  /* HREF always */
+#define REG_COM13	0x3d	/* Control 13 */
+#define   COM13_GAMMA	  0x80	  /* Gamma enable */
+#define	  COM13_UVSAT	  0x40	  /* UV saturation auto adjustment */
+#define   COM13_UVSWAP	  0x01	  /* V before U - w/TSLB */
+#define REG_COM14	0x3e	/* Control 14 */
+#define   COM14_DCWEN	  0x10	  /* DCW/PCLK-scale enable */
+#define REG_EDGE	0x3f	/* Edge enhancement factor */
+#define REG_COM15	0x40	/* Control 15 */
+#define   COM15_R10F0	  0x00	  /* Data range 10 to F0 */
+#define	  COM15_R01FE	  0x80	  /*            01 to FE */
+#define   COM15_R00FF	  0xc0	  /*            00 to FF */
+#define   COM15_RGB565	  0x10	  /* RGB565 output */
+#define   COM15_RGB555	  0x30	  /* RGB555 output */
+#define REG_COM16	0x41	/* Control 16 */
+#define   COM16_AWBGAIN   0x08	  /* AWB gain enable */
+#define REG_COM17	0x42	/* Control 17 */
+#define   COM17_AECWIN	  0xc0	  /* AEC window - must match COM4 */
+#define   COM17_CBAR	  0x08	  /* DSP Color bar */
+
+/*
+ * This matrix defines how the colors are generated, must be
+ * tweaked to adjust hue and saturation.
+ *
+ * Order: v-red, v-green, v-blue, u-red, u-green, u-blue
+ *
+ * They are nine-bit signed quantities, with the sign bit
+ * stored in 0x58.  Sign for v-red is bit 0, and up from there.
+ */
+#define	REG_CMATRIX_BASE 0x4f
+#define   CMATRIX_LEN 6
+#define REG_CMATRIX_SIGN 0x58
+
+
+#define REG_BRIGHT	0x55	/* Brightness */
+#define REG_CONTRAS	0x56	/* Contrast control */
+
+#define REG_GFIX	0x69	/* Fix gain control */
+
+#define REG_DBLV	0x6b	/* PLL control an debugging */
+#define   DBLV_BYPASS	  0x0a	  /* Bypass PLL */
+#define   DBLV_X4	  0x4a	  /* clock x4 */
+#define   DBLV_X6	  0x8a	  /* clock x6 */
+#define   DBLV_X8	  0xca	  /* clock x8 */
+
+#define REG_SCALING_XSC	0x70	/* Test pattern and horizontal scale factor */
+#define   TEST_PATTTERN_0 0x80
+#define REG_SCALING_YSC	0x71	/* Test pattern and vertical scale factor */
+#define   TEST_PATTTERN_1 0x80
+
+#define REG_REG76	0x76	/* OV's name */
+#define   R76_BLKPCOR	  0x80	  /* Black pixel correction enable */
+#define   R76_WHTPCOR	  0x40	  /* White pixel correction enable */
+
+#define REG_RGB444	0x8c	/* RGB 444 control */
+#define   R444_ENABLE	  0x02	  /* Turn on RGB444, overrides 5x5 */
+#define   R444_RGBX	  0x01	  /* Empty nibble at end */
+
+#define REG_HAECC1	0x9f	/* Hist AEC/AGC control 1 */
+#define REG_HAECC2	0xa0	/* Hist AEC/AGC control 2 */
+
+#define REG_BD50MAX	0xa5	/* 50hz banding step limit */
+#define REG_HAECC3	0xa6	/* Hist AEC/AGC control 3 */
+#define REG_HAECC4	0xa7	/* Hist AEC/AGC control 4 */
+#define REG_HAECC5	0xa8	/* Hist AEC/AGC control 5 */
+#define REG_HAECC6	0xa9	/* Hist AEC/AGC control 6 */
+#define REG_HAECC7	0xaa	/* Hist AEC/AGC control 7 */
+#define REG_BD60MAX	0xab	/* 60hz banding step limit */
+
+enum ov7670_model {
+	MODEL_OV7670 = 0,
+	MODEL_OV7675,
+};
+
+struct ov7670_win_size {
+	int	width;
+	int	height;
+	unsigned char com7_bit;
+	int	hstart;		/* Start/stop values for the camera.  Note */
+	int	hstop;		/* that they do not always make complete */
+	int	vstart;		/* sense to humans, but evidently the sensor */
+	int	vstop;		/* will do the right thing... */
+	struct regval_list *regs; /* Regs to tweak */
+};
+
+struct ov7670_devtype {
+	/* formats supported for each model */
+	struct ov7670_win_size *win_sizes;
+	unsigned int n_win_sizes;
+	/* callbacks for frame rate control */
+	int (*set_framerate)(struct v4l2_subdev *, struct v4l2_fract *);
+	void (*get_framerate)(struct v4l2_subdev *, struct v4l2_fract *);
+};
+
+/*
+ * Information we maintain about a known sensor.
+ */
+struct ov7670_format_struct;  /* coming later */
+struct ov7670_info {
+	struct v4l2_subdev sd;
+#if defined(CONFIG_MEDIA_CONTROLLER)
+	struct media_pad pad;
+#endif
+	struct v4l2_ctrl_handler hdl;
+	struct {
+		/* gain cluster */
+		struct v4l2_ctrl *auto_gain;
+		struct v4l2_ctrl *gain;
+	};
+	struct {
+		/* exposure cluster */
+		struct v4l2_ctrl *auto_exposure;
+		struct v4l2_ctrl *exposure;
+	};
+	struct {
+		/* saturation/hue cluster */
+		struct v4l2_ctrl *saturation;
+		struct v4l2_ctrl *hue;
+	};
+	struct v4l2_mbus_framefmt format;
+	struct ov7670_format_struct *fmt;  /* Current format */
+	struct ov7670_win_size *wsize;
+	struct clk *clk;
+	int on;
+	struct gpio_desc *resetb_gpio;
+	struct gpio_desc *pwdn_gpio;
+	unsigned int mbus_config;	/* Media bus configuration flags */
+	int min_width;			/* Filter out smaller sizes */
+	int min_height;			/* Filter out smaller sizes */
+	int clock_speed;		/* External clock speed (MHz) */
+	u8 clkrc;			/* Clock divider value */
+	bool use_smbus;			/* Use smbus I/O instead of I2C */
+	bool pll_bypass;
+	bool pclk_hb_disable;
+	const struct ov7670_devtype *devtype; /* Device specifics */
+};
+
+static inline struct ov7670_info *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct ov7670_info, sd);
+}
+
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct ov7670_info, hdl)->sd;
+}
+
+
+
+/*
+ * The default register settings, as obtained from OmniVision.  There
+ * is really no making sense of most of these - lots of "reserved" values
+ * and such.
+ *
+ * These settings give VGA YUYV.
+ */
+
+struct regval_list {
+	unsigned char reg_num;
+	unsigned char value;
+};
+
+static struct regval_list ov7670_default_regs[] = {
+	{ REG_COM7, COM7_RESET },
+/*
+ * Clock scale: 3 = 15fps
+ *              2 = 20fps
+ *              1 = 30fps
+ */
+	{ REG_CLKRC, 0x1 },	/* OV: clock scale (30 fps) */
+	{ REG_TSLB,  0x04 },	/* OV */
+	{ REG_COM7, 0 },	/* VGA */
+	/*
+	 * Set the hardware window.  These values from OV don't entirely
+	 * make sense - hstop is less than hstart.  But they work...
+	 */
+	{ REG_HSTART, 0x13 },	{ REG_HSTOP, 0x01 },
+	{ REG_HREF, 0xb6 },	{ REG_VSTART, 0x02 },
+	{ REG_VSTOP, 0x7a },	{ REG_VREF, 0x0a },
+
+	{ REG_COM3, 0 },	{ REG_COM14, 0 },
+	/* Mystery scaling numbers */
+	{ REG_SCALING_XSC, 0x3a },
+	{ REG_SCALING_YSC, 0x35 },
+	{ 0x72, 0x11 },		{ 0x73, 0xf0 },
+	{ 0xa2, 0x02 },		{ REG_COM10, 0x0 },
+
+	/* Gamma curve values */
+	{ 0x7a, 0x20 },		{ 0x7b, 0x10 },
+	{ 0x7c, 0x1e },		{ 0x7d, 0x35 },
+	{ 0x7e, 0x5a },		{ 0x7f, 0x69 },
+	{ 0x80, 0x76 },		{ 0x81, 0x80 },
+	{ 0x82, 0x88 },		{ 0x83, 0x8f },
+	{ 0x84, 0x96 },		{ 0x85, 0xa3 },
+	{ 0x86, 0xaf },		{ 0x87, 0xc4 },
+	{ 0x88, 0xd7 },		{ 0x89, 0xe8 },
+
+	/* AGC and AEC parameters.  Note we start by disabling those features,
+	   then turn them only after tweaking the values. */
+	{ REG_COM8, COM8_FASTAEC | COM8_AECSTEP | COM8_BFILT },
+	{ REG_GAIN, 0 },	{ REG_AECH, 0 },
+	{ REG_COM4, 0x40 }, /* magic reserved bit */
+	{ REG_COM9, 0x18 }, /* 4x gain + magic rsvd bit */
+	{ REG_BD50MAX, 0x05 },	{ REG_BD60MAX, 0x07 },
+	{ REG_AEW, 0x95 },	{ REG_AEB, 0x33 },
+	{ REG_VPT, 0xe3 },	{ REG_HAECC1, 0x78 },
+	{ REG_HAECC2, 0x68 },	{ 0xa1, 0x03 }, /* magic */
+	{ REG_HAECC3, 0xd8 },	{ REG_HAECC4, 0xd8 },
+	{ REG_HAECC5, 0xf0 },	{ REG_HAECC6, 0x90 },
+	{ REG_HAECC7, 0x94 },
+	{ REG_COM8, COM8_FASTAEC|COM8_AECSTEP|COM8_BFILT|COM8_AGC|COM8_AEC },
+
+	/* Almost all of these are magic "reserved" values.  */
+	{ REG_COM5, 0x61 },	{ REG_COM6, 0x4b },
+	{ 0x16, 0x02 },		{ REG_MVFP, 0x07 },
+	{ 0x21, 0x02 },		{ 0x22, 0x91 },
+	{ 0x29, 0x07 },		{ 0x33, 0x0b },
+	{ 0x35, 0x0b },		{ 0x37, 0x1d },
+	{ 0x38, 0x71 },		{ 0x39, 0x2a },
+	{ REG_COM12, 0x78 },	{ 0x4d, 0x40 },
+	{ 0x4e, 0x20 },		{ REG_GFIX, 0 },
+	{ 0x6b, 0x4a },		{ 0x74, 0x10 },
+	{ 0x8d, 0x4f },		{ 0x8e, 0 },
+	{ 0x8f, 0 },		{ 0x90, 0 },
+	{ 0x91, 0 },		{ 0x96, 0 },
+	{ 0x9a, 0 },		{ 0xb0, 0x84 },
+	{ 0xb1, 0x0c },		{ 0xb2, 0x0e },
+	{ 0xb3, 0x82 },		{ 0xb8, 0x0a },
+
+	/* More reserved magic, some of which tweaks white balance */
+	{ 0x43, 0x0a },		{ 0x44, 0xf0 },
+	{ 0x45, 0x34 },		{ 0x46, 0x58 },
+	{ 0x47, 0x28 },		{ 0x48, 0x3a },
+	{ 0x59, 0x88 },		{ 0x5a, 0x88 },
+	{ 0x5b, 0x44 },		{ 0x5c, 0x67 },
+	{ 0x5d, 0x49 },		{ 0x5e, 0x0e },
+	{ 0x6c, 0x0a },		{ 0x6d, 0x55 },
+	{ 0x6e, 0x11 },		{ 0x6f, 0x9f }, /* "9e for advance AWB" */
+	{ 0x6a, 0x40 },		{ REG_BLUE, 0x40 },
+	{ REG_RED, 0x60 },
+	{ REG_COM8, COM8_FASTAEC|COM8_AECSTEP|COM8_BFILT|COM8_AGC|COM8_AEC|COM8_AWB },
+
+	/* Matrix coefficients */
+	{ 0x4f, 0x80 },		{ 0x50, 0x80 },
+	{ 0x51, 0 },		{ 0x52, 0x22 },
+	{ 0x53, 0x5e },		{ 0x54, 0x80 },
+	{ 0x58, 0x9e },
+
+	{ REG_COM16, COM16_AWBGAIN },	{ REG_EDGE, 0 },
+	{ 0x75, 0x05 },		{ 0x76, 0xe1 },
+	{ 0x4c, 0 },		{ 0x77, 0x01 },
+	{ REG_COM13, 0xc3 },	{ 0x4b, 0x09 },
+	{ 0xc9, 0x60 },		{ REG_COM16, 0x38 },
+	{ 0x56, 0x40 },
+
+	{ 0x34, 0x11 },		{ REG_COM11, COM11_EXP|COM11_HZAUTO },
+	{ 0xa4, 0x88 },		{ 0x96, 0 },
+	{ 0x97, 0x30 },		{ 0x98, 0x20 },
+	{ 0x99, 0x30 },		{ 0x9a, 0x84 },
+	{ 0x9b, 0x29 },		{ 0x9c, 0x03 },
+	{ 0x9d, 0x4c },		{ 0x9e, 0x3f },
+	{ 0x78, 0x04 },
+
+	/* Extra-weird stuff.  Some sort of multiplexor register */
+	{ 0x79, 0x01 },		{ 0xc8, 0xf0 },
+	{ 0x79, 0x0f },		{ 0xc8, 0x00 },
+	{ 0x79, 0x10 },		{ 0xc8, 0x7e },
+	{ 0x79, 0x0a },		{ 0xc8, 0x80 },
+	{ 0x79, 0x0b },		{ 0xc8, 0x01 },
+	{ 0x79, 0x0c },		{ 0xc8, 0x0f },
+	{ 0x79, 0x0d },		{ 0xc8, 0x20 },
+	{ 0x79, 0x09 },		{ 0xc8, 0x80 },
+	{ 0x79, 0x02 },		{ 0xc8, 0xc0 },
+	{ 0x79, 0x03 },		{ 0xc8, 0x40 },
+	{ 0x79, 0x05 },		{ 0xc8, 0x30 },
+	{ 0x79, 0x26 },
+
+	{ 0xff, 0xff },	/* END MARKER */
+};
+
+
+/*
+ * Here we'll try to encapsulate the changes for just the output
+ * video format.
+ *
+ * RGB656 and YUV422 come from OV; RGB444 is homebrewed.
+ *
+ * IMPORTANT RULE: the first entry must be for COM7, see ov7670_s_fmt for why.
+ */
+
+
+static struct regval_list ov7670_fmt_yuv422[] = {
+	{ REG_COM7, 0x0 },  /* Selects YUV mode */
+	{ REG_RGB444, 0 },	/* No RGB444 please */
+	{ REG_COM1, 0 },	/* CCIR601 */
+	{ REG_COM15, COM15_R00FF },
+	{ REG_COM9, 0x48 }, /* 32x gain ceiling; 0x8 is reserved bit */
+	{ 0x4f, 0x80 },		/* "matrix coefficient 1" */
+	{ 0x50, 0x80 },		/* "matrix coefficient 2" */
+	{ 0x51, 0    },		/* vb */
+	{ 0x52, 0x22 },		/* "matrix coefficient 4" */
+	{ 0x53, 0x5e },		/* "matrix coefficient 5" */
+	{ 0x54, 0x80 },		/* "matrix coefficient 6" */
+	{ REG_COM13, COM13_GAMMA|COM13_UVSAT },
+	{ 0xff, 0xff },
+};
+
+static struct regval_list ov7670_fmt_rgb565[] = {
+	{ REG_COM7, COM7_RGB },	/* Selects RGB mode */
+	{ REG_RGB444, 0 },	/* No RGB444 please */
+	{ REG_COM1, 0x0 },	/* CCIR601 */
+	{ REG_COM15, COM15_RGB565 },
+	{ REG_COM9, 0x38 },	/* 16x gain ceiling; 0x8 is reserved bit */
+	{ 0x4f, 0xb3 },		/* "matrix coefficient 1" */
+	{ 0x50, 0xb3 },		/* "matrix coefficient 2" */
+	{ 0x51, 0    },		/* vb */
+	{ 0x52, 0x3d },		/* "matrix coefficient 4" */
+	{ 0x53, 0xa7 },		/* "matrix coefficient 5" */
+	{ 0x54, 0xe4 },		/* "matrix coefficient 6" */
+	{ REG_COM13, COM13_GAMMA|COM13_UVSAT },
+	{ 0xff, 0xff },
+};
+
+static struct regval_list ov7670_fmt_rgb444[] = {
+	{ REG_COM7, COM7_RGB },	/* Selects RGB mode */
+	{ REG_RGB444, R444_ENABLE },	/* Enable xxxxrrrr ggggbbbb */
+	{ REG_COM1, 0x0 },	/* CCIR601 */
+	{ REG_COM15, COM15_R01FE|COM15_RGB565 }, /* Data range needed? */
+	{ REG_COM9, 0x38 },	/* 16x gain ceiling; 0x8 is reserved bit */
+	{ 0x4f, 0xb3 },		/* "matrix coefficient 1" */
+	{ 0x50, 0xb3 },		/* "matrix coefficient 2" */
+	{ 0x51, 0    },		/* vb */
+	{ 0x52, 0x3d },		/* "matrix coefficient 4" */
+	{ 0x53, 0xa7 },		/* "matrix coefficient 5" */
+	{ 0x54, 0xe4 },		/* "matrix coefficient 6" */
+	{ REG_COM13, COM13_GAMMA|COM13_UVSAT|0x2 },  /* Magic rsvd bit */
+	{ 0xff, 0xff },
+};
+
+static struct regval_list ov7670_fmt_raw[] = {
+	{ REG_COM7, COM7_BAYER },
+	{ REG_COM13, 0x08 }, /* No gamma, magic rsvd bit */
+	{ REG_COM16, 0x3d }, /* Edge enhancement, denoise */
+	{ REG_REG76, 0xe1 }, /* Pix correction, magic rsvd */
+	{ 0xff, 0xff },
+};
+
+
+
+/*
+ * Low-level register I/O.
+ *
+ * Note that there are two versions of these.  On the XO 1, the
+ * i2c controller only does SMBUS, so that's what we use.  The
+ * ov7670 is not really an SMBUS device, though, so the communication
+ * is not always entirely reliable.
+ */
+static int ov7670_read_smbus(struct v4l2_subdev *sd, unsigned char reg,
+		unsigned char *value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(client, reg);
+	if (ret >= 0) {
+		*value = (unsigned char)ret;
+		ret = 0;
+	}
+	return ret;
+}
+
+
+static int ov7670_write_smbus(struct v4l2_subdev *sd, unsigned char reg,
+		unsigned char value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = i2c_smbus_write_byte_data(client, reg, value);
+
+	if (reg == REG_COM7 && (value & COM7_RESET))
+		msleep(5);  /* Wait for reset to run */
+	return ret;
+}
+
+/*
+ * On most platforms, we'd rather do straight i2c I/O.
+ */
+static int ov7670_read_i2c(struct v4l2_subdev *sd, unsigned char reg,
+		unsigned char *value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u8 data = reg;
+	struct i2c_msg msg;
+	int ret;
+
+	/*
+	 * Send out the register address...
+	 */
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = 1;
+	msg.buf = &data;
+	ret = i2c_transfer(client->adapter, &msg, 1);
+	if (ret < 0) {
+		printk(KERN_ERR "Error %d on register write\n", ret);
+		return ret;
+	}
+	/*
+	 * ...then read back the result.
+	 */
+	msg.flags = I2C_M_RD;
+	ret = i2c_transfer(client->adapter, &msg, 1);
+	if (ret >= 0) {
+		*value = data;
+		ret = 0;
+	}
+	return ret;
+}
+
+
+static int ov7670_write_i2c(struct v4l2_subdev *sd, unsigned char reg,
+		unsigned char value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct i2c_msg msg;
+	unsigned char data[2] = { reg, value };
+	int ret;
+
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = 2;
+	msg.buf = data;
+	ret = i2c_transfer(client->adapter, &msg, 1);
+	if (ret > 0)
+		ret = 0;
+	if (reg == REG_COM7 && (value & COM7_RESET))
+		msleep(5);  /* Wait for reset to run */
+	return ret;
+}
+
+static int ov7670_read(struct v4l2_subdev *sd, unsigned char reg,
+		unsigned char *value)
+{
+	struct ov7670_info *info = to_state(sd);
+
+	if (info->use_smbus)
+		return ov7670_read_smbus(sd, reg, value);
+	else
+		return ov7670_read_i2c(sd, reg, value);
+}
+
+static int ov7670_write(struct v4l2_subdev *sd, unsigned char reg,
+		unsigned char value)
+{
+	struct ov7670_info *info = to_state(sd);
+
+	if (info->use_smbus)
+		return ov7670_write_smbus(sd, reg, value);
+	else
+		return ov7670_write_i2c(sd, reg, value);
+}
+
+static int ov7670_update_bits(struct v4l2_subdev *sd, unsigned char reg,
+		unsigned char mask, unsigned char value)
+{
+	unsigned char orig;
+	int ret;
+
+	ret = ov7670_read(sd, reg, &orig);
+	if (ret)
+		return ret;
+
+	return ov7670_write(sd, reg, (orig & ~mask) | (value & mask));
+}
+
+/*
+ * Write a list of register settings; ff/ff stops the process.
+ */
+static int ov7670_write_array(struct v4l2_subdev *sd, struct regval_list *vals)
+{
+	while (vals->reg_num != 0xff || vals->value != 0xff) {
+		int ret = ov7670_write(sd, vals->reg_num, vals->value);
+
+		if (ret < 0)
+			return ret;
+		vals++;
+	}
+	return 0;
+}
+
+
+/*
+ * Stuff that knows about the sensor.
+ */
+static int ov7670_reset(struct v4l2_subdev *sd, u32 val)
+{
+	ov7670_write(sd, REG_COM7, COM7_RESET);
+	msleep(1);
+	return 0;
+}
+
+
+static int ov7670_init(struct v4l2_subdev *sd, u32 val)
+{
+	return ov7670_write_array(sd, ov7670_default_regs);
+}
+
+static int ov7670_detect(struct v4l2_subdev *sd)
+{
+	unsigned char v;
+	int ret;
+
+	ret = ov7670_init(sd, 0);
+	if (ret < 0)
+		return ret;
+	ret = ov7670_read(sd, REG_MIDH, &v);
+	if (ret < 0)
+		return ret;
+	if (v != 0x7f) /* OV manuf. id. */
+		return -ENODEV;
+	ret = ov7670_read(sd, REG_MIDL, &v);
+	if (ret < 0)
+		return ret;
+	if (v != 0xa2)
+		return -ENODEV;
+	/*
+	 * OK, we know we have an OmniVision chip...but which one?
+	 */
+	ret = ov7670_read(sd, REG_PID, &v);
+	if (ret < 0)
+		return ret;
+	if (v != 0x76)  /* PID + VER = 0x76 / 0x73 */
+		return -ENODEV;
+	ret = ov7670_read(sd, REG_VER, &v);
+	if (ret < 0)
+		return ret;
+	if (v != 0x73)  /* PID + VER = 0x76 / 0x73 */
+		return -ENODEV;
+	return 0;
+}
+
+
+/*
+ * Store information about the video data format.  The color matrix
+ * is deeply tied into the format, so keep the relevant values here.
+ * The magic matrix numbers come from OmniVision.
+ */
+static struct ov7670_format_struct {
+	u32 mbus_code;
+	enum v4l2_colorspace colorspace;
+	struct regval_list *regs;
+	int cmatrix[CMATRIX_LEN];
+} ov7670_formats[] = {
+	{
+		.mbus_code	= MEDIA_BUS_FMT_YUYV8_2X8,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.regs		= ov7670_fmt_yuv422,
+		.cmatrix	= { 128, -128, 0, -34, -94, 128 },
+	},
+	{
+		.mbus_code	= MEDIA_BUS_FMT_RGB444_2X8_PADHI_LE,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.regs		= ov7670_fmt_rgb444,
+		.cmatrix	= { 179, -179, 0, -61, -176, 228 },
+	},
+	{
+		.mbus_code	= MEDIA_BUS_FMT_RGB565_2X8_LE,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.regs		= ov7670_fmt_rgb565,
+		.cmatrix	= { 179, -179, 0, -61, -176, 228 },
+	},
+	{
+		.mbus_code	= MEDIA_BUS_FMT_SBGGR8_1X8,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.regs		= ov7670_fmt_raw,
+		.cmatrix	= { 0, 0, 0, 0, 0, 0 },
+	},
+};
+#define N_OV7670_FMTS ARRAY_SIZE(ov7670_formats)
+
+
+/*
+ * Then there is the issue of window sizes.  Try to capture the info here.
+ */
+
+/*
+ * QCIF mode is done (by OV) in a very strange way - it actually looks like
+ * VGA with weird scaling options - they do *not* use the canned QCIF mode
+ * which is allegedly provided by the sensor.  So here's the weird register
+ * settings.
+ */
+static struct regval_list ov7670_qcif_regs[] = {
+	{ REG_COM3, COM3_SCALEEN|COM3_DCWEN },
+	{ REG_COM3, COM3_DCWEN },
+	{ REG_COM14, COM14_DCWEN | 0x01},
+	{ 0x73, 0xf1 },
+	{ 0xa2, 0x52 },
+	{ 0x7b, 0x1c },
+	{ 0x7c, 0x28 },
+	{ 0x7d, 0x3c },
+	{ 0x7f, 0x69 },
+	{ REG_COM9, 0x38 },
+	{ 0xa1, 0x0b },
+	{ 0x74, 0x19 },
+	{ 0x9a, 0x80 },
+	{ 0x43, 0x14 },
+	{ REG_COM13, 0xc0 },
+	{ 0xff, 0xff },
+};
+
+static struct ov7670_win_size ov7670_win_sizes[] = {
+	/* VGA */
+	{
+		.width		= VGA_WIDTH,
+		.height		= VGA_HEIGHT,
+		.com7_bit	= COM7_FMT_VGA,
+		.hstart		= 158,	/* These values from */
+		.hstop		=  14,	/* Omnivision */
+		.vstart		=  10,
+		.vstop		= 490,
+		.regs		= NULL,
+	},
+	/* CIF */
+	{
+		.width		= CIF_WIDTH,
+		.height		= CIF_HEIGHT,
+		.com7_bit	= COM7_FMT_CIF,
+		.hstart		= 170,	/* Empirically determined */
+		.hstop		=  90,
+		.vstart		=  14,
+		.vstop		= 494,
+		.regs		= NULL,
+	},
+	/* QVGA */
+	{
+		.width		= QVGA_WIDTH,
+		.height		= QVGA_HEIGHT,
+		.com7_bit	= COM7_FMT_QVGA,
+		.hstart		= 168,	/* Empirically determined */
+		.hstop		=  24,
+		.vstart		=  12,
+		.vstop		= 492,
+		.regs		= NULL,
+	},
+	/* QCIF */
+	{
+		.width		= QCIF_WIDTH,
+		.height		= QCIF_HEIGHT,
+		.com7_bit	= COM7_FMT_VGA, /* see comment above */
+		.hstart		= 456,	/* Empirically determined */
+		.hstop		=  24,
+		.vstart		=  14,
+		.vstop		= 494,
+		.regs		= ov7670_qcif_regs,
+	}
+};
+
+static struct ov7670_win_size ov7675_win_sizes[] = {
+	/*
+	 * Currently, only VGA is supported. Theoretically it could be possible
+	 * to support CIF, QVGA and QCIF too. Taking values for ov7670 as a
+	 * base and tweak them empirically could be required.
+	 */
+	{
+		.width		= VGA_WIDTH,
+		.height		= VGA_HEIGHT,
+		.com7_bit	= COM7_FMT_VGA,
+		.hstart		= 158,	/* These values from */
+		.hstop		=  14,	/* Omnivision */
+		.vstart		=  14,  /* Empirically determined */
+		.vstop		= 494,
+		.regs		= NULL,
+	}
+};
+
+static void ov7675_get_framerate(struct v4l2_subdev *sd,
+				 struct v4l2_fract *tpf)
+{
+	struct ov7670_info *info = to_state(sd);
+	u32 clkrc = info->clkrc;
+	int pll_factor;
+
+	if (info->pll_bypass)
+		pll_factor = 1;
+	else
+		pll_factor = PLL_FACTOR;
+
+	clkrc++;
+	if (info->fmt->mbus_code == MEDIA_BUS_FMT_SBGGR8_1X8)
+		clkrc = (clkrc >> 1);
+
+	tpf->numerator = 1;
+	tpf->denominator = (5 * pll_factor * info->clock_speed) /
+			(4 * clkrc);
+}
+
+static int ov7675_apply_framerate(struct v4l2_subdev *sd)
+{
+	struct ov7670_info *info = to_state(sd);
+	int ret;
+
+	ret = ov7670_write(sd, REG_CLKRC, info->clkrc);
+	if (ret < 0)
+		return ret;
+
+	return ov7670_write(sd, REG_DBLV,
+			    info->pll_bypass ? DBLV_BYPASS : DBLV_X4);
+}
+
+static int ov7675_set_framerate(struct v4l2_subdev *sd,
+				 struct v4l2_fract *tpf)
+{
+	struct ov7670_info *info = to_state(sd);
+	u32 clkrc;
+	int pll_factor;
+
+	/*
+	 * The formula is fps = 5/4*pixclk for YUV/RGB and
+	 * fps = 5/2*pixclk for RAW.
+	 *
+	 * pixclk = clock_speed / (clkrc + 1) * PLLfactor
+	 *
+	 */
+	if (tpf->numerator == 0 || tpf->denominator == 0) {
+		clkrc = 0;
+	} else {
+		pll_factor = info->pll_bypass ? 1 : PLL_FACTOR;
+		clkrc = (5 * pll_factor * info->clock_speed * tpf->numerator) /
+			(4 * tpf->denominator);
+		if (info->fmt->mbus_code == MEDIA_BUS_FMT_SBGGR8_1X8)
+			clkrc = (clkrc << 1);
+		clkrc--;
+	}
+
+	/*
+	 * The datasheet claims that clkrc = 0 will divide the input clock by 1
+	 * but we've checked with an oscilloscope that it divides by 2 instead.
+	 * So, if clkrc = 0 just bypass the divider.
+	 */
+	if (clkrc <= 0)
+		clkrc = CLK_EXT;
+	else if (clkrc > CLK_SCALE)
+		clkrc = CLK_SCALE;
+	info->clkrc = clkrc;
+
+	/* Recalculate frame rate */
+	ov7675_get_framerate(sd, tpf);
+
+	/*
+	 * If the device is not powered up by the host driver do
+	 * not apply any changes to H/W at this time. Instead
+	 * the framerate will be restored right after power-up.
+	 */
+	if (info->on)
+		return ov7675_apply_framerate(sd);
+
+	return 0;
+}
+
+static void ov7670_get_framerate_legacy(struct v4l2_subdev *sd,
+				 struct v4l2_fract *tpf)
+{
+	struct ov7670_info *info = to_state(sd);
+
+	tpf->numerator = 1;
+	tpf->denominator = info->clock_speed;
+	if ((info->clkrc & CLK_EXT) == 0 && (info->clkrc & CLK_SCALE) > 1)
+		tpf->denominator /= (info->clkrc & CLK_SCALE);
+}
+
+static int ov7670_set_framerate_legacy(struct v4l2_subdev *sd,
+					struct v4l2_fract *tpf)
+{
+	struct ov7670_info *info = to_state(sd);
+	int div;
+
+	if (tpf->numerator == 0 || tpf->denominator == 0)
+		div = 1;  /* Reset to full rate */
+	else
+		div = (tpf->numerator * info->clock_speed) / tpf->denominator;
+	if (div == 0)
+		div = 1;
+	else if (div > CLK_SCALE)
+		div = CLK_SCALE;
+	info->clkrc = (info->clkrc & 0x80) | div;
+	tpf->numerator = 1;
+	tpf->denominator = info->clock_speed / div;
+
+	/*
+	 * If the device is not powered up by the host driver do
+	 * not apply any changes to H/W at this time. Instead
+	 * the framerate will be restored right after power-up.
+	 */
+	if (info->on)
+		return ov7670_write(sd, REG_CLKRC, info->clkrc);
+
+	return 0;
+}
+
+/*
+ * Store a set of start/stop values into the camera.
+ */
+static int ov7670_set_hw(struct v4l2_subdev *sd, int hstart, int hstop,
+		int vstart, int vstop)
+{
+	int ret;
+	unsigned char v;
+	/*
+	 * Horizontal: 11 bits, top 8 live in hstart and hstop.  Bottom 3 of
+	 * hstart are in href[2:0], bottom 3 of hstop in href[5:3].  There is
+	 * a mystery "edge offset" value in the top two bits of href.
+	 */
+	ret = ov7670_write(sd, REG_HSTART, (hstart >> 3) & 0xff);
+	if (ret)
+		return ret;
+	ret = ov7670_write(sd, REG_HSTOP, (hstop >> 3) & 0xff);
+	if (ret)
+		return ret;
+	ret = ov7670_read(sd, REG_HREF, &v);
+	if (ret)
+		return ret;
+	v = (v & 0xc0) | ((hstop & 0x7) << 3) | (hstart & 0x7);
+	msleep(10);
+	ret = ov7670_write(sd, REG_HREF, v);
+	if (ret)
+		return ret;
+	/* Vertical: similar arrangement, but only 10 bits. */
+	ret = ov7670_write(sd, REG_VSTART, (vstart >> 2) & 0xff);
+	if (ret)
+		return ret;
+	ret = ov7670_write(sd, REG_VSTOP, (vstop >> 2) & 0xff);
+	if (ret)
+		return ret;
+	ret = ov7670_read(sd, REG_VREF, &v);
+	if (ret)
+		return ret;
+	v = (v & 0xf0) | ((vstop & 0x3) << 2) | (vstart & 0x3);
+	msleep(10);
+	return ov7670_write(sd, REG_VREF, v);
+}
+
+
+static int ov7670_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->pad || code->index >= N_OV7670_FMTS)
+		return -EINVAL;
+
+	code->code = ov7670_formats[code->index].mbus_code;
+	return 0;
+}
+
+static int ov7670_try_fmt_internal(struct v4l2_subdev *sd,
+		struct v4l2_mbus_framefmt *fmt,
+		struct ov7670_format_struct **ret_fmt,
+		struct ov7670_win_size **ret_wsize)
+{
+	int index, i;
+	struct ov7670_win_size *wsize;
+	struct ov7670_info *info = to_state(sd);
+	unsigned int n_win_sizes = info->devtype->n_win_sizes;
+	unsigned int win_sizes_limit = n_win_sizes;
+
+	for (index = 0; index < N_OV7670_FMTS; index++)
+		if (ov7670_formats[index].mbus_code == fmt->code)
+			break;
+	if (index >= N_OV7670_FMTS) {
+		/* default to first format */
+		index = 0;
+		fmt->code = ov7670_formats[0].mbus_code;
+	}
+	if (ret_fmt != NULL)
+		*ret_fmt = ov7670_formats + index;
+	/*
+	 * Fields: the OV devices claim to be progressive.
+	 */
+	fmt->field = V4L2_FIELD_NONE;
+
+	/*
+	 * Don't consider values that don't match min_height and min_width
+	 * constraints.
+	 */
+	if (info->min_width || info->min_height)
+		for (i = 0; i < n_win_sizes; i++) {
+			wsize = info->devtype->win_sizes + i;
+
+			if (wsize->width < info->min_width ||
+				wsize->height < info->min_height) {
+				win_sizes_limit = i;
+				break;
+			}
+		}
+	/*
+	 * Round requested image size down to the nearest
+	 * we support, but not below the smallest.
+	 */
+	for (wsize = info->devtype->win_sizes;
+	     wsize < info->devtype->win_sizes + win_sizes_limit; wsize++)
+		if (fmt->width >= wsize->width && fmt->height >= wsize->height)
+			break;
+	if (wsize >= info->devtype->win_sizes + win_sizes_limit)
+		wsize--;   /* Take the smallest one */
+	if (ret_wsize != NULL)
+		*ret_wsize = wsize;
+	/*
+	 * Note the size we'll actually handle.
+	 */
+	fmt->width = wsize->width;
+	fmt->height = wsize->height;
+	fmt->colorspace = ov7670_formats[index].colorspace;
+
+	info->format = *fmt;
+
+	return 0;
+}
+
+static int ov7670_apply_fmt(struct v4l2_subdev *sd)
+{
+	struct ov7670_info *info = to_state(sd);
+	struct ov7670_win_size *wsize = info->wsize;
+	unsigned char com7, com10 = 0;
+	int ret;
+
+	/*
+	 * COM7 is a pain in the ass, it doesn't like to be read then
+	 * quickly written afterward.  But we have everything we need
+	 * to set it absolutely here, as long as the format-specific
+	 * register sets list it first.
+	 */
+	com7 = info->fmt->regs[0].value;
+	com7 |= wsize->com7_bit;
+	ret = ov7670_write(sd, REG_COM7, com7);
+	if (ret)
+		return ret;
+
+	/*
+	 * Configure the media bus through COM10 register
+	 */
+	if (info->mbus_config & V4L2_MBUS_VSYNC_ACTIVE_LOW)
+		com10 |= COM10_VS_NEG;
+	if (info->mbus_config & V4L2_MBUS_HSYNC_ACTIVE_LOW)
+		com10 |= COM10_HREF_REV;
+	if (info->pclk_hb_disable)
+		com10 |= COM10_PCLK_HB;
+	ret = ov7670_write(sd, REG_COM10, com10);
+	if (ret)
+		return ret;
+
+	/*
+	 * Now write the rest of the array.  Also store start/stops
+	 */
+	ret = ov7670_write_array(sd, info->fmt->regs + 1);
+	if (ret)
+		return ret;
+
+	ret = ov7670_set_hw(sd, wsize->hstart, wsize->hstop, wsize->vstart,
+			    wsize->vstop);
+	if (ret)
+		return ret;
+
+	if (wsize->regs) {
+		ret = ov7670_write_array(sd, wsize->regs);
+		if (ret)
+			return ret;
+	}
+
+	/*
+	 * If we're running RGB565, we must rewrite clkrc after setting
+	 * the other parameters or the image looks poor.  If we're *not*
+	 * doing RGB565, we must not rewrite clkrc or the image looks
+	 * *really* poor.
+	 *
+	 * (Update) Now that we retain clkrc state, we should be able
+	 * to write it unconditionally, and that will make the frame
+	 * rate persistent too.
+	 */
+	ret = ov7670_write(sd, REG_CLKRC, info->clkrc);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+/*
+ * Set a format.
+ */
+static int ov7670_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_format *format)
+{
+	struct ov7670_info *info = to_state(sd);
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
+	struct v4l2_mbus_framefmt *mbus_fmt;
+#endif
+	int ret;
+
+	if (format->pad)
+		return -EINVAL;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		ret = ov7670_try_fmt_internal(sd, &format->format, NULL, NULL);
+		if (ret)
+			return ret;
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
+		mbus_fmt = v4l2_subdev_get_try_format(sd, sd_state,
+						      format->pad);
+		*mbus_fmt = format->format;
+#endif
+		return 0;
+	}
+
+	ret = ov7670_try_fmt_internal(sd, &format->format, &info->fmt, &info->wsize);
+	if (ret)
+		return ret;
+
+	/*
+	 * If the device is not powered up by the host driver do
+	 * not apply any changes to H/W at this time. Instead
+	 * the frame format will be restored right after power-up.
+	 */
+	if (info->on)
+		return ov7670_apply_fmt(sd);
+
+	return 0;
+}
+
+static int ov7670_get_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *format)
+{
+	struct ov7670_info *info = to_state(sd);
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
+	struct v4l2_mbus_framefmt *mbus_fmt;
+#endif
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
+		mbus_fmt = v4l2_subdev_get_try_format(sd, sd_state, 0);
+		format->format = *mbus_fmt;
+		return 0;
+#else
+		return -EINVAL;
+#endif
+	} else {
+		format->format = info->format;
+	}
+
+	return 0;
+}
+
+/*
+ * Implement G/S_PARM.  There is a "high quality" mode we could try
+ * to do someday; for now, we just do the frame rate tweak.
+ */
+static int ov7670_g_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *ival)
+{
+	struct ov7670_info *info = to_state(sd);
+
+
+	info->devtype->get_framerate(sd, &ival->interval);
+
+	return 0;
+}
+
+static int ov7670_s_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *ival)
+{
+	struct v4l2_fract *tpf = &ival->interval;
+	struct ov7670_info *info = to_state(sd);
+
+
+	return info->devtype->set_framerate(sd, tpf);
+}
+
+
+/*
+ * Frame intervals.  Since frame rates are controlled with the clock
+ * divider, we can only do 30/n for integer n values.  So no continuous
+ * or stepwise options.  Here we just pick a handful of logical values.
+ */
+
+static int ov7670_frame_rates[] = { 30, 15, 10, 5, 1 };
+
+static int ov7670_enum_frame_interval(struct v4l2_subdev *sd,
+				      struct v4l2_subdev_state *sd_state,
+				      struct v4l2_subdev_frame_interval_enum *fie)
+{
+	struct ov7670_info *info = to_state(sd);
+	unsigned int n_win_sizes = info->devtype->n_win_sizes;
+	int i;
+
+	if (fie->pad)
+		return -EINVAL;
+	if (fie->index >= ARRAY_SIZE(ov7670_frame_rates))
+		return -EINVAL;
+
+	/*
+	 * Check if the width/height is valid.
+	 *
+	 * If a minimum width/height was requested, filter out the capture
+	 * windows that fall outside that.
+	 */
+	for (i = 0; i < n_win_sizes; i++) {
+		struct ov7670_win_size *win = &info->devtype->win_sizes[i];
+
+		if (info->min_width && win->width < info->min_width)
+			continue;
+		if (info->min_height && win->height < info->min_height)
+			continue;
+		if (fie->width == win->width && fie->height == win->height)
+			break;
+	}
+	if (i == n_win_sizes)
+		return -EINVAL;
+	fie->interval.numerator = 1;
+	fie->interval.denominator = ov7670_frame_rates[fie->index];
+	return 0;
+}
+
+/*
+ * Frame size enumeration
+ */
+static int ov7670_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct ov7670_info *info = to_state(sd);
+	int i;
+	int num_valid = -1;
+	__u32 index = fse->index;
+	unsigned int n_win_sizes = info->devtype->n_win_sizes;
+
+	if (fse->pad)
+		return -EINVAL;
+
+	/*
+	 * If a minimum width/height was requested, filter out the capture
+	 * windows that fall outside that.
+	 */
+	for (i = 0; i < n_win_sizes; i++) {
+		struct ov7670_win_size *win = &info->devtype->win_sizes[i];
+
+		if (info->min_width && win->width < info->min_width)
+			continue;
+		if (info->min_height && win->height < info->min_height)
+			continue;
+		if (index == ++num_valid) {
+			fse->min_width = fse->max_width = win->width;
+			fse->min_height = fse->max_height = win->height;
+			return 0;
+		}
+	}
+
+	return -EINVAL;
+}
+
+/*
+ * Code for dealing with controls.
+ */
+
+static int ov7670_store_cmatrix(struct v4l2_subdev *sd,
+		int matrix[CMATRIX_LEN])
+{
+	int i, ret;
+	unsigned char signbits = 0;
+
+	/*
+	 * Weird crap seems to exist in the upper part of
+	 * the sign bits register, so let's preserve it.
+	 */
+	ret = ov7670_read(sd, REG_CMATRIX_SIGN, &signbits);
+	signbits &= 0xc0;
+
+	for (i = 0; i < CMATRIX_LEN; i++) {
+		unsigned char raw;
+
+		if (matrix[i] < 0) {
+			signbits |= (1 << i);
+			if (matrix[i] < -255)
+				raw = 0xff;
+			else
+				raw = (-1 * matrix[i]) & 0xff;
+		} else {
+			if (matrix[i] > 255)
+				raw = 0xff;
+			else
+				raw = matrix[i] & 0xff;
+		}
+		ret = ov7670_write(sd, REG_CMATRIX_BASE + i, raw);
+		if (ret)
+			return ret;
+	}
+	return ov7670_write(sd, REG_CMATRIX_SIGN, signbits);
+}
+
+
+/*
+ * Hue also requires messing with the color matrix.  It also requires
+ * trig functions, which tend not to be well supported in the kernel.
+ * So here is a simple table of sine values, 0-90 degrees, in steps
+ * of five degrees.  Values are multiplied by 1000.
+ *
+ * The following naive approximate trig functions require an argument
+ * carefully limited to -180 <= theta <= 180.
+ */
+#define SIN_STEP 5
+static const int ov7670_sin_table[] = {
+	   0,	 87,   173,   258,   342,   422,
+	 499,	573,   642,   707,   766,   819,
+	 866,	906,   939,   965,   984,   996,
+	1000
+};
+
+static int ov7670_sine(int theta)
+{
+	int chs = 1;
+	int sine;
+
+	if (theta < 0) {
+		theta = -theta;
+		chs = -1;
+	}
+	if (theta <= 90)
+		sine = ov7670_sin_table[theta/SIN_STEP];
+	else {
+		theta -= 90;
+		sine = 1000 - ov7670_sin_table[theta/SIN_STEP];
+	}
+	return sine*chs;
+}
+
+static int ov7670_cosine(int theta)
+{
+	theta = 90 - theta;
+	if (theta > 180)
+		theta -= 360;
+	else if (theta < -180)
+		theta += 360;
+	return ov7670_sine(theta);
+}
+
+
+
+
+static void ov7670_calc_cmatrix(struct ov7670_info *info,
+		int matrix[CMATRIX_LEN], int sat, int hue)
+{
+	int i;
+	/*
+	 * Apply the current saturation setting first.
+	 */
+	for (i = 0; i < CMATRIX_LEN; i++)
+		matrix[i] = (info->fmt->cmatrix[i] * sat) >> 7;
+	/*
+	 * Then, if need be, rotate the hue value.
+	 */
+	if (hue != 0) {
+		int sinth, costh, tmpmatrix[CMATRIX_LEN];
+
+		memcpy(tmpmatrix, matrix, CMATRIX_LEN*sizeof(int));
+		sinth = ov7670_sine(hue);
+		costh = ov7670_cosine(hue);
+
+		matrix[0] = (matrix[3]*sinth + matrix[0]*costh)/1000;
+		matrix[1] = (matrix[4]*sinth + matrix[1]*costh)/1000;
+		matrix[2] = (matrix[5]*sinth + matrix[2]*costh)/1000;
+		matrix[3] = (matrix[3]*costh - matrix[0]*sinth)/1000;
+		matrix[4] = (matrix[4]*costh - matrix[1]*sinth)/1000;
+		matrix[5] = (matrix[5]*costh - matrix[2]*sinth)/1000;
+	}
+}
+
+
+
+static int ov7670_s_sat_hue(struct v4l2_subdev *sd, int sat, int hue)
+{
+	struct ov7670_info *info = to_state(sd);
+	int matrix[CMATRIX_LEN];
+
+	ov7670_calc_cmatrix(info, matrix, sat, hue);
+	return ov7670_store_cmatrix(sd, matrix);
+}
+
+
+/*
+ * Some weird registers seem to store values in a sign/magnitude format!
+ */
+
+static unsigned char ov7670_abs_to_sm(unsigned char v)
+{
+	if (v > 127)
+		return v & 0x7f;
+	return (128 - v) | 0x80;
+}
+
+static int ov7670_s_brightness(struct v4l2_subdev *sd, int value)
+{
+	unsigned char com8 = 0, v;
+
+	ov7670_read(sd, REG_COM8, &com8);
+	com8 &= ~COM8_AEC;
+	ov7670_write(sd, REG_COM8, com8);
+	v = ov7670_abs_to_sm(value);
+	return ov7670_write(sd, REG_BRIGHT, v);
+}
+
+static int ov7670_s_contrast(struct v4l2_subdev *sd, int value)
+{
+	return ov7670_write(sd, REG_CONTRAS, (unsigned char) value);
+}
+
+static int ov7670_s_hflip(struct v4l2_subdev *sd, int value)
+{
+	unsigned char v = 0;
+	int ret;
+
+	ret = ov7670_read(sd, REG_MVFP, &v);
+	if (ret)
+		return ret;
+	if (value)
+		v |= MVFP_MIRROR;
+	else
+		v &= ~MVFP_MIRROR;
+	msleep(10);  /* FIXME */
+	return ov7670_write(sd, REG_MVFP, v);
+}
+
+static int ov7670_s_vflip(struct v4l2_subdev *sd, int value)
+{
+	unsigned char v = 0;
+	int ret;
+
+	ret = ov7670_read(sd, REG_MVFP, &v);
+	if (ret)
+		return ret;
+	if (value)
+		v |= MVFP_FLIP;
+	else
+		v &= ~MVFP_FLIP;
+	msleep(10);  /* FIXME */
+	return ov7670_write(sd, REG_MVFP, v);
+}
+
+/*
+ * GAIN is split between REG_GAIN and REG_VREF[7:6].  If one believes
+ * the data sheet, the VREF parts should be the most significant, but
+ * experience shows otherwise.  There seems to be little value in
+ * messing with the VREF bits, so we leave them alone.
+ */
+static int ov7670_g_gain(struct v4l2_subdev *sd, __s32 *value)
+{
+	int ret;
+	unsigned char gain;
+
+	ret = ov7670_read(sd, REG_GAIN, &gain);
+	if (ret)
+		return ret;
+	*value = gain;
+	return 0;
+}
+
+static int ov7670_s_gain(struct v4l2_subdev *sd, int value)
+{
+	int ret;
+	unsigned char com8;
+
+	ret = ov7670_write(sd, REG_GAIN, value & 0xff);
+	if (ret)
+		return ret;
+	/* Have to turn off AGC as well */
+	ret = ov7670_read(sd, REG_COM8, &com8);
+	if (ret)
+		return ret;
+	return ov7670_write(sd, REG_COM8, com8 & ~COM8_AGC);
+}
+
+/*
+ * Tweak autogain.
+ */
+static int ov7670_s_autogain(struct v4l2_subdev *sd, int value)
+{
+	int ret;
+	unsigned char com8;
+
+	ret = ov7670_read(sd, REG_COM8, &com8);
+	if (ret == 0) {
+		if (value)
+			com8 |= COM8_AGC;
+		else
+			com8 &= ~COM8_AGC;
+		ret = ov7670_write(sd, REG_COM8, com8);
+	}
+	return ret;
+}
+
+static int ov7670_s_exp(struct v4l2_subdev *sd, int value)
+{
+	int ret;
+	unsigned char com1, com8, aech, aechh;
+
+	ret = ov7670_read(sd, REG_COM1, &com1) +
+		ov7670_read(sd, REG_COM8, &com8) +
+		ov7670_read(sd, REG_AECHH, &aechh);
+	if (ret)
+		return ret;
+
+	com1 = (com1 & 0xfc) | (value & 0x03);
+	aech = (value >> 2) & 0xff;
+	aechh = (aechh & 0xc0) | ((value >> 10) & 0x3f);
+	ret = ov7670_write(sd, REG_COM1, com1) +
+		ov7670_write(sd, REG_AECH, aech) +
+		ov7670_write(sd, REG_AECHH, aechh);
+	/* Have to turn off AEC as well */
+	if (ret == 0)
+		ret = ov7670_write(sd, REG_COM8, com8 & ~COM8_AEC);
+	return ret;
+}
+
+/*
+ * Tweak autoexposure.
+ */
+static int ov7670_s_autoexp(struct v4l2_subdev *sd,
+		enum v4l2_exposure_auto_type value)
+{
+	int ret;
+	unsigned char com8;
+
+	ret = ov7670_read(sd, REG_COM8, &com8);
+	if (ret == 0) {
+		if (value == V4L2_EXPOSURE_AUTO)
+			com8 |= COM8_AEC;
+		else
+			com8 &= ~COM8_AEC;
+		ret = ov7670_write(sd, REG_COM8, com8);
+	}
+	return ret;
+}
+
+static const char * const ov7670_test_pattern_menu[] = {
+	"No test output",
+	"Shifting \"1\"",
+	"8-bar color bar",
+	"Fade to gray color bar",
+};
+
+static int ov7670_s_test_pattern(struct v4l2_subdev *sd, int value)
+{
+	int ret;
+
+	ret = ov7670_update_bits(sd, REG_SCALING_XSC, TEST_PATTTERN_0,
+				value & BIT(0) ? TEST_PATTTERN_0 : 0);
+	if (ret)
+		return ret;
+
+	return ov7670_update_bits(sd, REG_SCALING_YSC, TEST_PATTTERN_1,
+				value & BIT(1) ? TEST_PATTTERN_1 : 0);
+}
+
+static int ov7670_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+	struct ov7670_info *info = to_state(sd);
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTOGAIN:
+		return ov7670_g_gain(sd, &info->gain->val);
+	}
+	return -EINVAL;
+}
+
+static int ov7670_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+	struct ov7670_info *info = to_state(sd);
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		return ov7670_s_brightness(sd, ctrl->val);
+	case V4L2_CID_CONTRAST:
+		return ov7670_s_contrast(sd, ctrl->val);
+	case V4L2_CID_SATURATION:
+		return ov7670_s_sat_hue(sd,
+				info->saturation->val, info->hue->val);
+	case V4L2_CID_VFLIP:
+		return ov7670_s_vflip(sd, ctrl->val);
+	case V4L2_CID_HFLIP:
+		return ov7670_s_hflip(sd, ctrl->val);
+	case V4L2_CID_AUTOGAIN:
+		/* Only set manual gain if auto gain is not explicitly
+		   turned on. */
+		if (!ctrl->val) {
+			/* ov7670_s_gain turns off auto gain */
+			return ov7670_s_gain(sd, info->gain->val);
+		}
+		return ov7670_s_autogain(sd, ctrl->val);
+	case V4L2_CID_EXPOSURE_AUTO:
+		/* Only set manual exposure if auto exposure is not explicitly
+		   turned on. */
+		if (ctrl->val == V4L2_EXPOSURE_MANUAL) {
+			/* ov7670_s_exp turns off auto exposure */
+			return ov7670_s_exp(sd, info->exposure->val);
+		}
+		return ov7670_s_autoexp(sd, ctrl->val);
+	case V4L2_CID_TEST_PATTERN:
+		return ov7670_s_test_pattern(sd, ctrl->val);
+	}
+	return -EINVAL;
+}
+
+static const struct v4l2_ctrl_ops ov7670_ctrl_ops = {
+	.s_ctrl = ov7670_s_ctrl,
+	.g_volatile_ctrl = ov7670_g_volatile_ctrl,
+};
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int ov7670_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+{
+	unsigned char val = 0;
+	int ret;
+
+	ret = ov7670_read(sd, reg->reg & 0xff, &val);
+	reg->val = val;
+	reg->size = 1;
+	return ret;
+}
+
+static int ov7670_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
+{
+	ov7670_write(sd, reg->reg & 0xff, reg->val & 0xff);
+	return 0;
+}
+#endif
+
+static void ov7670_power_on(struct v4l2_subdev *sd)
+{
+	struct ov7670_info *info = to_state(sd);
+
+	if (info->on)
+		return;
+
+	clk_prepare_enable(info->clk);
+
+	if (info->pwdn_gpio)
+		gpiod_set_value(info->pwdn_gpio, 0);
+	if (info->resetb_gpio) {
+		gpiod_set_value(info->resetb_gpio, 1);
+		usleep_range(500, 1000);
+		gpiod_set_value(info->resetb_gpio, 0);
+	}
+	if (info->pwdn_gpio || info->resetb_gpio || info->clk)
+		usleep_range(3000, 5000);
+
+	info->on = true;
+}
+
+static void ov7670_power_off(struct v4l2_subdev *sd)
+{
+	struct ov7670_info *info = to_state(sd);
+
+	if (!info->on)
+		return;
+
+	clk_disable_unprepare(info->clk);
+
+	if (info->pwdn_gpio)
+		gpiod_set_value(info->pwdn_gpio, 1);
+
+	info->on = false;
+}
+
+static int ov7670_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct ov7670_info *info = to_state(sd);
+
+	if (info->on == on)
+		return 0;
+
+	if (on) {
+		ov7670_power_on(sd);
+		ov7670_init(sd, 0);
+		ov7670_apply_fmt(sd);
+		ov7675_apply_framerate(sd);
+		v4l2_ctrl_handler_setup(&info->hdl);
+	} else {
+		ov7670_power_off(sd);
+	}
+
+	return 0;
+}
+
+static void ov7670_get_default_format(struct v4l2_subdev *sd,
+				      struct v4l2_mbus_framefmt *format)
+{
+	struct ov7670_info *info = to_state(sd);
+
+	format->width = info->devtype->win_sizes[0].width;
+	format->height = info->devtype->win_sizes[0].height;
+	format->colorspace = info->fmt->colorspace;
+	format->code = info->fmt->mbus_code;
+	format->field = V4L2_FIELD_NONE;
+}
+
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
+static int ov7670_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_mbus_framefmt *format =
+				v4l2_subdev_get_try_format(sd, fh->state, 0);
+
+	ov7670_get_default_format(sd, format);
+
+	return 0;
+}
+#endif
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_core_ops ov7670_core_ops = {
+	.reset = ov7670_reset,
+	.init = ov7670_init,
+	.s_power = ov7670_s_power,
+	.log_status = v4l2_ctrl_subdev_log_status,
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = ov7670_g_register,
+	.s_register = ov7670_s_register,
+#endif
+};
+
+static const struct v4l2_subdev_video_ops ov7670_video_ops = {
+	.s_frame_interval = ov7670_s_frame_interval,
+	.g_frame_interval = ov7670_g_frame_interval,
+};
+
+static const struct v4l2_subdev_pad_ops ov7670_pad_ops = {
+	.enum_frame_interval = ov7670_enum_frame_interval,
+	.enum_frame_size = ov7670_enum_frame_size,
+	.enum_mbus_code = ov7670_enum_mbus_code,
+	.get_fmt = ov7670_get_fmt,
+	.set_fmt = ov7670_set_fmt,
+};
+
+static const struct v4l2_subdev_ops ov7670_ops = {
+	.core = &ov7670_core_ops,
+	.video = &ov7670_video_ops,
+	.pad = &ov7670_pad_ops,
+};
+
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
+static const struct v4l2_subdev_internal_ops ov7670_subdev_internal_ops = {
+	.open = ov7670_open,
+};
+#endif
+
+/* ----------------------------------------------------------------------- */
+
+static const struct ov7670_devtype ov7670_devdata[] = {
+	[MODEL_OV7670] = {
+		.win_sizes = ov7670_win_sizes,
+		.n_win_sizes = ARRAY_SIZE(ov7670_win_sizes),
+		.set_framerate = ov7670_set_framerate_legacy,
+		.get_framerate = ov7670_get_framerate_legacy,
+	},
+	[MODEL_OV7675] = {
+		.win_sizes = ov7675_win_sizes,
+		.n_win_sizes = ARRAY_SIZE(ov7675_win_sizes),
+		.set_framerate = ov7675_set_framerate,
+		.get_framerate = ov7675_get_framerate,
+	},
+};
+
+static int ov7670_init_gpio(struct i2c_client *client, struct ov7670_info *info)
+{
+	info->pwdn_gpio = devm_gpiod_get_optional(&client->dev, "powerdown",
+			GPIOD_OUT_LOW);
+	if (IS_ERR(info->pwdn_gpio)) {
+		dev_info(&client->dev, "can't get %s GPIO\n", "powerdown");
+		return PTR_ERR(info->pwdn_gpio);
+	}
+
+	info->resetb_gpio = devm_gpiod_get_optional(&client->dev, "reset",
+			GPIOD_OUT_LOW);
+	if (IS_ERR(info->resetb_gpio)) {
+		dev_info(&client->dev, "can't get %s GPIO\n", "reset");
+		return PTR_ERR(info->resetb_gpio);
+	}
+
+	usleep_range(3000, 5000);
+
+	return 0;
+}
+
+/*
+ * ov7670_parse_dt() - Parse device tree to collect mbus configuration
+ *			properties
+ */
+static int ov7670_parse_dt(struct device *dev,
+			   struct ov7670_info *info)
+{
+	struct fwnode_handle *fwnode = dev_fwnode(dev);
+	struct v4l2_fwnode_endpoint bus_cfg = { .bus_type = 0 };
+	struct fwnode_handle *ep;
+	int ret;
+
+	if (!fwnode)
+		return -EINVAL;
+
+	info->pclk_hb_disable = false;
+	if (fwnode_property_present(fwnode, "ov7670,pclk-hb-disable"))
+		info->pclk_hb_disable = true;
+
+	ep = fwnode_graph_get_next_endpoint(fwnode, NULL);
+	if (!ep)
+		return -EINVAL;
+
+	ret = v4l2_fwnode_endpoint_parse(ep, &bus_cfg);
+	fwnode_handle_put(ep);
+	if (ret)
+		return ret;
+
+	if (bus_cfg.bus_type != V4L2_MBUS_PARALLEL) {
+		dev_err(dev, "Unsupported media bus type\n");
+		return -EINVAL;
+	}
+	info->mbus_config = bus_cfg.bus.parallel.flags;
+
+	return 0;
+}
+
+static int ov7670_probe(struct i2c_client *client)
+{
+	const struct i2c_device_id *id = i2c_client_get_device_id(client);
+	struct v4l2_fract tpf;
+	struct v4l2_subdev *sd;
+	struct ov7670_info *info;
+	int ret;
+
+	info = devm_kzalloc(&client->dev, sizeof(*info), GFP_KERNEL);
+	if (info == NULL)
+		return -ENOMEM;
+	sd = &info->sd;
+	v4l2_i2c_subdev_init(sd, client, &ov7670_ops);
+
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
+	sd->internal_ops = &ov7670_subdev_internal_ops;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
+#endif
+
+	info->clock_speed = 30; /* default: a guess */
+
+	if (dev_fwnode(&client->dev)) {
+		ret = ov7670_parse_dt(&client->dev, info);
+		if (ret)
+			return ret;
+
+	} else if (client->dev.platform_data) {
+		struct ov7670_config *config = client->dev.platform_data;
+
+		/*
+		 * Must apply configuration before initializing device, because it
+		 * selects I/O method.
+		 */
+		info->min_width = config->min_width;
+		info->min_height = config->min_height;
+		info->use_smbus = config->use_smbus;
+
+		if (config->clock_speed)
+			info->clock_speed = config->clock_speed;
+
+		if (config->pll_bypass)
+			info->pll_bypass = true;
+
+		if (config->pclk_hb_disable)
+			info->pclk_hb_disable = true;
+	}
+
+	info->clk = devm_clk_get_optional(&client->dev, "xclk");
+	if (IS_ERR(info->clk))
+		return PTR_ERR(info->clk);
+
+	ret = ov7670_init_gpio(client, info);
+	if (ret)
+		return ret;
+
+	ov7670_power_on(sd);
+
+	if (info->clk) {
+		info->clock_speed = clk_get_rate(info->clk) / 1000000;
+		if (info->clock_speed < 10 || info->clock_speed > 48) {
+			ret = -EINVAL;
+			goto power_off;
+		}
+	}
+
+	/* Make sure it's an ov7670 */
+	ret = ov7670_detect(sd);
+	if (ret) {
+		v4l_dbg(1, debug, client,
+			"chip found @ 0x%x (%s) is not an ov7670 chip.\n",
+			client->addr << 1, client->adapter->name);
+		goto power_off;
+	}
+	v4l_info(client, "chip found @ 0x%02x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	info->devtype = &ov7670_devdata[id->driver_data];
+	info->fmt = &ov7670_formats[0];
+	info->wsize = &info->devtype->win_sizes[0];
+
+	ov7670_get_default_format(sd, &info->format);
+
+	info->clkrc = 0;
+
+	/* Set default frame rate to 30 fps */
+	tpf.numerator = 1;
+	tpf.denominator = 30;
+	info->devtype->set_framerate(sd, &tpf);
+
+	v4l2_ctrl_handler_init(&info->hdl, 10);
+	v4l2_ctrl_new_std(&info->hdl, &ov7670_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(&info->hdl, &ov7670_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 127, 1, 64);
+	v4l2_ctrl_new_std(&info->hdl, &ov7670_ctrl_ops,
+			V4L2_CID_VFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&info->hdl, &ov7670_ctrl_ops,
+			V4L2_CID_HFLIP, 0, 1, 1, 0);
+	info->saturation = v4l2_ctrl_new_std(&info->hdl, &ov7670_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 256, 1, 128);
+	info->hue = v4l2_ctrl_new_std(&info->hdl, &ov7670_ctrl_ops,
+			V4L2_CID_HUE, -180, 180, 5, 0);
+	info->gain = v4l2_ctrl_new_std(&info->hdl, &ov7670_ctrl_ops,
+			V4L2_CID_GAIN, 0, 255, 1, 128);
+	info->auto_gain = v4l2_ctrl_new_std(&info->hdl, &ov7670_ctrl_ops,
+			V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+	info->exposure = v4l2_ctrl_new_std(&info->hdl, &ov7670_ctrl_ops,
+			V4L2_CID_EXPOSURE, 0, 65535, 1, 500);
+	info->auto_exposure = v4l2_ctrl_new_std_menu(&info->hdl, &ov7670_ctrl_ops,
+			V4L2_CID_EXPOSURE_AUTO, V4L2_EXPOSURE_MANUAL, 0,
+			V4L2_EXPOSURE_AUTO);
+	v4l2_ctrl_new_std_menu_items(&info->hdl, &ov7670_ctrl_ops,
+			V4L2_CID_TEST_PATTERN,
+			ARRAY_SIZE(ov7670_test_pattern_menu) - 1, 0, 0,
+			ov7670_test_pattern_menu);
+	sd->ctrl_handler = &info->hdl;
+	if (info->hdl.error) {
+		ret = info->hdl.error;
+
+		goto hdl_free;
+	}
+	/*
+	 * We have checked empirically that hw allows to read back the gain
+	 * value chosen by auto gain but that's not the case for auto exposure.
+	 */
+	v4l2_ctrl_auto_cluster(2, &info->auto_gain, 0, true);
+	v4l2_ctrl_auto_cluster(2, &info->auto_exposure,
+			       V4L2_EXPOSURE_MANUAL, false);
+	v4l2_ctrl_cluster(2, &info->saturation);
+
+#if defined(CONFIG_MEDIA_CONTROLLER)
+	info->pad.flags = MEDIA_PAD_FL_SOURCE;
+	info->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret = media_entity_pads_init(&info->sd.entity, 1, &info->pad);
+	if (ret < 0)
+		goto hdl_free;
+#endif
+
+	v4l2_ctrl_handler_setup(&info->hdl);
+
+	ret = v4l2_async_register_subdev(&info->sd);
+	if (ret < 0)
+		goto entity_cleanup;
+
+	ov7670_power_off(sd);
+	return 0;
+
+entity_cleanup:
+	media_entity_cleanup(&info->sd.entity);
+hdl_free:
+	v4l2_ctrl_handler_free(&info->hdl);
+power_off:
+	ov7670_power_off(sd);
+	return ret;
+}
+
+static void ov7670_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov7670_info *info = to_state(sd);
+
+	v4l2_async_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&info->hdl);
+	media_entity_cleanup(&info->sd.entity);
+}
+
+static const struct i2c_device_id ov7670_id[] = {
+	{ "ov7670", MODEL_OV7670 },
+	{ "ov7675", MODEL_OV7675 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ov7670_id);
+
+#if IS_ENABLED(CONFIG_OF)
+static const struct of_device_id ov7670_of_match[] = {
+	{ .compatible = "ovti,ov7670", },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, ov7670_of_match);
+#endif
+
+static struct i2c_driver ov7670_driver = {
+	.driver = {
+		.name	= "ov7670",
+		.of_match_table = of_match_ptr(ov7670_of_match),
+	},
+	.probe		= ov7670_probe,
+	.remove		= ov7670_remove,
+	.id_table	= ov7670_id,
+};
+
+module_i2c_driver(ov7670_driver);
diff --git a/drivers/media/i2c/ov772x.c b/drivers/media/i2c/ov772x.c
new file mode 100644
index 000000000..386d69c8e
--- /dev/null
+++ b/drivers/media/i2c/ov772x.c
@@ -0,0 +1,1563 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * ov772x Camera Driver
+ *
+ * Copyright (C) 2017 Jacopo Mondi <jacopo+renesas@jmondi.org>
+ *
+ * Copyright (C) 2008 Renesas Solutions Corp.
+ * Kuninori Morimoto <morimoto.kuninori@renesas.com>
+ *
+ * Based on ov7670 and soc_camera_platform driver,
+ *
+ * Copyright 2006-7 Jonathan Corbet <corbet@lwn.net>
+ * Copyright (C) 2008 Magnus Damm
+ * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+#include <linux/v4l2-mediabus.h>
+#include <linux/videodev2.h>
+
+#include <media/i2c/ov772x.h>
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-image-sizes.h>
+#include <media/v4l2-subdev.h>
+
+/*
+ * register offset
+ */
+#define GAIN        0x00 /* AGC - Gain control gain setting */
+#define BLUE        0x01 /* AWB - Blue channel gain setting */
+#define RED         0x02 /* AWB - Red   channel gain setting */
+#define GREEN       0x03 /* AWB - Green channel gain setting */
+#define COM1        0x04 /* Common control 1 */
+#define BAVG        0x05 /* U/B Average Level */
+#define GAVG        0x06 /* Y/Gb Average Level */
+#define RAVG        0x07 /* V/R Average Level */
+#define AECH        0x08 /* Exposure Value - AEC MSBs */
+#define COM2        0x09 /* Common control 2 */
+#define PID         0x0A /* Product ID Number MSB */
+#define VER         0x0B /* Product ID Number LSB */
+#define COM3        0x0C /* Common control 3 */
+#define COM4        0x0D /* Common control 4 */
+#define COM5        0x0E /* Common control 5 */
+#define COM6        0x0F /* Common control 6 */
+#define AEC         0x10 /* Exposure Value */
+#define CLKRC       0x11 /* Internal clock */
+#define COM7        0x12 /* Common control 7 */
+#define COM8        0x13 /* Common control 8 */
+#define COM9        0x14 /* Common control 9 */
+#define COM10       0x15 /* Common control 10 */
+#define REG16       0x16 /* Register 16 */
+#define HSTART      0x17 /* Horizontal sensor size */
+#define HSIZE       0x18 /* Horizontal frame (HREF column) end high 8-bit */
+#define VSTART      0x19 /* Vertical frame (row) start high 8-bit */
+#define VSIZE       0x1A /* Vertical sensor size */
+#define PSHFT       0x1B /* Data format - pixel delay select */
+#define MIDH        0x1C /* Manufacturer ID byte - high */
+#define MIDL        0x1D /* Manufacturer ID byte - low  */
+#define LAEC        0x1F /* Fine AEC value */
+#define COM11       0x20 /* Common control 11 */
+#define BDBASE      0x22 /* Banding filter Minimum AEC value */
+#define DBSTEP      0x23 /* Banding filter Maximum Setp */
+#define AEW         0x24 /* AGC/AEC - Stable operating region (upper limit) */
+#define AEB         0x25 /* AGC/AEC - Stable operating region (lower limit) */
+#define VPT         0x26 /* AGC/AEC Fast mode operating region */
+#define REG28       0x28 /* Register 28 */
+#define HOUTSIZE    0x29 /* Horizontal data output size MSBs */
+#define EXHCH       0x2A /* Dummy pixel insert MSB */
+#define EXHCL       0x2B /* Dummy pixel insert LSB */
+#define VOUTSIZE    0x2C /* Vertical data output size MSBs */
+#define ADVFL       0x2D /* LSB of insert dummy lines in Vertical direction */
+#define ADVFH       0x2E /* MSG of insert dummy lines in Vertical direction */
+#define YAVE        0x2F /* Y/G Channel Average value */
+#define LUMHTH      0x30 /* Histogram AEC/AGC Luminance high level threshold */
+#define LUMLTH      0x31 /* Histogram AEC/AGC Luminance low  level threshold */
+#define HREF        0x32 /* Image start and size control */
+#define DM_LNL      0x33 /* Dummy line low  8 bits */
+#define DM_LNH      0x34 /* Dummy line high 8 bits */
+#define ADOFF_B     0x35 /* AD offset compensation value for B  channel */
+#define ADOFF_R     0x36 /* AD offset compensation value for R  channel */
+#define ADOFF_GB    0x37 /* AD offset compensation value for Gb channel */
+#define ADOFF_GR    0x38 /* AD offset compensation value for Gr channel */
+#define OFF_B       0x39 /* Analog process B  channel offset value */
+#define OFF_R       0x3A /* Analog process R  channel offset value */
+#define OFF_GB      0x3B /* Analog process Gb channel offset value */
+#define OFF_GR      0x3C /* Analog process Gr channel offset value */
+#define COM12       0x3D /* Common control 12 */
+#define COM13       0x3E /* Common control 13 */
+#define COM14       0x3F /* Common control 14 */
+#define COM15       0x40 /* Common control 15*/
+#define COM16       0x41 /* Common control 16 */
+#define TGT_B       0x42 /* BLC blue channel target value */
+#define TGT_R       0x43 /* BLC red  channel target value */
+#define TGT_GB      0x44 /* BLC Gb   channel target value */
+#define TGT_GR      0x45 /* BLC Gr   channel target value */
+/* for ov7720 */
+#define LCC0        0x46 /* Lens correction control 0 */
+#define LCC1        0x47 /* Lens correction option 1 - X coordinate */
+#define LCC2        0x48 /* Lens correction option 2 - Y coordinate */
+#define LCC3        0x49 /* Lens correction option 3 */
+#define LCC4        0x4A /* Lens correction option 4 - radius of the circular */
+#define LCC5        0x4B /* Lens correction option 5 */
+#define LCC6        0x4C /* Lens correction option 6 */
+/* for ov7725 */
+#define LC_CTR      0x46 /* Lens correction control */
+#define LC_XC       0x47 /* X coordinate of lens correction center relative */
+#define LC_YC       0x48 /* Y coordinate of lens correction center relative */
+#define LC_COEF     0x49 /* Lens correction coefficient */
+#define LC_RADI     0x4A /* Lens correction radius */
+#define LC_COEFB    0x4B /* Lens B channel compensation coefficient */
+#define LC_COEFR    0x4C /* Lens R channel compensation coefficient */
+
+#define FIXGAIN     0x4D /* Analog fix gain amplifer */
+#define AREF0       0x4E /* Sensor reference control */
+#define AREF1       0x4F /* Sensor reference current control */
+#define AREF2       0x50 /* Analog reference control */
+#define AREF3       0x51 /* ADC    reference control */
+#define AREF4       0x52 /* ADC    reference control */
+#define AREF5       0x53 /* ADC    reference control */
+#define AREF6       0x54 /* Analog reference control */
+#define AREF7       0x55 /* Analog reference control */
+#define UFIX        0x60 /* U channel fixed value output */
+#define VFIX        0x61 /* V channel fixed value output */
+#define AWBB_BLK    0x62 /* AWB option for advanced AWB */
+#define AWB_CTRL0   0x63 /* AWB control byte 0 */
+#define DSP_CTRL1   0x64 /* DSP control byte 1 */
+#define DSP_CTRL2   0x65 /* DSP control byte 2 */
+#define DSP_CTRL3   0x66 /* DSP control byte 3 */
+#define DSP_CTRL4   0x67 /* DSP control byte 4 */
+#define AWB_BIAS    0x68 /* AWB BLC level clip */
+#define AWB_CTRL1   0x69 /* AWB control  1 */
+#define AWB_CTRL2   0x6A /* AWB control  2 */
+#define AWB_CTRL3   0x6B /* AWB control  3 */
+#define AWB_CTRL4   0x6C /* AWB control  4 */
+#define AWB_CTRL5   0x6D /* AWB control  5 */
+#define AWB_CTRL6   0x6E /* AWB control  6 */
+#define AWB_CTRL7   0x6F /* AWB control  7 */
+#define AWB_CTRL8   0x70 /* AWB control  8 */
+#define AWB_CTRL9   0x71 /* AWB control  9 */
+#define AWB_CTRL10  0x72 /* AWB control 10 */
+#define AWB_CTRL11  0x73 /* AWB control 11 */
+#define AWB_CTRL12  0x74 /* AWB control 12 */
+#define AWB_CTRL13  0x75 /* AWB control 13 */
+#define AWB_CTRL14  0x76 /* AWB control 14 */
+#define AWB_CTRL15  0x77 /* AWB control 15 */
+#define AWB_CTRL16  0x78 /* AWB control 16 */
+#define AWB_CTRL17  0x79 /* AWB control 17 */
+#define AWB_CTRL18  0x7A /* AWB control 18 */
+#define AWB_CTRL19  0x7B /* AWB control 19 */
+#define AWB_CTRL20  0x7C /* AWB control 20 */
+#define AWB_CTRL21  0x7D /* AWB control 21 */
+#define GAM1        0x7E /* Gamma Curve  1st segment input end point */
+#define GAM2        0x7F /* Gamma Curve  2nd segment input end point */
+#define GAM3        0x80 /* Gamma Curve  3rd segment input end point */
+#define GAM4        0x81 /* Gamma Curve  4th segment input end point */
+#define GAM5        0x82 /* Gamma Curve  5th segment input end point */
+#define GAM6        0x83 /* Gamma Curve  6th segment input end point */
+#define GAM7        0x84 /* Gamma Curve  7th segment input end point */
+#define GAM8        0x85 /* Gamma Curve  8th segment input end point */
+#define GAM9        0x86 /* Gamma Curve  9th segment input end point */
+#define GAM10       0x87 /* Gamma Curve 10th segment input end point */
+#define GAM11       0x88 /* Gamma Curve 11th segment input end point */
+#define GAM12       0x89 /* Gamma Curve 12th segment input end point */
+#define GAM13       0x8A /* Gamma Curve 13th segment input end point */
+#define GAM14       0x8B /* Gamma Curve 14th segment input end point */
+#define GAM15       0x8C /* Gamma Curve 15th segment input end point */
+#define SLOP        0x8D /* Gamma curve highest segment slope */
+#define DNSTH       0x8E /* De-noise threshold */
+#define EDGE_STRNGT 0x8F /* Edge strength  control when manual mode */
+#define EDGE_TRSHLD 0x90 /* Edge threshold control when manual mode */
+#define DNSOFF      0x91 /* Auto De-noise threshold control */
+#define EDGE_UPPER  0x92 /* Edge strength upper limit when Auto mode */
+#define EDGE_LOWER  0x93 /* Edge strength lower limit when Auto mode */
+#define MTX1        0x94 /* Matrix coefficient 1 */
+#define MTX2        0x95 /* Matrix coefficient 2 */
+#define MTX3        0x96 /* Matrix coefficient 3 */
+#define MTX4        0x97 /* Matrix coefficient 4 */
+#define MTX5        0x98 /* Matrix coefficient 5 */
+#define MTX6        0x99 /* Matrix coefficient 6 */
+#define MTX_CTRL    0x9A /* Matrix control */
+#define BRIGHT      0x9B /* Brightness control */
+#define CNTRST      0x9C /* Contrast contrast */
+#define CNTRST_CTRL 0x9D /* Contrast contrast center */
+#define UVAD_J0     0x9E /* Auto UV adjust contrast 0 */
+#define UVAD_J1     0x9F /* Auto UV adjust contrast 1 */
+#define SCAL0       0xA0 /* Scaling control 0 */
+#define SCAL1       0xA1 /* Scaling control 1 */
+#define SCAL2       0xA2 /* Scaling control 2 */
+#define FIFODLYM    0xA3 /* FIFO manual mode delay control */
+#define FIFODLYA    0xA4 /* FIFO auto   mode delay control */
+#define SDE         0xA6 /* Special digital effect control */
+#define USAT        0xA7 /* U component saturation control */
+#define VSAT        0xA8 /* V component saturation control */
+/* for ov7720 */
+#define HUE0        0xA9 /* Hue control 0 */
+#define HUE1        0xAA /* Hue control 1 */
+/* for ov7725 */
+#define HUECOS      0xA9 /* Cosine value */
+#define HUESIN      0xAA /* Sine value */
+
+#define SIGN        0xAB /* Sign bit for Hue and contrast */
+#define DSPAUTO     0xAC /* DSP auto function ON/OFF control */
+
+/*
+ * register detail
+ */
+
+/* COM2 */
+#define SOFT_SLEEP_MODE 0x10	/* Soft sleep mode */
+				/* Output drive capability */
+#define OCAP_1x         0x00	/* 1x */
+#define OCAP_2x         0x01	/* 2x */
+#define OCAP_3x         0x02	/* 3x */
+#define OCAP_4x         0x03	/* 4x */
+
+/* COM3 */
+#define SWAP_MASK       (SWAP_RGB | SWAP_YUV | SWAP_ML)
+#define IMG_MASK        (VFLIP_IMG | HFLIP_IMG | SCOLOR_TEST)
+
+#define VFLIP_IMG       0x80	/* Vertical flip image ON/OFF selection */
+#define HFLIP_IMG       0x40	/* Horizontal mirror image ON/OFF selection */
+#define SWAP_RGB        0x20	/* Swap B/R  output sequence in RGB mode */
+#define SWAP_YUV        0x10	/* Swap Y/UV output sequence in YUV mode */
+#define SWAP_ML         0x08	/* Swap output MSB/LSB */
+				/* Tri-state option for output clock */
+#define NOTRI_CLOCK     0x04	/*   0: Tri-state    at this period */
+				/*   1: No tri-state at this period */
+				/* Tri-state option for output data */
+#define NOTRI_DATA      0x02	/*   0: Tri-state    at this period */
+				/*   1: No tri-state at this period */
+#define SCOLOR_TEST     0x01	/* Sensor color bar test pattern */
+
+/* COM4 */
+				/* PLL frequency control */
+#define PLL_BYPASS      0x00	/*  00: Bypass PLL */
+#define PLL_4x          0x40	/*  01: PLL 4x */
+#define PLL_6x          0x80	/*  10: PLL 6x */
+#define PLL_8x          0xc0	/*  11: PLL 8x */
+				/* AEC evaluate window */
+#define AEC_FULL        0x00	/*  00: Full window */
+#define AEC_1p2         0x10	/*  01: 1/2  window */
+#define AEC_1p4         0x20	/*  10: 1/4  window */
+#define AEC_2p3         0x30	/*  11: Low 2/3 window */
+#define COM4_RESERVED   0x01	/* Reserved bit */
+
+/* COM5 */
+#define AFR_ON_OFF      0x80	/* Auto frame rate control ON/OFF selection */
+#define AFR_SPPED       0x40	/* Auto frame rate control speed selection */
+				/* Auto frame rate max rate control */
+#define AFR_NO_RATE     0x00	/*     No  reduction of frame rate */
+#define AFR_1p2         0x10	/*     Max reduction to 1/2 frame rate */
+#define AFR_1p4         0x20	/*     Max reduction to 1/4 frame rate */
+#define AFR_1p8         0x30	/* Max reduction to 1/8 frame rate */
+				/* Auto frame rate active point control */
+#define AF_2x           0x00	/*     Add frame when AGC reaches  2x gain */
+#define AF_4x           0x04	/*     Add frame when AGC reaches  4x gain */
+#define AF_8x           0x08	/*     Add frame when AGC reaches  8x gain */
+#define AF_16x          0x0c	/* Add frame when AGC reaches 16x gain */
+				/* AEC max step control */
+#define AEC_NO_LIMIT    0x01	/*   0 : AEC incease step has limit */
+				/*   1 : No limit to AEC increase step */
+/* CLKRC */
+				/* Input clock divider register */
+#define CLKRC_RESERVED  0x80	/* Reserved bit */
+#define CLKRC_DIV(n)    ((n) - 1)
+
+/* COM7 */
+				/* SCCB Register Reset */
+#define SCCB_RESET      0x80	/*   0 : No change */
+				/*   1 : Resets all registers to default */
+				/* Resolution selection */
+#define SLCT_MASK       0x40	/*   Mask of VGA or QVGA */
+#define SLCT_VGA        0x00	/*   0 : VGA */
+#define SLCT_QVGA       0x40	/*   1 : QVGA */
+#define ITU656_ON_OFF   0x20	/* ITU656 protocol ON/OFF selection */
+#define SENSOR_RAW	0x10	/* Sensor RAW */
+				/* RGB output format control */
+#define FMT_MASK        0x0c	/*      Mask of color format */
+#define FMT_GBR422      0x00	/*      00 : GBR 4:2:2 */
+#define FMT_RGB565      0x04	/*      01 : RGB 565 */
+#define FMT_RGB555      0x08	/*      10 : RGB 555 */
+#define FMT_RGB444      0x0c	/* 11 : RGB 444 */
+				/* Output format control */
+#define OFMT_MASK       0x03    /*      Mask of output format */
+#define OFMT_YUV        0x00	/*      00 : YUV */
+#define OFMT_P_BRAW     0x01	/*      01 : Processed Bayer RAW */
+#define OFMT_RGB        0x02	/*      10 : RGB */
+#define OFMT_BRAW       0x03	/* 11 : Bayer RAW */
+
+/* COM8 */
+#define FAST_ALGO       0x80	/* Enable fast AGC/AEC algorithm */
+				/* AEC Setp size limit */
+#define UNLMT_STEP      0x40	/*   0 : Step size is limited */
+				/*   1 : Unlimited step size */
+#define BNDF_ON_OFF     0x20	/* Banding filter ON/OFF */
+#define AEC_BND         0x10	/* Enable AEC below banding value */
+#define AEC_ON_OFF      0x08	/* Fine AEC ON/OFF control */
+#define AGC_ON          0x04	/* AGC Enable */
+#define AWB_ON          0x02	/* AWB Enable */
+#define AEC_ON          0x01	/* AEC Enable */
+
+/* COM9 */
+#define BASE_AECAGC     0x80	/* Histogram or average based AEC/AGC */
+				/* Automatic gain ceiling - maximum AGC value */
+#define GAIN_2x         0x00	/*    000 :   2x */
+#define GAIN_4x         0x10	/*    001 :   4x */
+#define GAIN_8x         0x20	/*    010 :   8x */
+#define GAIN_16x        0x30	/*    011 :  16x */
+#define GAIN_32x        0x40	/*    100 :  32x */
+#define GAIN_64x        0x50	/* 101 :  64x */
+#define GAIN_128x       0x60	/* 110 : 128x */
+#define DROP_VSYNC      0x04	/* Drop VSYNC output of corrupt frame */
+#define DROP_HREF       0x02	/* Drop HREF  output of corrupt frame */
+
+/* COM11 */
+#define SGLF_ON_OFF     0x02	/* Single frame ON/OFF selection */
+#define SGLF_TRIG       0x01	/* Single frame transfer trigger */
+
+/* HREF */
+#define HREF_VSTART_SHIFT	6	/* VSTART LSB */
+#define HREF_HSTART_SHIFT	4	/* HSTART 2 LSBs */
+#define HREF_VSIZE_SHIFT	2	/* VSIZE LSB */
+#define HREF_HSIZE_SHIFT	0	/* HSIZE 2 LSBs */
+
+/* EXHCH */
+#define EXHCH_VSIZE_SHIFT	2	/* VOUTSIZE LSB */
+#define EXHCH_HSIZE_SHIFT	0	/* HOUTSIZE 2 LSBs */
+
+/* DSP_CTRL1 */
+#define FIFO_ON         0x80	/* FIFO enable/disable selection */
+#define UV_ON_OFF       0x40	/* UV adjust function ON/OFF selection */
+#define YUV444_2_422    0x20	/* YUV444 to 422 UV channel option selection */
+#define CLR_MTRX_ON_OFF 0x10	/* Color matrix ON/OFF selection */
+#define INTPLT_ON_OFF   0x08	/* Interpolation ON/OFF selection */
+#define GMM_ON_OFF      0x04	/* Gamma function ON/OFF selection */
+#define AUTO_BLK_ON_OFF 0x02	/* Black defect auto correction ON/OFF */
+#define AUTO_WHT_ON_OFF 0x01	/* White define auto correction ON/OFF */
+
+/* DSP_CTRL3 */
+#define UV_MASK         0x80	/* UV output sequence option */
+#define UV_ON           0x80	/*   ON */
+#define UV_OFF          0x00	/*   OFF */
+#define CBAR_MASK       0x20	/* DSP Color bar mask */
+#define CBAR_ON         0x20	/*   ON */
+#define CBAR_OFF        0x00	/*   OFF */
+
+/* DSP_CTRL4 */
+#define DSP_OFMT_YUV	0x00
+#define DSP_OFMT_RGB	0x00
+#define DSP_OFMT_RAW8	0x02
+#define DSP_OFMT_RAW10	0x03
+
+/* DSPAUTO (DSP Auto Function ON/OFF Control) */
+#define AWB_ACTRL       0x80 /* AWB auto threshold control */
+#define DENOISE_ACTRL   0x40 /* De-noise auto threshold control */
+#define EDGE_ACTRL      0x20 /* Edge enhancement auto strength control */
+#define UV_ACTRL        0x10 /* UV adjust auto slope control */
+#define SCAL0_ACTRL     0x08 /* Auto scaling factor control */
+#define SCAL1_2_ACTRL   0x04 /* Auto scaling factor control */
+
+#define OV772X_MAX_WIDTH	VGA_WIDTH
+#define OV772X_MAX_HEIGHT	VGA_HEIGHT
+
+/*
+ * ID
+ */
+#define OV7720  0x7720
+#define OV7725  0x7721
+#define VERSION(pid, ver) ((pid << 8) | (ver & 0xFF))
+
+/*
+ * PLL multipliers
+ */
+static struct {
+	unsigned int mult;
+	u8 com4;
+} ov772x_pll[] = {
+	{ 1, PLL_BYPASS, },
+	{ 4, PLL_4x, },
+	{ 6, PLL_6x, },
+	{ 8, PLL_8x, },
+};
+
+/*
+ * struct
+ */
+
+struct ov772x_color_format {
+	u32 code;
+	enum v4l2_colorspace colorspace;
+	u8 dsp3;
+	u8 dsp4;
+	u8 com3;
+	u8 com7;
+};
+
+struct ov772x_win_size {
+	char                     *name;
+	unsigned char             com7_bit;
+	unsigned int		  sizeimage;
+	struct v4l2_rect	  rect;
+};
+
+struct ov772x_priv {
+	struct v4l2_subdev                subdev;
+	struct v4l2_ctrl_handler	  hdl;
+	struct clk			 *clk;
+	struct regmap			 *regmap;
+	struct ov772x_camera_info        *info;
+	struct gpio_desc		 *pwdn_gpio;
+	struct gpio_desc		 *rstb_gpio;
+	const struct ov772x_color_format *cfmt;
+	const struct ov772x_win_size     *win;
+	struct v4l2_ctrl		 *vflip_ctrl;
+	struct v4l2_ctrl		 *hflip_ctrl;
+	unsigned int			  test_pattern;
+	/* band_filter = COM8[5] ? 256 - BDBASE : 0 */
+	struct v4l2_ctrl		 *band_filter_ctrl;
+	unsigned int			  fps;
+	/* lock to protect power_count and streaming */
+	struct mutex			  lock;
+	int				  power_count;
+	int				  streaming;
+#ifdef CONFIG_MEDIA_CONTROLLER
+	struct media_pad pad;
+#endif
+	enum v4l2_mbus_type		  bus_type;
+};
+
+/*
+ * supported color format list
+ */
+static const struct ov772x_color_format ov772x_cfmts[] = {
+	{
+		.code		= MEDIA_BUS_FMT_YUYV8_2X8,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.dsp3		= 0x0,
+		.dsp4		= DSP_OFMT_YUV,
+		.com3		= SWAP_YUV,
+		.com7		= OFMT_YUV,
+	},
+	{
+		.code		= MEDIA_BUS_FMT_YVYU8_2X8,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.dsp3		= UV_ON,
+		.dsp4		= DSP_OFMT_YUV,
+		.com3		= SWAP_YUV,
+		.com7		= OFMT_YUV,
+	},
+	{
+		.code		= MEDIA_BUS_FMT_UYVY8_2X8,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.dsp3		= 0x0,
+		.dsp4		= DSP_OFMT_YUV,
+		.com3		= 0x0,
+		.com7		= OFMT_YUV,
+	},
+	{
+		.code		= MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.dsp3		= 0x0,
+		.dsp4		= DSP_OFMT_YUV,
+		.com3		= SWAP_RGB,
+		.com7		= FMT_RGB555 | OFMT_RGB,
+	},
+	{
+		.code		= MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.dsp3		= 0x0,
+		.dsp4		= DSP_OFMT_YUV,
+		.com3		= 0x0,
+		.com7		= FMT_RGB555 | OFMT_RGB,
+	},
+	{
+		.code		= MEDIA_BUS_FMT_RGB565_2X8_LE,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.dsp3		= 0x0,
+		.dsp4		= DSP_OFMT_YUV,
+		.com3		= SWAP_RGB,
+		.com7		= FMT_RGB565 | OFMT_RGB,
+	},
+	{
+		.code		= MEDIA_BUS_FMT_RGB565_2X8_BE,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.dsp3		= 0x0,
+		.dsp4		= DSP_OFMT_YUV,
+		.com3		= 0x0,
+		.com7		= FMT_RGB565 | OFMT_RGB,
+	},
+	{
+		/* Setting DSP4 to DSP_OFMT_RAW8 still gives 10-bit output,
+		 * regardless of the COM7 value. We can thus only support 10-bit
+		 * Bayer until someone figures it out.
+		 */
+		.code		= MEDIA_BUS_FMT_SBGGR10_1X10,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.dsp3		= 0x0,
+		.dsp4		= DSP_OFMT_RAW10,
+		.com3		= 0x0,
+		.com7		= SENSOR_RAW | OFMT_BRAW,
+	},
+};
+
+/*
+ * window size list
+ */
+
+static const struct ov772x_win_size ov772x_win_sizes[] = {
+	{
+		.name		= "VGA",
+		.com7_bit	= SLCT_VGA,
+		.sizeimage	= 510 * 748,
+		.rect = {
+			.left	= 140,
+			.top	= 14,
+			.width	= VGA_WIDTH,
+			.height	= VGA_HEIGHT,
+		},
+	}, {
+		.name		= "QVGA",
+		.com7_bit	= SLCT_QVGA,
+		.sizeimage	= 278 * 576,
+		.rect = {
+			.left	= 252,
+			.top	= 6,
+			.width	= QVGA_WIDTH,
+			.height	= QVGA_HEIGHT,
+		},
+	},
+};
+
+static const char * const ov772x_test_pattern_menu[] = {
+	"Disabled",
+	"Vertical Color Bar Type 1",
+};
+
+/*
+ * frame rate settings lists
+ */
+static const unsigned int ov772x_frame_intervals[] = { 5, 10, 15, 20, 30, 60 };
+
+/*
+ * general function
+ */
+
+static struct ov772x_priv *to_ov772x(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct ov772x_priv, subdev);
+}
+
+static int ov772x_reset(struct ov772x_priv *priv)
+{
+	int ret;
+
+	ret = regmap_write(priv->regmap, COM7, SCCB_RESET);
+	if (ret < 0)
+		return ret;
+
+	usleep_range(1000, 5000);
+
+	return regmap_update_bits(priv->regmap, COM2, SOFT_SLEEP_MODE,
+				  SOFT_SLEEP_MODE);
+}
+
+/*
+ * subdev ops
+ */
+
+static int ov772x_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov772x_priv *priv = to_ov772x(sd);
+	int ret = 0;
+
+	mutex_lock(&priv->lock);
+
+	if (priv->streaming == enable)
+		goto done;
+
+	if (priv->bus_type == V4L2_MBUS_BT656) {
+		ret = regmap_update_bits(priv->regmap, COM7, ITU656_ON_OFF,
+					 enable ?
+					 ITU656_ON_OFF : ~ITU656_ON_OFF);
+		if (ret)
+			goto done;
+	}
+
+	ret = regmap_update_bits(priv->regmap, COM2, SOFT_SLEEP_MODE,
+				 enable ? 0 : SOFT_SLEEP_MODE);
+	if (ret)
+		goto done;
+
+	if (enable) {
+		dev_dbg(&client->dev, "format %d, win %s\n",
+			priv->cfmt->code, priv->win->name);
+	}
+	priv->streaming = enable;
+
+done:
+	mutex_unlock(&priv->lock);
+
+	return ret;
+}
+
+static unsigned int ov772x_select_fps(struct ov772x_priv *priv,
+				      struct v4l2_fract *tpf)
+{
+	unsigned int fps = tpf->numerator ?
+			   tpf->denominator / tpf->numerator :
+			   tpf->denominator;
+	unsigned int best_diff;
+	unsigned int diff;
+	unsigned int idx;
+	unsigned int i;
+
+	/* Approximate to the closest supported frame interval. */
+	best_diff = ~0L;
+	for (i = 0, idx = 0; i < ARRAY_SIZE(ov772x_frame_intervals); i++) {
+		diff = abs(fps - ov772x_frame_intervals[i]);
+		if (diff < best_diff) {
+			idx = i;
+			best_diff = diff;
+		}
+	}
+
+	return ov772x_frame_intervals[idx];
+}
+
+static int ov772x_set_frame_rate(struct ov772x_priv *priv,
+				 unsigned int fps,
+				 const struct ov772x_color_format *cfmt,
+				 const struct ov772x_win_size *win)
+{
+	unsigned long fin = clk_get_rate(priv->clk);
+	unsigned int best_diff;
+	unsigned int fsize;
+	unsigned int pclk;
+	unsigned int diff;
+	unsigned int i;
+	u8 clkrc = 0;
+	u8 com4 = 0;
+	int ret;
+
+	/* Use image size (with blankings) to calculate desired pixel clock. */
+	switch (cfmt->com7 & OFMT_MASK) {
+	case OFMT_BRAW:
+		fsize = win->sizeimage;
+		break;
+	case OFMT_RGB:
+	case OFMT_YUV:
+	default:
+		fsize = win->sizeimage * 2;
+		break;
+	}
+
+	pclk = fps * fsize;
+
+	/*
+	 * Pixel clock generation circuit is pretty simple:
+	 *
+	 * Fin -> [ / CLKRC_div] -> [ * PLL_mult] -> pclk
+	 *
+	 * Try to approximate the desired pixel clock testing all available
+	 * PLL multipliers (1x, 4x, 6x, 8x) and calculate corresponding
+	 * divisor with:
+	 *
+	 * div = PLL_mult * Fin / pclk
+	 *
+	 * and re-calculate the pixel clock using it:
+	 *
+	 * pclk = Fin * PLL_mult / CLKRC_div
+	 *
+	 * Choose the PLL_mult and CLKRC_div pair that gives a pixel clock
+	 * closer to the desired one.
+	 *
+	 * The desired pixel clock is calculated using a known frame size
+	 * (blanking included) and FPS.
+	 */
+	best_diff = ~0L;
+	for (i = 0; i < ARRAY_SIZE(ov772x_pll); i++) {
+		unsigned int pll_mult = ov772x_pll[i].mult;
+		unsigned int pll_out = pll_mult * fin;
+		unsigned int t_pclk;
+		unsigned int div;
+
+		if (pll_out < pclk)
+			continue;
+
+		div = DIV_ROUND_CLOSEST(pll_out, pclk);
+		t_pclk = DIV_ROUND_CLOSEST(fin * pll_mult, div);
+		diff = abs(pclk - t_pclk);
+		if (diff < best_diff) {
+			best_diff = diff;
+			clkrc = CLKRC_DIV(div);
+			com4 = ov772x_pll[i].com4;
+		}
+	}
+
+	ret = regmap_write(priv->regmap, COM4, com4 | COM4_RESERVED);
+	if (ret < 0)
+		return ret;
+
+	ret = regmap_write(priv->regmap, CLKRC, clkrc | CLKRC_RESERVED);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+static int ov772x_g_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *ival)
+{
+	struct ov772x_priv *priv = to_ov772x(sd);
+	struct v4l2_fract *tpf = &ival->interval;
+
+	tpf->numerator = 1;
+	tpf->denominator = priv->fps;
+
+	return 0;
+}
+
+static int ov772x_s_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *ival)
+{
+	struct ov772x_priv *priv = to_ov772x(sd);
+	struct v4l2_fract *tpf = &ival->interval;
+	unsigned int fps;
+	int ret = 0;
+
+	mutex_lock(&priv->lock);
+
+	if (priv->streaming) {
+		ret = -EBUSY;
+		goto error;
+	}
+
+	fps = ov772x_select_fps(priv, tpf);
+
+	/*
+	 * If the device is not powered up by the host driver do
+	 * not apply any changes to H/W at this time. Instead
+	 * the frame rate will be restored right after power-up.
+	 */
+	if (priv->power_count > 0) {
+		ret = ov772x_set_frame_rate(priv, fps, priv->cfmt, priv->win);
+		if (ret)
+			goto error;
+	}
+
+	tpf->numerator = 1;
+	tpf->denominator = fps;
+	priv->fps = fps;
+
+error:
+	mutex_unlock(&priv->lock);
+
+	return ret;
+}
+
+static int ov772x_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov772x_priv *priv = container_of(ctrl->handler,
+						struct ov772x_priv, hdl);
+	struct regmap *regmap = priv->regmap;
+	int ret = 0;
+	u8 val;
+
+	/* v4l2_ctrl_lock() locks our own mutex */
+
+	/*
+	 * If the device is not powered up by the host driver do
+	 * not apply any controls to H/W at this time. Instead
+	 * the controls will be restored right after power-up.
+	 */
+	if (priv->power_count == 0)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_VFLIP:
+		val = ctrl->val ? VFLIP_IMG : 0x00;
+		if (priv->info && (priv->info->flags & OV772X_FLAG_VFLIP))
+			val ^= VFLIP_IMG;
+		return regmap_update_bits(regmap, COM3, VFLIP_IMG, val);
+	case V4L2_CID_HFLIP:
+		val = ctrl->val ? HFLIP_IMG : 0x00;
+		if (priv->info && (priv->info->flags & OV772X_FLAG_HFLIP))
+			val ^= HFLIP_IMG;
+		return regmap_update_bits(regmap, COM3, HFLIP_IMG, val);
+	case V4L2_CID_BAND_STOP_FILTER:
+		if (!ctrl->val) {
+			/* Switch the filter off, it is on now */
+			ret = regmap_update_bits(regmap, BDBASE, 0xff, 0xff);
+			if (!ret)
+				ret = regmap_update_bits(regmap, COM8,
+							 BNDF_ON_OFF, 0);
+		} else {
+			/* Switch the filter on, set AEC low limit */
+			val = 256 - ctrl->val;
+			ret = regmap_update_bits(regmap, COM8,
+						 BNDF_ON_OFF, BNDF_ON_OFF);
+			if (!ret)
+				ret = regmap_update_bits(regmap, BDBASE,
+							 0xff, val);
+		}
+
+		return ret;
+	case V4L2_CID_TEST_PATTERN:
+		priv->test_pattern = ctrl->val;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int ov772x_g_register(struct v4l2_subdev *sd,
+			     struct v4l2_dbg_register *reg)
+{
+	struct ov772x_priv *priv = to_ov772x(sd);
+	int ret;
+	unsigned int val;
+
+	reg->size = 1;
+	if (reg->reg > 0xff)
+		return -EINVAL;
+
+	ret = regmap_read(priv->regmap, reg->reg, &val);
+	if (ret < 0)
+		return ret;
+
+	reg->val = (__u64)val;
+
+	return 0;
+}
+
+static int ov772x_s_register(struct v4l2_subdev *sd,
+			     const struct v4l2_dbg_register *reg)
+{
+	struct ov772x_priv *priv = to_ov772x(sd);
+
+	if (reg->reg > 0xff ||
+	    reg->val > 0xff)
+		return -EINVAL;
+
+	return regmap_write(priv->regmap, reg->reg, reg->val);
+}
+#endif
+
+static int ov772x_power_on(struct ov772x_priv *priv)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&priv->subdev);
+	int ret;
+
+	if (priv->clk) {
+		ret = clk_prepare_enable(priv->clk);
+		if (ret)
+			return ret;
+	}
+
+	if (priv->pwdn_gpio) {
+		gpiod_set_value(priv->pwdn_gpio, 1);
+		usleep_range(500, 1000);
+	}
+
+	/*
+	 * FIXME: The reset signal is connected to a shared GPIO on some
+	 * platforms (namely the SuperH Migo-R). Until a framework becomes
+	 * available to handle this cleanly, request the GPIO temporarily
+	 * to avoid conflicts.
+	 */
+	priv->rstb_gpio = gpiod_get_optional(&client->dev, "reset",
+					     GPIOD_OUT_LOW);
+	if (IS_ERR(priv->rstb_gpio)) {
+		dev_info(&client->dev, "Unable to get GPIO \"reset\"");
+		clk_disable_unprepare(priv->clk);
+		return PTR_ERR(priv->rstb_gpio);
+	}
+
+	if (priv->rstb_gpio) {
+		gpiod_set_value(priv->rstb_gpio, 1);
+		usleep_range(500, 1000);
+		gpiod_set_value(priv->rstb_gpio, 0);
+		usleep_range(500, 1000);
+
+		gpiod_put(priv->rstb_gpio);
+	}
+
+	return 0;
+}
+
+static int ov772x_power_off(struct ov772x_priv *priv)
+{
+	clk_disable_unprepare(priv->clk);
+
+	if (priv->pwdn_gpio) {
+		gpiod_set_value(priv->pwdn_gpio, 0);
+		usleep_range(500, 1000);
+	}
+
+	return 0;
+}
+
+static int ov772x_set_params(struct ov772x_priv *priv,
+			     const struct ov772x_color_format *cfmt,
+			     const struct ov772x_win_size *win);
+
+static int ov772x_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct ov772x_priv *priv = to_ov772x(sd);
+	int ret = 0;
+
+	mutex_lock(&priv->lock);
+
+	/* If the power count is modified from 0 to != 0 or from != 0 to 0,
+	 * update the power state.
+	 */
+	if (priv->power_count == !on) {
+		if (on) {
+			ret = ov772x_power_on(priv);
+			/*
+			 * Restore the format, the frame rate, and
+			 * the controls
+			 */
+			if (!ret)
+				ret = ov772x_set_params(priv, priv->cfmt,
+							priv->win);
+		} else {
+			ret = ov772x_power_off(priv);
+		}
+	}
+
+	if (!ret) {
+		/* Update the power count. */
+		priv->power_count += on ? 1 : -1;
+		WARN(priv->power_count < 0, "Unbalanced power count\n");
+		WARN(priv->power_count > 1, "Duplicated s_power call\n");
+	}
+
+	mutex_unlock(&priv->lock);
+
+	return ret;
+}
+
+static const struct ov772x_win_size *ov772x_select_win(u32 width, u32 height)
+{
+	const struct ov772x_win_size *win = &ov772x_win_sizes[0];
+	u32 best_diff = UINT_MAX;
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(ov772x_win_sizes); ++i) {
+		u32 diff = abs(width - ov772x_win_sizes[i].rect.width)
+			 + abs(height - ov772x_win_sizes[i].rect.height);
+		if (diff < best_diff) {
+			best_diff = diff;
+			win = &ov772x_win_sizes[i];
+		}
+	}
+
+	return win;
+}
+
+static void ov772x_select_params(const struct v4l2_mbus_framefmt *mf,
+				 const struct ov772x_color_format **cfmt,
+				 const struct ov772x_win_size **win)
+{
+	unsigned int i;
+
+	/* Select a format. */
+	*cfmt = &ov772x_cfmts[0];
+
+	for (i = 0; i < ARRAY_SIZE(ov772x_cfmts); i++) {
+		if (mf->code == ov772x_cfmts[i].code) {
+			*cfmt = &ov772x_cfmts[i];
+			break;
+		}
+	}
+
+	/* Select a window size. */
+	*win = ov772x_select_win(mf->width, mf->height);
+}
+
+static int ov772x_edgectrl(struct ov772x_priv *priv)
+{
+	struct regmap *regmap = priv->regmap;
+	int ret;
+
+	if (!priv->info)
+		return 0;
+
+	if (priv->info->edgectrl.strength & OV772X_MANUAL_EDGE_CTRL) {
+		/*
+		 * Manual Edge Control Mode.
+		 *
+		 * Edge auto strength bit is set by default.
+		 * Remove it when manual mode.
+		 */
+
+		ret = regmap_update_bits(regmap, DSPAUTO, EDGE_ACTRL, 0x00);
+		if (ret < 0)
+			return ret;
+
+		ret = regmap_update_bits(regmap, EDGE_TRSHLD,
+					 OV772X_EDGE_THRESHOLD_MASK,
+					 priv->info->edgectrl.threshold);
+		if (ret < 0)
+			return ret;
+
+		ret = regmap_update_bits(regmap, EDGE_STRNGT,
+					 OV772X_EDGE_STRENGTH_MASK,
+					 priv->info->edgectrl.strength);
+		if (ret < 0)
+			return ret;
+
+	} else if (priv->info->edgectrl.upper > priv->info->edgectrl.lower) {
+		/*
+		 * Auto Edge Control Mode.
+		 *
+		 * Set upper and lower limit.
+		 */
+		ret = regmap_update_bits(regmap, EDGE_UPPER,
+					 OV772X_EDGE_UPPER_MASK,
+					 priv->info->edgectrl.upper);
+		if (ret < 0)
+			return ret;
+
+		ret = regmap_update_bits(regmap, EDGE_LOWER,
+					 OV772X_EDGE_LOWER_MASK,
+					 priv->info->edgectrl.lower);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int ov772x_set_params(struct ov772x_priv *priv,
+			     const struct ov772x_color_format *cfmt,
+			     const struct ov772x_win_size *win)
+{
+	int ret;
+	u8  val;
+
+	/* Reset hardware. */
+	ov772x_reset(priv);
+
+	/* Edge Ctrl. */
+	ret = ov772x_edgectrl(priv);
+	if (ret < 0)
+		return ret;
+
+	/* Format and window size. */
+	ret = regmap_write(priv->regmap, HSTART, win->rect.left >> 2);
+	if (ret < 0)
+		goto ov772x_set_fmt_error;
+	ret = regmap_write(priv->regmap, HSIZE, win->rect.width >> 2);
+	if (ret < 0)
+		goto ov772x_set_fmt_error;
+	ret = regmap_write(priv->regmap, VSTART, win->rect.top >> 1);
+	if (ret < 0)
+		goto ov772x_set_fmt_error;
+	ret = regmap_write(priv->regmap, VSIZE, win->rect.height >> 1);
+	if (ret < 0)
+		goto ov772x_set_fmt_error;
+	ret = regmap_write(priv->regmap, HOUTSIZE, win->rect.width >> 2);
+	if (ret < 0)
+		goto ov772x_set_fmt_error;
+	ret = regmap_write(priv->regmap, VOUTSIZE, win->rect.height >> 1);
+	if (ret < 0)
+		goto ov772x_set_fmt_error;
+	ret = regmap_write(priv->regmap, HREF,
+			   ((win->rect.top & 1) << HREF_VSTART_SHIFT) |
+			   ((win->rect.left & 3) << HREF_HSTART_SHIFT) |
+			   ((win->rect.height & 1) << HREF_VSIZE_SHIFT) |
+			   ((win->rect.width & 3) << HREF_HSIZE_SHIFT));
+	if (ret < 0)
+		goto ov772x_set_fmt_error;
+	ret = regmap_write(priv->regmap, EXHCH,
+			   ((win->rect.height & 1) << EXHCH_VSIZE_SHIFT) |
+			   ((win->rect.width & 3) << EXHCH_HSIZE_SHIFT));
+	if (ret < 0)
+		goto ov772x_set_fmt_error;
+
+	/* Set DSP_CTRL3. */
+	val = cfmt->dsp3;
+	if (val) {
+		ret = regmap_update_bits(priv->regmap, DSP_CTRL3, UV_MASK, val);
+		if (ret < 0)
+			goto ov772x_set_fmt_error;
+	}
+
+	/* DSP_CTRL4: AEC reference point and DSP output format. */
+	if (cfmt->dsp4) {
+		ret = regmap_write(priv->regmap, DSP_CTRL4, cfmt->dsp4);
+		if (ret < 0)
+			goto ov772x_set_fmt_error;
+	}
+
+	/* Set COM3. */
+	val = cfmt->com3;
+	if (priv->info && (priv->info->flags & OV772X_FLAG_VFLIP))
+		val |= VFLIP_IMG;
+	if (priv->info && (priv->info->flags & OV772X_FLAG_HFLIP))
+		val |= HFLIP_IMG;
+	if (priv->vflip_ctrl->val)
+		val ^= VFLIP_IMG;
+	if (priv->hflip_ctrl->val)
+		val ^= HFLIP_IMG;
+	if (priv->test_pattern)
+		val |= SCOLOR_TEST;
+
+	ret = regmap_update_bits(priv->regmap, COM3, SWAP_MASK | IMG_MASK, val);
+	if (ret < 0)
+		goto ov772x_set_fmt_error;
+
+	/* COM7: Sensor resolution and output format control. */
+	ret = regmap_write(priv->regmap, COM7, win->com7_bit | cfmt->com7);
+	if (ret < 0)
+		goto ov772x_set_fmt_error;
+
+	/* COM4, CLKRC: Set pixel clock and framerate. */
+	ret = ov772x_set_frame_rate(priv, priv->fps, cfmt, win);
+	if (ret < 0)
+		goto ov772x_set_fmt_error;
+
+	/* Set COM8. */
+	if (priv->band_filter_ctrl->val) {
+		unsigned short band_filter = priv->band_filter_ctrl->val;
+
+		ret = regmap_update_bits(priv->regmap, COM8,
+					 BNDF_ON_OFF, BNDF_ON_OFF);
+		if (!ret)
+			ret = regmap_update_bits(priv->regmap, BDBASE,
+						 0xff, 256 - band_filter);
+		if (ret < 0)
+			goto ov772x_set_fmt_error;
+	}
+
+	return ret;
+
+ov772x_set_fmt_error:
+
+	ov772x_reset(priv);
+
+	return ret;
+}
+
+static int ov772x_get_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *sd_state,
+				struct v4l2_subdev_selection *sel)
+{
+	struct ov772x_priv *priv = to_ov772x(sd);
+
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
+		return -EINVAL;
+
+	sel->r.left = 0;
+	sel->r.top = 0;
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP:
+		sel->r.width = priv->win->rect.width;
+		sel->r.height = priv->win->rect.height;
+		return 0;
+	default:
+		return -EINVAL;
+	}
+}
+
+static int ov772x_get_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *mf = &format->format;
+	struct ov772x_priv *priv = to_ov772x(sd);
+
+	if (format->pad)
+		return -EINVAL;
+
+	mf->width	= priv->win->rect.width;
+	mf->height	= priv->win->rect.height;
+	mf->code	= priv->cfmt->code;
+	mf->colorspace	= priv->cfmt->colorspace;
+	mf->field	= V4L2_FIELD_NONE;
+
+	return 0;
+}
+
+static int ov772x_set_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *format)
+{
+	struct ov772x_priv *priv = to_ov772x(sd);
+	struct v4l2_mbus_framefmt *mf = &format->format;
+	const struct ov772x_color_format *cfmt;
+	const struct ov772x_win_size *win;
+	int ret = 0;
+
+	if (format->pad)
+		return -EINVAL;
+
+	ov772x_select_params(mf, &cfmt, &win);
+
+	mf->code = cfmt->code;
+	mf->width = win->rect.width;
+	mf->height = win->rect.height;
+	mf->field = V4L2_FIELD_NONE;
+	mf->colorspace = cfmt->colorspace;
+	mf->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+	mf->quantization = V4L2_QUANTIZATION_DEFAULT;
+	mf->xfer_func = V4L2_XFER_FUNC_DEFAULT;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		sd_state->pads->try_fmt = *mf;
+		return 0;
+	}
+
+	mutex_lock(&priv->lock);
+
+	if (priv->streaming) {
+		ret = -EBUSY;
+		goto error;
+	}
+
+	/*
+	 * If the device is not powered up by the host driver do
+	 * not apply any changes to H/W at this time. Instead
+	 * the format will be restored right after power-up.
+	 */
+	if (priv->power_count > 0) {
+		ret = ov772x_set_params(priv, cfmt, win);
+		if (ret < 0)
+			goto error;
+	}
+	priv->win = win;
+	priv->cfmt = cfmt;
+
+error:
+	mutex_unlock(&priv->lock);
+
+	return ret;
+}
+
+static int ov772x_video_probe(struct ov772x_priv *priv)
+{
+	struct i2c_client  *client = v4l2_get_subdevdata(&priv->subdev);
+	int		    pid, ver, midh, midl;
+	const char         *devname;
+	int		    ret;
+
+	ret = ov772x_power_on(priv);
+	if (ret < 0)
+		return ret;
+
+	/* Check and show product ID and manufacturer ID. */
+	ret = regmap_read(priv->regmap, PID, &pid);
+	if (ret < 0)
+		return ret;
+	ret = regmap_read(priv->regmap, VER, &ver);
+	if (ret < 0)
+		return ret;
+
+	switch (VERSION(pid, ver)) {
+	case OV7720:
+		devname     = "ov7720";
+		break;
+	case OV7725:
+		devname     = "ov7725";
+		break;
+	default:
+		dev_err(&client->dev,
+			"Product ID error %x:%x\n", pid, ver);
+		ret = -ENODEV;
+		goto done;
+	}
+
+	ret = regmap_read(priv->regmap, MIDH, &midh);
+	if (ret < 0)
+		return ret;
+	ret = regmap_read(priv->regmap, MIDL, &midl);
+	if (ret < 0)
+		return ret;
+
+	dev_info(&client->dev,
+		 "%s Product ID %0x:%0x Manufacturer ID %x:%x\n",
+		 devname, pid, ver, midh, midl);
+
+	ret = v4l2_ctrl_handler_setup(&priv->hdl);
+
+done:
+	ov772x_power_off(priv);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ov772x_ctrl_ops = {
+	.s_ctrl = ov772x_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops ov772x_subdev_core_ops = {
+	.log_status = v4l2_ctrl_subdev_log_status,
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register	= ov772x_g_register,
+	.s_register	= ov772x_s_register,
+#endif
+	.s_power	= ov772x_s_power,
+};
+
+static int ov772x_enum_frame_interval(struct v4l2_subdev *sd,
+				      struct v4l2_subdev_state *sd_state,
+				      struct v4l2_subdev_frame_interval_enum *fie)
+{
+	if (fie->pad || fie->index >= ARRAY_SIZE(ov772x_frame_intervals))
+		return -EINVAL;
+
+	if (fie->width != VGA_WIDTH && fie->width != QVGA_WIDTH)
+		return -EINVAL;
+	if (fie->height != VGA_HEIGHT && fie->height != QVGA_HEIGHT)
+		return -EINVAL;
+
+	fie->interval.numerator = 1;
+	fie->interval.denominator = ov772x_frame_intervals[fie->index];
+
+	return 0;
+}
+
+static int ov772x_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->pad || code->index >= ARRAY_SIZE(ov772x_cfmts))
+		return -EINVAL;
+
+	code->code = ov772x_cfmts[code->index].code;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops ov772x_subdev_video_ops = {
+	.s_stream		= ov772x_s_stream,
+	.s_frame_interval	= ov772x_s_frame_interval,
+	.g_frame_interval	= ov772x_g_frame_interval,
+};
+
+static const struct v4l2_subdev_pad_ops ov772x_subdev_pad_ops = {
+	.enum_frame_interval	= ov772x_enum_frame_interval,
+	.enum_mbus_code		= ov772x_enum_mbus_code,
+	.get_selection		= ov772x_get_selection,
+	.get_fmt		= ov772x_get_fmt,
+	.set_fmt		= ov772x_set_fmt,
+};
+
+static const struct v4l2_subdev_ops ov772x_subdev_ops = {
+	.core	= &ov772x_subdev_core_ops,
+	.video	= &ov772x_subdev_video_ops,
+	.pad	= &ov772x_subdev_pad_ops,
+};
+
+static int ov772x_parse_dt(struct i2c_client *client,
+			   struct ov772x_priv *priv)
+{
+	struct v4l2_fwnode_endpoint bus_cfg = {
+		.bus_type = V4L2_MBUS_PARALLEL
+	};
+	struct fwnode_handle *ep;
+	int ret;
+
+	ep = fwnode_graph_get_next_endpoint(dev_fwnode(&client->dev), NULL);
+	if (!ep) {
+		dev_err(&client->dev, "Endpoint node not found\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * For backward compatibility with older DTS where the
+	 * bus-type property was not mandatory, assume
+	 * V4L2_MBUS_PARALLEL as it was the only supported bus at the
+	 * time. v4l2_fwnode_endpoint_alloc_parse() will not fail if
+	 * 'bus-type' is not specified.
+	 */
+	ret = v4l2_fwnode_endpoint_alloc_parse(ep, &bus_cfg);
+	if (ret) {
+		bus_cfg = (struct v4l2_fwnode_endpoint)
+			  { .bus_type = V4L2_MBUS_BT656 };
+		ret = v4l2_fwnode_endpoint_alloc_parse(ep, &bus_cfg);
+		if (ret)
+			goto error_fwnode_put;
+	}
+
+	priv->bus_type = bus_cfg.bus_type;
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+
+error_fwnode_put:
+	fwnode_handle_put(ep);
+
+	return ret;
+}
+
+/*
+ * i2c_driver function
+ */
+
+static int ov772x_probe(struct i2c_client *client)
+{
+	struct ov772x_priv	*priv;
+	int			ret;
+	static const struct regmap_config ov772x_regmap_config = {
+		.reg_bits = 8,
+		.val_bits = 8,
+		.max_register = DSPAUTO,
+	};
+
+	if (!client->dev.of_node && !client->dev.platform_data) {
+		dev_err(&client->dev,
+			"Missing ov772x platform data for non-DT device\n");
+		return -EINVAL;
+	}
+
+	priv = devm_kzalloc(&client->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->regmap = devm_regmap_init_sccb(client, &ov772x_regmap_config);
+	if (IS_ERR(priv->regmap)) {
+		dev_err(&client->dev, "Failed to allocate register map\n");
+		return PTR_ERR(priv->regmap);
+	}
+
+	priv->info = client->dev.platform_data;
+	mutex_init(&priv->lock);
+
+	v4l2_i2c_subdev_init(&priv->subdev, client, &ov772x_subdev_ops);
+	priv->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+			      V4L2_SUBDEV_FL_HAS_EVENTS;
+	v4l2_ctrl_handler_init(&priv->hdl, 3);
+	/* Use our mutex for the controls */
+	priv->hdl.lock = &priv->lock;
+	priv->vflip_ctrl = v4l2_ctrl_new_std(&priv->hdl, &ov772x_ctrl_ops,
+					     V4L2_CID_VFLIP, 0, 1, 1, 0);
+	priv->hflip_ctrl = v4l2_ctrl_new_std(&priv->hdl, &ov772x_ctrl_ops,
+					     V4L2_CID_HFLIP, 0, 1, 1, 0);
+	priv->band_filter_ctrl = v4l2_ctrl_new_std(&priv->hdl, &ov772x_ctrl_ops,
+						   V4L2_CID_BAND_STOP_FILTER,
+						   0, 256, 1, 0);
+	v4l2_ctrl_new_std_menu_items(&priv->hdl, &ov772x_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(ov772x_test_pattern_menu) - 1,
+				     0, 0, ov772x_test_pattern_menu);
+	priv->subdev.ctrl_handler = &priv->hdl;
+	if (priv->hdl.error) {
+		ret = priv->hdl.error;
+		goto error_ctrl_free;
+	}
+
+	priv->clk = clk_get(&client->dev, NULL);
+	if (IS_ERR(priv->clk)) {
+		dev_err(&client->dev, "Unable to get xclk clock\n");
+		ret = PTR_ERR(priv->clk);
+		goto error_ctrl_free;
+	}
+
+	priv->pwdn_gpio = gpiod_get_optional(&client->dev, "powerdown",
+					     GPIOD_OUT_LOW);
+	if (IS_ERR(priv->pwdn_gpio)) {
+		dev_info(&client->dev, "Unable to get GPIO \"powerdown\"");
+		ret = PTR_ERR(priv->pwdn_gpio);
+		goto error_clk_put;
+	}
+
+	ret = ov772x_parse_dt(client, priv);
+	if (ret)
+		goto error_clk_put;
+
+	ret = ov772x_video_probe(priv);
+	if (ret < 0)
+		goto error_gpio_put;
+
+#ifdef CONFIG_MEDIA_CONTROLLER
+	priv->pad.flags = MEDIA_PAD_FL_SOURCE;
+	priv->subdev.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret = media_entity_pads_init(&priv->subdev.entity, 1, &priv->pad);
+	if (ret < 0)
+		goto error_gpio_put;
+#endif
+
+	priv->cfmt = &ov772x_cfmts[0];
+	priv->win = &ov772x_win_sizes[0];
+	priv->fps = 15;
+
+	ret = v4l2_async_register_subdev(&priv->subdev);
+	if (ret)
+		goto error_entity_cleanup;
+
+	return 0;
+
+error_entity_cleanup:
+	media_entity_cleanup(&priv->subdev.entity);
+error_gpio_put:
+	if (priv->pwdn_gpio)
+		gpiod_put(priv->pwdn_gpio);
+error_clk_put:
+	clk_put(priv->clk);
+error_ctrl_free:
+	v4l2_ctrl_handler_free(&priv->hdl);
+	mutex_destroy(&priv->lock);
+
+	return ret;
+}
+
+static void ov772x_remove(struct i2c_client *client)
+{
+	struct ov772x_priv *priv = to_ov772x(i2c_get_clientdata(client));
+
+	media_entity_cleanup(&priv->subdev.entity);
+	clk_put(priv->clk);
+	if (priv->pwdn_gpio)
+		gpiod_put(priv->pwdn_gpio);
+	v4l2_async_unregister_subdev(&priv->subdev);
+	v4l2_ctrl_handler_free(&priv->hdl);
+	mutex_destroy(&priv->lock);
+}
+
+static const struct i2c_device_id ov772x_id[] = {
+	{ "ov772x", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ov772x_id);
+
+static const struct of_device_id ov772x_of_match[] = {
+	{ .compatible = "ovti,ov7725", },
+	{ .compatible = "ovti,ov7720", },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, ov772x_of_match);
+
+static struct i2c_driver ov772x_i2c_driver = {
+	.driver = {
+		.name = "ov772x",
+		.of_match_table = ov772x_of_match,
+	},
+	.probe    = ov772x_probe,
+	.remove   = ov772x_remove,
+	.id_table = ov772x_id,
+};
+
+module_i2c_driver(ov772x_i2c_driver);
+
+MODULE_DESCRIPTION("V4L2 driver for OV772x image sensor");
+MODULE_AUTHOR("Kuninori Morimoto");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/ov7740.c b/drivers/media/i2c/ov7740.c
new file mode 100644
index 000000000..dffdb475e
--- /dev/null
+++ b/drivers/media/i2c/ov7740.c
@@ -0,0 +1,1223 @@
+// SPDX-License-Identifier: GPL-2.0
+// Copyright (c) 2017 Microchip Corporation.
+
+#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/regmap.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-image-sizes.h>
+#include <media/v4l2-subdev.h>
+
+#define REG_OUTSIZE_LSB 0x34
+
+/* OV7740 register tables */
+#define REG_GAIN	0x00	/* Gain lower 8 bits (rest in vref) */
+#define REG_BGAIN	0x01	/* blue gain */
+#define REG_RGAIN	0x02	/* red gain */
+#define REG_GGAIN	0x03	/* green gain */
+#define REG_REG04	0x04	/* analog setting, don't change*/
+#define REG_BAVG	0x05	/* b channel average */
+#define REG_GAVG	0x06	/* g channel average */
+#define REG_RAVG	0x07	/* r channel average */
+
+#define REG_REG0C	0x0C	/* filp enable */
+#define REG0C_IMG_FLIP		0x80
+#define REG0C_IMG_MIRROR	0x40
+
+#define REG_REG0E	0x0E	/* blc line */
+#define REG_HAEC	0x0F	/* auto exposure cntrl */
+#define REG_AEC		0x10	/* auto exposure cntrl */
+
+#define REG_CLK		0x11	/* Clock control */
+#define REG_REG55	0x55	/* Clock PLL DIV/PreDiv */
+
+#define REG_REG12	0x12
+
+#define REG_REG13	0x13	/* auto/manual AGC, AEC, Write Balance*/
+#define REG13_AEC_EN	0x01
+#define REG13_AGC_EN	0x04
+
+#define REG_REG14	0x14
+#define REG_CTRL15	0x15
+#define REG15_GAIN_MSB	0x03
+
+#define REG_REG16	0x16
+
+#define REG_MIDH	0x1C	/* manufacture id byte */
+#define REG_MIDL	0x1D	/* manufacture id byre */
+#define REG_PIDH	0x0A	/* Product ID MSB */
+#define REG_PIDL	0x0B	/* Product ID LSB */
+
+#define REG_84		0x84	/* lots of stuff */
+#define REG_REG38	0x38	/* sub-addr */
+
+#define REG_AHSTART	0x17	/* Horiz start high bits */
+#define REG_AHSIZE	0x18
+#define REG_AVSTART	0x19	/* Vert start high bits */
+#define REG_AVSIZE	0x1A
+#define REG_PSHFT	0x1b	/* Pixel delay after HREF */
+
+#define REG_HOUTSIZE	0x31
+#define REG_VOUTSIZE	0x32
+#define REG_HVSIZEOFF	0x33
+#define REG_REG34	0x34	/* DSP output size H/V LSB*/
+
+#define REG_ISP_CTRL00	0x80
+#define ISPCTRL00_AWB_EN	0x10
+#define ISPCTRL00_AWB_GAIN_EN	0x04
+
+#define	REG_YGAIN	0xE2	/* ygain for contrast control */
+
+#define	REG_YBRIGHT	  0xE3
+#define	REG_SGNSET	  0xE4
+#define	SGNSET_YBRIGHT_MASK	  0x08
+
+#define REG_USAT	0xDD
+#define REG_VSAT	0xDE
+
+
+struct ov7740 {
+	struct v4l2_subdev subdev;
+#if defined(CONFIG_MEDIA_CONTROLLER)
+	struct media_pad pad;
+#endif
+	struct v4l2_mbus_framefmt format;
+	const struct ov7740_pixfmt *fmt;  /* Current format */
+	const struct ov7740_framesize *frmsize;
+	struct regmap *regmap;
+	struct clk *xvclk;
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct {
+		/* gain cluster */
+		struct v4l2_ctrl *auto_gain;
+		struct v4l2_ctrl *gain;
+	};
+	struct {
+		struct v4l2_ctrl *auto_wb;
+		struct v4l2_ctrl *blue_balance;
+		struct v4l2_ctrl *red_balance;
+	};
+	struct {
+		struct v4l2_ctrl *hflip;
+		struct v4l2_ctrl *vflip;
+	};
+	struct {
+		/* exposure cluster */
+		struct v4l2_ctrl *auto_exposure;
+		struct v4l2_ctrl *exposure;
+	};
+	struct {
+		/* saturation/hue cluster */
+		struct v4l2_ctrl *saturation;
+		struct v4l2_ctrl *hue;
+	};
+	struct v4l2_ctrl *brightness;
+	struct v4l2_ctrl *contrast;
+
+	struct mutex mutex;	/* To serialize asynchronus callbacks */
+	bool streaming;		/* Streaming on/off */
+
+	struct gpio_desc *resetb_gpio;
+	struct gpio_desc *pwdn_gpio;
+};
+
+struct ov7740_pixfmt {
+	u32 mbus_code;
+	enum v4l2_colorspace colorspace;
+	const struct reg_sequence *regs;
+	u32 reg_num;
+};
+
+struct ov7740_framesize {
+	u16 width;
+	u16 height;
+	const struct reg_sequence *regs;
+	u32 reg_num;
+};
+
+static const struct reg_sequence ov7740_vga[] = {
+	{0x55, 0x40},
+	{0x11, 0x02},
+
+	{0xd5, 0x10},
+	{0x0c, 0x12},
+	{0x0d, 0x34},
+	{0x17, 0x25},
+	{0x18, 0xa0},
+	{0x19, 0x03},
+	{0x1a, 0xf0},
+	{0x1b, 0x89},
+	{0x22, 0x03},
+	{0x29, 0x18},
+	{0x2b, 0xf8},
+	{0x2c, 0x01},
+	{REG_HOUTSIZE, 0xa0},
+	{REG_VOUTSIZE, 0xf0},
+	{0x33, 0xc4},
+	{REG_OUTSIZE_LSB, 0x0},
+	{0x35, 0x05},
+	{0x04, 0x60},
+	{0x27, 0x80},
+	{0x3d, 0x0f},
+	{0x3e, 0x80},
+	{0x3f, 0x40},
+	{0x40, 0x7f},
+	{0x41, 0x6a},
+	{0x42, 0x29},
+	{0x44, 0x22},
+	{0x45, 0x41},
+	{0x47, 0x02},
+	{0x49, 0x64},
+	{0x4a, 0xa1},
+	{0x4b, 0x40},
+	{0x4c, 0x1a},
+	{0x4d, 0x50},
+	{0x4e, 0x13},
+	{0x64, 0x00},
+	{0x67, 0x88},
+	{0x68, 0x1a},
+
+	{0x14, 0x28},
+	{0x24, 0x3c},
+	{0x25, 0x30},
+	{0x26, 0x72},
+	{0x50, 0x97},
+	{0x51, 0x1f},
+	{0x52, 0x00},
+	{0x53, 0x00},
+	{0x20, 0x00},
+	{0x21, 0xcf},
+	{0x50, 0x4b},
+	{0x38, 0x14},
+	{0xe9, 0x00},
+	{0x56, 0x55},
+	{0x57, 0xff},
+	{0x58, 0xff},
+	{0x59, 0xff},
+	{0x5f, 0x04},
+	{0xec, 0x00},
+	{0x13, 0xff},
+
+	{0x81, 0x3f},
+	{0x82, 0x32},
+	{0x38, 0x11},
+	{0x84, 0x70},
+	{0x85, 0x00},
+	{0x86, 0x03},
+	{0x87, 0x01},
+	{0x88, 0x05},
+	{0x89, 0x30},
+	{0x8d, 0x30},
+	{0x8f, 0x85},
+	{0x93, 0x30},
+	{0x95, 0x85},
+	{0x99, 0x30},
+	{0x9b, 0x85},
+
+	{0x9c, 0x08},
+	{0x9d, 0x12},
+	{0x9e, 0x23},
+	{0x9f, 0x45},
+	{0xa0, 0x55},
+	{0xa1, 0x64},
+	{0xa2, 0x72},
+	{0xa3, 0x7f},
+	{0xa4, 0x8b},
+	{0xa5, 0x95},
+	{0xa6, 0xa7},
+	{0xa7, 0xb5},
+	{0xa8, 0xcb},
+	{0xa9, 0xdd},
+	{0xaa, 0xec},
+	{0xab, 0x1a},
+
+	{0xce, 0x78},
+	{0xcf, 0x6e},
+	{0xd0, 0x0a},
+	{0xd1, 0x0c},
+	{0xd2, 0x84},
+	{0xd3, 0x90},
+	{0xd4, 0x1e},
+
+	{0x5a, 0x24},
+	{0x5b, 0x1f},
+	{0x5c, 0x88},
+	{0x5d, 0x60},
+
+	{0xac, 0x6e},
+	{0xbe, 0xff},
+	{0xbf, 0x00},
+
+	{0x0f, 0x1d},
+	{0x0f, 0x1f},
+};
+
+static const struct ov7740_framesize ov7740_framesizes[] = {
+	{
+		.width		= VGA_WIDTH,
+		.height		= VGA_HEIGHT,
+		.regs		= ov7740_vga,
+		.reg_num	= ARRAY_SIZE(ov7740_vga),
+	},
+};
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int ov7740_get_register(struct v4l2_subdev *sd,
+			       struct v4l2_dbg_register *reg)
+{
+	struct ov7740 *ov7740 = container_of(sd, struct ov7740, subdev);
+	struct regmap *regmap = ov7740->regmap;
+	unsigned int val = 0;
+	int ret;
+
+	ret = regmap_read(regmap, reg->reg & 0xff, &val);
+	reg->val = val;
+	reg->size = 1;
+
+	return ret;
+}
+
+static int ov7740_set_register(struct v4l2_subdev *sd,
+			       const struct v4l2_dbg_register *reg)
+{
+	struct ov7740 *ov7740 = container_of(sd, struct ov7740, subdev);
+	struct regmap *regmap = ov7740->regmap;
+
+	regmap_write(regmap, reg->reg & 0xff, reg->val & 0xff);
+
+	return 0;
+}
+#endif
+
+static int ov7740_set_power(struct ov7740 *ov7740, int on)
+{
+	int ret;
+
+	if (on) {
+		ret = clk_prepare_enable(ov7740->xvclk);
+		if (ret)
+			return ret;
+
+		if (ov7740->pwdn_gpio)
+			gpiod_direction_output(ov7740->pwdn_gpio, 0);
+
+		if (ov7740->resetb_gpio) {
+			gpiod_set_value(ov7740->resetb_gpio, 1);
+			usleep_range(500, 1000);
+			gpiod_set_value(ov7740->resetb_gpio, 0);
+			usleep_range(3000, 5000);
+		}
+	} else {
+		clk_disable_unprepare(ov7740->xvclk);
+
+		if (ov7740->pwdn_gpio)
+			gpiod_direction_output(ov7740->pwdn_gpio, 0);
+	}
+
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops ov7740_subdev_core_ops = {
+	.log_status = v4l2_ctrl_subdev_log_status,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = ov7740_get_register,
+	.s_register = ov7740_set_register,
+#endif
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static int ov7740_set_white_balance(struct ov7740 *ov7740, int awb)
+{
+	struct regmap *regmap = ov7740->regmap;
+	unsigned int value;
+	int ret;
+
+	ret = regmap_read(regmap, REG_ISP_CTRL00, &value);
+	if (!ret) {
+		if (awb)
+			value |= (ISPCTRL00_AWB_EN | ISPCTRL00_AWB_GAIN_EN);
+		else
+			value &= ~(ISPCTRL00_AWB_EN | ISPCTRL00_AWB_GAIN_EN);
+		ret = regmap_write(regmap, REG_ISP_CTRL00, value);
+		if (ret)
+			return ret;
+	}
+
+	if (!awb) {
+		ret = regmap_write(regmap, REG_BGAIN,
+				   ov7740->blue_balance->val);
+		if (ret)
+			return ret;
+
+		ret = regmap_write(regmap, REG_RGAIN, ov7740->red_balance->val);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int ov7740_set_saturation(struct regmap *regmap, int value)
+{
+	int ret;
+
+	ret = regmap_write(regmap, REG_USAT, (unsigned char)value);
+	if (ret)
+		return ret;
+
+	return regmap_write(regmap, REG_VSAT, (unsigned char)value);
+}
+
+static int ov7740_set_gain(struct regmap *regmap, int value)
+{
+	int ret;
+
+	ret = regmap_write(regmap, REG_GAIN, value & 0xff);
+	if (ret)
+		return ret;
+
+	ret = regmap_update_bits(regmap, REG_CTRL15,
+				 REG15_GAIN_MSB, (value >> 8) & 0x3);
+	if (!ret)
+		ret = regmap_update_bits(regmap, REG_REG13, REG13_AGC_EN, 0);
+
+	return ret;
+}
+
+static int ov7740_set_autogain(struct regmap *regmap, int value)
+{
+	unsigned int reg;
+	int ret;
+
+	ret = regmap_read(regmap, REG_REG13, &reg);
+	if (ret)
+		return ret;
+	if (value)
+		reg |= REG13_AGC_EN;
+	else
+		reg &= ~REG13_AGC_EN;
+	return regmap_write(regmap, REG_REG13, reg);
+}
+
+static int ov7740_set_brightness(struct regmap *regmap, int value)
+{
+	/* Turn off AEC/AGC */
+	regmap_update_bits(regmap, REG_REG13, REG13_AEC_EN, 0);
+	regmap_update_bits(regmap, REG_REG13, REG13_AGC_EN, 0);
+
+	if (value >= 0) {
+		regmap_write(regmap, REG_YBRIGHT, (unsigned char)value);
+		regmap_update_bits(regmap, REG_SGNSET, SGNSET_YBRIGHT_MASK, 0);
+	} else{
+		regmap_write(regmap, REG_YBRIGHT, (unsigned char)(-value));
+		regmap_update_bits(regmap, REG_SGNSET, SGNSET_YBRIGHT_MASK, 1);
+	}
+
+	return 0;
+}
+
+static int ov7740_set_contrast(struct regmap *regmap, int value)
+{
+	return regmap_write(regmap, REG_YGAIN, (unsigned char)value);
+}
+
+static int ov7740_get_gain(struct ov7740 *ov7740, struct v4l2_ctrl *ctrl)
+{
+	struct regmap *regmap = ov7740->regmap;
+	unsigned int value0, value1;
+	int ret;
+
+	if (!ctrl->val)
+		return 0;
+
+	ret = regmap_read(regmap, REG_GAIN, &value0);
+	if (ret)
+		return ret;
+	ret = regmap_read(regmap, REG_CTRL15, &value1);
+	if (ret)
+		return ret;
+
+	ov7740->gain->val = (value1 << 8) | (value0 & 0xff);
+
+	return 0;
+}
+
+static int ov7740_get_exp(struct ov7740 *ov7740, struct v4l2_ctrl *ctrl)
+{
+	struct regmap *regmap = ov7740->regmap;
+	unsigned int value0, value1;
+	int ret;
+
+	if (ctrl->val == V4L2_EXPOSURE_MANUAL)
+		return 0;
+
+	ret = regmap_read(regmap, REG_AEC, &value0);
+	if (ret)
+		return ret;
+	ret = regmap_read(regmap, REG_HAEC, &value1);
+	if (ret)
+		return ret;
+
+	ov7740->exposure->val = (value1 << 8) | (value0 & 0xff);
+
+	return 0;
+}
+
+static int ov7740_set_exp(struct regmap *regmap, int value)
+{
+	int ret;
+
+	/* Turn off AEC/AGC */
+	ret = regmap_update_bits(regmap, REG_REG13,
+				 REG13_AEC_EN | REG13_AGC_EN, 0);
+	if (ret)
+		return ret;
+
+	ret = regmap_write(regmap, REG_AEC, (unsigned char)value);
+	if (ret)
+		return ret;
+
+	return regmap_write(regmap, REG_HAEC, (unsigned char)(value >> 8));
+}
+
+static int ov7740_set_autoexp(struct regmap *regmap,
+			      enum v4l2_exposure_auto_type value)
+{
+	unsigned int reg;
+	int ret;
+
+	ret = regmap_read(regmap, REG_REG13, &reg);
+	if (!ret) {
+		if (value == V4L2_EXPOSURE_AUTO)
+			reg |= (REG13_AEC_EN | REG13_AGC_EN);
+		else
+			reg &= ~(REG13_AEC_EN | REG13_AGC_EN);
+		ret = regmap_write(regmap, REG_REG13, reg);
+	}
+
+	return ret;
+}
+
+
+static int ov7740_get_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov7740 *ov7740 = container_of(ctrl->handler,
+					     struct ov7740, ctrl_handler);
+	int ret;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTOGAIN:
+		ret = ov7740_get_gain(ov7740, ctrl);
+		break;
+	case V4L2_CID_EXPOSURE_AUTO:
+		ret = ov7740_get_exp(ov7740, ctrl);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+	return ret;
+}
+
+static int ov7740_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov7740 *ov7740 = container_of(ctrl->handler,
+					     struct ov7740, ctrl_handler);
+	struct i2c_client *client = v4l2_get_subdevdata(&ov7740->subdev);
+	struct regmap *regmap = ov7740->regmap;
+	int ret;
+	u8 val;
+
+	if (!pm_runtime_get_if_in_use(&client->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		ret = ov7740_set_white_balance(ov7740, ctrl->val);
+		break;
+	case V4L2_CID_SATURATION:
+		ret = ov7740_set_saturation(regmap, ctrl->val);
+		break;
+	case V4L2_CID_BRIGHTNESS:
+		ret = ov7740_set_brightness(regmap, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		ret = ov7740_set_contrast(regmap, ctrl->val);
+		break;
+	case V4L2_CID_VFLIP:
+		val = ctrl->val ? REG0C_IMG_FLIP : 0x00;
+		ret = regmap_update_bits(regmap, REG_REG0C,
+					 REG0C_IMG_FLIP, val);
+		break;
+	case V4L2_CID_HFLIP:
+		val = ctrl->val ? REG0C_IMG_MIRROR : 0x00;
+		ret = regmap_update_bits(regmap, REG_REG0C,
+					 REG0C_IMG_MIRROR, val);
+		break;
+	case V4L2_CID_AUTOGAIN:
+		if (!ctrl->val)
+			ret = ov7740_set_gain(regmap, ov7740->gain->val);
+		else
+			ret = ov7740_set_autogain(regmap, ctrl->val);
+		break;
+
+	case V4L2_CID_EXPOSURE_AUTO:
+		if (ctrl->val == V4L2_EXPOSURE_MANUAL)
+			ret = ov7740_set_exp(regmap, ov7740->exposure->val);
+		else
+			ret = ov7740_set_autoexp(regmap, ctrl->val);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	pm_runtime_put(&client->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ov7740_ctrl_ops = {
+	.g_volatile_ctrl = ov7740_get_volatile_ctrl,
+	.s_ctrl = ov7740_set_ctrl,
+};
+
+static int ov7740_start_streaming(struct ov7740 *ov7740)
+{
+	int ret;
+
+	if (ov7740->fmt) {
+		ret = regmap_multi_reg_write(ov7740->regmap,
+					     ov7740->fmt->regs,
+					     ov7740->fmt->reg_num);
+		if (ret)
+			return ret;
+	}
+
+	if (ov7740->frmsize) {
+		ret = regmap_multi_reg_write(ov7740->regmap,
+					     ov7740->frmsize->regs,
+					     ov7740->frmsize->reg_num);
+		if (ret)
+			return ret;
+	}
+
+	return __v4l2_ctrl_handler_setup(ov7740->subdev.ctrl_handler);
+}
+
+static int ov7740_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct ov7740 *ov7740 = container_of(sd, struct ov7740, subdev);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	mutex_lock(&ov7740->mutex);
+	if (ov7740->streaming == enable) {
+		mutex_unlock(&ov7740->mutex);
+		return 0;
+	}
+
+	if (enable) {
+		ret = pm_runtime_resume_and_get(&client->dev);
+		if (ret < 0)
+			goto err_unlock;
+
+		ret = ov7740_start_streaming(ov7740);
+		if (ret)
+			goto err_rpm_put;
+	} else {
+		pm_runtime_put(&client->dev);
+	}
+
+	ov7740->streaming = enable;
+
+	mutex_unlock(&ov7740->mutex);
+	return ret;
+
+err_rpm_put:
+	pm_runtime_put(&client->dev);
+err_unlock:
+	mutex_unlock(&ov7740->mutex);
+	return ret;
+}
+
+static int ov7740_g_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *ival)
+{
+	struct v4l2_fract *tpf = &ival->interval;
+
+
+	tpf->numerator = 1;
+	tpf->denominator = 60;
+
+	return 0;
+}
+
+static int ov7740_s_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *ival)
+{
+	struct v4l2_fract *tpf = &ival->interval;
+
+
+	tpf->numerator = 1;
+	tpf->denominator = 60;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops ov7740_subdev_video_ops = {
+	.s_stream = ov7740_set_stream,
+	.s_frame_interval = ov7740_s_frame_interval,
+	.g_frame_interval = ov7740_g_frame_interval,
+};
+
+static const struct reg_sequence ov7740_format_yuyv[] = {
+	{0x12, 0x00},
+	{0x36, 0x3f},
+	{0x80, 0x7f},
+	{0x83, 0x01},
+};
+
+static const struct reg_sequence ov7740_format_bggr8[] = {
+	{0x36, 0x2f},
+	{0x80, 0x01},
+	{0x83, 0x04},
+};
+
+static const struct ov7740_pixfmt ov7740_formats[] = {
+	{
+		.mbus_code = MEDIA_BUS_FMT_YUYV8_2X8,
+		.colorspace = V4L2_COLORSPACE_SRGB,
+		.regs = ov7740_format_yuyv,
+		.reg_num = ARRAY_SIZE(ov7740_format_yuyv),
+	},
+	{
+		.mbus_code = MEDIA_BUS_FMT_SBGGR8_1X8,
+		.colorspace = V4L2_COLORSPACE_SRGB,
+		.regs = ov7740_format_bggr8,
+		.reg_num = ARRAY_SIZE(ov7740_format_bggr8),
+	}
+};
+#define N_OV7740_FMTS ARRAY_SIZE(ov7740_formats)
+
+static int ov7740_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->pad || code->index >= N_OV7740_FMTS)
+		return -EINVAL;
+
+	code->code = ov7740_formats[code->index].mbus_code;
+
+	return 0;
+}
+
+static int ov7740_enum_frame_interval(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *sd_state,
+				struct v4l2_subdev_frame_interval_enum *fie)
+{
+	if (fie->pad)
+		return -EINVAL;
+
+	if (fie->index >= 1)
+		return -EINVAL;
+
+	if ((fie->width != VGA_WIDTH) || (fie->height != VGA_HEIGHT))
+		return -EINVAL;
+
+	fie->interval.numerator = 1;
+	fie->interval.denominator = 60;
+
+	return 0;
+}
+
+static int ov7740_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->pad)
+		return -EINVAL;
+
+	if (fse->index > 0)
+		return -EINVAL;
+
+	fse->min_width = fse->max_width = VGA_WIDTH;
+	fse->min_height = fse->max_height = VGA_HEIGHT;
+
+	return 0;
+}
+
+static int ov7740_try_fmt_internal(struct v4l2_subdev *sd,
+				   struct v4l2_mbus_framefmt *fmt,
+				   const struct ov7740_pixfmt **ret_fmt,
+				   const struct ov7740_framesize **ret_frmsize)
+{
+	struct ov7740 *ov7740 = container_of(sd, struct ov7740, subdev);
+	const struct ov7740_framesize *fsize = &ov7740_framesizes[0];
+	int index, i;
+
+	for (index = 0; index < N_OV7740_FMTS; index++) {
+		if (ov7740_formats[index].mbus_code == fmt->code)
+			break;
+	}
+	if (index >= N_OV7740_FMTS) {
+		/* default to first format */
+		index = 0;
+		fmt->code = ov7740_formats[0].mbus_code;
+	}
+	if (ret_fmt != NULL)
+		*ret_fmt = ov7740_formats + index;
+
+	for (i = 0; i < ARRAY_SIZE(ov7740_framesizes); i++) {
+		if ((fsize->width >= fmt->width) &&
+		    (fsize->height >= fmt->height)) {
+			fmt->width = fsize->width;
+			fmt->height = fsize->height;
+			break;
+		}
+
+		fsize++;
+	}
+	if (i >= ARRAY_SIZE(ov7740_framesizes)) {
+		fsize = &ov7740_framesizes[0];
+		fmt->width = fsize->width;
+		fmt->height = fsize->height;
+	}
+	if (ret_frmsize != NULL)
+		*ret_frmsize = fsize;
+
+	fmt->field = V4L2_FIELD_NONE;
+	fmt->colorspace = ov7740_formats[index].colorspace;
+
+	ov7740->format = *fmt;
+
+	return 0;
+}
+
+static int ov7740_set_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *format)
+{
+	struct ov7740 *ov7740 = container_of(sd, struct ov7740, subdev);
+	const struct ov7740_pixfmt *ovfmt;
+	const struct ov7740_framesize *fsize;
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
+	struct v4l2_mbus_framefmt *mbus_fmt;
+#endif
+	int ret;
+
+	mutex_lock(&ov7740->mutex);
+	if (format->pad) {
+		ret = -EINVAL;
+		goto error;
+	}
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		ret = ov7740_try_fmt_internal(sd, &format->format, NULL, NULL);
+		if (ret)
+			goto error;
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
+		mbus_fmt = v4l2_subdev_get_try_format(sd, sd_state,
+						      format->pad);
+		*mbus_fmt = format->format;
+#endif
+		mutex_unlock(&ov7740->mutex);
+		return 0;
+	}
+
+	ret = ov7740_try_fmt_internal(sd, &format->format, &ovfmt, &fsize);
+	if (ret)
+		goto error;
+
+	ov7740->fmt = ovfmt;
+	ov7740->frmsize = fsize;
+
+	mutex_unlock(&ov7740->mutex);
+	return 0;
+
+error:
+	mutex_unlock(&ov7740->mutex);
+	return ret;
+}
+
+static int ov7740_get_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *format)
+{
+	struct ov7740 *ov7740 = container_of(sd, struct ov7740, subdev);
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
+	struct v4l2_mbus_framefmt *mbus_fmt;
+#endif
+	int ret = 0;
+
+	mutex_lock(&ov7740->mutex);
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
+		mbus_fmt = v4l2_subdev_get_try_format(sd, sd_state, 0);
+		format->format = *mbus_fmt;
+		ret = 0;
+#else
+		ret = -EINVAL;
+#endif
+	} else {
+		format->format = ov7740->format;
+	}
+	mutex_unlock(&ov7740->mutex);
+
+	return ret;
+}
+
+static const struct v4l2_subdev_pad_ops ov7740_subdev_pad_ops = {
+	.enum_frame_interval = ov7740_enum_frame_interval,
+	.enum_frame_size = ov7740_enum_frame_size,
+	.enum_mbus_code = ov7740_enum_mbus_code,
+	.get_fmt = ov7740_get_fmt,
+	.set_fmt = ov7740_set_fmt,
+};
+
+static const struct v4l2_subdev_ops ov7740_subdev_ops = {
+	.core	= &ov7740_subdev_core_ops,
+	.video	= &ov7740_subdev_video_ops,
+	.pad	= &ov7740_subdev_pad_ops,
+};
+
+static void ov7740_get_default_format(struct v4l2_subdev *sd,
+				      struct v4l2_mbus_framefmt *format)
+{
+	struct ov7740 *ov7740 = container_of(sd, struct ov7740, subdev);
+
+	format->width = ov7740->frmsize->width;
+	format->height = ov7740->frmsize->height;
+	format->colorspace = ov7740->fmt->colorspace;
+	format->code = ov7740->fmt->mbus_code;
+	format->field = V4L2_FIELD_NONE;
+}
+
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
+static int ov7740_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct ov7740 *ov7740 = container_of(sd, struct ov7740, subdev);
+	struct v4l2_mbus_framefmt *format =
+				v4l2_subdev_get_try_format(sd, fh->state, 0);
+
+	mutex_lock(&ov7740->mutex);
+	ov7740_get_default_format(sd, format);
+	mutex_unlock(&ov7740->mutex);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_internal_ops ov7740_subdev_internal_ops = {
+	.open = ov7740_open,
+};
+#endif
+
+static int ov7740_probe_dt(struct i2c_client *client,
+			   struct ov7740 *ov7740)
+{
+	ov7740->resetb_gpio = devm_gpiod_get_optional(&client->dev, "reset",
+			GPIOD_OUT_HIGH);
+	if (IS_ERR(ov7740->resetb_gpio)) {
+		dev_info(&client->dev, "can't get %s GPIO\n", "reset");
+		return PTR_ERR(ov7740->resetb_gpio);
+	}
+
+	ov7740->pwdn_gpio = devm_gpiod_get_optional(&client->dev, "powerdown",
+			GPIOD_OUT_LOW);
+	if (IS_ERR(ov7740->pwdn_gpio)) {
+		dev_info(&client->dev, "can't get %s GPIO\n", "powerdown");
+		return PTR_ERR(ov7740->pwdn_gpio);
+	}
+
+	return 0;
+}
+
+static int ov7740_detect(struct ov7740 *ov7740)
+{
+	struct regmap *regmap = ov7740->regmap;
+	unsigned int midh, midl, pidh, pidl;
+	int ret;
+
+	ret = regmap_read(regmap, REG_MIDH, &midh);
+	if (ret)
+		return ret;
+	if (midh != 0x7f)
+		return -ENODEV;
+
+	ret = regmap_read(regmap, REG_MIDL, &midl);
+	if (ret)
+		return ret;
+	if (midl != 0xa2)
+		return -ENODEV;
+
+	ret = regmap_read(regmap, REG_PIDH, &pidh);
+	if (ret)
+		return ret;
+	if (pidh != 0x77)
+		return -ENODEV;
+
+	ret = regmap_read(regmap, REG_PIDL, &pidl);
+	if (ret)
+		return ret;
+	if ((pidl != 0x40) && (pidl != 0x41) && (pidl != 0x42))
+		return -ENODEV;
+
+	return 0;
+}
+
+static int ov7740_init_controls(struct ov7740 *ov7740)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov7740->subdev);
+	struct v4l2_ctrl_handler *ctrl_hdlr = &ov7740->ctrl_handler;
+	int ret;
+
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 12);
+	if (ret < 0)
+		return ret;
+
+	ctrl_hdlr->lock = &ov7740->mutex;
+	ov7740->auto_wb = v4l2_ctrl_new_std(ctrl_hdlr, &ov7740_ctrl_ops,
+					  V4L2_CID_AUTO_WHITE_BALANCE,
+					  0, 1, 1, 1);
+	ov7740->blue_balance = v4l2_ctrl_new_std(ctrl_hdlr, &ov7740_ctrl_ops,
+					       V4L2_CID_BLUE_BALANCE,
+					       0, 0xff, 1, 0x80);
+	ov7740->red_balance = v4l2_ctrl_new_std(ctrl_hdlr, &ov7740_ctrl_ops,
+					      V4L2_CID_RED_BALANCE,
+					      0, 0xff, 1, 0x80);
+
+	ov7740->brightness = v4l2_ctrl_new_std(ctrl_hdlr, &ov7740_ctrl_ops,
+					     V4L2_CID_BRIGHTNESS,
+					     -255, 255, 1, 0);
+	ov7740->contrast = v4l2_ctrl_new_std(ctrl_hdlr, &ov7740_ctrl_ops,
+					   V4L2_CID_CONTRAST,
+					   0, 127, 1, 0x20);
+	ov7740->saturation = v4l2_ctrl_new_std(ctrl_hdlr, &ov7740_ctrl_ops,
+			  V4L2_CID_SATURATION, 0, 256, 1, 0x80);
+	ov7740->hflip = v4l2_ctrl_new_std(ctrl_hdlr, &ov7740_ctrl_ops,
+					V4L2_CID_HFLIP, 0, 1, 1, 0);
+	ov7740->vflip = v4l2_ctrl_new_std(ctrl_hdlr, &ov7740_ctrl_ops,
+					V4L2_CID_VFLIP, 0, 1, 1, 0);
+
+	ov7740->gain = v4l2_ctrl_new_std(ctrl_hdlr, &ov7740_ctrl_ops,
+				       V4L2_CID_GAIN, 0, 1023, 1, 500);
+
+	ov7740->auto_gain = v4l2_ctrl_new_std(ctrl_hdlr, &ov7740_ctrl_ops,
+					    V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+
+	ov7740->exposure = v4l2_ctrl_new_std(ctrl_hdlr, &ov7740_ctrl_ops,
+					   V4L2_CID_EXPOSURE, 0, 65535, 1, 500);
+
+	ov7740->auto_exposure = v4l2_ctrl_new_std_menu(ctrl_hdlr,
+					&ov7740_ctrl_ops,
+					V4L2_CID_EXPOSURE_AUTO,
+					V4L2_EXPOSURE_MANUAL, 0,
+					V4L2_EXPOSURE_AUTO);
+
+	v4l2_ctrl_auto_cluster(3, &ov7740->auto_wb, 0, false);
+	v4l2_ctrl_auto_cluster(2, &ov7740->auto_gain, 0, true);
+	v4l2_ctrl_auto_cluster(2, &ov7740->auto_exposure,
+			       V4L2_EXPOSURE_MANUAL, true);
+
+	if (ctrl_hdlr->error) {
+		ret = ctrl_hdlr->error;
+		dev_err(&client->dev, "controls initialisation failed (%d)\n",
+			ret);
+		goto error;
+	}
+
+	ret = v4l2_ctrl_handler_setup(ctrl_hdlr);
+	if (ret) {
+		dev_err(&client->dev, "%s control init failed (%d)\n",
+			__func__, ret);
+		goto error;
+	}
+
+	ov7740->subdev.ctrl_handler = ctrl_hdlr;
+	return 0;
+
+error:
+	v4l2_ctrl_handler_free(ctrl_hdlr);
+	mutex_destroy(&ov7740->mutex);
+	return ret;
+}
+
+static void ov7740_free_controls(struct ov7740 *ov7740)
+{
+	v4l2_ctrl_handler_free(ov7740->subdev.ctrl_handler);
+	mutex_destroy(&ov7740->mutex);
+}
+
+#define OV7740_MAX_REGISTER     0xff
+static const struct regmap_config ov7740_regmap_config = {
+	.reg_bits	= 8,
+	.val_bits	= 8,
+	.max_register	= OV7740_MAX_REGISTER,
+};
+
+static int ov7740_probe(struct i2c_client *client)
+{
+	struct ov7740 *ov7740;
+	struct v4l2_subdev *sd;
+	int ret;
+
+	ov7740 = devm_kzalloc(&client->dev, sizeof(*ov7740), GFP_KERNEL);
+	if (!ov7740)
+		return -ENOMEM;
+
+	ov7740->xvclk = devm_clk_get(&client->dev, "xvclk");
+	if (IS_ERR(ov7740->xvclk)) {
+		ret = PTR_ERR(ov7740->xvclk);
+		dev_err(&client->dev,
+			"OV7740: fail to get xvclk: %d\n", ret);
+		return ret;
+	}
+
+	ret = ov7740_probe_dt(client, ov7740);
+	if (ret)
+		return ret;
+
+	ov7740->regmap = devm_regmap_init_sccb(client, &ov7740_regmap_config);
+	if (IS_ERR(ov7740->regmap)) {
+		ret = PTR_ERR(ov7740->regmap);
+		dev_err(&client->dev, "Failed to allocate register map: %d\n",
+			ret);
+		return ret;
+	}
+
+	sd = &ov7740->subdev;
+	v4l2_i2c_subdev_init(sd, client, &ov7740_subdev_ops);
+
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
+	sd->internal_ops = &ov7740_subdev_internal_ops;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
+#endif
+
+#if defined(CONFIG_MEDIA_CONTROLLER)
+	ov7740->pad.flags = MEDIA_PAD_FL_SOURCE;
+	sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret = media_entity_pads_init(&sd->entity, 1, &ov7740->pad);
+	if (ret)
+		return ret;
+#endif
+
+	ret = ov7740_set_power(ov7740, 1);
+	if (ret)
+		return ret;
+
+	pm_runtime_set_active(&client->dev);
+	pm_runtime_enable(&client->dev);
+
+	ret = ov7740_detect(ov7740);
+	if (ret)
+		goto error_detect;
+
+	mutex_init(&ov7740->mutex);
+
+	ret = ov7740_init_controls(ov7740);
+	if (ret)
+		goto error_init_controls;
+
+	v4l_info(client, "chip found @ 0x%02x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	ov7740->fmt = &ov7740_formats[0];
+	ov7740->frmsize = &ov7740_framesizes[0];
+
+	ov7740_get_default_format(sd, &ov7740->format);
+
+	ret = v4l2_async_register_subdev(sd);
+	if (ret)
+		goto error_async_register;
+
+	pm_runtime_idle(&client->dev);
+
+	return 0;
+
+error_async_register:
+	v4l2_ctrl_handler_free(ov7740->subdev.ctrl_handler);
+error_init_controls:
+	ov7740_free_controls(ov7740);
+error_detect:
+	pm_runtime_disable(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+	ov7740_set_power(ov7740, 0);
+	media_entity_cleanup(&ov7740->subdev.entity);
+
+	return ret;
+}
+
+static void ov7740_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov7740 *ov7740 = container_of(sd, struct ov7740, subdev);
+
+	mutex_destroy(&ov7740->mutex);
+	v4l2_ctrl_handler_free(ov7740->subdev.ctrl_handler);
+	media_entity_cleanup(&ov7740->subdev.entity);
+	v4l2_async_unregister_subdev(sd);
+	ov7740_free_controls(ov7740);
+
+	pm_runtime_get_sync(&client->dev);
+	pm_runtime_disable(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+	pm_runtime_put_noidle(&client->dev);
+
+	ov7740_set_power(ov7740, 0);
+}
+
+static int __maybe_unused ov7740_runtime_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov7740 *ov7740 = container_of(sd, struct ov7740, subdev);
+
+	ov7740_set_power(ov7740, 0);
+
+	return 0;
+}
+
+static int __maybe_unused ov7740_runtime_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov7740 *ov7740 = container_of(sd, struct ov7740, subdev);
+
+	return ov7740_set_power(ov7740, 1);
+}
+
+static const struct i2c_device_id ov7740_id[] = {
+	{ "ov7740", 0 },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(i2c, ov7740_id);
+
+static const struct dev_pm_ops ov7740_pm_ops = {
+	SET_RUNTIME_PM_OPS(ov7740_runtime_suspend, ov7740_runtime_resume, NULL)
+};
+
+static const struct of_device_id ov7740_of_match[] = {
+	{.compatible = "ovti,ov7740", },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, ov7740_of_match);
+
+static struct i2c_driver ov7740_i2c_driver = {
+	.driver = {
+		.name = "ov7740",
+		.pm = &ov7740_pm_ops,
+		.of_match_table = ov7740_of_match,
+	},
+	.probe    = ov7740_probe,
+	.remove   = ov7740_remove,
+	.id_table = ov7740_id,
+};
+module_i2c_driver(ov7740_i2c_driver);
+
+MODULE_DESCRIPTION("The V4L2 driver for Omnivision 7740 sensor");
+MODULE_AUTHOR("Songjun Wu <songjun.wu@atmel.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/ov8856.c b/drivers/media/i2c/ov8856.c
new file mode 100644
index 000000000..f053c3a76
--- /dev/null
+++ b/drivers/media/i2c/ov8856.c
@@ -0,0 +1,2539 @@
+// SPDX-License-Identifier: GPL-2.0
+// Copyright (c) 2019 Intel Corporation.
+
+#include <asm/unaligned.h>
+#include <linux/acpi.h>
+#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-fwnode.h>
+
+#define OV8856_REG_VALUE_08BIT		1
+#define OV8856_REG_VALUE_16BIT		2
+#define OV8856_REG_VALUE_24BIT		3
+
+#define OV8856_SCLK			144000000ULL
+#define OV8856_XVCLK_19_2		19200000
+#define OV8856_DATA_LANES		4
+#define OV8856_RGB_DEPTH		10
+
+#define OV8856_REG_CHIP_ID		0x300a
+#define OV8856_CHIP_ID			0x00885a
+
+#define OV8856_REG_MODE_SELECT		0x0100
+#define OV8856_MODE_STANDBY		0x00
+#define OV8856_MODE_STREAMING		0x01
+
+/* module revisions */
+#define OV8856_2A_MODULE		0x01
+#define OV8856_1B_MODULE		0x02
+
+/* the OTP read-out buffer is at 0x7000 and 0xf is the offset
+ * of the byte in the OTP that means the module revision
+ */
+#define OV8856_MODULE_REVISION		0x700f
+#define OV8856_OTP_MODE_CTRL		0x3d84
+#define OV8856_OTP_LOAD_CTRL		0x3d81
+#define OV8856_OTP_MODE_AUTO		0x00
+#define OV8856_OTP_LOAD_CTRL_ENABLE	BIT(0)
+
+/* vertical-timings from sensor */
+#define OV8856_REG_VTS			0x380e
+#define OV8856_VTS_MAX			0x7fff
+
+/* horizontal-timings from sensor */
+#define OV8856_REG_HTS			0x380c
+
+/* Exposure controls from sensor */
+#define OV8856_REG_EXPOSURE		0x3500
+#define	OV8856_EXPOSURE_MIN		6
+#define OV8856_EXPOSURE_MAX_MARGIN	6
+#define	OV8856_EXPOSURE_STEP		1
+
+/* Analog gain controls from sensor */
+#define OV8856_REG_ANALOG_GAIN		0x3508
+#define	OV8856_ANAL_GAIN_MIN		128
+#define	OV8856_ANAL_GAIN_MAX		2047
+#define	OV8856_ANAL_GAIN_STEP		1
+
+/* Digital gain controls from sensor */
+#define OV8856_REG_DIGITAL_GAIN		0x350a
+#define OV8856_REG_MWB_R_GAIN		0x5019
+#define OV8856_REG_MWB_G_GAIN		0x501b
+#define OV8856_REG_MWB_B_GAIN		0x501d
+#define OV8856_DGTL_GAIN_MIN		0
+#define OV8856_DGTL_GAIN_MAX		4095
+#define OV8856_DGTL_GAIN_STEP		1
+#define OV8856_DGTL_GAIN_DEFAULT	1024
+
+/* Test Pattern Control */
+#define OV8856_REG_TEST_PATTERN		0x5e00
+#define OV8856_TEST_PATTERN_ENABLE	BIT(7)
+#define OV8856_TEST_PATTERN_BAR_SHIFT	2
+
+#define NUM_REGS				7
+#define NUM_MODE_REGS				187
+#define NUM_MODE_REGS_2				200
+
+/* Flip Mirror Controls from sensor */
+#define OV8856_REG_FORMAT1			0x3820
+#define OV8856_REG_FORMAT2			0x3821
+#define OV8856_REG_FORMAT1_OP_1			BIT(1)
+#define OV8856_REG_FORMAT1_OP_2			BIT(2)
+#define OV8856_REG_FORMAT1_OP_3			BIT(6)
+#define OV8856_REG_FORMAT2_OP_1			BIT(1)
+#define OV8856_REG_FORMAT2_OP_2			BIT(2)
+#define OV8856_REG_FORMAT2_OP_3			BIT(6)
+#define OV8856_REG_FLIP_OPT_1			0x376b
+#define OV8856_REG_FLIP_OPT_2			0x5001
+#define OV8856_REG_FLIP_OPT_3			0x502e
+#define OV8856_REG_MIRROR_OPT_1			0x5004
+#define OV8856_REG_FLIP_OP_0			BIT(0)
+#define OV8856_REG_FLIP_OP_1			BIT(1)
+#define OV8856_REG_FLIP_OP_2			BIT(2)
+#define OV8856_REG_MIRROR_OP_1			BIT(1)
+#define OV8856_REG_MIRROR_OP_2			BIT(2)
+
+#define to_ov8856(_sd)			container_of(_sd, struct ov8856, sd)
+
+static const char * const ov8856_supply_names[] = {
+	"dovdd",	/* Digital I/O power */
+	"avdd",		/* Analog power */
+	"dvdd",		/* Digital core power */
+};
+
+enum {
+	OV8856_MEDIA_BUS_FMT_SBGGR10_1X10,
+	OV8856_MEDIA_BUS_FMT_SGRBG10_1X10,
+};
+
+struct ov8856_reg {
+	u16 address;
+	u8 val;
+};
+
+struct ov8856_reg_list {
+	u32 num_of_regs;
+	const struct ov8856_reg *regs;
+};
+
+struct ov8856_link_freq_config {
+	const struct ov8856_reg_list reg_list;
+};
+
+struct ov8856_mode {
+	/* Frame width in pixels */
+	u32 width;
+
+	/* Frame height in pixels */
+	u32 height;
+
+	/* Horizontal timining size */
+	u32 hts;
+
+	/* Default vertical timining size */
+	u32 vts_def;
+
+	/* Min vertical timining size */
+	u32 vts_min;
+
+	/* Link frequency needed for this resolution */
+	u32 link_freq_index;
+
+	/* Sensor register settings for this resolution */
+	const struct ov8856_reg_list reg_list;
+
+	/* Number of data lanes */
+	u8 data_lanes;
+
+	/* Default MEDIA_BUS_FMT for this mode */
+	u32 default_mbus_index;
+};
+
+struct ov8856_mipi_data_rates {
+	const struct ov8856_reg regs_0[NUM_REGS];
+	const struct ov8856_reg regs_1[NUM_REGS];
+};
+
+static const struct ov8856_mipi_data_rates mipi_data_rate_lane_2 = {
+	//mipi_data_rate_1440mbps
+	{
+		{0x0103, 0x01},
+		{0x0100, 0x00},
+		{0x0302, 0x43},
+		{0x0303, 0x00},
+		{0x030b, 0x02},
+		{0x030d, 0x4b},
+		{0x031e, 0x0c}
+	},
+	//mipi_data_rate_720mbps
+	{
+		{0x0103, 0x01},
+		{0x0100, 0x00},
+		{0x0302, 0x4b},
+		{0x0303, 0x01},
+		{0x030b, 0x02},
+		{0x030d, 0x4b},
+		{0x031e, 0x0c}
+	}
+};
+
+static const struct ov8856_mipi_data_rates mipi_data_rate_lane_4 = {
+	//mipi_data_rate_720mbps
+	{
+		{0x0103, 0x01},
+		{0x0100, 0x00},
+		{0x0302, 0x4b},
+		{0x0303, 0x01},
+		{0x030b, 0x02},
+		{0x030d, 0x4b},
+		{0x031e, 0x0c}
+	},
+	//mipi_data_rate_360mbps
+	{
+		{0x0103, 0x01},
+		{0x0100, 0x00},
+		{0x0302, 0x4b},
+		{0x0303, 0x03},
+		{0x030b, 0x02},
+		{0x030d, 0x4b},
+		{0x031e, 0x0c}
+	}
+};
+
+static const struct ov8856_reg lane_2_mode_3280x2464[] = {
+	/* 3280x2464 resolution */
+		{0x3000, 0x20},
+		{0x3003, 0x08},
+		{0x300e, 0x20},
+		{0x3010, 0x00},
+		{0x3015, 0x84},
+		{0x3018, 0x32},
+		{0x3021, 0x23},
+		{0x3033, 0x24},
+		{0x3500, 0x00},
+		{0x3501, 0x9a},
+		{0x3502, 0x20},
+		{0x3503, 0x08},
+		{0x3505, 0x83},
+		{0x3508, 0x01},
+		{0x3509, 0x80},
+		{0x350c, 0x00},
+		{0x350d, 0x80},
+		{0x350e, 0x04},
+		{0x350f, 0x00},
+		{0x3510, 0x00},
+		{0x3511, 0x02},
+		{0x3512, 0x00},
+		{0x3600, 0x72},
+		{0x3601, 0x40},
+		{0x3602, 0x30},
+		{0x3610, 0xc5},
+		{0x3611, 0x58},
+		{0x3612, 0x5c},
+		{0x3613, 0xca},
+		{0x3614, 0x50},
+		{0x3628, 0xff},
+		{0x3629, 0xff},
+		{0x362a, 0xff},
+		{0x3633, 0x10},
+		{0x3634, 0x10},
+		{0x3635, 0x10},
+		{0x3636, 0x10},
+		{0x3663, 0x08},
+		{0x3669, 0x34},
+		{0x366e, 0x10},
+		{0x3706, 0x86},
+		{0x370b, 0x7e},
+		{0x3714, 0x23},
+		{0x3730, 0x12},
+		{0x3733, 0x10},
+		{0x3764, 0x00},
+		{0x3765, 0x00},
+		{0x3769, 0x62},
+		{0x376a, 0x2a},
+		{0x376b, 0x30},
+		{0x3780, 0x00},
+		{0x3781, 0x24},
+		{0x3782, 0x00},
+		{0x3783, 0x23},
+		{0x3798, 0x2f},
+		{0x37a1, 0x60},
+		{0x37a8, 0x6a},
+		{0x37ab, 0x3f},
+		{0x37c2, 0x04},
+		{0x37c3, 0xf1},
+		{0x37c9, 0x80},
+		{0x37cb, 0x16},
+		{0x37cc, 0x16},
+		{0x37cd, 0x16},
+		{0x37ce, 0x16},
+		{0x3800, 0x00},
+		{0x3801, 0x00},
+		{0x3802, 0x00},
+		{0x3803, 0x06},
+		{0x3804, 0x0c},
+		{0x3805, 0xdf},
+		{0x3806, 0x09},
+		{0x3807, 0xa7},
+		{0x3808, 0x0c},
+		{0x3809, 0xd0},
+		{0x380a, 0x09},
+		{0x380b, 0xa0},
+		{0x380c, 0x07},
+		{0x380d, 0x88},
+		{0x380e, 0x09},
+		{0x380f, 0xb8},
+		{0x3810, 0x00},
+		{0x3811, 0x00},
+		{0x3812, 0x00},
+		{0x3813, 0x01},
+		{0x3814, 0x01},
+		{0x3815, 0x01},
+		{0x3816, 0x00},
+		{0x3817, 0x00},
+		{0x3818, 0x00},
+		{0x3819, 0x00},
+		{0x3820, 0x80},
+		{0x3821, 0x46},
+		{0x382a, 0x01},
+		{0x382b, 0x01},
+		{0x3830, 0x06},
+		{0x3836, 0x02},
+		{0x3837, 0x10},
+		{0x3862, 0x04},
+		{0x3863, 0x08},
+		{0x3cc0, 0x33},
+		{0x3d85, 0x14},
+		{0x3d8c, 0x73},
+		{0x3d8d, 0xde},
+		{0x4001, 0xe0},
+		{0x4003, 0x40},
+		{0x4008, 0x00},
+		{0x4009, 0x0b},
+		{0x400a, 0x00},
+		{0x400b, 0x84},
+		{0x400f, 0x80},
+		{0x4010, 0xf0},
+		{0x4011, 0xff},
+		{0x4012, 0x02},
+		{0x4013, 0x01},
+		{0x4014, 0x01},
+		{0x4015, 0x01},
+		{0x4042, 0x00},
+		{0x4043, 0x80},
+		{0x4044, 0x00},
+		{0x4045, 0x80},
+		{0x4046, 0x00},
+		{0x4047, 0x80},
+		{0x4048, 0x00},
+		{0x4049, 0x80},
+		{0x4041, 0x03},
+		{0x404c, 0x20},
+		{0x404d, 0x00},
+		{0x404e, 0x20},
+		{0x4203, 0x80},
+		{0x4307, 0x30},
+		{0x4317, 0x00},
+		{0x4503, 0x08},
+		{0x4601, 0x80},
+		{0x4800, 0x44},
+		{0x4816, 0x53},
+		{0x481b, 0x58},
+		{0x481f, 0x27},
+		{0x4837, 0x0c},
+		{0x483c, 0x0f},
+		{0x484b, 0x05},
+		{0x5000, 0x57},
+		{0x5001, 0x0a},
+		{0x5004, 0x06},
+		{0x502e, 0x03},
+		{0x5030, 0x41},
+		{0x5795, 0x02},
+		{0x5796, 0x20},
+		{0x5797, 0x20},
+		{0x5798, 0xd5},
+		{0x5799, 0xd5},
+		{0x579a, 0x00},
+		{0x579b, 0x50},
+		{0x579c, 0x00},
+		{0x579d, 0x2c},
+		{0x579e, 0x0c},
+		{0x579f, 0x40},
+		{0x57a0, 0x09},
+		{0x57a1, 0x40},
+		{0x5780, 0x14},
+		{0x5781, 0x0f},
+		{0x5782, 0x44},
+		{0x5783, 0x02},
+		{0x5784, 0x01},
+		{0x5785, 0x01},
+		{0x5786, 0x00},
+		{0x5787, 0x04},
+		{0x5788, 0x02},
+		{0x5789, 0x0f},
+		{0x578a, 0xfd},
+		{0x578b, 0xf5},
+		{0x578c, 0xf5},
+		{0x578d, 0x03},
+		{0x578e, 0x08},
+		{0x578f, 0x0c},
+		{0x5790, 0x08},
+		{0x5791, 0x04},
+		{0x5792, 0x00},
+		{0x5793, 0x52},
+		{0x5794, 0xa3},
+		{0x59f8, 0x3d},
+		{0x5a08, 0x02},
+		{0x5b00, 0x02},
+		{0x5b01, 0x10},
+		{0x5b02, 0x03},
+		{0x5b03, 0xcf},
+		{0x5b05, 0x6c},
+		{0x5e00, 0x00}
+};
+
+static const struct ov8856_reg lane_2_mode_1640x1232[] = {
+	/* 1640x1232 resolution */
+		{0x3000, 0x20},
+		{0x3003, 0x08},
+		{0x300e, 0x20},
+		{0x3010, 0x00},
+		{0x3015, 0x84},
+		{0x3018, 0x32},
+		{0x3021, 0x23},
+		{0x3033, 0x24},
+		{0x3500, 0x00},
+		{0x3501, 0x4c},
+		{0x3502, 0xe0},
+		{0x3503, 0x08},
+		{0x3505, 0x83},
+		{0x3508, 0x01},
+		{0x3509, 0x80},
+		{0x350c, 0x00},
+		{0x350d, 0x80},
+		{0x350e, 0x04},
+		{0x350f, 0x00},
+		{0x3510, 0x00},
+		{0x3511, 0x02},
+		{0x3512, 0x00},
+		{0x3600, 0x72},
+		{0x3601, 0x40},
+		{0x3602, 0x30},
+		{0x3610, 0xc5},
+		{0x3611, 0x58},
+		{0x3612, 0x5c},
+		{0x3613, 0xca},
+		{0x3614, 0x50},
+		{0x3628, 0xff},
+		{0x3629, 0xff},
+		{0x362a, 0xff},
+		{0x3633, 0x10},
+		{0x3634, 0x10},
+		{0x3635, 0x10},
+		{0x3636, 0x10},
+		{0x3663, 0x08},
+		{0x3669, 0x34},
+		{0x366e, 0x08},
+		{0x3706, 0x86},
+		{0x370b, 0x7e},
+		{0x3714, 0x27},
+		{0x3730, 0x12},
+		{0x3733, 0x10},
+		{0x3764, 0x00},
+		{0x3765, 0x00},
+		{0x3769, 0x62},
+		{0x376a, 0x2a},
+		{0x376b, 0x30},
+		{0x3780, 0x00},
+		{0x3781, 0x24},
+		{0x3782, 0x00},
+		{0x3783, 0x23},
+		{0x3798, 0x2f},
+		{0x37a1, 0x60},
+		{0x37a8, 0x6a},
+		{0x37ab, 0x3f},
+		{0x37c2, 0x14},
+		{0x37c3, 0xf1},
+		{0x37c9, 0x80},
+		{0x37cb, 0x16},
+		{0x37cc, 0x16},
+		{0x37cd, 0x16},
+		{0x37ce, 0x16},
+		{0x3800, 0x00},
+		{0x3801, 0x00},
+		{0x3802, 0x00},
+		{0x3803, 0x00},
+		{0x3804, 0x0c},
+		{0x3805, 0xdf},
+		{0x3806, 0x09},
+		{0x3807, 0xaf},
+		{0x3808, 0x06},
+		{0x3809, 0x68},
+		{0x380a, 0x04},
+		{0x380b, 0xd0},
+		{0x380c, 0x0c},
+		{0x380d, 0x60},
+		{0x380e, 0x05},
+		{0x380f, 0xea},
+		{0x3810, 0x00},
+		{0x3811, 0x04},
+		{0x3812, 0x00},
+		{0x3813, 0x05},
+		{0x3814, 0x03},
+		{0x3815, 0x01},
+		{0x3816, 0x00},
+		{0x3817, 0x00},
+		{0x3818, 0x00},
+		{0x3819, 0x00},
+		{0x3820, 0x90},
+		{0x3821, 0x67},
+		{0x382a, 0x03},
+		{0x382b, 0x01},
+		{0x3830, 0x06},
+		{0x3836, 0x02},
+		{0x3837, 0x10},
+		{0x3862, 0x04},
+		{0x3863, 0x08},
+		{0x3cc0, 0x33},
+		{0x3d85, 0x14},
+		{0x3d8c, 0x73},
+		{0x3d8d, 0xde},
+		{0x4001, 0xe0},
+		{0x4003, 0x40},
+		{0x4008, 0x00},
+		{0x4009, 0x05},
+		{0x400a, 0x00},
+		{0x400b, 0x84},
+		{0x400f, 0x80},
+		{0x4010, 0xf0},
+		{0x4011, 0xff},
+		{0x4012, 0x02},
+		{0x4013, 0x01},
+		{0x4014, 0x01},
+		{0x4015, 0x01},
+		{0x4042, 0x00},
+		{0x4043, 0x80},
+		{0x4044, 0x00},
+		{0x4045, 0x80},
+		{0x4046, 0x00},
+		{0x4047, 0x80},
+		{0x4048, 0x00},
+		{0x4049, 0x80},
+		{0x4041, 0x03},
+		{0x404c, 0x20},
+		{0x404d, 0x00},
+		{0x404e, 0x20},
+		{0x4203, 0x80},
+		{0x4307, 0x30},
+		{0x4317, 0x00},
+		{0x4503, 0x08},
+		{0x4601, 0x80},
+		{0x4800, 0x44},
+		{0x4816, 0x53},
+		{0x481b, 0x58},
+		{0x481f, 0x27},
+		{0x4837, 0x16},
+		{0x483c, 0x0f},
+		{0x484b, 0x05},
+		{0x5000, 0x57},
+		{0x5001, 0x0a},
+		{0x5004, 0x06},
+		{0x502e, 0x03},
+		{0x5030, 0x41},
+		{0x5795, 0x00},
+		{0x5796, 0x10},
+		{0x5797, 0x10},
+		{0x5798, 0x73},
+		{0x5799, 0x73},
+		{0x579a, 0x00},
+		{0x579b, 0x28},
+		{0x579c, 0x00},
+		{0x579d, 0x16},
+		{0x579e, 0x06},
+		{0x579f, 0x20},
+		{0x57a0, 0x04},
+		{0x57a1, 0xa0},
+		{0x5780, 0x14},
+		{0x5781, 0x0f},
+		{0x5782, 0x44},
+		{0x5783, 0x02},
+		{0x5784, 0x01},
+		{0x5785, 0x01},
+		{0x5786, 0x00},
+		{0x5787, 0x04},
+		{0x5788, 0x02},
+		{0x5789, 0x0f},
+		{0x578a, 0xfd},
+		{0x578b, 0xf5},
+		{0x578c, 0xf5},
+		{0x578d, 0x03},
+		{0x578e, 0x08},
+		{0x578f, 0x0c},
+		{0x5790, 0x08},
+		{0x5791, 0x04},
+		{0x5792, 0x00},
+		{0x5793, 0x52},
+		{0x5794, 0xa3},
+		{0x59f8, 0x3d},
+		{0x5a08, 0x02},
+		{0x5b00, 0x02},
+		{0x5b01, 0x10},
+		{0x5b02, 0x03},
+		{0x5b03, 0xcf},
+		{0x5b05, 0x6c},
+		{0x5e00, 0x00}
+};
+
+static const struct ov8856_reg lane_4_mode_3280x2464[] = {
+	/* 3280x2464 resolution */
+		{0x3000, 0x20},
+		{0x3003, 0x08},
+		{0x300e, 0x20},
+		{0x3010, 0x00},
+		{0x3015, 0x84},
+		{0x3018, 0x72},
+		{0x3021, 0x23},
+		{0x3033, 0x24},
+		{0x3500, 0x00},
+		{0x3501, 0x9a},
+		{0x3502, 0x20},
+		{0x3503, 0x08},
+		{0x3505, 0x83},
+		{0x3508, 0x01},
+		{0x3509, 0x80},
+		{0x350c, 0x00},
+		{0x350d, 0x80},
+		{0x350e, 0x04},
+		{0x350f, 0x00},
+		{0x3510, 0x00},
+		{0x3511, 0x02},
+		{0x3512, 0x00},
+		{0x3600, 0x72},
+		{0x3601, 0x40},
+		{0x3602, 0x30},
+		{0x3610, 0xc5},
+		{0x3611, 0x58},
+		{0x3612, 0x5c},
+		{0x3613, 0xca},
+		{0x3614, 0x20},
+		{0x3628, 0xff},
+		{0x3629, 0xff},
+		{0x362a, 0xff},
+		{0x3633, 0x10},
+		{0x3634, 0x10},
+		{0x3635, 0x10},
+		{0x3636, 0x10},
+		{0x3663, 0x08},
+		{0x3669, 0x34},
+		{0x366e, 0x10},
+		{0x3706, 0x86},
+		{0x370b, 0x7e},
+		{0x3714, 0x23},
+		{0x3730, 0x12},
+		{0x3733, 0x10},
+		{0x3764, 0x00},
+		{0x3765, 0x00},
+		{0x3769, 0x62},
+		{0x376a, 0x2a},
+		{0x376b, 0x30},
+		{0x3780, 0x00},
+		{0x3781, 0x24},
+		{0x3782, 0x00},
+		{0x3783, 0x23},
+		{0x3798, 0x2f},
+		{0x37a1, 0x60},
+		{0x37a8, 0x6a},
+		{0x37ab, 0x3f},
+		{0x37c2, 0x04},
+		{0x37c3, 0xf1},
+		{0x37c9, 0x80},
+		{0x37cb, 0x16},
+		{0x37cc, 0x16},
+		{0x37cd, 0x16},
+		{0x37ce, 0x16},
+		{0x3800, 0x00},
+		{0x3801, 0x00},
+		{0x3802, 0x00},
+		{0x3803, 0x06},
+		{0x3804, 0x0c},
+		{0x3805, 0xdf},
+		{0x3806, 0x09},
+		{0x3807, 0xa7},
+		{0x3808, 0x0c},
+		{0x3809, 0xd0},
+		{0x380a, 0x09},
+		{0x380b, 0xa0},
+		{0x380c, 0x07},
+		{0x380d, 0x88},
+		{0x380e, 0x09},
+		{0x380f, 0xb8},
+		{0x3810, 0x00},
+		{0x3811, 0x00},
+		{0x3812, 0x00},
+		{0x3813, 0x01},
+		{0x3814, 0x01},
+		{0x3815, 0x01},
+		{0x3816, 0x00},
+		{0x3817, 0x00},
+		{0x3818, 0x00},
+		{0x3819, 0x10},
+		{0x3820, 0x80},
+		{0x3821, 0x46},
+		{0x382a, 0x01},
+		{0x382b, 0x01},
+		{0x3830, 0x06},
+		{0x3836, 0x02},
+		{0x3862, 0x04},
+		{0x3863, 0x08},
+		{0x3cc0, 0x33},
+		{0x3d85, 0x17},
+		{0x3d8c, 0x73},
+		{0x3d8d, 0xde},
+		{0x4001, 0xe0},
+		{0x4003, 0x40},
+		{0x4008, 0x00},
+		{0x4009, 0x0b},
+		{0x400a, 0x00},
+		{0x400b, 0x84},
+		{0x400f, 0x80},
+		{0x4010, 0xf0},
+		{0x4011, 0xff},
+		{0x4012, 0x02},
+		{0x4013, 0x01},
+		{0x4014, 0x01},
+		{0x4015, 0x01},
+		{0x4042, 0x00},
+		{0x4043, 0x80},
+		{0x4044, 0x00},
+		{0x4045, 0x80},
+		{0x4046, 0x00},
+		{0x4047, 0x80},
+		{0x4048, 0x00},
+		{0x4049, 0x80},
+		{0x4041, 0x03},
+		{0x404c, 0x20},
+		{0x404d, 0x00},
+		{0x404e, 0x20},
+		{0x4203, 0x80},
+		{0x4307, 0x30},
+		{0x4317, 0x00},
+		{0x4503, 0x08},
+		{0x4601, 0x80},
+		{0x4800, 0x44},
+		{0x4816, 0x53},
+		{0x481b, 0x58},
+		{0x481f, 0x27},
+		{0x4837, 0x16},
+		{0x483c, 0x0f},
+		{0x484b, 0x05},
+		{0x5000, 0x57},
+		{0x5001, 0x0a},
+		{0x5004, 0x06},
+		{0x502e, 0x03},
+		{0x5030, 0x41},
+		{0x5780, 0x14},
+		{0x5781, 0x0f},
+		{0x5782, 0x44},
+		{0x5783, 0x02},
+		{0x5784, 0x01},
+		{0x5785, 0x01},
+		{0x5786, 0x00},
+		{0x5787, 0x04},
+		{0x5788, 0x02},
+		{0x5789, 0x0f},
+		{0x578a, 0xfd},
+		{0x578b, 0xf5},
+		{0x578c, 0xf5},
+		{0x578d, 0x03},
+		{0x578e, 0x08},
+		{0x578f, 0x0c},
+		{0x5790, 0x08},
+		{0x5791, 0x04},
+		{0x5792, 0x00},
+		{0x5793, 0x52},
+		{0x5794, 0xa3},
+		{0x5795, 0x02},
+		{0x5796, 0x20},
+		{0x5797, 0x20},
+		{0x5798, 0xd5},
+		{0x5799, 0xd5},
+		{0x579a, 0x00},
+		{0x579b, 0x50},
+		{0x579c, 0x00},
+		{0x579d, 0x2c},
+		{0x579e, 0x0c},
+		{0x579f, 0x40},
+		{0x57a0, 0x09},
+		{0x57a1, 0x40},
+		{0x59f8, 0x3d},
+		{0x5a08, 0x02},
+		{0x5b00, 0x02},
+		{0x5b01, 0x10},
+		{0x5b02, 0x03},
+		{0x5b03, 0xcf},
+		{0x5b05, 0x6c},
+		{0x5e00, 0x00}
+};
+
+static const struct ov8856_reg lane_4_mode_1640x1232[] = {
+	/* 1640x1232 resolution */
+		{0x3000, 0x20},
+		{0x3003, 0x08},
+		{0x300e, 0x20},
+		{0x3010, 0x00},
+		{0x3015, 0x84},
+		{0x3018, 0x72},
+		{0x3021, 0x23},
+		{0x3033, 0x24},
+		{0x3500, 0x00},
+		{0x3501, 0x4c},
+		{0x3502, 0xe0},
+		{0x3503, 0x08},
+		{0x3505, 0x83},
+		{0x3508, 0x01},
+		{0x3509, 0x80},
+		{0x350c, 0x00},
+		{0x350d, 0x80},
+		{0x350e, 0x04},
+		{0x350f, 0x00},
+		{0x3510, 0x00},
+		{0x3511, 0x02},
+		{0x3512, 0x00},
+		{0x3600, 0x72},
+		{0x3601, 0x40},
+		{0x3602, 0x30},
+		{0x3610, 0xc5},
+		{0x3611, 0x58},
+		{0x3612, 0x5c},
+		{0x3613, 0xca},
+		{0x3614, 0x20},
+		{0x3628, 0xff},
+		{0x3629, 0xff},
+		{0x362a, 0xff},
+		{0x3633, 0x10},
+		{0x3634, 0x10},
+		{0x3635, 0x10},
+		{0x3636, 0x10},
+		{0x3663, 0x08},
+		{0x3669, 0x34},
+		{0x366e, 0x08},
+		{0x3706, 0x86},
+		{0x370b, 0x7e},
+		{0x3714, 0x27},
+		{0x3730, 0x12},
+		{0x3733, 0x10},
+		{0x3764, 0x00},
+		{0x3765, 0x00},
+		{0x3769, 0x62},
+		{0x376a, 0x2a},
+		{0x376b, 0x30},
+		{0x3780, 0x00},
+		{0x3781, 0x24},
+		{0x3782, 0x00},
+		{0x3783, 0x23},
+		{0x3798, 0x2f},
+		{0x37a1, 0x60},
+		{0x37a8, 0x6a},
+		{0x37ab, 0x3f},
+		{0x37c2, 0x14},
+		{0x37c3, 0xf1},
+		{0x37c9, 0x80},
+		{0x37cb, 0x16},
+		{0x37cc, 0x16},
+		{0x37cd, 0x16},
+		{0x37ce, 0x16},
+		{0x3800, 0x00},
+		{0x3801, 0x00},
+		{0x3802, 0x00},
+		{0x3803, 0x00},
+		{0x3804, 0x0c},
+		{0x3805, 0xdf},
+		{0x3806, 0x09},
+		{0x3807, 0xaf},
+		{0x3808, 0x06},
+		{0x3809, 0x68},
+		{0x380a, 0x04},
+		{0x380b, 0xd0},
+		{0x380c, 0x0e},
+		{0x380d, 0xec},
+		{0x380e, 0x04},
+		{0x380f, 0xe8},
+		{0x3810, 0x00},
+		{0x3811, 0x04},
+		{0x3812, 0x00},
+		{0x3813, 0x05},
+		{0x3814, 0x03},
+		{0x3815, 0x01},
+		{0x3816, 0x00},
+		{0x3817, 0x00},
+		{0x3818, 0x00},
+		{0x3819, 0x10},
+		{0x3820, 0x90},
+		{0x3821, 0x67},
+		{0x382a, 0x03},
+		{0x382b, 0x01},
+		{0x3830, 0x06},
+		{0x3836, 0x02},
+		{0x3862, 0x04},
+		{0x3863, 0x08},
+		{0x3cc0, 0x33},
+		{0x3d85, 0x17},
+		{0x3d8c, 0x73},
+		{0x3d8d, 0xde},
+		{0x4001, 0xe0},
+		{0x4003, 0x40},
+		{0x4008, 0x00},
+		{0x4009, 0x05},
+		{0x400a, 0x00},
+		{0x400b, 0x84},
+		{0x400f, 0x80},
+		{0x4010, 0xf0},
+		{0x4011, 0xff},
+		{0x4012, 0x02},
+		{0x4013, 0x01},
+		{0x4014, 0x01},
+		{0x4015, 0x01},
+		{0x4042, 0x00},
+		{0x4043, 0x80},
+		{0x4044, 0x00},
+		{0x4045, 0x80},
+		{0x4046, 0x00},
+		{0x4047, 0x80},
+		{0x4048, 0x00},
+		{0x4049, 0x80},
+		{0x4041, 0x03},
+		{0x404c, 0x20},
+		{0x404d, 0x00},
+		{0x404e, 0x20},
+		{0x4203, 0x80},
+		{0x4307, 0x30},
+		{0x4317, 0x00},
+		{0x4503, 0x08},
+		{0x4601, 0x80},
+		{0x4800, 0x44},
+		{0x4816, 0x53},
+		{0x481b, 0x58},
+		{0x481f, 0x27},
+		{0x4837, 0x16},
+		{0x483c, 0x0f},
+		{0x484b, 0x05},
+		{0x5000, 0x57},
+		{0x5001, 0x0a},
+		{0x5004, 0x06},
+		{0x502e, 0x03},
+		{0x5030, 0x41},
+		{0x5780, 0x14},
+		{0x5781, 0x0f},
+		{0x5782, 0x44},
+		{0x5783, 0x02},
+		{0x5784, 0x01},
+		{0x5785, 0x01},
+		{0x5786, 0x00},
+		{0x5787, 0x04},
+		{0x5788, 0x02},
+		{0x5789, 0x0f},
+		{0x578a, 0xfd},
+		{0x578b, 0xf5},
+		{0x578c, 0xf5},
+		{0x578d, 0x03},
+		{0x578e, 0x08},
+		{0x578f, 0x0c},
+		{0x5790, 0x08},
+		{0x5791, 0x04},
+		{0x5792, 0x00},
+		{0x5793, 0x52},
+		{0x5794, 0xa3},
+		{0x5795, 0x00},
+		{0x5796, 0x10},
+		{0x5797, 0x10},
+		{0x5798, 0x73},
+		{0x5799, 0x73},
+		{0x579a, 0x00},
+		{0x579b, 0x28},
+		{0x579c, 0x00},
+		{0x579d, 0x16},
+		{0x579e, 0x06},
+		{0x579f, 0x20},
+		{0x57a0, 0x04},
+		{0x57a1, 0xa0},
+		{0x59f8, 0x3d},
+		{0x5a08, 0x02},
+		{0x5b00, 0x02},
+		{0x5b01, 0x10},
+		{0x5b02, 0x03},
+		{0x5b03, 0xcf},
+		{0x5b05, 0x6c},
+		{0x5e00, 0x00}
+};
+
+static const struct ov8856_reg lane_4_mode_3264x2448[] = {
+	/* 3264x2448 resolution */
+		{0x0103, 0x01},
+		{0x0302, 0x3c},
+		{0x0303, 0x01},
+		{0x031e, 0x0c},
+		{0x3000, 0x20},
+		{0x3003, 0x08},
+		{0x300e, 0x20},
+		{0x3010, 0x00},
+		{0x3015, 0x84},
+		{0x3018, 0x72},
+		{0x3021, 0x23},
+		{0x3033, 0x24},
+		{0x3500, 0x00},
+		{0x3501, 0x9a},
+		{0x3502, 0x20},
+		{0x3503, 0x08},
+		{0x3505, 0x83},
+		{0x3508, 0x01},
+		{0x3509, 0x80},
+		{0x350c, 0x00},
+		{0x350d, 0x80},
+		{0x350e, 0x04},
+		{0x350f, 0x00},
+		{0x3510, 0x00},
+		{0x3511, 0x02},
+		{0x3512, 0x00},
+		{0x3600, 0x72},
+		{0x3601, 0x40},
+		{0x3602, 0x30},
+		{0x3610, 0xc5},
+		{0x3611, 0x58},
+		{0x3612, 0x5c},
+		{0x3613, 0xca},
+		{0x3614, 0x60},
+		{0x3628, 0xff},
+		{0x3629, 0xff},
+		{0x362a, 0xff},
+		{0x3633, 0x10},
+		{0x3634, 0x10},
+		{0x3635, 0x10},
+		{0x3636, 0x10},
+		{0x3663, 0x08},
+		{0x3669, 0x34},
+		{0x366d, 0x00},
+		{0x366e, 0x10},
+		{0x3706, 0x86},
+		{0x370b, 0x7e},
+		{0x3714, 0x23},
+		{0x3730, 0x12},
+		{0x3733, 0x10},
+		{0x3764, 0x00},
+		{0x3765, 0x00},
+		{0x3769, 0x62},
+		{0x376a, 0x2a},
+		{0x376b, 0x30},
+		{0x3780, 0x00},
+		{0x3781, 0x24},
+		{0x3782, 0x00},
+		{0x3783, 0x23},
+		{0x3798, 0x2f},
+		{0x37a1, 0x60},
+		{0x37a8, 0x6a},
+		{0x37ab, 0x3f},
+		{0x37c2, 0x04},
+		{0x37c3, 0xf1},
+		{0x37c9, 0x80},
+		{0x37cb, 0x16},
+		{0x37cc, 0x16},
+		{0x37cd, 0x16},
+		{0x37ce, 0x16},
+		{0x3800, 0x00},
+		{0x3801, 0x00},
+		{0x3802, 0x00},
+		{0x3803, 0x0c},
+		{0x3804, 0x0c},
+		{0x3805, 0xdf},
+		{0x3806, 0x09},
+		{0x3807, 0xa3},
+		{0x3808, 0x0c},
+		{0x3809, 0xc0},
+		{0x380a, 0x09},
+		{0x380b, 0x90},
+		{0x380c, 0x07},
+		{0x380d, 0x8c},
+		{0x380e, 0x09},
+		{0x380f, 0xb2},
+		{0x3810, 0x00},
+		{0x3811, 0x04},
+		{0x3812, 0x00},
+		{0x3813, 0x02},
+		{0x3814, 0x01},
+		{0x3815, 0x01},
+		{0x3816, 0x00},
+		{0x3817, 0x00},
+		{0x3818, 0x00},
+		{0x3819, 0x10},
+		{0x3820, 0x80},
+		{0x3821, 0x46},
+		{0x382a, 0x01},
+		{0x382b, 0x01},
+		{0x3830, 0x06},
+		{0x3836, 0x02},
+		{0x3862, 0x04},
+		{0x3863, 0x08},
+		{0x3cc0, 0x33},
+		{0x3d85, 0x17},
+		{0x3d8c, 0x73},
+		{0x3d8d, 0xde},
+		{0x4001, 0xe0},
+		{0x4003, 0x40},
+		{0x4008, 0x00},
+		{0x4009, 0x0b},
+		{0x400a, 0x00},
+		{0x400b, 0x84},
+		{0x400f, 0x80},
+		{0x4010, 0xf0},
+		{0x4011, 0xff},
+		{0x4012, 0x02},
+		{0x4013, 0x01},
+		{0x4014, 0x01},
+		{0x4015, 0x01},
+		{0x4042, 0x00},
+		{0x4043, 0x80},
+		{0x4044, 0x00},
+		{0x4045, 0x80},
+		{0x4046, 0x00},
+		{0x4047, 0x80},
+		{0x4048, 0x00},
+		{0x4049, 0x80},
+		{0x4041, 0x03},
+		{0x404c, 0x20},
+		{0x404d, 0x00},
+		{0x404e, 0x20},
+		{0x4203, 0x80},
+		{0x4307, 0x30},
+		{0x4317, 0x00},
+		{0x4502, 0x50},
+		{0x4503, 0x08},
+		{0x4601, 0x80},
+		{0x4800, 0x44},
+		{0x4816, 0x53},
+		{0x481b, 0x50},
+		{0x481f, 0x27},
+		{0x4823, 0x3c},
+		{0x482b, 0x00},
+		{0x4831, 0x66},
+		{0x4837, 0x16},
+		{0x483c, 0x0f},
+		{0x484b, 0x05},
+		{0x5000, 0x77},
+		{0x5001, 0x0a},
+		{0x5003, 0xc8},
+		{0x5004, 0x04},
+		{0x5006, 0x00},
+		{0x5007, 0x00},
+		{0x502e, 0x03},
+		{0x5030, 0x41},
+		{0x5780, 0x14},
+		{0x5781, 0x0f},
+		{0x5782, 0x44},
+		{0x5783, 0x02},
+		{0x5784, 0x01},
+		{0x5785, 0x01},
+		{0x5786, 0x00},
+		{0x5787, 0x04},
+		{0x5788, 0x02},
+		{0x5789, 0x0f},
+		{0x578a, 0xfd},
+		{0x578b, 0xf5},
+		{0x578c, 0xf5},
+		{0x578d, 0x03},
+		{0x578e, 0x08},
+		{0x578f, 0x0c},
+		{0x5790, 0x08},
+		{0x5791, 0x04},
+		{0x5792, 0x00},
+		{0x5793, 0x52},
+		{0x5794, 0xa3},
+		{0x5795, 0x02},
+		{0x5796, 0x20},
+		{0x5797, 0x20},
+		{0x5798, 0xd5},
+		{0x5799, 0xd5},
+		{0x579a, 0x00},
+		{0x579b, 0x50},
+		{0x579c, 0x00},
+		{0x579d, 0x2c},
+		{0x579e, 0x0c},
+		{0x579f, 0x40},
+		{0x57a0, 0x09},
+		{0x57a1, 0x40},
+		{0x59f8, 0x3d},
+		{0x5a08, 0x02},
+		{0x5b00, 0x02},
+		{0x5b01, 0x10},
+		{0x5b02, 0x03},
+		{0x5b03, 0xcf},
+		{0x5b05, 0x6c},
+		{0x5e00, 0x00},
+		{0x5e10, 0xfc}
+};
+
+static const struct ov8856_reg lane_4_mode_1632x1224[] = {
+	/* 1632x1224 resolution */
+		{0x0103, 0x01},
+		{0x0302, 0x3c},
+		{0x0303, 0x01},
+		{0x031e, 0x0c},
+		{0x3000, 0x20},
+		{0x3003, 0x08},
+		{0x300e, 0x20},
+		{0x3010, 0x00},
+		{0x3015, 0x84},
+		{0x3018, 0x72},
+		{0x3021, 0x23},
+		{0x3033, 0x24},
+		{0x3500, 0x00},
+		{0x3501, 0x4c},
+		{0x3502, 0xe0},
+		{0x3503, 0x08},
+		{0x3505, 0x83},
+		{0x3508, 0x01},
+		{0x3509, 0x80},
+		{0x350c, 0x00},
+		{0x350d, 0x80},
+		{0x350e, 0x04},
+		{0x350f, 0x00},
+		{0x3510, 0x00},
+		{0x3511, 0x02},
+		{0x3512, 0x00},
+		{0x3600, 0x72},
+		{0x3601, 0x40},
+		{0x3602, 0x30},
+		{0x3610, 0xc5},
+		{0x3611, 0x58},
+		{0x3612, 0x5c},
+		{0x3613, 0xca},
+		{0x3614, 0x60},
+		{0x3628, 0xff},
+		{0x3629, 0xff},
+		{0x362a, 0xff},
+		{0x3633, 0x10},
+		{0x3634, 0x10},
+		{0x3635, 0x10},
+		{0x3636, 0x10},
+		{0x3663, 0x08},
+		{0x3669, 0x34},
+		{0x366d, 0x00},
+		{0x366e, 0x08},
+		{0x3706, 0x86},
+		{0x370b, 0x7e},
+		{0x3714, 0x27},
+		{0x3730, 0x12},
+		{0x3733, 0x10},
+		{0x3764, 0x00},
+		{0x3765, 0x00},
+		{0x3769, 0x62},
+		{0x376a, 0x2a},
+		{0x376b, 0x30},
+		{0x3780, 0x00},
+		{0x3781, 0x24},
+		{0x3782, 0x00},
+		{0x3783, 0x23},
+		{0x3798, 0x2f},
+		{0x37a1, 0x60},
+		{0x37a8, 0x6a},
+		{0x37ab, 0x3f},
+		{0x37c2, 0x14},
+		{0x37c3, 0xf1},
+		{0x37c9, 0x80},
+		{0x37cb, 0x16},
+		{0x37cc, 0x16},
+		{0x37cd, 0x16},
+		{0x37ce, 0x16},
+		{0x3800, 0x00},
+		{0x3801, 0x00},
+		{0x3802, 0x00},
+		{0x3803, 0x0c},
+		{0x3804, 0x0c},
+		{0x3805, 0xdf},
+		{0x3806, 0x09},
+		{0x3807, 0xa3},
+		{0x3808, 0x06},
+		{0x3809, 0x60},
+		{0x380a, 0x04},
+		{0x380b, 0xc8},
+		{0x380c, 0x07},
+		{0x380d, 0x8c},
+		{0x380e, 0x09},
+		{0x380f, 0xb2},
+		{0x3810, 0x00},
+		{0x3811, 0x02},
+		{0x3812, 0x00},
+		{0x3813, 0x02},
+		{0x3814, 0x03},
+		{0x3815, 0x01},
+		{0x3816, 0x00},
+		{0x3817, 0x00},
+		{0x3818, 0x00},
+		{0x3819, 0x10},
+		{0x3820, 0x80},
+		{0x3821, 0x47},
+		{0x382a, 0x03},
+		{0x382b, 0x01},
+		{0x3830, 0x06},
+		{0x3836, 0x02},
+		{0x3862, 0x04},
+		{0x3863, 0x08},
+		{0x3cc0, 0x33},
+		{0x3d85, 0x17},
+		{0x3d8c, 0x73},
+		{0x3d8d, 0xde},
+		{0x4001, 0xe0},
+		{0x4003, 0x40},
+		{0x4008, 0x00},
+		{0x4009, 0x05},
+		{0x400a, 0x00},
+		{0x400b, 0x84},
+		{0x400f, 0x80},
+		{0x4010, 0xf0},
+		{0x4011, 0xff},
+		{0x4012, 0x02},
+		{0x4013, 0x01},
+		{0x4014, 0x01},
+		{0x4015, 0x01},
+		{0x4042, 0x00},
+		{0x4043, 0x80},
+		{0x4044, 0x00},
+		{0x4045, 0x80},
+		{0x4046, 0x00},
+		{0x4047, 0x80},
+		{0x4048, 0x00},
+		{0x4049, 0x80},
+		{0x4041, 0x03},
+		{0x404c, 0x20},
+		{0x404d, 0x00},
+		{0x404e, 0x20},
+		{0x4203, 0x80},
+		{0x4307, 0x30},
+		{0x4317, 0x00},
+		{0x4502, 0x50},
+		{0x4503, 0x08},
+		{0x4601, 0x80},
+		{0x4800, 0x44},
+		{0x4816, 0x53},
+		{0x481b, 0x50},
+		{0x481f, 0x27},
+		{0x4823, 0x3c},
+		{0x482b, 0x00},
+		{0x4831, 0x66},
+		{0x4837, 0x16},
+		{0x483c, 0x0f},
+		{0x484b, 0x05},
+		{0x5000, 0x77},
+		{0x5001, 0x0a},
+		{0x5003, 0xc8},
+		{0x5004, 0x04},
+		{0x5006, 0x00},
+		{0x5007, 0x00},
+		{0x502e, 0x03},
+		{0x5030, 0x41},
+		{0x5795, 0x00},
+		{0x5796, 0x10},
+		{0x5797, 0x10},
+		{0x5798, 0x73},
+		{0x5799, 0x73},
+		{0x579a, 0x00},
+		{0x579b, 0x28},
+		{0x579c, 0x00},
+		{0x579d, 0x16},
+		{0x579e, 0x06},
+		{0x579f, 0x20},
+		{0x57a0, 0x04},
+		{0x57a1, 0xa0},
+		{0x5780, 0x14},
+		{0x5781, 0x0f},
+		{0x5782, 0x44},
+		{0x5783, 0x02},
+		{0x5784, 0x01},
+		{0x5785, 0x01},
+		{0x5786, 0x00},
+		{0x5787, 0x04},
+		{0x5788, 0x02},
+		{0x5789, 0x0f},
+		{0x578a, 0xfd},
+		{0x578b, 0xf5},
+		{0x578c, 0xf5},
+		{0x578d, 0x03},
+		{0x578e, 0x08},
+		{0x578f, 0x0c},
+		{0x5790, 0x08},
+		{0x5791, 0x04},
+		{0x5792, 0x00},
+		{0x5793, 0x52},
+		{0x5794, 0xa3},
+		{0x59f8, 0x3d},
+		{0x5a08, 0x02},
+		{0x5b00, 0x02},
+		{0x5b01, 0x10},
+		{0x5b02, 0x03},
+		{0x5b03, 0xcf},
+		{0x5b05, 0x6c},
+		{0x5e00, 0x00},
+		{0x5e10, 0xfc}
+};
+
+static const struct ov8856_reg mipi_data_mbus_sbggr10_1x10[] = {
+	{0x3813, 0x02},
+};
+
+static const struct ov8856_reg mipi_data_mbus_sgrbg10_1x10[] = {
+	{0x3813, 0x01},
+};
+
+static const u32 ov8856_mbus_codes[] = {
+	MEDIA_BUS_FMT_SBGGR10_1X10,
+	MEDIA_BUS_FMT_SGRBG10_1X10
+};
+
+static const char * const ov8856_test_pattern_menu[] = {
+	"Disabled",
+	"Standard Color Bar",
+	"Top-Bottom Darker Color Bar",
+	"Right-Left Darker Color Bar",
+	"Bottom-Top Darker Color Bar"
+};
+
+static const struct ov8856_reg_list bayer_offset_configs[] = {
+	[OV8856_MEDIA_BUS_FMT_SBGGR10_1X10] = {
+		.num_of_regs = ARRAY_SIZE(mipi_data_mbus_sbggr10_1x10),
+		.regs = mipi_data_mbus_sbggr10_1x10,
+	},
+	[OV8856_MEDIA_BUS_FMT_SGRBG10_1X10] = {
+		.num_of_regs = ARRAY_SIZE(mipi_data_mbus_sgrbg10_1x10),
+		.regs = mipi_data_mbus_sgrbg10_1x10,
+	}
+};
+
+struct ov8856 {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct v4l2_ctrl_handler ctrl_handler;
+
+	struct clk		*xvclk;
+	struct gpio_desc	*reset_gpio;
+	struct regulator_bulk_data supplies[ARRAY_SIZE(ov8856_supply_names)];
+
+	/* V4L2 Controls */
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *pixel_rate;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *exposure;
+
+	/* Current mode */
+	const struct ov8856_mode *cur_mode;
+
+	/* Application specified mbus format */
+	u32 cur_mbus_index;
+
+	/* To serialize asynchronus callbacks */
+	struct mutex mutex;
+
+	/* Streaming on/off */
+	bool streaming;
+
+	/* lanes index */
+	u8 nlanes;
+
+	const struct ov8856_lane_cfg *priv_lane;
+	u8 modes_size;
+
+	/* True if the device has been identified */
+	bool identified;
+};
+
+struct ov8856_lane_cfg {
+	const s64 link_freq_menu_items[2];
+	const struct ov8856_link_freq_config link_freq_configs[2];
+	const struct ov8856_mode supported_modes[4];
+};
+
+static const struct ov8856_lane_cfg lane_cfg_2 = {
+	{
+		720000000,
+		360000000,
+	},
+	{{
+		.reg_list = {
+			.num_of_regs =
+			ARRAY_SIZE(mipi_data_rate_lane_2.regs_0),
+			.regs = mipi_data_rate_lane_2.regs_0,
+		}
+	},
+	{
+		.reg_list = {
+			.num_of_regs =
+			ARRAY_SIZE(mipi_data_rate_lane_2.regs_1),
+			.regs = mipi_data_rate_lane_2.regs_1,
+		}
+	}},
+	{{
+		.width = 3280,
+		.height = 2464,
+		.hts = 1928,
+		.vts_def = 2488,
+		.vts_min = 2488,
+		.reg_list = {
+			.num_of_regs =
+			ARRAY_SIZE(lane_2_mode_3280x2464),
+			.regs = lane_2_mode_3280x2464,
+		},
+		.link_freq_index = 0,
+		.data_lanes = 2,
+		.default_mbus_index = OV8856_MEDIA_BUS_FMT_SGRBG10_1X10,
+	},
+	{
+		.width = 1640,
+		.height = 1232,
+		.hts = 3168,
+		.vts_def = 1514,
+		.vts_min = 1514,
+		.reg_list = {
+			.num_of_regs =
+			ARRAY_SIZE(lane_2_mode_1640x1232),
+			.regs = lane_2_mode_1640x1232,
+		},
+		.link_freq_index = 1,
+		.data_lanes = 2,
+		.default_mbus_index = OV8856_MEDIA_BUS_FMT_SGRBG10_1X10,
+	}}
+};
+
+static const struct ov8856_lane_cfg lane_cfg_4 = {
+		{
+			360000000,
+			180000000,
+		},
+		{{
+			.reg_list = {
+				.num_of_regs =
+				 ARRAY_SIZE(mipi_data_rate_lane_4.regs_0),
+				.regs = mipi_data_rate_lane_4.regs_0,
+			}
+		},
+		{
+			.reg_list = {
+				.num_of_regs =
+				 ARRAY_SIZE(mipi_data_rate_lane_4.regs_1),
+				.regs = mipi_data_rate_lane_4.regs_1,
+			}
+		}},
+		{{
+			.width = 3280,
+			.height = 2464,
+			.hts = 1928,
+			.vts_def = 2488,
+			.vts_min = 2488,
+			.reg_list = {
+				.num_of_regs =
+				 ARRAY_SIZE(lane_4_mode_3280x2464),
+				.regs = lane_4_mode_3280x2464,
+			},
+			.link_freq_index = 0,
+			.data_lanes = 4,
+			.default_mbus_index = OV8856_MEDIA_BUS_FMT_SGRBG10_1X10,
+		},
+		{
+			.width = 1640,
+			.height = 1232,
+			.hts = 3820,
+			.vts_def = 1256,
+			.vts_min = 1256,
+			.reg_list = {
+				.num_of_regs =
+				 ARRAY_SIZE(lane_4_mode_1640x1232),
+				.regs = lane_4_mode_1640x1232,
+			},
+			.link_freq_index = 1,
+			.data_lanes = 4,
+			.default_mbus_index = OV8856_MEDIA_BUS_FMT_SGRBG10_1X10,
+		},
+		{
+			.width = 3264,
+			.height = 2448,
+			.hts = 1932,
+			.vts_def = 2482,
+			.vts_min = 2482,
+			.reg_list = {
+				.num_of_regs =
+				 ARRAY_SIZE(lane_4_mode_3264x2448),
+				.regs = lane_4_mode_3264x2448,
+			},
+			.link_freq_index = 0,
+			.data_lanes = 4,
+			.default_mbus_index = OV8856_MEDIA_BUS_FMT_SBGGR10_1X10,
+		},
+		{
+			.width = 1632,
+			.height = 1224,
+			.hts = 1932,
+			.vts_def = 2482,
+			.vts_min = 2482,
+			.reg_list = {
+				.num_of_regs =
+				 ARRAY_SIZE(lane_4_mode_1632x1224),
+				.regs = lane_4_mode_1632x1224,
+			},
+			.link_freq_index = 1,
+			.data_lanes = 4,
+			.default_mbus_index = OV8856_MEDIA_BUS_FMT_SBGGR10_1X10,
+		}}
+};
+
+static unsigned int ov8856_modes_num(const struct ov8856 *ov8856)
+{
+	unsigned int i, count = 0;
+
+	for (i = 0; i < ARRAY_SIZE(ov8856->priv_lane->supported_modes); i++) {
+		if (ov8856->priv_lane->supported_modes[i].width == 0)
+			break;
+		count++;
+	}
+
+	return count;
+}
+
+static u64 to_rate(const s64 *link_freq_menu_items,
+		   u32 f_index, u8 nlanes)
+{
+	u64 pixel_rate = link_freq_menu_items[f_index] * 2 * nlanes;
+
+	do_div(pixel_rate, OV8856_RGB_DEPTH);
+
+	return pixel_rate;
+}
+
+static u64 to_pixels_per_line(const s64 *link_freq_menu_items, u32 hts,
+			      u32 f_index, u8 nlanes)
+{
+	u64 ppl = hts * to_rate(link_freq_menu_items, f_index, nlanes);
+
+	do_div(ppl, OV8856_SCLK);
+
+	return ppl;
+}
+
+static int ov8856_read_reg(struct ov8856 *ov8856, u16 reg, u16 len, u32 *val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov8856->sd);
+	struct i2c_msg msgs[2];
+	u8 addr_buf[2];
+	u8 data_buf[4] = {0};
+	int ret;
+
+	if (len > 4)
+		return -EINVAL;
+
+	put_unaligned_be16(reg, addr_buf);
+	msgs[0].addr = client->addr;
+	msgs[0].flags = 0;
+	msgs[0].len = sizeof(addr_buf);
+	msgs[0].buf = addr_buf;
+	msgs[1].addr = client->addr;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].len = len;
+	msgs[1].buf = &data_buf[4 - len];
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret != ARRAY_SIZE(msgs))
+		return -EIO;
+
+	*val = get_unaligned_be32(data_buf);
+
+	return 0;
+}
+
+static int ov8856_write_reg(struct ov8856 *ov8856, u16 reg, u16 len, u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov8856->sd);
+	u8 buf[6];
+
+	if (len > 4)
+		return -EINVAL;
+
+	put_unaligned_be16(reg, buf);
+	put_unaligned_be32(val << 8 * (4 - len), buf + 2);
+	if (i2c_master_send(client, buf, len + 2) != len + 2)
+		return -EIO;
+
+	return 0;
+}
+
+static int ov8856_write_reg_list(struct ov8856 *ov8856,
+				 const struct ov8856_reg_list *r_list)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov8856->sd);
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < r_list->num_of_regs; i++) {
+		ret = ov8856_write_reg(ov8856, r_list->regs[i].address, 1,
+				       r_list->regs[i].val);
+		if (ret) {
+			dev_err_ratelimited(&client->dev,
+				    "failed to write reg 0x%4.4x. error = %d",
+				    r_list->regs[i].address, ret);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int ov8856_identify_module(struct ov8856 *ov8856)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov8856->sd);
+	int ret;
+	u32 val;
+
+	if (ov8856->identified)
+		return 0;
+
+	ret = ov8856_read_reg(ov8856, OV8856_REG_CHIP_ID,
+			      OV8856_REG_VALUE_24BIT, &val);
+	if (ret)
+		return ret;
+
+	if (val != OV8856_CHIP_ID) {
+		dev_err(&client->dev, "chip id mismatch: %x!=%x",
+			OV8856_CHIP_ID, val);
+		return -ENXIO;
+	}
+
+	ov8856->identified = true;
+
+	return 0;
+}
+
+static int ov8856_update_digital_gain(struct ov8856 *ov8856, u32 d_gain)
+{
+	return ov8856_write_reg(ov8856, OV8856_REG_DIGITAL_GAIN,
+				OV8856_REG_VALUE_16BIT, d_gain);
+}
+
+static int ov8856_test_pattern(struct ov8856 *ov8856, u32 pattern)
+{
+	if (pattern)
+		pattern = (pattern - 1) << OV8856_TEST_PATTERN_BAR_SHIFT |
+			  OV8856_TEST_PATTERN_ENABLE;
+
+	return ov8856_write_reg(ov8856, OV8856_REG_TEST_PATTERN,
+				OV8856_REG_VALUE_08BIT, pattern);
+}
+
+static int ov8856_set_ctrl_hflip(struct ov8856 *ov8856, u32 ctrl_val)
+{
+	int ret;
+	u32 val;
+
+	ret = ov8856_read_reg(ov8856, OV8856_REG_MIRROR_OPT_1,
+			      OV8856_REG_VALUE_08BIT, &val);
+	if (ret)
+		return ret;
+
+	ret = ov8856_write_reg(ov8856, OV8856_REG_MIRROR_OPT_1,
+			       OV8856_REG_VALUE_08BIT,
+			       ctrl_val ? val & ~OV8856_REG_MIRROR_OP_2 :
+			       val | OV8856_REG_MIRROR_OP_2);
+
+	if (ret)
+		return ret;
+
+	ret = ov8856_read_reg(ov8856, OV8856_REG_FORMAT2,
+			      OV8856_REG_VALUE_08BIT, &val);
+	if (ret)
+		return ret;
+
+	return ov8856_write_reg(ov8856, OV8856_REG_FORMAT2,
+				OV8856_REG_VALUE_08BIT,
+				ctrl_val ? val & ~OV8856_REG_FORMAT2_OP_1 &
+				~OV8856_REG_FORMAT2_OP_2 &
+				~OV8856_REG_FORMAT2_OP_3 :
+				val | OV8856_REG_FORMAT2_OP_1 |
+				OV8856_REG_FORMAT2_OP_2 |
+				OV8856_REG_FORMAT2_OP_3);
+}
+
+static int ov8856_set_ctrl_vflip(struct ov8856 *ov8856, u8 ctrl_val)
+{
+	int ret;
+	u32 val;
+
+	ret = ov8856_read_reg(ov8856, OV8856_REG_FLIP_OPT_1,
+			      OV8856_REG_VALUE_08BIT, &val);
+	if (ret)
+		return ret;
+
+	ret = ov8856_write_reg(ov8856, OV8856_REG_FLIP_OPT_1,
+			       OV8856_REG_VALUE_08BIT,
+			       ctrl_val ? val | OV8856_REG_FLIP_OP_1 |
+			       OV8856_REG_FLIP_OP_2 :
+			       val & ~OV8856_REG_FLIP_OP_1 &
+			       ~OV8856_REG_FLIP_OP_2);
+
+	ret = ov8856_read_reg(ov8856, OV8856_REG_FLIP_OPT_2,
+			      OV8856_REG_VALUE_08BIT, &val);
+	if (ret)
+		return ret;
+
+	ret = ov8856_write_reg(ov8856, OV8856_REG_FLIP_OPT_2,
+			       OV8856_REG_VALUE_08BIT,
+			       ctrl_val ? val | OV8856_REG_FLIP_OP_2 :
+			       val & ~OV8856_REG_FLIP_OP_2);
+
+	ret = ov8856_read_reg(ov8856, OV8856_REG_FLIP_OPT_3,
+			      OV8856_REG_VALUE_08BIT, &val);
+	if (ret)
+		return ret;
+
+	ret = ov8856_write_reg(ov8856, OV8856_REG_FLIP_OPT_3,
+			       OV8856_REG_VALUE_08BIT,
+			       ctrl_val ? val & ~OV8856_REG_FLIP_OP_0 &
+			       ~OV8856_REG_FLIP_OP_1 :
+			       val | OV8856_REG_FLIP_OP_0 |
+			       OV8856_REG_FLIP_OP_1);
+
+	ret = ov8856_read_reg(ov8856, OV8856_REG_FORMAT1,
+			      OV8856_REG_VALUE_08BIT, &val);
+	if (ret)
+		return ret;
+
+	return ov8856_write_reg(ov8856, OV8856_REG_FORMAT1,
+			       OV8856_REG_VALUE_08BIT,
+			       ctrl_val ? val | OV8856_REG_FORMAT1_OP_1 |
+			       OV8856_REG_FORMAT1_OP_3 |
+			       OV8856_REG_FORMAT1_OP_2 :
+			       val & ~OV8856_REG_FORMAT1_OP_1 &
+			       ~OV8856_REG_FORMAT1_OP_3 &
+			       ~OV8856_REG_FORMAT1_OP_2);
+}
+
+static int ov8856_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov8856 *ov8856 = container_of(ctrl->handler,
+					     struct ov8856, ctrl_handler);
+	struct i2c_client *client = v4l2_get_subdevdata(&ov8856->sd);
+	s64 exposure_max;
+	int ret = 0;
+
+	/* Propagate change of current control to all related controls */
+	if (ctrl->id == V4L2_CID_VBLANK) {
+		/* Update max exposure while meeting expected vblanking */
+		exposure_max = ov8856->cur_mode->height + ctrl->val -
+			       OV8856_EXPOSURE_MAX_MARGIN;
+		__v4l2_ctrl_modify_range(ov8856->exposure,
+					 ov8856->exposure->minimum,
+					 exposure_max, ov8856->exposure->step,
+					 exposure_max);
+	}
+
+	/* V4L2 controls values will be applied only when power is already up */
+	if (!pm_runtime_get_if_in_use(&client->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_ANALOGUE_GAIN:
+		ret = ov8856_write_reg(ov8856, OV8856_REG_ANALOG_GAIN,
+				       OV8856_REG_VALUE_16BIT, ctrl->val);
+		break;
+
+	case V4L2_CID_DIGITAL_GAIN:
+		ret = ov8856_update_digital_gain(ov8856, ctrl->val);
+		break;
+
+	case V4L2_CID_EXPOSURE:
+		/* 4 least significant bits of expsoure are fractional part */
+		ret = ov8856_write_reg(ov8856, OV8856_REG_EXPOSURE,
+				       OV8856_REG_VALUE_24BIT, ctrl->val << 4);
+		break;
+
+	case V4L2_CID_VBLANK:
+		ret = ov8856_write_reg(ov8856, OV8856_REG_VTS,
+				       OV8856_REG_VALUE_16BIT,
+				       ov8856->cur_mode->height + ctrl->val);
+		break;
+
+	case V4L2_CID_TEST_PATTERN:
+		ret = ov8856_test_pattern(ov8856, ctrl->val);
+		break;
+
+	case V4L2_CID_HFLIP:
+		ret = ov8856_set_ctrl_hflip(ov8856, ctrl->val);
+		break;
+
+	case V4L2_CID_VFLIP:
+		ret = ov8856_set_ctrl_vflip(ov8856, ctrl->val);
+		break;
+
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	pm_runtime_put(&client->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ov8856_ctrl_ops = {
+	.s_ctrl = ov8856_set_ctrl,
+};
+
+static int ov8856_init_controls(struct ov8856 *ov8856)
+{
+	struct v4l2_ctrl_handler *ctrl_hdlr;
+	s64 exposure_max, h_blank;
+	int ret;
+
+	ctrl_hdlr = &ov8856->ctrl_handler;
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 8);
+	if (ret)
+		return ret;
+
+	ctrl_hdlr->lock = &ov8856->mutex;
+	ov8856->link_freq = v4l2_ctrl_new_int_menu(ctrl_hdlr, &ov8856_ctrl_ops,
+					   V4L2_CID_LINK_FREQ,
+					   ARRAY_SIZE
+					   (ov8856->priv_lane->link_freq_menu_items)
+					   - 1,
+					   0, ov8856->priv_lane->link_freq_menu_items);
+	if (ov8856->link_freq)
+		ov8856->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	ov8856->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, &ov8856_ctrl_ops,
+				       V4L2_CID_PIXEL_RATE, 0,
+				       to_rate(ov8856->priv_lane->link_freq_menu_items,
+					       0,
+					       ov8856->cur_mode->data_lanes), 1,
+				       to_rate(ov8856->priv_lane->link_freq_menu_items,
+					       0,
+					       ov8856->cur_mode->data_lanes));
+	ov8856->vblank = v4l2_ctrl_new_std(ctrl_hdlr, &ov8856_ctrl_ops,
+			  V4L2_CID_VBLANK,
+			  ov8856->cur_mode->vts_min - ov8856->cur_mode->height,
+			  OV8856_VTS_MAX - ov8856->cur_mode->height, 1,
+			  ov8856->cur_mode->vts_def -
+			  ov8856->cur_mode->height);
+	h_blank = to_pixels_per_line(ov8856->priv_lane->link_freq_menu_items,
+				     ov8856->cur_mode->hts,
+				     ov8856->cur_mode->link_freq_index,
+				     ov8856->cur_mode->data_lanes) -
+				     ov8856->cur_mode->width;
+	ov8856->hblank = v4l2_ctrl_new_std(ctrl_hdlr, &ov8856_ctrl_ops,
+					   V4L2_CID_HBLANK, h_blank, h_blank, 1,
+					   h_blank);
+	if (ov8856->hblank)
+		ov8856->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov8856_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
+			  OV8856_ANAL_GAIN_MIN, OV8856_ANAL_GAIN_MAX,
+			  OV8856_ANAL_GAIN_STEP, OV8856_ANAL_GAIN_MIN);
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov8856_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
+			  OV8856_DGTL_GAIN_MIN, OV8856_DGTL_GAIN_MAX,
+			  OV8856_DGTL_GAIN_STEP, OV8856_DGTL_GAIN_DEFAULT);
+	exposure_max = ov8856->cur_mode->vts_def - OV8856_EXPOSURE_MAX_MARGIN;
+	ov8856->exposure = v4l2_ctrl_new_std(ctrl_hdlr, &ov8856_ctrl_ops,
+					     V4L2_CID_EXPOSURE,
+					     OV8856_EXPOSURE_MIN, exposure_max,
+					     OV8856_EXPOSURE_STEP,
+					     exposure_max);
+	v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &ov8856_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(ov8856_test_pattern_menu) - 1,
+				     0, 0, ov8856_test_pattern_menu);
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov8856_ctrl_ops,
+			  V4L2_CID_HFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov8856_ctrl_ops,
+			  V4L2_CID_VFLIP, 0, 1, 1, 0);
+	if (ctrl_hdlr->error)
+		return ctrl_hdlr->error;
+
+	ov8856->sd.ctrl_handler = ctrl_hdlr;
+
+	return 0;
+}
+
+static void ov8856_update_pad_format(struct ov8856 *ov8856,
+				     const struct ov8856_mode *mode,
+				     struct v4l2_mbus_framefmt *fmt)
+{
+	int index;
+
+	fmt->width = mode->width;
+	fmt->height = mode->height;
+	for (index = 0; index < ARRAY_SIZE(ov8856_mbus_codes); ++index)
+		if (ov8856_mbus_codes[index] == fmt->code)
+			break;
+	if (index == ARRAY_SIZE(ov8856_mbus_codes))
+		index = mode->default_mbus_index;
+	fmt->code = ov8856_mbus_codes[index];
+	ov8856->cur_mbus_index = index;
+	fmt->field = V4L2_FIELD_NONE;
+}
+
+static int ov8856_start_streaming(struct ov8856 *ov8856)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov8856->sd);
+	const struct ov8856_reg_list *reg_list;
+	int link_freq_index, ret;
+
+	ret = ov8856_identify_module(ov8856);
+	if (ret)
+		return ret;
+
+	link_freq_index = ov8856->cur_mode->link_freq_index;
+	reg_list = &ov8856->priv_lane->link_freq_configs[link_freq_index].reg_list;
+
+	ret = ov8856_write_reg_list(ov8856, reg_list);
+	if (ret) {
+		dev_err(&client->dev, "failed to set plls");
+		return ret;
+	}
+
+	reg_list = &ov8856->cur_mode->reg_list;
+	ret = ov8856_write_reg_list(ov8856, reg_list);
+	if (ret) {
+		dev_err(&client->dev, "failed to set mode");
+		return ret;
+	}
+
+	reg_list = &bayer_offset_configs[ov8856->cur_mbus_index];
+	ret = ov8856_write_reg_list(ov8856, reg_list);
+	if (ret) {
+		dev_err(&client->dev, "failed to set mbus format");
+		return ret;
+	}
+
+	ret = __v4l2_ctrl_handler_setup(ov8856->sd.ctrl_handler);
+	if (ret)
+		return ret;
+
+	ret = ov8856_write_reg(ov8856, OV8856_REG_MODE_SELECT,
+			       OV8856_REG_VALUE_08BIT, OV8856_MODE_STREAMING);
+	if (ret) {
+		dev_err(&client->dev, "failed to set stream");
+		return ret;
+	}
+
+	return 0;
+}
+
+static void ov8856_stop_streaming(struct ov8856 *ov8856)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov8856->sd);
+
+	if (ov8856_write_reg(ov8856, OV8856_REG_MODE_SELECT,
+			     OV8856_REG_VALUE_08BIT, OV8856_MODE_STANDBY))
+		dev_err(&client->dev, "failed to set stream");
+}
+
+static int ov8856_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct ov8856 *ov8856 = to_ov8856(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	if (ov8856->streaming == enable)
+		return 0;
+
+	mutex_lock(&ov8856->mutex);
+	if (enable) {
+		ret = pm_runtime_resume_and_get(&client->dev);
+		if (ret < 0) {
+			mutex_unlock(&ov8856->mutex);
+			return ret;
+		}
+
+		ret = ov8856_start_streaming(ov8856);
+		if (ret) {
+			enable = 0;
+			ov8856_stop_streaming(ov8856);
+			pm_runtime_put(&client->dev);
+		}
+	} else {
+		ov8856_stop_streaming(ov8856);
+		pm_runtime_put(&client->dev);
+	}
+
+	ov8856->streaming = enable;
+	mutex_unlock(&ov8856->mutex);
+
+	return ret;
+}
+
+static int ov8856_power_on(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov8856 *ov8856 = to_ov8856(sd);
+	int ret;
+
+	if (is_acpi_node(dev_fwnode(dev)))
+		return 0;
+
+	ret = clk_prepare_enable(ov8856->xvclk);
+	if (ret < 0) {
+		dev_err(dev, "failed to enable xvclk\n");
+		return ret;
+	}
+
+	if (ov8856->reset_gpio) {
+		gpiod_set_value_cansleep(ov8856->reset_gpio, 1);
+		usleep_range(1000, 2000);
+	}
+
+	ret = regulator_bulk_enable(ARRAY_SIZE(ov8856_supply_names),
+				    ov8856->supplies);
+	if (ret < 0) {
+		dev_err(dev, "failed to enable regulators\n");
+		goto disable_clk;
+	}
+
+	gpiod_set_value_cansleep(ov8856->reset_gpio, 0);
+	usleep_range(1500, 1800);
+
+	return 0;
+
+disable_clk:
+	gpiod_set_value_cansleep(ov8856->reset_gpio, 1);
+	clk_disable_unprepare(ov8856->xvclk);
+
+	return ret;
+}
+
+static int ov8856_power_off(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov8856 *ov8856 = to_ov8856(sd);
+
+	if (is_acpi_node(dev_fwnode(dev)))
+		return 0;
+
+	gpiod_set_value_cansleep(ov8856->reset_gpio, 1);
+	regulator_bulk_disable(ARRAY_SIZE(ov8856_supply_names),
+			       ov8856->supplies);
+	clk_disable_unprepare(ov8856->xvclk);
+
+	return 0;
+}
+
+static int __maybe_unused ov8856_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov8856 *ov8856 = to_ov8856(sd);
+
+	mutex_lock(&ov8856->mutex);
+	if (ov8856->streaming)
+		ov8856_stop_streaming(ov8856);
+
+	ov8856_power_off(dev);
+	mutex_unlock(&ov8856->mutex);
+
+	return 0;
+}
+
+static int __maybe_unused ov8856_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov8856 *ov8856 = to_ov8856(sd);
+	int ret;
+
+	mutex_lock(&ov8856->mutex);
+
+	ov8856_power_on(dev);
+	if (ov8856->streaming) {
+		ret = ov8856_start_streaming(ov8856);
+		if (ret) {
+			ov8856->streaming = false;
+			ov8856_stop_streaming(ov8856);
+			mutex_unlock(&ov8856->mutex);
+			return ret;
+		}
+	}
+
+	mutex_unlock(&ov8856->mutex);
+
+	return 0;
+}
+
+static int ov8856_set_format(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *sd_state,
+			     struct v4l2_subdev_format *fmt)
+{
+	struct ov8856 *ov8856 = to_ov8856(sd);
+	const struct ov8856_mode *mode;
+	s32 vblank_def, h_blank;
+
+	mode = v4l2_find_nearest_size(ov8856->priv_lane->supported_modes,
+				      ov8856->modes_size,
+				      width, height, fmt->format.width,
+				      fmt->format.height);
+
+	mutex_lock(&ov8856->mutex);
+	ov8856_update_pad_format(ov8856, mode, &fmt->format);
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		*v4l2_subdev_get_try_format(sd, sd_state, fmt->pad) = fmt->format;
+	} else {
+		ov8856->cur_mode = mode;
+		__v4l2_ctrl_s_ctrl(ov8856->link_freq, mode->link_freq_index);
+		__v4l2_ctrl_s_ctrl_int64(ov8856->pixel_rate,
+					 to_rate(ov8856->priv_lane->link_freq_menu_items,
+						 mode->link_freq_index,
+						 ov8856->cur_mode->data_lanes));
+
+		/* Update limits and set FPS to default */
+		vblank_def = mode->vts_def - mode->height;
+		__v4l2_ctrl_modify_range(ov8856->vblank,
+					 mode->vts_min - mode->height,
+					 OV8856_VTS_MAX - mode->height, 1,
+					 vblank_def);
+		__v4l2_ctrl_s_ctrl(ov8856->vblank, vblank_def);
+		h_blank = to_pixels_per_line(ov8856->priv_lane->link_freq_menu_items,
+					     mode->hts,
+					     mode->link_freq_index,
+					     ov8856->cur_mode->data_lanes)
+					     - mode->width;
+		__v4l2_ctrl_modify_range(ov8856->hblank, h_blank, h_blank, 1,
+					 h_blank);
+	}
+
+	mutex_unlock(&ov8856->mutex);
+
+	return 0;
+}
+
+static int ov8856_get_format(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *sd_state,
+			     struct v4l2_subdev_format *fmt)
+{
+	struct ov8856 *ov8856 = to_ov8856(sd);
+
+	mutex_lock(&ov8856->mutex);
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
+		fmt->format = *v4l2_subdev_get_try_format(&ov8856->sd,
+							  sd_state,
+							  fmt->pad);
+	else
+		ov8856_update_pad_format(ov8856, ov8856->cur_mode, &fmt->format);
+
+	mutex_unlock(&ov8856->mutex);
+
+	return 0;
+}
+
+static int ov8856_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index >= ARRAY_SIZE(ov8856_mbus_codes))
+		return -EINVAL;
+
+	code->code = ov8856_mbus_codes[code->index];
+
+	return 0;
+}
+
+static int ov8856_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct ov8856 *ov8856 = to_ov8856(sd);
+	int index;
+
+	if (fse->index >= ov8856->modes_size)
+		return -EINVAL;
+
+	for (index = 0; index < ARRAY_SIZE(ov8856_mbus_codes); ++index)
+		if (fse->code == ov8856_mbus_codes[index])
+			break;
+	if (index == ARRAY_SIZE(ov8856_mbus_codes))
+		return -EINVAL;
+
+	fse->min_width = ov8856->priv_lane->supported_modes[fse->index].width;
+	fse->max_width = fse->min_width;
+	fse->min_height = ov8856->priv_lane->supported_modes[fse->index].height;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static int ov8856_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct ov8856 *ov8856 = to_ov8856(sd);
+
+	mutex_lock(&ov8856->mutex);
+	ov8856_update_pad_format(ov8856, &ov8856->priv_lane->supported_modes[0],
+				 v4l2_subdev_get_try_format(sd, fh->state, 0));
+	mutex_unlock(&ov8856->mutex);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops ov8856_video_ops = {
+	.s_stream = ov8856_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops ov8856_pad_ops = {
+	.set_fmt = ov8856_set_format,
+	.get_fmt = ov8856_get_format,
+	.enum_mbus_code = ov8856_enum_mbus_code,
+	.enum_frame_size = ov8856_enum_frame_size,
+};
+
+static const struct v4l2_subdev_ops ov8856_subdev_ops = {
+	.video = &ov8856_video_ops,
+	.pad = &ov8856_pad_ops,
+};
+
+static const struct media_entity_operations ov8856_subdev_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static const struct v4l2_subdev_internal_ops ov8856_internal_ops = {
+	.open = ov8856_open,
+};
+
+
+static int ov8856_get_hwcfg(struct ov8856 *ov8856, struct device *dev)
+{
+	struct fwnode_handle *ep;
+	struct fwnode_handle *fwnode = dev_fwnode(dev);
+	struct v4l2_fwnode_endpoint bus_cfg = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY
+	};
+	u32 xvclk_rate;
+	int ret;
+	unsigned int i, j;
+
+	if (!fwnode)
+		return -ENXIO;
+
+	ret = fwnode_property_read_u32(fwnode, "clock-frequency", &xvclk_rate);
+	if (ret)
+		return ret;
+
+	if (!is_acpi_node(fwnode)) {
+		ov8856->xvclk = devm_clk_get(dev, "xvclk");
+		if (IS_ERR(ov8856->xvclk)) {
+			dev_err(dev, "could not get xvclk clock (%pe)\n",
+				ov8856->xvclk);
+			return PTR_ERR(ov8856->xvclk);
+		}
+
+		clk_set_rate(ov8856->xvclk, xvclk_rate);
+		xvclk_rate = clk_get_rate(ov8856->xvclk);
+
+		ov8856->reset_gpio = devm_gpiod_get_optional(dev, "reset",
+							     GPIOD_OUT_LOW);
+		if (IS_ERR(ov8856->reset_gpio))
+			return PTR_ERR(ov8856->reset_gpio);
+
+		for (i = 0; i < ARRAY_SIZE(ov8856_supply_names); i++)
+			ov8856->supplies[i].supply = ov8856_supply_names[i];
+
+		ret = devm_regulator_bulk_get(dev,
+					      ARRAY_SIZE(ov8856_supply_names),
+					      ov8856->supplies);
+		if (ret)
+			return ret;
+	}
+
+	if (xvclk_rate != OV8856_XVCLK_19_2)
+		dev_warn(dev, "external clock rate %u is unsupported",
+			 xvclk_rate);
+
+	ep = fwnode_graph_get_next_endpoint(fwnode, NULL);
+	if (!ep)
+		return -ENXIO;
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(ep, &bus_cfg);
+	fwnode_handle_put(ep);
+	if (ret)
+		return ret;
+
+	/* Get number of data lanes */
+	if (bus_cfg.bus.mipi_csi2.num_data_lanes != 2 &&
+	    bus_cfg.bus.mipi_csi2.num_data_lanes != 4) {
+		dev_err(dev, "number of CSI2 data lanes %d is not supported",
+			bus_cfg.bus.mipi_csi2.num_data_lanes);
+		ret = -EINVAL;
+		goto check_hwcfg_error;
+	}
+
+	dev_dbg(dev, "Using %u data lanes\n", ov8856->cur_mode->data_lanes);
+
+	if (bus_cfg.bus.mipi_csi2.num_data_lanes == 2)
+		ov8856->priv_lane = &lane_cfg_2;
+	else
+		ov8856->priv_lane = &lane_cfg_4;
+
+	ov8856->modes_size = ov8856_modes_num(ov8856);
+
+	if (!bus_cfg.nr_of_link_frequencies) {
+		dev_err(dev, "no link frequencies defined");
+		ret = -EINVAL;
+		goto check_hwcfg_error;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(ov8856->priv_lane->link_freq_menu_items); i++) {
+		for (j = 0; j < bus_cfg.nr_of_link_frequencies; j++) {
+			if (ov8856->priv_lane->link_freq_menu_items[i] ==
+			    bus_cfg.link_frequencies[j])
+				break;
+		}
+
+		if (j == bus_cfg.nr_of_link_frequencies) {
+			dev_err(dev, "no link frequency %lld supported",
+				ov8856->priv_lane->link_freq_menu_items[i]);
+			ret = -EINVAL;
+			goto check_hwcfg_error;
+		}
+	}
+
+check_hwcfg_error:
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+
+	return ret;
+}
+
+static void ov8856_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov8856 *ov8856 = to_ov8856(sd);
+
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+	pm_runtime_disable(&client->dev);
+	mutex_destroy(&ov8856->mutex);
+
+	ov8856_power_off(&client->dev);
+}
+
+static int ov8856_probe(struct i2c_client *client)
+{
+	struct ov8856 *ov8856;
+	int ret;
+	bool full_power;
+
+	ov8856 = devm_kzalloc(&client->dev, sizeof(*ov8856), GFP_KERNEL);
+	if (!ov8856)
+		return -ENOMEM;
+
+	ret = ov8856_get_hwcfg(ov8856, &client->dev);
+	if (ret) {
+		dev_err(&client->dev, "failed to get HW configuration: %d",
+			ret);
+		return ret;
+	}
+
+	v4l2_i2c_subdev_init(&ov8856->sd, client, &ov8856_subdev_ops);
+
+	full_power = acpi_dev_state_d0(&client->dev);
+	if (full_power) {
+		ret = ov8856_power_on(&client->dev);
+		if (ret) {
+			dev_err(&client->dev, "failed to power on\n");
+			return ret;
+		}
+
+		ret = ov8856_identify_module(ov8856);
+		if (ret) {
+			dev_err(&client->dev, "failed to find sensor: %d", ret);
+			goto probe_power_off;
+		}
+	}
+
+	mutex_init(&ov8856->mutex);
+	ov8856->cur_mode = &ov8856->priv_lane->supported_modes[0];
+	ov8856->cur_mbus_index = ov8856->cur_mode->default_mbus_index;
+	ret = ov8856_init_controls(ov8856);
+	if (ret) {
+		dev_err(&client->dev, "failed to init controls: %d", ret);
+		goto probe_error_v4l2_ctrl_handler_free;
+	}
+
+	ov8856->sd.internal_ops = &ov8856_internal_ops;
+	ov8856->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	ov8856->sd.entity.ops = &ov8856_subdev_entity_ops;
+	ov8856->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ov8856->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&ov8856->sd.entity, 1, &ov8856->pad);
+	if (ret) {
+		dev_err(&client->dev, "failed to init entity pads: %d", ret);
+		goto probe_error_v4l2_ctrl_handler_free;
+	}
+
+	ret = v4l2_async_register_subdev_sensor(&ov8856->sd);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed to register V4L2 subdev: %d",
+			ret);
+		goto probe_error_media_entity_cleanup;
+	}
+
+	/* Set the device's state to active if it's in D0 state. */
+	if (full_power)
+		pm_runtime_set_active(&client->dev);
+	pm_runtime_enable(&client->dev);
+	pm_runtime_idle(&client->dev);
+
+	return 0;
+
+probe_error_media_entity_cleanup:
+	media_entity_cleanup(&ov8856->sd.entity);
+
+probe_error_v4l2_ctrl_handler_free:
+	v4l2_ctrl_handler_free(ov8856->sd.ctrl_handler);
+	mutex_destroy(&ov8856->mutex);
+
+probe_power_off:
+	ov8856_power_off(&client->dev);
+
+	return ret;
+}
+
+static const struct dev_pm_ops ov8856_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(ov8856_suspend, ov8856_resume)
+	SET_RUNTIME_PM_OPS(ov8856_power_off, ov8856_power_on, NULL)
+};
+
+#ifdef CONFIG_ACPI
+static const struct acpi_device_id ov8856_acpi_ids[] = {
+	{"OVTI8856"},
+	{}
+};
+
+MODULE_DEVICE_TABLE(acpi, ov8856_acpi_ids);
+#endif
+
+static const struct of_device_id ov8856_of_match[] = {
+	{ .compatible = "ovti,ov8856" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, ov8856_of_match);
+
+static struct i2c_driver ov8856_i2c_driver = {
+	.driver = {
+		.name = "ov8856",
+		.pm = &ov8856_pm_ops,
+		.acpi_match_table = ACPI_PTR(ov8856_acpi_ids),
+		.of_match_table = ov8856_of_match,
+	},
+	.probe = ov8856_probe,
+	.remove = ov8856_remove,
+	.flags = I2C_DRV_ACPI_WAIVE_D0_PROBE,
+};
+
+module_i2c_driver(ov8856_i2c_driver);
+
+MODULE_AUTHOR("Ben Kao <ben.kao@intel.com>");
+MODULE_DESCRIPTION("OmniVision OV8856 sensor driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/ov8858.c b/drivers/media/i2c/ov8858.c
new file mode 100644
index 000000000..4d9fd76e2
--- /dev/null
+++ b/drivers/media/i2c/ov8858.c
@@ -0,0 +1,2002 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2023 Jacopo Mondi <jacopo.mondi@ideasonboard.com>
+ * Copyright (C) 2022 Nicholas Roth <nicholas@rothemail.net>
+ * Copyright (C) 2017 Fuzhou Rockchip Electronics Co., Ltd.
+ */
+
+#include <asm/unaligned.h>
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/pm_runtime.h>
+#include <linux/property.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+
+#include <media/media-entity.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-mediabus.h>
+#include <media/v4l2-subdev.h>
+
+#define OV8858_LINK_FREQ		360000000U
+#define OV8858_XVCLK_FREQ		24000000
+
+#define OV8858_REG_SIZE_SHIFT		16
+#define OV8858_REG_ADDR_MASK		0xffff
+#define OV8858_REG_8BIT(n)		((1U << OV8858_REG_SIZE_SHIFT) | (n))
+#define OV8858_REG_16BIT(n)		((2U << OV8858_REG_SIZE_SHIFT) | (n))
+#define OV8858_REG_24BIT(n)		((3U << OV8858_REG_SIZE_SHIFT) | (n))
+
+#define OV8858_REG_SC_CTRL0100		OV8858_REG_8BIT(0x0100)
+#define OV8858_MODE_SW_STANDBY		0x0
+#define OV8858_MODE_STREAMING		0x1
+
+#define OV8858_REG_CHIP_ID		OV8858_REG_24BIT(0x300a)
+#define OV8858_CHIP_ID			0x008858
+
+#define OV8858_REG_SUB_ID		OV8858_REG_8BIT(0x302a)
+#define OV8858_R1A			0xb0
+#define OV8858_R2A			0xb2
+
+#define OV8858_REG_LONG_EXPO		OV8858_REG_24BIT(0x3500)
+#define OV8858_EXPOSURE_MIN		4
+#define OV8858_EXPOSURE_STEP		1
+#define OV8858_EXPOSURE_MARGIN		4
+
+#define OV8858_REG_LONG_GAIN		OV8858_REG_16BIT(0x3508)
+#define OV8858_LONG_GAIN_MIN		0x0
+#define OV8858_LONG_GAIN_MAX		0x7ff
+#define OV8858_LONG_GAIN_STEP		1
+#define OV8858_LONG_GAIN_DEFAULT	0x80
+
+#define OV8858_REG_LONG_DIGIGAIN	OV8858_REG_16BIT(0x350a)
+#define OV8858_LONG_DIGIGAIN_H_MASK	0x3fc0
+#define OV8858_LONG_DIGIGAIN_L_MASK	0x3f
+#define OV8858_LONG_DIGIGAIN_H_SHIFT	2
+#define OV8858_LONG_DIGIGAIN_MIN	0x0
+#define OV8858_LONG_DIGIGAIN_MAX	0x3fff
+#define OV8858_LONG_DIGIGAIN_STEP	1
+#define OV8858_LONG_DIGIGAIN_DEFAULT	0x200
+
+#define OV8858_REG_VTS			OV8858_REG_16BIT(0x380e)
+#define OV8858_VTS_MAX			0x7fff
+
+#define OV8858_REG_TEST_PATTERN		OV8858_REG_8BIT(0x5e00)
+#define OV8858_TEST_PATTERN_ENABLE	0x80
+#define OV8858_TEST_PATTERN_DISABLE	0x0
+
+#define REG_NULL			0xffff
+
+static const char * const ov8858_supply_names[] = {
+	"avdd",		/* Analog power */
+	"dovdd",	/* Digital I/O power */
+	"dvdd",		/* Digital core power */
+};
+
+struct regval {
+	u16 addr;
+	u8 val;
+};
+
+struct regval_modes {
+	const struct regval *mode_2lanes;
+	const struct regval *mode_4lanes;
+};
+
+struct ov8858_mode {
+	u32 width;
+	u32 height;
+	u32 hts_def;
+	u32 vts_def;
+	u32 exp_def;
+	const struct regval_modes reg_modes;
+};
+
+struct ov8858 {
+	struct clk		*xvclk;
+	struct gpio_desc	*reset_gpio;
+	struct gpio_desc	*pwdn_gpio;
+	struct regulator_bulk_data supplies[ARRAY_SIZE(ov8858_supply_names)];
+
+	struct v4l2_subdev	subdev;
+	struct media_pad	pad;
+
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_ctrl	*exposure;
+	struct v4l2_ctrl	*hblank;
+	struct v4l2_ctrl	*vblank;
+
+	const struct regval	*global_regs;
+
+	unsigned int		num_lanes;
+};
+
+static inline struct ov8858 *sd_to_ov8858(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct ov8858, subdev);
+}
+
+static const struct regval ov8858_global_regs_r1a[] = {
+	{0x0100, 0x00},
+	{0x0100, 0x00},
+	{0x0100, 0x00},
+	{0x0100, 0x00},
+	{0x0302, 0x1e},
+	{0x0303, 0x00},
+	{0x0304, 0x03},
+	{0x030e, 0x00},
+	{0x030f, 0x09},
+	{0x0312, 0x01},
+	{0x031e, 0x0c},
+	{0x3600, 0x00},
+	{0x3601, 0x00},
+	{0x3602, 0x00},
+	{0x3603, 0x00},
+	{0x3604, 0x22},
+	{0x3605, 0x30},
+	{0x3606, 0x00},
+	{0x3607, 0x20},
+	{0x3608, 0x11},
+	{0x3609, 0x28},
+	{0x360a, 0x00},
+	{0x360b, 0x06},
+	{0x360c, 0xdc},
+	{0x360d, 0x40},
+	{0x360e, 0x0c},
+	{0x360f, 0x20},
+	{0x3610, 0x07},
+	{0x3611, 0x20},
+	{0x3612, 0x88},
+	{0x3613, 0x80},
+	{0x3614, 0x58},
+	{0x3615, 0x00},
+	{0x3616, 0x4a},
+	{0x3617, 0xb0},
+	{0x3618, 0x56},
+	{0x3619, 0x70},
+	{0x361a, 0x99},
+	{0x361b, 0x00},
+	{0x361c, 0x07},
+	{0x361d, 0x00},
+	{0x361e, 0x00},
+	{0x361f, 0x00},
+	{0x3638, 0xff},
+	{0x3633, 0x0c},
+	{0x3634, 0x0c},
+	{0x3635, 0x0c},
+	{0x3636, 0x0c},
+	{0x3645, 0x13},
+	{0x3646, 0x83},
+	{0x364a, 0x07},
+	{0x3015, 0x01},
+	{0x3018, 0x32},
+	{0x3020, 0x93},
+	{0x3022, 0x01},
+	{0x3031, 0x0a},
+	{0x3034, 0x00},
+	{0x3106, 0x01},
+	{0x3305, 0xf1},
+	{0x3308, 0x00},
+	{0x3309, 0x28},
+	{0x330a, 0x00},
+	{0x330b, 0x20},
+	{0x330c, 0x00},
+	{0x330d, 0x00},
+	{0x330e, 0x00},
+	{0x330f, 0x40},
+	{0x3307, 0x04},
+	{0x3500, 0x00},
+	{0x3501, 0x4d},
+	{0x3502, 0x40},
+	{0x3503, 0x00},
+	{0x3505, 0x80},
+	{0x3508, 0x04},
+	{0x3509, 0x00},
+	{0x350c, 0x00},
+	{0x350d, 0x80},
+	{0x3510, 0x00},
+	{0x3511, 0x02},
+	{0x3512, 0x00},
+	{0x3700, 0x18},
+	{0x3701, 0x0c},
+	{0x3702, 0x28},
+	{0x3703, 0x19},
+	{0x3704, 0x14},
+	{0x3705, 0x00},
+	{0x3706, 0x35},
+	{0x3707, 0x04},
+	{0x3708, 0x24},
+	{0x3709, 0x33},
+	{0x370a, 0x00},
+	{0x370b, 0xb5},
+	{0x370c, 0x04},
+	{0x3718, 0x12},
+	{0x3719, 0x31},
+	{0x3712, 0x42},
+	{0x3714, 0x24},
+	{0x371e, 0x19},
+	{0x371f, 0x40},
+	{0x3720, 0x05},
+	{0x3721, 0x05},
+	{0x3724, 0x06},
+	{0x3725, 0x01},
+	{0x3726, 0x06},
+	{0x3728, 0x05},
+	{0x3729, 0x02},
+	{0x372a, 0x03},
+	{0x372b, 0x53},
+	{0x372c, 0xa3},
+	{0x372d, 0x53},
+	{0x372e, 0x06},
+	{0x372f, 0x10},
+	{0x3730, 0x01},
+	{0x3731, 0x06},
+	{0x3732, 0x14},
+	{0x3733, 0x10},
+	{0x3734, 0x40},
+	{0x3736, 0x20},
+	{0x373a, 0x05},
+	{0x373b, 0x06},
+	{0x373c, 0x0a},
+	{0x373e, 0x03},
+	{0x3755, 0x10},
+	{0x3758, 0x00},
+	{0x3759, 0x4c},
+	{0x375a, 0x06},
+	{0x375b, 0x13},
+	{0x375c, 0x20},
+	{0x375d, 0x02},
+	{0x375e, 0x00},
+	{0x375f, 0x14},
+	{0x3768, 0x22},
+	{0x3769, 0x44},
+	{0x376a, 0x44},
+	{0x3761, 0x00},
+	{0x3762, 0x00},
+	{0x3763, 0x00},
+	{0x3766, 0xff},
+	{0x376b, 0x00},
+	{0x3772, 0x23},
+	{0x3773, 0x02},
+	{0x3774, 0x16},
+	{0x3775, 0x12},
+	{0x3776, 0x04},
+	{0x3777, 0x00},
+	{0x3778, 0x1b},
+	{0x37a0, 0x44},
+	{0x37a1, 0x3d},
+	{0x37a2, 0x3d},
+	{0x37a3, 0x00},
+	{0x37a4, 0x00},
+	{0x37a5, 0x00},
+	{0x37a6, 0x00},
+	{0x37a7, 0x44},
+	{0x37a8, 0x4c},
+	{0x37a9, 0x4c},
+	{0x3760, 0x00},
+	{0x376f, 0x01},
+	{0x37aa, 0x44},
+	{0x37ab, 0x2e},
+	{0x37ac, 0x2e},
+	{0x37ad, 0x33},
+	{0x37ae, 0x0d},
+	{0x37af, 0x0d},
+	{0x37b0, 0x00},
+	{0x37b1, 0x00},
+	{0x37b2, 0x00},
+	{0x37b3, 0x42},
+	{0x37b4, 0x42},
+	{0x37b5, 0x33},
+	{0x37b6, 0x00},
+	{0x37b7, 0x00},
+	{0x37b8, 0x00},
+	{0x37b9, 0xff},
+	{0x3800, 0x00},
+	{0x3801, 0x0c},
+	{0x3802, 0x00},
+	{0x3803, 0x0c},
+	{0x3804, 0x0c},
+	{0x3805, 0xd3},
+	{0x3806, 0x09},
+	{0x3807, 0xa3},
+	{0x3808, 0x06},
+	{0x3809, 0x60},
+	{0x380a, 0x04},
+	{0x380b, 0xc8},
+	{0x380c, 0x07},
+	{0x380d, 0x88},
+	{0x380e, 0x04},
+	{0x380f, 0xdc},
+	{0x3810, 0x00},
+	{0x3811, 0x04},
+	{0x3813, 0x02},
+	{0x3814, 0x03},
+	{0x3815, 0x01},
+	{0x3820, 0x00},
+	{0x3821, 0x67},
+	{0x382a, 0x03},
+	{0x382b, 0x01},
+	{0x3830, 0x08},
+	{0x3836, 0x02},
+	{0x3837, 0x18},
+	{0x3841, 0xff},
+	{0x3846, 0x48},
+	{0x3d85, 0x14},
+	{0x3f08, 0x08},
+	{0x3f0a, 0x80},
+	{0x4000, 0xf1},
+	{0x4001, 0x10},
+	{0x4005, 0x10},
+	{0x4002, 0x27},
+	{0x4009, 0x81},
+	{0x400b, 0x0c},
+	{0x401b, 0x00},
+	{0x401d, 0x00},
+	{0x4020, 0x00},
+	{0x4021, 0x04},
+	{0x4022, 0x04},
+	{0x4023, 0xb9},
+	{0x4024, 0x05},
+	{0x4025, 0x2a},
+	{0x4026, 0x05},
+	{0x4027, 0x2b},
+	{0x4028, 0x00},
+	{0x4029, 0x02},
+	{0x402a, 0x04},
+	{0x402b, 0x04},
+	{0x402c, 0x02},
+	{0x402d, 0x02},
+	{0x402e, 0x08},
+	{0x402f, 0x02},
+	{0x401f, 0x00},
+	{0x4034, 0x3f},
+	{0x403d, 0x04},
+	{0x4300, 0xff},
+	{0x4301, 0x00},
+	{0x4302, 0x0f},
+	{0x4316, 0x00},
+	{0x4500, 0x38},
+	{0x4503, 0x18},
+	{0x4600, 0x00},
+	{0x4601, 0xcb},
+	{0x481f, 0x32},
+	{0x4837, 0x16},
+	{0x4850, 0x10},
+	{0x4851, 0x32},
+	{0x4b00, 0x2a},
+	{0x4b0d, 0x00},
+	{0x4d00, 0x04},
+	{0x4d01, 0x18},
+	{0x4d02, 0xc3},
+	{0x4d03, 0xff},
+	{0x4d04, 0xff},
+	{0x4d05, 0xff},
+	{0x5000, 0x7e},
+	{0x5001, 0x01},
+	{0x5002, 0x08},
+	{0x5003, 0x20},
+	{0x5046, 0x12},
+	{0x5901, 0x00},
+	{0x5e00, 0x00},
+	{0x5e01, 0x41},
+	{0x382d, 0x7f},
+	{0x4825, 0x3a},
+	{0x4826, 0x40},
+	{0x4808, 0x25},
+	{REG_NULL, 0x00},
+};
+
+static const struct regval ov8858_global_regs_r2a_2lane[] = {
+	/*
+	 * MIPI=720Mbps, SysClk=144Mhz,Dac Clock=360Mhz.
+	 * v00_01_00 (05/29/2014) : initial setting
+	 * AM19 : 3617 <- 0xC0
+	 * AM20 : change FWC_6K_EN to be default 0x3618=0x5a
+	 */
+	{0x0103, 0x01}, /* software reset */
+	{0x0100, 0x00}, /* software standby */
+	{0x0302, 0x1e}, /* pll1_multi */
+	{0x0303, 0x00}, /* pll1_divm */
+	{0x0304, 0x03}, /* pll1_div_mipi */
+	{0x030e, 0x02}, /* pll2_rdiv */
+	{0x030f, 0x04}, /* pll2_divsp */
+	{0x0312, 0x03}, /* pll2_pre_div0, pll2_r_divdac */
+	{0x031e, 0x0c}, /* pll1_no_lat */
+	{0x3600, 0x00},
+	{0x3601, 0x00},
+	{0x3602, 0x00},
+	{0x3603, 0x00},
+	{0x3604, 0x22},
+	{0x3605, 0x20},
+	{0x3606, 0x00},
+	{0x3607, 0x20},
+	{0x3608, 0x11},
+	{0x3609, 0x28},
+	{0x360a, 0x00},
+	{0x360b, 0x05},
+	{0x360c, 0xd4},
+	{0x360d, 0x40},
+	{0x360e, 0x0c},
+	{0x360f, 0x20},
+	{0x3610, 0x07},
+	{0x3611, 0x20},
+	{0x3612, 0x88},
+	{0x3613, 0x80},
+	{0x3614, 0x58},
+	{0x3615, 0x00},
+	{0x3616, 0x4a},
+	{0x3617, 0x90},
+	{0x3618, 0x5a},
+	{0x3619, 0x70},
+	{0x361a, 0x99},
+	{0x361b, 0x0a},
+	{0x361c, 0x07},
+	{0x361d, 0x00},
+	{0x361e, 0x00},
+	{0x361f, 0x00},
+	{0x3638, 0xff},
+	{0x3633, 0x0f},
+	{0x3634, 0x0f},
+	{0x3635, 0x0f},
+	{0x3636, 0x12},
+	{0x3645, 0x13},
+	{0x3646, 0x83},
+	{0x364a, 0x07},
+	{0x3015, 0x00},
+	{0x3018, 0x32}, /* MIPI 2 lane */
+	{0x3020, 0x93}, /* Clock switch output normal, pclk_div =/1 */
+	{0x3022, 0x01}, /* pd_mipi enable when rst_sync */
+	{0x3031, 0x0a}, /* MIPI 10-bit mode */
+	{0x3034, 0x00},
+	{0x3106, 0x01}, /* sclk_div, sclk_pre_div */
+	{0x3305, 0xf1},
+	{0x3308, 0x00},
+	{0x3309, 0x28},
+	{0x330a, 0x00},
+	{0x330b, 0x20},
+	{0x330c, 0x00},
+	{0x330d, 0x00},
+	{0x330e, 0x00},
+	{0x330f, 0x40},
+	{0x3307, 0x04},
+	{0x3500, 0x00}, /* exposure H */
+	{0x3501, 0x4d}, /* exposure M */
+	{0x3502, 0x40}, /* exposure L */
+	{0x3503, 0x80}, /* gain delay ?, exposure delay 1 frame, real gain */
+	{0x3505, 0x80}, /* gain option */
+	{0x3508, 0x02}, /* gain H */
+	{0x3509, 0x00}, /* gain L */
+	{0x350c, 0x00}, /* short gain H */
+	{0x350d, 0x80}, /* short gain L */
+	{0x3510, 0x00}, /* short exposure H */
+	{0x3511, 0x02}, /* short exposure M */
+	{0x3512, 0x00}, /* short exposure L */
+	{0x3700, 0x18},
+	{0x3701, 0x0c},
+	{0x3702, 0x28},
+	{0x3703, 0x19},
+	{0x3704, 0x14},
+	{0x3705, 0x00},
+	{0x3706, 0x82},
+	{0x3707, 0x04},
+	{0x3708, 0x24},
+	{0x3709, 0x33},
+	{0x370a, 0x01},
+	{0x370b, 0x82},
+	{0x370c, 0x04},
+	{0x3718, 0x12},
+	{0x3719, 0x31},
+	{0x3712, 0x42},
+	{0x3714, 0x24},
+	{0x371e, 0x19},
+	{0x371f, 0x40},
+	{0x3720, 0x05},
+	{0x3721, 0x05},
+	{0x3724, 0x06},
+	{0x3725, 0x01},
+	{0x3726, 0x06},
+	{0x3728, 0x05},
+	{0x3729, 0x02},
+	{0x372a, 0x03},
+	{0x372b, 0x53},
+	{0x372c, 0xa3},
+	{0x372d, 0x53},
+	{0x372e, 0x06},
+	{0x372f, 0x10},
+	{0x3730, 0x01},
+	{0x3731, 0x06},
+	{0x3732, 0x14},
+	{0x3733, 0x10},
+	{0x3734, 0x40},
+	{0x3736, 0x20},
+	{0x373a, 0x05},
+	{0x373b, 0x06},
+	{0x373c, 0x0a},
+	{0x373e, 0x03},
+	{0x3750, 0x0a},
+	{0x3751, 0x0e},
+	{0x3755, 0x10},
+	{0x3758, 0x00},
+	{0x3759, 0x4c},
+	{0x375a, 0x06},
+	{0x375b, 0x13},
+	{0x375c, 0x20},
+	{0x375d, 0x02},
+	{0x375e, 0x00},
+	{0x375f, 0x14},
+	{0x3768, 0x22},
+	{0x3769, 0x44},
+	{0x376a, 0x44},
+	{0x3761, 0x00},
+	{0x3762, 0x00},
+	{0x3763, 0x00},
+	{0x3766, 0xff},
+	{0x376b, 0x00},
+	{0x3772, 0x23},
+	{0x3773, 0x02},
+	{0x3774, 0x16},
+	{0x3775, 0x12},
+	{0x3776, 0x04},
+	{0x3777, 0x00},
+	{0x3778, 0x17},
+	{0x37a0, 0x44},
+	{0x37a1, 0x3d},
+	{0x37a2, 0x3d},
+	{0x37a3, 0x00},
+	{0x37a4, 0x00},
+	{0x37a5, 0x00},
+	{0x37a6, 0x00},
+	{0x37a7, 0x44},
+	{0x37a8, 0x4c},
+	{0x37a9, 0x4c},
+	{0x3760, 0x00},
+	{0x376f, 0x01},
+	{0x37aa, 0x44},
+	{0x37ab, 0x2e},
+	{0x37ac, 0x2e},
+	{0x37ad, 0x33},
+	{0x37ae, 0x0d},
+	{0x37af, 0x0d},
+	{0x37b0, 0x00},
+	{0x37b1, 0x00},
+	{0x37b2, 0x00},
+	{0x37b3, 0x42},
+	{0x37b4, 0x42},
+	{0x37b5, 0x31},
+	{0x37b6, 0x00},
+	{0x37b7, 0x00},
+	{0x37b8, 0x00},
+	{0x37b9, 0xff},
+	{0x3800, 0x00}, /* x start H */
+	{0x3801, 0x0c}, /* x start L */
+	{0x3802, 0x00}, /* y start H */
+	{0x3803, 0x0c}, /* y start L */
+	{0x3804, 0x0c}, /* x end H */
+	{0x3805, 0xd3}, /* x end L */
+	{0x3806, 0x09}, /* y end H */
+	{0x3807, 0xa3}, /* y end L */
+	{0x3808, 0x06}, /* x output size H */
+	{0x3809, 0x60}, /* x output size L */
+	{0x380a, 0x04}, /* y output size H */
+	{0x380b, 0xc8}, /* y output size L */
+	{0x380c, 0x07}, /* HTS H */
+	{0x380d, 0x88}, /* HTS L */
+	{0x380e, 0x04}, /* VTS H */
+	{0x380f, 0xdc}, /* VTS L */
+	{0x3810, 0x00}, /* ISP x win H */
+	{0x3811, 0x04}, /* ISP x win L */
+	{0x3813, 0x02}, /* ISP y win L */
+	{0x3814, 0x03}, /* x odd inc */
+	{0x3815, 0x01}, /* x even inc */
+	{0x3820, 0x00}, /* vflip off */
+	{0x3821, 0x67}, /* mirror on, bin on */
+	{0x382a, 0x03}, /* y odd inc */
+	{0x382b, 0x01}, /* y even inc */
+	{0x3830, 0x08},
+	{0x3836, 0x02},
+	{0x3837, 0x18},
+	{0x3841, 0xff}, /* window auto size enable */
+	{0x3846, 0x48},
+	{0x3d85, 0x16}, /* OTP power up load data enable with BIST */
+	{0x3d8c, 0x73}, /* OTP setting start High */
+	{0x3d8d, 0xde}, /* OTP setting start Low */
+	{0x3f08, 0x08},
+	{0x3f0a, 0x00},
+	{0x4000, 0xf1}, /* out_range_trig, format_chg_trig */
+	{0x4001, 0x10}, /* total 128 black column */
+	{0x4005, 0x10}, /* BLC target L */
+	{0x4002, 0x27}, /* value used to limit BLC offset */
+	{0x4009, 0x81}, /* final BLC offset limitation enable */
+	{0x400b, 0x0c}, /* DCBLC on, DCBLC manual mode on */
+	{0x401b, 0x00}, /* zero line R coefficient */
+	{0x401d, 0x00}, /* zoro line T coefficient */
+	{0x4020, 0x00}, /* Anchor left start H */
+	{0x4021, 0x04}, /* Anchor left start L */
+	{0x4022, 0x06}, /* Anchor left end H */
+	{0x4023, 0x00}, /* Anchor left end L */
+	{0x4024, 0x0f}, /* Anchor right start H */
+	{0x4025, 0x2a}, /* Anchor right start L */
+	{0x4026, 0x0f}, /* Anchor right end H */
+	{0x4027, 0x2b}, /* Anchor right end L */
+	{0x4028, 0x00}, /* top zero line start */
+	{0x4029, 0x02}, /* top zero line number */
+	{0x402a, 0x04}, /* top black line start */
+	{0x402b, 0x04}, /* top black line number */
+	{0x402c, 0x00}, /* bottom zero line start */
+	{0x402d, 0x02}, /* bottom zoro line number */
+	{0x402e, 0x04}, /* bottom black line start */
+	{0x402f, 0x04}, /* bottom black line number */
+	{0x401f, 0x00}, /* interpolation x/y disable, Anchor one disable */
+	{0x4034, 0x3f},
+	{0x403d, 0x04}, /* md_precision_en */
+	{0x4300, 0xff}, /* clip max H */
+	{0x4301, 0x00}, /* clip min H */
+	{0x4302, 0x0f}, /* clip min L, clip max L */
+	{0x4316, 0x00},
+	{0x4500, 0x58},
+	{0x4503, 0x18},
+	{0x4600, 0x00},
+	{0x4601, 0xcb},
+	{0x481f, 0x32}, /* clk prepare min */
+	{0x4837, 0x16}, /* global timing */
+	{0x4850, 0x10}, /* lane 1 = 1, lane 0 = 0 */
+	{0x4851, 0x32}, /* lane 3 = 3, lane 2 = 2 */
+	{0x4b00, 0x2a},
+	{0x4b0d, 0x00},
+	{0x4d00, 0x04}, /* temperature sensor */
+	{0x4d01, 0x18},
+	{0x4d02, 0xc3},
+	{0x4d03, 0xff},
+	{0x4d04, 0xff},
+	{0x4d05, 0xff}, /* temperature sensor */
+	{0x5000, 0xfe}, /* lenc on, slave/master AWB gain/statistics enable */
+	{0x5001, 0x01}, /* BLC on */
+	{0x5002, 0x08}, /* H scale off, WBMATCH off, OTP_DPC */
+	{0x5003, 0x20}, /* DPC_DBC buffer control enable, WB */
+	{0x501e, 0x93}, /* enable digital gain */
+	{0x5046, 0x12},
+	{0x5780, 0x3e}, /* DPC */
+	{0x5781, 0x0f},
+	{0x5782, 0x44},
+	{0x5783, 0x02},
+	{0x5784, 0x01},
+	{0x5785, 0x00},
+	{0x5786, 0x00},
+	{0x5787, 0x04},
+	{0x5788, 0x02},
+	{0x5789, 0x0f},
+	{0x578a, 0xfd},
+	{0x578b, 0xf5},
+	{0x578c, 0xf5},
+	{0x578d, 0x03},
+	{0x578e, 0x08},
+	{0x578f, 0x0c},
+	{0x5790, 0x08},
+	{0x5791, 0x04},
+	{0x5792, 0x00},
+	{0x5793, 0x52},
+	{0x5794, 0xa3}, /* DPC */
+	{0x5871, 0x0d}, /* Lenc */
+	{0x5870, 0x18},
+	{0x586e, 0x10},
+	{0x586f, 0x08},
+	{0x58f7, 0x01},
+	{0x58f8, 0x3d}, /* Lenc */
+	{0x5901, 0x00}, /* H skip off, V skip off */
+	{0x5b00, 0x02}, /* OTP DPC start address */
+	{0x5b01, 0x10}, /* OTP DPC start address */
+	{0x5b02, 0x03}, /* OTP DPC end address */
+	{0x5b03, 0xcf}, /* OTP DPC end address */
+	{0x5b05, 0x6c}, /* recover method = 2b11, */
+	{0x5e00, 0x00}, /* use 0x3ff to test pattern off */
+	{0x5e01, 0x41}, /* window cut enable */
+	{0x382d, 0x7f},
+	{0x4825, 0x3a}, /* lpx_p_min */
+	{0x4826, 0x40}, /* hs_prepare_min */
+	{0x4808, 0x25}, /* wake up delay in 1/1024 s */
+	{0x3763, 0x18},
+	{0x3768, 0xcc},
+	{0x470b, 0x28},
+	{0x4202, 0x00},
+	{0x400d, 0x10}, /* BLC offset trigger L */
+	{0x4040, 0x04}, /* BLC gain th2 */
+	{0x403e, 0x04}, /* BLC gain th1 */
+	{0x4041, 0xc6}, /* BLC */
+	{0x3007, 0x80},
+	{0x400a, 0x01},
+	{REG_NULL, 0x00},
+};
+
+/*
+ * Xclk 24Mhz
+ * max_framerate 30fps
+ * mipi_datarate per lane 720Mbps
+ */
+static const struct regval ov8858_1632x1224_regs_2lane[] = {
+	/*
+	 * MIPI=720Mbps, SysClk=144Mhz,Dac Clock=360Mhz.
+	 * v00_01_00 (05/29/2014) : initial setting
+	 * AM19 : 3617 <- 0xC0
+	 * AM20 : change FWC_6K_EN to be default 0x3618=0x5a
+	 */
+	{0x0100, 0x00},
+	{0x3501, 0x4d}, /* exposure M */
+	{0x3502, 0x40}, /* exposure L */
+	{0x3778, 0x17},
+	{0x3808, 0x06}, /* x output size H */
+	{0x3809, 0x60}, /* x output size L */
+	{0x380a, 0x04}, /* y output size H */
+	{0x380b, 0xc8}, /* y output size L */
+	{0x380c, 0x07}, /* HTS H */
+	{0x380d, 0x88}, /* HTS L */
+	{0x380e, 0x04}, /* VTS H */
+	{0x380f, 0xdc}, /* VTS L */
+	{0x3814, 0x03}, /* x odd inc */
+	{0x3821, 0x67}, /* mirror on, bin on */
+	{0x382a, 0x03}, /* y odd inc */
+	{0x3830, 0x08},
+	{0x3836, 0x02},
+	{0x3f0a, 0x00},
+	{0x4001, 0x10}, /* total 128 black column */
+	{0x4022, 0x06}, /* Anchor left end H */
+	{0x4023, 0x00}, /* Anchor left end L */
+	{0x4025, 0x2a}, /* Anchor right start L */
+	{0x4027, 0x2b}, /* Anchor right end L */
+	{0x402b, 0x04}, /* top black line number */
+	{0x402f, 0x04}, /* bottom black line number */
+	{0x4500, 0x58},
+	{0x4600, 0x00},
+	{0x4601, 0xcb},
+	{0x382d, 0x7f},
+	{0x0100, 0x01},
+	{REG_NULL, 0x00},
+};
+
+/*
+ * Xclk 24Mhz
+ * max_framerate 15fps
+ * mipi_datarate per lane 720Mbps
+ */
+static const struct regval ov8858_3264x2448_regs_2lane[] = {
+	{0x0100, 0x00},
+	{0x3501, 0x9a}, /* exposure M */
+	{0x3502, 0x20}, /* exposure L */
+	{0x3778, 0x1a},
+	{0x3808, 0x0c}, /* x output size H */
+	{0x3809, 0xc0}, /* x output size L */
+	{0x380a, 0x09}, /* y output size H */
+	{0x380b, 0x90}, /* y output size L */
+	{0x380c, 0x07}, /* HTS H */
+	{0x380d, 0x94}, /* HTS L */
+	{0x380e, 0x09}, /* VTS H */
+	{0x380f, 0xaa}, /* VTS L */
+	{0x3814, 0x01}, /* x odd inc */
+	{0x3821, 0x46}, /* mirror on, bin off */
+	{0x382a, 0x01}, /* y odd inc */
+	{0x3830, 0x06},
+	{0x3836, 0x01},
+	{0x3f0a, 0x00},
+	{0x4001, 0x00}, /* total 256 black column */
+	{0x4022, 0x0c}, /* Anchor left end H */
+	{0x4023, 0x60}, /* Anchor left end L */
+	{0x4025, 0x36}, /* Anchor right start L */
+	{0x4027, 0x37}, /* Anchor right end L */
+	{0x402b, 0x08}, /* top black line number */
+	{0x402f, 0x08}, /* bottom black line number */
+	{0x4500, 0x58},
+	{0x4600, 0x01},
+	{0x4601, 0x97},
+	{0x382d, 0xff},
+	{REG_NULL, 0x00},
+};
+
+static const struct regval ov8858_global_regs_r2a_4lane[] = {
+	/*
+	 * MIPI=720Mbps, SysClk=144Mhz,Dac Clock=360Mhz.
+	 * v00_01_00 (05/29/2014) : initial setting
+	 * AM19 : 3617 <- 0xC0
+	 * AM20 : change FWC_6K_EN to be default 0x3618=0x5a
+	 */
+	{0x0103, 0x01}, /* software reset for OVTATool only */
+	{0x0103, 0x01}, /* software reset */
+	{0x0100, 0x00}, /* software standby */
+	{0x0302, 0x1e}, /* pll1_multi */
+	{0x0303, 0x00}, /* pll1_divm */
+	{0x0304, 0x03}, /* pll1_div_mipi */
+	{0x030e, 0x00}, /* pll2_rdiv */
+	{0x030f, 0x04}, /* pll2_divsp */
+	{0x0312, 0x01}, /* pll2_pre_div0, pll2_r_divdac */
+	{0x031e, 0x0c}, /* pll1_no_lat */
+	{0x3600, 0x00},
+	{0x3601, 0x00},
+	{0x3602, 0x00},
+	{0x3603, 0x00},
+	{0x3604, 0x22},
+	{0x3605, 0x20},
+	{0x3606, 0x00},
+	{0x3607, 0x20},
+	{0x3608, 0x11},
+	{0x3609, 0x28},
+	{0x360a, 0x00},
+	{0x360b, 0x05},
+	{0x360c, 0xd4},
+	{0x360d, 0x40},
+	{0x360e, 0x0c},
+	{0x360f, 0x20},
+	{0x3610, 0x07},
+	{0x3611, 0x20},
+	{0x3612, 0x88},
+	{0x3613, 0x80},
+	{0x3614, 0x58},
+	{0x3615, 0x00},
+	{0x3616, 0x4a},
+	{0x3617, 0x90},
+	{0x3618, 0x5a},
+	{0x3619, 0x70},
+	{0x361a, 0x99},
+	{0x361b, 0x0a},
+	{0x361c, 0x07},
+	{0x361d, 0x00},
+	{0x361e, 0x00},
+	{0x361f, 0x00},
+	{0x3638, 0xff},
+	{0x3633, 0x0f},
+	{0x3634, 0x0f},
+	{0x3635, 0x0f},
+	{0x3636, 0x12},
+	{0x3645, 0x13},
+	{0x3646, 0x83},
+	{0x364a, 0x07},
+	{0x3015, 0x01},
+	{0x3018, 0x72}, /* MIPI 4 lane */
+	{0x3020, 0x93}, /* Clock switch output normal, pclk_div =/1 */
+	{0x3022, 0x01}, /* pd_mipi enable when rst_sync */
+	{0x3031, 0x0a}, /* MIPI 10-bit mode */
+	{0x3034, 0x00},
+	{0x3106, 0x01}, /* sclk_div, sclk_pre_div */
+	{0x3305, 0xf1},
+	{0x3308, 0x00},
+	{0x3309, 0x28},
+	{0x330a, 0x00},
+	{0x330b, 0x20},
+	{0x330c, 0x00},
+	{0x330d, 0x00},
+	{0x330e, 0x00},
+	{0x330f, 0x40},
+	{0x3307, 0x04},
+	{0x3500, 0x00}, /* exposure H */
+	{0x3501, 0x4d}, /* exposure M */
+	{0x3502, 0x40}, /* exposure L */
+	{0x3503, 0x80}, /* gain delay ?, exposure delay 1 frame, real gain */
+	{0x3505, 0x80}, /* gain option */
+	{0x3508, 0x02}, /* gain H */
+	{0x3509, 0x00}, /* gain L */
+	{0x350c, 0x00}, /* short gain H */
+	{0x350d, 0x80}, /* short gain L */
+	{0x3510, 0x00}, /* short exposure H */
+	{0x3511, 0x02}, /* short exposure M */
+	{0x3512, 0x00}, /* short exposure L */
+	{0x3700, 0x30},
+	{0x3701, 0x18},
+	{0x3702, 0x50},
+	{0x3703, 0x32},
+	{0x3704, 0x28},
+	{0x3705, 0x00},
+	{0x3706, 0x82},
+	{0x3707, 0x08},
+	{0x3708, 0x48},
+	{0x3709, 0x66},
+	{0x370a, 0x01},
+	{0x370b, 0x82},
+	{0x370c, 0x07},
+	{0x3718, 0x14},
+	{0x3719, 0x31},
+	{0x3712, 0x44},
+	{0x3714, 0x24},
+	{0x371e, 0x31},
+	{0x371f, 0x7f},
+	{0x3720, 0x0a},
+	{0x3721, 0x0a},
+	{0x3724, 0x0c},
+	{0x3725, 0x02},
+	{0x3726, 0x0c},
+	{0x3728, 0x0a},
+	{0x3729, 0x03},
+	{0x372a, 0x06},
+	{0x372b, 0xa6},
+	{0x372c, 0xa6},
+	{0x372d, 0xa6},
+	{0x372e, 0x0c},
+	{0x372f, 0x20},
+	{0x3730, 0x02},
+	{0x3731, 0x0c},
+	{0x3732, 0x28},
+	{0x3733, 0x10},
+	{0x3734, 0x40},
+	{0x3736, 0x30},
+	{0x373a, 0x0a},
+	{0x373b, 0x0b},
+	{0x373c, 0x14},
+	{0x373e, 0x06},
+	{0x3750, 0x0a},
+	{0x3751, 0x0e},
+	{0x3755, 0x10},
+	{0x3758, 0x00},
+	{0x3759, 0x4c},
+	{0x375a, 0x0c},
+	{0x375b, 0x26},
+	{0x375c, 0x20},
+	{0x375d, 0x04},
+	{0x375e, 0x00},
+	{0x375f, 0x28},
+	{0x3768, 0x22},
+	{0x3769, 0x44},
+	{0x376a, 0x44},
+	{0x3761, 0x00},
+	{0x3762, 0x00},
+	{0x3763, 0x00},
+	{0x3766, 0xff},
+	{0x376b, 0x00},
+	{0x3772, 0x46},
+	{0x3773, 0x04},
+	{0x3774, 0x2c},
+	{0x3775, 0x13},
+	{0x3776, 0x08},
+	{0x3777, 0x00},
+	{0x3778, 0x17},
+	{0x37a0, 0x88},
+	{0x37a1, 0x7a},
+	{0x37a2, 0x7a},
+	{0x37a3, 0x00},
+	{0x37a4, 0x00},
+	{0x37a5, 0x00},
+	{0x37a6, 0x00},
+	{0x37a7, 0x88},
+	{0x37a8, 0x98},
+	{0x37a9, 0x98},
+	{0x3760, 0x00},
+	{0x376f, 0x01},
+	{0x37aa, 0x88},
+	{0x37ab, 0x5c},
+	{0x37ac, 0x5c},
+	{0x37ad, 0x55},
+	{0x37ae, 0x19},
+	{0x37af, 0x19},
+	{0x37b0, 0x00},
+	{0x37b1, 0x00},
+	{0x37b2, 0x00},
+	{0x37b3, 0x84},
+	{0x37b4, 0x84},
+	{0x37b5, 0x60},
+	{0x37b6, 0x00},
+	{0x37b7, 0x00},
+	{0x37b8, 0x00},
+	{0x37b9, 0xff},
+	{0x3800, 0x00}, /* x start H */
+	{0x3801, 0x0c}, /* x start L */
+	{0x3802, 0x00}, /* y start H */
+	{0x3803, 0x0c}, /* y start L */
+	{0x3804, 0x0c}, /* x end H */
+	{0x3805, 0xd3}, /* x end L */
+	{0x3806, 0x09}, /* y end H */
+	{0x3807, 0xa3}, /* y end L */
+	{0x3808, 0x06}, /* x output size H */
+	{0x3809, 0x60}, /* x output size L */
+	{0x380a, 0x04}, /* y output size H */
+	{0x380b, 0xc8}, /* y output size L */
+	{0x380c, 0x07}, /* HTS H */
+	{0x380d, 0x88}, /* HTS L */
+	{0x380e, 0x04}, /* VTS H */
+	{0x380f, 0xdc}, /* VTS L */
+	{0x3810, 0x00}, /* ISP x win H */
+	{0x3811, 0x04}, /* ISP x win L */
+	{0x3813, 0x02}, /* ISP y win L */
+	{0x3814, 0x03}, /* x odd inc */
+	{0x3815, 0x01}, /* x even inc */
+	{0x3820, 0x00}, /* vflip off */
+	{0x3821, 0x67}, /* mirror on, bin o */
+	{0x382a, 0x03}, /* y odd inc */
+	{0x382b, 0x01}, /* y even inc */
+	{0x3830, 0x08},
+	{0x3836, 0x02},
+	{0x3837, 0x18},
+	{0x3841, 0xff}, /* window auto size enable */
+	{0x3846, 0x48},
+	{0x3d85, 0x16}, /* OTP power up load data/setting enable */
+	{0x3d8c, 0x73}, /* OTP setting start High */
+	{0x3d8d, 0xde}, /* OTP setting start Low */
+	{0x3f08, 0x10},
+	{0x3f0a, 0x00},
+	{0x4000, 0xf1}, /* out_range/format_chg/gain/exp_chg trig enable */
+	{0x4001, 0x10}, /* total 128 black column */
+	{0x4005, 0x10}, /* BLC target L */
+	{0x4002, 0x27}, /* value used to limit BLC offset */
+	{0x4009, 0x81}, /* final BLC offset limitation enable */
+	{0x400b, 0x0c}, /* DCBLC on, DCBLC manual mode on */
+	{0x401b, 0x00}, /* zero line R coefficient */
+	{0x401d, 0x00}, /* zoro line T coefficient */
+	{0x4020, 0x00}, /* Anchor left start H */
+	{0x4021, 0x04}, /* Anchor left start L */
+	{0x4022, 0x06}, /* Anchor left end H */
+	{0x4023, 0x00}, /* Anchor left end L */
+	{0x4024, 0x0f}, /* Anchor right start H */
+	{0x4025, 0x2a}, /* Anchor right start L */
+	{0x4026, 0x0f}, /* Anchor right end H */
+	{0x4027, 0x2b}, /* Anchor right end L */
+	{0x4028, 0x00}, /* top zero line start */
+	{0x4029, 0x02}, /* top zero line number */
+	{0x402a, 0x04}, /* top black line start */
+	{0x402b, 0x04}, /* top black line number */
+	{0x402c, 0x00}, /* bottom zero line start */
+	{0x402d, 0x02}, /* bottom zoro line number */
+	{0x402e, 0x04}, /* bottom black line start */
+	{0x402f, 0x04}, /* bottom black line number */
+	{0x401f, 0x00}, /* interpolation x/y disable, Anchor one disable */
+	{0x4034, 0x3f},
+	{0x403d, 0x04}, /* md_precision_en */
+	{0x4300, 0xff}, /* clip max H */
+	{0x4301, 0x00}, /* clip min H */
+	{0x4302, 0x0f}, /* clip min L, clip max L */
+	{0x4316, 0x00},
+	{0x4500, 0x58},
+	{0x4503, 0x18},
+	{0x4600, 0x00},
+	{0x4601, 0xcb},
+	{0x481f, 0x32}, /* clk prepare min */
+	{0x4837, 0x16}, /* global timing */
+	{0x4850, 0x10}, /* lane 1 = 1, lane 0 = 0 */
+	{0x4851, 0x32}, /* lane 3 = 3, lane 2 = 2 */
+	{0x4b00, 0x2a},
+	{0x4b0d, 0x00},
+	{0x4d00, 0x04}, /* temperature sensor */
+	{0x4d01, 0x18},
+	{0x4d02, 0xc3},
+	{0x4d03, 0xff},
+	{0x4d04, 0xff},
+	{0x4d05, 0xff}, /* temperature sensor */
+	{0x5000, 0xfe}, /* lenc on, slave/master AWB gain/statistics enable */
+	{0x5001, 0x01}, /* BLC on */
+	{0x5002, 0x08}, /* WBMATCH sensor's gain, H scale/WBMATCH/OTP_DPC off */
+	{0x5003, 0x20}, /* DPC_DBC buffer control enable, WB */
+	{0x501e, 0x93}, /* enable digital gain */
+	{0x5046, 0x12},
+	{0x5780, 0x3e}, /* DPC */
+	{0x5781, 0x0f},
+	{0x5782, 0x44},
+	{0x5783, 0x02},
+	{0x5784, 0x01},
+	{0x5785, 0x00},
+	{0x5786, 0x00},
+	{0x5787, 0x04},
+	{0x5788, 0x02},
+	{0x5789, 0x0f},
+	{0x578a, 0xfd},
+	{0x578b, 0xf5},
+	{0x578c, 0xf5},
+	{0x578d, 0x03},
+	{0x578e, 0x08},
+	{0x578f, 0x0c},
+	{0x5790, 0x08},
+	{0x5791, 0x04},
+	{0x5792, 0x00},
+	{0x5793, 0x52},
+	{0x5794, 0xa3}, /* DPC */
+	{0x5871, 0x0d}, /* Lenc */
+	{0x5870, 0x18},
+	{0x586e, 0x10},
+	{0x586f, 0x08},
+	{0x58f7, 0x01},
+	{0x58f8, 0x3d}, /* Lenc */
+	{0x5901, 0x00}, /* H skip off, V skip off */
+	{0x5b00, 0x02}, /* OTP DPC start address */
+	{0x5b01, 0x10}, /* OTP DPC start address */
+	{0x5b02, 0x03}, /* OTP DPC end address */
+	{0x5b03, 0xcf}, /* OTP DPC end address */
+	{0x5b05, 0x6c}, /* recover method = 2b11 */
+	{0x5e00, 0x00}, /* use 0x3ff to test pattern off */
+	{0x5e01, 0x41}, /* window cut enable */
+	{0x382d, 0x7f},
+	{0x4825, 0x3a}, /* lpx_p_min */
+	{0x4826, 0x40}, /* hs_prepare_min */
+	{0x4808, 0x25}, /* wake up delay in 1/1024 s */
+	{0x3763, 0x18},
+	{0x3768, 0xcc},
+	{0x470b, 0x28},
+	{0x4202, 0x00},
+	{0x400d, 0x10}, /* BLC offset trigger L */
+	{0x4040, 0x04}, /* BLC gain th2 */
+	{0x403e, 0x04}, /* BLC gain th1 */
+	{0x4041, 0xc6}, /* BLC */
+	{0x3007, 0x80},
+	{0x400a, 0x01},
+	{REG_NULL, 0x00},
+};
+
+/*
+ * Xclk 24Mhz
+ * max_framerate 60fps
+ * mipi_datarate per lane 720Mbps
+ */
+static const struct regval ov8858_1632x1224_regs_4lane[] = {
+	{0x0100, 0x00},
+	{0x3501, 0x4d}, /* exposure M */
+	{0x3502, 0x40}, /* exposure L */
+	{0x3808, 0x06}, /* x output size H */
+	{0x3809, 0x60}, /* x output size L */
+	{0x380a, 0x04}, /* y output size H */
+	{0x380b, 0xc8}, /* y output size L */
+	{0x380c, 0x07}, /* HTS H */
+	{0x380d, 0x88}, /* HTS L */
+	{0x380e, 0x04}, /* VTS H */
+	{0x380f, 0xdc}, /* VTS L */
+	{0x3814, 0x03}, /* x odd inc */
+	{0x3821, 0x67}, /* mirror on, bin on */
+	{0x382a, 0x03}, /* y odd inc */
+	{0x3830, 0x08},
+	{0x3836, 0x02},
+	{0x3f0a, 0x00},
+	{0x4001, 0x10}, /* total 128 black column */
+	{0x4022, 0x06}, /* Anchor left end H */
+	{0x4023, 0x00}, /* Anchor left end L */
+	{0x4025, 0x2a}, /* Anchor right start L */
+	{0x4027, 0x2b}, /* Anchor right end L */
+	{0x402b, 0x04}, /* top black line number */
+	{0x402f, 0x04}, /* bottom black line number */
+	{0x4500, 0x58},
+	{0x4600, 0x00},
+	{0x4601, 0xcb},
+	{0x382d, 0x7f},
+	{0x0100, 0x01},
+	{REG_NULL, 0x00},
+};
+
+/*
+ * Xclk 24Mhz
+ * max_framerate 30fps
+ * mipi_datarate per lane 720Mbps
+ */
+static const struct regval ov8858_3264x2448_regs_4lane[] = {
+	{0x0100, 0x00},
+	{0x3501, 0x9a}, /* exposure M */
+	{0x3502, 0x20}, /* exposure L */
+	{0x3808, 0x0c}, /* x output size H */
+	{0x3809, 0xc0}, /* x output size L */
+	{0x380a, 0x09}, /* y output size H */
+	{0x380b, 0x90}, /* y output size L */
+	{0x380c, 0x07}, /* HTS H */
+	{0x380d, 0x94}, /* HTS L */
+	{0x380e, 0x09}, /* VTS H */
+	{0x380f, 0xaa}, /* VTS L */
+	{0x3814, 0x01}, /* x odd inc */
+	{0x3821, 0x46}, /* mirror on, bin off */
+	{0x382a, 0x01}, /* y odd inc */
+	{0x3830, 0x06},
+	{0x3836, 0x01},
+	{0x3f0a, 0x00},
+	{0x4001, 0x00}, /* total 256 black column */
+	{0x4022, 0x0c}, /* Anchor left end H */
+	{0x4023, 0x60}, /* Anchor left end L */
+	{0x4025, 0x36}, /* Anchor right start L */
+	{0x4027, 0x37}, /* Anchor right end L */
+	{0x402b, 0x08}, /* top black line number */
+	{0x402f, 0x08}, /* interpolation x/y disable, Anchor one disable */
+	{0x4500, 0x58},
+	{0x4600, 0x01},
+	{0x4601, 0x97},
+	{0x382d, 0xff},
+	{REG_NULL, 0x00},
+};
+
+static const struct ov8858_mode ov8858_modes[] = {
+	{
+		.width = 3264,
+		.height = 2448,
+		.exp_def = 2464,
+		.hts_def = 1940 * 2,
+		.vts_def = 2472,
+		.reg_modes = {
+			.mode_2lanes = ov8858_3264x2448_regs_2lane,
+			.mode_4lanes = ov8858_3264x2448_regs_4lane,
+		},
+	},
+	{
+		.width = 1632,
+		.height = 1224,
+		.exp_def = 1232,
+		.hts_def = 1928 * 2,
+		.vts_def = 1244,
+		.reg_modes = {
+			.mode_2lanes = ov8858_1632x1224_regs_2lane,
+			.mode_4lanes = ov8858_1632x1224_regs_4lane,
+		},
+	},
+};
+
+static const s64 link_freq_menu_items[] = {
+	OV8858_LINK_FREQ
+};
+
+static const char * const ov8858_test_pattern_menu[] = {
+	"Disabled",
+	"Vertical Color Bar Type 1",
+	"Vertical Color Bar Type 2",
+	"Vertical Color Bar Type 3",
+	"Vertical Color Bar Type 4"
+};
+
+/* ----------------------------------------------------------------------------
+ * HW access
+ */
+
+static int ov8858_write(struct ov8858 *ov8858, u32 reg, u32 val, int *err)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov8858->subdev);
+	unsigned int len = (reg >> OV8858_REG_SIZE_SHIFT) & 3;
+	u16 addr = reg & OV8858_REG_ADDR_MASK;
+	u8 buf[6];
+	int ret;
+
+	if (err && *err)
+		return *err;
+
+	put_unaligned_be16(addr, buf);
+	put_unaligned_be32(val << (8 * (4 - len)), buf + 2);
+
+	ret = i2c_master_send(client, buf, len + 2);
+	if (ret != len + 2) {
+		ret = ret < 0 ? ret : -EIO;
+		if (err)
+			*err = ret;
+
+		dev_err(&client->dev,
+			"Failed to write reg %u: %d\n", addr, ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ov8858_write_array(struct ov8858 *ov8858, const struct regval *regs)
+{
+	unsigned int i;
+	int ret = 0;
+
+	for (i = 0; ret == 0 && regs[i].addr != REG_NULL; ++i) {
+		ov8858_write(ov8858, OV8858_REG_8BIT(regs[i].addr),
+			     regs[i].val, &ret);
+	}
+
+	return ret;
+}
+
+static int ov8858_read(struct ov8858 *ov8858, u32 reg, u32 *val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov8858->subdev);
+	__be16 reg_addr_be = cpu_to_be16(reg & OV8858_REG_ADDR_MASK);
+	unsigned int len = (reg >> OV8858_REG_SIZE_SHIFT) & 3;
+	struct i2c_msg msgs[2];
+	__be32 data_be = 0;
+	u8 *data_be_p;
+	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[4 - len];
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret != ARRAY_SIZE(msgs)) {
+		ret = ret < 0 ? ret : -EIO;
+		dev_err(&client->dev,
+			"Failed to read reg %u: %d\n", reg, ret);
+		return ret;
+	}
+
+	*val = be32_to_cpu(data_be);
+
+	return 0;
+}
+
+/* ----------------------------------------------------------------------------
+ * Streaming
+ */
+
+static int ov8858_start_stream(struct ov8858 *ov8858,
+			       struct v4l2_subdev_state *state)
+{
+	struct v4l2_mbus_framefmt *format;
+	const struct ov8858_mode *mode;
+	const struct regval *reg_list;
+	int ret;
+
+	ret = ov8858_write_array(ov8858, ov8858->global_regs);
+	if (ret)
+		return ret;
+
+	format = v4l2_subdev_get_pad_format(&ov8858->subdev, state, 0);
+	mode = v4l2_find_nearest_size(ov8858_modes, ARRAY_SIZE(ov8858_modes),
+				      width, height, format->width,
+				      format->height);
+
+	reg_list = ov8858->num_lanes == 4
+		 ? mode->reg_modes.mode_4lanes
+		 : mode->reg_modes.mode_2lanes;
+
+	ret = ov8858_write_array(ov8858, reg_list);
+	if (ret)
+		return ret;
+
+	/* 200 usec max to let PLL stabilize. */
+	fsleep(200);
+
+	ret = __v4l2_ctrl_handler_setup(&ov8858->ctrl_handler);
+	if (ret)
+		return ret;
+
+	ret = ov8858_write(ov8858, OV8858_REG_SC_CTRL0100,
+			   OV8858_MODE_STREAMING, NULL);
+	if (ret)
+		return ret;
+
+	/* t5 (fixed) = 10msec before entering streaming state */
+	fsleep(10000);
+
+	return 0;
+}
+
+static int ov8858_stop_stream(struct ov8858 *ov8858)
+{
+	return ov8858_write(ov8858, OV8858_REG_SC_CTRL0100,
+			    OV8858_MODE_SW_STANDBY, NULL);
+}
+
+static int ov8858_s_stream(struct v4l2_subdev *sd, int on)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov8858 *ov8858 = sd_to_ov8858(sd);
+	struct v4l2_subdev_state *state;
+	int ret = 0;
+
+	state = v4l2_subdev_lock_and_get_active_state(sd);
+
+	if (on) {
+		ret = pm_runtime_resume_and_get(&client->dev);
+		if (ret < 0)
+			goto unlock_and_return;
+
+		ret = ov8858_start_stream(ov8858, state);
+		if (ret) {
+			dev_err(&client->dev, "Failed to start streaming\n");
+			pm_runtime_put_sync(&client->dev);
+			goto unlock_and_return;
+		}
+	} else {
+		ov8858_stop_stream(ov8858);
+		pm_runtime_mark_last_busy(&client->dev);
+		pm_runtime_put_autosuspend(&client->dev);
+	}
+
+unlock_and_return:
+	v4l2_subdev_unlock_state(state);
+
+	return ret;
+}
+
+static const struct v4l2_subdev_video_ops ov8858_video_ops = {
+	.s_stream = ov8858_s_stream,
+};
+
+/* ----------------------------------------------------------------------------
+ * Pad ops
+ */
+
+static int ov8858_set_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *state,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct ov8858 *ov8858 = sd_to_ov8858(sd);
+	const struct ov8858_mode *mode;
+	s64 h_blank, vblank_def;
+
+	mode = v4l2_find_nearest_size(ov8858_modes, ARRAY_SIZE(ov8858_modes),
+				      width, height, fmt->format.width,
+				      fmt->format.height);
+
+	fmt->format.code = MEDIA_BUS_FMT_SBGGR10_1X10;
+	fmt->format.width = mode->width;
+	fmt->format.height = mode->height;
+	fmt->format.field = V4L2_FIELD_NONE;
+
+	/* Store the format in the current subdev state. */
+	*v4l2_subdev_get_pad_format(sd, state, 0) =  fmt->format;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
+		return 0;
+
+	/* Adjust control limits when a new mode is applied. */
+	h_blank = mode->hts_def - mode->width;
+	__v4l2_ctrl_modify_range(ov8858->hblank, h_blank, h_blank, 1,
+				 h_blank);
+
+	vblank_def = mode->vts_def - mode->height;
+	__v4l2_ctrl_modify_range(ov8858->vblank, vblank_def,
+				 OV8858_VTS_MAX - mode->height, 1,
+				 vblank_def);
+
+	return 0;
+}
+
+static int ov8858_enum_frame_sizes(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *state,
+				   struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->index >= ARRAY_SIZE(ov8858_modes))
+		return -EINVAL;
+
+	if (fse->code != MEDIA_BUS_FMT_SBGGR10_1X10)
+		return -EINVAL;
+
+	fse->min_width  = ov8858_modes[fse->index].width;
+	fse->max_width  = ov8858_modes[fse->index].width;
+	fse->max_height = ov8858_modes[fse->index].height;
+	fse->min_height = ov8858_modes[fse->index].height;
+
+	return 0;
+}
+
+static int ov8858_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_SBGGR10_1X10;
+
+	return 0;
+}
+
+static int ov8858_init_cfg(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_state *sd_state)
+{
+	const struct ov8858_mode *def_mode = &ov8858_modes[0];
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+		.format = {
+			.width = def_mode->width,
+			.height = def_mode->height,
+		},
+	};
+
+	ov8858_set_fmt(sd, sd_state, &fmt);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_pad_ops ov8858_pad_ops = {
+	.init_cfg = ov8858_init_cfg,
+	.enum_mbus_code = ov8858_enum_mbus_code,
+	.enum_frame_size = ov8858_enum_frame_sizes,
+	.get_fmt = v4l2_subdev_get_fmt,
+	.set_fmt = ov8858_set_fmt,
+};
+
+static const struct v4l2_subdev_core_ops ov8858_core_ops = {
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_ops ov8858_subdev_ops = {
+	.core	= &ov8858_core_ops,
+	.video	= &ov8858_video_ops,
+	.pad	= &ov8858_pad_ops,
+};
+
+/* ----------------------------------------------------------------------------
+ * Controls handling
+ */
+
+static int ov8858_enable_test_pattern(struct ov8858 *ov8858, u32 pattern)
+{
+	u32 val;
+
+	if (pattern)
+		val = (pattern - 1) | OV8858_TEST_PATTERN_ENABLE;
+	else
+		val = OV8858_TEST_PATTERN_DISABLE;
+
+	return ov8858_write(ov8858, OV8858_REG_TEST_PATTERN, val, NULL);
+}
+
+static int ov8858_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov8858 *ov8858 = container_of(ctrl->handler,
+					     struct ov8858, ctrl_handler);
+
+	struct i2c_client *client = v4l2_get_subdevdata(&ov8858->subdev);
+	struct v4l2_mbus_framefmt *format;
+	struct v4l2_subdev_state *state;
+	u16 digi_gain;
+	s64 max_exp;
+	int ret;
+
+	/*
+	 * The control handler and the subdev state use the same mutex and the
+	 * mutex is guaranteed to be locked:
+	 * - by the core when s_ctrl is called int the VIDIOC_S_CTRL call path
+	 * - by the driver when s_ctrl is called in the s_stream(1) call path
+	 */
+	state = v4l2_subdev_get_locked_active_state(&ov8858->subdev);
+	format = v4l2_subdev_get_pad_format(&ov8858->subdev, state, 0);
+
+	/* Propagate change of current control to all related controls */
+	switch (ctrl->id) {
+	case V4L2_CID_VBLANK:
+		/* Update max exposure while meeting expected vblanking */
+		max_exp = format->height + ctrl->val - OV8858_EXPOSURE_MARGIN;
+		__v4l2_ctrl_modify_range(ov8858->exposure,
+					 ov8858->exposure->minimum, max_exp,
+					 ov8858->exposure->step,
+					 ov8858->exposure->default_value);
+		break;
+	}
+
+	if (!pm_runtime_get_if_in_use(&client->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE:
+		/* 4 least significant bits of exposure are fractional part */
+		ret = ov8858_write(ov8858, OV8858_REG_LONG_EXPO,
+				   ctrl->val << 4, NULL);
+		break;
+	case V4L2_CID_ANALOGUE_GAIN:
+		ret = ov8858_write(ov8858, OV8858_REG_LONG_GAIN,
+				   ctrl->val, NULL);
+		break;
+	case V4L2_CID_DIGITAL_GAIN:
+		/*
+		 * Digital gain is assembled as:
+		 * 0x350a[7:0] = dgain[13:6]
+		 * 0x350b[5:0] = dgain[5:0]
+		 * Reassemble the control value to write it in one go.
+		 */
+		digi_gain = (ctrl->val & OV8858_LONG_DIGIGAIN_L_MASK)
+			  | ((ctrl->val & OV8858_LONG_DIGIGAIN_H_MASK) <<
+			      OV8858_LONG_DIGIGAIN_H_SHIFT);
+		ret = ov8858_write(ov8858, OV8858_REG_LONG_DIGIGAIN,
+				   digi_gain, NULL);
+		break;
+	case V4L2_CID_VBLANK:
+		ret = ov8858_write(ov8858, OV8858_REG_VTS,
+				   ctrl->val + format->height, NULL);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		ret = ov8858_enable_test_pattern(ov8858, ctrl->val);
+		break;
+	default:
+		ret = -EINVAL;
+		dev_warn(&client->dev, "%s Unhandled id: 0x%x\n",
+			 __func__, ctrl->id);
+		break;
+	}
+
+	pm_runtime_put(&client->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ov8858_ctrl_ops = {
+	.s_ctrl = ov8858_set_ctrl,
+};
+
+/* ----------------------------------------------------------------------------
+ * Power Management
+ */
+
+static int ov8858_power_on(struct ov8858 *ov8858)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov8858->subdev);
+	struct device *dev = &client->dev;
+	unsigned long delay_us;
+	int ret;
+
+	if (clk_get_rate(ov8858->xvclk) != OV8858_XVCLK_FREQ)
+		dev_warn(dev, "xvclk mismatched, modes are based on 24MHz\n");
+
+	ret = clk_prepare_enable(ov8858->xvclk);
+	if (ret < 0) {
+		dev_err(dev, "Failed to enable xvclk\n");
+		return ret;
+	}
+
+	ret = regulator_bulk_enable(ARRAY_SIZE(ov8858_supply_names),
+				    ov8858->supplies);
+	if (ret < 0) {
+		dev_err(dev, "Failed to enable regulators\n");
+		goto disable_clk;
+	}
+
+	/*
+	 * The chip manual only suggests 8192 cycles prior to first SCCB
+	 * transaction, but a double sleep between the release of gpios
+	 * helps with sporadic failures observed at probe time.
+	 */
+	delay_us = DIV_ROUND_UP(8192, OV8858_XVCLK_FREQ / 1000 / 1000);
+
+	gpiod_set_value_cansleep(ov8858->reset_gpio, 0);
+	fsleep(delay_us);
+	gpiod_set_value_cansleep(ov8858->pwdn_gpio, 0);
+	fsleep(delay_us);
+
+	return 0;
+
+disable_clk:
+	clk_disable_unprepare(ov8858->xvclk);
+
+	return ret;
+}
+
+static void ov8858_power_off(struct ov8858 *ov8858)
+{
+	gpiod_set_value_cansleep(ov8858->pwdn_gpio, 1);
+	clk_disable_unprepare(ov8858->xvclk);
+	gpiod_set_value_cansleep(ov8858->reset_gpio, 1);
+
+	regulator_bulk_disable(ARRAY_SIZE(ov8858_supply_names),
+			       ov8858->supplies);
+}
+
+static int ov8858_runtime_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov8858 *ov8858 = sd_to_ov8858(sd);
+
+	return ov8858_power_on(ov8858);
+}
+
+static int ov8858_runtime_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov8858 *ov8858 = sd_to_ov8858(sd);
+
+	ov8858_power_off(ov8858);
+
+	return 0;
+}
+
+static const struct dev_pm_ops ov8858_pm_ops = {
+	SET_RUNTIME_PM_OPS(ov8858_runtime_suspend,
+			   ov8858_runtime_resume, NULL)
+};
+
+/* ----------------------------------------------------------------------------
+ * Probe and initialization
+ */
+
+static int ov8858_init_ctrls(struct ov8858 *ov8858)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov8858->subdev);
+	struct v4l2_ctrl_handler *handler = &ov8858->ctrl_handler;
+	const struct ov8858_mode *mode = &ov8858_modes[0];
+	struct v4l2_fwnode_device_properties props;
+	s64 exposure_max, vblank_def;
+	unsigned int pixel_rate;
+	struct v4l2_ctrl *ctrl;
+	u32 h_blank;
+	int ret;
+
+	ret = v4l2_ctrl_handler_init(handler, 10);
+	if (ret)
+		return ret;
+
+	ctrl = v4l2_ctrl_new_int_menu(handler, NULL, V4L2_CID_LINK_FREQ,
+				      0, 0, link_freq_menu_items);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	/* pixel rate = link frequency * 2 * lanes / bpp */
+	pixel_rate = OV8858_LINK_FREQ * 2 * ov8858->num_lanes / 10;
+	v4l2_ctrl_new_std(handler, NULL, V4L2_CID_PIXEL_RATE,
+			  0, pixel_rate, 1, pixel_rate);
+
+	h_blank = mode->hts_def - mode->width;
+	ov8858->hblank = v4l2_ctrl_new_std(handler, NULL, V4L2_CID_HBLANK,
+					   h_blank, h_blank, 1, h_blank);
+	if (ov8858->hblank)
+		ov8858->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	vblank_def = mode->vts_def - mode->height;
+	ov8858->vblank = v4l2_ctrl_new_std(handler, &ov8858_ctrl_ops,
+					   V4L2_CID_VBLANK, vblank_def,
+					   OV8858_VTS_MAX - mode->height,
+					   1, vblank_def);
+
+	exposure_max = mode->vts_def - OV8858_EXPOSURE_MARGIN;
+	ov8858->exposure = v4l2_ctrl_new_std(handler, &ov8858_ctrl_ops,
+					     V4L2_CID_EXPOSURE,
+					     OV8858_EXPOSURE_MIN,
+					     exposure_max, OV8858_EXPOSURE_STEP,
+					     mode->exp_def);
+
+	v4l2_ctrl_new_std(handler, &ov8858_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
+			  OV8858_LONG_GAIN_MIN, OV8858_LONG_GAIN_MAX,
+			  OV8858_LONG_GAIN_STEP, OV8858_LONG_GAIN_DEFAULT);
+
+	v4l2_ctrl_new_std(handler, &ov8858_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
+			  OV8858_LONG_DIGIGAIN_MIN, OV8858_LONG_DIGIGAIN_MAX,
+			  OV8858_LONG_DIGIGAIN_STEP,
+			  OV8858_LONG_DIGIGAIN_DEFAULT);
+
+	v4l2_ctrl_new_std_menu_items(handler, &ov8858_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(ov8858_test_pattern_menu) - 1,
+				     0, 0, ov8858_test_pattern_menu);
+
+	if (handler->error) {
+		ret = handler->error;
+		goto err_free_handler;
+	}
+
+	ret = v4l2_fwnode_device_parse(&client->dev, &props);
+	if (ret)
+		goto err_free_handler;
+
+	ret = v4l2_ctrl_new_fwnode_properties(handler, &ov8858_ctrl_ops,
+					      &props);
+	if (ret)
+		goto err_free_handler;
+
+	ov8858->subdev.ctrl_handler = handler;
+
+	return 0;
+
+err_free_handler:
+	dev_err(&client->dev, "Failed to init controls: %d\n", ret);
+	v4l2_ctrl_handler_free(handler);
+
+	return ret;
+}
+
+static int ov8858_check_sensor_id(struct ov8858 *ov8858)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov8858->subdev);
+	u32 id = 0;
+	int ret;
+
+	ret = ov8858_read(ov8858, OV8858_REG_CHIP_ID, &id);
+	if (ret)
+		return ret;
+
+	if (id != OV8858_CHIP_ID) {
+		dev_err(&client->dev, "Unexpected sensor id 0x%x\n", id);
+		return -ENODEV;
+	}
+
+	ret = ov8858_read(ov8858, OV8858_REG_SUB_ID, &id);
+	if (ret)
+		return ret;
+
+	dev_info(&client->dev, "Detected OV8858 sensor, revision 0x%x\n", id);
+
+	if (id == OV8858_R2A) {
+		/* R2A supports 2 and 4 lanes modes. */
+		ov8858->global_regs = ov8858->num_lanes == 4
+				    ? ov8858_global_regs_r2a_4lane
+				    : ov8858_global_regs_r2a_2lane;
+	} else if (ov8858->num_lanes == 2) {
+		/*
+		 * R1A only supports 2 lanes mode and it's only partially
+		 * supported.
+		 */
+		ov8858->global_regs = ov8858_global_regs_r1a;
+		dev_warn(&client->dev, "R1A may not work well!\n");
+	} else {
+		dev_err(&client->dev,
+			"Unsupported number of data lanes for R1A revision.\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int ov8858_configure_regulators(struct ov8858 *ov8858)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov8858->subdev);
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(ov8858_supply_names); i++)
+		ov8858->supplies[i].supply = ov8858_supply_names[i];
+
+	return devm_regulator_bulk_get(&client->dev,
+				       ARRAY_SIZE(ov8858_supply_names),
+				       ov8858->supplies);
+}
+
+static int ov8858_parse_of(struct ov8858 *ov8858)
+{
+	struct v4l2_fwnode_endpoint vep = { .bus_type = V4L2_MBUS_CSI2_DPHY };
+	struct i2c_client *client = v4l2_get_subdevdata(&ov8858->subdev);
+	struct device *dev = &client->dev;
+	struct fwnode_handle *endpoint;
+	int ret;
+
+	endpoint = fwnode_graph_get_next_endpoint(dev_fwnode(dev), NULL);
+	if (!endpoint) {
+		dev_err(dev, "Failed to get endpoint\n");
+		return -EINVAL;
+	}
+
+	ret = v4l2_fwnode_endpoint_parse(endpoint, &vep);
+	fwnode_handle_put(endpoint);
+	if (ret) {
+		dev_err(dev, "Failed to parse endpoint: %d\n", ret);
+		return ret;
+	}
+
+	ov8858->num_lanes = vep.bus.mipi_csi2.num_data_lanes;
+	switch (ov8858->num_lanes) {
+	case 4:
+	case 2:
+		break;
+	default:
+		dev_err(dev, "Unsupported number of data lanes %u\n",
+			ov8858->num_lanes);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int ov8858_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct v4l2_subdev *sd;
+	struct ov8858 *ov8858;
+	int ret;
+
+	ov8858 = devm_kzalloc(dev, sizeof(*ov8858), GFP_KERNEL);
+	if (!ov8858)
+		return -ENOMEM;
+
+	ov8858->xvclk = devm_clk_get(dev, "xvclk");
+	if (IS_ERR(ov8858->xvclk))
+		return dev_err_probe(dev, PTR_ERR(ov8858->xvclk),
+				     "Failed to get xvclk\n");
+
+	ov8858->reset_gpio = devm_gpiod_get_optional(dev, "reset",
+						     GPIOD_OUT_HIGH);
+	if (IS_ERR(ov8858->reset_gpio))
+		return dev_err_probe(dev, PTR_ERR(ov8858->reset_gpio),
+				     "Failed to get reset gpio\n");
+
+	ov8858->pwdn_gpio = devm_gpiod_get_optional(dev, "powerdown",
+						    GPIOD_OUT_HIGH);
+	if (IS_ERR(ov8858->pwdn_gpio))
+		return dev_err_probe(dev, PTR_ERR(ov8858->pwdn_gpio),
+				     "Failed to get powerdown gpio\n");
+
+	v4l2_i2c_subdev_init(&ov8858->subdev, client, &ov8858_subdev_ops);
+
+	ret = ov8858_configure_regulators(ov8858);
+	if (ret)
+		return dev_err_probe(dev, ret, "Failed to get regulators\n");
+
+	ret = ov8858_parse_of(ov8858);
+	if (ret)
+		return ret;
+
+	ret = ov8858_init_ctrls(ov8858);
+	if (ret)
+		return ret;
+
+	sd = &ov8858->subdev;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
+	ov8858->pad.flags = MEDIA_PAD_FL_SOURCE;
+	sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret = media_entity_pads_init(&sd->entity, 1, &ov8858->pad);
+	if (ret < 0)
+		goto err_free_handler;
+
+	sd->state_lock = ov8858->ctrl_handler.lock;
+	ret = v4l2_subdev_init_finalize(sd);
+	if (ret < 0) {
+		dev_err(&client->dev, "Subdev initialization error %d\n", ret);
+		goto err_clean_entity;
+	}
+
+	ret = ov8858_power_on(ov8858);
+	if (ret)
+		goto err_clean_entity;
+
+	pm_runtime_set_active(dev);
+	pm_runtime_get_noresume(dev);
+	pm_runtime_enable(dev);
+
+	ret = ov8858_check_sensor_id(ov8858);
+	if (ret)
+		goto err_power_off;
+
+	pm_runtime_set_autosuspend_delay(dev, 1000);
+	pm_runtime_use_autosuspend(dev);
+
+	ret = v4l2_async_register_subdev_sensor(sd);
+	if (ret) {
+		dev_err(dev, "v4l2 async register subdev failed\n");
+		goto err_power_off;
+	}
+
+	pm_runtime_mark_last_busy(dev);
+	pm_runtime_put_autosuspend(dev);
+
+	return 0;
+
+err_power_off:
+	pm_runtime_disable(dev);
+	pm_runtime_put_noidle(dev);
+	ov8858_power_off(ov8858);
+err_clean_entity:
+	media_entity_cleanup(&sd->entity);
+err_free_handler:
+	v4l2_ctrl_handler_free(&ov8858->ctrl_handler);
+
+	return ret;
+}
+
+static void ov8858_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov8858 *ov8858 = sd_to_ov8858(sd);
+
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(&ov8858->ctrl_handler);
+
+	pm_runtime_disable(&client->dev);
+	if (!pm_runtime_status_suspended(&client->dev))
+		ov8858_power_off(ov8858);
+	pm_runtime_set_suspended(&client->dev);
+}
+
+static const struct of_device_id ov8858_of_match[] = {
+	{ .compatible = "ovti,ov8858" },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, ov8858_of_match);
+
+static struct i2c_driver ov8858_i2c_driver = {
+	.driver = {
+		.name = "ov8858",
+		.pm = &ov8858_pm_ops,
+		.of_match_table = ov8858_of_match,
+	},
+	.probe		= ov8858_probe,
+	.remove		= ov8858_remove,
+};
+
+module_i2c_driver(ov8858_i2c_driver);
+
+MODULE_DESCRIPTION("OmniVision OV8858 sensor driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/ov8865.c b/drivers/media/i2c/ov8865.c
new file mode 100644
index 000000000..f2213c615
--- /dev/null
+++ b/drivers/media/i2c/ov8865.c
@@ -0,0 +1,3169 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright 2020 Kévin L'hôpital <kevin.lhopital@bootlin.com>
+ * Copyright 2020 Bootlin
+ * Author: Paul Kocialkowski <paul.kocialkowski@bootlin.com>
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/i2c.h>
+#include <linux/mod_devicetable.h>
+#include <linux/module.h>
+#include <linux/of_graph.h>
+#include <linux/pm_runtime.h>
+#include <linux/regulator/consumer.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-image-sizes.h>
+#include <media/v4l2-mediabus.h>
+
+/* Register definitions */
+
+/* System */
+
+#define OV8865_SW_STANDBY_REG			0x100
+#define OV8865_SW_STANDBY_STREAM_ON		BIT(0)
+
+#define OV8865_SW_RESET_REG			0x103
+#define OV8865_SW_RESET_RESET			BIT(0)
+
+#define OV8865_PLL_CTRL0_REG			0x300
+#define OV8865_PLL_CTRL0_PRE_DIV(v)		((v) & GENMASK(2, 0))
+#define OV8865_PLL_CTRL1_REG			0x301
+#define OV8865_PLL_CTRL1_MUL_H(v)		(((v) & GENMASK(9, 8)) >> 8)
+#define OV8865_PLL_CTRL2_REG			0x302
+#define OV8865_PLL_CTRL2_MUL_L(v)		((v) & GENMASK(7, 0))
+#define OV8865_PLL_CTRL3_REG			0x303
+#define OV8865_PLL_CTRL3_M_DIV(v)		(((v) - 1) & GENMASK(3, 0))
+#define OV8865_PLL_CTRL4_REG			0x304
+#define OV8865_PLL_CTRL4_MIPI_DIV(v)		((v) & GENMASK(1, 0))
+#define OV8865_PLL_CTRL5_REG			0x305
+#define OV8865_PLL_CTRL5_SYS_PRE_DIV(v)		((v) & GENMASK(1, 0))
+#define OV8865_PLL_CTRL6_REG			0x306
+#define OV8865_PLL_CTRL6_SYS_DIV(v)		(((v) - 1) & BIT(0))
+
+#define OV8865_PLL_CTRL8_REG			0x308
+#define OV8865_PLL_CTRL9_REG			0x309
+#define OV8865_PLL_CTRLA_REG			0x30a
+#define OV8865_PLL_CTRLA_PRE_DIV_HALF(v)	(((v) - 1) & BIT(0))
+#define OV8865_PLL_CTRLB_REG			0x30b
+#define OV8865_PLL_CTRLB_PRE_DIV(v)		((v) & GENMASK(2, 0))
+#define OV8865_PLL_CTRLC_REG			0x30c
+#define OV8865_PLL_CTRLC_MUL_H(v)		(((v) & GENMASK(9, 8)) >> 8)
+#define OV8865_PLL_CTRLD_REG			0x30d
+#define OV8865_PLL_CTRLD_MUL_L(v)		((v) & GENMASK(7, 0))
+#define OV8865_PLL_CTRLE_REG			0x30e
+#define OV8865_PLL_CTRLE_SYS_DIV(v)		((v) & GENMASK(2, 0))
+#define OV8865_PLL_CTRLF_REG			0x30f
+#define OV8865_PLL_CTRLF_SYS_PRE_DIV(v)		(((v) - 1) & GENMASK(3, 0))
+#define OV8865_PLL_CTRL10_REG			0x310
+#define OV8865_PLL_CTRL11_REG			0x311
+#define OV8865_PLL_CTRL12_REG			0x312
+#define OV8865_PLL_CTRL12_PRE_DIV_HALF(v)	((((v) - 1) << 4) & BIT(4))
+#define OV8865_PLL_CTRL12_DAC_DIV(v)		(((v) - 1) & GENMASK(3, 0))
+
+#define OV8865_PLL_CTRL1B_REG			0x31b
+#define OV8865_PLL_CTRL1C_REG			0x31c
+
+#define OV8865_PLL_CTRL1E_REG			0x31e
+#define OV8865_PLL_CTRL1E_PLL1_NO_LAT		BIT(3)
+
+#define OV8865_PAD_OEN0_REG			0x3000
+
+#define OV8865_PAD_OEN2_REG			0x3002
+
+#define OV8865_CLK_RST5_REG			0x3005
+
+#define OV8865_CHIP_ID_HH_REG			0x300a
+#define OV8865_CHIP_ID_HH_VALUE			0x00
+#define OV8865_CHIP_ID_H_REG			0x300b
+#define OV8865_CHIP_ID_H_VALUE			0x88
+#define OV8865_CHIP_ID_L_REG			0x300c
+#define OV8865_CHIP_ID_L_VALUE			0x65
+#define OV8865_PAD_OUT2_REG			0x300d
+
+#define OV8865_PAD_SEL2_REG			0x3010
+#define OV8865_PAD_PK_REG			0x3011
+#define OV8865_PAD_PK_DRIVE_STRENGTH_1X		(0 << 5)
+#define OV8865_PAD_PK_DRIVE_STRENGTH_2X		(1 << 5)
+#define OV8865_PAD_PK_DRIVE_STRENGTH_3X		(2 << 5)
+#define OV8865_PAD_PK_DRIVE_STRENGTH_4X		(3 << 5)
+
+#define OV8865_PUMP_CLK_DIV_REG			0x3015
+#define OV8865_PUMP_CLK_DIV_PUMP_N(v)		(((v) << 4) & GENMASK(6, 4))
+#define OV8865_PUMP_CLK_DIV_PUMP_P(v)		((v) & GENMASK(2, 0))
+
+#define OV8865_MIPI_SC_CTRL0_REG		0x3018
+#define OV8865_MIPI_SC_CTRL0_LANES(v)		((((v) - 1) << 5) & \
+						 GENMASK(7, 5))
+#define OV8865_MIPI_SC_CTRL0_MIPI_EN		BIT(4)
+#define OV8865_MIPI_SC_CTRL0_UNKNOWN		BIT(1)
+#define OV8865_MIPI_SC_CTRL0_LANES_PD_MIPI	BIT(0)
+#define OV8865_MIPI_SC_CTRL1_REG		0x3019
+#define OV8865_CLK_RST0_REG			0x301a
+#define OV8865_CLK_RST1_REG			0x301b
+#define OV8865_CLK_RST2_REG			0x301c
+#define OV8865_CLK_RST3_REG			0x301d
+#define OV8865_CLK_RST4_REG			0x301e
+
+#define OV8865_PCLK_SEL_REG			0x3020
+#define OV8865_PCLK_SEL_PCLK_DIV_MASK		BIT(3)
+#define OV8865_PCLK_SEL_PCLK_DIV(v)		((((v) - 1) << 3) & BIT(3))
+
+#define OV8865_MISC_CTRL_REG			0x3021
+#define OV8865_MIPI_SC_CTRL2_REG		0x3022
+#define OV8865_MIPI_SC_CTRL2_CLK_LANES_PD_MIPI	BIT(1)
+#define OV8865_MIPI_SC_CTRL2_PD_MIPI_RST_SYNC	BIT(0)
+
+#define OV8865_MIPI_BIT_SEL_REG			0x3031
+#define OV8865_MIPI_BIT_SEL(v)			(((v) << 0) & GENMASK(4, 0))
+#define OV8865_CLK_SEL0_REG			0x3032
+#define OV8865_CLK_SEL0_PLL1_SYS_SEL(v)		(((v) << 7) & BIT(7))
+#define OV8865_CLK_SEL1_REG			0x3033
+#define OV8865_CLK_SEL1_MIPI_EOF		BIT(5)
+#define OV8865_CLK_SEL1_UNKNOWN			BIT(2)
+#define OV8865_CLK_SEL1_PLL_SCLK_SEL_MASK	BIT(1)
+#define OV8865_CLK_SEL1_PLL_SCLK_SEL(v)		(((v) << 1) & BIT(1))
+
+#define OV8865_SCLK_CTRL_REG			0x3106
+#define OV8865_SCLK_CTRL_SCLK_DIV(v)		(((v) << 4) & GENMASK(7, 4))
+#define OV8865_SCLK_CTRL_SCLK_PRE_DIV(v)	(((v) << 2) & GENMASK(3, 2))
+#define OV8865_SCLK_CTRL_UNKNOWN		BIT(0)
+
+/* Exposure/gain */
+
+#define OV8865_EXPOSURE_CTRL_HH_REG		0x3500
+#define OV8865_EXPOSURE_CTRL_HH(v)		(((v) & GENMASK(19, 16)) >> 16)
+#define OV8865_EXPOSURE_CTRL_H_REG		0x3501
+#define OV8865_EXPOSURE_CTRL_H(v)		(((v) & GENMASK(15, 8)) >> 8)
+#define OV8865_EXPOSURE_CTRL_L_REG		0x3502
+#define OV8865_EXPOSURE_CTRL_L(v)		((v) & GENMASK(7, 0))
+#define OV8865_EXPOSURE_GAIN_MANUAL_REG		0x3503
+#define OV8865_INTEGRATION_TIME_MARGIN		8
+
+#define OV8865_GAIN_CTRL_H_REG			0x3508
+#define OV8865_GAIN_CTRL_H(v)			(((v) & GENMASK(12, 8)) >> 8)
+#define OV8865_GAIN_CTRL_L_REG			0x3509
+#define OV8865_GAIN_CTRL_L(v)			((v) & GENMASK(7, 0))
+
+/* Timing */
+
+#define OV8865_CROP_START_X_H_REG		0x3800
+#define OV8865_CROP_START_X_H(v)		(((v) & GENMASK(11, 8)) >> 8)
+#define OV8865_CROP_START_X_L_REG		0x3801
+#define OV8865_CROP_START_X_L(v)		((v) & GENMASK(7, 0))
+#define OV8865_CROP_START_Y_H_REG		0x3802
+#define OV8865_CROP_START_Y_H(v)		(((v) & GENMASK(11, 8)) >> 8)
+#define OV8865_CROP_START_Y_L_REG		0x3803
+#define OV8865_CROP_START_Y_L(v)		((v) & GENMASK(7, 0))
+#define OV8865_CROP_END_X_H_REG			0x3804
+#define OV8865_CROP_END_X_H(v)			(((v) & GENMASK(11, 8)) >> 8)
+#define OV8865_CROP_END_X_L_REG			0x3805
+#define OV8865_CROP_END_X_L(v)			((v) & GENMASK(7, 0))
+#define OV8865_CROP_END_Y_H_REG			0x3806
+#define OV8865_CROP_END_Y_H(v)			(((v) & GENMASK(11, 8)) >> 8)
+#define OV8865_CROP_END_Y_L_REG			0x3807
+#define OV8865_CROP_END_Y_L(v)			((v) & GENMASK(7, 0))
+#define OV8865_OUTPUT_SIZE_X_H_REG		0x3808
+#define OV8865_OUTPUT_SIZE_X_H(v)		(((v) & GENMASK(11, 8)) >> 8)
+#define OV8865_OUTPUT_SIZE_X_L_REG		0x3809
+#define OV8865_OUTPUT_SIZE_X_L(v)		((v) & GENMASK(7, 0))
+#define OV8865_OUTPUT_SIZE_Y_H_REG		0x380a
+#define OV8865_OUTPUT_SIZE_Y_H(v)		(((v) & GENMASK(11, 8)) >> 8)
+#define OV8865_OUTPUT_SIZE_Y_L_REG		0x380b
+#define OV8865_OUTPUT_SIZE_Y_L(v)		((v) & GENMASK(7, 0))
+#define OV8865_HTS_H_REG			0x380c
+#define OV8865_HTS_H(v)				(((v) & GENMASK(11, 8)) >> 8)
+#define OV8865_HTS_L_REG			0x380d
+#define OV8865_HTS_L(v)				((v) & GENMASK(7, 0))
+#define OV8865_VTS_H_REG			0x380e
+#define OV8865_VTS_H(v)				(((v) & GENMASK(11, 8)) >> 8)
+#define OV8865_VTS_L_REG			0x380f
+#define OV8865_VTS_L(v)				((v) & GENMASK(7, 0))
+#define OV8865_TIMING_MAX_VTS			0xffff
+#define OV8865_TIMING_MIN_VTS			0x04
+#define OV8865_OFFSET_X_H_REG			0x3810
+#define OV8865_OFFSET_X_H(v)			(((v) & GENMASK(15, 8)) >> 8)
+#define OV8865_OFFSET_X_L_REG			0x3811
+#define OV8865_OFFSET_X_L(v)			((v) & GENMASK(7, 0))
+#define OV8865_OFFSET_Y_H_REG			0x3812
+#define OV8865_OFFSET_Y_H(v)			(((v) & GENMASK(14, 8)) >> 8)
+#define OV8865_OFFSET_Y_L_REG			0x3813
+#define OV8865_OFFSET_Y_L(v)			((v) & GENMASK(7, 0))
+#define OV8865_INC_X_ODD_REG			0x3814
+#define OV8865_INC_X_ODD(v)			((v) & GENMASK(4, 0))
+#define OV8865_INC_X_EVEN_REG			0x3815
+#define OV8865_INC_X_EVEN(v)			((v) & GENMASK(4, 0))
+#define OV8865_VSYNC_START_H_REG		0x3816
+#define OV8865_VSYNC_START_H(v)			(((v) & GENMASK(15, 8)) >> 8)
+#define OV8865_VSYNC_START_L_REG		0x3817
+#define OV8865_VSYNC_START_L(v)			((v) & GENMASK(7, 0))
+#define OV8865_VSYNC_END_H_REG			0x3818
+#define OV8865_VSYNC_END_H(v)			(((v) & GENMASK(15, 8)) >> 8)
+#define OV8865_VSYNC_END_L_REG			0x3819
+#define OV8865_VSYNC_END_L(v)			((v) & GENMASK(7, 0))
+#define OV8865_HSYNC_FIRST_H_REG		0x381a
+#define OV8865_HSYNC_FIRST_H(v)			(((v) & GENMASK(15, 8)) >> 8)
+#define OV8865_HSYNC_FIRST_L_REG		0x381b
+#define OV8865_HSYNC_FIRST_L(v)			((v) & GENMASK(7, 0))
+
+#define OV8865_FORMAT1_REG			0x3820
+#define OV8865_FORMAT1_FLIP_VERT_ISP_EN		BIT(2)
+#define OV8865_FORMAT1_FLIP_VERT_SENSOR_EN	BIT(1)
+#define OV8865_FORMAT2_REG			0x3821
+#define OV8865_FORMAT2_HSYNC_EN			BIT(6)
+#define OV8865_FORMAT2_FST_VBIN_EN		BIT(5)
+#define OV8865_FORMAT2_FST_HBIN_EN		BIT(4)
+#define OV8865_FORMAT2_ISP_HORZ_VAR2_EN		BIT(3)
+#define OV8865_FORMAT2_FLIP_HORZ_ISP_EN		BIT(2)
+#define OV8865_FORMAT2_FLIP_HORZ_SENSOR_EN	BIT(1)
+#define OV8865_FORMAT2_SYNC_HBIN_EN		BIT(0)
+
+#define OV8865_INC_Y_ODD_REG			0x382a
+#define OV8865_INC_Y_ODD(v)			((v) & GENMASK(4, 0))
+#define OV8865_INC_Y_EVEN_REG			0x382b
+#define OV8865_INC_Y_EVEN(v)			((v) & GENMASK(4, 0))
+
+#define OV8865_ABLC_NUM_REG			0x3830
+#define OV8865_ABLC_NUM(v)			((v) & GENMASK(4, 0))
+
+#define OV8865_ZLINE_NUM_REG			0x3836
+#define OV8865_ZLINE_NUM(v)			((v) & GENMASK(4, 0))
+
+#define OV8865_AUTO_SIZE_CTRL_REG		0x3841
+#define OV8865_AUTO_SIZE_CTRL_OFFSET_Y_REG	BIT(5)
+#define OV8865_AUTO_SIZE_CTRL_OFFSET_X_REG	BIT(4)
+#define OV8865_AUTO_SIZE_CTRL_CROP_END_Y_REG	BIT(3)
+#define OV8865_AUTO_SIZE_CTRL_CROP_END_X_REG	BIT(2)
+#define OV8865_AUTO_SIZE_CTRL_CROP_START_Y_REG	BIT(1)
+#define OV8865_AUTO_SIZE_CTRL_CROP_START_X_REG	BIT(0)
+#define OV8865_AUTO_SIZE_X_OFFSET_H_REG		0x3842
+#define OV8865_AUTO_SIZE_X_OFFSET_L_REG		0x3843
+#define OV8865_AUTO_SIZE_Y_OFFSET_H_REG		0x3844
+#define OV8865_AUTO_SIZE_Y_OFFSET_L_REG		0x3845
+#define OV8865_AUTO_SIZE_BOUNDARIES_REG		0x3846
+#define OV8865_AUTO_SIZE_BOUNDARIES_Y(v)	(((v) << 4) & GENMASK(7, 4))
+#define OV8865_AUTO_SIZE_BOUNDARIES_X(v)	((v) & GENMASK(3, 0))
+
+/* PSRAM */
+
+#define OV8865_PSRAM_CTRL8_REG			0x3f08
+
+/* Black Level */
+
+#define OV8865_BLC_CTRL0_REG			0x4000
+#define OV8865_BLC_CTRL0_TRIG_RANGE_EN		BIT(7)
+#define OV8865_BLC_CTRL0_TRIG_FORMAT_EN		BIT(6)
+#define OV8865_BLC_CTRL0_TRIG_GAIN_EN		BIT(5)
+#define OV8865_BLC_CTRL0_TRIG_EXPOSURE_EN	BIT(4)
+#define OV8865_BLC_CTRL0_TRIG_MANUAL_EN		BIT(3)
+#define OV8865_BLC_CTRL0_FREEZE_EN		BIT(2)
+#define OV8865_BLC_CTRL0_ALWAYS_EN		BIT(1)
+#define OV8865_BLC_CTRL0_FILTER_EN		BIT(0)
+#define OV8865_BLC_CTRL1_REG			0x4001
+#define OV8865_BLC_CTRL1_DITHER_EN		BIT(7)
+#define OV8865_BLC_CTRL1_ZERO_LINE_DIFF_EN	BIT(6)
+#define OV8865_BLC_CTRL1_COL_SHIFT_256		(0 << 4)
+#define OV8865_BLC_CTRL1_COL_SHIFT_128		(1 << 4)
+#define OV8865_BLC_CTRL1_COL_SHIFT_64		(2 << 4)
+#define OV8865_BLC_CTRL1_COL_SHIFT_32		(3 << 4)
+#define OV8865_BLC_CTRL1_OFFSET_LIMIT_EN	BIT(2)
+#define OV8865_BLC_CTRL1_COLUMN_CANCEL_EN	BIT(1)
+#define OV8865_BLC_CTRL2_REG			0x4002
+#define OV8865_BLC_CTRL3_REG			0x4003
+#define OV8865_BLC_CTRL4_REG			0x4004
+#define OV8865_BLC_CTRL5_REG			0x4005
+#define OV8865_BLC_CTRL6_REG			0x4006
+#define OV8865_BLC_CTRL7_REG			0x4007
+#define OV8865_BLC_CTRL8_REG			0x4008
+#define OV8865_BLC_CTRL9_REG			0x4009
+#define OV8865_BLC_CTRLA_REG			0x400a
+#define OV8865_BLC_CTRLB_REG			0x400b
+#define OV8865_BLC_CTRLC_REG			0x400c
+#define OV8865_BLC_CTRLD_REG			0x400d
+#define OV8865_BLC_CTRLD_OFFSET_TRIGGER(v)	((v) & GENMASK(7, 0))
+
+#define OV8865_BLC_CTRL1F_REG			0x401f
+#define OV8865_BLC_CTRL1F_RB_REVERSE		BIT(3)
+#define OV8865_BLC_CTRL1F_INTERPOL_X_EN		BIT(2)
+#define OV8865_BLC_CTRL1F_INTERPOL_Y_EN		BIT(1)
+
+#define OV8865_BLC_ANCHOR_LEFT_START_H_REG	0x4020
+#define OV8865_BLC_ANCHOR_LEFT_START_H(v)	(((v) & GENMASK(11, 8)) >> 8)
+#define OV8865_BLC_ANCHOR_LEFT_START_L_REG	0x4021
+#define OV8865_BLC_ANCHOR_LEFT_START_L(v)	((v) & GENMASK(7, 0))
+#define OV8865_BLC_ANCHOR_LEFT_END_H_REG	0x4022
+#define OV8865_BLC_ANCHOR_LEFT_END_H(v)		(((v) & GENMASK(11, 8)) >> 8)
+#define OV8865_BLC_ANCHOR_LEFT_END_L_REG	0x4023
+#define OV8865_BLC_ANCHOR_LEFT_END_L(v)		((v) & GENMASK(7, 0))
+#define OV8865_BLC_ANCHOR_RIGHT_START_H_REG	0x4024
+#define OV8865_BLC_ANCHOR_RIGHT_START_H(v)	(((v) & GENMASK(11, 8)) >> 8)
+#define OV8865_BLC_ANCHOR_RIGHT_START_L_REG	0x4025
+#define OV8865_BLC_ANCHOR_RIGHT_START_L(v)	((v) & GENMASK(7, 0))
+#define OV8865_BLC_ANCHOR_RIGHT_END_H_REG	0x4026
+#define OV8865_BLC_ANCHOR_RIGHT_END_H(v)	(((v) & GENMASK(11, 8)) >> 8)
+#define OV8865_BLC_ANCHOR_RIGHT_END_L_REG	0x4027
+#define OV8865_BLC_ANCHOR_RIGHT_END_L(v)	((v) & GENMASK(7, 0))
+
+#define OV8865_BLC_TOP_ZLINE_START_REG		0x4028
+#define OV8865_BLC_TOP_ZLINE_START(v)		((v) & GENMASK(5, 0))
+#define OV8865_BLC_TOP_ZLINE_NUM_REG		0x4029
+#define OV8865_BLC_TOP_ZLINE_NUM(v)		((v) & GENMASK(4, 0))
+#define OV8865_BLC_TOP_BLKLINE_START_REG	0x402a
+#define OV8865_BLC_TOP_BLKLINE_START(v)		((v) & GENMASK(5, 0))
+#define OV8865_BLC_TOP_BLKLINE_NUM_REG		0x402b
+#define OV8865_BLC_TOP_BLKLINE_NUM(v)		((v) & GENMASK(4, 0))
+#define OV8865_BLC_BOT_ZLINE_START_REG		0x402c
+#define OV8865_BLC_BOT_ZLINE_START(v)		((v) & GENMASK(5, 0))
+#define OV8865_BLC_BOT_ZLINE_NUM_REG		0x402d
+#define OV8865_BLC_BOT_ZLINE_NUM(v)		((v) & GENMASK(4, 0))
+#define OV8865_BLC_BOT_BLKLINE_START_REG	0x402e
+#define OV8865_BLC_BOT_BLKLINE_START(v)		((v) & GENMASK(5, 0))
+#define OV8865_BLC_BOT_BLKLINE_NUM_REG		0x402f
+#define OV8865_BLC_BOT_BLKLINE_NUM(v)		((v) & GENMASK(4, 0))
+
+#define OV8865_BLC_OFFSET_LIMIT_REG		0x4034
+#define OV8865_BLC_OFFSET_LIMIT(v)		((v) & GENMASK(7, 0))
+
+/* VFIFO */
+
+#define OV8865_VFIFO_READ_START_H_REG		0x4600
+#define OV8865_VFIFO_READ_START_H(v)		(((v) & GENMASK(15, 8)) >> 8)
+#define OV8865_VFIFO_READ_START_L_REG		0x4601
+#define OV8865_VFIFO_READ_START_L(v)		((v) & GENMASK(7, 0))
+
+/* MIPI */
+
+#define OV8865_MIPI_CTRL0_REG			0x4800
+#define OV8865_MIPI_CTRL1_REG			0x4801
+#define OV8865_MIPI_CTRL2_REG			0x4802
+#define OV8865_MIPI_CTRL3_REG			0x4803
+#define OV8865_MIPI_CTRL4_REG			0x4804
+#define OV8865_MIPI_CTRL5_REG			0x4805
+#define OV8865_MIPI_CTRL6_REG			0x4806
+#define OV8865_MIPI_CTRL7_REG			0x4807
+#define OV8865_MIPI_CTRL8_REG			0x4808
+
+#define OV8865_MIPI_FCNT_MAX_H_REG		0x4810
+#define OV8865_MIPI_FCNT_MAX_L_REG		0x4811
+
+#define OV8865_MIPI_CTRL13_REG			0x4813
+#define OV8865_MIPI_CTRL14_REG			0x4814
+#define OV8865_MIPI_CTRL15_REG			0x4815
+#define OV8865_MIPI_EMBEDDED_DT_REG		0x4816
+
+#define OV8865_MIPI_HS_ZERO_MIN_H_REG		0x4818
+#define OV8865_MIPI_HS_ZERO_MIN_L_REG		0x4819
+#define OV8865_MIPI_HS_TRAIL_MIN_H_REG		0x481a
+#define OV8865_MIPI_HS_TRAIL_MIN_L_REG		0x481b
+#define OV8865_MIPI_CLK_ZERO_MIN_H_REG		0x481c
+#define OV8865_MIPI_CLK_ZERO_MIN_L_REG		0x481d
+#define OV8865_MIPI_CLK_PREPARE_MAX_REG		0x481e
+#define OV8865_MIPI_CLK_PREPARE_MIN_REG		0x481f
+#define OV8865_MIPI_CLK_POST_MIN_H_REG		0x4820
+#define OV8865_MIPI_CLK_POST_MIN_L_REG		0x4821
+#define OV8865_MIPI_CLK_TRAIL_MIN_H_REG		0x4822
+#define OV8865_MIPI_CLK_TRAIL_MIN_L_REG		0x4823
+#define OV8865_MIPI_LPX_P_MIN_H_REG		0x4824
+#define OV8865_MIPI_LPX_P_MIN_L_REG		0x4825
+#define OV8865_MIPI_HS_PREPARE_MIN_REG		0x4826
+#define OV8865_MIPI_HS_PREPARE_MAX_REG		0x4827
+#define OV8865_MIPI_HS_EXIT_MIN_H_REG		0x4828
+#define OV8865_MIPI_HS_EXIT_MIN_L_REG		0x4829
+#define OV8865_MIPI_UI_HS_ZERO_MIN_REG		0x482a
+#define OV8865_MIPI_UI_HS_TRAIL_MIN_REG		0x482b
+#define OV8865_MIPI_UI_CLK_ZERO_MIN_REG		0x482c
+#define OV8865_MIPI_UI_CLK_PREPARE_REG		0x482d
+#define OV8865_MIPI_UI_CLK_POST_MIN_REG		0x482e
+#define OV8865_MIPI_UI_CLK_TRAIL_MIN_REG	0x482f
+#define OV8865_MIPI_UI_LPX_P_MIN_REG		0x4830
+#define OV8865_MIPI_UI_HS_PREPARE_REG		0x4831
+#define OV8865_MIPI_UI_HS_EXIT_MIN_REG		0x4832
+#define OV8865_MIPI_PKT_START_SIZE_REG		0x4833
+
+#define OV8865_MIPI_PCLK_PERIOD_REG		0x4837
+#define OV8865_MIPI_LP_GPIO0_REG		0x4838
+#define OV8865_MIPI_LP_GPIO1_REG		0x4839
+
+#define OV8865_MIPI_CTRL3C_REG			0x483c
+#define OV8865_MIPI_LP_GPIO4_REG		0x483d
+
+#define OV8865_MIPI_CTRL4A_REG			0x484a
+#define OV8865_MIPI_CTRL4B_REG			0x484b
+#define OV8865_MIPI_CTRL4C_REG			0x484c
+#define OV8865_MIPI_LANE_TEST_PATTERN_REG	0x484d
+#define OV8865_MIPI_FRAME_END_DELAY_REG		0x484e
+#define OV8865_MIPI_CLOCK_TEST_PATTERN_REG	0x484f
+#define OV8865_MIPI_LANE_SEL01_REG		0x4850
+#define OV8865_MIPI_LANE_SEL01_LANE0(v)		(((v) << 0) & GENMASK(2, 0))
+#define OV8865_MIPI_LANE_SEL01_LANE1(v)		(((v) << 4) & GENMASK(6, 4))
+#define OV8865_MIPI_LANE_SEL23_REG		0x4851
+#define OV8865_MIPI_LANE_SEL23_LANE2(v)		(((v) << 0) & GENMASK(2, 0))
+#define OV8865_MIPI_LANE_SEL23_LANE3(v)		(((v) << 4) & GENMASK(6, 4))
+
+/* ISP */
+
+#define OV8865_ISP_CTRL0_REG			0x5000
+#define OV8865_ISP_CTRL0_LENC_EN		BIT(7)
+#define OV8865_ISP_CTRL0_WHITE_BALANCE_EN	BIT(4)
+#define OV8865_ISP_CTRL0_DPC_BLACK_EN		BIT(2)
+#define OV8865_ISP_CTRL0_DPC_WHITE_EN		BIT(1)
+#define OV8865_ISP_CTRL1_REG			0x5001
+#define OV8865_ISP_CTRL1_BLC_EN			BIT(0)
+#define OV8865_ISP_CTRL2_REG			0x5002
+#define OV8865_ISP_CTRL2_DEBUG			BIT(3)
+#define OV8865_ISP_CTRL2_VARIOPIXEL_EN		BIT(2)
+#define OV8865_ISP_CTRL2_VSYNC_LATCH_EN		BIT(0)
+#define OV8865_ISP_CTRL3_REG			0x5003
+
+#define OV8865_ISP_GAIN_RED_H_REG		0x5018
+#define OV8865_ISP_GAIN_RED_H(v)		(((v) & GENMASK(13, 6)) >> 6)
+#define OV8865_ISP_GAIN_RED_L_REG		0x5019
+#define OV8865_ISP_GAIN_RED_L(v)		((v) & GENMASK(5, 0))
+#define OV8865_ISP_GAIN_GREEN_H_REG		0x501a
+#define OV8865_ISP_GAIN_GREEN_H(v)		(((v) & GENMASK(13, 6)) >> 6)
+#define OV8865_ISP_GAIN_GREEN_L_REG		0x501b
+#define OV8865_ISP_GAIN_GREEN_L(v)		((v) & GENMASK(5, 0))
+#define OV8865_ISP_GAIN_BLUE_H_REG		0x501c
+#define OV8865_ISP_GAIN_BLUE_H(v)		(((v) & GENMASK(13, 6)) >> 6)
+#define OV8865_ISP_GAIN_BLUE_L_REG		0x501d
+#define OV8865_ISP_GAIN_BLUE_L(v)		((v) & GENMASK(5, 0))
+
+/* VarioPixel */
+
+#define OV8865_VAP_CTRL0_REG			0x5900
+#define OV8865_VAP_CTRL1_REG			0x5901
+#define OV8865_VAP_CTRL1_HSUB_COEF(v)		((((v) - 1) << 2) & \
+						 GENMASK(3, 2))
+#define OV8865_VAP_CTRL1_VSUB_COEF(v)		(((v) - 1) & GENMASK(1, 0))
+
+/* Pre-DSP */
+
+#define OV8865_PRE_CTRL0_REG			0x5e00
+#define OV8865_PRE_CTRL0_PATTERN_EN		BIT(7)
+#define OV8865_PRE_CTRL0_ROLLING_BAR_EN		BIT(6)
+#define OV8865_PRE_CTRL0_TRANSPARENT_MODE	BIT(5)
+#define OV8865_PRE_CTRL0_SQUARES_BW_MODE	BIT(4)
+#define OV8865_PRE_CTRL0_PATTERN_COLOR_BARS	0
+#define OV8865_PRE_CTRL0_PATTERN_RANDOM_DATA	1
+#define OV8865_PRE_CTRL0_PATTERN_COLOR_SQUARES	2
+#define OV8865_PRE_CTRL0_PATTERN_BLACK		3
+
+/* Pixel Array */
+
+#define OV8865_NATIVE_WIDTH			3296
+#define OV8865_NATIVE_HEIGHT			2528
+#define OV8865_ACTIVE_START_LEFT		16
+#define OV8865_ACTIVE_START_TOP			40
+#define OV8865_ACTIVE_WIDTH			3264
+#define OV8865_ACTIVE_HEIGHT			2448
+
+/* Macros */
+
+#define ov8865_subdev_sensor(s) \
+	container_of(s, struct ov8865_sensor, subdev)
+
+#define ov8865_ctrl_subdev(c) \
+	(&container_of((c)->handler, struct ov8865_sensor, \
+		       ctrls.handler)->subdev)
+
+/* Data structures */
+
+struct ov8865_register_value {
+	u16 address;
+	u8 value;
+	unsigned int delay_ms;
+};
+
+/*
+ * PLL1 Clock Tree:
+ *
+ * +-< EXTCLK
+ * |
+ * +-+ pll_pre_div_half (0x30a [0])
+ *   |
+ *   +-+ pll_pre_div (0x300 [2:0], special values:
+ *     |              0: 1, 1: 1.5, 3: 2.5, 4: 3, 5: 4, 7: 8)
+ *     +-+ pll_mul (0x301 [1:0], 0x302 [7:0])
+ *       |
+ *       +-+ m_div (0x303 [3:0])
+ *       | |
+ *       | +-> PHY_SCLK
+ *       | |
+ *       | +-+ mipi_div (0x304 [1:0], special values: 0: 4, 1: 5, 2: 6, 3: 8)
+ *       |   |
+ *       |   +-+ pclk_div (0x3020 [3])
+ *       |     |
+ *       |     +-> PCLK
+ *       |
+ *       +-+ sys_pre_div (0x305 [1:0], special values: 0: 3, 1: 4, 2: 5, 3: 6)
+ *         |
+ *         +-+ sys_div (0x306 [0])
+ *           |
+ *           +-+ sys_sel (0x3032 [7], 0: PLL1, 1: PLL2)
+ *             |
+ *             +-+ sclk_sel (0x3033 [1], 0: sys_sel, 1: PLL2 DAC_CLK)
+ *               |
+ *               +-+ sclk_pre_div (0x3106 [3:2], special values:
+ *                 |               0: 1, 1: 2, 2: 4, 3: 1)
+ *                 |
+ *                 +-+ sclk_div (0x3106 [7:4], special values: 0: 1)
+ *                   |
+ *                   +-> SCLK
+ */
+
+struct ov8865_pll1_config {
+	unsigned int pll_pre_div_half;
+	unsigned int pll_pre_div;
+	unsigned int pll_mul;
+	unsigned int m_div;
+	unsigned int mipi_div;
+	unsigned int pclk_div;
+	unsigned int sys_pre_div;
+	unsigned int sys_div;
+};
+
+/*
+ * PLL2 Clock Tree:
+ *
+ * +-< EXTCLK
+ * |
+ * +-+ pll_pre_div_half (0x312 [4])
+ *   |
+ *   +-+ pll_pre_div (0x30b [2:0], special values:
+ *     |              0: 1, 1: 1.5, 3: 2.5, 4: 3, 5: 4, 7: 8)
+ *     +-+ pll_mul (0x30c [1:0], 0x30d [7:0])
+ *       |
+ *       +-+ dac_div (0x312 [3:0])
+ *       | |
+ *       | +-> DAC_CLK
+ *       |
+ *       +-+ sys_pre_div (0x30f [3:0])
+ *         |
+ *         +-+ sys_div (0x30e [2:0], special values:
+ *           |          0: 1, 1: 1.5, 3: 2.5, 4: 3, 5: 3.5, 6: 4, 7:5)
+ *           |
+ *           +-+ sys_sel (0x3032 [7], 0: PLL1, 1: PLL2)
+ *             |
+ *             +-+ sclk_sel (0x3033 [1], 0: sys_sel, 1: PLL2 DAC_CLK)
+ *               |
+ *               +-+ sclk_pre_div (0x3106 [3:2], special values:
+ *                 |               0: 1, 1: 2, 2: 4, 3: 1)
+ *                 |
+ *                 +-+ sclk_div (0x3106 [7:4], special values: 0: 1)
+ *                   |
+ *                   +-> SCLK
+ */
+
+struct ov8865_pll2_config {
+	unsigned int pll_pre_div_half;
+	unsigned int pll_pre_div;
+	unsigned int pll_mul;
+	unsigned int dac_div;
+	unsigned int sys_pre_div;
+	unsigned int sys_div;
+};
+
+struct ov8865_sclk_config {
+	unsigned int sys_sel;
+	unsigned int sclk_sel;
+	unsigned int sclk_pre_div;
+	unsigned int sclk_div;
+};
+
+struct ov8865_pll_configs {
+	const struct ov8865_pll1_config *pll1_config;
+	const struct ov8865_pll2_config *pll2_config_native;
+	const struct ov8865_pll2_config *pll2_config_binning;
+};
+
+/* Clock rate */
+
+enum extclk_rate {
+	OV8865_19_2_MHZ,
+	OV8865_24_MHZ,
+	OV8865_NUM_SUPPORTED_RATES
+};
+
+static const unsigned long supported_extclk_rates[] = {
+	[OV8865_19_2_MHZ] = 19200000,
+	[OV8865_24_MHZ] = 24000000,
+};
+
+/*
+ * General formulas for (array-centered) mode calculation:
+ * - photo_array_width = 3296
+ * - crop_start_x = (photo_array_width - output_size_x) / 2
+ * - crop_end_x = crop_start_x + offset_x + output_size_x - 1
+ *
+ * - photo_array_height = 2480
+ * - crop_start_y = (photo_array_height - output_size_y) / 2
+ * - crop_end_y = crop_start_y + offset_y + output_size_y - 1
+ */
+
+struct ov8865_mode {
+	unsigned int crop_start_x;
+	unsigned int offset_x;
+	unsigned int output_size_x;
+	unsigned int crop_end_x;
+	unsigned int hts;
+
+	unsigned int crop_start_y;
+	unsigned int offset_y;
+	unsigned int output_size_y;
+	unsigned int crop_end_y;
+	unsigned int vts;
+
+	/* With auto size, only output and total sizes need to be set. */
+	bool size_auto;
+	unsigned int size_auto_boundary_x;
+	unsigned int size_auto_boundary_y;
+
+	bool binning_x;
+	bool binning_y;
+	bool variopixel;
+	unsigned int variopixel_hsub_coef;
+	unsigned int variopixel_vsub_coef;
+
+	/* Bits for the format register, used for binning. */
+	bool sync_hbin;
+	bool horz_var2;
+
+	unsigned int inc_x_odd;
+	unsigned int inc_x_even;
+	unsigned int inc_y_odd;
+	unsigned int inc_y_even;
+
+	unsigned int vfifo_read_start;
+
+	unsigned int ablc_num;
+	unsigned int zline_num;
+
+	unsigned int blc_top_zero_line_start;
+	unsigned int blc_top_zero_line_num;
+	unsigned int blc_top_black_line_start;
+	unsigned int blc_top_black_line_num;
+
+	unsigned int blc_bottom_zero_line_start;
+	unsigned int blc_bottom_zero_line_num;
+	unsigned int blc_bottom_black_line_start;
+	unsigned int blc_bottom_black_line_num;
+
+	u8 blc_col_shift_mask;
+
+	unsigned int blc_anchor_left_start;
+	unsigned int blc_anchor_left_end;
+	unsigned int blc_anchor_right_start;
+	unsigned int blc_anchor_right_end;
+
+	bool pll2_binning;
+
+	const struct ov8865_register_value *register_values;
+	unsigned int register_values_count;
+};
+
+struct ov8865_state {
+	const struct ov8865_mode *mode;
+	u32 mbus_code;
+
+	bool streaming;
+};
+
+struct ov8865_ctrls {
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *pixel_rate;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *exposure;
+
+	struct v4l2_ctrl_handler handler;
+};
+
+struct ov8865_sensor {
+	struct device *dev;
+	struct i2c_client *i2c_client;
+	struct gpio_desc *reset;
+	struct gpio_desc *powerdown;
+	struct regulator *avdd;
+	struct regulator *dvdd;
+	struct regulator *dovdd;
+
+	unsigned long extclk_rate;
+	const struct ov8865_pll_configs *pll_configs;
+	struct clk *extclk;
+
+	struct v4l2_fwnode_endpoint endpoint;
+	struct v4l2_subdev subdev;
+	struct media_pad pad;
+
+	struct mutex mutex;
+
+	struct ov8865_state state;
+	struct ov8865_ctrls ctrls;
+};
+
+/* Static definitions */
+
+/*
+ * PHY_SCLK = 720 MHz
+ * MIPI_PCLK = 90 MHz
+ */
+
+static const struct ov8865_pll1_config ov8865_pll1_config_native_19_2mhz = {
+		.pll_pre_div_half	= 1,
+		.pll_pre_div		= 2,
+		.pll_mul		= 75,
+		.m_div			= 1,
+		.mipi_div		= 3,
+		.pclk_div		= 1,
+		.sys_pre_div		= 1,
+		.sys_div		= 2,
+};
+
+static const struct ov8865_pll1_config ov8865_pll1_config_native_24mhz = {
+		.pll_pre_div_half	= 1,
+		.pll_pre_div		= 0,
+		.pll_mul		= 30,
+		.m_div			= 1,
+		.mipi_div		= 3,
+		.pclk_div		= 1,
+		.sys_pre_div		= 1,
+		.sys_div		= 2,
+};
+
+/*
+ * DAC_CLK = 360 MHz
+ * SCLK = 144 MHz
+ */
+
+static const struct ov8865_pll2_config ov8865_pll2_config_native_19_2mhz = {
+		.pll_pre_div_half	= 1,
+		.pll_pre_div		= 5,
+		.pll_mul		= 75,
+		.dac_div		= 1,
+		.sys_pre_div		= 1,
+		.sys_div		= 3,
+};
+
+static const struct ov8865_pll2_config ov8865_pll2_config_native_24mhz = {
+		.pll_pre_div_half	= 1,
+		.pll_pre_div		= 0,
+		.pll_mul		= 30,
+		.dac_div		= 2,
+		.sys_pre_div		= 5,
+		.sys_div		= 0,
+};
+
+/*
+ * DAC_CLK = 360 MHz
+ * SCLK = 72 MHz
+ */
+
+static const struct ov8865_pll2_config ov8865_pll2_config_binning_19_2mhz = {
+	.pll_pre_div_half	= 1,
+	.pll_pre_div		= 2,
+	.pll_mul		= 75,
+	.dac_div		= 2,
+	.sys_pre_div		= 10,
+	.sys_div		= 0,
+};
+
+static const struct ov8865_pll2_config ov8865_pll2_config_binning_24mhz = {
+	.pll_pre_div_half	= 1,
+	.pll_pre_div		= 0,
+	.pll_mul		= 30,
+	.dac_div		= 2,
+	.sys_pre_div		= 10,
+	.sys_div		= 0,
+};
+
+static const struct ov8865_pll_configs ov8865_pll_configs_19_2mhz = {
+	.pll1_config = &ov8865_pll1_config_native_19_2mhz,
+	.pll2_config_native = &ov8865_pll2_config_native_19_2mhz,
+	.pll2_config_binning = &ov8865_pll2_config_binning_19_2mhz,
+};
+
+static const struct ov8865_pll_configs ov8865_pll_configs_24mhz = {
+	.pll1_config = &ov8865_pll1_config_native_24mhz,
+	.pll2_config_native = &ov8865_pll2_config_native_24mhz,
+	.pll2_config_binning = &ov8865_pll2_config_binning_24mhz,
+};
+
+static const struct ov8865_pll_configs *ov8865_pll_configs[] = {
+	&ov8865_pll_configs_19_2mhz,
+	&ov8865_pll_configs_24mhz,
+};
+
+static const struct ov8865_sclk_config ov8865_sclk_config_native = {
+	.sys_sel		= 1,
+	.sclk_sel		= 0,
+	.sclk_pre_div		= 0,
+	.sclk_div		= 0,
+};
+
+static const struct ov8865_register_value ov8865_register_values_native[] = {
+	/* Sensor */
+
+	{ 0x3700, 0x48 },
+	{ 0x3701, 0x18 },
+	{ 0x3702, 0x50 },
+	{ 0x3703, 0x32 },
+	{ 0x3704, 0x28 },
+	{ 0x3706, 0x70 },
+	{ 0x3707, 0x08 },
+	{ 0x3708, 0x48 },
+	{ 0x3709, 0x80 },
+	{ 0x370a, 0x01 },
+	{ 0x370b, 0x70 },
+	{ 0x370c, 0x07 },
+	{ 0x3718, 0x14 },
+	{ 0x3712, 0x44 },
+	{ 0x371e, 0x31 },
+	{ 0x371f, 0x7f },
+	{ 0x3720, 0x0a },
+	{ 0x3721, 0x0a },
+	{ 0x3724, 0x04 },
+	{ 0x3725, 0x04 },
+	{ 0x3726, 0x0c },
+	{ 0x3728, 0x0a },
+	{ 0x3729, 0x03 },
+	{ 0x372a, 0x06 },
+	{ 0x372b, 0xa6 },
+	{ 0x372c, 0xa6 },
+	{ 0x372d, 0xa6 },
+	{ 0x372e, 0x0c },
+	{ 0x372f, 0x20 },
+	{ 0x3730, 0x02 },
+	{ 0x3731, 0x0c },
+	{ 0x3732, 0x28 },
+	{ 0x3736, 0x30 },
+	{ 0x373a, 0x04 },
+	{ 0x373b, 0x18 },
+	{ 0x373c, 0x14 },
+	{ 0x373e, 0x06 },
+	{ 0x375a, 0x0c },
+	{ 0x375b, 0x26 },
+	{ 0x375d, 0x04 },
+	{ 0x375f, 0x28 },
+	{ 0x3767, 0x1e },
+	{ 0x3772, 0x46 },
+	{ 0x3773, 0x04 },
+	{ 0x3774, 0x2c },
+	{ 0x3775, 0x13 },
+	{ 0x3776, 0x10 },
+	{ 0x37a0, 0x88 },
+	{ 0x37a1, 0x7a },
+	{ 0x37a2, 0x7a },
+	{ 0x37a3, 0x02 },
+	{ 0x37a5, 0x09 },
+	{ 0x37a7, 0x88 },
+	{ 0x37a8, 0xb0 },
+	{ 0x37a9, 0xb0 },
+	{ 0x37aa, 0x88 },
+	{ 0x37ab, 0x5c },
+	{ 0x37ac, 0x5c },
+	{ 0x37ad, 0x55 },
+	{ 0x37ae, 0x19 },
+	{ 0x37af, 0x19 },
+	{ 0x37b3, 0x84 },
+	{ 0x37b4, 0x84 },
+	{ 0x37b5, 0x66 },
+
+	/* PSRAM */
+
+	{ OV8865_PSRAM_CTRL8_REG, 0x16 },
+
+	/* ADC Sync */
+
+	{ 0x4500, 0x68 },
+};
+
+static const struct ov8865_register_value ov8865_register_values_binning[] = {
+	/* Sensor */
+
+	{ 0x3700, 0x24 },
+	{ 0x3701, 0x0c },
+	{ 0x3702, 0x28 },
+	{ 0x3703, 0x19 },
+	{ 0x3704, 0x14 },
+	{ 0x3706, 0x38 },
+	{ 0x3707, 0x04 },
+	{ 0x3708, 0x24 },
+	{ 0x3709, 0x40 },
+	{ 0x370a, 0x00 },
+	{ 0x370b, 0xb8 },
+	{ 0x370c, 0x04 },
+	{ 0x3718, 0x12 },
+	{ 0x3712, 0x42 },
+	{ 0x371e, 0x19 },
+	{ 0x371f, 0x40 },
+	{ 0x3720, 0x05 },
+	{ 0x3721, 0x05 },
+	{ 0x3724, 0x02 },
+	{ 0x3725, 0x02 },
+	{ 0x3726, 0x06 },
+	{ 0x3728, 0x05 },
+	{ 0x3729, 0x02 },
+	{ 0x372a, 0x03 },
+	{ 0x372b, 0x53 },
+	{ 0x372c, 0xa3 },
+	{ 0x372d, 0x53 },
+	{ 0x372e, 0x06 },
+	{ 0x372f, 0x10 },
+	{ 0x3730, 0x01 },
+	{ 0x3731, 0x06 },
+	{ 0x3732, 0x14 },
+	{ 0x3736, 0x20 },
+	{ 0x373a, 0x02 },
+	{ 0x373b, 0x0c },
+	{ 0x373c, 0x0a },
+	{ 0x373e, 0x03 },
+	{ 0x375a, 0x06 },
+	{ 0x375b, 0x13 },
+	{ 0x375d, 0x02 },
+	{ 0x375f, 0x14 },
+	{ 0x3767, 0x1c },
+	{ 0x3772, 0x23 },
+	{ 0x3773, 0x02 },
+	{ 0x3774, 0x16 },
+	{ 0x3775, 0x12 },
+	{ 0x3776, 0x08 },
+	{ 0x37a0, 0x44 },
+	{ 0x37a1, 0x3d },
+	{ 0x37a2, 0x3d },
+	{ 0x37a3, 0x01 },
+	{ 0x37a5, 0x08 },
+	{ 0x37a7, 0x44 },
+	{ 0x37a8, 0x58 },
+	{ 0x37a9, 0x58 },
+	{ 0x37aa, 0x44 },
+	{ 0x37ab, 0x2e },
+	{ 0x37ac, 0x2e },
+	{ 0x37ad, 0x33 },
+	{ 0x37ae, 0x0d },
+	{ 0x37af, 0x0d },
+	{ 0x37b3, 0x42 },
+	{ 0x37b4, 0x42 },
+	{ 0x37b5, 0x33 },
+
+	/* PSRAM */
+
+	{ OV8865_PSRAM_CTRL8_REG, 0x0b },
+
+	/* ADC Sync */
+
+	{ 0x4500, 0x40 },
+};
+
+static const struct ov8865_mode ov8865_modes[] = {
+	/* 3264x2448 */
+	{
+		/* Horizontal */
+		.output_size_x			= 3264,
+		.hts				= 3888,
+
+		/* Vertical */
+		.output_size_y			= 2448,
+		.vts				= 2470,
+
+		.size_auto			= true,
+		.size_auto_boundary_x		= 8,
+		.size_auto_boundary_y		= 4,
+
+		/* Subsample increase */
+		.inc_x_odd			= 1,
+		.inc_x_even			= 1,
+		.inc_y_odd			= 1,
+		.inc_y_even			= 1,
+
+		/* VFIFO */
+		.vfifo_read_start		= 16,
+
+		.ablc_num			= 4,
+		.zline_num			= 1,
+
+		/* Black Level */
+
+		.blc_top_zero_line_start	= 0,
+		.blc_top_zero_line_num		= 2,
+		.blc_top_black_line_start	= 4,
+		.blc_top_black_line_num		= 4,
+
+		.blc_bottom_zero_line_start	= 2,
+		.blc_bottom_zero_line_num	= 2,
+		.blc_bottom_black_line_start	= 8,
+		.blc_bottom_black_line_num	= 2,
+
+		.blc_anchor_left_start		= 576,
+		.blc_anchor_left_end		= 831,
+		.blc_anchor_right_start		= 1984,
+		.blc_anchor_right_end		= 2239,
+
+		/* PLL */
+		.pll2_binning			= false,
+
+		/* Registers */
+		.register_values	= ov8865_register_values_native,
+		.register_values_count	=
+			ARRAY_SIZE(ov8865_register_values_native),
+	},
+	/* 3264x1836 */
+	{
+		/* Horizontal */
+		.output_size_x			= 3264,
+		.hts				= 3888,
+
+		/* Vertical */
+		.output_size_y			= 1836,
+		.vts				= 2470,
+
+		.size_auto			= true,
+		.size_auto_boundary_x		= 8,
+		.size_auto_boundary_y		= 4,
+
+		/* Subsample increase */
+		.inc_x_odd			= 1,
+		.inc_x_even			= 1,
+		.inc_y_odd			= 1,
+		.inc_y_even			= 1,
+
+		/* VFIFO */
+		.vfifo_read_start		= 16,
+
+		.ablc_num			= 4,
+		.zline_num			= 1,
+
+		/* Black Level */
+
+		.blc_top_zero_line_start	= 0,
+		.blc_top_zero_line_num		= 2,
+		.blc_top_black_line_start	= 4,
+		.blc_top_black_line_num		= 4,
+
+		.blc_bottom_zero_line_start	= 2,
+		.blc_bottom_zero_line_num	= 2,
+		.blc_bottom_black_line_start	= 8,
+		.blc_bottom_black_line_num	= 2,
+
+		.blc_anchor_left_start		= 576,
+		.blc_anchor_left_end		= 831,
+		.blc_anchor_right_start		= 1984,
+		.blc_anchor_right_end		= 2239,
+
+		/* PLL */
+		.pll2_binning			= false,
+
+		/* Registers */
+		.register_values	= ov8865_register_values_native,
+		.register_values_count	=
+			ARRAY_SIZE(ov8865_register_values_native),
+	},
+	/* 1632x1224 */
+	{
+		/* Horizontal */
+		.output_size_x			= 1632,
+		.hts				= 1923,
+
+		/* Vertical */
+		.output_size_y			= 1224,
+		.vts				= 1248,
+
+		.size_auto			= true,
+		.size_auto_boundary_x		= 8,
+		.size_auto_boundary_y		= 8,
+
+		/* Subsample increase */
+		.inc_x_odd			= 3,
+		.inc_x_even			= 1,
+		.inc_y_odd			= 3,
+		.inc_y_even			= 1,
+
+		/* Binning */
+		.binning_y			= true,
+		.sync_hbin			= true,
+
+		/* VFIFO */
+		.vfifo_read_start		= 116,
+
+		.ablc_num			= 8,
+		.zline_num			= 2,
+
+		/* Black Level */
+
+		.blc_top_zero_line_start	= 0,
+		.blc_top_zero_line_num		= 2,
+		.blc_top_black_line_start	= 4,
+		.blc_top_black_line_num		= 4,
+
+		.blc_bottom_zero_line_start	= 2,
+		.blc_bottom_zero_line_num	= 2,
+		.blc_bottom_black_line_start	= 8,
+		.blc_bottom_black_line_num	= 2,
+
+		.blc_anchor_left_start		= 288,
+		.blc_anchor_left_end		= 415,
+		.blc_anchor_right_start		= 992,
+		.blc_anchor_right_end		= 1119,
+
+		/* PLL */
+		.pll2_binning			= true,
+
+		/* Registers */
+		.register_values	= ov8865_register_values_binning,
+		.register_values_count	=
+			ARRAY_SIZE(ov8865_register_values_binning),
+	},
+	/* 800x600 (SVGA) */
+	{
+		/* Horizontal */
+		.output_size_x			= 800,
+		.hts				= 1250,
+
+		/* Vertical */
+		.output_size_y			= 600,
+		.vts				= 640,
+
+		.size_auto			= true,
+		.size_auto_boundary_x		= 8,
+		.size_auto_boundary_y		= 8,
+
+		/* Subsample increase */
+		.inc_x_odd			= 3,
+		.inc_x_even			= 1,
+		.inc_y_odd			= 5,
+		.inc_y_even			= 3,
+
+		/* Binning */
+		.binning_y			= true,
+		.variopixel			= true,
+		.variopixel_hsub_coef		= 2,
+		.variopixel_vsub_coef		= 1,
+		.sync_hbin			= true,
+		.horz_var2			= true,
+
+		/* VFIFO */
+		.vfifo_read_start		= 80,
+
+		.ablc_num			= 8,
+		.zline_num			= 2,
+
+		/* Black Level */
+
+		.blc_top_zero_line_start	= 0,
+		.blc_top_zero_line_num		= 2,
+		.blc_top_black_line_start	= 2,
+		.blc_top_black_line_num		= 2,
+
+		.blc_bottom_zero_line_start	= 0,
+		.blc_bottom_zero_line_num	= 0,
+		.blc_bottom_black_line_start	= 4,
+		.blc_bottom_black_line_num	= 2,
+
+		.blc_col_shift_mask	= OV8865_BLC_CTRL1_COL_SHIFT_128,
+
+		.blc_anchor_left_start		= 288,
+		.blc_anchor_left_end		= 415,
+		.blc_anchor_right_start		= 992,
+		.blc_anchor_right_end		= 1119,
+
+		/* PLL */
+		.pll2_binning			= true,
+
+		/* Registers */
+		.register_values	= ov8865_register_values_binning,
+		.register_values_count	=
+			ARRAY_SIZE(ov8865_register_values_binning),
+	},
+};
+
+static const u32 ov8865_mbus_codes[] = {
+	MEDIA_BUS_FMT_SBGGR10_1X10,
+};
+
+static const struct ov8865_register_value ov8865_init_sequence[] = {
+	/* Analog */
+
+	{ 0x3604, 0x04 },
+	{ 0x3602, 0x30 },
+	{ 0x3605, 0x00 },
+	{ 0x3607, 0x20 },
+	{ 0x3608, 0x11 },
+	{ 0x3609, 0x68 },
+	{ 0x360a, 0x40 },
+	{ 0x360c, 0xdd },
+	{ 0x360e, 0x0c },
+	{ 0x3610, 0x07 },
+	{ 0x3612, 0x86 },
+	{ 0x3613, 0x58 },
+	{ 0x3614, 0x28 },
+	{ 0x3617, 0x40 },
+	{ 0x3618, 0x5a },
+	{ 0x3619, 0x9b },
+	{ 0x361c, 0x00 },
+	{ 0x361d, 0x60 },
+	{ 0x3631, 0x60 },
+	{ 0x3633, 0x10 },
+	{ 0x3634, 0x10 },
+	{ 0x3635, 0x10 },
+	{ 0x3636, 0x10 },
+	{ 0x3638, 0xff },
+	{ 0x3641, 0x55 },
+	{ 0x3646, 0x86 },
+	{ 0x3647, 0x27 },
+	{ 0x364a, 0x1b },
+
+	/* Sensor */
+
+	{ 0x3700, 0x24 },
+	{ 0x3701, 0x0c },
+	{ 0x3702, 0x28 },
+	{ 0x3703, 0x19 },
+	{ 0x3704, 0x14 },
+	{ 0x3705, 0x00 },
+	{ 0x3706, 0x38 },
+	{ 0x3707, 0x04 },
+	{ 0x3708, 0x24 },
+	{ 0x3709, 0x40 },
+	{ 0x370a, 0x00 },
+	{ 0x370b, 0xb8 },
+	{ 0x370c, 0x04 },
+	{ 0x3718, 0x12 },
+	{ 0x3719, 0x31 },
+	{ 0x3712, 0x42 },
+	{ 0x3714, 0x12 },
+	{ 0x371e, 0x19 },
+	{ 0x371f, 0x40 },
+	{ 0x3720, 0x05 },
+	{ 0x3721, 0x05 },
+	{ 0x3724, 0x02 },
+	{ 0x3725, 0x02 },
+	{ 0x3726, 0x06 },
+	{ 0x3728, 0x05 },
+	{ 0x3729, 0x02 },
+	{ 0x372a, 0x03 },
+	{ 0x372b, 0x53 },
+	{ 0x372c, 0xa3 },
+	{ 0x372d, 0x53 },
+	{ 0x372e, 0x06 },
+	{ 0x372f, 0x10 },
+	{ 0x3730, 0x01 },
+	{ 0x3731, 0x06 },
+	{ 0x3732, 0x14 },
+	{ 0x3733, 0x10 },
+	{ 0x3734, 0x40 },
+	{ 0x3736, 0x20 },
+	{ 0x373a, 0x02 },
+	{ 0x373b, 0x0c },
+	{ 0x373c, 0x0a },
+	{ 0x373e, 0x03 },
+	{ 0x3755, 0x40 },
+	{ 0x3758, 0x00 },
+	{ 0x3759, 0x4c },
+	{ 0x375a, 0x06 },
+	{ 0x375b, 0x13 },
+	{ 0x375c, 0x40 },
+	{ 0x375d, 0x02 },
+	{ 0x375e, 0x00 },
+	{ 0x375f, 0x14 },
+	{ 0x3767, 0x1c },
+	{ 0x3768, 0x04 },
+	{ 0x3769, 0x20 },
+	{ 0x376c, 0xc0 },
+	{ 0x376d, 0xc0 },
+	{ 0x376a, 0x08 },
+	{ 0x3761, 0x00 },
+	{ 0x3762, 0x00 },
+	{ 0x3763, 0x00 },
+	{ 0x3766, 0xff },
+	{ 0x376b, 0x42 },
+	{ 0x3772, 0x23 },
+	{ 0x3773, 0x02 },
+	{ 0x3774, 0x16 },
+	{ 0x3775, 0x12 },
+	{ 0x3776, 0x08 },
+	{ 0x37a0, 0x44 },
+	{ 0x37a1, 0x3d },
+	{ 0x37a2, 0x3d },
+	{ 0x37a3, 0x01 },
+	{ 0x37a4, 0x00 },
+	{ 0x37a5, 0x08 },
+	{ 0x37a6, 0x00 },
+	{ 0x37a7, 0x44 },
+	{ 0x37a8, 0x58 },
+	{ 0x37a9, 0x58 },
+	{ 0x3760, 0x00 },
+	{ 0x376f, 0x01 },
+	{ 0x37aa, 0x44 },
+	{ 0x37ab, 0x2e },
+	{ 0x37ac, 0x2e },
+	{ 0x37ad, 0x33 },
+	{ 0x37ae, 0x0d },
+	{ 0x37af, 0x0d },
+	{ 0x37b0, 0x00 },
+	{ 0x37b1, 0x00 },
+	{ 0x37b2, 0x00 },
+	{ 0x37b3, 0x42 },
+	{ 0x37b4, 0x42 },
+	{ 0x37b5, 0x33 },
+	{ 0x37b6, 0x00 },
+	{ 0x37b7, 0x00 },
+	{ 0x37b8, 0x00 },
+	{ 0x37b9, 0xff },
+
+	/* ADC Sync */
+
+	{ 0x4503, 0x10 },
+};
+
+static const s64 ov8865_link_freq_menu[] = {
+	360000000,
+};
+
+static const char *const ov8865_test_pattern_menu[] = {
+	"Disabled",
+	"Random data",
+	"Color bars",
+	"Color bars with rolling bar",
+	"Color squares",
+	"Color squares with rolling bar"
+};
+
+static const u8 ov8865_test_pattern_bits[] = {
+	0,
+	OV8865_PRE_CTRL0_PATTERN_EN | OV8865_PRE_CTRL0_PATTERN_RANDOM_DATA,
+	OV8865_PRE_CTRL0_PATTERN_EN | OV8865_PRE_CTRL0_PATTERN_COLOR_BARS,
+	OV8865_PRE_CTRL0_PATTERN_EN | OV8865_PRE_CTRL0_ROLLING_BAR_EN |
+	OV8865_PRE_CTRL0_PATTERN_COLOR_BARS,
+	OV8865_PRE_CTRL0_PATTERN_EN | OV8865_PRE_CTRL0_PATTERN_COLOR_SQUARES,
+	OV8865_PRE_CTRL0_PATTERN_EN | OV8865_PRE_CTRL0_ROLLING_BAR_EN |
+	OV8865_PRE_CTRL0_PATTERN_COLOR_SQUARES,
+};
+
+/* Input/Output */
+
+static int ov8865_read(struct ov8865_sensor *sensor, u16 address, u8 *value)
+{
+	unsigned char data[2] = { address >> 8, address & 0xff };
+	struct i2c_client *client = sensor->i2c_client;
+	int ret;
+
+	ret = i2c_master_send(client, data, sizeof(data));
+	if (ret < 0) {
+		dev_dbg(&client->dev, "i2c send error at address %#04x\n",
+			address);
+		return ret;
+	}
+
+	ret = i2c_master_recv(client, value, 1);
+	if (ret < 0) {
+		dev_dbg(&client->dev, "i2c recv error at address %#04x\n",
+			address);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ov8865_write(struct ov8865_sensor *sensor, u16 address, u8 value)
+{
+	unsigned char data[3] = { address >> 8, address & 0xff, value };
+	struct i2c_client *client = sensor->i2c_client;
+	int ret;
+
+	ret = i2c_master_send(client, data, sizeof(data));
+	if (ret < 0) {
+		dev_dbg(&client->dev, "i2c send error at address %#04x\n",
+			address);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ov8865_write_sequence(struct ov8865_sensor *sensor,
+				 const struct ov8865_register_value *sequence,
+				 unsigned int sequence_count)
+{
+	unsigned int i;
+	int ret = 0;
+
+	for (i = 0; i < sequence_count; i++) {
+		ret = ov8865_write(sensor, sequence[i].address,
+				   sequence[i].value);
+		if (ret)
+			break;
+
+		if (sequence[i].delay_ms)
+			msleep(sequence[i].delay_ms);
+	}
+
+	return ret;
+}
+
+static int ov8865_update_bits(struct ov8865_sensor *sensor, u16 address,
+			      u8 mask, u8 bits)
+{
+	u8 value = 0;
+	int ret;
+
+	ret = ov8865_read(sensor, address, &value);
+	if (ret)
+		return ret;
+
+	value &= ~mask;
+	value |= bits;
+
+	return ov8865_write(sensor, address, value);
+}
+
+/* Sensor */
+
+static int ov8865_sw_reset(struct ov8865_sensor *sensor)
+{
+	return ov8865_write(sensor, OV8865_SW_RESET_REG, OV8865_SW_RESET_RESET);
+}
+
+static int ov8865_sw_standby(struct ov8865_sensor *sensor, int standby)
+{
+	u8 value = 0;
+
+	if (!standby)
+		value = OV8865_SW_STANDBY_STREAM_ON;
+
+	return ov8865_write(sensor, OV8865_SW_STANDBY_REG, value);
+}
+
+static int ov8865_chip_id_check(struct ov8865_sensor *sensor)
+{
+	u16 regs[] = { OV8865_CHIP_ID_HH_REG, OV8865_CHIP_ID_H_REG,
+		       OV8865_CHIP_ID_L_REG };
+	u8 values[] = { OV8865_CHIP_ID_HH_VALUE, OV8865_CHIP_ID_H_VALUE,
+			OV8865_CHIP_ID_L_VALUE };
+	unsigned int i;
+	u8 value;
+	int ret;
+
+	for (i = 0; i < ARRAY_SIZE(regs); i++) {
+		ret = ov8865_read(sensor, regs[i], &value);
+		if (ret < 0)
+			return ret;
+
+		if (value != values[i]) {
+			dev_err(sensor->dev,
+				"chip id value mismatch: %#x instead of %#x\n",
+				value, values[i]);
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+static int ov8865_charge_pump_configure(struct ov8865_sensor *sensor)
+{
+	return ov8865_write(sensor, OV8865_PUMP_CLK_DIV_REG,
+			    OV8865_PUMP_CLK_DIV_PUMP_P(1));
+}
+
+static int ov8865_mipi_configure(struct ov8865_sensor *sensor)
+{
+	struct v4l2_mbus_config_mipi_csi2 *bus_mipi_csi2 =
+		&sensor->endpoint.bus.mipi_csi2;
+	unsigned int lanes_count = bus_mipi_csi2->num_data_lanes;
+	int ret;
+
+	ret = ov8865_write(sensor, OV8865_MIPI_SC_CTRL0_REG,
+			   OV8865_MIPI_SC_CTRL0_LANES(lanes_count) |
+			   OV8865_MIPI_SC_CTRL0_MIPI_EN |
+			   OV8865_MIPI_SC_CTRL0_UNKNOWN);
+	if (ret)
+		return ret;
+
+	ret = ov8865_write(sensor, OV8865_MIPI_SC_CTRL2_REG,
+			   OV8865_MIPI_SC_CTRL2_PD_MIPI_RST_SYNC);
+	if (ret)
+		return ret;
+
+	if (lanes_count >= 2) {
+		ret = ov8865_write(sensor, OV8865_MIPI_LANE_SEL01_REG,
+				   OV8865_MIPI_LANE_SEL01_LANE0(0) |
+				   OV8865_MIPI_LANE_SEL01_LANE1(1));
+		if (ret)
+			return ret;
+	}
+
+	if (lanes_count >= 4) {
+		ret = ov8865_write(sensor, OV8865_MIPI_LANE_SEL23_REG,
+				   OV8865_MIPI_LANE_SEL23_LANE2(2) |
+				   OV8865_MIPI_LANE_SEL23_LANE3(3));
+		if (ret)
+			return ret;
+	}
+
+	ret = ov8865_update_bits(sensor, OV8865_CLK_SEL1_REG,
+				 OV8865_CLK_SEL1_MIPI_EOF,
+				 OV8865_CLK_SEL1_MIPI_EOF);
+	if (ret)
+		return ret;
+
+	/*
+	 * This value might need to change depending on PCLK rate,
+	 * but it's unclear how. This value seems to generally work
+	 * while the default value was found to cause transmission errors.
+	 */
+	return ov8865_write(sensor, OV8865_MIPI_PCLK_PERIOD_REG, 0x16);
+}
+
+static int ov8865_black_level_configure(struct ov8865_sensor *sensor)
+{
+	int ret;
+
+	/* Trigger BLC on relevant events and enable filter. */
+	ret = ov8865_write(sensor, OV8865_BLC_CTRL0_REG,
+			   OV8865_BLC_CTRL0_TRIG_RANGE_EN |
+			   OV8865_BLC_CTRL0_TRIG_FORMAT_EN |
+			   OV8865_BLC_CTRL0_TRIG_GAIN_EN |
+			   OV8865_BLC_CTRL0_TRIG_EXPOSURE_EN |
+			   OV8865_BLC_CTRL0_FILTER_EN);
+	if (ret)
+		return ret;
+
+	/* Lower BLC offset trigger threshold. */
+	ret = ov8865_write(sensor, OV8865_BLC_CTRLD_REG,
+			   OV8865_BLC_CTRLD_OFFSET_TRIGGER(16));
+	if (ret)
+		return ret;
+
+	ret = ov8865_write(sensor, OV8865_BLC_CTRL1F_REG, 0);
+	if (ret)
+		return ret;
+
+	/* Increase BLC offset maximum limit. */
+	return ov8865_write(sensor, OV8865_BLC_OFFSET_LIMIT_REG,
+			    OV8865_BLC_OFFSET_LIMIT(63));
+}
+
+static int ov8865_isp_configure(struct ov8865_sensor *sensor)
+{
+	int ret;
+
+	/* Disable lens correction. */
+	ret = ov8865_write(sensor, OV8865_ISP_CTRL0_REG,
+			   OV8865_ISP_CTRL0_WHITE_BALANCE_EN |
+			   OV8865_ISP_CTRL0_DPC_BLACK_EN |
+			   OV8865_ISP_CTRL0_DPC_WHITE_EN);
+	if (ret)
+		return ret;
+
+	return ov8865_write(sensor, OV8865_ISP_CTRL1_REG,
+			    OV8865_ISP_CTRL1_BLC_EN);
+}
+
+static unsigned long ov8865_mode_pll1_rate(struct ov8865_sensor *sensor,
+					   const struct ov8865_mode *mode)
+{
+	const struct ov8865_pll1_config *config;
+	unsigned long pll1_rate;
+
+	config = sensor->pll_configs->pll1_config;
+	pll1_rate = sensor->extclk_rate * config->pll_mul / config->pll_pre_div_half;
+
+	switch (config->pll_pre_div) {
+	case 0:
+		break;
+	case 1:
+		pll1_rate *= 3;
+		pll1_rate /= 2;
+		break;
+	case 3:
+		pll1_rate *= 5;
+		pll1_rate /= 2;
+		break;
+	case 4:
+		pll1_rate /= 3;
+		break;
+	case 5:
+		pll1_rate /= 4;
+		break;
+	case 7:
+		pll1_rate /= 8;
+		break;
+	default:
+		pll1_rate /= config->pll_pre_div;
+		break;
+	}
+
+	return pll1_rate;
+}
+
+static int ov8865_mode_pll1_configure(struct ov8865_sensor *sensor,
+				      const struct ov8865_mode *mode,
+				      u32 mbus_code)
+{
+	const struct ov8865_pll1_config *config;
+	u8 value;
+	int ret;
+
+	config = sensor->pll_configs->pll1_config;
+
+	switch (mbus_code) {
+	case MEDIA_BUS_FMT_SBGGR10_1X10:
+		value = OV8865_MIPI_BIT_SEL(10);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	ret = ov8865_write(sensor, OV8865_MIPI_BIT_SEL_REG, value);
+	if (ret)
+		return ret;
+
+	ret = ov8865_write(sensor, OV8865_PLL_CTRLA_REG,
+			   OV8865_PLL_CTRLA_PRE_DIV_HALF(config->pll_pre_div_half));
+	if (ret)
+		return ret;
+
+	ret = ov8865_write(sensor, OV8865_PLL_CTRL0_REG,
+			   OV8865_PLL_CTRL0_PRE_DIV(config->pll_pre_div));
+	if (ret)
+		return ret;
+
+	ret = ov8865_write(sensor, OV8865_PLL_CTRL1_REG,
+			   OV8865_PLL_CTRL1_MUL_H(config->pll_mul));
+	if (ret)
+		return ret;
+
+	ret = ov8865_write(sensor, OV8865_PLL_CTRL2_REG,
+			   OV8865_PLL_CTRL2_MUL_L(config->pll_mul));
+	if (ret)
+		return ret;
+
+	ret = ov8865_write(sensor, OV8865_PLL_CTRL3_REG,
+			   OV8865_PLL_CTRL3_M_DIV(config->m_div));
+	if (ret)
+		return ret;
+
+	ret = ov8865_write(sensor, OV8865_PLL_CTRL4_REG,
+			   OV8865_PLL_CTRL4_MIPI_DIV(config->mipi_div));
+	if (ret)
+		return ret;
+
+	ret = ov8865_update_bits(sensor, OV8865_PCLK_SEL_REG,
+				 OV8865_PCLK_SEL_PCLK_DIV_MASK,
+				 OV8865_PCLK_SEL_PCLK_DIV(config->pclk_div));
+	if (ret)
+		return ret;
+
+	ret = ov8865_write(sensor, OV8865_PLL_CTRL5_REG,
+			   OV8865_PLL_CTRL5_SYS_PRE_DIV(config->sys_pre_div));
+	if (ret)
+		return ret;
+
+	ret = ov8865_write(sensor, OV8865_PLL_CTRL6_REG,
+			   OV8865_PLL_CTRL6_SYS_DIV(config->sys_div));
+	if (ret)
+		return ret;
+
+	return ov8865_update_bits(sensor, OV8865_PLL_CTRL1E_REG,
+				  OV8865_PLL_CTRL1E_PLL1_NO_LAT,
+				  OV8865_PLL_CTRL1E_PLL1_NO_LAT);
+}
+
+static int ov8865_mode_pll2_configure(struct ov8865_sensor *sensor,
+				      const struct ov8865_mode *mode)
+{
+	const struct ov8865_pll2_config *config;
+	int ret;
+
+	config = mode->pll2_binning ? sensor->pll_configs->pll2_config_binning :
+				      sensor->pll_configs->pll2_config_native;
+
+	ret = ov8865_write(sensor, OV8865_PLL_CTRL12_REG,
+			   OV8865_PLL_CTRL12_PRE_DIV_HALF(config->pll_pre_div_half) |
+			   OV8865_PLL_CTRL12_DAC_DIV(config->dac_div));
+	if (ret)
+		return ret;
+
+	ret = ov8865_write(sensor, OV8865_PLL_CTRLB_REG,
+			   OV8865_PLL_CTRLB_PRE_DIV(config->pll_pre_div));
+	if (ret)
+		return ret;
+
+	ret = ov8865_write(sensor, OV8865_PLL_CTRLC_REG,
+			   OV8865_PLL_CTRLC_MUL_H(config->pll_mul));
+	if (ret)
+		return ret;
+
+	ret = ov8865_write(sensor, OV8865_PLL_CTRLD_REG,
+			   OV8865_PLL_CTRLD_MUL_L(config->pll_mul));
+	if (ret)
+		return ret;
+
+	ret = ov8865_write(sensor, OV8865_PLL_CTRLF_REG,
+			   OV8865_PLL_CTRLF_SYS_PRE_DIV(config->sys_pre_div));
+	if (ret)
+		return ret;
+
+	return ov8865_write(sensor, OV8865_PLL_CTRLE_REG,
+			    OV8865_PLL_CTRLE_SYS_DIV(config->sys_div));
+}
+
+static int ov8865_mode_sclk_configure(struct ov8865_sensor *sensor,
+				      const struct ov8865_mode *mode)
+{
+	const struct ov8865_sclk_config *config = &ov8865_sclk_config_native;
+	int ret;
+
+	ret = ov8865_write(sensor, OV8865_CLK_SEL0_REG,
+			   OV8865_CLK_SEL0_PLL1_SYS_SEL(config->sys_sel));
+	if (ret)
+		return ret;
+
+	ret = ov8865_update_bits(sensor, OV8865_CLK_SEL1_REG,
+				 OV8865_CLK_SEL1_PLL_SCLK_SEL_MASK,
+				 OV8865_CLK_SEL1_PLL_SCLK_SEL(config->sclk_sel));
+	if (ret)
+		return ret;
+
+	return ov8865_write(sensor, OV8865_SCLK_CTRL_REG,
+			    OV8865_SCLK_CTRL_UNKNOWN |
+			    OV8865_SCLK_CTRL_SCLK_DIV(config->sclk_div) |
+			    OV8865_SCLK_CTRL_SCLK_PRE_DIV(config->sclk_pre_div));
+}
+
+static int ov8865_mode_binning_configure(struct ov8865_sensor *sensor,
+					 const struct ov8865_mode *mode)
+{
+	unsigned int variopixel_hsub_coef, variopixel_vsub_coef;
+	u8 value;
+	int ret;
+
+	ret = ov8865_write(sensor, OV8865_FORMAT1_REG, 0);
+	if (ret)
+		return ret;
+
+	value = OV8865_FORMAT2_HSYNC_EN;
+
+	if (mode->binning_x)
+		value |= OV8865_FORMAT2_FST_HBIN_EN;
+
+	if (mode->binning_y)
+		value |= OV8865_FORMAT2_FST_VBIN_EN;
+
+	if (mode->sync_hbin)
+		value |= OV8865_FORMAT2_SYNC_HBIN_EN;
+
+	if (mode->horz_var2)
+		value |= OV8865_FORMAT2_ISP_HORZ_VAR2_EN;
+
+	ret = ov8865_write(sensor, OV8865_FORMAT2_REG, value);
+	if (ret)
+		return ret;
+
+	ret = ov8865_update_bits(sensor, OV8865_ISP_CTRL2_REG,
+				 OV8865_ISP_CTRL2_VARIOPIXEL_EN,
+				 mode->variopixel ?
+				 OV8865_ISP_CTRL2_VARIOPIXEL_EN : 0);
+	if (ret)
+		return ret;
+
+	if (mode->variopixel) {
+		/* VarioPixel coefs needs to be > 1. */
+		variopixel_hsub_coef = mode->variopixel_hsub_coef;
+		variopixel_vsub_coef = mode->variopixel_vsub_coef;
+	} else {
+		variopixel_hsub_coef = 1;
+		variopixel_vsub_coef = 1;
+	}
+
+	ret = ov8865_write(sensor, OV8865_VAP_CTRL1_REG,
+			   OV8865_VAP_CTRL1_HSUB_COEF(variopixel_hsub_coef) |
+			   OV8865_VAP_CTRL1_VSUB_COEF(variopixel_vsub_coef));
+	if (ret)
+		return ret;
+
+	ret = ov8865_write(sensor, OV8865_INC_X_ODD_REG,
+			   OV8865_INC_X_ODD(mode->inc_x_odd));
+	if (ret)
+		return ret;
+
+	ret = ov8865_write(sensor, OV8865_INC_X_EVEN_REG,
+			   OV8865_INC_X_EVEN(mode->inc_x_even));
+	if (ret)
+		return ret;
+
+	ret = ov8865_write(sensor, OV8865_INC_Y_ODD_REG,
+			   OV8865_INC_Y_ODD(mode->inc_y_odd));
+	if (ret)
+		return ret;
+
+	return ov8865_write(sensor, OV8865_INC_Y_EVEN_REG,
+			    OV8865_INC_Y_EVEN(mode->inc_y_even));
+}
+
+static int ov8865_mode_black_level_configure(struct ov8865_sensor *sensor,
+					     const struct ov8865_mode *mode)
+{
+	int ret;
+
+	/* Note that a zero value for blc_col_shift_mask is the default 256. */
+	ret = ov8865_write(sensor, OV8865_BLC_CTRL1_REG,
+			   mode->blc_col_shift_mask |
+			   OV8865_BLC_CTRL1_OFFSET_LIMIT_EN);
+	if (ret)
+		return ret;
+
+	/* BLC top zero line */
+
+	ret = ov8865_write(sensor, OV8865_BLC_TOP_ZLINE_START_REG,
+			   OV8865_BLC_TOP_ZLINE_START(mode->blc_top_zero_line_start));
+	if (ret)
+		return ret;
+
+	ret = ov8865_write(sensor, OV8865_BLC_TOP_ZLINE_NUM_REG,
+			   OV8865_BLC_TOP_ZLINE_NUM(mode->blc_top_zero_line_num));
+	if (ret)
+		return ret;
+
+	/* BLC top black line */
+
+	ret = ov8865_write(sensor, OV8865_BLC_TOP_BLKLINE_START_REG,
+			   OV8865_BLC_TOP_BLKLINE_START(mode->blc_top_black_line_start));
+	if (ret)
+		return ret;
+
+	ret = ov8865_write(sensor, OV8865_BLC_TOP_BLKLINE_NUM_REG,
+			   OV8865_BLC_TOP_BLKLINE_NUM(mode->blc_top_black_line_num));
+	if (ret)
+		return ret;
+
+	/* BLC bottom zero line */
+
+	ret = ov8865_write(sensor, OV8865_BLC_BOT_ZLINE_START_REG,
+			   OV8865_BLC_BOT_ZLINE_START(mode->blc_bottom_zero_line_start));
+	if (ret)
+		return ret;
+
+	ret = ov8865_write(sensor, OV8865_BLC_BOT_ZLINE_NUM_REG,
+			   OV8865_BLC_BOT_ZLINE_NUM(mode->blc_bottom_zero_line_num));
+	if (ret)
+		return ret;
+
+	/* BLC bottom black line */
+
+	ret = ov8865_write(sensor, OV8865_BLC_BOT_BLKLINE_START_REG,
+			   OV8865_BLC_BOT_BLKLINE_START(mode->blc_bottom_black_line_start));
+	if (ret)
+		return ret;
+
+	ret = ov8865_write(sensor, OV8865_BLC_BOT_BLKLINE_NUM_REG,
+			   OV8865_BLC_BOT_BLKLINE_NUM(mode->blc_bottom_black_line_num));
+	if (ret)
+		return ret;
+
+	/* BLC anchor */
+
+	ret = ov8865_write(sensor, OV8865_BLC_ANCHOR_LEFT_START_H_REG,
+			   OV8865_BLC_ANCHOR_LEFT_START_H(mode->blc_anchor_left_start));
+	if (ret)
+		return ret;
+
+	ret = ov8865_write(sensor, OV8865_BLC_ANCHOR_LEFT_START_L_REG,
+			   OV8865_BLC_ANCHOR_LEFT_START_L(mode->blc_anchor_left_start));
+	if (ret)
+		return ret;
+
+	ret = ov8865_write(sensor, OV8865_BLC_ANCHOR_LEFT_END_H_REG,
+			   OV8865_BLC_ANCHOR_LEFT_END_H(mode->blc_anchor_left_end));
+	if (ret)
+		return ret;
+
+	ret = ov8865_write(sensor, OV8865_BLC_ANCHOR_LEFT_END_L_REG,
+			   OV8865_BLC_ANCHOR_LEFT_END_L(mode->blc_anchor_left_end));
+	if (ret)
+		return ret;
+
+	ret = ov8865_write(sensor, OV8865_BLC_ANCHOR_RIGHT_START_H_REG,
+			   OV8865_BLC_ANCHOR_RIGHT_START_H(mode->blc_anchor_right_start));
+	if (ret)
+		return ret;
+
+	ret = ov8865_write(sensor, OV8865_BLC_ANCHOR_RIGHT_START_L_REG,
+			   OV8865_BLC_ANCHOR_RIGHT_START_L(mode->blc_anchor_right_start));
+	if (ret)
+		return ret;
+
+	ret = ov8865_write(sensor, OV8865_BLC_ANCHOR_RIGHT_END_H_REG,
+			   OV8865_BLC_ANCHOR_RIGHT_END_H(mode->blc_anchor_right_end));
+	if (ret)
+		return ret;
+
+	return ov8865_write(sensor, OV8865_BLC_ANCHOR_RIGHT_END_L_REG,
+			    OV8865_BLC_ANCHOR_RIGHT_END_L(mode->blc_anchor_right_end));
+}
+
+static int ov8865_mode_configure(struct ov8865_sensor *sensor,
+				 const struct ov8865_mode *mode, u32 mbus_code)
+{
+	int ret;
+
+	/* Output Size X */
+
+	ret = ov8865_write(sensor, OV8865_OUTPUT_SIZE_X_H_REG,
+			   OV8865_OUTPUT_SIZE_X_H(mode->output_size_x));
+	if (ret)
+		return ret;
+
+	ret = ov8865_write(sensor, OV8865_OUTPUT_SIZE_X_L_REG,
+			   OV8865_OUTPUT_SIZE_X_L(mode->output_size_x));
+	if (ret)
+		return ret;
+
+	/* Horizontal Total Size */
+
+	ret = ov8865_write(sensor, OV8865_HTS_H_REG, OV8865_HTS_H(mode->hts));
+	if (ret)
+		return ret;
+
+	ret = ov8865_write(sensor, OV8865_HTS_L_REG, OV8865_HTS_L(mode->hts));
+	if (ret)
+		return ret;
+
+	/* Output Size Y */
+
+	ret = ov8865_write(sensor, OV8865_OUTPUT_SIZE_Y_H_REG,
+			   OV8865_OUTPUT_SIZE_Y_H(mode->output_size_y));
+	if (ret)
+		return ret;
+
+	ret = ov8865_write(sensor, OV8865_OUTPUT_SIZE_Y_L_REG,
+			   OV8865_OUTPUT_SIZE_Y_L(mode->output_size_y));
+	if (ret)
+		return ret;
+
+	/* Vertical Total Size */
+
+	ret = ov8865_write(sensor, OV8865_VTS_H_REG, OV8865_VTS_H(mode->vts));
+	if (ret)
+		return ret;
+
+	ret = ov8865_write(sensor, OV8865_VTS_L_REG, OV8865_VTS_L(mode->vts));
+	if (ret)
+		return ret;
+
+	if (mode->size_auto) {
+		/* Auto Size */
+
+		ret = ov8865_write(sensor, OV8865_AUTO_SIZE_CTRL_REG,
+				   OV8865_AUTO_SIZE_CTRL_OFFSET_Y_REG |
+				   OV8865_AUTO_SIZE_CTRL_OFFSET_X_REG |
+				   OV8865_AUTO_SIZE_CTRL_CROP_END_Y_REG |
+				   OV8865_AUTO_SIZE_CTRL_CROP_END_X_REG |
+				   OV8865_AUTO_SIZE_CTRL_CROP_START_Y_REG |
+				   OV8865_AUTO_SIZE_CTRL_CROP_START_X_REG);
+		if (ret)
+			return ret;
+
+		ret = ov8865_write(sensor, OV8865_AUTO_SIZE_BOUNDARIES_REG,
+				   OV8865_AUTO_SIZE_BOUNDARIES_Y(mode->size_auto_boundary_y) |
+				   OV8865_AUTO_SIZE_BOUNDARIES_X(mode->size_auto_boundary_x));
+		if (ret)
+			return ret;
+	} else {
+		/* Crop Start X */
+
+		ret = ov8865_write(sensor, OV8865_CROP_START_X_H_REG,
+				   OV8865_CROP_START_X_H(mode->crop_start_x));
+		if (ret)
+			return ret;
+
+		ret = ov8865_write(sensor, OV8865_CROP_START_X_L_REG,
+				   OV8865_CROP_START_X_L(mode->crop_start_x));
+		if (ret)
+			return ret;
+
+		/* Offset X */
+
+		ret = ov8865_write(sensor, OV8865_OFFSET_X_H_REG,
+				   OV8865_OFFSET_X_H(mode->offset_x));
+		if (ret)
+			return ret;
+
+		ret = ov8865_write(sensor, OV8865_OFFSET_X_L_REG,
+				   OV8865_OFFSET_X_L(mode->offset_x));
+		if (ret)
+			return ret;
+
+		/* Crop End X */
+
+		ret = ov8865_write(sensor, OV8865_CROP_END_X_H_REG,
+				   OV8865_CROP_END_X_H(mode->crop_end_x));
+		if (ret)
+			return ret;
+
+		ret = ov8865_write(sensor, OV8865_CROP_END_X_L_REG,
+				   OV8865_CROP_END_X_L(mode->crop_end_x));
+		if (ret)
+			return ret;
+
+		/* Crop Start Y */
+
+		ret = ov8865_write(sensor, OV8865_CROP_START_Y_H_REG,
+				   OV8865_CROP_START_Y_H(mode->crop_start_y));
+		if (ret)
+			return ret;
+
+		ret = ov8865_write(sensor, OV8865_CROP_START_Y_L_REG,
+				   OV8865_CROP_START_Y_L(mode->crop_start_y));
+		if (ret)
+			return ret;
+
+		/* Offset Y */
+
+		ret = ov8865_write(sensor, OV8865_OFFSET_Y_H_REG,
+				   OV8865_OFFSET_Y_H(mode->offset_y));
+		if (ret)
+			return ret;
+
+		ret = ov8865_write(sensor, OV8865_OFFSET_Y_L_REG,
+				   OV8865_OFFSET_Y_L(mode->offset_y));
+		if (ret)
+			return ret;
+
+		/* Crop End Y */
+
+		ret = ov8865_write(sensor, OV8865_CROP_END_Y_H_REG,
+				   OV8865_CROP_END_Y_H(mode->crop_end_y));
+		if (ret)
+			return ret;
+
+		ret = ov8865_write(sensor, OV8865_CROP_END_Y_L_REG,
+				   OV8865_CROP_END_Y_L(mode->crop_end_y));
+		if (ret)
+			return ret;
+	}
+
+	/* VFIFO */
+
+	ret = ov8865_write(sensor, OV8865_VFIFO_READ_START_H_REG,
+			   OV8865_VFIFO_READ_START_H(mode->vfifo_read_start));
+	if (ret)
+		return ret;
+
+	ret = ov8865_write(sensor, OV8865_VFIFO_READ_START_L_REG,
+			   OV8865_VFIFO_READ_START_L(mode->vfifo_read_start));
+	if (ret)
+		return ret;
+
+	ret = ov8865_write(sensor, OV8865_ABLC_NUM_REG,
+			   OV8865_ABLC_NUM(mode->ablc_num));
+	if (ret)
+		return ret;
+
+	ret = ov8865_write(sensor, OV8865_ZLINE_NUM_REG,
+			   OV8865_ZLINE_NUM(mode->zline_num));
+	if (ret)
+		return ret;
+
+	/* Binning */
+
+	ret = ov8865_mode_binning_configure(sensor, mode);
+	if (ret)
+		return ret;
+
+	/* Black Level */
+
+	ret = ov8865_mode_black_level_configure(sensor, mode);
+	if (ret)
+		return ret;
+
+	/* PLLs */
+
+	ret = ov8865_mode_pll1_configure(sensor, mode, mbus_code);
+	if (ret)
+		return ret;
+
+	ret = ov8865_mode_pll2_configure(sensor, mode);
+	if (ret)
+		return ret;
+
+	ret = ov8865_mode_sclk_configure(sensor, mode);
+	if (ret)
+		return ret;
+
+	/* Extra registers */
+
+	if (mode->register_values) {
+		ret = ov8865_write_sequence(sensor, mode->register_values,
+					    mode->register_values_count);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static unsigned long ov8865_mode_mipi_clk_rate(struct ov8865_sensor *sensor,
+					       const struct ov8865_mode *mode)
+{
+	const struct ov8865_pll1_config *config;
+	unsigned long pll1_rate;
+
+	config = sensor->pll_configs->pll1_config;
+
+	pll1_rate = ov8865_mode_pll1_rate(sensor, mode);
+
+	return pll1_rate / config->m_div / 2;
+}
+
+/* Exposure */
+
+static int ov8865_exposure_configure(struct ov8865_sensor *sensor, u32 exposure)
+{
+	int ret;
+
+	/* The sensor stores exposure in units of 1/16th of a line */
+	exposure *= 16;
+
+	ret = ov8865_write(sensor, OV8865_EXPOSURE_CTRL_HH_REG,
+			   OV8865_EXPOSURE_CTRL_HH(exposure));
+	if (ret)
+		return ret;
+
+	ret = ov8865_write(sensor, OV8865_EXPOSURE_CTRL_H_REG,
+			   OV8865_EXPOSURE_CTRL_H(exposure));
+	if (ret)
+		return ret;
+
+	return ov8865_write(sensor, OV8865_EXPOSURE_CTRL_L_REG,
+			    OV8865_EXPOSURE_CTRL_L(exposure));
+}
+
+/* Gain */
+
+static int ov8865_analog_gain_configure(struct ov8865_sensor *sensor, u32 gain)
+{
+	int ret;
+
+	ret = ov8865_write(sensor, OV8865_GAIN_CTRL_H_REG,
+			   OV8865_GAIN_CTRL_H(gain));
+	if (ret)
+		return ret;
+
+	return ov8865_write(sensor, OV8865_GAIN_CTRL_L_REG,
+			    OV8865_GAIN_CTRL_L(gain));
+}
+
+/* White Balance */
+
+static int ov8865_red_balance_configure(struct ov8865_sensor *sensor,
+					u32 red_balance)
+{
+	int ret;
+
+	ret = ov8865_write(sensor, OV8865_ISP_GAIN_RED_H_REG,
+			   OV8865_ISP_GAIN_RED_H(red_balance));
+	if (ret)
+		return ret;
+
+	return ov8865_write(sensor, OV8865_ISP_GAIN_RED_L_REG,
+			    OV8865_ISP_GAIN_RED_L(red_balance));
+}
+
+static int ov8865_blue_balance_configure(struct ov8865_sensor *sensor,
+					 u32 blue_balance)
+{
+	int ret;
+
+	ret = ov8865_write(sensor, OV8865_ISP_GAIN_BLUE_H_REG,
+			   OV8865_ISP_GAIN_BLUE_H(blue_balance));
+	if (ret)
+		return ret;
+
+	return ov8865_write(sensor, OV8865_ISP_GAIN_BLUE_L_REG,
+			    OV8865_ISP_GAIN_BLUE_L(blue_balance));
+}
+
+/* Flip */
+
+static int ov8865_flip_vert_configure(struct ov8865_sensor *sensor, bool enable)
+{
+	u8 bits = OV8865_FORMAT1_FLIP_VERT_ISP_EN |
+		  OV8865_FORMAT1_FLIP_VERT_SENSOR_EN;
+
+	return ov8865_update_bits(sensor, OV8865_FORMAT1_REG, bits,
+				  enable ? bits : 0);
+}
+
+static int ov8865_flip_horz_configure(struct ov8865_sensor *sensor, bool enable)
+{
+	u8 bits = OV8865_FORMAT2_FLIP_HORZ_ISP_EN |
+		  OV8865_FORMAT2_FLIP_HORZ_SENSOR_EN;
+
+	return ov8865_update_bits(sensor, OV8865_FORMAT2_REG, bits,
+				  enable ? bits : 0);
+}
+
+/* Test Pattern */
+
+static int ov8865_test_pattern_configure(struct ov8865_sensor *sensor,
+					 unsigned int index)
+{
+	if (index >= ARRAY_SIZE(ov8865_test_pattern_bits))
+		return -EINVAL;
+
+	return ov8865_write(sensor, OV8865_PRE_CTRL0_REG,
+			    ov8865_test_pattern_bits[index]);
+}
+
+/* Blanking */
+
+static int ov8865_vts_configure(struct ov8865_sensor *sensor, u32 vblank)
+{
+	u16 vts = sensor->state.mode->output_size_y + vblank;
+	int ret;
+
+	ret = ov8865_write(sensor, OV8865_VTS_H_REG, OV8865_VTS_H(vts));
+	if (ret)
+		return ret;
+
+	return ov8865_write(sensor, OV8865_VTS_L_REG, OV8865_VTS_L(vts));
+}
+
+/* State */
+
+static int ov8865_state_mipi_configure(struct ov8865_sensor *sensor,
+				       const struct ov8865_mode *mode,
+				       u32 mbus_code)
+{
+	struct ov8865_ctrls *ctrls = &sensor->ctrls;
+	struct v4l2_mbus_config_mipi_csi2 *bus_mipi_csi2 =
+		&sensor->endpoint.bus.mipi_csi2;
+	unsigned long mipi_clk_rate;
+	unsigned int bits_per_sample;
+	unsigned int lanes_count;
+	unsigned int i, j;
+	s64 mipi_pixel_rate;
+
+	mipi_clk_rate = ov8865_mode_mipi_clk_rate(sensor, mode);
+	if (!mipi_clk_rate)
+		return -EINVAL;
+
+	for (i = 0; i < ARRAY_SIZE(ov8865_link_freq_menu); i++) {
+		s64 freq = ov8865_link_freq_menu[i];
+
+		if (freq == mipi_clk_rate)
+			break;
+	}
+
+	for (j = 0; j < sensor->endpoint.nr_of_link_frequencies; j++) {
+		u64 freq = sensor->endpoint.link_frequencies[j];
+
+		if (freq == mipi_clk_rate)
+			break;
+	}
+
+	if (i == ARRAY_SIZE(ov8865_link_freq_menu)) {
+		dev_err(sensor->dev,
+			"failed to find %lu clk rate in link freq\n",
+			mipi_clk_rate);
+	} else if (j == sensor->endpoint.nr_of_link_frequencies) {
+		dev_err(sensor->dev,
+			"failed to find %lu clk rate in endpoint link-frequencies\n",
+			mipi_clk_rate);
+	} else {
+		__v4l2_ctrl_s_ctrl(ctrls->link_freq, i);
+	}
+
+	switch (mbus_code) {
+	case MEDIA_BUS_FMT_SBGGR10_1X10:
+		bits_per_sample = 10;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	lanes_count = bus_mipi_csi2->num_data_lanes;
+	mipi_pixel_rate = mipi_clk_rate * 2 * lanes_count / bits_per_sample;
+
+	__v4l2_ctrl_s_ctrl_int64(ctrls->pixel_rate, mipi_pixel_rate);
+
+	return 0;
+}
+
+static int ov8865_state_configure(struct ov8865_sensor *sensor,
+				  const struct ov8865_mode *mode,
+				  u32 mbus_code)
+{
+	int ret;
+
+	if (sensor->state.streaming)
+		return -EBUSY;
+
+	/* State will be configured at first power on otherwise. */
+	if (pm_runtime_enabled(sensor->dev) &&
+	    !pm_runtime_suspended(sensor->dev)) {
+		ret = ov8865_mode_configure(sensor, mode, mbus_code);
+		if (ret)
+			return ret;
+	}
+
+	ret = ov8865_state_mipi_configure(sensor, mode, mbus_code);
+	if (ret)
+		return ret;
+
+	sensor->state.mode = mode;
+	sensor->state.mbus_code = mbus_code;
+
+	return 0;
+}
+
+static int ov8865_state_init(struct ov8865_sensor *sensor)
+{
+	return ov8865_state_configure(sensor, &ov8865_modes[0],
+				      ov8865_mbus_codes[0]);
+}
+
+/* Sensor Base */
+
+static int ov8865_sensor_init(struct ov8865_sensor *sensor)
+{
+	int ret;
+
+	ret = ov8865_sw_reset(sensor);
+	if (ret) {
+		dev_err(sensor->dev, "failed to perform sw reset\n");
+		return ret;
+	}
+
+	ret = ov8865_sw_standby(sensor, 1);
+	if (ret) {
+		dev_err(sensor->dev, "failed to set sensor standby\n");
+		return ret;
+	}
+
+	ret = ov8865_chip_id_check(sensor);
+	if (ret) {
+		dev_err(sensor->dev, "failed to check sensor chip id\n");
+		return ret;
+	}
+
+	ret = ov8865_write_sequence(sensor, ov8865_init_sequence,
+				    ARRAY_SIZE(ov8865_init_sequence));
+	if (ret) {
+		dev_err(sensor->dev, "failed to write init sequence\n");
+		return ret;
+	}
+
+	ret = ov8865_charge_pump_configure(sensor);
+	if (ret) {
+		dev_err(sensor->dev, "failed to configure pad\n");
+		return ret;
+	}
+
+	ret = ov8865_mipi_configure(sensor);
+	if (ret) {
+		dev_err(sensor->dev, "failed to configure MIPI\n");
+		return ret;
+	}
+
+	ret = ov8865_isp_configure(sensor);
+	if (ret) {
+		dev_err(sensor->dev, "failed to configure ISP\n");
+		return ret;
+	}
+
+	ret = ov8865_black_level_configure(sensor);
+	if (ret) {
+		dev_err(sensor->dev, "failed to configure black level\n");
+		return ret;
+	}
+
+	/* Configure current mode. */
+	ret = ov8865_state_configure(sensor, sensor->state.mode,
+				     sensor->state.mbus_code);
+	if (ret) {
+		dev_err(sensor->dev, "failed to configure state\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ov8865_sensor_power(struct ov8865_sensor *sensor, bool on)
+{
+	/* Keep initialized to zero for disable label. */
+	int ret = 0;
+
+	if (on) {
+		gpiod_set_value_cansleep(sensor->reset, 1);
+		gpiod_set_value_cansleep(sensor->powerdown, 1);
+
+		ret = regulator_enable(sensor->dovdd);
+		if (ret) {
+			dev_err(sensor->dev,
+				"failed to enable DOVDD regulator\n");
+			return ret;
+		}
+
+		ret = regulator_enable(sensor->avdd);
+		if (ret) {
+			dev_err(sensor->dev,
+				"failed to enable AVDD regulator\n");
+			goto disable_dovdd;
+		}
+
+		ret = regulator_enable(sensor->dvdd);
+		if (ret) {
+			dev_err(sensor->dev,
+				"failed to enable DVDD regulator\n");
+			goto disable_avdd;
+		}
+
+		ret = clk_prepare_enable(sensor->extclk);
+		if (ret) {
+			dev_err(sensor->dev, "failed to enable EXTCLK clock\n");
+			goto disable_dvdd;
+		}
+
+		gpiod_set_value_cansleep(sensor->reset, 0);
+		gpiod_set_value_cansleep(sensor->powerdown, 0);
+
+		/* Time to enter streaming mode according to power timings. */
+		usleep_range(10000, 12000);
+	} else {
+		gpiod_set_value_cansleep(sensor->powerdown, 1);
+		gpiod_set_value_cansleep(sensor->reset, 1);
+
+		clk_disable_unprepare(sensor->extclk);
+
+disable_dvdd:
+		regulator_disable(sensor->dvdd);
+disable_avdd:
+		regulator_disable(sensor->avdd);
+disable_dovdd:
+		regulator_disable(sensor->dovdd);
+	}
+
+	return ret;
+}
+
+/* Controls */
+
+static int ov8865_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *subdev = ov8865_ctrl_subdev(ctrl);
+	struct ov8865_sensor *sensor = ov8865_subdev_sensor(subdev);
+	unsigned int index;
+	int ret;
+
+	/* If VBLANK is altered we need to update exposure to compensate */
+	if (ctrl->id == V4L2_CID_VBLANK) {
+		int exposure_max;
+
+		exposure_max = sensor->state.mode->output_size_y + ctrl->val -
+			       OV8865_INTEGRATION_TIME_MARGIN;
+		__v4l2_ctrl_modify_range(sensor->ctrls.exposure,
+					 sensor->ctrls.exposure->minimum,
+					 exposure_max,
+					 sensor->ctrls.exposure->step,
+					 min(sensor->ctrls.exposure->val,
+					     exposure_max));
+	}
+
+	/* Wait for the sensor to be on before setting controls. */
+	if (pm_runtime_suspended(sensor->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE:
+		ret = ov8865_exposure_configure(sensor, ctrl->val);
+		if (ret)
+			return ret;
+		break;
+	case V4L2_CID_ANALOGUE_GAIN:
+		ret = ov8865_analog_gain_configure(sensor, ctrl->val);
+		if (ret)
+			return ret;
+		break;
+	case V4L2_CID_RED_BALANCE:
+		return ov8865_red_balance_configure(sensor, ctrl->val);
+	case V4L2_CID_BLUE_BALANCE:
+		return ov8865_blue_balance_configure(sensor, ctrl->val);
+	case V4L2_CID_HFLIP:
+		return ov8865_flip_horz_configure(sensor, !!ctrl->val);
+	case V4L2_CID_VFLIP:
+		return ov8865_flip_vert_configure(sensor, !!ctrl->val);
+	case V4L2_CID_TEST_PATTERN:
+		index = (unsigned int)ctrl->val;
+		return ov8865_test_pattern_configure(sensor, index);
+	case V4L2_CID_VBLANK:
+		return ov8865_vts_configure(sensor, ctrl->val);
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops ov8865_ctrl_ops = {
+	.s_ctrl			= ov8865_s_ctrl,
+};
+
+static int ov8865_ctrls_init(struct ov8865_sensor *sensor)
+{
+	struct ov8865_ctrls *ctrls = &sensor->ctrls;
+	struct v4l2_ctrl_handler *handler = &ctrls->handler;
+	const struct v4l2_ctrl_ops *ops = &ov8865_ctrl_ops;
+	const struct ov8865_mode *mode = &ov8865_modes[0];
+	struct v4l2_fwnode_device_properties props;
+	unsigned int vblank_max, vblank_def;
+	unsigned int hblank;
+	int ret;
+
+	v4l2_ctrl_handler_init(handler, 32);
+
+	/* Use our mutex for ctrl locking. */
+	handler->lock = &sensor->mutex;
+
+	/* Exposure */
+
+	ctrls->exposure = v4l2_ctrl_new_std(handler, ops, V4L2_CID_EXPOSURE, 2,
+					    65535, 1, 32);
+
+	/* Gain */
+
+	v4l2_ctrl_new_std(handler, ops, V4L2_CID_ANALOGUE_GAIN, 128, 2048, 128,
+			  128);
+
+	/* White Balance */
+
+	v4l2_ctrl_new_std(handler, ops, V4L2_CID_RED_BALANCE, 1, 32767, 1,
+			  1024);
+
+	v4l2_ctrl_new_std(handler, ops, V4L2_CID_BLUE_BALANCE, 1, 32767, 1,
+			  1024);
+
+	/* Flip */
+
+	v4l2_ctrl_new_std(handler, ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(handler, ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
+
+	/* Test Pattern */
+
+	v4l2_ctrl_new_std_menu_items(handler, ops, V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(ov8865_test_pattern_menu) - 1,
+				     0, 0, ov8865_test_pattern_menu);
+
+	/* Blanking */
+	hblank = mode->hts - mode->output_size_x;
+	ctrls->hblank = v4l2_ctrl_new_std(handler, ops, V4L2_CID_HBLANK, hblank,
+					  hblank, 1, hblank);
+
+	if (ctrls->hblank)
+		ctrls->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	vblank_max = OV8865_TIMING_MAX_VTS - mode->output_size_y;
+	vblank_def = mode->vts - mode->output_size_y;
+	ctrls->vblank = v4l2_ctrl_new_std(handler, ops, V4L2_CID_VBLANK,
+					  OV8865_TIMING_MIN_VTS, vblank_max, 1,
+					  vblank_def);
+
+	/* MIPI CSI-2 */
+
+	ctrls->link_freq =
+		v4l2_ctrl_new_int_menu(handler, NULL, V4L2_CID_LINK_FREQ,
+				       ARRAY_SIZE(ov8865_link_freq_menu) - 1,
+				       0, ov8865_link_freq_menu);
+
+	ctrls->pixel_rate =
+		v4l2_ctrl_new_std(handler, NULL, V4L2_CID_PIXEL_RATE, 1,
+				  INT_MAX, 1, 1);
+
+	/* set properties from fwnode (e.g. rotation, orientation) */
+	ret = v4l2_fwnode_device_parse(sensor->dev, &props);
+	if (ret)
+		goto error_ctrls;
+
+	ret = v4l2_ctrl_new_fwnode_properties(handler, ops, &props);
+	if (ret)
+		goto error_ctrls;
+
+	if (handler->error) {
+		ret = handler->error;
+		goto error_ctrls;
+	}
+
+	ctrls->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+	ctrls->pixel_rate->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	sensor->subdev.ctrl_handler = handler;
+
+	return 0;
+
+error_ctrls:
+	v4l2_ctrl_handler_free(handler);
+
+	return ret;
+}
+
+/* Subdev Video Operations */
+
+static int ov8865_s_stream(struct v4l2_subdev *subdev, int enable)
+{
+	struct ov8865_sensor *sensor = ov8865_subdev_sensor(subdev);
+	struct ov8865_state *state = &sensor->state;
+	int ret;
+
+	if (enable) {
+		ret = pm_runtime_resume_and_get(sensor->dev);
+		if (ret < 0)
+			return ret;
+	}
+
+	mutex_lock(&sensor->mutex);
+	ret = ov8865_sw_standby(sensor, !enable);
+	mutex_unlock(&sensor->mutex);
+
+	if (ret)
+		return ret;
+
+	state->streaming = !!enable;
+
+	if (!enable)
+		pm_runtime_put(sensor->dev);
+
+	return 0;
+}
+
+static int ov8865_g_frame_interval(struct v4l2_subdev *subdev,
+				   struct v4l2_subdev_frame_interval *interval)
+{
+	struct ov8865_sensor *sensor = ov8865_subdev_sensor(subdev);
+	const struct ov8865_mode *mode;
+	unsigned int framesize;
+	unsigned int fps;
+
+	mutex_lock(&sensor->mutex);
+
+	mode = sensor->state.mode;
+	framesize = mode->hts * (mode->output_size_y +
+				 sensor->ctrls.vblank->val);
+	fps = DIV_ROUND_CLOSEST(sensor->ctrls.pixel_rate->val, framesize);
+
+	interval->interval.numerator = 1;
+	interval->interval.denominator = fps;
+
+	mutex_unlock(&sensor->mutex);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops ov8865_subdev_video_ops = {
+	.s_stream		= ov8865_s_stream,
+	.g_frame_interval	= ov8865_g_frame_interval,
+	.s_frame_interval	= ov8865_g_frame_interval,
+};
+
+/* Subdev Pad Operations */
+
+static int ov8865_enum_mbus_code(struct v4l2_subdev *subdev,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code_enum)
+{
+	if (code_enum->index >= ARRAY_SIZE(ov8865_mbus_codes))
+		return -EINVAL;
+
+	code_enum->code = ov8865_mbus_codes[code_enum->index];
+
+	return 0;
+}
+
+static void ov8865_mbus_format_fill(struct v4l2_mbus_framefmt *mbus_format,
+				    u32 mbus_code,
+				    const struct ov8865_mode *mode)
+{
+	mbus_format->width = mode->output_size_x;
+	mbus_format->height = mode->output_size_y;
+	mbus_format->code = mbus_code;
+
+	mbus_format->field = V4L2_FIELD_NONE;
+	mbus_format->colorspace = V4L2_COLORSPACE_RAW;
+	mbus_format->ycbcr_enc =
+		V4L2_MAP_YCBCR_ENC_DEFAULT(mbus_format->colorspace);
+	mbus_format->quantization = V4L2_QUANTIZATION_FULL_RANGE;
+	mbus_format->xfer_func =
+		V4L2_MAP_XFER_FUNC_DEFAULT(mbus_format->colorspace);
+}
+
+static int ov8865_get_fmt(struct v4l2_subdev *subdev,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *format)
+{
+	struct ov8865_sensor *sensor = ov8865_subdev_sensor(subdev);
+	struct v4l2_mbus_framefmt *mbus_format = &format->format;
+
+	mutex_lock(&sensor->mutex);
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+		*mbus_format = *v4l2_subdev_get_try_format(subdev, sd_state,
+							   format->pad);
+	else
+		ov8865_mbus_format_fill(mbus_format, sensor->state.mbus_code,
+					sensor->state.mode);
+
+	mutex_unlock(&sensor->mutex);
+
+	return 0;
+}
+
+static int ov8865_set_fmt(struct v4l2_subdev *subdev,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *format)
+{
+	struct ov8865_sensor *sensor = ov8865_subdev_sensor(subdev);
+	struct v4l2_mbus_framefmt *mbus_format = &format->format;
+	const struct ov8865_mode *mode;
+	u32 mbus_code = 0;
+	unsigned int hblank;
+	unsigned int index;
+	int exposure_max;
+	int ret = 0;
+
+	mutex_lock(&sensor->mutex);
+
+	if (sensor->state.streaming) {
+		ret = -EBUSY;
+		goto complete;
+	}
+
+	/* Try to find requested mbus code. */
+	for (index = 0; index < ARRAY_SIZE(ov8865_mbus_codes); index++) {
+		if (ov8865_mbus_codes[index] == mbus_format->code) {
+			mbus_code = mbus_format->code;
+			break;
+		}
+	}
+
+	/* Fallback to default. */
+	if (!mbus_code)
+		mbus_code = ov8865_mbus_codes[0];
+
+	/* Find the mode with nearest dimensions. */
+	mode = v4l2_find_nearest_size(ov8865_modes, ARRAY_SIZE(ov8865_modes),
+				      output_size_x, output_size_y,
+				      mbus_format->width, mbus_format->height);
+	if (!mode) {
+		ret = -EINVAL;
+		goto complete;
+	}
+
+	ov8865_mbus_format_fill(mbus_format, mbus_code, mode);
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+		*v4l2_subdev_get_try_format(subdev, sd_state, format->pad) =
+			*mbus_format;
+	else if (sensor->state.mode != mode ||
+		 sensor->state.mbus_code != mbus_code)
+		ret = ov8865_state_configure(sensor, mode, mbus_code);
+
+	__v4l2_ctrl_modify_range(sensor->ctrls.vblank, OV8865_TIMING_MIN_VTS,
+				 OV8865_TIMING_MAX_VTS - mode->output_size_y,
+				 1, mode->vts - mode->output_size_y);
+
+	hblank = mode->hts - mode->output_size_x;
+	__v4l2_ctrl_modify_range(sensor->ctrls.hblank, hblank, hblank, 1,
+				 hblank);
+
+	exposure_max = mode->vts - OV8865_INTEGRATION_TIME_MARGIN;
+	__v4l2_ctrl_modify_range(sensor->ctrls.exposure,
+				 sensor->ctrls.exposure->minimum, exposure_max,
+				 sensor->ctrls.exposure->step,
+				 min(sensor->ctrls.exposure->val,
+				     exposure_max));
+
+complete:
+	mutex_unlock(&sensor->mutex);
+
+	return ret;
+}
+
+static int ov8865_enum_frame_size(struct v4l2_subdev *subdev,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *size_enum)
+{
+	const struct ov8865_mode *mode;
+
+	if (size_enum->index >= ARRAY_SIZE(ov8865_modes))
+		return -EINVAL;
+
+	mode = &ov8865_modes[size_enum->index];
+
+	size_enum->min_width = size_enum->max_width = mode->output_size_x;
+	size_enum->min_height = size_enum->max_height = mode->output_size_y;
+
+	return 0;
+}
+
+static void
+__ov8865_get_pad_crop(struct ov8865_sensor *sensor,
+		      struct v4l2_subdev_state *state, unsigned int pad,
+		      enum v4l2_subdev_format_whence which, struct v4l2_rect *r)
+{
+	const struct ov8865_mode *mode = sensor->state.mode;
+
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		*r = *v4l2_subdev_get_try_crop(&sensor->subdev, state, pad);
+		break;
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		r->height = mode->output_size_y;
+		r->width = mode->output_size_x;
+		r->top = (OV8865_NATIVE_HEIGHT - mode->output_size_y) / 2;
+		r->left = (OV8865_NATIVE_WIDTH - mode->output_size_x) / 2;
+		break;
+	}
+}
+
+static int ov8865_get_selection(struct v4l2_subdev *subdev,
+				struct v4l2_subdev_state *state,
+				struct v4l2_subdev_selection *sel)
+{
+	struct ov8865_sensor *sensor = ov8865_subdev_sensor(subdev);
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		mutex_lock(&sensor->mutex);
+		__ov8865_get_pad_crop(sensor, state, sel->pad,
+				      sel->which, &sel->r);
+		mutex_unlock(&sensor->mutex);
+		break;
+	case V4L2_SEL_TGT_NATIVE_SIZE:
+		sel->r.top = 0;
+		sel->r.left = 0;
+		sel->r.width = OV8865_NATIVE_WIDTH;
+		sel->r.height = OV8865_NATIVE_HEIGHT;
+		break;
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		sel->r.top = OV8865_ACTIVE_START_TOP;
+		sel->r.left = OV8865_ACTIVE_START_LEFT;
+		sel->r.width = OV8865_ACTIVE_WIDTH;
+		sel->r.height = OV8865_ACTIVE_HEIGHT;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_subdev_pad_ops ov8865_subdev_pad_ops = {
+	.enum_mbus_code		= ov8865_enum_mbus_code,
+	.get_fmt		= ov8865_get_fmt,
+	.set_fmt		= ov8865_set_fmt,
+	.enum_frame_size	= ov8865_enum_frame_size,
+	.get_selection		= ov8865_get_selection,
+	.set_selection		= ov8865_get_selection,
+};
+
+static const struct v4l2_subdev_ops ov8865_subdev_ops = {
+	.video		= &ov8865_subdev_video_ops,
+	.pad		= &ov8865_subdev_pad_ops,
+};
+
+static int ov8865_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct ov8865_sensor *sensor = ov8865_subdev_sensor(subdev);
+	struct ov8865_state *state = &sensor->state;
+	int ret = 0;
+
+	mutex_lock(&sensor->mutex);
+
+	if (state->streaming) {
+		ret = ov8865_sw_standby(sensor, true);
+		if (ret)
+			goto complete;
+	}
+
+	ret = ov8865_sensor_power(sensor, false);
+	if (ret)
+		ov8865_sw_standby(sensor, false);
+
+complete:
+	mutex_unlock(&sensor->mutex);
+
+	return ret;
+}
+
+static int ov8865_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct ov8865_sensor *sensor = ov8865_subdev_sensor(subdev);
+	struct ov8865_state *state = &sensor->state;
+	int ret = 0;
+
+	mutex_lock(&sensor->mutex);
+
+	ret = ov8865_sensor_power(sensor, true);
+	if (ret)
+		goto complete;
+
+	ret = ov8865_sensor_init(sensor);
+	if (ret)
+		goto error_power;
+
+	ret = __v4l2_ctrl_handler_setup(&sensor->ctrls.handler);
+	if (ret)
+		goto error_power;
+
+	if (state->streaming) {
+		ret = ov8865_sw_standby(sensor, false);
+		if (ret)
+			goto error_power;
+	}
+
+	goto complete;
+
+error_power:
+	ov8865_sensor_power(sensor, false);
+
+complete:
+	mutex_unlock(&sensor->mutex);
+
+	return ret;
+}
+
+static int ov8865_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct fwnode_handle *handle;
+	struct ov8865_sensor *sensor;
+	struct v4l2_subdev *subdev;
+	struct media_pad *pad;
+	unsigned int rate = 0;
+	unsigned int i;
+	int ret;
+
+	sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
+	if (!sensor)
+		return -ENOMEM;
+
+	sensor->dev = dev;
+	sensor->i2c_client = client;
+
+	/* Regulators */
+
+	/* DVDD: digital core */
+	sensor->dvdd = devm_regulator_get(dev, "dvdd");
+	if (IS_ERR(sensor->dvdd))
+		return dev_err_probe(dev, PTR_ERR(sensor->dvdd),
+				     "cannot get DVDD regulator\n");
+
+	/* DOVDD: digital I/O */
+	sensor->dovdd = devm_regulator_get(dev, "dovdd");
+	if (IS_ERR(sensor->dovdd))
+		return dev_err_probe(dev, PTR_ERR(sensor->dovdd),
+				     "cannot get DOVDD regulator\n");
+
+	/* AVDD: analog */
+	sensor->avdd = devm_regulator_get(dev, "avdd");
+	if (IS_ERR(sensor->avdd))
+		return dev_err_probe(dev, PTR_ERR(sensor->avdd),
+				     "cannot get AVDD (analog) regulator\n");
+
+	/* Graph Endpoint */
+
+	handle = fwnode_graph_get_next_endpoint(dev_fwnode(dev), NULL);
+	if (!handle)
+		return -EPROBE_DEFER;
+
+	sensor->endpoint.bus_type = V4L2_MBUS_CSI2_DPHY;
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(handle, &sensor->endpoint);
+	fwnode_handle_put(handle);
+	if (ret) {
+		dev_err(dev, "failed to parse endpoint node\n");
+		return ret;
+	}
+
+	/* GPIOs */
+
+	sensor->powerdown = devm_gpiod_get_optional(dev, "powerdown",
+						    GPIOD_OUT_HIGH);
+	if (IS_ERR(sensor->powerdown)) {
+		ret = PTR_ERR(sensor->powerdown);
+		goto error_endpoint;
+	}
+
+	sensor->reset = devm_gpiod_get_optional(dev, "reset", GPIOD_OUT_HIGH);
+	if (IS_ERR(sensor->reset)) {
+		ret = PTR_ERR(sensor->reset);
+		goto error_endpoint;
+	}
+
+	/* External Clock */
+
+	sensor->extclk = devm_clk_get(dev, NULL);
+	if (PTR_ERR(sensor->extclk) == -ENOENT) {
+		dev_info(dev, "no external clock found, continuing...\n");
+		sensor->extclk = NULL;
+	} else if (IS_ERR(sensor->extclk)) {
+		dev_err(dev, "failed to get external clock\n");
+		ret = PTR_ERR(sensor->extclk);
+		goto error_endpoint;
+	}
+
+	/*
+	 * We could have either a 24MHz or 19.2MHz clock rate from either dt or
+	 * ACPI...but we also need to support the weird IPU3 case which will
+	 * have an external clock AND a clock-frequency property. Check for the
+	 * clock-frequency property and if found, set that rate if we managed
+	 * to acquire a clock. This should cover the ACPI case. If the system
+	 * uses devicetree then the configured rate should already be set, so
+	 * we can just read it.
+	 */
+	ret = fwnode_property_read_u32(dev_fwnode(dev), "clock-frequency",
+				       &rate);
+	if (!ret && sensor->extclk) {
+		ret = clk_set_rate(sensor->extclk, rate);
+		if (ret) {
+			dev_err_probe(dev, ret, "failed to set clock rate\n");
+			goto error_endpoint;
+		}
+	} else if (ret && !sensor->extclk) {
+		dev_err_probe(dev, ret, "invalid clock config\n");
+		goto error_endpoint;
+	}
+
+	sensor->extclk_rate = rate ? rate : clk_get_rate(sensor->extclk);
+
+	for (i = 0; i < ARRAY_SIZE(supported_extclk_rates); i++) {
+		if (sensor->extclk_rate == supported_extclk_rates[i])
+			break;
+	}
+
+	if (i == ARRAY_SIZE(supported_extclk_rates)) {
+		dev_err(dev, "clock rate %lu Hz is unsupported\n",
+			sensor->extclk_rate);
+		ret = -EINVAL;
+		goto error_endpoint;
+	}
+
+	sensor->pll_configs = ov8865_pll_configs[i];
+
+	/* Subdev, entity and pad */
+
+	subdev = &sensor->subdev;
+	v4l2_i2c_subdev_init(subdev, client, &ov8865_subdev_ops);
+
+	subdev->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	subdev->entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+	pad = &sensor->pad;
+	pad->flags = MEDIA_PAD_FL_SOURCE;
+
+	ret = media_entity_pads_init(&subdev->entity, 1, pad);
+	if (ret)
+		goto error_entity;
+
+	/* Mutex */
+
+	mutex_init(&sensor->mutex);
+
+	/* Sensor */
+
+	ret = ov8865_ctrls_init(sensor);
+	if (ret)
+		goto error_mutex;
+
+	mutex_lock(&sensor->mutex);
+	ret = ov8865_state_init(sensor);
+	mutex_unlock(&sensor->mutex);
+	if (ret)
+		goto error_ctrls;
+
+	/* Runtime PM */
+
+	pm_runtime_set_suspended(sensor->dev);
+	pm_runtime_enable(sensor->dev);
+
+	/* V4L2 subdev register */
+
+	ret = v4l2_async_register_subdev_sensor(subdev);
+	if (ret)
+		goto error_pm;
+
+	return 0;
+
+error_pm:
+	pm_runtime_disable(sensor->dev);
+
+error_ctrls:
+	v4l2_ctrl_handler_free(&sensor->ctrls.handler);
+
+error_mutex:
+	mutex_destroy(&sensor->mutex);
+
+error_entity:
+	media_entity_cleanup(&sensor->subdev.entity);
+
+error_endpoint:
+	v4l2_fwnode_endpoint_free(&sensor->endpoint);
+
+	return ret;
+}
+
+static void ov8865_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct ov8865_sensor *sensor = ov8865_subdev_sensor(subdev);
+
+	v4l2_async_unregister_subdev(subdev);
+	pm_runtime_disable(sensor->dev);
+	v4l2_ctrl_handler_free(&sensor->ctrls.handler);
+	mutex_destroy(&sensor->mutex);
+	media_entity_cleanup(&subdev->entity);
+
+	v4l2_fwnode_endpoint_free(&sensor->endpoint);
+}
+
+static const struct dev_pm_ops ov8865_pm_ops = {
+	SET_RUNTIME_PM_OPS(ov8865_suspend, ov8865_resume, NULL)
+};
+
+static const struct acpi_device_id ov8865_acpi_match[] = {
+	{"INT347A"},
+	{ }
+};
+MODULE_DEVICE_TABLE(acpi, ov8865_acpi_match);
+
+static const struct of_device_id ov8865_of_match[] = {
+	{ .compatible = "ovti,ov8865" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, ov8865_of_match);
+
+static struct i2c_driver ov8865_driver = {
+	.driver = {
+		.name = "ov8865",
+		.of_match_table = ov8865_of_match,
+		.acpi_match_table = ov8865_acpi_match,
+		.pm = &ov8865_pm_ops,
+	},
+	.probe = ov8865_probe,
+	.remove = ov8865_remove,
+};
+
+module_i2c_driver(ov8865_driver);
+
+MODULE_AUTHOR("Paul Kocialkowski <paul.kocialkowski@bootlin.com>");
+MODULE_DESCRIPTION("V4L2 driver for the OmniVision OV8865 image sensor");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/ov9282.c b/drivers/media/i2c/ov9282.c
new file mode 100644
index 000000000..068c7449f
--- /dev/null
+++ b/drivers/media/i2c/ov9282.c
@@ -0,0 +1,1527 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * OmniVision ov9282 Camera Sensor Driver
+ *
+ * Copyright (C) 2021 Intel Corporation
+ */
+#include <asm/unaligned.h>
+
+#include <linux/clk.h>
+#include <linux/delay.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-event.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+/* Streaming Mode */
+#define OV9282_REG_MODE_SELECT	0x0100
+#define OV9282_MODE_STANDBY	0x00
+#define OV9282_MODE_STREAMING	0x01
+
+#define OV9282_REG_PLL_CTRL_0D	0x030d
+#define OV9282_PLL_CTRL_0D_RAW8		0x60
+#define OV9282_PLL_CTRL_0D_RAW10	0x50
+
+#define OV9282_REG_TIMING_HTS	0x380c
+#define OV9282_TIMING_HTS_MAX	0x7fff
+
+/* Lines per frame */
+#define OV9282_REG_LPFR		0x380e
+
+/* Chip ID */
+#define OV9282_REG_ID		0x300a
+#define OV9282_ID		0x9281
+
+/* Exposure control */
+#define OV9282_REG_EXPOSURE	0x3500
+#define OV9282_EXPOSURE_MIN	1
+#define OV9282_EXPOSURE_OFFSET	12
+#define OV9282_EXPOSURE_STEP	1
+#define OV9282_EXPOSURE_DEFAULT	0x0282
+
+/* Analog gain control */
+#define OV9282_REG_AGAIN	0x3509
+#define OV9282_AGAIN_MIN	0x10
+#define OV9282_AGAIN_MAX	0xff
+#define OV9282_AGAIN_STEP	1
+#define OV9282_AGAIN_DEFAULT	0x10
+
+/* Group hold register */
+#define OV9282_REG_HOLD		0x3308
+
+#define OV9282_REG_ANA_CORE_2	0x3662
+#define OV9282_ANA_CORE2_RAW8	0x07
+#define OV9282_ANA_CORE2_RAW10	0x05
+
+#define OV9282_REG_TIMING_FORMAT_1	0x3820
+#define OV9282_REG_TIMING_FORMAT_2	0x3821
+#define OV9282_FLIP_BIT			BIT(2)
+
+#define OV9282_REG_MIPI_CTRL00	0x4800
+#define OV9282_GATED_CLOCK	BIT(5)
+
+/* Input clock rate */
+#define OV9282_INCLK_RATE	24000000
+
+/* CSI2 HW configuration */
+#define OV9282_LINK_FREQ	400000000
+#define OV9282_NUM_DATA_LANES	2
+
+/* Pixel rate */
+#define OV9282_PIXEL_RATE_10BIT		(OV9282_LINK_FREQ * 2 * \
+					 OV9282_NUM_DATA_LANES / 10)
+#define OV9282_PIXEL_RATE_8BIT		(OV9282_LINK_FREQ * 2 * \
+					 OV9282_NUM_DATA_LANES / 8)
+
+/*
+ * OV9282 native and active pixel array size.
+ * 8 dummy rows/columns on each edge of a 1280x800 active array
+ */
+#define OV9282_NATIVE_WIDTH		1296U
+#define OV9282_NATIVE_HEIGHT		816U
+#define OV9282_PIXEL_ARRAY_LEFT		8U
+#define OV9282_PIXEL_ARRAY_TOP		8U
+#define OV9282_PIXEL_ARRAY_WIDTH	1280U
+#define OV9282_PIXEL_ARRAY_HEIGHT	800U
+
+#define OV9282_REG_MIN		0x00
+#define OV9282_REG_MAX		0xfffff
+
+static const char * const ov9282_supply_names[] = {
+	"avdd",		/* Analog power */
+	"dovdd",	/* Digital I/O power */
+	"dvdd",		/* Digital core power */
+};
+
+#define OV9282_NUM_SUPPLIES ARRAY_SIZE(ov9282_supply_names)
+
+/**
+ * struct ov9282_reg - ov9282 sensor register
+ * @address: Register address
+ * @val: Register value
+ */
+struct ov9282_reg {
+	u16 address;
+	u8 val;
+};
+
+/**
+ * struct ov9282_reg_list - ov9282 sensor register list
+ * @num_of_regs: Number of registers in the list
+ * @regs: Pointer to register list
+ */
+struct ov9282_reg_list {
+	u32 num_of_regs;
+	const struct ov9282_reg *regs;
+};
+
+/**
+ * struct ov9282_mode - ov9282 sensor mode structure
+ * @width: Frame width
+ * @height: Frame height
+ * @hblank_min: Minimum horizontal blanking in lines for non-continuous[0] and
+ *		continuous[1] clock modes
+ * @vblank: Vertical blanking in lines
+ * @vblank_min: Minimum vertical blanking in lines
+ * @vblank_max: Maximum vertical blanking in lines
+ * @link_freq_idx: Link frequency index
+ * @crop: on-sensor cropping for this mode
+ * @reg_list: Register list for sensor mode
+ */
+struct ov9282_mode {
+	u32 width;
+	u32 height;
+	u32 hblank_min[2];
+	u32 vblank;
+	u32 vblank_min;
+	u32 vblank_max;
+	u32 link_freq_idx;
+	struct v4l2_rect crop;
+	struct ov9282_reg_list reg_list;
+};
+
+/**
+ * struct ov9282 - ov9282 sensor device structure
+ * @dev: Pointer to generic device
+ * @sd: V4L2 sub-device
+ * @pad: Media pad. Only one pad supported
+ * @reset_gpio: Sensor reset gpio
+ * @inclk: Sensor input clock
+ * @supplies: Regulator supplies for the sensor
+ * @ctrl_handler: V4L2 control handler
+ * @link_freq_ctrl: Pointer to link frequency control
+ * @hblank_ctrl: Pointer to horizontal blanking control
+ * @vblank_ctrl: Pointer to vertical blanking control
+ * @exp_ctrl: Pointer to exposure control
+ * @again_ctrl: Pointer to analog gain control
+ * @pixel_rate: Pointer to pixel rate control
+ * @vblank: Vertical blanking in lines
+ * @noncontinuous_clock: Selection of CSI2 noncontinuous clock mode
+ * @cur_mode: Pointer to current selected sensor mode
+ * @code: Mbus code currently selected
+ * @mutex: Mutex for serializing sensor controls
+ * @streaming: Flag indicating streaming state
+ */
+struct ov9282 {
+	struct device *dev;
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct gpio_desc *reset_gpio;
+	struct clk *inclk;
+	struct regulator_bulk_data supplies[OV9282_NUM_SUPPLIES];
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_ctrl *link_freq_ctrl;
+	struct v4l2_ctrl *hblank_ctrl;
+	struct v4l2_ctrl *vblank_ctrl;
+	struct {
+		struct v4l2_ctrl *exp_ctrl;
+		struct v4l2_ctrl *again_ctrl;
+	};
+	struct v4l2_ctrl *pixel_rate;
+	u32 vblank;
+	bool noncontinuous_clock;
+	const struct ov9282_mode *cur_mode;
+	u32 code;
+	struct mutex mutex;
+	bool streaming;
+};
+
+static const s64 link_freq[] = {
+	OV9282_LINK_FREQ,
+};
+
+/*
+ * Common registers
+ *
+ * Note: Do NOT include a software reset (0x0103, 0x01) in any of these
+ * register arrays as some settings are written as part of ov9282_power_on,
+ * and the reset will clear them.
+ */
+static const struct ov9282_reg common_regs[] = {
+	{0x0302, 0x32},
+	{0x030e, 0x02},
+	{0x3001, 0x00},
+	{0x3004, 0x00},
+	{0x3005, 0x00},
+	{0x3006, 0x04},
+	{0x3011, 0x0a},
+	{0x3013, 0x18},
+	{0x301c, 0xf0},
+	{0x3022, 0x01},
+	{0x3030, 0x10},
+	{0x3039, 0x32},
+	{0x303a, 0x00},
+	{0x3503, 0x08},
+	{0x3505, 0x8c},
+	{0x3507, 0x03},
+	{0x3508, 0x00},
+	{0x3610, 0x80},
+	{0x3611, 0xa0},
+	{0x3620, 0x6e},
+	{0x3632, 0x56},
+	{0x3633, 0x78},
+	{0x3666, 0x00},
+	{0x366f, 0x5a},
+	{0x3680, 0x84},
+	{0x3712, 0x80},
+	{0x372d, 0x22},
+	{0x3731, 0x80},
+	{0x3732, 0x30},
+	{0x377d, 0x22},
+	{0x3788, 0x02},
+	{0x3789, 0xa4},
+	{0x378a, 0x00},
+	{0x378b, 0x4a},
+	{0x3799, 0x20},
+	{0x3881, 0x42},
+	{0x38a8, 0x02},
+	{0x38a9, 0x80},
+	{0x38b1, 0x00},
+	{0x38c4, 0x00},
+	{0x38c5, 0xc0},
+	{0x38c6, 0x04},
+	{0x38c7, 0x80},
+	{0x3920, 0xff},
+	{0x4010, 0x40},
+	{0x4043, 0x40},
+	{0x4307, 0x30},
+	{0x4317, 0x00},
+	{0x4501, 0x00},
+	{0x450a, 0x08},
+	{0x4601, 0x04},
+	{0x470f, 0x00},
+	{0x4f07, 0x00},
+	{0x5000, 0x9f},
+	{0x5001, 0x00},
+	{0x5e00, 0x00},
+	{0x5d00, 0x07},
+	{0x5d01, 0x00},
+	{0x0101, 0x01},
+	{0x1000, 0x03},
+	{0x5a08, 0x84},
+};
+
+static struct ov9282_reg_list common_regs_list = {
+	.num_of_regs = ARRAY_SIZE(common_regs),
+	.regs = common_regs,
+};
+
+#define MODE_1280_800		0
+#define MODE_1280_720		1
+#define MODE_640_400		2
+
+#define DEFAULT_MODE		MODE_1280_720
+
+/* Sensor mode registers */
+static const struct ov9282_reg mode_1280x800_regs[] = {
+	{0x3778, 0x00},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x00},
+	{0x3804, 0x05},
+	{0x3805, 0x0f},
+	{0x3806, 0x03},
+	{0x3807, 0x2f},
+	{0x3808, 0x05},
+	{0x3809, 0x00},
+	{0x380a, 0x03},
+	{0x380b, 0x20},
+	{0x3810, 0x00},
+	{0x3811, 0x08},
+	{0x3812, 0x00},
+	{0x3813, 0x08},
+	{0x3814, 0x11},
+	{0x3815, 0x11},
+	{0x3820, 0x40},
+	{0x3821, 0x00},
+	{0x4003, 0x40},
+	{0x4008, 0x04},
+	{0x4009, 0x0b},
+	{0x400c, 0x00},
+	{0x400d, 0x07},
+	{0x4507, 0x00},
+	{0x4509, 0x00},
+};
+
+static const struct ov9282_reg mode_1280x720_regs[] = {
+	{0x3778, 0x00},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x00},
+	{0x3804, 0x05},
+	{0x3805, 0x0f},
+	{0x3806, 0x02},
+	{0x3807, 0xdf},
+	{0x3808, 0x05},
+	{0x3809, 0x00},
+	{0x380a, 0x02},
+	{0x380b, 0xd0},
+	{0x3810, 0x00},
+	{0x3811, 0x08},
+	{0x3812, 0x00},
+	{0x3813, 0x08},
+	{0x3814, 0x11},
+	{0x3815, 0x11},
+	{0x3820, 0x3c},
+	{0x3821, 0x84},
+	{0x4003, 0x40},
+	{0x4008, 0x02},
+	{0x4009, 0x05},
+	{0x400c, 0x00},
+	{0x400d, 0x03},
+	{0x4507, 0x00},
+	{0x4509, 0x80},
+};
+
+static const struct ov9282_reg mode_640x400_regs[] = {
+	{0x3778, 0x10},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x00},
+	{0x3804, 0x05},
+	{0x3805, 0x0f},
+	{0x3806, 0x03},
+	{0x3807, 0x2f},
+	{0x3808, 0x02},
+	{0x3809, 0x80},
+	{0x380a, 0x01},
+	{0x380b, 0x90},
+	{0x3810, 0x00},
+	{0x3811, 0x04},
+	{0x3812, 0x00},
+	{0x3813, 0x04},
+	{0x3814, 0x31},
+	{0x3815, 0x22},
+	{0x3820, 0x60},
+	{0x3821, 0x01},
+	{0x4008, 0x02},
+	{0x4009, 0x05},
+	{0x400c, 0x00},
+	{0x400d, 0x03},
+	{0x4507, 0x03},
+	{0x4509, 0x80},
+};
+
+/* Supported sensor mode configurations */
+static const struct ov9282_mode supported_modes[] = {
+	[MODE_1280_800] = {
+		.width = 1280,
+		.height = 800,
+		.hblank_min = { 250, 176 },
+		.vblank = 1022,
+		.vblank_min = 110,
+		.vblank_max = 51540,
+		.link_freq_idx = 0,
+		.crop = {
+			.left = OV9282_PIXEL_ARRAY_LEFT,
+			.top = OV9282_PIXEL_ARRAY_TOP,
+			.width = 1280,
+			.height = 800
+		},
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1280x800_regs),
+			.regs = mode_1280x800_regs,
+		},
+	},
+	[MODE_1280_720] = {
+		.width = 1280,
+		.height = 720,
+		.hblank_min = { 250, 176 },
+		.vblank = 1022,
+		.vblank_min = 41,
+		.vblank_max = 51540,
+		.link_freq_idx = 0,
+		.crop = {
+			/*
+			 * Note that this mode takes the top 720 lines from the
+			 * 800 of the sensor. It does not take a middle crop.
+			 */
+			.left = OV9282_PIXEL_ARRAY_LEFT,
+			.top = OV9282_PIXEL_ARRAY_TOP,
+			.width = 1280,
+			.height = 720
+		},
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1280x720_regs),
+			.regs = mode_1280x720_regs,
+		},
+	},
+	[MODE_640_400] = {
+		.width = 640,
+		.height = 400,
+		.hblank_min = { 890, 816 },
+		.vblank = 1022,
+		.vblank_min = 22,
+		.vblank_max = 51540,
+		.link_freq_idx = 0,
+		.crop = {
+			.left = OV9282_PIXEL_ARRAY_LEFT,
+			.top = OV9282_PIXEL_ARRAY_TOP,
+			.width = 1280,
+			.height = 800
+		},
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_640x400_regs),
+			.regs = mode_640x400_regs,
+		},
+	},
+};
+
+/**
+ * to_ov9282() - ov9282 V4L2 sub-device to ov9282 device.
+ * @subdev: pointer to ov9282 V4L2 sub-device
+ *
+ * Return: pointer to ov9282 device
+ */
+static inline struct ov9282 *to_ov9282(struct v4l2_subdev *subdev)
+{
+	return container_of(subdev, struct ov9282, sd);
+}
+
+/**
+ * ov9282_read_reg() - Read registers.
+ * @ov9282: pointer to ov9282 device
+ * @reg: register address
+ * @len: length of bytes to read. Max supported bytes is 4
+ * @val: pointer to register value to be filled.
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int ov9282_read_reg(struct ov9282 *ov9282, u16 reg, u32 len, u32 *val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov9282->sd);
+	struct i2c_msg msgs[2] = {0};
+	u8 addr_buf[2] = {0};
+	u8 data_buf[4] = {0};
+	int ret;
+
+	if (WARN_ON(len > 4))
+		return -EINVAL;
+
+	put_unaligned_be16(reg, addr_buf);
+
+	/* Write register address */
+	msgs[0].addr = client->addr;
+	msgs[0].flags = 0;
+	msgs[0].len = ARRAY_SIZE(addr_buf);
+	msgs[0].buf = addr_buf;
+
+	/* Read data from register */
+	msgs[1].addr = client->addr;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].len = len;
+	msgs[1].buf = &data_buf[4 - len];
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret != ARRAY_SIZE(msgs))
+		return -EIO;
+
+	*val = get_unaligned_be32(data_buf);
+
+	return 0;
+}
+
+/**
+ * ov9282_write_reg() - Write register
+ * @ov9282: pointer to ov9282 device
+ * @reg: register address
+ * @len: length of bytes. Max supported bytes is 4
+ * @val: register value
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int ov9282_write_reg(struct ov9282 *ov9282, u16 reg, u32 len, u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov9282->sd);
+	u8 buf[6] = {0};
+
+	if (WARN_ON(len > 4))
+		return -EINVAL;
+
+	put_unaligned_be16(reg, buf);
+	put_unaligned_be32(val << (8 * (4 - len)), buf + 2);
+	if (i2c_master_send(client, buf, len + 2) != len + 2)
+		return -EIO;
+
+	return 0;
+}
+
+/**
+ * ov9282_write_regs() - Write a list of registers
+ * @ov9282: pointer to ov9282 device
+ * @regs: list of registers to be written
+ * @len: length of registers array
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int ov9282_write_regs(struct ov9282 *ov9282,
+			     const struct ov9282_reg *regs, u32 len)
+{
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < len; i++) {
+		ret = ov9282_write_reg(ov9282, regs[i].address, 1, regs[i].val);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+/**
+ * ov9282_update_controls() - Update control ranges based on streaming mode
+ * @ov9282: pointer to ov9282 device
+ * @mode: pointer to ov9282_mode sensor mode
+ * @fmt: pointer to the requested mode
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int ov9282_update_controls(struct ov9282 *ov9282,
+				  const struct ov9282_mode *mode,
+				  const struct v4l2_subdev_format *fmt)
+{
+	u32 hblank_min;
+	s64 pixel_rate;
+	int ret;
+
+	ret = __v4l2_ctrl_s_ctrl(ov9282->link_freq_ctrl, mode->link_freq_idx);
+	if (ret)
+		return ret;
+
+	pixel_rate = (fmt->format.code == MEDIA_BUS_FMT_Y10_1X10) ?
+		OV9282_PIXEL_RATE_10BIT : OV9282_PIXEL_RATE_8BIT;
+	ret = __v4l2_ctrl_modify_range(ov9282->pixel_rate, pixel_rate,
+				       pixel_rate, 1, pixel_rate);
+	if (ret)
+		return ret;
+
+	hblank_min = mode->hblank_min[ov9282->noncontinuous_clock ? 0 : 1];
+	ret =  __v4l2_ctrl_modify_range(ov9282->hblank_ctrl, hblank_min,
+					OV9282_TIMING_HTS_MAX - mode->width, 1,
+					hblank_min);
+	if (ret)
+		return ret;
+
+	return __v4l2_ctrl_modify_range(ov9282->vblank_ctrl, mode->vblank_min,
+					mode->vblank_max, 1, mode->vblank);
+}
+
+/**
+ * ov9282_update_exp_gain() - Set updated exposure and gain
+ * @ov9282: pointer to ov9282 device
+ * @exposure: updated exposure value
+ * @gain: updated analog gain value
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int ov9282_update_exp_gain(struct ov9282 *ov9282, u32 exposure, u32 gain)
+{
+	int ret;
+
+	dev_dbg(ov9282->dev, "Set exp %u, analog gain %u",
+		exposure, gain);
+
+	ret = ov9282_write_reg(ov9282, OV9282_REG_HOLD, 1, 1);
+	if (ret)
+		return ret;
+
+	ret = ov9282_write_reg(ov9282, OV9282_REG_EXPOSURE, 3, exposure << 4);
+	if (ret)
+		goto error_release_group_hold;
+
+	ret = ov9282_write_reg(ov9282, OV9282_REG_AGAIN, 1, gain);
+
+error_release_group_hold:
+	ov9282_write_reg(ov9282, OV9282_REG_HOLD, 1, 0);
+
+	return ret;
+}
+
+static int ov9282_set_ctrl_hflip(struct ov9282 *ov9282, int value)
+{
+	u32 current_val;
+	int ret = ov9282_read_reg(ov9282, OV9282_REG_TIMING_FORMAT_2, 1,
+				  &current_val);
+	if (ret)
+		return ret;
+
+	if (value)
+		current_val |= OV9282_FLIP_BIT;
+	else
+		current_val &= ~OV9282_FLIP_BIT;
+
+	return ov9282_write_reg(ov9282, OV9282_REG_TIMING_FORMAT_2, 1,
+				current_val);
+}
+
+static int ov9282_set_ctrl_vflip(struct ov9282 *ov9282, int value)
+{
+	u32 current_val;
+	int ret = ov9282_read_reg(ov9282, OV9282_REG_TIMING_FORMAT_1, 1,
+				  &current_val);
+	if (ret)
+		return ret;
+
+	if (value)
+		current_val |= OV9282_FLIP_BIT;
+	else
+		current_val &= ~OV9282_FLIP_BIT;
+
+	return ov9282_write_reg(ov9282, OV9282_REG_TIMING_FORMAT_1, 1,
+				current_val);
+}
+
+/**
+ * ov9282_set_ctrl() - Set subdevice control
+ * @ctrl: pointer to v4l2_ctrl structure
+ *
+ * Supported controls:
+ * - V4L2_CID_VBLANK
+ * - cluster controls:
+ *   - V4L2_CID_ANALOGUE_GAIN
+ *   - V4L2_CID_EXPOSURE
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int ov9282_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov9282 *ov9282 =
+		container_of(ctrl->handler, struct ov9282, ctrl_handler);
+	u32 analog_gain;
+	u32 exposure;
+	u32 lpfr;
+	int ret;
+
+	switch (ctrl->id) {
+	case V4L2_CID_VBLANK:
+		ov9282->vblank = ov9282->vblank_ctrl->val;
+
+		dev_dbg(ov9282->dev, "Received vblank %u, new lpfr %u",
+			ov9282->vblank,
+			ov9282->vblank + ov9282->cur_mode->height);
+
+		ret = __v4l2_ctrl_modify_range(ov9282->exp_ctrl,
+					       OV9282_EXPOSURE_MIN,
+					       ov9282->vblank +
+					       ov9282->cur_mode->height -
+					       OV9282_EXPOSURE_OFFSET,
+					       1, OV9282_EXPOSURE_DEFAULT);
+		break;
+	}
+
+	/* Set controls only if sensor is in power on state */
+	if (!pm_runtime_get_if_in_use(ov9282->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE:
+		exposure = ctrl->val;
+		analog_gain = ov9282->again_ctrl->val;
+
+		dev_dbg(ov9282->dev, "Received exp %u, analog gain %u",
+			exposure, analog_gain);
+
+		ret = ov9282_update_exp_gain(ov9282, exposure, analog_gain);
+		break;
+	case V4L2_CID_VBLANK:
+		lpfr = ov9282->vblank + ov9282->cur_mode->height;
+		ret = ov9282_write_reg(ov9282, OV9282_REG_LPFR, 2, lpfr);
+		break;
+	case V4L2_CID_HFLIP:
+		ret = ov9282_set_ctrl_hflip(ov9282, ctrl->val);
+		break;
+	case V4L2_CID_VFLIP:
+		ret = ov9282_set_ctrl_vflip(ov9282, ctrl->val);
+		break;
+	case V4L2_CID_HBLANK:
+		ret = ov9282_write_reg(ov9282, OV9282_REG_TIMING_HTS, 2,
+				       (ctrl->val + ov9282->cur_mode->width) >> 1);
+		break;
+	default:
+		dev_err(ov9282->dev, "Invalid control %d", ctrl->id);
+		ret = -EINVAL;
+	}
+
+	pm_runtime_put(ov9282->dev);
+
+	return ret;
+}
+
+/* V4l2 subdevice control ops*/
+static const struct v4l2_ctrl_ops ov9282_ctrl_ops = {
+	.s_ctrl = ov9282_set_ctrl,
+};
+
+/**
+ * ov9282_enum_mbus_code() - Enumerate V4L2 sub-device mbus codes
+ * @sd: pointer to ov9282 V4L2 sub-device structure
+ * @sd_state: V4L2 sub-device configuration
+ * @code: V4L2 sub-device code enumeration need to be filled
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int ov9282_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	switch (code->index) {
+	case 0:
+		code->code = MEDIA_BUS_FMT_Y10_1X10;
+		break;
+	case 1:
+		code->code = MEDIA_BUS_FMT_Y8_1X8;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/**
+ * ov9282_enum_frame_size() - Enumerate V4L2 sub-device frame sizes
+ * @sd: pointer to ov9282 V4L2 sub-device structure
+ * @sd_state: V4L2 sub-device configuration
+ * @fsize: V4L2 sub-device size enumeration need to be filled
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int ov9282_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fsize)
+{
+	if (fsize->index >= ARRAY_SIZE(supported_modes))
+		return -EINVAL;
+
+	if (fsize->code != MEDIA_BUS_FMT_Y10_1X10 &&
+	    fsize->code != MEDIA_BUS_FMT_Y8_1X8)
+		return -EINVAL;
+
+	fsize->min_width = supported_modes[fsize->index].width;
+	fsize->max_width = fsize->min_width;
+	fsize->min_height = supported_modes[fsize->index].height;
+	fsize->max_height = fsize->min_height;
+
+	return 0;
+}
+
+/**
+ * ov9282_fill_pad_format() - Fill subdevice pad format
+ *                            from selected sensor mode
+ * @ov9282: pointer to ov9282 device
+ * @mode: pointer to ov9282_mode sensor mode
+ * @code: mbus code to be stored
+ * @fmt: V4L2 sub-device format need to be filled
+ */
+static void ov9282_fill_pad_format(struct ov9282 *ov9282,
+				   const struct ov9282_mode *mode,
+				   u32 code,
+				   struct v4l2_subdev_format *fmt)
+{
+	fmt->format.width = mode->width;
+	fmt->format.height = mode->height;
+	fmt->format.code = code;
+	fmt->format.field = V4L2_FIELD_NONE;
+	fmt->format.colorspace = V4L2_COLORSPACE_RAW;
+	fmt->format.ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+	fmt->format.quantization = V4L2_QUANTIZATION_DEFAULT;
+	fmt->format.xfer_func = V4L2_XFER_FUNC_NONE;
+}
+
+/**
+ * ov9282_get_pad_format() - Get subdevice pad format
+ * @sd: pointer to ov9282 V4L2 sub-device structure
+ * @sd_state: V4L2 sub-device configuration
+ * @fmt: V4L2 sub-device format need to be set
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int ov9282_get_pad_format(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_format *fmt)
+{
+	struct ov9282 *ov9282 = to_ov9282(sd);
+
+	mutex_lock(&ov9282->mutex);
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		struct v4l2_mbus_framefmt *framefmt;
+
+		framefmt = v4l2_subdev_get_try_format(sd, sd_state, fmt->pad);
+		fmt->format = *framefmt;
+	} else {
+		ov9282_fill_pad_format(ov9282, ov9282->cur_mode, ov9282->code,
+				       fmt);
+	}
+
+	mutex_unlock(&ov9282->mutex);
+
+	return 0;
+}
+
+/**
+ * ov9282_set_pad_format() - Set subdevice pad format
+ * @sd: pointer to ov9282 V4L2 sub-device structure
+ * @sd_state: V4L2 sub-device configuration
+ * @fmt: V4L2 sub-device format need to be set
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int ov9282_set_pad_format(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_format *fmt)
+{
+	struct ov9282 *ov9282 = to_ov9282(sd);
+	const struct ov9282_mode *mode;
+	u32 code;
+	int ret = 0;
+
+	mutex_lock(&ov9282->mutex);
+
+	mode = v4l2_find_nearest_size(supported_modes,
+				      ARRAY_SIZE(supported_modes),
+				      width, height,
+				      fmt->format.width,
+				      fmt->format.height);
+	if (fmt->format.code == MEDIA_BUS_FMT_Y8_1X8)
+		code = MEDIA_BUS_FMT_Y8_1X8;
+	else
+		code = MEDIA_BUS_FMT_Y10_1X10;
+
+	ov9282_fill_pad_format(ov9282, mode, code, fmt);
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		struct v4l2_mbus_framefmt *framefmt;
+
+		framefmt = v4l2_subdev_get_try_format(sd, sd_state, fmt->pad);
+		*framefmt = fmt->format;
+	} else {
+		ret = ov9282_update_controls(ov9282, mode, fmt);
+		if (!ret) {
+			ov9282->cur_mode = mode;
+			ov9282->code = code;
+		}
+	}
+
+	mutex_unlock(&ov9282->mutex);
+
+	return ret;
+}
+
+/**
+ * ov9282_init_pad_cfg() - Initialize sub-device pad configuration
+ * @sd: pointer to ov9282 V4L2 sub-device structure
+ * @sd_state: V4L2 sub-device configuration
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int ov9282_init_pad_cfg(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_state *sd_state)
+{
+	struct ov9282 *ov9282 = to_ov9282(sd);
+	struct v4l2_subdev_format fmt = { 0 };
+
+	fmt.which = sd_state ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE;
+	ov9282_fill_pad_format(ov9282, &supported_modes[DEFAULT_MODE],
+			       ov9282->code, &fmt);
+
+	return ov9282_set_pad_format(sd, sd_state, &fmt);
+}
+
+static const struct v4l2_rect *
+__ov9282_get_pad_crop(struct ov9282 *ov9282,
+		      struct v4l2_subdev_state *sd_state,
+		      unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_crop(&ov9282->sd, sd_state, pad);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return &ov9282->cur_mode->crop;
+	}
+
+	return NULL;
+}
+
+static int ov9282_get_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *sd_state,
+				struct v4l2_subdev_selection *sel)
+{
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP: {
+		struct ov9282 *ov9282 = to_ov9282(sd);
+
+		mutex_lock(&ov9282->mutex);
+		sel->r = *__ov9282_get_pad_crop(ov9282, sd_state, sel->pad,
+						sel->which);
+		mutex_unlock(&ov9282->mutex);
+
+		return 0;
+	}
+
+	case V4L2_SEL_TGT_NATIVE_SIZE:
+		sel->r.top = 0;
+		sel->r.left = 0;
+		sel->r.width = OV9282_NATIVE_WIDTH;
+		sel->r.height = OV9282_NATIVE_HEIGHT;
+
+		return 0;
+
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.top = OV9282_PIXEL_ARRAY_TOP;
+		sel->r.left = OV9282_PIXEL_ARRAY_LEFT;
+		sel->r.width = OV9282_PIXEL_ARRAY_WIDTH;
+		sel->r.height = OV9282_PIXEL_ARRAY_HEIGHT;
+
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+/**
+ * ov9282_start_streaming() - Start sensor stream
+ * @ov9282: pointer to ov9282 device
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int ov9282_start_streaming(struct ov9282 *ov9282)
+{
+	const struct ov9282_reg bitdepth_regs[2][2] = {
+		{
+			{OV9282_REG_PLL_CTRL_0D, OV9282_PLL_CTRL_0D_RAW10},
+			{OV9282_REG_ANA_CORE_2, OV9282_ANA_CORE2_RAW10},
+		}, {
+			{OV9282_REG_PLL_CTRL_0D, OV9282_PLL_CTRL_0D_RAW8},
+			{OV9282_REG_ANA_CORE_2, OV9282_ANA_CORE2_RAW8},
+		}
+	};
+	const struct ov9282_reg_list *reg_list;
+	int bitdepth_index;
+	int ret;
+
+	/* Write common registers */
+	ret = ov9282_write_regs(ov9282, common_regs_list.regs,
+				common_regs_list.num_of_regs);
+	if (ret) {
+		dev_err(ov9282->dev, "fail to write common registers");
+		return ret;
+	}
+
+	bitdepth_index = ov9282->code == MEDIA_BUS_FMT_Y10_1X10 ? 0 : 1;
+	ret = ov9282_write_regs(ov9282, bitdepth_regs[bitdepth_index], 2);
+	if (ret) {
+		dev_err(ov9282->dev, "fail to write bitdepth regs");
+		return ret;
+	}
+
+	/* Write sensor mode registers */
+	reg_list = &ov9282->cur_mode->reg_list;
+	ret = ov9282_write_regs(ov9282, reg_list->regs, reg_list->num_of_regs);
+	if (ret) {
+		dev_err(ov9282->dev, "fail to write initial registers");
+		return ret;
+	}
+
+	/* Setup handler will write actual exposure and gain */
+	ret =  __v4l2_ctrl_handler_setup(ov9282->sd.ctrl_handler);
+	if (ret) {
+		dev_err(ov9282->dev, "fail to setup handler");
+		return ret;
+	}
+
+	/* Start streaming */
+	ret = ov9282_write_reg(ov9282, OV9282_REG_MODE_SELECT,
+			       1, OV9282_MODE_STREAMING);
+	if (ret) {
+		dev_err(ov9282->dev, "fail to start streaming");
+		return ret;
+	}
+
+	return 0;
+}
+
+/**
+ * ov9282_stop_streaming() - Stop sensor stream
+ * @ov9282: pointer to ov9282 device
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int ov9282_stop_streaming(struct ov9282 *ov9282)
+{
+	return ov9282_write_reg(ov9282, OV9282_REG_MODE_SELECT,
+				1, OV9282_MODE_STANDBY);
+}
+
+/**
+ * ov9282_set_stream() - Enable sensor streaming
+ * @sd: pointer to ov9282 subdevice
+ * @enable: set to enable sensor streaming
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int ov9282_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct ov9282 *ov9282 = to_ov9282(sd);
+	int ret;
+
+	mutex_lock(&ov9282->mutex);
+
+	if (ov9282->streaming == enable) {
+		mutex_unlock(&ov9282->mutex);
+		return 0;
+	}
+
+	if (enable) {
+		ret = pm_runtime_resume_and_get(ov9282->dev);
+		if (ret)
+			goto error_unlock;
+
+		ret = ov9282_start_streaming(ov9282);
+		if (ret)
+			goto error_power_off;
+	} else {
+		ov9282_stop_streaming(ov9282);
+		pm_runtime_put(ov9282->dev);
+	}
+
+	ov9282->streaming = enable;
+
+	mutex_unlock(&ov9282->mutex);
+
+	return 0;
+
+error_power_off:
+	pm_runtime_put(ov9282->dev);
+error_unlock:
+	mutex_unlock(&ov9282->mutex);
+
+	return ret;
+}
+
+/**
+ * ov9282_detect() - Detect ov9282 sensor
+ * @ov9282: pointer to ov9282 device
+ *
+ * Return: 0 if successful, -EIO if sensor id does not match
+ */
+static int ov9282_detect(struct ov9282 *ov9282)
+{
+	int ret;
+	u32 val;
+
+	ret = ov9282_read_reg(ov9282, OV9282_REG_ID, 2, &val);
+	if (ret)
+		return ret;
+
+	if (val != OV9282_ID) {
+		dev_err(ov9282->dev, "chip id mismatch: %x!=%x",
+			OV9282_ID, val);
+		return -ENXIO;
+	}
+
+	return 0;
+}
+
+static int ov9282_configure_regulators(struct ov9282 *ov9282)
+{
+	unsigned int i;
+
+	for (i = 0; i < OV9282_NUM_SUPPLIES; i++)
+		ov9282->supplies[i].supply = ov9282_supply_names[i];
+
+	return devm_regulator_bulk_get(ov9282->dev,
+				       OV9282_NUM_SUPPLIES,
+				       ov9282->supplies);
+}
+
+/**
+ * ov9282_parse_hw_config() - Parse HW configuration and check if supported
+ * @ov9282: pointer to ov9282 device
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int ov9282_parse_hw_config(struct ov9282 *ov9282)
+{
+	struct fwnode_handle *fwnode = dev_fwnode(ov9282->dev);
+	struct v4l2_fwnode_endpoint bus_cfg = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY
+	};
+	struct fwnode_handle *ep;
+	unsigned long rate;
+	unsigned int i;
+	int ret;
+
+	if (!fwnode)
+		return -ENXIO;
+
+	/* Request optional reset pin */
+	ov9282->reset_gpio = devm_gpiod_get_optional(ov9282->dev, "reset",
+						     GPIOD_OUT_LOW);
+	if (IS_ERR(ov9282->reset_gpio)) {
+		dev_err(ov9282->dev, "failed to get reset gpio %ld",
+			PTR_ERR(ov9282->reset_gpio));
+		return PTR_ERR(ov9282->reset_gpio);
+	}
+
+	/* Get sensor input clock */
+	ov9282->inclk = devm_clk_get(ov9282->dev, NULL);
+	if (IS_ERR(ov9282->inclk)) {
+		dev_err(ov9282->dev, "could not get inclk");
+		return PTR_ERR(ov9282->inclk);
+	}
+
+	ret = ov9282_configure_regulators(ov9282);
+	if (ret)
+		return dev_err_probe(ov9282->dev, ret,
+				     "Failed to get power regulators\n");
+
+	rate = clk_get_rate(ov9282->inclk);
+	if (rate != OV9282_INCLK_RATE) {
+		dev_err(ov9282->dev, "inclk frequency mismatch");
+		return -EINVAL;
+	}
+
+	ep = fwnode_graph_get_next_endpoint(fwnode, NULL);
+	if (!ep)
+		return -ENXIO;
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(ep, &bus_cfg);
+	fwnode_handle_put(ep);
+	if (ret)
+		return ret;
+
+	ov9282->noncontinuous_clock =
+		bus_cfg.bus.mipi_csi2.flags & V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK;
+
+	if (bus_cfg.bus.mipi_csi2.num_data_lanes != OV9282_NUM_DATA_LANES) {
+		dev_err(ov9282->dev,
+			"number of CSI2 data lanes %d is not supported",
+			bus_cfg.bus.mipi_csi2.num_data_lanes);
+		ret = -EINVAL;
+		goto done_endpoint_free;
+	}
+
+	if (!bus_cfg.nr_of_link_frequencies) {
+		dev_err(ov9282->dev, "no link frequencies defined");
+		ret = -EINVAL;
+		goto done_endpoint_free;
+	}
+
+	for (i = 0; i < bus_cfg.nr_of_link_frequencies; i++)
+		if (bus_cfg.link_frequencies[i] == OV9282_LINK_FREQ)
+			goto done_endpoint_free;
+
+	ret = -EINVAL;
+
+done_endpoint_free:
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+
+	return ret;
+}
+
+/* V4l2 subdevice ops */
+static const struct v4l2_subdev_core_ops ov9282_core_ops = {
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_video_ops ov9282_video_ops = {
+	.s_stream = ov9282_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops ov9282_pad_ops = {
+	.init_cfg = ov9282_init_pad_cfg,
+	.enum_mbus_code = ov9282_enum_mbus_code,
+	.enum_frame_size = ov9282_enum_frame_size,
+	.get_fmt = ov9282_get_pad_format,
+	.set_fmt = ov9282_set_pad_format,
+	.get_selection = ov9282_get_selection,
+};
+
+static const struct v4l2_subdev_ops ov9282_subdev_ops = {
+	.core = &ov9282_core_ops,
+	.video = &ov9282_video_ops,
+	.pad = &ov9282_pad_ops,
+};
+
+/**
+ * ov9282_power_on() - Sensor power on sequence
+ * @dev: pointer to i2c device
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int ov9282_power_on(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov9282 *ov9282 = to_ov9282(sd);
+	int ret;
+
+	ret = regulator_bulk_enable(OV9282_NUM_SUPPLIES, ov9282->supplies);
+	if (ret < 0) {
+		dev_err(dev, "Failed to enable regulators\n");
+		return ret;
+	}
+
+	usleep_range(400, 600);
+
+	gpiod_set_value_cansleep(ov9282->reset_gpio, 1);
+
+	ret = clk_prepare_enable(ov9282->inclk);
+	if (ret) {
+		dev_err(ov9282->dev, "fail to enable inclk");
+		goto error_reset;
+	}
+
+	usleep_range(400, 600);
+
+	ret = ov9282_write_reg(ov9282, OV9282_REG_MIPI_CTRL00, 1,
+			       ov9282->noncontinuous_clock ?
+					OV9282_GATED_CLOCK : 0);
+	if (ret) {
+		dev_err(ov9282->dev, "fail to write MIPI_CTRL00");
+		goto error_clk;
+	}
+
+	return 0;
+
+error_clk:
+	clk_disable_unprepare(ov9282->inclk);
+error_reset:
+	gpiod_set_value_cansleep(ov9282->reset_gpio, 0);
+
+	regulator_bulk_disable(OV9282_NUM_SUPPLIES, ov9282->supplies);
+
+	return ret;
+}
+
+/**
+ * ov9282_power_off() - Sensor power off sequence
+ * @dev: pointer to i2c device
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int ov9282_power_off(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov9282 *ov9282 = to_ov9282(sd);
+
+	gpiod_set_value_cansleep(ov9282->reset_gpio, 0);
+
+	clk_disable_unprepare(ov9282->inclk);
+
+	regulator_bulk_disable(OV9282_NUM_SUPPLIES, ov9282->supplies);
+
+	return 0;
+}
+
+/**
+ * ov9282_init_controls() - Initialize sensor subdevice controls
+ * @ov9282: pointer to ov9282 device
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int ov9282_init_controls(struct ov9282 *ov9282)
+{
+	struct v4l2_ctrl_handler *ctrl_hdlr = &ov9282->ctrl_handler;
+	const struct ov9282_mode *mode = ov9282->cur_mode;
+	struct v4l2_fwnode_device_properties props;
+	u32 hblank_min;
+	u32 lpfr;
+	int ret;
+
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 10);
+	if (ret)
+		return ret;
+
+	/* Serialize controls with sensor device */
+	ctrl_hdlr->lock = &ov9282->mutex;
+
+	/* Initialize exposure and gain */
+	lpfr = mode->vblank + mode->height;
+	ov9282->exp_ctrl = v4l2_ctrl_new_std(ctrl_hdlr,
+					     &ov9282_ctrl_ops,
+					     V4L2_CID_EXPOSURE,
+					     OV9282_EXPOSURE_MIN,
+					     lpfr - OV9282_EXPOSURE_OFFSET,
+					     OV9282_EXPOSURE_STEP,
+					     OV9282_EXPOSURE_DEFAULT);
+
+	ov9282->again_ctrl = v4l2_ctrl_new_std(ctrl_hdlr,
+					       &ov9282_ctrl_ops,
+					       V4L2_CID_ANALOGUE_GAIN,
+					       OV9282_AGAIN_MIN,
+					       OV9282_AGAIN_MAX,
+					       OV9282_AGAIN_STEP,
+					       OV9282_AGAIN_DEFAULT);
+
+	v4l2_ctrl_cluster(2, &ov9282->exp_ctrl);
+
+	ov9282->vblank_ctrl = v4l2_ctrl_new_std(ctrl_hdlr,
+						&ov9282_ctrl_ops,
+						V4L2_CID_VBLANK,
+						mode->vblank_min,
+						mode->vblank_max,
+						1, mode->vblank);
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov9282_ctrl_ops, V4L2_CID_VFLIP,
+			  0, 1, 1, 1);
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov9282_ctrl_ops, V4L2_CID_HFLIP,
+			  0, 1, 1, 1);
+
+	/* Read only controls */
+	ov9282->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, &ov9282_ctrl_ops,
+					       V4L2_CID_PIXEL_RATE,
+					       OV9282_PIXEL_RATE_10BIT,
+					       OV9282_PIXEL_RATE_10BIT, 1,
+					       OV9282_PIXEL_RATE_10BIT);
+
+	ov9282->link_freq_ctrl = v4l2_ctrl_new_int_menu(ctrl_hdlr,
+							&ov9282_ctrl_ops,
+							V4L2_CID_LINK_FREQ,
+							ARRAY_SIZE(link_freq) -
+							1,
+							mode->link_freq_idx,
+							link_freq);
+	if (ov9282->link_freq_ctrl)
+		ov9282->link_freq_ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	hblank_min = mode->hblank_min[ov9282->noncontinuous_clock ? 0 : 1];
+	ov9282->hblank_ctrl = v4l2_ctrl_new_std(ctrl_hdlr,
+						&ov9282_ctrl_ops,
+						V4L2_CID_HBLANK,
+						hblank_min,
+						OV9282_TIMING_HTS_MAX - mode->width,
+						1, hblank_min);
+
+	ret = v4l2_fwnode_device_parse(ov9282->dev, &props);
+	if (!ret) {
+		/* Failure sets ctrl_hdlr->error, which we check afterwards anyway */
+		v4l2_ctrl_new_fwnode_properties(ctrl_hdlr, &ov9282_ctrl_ops,
+						&props);
+	}
+
+	if (ctrl_hdlr->error || ret) {
+		dev_err(ov9282->dev, "control init failed: %d",
+			ctrl_hdlr->error);
+		v4l2_ctrl_handler_free(ctrl_hdlr);
+		return ctrl_hdlr->error;
+	}
+
+	ov9282->sd.ctrl_handler = ctrl_hdlr;
+
+	return 0;
+}
+
+/**
+ * ov9282_probe() - I2C client device binding
+ * @client: pointer to i2c client device
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static int ov9282_probe(struct i2c_client *client)
+{
+	struct ov9282 *ov9282;
+	int ret;
+
+	ov9282 = devm_kzalloc(&client->dev, sizeof(*ov9282), GFP_KERNEL);
+	if (!ov9282)
+		return -ENOMEM;
+
+	ov9282->dev = &client->dev;
+
+	/* Initialize subdev */
+	v4l2_i2c_subdev_init(&ov9282->sd, client, &ov9282_subdev_ops);
+	v4l2_i2c_subdev_set_name(&ov9282->sd, client,
+				 device_get_match_data(ov9282->dev), NULL);
+
+	ret = ov9282_parse_hw_config(ov9282);
+	if (ret) {
+		dev_err(ov9282->dev, "HW configuration is not supported");
+		return ret;
+	}
+
+	mutex_init(&ov9282->mutex);
+
+	ret = ov9282_power_on(ov9282->dev);
+	if (ret) {
+		dev_err(ov9282->dev, "failed to power-on the sensor");
+		goto error_mutex_destroy;
+	}
+
+	/* Check module identity */
+	ret = ov9282_detect(ov9282);
+	if (ret) {
+		dev_err(ov9282->dev, "failed to find sensor: %d", ret);
+		goto error_power_off;
+	}
+
+	/* Set default mode to first mode */
+	ov9282->cur_mode = &supported_modes[DEFAULT_MODE];
+	ov9282->code = MEDIA_BUS_FMT_Y10_1X10;
+	ov9282->vblank = ov9282->cur_mode->vblank;
+
+	ret = ov9282_init_controls(ov9282);
+	if (ret) {
+		dev_err(ov9282->dev, "failed to init controls: %d", ret);
+		goto error_power_off;
+	}
+
+	/* Initialize subdev */
+	ov9282->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+			    V4L2_SUBDEV_FL_HAS_EVENTS;
+	ov9282->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+	/* Initialize source pad */
+	ov9282->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&ov9282->sd.entity, 1, &ov9282->pad);
+	if (ret) {
+		dev_err(ov9282->dev, "failed to init entity pads: %d", ret);
+		goto error_handler_free;
+	}
+
+	ret = v4l2_async_register_subdev_sensor(&ov9282->sd);
+	if (ret < 0) {
+		dev_err(ov9282->dev,
+			"failed to register async subdev: %d", ret);
+		goto error_media_entity;
+	}
+
+	pm_runtime_set_active(ov9282->dev);
+	pm_runtime_enable(ov9282->dev);
+	pm_runtime_idle(ov9282->dev);
+
+	return 0;
+
+error_media_entity:
+	media_entity_cleanup(&ov9282->sd.entity);
+error_handler_free:
+	v4l2_ctrl_handler_free(ov9282->sd.ctrl_handler);
+error_power_off:
+	ov9282_power_off(ov9282->dev);
+error_mutex_destroy:
+	mutex_destroy(&ov9282->mutex);
+
+	return ret;
+}
+
+/**
+ * ov9282_remove() - I2C client device unbinding
+ * @client: pointer to I2C client device
+ *
+ * Return: 0 if successful, error code otherwise.
+ */
+static void ov9282_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov9282 *ov9282 = to_ov9282(sd);
+
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+
+	pm_runtime_disable(&client->dev);
+	if (!pm_runtime_status_suspended(&client->dev))
+		ov9282_power_off(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+
+	mutex_destroy(&ov9282->mutex);
+}
+
+static const struct dev_pm_ops ov9282_pm_ops = {
+	SET_RUNTIME_PM_OPS(ov9282_power_off, ov9282_power_on, NULL)
+};
+
+static const struct of_device_id ov9282_of_match[] = {
+	{ .compatible = "ovti,ov9281", .data = "ov9281" },
+	{ .compatible = "ovti,ov9282", .data = "ov9282" },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(of, ov9282_of_match);
+
+static struct i2c_driver ov9282_driver = {
+	.probe = ov9282_probe,
+	.remove = ov9282_remove,
+	.driver = {
+		.name = "ov9282",
+		.pm = &ov9282_pm_ops,
+		.of_match_table = ov9282_of_match,
+	},
+};
+
+module_i2c_driver(ov9282_driver);
+
+MODULE_DESCRIPTION("OmniVision ov9282 sensor driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/ov9640.c b/drivers/media/i2c/ov9640.c
new file mode 100644
index 000000000..cbaea0495
--- /dev/null
+++ b/drivers/media/i2c/ov9640.c
@@ -0,0 +1,774 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * OmniVision OV96xx Camera Driver
+ *
+ * Copyright (C) 2009 Marek Vasut <marek.vasut@gmail.com>
+ *
+ * Based on ov772x camera driver:
+ *
+ * Copyright (C) 2008 Renesas Solutions Corp.
+ * Kuninori Morimoto <morimoto.kuninori@renesas.com>
+ *
+ * Based on ov7670 and soc_camera_platform driver,
+ * transition from soc_camera to pxa_camera based on mt9m111
+ *
+ * Copyright 2006-7 Jonathan Corbet <corbet@lwn.net>
+ * Copyright (C) 2008 Magnus Damm
+ * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
+ */
+
+#include <linux/clk.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/v4l2-mediabus.h>
+#include <linux/videodev2.h>
+
+#include <media/v4l2-async.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+
+#include <linux/gpio/consumer.h>
+
+#include "ov9640.h"
+
+#define to_ov9640_sensor(sd)	container_of(sd, struct ov9640_priv, subdev)
+
+/* default register setup */
+static const struct ov9640_reg ov9640_regs_dflt[] = {
+	{ OV9640_COM5,	OV9640_COM5_SYSCLK | OV9640_COM5_LONGEXP },
+	{ OV9640_COM6,	OV9640_COM6_OPT_BLC | OV9640_COM6_ADBLC_BIAS |
+			OV9640_COM6_FMT_RST | OV9640_COM6_ADBLC_OPTEN },
+	{ OV9640_PSHFT,	OV9640_PSHFT_VAL(0x01) },
+	{ OV9640_ACOM,	OV9640_ACOM_2X_ANALOG | OV9640_ACOM_RSVD },
+	{ OV9640_TSLB,	OV9640_TSLB_YUYV_UYVY },
+	{ OV9640_COM16,	OV9640_COM16_RB_AVG },
+
+	/* Gamma curve P */
+	{ 0x6c, 0x40 },	{ 0x6d, 0x30 },	{ 0x6e, 0x4b },	{ 0x6f, 0x60 },
+	{ 0x70, 0x70 },	{ 0x71, 0x70 },	{ 0x72, 0x70 },	{ 0x73, 0x70 },
+	{ 0x74, 0x60 },	{ 0x75, 0x60 },	{ 0x76, 0x50 },	{ 0x77, 0x48 },
+	{ 0x78, 0x3a },	{ 0x79, 0x2e },	{ 0x7a, 0x28 },	{ 0x7b, 0x22 },
+
+	/* Gamma curve T */
+	{ 0x7c, 0x04 },	{ 0x7d, 0x07 },	{ 0x7e, 0x10 },	{ 0x7f, 0x28 },
+	{ 0x80, 0x36 },	{ 0x81, 0x44 },	{ 0x82, 0x52 },	{ 0x83, 0x60 },
+	{ 0x84, 0x6c },	{ 0x85, 0x78 },	{ 0x86, 0x8c },	{ 0x87, 0x9e },
+	{ 0x88, 0xbb },	{ 0x89, 0xd2 },	{ 0x8a, 0xe6 },
+};
+
+/* Configurations
+ * NOTE: for YUV, alter the following registers:
+ *		COM12 |= OV9640_COM12_YUV_AVG
+ *
+ *	 for RGB, alter the following registers:
+ *		COM7  |= OV9640_COM7_RGB
+ *		COM13 |= OV9640_COM13_RGB_AVG
+ *		COM15 |= proper RGB color encoding mode
+ */
+static const struct ov9640_reg ov9640_regs_qqcif[] = {
+	{ OV9640_CLKRC,	OV9640_CLKRC_DPLL_EN | OV9640_CLKRC_DIV(0x0f) },
+	{ OV9640_COM1,	OV9640_COM1_QQFMT | OV9640_COM1_HREF_2SKIP },
+	{ OV9640_COM4,	OV9640_COM4_QQ_VP | OV9640_COM4_RSVD },
+	{ OV9640_COM7,	OV9640_COM7_QCIF },
+	{ OV9640_COM12,	OV9640_COM12_RSVD },
+	{ OV9640_COM13,	OV9640_COM13_GAMMA_RAW | OV9640_COM13_MATRIX_EN },
+	{ OV9640_COM15,	OV9640_COM15_OR_10F0 },
+};
+
+static const struct ov9640_reg ov9640_regs_qqvga[] = {
+	{ OV9640_CLKRC,	OV9640_CLKRC_DPLL_EN | OV9640_CLKRC_DIV(0x07) },
+	{ OV9640_COM1,	OV9640_COM1_QQFMT | OV9640_COM1_HREF_2SKIP },
+	{ OV9640_COM4,	OV9640_COM4_QQ_VP | OV9640_COM4_RSVD },
+	{ OV9640_COM7,	OV9640_COM7_QVGA },
+	{ OV9640_COM12,	OV9640_COM12_RSVD },
+	{ OV9640_COM13,	OV9640_COM13_GAMMA_RAW | OV9640_COM13_MATRIX_EN },
+	{ OV9640_COM15,	OV9640_COM15_OR_10F0 },
+};
+
+static const struct ov9640_reg ov9640_regs_qcif[] = {
+	{ OV9640_CLKRC,	OV9640_CLKRC_DPLL_EN | OV9640_CLKRC_DIV(0x07) },
+	{ OV9640_COM4,	OV9640_COM4_QQ_VP | OV9640_COM4_RSVD },
+	{ OV9640_COM7,	OV9640_COM7_QCIF },
+	{ OV9640_COM12,	OV9640_COM12_RSVD },
+	{ OV9640_COM13,	OV9640_COM13_GAMMA_RAW | OV9640_COM13_MATRIX_EN },
+	{ OV9640_COM15,	OV9640_COM15_OR_10F0 },
+};
+
+static const struct ov9640_reg ov9640_regs_qvga[] = {
+	{ OV9640_CLKRC,	OV9640_CLKRC_DPLL_EN | OV9640_CLKRC_DIV(0x03) },
+	{ OV9640_COM4,	OV9640_COM4_QQ_VP | OV9640_COM4_RSVD },
+	{ OV9640_COM7,	OV9640_COM7_QVGA },
+	{ OV9640_COM12,	OV9640_COM12_RSVD },
+	{ OV9640_COM13,	OV9640_COM13_GAMMA_RAW | OV9640_COM13_MATRIX_EN },
+	{ OV9640_COM15,	OV9640_COM15_OR_10F0 },
+};
+
+static const struct ov9640_reg ov9640_regs_cif[] = {
+	{ OV9640_CLKRC,	OV9640_CLKRC_DPLL_EN | OV9640_CLKRC_DIV(0x03) },
+	{ OV9640_COM3,	OV9640_COM3_VP },
+	{ OV9640_COM7,	OV9640_COM7_CIF },
+	{ OV9640_COM12,	OV9640_COM12_RSVD },
+	{ OV9640_COM13,	OV9640_COM13_GAMMA_RAW | OV9640_COM13_MATRIX_EN },
+	{ OV9640_COM15,	OV9640_COM15_OR_10F0 },
+};
+
+static const struct ov9640_reg ov9640_regs_vga[] = {
+	{ OV9640_CLKRC,	OV9640_CLKRC_DPLL_EN | OV9640_CLKRC_DIV(0x01) },
+	{ OV9640_COM3,	OV9640_COM3_VP },
+	{ OV9640_COM7,	OV9640_COM7_VGA },
+	{ OV9640_COM12,	OV9640_COM12_RSVD },
+	{ OV9640_COM13,	OV9640_COM13_GAMMA_RAW | OV9640_COM13_MATRIX_EN },
+	{ OV9640_COM15,	OV9640_COM15_OR_10F0 },
+};
+
+static const struct ov9640_reg ov9640_regs_sxga[] = {
+	{ OV9640_CLKRC,	OV9640_CLKRC_DPLL_EN | OV9640_CLKRC_DIV(0x01) },
+	{ OV9640_COM3,	OV9640_COM3_VP },
+	{ OV9640_COM7,	0 },
+	{ OV9640_COM12,	OV9640_COM12_RSVD },
+	{ OV9640_COM13,	OV9640_COM13_GAMMA_RAW | OV9640_COM13_MATRIX_EN },
+	{ OV9640_COM15,	OV9640_COM15_OR_10F0 },
+};
+
+static const struct ov9640_reg ov9640_regs_yuv[] = {
+	{ OV9640_MTX1,	0x58 },
+	{ OV9640_MTX2,	0x48 },
+	{ OV9640_MTX3,	0x10 },
+	{ OV9640_MTX4,	0x28 },
+	{ OV9640_MTX5,	0x48 },
+	{ OV9640_MTX6,	0x70 },
+	{ OV9640_MTX7,	0x40 },
+	{ OV9640_MTX8,	0x40 },
+	{ OV9640_MTX9,	0x40 },
+	{ OV9640_MTXS,	0x0f },
+};
+
+static const struct ov9640_reg ov9640_regs_rgb[] = {
+	{ OV9640_MTX1,	0x71 },
+	{ OV9640_MTX2,	0x3e },
+	{ OV9640_MTX3,	0x0c },
+	{ OV9640_MTX4,	0x33 },
+	{ OV9640_MTX5,	0x72 },
+	{ OV9640_MTX6,	0x00 },
+	{ OV9640_MTX7,	0x2b },
+	{ OV9640_MTX8,	0x66 },
+	{ OV9640_MTX9,	0xd2 },
+	{ OV9640_MTXS,	0x65 },
+};
+
+static const u32 ov9640_codes[] = {
+	MEDIA_BUS_FMT_UYVY8_2X8,
+	MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE,
+	MEDIA_BUS_FMT_RGB565_2X8_LE,
+};
+
+/* read a register */
+static int ov9640_reg_read(struct i2c_client *client, u8 reg, u8 *val)
+{
+	int ret;
+	u8 data = reg;
+	struct i2c_msg msg = {
+		.addr	= client->addr,
+		.flags	= 0,
+		.len	= 1,
+		.buf	= &data,
+	};
+
+	ret = i2c_transfer(client->adapter, &msg, 1);
+	if (ret < 0)
+		goto err;
+
+	msg.flags = I2C_M_RD;
+	ret = i2c_transfer(client->adapter, &msg, 1);
+	if (ret < 0)
+		goto err;
+
+	*val = data;
+	return 0;
+
+err:
+	dev_err(&client->dev, "Failed reading register 0x%02x!\n", reg);
+	return ret;
+}
+
+/* write a register */
+static int ov9640_reg_write(struct i2c_client *client, u8 reg, u8 val)
+{
+	int ret;
+	u8 _val;
+	unsigned char data[2] = { reg, val };
+	struct i2c_msg msg = {
+		.addr	= client->addr,
+		.flags	= 0,
+		.len	= 2,
+		.buf	= data,
+	};
+
+	ret = i2c_transfer(client->adapter, &msg, 1);
+	if (ret < 0) {
+		dev_err(&client->dev, "Failed writing register 0x%02x!\n", reg);
+		return ret;
+	}
+
+	/* we have to read the register back ... no idea why, maybe HW bug */
+	ret = ov9640_reg_read(client, reg, &_val);
+	if (ret)
+		dev_err(&client->dev,
+			"Failed reading back register 0x%02x!\n", reg);
+
+	return 0;
+}
+
+
+/* Read a register, alter its bits, write it back */
+static int ov9640_reg_rmw(struct i2c_client *client, u8 reg, u8 set, u8 unset)
+{
+	u8 val;
+	int ret;
+
+	ret = ov9640_reg_read(client, reg, &val);
+	if (ret) {
+		dev_err(&client->dev,
+			"[Read]-Modify-Write of register %02x failed!\n", reg);
+		return ret;
+	}
+
+	val |= set;
+	val &= ~unset;
+
+	ret = ov9640_reg_write(client, reg, val);
+	if (ret)
+		dev_err(&client->dev,
+			"Read-Modify-[Write] of register %02x failed!\n", reg);
+
+	return ret;
+}
+
+/* Soft reset the camera. This has nothing to do with the RESET pin! */
+static int ov9640_reset(struct i2c_client *client)
+{
+	int ret;
+
+	ret = ov9640_reg_write(client, OV9640_COM7, OV9640_COM7_SCCB_RESET);
+	if (ret)
+		dev_err(&client->dev,
+			"An error occurred while entering soft reset!\n");
+
+	return ret;
+}
+
+/* Start/Stop streaming from the device */
+static int ov9640_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	return 0;
+}
+
+/* Set status of additional camera capabilities */
+static int ov9640_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov9640_priv *priv = container_of(ctrl->handler,
+						struct ov9640_priv, hdl);
+	struct i2c_client *client = v4l2_get_subdevdata(&priv->subdev);
+
+	switch (ctrl->id) {
+	case V4L2_CID_VFLIP:
+		if (ctrl->val)
+			return ov9640_reg_rmw(client, OV9640_MVFP,
+					      OV9640_MVFP_V, 0);
+		return ov9640_reg_rmw(client, OV9640_MVFP, 0, OV9640_MVFP_V);
+	case V4L2_CID_HFLIP:
+		if (ctrl->val)
+			return ov9640_reg_rmw(client, OV9640_MVFP,
+					      OV9640_MVFP_H, 0);
+		return ov9640_reg_rmw(client, OV9640_MVFP, 0, OV9640_MVFP_H);
+	}
+
+	return -EINVAL;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int ov9640_get_register(struct v4l2_subdev *sd,
+				struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+	u8 val;
+
+	if (reg->reg & ~0xff)
+		return -EINVAL;
+
+	reg->size = 1;
+
+	ret = ov9640_reg_read(client, reg->reg, &val);
+	if (ret)
+		return ret;
+
+	reg->val = (__u64)val;
+
+	return 0;
+}
+
+static int ov9640_set_register(struct v4l2_subdev *sd,
+				const struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (reg->reg & ~0xff || reg->val & ~0xff)
+		return -EINVAL;
+
+	return ov9640_reg_write(client, reg->reg, reg->val);
+}
+#endif
+
+static int ov9640_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct ov9640_priv *priv = to_ov9640_sensor(sd);
+	int ret = 0;
+
+	if (on) {
+		gpiod_set_value(priv->gpio_power, 1);
+		usleep_range(1000, 2000);
+		ret = clk_prepare_enable(priv->clk);
+		usleep_range(1000, 2000);
+		gpiod_set_value(priv->gpio_reset, 0);
+	} else {
+		gpiod_set_value(priv->gpio_reset, 1);
+		usleep_range(1000, 2000);
+		clk_disable_unprepare(priv->clk);
+		usleep_range(1000, 2000);
+		gpiod_set_value(priv->gpio_power, 0);
+	}
+
+	return ret;
+}
+
+/* select nearest higher resolution for capture */
+static void ov9640_res_roundup(u32 *width, u32 *height)
+{
+	unsigned int i;
+	enum { QQCIF, QQVGA, QCIF, QVGA, CIF, VGA, SXGA };
+	static const u32 res_x[] = { 88, 160, 176, 320, 352, 640, 1280 };
+	static const u32 res_y[] = { 72, 120, 144, 240, 288, 480, 960 };
+
+	for (i = 0; i < ARRAY_SIZE(res_x); i++) {
+		if (res_x[i] >= *width && res_y[i] >= *height) {
+			*width = res_x[i];
+			*height = res_y[i];
+			return;
+		}
+	}
+
+	*width = res_x[SXGA];
+	*height = res_y[SXGA];
+}
+
+/* Prepare necessary register changes depending on color encoding */
+static void ov9640_alter_regs(u32 code,
+			      struct ov9640_reg_alt *alt)
+{
+	switch (code) {
+	default:
+	case MEDIA_BUS_FMT_UYVY8_2X8:
+		alt->com12	= OV9640_COM12_YUV_AVG;
+		alt->com13	= OV9640_COM13_Y_DELAY_EN |
+					OV9640_COM13_YUV_DLY(0x01);
+		break;
+	case MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE:
+		alt->com7	= OV9640_COM7_RGB;
+		alt->com13	= OV9640_COM13_RGB_AVG;
+		alt->com15	= OV9640_COM15_RGB_555;
+		break;
+	case MEDIA_BUS_FMT_RGB565_2X8_LE:
+		alt->com7	= OV9640_COM7_RGB;
+		alt->com13	= OV9640_COM13_RGB_AVG;
+		alt->com15	= OV9640_COM15_RGB_565;
+		break;
+	}
+}
+
+/* Setup registers according to resolution and color encoding */
+static int ov9640_write_regs(struct i2c_client *client, u32 width,
+		u32 code, struct ov9640_reg_alt *alts)
+{
+	const struct ov9640_reg	*ov9640_regs, *matrix_regs;
+	unsigned int		ov9640_regs_len, matrix_regs_len;
+	unsigned int		i;
+	int			ret;
+	u8			val;
+
+	/* select register configuration for given resolution */
+	switch (width) {
+	case W_QQCIF:
+		ov9640_regs	= ov9640_regs_qqcif;
+		ov9640_regs_len	= ARRAY_SIZE(ov9640_regs_qqcif);
+		break;
+	case W_QQVGA:
+		ov9640_regs	= ov9640_regs_qqvga;
+		ov9640_regs_len	= ARRAY_SIZE(ov9640_regs_qqvga);
+		break;
+	case W_QCIF:
+		ov9640_regs	= ov9640_regs_qcif;
+		ov9640_regs_len	= ARRAY_SIZE(ov9640_regs_qcif);
+		break;
+	case W_QVGA:
+		ov9640_regs	= ov9640_regs_qvga;
+		ov9640_regs_len	= ARRAY_SIZE(ov9640_regs_qvga);
+		break;
+	case W_CIF:
+		ov9640_regs	= ov9640_regs_cif;
+		ov9640_regs_len	= ARRAY_SIZE(ov9640_regs_cif);
+		break;
+	case W_VGA:
+		ov9640_regs	= ov9640_regs_vga;
+		ov9640_regs_len	= ARRAY_SIZE(ov9640_regs_vga);
+		break;
+	case W_SXGA:
+		ov9640_regs	= ov9640_regs_sxga;
+		ov9640_regs_len	= ARRAY_SIZE(ov9640_regs_sxga);
+		break;
+	default:
+		dev_err(&client->dev, "Failed to select resolution!\n");
+		return -EINVAL;
+	}
+
+	/* select color matrix configuration for given color encoding */
+	if (code == MEDIA_BUS_FMT_UYVY8_2X8) {
+		matrix_regs	= ov9640_regs_yuv;
+		matrix_regs_len	= ARRAY_SIZE(ov9640_regs_yuv);
+	} else {
+		matrix_regs	= ov9640_regs_rgb;
+		matrix_regs_len	= ARRAY_SIZE(ov9640_regs_rgb);
+	}
+
+	/* write register settings into the module */
+	for (i = 0; i < ov9640_regs_len; i++) {
+		val = ov9640_regs[i].val;
+
+		switch (ov9640_regs[i].reg) {
+		case OV9640_COM7:
+			val |= alts->com7;
+			break;
+		case OV9640_COM12:
+			val |= alts->com12;
+			break;
+		case OV9640_COM13:
+			val |= alts->com13;
+			break;
+		case OV9640_COM15:
+			val |= alts->com15;
+			break;
+		}
+
+		ret = ov9640_reg_write(client, ov9640_regs[i].reg, val);
+		if (ret)
+			return ret;
+	}
+
+	/* write color matrix configuration into the module */
+	for (i = 0; i < matrix_regs_len; i++) {
+		ret = ov9640_reg_write(client, matrix_regs[i].reg,
+				       matrix_regs[i].val);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+/* program default register values */
+static int ov9640_prog_dflt(struct i2c_client *client)
+{
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < ARRAY_SIZE(ov9640_regs_dflt); i++) {
+		ret = ov9640_reg_write(client, ov9640_regs_dflt[i].reg,
+				       ov9640_regs_dflt[i].val);
+		if (ret)
+			return ret;
+	}
+
+	/* wait for the changes to actually happen, 140ms are not enough yet */
+	msleep(150);
+
+	return 0;
+}
+
+/* set the format we will capture in */
+static int ov9640_s_fmt(struct v4l2_subdev *sd,
+			struct v4l2_mbus_framefmt *mf)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov9640_reg_alt alts = {0};
+	int ret;
+
+	ov9640_alter_regs(mf->code, &alts);
+
+	ov9640_reset(client);
+
+	ret = ov9640_prog_dflt(client);
+	if (ret)
+		return ret;
+
+	return ov9640_write_regs(client, mf->width, mf->code, &alts);
+}
+
+static int ov9640_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *mf = &format->format;
+
+	if (format->pad)
+		return -EINVAL;
+
+	ov9640_res_roundup(&mf->width, &mf->height);
+
+	mf->field = V4L2_FIELD_NONE;
+
+	switch (mf->code) {
+	case MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE:
+	case MEDIA_BUS_FMT_RGB565_2X8_LE:
+		mf->colorspace = V4L2_COLORSPACE_SRGB;
+		break;
+	default:
+		mf->code = MEDIA_BUS_FMT_UYVY8_2X8;
+		fallthrough;
+	case MEDIA_BUS_FMT_UYVY8_2X8:
+		mf->colorspace = V4L2_COLORSPACE_JPEG;
+		break;
+	}
+
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		return ov9640_s_fmt(sd, mf);
+
+	sd_state->pads->try_fmt = *mf;
+
+	return 0;
+}
+
+static int ov9640_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->pad || code->index >= ARRAY_SIZE(ov9640_codes))
+		return -EINVAL;
+
+	code->code = ov9640_codes[code->index];
+
+	return 0;
+}
+
+static int ov9640_get_selection(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_selection *sel)
+{
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
+		return -EINVAL;
+
+	sel->r.left = 0;
+	sel->r.top = 0;
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP:
+		sel->r.width = W_SXGA;
+		sel->r.height = H_SXGA;
+		return 0;
+	default:
+		return -EINVAL;
+	}
+}
+
+static int ov9640_video_probe(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov9640_priv *priv = to_ov9640_sensor(sd);
+	u8		pid, ver, midh, midl;
+	const char	*devname;
+	int		ret;
+
+	ret = ov9640_s_power(&priv->subdev, 1);
+	if (ret < 0)
+		return ret;
+
+	/*
+	 * check and show product ID and manufacturer ID
+	 */
+
+	ret = ov9640_reg_read(client, OV9640_PID, &pid);
+	if (!ret)
+		ret = ov9640_reg_read(client, OV9640_VER, &ver);
+	if (!ret)
+		ret = ov9640_reg_read(client, OV9640_MIDH, &midh);
+	if (!ret)
+		ret = ov9640_reg_read(client, OV9640_MIDL, &midl);
+	if (ret)
+		goto done;
+
+	switch (VERSION(pid, ver)) {
+	case OV9640_V2:
+		devname		= "ov9640";
+		priv->revision	= 2;
+		break;
+	case OV9640_V3:
+		devname		= "ov9640";
+		priv->revision	= 3;
+		break;
+	default:
+		dev_err(&client->dev, "Product ID error %x:%x\n", pid, ver);
+		ret = -ENODEV;
+		goto done;
+	}
+
+	dev_info(&client->dev, "%s Product ID %0x:%0x Manufacturer ID %x:%x\n",
+		 devname, pid, ver, midh, midl);
+
+	ret = v4l2_ctrl_handler_setup(&priv->hdl);
+
+done:
+	ov9640_s_power(&priv->subdev, 0);
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ov9640_ctrl_ops = {
+	.s_ctrl = ov9640_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops ov9640_core_ops = {
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register		= ov9640_get_register,
+	.s_register		= ov9640_set_register,
+#endif
+	.s_power		= ov9640_s_power,
+};
+
+/* Request bus settings on camera side */
+static int ov9640_get_mbus_config(struct v4l2_subdev *sd,
+				  unsigned int pad,
+				  struct v4l2_mbus_config *cfg)
+{
+	cfg->type = V4L2_MBUS_PARALLEL;
+	cfg->bus.parallel.flags = V4L2_MBUS_PCLK_SAMPLE_RISING |
+				  V4L2_MBUS_MASTER |
+				  V4L2_MBUS_VSYNC_ACTIVE_HIGH |
+				  V4L2_MBUS_HSYNC_ACTIVE_HIGH |
+				  V4L2_MBUS_DATA_ACTIVE_HIGH;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops ov9640_video_ops = {
+	.s_stream	= ov9640_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops ov9640_pad_ops = {
+	.enum_mbus_code = ov9640_enum_mbus_code,
+	.get_selection	= ov9640_get_selection,
+	.set_fmt	= ov9640_set_fmt,
+	.get_mbus_config = ov9640_get_mbus_config,
+};
+
+static const struct v4l2_subdev_ops ov9640_subdev_ops = {
+	.core	= &ov9640_core_ops,
+	.video	= &ov9640_video_ops,
+	.pad	= &ov9640_pad_ops,
+};
+
+/*
+ * i2c_driver function
+ */
+static int ov9640_probe(struct i2c_client *client)
+{
+	struct ov9640_priv *priv;
+	int ret;
+
+	priv = devm_kzalloc(&client->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->gpio_power = devm_gpiod_get(&client->dev, "Camera power",
+					  GPIOD_OUT_LOW);
+	if (IS_ERR(priv->gpio_power)) {
+		ret = PTR_ERR(priv->gpio_power);
+		return ret;
+	}
+
+	priv->gpio_reset = devm_gpiod_get(&client->dev, "Camera reset",
+					  GPIOD_OUT_HIGH);
+	if (IS_ERR(priv->gpio_reset)) {
+		ret = PTR_ERR(priv->gpio_reset);
+		return ret;
+	}
+
+	v4l2_i2c_subdev_init(&priv->subdev, client, &ov9640_subdev_ops);
+
+	v4l2_ctrl_handler_init(&priv->hdl, 2);
+	v4l2_ctrl_new_std(&priv->hdl, &ov9640_ctrl_ops,
+			  V4L2_CID_VFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&priv->hdl, &ov9640_ctrl_ops,
+			  V4L2_CID_HFLIP, 0, 1, 1, 0);
+
+	if (priv->hdl.error) {
+		ret = priv->hdl.error;
+		goto ectrlinit;
+	}
+
+	priv->subdev.ctrl_handler = &priv->hdl;
+
+	priv->clk = devm_clk_get(&client->dev, "mclk");
+	if (IS_ERR(priv->clk)) {
+		ret = PTR_ERR(priv->clk);
+		goto ectrlinit;
+	}
+
+	ret = ov9640_video_probe(client);
+	if (ret)
+		goto ectrlinit;
+
+	priv->subdev.dev = &client->dev;
+	ret = v4l2_async_register_subdev(&priv->subdev);
+	if (ret)
+		goto ectrlinit;
+
+	return 0;
+
+ectrlinit:
+	v4l2_ctrl_handler_free(&priv->hdl);
+
+	return ret;
+}
+
+static void ov9640_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov9640_priv *priv = to_ov9640_sensor(sd);
+
+	v4l2_async_unregister_subdev(&priv->subdev);
+	v4l2_ctrl_handler_free(&priv->hdl);
+}
+
+static const struct i2c_device_id ov9640_id[] = {
+	{ "ov9640", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ov9640_id);
+
+static struct i2c_driver ov9640_i2c_driver = {
+	.driver = {
+		.name = "ov9640",
+	},
+	.probe    = ov9640_probe,
+	.remove   = ov9640_remove,
+	.id_table = ov9640_id,
+};
+
+module_i2c_driver(ov9640_i2c_driver);
+
+MODULE_DESCRIPTION("OmniVision OV96xx CMOS Image Sensor driver");
+MODULE_AUTHOR("Marek Vasut <marek.vasut@gmail.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/ov9640.h b/drivers/media/i2c/ov9640.h
new file mode 100644
index 000000000..c105594b2
--- /dev/null
+++ b/drivers/media/i2c/ov9640.h
@@ -0,0 +1,207 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * OmniVision OV96xx Camera Header File
+ *
+ * Copyright (C) 2009 Marek Vasut <marek.vasut@gmail.com>
+ */
+
+#ifndef	__DRIVERS_MEDIA_VIDEO_OV9640_H__
+#define	__DRIVERS_MEDIA_VIDEO_OV9640_H__
+
+/* Register definitions */
+#define	OV9640_GAIN	0x00
+#define	OV9640_BLUE	0x01
+#define	OV9640_RED	0x02
+#define	OV9640_VFER	0x03
+#define	OV9640_COM1	0x04
+#define	OV9640_BAVE	0x05
+#define	OV9640_GEAVE	0x06
+#define	OV9640_RSID	0x07
+#define	OV9640_RAVE	0x08
+#define	OV9640_COM2	0x09
+#define	OV9640_PID	0x0a
+#define	OV9640_VER	0x0b
+#define	OV9640_COM3	0x0c
+#define	OV9640_COM4	0x0d
+#define	OV9640_COM5	0x0e
+#define	OV9640_COM6	0x0f
+#define	OV9640_AECH	0x10
+#define	OV9640_CLKRC	0x11
+#define	OV9640_COM7	0x12
+#define	OV9640_COM8	0x13
+#define	OV9640_COM9	0x14
+#define	OV9640_COM10	0x15
+/* 0x16 - RESERVED */
+#define	OV9640_HSTART	0x17
+#define	OV9640_HSTOP	0x18
+#define	OV9640_VSTART	0x19
+#define	OV9640_VSTOP	0x1a
+#define	OV9640_PSHFT	0x1b
+#define	OV9640_MIDH	0x1c
+#define	OV9640_MIDL	0x1d
+#define	OV9640_MVFP	0x1e
+#define	OV9640_LAEC	0x1f
+#define	OV9640_BOS	0x20
+#define	OV9640_GBOS	0x21
+#define	OV9640_GROS	0x22
+#define	OV9640_ROS	0x23
+#define	OV9640_AEW	0x24
+#define	OV9640_AEB	0x25
+#define	OV9640_VPT	0x26
+#define	OV9640_BBIAS	0x27
+#define	OV9640_GBBIAS	0x28
+/* 0x29 - RESERVED */
+#define	OV9640_EXHCH	0x2a
+#define	OV9640_EXHCL	0x2b
+#define	OV9640_RBIAS	0x2c
+#define	OV9640_ADVFL	0x2d
+#define	OV9640_ADVFH	0x2e
+#define	OV9640_YAVE	0x2f
+#define	OV9640_HSYST	0x30
+#define	OV9640_HSYEN	0x31
+#define	OV9640_HREF	0x32
+#define	OV9640_CHLF	0x33
+#define	OV9640_ARBLM	0x34
+/* 0x35..0x36 - RESERVED */
+#define	OV9640_ADC	0x37
+#define	OV9640_ACOM	0x38
+#define	OV9640_OFON	0x39
+#define	OV9640_TSLB	0x3a
+#define	OV9640_COM11	0x3b
+#define	OV9640_COM12	0x3c
+#define	OV9640_COM13	0x3d
+#define	OV9640_COM14	0x3e
+#define	OV9640_EDGE	0x3f
+#define	OV9640_COM15	0x40
+#define	OV9640_COM16	0x41
+#define	OV9640_COM17	0x42
+/* 0x43..0x4e - RESERVED */
+#define	OV9640_MTX1	0x4f
+#define	OV9640_MTX2	0x50
+#define	OV9640_MTX3	0x51
+#define	OV9640_MTX4	0x52
+#define	OV9640_MTX5	0x53
+#define	OV9640_MTX6	0x54
+#define	OV9640_MTX7	0x55
+#define	OV9640_MTX8	0x56
+#define	OV9640_MTX9	0x57
+#define	OV9640_MTXS	0x58
+/* 0x59..0x61 - RESERVED */
+#define	OV9640_LCC1	0x62
+#define	OV9640_LCC2	0x63
+#define	OV9640_LCC3	0x64
+#define	OV9640_LCC4	0x65
+#define	OV9640_LCC5	0x66
+#define	OV9640_MANU	0x67
+#define	OV9640_MANV	0x68
+#define	OV9640_HV	0x69
+#define	OV9640_MBD	0x6a
+#define	OV9640_DBLV	0x6b
+#define	OV9640_GSP	0x6c	/* ... till 0x7b */
+#define	OV9640_GST	0x7c	/* ... till 0x8a */
+
+#define	OV9640_CLKRC_DPLL_EN	0x80
+#define	OV9640_CLKRC_DIRECT	0x40
+#define	OV9640_CLKRC_DIV(x)	((x) & 0x3f)
+
+#define	OV9640_PSHFT_VAL(x)	((x) & 0xff)
+
+#define	OV9640_ACOM_2X_ANALOG	0x80
+#define	OV9640_ACOM_RSVD	0x12
+
+#define	OV9640_MVFP_V		0x10
+#define	OV9640_MVFP_H		0x20
+
+#define	OV9640_COM1_HREF_NOSKIP	0x00
+#define	OV9640_COM1_HREF_2SKIP	0x04
+#define	OV9640_COM1_HREF_3SKIP	0x08
+#define	OV9640_COM1_QQFMT	0x20
+
+#define	OV9640_COM2_SSM		0x10
+
+#define	OV9640_COM3_VP		0x04
+
+#define	OV9640_COM4_QQ_VP	0x80
+#define	OV9640_COM4_RSVD	0x40
+
+#define	OV9640_COM5_SYSCLK	0x80
+#define	OV9640_COM5_LONGEXP	0x01
+
+#define	OV9640_COM6_OPT_BLC	0x40
+#define	OV9640_COM6_ADBLC_BIAS	0x08
+#define	OV9640_COM6_FMT_RST	0x82
+#define	OV9640_COM6_ADBLC_OPTEN	0x01
+
+#define	OV9640_COM7_RAW_RGB	0x01
+#define	OV9640_COM7_RGB		0x04
+#define	OV9640_COM7_QCIF	0x08
+#define	OV9640_COM7_QVGA	0x10
+#define	OV9640_COM7_CIF		0x20
+#define	OV9640_COM7_VGA		0x40
+#define	OV9640_COM7_SCCB_RESET	0x80
+
+#define	OV9640_TSLB_YVYU_YUYV	0x04
+#define	OV9640_TSLB_YUYV_UYVY	0x08
+
+#define	OV9640_COM12_YUV_AVG	0x04
+#define	OV9640_COM12_RSVD	0x40
+
+#define	OV9640_COM13_GAMMA_NONE	0x00
+#define	OV9640_COM13_GAMMA_Y	0x40
+#define	OV9640_COM13_GAMMA_RAW	0x80
+#define	OV9640_COM13_RGB_AVG	0x20
+#define	OV9640_COM13_MATRIX_EN	0x10
+#define	OV9640_COM13_Y_DELAY_EN	0x08
+#define	OV9640_COM13_YUV_DLY(x)	((x) & 0x07)
+
+#define	OV9640_COM15_OR_00FF	0x00
+#define	OV9640_COM15_OR_01FE	0x40
+#define	OV9640_COM15_OR_10F0	0xc0
+#define	OV9640_COM15_RGB_NORM	0x00
+#define	OV9640_COM15_RGB_565	0x10
+#define	OV9640_COM15_RGB_555	0x30
+
+#define	OV9640_COM16_RB_AVG	0x01
+
+/* IDs */
+#define	OV9640_V2		0x9648
+#define	OV9640_V3		0x9649
+#define	VERSION(pid, ver)	(((pid) << 8) | ((ver) & 0xFF))
+
+/* supported resolutions */
+enum {
+	W_QQCIF	= 88,
+	W_QQVGA	= 160,
+	W_QCIF	= 176,
+	W_QVGA	= 320,
+	W_CIF	= 352,
+	W_VGA	= 640,
+	W_SXGA	= 1280
+};
+#define	H_SXGA	960
+
+/* Misc. structures */
+struct ov9640_reg_alt {
+	u8	com7;
+	u8	com12;
+	u8	com13;
+	u8	com15;
+};
+
+struct ov9640_reg {
+	u8	reg;
+	u8	val;
+};
+
+struct ov9640_priv {
+	struct v4l2_subdev		subdev;
+	struct v4l2_ctrl_handler	hdl;
+	struct clk			*clk;
+	struct gpio_desc		*gpio_power;
+	struct gpio_desc		*gpio_reset;
+
+	int				model;
+	int				revision;
+};
+
+#endif	/* __DRIVERS_MEDIA_VIDEO_OV9640_H__ */
diff --git a/drivers/media/i2c/ov9650.c b/drivers/media/i2c/ov9650.c
new file mode 100644
index 000000000..da1ab5135
--- /dev/null
+++ b/drivers/media/i2c/ov9650.c
@@ -0,0 +1,1583 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Omnivision OV9650/OV9652 CMOS Image Sensor driver
+ *
+ * Copyright (C) 2013, Sylwester Nawrocki <sylvester.nawrocki@gmail.com>
+ *
+ * Register definitions and initial settings based on a driver written
+ * by Vladimir Fonov.
+ * Copyright (c) 2010, Vladimir Fonov
+ */
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/kernel.h>
+#include <linux/media.h>
+#include <linux/module.h>
+#include <linux/ratelimit.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+#include <linux/videodev2.h>
+
+#include <media/media-entity.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-image-sizes.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-mediabus.h>
+
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "Debug level (0-2)");
+
+#define DRIVER_NAME "OV9650"
+
+/*
+ * OV9650/OV9652 register definitions
+ */
+#define REG_GAIN		0x00	/* Gain control, AGC[7:0] */
+#define REG_BLUE		0x01	/* AWB - Blue channel gain */
+#define REG_RED			0x02	/* AWB - Red channel gain */
+#define REG_VREF		0x03	/* [7:6] - AGC[9:8], [5:3]/[2:0] */
+#define  VREF_GAIN_MASK		0xc0	/* - VREF end/start low 3 bits */
+#define REG_COM1		0x04
+#define  COM1_CCIR656		0x40
+#define REG_B_AVE		0x05
+#define REG_GB_AVE		0x06
+#define REG_GR_AVE		0x07
+#define REG_R_AVE		0x08
+#define REG_COM2		0x09
+#define REG_PID			0x0a	/* Product ID MSB */
+#define REG_VER			0x0b	/* Product ID LSB */
+#define REG_COM3		0x0c
+#define  COM3_SWAP		0x40
+#define  COM3_VARIOPIXEL1	0x04
+#define REG_COM4		0x0d	/* Vario Pixels  */
+#define  COM4_VARIOPIXEL2	0x80
+#define REG_COM5		0x0e	/* System clock options */
+#define  COM5_SLAVE_MODE	0x10
+#define  COM5_SYSTEMCLOCK48MHZ	0x80
+#define REG_COM6		0x0f	/* HREF & ADBLC options */
+#define REG_AECH		0x10	/* Exposure value, AEC[9:2] */
+#define REG_CLKRC		0x11	/* Clock control */
+#define  CLK_EXT		0x40	/* Use external clock directly */
+#define  CLK_SCALE		0x3f	/* Mask for internal clock scale */
+#define REG_COM7		0x12	/* SCCB reset, output format */
+#define  COM7_RESET		0x80
+#define  COM7_FMT_MASK		0x38
+#define  COM7_FMT_VGA		0x40
+#define	 COM7_FMT_CIF		0x20
+#define  COM7_FMT_QVGA		0x10
+#define  COM7_FMT_QCIF		0x08
+#define	 COM7_RGB		0x04
+#define	 COM7_YUV		0x00
+#define	 COM7_BAYER		0x01
+#define	 COM7_PBAYER		0x05
+#define REG_COM8		0x13	/* AGC/AEC options */
+#define  COM8_FASTAEC		0x80	/* Enable fast AGC/AEC */
+#define  COM8_AECSTEP		0x40	/* Unlimited AEC step size */
+#define  COM8_BFILT		0x20	/* Band filter enable */
+#define  COM8_AGC		0x04	/* Auto gain enable */
+#define  COM8_AWB		0x02	/* White balance enable */
+#define  COM8_AEC		0x01	/* Auto exposure enable */
+#define REG_COM9		0x14	/* Gain ceiling */
+#define  COM9_GAIN_CEIL_MASK	0x70	/* */
+#define REG_COM10		0x15	/* PCLK, HREF, HSYNC signals polarity */
+#define  COM10_HSYNC		0x40	/* HSYNC instead of HREF */
+#define  COM10_PCLK_HB		0x20	/* Suppress PCLK on horiz blank */
+#define  COM10_HREF_REV		0x08	/* Reverse HREF */
+#define  COM10_VS_LEAD		0x04	/* VSYNC on clock leading edge */
+#define  COM10_VS_NEG		0x02	/* VSYNC negative */
+#define  COM10_HS_NEG		0x01	/* HSYNC negative */
+#define REG_HSTART		0x17	/* Horiz start high bits */
+#define REG_HSTOP		0x18	/* Horiz stop high bits */
+#define REG_VSTART		0x19	/* Vert start high bits */
+#define REG_VSTOP		0x1a	/* Vert stop high bits */
+#define REG_PSHFT		0x1b	/* Pixel delay after HREF */
+#define REG_MIDH		0x1c	/* Manufacturer ID MSB */
+#define REG_MIDL		0x1d	/* Manufufacturer ID LSB */
+#define REG_MVFP		0x1e	/* Image mirror/flip */
+#define  MVFP_MIRROR		0x20	/* Mirror image */
+#define  MVFP_FLIP		0x10	/* Vertical flip */
+#define REG_BOS			0x20	/* B channel Offset */
+#define REG_GBOS		0x21	/* Gb channel Offset */
+#define REG_GROS		0x22	/* Gr channel Offset */
+#define REG_ROS			0x23	/* R channel Offset */
+#define REG_AEW			0x24	/* AGC upper limit */
+#define REG_AEB			0x25	/* AGC lower limit */
+#define REG_VPT			0x26	/* AGC/AEC fast mode op region */
+#define REG_BBIAS		0x27	/* B channel output bias */
+#define REG_GBBIAS		0x28	/* Gb channel output bias */
+#define REG_GRCOM		0x29	/* Analog BLC & regulator */
+#define REG_EXHCH		0x2a	/* Dummy pixel insert MSB */
+#define REG_EXHCL		0x2b	/* Dummy pixel insert LSB */
+#define REG_RBIAS		0x2c	/* R channel output bias */
+#define REG_ADVFL		0x2d	/* LSB of dummy line insert */
+#define REG_ADVFH		0x2e	/* MSB of dummy line insert */
+#define REG_YAVE		0x2f	/* Y/G channel average value */
+#define REG_HSYST		0x30	/* HSYNC rising edge delay LSB*/
+#define REG_HSYEN		0x31	/* HSYNC falling edge delay LSB*/
+#define REG_HREF		0x32	/* HREF pieces */
+#define REG_CHLF		0x33	/* reserved */
+#define REG_ADC			0x37	/* reserved */
+#define REG_ACOM		0x38	/* reserved */
+#define REG_OFON		0x39	/* Power down register */
+#define  OFON_PWRDN		0x08	/* Power down bit */
+#define REG_TSLB		0x3a	/* YUVU format */
+#define  TSLB_YUYV_MASK		0x0c	/* UYVY or VYUY - see com13 */
+#define REG_COM11		0x3b	/* Night mode, banding filter enable */
+#define  COM11_NIGHT		0x80	/* Night mode enable */
+#define  COM11_NMFR		0x60	/* Two bit NM frame rate */
+#define  COM11_BANDING		0x01	/* Banding filter */
+#define  COM11_AEC_REF_MASK	0x18	/* AEC reference area selection */
+#define REG_COM12		0x3c	/* HREF option, UV average */
+#define  COM12_HREF		0x80	/* HREF always */
+#define REG_COM13		0x3d	/* Gamma selection, Color matrix en. */
+#define  COM13_GAMMA		0x80	/* Gamma enable */
+#define	 COM13_UVSAT		0x40	/* UV saturation auto adjustment */
+#define  COM13_UVSWAP		0x01	/* V before U - w/TSLB */
+#define REG_COM14		0x3e	/* Edge enhancement options */
+#define  COM14_EDGE_EN		0x02
+#define  COM14_EEF_X2		0x01
+#define REG_EDGE		0x3f	/* Edge enhancement factor */
+#define  EDGE_FACTOR_MASK	0x0f
+#define REG_COM15		0x40	/* Output range, RGB 555/565 */
+#define  COM15_R10F0		0x00	/* Data range 10 to F0 */
+#define	 COM15_R01FE		0x80	/* 01 to FE */
+#define  COM15_R00FF		0xc0	/* 00 to FF */
+#define  COM15_RGB565		0x10	/* RGB565 output */
+#define  COM15_RGB555		0x30	/* RGB555 output */
+#define  COM15_SWAPRB		0x04	/* Swap R&B */
+#define REG_COM16		0x41	/* Color matrix coeff options */
+#define REG_COM17		0x42	/* Single frame out, banding filter */
+/* n = 1...9, 0x4f..0x57 */
+#define	REG_MTX(__n)		(0x4f + (__n) - 1)
+#define REG_MTXS		0x58
+/* Lens Correction Option 1...5, __n = 0...5 */
+#define REG_LCC(__n)		(0x62 + (__n) - 1)
+#define  LCC5_LCC_ENABLE	0x01	/* LCC5, enable lens correction */
+#define  LCC5_LCC_COLOR		0x04
+#define REG_MANU		0x67	/* Manual U value */
+#define REG_MANV		0x68	/* Manual V value */
+#define REG_HV			0x69	/* Manual banding filter MSB */
+#define REG_MBD			0x6a	/* Manual banding filter value */
+#define REG_DBLV		0x6b	/* reserved */
+#define REG_GSP			0x6c	/* Gamma curve */
+#define  GSP_LEN		15
+#define REG_GST			0x7c	/* Gamma curve */
+#define  GST_LEN		15
+#define REG_COM21		0x8b
+#define REG_COM22		0x8c	/* Edge enhancement, denoising */
+#define  COM22_WHTPCOR		0x02	/* White pixel correction enable */
+#define  COM22_WHTPCOROPT	0x01	/* White pixel correction option */
+#define  COM22_DENOISE		0x10	/* White pixel correction option */
+#define REG_COM23		0x8d	/* Color bar test, color gain */
+#define  COM23_TEST_MODE	0x10
+#define REG_DBLC1		0x8f	/* Digital BLC */
+#define REG_DBLC_B		0x90	/* Digital BLC B channel offset */
+#define REG_DBLC_R		0x91	/* Digital BLC R channel offset */
+#define REG_DM_LNL		0x92	/* Dummy line low 8 bits */
+#define REG_DM_LNH		0x93	/* Dummy line high 8 bits */
+#define REG_LCCFB		0x9d	/* Lens Correction B channel */
+#define REG_LCCFR		0x9e	/* Lens Correction R channel */
+#define REG_DBLC_GB		0x9f	/* Digital BLC GB chan offset */
+#define REG_DBLC_GR		0xa0	/* Digital BLC GR chan offset */
+#define REG_AECHM		0xa1	/* Exposure value - bits AEC[15:10] */
+#define REG_BD50ST		0xa2	/* Banding filter value for 50Hz */
+#define REG_BD60ST		0xa3	/* Banding filter value for 60Hz */
+#define REG_NULL		0xff	/* Array end token */
+
+#define DEF_CLKRC		0x80
+
+#define OV965X_ID(_msb, _lsb)	((_msb) << 8 | (_lsb))
+#define OV9650_ID		0x9650
+#define OV9652_ID		0x9652
+
+struct ov965x_ctrls {
+	struct v4l2_ctrl_handler handler;
+	struct {
+		struct v4l2_ctrl *auto_exp;
+		struct v4l2_ctrl *exposure;
+	};
+	struct {
+		struct v4l2_ctrl *auto_wb;
+		struct v4l2_ctrl *blue_balance;
+		struct v4l2_ctrl *red_balance;
+	};
+	struct {
+		struct v4l2_ctrl *hflip;
+		struct v4l2_ctrl *vflip;
+	};
+	struct {
+		struct v4l2_ctrl *auto_gain;
+		struct v4l2_ctrl *gain;
+	};
+	struct v4l2_ctrl *brightness;
+	struct v4l2_ctrl *saturation;
+	struct v4l2_ctrl *sharpness;
+	struct v4l2_ctrl *light_freq;
+	u8 update;
+};
+
+struct ov965x_framesize {
+	u16 width;
+	u16 height;
+	u16 max_exp_lines;
+	const u8 *regs;
+};
+
+struct ov965x_interval {
+	struct v4l2_fract interval;
+	/* Maximum resolution for this interval */
+	struct v4l2_frmsize_discrete size;
+	u8 clkrc_div;
+};
+
+enum gpio_id {
+	GPIO_PWDN,
+	GPIO_RST,
+	NUM_GPIOS,
+};
+
+struct ov965x {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	enum v4l2_mbus_type bus_type;
+	struct gpio_desc *gpios[NUM_GPIOS];
+	/* External master clock frequency */
+	unsigned long mclk_frequency;
+	struct clk *clk;
+
+	/* Protects the struct fields below */
+	struct mutex lock;
+
+	struct regmap *regmap;
+
+	/* Exposure row interval in us */
+	unsigned int exp_row_interval;
+
+	unsigned short id;
+	const struct ov965x_framesize *frame_size;
+	/* YUYV sequence (pixel format) control register */
+	u8 tslb_reg;
+	struct v4l2_mbus_framefmt format;
+
+	struct ov965x_ctrls ctrls;
+	/* Pointer to frame rate control data structure */
+	const struct ov965x_interval *fiv;
+
+	int streaming;
+	int power;
+
+	u8 apply_frame_fmt;
+};
+
+struct i2c_rv {
+	u8 addr;
+	u8 value;
+};
+
+static const struct i2c_rv ov965x_init_regs[] = {
+	{ REG_COM2, 0x10 },	/* Set soft sleep mode */
+	{ REG_COM5, 0x00 },	/* System clock options */
+	{ REG_COM2, 0x01 },	/* Output drive, soft sleep mode */
+	{ REG_COM10, 0x00 },	/* Slave mode, HREF vs HSYNC, signals negate */
+	{ REG_EDGE, 0xa6 },	/* Edge enhancement treshhold and factor */
+	{ REG_COM16, 0x02 },	/* Color matrix coeff double option */
+	{ REG_COM17, 0x08 },	/* Single frame out, banding filter */
+	{ 0x16, 0x06 },
+	{ REG_CHLF, 0xc0 },	/* Reserved  */
+	{ 0x34, 0xbf },
+	{ 0xa8, 0x80 },
+	{ 0x96, 0x04 },
+	{ 0x8e, 0x00 },
+	{ REG_COM12, 0x77 },	/* HREF option, UV average  */
+	{ 0x8b, 0x06 },
+	{ 0x35, 0x91 },
+	{ 0x94, 0x88 },
+	{ 0x95, 0x88 },
+	{ REG_COM15, 0xc1 },	/* Output range, RGB 555/565 */
+	{ REG_GRCOM, 0x2f },	/* Analog BLC & regulator */
+	{ REG_COM6, 0x43 },	/* HREF & ADBLC options */
+	{ REG_COM8, 0xe5 },	/* AGC/AEC options */
+	{ REG_COM13, 0x90 },	/* Gamma selection, colour matrix, UV delay */
+	{ REG_HV, 0x80 },	/* Manual banding filter MSB  */
+	{ 0x5c, 0x96 },		/* Reserved up to 0xa5 */
+	{ 0x5d, 0x96 },
+	{ 0x5e, 0x10 },
+	{ 0x59, 0xeb },
+	{ 0x5a, 0x9c },
+	{ 0x5b, 0x55 },
+	{ 0x43, 0xf0 },
+	{ 0x44, 0x10 },
+	{ 0x45, 0x55 },
+	{ 0x46, 0x86 },
+	{ 0x47, 0x64 },
+	{ 0x48, 0x86 },
+	{ 0x5f, 0xe0 },
+	{ 0x60, 0x8c },
+	{ 0x61, 0x20 },
+	{ 0xa5, 0xd9 },
+	{ 0xa4, 0x74 },		/* reserved */
+	{ REG_COM23, 0x02 },	/* Color gain analog/_digital_ */
+	{ REG_COM8, 0xe7 },	/* Enable AEC, AWB, AEC */
+	{ REG_COM22, 0x23 },	/* Edge enhancement, denoising */
+	{ 0xa9, 0xb8 },
+	{ 0xaa, 0x92 },
+	{ 0xab, 0x0a },
+	{ REG_DBLC1, 0xdf },	/* Digital BLC */
+	{ REG_DBLC_B, 0x00 },	/* Digital BLC B chan offset */
+	{ REG_DBLC_R, 0x00 },	/* Digital BLC R chan offset */
+	{ REG_DBLC_GB, 0x00 },	/* Digital BLC GB chan offset */
+	{ REG_DBLC_GR, 0x00 },
+	{ REG_COM9, 0x3a },	/* Gain ceiling 16x */
+	{ REG_NULL, 0 }
+};
+
+#define NUM_FMT_REGS 14
+/*
+ * COM7,  COM3,  COM4, HSTART, HSTOP, HREF, VSTART, VSTOP, VREF,
+ * EXHCH, EXHCL, ADC,  OCOM,   OFON
+ */
+static const u8 frame_size_reg_addr[NUM_FMT_REGS] = {
+	0x12, 0x0c, 0x0d, 0x17, 0x18, 0x32, 0x19, 0x1a, 0x03,
+	0x2a, 0x2b, 0x37, 0x38, 0x39,
+};
+
+static const u8 ov965x_sxga_regs[NUM_FMT_REGS] = {
+	0x00, 0x00, 0x00, 0x1e, 0xbe, 0xbf, 0x01, 0x81, 0x12,
+	0x10, 0x34, 0x81, 0x93, 0x51,
+};
+
+static const u8 ov965x_vga_regs[NUM_FMT_REGS] = {
+	0x40, 0x04, 0x80, 0x26, 0xc6, 0xed, 0x01, 0x3d, 0x00,
+	0x10, 0x40, 0x91, 0x12, 0x43,
+};
+
+/* Determined empirically. */
+static const u8 ov965x_qvga_regs[NUM_FMT_REGS] = {
+	0x10, 0x04, 0x80, 0x25, 0xc5, 0xbf, 0x00, 0x80, 0x12,
+	0x10, 0x40, 0x91, 0x12, 0x43,
+};
+
+static const struct ov965x_framesize ov965x_framesizes[] = {
+	{
+		.width		= SXGA_WIDTH,
+		.height		= SXGA_HEIGHT,
+		.regs		= ov965x_sxga_regs,
+		.max_exp_lines	= 1048,
+	}, {
+		.width		= VGA_WIDTH,
+		.height		= VGA_HEIGHT,
+		.regs		= ov965x_vga_regs,
+		.max_exp_lines	= 498,
+	}, {
+		.width		= QVGA_WIDTH,
+		.height		= QVGA_HEIGHT,
+		.regs		= ov965x_qvga_regs,
+		.max_exp_lines	= 248,
+	},
+};
+
+struct ov965x_pixfmt {
+	u32 code;
+	u32 colorspace;
+	/* REG_TSLB value, only bits [3:2] may be set. */
+	u8 tslb_reg;
+};
+
+static const struct ov965x_pixfmt ov965x_formats[] = {
+	{ MEDIA_BUS_FMT_YUYV8_2X8, V4L2_COLORSPACE_JPEG, 0x00},
+	{ MEDIA_BUS_FMT_YVYU8_2X8, V4L2_COLORSPACE_JPEG, 0x04},
+	{ MEDIA_BUS_FMT_UYVY8_2X8, V4L2_COLORSPACE_JPEG, 0x0c},
+	{ MEDIA_BUS_FMT_VYUY8_2X8, V4L2_COLORSPACE_JPEG, 0x08},
+};
+
+/*
+ * This table specifies possible frame resolution and interval
+ * combinations. Default CLKRC[5:0] divider values are valid
+ * only for 24 MHz external clock frequency.
+ */
+static struct ov965x_interval ov965x_intervals[] = {
+	{{ 100, 625 }, { SXGA_WIDTH, SXGA_HEIGHT }, 0 },  /* 6.25 fps */
+	{{ 10,  125 }, { VGA_WIDTH, VGA_HEIGHT },   1 },  /* 12.5 fps */
+	{{ 10,  125 }, { QVGA_WIDTH, QVGA_HEIGHT }, 3 },  /* 12.5 fps */
+	{{ 1,   25  }, { VGA_WIDTH, VGA_HEIGHT },   0 },  /* 25 fps */
+	{{ 1,   25  }, { QVGA_WIDTH, QVGA_HEIGHT }, 1 },  /* 25 fps */
+};
+
+static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct ov965x, ctrls.handler)->sd;
+}
+
+static inline struct ov965x *to_ov965x(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct ov965x, sd);
+}
+
+static int ov965x_read(struct ov965x *ov965x, u8 addr, u8 *val)
+{
+	int ret;
+	unsigned int buf;
+
+	ret = regmap_read(ov965x->regmap, addr, &buf);
+	if (!ret)
+		*val = buf;
+	else
+		*val = -1;
+
+	v4l2_dbg(2, debug, &ov965x->sd, "%s: 0x%02x @ 0x%02x. (%d)\n",
+		 __func__, *val, addr, ret);
+
+	return ret;
+}
+
+static int ov965x_write(struct ov965x *ov965x, u8 addr, u8 val)
+{
+	int ret;
+
+	ret = regmap_write(ov965x->regmap, addr, val);
+
+	v4l2_dbg(2, debug, &ov965x->sd, "%s: 0x%02x @ 0x%02X (%d)\n",
+		 __func__, val, addr, ret);
+
+	return ret;
+}
+
+static int ov965x_write_array(struct ov965x *ov965x,
+			      const struct i2c_rv *regs)
+{
+	int i, ret = 0;
+
+	for (i = 0; ret == 0 && regs[i].addr != REG_NULL; i++)
+		ret = ov965x_write(ov965x, regs[i].addr, regs[i].value);
+
+	return ret;
+}
+
+static int ov965x_set_default_gamma_curve(struct ov965x *ov965x)
+{
+	static const u8 gamma_curve[] = {
+		/* Values taken from OV application note. */
+		0x40, 0x30, 0x4b, 0x60, 0x70, 0x70, 0x70, 0x70,
+		0x60, 0x60, 0x50, 0x48, 0x3a, 0x2e, 0x28, 0x22,
+		0x04, 0x07, 0x10, 0x28,	0x36, 0x44, 0x52, 0x60,
+		0x6c, 0x78, 0x8c, 0x9e, 0xbb, 0xd2, 0xe6
+	};
+	u8 addr = REG_GSP;
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(gamma_curve); i++) {
+		int ret = ov965x_write(ov965x, addr, gamma_curve[i]);
+
+		if (ret < 0)
+			return ret;
+		addr++;
+	}
+
+	return 0;
+};
+
+static int ov965x_set_color_matrix(struct ov965x *ov965x)
+{
+	static const u8 mtx[] = {
+		/* MTX1..MTX9, MTXS */
+		0x3a, 0x3d, 0x03, 0x12, 0x26, 0x38, 0x40, 0x40, 0x40, 0x0d
+	};
+	u8 addr = REG_MTX(1);
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(mtx); i++) {
+		int ret = ov965x_write(ov965x, addr, mtx[i]);
+
+		if (ret < 0)
+			return ret;
+		addr++;
+	}
+
+	return 0;
+}
+
+static int __ov965x_set_power(struct ov965x *ov965x, int on)
+{
+	if (on) {
+		int ret = clk_prepare_enable(ov965x->clk);
+
+		if (ret)
+			return ret;
+
+		gpiod_set_value_cansleep(ov965x->gpios[GPIO_PWDN], 0);
+		gpiod_set_value_cansleep(ov965x->gpios[GPIO_RST], 0);
+		msleep(25);
+	} else {
+		gpiod_set_value_cansleep(ov965x->gpios[GPIO_RST], 1);
+		gpiod_set_value_cansleep(ov965x->gpios[GPIO_PWDN], 1);
+
+		clk_disable_unprepare(ov965x->clk);
+	}
+
+	ov965x->streaming = 0;
+
+	return 0;
+}
+
+static int ov965x_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct ov965x *ov965x = to_ov965x(sd);
+	int ret = 0;
+
+	v4l2_dbg(1, debug, sd, "%s: on: %d\n", __func__, on);
+
+	mutex_lock(&ov965x->lock);
+	if (ov965x->power == !on) {
+		ret = __ov965x_set_power(ov965x, on);
+		if (!ret && on) {
+			ret = ov965x_write_array(ov965x,
+						 ov965x_init_regs);
+			ov965x->apply_frame_fmt = 1;
+			ov965x->ctrls.update = 1;
+		}
+	}
+	if (!ret)
+		ov965x->power += on ? 1 : -1;
+
+	WARN_ON(ov965x->power < 0);
+	mutex_unlock(&ov965x->lock);
+	return ret;
+}
+
+/*
+ * V4L2 controls
+ */
+
+static void ov965x_update_exposure_ctrl(struct ov965x *ov965x)
+{
+	struct v4l2_ctrl *ctrl = ov965x->ctrls.exposure;
+	unsigned long fint, trow;
+	int min, max, def;
+	u8 clkrc;
+
+	mutex_lock(&ov965x->lock);
+	if (WARN_ON(!ctrl || !ov965x->frame_size)) {
+		mutex_unlock(&ov965x->lock);
+		return;
+	}
+	clkrc = DEF_CLKRC + ov965x->fiv->clkrc_div;
+	/* Calculate internal clock frequency */
+	fint = ov965x->mclk_frequency * ((clkrc >> 7) + 1) /
+				((2 * ((clkrc & 0x3f) + 1)));
+	/* and the row interval (in us). */
+	trow = (2 * 1520 * 1000000UL) / fint;
+	max = ov965x->frame_size->max_exp_lines * trow;
+	ov965x->exp_row_interval = trow;
+	mutex_unlock(&ov965x->lock);
+
+	v4l2_dbg(1, debug, &ov965x->sd, "clkrc: %#x, fi: %lu, tr: %lu, %d\n",
+		 clkrc, fint, trow, max);
+
+	/* Update exposure time range to match current frame format. */
+	min = (trow + 100) / 100;
+	max = (max - 100) / 100;
+	def = min + (max - min) / 2;
+
+	if (v4l2_ctrl_modify_range(ctrl, min, max, 1, def))
+		v4l2_err(&ov965x->sd, "Exposure ctrl range update failed\n");
+}
+
+static int ov965x_set_banding_filter(struct ov965x *ov965x, int value)
+{
+	unsigned long mbd, light_freq;
+	int ret;
+	u8 reg;
+
+	ret = ov965x_read(ov965x, REG_COM8, &reg);
+	if (!ret) {
+		if (value == V4L2_CID_POWER_LINE_FREQUENCY_DISABLED)
+			reg &= ~COM8_BFILT;
+		else
+			reg |= COM8_BFILT;
+		ret = ov965x_write(ov965x, REG_COM8, reg);
+	}
+	if (value == V4L2_CID_POWER_LINE_FREQUENCY_DISABLED)
+		return 0;
+	if (WARN_ON(!ov965x->fiv))
+		return -EINVAL;
+	/* Set minimal exposure time for 50/60 HZ lighting */
+	if (value == V4L2_CID_POWER_LINE_FREQUENCY_50HZ)
+		light_freq = 50;
+	else
+		light_freq = 60;
+	mbd = (1000UL * ov965x->fiv->interval.denominator *
+	       ov965x->frame_size->max_exp_lines) /
+	       ov965x->fiv->interval.numerator;
+	mbd = ((mbd / (light_freq * 2)) + 500) / 1000UL;
+
+	return ov965x_write(ov965x, REG_MBD, mbd);
+}
+
+static int ov965x_set_white_balance(struct ov965x *ov965x, int awb)
+{
+	int ret;
+	u8 reg;
+
+	ret = ov965x_read(ov965x, REG_COM8, &reg);
+	if (!ret) {
+		reg = awb ? reg | REG_COM8 : reg & ~REG_COM8;
+		ret = ov965x_write(ov965x, REG_COM8, reg);
+	}
+	if (!ret && !awb) {
+		ret = ov965x_write(ov965x, REG_BLUE,
+				   ov965x->ctrls.blue_balance->val);
+		if (ret < 0)
+			return ret;
+		ret = ov965x_write(ov965x, REG_RED,
+				   ov965x->ctrls.red_balance->val);
+	}
+	return ret;
+}
+
+#define NUM_BR_LEVELS	7
+#define NUM_BR_REGS	3
+
+static int ov965x_set_brightness(struct ov965x *ov965x, int val)
+{
+	static const u8 regs[NUM_BR_LEVELS + 1][NUM_BR_REGS] = {
+		{ REG_AEW, REG_AEB, REG_VPT },
+		{ 0x1c, 0x12, 0x50 }, /* -3 */
+		{ 0x3d, 0x30, 0x71 }, /* -2 */
+		{ 0x50, 0x44, 0x92 }, /* -1 */
+		{ 0x70, 0x64, 0xc3 }, /*  0 */
+		{ 0x90, 0x84, 0xd4 }, /* +1 */
+		{ 0xc4, 0xbf, 0xf9 }, /* +2 */
+		{ 0xd8, 0xd0, 0xfa }, /* +3 */
+	};
+	int i, ret = 0;
+
+	val += (NUM_BR_LEVELS / 2 + 1);
+	if (val > NUM_BR_LEVELS)
+		return -EINVAL;
+
+	for (i = 0; i < NUM_BR_REGS && !ret; i++)
+		ret = ov965x_write(ov965x, regs[0][i],
+				   regs[val][i]);
+	return ret;
+}
+
+static int ov965x_set_gain(struct ov965x *ov965x, int auto_gain)
+{
+	struct ov965x_ctrls *ctrls = &ov965x->ctrls;
+	int ret = 0;
+	u8 reg;
+	/*
+	 * For manual mode we need to disable AGC first, so
+	 * gain value in REG_VREF, REG_GAIN is not overwritten.
+	 */
+	if (ctrls->auto_gain->is_new) {
+		ret = ov965x_read(ov965x, REG_COM8, &reg);
+		if (ret < 0)
+			return ret;
+		if (ctrls->auto_gain->val)
+			reg |= COM8_AGC;
+		else
+			reg &= ~COM8_AGC;
+		ret = ov965x_write(ov965x, REG_COM8, reg);
+		if (ret < 0)
+			return ret;
+	}
+
+	if (ctrls->gain->is_new && !auto_gain) {
+		unsigned int gain = ctrls->gain->val;
+		unsigned int rgain;
+		int m;
+		/*
+		 * Convert gain control value to the sensor's gain
+		 * registers (VREF[7:6], GAIN[7:0]) format.
+		 */
+		for (m = 6; m >= 0; m--)
+			if (gain >= (1 << m) * 16)
+				break;
+
+		/* Sanity check: don't adjust the gain with a negative value */
+		if (m < 0)
+			return -EINVAL;
+
+		rgain = (gain - ((1 << m) * 16)) / (1 << m);
+		rgain |= (((1 << m) - 1) << 4);
+
+		ret = ov965x_write(ov965x, REG_GAIN, rgain & 0xff);
+		if (ret < 0)
+			return ret;
+		ret = ov965x_read(ov965x, REG_VREF, &reg);
+		if (ret < 0)
+			return ret;
+		reg &= ~VREF_GAIN_MASK;
+		reg |= (((rgain >> 8) & 0x3) << 6);
+		ret = ov965x_write(ov965x, REG_VREF, reg);
+		if (ret < 0)
+			return ret;
+		/* Return updated control's value to userspace */
+		ctrls->gain->val = (1 << m) * (16 + (rgain & 0xf));
+	}
+
+	return ret;
+}
+
+static int ov965x_set_sharpness(struct ov965x *ov965x, unsigned int value)
+{
+	u8 com14, edge;
+	int ret;
+
+	ret = ov965x_read(ov965x, REG_COM14, &com14);
+	if (ret < 0)
+		return ret;
+	ret = ov965x_read(ov965x, REG_EDGE, &edge);
+	if (ret < 0)
+		return ret;
+	com14 = value ? com14 | COM14_EDGE_EN : com14 & ~COM14_EDGE_EN;
+	value--;
+	if (value > 0x0f) {
+		com14 |= COM14_EEF_X2;
+		value >>= 1;
+	} else {
+		com14 &= ~COM14_EEF_X2;
+	}
+	ret = ov965x_write(ov965x, REG_COM14, com14);
+	if (ret < 0)
+		return ret;
+
+	edge &= ~EDGE_FACTOR_MASK;
+	edge |= ((u8)value & 0x0f);
+
+	return ov965x_write(ov965x, REG_EDGE, edge);
+}
+
+static int ov965x_set_exposure(struct ov965x *ov965x, int exp)
+{
+	struct ov965x_ctrls *ctrls = &ov965x->ctrls;
+	bool auto_exposure = (exp == V4L2_EXPOSURE_AUTO);
+	int ret;
+	u8 reg;
+
+	if (ctrls->auto_exp->is_new) {
+		ret = ov965x_read(ov965x, REG_COM8, &reg);
+		if (ret < 0)
+			return ret;
+		if (auto_exposure)
+			reg |= (COM8_AEC | COM8_AGC);
+		else
+			reg &= ~(COM8_AEC | COM8_AGC);
+		ret = ov965x_write(ov965x, REG_COM8, reg);
+		if (ret < 0)
+			return ret;
+	}
+
+	if (!auto_exposure && ctrls->exposure->is_new) {
+		unsigned int exposure = (ctrls->exposure->val * 100)
+					 / ov965x->exp_row_interval;
+		/*
+		 * Manual exposure value
+		 * [b15:b0] - AECHM (b15:b10), AECH (b9:b2), COM1 (b1:b0)
+		 */
+		ret = ov965x_write(ov965x, REG_COM1, exposure & 0x3);
+		if (!ret)
+			ret = ov965x_write(ov965x, REG_AECH,
+					   (exposure >> 2) & 0xff);
+		if (!ret)
+			ret = ov965x_write(ov965x, REG_AECHM,
+					   (exposure >> 10) & 0x3f);
+		/* Update the value to minimize rounding errors */
+		ctrls->exposure->val = ((exposure * ov965x->exp_row_interval)
+							+ 50) / 100;
+		if (ret < 0)
+			return ret;
+	}
+
+	v4l2_ctrl_activate(ov965x->ctrls.brightness, !exp);
+	return 0;
+}
+
+static int ov965x_set_flip(struct ov965x *ov965x)
+{
+	u8 mvfp = 0;
+
+	if (ov965x->ctrls.hflip->val)
+		mvfp |= MVFP_MIRROR;
+
+	if (ov965x->ctrls.vflip->val)
+		mvfp |= MVFP_FLIP;
+
+	return ov965x_write(ov965x, REG_MVFP, mvfp);
+}
+
+#define NUM_SAT_LEVELS	5
+#define NUM_SAT_REGS	6
+
+static int ov965x_set_saturation(struct ov965x *ov965x, int val)
+{
+	static const u8 regs[NUM_SAT_LEVELS][NUM_SAT_REGS] = {
+		/* MTX(1)...MTX(6) */
+		{ 0x1d, 0x1f, 0x02, 0x09, 0x13, 0x1c }, /* -2 */
+		{ 0x2e, 0x31, 0x02, 0x0e, 0x1e, 0x2d }, /* -1 */
+		{ 0x3a, 0x3d, 0x03, 0x12, 0x26, 0x38 }, /*  0 */
+		{ 0x46, 0x49, 0x04, 0x16, 0x2e, 0x43 }, /* +1 */
+		{ 0x57, 0x5c, 0x05, 0x1b, 0x39, 0x54 }, /* +2 */
+	};
+	u8 addr = REG_MTX(1);
+	int i, ret = 0;
+
+	val += (NUM_SAT_LEVELS / 2);
+	if (val >= NUM_SAT_LEVELS)
+		return -EINVAL;
+
+	for (i = 0; i < NUM_SAT_REGS && !ret; i++)
+		ret = ov965x_write(ov965x, addr + i, regs[val][i]);
+
+	return ret;
+}
+
+static int ov965x_set_test_pattern(struct ov965x *ov965x, int value)
+{
+	int ret;
+	u8 reg;
+
+	ret = ov965x_read(ov965x, REG_COM23, &reg);
+	if (ret < 0)
+		return ret;
+	reg = value ? reg | COM23_TEST_MODE : reg & ~COM23_TEST_MODE;
+	return ov965x_write(ov965x, REG_COM23, reg);
+}
+
+static int __g_volatile_ctrl(struct ov965x *ov965x, struct v4l2_ctrl *ctrl)
+{
+	unsigned int exposure, gain, m;
+	u8 reg0, reg1, reg2;
+	int ret;
+
+	if (!ov965x->power)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTOGAIN:
+		if (!ctrl->val)
+			return 0;
+		ret = ov965x_read(ov965x, REG_GAIN, &reg0);
+		if (ret < 0)
+			return ret;
+		ret = ov965x_read(ov965x, REG_VREF, &reg1);
+		if (ret < 0)
+			return ret;
+		gain = ((reg1 >> 6) << 8) | reg0;
+		m = 0x01 << fls(gain >> 4);
+		ov965x->ctrls.gain->val = m * (16 + (gain & 0xf));
+		break;
+
+	case V4L2_CID_EXPOSURE_AUTO:
+		if (ctrl->val == V4L2_EXPOSURE_MANUAL)
+			return 0;
+		ret = ov965x_read(ov965x, REG_COM1, &reg0);
+		if (ret < 0)
+			return ret;
+		ret = ov965x_read(ov965x, REG_AECH, &reg1);
+		if (ret < 0)
+			return ret;
+		ret = ov965x_read(ov965x, REG_AECHM, &reg2);
+		if (ret < 0)
+			return ret;
+		exposure = ((reg2 & 0x3f) << 10) | (reg1 << 2) |
+						(reg0 & 0x3);
+		ov965x->ctrls.exposure->val = ((exposure *
+				ov965x->exp_row_interval) + 50) / 100;
+		break;
+	}
+
+	return 0;
+}
+
+static int ov965x_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
+	struct ov965x *ov965x = to_ov965x(sd);
+	int ret;
+
+	v4l2_dbg(1, debug, sd, "g_ctrl: %s\n", ctrl->name);
+
+	mutex_lock(&ov965x->lock);
+	ret = __g_volatile_ctrl(ov965x, ctrl);
+	mutex_unlock(&ov965x->lock);
+	return ret;
+}
+
+static int ov965x_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
+	struct ov965x *ov965x = to_ov965x(sd);
+	int ret = -EINVAL;
+
+	v4l2_dbg(1, debug, sd, "s_ctrl: %s, value: %d. power: %d\n",
+		 ctrl->name, ctrl->val, ov965x->power);
+
+	mutex_lock(&ov965x->lock);
+	/*
+	 * If the device is not powered up now postpone applying control's
+	 * value to the hardware, until it is ready to accept commands.
+	 */
+	if (ov965x->power == 0) {
+		mutex_unlock(&ov965x->lock);
+		return 0;
+	}
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		ret = ov965x_set_white_balance(ov965x, ctrl->val);
+		break;
+
+	case V4L2_CID_BRIGHTNESS:
+		ret = ov965x_set_brightness(ov965x, ctrl->val);
+		break;
+
+	case V4L2_CID_EXPOSURE_AUTO:
+		ret = ov965x_set_exposure(ov965x, ctrl->val);
+		break;
+
+	case V4L2_CID_AUTOGAIN:
+		ret = ov965x_set_gain(ov965x, ctrl->val);
+		break;
+
+	case V4L2_CID_HFLIP:
+		ret = ov965x_set_flip(ov965x);
+		break;
+
+	case V4L2_CID_POWER_LINE_FREQUENCY:
+		ret = ov965x_set_banding_filter(ov965x, ctrl->val);
+		break;
+
+	case V4L2_CID_SATURATION:
+		ret = ov965x_set_saturation(ov965x, ctrl->val);
+		break;
+
+	case V4L2_CID_SHARPNESS:
+		ret = ov965x_set_sharpness(ov965x, ctrl->val);
+		break;
+
+	case V4L2_CID_TEST_PATTERN:
+		ret = ov965x_set_test_pattern(ov965x, ctrl->val);
+		break;
+	}
+
+	mutex_unlock(&ov965x->lock);
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ov965x_ctrl_ops = {
+	.g_volatile_ctrl = ov965x_g_volatile_ctrl,
+	.s_ctrl	= ov965x_s_ctrl,
+};
+
+static const char * const test_pattern_menu[] = {
+	"Disabled",
+	"Color bars",
+};
+
+static int ov965x_initialize_controls(struct ov965x *ov965x)
+{
+	const struct v4l2_ctrl_ops *ops = &ov965x_ctrl_ops;
+	struct ov965x_ctrls *ctrls = &ov965x->ctrls;
+	struct v4l2_ctrl_handler *hdl = &ctrls->handler;
+	int ret;
+
+	ret = v4l2_ctrl_handler_init(hdl, 16);
+	if (ret < 0)
+		return ret;
+
+	/* Auto/manual white balance */
+	ctrls->auto_wb = v4l2_ctrl_new_std(hdl, ops,
+					   V4L2_CID_AUTO_WHITE_BALANCE,
+					   0, 1, 1, 1);
+	ctrls->blue_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BLUE_BALANCE,
+						0, 0xff, 1, 0x80);
+	ctrls->red_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_RED_BALANCE,
+					       0, 0xff, 1, 0x80);
+	/* Auto/manual exposure */
+	ctrls->auto_exp =
+		v4l2_ctrl_new_std_menu(hdl, ops,
+				       V4L2_CID_EXPOSURE_AUTO,
+				       V4L2_EXPOSURE_MANUAL, 0,
+				       V4L2_EXPOSURE_AUTO);
+	/* Exposure time, in 100 us units. min/max is updated dynamically. */
+	ctrls->exposure = v4l2_ctrl_new_std(hdl, ops,
+					    V4L2_CID_EXPOSURE_ABSOLUTE,
+					    2, 1500, 1, 500);
+	/* Auto/manual gain */
+	ctrls->auto_gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_AUTOGAIN,
+					     0, 1, 1, 1);
+	ctrls->gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_GAIN,
+					16, 64 * (16 + 15), 1, 64 * 16);
+
+	ctrls->saturation = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SATURATION,
+					      -2, 2, 1, 0);
+	ctrls->brightness = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BRIGHTNESS,
+					      -3, 3, 1, 0);
+	ctrls->sharpness = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SHARPNESS,
+					     0, 32, 1, 6);
+
+	ctrls->hflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
+	ctrls->vflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
+
+	ctrls->light_freq =
+		v4l2_ctrl_new_std_menu(hdl, ops,
+				       V4L2_CID_POWER_LINE_FREQUENCY,
+				       V4L2_CID_POWER_LINE_FREQUENCY_60HZ, ~0x7,
+				       V4L2_CID_POWER_LINE_FREQUENCY_50HZ);
+
+	v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(test_pattern_menu) - 1, 0, 0,
+				     test_pattern_menu);
+	if (hdl->error) {
+		ret = hdl->error;
+		v4l2_ctrl_handler_free(hdl);
+		return ret;
+	}
+
+	ctrls->gain->flags |= V4L2_CTRL_FLAG_VOLATILE;
+	ctrls->exposure->flags |= V4L2_CTRL_FLAG_VOLATILE;
+
+	v4l2_ctrl_auto_cluster(3, &ctrls->auto_wb, 0, false);
+	v4l2_ctrl_auto_cluster(2, &ctrls->auto_gain, 0, true);
+	v4l2_ctrl_auto_cluster(2, &ctrls->auto_exp, 1, true);
+	v4l2_ctrl_cluster(2, &ctrls->hflip);
+
+	ov965x->sd.ctrl_handler = hdl;
+	return 0;
+}
+
+/*
+ * V4L2 subdev video and pad level operations
+ */
+static void ov965x_get_default_format(struct v4l2_mbus_framefmt *mf)
+{
+	mf->width = ov965x_framesizes[0].width;
+	mf->height = ov965x_framesizes[0].height;
+	mf->colorspace = ov965x_formats[0].colorspace;
+	mf->code = ov965x_formats[0].code;
+	mf->field = V4L2_FIELD_NONE;
+}
+
+static int ov965x_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index >= ARRAY_SIZE(ov965x_formats))
+		return -EINVAL;
+
+	code->code = ov965x_formats[code->index].code;
+	return 0;
+}
+
+static int ov965x_enum_frame_sizes(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_frame_size_enum *fse)
+{
+	int i = ARRAY_SIZE(ov965x_formats);
+
+	if (fse->index >= ARRAY_SIZE(ov965x_framesizes))
+		return -EINVAL;
+
+	while (--i)
+		if (fse->code == ov965x_formats[i].code)
+			break;
+
+	fse->code = ov965x_formats[i].code;
+
+	fse->min_width  = ov965x_framesizes[fse->index].width;
+	fse->max_width  = fse->min_width;
+	fse->max_height = ov965x_framesizes[fse->index].height;
+	fse->min_height = fse->max_height;
+
+	return 0;
+}
+
+static int ov965x_g_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *fi)
+{
+	struct ov965x *ov965x = to_ov965x(sd);
+
+	mutex_lock(&ov965x->lock);
+	fi->interval = ov965x->fiv->interval;
+	mutex_unlock(&ov965x->lock);
+
+	return 0;
+}
+
+static int __ov965x_set_frame_interval(struct ov965x *ov965x,
+				       struct v4l2_subdev_frame_interval *fi)
+{
+	struct v4l2_mbus_framefmt *mbus_fmt = &ov965x->format;
+	const struct ov965x_interval *fiv = &ov965x_intervals[0];
+	u64 req_int, err, min_err = ~0ULL;
+	unsigned int i;
+
+	if (fi->interval.denominator == 0)
+		return -EINVAL;
+
+	req_int = (u64)fi->interval.numerator * 10000;
+	do_div(req_int, fi->interval.denominator);
+
+	for (i = 0; i < ARRAY_SIZE(ov965x_intervals); i++) {
+		const struct ov965x_interval *iv = &ov965x_intervals[i];
+
+		if (mbus_fmt->width != iv->size.width ||
+		    mbus_fmt->height != iv->size.height)
+			continue;
+		err = abs((u64)(iv->interval.numerator * 10000) /
+			    iv->interval.denominator - req_int);
+		if (err < min_err) {
+			fiv = iv;
+			min_err = err;
+		}
+	}
+	ov965x->fiv = fiv;
+
+	v4l2_dbg(1, debug, &ov965x->sd, "Changed frame interval to %u us\n",
+		 fiv->interval.numerator * 1000000 / fiv->interval.denominator);
+
+	return 0;
+}
+
+static int ov965x_s_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *fi)
+{
+	struct ov965x *ov965x = to_ov965x(sd);
+	int ret;
+
+	v4l2_dbg(1, debug, sd, "Setting %d/%d frame interval\n",
+		 fi->interval.numerator, fi->interval.denominator);
+
+	mutex_lock(&ov965x->lock);
+	ret = __ov965x_set_frame_interval(ov965x, fi);
+	ov965x->apply_frame_fmt = 1;
+	mutex_unlock(&ov965x->lock);
+	return ret;
+}
+
+static int ov965x_get_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct ov965x *ov965x = to_ov965x(sd);
+	struct v4l2_mbus_framefmt *mf;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		mf = v4l2_subdev_get_try_format(sd, sd_state, 0);
+		fmt->format = *mf;
+		return 0;
+	}
+
+	mutex_lock(&ov965x->lock);
+	fmt->format = ov965x->format;
+	mutex_unlock(&ov965x->lock);
+
+	return 0;
+}
+
+static void __ov965x_try_frame_size(struct v4l2_mbus_framefmt *mf,
+				    const struct ov965x_framesize **size)
+{
+	const struct ov965x_framesize *fsize = &ov965x_framesizes[0],
+		*match = NULL;
+	int i = ARRAY_SIZE(ov965x_framesizes);
+	unsigned int min_err = UINT_MAX;
+
+	while (i--) {
+		int err = abs(fsize->width - mf->width)
+				+ abs(fsize->height - mf->height);
+		if (err < min_err) {
+			min_err = err;
+			match = fsize;
+		}
+		fsize++;
+	}
+	if (!match)
+		match = &ov965x_framesizes[0];
+	mf->width  = match->width;
+	mf->height = match->height;
+	if (size)
+		*size = match;
+}
+
+static int ov965x_set_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *fmt)
+{
+	unsigned int index = ARRAY_SIZE(ov965x_formats);
+	struct v4l2_mbus_framefmt *mf = &fmt->format;
+	struct ov965x *ov965x = to_ov965x(sd);
+	const struct ov965x_framesize *size = NULL;
+	int ret = 0;
+
+	__ov965x_try_frame_size(mf, &size);
+
+	while (--index)
+		if (ov965x_formats[index].code == mf->code)
+			break;
+
+	mf->colorspace	= V4L2_COLORSPACE_JPEG;
+	mf->code	= ov965x_formats[index].code;
+	mf->field	= V4L2_FIELD_NONE;
+
+	mutex_lock(&ov965x->lock);
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		if (sd_state) {
+			mf = v4l2_subdev_get_try_format(sd, sd_state,
+							fmt->pad);
+			*mf = fmt->format;
+		}
+	} else {
+		if (ov965x->streaming) {
+			ret = -EBUSY;
+		} else {
+			ov965x->frame_size = size;
+			ov965x->format = fmt->format;
+			ov965x->tslb_reg = ov965x_formats[index].tslb_reg;
+			ov965x->apply_frame_fmt = 1;
+		}
+	}
+
+	if (!ret && fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		struct v4l2_subdev_frame_interval fiv = {
+			.interval = { 0, 1 }
+		};
+		/* Reset to minimum possible frame interval */
+		__ov965x_set_frame_interval(ov965x, &fiv);
+	}
+	mutex_unlock(&ov965x->lock);
+
+	if (!ret)
+		ov965x_update_exposure_ctrl(ov965x);
+
+	return ret;
+}
+
+static int ov965x_set_frame_size(struct ov965x *ov965x)
+{
+	int i, ret = 0;
+
+	for (i = 0; ret == 0 && i < NUM_FMT_REGS; i++)
+		ret = ov965x_write(ov965x, frame_size_reg_addr[i],
+				   ov965x->frame_size->regs[i]);
+	return ret;
+}
+
+static int __ov965x_set_params(struct ov965x *ov965x)
+{
+	struct ov965x_ctrls *ctrls = &ov965x->ctrls;
+	int ret = 0;
+	u8 reg;
+
+	if (ov965x->apply_frame_fmt) {
+		reg = DEF_CLKRC + ov965x->fiv->clkrc_div;
+		ret = ov965x_write(ov965x, REG_CLKRC, reg);
+		if (ret < 0)
+			return ret;
+		ret = ov965x_set_frame_size(ov965x);
+		if (ret < 0)
+			return ret;
+		ret = ov965x_read(ov965x, REG_TSLB, &reg);
+		if (ret < 0)
+			return ret;
+		reg &= ~TSLB_YUYV_MASK;
+		reg |= ov965x->tslb_reg;
+		ret = ov965x_write(ov965x, REG_TSLB, reg);
+		if (ret < 0)
+			return ret;
+	}
+	ret = ov965x_set_default_gamma_curve(ov965x);
+	if (ret < 0)
+		return ret;
+	ret = ov965x_set_color_matrix(ov965x);
+	if (ret < 0)
+		return ret;
+	/*
+	 * Select manual banding filter, the filter will
+	 * be enabled further if required.
+	 */
+	ret = ov965x_read(ov965x, REG_COM11, &reg);
+	if (!ret)
+		reg |= COM11_BANDING;
+	ret = ov965x_write(ov965x, REG_COM11, reg);
+	if (ret < 0)
+		return ret;
+	/*
+	 * Banding filter (REG_MBD value) needs to match selected
+	 * resolution and frame rate, so it's always updated here.
+	 */
+	return ov965x_set_banding_filter(ov965x, ctrls->light_freq->val);
+}
+
+static int ov965x_s_stream(struct v4l2_subdev *sd, int on)
+{
+	struct ov965x *ov965x = to_ov965x(sd);
+	struct ov965x_ctrls *ctrls = &ov965x->ctrls;
+	int ret = 0;
+
+	v4l2_dbg(1, debug, sd, "%s: on: %d\n", __func__, on);
+
+	mutex_lock(&ov965x->lock);
+	if (ov965x->streaming == !on) {
+		if (on)
+			ret = __ov965x_set_params(ov965x);
+
+		if (!ret && ctrls->update) {
+			/*
+			 * ov965x_s_ctrl callback takes the mutex
+			 * so it needs to be released here.
+			 */
+			mutex_unlock(&ov965x->lock);
+			ret = v4l2_ctrl_handler_setup(&ctrls->handler);
+
+			mutex_lock(&ov965x->lock);
+			if (!ret)
+				ctrls->update = 0;
+		}
+		if (!ret)
+			ret = ov965x_write(ov965x, REG_COM2,
+					   on ? 0x01 : 0x11);
+	}
+	if (!ret)
+		ov965x->streaming += on ? 1 : -1;
+
+	WARN_ON(ov965x->streaming < 0);
+	mutex_unlock(&ov965x->lock);
+
+	return ret;
+}
+
+/*
+ * V4L2 subdev internal operations
+ */
+static int ov965x_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_mbus_framefmt *mf =
+		v4l2_subdev_get_try_format(sd, fh->state, 0);
+
+	ov965x_get_default_format(mf);
+	return 0;
+}
+
+static const struct v4l2_subdev_pad_ops ov965x_pad_ops = {
+	.enum_mbus_code = ov965x_enum_mbus_code,
+	.enum_frame_size = ov965x_enum_frame_sizes,
+	.get_fmt = ov965x_get_fmt,
+	.set_fmt = ov965x_set_fmt,
+};
+
+static const struct v4l2_subdev_video_ops ov965x_video_ops = {
+	.s_stream = ov965x_s_stream,
+	.g_frame_interval = ov965x_g_frame_interval,
+	.s_frame_interval = ov965x_s_frame_interval,
+
+};
+
+static const struct v4l2_subdev_internal_ops ov965x_sd_internal_ops = {
+	.open = ov965x_open,
+};
+
+static const struct v4l2_subdev_core_ops ov965x_core_ops = {
+	.s_power = ov965x_s_power,
+	.log_status = v4l2_ctrl_subdev_log_status,
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_ops ov965x_subdev_ops = {
+	.core = &ov965x_core_ops,
+	.pad = &ov965x_pad_ops,
+	.video = &ov965x_video_ops,
+};
+
+static int ov965x_configure_gpios(struct ov965x *ov965x)
+{
+	struct device *dev = regmap_get_device(ov965x->regmap);
+
+	ov965x->gpios[GPIO_PWDN] = devm_gpiod_get_optional(dev, "powerdown",
+							GPIOD_OUT_HIGH);
+	if (IS_ERR(ov965x->gpios[GPIO_PWDN])) {
+		dev_info(dev, "can't get %s GPIO\n", "powerdown");
+		return PTR_ERR(ov965x->gpios[GPIO_PWDN]);
+	}
+
+	ov965x->gpios[GPIO_RST] = devm_gpiod_get_optional(dev, "reset",
+							GPIOD_OUT_HIGH);
+	if (IS_ERR(ov965x->gpios[GPIO_RST])) {
+		dev_info(dev, "can't get %s GPIO\n", "reset");
+		return PTR_ERR(ov965x->gpios[GPIO_RST]);
+	}
+
+	return 0;
+}
+
+static int ov965x_detect_sensor(struct v4l2_subdev *sd)
+{
+	struct ov965x *ov965x = to_ov965x(sd);
+	u8 pid, ver;
+	int ret;
+
+	mutex_lock(&ov965x->lock);
+	ret = __ov965x_set_power(ov965x, 1);
+	if (ret)
+		goto out;
+
+	msleep(25);
+
+	/* Check sensor revision */
+	ret = ov965x_read(ov965x, REG_PID, &pid);
+	if (!ret)
+		ret = ov965x_read(ov965x, REG_VER, &ver);
+
+	__ov965x_set_power(ov965x, 0);
+
+	if (!ret) {
+		ov965x->id = OV965X_ID(pid, ver);
+		if (ov965x->id == OV9650_ID || ov965x->id == OV9652_ID) {
+			v4l2_info(sd, "Found OV%04X sensor\n", ov965x->id);
+		} else {
+			v4l2_err(sd, "Sensor detection failed (%04X)\n",
+				 ov965x->id);
+			ret = -ENODEV;
+		}
+	}
+out:
+	mutex_unlock(&ov965x->lock);
+
+	return ret;
+}
+
+static int ov965x_probe(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd;
+	struct ov965x *ov965x;
+	int ret;
+	static const struct regmap_config ov965x_regmap_config = {
+		.reg_bits = 8,
+		.val_bits = 8,
+		.max_register = 0xab,
+	};
+
+	ov965x = devm_kzalloc(&client->dev, sizeof(*ov965x), GFP_KERNEL);
+	if (!ov965x)
+		return -ENOMEM;
+
+	ov965x->regmap = devm_regmap_init_sccb(client, &ov965x_regmap_config);
+	if (IS_ERR(ov965x->regmap)) {
+		dev_err(&client->dev, "Failed to allocate register map\n");
+		return PTR_ERR(ov965x->regmap);
+	}
+
+	if (dev_fwnode(&client->dev)) {
+		ov965x->clk = devm_clk_get(&client->dev, NULL);
+		if (IS_ERR(ov965x->clk))
+			return PTR_ERR(ov965x->clk);
+		ov965x->mclk_frequency = clk_get_rate(ov965x->clk);
+
+		ret = ov965x_configure_gpios(ov965x);
+		if (ret < 0)
+			return ret;
+	} else {
+		dev_err(&client->dev,
+			"No device properties specified\n");
+
+		return -EINVAL;
+	}
+
+	mutex_init(&ov965x->lock);
+
+	sd = &ov965x->sd;
+	v4l2_i2c_subdev_init(sd, client, &ov965x_subdev_ops);
+	strscpy(sd->name, DRIVER_NAME, sizeof(sd->name));
+
+	sd->internal_ops = &ov965x_sd_internal_ops;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+		     V4L2_SUBDEV_FL_HAS_EVENTS;
+
+	ov965x->pad.flags = MEDIA_PAD_FL_SOURCE;
+	sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret = media_entity_pads_init(&sd->entity, 1, &ov965x->pad);
+	if (ret < 0)
+		goto err_mutex;
+
+	ret = ov965x_initialize_controls(ov965x);
+	if (ret < 0)
+		goto err_me;
+
+	ov965x_get_default_format(&ov965x->format);
+	ov965x->frame_size = &ov965x_framesizes[0];
+	ov965x->fiv = &ov965x_intervals[0];
+
+	ret = ov965x_detect_sensor(sd);
+	if (ret < 0)
+		goto err_ctrls;
+
+	/* Update exposure time min/max to match frame format */
+	ov965x_update_exposure_ctrl(ov965x);
+
+	ret = v4l2_async_register_subdev(sd);
+	if (ret < 0)
+		goto err_ctrls;
+
+	return 0;
+err_ctrls:
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+err_me:
+	media_entity_cleanup(&sd->entity);
+err_mutex:
+	mutex_destroy(&ov965x->lock);
+	return ret;
+}
+
+static void ov965x_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov965x *ov965x = to_ov965x(sd);
+
+	v4l2_async_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+	media_entity_cleanup(&sd->entity);
+	mutex_destroy(&ov965x->lock);
+}
+
+static const struct i2c_device_id ov965x_id[] = {
+	{ "OV9650", 0 },
+	{ "OV9652", 0 },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(i2c, ov965x_id);
+
+#if IS_ENABLED(CONFIG_OF)
+static const struct of_device_id ov965x_of_match[] = {
+	{ .compatible = "ovti,ov9650", },
+	{ .compatible = "ovti,ov9652", },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, ov965x_of_match);
+#endif
+
+static struct i2c_driver ov965x_i2c_driver = {
+	.driver = {
+		.name	= DRIVER_NAME,
+		.of_match_table = of_match_ptr(ov965x_of_match),
+	},
+	.probe		= ov965x_probe,
+	.remove		= ov965x_remove,
+	.id_table	= ov965x_id,
+};
+
+module_i2c_driver(ov965x_i2c_driver);
+
+MODULE_AUTHOR("Sylwester Nawrocki <sylvester.nawrocki@gmail.com>");
+MODULE_DESCRIPTION("OV9650/OV9652 CMOS Image Sensor driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/ov9734.c b/drivers/media/i2c/ov9734.c
new file mode 100644
index 000000000..b36fc0fed
--- /dev/null
+++ b/drivers/media/i2c/ov9734.c
@@ -0,0 +1,1043 @@
+// SPDX-License-Identifier: GPL-2.0
+// Copyright (c) 2020 Intel Corporation.
+
+#include <asm/unaligned.h>
+#include <linux/acpi.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+
+#define OV9734_LINK_FREQ_180MHZ		180000000ULL
+#define OV9734_SCLK			36000000LL
+#define OV9734_MCLK			19200000
+/* ov9734 only support 1-lane mipi output */
+#define OV9734_DATA_LANES		1
+#define OV9734_RGB_DEPTH		10
+
+#define OV9734_REG_CHIP_ID		0x300a
+#define OV9734_CHIP_ID			0x9734
+
+#define OV9734_REG_MODE_SELECT		0x0100
+#define OV9734_MODE_STANDBY		0x00
+#define OV9734_MODE_STREAMING		0x01
+
+/* vertical-timings from sensor */
+#define OV9734_REG_VTS			0x380e
+#define OV9734_VTS_30FPS		0x0322
+#define OV9734_VTS_30FPS_MIN		0x0322
+#define OV9734_VTS_MAX			0x7fff
+
+/* horizontal-timings from sensor */
+#define OV9734_REG_HTS			0x380c
+
+/* Exposure controls from sensor */
+#define OV9734_REG_EXPOSURE		0x3500
+#define OV9734_EXPOSURE_MIN		4
+#define OV9734_EXPOSURE_MAX_MARGIN	4
+#define	OV9734_EXPOSURE_STEP		1
+
+/* Analog gain controls from sensor */
+#define OV9734_REG_ANALOG_GAIN		0x350a
+#define OV9734_ANAL_GAIN_MIN		16
+#define OV9734_ANAL_GAIN_MAX		248
+#define OV9734_ANAL_GAIN_STEP		1
+
+/* Digital gain controls from sensor */
+#define OV9734_REG_MWB_R_GAIN		0x5180
+#define OV9734_REG_MWB_G_GAIN		0x5182
+#define OV9734_REG_MWB_B_GAIN		0x5184
+#define OV9734_DGTL_GAIN_MIN		256
+#define OV9734_DGTL_GAIN_MAX		1023
+#define OV9734_DGTL_GAIN_STEP		1
+#define OV9734_DGTL_GAIN_DEFAULT	256
+
+/* Test Pattern Control */
+#define OV9734_REG_TEST_PATTERN		0x5080
+#define OV9734_TEST_PATTERN_ENABLE	BIT(7)
+#define OV9734_TEST_PATTERN_BAR_SHIFT	2
+
+/* Group Access */
+#define OV9734_REG_GROUP_ACCESS		0x3208
+#define OV9734_GROUP_HOLD_START		0x0
+#define OV9734_GROUP_HOLD_END		0x10
+#define OV9734_GROUP_HOLD_LAUNCH	0xa0
+
+enum {
+	OV9734_LINK_FREQ_180MHZ_INDEX,
+};
+
+struct ov9734_reg {
+	u16 address;
+	u8 val;
+};
+
+struct ov9734_reg_list {
+	u32 num_of_regs;
+	const struct ov9734_reg *regs;
+};
+
+struct ov9734_link_freq_config {
+	const struct ov9734_reg_list reg_list;
+};
+
+struct ov9734_mode {
+	/* Frame width in pixels */
+	u32 width;
+
+	/* Frame height in pixels */
+	u32 height;
+
+	/* Horizontal timining size */
+	u32 hts;
+
+	/* Default vertical timining size */
+	u32 vts_def;
+
+	/* Min vertical timining size */
+	u32 vts_min;
+
+	/* Link frequency needed for this resolution */
+	u32 link_freq_index;
+
+	/* Sensor register settings for this resolution */
+	const struct ov9734_reg_list reg_list;
+};
+
+static const struct ov9734_reg mipi_data_rate_360mbps[] = {
+	{0x3030, 0x19},
+	{0x3080, 0x02},
+	{0x3081, 0x4b},
+	{0x3082, 0x04},
+	{0x3083, 0x00},
+	{0x3084, 0x02},
+	{0x3085, 0x01},
+	{0x3086, 0x01},
+	{0x3089, 0x01},
+	{0x308a, 0x00},
+	{0x301e, 0x15},
+	{0x3103, 0x01},
+};
+
+static const struct ov9734_reg mode_1296x734_regs[] = {
+	{0x3001, 0x00},
+	{0x3002, 0x00},
+	{0x3007, 0x00},
+	{0x3010, 0x00},
+	{0x3011, 0x08},
+	{0x3014, 0x22},
+	{0x3600, 0x55},
+	{0x3601, 0x02},
+	{0x3605, 0x22},
+	{0x3611, 0xe7},
+	{0x3654, 0x10},
+	{0x3655, 0x77},
+	{0x3656, 0x77},
+	{0x3657, 0x07},
+	{0x3658, 0x22},
+	{0x3659, 0x22},
+	{0x365a, 0x02},
+	{0x3784, 0x05},
+	{0x3785, 0x55},
+	{0x37c0, 0x07},
+	{0x3800, 0x00},
+	{0x3801, 0x04},
+	{0x3802, 0x00},
+	{0x3803, 0x04},
+	{0x3804, 0x05},
+	{0x3805, 0x0b},
+	{0x3806, 0x02},
+	{0x3807, 0xdb},
+	{0x3808, 0x05},
+	{0x3809, 0x00},
+	{0x380a, 0x02},
+	{0x380b, 0xd0},
+	{0x380c, 0x05},
+	{0x380d, 0xc6},
+	{0x380e, 0x03},
+	{0x380f, 0x22},
+	{0x3810, 0x00},
+	{0x3811, 0x04},
+	{0x3812, 0x00},
+	{0x3813, 0x04},
+	{0x3816, 0x00},
+	{0x3817, 0x00},
+	{0x3818, 0x00},
+	{0x3819, 0x04},
+	{0x3820, 0x18},
+	{0x3821, 0x00},
+	{0x382c, 0x06},
+	{0x3500, 0x00},
+	{0x3501, 0x31},
+	{0x3502, 0x00},
+	{0x3503, 0x03},
+	{0x3504, 0x00},
+	{0x3505, 0x00},
+	{0x3509, 0x10},
+	{0x350a, 0x00},
+	{0x350b, 0x40},
+	{0x3d00, 0x00},
+	{0x3d01, 0x00},
+	{0x3d02, 0x00},
+	{0x3d03, 0x00},
+	{0x3d04, 0x00},
+	{0x3d05, 0x00},
+	{0x3d06, 0x00},
+	{0x3d07, 0x00},
+	{0x3d08, 0x00},
+	{0x3d09, 0x00},
+	{0x3d0a, 0x00},
+	{0x3d0b, 0x00},
+	{0x3d0c, 0x00},
+	{0x3d0d, 0x00},
+	{0x3d0e, 0x00},
+	{0x3d0f, 0x00},
+	{0x3d80, 0x00},
+	{0x3d81, 0x00},
+	{0x3d82, 0x38},
+	{0x3d83, 0xa4},
+	{0x3d84, 0x00},
+	{0x3d85, 0x00},
+	{0x3d86, 0x1f},
+	{0x3d87, 0x03},
+	{0x3d8b, 0x00},
+	{0x3d8f, 0x00},
+	{0x4001, 0xe0},
+	{0x4009, 0x0b},
+	{0x4300, 0x03},
+	{0x4301, 0xff},
+	{0x4304, 0x00},
+	{0x4305, 0x00},
+	{0x4309, 0x00},
+	{0x4600, 0x00},
+	{0x4601, 0x80},
+	{0x4800, 0x00},
+	{0x4805, 0x00},
+	{0x4821, 0x50},
+	{0x4823, 0x50},
+	{0x4837, 0x2d},
+	{0x4a00, 0x00},
+	{0x4f00, 0x80},
+	{0x4f01, 0x10},
+	{0x4f02, 0x00},
+	{0x4f03, 0x00},
+	{0x4f04, 0x00},
+	{0x4f05, 0x00},
+	{0x4f06, 0x00},
+	{0x4f07, 0x00},
+	{0x4f08, 0x00},
+	{0x4f09, 0x00},
+	{0x5000, 0x2f},
+	{0x500c, 0x00},
+	{0x500d, 0x00},
+	{0x500e, 0x00},
+	{0x500f, 0x00},
+	{0x5010, 0x00},
+	{0x5011, 0x00},
+	{0x5012, 0x00},
+	{0x5013, 0x00},
+	{0x5014, 0x00},
+	{0x5015, 0x00},
+	{0x5016, 0x00},
+	{0x5017, 0x00},
+	{0x5080, 0x00},
+	{0x5180, 0x01},
+	{0x5181, 0x00},
+	{0x5182, 0x01},
+	{0x5183, 0x00},
+	{0x5184, 0x01},
+	{0x5185, 0x00},
+	{0x5708, 0x06},
+	{0x380f, 0x2a},
+	{0x5780, 0x3e},
+	{0x5781, 0x0f},
+	{0x5782, 0x44},
+	{0x5783, 0x02},
+	{0x5784, 0x01},
+	{0x5785, 0x01},
+	{0x5786, 0x00},
+	{0x5787, 0x04},
+	{0x5788, 0x02},
+	{0x5789, 0x0f},
+	{0x578a, 0xfd},
+	{0x578b, 0xf5},
+	{0x578c, 0xf5},
+	{0x578d, 0x03},
+	{0x578e, 0x08},
+	{0x578f, 0x0c},
+	{0x5790, 0x08},
+	{0x5791, 0x04},
+	{0x5792, 0x00},
+	{0x5793, 0x52},
+	{0x5794, 0xa3},
+	{0x5000, 0x3f},
+	{0x3801, 0x00},
+	{0x3803, 0x00},
+	{0x3805, 0x0f},
+	{0x3807, 0xdf},
+	{0x3809, 0x10},
+	{0x380b, 0xde},
+	{0x3811, 0x00},
+	{0x3813, 0x01},
+};
+
+static const char * const ov9734_test_pattern_menu[] = {
+	"Disabled",
+	"Standard Color Bar",
+	"Top-Bottom Darker Color Bar",
+	"Right-Left Darker Color Bar",
+	"Bottom-Top Darker Color Bar",
+};
+
+static const s64 link_freq_menu_items[] = {
+	OV9734_LINK_FREQ_180MHZ,
+};
+
+static const struct ov9734_link_freq_config link_freq_configs[] = {
+	[OV9734_LINK_FREQ_180MHZ_INDEX] = {
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mipi_data_rate_360mbps),
+			.regs = mipi_data_rate_360mbps,
+		}
+	},
+};
+
+static const struct ov9734_mode supported_modes[] = {
+	{
+		.width = 1296,
+		.height = 734,
+		.hts = 0x5c6,
+		.vts_def = OV9734_VTS_30FPS,
+		.vts_min = OV9734_VTS_30FPS_MIN,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1296x734_regs),
+			.regs = mode_1296x734_regs,
+		},
+		.link_freq_index = OV9734_LINK_FREQ_180MHZ_INDEX,
+	},
+};
+
+struct ov9734 {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct v4l2_ctrl_handler ctrl_handler;
+
+	/* V4L2 Controls */
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *pixel_rate;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *exposure;
+
+	/* Current mode */
+	const struct ov9734_mode *cur_mode;
+
+	/* To serialize asynchronus callbacks */
+	struct mutex mutex;
+
+	/* Streaming on/off */
+	bool streaming;
+};
+
+static inline struct ov9734 *to_ov9734(struct v4l2_subdev *subdev)
+{
+	return container_of(subdev, struct ov9734, sd);
+}
+
+static u64 to_pixel_rate(u32 f_index)
+{
+	u64 pixel_rate = link_freq_menu_items[f_index] * 2 * OV9734_DATA_LANES;
+
+	do_div(pixel_rate, OV9734_RGB_DEPTH);
+
+	return pixel_rate;
+}
+
+static u64 to_pixels_per_line(u32 hts, u32 f_index)
+{
+	u64 ppl = hts * to_pixel_rate(f_index);
+
+	do_div(ppl, OV9734_SCLK);
+
+	return ppl;
+}
+
+static int ov9734_read_reg(struct ov9734 *ov9734, u16 reg, u16 len, u32 *val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov9734->sd);
+	struct i2c_msg msgs[2];
+	u8 addr_buf[2];
+	u8 data_buf[4] = {0};
+	int ret;
+
+	if (len > sizeof(data_buf))
+		return -EINVAL;
+
+	put_unaligned_be16(reg, addr_buf);
+	msgs[0].addr = client->addr;
+	msgs[0].flags = 0;
+	msgs[0].len = sizeof(addr_buf);
+	msgs[0].buf = addr_buf;
+	msgs[1].addr = client->addr;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].len = len;
+	msgs[1].buf = &data_buf[sizeof(data_buf) - len];
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret != ARRAY_SIZE(msgs))
+		return ret < 0 ? ret : -EIO;
+
+	*val = get_unaligned_be32(data_buf);
+
+	return 0;
+}
+
+static int ov9734_write_reg(struct ov9734 *ov9734, u16 reg, u16 len, u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov9734->sd);
+	u8 buf[6];
+	int ret = 0;
+
+	if (len > 4)
+		return -EINVAL;
+
+	put_unaligned_be16(reg, buf);
+	put_unaligned_be32(val << 8 * (4 - len), buf + 2);
+
+	ret = i2c_master_send(client, buf, len + 2);
+	if (ret != len + 2)
+		return ret < 0 ? ret : -EIO;
+
+	return 0;
+}
+
+static int ov9734_write_reg_list(struct ov9734 *ov9734,
+				 const struct ov9734_reg_list *r_list)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov9734->sd);
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < r_list->num_of_regs; i++) {
+		ret = ov9734_write_reg(ov9734, r_list->regs[i].address, 1,
+				       r_list->regs[i].val);
+		if (ret) {
+			dev_err_ratelimited(&client->dev,
+					    "write reg 0x%4.4x return err = %d",
+					    r_list->regs[i].address, ret);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int ov9734_update_digital_gain(struct ov9734 *ov9734, u32 d_gain)
+{
+	int ret;
+
+	ret = ov9734_write_reg(ov9734, OV9734_REG_GROUP_ACCESS, 1,
+			       OV9734_GROUP_HOLD_START);
+	if (ret)
+		return ret;
+
+	ret = ov9734_write_reg(ov9734, OV9734_REG_MWB_R_GAIN, 2, d_gain);
+	if (ret)
+		return ret;
+
+	ret = ov9734_write_reg(ov9734, OV9734_REG_MWB_G_GAIN, 2, d_gain);
+	if (ret)
+		return ret;
+
+	ret = ov9734_write_reg(ov9734, OV9734_REG_MWB_B_GAIN, 2, d_gain);
+	if (ret)
+		return ret;
+
+	ret = ov9734_write_reg(ov9734, OV9734_REG_GROUP_ACCESS, 1,
+			       OV9734_GROUP_HOLD_END);
+	if (ret)
+		return ret;
+
+	ret = ov9734_write_reg(ov9734, OV9734_REG_GROUP_ACCESS, 1,
+			       OV9734_GROUP_HOLD_LAUNCH);
+	return ret;
+}
+
+static int ov9734_test_pattern(struct ov9734 *ov9734, u32 pattern)
+{
+	if (pattern)
+		pattern = (pattern - 1) << OV9734_TEST_PATTERN_BAR_SHIFT |
+			OV9734_TEST_PATTERN_ENABLE;
+
+	return ov9734_write_reg(ov9734, OV9734_REG_TEST_PATTERN, 1, pattern);
+}
+
+static int ov9734_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov9734 *ov9734 = container_of(ctrl->handler,
+					     struct ov9734, ctrl_handler);
+	struct i2c_client *client = v4l2_get_subdevdata(&ov9734->sd);
+	s64 exposure_max;
+	int ret = 0;
+
+	/* Propagate change of current control to all related controls */
+	if (ctrl->id == V4L2_CID_VBLANK) {
+		/* Update max exposure while meeting expected vblanking */
+		exposure_max = ov9734->cur_mode->height + ctrl->val -
+			OV9734_EXPOSURE_MAX_MARGIN;
+		__v4l2_ctrl_modify_range(ov9734->exposure,
+					 ov9734->exposure->minimum,
+					 exposure_max, ov9734->exposure->step,
+					 exposure_max);
+	}
+
+	/* V4L2 controls values will be applied only when power is already up */
+	if (!pm_runtime_get_if_in_use(&client->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_ANALOGUE_GAIN:
+		ret = ov9734_write_reg(ov9734, OV9734_REG_ANALOG_GAIN,
+				       2, ctrl->val);
+		break;
+
+	case V4L2_CID_DIGITAL_GAIN:
+		ret = ov9734_update_digital_gain(ov9734, ctrl->val);
+		break;
+
+	case V4L2_CID_EXPOSURE:
+		/* 4 least significant bits of expsoure are fractional part */
+		ret = ov9734_write_reg(ov9734, OV9734_REG_EXPOSURE,
+				       3, ctrl->val << 4);
+		break;
+
+	case V4L2_CID_VBLANK:
+		ret = ov9734_write_reg(ov9734, OV9734_REG_VTS, 2,
+				       ov9734->cur_mode->height + ctrl->val);
+		break;
+
+	case V4L2_CID_TEST_PATTERN:
+		ret = ov9734_test_pattern(ov9734, ctrl->val);
+		break;
+
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	pm_runtime_put(&client->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ov9734_ctrl_ops = {
+	.s_ctrl = ov9734_set_ctrl,
+};
+
+static int ov9734_init_controls(struct ov9734 *ov9734)
+{
+	struct v4l2_ctrl_handler *ctrl_hdlr;
+	const struct ov9734_mode *cur_mode;
+	s64 exposure_max, h_blank, pixel_rate;
+	u32 vblank_min, vblank_max, vblank_default;
+	int ret, size;
+
+	ctrl_hdlr = &ov9734->ctrl_handler;
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 8);
+	if (ret)
+		return ret;
+
+	ctrl_hdlr->lock = &ov9734->mutex;
+	cur_mode = ov9734->cur_mode;
+	size = ARRAY_SIZE(link_freq_menu_items);
+	ov9734->link_freq = v4l2_ctrl_new_int_menu(ctrl_hdlr, &ov9734_ctrl_ops,
+						   V4L2_CID_LINK_FREQ,
+						   size - 1, 0,
+						   link_freq_menu_items);
+	if (ov9734->link_freq)
+		ov9734->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	pixel_rate = to_pixel_rate(OV9734_LINK_FREQ_180MHZ_INDEX);
+	ov9734->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, &ov9734_ctrl_ops,
+					       V4L2_CID_PIXEL_RATE, 0,
+					       pixel_rate, 1, pixel_rate);
+	vblank_min = cur_mode->vts_min - cur_mode->height;
+	vblank_max = OV9734_VTS_MAX - cur_mode->height;
+	vblank_default = cur_mode->vts_def - cur_mode->height;
+	ov9734->vblank = v4l2_ctrl_new_std(ctrl_hdlr, &ov9734_ctrl_ops,
+					   V4L2_CID_VBLANK, vblank_min,
+					   vblank_max, 1, vblank_default);
+	h_blank = to_pixels_per_line(cur_mode->hts, cur_mode->link_freq_index);
+	h_blank -= cur_mode->width;
+	ov9734->hblank = v4l2_ctrl_new_std(ctrl_hdlr, &ov9734_ctrl_ops,
+					   V4L2_CID_HBLANK, h_blank, h_blank, 1,
+					   h_blank);
+	if (ov9734->hblank)
+		ov9734->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov9734_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
+			  OV9734_ANAL_GAIN_MIN, OV9734_ANAL_GAIN_MAX,
+			  OV9734_ANAL_GAIN_STEP, OV9734_ANAL_GAIN_MIN);
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov9734_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
+			  OV9734_DGTL_GAIN_MIN, OV9734_DGTL_GAIN_MAX,
+			  OV9734_DGTL_GAIN_STEP, OV9734_DGTL_GAIN_DEFAULT);
+	exposure_max = ov9734->cur_mode->vts_def - OV9734_EXPOSURE_MAX_MARGIN;
+	ov9734->exposure = v4l2_ctrl_new_std(ctrl_hdlr, &ov9734_ctrl_ops,
+					     V4L2_CID_EXPOSURE,
+					     OV9734_EXPOSURE_MIN, exposure_max,
+					     OV9734_EXPOSURE_STEP,
+					     exposure_max);
+	v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &ov9734_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(ov9734_test_pattern_menu) - 1,
+				     0, 0, ov9734_test_pattern_menu);
+	if (ctrl_hdlr->error)
+		return ctrl_hdlr->error;
+
+	ov9734->sd.ctrl_handler = ctrl_hdlr;
+
+	return 0;
+}
+
+static void ov9734_update_pad_format(const struct ov9734_mode *mode,
+				     struct v4l2_mbus_framefmt *fmt)
+{
+	fmt->width = mode->width;
+	fmt->height = mode->height;
+	fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	fmt->field = V4L2_FIELD_NONE;
+}
+
+static int ov9734_start_streaming(struct ov9734 *ov9734)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov9734->sd);
+	const struct ov9734_reg_list *reg_list;
+	int link_freq_index, ret;
+
+	link_freq_index = ov9734->cur_mode->link_freq_index;
+	reg_list = &link_freq_configs[link_freq_index].reg_list;
+	ret = ov9734_write_reg_list(ov9734, reg_list);
+	if (ret) {
+		dev_err(&client->dev, "failed to set plls");
+		return ret;
+	}
+
+	reg_list = &ov9734->cur_mode->reg_list;
+	ret = ov9734_write_reg_list(ov9734, reg_list);
+	if (ret) {
+		dev_err(&client->dev, "failed to set mode");
+		return ret;
+	}
+
+	ret = __v4l2_ctrl_handler_setup(ov9734->sd.ctrl_handler);
+	if (ret)
+		return ret;
+
+	ret = ov9734_write_reg(ov9734, OV9734_REG_MODE_SELECT,
+			       1, OV9734_MODE_STREAMING);
+	if (ret)
+		dev_err(&client->dev, "failed to start stream");
+
+	return ret;
+}
+
+static void ov9734_stop_streaming(struct ov9734 *ov9734)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov9734->sd);
+
+	if (ov9734_write_reg(ov9734, OV9734_REG_MODE_SELECT,
+			     1, OV9734_MODE_STANDBY))
+		dev_err(&client->dev, "failed to stop stream");
+}
+
+static int ov9734_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct ov9734 *ov9734 = to_ov9734(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	mutex_lock(&ov9734->mutex);
+	if (ov9734->streaming == enable) {
+		mutex_unlock(&ov9734->mutex);
+		return 0;
+	}
+
+	if (enable) {
+		ret = pm_runtime_resume_and_get(&client->dev);
+		if (ret < 0) {
+			mutex_unlock(&ov9734->mutex);
+			return ret;
+		}
+
+		ret = ov9734_start_streaming(ov9734);
+		if (ret) {
+			enable = 0;
+			ov9734_stop_streaming(ov9734);
+			pm_runtime_put(&client->dev);
+		}
+	} else {
+		ov9734_stop_streaming(ov9734);
+		pm_runtime_put(&client->dev);
+	}
+
+	ov9734->streaming = enable;
+	mutex_unlock(&ov9734->mutex);
+
+	return ret;
+}
+
+static int __maybe_unused ov9734_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov9734 *ov9734 = to_ov9734(sd);
+
+	mutex_lock(&ov9734->mutex);
+	if (ov9734->streaming)
+		ov9734_stop_streaming(ov9734);
+
+	mutex_unlock(&ov9734->mutex);
+
+	return 0;
+}
+
+static int __maybe_unused ov9734_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov9734 *ov9734 = to_ov9734(sd);
+	int ret = 0;
+
+	mutex_lock(&ov9734->mutex);
+	if (!ov9734->streaming)
+		goto exit;
+
+	ret = ov9734_start_streaming(ov9734);
+	if (ret) {
+		ov9734->streaming = false;
+		ov9734_stop_streaming(ov9734);
+	}
+
+exit:
+	mutex_unlock(&ov9734->mutex);
+	return ret;
+}
+
+static int ov9734_set_format(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *sd_state,
+			     struct v4l2_subdev_format *fmt)
+{
+	struct ov9734 *ov9734 = to_ov9734(sd);
+	const struct ov9734_mode *mode;
+	s32 vblank_def, h_blank;
+
+	mode = v4l2_find_nearest_size(supported_modes,
+				      ARRAY_SIZE(supported_modes), width,
+				      height, fmt->format.width,
+				      fmt->format.height);
+
+	mutex_lock(&ov9734->mutex);
+	ov9734_update_pad_format(mode, &fmt->format);
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		*v4l2_subdev_get_try_format(sd, sd_state, fmt->pad) = fmt->format;
+	} else {
+		ov9734->cur_mode = mode;
+		__v4l2_ctrl_s_ctrl(ov9734->link_freq, mode->link_freq_index);
+		__v4l2_ctrl_s_ctrl_int64(ov9734->pixel_rate,
+					 to_pixel_rate(mode->link_freq_index));
+
+		/* Update limits and set FPS to default */
+		vblank_def = mode->vts_def - mode->height;
+		__v4l2_ctrl_modify_range(ov9734->vblank,
+					 mode->vts_min - mode->height,
+					 OV9734_VTS_MAX - mode->height, 1,
+					 vblank_def);
+		__v4l2_ctrl_s_ctrl(ov9734->vblank, vblank_def);
+		h_blank = to_pixels_per_line(mode->hts, mode->link_freq_index) -
+			mode->width;
+		__v4l2_ctrl_modify_range(ov9734->hblank, h_blank, h_blank, 1,
+					 h_blank);
+	}
+
+	mutex_unlock(&ov9734->mutex);
+
+	return 0;
+}
+
+static int ov9734_get_format(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *sd_state,
+			     struct v4l2_subdev_format *fmt)
+{
+	struct ov9734 *ov9734 = to_ov9734(sd);
+
+	mutex_lock(&ov9734->mutex);
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
+		fmt->format = *v4l2_subdev_get_try_format(&ov9734->sd,
+							  sd_state,
+							  fmt->pad);
+	else
+		ov9734_update_pad_format(ov9734->cur_mode, &fmt->format);
+
+	mutex_unlock(&ov9734->mutex);
+
+	return 0;
+}
+
+static int ov9734_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index > 0)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+
+	return 0;
+}
+
+static int ov9734_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->index >= ARRAY_SIZE(supported_modes))
+		return -EINVAL;
+
+	if (fse->code != MEDIA_BUS_FMT_SGRBG10_1X10)
+		return -EINVAL;
+
+	fse->min_width = supported_modes[fse->index].width;
+	fse->max_width = fse->min_width;
+	fse->min_height = supported_modes[fse->index].height;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static int ov9734_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct ov9734 *ov9734 = to_ov9734(sd);
+
+	mutex_lock(&ov9734->mutex);
+	ov9734_update_pad_format(&supported_modes[0],
+				 v4l2_subdev_get_try_format(sd, fh->state, 0));
+	mutex_unlock(&ov9734->mutex);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops ov9734_video_ops = {
+	.s_stream = ov9734_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops ov9734_pad_ops = {
+	.set_fmt = ov9734_set_format,
+	.get_fmt = ov9734_get_format,
+	.enum_mbus_code = ov9734_enum_mbus_code,
+	.enum_frame_size = ov9734_enum_frame_size,
+};
+
+static const struct v4l2_subdev_ops ov9734_subdev_ops = {
+	.video = &ov9734_video_ops,
+	.pad = &ov9734_pad_ops,
+};
+
+static const struct media_entity_operations ov9734_subdev_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static const struct v4l2_subdev_internal_ops ov9734_internal_ops = {
+	.open = ov9734_open,
+};
+
+static int ov9734_identify_module(struct ov9734 *ov9734)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov9734->sd);
+	int ret;
+	u32 val;
+
+	ret = ov9734_read_reg(ov9734, OV9734_REG_CHIP_ID, 2, &val);
+	if (ret)
+		return ret;
+
+	if (val != OV9734_CHIP_ID) {
+		dev_err(&client->dev, "chip id mismatch: %x!=%x",
+			OV9734_CHIP_ID, val);
+		return -ENXIO;
+	}
+
+	return 0;
+}
+
+static int ov9734_check_hwcfg(struct device *dev)
+{
+	struct fwnode_handle *ep;
+	struct fwnode_handle *fwnode = dev_fwnode(dev);
+	struct v4l2_fwnode_endpoint bus_cfg = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY
+	};
+	u32 mclk;
+	int ret;
+	unsigned int i, j;
+
+	if (!fwnode)
+		return -ENXIO;
+
+	ret = fwnode_property_read_u32(fwnode, "clock-frequency", &mclk);
+	if (ret)
+		return ret;
+
+	if (mclk != OV9734_MCLK) {
+		dev_err(dev, "external clock %d is not supported", mclk);
+		return -EINVAL;
+	}
+
+	ep = fwnode_graph_get_next_endpoint(fwnode, NULL);
+	if (!ep)
+		return -ENXIO;
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(ep, &bus_cfg);
+	fwnode_handle_put(ep);
+	if (ret)
+		return ret;
+
+	if (!bus_cfg.nr_of_link_frequencies) {
+		dev_err(dev, "no link frequencies defined");
+		ret = -EINVAL;
+		goto check_hwcfg_error;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(link_freq_menu_items); i++) {
+		for (j = 0; j < bus_cfg.nr_of_link_frequencies; j++) {
+			if (link_freq_menu_items[i] ==
+			    bus_cfg.link_frequencies[j])
+				break;
+		}
+
+		if (j == bus_cfg.nr_of_link_frequencies) {
+			dev_err(dev, "no link frequency %lld supported",
+				link_freq_menu_items[i]);
+			ret = -EINVAL;
+			goto check_hwcfg_error;
+		}
+	}
+
+check_hwcfg_error:
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+
+	return ret;
+}
+
+static void ov9734_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov9734 *ov9734 = to_ov9734(sd);
+
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+	pm_runtime_disable(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+	mutex_destroy(&ov9734->mutex);
+}
+
+static int ov9734_probe(struct i2c_client *client)
+{
+	struct ov9734 *ov9734;
+	int ret;
+
+	ret = ov9734_check_hwcfg(&client->dev);
+	if (ret) {
+		dev_err(&client->dev, "failed to check HW configuration: %d",
+			ret);
+		return ret;
+	}
+
+	ov9734 = devm_kzalloc(&client->dev, sizeof(*ov9734), GFP_KERNEL);
+	if (!ov9734)
+		return -ENOMEM;
+
+	v4l2_i2c_subdev_init(&ov9734->sd, client, &ov9734_subdev_ops);
+	ret = ov9734_identify_module(ov9734);
+	if (ret) {
+		dev_err(&client->dev, "failed to find sensor: %d", ret);
+		return ret;
+	}
+
+	mutex_init(&ov9734->mutex);
+	ov9734->cur_mode = &supported_modes[0];
+	ret = ov9734_init_controls(ov9734);
+	if (ret) {
+		dev_err(&client->dev, "failed to init controls: %d", ret);
+		goto probe_error_v4l2_ctrl_handler_free;
+	}
+
+	ov9734->sd.internal_ops = &ov9734_internal_ops;
+	ov9734->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	ov9734->sd.entity.ops = &ov9734_subdev_entity_ops;
+	ov9734->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ov9734->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&ov9734->sd.entity, 1, &ov9734->pad);
+	if (ret) {
+		dev_err(&client->dev, "failed to init entity pads: %d", ret);
+		goto probe_error_v4l2_ctrl_handler_free;
+	}
+
+	/*
+	 * Device is already turned on by i2c-core with ACPI domain PM.
+	 * Enable runtime PM and turn off the device.
+	 */
+	pm_runtime_set_active(&client->dev);
+	pm_runtime_enable(&client->dev);
+	pm_runtime_idle(&client->dev);
+
+	ret = v4l2_async_register_subdev_sensor(&ov9734->sd);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed to register V4L2 subdev: %d",
+			ret);
+		goto probe_error_media_entity_cleanup_pm;
+	}
+
+	return 0;
+
+probe_error_media_entity_cleanup_pm:
+	pm_runtime_disable(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+	media_entity_cleanup(&ov9734->sd.entity);
+
+probe_error_v4l2_ctrl_handler_free:
+	v4l2_ctrl_handler_free(ov9734->sd.ctrl_handler);
+	mutex_destroy(&ov9734->mutex);
+
+	return ret;
+}
+
+static const struct dev_pm_ops ov9734_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(ov9734_suspend, ov9734_resume)
+};
+
+static const struct acpi_device_id ov9734_acpi_ids[] = {
+	{ "OVTI9734", },
+	{}
+};
+
+MODULE_DEVICE_TABLE(acpi, ov9734_acpi_ids);
+
+static struct i2c_driver ov9734_i2c_driver = {
+	.driver = {
+		.name = "ov9734",
+		.pm = &ov9734_pm_ops,
+		.acpi_match_table = ov9734_acpi_ids,
+	},
+	.probe = ov9734_probe,
+	.remove = ov9734_remove,
+};
+
+module_i2c_driver(ov9734_i2c_driver);
+
+MODULE_AUTHOR("Qiu, Tianshu <tian.shu.qiu@intel.com>");
+MODULE_AUTHOR("Bingbu Cao <bingbu.cao@intel.com>");
+MODULE_DESCRIPTION("OmniVision OV9734 sensor driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/rdacm20.c b/drivers/media/i2c/rdacm20.c
new file mode 100644
index 000000000..f4e2e2f39
--- /dev/null
+++ b/drivers/media/i2c/rdacm20.c
@@ -0,0 +1,674 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * IMI RDACM20 GMSL Camera Driver
+ *
+ * Copyright (C) 2017-2020 Jacopo Mondi
+ * Copyright (C) 2017-2020 Kieran Bingham
+ * Copyright (C) 2017-2019 Laurent Pinchart
+ * Copyright (C) 2017-2019 Niklas Söderlund
+ * Copyright (C) 2016 Renesas Electronics Corporation
+ * Copyright (C) 2015 Cogent Embedded, Inc.
+ */
+
+/*
+ * The camera is made of an Omnivision OV10635 sensor connected to a Maxim
+ * MAX9271 GMSL serializer.
+ */
+
+#include <linux/delay.h>
+#include <linux/fwnode.h>
+#include <linux/init.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+
+#include <media/v4l2-async.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-subdev.h>
+
+#include "max9271.h"
+
+#define OV10635_I2C_ADDRESS		0x30
+
+#define OV10635_SOFTWARE_RESET		0x0103
+#define OV10635_PID			0x300a
+#define OV10635_VER			0x300b
+#define OV10635_SC_CMMN_SCCB_ID		0x300c
+#define OV10635_SC_CMMN_SCCB_ID_SELECT	BIT(0)
+#define OV10635_VERSION			0xa635
+
+#define OV10635_WIDTH			1280
+#define OV10635_HEIGHT			800
+
+/* VTS = PCLK / FPS / HTS / 2 (= 88MHz / 1572 / 30 / 2) */
+#define OV10635_HTS			1572
+/* FPS = 29,9998 */
+#define OV10635_VTS			933
+
+/*
+ * As the drivers supports a single MEDIA_BUS_FMT_UYVY8_1X16 format we
+ * can harcode the pixel rate.
+ *
+ * PCLK is fed through the system clock, programmed @88MHz.
+ * MEDIA_BUS_FMT_UYVY8_1X16 format = 2 samples per pixel.
+ *
+ * Pixelrate = PCLK / 2
+ * FPS = (OV10635_VTS * OV10635_HTS) / PixelRate
+ *     = 29,9998
+ */
+#define OV10635_PIXEL_RATE		(44000000)
+
+static const struct ov10635_reg {
+	u16	reg;
+	u8	val;
+} ov10635_regs_wizard[] = {
+	{ 0x301b, 0xff }, { 0x301c, 0xff }, { 0x301a, 0xff }, { 0x3011, 0x42 },
+	{ 0x6900, 0x0c }, { 0x6901, 0x19 }, { 0x3503, 0x10 }, { 0x3025, 0x03 },
+	{ 0x3003, 0x16 }, { 0x3004, 0x30 }, { 0x3005, 0x40 }, { 0x3006, 0x91 },
+	{ 0x3600, 0x74 }, { 0x3601, 0x2b }, { 0x3612, 0x00 }, { 0x3611, 0x67 },
+	{ 0x3633, 0xca }, { 0x3602, 0xaf }, { 0x3603, 0x04 }, { 0x3630, 0x28 },
+	{ 0x3631, 0x16 }, { 0x3714, 0x10 }, { 0x371d, 0x01 }, { 0x4300, 0x3a },
+	{ 0x3007, 0x01 }, { 0x3024, 0x03 }, { 0x3020, 0x0a }, { 0x3702, 0x0d },
+	{ 0x3703, 0x20 }, { 0x3704, 0x15 }, { 0x3709, 0xa8 }, { 0x370c, 0xc7 },
+	{ 0x370d, 0x80 }, { 0x3712, 0x00 }, { 0x3713, 0x20 }, { 0x3715, 0x04 },
+	{ 0x381d, 0x40 }, { 0x381c, 0x00 }, { 0x3822, 0x50 }, { 0x3824, 0x10 },
+	{ 0x3815, 0x8c }, { 0x3804, 0x05 }, { 0x3805, 0x1f }, { 0x3800, 0x00 },
+	{ 0x3801, 0x00 }, { 0x3806, 0x03 }, { 0x3807, 0x28 }, { 0x3802, 0x00 },
+	{ 0x3803, 0x07 }, { 0x3808, 0x05 }, { 0x3809, 0x00 }, { 0x380a, 0x03 },
+	{ 0x380b, 0x20 }, { 0x380c, OV10635_HTS >> 8 },
+	{ 0x380d, OV10635_HTS & 0xff }, { 0x380e, OV10635_VTS >> 8 },
+	{ 0x380f, OV10635_VTS & 0xff },	{ 0x3813, 0x02 }, { 0x3811, 0x08 },
+	{ 0x381f, 0x0c }, { 0x3819, 0x04 }, { 0x3804, 0x01 }, { 0x3805, 0x00 },
+	{ 0x3828, 0x03 }, { 0x3829, 0x10 }, { 0x382a, 0x10 }, { 0x3621, 0x63 },
+	{ 0x5005, 0x08 }, { 0x56d5, 0x00 }, { 0x56d6, 0x80 }, { 0x56d7, 0x00 },
+	{ 0x56d8, 0x00 }, { 0x56d9, 0x00 }, { 0x56da, 0x80 }, { 0x56db, 0x00 },
+	{ 0x56dc, 0x00 }, { 0x56e8, 0x00 }, { 0x56e9, 0x7f }, { 0x56ea, 0x00 },
+	{ 0x56eb, 0x7f }, { 0x5100, 0x00 }, { 0x5101, 0x80 }, { 0x5102, 0x00 },
+	{ 0x5103, 0x80 }, { 0x5104, 0x00 }, { 0x5105, 0x80 }, { 0x5106, 0x00 },
+	{ 0x5107, 0x80 }, { 0x5108, 0x00 }, { 0x5109, 0x00 }, { 0x510a, 0x00 },
+	{ 0x510b, 0x00 }, { 0x510c, 0x00 }, { 0x510d, 0x00 }, { 0x510e, 0x00 },
+	{ 0x510f, 0x00 }, { 0x5110, 0x00 }, { 0x5111, 0x80 }, { 0x5112, 0x00 },
+	{ 0x5113, 0x80 }, { 0x5114, 0x00 }, { 0x5115, 0x80 }, { 0x5116, 0x00 },
+	{ 0x5117, 0x80 }, { 0x5118, 0x00 }, { 0x5119, 0x00 }, { 0x511a, 0x00 },
+	{ 0x511b, 0x00 }, { 0x511c, 0x00 }, { 0x511d, 0x00 }, { 0x511e, 0x00 },
+	{ 0x511f, 0x00 }, { 0x56d0, 0x00 }, { 0x5006, 0x04 }, { 0x5608, 0x05 },
+	{ 0x52d7, 0x06 }, { 0x528d, 0x08 }, { 0x5293, 0x12 }, { 0x52d3, 0x12 },
+	{ 0x5288, 0x06 }, { 0x5289, 0x20 }, { 0x52c8, 0x06 }, { 0x52c9, 0x20 },
+	{ 0x52cd, 0x04 }, { 0x5381, 0x00 }, { 0x5382, 0xff }, { 0x5589, 0x76 },
+	{ 0x558a, 0x47 }, { 0x558b, 0xef }, { 0x558c, 0xc9 }, { 0x558d, 0x49 },
+	{ 0x558e, 0x30 }, { 0x558f, 0x67 }, { 0x5590, 0x3f }, { 0x5591, 0xf0 },
+	{ 0x5592, 0x10 }, { 0x55a2, 0x6d }, { 0x55a3, 0x55 }, { 0x55a4, 0xc3 },
+	{ 0x55a5, 0xb5 }, { 0x55a6, 0x43 }, { 0x55a7, 0x38 }, { 0x55a8, 0x5f },
+	{ 0x55a9, 0x4b }, { 0x55aa, 0xf0 }, { 0x55ab, 0x10 }, { 0x5581, 0x52 },
+	{ 0x5300, 0x01 }, { 0x5301, 0x00 }, { 0x5302, 0x00 }, { 0x5303, 0x0e },
+	{ 0x5304, 0x00 }, { 0x5305, 0x0e }, { 0x5306, 0x00 }, { 0x5307, 0x36 },
+	{ 0x5308, 0x00 }, { 0x5309, 0xd9 }, { 0x530a, 0x00 }, { 0x530b, 0x0f },
+	{ 0x530c, 0x00 }, { 0x530d, 0x2c }, { 0x530e, 0x00 }, { 0x530f, 0x59 },
+	{ 0x5310, 0x00 }, { 0x5311, 0x7b }, { 0x5312, 0x00 }, { 0x5313, 0x22 },
+	{ 0x5314, 0x00 }, { 0x5315, 0xd5 }, { 0x5316, 0x00 }, { 0x5317, 0x13 },
+	{ 0x5318, 0x00 }, { 0x5319, 0x18 }, { 0x531a, 0x00 }, { 0x531b, 0x26 },
+	{ 0x531c, 0x00 }, { 0x531d, 0xdc }, { 0x531e, 0x00 }, { 0x531f, 0x02 },
+	{ 0x5320, 0x00 }, { 0x5321, 0x24 }, { 0x5322, 0x00 }, { 0x5323, 0x56 },
+	{ 0x5324, 0x00 }, { 0x5325, 0x85 }, { 0x5326, 0x00 }, { 0x5327, 0x20 },
+	{ 0x5609, 0x01 }, { 0x560a, 0x40 }, { 0x560b, 0x01 }, { 0x560c, 0x40 },
+	{ 0x560d, 0x00 }, { 0x560e, 0xfa }, { 0x560f, 0x00 }, { 0x5610, 0xfa },
+	{ 0x5611, 0x02 }, { 0x5612, 0x80 }, { 0x5613, 0x02 }, { 0x5614, 0x80 },
+	{ 0x5615, 0x01 }, { 0x5616, 0x2c }, { 0x5617, 0x01 }, { 0x5618, 0x2c },
+	{ 0x563b, 0x01 }, { 0x563c, 0x01 }, { 0x563d, 0x01 }, { 0x563e, 0x01 },
+	{ 0x563f, 0x03 }, { 0x5640, 0x03 }, { 0x5641, 0x03 }, { 0x5642, 0x05 },
+	{ 0x5643, 0x09 }, { 0x5644, 0x05 }, { 0x5645, 0x05 }, { 0x5646, 0x05 },
+	{ 0x5647, 0x05 }, { 0x5651, 0x00 }, { 0x5652, 0x80 }, { 0x521a, 0x01 },
+	{ 0x521b, 0x03 }, { 0x521c, 0x06 }, { 0x521d, 0x0a }, { 0x521e, 0x0e },
+	{ 0x521f, 0x12 }, { 0x5220, 0x16 }, { 0x5223, 0x02 }, { 0x5225, 0x04 },
+	{ 0x5227, 0x08 }, { 0x5229, 0x0c }, { 0x522b, 0x12 }, { 0x522d, 0x18 },
+	{ 0x522f, 0x1e }, { 0x5241, 0x04 }, { 0x5242, 0x01 }, { 0x5243, 0x03 },
+	{ 0x5244, 0x06 }, { 0x5245, 0x0a }, { 0x5246, 0x0e }, { 0x5247, 0x12 },
+	{ 0x5248, 0x16 }, { 0x524a, 0x03 }, { 0x524c, 0x04 }, { 0x524e, 0x08 },
+	{ 0x5250, 0x0c }, { 0x5252, 0x12 }, { 0x5254, 0x18 }, { 0x5256, 0x1e },
+	/* fifo_line_length = 2*hts */
+	{ 0x4606, (2 * OV10635_HTS) >> 8 }, { 0x4607, (2 * OV10635_HTS) & 0xff },
+	/* fifo_hsync_start = 2*(hts - xres) */
+	{ 0x460a, (2 * (OV10635_HTS - OV10635_WIDTH)) >> 8 },
+	{ 0x460b, (2 * (OV10635_HTS - OV10635_WIDTH)) & 0xff },
+	{ 0x460c, 0x00 }, { 0x4620, 0x0e },
+	/* BT601: 0x08 is also acceptable as HS/VS mode */
+	{ 0x4700, 0x04 }, { 0x4701, 0x00 }, { 0x4702, 0x01 }, { 0x4004, 0x04 },
+	{ 0x4005, 0x18 }, { 0x4001, 0x06 }, { 0x4050, 0x22 }, { 0x4051, 0x24 },
+	{ 0x4052, 0x02 }, { 0x4057, 0x9c }, { 0x405a, 0x00 }, { 0x4202, 0x02 },
+	{ 0x3023, 0x10 }, { 0x0100, 0x01 }, { 0x0100, 0x01 }, { 0x6f10, 0x07 },
+	{ 0x6f11, 0x82 }, { 0x6f12, 0x04 }, { 0x6f13, 0x00 }, { 0xd000, 0x19 },
+	{ 0xd001, 0xa0 }, { 0xd002, 0x00 }, { 0xd003, 0x01 }, { 0xd004, 0xa9 },
+	{ 0xd005, 0xad }, { 0xd006, 0x10 }, { 0xd007, 0x40 }, { 0xd008, 0x44 },
+	{ 0xd009, 0x00 }, { 0xd00a, 0x68 }, { 0xd00b, 0x00 }, { 0xd00c, 0x15 },
+	{ 0xd00d, 0x00 }, { 0xd00e, 0x00 }, { 0xd00f, 0x00 }, { 0xd040, 0x9c },
+	{ 0xd041, 0x21 }, { 0xd042, 0xff }, { 0xd043, 0xf8 }, { 0xd044, 0xd4 },
+	{ 0xd045, 0x01 }, { 0xd046, 0x48 }, { 0xd047, 0x00 }, { 0xd048, 0xd4 },
+	{ 0xd049, 0x01 }, { 0xd04a, 0x50 }, { 0xd04b, 0x04 }, { 0xd04c, 0x18 },
+	{ 0xd04d, 0x60 }, { 0xd04e, 0x00 }, { 0xd04f, 0x01 }, { 0xd050, 0xa8 },
+	{ 0xd051, 0x63 }, { 0xd052, 0x02 }, { 0xd053, 0xa4 }, { 0xd054, 0x85 },
+	{ 0xd055, 0x43 }, { 0xd056, 0x00 }, { 0xd057, 0x00 }, { 0xd058, 0x18 },
+	{ 0xd059, 0x60 }, { 0xd05a, 0x00 }, { 0xd05b, 0x01 }, { 0xd05c, 0xa8 },
+	{ 0xd05d, 0x63 }, { 0xd05e, 0x03 }, { 0xd05f, 0xf0 }, { 0xd060, 0x98 },
+	{ 0xd061, 0xa3 }, { 0xd062, 0x00 }, { 0xd063, 0x00 }, { 0xd064, 0x8c },
+	{ 0xd065, 0x6a }, { 0xd066, 0x00 }, { 0xd067, 0x6e }, { 0xd068, 0xe5 },
+	{ 0xd069, 0x85 }, { 0xd06a, 0x18 }, { 0xd06b, 0x00 }, { 0xd06c, 0x10 },
+	{ 0xd06d, 0x00 }, { 0xd06e, 0x00 }, { 0xd06f, 0x10 }, { 0xd070, 0x9c },
+	{ 0xd071, 0x80 }, { 0xd072, 0x00 }, { 0xd073, 0x03 }, { 0xd074, 0x18 },
+	{ 0xd075, 0x60 }, { 0xd076, 0x00 }, { 0xd077, 0x01 }, { 0xd078, 0xa8 },
+	{ 0xd079, 0x63 }, { 0xd07a, 0x07 }, { 0xd07b, 0x80 }, { 0xd07c, 0x07 },
+	{ 0xd07d, 0xff }, { 0xd07e, 0xf9 }, { 0xd07f, 0x03 }, { 0xd080, 0x8c },
+	{ 0xd081, 0x63 }, { 0xd082, 0x00 }, { 0xd083, 0x00 }, { 0xd084, 0xa5 },
+	{ 0xd085, 0x6b }, { 0xd086, 0x00 }, { 0xd087, 0xff }, { 0xd088, 0x18 },
+	{ 0xd089, 0x80 }, { 0xd08a, 0x00 }, { 0xd08b, 0x01 }, { 0xd08c, 0xa8 },
+	{ 0xd08d, 0x84 }, { 0xd08e, 0x01 }, { 0xd08f, 0x04 }, { 0xd090, 0xe1 },
+	{ 0xd091, 0x6b }, { 0xd092, 0x58 }, { 0xd093, 0x00 }, { 0xd094, 0x94 },
+	{ 0xd095, 0x6a }, { 0xd096, 0x00 }, { 0xd097, 0x70 }, { 0xd098, 0xe1 },
+	{ 0xd099, 0x6b }, { 0xd09a, 0x20 }, { 0xd09b, 0x00 }, { 0xd09c, 0x95 },
+	{ 0xd09d, 0x6b }, { 0xd09e, 0x00 }, { 0xd09f, 0x00 }, { 0xd0a0, 0xe4 },
+	{ 0xd0a1, 0x8b }, { 0xd0a2, 0x18 }, { 0xd0a3, 0x00 }, { 0xd0a4, 0x0c },
+	{ 0xd0a5, 0x00 }, { 0xd0a6, 0x00 }, { 0xd0a7, 0x23 }, { 0xd0a8, 0x15 },
+	{ 0xd0a9, 0x00 }, { 0xd0aa, 0x00 }, { 0xd0ab, 0x00 }, { 0xd0ac, 0x18 },
+	{ 0xd0ad, 0x60 }, { 0xd0ae, 0x80 }, { 0xd0af, 0x06 }, { 0xd0b0, 0xa8 },
+	{ 0xd0b1, 0x83 }, { 0xd0b2, 0x40 }, { 0xd0b3, 0x08 }, { 0xd0b4, 0xa8 },
+	{ 0xd0b5, 0xe3 }, { 0xd0b6, 0x38 }, { 0xd0b7, 0x2a }, { 0xd0b8, 0xa8 },
+	{ 0xd0b9, 0xc3 }, { 0xd0ba, 0x40 }, { 0xd0bb, 0x09 }, { 0xd0bc, 0xa8 },
+	{ 0xd0bd, 0xa3 }, { 0xd0be, 0x38 }, { 0xd0bf, 0x29 }, { 0xd0c0, 0x8c },
+	{ 0xd0c1, 0x65 }, { 0xd0c2, 0x00 }, { 0xd0c3, 0x00 }, { 0xd0c4, 0xd8 },
+	{ 0xd0c5, 0x04 }, { 0xd0c6, 0x18 }, { 0xd0c7, 0x00 }, { 0xd0c8, 0x8c },
+	{ 0xd0c9, 0x67 }, { 0xd0ca, 0x00 }, { 0xd0cb, 0x00 }, { 0xd0cc, 0xd8 },
+	{ 0xd0cd, 0x06 }, { 0xd0ce, 0x18 }, { 0xd0cf, 0x00 }, { 0xd0d0, 0x18 },
+	{ 0xd0d1, 0x60 }, { 0xd0d2, 0x80 }, { 0xd0d3, 0x06 }, { 0xd0d4, 0xa8 },
+	{ 0xd0d5, 0xe3 }, { 0xd0d6, 0x67 }, { 0xd0d7, 0x02 }, { 0xd0d8, 0xa9 },
+	{ 0xd0d9, 0x03 }, { 0xd0da, 0x67 }, { 0xd0db, 0x03 }, { 0xd0dc, 0xa8 },
+	{ 0xd0dd, 0xc3 }, { 0xd0de, 0x3d }, { 0xd0df, 0x05 }, { 0xd0e0, 0x8c },
+	{ 0xd0e1, 0x66 }, { 0xd0e2, 0x00 }, { 0xd0e3, 0x00 }, { 0xd0e4, 0xb8 },
+	{ 0xd0e5, 0x63 }, { 0xd0e6, 0x00 }, { 0xd0e7, 0x18 }, { 0xd0e8, 0xb8 },
+	{ 0xd0e9, 0x63 }, { 0xd0ea, 0x00 }, { 0xd0eb, 0x98 }, { 0xd0ec, 0xbc },
+	{ 0xd0ed, 0x03 }, { 0xd0ee, 0x00 }, { 0xd0ef, 0x00 }, { 0xd0f0, 0x10 },
+	{ 0xd0f1, 0x00 }, { 0xd0f2, 0x00 }, { 0xd0f3, 0x16 }, { 0xd0f4, 0xb8 },
+	{ 0xd0f5, 0x83 }, { 0xd0f6, 0x00 }, { 0xd0f7, 0x19 }, { 0xd0f8, 0x8c },
+	{ 0xd0f9, 0x67 }, { 0xd0fa, 0x00 }, { 0xd0fb, 0x00 }, { 0xd0fc, 0xb8 },
+	{ 0xd0fd, 0xa4 }, { 0xd0fe, 0x00 }, { 0xd0ff, 0x98 }, { 0xd100, 0xb8 },
+	{ 0xd101, 0x83 }, { 0xd102, 0x00 }, { 0xd103, 0x08 }, { 0xd104, 0x8c },
+	{ 0xd105, 0x68 }, { 0xd106, 0x00 }, { 0xd107, 0x00 }, { 0xd108, 0xe0 },
+	{ 0xd109, 0x63 }, { 0xd10a, 0x20 }, { 0xd10b, 0x04 }, { 0xd10c, 0xe0 },
+	{ 0xd10d, 0x65 }, { 0xd10e, 0x18 }, { 0xd10f, 0x00 }, { 0xd110, 0xa4 },
+	{ 0xd111, 0x83 }, { 0xd112, 0xff }, { 0xd113, 0xff }, { 0xd114, 0xb8 },
+	{ 0xd115, 0x64 }, { 0xd116, 0x00 }, { 0xd117, 0x48 }, { 0xd118, 0xd8 },
+	{ 0xd119, 0x07 }, { 0xd11a, 0x18 }, { 0xd11b, 0x00 }, { 0xd11c, 0xd8 },
+	{ 0xd11d, 0x08 }, { 0xd11e, 0x20 }, { 0xd11f, 0x00 }, { 0xd120, 0x9c },
+	{ 0xd121, 0x60 }, { 0xd122, 0x00 }, { 0xd123, 0x00 }, { 0xd124, 0xd8 },
+	{ 0xd125, 0x06 }, { 0xd126, 0x18 }, { 0xd127, 0x00 }, { 0xd128, 0x00 },
+	{ 0xd129, 0x00 }, { 0xd12a, 0x00 }, { 0xd12b, 0x08 }, { 0xd12c, 0x15 },
+	{ 0xd12d, 0x00 }, { 0xd12e, 0x00 }, { 0xd12f, 0x00 }, { 0xd130, 0x8c },
+	{ 0xd131, 0x6a }, { 0xd132, 0x00 }, { 0xd133, 0x76 }, { 0xd134, 0xbc },
+	{ 0xd135, 0x23 }, { 0xd136, 0x00 }, { 0xd137, 0x00 }, { 0xd138, 0x13 },
+	{ 0xd139, 0xff }, { 0xd13a, 0xff }, { 0xd13b, 0xe6 }, { 0xd13c, 0x18 },
+	{ 0xd13d, 0x60 }, { 0xd13e, 0x80 }, { 0xd13f, 0x06 }, { 0xd140, 0x03 },
+	{ 0xd141, 0xff }, { 0xd142, 0xff }, { 0xd143, 0xdd }, { 0xd144, 0xa8 },
+	{ 0xd145, 0x83 }, { 0xd146, 0x40 }, { 0xd147, 0x08 }, { 0xd148, 0x85 },
+	{ 0xd149, 0x21 }, { 0xd14a, 0x00 }, { 0xd14b, 0x00 }, { 0xd14c, 0x85 },
+	{ 0xd14d, 0x41 }, { 0xd14e, 0x00 }, { 0xd14f, 0x04 }, { 0xd150, 0x44 },
+	{ 0xd151, 0x00 }, { 0xd152, 0x48 }, { 0xd153, 0x00 }, { 0xd154, 0x9c },
+	{ 0xd155, 0x21 }, { 0xd156, 0x00 }, { 0xd157, 0x08 }, { 0x6f0e, 0x03 },
+	{ 0x6f0f, 0x00 }, { 0x460e, 0x08 }, { 0x460f, 0x01 }, { 0x4610, 0x00 },
+	{ 0x4611, 0x01 }, { 0x4612, 0x00 }, { 0x4613, 0x01 },
+	/* 8 bits */
+	{ 0x4605, 0x08 },
+	/* Swap data bits order [9:0] -> [0:9] */
+	{ 0x4709, 0x10 }, { 0x4608, 0x00 }, { 0x4609, 0x08 }, { 0x6804, 0x00 },
+	{ 0x6805, 0x06 }, { 0x6806, 0x00 }, { 0x5120, 0x00 }, { 0x3510, 0x00 },
+	{ 0x3504, 0x00 }, { 0x6800, 0x00 }, { 0x6f0d, 0x01 },
+	/* PCLK falling edge */
+	{ 0x4708, 0x01 }, { 0x5000, 0xff }, { 0x5001, 0xbf }, { 0x5002, 0x7e },
+	{ 0x503d, 0x00 }, { 0xc450, 0x01 }, { 0xc452, 0x04 }, { 0xc453, 0x00 },
+	{ 0xc454, 0x00 }, { 0xc455, 0x01 }, { 0xc456, 0x01 }, { 0xc457, 0x00 },
+	{ 0xc458, 0x00 }, { 0xc459, 0x00 }, { 0xc45b, 0x00 }, { 0xc45c, 0x01 },
+	{ 0xc45d, 0x00 }, { 0xc45e, 0x00 }, { 0xc45f, 0x00 }, { 0xc460, 0x00 },
+	{ 0xc461, 0x01 }, { 0xc462, 0x01 }, { 0xc464, 0x03 }, { 0xc465, 0x00 },
+	{ 0xc466, 0x8a }, { 0xc467, 0x00 }, { 0xc468, 0x86 }, { 0xc469, 0x00 },
+	{ 0xc46a, 0x40 }, { 0xc46b, 0x50 }, { 0xc46c, 0x30 }, { 0xc46d, 0x28 },
+	{ 0xc46e, 0x60 }, { 0xc46f, 0x40 }, { 0xc47c, 0x01 }, { 0xc47d, 0x38 },
+	{ 0xc47e, 0x00 }, { 0xc47f, 0x00 }, { 0xc480, 0x00 }, { 0xc481, 0xff },
+	{ 0xc482, 0x00 }, { 0xc483, 0x40 }, { 0xc484, 0x00 }, { 0xc485, 0x18 },
+	{ 0xc486, 0x00 }, { 0xc487, 0x18 },
+	{ 0xc488, (OV10635_VTS - 8) * 16 >> 8},
+	{ 0xc489, (OV10635_VTS - 8) * 16 & 0xff},
+	{ 0xc48a, (OV10635_VTS - 8) * 16 >> 8},
+	{ 0xc48b, (OV10635_VTS - 8) * 16 & 0xff}, { 0xc48c, 0x00 },
+	{ 0xc48d, 0x04 }, { 0xc48e, 0x00 }, { 0xc48f, 0x04 }, { 0xc490, 0x03 },
+	{ 0xc492, 0x20 }, { 0xc493, 0x08 }, { 0xc498, 0x02 }, { 0xc499, 0x00 },
+	{ 0xc49a, 0x02 }, { 0xc49b, 0x00 }, { 0xc49c, 0x02 }, { 0xc49d, 0x00 },
+	{ 0xc49e, 0x02 }, { 0xc49f, 0x60 }, { 0xc4a0, 0x03 }, { 0xc4a1, 0x00 },
+	{ 0xc4a2, 0x04 }, { 0xc4a3, 0x00 }, { 0xc4a4, 0x00 }, { 0xc4a5, 0x10 },
+	{ 0xc4a6, 0x00 }, { 0xc4a7, 0x40 }, { 0xc4a8, 0x00 }, { 0xc4a9, 0x80 },
+	{ 0xc4aa, 0x0d }, { 0xc4ab, 0x00 }, { 0xc4ac, 0x0f }, { 0xc4ad, 0xc0 },
+	{ 0xc4b4, 0x01 }, { 0xc4b5, 0x01 }, { 0xc4b6, 0x00 }, { 0xc4b7, 0x01 },
+	{ 0xc4b8, 0x00 }, { 0xc4b9, 0x01 }, { 0xc4ba, 0x01 }, { 0xc4bb, 0x00 },
+	{ 0xc4bc, 0x01 }, { 0xc4bd, 0x60 }, { 0xc4be, 0x02 }, { 0xc4bf, 0x33 },
+	{ 0xc4c8, 0x03 }, { 0xc4c9, 0xd0 }, { 0xc4ca, 0x0e }, { 0xc4cb, 0x00 },
+	{ 0xc4cc, 0x0e }, { 0xc4cd, 0x51 }, { 0xc4ce, 0x0e }, { 0xc4cf, 0x51 },
+	{ 0xc4d0, 0x04 }, { 0xc4d1, 0x80 }, { 0xc4e0, 0x04 }, { 0xc4e1, 0x02 },
+	{ 0xc4e2, 0x01 }, { 0xc4e4, 0x10 }, { 0xc4e5, 0x20 }, { 0xc4e6, 0x30 },
+	{ 0xc4e7, 0x40 }, { 0xc4e8, 0x50 }, { 0xc4e9, 0x60 }, { 0xc4ea, 0x70 },
+	{ 0xc4eb, 0x80 }, { 0xc4ec, 0x90 }, { 0xc4ed, 0xa0 }, { 0xc4ee, 0xb0 },
+	{ 0xc4ef, 0xc0 }, { 0xc4f0, 0xd0 }, { 0xc4f1, 0xe0 }, { 0xc4f2, 0xf0 },
+	{ 0xc4f3, 0x80 }, { 0xc4f4, 0x00 }, { 0xc4f5, 0x20 }, { 0xc4f6, 0x02 },
+	{ 0xc4f7, 0x00 }, { 0xc4f8, 0x00 }, { 0xc4f9, 0x00 }, { 0xc4fa, 0x00 },
+	{ 0xc4fb, 0x01 }, { 0xc4fc, 0x01 }, { 0xc4fd, 0x00 }, { 0xc4fe, 0x04 },
+	{ 0xc4ff, 0x02 }, { 0xc500, 0x48 }, { 0xc501, 0x74 }, { 0xc502, 0x58 },
+	{ 0xc503, 0x80 }, { 0xc504, 0x05 }, { 0xc505, 0x80 }, { 0xc506, 0x03 },
+	{ 0xc507, 0x80 }, { 0xc508, 0x01 }, { 0xc509, 0xc0 }, { 0xc50a, 0x01 },
+	{ 0xc50b, 0xa0 }, { 0xc50c, 0x01 }, { 0xc50d, 0x2c }, { 0xc50e, 0x01 },
+	{ 0xc50f, 0x0a }, { 0xc510, 0x00 }, { 0xc511, 0x00 }, { 0xc512, 0xe5 },
+	{ 0xc513, 0x14 }, { 0xc514, 0x04 }, { 0xc515, 0x00 }, { 0xc518, OV10635_VTS >> 8},
+	{ 0xc519, OV10635_VTS & 0xff}, { 0xc51a, OV10635_HTS >> 8},
+	{ 0xc51b, OV10635_HTS & 0xff}, { 0xc2e0, 0x00 }, { 0xc2e1, 0x51 },
+	{ 0xc2e2, 0x00 }, { 0xc2e3, 0xd6 }, { 0xc2e4, 0x01 }, { 0xc2e5, 0x5e },
+	{ 0xc2e9, 0x01 }, { 0xc2ea, 0x7a }, { 0xc2eb, 0x90 }, { 0xc2ed, 0x00 },
+	{ 0xc2ee, 0x7a }, { 0xc2ef, 0x64 }, { 0xc308, 0x00 }, { 0xc309, 0x00 },
+	{ 0xc30a, 0x00 }, { 0xc30c, 0x00 }, { 0xc30d, 0x01 }, { 0xc30e, 0x00 },
+	{ 0xc30f, 0x00 }, { 0xc310, 0x01 }, { 0xc311, 0x60 }, { 0xc312, 0xff },
+	{ 0xc313, 0x08 }, { 0xc314, 0x01 }, { 0xc315, 0x00 }, { 0xc316, 0xff },
+	{ 0xc317, 0x0b }, { 0xc318, 0x00 }, { 0xc319, 0x0c }, { 0xc31a, 0x00 },
+	{ 0xc31b, 0xe0 }, { 0xc31c, 0x00 }, { 0xc31d, 0x14 }, { 0xc31e, 0x00 },
+	{ 0xc31f, 0xc5 }, { 0xc320, 0xff }, { 0xc321, 0x4b }, { 0xc322, 0xff },
+	{ 0xc323, 0xf0 }, { 0xc324, 0xff }, { 0xc325, 0xe8 }, { 0xc326, 0x00 },
+	{ 0xc327, 0x46 }, { 0xc328, 0xff }, { 0xc329, 0xd2 }, { 0xc32a, 0xff },
+	{ 0xc32b, 0xe4 }, { 0xc32c, 0xff }, { 0xc32d, 0xbb }, { 0xc32e, 0x00 },
+	{ 0xc32f, 0x61 }, { 0xc330, 0xff }, { 0xc331, 0xf9 }, { 0xc332, 0x00 },
+	{ 0xc333, 0xd9 }, { 0xc334, 0x00 }, { 0xc335, 0x2e }, { 0xc336, 0x00 },
+	{ 0xc337, 0xb1 }, { 0xc338, 0xff }, { 0xc339, 0x64 }, { 0xc33a, 0xff },
+	{ 0xc33b, 0xeb }, { 0xc33c, 0xff }, { 0xc33d, 0xe8 }, { 0xc33e, 0x00 },
+	{ 0xc33f, 0x48 }, { 0xc340, 0xff }, { 0xc341, 0xd0 }, { 0xc342, 0xff },
+	{ 0xc343, 0xed }, { 0xc344, 0xff }, { 0xc345, 0xad }, { 0xc346, 0x00 },
+	{ 0xc347, 0x66 }, { 0xc348, 0x01 }, { 0xc349, 0x00 }, { 0x6700, 0x04 },
+	{ 0x6701, 0x7b }, { 0x6702, 0xfd }, { 0x6703, 0xf9 }, { 0x6704, 0x3d },
+	{ 0x6705, 0x71 }, { 0x6706, 0x78 }, { 0x6708, 0x05 }, { 0x6f06, 0x6f },
+	{ 0x6f07, 0x00 }, { 0x6f0a, 0x6f }, { 0x6f0b, 0x00 }, { 0x6f00, 0x03 },
+	{ 0xc34c, 0x01 }, { 0xc34d, 0x00 }, { 0xc34e, 0x46 }, { 0xc34f, 0x55 },
+	{ 0xc350, 0x00 }, { 0xc351, 0x40 }, { 0xc352, 0x00 }, { 0xc353, 0xff },
+	{ 0xc354, 0x04 }, { 0xc355, 0x08 }, { 0xc356, 0x01 }, { 0xc357, 0xef },
+	{ 0xc358, 0x30 }, { 0xc359, 0x01 }, { 0xc35a, 0x64 }, { 0xc35b, 0x46 },
+	{ 0xc35c, 0x00 }, { 0x3042, 0xf0 }, { 0x3042, 0xf0 }, { 0x3042, 0xf0 },
+	{ 0x3042, 0xf0 }, { 0x3042, 0xf0 }, { 0x3042, 0xf0 }, { 0x3042, 0xf0 },
+	{ 0x3042, 0xf0 }, { 0x3042, 0xf0 }, { 0x3042, 0xf0 }, { 0x3042, 0xf0 },
+	{ 0x3042, 0xf0 }, { 0x3042, 0xf0 }, { 0x3042, 0xf0 }, { 0x3042, 0xf0 },
+	{ 0x3042, 0xf0 }, { 0x3042, 0xf0 }, { 0x3042, 0xf0 }, { 0x3042, 0xf0 },
+	{ 0x3042, 0xf0 }, { 0x3042, 0xf0 }, { 0x3042, 0xf0 }, { 0x3042, 0xf0 },
+	{ 0x3042, 0xf0 }, { 0x3042, 0xf0 }, { 0x3042, 0xf0 }, { 0xc261, 0x01 },
+	{ 0x301b, 0xf0 }, { 0x301c, 0xf0 }, { 0x301a, 0xf0 }, { 0x6f00, 0xc3 },
+	{ 0xc46a, 0x30 }, { 0xc46d, 0x20 }, { 0xc464, 0x84 }, { 0xc465, 0x00 },
+	{ 0x6f00, 0x03 }, { 0x6f00, 0x43 }, { 0x381c, 0x00 }, { 0x381d, 0x40 },
+	{ 0xc454, 0x01 }, { 0x6f00, 0xc3 }, { 0xc454, 0x00 }, { 0xc4b1, 0x02 },
+	{ 0xc4b2, 0x01 }, { 0xc4b3, 0x03 }, { 0x6f00, 0x03 }, { 0x6f00, 0x43 },
+	/* enable FSIN (FRAMESYNC input) functionality */
+	{ 0x3832, (0x0d + 2 * 0x20 + 0x15 + 38) >> 8 },
+	{ 0x3833, (0x0d + 2 * 0x20 + 0x15 + 38) & 0xff },
+	{ 0x3834, OV10635_VTS >> 8 }, { 0x3835, OV10635_VTS & 0xff },
+	{ 0x302e, 0x01 },
+};
+
+struct rdacm20_device {
+	struct device			*dev;
+	struct max9271_device		serializer;
+	struct i2c_client		*sensor;
+	struct v4l2_subdev		sd;
+	struct media_pad		pad;
+	struct v4l2_ctrl_handler	ctrls;
+	u32				addrs[2];
+};
+
+static inline struct rdacm20_device *sd_to_rdacm20(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct rdacm20_device, sd);
+}
+
+static inline struct rdacm20_device *i2c_to_rdacm20(struct i2c_client *client)
+{
+	return sd_to_rdacm20(i2c_get_clientdata(client));
+}
+
+static int ov10635_read16(struct rdacm20_device *dev, u16 reg)
+{
+	u8 buf[2] = { reg >> 8, reg & 0xff };
+	int ret;
+
+	ret = i2c_master_send(dev->sensor, buf, 2);
+	if (ret != 2) {
+		dev_dbg(dev->dev, "%s: register 0x%04x write failed (%d)\n",
+			__func__, reg, ret);
+		return ret;
+	}
+
+	ret = i2c_master_recv(dev->sensor, buf, 2);
+	if (ret < 0) {
+		dev_dbg(dev->dev, "%s: register 0x%04x read failed (%d)\n",
+			__func__, reg, ret);
+		return ret;
+	}
+
+	return (buf[0] << 8) | buf[1];
+}
+
+static int __ov10635_write(struct rdacm20_device *dev, u16 reg, u8 val)
+{
+	u8 buf[3] = { reg >> 8, reg & 0xff, val };
+	int ret;
+
+	dev_dbg(dev->dev, "%s(0x%04x, 0x%02x)\n", __func__, reg, val);
+
+	ret = i2c_master_send(dev->sensor, buf, 3);
+	return ret < 0 ? ret : 0;
+}
+
+static int ov10635_write(struct rdacm20_device *dev, u16 reg, u8 val)
+{
+	int ret;
+
+	ret = __ov10635_write(dev, reg, val);
+	if (ret < 0)
+		dev_err(dev->dev, "%s: register 0x%04x write failed (%d)\n",
+			__func__, reg, ret);
+
+	return ret;
+}
+
+static int ov10635_set_regs(struct rdacm20_device *dev,
+			    const struct ov10635_reg *regs,
+			    unsigned int nr_regs)
+{
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < nr_regs; i++) {
+		ret = __ov10635_write(dev, regs[i].reg, regs[i].val);
+		if (ret) {
+			dev_err(dev->dev,
+				"%s: register %u (0x%04x) write failed (%d)\n",
+				__func__, i, regs[i].reg, ret);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int rdacm20_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct rdacm20_device *dev = sd_to_rdacm20(sd);
+
+	return max9271_set_serial_link(&dev->serializer, enable);
+}
+
+static int rdacm20_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->pad || code->index > 0)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_UYVY8_1X16;
+
+	return 0;
+}
+
+static int rdacm20_get_fmt(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_state *sd_state,
+			   struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *mf = &format->format;
+
+	if (format->pad)
+		return -EINVAL;
+
+	mf->width		= OV10635_WIDTH;
+	mf->height		= OV10635_HEIGHT;
+	mf->code		= MEDIA_BUS_FMT_UYVY8_1X16;
+	mf->colorspace		= V4L2_COLORSPACE_RAW;
+	mf->field		= V4L2_FIELD_NONE;
+	mf->ycbcr_enc		= V4L2_YCBCR_ENC_601;
+	mf->quantization	= V4L2_QUANTIZATION_FULL_RANGE;
+	mf->xfer_func		= V4L2_XFER_FUNC_NONE;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops rdacm20_video_ops = {
+	.s_stream	= rdacm20_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops rdacm20_subdev_pad_ops = {
+	.enum_mbus_code = rdacm20_enum_mbus_code,
+	.get_fmt	= rdacm20_get_fmt,
+	.set_fmt	= rdacm20_get_fmt,
+};
+
+static const struct v4l2_subdev_ops rdacm20_subdev_ops = {
+	.video		= &rdacm20_video_ops,
+	.pad		= &rdacm20_subdev_pad_ops,
+};
+
+static int rdacm20_initialize(struct rdacm20_device *dev)
+{
+	unsigned int retry = 3;
+	int ret;
+
+	max9271_wake_up(&dev->serializer);
+
+	/* Serial link disabled during config as it needs a valid pixel clock. */
+	ret = max9271_set_serial_link(&dev->serializer, false);
+	if (ret)
+		return ret;
+
+	/*
+	 *  Ensure that we have a good link configuration before attempting to
+	 *  identify the device.
+	 */
+	ret = max9271_configure_i2c(&dev->serializer,
+				    MAX9271_I2CSLVSH_469NS_234NS |
+				    MAX9271_I2CSLVTO_1024US |
+				    MAX9271_I2CMSTBT_105KBPS);
+	if (ret)
+		return ret;
+
+	/*
+	 * Hold OV10635 in reset during max9271 configuration. The reset signal
+	 * has to be asserted for at least 200 microseconds.
+	 */
+	ret = max9271_enable_gpios(&dev->serializer, MAX9271_GPIO1OUT);
+	if (ret)
+		return ret;
+
+	ret = max9271_clear_gpios(&dev->serializer, MAX9271_GPIO1OUT);
+	if (ret)
+		return ret;
+	usleep_range(200, 500);
+
+	ret = max9271_configure_gmsl_link(&dev->serializer);
+	if (ret)
+		return ret;
+
+	ret = max9271_verify_id(&dev->serializer);
+	if (ret < 0)
+		return ret;
+
+	ret = max9271_set_address(&dev->serializer, dev->addrs[0]);
+	if (ret < 0)
+		return ret;
+	dev->serializer.client->addr = dev->addrs[0];
+
+	/*
+	 * Release ov10635 from reset and initialize it. The image sensor
+	 * requires at least 2048 XVCLK cycles (85 micro-seconds at 24MHz)
+	 * before being available. Stay safe and wait up to 500 micro-seconds.
+	 */
+	ret = max9271_set_gpios(&dev->serializer, MAX9271_GPIO1OUT);
+	if (ret)
+		return ret;
+	usleep_range(100, 500);
+
+again:
+	ret = ov10635_read16(dev, OV10635_PID);
+	if (ret < 0) {
+		if (retry--)
+			goto again;
+
+		dev_err(dev->dev, "OV10635 ID read failed (%d)\n",
+			ret);
+		return -ENXIO;
+	}
+
+	if (ret != OV10635_VERSION) {
+		if (retry--)
+			goto again;
+
+		dev_err(dev->dev, "OV10635 ID mismatch (0x%04x)\n",
+			ret);
+		return -ENXIO;
+	}
+
+	/* Change the sensor I2C address. */
+	ret = ov10635_write(dev, OV10635_SC_CMMN_SCCB_ID,
+			    (dev->addrs[1] << 1) |
+			    OV10635_SC_CMMN_SCCB_ID_SELECT);
+	if (ret < 0) {
+		dev_err(dev->dev,
+			"OV10635 I2C address change failed (%d)\n", ret);
+		return ret;
+	}
+	dev->sensor->addr = dev->addrs[1];
+	usleep_range(3500, 5000);
+
+	/* Program the 0V10635 initial configuration. */
+	ret = ov10635_set_regs(dev, ov10635_regs_wizard,
+			       ARRAY_SIZE(ov10635_regs_wizard));
+	if (ret)
+		return ret;
+
+	dev_info(dev->dev, "Identified RDACM20 camera module\n");
+
+	/*
+	 * Set reverse channel high threshold to increase noise immunity.
+	 *
+	 * This should be compensated by increasing the reverse channel
+	 * amplitude on the remote deserializer side.
+	 *
+	 * TODO Inspect the embedded MCU programming sequence to make sure
+	 * there are no conflicts with the configuration applied here.
+	 *
+	 * TODO Clarify the embedded MCU startup delay to avoid write
+	 * collisions on the I2C bus.
+	 */
+	return max9271_set_high_threshold(&dev->serializer, true);
+}
+
+static int rdacm20_probe(struct i2c_client *client)
+{
+	struct rdacm20_device *dev;
+	int ret;
+
+	dev = devm_kzalloc(&client->dev, sizeof(*dev), GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
+	dev->dev = &client->dev;
+	dev->serializer.client = client;
+
+	ret = of_property_read_u32_array(client->dev.of_node, "reg",
+					 dev->addrs, 2);
+	if (ret < 0) {
+		dev_err(dev->dev, "Invalid DT reg property: %d\n", ret);
+		return -EINVAL;
+	}
+
+	/* Create the dummy I2C client for the sensor. */
+	dev->sensor = i2c_new_dummy_device(client->adapter,
+					   OV10635_I2C_ADDRESS);
+	if (IS_ERR(dev->sensor)) {
+		ret = PTR_ERR(dev->sensor);
+		goto error;
+	}
+
+	/* Initialize the hardware. */
+	ret = rdacm20_initialize(dev);
+	if (ret < 0)
+		goto error;
+
+	/* Initialize and register the subdevice. */
+	v4l2_i2c_subdev_init(&dev->sd, client, &rdacm20_subdev_ops);
+	dev->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	v4l2_ctrl_handler_init(&dev->ctrls, 1);
+	v4l2_ctrl_new_std(&dev->ctrls, NULL, V4L2_CID_PIXEL_RATE,
+			  OV10635_PIXEL_RATE, OV10635_PIXEL_RATE, 1,
+			  OV10635_PIXEL_RATE);
+	dev->sd.ctrl_handler = &dev->ctrls;
+
+	ret = dev->ctrls.error;
+	if (ret)
+		goto error_free_ctrls;
+
+	dev->pad.flags = MEDIA_PAD_FL_SOURCE;
+	dev->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret = media_entity_pads_init(&dev->sd.entity, 1, &dev->pad);
+	if (ret < 0)
+		goto error_free_ctrls;
+
+	ret = v4l2_async_register_subdev(&dev->sd);
+	if (ret)
+		goto error_free_ctrls;
+
+	return 0;
+
+error_free_ctrls:
+	v4l2_ctrl_handler_free(&dev->ctrls);
+error:
+	media_entity_cleanup(&dev->sd.entity);
+	if (dev->sensor)
+		i2c_unregister_device(dev->sensor);
+
+	dev_err(&client->dev, "probe failed\n");
+
+	return ret;
+}
+
+static void rdacm20_remove(struct i2c_client *client)
+{
+	struct rdacm20_device *dev = i2c_to_rdacm20(client);
+
+	v4l2_async_unregister_subdev(&dev->sd);
+	v4l2_ctrl_handler_free(&dev->ctrls);
+	media_entity_cleanup(&dev->sd.entity);
+	i2c_unregister_device(dev->sensor);
+}
+
+static void rdacm20_shutdown(struct i2c_client *client)
+{
+	struct rdacm20_device *dev = i2c_to_rdacm20(client);
+
+	/* make sure stream off during shutdown (reset/reboot) */
+	rdacm20_s_stream(&dev->sd, 0);
+}
+
+static const struct of_device_id rdacm20_of_ids[] = {
+	{ .compatible = "imi,rdacm20", },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, rdacm20_of_ids);
+
+static struct i2c_driver rdacm20_i2c_driver = {
+	.driver	= {
+		.name	= "rdacm20",
+		.of_match_table = rdacm20_of_ids,
+	},
+	.probe		= rdacm20_probe,
+	.remove		= rdacm20_remove,
+	.shutdown	= rdacm20_shutdown,
+};
+
+module_i2c_driver(rdacm20_i2c_driver);
+
+MODULE_DESCRIPTION("GMSL Camera driver for RDACM20");
+MODULE_AUTHOR("Vladimir Barinov");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/rdacm21.c b/drivers/media/i2c/rdacm21.c
new file mode 100644
index 000000000..3e22df363
--- /dev/null
+++ b/drivers/media/i2c/rdacm21.c
@@ -0,0 +1,632 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * IMI RDACM21 GMSL Camera Driver
+ *
+ * Copyright (C) 2017-2020 Jacopo Mondi
+ * Copyright (C) 2017-2019 Kieran Bingham
+ * Copyright (C) 2017-2019 Laurent Pinchart
+ * Copyright (C) 2017-2019 Niklas Söderlund
+ * Copyright (C) 2016 Renesas Electronics Corporation
+ * Copyright (C) 2015 Cogent Embedded, Inc.
+ */
+
+#include <linux/delay.h>
+#include <linux/fwnode.h>
+#include <linux/init.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+
+#include <media/v4l2-async.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-subdev.h>
+#include "max9271.h"
+
+#define MAX9271_RESET_CYCLES		10
+
+#define OV490_I2C_ADDRESS		0x24
+
+#define OV490_PAGE_HIGH_REG		0xfffd
+#define OV490_PAGE_LOW_REG		0xfffe
+
+/*
+ * The SCCB slave handling is undocumented; the registers naming scheme is
+ * totally arbitrary.
+ */
+#define OV490_SCCB_SLAVE_WRITE		0x00
+#define OV490_SCCB_SLAVE_READ		0x01
+#define OV490_SCCB_SLAVE0_DIR		0x80195000
+#define OV490_SCCB_SLAVE0_ADDR_HIGH	0x80195001
+#define OV490_SCCB_SLAVE0_ADDR_LOW	0x80195002
+
+#define OV490_DVP_CTRL3			0x80286009
+
+#define OV490_ODS_CTRL_FRAME_OUTPUT_EN	0x0c
+#define OV490_ODS_CTRL			0x8029d000
+
+#define OV490_HOST_CMD			0x808000c0
+#define OV490_HOST_CMD_TRIGGER		0xc1
+
+#define OV490_ID_VAL			0x0490
+#define OV490_ID(_p, _v)		((((_p) & 0xff) << 8) | ((_v) & 0xff))
+#define OV490_PID			0x8080300a
+#define OV490_VER			0x8080300b
+#define OV490_PID_TIMEOUT		20
+#define OV490_OUTPUT_EN_TIMEOUT		300
+
+#define OV490_GPIO0			BIT(0)
+#define OV490_SPWDN0			BIT(0)
+#define OV490_GPIO_SEL0			0x80800050
+#define OV490_GPIO_SEL1			0x80800051
+#define OV490_GPIO_DIRECTION0		0x80800054
+#define OV490_GPIO_DIRECTION1		0x80800055
+#define OV490_GPIO_OUTPUT_VALUE0	0x80800058
+#define OV490_GPIO_OUTPUT_VALUE1	0x80800059
+
+#define OV490_ISP_HSIZE_LOW		0x80820060
+#define OV490_ISP_HSIZE_HIGH		0x80820061
+#define OV490_ISP_VSIZE_LOW		0x80820062
+#define OV490_ISP_VSIZE_HIGH		0x80820063
+
+#define OV10640_PID_TIMEOUT		20
+#define OV10640_ID_HIGH			0xa6
+#define OV10640_CHIP_ID			0x300a
+#define OV10640_PIXEL_RATE		55000000
+
+struct rdacm21_device {
+	struct device			*dev;
+	struct max9271_device		serializer;
+	struct i2c_client		*isp;
+	struct v4l2_subdev		sd;
+	struct media_pad		pad;
+	struct v4l2_mbus_framefmt	fmt;
+	struct v4l2_ctrl_handler	ctrls;
+	u32				addrs[2];
+	u16				last_page;
+};
+
+static inline struct rdacm21_device *sd_to_rdacm21(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct rdacm21_device, sd);
+}
+
+static const struct ov490_reg {
+	u16 reg;
+	u8 val;
+} ov490_regs_wizard[] = {
+	{0xfffd, 0x80},
+	{0xfffe, 0x82},
+	{0x0071, 0x11},
+	{0x0075, 0x11},
+	{0xfffe, 0x29},
+	{0x6010, 0x01},
+	/*
+	 * OV490 EMB line disable in YUV and RAW data,
+	 * NOTE: EMB line is still used in ISP and sensor
+	 */
+	{0xe000, 0x14},
+	{0xfffe, 0x28},
+	{0x6000, 0x04},
+	{0x6004, 0x00},
+	/*
+	 * PCLK polarity - useless due to silicon bug.
+	 * Use 0x808000bb register instead.
+	 */
+	{0x6008, 0x00},
+	{0xfffe, 0x80},
+	{0x0091, 0x00},
+	/* bit[3]=0 - PCLK polarity workaround. */
+	{0x00bb, 0x1d},
+	/* Ov490 FSIN: app_fsin_from_fsync */
+	{0xfffe, 0x85},
+	{0x0008, 0x00},
+	{0x0009, 0x01},
+	/* FSIN0 source. */
+	{0x000A, 0x05},
+	{0x000B, 0x00},
+	/* FSIN0 delay. */
+	{0x0030, 0x02},
+	{0x0031, 0x00},
+	{0x0032, 0x00},
+	{0x0033, 0x00},
+	/* FSIN1 delay. */
+	{0x0038, 0x02},
+	{0x0039, 0x00},
+	{0x003A, 0x00},
+	{0x003B, 0x00},
+	/* FSIN0 length. */
+	{0x0070, 0x2C},
+	{0x0071, 0x01},
+	{0x0072, 0x00},
+	{0x0073, 0x00},
+	/* FSIN1 length. */
+	{0x0074, 0x64},
+	{0x0075, 0x00},
+	{0x0076, 0x00},
+	{0x0077, 0x00},
+	{0x0000, 0x14},
+	{0x0001, 0x00},
+	{0x0002, 0x00},
+	{0x0003, 0x00},
+	/*
+	 * Load fsin0,load fsin1,load other,
+	 * It will be cleared automatically.
+	 */
+	{0x0004, 0x32},
+	{0x0005, 0x00},
+	{0x0006, 0x00},
+	{0x0007, 0x00},
+	{0xfffe, 0x80},
+	/* Sensor FSIN. */
+	{0x0081, 0x00},
+	/* ov10640 FSIN enable */
+	{0xfffe, 0x19},
+	{0x5000, 0x00},
+	{0x5001, 0x30},
+	{0x5002, 0x8c},
+	{0x5003, 0xb2},
+	{0xfffe, 0x80},
+	{0x00c0, 0xc1},
+	/* ov10640 HFLIP=1 by default */
+	{0xfffe, 0x19},
+	{0x5000, 0x01},
+	{0x5001, 0x00},
+	{0xfffe, 0x80},
+	{0x00c0, 0xdc},
+};
+
+static int ov490_read(struct rdacm21_device *dev, u16 reg, u8 *val)
+{
+	u8 buf[2] = { reg >> 8, reg };
+	int ret;
+
+	ret = i2c_master_send(dev->isp, buf, 2);
+	if (ret == 2)
+		ret = i2c_master_recv(dev->isp, val, 1);
+
+	if (ret < 0) {
+		dev_dbg(dev->dev, "%s: register 0x%04x read failed (%d)\n",
+			__func__, reg, ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ov490_write(struct rdacm21_device *dev, u16 reg, u8 val)
+{
+	u8 buf[3] = { reg >> 8, reg, val };
+	int ret;
+
+	ret = i2c_master_send(dev->isp, buf, 3);
+	if (ret < 0) {
+		dev_err(dev->dev, "%s: register 0x%04x write failed (%d)\n",
+			__func__, reg, ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ov490_set_page(struct rdacm21_device *dev, u16 page)
+{
+	u8 page_high = page >> 8;
+	u8 page_low = page;
+	int ret;
+
+	if (page == dev->last_page)
+		return 0;
+
+	if (page_high != (dev->last_page >> 8)) {
+		ret = ov490_write(dev, OV490_PAGE_HIGH_REG, page_high);
+		if (ret)
+			return ret;
+	}
+
+	if (page_low != (u8)dev->last_page) {
+		ret = ov490_write(dev, OV490_PAGE_LOW_REG, page_low);
+		if (ret)
+			return ret;
+	}
+
+	dev->last_page = page;
+	usleep_range(100, 150);
+
+	return 0;
+}
+
+static int ov490_read_reg(struct rdacm21_device *dev, u32 reg, u8 *val)
+{
+	int ret;
+
+	ret = ov490_set_page(dev, reg >> 16);
+	if (ret)
+		return ret;
+
+	ret = ov490_read(dev, (u16)reg, val);
+	if (ret)
+		return ret;
+
+	dev_dbg(dev->dev, "%s: 0x%08x = 0x%02x\n", __func__, reg, *val);
+
+	return 0;
+}
+
+static int ov490_write_reg(struct rdacm21_device *dev, u32 reg, u8 val)
+{
+	int ret;
+
+	ret = ov490_set_page(dev, reg >> 16);
+	if (ret)
+		return ret;
+
+	ret = ov490_write(dev, (u16)reg, val);
+	if (ret)
+		return ret;
+
+	dev_dbg(dev->dev, "%s: 0x%08x = 0x%02x\n", __func__, reg, val);
+
+	return 0;
+}
+
+static int rdacm21_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct rdacm21_device *dev = sd_to_rdacm21(sd);
+
+	/*
+	 * Enable serial link now that the ISP provides a valid pixel clock
+	 * to start serializing video data on the GMSL link.
+	 */
+	return max9271_set_serial_link(&dev->serializer, enable);
+}
+
+static int rdacm21_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->pad || code->index > 0)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_YUYV8_1X16;
+
+	return 0;
+}
+
+static int rdacm21_get_fmt(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_state *sd_state,
+			   struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *mf = &format->format;
+	struct rdacm21_device *dev = sd_to_rdacm21(sd);
+
+	if (format->pad)
+		return -EINVAL;
+
+	mf->width		= dev->fmt.width;
+	mf->height		= dev->fmt.height;
+	mf->code		= MEDIA_BUS_FMT_YUYV8_1X16;
+	mf->colorspace		= V4L2_COLORSPACE_SRGB;
+	mf->field		= V4L2_FIELD_NONE;
+	mf->ycbcr_enc		= V4L2_YCBCR_ENC_601;
+	mf->quantization	= V4L2_QUANTIZATION_FULL_RANGE;
+	mf->xfer_func		= V4L2_XFER_FUNC_NONE;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops rdacm21_video_ops = {
+	.s_stream	= rdacm21_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops rdacm21_subdev_pad_ops = {
+	.enum_mbus_code = rdacm21_enum_mbus_code,
+	.get_fmt	= rdacm21_get_fmt,
+	.set_fmt	= rdacm21_get_fmt,
+};
+
+static const struct v4l2_subdev_ops rdacm21_subdev_ops = {
+	.video		= &rdacm21_video_ops,
+	.pad		= &rdacm21_subdev_pad_ops,
+};
+
+static void ov10640_power_up(struct rdacm21_device *dev)
+{
+	/* Enable GPIO0#0 (reset) and GPIO1#0 (pwdn) as output lines. */
+	ov490_write_reg(dev, OV490_GPIO_SEL0, OV490_GPIO0);
+	ov490_write_reg(dev, OV490_GPIO_SEL1, OV490_SPWDN0);
+	ov490_write_reg(dev, OV490_GPIO_DIRECTION0, OV490_GPIO0);
+	ov490_write_reg(dev, OV490_GPIO_DIRECTION1, OV490_SPWDN0);
+
+	/* Power up OV10640 and then reset it. */
+	ov490_write_reg(dev, OV490_GPIO_OUTPUT_VALUE1, OV490_SPWDN0);
+	usleep_range(1500, 3000);
+
+	ov490_write_reg(dev, OV490_GPIO_OUTPUT_VALUE0, 0x00);
+	usleep_range(1500, 3000);
+	ov490_write_reg(dev, OV490_GPIO_OUTPUT_VALUE0, OV490_GPIO0);
+	usleep_range(3000, 5000);
+}
+
+static int ov10640_check_id(struct rdacm21_device *dev)
+{
+	unsigned int i;
+	u8 val = 0;
+
+	/* Read OV10640 ID to test communications. */
+	for (i = 0; i < OV10640_PID_TIMEOUT; ++i) {
+		ov490_write_reg(dev, OV490_SCCB_SLAVE0_DIR,
+				OV490_SCCB_SLAVE_READ);
+		ov490_write_reg(dev, OV490_SCCB_SLAVE0_ADDR_HIGH,
+				OV10640_CHIP_ID >> 8);
+		ov490_write_reg(dev, OV490_SCCB_SLAVE0_ADDR_LOW,
+				OV10640_CHIP_ID & 0xff);
+
+		/*
+		 * Trigger SCCB slave transaction and give it some time
+		 * to complete.
+		 */
+		ov490_write_reg(dev, OV490_HOST_CMD, OV490_HOST_CMD_TRIGGER);
+		usleep_range(1000, 1500);
+
+		ov490_read_reg(dev, OV490_SCCB_SLAVE0_DIR, &val);
+		if (val == OV10640_ID_HIGH)
+			break;
+		usleep_range(1000, 1500);
+	}
+	if (i == OV10640_PID_TIMEOUT) {
+		dev_err(dev->dev, "OV10640 ID mismatch: (0x%02x)\n", val);
+		return -ENODEV;
+	}
+
+	dev_dbg(dev->dev, "OV10640 ID = 0x%2x\n", val);
+
+	return 0;
+}
+
+static int ov490_initialize(struct rdacm21_device *dev)
+{
+	u8 pid, ver, val;
+	unsigned int i;
+	int ret;
+
+	ov10640_power_up(dev);
+
+	/*
+	 * Read OV490 Id to test communications. Give it up to 40msec to
+	 * exit from reset.
+	 */
+	for (i = 0; i < OV490_PID_TIMEOUT; ++i) {
+		ret = ov490_read_reg(dev, OV490_PID, &pid);
+		if (ret == 0)
+			break;
+		usleep_range(1000, 2000);
+	}
+	if (i == OV490_PID_TIMEOUT) {
+		dev_err(dev->dev, "OV490 PID read failed (%d)\n", ret);
+		return ret;
+	}
+
+	ret = ov490_read_reg(dev, OV490_VER, &ver);
+	if (ret < 0)
+		return ret;
+
+	if (OV490_ID(pid, ver) != OV490_ID_VAL) {
+		dev_err(dev->dev, "OV490 ID mismatch (0x%04x)\n",
+			OV490_ID(pid, ver));
+		return -ENODEV;
+	}
+
+	/* Wait for firmware boot by reading streamon status. */
+	for (i = 0; i < OV490_OUTPUT_EN_TIMEOUT; ++i) {
+		ov490_read_reg(dev, OV490_ODS_CTRL, &val);
+		if (val == OV490_ODS_CTRL_FRAME_OUTPUT_EN)
+			break;
+		usleep_range(1000, 2000);
+	}
+	if (i == OV490_OUTPUT_EN_TIMEOUT) {
+		dev_err(dev->dev, "Timeout waiting for firmware boot\n");
+		return -ENODEV;
+	}
+
+	ret = ov10640_check_id(dev);
+	if (ret)
+		return ret;
+
+	/* Program OV490 with register-value table. */
+	for (i = 0; i < ARRAY_SIZE(ov490_regs_wizard); ++i) {
+		ret = ov490_write(dev, ov490_regs_wizard[i].reg,
+				  ov490_regs_wizard[i].val);
+		if (ret < 0) {
+			dev_err(dev->dev,
+				"%s: register %u (0x%04x) write failed (%d)\n",
+				__func__, i, ov490_regs_wizard[i].reg, ret);
+
+			return -EIO;
+		}
+
+		usleep_range(100, 150);
+	}
+
+	/*
+	 * The ISP is programmed with the content of a serial flash memory.
+	 * Read the firmware configuration to reflect it through the V4L2 APIs.
+	 */
+	ov490_read_reg(dev, OV490_ISP_HSIZE_HIGH, &val);
+	dev->fmt.width = (val & 0xf) << 8;
+	ov490_read_reg(dev, OV490_ISP_HSIZE_LOW, &val);
+	dev->fmt.width |= (val & 0xff);
+
+	ov490_read_reg(dev, OV490_ISP_VSIZE_HIGH, &val);
+	dev->fmt.height = (val & 0xf) << 8;
+	ov490_read_reg(dev, OV490_ISP_VSIZE_LOW, &val);
+	dev->fmt.height |= val & 0xff;
+
+	/* Set bus width to 12 bits with [0:11] ordering. */
+	ov490_write_reg(dev, OV490_DVP_CTRL3, 0x10);
+
+	dev_info(dev->dev, "Identified RDACM21 camera module\n");
+
+	return 0;
+}
+
+static int rdacm21_initialize(struct rdacm21_device *dev)
+{
+	int ret;
+
+	max9271_wake_up(&dev->serializer);
+
+	/* Enable reverse channel and disable the serial link. */
+	ret = max9271_set_serial_link(&dev->serializer, false);
+	if (ret)
+		return ret;
+
+	/* Configure I2C bus at 105Kbps speed and configure GMSL. */
+	ret = max9271_configure_i2c(&dev->serializer,
+				    MAX9271_I2CSLVSH_469NS_234NS |
+				    MAX9271_I2CSLVTO_1024US |
+				    MAX9271_I2CMSTBT_105KBPS);
+	if (ret)
+		return ret;
+
+	ret = max9271_verify_id(&dev->serializer);
+	if (ret)
+		return ret;
+
+	/*
+	 * Enable GPIO1 and hold OV490 in reset during max9271 configuration.
+	 * The reset signal has to be asserted for at least 250 useconds.
+	 */
+	ret = max9271_enable_gpios(&dev->serializer, MAX9271_GPIO1OUT);
+	if (ret)
+		return ret;
+
+	ret = max9271_clear_gpios(&dev->serializer, MAX9271_GPIO1OUT);
+	if (ret)
+		return ret;
+	usleep_range(250, 500);
+
+	ret = max9271_configure_gmsl_link(&dev->serializer);
+	if (ret)
+		return ret;
+
+	ret = max9271_set_address(&dev->serializer, dev->addrs[0]);
+	if (ret)
+		return ret;
+	dev->serializer.client->addr = dev->addrs[0];
+
+	ret = max9271_set_translation(&dev->serializer, dev->addrs[1],
+				      OV490_I2C_ADDRESS);
+	if (ret)
+		return ret;
+	dev->isp->addr = dev->addrs[1];
+
+	/* Release OV490 from reset and initialize it. */
+	ret = max9271_set_gpios(&dev->serializer, MAX9271_GPIO1OUT);
+	if (ret)
+		return ret;
+	usleep_range(3000, 5000);
+
+	ret = ov490_initialize(dev);
+	if (ret)
+		return ret;
+
+	/*
+	 * Set reverse channel high threshold to increase noise immunity.
+	 *
+	 * This should be compensated by increasing the reverse channel
+	 * amplitude on the remote deserializer side.
+	 */
+	return max9271_set_high_threshold(&dev->serializer, true);
+}
+
+static int rdacm21_probe(struct i2c_client *client)
+{
+	struct rdacm21_device *dev;
+	int ret;
+
+	dev = devm_kzalloc(&client->dev, sizeof(*dev), GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
+	dev->dev = &client->dev;
+	dev->serializer.client = client;
+
+	ret = of_property_read_u32_array(client->dev.of_node, "reg",
+					 dev->addrs, 2);
+	if (ret < 0) {
+		dev_err(dev->dev, "Invalid DT reg property: %d\n", ret);
+		return -EINVAL;
+	}
+
+	/* Create the dummy I2C client for the sensor. */
+	dev->isp = i2c_new_dummy_device(client->adapter, OV490_I2C_ADDRESS);
+	if (IS_ERR(dev->isp))
+		return PTR_ERR(dev->isp);
+
+	ret = rdacm21_initialize(dev);
+	if (ret < 0)
+		goto error;
+
+	/* Initialize and register the subdevice. */
+	v4l2_i2c_subdev_init(&dev->sd, client, &rdacm21_subdev_ops);
+	dev->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	v4l2_ctrl_handler_init(&dev->ctrls, 1);
+	v4l2_ctrl_new_std(&dev->ctrls, NULL, V4L2_CID_PIXEL_RATE,
+			  OV10640_PIXEL_RATE, OV10640_PIXEL_RATE, 1,
+			  OV10640_PIXEL_RATE);
+	dev->sd.ctrl_handler = &dev->ctrls;
+
+	ret = dev->ctrls.error;
+	if (ret)
+		goto error_free_ctrls;
+
+	dev->pad.flags = MEDIA_PAD_FL_SOURCE;
+	dev->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret = media_entity_pads_init(&dev->sd.entity, 1, &dev->pad);
+	if (ret < 0)
+		goto error_free_ctrls;
+
+	ret = v4l2_async_register_subdev(&dev->sd);
+	if (ret)
+		goto error_free_ctrls;
+
+	return 0;
+
+error_free_ctrls:
+	v4l2_ctrl_handler_free(&dev->ctrls);
+error:
+	i2c_unregister_device(dev->isp);
+
+	return ret;
+}
+
+static void rdacm21_remove(struct i2c_client *client)
+{
+	struct rdacm21_device *dev = sd_to_rdacm21(i2c_get_clientdata(client));
+
+	v4l2_async_unregister_subdev(&dev->sd);
+	v4l2_ctrl_handler_free(&dev->ctrls);
+	i2c_unregister_device(dev->isp);
+}
+
+static const struct of_device_id rdacm21_of_ids[] = {
+	{ .compatible = "imi,rdacm21" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, rdacm21_of_ids);
+
+static struct i2c_driver rdacm21_i2c_driver = {
+	.driver	= {
+		.name	= "rdacm21",
+		.of_match_table = rdacm21_of_ids,
+	},
+	.probe		= rdacm21_probe,
+	.remove		= rdacm21_remove,
+};
+
+module_i2c_driver(rdacm21_i2c_driver);
+
+MODULE_DESCRIPTION("GMSL Camera driver for RDACM21");
+MODULE_AUTHOR("Jacopo Mondi");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/rj54n1cb0c.c b/drivers/media/i2c/rj54n1cb0c.c
new file mode 100644
index 000000000..b430046f9
--- /dev/null
+++ b/drivers/media/i2c/rj54n1cb0c.c
@@ -0,0 +1,1433 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Driver for RJ54N1CB0C CMOS Image Sensor from Sharp
+ *
+ * Copyright (C) 2018, Jacopo Mondi <jacopo@jmondi.org>
+ *
+ * Copyright (C) 2009, Guennadi Liakhovetski <g.liakhovetski@gmx.de>
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/v4l2-mediabus.h>
+#include <linux/videodev2.h>
+
+#include <media/i2c/rj54n1cb0c.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-subdev.h>
+
+#define RJ54N1_DEV_CODE			0x0400
+#define RJ54N1_DEV_CODE2		0x0401
+#define RJ54N1_OUT_SEL			0x0403
+#define RJ54N1_XY_OUTPUT_SIZE_S_H	0x0404
+#define RJ54N1_X_OUTPUT_SIZE_S_L	0x0405
+#define RJ54N1_Y_OUTPUT_SIZE_S_L	0x0406
+#define RJ54N1_XY_OUTPUT_SIZE_P_H	0x0407
+#define RJ54N1_X_OUTPUT_SIZE_P_L	0x0408
+#define RJ54N1_Y_OUTPUT_SIZE_P_L	0x0409
+#define RJ54N1_LINE_LENGTH_PCK_S_H	0x040a
+#define RJ54N1_LINE_LENGTH_PCK_S_L	0x040b
+#define RJ54N1_LINE_LENGTH_PCK_P_H	0x040c
+#define RJ54N1_LINE_LENGTH_PCK_P_L	0x040d
+#define RJ54N1_RESIZE_N			0x040e
+#define RJ54N1_RESIZE_N_STEP		0x040f
+#define RJ54N1_RESIZE_STEP		0x0410
+#define RJ54N1_RESIZE_HOLD_H		0x0411
+#define RJ54N1_RESIZE_HOLD_L		0x0412
+#define RJ54N1_H_OBEN_OFS		0x0413
+#define RJ54N1_V_OBEN_OFS		0x0414
+#define RJ54N1_RESIZE_CONTROL		0x0415
+#define RJ54N1_STILL_CONTROL		0x0417
+#define RJ54N1_INC_USE_SEL_H		0x0425
+#define RJ54N1_INC_USE_SEL_L		0x0426
+#define RJ54N1_MIRROR_STILL_MODE	0x0427
+#define RJ54N1_INIT_START		0x0428
+#define RJ54N1_SCALE_1_2_LEV		0x0429
+#define RJ54N1_SCALE_4_LEV		0x042a
+#define RJ54N1_Y_GAIN			0x04d8
+#define RJ54N1_APT_GAIN_UP		0x04fa
+#define RJ54N1_RA_SEL_UL		0x0530
+#define RJ54N1_BYTE_SWAP		0x0531
+#define RJ54N1_OUT_SIGPO		0x053b
+#define RJ54N1_WB_SEL_WEIGHT_I		0x054e
+#define RJ54N1_BIT8_WB			0x0569
+#define RJ54N1_HCAPS_WB			0x056a
+#define RJ54N1_VCAPS_WB			0x056b
+#define RJ54N1_HCAPE_WB			0x056c
+#define RJ54N1_VCAPE_WB			0x056d
+#define RJ54N1_EXPOSURE_CONTROL		0x058c
+#define RJ54N1_FRAME_LENGTH_S_H		0x0595
+#define RJ54N1_FRAME_LENGTH_S_L		0x0596
+#define RJ54N1_FRAME_LENGTH_P_H		0x0597
+#define RJ54N1_FRAME_LENGTH_P_L		0x0598
+#define RJ54N1_PEAK_H			0x05b7
+#define RJ54N1_PEAK_50			0x05b8
+#define RJ54N1_PEAK_60			0x05b9
+#define RJ54N1_PEAK_DIFF		0x05ba
+#define RJ54N1_IOC			0x05ef
+#define RJ54N1_TG_BYPASS		0x0700
+#define RJ54N1_PLL_L			0x0701
+#define RJ54N1_PLL_N			0x0702
+#define RJ54N1_PLL_EN			0x0704
+#define RJ54N1_RATIO_TG			0x0706
+#define RJ54N1_RATIO_T			0x0707
+#define RJ54N1_RATIO_R			0x0708
+#define RJ54N1_RAMP_TGCLK_EN		0x0709
+#define RJ54N1_OCLK_DSP			0x0710
+#define RJ54N1_RATIO_OP			0x0711
+#define RJ54N1_RATIO_O			0x0712
+#define RJ54N1_OCLK_SEL_EN		0x0713
+#define RJ54N1_CLK_RST			0x0717
+#define RJ54N1_RESET_STANDBY		0x0718
+#define RJ54N1_FWFLG			0x07fe
+
+#define E_EXCLK				(1 << 7)
+#define SOFT_STDBY			(1 << 4)
+#define SEN_RSTX			(1 << 2)
+#define TG_RSTX				(1 << 1)
+#define DSP_RSTX			(1 << 0)
+
+#define RESIZE_HOLD_SEL			(1 << 2)
+#define RESIZE_GO			(1 << 1)
+
+/*
+ * When cropping, the camera automatically centers the cropped region, there
+ * doesn't seem to be a way to specify an explicit location of the rectangle.
+ */
+#define RJ54N1_COLUMN_SKIP		0
+#define RJ54N1_ROW_SKIP			0
+#define RJ54N1_MAX_WIDTH		1600
+#define RJ54N1_MAX_HEIGHT		1200
+
+#define PLL_L				2
+#define PLL_N				0x31
+
+/* I2C addresses: 0x50, 0x51, 0x60, 0x61 */
+
+/* RJ54N1CB0C has only one fixed colorspace per pixelcode */
+struct rj54n1_datafmt {
+	u32	code;
+	enum v4l2_colorspace		colorspace;
+};
+
+/* Find a data format by a pixel code in an array */
+static const struct rj54n1_datafmt *rj54n1_find_datafmt(
+	u32 code, const struct rj54n1_datafmt *fmt,
+	int n)
+{
+	int i;
+	for (i = 0; i < n; i++)
+		if (fmt[i].code == code)
+			return fmt + i;
+
+	return NULL;
+}
+
+static const struct rj54n1_datafmt rj54n1_colour_fmts[] = {
+	{MEDIA_BUS_FMT_YUYV8_2X8, V4L2_COLORSPACE_JPEG},
+	{MEDIA_BUS_FMT_YVYU8_2X8, V4L2_COLORSPACE_JPEG},
+	{MEDIA_BUS_FMT_RGB565_2X8_LE, V4L2_COLORSPACE_SRGB},
+	{MEDIA_BUS_FMT_RGB565_2X8_BE, V4L2_COLORSPACE_SRGB},
+	{MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE, V4L2_COLORSPACE_SRGB},
+	{MEDIA_BUS_FMT_SBGGR10_2X8_PADLO_LE, V4L2_COLORSPACE_SRGB},
+	{MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_BE, V4L2_COLORSPACE_SRGB},
+	{MEDIA_BUS_FMT_SBGGR10_2X8_PADLO_BE, V4L2_COLORSPACE_SRGB},
+	{MEDIA_BUS_FMT_SBGGR10_1X10, V4L2_COLORSPACE_SRGB},
+};
+
+struct rj54n1_clock_div {
+	u8 ratio_tg;	/* can be 0 or an odd number */
+	u8 ratio_t;
+	u8 ratio_r;
+	u8 ratio_op;
+	u8 ratio_o;
+};
+
+struct rj54n1 {
+	struct v4l2_subdev subdev;
+	struct v4l2_ctrl_handler hdl;
+	struct clk *clk;
+	struct gpio_desc *pwup_gpio;
+	struct gpio_desc *enable_gpio;
+	struct rj54n1_clock_div clk_div;
+	const struct rj54n1_datafmt *fmt;
+	struct v4l2_rect rect;	/* Sensor window */
+	unsigned int tgclk_mhz;
+	bool auto_wb;
+	unsigned short width;	/* Output window */
+	unsigned short height;
+	unsigned short resize;	/* Sensor * 1024 / resize = Output */
+	unsigned short scale;
+	u8 bank;
+};
+
+struct rj54n1_reg_val {
+	u16 reg;
+	u8 val;
+};
+
+static const struct rj54n1_reg_val bank_4[] = {
+	{0x417, 0},
+	{0x42c, 0},
+	{0x42d, 0xf0},
+	{0x42e, 0},
+	{0x42f, 0x50},
+	{0x430, 0xf5},
+	{0x431, 0x16},
+	{0x432, 0x20},
+	{0x433, 0},
+	{0x434, 0xc8},
+	{0x43c, 8},
+	{0x43e, 0x90},
+	{0x445, 0x83},
+	{0x4ba, 0x58},
+	{0x4bb, 4},
+	{0x4bc, 0x20},
+	{0x4db, 4},
+	{0x4fe, 2},
+};
+
+static const struct rj54n1_reg_val bank_5[] = {
+	{0x514, 0},
+	{0x516, 0},
+	{0x518, 0},
+	{0x51a, 0},
+	{0x51d, 0xff},
+	{0x56f, 0x28},
+	{0x575, 0x40},
+	{0x5bc, 0x48},
+	{0x5c1, 6},
+	{0x5e5, 0x11},
+	{0x5e6, 0x43},
+	{0x5e7, 0x33},
+	{0x5e8, 0x21},
+	{0x5e9, 0x30},
+	{0x5ea, 0x0},
+	{0x5eb, 0xa5},
+	{0x5ec, 0xff},
+	{0x5fe, 2},
+};
+
+static const struct rj54n1_reg_val bank_7[] = {
+	{0x70a, 0},
+	{0x714, 0xff},
+	{0x715, 0xff},
+	{0x716, 0x1f},
+	{0x7FE, 2},
+};
+
+static const struct rj54n1_reg_val bank_8[] = {
+	{0x800, 0x00},
+	{0x801, 0x01},
+	{0x802, 0x61},
+	{0x805, 0x00},
+	{0x806, 0x00},
+	{0x807, 0x00},
+	{0x808, 0x00},
+	{0x809, 0x01},
+	{0x80A, 0x61},
+	{0x80B, 0x00},
+	{0x80C, 0x01},
+	{0x80D, 0x00},
+	{0x80E, 0x00},
+	{0x80F, 0x00},
+	{0x810, 0x00},
+	{0x811, 0x01},
+	{0x812, 0x61},
+	{0x813, 0x00},
+	{0x814, 0x11},
+	{0x815, 0x00},
+	{0x816, 0x41},
+	{0x817, 0x00},
+	{0x818, 0x51},
+	{0x819, 0x01},
+	{0x81A, 0x1F},
+	{0x81B, 0x00},
+	{0x81C, 0x01},
+	{0x81D, 0x00},
+	{0x81E, 0x11},
+	{0x81F, 0x00},
+	{0x820, 0x41},
+	{0x821, 0x00},
+	{0x822, 0x51},
+	{0x823, 0x00},
+	{0x824, 0x00},
+	{0x825, 0x00},
+	{0x826, 0x47},
+	{0x827, 0x01},
+	{0x828, 0x4F},
+	{0x829, 0x00},
+	{0x82A, 0x00},
+	{0x82B, 0x00},
+	{0x82C, 0x30},
+	{0x82D, 0x00},
+	{0x82E, 0x40},
+	{0x82F, 0x00},
+	{0x830, 0xB3},
+	{0x831, 0x00},
+	{0x832, 0xE3},
+	{0x833, 0x00},
+	{0x834, 0x00},
+	{0x835, 0x00},
+	{0x836, 0x00},
+	{0x837, 0x00},
+	{0x838, 0x00},
+	{0x839, 0x01},
+	{0x83A, 0x61},
+	{0x83B, 0x00},
+	{0x83C, 0x01},
+	{0x83D, 0x00},
+	{0x83E, 0x00},
+	{0x83F, 0x00},
+	{0x840, 0x00},
+	{0x841, 0x01},
+	{0x842, 0x61},
+	{0x843, 0x00},
+	{0x844, 0x1D},
+	{0x845, 0x00},
+	{0x846, 0x00},
+	{0x847, 0x00},
+	{0x848, 0x00},
+	{0x849, 0x01},
+	{0x84A, 0x1F},
+	{0x84B, 0x00},
+	{0x84C, 0x05},
+	{0x84D, 0x00},
+	{0x84E, 0x19},
+	{0x84F, 0x01},
+	{0x850, 0x21},
+	{0x851, 0x01},
+	{0x852, 0x5D},
+	{0x853, 0x00},
+	{0x854, 0x00},
+	{0x855, 0x00},
+	{0x856, 0x19},
+	{0x857, 0x01},
+	{0x858, 0x21},
+	{0x859, 0x00},
+	{0x85A, 0x00},
+	{0x85B, 0x00},
+	{0x85C, 0x00},
+	{0x85D, 0x00},
+	{0x85E, 0x00},
+	{0x85F, 0x00},
+	{0x860, 0xB3},
+	{0x861, 0x00},
+	{0x862, 0xE3},
+	{0x863, 0x00},
+	{0x864, 0x00},
+	{0x865, 0x00},
+	{0x866, 0x00},
+	{0x867, 0x00},
+	{0x868, 0x00},
+	{0x869, 0xE2},
+	{0x86A, 0x00},
+	{0x86B, 0x01},
+	{0x86C, 0x06},
+	{0x86D, 0x00},
+	{0x86E, 0x00},
+	{0x86F, 0x00},
+	{0x870, 0x60},
+	{0x871, 0x8C},
+	{0x872, 0x10},
+	{0x873, 0x00},
+	{0x874, 0xE0},
+	{0x875, 0x00},
+	{0x876, 0x27},
+	{0x877, 0x01},
+	{0x878, 0x00},
+	{0x879, 0x00},
+	{0x87A, 0x00},
+	{0x87B, 0x03},
+	{0x87C, 0x00},
+	{0x87D, 0x00},
+	{0x87E, 0x00},
+	{0x87F, 0x00},
+	{0x880, 0x00},
+	{0x881, 0x00},
+	{0x882, 0x00},
+	{0x883, 0x00},
+	{0x884, 0x00},
+	{0x885, 0x00},
+	{0x886, 0xF8},
+	{0x887, 0x00},
+	{0x888, 0x03},
+	{0x889, 0x00},
+	{0x88A, 0x64},
+	{0x88B, 0x00},
+	{0x88C, 0x03},
+	{0x88D, 0x00},
+	{0x88E, 0xB1},
+	{0x88F, 0x00},
+	{0x890, 0x03},
+	{0x891, 0x01},
+	{0x892, 0x1D},
+	{0x893, 0x00},
+	{0x894, 0x03},
+	{0x895, 0x01},
+	{0x896, 0x4B},
+	{0x897, 0x00},
+	{0x898, 0xE5},
+	{0x899, 0x00},
+	{0x89A, 0x01},
+	{0x89B, 0x00},
+	{0x89C, 0x01},
+	{0x89D, 0x04},
+	{0x89E, 0xC8},
+	{0x89F, 0x00},
+	{0x8A0, 0x01},
+	{0x8A1, 0x01},
+	{0x8A2, 0x61},
+	{0x8A3, 0x00},
+	{0x8A4, 0x01},
+	{0x8A5, 0x00},
+	{0x8A6, 0x00},
+	{0x8A7, 0x00},
+	{0x8A8, 0x00},
+	{0x8A9, 0x00},
+	{0x8AA, 0x7F},
+	{0x8AB, 0x03},
+	{0x8AC, 0x00},
+	{0x8AD, 0x00},
+	{0x8AE, 0x00},
+	{0x8AF, 0x00},
+	{0x8B0, 0x00},
+	{0x8B1, 0x00},
+	{0x8B6, 0x00},
+	{0x8B7, 0x01},
+	{0x8B8, 0x00},
+	{0x8B9, 0x00},
+	{0x8BA, 0x02},
+	{0x8BB, 0x00},
+	{0x8BC, 0xFF},
+	{0x8BD, 0x00},
+	{0x8FE, 2},
+};
+
+static const struct rj54n1_reg_val bank_10[] = {
+	{0x10bf, 0x69}
+};
+
+/* Clock dividers - these are default register values, divider = register + 1 */
+static const struct rj54n1_clock_div clk_div = {
+	.ratio_tg	= 3 /* default: 5 */,
+	.ratio_t	= 4 /* default: 1 */,
+	.ratio_r	= 4 /* default: 0 */,
+	.ratio_op	= 1 /* default: 5 */,
+	.ratio_o	= 9 /* default: 0 */,
+};
+
+static struct rj54n1 *to_rj54n1(const struct i2c_client *client)
+{
+	return container_of(i2c_get_clientdata(client), struct rj54n1, subdev);
+}
+
+static int reg_read(struct i2c_client *client, const u16 reg)
+{
+	struct rj54n1 *rj54n1 = to_rj54n1(client);
+	int ret;
+
+	/* set bank */
+	if (rj54n1->bank != reg >> 8) {
+		dev_dbg(&client->dev, "[0x%x] = 0x%x\n", 0xff, reg >> 8);
+		ret = i2c_smbus_write_byte_data(client, 0xff, reg >> 8);
+		if (ret < 0)
+			return ret;
+		rj54n1->bank = reg >> 8;
+	}
+	return i2c_smbus_read_byte_data(client, reg & 0xff);
+}
+
+static int reg_write(struct i2c_client *client, const u16 reg,
+		     const u8 data)
+{
+	struct rj54n1 *rj54n1 = to_rj54n1(client);
+	int ret;
+
+	/* set bank */
+	if (rj54n1->bank != reg >> 8) {
+		dev_dbg(&client->dev, "[0x%x] = 0x%x\n", 0xff, reg >> 8);
+		ret = i2c_smbus_write_byte_data(client, 0xff, reg >> 8);
+		if (ret < 0)
+			return ret;
+		rj54n1->bank = reg >> 8;
+	}
+	dev_dbg(&client->dev, "[0x%x] = 0x%x\n", reg & 0xff, data);
+	return i2c_smbus_write_byte_data(client, reg & 0xff, data);
+}
+
+static int reg_set(struct i2c_client *client, const u16 reg,
+		   const u8 data, const u8 mask)
+{
+	int ret;
+
+	ret = reg_read(client, reg);
+	if (ret < 0)
+		return ret;
+	return reg_write(client, reg, (ret & ~mask) | (data & mask));
+}
+
+static int reg_write_multiple(struct i2c_client *client,
+			      const struct rj54n1_reg_val *rv, const int n)
+{
+	int i, ret;
+
+	for (i = 0; i < n; i++) {
+		ret = reg_write(client, rv->reg, rv->val);
+		if (ret < 0)
+			return ret;
+		rv++;
+	}
+
+	return 0;
+}
+
+static int rj54n1_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->pad || code->index >= ARRAY_SIZE(rj54n1_colour_fmts))
+		return -EINVAL;
+
+	code->code = rj54n1_colour_fmts[code->index].code;
+	return 0;
+}
+
+static int rj54n1_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	/* Switch between preview and still shot modes */
+	return reg_set(client, RJ54N1_STILL_CONTROL, (!enable) << 7, 0x80);
+}
+
+static int rj54n1_set_rect(struct i2c_client *client,
+			   u16 reg_x, u16 reg_y, u16 reg_xy,
+			   u32 width, u32 height)
+{
+	int ret;
+
+	ret = reg_write(client, reg_xy,
+			((width >> 4) & 0x70) |
+			((height >> 8) & 7));
+
+	if (!ret)
+		ret = reg_write(client, reg_x, width & 0xff);
+	if (!ret)
+		ret = reg_write(client, reg_y, height & 0xff);
+
+	return ret;
+}
+
+/*
+ * Some commands, specifically certain initialisation sequences, require
+ * a commit operation.
+ */
+static int rj54n1_commit(struct i2c_client *client)
+{
+	int ret = reg_write(client, RJ54N1_INIT_START, 1);
+	msleep(10);
+	if (!ret)
+		ret = reg_write(client, RJ54N1_INIT_START, 0);
+	return ret;
+}
+
+static int rj54n1_sensor_scale(struct v4l2_subdev *sd, s32 *in_w, s32 *in_h,
+			       s32 *out_w, s32 *out_h);
+
+static int rj54n1_set_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *sd_state,
+				struct v4l2_subdev_selection *sel)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct rj54n1 *rj54n1 = to_rj54n1(client);
+	const struct v4l2_rect *rect = &sel->r;
+	int output_w, output_h, input_w = rect->width, input_h = rect->height;
+	int ret;
+
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE ||
+	    sel->target != V4L2_SEL_TGT_CROP)
+		return -EINVAL;
+
+	/* arbitrary minimum width and height, edges unimportant */
+	v4l_bound_align_image(&input_w, 8, RJ54N1_MAX_WIDTH, 0,
+			      &input_h, 8, RJ54N1_MAX_HEIGHT, 0, 0);
+
+	output_w = (input_w * 1024 + rj54n1->resize / 2) / rj54n1->resize;
+	output_h = (input_h * 1024 + rj54n1->resize / 2) / rj54n1->resize;
+
+	dev_dbg(&client->dev, "Scaling for %dx%d : %u = %dx%d\n",
+		input_w, input_h, rj54n1->resize, output_w, output_h);
+
+	ret = rj54n1_sensor_scale(sd, &input_w, &input_h, &output_w, &output_h);
+	if (ret < 0)
+		return ret;
+
+	rj54n1->width		= output_w;
+	rj54n1->height		= output_h;
+	rj54n1->resize		= ret;
+	rj54n1->rect.width	= input_w;
+	rj54n1->rect.height	= input_h;
+
+	return 0;
+}
+
+static int rj54n1_get_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *sd_state,
+				struct v4l2_subdev_selection *sel)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct rj54n1 *rj54n1 = to_rj54n1(client);
+
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
+		return -EINVAL;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.left = RJ54N1_COLUMN_SKIP;
+		sel->r.top = RJ54N1_ROW_SKIP;
+		sel->r.width = RJ54N1_MAX_WIDTH;
+		sel->r.height = RJ54N1_MAX_HEIGHT;
+		return 0;
+	case V4L2_SEL_TGT_CROP:
+		sel->r = rj54n1->rect;
+		return 0;
+	default:
+		return -EINVAL;
+	}
+}
+
+static int rj54n1_get_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *mf = &format->format;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct rj54n1 *rj54n1 = to_rj54n1(client);
+
+	if (format->pad)
+		return -EINVAL;
+
+	mf->code	= rj54n1->fmt->code;
+	mf->colorspace	= rj54n1->fmt->colorspace;
+	mf->ycbcr_enc	= V4L2_YCBCR_ENC_601;
+	mf->xfer_func	= V4L2_XFER_FUNC_SRGB;
+	mf->quantization = V4L2_QUANTIZATION_DEFAULT;
+	mf->field	= V4L2_FIELD_NONE;
+	mf->width	= rj54n1->width;
+	mf->height	= rj54n1->height;
+
+	return 0;
+}
+
+/*
+ * The actual geometry configuration routine. It scales the input window into
+ * the output one, updates the window sizes and returns an error or the resize
+ * coefficient on success. Note: we only use the "Fixed Scaling" on this camera.
+ */
+static int rj54n1_sensor_scale(struct v4l2_subdev *sd, s32 *in_w, s32 *in_h,
+			       s32 *out_w, s32 *out_h)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct rj54n1 *rj54n1 = to_rj54n1(client);
+	unsigned int skip, resize, input_w = *in_w, input_h = *in_h,
+		output_w = *out_w, output_h = *out_h;
+	u16 inc_sel, wb_bit8, wb_left, wb_right, wb_top, wb_bottom;
+	unsigned int peak, peak_50, peak_60;
+	int ret;
+
+	/*
+	 * We have a problem with crops, where the window is larger than 512x384
+	 * and output window is larger than a half of the input one. In this
+	 * case we have to either reduce the input window to equal or below
+	 * 512x384 or the output window to equal or below 1/2 of the input.
+	 */
+	if (output_w > max(512U, input_w / 2)) {
+		if (2 * output_w > RJ54N1_MAX_WIDTH) {
+			input_w = RJ54N1_MAX_WIDTH;
+			output_w = RJ54N1_MAX_WIDTH / 2;
+		} else {
+			input_w = output_w * 2;
+		}
+
+		dev_dbg(&client->dev, "Adjusted output width: in %u, out %u\n",
+			input_w, output_w);
+	}
+
+	if (output_h > max(384U, input_h / 2)) {
+		if (2 * output_h > RJ54N1_MAX_HEIGHT) {
+			input_h = RJ54N1_MAX_HEIGHT;
+			output_h = RJ54N1_MAX_HEIGHT / 2;
+		} else {
+			input_h = output_h * 2;
+		}
+
+		dev_dbg(&client->dev, "Adjusted output height: in %u, out %u\n",
+			input_h, output_h);
+	}
+
+	/* Idea: use the read mode for snapshots, handle separate geometries */
+	ret = rj54n1_set_rect(client, RJ54N1_X_OUTPUT_SIZE_S_L,
+			      RJ54N1_Y_OUTPUT_SIZE_S_L,
+			      RJ54N1_XY_OUTPUT_SIZE_S_H, output_w, output_h);
+	if (!ret)
+		ret = rj54n1_set_rect(client, RJ54N1_X_OUTPUT_SIZE_P_L,
+			      RJ54N1_Y_OUTPUT_SIZE_P_L,
+			      RJ54N1_XY_OUTPUT_SIZE_P_H, output_w, output_h);
+
+	if (ret < 0)
+		return ret;
+
+	if (output_w > input_w && output_h > input_h) {
+		input_w = output_w;
+		input_h = output_h;
+
+		resize = 1024;
+	} else {
+		unsigned int resize_x, resize_y;
+		resize_x = (input_w * 1024 + output_w / 2) / output_w;
+		resize_y = (input_h * 1024 + output_h / 2) / output_h;
+
+		/* We want max(resize_x, resize_y), check if it still fits */
+		if (resize_x > resize_y &&
+		    (output_h * resize_x + 512) / 1024 > RJ54N1_MAX_HEIGHT)
+			resize = (RJ54N1_MAX_HEIGHT * 1024 + output_h / 2) /
+				output_h;
+		else if (resize_y > resize_x &&
+			 (output_w * resize_y + 512) / 1024 > RJ54N1_MAX_WIDTH)
+			resize = (RJ54N1_MAX_WIDTH * 1024 + output_w / 2) /
+				output_w;
+		else
+			resize = max(resize_x, resize_y);
+
+		/* Prohibited value ranges */
+		switch (resize) {
+		case 2040 ... 2047:
+			resize = 2039;
+			break;
+		case 4080 ... 4095:
+			resize = 4079;
+			break;
+		case 8160 ... 8191:
+			resize = 8159;
+			break;
+		case 16320 ... 16384:
+			resize = 16319;
+		}
+	}
+
+	/* Set scaling */
+	ret = reg_write(client, RJ54N1_RESIZE_HOLD_L, resize & 0xff);
+	if (!ret)
+		ret = reg_write(client, RJ54N1_RESIZE_HOLD_H, resize >> 8);
+
+	if (ret < 0)
+		return ret;
+
+	/*
+	 * Configure a skipping bitmask. The sensor will select a skipping value
+	 * among set bits automatically. This is very unclear in the datasheet
+	 * too. I was told, in this register one enables all skipping values,
+	 * that are required for a specific resize, and the camera selects
+	 * automatically, which ones to use. But it is unclear how to identify,
+	 * which cropping values are needed. Secondly, why don't we just set all
+	 * bits and let the camera choose? Would it increase processing time and
+	 * reduce the framerate? Using 0xfffc for INC_USE_SEL doesn't seem to
+	 * improve the image quality or stability for larger frames (see comment
+	 * above), but I didn't check the framerate.
+	 */
+	skip = min(resize / 1024, 15U);
+
+	inc_sel = 1 << skip;
+
+	if (inc_sel <= 2)
+		inc_sel = 0xc;
+	else if (resize & 1023 && skip < 15)
+		inc_sel |= 1 << (skip + 1);
+
+	ret = reg_write(client, RJ54N1_INC_USE_SEL_L, inc_sel & 0xfc);
+	if (!ret)
+		ret = reg_write(client, RJ54N1_INC_USE_SEL_H, inc_sel >> 8);
+
+	if (!rj54n1->auto_wb) {
+		/* Auto white balance window */
+		wb_left	  = output_w / 16;
+		wb_right  = (3 * output_w / 4 - 3) / 4;
+		wb_top	  = output_h / 16;
+		wb_bottom = (3 * output_h / 4 - 3) / 4;
+		wb_bit8	  = ((wb_left >> 2) & 0x40) | ((wb_top >> 4) & 0x10) |
+			((wb_right >> 6) & 4) | ((wb_bottom >> 8) & 1);
+
+		if (!ret)
+			ret = reg_write(client, RJ54N1_BIT8_WB, wb_bit8);
+		if (!ret)
+			ret = reg_write(client, RJ54N1_HCAPS_WB, wb_left);
+		if (!ret)
+			ret = reg_write(client, RJ54N1_VCAPS_WB, wb_top);
+		if (!ret)
+			ret = reg_write(client, RJ54N1_HCAPE_WB, wb_right);
+		if (!ret)
+			ret = reg_write(client, RJ54N1_VCAPE_WB, wb_bottom);
+	}
+
+	/* Antiflicker */
+	peak = 12 * RJ54N1_MAX_WIDTH * (1 << 14) * resize / rj54n1->tgclk_mhz /
+		10000;
+	peak_50 = peak / 6;
+	peak_60 = peak / 5;
+
+	if (!ret)
+		ret = reg_write(client, RJ54N1_PEAK_H,
+				((peak_50 >> 4) & 0xf0) | (peak_60 >> 8));
+	if (!ret)
+		ret = reg_write(client, RJ54N1_PEAK_50, peak_50);
+	if (!ret)
+		ret = reg_write(client, RJ54N1_PEAK_60, peak_60);
+	if (!ret)
+		ret = reg_write(client, RJ54N1_PEAK_DIFF, peak / 150);
+
+	/* Start resizing */
+	if (!ret)
+		ret = reg_write(client, RJ54N1_RESIZE_CONTROL,
+				RESIZE_HOLD_SEL | RESIZE_GO | 1);
+
+	if (ret < 0)
+		return ret;
+
+	/* Constant taken from manufacturer's example */
+	msleep(230);
+
+	ret = reg_write(client, RJ54N1_RESIZE_CONTROL, RESIZE_HOLD_SEL | 1);
+	if (ret < 0)
+		return ret;
+
+	*in_w = (output_w * resize + 512) / 1024;
+	*in_h = (output_h * resize + 512) / 1024;
+	*out_w = output_w;
+	*out_h = output_h;
+
+	dev_dbg(&client->dev, "Scaled for %dx%d : %u = %ux%u, skip %u\n",
+		*in_w, *in_h, resize, output_w, output_h, skip);
+
+	return resize;
+}
+
+static int rj54n1_set_clock(struct i2c_client *client)
+{
+	struct rj54n1 *rj54n1 = to_rj54n1(client);
+	int ret;
+
+	/* Enable external clock */
+	ret = reg_write(client, RJ54N1_RESET_STANDBY, E_EXCLK | SOFT_STDBY);
+	/* Leave stand-by. Note: use this when implementing suspend / resume */
+	if (!ret)
+		ret = reg_write(client, RJ54N1_RESET_STANDBY, E_EXCLK);
+
+	if (!ret)
+		ret = reg_write(client, RJ54N1_PLL_L, PLL_L);
+	if (!ret)
+		ret = reg_write(client, RJ54N1_PLL_N, PLL_N);
+
+	/* TGCLK dividers */
+	if (!ret)
+		ret = reg_write(client, RJ54N1_RATIO_TG,
+				rj54n1->clk_div.ratio_tg);
+	if (!ret)
+		ret = reg_write(client, RJ54N1_RATIO_T,
+				rj54n1->clk_div.ratio_t);
+	if (!ret)
+		ret = reg_write(client, RJ54N1_RATIO_R,
+				rj54n1->clk_div.ratio_r);
+
+	/* Enable TGCLK & RAMP */
+	if (!ret)
+		ret = reg_write(client, RJ54N1_RAMP_TGCLK_EN, 3);
+
+	/* Disable clock output */
+	if (!ret)
+		ret = reg_write(client, RJ54N1_OCLK_DSP, 0);
+
+	/* Set divisors */
+	if (!ret)
+		ret = reg_write(client, RJ54N1_RATIO_OP,
+				rj54n1->clk_div.ratio_op);
+	if (!ret)
+		ret = reg_write(client, RJ54N1_RATIO_O,
+				rj54n1->clk_div.ratio_o);
+
+	/* Enable OCLK */
+	if (!ret)
+		ret = reg_write(client, RJ54N1_OCLK_SEL_EN, 1);
+
+	/* Use PLL for Timing Generator, write 2 to reserved bits */
+	if (!ret)
+		ret = reg_write(client, RJ54N1_TG_BYPASS, 2);
+
+	/* Take sensor out of reset */
+	if (!ret)
+		ret = reg_write(client, RJ54N1_RESET_STANDBY,
+				E_EXCLK | SEN_RSTX);
+	/* Enable PLL */
+	if (!ret)
+		ret = reg_write(client, RJ54N1_PLL_EN, 1);
+
+	/* Wait for PLL to stabilise */
+	msleep(10);
+
+	/* Enable clock to frequency divider */
+	if (!ret)
+		ret = reg_write(client, RJ54N1_CLK_RST, 1);
+
+	if (!ret)
+		ret = reg_read(client, RJ54N1_CLK_RST);
+	if (ret != 1) {
+		dev_err(&client->dev,
+			"Resetting RJ54N1CB0C clock failed: %d!\n", ret);
+		return -EIO;
+	}
+
+	/* Start the PLL */
+	ret = reg_set(client, RJ54N1_OCLK_DSP, 1, 1);
+
+	/* Enable OCLK */
+	if (!ret)
+		ret = reg_write(client, RJ54N1_OCLK_SEL_EN, 1);
+
+	return ret;
+}
+
+static int rj54n1_reg_init(struct i2c_client *client)
+{
+	struct rj54n1 *rj54n1 = to_rj54n1(client);
+	int ret = rj54n1_set_clock(client);
+
+	if (!ret)
+		ret = reg_write_multiple(client, bank_7, ARRAY_SIZE(bank_7));
+	if (!ret)
+		ret = reg_write_multiple(client, bank_10, ARRAY_SIZE(bank_10));
+
+	/* Set binning divisors */
+	if (!ret)
+		ret = reg_write(client, RJ54N1_SCALE_1_2_LEV, 3 | (7 << 4));
+	if (!ret)
+		ret = reg_write(client, RJ54N1_SCALE_4_LEV, 0xf);
+
+	/* Switch to fixed resize mode */
+	if (!ret)
+		ret = reg_write(client, RJ54N1_RESIZE_CONTROL,
+				RESIZE_HOLD_SEL | 1);
+
+	/* Set gain */
+	if (!ret)
+		ret = reg_write(client, RJ54N1_Y_GAIN, 0x84);
+
+	/*
+	 * Mirror the image back: default is upside down and left-to-right...
+	 * Set manual preview / still shot switching
+	 */
+	if (!ret)
+		ret = reg_write(client, RJ54N1_MIRROR_STILL_MODE, 0x27);
+
+	if (!ret)
+		ret = reg_write_multiple(client, bank_4, ARRAY_SIZE(bank_4));
+
+	/* Auto exposure area */
+	if (!ret)
+		ret = reg_write(client, RJ54N1_EXPOSURE_CONTROL, 0x80);
+	/* Check current auto WB config */
+	if (!ret)
+		ret = reg_read(client, RJ54N1_WB_SEL_WEIGHT_I);
+	if (ret >= 0) {
+		rj54n1->auto_wb = ret & 0x80;
+		ret = reg_write_multiple(client, bank_5, ARRAY_SIZE(bank_5));
+	}
+	if (!ret)
+		ret = reg_write_multiple(client, bank_8, ARRAY_SIZE(bank_8));
+
+	if (!ret)
+		ret = reg_write(client, RJ54N1_RESET_STANDBY,
+				E_EXCLK | DSP_RSTX | SEN_RSTX);
+
+	/* Commit init */
+	if (!ret)
+		ret = rj54n1_commit(client);
+
+	/* Take DSP, TG, sensor out of reset */
+	if (!ret)
+		ret = reg_write(client, RJ54N1_RESET_STANDBY,
+				E_EXCLK | DSP_RSTX | TG_RSTX | SEN_RSTX);
+
+	/* Start register update? Same register as 0x?FE in many bank_* sets */
+	if (!ret)
+		ret = reg_write(client, RJ54N1_FWFLG, 2);
+
+	/* Constant taken from manufacturer's example */
+	msleep(700);
+
+	return ret;
+}
+
+static int rj54n1_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *mf = &format->format;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct rj54n1 *rj54n1 = to_rj54n1(client);
+	const struct rj54n1_datafmt *fmt;
+	int output_w, output_h, max_w, max_h,
+		input_w = rj54n1->rect.width, input_h = rj54n1->rect.height;
+	int align = mf->code == MEDIA_BUS_FMT_SBGGR10_1X10 ||
+		mf->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_BE ||
+		mf->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADLO_BE ||
+		mf->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE ||
+		mf->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADLO_LE;
+	int ret;
+
+	if (format->pad)
+		return -EINVAL;
+
+	dev_dbg(&client->dev, "%s: code = %d, width = %u, height = %u\n",
+		__func__, mf->code, mf->width, mf->height);
+
+	fmt = rj54n1_find_datafmt(mf->code, rj54n1_colour_fmts,
+				  ARRAY_SIZE(rj54n1_colour_fmts));
+	if (!fmt) {
+		fmt = rj54n1->fmt;
+		mf->code = fmt->code;
+	}
+
+	mf->field	= V4L2_FIELD_NONE;
+	mf->colorspace	= fmt->colorspace;
+
+	v4l_bound_align_image(&mf->width, 112, RJ54N1_MAX_WIDTH, align,
+			      &mf->height, 84, RJ54N1_MAX_HEIGHT, align, 0);
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		sd_state->pads->try_fmt = *mf;
+		return 0;
+	}
+
+	/*
+	 * Verify if the sensor has just been powered on. TODO: replace this
+	 * with proper PM, when a suitable API is available.
+	 */
+	ret = reg_read(client, RJ54N1_RESET_STANDBY);
+	if (ret < 0)
+		return ret;
+
+	if (!(ret & E_EXCLK)) {
+		ret = rj54n1_reg_init(client);
+		if (ret < 0)
+			return ret;
+	}
+
+	/* RA_SEL_UL is only relevant for raw modes, ignored otherwise. */
+	switch (mf->code) {
+	case MEDIA_BUS_FMT_YUYV8_2X8:
+		ret = reg_write(client, RJ54N1_OUT_SEL, 0);
+		if (!ret)
+			ret = reg_set(client, RJ54N1_BYTE_SWAP, 8, 8);
+		break;
+	case MEDIA_BUS_FMT_YVYU8_2X8:
+		ret = reg_write(client, RJ54N1_OUT_SEL, 0);
+		if (!ret)
+			ret = reg_set(client, RJ54N1_BYTE_SWAP, 0, 8);
+		break;
+	case MEDIA_BUS_FMT_RGB565_2X8_LE:
+		ret = reg_write(client, RJ54N1_OUT_SEL, 0x11);
+		if (!ret)
+			ret = reg_set(client, RJ54N1_BYTE_SWAP, 8, 8);
+		break;
+	case MEDIA_BUS_FMT_RGB565_2X8_BE:
+		ret = reg_write(client, RJ54N1_OUT_SEL, 0x11);
+		if (!ret)
+			ret = reg_set(client, RJ54N1_BYTE_SWAP, 0, 8);
+		break;
+	case MEDIA_BUS_FMT_SBGGR10_2X8_PADLO_LE:
+		ret = reg_write(client, RJ54N1_OUT_SEL, 4);
+		if (!ret)
+			ret = reg_set(client, RJ54N1_BYTE_SWAP, 8, 8);
+		if (!ret)
+			ret = reg_write(client, RJ54N1_RA_SEL_UL, 0);
+		break;
+	case MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE:
+		ret = reg_write(client, RJ54N1_OUT_SEL, 4);
+		if (!ret)
+			ret = reg_set(client, RJ54N1_BYTE_SWAP, 8, 8);
+		if (!ret)
+			ret = reg_write(client, RJ54N1_RA_SEL_UL, 8);
+		break;
+	case MEDIA_BUS_FMT_SBGGR10_2X8_PADLO_BE:
+		ret = reg_write(client, RJ54N1_OUT_SEL, 4);
+		if (!ret)
+			ret = reg_set(client, RJ54N1_BYTE_SWAP, 0, 8);
+		if (!ret)
+			ret = reg_write(client, RJ54N1_RA_SEL_UL, 0);
+		break;
+	case MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_BE:
+		ret = reg_write(client, RJ54N1_OUT_SEL, 4);
+		if (!ret)
+			ret = reg_set(client, RJ54N1_BYTE_SWAP, 0, 8);
+		if (!ret)
+			ret = reg_write(client, RJ54N1_RA_SEL_UL, 8);
+		break;
+	case MEDIA_BUS_FMT_SBGGR10_1X10:
+		ret = reg_write(client, RJ54N1_OUT_SEL, 5);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	/* Special case: a raw mode with 10 bits of data per clock tick */
+	if (!ret)
+		ret = reg_set(client, RJ54N1_OCLK_SEL_EN,
+			      (mf->code == MEDIA_BUS_FMT_SBGGR10_1X10) << 1, 2);
+
+	if (ret < 0)
+		return ret;
+
+	/* Supported scales 1:1 >= scale > 1:16 */
+	max_w = mf->width * (16 * 1024 - 1) / 1024;
+	if (input_w > max_w)
+		input_w = max_w;
+	max_h = mf->height * (16 * 1024 - 1) / 1024;
+	if (input_h > max_h)
+		input_h = max_h;
+
+	output_w = mf->width;
+	output_h = mf->height;
+
+	ret = rj54n1_sensor_scale(sd, &input_w, &input_h, &output_w, &output_h);
+	if (ret < 0)
+		return ret;
+
+	fmt = rj54n1_find_datafmt(mf->code, rj54n1_colour_fmts,
+				  ARRAY_SIZE(rj54n1_colour_fmts));
+
+	rj54n1->fmt		= fmt;
+	rj54n1->resize		= ret;
+	rj54n1->rect.width	= input_w;
+	rj54n1->rect.height	= input_h;
+	rj54n1->width		= output_w;
+	rj54n1->height		= output_h;
+
+	mf->width		= output_w;
+	mf->height		= output_h;
+	mf->field		= V4L2_FIELD_NONE;
+	mf->colorspace		= fmt->colorspace;
+
+	return 0;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int rj54n1_g_register(struct v4l2_subdev *sd,
+			     struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (reg->reg < 0x400 || reg->reg > 0x1fff)
+		/* Registers > 0x0800 are only available from Sharp support */
+		return -EINVAL;
+
+	reg->size = 1;
+	reg->val = reg_read(client, reg->reg);
+
+	if (reg->val > 0xff)
+		return -EIO;
+
+	return 0;
+}
+
+static int rj54n1_s_register(struct v4l2_subdev *sd,
+			     const struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (reg->reg < 0x400 || reg->reg > 0x1fff)
+		/* Registers >= 0x0800 are only available from Sharp support */
+		return -EINVAL;
+
+	if (reg_write(client, reg->reg, reg->val) < 0)
+		return -EIO;
+
+	return 0;
+}
+#endif
+
+static int rj54n1_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct rj54n1 *rj54n1 = to_rj54n1(client);
+
+	if (on) {
+		if (rj54n1->pwup_gpio)
+			gpiod_set_value(rj54n1->pwup_gpio, 1);
+		if (rj54n1->enable_gpio)
+			gpiod_set_value(rj54n1->enable_gpio, 1);
+
+		msleep(1);
+
+		return clk_prepare_enable(rj54n1->clk);
+	}
+
+	clk_disable_unprepare(rj54n1->clk);
+
+	if (rj54n1->enable_gpio)
+		gpiod_set_value(rj54n1->enable_gpio, 0);
+	if (rj54n1->pwup_gpio)
+		gpiod_set_value(rj54n1->pwup_gpio, 0);
+
+	return 0;
+}
+
+static int rj54n1_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct rj54n1 *rj54n1 = container_of(ctrl->handler, struct rj54n1, hdl);
+	struct v4l2_subdev *sd = &rj54n1->subdev;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int data;
+
+	switch (ctrl->id) {
+	case V4L2_CID_VFLIP:
+		if (ctrl->val)
+			data = reg_set(client, RJ54N1_MIRROR_STILL_MODE, 0, 1);
+		else
+			data = reg_set(client, RJ54N1_MIRROR_STILL_MODE, 1, 1);
+		if (data < 0)
+			return -EIO;
+		return 0;
+	case V4L2_CID_HFLIP:
+		if (ctrl->val)
+			data = reg_set(client, RJ54N1_MIRROR_STILL_MODE, 0, 2);
+		else
+			data = reg_set(client, RJ54N1_MIRROR_STILL_MODE, 2, 2);
+		if (data < 0)
+			return -EIO;
+		return 0;
+	case V4L2_CID_GAIN:
+		if (reg_write(client, RJ54N1_Y_GAIN, ctrl->val * 2) < 0)
+			return -EIO;
+		return 0;
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		/* Auto WB area - whole image */
+		if (reg_set(client, RJ54N1_WB_SEL_WEIGHT_I, ctrl->val << 7,
+			    0x80) < 0)
+			return -EIO;
+		rj54n1->auto_wb = ctrl->val;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static const struct v4l2_ctrl_ops rj54n1_ctrl_ops = {
+	.s_ctrl = rj54n1_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops rj54n1_subdev_core_ops = {
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register	= rj54n1_g_register,
+	.s_register	= rj54n1_s_register,
+#endif
+	.s_power	= rj54n1_s_power,
+};
+
+static const struct v4l2_subdev_video_ops rj54n1_subdev_video_ops = {
+	.s_stream	= rj54n1_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops rj54n1_subdev_pad_ops = {
+	.enum_mbus_code = rj54n1_enum_mbus_code,
+	.get_selection	= rj54n1_get_selection,
+	.set_selection	= rj54n1_set_selection,
+	.get_fmt	= rj54n1_get_fmt,
+	.set_fmt	= rj54n1_set_fmt,
+};
+
+static const struct v4l2_subdev_ops rj54n1_subdev_ops = {
+	.core	= &rj54n1_subdev_core_ops,
+	.video	= &rj54n1_subdev_video_ops,
+	.pad	= &rj54n1_subdev_pad_ops,
+};
+
+/*
+ * Interface active, can use i2c. If it fails, it can indeed mean, that
+ * this wasn't our capture interface, so, we wait for the right one
+ */
+static int rj54n1_video_probe(struct i2c_client *client,
+			      struct rj54n1_pdata *priv)
+{
+	struct rj54n1 *rj54n1 = to_rj54n1(client);
+	int data1, data2;
+	int ret;
+
+	ret = rj54n1_s_power(&rj54n1->subdev, 1);
+	if (ret < 0)
+		return ret;
+
+	/* Read out the chip version register */
+	data1 = reg_read(client, RJ54N1_DEV_CODE);
+	data2 = reg_read(client, RJ54N1_DEV_CODE2);
+
+	if (data1 != 0x51 || data2 != 0x10) {
+		ret = -ENODEV;
+		dev_info(&client->dev, "No RJ54N1CB0C found, read 0x%x:0x%x\n",
+			 data1, data2);
+		goto done;
+	}
+
+	/* Configure IOCTL polarity from the platform data: 0 or 1 << 7. */
+	ret = reg_write(client, RJ54N1_IOC, priv->ioctl_high << 7);
+	if (ret < 0)
+		goto done;
+
+	dev_info(&client->dev, "Detected a RJ54N1CB0C chip ID 0x%x:0x%x\n",
+		 data1, data2);
+
+	ret = v4l2_ctrl_handler_setup(&rj54n1->hdl);
+
+done:
+	rj54n1_s_power(&rj54n1->subdev, 0);
+	return ret;
+}
+
+static int rj54n1_probe(struct i2c_client *client)
+{
+	struct rj54n1 *rj54n1;
+	struct i2c_adapter *adapter = client->adapter;
+	struct rj54n1_pdata *rj54n1_priv;
+	int ret;
+
+	if (!client->dev.platform_data) {
+		dev_err(&client->dev, "RJ54N1CB0C: missing platform data!\n");
+		return -EINVAL;
+	}
+
+	rj54n1_priv = client->dev.platform_data;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA)) {
+		dev_warn(&adapter->dev,
+			 "I2C-Adapter doesn't support I2C_FUNC_SMBUS_BYTE\n");
+		return -EIO;
+	}
+
+	rj54n1 = devm_kzalloc(&client->dev, sizeof(struct rj54n1), GFP_KERNEL);
+	if (!rj54n1)
+		return -ENOMEM;
+
+	v4l2_i2c_subdev_init(&rj54n1->subdev, client, &rj54n1_subdev_ops);
+	v4l2_ctrl_handler_init(&rj54n1->hdl, 4);
+	v4l2_ctrl_new_std(&rj54n1->hdl, &rj54n1_ctrl_ops,
+			V4L2_CID_VFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&rj54n1->hdl, &rj54n1_ctrl_ops,
+			V4L2_CID_HFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&rj54n1->hdl, &rj54n1_ctrl_ops,
+			V4L2_CID_GAIN, 0, 127, 1, 66);
+	v4l2_ctrl_new_std(&rj54n1->hdl, &rj54n1_ctrl_ops,
+			V4L2_CID_AUTO_WHITE_BALANCE, 0, 1, 1, 1);
+	rj54n1->subdev.ctrl_handler = &rj54n1->hdl;
+	if (rj54n1->hdl.error)
+		return rj54n1->hdl.error;
+
+	rj54n1->clk_div		= clk_div;
+	rj54n1->rect.left	= RJ54N1_COLUMN_SKIP;
+	rj54n1->rect.top	= RJ54N1_ROW_SKIP;
+	rj54n1->rect.width	= RJ54N1_MAX_WIDTH;
+	rj54n1->rect.height	= RJ54N1_MAX_HEIGHT;
+	rj54n1->width		= RJ54N1_MAX_WIDTH;
+	rj54n1->height		= RJ54N1_MAX_HEIGHT;
+	rj54n1->fmt		= &rj54n1_colour_fmts[0];
+	rj54n1->resize		= 1024;
+	rj54n1->tgclk_mhz	= (rj54n1_priv->mclk_freq / PLL_L * PLL_N) /
+		(clk_div.ratio_tg + 1) / (clk_div.ratio_t + 1);
+
+	rj54n1->clk = clk_get(&client->dev, NULL);
+	if (IS_ERR(rj54n1->clk)) {
+		ret = PTR_ERR(rj54n1->clk);
+		goto err_free_ctrl;
+	}
+
+	rj54n1->pwup_gpio = gpiod_get_optional(&client->dev, "powerup",
+					       GPIOD_OUT_LOW);
+	if (IS_ERR(rj54n1->pwup_gpio)) {
+		dev_info(&client->dev, "Unable to get GPIO \"powerup\": %ld\n",
+			 PTR_ERR(rj54n1->pwup_gpio));
+		ret = PTR_ERR(rj54n1->pwup_gpio);
+		goto err_clk_put;
+	}
+
+	rj54n1->enable_gpio = gpiod_get_optional(&client->dev, "enable",
+						 GPIOD_OUT_LOW);
+	if (IS_ERR(rj54n1->enable_gpio)) {
+		dev_info(&client->dev, "Unable to get GPIO \"enable\": %ld\n",
+			 PTR_ERR(rj54n1->enable_gpio));
+		ret = PTR_ERR(rj54n1->enable_gpio);
+		goto err_gpio_put;
+	}
+
+	ret = rj54n1_video_probe(client, rj54n1_priv);
+	if (ret < 0)
+		goto err_gpio_put;
+
+	ret = v4l2_async_register_subdev(&rj54n1->subdev);
+	if (ret)
+		goto err_gpio_put;
+
+	return 0;
+
+err_gpio_put:
+	if (rj54n1->enable_gpio)
+		gpiod_put(rj54n1->enable_gpio);
+
+	if (rj54n1->pwup_gpio)
+		gpiod_put(rj54n1->pwup_gpio);
+
+err_clk_put:
+	clk_put(rj54n1->clk);
+
+err_free_ctrl:
+	v4l2_ctrl_handler_free(&rj54n1->hdl);
+
+	return ret;
+}
+
+static void rj54n1_remove(struct i2c_client *client)
+{
+	struct rj54n1 *rj54n1 = to_rj54n1(client);
+
+	if (rj54n1->enable_gpio)
+		gpiod_put(rj54n1->enable_gpio);
+	if (rj54n1->pwup_gpio)
+		gpiod_put(rj54n1->pwup_gpio);
+
+	clk_put(rj54n1->clk);
+	v4l2_ctrl_handler_free(&rj54n1->hdl);
+	v4l2_async_unregister_subdev(&rj54n1->subdev);
+}
+
+static const struct i2c_device_id rj54n1_id[] = {
+	{ "rj54n1cb0c", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, rj54n1_id);
+
+static struct i2c_driver rj54n1_i2c_driver = {
+	.driver = {
+		.name = "rj54n1cb0c",
+	},
+	.probe		= rj54n1_probe,
+	.remove		= rj54n1_remove,
+	.id_table	= rj54n1_id,
+};
+
+module_i2c_driver(rj54n1_i2c_driver);
+
+MODULE_DESCRIPTION("Sharp RJ54N1CB0C Camera driver");
+MODULE_AUTHOR("Guennadi Liakhovetski <g.liakhovetski@gmx.de>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/s5c73m3/Makefile b/drivers/media/i2c/s5c73m3/Makefile
new file mode 100644
index 000000000..ddb9dc62d
--- /dev/null
+++ b/drivers/media/i2c/s5c73m3/Makefile
@@ -0,0 +1,3 @@
+# SPDX-License-Identifier: GPL-2.0-only
+s5c73m3-objs			:= s5c73m3-core.o s5c73m3-spi.o s5c73m3-ctrls.o
+obj-$(CONFIG_VIDEO_S5C73M3)	+= s5c73m3.o
diff --git a/drivers/media/i2c/s5c73m3/s5c73m3-core.c b/drivers/media/i2c/s5c73m3/s5c73m3-core.c
new file mode 100644
index 000000000..ed5b10731
--- /dev/null
+++ b/drivers/media/i2c/s5c73m3/s5c73m3-core.c
@@ -0,0 +1,1741 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Samsung LSI S5C73M3 8M pixel camera driver
+ *
+ * Copyright (C) 2012, Samsung Electronics, Co., Ltd.
+ * Sylwester Nawrocki <s.nawrocki@samsung.com>
+ * Andrzej Hajda <a.hajda@samsung.com>
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/firmware.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/media.h>
+#include <linux/module.h>
+#include <linux/of_graph.h>
+#include <linux/regulator/consumer.h>
+#include <linux/sizes.h>
+#include <linux/slab.h>
+#include <linux/spi/spi.h>
+#include <linux/videodev2.h>
+#include <media/media-entity.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-mediabus.h>
+#include <media/v4l2-fwnode.h>
+
+#include "s5c73m3.h"
+
+int s5c73m3_dbg;
+module_param_named(debug, s5c73m3_dbg, int, 0644);
+
+static int boot_from_rom = 1;
+module_param(boot_from_rom, int, 0644);
+
+static int update_fw;
+module_param(update_fw, int, 0644);
+
+#define S5C73M3_EMBEDDED_DATA_MAXLEN	SZ_4K
+#define S5C73M3_MIPI_DATA_LANES		4
+#define S5C73M3_CLK_NAME		"cis_extclk"
+
+static const char * const s5c73m3_supply_names[S5C73M3_MAX_SUPPLIES] = {
+	"vdd-int",	/* Digital Core supply (1.2V), CAM_ISP_CORE_1.2V */
+	"vdda",		/* Analog Core supply (1.2V), CAM_SENSOR_CORE_1.2V */
+	"vdd-reg",	/* Regulator input supply (2.8V), CAM_SENSOR_A2.8V */
+	"vddio-host",	/* Digital Host I/O power supply (1.8V...2.8V),
+			   CAM_ISP_SENSOR_1.8V */
+	"vddio-cis",	/* Digital CIS I/O power (1.2V...1.8V),
+			   CAM_ISP_MIPI_1.2V */
+	"vdd-af",	/* Lens, CAM_AF_2.8V */
+};
+
+static const struct s5c73m3_frame_size s5c73m3_isp_resolutions[] = {
+	{ 320,	240,	COMM_CHG_MODE_YUV_320_240 },
+	{ 352,	288,	COMM_CHG_MODE_YUV_352_288 },
+	{ 640,	480,	COMM_CHG_MODE_YUV_640_480 },
+	{ 880,	720,	COMM_CHG_MODE_YUV_880_720 },
+	{ 960,	720,	COMM_CHG_MODE_YUV_960_720 },
+	{ 1008,	672,	COMM_CHG_MODE_YUV_1008_672 },
+	{ 1184,	666,	COMM_CHG_MODE_YUV_1184_666 },
+	{ 1280,	720,	COMM_CHG_MODE_YUV_1280_720 },
+	{ 1536,	864,	COMM_CHG_MODE_YUV_1536_864 },
+	{ 1600,	1200,	COMM_CHG_MODE_YUV_1600_1200 },
+	{ 1632,	1224,	COMM_CHG_MODE_YUV_1632_1224 },
+	{ 1920,	1080,	COMM_CHG_MODE_YUV_1920_1080 },
+	{ 1920,	1440,	COMM_CHG_MODE_YUV_1920_1440 },
+	{ 2304,	1296,	COMM_CHG_MODE_YUV_2304_1296 },
+	{ 3264,	2448,	COMM_CHG_MODE_YUV_3264_2448 },
+};
+
+static const struct s5c73m3_frame_size s5c73m3_jpeg_resolutions[] = {
+	{ 640,	480,	COMM_CHG_MODE_JPEG_640_480 },
+	{ 800,	450,	COMM_CHG_MODE_JPEG_800_450 },
+	{ 800,	600,	COMM_CHG_MODE_JPEG_800_600 },
+	{ 1024,	768,	COMM_CHG_MODE_JPEG_1024_768 },
+	{ 1280,	720,	COMM_CHG_MODE_JPEG_1280_720 },
+	{ 1280,	960,	COMM_CHG_MODE_JPEG_1280_960 },
+	{ 1600,	900,	COMM_CHG_MODE_JPEG_1600_900 },
+	{ 1600,	1200,	COMM_CHG_MODE_JPEG_1600_1200 },
+	{ 2048,	1152,	COMM_CHG_MODE_JPEG_2048_1152 },
+	{ 2048,	1536,	COMM_CHG_MODE_JPEG_2048_1536 },
+	{ 2560,	1440,	COMM_CHG_MODE_JPEG_2560_1440 },
+	{ 2560,	1920,	COMM_CHG_MODE_JPEG_2560_1920 },
+	{ 3264,	1836,	COMM_CHG_MODE_JPEG_3264_1836 },
+	{ 3264,	2176,	COMM_CHG_MODE_JPEG_3264_2176 },
+	{ 3264,	2448,	COMM_CHG_MODE_JPEG_3264_2448 },
+};
+
+static const struct s5c73m3_frame_size * const s5c73m3_resolutions[] = {
+	[RES_ISP] = s5c73m3_isp_resolutions,
+	[RES_JPEG] = s5c73m3_jpeg_resolutions
+};
+
+static const int s5c73m3_resolutions_len[] = {
+	[RES_ISP] = ARRAY_SIZE(s5c73m3_isp_resolutions),
+	[RES_JPEG] = ARRAY_SIZE(s5c73m3_jpeg_resolutions)
+};
+
+static const struct s5c73m3_interval s5c73m3_intervals[] = {
+	{ COMM_FRAME_RATE_FIXED_7FPS, {142857, 1000000}, {3264, 2448} },
+	{ COMM_FRAME_RATE_FIXED_15FPS, {66667, 1000000}, {3264, 2448} },
+	{ COMM_FRAME_RATE_FIXED_20FPS, {50000, 1000000}, {2304, 1296} },
+	{ COMM_FRAME_RATE_FIXED_30FPS, {33333, 1000000}, {2304, 1296} },
+};
+
+#define S5C73M3_DEFAULT_FRAME_INTERVAL 3 /* 30 fps */
+
+static void s5c73m3_fill_mbus_fmt(struct v4l2_mbus_framefmt *mf,
+				  const struct s5c73m3_frame_size *fs,
+				  u32 code)
+{
+	mf->width = fs->width;
+	mf->height = fs->height;
+	mf->code = code;
+	mf->colorspace = V4L2_COLORSPACE_JPEG;
+	mf->field = V4L2_FIELD_NONE;
+}
+
+static int s5c73m3_i2c_write(struct i2c_client *client, u16 addr, u16 data)
+{
+	u8 buf[4] = { addr >> 8, addr & 0xff, data >> 8, data & 0xff };
+
+	int ret = i2c_master_send(client, buf, sizeof(buf));
+
+	v4l_dbg(4, s5c73m3_dbg, client, "%s: addr 0x%04x, data 0x%04x\n",
+		 __func__, addr, data);
+
+	if (ret == 4)
+		return 0;
+
+	return ret < 0 ? ret : -EREMOTEIO;
+}
+
+static int s5c73m3_i2c_read(struct i2c_client *client, u16 addr, u16 *data)
+{
+	int ret;
+	u8 rbuf[2], wbuf[2] = { addr >> 8, addr & 0xff };
+	struct i2c_msg msg[2] = {
+		{
+			.addr = client->addr,
+			.flags = 0,
+			.len = sizeof(wbuf),
+			.buf = wbuf
+		}, {
+			.addr = client->addr,
+			.flags = I2C_M_RD,
+			.len = sizeof(rbuf),
+			.buf = rbuf
+		}
+	};
+	/*
+	 * Issue repeated START after writing 2 address bytes and
+	 * just one STOP only after reading the data bytes.
+	 */
+	ret = i2c_transfer(client->adapter, msg, 2);
+	if (ret == 2) {
+		*data = be16_to_cpup((__be16 *)rbuf);
+		v4l2_dbg(4, s5c73m3_dbg, client,
+			 "%s: addr: 0x%04x, data: 0x%04x\n",
+			 __func__, addr, *data);
+		return 0;
+	}
+
+	v4l2_err(client, "I2C read failed: addr: %04x, (%d)\n", addr, ret);
+
+	return ret >= 0 ? -EREMOTEIO : ret;
+}
+
+int s5c73m3_write(struct s5c73m3 *state, u32 addr, u16 data)
+{
+	struct i2c_client *client = state->i2c_client;
+	int ret;
+
+	if ((addr ^ state->i2c_write_address) & 0xffff0000) {
+		ret = s5c73m3_i2c_write(client, REG_CMDWR_ADDRH, addr >> 16);
+		if (ret < 0) {
+			state->i2c_write_address = 0;
+			return ret;
+		}
+	}
+
+	if ((addr ^ state->i2c_write_address) & 0xffff) {
+		ret = s5c73m3_i2c_write(client, REG_CMDWR_ADDRL, addr & 0xffff);
+		if (ret < 0) {
+			state->i2c_write_address = 0;
+			return ret;
+		}
+	}
+
+	state->i2c_write_address = addr;
+
+	ret = s5c73m3_i2c_write(client, REG_CMDBUF_ADDR, data);
+	if (ret < 0)
+		return ret;
+
+	state->i2c_write_address += 2;
+
+	return ret;
+}
+
+int s5c73m3_read(struct s5c73m3 *state, u32 addr, u16 *data)
+{
+	struct i2c_client *client = state->i2c_client;
+	int ret;
+
+	if ((addr ^ state->i2c_read_address) & 0xffff0000) {
+		ret = s5c73m3_i2c_write(client, REG_CMDRD_ADDRH, addr >> 16);
+		if (ret < 0) {
+			state->i2c_read_address = 0;
+			return ret;
+		}
+	}
+
+	if ((addr ^ state->i2c_read_address) & 0xffff) {
+		ret = s5c73m3_i2c_write(client, REG_CMDRD_ADDRL, addr & 0xffff);
+		if (ret < 0) {
+			state->i2c_read_address = 0;
+			return ret;
+		}
+	}
+
+	state->i2c_read_address = addr;
+
+	ret = s5c73m3_i2c_read(client, REG_CMDBUF_ADDR, data);
+	if (ret < 0)
+		return ret;
+
+	state->i2c_read_address += 2;
+
+	return ret;
+}
+
+static int s5c73m3_check_status(struct s5c73m3 *state, unsigned int value)
+{
+	unsigned long start = jiffies;
+	unsigned long end = start + msecs_to_jiffies(2000);
+	int ret;
+	u16 status;
+	int count = 0;
+
+	do {
+		ret = s5c73m3_read(state, REG_STATUS, &status);
+		if (ret < 0 || status == value)
+			break;
+		usleep_range(500, 1000);
+		++count;
+	} while (time_is_after_jiffies(end));
+
+	if (count > 0)
+		v4l2_dbg(1, s5c73m3_dbg, &state->sensor_sd,
+			 "status check took %dms\n",
+			 jiffies_to_msecs(jiffies - start));
+
+	if (ret == 0 && status != value) {
+		u16 i2c_status = 0;
+		u16 i2c_seq_status = 0;
+
+		s5c73m3_read(state, REG_I2C_STATUS, &i2c_status);
+		s5c73m3_read(state, REG_I2C_SEQ_STATUS, &i2c_seq_status);
+
+		v4l2_err(&state->sensor_sd,
+			 "wrong status %#x, expected: %#x, i2c_status: %#x/%#x\n",
+			 status, value, i2c_status, i2c_seq_status);
+
+		return -ETIMEDOUT;
+	}
+
+	return ret;
+}
+
+int s5c73m3_isp_command(struct s5c73m3 *state, u16 command, u16 data)
+{
+	int ret;
+
+	ret = s5c73m3_check_status(state, REG_STATUS_ISP_COMMAND_COMPLETED);
+	if (ret < 0)
+		return ret;
+
+	ret = s5c73m3_write(state, 0x00095000, command);
+	if (ret < 0)
+		return ret;
+
+	ret = s5c73m3_write(state, 0x00095002, data);
+	if (ret < 0)
+		return ret;
+
+	return s5c73m3_write(state, REG_STATUS, 0x0001);
+}
+
+static int s5c73m3_isp_comm_result(struct s5c73m3 *state, u16 command,
+				   u16 *data)
+{
+	return s5c73m3_read(state, COMM_RESULT_OFFSET + command, data);
+}
+
+static int s5c73m3_set_af_softlanding(struct s5c73m3 *state)
+{
+	unsigned long start = jiffies;
+	u16 af_softlanding;
+	int count = 0;
+	int ret;
+	const char *msg;
+
+	ret = s5c73m3_isp_command(state, COMM_AF_SOFTLANDING,
+					COMM_AF_SOFTLANDING_ON);
+	if (ret < 0) {
+		v4l2_info(&state->sensor_sd, "AF soft-landing failed\n");
+		return ret;
+	}
+
+	for (;;) {
+		ret = s5c73m3_isp_comm_result(state, COMM_AF_SOFTLANDING,
+							&af_softlanding);
+		if (ret < 0) {
+			msg = "failed";
+			break;
+		}
+		if (af_softlanding == COMM_AF_SOFTLANDING_RES_COMPLETE) {
+			msg = "succeeded";
+			break;
+		}
+		if (++count > 100) {
+			ret = -ETIME;
+			msg = "timed out";
+			break;
+		}
+		msleep(25);
+	}
+
+	v4l2_info(&state->sensor_sd, "AF soft-landing %s after %dms\n",
+		  msg, jiffies_to_msecs(jiffies - start));
+
+	return ret;
+}
+
+static int s5c73m3_load_fw(struct v4l2_subdev *sd)
+{
+	struct s5c73m3 *state = sensor_sd_to_s5c73m3(sd);
+	struct i2c_client *client = state->i2c_client;
+	const struct firmware *fw;
+	int ret;
+	char fw_name[20];
+
+	snprintf(fw_name, sizeof(fw_name), "SlimISP_%.2s.bin",
+							state->fw_file_version);
+	ret = request_firmware(&fw, fw_name, &client->dev);
+	if (ret < 0) {
+		v4l2_err(sd, "Firmware request failed (%s)\n", fw_name);
+		return -EINVAL;
+	}
+
+	v4l2_info(sd, "Loading firmware (%s, %zu B)\n", fw_name, fw->size);
+
+	ret = s5c73m3_spi_write(state, fw->data, fw->size, 64);
+
+	if (ret >= 0)
+		state->isp_ready = 1;
+	else
+		v4l2_err(sd, "SPI write failed\n");
+
+	release_firmware(fw);
+
+	return ret;
+}
+
+static int s5c73m3_set_frame_size(struct s5c73m3 *state)
+{
+	const struct s5c73m3_frame_size *prev_size =
+					state->sensor_pix_size[RES_ISP];
+	const struct s5c73m3_frame_size *cap_size =
+					state->sensor_pix_size[RES_JPEG];
+	unsigned int chg_mode;
+
+	v4l2_dbg(1, s5c73m3_dbg, &state->sensor_sd,
+		 "Preview size: %dx%d, reg_val: 0x%x\n",
+		 prev_size->width, prev_size->height, prev_size->reg_val);
+
+	chg_mode = prev_size->reg_val | COMM_CHG_MODE_NEW;
+
+	if (state->mbus_code == S5C73M3_JPEG_FMT) {
+		v4l2_dbg(1, s5c73m3_dbg, &state->sensor_sd,
+			 "Capture size: %dx%d, reg_val: 0x%x\n",
+			 cap_size->width, cap_size->height, cap_size->reg_val);
+		chg_mode |= cap_size->reg_val;
+	}
+
+	return s5c73m3_isp_command(state, COMM_CHG_MODE, chg_mode);
+}
+
+static int s5c73m3_set_frame_rate(struct s5c73m3 *state)
+{
+	int ret;
+
+	if (state->ctrls.stabilization->val)
+		return 0;
+
+	if (WARN_ON(state->fiv == NULL))
+		return -EINVAL;
+
+	ret = s5c73m3_isp_command(state, COMM_FRAME_RATE, state->fiv->fps_reg);
+	if (!ret)
+		state->apply_fiv = 0;
+
+	return ret;
+}
+
+static int __s5c73m3_s_stream(struct s5c73m3 *state, struct v4l2_subdev *sd,
+								int on)
+{
+	u16 mode;
+	int ret;
+
+	if (on && state->apply_fmt) {
+		if (state->mbus_code == S5C73M3_JPEG_FMT)
+			mode = COMM_IMG_OUTPUT_INTERLEAVED;
+		else
+			mode = COMM_IMG_OUTPUT_YUV;
+
+		ret = s5c73m3_isp_command(state, COMM_IMG_OUTPUT, mode);
+		if (!ret)
+			ret = s5c73m3_set_frame_size(state);
+		if (ret)
+			return ret;
+		state->apply_fmt = 0;
+	}
+
+	ret = s5c73m3_isp_command(state, COMM_SENSOR_STREAMING, !!on);
+	if (ret)
+		return ret;
+
+	state->streaming = !!on;
+
+	if (!on)
+		return 0;
+
+	if (state->apply_fiv) {
+		ret = s5c73m3_set_frame_rate(state);
+		if (ret < 0)
+			v4l2_err(sd, "Error setting frame rate(%d)\n", ret);
+	}
+
+	return s5c73m3_check_status(state, REG_STATUS_ISP_COMMAND_COMPLETED);
+}
+
+static int s5c73m3_oif_s_stream(struct v4l2_subdev *sd, int on)
+{
+	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
+	int ret;
+
+	mutex_lock(&state->lock);
+	ret = __s5c73m3_s_stream(state, sd, on);
+	mutex_unlock(&state->lock);
+
+	return ret;
+}
+
+static int s5c73m3_system_status_wait(struct s5c73m3 *state, u32 value,
+				      unsigned int delay, unsigned int steps)
+{
+	u16 reg = 0;
+
+	while (steps-- > 0) {
+		int ret = s5c73m3_read(state, 0x30100010, &reg);
+		if (ret < 0)
+			return ret;
+		if (reg == value)
+			return 0;
+		usleep_range(delay, delay + 25);
+	}
+	return -ETIMEDOUT;
+}
+
+static int s5c73m3_read_fw_version(struct s5c73m3 *state)
+{
+	struct v4l2_subdev *sd = &state->sensor_sd;
+	int i, ret;
+	u16 data[2];
+	int offset;
+
+	offset = state->isp_ready ? 0x60 : 0;
+
+	for (i = 0; i < S5C73M3_SENSOR_FW_LEN / 2; i++) {
+		ret = s5c73m3_read(state, offset + i * 2, data);
+		if (ret < 0)
+			return ret;
+		state->sensor_fw[i * 2] = (char)(*data & 0xff);
+		state->sensor_fw[i * 2 + 1] = (char)(*data >> 8);
+	}
+	state->sensor_fw[S5C73M3_SENSOR_FW_LEN] = '\0';
+
+
+	for (i = 0; i < S5C73M3_SENSOR_TYPE_LEN / 2; i++) {
+		ret = s5c73m3_read(state, offset + 6 + i * 2, data);
+		if (ret < 0)
+			return ret;
+		state->sensor_type[i * 2] = (char)(*data & 0xff);
+		state->sensor_type[i * 2 + 1] = (char)(*data >> 8);
+	}
+	state->sensor_type[S5C73M3_SENSOR_TYPE_LEN] = '\0';
+
+	ret = s5c73m3_read(state, offset + 0x14, data);
+	if (ret >= 0) {
+		ret = s5c73m3_read(state, offset + 0x16, data + 1);
+		if (ret >= 0)
+			state->fw_size = data[0] + (data[1] << 16);
+	}
+
+	v4l2_info(sd, "Sensor type: %s, FW version: %s\n",
+		  state->sensor_type, state->sensor_fw);
+	return ret;
+}
+
+static int s5c73m3_fw_update_from(struct s5c73m3 *state)
+{
+	struct v4l2_subdev *sd = &state->sensor_sd;
+	u16 status = COMM_FW_UPDATE_NOT_READY;
+	int ret;
+	int count = 0;
+
+	v4l2_warn(sd, "Updating F-ROM firmware.\n");
+	do {
+		if (status == COMM_FW_UPDATE_NOT_READY) {
+			ret = s5c73m3_isp_command(state, COMM_FW_UPDATE, 0);
+			if (ret < 0)
+				return ret;
+		}
+
+		ret = s5c73m3_read(state, 0x00095906, &status);
+		if (ret < 0)
+			return ret;
+		switch (status) {
+		case COMM_FW_UPDATE_FAIL:
+			v4l2_warn(sd, "Updating F-ROM firmware failed.\n");
+			return -EIO;
+		case COMM_FW_UPDATE_SUCCESS:
+			v4l2_warn(sd, "Updating F-ROM firmware finished.\n");
+			return 0;
+		}
+		++count;
+		msleep(20);
+	} while (count < 500);
+
+	v4l2_warn(sd, "Updating F-ROM firmware timed-out.\n");
+	return -ETIMEDOUT;
+}
+
+static int s5c73m3_spi_boot(struct s5c73m3 *state, bool load_fw)
+{
+	struct v4l2_subdev *sd = &state->sensor_sd;
+	int ret;
+
+	/* Run ARM MCU */
+	ret = s5c73m3_write(state, 0x30000004, 0xffff);
+	if (ret < 0)
+		return ret;
+
+	usleep_range(400, 500);
+
+	/* Check booting status */
+	ret = s5c73m3_system_status_wait(state, 0x0c, 100, 3);
+	if (ret < 0) {
+		v4l2_err(sd, "booting failed: %d\n", ret);
+		return ret;
+	}
+
+	/* P,M,S and Boot Mode */
+	ret = s5c73m3_write(state, 0x30100014, 0x2146);
+	if (ret < 0)
+		return ret;
+
+	ret = s5c73m3_write(state, 0x30100010, 0x210c);
+	if (ret < 0)
+		return ret;
+
+	usleep_range(200, 250);
+
+	/* Check SPI status */
+	ret = s5c73m3_system_status_wait(state, 0x210d, 100, 300);
+	if (ret < 0)
+		v4l2_err(sd, "SPI not ready: %d\n", ret);
+
+	/* Firmware download over SPI */
+	if (load_fw)
+		s5c73m3_load_fw(sd);
+
+	/* MCU reset */
+	ret = s5c73m3_write(state, 0x30000004, 0xfffd);
+	if (ret < 0)
+		return ret;
+
+	/* Remap */
+	ret = s5c73m3_write(state, 0x301000a4, 0x0183);
+	if (ret < 0)
+		return ret;
+
+	/* MCU restart */
+	ret = s5c73m3_write(state, 0x30000004, 0xffff);
+	if (ret < 0 || !load_fw)
+		return ret;
+
+	ret = s5c73m3_read_fw_version(state);
+	if (ret < 0)
+		return ret;
+
+	if (load_fw && update_fw) {
+		ret = s5c73m3_fw_update_from(state);
+		update_fw = 0;
+	}
+
+	return ret;
+}
+
+static int s5c73m3_set_timing_register_for_vdd(struct s5c73m3 *state)
+{
+	static const u32 regs[][2] = {
+		{ 0x30100018, 0x0618 },
+		{ 0x3010001c, 0x10c1 },
+		{ 0x30100020, 0x249e }
+	};
+	int ret;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(regs); i++) {
+		ret = s5c73m3_write(state, regs[i][0], regs[i][1]);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+static void s5c73m3_set_fw_file_version(struct s5c73m3 *state)
+{
+	switch (state->sensor_fw[0]) {
+	case 'G':
+	case 'O':
+		state->fw_file_version[0] = 'G';
+		break;
+	case 'S':
+	case 'Z':
+		state->fw_file_version[0] = 'Z';
+		break;
+	}
+
+	switch (state->sensor_fw[1]) {
+	case 'C'...'F':
+		state->fw_file_version[1] = state->sensor_fw[1];
+		break;
+	}
+}
+
+static int s5c73m3_get_fw_version(struct s5c73m3 *state)
+{
+	struct v4l2_subdev *sd = &state->sensor_sd;
+	int ret;
+
+	/* Run ARM MCU */
+	ret = s5c73m3_write(state, 0x30000004, 0xffff);
+	if (ret < 0)
+		return ret;
+	usleep_range(400, 500);
+
+	/* Check booting status */
+	ret = s5c73m3_system_status_wait(state, 0x0c, 100, 3);
+	if (ret < 0) {
+
+		v4l2_err(sd, "%s: booting failed: %d\n", __func__, ret);
+		return ret;
+	}
+
+	/* Change I/O Driver Current in order to read from F-ROM */
+	ret = s5c73m3_write(state, 0x30100120, 0x0820);
+	ret = s5c73m3_write(state, 0x30100124, 0x0820);
+
+	/* Offset Setting */
+	ret = s5c73m3_write(state, 0x00010418, 0x0008);
+
+	/* P,M,S and Boot Mode */
+	ret = s5c73m3_write(state, 0x30100014, 0x2146);
+	if (ret < 0)
+		return ret;
+	ret = s5c73m3_write(state, 0x30100010, 0x230c);
+	if (ret < 0)
+		return ret;
+
+	usleep_range(200, 250);
+
+	/* Check SPI status */
+	ret = s5c73m3_system_status_wait(state, 0x230e, 100, 300);
+	if (ret < 0)
+		v4l2_err(sd, "SPI not ready: %d\n", ret);
+
+	/* ARM reset */
+	ret = s5c73m3_write(state, 0x30000004, 0xfffd);
+	if (ret < 0)
+		return ret;
+
+	/* Remap */
+	ret = s5c73m3_write(state, 0x301000a4, 0x0183);
+	if (ret < 0)
+		return ret;
+
+	s5c73m3_set_timing_register_for_vdd(state);
+
+	ret = s5c73m3_read_fw_version(state);
+
+	s5c73m3_set_fw_file_version(state);
+
+	return ret;
+}
+
+static int s5c73m3_rom_boot(struct s5c73m3 *state, bool load_fw)
+{
+	static const u32 boot_regs[][2] = {
+		{ 0x3100010c, 0x0044 },
+		{ 0x31000108, 0x000d },
+		{ 0x31000304, 0x0001 },
+		{ 0x00010000, 0x5800 },
+		{ 0x00010002, 0x0002 },
+		{ 0x31000000, 0x0001 },
+		{ 0x30100014, 0x1b85 },
+		{ 0x30100010, 0x230c }
+	};
+	struct v4l2_subdev *sd = &state->sensor_sd;
+	int i, ret;
+
+	/* Run ARM MCU */
+	ret = s5c73m3_write(state, 0x30000004, 0xffff);
+	if (ret < 0)
+		return ret;
+	usleep_range(400, 450);
+
+	/* Check booting status */
+	ret = s5c73m3_system_status_wait(state, 0x0c, 100, 4);
+	if (ret < 0) {
+		v4l2_err(sd, "Booting failed: %d\n", ret);
+		return ret;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(boot_regs); i++) {
+		ret = s5c73m3_write(state, boot_regs[i][0], boot_regs[i][1]);
+		if (ret < 0)
+			return ret;
+	}
+	msleep(200);
+
+	/* Check the binary read status */
+	ret = s5c73m3_system_status_wait(state, 0x230e, 1000, 150);
+	if (ret < 0) {
+		v4l2_err(sd, "Binary read failed: %d\n", ret);
+		return ret;
+	}
+
+	/* ARM reset */
+	ret = s5c73m3_write(state, 0x30000004, 0xfffd);
+	if (ret < 0)
+		return ret;
+	/* Remap */
+	ret = s5c73m3_write(state, 0x301000a4, 0x0183);
+	if (ret < 0)
+		return ret;
+	/* MCU re-start */
+	ret = s5c73m3_write(state, 0x30000004, 0xffff);
+	if (ret < 0)
+		return ret;
+
+	state->isp_ready = 1;
+
+	return s5c73m3_read_fw_version(state);
+}
+
+static int s5c73m3_isp_init(struct s5c73m3 *state)
+{
+	int ret;
+
+	state->i2c_read_address = 0;
+	state->i2c_write_address = 0;
+
+	ret = s5c73m3_i2c_write(state->i2c_client, AHB_MSB_ADDR_PTR, 0x3310);
+	if (ret < 0)
+		return ret;
+
+	if (boot_from_rom)
+		return s5c73m3_rom_boot(state, true);
+	else
+		return s5c73m3_spi_boot(state, true);
+}
+
+static const struct s5c73m3_frame_size *s5c73m3_find_frame_size(
+					struct v4l2_mbus_framefmt *fmt,
+					enum s5c73m3_resolution_types idx)
+{
+	const struct s5c73m3_frame_size *fs;
+	const struct s5c73m3_frame_size *best_fs;
+	int best_dist = INT_MAX;
+	int i;
+
+	fs = s5c73m3_resolutions[idx];
+	best_fs = NULL;
+	for (i = 0; i < s5c73m3_resolutions_len[idx]; ++i) {
+		int dist = abs(fs->width - fmt->width) +
+						abs(fs->height - fmt->height);
+		if (dist < best_dist) {
+			best_dist = dist;
+			best_fs = fs;
+		}
+		++fs;
+	}
+
+	return best_fs;
+}
+
+static void s5c73m3_oif_try_format(struct s5c73m3 *state,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_format *fmt,
+				   const struct s5c73m3_frame_size **fs)
+{
+	struct v4l2_subdev *sd = &state->sensor_sd;
+	u32 code;
+
+	switch (fmt->pad) {
+	case OIF_ISP_PAD:
+		*fs = s5c73m3_find_frame_size(&fmt->format, RES_ISP);
+		code = S5C73M3_ISP_FMT;
+		break;
+	case OIF_JPEG_PAD:
+		*fs = s5c73m3_find_frame_size(&fmt->format, RES_JPEG);
+		code = S5C73M3_JPEG_FMT;
+		break;
+	case OIF_SOURCE_PAD:
+	default:
+		if (fmt->format.code == S5C73M3_JPEG_FMT)
+			code = S5C73M3_JPEG_FMT;
+		else
+			code = S5C73M3_ISP_FMT;
+
+		if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+			*fs = state->oif_pix_size[RES_ISP];
+		else
+			*fs = s5c73m3_find_frame_size(
+						v4l2_subdev_get_try_format(sd, sd_state,
+									   OIF_ISP_PAD),
+						RES_ISP);
+		break;
+	}
+
+	s5c73m3_fill_mbus_fmt(&fmt->format, *fs, code);
+}
+
+static void s5c73m3_try_format(struct s5c73m3 *state,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_format *fmt,
+			      const struct s5c73m3_frame_size **fs)
+{
+	u32 code;
+
+	if (fmt->pad == S5C73M3_ISP_PAD) {
+		*fs = s5c73m3_find_frame_size(&fmt->format, RES_ISP);
+		code = S5C73M3_ISP_FMT;
+	} else {
+		*fs = s5c73m3_find_frame_size(&fmt->format, RES_JPEG);
+		code = S5C73M3_JPEG_FMT;
+	}
+
+	s5c73m3_fill_mbus_fmt(&fmt->format, *fs, code);
+}
+
+static int s5c73m3_oif_g_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *fi)
+{
+	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
+
+	if (fi->pad != OIF_SOURCE_PAD)
+		return -EINVAL;
+
+	mutex_lock(&state->lock);
+	fi->interval = state->fiv->interval;
+	mutex_unlock(&state->lock);
+
+	return 0;
+}
+
+static int __s5c73m3_set_frame_interval(struct s5c73m3 *state,
+					struct v4l2_subdev_frame_interval *fi)
+{
+	const struct s5c73m3_frame_size *prev_size =
+						state->sensor_pix_size[RES_ISP];
+	const struct s5c73m3_interval *fiv = &s5c73m3_intervals[0];
+	unsigned int ret, min_err = UINT_MAX;
+	unsigned int i, fr_time;
+
+	if (fi->interval.denominator == 0)
+		return -EINVAL;
+
+	fr_time = fi->interval.numerator * 1000 / fi->interval.denominator;
+
+	for (i = 0; i < ARRAY_SIZE(s5c73m3_intervals); i++) {
+		const struct s5c73m3_interval *iv = &s5c73m3_intervals[i];
+
+		if (prev_size->width > iv->size.width ||
+		    prev_size->height > iv->size.height)
+			continue;
+
+		ret = abs(iv->interval.numerator / 1000 - fr_time);
+		if (ret < min_err) {
+			fiv = iv;
+			min_err = ret;
+		}
+	}
+	state->fiv = fiv;
+
+	v4l2_dbg(1, s5c73m3_dbg, &state->sensor_sd,
+		 "Changed frame interval to %u us\n", fiv->interval.numerator);
+	return 0;
+}
+
+static int s5c73m3_oif_s_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *fi)
+{
+	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
+	int ret;
+
+	if (fi->pad != OIF_SOURCE_PAD)
+		return -EINVAL;
+
+	v4l2_dbg(1, s5c73m3_dbg, sd, "Setting %d/%d frame interval\n",
+		 fi->interval.numerator, fi->interval.denominator);
+
+	mutex_lock(&state->lock);
+
+	ret = __s5c73m3_set_frame_interval(state, fi);
+	if (!ret) {
+		if (state->streaming)
+			ret = s5c73m3_set_frame_rate(state);
+		else
+			state->apply_fiv = 1;
+	}
+	mutex_unlock(&state->lock);
+	return ret;
+}
+
+static int s5c73m3_oif_enum_frame_interval(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_frame_interval_enum *fie)
+{
+	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
+	const struct s5c73m3_interval *fi;
+	int ret = 0;
+
+	if (fie->pad != OIF_SOURCE_PAD)
+		return -EINVAL;
+	if (fie->index >= ARRAY_SIZE(s5c73m3_intervals))
+		return -EINVAL;
+
+	mutex_lock(&state->lock);
+	fi = &s5c73m3_intervals[fie->index];
+	if (fie->width > fi->size.width || fie->height > fi->size.height)
+		ret = -EINVAL;
+	else
+		fie->interval = fi->interval;
+	mutex_unlock(&state->lock);
+
+	return ret;
+}
+
+static int s5c73m3_oif_get_pad_code(int pad, int index)
+{
+	if (pad == OIF_SOURCE_PAD) {
+		if (index > 1)
+			return -EINVAL;
+		return (index == 0) ? S5C73M3_ISP_FMT : S5C73M3_JPEG_FMT;
+	}
+
+	if (index > 0)
+		return -EINVAL;
+
+	return (pad == OIF_ISP_PAD) ? S5C73M3_ISP_FMT : S5C73M3_JPEG_FMT;
+}
+
+static int s5c73m3_get_fmt(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_state *sd_state,
+			   struct v4l2_subdev_format *fmt)
+{
+	struct s5c73m3 *state = sensor_sd_to_s5c73m3(sd);
+	const struct s5c73m3_frame_size *fs;
+	u32 code;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		fmt->format = *v4l2_subdev_get_try_format(sd, sd_state,
+							  fmt->pad);
+		return 0;
+	}
+
+	mutex_lock(&state->lock);
+
+	switch (fmt->pad) {
+	case S5C73M3_ISP_PAD:
+		code = S5C73M3_ISP_FMT;
+		fs = state->sensor_pix_size[RES_ISP];
+		break;
+	case S5C73M3_JPEG_PAD:
+		code = S5C73M3_JPEG_FMT;
+		fs = state->sensor_pix_size[RES_JPEG];
+		break;
+	default:
+		mutex_unlock(&state->lock);
+		return -EINVAL;
+	}
+	s5c73m3_fill_mbus_fmt(&fmt->format, fs, code);
+
+	mutex_unlock(&state->lock);
+	return 0;
+}
+
+static int s5c73m3_oif_get_fmt(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_state *sd_state,
+			   struct v4l2_subdev_format *fmt)
+{
+	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
+	const struct s5c73m3_frame_size *fs;
+	u32 code;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		fmt->format = *v4l2_subdev_get_try_format(sd, sd_state,
+							  fmt->pad);
+		return 0;
+	}
+
+	mutex_lock(&state->lock);
+
+	switch (fmt->pad) {
+	case OIF_ISP_PAD:
+		code = S5C73M3_ISP_FMT;
+		fs = state->oif_pix_size[RES_ISP];
+		break;
+	case OIF_JPEG_PAD:
+		code = S5C73M3_JPEG_FMT;
+		fs = state->oif_pix_size[RES_JPEG];
+		break;
+	case OIF_SOURCE_PAD:
+		code = state->mbus_code;
+		fs = state->oif_pix_size[RES_ISP];
+		break;
+	default:
+		mutex_unlock(&state->lock);
+		return -EINVAL;
+	}
+	s5c73m3_fill_mbus_fmt(&fmt->format, fs, code);
+
+	mutex_unlock(&state->lock);
+	return 0;
+}
+
+static int s5c73m3_set_fmt(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_state *sd_state,
+			   struct v4l2_subdev_format *fmt)
+{
+	const struct s5c73m3_frame_size *frame_size = NULL;
+	struct s5c73m3 *state = sensor_sd_to_s5c73m3(sd);
+	struct v4l2_mbus_framefmt *mf;
+	int ret = 0;
+
+	mutex_lock(&state->lock);
+
+	s5c73m3_try_format(state, sd_state, fmt, &frame_size);
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		mf = v4l2_subdev_get_try_format(sd, sd_state, fmt->pad);
+		*mf = fmt->format;
+	} else {
+		switch (fmt->pad) {
+		case S5C73M3_ISP_PAD:
+			state->sensor_pix_size[RES_ISP] = frame_size;
+			break;
+		case S5C73M3_JPEG_PAD:
+			state->sensor_pix_size[RES_JPEG] = frame_size;
+			break;
+		default:
+			ret = -EBUSY;
+		}
+
+		if (state->streaming)
+			ret = -EBUSY;
+		else
+			state->apply_fmt = 1;
+	}
+
+	mutex_unlock(&state->lock);
+
+	return ret;
+}
+
+static int s5c73m3_oif_set_fmt(struct v4l2_subdev *sd,
+			 struct v4l2_subdev_state *sd_state,
+			 struct v4l2_subdev_format *fmt)
+{
+	const struct s5c73m3_frame_size *frame_size = NULL;
+	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
+	struct v4l2_mbus_framefmt *mf;
+	int ret = 0;
+
+	mutex_lock(&state->lock);
+
+	s5c73m3_oif_try_format(state, sd_state, fmt, &frame_size);
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		mf = v4l2_subdev_get_try_format(sd, sd_state, fmt->pad);
+		*mf = fmt->format;
+		if (fmt->pad == OIF_ISP_PAD) {
+			mf = v4l2_subdev_get_try_format(sd, sd_state,
+							OIF_SOURCE_PAD);
+			mf->width = fmt->format.width;
+			mf->height = fmt->format.height;
+		}
+	} else {
+		switch (fmt->pad) {
+		case OIF_ISP_PAD:
+			state->oif_pix_size[RES_ISP] = frame_size;
+			break;
+		case OIF_JPEG_PAD:
+			state->oif_pix_size[RES_JPEG] = frame_size;
+			break;
+		case OIF_SOURCE_PAD:
+			state->mbus_code = fmt->format.code;
+			break;
+		default:
+			ret = -EBUSY;
+		}
+
+		if (state->streaming)
+			ret = -EBUSY;
+		else
+			state->apply_fmt = 1;
+	}
+
+	mutex_unlock(&state->lock);
+
+	return ret;
+}
+
+static int s5c73m3_oif_get_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
+				  struct v4l2_mbus_frame_desc *fd)
+{
+	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
+	int i;
+
+	if (pad != OIF_SOURCE_PAD || fd == NULL)
+		return -EINVAL;
+
+	mutex_lock(&state->lock);
+	fd->num_entries = 2;
+	for (i = 0; i < fd->num_entries; i++)
+		fd->entry[i] = state->frame_desc.entry[i];
+	mutex_unlock(&state->lock);
+
+	return 0;
+}
+
+static int s5c73m3_oif_set_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
+				      struct v4l2_mbus_frame_desc *fd)
+{
+	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
+	struct v4l2_mbus_frame_desc *frame_desc = &state->frame_desc;
+	int i;
+
+	if (pad != OIF_SOURCE_PAD || fd == NULL)
+		return -EINVAL;
+
+	fd->entry[0].length = 10 * SZ_1M;
+	fd->entry[1].length = max_t(u32, fd->entry[1].length,
+				    S5C73M3_EMBEDDED_DATA_MAXLEN);
+	fd->num_entries = 2;
+
+	mutex_lock(&state->lock);
+	for (i = 0; i < fd->num_entries; i++)
+		frame_desc->entry[i] = fd->entry[i];
+	mutex_unlock(&state->lock);
+
+	return 0;
+}
+
+static int s5c73m3_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	static const int codes[] = {
+			[S5C73M3_ISP_PAD] = S5C73M3_ISP_FMT,
+			[S5C73M3_JPEG_PAD] = S5C73M3_JPEG_FMT};
+
+	if (code->index > 0 || code->pad >= S5C73M3_NUM_PADS)
+		return -EINVAL;
+
+	code->code = codes[code->pad];
+
+	return 0;
+}
+
+static int s5c73m3_oif_enum_mbus_code(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *sd_state,
+				struct v4l2_subdev_mbus_code_enum *code)
+{
+	int ret;
+
+	ret = s5c73m3_oif_get_pad_code(code->pad, code->index);
+	if (ret < 0)
+		return ret;
+
+	code->code = ret;
+
+	return 0;
+}
+
+static int s5c73m3_enum_frame_size(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_frame_size_enum *fse)
+{
+	int idx;
+
+	if (fse->pad == S5C73M3_ISP_PAD) {
+		if (fse->code != S5C73M3_ISP_FMT)
+			return -EINVAL;
+		idx = RES_ISP;
+	} else{
+		if (fse->code != S5C73M3_JPEG_FMT)
+			return -EINVAL;
+		idx = RES_JPEG;
+	}
+
+	if (fse->index >= s5c73m3_resolutions_len[idx])
+		return -EINVAL;
+
+	fse->min_width  = s5c73m3_resolutions[idx][fse->index].width;
+	fse->max_width  = fse->min_width;
+	fse->max_height = s5c73m3_resolutions[idx][fse->index].height;
+	fse->min_height = fse->max_height;
+
+	return 0;
+}
+
+static int s5c73m3_oif_enum_frame_size(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
+	int idx;
+
+	if (fse->pad == OIF_SOURCE_PAD) {
+		if (fse->index > 0)
+			return -EINVAL;
+
+		switch (fse->code) {
+		case S5C73M3_JPEG_FMT:
+		case S5C73M3_ISP_FMT: {
+			unsigned w, h;
+
+			if (fse->which == V4L2_SUBDEV_FORMAT_TRY) {
+				struct v4l2_mbus_framefmt *mf;
+
+				mf = v4l2_subdev_get_try_format(sd, sd_state,
+								OIF_ISP_PAD);
+
+				w = mf->width;
+				h = mf->height;
+			} else {
+				const struct s5c73m3_frame_size *fs;
+
+				fs = state->oif_pix_size[RES_ISP];
+				w = fs->width;
+				h = fs->height;
+			}
+			fse->max_width = fse->min_width = w;
+			fse->max_height = fse->min_height = h;
+			return 0;
+		}
+		default:
+			return -EINVAL;
+		}
+	}
+
+	if (fse->code != s5c73m3_oif_get_pad_code(fse->pad, 0))
+		return -EINVAL;
+
+	if (fse->pad == OIF_JPEG_PAD)
+		idx = RES_JPEG;
+	else
+		idx = RES_ISP;
+
+	if (fse->index >= s5c73m3_resolutions_len[idx])
+		return -EINVAL;
+
+	fse->min_width  = s5c73m3_resolutions[idx][fse->index].width;
+	fse->max_width  = fse->min_width;
+	fse->max_height = s5c73m3_resolutions[idx][fse->index].height;
+	fse->min_height = fse->max_height;
+
+	return 0;
+}
+
+static int s5c73m3_oif_log_status(struct v4l2_subdev *sd)
+{
+	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
+
+	v4l2_ctrl_handler_log_status(sd->ctrl_handler, sd->name);
+
+	v4l2_info(sd, "power: %d, apply_fmt: %d\n", state->power,
+							state->apply_fmt);
+
+	return 0;
+}
+
+static int s5c73m3_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_mbus_framefmt *mf;
+
+	mf = v4l2_subdev_get_try_format(sd, fh->state, S5C73M3_ISP_PAD);
+	s5c73m3_fill_mbus_fmt(mf, &s5c73m3_isp_resolutions[1],
+						S5C73M3_ISP_FMT);
+
+	mf = v4l2_subdev_get_try_format(sd, fh->state, S5C73M3_JPEG_PAD);
+	s5c73m3_fill_mbus_fmt(mf, &s5c73m3_jpeg_resolutions[1],
+					S5C73M3_JPEG_FMT);
+
+	return 0;
+}
+
+static int s5c73m3_oif_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_mbus_framefmt *mf;
+
+	mf = v4l2_subdev_get_try_format(sd, fh->state, OIF_ISP_PAD);
+	s5c73m3_fill_mbus_fmt(mf, &s5c73m3_isp_resolutions[1],
+						S5C73M3_ISP_FMT);
+
+	mf = v4l2_subdev_get_try_format(sd, fh->state, OIF_JPEG_PAD);
+	s5c73m3_fill_mbus_fmt(mf, &s5c73m3_jpeg_resolutions[1],
+					S5C73M3_JPEG_FMT);
+
+	mf = v4l2_subdev_get_try_format(sd, fh->state, OIF_SOURCE_PAD);
+	s5c73m3_fill_mbus_fmt(mf, &s5c73m3_isp_resolutions[1],
+						S5C73M3_ISP_FMT);
+	return 0;
+}
+
+static int __s5c73m3_power_on(struct s5c73m3 *state)
+{
+	int i, ret;
+
+	for (i = 0; i < S5C73M3_MAX_SUPPLIES; i++) {
+		ret = regulator_enable(state->supplies[i].consumer);
+		if (ret)
+			goto err_reg_dis;
+	}
+
+	ret = clk_set_rate(state->clock, state->mclk_frequency);
+	if (ret < 0)
+		goto err_reg_dis;
+
+	ret = clk_prepare_enable(state->clock);
+	if (ret < 0)
+		goto err_reg_dis;
+
+	v4l2_dbg(1, s5c73m3_dbg, &state->oif_sd, "clock frequency: %ld\n",
+					clk_get_rate(state->clock));
+
+	gpiod_set_value(state->stby, 0);
+	usleep_range(100, 200);
+	gpiod_set_value(state->reset, 0);
+	usleep_range(50, 100);
+
+	return 0;
+
+err_reg_dis:
+	for (--i; i >= 0; i--)
+		regulator_disable(state->supplies[i].consumer);
+	return ret;
+}
+
+static int __s5c73m3_power_off(struct s5c73m3 *state)
+{
+	int i, ret;
+
+	gpiod_set_value(state->reset, 1);
+	usleep_range(10, 50);
+	gpiod_set_value(state->stby, 1);
+	usleep_range(100, 200);
+
+	clk_disable_unprepare(state->clock);
+
+	state->streaming = 0;
+	state->isp_ready = 0;
+
+	for (i = S5C73M3_MAX_SUPPLIES - 1; i >= 0; i--) {
+		ret = regulator_disable(state->supplies[i].consumer);
+		if (ret)
+			goto err;
+	}
+
+	return 0;
+err:
+	for (++i; i < S5C73M3_MAX_SUPPLIES; i++) {
+		int r = regulator_enable(state->supplies[i].consumer);
+		if (r < 0)
+			v4l2_err(&state->oif_sd, "Failed to re-enable %s: %d\n",
+				 state->supplies[i].supply, r);
+	}
+
+	clk_prepare_enable(state->clock);
+	return ret;
+}
+
+static int s5c73m3_oif_set_power(struct v4l2_subdev *sd, int on)
+{
+	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
+	int ret = 0;
+
+	mutex_lock(&state->lock);
+
+	if (on && !state->power) {
+		ret = __s5c73m3_power_on(state);
+		if (!ret)
+			ret = s5c73m3_isp_init(state);
+		if (!ret) {
+			state->apply_fiv = 1;
+			state->apply_fmt = 1;
+		}
+	} else if (state->power == !on) {
+		ret = s5c73m3_set_af_softlanding(state);
+		if (!ret)
+			ret = __s5c73m3_power_off(state);
+		else
+			v4l2_err(sd, "Soft landing lens failed\n");
+	}
+	if (!ret)
+		state->power += on ? 1 : -1;
+
+	v4l2_dbg(1, s5c73m3_dbg, sd, "%s: power: %d\n",
+		 __func__, state->power);
+
+	mutex_unlock(&state->lock);
+	return ret;
+}
+
+static int s5c73m3_oif_registered(struct v4l2_subdev *sd)
+{
+	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
+	int ret;
+
+	ret = v4l2_device_register_subdev(sd->v4l2_dev, &state->sensor_sd);
+	if (ret) {
+		v4l2_err(sd->v4l2_dev, "Failed to register %s\n",
+							state->oif_sd.name);
+		return ret;
+	}
+
+	ret = media_create_pad_link(&state->sensor_sd.entity,
+			S5C73M3_ISP_PAD, &state->oif_sd.entity, OIF_ISP_PAD,
+			MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
+
+	ret = media_create_pad_link(&state->sensor_sd.entity,
+			S5C73M3_JPEG_PAD, &state->oif_sd.entity, OIF_JPEG_PAD,
+			MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
+
+	return ret;
+}
+
+static void s5c73m3_oif_unregistered(struct v4l2_subdev *sd)
+{
+	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
+	v4l2_device_unregister_subdev(&state->sensor_sd);
+}
+
+static const struct v4l2_subdev_internal_ops s5c73m3_internal_ops = {
+	.open		= s5c73m3_open,
+};
+
+static const struct v4l2_subdev_pad_ops s5c73m3_pad_ops = {
+	.enum_mbus_code		= s5c73m3_enum_mbus_code,
+	.enum_frame_size	= s5c73m3_enum_frame_size,
+	.get_fmt		= s5c73m3_get_fmt,
+	.set_fmt		= s5c73m3_set_fmt,
+};
+
+static const struct v4l2_subdev_ops s5c73m3_subdev_ops = {
+	.pad	= &s5c73m3_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops oif_internal_ops = {
+	.registered	= s5c73m3_oif_registered,
+	.unregistered	= s5c73m3_oif_unregistered,
+	.open		= s5c73m3_oif_open,
+};
+
+static const struct v4l2_subdev_pad_ops s5c73m3_oif_pad_ops = {
+	.enum_mbus_code		= s5c73m3_oif_enum_mbus_code,
+	.enum_frame_size	= s5c73m3_oif_enum_frame_size,
+	.enum_frame_interval	= s5c73m3_oif_enum_frame_interval,
+	.get_fmt		= s5c73m3_oif_get_fmt,
+	.set_fmt		= s5c73m3_oif_set_fmt,
+	.get_frame_desc		= s5c73m3_oif_get_frame_desc,
+	.set_frame_desc		= s5c73m3_oif_set_frame_desc,
+};
+
+static const struct v4l2_subdev_core_ops s5c73m3_oif_core_ops = {
+	.s_power	= s5c73m3_oif_set_power,
+	.log_status	= s5c73m3_oif_log_status,
+};
+
+static const struct v4l2_subdev_video_ops s5c73m3_oif_video_ops = {
+	.s_stream		= s5c73m3_oif_s_stream,
+	.g_frame_interval	= s5c73m3_oif_g_frame_interval,
+	.s_frame_interval	= s5c73m3_oif_s_frame_interval,
+};
+
+static const struct v4l2_subdev_ops oif_subdev_ops = {
+	.core	= &s5c73m3_oif_core_ops,
+	.pad	= &s5c73m3_oif_pad_ops,
+	.video	= &s5c73m3_oif_video_ops,
+};
+
+static int s5c73m3_get_dt_data(struct s5c73m3 *state)
+{
+	struct device *dev = &state->i2c_client->dev;
+	struct device_node *node = dev->of_node;
+	struct device_node *node_ep;
+	struct v4l2_fwnode_endpoint ep = { .bus_type = 0 };
+	int ret;
+
+	if (!node)
+		return -EINVAL;
+
+	state->clock = devm_clk_get(dev, S5C73M3_CLK_NAME);
+	if (IS_ERR(state->clock))
+		return PTR_ERR(state->clock);
+
+	if (of_property_read_u32(node, "clock-frequency",
+				 &state->mclk_frequency)) {
+		state->mclk_frequency = S5C73M3_DEFAULT_MCLK_FREQ;
+		dev_info(dev, "using default %u Hz clock frequency\n",
+					state->mclk_frequency);
+	}
+
+	/* Request GPIO lines asserted */
+	state->stby = devm_gpiod_get(dev, "standby", GPIOD_OUT_HIGH);
+	if (IS_ERR(state->stby))
+		return dev_err_probe(dev, PTR_ERR(state->stby),
+				     "failed to request gpio S5C73M3_STBY\n");
+	gpiod_set_consumer_name(state->stby, "S5C73M3_STBY");
+	state->reset = devm_gpiod_get(dev, "xshutdown", GPIOD_OUT_HIGH);
+	if (IS_ERR(state->reset))
+		return dev_err_probe(dev, PTR_ERR(state->reset),
+				     "failed to request gpio S5C73M3_RST\n");
+	gpiod_set_consumer_name(state->reset, "S5C73M3_RST");
+
+	node_ep = of_graph_get_next_endpoint(node, NULL);
+	if (!node_ep) {
+		dev_warn(dev, "no endpoint defined for node: %pOF\n", node);
+		return 0;
+	}
+
+	ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(node_ep), &ep);
+	of_node_put(node_ep);
+	if (ret)
+		return ret;
+
+	if (ep.bus_type != V4L2_MBUS_CSI2_DPHY) {
+		dev_err(dev, "unsupported bus type\n");
+		return -EINVAL;
+	}
+	/*
+	 * Number of MIPI CSI-2 data lanes is currently not configurable,
+	 * always a default value of 4 lanes is used.
+	 */
+	if (ep.bus.mipi_csi2.num_data_lanes != S5C73M3_MIPI_DATA_LANES)
+		dev_info(dev, "falling back to 4 MIPI CSI-2 data lanes\n");
+
+	return 0;
+}
+
+static int s5c73m3_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct v4l2_subdev *sd;
+	struct v4l2_subdev *oif_sd;
+	struct s5c73m3 *state;
+	int ret, i;
+
+	state = devm_kzalloc(dev, sizeof(*state), GFP_KERNEL);
+	if (!state)
+		return -ENOMEM;
+
+	state->i2c_client = client;
+	ret = s5c73m3_get_dt_data(state);
+	if (ret < 0)
+		return ret;
+
+	mutex_init(&state->lock);
+	sd = &state->sensor_sd;
+	oif_sd = &state->oif_sd;
+
+	v4l2_subdev_init(sd, &s5c73m3_subdev_ops);
+	sd->owner = client->dev.driver->owner;
+	v4l2_set_subdevdata(sd, state);
+	strscpy(sd->name, "S5C73M3", sizeof(sd->name));
+
+	sd->internal_ops = &s5c73m3_internal_ops;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	state->sensor_pads[S5C73M3_JPEG_PAD].flags = MEDIA_PAD_FL_SOURCE;
+	state->sensor_pads[S5C73M3_ISP_PAD].flags = MEDIA_PAD_FL_SOURCE;
+	sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+	ret = media_entity_pads_init(&sd->entity, S5C73M3_NUM_PADS,
+							state->sensor_pads);
+	if (ret < 0)
+		return ret;
+
+	v4l2_i2c_subdev_init(oif_sd, client, &oif_subdev_ops);
+	/* Static name; NEVER use in new drivers! */
+	strscpy(oif_sd->name, "S5C73M3-OIF", sizeof(oif_sd->name));
+
+	oif_sd->internal_ops = &oif_internal_ops;
+	oif_sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	state->oif_pads[OIF_ISP_PAD].flags = MEDIA_PAD_FL_SINK;
+	state->oif_pads[OIF_JPEG_PAD].flags = MEDIA_PAD_FL_SINK;
+	state->oif_pads[OIF_SOURCE_PAD].flags = MEDIA_PAD_FL_SOURCE;
+	oif_sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_SCALER;
+
+	ret = media_entity_pads_init(&oif_sd->entity, OIF_NUM_PADS,
+							state->oif_pads);
+	if (ret < 0)
+		return ret;
+
+	for (i = 0; i < S5C73M3_MAX_SUPPLIES; i++)
+		state->supplies[i].supply = s5c73m3_supply_names[i];
+
+	ret = devm_regulator_bulk_get(dev, S5C73M3_MAX_SUPPLIES,
+			       state->supplies);
+	if (ret) {
+		dev_err(dev, "failed to get regulators\n");
+		goto out_err;
+	}
+
+	ret = s5c73m3_init_controls(state);
+	if (ret)
+		goto out_err;
+
+	state->sensor_pix_size[RES_ISP] = &s5c73m3_isp_resolutions[1];
+	state->sensor_pix_size[RES_JPEG] = &s5c73m3_jpeg_resolutions[1];
+	state->oif_pix_size[RES_ISP] = state->sensor_pix_size[RES_ISP];
+	state->oif_pix_size[RES_JPEG] = state->sensor_pix_size[RES_JPEG];
+
+	state->mbus_code = S5C73M3_ISP_FMT;
+
+	state->fiv = &s5c73m3_intervals[S5C73M3_DEFAULT_FRAME_INTERVAL];
+
+	state->fw_file_version[0] = 'G';
+	state->fw_file_version[1] = 'C';
+
+	ret = s5c73m3_register_spi_driver(state);
+	if (ret < 0)
+		goto out_err;
+
+	oif_sd->dev = dev;
+
+	ret = __s5c73m3_power_on(state);
+	if (ret < 0)
+		goto out_err1;
+
+	ret = s5c73m3_get_fw_version(state);
+	__s5c73m3_power_off(state);
+
+	if (ret < 0) {
+		dev_err(dev, "Device detection failed: %d\n", ret);
+		goto out_err1;
+	}
+
+	ret = v4l2_async_register_subdev(oif_sd);
+	if (ret < 0)
+		goto out_err1;
+
+	v4l2_info(sd, "%s: completed successfully\n", __func__);
+	return 0;
+
+out_err1:
+	s5c73m3_unregister_spi_driver(state);
+out_err:
+	media_entity_cleanup(&sd->entity);
+	return ret;
+}
+
+static void s5c73m3_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *oif_sd = i2c_get_clientdata(client);
+	struct s5c73m3 *state = oif_sd_to_s5c73m3(oif_sd);
+	struct v4l2_subdev *sensor_sd = &state->sensor_sd;
+
+	v4l2_async_unregister_subdev(oif_sd);
+
+	v4l2_ctrl_handler_free(oif_sd->ctrl_handler);
+	media_entity_cleanup(&oif_sd->entity);
+
+	v4l2_device_unregister_subdev(sensor_sd);
+	media_entity_cleanup(&sensor_sd->entity);
+
+	s5c73m3_unregister_spi_driver(state);
+}
+
+static const struct i2c_device_id s5c73m3_id[] = {
+	{ DRIVER_NAME, 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, s5c73m3_id);
+
+#ifdef CONFIG_OF
+static const struct of_device_id s5c73m3_of_match[] = {
+	{ .compatible = "samsung,s5c73m3" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, s5c73m3_of_match);
+#endif
+
+static struct i2c_driver s5c73m3_i2c_driver = {
+	.driver = {
+		.of_match_table = of_match_ptr(s5c73m3_of_match),
+		.name	= DRIVER_NAME,
+	},
+	.probe		= s5c73m3_probe,
+	.remove		= s5c73m3_remove,
+	.id_table	= s5c73m3_id,
+};
+
+module_i2c_driver(s5c73m3_i2c_driver);
+
+MODULE_DESCRIPTION("Samsung S5C73M3 camera driver");
+MODULE_AUTHOR("Sylwester Nawrocki <s.nawrocki@samsung.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/s5c73m3/s5c73m3-ctrls.c b/drivers/media/i2c/s5c73m3/s5c73m3-ctrls.c
new file mode 100644
index 000000000..1c8103670
--- /dev/null
+++ b/drivers/media/i2c/s5c73m3/s5c73m3-ctrls.c
@@ -0,0 +1,553 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Samsung LSI S5C73M3 8M pixel camera driver
+ *
+ * Copyright (C) 2012, Samsung Electronics, Co., Ltd.
+ * Sylwester Nawrocki <s.nawrocki@samsung.com>
+ * Andrzej Hajda <a.hajda@samsung.com>
+ */
+
+#include <linux/sizes.h>
+#include <linux/delay.h>
+#include <linux/firmware.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/media.h>
+#include <linux/module.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/spi/spi.h>
+#include <linux/videodev2.h>
+#include <media/media-entity.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-mediabus.h>
+
+#include "s5c73m3.h"
+
+static int s5c73m3_get_af_status(struct s5c73m3 *state, struct v4l2_ctrl *ctrl)
+{
+	u16 reg = REG_AF_STATUS_UNFOCUSED;
+
+	int ret = s5c73m3_read(state, REG_AF_STATUS, &reg);
+
+	switch (reg) {
+	case REG_CAF_STATUS_FIND_SEARCH_DIR:
+	case REG_AF_STATUS_FOCUSING:
+	case REG_CAF_STATUS_FOCUSING:
+		ctrl->val = V4L2_AUTO_FOCUS_STATUS_BUSY;
+		break;
+	case REG_CAF_STATUS_FOCUSED:
+	case REG_AF_STATUS_FOCUSED:
+		ctrl->val = V4L2_AUTO_FOCUS_STATUS_REACHED;
+		break;
+	default:
+		v4l2_info(&state->sensor_sd, "Unknown AF status %#x\n", reg);
+		fallthrough;
+	case REG_CAF_STATUS_UNFOCUSED:
+	case REG_AF_STATUS_UNFOCUSED:
+	case REG_AF_STATUS_INVALID:
+		ctrl->val = V4L2_AUTO_FOCUS_STATUS_FAILED;
+		break;
+	}
+
+	return ret;
+}
+
+static int s5c73m3_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = ctrl_to_sensor_sd(ctrl);
+	struct s5c73m3 *state = sensor_sd_to_s5c73m3(sd);
+	int ret;
+
+	if (state->power == 0)
+		return -EBUSY;
+
+	switch (ctrl->id) {
+	case V4L2_CID_FOCUS_AUTO:
+		ret = s5c73m3_get_af_status(state, state->ctrls.af_status);
+		if (ret)
+			return ret;
+		break;
+	}
+
+	return 0;
+}
+
+static int s5c73m3_set_colorfx(struct s5c73m3 *state, int val)
+{
+	static const unsigned short colorfx[][2] = {
+		{ V4L2_COLORFX_NONE,	 COMM_IMAGE_EFFECT_NONE },
+		{ V4L2_COLORFX_BW,	 COMM_IMAGE_EFFECT_MONO },
+		{ V4L2_COLORFX_SEPIA,	 COMM_IMAGE_EFFECT_SEPIA },
+		{ V4L2_COLORFX_NEGATIVE, COMM_IMAGE_EFFECT_NEGATIVE },
+		{ V4L2_COLORFX_AQUA,	 COMM_IMAGE_EFFECT_AQUA },
+	};
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(colorfx); i++) {
+		if (colorfx[i][0] != val)
+			continue;
+
+		v4l2_dbg(1, s5c73m3_dbg, &state->sensor_sd,
+			 "Setting %s color effect\n",
+			 v4l2_ctrl_get_menu(state->ctrls.colorfx->id)[i]);
+
+		return s5c73m3_isp_command(state, COMM_IMAGE_EFFECT,
+					 colorfx[i][1]);
+	}
+	return -EINVAL;
+}
+
+/* Set exposure metering/exposure bias */
+static int s5c73m3_set_exposure(struct s5c73m3 *state, int auto_exp)
+{
+	struct v4l2_subdev *sd = &state->sensor_sd;
+	struct s5c73m3_ctrls *ctrls = &state->ctrls;
+	int ret = 0;
+
+	if (ctrls->exposure_metering->is_new) {
+		u16 metering;
+
+		switch (ctrls->exposure_metering->val) {
+		case V4L2_EXPOSURE_METERING_CENTER_WEIGHTED:
+			metering = COMM_METERING_CENTER;
+			break;
+		case V4L2_EXPOSURE_METERING_SPOT:
+			metering = COMM_METERING_SPOT;
+			break;
+		default:
+			metering = COMM_METERING_AVERAGE;
+			break;
+		}
+
+		ret = s5c73m3_isp_command(state, COMM_METERING, metering);
+	}
+
+	if (!ret && ctrls->exposure_bias->is_new) {
+		u16 exp_bias = ctrls->exposure_bias->val;
+		ret = s5c73m3_isp_command(state, COMM_EV, exp_bias);
+	}
+
+	v4l2_dbg(1, s5c73m3_dbg, sd,
+		 "%s: exposure bias: %#x, metering: %#x (%d)\n",  __func__,
+		 ctrls->exposure_bias->val, ctrls->exposure_metering->val, ret);
+
+	return ret;
+}
+
+static int s5c73m3_set_white_balance(struct s5c73m3 *state, int val)
+{
+	static const unsigned short wb[][2] = {
+		{ V4L2_WHITE_BALANCE_INCANDESCENT,  COMM_AWB_MODE_INCANDESCENT},
+		{ V4L2_WHITE_BALANCE_FLUORESCENT,   COMM_AWB_MODE_FLUORESCENT1},
+		{ V4L2_WHITE_BALANCE_FLUORESCENT_H, COMM_AWB_MODE_FLUORESCENT2},
+		{ V4L2_WHITE_BALANCE_CLOUDY,        COMM_AWB_MODE_CLOUDY},
+		{ V4L2_WHITE_BALANCE_DAYLIGHT,      COMM_AWB_MODE_DAYLIGHT},
+		{ V4L2_WHITE_BALANCE_AUTO,          COMM_AWB_MODE_AUTO},
+	};
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(wb); i++) {
+		if (wb[i][0] != val)
+			continue;
+
+		v4l2_dbg(1, s5c73m3_dbg, &state->sensor_sd,
+			 "Setting white balance to: %s\n",
+			 v4l2_ctrl_get_menu(state->ctrls.auto_wb->id)[i]);
+
+		return s5c73m3_isp_command(state, COMM_AWB_MODE, wb[i][1]);
+	}
+
+	return -EINVAL;
+}
+
+static int s5c73m3_af_run(struct s5c73m3 *state, bool on)
+{
+	struct s5c73m3_ctrls *c = &state->ctrls;
+
+	if (!on)
+		return s5c73m3_isp_command(state, COMM_AF_CON,
+							COMM_AF_CON_STOP);
+
+	if (c->focus_auto->val)
+		return s5c73m3_isp_command(state, COMM_AF_MODE,
+					   COMM_AF_MODE_PREVIEW_CAF_START);
+
+	return s5c73m3_isp_command(state, COMM_AF_CON, COMM_AF_CON_START);
+}
+
+static int s5c73m3_3a_lock(struct s5c73m3 *state, struct v4l2_ctrl *ctrl)
+{
+	bool awb_lock = ctrl->val & V4L2_LOCK_WHITE_BALANCE;
+	bool ae_lock = ctrl->val & V4L2_LOCK_EXPOSURE;
+	bool af_lock = ctrl->val & V4L2_LOCK_FOCUS;
+	int ret = 0;
+
+	if ((ctrl->val ^ ctrl->cur.val) & V4L2_LOCK_EXPOSURE) {
+		ret = s5c73m3_isp_command(state, COMM_AE_CON,
+				ae_lock ? COMM_AE_STOP : COMM_AE_START);
+		if (ret)
+			return ret;
+	}
+
+	if (((ctrl->val ^ ctrl->cur.val) & V4L2_LOCK_WHITE_BALANCE)
+	    && state->ctrls.auto_wb->val) {
+		ret = s5c73m3_isp_command(state, COMM_AWB_CON,
+			awb_lock ? COMM_AWB_STOP : COMM_AWB_START);
+		if (ret)
+			return ret;
+	}
+
+	if ((ctrl->val ^ ctrl->cur.val) & V4L2_LOCK_FOCUS)
+		ret = s5c73m3_af_run(state, !af_lock);
+
+	return ret;
+}
+
+static int s5c73m3_set_auto_focus(struct s5c73m3 *state, int caf)
+{
+	struct s5c73m3_ctrls *c = &state->ctrls;
+	int ret = 1;
+
+	if (c->af_distance->is_new) {
+		u16 mode = (c->af_distance->val == V4L2_AUTO_FOCUS_RANGE_MACRO)
+				? COMM_AF_MODE_MACRO : COMM_AF_MODE_NORMAL;
+		ret = s5c73m3_isp_command(state, COMM_AF_MODE, mode);
+		if (ret != 0)
+			return ret;
+	}
+
+	if (!ret || (c->focus_auto->is_new && c->focus_auto->val) ||
+							c->af_start->is_new)
+		ret = s5c73m3_af_run(state, 1);
+	else if ((c->focus_auto->is_new && !c->focus_auto->val) ||
+							c->af_stop->is_new)
+		ret = s5c73m3_af_run(state, 0);
+	else
+		ret = 0;
+
+	return ret;
+}
+
+static int s5c73m3_set_contrast(struct s5c73m3 *state, int val)
+{
+	u16 reg = (val < 0) ? -val + 2 : val;
+	return s5c73m3_isp_command(state, COMM_CONTRAST, reg);
+}
+
+static int s5c73m3_set_saturation(struct s5c73m3 *state, int val)
+{
+	u16 reg = (val < 0) ? -val + 2 : val;
+	return s5c73m3_isp_command(state, COMM_SATURATION, reg);
+}
+
+static int s5c73m3_set_sharpness(struct s5c73m3 *state, int val)
+{
+	u16 reg = (val < 0) ? -val + 2 : val;
+	return s5c73m3_isp_command(state, COMM_SHARPNESS, reg);
+}
+
+static int s5c73m3_set_iso(struct s5c73m3 *state, int val)
+{
+	u32 iso;
+
+	if (val == V4L2_ISO_SENSITIVITY_MANUAL)
+		iso = state->ctrls.iso->val + 1;
+	else
+		iso = 0;
+
+	return s5c73m3_isp_command(state, COMM_ISO, iso);
+}
+
+static int s5c73m3_set_stabilization(struct s5c73m3 *state, int val)
+{
+	struct v4l2_subdev *sd = &state->sensor_sd;
+
+	v4l2_dbg(1, s5c73m3_dbg, sd, "Image stabilization: %d\n", val);
+
+	return s5c73m3_isp_command(state, COMM_FRAME_RATE, val ?
+			COMM_FRAME_RATE_ANTI_SHAKE : COMM_FRAME_RATE_AUTO_SET);
+}
+
+static int s5c73m3_set_jpeg_quality(struct s5c73m3 *state, int quality)
+{
+	int reg;
+
+	if (quality <= 65)
+		reg = COMM_IMAGE_QUALITY_NORMAL;
+	else if (quality <= 75)
+		reg = COMM_IMAGE_QUALITY_FINE;
+	else
+		reg = COMM_IMAGE_QUALITY_SUPERFINE;
+
+	return s5c73m3_isp_command(state, COMM_IMAGE_QUALITY, reg);
+}
+
+static int s5c73m3_set_scene_program(struct s5c73m3 *state, int val)
+{
+	static const unsigned short scene_lookup[] = {
+		COMM_SCENE_MODE_NONE,	     /* V4L2_SCENE_MODE_NONE */
+		COMM_SCENE_MODE_AGAINST_LIGHT,/* V4L2_SCENE_MODE_BACKLIGHT */
+		COMM_SCENE_MODE_BEACH,	     /* V4L2_SCENE_MODE_BEACH_SNOW */
+		COMM_SCENE_MODE_CANDLE,	     /* V4L2_SCENE_MODE_CANDLE_LIGHT */
+		COMM_SCENE_MODE_DAWN,	     /* V4L2_SCENE_MODE_DAWN_DUSK */
+		COMM_SCENE_MODE_FALL,	     /* V4L2_SCENE_MODE_FALL_COLORS */
+		COMM_SCENE_MODE_FIRE,	     /* V4L2_SCENE_MODE_FIREWORKS */
+		COMM_SCENE_MODE_LANDSCAPE,    /* V4L2_SCENE_MODE_LANDSCAPE */
+		COMM_SCENE_MODE_NIGHT,	     /* V4L2_SCENE_MODE_NIGHT */
+		COMM_SCENE_MODE_INDOOR,	     /* V4L2_SCENE_MODE_PARTY_INDOOR */
+		COMM_SCENE_MODE_PORTRAIT,     /* V4L2_SCENE_MODE_PORTRAIT */
+		COMM_SCENE_MODE_SPORTS,	     /* V4L2_SCENE_MODE_SPORTS */
+		COMM_SCENE_MODE_SUNSET,	     /* V4L2_SCENE_MODE_SUNSET */
+		COMM_SCENE_MODE_TEXT,	     /* V4L2_SCENE_MODE_TEXT */
+	};
+
+	v4l2_dbg(1, s5c73m3_dbg, &state->sensor_sd, "Setting %s scene mode\n",
+		 v4l2_ctrl_get_menu(state->ctrls.scene_mode->id)[val]);
+
+	return s5c73m3_isp_command(state, COMM_SCENE_MODE, scene_lookup[val]);
+}
+
+static int s5c73m3_set_power_line_freq(struct s5c73m3 *state, int val)
+{
+	unsigned int pwr_line_freq = COMM_FLICKER_NONE;
+
+	switch (val) {
+	case V4L2_CID_POWER_LINE_FREQUENCY_DISABLED:
+		pwr_line_freq = COMM_FLICKER_NONE;
+		break;
+	case V4L2_CID_POWER_LINE_FREQUENCY_50HZ:
+		pwr_line_freq = COMM_FLICKER_AUTO_50HZ;
+		break;
+	case V4L2_CID_POWER_LINE_FREQUENCY_60HZ:
+		pwr_line_freq = COMM_FLICKER_AUTO_60HZ;
+		break;
+	default:
+	case V4L2_CID_POWER_LINE_FREQUENCY_AUTO:
+		pwr_line_freq = COMM_FLICKER_NONE;
+	}
+
+	return s5c73m3_isp_command(state, COMM_FLICKER_MODE, pwr_line_freq);
+}
+
+static int s5c73m3_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = ctrl_to_sensor_sd(ctrl);
+	struct s5c73m3 *state = sensor_sd_to_s5c73m3(sd);
+	int ret = 0;
+
+	v4l2_dbg(1, s5c73m3_dbg, sd, "set_ctrl: %s, value: %d\n",
+		 ctrl->name, ctrl->val);
+
+	mutex_lock(&state->lock);
+	/*
+	 * If the device is not powered up by the host driver do
+	 * not apply any controls to H/W at this time. Instead
+	 * the controls will be restored right after power-up.
+	 */
+	if (state->power == 0)
+		goto unlock;
+
+	if (ctrl->flags & V4L2_CTRL_FLAG_INACTIVE) {
+		ret = -EINVAL;
+		goto unlock;
+	}
+
+	switch (ctrl->id) {
+	case V4L2_CID_3A_LOCK:
+		ret = s5c73m3_3a_lock(state, ctrl);
+		break;
+
+	case V4L2_CID_AUTO_N_PRESET_WHITE_BALANCE:
+		ret = s5c73m3_set_white_balance(state, ctrl->val);
+		break;
+
+	case V4L2_CID_CONTRAST:
+		ret = s5c73m3_set_contrast(state, ctrl->val);
+		break;
+
+	case V4L2_CID_COLORFX:
+		ret = s5c73m3_set_colorfx(state, ctrl->val);
+		break;
+
+	case V4L2_CID_EXPOSURE_AUTO:
+		ret = s5c73m3_set_exposure(state, ctrl->val);
+		break;
+
+	case V4L2_CID_FOCUS_AUTO:
+		ret = s5c73m3_set_auto_focus(state, ctrl->val);
+		break;
+
+	case V4L2_CID_IMAGE_STABILIZATION:
+		ret = s5c73m3_set_stabilization(state, ctrl->val);
+		break;
+
+	case V4L2_CID_ISO_SENSITIVITY:
+		ret = s5c73m3_set_iso(state, ctrl->val);
+		break;
+
+	case V4L2_CID_JPEG_COMPRESSION_QUALITY:
+		ret = s5c73m3_set_jpeg_quality(state, ctrl->val);
+		break;
+
+	case V4L2_CID_POWER_LINE_FREQUENCY:
+		ret = s5c73m3_set_power_line_freq(state, ctrl->val);
+		break;
+
+	case V4L2_CID_SATURATION:
+		ret = s5c73m3_set_saturation(state, ctrl->val);
+		break;
+
+	case V4L2_CID_SCENE_MODE:
+		ret = s5c73m3_set_scene_program(state, ctrl->val);
+		break;
+
+	case V4L2_CID_SHARPNESS:
+		ret = s5c73m3_set_sharpness(state, ctrl->val);
+		break;
+
+	case V4L2_CID_WIDE_DYNAMIC_RANGE:
+		ret = s5c73m3_isp_command(state, COMM_WDR, !!ctrl->val);
+		break;
+
+	case V4L2_CID_ZOOM_ABSOLUTE:
+		ret = s5c73m3_isp_command(state, COMM_ZOOM_STEP, ctrl->val);
+		break;
+	}
+unlock:
+	mutex_unlock(&state->lock);
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops s5c73m3_ctrl_ops = {
+	.g_volatile_ctrl	= s5c73m3_g_volatile_ctrl,
+	.s_ctrl			= s5c73m3_s_ctrl,
+};
+
+/* Supported manual ISO values */
+static const s64 iso_qmenu[] = {
+	/* COMM_ISO: 0x0001...0x0004 */
+	100, 200, 400, 800,
+};
+
+/* Supported exposure bias values (-2.0EV...+2.0EV) */
+static const s64 ev_bias_qmenu[] = {
+	/* COMM_EV: 0x0000...0x0008 */
+	-2000, -1500, -1000, -500, 0, 500, 1000, 1500, 2000
+};
+
+int s5c73m3_init_controls(struct s5c73m3 *state)
+{
+	const struct v4l2_ctrl_ops *ops = &s5c73m3_ctrl_ops;
+	struct s5c73m3_ctrls *ctrls = &state->ctrls;
+	struct v4l2_ctrl_handler *hdl = &ctrls->handler;
+
+	int ret = v4l2_ctrl_handler_init(hdl, 22);
+	if (ret)
+		return ret;
+
+	/* White balance */
+	ctrls->auto_wb = v4l2_ctrl_new_std_menu(hdl, ops,
+			V4L2_CID_AUTO_N_PRESET_WHITE_BALANCE,
+			9, ~0x15e, V4L2_WHITE_BALANCE_AUTO);
+
+	/* Exposure (only automatic exposure) */
+	ctrls->auto_exposure = v4l2_ctrl_new_std_menu(hdl, ops,
+			V4L2_CID_EXPOSURE_AUTO, 0, ~0x01, V4L2_EXPOSURE_AUTO);
+
+	ctrls->exposure_bias = v4l2_ctrl_new_int_menu(hdl, ops,
+			V4L2_CID_AUTO_EXPOSURE_BIAS,
+			ARRAY_SIZE(ev_bias_qmenu) - 1,
+			ARRAY_SIZE(ev_bias_qmenu)/2 - 1,
+			ev_bias_qmenu);
+
+	ctrls->exposure_metering = v4l2_ctrl_new_std_menu(hdl, ops,
+			V4L2_CID_EXPOSURE_METERING,
+			2, ~0x7, V4L2_EXPOSURE_METERING_AVERAGE);
+
+	/* Auto focus */
+	ctrls->focus_auto = v4l2_ctrl_new_std(hdl, ops,
+			V4L2_CID_FOCUS_AUTO, 0, 1, 1, 0);
+
+	ctrls->af_start = v4l2_ctrl_new_std(hdl, ops,
+			V4L2_CID_AUTO_FOCUS_START, 0, 1, 1, 0);
+
+	ctrls->af_stop = v4l2_ctrl_new_std(hdl, ops,
+			V4L2_CID_AUTO_FOCUS_STOP, 0, 1, 1, 0);
+
+	ctrls->af_status = v4l2_ctrl_new_std(hdl, ops,
+			V4L2_CID_AUTO_FOCUS_STATUS, 0,
+			(V4L2_AUTO_FOCUS_STATUS_BUSY |
+			 V4L2_AUTO_FOCUS_STATUS_REACHED |
+			 V4L2_AUTO_FOCUS_STATUS_FAILED),
+			0, V4L2_AUTO_FOCUS_STATUS_IDLE);
+
+	ctrls->af_distance = v4l2_ctrl_new_std_menu(hdl, ops,
+			V4L2_CID_AUTO_FOCUS_RANGE,
+			V4L2_AUTO_FOCUS_RANGE_MACRO,
+			~(1 << V4L2_AUTO_FOCUS_RANGE_NORMAL |
+			  1 << V4L2_AUTO_FOCUS_RANGE_MACRO),
+			V4L2_AUTO_FOCUS_RANGE_NORMAL);
+	/* ISO sensitivity */
+	ctrls->auto_iso = v4l2_ctrl_new_std_menu(hdl, ops,
+			V4L2_CID_ISO_SENSITIVITY_AUTO, 1, 0,
+			V4L2_ISO_SENSITIVITY_AUTO);
+
+	ctrls->iso = v4l2_ctrl_new_int_menu(hdl, ops,
+			V4L2_CID_ISO_SENSITIVITY, ARRAY_SIZE(iso_qmenu) - 1,
+			ARRAY_SIZE(iso_qmenu)/2 - 1, iso_qmenu);
+
+	ctrls->contrast = v4l2_ctrl_new_std(hdl, ops,
+			V4L2_CID_CONTRAST, -2, 2, 1, 0);
+
+	ctrls->saturation = v4l2_ctrl_new_std(hdl, ops,
+			V4L2_CID_SATURATION, -2, 2, 1, 0);
+
+	ctrls->sharpness = v4l2_ctrl_new_std(hdl, ops,
+			V4L2_CID_SHARPNESS, -2, 2, 1, 0);
+
+	ctrls->zoom = v4l2_ctrl_new_std(hdl, ops,
+			V4L2_CID_ZOOM_ABSOLUTE, 0, 30, 1, 0);
+
+	ctrls->colorfx = v4l2_ctrl_new_std_menu(hdl, ops, V4L2_CID_COLORFX,
+			V4L2_COLORFX_AQUA, ~0x40f, V4L2_COLORFX_NONE);
+
+	ctrls->wdr = v4l2_ctrl_new_std(hdl, ops,
+			V4L2_CID_WIDE_DYNAMIC_RANGE, 0, 1, 1, 0);
+
+	ctrls->stabilization = v4l2_ctrl_new_std(hdl, ops,
+			V4L2_CID_IMAGE_STABILIZATION, 0, 1, 1, 0);
+
+	v4l2_ctrl_new_std_menu(hdl, ops, V4L2_CID_POWER_LINE_FREQUENCY,
+			       V4L2_CID_POWER_LINE_FREQUENCY_AUTO, 0,
+			       V4L2_CID_POWER_LINE_FREQUENCY_AUTO);
+
+	ctrls->jpeg_quality = v4l2_ctrl_new_std(hdl, ops,
+			V4L2_CID_JPEG_COMPRESSION_QUALITY, 1, 100, 1, 80);
+
+	ctrls->scene_mode = v4l2_ctrl_new_std_menu(hdl, ops,
+			V4L2_CID_SCENE_MODE, V4L2_SCENE_MODE_TEXT, ~0x3fff,
+			V4L2_SCENE_MODE_NONE);
+
+	ctrls->aaa_lock = v4l2_ctrl_new_std(hdl, ops,
+			V4L2_CID_3A_LOCK, 0, 0x7, 0, 0);
+
+	if (hdl->error) {
+		ret = hdl->error;
+		v4l2_ctrl_handler_free(hdl);
+		return ret;
+	}
+
+	v4l2_ctrl_auto_cluster(3, &ctrls->auto_exposure, 0, false);
+	ctrls->auto_iso->flags |= V4L2_CTRL_FLAG_VOLATILE |
+				V4L2_CTRL_FLAG_UPDATE;
+	v4l2_ctrl_auto_cluster(2, &ctrls->auto_iso, 0, false);
+	ctrls->af_status->flags |= V4L2_CTRL_FLAG_VOLATILE;
+	v4l2_ctrl_cluster(5, &ctrls->focus_auto);
+
+	state->sensor_sd.ctrl_handler = hdl;
+
+	return 0;
+}
diff --git a/drivers/media/i2c/s5c73m3/s5c73m3-spi.c b/drivers/media/i2c/s5c73m3/s5c73m3-spi.c
new file mode 100644
index 000000000..7fe61187a
--- /dev/null
+++ b/drivers/media/i2c/s5c73m3/s5c73m3-spi.c
@@ -0,0 +1,147 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Samsung LSI S5C73M3 8M pixel camera driver
+ *
+ * Copyright (C) 2012, Samsung Electronics, Co., Ltd.
+ * Sylwester Nawrocki <s.nawrocki@samsung.com>
+ * Andrzej Hajda <a.hajda@samsung.com>
+ */
+
+#include <linux/sizes.h>
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/media.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/spi/spi.h>
+
+#include "s5c73m3.h"
+
+#define S5C73M3_SPI_DRV_NAME "S5C73M3-SPI"
+
+static const struct of_device_id s5c73m3_spi_ids[] = {
+	{ .compatible = "samsung,s5c73m3" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, s5c73m3_spi_ids);
+
+enum spi_direction {
+	SPI_DIR_RX,
+	SPI_DIR_TX
+};
+
+static int spi_xmit(struct spi_device *spi_dev, void *addr, const int len,
+							enum spi_direction dir)
+{
+	struct spi_message msg;
+	int r;
+	struct spi_transfer xfer = {
+		.len	= len,
+	};
+
+	if (dir == SPI_DIR_TX)
+		xfer.tx_buf = addr;
+	else
+		xfer.rx_buf = addr;
+
+	if (spi_dev == NULL) {
+		pr_err("SPI device is uninitialized\n");
+		return -ENODEV;
+	}
+
+	spi_message_init(&msg);
+	spi_message_add_tail(&xfer, &msg);
+
+	r = spi_sync(spi_dev, &msg);
+	if (r < 0)
+		dev_err(&spi_dev->dev, "%s spi_sync failed %d\n", __func__, r);
+
+	return r;
+}
+
+int s5c73m3_spi_write(struct s5c73m3 *state, const void *addr,
+		      const unsigned int len, const unsigned int tx_size)
+{
+	struct spi_device *spi_dev = state->spi_dev;
+	u32 count = len / tx_size;
+	u32 extra = len % tx_size;
+	unsigned int i, j = 0;
+	u8 padding[32];
+	int r = 0;
+
+	memset(padding, 0, sizeof(padding));
+
+	for (i = 0; i < count; i++) {
+		r = spi_xmit(spi_dev, (void *)addr + j, tx_size, SPI_DIR_TX);
+		if (r < 0)
+			return r;
+		j += tx_size;
+	}
+
+	if (extra > 0) {
+		r = spi_xmit(spi_dev, (void *)addr + j, extra, SPI_DIR_TX);
+		if (r < 0)
+			return r;
+	}
+
+	return spi_xmit(spi_dev, padding, sizeof(padding), SPI_DIR_TX);
+}
+
+int s5c73m3_spi_read(struct s5c73m3 *state, void *addr,
+		     const unsigned int len, const unsigned int tx_size)
+{
+	struct spi_device *spi_dev = state->spi_dev;
+	u32 count = len / tx_size;
+	u32 extra = len % tx_size;
+	unsigned int i, j = 0;
+	int r = 0;
+
+	for (i = 0; i < count; i++) {
+		r = spi_xmit(spi_dev, addr + j, tx_size, SPI_DIR_RX);
+		if (r < 0)
+			return r;
+		j += tx_size;
+	}
+
+	if (extra > 0)
+		return spi_xmit(spi_dev, addr + j, extra, SPI_DIR_RX);
+
+	return 0;
+}
+
+static int s5c73m3_spi_probe(struct spi_device *spi)
+{
+	int r;
+	struct s5c73m3 *state = container_of(spi->dev.driver, struct s5c73m3,
+					     spidrv.driver);
+	spi->bits_per_word = 32;
+
+	r = spi_setup(spi);
+	if (r < 0) {
+		dev_err(&spi->dev, "spi_setup() failed\n");
+		return r;
+	}
+
+	mutex_lock(&state->lock);
+	state->spi_dev = spi;
+	mutex_unlock(&state->lock);
+
+	v4l2_info(&state->sensor_sd, "S5C73M3 SPI probed successfully\n");
+	return 0;
+}
+
+int s5c73m3_register_spi_driver(struct s5c73m3 *state)
+{
+	struct spi_driver *spidrv = &state->spidrv;
+
+	spidrv->probe = s5c73m3_spi_probe;
+	spidrv->driver.name = S5C73M3_SPI_DRV_NAME;
+	spidrv->driver.of_match_table = s5c73m3_spi_ids;
+
+	return spi_register_driver(spidrv);
+}
+
+void s5c73m3_unregister_spi_driver(struct s5c73m3 *state)
+{
+	spi_unregister_driver(&state->spidrv);
+}
diff --git a/drivers/media/i2c/s5c73m3/s5c73m3.h b/drivers/media/i2c/s5c73m3/s5c73m3.h
new file mode 100644
index 000000000..627e80cf5
--- /dev/null
+++ b/drivers/media/i2c/s5c73m3/s5c73m3.h
@@ -0,0 +1,450 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Samsung LSI S5C73M3 8M pixel camera driver
+ *
+ * Copyright (C) 2012, Samsung Electronics, Co., Ltd.
+ * Sylwester Nawrocki <s.nawrocki@samsung.com>
+ * Andrzej Hajda <a.hajda@samsung.com>
+ */
+#ifndef S5C73M3_H_
+#define S5C73M3_H_
+
+#include <linux/clk.h>
+#include <linux/kernel.h>
+#include <linux/regulator/consumer.h>
+#include <linux/gpio/consumer.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-subdev.h>
+
+#define DRIVER_NAME			"S5C73M3"
+
+#define S5C73M3_ISP_FMT			MEDIA_BUS_FMT_VYUY8_2X8
+#define S5C73M3_JPEG_FMT		MEDIA_BUS_FMT_S5C_UYVY_JPEG_1X8
+
+/* Subdevs pad index definitions */
+enum s5c73m3_pads {
+	S5C73M3_ISP_PAD,
+	S5C73M3_JPEG_PAD,
+	S5C73M3_NUM_PADS
+};
+
+enum s5c73m3_oif_pads {
+	OIF_ISP_PAD,
+	OIF_JPEG_PAD,
+	OIF_SOURCE_PAD,
+	OIF_NUM_PADS
+};
+
+#define S5C73M3_SENSOR_FW_LEN		6
+#define S5C73M3_SENSOR_TYPE_LEN		12
+
+#define S5C73M3_REG(_addrh, _addrl) (((_addrh) << 16) | _addrl)
+
+#define AHB_MSB_ADDR_PTR			0xfcfc
+#define REG_CMDWR_ADDRH				0x0050
+#define REG_CMDWR_ADDRL				0x0054
+#define REG_CMDRD_ADDRH				0x0058
+#define REG_CMDRD_ADDRL				0x005c
+#define REG_CMDBUF_ADDR				0x0f14
+
+#define REG_I2C_SEQ_STATUS			S5C73M3_REG(0x0009, 0x59A6)
+#define  SEQ_END_PLL				(1<<0x0)
+#define  SEQ_END_SENSOR				(1<<0x1)
+#define  SEQ_END_GPIO				(1<<0x2)
+#define  SEQ_END_FROM				(1<<0x3)
+#define  SEQ_END_STABLE_AE_AWB			(1<<0x4)
+#define  SEQ_END_READY_I2C_CMD			(1<<0x5)
+
+#define REG_I2C_STATUS				S5C73M3_REG(0x0009, 0x599E)
+#define  I2C_STATUS_CIS_I2C			(1<<0x0)
+#define  I2C_STATUS_AF_INIT			(1<<0x1)
+#define  I2C_STATUS_CAL_DATA			(1<<0x2)
+#define  I2C_STATUS_FRAME_COUNT			(1<<0x3)
+#define  I2C_STATUS_FROM_INIT			(1<<0x4)
+#define  I2C_STATUS_I2C_CIS_STREAM_OFF		(1<<0x5)
+#define  I2C_STATUS_I2C_N_CMD_OVER		(1<<0x6)
+#define  I2C_STATUS_I2C_N_CMD_MISMATCH		(1<<0x7)
+#define  I2C_STATUS_CHECK_BIN_CRC		(1<<0x8)
+#define  I2C_STATUS_EXCEPTION			(1<<0x9)
+#define  I2C_STATUS_INIF_INIT_STATE		(0x8)
+
+#define REG_STATUS				S5C73M3_REG(0x0009, 0x5080)
+#define  REG_STATUS_BOOT_SUB_MAIN_ENTER		0xff01
+#define  REG_STATUS_BOOT_SRAM_TIMING_OK		0xff02
+#define  REG_STATUS_BOOT_INTERRUPTS_EN		0xff03
+#define  REG_STATUS_BOOT_R_PLL_DONE		0xff04
+#define  REG_STATUS_BOOT_R_PLL_LOCKTIME_DONE	0xff05
+#define  REG_STATUS_BOOT_DELAY_COUNT_DONE	0xff06
+#define  REG_STATUS_BOOT_I_PLL_DONE		0xff07
+#define  REG_STATUS_BOOT_I_PLL_LOCKTIME_DONE	0xff08
+#define  REG_STATUS_BOOT_PLL_INIT_OK		0xff09
+#define  REG_STATUS_BOOT_SENSOR_INIT_OK		0xff0a
+#define  REG_STATUS_BOOT_GPIO_SETTING_OK	0xff0b
+#define  REG_STATUS_BOOT_READ_CAL_DATA_OK	0xff0c
+#define  REG_STATUS_BOOT_STABLE_AE_AWB_OK	0xff0d
+#define  REG_STATUS_ISP_COMMAND_COMPLETED	0xffff
+#define  REG_STATUS_EXCEPTION_OCCURED		0xdead
+
+#define COMM_RESULT_OFFSET			S5C73M3_REG(0x0009, 0x5000)
+
+#define COMM_IMG_OUTPUT				0x0902
+#define  COMM_IMG_OUTPUT_HDR			0x0008
+#define  COMM_IMG_OUTPUT_YUV			0x0009
+#define  COMM_IMG_OUTPUT_INTERLEAVED		0x000d
+
+#define COMM_STILL_PRE_FLASH			0x0a00
+#define  COMM_STILL_PRE_FLASH_FIRE		0x0000
+#define  COMM_STILL_PRE_FLASH_NON_FIRED		0x0000
+#define  COMM_STILL_PRE_FLASH_FIRED		0x0001
+
+#define COMM_STILL_MAIN_FLASH			0x0a02
+#define  COMM_STILL_MAIN_FLASH_CANCEL		0x0001
+#define  COMM_STILL_MAIN_FLASH_FIRE		0x0002
+
+#define COMM_ZOOM_STEP				0x0b00
+
+#define COMM_IMAGE_EFFECT			0x0b0a
+#define  COMM_IMAGE_EFFECT_NONE			0x0001
+#define  COMM_IMAGE_EFFECT_NEGATIVE		0x0002
+#define  COMM_IMAGE_EFFECT_AQUA			0x0003
+#define  COMM_IMAGE_EFFECT_SEPIA		0x0004
+#define  COMM_IMAGE_EFFECT_MONO			0x0005
+
+#define COMM_IMAGE_QUALITY			0x0b0c
+#define  COMM_IMAGE_QUALITY_SUPERFINE		0x0000
+#define  COMM_IMAGE_QUALITY_FINE		0x0001
+#define  COMM_IMAGE_QUALITY_NORMAL		0x0002
+
+#define COMM_FLASH_MODE				0x0b0e
+#define  COMM_FLASH_MODE_OFF			0x0000
+#define  COMM_FLASH_MODE_ON			0x0001
+#define  COMM_FLASH_MODE_AUTO			0x0002
+
+#define COMM_FLASH_STATUS			0x0b80
+#define  COMM_FLASH_STATUS_OFF			0x0001
+#define  COMM_FLASH_STATUS_ON			0x0002
+#define  COMM_FLASH_STATUS_AUTO			0x0003
+
+#define COMM_FLASH_TORCH			0x0b12
+#define  COMM_FLASH_TORCH_OFF			0x0000
+#define  COMM_FLASH_TORCH_ON			0x0001
+
+#define COMM_AE_NEEDS_FLASH			0x0cba
+#define  COMM_AE_NEEDS_FLASH_OFF		0x0000
+#define  COMM_AE_NEEDS_FLASH_ON			0x0001
+
+#define COMM_CHG_MODE				0x0b10
+#define  COMM_CHG_MODE_NEW			0x8000
+#define  COMM_CHG_MODE_SUBSAMPLING_HALF		0x2000
+#define  COMM_CHG_MODE_SUBSAMPLING_QUARTER	0x4000
+
+#define  COMM_CHG_MODE_YUV_320_240		0x0001
+#define  COMM_CHG_MODE_YUV_640_480		0x0002
+#define  COMM_CHG_MODE_YUV_880_720		0x0003
+#define  COMM_CHG_MODE_YUV_960_720		0x0004
+#define  COMM_CHG_MODE_YUV_1184_666		0x0005
+#define  COMM_CHG_MODE_YUV_1280_720		0x0006
+#define  COMM_CHG_MODE_YUV_1536_864		0x0007
+#define  COMM_CHG_MODE_YUV_1600_1200		0x0008
+#define  COMM_CHG_MODE_YUV_1632_1224		0x0009
+#define  COMM_CHG_MODE_YUV_1920_1080		0x000a
+#define  COMM_CHG_MODE_YUV_1920_1440		0x000b
+#define  COMM_CHG_MODE_YUV_2304_1296		0x000c
+#define  COMM_CHG_MODE_YUV_3264_2448		0x000d
+#define  COMM_CHG_MODE_YUV_352_288		0x000e
+#define  COMM_CHG_MODE_YUV_1008_672		0x000f
+
+#define  COMM_CHG_MODE_JPEG_640_480		0x0010
+#define  COMM_CHG_MODE_JPEG_800_450		0x0020
+#define  COMM_CHG_MODE_JPEG_800_600		0x0030
+#define  COMM_CHG_MODE_JPEG_1280_720		0x0040
+#define  COMM_CHG_MODE_JPEG_1280_960		0x0050
+#define  COMM_CHG_MODE_JPEG_1600_900		0x0060
+#define  COMM_CHG_MODE_JPEG_1600_1200		0x0070
+#define  COMM_CHG_MODE_JPEG_2048_1152		0x0080
+#define  COMM_CHG_MODE_JPEG_2048_1536		0x0090
+#define  COMM_CHG_MODE_JPEG_2560_1440		0x00a0
+#define  COMM_CHG_MODE_JPEG_2560_1920		0x00b0
+#define  COMM_CHG_MODE_JPEG_3264_2176		0x00c0
+#define  COMM_CHG_MODE_JPEG_1024_768		0x00d0
+#define  COMM_CHG_MODE_JPEG_3264_1836		0x00e0
+#define  COMM_CHG_MODE_JPEG_3264_2448		0x00f0
+
+#define COMM_AF_CON				0x0e00
+#define  COMM_AF_CON_STOP			0x0000
+#define  COMM_AF_CON_SCAN			0x0001 /* Full Search */
+#define  COMM_AF_CON_START			0x0002 /* Fast Search */
+
+#define COMM_AF_CAL				0x0e06
+#define COMM_AF_TOUCH_AF			0x0e0a
+
+#define REG_AF_STATUS				S5C73M3_REG(0x0009, 0x5e80)
+#define  REG_CAF_STATUS_FIND_SEARCH_DIR		0x0001
+#define  REG_CAF_STATUS_FOCUSING		0x0002
+#define  REG_CAF_STATUS_FOCUSED			0x0003
+#define  REG_CAF_STATUS_UNFOCUSED		0x0004
+#define  REG_AF_STATUS_INVALID			0x0010
+#define  REG_AF_STATUS_FOCUSING			0x0020
+#define  REG_AF_STATUS_FOCUSED			0x0030
+#define  REG_AF_STATUS_UNFOCUSED		0x0040
+
+#define REG_AF_TOUCH_POSITION			S5C73M3_REG(0x0009, 0x5e8e)
+#define COMM_AF_FACE_ZOOM			0x0e10
+
+#define COMM_AF_MODE				0x0e02
+#define  COMM_AF_MODE_NORMAL			0x0000
+#define  COMM_AF_MODE_MACRO			0x0001
+#define  COMM_AF_MODE_MOVIE_CAF_START		0x0002
+#define  COMM_AF_MODE_MOVIE_CAF_STOP		0x0003
+#define  COMM_AF_MODE_PREVIEW_CAF_START		0x0004
+#define  COMM_AF_MODE_PREVIEW_CAF_STOP		0x0005
+
+#define COMM_AF_SOFTLANDING			0x0e16
+#define  COMM_AF_SOFTLANDING_ON			0x0000
+#define  COMM_AF_SOFTLANDING_RES_COMPLETE	0x0001
+
+#define COMM_FACE_DET				0x0e0c
+#define  COMM_FACE_DET_OFF			0x0000
+#define  COMM_FACE_DET_ON			0x0001
+
+#define COMM_FACE_DET_OSD			0x0e0e
+#define  COMM_FACE_DET_OSD_OFF			0x0000
+#define  COMM_FACE_DET_OSD_ON			0x0001
+
+#define COMM_AE_CON				0x0c00
+#define  COMM_AE_STOP				0x0000 /* lock */
+#define  COMM_AE_START				0x0001 /* unlock */
+
+#define COMM_ISO				0x0c02
+#define  COMM_ISO_AUTO				0x0000
+#define  COMM_ISO_100				0x0001
+#define  COMM_ISO_200				0x0002
+#define  COMM_ISO_400				0x0003
+#define  COMM_ISO_800				0x0004
+#define  COMM_ISO_SPORTS			0x0005
+#define  COMM_ISO_NIGHT				0x0006
+#define  COMM_ISO_INDOOR			0x0007
+
+/* 0x00000 (-2.0 EV)...0x0008 (2.0 EV), 0.5EV step */
+#define COMM_EV					0x0c04
+
+#define COMM_METERING				0x0c06
+#define  COMM_METERING_CENTER			0x0000
+#define  COMM_METERING_SPOT			0x0001
+#define  COMM_METERING_AVERAGE			0x0002
+#define  COMM_METERING_SMART			0x0003
+
+#define COMM_WDR				0x0c08
+#define  COMM_WDR_OFF				0x0000
+#define  COMM_WDR_ON				0x0001
+
+#define COMM_FLICKER_MODE			0x0c12
+#define  COMM_FLICKER_NONE			0x0000
+#define  COMM_FLICKER_MANUAL_50HZ		0x0001
+#define  COMM_FLICKER_MANUAL_60HZ		0x0002
+#define  COMM_FLICKER_AUTO			0x0003
+#define  COMM_FLICKER_AUTO_50HZ			0x0004
+#define  COMM_FLICKER_AUTO_60HZ			0x0005
+
+#define COMM_FRAME_RATE				0x0c1e
+#define  COMM_FRAME_RATE_AUTO_SET		0x0000
+#define  COMM_FRAME_RATE_FIXED_30FPS		0x0002
+#define  COMM_FRAME_RATE_FIXED_20FPS		0x0003
+#define  COMM_FRAME_RATE_FIXED_15FPS		0x0004
+#define  COMM_FRAME_RATE_FIXED_60FPS		0x0007
+#define  COMM_FRAME_RATE_FIXED_120FPS		0x0008
+#define  COMM_FRAME_RATE_FIXED_7FPS		0x0009
+#define  COMM_FRAME_RATE_FIXED_10FPS		0x000a
+#define  COMM_FRAME_RATE_FIXED_90FPS		0x000b
+#define  COMM_FRAME_RATE_ANTI_SHAKE		0x0013
+
+/* 0x0000...0x0004 -> sharpness: 0, 1, 2, -1, -2 */
+#define COMM_SHARPNESS				0x0c14
+
+/* 0x0000...0x0004 -> saturation: 0, 1, 2, -1, -2 */
+#define COMM_SATURATION				0x0c16
+
+/* 0x0000...0x0004 -> contrast: 0, 1, 2, -1, -2 */
+#define COMM_CONTRAST				0x0c18
+
+#define COMM_SCENE_MODE				0x0c1a
+#define  COMM_SCENE_MODE_NONE			0x0000
+#define  COMM_SCENE_MODE_PORTRAIT		0x0001
+#define  COMM_SCENE_MODE_LANDSCAPE		0x0002
+#define  COMM_SCENE_MODE_SPORTS			0x0003
+#define  COMM_SCENE_MODE_INDOOR			0x0004
+#define  COMM_SCENE_MODE_BEACH			0x0005
+#define  COMM_SCENE_MODE_SUNSET			0x0006
+#define  COMM_SCENE_MODE_DAWN			0x0007
+#define  COMM_SCENE_MODE_FALL			0x0008
+#define  COMM_SCENE_MODE_NIGHT			0x0009
+#define  COMM_SCENE_MODE_AGAINST_LIGHT		0x000a
+#define  COMM_SCENE_MODE_FIRE			0x000b
+#define  COMM_SCENE_MODE_TEXT			0x000c
+#define  COMM_SCENE_MODE_CANDLE			0x000d
+
+#define COMM_AE_AUTO_BRACKET			0x0b14
+#define  COMM_AE_AUTO_BRAKET_EV05		0x0080
+#define  COMM_AE_AUTO_BRAKET_EV10		0x0100
+#define  COMM_AE_AUTO_BRAKET_EV15		0x0180
+#define  COMM_AE_AUTO_BRAKET_EV20		0x0200
+
+#define COMM_SENSOR_STREAMING			0x090a
+#define  COMM_SENSOR_STREAMING_OFF		0x0000
+#define  COMM_SENSOR_STREAMING_ON		0x0001
+
+#define COMM_AWB_MODE				0x0d02
+#define  COMM_AWB_MODE_INCANDESCENT		0x0000
+#define  COMM_AWB_MODE_FLUORESCENT1		0x0001
+#define  COMM_AWB_MODE_FLUORESCENT2		0x0002
+#define  COMM_AWB_MODE_DAYLIGHT			0x0003
+#define  COMM_AWB_MODE_CLOUDY			0x0004
+#define  COMM_AWB_MODE_AUTO			0x0005
+
+#define COMM_AWB_CON				0x0d00
+#define  COMM_AWB_STOP				0x0000 /* lock */
+#define  COMM_AWB_START				0x0001 /* unlock */
+
+#define COMM_FW_UPDATE				0x0906
+#define  COMM_FW_UPDATE_NOT_READY		0x0000
+#define  COMM_FW_UPDATE_SUCCESS			0x0005
+#define  COMM_FW_UPDATE_FAIL			0x0007
+#define  COMM_FW_UPDATE_BUSY			0xffff
+
+
+#define S5C73M3_MAX_SUPPLIES			6
+#define S5C73M3_DEFAULT_MCLK_FREQ		24000000U
+
+struct s5c73m3_ctrls {
+	struct v4l2_ctrl_handler handler;
+	struct {
+		/* exposure/exposure bias cluster */
+		struct v4l2_ctrl *auto_exposure;
+		struct v4l2_ctrl *exposure_bias;
+		struct v4l2_ctrl *exposure_metering;
+	};
+	struct {
+		/* iso/auto iso cluster */
+		struct v4l2_ctrl *auto_iso;
+		struct v4l2_ctrl *iso;
+	};
+	struct v4l2_ctrl *auto_wb;
+	struct {
+		/* continuous auto focus/auto focus cluster */
+		struct v4l2_ctrl *focus_auto;
+		struct v4l2_ctrl *af_start;
+		struct v4l2_ctrl *af_stop;
+		struct v4l2_ctrl *af_status;
+		struct v4l2_ctrl *af_distance;
+	};
+
+	struct v4l2_ctrl *aaa_lock;
+	struct v4l2_ctrl *colorfx;
+	struct v4l2_ctrl *contrast;
+	struct v4l2_ctrl *saturation;
+	struct v4l2_ctrl *sharpness;
+	struct v4l2_ctrl *zoom;
+	struct v4l2_ctrl *wdr;
+	struct v4l2_ctrl *stabilization;
+	struct v4l2_ctrl *jpeg_quality;
+	struct v4l2_ctrl *scene_mode;
+};
+
+enum s5c73m3_resolution_types {
+	RES_ISP,
+	RES_JPEG,
+};
+
+struct s5c73m3_interval {
+	u16 fps_reg;
+	struct v4l2_fract interval;
+	/* Maximum rectangle for the interval */
+	struct v4l2_frmsize_discrete size;
+};
+
+struct s5c73m3 {
+	struct v4l2_subdev sensor_sd;
+	struct media_pad sensor_pads[S5C73M3_NUM_PADS];
+
+	struct v4l2_subdev oif_sd;
+	struct media_pad oif_pads[OIF_NUM_PADS];
+
+	struct spi_driver spidrv;
+	struct spi_device *spi_dev;
+	struct i2c_client *i2c_client;
+	u32 i2c_write_address;
+	u32 i2c_read_address;
+
+	struct regulator_bulk_data supplies[S5C73M3_MAX_SUPPLIES];
+	struct gpio_desc *stby;
+	struct gpio_desc *reset;
+
+	struct clk *clock;
+
+	/* External master clock frequency */
+	u32 mclk_frequency;
+	/* Video bus type - MIPI-CSI2/parallel */
+	enum v4l2_mbus_type bus_type;
+
+	const struct s5c73m3_frame_size *sensor_pix_size[2];
+	const struct s5c73m3_frame_size *oif_pix_size[2];
+	u32 mbus_code;
+
+	const struct s5c73m3_interval *fiv;
+
+	struct v4l2_mbus_frame_desc frame_desc;
+	/* protects the struct members below */
+	struct mutex lock;
+
+	struct s5c73m3_ctrls ctrls;
+
+	u8 streaming:1;
+	u8 apply_fmt:1;
+	u8 apply_fiv:1;
+	u8 isp_ready:1;
+
+	short power;
+
+	char sensor_fw[S5C73M3_SENSOR_FW_LEN + 2];
+	char sensor_type[S5C73M3_SENSOR_TYPE_LEN + 2];
+	char fw_file_version[2];
+	unsigned int fw_size;
+};
+
+struct s5c73m3_frame_size {
+	u32 width;
+	u32 height;
+	u8 reg_val;
+};
+
+extern int s5c73m3_dbg;
+
+int s5c73m3_register_spi_driver(struct s5c73m3 *state);
+void s5c73m3_unregister_spi_driver(struct s5c73m3 *state);
+int s5c73m3_spi_write(struct s5c73m3 *state, const void *addr,
+		      const unsigned int len, const unsigned int tx_size);
+int s5c73m3_spi_read(struct s5c73m3 *state, void *addr,
+		      const unsigned int len, const unsigned int tx_size);
+
+int s5c73m3_read(struct s5c73m3 *state, u32 addr, u16 *data);
+int s5c73m3_write(struct s5c73m3 *state, u32 addr, u16 data);
+int s5c73m3_isp_command(struct s5c73m3 *state, u16 command, u16 data);
+int s5c73m3_init_controls(struct s5c73m3 *state);
+
+static inline struct v4l2_subdev *ctrl_to_sensor_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct s5c73m3,
+			     ctrls.handler)->sensor_sd;
+}
+
+static inline struct s5c73m3 *sensor_sd_to_s5c73m3(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct s5c73m3, sensor_sd);
+}
+
+static inline struct s5c73m3 *oif_sd_to_s5c73m3(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct s5c73m3, oif_sd);
+}
+#endif	/* S5C73M3_H_ */
diff --git a/drivers/media/i2c/s5k5baf.c b/drivers/media/i2c/s5k5baf.c
new file mode 100644
index 000000000..67da2045f
--- /dev/null
+++ b/drivers/media/i2c/s5k5baf.c
@@ -0,0 +1,2033 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Driver for Samsung S5K5BAF UXGA 1/5" 2M CMOS Image Sensor
+ * with embedded SoC ISP.
+ *
+ * Copyright (C) 2013, Samsung Electronics Co., Ltd.
+ * Andrzej Hajda <a.hajda@samsung.com>
+ *
+ * Based on S5K6AA driver authored by Sylwester Nawrocki
+ * Copyright (C) 2013, Samsung Electronics Co., Ltd.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/firmware.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/media.h>
+#include <linux/module.h>
+#include <linux/of_graph.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+
+#include <media/media-entity.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-mediabus.h>
+#include <media/v4l2-fwnode.h>
+
+static int debug;
+module_param(debug, int, 0644);
+
+#define S5K5BAF_DRIVER_NAME		"s5k5baf"
+#define S5K5BAF_DEFAULT_MCLK_FREQ	24000000U
+#define S5K5BAF_CLK_NAME		"mclk"
+
+#define S5K5BAF_FW_FILENAME		"s5k5baf-cfg.bin"
+#define S5K5BAF_FW_TAG			"SF00"
+#define S5K5BAG_FW_TAG_LEN		2
+#define S5K5BAG_FW_MAX_COUNT		16
+
+#define S5K5BAF_CIS_WIDTH		1600
+#define S5K5BAF_CIS_HEIGHT		1200
+#define S5K5BAF_WIN_WIDTH_MIN		8
+#define S5K5BAF_WIN_HEIGHT_MIN		8
+#define S5K5BAF_GAIN_RED_DEF		127
+#define S5K5BAF_GAIN_GREEN_DEF		95
+#define S5K5BAF_GAIN_BLUE_DEF		180
+/* Default number of MIPI CSI-2 data lanes used */
+#define S5K5BAF_DEF_NUM_LANES		1
+
+#define AHB_MSB_ADDR_PTR		0xfcfc
+
+/*
+ * Register interface pages (the most significant word of the address)
+ */
+#define PAGE_IF_HW			0xd000
+#define PAGE_IF_SW			0x7000
+
+/*
+ * H/W register Interface (PAGE_IF_HW)
+ */
+#define REG_SW_LOAD_COMPLETE		0x0014
+#define REG_CMDWR_PAGE			0x0028
+#define REG_CMDWR_ADDR			0x002a
+#define REG_CMDRD_PAGE			0x002c
+#define REG_CMDRD_ADDR			0x002e
+#define REG_CMD_BUF			0x0f12
+#define REG_SET_HOST_INT		0x1000
+#define REG_CLEAR_HOST_INT		0x1030
+#define REG_PATTERN_SET			0x3100
+#define REG_PATTERN_WIDTH		0x3118
+#define REG_PATTERN_HEIGHT		0x311a
+#define REG_PATTERN_PARAM		0x311c
+
+/*
+ * S/W register interface (PAGE_IF_SW)
+ */
+
+/* Firmware revision information */
+#define REG_FW_APIVER			0x012e
+#define  S5K5BAF_FW_APIVER		0x0001
+#define REG_FW_REVISION			0x0130
+#define REG_FW_SENSOR_ID		0x0152
+
+/* Initialization parameters */
+/* Master clock frequency in KHz */
+#define REG_I_INCLK_FREQ_L		0x01b8
+#define REG_I_INCLK_FREQ_H		0x01ba
+#define  MIN_MCLK_FREQ_KHZ		6000U
+#define  MAX_MCLK_FREQ_KHZ		48000U
+#define REG_I_USE_NPVI_CLOCKS		0x01c6
+#define  NPVI_CLOCKS			1
+#define REG_I_USE_NMIPI_CLOCKS		0x01c8
+#define  NMIPI_CLOCKS			1
+#define REG_I_BLOCK_INTERNAL_PLL_CALC	0x01ca
+
+/* Clock configurations, n = 0..2. REG_I_* frequency unit is 4 kHz. */
+#define REG_I_OPCLK_4KHZ(n)		((n) * 6 + 0x01cc)
+#define REG_I_MIN_OUTRATE_4KHZ(n)	((n) * 6 + 0x01ce)
+#define REG_I_MAX_OUTRATE_4KHZ(n)	((n) * 6 + 0x01d0)
+#define  SCLK_PVI_FREQ			24000
+#define  SCLK_MIPI_FREQ			48000
+#define  PCLK_MIN_FREQ			6000
+#define  PCLK_MAX_FREQ			48000
+#define REG_I_USE_REGS_API		0x01de
+#define REG_I_INIT_PARAMS_UPDATED	0x01e0
+#define REG_I_ERROR_INFO		0x01e2
+
+/* General purpose parameters */
+#define REG_USER_BRIGHTNESS		0x01e4
+#define REG_USER_CONTRAST		0x01e6
+#define REG_USER_SATURATION		0x01e8
+#define REG_USER_SHARPBLUR		0x01ea
+
+#define REG_G_SPEC_EFFECTS		0x01ee
+#define REG_G_ENABLE_PREV		0x01f0
+#define REG_G_ENABLE_PREV_CHG		0x01f2
+#define REG_G_NEW_CFG_SYNC		0x01f8
+#define REG_G_PREVREQ_IN_WIDTH		0x01fa
+#define REG_G_PREVREQ_IN_HEIGHT		0x01fc
+#define REG_G_PREVREQ_IN_XOFFS		0x01fe
+#define REG_G_PREVREQ_IN_YOFFS		0x0200
+#define REG_G_PREVZOOM_IN_WIDTH		0x020a
+#define REG_G_PREVZOOM_IN_HEIGHT	0x020c
+#define REG_G_PREVZOOM_IN_XOFFS		0x020e
+#define REG_G_PREVZOOM_IN_YOFFS		0x0210
+#define REG_G_INPUTS_CHANGE_REQ		0x021a
+#define REG_G_ACTIVE_PREV_CFG		0x021c
+#define REG_G_PREV_CFG_CHG		0x021e
+#define REG_G_PREV_OPEN_AFTER_CH	0x0220
+#define REG_G_PREV_CFG_ERROR		0x0222
+#define  CFG_ERROR_RANGE		0x0b
+#define REG_G_PREV_CFG_BYPASS_CHANGED	0x022a
+#define REG_G_ACTUAL_P_FR_TIME		0x023a
+#define REG_G_ACTUAL_P_OUT_RATE		0x023c
+#define REG_G_ACTUAL_C_FR_TIME		0x023e
+#define REG_G_ACTUAL_C_OUT_RATE		0x0240
+
+/* Preview control section. n = 0...4. */
+#define PREG(n, x)			((n) * 0x26 + x)
+#define REG_P_OUT_WIDTH(n)		PREG(n, 0x0242)
+#define REG_P_OUT_HEIGHT(n)		PREG(n, 0x0244)
+#define REG_P_FMT(n)			PREG(n, 0x0246)
+#define REG_P_MAX_OUT_RATE(n)		PREG(n, 0x0248)
+#define REG_P_MIN_OUT_RATE(n)		PREG(n, 0x024a)
+#define REG_P_PVI_MASK(n)		PREG(n, 0x024c)
+#define  PVI_MASK_MIPI			0x52
+#define REG_P_CLK_INDEX(n)		PREG(n, 0x024e)
+#define  CLK_PVI_INDEX			0
+#define  CLK_MIPI_INDEX			NPVI_CLOCKS
+#define REG_P_FR_RATE_TYPE(n)		PREG(n, 0x0250)
+#define  FR_RATE_DYNAMIC		0
+#define  FR_RATE_FIXED			1
+#define  FR_RATE_FIXED_ACCURATE		2
+#define REG_P_FR_RATE_Q_TYPE(n)		PREG(n, 0x0252)
+#define  FR_RATE_Q_DYNAMIC		0
+#define  FR_RATE_Q_BEST_FRRATE		1 /* Binning enabled */
+#define  FR_RATE_Q_BEST_QUALITY		2 /* Binning disabled */
+/* Frame period in 0.1 ms units */
+#define REG_P_MAX_FR_TIME(n)		PREG(n, 0x0254)
+#define REG_P_MIN_FR_TIME(n)		PREG(n, 0x0256)
+#define  S5K5BAF_MIN_FR_TIME		333  /* x100 us */
+#define  S5K5BAF_MAX_FR_TIME		6500 /* x100 us */
+/* The below 5 registers are for "device correction" values */
+#define REG_P_SATURATION(n)		PREG(n, 0x0258)
+#define REG_P_SHARP_BLUR(n)		PREG(n, 0x025a)
+#define REG_P_GLAMOUR(n)		PREG(n, 0x025c)
+#define REG_P_COLORTEMP(n)		PREG(n, 0x025e)
+#define REG_P_GAMMA_INDEX(n)		PREG(n, 0x0260)
+#define REG_P_PREV_MIRROR(n)		PREG(n, 0x0262)
+#define REG_P_CAP_MIRROR(n)		PREG(n, 0x0264)
+#define REG_P_CAP_ROTATION(n)		PREG(n, 0x0266)
+
+/* Extended image property controls */
+/* Exposure time in 10 us units */
+#define REG_SF_USR_EXPOSURE_L		0x03bc
+#define REG_SF_USR_EXPOSURE_H		0x03be
+#define REG_SF_USR_EXPOSURE_CHG		0x03c0
+#define REG_SF_USR_TOT_GAIN		0x03c2
+#define REG_SF_USR_TOT_GAIN_CHG		0x03c4
+#define REG_SF_RGAIN			0x03c6
+#define REG_SF_RGAIN_CHG		0x03c8
+#define REG_SF_GGAIN			0x03ca
+#define REG_SF_GGAIN_CHG		0x03cc
+#define REG_SF_BGAIN			0x03ce
+#define REG_SF_BGAIN_CHG		0x03d0
+#define REG_SF_WBGAIN_CHG		0x03d2
+#define REG_SF_FLICKER_QUANT		0x03d4
+#define REG_SF_FLICKER_QUANT_CHG	0x03d6
+
+/* Output interface (parallel/MIPI) setup */
+#define REG_OIF_EN_MIPI_LANES		0x03f2
+#define REG_OIF_EN_PACKETS		0x03f4
+#define  EN_PACKETS_CSI2		0xc3
+#define REG_OIF_CFG_CHG			0x03f6
+
+/* Auto-algorithms enable mask */
+#define REG_DBG_AUTOALG_EN		0x03f8
+#define  AALG_ALL_EN			BIT(0)
+#define  AALG_AE_EN			BIT(1)
+#define  AALG_DIVLEI_EN			BIT(2)
+#define  AALG_WB_EN			BIT(3)
+#define  AALG_USE_WB_FOR_ISP		BIT(4)
+#define  AALG_FLICKER_EN		BIT(5)
+#define  AALG_FIT_EN			BIT(6)
+#define  AALG_WRHW_EN			BIT(7)
+
+/* Pointers to color correction matrices */
+#define REG_PTR_CCM_HORIZON		0x06d0
+#define REG_PTR_CCM_INCANDESCENT	0x06d4
+#define REG_PTR_CCM_WARM_WHITE		0x06d8
+#define REG_PTR_CCM_COOL_WHITE		0x06dc
+#define REG_PTR_CCM_DL50		0x06e0
+#define REG_PTR_CCM_DL65		0x06e4
+#define REG_PTR_CCM_OUTDOOR		0x06ec
+
+#define REG_ARR_CCM(n)			(0x2800 + 36 * (n))
+
+static const char * const s5k5baf_supply_names[] = {
+	"vdda",		/* Analog power supply 2.8V (2.6V to 3.0V) */
+	"vddreg",	/* Regulator input power supply 1.8V (1.7V to 1.9V)
+			   or 2.8V (2.6V to 3.0) */
+	"vddio",	/* I/O power supply 1.8V (1.65V to 1.95V)
+			   or 2.8V (2.5V to 3.1V) */
+};
+#define S5K5BAF_NUM_SUPPLIES ARRAY_SIZE(s5k5baf_supply_names)
+
+enum s5k5baf_gpio_id {
+	STBY,
+	RSET,
+	NUM_GPIOS,
+};
+
+#define PAD_CIS 0
+#define PAD_OUT 1
+#define NUM_CIS_PADS 1
+#define NUM_ISP_PADS 2
+
+struct s5k5baf_pixfmt {
+	u32 code;
+	u32 colorspace;
+	/* REG_P_FMT(x) register value */
+	u16 reg_p_fmt;
+};
+
+struct s5k5baf_ctrls {
+	struct v4l2_ctrl_handler handler;
+	struct { /* Auto / manual white balance cluster */
+		struct v4l2_ctrl *awb;
+		struct v4l2_ctrl *gain_red;
+		struct v4l2_ctrl *gain_blue;
+	};
+	struct { /* Mirror cluster */
+		struct v4l2_ctrl *hflip;
+		struct v4l2_ctrl *vflip;
+	};
+	struct { /* Auto exposure / manual exposure and gain cluster */
+		struct v4l2_ctrl *auto_exp;
+		struct v4l2_ctrl *exposure;
+		struct v4l2_ctrl *gain;
+	};
+};
+
+enum {
+	S5K5BAF_FW_ID_PATCH,
+	S5K5BAF_FW_ID_CCM,
+	S5K5BAF_FW_ID_CIS,
+};
+
+struct s5k5baf_fw {
+	u16 count;
+	struct {
+		u16 id;
+		u16 offset;
+	} seq[];
+};
+
+struct s5k5baf {
+	struct gpio_desc *gpios[NUM_GPIOS];
+	enum v4l2_mbus_type bus_type;
+	u8 nlanes;
+	struct regulator_bulk_data supplies[S5K5BAF_NUM_SUPPLIES];
+
+	struct clk *clock;
+	u32 mclk_frequency;
+
+	struct s5k5baf_fw *fw;
+
+	struct v4l2_subdev cis_sd;
+	struct media_pad cis_pad;
+
+	struct v4l2_subdev sd;
+	struct media_pad pads[NUM_ISP_PADS];
+
+	/* protects the struct members below */
+	struct mutex lock;
+
+	int error;
+
+	struct v4l2_rect crop_sink;
+	struct v4l2_rect compose;
+	struct v4l2_rect crop_source;
+	/* index to s5k5baf_formats array */
+	int pixfmt;
+	/* actual frame interval in 100us */
+	u16 fiv;
+	/* requested frame interval in 100us */
+	u16 req_fiv;
+	/* cache for REG_DBG_AUTOALG_EN register */
+	u16 auto_alg;
+
+	struct s5k5baf_ctrls ctrls;
+
+	unsigned int streaming:1;
+	unsigned int apply_cfg:1;
+	unsigned int apply_crop:1;
+	unsigned int valid_auto_alg:1;
+	unsigned int power;
+};
+
+static const struct s5k5baf_pixfmt s5k5baf_formats[] = {
+	{ MEDIA_BUS_FMT_VYUY8_2X8,	V4L2_COLORSPACE_JPEG,	5 },
+	/* range 16-240 */
+	{ MEDIA_BUS_FMT_VYUY8_2X8,	V4L2_COLORSPACE_REC709,	6 },
+	{ MEDIA_BUS_FMT_RGB565_2X8_BE,	V4L2_COLORSPACE_JPEG,	0 },
+};
+
+static struct v4l2_rect s5k5baf_cis_rect = {
+	0, 0, S5K5BAF_CIS_WIDTH, S5K5BAF_CIS_HEIGHT
+};
+
+/* Setfile contains set of I2C command sequences. Each sequence has its ID.
+ * setfile format:
+ *	u8 magic[4];
+ *	u16 count;		number of sequences
+ *	struct {
+ *		u16 id;		sequence id
+ *		u16 offset;	sequence offset in data array
+ *	} seq[count];
+ *	u16 data[*];		array containing sequences
+ *
+ */
+static int s5k5baf_fw_parse(struct device *dev, struct s5k5baf_fw **fw,
+			    size_t count, const __le16 *data)
+{
+	struct s5k5baf_fw *f;
+	u16 *d, i, *end;
+	int ret;
+
+	if (count < S5K5BAG_FW_TAG_LEN + 1) {
+		dev_err(dev, "firmware file too short (%zu)\n", count);
+		return -EINVAL;
+	}
+
+	ret = memcmp(data, S5K5BAF_FW_TAG, S5K5BAG_FW_TAG_LEN * sizeof(u16));
+	if (ret != 0) {
+		dev_err(dev, "invalid firmware magic number\n");
+		return -EINVAL;
+	}
+
+	data += S5K5BAG_FW_TAG_LEN;
+	count -= S5K5BAG_FW_TAG_LEN;
+
+	d = devm_kcalloc(dev, count, sizeof(u16), GFP_KERNEL);
+	if (!d)
+		return -ENOMEM;
+
+	for (i = 0; i < count; ++i)
+		d[i] = le16_to_cpu(data[i]);
+
+	f = (struct s5k5baf_fw *)d;
+	if (count < 1 + 2 * f->count) {
+		dev_err(dev, "invalid firmware header (count=%d size=%zu)\n",
+			f->count, 2 * (count + S5K5BAG_FW_TAG_LEN));
+		return -EINVAL;
+	}
+	end = d + count;
+	d += 1 + 2 * f->count;
+
+	for (i = 0; i < f->count; ++i) {
+		if (f->seq[i].offset + d <= end)
+			continue;
+		dev_err(dev, "invalid firmware header (seq=%d)\n", i);
+		return -EINVAL;
+	}
+
+	*fw = f;
+
+	return 0;
+}
+
+static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct s5k5baf, ctrls.handler)->sd;
+}
+
+static inline bool s5k5baf_is_cis_subdev(struct v4l2_subdev *sd)
+{
+	return sd->entity.function == MEDIA_ENT_F_CAM_SENSOR;
+}
+
+static inline struct s5k5baf *to_s5k5baf(struct v4l2_subdev *sd)
+{
+	if (s5k5baf_is_cis_subdev(sd))
+		return container_of(sd, struct s5k5baf, cis_sd);
+	else
+		return container_of(sd, struct s5k5baf, sd);
+}
+
+static u16 s5k5baf_i2c_read(struct s5k5baf *state, u16 addr)
+{
+	struct i2c_client *c = v4l2_get_subdevdata(&state->sd);
+	__be16 w, r;
+	u16 res;
+	struct i2c_msg msg[] = {
+		{ .addr = c->addr, .flags = 0,
+		  .len = 2, .buf = (u8 *)&w },
+		{ .addr = c->addr, .flags = I2C_M_RD,
+		  .len = 2, .buf = (u8 *)&r },
+	};
+	int ret;
+
+	if (state->error)
+		return 0;
+
+	w = cpu_to_be16(addr);
+	ret = i2c_transfer(c->adapter, msg, 2);
+	res = be16_to_cpu(r);
+
+	v4l2_dbg(3, debug, c, "i2c_read: 0x%04x : 0x%04x\n", addr, res);
+
+	if (ret != 2) {
+		v4l2_err(c, "i2c_read: error during transfer (%d)\n", ret);
+		state->error = ret;
+	}
+	return res;
+}
+
+static void s5k5baf_i2c_write(struct s5k5baf *state, u16 addr, u16 val)
+{
+	u8 buf[4] = { addr >> 8, addr & 0xFF, val >> 8, val & 0xFF };
+	struct i2c_client *c = v4l2_get_subdevdata(&state->sd);
+	int ret;
+
+	if (state->error)
+		return;
+
+	ret = i2c_master_send(c, buf, 4);
+	v4l2_dbg(3, debug, c, "i2c_write: 0x%04x : 0x%04x\n", addr, val);
+
+	if (ret != 4) {
+		v4l2_err(c, "i2c_write: error during transfer (%d)\n", ret);
+		state->error = ret;
+	}
+}
+
+static u16 s5k5baf_read(struct s5k5baf *state, u16 addr)
+{
+	s5k5baf_i2c_write(state, REG_CMDRD_ADDR, addr);
+	return s5k5baf_i2c_read(state, REG_CMD_BUF);
+}
+
+static void s5k5baf_write(struct s5k5baf *state, u16 addr, u16 val)
+{
+	s5k5baf_i2c_write(state, REG_CMDWR_ADDR, addr);
+	s5k5baf_i2c_write(state, REG_CMD_BUF, val);
+}
+
+static void s5k5baf_write_arr_seq(struct s5k5baf *state, u16 addr,
+				  u16 count, const u16 *seq)
+{
+	struct i2c_client *c = v4l2_get_subdevdata(&state->sd);
+	__be16 buf[65];
+
+	s5k5baf_i2c_write(state, REG_CMDWR_ADDR, addr);
+	if (state->error)
+		return;
+
+	v4l2_dbg(3, debug, c, "i2c_write_seq(count=%d): %*ph\n", count,
+		 min(2 * count, 64), seq);
+
+	buf[0] = cpu_to_be16(REG_CMD_BUF);
+
+	while (count > 0) {
+		int n = min_t(int, count, ARRAY_SIZE(buf) - 1);
+		int ret, i;
+
+		for (i = 1; i <= n; ++i)
+			buf[i] = cpu_to_be16(*seq++);
+
+		i *= 2;
+		ret = i2c_master_send(c, (char *)buf, i);
+		if (ret != i) {
+			v4l2_err(c, "i2c_write_seq: error during transfer (%d)\n", ret);
+			state->error = ret;
+			break;
+		}
+
+		count -= n;
+	}
+}
+
+#define s5k5baf_write_seq(state, addr, seq...) \
+	s5k5baf_write_arr_seq(state, addr, sizeof((char[]){ seq }), \
+			      (const u16 []){ seq })
+
+/* add items count at the beginning of the list */
+#define NSEQ(seq...) sizeof((char[]){ seq }), seq
+
+/*
+ * s5k5baf_write_nseq() - Writes sequences of values to sensor memory via i2c
+ * @nseq: sequence of u16 words in format:
+ *	(N, address, value[1]...value[N-1])*,0
+ * Ex.:
+ *	u16 seq[] = { NSEQ(0x4000, 1, 1), NSEQ(0x4010, 640, 480), 0 };
+ *	ret = s5k5baf_write_nseq(c, seq);
+ */
+static void s5k5baf_write_nseq(struct s5k5baf *state, const u16 *nseq)
+{
+	int count;
+
+	while ((count = *nseq++)) {
+		u16 addr = *nseq++;
+		--count;
+
+		s5k5baf_write_arr_seq(state, addr, count, nseq);
+		nseq += count;
+	}
+}
+
+static void s5k5baf_synchronize(struct s5k5baf *state, int timeout, u16 addr)
+{
+	unsigned long end = jiffies + msecs_to_jiffies(timeout);
+	u16 reg;
+
+	s5k5baf_write(state, addr, 1);
+	do {
+		reg = s5k5baf_read(state, addr);
+		if (state->error || !reg)
+			return;
+		usleep_range(5000, 10000);
+	} while (time_is_after_jiffies(end));
+
+	v4l2_err(&state->sd, "timeout on register synchronize (%#x)\n", addr);
+	state->error = -ETIMEDOUT;
+}
+
+static u16 *s5k5baf_fw_get_seq(struct s5k5baf *state, u16 seq_id)
+{
+	struct s5k5baf_fw *fw = state->fw;
+	u16 *data;
+	int i;
+
+	if (fw == NULL)
+		return NULL;
+
+	data = &fw->seq[0].id + 2 * fw->count;
+
+	for (i = 0; i < fw->count; ++i) {
+		if (fw->seq[i].id == seq_id)
+			return data + fw->seq[i].offset;
+	}
+
+	return NULL;
+}
+
+static void s5k5baf_hw_patch(struct s5k5baf *state)
+{
+	u16 *seq = s5k5baf_fw_get_seq(state, S5K5BAF_FW_ID_PATCH);
+
+	if (seq)
+		s5k5baf_write_nseq(state, seq);
+}
+
+static void s5k5baf_hw_set_clocks(struct s5k5baf *state)
+{
+	unsigned long mclk = state->mclk_frequency / 1000;
+	u16 status;
+	static const u16 nseq_clk_cfg[] = {
+		NSEQ(REG_I_USE_NPVI_CLOCKS,
+		  NPVI_CLOCKS, NMIPI_CLOCKS, 0,
+		  SCLK_PVI_FREQ / 4, PCLK_MIN_FREQ / 4, PCLK_MAX_FREQ / 4,
+		  SCLK_MIPI_FREQ / 4, PCLK_MIN_FREQ / 4, PCLK_MAX_FREQ / 4),
+		NSEQ(REG_I_USE_REGS_API, 1),
+		0
+	};
+
+	s5k5baf_write_seq(state, REG_I_INCLK_FREQ_L, mclk & 0xffff, mclk >> 16);
+	s5k5baf_write_nseq(state, nseq_clk_cfg);
+
+	s5k5baf_synchronize(state, 250, REG_I_INIT_PARAMS_UPDATED);
+	status = s5k5baf_read(state, REG_I_ERROR_INFO);
+	if (!state->error && status) {
+		v4l2_err(&state->sd, "error configuring PLL (%d)\n", status);
+		state->error = -EINVAL;
+	}
+}
+
+/* set custom color correction matrices for various illuminations */
+static void s5k5baf_hw_set_ccm(struct s5k5baf *state)
+{
+	u16 *seq = s5k5baf_fw_get_seq(state, S5K5BAF_FW_ID_CCM);
+
+	if (seq)
+		s5k5baf_write_nseq(state, seq);
+}
+
+/* CIS sensor tuning, based on undocumented android driver code */
+static void s5k5baf_hw_set_cis(struct s5k5baf *state)
+{
+	u16 *seq = s5k5baf_fw_get_seq(state, S5K5BAF_FW_ID_CIS);
+
+	if (!seq)
+		return;
+
+	s5k5baf_i2c_write(state, REG_CMDWR_PAGE, PAGE_IF_HW);
+	s5k5baf_write_nseq(state, seq);
+	s5k5baf_i2c_write(state, REG_CMDWR_PAGE, PAGE_IF_SW);
+}
+
+static void s5k5baf_hw_sync_cfg(struct s5k5baf *state)
+{
+	s5k5baf_write(state, REG_G_PREV_CFG_CHG, 1);
+	if (state->apply_crop) {
+		s5k5baf_write(state, REG_G_INPUTS_CHANGE_REQ, 1);
+		s5k5baf_write(state, REG_G_PREV_CFG_BYPASS_CHANGED, 1);
+	}
+	s5k5baf_synchronize(state, 500, REG_G_NEW_CFG_SYNC);
+}
+/* Set horizontal and vertical image flipping */
+static void s5k5baf_hw_set_mirror(struct s5k5baf *state)
+{
+	u16 flip = state->ctrls.vflip->val | (state->ctrls.vflip->val << 1);
+
+	s5k5baf_write(state, REG_P_PREV_MIRROR(0), flip);
+	if (state->streaming)
+		s5k5baf_hw_sync_cfg(state);
+}
+
+static void s5k5baf_hw_set_alg(struct s5k5baf *state, u16 alg, bool enable)
+{
+	u16 cur_alg, new_alg;
+
+	if (!state->valid_auto_alg)
+		cur_alg = s5k5baf_read(state, REG_DBG_AUTOALG_EN);
+	else
+		cur_alg = state->auto_alg;
+
+	new_alg = enable ? (cur_alg | alg) : (cur_alg & ~alg);
+
+	if (new_alg != cur_alg)
+		s5k5baf_write(state, REG_DBG_AUTOALG_EN, new_alg);
+
+	if (state->error)
+		return;
+
+	state->valid_auto_alg = 1;
+	state->auto_alg = new_alg;
+}
+
+/* Configure auto/manual white balance and R/G/B gains */
+static void s5k5baf_hw_set_awb(struct s5k5baf *state, int awb)
+{
+	struct s5k5baf_ctrls *ctrls = &state->ctrls;
+
+	if (!awb)
+		s5k5baf_write_seq(state, REG_SF_RGAIN,
+				  ctrls->gain_red->val, 1,
+				  S5K5BAF_GAIN_GREEN_DEF, 1,
+				  ctrls->gain_blue->val, 1,
+				  1);
+
+	s5k5baf_hw_set_alg(state, AALG_WB_EN, awb);
+}
+
+/* Program FW with exposure time, 'exposure' in us units */
+static void s5k5baf_hw_set_user_exposure(struct s5k5baf *state, int exposure)
+{
+	unsigned int time = exposure / 10;
+
+	s5k5baf_write_seq(state, REG_SF_USR_EXPOSURE_L,
+			  time & 0xffff, time >> 16, 1);
+}
+
+static void s5k5baf_hw_set_user_gain(struct s5k5baf *state, int gain)
+{
+	s5k5baf_write_seq(state, REG_SF_USR_TOT_GAIN, gain, 1);
+}
+
+/* Set auto/manual exposure and total gain */
+static void s5k5baf_hw_set_auto_exposure(struct s5k5baf *state, int value)
+{
+	if (value == V4L2_EXPOSURE_AUTO) {
+		s5k5baf_hw_set_alg(state, AALG_AE_EN | AALG_DIVLEI_EN, true);
+	} else {
+		unsigned int exp_time = state->ctrls.exposure->val;
+
+		s5k5baf_hw_set_user_exposure(state, exp_time);
+		s5k5baf_hw_set_user_gain(state, state->ctrls.gain->val);
+		s5k5baf_hw_set_alg(state, AALG_AE_EN | AALG_DIVLEI_EN, false);
+	}
+}
+
+static void s5k5baf_hw_set_anti_flicker(struct s5k5baf *state, int v)
+{
+	if (v == V4L2_CID_POWER_LINE_FREQUENCY_AUTO) {
+		s5k5baf_hw_set_alg(state, AALG_FLICKER_EN, true);
+	} else {
+		/* The V4L2_CID_LINE_FREQUENCY control values match
+		 * the register values */
+		s5k5baf_write_seq(state, REG_SF_FLICKER_QUANT, v, 1);
+		s5k5baf_hw_set_alg(state, AALG_FLICKER_EN, false);
+	}
+}
+
+static void s5k5baf_hw_set_colorfx(struct s5k5baf *state, int val)
+{
+	static const u16 colorfx[] = {
+		[V4L2_COLORFX_NONE] = 0,
+		[V4L2_COLORFX_BW] = 1,
+		[V4L2_COLORFX_NEGATIVE] = 2,
+		[V4L2_COLORFX_SEPIA] = 3,
+		[V4L2_COLORFX_SKY_BLUE] = 4,
+		[V4L2_COLORFX_SKETCH] = 5,
+	};
+
+	s5k5baf_write(state, REG_G_SPEC_EFFECTS, colorfx[val]);
+}
+
+static int s5k5baf_find_pixfmt(struct v4l2_mbus_framefmt *mf)
+{
+	int i, c = -1;
+
+	for (i = 0; i < ARRAY_SIZE(s5k5baf_formats); i++) {
+		if (mf->colorspace != s5k5baf_formats[i].colorspace)
+			continue;
+		if (mf->code == s5k5baf_formats[i].code)
+			return i;
+		if (c < 0)
+			c = i;
+	}
+	return (c < 0) ? 0 : c;
+}
+
+static int s5k5baf_clear_error(struct s5k5baf *state)
+{
+	int ret = state->error;
+
+	state->error = 0;
+	return ret;
+}
+
+static int s5k5baf_hw_set_video_bus(struct s5k5baf *state)
+{
+	u16 en_pkts;
+
+	if (state->bus_type == V4L2_MBUS_CSI2_DPHY)
+		en_pkts = EN_PACKETS_CSI2;
+	else
+		en_pkts = 0;
+
+	s5k5baf_write_seq(state, REG_OIF_EN_MIPI_LANES,
+			  state->nlanes, en_pkts, 1);
+
+	return s5k5baf_clear_error(state);
+}
+
+static u16 s5k5baf_get_cfg_error(struct s5k5baf *state)
+{
+	u16 err = s5k5baf_read(state, REG_G_PREV_CFG_ERROR);
+	if (err)
+		s5k5baf_write(state, REG_G_PREV_CFG_ERROR, 0);
+	return err;
+}
+
+static void s5k5baf_hw_set_fiv(struct s5k5baf *state, u16 fiv)
+{
+	s5k5baf_write(state, REG_P_MAX_FR_TIME(0), fiv);
+	s5k5baf_hw_sync_cfg(state);
+}
+
+static void s5k5baf_hw_find_min_fiv(struct s5k5baf *state)
+{
+	u16 err, fiv;
+	int n;
+
+	fiv = s5k5baf_read(state,  REG_G_ACTUAL_P_FR_TIME);
+	if (state->error)
+		return;
+
+	for (n = 5; n > 0; --n) {
+		s5k5baf_hw_set_fiv(state, fiv);
+		err = s5k5baf_get_cfg_error(state);
+		if (state->error)
+			return;
+		switch (err) {
+		case CFG_ERROR_RANGE:
+			++fiv;
+			break;
+		case 0:
+			state->fiv = fiv;
+			v4l2_info(&state->sd,
+				  "found valid frame interval: %d00us\n", fiv);
+			return;
+		default:
+			v4l2_err(&state->sd,
+				 "error setting frame interval: %d\n", err);
+			state->error = -EINVAL;
+		}
+	}
+	v4l2_err(&state->sd, "cannot find correct frame interval\n");
+	state->error = -ERANGE;
+}
+
+static void s5k5baf_hw_validate_cfg(struct s5k5baf *state)
+{
+	u16 err;
+
+	err = s5k5baf_get_cfg_error(state);
+	if (state->error)
+		return;
+
+	switch (err) {
+	case 0:
+		state->apply_cfg = 1;
+		return;
+	case CFG_ERROR_RANGE:
+		s5k5baf_hw_find_min_fiv(state);
+		if (!state->error)
+			state->apply_cfg = 1;
+		return;
+	default:
+		v4l2_err(&state->sd,
+			 "error setting format: %d\n", err);
+		state->error = -EINVAL;
+	}
+}
+
+static void s5k5baf_rescale(struct v4l2_rect *r, const struct v4l2_rect *v,
+			    const struct v4l2_rect *n,
+			    const struct v4l2_rect *d)
+{
+	r->left = v->left * n->width / d->width;
+	r->top = v->top * n->height / d->height;
+	r->width = v->width * n->width / d->width;
+	r->height = v->height * n->height / d->height;
+}
+
+static int s5k5baf_hw_set_crop_rects(struct s5k5baf *state)
+{
+	struct v4l2_rect *p, r;
+	u16 err;
+	int ret;
+
+	p = &state->crop_sink;
+	s5k5baf_write_seq(state, REG_G_PREVREQ_IN_WIDTH, p->width, p->height,
+			  p->left, p->top);
+
+	s5k5baf_rescale(&r, &state->crop_source, &state->crop_sink,
+			&state->compose);
+	s5k5baf_write_seq(state, REG_G_PREVZOOM_IN_WIDTH, r.width, r.height,
+			  r.left, r.top);
+
+	s5k5baf_synchronize(state, 500, REG_G_INPUTS_CHANGE_REQ);
+	s5k5baf_synchronize(state, 500, REG_G_PREV_CFG_BYPASS_CHANGED);
+	err = s5k5baf_get_cfg_error(state);
+	ret = s5k5baf_clear_error(state);
+	if (ret < 0)
+		return ret;
+
+	switch (err) {
+	case 0:
+		break;
+	case CFG_ERROR_RANGE:
+		/* retry crop with frame interval set to max */
+		s5k5baf_hw_set_fiv(state, S5K5BAF_MAX_FR_TIME);
+		err = s5k5baf_get_cfg_error(state);
+		ret = s5k5baf_clear_error(state);
+		if (ret < 0)
+			return ret;
+		if (err) {
+			v4l2_err(&state->sd,
+				 "crop error on max frame interval: %d\n", err);
+			state->error = -EINVAL;
+		}
+		s5k5baf_hw_set_fiv(state, state->req_fiv);
+		s5k5baf_hw_validate_cfg(state);
+		break;
+	default:
+		v4l2_err(&state->sd, "crop error: %d\n", err);
+		return -EINVAL;
+	}
+
+	if (!state->apply_cfg)
+		return 0;
+
+	p = &state->crop_source;
+	s5k5baf_write_seq(state, REG_P_OUT_WIDTH(0), p->width, p->height);
+	s5k5baf_hw_set_fiv(state, state->req_fiv);
+	s5k5baf_hw_validate_cfg(state);
+
+	return s5k5baf_clear_error(state);
+}
+
+static void s5k5baf_hw_set_config(struct s5k5baf *state)
+{
+	u16 reg_fmt = s5k5baf_formats[state->pixfmt].reg_p_fmt;
+	struct v4l2_rect *r = &state->crop_source;
+
+	s5k5baf_write_seq(state, REG_P_OUT_WIDTH(0),
+			  r->width, r->height, reg_fmt,
+			  PCLK_MAX_FREQ >> 2, PCLK_MIN_FREQ >> 2,
+			  PVI_MASK_MIPI, CLK_MIPI_INDEX,
+			  FR_RATE_FIXED, FR_RATE_Q_DYNAMIC,
+			  state->req_fiv, S5K5BAF_MIN_FR_TIME);
+	s5k5baf_hw_sync_cfg(state);
+	s5k5baf_hw_validate_cfg(state);
+}
+
+
+static void s5k5baf_hw_set_test_pattern(struct s5k5baf *state, int id)
+{
+	s5k5baf_i2c_write(state, REG_PATTERN_WIDTH, 800);
+	s5k5baf_i2c_write(state, REG_PATTERN_HEIGHT, 511);
+	s5k5baf_i2c_write(state, REG_PATTERN_PARAM, 0);
+	s5k5baf_i2c_write(state, REG_PATTERN_SET, id);
+}
+
+static void s5k5baf_gpio_assert(struct s5k5baf *state, int id)
+{
+	gpiod_set_value_cansleep(state->gpios[id], 1);
+}
+
+static void s5k5baf_gpio_deassert(struct s5k5baf *state, int id)
+{
+	gpiod_set_value_cansleep(state->gpios[id], 0);
+}
+
+static int s5k5baf_power_on(struct s5k5baf *state)
+{
+	int ret;
+
+	ret = regulator_bulk_enable(S5K5BAF_NUM_SUPPLIES, state->supplies);
+	if (ret < 0)
+		goto err;
+
+	ret = clk_set_rate(state->clock, state->mclk_frequency);
+	if (ret < 0)
+		goto err_reg_dis;
+
+	ret = clk_prepare_enable(state->clock);
+	if (ret < 0)
+		goto err_reg_dis;
+
+	v4l2_dbg(1, debug, &state->sd, "clock frequency: %ld\n",
+		 clk_get_rate(state->clock));
+
+	s5k5baf_gpio_deassert(state, STBY);
+	usleep_range(50, 100);
+	s5k5baf_gpio_deassert(state, RSET);
+	return 0;
+
+err_reg_dis:
+	regulator_bulk_disable(S5K5BAF_NUM_SUPPLIES, state->supplies);
+err:
+	v4l2_err(&state->sd, "%s() failed (%d)\n", __func__, ret);
+	return ret;
+}
+
+static int s5k5baf_power_off(struct s5k5baf *state)
+{
+	int ret;
+
+	state->streaming = 0;
+	state->apply_cfg = 0;
+	state->apply_crop = 0;
+
+	s5k5baf_gpio_assert(state, RSET);
+	s5k5baf_gpio_assert(state, STBY);
+
+	if (!IS_ERR(state->clock))
+		clk_disable_unprepare(state->clock);
+
+	ret = regulator_bulk_disable(S5K5BAF_NUM_SUPPLIES,
+					state->supplies);
+	if (ret < 0)
+		v4l2_err(&state->sd, "failed to disable regulators\n");
+
+	return 0;
+}
+
+static void s5k5baf_hw_init(struct s5k5baf *state)
+{
+	s5k5baf_i2c_write(state, AHB_MSB_ADDR_PTR, PAGE_IF_HW);
+	s5k5baf_i2c_write(state, REG_CLEAR_HOST_INT, 0);
+	s5k5baf_i2c_write(state, REG_SW_LOAD_COMPLETE, 1);
+	s5k5baf_i2c_write(state, REG_CMDRD_PAGE, PAGE_IF_SW);
+	s5k5baf_i2c_write(state, REG_CMDWR_PAGE, PAGE_IF_SW);
+}
+
+/*
+ * V4L2 subdev core and video operations
+ */
+
+static void s5k5baf_initialize_data(struct s5k5baf *state)
+{
+	state->pixfmt = 0;
+	state->req_fiv = 10000 / 15;
+	state->fiv = state->req_fiv;
+	state->valid_auto_alg = 0;
+}
+
+static int s5k5baf_load_setfile(struct s5k5baf *state)
+{
+	struct i2c_client *c = v4l2_get_subdevdata(&state->sd);
+	const struct firmware *fw;
+	int ret;
+
+	ret = request_firmware(&fw, S5K5BAF_FW_FILENAME, &c->dev);
+	if (ret < 0) {
+		dev_warn(&c->dev, "firmware file (%s) not loaded\n",
+			 S5K5BAF_FW_FILENAME);
+		return ret;
+	}
+
+	ret = s5k5baf_fw_parse(&c->dev, &state->fw, fw->size / 2,
+			       (__le16 *)fw->data);
+
+	release_firmware(fw);
+
+	return ret;
+}
+
+static int s5k5baf_set_power(struct v4l2_subdev *sd, int on)
+{
+	struct s5k5baf *state = to_s5k5baf(sd);
+	int ret = 0;
+
+	mutex_lock(&state->lock);
+
+	if (state->power != !on)
+		goto out;
+
+	if (on) {
+		if (state->fw == NULL)
+			s5k5baf_load_setfile(state);
+
+		s5k5baf_initialize_data(state);
+		ret = s5k5baf_power_on(state);
+		if (ret < 0)
+			goto out;
+
+		s5k5baf_hw_init(state);
+		s5k5baf_hw_patch(state);
+		s5k5baf_i2c_write(state, REG_SET_HOST_INT, 1);
+		s5k5baf_hw_set_clocks(state);
+
+		ret = s5k5baf_hw_set_video_bus(state);
+		if (ret < 0)
+			goto out;
+
+		s5k5baf_hw_set_cis(state);
+		s5k5baf_hw_set_ccm(state);
+
+		ret = s5k5baf_clear_error(state);
+		if (!ret)
+			state->power++;
+	} else {
+		s5k5baf_power_off(state);
+		state->power--;
+	}
+
+out:
+	mutex_unlock(&state->lock);
+
+	if (!ret && on)
+		ret = v4l2_ctrl_handler_setup(&state->ctrls.handler);
+
+	return ret;
+}
+
+static void s5k5baf_hw_set_stream(struct s5k5baf *state, int enable)
+{
+	s5k5baf_write_seq(state, REG_G_ENABLE_PREV, enable, 1);
+}
+
+static int s5k5baf_s_stream(struct v4l2_subdev *sd, int on)
+{
+	struct s5k5baf *state = to_s5k5baf(sd);
+	int ret;
+
+	mutex_lock(&state->lock);
+
+	if (state->streaming == !!on) {
+		ret = 0;
+		goto out;
+	}
+
+	if (on) {
+		s5k5baf_hw_set_config(state);
+		ret = s5k5baf_hw_set_crop_rects(state);
+		if (ret < 0)
+			goto out;
+		s5k5baf_hw_set_stream(state, 1);
+		s5k5baf_i2c_write(state, 0xb0cc, 0x000b);
+	} else {
+		s5k5baf_hw_set_stream(state, 0);
+	}
+	ret = s5k5baf_clear_error(state);
+	if (!ret)
+		state->streaming = !state->streaming;
+
+out:
+	mutex_unlock(&state->lock);
+
+	return ret;
+}
+
+static int s5k5baf_g_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *fi)
+{
+	struct s5k5baf *state = to_s5k5baf(sd);
+
+	mutex_lock(&state->lock);
+	fi->interval.numerator = state->fiv;
+	fi->interval.denominator = 10000;
+	mutex_unlock(&state->lock);
+
+	return 0;
+}
+
+static void s5k5baf_set_frame_interval(struct s5k5baf *state,
+				       struct v4l2_subdev_frame_interval *fi)
+{
+	struct v4l2_fract *i = &fi->interval;
+
+	if (fi->interval.denominator == 0)
+		state->req_fiv = S5K5BAF_MAX_FR_TIME;
+	else
+		state->req_fiv = clamp_t(u32,
+					 i->numerator * 10000 / i->denominator,
+					 S5K5BAF_MIN_FR_TIME,
+					 S5K5BAF_MAX_FR_TIME);
+
+	state->fiv = state->req_fiv;
+	if (state->apply_cfg) {
+		s5k5baf_hw_set_fiv(state, state->req_fiv);
+		s5k5baf_hw_validate_cfg(state);
+	}
+	*i = (struct v4l2_fract){ state->fiv, 10000 };
+	if (state->fiv == state->req_fiv)
+		v4l2_info(&state->sd, "frame interval changed to %d00us\n",
+			  state->fiv);
+}
+
+static int s5k5baf_s_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *fi)
+{
+	struct s5k5baf *state = to_s5k5baf(sd);
+
+	mutex_lock(&state->lock);
+	s5k5baf_set_frame_interval(state, fi);
+	mutex_unlock(&state->lock);
+	return 0;
+}
+
+/*
+ * V4L2 subdev pad level and video operations
+ */
+static int s5k5baf_enum_frame_interval(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_frame_interval_enum *fie)
+{
+	if (fie->index > S5K5BAF_MAX_FR_TIME - S5K5BAF_MIN_FR_TIME ||
+	    fie->pad != PAD_CIS)
+		return -EINVAL;
+
+	v4l_bound_align_image(&fie->width, S5K5BAF_WIN_WIDTH_MIN,
+			      S5K5BAF_CIS_WIDTH, 1,
+			      &fie->height, S5K5BAF_WIN_HEIGHT_MIN,
+			      S5K5BAF_CIS_HEIGHT, 1, 0);
+
+	fie->interval.numerator = S5K5BAF_MIN_FR_TIME + fie->index;
+	fie->interval.denominator = 10000;
+
+	return 0;
+}
+
+static int s5k5baf_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->pad == PAD_CIS) {
+		if (code->index > 0)
+			return -EINVAL;
+		code->code = MEDIA_BUS_FMT_FIXED;
+		return 0;
+	}
+
+	if (code->index >= ARRAY_SIZE(s5k5baf_formats))
+		return -EINVAL;
+
+	code->code = s5k5baf_formats[code->index].code;
+	return 0;
+}
+
+static int s5k5baf_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	int i;
+
+	if (fse->index > 0)
+		return -EINVAL;
+
+	if (fse->pad == PAD_CIS) {
+		fse->code = MEDIA_BUS_FMT_FIXED;
+		fse->min_width = S5K5BAF_CIS_WIDTH;
+		fse->max_width = S5K5BAF_CIS_WIDTH;
+		fse->min_height = S5K5BAF_CIS_HEIGHT;
+		fse->max_height = S5K5BAF_CIS_HEIGHT;
+		return 0;
+	}
+
+	i = ARRAY_SIZE(s5k5baf_formats);
+	while (--i)
+		if (fse->code == s5k5baf_formats[i].code)
+			break;
+	fse->code = s5k5baf_formats[i].code;
+	fse->min_width = S5K5BAF_WIN_WIDTH_MIN;
+	fse->max_width = S5K5BAF_CIS_WIDTH;
+	fse->max_height = S5K5BAF_WIN_HEIGHT_MIN;
+	fse->min_height = S5K5BAF_CIS_HEIGHT;
+
+	return 0;
+}
+
+static void s5k5baf_try_cis_format(struct v4l2_mbus_framefmt *mf)
+{
+	mf->width = S5K5BAF_CIS_WIDTH;
+	mf->height = S5K5BAF_CIS_HEIGHT;
+	mf->code = MEDIA_BUS_FMT_FIXED;
+	mf->colorspace = V4L2_COLORSPACE_JPEG;
+	mf->field = V4L2_FIELD_NONE;
+}
+
+static int s5k5baf_try_isp_format(struct v4l2_mbus_framefmt *mf)
+{
+	int pixfmt;
+
+	v4l_bound_align_image(&mf->width, S5K5BAF_WIN_WIDTH_MIN,
+			      S5K5BAF_CIS_WIDTH, 1,
+			      &mf->height, S5K5BAF_WIN_HEIGHT_MIN,
+			      S5K5BAF_CIS_HEIGHT, 1, 0);
+
+	pixfmt = s5k5baf_find_pixfmt(mf);
+
+	mf->colorspace = s5k5baf_formats[pixfmt].colorspace;
+	mf->code = s5k5baf_formats[pixfmt].code;
+	mf->field = V4L2_FIELD_NONE;
+
+	return pixfmt;
+}
+
+static int s5k5baf_get_fmt(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_state *sd_state,
+			   struct v4l2_subdev_format *fmt)
+{
+	struct s5k5baf *state = to_s5k5baf(sd);
+	const struct s5k5baf_pixfmt *pixfmt;
+	struct v4l2_mbus_framefmt *mf;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		mf = v4l2_subdev_get_try_format(sd, sd_state, fmt->pad);
+		fmt->format = *mf;
+		return 0;
+	}
+
+	mf = &fmt->format;
+	if (fmt->pad == PAD_CIS) {
+		s5k5baf_try_cis_format(mf);
+		return 0;
+	}
+	mf->field = V4L2_FIELD_NONE;
+	mutex_lock(&state->lock);
+	pixfmt = &s5k5baf_formats[state->pixfmt];
+	mf->width = state->crop_source.width;
+	mf->height = state->crop_source.height;
+	mf->code = pixfmt->code;
+	mf->colorspace = pixfmt->colorspace;
+	mutex_unlock(&state->lock);
+
+	return 0;
+}
+
+static int s5k5baf_set_fmt(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_state *sd_state,
+			   struct v4l2_subdev_format *fmt)
+{
+	struct v4l2_mbus_framefmt *mf = &fmt->format;
+	struct s5k5baf *state = to_s5k5baf(sd);
+	const struct s5k5baf_pixfmt *pixfmt;
+	int ret = 0;
+
+	mf->field = V4L2_FIELD_NONE;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		*v4l2_subdev_get_try_format(sd, sd_state, fmt->pad) = *mf;
+		return 0;
+	}
+
+	if (fmt->pad == PAD_CIS) {
+		s5k5baf_try_cis_format(mf);
+		return 0;
+	}
+
+	mutex_lock(&state->lock);
+
+	if (state->streaming) {
+		mutex_unlock(&state->lock);
+		return -EBUSY;
+	}
+
+	state->pixfmt = s5k5baf_try_isp_format(mf);
+	pixfmt = &s5k5baf_formats[state->pixfmt];
+	mf->code = pixfmt->code;
+	mf->colorspace = pixfmt->colorspace;
+	mf->width = state->crop_source.width;
+	mf->height = state->crop_source.height;
+
+	mutex_unlock(&state->lock);
+	return ret;
+}
+
+enum selection_rect { R_CIS, R_CROP_SINK, R_COMPOSE, R_CROP_SOURCE, R_INVALID };
+
+static enum selection_rect s5k5baf_get_sel_rect(u32 pad, u32 target)
+{
+	switch (target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		return pad ? R_COMPOSE : R_CIS;
+	case V4L2_SEL_TGT_CROP:
+		return pad ? R_CROP_SOURCE : R_CROP_SINK;
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+		return pad ? R_INVALID : R_CROP_SINK;
+	case V4L2_SEL_TGT_COMPOSE:
+		return pad ? R_INVALID : R_COMPOSE;
+	default:
+		return R_INVALID;
+	}
+}
+
+static int s5k5baf_is_bound_target(u32 target)
+{
+	return target == V4L2_SEL_TGT_CROP_BOUNDS ||
+		target == V4L2_SEL_TGT_COMPOSE_BOUNDS;
+}
+
+static int s5k5baf_get_selection(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_selection *sel)
+{
+	enum selection_rect rtype;
+	struct s5k5baf *state = to_s5k5baf(sd);
+
+	rtype = s5k5baf_get_sel_rect(sel->pad, sel->target);
+
+	switch (rtype) {
+	case R_INVALID:
+		return -EINVAL;
+	case R_CIS:
+		sel->r = s5k5baf_cis_rect;
+		return 0;
+	default:
+		break;
+	}
+
+	if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
+		if (rtype == R_COMPOSE)
+			sel->r = *v4l2_subdev_get_try_compose(sd, sd_state,
+							      sel->pad);
+		else
+			sel->r = *v4l2_subdev_get_try_crop(sd, sd_state,
+							   sel->pad);
+		return 0;
+	}
+
+	mutex_lock(&state->lock);
+	switch (rtype) {
+	case R_CROP_SINK:
+		sel->r = state->crop_sink;
+		break;
+	case R_COMPOSE:
+		sel->r = state->compose;
+		break;
+	case R_CROP_SOURCE:
+		sel->r = state->crop_source;
+		break;
+	default:
+		break;
+	}
+	if (s5k5baf_is_bound_target(sel->target)) {
+		sel->r.left = 0;
+		sel->r.top = 0;
+	}
+	mutex_unlock(&state->lock);
+
+	return 0;
+}
+
+/* bounds range [start, start+len) to [0, max) and aligns to 2 */
+static void s5k5baf_bound_range(u32 *start, u32 *len, u32 max)
+{
+	if (*len > max)
+		*len = max;
+	if (*start + *len > max)
+		*start = max - *len;
+	*start &= ~1;
+	*len &= ~1;
+	if (*len < S5K5BAF_WIN_WIDTH_MIN)
+		*len = S5K5BAF_WIN_WIDTH_MIN;
+}
+
+static void s5k5baf_bound_rect(struct v4l2_rect *r, u32 width, u32 height)
+{
+	s5k5baf_bound_range(&r->left, &r->width, width);
+	s5k5baf_bound_range(&r->top, &r->height, height);
+}
+
+static void s5k5baf_set_rect_and_adjust(struct v4l2_rect **rects,
+					enum selection_rect first,
+					struct v4l2_rect *v)
+{
+	struct v4l2_rect *r, *br;
+	enum selection_rect i = first;
+
+	*rects[first] = *v;
+	do {
+		r = rects[i];
+		br = rects[i - 1];
+		s5k5baf_bound_rect(r, br->width, br->height);
+	} while (++i != R_INVALID);
+	*v = *rects[first];
+}
+
+static bool s5k5baf_cmp_rect(const struct v4l2_rect *r1,
+			     const struct v4l2_rect *r2)
+{
+	return !memcmp(r1, r2, sizeof(*r1));
+}
+
+static int s5k5baf_set_selection(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_selection *sel)
+{
+	static enum selection_rect rtype;
+	struct s5k5baf *state = to_s5k5baf(sd);
+	struct v4l2_rect **rects;
+	int ret = 0;
+
+	rtype = s5k5baf_get_sel_rect(sel->pad, sel->target);
+	if (rtype == R_INVALID || s5k5baf_is_bound_target(sel->target))
+		return -EINVAL;
+
+	/* allow only scaling on compose */
+	if (rtype == R_COMPOSE) {
+		sel->r.left = 0;
+		sel->r.top = 0;
+	}
+
+	if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
+		rects = (struct v4l2_rect * []) {
+				&s5k5baf_cis_rect,
+				v4l2_subdev_get_try_crop(sd, sd_state,
+							 PAD_CIS),
+				v4l2_subdev_get_try_compose(sd, sd_state,
+							    PAD_CIS),
+				v4l2_subdev_get_try_crop(sd, sd_state,
+							 PAD_OUT)
+			};
+		s5k5baf_set_rect_and_adjust(rects, rtype, &sel->r);
+		return 0;
+	}
+
+	rects = (struct v4l2_rect * []) {
+			&s5k5baf_cis_rect,
+			&state->crop_sink,
+			&state->compose,
+			&state->crop_source
+		};
+	mutex_lock(&state->lock);
+	if (state->streaming) {
+		/* adjust sel->r to avoid output resolution change */
+		if (rtype < R_CROP_SOURCE) {
+			if (sel->r.width < state->crop_source.width)
+				sel->r.width = state->crop_source.width;
+			if (sel->r.height < state->crop_source.height)
+				sel->r.height = state->crop_source.height;
+		} else {
+			sel->r.width = state->crop_source.width;
+			sel->r.height = state->crop_source.height;
+		}
+	}
+	s5k5baf_set_rect_and_adjust(rects, rtype, &sel->r);
+	if (!s5k5baf_cmp_rect(&state->crop_sink, &s5k5baf_cis_rect) ||
+	    !s5k5baf_cmp_rect(&state->compose, &s5k5baf_cis_rect))
+		state->apply_crop = 1;
+	if (state->streaming)
+		ret = s5k5baf_hw_set_crop_rects(state);
+	mutex_unlock(&state->lock);
+
+	return ret;
+}
+
+static const struct v4l2_subdev_pad_ops s5k5baf_cis_pad_ops = {
+	.enum_mbus_code		= s5k5baf_enum_mbus_code,
+	.enum_frame_size	= s5k5baf_enum_frame_size,
+	.get_fmt		= s5k5baf_get_fmt,
+	.set_fmt		= s5k5baf_set_fmt,
+};
+
+static const struct v4l2_subdev_pad_ops s5k5baf_pad_ops = {
+	.enum_mbus_code		= s5k5baf_enum_mbus_code,
+	.enum_frame_size	= s5k5baf_enum_frame_size,
+	.enum_frame_interval	= s5k5baf_enum_frame_interval,
+	.get_fmt		= s5k5baf_get_fmt,
+	.set_fmt		= s5k5baf_set_fmt,
+	.get_selection		= s5k5baf_get_selection,
+	.set_selection		= s5k5baf_set_selection,
+};
+
+static const struct v4l2_subdev_video_ops s5k5baf_video_ops = {
+	.g_frame_interval	= s5k5baf_g_frame_interval,
+	.s_frame_interval	= s5k5baf_s_frame_interval,
+	.s_stream		= s5k5baf_s_stream,
+};
+
+/*
+ * V4L2 subdev controls
+ */
+
+static int s5k5baf_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
+	struct s5k5baf *state = to_s5k5baf(sd);
+	int ret;
+
+	v4l2_dbg(1, debug, sd, "ctrl: %s, value: %d\n", ctrl->name, ctrl->val);
+
+	mutex_lock(&state->lock);
+
+	if (state->power == 0)
+		goto unlock;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		s5k5baf_hw_set_awb(state, ctrl->val);
+		break;
+
+	case V4L2_CID_BRIGHTNESS:
+		s5k5baf_write(state, REG_USER_BRIGHTNESS, ctrl->val);
+		break;
+
+	case V4L2_CID_COLORFX:
+		s5k5baf_hw_set_colorfx(state, ctrl->val);
+		break;
+
+	case V4L2_CID_CONTRAST:
+		s5k5baf_write(state, REG_USER_CONTRAST, ctrl->val);
+		break;
+
+	case V4L2_CID_EXPOSURE_AUTO:
+		s5k5baf_hw_set_auto_exposure(state, ctrl->val);
+		break;
+
+	case V4L2_CID_HFLIP:
+		s5k5baf_hw_set_mirror(state);
+		break;
+
+	case V4L2_CID_POWER_LINE_FREQUENCY:
+		s5k5baf_hw_set_anti_flicker(state, ctrl->val);
+		break;
+
+	case V4L2_CID_SATURATION:
+		s5k5baf_write(state, REG_USER_SATURATION, ctrl->val);
+		break;
+
+	case V4L2_CID_SHARPNESS:
+		s5k5baf_write(state, REG_USER_SHARPBLUR, ctrl->val);
+		break;
+
+	case V4L2_CID_WHITE_BALANCE_TEMPERATURE:
+		s5k5baf_write(state, REG_P_COLORTEMP(0), ctrl->val);
+		if (state->apply_cfg)
+			s5k5baf_hw_sync_cfg(state);
+		break;
+
+	case V4L2_CID_TEST_PATTERN:
+		s5k5baf_hw_set_test_pattern(state, ctrl->val);
+		break;
+	}
+unlock:
+	ret = s5k5baf_clear_error(state);
+	mutex_unlock(&state->lock);
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops s5k5baf_ctrl_ops = {
+	.s_ctrl	= s5k5baf_s_ctrl,
+};
+
+static const char * const s5k5baf_test_pattern_menu[] = {
+	"Disabled",
+	"Blank",
+	"Bars",
+	"Gradients",
+	"Textile",
+	"Textile2",
+	"Squares"
+};
+
+static int s5k5baf_initialize_ctrls(struct s5k5baf *state)
+{
+	const struct v4l2_ctrl_ops *ops = &s5k5baf_ctrl_ops;
+	struct s5k5baf_ctrls *ctrls = &state->ctrls;
+	struct v4l2_ctrl_handler *hdl = &ctrls->handler;
+	int ret;
+
+	ret = v4l2_ctrl_handler_init(hdl, 16);
+	if (ret < 0) {
+		v4l2_err(&state->sd, "cannot init ctrl handler (%d)\n", ret);
+		return ret;
+	}
+
+	/* Auto white balance cluster */
+	ctrls->awb = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_AUTO_WHITE_BALANCE,
+				       0, 1, 1, 1);
+	ctrls->gain_red = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_RED_BALANCE,
+					    0, 255, 1, S5K5BAF_GAIN_RED_DEF);
+	ctrls->gain_blue = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BLUE_BALANCE,
+					     0, 255, 1, S5K5BAF_GAIN_BLUE_DEF);
+	v4l2_ctrl_auto_cluster(3, &ctrls->awb, 0, false);
+
+	ctrls->hflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
+	ctrls->vflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_cluster(2, &ctrls->hflip);
+
+	ctrls->auto_exp = v4l2_ctrl_new_std_menu(hdl, ops,
+				V4L2_CID_EXPOSURE_AUTO,
+				V4L2_EXPOSURE_MANUAL, 0, V4L2_EXPOSURE_AUTO);
+	/* Exposure time: x 1 us */
+	ctrls->exposure = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
+					    0, 6000000U, 1, 100000U);
+	/* Total gain: 256 <=> 1x */
+	ctrls->gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_GAIN,
+					0, 256, 1, 256);
+	v4l2_ctrl_auto_cluster(3, &ctrls->auto_exp, 0, false);
+
+	v4l2_ctrl_new_std_menu(hdl, ops, V4L2_CID_POWER_LINE_FREQUENCY,
+			       V4L2_CID_POWER_LINE_FREQUENCY_AUTO, 0,
+			       V4L2_CID_POWER_LINE_FREQUENCY_AUTO);
+
+	v4l2_ctrl_new_std_menu(hdl, ops, V4L2_CID_COLORFX,
+			       V4L2_COLORFX_SKY_BLUE, ~0x6f, V4L2_COLORFX_NONE);
+
+	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_WHITE_BALANCE_TEMPERATURE,
+			  0, 256, 1, 0);
+
+	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SATURATION, -127, 127, 1, 0);
+	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BRIGHTNESS, -127, 127, 1, 0);
+	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_CONTRAST, -127, 127, 1, 0);
+	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SHARPNESS, -127, 127, 1, 0);
+
+	v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(s5k5baf_test_pattern_menu) - 1,
+				     0, 0, s5k5baf_test_pattern_menu);
+
+	if (hdl->error) {
+		v4l2_err(&state->sd, "error creating controls (%d)\n",
+			 hdl->error);
+		ret = hdl->error;
+		v4l2_ctrl_handler_free(hdl);
+		return ret;
+	}
+
+	state->sd.ctrl_handler = hdl;
+	return 0;
+}
+
+/*
+ * V4L2 subdev internal operations
+ */
+static int s5k5baf_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_mbus_framefmt *mf;
+
+	mf = v4l2_subdev_get_try_format(sd, fh->state, PAD_CIS);
+	s5k5baf_try_cis_format(mf);
+
+	if (s5k5baf_is_cis_subdev(sd))
+		return 0;
+
+	mf = v4l2_subdev_get_try_format(sd, fh->state, PAD_OUT);
+	mf->colorspace = s5k5baf_formats[0].colorspace;
+	mf->code = s5k5baf_formats[0].code;
+	mf->width = s5k5baf_cis_rect.width;
+	mf->height = s5k5baf_cis_rect.height;
+	mf->field = V4L2_FIELD_NONE;
+
+	*v4l2_subdev_get_try_crop(sd, fh->state, PAD_CIS) = s5k5baf_cis_rect;
+	*v4l2_subdev_get_try_compose(sd, fh->state, PAD_CIS) = s5k5baf_cis_rect;
+	*v4l2_subdev_get_try_crop(sd, fh->state, PAD_OUT) = s5k5baf_cis_rect;
+
+	return 0;
+}
+
+static int s5k5baf_check_fw_revision(struct s5k5baf *state)
+{
+	u16 api_ver = 0, fw_rev = 0, s_id = 0;
+	int ret;
+
+	api_ver = s5k5baf_read(state, REG_FW_APIVER);
+	fw_rev = s5k5baf_read(state, REG_FW_REVISION) & 0xff;
+	s_id = s5k5baf_read(state, REG_FW_SENSOR_ID);
+	ret = s5k5baf_clear_error(state);
+	if (ret < 0)
+		return ret;
+
+	v4l2_info(&state->sd, "FW API=%#x, revision=%#x sensor_id=%#x\n",
+		  api_ver, fw_rev, s_id);
+
+	if (api_ver != S5K5BAF_FW_APIVER) {
+		v4l2_err(&state->sd, "FW API version not supported\n");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static int s5k5baf_registered(struct v4l2_subdev *sd)
+{
+	struct s5k5baf *state = to_s5k5baf(sd);
+	int ret;
+
+	ret = v4l2_device_register_subdev(sd->v4l2_dev, &state->cis_sd);
+	if (ret < 0)
+		v4l2_err(sd, "failed to register subdev %s\n",
+			 state->cis_sd.name);
+	else
+		ret = media_create_pad_link(&state->cis_sd.entity, PAD_CIS,
+					       &state->sd.entity, PAD_CIS,
+					       MEDIA_LNK_FL_IMMUTABLE |
+					       MEDIA_LNK_FL_ENABLED);
+	return ret;
+}
+
+static void s5k5baf_unregistered(struct v4l2_subdev *sd)
+{
+	struct s5k5baf *state = to_s5k5baf(sd);
+	v4l2_device_unregister_subdev(&state->cis_sd);
+}
+
+static const struct v4l2_subdev_ops s5k5baf_cis_subdev_ops = {
+	.pad	= &s5k5baf_cis_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops s5k5baf_cis_subdev_internal_ops = {
+	.open = s5k5baf_open,
+};
+
+static const struct v4l2_subdev_internal_ops s5k5baf_subdev_internal_ops = {
+	.registered = s5k5baf_registered,
+	.unregistered = s5k5baf_unregistered,
+	.open = s5k5baf_open,
+};
+
+static const struct v4l2_subdev_core_ops s5k5baf_core_ops = {
+	.s_power = s5k5baf_set_power,
+	.log_status = v4l2_ctrl_subdev_log_status,
+};
+
+static const struct v4l2_subdev_ops s5k5baf_subdev_ops = {
+	.core = &s5k5baf_core_ops,
+	.pad = &s5k5baf_pad_ops,
+	.video = &s5k5baf_video_ops,
+};
+
+static int s5k5baf_configure_gpios(struct s5k5baf *state)
+{
+	static const char * const name[] = { "stbyn", "rstn" };
+	static const char * const label[] = { "S5K5BAF_STBY", "S5K5BAF_RST" };
+	struct i2c_client *c = v4l2_get_subdevdata(&state->sd);
+	struct gpio_desc *gpio;
+	int ret, i;
+
+	for (i = 0; i < NUM_GPIOS; ++i) {
+		gpio = devm_gpiod_get(&c->dev, name[i], GPIOD_OUT_HIGH);
+		ret = PTR_ERR_OR_ZERO(gpio);
+		if (ret) {
+			v4l2_err(c, "failed to request gpio %s: %d\n",
+				 name[i], ret);
+			return ret;
+		}
+
+		ret = gpiod_set_consumer_name(gpio, label[i]);
+		if (ret) {
+			v4l2_err(c, "failed to set up name for gpio %s: %d\n",
+				 name[i], ret);
+			return ret;
+		}
+
+		state->gpios[i] = gpio;
+	}
+	return 0;
+}
+
+static int s5k5baf_parse_device_node(struct s5k5baf *state, struct device *dev)
+{
+	struct device_node *node = dev->of_node;
+	struct device_node *node_ep;
+	struct v4l2_fwnode_endpoint ep = { .bus_type = 0 };
+	int ret;
+
+	if (!node) {
+		dev_err(dev, "no device-tree node provided\n");
+		return -EINVAL;
+	}
+
+	ret = of_property_read_u32(node, "clock-frequency",
+				   &state->mclk_frequency);
+	if (ret < 0) {
+		state->mclk_frequency = S5K5BAF_DEFAULT_MCLK_FREQ;
+		dev_info(dev, "using default %u Hz clock frequency\n",
+			 state->mclk_frequency);
+	}
+
+	node_ep = of_graph_get_next_endpoint(node, NULL);
+	if (!node_ep) {
+		dev_err(dev, "no endpoint defined at node %pOF\n", node);
+		return -EINVAL;
+	}
+
+	ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(node_ep), &ep);
+	of_node_put(node_ep);
+	if (ret)
+		return ret;
+
+	state->bus_type = ep.bus_type;
+
+	switch (state->bus_type) {
+	case V4L2_MBUS_CSI2_DPHY:
+		state->nlanes = ep.bus.mipi_csi2.num_data_lanes;
+		break;
+	case V4L2_MBUS_PARALLEL:
+		break;
+	default:
+		dev_err(dev, "unsupported bus in endpoint defined at node %pOF\n",
+			node);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int s5k5baf_configure_subdevs(struct s5k5baf *state,
+				     struct i2c_client *c)
+{
+	struct v4l2_subdev *sd;
+	int ret;
+
+	sd = &state->cis_sd;
+	v4l2_subdev_init(sd, &s5k5baf_cis_subdev_ops);
+	sd->owner = THIS_MODULE;
+	v4l2_set_subdevdata(sd, state);
+	snprintf(sd->name, sizeof(sd->name), "S5K5BAF-CIS %d-%04x",
+		 i2c_adapter_id(c->adapter), c->addr);
+
+	sd->internal_ops = &s5k5baf_cis_subdev_internal_ops;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	state->cis_pad.flags = MEDIA_PAD_FL_SOURCE;
+	sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret = media_entity_pads_init(&sd->entity, NUM_CIS_PADS, &state->cis_pad);
+	if (ret < 0)
+		goto err;
+
+	sd = &state->sd;
+	v4l2_i2c_subdev_init(sd, c, &s5k5baf_subdev_ops);
+	snprintf(sd->name, sizeof(sd->name), "S5K5BAF-ISP %d-%04x",
+		 i2c_adapter_id(c->adapter), c->addr);
+
+	sd->internal_ops = &s5k5baf_subdev_internal_ops;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	state->pads[PAD_CIS].flags = MEDIA_PAD_FL_SINK;
+	state->pads[PAD_OUT].flags = MEDIA_PAD_FL_SOURCE;
+	sd->entity.function = MEDIA_ENT_F_V4L2_SUBDEV_UNKNOWN;
+	ret = media_entity_pads_init(&sd->entity, NUM_ISP_PADS, state->pads);
+
+	if (!ret)
+		return 0;
+
+	media_entity_cleanup(&state->cis_sd.entity);
+err:
+	dev_err(&c->dev, "cannot init media entity %s\n", sd->name);
+	return ret;
+}
+
+static int s5k5baf_configure_regulators(struct s5k5baf *state)
+{
+	struct i2c_client *c = v4l2_get_subdevdata(&state->sd);
+	int ret;
+	int i;
+
+	for (i = 0; i < S5K5BAF_NUM_SUPPLIES; i++)
+		state->supplies[i].supply = s5k5baf_supply_names[i];
+
+	ret = devm_regulator_bulk_get(&c->dev, S5K5BAF_NUM_SUPPLIES,
+				      state->supplies);
+	if (ret < 0)
+		v4l2_err(c, "failed to get regulators\n");
+	return ret;
+}
+
+static int s5k5baf_probe(struct i2c_client *c)
+{
+	struct s5k5baf *state;
+	int ret;
+
+	state = devm_kzalloc(&c->dev, sizeof(*state), GFP_KERNEL);
+	if (!state)
+		return -ENOMEM;
+
+	mutex_init(&state->lock);
+	state->crop_sink = s5k5baf_cis_rect;
+	state->compose = s5k5baf_cis_rect;
+	state->crop_source = s5k5baf_cis_rect;
+
+	ret = s5k5baf_parse_device_node(state, &c->dev);
+	if (ret < 0)
+		return ret;
+
+	ret = s5k5baf_configure_subdevs(state, c);
+	if (ret < 0)
+		return ret;
+
+	ret = s5k5baf_configure_gpios(state);
+	if (ret < 0)
+		goto err_me;
+
+	ret = s5k5baf_configure_regulators(state);
+	if (ret < 0)
+		goto err_me;
+
+	state->clock = devm_clk_get(state->sd.dev, S5K5BAF_CLK_NAME);
+	if (IS_ERR(state->clock)) {
+		ret = -EPROBE_DEFER;
+		goto err_me;
+	}
+
+	ret = s5k5baf_power_on(state);
+	if (ret < 0) {
+		ret = -EPROBE_DEFER;
+		goto err_me;
+	}
+	s5k5baf_hw_init(state);
+	ret = s5k5baf_check_fw_revision(state);
+
+	s5k5baf_power_off(state);
+	if (ret < 0)
+		goto err_me;
+
+	ret = s5k5baf_initialize_ctrls(state);
+	if (ret < 0)
+		goto err_me;
+
+	ret = v4l2_async_register_subdev(&state->sd);
+	if (ret < 0)
+		goto err_ctrl;
+
+	return 0;
+
+err_ctrl:
+	v4l2_ctrl_handler_free(state->sd.ctrl_handler);
+err_me:
+	media_entity_cleanup(&state->sd.entity);
+	media_entity_cleanup(&state->cis_sd.entity);
+	return ret;
+}
+
+static void s5k5baf_remove(struct i2c_client *c)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(c);
+	struct s5k5baf *state = to_s5k5baf(sd);
+
+	v4l2_async_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+	media_entity_cleanup(&sd->entity);
+
+	sd = &state->cis_sd;
+	v4l2_device_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+}
+
+static const struct i2c_device_id s5k5baf_id[] = {
+	{ S5K5BAF_DRIVER_NAME, 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(i2c, s5k5baf_id);
+
+static const struct of_device_id s5k5baf_of_match[] = {
+	{ .compatible = "samsung,s5k5baf" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, s5k5baf_of_match);
+
+static struct i2c_driver s5k5baf_i2c_driver = {
+	.driver = {
+		.of_match_table = s5k5baf_of_match,
+		.name = S5K5BAF_DRIVER_NAME
+	},
+	.probe		= s5k5baf_probe,
+	.remove		= s5k5baf_remove,
+	.id_table	= s5k5baf_id,
+};
+
+module_i2c_driver(s5k5baf_i2c_driver);
+
+MODULE_DESCRIPTION("Samsung S5K5BAF(X) UXGA camera driver");
+MODULE_AUTHOR("Andrzej Hajda <a.hajda@samsung.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/s5k6a3.c b/drivers/media/i2c/s5k6a3.c
new file mode 100644
index 000000000..b3560c8f8
--- /dev/null
+++ b/drivers/media/i2c/s5k6a3.c
@@ -0,0 +1,385 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Samsung S5K6A3 image sensor driver
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ * Author: Sylwester Nawrocki <s.nawrocki@samsung.com>
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/errno.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-subdev.h>
+
+#define S5K6A3_SENSOR_MAX_WIDTH		1412
+#define S5K6A3_SENSOR_MAX_HEIGHT	1412
+#define S5K6A3_SENSOR_MIN_WIDTH		32
+#define S5K6A3_SENSOR_MIN_HEIGHT	32
+
+#define S5K6A3_DEFAULT_WIDTH		1296
+#define S5K6A3_DEFAULT_HEIGHT		732
+
+#define S5K6A3_DRV_NAME			"S5K6A3"
+#define S5K6A3_CLK_NAME			"extclk"
+#define S5K6A3_DEFAULT_CLK_FREQ		24000000U
+
+enum {
+	S5K6A3_SUPP_VDDA,
+	S5K6A3_SUPP_VDDIO,
+	S5K6A3_SUPP_AFVDD,
+	S5K6A3_NUM_SUPPLIES,
+};
+
+/**
+ * struct s5k6a3 - fimc-is sensor data structure
+ * @dev: pointer to this I2C client device structure
+ * @subdev: the image sensor's v4l2 subdev
+ * @pad: subdev media source pad
+ * @supplies: image sensor's voltage regulator supplies
+ * @gpio_reset: GPIO connected to the sensor's reset pin
+ * @lock: mutex protecting the structure's members below
+ * @format: media bus format at the sensor's source pad
+ * @clock: pointer to &struct clk.
+ * @clock_frequency: clock frequency
+ * @power_count: stores state if device is powered
+ */
+struct s5k6a3 {
+	struct device *dev;
+	struct v4l2_subdev subdev;
+	struct media_pad pad;
+	struct regulator_bulk_data supplies[S5K6A3_NUM_SUPPLIES];
+	struct gpio_desc *gpio_reset;
+	struct mutex lock;
+	struct v4l2_mbus_framefmt format;
+	struct clk *clock;
+	u32 clock_frequency;
+	int power_count;
+};
+
+static const char * const s5k6a3_supply_names[] = {
+	[S5K6A3_SUPP_VDDA]	= "svdda",
+	[S5K6A3_SUPP_VDDIO]	= "svddio",
+	[S5K6A3_SUPP_AFVDD]	= "afvdd",
+};
+
+static inline struct s5k6a3 *sd_to_s5k6a3(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct s5k6a3, subdev);
+}
+
+static const struct v4l2_mbus_framefmt s5k6a3_formats[] = {
+	{
+		.code = MEDIA_BUS_FMT_SGRBG10_1X10,
+		.colorspace = V4L2_COLORSPACE_SRGB,
+		.field = V4L2_FIELD_NONE,
+	}
+};
+
+static const struct v4l2_mbus_framefmt *find_sensor_format(
+	struct v4l2_mbus_framefmt *mf)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(s5k6a3_formats); i++)
+		if (mf->code == s5k6a3_formats[i].code)
+			return &s5k6a3_formats[i];
+
+	return &s5k6a3_formats[0];
+}
+
+static int s5k6a3_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index >= ARRAY_SIZE(s5k6a3_formats))
+		return -EINVAL;
+
+	code->code = s5k6a3_formats[code->index].code;
+	return 0;
+}
+
+static void s5k6a3_try_format(struct v4l2_mbus_framefmt *mf)
+{
+	const struct v4l2_mbus_framefmt *fmt;
+
+	fmt = find_sensor_format(mf);
+	mf->code = fmt->code;
+	mf->field = V4L2_FIELD_NONE;
+	v4l_bound_align_image(&mf->width, S5K6A3_SENSOR_MIN_WIDTH,
+			      S5K6A3_SENSOR_MAX_WIDTH, 0,
+			      &mf->height, S5K6A3_SENSOR_MIN_HEIGHT,
+			      S5K6A3_SENSOR_MAX_HEIGHT, 0, 0);
+}
+
+static struct v4l2_mbus_framefmt *__s5k6a3_get_format(
+		struct s5k6a3 *sensor, struct v4l2_subdev_state *sd_state,
+		u32 pad, enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return sd_state ? v4l2_subdev_get_try_format(&sensor->subdev,
+							     sd_state, pad) : NULL;
+
+	return &sensor->format;
+}
+
+static int s5k6a3_set_fmt(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_format *fmt)
+{
+	struct s5k6a3 *sensor = sd_to_s5k6a3(sd);
+	struct v4l2_mbus_framefmt *mf;
+
+	s5k6a3_try_format(&fmt->format);
+
+	mf = __s5k6a3_get_format(sensor, sd_state, fmt->pad, fmt->which);
+	if (mf) {
+		mutex_lock(&sensor->lock);
+		*mf = fmt->format;
+		mutex_unlock(&sensor->lock);
+	}
+	return 0;
+}
+
+static int s5k6a3_get_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct s5k6a3 *sensor = sd_to_s5k6a3(sd);
+	struct v4l2_mbus_framefmt *mf;
+
+	mf = __s5k6a3_get_format(sensor, sd_state, fmt->pad, fmt->which);
+
+	mutex_lock(&sensor->lock);
+	fmt->format = *mf;
+	mutex_unlock(&sensor->lock);
+	return 0;
+}
+
+static const struct v4l2_subdev_pad_ops s5k6a3_pad_ops = {
+	.enum_mbus_code	= s5k6a3_enum_mbus_code,
+	.get_fmt	= s5k6a3_get_fmt,
+	.set_fmt	= s5k6a3_set_fmt,
+};
+
+static int s5k6a3_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_mbus_framefmt *format = v4l2_subdev_get_try_format(sd,
+								       fh->state,
+								       0);
+
+	*format		= s5k6a3_formats[0];
+	format->width	= S5K6A3_DEFAULT_WIDTH;
+	format->height	= S5K6A3_DEFAULT_HEIGHT;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_internal_ops s5k6a3_sd_internal_ops = {
+	.open = s5k6a3_open,
+};
+
+static int __s5k6a3_power_on(struct s5k6a3 *sensor)
+{
+	int i = S5K6A3_SUPP_VDDA;
+	int ret;
+
+	ret = clk_set_rate(sensor->clock, sensor->clock_frequency);
+	if (ret < 0)
+		return ret;
+
+	ret = pm_runtime_get(sensor->dev);
+	if (ret < 0)
+		goto error_rpm_put;
+
+	ret = regulator_enable(sensor->supplies[i].consumer);
+	if (ret < 0)
+		goto error_rpm_put;
+
+	ret = clk_prepare_enable(sensor->clock);
+	if (ret < 0)
+		goto error_reg_dis;
+
+	for (i++; i < S5K6A3_NUM_SUPPLIES; i++) {
+		ret = regulator_enable(sensor->supplies[i].consumer);
+		if (ret < 0)
+			goto error_clk;
+	}
+
+	gpiod_set_value_cansleep(sensor->gpio_reset, 0);
+	usleep_range(600, 800);
+	gpiod_set_value_cansleep(sensor->gpio_reset, 1);
+	usleep_range(600, 800);
+	gpiod_set_value_cansleep(sensor->gpio_reset, 0);
+
+	/* Delay needed for the sensor initialization */
+	msleep(20);
+	return 0;
+
+error_clk:
+	clk_disable_unprepare(sensor->clock);
+error_reg_dis:
+	for (--i; i >= 0; --i)
+		regulator_disable(sensor->supplies[i].consumer);
+error_rpm_put:
+	pm_runtime_put(sensor->dev);
+	return ret;
+}
+
+static int __s5k6a3_power_off(struct s5k6a3 *sensor)
+{
+	int i;
+
+	gpiod_set_value_cansleep(sensor->gpio_reset, 1);
+
+	for (i = S5K6A3_NUM_SUPPLIES - 1; i >= 0; i--)
+		regulator_disable(sensor->supplies[i].consumer);
+
+	clk_disable_unprepare(sensor->clock);
+	pm_runtime_put(sensor->dev);
+	return 0;
+}
+
+static int s5k6a3_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct s5k6a3 *sensor = sd_to_s5k6a3(sd);
+	int ret = 0;
+
+	mutex_lock(&sensor->lock);
+
+	if (sensor->power_count == !on) {
+		if (on)
+			ret = __s5k6a3_power_on(sensor);
+		else
+			ret = __s5k6a3_power_off(sensor);
+
+		if (ret == 0)
+			sensor->power_count += on ? 1 : -1;
+	}
+
+	mutex_unlock(&sensor->lock);
+	return ret;
+}
+
+static const struct v4l2_subdev_core_ops s5k6a3_core_ops = {
+	.s_power = s5k6a3_s_power,
+};
+
+static const struct v4l2_subdev_ops s5k6a3_subdev_ops = {
+	.core = &s5k6a3_core_ops,
+	.pad = &s5k6a3_pad_ops,
+};
+
+static int s5k6a3_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct s5k6a3 *sensor;
+	struct v4l2_subdev *sd;
+	int i, ret;
+
+	sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
+	if (!sensor)
+		return -ENOMEM;
+
+	mutex_init(&sensor->lock);
+	sensor->dev = dev;
+
+	sensor->clock = devm_clk_get(sensor->dev, S5K6A3_CLK_NAME);
+	if (IS_ERR(sensor->clock))
+		return PTR_ERR(sensor->clock);
+
+	sensor->gpio_reset = devm_gpiod_get(dev, NULL, GPIOD_OUT_HIGH);
+	ret = PTR_ERR_OR_ZERO(sensor->gpio_reset);
+	if (ret)
+		return ret;
+
+	if (of_property_read_u32(dev->of_node, "clock-frequency",
+				 &sensor->clock_frequency)) {
+		sensor->clock_frequency = S5K6A3_DEFAULT_CLK_FREQ;
+		dev_info(dev, "using default %u Hz clock frequency\n",
+					sensor->clock_frequency);
+	}
+
+	for (i = 0; i < S5K6A3_NUM_SUPPLIES; i++)
+		sensor->supplies[i].supply = s5k6a3_supply_names[i];
+
+	ret = devm_regulator_bulk_get(&client->dev, S5K6A3_NUM_SUPPLIES,
+				      sensor->supplies);
+	if (ret < 0)
+		return ret;
+
+	sd = &sensor->subdev;
+	v4l2_i2c_subdev_init(sd, client, &s5k6a3_subdev_ops);
+	sensor->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	sd->internal_ops = &s5k6a3_sd_internal_ops;
+
+	sensor->format.code = s5k6a3_formats[0].code;
+	sensor->format.width = S5K6A3_DEFAULT_WIDTH;
+	sensor->format.height = S5K6A3_DEFAULT_HEIGHT;
+
+	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)
+		return ret;
+
+	pm_runtime_no_callbacks(dev);
+	pm_runtime_enable(dev);
+
+	ret = v4l2_async_register_subdev(sd);
+
+	if (ret < 0) {
+		pm_runtime_disable(&client->dev);
+		media_entity_cleanup(&sd->entity);
+	}
+
+	return ret;
+}
+
+static void s5k6a3_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	pm_runtime_disable(&client->dev);
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+}
+
+static const struct i2c_device_id s5k6a3_ids[] = {
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, s5k6a3_ids);
+
+#ifdef CONFIG_OF
+static const struct of_device_id s5k6a3_of_match[] = {
+	{ .compatible = "samsung,s5k6a3" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, s5k6a3_of_match);
+#endif
+
+static struct i2c_driver s5k6a3_driver = {
+	.driver = {
+		.of_match_table	= of_match_ptr(s5k6a3_of_match),
+		.name		= S5K6A3_DRV_NAME,
+	},
+	.probe		= s5k6a3_probe,
+	.remove		= s5k6a3_remove,
+	.id_table	= s5k6a3_ids,
+};
+
+module_i2c_driver(s5k6a3_driver);
+
+MODULE_DESCRIPTION("S5K6A3 image sensor subdev driver");
+MODULE_AUTHOR("Sylwester Nawrocki <s.nawrocki@samsung.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/saa6588.c b/drivers/media/i2c/saa6588.c
new file mode 100644
index 000000000..dea9fc093
--- /dev/null
+++ b/drivers/media/i2c/saa6588.c
@@ -0,0 +1,513 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+    Driver for SAA6588 RDS decoder
+
+    (c) 2005 Hans J. Koch
+
+*/
+
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/i2c.h>
+#include <linux/types.h>
+#include <linux/videodev2.h>
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/poll.h>
+#include <linux/wait.h>
+#include <linux/uaccess.h>
+
+#include <media/i2c/saa6588.h>
+#include <media/v4l2-device.h>
+
+
+/* insmod options */
+static unsigned int debug;
+static unsigned int xtal;
+static unsigned int mmbs;
+static unsigned int plvl;
+static unsigned int bufblocks = 100;
+
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "enable debug messages");
+module_param(xtal, int, 0);
+MODULE_PARM_DESC(xtal, "select oscillator frequency (0..3), default 0");
+module_param(mmbs, int, 0);
+MODULE_PARM_DESC(mmbs, "enable MMBS mode: 0=off (default), 1=on");
+module_param(plvl, int, 0);
+MODULE_PARM_DESC(plvl, "select pause level (0..3), default 0");
+module_param(bufblocks, int, 0);
+MODULE_PARM_DESC(bufblocks, "number of buffered blocks, default 100");
+
+MODULE_DESCRIPTION("v4l2 driver module for SAA6588 RDS decoder");
+MODULE_AUTHOR("Hans J. Koch <koch@hjk-az.de>");
+
+MODULE_LICENSE("GPL");
+
+/* ---------------------------------------------------------------------- */
+
+#define UNSET       (-1U)
+#define PREFIX      "saa6588: "
+#define dprintk     if (debug) printk
+
+struct saa6588 {
+	struct v4l2_subdev sd;
+	struct delayed_work work;
+	spinlock_t lock;
+	unsigned char *buffer;
+	unsigned int buf_size;
+	unsigned int rd_index;
+	unsigned int wr_index;
+	unsigned int block_count;
+	unsigned char last_blocknum;
+	wait_queue_head_t read_queue;
+	int data_available_for_read;
+	u8 sync;
+};
+
+static inline struct saa6588 *to_saa6588(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct saa6588, sd);
+}
+
+/* ---------------------------------------------------------------------- */
+
+/*
+ * SAA6588 defines
+ */
+
+/* Initialization and mode control byte (0w) */
+
+/* bit 0+1 (DAC0/DAC1) */
+#define cModeStandard           0x00
+#define cModeFastPI             0x01
+#define cModeReducedRequest     0x02
+#define cModeInvalid            0x03
+
+/* bit 2 (RBDS) */
+#define cProcessingModeRDS      0x00
+#define cProcessingModeRBDS     0x04
+
+/* bit 3+4 (SYM0/SYM1) */
+#define cErrCorrectionNone      0x00
+#define cErrCorrection2Bits     0x08
+#define cErrCorrection5Bits     0x10
+#define cErrCorrectionNoneRBDS  0x18
+
+/* bit 5 (NWSY) */
+#define cSyncNormal             0x00
+#define cSyncRestart            0x20
+
+/* bit 6 (TSQD) */
+#define cSigQualityDetectOFF    0x00
+#define cSigQualityDetectON     0x40
+
+/* bit 7 (SQCM) */
+#define cSigQualityTriggered    0x00
+#define cSigQualityContinous    0x80
+
+/* Pause level and flywheel control byte (1w) */
+
+/* bits 0..5 (FEB0..FEB5) */
+#define cFlywheelMaxBlocksMask  0x3F
+#define cFlywheelDefault        0x20
+
+/* bits 6+7 (PL0/PL1) */
+#define cPauseLevel_11mV	0x00
+#define cPauseLevel_17mV        0x40
+#define cPauseLevel_27mV        0x80
+#define cPauseLevel_43mV        0xC0
+
+/* Pause time/oscillator frequency/quality detector control byte (1w) */
+
+/* bits 0..4 (SQS0..SQS4) */
+#define cQualityDetectSensMask  0x1F
+#define cQualityDetectDefault   0x0F
+
+/* bit 5 (SOSC) */
+#define cSelectOscFreqOFF	0x00
+#define cSelectOscFreqON	0x20
+
+/* bit 6+7 (PTF0/PTF1) */
+#define cOscFreq_4332kHz	0x00
+#define cOscFreq_8664kHz	0x40
+#define cOscFreq_12996kHz	0x80
+#define cOscFreq_17328kHz	0xC0
+
+/* ---------------------------------------------------------------------- */
+
+static bool block_from_buf(struct saa6588 *s, unsigned char *buf)
+{
+	int i;
+
+	if (s->rd_index == s->wr_index) {
+		if (debug > 2)
+			dprintk(PREFIX "Read: buffer empty.\n");
+		return false;
+	}
+
+	if (debug > 2) {
+		dprintk(PREFIX "Read: ");
+		for (i = s->rd_index; i < s->rd_index + 3; i++)
+			dprintk("0x%02x ", s->buffer[i]);
+	}
+
+	memcpy(buf, &s->buffer[s->rd_index], 3);
+
+	s->rd_index += 3;
+	if (s->rd_index >= s->buf_size)
+		s->rd_index = 0;
+	s->block_count--;
+
+	if (debug > 2)
+		dprintk("%d blocks total.\n", s->block_count);
+
+	return true;
+}
+
+static void read_from_buf(struct saa6588 *s, struct saa6588_command *a)
+{
+	unsigned char __user *buf_ptr = a->buffer;
+	unsigned char buf[3];
+	unsigned long flags;
+	unsigned int rd_blocks;
+	unsigned int i;
+
+	a->result = 0;
+	if (!a->buffer)
+		return;
+
+	while (!a->nonblocking && !s->data_available_for_read) {
+		int ret = wait_event_interruptible(s->read_queue,
+					     s->data_available_for_read);
+		if (ret == -ERESTARTSYS) {
+			a->result = -EINTR;
+			return;
+		}
+	}
+
+	rd_blocks = a->block_count;
+	spin_lock_irqsave(&s->lock, flags);
+	if (rd_blocks > s->block_count)
+		rd_blocks = s->block_count;
+	spin_unlock_irqrestore(&s->lock, flags);
+
+	if (!rd_blocks)
+		return;
+
+	for (i = 0; i < rd_blocks; i++) {
+		bool got_block;
+
+		spin_lock_irqsave(&s->lock, flags);
+		got_block = block_from_buf(s, buf);
+		spin_unlock_irqrestore(&s->lock, flags);
+		if (!got_block)
+			break;
+		if (copy_to_user(buf_ptr, buf, 3)) {
+			a->result = -EFAULT;
+			return;
+		}
+		buf_ptr += 3;
+		a->result += 3;
+	}
+	spin_lock_irqsave(&s->lock, flags);
+	s->data_available_for_read = (s->block_count > 0);
+	spin_unlock_irqrestore(&s->lock, flags);
+}
+
+static void block_to_buf(struct saa6588 *s, unsigned char *blockbuf)
+{
+	unsigned int i;
+
+	if (debug > 3)
+		dprintk(PREFIX "New block: ");
+
+	for (i = 0; i < 3; ++i) {
+		if (debug > 3)
+			dprintk("0x%02x ", blockbuf[i]);
+		s->buffer[s->wr_index] = blockbuf[i];
+		s->wr_index++;
+	}
+
+	if (s->wr_index >= s->buf_size)
+		s->wr_index = 0;
+
+	if (s->wr_index == s->rd_index) {
+		s->rd_index += 3;
+		if (s->rd_index >= s->buf_size)
+			s->rd_index = 0;
+	} else
+		s->block_count++;
+
+	if (debug > 3)
+		dprintk("%d blocks total.\n", s->block_count);
+}
+
+static void saa6588_i2c_poll(struct saa6588 *s)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&s->sd);
+	unsigned long flags;
+	unsigned char tmpbuf[6];
+	unsigned char blocknum;
+	unsigned char tmp;
+
+	/* Although we only need 3 bytes, we have to read at least 6.
+	   SAA6588 returns garbage otherwise. */
+	if (6 != i2c_master_recv(client, &tmpbuf[0], 6)) {
+		if (debug > 1)
+			dprintk(PREFIX "read error!\n");
+		return;
+	}
+
+	s->sync = tmpbuf[0] & 0x10;
+	if (!s->sync)
+		return;
+	blocknum = tmpbuf[0] >> 5;
+	if (blocknum == s->last_blocknum) {
+		if (debug > 3)
+			dprintk("Saw block %d again.\n", blocknum);
+		return;
+	}
+
+	s->last_blocknum = blocknum;
+
+	/*
+	   Byte order according to v4l2 specification:
+
+	   Byte 0: Least Significant Byte of RDS Block
+	   Byte 1: Most Significant Byte of RDS Block
+	   Byte 2 Bit 7: Error bit. Indicates that an uncorrectable error
+	   occurred during reception of this block.
+	   Bit 6: Corrected bit. Indicates that an error was
+	   corrected for this data block.
+	   Bits 5-3: Same as bits 0-2.
+	   Bits 2-0: Block number.
+
+	   SAA6588 byte order is Status-MSB-LSB, so we have to swap the
+	   first and the last of the 3 bytes block.
+	 */
+
+	swap(tmpbuf[2], tmpbuf[0]);
+
+	/* Map 'Invalid block E' to 'Invalid Block' */
+	if (blocknum == 6)
+		blocknum = V4L2_RDS_BLOCK_INVALID;
+	/* And if are not in mmbs mode, then 'Block E' is also mapped
+	   to 'Invalid Block'. As far as I can tell MMBS is discontinued,
+	   and if there is ever a need to support E blocks, then please
+	   contact the linux-media mailinglist. */
+	else if (!mmbs && blocknum == 5)
+		blocknum = V4L2_RDS_BLOCK_INVALID;
+	tmp = blocknum;
+	tmp |= blocknum << 3;	/* Received offset == Offset Name (OK ?) */
+	if ((tmpbuf[2] & 0x03) == 0x03)
+		tmp |= V4L2_RDS_BLOCK_ERROR;	 /* uncorrectable error */
+	else if ((tmpbuf[2] & 0x03) != 0x00)
+		tmp |= V4L2_RDS_BLOCK_CORRECTED; /* corrected error */
+	tmpbuf[2] = tmp;	/* Is this enough ? Should we also check other bits ? */
+
+	spin_lock_irqsave(&s->lock, flags);
+	block_to_buf(s, tmpbuf);
+	spin_unlock_irqrestore(&s->lock, flags);
+	s->data_available_for_read = 1;
+	wake_up_interruptible(&s->read_queue);
+}
+
+static void saa6588_work(struct work_struct *work)
+{
+	struct saa6588 *s = container_of(work, struct saa6588, work.work);
+
+	saa6588_i2c_poll(s);
+	schedule_delayed_work(&s->work, msecs_to_jiffies(20));
+}
+
+static void saa6588_configure(struct saa6588 *s)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&s->sd);
+	unsigned char buf[3];
+	int rc;
+
+	buf[0] = cSyncRestart;
+	if (mmbs)
+		buf[0] |= cProcessingModeRBDS;
+
+	buf[1] = cFlywheelDefault;
+	switch (plvl) {
+	case 0:
+		buf[1] |= cPauseLevel_11mV;
+		break;
+	case 1:
+		buf[1] |= cPauseLevel_17mV;
+		break;
+	case 2:
+		buf[1] |= cPauseLevel_27mV;
+		break;
+	case 3:
+		buf[1] |= cPauseLevel_43mV;
+		break;
+	default:		/* nothing */
+		break;
+	}
+
+	buf[2] = cQualityDetectDefault | cSelectOscFreqON;
+
+	switch (xtal) {
+	case 0:
+		buf[2] |= cOscFreq_4332kHz;
+		break;
+	case 1:
+		buf[2] |= cOscFreq_8664kHz;
+		break;
+	case 2:
+		buf[2] |= cOscFreq_12996kHz;
+		break;
+	case 3:
+		buf[2] |= cOscFreq_17328kHz;
+		break;
+	default:		/* nothing */
+		break;
+	}
+
+	dprintk(PREFIX "writing: 0w=0x%02x 1w=0x%02x 2w=0x%02x\n",
+		buf[0], buf[1], buf[2]);
+
+	rc = i2c_master_send(client, buf, 3);
+	if (rc != 3)
+		printk(PREFIX "i2c i/o error: rc == %d (should be 3)\n", rc);
+}
+
+/* ---------------------------------------------------------------------- */
+
+static long saa6588_command(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
+{
+	struct saa6588 *s = to_saa6588(sd);
+	struct saa6588_command *a = arg;
+
+	switch (cmd) {
+		/* --- close() for /dev/radio --- */
+	case SAA6588_CMD_CLOSE:
+		s->data_available_for_read = 1;
+		wake_up_interruptible(&s->read_queue);
+		s->data_available_for_read = 0;
+		a->result = 0;
+		break;
+		/* --- read() for /dev/radio --- */
+	case SAA6588_CMD_READ:
+		read_from_buf(s, a);
+		break;
+		/* --- poll() for /dev/radio --- */
+	case SAA6588_CMD_POLL:
+		a->poll_mask = 0;
+		if (s->data_available_for_read)
+			a->poll_mask |= EPOLLIN | EPOLLRDNORM;
+		poll_wait(a->instance, &s->read_queue, a->event_list);
+		break;
+
+	default:
+		/* nothing */
+		return -ENOIOCTLCMD;
+	}
+	return 0;
+}
+
+static int saa6588_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
+{
+	struct saa6588 *s = to_saa6588(sd);
+
+	vt->capability |= V4L2_TUNER_CAP_RDS | V4L2_TUNER_CAP_RDS_BLOCK_IO;
+	if (s->sync)
+		vt->rxsubchans |= V4L2_TUNER_SUB_RDS;
+	return 0;
+}
+
+static int saa6588_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *vt)
+{
+	struct saa6588 *s = to_saa6588(sd);
+
+	saa6588_configure(s);
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_core_ops saa6588_core_ops = {
+	.command = saa6588_command,
+};
+
+static const struct v4l2_subdev_tuner_ops saa6588_tuner_ops = {
+	.g_tuner = saa6588_g_tuner,
+	.s_tuner = saa6588_s_tuner,
+};
+
+static const struct v4l2_subdev_ops saa6588_ops = {
+	.core = &saa6588_core_ops,
+	.tuner = &saa6588_tuner_ops,
+};
+
+/* ---------------------------------------------------------------------- */
+
+static int saa6588_probe(struct i2c_client *client)
+{
+	struct saa6588 *s;
+	struct v4l2_subdev *sd;
+
+	v4l_info(client, "saa6588 found @ 0x%x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	s = devm_kzalloc(&client->dev, sizeof(*s), GFP_KERNEL);
+	if (s == NULL)
+		return -ENOMEM;
+
+	s->buf_size = bufblocks * 3;
+
+	s->buffer = devm_kzalloc(&client->dev, s->buf_size, GFP_KERNEL);
+	if (s->buffer == NULL)
+		return -ENOMEM;
+	sd = &s->sd;
+	v4l2_i2c_subdev_init(sd, client, &saa6588_ops);
+	spin_lock_init(&s->lock);
+	s->block_count = 0;
+	s->wr_index = 0;
+	s->rd_index = 0;
+	s->last_blocknum = 0xff;
+	init_waitqueue_head(&s->read_queue);
+	s->data_available_for_read = 0;
+
+	saa6588_configure(s);
+
+	/* start polling via eventd */
+	INIT_DELAYED_WORK(&s->work, saa6588_work);
+	schedule_delayed_work(&s->work, 0);
+	return 0;
+}
+
+static void saa6588_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct saa6588 *s = to_saa6588(sd);
+
+	v4l2_device_unregister_subdev(sd);
+
+	cancel_delayed_work_sync(&s->work);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id saa6588_id[] = {
+	{ "saa6588", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, saa6588_id);
+
+static struct i2c_driver saa6588_driver = {
+	.driver = {
+		.name	= "saa6588",
+	},
+	.probe		= saa6588_probe,
+	.remove		= saa6588_remove,
+	.id_table	= saa6588_id,
+};
+
+module_i2c_driver(saa6588_driver);
diff --git a/drivers/media/i2c/saa6752hs.c b/drivers/media/i2c/saa6752hs.c
new file mode 100644
index 000000000..c106e7a7d
--- /dev/null
+++ b/drivers/media/i2c/saa6752hs.c
@@ -0,0 +1,789 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+ /*
+    saa6752hs - i2c-driver for the saa6752hs by Philips
+
+    Copyright (C) 2004 Andrew de Quincey
+
+    AC-3 support:
+
+    Copyright (C) 2008 Hans Verkuil <hverkuil@xs4all.nl>
+
+  */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/string.h>
+#include <linux/timer.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/poll.h>
+#include <linux/i2c.h>
+#include <linux/types.h>
+#include <linux/videodev2.h>
+#include <linux/init.h>
+#include <linux/crc32.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-common.h>
+
+#define MPEG_VIDEO_TARGET_BITRATE_MAX  27000
+#define MPEG_VIDEO_MAX_BITRATE_MAX     27000
+#define MPEG_TOTAL_TARGET_BITRATE_MAX  27000
+#define MPEG_PID_MAX ((1 << 14) - 1)
+
+
+MODULE_DESCRIPTION("device driver for saa6752hs MPEG2 encoder");
+MODULE_AUTHOR("Andrew de Quincey");
+MODULE_LICENSE("GPL");
+
+enum saa6752hs_videoformat {
+	SAA6752HS_VF_D1 = 0,    /* standard D1 video format: 720x576 */
+	SAA6752HS_VF_2_3_D1 = 1,/* 2/3D1 video format: 480x576 */
+	SAA6752HS_VF_1_2_D1 = 2,/* 1/2D1 video format: 352x576 */
+	SAA6752HS_VF_SIF = 3,   /* SIF video format: 352x288 */
+	SAA6752HS_VF_UNKNOWN,
+};
+
+struct saa6752hs_mpeg_params {
+	/* transport streams */
+	__u16				ts_pid_pmt;
+	__u16				ts_pid_audio;
+	__u16				ts_pid_video;
+	__u16				ts_pid_pcr;
+
+	/* audio */
+	enum v4l2_mpeg_audio_encoding    au_encoding;
+	enum v4l2_mpeg_audio_l2_bitrate  au_l2_bitrate;
+	enum v4l2_mpeg_audio_ac3_bitrate au_ac3_bitrate;
+
+	/* video */
+	enum v4l2_mpeg_video_aspect	vi_aspect;
+	enum v4l2_mpeg_video_bitrate_mode vi_bitrate_mode;
+	__u32				vi_bitrate;
+	__u32				vi_bitrate_peak;
+};
+
+static const struct v4l2_format v4l2_format_table[] =
+{
+	[SAA6752HS_VF_D1] =
+		{ .fmt = { .pix = { .width = 720, .height = 576 }}},
+	[SAA6752HS_VF_2_3_D1] =
+		{ .fmt = { .pix = { .width = 480, .height = 576 }}},
+	[SAA6752HS_VF_1_2_D1] =
+		{ .fmt = { .pix = { .width = 352, .height = 576 }}},
+	[SAA6752HS_VF_SIF] =
+		{ .fmt = { .pix = { .width = 352, .height = 288 }}},
+	[SAA6752HS_VF_UNKNOWN] =
+		{ .fmt = { .pix = { .width = 0, .height = 0}}},
+};
+
+struct saa6752hs_state {
+	struct v4l2_subdev            sd;
+	struct v4l2_ctrl_handler      hdl;
+	struct { /* video bitrate mode control cluster */
+		struct v4l2_ctrl *video_bitrate_mode;
+		struct v4l2_ctrl *video_bitrate;
+		struct v4l2_ctrl *video_bitrate_peak;
+	};
+	u32			      revision;
+	int			      has_ac3;
+	struct saa6752hs_mpeg_params  params;
+	enum saa6752hs_videoformat    video_format;
+	v4l2_std_id                   standard;
+};
+
+enum saa6752hs_command {
+	SAA6752HS_COMMAND_RESET = 0,
+	SAA6752HS_COMMAND_STOP = 1,
+	SAA6752HS_COMMAND_START = 2,
+	SAA6752HS_COMMAND_PAUSE = 3,
+	SAA6752HS_COMMAND_RECONFIGURE = 4,
+	SAA6752HS_COMMAND_SLEEP = 5,
+	SAA6752HS_COMMAND_RECONFIGURE_FORCE = 6,
+
+	SAA6752HS_COMMAND_MAX
+};
+
+static inline struct saa6752hs_state *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct saa6752hs_state, sd);
+}
+
+/* ---------------------------------------------------------------------- */
+
+static const u8 PAT[] = {
+	0xc2, /* i2c register */
+	0x00, /* table number for encoder */
+
+	0x47, /* sync */
+	0x40, 0x00, /* transport_error_indicator(0), payload_unit_start(1), transport_priority(0), pid(0) */
+	0x10, /* transport_scrambling_control(00), adaptation_field_control(01), continuity_counter(0) */
+
+	0x00, /* PSI pointer to start of table */
+
+	0x00, /* tid(0) */
+	0xb0, 0x0d, /* section_syntax_indicator(1), section_length(13) */
+
+	0x00, 0x01, /* transport_stream_id(1) */
+
+	0xc1, /* version_number(0), current_next_indicator(1) */
+
+	0x00, 0x00, /* section_number(0), last_section_number(0) */
+
+	0x00, 0x01, /* program_number(1) */
+
+	0xe0, 0x00, /* PMT PID */
+
+	0x00, 0x00, 0x00, 0x00 /* CRC32 */
+};
+
+static const u8 PMT[] = {
+	0xc2, /* i2c register */
+	0x01, /* table number for encoder */
+
+	0x47, /* sync */
+	0x40, 0x00, /* transport_error_indicator(0), payload_unit_start(1), transport_priority(0), pid */
+	0x10, /* transport_scrambling_control(00), adaptation_field_control(01), continuity_counter(0) */
+
+	0x00, /* PSI pointer to start of table */
+
+	0x02, /* tid(2) */
+	0xb0, 0x17, /* section_syntax_indicator(1), section_length(23) */
+
+	0x00, 0x01, /* program_number(1) */
+
+	0xc1, /* version_number(0), current_next_indicator(1) */
+
+	0x00, 0x00, /* section_number(0), last_section_number(0) */
+
+	0xe0, 0x00, /* PCR_PID */
+
+	0xf0, 0x00, /* program_info_length(0) */
+
+	0x02, 0xe0, 0x00, 0xf0, 0x00, /* video stream type(2), pid */
+	0x04, 0xe0, 0x00, 0xf0, 0x00, /* audio stream type(4), pid */
+
+	0x00, 0x00, 0x00, 0x00 /* CRC32 */
+};
+
+static const u8 PMT_AC3[] = {
+	0xc2, /* i2c register */
+	0x01, /* table number for encoder(1) */
+	0x47, /* sync */
+
+	0x40, /* transport_error_indicator(0), payload_unit_start(1), transport_priority(0) */
+	0x10, /* PMT PID (0x0010) */
+	0x10, /* transport_scrambling_control(00), adaptation_field_control(01), continuity_counter(0) */
+
+	0x00, /* PSI pointer to start of table */
+
+	0x02, /* TID (2) */
+	0xb0, 0x1a, /* section_syntax_indicator(1), section_length(26) */
+
+	0x00, 0x01, /* program_number(1) */
+
+	0xc1, /* version_number(0), current_next_indicator(1) */
+
+	0x00, 0x00, /* section_number(0), last_section_number(0) */
+
+	0xe1, 0x04, /* PCR_PID (0x0104) */
+
+	0xf0, 0x00, /* program_info_length(0) */
+
+	0x02, 0xe1, 0x00, 0xf0, 0x00, /* video stream type(2), pid */
+	0x06, 0xe1, 0x03, 0xf0, 0x03, /* audio stream type(6), pid */
+	0x6a, /* AC3 */
+	0x01, /* Descriptor_length(1) */
+	0x00, /* component_type_flag(0), bsid_flag(0), mainid_flag(0), asvc_flag(0), reserved flags(0) */
+
+	0xED, 0xDE, 0x2D, 0xF3 /* CRC32 BE */
+};
+
+static const struct saa6752hs_mpeg_params param_defaults =
+{
+	.ts_pid_pmt      = 16,
+	.ts_pid_video    = 260,
+	.ts_pid_audio    = 256,
+	.ts_pid_pcr      = 259,
+
+	.vi_aspect       = V4L2_MPEG_VIDEO_ASPECT_4x3,
+	.vi_bitrate      = 4000,
+	.vi_bitrate_peak = 6000,
+	.vi_bitrate_mode = V4L2_MPEG_VIDEO_BITRATE_MODE_VBR,
+
+	.au_encoding     = V4L2_MPEG_AUDIO_ENCODING_LAYER_2,
+	.au_l2_bitrate   = V4L2_MPEG_AUDIO_L2_BITRATE_256K,
+	.au_ac3_bitrate  = V4L2_MPEG_AUDIO_AC3_BITRATE_256K,
+};
+
+/* ---------------------------------------------------------------------- */
+
+static int saa6752hs_chip_command(struct i2c_client *client,
+				  enum saa6752hs_command command)
+{
+	unsigned char buf[3];
+	unsigned long timeout;
+	int status = 0;
+
+	/* execute the command */
+	switch(command) {
+	case SAA6752HS_COMMAND_RESET:
+		buf[0] = 0x00;
+		break;
+
+	case SAA6752HS_COMMAND_STOP:
+		buf[0] = 0x03;
+		break;
+
+	case SAA6752HS_COMMAND_START:
+		buf[0] = 0x02;
+		break;
+
+	case SAA6752HS_COMMAND_PAUSE:
+		buf[0] = 0x04;
+		break;
+
+	case SAA6752HS_COMMAND_RECONFIGURE:
+		buf[0] = 0x05;
+		break;
+
+	case SAA6752HS_COMMAND_SLEEP:
+		buf[0] = 0x06;
+		break;
+
+	case SAA6752HS_COMMAND_RECONFIGURE_FORCE:
+		buf[0] = 0x07;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	/* set it and wait for it to be so */
+	i2c_master_send(client, buf, 1);
+	timeout = jiffies + HZ * 3;
+	for (;;) {
+		/* get the current status */
+		buf[0] = 0x10;
+		i2c_master_send(client, buf, 1);
+		i2c_master_recv(client, buf, 1);
+
+		if (!(buf[0] & 0x20))
+			break;
+		if (time_after(jiffies,timeout)) {
+			status = -ETIMEDOUT;
+			break;
+		}
+
+		msleep(10);
+	}
+
+	/* delay a bit to let encoder settle */
+	msleep(50);
+
+	return status;
+}
+
+
+static inline void set_reg8(struct i2c_client *client, uint8_t reg, uint8_t val)
+{
+	u8 buf[2];
+
+	buf[0] = reg;
+	buf[1] = val;
+	i2c_master_send(client, buf, 2);
+}
+
+static inline void set_reg16(struct i2c_client *client, uint8_t reg, uint16_t val)
+{
+	u8 buf[3];
+
+	buf[0] = reg;
+	buf[1] = val >> 8;
+	buf[2] = val & 0xff;
+	i2c_master_send(client, buf, 3);
+}
+
+static int saa6752hs_set_bitrate(struct i2c_client *client,
+				 struct saa6752hs_state *h)
+{
+	struct saa6752hs_mpeg_params *params = &h->params;
+	int tot_bitrate;
+	int is_384k;
+
+	/* set the bitrate mode */
+	set_reg8(client, 0x71,
+		params->vi_bitrate_mode != V4L2_MPEG_VIDEO_BITRATE_MODE_VBR);
+
+	/* set the video bitrate */
+	if (params->vi_bitrate_mode == V4L2_MPEG_VIDEO_BITRATE_MODE_VBR) {
+		/* set the target bitrate */
+		set_reg16(client, 0x80, params->vi_bitrate);
+
+		/* set the max bitrate */
+		set_reg16(client, 0x81, params->vi_bitrate_peak);
+		tot_bitrate = params->vi_bitrate_peak;
+	} else {
+		/* set the target bitrate (no max bitrate for CBR) */
+		set_reg16(client, 0x81, params->vi_bitrate);
+		tot_bitrate = params->vi_bitrate;
+	}
+
+	/* set the audio encoding */
+	set_reg8(client, 0x93,
+			params->au_encoding == V4L2_MPEG_AUDIO_ENCODING_AC3);
+
+	/* set the audio bitrate */
+	if (params->au_encoding == V4L2_MPEG_AUDIO_ENCODING_AC3)
+		is_384k = V4L2_MPEG_AUDIO_AC3_BITRATE_384K == params->au_ac3_bitrate;
+	else
+		is_384k = V4L2_MPEG_AUDIO_L2_BITRATE_384K == params->au_l2_bitrate;
+	set_reg8(client, 0x94, is_384k);
+	tot_bitrate += is_384k ? 384 : 256;
+
+	/* Note: the total max bitrate is determined by adding the video and audio
+	   bitrates together and also adding an extra 768kbit/s to stay on the
+	   safe side. If more control should be required, then an extra MPEG control
+	   should be added. */
+	tot_bitrate += 768;
+	if (tot_bitrate > MPEG_TOTAL_TARGET_BITRATE_MAX)
+		tot_bitrate = MPEG_TOTAL_TARGET_BITRATE_MAX;
+
+	/* set the total bitrate */
+	set_reg16(client, 0xb1, tot_bitrate);
+	return 0;
+}
+
+static int saa6752hs_try_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct saa6752hs_state *h =
+		container_of(ctrl->handler, struct saa6752hs_state, hdl);
+
+	switch (ctrl->id) {
+	case V4L2_CID_MPEG_VIDEO_BITRATE_MODE:
+		/* peak bitrate shall be >= normal bitrate */
+		if (ctrl->val == V4L2_MPEG_VIDEO_BITRATE_MODE_VBR &&
+		    h->video_bitrate_peak->val < h->video_bitrate->val)
+			h->video_bitrate_peak->val = h->video_bitrate->val;
+		break;
+	}
+	return 0;
+}
+
+static int saa6752hs_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct saa6752hs_state *h =
+		container_of(ctrl->handler, struct saa6752hs_state, hdl);
+	struct saa6752hs_mpeg_params *params = &h->params;
+
+	switch (ctrl->id) {
+	case V4L2_CID_MPEG_STREAM_TYPE:
+		break;
+	case V4L2_CID_MPEG_STREAM_PID_PMT:
+		params->ts_pid_pmt = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_STREAM_PID_AUDIO:
+		params->ts_pid_audio = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_STREAM_PID_VIDEO:
+		params->ts_pid_video = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_STREAM_PID_PCR:
+		params->ts_pid_pcr = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_AUDIO_ENCODING:
+		params->au_encoding = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_AUDIO_L2_BITRATE:
+		params->au_l2_bitrate = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_AUDIO_AC3_BITRATE:
+		params->au_ac3_bitrate = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_AUDIO_SAMPLING_FREQ:
+		break;
+	case V4L2_CID_MPEG_VIDEO_ENCODING:
+		break;
+	case V4L2_CID_MPEG_VIDEO_ASPECT:
+		params->vi_aspect = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_BITRATE_MODE:
+		params->vi_bitrate_mode = ctrl->val;
+		params->vi_bitrate = h->video_bitrate->val / 1000;
+		params->vi_bitrate_peak = h->video_bitrate_peak->val / 1000;
+		v4l2_ctrl_activate(h->video_bitrate_peak,
+				ctrl->val == V4L2_MPEG_VIDEO_BITRATE_MODE_VBR);
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int saa6752hs_init(struct v4l2_subdev *sd, u32 leading_null_bytes)
+{
+	unsigned char buf[9], buf2[4];
+	struct saa6752hs_state *h = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	unsigned size;
+	u32 crc;
+	unsigned char localPAT[256];
+	unsigned char localPMT[256];
+
+	/* Set video format - must be done first as it resets other settings */
+	set_reg8(client, 0x41, h->video_format);
+
+	/* Set number of lines in input signal */
+	set_reg8(client, 0x40, (h->standard & V4L2_STD_525_60) ? 1 : 0);
+
+	/* set bitrate */
+	saa6752hs_set_bitrate(client, h);
+
+	/* Set GOP structure {3, 13} */
+	set_reg16(client, 0x72, 0x030d);
+
+	/* Set minimum Q-scale {4} */
+	set_reg8(client, 0x82, 0x04);
+
+	/* Set maximum Q-scale {12} */
+	set_reg8(client, 0x83, 0x0c);
+
+	/* Set Output Protocol */
+	set_reg8(client, 0xd0, 0x81);
+
+	/* Set video output stream format {TS} */
+	set_reg8(client, 0xb0, 0x05);
+
+	/* Set leading null byte for TS */
+	set_reg16(client, 0xf6, leading_null_bytes);
+
+	/* compute PAT */
+	memcpy(localPAT, PAT, sizeof(PAT));
+	localPAT[17] = 0xe0 | ((h->params.ts_pid_pmt >> 8) & 0x0f);
+	localPAT[18] = h->params.ts_pid_pmt & 0xff;
+	crc = crc32_be(~0, &localPAT[7], sizeof(PAT) - 7 - 4);
+	localPAT[sizeof(PAT) - 4] = (crc >> 24) & 0xFF;
+	localPAT[sizeof(PAT) - 3] = (crc >> 16) & 0xFF;
+	localPAT[sizeof(PAT) - 2] = (crc >> 8) & 0xFF;
+	localPAT[sizeof(PAT) - 1] = crc & 0xFF;
+
+	/* compute PMT */
+	if (h->params.au_encoding == V4L2_MPEG_AUDIO_ENCODING_AC3) {
+		size = sizeof(PMT_AC3);
+		memcpy(localPMT, PMT_AC3, size);
+	} else {
+		size = sizeof(PMT);
+		memcpy(localPMT, PMT, size);
+	}
+	localPMT[3] = 0x40 | ((h->params.ts_pid_pmt >> 8) & 0x0f);
+	localPMT[4] = h->params.ts_pid_pmt & 0xff;
+	localPMT[15] = 0xE0 | ((h->params.ts_pid_pcr >> 8) & 0x0F);
+	localPMT[16] = h->params.ts_pid_pcr & 0xFF;
+	localPMT[20] = 0xE0 | ((h->params.ts_pid_video >> 8) & 0x0F);
+	localPMT[21] = h->params.ts_pid_video & 0xFF;
+	localPMT[25] = 0xE0 | ((h->params.ts_pid_audio >> 8) & 0x0F);
+	localPMT[26] = h->params.ts_pid_audio & 0xFF;
+	crc = crc32_be(~0, &localPMT[7], size - 7 - 4);
+	localPMT[size - 4] = (crc >> 24) & 0xFF;
+	localPMT[size - 3] = (crc >> 16) & 0xFF;
+	localPMT[size - 2] = (crc >> 8) & 0xFF;
+	localPMT[size - 1] = crc & 0xFF;
+
+	/* Set Audio PID */
+	set_reg16(client, 0xc1, h->params.ts_pid_audio);
+
+	/* Set Video PID */
+	set_reg16(client, 0xc0, h->params.ts_pid_video);
+
+	/* Set PCR PID */
+	set_reg16(client, 0xc4, h->params.ts_pid_pcr);
+
+	/* Send SI tables */
+	i2c_master_send(client, localPAT, sizeof(PAT));
+	i2c_master_send(client, localPMT, size);
+
+	/* mute then unmute audio. This removes buzzing artefacts */
+	set_reg8(client, 0xa4, 1);
+	set_reg8(client, 0xa4, 0);
+
+	/* start it going */
+	saa6752hs_chip_command(client, SAA6752HS_COMMAND_START);
+
+	/* readout current state */
+	buf[0] = 0xE1;
+	buf[1] = 0xA7;
+	buf[2] = 0xFE;
+	buf[3] = 0x82;
+	buf[4] = 0xB0;
+	i2c_master_send(client, buf, 5);
+	i2c_master_recv(client, buf2, 4);
+
+	/* change aspect ratio */
+	buf[0] = 0xE0;
+	buf[1] = 0xA7;
+	buf[2] = 0xFE;
+	buf[3] = 0x82;
+	buf[4] = 0xB0;
+	buf[5] = buf2[0];
+	switch (h->params.vi_aspect) {
+	case V4L2_MPEG_VIDEO_ASPECT_16x9:
+		buf[6] = buf2[1] | 0x40;
+		break;
+	case V4L2_MPEG_VIDEO_ASPECT_4x3:
+	default:
+		buf[6] = buf2[1] & 0xBF;
+		break;
+	}
+	buf[7] = buf2[2];
+	buf[8] = buf2[3];
+	i2c_master_send(client, buf, 9);
+
+	return 0;
+}
+
+static int saa6752hs_get_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *f = &format->format;
+	struct saa6752hs_state *h = to_state(sd);
+
+	if (format->pad)
+		return -EINVAL;
+
+	if (h->video_format == SAA6752HS_VF_UNKNOWN)
+		h->video_format = SAA6752HS_VF_D1;
+	f->width = v4l2_format_table[h->video_format].fmt.pix.width;
+	f->height = v4l2_format_table[h->video_format].fmt.pix.height;
+	f->code = MEDIA_BUS_FMT_FIXED;
+	f->field = V4L2_FIELD_INTERLACED;
+	f->colorspace = V4L2_COLORSPACE_SMPTE170M;
+	return 0;
+}
+
+static int saa6752hs_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *f = &format->format;
+	struct saa6752hs_state *h = to_state(sd);
+	int dist_352, dist_480, dist_720;
+
+	if (format->pad)
+		return -EINVAL;
+
+	f->code = MEDIA_BUS_FMT_FIXED;
+
+	dist_352 = abs(f->width - 352);
+	dist_480 = abs(f->width - 480);
+	dist_720 = abs(f->width - 720);
+	if (dist_720 < dist_480) {
+		f->width = 720;
+		f->height = 576;
+	} else if (dist_480 < dist_352) {
+		f->width = 480;
+		f->height = 576;
+	} else {
+		f->width = 352;
+		if (abs(f->height - 576) < abs(f->height - 288))
+			f->height = 576;
+		else
+			f->height = 288;
+	}
+	f->field = V4L2_FIELD_INTERLACED;
+	f->colorspace = V4L2_COLORSPACE_SMPTE170M;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		sd_state->pads->try_fmt = *f;
+		return 0;
+	}
+
+	/*
+	  FIXME: translate and round width/height into EMPRESS
+	  subsample type:
+
+	  type   |   PAL   |  NTSC
+	  ---------------------------
+	  SIF    | 352x288 | 352x240
+	  1/2 D1 | 352x576 | 352x480
+	  2/3 D1 | 480x576 | 480x480
+	  D1     | 720x576 | 720x480
+	*/
+
+	if (f->code != MEDIA_BUS_FMT_FIXED)
+		return -EINVAL;
+
+	if (f->width == 720)
+		h->video_format = SAA6752HS_VF_D1;
+	else if (f->width == 480)
+		h->video_format = SAA6752HS_VF_2_3_D1;
+	else if (f->height == 576)
+		h->video_format = SAA6752HS_VF_1_2_D1;
+	else
+		h->video_format = SAA6752HS_VF_SIF;
+	return 0;
+}
+
+static int saa6752hs_s_std(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct saa6752hs_state *h = to_state(sd);
+
+	h->standard = std;
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_ctrl_ops saa6752hs_ctrl_ops = {
+	.try_ctrl = saa6752hs_try_ctrl,
+	.s_ctrl = saa6752hs_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops saa6752hs_core_ops = {
+	.init = saa6752hs_init,
+};
+
+static const struct v4l2_subdev_video_ops saa6752hs_video_ops = {
+	.s_std = saa6752hs_s_std,
+};
+
+static const struct v4l2_subdev_pad_ops saa6752hs_pad_ops = {
+	.get_fmt = saa6752hs_get_fmt,
+	.set_fmt = saa6752hs_set_fmt,
+};
+
+static const struct v4l2_subdev_ops saa6752hs_ops = {
+	.core = &saa6752hs_core_ops,
+	.video = &saa6752hs_video_ops,
+	.pad = &saa6752hs_pad_ops,
+};
+
+static int saa6752hs_probe(struct i2c_client *client)
+{
+	struct saa6752hs_state *h;
+	struct v4l2_subdev *sd;
+	struct v4l2_ctrl_handler *hdl;
+	u8 addr = 0x13;
+	u8 data[12];
+
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	h = devm_kzalloc(&client->dev, sizeof(*h), GFP_KERNEL);
+	if (h == NULL)
+		return -ENOMEM;
+	sd = &h->sd;
+	v4l2_i2c_subdev_init(sd, client, &saa6752hs_ops);
+
+	i2c_master_send(client, &addr, 1);
+	i2c_master_recv(client, data, sizeof(data));
+	h->revision = (data[8] << 8) | data[9];
+	h->has_ac3 = 0;
+	if (h->revision == 0x0206) {
+		h->has_ac3 = 1;
+		v4l_info(client, "supports AC-3\n");
+	}
+	h->params = param_defaults;
+
+	hdl = &h->hdl;
+	v4l2_ctrl_handler_init(hdl, 14);
+	v4l2_ctrl_new_std_menu(hdl, &saa6752hs_ctrl_ops,
+		V4L2_CID_MPEG_AUDIO_ENCODING,
+		h->has_ac3 ? V4L2_MPEG_AUDIO_ENCODING_AC3 :
+			V4L2_MPEG_AUDIO_ENCODING_LAYER_2,
+		0x0d, V4L2_MPEG_AUDIO_ENCODING_LAYER_2);
+
+	v4l2_ctrl_new_std_menu(hdl, &saa6752hs_ctrl_ops,
+		V4L2_CID_MPEG_AUDIO_L2_BITRATE,
+		V4L2_MPEG_AUDIO_L2_BITRATE_384K,
+		~((1 << V4L2_MPEG_AUDIO_L2_BITRATE_256K) |
+		  (1 << V4L2_MPEG_AUDIO_L2_BITRATE_384K)),
+		V4L2_MPEG_AUDIO_L2_BITRATE_256K);
+
+	if (h->has_ac3)
+		v4l2_ctrl_new_std_menu(hdl, &saa6752hs_ctrl_ops,
+			V4L2_CID_MPEG_AUDIO_AC3_BITRATE,
+			V4L2_MPEG_AUDIO_AC3_BITRATE_384K,
+			~((1 << V4L2_MPEG_AUDIO_AC3_BITRATE_256K) |
+			  (1 << V4L2_MPEG_AUDIO_AC3_BITRATE_384K)),
+			V4L2_MPEG_AUDIO_AC3_BITRATE_256K);
+
+	v4l2_ctrl_new_std_menu(hdl, &saa6752hs_ctrl_ops,
+		V4L2_CID_MPEG_AUDIO_SAMPLING_FREQ,
+		V4L2_MPEG_AUDIO_SAMPLING_FREQ_48000,
+		~(1 << V4L2_MPEG_AUDIO_SAMPLING_FREQ_48000),
+		V4L2_MPEG_AUDIO_SAMPLING_FREQ_48000);
+
+	v4l2_ctrl_new_std_menu(hdl, &saa6752hs_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_ENCODING,
+		V4L2_MPEG_VIDEO_ENCODING_MPEG_2,
+		~(1 << V4L2_MPEG_VIDEO_ENCODING_MPEG_2),
+		V4L2_MPEG_VIDEO_ENCODING_MPEG_2);
+
+	v4l2_ctrl_new_std_menu(hdl, &saa6752hs_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_ASPECT,
+		V4L2_MPEG_VIDEO_ASPECT_16x9, 0x01,
+		V4L2_MPEG_VIDEO_ASPECT_4x3);
+
+	h->video_bitrate_peak = v4l2_ctrl_new_std(hdl, &saa6752hs_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_BITRATE_PEAK,
+		1000000, 27000000, 1000, 8000000);
+
+	v4l2_ctrl_new_std_menu(hdl, &saa6752hs_ctrl_ops,
+		V4L2_CID_MPEG_STREAM_TYPE,
+		V4L2_MPEG_STREAM_TYPE_MPEG2_TS,
+		~(1 << V4L2_MPEG_STREAM_TYPE_MPEG2_TS),
+		V4L2_MPEG_STREAM_TYPE_MPEG2_TS);
+
+	h->video_bitrate_mode = v4l2_ctrl_new_std_menu(hdl, &saa6752hs_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_BITRATE_MODE,
+		V4L2_MPEG_VIDEO_BITRATE_MODE_CBR, 0,
+		V4L2_MPEG_VIDEO_BITRATE_MODE_VBR);
+	h->video_bitrate = v4l2_ctrl_new_std(hdl, &saa6752hs_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_BITRATE, 1000000, 27000000, 1000, 6000000);
+	v4l2_ctrl_new_std(hdl, &saa6752hs_ctrl_ops,
+		V4L2_CID_MPEG_STREAM_PID_PMT, 0, (1 << 14) - 1, 1, 16);
+	v4l2_ctrl_new_std(hdl, &saa6752hs_ctrl_ops,
+		V4L2_CID_MPEG_STREAM_PID_AUDIO, 0, (1 << 14) - 1, 1, 260);
+	v4l2_ctrl_new_std(hdl, &saa6752hs_ctrl_ops,
+		V4L2_CID_MPEG_STREAM_PID_VIDEO, 0, (1 << 14) - 1, 1, 256);
+	v4l2_ctrl_new_std(hdl, &saa6752hs_ctrl_ops,
+		V4L2_CID_MPEG_STREAM_PID_PCR, 0, (1 << 14) - 1, 1, 259);
+	sd->ctrl_handler = hdl;
+	if (hdl->error) {
+		int err = hdl->error;
+
+		v4l2_ctrl_handler_free(hdl);
+		return err;
+	}
+	v4l2_ctrl_cluster(3, &h->video_bitrate_mode);
+	v4l2_ctrl_handler_setup(hdl);
+	h->standard = 0; /* Assume 625 input lines */
+	return 0;
+}
+
+static void saa6752hs_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&to_state(sd)->hdl);
+}
+
+static const struct i2c_device_id saa6752hs_id[] = {
+	{ "saa6752hs", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, saa6752hs_id);
+
+static struct i2c_driver saa6752hs_driver = {
+	.driver = {
+		.name	= "saa6752hs",
+	},
+	.probe		= saa6752hs_probe,
+	.remove		= saa6752hs_remove,
+	.id_table	= saa6752hs_id,
+};
+
+module_i2c_driver(saa6752hs_driver);
diff --git a/drivers/media/i2c/saa7110.c b/drivers/media/i2c/saa7110.c
new file mode 100644
index 000000000..152079033
--- /dev/null
+++ b/drivers/media/i2c/saa7110.c
@@ -0,0 +1,456 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * saa7110 - Philips SAA7110(A) video decoder driver
+ *
+ * Copyright (C) 1998 Pauline Middelink <middelin@polyware.nl>
+ *
+ * Copyright (C) 1999 Wolfgang Scherr <scherr@net4you.net>
+ * Copyright (C) 2000 Serguei Miridonov <mirsev@cicese.mx>
+ *    - some corrections for Pinnacle Systems Inc. DC10plus card.
+ *
+ * Changes by Ronald Bultje <rbultje@ronald.bitfreak.net>
+ *    - moved over to linux>=2.4.x i2c protocol (1/1/2003)
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/wait.h>
+#include <linux/uaccess.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+
+MODULE_DESCRIPTION("Philips SAA7110 video decoder driver");
+MODULE_AUTHOR("Pauline Middelink");
+MODULE_LICENSE("GPL");
+
+
+static int debug;
+module_param(debug, int, 0);
+MODULE_PARM_DESC(debug, "Debug level (0-1)");
+
+#define SAA7110_MAX_INPUT	9	/* 6 CVBS, 3 SVHS */
+#define SAA7110_MAX_OUTPUT	1	/* 1 YUV */
+
+#define SAA7110_NR_REG		0x35
+
+struct saa7110 {
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
+	u8 reg[SAA7110_NR_REG];
+
+	v4l2_std_id norm;
+	int input;
+	int enable;
+
+	wait_queue_head_t wq;
+};
+
+static inline struct saa7110 *to_saa7110(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct saa7110, sd);
+}
+
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct saa7110, hdl)->sd;
+}
+
+/* ----------------------------------------------------------------------- */
+/* I2C support functions						   */
+/* ----------------------------------------------------------------------- */
+
+static int saa7110_write(struct v4l2_subdev *sd, u8 reg, u8 value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct saa7110 *decoder = to_saa7110(sd);
+
+	decoder->reg[reg] = value;
+	return i2c_smbus_write_byte_data(client, reg, value);
+}
+
+static int saa7110_write_block(struct v4l2_subdev *sd, const u8 *data, unsigned int len)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct saa7110 *decoder = to_saa7110(sd);
+	int ret = -1;
+	u8 reg = *data;		/* first register to write to */
+
+	/* Sanity check */
+	if (reg + (len - 1) > SAA7110_NR_REG)
+		return ret;
+
+	/* the saa7110 has an autoincrement function, use it if
+	 * the adapter understands raw I2C */
+	if (i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		ret = i2c_master_send(client, data, len);
+
+		/* Cache the written data */
+		memcpy(decoder->reg + reg, data + 1, len - 1);
+	} else {
+		for (++data, --len; len; len--) {
+			ret = saa7110_write(sd, reg++, *data++);
+			if (ret < 0)
+				break;
+		}
+	}
+
+	return ret;
+}
+
+static inline int saa7110_read(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return i2c_smbus_read_byte(client);
+}
+
+/* ----------------------------------------------------------------------- */
+/* SAA7110 functions							   */
+/* ----------------------------------------------------------------------- */
+
+#define FRESP_06H_COMPST 0x03	/*0x13*/
+#define FRESP_06H_SVIDEO 0x83	/*0xC0*/
+
+
+static int saa7110_selmux(struct v4l2_subdev *sd, int chan)
+{
+	static const unsigned char modes[9][8] = {
+		/* mode 0 */
+		{FRESP_06H_COMPST, 0xD9, 0x17, 0x40, 0x03,
+			      0x44, 0x75, 0x16},
+		/* mode 1 */
+		{FRESP_06H_COMPST, 0xD8, 0x17, 0x40, 0x03,
+			      0x44, 0x75, 0x16},
+		/* mode 2 */
+		{FRESP_06H_COMPST, 0xBA, 0x07, 0x91, 0x03,
+			      0x60, 0xB5, 0x05},
+		/* mode 3 */
+		{FRESP_06H_COMPST, 0xB8, 0x07, 0x91, 0x03,
+			      0x60, 0xB5, 0x05},
+		/* mode 4 */
+		{FRESP_06H_COMPST, 0x7C, 0x07, 0xD2, 0x83,
+			      0x60, 0xB5, 0x03},
+		/* mode 5 */
+		{FRESP_06H_COMPST, 0x78, 0x07, 0xD2, 0x83,
+			      0x60, 0xB5, 0x03},
+		/* mode 6 */
+		{FRESP_06H_SVIDEO, 0x59, 0x17, 0x42, 0xA3,
+			      0x44, 0x75, 0x12},
+		/* mode 7 */
+		{FRESP_06H_SVIDEO, 0x9A, 0x17, 0xB1, 0x13,
+			      0x60, 0xB5, 0x14},
+		/* mode 8 */
+		{FRESP_06H_SVIDEO, 0x3C, 0x27, 0xC1, 0x23,
+			      0x44, 0x75, 0x21}
+	};
+	struct saa7110 *decoder = to_saa7110(sd);
+	const unsigned char *ptr = modes[chan];
+
+	saa7110_write(sd, 0x06, ptr[0]);	/* Luminance control    */
+	saa7110_write(sd, 0x20, ptr[1]);	/* Analog Control #1    */
+	saa7110_write(sd, 0x21, ptr[2]);	/* Analog Control #2    */
+	saa7110_write(sd, 0x22, ptr[3]);	/* Mixer Control #1     */
+	saa7110_write(sd, 0x2C, ptr[4]);	/* Mixer Control #2     */
+	saa7110_write(sd, 0x30, ptr[5]);	/* ADCs gain control    */
+	saa7110_write(sd, 0x31, ptr[6]);	/* Mixer Control #3     */
+	saa7110_write(sd, 0x21, ptr[7]);	/* Analog Control #2    */
+	decoder->input = chan;
+
+	return 0;
+}
+
+static const unsigned char initseq[1 + SAA7110_NR_REG] = {
+	0, 0x4C, 0x3C, 0x0D, 0xEF, 0xBD, 0xF2, 0x03, 0x00,
+	/* 0x08 */ 0xF8, 0xF8, 0x60, 0x60, 0x00, 0x86, 0x18, 0x90,
+	/* 0x10 */ 0x00, 0x59, 0x40, 0x46, 0x42, 0x1A, 0xFF, 0xDA,
+	/* 0x18 */ 0xF2, 0x8B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	/* 0x20 */ 0xD9, 0x16, 0x40, 0x41, 0x80, 0x41, 0x80, 0x4F,
+	/* 0x28 */ 0xFE, 0x01, 0xCF, 0x0F, 0x03, 0x01, 0x03, 0x0C,
+	/* 0x30 */ 0x44, 0x71, 0x02, 0x8C, 0x02
+};
+
+static v4l2_std_id determine_norm(struct v4l2_subdev *sd)
+{
+	DEFINE_WAIT(wait);
+	struct saa7110 *decoder = to_saa7110(sd);
+	int status;
+
+	/* mode changed, start automatic detection */
+	saa7110_write_block(sd, initseq, sizeof(initseq));
+	saa7110_selmux(sd, decoder->input);
+	prepare_to_wait(&decoder->wq, &wait, TASK_UNINTERRUPTIBLE);
+	schedule_timeout(msecs_to_jiffies(250));
+	finish_wait(&decoder->wq, &wait);
+	status = saa7110_read(sd);
+	if (status & 0x40) {
+		v4l2_dbg(1, debug, sd, "status=0x%02x (no signal)\n", status);
+		return V4L2_STD_UNKNOWN;
+	}
+	if ((status & 3) == 0) {
+		saa7110_write(sd, 0x06, 0x83);
+		if (status & 0x20) {
+			v4l2_dbg(1, debug, sd, "status=0x%02x (NTSC/no color)\n", status);
+			/*saa7110_write(sd,0x2E,0x81);*/
+			return V4L2_STD_NTSC;
+		}
+		v4l2_dbg(1, debug, sd, "status=0x%02x (PAL/no color)\n", status);
+		/*saa7110_write(sd,0x2E,0x9A);*/
+		return V4L2_STD_PAL;
+	}
+	/*saa7110_write(sd,0x06,0x03);*/
+	if (status & 0x20) {	/* 60Hz */
+		v4l2_dbg(1, debug, sd, "status=0x%02x (NTSC)\n", status);
+		saa7110_write(sd, 0x0D, 0x86);
+		saa7110_write(sd, 0x0F, 0x50);
+		saa7110_write(sd, 0x11, 0x2C);
+		/*saa7110_write(sd,0x2E,0x81);*/
+		return V4L2_STD_NTSC;
+	}
+
+	/* 50Hz -> PAL/SECAM */
+	saa7110_write(sd, 0x0D, 0x86);
+	saa7110_write(sd, 0x0F, 0x10);
+	saa7110_write(sd, 0x11, 0x59);
+	/*saa7110_write(sd,0x2E,0x9A);*/
+
+	prepare_to_wait(&decoder->wq, &wait, TASK_UNINTERRUPTIBLE);
+	schedule_timeout(msecs_to_jiffies(250));
+	finish_wait(&decoder->wq, &wait);
+
+	status = saa7110_read(sd);
+	if ((status & 0x03) == 0x01) {
+		v4l2_dbg(1, debug, sd, "status=0x%02x (SECAM)\n", status);
+		saa7110_write(sd, 0x0D, 0x87);
+		return V4L2_STD_SECAM;
+	}
+	v4l2_dbg(1, debug, sd, "status=0x%02x (PAL)\n", status);
+	return V4L2_STD_PAL;
+}
+
+static int saa7110_g_input_status(struct v4l2_subdev *sd, u32 *pstatus)
+{
+	struct saa7110 *decoder = to_saa7110(sd);
+	int res = V4L2_IN_ST_NO_SIGNAL;
+	int status = saa7110_read(sd);
+
+	v4l2_dbg(1, debug, sd, "status=0x%02x norm=%llx\n",
+		       status, (unsigned long long)decoder->norm);
+	if (!(status & 0x40))
+		res = 0;
+	if (!(status & 0x03))
+		res |= V4L2_IN_ST_NO_COLOR;
+
+	*pstatus = res;
+	return 0;
+}
+
+static int saa7110_querystd(struct v4l2_subdev *sd, v4l2_std_id *std)
+{
+	*std &= determine_norm(sd);
+	return 0;
+}
+
+static int saa7110_s_std(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct saa7110 *decoder = to_saa7110(sd);
+
+	if (decoder->norm != std) {
+		decoder->norm = std;
+		/*saa7110_write(sd, 0x06, 0x03);*/
+		if (std & V4L2_STD_NTSC) {
+			saa7110_write(sd, 0x0D, 0x86);
+			saa7110_write(sd, 0x0F, 0x50);
+			saa7110_write(sd, 0x11, 0x2C);
+			/*saa7110_write(sd, 0x2E, 0x81);*/
+			v4l2_dbg(1, debug, sd, "switched to NTSC\n");
+		} else if (std & V4L2_STD_PAL) {
+			saa7110_write(sd, 0x0D, 0x86);
+			saa7110_write(sd, 0x0F, 0x10);
+			saa7110_write(sd, 0x11, 0x59);
+			/*saa7110_write(sd, 0x2E, 0x9A);*/
+			v4l2_dbg(1, debug, sd, "switched to PAL\n");
+		} else if (std & V4L2_STD_SECAM) {
+			saa7110_write(sd, 0x0D, 0x87);
+			saa7110_write(sd, 0x0F, 0x10);
+			saa7110_write(sd, 0x11, 0x59);
+			/*saa7110_write(sd, 0x2E, 0x9A);*/
+			v4l2_dbg(1, debug, sd, "switched to SECAM\n");
+		} else {
+			return -EINVAL;
+		}
+	}
+	return 0;
+}
+
+static int saa7110_s_routing(struct v4l2_subdev *sd,
+			     u32 input, u32 output, u32 config)
+{
+	struct saa7110 *decoder = to_saa7110(sd);
+
+	if (input >= SAA7110_MAX_INPUT) {
+		v4l2_dbg(1, debug, sd, "input=%d not available\n", input);
+		return -EINVAL;
+	}
+	if (decoder->input != input) {
+		saa7110_selmux(sd, input);
+		v4l2_dbg(1, debug, sd, "switched to input=%d\n", input);
+	}
+	return 0;
+}
+
+static int saa7110_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct saa7110 *decoder = to_saa7110(sd);
+
+	if (decoder->enable != enable) {
+		decoder->enable = enable;
+		saa7110_write(sd, 0x0E, enable ? 0x18 : 0x80);
+		v4l2_dbg(1, debug, sd, "YUV %s\n", enable ? "on" : "off");
+	}
+	return 0;
+}
+
+static int saa7110_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		saa7110_write(sd, 0x19, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		saa7110_write(sd, 0x13, ctrl->val);
+		break;
+	case V4L2_CID_SATURATION:
+		saa7110_write(sd, 0x12, ctrl->val);
+		break;
+	case V4L2_CID_HUE:
+		saa7110_write(sd, 0x07, ctrl->val);
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_ctrl_ops saa7110_ctrl_ops = {
+	.s_ctrl = saa7110_s_ctrl,
+};
+
+static const struct v4l2_subdev_video_ops saa7110_video_ops = {
+	.s_std = saa7110_s_std,
+	.s_routing = saa7110_s_routing,
+	.s_stream = saa7110_s_stream,
+	.querystd = saa7110_querystd,
+	.g_input_status = saa7110_g_input_status,
+};
+
+static const struct v4l2_subdev_ops saa7110_ops = {
+	.video = &saa7110_video_ops,
+};
+
+/* ----------------------------------------------------------------------- */
+
+static int saa7110_probe(struct i2c_client *client)
+{
+	struct saa7110 *decoder;
+	struct v4l2_subdev *sd;
+	int rv;
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter,
+		I2C_FUNC_SMBUS_READ_BYTE | I2C_FUNC_SMBUS_WRITE_BYTE_DATA))
+		return -ENODEV;
+
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	decoder = devm_kzalloc(&client->dev, sizeof(*decoder), GFP_KERNEL);
+	if (!decoder)
+		return -ENOMEM;
+	sd = &decoder->sd;
+	v4l2_i2c_subdev_init(sd, client, &saa7110_ops);
+	decoder->norm = V4L2_STD_PAL;
+	decoder->input = 0;
+	decoder->enable = 1;
+	v4l2_ctrl_handler_init(&decoder->hdl, 2);
+	v4l2_ctrl_new_std(&decoder->hdl, &saa7110_ctrl_ops,
+		V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(&decoder->hdl, &saa7110_ctrl_ops,
+		V4L2_CID_CONTRAST, 0, 127, 1, 64);
+	v4l2_ctrl_new_std(&decoder->hdl, &saa7110_ctrl_ops,
+		V4L2_CID_SATURATION, 0, 127, 1, 64);
+	v4l2_ctrl_new_std(&decoder->hdl, &saa7110_ctrl_ops,
+		V4L2_CID_HUE, -128, 127, 1, 0);
+	sd->ctrl_handler = &decoder->hdl;
+	if (decoder->hdl.error) {
+		int err = decoder->hdl.error;
+
+		v4l2_ctrl_handler_free(&decoder->hdl);
+		return err;
+	}
+	v4l2_ctrl_handler_setup(&decoder->hdl);
+
+	init_waitqueue_head(&decoder->wq);
+
+	rv = saa7110_write_block(sd, initseq, sizeof(initseq));
+	if (rv < 0) {
+		v4l2_dbg(1, debug, sd, "init status %d\n", rv);
+	} else {
+		int ver, status;
+		saa7110_write(sd, 0x21, 0x10);
+		saa7110_write(sd, 0x0e, 0x18);
+		saa7110_write(sd, 0x0D, 0x04);
+		ver = saa7110_read(sd);
+		saa7110_write(sd, 0x0D, 0x06);
+		/*mdelay(150);*/
+		status = saa7110_read(sd);
+		v4l2_dbg(1, debug, sd, "version %x, status=0x%02x\n",
+			       ver, status);
+		saa7110_write(sd, 0x0D, 0x86);
+		saa7110_write(sd, 0x0F, 0x10);
+		saa7110_write(sd, 0x11, 0x59);
+		/*saa7110_write(sd, 0x2E, 0x9A);*/
+	}
+
+	/*saa7110_selmux(sd,0);*/
+	/*determine_norm(sd);*/
+	/* setup and implicit mode 0 select has been performed */
+
+	return 0;
+}
+
+static void saa7110_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct saa7110 *decoder = to_saa7110(sd);
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&decoder->hdl);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id saa7110_id[] = {
+	{ "saa7110", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, saa7110_id);
+
+static struct i2c_driver saa7110_driver = {
+	.driver = {
+		.name	= "saa7110",
+	},
+	.probe		= saa7110_probe,
+	.remove		= saa7110_remove,
+	.id_table	= saa7110_id,
+};
+
+module_i2c_driver(saa7110_driver);
diff --git a/drivers/media/i2c/saa7115.c b/drivers/media/i2c/saa7115.c
new file mode 100644
index 000000000..a1c71187e
--- /dev/null
+++ b/drivers/media/i2c/saa7115.c
@@ -0,0 +1,1959 @@
+// SPDX-License-Identifier: GPL-2.0+
+// saa711x - Philips SAA711x video decoder driver
+// This driver can work with saa7111, saa7111a, saa7113, saa7114,
+//			     saa7115 and saa7118.
+//
+// Based on saa7114 driver by Maxim Yevtyushkin, which is based on
+// the saa7111 driver by Dave Perks.
+//
+// Copyright (C) 1998 Dave Perks <dperks@ibm.net>
+// Copyright (C) 2002 Maxim Yevtyushkin <max@linuxmedialabs.com>
+//
+// Slight changes for video timing and attachment output by
+// Wolfgang Scherr <scherr@net4you.net>
+//
+// Moved over to the linux >= 2.4.x i2c protocol (1/1/2003)
+// by Ronald Bultje <rbultje@ronald.bitfreak.net>
+//
+// Added saa7115 support by Kevin Thayer <nufan_wfk at yahoo.com>
+// (2/17/2003)
+//
+// VBI support (2004) and cleanups (2005) by Hans Verkuil <hverkuil@xs4all.nl>
+//
+// Copyright (c) 2005-2006 Mauro Carvalho Chehab <mchehab@kernel.org>
+//	SAA7111, SAA7113 and SAA7118 support
+
+#include "saa711x_regs.h"
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-mc.h>
+#include <media/i2c/saa7115.h>
+#include <asm/div64.h>
+
+#define VRES_60HZ	(480+16)
+
+MODULE_DESCRIPTION("Philips SAA7111/SAA7113/SAA7114/SAA7115/SAA7118 video decoder driver");
+MODULE_AUTHOR(  "Maxim Yevtyushkin, Kevin Thayer, Chris Kennedy, "
+		"Hans Verkuil, Mauro Carvalho Chehab");
+MODULE_LICENSE("GPL");
+
+static bool debug;
+module_param(debug, bool, 0644);
+
+MODULE_PARM_DESC(debug, "Debug level (0-1)");
+
+
+enum saa711x_model {
+	SAA7111A,
+	SAA7111,
+	SAA7113,
+	GM7113C,
+	SAA7114,
+	SAA7115,
+	SAA7118,
+};
+
+enum saa711x_pads {
+	SAA711X_PAD_IF_INPUT,
+	SAA711X_PAD_VID_OUT,
+	SAA711X_NUM_PADS
+};
+
+struct saa711x_state {
+	struct v4l2_subdev sd;
+#ifdef CONFIG_MEDIA_CONTROLLER
+	struct media_pad pads[SAA711X_NUM_PADS];
+#endif
+	struct v4l2_ctrl_handler hdl;
+
+	struct {
+		/* chroma gain control cluster */
+		struct v4l2_ctrl *agc;
+		struct v4l2_ctrl *gain;
+	};
+
+	v4l2_std_id std;
+	int input;
+	int output;
+	int enable;
+	int radio;
+	int width;
+	int height;
+	enum saa711x_model ident;
+	u32 audclk_freq;
+	u32 crystal_freq;
+	bool ucgc;
+	u8 cgcdiv;
+	bool apll;
+	bool double_asclk;
+};
+
+static inline struct saa711x_state *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct saa711x_state, sd);
+}
+
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct saa711x_state, hdl)->sd;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static inline int saa711x_write(struct v4l2_subdev *sd, u8 reg, u8 value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return i2c_smbus_write_byte_data(client, reg, value);
+}
+
+/* Sanity routine to check if a register is present */
+static int saa711x_has_reg(const int id, const u8 reg)
+{
+	if (id == SAA7111)
+		return reg < 0x20 && reg != 0x01 && reg != 0x0f &&
+		       (reg < 0x13 || reg > 0x19) && reg != 0x1d && reg != 0x1e;
+	if (id == SAA7111A)
+		return reg < 0x20 && reg != 0x01 && reg != 0x0f &&
+		       reg != 0x14 && reg != 0x18 && reg != 0x19 &&
+		       reg != 0x1d && reg != 0x1e;
+
+	/* common for saa7113/4/5/8 */
+	if (unlikely((reg >= 0x3b && reg <= 0x3f) || reg == 0x5c || reg == 0x5f ||
+	    reg == 0xa3 || reg == 0xa7 || reg == 0xab || reg == 0xaf || (reg >= 0xb5 && reg <= 0xb7) ||
+	    reg == 0xd3 || reg == 0xd7 || reg == 0xdb || reg == 0xdf || (reg >= 0xe5 && reg <= 0xe7) ||
+	    reg == 0x82 || (reg >= 0x89 && reg <= 0x8e)))
+		return 0;
+
+	switch (id) {
+	case GM7113C:
+		return reg != 0x14 && (reg < 0x18 || reg > 0x1e) && reg < 0x20;
+	case SAA7113:
+		return reg != 0x14 && (reg < 0x18 || reg > 0x1e) && (reg < 0x20 || reg > 0x3f) &&
+		       reg != 0x5d && reg < 0x63;
+	case SAA7114:
+		return (reg < 0x1a || reg > 0x1e) && (reg < 0x20 || reg > 0x2f) &&
+		       (reg < 0x63 || reg > 0x7f) && reg != 0x33 && reg != 0x37 &&
+		       reg != 0x81 && reg < 0xf0;
+	case SAA7115:
+		return (reg < 0x20 || reg > 0x2f) && reg != 0x65 && (reg < 0xfc || reg > 0xfe);
+	case SAA7118:
+		return (reg < 0x1a || reg > 0x1d) && (reg < 0x20 || reg > 0x22) &&
+		       (reg < 0x26 || reg > 0x28) && reg != 0x33 && reg != 0x37 &&
+		       (reg < 0x63 || reg > 0x7f) && reg != 0x81 && reg < 0xf0;
+	}
+	return 1;
+}
+
+static int saa711x_writeregs(struct v4l2_subdev *sd, const unsigned char *regs)
+{
+	struct saa711x_state *state = to_state(sd);
+	unsigned char reg, data;
+
+	while (*regs != 0x00) {
+		reg = *(regs++);
+		data = *(regs++);
+
+		/* According with datasheets, reserved regs should be
+		   filled with 0 - seems better not to touch on they */
+		if (saa711x_has_reg(state->ident, reg)) {
+			if (saa711x_write(sd, reg, data) < 0)
+				return -1;
+		} else {
+			v4l2_dbg(1, debug, sd, "tried to access reserved reg 0x%02x\n", reg);
+		}
+	}
+	return 0;
+}
+
+static inline int saa711x_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return i2c_smbus_read_byte_data(client, reg);
+}
+
+/* ----------------------------------------------------------------------- */
+
+/* SAA7111 initialization table */
+static const unsigned char saa7111_init[] = {
+	R_01_INC_DELAY, 0x00,		/* reserved */
+
+	/*front end */
+	R_02_INPUT_CNTL_1, 0xd0,	/* FUSE=3, GUDL=2, MODE=0 */
+	R_03_INPUT_CNTL_2, 0x23,	/* HLNRS=0, VBSL=1, WPOFF=0, HOLDG=0,
+					 * GAFIX=0, GAI1=256, GAI2=256 */
+	R_04_INPUT_CNTL_3, 0x00,	/* GAI1=256 */
+	R_05_INPUT_CNTL_4, 0x00,	/* GAI2=256 */
+
+	/* decoder */
+	R_06_H_SYNC_START, 0xf3,	/* HSB at  13(50Hz) /  17(60Hz)
+					 * pixels after end of last line */
+	R_07_H_SYNC_STOP, 0xe8,		/* HSS seems to be needed to
+					 * work with NTSC, too */
+	R_08_SYNC_CNTL, 0xc8,		/* AUFD=1, FSEL=1, EXFIL=0,
+					 * VTRC=1, HPLL=0, VNOI=0 */
+	R_09_LUMA_CNTL, 0x01,		/* BYPS=0, PREF=0, BPSS=0,
+					 * VBLB=0, UPTCV=0, APER=1 */
+	R_0A_LUMA_BRIGHT_CNTL, 0x80,
+	R_0B_LUMA_CONTRAST_CNTL, 0x47,	/* 0b - CONT=1.109 */
+	R_0C_CHROMA_SAT_CNTL, 0x40,
+	R_0D_CHROMA_HUE_CNTL, 0x00,
+	R_0E_CHROMA_CNTL_1, 0x01,	/* 0e - CDTO=0, CSTD=0, DCCF=0,
+					 * FCTC=0, CHBW=1 */
+	R_0F_CHROMA_GAIN_CNTL, 0x00,	/* reserved */
+	R_10_CHROMA_CNTL_2, 0x48,	/* 10 - OFTS=1, HDEL=0, VRLN=1, YDEL=0 */
+	R_11_MODE_DELAY_CNTL, 0x1c,	/* 11 - GPSW=0, CM99=0, FECO=0, COMPO=1,
+					 * OEYC=1, OEHV=1, VIPB=0, COLO=0 */
+	R_12_RT_SIGNAL_CNTL, 0x00,	/* 12 - output control 2 */
+	R_13_RT_X_PORT_OUT_CNTL, 0x00,	/* 13 - output control 3 */
+	R_14_ANAL_ADC_COMPAT_CNTL, 0x00,
+	R_15_VGATE_START_FID_CHG, 0x00,
+	R_16_VGATE_STOP, 0x00,
+	R_17_MISC_VGATE_CONF_AND_MSB, 0x00,
+
+	0x00, 0x00
+};
+
+/*
+ * This table has one illegal value, and some values that are not
+ * correct according to the datasheet initialization table.
+ *
+ *  If you need a table with legal/default values tell the driver in
+ *  i2c_board_info.platform_data, and you will get the gm7113c_init
+ *  table instead.
+ */
+
+/* SAA7113 Init codes */
+static const unsigned char saa7113_init[] = {
+	R_01_INC_DELAY, 0x08,
+	R_02_INPUT_CNTL_1, 0xc2,
+	R_03_INPUT_CNTL_2, 0x30,
+	R_04_INPUT_CNTL_3, 0x00,
+	R_05_INPUT_CNTL_4, 0x00,
+	R_06_H_SYNC_START, 0x89,	/* Illegal value -119,
+					 * min. value = -108 (0x94) */
+	R_07_H_SYNC_STOP, 0x0d,
+	R_08_SYNC_CNTL, 0x88,		/* Not datasheet default.
+					 * HTC = VTR mode, should be 0x98 */
+	R_09_LUMA_CNTL, 0x01,
+	R_0A_LUMA_BRIGHT_CNTL, 0x80,
+	R_0B_LUMA_CONTRAST_CNTL, 0x47,
+	R_0C_CHROMA_SAT_CNTL, 0x40,
+	R_0D_CHROMA_HUE_CNTL, 0x00,
+	R_0E_CHROMA_CNTL_1, 0x01,
+	R_0F_CHROMA_GAIN_CNTL, 0x2a,
+	R_10_CHROMA_CNTL_2, 0x08,	/* Not datsheet default.
+					 * VRLN enabled, should be 0x00 */
+	R_11_MODE_DELAY_CNTL, 0x0c,
+	R_12_RT_SIGNAL_CNTL, 0x07,	/* Not datasheet default,
+					 * should be 0x01 */
+	R_13_RT_X_PORT_OUT_CNTL, 0x00,
+	R_14_ANAL_ADC_COMPAT_CNTL, 0x00,
+	R_15_VGATE_START_FID_CHG, 0x00,
+	R_16_VGATE_STOP, 0x00,
+	R_17_MISC_VGATE_CONF_AND_MSB, 0x00,
+
+	0x00, 0x00
+};
+
+/*
+ * GM7113C is a clone of the SAA7113 chip
+ *  This init table is copied out of the saa7113 datasheet.
+ *  In R_08 we enable "Automatic Field Detection" [AUFD],
+ *  this is disabled when saa711x_set_v4lstd is called.
+ */
+static const unsigned char gm7113c_init[] = {
+	R_01_INC_DELAY, 0x08,
+	R_02_INPUT_CNTL_1, 0xc0,
+	R_03_INPUT_CNTL_2, 0x33,
+	R_04_INPUT_CNTL_3, 0x00,
+	R_05_INPUT_CNTL_4, 0x00,
+	R_06_H_SYNC_START, 0xe9,
+	R_07_H_SYNC_STOP, 0x0d,
+	R_08_SYNC_CNTL, 0x98,
+	R_09_LUMA_CNTL, 0x01,
+	R_0A_LUMA_BRIGHT_CNTL, 0x80,
+	R_0B_LUMA_CONTRAST_CNTL, 0x47,
+	R_0C_CHROMA_SAT_CNTL, 0x40,
+	R_0D_CHROMA_HUE_CNTL, 0x00,
+	R_0E_CHROMA_CNTL_1, 0x01,
+	R_0F_CHROMA_GAIN_CNTL, 0x2a,
+	R_10_CHROMA_CNTL_2, 0x00,
+	R_11_MODE_DELAY_CNTL, 0x0c,
+	R_12_RT_SIGNAL_CNTL, 0x01,
+	R_13_RT_X_PORT_OUT_CNTL, 0x00,
+	R_14_ANAL_ADC_COMPAT_CNTL, 0x00,
+	R_15_VGATE_START_FID_CHG, 0x00,
+	R_16_VGATE_STOP, 0x00,
+	R_17_MISC_VGATE_CONF_AND_MSB, 0x00,
+
+	0x00, 0x00
+};
+
+/* If a value differs from the Hauppauge driver values, then the comment starts with
+   'was 0xXX' to denote the Hauppauge value. Otherwise the value is identical to what the
+   Hauppauge driver sets. */
+
+/* SAA7114 and SAA7115 initialization table */
+static const unsigned char saa7115_init_auto_input[] = {
+		/* Front-End Part */
+	R_01_INC_DELAY, 0x48,			/* white peak control disabled */
+	R_03_INPUT_CNTL_2, 0x20,		/* was 0x30. 0x20: long vertical blanking */
+	R_04_INPUT_CNTL_3, 0x90,		/* analog gain set to 0 */
+	R_05_INPUT_CNTL_4, 0x90,		/* analog gain set to 0 */
+		/* Decoder Part */
+	R_06_H_SYNC_START, 0xeb,		/* horiz sync begin = -21 */
+	R_07_H_SYNC_STOP, 0xe0,			/* horiz sync stop = -17 */
+	R_09_LUMA_CNTL, 0x53,			/* 0x53, was 0x56 for 60hz. luminance control */
+	R_0A_LUMA_BRIGHT_CNTL, 0x80,		/* was 0x88. decoder brightness, 0x80 is itu standard */
+	R_0B_LUMA_CONTRAST_CNTL, 0x44,		/* was 0x48. decoder contrast, 0x44 is itu standard */
+	R_0C_CHROMA_SAT_CNTL, 0x40,		/* was 0x47. decoder saturation, 0x40 is itu standard */
+	R_0D_CHROMA_HUE_CNTL, 0x00,
+	R_0F_CHROMA_GAIN_CNTL, 0x00,		/* use automatic gain  */
+	R_10_CHROMA_CNTL_2, 0x06,		/* chroma: active adaptive combfilter */
+	R_11_MODE_DELAY_CNTL, 0x00,
+	R_12_RT_SIGNAL_CNTL, 0x9d,		/* RTS0 output control: VGATE */
+	R_13_RT_X_PORT_OUT_CNTL, 0x80,		/* ITU656 standard mode, RTCO output enable RTCE */
+	R_14_ANAL_ADC_COMPAT_CNTL, 0x00,
+	R_18_RAW_DATA_GAIN_CNTL, 0x40,		/* gain 0x00 = nominal */
+	R_19_RAW_DATA_OFF_CNTL, 0x80,
+	R_1A_COLOR_KILL_LVL_CNTL, 0x77,		/* recommended value */
+	R_1B_MISC_TVVCRDET, 0x42,		/* recommended value */
+	R_1C_ENHAN_COMB_CTRL1, 0xa9,		/* recommended value */
+	R_1D_ENHAN_COMB_CTRL2, 0x01,		/* recommended value */
+
+
+	R_80_GLOBAL_CNTL_1, 0x0,		/* No tasks enabled at init */
+
+		/* Power Device Control */
+	R_88_POWER_SAVE_ADC_PORT_CNTL, 0xd0,	/* reset device */
+	R_88_POWER_SAVE_ADC_PORT_CNTL, 0xf0,	/* set device programmed, all in operational mode */
+	0x00, 0x00
+};
+
+/* Used to reset saa7113, saa7114 and saa7115 */
+static const unsigned char saa7115_cfg_reset_scaler[] = {
+	R_87_I_PORT_I_O_ENA_OUT_CLK_AND_GATED, 0x00,	/* disable I-port output */
+	R_88_POWER_SAVE_ADC_PORT_CNTL, 0xd0,		/* reset scaler */
+	R_88_POWER_SAVE_ADC_PORT_CNTL, 0xf0,		/* activate scaler */
+	R_87_I_PORT_I_O_ENA_OUT_CLK_AND_GATED, 0x01,	/* enable I-port output */
+	0x00, 0x00
+};
+
+/* ============== SAA7715 VIDEO templates =============  */
+
+static const unsigned char saa7115_cfg_60hz_video[] = {
+	R_80_GLOBAL_CNTL_1, 0x00,			/* reset tasks */
+	R_88_POWER_SAVE_ADC_PORT_CNTL, 0xd0,		/* reset scaler */
+
+	R_15_VGATE_START_FID_CHG, 0x03,
+	R_16_VGATE_STOP, 0x11,
+	R_17_MISC_VGATE_CONF_AND_MSB, 0x9c,
+
+	R_08_SYNC_CNTL, 0x68,			/* 0xBO: auto detection, 0x68 = NTSC */
+	R_0E_CHROMA_CNTL_1, 0x07,		/* video autodetection is on */
+
+	R_5A_V_OFF_FOR_SLICER, 0x06,		/* standard 60hz value for ITU656 line counting */
+
+	/* Task A */
+	R_90_A_TASK_HANDLING_CNTL, 0x80,
+	R_91_A_X_PORT_FORMATS_AND_CONF, 0x48,
+	R_92_A_X_PORT_INPUT_REFERENCE_SIGNAL, 0x40,
+	R_93_A_I_PORT_OUTPUT_FORMATS_AND_CONF, 0x84,
+
+	/* hoffset low (input), 0x0002 is minimum */
+	R_94_A_HORIZ_INPUT_WINDOW_START, 0x01,
+	R_95_A_HORIZ_INPUT_WINDOW_START_MSB, 0x00,
+
+	/* hsize low (input), 0x02d0 = 720 */
+	R_96_A_HORIZ_INPUT_WINDOW_LENGTH, 0xd0,
+	R_97_A_HORIZ_INPUT_WINDOW_LENGTH_MSB, 0x02,
+
+	R_98_A_VERT_INPUT_WINDOW_START, 0x05,
+	R_99_A_VERT_INPUT_WINDOW_START_MSB, 0x00,
+
+	R_9A_A_VERT_INPUT_WINDOW_LENGTH, 0x0c,
+	R_9B_A_VERT_INPUT_WINDOW_LENGTH_MSB, 0x00,
+
+	R_9C_A_HORIZ_OUTPUT_WINDOW_LENGTH, 0xa0,
+	R_9D_A_HORIZ_OUTPUT_WINDOW_LENGTH_MSB, 0x05,
+
+	R_9E_A_VERT_OUTPUT_WINDOW_LENGTH, 0x0c,
+	R_9F_A_VERT_OUTPUT_WINDOW_LENGTH_MSB, 0x00,
+
+	/* Task B */
+	R_C0_B_TASK_HANDLING_CNTL, 0x00,
+	R_C1_B_X_PORT_FORMATS_AND_CONF, 0x08,
+	R_C2_B_INPUT_REFERENCE_SIGNAL_DEFINITION, 0x00,
+	R_C3_B_I_PORT_FORMATS_AND_CONF, 0x80,
+
+	/* 0x0002 is minimum */
+	R_C4_B_HORIZ_INPUT_WINDOW_START, 0x02,
+	R_C5_B_HORIZ_INPUT_WINDOW_START_MSB, 0x00,
+
+	/* 0x02d0 = 720 */
+	R_C6_B_HORIZ_INPUT_WINDOW_LENGTH, 0xd0,
+	R_C7_B_HORIZ_INPUT_WINDOW_LENGTH_MSB, 0x02,
+
+	/* vwindow start 0x12 = 18 */
+	R_C8_B_VERT_INPUT_WINDOW_START, 0x12,
+	R_C9_B_VERT_INPUT_WINDOW_START_MSB, 0x00,
+
+	/* vwindow length 0xf8 = 248 */
+	R_CA_B_VERT_INPUT_WINDOW_LENGTH, VRES_60HZ>>1,
+	R_CB_B_VERT_INPUT_WINDOW_LENGTH_MSB, VRES_60HZ>>9,
+
+	/* hwindow 0x02d0 = 720 */
+	R_CC_B_HORIZ_OUTPUT_WINDOW_LENGTH, 0xd0,
+	R_CD_B_HORIZ_OUTPUT_WINDOW_LENGTH_MSB, 0x02,
+
+	R_F0_LFCO_PER_LINE, 0xad,		/* Set PLL Register. 60hz 525 lines per frame, 27 MHz */
+	R_F1_P_I_PARAM_SELECT, 0x05,		/* low bit with 0xF0 */
+	R_F5_PULSGEN_LINE_LENGTH, 0xad,
+	R_F6_PULSE_A_POS_LSB_AND_PULSEGEN_CONFIG, 0x01,
+
+	0x00, 0x00
+};
+
+static const unsigned char saa7115_cfg_50hz_video[] = {
+	R_80_GLOBAL_CNTL_1, 0x00,
+	R_88_POWER_SAVE_ADC_PORT_CNTL, 0xd0,	/* reset scaler */
+
+	R_15_VGATE_START_FID_CHG, 0x37,		/* VGATE start */
+	R_16_VGATE_STOP, 0x16,
+	R_17_MISC_VGATE_CONF_AND_MSB, 0x99,
+
+	R_08_SYNC_CNTL, 0x28,			/* 0x28 = PAL */
+	R_0E_CHROMA_CNTL_1, 0x07,
+
+	R_5A_V_OFF_FOR_SLICER, 0x03,		/* standard 50hz value */
+
+	/* Task A */
+	R_90_A_TASK_HANDLING_CNTL, 0x81,
+	R_91_A_X_PORT_FORMATS_AND_CONF, 0x48,
+	R_92_A_X_PORT_INPUT_REFERENCE_SIGNAL, 0x40,
+	R_93_A_I_PORT_OUTPUT_FORMATS_AND_CONF, 0x84,
+
+	/* This is weird: the datasheet says that you should use 2 as the minimum value, */
+	/* but Hauppauge uses 0, and changing that to 2 causes indeed problems (for 50hz) */
+	/* hoffset low (input), 0x0002 is minimum */
+	R_94_A_HORIZ_INPUT_WINDOW_START, 0x00,
+	R_95_A_HORIZ_INPUT_WINDOW_START_MSB, 0x00,
+
+	/* hsize low (input), 0x02d0 = 720 */
+	R_96_A_HORIZ_INPUT_WINDOW_LENGTH, 0xd0,
+	R_97_A_HORIZ_INPUT_WINDOW_LENGTH_MSB, 0x02,
+
+	R_98_A_VERT_INPUT_WINDOW_START, 0x03,
+	R_99_A_VERT_INPUT_WINDOW_START_MSB, 0x00,
+
+	/* vsize 0x12 = 18 */
+	R_9A_A_VERT_INPUT_WINDOW_LENGTH, 0x12,
+	R_9B_A_VERT_INPUT_WINDOW_LENGTH_MSB, 0x00,
+
+	/* hsize 0x05a0 = 1440 */
+	R_9C_A_HORIZ_OUTPUT_WINDOW_LENGTH, 0xa0,
+	R_9D_A_HORIZ_OUTPUT_WINDOW_LENGTH_MSB, 0x05,	/* hsize hi (output) */
+	R_9E_A_VERT_OUTPUT_WINDOW_LENGTH, 0x12,		/* vsize low (output), 0x12 = 18 */
+	R_9F_A_VERT_OUTPUT_WINDOW_LENGTH_MSB, 0x00,	/* vsize hi (output) */
+
+	/* Task B */
+	R_C0_B_TASK_HANDLING_CNTL, 0x00,
+	R_C1_B_X_PORT_FORMATS_AND_CONF, 0x08,
+	R_C2_B_INPUT_REFERENCE_SIGNAL_DEFINITION, 0x00,
+	R_C3_B_I_PORT_FORMATS_AND_CONF, 0x80,
+
+	/* This is weird: the datasheet says that you should use 2 as the minimum value, */
+	/* but Hauppauge uses 0, and changing that to 2 causes indeed problems (for 50hz) */
+	/* hoffset low (input), 0x0002 is minimum. See comment above. */
+	R_C4_B_HORIZ_INPUT_WINDOW_START, 0x00,
+	R_C5_B_HORIZ_INPUT_WINDOW_START_MSB, 0x00,
+
+	/* hsize 0x02d0 = 720 */
+	R_C6_B_HORIZ_INPUT_WINDOW_LENGTH, 0xd0,
+	R_C7_B_HORIZ_INPUT_WINDOW_LENGTH_MSB, 0x02,
+
+	/* voffset 0x16 = 22 */
+	R_C8_B_VERT_INPUT_WINDOW_START, 0x16,
+	R_C9_B_VERT_INPUT_WINDOW_START_MSB, 0x00,
+
+	/* vsize 0x0120 = 288 */
+	R_CA_B_VERT_INPUT_WINDOW_LENGTH, 0x20,
+	R_CB_B_VERT_INPUT_WINDOW_LENGTH_MSB, 0x01,
+
+	/* hsize 0x02d0 = 720 */
+	R_CC_B_HORIZ_OUTPUT_WINDOW_LENGTH, 0xd0,
+	R_CD_B_HORIZ_OUTPUT_WINDOW_LENGTH_MSB, 0x02,
+
+	R_F0_LFCO_PER_LINE, 0xb0,		/* Set PLL Register. 50hz 625 lines per frame, 27 MHz */
+	R_F1_P_I_PARAM_SELECT, 0x05,		/* low bit with 0xF0, (was 0x05) */
+	R_F5_PULSGEN_LINE_LENGTH, 0xb0,
+	R_F6_PULSE_A_POS_LSB_AND_PULSEGEN_CONFIG, 0x01,
+
+	0x00, 0x00
+};
+
+/* ============== SAA7715 VIDEO templates (end) =======  */
+
+static const unsigned char saa7115_cfg_vbi_on[] = {
+	R_80_GLOBAL_CNTL_1, 0x00,			/* reset tasks */
+	R_88_POWER_SAVE_ADC_PORT_CNTL, 0xd0,		/* reset scaler */
+	R_80_GLOBAL_CNTL_1, 0x30,			/* Activate both tasks */
+	R_88_POWER_SAVE_ADC_PORT_CNTL, 0xf0,		/* activate scaler */
+	R_87_I_PORT_I_O_ENA_OUT_CLK_AND_GATED, 0x01,	/* Enable I-port output */
+
+	0x00, 0x00
+};
+
+static const unsigned char saa7115_cfg_vbi_off[] = {
+	R_80_GLOBAL_CNTL_1, 0x00,			/* reset tasks */
+	R_88_POWER_SAVE_ADC_PORT_CNTL, 0xd0,		/* reset scaler */
+	R_80_GLOBAL_CNTL_1, 0x20,			/* Activate only task "B" */
+	R_88_POWER_SAVE_ADC_PORT_CNTL, 0xf0,		/* activate scaler */
+	R_87_I_PORT_I_O_ENA_OUT_CLK_AND_GATED, 0x01,	/* Enable I-port output */
+
+	0x00, 0x00
+};
+
+
+static const unsigned char saa7115_init_misc[] = {
+	R_81_V_SYNC_FLD_ID_SRC_SEL_AND_RETIMED_V_F, 0x01,
+	R_83_X_PORT_I_O_ENA_AND_OUT_CLK, 0x01,
+	R_84_I_PORT_SIGNAL_DEF, 0x20,
+	R_85_I_PORT_SIGNAL_POLAR, 0x21,
+	R_86_I_PORT_FIFO_FLAG_CNTL_AND_ARBIT, 0xc5,
+	R_87_I_PORT_I_O_ENA_OUT_CLK_AND_GATED, 0x01,
+
+	/* Task A */
+	R_A0_A_HORIZ_PRESCALING, 0x01,
+	R_A1_A_ACCUMULATION_LENGTH, 0x00,
+	R_A2_A_PRESCALER_DC_GAIN_AND_FIR_PREFILTER, 0x00,
+
+	/* Configure controls at nominal value*/
+	R_A4_A_LUMA_BRIGHTNESS_CNTL, 0x80,
+	R_A5_A_LUMA_CONTRAST_CNTL, 0x40,
+	R_A6_A_CHROMA_SATURATION_CNTL, 0x40,
+
+	/* note: 2 x zoom ensures that VBI lines have same length as video lines. */
+	R_A8_A_HORIZ_LUMA_SCALING_INC, 0x00,
+	R_A9_A_HORIZ_LUMA_SCALING_INC_MSB, 0x02,
+
+	R_AA_A_HORIZ_LUMA_PHASE_OFF, 0x00,
+
+	/* must be horiz lum scaling / 2 */
+	R_AC_A_HORIZ_CHROMA_SCALING_INC, 0x00,
+	R_AD_A_HORIZ_CHROMA_SCALING_INC_MSB, 0x01,
+
+	/* must be offset luma / 2 */
+	R_AE_A_HORIZ_CHROMA_PHASE_OFF, 0x00,
+
+	R_B0_A_VERT_LUMA_SCALING_INC, 0x00,
+	R_B1_A_VERT_LUMA_SCALING_INC_MSB, 0x04,
+
+	R_B2_A_VERT_CHROMA_SCALING_INC, 0x00,
+	R_B3_A_VERT_CHROMA_SCALING_INC_MSB, 0x04,
+
+	R_B4_A_VERT_SCALING_MODE_CNTL, 0x01,
+
+	R_B8_A_VERT_CHROMA_PHASE_OFF_00, 0x00,
+	R_B9_A_VERT_CHROMA_PHASE_OFF_01, 0x00,
+	R_BA_A_VERT_CHROMA_PHASE_OFF_10, 0x00,
+	R_BB_A_VERT_CHROMA_PHASE_OFF_11, 0x00,
+
+	R_BC_A_VERT_LUMA_PHASE_OFF_00, 0x00,
+	R_BD_A_VERT_LUMA_PHASE_OFF_01, 0x00,
+	R_BE_A_VERT_LUMA_PHASE_OFF_10, 0x00,
+	R_BF_A_VERT_LUMA_PHASE_OFF_11, 0x00,
+
+	/* Task B */
+	R_D0_B_HORIZ_PRESCALING, 0x01,
+	R_D1_B_ACCUMULATION_LENGTH, 0x00,
+	R_D2_B_PRESCALER_DC_GAIN_AND_FIR_PREFILTER, 0x00,
+
+	/* Configure controls at nominal value*/
+	R_D4_B_LUMA_BRIGHTNESS_CNTL, 0x80,
+	R_D5_B_LUMA_CONTRAST_CNTL, 0x40,
+	R_D6_B_CHROMA_SATURATION_CNTL, 0x40,
+
+	/* hor lum scaling 0x0400 = 1 */
+	R_D8_B_HORIZ_LUMA_SCALING_INC, 0x00,
+	R_D9_B_HORIZ_LUMA_SCALING_INC_MSB, 0x04,
+
+	R_DA_B_HORIZ_LUMA_PHASE_OFF, 0x00,
+
+	/* must be hor lum scaling / 2 */
+	R_DC_B_HORIZ_CHROMA_SCALING, 0x00,
+	R_DD_B_HORIZ_CHROMA_SCALING_MSB, 0x02,
+
+	/* must be offset luma / 2 */
+	R_DE_B_HORIZ_PHASE_OFFSET_CRHOMA, 0x00,
+
+	R_E0_B_VERT_LUMA_SCALING_INC, 0x00,
+	R_E1_B_VERT_LUMA_SCALING_INC_MSB, 0x04,
+
+	R_E2_B_VERT_CHROMA_SCALING_INC, 0x00,
+	R_E3_B_VERT_CHROMA_SCALING_INC_MSB, 0x04,
+
+	R_E4_B_VERT_SCALING_MODE_CNTL, 0x01,
+
+	R_E8_B_VERT_CHROMA_PHASE_OFF_00, 0x00,
+	R_E9_B_VERT_CHROMA_PHASE_OFF_01, 0x00,
+	R_EA_B_VERT_CHROMA_PHASE_OFF_10, 0x00,
+	R_EB_B_VERT_CHROMA_PHASE_OFF_11, 0x00,
+
+	R_EC_B_VERT_LUMA_PHASE_OFF_00, 0x00,
+	R_ED_B_VERT_LUMA_PHASE_OFF_01, 0x00,
+	R_EE_B_VERT_LUMA_PHASE_OFF_10, 0x00,
+	R_EF_B_VERT_LUMA_PHASE_OFF_11, 0x00,
+
+	R_F2_NOMINAL_PLL2_DTO, 0x50,		/* crystal clock = 24.576 MHz, target = 27MHz */
+	R_F3_PLL_INCREMENT, 0x46,
+	R_F4_PLL2_STATUS, 0x00,
+	R_F7_PULSE_A_POS_MSB, 0x4b,		/* not the recommended settings! */
+	R_F8_PULSE_B_POS, 0x00,
+	R_F9_PULSE_B_POS_MSB, 0x4b,
+	R_FA_PULSE_C_POS, 0x00,
+	R_FB_PULSE_C_POS_MSB, 0x4b,
+
+	/* PLL2 lock detection settings: 71 lines 50% phase error */
+	R_FF_S_PLL_MAX_PHASE_ERR_THRESH_NUM_LINES, 0x88,
+
+	/* Turn off VBI */
+	R_40_SLICER_CNTL_1, 0x20,             /* No framing code errors allowed. */
+	R_41_LCR_BASE, 0xff,
+	R_41_LCR_BASE+1, 0xff,
+	R_41_LCR_BASE+2, 0xff,
+	R_41_LCR_BASE+3, 0xff,
+	R_41_LCR_BASE+4, 0xff,
+	R_41_LCR_BASE+5, 0xff,
+	R_41_LCR_BASE+6, 0xff,
+	R_41_LCR_BASE+7, 0xff,
+	R_41_LCR_BASE+8, 0xff,
+	R_41_LCR_BASE+9, 0xff,
+	R_41_LCR_BASE+10, 0xff,
+	R_41_LCR_BASE+11, 0xff,
+	R_41_LCR_BASE+12, 0xff,
+	R_41_LCR_BASE+13, 0xff,
+	R_41_LCR_BASE+14, 0xff,
+	R_41_LCR_BASE+15, 0xff,
+	R_41_LCR_BASE+16, 0xff,
+	R_41_LCR_BASE+17, 0xff,
+	R_41_LCR_BASE+18, 0xff,
+	R_41_LCR_BASE+19, 0xff,
+	R_41_LCR_BASE+20, 0xff,
+	R_41_LCR_BASE+21, 0xff,
+	R_41_LCR_BASE+22, 0xff,
+	R_58_PROGRAM_FRAMING_CODE, 0x40,
+	R_59_H_OFF_FOR_SLICER, 0x47,
+	R_5B_FLD_OFF_AND_MSB_FOR_H_AND_V_OFF, 0x83,
+	R_5D_DID, 0xbd,
+	R_5E_SDID, 0x35,
+
+	R_02_INPUT_CNTL_1, 0xc4, /* input tuner -> input 4, amplifier active */
+
+	R_80_GLOBAL_CNTL_1, 0x20,		/* enable task B */
+	R_88_POWER_SAVE_ADC_PORT_CNTL, 0xd0,
+	R_88_POWER_SAVE_ADC_PORT_CNTL, 0xf0,
+	0x00, 0x00
+};
+
+static int saa711x_odd_parity(u8 c)
+{
+	c ^= (c >> 4);
+	c ^= (c >> 2);
+	c ^= (c >> 1);
+
+	return c & 1;
+}
+
+static int saa711x_decode_vps(u8 *dst, u8 *p)
+{
+	static const u8 biphase_tbl[] = {
+		0xf0, 0x78, 0x70, 0xf0, 0xb4, 0x3c, 0x34, 0xb4,
+		0xb0, 0x38, 0x30, 0xb0, 0xf0, 0x78, 0x70, 0xf0,
+		0xd2, 0x5a, 0x52, 0xd2, 0x96, 0x1e, 0x16, 0x96,
+		0x92, 0x1a, 0x12, 0x92, 0xd2, 0x5a, 0x52, 0xd2,
+		0xd0, 0x58, 0x50, 0xd0, 0x94, 0x1c, 0x14, 0x94,
+		0x90, 0x18, 0x10, 0x90, 0xd0, 0x58, 0x50, 0xd0,
+		0xf0, 0x78, 0x70, 0xf0, 0xb4, 0x3c, 0x34, 0xb4,
+		0xb0, 0x38, 0x30, 0xb0, 0xf0, 0x78, 0x70, 0xf0,
+		0xe1, 0x69, 0x61, 0xe1, 0xa5, 0x2d, 0x25, 0xa5,
+		0xa1, 0x29, 0x21, 0xa1, 0xe1, 0x69, 0x61, 0xe1,
+		0xc3, 0x4b, 0x43, 0xc3, 0x87, 0x0f, 0x07, 0x87,
+		0x83, 0x0b, 0x03, 0x83, 0xc3, 0x4b, 0x43, 0xc3,
+		0xc1, 0x49, 0x41, 0xc1, 0x85, 0x0d, 0x05, 0x85,
+		0x81, 0x09, 0x01, 0x81, 0xc1, 0x49, 0x41, 0xc1,
+		0xe1, 0x69, 0x61, 0xe1, 0xa5, 0x2d, 0x25, 0xa5,
+		0xa1, 0x29, 0x21, 0xa1, 0xe1, 0x69, 0x61, 0xe1,
+		0xe0, 0x68, 0x60, 0xe0, 0xa4, 0x2c, 0x24, 0xa4,
+		0xa0, 0x28, 0x20, 0xa0, 0xe0, 0x68, 0x60, 0xe0,
+		0xc2, 0x4a, 0x42, 0xc2, 0x86, 0x0e, 0x06, 0x86,
+		0x82, 0x0a, 0x02, 0x82, 0xc2, 0x4a, 0x42, 0xc2,
+		0xc0, 0x48, 0x40, 0xc0, 0x84, 0x0c, 0x04, 0x84,
+		0x80, 0x08, 0x00, 0x80, 0xc0, 0x48, 0x40, 0xc0,
+		0xe0, 0x68, 0x60, 0xe0, 0xa4, 0x2c, 0x24, 0xa4,
+		0xa0, 0x28, 0x20, 0xa0, 0xe0, 0x68, 0x60, 0xe0,
+		0xf0, 0x78, 0x70, 0xf0, 0xb4, 0x3c, 0x34, 0xb4,
+		0xb0, 0x38, 0x30, 0xb0, 0xf0, 0x78, 0x70, 0xf0,
+		0xd2, 0x5a, 0x52, 0xd2, 0x96, 0x1e, 0x16, 0x96,
+		0x92, 0x1a, 0x12, 0x92, 0xd2, 0x5a, 0x52, 0xd2,
+		0xd0, 0x58, 0x50, 0xd0, 0x94, 0x1c, 0x14, 0x94,
+		0x90, 0x18, 0x10, 0x90, 0xd0, 0x58, 0x50, 0xd0,
+		0xf0, 0x78, 0x70, 0xf0, 0xb4, 0x3c, 0x34, 0xb4,
+		0xb0, 0x38, 0x30, 0xb0, 0xf0, 0x78, 0x70, 0xf0,
+	};
+	int i;
+	u8 c, err = 0;
+
+	for (i = 0; i < 2 * 13; i += 2) {
+		err |= biphase_tbl[p[i]] | biphase_tbl[p[i + 1]];
+		c = (biphase_tbl[p[i + 1]] & 0xf) | ((biphase_tbl[p[i]] & 0xf) << 4);
+		dst[i / 2] = c;
+	}
+	return err & 0xf0;
+}
+
+static int saa711x_decode_wss(u8 *p)
+{
+	static const int wss_bits[8] = {
+		0, 0, 0, 1, 0, 1, 1, 1
+	};
+	unsigned char parity;
+	int wss = 0;
+	int i;
+
+	for (i = 0; i < 16; i++) {
+		int b1 = wss_bits[p[i] & 7];
+		int b2 = wss_bits[(p[i] >> 3) & 7];
+
+		if (b1 == b2)
+			return -1;
+		wss |= b2 << i;
+	}
+	parity = wss & 15;
+	parity ^= parity >> 2;
+	parity ^= parity >> 1;
+
+	if (!(parity & 1))
+		return -1;
+
+	return wss;
+}
+
+static int saa711x_s_clock_freq(struct v4l2_subdev *sd, u32 freq)
+{
+	struct saa711x_state *state = to_state(sd);
+	u32 acpf;
+	u32 acni;
+	u32 hz;
+	u64 f;
+	u8 acc = 0;	/* reg 0x3a, audio clock control */
+
+	/* Checks for chips that don't have audio clock (saa7111, saa7113) */
+	if (!saa711x_has_reg(state->ident, R_30_AUD_MAST_CLK_CYCLES_PER_FIELD))
+		return 0;
+
+	v4l2_dbg(1, debug, sd, "set audio clock freq: %d\n", freq);
+
+	/* sanity check */
+	if (freq < 32000 || freq > 48000)
+		return -EINVAL;
+
+	/* hz is the refresh rate times 100 */
+	hz = (state->std & V4L2_STD_525_60) ? 5994 : 5000;
+	/* acpf = (256 * freq) / field_frequency == (256 * 100 * freq) / hz */
+	acpf = (25600 * freq) / hz;
+	/* acni = (256 * freq * 2^23) / crystal_frequency =
+		  (freq * 2^(8+23)) / crystal_frequency =
+		  (freq << 31) / crystal_frequency */
+	f = freq;
+	f = f << 31;
+	do_div(f, state->crystal_freq);
+	acni = f;
+	if (state->ucgc) {
+		acpf = acpf * state->cgcdiv / 16;
+		acni = acni * state->cgcdiv / 16;
+		acc = 0x80;
+		if (state->cgcdiv == 3)
+			acc |= 0x40;
+	}
+	if (state->apll)
+		acc |= 0x08;
+
+	if (state->double_asclk) {
+		acpf <<= 1;
+		acni <<= 1;
+	}
+	saa711x_write(sd, R_38_CLK_RATIO_AMXCLK_TO_ASCLK, 0x03);
+	saa711x_write(sd, R_39_CLK_RATIO_ASCLK_TO_ALRCLK, 0x10 << state->double_asclk);
+	saa711x_write(sd, R_3A_AUD_CLK_GEN_BASIC_SETUP, acc);
+
+	saa711x_write(sd, R_30_AUD_MAST_CLK_CYCLES_PER_FIELD, acpf & 0xff);
+	saa711x_write(sd, R_30_AUD_MAST_CLK_CYCLES_PER_FIELD+1,
+							(acpf >> 8) & 0xff);
+	saa711x_write(sd, R_30_AUD_MAST_CLK_CYCLES_PER_FIELD+2,
+							(acpf >> 16) & 0x03);
+
+	saa711x_write(sd, R_34_AUD_MAST_CLK_NOMINAL_INC, acni & 0xff);
+	saa711x_write(sd, R_34_AUD_MAST_CLK_NOMINAL_INC+1, (acni >> 8) & 0xff);
+	saa711x_write(sd, R_34_AUD_MAST_CLK_NOMINAL_INC+2, (acni >> 16) & 0x3f);
+	state->audclk_freq = freq;
+	return 0;
+}
+
+static int saa711x_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+	struct saa711x_state *state = to_state(sd);
+
+	switch (ctrl->id) {
+	case V4L2_CID_CHROMA_AGC:
+		/* chroma gain cluster */
+		if (state->agc->val)
+			state->gain->val =
+				saa711x_read(sd, R_0F_CHROMA_GAIN_CNTL) & 0x7f;
+		break;
+	}
+	return 0;
+}
+
+static int saa711x_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+	struct saa711x_state *state = to_state(sd);
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		saa711x_write(sd, R_0A_LUMA_BRIGHT_CNTL, ctrl->val);
+		break;
+
+	case V4L2_CID_CONTRAST:
+		saa711x_write(sd, R_0B_LUMA_CONTRAST_CNTL, ctrl->val);
+		break;
+
+	case V4L2_CID_SATURATION:
+		saa711x_write(sd, R_0C_CHROMA_SAT_CNTL, ctrl->val);
+		break;
+
+	case V4L2_CID_HUE:
+		saa711x_write(sd, R_0D_CHROMA_HUE_CNTL, ctrl->val);
+		break;
+
+	case V4L2_CID_CHROMA_AGC:
+		/* chroma gain cluster */
+		if (state->agc->val)
+			saa711x_write(sd, R_0F_CHROMA_GAIN_CNTL, state->gain->val);
+		else
+			saa711x_write(sd, R_0F_CHROMA_GAIN_CNTL, state->gain->val | 0x80);
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int saa711x_set_size(struct v4l2_subdev *sd, int width, int height)
+{
+	struct saa711x_state *state = to_state(sd);
+	int HPSC, HFSC;
+	int VSCY;
+	int res;
+	int is_50hz = state->std & V4L2_STD_625_50;
+	int Vsrc = is_50hz ? 576 : 480;
+
+	v4l2_dbg(1, debug, sd, "decoder set size to %ix%i\n", width, height);
+
+	/* FIXME need better bounds checking here */
+	if ((width < 1) || (width > 1440))
+		return -EINVAL;
+	if ((height < 1) || (height > Vsrc))
+		return -EINVAL;
+
+	if (!saa711x_has_reg(state->ident, R_D0_B_HORIZ_PRESCALING)) {
+		/* Decoder only supports 720 columns and 480 or 576 lines */
+		if (width != 720)
+			return -EINVAL;
+		if (height != Vsrc)
+			return -EINVAL;
+	}
+
+	state->width = width;
+	state->height = height;
+
+	if (!saa711x_has_reg(state->ident, R_CC_B_HORIZ_OUTPUT_WINDOW_LENGTH))
+		return 0;
+
+	/* probably have a valid size, let's set it */
+	/* Set output width/height */
+	/* width */
+
+	saa711x_write(sd, R_CC_B_HORIZ_OUTPUT_WINDOW_LENGTH,
+					(u8) (width & 0xff));
+	saa711x_write(sd, R_CD_B_HORIZ_OUTPUT_WINDOW_LENGTH_MSB,
+					(u8) ((width >> 8) & 0xff));
+
+	/* Vertical Scaling uses height/2 */
+	res = height / 2;
+
+	/* On 60Hz, it is using a higher Vertical Output Size */
+	if (!is_50hz)
+		res += (VRES_60HZ - 480) >> 1;
+
+		/* height */
+	saa711x_write(sd, R_CE_B_VERT_OUTPUT_WINDOW_LENGTH,
+					(u8) (res & 0xff));
+	saa711x_write(sd, R_CF_B_VERT_OUTPUT_WINDOW_LENGTH_MSB,
+					(u8) ((res >> 8) & 0xff));
+
+	/* Scaling settings */
+	/* Hprescaler is floor(inres/outres) */
+	HPSC = (int)(720 / width);
+	/* 0 is not allowed (div. by zero) */
+	HPSC = HPSC ? HPSC : 1;
+	HFSC = (int)((1024 * 720) / (HPSC * width));
+	/* FIXME hardcodes to "Task B"
+	 * write H prescaler integer */
+	saa711x_write(sd, R_D0_B_HORIZ_PRESCALING,
+				(u8) (HPSC & 0x3f));
+
+	v4l2_dbg(1, debug, sd, "Hpsc: 0x%05x, Hfsc: 0x%05x\n", HPSC, HFSC);
+	/* write H fine-scaling (luminance) */
+	saa711x_write(sd, R_D8_B_HORIZ_LUMA_SCALING_INC,
+				(u8) (HFSC & 0xff));
+	saa711x_write(sd, R_D9_B_HORIZ_LUMA_SCALING_INC_MSB,
+				(u8) ((HFSC >> 8) & 0xff));
+	/* write H fine-scaling (chrominance)
+	 * must be lum/2, so i'll just bitshift :) */
+	saa711x_write(sd, R_DC_B_HORIZ_CHROMA_SCALING,
+				(u8) ((HFSC >> 1) & 0xff));
+	saa711x_write(sd, R_DD_B_HORIZ_CHROMA_SCALING_MSB,
+				(u8) ((HFSC >> 9) & 0xff));
+
+	VSCY = (int)((1024 * Vsrc) / height);
+	v4l2_dbg(1, debug, sd, "Vsrc: %d, Vscy: 0x%05x\n", Vsrc, VSCY);
+
+	/* Correct Contrast and Luminance */
+	saa711x_write(sd, R_D5_B_LUMA_CONTRAST_CNTL,
+					(u8) (64 * 1024 / VSCY));
+	saa711x_write(sd, R_D6_B_CHROMA_SATURATION_CNTL,
+					(u8) (64 * 1024 / VSCY));
+
+		/* write V fine-scaling (luminance) */
+	saa711x_write(sd, R_E0_B_VERT_LUMA_SCALING_INC,
+					(u8) (VSCY & 0xff));
+	saa711x_write(sd, R_E1_B_VERT_LUMA_SCALING_INC_MSB,
+					(u8) ((VSCY >> 8) & 0xff));
+		/* write V fine-scaling (chrominance) */
+	saa711x_write(sd, R_E2_B_VERT_CHROMA_SCALING_INC,
+					(u8) (VSCY & 0xff));
+	saa711x_write(sd, R_E3_B_VERT_CHROMA_SCALING_INC_MSB,
+					(u8) ((VSCY >> 8) & 0xff));
+
+	saa711x_writeregs(sd, saa7115_cfg_reset_scaler);
+
+	/* Activates task "B" */
+	saa711x_write(sd, R_80_GLOBAL_CNTL_1,
+				saa711x_read(sd, R_80_GLOBAL_CNTL_1) | 0x20);
+
+	return 0;
+}
+
+static void saa711x_set_v4lstd(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct saa711x_state *state = to_state(sd);
+
+	/* Prevent unnecessary standard changes. During a standard
+	   change the I-Port is temporarily disabled. Any devices
+	   reading from that port can get confused.
+	   Note that s_std is also used to switch from
+	   radio to TV mode, so if a s_std is broadcast to
+	   all I2C devices then you do not want to have an unwanted
+	   side-effect here. */
+	if (std == state->std)
+		return;
+
+	state->std = std;
+
+	// This works for NTSC-M, SECAM-L and the 50Hz PAL variants.
+	if (std & V4L2_STD_525_60) {
+		v4l2_dbg(1, debug, sd, "decoder set standard 60 Hz\n");
+		if (state->ident == GM7113C) {
+			u8 reg = saa711x_read(sd, R_08_SYNC_CNTL);
+			reg &= ~(SAA7113_R_08_FSEL | SAA7113_R_08_AUFD);
+			reg |= SAA7113_R_08_FSEL;
+			saa711x_write(sd, R_08_SYNC_CNTL, reg);
+		} else {
+			saa711x_writeregs(sd, saa7115_cfg_60hz_video);
+		}
+		saa711x_set_size(sd, 720, 480);
+	} else {
+		v4l2_dbg(1, debug, sd, "decoder set standard 50 Hz\n");
+		if (state->ident == GM7113C) {
+			u8 reg = saa711x_read(sd, R_08_SYNC_CNTL);
+			reg &= ~(SAA7113_R_08_FSEL | SAA7113_R_08_AUFD);
+			saa711x_write(sd, R_08_SYNC_CNTL, reg);
+		} else {
+			saa711x_writeregs(sd, saa7115_cfg_50hz_video);
+		}
+		saa711x_set_size(sd, 720, 576);
+	}
+
+	/* Register 0E - Bits D6-D4 on NO-AUTO mode
+		(SAA7111 and SAA7113 doesn't have auto mode)
+	    50 Hz / 625 lines           60 Hz / 525 lines
+	000 PAL BGDHI (4.43Mhz)         NTSC M (3.58MHz)
+	001 NTSC 4.43 (50 Hz)           PAL 4.43 (60 Hz)
+	010 Combination-PAL N (3.58MHz) NTSC 4.43 (60 Hz)
+	011 NTSC N (3.58MHz)            PAL M (3.58MHz)
+	100 reserved                    NTSC-Japan (3.58MHz)
+	*/
+	if (state->ident <= SAA7113 ||
+	    state->ident == GM7113C) {
+		u8 reg = saa711x_read(sd, R_0E_CHROMA_CNTL_1) & 0x8f;
+
+		if (std == V4L2_STD_PAL_M) {
+			reg |= 0x30;
+		} else if (std == V4L2_STD_PAL_Nc) {
+			reg |= 0x20;
+		} else if (std == V4L2_STD_PAL_60) {
+			reg |= 0x10;
+		} else if (std == V4L2_STD_NTSC_M_JP) {
+			reg |= 0x40;
+		} else if (std & V4L2_STD_SECAM) {
+			reg |= 0x50;
+		}
+		saa711x_write(sd, R_0E_CHROMA_CNTL_1, reg);
+	} else {
+		/* restart task B if needed */
+		int taskb = saa711x_read(sd, R_80_GLOBAL_CNTL_1) & 0x10;
+
+		if (taskb && state->ident == SAA7114)
+			saa711x_writeregs(sd, saa7115_cfg_vbi_on);
+
+		/* switch audio mode too! */
+		saa711x_s_clock_freq(sd, state->audclk_freq);
+	}
+}
+
+/* setup the sliced VBI lcr registers according to the sliced VBI format */
+static void saa711x_set_lcr(struct v4l2_subdev *sd, struct v4l2_sliced_vbi_format *fmt)
+{
+	struct saa711x_state *state = to_state(sd);
+	int is_50hz = (state->std & V4L2_STD_625_50);
+	u8 lcr[24];
+	int i, x;
+
+#if 1
+	/* saa7113/7114/7118 VBI support are experimental */
+	if (!saa711x_has_reg(state->ident, R_41_LCR_BASE))
+		return;
+
+#else
+	/* SAA7113 and SAA7118 also should support VBI - Need testing */
+	if (state->ident != SAA7115)
+		return;
+#endif
+
+	for (i = 0; i <= 23; i++)
+		lcr[i] = 0xff;
+
+	if (fmt == NULL) {
+		/* raw VBI */
+		if (is_50hz)
+			for (i = 6; i <= 23; i++)
+				lcr[i] = 0xdd;
+		else
+			for (i = 10; i <= 21; i++)
+				lcr[i] = 0xdd;
+	} else {
+		/* sliced VBI */
+		/* first clear lines that cannot be captured */
+		if (is_50hz) {
+			for (i = 0; i <= 5; i++)
+				fmt->service_lines[0][i] =
+					fmt->service_lines[1][i] = 0;
+		}
+		else {
+			for (i = 0; i <= 9; i++)
+				fmt->service_lines[0][i] =
+					fmt->service_lines[1][i] = 0;
+			for (i = 22; i <= 23; i++)
+				fmt->service_lines[0][i] =
+					fmt->service_lines[1][i] = 0;
+		}
+
+		/* Now set the lcr values according to the specified service */
+		for (i = 6; i <= 23; i++) {
+			lcr[i] = 0;
+			for (x = 0; x <= 1; x++) {
+				switch (fmt->service_lines[1-x][i]) {
+					case 0:
+						lcr[i] |= 0xf << (4 * x);
+						break;
+					case V4L2_SLICED_TELETEXT_B:
+						lcr[i] |= 1 << (4 * x);
+						break;
+					case V4L2_SLICED_CAPTION_525:
+						lcr[i] |= 4 << (4 * x);
+						break;
+					case V4L2_SLICED_WSS_625:
+						lcr[i] |= 5 << (4 * x);
+						break;
+					case V4L2_SLICED_VPS:
+						lcr[i] |= 7 << (4 * x);
+						break;
+				}
+			}
+		}
+	}
+
+	/* write the lcr registers */
+	for (i = 2; i <= 23; i++) {
+		saa711x_write(sd, i - 2 + R_41_LCR_BASE, lcr[i]);
+	}
+
+	/* enable/disable raw VBI capturing */
+	saa711x_writeregs(sd, fmt == NULL ?
+				saa7115_cfg_vbi_on :
+				saa7115_cfg_vbi_off);
+}
+
+static int saa711x_g_sliced_fmt(struct v4l2_subdev *sd, struct v4l2_sliced_vbi_format *sliced)
+{
+	static const u16 lcr2vbi[] = {
+		0, V4L2_SLICED_TELETEXT_B, 0,	/* 1 */
+		0, V4L2_SLICED_CAPTION_525,	/* 4 */
+		V4L2_SLICED_WSS_625, 0,		/* 5 */
+		V4L2_SLICED_VPS, 0, 0, 0, 0,	/* 7 */
+		0, 0, 0, 0
+	};
+	int i;
+
+	memset(sliced->service_lines, 0, sizeof(sliced->service_lines));
+	sliced->service_set = 0;
+	/* done if using raw VBI */
+	if (saa711x_read(sd, R_80_GLOBAL_CNTL_1) & 0x10)
+		return 0;
+	for (i = 2; i <= 23; i++) {
+		u8 v = saa711x_read(sd, i - 2 + R_41_LCR_BASE);
+
+		sliced->service_lines[0][i] = lcr2vbi[v >> 4];
+		sliced->service_lines[1][i] = lcr2vbi[v & 0xf];
+		sliced->service_set |=
+			sliced->service_lines[0][i] | sliced->service_lines[1][i];
+	}
+	return 0;
+}
+
+static int saa711x_s_raw_fmt(struct v4l2_subdev *sd, struct v4l2_vbi_format *fmt)
+{
+	saa711x_set_lcr(sd, NULL);
+	return 0;
+}
+
+static int saa711x_s_sliced_fmt(struct v4l2_subdev *sd, struct v4l2_sliced_vbi_format *fmt)
+{
+	saa711x_set_lcr(sd, fmt);
+	return 0;
+}
+
+static int saa711x_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+
+	if (format->pad || fmt->code != MEDIA_BUS_FMT_FIXED)
+		return -EINVAL;
+	fmt->field = V4L2_FIELD_INTERLACED;
+	fmt->colorspace = V4L2_COLORSPACE_SMPTE170M;
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+		return 0;
+	return saa711x_set_size(sd, fmt->width, fmt->height);
+}
+
+/* Decode the sliced VBI data stream as created by the saa7115.
+   The format is described in the saa7115 datasheet in Tables 25 and 26
+   and in Figure 33.
+   The current implementation uses SAV/EAV codes and not the ancillary data
+   headers. The vbi->p pointer points to the R_5E_SDID byte right after the SAV
+   code. */
+static int saa711x_decode_vbi_line(struct v4l2_subdev *sd, struct v4l2_decode_vbi_line *vbi)
+{
+	struct saa711x_state *state = to_state(sd);
+	static const char vbi_no_data_pattern[] = {
+		0xa0, 0xa0, 0xa0, 0xa0, 0xa0, 0xa0, 0xa0, 0xa0, 0xa0, 0xa0
+	};
+	u8 *p = vbi->p;
+	u32 wss;
+	int id1, id2;   /* the ID1 and ID2 bytes from the internal header */
+
+	vbi->type = 0;  /* mark result as a failure */
+	id1 = p[2];
+	id2 = p[3];
+	/* Note: the field bit is inverted for 60 Hz video */
+	if (state->std & V4L2_STD_525_60)
+		id1 ^= 0x40;
+
+	/* Skip internal header, p now points to the start of the payload */
+	p += 4;
+	vbi->p = p;
+
+	/* calculate field and line number of the VBI packet (1-23) */
+	vbi->is_second_field = ((id1 & 0x40) != 0);
+	vbi->line = (id1 & 0x3f) << 3;
+	vbi->line |= (id2 & 0x70) >> 4;
+
+	/* Obtain data type */
+	id2 &= 0xf;
+
+	/* If the VBI slicer does not detect any signal it will fill up
+	   the payload buffer with 0xa0 bytes. */
+	if (!memcmp(p, vbi_no_data_pattern, sizeof(vbi_no_data_pattern)))
+		return 0;
+
+	/* decode payloads */
+	switch (id2) {
+	case 1:
+		vbi->type = V4L2_SLICED_TELETEXT_B;
+		break;
+	case 4:
+		if (!saa711x_odd_parity(p[0]) || !saa711x_odd_parity(p[1]))
+			return 0;
+		vbi->type = V4L2_SLICED_CAPTION_525;
+		break;
+	case 5:
+		wss = saa711x_decode_wss(p);
+		if (wss == -1)
+			return 0;
+		p[0] = wss & 0xff;
+		p[1] = wss >> 8;
+		vbi->type = V4L2_SLICED_WSS_625;
+		break;
+	case 7:
+		if (saa711x_decode_vps(p, p) != 0)
+			return 0;
+		vbi->type = V4L2_SLICED_VPS;
+		break;
+	default:
+		break;
+	}
+	return 0;
+}
+
+/* ============ SAA7115 AUDIO settings (end) ============= */
+
+static int saa711x_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
+{
+	struct saa711x_state *state = to_state(sd);
+	int status;
+
+	if (state->radio)
+		return 0;
+	status = saa711x_read(sd, R_1F_STATUS_BYTE_2_VD_DEC);
+
+	v4l2_dbg(1, debug, sd, "status: 0x%02x\n", status);
+	vt->signal = ((status & (1 << 6)) == 0) ? 0xffff : 0x0;
+	return 0;
+}
+
+static int saa711x_s_std(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct saa711x_state *state = to_state(sd);
+
+	state->radio = 0;
+	saa711x_set_v4lstd(sd, std);
+	return 0;
+}
+
+static int saa711x_s_radio(struct v4l2_subdev *sd)
+{
+	struct saa711x_state *state = to_state(sd);
+
+	state->radio = 1;
+	return 0;
+}
+
+static int saa711x_s_routing(struct v4l2_subdev *sd,
+			     u32 input, u32 output, u32 config)
+{
+	struct saa711x_state *state = to_state(sd);
+	u8 mask = (state->ident <= SAA7111A) ? 0xf8 : 0xf0;
+
+	v4l2_dbg(1, debug, sd, "decoder set input %d output %d\n",
+		input, output);
+
+	/* saa7111/3 does not have these inputs */
+	if ((state->ident <= SAA7113 ||
+	     state->ident == GM7113C) &&
+	    (input == SAA7115_COMPOSITE4 ||
+	     input == SAA7115_COMPOSITE5)) {
+		return -EINVAL;
+	}
+	if (input > SAA7115_SVIDEO3)
+		return -EINVAL;
+	if (state->input == input && state->output == output)
+		return 0;
+	v4l2_dbg(1, debug, sd, "now setting %s input %s output\n",
+		(input >= SAA7115_SVIDEO0) ? "S-Video" : "Composite",
+		(output == SAA7115_IPORT_ON) ? "iport on" : "iport off");
+	state->input = input;
+
+	/* saa7111 has slightly different input numbering */
+	if (state->ident <= SAA7111A) {
+		if (input >= SAA7115_COMPOSITE4)
+			input -= 2;
+		/* saa7111 specific */
+		saa711x_write(sd, R_10_CHROMA_CNTL_2,
+				(saa711x_read(sd, R_10_CHROMA_CNTL_2) & 0x3f) |
+				((output & 0xc0) ^ 0x40));
+		saa711x_write(sd, R_13_RT_X_PORT_OUT_CNTL,
+				(saa711x_read(sd, R_13_RT_X_PORT_OUT_CNTL) & 0xf0) |
+				((output & 2) ? 0x0a : 0));
+	}
+
+	/* select mode */
+	saa711x_write(sd, R_02_INPUT_CNTL_1,
+		      (saa711x_read(sd, R_02_INPUT_CNTL_1) & mask) |
+		       input);
+
+	/* bypass chrominance trap for S-Video modes */
+	saa711x_write(sd, R_09_LUMA_CNTL,
+			(saa711x_read(sd, R_09_LUMA_CNTL) & 0x7f) |
+			(state->input >= SAA7115_SVIDEO0 ? 0x80 : 0x0));
+
+	state->output = output;
+	if (state->ident == SAA7114 ||
+			state->ident == SAA7115) {
+		saa711x_write(sd, R_83_X_PORT_I_O_ENA_AND_OUT_CLK,
+				(saa711x_read(sd, R_83_X_PORT_I_O_ENA_AND_OUT_CLK) & 0xfe) |
+				(state->output & 0x01));
+	}
+	if (state->ident > SAA7111A) {
+		if (config & SAA7115_IDQ_IS_DEFAULT)
+			saa711x_write(sd, R_85_I_PORT_SIGNAL_POLAR, 0x20);
+		else
+			saa711x_write(sd, R_85_I_PORT_SIGNAL_POLAR, 0x21);
+	}
+	return 0;
+}
+
+static int saa711x_s_gpio(struct v4l2_subdev *sd, u32 val)
+{
+	struct saa711x_state *state = to_state(sd);
+
+	if (state->ident > SAA7111A)
+		return -EINVAL;
+	saa711x_write(sd, 0x11, (saa711x_read(sd, 0x11) & 0x7f) |
+		(val ? 0x80 : 0));
+	return 0;
+}
+
+static int saa711x_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct saa711x_state *state = to_state(sd);
+
+	v4l2_dbg(1, debug, sd, "%s output\n",
+			enable ? "enable" : "disable");
+
+	if (state->enable == enable)
+		return 0;
+	state->enable = enable;
+	if (!saa711x_has_reg(state->ident, R_87_I_PORT_I_O_ENA_OUT_CLK_AND_GATED))
+		return 0;
+	saa711x_write(sd, R_87_I_PORT_I_O_ENA_OUT_CLK_AND_GATED, state->enable);
+	return 0;
+}
+
+static int saa711x_s_crystal_freq(struct v4l2_subdev *sd, u32 freq, u32 flags)
+{
+	struct saa711x_state *state = to_state(sd);
+
+	if (freq != SAA7115_FREQ_32_11_MHZ && freq != SAA7115_FREQ_24_576_MHZ)
+		return -EINVAL;
+	state->crystal_freq = freq;
+	state->double_asclk = flags & SAA7115_FREQ_FL_DOUBLE_ASCLK;
+	state->cgcdiv = (flags & SAA7115_FREQ_FL_CGCDIV) ? 3 : 4;
+	state->ucgc = flags & SAA7115_FREQ_FL_UCGC;
+	state->apll = flags & SAA7115_FREQ_FL_APLL;
+	saa711x_s_clock_freq(sd, state->audclk_freq);
+	return 0;
+}
+
+static int saa711x_reset(struct v4l2_subdev *sd, u32 val)
+{
+	v4l2_dbg(1, debug, sd, "decoder RESET\n");
+	saa711x_writeregs(sd, saa7115_cfg_reset_scaler);
+	return 0;
+}
+
+static int saa711x_g_vbi_data(struct v4l2_subdev *sd, struct v4l2_sliced_vbi_data *data)
+{
+	/* Note: the internal field ID is inverted for NTSC,
+	   so data->field 0 maps to the saa7115 even field,
+	   whereas for PAL it maps to the saa7115 odd field. */
+	switch (data->id) {
+	case V4L2_SLICED_WSS_625:
+		if (saa711x_read(sd, 0x6b) & 0xc0)
+			return -EIO;
+		data->data[0] = saa711x_read(sd, 0x6c);
+		data->data[1] = saa711x_read(sd, 0x6d);
+		return 0;
+	case V4L2_SLICED_CAPTION_525:
+		if (data->field == 0) {
+			/* CC */
+			if (saa711x_read(sd, 0x66) & 0x30)
+				return -EIO;
+			data->data[0] = saa711x_read(sd, 0x69);
+			data->data[1] = saa711x_read(sd, 0x6a);
+			return 0;
+		}
+		/* XDS */
+		if (saa711x_read(sd, 0x66) & 0xc0)
+			return -EIO;
+		data->data[0] = saa711x_read(sd, 0x67);
+		data->data[1] = saa711x_read(sd, 0x68);
+		return 0;
+	default:
+		return -EINVAL;
+	}
+}
+
+static int saa711x_querystd(struct v4l2_subdev *sd, v4l2_std_id *std)
+{
+	struct saa711x_state *state = to_state(sd);
+	int reg1f, reg1e;
+
+	/*
+	 * The V4L2 core already initializes std with all supported
+	 * Standards. All driver needs to do is to mask it, to remove
+	 * standards that don't apply from the mask
+	 */
+
+	reg1f = saa711x_read(sd, R_1F_STATUS_BYTE_2_VD_DEC);
+
+	if (state->ident == SAA7115) {
+		reg1e = saa711x_read(sd, R_1E_STATUS_BYTE_1_VD_DEC);
+
+		v4l2_dbg(1, debug, sd, "Status byte 1 (0x1e)=0x%02x\n", reg1e);
+
+		switch (reg1e & 0x03) {
+		case 1:
+			*std &= V4L2_STD_NTSC;
+			break;
+		case 2:
+			/*
+			 * V4L2_STD_PAL just cover the european PAL standards.
+			 * This is wrong, as the device could also be using an
+			 * other PAL standard.
+			 */
+			*std &= V4L2_STD_PAL   | V4L2_STD_PAL_N  | V4L2_STD_PAL_Nc |
+				V4L2_STD_PAL_M | V4L2_STD_PAL_60;
+			break;
+		case 3:
+			*std &= V4L2_STD_SECAM;
+			break;
+		default:
+			*std = V4L2_STD_UNKNOWN;
+			/* Can't detect anything */
+			break;
+		}
+	}
+
+	v4l2_dbg(1, debug, sd, "Status byte 2 (0x1f)=0x%02x\n", reg1f);
+
+	/* horizontal/vertical not locked */
+	if (reg1f & 0x40) {
+		*std = V4L2_STD_UNKNOWN;
+		goto ret;
+	}
+
+	if (reg1f & 0x20)
+		*std &= V4L2_STD_525_60;
+	else
+		*std &= V4L2_STD_625_50;
+
+ret:
+	v4l2_dbg(1, debug, sd, "detected std mask = %08Lx\n", *std);
+
+	return 0;
+}
+
+static int saa711x_g_input_status(struct v4l2_subdev *sd, u32 *status)
+{
+	struct saa711x_state *state = to_state(sd);
+	int reg1e = 0x80;
+	int reg1f;
+
+	*status = V4L2_IN_ST_NO_SIGNAL;
+	if (state->ident == SAA7115)
+		reg1e = saa711x_read(sd, R_1E_STATUS_BYTE_1_VD_DEC);
+	reg1f = saa711x_read(sd, R_1F_STATUS_BYTE_2_VD_DEC);
+	if ((reg1f & 0xc1) == 0x81 && (reg1e & 0xc0) == 0x80)
+		*status = 0;
+	return 0;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int saa711x_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+{
+	reg->val = saa711x_read(sd, reg->reg & 0xff);
+	reg->size = 1;
+	return 0;
+}
+
+static int saa711x_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
+{
+	saa711x_write(sd, reg->reg & 0xff, reg->val & 0xff);
+	return 0;
+}
+#endif
+
+static int saa711x_log_status(struct v4l2_subdev *sd)
+{
+	struct saa711x_state *state = to_state(sd);
+	int reg1e, reg1f;
+	int signalOk;
+	int vcr;
+
+	v4l2_info(sd, "Audio frequency: %d Hz\n", state->audclk_freq);
+	if (state->ident != SAA7115) {
+		/* status for the saa7114 */
+		reg1f = saa711x_read(sd, R_1F_STATUS_BYTE_2_VD_DEC);
+		signalOk = (reg1f & 0xc1) == 0x81;
+		v4l2_info(sd, "Video signal:    %s\n", signalOk ? "ok" : "bad");
+		v4l2_info(sd, "Frequency:       %s\n", (reg1f & 0x20) ? "60 Hz" : "50 Hz");
+		return 0;
+	}
+
+	/* status for the saa7115 */
+	reg1e = saa711x_read(sd, R_1E_STATUS_BYTE_1_VD_DEC);
+	reg1f = saa711x_read(sd, R_1F_STATUS_BYTE_2_VD_DEC);
+
+	signalOk = (reg1f & 0xc1) == 0x81 && (reg1e & 0xc0) == 0x80;
+	vcr = !(reg1f & 0x10);
+
+	if (state->input >= 6)
+		v4l2_info(sd, "Input:           S-Video %d\n", state->input - 6);
+	else
+		v4l2_info(sd, "Input:           Composite %d\n", state->input);
+	v4l2_info(sd, "Video signal:    %s\n", signalOk ? (vcr ? "VCR" : "broadcast/DVD") : "bad");
+	v4l2_info(sd, "Frequency:       %s\n", (reg1f & 0x20) ? "60 Hz" : "50 Hz");
+
+	switch (reg1e & 0x03) {
+	case 1:
+		v4l2_info(sd, "Detected format: NTSC\n");
+		break;
+	case 2:
+		v4l2_info(sd, "Detected format: PAL\n");
+		break;
+	case 3:
+		v4l2_info(sd, "Detected format: SECAM\n");
+		break;
+	default:
+		v4l2_info(sd, "Detected format: BW/No color\n");
+		break;
+	}
+	v4l2_info(sd, "Width, Height:   %d, %d\n", state->width, state->height);
+	v4l2_ctrl_handler_log_status(&state->hdl, sd->name);
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_ctrl_ops saa711x_ctrl_ops = {
+	.s_ctrl = saa711x_s_ctrl,
+	.g_volatile_ctrl = saa711x_g_volatile_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops saa711x_core_ops = {
+	.log_status = saa711x_log_status,
+	.reset = saa711x_reset,
+	.s_gpio = saa711x_s_gpio,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = saa711x_g_register,
+	.s_register = saa711x_s_register,
+#endif
+};
+
+static const struct v4l2_subdev_tuner_ops saa711x_tuner_ops = {
+	.s_radio = saa711x_s_radio,
+	.g_tuner = saa711x_g_tuner,
+};
+
+static const struct v4l2_subdev_audio_ops saa711x_audio_ops = {
+	.s_clock_freq = saa711x_s_clock_freq,
+};
+
+static const struct v4l2_subdev_video_ops saa711x_video_ops = {
+	.s_std = saa711x_s_std,
+	.s_routing = saa711x_s_routing,
+	.s_crystal_freq = saa711x_s_crystal_freq,
+	.s_stream = saa711x_s_stream,
+	.querystd = saa711x_querystd,
+	.g_input_status = saa711x_g_input_status,
+};
+
+static const struct v4l2_subdev_vbi_ops saa711x_vbi_ops = {
+	.g_vbi_data = saa711x_g_vbi_data,
+	.decode_vbi_line = saa711x_decode_vbi_line,
+	.g_sliced_fmt = saa711x_g_sliced_fmt,
+	.s_sliced_fmt = saa711x_s_sliced_fmt,
+	.s_raw_fmt = saa711x_s_raw_fmt,
+};
+
+static const struct v4l2_subdev_pad_ops saa711x_pad_ops = {
+	.set_fmt = saa711x_set_fmt,
+};
+
+static const struct v4l2_subdev_ops saa711x_ops = {
+	.core = &saa711x_core_ops,
+	.tuner = &saa711x_tuner_ops,
+	.audio = &saa711x_audio_ops,
+	.video = &saa711x_video_ops,
+	.vbi = &saa711x_vbi_ops,
+	.pad = &saa711x_pad_ops,
+};
+
+#define CHIP_VER_SIZE	16
+
+/* ----------------------------------------------------------------------- */
+
+static void saa711x_write_platform_data(struct saa711x_state *state,
+					struct saa7115_platform_data *data)
+{
+	struct v4l2_subdev *sd = &state->sd;
+	u8 work;
+
+	if (state->ident != GM7113C &&
+	    state->ident != SAA7113)
+		return;
+
+	if (data->saa7113_r08_htc) {
+		work = saa711x_read(sd, R_08_SYNC_CNTL);
+		work &= ~SAA7113_R_08_HTC_MASK;
+		work |= ((*data->saa7113_r08_htc) << SAA7113_R_08_HTC_OFFSET);
+		saa711x_write(sd, R_08_SYNC_CNTL, work);
+	}
+
+	if (data->saa7113_r10_vrln) {
+		work = saa711x_read(sd, R_10_CHROMA_CNTL_2);
+		work &= ~SAA7113_R_10_VRLN_MASK;
+		if (*data->saa7113_r10_vrln)
+			work |= (1 << SAA7113_R_10_VRLN_OFFSET);
+		saa711x_write(sd, R_10_CHROMA_CNTL_2, work);
+	}
+
+	if (data->saa7113_r10_ofts) {
+		work = saa711x_read(sd, R_10_CHROMA_CNTL_2);
+		work &= ~SAA7113_R_10_OFTS_MASK;
+		work |= (*data->saa7113_r10_ofts << SAA7113_R_10_OFTS_OFFSET);
+		saa711x_write(sd, R_10_CHROMA_CNTL_2, work);
+	}
+
+	if (data->saa7113_r12_rts0) {
+		work = saa711x_read(sd, R_12_RT_SIGNAL_CNTL);
+		work &= ~SAA7113_R_12_RTS0_MASK;
+		work |= (*data->saa7113_r12_rts0 << SAA7113_R_12_RTS0_OFFSET);
+
+		/* According to the datasheet,
+		 * SAA7113_RTS_DOT_IN should only be used on RTS1 */
+		WARN_ON(*data->saa7113_r12_rts0 == SAA7113_RTS_DOT_IN);
+		saa711x_write(sd, R_12_RT_SIGNAL_CNTL, work);
+	}
+
+	if (data->saa7113_r12_rts1) {
+		work = saa711x_read(sd, R_12_RT_SIGNAL_CNTL);
+		work &= ~SAA7113_R_12_RTS1_MASK;
+		work |= (*data->saa7113_r12_rts1 << SAA7113_R_12_RTS1_OFFSET);
+		saa711x_write(sd, R_12_RT_SIGNAL_CNTL, work);
+	}
+
+	if (data->saa7113_r13_adlsb) {
+		work = saa711x_read(sd, R_13_RT_X_PORT_OUT_CNTL);
+		work &= ~SAA7113_R_13_ADLSB_MASK;
+		if (*data->saa7113_r13_adlsb)
+			work |= (1 << SAA7113_R_13_ADLSB_OFFSET);
+		saa711x_write(sd, R_13_RT_X_PORT_OUT_CNTL, work);
+	}
+}
+
+/**
+ * saa711x_detect_chip - Detects the saa711x (or clone) variant
+ * @client:		I2C client structure.
+ * @id:			I2C device ID structure.
+ * @name:		Name of the device to be filled.
+ *
+ * Detects the Philips/NXP saa711x chip, or some clone of it.
+ * if 'id' is NULL or id->driver_data is equal to 1, it auto-probes
+ * the analog demod.
+ * If the tuner is not found, it returns -ENODEV.
+ * If auto-detection is disabled and the tuner doesn't match what it was
+ *	required, it returns -EINVAL and fills 'name'.
+ * If the chip is found, it returns the chip ID and fills 'name'.
+ */
+static int saa711x_detect_chip(struct i2c_client *client,
+			       const struct i2c_device_id *id,
+			       char *name)
+{
+	char chip_ver[CHIP_VER_SIZE];
+	char chip_id;
+	int i;
+	int autodetect;
+
+	autodetect = !id || id->driver_data == 1;
+
+	/* Read the chip version register */
+	for (i = 0; i < CHIP_VER_SIZE; i++) {
+		i2c_smbus_write_byte_data(client, 0, i);
+		chip_ver[i] = i2c_smbus_read_byte_data(client, 0);
+		name[i] = (chip_ver[i] & 0x0f) + '0';
+		if (name[i] > '9')
+			name[i] += 'a' - '9' - 1;
+	}
+	name[i] = '\0';
+
+	/* Check if it is a Philips/NXP chip */
+	if (!memcmp(name + 1, "f711", 4)) {
+		chip_id = name[5];
+		snprintf(name, CHIP_VER_SIZE, "saa711%c", chip_id);
+
+		if (!autodetect && strcmp(name, id->name))
+			return -EINVAL;
+
+		switch (chip_id) {
+		case '1':
+			if (chip_ver[0] & 0xf0) {
+				snprintf(name, CHIP_VER_SIZE, "saa711%ca", chip_id);
+				v4l_info(client, "saa7111a variant found\n");
+				return SAA7111A;
+			}
+			return SAA7111;
+		case '3':
+			return SAA7113;
+		case '4':
+			return SAA7114;
+		case '5':
+			return SAA7115;
+		case '8':
+			return SAA7118;
+		default:
+			v4l2_info(client,
+				  "WARNING: Philips/NXP chip unknown - Falling back to saa7111\n");
+			return SAA7111;
+		}
+	}
+
+	/* Check if it is a gm7113c */
+	if (!memcmp(name, "0000", 4)) {
+		chip_id = 0;
+		for (i = 0; i < 4; i++) {
+			chip_id = chip_id << 1;
+			chip_id |= (chip_ver[i] & 0x80) ? 1 : 0;
+		}
+
+		/*
+		 * Note: From the datasheet, only versions 1 and 2
+		 * exists. However, tests on a device labeled as:
+		 * "GM7113C 1145" returned "10" on all 16 chip
+		 * version (reg 0x00) reads. So, we need to also
+		 * accept at least version 0. For now, let's just
+		 * assume that a device that returns "0000" for
+		 * the lower nibble is a gm7113c.
+		 */
+
+		strscpy(name, "gm7113c", CHIP_VER_SIZE);
+
+		if (!autodetect && strcmp(name, id->name))
+			return -EINVAL;
+
+		v4l_dbg(1, debug, client,
+			"It seems to be a %s chip (%*ph) @ 0x%x.\n",
+			name, 16, chip_ver, client->addr << 1);
+
+		return GM7113C;
+	}
+
+	/* Check if it is a CJC7113 */
+	if (!memcmp(name, "1111111111111111", CHIP_VER_SIZE)) {
+		strscpy(name, "cjc7113", CHIP_VER_SIZE);
+
+		if (!autodetect && strcmp(name, id->name))
+			return -EINVAL;
+
+		v4l_dbg(1, debug, client,
+			"It seems to be a %s chip (%*ph) @ 0x%x.\n",
+			name, 16, chip_ver, client->addr << 1);
+
+		/* CJC7113 seems to be SAA7113-compatible */
+		return SAA7113;
+	}
+
+	/* Chip was not discovered. Return its ID and don't bind */
+	v4l_dbg(1, debug, client, "chip %*ph @ 0x%x is unknown.\n",
+		16, chip_ver, client->addr << 1);
+	return -ENODEV;
+}
+
+static int saa711x_probe(struct i2c_client *client)
+{
+	const struct i2c_device_id *id = i2c_client_get_device_id(client);
+	struct saa711x_state *state;
+	struct v4l2_subdev *sd;
+	struct v4l2_ctrl_handler *hdl;
+	struct saa7115_platform_data *pdata;
+	int ident;
+	char name[CHIP_VER_SIZE + 1];
+#if defined(CONFIG_MEDIA_CONTROLLER)
+	int ret;
+#endif
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+
+	ident = saa711x_detect_chip(client, id, name);
+	if (ident == -EINVAL) {
+		/* Chip exists, but doesn't match */
+		v4l_warn(client, "found %s while %s was expected\n",
+			 name, id->name);
+		return -ENODEV;
+	}
+	if (ident < 0)
+		return ident;
+
+	strscpy(client->name, name, sizeof(client->name));
+
+	state = devm_kzalloc(&client->dev, sizeof(*state), GFP_KERNEL);
+	if (state == NULL)
+		return -ENOMEM;
+	sd = &state->sd;
+	v4l2_i2c_subdev_init(sd, client, &saa711x_ops);
+
+#if defined(CONFIG_MEDIA_CONTROLLER)
+	state->pads[SAA711X_PAD_IF_INPUT].flags = MEDIA_PAD_FL_SINK;
+	state->pads[SAA711X_PAD_IF_INPUT].sig_type = PAD_SIGNAL_ANALOG;
+	state->pads[SAA711X_PAD_VID_OUT].flags = MEDIA_PAD_FL_SOURCE;
+	state->pads[SAA711X_PAD_VID_OUT].sig_type = PAD_SIGNAL_DV;
+
+	sd->entity.function = MEDIA_ENT_F_ATV_DECODER;
+
+	ret = media_entity_pads_init(&sd->entity, SAA711X_NUM_PADS,
+				     state->pads);
+	if (ret < 0)
+		return ret;
+#endif
+
+	v4l_info(client, "%s found @ 0x%x (%s)\n", name,
+		 client->addr << 1, client->adapter->name);
+	hdl = &state->hdl;
+	v4l2_ctrl_handler_init(hdl, 6);
+	/* add in ascending ID order */
+	v4l2_ctrl_new_std(hdl, &saa711x_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(hdl, &saa711x_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 127, 1, 64);
+	v4l2_ctrl_new_std(hdl, &saa711x_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 127, 1, 64);
+	v4l2_ctrl_new_std(hdl, &saa711x_ctrl_ops,
+			V4L2_CID_HUE, -128, 127, 1, 0);
+	state->agc = v4l2_ctrl_new_std(hdl, &saa711x_ctrl_ops,
+			V4L2_CID_CHROMA_AGC, 0, 1, 1, 1);
+	state->gain = v4l2_ctrl_new_std(hdl, &saa711x_ctrl_ops,
+			V4L2_CID_CHROMA_GAIN, 0, 127, 1, 40);
+	sd->ctrl_handler = hdl;
+	if (hdl->error) {
+		int err = hdl->error;
+
+		v4l2_ctrl_handler_free(hdl);
+		return err;
+	}
+	v4l2_ctrl_auto_cluster(2, &state->agc, 0, true);
+
+	state->input = -1;
+	state->output = SAA7115_IPORT_ON;
+	state->enable = 1;
+	state->radio = 0;
+	state->ident = ident;
+
+	state->audclk_freq = 48000;
+
+	v4l2_dbg(1, debug, sd, "writing init values\n");
+
+	/* init to 60hz/48khz */
+	state->crystal_freq = SAA7115_FREQ_24_576_MHZ;
+	pdata = client->dev.platform_data;
+	switch (state->ident) {
+	case SAA7111:
+	case SAA7111A:
+		saa711x_writeregs(sd, saa7111_init);
+		break;
+	case GM7113C:
+		saa711x_writeregs(sd, gm7113c_init);
+		break;
+	case SAA7113:
+		if (pdata && pdata->saa7113_force_gm7113c_init)
+			saa711x_writeregs(sd, gm7113c_init);
+		else
+			saa711x_writeregs(sd, saa7113_init);
+		break;
+	default:
+		state->crystal_freq = SAA7115_FREQ_32_11_MHZ;
+		saa711x_writeregs(sd, saa7115_init_auto_input);
+	}
+	if (state->ident > SAA7111A && state->ident != GM7113C)
+		saa711x_writeregs(sd, saa7115_init_misc);
+
+	if (pdata)
+		saa711x_write_platform_data(state, pdata);
+
+	saa711x_set_v4lstd(sd, V4L2_STD_NTSC);
+	v4l2_ctrl_handler_setup(hdl);
+
+	v4l2_dbg(1, debug, sd, "status: (1E) 0x%02x, (1F) 0x%02x\n",
+		saa711x_read(sd, R_1E_STATUS_BYTE_1_VD_DEC),
+		saa711x_read(sd, R_1F_STATUS_BYTE_2_VD_DEC));
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static void saa711x_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+}
+
+static const struct i2c_device_id saa711x_id[] = {
+	{ "saa7115_auto", 1 }, /* autodetect */
+	{ "saa7111", 0 },
+	{ "saa7113", 0 },
+	{ "saa7114", 0 },
+	{ "saa7115", 0 },
+	{ "saa7118", 0 },
+	{ "gm7113c", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, saa711x_id);
+
+static struct i2c_driver saa711x_driver = {
+	.driver = {
+		.name	= "saa7115",
+	},
+	.probe		= saa711x_probe,
+	.remove		= saa711x_remove,
+	.id_table	= saa711x_id,
+};
+
+module_i2c_driver(saa711x_driver);
diff --git a/drivers/media/i2c/saa711x_regs.h b/drivers/media/i2c/saa711x_regs.h
new file mode 100644
index 000000000..4b5f69857
--- /dev/null
+++ b/drivers/media/i2c/saa711x_regs.h
@@ -0,0 +1,560 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * saa711x - Philips SAA711x video decoder register specifications
+ *
+ * Copyright (c) 2006 Mauro Carvalho Chehab <mchehab@kernel.org>
+ */
+
+#define R_00_CHIP_VERSION                             0x00
+/* Video Decoder */
+	/* Video Decoder - Frontend part */
+#define R_01_INC_DELAY                                0x01
+#define R_02_INPUT_CNTL_1                             0x02
+#define R_03_INPUT_CNTL_2                             0x03
+#define R_04_INPUT_CNTL_3                             0x04
+#define R_05_INPUT_CNTL_4                             0x05
+	/* Video Decoder - Decoder part */
+#define R_06_H_SYNC_START                             0x06
+#define R_07_H_SYNC_STOP                              0x07
+#define R_08_SYNC_CNTL                                0x08
+#define R_09_LUMA_CNTL                                0x09
+#define R_0A_LUMA_BRIGHT_CNTL                         0x0a
+#define R_0B_LUMA_CONTRAST_CNTL                       0x0b
+#define R_0C_CHROMA_SAT_CNTL                          0x0c
+#define R_0D_CHROMA_HUE_CNTL                          0x0d
+#define R_0E_CHROMA_CNTL_1                            0x0e
+#define R_0F_CHROMA_GAIN_CNTL                         0x0f
+#define R_10_CHROMA_CNTL_2                            0x10
+#define R_11_MODE_DELAY_CNTL                          0x11
+#define R_12_RT_SIGNAL_CNTL                           0x12
+#define R_13_RT_X_PORT_OUT_CNTL                       0x13
+#define R_14_ANAL_ADC_COMPAT_CNTL                     0x14
+#define R_15_VGATE_START_FID_CHG                      0x15
+#define R_16_VGATE_STOP                               0x16
+#define R_17_MISC_VGATE_CONF_AND_MSB                  0x17
+#define R_18_RAW_DATA_GAIN_CNTL                       0x18
+#define R_19_RAW_DATA_OFF_CNTL                        0x19
+#define R_1A_COLOR_KILL_LVL_CNTL                      0x1a
+#define R_1B_MISC_TVVCRDET                            0x1b
+#define R_1C_ENHAN_COMB_CTRL1                         0x1c
+#define R_1D_ENHAN_COMB_CTRL2                         0x1d
+#define R_1E_STATUS_BYTE_1_VD_DEC                     0x1e
+#define R_1F_STATUS_BYTE_2_VD_DEC                     0x1f
+
+/* Component processing and interrupt masking part */
+#define R_23_INPUT_CNTL_5                             0x23
+#define R_24_INPUT_CNTL_6                             0x24
+#define R_25_INPUT_CNTL_7                             0x25
+#define R_29_COMP_DELAY                               0x29
+#define R_2A_COMP_BRIGHT_CNTL                         0x2a
+#define R_2B_COMP_CONTRAST_CNTL                       0x2b
+#define R_2C_COMP_SAT_CNTL                            0x2c
+#define R_2D_INTERRUPT_MASK_1                         0x2d
+#define R_2E_INTERRUPT_MASK_2                         0x2e
+#define R_2F_INTERRUPT_MASK_3                         0x2f
+
+/* Audio clock generator part */
+#define R_30_AUD_MAST_CLK_CYCLES_PER_FIELD            0x30
+#define R_34_AUD_MAST_CLK_NOMINAL_INC                 0x34
+#define R_38_CLK_RATIO_AMXCLK_TO_ASCLK                0x38
+#define R_39_CLK_RATIO_ASCLK_TO_ALRCLK                0x39
+#define R_3A_AUD_CLK_GEN_BASIC_SETUP                  0x3a
+
+/* General purpose VBI data slicer part */
+#define R_40_SLICER_CNTL_1                            0x40
+#define R_41_LCR_BASE                                 0x41
+#define R_58_PROGRAM_FRAMING_CODE                     0x58
+#define R_59_H_OFF_FOR_SLICER                         0x59
+#define R_5A_V_OFF_FOR_SLICER                         0x5a
+#define R_5B_FLD_OFF_AND_MSB_FOR_H_AND_V_OFF          0x5b
+#define R_5D_DID                                      0x5d
+#define R_5E_SDID                                     0x5e
+#define R_60_SLICER_STATUS_BYTE_0                     0x60
+#define R_61_SLICER_STATUS_BYTE_1                     0x61
+#define R_62_SLICER_STATUS_BYTE_2                     0x62
+
+/* X port, I port and the scaler part */
+	/* Task independent global settings */
+#define R_80_GLOBAL_CNTL_1                            0x80
+#define R_81_V_SYNC_FLD_ID_SRC_SEL_AND_RETIMED_V_F    0x81
+#define R_83_X_PORT_I_O_ENA_AND_OUT_CLK               0x83
+#define R_84_I_PORT_SIGNAL_DEF                        0x84
+#define R_85_I_PORT_SIGNAL_POLAR                      0x85
+#define R_86_I_PORT_FIFO_FLAG_CNTL_AND_ARBIT          0x86
+#define R_87_I_PORT_I_O_ENA_OUT_CLK_AND_GATED         0x87
+#define R_88_POWER_SAVE_ADC_PORT_CNTL                 0x88
+#define R_8F_STATUS_INFO_SCALER                       0x8f
+	/* Task A definition */
+		/* Basic settings and acquisition window definition */
+#define R_90_A_TASK_HANDLING_CNTL                     0x90
+#define R_91_A_X_PORT_FORMATS_AND_CONF                0x91
+#define R_92_A_X_PORT_INPUT_REFERENCE_SIGNAL          0x92
+#define R_93_A_I_PORT_OUTPUT_FORMATS_AND_CONF         0x93
+#define R_94_A_HORIZ_INPUT_WINDOW_START               0x94
+#define R_95_A_HORIZ_INPUT_WINDOW_START_MSB           0x95
+#define R_96_A_HORIZ_INPUT_WINDOW_LENGTH              0x96
+#define R_97_A_HORIZ_INPUT_WINDOW_LENGTH_MSB          0x97
+#define R_98_A_VERT_INPUT_WINDOW_START                0x98
+#define R_99_A_VERT_INPUT_WINDOW_START_MSB            0x99
+#define R_9A_A_VERT_INPUT_WINDOW_LENGTH               0x9a
+#define R_9B_A_VERT_INPUT_WINDOW_LENGTH_MSB           0x9b
+#define R_9C_A_HORIZ_OUTPUT_WINDOW_LENGTH             0x9c
+#define R_9D_A_HORIZ_OUTPUT_WINDOW_LENGTH_MSB         0x9d
+#define R_9E_A_VERT_OUTPUT_WINDOW_LENGTH              0x9e
+#define R_9F_A_VERT_OUTPUT_WINDOW_LENGTH_MSB          0x9f
+		/* FIR filtering and prescaling */
+#define R_A0_A_HORIZ_PRESCALING                       0xa0
+#define R_A1_A_ACCUMULATION_LENGTH                    0xa1
+#define R_A2_A_PRESCALER_DC_GAIN_AND_FIR_PREFILTER    0xa2
+#define R_A4_A_LUMA_BRIGHTNESS_CNTL                   0xa4
+#define R_A5_A_LUMA_CONTRAST_CNTL                     0xa5
+#define R_A6_A_CHROMA_SATURATION_CNTL                 0xa6
+		/* Horizontal phase scaling */
+#define R_A8_A_HORIZ_LUMA_SCALING_INC                 0xa8
+#define R_A9_A_HORIZ_LUMA_SCALING_INC_MSB             0xa9
+#define R_AA_A_HORIZ_LUMA_PHASE_OFF                   0xaa
+#define R_AC_A_HORIZ_CHROMA_SCALING_INC               0xac
+#define R_AD_A_HORIZ_CHROMA_SCALING_INC_MSB           0xad
+#define R_AE_A_HORIZ_CHROMA_PHASE_OFF                 0xae
+#define R_AF_A_HORIZ_CHROMA_PHASE_OFF_MSB             0xaf
+		/* Vertical scaling */
+#define R_B0_A_VERT_LUMA_SCALING_INC                  0xb0
+#define R_B1_A_VERT_LUMA_SCALING_INC_MSB              0xb1
+#define R_B2_A_VERT_CHROMA_SCALING_INC                0xb2
+#define R_B3_A_VERT_CHROMA_SCALING_INC_MSB            0xb3
+#define R_B4_A_VERT_SCALING_MODE_CNTL                 0xb4
+#define R_B8_A_VERT_CHROMA_PHASE_OFF_00               0xb8
+#define R_B9_A_VERT_CHROMA_PHASE_OFF_01               0xb9
+#define R_BA_A_VERT_CHROMA_PHASE_OFF_10               0xba
+#define R_BB_A_VERT_CHROMA_PHASE_OFF_11               0xbb
+#define R_BC_A_VERT_LUMA_PHASE_OFF_00                 0xbc
+#define R_BD_A_VERT_LUMA_PHASE_OFF_01                 0xbd
+#define R_BE_A_VERT_LUMA_PHASE_OFF_10                 0xbe
+#define R_BF_A_VERT_LUMA_PHASE_OFF_11                 0xbf
+	/* Task B definition */
+		/* Basic settings and acquisition window definition */
+#define R_C0_B_TASK_HANDLING_CNTL                     0xc0
+#define R_C1_B_X_PORT_FORMATS_AND_CONF                0xc1
+#define R_C2_B_INPUT_REFERENCE_SIGNAL_DEFINITION      0xc2
+#define R_C3_B_I_PORT_FORMATS_AND_CONF                0xc3
+#define R_C4_B_HORIZ_INPUT_WINDOW_START               0xc4
+#define R_C5_B_HORIZ_INPUT_WINDOW_START_MSB           0xc5
+#define R_C6_B_HORIZ_INPUT_WINDOW_LENGTH              0xc6
+#define R_C7_B_HORIZ_INPUT_WINDOW_LENGTH_MSB          0xc7
+#define R_C8_B_VERT_INPUT_WINDOW_START                0xc8
+#define R_C9_B_VERT_INPUT_WINDOW_START_MSB            0xc9
+#define R_CA_B_VERT_INPUT_WINDOW_LENGTH               0xca
+#define R_CB_B_VERT_INPUT_WINDOW_LENGTH_MSB           0xcb
+#define R_CC_B_HORIZ_OUTPUT_WINDOW_LENGTH             0xcc
+#define R_CD_B_HORIZ_OUTPUT_WINDOW_LENGTH_MSB         0xcd
+#define R_CE_B_VERT_OUTPUT_WINDOW_LENGTH              0xce
+#define R_CF_B_VERT_OUTPUT_WINDOW_LENGTH_MSB          0xcf
+		/* FIR filtering and prescaling */
+#define R_D0_B_HORIZ_PRESCALING                       0xd0
+#define R_D1_B_ACCUMULATION_LENGTH                    0xd1
+#define R_D2_B_PRESCALER_DC_GAIN_AND_FIR_PREFILTER    0xd2
+#define R_D4_B_LUMA_BRIGHTNESS_CNTL                   0xd4
+#define R_D5_B_LUMA_CONTRAST_CNTL                     0xd5
+#define R_D6_B_CHROMA_SATURATION_CNTL                 0xd6
+		/* Horizontal phase scaling */
+#define R_D8_B_HORIZ_LUMA_SCALING_INC                 0xd8
+#define R_D9_B_HORIZ_LUMA_SCALING_INC_MSB             0xd9
+#define R_DA_B_HORIZ_LUMA_PHASE_OFF                   0xda
+#define R_DC_B_HORIZ_CHROMA_SCALING                   0xdc
+#define R_DD_B_HORIZ_CHROMA_SCALING_MSB               0xdd
+#define R_DE_B_HORIZ_PHASE_OFFSET_CRHOMA              0xde
+		/* Vertical scaling */
+#define R_E0_B_VERT_LUMA_SCALING_INC                  0xe0
+#define R_E1_B_VERT_LUMA_SCALING_INC_MSB              0xe1
+#define R_E2_B_VERT_CHROMA_SCALING_INC                0xe2
+#define R_E3_B_VERT_CHROMA_SCALING_INC_MSB            0xe3
+#define R_E4_B_VERT_SCALING_MODE_CNTL                 0xe4
+#define R_E8_B_VERT_CHROMA_PHASE_OFF_00               0xe8
+#define R_E9_B_VERT_CHROMA_PHASE_OFF_01               0xe9
+#define R_EA_B_VERT_CHROMA_PHASE_OFF_10               0xea
+#define R_EB_B_VERT_CHROMA_PHASE_OFF_11               0xeb
+#define R_EC_B_VERT_LUMA_PHASE_OFF_00                 0xec
+#define R_ED_B_VERT_LUMA_PHASE_OFF_01                 0xed
+#define R_EE_B_VERT_LUMA_PHASE_OFF_10                 0xee
+#define R_EF_B_VERT_LUMA_PHASE_OFF_11                 0xef
+
+/* second PLL (PLL2) and Pulsegenerator Programming */
+#define R_F0_LFCO_PER_LINE                            0xf0
+#define R_F1_P_I_PARAM_SELECT                         0xf1
+#define R_F2_NOMINAL_PLL2_DTO                         0xf2
+#define R_F3_PLL_INCREMENT                            0xf3
+#define R_F4_PLL2_STATUS                              0xf4
+#define R_F5_PULSGEN_LINE_LENGTH                      0xf5
+#define R_F6_PULSE_A_POS_LSB_AND_PULSEGEN_CONFIG      0xf6
+#define R_F7_PULSE_A_POS_MSB                          0xf7
+#define R_F8_PULSE_B_POS                              0xf8
+#define R_F9_PULSE_B_POS_MSB                          0xf9
+#define R_FA_PULSE_C_POS                              0xfa
+#define R_FB_PULSE_C_POS_MSB                          0xfb
+#define R_FF_S_PLL_MAX_PHASE_ERR_THRESH_NUM_LINES     0xff
+
+/* SAA7113 bit-masks */
+#define SAA7113_R_08_HTC_OFFSET 3
+#define SAA7113_R_08_HTC_MASK (0x3 << SAA7113_R_08_HTC_OFFSET)
+#define SAA7113_R_08_FSEL 0x40
+#define SAA7113_R_08_AUFD 0x80
+
+#define SAA7113_R_10_VRLN_OFFSET 3
+#define SAA7113_R_10_VRLN_MASK (0x1 << SAA7113_R_10_VRLN_OFFSET)
+#define SAA7113_R_10_OFTS_OFFSET 6
+#define SAA7113_R_10_OFTS_MASK (0x3 << SAA7113_R_10_OFTS_OFFSET)
+
+#define SAA7113_R_12_RTS0_OFFSET 0
+#define SAA7113_R_12_RTS0_MASK (0xf << SAA7113_R_12_RTS0_OFFSET)
+#define SAA7113_R_12_RTS1_OFFSET 4
+#define SAA7113_R_12_RTS1_MASK (0xf << SAA7113_R_12_RTS1_OFFSET)
+
+#define SAA7113_R_13_ADLSB_OFFSET 7
+#define SAA7113_R_13_ADLSB_MASK (0x1 << SAA7113_R_13_ADLSB_OFFSET)
+
+#if 0
+/* Those structs will be used in the future for debug purposes */
+struct saa711x_reg_descr {
+	u8 reg;
+	int count;
+	char *name;
+};
+
+struct saa711x_reg_descr saa711x_regs[] = {
+	/* REG COUNT NAME */
+	{R_00_CHIP_VERSION,1,
+	 "Chip version"},
+
+	/* Video Decoder: R_01_INC_DELAY to R_1F_STATUS_BYTE_2_VD_DEC */
+
+	/* Video Decoder - Frontend part: R_01_INC_DELAY to R_05_INPUT_CNTL_4 */
+	{R_01_INC_DELAY,1,
+	 "Increment delay"},
+	{R_02_INPUT_CNTL_1,1,
+	 "Analog input control 1"},
+	{R_03_INPUT_CNTL_2,1,
+	 "Analog input control 2"},
+	{R_04_INPUT_CNTL_3,1,
+	 "Analog input control 3"},
+	{R_05_INPUT_CNTL_4,1,
+	 "Analog input control 4"},
+
+	/* Video Decoder - Decoder part: R_06_H_SYNC_START to R_1F_STATUS_BYTE_2_VD_DEC */
+	{R_06_H_SYNC_START,1,
+	 "Horizontal sync start"},
+	{R_07_H_SYNC_STOP,1,
+	 "Horizontal sync stop"},
+	{R_08_SYNC_CNTL,1,
+	 "Sync control"},
+	{R_09_LUMA_CNTL,1,
+	 "Luminance control"},
+	{R_0A_LUMA_BRIGHT_CNTL,1,
+	 "Luminance brightness control"},
+	{R_0B_LUMA_CONTRAST_CNTL,1,
+	 "Luminance contrast control"},
+	{R_0C_CHROMA_SAT_CNTL,1,
+	 "Chrominance saturation control"},
+	{R_0D_CHROMA_HUE_CNTL,1,
+	 "Chrominance hue control"},
+	{R_0E_CHROMA_CNTL_1,1,
+	 "Chrominance control 1"},
+	{R_0F_CHROMA_GAIN_CNTL,1,
+	 "Chrominance gain control"},
+	{R_10_CHROMA_CNTL_2,1,
+	 "Chrominance control 2"},
+	{R_11_MODE_DELAY_CNTL,1,
+	 "Mode/delay control"},
+	{R_12_RT_SIGNAL_CNTL,1,
+	 "RT signal control"},
+	{R_13_RT_X_PORT_OUT_CNTL,1,
+	 "RT/X port output control"},
+	{R_14_ANAL_ADC_COMPAT_CNTL,1,
+	 "Analog/ADC/compatibility control"},
+	{R_15_VGATE_START_FID_CHG,  1,
+	 "VGATE start FID change"},
+	{R_16_VGATE_STOP,1,
+	 "VGATE stop"},
+	{R_17_MISC_VGATE_CONF_AND_MSB,  1,
+	 "Miscellaneous VGATE configuration and MSBs"},
+	{R_18_RAW_DATA_GAIN_CNTL,1,
+	 "Raw data gain control",},
+	{R_19_RAW_DATA_OFF_CNTL,1,
+	 "Raw data offset control",},
+	{R_1A_COLOR_KILL_LVL_CNTL,1,
+	 "Color Killer Level Control"},
+	{ R_1B_MISC_TVVCRDET, 1,
+	  "MISC /TVVCRDET"},
+	{ R_1C_ENHAN_COMB_CTRL1, 1,
+	 "Enhanced comb ctrl1"},
+	{ R_1D_ENHAN_COMB_CTRL2, 1,
+	 "Enhanced comb ctrl1"},
+	{R_1E_STATUS_BYTE_1_VD_DEC,1,
+	 "Status byte 1 video decoder"},
+	{R_1F_STATUS_BYTE_2_VD_DEC,1,
+	 "Status byte 2 video decoder"},
+
+	/* Component processing and interrupt masking part:  0x20h to R_2F_INTERRUPT_MASK_3 */
+	/* 0x20 to 0x22 - Reserved */
+	{R_23_INPUT_CNTL_5,1,
+	 "Analog input control 5"},
+	{R_24_INPUT_CNTL_6,1,
+	 "Analog input control 6"},
+	{R_25_INPUT_CNTL_7,1,
+	 "Analog input control 7"},
+	/* 0x26 to 0x28 - Reserved */
+	{R_29_COMP_DELAY,1,
+	 "Component delay"},
+	{R_2A_COMP_BRIGHT_CNTL,1,
+	 "Component brightness control"},
+	{R_2B_COMP_CONTRAST_CNTL,1,
+	 "Component contrast control"},
+	{R_2C_COMP_SAT_CNTL,1,
+	 "Component saturation control"},
+	{R_2D_INTERRUPT_MASK_1,1,
+	 "Interrupt mask 1"},
+	{R_2E_INTERRUPT_MASK_2,1,
+	 "Interrupt mask 2"},
+	{R_2F_INTERRUPT_MASK_3,1,
+	 "Interrupt mask 3"},
+
+	/* Audio clock generator part: R_30_AUD_MAST_CLK_CYCLES_PER_FIELD to 0x3f */
+	{R_30_AUD_MAST_CLK_CYCLES_PER_FIELD,3,
+	 "Audio master clock cycles per field"},
+	/* 0x33 - Reserved */
+	{R_34_AUD_MAST_CLK_NOMINAL_INC,3,
+	 "Audio master clock nominal increment"},
+	/* 0x37 - Reserved */
+	{R_38_CLK_RATIO_AMXCLK_TO_ASCLK,1,
+	 "Clock ratio AMXCLK to ASCLK"},
+	{R_39_CLK_RATIO_ASCLK_TO_ALRCLK,1,
+	 "Clock ratio ASCLK to ALRCLK"},
+	{R_3A_AUD_CLK_GEN_BASIC_SETUP,1,
+	 "Audio clock generator basic setup"},
+	/* 0x3b-0x3f - Reserved */
+
+	/* General purpose VBI data slicer part: R_40_SLICER_CNTL_1 to 0x7f */
+	{R_40_SLICER_CNTL_1,1,
+	 "Slicer control 1"},
+	{R_41_LCR,23,
+	 "R_41_LCR"},
+	{R_58_PROGRAM_FRAMING_CODE,1,
+	 "Programmable framing code"},
+	{R_59_H_OFF_FOR_SLICER,1,
+	 "Horizontal offset for slicer"},
+	{R_5A_V_OFF_FOR_SLICER,1,
+	 "Vertical offset for slicer"},
+	{R_5B_FLD_OFF_AND_MSB_FOR_H_AND_V_OFF,1,
+	 "Field offset and MSBs for horizontal and vertical offset"},
+	{R_5D_DID,1,
+	 "Header and data identification (R_5D_DID)"},
+	{R_5E_SDID,1,
+	 "Sliced data identification (R_5E_SDID) code"},
+	{R_60_SLICER_STATUS_BYTE_0,1,
+	 "Slicer status byte 0"},
+	{R_61_SLICER_STATUS_BYTE_1,1,
+	 "Slicer status byte 1"},
+	{R_62_SLICER_STATUS_BYTE_2,1,
+	 "Slicer status byte 2"},
+	/* 0x63-0x7f - Reserved */
+
+	/* X port, I port and the scaler part: R_80_GLOBAL_CNTL_1 to R_EF_B_VERT_LUMA_PHASE_OFF_11 */
+	/* Task independent global settings: R_80_GLOBAL_CNTL_1 to R_8F_STATUS_INFO_SCALER */
+	{R_80_GLOBAL_CNTL_1,1,
+	 "Global control 1"},
+	{R_81_V_SYNC_FLD_ID_SRC_SEL_AND_RETIMED_V_F,1,
+	 "Vertical sync and Field ID source selection, retimed V and F signals"},
+	/* 0x82 - Reserved */
+	{R_83_X_PORT_I_O_ENA_AND_OUT_CLK,1,
+	 "X port I/O enable and output clock"},
+	{R_84_I_PORT_SIGNAL_DEF,1,
+	 "I port signal definitions"},
+	{R_85_I_PORT_SIGNAL_POLAR,1,
+	 "I port signal polarities"},
+	{R_86_I_PORT_FIFO_FLAG_CNTL_AND_ARBIT,1,
+	 "I port FIFO flag control and arbitration"},
+	{R_87_I_PORT_I_O_ENA_OUT_CLK_AND_GATED,  1,
+	 "I port I/O enable output clock and gated"},
+	{R_88_POWER_SAVE_ADC_PORT_CNTL,1,
+	 "Power save/ADC port control"},
+	/* 089-0x8e - Reserved */
+	{R_8F_STATUS_INFO_SCALER,1,
+	 "Status information scaler part"},
+
+	/* Task A definition: R_90_A_TASK_HANDLING_CNTL to R_BF_A_VERT_LUMA_PHASE_OFF_11 */
+	/* Task A: Basic settings and acquisition window definition */
+	{R_90_A_TASK_HANDLING_CNTL,1,
+	 "Task A: Task handling control"},
+	{R_91_A_X_PORT_FORMATS_AND_CONF,1,
+	 "Task A: X port formats and configuration"},
+	{R_92_A_X_PORT_INPUT_REFERENCE_SIGNAL,1,
+	 "Task A: X port input reference signal definition"},
+	{R_93_A_I_PORT_OUTPUT_FORMATS_AND_CONF,1,
+	 "Task A: I port output formats and configuration"},
+	{R_94_A_HORIZ_INPUT_WINDOW_START,2,
+	 "Task A: Horizontal input window start"},
+	{R_96_A_HORIZ_INPUT_WINDOW_LENGTH,2,
+	 "Task A: Horizontal input window length"},
+	{R_98_A_VERT_INPUT_WINDOW_START,2,
+	 "Task A: Vertical input window start"},
+	{R_9A_A_VERT_INPUT_WINDOW_LENGTH,2,
+	 "Task A: Vertical input window length"},
+	{R_9C_A_HORIZ_OUTPUT_WINDOW_LENGTH,2,
+	 "Task A: Horizontal output window length"},
+	{R_9E_A_VERT_OUTPUT_WINDOW_LENGTH,2,
+	 "Task A: Vertical output window length"},
+
+	/* Task A: FIR filtering and prescaling */
+	{R_A0_A_HORIZ_PRESCALING,1,
+	 "Task A: Horizontal prescaling"},
+	{R_A1_A_ACCUMULATION_LENGTH,1,
+	 "Task A: Accumulation length"},
+	{R_A2_A_PRESCALER_DC_GAIN_AND_FIR_PREFILTER,1,
+	 "Task A: Prescaler DC gain and FIR prefilter"},
+	/* 0xa3 - Reserved */
+	{R_A4_A_LUMA_BRIGHTNESS_CNTL,1,
+	 "Task A: Luminance brightness control"},
+	{R_A5_A_LUMA_CONTRAST_CNTL,1,
+	 "Task A: Luminance contrast control"},
+	{R_A6_A_CHROMA_SATURATION_CNTL,1,
+	 "Task A: Chrominance saturation control"},
+	/* 0xa7 - Reserved */
+
+	/* Task A: Horizontal phase scaling */
+	{R_A8_A_HORIZ_LUMA_SCALING_INC,2,
+	 "Task A: Horizontal luminance scaling increment"},
+	{R_AA_A_HORIZ_LUMA_PHASE_OFF,1,
+	 "Task A: Horizontal luminance phase offset"},
+	/* 0xab - Reserved */
+	{R_AC_A_HORIZ_CHROMA_SCALING_INC,2,
+	 "Task A: Horizontal chrominance scaling increment"},
+	{R_AE_A_HORIZ_CHROMA_PHASE_OFF,1,
+	 "Task A: Horizontal chrominance phase offset"},
+	/* 0xaf - Reserved */
+
+	/* Task A: Vertical scaling */
+	{R_B0_A_VERT_LUMA_SCALING_INC,2,
+	 "Task A: Vertical luminance scaling increment"},
+	{R_B2_A_VERT_CHROMA_SCALING_INC,2,
+	 "Task A: Vertical chrominance scaling increment"},
+	{R_B4_A_VERT_SCALING_MODE_CNTL,1,
+	 "Task A: Vertical scaling mode control"},
+	/* 0xb5-0xb7 - Reserved */
+	{R_B8_A_VERT_CHROMA_PHASE_OFF_00,1,
+	 "Task A: Vertical chrominance phase offset '00'"},
+	{R_B9_A_VERT_CHROMA_PHASE_OFF_01,1,
+	 "Task A: Vertical chrominance phase offset '01'"},
+	{R_BA_A_VERT_CHROMA_PHASE_OFF_10,1,
+	 "Task A: Vertical chrominance phase offset '10'"},
+	{R_BB_A_VERT_CHROMA_PHASE_OFF_11,1,
+	 "Task A: Vertical chrominance phase offset '11'"},
+	{R_BC_A_VERT_LUMA_PHASE_OFF_00,1,
+	 "Task A: Vertical luminance phase offset '00'"},
+	{R_BD_A_VERT_LUMA_PHASE_OFF_01,1,
+	 "Task A: Vertical luminance phase offset '01'"},
+	{R_BE_A_VERT_LUMA_PHASE_OFF_10,1,
+	 "Task A: Vertical luminance phase offset '10'"},
+	{R_BF_A_VERT_LUMA_PHASE_OFF_11,1,
+	 "Task A: Vertical luminance phase offset '11'"},
+
+	/* Task B definition: R_C0_B_TASK_HANDLING_CNTL to R_EF_B_VERT_LUMA_PHASE_OFF_11 */
+	/* Task B: Basic settings and acquisition window definition */
+	{R_C0_B_TASK_HANDLING_CNTL,1,
+	 "Task B: Task handling control"},
+	{R_C1_B_X_PORT_FORMATS_AND_CONF,1,
+	 "Task B: X port formats and configuration"},
+	{R_C2_B_INPUT_REFERENCE_SIGNAL_DEFINITION,1,
+	 "Task B: Input reference signal definition"},
+	{R_C3_B_I_PORT_FORMATS_AND_CONF,1,
+	 "Task B: I port formats and configuration"},
+	{R_C4_B_HORIZ_INPUT_WINDOW_START,2,
+	 "Task B: Horizontal input window start"},
+	{R_C6_B_HORIZ_INPUT_WINDOW_LENGTH,2,
+	 "Task B: Horizontal input window length"},
+	{R_C8_B_VERT_INPUT_WINDOW_START,2,
+	 "Task B: Vertical input window start"},
+	{R_CA_B_VERT_INPUT_WINDOW_LENGTH,2,
+	 "Task B: Vertical input window length"},
+	{R_CC_B_HORIZ_OUTPUT_WINDOW_LENGTH,2,
+	 "Task B: Horizontal output window length"},
+	{R_CE_B_VERT_OUTPUT_WINDOW_LENGTH,2,
+	 "Task B: Vertical output window length"},
+
+	/* Task B: FIR filtering and prescaling */
+	{R_D0_B_HORIZ_PRESCALING,1,
+	 "Task B: Horizontal prescaling"},
+	{R_D1_B_ACCUMULATION_LENGTH,1,
+	 "Task B: Accumulation length"},
+	{R_D2_B_PRESCALER_DC_GAIN_AND_FIR_PREFILTER,1,
+	 "Task B: Prescaler DC gain and FIR prefilter"},
+	/* 0xd3 - Reserved */
+	{R_D4_B_LUMA_BRIGHTNESS_CNTL,1,
+	 "Task B: Luminance brightness control"},
+	{R_D5_B_LUMA_CONTRAST_CNTL,1,
+	 "Task B: Luminance contrast control"},
+	{R_D6_B_CHROMA_SATURATION_CNTL,1,
+	 "Task B: Chrominance saturation control"},
+	/* 0xd7 - Reserved */
+
+	/* Task B: Horizontal phase scaling */
+	{R_D8_B_HORIZ_LUMA_SCALING_INC,2,
+	 "Task B: Horizontal luminance scaling increment"},
+	{R_DA_B_HORIZ_LUMA_PHASE_OFF,1,
+	 "Task B: Horizontal luminance phase offset"},
+	/* 0xdb - Reserved */
+	{R_DC_B_HORIZ_CHROMA_SCALING,2,
+	 "Task B: Horizontal chrominance scaling"},
+	{R_DE_B_HORIZ_PHASE_OFFSET_CRHOMA,1,
+	 "Task B: Horizontal Phase Offset Chroma"},
+	/* 0xdf - Reserved */
+
+	/* Task B: Vertical scaling */
+	{R_E0_B_VERT_LUMA_SCALING_INC,2,
+	 "Task B: Vertical luminance scaling increment"},
+	{R_E2_B_VERT_CHROMA_SCALING_INC,2,
+	 "Task B: Vertical chrominance scaling increment"},
+	{R_E4_B_VERT_SCALING_MODE_CNTL,1,
+	 "Task B: Vertical scaling mode control"},
+	/* 0xe5-0xe7 - Reserved */
+	{R_E8_B_VERT_CHROMA_PHASE_OFF_00,1,
+	 "Task B: Vertical chrominance phase offset '00'"},
+	{R_E9_B_VERT_CHROMA_PHASE_OFF_01,1,
+	 "Task B: Vertical chrominance phase offset '01'"},
+	{R_EA_B_VERT_CHROMA_PHASE_OFF_10,1,
+	 "Task B: Vertical chrominance phase offset '10'"},
+	{R_EB_B_VERT_CHROMA_PHASE_OFF_11,1,
+	 "Task B: Vertical chrominance phase offset '11'"},
+	{R_EC_B_VERT_LUMA_PHASE_OFF_00,1,
+	 "Task B: Vertical luminance phase offset '00'"},
+	{R_ED_B_VERT_LUMA_PHASE_OFF_01,1,
+	 "Task B: Vertical luminance phase offset '01'"},
+	{R_EE_B_VERT_LUMA_PHASE_OFF_10,1,
+	 "Task B: Vertical luminance phase offset '10'"},
+	{R_EF_B_VERT_LUMA_PHASE_OFF_11,1,
+	 "Task B: Vertical luminance phase offset '11'"},
+
+	/* second PLL (PLL2) and Pulsegenerator Programming */
+	{ R_F0_LFCO_PER_LINE, 1,
+	  "LFCO's per line"},
+	{ R_F1_P_I_PARAM_SELECT,1,
+	  "P-/I- Param. Select., PLL Mode, PLL H-Src., LFCO's per line"},
+	{ R_F2_NOMINAL_PLL2_DTO,1,
+	 "Nominal PLL2 DTO"},
+	{R_F3_PLL_INCREMENT,1,
+	 "PLL2 Increment"},
+	{R_F4_PLL2_STATUS,1,
+	 "PLL2 Status"},
+	{R_F5_PULSGEN_LINE_LENGTH,1,
+	 "Pulsgen. line length"},
+	{R_F6_PULSE_A_POS_LSB_AND_PULSEGEN_CONFIG,1,
+	 "Pulse A Position, Pulsgen Resync., Pulsgen. H-Src., Pulsgen. line length"},
+	{R_F7_PULSE_A_POS_MSB,1,
+	 "Pulse A Position"},
+	{R_F8_PULSE_B_POS,2,
+	 "Pulse B Position"},
+	{R_FA_PULSE_C_POS,2,
+	 "Pulse C Position"},
+	/* 0xfc to 0xfe - Reserved */
+	{R_FF_S_PLL_MAX_PHASE_ERR_THRESH_NUM_LINES,1,
+	 "S_PLL max. phase, error threshold, PLL2 no. of lines, threshold"},
+};
+#endif
diff --git a/drivers/media/i2c/saa7127.c b/drivers/media/i2c/saa7127.c
new file mode 100644
index 000000000..818ed19cf
--- /dev/null
+++ b/drivers/media/i2c/saa7127.c
@@ -0,0 +1,818 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * saa7127 - Philips SAA7127/SAA7129 video encoder driver
+ *
+ * Copyright (C) 2003 Roy Bulter <rbulter@hetnet.nl>
+ *
+ * Based on SAA7126 video encoder driver by Gillem & Andreas Oberritter
+ *
+ * Copyright (C) 2000-2001 Gillem <htoa@gmx.net>
+ * Copyright (C) 2002 Andreas Oberritter <obi@saftware.de>
+ *
+ * Based on Stadis 4:2:2 MPEG-2 Decoder Driver by Nathan Laredo
+ *
+ * Copyright (C) 1999 Nathan Laredo <laredo@gnu.org>
+ *
+ * This driver is designed for the Hauppauge 250/350 Linux driver
+ * from the ivtv Project
+ *
+ * Copyright (C) 2003 Kevin Thayer <nufan_wfk@yahoo.com>
+ *
+ * Dual output support:
+ * Copyright (C) 2004 Eric Varsanyi
+ *
+ * NTSC Tuning and 7.5 IRE Setup
+ * Copyright (C) 2004  Chris Kennedy <c@groovy.org>
+ *
+ * VBI additions & cleanup:
+ * Copyright (C) 2004, 2005 Hans Verkuil <hverkuil@xs4all.nl>
+ *
+ * Note: the saa7126 is identical to the saa7127, and the saa7128 is
+ * identical to the saa7129, except that the saa7126 and saa7128 have
+ * macrovision anti-taping support. This driver will almost certainly
+ * work fine for those chips, except of course for the missing anti-taping
+ * support.
+ */
+
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/i2c/saa7127.h>
+
+static int debug;
+static int test_image;
+
+MODULE_DESCRIPTION("Philips SAA7127/9 video encoder driver");
+MODULE_AUTHOR("Kevin Thayer, Chris Kennedy, Hans Verkuil");
+MODULE_LICENSE("GPL");
+module_param(debug, int, 0644);
+module_param(test_image, int, 0644);
+MODULE_PARM_DESC(debug, "debug level (0-2)");
+MODULE_PARM_DESC(test_image, "test_image (0-1)");
+
+
+/*
+ * SAA7127 registers
+ */
+
+#define SAA7127_REG_STATUS                           0x00
+#define SAA7127_REG_WIDESCREEN_CONFIG                0x26
+#define SAA7127_REG_WIDESCREEN_ENABLE                0x27
+#define SAA7127_REG_BURST_START                      0x28
+#define SAA7127_REG_BURST_END                        0x29
+#define SAA7127_REG_COPYGEN_0                        0x2a
+#define SAA7127_REG_COPYGEN_1                        0x2b
+#define SAA7127_REG_COPYGEN_2                        0x2c
+#define SAA7127_REG_OUTPUT_PORT_CONTROL              0x2d
+#define SAA7127_REG_GAIN_LUMINANCE_RGB               0x38
+#define SAA7127_REG_GAIN_COLORDIFF_RGB               0x39
+#define SAA7127_REG_INPUT_PORT_CONTROL_1             0x3a
+#define SAA7129_REG_FADE_KEY_COL2		     0x4f
+#define SAA7127_REG_CHROMA_PHASE                     0x5a
+#define SAA7127_REG_GAINU                            0x5b
+#define SAA7127_REG_GAINV                            0x5c
+#define SAA7127_REG_BLACK_LEVEL                      0x5d
+#define SAA7127_REG_BLANKING_LEVEL                   0x5e
+#define SAA7127_REG_VBI_BLANKING                     0x5f
+#define SAA7127_REG_DAC_CONTROL                      0x61
+#define SAA7127_REG_BURST_AMP                        0x62
+#define SAA7127_REG_SUBC3                            0x63
+#define SAA7127_REG_SUBC2                            0x64
+#define SAA7127_REG_SUBC1                            0x65
+#define SAA7127_REG_SUBC0                            0x66
+#define SAA7127_REG_LINE_21_ODD_0                    0x67
+#define SAA7127_REG_LINE_21_ODD_1                    0x68
+#define SAA7127_REG_LINE_21_EVEN_0                   0x69
+#define SAA7127_REG_LINE_21_EVEN_1                   0x6a
+#define SAA7127_REG_RCV_PORT_CONTROL                 0x6b
+#define SAA7127_REG_VTRIG                            0x6c
+#define SAA7127_REG_HTRIG_HI                         0x6d
+#define SAA7127_REG_MULTI                            0x6e
+#define SAA7127_REG_CLOSED_CAPTION                   0x6f
+#define SAA7127_REG_RCV2_OUTPUT_START                0x70
+#define SAA7127_REG_RCV2_OUTPUT_END                  0x71
+#define SAA7127_REG_RCV2_OUTPUT_MSBS                 0x72
+#define SAA7127_REG_TTX_REQUEST_H_START              0x73
+#define SAA7127_REG_TTX_REQUEST_H_DELAY_LENGTH       0x74
+#define SAA7127_REG_CSYNC_ADVANCE_VSYNC_SHIFT        0x75
+#define SAA7127_REG_TTX_ODD_REQ_VERT_START           0x76
+#define SAA7127_REG_TTX_ODD_REQ_VERT_END             0x77
+#define SAA7127_REG_TTX_EVEN_REQ_VERT_START          0x78
+#define SAA7127_REG_TTX_EVEN_REQ_VERT_END            0x79
+#define SAA7127_REG_FIRST_ACTIVE                     0x7a
+#define SAA7127_REG_LAST_ACTIVE                      0x7b
+#define SAA7127_REG_MSB_VERTICAL                     0x7c
+#define SAA7127_REG_DISABLE_TTX_LINE_LO_0            0x7e
+#define SAA7127_REG_DISABLE_TTX_LINE_LO_1            0x7f
+
+/*
+ **********************************************************************
+ *
+ *  Arrays with configuration parameters for the SAA7127
+ *
+ **********************************************************************
+ */
+
+struct i2c_reg_value {
+	unsigned char reg;
+	unsigned char value;
+};
+
+static const struct i2c_reg_value saa7129_init_config_extra[] = {
+	{ SAA7127_REG_OUTPUT_PORT_CONTROL,		0x38 },
+	{ SAA7127_REG_VTRIG,				0xfa },
+	{ 0, 0 }
+};
+
+static const struct i2c_reg_value saa7127_init_config_common[] = {
+	{ SAA7127_REG_WIDESCREEN_CONFIG,		0x0d },
+	{ SAA7127_REG_WIDESCREEN_ENABLE,		0x00 },
+	{ SAA7127_REG_COPYGEN_0,			0x77 },
+	{ SAA7127_REG_COPYGEN_1,			0x41 },
+	{ SAA7127_REG_COPYGEN_2,			0x00 },	/* Macrovision enable/disable */
+	{ SAA7127_REG_OUTPUT_PORT_CONTROL,		0xbf },
+	{ SAA7127_REG_GAIN_LUMINANCE_RGB,		0x00 },
+	{ SAA7127_REG_GAIN_COLORDIFF_RGB,		0x00 },
+	{ SAA7127_REG_INPUT_PORT_CONTROL_1,		0x80 },	/* for color bars */
+	{ SAA7127_REG_LINE_21_ODD_0,			0x77 },
+	{ SAA7127_REG_LINE_21_ODD_1,			0x41 },
+	{ SAA7127_REG_LINE_21_EVEN_0,			0x88 },
+	{ SAA7127_REG_LINE_21_EVEN_1,			0x41 },
+	{ SAA7127_REG_RCV_PORT_CONTROL,			0x12 },
+	{ SAA7127_REG_VTRIG,				0xf9 },
+	{ SAA7127_REG_HTRIG_HI,				0x00 },
+	{ SAA7127_REG_RCV2_OUTPUT_START,		0x41 },
+	{ SAA7127_REG_RCV2_OUTPUT_END,			0xc3 },
+	{ SAA7127_REG_RCV2_OUTPUT_MSBS,			0x00 },
+	{ SAA7127_REG_TTX_REQUEST_H_START,		0x3e },
+	{ SAA7127_REG_TTX_REQUEST_H_DELAY_LENGTH,	0xb8 },
+	{ SAA7127_REG_CSYNC_ADVANCE_VSYNC_SHIFT,	0x03 },
+	{ SAA7127_REG_TTX_ODD_REQ_VERT_START,		0x15 },
+	{ SAA7127_REG_TTX_ODD_REQ_VERT_END,		0x16 },
+	{ SAA7127_REG_TTX_EVEN_REQ_VERT_START,		0x15 },
+	{ SAA7127_REG_TTX_EVEN_REQ_VERT_END,		0x16 },
+	{ SAA7127_REG_FIRST_ACTIVE,			0x1a },
+	{ SAA7127_REG_LAST_ACTIVE,			0x01 },
+	{ SAA7127_REG_MSB_VERTICAL,			0xc0 },
+	{ SAA7127_REG_DISABLE_TTX_LINE_LO_0,		0x00 },
+	{ SAA7127_REG_DISABLE_TTX_LINE_LO_1,		0x00 },
+	{ 0, 0 }
+};
+
+#define SAA7127_60HZ_DAC_CONTROL 0x15
+static const struct i2c_reg_value saa7127_init_config_60hz[] = {
+	{ SAA7127_REG_BURST_START,			0x19 },
+	/* BURST_END is also used as a chip ID in saa7127_probe */
+	{ SAA7127_REG_BURST_END,			0x1d },
+	{ SAA7127_REG_CHROMA_PHASE,			0xa3 },
+	{ SAA7127_REG_GAINU,				0x98 },
+	{ SAA7127_REG_GAINV,				0xd3 },
+	{ SAA7127_REG_BLACK_LEVEL,			0x39 },
+	{ SAA7127_REG_BLANKING_LEVEL,			0x2e },
+	{ SAA7127_REG_VBI_BLANKING,			0x2e },
+	{ SAA7127_REG_DAC_CONTROL,			0x15 },
+	{ SAA7127_REG_BURST_AMP,			0x4d },
+	{ SAA7127_REG_SUBC3,				0x1f },
+	{ SAA7127_REG_SUBC2,				0x7c },
+	{ SAA7127_REG_SUBC1,				0xf0 },
+	{ SAA7127_REG_SUBC0,				0x21 },
+	{ SAA7127_REG_MULTI,				0x90 },
+	{ SAA7127_REG_CLOSED_CAPTION,			0x11 },
+	{ 0, 0 }
+};
+
+#define SAA7127_50HZ_PAL_DAC_CONTROL 0x02
+static struct i2c_reg_value saa7127_init_config_50hz_pal[] = {
+	{ SAA7127_REG_BURST_START,			0x21 },
+	/* BURST_END is also used as a chip ID in saa7127_probe */
+	{ SAA7127_REG_BURST_END,			0x1d },
+	{ SAA7127_REG_CHROMA_PHASE,			0x3f },
+	{ SAA7127_REG_GAINU,				0x7d },
+	{ SAA7127_REG_GAINV,				0xaf },
+	{ SAA7127_REG_BLACK_LEVEL,			0x33 },
+	{ SAA7127_REG_BLANKING_LEVEL,			0x35 },
+	{ SAA7127_REG_VBI_BLANKING,			0x35 },
+	{ SAA7127_REG_DAC_CONTROL,			0x02 },
+	{ SAA7127_REG_BURST_AMP,			0x2f },
+	{ SAA7127_REG_SUBC3,				0xcb },
+	{ SAA7127_REG_SUBC2,				0x8a },
+	{ SAA7127_REG_SUBC1,				0x09 },
+	{ SAA7127_REG_SUBC0,				0x2a },
+	{ SAA7127_REG_MULTI,				0xa0 },
+	{ SAA7127_REG_CLOSED_CAPTION,			0x00 },
+	{ 0, 0 }
+};
+
+#define SAA7127_50HZ_SECAM_DAC_CONTROL 0x08
+static struct i2c_reg_value saa7127_init_config_50hz_secam[] = {
+	{ SAA7127_REG_BURST_START,			0x21 },
+	/* BURST_END is also used as a chip ID in saa7127_probe */
+	{ SAA7127_REG_BURST_END,			0x1d },
+	{ SAA7127_REG_CHROMA_PHASE,			0x3f },
+	{ SAA7127_REG_GAINU,				0x6a },
+	{ SAA7127_REG_GAINV,				0x81 },
+	{ SAA7127_REG_BLACK_LEVEL,			0x33 },
+	{ SAA7127_REG_BLANKING_LEVEL,			0x35 },
+	{ SAA7127_REG_VBI_BLANKING,			0x35 },
+	{ SAA7127_REG_DAC_CONTROL,			0x08 },
+	{ SAA7127_REG_BURST_AMP,			0x2f },
+	{ SAA7127_REG_SUBC3,				0xb2 },
+	{ SAA7127_REG_SUBC2,				0x3b },
+	{ SAA7127_REG_SUBC1,				0xa3 },
+	{ SAA7127_REG_SUBC0,				0x28 },
+	{ SAA7127_REG_MULTI,				0x90 },
+	{ SAA7127_REG_CLOSED_CAPTION,			0x00 },
+	{ 0, 0 }
+};
+
+/*
+ **********************************************************************
+ *
+ *  Encoder Struct, holds the configuration state of the encoder
+ *
+ **********************************************************************
+ */
+
+enum saa712x_model {
+	SAA7127,
+	SAA7129,
+};
+
+struct saa7127_state {
+	struct v4l2_subdev sd;
+	v4l2_std_id std;
+	enum saa712x_model ident;
+	enum saa7127_input_type input_type;
+	enum saa7127_output_type output_type;
+	int video_enable;
+	int wss_enable;
+	u16 wss_mode;
+	int cc_enable;
+	u16 cc_data;
+	int xds_enable;
+	u16 xds_data;
+	int vps_enable;
+	u8 vps_data[5];
+	u8 reg_2d;
+	u8 reg_3a;
+	u8 reg_3a_cb;   /* colorbar bit */
+	u8 reg_61;
+};
+
+static inline struct saa7127_state *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct saa7127_state, sd);
+}
+
+static const char * const output_strs[] =
+{
+	"S-Video + Composite",
+	"Composite",
+	"S-Video",
+	"RGB",
+	"YUV C",
+	"YUV V"
+};
+
+static const char * const wss_strs[] = {
+	"invalid",
+	"letterbox 14:9 center",
+	"letterbox 14:9 top",
+	"invalid",
+	"letterbox 16:9 top",
+	"invalid",
+	"invalid",
+	"16:9 full format anamorphic",
+	"4:3 full format",
+	"invalid",
+	"invalid",
+	"letterbox 16:9 center",
+	"invalid",
+	"letterbox >16:9 center",
+	"14:9 full format center",
+	"invalid",
+};
+
+/* ----------------------------------------------------------------------- */
+
+static int saa7127_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return i2c_smbus_read_byte_data(client, reg);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static int saa7127_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int i;
+
+	for (i = 0; i < 3; i++) {
+		if (i2c_smbus_write_byte_data(client, reg, val) == 0)
+			return 0;
+	}
+	v4l2_err(sd, "I2C Write Problem\n");
+	return -1;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static int saa7127_write_inittab(struct v4l2_subdev *sd,
+				 const struct i2c_reg_value *regs)
+{
+	while (regs->reg != 0) {
+		saa7127_write(sd, regs->reg, regs->value);
+		regs++;
+	}
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static int saa7127_set_vps(struct v4l2_subdev *sd, const struct v4l2_sliced_vbi_data *data)
+{
+	struct saa7127_state *state = to_state(sd);
+	int enable = (data->line != 0);
+
+	if (enable && (data->field != 0 || data->line != 16))
+		return -EINVAL;
+	if (state->vps_enable != enable) {
+		v4l2_dbg(1, debug, sd, "Turn VPS Signal %s\n", enable ? "on" : "off");
+		saa7127_write(sd, 0x54, enable << 7);
+		state->vps_enable = enable;
+	}
+	if (!enable)
+		return 0;
+
+	state->vps_data[0] = data->data[2];
+	state->vps_data[1] = data->data[8];
+	state->vps_data[2] = data->data[9];
+	state->vps_data[3] = data->data[10];
+	state->vps_data[4] = data->data[11];
+	v4l2_dbg(1, debug, sd, "Set VPS data %*ph\n", 5, state->vps_data);
+	saa7127_write(sd, 0x55, state->vps_data[0]);
+	saa7127_write(sd, 0x56, state->vps_data[1]);
+	saa7127_write(sd, 0x57, state->vps_data[2]);
+	saa7127_write(sd, 0x58, state->vps_data[3]);
+	saa7127_write(sd, 0x59, state->vps_data[4]);
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static int saa7127_set_cc(struct v4l2_subdev *sd, const struct v4l2_sliced_vbi_data *data)
+{
+	struct saa7127_state *state = to_state(sd);
+	u16 cc = data->data[1] << 8 | data->data[0];
+	int enable = (data->line != 0);
+
+	if (enable && (data->field != 0 || data->line != 21))
+		return -EINVAL;
+	if (state->cc_enable != enable) {
+		v4l2_dbg(1, debug, sd,
+			"Turn CC %s\n", enable ? "on" : "off");
+		saa7127_write(sd, SAA7127_REG_CLOSED_CAPTION,
+			(state->xds_enable << 7) | (enable << 6) | 0x11);
+		state->cc_enable = enable;
+	}
+	if (!enable)
+		return 0;
+
+	v4l2_dbg(2, debug, sd, "CC data: %04x\n", cc);
+	saa7127_write(sd, SAA7127_REG_LINE_21_ODD_0, cc & 0xff);
+	saa7127_write(sd, SAA7127_REG_LINE_21_ODD_1, cc >> 8);
+	state->cc_data = cc;
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static int saa7127_set_xds(struct v4l2_subdev *sd, const struct v4l2_sliced_vbi_data *data)
+{
+	struct saa7127_state *state = to_state(sd);
+	u16 xds = data->data[1] << 8 | data->data[0];
+	int enable = (data->line != 0);
+
+	if (enable && (data->field != 1 || data->line != 21))
+		return -EINVAL;
+	if (state->xds_enable != enable) {
+		v4l2_dbg(1, debug, sd, "Turn XDS %s\n", enable ? "on" : "off");
+		saa7127_write(sd, SAA7127_REG_CLOSED_CAPTION,
+				(enable << 7) | (state->cc_enable << 6) | 0x11);
+		state->xds_enable = enable;
+	}
+	if (!enable)
+		return 0;
+
+	v4l2_dbg(2, debug, sd, "XDS data: %04x\n", xds);
+	saa7127_write(sd, SAA7127_REG_LINE_21_EVEN_0, xds & 0xff);
+	saa7127_write(sd, SAA7127_REG_LINE_21_EVEN_1, xds >> 8);
+	state->xds_data = xds;
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static int saa7127_set_wss(struct v4l2_subdev *sd, const struct v4l2_sliced_vbi_data *data)
+{
+	struct saa7127_state *state = to_state(sd);
+	int enable = (data->line != 0);
+
+	if (enable && (data->field != 0 || data->line != 23))
+		return -EINVAL;
+	if (state->wss_enable != enable) {
+		v4l2_dbg(1, debug, sd, "Turn WSS %s\n", enable ? "on" : "off");
+		saa7127_write(sd, 0x27, enable << 7);
+		state->wss_enable = enable;
+	}
+	if (!enable)
+		return 0;
+
+	saa7127_write(sd, 0x26, data->data[0]);
+	saa7127_write(sd, 0x27, 0x80 | (data->data[1] & 0x3f));
+	v4l2_dbg(1, debug, sd,
+		"WSS mode: %s\n", wss_strs[data->data[0] & 0xf]);
+	state->wss_mode = (data->data[1] & 0x3f) << 8 | data->data[0];
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static int saa7127_set_video_enable(struct v4l2_subdev *sd, int enable)
+{
+	struct saa7127_state *state = to_state(sd);
+
+	if (enable) {
+		v4l2_dbg(1, debug, sd, "Enable Video Output\n");
+		saa7127_write(sd, 0x2d, state->reg_2d);
+		saa7127_write(sd, 0x61, state->reg_61);
+	} else {
+		v4l2_dbg(1, debug, sd, "Disable Video Output\n");
+		saa7127_write(sd, 0x2d, (state->reg_2d & 0xf0));
+		saa7127_write(sd, 0x61, (state->reg_61 | 0xc0));
+	}
+	state->video_enable = enable;
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static int saa7127_set_std(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct saa7127_state *state = to_state(sd);
+	const struct i2c_reg_value *inittab;
+
+	if (std & V4L2_STD_525_60) {
+		v4l2_dbg(1, debug, sd, "Selecting 60 Hz video Standard\n");
+		inittab = saa7127_init_config_60hz;
+		state->reg_61 = SAA7127_60HZ_DAC_CONTROL;
+
+	} else if (state->ident == SAA7129 &&
+		   (std & V4L2_STD_SECAM) &&
+		   !(std & (V4L2_STD_625_50 & ~V4L2_STD_SECAM))) {
+
+		/* If and only if SECAM, with a SAA712[89] */
+		v4l2_dbg(1, debug, sd,
+			 "Selecting 50 Hz SECAM video Standard\n");
+		inittab = saa7127_init_config_50hz_secam;
+		state->reg_61 = SAA7127_50HZ_SECAM_DAC_CONTROL;
+
+	} else {
+		v4l2_dbg(1, debug, sd, "Selecting 50 Hz PAL video Standard\n");
+		inittab = saa7127_init_config_50hz_pal;
+		state->reg_61 = SAA7127_50HZ_PAL_DAC_CONTROL;
+	}
+
+	/* Write Table */
+	saa7127_write_inittab(sd, inittab);
+	state->std = std;
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static int saa7127_set_output_type(struct v4l2_subdev *sd, int output)
+{
+	struct saa7127_state *state = to_state(sd);
+
+	switch (output) {
+	case SAA7127_OUTPUT_TYPE_RGB:
+		state->reg_2d = 0x0f;	/* RGB + CVBS (for sync) */
+		state->reg_3a = 0x13;	/* by default switch YUV to RGB-matrix on */
+		break;
+
+	case SAA7127_OUTPUT_TYPE_COMPOSITE:
+		if (state->ident == SAA7129)
+			state->reg_2d = 0x20;	/* CVBS only */
+		else
+			state->reg_2d = 0x08;	/* 00001000 CVBS only, RGB DAC's off (high impedance mode) */
+		state->reg_3a = 0x13;	/* by default switch YUV to RGB-matrix on */
+		break;
+
+	case SAA7127_OUTPUT_TYPE_SVIDEO:
+		if (state->ident == SAA7129)
+			state->reg_2d = 0x18;	/* Y + C */
+		else
+			state->reg_2d = 0xff;   /*11111111  croma -> R, luma -> CVBS + G + B */
+		state->reg_3a = 0x13;	/* by default switch YUV to RGB-matrix on */
+		break;
+
+	case SAA7127_OUTPUT_TYPE_YUV_V:
+		state->reg_2d = 0x4f;	/* reg 2D = 01001111, all DAC's on, RGB + VBS */
+		state->reg_3a = 0x0b;	/* reg 3A = 00001011, bypass RGB-matrix */
+		break;
+
+	case SAA7127_OUTPUT_TYPE_YUV_C:
+		state->reg_2d = 0x0f;	/* reg 2D = 00001111, all DAC's on, RGB + CVBS */
+		state->reg_3a = 0x0b;	/* reg 3A = 00001011, bypass RGB-matrix */
+		break;
+
+	case SAA7127_OUTPUT_TYPE_BOTH:
+		if (state->ident == SAA7129)
+			state->reg_2d = 0x38;
+		else
+			state->reg_2d = 0xbf;
+		state->reg_3a = 0x13;	/* by default switch YUV to RGB-matrix on */
+		break;
+
+	default:
+		return -EINVAL;
+	}
+	v4l2_dbg(1, debug, sd,
+		"Selecting %s output type\n", output_strs[output]);
+
+	/* Configure Encoder */
+	saa7127_write(sd, 0x2d, state->reg_2d);
+	saa7127_write(sd, 0x3a, state->reg_3a | state->reg_3a_cb);
+	state->output_type = output;
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static int saa7127_set_input_type(struct v4l2_subdev *sd, int input)
+{
+	struct saa7127_state *state = to_state(sd);
+
+	switch (input) {
+	case SAA7127_INPUT_TYPE_NORMAL:	/* avia */
+		v4l2_dbg(1, debug, sd, "Selecting Normal Encoder Input\n");
+		state->reg_3a_cb = 0;
+		break;
+
+	case SAA7127_INPUT_TYPE_TEST_IMAGE:	/* color bar */
+		v4l2_dbg(1, debug, sd, "Selecting Color Bar generator\n");
+		state->reg_3a_cb = 0x80;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+	saa7127_write(sd, 0x3a, state->reg_3a | state->reg_3a_cb);
+	state->input_type = input;
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static int saa7127_s_std_output(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct saa7127_state *state = to_state(sd);
+
+	if (state->std == std)
+		return 0;
+	return saa7127_set_std(sd, std);
+}
+
+static int saa7127_s_routing(struct v4l2_subdev *sd,
+			     u32 input, u32 output, u32 config)
+{
+	struct saa7127_state *state = to_state(sd);
+	int rc = 0;
+
+	if (state->input_type != input)
+		rc = saa7127_set_input_type(sd, input);
+	if (rc == 0 && state->output_type != output)
+		rc = saa7127_set_output_type(sd, output);
+	return rc;
+}
+
+static int saa7127_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct saa7127_state *state = to_state(sd);
+
+	if (state->video_enable == enable)
+		return 0;
+	return saa7127_set_video_enable(sd, enable);
+}
+
+static int saa7127_g_sliced_fmt(struct v4l2_subdev *sd, struct v4l2_sliced_vbi_format *fmt)
+{
+	struct saa7127_state *state = to_state(sd);
+
+	memset(fmt->service_lines, 0, sizeof(fmt->service_lines));
+	if (state->vps_enable)
+		fmt->service_lines[0][16] = V4L2_SLICED_VPS;
+	if (state->wss_enable)
+		fmt->service_lines[0][23] = V4L2_SLICED_WSS_625;
+	if (state->cc_enable) {
+		fmt->service_lines[0][21] = V4L2_SLICED_CAPTION_525;
+		fmt->service_lines[1][21] = V4L2_SLICED_CAPTION_525;
+	}
+	fmt->service_set =
+		(state->vps_enable ? V4L2_SLICED_VPS : 0) |
+		(state->wss_enable ? V4L2_SLICED_WSS_625 : 0) |
+		(state->cc_enable ? V4L2_SLICED_CAPTION_525 : 0);
+	return 0;
+}
+
+static int saa7127_s_vbi_data(struct v4l2_subdev *sd, const struct v4l2_sliced_vbi_data *data)
+{
+	switch (data->id) {
+	case V4L2_SLICED_WSS_625:
+		return saa7127_set_wss(sd, data);
+	case V4L2_SLICED_VPS:
+		return saa7127_set_vps(sd, data);
+	case V4L2_SLICED_CAPTION_525:
+		if (data->field == 0)
+			return saa7127_set_cc(sd, data);
+		return saa7127_set_xds(sd, data);
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int saa7127_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+{
+	reg->val = saa7127_read(sd, reg->reg & 0xff);
+	reg->size = 1;
+	return 0;
+}
+
+static int saa7127_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
+{
+	saa7127_write(sd, reg->reg & 0xff, reg->val & 0xff);
+	return 0;
+}
+#endif
+
+static int saa7127_log_status(struct v4l2_subdev *sd)
+{
+	struct saa7127_state *state = to_state(sd);
+
+	v4l2_info(sd, "Standard: %s\n", (state->std & V4L2_STD_525_60) ? "60 Hz" : "50 Hz");
+	v4l2_info(sd, "Input:    %s\n", state->input_type ?  "color bars" : "normal");
+	v4l2_info(sd, "Output:   %s\n", state->video_enable ?
+			output_strs[state->output_type] : "disabled");
+	v4l2_info(sd, "WSS:      %s\n", state->wss_enable ?
+			wss_strs[state->wss_mode] : "disabled");
+	v4l2_info(sd, "VPS:      %s\n", state->vps_enable ? "enabled" : "disabled");
+	v4l2_info(sd, "CC:       %s\n", state->cc_enable ? "enabled" : "disabled");
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_core_ops saa7127_core_ops = {
+	.log_status = saa7127_log_status,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = saa7127_g_register,
+	.s_register = saa7127_s_register,
+#endif
+};
+
+static const struct v4l2_subdev_video_ops saa7127_video_ops = {
+	.s_std_output = saa7127_s_std_output,
+	.s_routing = saa7127_s_routing,
+	.s_stream = saa7127_s_stream,
+};
+
+static const struct v4l2_subdev_vbi_ops saa7127_vbi_ops = {
+	.s_vbi_data = saa7127_s_vbi_data,
+	.g_sliced_fmt = saa7127_g_sliced_fmt,
+};
+
+static const struct v4l2_subdev_ops saa7127_ops = {
+	.core = &saa7127_core_ops,
+	.video = &saa7127_video_ops,
+	.vbi = &saa7127_vbi_ops,
+};
+
+/* ----------------------------------------------------------------------- */
+
+static int saa7127_probe(struct i2c_client *client)
+{
+	const struct i2c_device_id *id = i2c_client_get_device_id(client);
+	struct saa7127_state *state;
+	struct v4l2_subdev *sd;
+	struct v4l2_sliced_vbi_data vbi = { 0, 0, 0, 0 };  /* set to disabled */
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+
+	v4l_dbg(1, debug, client, "detecting saa7127 client on address 0x%x\n",
+			client->addr << 1);
+
+	state = devm_kzalloc(&client->dev, sizeof(*state), GFP_KERNEL);
+	if (state == NULL)
+		return -ENOMEM;
+
+	sd = &state->sd;
+	v4l2_i2c_subdev_init(sd, client, &saa7127_ops);
+
+	/* First test register 0: Bits 5-7 are a version ID (should be 0),
+	   and bit 2 should also be 0.
+	   This is rather general, so the second test is more specific and
+	   looks at the 'ending point of burst in clock cycles' which is
+	   0x1d after a reset and not expected to ever change. */
+	if ((saa7127_read(sd, 0) & 0xe4) != 0 ||
+			(saa7127_read(sd, 0x29) & 0x3f) != 0x1d) {
+		v4l2_dbg(1, debug, sd, "saa7127 not found\n");
+		return -ENODEV;
+	}
+
+	if (id->driver_data) {	/* Chip type is already known */
+		state->ident = id->driver_data;
+	} else {		/* Needs detection */
+		int read_result;
+
+		/* Detect if it's an saa7129 */
+		read_result = saa7127_read(sd, SAA7129_REG_FADE_KEY_COL2);
+		saa7127_write(sd, SAA7129_REG_FADE_KEY_COL2, 0xaa);
+		if (saa7127_read(sd, SAA7129_REG_FADE_KEY_COL2) == 0xaa) {
+			saa7127_write(sd, SAA7129_REG_FADE_KEY_COL2,
+					read_result);
+			state->ident = SAA7129;
+			strscpy(client->name, "saa7129", I2C_NAME_SIZE);
+		} else {
+			state->ident = SAA7127;
+			strscpy(client->name, "saa7127", I2C_NAME_SIZE);
+		}
+	}
+
+	v4l2_info(sd, "%s found @ 0x%x (%s)\n", client->name,
+			client->addr << 1, client->adapter->name);
+
+	v4l2_dbg(1, debug, sd, "Configuring encoder\n");
+	saa7127_write_inittab(sd, saa7127_init_config_common);
+	saa7127_set_std(sd, V4L2_STD_NTSC);
+	saa7127_set_output_type(sd, SAA7127_OUTPUT_TYPE_BOTH);
+	saa7127_set_vps(sd, &vbi);
+	saa7127_set_wss(sd, &vbi);
+	saa7127_set_cc(sd, &vbi);
+	saa7127_set_xds(sd, &vbi);
+	if (test_image == 1)
+		/* The Encoder has an internal Colorbar generator */
+		/* This can be used for debugging */
+		saa7127_set_input_type(sd, SAA7127_INPUT_TYPE_TEST_IMAGE);
+	else
+		saa7127_set_input_type(sd, SAA7127_INPUT_TYPE_NORMAL);
+	saa7127_set_video_enable(sd, 1);
+
+	if (state->ident == SAA7129)
+		saa7127_write_inittab(sd, saa7129_init_config_extra);
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static void saa7127_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	/* Turn off TV output */
+	saa7127_set_video_enable(sd, 0);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id saa7127_id[] = {
+	{ "saa7127_auto", 0 },	/* auto-detection */
+	{ "saa7126", SAA7127 },
+	{ "saa7127", SAA7127 },
+	{ "saa7128", SAA7129 },
+	{ "saa7129", SAA7129 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, saa7127_id);
+
+static struct i2c_driver saa7127_driver = {
+	.driver = {
+		.name	= "saa7127",
+	},
+	.probe		= saa7127_probe,
+	.remove		= saa7127_remove,
+	.id_table	= saa7127_id,
+};
+
+module_i2c_driver(saa7127_driver);
diff --git a/drivers/media/i2c/saa717x.c b/drivers/media/i2c/saa717x.c
new file mode 100644
index 000000000..933ec0171
--- /dev/null
+++ b/drivers/media/i2c/saa717x.c
@@ -0,0 +1,1351 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * saa717x - Philips SAA717xHL video decoder driver
+ *
+ * Based on the saa7115 driver
+ *
+ * Changes by Ohta Kyuma <alpha292@bremen.or.jp>
+ *    - Apply to SAA717x,NEC uPD64031,uPD64083. (1/31/2004)
+ *
+ * Changes by T.Adachi (tadachi@tadachi-net.com)
+ *    - support audio, video scaler etc, and checked the initialize sequence.
+ *
+ * Cleaned up by Hans Verkuil <hverkuil@xs4all.nl>
+ *
+ * Note: this is a reversed engineered driver based on captures from
+ * the I2C bus under Windows. This chip is very similar to the saa7134,
+ * though. Unfortunately, this driver is currently only working for NTSC.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/sched.h>
+
+#include <linux/videodev2.h>
+#include <linux/i2c.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+
+MODULE_DESCRIPTION("Philips SAA717x audio/video decoder driver");
+MODULE_AUTHOR("K. Ohta, T. Adachi, Hans Verkuil");
+MODULE_LICENSE("GPL");
+
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "Debug level (0-1)");
+
+/*
+ * Generic i2c probe
+ * concerning the addresses: i2c wants 7 bit (without the r/w bit), so '>>1'
+ */
+
+struct saa717x_state {
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
+	v4l2_std_id std;
+	int input;
+	int enable;
+	int radio;
+	int playback;
+	int audio;
+	int tuner_audio_mode;
+	int audio_main_mute;
+	int audio_main_vol_r;
+	int audio_main_vol_l;
+	u16 audio_main_bass;
+	u16 audio_main_treble;
+	u16 audio_main_volume;
+	u16 audio_main_balance;
+	int audio_input;
+};
+
+static inline struct saa717x_state *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct saa717x_state, sd);
+}
+
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct saa717x_state, hdl)->sd;
+}
+
+/* ----------------------------------------------------------------------- */
+
+/* for audio mode */
+#define TUNER_AUDIO_MONO	0  /* LL */
+#define TUNER_AUDIO_STEREO	1  /* LR */
+#define TUNER_AUDIO_LANG1	2  /* LL */
+#define TUNER_AUDIO_LANG2	3  /* RR */
+
+#define SAA717X_NTSC_WIDTH	(704)
+#define SAA717X_NTSC_HEIGHT	(480)
+
+/* ----------------------------------------------------------------------- */
+
+static int saa717x_write(struct v4l2_subdev *sd, u32 reg, u32 value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct i2c_adapter *adap = client->adapter;
+	int fw_addr = reg == 0x454 || (reg >= 0x464 && reg <= 0x478) || reg == 0x480 || reg == 0x488;
+	unsigned char mm1[6];
+	struct i2c_msg msg;
+
+	msg.flags = 0;
+	msg.addr = client->addr;
+	mm1[0] = (reg >> 8) & 0xff;
+	mm1[1] = reg & 0xff;
+
+	if (fw_addr) {
+		mm1[4] = (value >> 16) & 0xff;
+		mm1[3] = (value >> 8) & 0xff;
+		mm1[2] = value & 0xff;
+	} else {
+		mm1[2] = value & 0xff;
+	}
+	msg.len = fw_addr ? 5 : 3; /* Long Registers have *only* three bytes! */
+	msg.buf = mm1;
+	v4l2_dbg(2, debug, sd, "wrote:  reg 0x%03x=%08x\n", reg, value);
+	return i2c_transfer(adap, &msg, 1) == 1;
+}
+
+static void saa717x_write_regs(struct v4l2_subdev *sd, u32 *data)
+{
+	while (data[0] || data[1]) {
+		saa717x_write(sd, data[0], data[1]);
+		data += 2;
+	}
+}
+
+static u32 saa717x_read(struct v4l2_subdev *sd, u32 reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct i2c_adapter *adap = client->adapter;
+	int fw_addr = (reg >= 0x404 && reg <= 0x4b8) || reg == 0x528;
+	unsigned char mm1[2];
+	unsigned char mm2[4] = { 0, 0, 0, 0 };
+	struct i2c_msg msgs[2];
+	u32 value;
+
+	msgs[0].flags = 0;
+	msgs[1].flags = I2C_M_RD;
+	msgs[0].addr = msgs[1].addr = client->addr;
+	mm1[0] = (reg >> 8) & 0xff;
+	mm1[1] = reg & 0xff;
+	msgs[0].len = 2;
+	msgs[0].buf = mm1;
+	msgs[1].len = fw_addr ? 3 : 1; /* Multibyte Registers contains *only* 3 bytes */
+	msgs[1].buf = mm2;
+	i2c_transfer(adap, msgs, 2);
+
+	if (fw_addr)
+		value = (mm2[2] << 16)  | (mm2[1] << 8) | mm2[0];
+	else
+		value = mm2[0];
+
+	v4l2_dbg(2, debug, sd, "read:  reg 0x%03x=0x%08x\n", reg, value);
+	return value;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static u32 reg_init_initialize[] =
+{
+	/* from linux driver */
+	0x101, 0x008, /* Increment delay */
+
+	0x103, 0x000, /* Analog input control 2 */
+	0x104, 0x090, /* Analog input control 3 */
+	0x105, 0x090, /* Analog input control 4 */
+	0x106, 0x0eb, /* Horizontal sync start */
+	0x107, 0x0e0, /* Horizontal sync stop */
+	0x109, 0x055, /* Luminance control */
+
+	0x10f, 0x02a, /* Chroma gain control */
+	0x110, 0x000, /* Chroma control 2 */
+
+	0x114, 0x045, /* analog/ADC */
+
+	0x118, 0x040, /* RAW data gain */
+	0x119, 0x080, /* RAW data offset */
+
+	0x044, 0x000, /* VBI horizontal input window start (L) TASK A */
+	0x045, 0x000, /* VBI horizontal input window start (H) TASK A */
+	0x046, 0x0cf, /* VBI horizontal input window stop (L) TASK A */
+	0x047, 0x002, /* VBI horizontal input window stop (H) TASK A */
+
+	0x049, 0x000, /* VBI vertical input window start (H) TASK A */
+
+	0x04c, 0x0d0, /* VBI horizontal output length (L) TASK A */
+	0x04d, 0x002, /* VBI horizontal output length (H) TASK A */
+
+	0x064, 0x080, /* Lumina brightness TASK A */
+	0x065, 0x040, /* Luminance contrast TASK A */
+	0x066, 0x040, /* Chroma saturation TASK A */
+	/* 067H: Reserved */
+	0x068, 0x000, /* VBI horizontal scaling increment (L) TASK A */
+	0x069, 0x004, /* VBI horizontal scaling increment (H) TASK A */
+	0x06a, 0x000, /* VBI phase offset TASK A */
+
+	0x06e, 0x000, /* Horizontal phase offset Luma TASK A */
+	0x06f, 0x000, /* Horizontal phase offset Chroma TASK A */
+
+	0x072, 0x000, /* Vertical filter mode TASK A */
+
+	0x084, 0x000, /* VBI horizontal input window start (L) TAKS B */
+	0x085, 0x000, /* VBI horizontal input window start (H) TAKS B */
+	0x086, 0x0cf, /* VBI horizontal input window stop (L) TAKS B */
+	0x087, 0x002, /* VBI horizontal input window stop (H) TAKS B */
+
+	0x089, 0x000, /* VBI vertical input window start (H) TAKS B */
+
+	0x08c, 0x0d0, /* VBI horizontal output length (L) TASK B */
+	0x08d, 0x002, /* VBI horizontal output length (H) TASK B */
+
+	0x0a4, 0x080, /* Lumina brightness TASK B */
+	0x0a5, 0x040, /* Luminance contrast TASK B */
+	0x0a6, 0x040, /* Chroma saturation TASK B */
+	/* 0A7H reserved */
+	0x0a8, 0x000, /* VBI horizontal scaling increment (L) TASK B */
+	0x0a9, 0x004, /* VBI horizontal scaling increment (H) TASK B */
+	0x0aa, 0x000, /* VBI phase offset TASK B */
+
+	0x0ae, 0x000, /* Horizontal phase offset Luma TASK B */
+	0x0af, 0x000, /*Horizontal phase offset Chroma TASK B */
+
+	0x0b2, 0x000, /* Vertical filter mode TASK B */
+
+	0x00c, 0x000, /* Start point GREEN path */
+	0x00d, 0x000, /* Start point BLUE path */
+	0x00e, 0x000, /* Start point RED path */
+
+	0x010, 0x010, /* GREEN path gamma curve --- */
+	0x011, 0x020,
+	0x012, 0x030,
+	0x013, 0x040,
+	0x014, 0x050,
+	0x015, 0x060,
+	0x016, 0x070,
+	0x017, 0x080,
+	0x018, 0x090,
+	0x019, 0x0a0,
+	0x01a, 0x0b0,
+	0x01b, 0x0c0,
+	0x01c, 0x0d0,
+	0x01d, 0x0e0,
+	0x01e, 0x0f0,
+	0x01f, 0x0ff, /* --- GREEN path gamma curve */
+
+	0x020, 0x010, /* BLUE path gamma curve --- */
+	0x021, 0x020,
+	0x022, 0x030,
+	0x023, 0x040,
+	0x024, 0x050,
+	0x025, 0x060,
+	0x026, 0x070,
+	0x027, 0x080,
+	0x028, 0x090,
+	0x029, 0x0a0,
+	0x02a, 0x0b0,
+	0x02b, 0x0c0,
+	0x02c, 0x0d0,
+	0x02d, 0x0e0,
+	0x02e, 0x0f0,
+	0x02f, 0x0ff, /* --- BLUE path gamma curve */
+
+	0x030, 0x010, /* RED path gamma curve --- */
+	0x031, 0x020,
+	0x032, 0x030,
+	0x033, 0x040,
+	0x034, 0x050,
+	0x035, 0x060,
+	0x036, 0x070,
+	0x037, 0x080,
+	0x038, 0x090,
+	0x039, 0x0a0,
+	0x03a, 0x0b0,
+	0x03b, 0x0c0,
+	0x03c, 0x0d0,
+	0x03d, 0x0e0,
+	0x03e, 0x0f0,
+	0x03f, 0x0ff, /* --- RED path gamma curve */
+
+	0x109, 0x085, /* Luminance control  */
+
+	/**** from app start ****/
+	0x584, 0x000, /* AGC gain control */
+	0x585, 0x000, /* Program count */
+	0x586, 0x003, /* Status reset */
+	0x588, 0x0ff, /* Number of audio samples (L) */
+	0x589, 0x00f, /* Number of audio samples (M) */
+	0x58a, 0x000, /* Number of audio samples (H) */
+	0x58b, 0x000, /* Audio select */
+	0x58c, 0x010, /* Audio channel assign1 */
+	0x58d, 0x032, /* Audio channel assign2 */
+	0x58e, 0x054, /* Audio channel assign3 */
+	0x58f, 0x023, /* Audio format */
+	0x590, 0x000, /* SIF control */
+
+	0x595, 0x000, /* ?? */
+	0x596, 0x000, /* ?? */
+	0x597, 0x000, /* ?? */
+
+	0x464, 0x00, /* Digital input crossbar1 */
+
+	0x46c, 0xbbbb10, /* Digital output selection1-3 */
+	0x470, 0x101010, /* Digital output selection4-6 */
+
+	0x478, 0x00, /* Sound feature control */
+
+	0x474, 0x18, /* Softmute control */
+
+	0x454, 0x0425b9, /* Sound Easy programming(reset) */
+	0x454, 0x042539, /* Sound Easy programming(reset) */
+
+
+	/**** common setting( of DVD play, including scaler commands) ****/
+	0x042, 0x003, /* Data path configuration for VBI (TASK A) */
+
+	0x082, 0x003, /* Data path configuration for VBI (TASK B) */
+
+	0x108, 0x0f8, /* Sync control */
+	0x2a9, 0x0fd, /* ??? */
+	0x102, 0x089, /* select video input "mode 9" */
+	0x111, 0x000, /* Mode/delay control */
+
+	0x10e, 0x00a, /* Chroma control 1 */
+
+	0x594, 0x002, /* SIF, analog I/O select */
+
+	0x454, 0x0425b9, /* Sound  */
+	0x454, 0x042539,
+
+	0x111, 0x000,
+	0x10e, 0x00a,
+	0x464, 0x000,
+	0x300, 0x000,
+	0x301, 0x006,
+	0x302, 0x000,
+	0x303, 0x006,
+	0x308, 0x040,
+	0x309, 0x000,
+	0x30a, 0x000,
+	0x30b, 0x000,
+	0x000, 0x002,
+	0x001, 0x000,
+	0x002, 0x000,
+	0x003, 0x000,
+	0x004, 0x033,
+	0x040, 0x01d,
+	0x041, 0x001,
+	0x042, 0x004,
+	0x043, 0x000,
+	0x080, 0x01e,
+	0x081, 0x001,
+	0x082, 0x004,
+	0x083, 0x000,
+	0x190, 0x018,
+	0x115, 0x000,
+	0x116, 0x012,
+	0x117, 0x018,
+	0x04a, 0x011,
+	0x08a, 0x011,
+	0x04b, 0x000,
+	0x08b, 0x000,
+	0x048, 0x000,
+	0x088, 0x000,
+	0x04e, 0x012,
+	0x08e, 0x012,
+	0x058, 0x012,
+	0x098, 0x012,
+	0x059, 0x000,
+	0x099, 0x000,
+	0x05a, 0x003,
+	0x09a, 0x003,
+	0x05b, 0x001,
+	0x09b, 0x001,
+	0x054, 0x008,
+	0x094, 0x008,
+	0x055, 0x000,
+	0x095, 0x000,
+	0x056, 0x0c7,
+	0x096, 0x0c7,
+	0x057, 0x002,
+	0x097, 0x002,
+	0x0ff, 0x0ff,
+	0x060, 0x001,
+	0x0a0, 0x001,
+	0x061, 0x000,
+	0x0a1, 0x000,
+	0x062, 0x000,
+	0x0a2, 0x000,
+	0x063, 0x000,
+	0x0a3, 0x000,
+	0x070, 0x000,
+	0x0b0, 0x000,
+	0x071, 0x004,
+	0x0b1, 0x004,
+	0x06c, 0x0e9,
+	0x0ac, 0x0e9,
+	0x06d, 0x003,
+	0x0ad, 0x003,
+	0x05c, 0x0d0,
+	0x09c, 0x0d0,
+	0x05d, 0x002,
+	0x09d, 0x002,
+	0x05e, 0x0f2,
+	0x09e, 0x0f2,
+	0x05f, 0x000,
+	0x09f, 0x000,
+	0x074, 0x000,
+	0x0b4, 0x000,
+	0x075, 0x000,
+	0x0b5, 0x000,
+	0x076, 0x000,
+	0x0b6, 0x000,
+	0x077, 0x000,
+	0x0b7, 0x000,
+	0x195, 0x008,
+	0x0ff, 0x0ff,
+	0x108, 0x0f8,
+	0x111, 0x000,
+	0x10e, 0x00a,
+	0x2a9, 0x0fd,
+	0x464, 0x001,
+	0x454, 0x042135,
+	0x598, 0x0e7,
+	0x599, 0x07d,
+	0x59a, 0x018,
+	0x59c, 0x066,
+	0x59d, 0x090,
+	0x59e, 0x001,
+	0x584, 0x000,
+	0x585, 0x000,
+	0x586, 0x003,
+	0x588, 0x0ff,
+	0x589, 0x00f,
+	0x58a, 0x000,
+	0x58b, 0x000,
+	0x58c, 0x010,
+	0x58d, 0x032,
+	0x58e, 0x054,
+	0x58f, 0x023,
+	0x590, 0x000,
+	0x595, 0x000,
+	0x596, 0x000,
+	0x597, 0x000,
+	0x464, 0x000,
+	0x46c, 0xbbbb10,
+	0x470, 0x101010,
+
+
+	0x478, 0x000,
+	0x474, 0x018,
+	0x454, 0x042135,
+	0x598, 0x0e7,
+	0x599, 0x07d,
+	0x59a, 0x018,
+	0x59c, 0x066,
+	0x59d, 0x090,
+	0x59e, 0x001,
+	0x584, 0x000,
+	0x585, 0x000,
+	0x586, 0x003,
+	0x588, 0x0ff,
+	0x589, 0x00f,
+	0x58a, 0x000,
+	0x58b, 0x000,
+	0x58c, 0x010,
+	0x58d, 0x032,
+	0x58e, 0x054,
+	0x58f, 0x023,
+	0x590, 0x000,
+	0x595, 0x000,
+	0x596, 0x000,
+	0x597, 0x000,
+	0x464, 0x000,
+	0x46c, 0xbbbb10,
+	0x470, 0x101010,
+
+	0x478, 0x000,
+	0x474, 0x018,
+	0x454, 0x042135,
+	0x598, 0x0e7,
+	0x599, 0x07d,
+	0x59a, 0x018,
+	0x59c, 0x066,
+	0x59d, 0x090,
+	0x59e, 0x001,
+	0x584, 0x000,
+	0x585, 0x000,
+	0x586, 0x003,
+	0x588, 0x0ff,
+	0x589, 0x00f,
+	0x58a, 0x000,
+	0x58b, 0x000,
+	0x58c, 0x010,
+	0x58d, 0x032,
+	0x58e, 0x054,
+	0x58f, 0x023,
+	0x590, 0x000,
+	0x595, 0x000,
+	0x596, 0x000,
+	0x597, 0x000,
+	0x464, 0x000,
+	0x46c, 0xbbbb10,
+	0x470, 0x101010,
+	0x478, 0x000,
+	0x474, 0x018,
+	0x454, 0x042135,
+	0x193, 0x000,
+	0x300, 0x000,
+	0x301, 0x006,
+	0x302, 0x000,
+	0x303, 0x006,
+	0x308, 0x040,
+	0x309, 0x000,
+	0x30a, 0x000,
+	0x30b, 0x000,
+	0x000, 0x002,
+	0x001, 0x000,
+	0x002, 0x000,
+	0x003, 0x000,
+	0x004, 0x033,
+	0x040, 0x01d,
+	0x041, 0x001,
+	0x042, 0x004,
+	0x043, 0x000,
+	0x080, 0x01e,
+	0x081, 0x001,
+	0x082, 0x004,
+	0x083, 0x000,
+	0x190, 0x018,
+	0x115, 0x000,
+	0x116, 0x012,
+	0x117, 0x018,
+	0x04a, 0x011,
+	0x08a, 0x011,
+	0x04b, 0x000,
+	0x08b, 0x000,
+	0x048, 0x000,
+	0x088, 0x000,
+	0x04e, 0x012,
+	0x08e, 0x012,
+	0x058, 0x012,
+	0x098, 0x012,
+	0x059, 0x000,
+	0x099, 0x000,
+	0x05a, 0x003,
+	0x09a, 0x003,
+	0x05b, 0x001,
+	0x09b, 0x001,
+	0x054, 0x008,
+	0x094, 0x008,
+	0x055, 0x000,
+	0x095, 0x000,
+	0x056, 0x0c7,
+	0x096, 0x0c7,
+	0x057, 0x002,
+	0x097, 0x002,
+	0x060, 0x001,
+	0x0a0, 0x001,
+	0x061, 0x000,
+	0x0a1, 0x000,
+	0x062, 0x000,
+	0x0a2, 0x000,
+	0x063, 0x000,
+	0x0a3, 0x000,
+	0x070, 0x000,
+	0x0b0, 0x000,
+	0x071, 0x004,
+	0x0b1, 0x004,
+	0x06c, 0x0e9,
+	0x0ac, 0x0e9,
+	0x06d, 0x003,
+	0x0ad, 0x003,
+	0x05c, 0x0d0,
+	0x09c, 0x0d0,
+	0x05d, 0x002,
+	0x09d, 0x002,
+	0x05e, 0x0f2,
+	0x09e, 0x0f2,
+	0x05f, 0x000,
+	0x09f, 0x000,
+	0x074, 0x000,
+	0x0b4, 0x000,
+	0x075, 0x000,
+	0x0b5, 0x000,
+	0x076, 0x000,
+	0x0b6, 0x000,
+	0x077, 0x000,
+	0x0b7, 0x000,
+	0x195, 0x008,
+	0x598, 0x0e7,
+	0x599, 0x07d,
+	0x59a, 0x018,
+	0x59c, 0x066,
+	0x59d, 0x090,
+	0x59e, 0x001,
+	0x584, 0x000,
+	0x585, 0x000,
+	0x586, 0x003,
+	0x588, 0x0ff,
+	0x589, 0x00f,
+	0x58a, 0x000,
+	0x58b, 0x000,
+	0x58c, 0x010,
+	0x58d, 0x032,
+	0x58e, 0x054,
+	0x58f, 0x023,
+	0x590, 0x000,
+	0x595, 0x000,
+	0x596, 0x000,
+	0x597, 0x000,
+	0x464, 0x000,
+	0x46c, 0xbbbb10,
+	0x470, 0x101010,
+	0x478, 0x000,
+	0x474, 0x018,
+	0x454, 0x042135,
+	0x193, 0x0a6,
+	0x108, 0x0f8,
+	0x042, 0x003,
+	0x082, 0x003,
+	0x454, 0x0425b9,
+	0x454, 0x042539,
+	0x193, 0x000,
+	0x193, 0x0a6,
+	0x464, 0x000,
+
+	0, 0
+};
+
+/* Tuner */
+static u32 reg_init_tuner_input[] = {
+	0x108, 0x0f8, /* Sync control */
+	0x111, 0x000, /* Mode/delay control */
+	0x10e, 0x00a, /* Chroma control 1 */
+	0, 0
+};
+
+/* Composite */
+static u32 reg_init_composite_input[] = {
+	0x108, 0x0e8, /* Sync control */
+	0x111, 0x000, /* Mode/delay control */
+	0x10e, 0x04a, /* Chroma control 1 */
+	0, 0
+};
+
+/* S-Video */
+static u32 reg_init_svideo_input[] = {
+	0x108, 0x0e8, /* Sync control */
+	0x111, 0x000, /* Mode/delay control */
+	0x10e, 0x04a, /* Chroma control 1 */
+	0, 0
+};
+
+static u32 reg_set_audio_template[4][2] =
+{
+	{ /* for MONO
+		tadachi 6/29 DMA audio output select?
+		Register 0x46c
+		7-4: DMA2, 3-0: DMA1 ch. DMA4, DMA3 DMA2, DMA1
+		0: MAIN left,  1: MAIN right
+		2: AUX1 left,  3: AUX1 right
+		4: AUX2 left,  5: AUX2 right
+		6: DPL left,   7: DPL  right
+		8: DPL center, 9: DPL surround
+		A: monitor output, B: digital sense */
+		0xbbbb00,
+
+		/* tadachi 6/29 DAC and I2S output select?
+		   Register 0x470
+		   7-4:DAC right ch. 3-0:DAC left ch.
+		   I2S1 right,left  I2S2 right,left */
+		0x00,
+	},
+	{ /* for STEREO */
+		0xbbbb10, 0x101010,
+	},
+	{ /* for LANG1 */
+		0xbbbb00, 0x00,
+	},
+	{ /* for LANG2/SAP */
+		0xbbbb11, 0x111111,
+	}
+};
+
+
+/* Get detected audio flags (from saa7134 driver) */
+static void get_inf_dev_status(struct v4l2_subdev *sd,
+		int *dual_flag, int *stereo_flag)
+{
+	u32 reg_data3;
+
+	static char *stdres[0x20] = {
+		[0x00] = "no standard detected",
+		[0x01] = "B/G (in progress)",
+		[0x02] = "D/K (in progress)",
+		[0x03] = "M (in progress)",
+
+		[0x04] = "B/G A2",
+		[0x05] = "B/G NICAM",
+		[0x06] = "D/K A2 (1)",
+		[0x07] = "D/K A2 (2)",
+		[0x08] = "D/K A2 (3)",
+		[0x09] = "D/K NICAM",
+		[0x0a] = "L NICAM",
+		[0x0b] = "I NICAM",
+
+		[0x0c] = "M Korea",
+		[0x0d] = "M BTSC ",
+		[0x0e] = "M EIAJ",
+
+		[0x0f] = "FM radio / IF 10.7 / 50 deemp",
+		[0x10] = "FM radio / IF 10.7 / 75 deemp",
+		[0x11] = "FM radio / IF sel / 50 deemp",
+		[0x12] = "FM radio / IF sel / 75 deemp",
+
+		[0x13 ... 0x1e] = "unknown",
+		[0x1f] = "??? [in progress]",
+	};
+
+
+	*dual_flag = *stereo_flag = 0;
+
+	/* (demdec status: 0x528) */
+
+	/* read current status */
+	reg_data3 = saa717x_read(sd, 0x0528);
+
+	v4l2_dbg(1, debug, sd, "tvaudio thread status: 0x%x [%s%s%s]\n",
+		reg_data3, stdres[reg_data3 & 0x1f],
+		(reg_data3 & 0x000020) ? ",stereo" : "",
+		(reg_data3 & 0x000040) ? ",dual"   : "");
+	v4l2_dbg(1, debug, sd, "detailed status: "
+		"%s#%s#%s#%s#%s#%s#%s#%s#%s#%s#%s#%s#%s#%s\n",
+		(reg_data3 & 0x000080) ? " A2/EIAJ pilot tone "     : "",
+		(reg_data3 & 0x000100) ? " A2/EIAJ dual "           : "",
+		(reg_data3 & 0x000200) ? " A2/EIAJ stereo "         : "",
+		(reg_data3 & 0x000400) ? " A2/EIAJ noise mute "     : "",
+
+		(reg_data3 & 0x000800) ? " BTSC/FM radio pilot "    : "",
+		(reg_data3 & 0x001000) ? " SAP carrier "            : "",
+		(reg_data3 & 0x002000) ? " BTSC stereo noise mute " : "",
+		(reg_data3 & 0x004000) ? " SAP noise mute "         : "",
+		(reg_data3 & 0x008000) ? " VDSP "                   : "",
+
+		(reg_data3 & 0x010000) ? " NICST "                  : "",
+		(reg_data3 & 0x020000) ? " NICDU "                  : "",
+		(reg_data3 & 0x040000) ? " NICAM muted "            : "",
+		(reg_data3 & 0x080000) ? " NICAM reserve sound "    : "",
+
+		(reg_data3 & 0x100000) ? " init done "              : "");
+
+	if (reg_data3 & 0x000220) {
+		v4l2_dbg(1, debug, sd, "ST!!!\n");
+		*stereo_flag = 1;
+	}
+
+	if (reg_data3 & 0x000140) {
+		v4l2_dbg(1, debug, sd, "DUAL!!!\n");
+		*dual_flag = 1;
+	}
+}
+
+/* regs write to set audio mode */
+static void set_audio_mode(struct v4l2_subdev *sd, int audio_mode)
+{
+	v4l2_dbg(1, debug, sd, "writing registers to set audio mode by set %d\n",
+			audio_mode);
+
+	saa717x_write(sd, 0x46c, reg_set_audio_template[audio_mode][0]);
+	saa717x_write(sd, 0x470, reg_set_audio_template[audio_mode][1]);
+}
+
+/* write regs to set audio volume, bass and treble */
+static int set_audio_regs(struct v4l2_subdev *sd,
+		struct saa717x_state *decoder)
+{
+	u8 mute = 0xac; /* -84 dB */
+	u32 val;
+	unsigned int work_l, work_r;
+
+	/* set SIF analog I/O select */
+	saa717x_write(sd, 0x0594, decoder->audio_input);
+	v4l2_dbg(1, debug, sd, "set audio input %d\n",
+			decoder->audio_input);
+
+	/* normalize ( 65535 to 0 -> 24 to -40 (not -84)) */
+	work_l = (min(65536 - decoder->audio_main_balance, 32768) * decoder->audio_main_volume) / 32768;
+	work_r = (min(decoder->audio_main_balance, (u16)32768) * decoder->audio_main_volume) / 32768;
+	decoder->audio_main_vol_l = (long)work_l * (24 - (-40)) / 65535 - 40;
+	decoder->audio_main_vol_r = (long)work_r * (24 - (-40)) / 65535 - 40;
+
+	/* set main volume */
+	/* main volume L[7-0],R[7-0],0x00  24=24dB,-83dB, -84(mute) */
+	/*    def:0dB->6dB(MPG600GR) */
+	/* if mute is on, set mute */
+	if (decoder->audio_main_mute) {
+		val = mute | (mute << 8);
+	} else {
+		val = (u8)decoder->audio_main_vol_l |
+			((u8)decoder->audio_main_vol_r << 8);
+	}
+
+	saa717x_write(sd, 0x480, val);
+
+	/* set bass and treble */
+	val = decoder->audio_main_bass & 0x1f;
+	val |= (decoder->audio_main_treble & 0x1f) << 5;
+	saa717x_write(sd, 0x488, val);
+	return 0;
+}
+
+/********** scaling staff ***********/
+static void set_h_prescale(struct v4l2_subdev *sd,
+		int task, int prescale)
+{
+	static const struct {
+		int xpsc;
+		int xacl;
+		int xc2_1;
+		int xdcg;
+		int vpfy;
+	} vals[] = {
+		/* XPSC XACL XC2_1 XDCG VPFY */
+		{    1,   0,    0,    0,   0 },
+		{    2,   2,    1,    2,   2 },
+		{    3,   4,    1,    3,   2 },
+		{    4,   8,    1,    4,   2 },
+		{    5,   8,    1,    4,   2 },
+		{    6,   8,    1,    4,   3 },
+		{    7,   8,    1,    4,   3 },
+		{    8,  15,    0,    4,   3 },
+		{    9,  15,    0,    4,   3 },
+		{   10,  16,    1,    5,   3 },
+	};
+	static const int count = ARRAY_SIZE(vals);
+	int i, task_shift;
+
+	task_shift = task * 0x40;
+	for (i = 0; i < count; i++)
+		if (vals[i].xpsc == prescale)
+			break;
+	if (i == count)
+		return;
+
+	/* horizontal prescaling */
+	saa717x_write(sd, 0x60 + task_shift, vals[i].xpsc);
+	/* accumulation length */
+	saa717x_write(sd, 0x61 + task_shift, vals[i].xacl);
+	/* level control */
+	saa717x_write(sd, 0x62 + task_shift,
+			(vals[i].xc2_1 << 3) | vals[i].xdcg);
+	/*FIR prefilter control */
+	saa717x_write(sd, 0x63 + task_shift,
+			(vals[i].vpfy << 2) | vals[i].vpfy);
+}
+
+/********** scaling staff ***********/
+static void set_v_scale(struct v4l2_subdev *sd, int task, int yscale)
+{
+	int task_shift;
+
+	task_shift = task * 0x40;
+	/* Vertical scaling ratio (LOW) */
+	saa717x_write(sd, 0x70 + task_shift, yscale & 0xff);
+	/* Vertical scaling ratio (HI) */
+	saa717x_write(sd, 0x71 + task_shift, yscale >> 8);
+}
+
+static int saa717x_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+	struct saa717x_state *state = to_state(sd);
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		saa717x_write(sd, 0x10a, ctrl->val);
+		return 0;
+
+	case V4L2_CID_CONTRAST:
+		saa717x_write(sd, 0x10b, ctrl->val);
+		return 0;
+
+	case V4L2_CID_SATURATION:
+		saa717x_write(sd, 0x10c, ctrl->val);
+		return 0;
+
+	case V4L2_CID_HUE:
+		saa717x_write(sd, 0x10d, ctrl->val);
+		return 0;
+
+	case V4L2_CID_AUDIO_MUTE:
+		state->audio_main_mute = ctrl->val;
+		break;
+
+	case V4L2_CID_AUDIO_VOLUME:
+		state->audio_main_volume = ctrl->val;
+		break;
+
+	case V4L2_CID_AUDIO_BALANCE:
+		state->audio_main_balance = ctrl->val;
+		break;
+
+	case V4L2_CID_AUDIO_TREBLE:
+		state->audio_main_treble = ctrl->val;
+		break;
+
+	case V4L2_CID_AUDIO_BASS:
+		state->audio_main_bass = ctrl->val;
+		break;
+
+	default:
+		return 0;
+	}
+	set_audio_regs(sd, state);
+	return 0;
+}
+
+static int saa717x_s_video_routing(struct v4l2_subdev *sd,
+				   u32 input, u32 output, u32 config)
+{
+	struct saa717x_state *decoder = to_state(sd);
+	int is_tuner = input & 0x80;  /* tuner input flag */
+
+	input &= 0x7f;
+
+	v4l2_dbg(1, debug, sd, "decoder set input (%d)\n", input);
+	/* inputs from 0-9 are available*/
+	/* saa717x have mode0-mode9 but mode5 is reserved. */
+	if (input > 9 || input == 5)
+		return -EINVAL;
+
+	if (decoder->input != input) {
+		int input_line = input;
+
+		decoder->input = input_line;
+		v4l2_dbg(1, debug, sd,  "now setting %s input %d\n",
+				input_line >= 6 ? "S-Video" : "Composite",
+				input_line);
+
+		/* select mode */
+		saa717x_write(sd, 0x102,
+				(saa717x_read(sd, 0x102) & 0xf0) |
+				input_line);
+
+		/* bypass chrominance trap for modes 6..9 */
+		saa717x_write(sd, 0x109,
+				(saa717x_read(sd, 0x109) & 0x7f) |
+				(input_line < 6 ? 0x0 : 0x80));
+
+		/* change audio_mode */
+		if (is_tuner) {
+			/* tuner */
+			set_audio_mode(sd, decoder->tuner_audio_mode);
+		} else {
+			/* Force to STEREO mode if Composite or
+			 * S-Video were chosen */
+			set_audio_mode(sd, TUNER_AUDIO_STEREO);
+		}
+		/* change initialize procedure (Composite/S-Video) */
+		if (is_tuner)
+			saa717x_write_regs(sd, reg_init_tuner_input);
+		else if (input_line >= 6)
+			saa717x_write_regs(sd, reg_init_svideo_input);
+		else
+			saa717x_write_regs(sd, reg_init_composite_input);
+	}
+
+	return 0;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int saa717x_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+{
+	reg->val = saa717x_read(sd, reg->reg);
+	reg->size = 1;
+	return 0;
+}
+
+static int saa717x_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
+{
+	u16 addr = reg->reg & 0xffff;
+	u8 val = reg->val & 0xff;
+
+	saa717x_write(sd, addr, val);
+	return 0;
+}
+#endif
+
+static int saa717x_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+	int prescale, h_scale, v_scale;
+
+	v4l2_dbg(1, debug, sd, "decoder set size\n");
+
+	if (format->pad || fmt->code != MEDIA_BUS_FMT_FIXED)
+		return -EINVAL;
+
+	/* FIXME need better bounds checking here */
+	if (fmt->width < 1 || fmt->width > 1440)
+		return -EINVAL;
+	if (fmt->height < 1 || fmt->height > 960)
+		return -EINVAL;
+
+	fmt->field = V4L2_FIELD_INTERLACED;
+	fmt->colorspace = V4L2_COLORSPACE_SMPTE170M;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+		return 0;
+
+	/* scaling setting */
+	/* NTSC and interlace only */
+	prescale = SAA717X_NTSC_WIDTH / fmt->width;
+	if (prescale == 0)
+		prescale = 1;
+	h_scale = 1024 * SAA717X_NTSC_WIDTH / prescale / fmt->width;
+	/* interlace */
+	v_scale = 512 * 2 * SAA717X_NTSC_HEIGHT / fmt->height;
+
+	/* Horizontal prescaling etc */
+	set_h_prescale(sd, 0, prescale);
+	set_h_prescale(sd, 1, prescale);
+
+	/* Horizontal scaling increment */
+	/* TASK A */
+	saa717x_write(sd, 0x6C, (u8)(h_scale & 0xFF));
+	saa717x_write(sd, 0x6D, (u8)((h_scale >> 8) & 0xFF));
+	/* TASK B */
+	saa717x_write(sd, 0xAC, (u8)(h_scale & 0xFF));
+	saa717x_write(sd, 0xAD, (u8)((h_scale >> 8) & 0xFF));
+
+	/* Vertical prescaling etc */
+	set_v_scale(sd, 0, v_scale);
+	set_v_scale(sd, 1, v_scale);
+
+	/* set video output size */
+	/* video number of pixels at output */
+	/* TASK A */
+	saa717x_write(sd, 0x5C, (u8)(fmt->width & 0xFF));
+	saa717x_write(sd, 0x5D, (u8)((fmt->width >> 8) & 0xFF));
+	/* TASK B */
+	saa717x_write(sd, 0x9C, (u8)(fmt->width & 0xFF));
+	saa717x_write(sd, 0x9D, (u8)((fmt->width >> 8) & 0xFF));
+
+	/* video number of lines at output */
+	/* TASK A */
+	saa717x_write(sd, 0x5E, (u8)(fmt->height & 0xFF));
+	saa717x_write(sd, 0x5F, (u8)((fmt->height >> 8) & 0xFF));
+	/* TASK B */
+	saa717x_write(sd, 0x9E, (u8)(fmt->height & 0xFF));
+	saa717x_write(sd, 0x9F, (u8)((fmt->height >> 8) & 0xFF));
+	return 0;
+}
+
+static int saa717x_s_radio(struct v4l2_subdev *sd)
+{
+	struct saa717x_state *decoder = to_state(sd);
+
+	decoder->radio = 1;
+	return 0;
+}
+
+static int saa717x_s_std(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct saa717x_state *decoder = to_state(sd);
+
+	v4l2_dbg(1, debug, sd, "decoder set norm ");
+	v4l2_dbg(1, debug, sd, "(not yet implemented)\n");
+
+	decoder->radio = 0;
+	decoder->std = std;
+	return 0;
+}
+
+static int saa717x_s_audio_routing(struct v4l2_subdev *sd,
+				   u32 input, u32 output, u32 config)
+{
+	struct saa717x_state *decoder = to_state(sd);
+
+	if (input < 3) { /* FIXME! --tadachi */
+		decoder->audio_input = input;
+		v4l2_dbg(1, debug, sd,
+				"set decoder audio input to %d\n",
+				decoder->audio_input);
+		set_audio_regs(sd, decoder);
+		return 0;
+	}
+	return -ERANGE;
+}
+
+static int saa717x_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct saa717x_state *decoder = to_state(sd);
+
+	v4l2_dbg(1, debug, sd, "decoder %s output\n",
+			enable ? "enable" : "disable");
+	decoder->enable = enable;
+	saa717x_write(sd, 0x193, enable ? 0xa6 : 0x26);
+	return 0;
+}
+
+/* change audio mode */
+static int saa717x_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *vt)
+{
+	struct saa717x_state *decoder = to_state(sd);
+	int audio_mode;
+	char *mes[4] = {
+		"MONO", "STEREO", "LANG1", "LANG2/SAP"
+	};
+
+	audio_mode = TUNER_AUDIO_STEREO;
+
+	switch (vt->audmode) {
+		case V4L2_TUNER_MODE_MONO:
+			audio_mode = TUNER_AUDIO_MONO;
+			break;
+		case V4L2_TUNER_MODE_STEREO:
+			audio_mode = TUNER_AUDIO_STEREO;
+			break;
+		case V4L2_TUNER_MODE_LANG2:
+			audio_mode = TUNER_AUDIO_LANG2;
+			break;
+		case V4L2_TUNER_MODE_LANG1:
+			audio_mode = TUNER_AUDIO_LANG1;
+			break;
+	}
+
+	v4l2_dbg(1, debug, sd, "change audio mode to %s\n",
+			mes[audio_mode]);
+	decoder->tuner_audio_mode = audio_mode;
+	/* The registers are not changed here. */
+	/* See DECODER_ENABLE_OUTPUT section. */
+	set_audio_mode(sd, decoder->tuner_audio_mode);
+	return 0;
+}
+
+static int saa717x_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
+{
+	struct saa717x_state *decoder = to_state(sd);
+	int dual_f, stereo_f;
+
+	if (decoder->radio)
+		return 0;
+	get_inf_dev_status(sd, &dual_f, &stereo_f);
+
+	v4l2_dbg(1, debug, sd, "DETECT==st:%d dual:%d\n",
+			stereo_f, dual_f);
+
+	/* mono */
+	if ((dual_f == 0) && (stereo_f == 0)) {
+		vt->rxsubchans = V4L2_TUNER_SUB_MONO;
+		v4l2_dbg(1, debug, sd, "DETECT==MONO\n");
+	}
+
+	/* stereo */
+	if (stereo_f == 1) {
+		if (vt->audmode == V4L2_TUNER_MODE_STEREO ||
+				vt->audmode == V4L2_TUNER_MODE_LANG1) {
+			vt->rxsubchans = V4L2_TUNER_SUB_STEREO;
+			v4l2_dbg(1, debug, sd, "DETECT==ST(ST)\n");
+		} else {
+			vt->rxsubchans = V4L2_TUNER_SUB_MONO;
+			v4l2_dbg(1, debug, sd, "DETECT==ST(MONO)\n");
+		}
+	}
+
+	/* dual */
+	if (dual_f == 1) {
+		if (vt->audmode == V4L2_TUNER_MODE_LANG2) {
+			vt->rxsubchans = V4L2_TUNER_SUB_LANG2 | V4L2_TUNER_SUB_MONO;
+			v4l2_dbg(1, debug, sd, "DETECT==DUAL1\n");
+		} else {
+			vt->rxsubchans = V4L2_TUNER_SUB_LANG1 | V4L2_TUNER_SUB_MONO;
+			v4l2_dbg(1, debug, sd, "DETECT==DUAL2\n");
+		}
+	}
+	return 0;
+}
+
+static int saa717x_log_status(struct v4l2_subdev *sd)
+{
+	struct saa717x_state *state = to_state(sd);
+
+	v4l2_ctrl_handler_log_status(&state->hdl, sd->name);
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_ctrl_ops saa717x_ctrl_ops = {
+	.s_ctrl = saa717x_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops saa717x_core_ops = {
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = saa717x_g_register,
+	.s_register = saa717x_s_register,
+#endif
+	.log_status = saa717x_log_status,
+};
+
+static const struct v4l2_subdev_tuner_ops saa717x_tuner_ops = {
+	.g_tuner = saa717x_g_tuner,
+	.s_tuner = saa717x_s_tuner,
+	.s_radio = saa717x_s_radio,
+};
+
+static const struct v4l2_subdev_video_ops saa717x_video_ops = {
+	.s_std = saa717x_s_std,
+	.s_routing = saa717x_s_video_routing,
+	.s_stream = saa717x_s_stream,
+};
+
+static const struct v4l2_subdev_audio_ops saa717x_audio_ops = {
+	.s_routing = saa717x_s_audio_routing,
+};
+
+static const struct v4l2_subdev_pad_ops saa717x_pad_ops = {
+	.set_fmt = saa717x_set_fmt,
+};
+
+static const struct v4l2_subdev_ops saa717x_ops = {
+	.core = &saa717x_core_ops,
+	.tuner = &saa717x_tuner_ops,
+	.audio = &saa717x_audio_ops,
+	.video = &saa717x_video_ops,
+	.pad = &saa717x_pad_ops,
+};
+
+/* ----------------------------------------------------------------------- */
+
+
+/* i2c implementation */
+
+/* ----------------------------------------------------------------------- */
+static int saa717x_probe(struct i2c_client *client)
+{
+	struct saa717x_state *decoder;
+	struct v4l2_ctrl_handler *hdl;
+	struct v4l2_subdev *sd;
+	u8 id = 0;
+	char *p = "";
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+
+	decoder = devm_kzalloc(&client->dev, sizeof(*decoder), GFP_KERNEL);
+	if (decoder == NULL)
+		return -ENOMEM;
+
+	sd = &decoder->sd;
+	v4l2_i2c_subdev_init(sd, client, &saa717x_ops);
+
+	if (saa717x_write(sd, 0x5a4, 0xfe) &&
+			saa717x_write(sd, 0x5a5, 0x0f) &&
+			saa717x_write(sd, 0x5a6, 0x00) &&
+			saa717x_write(sd, 0x5a7, 0x01))
+		id = saa717x_read(sd, 0x5a0);
+	if (id != 0xc2 && id != 0x32 && id != 0xf2 && id != 0x6c) {
+		v4l2_dbg(1, debug, sd, "saa717x not found (id=%02x)\n", id);
+		return -ENODEV;
+	}
+	if (id == 0xc2)
+		p = "saa7173";
+	else if (id == 0x32)
+		p = "saa7174A";
+	else if (id == 0x6c)
+		p = "saa7174HL";
+	else
+		p = "saa7171";
+	v4l2_info(sd, "%s found @ 0x%x (%s)\n", p,
+			client->addr << 1, client->adapter->name);
+
+	hdl = &decoder->hdl;
+	v4l2_ctrl_handler_init(hdl, 9);
+	/* add in ascending ID order */
+	v4l2_ctrl_new_std(hdl, &saa717x_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(hdl, &saa717x_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 255, 1, 68);
+	v4l2_ctrl_new_std(hdl, &saa717x_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 255, 1, 64);
+	v4l2_ctrl_new_std(hdl, &saa717x_ctrl_ops,
+			V4L2_CID_HUE, -128, 127, 1, 0);
+	v4l2_ctrl_new_std(hdl, &saa717x_ctrl_ops,
+			V4L2_CID_AUDIO_VOLUME, 0, 65535, 65535 / 100, 42000);
+	v4l2_ctrl_new_std(hdl, &saa717x_ctrl_ops,
+			V4L2_CID_AUDIO_BALANCE, 0, 65535, 65535 / 100, 32768);
+	v4l2_ctrl_new_std(hdl, &saa717x_ctrl_ops,
+			V4L2_CID_AUDIO_BASS, -16, 15, 1, 0);
+	v4l2_ctrl_new_std(hdl, &saa717x_ctrl_ops,
+			V4L2_CID_AUDIO_TREBLE, -16, 15, 1, 0);
+	v4l2_ctrl_new_std(hdl, &saa717x_ctrl_ops,
+			V4L2_CID_AUDIO_MUTE, 0, 1, 1, 0);
+	sd->ctrl_handler = hdl;
+	if (hdl->error) {
+		int err = hdl->error;
+
+		v4l2_ctrl_handler_free(hdl);
+		return err;
+	}
+
+	decoder->std = V4L2_STD_NTSC;
+	decoder->input = -1;
+	decoder->enable = 1;
+
+	/* FIXME!! */
+	decoder->playback = 0;	/* initially capture mode used */
+	decoder->audio = 1; /* DECODER_AUDIO_48_KHZ */
+
+	decoder->audio_input = 2; /* FIXME!! */
+
+	decoder->tuner_audio_mode = TUNER_AUDIO_STEREO;
+	/* set volume, bass and treble */
+	decoder->audio_main_vol_l = 6;
+	decoder->audio_main_vol_r = 6;
+
+	v4l2_dbg(1, debug, sd, "writing init values\n");
+
+	/* FIXME!! */
+	saa717x_write_regs(sd, reg_init_initialize);
+
+	v4l2_ctrl_handler_setup(hdl);
+
+	set_current_state(TASK_INTERRUPTIBLE);
+	schedule_timeout(2*HZ);
+	return 0;
+}
+
+static void saa717x_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id saa717x_id[] = {
+	{ "saa717x", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, saa717x_id);
+
+static struct i2c_driver saa717x_driver = {
+	.driver = {
+		.name	= "saa717x",
+	},
+	.probe		= saa717x_probe,
+	.remove		= saa717x_remove,
+	.id_table	= saa717x_id,
+};
+
+module_i2c_driver(saa717x_driver);
diff --git a/drivers/media/i2c/saa7185.c b/drivers/media/i2c/saa7185.c
new file mode 100644
index 000000000..5535d71f4
--- /dev/null
+++ b/drivers/media/i2c/saa7185.c
@@ -0,0 +1,351 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * saa7185 - Philips SAA7185B video encoder driver version 0.0.3
+ *
+ * Copyright (C) 1998 Dave Perks <dperks@ibm.net>
+ *
+ * Slight changes for video timing and attachment output by
+ * Wolfgang Scherr <scherr@net4you.net>
+ *
+ * Changes by Ronald Bultje <rbultje@ronald.bitfreak.net>
+ *    - moved over to linux>=2.4.x i2c protocol (1/1/2003)
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <linux/ioctl.h>
+#include <linux/uaccess.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+
+MODULE_DESCRIPTION("Philips SAA7185 video encoder driver");
+MODULE_AUTHOR("Dave Perks");
+MODULE_LICENSE("GPL");
+
+static int debug;
+module_param(debug, int, 0);
+MODULE_PARM_DESC(debug, "Debug level (0-1)");
+
+
+/* ----------------------------------------------------------------------- */
+
+struct saa7185 {
+	struct v4l2_subdev sd;
+	unsigned char reg[128];
+
+	v4l2_std_id norm;
+};
+
+static inline struct saa7185 *to_saa7185(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct saa7185, sd);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static inline int saa7185_read(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return i2c_smbus_read_byte(client);
+}
+
+static int saa7185_write(struct v4l2_subdev *sd, u8 reg, u8 value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct saa7185 *encoder = to_saa7185(sd);
+
+	v4l2_dbg(1, debug, sd, "%02x set to %02x\n", reg, value);
+	encoder->reg[reg] = value;
+	return i2c_smbus_write_byte_data(client, reg, value);
+}
+
+static int saa7185_write_block(struct v4l2_subdev *sd,
+		const u8 *data, unsigned int len)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct saa7185 *encoder = to_saa7185(sd);
+	int ret = -1;
+	u8 reg;
+
+	/* the adv7175 has an autoincrement function, use it if
+	 * the adapter understands raw I2C */
+	if (i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		/* do raw I2C, not smbus compatible */
+		u8 block_data[32];
+		int block_len;
+
+		while (len >= 2) {
+			block_len = 0;
+			block_data[block_len++] = reg = data[0];
+			do {
+				block_data[block_len++] =
+				    encoder->reg[reg++] = data[1];
+				len -= 2;
+				data += 2;
+			} while (len >= 2 && data[0] == reg && block_len < 32);
+			ret = i2c_master_send(client, block_data, block_len);
+			if (ret < 0)
+				break;
+		}
+	} else {
+		/* do some slow I2C emulation kind of thing */
+		while (len >= 2) {
+			reg = *data++;
+			ret = saa7185_write(sd, reg, *data++);
+			if (ret < 0)
+				break;
+			len -= 2;
+		}
+	}
+
+	return ret;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const unsigned char init_common[] = {
+	0x3a, 0x0f,		/* CBENB=0, V656=0, VY2C=1,
+				 * YUV2C=1, MY2C=1, MUV2C=1 */
+
+	0x42, 0x6b,		/* OVLY0=107 */
+	0x43, 0x00,		/* OVLU0=0     white */
+	0x44, 0x00,		/* OVLV0=0   */
+	0x45, 0x22,		/* OVLY1=34  */
+	0x46, 0xac,		/* OVLU1=172   yellow */
+	0x47, 0x0e,		/* OVLV1=14  */
+	0x48, 0x03,		/* OVLY2=3   */
+	0x49, 0x1d,		/* OVLU2=29    cyan */
+	0x4a, 0xac,		/* OVLV2=172 */
+	0x4b, 0xf0,		/* OVLY3=240 */
+	0x4c, 0xc8,		/* OVLU3=200   green */
+	0x4d, 0xb9,		/* OVLV3=185 */
+	0x4e, 0xd4,		/* OVLY4=212 */
+	0x4f, 0x38,		/* OVLU4=56    magenta */
+	0x50, 0x47,		/* OVLV4=71  */
+	0x51, 0xc1,		/* OVLY5=193 */
+	0x52, 0xe3,		/* OVLU5=227   red */
+	0x53, 0x54,		/* OVLV5=84  */
+	0x54, 0xa3,		/* OVLY6=163 */
+	0x55, 0x54,		/* OVLU6=84    blue */
+	0x56, 0xf2,		/* OVLV6=242 */
+	0x57, 0x90,		/* OVLY7=144 */
+	0x58, 0x00,		/* OVLU7=0     black */
+	0x59, 0x00,		/* OVLV7=0   */
+
+	0x5a, 0x00,		/* CHPS=0    */
+	0x5b, 0x76,		/* GAINU=118 */
+	0x5c, 0xa5,		/* GAINV=165 */
+	0x5d, 0x3c,		/* BLCKL=60  */
+	0x5e, 0x3a,		/* BLNNL=58  */
+	0x5f, 0x3a,		/* CCRS=0, BLNVB=58 */
+	0x60, 0x00,		/* NULL      */
+
+	/* 0x61 - 0x66 set according to norm */
+
+	0x67, 0x00,		/* 0 : caption 1st byte odd  field */
+	0x68, 0x00,		/* 0 : caption 2nd byte odd  field */
+	0x69, 0x00,		/* 0 : caption 1st byte even field */
+	0x6a, 0x00,		/* 0 : caption 2nd byte even field */
+
+	0x6b, 0x91,		/* MODIN=2, PCREF=0, SCCLN=17 */
+	0x6c, 0x20,		/* SRCV1=0, TRCV2=1, ORCV1=0, PRCV1=0,
+				 * CBLF=0, ORCV2=0, PRCV2=0 */
+	0x6d, 0x00,		/* SRCM1=0, CCEN=0 */
+
+	0x6e, 0x0e,		/* HTRIG=0x005, approx. centered, at
+				 * least for PAL */
+	0x6f, 0x00,		/* HTRIG upper bits */
+	0x70, 0x20,		/* PHRES=0, SBLN=1, VTRIG=0 */
+
+	/* The following should not be needed */
+
+	0x71, 0x15,		/* BMRQ=0x115 */
+	0x72, 0x90,		/* EMRQ=0x690 */
+	0x73, 0x61,		/* EMRQ=0x690, BMRQ=0x115 */
+	0x74, 0x00,		/* NULL       */
+	0x75, 0x00,		/* NULL       */
+	0x76, 0x00,		/* NULL       */
+	0x77, 0x15,		/* BRCV=0x115 */
+	0x78, 0x90,		/* ERCV=0x690 */
+	0x79, 0x61,		/* ERCV=0x690, BRCV=0x115 */
+
+	/* Field length controls */
+
+	0x7a, 0x70,		/* FLC=0 */
+
+	/* The following should not be needed if SBLN = 1 */
+
+	0x7b, 0x16,		/* FAL=22 */
+	0x7c, 0x35,		/* LAL=244 */
+	0x7d, 0x20,		/* LAL=244, FAL=22 */
+};
+
+static const unsigned char init_pal[] = {
+	0x61, 0x1e,		/* FISE=0, PAL=1, SCBW=1, RTCE=1,
+				 * YGS=1, INPI=0, DOWN=0 */
+	0x62, 0xc8,		/* DECTYP=1, BSTA=72 */
+	0x63, 0xcb,		/* FSC0 */
+	0x64, 0x8a,		/* FSC1 */
+	0x65, 0x09,		/* FSC2 */
+	0x66, 0x2a,		/* FSC3 */
+};
+
+static const unsigned char init_ntsc[] = {
+	0x61, 0x1d,		/* FISE=1, PAL=0, SCBW=1, RTCE=1,
+				 * YGS=1, INPI=0, DOWN=0 */
+	0x62, 0xe6,		/* DECTYP=1, BSTA=102 */
+	0x63, 0x1f,		/* FSC0 */
+	0x64, 0x7c,		/* FSC1 */
+	0x65, 0xf0,		/* FSC2 */
+	0x66, 0x21,		/* FSC3 */
+};
+
+
+static int saa7185_init(struct v4l2_subdev *sd, u32 val)
+{
+	struct saa7185 *encoder = to_saa7185(sd);
+
+	saa7185_write_block(sd, init_common, sizeof(init_common));
+	if (encoder->norm & V4L2_STD_NTSC)
+		saa7185_write_block(sd, init_ntsc, sizeof(init_ntsc));
+	else
+		saa7185_write_block(sd, init_pal, sizeof(init_pal));
+	return 0;
+}
+
+static int saa7185_s_std_output(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct saa7185 *encoder = to_saa7185(sd);
+
+	if (std & V4L2_STD_NTSC)
+		saa7185_write_block(sd, init_ntsc, sizeof(init_ntsc));
+	else if (std & V4L2_STD_PAL)
+		saa7185_write_block(sd, init_pal, sizeof(init_pal));
+	else
+		return -EINVAL;
+	encoder->norm = std;
+	return 0;
+}
+
+static int saa7185_s_routing(struct v4l2_subdev *sd,
+			     u32 input, u32 output, u32 config)
+{
+	struct saa7185 *encoder = to_saa7185(sd);
+
+	/* RJ: input = 0: input is from SA7111
+	 input = 1: input is from ZR36060 */
+
+	switch (input) {
+	case 0:
+		/* turn off colorbar */
+		saa7185_write(sd, 0x3a, 0x0f);
+		/* Switch RTCE to 1 */
+		saa7185_write(sd, 0x61, (encoder->reg[0x61] & 0xf7) | 0x08);
+		saa7185_write(sd, 0x6e, 0x01);
+		break;
+
+	case 1:
+		/* turn off colorbar */
+		saa7185_write(sd, 0x3a, 0x0f);
+		/* Switch RTCE to 0 */
+		saa7185_write(sd, 0x61, (encoder->reg[0x61] & 0xf7) | 0x00);
+		/* SW: a slight sync problem... */
+		saa7185_write(sd, 0x6e, 0x00);
+		break;
+
+	case 2:
+		/* turn on colorbar */
+		saa7185_write(sd, 0x3a, 0x8f);
+		/* Switch RTCE to 0 */
+		saa7185_write(sd, 0x61, (encoder->reg[0x61] & 0xf7) | 0x08);
+		/* SW: a slight sync problem... */
+		saa7185_write(sd, 0x6e, 0x01);
+		break;
+
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_core_ops saa7185_core_ops = {
+	.init = saa7185_init,
+};
+
+static const struct v4l2_subdev_video_ops saa7185_video_ops = {
+	.s_std_output = saa7185_s_std_output,
+	.s_routing = saa7185_s_routing,
+};
+
+static const struct v4l2_subdev_ops saa7185_ops = {
+	.core = &saa7185_core_ops,
+	.video = &saa7185_video_ops,
+};
+
+
+/* ----------------------------------------------------------------------- */
+
+static int saa7185_probe(struct i2c_client *client)
+{
+	int i;
+	struct saa7185 *encoder;
+	struct v4l2_subdev *sd;
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -ENODEV;
+
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	encoder = devm_kzalloc(&client->dev, sizeof(*encoder), GFP_KERNEL);
+	if (encoder == NULL)
+		return -ENOMEM;
+	encoder->norm = V4L2_STD_NTSC;
+	sd = &encoder->sd;
+	v4l2_i2c_subdev_init(sd, client, &saa7185_ops);
+
+	i = saa7185_write_block(sd, init_common, sizeof(init_common));
+	if (i >= 0)
+		i = saa7185_write_block(sd, init_ntsc, sizeof(init_ntsc));
+	if (i < 0)
+		v4l2_dbg(1, debug, sd, "init error %d\n", i);
+	else
+		v4l2_dbg(1, debug, sd, "revision 0x%x\n",
+				saa7185_read(sd) >> 5);
+	return 0;
+}
+
+static void saa7185_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct saa7185 *encoder = to_saa7185(sd);
+
+	v4l2_device_unregister_subdev(sd);
+	/* SW: output off is active */
+	saa7185_write(sd, 0x61, (encoder->reg[0x61]) | 0x40);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id saa7185_id[] = {
+	{ "saa7185", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, saa7185_id);
+
+static struct i2c_driver saa7185_driver = {
+	.driver = {
+		.name	= "saa7185",
+	},
+	.probe		= saa7185_probe,
+	.remove		= saa7185_remove,
+	.id_table	= saa7185_id,
+};
+
+module_i2c_driver(saa7185_driver);
diff --git a/drivers/media/i2c/sony-btf-mpx.c b/drivers/media/i2c/sony-btf-mpx.c
new file mode 100644
index 000000000..0f53834f3
--- /dev/null
+++ b/drivers/media/i2c/sony-btf-mpx.c
@@ -0,0 +1,382 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2005-2006 Micronas USA Inc.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <media/tuner.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-device.h>
+#include <linux/slab.h>
+
+MODULE_DESCRIPTION("sony-btf-mpx driver");
+MODULE_LICENSE("GPL v2");
+
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "debug level 0=off(default) 1=on");
+
+/* #define MPX_DEBUG */
+
+/*
+ * Note:
+ *
+ * AS(IF/MPX) pin:      LOW      HIGH/OPEN
+ * IF/MPX address:   0x42/0x40   0x43/0x44
+ */
+
+
+static int force_mpx_mode = -1;
+module_param(force_mpx_mode, int, 0644);
+
+struct sony_btf_mpx {
+	struct v4l2_subdev sd;
+	int mpxmode;
+	u32 audmode;
+};
+
+static inline struct sony_btf_mpx *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct sony_btf_mpx, sd);
+}
+
+static int mpx_write(struct i2c_client *client, int dev, int addr, int val)
+{
+	u8 buffer[5];
+	struct i2c_msg msg;
+
+	buffer[0] = dev;
+	buffer[1] = addr >> 8;
+	buffer[2] = addr & 0xff;
+	buffer[3] = val >> 8;
+	buffer[4] = val & 0xff;
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = 5;
+	msg.buf = buffer;
+	i2c_transfer(client->adapter, &msg, 1);
+	return 0;
+}
+
+/*
+ * MPX register values for the BTF-PG472Z:
+ *
+ *                                 FM_     NICAM_  SCART_
+ *          MODUS  SOURCE    ACB   PRESCAL PRESCAL PRESCAL SYSTEM  VOLUME
+ *         10/0030 12/0008 12/0013 12/000E 12/0010 12/0000 10/0020 12/0000
+ *         ---------------------------------------------------------------
+ * Auto     1003    0020    0100    2603    5000    XXXX    0001    7500
+ *
+ * B/G
+ *  Mono    1003    0020    0100    2603    5000    XXXX    0003    7500
+ *  A2      1003    0020    0100    2601    5000    XXXX    0003    7500
+ *  NICAM   1003    0120    0100    2603    5000    XXXX    0008    7500
+ *
+ * I
+ *  Mono    1003    0020    0100    2603    7900    XXXX    000A    7500
+ *  NICAM   1003    0120    0100    2603    7900    XXXX    000A    7500
+ *
+ * D/K
+ *  Mono    1003    0020    0100    2603    5000    XXXX    0004    7500
+ *  A2-1    1003    0020    0100    2601    5000    XXXX    0004    7500
+ *  A2-2    1003    0020    0100    2601    5000    XXXX    0005    7500
+ *  A2-3    1003    0020    0100    2601    5000    XXXX    0007    7500
+ *  NICAM   1003    0120    0100    2603    5000    XXXX    000B    7500
+ *
+ * L/L'
+ *  Mono    0003    0200    0100    7C03    5000    2200    0009    7500
+ *  NICAM   0003    0120    0100    7C03    5000    XXXX    0009    7500
+ *
+ * M
+ *  Mono    1003    0200    0100    2B03    5000    2B00    0002    7500
+ *
+ * For Asia, replace the 0x26XX in FM_PRESCALE with 0x14XX.
+ *
+ * Bilingual selection in A2/NICAM:
+ *
+ *         High byte of SOURCE     Left chan   Right chan
+ *                 0x01              MAIN         SUB
+ *                 0x03              MAIN         MAIN
+ *                 0x04              SUB          SUB
+ *
+ * Force mono in NICAM by setting the high byte of SOURCE to 0x02 (L/L') or
+ * 0x00 (all other bands).  Force mono in A2 with FMONO_A2:
+ *
+ *                      FMONO_A2
+ *                      10/0022
+ *                      --------
+ *     Forced mono ON     07F0
+ *     Forced mono OFF    0190
+ */
+
+static const struct {
+	enum { AUD_MONO, AUD_A2, AUD_NICAM, AUD_NICAM_L } audio_mode;
+	u16 modus;
+	u16 source;
+	u16 acb;
+	u16 fm_prescale;
+	u16 nicam_prescale;
+	u16 scart_prescale;
+	u16 system;
+	u16 volume;
+} mpx_audio_modes[] = {
+	/* Auto */	{ AUD_MONO,	0x1003, 0x0020, 0x0100, 0x2603,
+					0x5000, 0x0000, 0x0001, 0x7500 },
+	/* B/G Mono */	{ AUD_MONO,	0x1003, 0x0020, 0x0100, 0x2603,
+					0x5000, 0x0000, 0x0003, 0x7500 },
+	/* B/G A2 */	{ AUD_A2,	0x1003, 0x0020, 0x0100, 0x2601,
+					0x5000, 0x0000, 0x0003, 0x7500 },
+	/* B/G NICAM */ { AUD_NICAM,	0x1003, 0x0120, 0x0100, 0x2603,
+					0x5000, 0x0000, 0x0008, 0x7500 },
+	/* I Mono */	{ AUD_MONO,	0x1003, 0x0020, 0x0100, 0x2603,
+					0x7900, 0x0000, 0x000A, 0x7500 },
+	/* I NICAM */	{ AUD_NICAM,	0x1003, 0x0120, 0x0100, 0x2603,
+					0x7900, 0x0000, 0x000A, 0x7500 },
+	/* D/K Mono */	{ AUD_MONO,	0x1003, 0x0020, 0x0100, 0x2603,
+					0x5000, 0x0000, 0x0004, 0x7500 },
+	/* D/K A2-1 */	{ AUD_A2,	0x1003, 0x0020, 0x0100, 0x2601,
+					0x5000, 0x0000, 0x0004, 0x7500 },
+	/* D/K A2-2 */	{ AUD_A2,	0x1003, 0x0020, 0x0100, 0x2601,
+					0x5000, 0x0000, 0x0005, 0x7500 },
+	/* D/K A2-3 */	{ AUD_A2,	0x1003, 0x0020, 0x0100, 0x2601,
+					0x5000, 0x0000, 0x0007, 0x7500 },
+	/* D/K NICAM */	{ AUD_NICAM,	0x1003, 0x0120, 0x0100, 0x2603,
+					0x5000, 0x0000, 0x000B, 0x7500 },
+	/* L/L' Mono */	{ AUD_MONO,	0x0003, 0x0200, 0x0100, 0x7C03,
+					0x5000, 0x2200, 0x0009, 0x7500 },
+	/* L/L' NICAM */{ AUD_NICAM_L,	0x0003, 0x0120, 0x0100, 0x7C03,
+					0x5000, 0x0000, 0x0009, 0x7500 },
+};
+
+#define MPX_NUM_MODES	ARRAY_SIZE(mpx_audio_modes)
+
+static int mpx_setup(struct sony_btf_mpx *t)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&t->sd);
+	u16 source = 0;
+	u8 buffer[3];
+	struct i2c_msg msg;
+	int mode = t->mpxmode;
+
+	/* reset MPX */
+	buffer[0] = 0x00;
+	buffer[1] = 0x80;
+	buffer[2] = 0x00;
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = 3;
+	msg.buf = buffer;
+	i2c_transfer(client->adapter, &msg, 1);
+	buffer[1] = 0x00;
+	i2c_transfer(client->adapter, &msg, 1);
+
+	if (t->audmode != V4L2_TUNER_MODE_MONO)
+		mode++;
+
+	if (mpx_audio_modes[mode].audio_mode != AUD_MONO) {
+		switch (t->audmode) {
+		case V4L2_TUNER_MODE_MONO:
+			switch (mpx_audio_modes[mode].audio_mode) {
+			case AUD_A2:
+				source = mpx_audio_modes[mode].source;
+				break;
+			case AUD_NICAM:
+				source = 0x0000;
+				break;
+			case AUD_NICAM_L:
+				source = 0x0200;
+				break;
+			default:
+				break;
+			}
+			break;
+		case V4L2_TUNER_MODE_STEREO:
+			source = mpx_audio_modes[mode].source;
+			break;
+		case V4L2_TUNER_MODE_LANG1:
+			source = 0x0300;
+			break;
+		case V4L2_TUNER_MODE_LANG2:
+			source = 0x0400;
+			break;
+		}
+		source |= mpx_audio_modes[mode].source & 0x00ff;
+	} else
+		source = mpx_audio_modes[mode].source;
+
+	mpx_write(client, 0x10, 0x0030, mpx_audio_modes[mode].modus);
+	mpx_write(client, 0x12, 0x0008, source);
+	mpx_write(client, 0x12, 0x0013, mpx_audio_modes[mode].acb);
+	mpx_write(client, 0x12, 0x000e,
+			mpx_audio_modes[mode].fm_prescale);
+	mpx_write(client, 0x12, 0x0010,
+			mpx_audio_modes[mode].nicam_prescale);
+	mpx_write(client, 0x12, 0x000d,
+			mpx_audio_modes[mode].scart_prescale);
+	mpx_write(client, 0x10, 0x0020, mpx_audio_modes[mode].system);
+	mpx_write(client, 0x12, 0x0000, mpx_audio_modes[mode].volume);
+	if (mpx_audio_modes[mode].audio_mode == AUD_A2)
+		mpx_write(client, 0x10, 0x0022,
+			t->audmode == V4L2_TUNER_MODE_MONO ? 0x07f0 : 0x0190);
+
+#ifdef MPX_DEBUG
+	{
+		u8 buf1[3], buf2[2];
+		struct i2c_msg msgs[2];
+
+		v4l2_info(client,
+			"MPX registers: %04x %04x %04x %04x %04x %04x %04x %04x\n",
+			mpx_audio_modes[mode].modus,
+			source,
+			mpx_audio_modes[mode].acb,
+			mpx_audio_modes[mode].fm_prescale,
+			mpx_audio_modes[mode].nicam_prescale,
+			mpx_audio_modes[mode].scart_prescale,
+			mpx_audio_modes[mode].system,
+			mpx_audio_modes[mode].volume);
+		buf1[0] = 0x11;
+		buf1[1] = 0x00;
+		buf1[2] = 0x7e;
+		msgs[0].addr = client->addr;
+		msgs[0].flags = 0;
+		msgs[0].len = 3;
+		msgs[0].buf = buf1;
+		msgs[1].addr = client->addr;
+		msgs[1].flags = I2C_M_RD;
+		msgs[1].len = 2;
+		msgs[1].buf = buf2;
+		i2c_transfer(client->adapter, msgs, 2);
+		v4l2_info(client, "MPX system: %02x%02x\n",
+				buf2[0], buf2[1]);
+		buf1[0] = 0x11;
+		buf1[1] = 0x02;
+		buf1[2] = 0x00;
+		i2c_transfer(client->adapter, msgs, 2);
+		v4l2_info(client, "MPX status: %02x%02x\n",
+				buf2[0], buf2[1]);
+	}
+#endif
+	return 0;
+}
+
+
+static int sony_btf_mpx_s_std(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct sony_btf_mpx *t = to_state(sd);
+	int default_mpx_mode = 0;
+
+	if (std & V4L2_STD_PAL_BG)
+		default_mpx_mode = 1;
+	else if (std & V4L2_STD_PAL_I)
+		default_mpx_mode = 4;
+	else if (std & V4L2_STD_PAL_DK)
+		default_mpx_mode = 6;
+	else if (std & V4L2_STD_SECAM_L)
+		default_mpx_mode = 11;
+
+	if (default_mpx_mode != t->mpxmode) {
+		t->mpxmode = default_mpx_mode;
+		mpx_setup(t);
+	}
+	return 0;
+}
+
+static int sony_btf_mpx_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
+{
+	struct sony_btf_mpx *t = to_state(sd);
+
+	vt->capability = V4L2_TUNER_CAP_NORM |
+		V4L2_TUNER_CAP_STEREO | V4L2_TUNER_CAP_LANG1 |
+		V4L2_TUNER_CAP_LANG2;
+	vt->rxsubchans = V4L2_TUNER_SUB_MONO |
+		V4L2_TUNER_SUB_STEREO | V4L2_TUNER_SUB_LANG1 |
+		V4L2_TUNER_SUB_LANG2;
+	vt->audmode = t->audmode;
+	return 0;
+}
+
+static int sony_btf_mpx_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *vt)
+{
+	struct sony_btf_mpx *t = to_state(sd);
+
+	if (vt->type != V4L2_TUNER_ANALOG_TV)
+		return -EINVAL;
+
+	if (vt->audmode != t->audmode) {
+		t->audmode = vt->audmode;
+		mpx_setup(t);
+	}
+	return 0;
+}
+
+/* --------------------------------------------------------------------------*/
+
+static const struct v4l2_subdev_tuner_ops sony_btf_mpx_tuner_ops = {
+	.s_tuner = sony_btf_mpx_s_tuner,
+	.g_tuner = sony_btf_mpx_g_tuner,
+};
+
+static const struct v4l2_subdev_video_ops sony_btf_mpx_video_ops = {
+	.s_std = sony_btf_mpx_s_std,
+};
+
+static const struct v4l2_subdev_ops sony_btf_mpx_ops = {
+	.tuner = &sony_btf_mpx_tuner_ops,
+	.video = &sony_btf_mpx_video_ops,
+};
+
+/* --------------------------------------------------------------------------*/
+
+static int sony_btf_mpx_probe(struct i2c_client *client)
+{
+	struct sony_btf_mpx *t;
+	struct v4l2_subdev *sd;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_I2C_BLOCK))
+		return -ENODEV;
+
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	t = devm_kzalloc(&client->dev, sizeof(*t), GFP_KERNEL);
+	if (t == NULL)
+		return -ENOMEM;
+
+	sd = &t->sd;
+	v4l2_i2c_subdev_init(sd, client, &sony_btf_mpx_ops);
+
+	/* Initialize sony_btf_mpx */
+	t->mpxmode = 0;
+	t->audmode = V4L2_TUNER_MODE_STEREO;
+
+	return 0;
+}
+
+static void sony_btf_mpx_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id sony_btf_mpx_id[] = {
+	{ "sony-btf-mpx", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, sony_btf_mpx_id);
+
+static struct i2c_driver sony_btf_mpx_driver = {
+	.driver = {
+		.name	= "sony-btf-mpx",
+	},
+	.probe = sony_btf_mpx_probe,
+	.remove = sony_btf_mpx_remove,
+	.id_table = sony_btf_mpx_id,
+};
+module_i2c_driver(sony_btf_mpx_driver);
diff --git a/drivers/media/i2c/st-mipid02.c b/drivers/media/i2c/st-mipid02.c
new file mode 100644
index 000000000..dab147871
--- /dev/null
+++ b/drivers/media/i2c/st-mipid02.c
@@ -0,0 +1,1127 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Driver for ST MIPID02 CSI-2 to PARALLEL bridge
+ *
+ * Copyright (C) STMicroelectronics SA 2019
+ * Authors: Mickael Guene <mickael.guene@st.com>
+ *          for STMicroelectronics.
+ *
+ *
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/of_graph.h>
+#include <linux/regulator/consumer.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+#define MIPID02_CLK_LANE_WR_REG1			0x01
+#define MIPID02_CLK_LANE_REG1				0x02
+#define MIPID02_CLK_LANE_REG3				0x04
+#define MIPID02_DATA_LANE0_REG1				0x05
+#define MIPID02_DATA_LANE0_REG2				0x06
+#define MIPID02_DATA_LANE1_REG1				0x09
+#define MIPID02_DATA_LANE1_REG2				0x0a
+#define MIPID02_MODE_REG1				0x14
+#define MIPID02_MODE_REG2				0x15
+#define MIPID02_DATA_ID_RREG				0x17
+#define MIPID02_DATA_SELECTION_CTRL			0x19
+#define MIPID02_PIX_WIDTH_CTRL				0x1e
+#define MIPID02_PIX_WIDTH_CTRL_EMB			0x1f
+
+/* Bits definition for MIPID02_CLK_LANE_REG1 */
+#define CLK_ENABLE					BIT(0)
+/* Bits definition for MIPID02_CLK_LANE_REG3 */
+#define CLK_MIPI_CSI					BIT(1)
+/* Bits definition for MIPID02_DATA_LANE0_REG1 */
+#define DATA_ENABLE					BIT(0)
+/* Bits definition for MIPID02_DATA_LANEx_REG2 */
+#define DATA_MIPI_CSI					BIT(0)
+/* Bits definition for MIPID02_MODE_REG1 */
+#define MODE_DATA_SWAP					BIT(2)
+#define MODE_NO_BYPASS					BIT(6)
+/* Bits definition for MIPID02_MODE_REG2 */
+#define MODE_HSYNC_ACTIVE_HIGH				BIT(1)
+#define MODE_VSYNC_ACTIVE_HIGH				BIT(2)
+#define MODE_PCLK_SAMPLE_RISING				BIT(3)
+/* Bits definition for MIPID02_DATA_SELECTION_CTRL */
+#define SELECTION_MANUAL_DATA				BIT(2)
+#define SELECTION_MANUAL_WIDTH				BIT(3)
+
+static const u32 mipid02_supported_fmt_codes[] = {
+	MEDIA_BUS_FMT_SBGGR8_1X8, MEDIA_BUS_FMT_SGBRG8_1X8,
+	MEDIA_BUS_FMT_SGRBG8_1X8, MEDIA_BUS_FMT_SRGGB8_1X8,
+	MEDIA_BUS_FMT_SBGGR10_1X10, MEDIA_BUS_FMT_SGBRG10_1X10,
+	MEDIA_BUS_FMT_SGRBG10_1X10, MEDIA_BUS_FMT_SRGGB10_1X10,
+	MEDIA_BUS_FMT_SBGGR12_1X12, MEDIA_BUS_FMT_SGBRG12_1X12,
+	MEDIA_BUS_FMT_SGRBG12_1X12, MEDIA_BUS_FMT_SRGGB12_1X12,
+	MEDIA_BUS_FMT_YUYV8_1X16, MEDIA_BUS_FMT_YVYU8_1X16,
+	MEDIA_BUS_FMT_UYVY8_1X16, MEDIA_BUS_FMT_VYUY8_1X16,
+	MEDIA_BUS_FMT_RGB565_1X16, MEDIA_BUS_FMT_BGR888_1X24,
+	MEDIA_BUS_FMT_RGB565_2X8_LE, MEDIA_BUS_FMT_RGB565_2X8_BE,
+	MEDIA_BUS_FMT_YUYV8_2X8, MEDIA_BUS_FMT_YVYU8_2X8,
+	MEDIA_BUS_FMT_UYVY8_2X8, MEDIA_BUS_FMT_VYUY8_2X8,
+	MEDIA_BUS_FMT_JPEG_1X8
+};
+
+/* regulator supplies */
+static const char * const mipid02_supply_name[] = {
+	"VDDE", /* 1.8V digital I/O supply */
+	"VDDIN", /* 1V8 voltage regulator supply */
+};
+
+#define MIPID02_NUM_SUPPLIES		ARRAY_SIZE(mipid02_supply_name)
+
+#define MIPID02_SINK_0			0
+#define MIPID02_SINK_1			1
+#define MIPID02_SOURCE			2
+#define MIPID02_PAD_NB			3
+
+struct mipid02_dev {
+	struct i2c_client *i2c_client;
+	struct regulator_bulk_data supplies[MIPID02_NUM_SUPPLIES];
+	struct v4l2_subdev sd;
+	struct media_pad pad[MIPID02_PAD_NB];
+	struct clk *xclk;
+	struct gpio_desc *reset_gpio;
+	/* endpoints info */
+	struct v4l2_fwnode_endpoint rx;
+	u64 link_frequency;
+	struct v4l2_fwnode_endpoint tx;
+	/* remote source */
+	struct v4l2_async_notifier notifier;
+	struct v4l2_subdev *s_subdev;
+	/* registers */
+	struct {
+		u8 clk_lane_reg1;
+		u8 data_lane0_reg1;
+		u8 data_lane1_reg1;
+		u8 mode_reg1;
+		u8 mode_reg2;
+		u8 data_selection_ctrl;
+		u8 data_id_rreg;
+		u8 pix_width_ctrl;
+		u8 pix_width_ctrl_emb;
+	} r;
+	/* lock to protect all members below */
+	struct mutex lock;
+	bool streaming;
+	struct v4l2_mbus_framefmt fmt;
+};
+
+static int bpp_from_code(__u32 code)
+{
+	switch (code) {
+	case MEDIA_BUS_FMT_SBGGR8_1X8:
+	case MEDIA_BUS_FMT_SGBRG8_1X8:
+	case MEDIA_BUS_FMT_SGRBG8_1X8:
+	case MEDIA_BUS_FMT_SRGGB8_1X8:
+		return 8;
+	case MEDIA_BUS_FMT_SBGGR10_1X10:
+	case MEDIA_BUS_FMT_SGBRG10_1X10:
+	case MEDIA_BUS_FMT_SGRBG10_1X10:
+	case MEDIA_BUS_FMT_SRGGB10_1X10:
+		return 10;
+	case MEDIA_BUS_FMT_SBGGR12_1X12:
+	case MEDIA_BUS_FMT_SGBRG12_1X12:
+	case MEDIA_BUS_FMT_SGRBG12_1X12:
+	case MEDIA_BUS_FMT_SRGGB12_1X12:
+		return 12;
+	case MEDIA_BUS_FMT_YUYV8_1X16:
+	case MEDIA_BUS_FMT_YVYU8_1X16:
+	case MEDIA_BUS_FMT_UYVY8_1X16:
+	case MEDIA_BUS_FMT_VYUY8_1X16:
+	case MEDIA_BUS_FMT_RGB565_1X16:
+	case MEDIA_BUS_FMT_YUYV8_2X8:
+	case MEDIA_BUS_FMT_YVYU8_2X8:
+	case MEDIA_BUS_FMT_UYVY8_2X8:
+	case MEDIA_BUS_FMT_VYUY8_2X8:
+	case MEDIA_BUS_FMT_RGB565_2X8_LE:
+	case MEDIA_BUS_FMT_RGB565_2X8_BE:
+		return 16;
+	case MEDIA_BUS_FMT_BGR888_1X24:
+		return 24;
+	default:
+		return 0;
+	}
+}
+
+static u8 data_type_from_code(__u32 code)
+{
+	switch (code) {
+	case MEDIA_BUS_FMT_SBGGR8_1X8:
+	case MEDIA_BUS_FMT_SGBRG8_1X8:
+	case MEDIA_BUS_FMT_SGRBG8_1X8:
+	case MEDIA_BUS_FMT_SRGGB8_1X8:
+		return 0x2a;
+	case MEDIA_BUS_FMT_SBGGR10_1X10:
+	case MEDIA_BUS_FMT_SGBRG10_1X10:
+	case MEDIA_BUS_FMT_SGRBG10_1X10:
+	case MEDIA_BUS_FMT_SRGGB10_1X10:
+		return 0x2b;
+	case MEDIA_BUS_FMT_SBGGR12_1X12:
+	case MEDIA_BUS_FMT_SGBRG12_1X12:
+	case MEDIA_BUS_FMT_SGRBG12_1X12:
+	case MEDIA_BUS_FMT_SRGGB12_1X12:
+		return 0x2c;
+	case MEDIA_BUS_FMT_YUYV8_1X16:
+	case MEDIA_BUS_FMT_YVYU8_1X16:
+	case MEDIA_BUS_FMT_UYVY8_1X16:
+	case MEDIA_BUS_FMT_VYUY8_1X16:
+	case MEDIA_BUS_FMT_YUYV8_2X8:
+	case MEDIA_BUS_FMT_YVYU8_2X8:
+	case MEDIA_BUS_FMT_UYVY8_2X8:
+	case MEDIA_BUS_FMT_VYUY8_2X8:
+		return 0x1e;
+	case MEDIA_BUS_FMT_BGR888_1X24:
+		return 0x24;
+	case MEDIA_BUS_FMT_RGB565_1X16:
+	case MEDIA_BUS_FMT_RGB565_2X8_LE:
+	case MEDIA_BUS_FMT_RGB565_2X8_BE:
+		return 0x22;
+	default:
+		return 0;
+	}
+}
+
+static void init_format(struct v4l2_mbus_framefmt *fmt)
+{
+	fmt->code = MEDIA_BUS_FMT_SBGGR8_1X8;
+	fmt->field = V4L2_FIELD_NONE;
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+	fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(V4L2_COLORSPACE_SRGB);
+	fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
+	fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(V4L2_COLORSPACE_SRGB);
+	fmt->width = 640;
+	fmt->height = 480;
+}
+
+static __u32 get_fmt_code(__u32 code)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(mipid02_supported_fmt_codes); i++) {
+		if (code == mipid02_supported_fmt_codes[i])
+			return code;
+	}
+
+	return mipid02_supported_fmt_codes[0];
+}
+
+static __u32 serial_to_parallel_code(__u32 serial)
+{
+	if (serial == MEDIA_BUS_FMT_RGB565_1X16)
+		return MEDIA_BUS_FMT_RGB565_2X8_LE;
+	if (serial == MEDIA_BUS_FMT_YUYV8_1X16)
+		return MEDIA_BUS_FMT_YUYV8_2X8;
+	if (serial == MEDIA_BUS_FMT_YVYU8_1X16)
+		return MEDIA_BUS_FMT_YVYU8_2X8;
+	if (serial == MEDIA_BUS_FMT_UYVY8_1X16)
+		return MEDIA_BUS_FMT_UYVY8_2X8;
+	if (serial == MEDIA_BUS_FMT_VYUY8_1X16)
+		return MEDIA_BUS_FMT_VYUY8_2X8;
+	if (serial == MEDIA_BUS_FMT_BGR888_1X24)
+		return MEDIA_BUS_FMT_BGR888_3X8;
+
+	return serial;
+}
+
+static inline struct mipid02_dev *to_mipid02_dev(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct mipid02_dev, sd);
+}
+
+static int mipid02_read_reg(struct mipid02_dev *bridge, u16 reg, u8 *val)
+{
+	struct i2c_client *client = bridge->i2c_client;
+	struct i2c_msg msg[2];
+	u8 buf[2];
+	int ret;
+
+	buf[0] = reg >> 8;
+	buf[1] = reg & 0xff;
+
+	msg[0].addr = client->addr;
+	msg[0].flags = client->flags;
+	msg[0].buf = buf;
+	msg[0].len = sizeof(buf);
+
+	msg[1].addr = client->addr;
+	msg[1].flags = client->flags | I2C_M_RD;
+	msg[1].buf = val;
+	msg[1].len = 1;
+
+	ret = i2c_transfer(client->adapter, msg, 2);
+	if (ret < 0) {
+		dev_dbg(&client->dev, "%s: %x i2c_transfer, reg: %x => %d\n",
+			    __func__, client->addr, reg, ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int mipid02_write_reg(struct mipid02_dev *bridge, u16 reg, u8 val)
+{
+	struct i2c_client *client = bridge->i2c_client;
+	struct i2c_msg msg;
+	u8 buf[3];
+	int ret;
+
+	buf[0] = reg >> 8;
+	buf[1] = reg & 0xff;
+	buf[2] = val;
+
+	msg.addr = client->addr;
+	msg.flags = client->flags;
+	msg.buf = buf;
+	msg.len = sizeof(buf);
+
+	ret = i2c_transfer(client->adapter, &msg, 1);
+	if (ret < 0) {
+		dev_dbg(&client->dev, "%s: i2c_transfer, reg: %x => %d\n",
+			    __func__, reg, ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int mipid02_get_regulators(struct mipid02_dev *bridge)
+{
+	unsigned int i;
+
+	for (i = 0; i < MIPID02_NUM_SUPPLIES; i++)
+		bridge->supplies[i].supply = mipid02_supply_name[i];
+
+	return devm_regulator_bulk_get(&bridge->i2c_client->dev,
+				       MIPID02_NUM_SUPPLIES,
+				       bridge->supplies);
+}
+
+static void mipid02_apply_reset(struct mipid02_dev *bridge)
+{
+	gpiod_set_value_cansleep(bridge->reset_gpio, 0);
+	usleep_range(5000, 10000);
+	gpiod_set_value_cansleep(bridge->reset_gpio, 1);
+	usleep_range(5000, 10000);
+	gpiod_set_value_cansleep(bridge->reset_gpio, 0);
+	usleep_range(5000, 10000);
+}
+
+static int mipid02_set_power_on(struct mipid02_dev *bridge)
+{
+	struct i2c_client *client = bridge->i2c_client;
+	int ret;
+
+	ret = clk_prepare_enable(bridge->xclk);
+	if (ret) {
+		dev_err(&client->dev, "%s: failed to enable clock\n", __func__);
+		return ret;
+	}
+
+	ret = regulator_bulk_enable(MIPID02_NUM_SUPPLIES,
+				    bridge->supplies);
+	if (ret) {
+		dev_err(&client->dev, "%s: failed to enable regulators\n",
+			    __func__);
+		goto xclk_off;
+	}
+
+	if (bridge->reset_gpio) {
+		dev_dbg(&client->dev, "apply reset");
+		mipid02_apply_reset(bridge);
+	} else {
+		dev_dbg(&client->dev, "don't apply reset");
+		usleep_range(5000, 10000);
+	}
+
+	return 0;
+
+xclk_off:
+	clk_disable_unprepare(bridge->xclk);
+	return ret;
+}
+
+static void mipid02_set_power_off(struct mipid02_dev *bridge)
+{
+	regulator_bulk_disable(MIPID02_NUM_SUPPLIES, bridge->supplies);
+	clk_disable_unprepare(bridge->xclk);
+}
+
+static int mipid02_detect(struct mipid02_dev *bridge)
+{
+	u8 reg;
+
+	/*
+	 * There is no version registers. Just try to read register
+	 * MIPID02_CLK_LANE_WR_REG1.
+	 */
+	return mipid02_read_reg(bridge, MIPID02_CLK_LANE_WR_REG1, &reg);
+}
+
+static u32 mipid02_get_link_freq_from_cid_link_freq(struct mipid02_dev *bridge,
+						    struct v4l2_subdev *subdev)
+{
+	struct v4l2_querymenu qm = {.id = V4L2_CID_LINK_FREQ, };
+	struct v4l2_ctrl *ctrl;
+	int ret;
+
+	ctrl = v4l2_ctrl_find(subdev->ctrl_handler, V4L2_CID_LINK_FREQ);
+	if (!ctrl)
+		return 0;
+	qm.index = v4l2_ctrl_g_ctrl(ctrl);
+
+	ret = v4l2_querymenu(subdev->ctrl_handler, &qm);
+	if (ret)
+		return 0;
+
+	return qm.value;
+}
+
+static u32 mipid02_get_link_freq_from_cid_pixel_rate(struct mipid02_dev *bridge,
+						     struct v4l2_subdev *subdev)
+{
+	struct v4l2_fwnode_endpoint *ep = &bridge->rx;
+	struct v4l2_ctrl *ctrl;
+	u32 pixel_clock;
+	u32 bpp = bpp_from_code(bridge->fmt.code);
+
+	ctrl = v4l2_ctrl_find(subdev->ctrl_handler, V4L2_CID_PIXEL_RATE);
+	if (!ctrl)
+		return 0;
+	pixel_clock = v4l2_ctrl_g_ctrl_int64(ctrl);
+
+	return pixel_clock * bpp / (2 * ep->bus.mipi_csi2.num_data_lanes);
+}
+
+/*
+ * We need to know link frequency to setup clk_lane_reg1 timings. Link frequency
+ * will be computed using connected device V4L2_CID_PIXEL_RATE, bit per pixel
+ * and number of lanes.
+ */
+static int mipid02_configure_from_rx_speed(struct mipid02_dev *bridge)
+{
+	struct i2c_client *client = bridge->i2c_client;
+	struct v4l2_subdev *subdev = bridge->s_subdev;
+	u32 link_freq;
+
+	link_freq = mipid02_get_link_freq_from_cid_link_freq(bridge, subdev);
+	if (!link_freq) {
+		link_freq = mipid02_get_link_freq_from_cid_pixel_rate(bridge,
+								      subdev);
+		if (!link_freq) {
+			dev_err(&client->dev, "Failed to get link frequency");
+			return -EINVAL;
+		}
+	}
+
+	dev_dbg(&client->dev, "detect link_freq = %d Hz", link_freq);
+	bridge->r.clk_lane_reg1 |= (2000000000 / link_freq) << 2;
+
+	return 0;
+}
+
+static int mipid02_configure_clk_lane(struct mipid02_dev *bridge)
+{
+	struct i2c_client *client = bridge->i2c_client;
+	struct v4l2_fwnode_endpoint *ep = &bridge->rx;
+	bool *polarities = ep->bus.mipi_csi2.lane_polarities;
+
+	/* midid02 doesn't support clock lane remapping */
+	if (ep->bus.mipi_csi2.clock_lane != 0) {
+		dev_err(&client->dev, "clk lane must be map to lane 0\n");
+		return -EINVAL;
+	}
+	bridge->r.clk_lane_reg1 |= (polarities[0] << 1) | CLK_ENABLE;
+
+	return 0;
+}
+
+static int mipid02_configure_data0_lane(struct mipid02_dev *bridge, int nb,
+					bool are_lanes_swap, bool *polarities)
+{
+	bool are_pin_swap = are_lanes_swap ? polarities[2] : polarities[1];
+
+	if (nb == 1 && are_lanes_swap)
+		return 0;
+
+	/*
+	 * data lane 0 as pin swap polarity reversed compared to clock and
+	 * data lane 1
+	 */
+	if (!are_pin_swap)
+		bridge->r.data_lane0_reg1 = 1 << 1;
+	bridge->r.data_lane0_reg1 |= DATA_ENABLE;
+
+	return 0;
+}
+
+static int mipid02_configure_data1_lane(struct mipid02_dev *bridge, int nb,
+					bool are_lanes_swap, bool *polarities)
+{
+	bool are_pin_swap = are_lanes_swap ? polarities[1] : polarities[2];
+
+	if (nb == 1 && !are_lanes_swap)
+		return 0;
+
+	if (are_pin_swap)
+		bridge->r.data_lane1_reg1 = 1 << 1;
+	bridge->r.data_lane1_reg1 |= DATA_ENABLE;
+
+	return 0;
+}
+
+static int mipid02_configure_from_rx(struct mipid02_dev *bridge)
+{
+	struct v4l2_fwnode_endpoint *ep = &bridge->rx;
+	bool are_lanes_swap = ep->bus.mipi_csi2.data_lanes[0] == 2;
+	bool *polarities = ep->bus.mipi_csi2.lane_polarities;
+	int nb = ep->bus.mipi_csi2.num_data_lanes;
+	int ret;
+
+	ret = mipid02_configure_clk_lane(bridge);
+	if (ret)
+		return ret;
+
+	ret = mipid02_configure_data0_lane(bridge, nb, are_lanes_swap,
+					   polarities);
+	if (ret)
+		return ret;
+
+	ret = mipid02_configure_data1_lane(bridge, nb, are_lanes_swap,
+					   polarities);
+	if (ret)
+		return ret;
+
+	bridge->r.mode_reg1 |= are_lanes_swap ? MODE_DATA_SWAP : 0;
+	bridge->r.mode_reg1 |= (nb - 1) << 1;
+
+	return mipid02_configure_from_rx_speed(bridge);
+}
+
+static int mipid02_configure_from_tx(struct mipid02_dev *bridge)
+{
+	struct v4l2_fwnode_endpoint *ep = &bridge->tx;
+
+	bridge->r.data_selection_ctrl = SELECTION_MANUAL_WIDTH;
+	bridge->r.pix_width_ctrl = ep->bus.parallel.bus_width;
+	bridge->r.pix_width_ctrl_emb = ep->bus.parallel.bus_width;
+	if (ep->bus.parallel.flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
+		bridge->r.mode_reg2 |= MODE_HSYNC_ACTIVE_HIGH;
+	if (ep->bus.parallel.flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH)
+		bridge->r.mode_reg2 |= MODE_VSYNC_ACTIVE_HIGH;
+	if (ep->bus.parallel.flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
+		bridge->r.mode_reg2 |= MODE_PCLK_SAMPLE_RISING;
+
+	return 0;
+}
+
+static int mipid02_configure_from_code(struct mipid02_dev *bridge)
+{
+	u8 data_type;
+
+	bridge->r.data_id_rreg = 0;
+
+	if (bridge->fmt.code != MEDIA_BUS_FMT_JPEG_1X8) {
+		bridge->r.data_selection_ctrl |= SELECTION_MANUAL_DATA;
+
+		data_type = data_type_from_code(bridge->fmt.code);
+		if (!data_type)
+			return -EINVAL;
+		bridge->r.data_id_rreg = data_type;
+	}
+
+	return 0;
+}
+
+static int mipid02_stream_disable(struct mipid02_dev *bridge)
+{
+	struct i2c_client *client = bridge->i2c_client;
+	int ret = -EINVAL;
+
+	if (!bridge->s_subdev)
+		goto error;
+
+	ret = v4l2_subdev_call(bridge->s_subdev, video, s_stream, 0);
+	if (ret)
+		goto error;
+
+	/* Disable all lanes */
+	ret = mipid02_write_reg(bridge, MIPID02_CLK_LANE_REG1, 0);
+	if (ret)
+		goto error;
+	ret = mipid02_write_reg(bridge, MIPID02_DATA_LANE0_REG1, 0);
+	if (ret)
+		goto error;
+	ret = mipid02_write_reg(bridge, MIPID02_DATA_LANE1_REG1, 0);
+	if (ret)
+		goto error;
+error:
+	if (ret)
+		dev_err(&client->dev, "failed to stream off %d", ret);
+
+	return ret;
+}
+
+static int mipid02_stream_enable(struct mipid02_dev *bridge)
+{
+	struct i2c_client *client = bridge->i2c_client;
+	int ret = -EINVAL;
+
+	if (!bridge->s_subdev)
+		goto error;
+
+	memset(&bridge->r, 0, sizeof(bridge->r));
+	/* build registers content */
+	ret = mipid02_configure_from_rx(bridge);
+	if (ret)
+		goto error;
+	ret = mipid02_configure_from_tx(bridge);
+	if (ret)
+		goto error;
+	ret = mipid02_configure_from_code(bridge);
+	if (ret)
+		goto error;
+
+	/* write mipi registers */
+	ret = mipid02_write_reg(bridge, MIPID02_CLK_LANE_REG1,
+		bridge->r.clk_lane_reg1);
+	if (ret)
+		goto error;
+	ret = mipid02_write_reg(bridge, MIPID02_CLK_LANE_REG3, CLK_MIPI_CSI);
+	if (ret)
+		goto error;
+	ret = mipid02_write_reg(bridge, MIPID02_DATA_LANE0_REG1,
+		bridge->r.data_lane0_reg1);
+	if (ret)
+		goto error;
+	ret = mipid02_write_reg(bridge, MIPID02_DATA_LANE0_REG2,
+		DATA_MIPI_CSI);
+	if (ret)
+		goto error;
+	ret = mipid02_write_reg(bridge, MIPID02_DATA_LANE1_REG1,
+		bridge->r.data_lane1_reg1);
+	if (ret)
+		goto error;
+	ret = mipid02_write_reg(bridge, MIPID02_DATA_LANE1_REG2,
+		DATA_MIPI_CSI);
+	if (ret)
+		goto error;
+	ret = mipid02_write_reg(bridge, MIPID02_MODE_REG1,
+		MODE_NO_BYPASS | bridge->r.mode_reg1);
+	if (ret)
+		goto error;
+	ret = mipid02_write_reg(bridge, MIPID02_MODE_REG2,
+		bridge->r.mode_reg2);
+	if (ret)
+		goto error;
+	ret = mipid02_write_reg(bridge, MIPID02_DATA_ID_RREG,
+		bridge->r.data_id_rreg);
+	if (ret)
+		goto error;
+	ret = mipid02_write_reg(bridge, MIPID02_DATA_SELECTION_CTRL,
+		bridge->r.data_selection_ctrl);
+	if (ret)
+		goto error;
+	ret = mipid02_write_reg(bridge, MIPID02_PIX_WIDTH_CTRL,
+		bridge->r.pix_width_ctrl);
+	if (ret)
+		goto error;
+	ret = mipid02_write_reg(bridge, MIPID02_PIX_WIDTH_CTRL_EMB,
+		bridge->r.pix_width_ctrl_emb);
+	if (ret)
+		goto error;
+
+	ret = v4l2_subdev_call(bridge->s_subdev, video, s_stream, 1);
+	if (ret)
+		goto error;
+
+	return 0;
+
+error:
+	dev_err(&client->dev, "failed to stream on %d", ret);
+	mipid02_stream_disable(bridge);
+
+	return ret;
+}
+
+static int mipid02_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct mipid02_dev *bridge = to_mipid02_dev(sd);
+	struct i2c_client *client = bridge->i2c_client;
+	int ret = 0;
+
+	dev_dbg(&client->dev, "%s : requested %d / current = %d", __func__,
+		    enable, bridge->streaming);
+	mutex_lock(&bridge->lock);
+
+	if (bridge->streaming == enable)
+		goto out;
+
+	ret = enable ? mipid02_stream_enable(bridge) :
+		       mipid02_stream_disable(bridge);
+	if (!ret)
+		bridge->streaming = enable;
+
+out:
+	dev_dbg(&client->dev, "%s current now = %d / %d", __func__,
+		    bridge->streaming, ret);
+	mutex_unlock(&bridge->lock);
+
+	return ret;
+}
+
+static int mipid02_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct mipid02_dev *bridge = to_mipid02_dev(sd);
+	int ret = 0;
+
+	switch (code->pad) {
+	case MIPID02_SINK_0:
+		if (code->index >= ARRAY_SIZE(mipid02_supported_fmt_codes))
+			ret = -EINVAL;
+		else
+			code->code = mipid02_supported_fmt_codes[code->index];
+		break;
+	case MIPID02_SOURCE:
+		if (code->index == 0)
+			code->code = serial_to_parallel_code(bridge->fmt.code);
+		else
+			ret = -EINVAL;
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static int mipid02_get_fmt(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_state *sd_state,
+			   struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *mbus_fmt = &format->format;
+	struct mipid02_dev *bridge = to_mipid02_dev(sd);
+	struct i2c_client *client = bridge->i2c_client;
+	struct v4l2_mbus_framefmt *fmt;
+
+	dev_dbg(&client->dev, "%s probe %d", __func__, format->pad);
+
+	if (format->pad >= MIPID02_PAD_NB)
+		return -EINVAL;
+	/* second CSI-2 pad not yet supported */
+	if (format->pad == MIPID02_SINK_1)
+		return -EINVAL;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+		fmt = v4l2_subdev_get_try_format(&bridge->sd, sd_state,
+						 format->pad);
+	else
+		fmt = &bridge->fmt;
+
+	mutex_lock(&bridge->lock);
+
+	*mbus_fmt = *fmt;
+	/* code may need to be converted for source */
+	if (format->pad == MIPID02_SOURCE)
+		mbus_fmt->code = serial_to_parallel_code(mbus_fmt->code);
+
+	mutex_unlock(&bridge->lock);
+
+	return 0;
+}
+
+static void mipid02_set_fmt_source(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_format *format)
+{
+	struct mipid02_dev *bridge = to_mipid02_dev(sd);
+
+	/* source pad mirror sink pad */
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		format->format = bridge->fmt;
+	else
+		format->format = *v4l2_subdev_get_try_format(sd, sd_state,
+							     MIPID02_SINK_0);
+
+	/* but code may need to be converted */
+	format->format.code = serial_to_parallel_code(format->format.code);
+
+	/* only apply format for V4L2_SUBDEV_FORMAT_TRY case */
+	if (format->which != V4L2_SUBDEV_FORMAT_TRY)
+		return;
+
+	*v4l2_subdev_get_try_format(sd, sd_state, MIPID02_SOURCE) =
+		format->format;
+}
+
+static void mipid02_set_fmt_sink(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_format *format)
+{
+	struct mipid02_dev *bridge = to_mipid02_dev(sd);
+	struct v4l2_subdev_format source_fmt;
+	struct v4l2_mbus_framefmt *fmt;
+
+	format->format.code = get_fmt_code(format->format.code);
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+		fmt = v4l2_subdev_get_try_format(sd, sd_state, format->pad);
+	else
+		fmt = &bridge->fmt;
+
+	*fmt = format->format;
+
+	/*
+	 * Propagate the format change to the source pad, taking
+	 * care not to update the format pointer given back to user
+	 */
+	source_fmt = *format;
+	mipid02_set_fmt_source(sd, sd_state, &source_fmt);
+}
+
+static int mipid02_set_fmt(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_state *sd_state,
+			   struct v4l2_subdev_format *format)
+{
+	struct mipid02_dev *bridge = to_mipid02_dev(sd);
+	struct i2c_client *client = bridge->i2c_client;
+	int ret = 0;
+
+	dev_dbg(&client->dev, "%s for %d", __func__, format->pad);
+
+	if (format->pad >= MIPID02_PAD_NB)
+		return -EINVAL;
+	/* second CSI-2 pad not yet supported */
+	if (format->pad == MIPID02_SINK_1)
+		return -EINVAL;
+
+	mutex_lock(&bridge->lock);
+
+	if (bridge->streaming) {
+		ret = -EBUSY;
+		goto error;
+	}
+
+	if (format->pad == MIPID02_SOURCE)
+		mipid02_set_fmt_source(sd, sd_state, format);
+	else
+		mipid02_set_fmt_sink(sd, sd_state, format);
+
+error:
+	mutex_unlock(&bridge->lock);
+
+	return ret;
+}
+
+static const struct v4l2_subdev_video_ops mipid02_video_ops = {
+	.s_stream = mipid02_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops mipid02_pad_ops = {
+	.enum_mbus_code = mipid02_enum_mbus_code,
+	.get_fmt = mipid02_get_fmt,
+	.set_fmt = mipid02_set_fmt,
+};
+
+static const struct v4l2_subdev_ops mipid02_subdev_ops = {
+	.video = &mipid02_video_ops,
+	.pad = &mipid02_pad_ops,
+};
+
+static const struct media_entity_operations mipid02_subdev_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static int mipid02_async_bound(struct v4l2_async_notifier *notifier,
+			       struct v4l2_subdev *s_subdev,
+			       struct v4l2_async_connection *asd)
+{
+	struct mipid02_dev *bridge = to_mipid02_dev(notifier->sd);
+	struct i2c_client *client = bridge->i2c_client;
+	int source_pad;
+	int ret;
+
+	dev_dbg(&client->dev, "sensor_async_bound call %p", s_subdev);
+
+	source_pad = media_entity_get_fwnode_pad(&s_subdev->entity,
+						 s_subdev->fwnode,
+						 MEDIA_PAD_FL_SOURCE);
+	if (source_pad < 0) {
+		dev_err(&client->dev, "Couldn't find output pad for subdev %s\n",
+			s_subdev->name);
+		return source_pad;
+	}
+
+	ret = media_create_pad_link(&s_subdev->entity, source_pad,
+				    &bridge->sd.entity, 0,
+				    MEDIA_LNK_FL_ENABLED |
+				    MEDIA_LNK_FL_IMMUTABLE);
+	if (ret) {
+		dev_err(&client->dev, "Couldn't create media link %d", ret);
+		return ret;
+	}
+
+	bridge->s_subdev = s_subdev;
+
+	return 0;
+}
+
+static void mipid02_async_unbind(struct v4l2_async_notifier *notifier,
+				 struct v4l2_subdev *s_subdev,
+				 struct v4l2_async_connection *asd)
+{
+	struct mipid02_dev *bridge = to_mipid02_dev(notifier->sd);
+
+	bridge->s_subdev = NULL;
+}
+
+static const struct v4l2_async_notifier_operations mipid02_notifier_ops = {
+	.bound		= mipid02_async_bound,
+	.unbind		= mipid02_async_unbind,
+};
+
+static int mipid02_parse_rx_ep(struct mipid02_dev *bridge)
+{
+	struct v4l2_fwnode_endpoint ep = { .bus_type = V4L2_MBUS_CSI2_DPHY };
+	struct i2c_client *client = bridge->i2c_client;
+	struct v4l2_async_connection *asd;
+	struct device_node *ep_node;
+	int ret;
+
+	/* parse rx (endpoint 0) */
+	ep_node = of_graph_get_endpoint_by_regs(bridge->i2c_client->dev.of_node,
+						0, 0);
+	if (!ep_node) {
+		dev_err(&client->dev, "unable to find port0 ep");
+		ret = -EINVAL;
+		goto error;
+	}
+
+	ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(ep_node), &ep);
+	if (ret) {
+		dev_err(&client->dev, "Could not parse v4l2 endpoint %d\n",
+			ret);
+		goto error_of_node_put;
+	}
+
+	/* do some sanity checks */
+	if (ep.bus.mipi_csi2.num_data_lanes > 2) {
+		dev_err(&client->dev, "max supported data lanes is 2 / got %d",
+			ep.bus.mipi_csi2.num_data_lanes);
+		ret = -EINVAL;
+		goto error_of_node_put;
+	}
+
+	/* register it for later use */
+	bridge->rx = ep;
+
+	/* register async notifier so we get noticed when sensor is connected */
+	v4l2_async_subdev_nf_init(&bridge->notifier, &bridge->sd);
+	asd = v4l2_async_nf_add_fwnode_remote(&bridge->notifier,
+					      of_fwnode_handle(ep_node),
+					      struct v4l2_async_connection);
+	of_node_put(ep_node);
+
+	if (IS_ERR(asd)) {
+		dev_err(&client->dev, "fail to register asd to notifier %ld",
+			PTR_ERR(asd));
+		return PTR_ERR(asd);
+	}
+	bridge->notifier.ops = &mipid02_notifier_ops;
+
+	ret = v4l2_async_nf_register(&bridge->notifier);
+	if (ret)
+		v4l2_async_nf_cleanup(&bridge->notifier);
+
+	return ret;
+
+error_of_node_put:
+	of_node_put(ep_node);
+error:
+
+	return ret;
+}
+
+static int mipid02_parse_tx_ep(struct mipid02_dev *bridge)
+{
+	struct v4l2_fwnode_endpoint ep = { .bus_type = V4L2_MBUS_PARALLEL };
+	struct i2c_client *client = bridge->i2c_client;
+	struct device_node *ep_node;
+	int ret;
+
+	/* parse tx (endpoint 2) */
+	ep_node = of_graph_get_endpoint_by_regs(bridge->i2c_client->dev.of_node,
+						2, 0);
+	if (!ep_node) {
+		dev_err(&client->dev, "unable to find port1 ep");
+		ret = -EINVAL;
+		goto error;
+	}
+
+	ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(ep_node), &ep);
+	if (ret) {
+		dev_err(&client->dev, "Could not parse v4l2 endpoint\n");
+		goto error_of_node_put;
+	}
+
+	of_node_put(ep_node);
+	bridge->tx = ep;
+
+	return 0;
+
+error_of_node_put:
+	of_node_put(ep_node);
+error:
+
+	return -EINVAL;
+}
+
+static int mipid02_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct mipid02_dev *bridge;
+	u32 clk_freq;
+	int ret;
+
+	bridge = devm_kzalloc(dev, sizeof(*bridge), GFP_KERNEL);
+	if (!bridge)
+		return -ENOMEM;
+
+	init_format(&bridge->fmt);
+
+	bridge->i2c_client = client;
+	v4l2_i2c_subdev_init(&bridge->sd, client, &mipid02_subdev_ops);
+
+	/* got and check clock */
+	bridge->xclk = devm_clk_get(dev, "xclk");
+	if (IS_ERR(bridge->xclk)) {
+		dev_err(dev, "failed to get xclk\n");
+		return PTR_ERR(bridge->xclk);
+	}
+
+	clk_freq = clk_get_rate(bridge->xclk);
+	if (clk_freq < 6000000 || clk_freq > 27000000) {
+		dev_err(dev, "xclk freq must be in 6-27 Mhz range. got %d Hz\n",
+			clk_freq);
+		return -EINVAL;
+	}
+
+	bridge->reset_gpio = devm_gpiod_get_optional(dev, "reset",
+						     GPIOD_OUT_HIGH);
+
+	if (IS_ERR(bridge->reset_gpio)) {
+		dev_err(dev, "failed to get reset GPIO\n");
+		return PTR_ERR(bridge->reset_gpio);
+	}
+
+	ret = mipid02_get_regulators(bridge);
+	if (ret) {
+		dev_err(dev, "failed to get regulators %d", ret);
+		return ret;
+	}
+
+	mutex_init(&bridge->lock);
+	bridge->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	bridge->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
+	bridge->sd.entity.ops = &mipid02_subdev_entity_ops;
+	bridge->pad[0].flags = MEDIA_PAD_FL_SINK;
+	bridge->pad[1].flags = MEDIA_PAD_FL_SINK;
+	bridge->pad[2].flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&bridge->sd.entity, MIPID02_PAD_NB,
+				     bridge->pad);
+	if (ret) {
+		dev_err(&client->dev, "pads init failed %d", ret);
+		goto mutex_cleanup;
+	}
+
+	/* enable clock, power and reset device if available */
+	ret = mipid02_set_power_on(bridge);
+	if (ret)
+		goto entity_cleanup;
+
+	ret = mipid02_detect(bridge);
+	if (ret) {
+		dev_err(&client->dev, "failed to detect mipid02 %d", ret);
+		goto power_off;
+	}
+
+	ret = mipid02_parse_tx_ep(bridge);
+	if (ret) {
+		dev_err(&client->dev, "failed to parse tx %d", ret);
+		goto power_off;
+	}
+
+	ret = mipid02_parse_rx_ep(bridge);
+	if (ret) {
+		dev_err(&client->dev, "failed to parse rx %d", ret);
+		goto power_off;
+	}
+
+	ret = v4l2_async_register_subdev(&bridge->sd);
+	if (ret < 0) {
+		dev_err(&client->dev, "v4l2_async_register_subdev failed %d",
+			    ret);
+		goto unregister_notifier;
+	}
+
+	dev_info(&client->dev, "mipid02 device probe successfully");
+
+	return 0;
+
+unregister_notifier:
+	v4l2_async_nf_unregister(&bridge->notifier);
+	v4l2_async_nf_cleanup(&bridge->notifier);
+power_off:
+	mipid02_set_power_off(bridge);
+entity_cleanup:
+	media_entity_cleanup(&bridge->sd.entity);
+mutex_cleanup:
+	mutex_destroy(&bridge->lock);
+
+	return ret;
+}
+
+static void mipid02_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct mipid02_dev *bridge = to_mipid02_dev(sd);
+
+	v4l2_async_nf_unregister(&bridge->notifier);
+	v4l2_async_nf_cleanup(&bridge->notifier);
+	v4l2_async_unregister_subdev(&bridge->sd);
+	mipid02_set_power_off(bridge);
+	media_entity_cleanup(&bridge->sd.entity);
+	mutex_destroy(&bridge->lock);
+}
+
+static const struct of_device_id mipid02_dt_ids[] = {
+	{ .compatible = "st,st-mipid02" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, mipid02_dt_ids);
+
+static struct i2c_driver mipid02_i2c_driver = {
+	.driver = {
+		.name  = "st-mipid02",
+		.of_match_table = mipid02_dt_ids,
+	},
+	.probe = mipid02_probe,
+	.remove = mipid02_remove,
+};
+
+module_i2c_driver(mipid02_i2c_driver);
+
+MODULE_AUTHOR("Mickael Guene <mickael.guene@st.com>");
+MODULE_DESCRIPTION("STMicroelectronics MIPID02 CSI-2 bridge driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/st-vgxy61.c b/drivers/media/i2c/st-vgxy61.c
new file mode 100644
index 000000000..30f82ca34
--- /dev/null
+++ b/drivers/media/i2c/st-vgxy61.c
@@ -0,0 +1,1964 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Driver for VGXY61 global shutter sensor family driver
+ *
+ * Copyright (C) 2022 STMicroelectronics SA
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/iopoll.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <linux/regulator/consumer.h>
+#include <linux/units.h>
+
+#include <asm/unaligned.h>
+
+#include <media/mipi-csi2.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+#define VGXY61_REG_8BIT(n)				((1 << 16) | (n))
+#define VGXY61_REG_16BIT(n)				((2 << 16) | (n))
+#define VGXY61_REG_32BIT(n)				((4 << 16) | (n))
+#define VGXY61_REG_SIZE_SHIFT				16
+#define VGXY61_REG_ADDR_MASK				0xffff
+
+#define VGXY61_REG_MODEL_ID				VGXY61_REG_16BIT(0x0000)
+#define VG5661_MODEL_ID					0x5661
+#define VG5761_MODEL_ID					0x5761
+#define VGXY61_REG_REVISION				VGXY61_REG_16BIT(0x0002)
+#define VGXY61_REG_FWPATCH_REVISION			VGXY61_REG_16BIT(0x0014)
+#define VGXY61_REG_FWPATCH_START_ADDR			VGXY61_REG_8BIT(0x2000)
+#define VGXY61_REG_SYSTEM_FSM				VGXY61_REG_8BIT(0x0020)
+#define VGXY61_SYSTEM_FSM_SW_STBY			0x03
+#define VGXY61_SYSTEM_FSM_STREAMING			0x04
+#define VGXY61_REG_NVM					VGXY61_REG_8BIT(0x0023)
+#define VGXY61_NVM_OK					0x04
+#define VGXY61_REG_STBY					VGXY61_REG_8BIT(0x0201)
+#define VGXY61_STBY_NO_REQ				0
+#define VGXY61_STBY_REQ_TMP_READ			BIT(2)
+#define VGXY61_REG_STREAMING				VGXY61_REG_8BIT(0x0202)
+#define VGXY61_STREAMING_NO_REQ				0
+#define VGXY61_STREAMING_REQ_STOP			BIT(0)
+#define VGXY61_STREAMING_REQ_START			BIT(1)
+#define VGXY61_REG_EXT_CLOCK				VGXY61_REG_32BIT(0x0220)
+#define VGXY61_REG_CLK_PLL_PREDIV			VGXY61_REG_8BIT(0x0224)
+#define VGXY61_REG_CLK_SYS_PLL_MULT			VGXY61_REG_8BIT(0x0225)
+#define VGXY61_REG_GPIO_0_CTRL				VGXY61_REG_8BIT(0x0236)
+#define VGXY61_REG_GPIO_1_CTRL				VGXY61_REG_8BIT(0x0237)
+#define VGXY61_REG_GPIO_2_CTRL				VGXY61_REG_8BIT(0x0238)
+#define VGXY61_REG_GPIO_3_CTRL				VGXY61_REG_8BIT(0x0239)
+#define VGXY61_REG_SIGNALS_POLARITY_CTRL		VGXY61_REG_8BIT(0x023b)
+#define VGXY61_REG_LINE_LENGTH				VGXY61_REG_16BIT(0x0300)
+#define VGXY61_REG_ORIENTATION				VGXY61_REG_8BIT(0x0302)
+#define VGXY61_REG_VT_CTRL				VGXY61_REG_8BIT(0x0304)
+#define VGXY61_REG_FORMAT_CTRL				VGXY61_REG_8BIT(0x0305)
+#define VGXY61_REG_OIF_CTRL				VGXY61_REG_16BIT(0x0306)
+#define VGXY61_REG_OIF_ROI0_CTRL			VGXY61_REG_8BIT(0x030a)
+#define VGXY61_REG_ROI0_START_H				VGXY61_REG_16BIT(0x0400)
+#define VGXY61_REG_ROI0_START_V				VGXY61_REG_16BIT(0x0402)
+#define VGXY61_REG_ROI0_END_H				VGXY61_REG_16BIT(0x0404)
+#define VGXY61_REG_ROI0_END_V				VGXY61_REG_16BIT(0x0406)
+#define VGXY61_REG_PATGEN_CTRL				VGXY61_REG_32BIT(0x0440)
+#define VGXY61_PATGEN_LONG_ENABLE			BIT(16)
+#define VGXY61_PATGEN_SHORT_ENABLE			BIT(0)
+#define VGXY61_PATGEN_LONG_TYPE_SHIFT			18
+#define VGXY61_PATGEN_SHORT_TYPE_SHIFT			4
+#define VGXY61_REG_FRAME_CONTENT_CTRL			VGXY61_REG_8BIT(0x0478)
+#define VGXY61_REG_COARSE_EXPOSURE_LONG			VGXY61_REG_16BIT(0x0500)
+#define VGXY61_REG_COARSE_EXPOSURE_SHORT		VGXY61_REG_16BIT(0x0504)
+#define VGXY61_REG_ANALOG_GAIN				VGXY61_REG_8BIT(0x0508)
+#define VGXY61_REG_DIGITAL_GAIN_LONG			VGXY61_REG_16BIT(0x050a)
+#define VGXY61_REG_DIGITAL_GAIN_SHORT			VGXY61_REG_16BIT(0x0512)
+#define VGXY61_REG_FRAME_LENGTH				VGXY61_REG_16BIT(0x051a)
+#define VGXY61_REG_SIGNALS_CTRL				VGXY61_REG_16BIT(0x0522)
+#define VGXY61_SIGNALS_GPIO_ID_SHIFT			4
+#define VGXY61_REG_READOUT_CTRL				VGXY61_REG_8BIT(0x0530)
+#define VGXY61_REG_HDR_CTRL				VGXY61_REG_8BIT(0x0532)
+#define VGXY61_REG_PATGEN_LONG_DATA_GR			VGXY61_REG_16BIT(0x092c)
+#define VGXY61_REG_PATGEN_LONG_DATA_R			VGXY61_REG_16BIT(0x092e)
+#define VGXY61_REG_PATGEN_LONG_DATA_B			VGXY61_REG_16BIT(0x0930)
+#define VGXY61_REG_PATGEN_LONG_DATA_GB			VGXY61_REG_16BIT(0x0932)
+#define VGXY61_REG_PATGEN_SHORT_DATA_GR			VGXY61_REG_16BIT(0x0950)
+#define VGXY61_REG_PATGEN_SHORT_DATA_R			VGXY61_REG_16BIT(0x0952)
+#define VGXY61_REG_PATGEN_SHORT_DATA_B			VGXY61_REG_16BIT(0x0954)
+#define VGXY61_REG_PATGEN_SHORT_DATA_GB			VGXY61_REG_16BIT(0x0956)
+#define VGXY61_REG_BYPASS_CTRL				VGXY61_REG_8BIT(0x0a60)
+
+#define VGX661_WIDTH					1464
+#define VGX661_HEIGHT					1104
+#define VGX761_WIDTH					1944
+#define VGX761_HEIGHT					1204
+#define VGX661_DEFAULT_MODE				1
+#define VGX761_DEFAULT_MODE				1
+#define VGX661_SHORT_ROT_TERM				93
+#define VGX761_SHORT_ROT_TERM				90
+#define VGXY61_EXPOS_ROT_TERM				66
+#define VGXY61_WRITE_MULTIPLE_CHUNK_MAX			16
+#define VGXY61_NB_GPIOS					4
+#define VGXY61_NB_POLARITIES				5
+#define VGXY61_FRAME_LENGTH_DEF				1313
+#define VGXY61_MIN_FRAME_LENGTH				1288
+#define VGXY61_MIN_EXPOSURE				10
+#define VGXY61_HDR_LINEAR_RATIO				10
+#define VGXY61_TIMEOUT_MS				500
+#define VGXY61_MEDIA_BUS_FMT_DEF			MEDIA_BUS_FMT_Y8_1X8
+
+#define VGXY61_FWPATCH_REVISION_MAJOR			2
+#define VGXY61_FWPATCH_REVISION_MINOR			0
+#define VGXY61_FWPATCH_REVISION_MICRO			5
+
+static const u8 patch_array[] = {
+	0xbf, 0x00, 0x05, 0x20, 0x06, 0x01, 0xe0, 0xe0, 0x04, 0x80, 0xe6, 0x45,
+	0xed, 0x6f, 0xfe, 0xff, 0x14, 0x80, 0x1f, 0x84, 0x10, 0x42, 0x05, 0x7c,
+	0x01, 0xc4, 0x1e, 0x80, 0xb6, 0x42, 0x00, 0xe0, 0x1e, 0x82, 0x1e, 0xc0,
+	0x93, 0xdd, 0xc3, 0xc1, 0x0c, 0x04, 0x00, 0xfa, 0x86, 0x0d, 0x70, 0xe1,
+	0x04, 0x98, 0x15, 0x00, 0x28, 0xe0, 0x14, 0x02, 0x08, 0xfc, 0x15, 0x40,
+	0x28, 0xe0, 0x98, 0x58, 0xe0, 0xef, 0x04, 0x98, 0x0e, 0x04, 0x00, 0xf0,
+	0x15, 0x00, 0x28, 0xe0, 0x19, 0xc8, 0x15, 0x40, 0x28, 0xe0, 0xc6, 0x41,
+	0xfc, 0xe0, 0x14, 0x80, 0x1f, 0x84, 0x14, 0x02, 0xa0, 0xfc, 0x1e, 0x80,
+	0x14, 0x80, 0x14, 0x02, 0x80, 0xfb, 0x14, 0x02, 0xe0, 0xfc, 0x1e, 0x80,
+	0x14, 0xc0, 0x1f, 0x84, 0x14, 0x02, 0xa4, 0xfc, 0x1e, 0xc0, 0x14, 0xc0,
+	0x14, 0x02, 0x80, 0xfb, 0x14, 0x02, 0xe4, 0xfc, 0x1e, 0xc0, 0x0c, 0x0c,
+	0x00, 0xf2, 0x93, 0xdd, 0x86, 0x00, 0xf8, 0xe0, 0x04, 0x80, 0xc6, 0x03,
+	0x70, 0xe1, 0x0e, 0x84, 0x93, 0xdd, 0xc3, 0xc1, 0x0c, 0x04, 0x00, 0xfa,
+	0x6b, 0x80, 0x06, 0x40, 0x6c, 0xe1, 0x04, 0x80, 0x09, 0x00, 0xe0, 0xe0,
+	0x0b, 0xa1, 0x95, 0x84, 0x05, 0x0c, 0x1c, 0xe0, 0x86, 0x02, 0xf9, 0x60,
+	0xe0, 0xcf, 0x78, 0x6e, 0x80, 0xef, 0x25, 0x0c, 0x18, 0xe0, 0x05, 0x4c,
+	0x1c, 0xe0, 0x86, 0x02, 0xf9, 0x60, 0xe0, 0xcf, 0x0b, 0x84, 0xd8, 0x6d,
+	0x80, 0xef, 0x05, 0x4c, 0x18, 0xe0, 0x04, 0xd8, 0x0b, 0xa5, 0x95, 0x84,
+	0x05, 0x0c, 0x2c, 0xe0, 0x06, 0x02, 0x01, 0x60, 0xe0, 0xce, 0x18, 0x6d,
+	0x80, 0xef, 0x25, 0x0c, 0x30, 0xe0, 0x05, 0x4c, 0x2c, 0xe0, 0x06, 0x02,
+	0x01, 0x60, 0xe0, 0xce, 0x0b, 0x84, 0x78, 0x6c, 0x80, 0xef, 0x05, 0x4c,
+	0x30, 0xe0, 0x0c, 0x0c, 0x00, 0xf2, 0x93, 0xdd, 0x46, 0x01, 0x70, 0xe1,
+	0x08, 0x80, 0x0b, 0xa1, 0x08, 0x5c, 0x00, 0xda, 0x06, 0x01, 0x68, 0xe1,
+	0x04, 0x80, 0x4a, 0x40, 0x84, 0xe0, 0x08, 0x5c, 0x00, 0x9a, 0x06, 0x01,
+	0xe0, 0xe0, 0x04, 0x80, 0x15, 0x00, 0x60, 0xe0, 0x19, 0xc4, 0x15, 0x40,
+	0x60, 0xe0, 0x15, 0x00, 0x78, 0xe0, 0x19, 0xc4, 0x15, 0x40, 0x78, 0xe0,
+	0x93, 0xdd, 0xc3, 0xc1, 0x46, 0x01, 0x70, 0xe1, 0x08, 0x80, 0x0b, 0xa1,
+	0x08, 0x5c, 0x00, 0xda, 0x06, 0x01, 0x68, 0xe1, 0x04, 0x80, 0x4a, 0x40,
+	0x84, 0xe0, 0x08, 0x5c, 0x00, 0x9a, 0x06, 0x01, 0xe0, 0xe0, 0x14, 0x80,
+	0x25, 0x02, 0x54, 0xe0, 0x29, 0xc4, 0x25, 0x42, 0x54, 0xe0, 0x24, 0x80,
+	0x35, 0x04, 0x6c, 0xe0, 0x39, 0xc4, 0x35, 0x44, 0x6c, 0xe0, 0x25, 0x02,
+	0x64, 0xe0, 0x29, 0xc4, 0x25, 0x42, 0x64, 0xe0, 0x04, 0x80, 0x15, 0x00,
+	0x7c, 0xe0, 0x19, 0xc4, 0x15, 0x40, 0x7c, 0xe0, 0x93, 0xdd, 0xc3, 0xc1,
+	0x4c, 0x04, 0x7c, 0xfa, 0x86, 0x40, 0x98, 0xe0, 0x14, 0x80, 0x1b, 0xa1,
+	0x06, 0x00, 0x00, 0xc0, 0x08, 0x42, 0x38, 0xdc, 0x08, 0x64, 0xa0, 0xef,
+	0x86, 0x42, 0x3c, 0xe0, 0x68, 0x49, 0x80, 0xef, 0x6b, 0x80, 0x78, 0x53,
+	0xc8, 0xef, 0xc6, 0x54, 0x6c, 0xe1, 0x7b, 0x80, 0xb5, 0x14, 0x0c, 0xf8,
+	0x05, 0x14, 0x14, 0xf8, 0x1a, 0xac, 0x8a, 0x80, 0x0b, 0x90, 0x38, 0x55,
+	0x80, 0xef, 0x1a, 0xae, 0x17, 0xc2, 0x03, 0x82, 0x88, 0x65, 0x80, 0xef,
+	0x1b, 0x80, 0x0b, 0x8e, 0x68, 0x65, 0x80, 0xef, 0x9b, 0x80, 0x0b, 0x8c,
+	0x08, 0x65, 0x80, 0xef, 0x6b, 0x80, 0x0b, 0x92, 0x1b, 0x8c, 0x98, 0x64,
+	0x80, 0xef, 0x1a, 0xec, 0x9b, 0x80, 0x0b, 0x90, 0x95, 0x54, 0x10, 0xe0,
+	0xa8, 0x53, 0x80, 0xef, 0x1a, 0xee, 0x17, 0xc2, 0x03, 0x82, 0xf8, 0x63,
+	0x80, 0xef, 0x1b, 0x80, 0x0b, 0x8e, 0xd8, 0x63, 0x80, 0xef, 0x1b, 0x8c,
+	0x68, 0x63, 0x80, 0xef, 0x6b, 0x80, 0x0b, 0x92, 0x65, 0x54, 0x14, 0xe0,
+	0x08, 0x65, 0x84, 0xef, 0x68, 0x63, 0x80, 0xef, 0x7b, 0x80, 0x0b, 0x8c,
+	0xa8, 0x64, 0x84, 0xef, 0x08, 0x63, 0x80, 0xef, 0x14, 0xe8, 0x46, 0x44,
+	0x94, 0xe1, 0x24, 0x88, 0x4a, 0x4e, 0x04, 0xe0, 0x14, 0xea, 0x1a, 0x04,
+	0x08, 0xe0, 0x0a, 0x40, 0x84, 0xed, 0x0c, 0x04, 0x00, 0xe2, 0x4a, 0x40,
+	0x04, 0xe0, 0x19, 0x16, 0xc0, 0xe0, 0x0a, 0x40, 0x84, 0xed, 0x21, 0x54,
+	0x60, 0xe0, 0x0c, 0x04, 0x00, 0xe2, 0x1b, 0xa5, 0x0e, 0xea, 0x01, 0x89,
+	0x21, 0x54, 0x64, 0xe0, 0x7e, 0xe8, 0x65, 0x82, 0x1b, 0xa7, 0x26, 0x00,
+	0x00, 0x80, 0xa5, 0x82, 0x1b, 0xa9, 0x65, 0x82, 0x1b, 0xa3, 0x01, 0x85,
+	0x16, 0x00, 0x00, 0xc0, 0x01, 0x54, 0x04, 0xf8, 0x06, 0xaa, 0x01, 0x83,
+	0x06, 0xa8, 0x65, 0x81, 0x06, 0xa8, 0x01, 0x54, 0x04, 0xf8, 0x01, 0x83,
+	0x06, 0xaa, 0x09, 0x14, 0x18, 0xf8, 0x0b, 0xa1, 0x05, 0x84, 0xc6, 0x42,
+	0xd4, 0xe0, 0x14, 0x84, 0x01, 0x83, 0x01, 0x54, 0x60, 0xe0, 0x01, 0x54,
+	0x64, 0xe0, 0x0b, 0x02, 0x90, 0xe0, 0x10, 0x02, 0x90, 0xe5, 0x01, 0x54,
+	0x88, 0xe0, 0xb5, 0x81, 0xc6, 0x40, 0xd4, 0xe0, 0x14, 0x80, 0x0b, 0x02,
+	0xe0, 0xe4, 0x10, 0x02, 0x31, 0x66, 0x02, 0xc0, 0x01, 0x54, 0x88, 0xe0,
+	0x1a, 0x84, 0x29, 0x14, 0x10, 0xe0, 0x1c, 0xaa, 0x2b, 0xa1, 0xf5, 0x82,
+	0x25, 0x14, 0x10, 0xf8, 0x2b, 0x04, 0xa8, 0xe0, 0x20, 0x44, 0x0d, 0x70,
+	0x03, 0xc0, 0x2b, 0xa1, 0x04, 0x00, 0x80, 0x9a, 0x02, 0x40, 0x84, 0x90,
+	0x03, 0x54, 0x04, 0x80, 0x4c, 0x0c, 0x7c, 0xf2, 0x93, 0xdd, 0x00, 0x00,
+	0x02, 0xa9, 0x00, 0x00, 0x64, 0x4a, 0x40, 0x00, 0x08, 0x2d, 0x58, 0xe0,
+	0xa8, 0x98, 0x40, 0x00, 0x28, 0x07, 0x34, 0xe0, 0x05, 0xb9, 0x00, 0x00,
+	0x28, 0x00, 0x41, 0x05, 0x88, 0x00, 0x41, 0x3c, 0x98, 0x00, 0x41, 0x52,
+	0x04, 0x01, 0x41, 0x79, 0x3c, 0x01, 0x41, 0x6a, 0x3d, 0xfe, 0x00, 0x00,
+};
+
+static const char * const vgxy61_test_pattern_menu[] = {
+	"Disabled",
+	"Solid",
+	"Colorbar",
+	"Gradbar",
+	"Hgrey",
+	"Vgrey",
+	"Dgrey",
+	"PN28",
+};
+
+static const char * const vgxy61_hdr_mode_menu[] = {
+	"HDR linearize",
+	"HDR substraction",
+	"No HDR",
+};
+
+static const char * const vgxy61_supply_name[] = {
+	"VCORE",
+	"VDDIO",
+	"VANA",
+};
+
+static const s64 link_freq[] = {
+	/*
+	 * MIPI output freq is 804Mhz / 2, as it uses both rising edge and
+	 * falling edges to send data
+	 */
+	402000000ULL
+};
+
+enum vgxy61_bin_mode {
+	VGXY61_BIN_MODE_NORMAL,
+	VGXY61_BIN_MODE_DIGITAL_X2,
+	VGXY61_BIN_MODE_DIGITAL_X4,
+};
+
+enum vgxy61_hdr_mode {
+	VGXY61_HDR_LINEAR,
+	VGXY61_HDR_SUB,
+	VGXY61_NO_HDR,
+};
+
+enum vgxy61_strobe_mode {
+	VGXY61_STROBE_DISABLED,
+	VGXY61_STROBE_LONG,
+	VGXY61_STROBE_ENABLED,
+};
+
+struct vgxy61_mode_info {
+	u32 width;
+	u32 height;
+	enum vgxy61_bin_mode bin_mode;
+	struct v4l2_rect crop;
+};
+
+struct vgxy61_fmt_desc {
+	u32 code;
+	u8 bpp;
+	u8 data_type;
+};
+
+static const struct vgxy61_fmt_desc vgxy61_supported_codes[] = {
+	{
+		.code = MEDIA_BUS_FMT_Y8_1X8,
+		.bpp = 8,
+		.data_type = MIPI_CSI2_DT_RAW8,
+	},
+	{
+		.code = MEDIA_BUS_FMT_Y10_1X10,
+		.bpp = 10,
+		.data_type = MIPI_CSI2_DT_RAW10,
+	},
+	{
+		.code = MEDIA_BUS_FMT_Y12_1X12,
+		.bpp = 12,
+		.data_type = MIPI_CSI2_DT_RAW12,
+	},
+	{
+		.code = MEDIA_BUS_FMT_Y14_1X14,
+		.bpp = 14,
+		.data_type = MIPI_CSI2_DT_RAW14,
+	},
+	{
+		.code = MEDIA_BUS_FMT_Y16_1X16,
+		.bpp = 16,
+		.data_type = MIPI_CSI2_DT_RAW16,
+	},
+};
+
+static const struct vgxy61_mode_info vgx661_mode_data[] = {
+	{
+		.width = VGX661_WIDTH,
+		.height = VGX661_HEIGHT,
+		.bin_mode = VGXY61_BIN_MODE_NORMAL,
+		.crop = {
+			.left = 0,
+			.top = 0,
+			.width = VGX661_WIDTH,
+			.height = VGX661_HEIGHT,
+		},
+	},
+	{
+		.width = 1280,
+		.height = 720,
+		.bin_mode = VGXY61_BIN_MODE_NORMAL,
+		.crop = {
+			.left = 92,
+			.top = 192,
+			.width = 1280,
+			.height = 720,
+		},
+	},
+	{
+		.width = 640,
+		.height = 480,
+		.bin_mode = VGXY61_BIN_MODE_DIGITAL_X2,
+		.crop = {
+			.left = 92,
+			.top = 72,
+			.width = 1280,
+			.height = 960,
+		},
+	},
+	{
+		.width = 320,
+		.height = 240,
+		.bin_mode = VGXY61_BIN_MODE_DIGITAL_X4,
+		.crop = {
+			.left = 92,
+			.top = 72,
+			.width = 1280,
+			.height = 960,
+		},
+	},
+};
+
+static const struct vgxy61_mode_info vgx761_mode_data[] = {
+	{
+		.width = VGX761_WIDTH,
+		.height = VGX761_HEIGHT,
+		.bin_mode = VGXY61_BIN_MODE_NORMAL,
+		.crop = {
+			.left = 0,
+			.top = 0,
+			.width = VGX761_WIDTH,
+			.height = VGX761_HEIGHT,
+		},
+	},
+	{
+		.width = 1920,
+		.height = 1080,
+		.bin_mode = VGXY61_BIN_MODE_NORMAL,
+		.crop = {
+			.left = 12,
+			.top = 62,
+			.width = 1920,
+			.height = 1080,
+		},
+	},
+	{
+		.width = 1280,
+		.height = 720,
+		.bin_mode = VGXY61_BIN_MODE_NORMAL,
+		.crop = {
+			.left = 332,
+			.top = 242,
+			.width = 1280,
+			.height = 720,
+		},
+	},
+	{
+		.width = 640,
+		.height = 480,
+		.bin_mode = VGXY61_BIN_MODE_DIGITAL_X2,
+		.crop = {
+			.left = 332,
+			.top = 122,
+			.width = 1280,
+			.height = 960,
+		},
+	},
+	{
+		.width = 320,
+		.height = 240,
+		.bin_mode = VGXY61_BIN_MODE_DIGITAL_X4,
+		.crop = {
+			.left = 332,
+			.top = 122,
+			.width = 1280,
+			.height = 960,
+		},
+	},
+};
+
+struct vgxy61_dev {
+	struct i2c_client *i2c_client;
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct regulator_bulk_data supplies[ARRAY_SIZE(vgxy61_supply_name)];
+	struct gpio_desc *reset_gpio;
+	struct clk *xclk;
+	u32 clk_freq;
+	u16 id;
+	u16 sensor_width;
+	u16 sensor_height;
+	u16 oif_ctrl;
+	unsigned int nb_of_lane;
+	u32 data_rate_in_mbps;
+	u32 pclk;
+	u16 line_length;
+	u16 rot_term;
+	bool gpios_polarity;
+	/* Lock to protect all members below */
+	struct mutex lock;
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_ctrl *pixel_rate_ctrl;
+	struct v4l2_ctrl *expo_ctrl;
+	struct v4l2_ctrl *vblank_ctrl;
+	struct v4l2_ctrl *vflip_ctrl;
+	struct v4l2_ctrl *hflip_ctrl;
+	bool streaming;
+	struct v4l2_mbus_framefmt fmt;
+	const struct vgxy61_mode_info *sensor_modes;
+	unsigned int sensor_modes_nb;
+	const struct vgxy61_mode_info *default_mode;
+	const struct vgxy61_mode_info *current_mode;
+	bool hflip;
+	bool vflip;
+	enum vgxy61_hdr_mode hdr;
+	u16 expo_long;
+	u16 expo_short;
+	u16 expo_max;
+	u16 expo_min;
+	u16 vblank;
+	u16 vblank_min;
+	u16 frame_length;
+	u16 digital_gain;
+	u8 analog_gain;
+	enum vgxy61_strobe_mode strobe_mode;
+	u32 pattern;
+};
+
+static u8 get_bpp_by_code(__u32 code)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(vgxy61_supported_codes); i++) {
+		if (vgxy61_supported_codes[i].code == code)
+			return vgxy61_supported_codes[i].bpp;
+	}
+	/* Should never happen */
+	WARN(1, "Unsupported code %d. default to 8 bpp", code);
+	return 8;
+}
+
+static u8 get_data_type_by_code(__u32 code)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(vgxy61_supported_codes); i++) {
+		if (vgxy61_supported_codes[i].code == code)
+			return vgxy61_supported_codes[i].data_type;
+	}
+	/* Should never happen */
+	WARN(1, "Unsupported code %d. default to MIPI_CSI2_DT_RAW8 data type",
+	     code);
+	return MIPI_CSI2_DT_RAW8;
+}
+
+static void compute_pll_parameters_by_freq(u32 freq, u8 *prediv, u8 *mult)
+{
+	const unsigned int predivs[] = {1, 2, 4};
+	unsigned int i;
+
+	/*
+	 * Freq range is [6Mhz-27Mhz] already checked.
+	 * Output of divider should be in [6Mhz-12Mhz[.
+	 */
+	for (i = 0; i < ARRAY_SIZE(predivs); i++) {
+		*prediv = predivs[i];
+		if (freq / *prediv < 12 * HZ_PER_MHZ)
+			break;
+	}
+	WARN_ON(i == ARRAY_SIZE(predivs));
+
+	/*
+	 * Target freq is 804Mhz. Don't change this as it will impact image
+	 * quality.
+	 */
+	*mult = ((804 * HZ_PER_MHZ) * (*prediv) + freq / 2) / freq;
+}
+
+static s32 get_pixel_rate(struct vgxy61_dev *sensor)
+{
+	return div64_u64((u64)sensor->data_rate_in_mbps * sensor->nb_of_lane,
+			 get_bpp_by_code(sensor->fmt.code));
+}
+
+static inline struct vgxy61_dev *to_vgxy61_dev(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct vgxy61_dev, sd);
+}
+
+static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct vgxy61_dev,
+			     ctrl_handler)->sd;
+}
+
+static unsigned int get_chunk_size(struct vgxy61_dev *sensor)
+{
+	struct i2c_adapter *adapter = sensor->i2c_client->adapter;
+	int max_write_len = VGXY61_WRITE_MULTIPLE_CHUNK_MAX;
+
+	if (adapter->quirks && adapter->quirks->max_write_len)
+		max_write_len = adapter->quirks->max_write_len - 2;
+
+	max_write_len = min(max_write_len, VGXY61_WRITE_MULTIPLE_CHUNK_MAX);
+
+	return max(max_write_len, 1);
+}
+
+static int vgxy61_read_multiple(struct vgxy61_dev *sensor, u32 reg,
+				unsigned int len)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	struct i2c_msg msg[2];
+	u8 buf[2];
+	u8 val[sizeof(u32)] = {0};
+	int ret;
+
+	if (len > sizeof(u32))
+		return -EINVAL;
+	buf[0] = reg >> 8;
+	buf[1] = reg & 0xff;
+
+	msg[0].addr = client->addr;
+	msg[0].flags = client->flags;
+	msg[0].buf = buf;
+	msg[0].len = sizeof(buf);
+
+	msg[1].addr = client->addr;
+	msg[1].flags = client->flags | I2C_M_RD;
+	msg[1].buf = val;
+	msg[1].len = len;
+
+	ret = i2c_transfer(client->adapter, msg, 2);
+	if (ret < 0) {
+		dev_dbg(&client->dev, "%s: %x i2c_transfer, reg: %x => %d\n",
+			__func__, client->addr, reg, ret);
+		return ret;
+	}
+
+	return get_unaligned_le32(val);
+}
+
+static inline int vgxy61_read_reg(struct vgxy61_dev *sensor, u32 reg)
+{
+	return vgxy61_read_multiple(sensor, reg & VGXY61_REG_ADDR_MASK,
+				     (reg >> VGXY61_REG_SIZE_SHIFT) & 7);
+}
+
+static int vgxy61_write_multiple(struct vgxy61_dev *sensor, u32 reg,
+				 const u8 *data, unsigned int len, int *err)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	struct i2c_msg msg;
+	u8 buf[VGXY61_WRITE_MULTIPLE_CHUNK_MAX + 2];
+	unsigned int i;
+	int ret;
+
+	if (err && *err)
+		return *err;
+
+	if (len > VGXY61_WRITE_MULTIPLE_CHUNK_MAX)
+		return -EINVAL;
+	buf[0] = reg >> 8;
+	buf[1] = reg & 0xff;
+	for (i = 0; i < len; i++)
+		buf[i + 2] = data[i];
+
+	msg.addr = client->addr;
+	msg.flags = client->flags;
+	msg.buf = buf;
+	msg.len = len + 2;
+
+	ret = i2c_transfer(client->adapter, &msg, 1);
+	if (ret < 0) {
+		dev_dbg(&client->dev, "%s: i2c_transfer, reg: %x => %d\n",
+			__func__, reg, ret);
+		if (err)
+			*err = ret;
+		return ret;
+	}
+
+	return 0;
+}
+
+static int vgxy61_write_array(struct vgxy61_dev *sensor, u32 reg,
+			      unsigned int nb, const u8 *array)
+{
+	const unsigned int chunk_size = get_chunk_size(sensor);
+	int ret;
+	unsigned int sz;
+
+	while (nb) {
+		sz = min(nb, chunk_size);
+		ret = vgxy61_write_multiple(sensor, reg, array, sz, NULL);
+		if (ret < 0)
+			return ret;
+		nb -= sz;
+		reg += sz;
+		array += sz;
+	}
+
+	return 0;
+}
+
+static inline int vgxy61_write_reg(struct vgxy61_dev *sensor, u32 reg, u32 val,
+				   int *err)
+{
+	return vgxy61_write_multiple(sensor, reg & VGXY61_REG_ADDR_MASK,
+				     (u8 *)&val,
+				     (reg >> VGXY61_REG_SIZE_SHIFT) & 7, err);
+}
+
+static int vgxy61_poll_reg(struct vgxy61_dev *sensor, u32 reg, u8 poll_val,
+			   unsigned int timeout_ms)
+{
+	const unsigned int loop_delay_ms = 10;
+	int ret;
+
+	return read_poll_timeout(vgxy61_read_reg, ret,
+				 ((ret < 0) || (ret == poll_val)),
+				 loop_delay_ms * 1000, timeout_ms * 1000,
+				 false, sensor, reg);
+}
+
+static int vgxy61_wait_state(struct vgxy61_dev *sensor, int state,
+			     unsigned int timeout_ms)
+{
+	return vgxy61_poll_reg(sensor, VGXY61_REG_SYSTEM_FSM, state,
+			       timeout_ms);
+}
+
+static int vgxy61_check_bw(struct vgxy61_dev *sensor)
+{
+	/*
+	 * Simplification of time needed to send short packets and for the MIPI
+	 * to add transition times (EoT, LPS, and SoT packet delimiters) needed
+	 * by the protocol to go in low power between 2 packets of data. This
+	 * is a mipi IP constant for the sensor.
+	 */
+	const unsigned int mipi_margin = 1056;
+	unsigned int binning_scale = sensor->current_mode->crop.height /
+				     sensor->current_mode->height;
+	u8 bpp = get_bpp_by_code(sensor->fmt.code);
+	unsigned int max_bit_per_line;
+	unsigned int bit_per_line;
+	u64 line_rate;
+
+	line_rate = sensor->nb_of_lane * (u64)sensor->data_rate_in_mbps *
+		    sensor->line_length;
+	max_bit_per_line = div64_u64(line_rate, sensor->pclk) - mipi_margin;
+	bit_per_line = (bpp * sensor->current_mode->width) / binning_scale;
+
+	return bit_per_line > max_bit_per_line ? -EINVAL : 0;
+}
+
+static int vgxy61_apply_exposure(struct vgxy61_dev *sensor)
+{
+	int ret = 0;
+
+	 /* We first set expo to zero to avoid forbidden parameters couple */
+	vgxy61_write_reg(sensor, VGXY61_REG_COARSE_EXPOSURE_SHORT, 0, &ret);
+	vgxy61_write_reg(sensor, VGXY61_REG_COARSE_EXPOSURE_LONG,
+			 sensor->expo_long, &ret);
+	vgxy61_write_reg(sensor, VGXY61_REG_COARSE_EXPOSURE_SHORT,
+			 sensor->expo_short, &ret);
+
+	return ret;
+}
+
+static int vgxy61_get_regulators(struct vgxy61_dev *sensor)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(vgxy61_supply_name); i++)
+		sensor->supplies[i].supply = vgxy61_supply_name[i];
+
+	return devm_regulator_bulk_get(&sensor->i2c_client->dev,
+				       ARRAY_SIZE(vgxy61_supply_name),
+				       sensor->supplies);
+}
+
+static int vgxy61_apply_reset(struct vgxy61_dev *sensor)
+{
+	gpiod_set_value_cansleep(sensor->reset_gpio, 0);
+	usleep_range(5000, 10000);
+	gpiod_set_value_cansleep(sensor->reset_gpio, 1);
+	usleep_range(5000, 10000);
+	gpiod_set_value_cansleep(sensor->reset_gpio, 0);
+	usleep_range(40000, 100000);
+	return vgxy61_wait_state(sensor, VGXY61_SYSTEM_FSM_SW_STBY,
+				 VGXY61_TIMEOUT_MS);
+}
+
+static void vgxy61_fill_framefmt(struct vgxy61_dev *sensor,
+				 const struct vgxy61_mode_info *mode,
+				 struct v4l2_mbus_framefmt *fmt, u32 code)
+{
+	fmt->code = code;
+	fmt->width = mode->width;
+	fmt->height = mode->height;
+	fmt->colorspace = V4L2_COLORSPACE_RAW;
+	fmt->field = V4L2_FIELD_NONE;
+	fmt->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+	fmt->quantization = V4L2_QUANTIZATION_DEFAULT;
+	fmt->xfer_func = V4L2_XFER_FUNC_DEFAULT;
+}
+
+static int vgxy61_try_fmt_internal(struct v4l2_subdev *sd,
+				   struct v4l2_mbus_framefmt *fmt,
+				   const struct vgxy61_mode_info **new_mode)
+{
+	struct vgxy61_dev *sensor = to_vgxy61_dev(sd);
+	const struct vgxy61_mode_info *mode = sensor->sensor_modes;
+	unsigned int index;
+
+	for (index = 0; index < ARRAY_SIZE(vgxy61_supported_codes); index++) {
+		if (vgxy61_supported_codes[index].code == fmt->code)
+			break;
+	}
+	if (index == ARRAY_SIZE(vgxy61_supported_codes))
+		index = 0;
+
+	mode = v4l2_find_nearest_size(sensor->sensor_modes,
+				      sensor->sensor_modes_nb, width, height,
+				      fmt->width, fmt->height);
+	if (new_mode)
+		*new_mode = mode;
+
+	vgxy61_fill_framefmt(sensor, mode, fmt,
+			     vgxy61_supported_codes[index].code);
+
+	return 0;
+}
+
+static int vgxy61_get_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *sd_state,
+				struct v4l2_subdev_selection *sel)
+{
+	struct vgxy61_dev *sensor = to_vgxy61_dev(sd);
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		sel->r = sensor->current_mode->crop;
+		return 0;
+	case V4L2_SEL_TGT_NATIVE_SIZE:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.top = 0;
+		sel->r.left = 0;
+		sel->r.width = sensor->sensor_width;
+		sel->r.height = sensor->sensor_height;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int vgxy61_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index >= ARRAY_SIZE(vgxy61_supported_codes))
+		return -EINVAL;
+
+	code->code = vgxy61_supported_codes[code->index].code;
+
+	return 0;
+}
+
+static int vgxy61_get_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *format)
+{
+	struct vgxy61_dev *sensor = to_vgxy61_dev(sd);
+	struct v4l2_mbus_framefmt *fmt;
+
+	mutex_lock(&sensor->lock);
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+		fmt = v4l2_subdev_get_try_format(&sensor->sd, sd_state,
+						 format->pad);
+	else
+		fmt = &sensor->fmt;
+
+	format->format = *fmt;
+
+	mutex_unlock(&sensor->lock);
+
+	return 0;
+}
+
+static u16 vgxy61_get_vblank_min(struct vgxy61_dev *sensor,
+				 enum vgxy61_hdr_mode hdr)
+{
+	u16 min_vblank =  VGXY61_MIN_FRAME_LENGTH -
+			  sensor->current_mode->crop.height;
+	/* Ensure the first rule of thumb can't be negative */
+	u16 min_vblank_hdr =  VGXY61_MIN_EXPOSURE + sensor->rot_term + 1;
+
+	if (hdr != VGXY61_NO_HDR)
+		return max(min_vblank, min_vblank_hdr);
+	return min_vblank;
+}
+
+static int vgxy61_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct vgxy61_dev *sensor = to_vgxy61_dev(sd);
+
+	if (fse->index >= sensor->sensor_modes_nb)
+		return -EINVAL;
+
+	fse->min_width = sensor->sensor_modes[fse->index].width;
+	fse->max_width = fse->min_width;
+	fse->min_height = sensor->sensor_modes[fse->index].height;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static int vgxy61_update_analog_gain(struct vgxy61_dev *sensor, u32 target)
+{
+	sensor->analog_gain = target;
+
+	if (sensor->streaming)
+		return vgxy61_write_reg(sensor, VGXY61_REG_ANALOG_GAIN, target,
+					NULL);
+	return 0;
+}
+
+static int vgxy61_apply_digital_gain(struct vgxy61_dev *sensor,
+				     u32 digital_gain)
+{
+	int ret = 0;
+
+	/*
+	 * For a monochrome version, configuring DIGITAL_GAIN_LONG_CH0 and
+	 * DIGITAL_GAIN_SHORT_CH0 is enough to configure the gain of all
+	 * four sub pixels.
+	 */
+	vgxy61_write_reg(sensor, VGXY61_REG_DIGITAL_GAIN_LONG, digital_gain,
+			 &ret);
+	vgxy61_write_reg(sensor, VGXY61_REG_DIGITAL_GAIN_SHORT, digital_gain,
+			 &ret);
+
+	return ret;
+}
+
+static int vgxy61_update_digital_gain(struct vgxy61_dev *sensor, u32 target)
+{
+	sensor->digital_gain = target;
+
+	if (sensor->streaming)
+		return vgxy61_apply_digital_gain(sensor, sensor->digital_gain);
+	return 0;
+}
+
+static int vgxy61_apply_patgen(struct vgxy61_dev *sensor, u32 index)
+{
+	static const u8 index2val[] = {
+		0x0, 0x1, 0x2, 0x3, 0x10, 0x11, 0x12, 0x13
+	};
+	u32 pattern = index2val[index];
+	u32 reg = (pattern << VGXY61_PATGEN_LONG_TYPE_SHIFT) |
+	      (pattern << VGXY61_PATGEN_SHORT_TYPE_SHIFT);
+
+	if (pattern)
+		reg |= VGXY61_PATGEN_LONG_ENABLE | VGXY61_PATGEN_SHORT_ENABLE;
+	return vgxy61_write_reg(sensor, VGXY61_REG_PATGEN_CTRL, reg, NULL);
+}
+
+static int vgxy61_update_patgen(struct vgxy61_dev *sensor, u32 pattern)
+{
+	sensor->pattern = pattern;
+
+	if (sensor->streaming)
+		return vgxy61_apply_patgen(sensor, sensor->pattern);
+	return 0;
+}
+
+static int vgxy61_apply_gpiox_strobe_mode(struct vgxy61_dev *sensor,
+					  enum vgxy61_strobe_mode mode,
+					  unsigned int idx)
+{
+	static const u8 index2val[] = {0x0, 0x1, 0x3};
+	int reg;
+
+	reg = vgxy61_read_reg(sensor, VGXY61_REG_SIGNALS_CTRL);
+	if (reg < 0)
+		return reg;
+	reg &= ~(0xf << (idx * VGXY61_SIGNALS_GPIO_ID_SHIFT));
+	reg |= index2val[mode] << (idx * VGXY61_SIGNALS_GPIO_ID_SHIFT);
+
+	return vgxy61_write_reg(sensor, VGXY61_REG_SIGNALS_CTRL, reg, NULL);
+}
+
+static int vgxy61_update_gpios_strobe_mode(struct vgxy61_dev *sensor,
+					   enum vgxy61_hdr_mode hdr)
+{
+	unsigned int i;
+	int ret;
+
+	switch (hdr) {
+	case VGXY61_HDR_LINEAR:
+		sensor->strobe_mode = VGXY61_STROBE_ENABLED;
+		break;
+	case VGXY61_HDR_SUB:
+	case VGXY61_NO_HDR:
+		sensor->strobe_mode = VGXY61_STROBE_LONG;
+		break;
+	default:
+		/* Should never happen */
+		WARN_ON(true);
+		break;
+	}
+
+	if (!sensor->streaming)
+		return 0;
+
+	for (i = 0; i < VGXY61_NB_GPIOS; i++) {
+		ret = vgxy61_apply_gpiox_strobe_mode(sensor,
+						     sensor->strobe_mode,
+						     i);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int vgxy61_update_gpios_strobe_polarity(struct vgxy61_dev *sensor,
+					       bool polarity)
+{
+	int ret = 0;
+
+	if (sensor->streaming)
+		return -EBUSY;
+
+	vgxy61_write_reg(sensor, VGXY61_REG_GPIO_0_CTRL, polarity << 1, &ret);
+	vgxy61_write_reg(sensor, VGXY61_REG_GPIO_1_CTRL, polarity << 1, &ret);
+	vgxy61_write_reg(sensor, VGXY61_REG_GPIO_2_CTRL, polarity << 1, &ret);
+	vgxy61_write_reg(sensor, VGXY61_REG_GPIO_3_CTRL, polarity << 1, &ret);
+	vgxy61_write_reg(sensor, VGXY61_REG_SIGNALS_POLARITY_CTRL, polarity,
+			 &ret);
+
+	return ret;
+}
+
+static u32 vgxy61_get_expo_long_max(struct vgxy61_dev *sensor,
+				    unsigned int short_expo_ratio)
+{
+	u32 first_rot_max_expo, second_rot_max_expo, third_rot_max_expo;
+
+	/* Apply sensor's rules of thumb */
+	/*
+	 * Short exposure + height must be less than frame length to avoid bad
+	 * pixel line at the botom of the image
+	 */
+	first_rot_max_expo =
+		((sensor->frame_length - sensor->current_mode->crop.height -
+		sensor->rot_term) * short_expo_ratio) - 1;
+
+	/*
+	 * Total exposition time must be less than frame length to avoid sensor
+	 * crash
+	 */
+	second_rot_max_expo =
+		(((sensor->frame_length - VGXY61_EXPOS_ROT_TERM) *
+		short_expo_ratio) / (short_expo_ratio + 1)) - 1;
+
+	/*
+	 * Short exposure times 71 must be less than frame length to avoid
+	 * sensor crash
+	 */
+	third_rot_max_expo = (sensor->frame_length / 71) * short_expo_ratio;
+
+	/* Take the minimum from all rules */
+	return min(min(first_rot_max_expo, second_rot_max_expo),
+		   third_rot_max_expo);
+}
+
+static int vgxy61_update_exposure(struct vgxy61_dev *sensor, u16 new_expo_long,
+				  enum vgxy61_hdr_mode hdr)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	u16 new_expo_short = 0;
+	u16 expo_short_max = 0;
+	u16 expo_long_min = VGXY61_MIN_EXPOSURE;
+	u16 expo_long_max = 0;
+
+	/* Compute short exposure according to hdr mode and long exposure */
+	switch (hdr) {
+	case VGXY61_HDR_LINEAR:
+		/*
+		 * Take ratio into account for minimal exposures in
+		 * VGXY61_HDR_LINEAR
+		 */
+		expo_long_min = VGXY61_MIN_EXPOSURE * VGXY61_HDR_LINEAR_RATIO;
+		new_expo_long = max(expo_long_min, new_expo_long);
+
+		expo_long_max =
+			vgxy61_get_expo_long_max(sensor,
+						 VGXY61_HDR_LINEAR_RATIO);
+		expo_short_max = (expo_long_max +
+				 (VGXY61_HDR_LINEAR_RATIO / 2)) /
+				 VGXY61_HDR_LINEAR_RATIO;
+		new_expo_short = (new_expo_long +
+				 (VGXY61_HDR_LINEAR_RATIO / 2)) /
+				 VGXY61_HDR_LINEAR_RATIO;
+		break;
+	case VGXY61_HDR_SUB:
+		new_expo_long = max(expo_long_min, new_expo_long);
+
+		expo_long_max = vgxy61_get_expo_long_max(sensor, 1);
+		/* Short and long are the same in VGXY61_HDR_SUB */
+		expo_short_max = expo_long_max;
+		new_expo_short = new_expo_long;
+		break;
+	case VGXY61_NO_HDR:
+		new_expo_long = max(expo_long_min, new_expo_long);
+
+		/*
+		 * As short expo is 0 here, only the second rule of thumb
+		 * applies, see vgxy61_get_expo_long_max for more
+		 */
+		expo_long_max = sensor->frame_length - VGXY61_EXPOS_ROT_TERM;
+		break;
+	default:
+		/* Should never happen */
+		WARN_ON(true);
+		break;
+	}
+
+	/* If this happens, something is wrong with formulas */
+	WARN_ON(expo_long_min > expo_long_max);
+
+	if (new_expo_long > expo_long_max) {
+		dev_warn(&client->dev, "Exposure %d too high, clamping to %d\n",
+			 new_expo_long, expo_long_max);
+		new_expo_long = expo_long_max;
+		new_expo_short = expo_short_max;
+	}
+
+	sensor->expo_long = new_expo_long;
+	sensor->expo_short = new_expo_short;
+	sensor->expo_max = expo_long_max;
+	sensor->expo_min = expo_long_min;
+
+	if (sensor->streaming)
+		return vgxy61_apply_exposure(sensor);
+	return 0;
+}
+
+static int vgxy61_apply_framelength(struct vgxy61_dev *sensor)
+{
+	return vgxy61_write_reg(sensor, VGXY61_REG_FRAME_LENGTH,
+				sensor->frame_length, NULL);
+}
+
+static int vgxy61_update_vblank(struct vgxy61_dev *sensor, u16 vblank,
+				enum vgxy61_hdr_mode hdr)
+{
+	int ret;
+
+	sensor->vblank_min = vgxy61_get_vblank_min(sensor, hdr);
+	sensor->vblank = max(sensor->vblank_min, vblank);
+	sensor->frame_length = sensor->current_mode->crop.height +
+			       sensor->vblank;
+
+	/* Update exposure according to vblank */
+	ret = vgxy61_update_exposure(sensor, sensor->expo_long, hdr);
+	if (ret)
+		return ret;
+
+	if (sensor->streaming)
+		return vgxy61_apply_framelength(sensor);
+	return 0;
+}
+
+static int vgxy61_apply_hdr(struct vgxy61_dev *sensor,
+			    enum vgxy61_hdr_mode index)
+{
+	static const u8 index2val[] = {0x1, 0x4, 0xa};
+
+	return vgxy61_write_reg(sensor, VGXY61_REG_HDR_CTRL, index2val[index],
+				NULL);
+}
+
+static int vgxy61_update_hdr(struct vgxy61_dev *sensor,
+			     enum vgxy61_hdr_mode index)
+{
+	int ret;
+
+	/*
+	 * vblank and short exposure change according to HDR mode, do it first
+	 * as it can violate sensors 'rule of thumbs' and therefore will require
+	 * to change the long exposure.
+	 */
+	ret = vgxy61_update_vblank(sensor, sensor->vblank, index);
+	if (ret)
+		return ret;
+
+	/* Update strobe mode according to HDR */
+	ret = vgxy61_update_gpios_strobe_mode(sensor, index);
+	if (ret)
+		return ret;
+
+	sensor->hdr = index;
+
+	if (sensor->streaming)
+		return vgxy61_apply_hdr(sensor, sensor->hdr);
+	return 0;
+}
+
+static int vgxy61_apply_settings(struct vgxy61_dev *sensor)
+{
+	int ret;
+	unsigned int i;
+
+	ret = vgxy61_apply_hdr(sensor, sensor->hdr);
+	if (ret)
+		return ret;
+
+	ret = vgxy61_apply_framelength(sensor);
+	if (ret)
+		return ret;
+
+	ret = vgxy61_apply_exposure(sensor);
+	if (ret)
+		return ret;
+
+	ret = vgxy61_write_reg(sensor, VGXY61_REG_ANALOG_GAIN,
+			       sensor->analog_gain, NULL);
+	if (ret)
+		return ret;
+	ret = vgxy61_apply_digital_gain(sensor, sensor->digital_gain);
+	if (ret)
+		return ret;
+
+	ret = vgxy61_write_reg(sensor, VGXY61_REG_ORIENTATION,
+			       sensor->hflip | (sensor->vflip << 1), NULL);
+	if (ret)
+		return ret;
+
+	ret = vgxy61_apply_patgen(sensor, sensor->pattern);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < VGXY61_NB_GPIOS; i++) {
+		ret = vgxy61_apply_gpiox_strobe_mode(sensor,
+						     sensor->strobe_mode, i);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int vgxy61_stream_enable(struct vgxy61_dev *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->sd);
+	const struct v4l2_rect *crop = &sensor->current_mode->crop;
+	int ret = 0;
+
+	ret = vgxy61_check_bw(sensor);
+	if (ret)
+		return ret;
+
+	ret = pm_runtime_get_sync(&client->dev);
+	if (ret < 0) {
+		pm_runtime_put_autosuspend(&client->dev);
+		return ret;
+	}
+
+	/* pm_runtime_get_sync() can return 1 as a valid return code */
+	ret = 0;
+
+	vgxy61_write_reg(sensor, VGXY61_REG_FORMAT_CTRL,
+			 get_bpp_by_code(sensor->fmt.code), &ret);
+	vgxy61_write_reg(sensor, VGXY61_REG_OIF_ROI0_CTRL,
+			 get_data_type_by_code(sensor->fmt.code), &ret);
+
+	vgxy61_write_reg(sensor, VGXY61_REG_READOUT_CTRL,
+			 sensor->current_mode->bin_mode, &ret);
+	vgxy61_write_reg(sensor, VGXY61_REG_ROI0_START_H, crop->left, &ret);
+	vgxy61_write_reg(sensor, VGXY61_REG_ROI0_END_H,
+			 crop->left + crop->width - 1, &ret);
+	vgxy61_write_reg(sensor, VGXY61_REG_ROI0_START_V, crop->top, &ret);
+	vgxy61_write_reg(sensor, VGXY61_REG_ROI0_END_V,
+			 crop->top + crop->height - 1, &ret);
+	if (ret)
+		goto err_rpm_put;
+
+	ret = vgxy61_apply_settings(sensor);
+	if (ret)
+		goto err_rpm_put;
+
+	ret = vgxy61_write_reg(sensor, VGXY61_REG_STREAMING,
+			       VGXY61_STREAMING_REQ_START, NULL);
+	if (ret)
+		goto err_rpm_put;
+
+	ret = vgxy61_poll_reg(sensor, VGXY61_REG_STREAMING,
+			      VGXY61_STREAMING_NO_REQ, VGXY61_TIMEOUT_MS);
+	if (ret)
+		goto err_rpm_put;
+
+	ret = vgxy61_wait_state(sensor, VGXY61_SYSTEM_FSM_STREAMING,
+				VGXY61_TIMEOUT_MS);
+	if (ret)
+		goto err_rpm_put;
+
+	/* vflip and hflip cannot change during streaming */
+	__v4l2_ctrl_grab(sensor->vflip_ctrl, true);
+	__v4l2_ctrl_grab(sensor->hflip_ctrl, true);
+
+	return 0;
+
+err_rpm_put:
+	pm_runtime_put(&client->dev);
+	return ret;
+}
+
+static int vgxy61_stream_disable(struct vgxy61_dev *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->sd);
+	int ret;
+
+	ret = vgxy61_write_reg(sensor, VGXY61_REG_STREAMING,
+			       VGXY61_STREAMING_REQ_STOP, NULL);
+	if (ret)
+		goto err_str_dis;
+
+	ret = vgxy61_poll_reg(sensor, VGXY61_REG_STREAMING,
+			      VGXY61_STREAMING_NO_REQ, 2000);
+	if (ret)
+		goto err_str_dis;
+
+	ret = vgxy61_wait_state(sensor, VGXY61_SYSTEM_FSM_SW_STBY,
+				VGXY61_TIMEOUT_MS);
+	if (ret)
+		goto err_str_dis;
+
+	__v4l2_ctrl_grab(sensor->vflip_ctrl, false);
+	__v4l2_ctrl_grab(sensor->hflip_ctrl, false);
+
+err_str_dis:
+	if (ret)
+		WARN(1, "Can't disable stream");
+	pm_runtime_put(&client->dev);
+
+	return ret;
+}
+
+static int vgxy61_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct vgxy61_dev *sensor = to_vgxy61_dev(sd);
+	int ret = 0;
+
+	mutex_lock(&sensor->lock);
+
+	ret = enable ? vgxy61_stream_enable(sensor) :
+	      vgxy61_stream_disable(sensor);
+	if (!ret)
+		sensor->streaming = enable;
+
+	mutex_unlock(&sensor->lock);
+
+	return ret;
+}
+
+static int vgxy61_set_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *format)
+{
+	struct vgxy61_dev *sensor = to_vgxy61_dev(sd);
+	const struct vgxy61_mode_info *new_mode;
+	struct v4l2_mbus_framefmt *fmt;
+	int ret;
+
+	mutex_lock(&sensor->lock);
+
+	if (sensor->streaming) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	ret = vgxy61_try_fmt_internal(sd, &format->format, &new_mode);
+	if (ret)
+		goto out;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		fmt = v4l2_subdev_get_try_format(sd, sd_state, 0);
+		*fmt = format->format;
+	} else if (sensor->current_mode != new_mode ||
+		   sensor->fmt.code != format->format.code) {
+		fmt = &sensor->fmt;
+		*fmt = format->format;
+
+		sensor->current_mode = new_mode;
+
+		/* Reset vblank and framelength to default */
+		ret = vgxy61_update_vblank(sensor,
+					   VGXY61_FRAME_LENGTH_DEF -
+					   new_mode->crop.height,
+					   sensor->hdr);
+
+		/* Update controls to reflect new mode */
+		__v4l2_ctrl_s_ctrl_int64(sensor->pixel_rate_ctrl,
+					 get_pixel_rate(sensor));
+		__v4l2_ctrl_modify_range(sensor->vblank_ctrl,
+					 sensor->vblank_min,
+					 0xffff - new_mode->crop.height,
+					 1, sensor->vblank);
+		__v4l2_ctrl_s_ctrl(sensor->vblank_ctrl, sensor->vblank);
+		__v4l2_ctrl_modify_range(sensor->expo_ctrl, sensor->expo_min,
+					 sensor->expo_max, 1,
+					 sensor->expo_long);
+	}
+
+out:
+	mutex_unlock(&sensor->lock);
+
+	return ret;
+}
+
+static int vgxy61_init_cfg(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_state *sd_state)
+{
+	struct vgxy61_dev *sensor = to_vgxy61_dev(sd);
+	struct v4l2_subdev_format fmt = { 0 };
+
+	vgxy61_fill_framefmt(sensor, sensor->current_mode, &fmt.format,
+			     VGXY61_MEDIA_BUS_FMT_DEF);
+
+	return vgxy61_set_fmt(sd, sd_state, &fmt);
+}
+
+static int vgxy61_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
+	struct vgxy61_dev *sensor = to_vgxy61_dev(sd);
+	const struct vgxy61_mode_info *cur_mode = sensor->current_mode;
+	int ret;
+
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE:
+		ret = vgxy61_update_exposure(sensor, ctrl->val, sensor->hdr);
+		ctrl->val = sensor->expo_long;
+		break;
+	case V4L2_CID_ANALOGUE_GAIN:
+		ret = vgxy61_update_analog_gain(sensor, ctrl->val);
+		break;
+	case V4L2_CID_DIGITAL_GAIN:
+		ret = vgxy61_update_digital_gain(sensor, ctrl->val);
+		break;
+	case V4L2_CID_VFLIP:
+	case V4L2_CID_HFLIP:
+		if (sensor->streaming) {
+			ret = -EBUSY;
+			break;
+		}
+		if (ctrl->id == V4L2_CID_VFLIP)
+			sensor->vflip = ctrl->val;
+		if (ctrl->id == V4L2_CID_HFLIP)
+			sensor->hflip = ctrl->val;
+		ret = 0;
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		ret = vgxy61_update_patgen(sensor, ctrl->val);
+		break;
+	case V4L2_CID_HDR_SENSOR_MODE:
+		ret = vgxy61_update_hdr(sensor, ctrl->val);
+		/* Update vblank and exposure controls to match new hdr */
+		__v4l2_ctrl_modify_range(sensor->vblank_ctrl,
+					 sensor->vblank_min,
+					 0xffff - cur_mode->crop.height,
+					 1, sensor->vblank);
+		__v4l2_ctrl_modify_range(sensor->expo_ctrl, sensor->expo_min,
+					 sensor->expo_max, 1,
+					 sensor->expo_long);
+		break;
+	case V4L2_CID_VBLANK:
+		ret = vgxy61_update_vblank(sensor, ctrl->val, sensor->hdr);
+		/* Update exposure control to match new vblank */
+		__v4l2_ctrl_modify_range(sensor->expo_ctrl, sensor->expo_min,
+					 sensor->expo_max, 1,
+					 sensor->expo_long);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops vgxy61_ctrl_ops = {
+	.s_ctrl = vgxy61_s_ctrl,
+};
+
+static int vgxy61_init_controls(struct vgxy61_dev *sensor)
+{
+	const struct v4l2_ctrl_ops *ops = &vgxy61_ctrl_ops;
+	struct v4l2_ctrl_handler *hdl = &sensor->ctrl_handler;
+	const struct vgxy61_mode_info *cur_mode = sensor->current_mode;
+	struct v4l2_ctrl *ctrl;
+	int ret;
+
+	v4l2_ctrl_handler_init(hdl, 16);
+	/* We can use our own mutex for the ctrl lock */
+	hdl->lock = &sensor->lock;
+	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_ANALOGUE_GAIN, 0, 0x1c, 1,
+			  sensor->analog_gain);
+	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_DIGITAL_GAIN, 0, 0xfff, 1,
+			  sensor->digital_gain);
+	v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(vgxy61_test_pattern_menu) - 1,
+				     0, 0, vgxy61_test_pattern_menu);
+	ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HBLANK, 0,
+				 sensor->line_length, 1,
+				 sensor->line_length - cur_mode->width);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+	ctrl = v4l2_ctrl_new_int_menu(hdl, ops, V4L2_CID_LINK_FREQ,
+				      ARRAY_SIZE(link_freq) - 1, 0, link_freq);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+	v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_HDR_SENSOR_MODE,
+				     ARRAY_SIZE(vgxy61_hdr_mode_menu) - 1, 0,
+				     VGXY61_NO_HDR, vgxy61_hdr_mode_menu);
+
+	/*
+	 * Keep a pointer to these controls as we need to update them when
+	 * setting the format
+	 */
+	sensor->pixel_rate_ctrl = v4l2_ctrl_new_std(hdl, ops,
+						    V4L2_CID_PIXEL_RATE, 1,
+						    INT_MAX, 1,
+						    get_pixel_rate(sensor));
+	if (sensor->pixel_rate_ctrl)
+		sensor->pixel_rate_ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+	sensor->expo_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
+					      sensor->expo_min,
+					      sensor->expo_max, 1,
+					      sensor->expo_long);
+	sensor->vblank_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VBLANK,
+						sensor->vblank_min,
+						0xffff - cur_mode->crop.height,
+						1, sensor->vblank);
+	sensor->vflip_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP,
+					       0, 1, 1, sensor->vflip);
+	sensor->hflip_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP,
+					       0, 1, 1, sensor->hflip);
+
+	if (hdl->error) {
+		ret = hdl->error;
+		goto free_ctrls;
+	}
+
+	sensor->sd.ctrl_handler = hdl;
+	return 0;
+
+free_ctrls:
+	v4l2_ctrl_handler_free(hdl);
+	return ret;
+}
+
+static const struct v4l2_subdev_video_ops vgxy61_video_ops = {
+	.s_stream = vgxy61_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops vgxy61_pad_ops = {
+	.init_cfg = vgxy61_init_cfg,
+	.enum_mbus_code = vgxy61_enum_mbus_code,
+	.get_fmt = vgxy61_get_fmt,
+	.set_fmt = vgxy61_set_fmt,
+	.get_selection = vgxy61_get_selection,
+	.enum_frame_size = vgxy61_enum_frame_size,
+};
+
+static const struct v4l2_subdev_ops vgxy61_subdev_ops = {
+	.video = &vgxy61_video_ops,
+	.pad = &vgxy61_pad_ops,
+};
+
+static const struct media_entity_operations vgxy61_subdev_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static int vgxy61_tx_from_ep(struct vgxy61_dev *sensor,
+			     struct fwnode_handle *handle)
+{
+	struct v4l2_fwnode_endpoint ep = { .bus_type = V4L2_MBUS_CSI2_DPHY };
+	struct i2c_client *client = sensor->i2c_client;
+	u32 log2phy[VGXY61_NB_POLARITIES] = {~0, ~0, ~0, ~0, ~0};
+	u32 phy2log[VGXY61_NB_POLARITIES] = {~0, ~0, ~0, ~0, ~0};
+	int polarities[VGXY61_NB_POLARITIES] = {0, 0, 0, 0, 0};
+	int l_nb;
+	unsigned int p, l, i;
+	int ret;
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(handle, &ep);
+	if (ret)
+		return -EINVAL;
+
+	l_nb = ep.bus.mipi_csi2.num_data_lanes;
+	if (l_nb != 1 && l_nb != 2 && l_nb != 4) {
+		dev_err(&client->dev, "invalid data lane number %d\n", l_nb);
+		goto error_ep;
+	}
+
+	/* Build log2phy, phy2log and polarities from ep info */
+	log2phy[0] = ep.bus.mipi_csi2.clock_lane;
+	phy2log[log2phy[0]] = 0;
+	for (l = 1; l < l_nb + 1; l++) {
+		log2phy[l] = ep.bus.mipi_csi2.data_lanes[l - 1];
+		phy2log[log2phy[l]] = l;
+	}
+	/*
+	 * Then fill remaining slots for every physical slot to have something
+	 * valid for hardware stuff.
+	 */
+	for (p = 0; p < VGXY61_NB_POLARITIES; p++) {
+		if (phy2log[p] != ~0)
+			continue;
+		phy2log[p] = l;
+		log2phy[l] = p;
+		l++;
+	}
+	for (l = 0; l < l_nb + 1; l++)
+		polarities[l] = ep.bus.mipi_csi2.lane_polarities[l];
+
+	if (log2phy[0] != 0) {
+		dev_err(&client->dev, "clk lane must be map to physical lane 0\n");
+		goto error_ep;
+	}
+	sensor->oif_ctrl = (polarities[4] << 15) + ((phy2log[4] - 1) << 13) +
+			   (polarities[3] << 12) + ((phy2log[3] - 1) << 10) +
+			   (polarities[2] <<  9) + ((phy2log[2] - 1) <<  7) +
+			   (polarities[1] <<  6) + ((phy2log[1] - 1) <<  4) +
+			   (polarities[0] <<  3) +
+			   l_nb;
+	sensor->nb_of_lane = l_nb;
+
+	dev_dbg(&client->dev, "tx uses %d lanes", l_nb);
+	for (i = 0; i < VGXY61_NB_POLARITIES; i++) {
+		dev_dbg(&client->dev, "log2phy[%d] = %d\n", i, log2phy[i]);
+		dev_dbg(&client->dev, "phy2log[%d] = %d\n", i, phy2log[i]);
+		dev_dbg(&client->dev, "polarity[%d] = %d\n", i, polarities[i]);
+	}
+	dev_dbg(&client->dev, "oif_ctrl = 0x%04x\n", sensor->oif_ctrl);
+
+	v4l2_fwnode_endpoint_free(&ep);
+
+	return 0;
+
+error_ep:
+	v4l2_fwnode_endpoint_free(&ep);
+
+	return -EINVAL;
+}
+
+static int vgxy61_configure(struct vgxy61_dev *sensor)
+{
+	u32 sensor_freq;
+	u8 prediv, mult;
+	int line_length;
+	int ret = 0;
+
+	compute_pll_parameters_by_freq(sensor->clk_freq, &prediv, &mult);
+	sensor_freq = (mult * sensor->clk_freq) / prediv;
+	/* Frequency to data rate is 1:1 ratio for MIPI */
+	sensor->data_rate_in_mbps = sensor_freq;
+	/* Video timing ISP path (pixel clock)  requires 804/5 mhz = 160 mhz */
+	sensor->pclk = sensor_freq / 5;
+
+	line_length = vgxy61_read_reg(sensor, VGXY61_REG_LINE_LENGTH);
+	if (line_length < 0)
+		return line_length;
+	sensor->line_length = line_length;
+	vgxy61_write_reg(sensor, VGXY61_REG_EXT_CLOCK, sensor->clk_freq, &ret);
+	vgxy61_write_reg(sensor, VGXY61_REG_CLK_PLL_PREDIV, prediv, &ret);
+	vgxy61_write_reg(sensor, VGXY61_REG_CLK_SYS_PLL_MULT, mult, &ret);
+	vgxy61_write_reg(sensor, VGXY61_REG_OIF_CTRL, sensor->oif_ctrl, &ret);
+	vgxy61_write_reg(sensor, VGXY61_REG_FRAME_CONTENT_CTRL, 0, &ret);
+	vgxy61_write_reg(sensor, VGXY61_REG_BYPASS_CTRL, 4, &ret);
+	if (ret)
+		return ret;
+	vgxy61_update_gpios_strobe_polarity(sensor, sensor->gpios_polarity);
+	/* Set pattern generator solid to middle value */
+	vgxy61_write_reg(sensor, VGXY61_REG_PATGEN_LONG_DATA_GR, 0x800, &ret);
+	vgxy61_write_reg(sensor, VGXY61_REG_PATGEN_LONG_DATA_R, 0x800, &ret);
+	vgxy61_write_reg(sensor, VGXY61_REG_PATGEN_LONG_DATA_B, 0x800, &ret);
+	vgxy61_write_reg(sensor, VGXY61_REG_PATGEN_LONG_DATA_GB, 0x800, &ret);
+	vgxy61_write_reg(sensor, VGXY61_REG_PATGEN_SHORT_DATA_GR, 0x800, &ret);
+	vgxy61_write_reg(sensor, VGXY61_REG_PATGEN_SHORT_DATA_R, 0x800, &ret);
+	vgxy61_write_reg(sensor, VGXY61_REG_PATGEN_SHORT_DATA_B, 0x800, &ret);
+	vgxy61_write_reg(sensor, VGXY61_REG_PATGEN_SHORT_DATA_GB, 0x800, &ret);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int vgxy61_patch(struct vgxy61_dev *sensor)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	int patch, ret;
+
+	ret = vgxy61_write_array(sensor, VGXY61_REG_FWPATCH_START_ADDR,
+				 sizeof(patch_array), patch_array);
+	if (ret)
+		return ret;
+
+	ret = vgxy61_write_reg(sensor, VGXY61_REG_STBY, 0x10, NULL);
+	if (ret)
+		return ret;
+
+	ret = vgxy61_poll_reg(sensor, VGXY61_REG_STBY, 0, VGXY61_TIMEOUT_MS);
+	if (ret)
+		return ret;
+
+	patch = vgxy61_read_reg(sensor, VGXY61_REG_FWPATCH_REVISION);
+	if (patch < 0)
+		return patch;
+
+	if (patch != (VGXY61_FWPATCH_REVISION_MAJOR << 12) +
+		     (VGXY61_FWPATCH_REVISION_MINOR << 8) +
+		     VGXY61_FWPATCH_REVISION_MICRO) {
+		dev_err(&client->dev, "bad patch version expected %d.%d.%d got %d.%d.%d\n",
+			VGXY61_FWPATCH_REVISION_MAJOR,
+			VGXY61_FWPATCH_REVISION_MINOR,
+			VGXY61_FWPATCH_REVISION_MICRO,
+			patch >> 12, (patch >> 8) & 0x0f, patch & 0xff);
+		return -ENODEV;
+	}
+	dev_dbg(&client->dev, "patch %d.%d.%d applied\n",
+		patch >> 12, (patch >> 8) & 0x0f, patch & 0xff);
+
+	return 0;
+}
+
+static int vgxy61_detect_cut_version(struct vgxy61_dev *sensor)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	int device_rev;
+
+	device_rev = vgxy61_read_reg(sensor, VGXY61_REG_REVISION);
+	if (device_rev < 0)
+		return device_rev;
+
+	switch (device_rev >> 8) {
+	case 0xA:
+		dev_dbg(&client->dev, "Cut1 detected\n");
+		dev_err(&client->dev, "Cut1 not supported by this driver\n");
+		return -ENODEV;
+	case 0xB:
+		dev_dbg(&client->dev, "Cut2 detected\n");
+		return 0;
+	case 0xC:
+		dev_dbg(&client->dev, "Cut3 detected\n");
+		return 0;
+	default:
+		dev_err(&client->dev, "Unable to detect cut version\n");
+		return -ENODEV;
+	}
+}
+
+static int vgxy61_detect(struct vgxy61_dev *sensor)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	int id = 0;
+	int ret, st;
+
+	id = vgxy61_read_reg(sensor, VGXY61_REG_MODEL_ID);
+	if (id < 0)
+		return id;
+	if (id != VG5661_MODEL_ID && id != VG5761_MODEL_ID) {
+		dev_warn(&client->dev, "Unsupported sensor id %x\n", id);
+		return -ENODEV;
+	}
+	dev_dbg(&client->dev, "detected sensor id = 0x%04x\n", id);
+	sensor->id = id;
+
+	ret = vgxy61_wait_state(sensor, VGXY61_SYSTEM_FSM_SW_STBY,
+				VGXY61_TIMEOUT_MS);
+	if (ret)
+		return ret;
+
+	st = vgxy61_read_reg(sensor, VGXY61_REG_NVM);
+	if (st < 0)
+		return st;
+	if (st != VGXY61_NVM_OK)
+		dev_warn(&client->dev, "Bad nvm state got %d\n", st);
+
+	ret = vgxy61_detect_cut_version(sensor);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+/* Power/clock management functions */
+static int vgxy61_power_on(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct vgxy61_dev *sensor = to_vgxy61_dev(sd);
+	int ret;
+
+	ret = regulator_bulk_enable(ARRAY_SIZE(vgxy61_supply_name),
+				    sensor->supplies);
+	if (ret) {
+		dev_err(&client->dev, "failed to enable regulators %d\n", ret);
+		return ret;
+	}
+
+	ret = clk_prepare_enable(sensor->xclk);
+	if (ret) {
+		dev_err(&client->dev, "failed to enable clock %d\n", ret);
+		goto disable_bulk;
+	}
+
+	if (sensor->reset_gpio) {
+		ret = vgxy61_apply_reset(sensor);
+		if (ret) {
+			dev_err(&client->dev, "sensor reset failed %d\n", ret);
+			goto disable_clock;
+		}
+	}
+
+	ret = vgxy61_detect(sensor);
+	if (ret) {
+		dev_err(&client->dev, "sensor detect failed %d\n", ret);
+		goto disable_clock;
+	}
+
+	ret = vgxy61_patch(sensor);
+	if (ret) {
+		dev_err(&client->dev, "sensor patch failed %d\n", ret);
+		goto disable_clock;
+	}
+
+	ret = vgxy61_configure(sensor);
+	if (ret) {
+		dev_err(&client->dev, "sensor configuration failed %d\n", ret);
+		goto disable_clock;
+	}
+
+	return 0;
+
+disable_clock:
+	clk_disable_unprepare(sensor->xclk);
+disable_bulk:
+	regulator_bulk_disable(ARRAY_SIZE(vgxy61_supply_name),
+			       sensor->supplies);
+
+	return ret;
+}
+
+static int vgxy61_power_off(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct vgxy61_dev *sensor = to_vgxy61_dev(sd);
+
+	clk_disable_unprepare(sensor->xclk);
+	regulator_bulk_disable(ARRAY_SIZE(vgxy61_supply_name),
+			       sensor->supplies);
+	return 0;
+}
+
+static void vgxy61_fill_sensor_param(struct vgxy61_dev *sensor)
+{
+	if (sensor->id == VG5761_MODEL_ID) {
+		sensor->sensor_width = VGX761_WIDTH;
+		sensor->sensor_height = VGX761_HEIGHT;
+		sensor->sensor_modes = vgx761_mode_data;
+		sensor->sensor_modes_nb = ARRAY_SIZE(vgx761_mode_data);
+		sensor->default_mode = &vgx761_mode_data[VGX761_DEFAULT_MODE];
+		sensor->rot_term = VGX761_SHORT_ROT_TERM;
+	} else if (sensor->id == VG5661_MODEL_ID) {
+		sensor->sensor_width = VGX661_WIDTH;
+		sensor->sensor_height = VGX661_HEIGHT;
+		sensor->sensor_modes = vgx661_mode_data;
+		sensor->sensor_modes_nb = ARRAY_SIZE(vgx661_mode_data);
+		sensor->default_mode = &vgx661_mode_data[VGX661_DEFAULT_MODE];
+		sensor->rot_term = VGX661_SHORT_ROT_TERM;
+	} else {
+		/* Should never happen */
+		WARN_ON(true);
+	}
+	sensor->current_mode = sensor->default_mode;
+}
+
+static int vgxy61_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct fwnode_handle *handle;
+	struct vgxy61_dev *sensor;
+	int ret;
+
+	sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
+	if (!sensor)
+		return -ENOMEM;
+
+	sensor->i2c_client = client;
+	sensor->streaming = false;
+	sensor->hdr = VGXY61_NO_HDR;
+	sensor->expo_long = 200;
+	sensor->expo_short = 0;
+	sensor->hflip = false;
+	sensor->vflip = false;
+	sensor->analog_gain = 0;
+	sensor->digital_gain = 256;
+
+	handle = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev), 0, 0, 0);
+	if (!handle) {
+		dev_err(dev, "handle node not found\n");
+		return -EINVAL;
+	}
+
+	ret = vgxy61_tx_from_ep(sensor, handle);
+	fwnode_handle_put(handle);
+	if (ret) {
+		dev_err(dev, "Failed to parse handle %d\n", ret);
+		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);
+	}
+	sensor->clk_freq = clk_get_rate(sensor->xclk);
+	if (sensor->clk_freq < 6 * HZ_PER_MHZ ||
+	    sensor->clk_freq > 27 * HZ_PER_MHZ) {
+		dev_err(dev, "Only 6Mhz-27Mhz clock range supported. provide %lu MHz\n",
+			sensor->clk_freq / HZ_PER_MHZ);
+		return -EINVAL;
+	}
+	sensor->gpios_polarity =
+		device_property_read_bool(dev, "st,strobe-gpios-polarity");
+
+	v4l2_i2c_subdev_init(&sensor->sd, client, &vgxy61_subdev_ops);
+	sensor->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
+	sensor->sd.entity.ops = &vgxy61_subdev_entity_ops;
+	sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+	sensor->reset_gpio = devm_gpiod_get_optional(dev, "reset",
+						     GPIOD_OUT_HIGH);
+
+	ret = vgxy61_get_regulators(sensor);
+	if (ret) {
+		dev_err(&client->dev, "failed to get regulators %d\n", ret);
+		return ret;
+	}
+
+	ret = vgxy61_power_on(dev);
+	if (ret)
+		return ret;
+
+	vgxy61_fill_sensor_param(sensor);
+	vgxy61_fill_framefmt(sensor, sensor->current_mode, &sensor->fmt,
+			     VGXY61_MEDIA_BUS_FMT_DEF);
+
+	mutex_init(&sensor->lock);
+
+	ret = vgxy61_update_hdr(sensor, sensor->hdr);
+	if (ret)
+		goto error_power_off;
+
+	ret = vgxy61_init_controls(sensor);
+	if (ret) {
+		dev_err(&client->dev, "controls initialization failed %d\n",
+			ret);
+		goto error_power_off;
+	}
+
+	ret = media_entity_pads_init(&sensor->sd.entity, 1, &sensor->pad);
+	if (ret) {
+		dev_err(&client->dev, "pads init failed %d\n", ret);
+		goto error_handler_free;
+	}
+
+	/* Enable runtime PM and turn off the device */
+	pm_runtime_set_active(dev);
+	pm_runtime_enable(dev);
+	pm_runtime_idle(dev);
+
+	ret = v4l2_async_register_subdev(&sensor->sd);
+	if (ret) {
+		dev_err(&client->dev, "async subdev register failed %d\n", ret);
+		goto error_pm_runtime;
+	}
+
+	pm_runtime_set_autosuspend_delay(&client->dev, 1000);
+	pm_runtime_use_autosuspend(&client->dev);
+
+	dev_dbg(&client->dev, "vgxy61 probe successfully\n");
+
+	return 0;
+
+error_pm_runtime:
+	pm_runtime_disable(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+	media_entity_cleanup(&sensor->sd.entity);
+error_handler_free:
+	v4l2_ctrl_handler_free(sensor->sd.ctrl_handler);
+error_power_off:
+	mutex_destroy(&sensor->lock);
+	vgxy61_power_off(dev);
+
+	return ret;
+}
+
+static void vgxy61_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct vgxy61_dev *sensor = to_vgxy61_dev(sd);
+
+	v4l2_async_unregister_subdev(&sensor->sd);
+	mutex_destroy(&sensor->lock);
+	media_entity_cleanup(&sensor->sd.entity);
+
+	pm_runtime_disable(&client->dev);
+	if (!pm_runtime_status_suspended(&client->dev))
+		vgxy61_power_off(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+}
+
+static const struct of_device_id vgxy61_dt_ids[] = {
+	{ .compatible = "st,st-vgxy61" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, vgxy61_dt_ids);
+
+static const struct dev_pm_ops vgxy61_pm_ops = {
+	SET_RUNTIME_PM_OPS(vgxy61_power_off, vgxy61_power_on, NULL)
+};
+
+static struct i2c_driver vgxy61_i2c_driver = {
+	.driver = {
+		.name  = "st-vgxy61",
+		.of_match_table = vgxy61_dt_ids,
+		.pm = &vgxy61_pm_ops,
+	},
+	.probe = vgxy61_probe,
+	.remove = vgxy61_remove,
+};
+
+module_i2c_driver(vgxy61_i2c_driver);
+
+MODULE_AUTHOR("Benjamin Mugnier <benjamin.mugnier@foss.st.com>");
+MODULE_AUTHOR("Mickael Guene <mickael.guene@st.com>");
+MODULE_AUTHOR("Sylvain Petinot <sylvain.petinot@foss.st.com>");
+MODULE_DESCRIPTION("VGXY61 camera subdev driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/tc358743.c b/drivers/media/i2c/tc358743.c
new file mode 100644
index 000000000..2785935da
--- /dev/null
+++ b/drivers/media/i2c/tc358743.c
@@ -0,0 +1,2214 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * tc358743 - Toshiba HDMI to CSI-2 bridge
+ *
+ * Copyright 2015 Cisco Systems, Inc. and/or its affiliates. All rights
+ * reserved.
+ */
+
+/*
+ * References (c = chapter, p = page):
+ * REF_01 - Toshiba, TC358743XBG (H2C), Functional Specification, Rev 0.60
+ * REF_02 - Toshiba, TC358743XBG_HDMI-CSI_Tv11p_nm.xls
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/interrupt.h>
+#include <linux/timer.h>
+#include <linux/of_graph.h>
+#include <linux/videodev2.h>
+#include <linux/workqueue.h>
+#include <linux/v4l2-dv-timings.h>
+#include <linux/hdmi.h>
+#include <media/cec.h>
+#include <media/v4l2-dv-timings.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+#include <media/i2c/tc358743.h>
+
+#include "tc358743_regs.h"
+
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "debug level (0-3)");
+
+MODULE_DESCRIPTION("Toshiba TC358743 HDMI to CSI-2 bridge driver");
+MODULE_AUTHOR("Ramakrishnan Muthukrishnan <ram@rkrishnan.org>");
+MODULE_AUTHOR("Mikhail Khelik <mkhelik@cisco.com>");
+MODULE_AUTHOR("Mats Randgaard <matrandg@cisco.com>");
+MODULE_LICENSE("GPL");
+
+#define EDID_NUM_BLOCKS_MAX 8
+#define EDID_BLOCK_SIZE 128
+
+#define I2C_MAX_XFER_SIZE  (EDID_BLOCK_SIZE + 2)
+
+#define POLL_INTERVAL_CEC_MS	10
+#define POLL_INTERVAL_MS	1000
+
+static const struct v4l2_dv_timings_cap tc358743_timings_cap = {
+	.type = V4L2_DV_BT_656_1120,
+	/* keep this initialization for compatibility with GCC < 4.4.6 */
+	.reserved = { 0 },
+	/* Pixel clock from REF_01 p. 20. Min/max height/width are unknown */
+	V4L2_INIT_BT_TIMINGS(640, 1920, 350, 1200, 13000000, 165000000,
+			V4L2_DV_BT_STD_CEA861 | V4L2_DV_BT_STD_DMT |
+			V4L2_DV_BT_STD_GTF | V4L2_DV_BT_STD_CVT,
+			V4L2_DV_BT_CAP_PROGRESSIVE |
+			V4L2_DV_BT_CAP_REDUCED_BLANKING |
+			V4L2_DV_BT_CAP_CUSTOM)
+};
+
+struct tc358743_state {
+	struct tc358743_platform_data pdata;
+	struct v4l2_mbus_config_mipi_csi2 bus;
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct v4l2_ctrl_handler hdl;
+	struct i2c_client *i2c_client;
+	/* CONFCTL is modified in ops and tc358743_hdmi_sys_int_handler */
+	struct mutex confctl_mutex;
+
+	/* controls */
+	struct v4l2_ctrl *detect_tx_5v_ctrl;
+	struct v4l2_ctrl *audio_sampling_rate_ctrl;
+	struct v4l2_ctrl *audio_present_ctrl;
+
+	struct delayed_work delayed_work_enable_hotplug;
+
+	struct timer_list timer;
+	struct work_struct work_i2c_poll;
+
+	/* edid  */
+	u8 edid_blocks_written;
+
+	struct v4l2_dv_timings timings;
+	u32 mbus_fmt_code;
+	u8 csi_lanes_in_use;
+
+	struct gpio_desc *reset_gpio;
+
+	struct cec_adapter *cec_adap;
+};
+
+static void tc358743_enable_interrupts(struct v4l2_subdev *sd,
+		bool cable_connected);
+static int tc358743_s_ctrl_detect_tx_5v(struct v4l2_subdev *sd);
+
+static inline struct tc358743_state *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct tc358743_state, sd);
+}
+
+/* --------------- I2C --------------- */
+
+static void i2c_rd(struct v4l2_subdev *sd, u16 reg, u8 *values, u32 n)
+{
+	struct tc358743_state *state = to_state(sd);
+	struct i2c_client *client = state->i2c_client;
+	int err;
+	u8 buf[2] = { reg >> 8, reg & 0xff };
+	struct i2c_msg msgs[] = {
+		{
+			.addr = client->addr,
+			.flags = 0,
+			.len = 2,
+			.buf = buf,
+		},
+		{
+			.addr = client->addr,
+			.flags = I2C_M_RD,
+			.len = n,
+			.buf = values,
+		},
+	};
+
+	err = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (err != ARRAY_SIZE(msgs)) {
+		v4l2_err(sd, "%s: reading register 0x%x from 0x%x failed: %d\n",
+				__func__, reg, client->addr, err);
+	}
+}
+
+static void i2c_wr(struct v4l2_subdev *sd, u16 reg, u8 *values, u32 n)
+{
+	struct tc358743_state *state = to_state(sd);
+	struct i2c_client *client = state->i2c_client;
+	int err, i;
+	struct i2c_msg msg;
+	u8 data[I2C_MAX_XFER_SIZE];
+
+	if ((2 + n) > I2C_MAX_XFER_SIZE) {
+		n = I2C_MAX_XFER_SIZE - 2;
+		v4l2_warn(sd, "i2c wr reg=%04x: len=%d is too big!\n",
+			  reg, 2 + n);
+	}
+
+	msg.addr = client->addr;
+	msg.buf = data;
+	msg.len = 2 + n;
+	msg.flags = 0;
+
+	data[0] = reg >> 8;
+	data[1] = reg & 0xff;
+
+	for (i = 0; i < n; i++)
+		data[2 + i] = values[i];
+
+	err = i2c_transfer(client->adapter, &msg, 1);
+	if (err != 1) {
+		v4l2_err(sd, "%s: writing register 0x%x from 0x%x failed: %d\n",
+				__func__, reg, client->addr, err);
+		return;
+	}
+
+	if (debug < 3)
+		return;
+
+	switch (n) {
+	case 1:
+		v4l2_info(sd, "I2C write 0x%04x = 0x%02x",
+				reg, data[2]);
+		break;
+	case 2:
+		v4l2_info(sd, "I2C write 0x%04x = 0x%02x%02x",
+				reg, data[3], data[2]);
+		break;
+	case 4:
+		v4l2_info(sd, "I2C write 0x%04x = 0x%02x%02x%02x%02x",
+				reg, data[5], data[4], data[3], data[2]);
+		break;
+	default:
+		v4l2_info(sd, "I2C write %d bytes from address 0x%04x\n",
+				n, reg);
+	}
+}
+
+static noinline u32 i2c_rdreg(struct v4l2_subdev *sd, u16 reg, u32 n)
+{
+	__le32 val = 0;
+
+	i2c_rd(sd, reg, (u8 __force *)&val, n);
+
+	return le32_to_cpu(val);
+}
+
+static noinline void i2c_wrreg(struct v4l2_subdev *sd, u16 reg, u32 val, u32 n)
+{
+	__le32 raw = cpu_to_le32(val);
+
+	i2c_wr(sd, reg, (u8 __force *)&raw, n);
+}
+
+static u8 i2c_rd8(struct v4l2_subdev *sd, u16 reg)
+{
+	return i2c_rdreg(sd, reg, 1);
+}
+
+static void i2c_wr8(struct v4l2_subdev *sd, u16 reg, u8 val)
+{
+	i2c_wrreg(sd, reg, val, 1);
+}
+
+static void i2c_wr8_and_or(struct v4l2_subdev *sd, u16 reg,
+		u8 mask, u8 val)
+{
+	i2c_wrreg(sd, reg, (i2c_rdreg(sd, reg, 1) & mask) | val, 1);
+}
+
+static u16 i2c_rd16(struct v4l2_subdev *sd, u16 reg)
+{
+	return i2c_rdreg(sd, reg, 2);
+}
+
+static void i2c_wr16(struct v4l2_subdev *sd, u16 reg, u16 val)
+{
+	i2c_wrreg(sd, reg, val, 2);
+}
+
+static void i2c_wr16_and_or(struct v4l2_subdev *sd, u16 reg, u16 mask, u16 val)
+{
+	i2c_wrreg(sd, reg, (i2c_rdreg(sd, reg, 2) & mask) | val, 2);
+}
+
+static u32 i2c_rd32(struct v4l2_subdev *sd, u16 reg)
+{
+	return i2c_rdreg(sd, reg, 4);
+}
+
+static void i2c_wr32(struct v4l2_subdev *sd, u16 reg, u32 val)
+{
+	i2c_wrreg(sd, reg, val, 4);
+}
+
+/* --------------- STATUS --------------- */
+
+static inline bool is_hdmi(struct v4l2_subdev *sd)
+{
+	return i2c_rd8(sd, SYS_STATUS) & MASK_S_HDMI;
+}
+
+static inline bool tx_5v_power_present(struct v4l2_subdev *sd)
+{
+	return i2c_rd8(sd, SYS_STATUS) & MASK_S_DDC5V;
+}
+
+static inline bool no_signal(struct v4l2_subdev *sd)
+{
+	return !(i2c_rd8(sd, SYS_STATUS) & MASK_S_TMDS);
+}
+
+static inline bool no_sync(struct v4l2_subdev *sd)
+{
+	return !(i2c_rd8(sd, SYS_STATUS) & MASK_S_SYNC);
+}
+
+static inline bool audio_present(struct v4l2_subdev *sd)
+{
+	return i2c_rd8(sd, AU_STATUS0) & MASK_S_A_SAMPLE;
+}
+
+static int get_audio_sampling_rate(struct v4l2_subdev *sd)
+{
+	static const int code_to_rate[] = {
+		44100, 0, 48000, 32000, 22050, 384000, 24000, 352800,
+		88200, 768000, 96000, 705600, 176400, 0, 192000, 0
+	};
+
+	/* Register FS_SET is not cleared when the cable is disconnected */
+	if (no_signal(sd))
+		return 0;
+
+	return code_to_rate[i2c_rd8(sd, FS_SET) & MASK_FS];
+}
+
+/* --------------- TIMINGS --------------- */
+
+static inline unsigned fps(const struct v4l2_bt_timings *t)
+{
+	if (!V4L2_DV_BT_FRAME_HEIGHT(t) || !V4L2_DV_BT_FRAME_WIDTH(t))
+		return 0;
+
+	return DIV_ROUND_CLOSEST((unsigned)t->pixelclock,
+			V4L2_DV_BT_FRAME_HEIGHT(t) * V4L2_DV_BT_FRAME_WIDTH(t));
+}
+
+static int tc358743_get_detected_timings(struct v4l2_subdev *sd,
+				     struct v4l2_dv_timings *timings)
+{
+	struct v4l2_bt_timings *bt = &timings->bt;
+	unsigned width, height, frame_width, frame_height, frame_interval, fps;
+
+	memset(timings, 0, sizeof(struct v4l2_dv_timings));
+
+	if (no_signal(sd)) {
+		v4l2_dbg(1, debug, sd, "%s: no valid signal\n", __func__);
+		return -ENOLINK;
+	}
+	if (no_sync(sd)) {
+		v4l2_dbg(1, debug, sd, "%s: no sync on signal\n", __func__);
+		return -ENOLCK;
+	}
+
+	timings->type = V4L2_DV_BT_656_1120;
+	bt->interlaced = i2c_rd8(sd, VI_STATUS1) & MASK_S_V_INTERLACE ?
+		V4L2_DV_INTERLACED : V4L2_DV_PROGRESSIVE;
+
+	width = ((i2c_rd8(sd, DE_WIDTH_H_HI) & 0x1f) << 8) +
+		i2c_rd8(sd, DE_WIDTH_H_LO);
+	height = ((i2c_rd8(sd, DE_WIDTH_V_HI) & 0x1f) << 8) +
+		i2c_rd8(sd, DE_WIDTH_V_LO);
+	frame_width = ((i2c_rd8(sd, H_SIZE_HI) & 0x1f) << 8) +
+		i2c_rd8(sd, H_SIZE_LO);
+	frame_height = (((i2c_rd8(sd, V_SIZE_HI) & 0x3f) << 8) +
+		i2c_rd8(sd, V_SIZE_LO)) / 2;
+	/* frame interval in milliseconds * 10
+	 * Require SYS_FREQ0 and SYS_FREQ1 are precisely set */
+	frame_interval = ((i2c_rd8(sd, FV_CNT_HI) & 0x3) << 8) +
+		i2c_rd8(sd, FV_CNT_LO);
+	fps = (frame_interval > 0) ?
+		DIV_ROUND_CLOSEST(10000, frame_interval) : 0;
+
+	bt->width = width;
+	bt->height = height;
+	bt->vsync = frame_height - height;
+	bt->hsync = frame_width - width;
+	bt->pixelclock = frame_width * frame_height * fps;
+	if (bt->interlaced == V4L2_DV_INTERLACED) {
+		bt->height *= 2;
+		bt->il_vsync = bt->vsync + 1;
+		bt->pixelclock /= 2;
+	}
+
+	return 0;
+}
+
+/* --------------- HOTPLUG / HDCP / EDID --------------- */
+
+static void tc358743_delayed_work_enable_hotplug(struct work_struct *work)
+{
+	struct delayed_work *dwork = to_delayed_work(work);
+	struct tc358743_state *state = container_of(dwork,
+			struct tc358743_state, delayed_work_enable_hotplug);
+	struct v4l2_subdev *sd = &state->sd;
+
+	v4l2_dbg(2, debug, sd, "%s:\n", __func__);
+
+	i2c_wr8_and_or(sd, HPD_CTL, ~MASK_HPD_OUT0, MASK_HPD_OUT0);
+}
+
+static void tc358743_set_hdmi_hdcp(struct v4l2_subdev *sd, bool enable)
+{
+	v4l2_dbg(2, debug, sd, "%s: %s\n", __func__, enable ?
+				"enable" : "disable");
+
+	if (enable) {
+		i2c_wr8_and_or(sd, HDCP_REG3, ~KEY_RD_CMD, KEY_RD_CMD);
+
+		i2c_wr8_and_or(sd, HDCP_MODE, ~MASK_MANUAL_AUTHENTICATION, 0);
+
+		i2c_wr8_and_or(sd, HDCP_REG1, 0xff,
+				MASK_AUTH_UNAUTH_SEL_16_FRAMES |
+				MASK_AUTH_UNAUTH_AUTO);
+
+		i2c_wr8_and_or(sd, HDCP_REG2, ~MASK_AUTO_P3_RESET,
+				SET_AUTO_P3_RESET_FRAMES(0x0f));
+	} else {
+		i2c_wr8_and_or(sd, HDCP_MODE, ~MASK_MANUAL_AUTHENTICATION,
+				MASK_MANUAL_AUTHENTICATION);
+	}
+}
+
+static void tc358743_disable_edid(struct v4l2_subdev *sd)
+{
+	struct tc358743_state *state = to_state(sd);
+
+	v4l2_dbg(2, debug, sd, "%s:\n", __func__);
+
+	cancel_delayed_work_sync(&state->delayed_work_enable_hotplug);
+
+	/* DDC access to EDID is also disabled when hotplug is disabled. See
+	 * register DDC_CTL */
+	i2c_wr8_and_or(sd, HPD_CTL, ~MASK_HPD_OUT0, 0x0);
+}
+
+static void tc358743_enable_edid(struct v4l2_subdev *sd)
+{
+	struct tc358743_state *state = to_state(sd);
+
+	if (state->edid_blocks_written == 0) {
+		v4l2_dbg(2, debug, sd, "%s: no EDID -> no hotplug\n", __func__);
+		tc358743_s_ctrl_detect_tx_5v(sd);
+		return;
+	}
+
+	v4l2_dbg(2, debug, sd, "%s:\n", __func__);
+
+	/* Enable hotplug after 100 ms. DDC access to EDID is also enabled when
+	 * hotplug is enabled. See register DDC_CTL */
+	schedule_delayed_work(&state->delayed_work_enable_hotplug, HZ / 10);
+
+	tc358743_enable_interrupts(sd, true);
+	tc358743_s_ctrl_detect_tx_5v(sd);
+}
+
+static void tc358743_erase_bksv(struct v4l2_subdev *sd)
+{
+	int i;
+
+	for (i = 0; i < 5; i++)
+		i2c_wr8(sd, BKSV + i, 0);
+}
+
+/* --------------- AVI infoframe --------------- */
+
+static void print_avi_infoframe(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct device *dev = &client->dev;
+	union hdmi_infoframe frame;
+	u8 buffer[HDMI_INFOFRAME_SIZE(AVI)];
+
+	if (!is_hdmi(sd)) {
+		v4l2_info(sd, "DVI-D signal - AVI infoframe not supported\n");
+		return;
+	}
+
+	i2c_rd(sd, PK_AVI_0HEAD, buffer, HDMI_INFOFRAME_SIZE(AVI));
+
+	if (hdmi_infoframe_unpack(&frame, buffer, sizeof(buffer)) < 0) {
+		v4l2_err(sd, "%s: unpack of AVI infoframe failed\n", __func__);
+		return;
+	}
+
+	hdmi_infoframe_log(KERN_INFO, dev, &frame);
+}
+
+/* --------------- CTRLS --------------- */
+
+static int tc358743_s_ctrl_detect_tx_5v(struct v4l2_subdev *sd)
+{
+	struct tc358743_state *state = to_state(sd);
+
+	return v4l2_ctrl_s_ctrl(state->detect_tx_5v_ctrl,
+			tx_5v_power_present(sd));
+}
+
+static int tc358743_s_ctrl_audio_sampling_rate(struct v4l2_subdev *sd)
+{
+	struct tc358743_state *state = to_state(sd);
+
+	return v4l2_ctrl_s_ctrl(state->audio_sampling_rate_ctrl,
+			get_audio_sampling_rate(sd));
+}
+
+static int tc358743_s_ctrl_audio_present(struct v4l2_subdev *sd)
+{
+	struct tc358743_state *state = to_state(sd);
+
+	return v4l2_ctrl_s_ctrl(state->audio_present_ctrl,
+			audio_present(sd));
+}
+
+static int tc358743_update_controls(struct v4l2_subdev *sd)
+{
+	int ret = 0;
+
+	ret |= tc358743_s_ctrl_detect_tx_5v(sd);
+	ret |= tc358743_s_ctrl_audio_sampling_rate(sd);
+	ret |= tc358743_s_ctrl_audio_present(sd);
+
+	return ret;
+}
+
+/* --------------- INIT --------------- */
+
+static void tc358743_reset_phy(struct v4l2_subdev *sd)
+{
+	v4l2_dbg(1, debug, sd, "%s:\n", __func__);
+
+	i2c_wr8_and_or(sd, PHY_RST, ~MASK_RESET_CTRL, 0);
+	i2c_wr8_and_or(sd, PHY_RST, ~MASK_RESET_CTRL, MASK_RESET_CTRL);
+}
+
+static void tc358743_reset(struct v4l2_subdev *sd, uint16_t mask)
+{
+	u16 sysctl = i2c_rd16(sd, SYSCTL);
+
+	i2c_wr16(sd, SYSCTL, sysctl | mask);
+	i2c_wr16(sd, SYSCTL, sysctl & ~mask);
+}
+
+static inline void tc358743_sleep_mode(struct v4l2_subdev *sd, bool enable)
+{
+	i2c_wr16_and_or(sd, SYSCTL, ~MASK_SLEEP,
+			enable ? MASK_SLEEP : 0);
+}
+
+static inline void enable_stream(struct v4l2_subdev *sd, bool enable)
+{
+	struct tc358743_state *state = to_state(sd);
+
+	v4l2_dbg(3, debug, sd, "%s: %sable\n",
+			__func__, enable ? "en" : "dis");
+
+	if (enable) {
+		/* It is critical for CSI receiver to see lane transition
+		 * LP11->HS. Set to non-continuous mode to enable clock lane
+		 * LP11 state. */
+		i2c_wr32(sd, TXOPTIONCNTRL, 0);
+		/* Set to continuous mode to trigger LP11->HS transition */
+		i2c_wr32(sd, TXOPTIONCNTRL, MASK_CONTCLKMODE);
+		/* Unmute video */
+		i2c_wr8(sd, VI_MUTE, MASK_AUTO_MUTE);
+	} else {
+		/* Mute video so that all data lanes go to LSP11 state.
+		 * No data is output to CSI Tx block. */
+		i2c_wr8(sd, VI_MUTE, MASK_AUTO_MUTE | MASK_VI_MUTE);
+	}
+
+	mutex_lock(&state->confctl_mutex);
+	i2c_wr16_and_or(sd, CONFCTL, ~(MASK_VBUFEN | MASK_ABUFEN),
+			enable ? (MASK_VBUFEN | MASK_ABUFEN) : 0x0);
+	mutex_unlock(&state->confctl_mutex);
+}
+
+static void tc358743_set_pll(struct v4l2_subdev *sd)
+{
+	struct tc358743_state *state = to_state(sd);
+	struct tc358743_platform_data *pdata = &state->pdata;
+	u16 pllctl0 = i2c_rd16(sd, PLLCTL0);
+	u16 pllctl1 = i2c_rd16(sd, PLLCTL1);
+	u16 pllctl0_new = SET_PLL_PRD(pdata->pll_prd) |
+		SET_PLL_FBD(pdata->pll_fbd);
+	u32 hsck = (pdata->refclk_hz / pdata->pll_prd) * pdata->pll_fbd;
+
+	v4l2_dbg(2, debug, sd, "%s:\n", __func__);
+
+	/* Only rewrite when needed (new value or disabled), since rewriting
+	 * triggers another format change event. */
+	if ((pllctl0 != pllctl0_new) || ((pllctl1 & MASK_PLL_EN) == 0)) {
+		u16 pll_frs;
+
+		if (hsck > 500000000)
+			pll_frs = 0x0;
+		else if (hsck > 250000000)
+			pll_frs = 0x1;
+		else if (hsck > 125000000)
+			pll_frs = 0x2;
+		else
+			pll_frs = 0x3;
+
+		v4l2_dbg(1, debug, sd, "%s: updating PLL clock\n", __func__);
+		tc358743_sleep_mode(sd, true);
+		i2c_wr16(sd, PLLCTL0, pllctl0_new);
+		i2c_wr16_and_or(sd, PLLCTL1,
+				~(MASK_PLL_FRS | MASK_RESETB | MASK_PLL_EN),
+				(SET_PLL_FRS(pll_frs) | MASK_RESETB |
+				 MASK_PLL_EN));
+		udelay(10); /* REF_02, Sheet "Source HDMI" */
+		i2c_wr16_and_or(sd, PLLCTL1, ~MASK_CKEN, MASK_CKEN);
+		tc358743_sleep_mode(sd, false);
+	}
+}
+
+static void tc358743_set_ref_clk(struct v4l2_subdev *sd)
+{
+	struct tc358743_state *state = to_state(sd);
+	struct tc358743_platform_data *pdata = &state->pdata;
+	u32 sys_freq;
+	u32 lockdet_ref;
+	u32 cec_freq;
+	u16 fh_min;
+	u16 fh_max;
+
+	BUG_ON(!(pdata->refclk_hz == 26000000 ||
+		 pdata->refclk_hz == 27000000 ||
+		 pdata->refclk_hz == 42000000));
+
+	sys_freq = pdata->refclk_hz / 10000;
+	i2c_wr8(sd, SYS_FREQ0, sys_freq & 0x00ff);
+	i2c_wr8(sd, SYS_FREQ1, (sys_freq & 0xff00) >> 8);
+
+	i2c_wr8_and_or(sd, PHY_CTL0, ~MASK_PHY_SYSCLK_IND,
+			(pdata->refclk_hz == 42000000) ?
+			MASK_PHY_SYSCLK_IND : 0x0);
+
+	fh_min = pdata->refclk_hz / 100000;
+	i2c_wr8(sd, FH_MIN0, fh_min & 0x00ff);
+	i2c_wr8(sd, FH_MIN1, (fh_min & 0xff00) >> 8);
+
+	fh_max = (fh_min * 66) / 10;
+	i2c_wr8(sd, FH_MAX0, fh_max & 0x00ff);
+	i2c_wr8(sd, FH_MAX1, (fh_max & 0xff00) >> 8);
+
+	lockdet_ref = pdata->refclk_hz / 100;
+	i2c_wr8(sd, LOCKDET_REF0, lockdet_ref & 0x0000ff);
+	i2c_wr8(sd, LOCKDET_REF1, (lockdet_ref & 0x00ff00) >> 8);
+	i2c_wr8(sd, LOCKDET_REF2, (lockdet_ref & 0x0f0000) >> 16);
+
+	i2c_wr8_and_or(sd, NCO_F0_MOD, ~MASK_NCO_F0_MOD,
+			(pdata->refclk_hz == 27000000) ?
+			MASK_NCO_F0_MOD_27MHZ : 0x0);
+
+	/*
+	 * Trial and error suggests that the default register value
+	 * of 656 is for a 42 MHz reference clock. Use that to derive
+	 * a new value based on the actual reference clock.
+	 */
+	cec_freq = (656 * sys_freq) / 4200;
+	i2c_wr16(sd, CECHCLK, cec_freq);
+	i2c_wr16(sd, CECLCLK, cec_freq);
+}
+
+static void tc358743_set_csi_color_space(struct v4l2_subdev *sd)
+{
+	struct tc358743_state *state = to_state(sd);
+
+	switch (state->mbus_fmt_code) {
+	case MEDIA_BUS_FMT_UYVY8_1X16:
+		v4l2_dbg(2, debug, sd, "%s: YCbCr 422 16-bit\n", __func__);
+		i2c_wr8_and_or(sd, VOUT_SET2,
+				~(MASK_SEL422 | MASK_VOUT_422FIL_100) & 0xff,
+				MASK_SEL422 | MASK_VOUT_422FIL_100);
+		i2c_wr8_and_or(sd, VI_REP, ~MASK_VOUT_COLOR_SEL & 0xff,
+				MASK_VOUT_COLOR_601_YCBCR_LIMITED);
+		mutex_lock(&state->confctl_mutex);
+		i2c_wr16_and_or(sd, CONFCTL, ~MASK_YCBCRFMT,
+				MASK_YCBCRFMT_422_8_BIT);
+		mutex_unlock(&state->confctl_mutex);
+		break;
+	case MEDIA_BUS_FMT_RGB888_1X24:
+		v4l2_dbg(2, debug, sd, "%s: RGB 888 24-bit\n", __func__);
+		i2c_wr8_and_or(sd, VOUT_SET2,
+				~(MASK_SEL422 | MASK_VOUT_422FIL_100) & 0xff,
+				0x00);
+		i2c_wr8_and_or(sd, VI_REP, ~MASK_VOUT_COLOR_SEL & 0xff,
+				MASK_VOUT_COLOR_RGB_FULL);
+		mutex_lock(&state->confctl_mutex);
+		i2c_wr16_and_or(sd, CONFCTL, ~MASK_YCBCRFMT, 0);
+		mutex_unlock(&state->confctl_mutex);
+		break;
+	default:
+		v4l2_dbg(2, debug, sd, "%s: Unsupported format code 0x%x\n",
+				__func__, state->mbus_fmt_code);
+	}
+}
+
+static unsigned tc358743_num_csi_lanes_needed(struct v4l2_subdev *sd)
+{
+	struct tc358743_state *state = to_state(sd);
+	struct v4l2_bt_timings *bt = &state->timings.bt;
+	struct tc358743_platform_data *pdata = &state->pdata;
+	u32 bits_pr_pixel =
+		(state->mbus_fmt_code == MEDIA_BUS_FMT_UYVY8_1X16) ?  16 : 24;
+	u32 bps = bt->width * bt->height * fps(bt) * bits_pr_pixel;
+	u32 bps_pr_lane = (pdata->refclk_hz / pdata->pll_prd) * pdata->pll_fbd;
+
+	return DIV_ROUND_UP(bps, bps_pr_lane);
+}
+
+static void tc358743_set_csi(struct v4l2_subdev *sd)
+{
+	struct tc358743_state *state = to_state(sd);
+	struct tc358743_platform_data *pdata = &state->pdata;
+	unsigned lanes = tc358743_num_csi_lanes_needed(sd);
+
+	v4l2_dbg(3, debug, sd, "%s:\n", __func__);
+
+	state->csi_lanes_in_use = lanes;
+
+	tc358743_reset(sd, MASK_CTXRST);
+
+	if (lanes < 1)
+		i2c_wr32(sd, CLW_CNTRL, MASK_CLW_LANEDISABLE);
+	if (lanes < 1)
+		i2c_wr32(sd, D0W_CNTRL, MASK_D0W_LANEDISABLE);
+	if (lanes < 2)
+		i2c_wr32(sd, D1W_CNTRL, MASK_D1W_LANEDISABLE);
+	if (lanes < 3)
+		i2c_wr32(sd, D2W_CNTRL, MASK_D2W_LANEDISABLE);
+	if (lanes < 4)
+		i2c_wr32(sd, D3W_CNTRL, MASK_D3W_LANEDISABLE);
+
+	i2c_wr32(sd, LINEINITCNT, pdata->lineinitcnt);
+	i2c_wr32(sd, LPTXTIMECNT, pdata->lptxtimecnt);
+	i2c_wr32(sd, TCLK_HEADERCNT, pdata->tclk_headercnt);
+	i2c_wr32(sd, TCLK_TRAILCNT, pdata->tclk_trailcnt);
+	i2c_wr32(sd, THS_HEADERCNT, pdata->ths_headercnt);
+	i2c_wr32(sd, TWAKEUP, pdata->twakeup);
+	i2c_wr32(sd, TCLK_POSTCNT, pdata->tclk_postcnt);
+	i2c_wr32(sd, THS_TRAILCNT, pdata->ths_trailcnt);
+	i2c_wr32(sd, HSTXVREGCNT, pdata->hstxvregcnt);
+
+	i2c_wr32(sd, HSTXVREGEN,
+			((lanes > 0) ? MASK_CLM_HSTXVREGEN : 0x0) |
+			((lanes > 0) ? MASK_D0M_HSTXVREGEN : 0x0) |
+			((lanes > 1) ? MASK_D1M_HSTXVREGEN : 0x0) |
+			((lanes > 2) ? MASK_D2M_HSTXVREGEN : 0x0) |
+			((lanes > 3) ? MASK_D3M_HSTXVREGEN : 0x0));
+
+	i2c_wr32(sd, TXOPTIONCNTRL, (state->bus.flags &
+		 V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK) ? 0 : MASK_CONTCLKMODE);
+	i2c_wr32(sd, STARTCNTRL, MASK_START);
+	i2c_wr32(sd, CSI_START, MASK_STRT);
+
+	i2c_wr32(sd, CSI_CONFW, MASK_MODE_SET |
+			MASK_ADDRESS_CSI_CONTROL |
+			MASK_CSI_MODE |
+			MASK_TXHSMD |
+			((lanes == 4) ? MASK_NOL_4 :
+			 (lanes == 3) ? MASK_NOL_3 :
+			 (lanes == 2) ? MASK_NOL_2 : MASK_NOL_1));
+
+	i2c_wr32(sd, CSI_CONFW, MASK_MODE_SET |
+			MASK_ADDRESS_CSI_ERR_INTENA | MASK_TXBRK | MASK_QUNK |
+			MASK_WCER | MASK_INER);
+
+	i2c_wr32(sd, CSI_CONFW, MASK_MODE_CLEAR |
+			MASK_ADDRESS_CSI_ERR_HALT | MASK_TXBRK | MASK_QUNK);
+
+	i2c_wr32(sd, CSI_CONFW, MASK_MODE_SET |
+			MASK_ADDRESS_CSI_INT_ENA | MASK_INTER);
+}
+
+static void tc358743_set_hdmi_phy(struct v4l2_subdev *sd)
+{
+	struct tc358743_state *state = to_state(sd);
+	struct tc358743_platform_data *pdata = &state->pdata;
+
+	/* Default settings from REF_02, sheet "Source HDMI"
+	 * and custom settings as platform data */
+	i2c_wr8_and_or(sd, PHY_EN, ~MASK_ENABLE_PHY, 0x0);
+	i2c_wr8(sd, PHY_CTL1, SET_PHY_AUTO_RST1_US(1600) |
+			SET_FREQ_RANGE_MODE_CYCLES(1));
+	i2c_wr8_and_or(sd, PHY_CTL2, ~MASK_PHY_AUTO_RSTn,
+			(pdata->hdmi_phy_auto_reset_tmds_detected ?
+			 MASK_PHY_AUTO_RST2 : 0) |
+			(pdata->hdmi_phy_auto_reset_tmds_in_range ?
+			 MASK_PHY_AUTO_RST3 : 0) |
+			(pdata->hdmi_phy_auto_reset_tmds_valid ?
+			 MASK_PHY_AUTO_RST4 : 0));
+	i2c_wr8(sd, PHY_BIAS, 0x40);
+	i2c_wr8(sd, PHY_CSQ, SET_CSQ_CNT_LEVEL(0x0a));
+	i2c_wr8(sd, AVM_CTL, 45);
+	i2c_wr8_and_or(sd, HDMI_DET, ~MASK_HDMI_DET_V,
+			pdata->hdmi_detection_delay << 4);
+	i2c_wr8_and_or(sd, HV_RST, ~(MASK_H_PI_RST | MASK_V_PI_RST),
+			(pdata->hdmi_phy_auto_reset_hsync_out_of_range ?
+			 MASK_H_PI_RST : 0) |
+			(pdata->hdmi_phy_auto_reset_vsync_out_of_range ?
+			 MASK_V_PI_RST : 0));
+	i2c_wr8_and_or(sd, PHY_EN, ~MASK_ENABLE_PHY, MASK_ENABLE_PHY);
+}
+
+static void tc358743_set_hdmi_audio(struct v4l2_subdev *sd)
+{
+	struct tc358743_state *state = to_state(sd);
+
+	/* Default settings from REF_02, sheet "Source HDMI" */
+	i2c_wr8(sd, FORCE_MUTE, 0x00);
+	i2c_wr8(sd, AUTO_CMD0, MASK_AUTO_MUTE7 | MASK_AUTO_MUTE6 |
+			MASK_AUTO_MUTE5 | MASK_AUTO_MUTE4 |
+			MASK_AUTO_MUTE1 | MASK_AUTO_MUTE0);
+	i2c_wr8(sd, AUTO_CMD1, MASK_AUTO_MUTE9);
+	i2c_wr8(sd, AUTO_CMD2, MASK_AUTO_PLAY3 | MASK_AUTO_PLAY2);
+	i2c_wr8(sd, BUFINIT_START, SET_BUFINIT_START_MS(500));
+	i2c_wr8(sd, FS_MUTE, 0x00);
+	i2c_wr8(sd, FS_IMODE, MASK_NLPCM_SMODE | MASK_FS_SMODE);
+	i2c_wr8(sd, ACR_MODE, MASK_CTS_MODE);
+	i2c_wr8(sd, ACR_MDF0, MASK_ACR_L2MDF_1976_PPM | MASK_ACR_L1MDF_976_PPM);
+	i2c_wr8(sd, ACR_MDF1, MASK_ACR_L3MDF_3906_PPM);
+	i2c_wr8(sd, SDO_MODE1, MASK_SDO_FMT_I2S);
+	i2c_wr8(sd, DIV_MODE, SET_DIV_DLY_MS(100));
+
+	mutex_lock(&state->confctl_mutex);
+	i2c_wr16_and_or(sd, CONFCTL, 0xffff, MASK_AUDCHNUM_2 |
+			MASK_AUDOUTSEL_I2S | MASK_AUTOINDEX);
+	mutex_unlock(&state->confctl_mutex);
+}
+
+static void tc358743_set_hdmi_info_frame_mode(struct v4l2_subdev *sd)
+{
+	/* Default settings from REF_02, sheet "Source HDMI" */
+	i2c_wr8(sd, PK_INT_MODE, MASK_ISRC2_INT_MODE | MASK_ISRC_INT_MODE |
+			MASK_ACP_INT_MODE | MASK_VS_INT_MODE |
+			MASK_SPD_INT_MODE | MASK_MS_INT_MODE |
+			MASK_AUD_INT_MODE | MASK_AVI_INT_MODE);
+	i2c_wr8(sd, NO_PKT_LIMIT, 0x2c);
+	i2c_wr8(sd, NO_PKT_CLR, 0x53);
+	i2c_wr8(sd, ERR_PK_LIMIT, 0x01);
+	i2c_wr8(sd, NO_PKT_LIMIT2, 0x30);
+	i2c_wr8(sd, NO_GDB_LIMIT, 0x10);
+}
+
+static void tc358743_initial_setup(struct v4l2_subdev *sd)
+{
+	struct tc358743_state *state = to_state(sd);
+	struct tc358743_platform_data *pdata = &state->pdata;
+
+	/*
+	 * IR is not supported by this driver.
+	 * CEC is only enabled if needed.
+	 */
+	i2c_wr16_and_or(sd, SYSCTL, ~(MASK_IRRST | MASK_CECRST),
+				     (MASK_IRRST | MASK_CECRST));
+
+	tc358743_reset(sd, MASK_CTXRST | MASK_HDMIRST);
+#ifdef CONFIG_VIDEO_TC358743_CEC
+	tc358743_reset(sd, MASK_CECRST);
+#endif
+	tc358743_sleep_mode(sd, false);
+
+	i2c_wr16(sd, FIFOCTL, pdata->fifo_level);
+
+	tc358743_set_ref_clk(sd);
+
+	i2c_wr8_and_or(sd, DDC_CTL, ~MASK_DDC5V_MODE,
+			pdata->ddc5v_delay & MASK_DDC5V_MODE);
+	i2c_wr8_and_or(sd, EDID_MODE, ~MASK_EDID_MODE, MASK_EDID_MODE_E_DDC);
+
+	tc358743_set_hdmi_phy(sd);
+	tc358743_set_hdmi_hdcp(sd, pdata->enable_hdcp);
+	tc358743_set_hdmi_audio(sd);
+	tc358743_set_hdmi_info_frame_mode(sd);
+
+	/* All CE and IT formats are detected as RGB full range in DVI mode */
+	i2c_wr8_and_or(sd, VI_MODE, ~MASK_RGB_DVI, 0);
+
+	i2c_wr8_and_or(sd, VOUT_SET2, ~MASK_VOUTCOLORMODE,
+			MASK_VOUTCOLORMODE_AUTO);
+	i2c_wr8(sd, VOUT_SET3, MASK_VOUT_EXTCNT);
+}
+
+/* --------------- CEC --------------- */
+
+#ifdef CONFIG_VIDEO_TC358743_CEC
+static int tc358743_cec_adap_enable(struct cec_adapter *adap, bool enable)
+{
+	struct tc358743_state *state = adap->priv;
+	struct v4l2_subdev *sd = &state->sd;
+
+	i2c_wr32(sd, CECIMSK, enable ? MASK_CECTIM | MASK_CECRIM : 0);
+	i2c_wr32(sd, CECICLR, MASK_CECTICLR | MASK_CECRICLR);
+	i2c_wr32(sd, CECEN, enable);
+	if (enable)
+		i2c_wr32(sd, CECREN, MASK_CECREN);
+	return 0;
+}
+
+static int tc358743_cec_adap_monitor_all_enable(struct cec_adapter *adap,
+						bool enable)
+{
+	struct tc358743_state *state = adap->priv;
+	struct v4l2_subdev *sd = &state->sd;
+	u32 reg;
+
+	reg = i2c_rd32(sd, CECRCTL1);
+	if (enable)
+		reg |= MASK_CECOTH;
+	else
+		reg &= ~MASK_CECOTH;
+	i2c_wr32(sd, CECRCTL1, reg);
+	return 0;
+}
+
+static int tc358743_cec_adap_log_addr(struct cec_adapter *adap, u8 log_addr)
+{
+	struct tc358743_state *state = adap->priv;
+	struct v4l2_subdev *sd = &state->sd;
+	unsigned int la = 0;
+
+	if (log_addr != CEC_LOG_ADDR_INVALID) {
+		la = i2c_rd32(sd, CECADD);
+		la |= 1 << log_addr;
+	}
+	i2c_wr32(sd, CECADD, la);
+	return 0;
+}
+
+static int tc358743_cec_adap_transmit(struct cec_adapter *adap, u8 attempts,
+				   u32 signal_free_time, struct cec_msg *msg)
+{
+	struct tc358743_state *state = adap->priv;
+	struct v4l2_subdev *sd = &state->sd;
+	unsigned int i;
+
+	i2c_wr32(sd, CECTCTL,
+		 (cec_msg_is_broadcast(msg) ? MASK_CECBRD : 0) |
+		 (signal_free_time - 1));
+	for (i = 0; i < msg->len; i++)
+		i2c_wr32(sd, CECTBUF1 + i * 4,
+			msg->msg[i] | ((i == msg->len - 1) ? MASK_CECTEOM : 0));
+	i2c_wr32(sd, CECTEN, MASK_CECTEN);
+	return 0;
+}
+
+static const struct cec_adap_ops tc358743_cec_adap_ops = {
+	.adap_enable = tc358743_cec_adap_enable,
+	.adap_log_addr = tc358743_cec_adap_log_addr,
+	.adap_transmit = tc358743_cec_adap_transmit,
+	.adap_monitor_all_enable = tc358743_cec_adap_monitor_all_enable,
+};
+
+static void tc358743_cec_handler(struct v4l2_subdev *sd, u16 intstatus,
+				 bool *handled)
+{
+	struct tc358743_state *state = to_state(sd);
+	unsigned int cec_rxint, cec_txint;
+	unsigned int clr = 0;
+
+	cec_rxint = i2c_rd32(sd, CECRSTAT);
+	cec_txint = i2c_rd32(sd, CECTSTAT);
+
+	if (intstatus & MASK_CEC_RINT)
+		clr |= MASK_CECRICLR;
+	if (intstatus & MASK_CEC_TINT)
+		clr |= MASK_CECTICLR;
+	i2c_wr32(sd, CECICLR, clr);
+
+	if ((intstatus & MASK_CEC_TINT) && cec_txint) {
+		if (cec_txint & MASK_CECTIEND)
+			cec_transmit_attempt_done(state->cec_adap,
+						  CEC_TX_STATUS_OK);
+		else if (cec_txint & MASK_CECTIAL)
+			cec_transmit_attempt_done(state->cec_adap,
+						  CEC_TX_STATUS_ARB_LOST);
+		else if (cec_txint & MASK_CECTIACK)
+			cec_transmit_attempt_done(state->cec_adap,
+						  CEC_TX_STATUS_NACK);
+		else if (cec_txint & MASK_CECTIUR) {
+			/*
+			 * Not sure when this bit is set. Treat
+			 * it as an error for now.
+			 */
+			cec_transmit_attempt_done(state->cec_adap,
+						  CEC_TX_STATUS_ERROR);
+		}
+		if (handled)
+			*handled = true;
+	}
+	if ((intstatus & MASK_CEC_RINT) &&
+	    (cec_rxint & MASK_CECRIEND)) {
+		struct cec_msg msg = {};
+		unsigned int i;
+		unsigned int v;
+
+		v = i2c_rd32(sd, CECRCTR);
+		msg.len = v & 0x1f;
+		if (msg.len > CEC_MAX_MSG_SIZE)
+			msg.len = CEC_MAX_MSG_SIZE;
+		for (i = 0; i < msg.len; i++) {
+			v = i2c_rd32(sd, CECRBUF1 + i * 4);
+			msg.msg[i] = v & 0xff;
+		}
+		cec_received_msg(state->cec_adap, &msg);
+		if (handled)
+			*handled = true;
+	}
+	i2c_wr16(sd, INTSTATUS,
+		 intstatus & (MASK_CEC_RINT | MASK_CEC_TINT));
+}
+
+#endif
+
+/* --------------- IRQ --------------- */
+
+static void tc358743_format_change(struct v4l2_subdev *sd)
+{
+	struct tc358743_state *state = to_state(sd);
+	struct v4l2_dv_timings timings;
+	const struct v4l2_event tc358743_ev_fmt = {
+		.type = V4L2_EVENT_SOURCE_CHANGE,
+		.u.src_change.changes = V4L2_EVENT_SRC_CH_RESOLUTION,
+	};
+
+	if (tc358743_get_detected_timings(sd, &timings)) {
+		enable_stream(sd, false);
+
+		v4l2_dbg(1, debug, sd, "%s: No signal\n",
+				__func__);
+	} else {
+		if (!v4l2_match_dv_timings(&state->timings, &timings, 0, false))
+			enable_stream(sd, false);
+
+		if (debug)
+			v4l2_print_dv_timings(sd->name,
+					"tc358743_format_change: New format: ",
+					&timings, false);
+	}
+
+	if (sd->devnode)
+		v4l2_subdev_notify_event(sd, &tc358743_ev_fmt);
+}
+
+static void tc358743_init_interrupts(struct v4l2_subdev *sd)
+{
+	u16 i;
+
+	/* clear interrupt status registers */
+	for (i = SYS_INT; i <= KEY_INT; i++)
+		i2c_wr8(sd, i, 0xff);
+
+	i2c_wr16(sd, INTSTATUS, 0xffff);
+}
+
+static void tc358743_enable_interrupts(struct v4l2_subdev *sd,
+		bool cable_connected)
+{
+	v4l2_dbg(2, debug, sd, "%s: cable connected = %d\n", __func__,
+			cable_connected);
+
+	if (cable_connected) {
+		i2c_wr8(sd, SYS_INTM, ~(MASK_M_DDC | MASK_M_DVI_DET |
+					MASK_M_HDMI_DET) & 0xff);
+		i2c_wr8(sd, CLK_INTM, ~MASK_M_IN_DE_CHG);
+		i2c_wr8(sd, CBIT_INTM, ~(MASK_M_CBIT_FS | MASK_M_AF_LOCK |
+					MASK_M_AF_UNLOCK) & 0xff);
+		i2c_wr8(sd, AUDIO_INTM, ~MASK_M_BUFINIT_END);
+		i2c_wr8(sd, MISC_INTM, ~MASK_M_SYNC_CHG);
+	} else {
+		i2c_wr8(sd, SYS_INTM, ~MASK_M_DDC & 0xff);
+		i2c_wr8(sd, CLK_INTM, 0xff);
+		i2c_wr8(sd, CBIT_INTM, 0xff);
+		i2c_wr8(sd, AUDIO_INTM, 0xff);
+		i2c_wr8(sd, MISC_INTM, 0xff);
+	}
+}
+
+static void tc358743_hdmi_audio_int_handler(struct v4l2_subdev *sd,
+		bool *handled)
+{
+	u8 audio_int_mask = i2c_rd8(sd, AUDIO_INTM);
+	u8 audio_int = i2c_rd8(sd, AUDIO_INT) & ~audio_int_mask;
+
+	i2c_wr8(sd, AUDIO_INT, audio_int);
+
+	v4l2_dbg(3, debug, sd, "%s: AUDIO_INT = 0x%02x\n", __func__, audio_int);
+
+	tc358743_s_ctrl_audio_sampling_rate(sd);
+	tc358743_s_ctrl_audio_present(sd);
+}
+
+static void tc358743_csi_err_int_handler(struct v4l2_subdev *sd, bool *handled)
+{
+	v4l2_err(sd, "%s: CSI_ERR = 0x%x\n", __func__, i2c_rd32(sd, CSI_ERR));
+
+	i2c_wr32(sd, CSI_INT_CLR, MASK_ICRER);
+}
+
+static void tc358743_hdmi_misc_int_handler(struct v4l2_subdev *sd,
+		bool *handled)
+{
+	u8 misc_int_mask = i2c_rd8(sd, MISC_INTM);
+	u8 misc_int = i2c_rd8(sd, MISC_INT) & ~misc_int_mask;
+
+	i2c_wr8(sd, MISC_INT, misc_int);
+
+	v4l2_dbg(3, debug, sd, "%s: MISC_INT = 0x%02x\n", __func__, misc_int);
+
+	if (misc_int & MASK_I_SYNC_CHG) {
+		/* Reset the HDMI PHY to try to trigger proper lock on the
+		 * incoming video format. Erase BKSV to prevent that old keys
+		 * are used when a new source is connected. */
+		if (no_sync(sd) || no_signal(sd)) {
+			tc358743_reset_phy(sd);
+			tc358743_erase_bksv(sd);
+		}
+
+		tc358743_format_change(sd);
+
+		misc_int &= ~MASK_I_SYNC_CHG;
+		if (handled)
+			*handled = true;
+	}
+
+	if (misc_int) {
+		v4l2_err(sd, "%s: Unhandled MISC_INT interrupts: 0x%02x\n",
+				__func__, misc_int);
+	}
+}
+
+static void tc358743_hdmi_cbit_int_handler(struct v4l2_subdev *sd,
+		bool *handled)
+{
+	u8 cbit_int_mask = i2c_rd8(sd, CBIT_INTM);
+	u8 cbit_int = i2c_rd8(sd, CBIT_INT) & ~cbit_int_mask;
+
+	i2c_wr8(sd, CBIT_INT, cbit_int);
+
+	v4l2_dbg(3, debug, sd, "%s: CBIT_INT = 0x%02x\n", __func__, cbit_int);
+
+	if (cbit_int & MASK_I_CBIT_FS) {
+
+		v4l2_dbg(1, debug, sd, "%s: Audio sample rate changed\n",
+				__func__);
+		tc358743_s_ctrl_audio_sampling_rate(sd);
+
+		cbit_int &= ~MASK_I_CBIT_FS;
+		if (handled)
+			*handled = true;
+	}
+
+	if (cbit_int & (MASK_I_AF_LOCK | MASK_I_AF_UNLOCK)) {
+
+		v4l2_dbg(1, debug, sd, "%s: Audio present changed\n",
+				__func__);
+		tc358743_s_ctrl_audio_present(sd);
+
+		cbit_int &= ~(MASK_I_AF_LOCK | MASK_I_AF_UNLOCK);
+		if (handled)
+			*handled = true;
+	}
+
+	if (cbit_int) {
+		v4l2_err(sd, "%s: Unhandled CBIT_INT interrupts: 0x%02x\n",
+				__func__, cbit_int);
+	}
+}
+
+static void tc358743_hdmi_clk_int_handler(struct v4l2_subdev *sd, bool *handled)
+{
+	u8 clk_int_mask = i2c_rd8(sd, CLK_INTM);
+	u8 clk_int = i2c_rd8(sd, CLK_INT) & ~clk_int_mask;
+
+	/* Bit 7 and bit 6 are set even when they are masked */
+	i2c_wr8(sd, CLK_INT, clk_int | 0x80 | MASK_I_OUT_H_CHG);
+
+	v4l2_dbg(3, debug, sd, "%s: CLK_INT = 0x%02x\n", __func__, clk_int);
+
+	if (clk_int & (MASK_I_IN_DE_CHG)) {
+
+		v4l2_dbg(1, debug, sd, "%s: DE size or position has changed\n",
+				__func__);
+
+		/* If the source switch to a new resolution with the same pixel
+		 * frequency as the existing (e.g. 1080p25 -> 720p50), the
+		 * I_SYNC_CHG interrupt is not always triggered, while the
+		 * I_IN_DE_CHG interrupt seems to work fine. Format change
+		 * notifications are only sent when the signal is stable to
+		 * reduce the number of notifications. */
+		if (!no_signal(sd) && !no_sync(sd))
+			tc358743_format_change(sd);
+
+		clk_int &= ~(MASK_I_IN_DE_CHG);
+		if (handled)
+			*handled = true;
+	}
+
+	if (clk_int) {
+		v4l2_err(sd, "%s: Unhandled CLK_INT interrupts: 0x%02x\n",
+				__func__, clk_int);
+	}
+}
+
+static void tc358743_hdmi_sys_int_handler(struct v4l2_subdev *sd, bool *handled)
+{
+	struct tc358743_state *state = to_state(sd);
+	u8 sys_int_mask = i2c_rd8(sd, SYS_INTM);
+	u8 sys_int = i2c_rd8(sd, SYS_INT) & ~sys_int_mask;
+
+	i2c_wr8(sd, SYS_INT, sys_int);
+
+	v4l2_dbg(3, debug, sd, "%s: SYS_INT = 0x%02x\n", __func__, sys_int);
+
+	if (sys_int & MASK_I_DDC) {
+		bool tx_5v = tx_5v_power_present(sd);
+
+		v4l2_dbg(1, debug, sd, "%s: Tx 5V power present: %s\n",
+				__func__, tx_5v ?  "yes" : "no");
+
+		if (tx_5v) {
+			tc358743_enable_edid(sd);
+		} else {
+			tc358743_enable_interrupts(sd, false);
+			tc358743_disable_edid(sd);
+			memset(&state->timings, 0, sizeof(state->timings));
+			tc358743_erase_bksv(sd);
+			tc358743_update_controls(sd);
+		}
+
+		sys_int &= ~MASK_I_DDC;
+		if (handled)
+			*handled = true;
+	}
+
+	if (sys_int & MASK_I_DVI) {
+		v4l2_dbg(1, debug, sd, "%s: HDMI->DVI change detected\n",
+				__func__);
+
+		/* Reset the HDMI PHY to try to trigger proper lock on the
+		 * incoming video format. Erase BKSV to prevent that old keys
+		 * are used when a new source is connected. */
+		if (no_sync(sd) || no_signal(sd)) {
+			tc358743_reset_phy(sd);
+			tc358743_erase_bksv(sd);
+		}
+
+		sys_int &= ~MASK_I_DVI;
+		if (handled)
+			*handled = true;
+	}
+
+	if (sys_int & MASK_I_HDMI) {
+		v4l2_dbg(1, debug, sd, "%s: DVI->HDMI change detected\n",
+				__func__);
+
+		/* Register is reset in DVI mode (REF_01, c. 6.6.41) */
+		i2c_wr8(sd, ANA_CTL, MASK_APPL_PCSX_NORMAL | MASK_ANALOG_ON);
+
+		sys_int &= ~MASK_I_HDMI;
+		if (handled)
+			*handled = true;
+	}
+
+	if (sys_int) {
+		v4l2_err(sd, "%s: Unhandled SYS_INT interrupts: 0x%02x\n",
+				__func__, sys_int);
+	}
+}
+
+/* --------------- CORE OPS --------------- */
+
+static int tc358743_log_status(struct v4l2_subdev *sd)
+{
+	struct tc358743_state *state = to_state(sd);
+	struct v4l2_dv_timings timings;
+	uint8_t hdmi_sys_status =  i2c_rd8(sd, SYS_STATUS);
+	uint16_t sysctl = i2c_rd16(sd, SYSCTL);
+	u8 vi_status3 =  i2c_rd8(sd, VI_STATUS3);
+	const int deep_color_mode[4] = { 8, 10, 12, 16 };
+	static const char * const input_color_space[] = {
+		"RGB", "YCbCr 601", "opRGB", "YCbCr 709", "NA (4)",
+		"xvYCC 601", "NA(6)", "xvYCC 709", "NA(8)", "sYCC601",
+		"NA(10)", "NA(11)", "NA(12)", "opYCC 601"};
+
+	v4l2_info(sd, "-----Chip status-----\n");
+	v4l2_info(sd, "Chip ID: 0x%02x\n",
+			(i2c_rd16(sd, CHIPID) & MASK_CHIPID) >> 8);
+	v4l2_info(sd, "Chip revision: 0x%02x\n",
+			i2c_rd16(sd, CHIPID) & MASK_REVID);
+	v4l2_info(sd, "Reset: IR: %d, CEC: %d, CSI TX: %d, HDMI: %d\n",
+			!!(sysctl & MASK_IRRST),
+			!!(sysctl & MASK_CECRST),
+			!!(sysctl & MASK_CTXRST),
+			!!(sysctl & MASK_HDMIRST));
+	v4l2_info(sd, "Sleep mode: %s\n", sysctl & MASK_SLEEP ? "on" : "off");
+	v4l2_info(sd, "Cable detected (+5V power): %s\n",
+			hdmi_sys_status & MASK_S_DDC5V ? "yes" : "no");
+	v4l2_info(sd, "DDC lines enabled: %s\n",
+			(i2c_rd8(sd, EDID_MODE) & MASK_EDID_MODE_E_DDC) ?
+			"yes" : "no");
+	v4l2_info(sd, "Hotplug enabled: %s\n",
+			(i2c_rd8(sd, HPD_CTL) & MASK_HPD_OUT0) ?
+			"yes" : "no");
+	v4l2_info(sd, "CEC enabled: %s\n",
+			(i2c_rd16(sd, CECEN) & MASK_CECEN) ?  "yes" : "no");
+	v4l2_info(sd, "-----Signal status-----\n");
+	v4l2_info(sd, "TMDS signal detected: %s\n",
+			hdmi_sys_status & MASK_S_TMDS ? "yes" : "no");
+	v4l2_info(sd, "Stable sync signal: %s\n",
+			hdmi_sys_status & MASK_S_SYNC ? "yes" : "no");
+	v4l2_info(sd, "PHY PLL locked: %s\n",
+			hdmi_sys_status & MASK_S_PHY_PLL ? "yes" : "no");
+	v4l2_info(sd, "PHY DE detected: %s\n",
+			hdmi_sys_status & MASK_S_PHY_SCDT ? "yes" : "no");
+
+	if (tc358743_get_detected_timings(sd, &timings)) {
+		v4l2_info(sd, "No video detected\n");
+	} else {
+		v4l2_print_dv_timings(sd->name, "Detected format: ", &timings,
+				true);
+	}
+	v4l2_print_dv_timings(sd->name, "Configured format: ", &state->timings,
+			true);
+
+	v4l2_info(sd, "-----CSI-TX status-----\n");
+	v4l2_info(sd, "Lanes needed: %d\n",
+			tc358743_num_csi_lanes_needed(sd));
+	v4l2_info(sd, "Lanes in use: %d\n",
+			state->csi_lanes_in_use);
+	v4l2_info(sd, "Waiting for particular sync signal: %s\n",
+			(i2c_rd16(sd, CSI_STATUS) & MASK_S_WSYNC) ?
+			"yes" : "no");
+	v4l2_info(sd, "Transmit mode: %s\n",
+			(i2c_rd16(sd, CSI_STATUS) & MASK_S_TXACT) ?
+			"yes" : "no");
+	v4l2_info(sd, "Receive mode: %s\n",
+			(i2c_rd16(sd, CSI_STATUS) & MASK_S_RXACT) ?
+			"yes" : "no");
+	v4l2_info(sd, "Stopped: %s\n",
+			(i2c_rd16(sd, CSI_STATUS) & MASK_S_HLT) ?
+			"yes" : "no");
+	v4l2_info(sd, "Color space: %s\n",
+			state->mbus_fmt_code == MEDIA_BUS_FMT_UYVY8_1X16 ?
+			"YCbCr 422 16-bit" :
+			state->mbus_fmt_code == MEDIA_BUS_FMT_RGB888_1X24 ?
+			"RGB 888 24-bit" : "Unsupported");
+
+	v4l2_info(sd, "-----%s status-----\n", is_hdmi(sd) ? "HDMI" : "DVI-D");
+	v4l2_info(sd, "HDCP encrypted content: %s\n",
+			hdmi_sys_status & MASK_S_HDCP ? "yes" : "no");
+	v4l2_info(sd, "Input color space: %s %s range\n",
+			input_color_space[(vi_status3 & MASK_S_V_COLOR) >> 1],
+			(vi_status3 & MASK_LIMITED) ? "limited" : "full");
+	if (!is_hdmi(sd))
+		return 0;
+	v4l2_info(sd, "AV Mute: %s\n", hdmi_sys_status & MASK_S_AVMUTE ? "on" :
+			"off");
+	v4l2_info(sd, "Deep color mode: %d-bits per channel\n",
+			deep_color_mode[(i2c_rd8(sd, VI_STATUS1) &
+				MASK_S_DEEPCOLOR) >> 2]);
+	print_avi_infoframe(sd);
+
+	return 0;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static void tc358743_print_register_map(struct v4l2_subdev *sd)
+{
+	v4l2_info(sd, "0x0000-0x00FF: Global Control Register\n");
+	v4l2_info(sd, "0x0100-0x01FF: CSI2-TX PHY Register\n");
+	v4l2_info(sd, "0x0200-0x03FF: CSI2-TX PPI Register\n");
+	v4l2_info(sd, "0x0400-0x05FF: Reserved\n");
+	v4l2_info(sd, "0x0600-0x06FF: CEC Register\n");
+	v4l2_info(sd, "0x0700-0x84FF: Reserved\n");
+	v4l2_info(sd, "0x8500-0x85FF: HDMIRX System Control Register\n");
+	v4l2_info(sd, "0x8600-0x86FF: HDMIRX Audio Control Register\n");
+	v4l2_info(sd, "0x8700-0x87FF: HDMIRX InfoFrame packet data Register\n");
+	v4l2_info(sd, "0x8800-0x88FF: HDMIRX HDCP Port Register\n");
+	v4l2_info(sd, "0x8900-0x89FF: HDMIRX Video Output Port & 3D Register\n");
+	v4l2_info(sd, "0x8A00-0x8BFF: Reserved\n");
+	v4l2_info(sd, "0x8C00-0x8FFF: HDMIRX EDID-RAM (1024bytes)\n");
+	v4l2_info(sd, "0x9000-0x90FF: HDMIRX GBD Extraction Control\n");
+	v4l2_info(sd, "0x9100-0x92FF: HDMIRX GBD RAM read\n");
+	v4l2_info(sd, "0x9300-      : Reserved\n");
+}
+
+static int tc358743_get_reg_size(u16 address)
+{
+	/* REF_01 p. 66-72 */
+	if (address <= 0x00ff)
+		return 2;
+	else if ((address >= 0x0100) && (address <= 0x06FF))
+		return 4;
+	else if ((address >= 0x0700) && (address <= 0x84ff))
+		return 2;
+	else
+		return 1;
+}
+
+static int tc358743_g_register(struct v4l2_subdev *sd,
+			       struct v4l2_dbg_register *reg)
+{
+	if (reg->reg > 0xffff) {
+		tc358743_print_register_map(sd);
+		return -EINVAL;
+	}
+
+	reg->size = tc358743_get_reg_size(reg->reg);
+
+	reg->val = i2c_rdreg(sd, reg->reg, reg->size);
+
+	return 0;
+}
+
+static int tc358743_s_register(struct v4l2_subdev *sd,
+			       const struct v4l2_dbg_register *reg)
+{
+	if (reg->reg > 0xffff) {
+		tc358743_print_register_map(sd);
+		return -EINVAL;
+	}
+
+	/* It should not be possible for the user to enable HDCP with a simple
+	 * v4l2-dbg command.
+	 *
+	 * DO NOT REMOVE THIS unless all other issues with HDCP have been
+	 * resolved.
+	 */
+	if (reg->reg == HDCP_MODE ||
+	    reg->reg == HDCP_REG1 ||
+	    reg->reg == HDCP_REG2 ||
+	    reg->reg == HDCP_REG3 ||
+	    reg->reg == BCAPS)
+		return 0;
+
+	i2c_wrreg(sd, (u16)reg->reg, reg->val,
+			tc358743_get_reg_size(reg->reg));
+
+	return 0;
+}
+#endif
+
+static int tc358743_isr(struct v4l2_subdev *sd, u32 status, bool *handled)
+{
+	u16 intstatus = i2c_rd16(sd, INTSTATUS);
+
+	v4l2_dbg(1, debug, sd, "%s: IntStatus = 0x%04x\n", __func__, intstatus);
+
+	if (intstatus & MASK_HDMI_INT) {
+		u8 hdmi_int0 = i2c_rd8(sd, HDMI_INT0);
+		u8 hdmi_int1 = i2c_rd8(sd, HDMI_INT1);
+
+		if (hdmi_int0 & MASK_I_MISC)
+			tc358743_hdmi_misc_int_handler(sd, handled);
+		if (hdmi_int1 & MASK_I_CBIT)
+			tc358743_hdmi_cbit_int_handler(sd, handled);
+		if (hdmi_int1 & MASK_I_CLK)
+			tc358743_hdmi_clk_int_handler(sd, handled);
+		if (hdmi_int1 & MASK_I_SYS)
+			tc358743_hdmi_sys_int_handler(sd, handled);
+		if (hdmi_int1 & MASK_I_AUD)
+			tc358743_hdmi_audio_int_handler(sd, handled);
+
+		i2c_wr16(sd, INTSTATUS, MASK_HDMI_INT);
+		intstatus &= ~MASK_HDMI_INT;
+	}
+
+#ifdef CONFIG_VIDEO_TC358743_CEC
+	if (intstatus & (MASK_CEC_RINT | MASK_CEC_TINT)) {
+		tc358743_cec_handler(sd, intstatus, handled);
+		i2c_wr16(sd, INTSTATUS,
+			 intstatus & (MASK_CEC_RINT | MASK_CEC_TINT));
+		intstatus &= ~(MASK_CEC_RINT | MASK_CEC_TINT);
+	}
+#endif
+
+	if (intstatus & MASK_CSI_INT) {
+		u32 csi_int = i2c_rd32(sd, CSI_INT);
+
+		if (csi_int & MASK_INTER)
+			tc358743_csi_err_int_handler(sd, handled);
+
+		i2c_wr16(sd, INTSTATUS, MASK_CSI_INT);
+	}
+
+	intstatus = i2c_rd16(sd, INTSTATUS);
+	if (intstatus) {
+		v4l2_dbg(1, debug, sd,
+				"%s: Unhandled IntStatus interrupts: 0x%02x\n",
+				__func__, intstatus);
+	}
+
+	return 0;
+}
+
+static irqreturn_t tc358743_irq_handler(int irq, void *dev_id)
+{
+	struct tc358743_state *state = dev_id;
+	bool handled = false;
+
+	tc358743_isr(&state->sd, 0, &handled);
+
+	return handled ? IRQ_HANDLED : IRQ_NONE;
+}
+
+static void tc358743_irq_poll_timer(struct timer_list *t)
+{
+	struct tc358743_state *state = from_timer(state, t, timer);
+	unsigned int msecs;
+
+	schedule_work(&state->work_i2c_poll);
+	/*
+	 * If CEC is present, then we need to poll more frequently,
+	 * otherwise we will miss CEC messages.
+	 */
+	msecs = state->cec_adap ? POLL_INTERVAL_CEC_MS : POLL_INTERVAL_MS;
+	mod_timer(&state->timer, jiffies + msecs_to_jiffies(msecs));
+}
+
+static void tc358743_work_i2c_poll(struct work_struct *work)
+{
+	struct tc358743_state *state = container_of(work,
+			struct tc358743_state, work_i2c_poll);
+	bool handled;
+
+	tc358743_isr(&state->sd, 0, &handled);
+}
+
+static int tc358743_subscribe_event(struct v4l2_subdev *sd, struct v4l2_fh *fh,
+				    struct v4l2_event_subscription *sub)
+{
+	switch (sub->type) {
+	case V4L2_EVENT_SOURCE_CHANGE:
+		return v4l2_src_change_event_subdev_subscribe(sd, fh, sub);
+	case V4L2_EVENT_CTRL:
+		return v4l2_ctrl_subdev_subscribe_event(sd, fh, sub);
+	default:
+		return -EINVAL;
+	}
+}
+
+/* --------------- VIDEO OPS --------------- */
+
+static int tc358743_g_input_status(struct v4l2_subdev *sd, u32 *status)
+{
+	*status = 0;
+	*status |= no_signal(sd) ? V4L2_IN_ST_NO_SIGNAL : 0;
+	*status |= no_sync(sd) ? V4L2_IN_ST_NO_SYNC : 0;
+
+	v4l2_dbg(1, debug, sd, "%s: status = 0x%x\n", __func__, *status);
+
+	return 0;
+}
+
+static int tc358743_s_dv_timings(struct v4l2_subdev *sd,
+				 struct v4l2_dv_timings *timings)
+{
+	struct tc358743_state *state = to_state(sd);
+
+	if (!timings)
+		return -EINVAL;
+
+	if (debug)
+		v4l2_print_dv_timings(sd->name, "tc358743_s_dv_timings: ",
+				timings, false);
+
+	if (v4l2_match_dv_timings(&state->timings, timings, 0, false)) {
+		v4l2_dbg(1, debug, sd, "%s: no change\n", __func__);
+		return 0;
+	}
+
+	if (!v4l2_valid_dv_timings(timings,
+				&tc358743_timings_cap, NULL, NULL)) {
+		v4l2_dbg(1, debug, sd, "%s: timings out of range\n", __func__);
+		return -ERANGE;
+	}
+
+	state->timings = *timings;
+
+	enable_stream(sd, false);
+	tc358743_set_pll(sd);
+	tc358743_set_csi(sd);
+
+	return 0;
+}
+
+static int tc358743_g_dv_timings(struct v4l2_subdev *sd,
+				 struct v4l2_dv_timings *timings)
+{
+	struct tc358743_state *state = to_state(sd);
+
+	*timings = state->timings;
+
+	return 0;
+}
+
+static int tc358743_enum_dv_timings(struct v4l2_subdev *sd,
+				    struct v4l2_enum_dv_timings *timings)
+{
+	if (timings->pad != 0)
+		return -EINVAL;
+
+	return v4l2_enum_dv_timings_cap(timings,
+			&tc358743_timings_cap, NULL, NULL);
+}
+
+static int tc358743_query_dv_timings(struct v4l2_subdev *sd,
+		struct v4l2_dv_timings *timings)
+{
+	int ret;
+
+	ret = tc358743_get_detected_timings(sd, timings);
+	if (ret)
+		return ret;
+
+	if (debug)
+		v4l2_print_dv_timings(sd->name, "tc358743_query_dv_timings: ",
+				timings, false);
+
+	if (!v4l2_valid_dv_timings(timings,
+				&tc358743_timings_cap, NULL, NULL)) {
+		v4l2_dbg(1, debug, sd, "%s: timings out of range\n", __func__);
+		return -ERANGE;
+	}
+
+	return 0;
+}
+
+static int tc358743_dv_timings_cap(struct v4l2_subdev *sd,
+		struct v4l2_dv_timings_cap *cap)
+{
+	if (cap->pad != 0)
+		return -EINVAL;
+
+	*cap = tc358743_timings_cap;
+
+	return 0;
+}
+
+static int tc358743_get_mbus_config(struct v4l2_subdev *sd,
+				    unsigned int pad,
+				    struct v4l2_mbus_config *cfg)
+{
+	struct tc358743_state *state = to_state(sd);
+
+	cfg->type = V4L2_MBUS_CSI2_DPHY;
+
+	/* Support for non-continuous CSI-2 clock is missing in the driver */
+	cfg->bus.mipi_csi2.flags = 0;
+	cfg->bus.mipi_csi2.num_data_lanes = state->csi_lanes_in_use;
+
+	return 0;
+}
+
+static int tc358743_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	enable_stream(sd, enable);
+	if (!enable) {
+		/* Put all lanes in LP-11 state (STOPSTATE) */
+		tc358743_set_csi(sd);
+	}
+
+	return 0;
+}
+
+/* --------------- PAD OPS --------------- */
+
+static int tc358743_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_mbus_code_enum *code)
+{
+	switch (code->index) {
+	case 0:
+		code->code = MEDIA_BUS_FMT_RGB888_1X24;
+		break;
+	case 1:
+		code->code = MEDIA_BUS_FMT_UYVY8_1X16;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int tc358743_get_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_format *format)
+{
+	struct tc358743_state *state = to_state(sd);
+	u8 vi_rep = i2c_rd8(sd, VI_REP);
+
+	if (format->pad != 0)
+		return -EINVAL;
+
+	format->format.code = state->mbus_fmt_code;
+	format->format.width = state->timings.bt.width;
+	format->format.height = state->timings.bt.height;
+	format->format.field = V4L2_FIELD_NONE;
+
+	switch (vi_rep & MASK_VOUT_COLOR_SEL) {
+	case MASK_VOUT_COLOR_RGB_FULL:
+	case MASK_VOUT_COLOR_RGB_LIMITED:
+		format->format.colorspace = V4L2_COLORSPACE_SRGB;
+		break;
+	case MASK_VOUT_COLOR_601_YCBCR_LIMITED:
+	case MASK_VOUT_COLOR_601_YCBCR_FULL:
+		format->format.colorspace = V4L2_COLORSPACE_SMPTE170M;
+		break;
+	case MASK_VOUT_COLOR_709_YCBCR_FULL:
+	case MASK_VOUT_COLOR_709_YCBCR_LIMITED:
+		format->format.colorspace = V4L2_COLORSPACE_REC709;
+		break;
+	default:
+		format->format.colorspace = 0;
+		break;
+	}
+
+	return 0;
+}
+
+static int tc358743_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_format *format)
+{
+	struct tc358743_state *state = to_state(sd);
+
+	u32 code = format->format.code; /* is overwritten by get_fmt */
+	int ret = tc358743_get_fmt(sd, sd_state, format);
+
+	format->format.code = code;
+
+	if (ret)
+		return ret;
+
+	switch (code) {
+	case MEDIA_BUS_FMT_RGB888_1X24:
+	case MEDIA_BUS_FMT_UYVY8_1X16:
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+		return 0;
+
+	state->mbus_fmt_code = format->format.code;
+
+	enable_stream(sd, false);
+	tc358743_set_pll(sd);
+	tc358743_set_csi(sd);
+	tc358743_set_csi_color_space(sd);
+
+	return 0;
+}
+
+static int tc358743_g_edid(struct v4l2_subdev *sd,
+		struct v4l2_subdev_edid *edid)
+{
+	struct tc358743_state *state = to_state(sd);
+
+	memset(edid->reserved, 0, sizeof(edid->reserved));
+
+	if (edid->pad != 0)
+		return -EINVAL;
+
+	if (edid->start_block == 0 && edid->blocks == 0) {
+		edid->blocks = state->edid_blocks_written;
+		return 0;
+	}
+
+	if (state->edid_blocks_written == 0)
+		return -ENODATA;
+
+	if (edid->start_block >= state->edid_blocks_written ||
+			edid->blocks == 0)
+		return -EINVAL;
+
+	if (edid->start_block + edid->blocks > state->edid_blocks_written)
+		edid->blocks = state->edid_blocks_written - edid->start_block;
+
+	i2c_rd(sd, EDID_RAM + (edid->start_block * EDID_BLOCK_SIZE), edid->edid,
+			edid->blocks * EDID_BLOCK_SIZE);
+
+	return 0;
+}
+
+static int tc358743_s_edid(struct v4l2_subdev *sd,
+				struct v4l2_subdev_edid *edid)
+{
+	struct tc358743_state *state = to_state(sd);
+	u16 edid_len = edid->blocks * EDID_BLOCK_SIZE;
+	u16 pa;
+	int err;
+	int i;
+
+	v4l2_dbg(2, debug, sd, "%s, pad %d, start block %d, blocks %d\n",
+		 __func__, edid->pad, edid->start_block, edid->blocks);
+
+	memset(edid->reserved, 0, sizeof(edid->reserved));
+
+	if (edid->pad != 0)
+		return -EINVAL;
+
+	if (edid->start_block != 0)
+		return -EINVAL;
+
+	if (edid->blocks > EDID_NUM_BLOCKS_MAX) {
+		edid->blocks = EDID_NUM_BLOCKS_MAX;
+		return -E2BIG;
+	}
+	pa = cec_get_edid_phys_addr(edid->edid, edid->blocks * 128, NULL);
+	err = v4l2_phys_addr_validate(pa, &pa, NULL);
+	if (err)
+		return err;
+
+	cec_phys_addr_invalidate(state->cec_adap);
+
+	tc358743_disable_edid(sd);
+
+	i2c_wr8(sd, EDID_LEN1, edid_len & 0xff);
+	i2c_wr8(sd, EDID_LEN2, edid_len >> 8);
+
+	if (edid->blocks == 0) {
+		state->edid_blocks_written = 0;
+		return 0;
+	}
+
+	for (i = 0; i < edid_len; i += EDID_BLOCK_SIZE)
+		i2c_wr(sd, EDID_RAM + i, edid->edid + i, EDID_BLOCK_SIZE);
+
+	state->edid_blocks_written = edid->blocks;
+
+	cec_s_phys_addr(state->cec_adap, pa, false);
+
+	if (tx_5v_power_present(sd))
+		tc358743_enable_edid(sd);
+
+	return 0;
+}
+
+/* -------------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_core_ops tc358743_core_ops = {
+	.log_status = tc358743_log_status,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = tc358743_g_register,
+	.s_register = tc358743_s_register,
+#endif
+	.interrupt_service_routine = tc358743_isr,
+	.subscribe_event = tc358743_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_video_ops tc358743_video_ops = {
+	.g_input_status = tc358743_g_input_status,
+	.s_dv_timings = tc358743_s_dv_timings,
+	.g_dv_timings = tc358743_g_dv_timings,
+	.query_dv_timings = tc358743_query_dv_timings,
+	.s_stream = tc358743_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops tc358743_pad_ops = {
+	.enum_mbus_code = tc358743_enum_mbus_code,
+	.set_fmt = tc358743_set_fmt,
+	.get_fmt = tc358743_get_fmt,
+	.get_edid = tc358743_g_edid,
+	.set_edid = tc358743_s_edid,
+	.enum_dv_timings = tc358743_enum_dv_timings,
+	.dv_timings_cap = tc358743_dv_timings_cap,
+	.get_mbus_config = tc358743_get_mbus_config,
+};
+
+static const struct v4l2_subdev_ops tc358743_ops = {
+	.core = &tc358743_core_ops,
+	.video = &tc358743_video_ops,
+	.pad = &tc358743_pad_ops,
+};
+
+/* --------------- CUSTOM CTRLS --------------- */
+
+static const struct v4l2_ctrl_config tc358743_ctrl_audio_sampling_rate = {
+	.id = TC358743_CID_AUDIO_SAMPLING_RATE,
+	.name = "Audio sampling rate",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.min = 0,
+	.max = 768000,
+	.step = 1,
+	.def = 0,
+	.flags = V4L2_CTRL_FLAG_READ_ONLY,
+};
+
+static const struct v4l2_ctrl_config tc358743_ctrl_audio_present = {
+	.id = TC358743_CID_AUDIO_PRESENT,
+	.name = "Audio present",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.min = 0,
+	.max = 1,
+	.step = 1,
+	.def = 0,
+	.flags = V4L2_CTRL_FLAG_READ_ONLY,
+};
+
+/* --------------- PROBE / REMOVE --------------- */
+
+#ifdef CONFIG_OF
+static void tc358743_gpio_reset(struct tc358743_state *state)
+{
+	usleep_range(5000, 10000);
+	gpiod_set_value(state->reset_gpio, 1);
+	usleep_range(1000, 2000);
+	gpiod_set_value(state->reset_gpio, 0);
+	msleep(20);
+}
+
+static int tc358743_probe_of(struct tc358743_state *state)
+{
+	struct device *dev = &state->i2c_client->dev;
+	struct v4l2_fwnode_endpoint endpoint = { .bus_type = 0 };
+	struct device_node *ep;
+	struct clk *refclk;
+	u32 bps_pr_lane;
+	int ret;
+
+	refclk = devm_clk_get(dev, "refclk");
+	if (IS_ERR(refclk))
+		return dev_err_probe(dev, PTR_ERR(refclk),
+				     "failed to get refclk\n");
+
+	ep = of_graph_get_next_endpoint(dev->of_node, NULL);
+	if (!ep) {
+		dev_err(dev, "missing endpoint node\n");
+		return -EINVAL;
+	}
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(of_fwnode_handle(ep), &endpoint);
+	if (ret) {
+		dev_err(dev, "failed to parse endpoint\n");
+		goto put_node;
+	}
+
+	if (endpoint.bus_type != V4L2_MBUS_CSI2_DPHY ||
+	    endpoint.bus.mipi_csi2.num_data_lanes == 0 ||
+	    endpoint.nr_of_link_frequencies == 0) {
+		dev_err(dev, "missing CSI-2 properties in endpoint\n");
+		ret = -EINVAL;
+		goto free_endpoint;
+	}
+
+	if (endpoint.bus.mipi_csi2.num_data_lanes > 4) {
+		dev_err(dev, "invalid number of lanes\n");
+		ret = -EINVAL;
+		goto free_endpoint;
+	}
+
+	state->bus = endpoint.bus.mipi_csi2;
+
+	ret = clk_prepare_enable(refclk);
+	if (ret) {
+		dev_err(dev, "Failed! to enable clock\n");
+		goto free_endpoint;
+	}
+
+	state->pdata.refclk_hz = clk_get_rate(refclk);
+	state->pdata.ddc5v_delay = DDC5V_DELAY_100_MS;
+	state->pdata.enable_hdcp = false;
+	/* A FIFO level of 16 should be enough for 2-lane 720p60 at 594 MHz. */
+	state->pdata.fifo_level = 16;
+	/*
+	 * The PLL input clock is obtained by dividing refclk by pll_prd.
+	 * It must be between 6 MHz and 40 MHz, lower frequency is better.
+	 */
+	switch (state->pdata.refclk_hz) {
+	case 26000000:
+	case 27000000:
+	case 42000000:
+		state->pdata.pll_prd = state->pdata.refclk_hz / 6000000;
+		break;
+	default:
+		dev_err(dev, "unsupported refclk rate: %u Hz\n",
+			state->pdata.refclk_hz);
+		goto disable_clk;
+	}
+
+	/*
+	 * The CSI bps per lane must be between 62.5 Mbps and 1 Gbps.
+	 * The default is 594 Mbps for 4-lane 1080p60 or 2-lane 720p60.
+	 */
+	bps_pr_lane = 2 * endpoint.link_frequencies[0];
+	if (bps_pr_lane < 62500000U || bps_pr_lane > 1000000000U) {
+		dev_err(dev, "unsupported bps per lane: %u bps\n", bps_pr_lane);
+		ret = -EINVAL;
+		goto disable_clk;
+	}
+
+	/* The CSI speed per lane is refclk / pll_prd * pll_fbd */
+	state->pdata.pll_fbd = bps_pr_lane /
+			       state->pdata.refclk_hz * state->pdata.pll_prd;
+
+	/*
+	 * FIXME: These timings are from REF_02 for 594 Mbps per lane (297 MHz
+	 * link frequency). In principle it should be possible to calculate
+	 * them based on link frequency and resolution.
+	 */
+	if (bps_pr_lane != 594000000U)
+		dev_warn(dev, "untested bps per lane: %u bps\n", bps_pr_lane);
+	state->pdata.lineinitcnt = 0xe80;
+	state->pdata.lptxtimecnt = 0x003;
+	/* tclk-preparecnt: 3, tclk-zerocnt: 20 */
+	state->pdata.tclk_headercnt = 0x1403;
+	state->pdata.tclk_trailcnt = 0x00;
+	/* ths-preparecnt: 3, ths-zerocnt: 1 */
+	state->pdata.ths_headercnt = 0x0103;
+	state->pdata.twakeup = 0x4882;
+	state->pdata.tclk_postcnt = 0x008;
+	state->pdata.ths_trailcnt = 0x2;
+	state->pdata.hstxvregcnt = 0;
+
+	state->reset_gpio = devm_gpiod_get_optional(dev, "reset",
+						    GPIOD_OUT_LOW);
+	if (IS_ERR(state->reset_gpio)) {
+		dev_err(dev, "failed to get reset gpio\n");
+		ret = PTR_ERR(state->reset_gpio);
+		goto disable_clk;
+	}
+
+	if (state->reset_gpio)
+		tc358743_gpio_reset(state);
+
+	ret = 0;
+	goto free_endpoint;
+
+disable_clk:
+	clk_disable_unprepare(refclk);
+free_endpoint:
+	v4l2_fwnode_endpoint_free(&endpoint);
+put_node:
+	of_node_put(ep);
+	return ret;
+}
+#else
+static inline int tc358743_probe_of(struct tc358743_state *state)
+{
+	return -ENODEV;
+}
+#endif
+
+static int tc358743_probe(struct i2c_client *client)
+{
+	static struct v4l2_dv_timings default_timing =
+		V4L2_DV_BT_CEA_640X480P59_94;
+	struct tc358743_state *state;
+	struct tc358743_platform_data *pdata = client->dev.platform_data;
+	struct v4l2_subdev *sd;
+	u16 irq_mask = MASK_HDMI_MSK | MASK_CSI_MSK;
+	int err;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+	v4l_dbg(1, debug, client, "chip found @ 0x%x (%s)\n",
+		client->addr << 1, client->adapter->name);
+
+	state = devm_kzalloc(&client->dev, sizeof(struct tc358743_state),
+			GFP_KERNEL);
+	if (!state)
+		return -ENOMEM;
+
+	state->i2c_client = client;
+
+	/* platform data */
+	if (pdata) {
+		state->pdata = *pdata;
+		state->bus.flags = 0;
+	} else {
+		err = tc358743_probe_of(state);
+		if (err == -ENODEV)
+			v4l_err(client, "No platform data!\n");
+		if (err)
+			return err;
+	}
+
+	sd = &state->sd;
+	v4l2_i2c_subdev_init(sd, client, &tc358743_ops);
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
+
+	/* i2c access */
+	if ((i2c_rd16(sd, CHIPID) & MASK_CHIPID) != 0) {
+		v4l2_info(sd, "not a TC358743 on address 0x%x\n",
+			  client->addr << 1);
+		return -ENODEV;
+	}
+
+	/* control handlers */
+	v4l2_ctrl_handler_init(&state->hdl, 3);
+
+	state->detect_tx_5v_ctrl = v4l2_ctrl_new_std(&state->hdl, NULL,
+			V4L2_CID_DV_RX_POWER_PRESENT, 0, 1, 0, 0);
+
+	/* custom controls */
+	state->audio_sampling_rate_ctrl = v4l2_ctrl_new_custom(&state->hdl,
+			&tc358743_ctrl_audio_sampling_rate, NULL);
+
+	state->audio_present_ctrl = v4l2_ctrl_new_custom(&state->hdl,
+			&tc358743_ctrl_audio_present, NULL);
+
+	sd->ctrl_handler = &state->hdl;
+	if (state->hdl.error) {
+		err = state->hdl.error;
+		goto err_hdl;
+	}
+
+	if (tc358743_update_controls(sd)) {
+		err = -ENODEV;
+		goto err_hdl;
+	}
+
+	state->pad.flags = MEDIA_PAD_FL_SOURCE;
+	sd->entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
+	err = media_entity_pads_init(&sd->entity, 1, &state->pad);
+	if (err < 0)
+		goto err_hdl;
+
+	state->mbus_fmt_code = MEDIA_BUS_FMT_RGB888_1X24;
+
+	sd->dev = &client->dev;
+	err = v4l2_async_register_subdev(sd);
+	if (err < 0)
+		goto err_hdl;
+
+	mutex_init(&state->confctl_mutex);
+
+	INIT_DELAYED_WORK(&state->delayed_work_enable_hotplug,
+			tc358743_delayed_work_enable_hotplug);
+
+#ifdef CONFIG_VIDEO_TC358743_CEC
+	state->cec_adap = cec_allocate_adapter(&tc358743_cec_adap_ops,
+		state, dev_name(&client->dev),
+		CEC_CAP_DEFAULTS | CEC_CAP_MONITOR_ALL, CEC_MAX_LOG_ADDRS);
+	if (IS_ERR(state->cec_adap)) {
+		err = PTR_ERR(state->cec_adap);
+		goto err_hdl;
+	}
+	irq_mask |= MASK_CEC_RMSK | MASK_CEC_TMSK;
+#endif
+
+	tc358743_initial_setup(sd);
+
+	tc358743_s_dv_timings(sd, &default_timing);
+
+	tc358743_set_csi_color_space(sd);
+
+	tc358743_init_interrupts(sd);
+
+	if (state->i2c_client->irq) {
+		err = devm_request_threaded_irq(&client->dev,
+						state->i2c_client->irq,
+						NULL, tc358743_irq_handler,
+						IRQF_TRIGGER_HIGH | IRQF_ONESHOT,
+						"tc358743", state);
+		if (err)
+			goto err_work_queues;
+	} else {
+		INIT_WORK(&state->work_i2c_poll,
+			  tc358743_work_i2c_poll);
+		timer_setup(&state->timer, tc358743_irq_poll_timer, 0);
+		state->timer.expires = jiffies +
+				       msecs_to_jiffies(POLL_INTERVAL_MS);
+		add_timer(&state->timer);
+	}
+
+	err = cec_register_adapter(state->cec_adap, &client->dev);
+	if (err < 0) {
+		pr_err("%s: failed to register the cec device\n", __func__);
+		cec_delete_adapter(state->cec_adap);
+		state->cec_adap = NULL;
+		goto err_work_queues;
+	}
+
+	tc358743_enable_interrupts(sd, tx_5v_power_present(sd));
+	i2c_wr16(sd, INTMASK, ~irq_mask);
+
+	err = v4l2_ctrl_handler_setup(sd->ctrl_handler);
+	if (err)
+		goto err_work_queues;
+
+	v4l2_info(sd, "%s found @ 0x%x (%s)\n", client->name,
+		  client->addr << 1, client->adapter->name);
+
+	return 0;
+
+err_work_queues:
+	cec_unregister_adapter(state->cec_adap);
+	if (!state->i2c_client->irq)
+		flush_work(&state->work_i2c_poll);
+	cancel_delayed_work(&state->delayed_work_enable_hotplug);
+	mutex_destroy(&state->confctl_mutex);
+err_hdl:
+	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(&state->hdl);
+	return err;
+}
+
+static void tc358743_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct tc358743_state *state = to_state(sd);
+
+	if (!state->i2c_client->irq) {
+		del_timer_sync(&state->timer);
+		flush_work(&state->work_i2c_poll);
+	}
+	cancel_delayed_work_sync(&state->delayed_work_enable_hotplug);
+	cec_unregister_adapter(state->cec_adap);
+	v4l2_async_unregister_subdev(sd);
+	v4l2_device_unregister_subdev(sd);
+	mutex_destroy(&state->confctl_mutex);
+	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(&state->hdl);
+}
+
+static const struct i2c_device_id tc358743_id[] = {
+	{"tc358743", 0},
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, tc358743_id);
+
+#if IS_ENABLED(CONFIG_OF)
+static const struct of_device_id tc358743_of_match[] = {
+	{ .compatible = "toshiba,tc358743" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, tc358743_of_match);
+#endif
+
+static struct i2c_driver tc358743_driver = {
+	.driver = {
+		.name = "tc358743",
+		.of_match_table = of_match_ptr(tc358743_of_match),
+	},
+	.probe = tc358743_probe,
+	.remove = tc358743_remove,
+	.id_table = tc358743_id,
+};
+
+module_i2c_driver(tc358743_driver);
diff --git a/drivers/media/i2c/tc358743_regs.h b/drivers/media/i2c/tc358743_regs.h
new file mode 100644
index 000000000..2495878dc
--- /dev/null
+++ b/drivers/media/i2c/tc358743_regs.h
@@ -0,0 +1,759 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * tc358743 - Toshiba HDMI to CSI-2 bridge - register names and bit masks
+ *
+ * Copyright 2015 Cisco Systems, Inc. and/or its affiliates. All rights
+ * reserved.
+ */
+
+/*
+ * References (c = chapter, p = page):
+ * REF_01 - Toshiba, TC358743XBG (H2C), Functional Specification, Rev 0.60
+ */
+
+/* Bit masks has prefix 'MASK_' and options after '_'. */
+
+#ifndef __TC358743_REGS_H
+#define __TC358743_REGS_H
+
+#define CHIPID                                0x0000
+#define MASK_CHIPID                           0xff00
+#define MASK_REVID                            0x00ff
+
+#define SYSCTL                                0x0002
+#define MASK_IRRST                            0x0800
+#define MASK_CECRST                           0x0400
+#define MASK_CTXRST                           0x0200
+#define MASK_HDMIRST                          0x0100
+#define MASK_SLEEP                            0x0001
+
+#define CONFCTL                               0x0004
+#define MASK_PWRISO                           0x8000
+#define MASK_ACLKOPT                          0x1000
+#define MASK_AUDCHNUM                         0x0c00
+#define MASK_AUDCHNUM_8                       0x0000
+#define MASK_AUDCHNUM_6                       0x0400
+#define MASK_AUDCHNUM_4                       0x0800
+#define MASK_AUDCHNUM_2                       0x0c00
+#define MASK_AUDCHSEL                         0x0200
+#define MASK_I2SDLYOPT                        0x0100
+#define MASK_YCBCRFMT                         0x00c0
+#define MASK_YCBCRFMT_444                     0x0000
+#define MASK_YCBCRFMT_422_12_BIT              0x0040
+#define MASK_YCBCRFMT_COLORBAR                0x0080
+#define MASK_YCBCRFMT_422_8_BIT               0x00c0
+#define MASK_INFRMEN                          0x0020
+#define MASK_AUDOUTSEL                        0x0018
+#define MASK_AUDOUTSEL_CSI                    0x0000
+#define MASK_AUDOUTSEL_I2S                    0x0010
+#define MASK_AUDOUTSEL_TDM                    0x0018
+#define MASK_AUTOINDEX                        0x0004
+#define MASK_ABUFEN                           0x0002
+#define MASK_VBUFEN                           0x0001
+
+#define FIFOCTL                               0x0006
+
+#define INTSTATUS                             0x0014
+#define MASK_AMUTE_INT                        0x0400
+#define MASK_HDMI_INT                         0x0200
+#define MASK_CSI_INT                          0x0100
+#define MASK_SYS_INT                          0x0020
+#define MASK_CEC_EINT                         0x0010
+#define MASK_CEC_TINT                         0x0008
+#define MASK_CEC_RINT                         0x0004
+#define MASK_IR_EINT                          0x0002
+#define MASK_IR_DINT                          0x0001
+
+#define INTMASK                               0x0016
+#define MASK_AMUTE_MSK                        0x0400
+#define MASK_HDMI_MSK                         0x0200
+#define MASK_CSI_MSK                          0x0100
+#define MASK_SYS_MSK                          0x0020
+#define MASK_CEC_EMSK                         0x0010
+#define MASK_CEC_TMSK                         0x0008
+#define MASK_CEC_RMSK                         0x0004
+#define MASK_IR_EMSK                          0x0002
+#define MASK_IR_DMSK                          0x0001
+
+#define INTFLAG                               0x0018
+#define INTSYSSTATUS                          0x001A
+
+#define PLLCTL0                               0x0020
+#define MASK_PLL_PRD                          0xf000
+#define SET_PLL_PRD(prd)                      ((((prd) - 1) << 12) &\
+						MASK_PLL_PRD)
+#define MASK_PLL_FBD                          0x01ff
+#define SET_PLL_FBD(fbd)                      (((fbd) - 1) & MASK_PLL_FBD)
+
+#define PLLCTL1                               0x0022
+#define MASK_PLL_FRS                          0x0c00
+#define SET_PLL_FRS(frs)                      (((frs) << 10) & MASK_PLL_FRS)
+#define MASK_PLL_LBWS                         0x0300
+#define MASK_LFBREN                           0x0040
+#define MASK_BYPCKEN                          0x0020
+#define MASK_CKEN                             0x0010
+#define MASK_RESETB                           0x0002
+#define MASK_PLL_EN                           0x0001
+
+#define CLW_CNTRL                             0x0140
+#define MASK_CLW_LANEDISABLE                  0x0001
+
+#define D0W_CNTRL                             0x0144
+#define MASK_D0W_LANEDISABLE                  0x0001
+
+#define D1W_CNTRL                             0x0148
+#define MASK_D1W_LANEDISABLE                  0x0001
+
+#define D2W_CNTRL                             0x014C
+#define MASK_D2W_LANEDISABLE                  0x0001
+
+#define D3W_CNTRL                             0x0150
+#define MASK_D3W_LANEDISABLE                  0x0001
+
+#define STARTCNTRL                            0x0204
+#define MASK_START                            0x00000001
+
+#define LINEINITCNT                           0x0210
+#define LPTXTIMECNT                           0x0214
+#define TCLK_HEADERCNT                        0x0218
+#define TCLK_TRAILCNT                         0x021C
+#define THS_HEADERCNT                         0x0220
+#define TWAKEUP                               0x0224
+#define TCLK_POSTCNT                          0x0228
+#define THS_TRAILCNT                          0x022C
+#define HSTXVREGCNT                           0x0230
+
+#define HSTXVREGEN                            0x0234
+#define MASK_D3M_HSTXVREGEN                   0x0010
+#define MASK_D2M_HSTXVREGEN                   0x0008
+#define MASK_D1M_HSTXVREGEN                   0x0004
+#define MASK_D0M_HSTXVREGEN                   0x0002
+#define MASK_CLM_HSTXVREGEN                   0x0001
+
+
+#define TXOPTIONCNTRL                         0x0238
+#define MASK_CONTCLKMODE                      0x00000001
+
+#define CSI_CONTROL                           0x040C
+#define MASK_CSI_MODE                         0x8000
+#define MASK_HTXTOEN                          0x0400
+#define MASK_TXHSMD                           0x0080
+#define MASK_HSCKMD                           0x0020
+#define MASK_NOL                              0x0006
+#define MASK_NOL_1                            0x0000
+#define MASK_NOL_2                            0x0002
+#define MASK_NOL_3                            0x0004
+#define MASK_NOL_4                            0x0006
+#define MASK_EOTDIS                           0x0001
+
+#define CSI_INT                               0x0414
+#define MASK_INTHLT                           0x00000008
+#define MASK_INTER                            0x00000004
+
+#define CSI_INT_ENA                           0x0418
+#define MASK_IENHLT                           0x00000008
+#define MASK_IENER                            0x00000004
+
+#define CSI_ERR                               0x044C
+#define MASK_INER                             0x00000200
+#define MASK_WCER                             0x00000100
+#define MASK_QUNK                             0x00000010
+#define MASK_TXBRK                            0x00000002
+
+#define CSI_ERR_INTENA                        0x0450
+#define CSI_ERR_HALT                          0x0454
+
+#define CSI_CONFW                             0x0500
+#define MASK_MODE                             0xe0000000
+#define MASK_MODE_SET                         0xa0000000
+#define MASK_MODE_CLEAR                       0xc0000000
+#define MASK_ADDRESS                          0x1f000000
+#define MASK_ADDRESS_CSI_CONTROL              0x03000000
+#define MASK_ADDRESS_CSI_INT_ENA              0x06000000
+#define MASK_ADDRESS_CSI_ERR_INTENA           0x14000000
+#define MASK_ADDRESS_CSI_ERR_HALT             0x15000000
+#define MASK_DATA                             0x0000ffff
+
+#define CSI_INT_CLR                           0x050C
+#define MASK_ICRER                            0x00000004
+
+#define CSI_START                             0x0518
+#define MASK_STRT                             0x00000001
+
+/* *** CEC (32 bit) *** */
+#define CECHCLK				      0x0028	/* 16 bits */
+#define MASK_CECHCLK			      (0x7ff << 0)
+
+#define CECLCLK				      0x002a	/* 16 bits */
+#define MASK_CECLCLK			      (0x7ff << 0)
+
+#define CECEN				      0x0600
+#define MASK_CECEN			      0x0001
+
+#define CECADD				      0x0604
+#define CECRST				      0x0608
+#define MASK_CECRESET			      0x0001
+
+#define CECREN				      0x060c
+#define MASK_CECREN			      0x0001
+
+#define CECRCTL1			      0x0614
+#define MASK_CECACKDIS			      (1 << 24)
+#define MASK_CECHNC			      (3 << 20)
+#define MASK_CECLNC			      (7 << 16)
+#define MASK_CECMIN			      (7 << 12)
+#define MASK_CECMAX			      (7 << 8)
+#define MASK_CECDAT			      (7 << 4)
+#define MASK_CECTOUT			      (3 << 2)
+#define MASK_CECRIHLD			      (1 << 1)
+#define MASK_CECOTH			      (1 << 0)
+
+#define CECRCTL2			      0x0618
+#define MASK_CECSWAV3			      (7 << 12)
+#define MASK_CECSWAV2			      (7 << 8)
+#define MASK_CECSWAV1			      (7 << 4)
+#define MASK_CECSWAV0			      (7 << 0)
+
+#define CECRCTL3			      0x061c
+#define MASK_CECWAV3			      (7 << 20)
+#define MASK_CECWAV2			      (7 << 16)
+#define MASK_CECWAV1			      (7 << 12)
+#define MASK_CECWAV0			      (7 << 8)
+#define MASK_CECACKEI			      (1 << 4)
+#define MASK_CECMINEI			      (1 << 3)
+#define MASK_CECMAXEI			      (1 << 2)
+#define MASK_CECRSTEI			      (1 << 1)
+#define MASK_CECWAVEI			      (1 << 0)
+
+#define CECTEN				      0x0620
+#define MASK_CECTBUSY			      (1 << 1)
+#define MASK_CECTEN			      (1 << 0)
+
+#define CECTCTL				      0x0628
+#define MASK_CECSTRS			      (7 << 20)
+#define MASK_CECSPRD			      (7 << 16)
+#define MASK_CECDTRS			      (7 << 12)
+#define MASK_CECDPRD			      (15 << 8)
+#define MASK_CECBRD			      (1 << 4)
+#define MASK_CECFREE			      (15 << 0)
+
+#define CECRSTAT			      0x062c
+#define MASK_CECRIWA			      (1 << 6)
+#define MASK_CECRIOR			      (1 << 5)
+#define MASK_CECRIACK			      (1 << 4)
+#define MASK_CECRIMIN			      (1 << 3)
+#define MASK_CECRIMAX			      (1 << 2)
+#define MASK_CECRISTA			      (1 << 1)
+#define MASK_CECRIEND			      (1 << 0)
+
+#define CECTSTAT			      0x0630
+#define MASK_CECTIUR			      (1 << 4)
+#define MASK_CECTIACK			      (1 << 3)
+#define MASK_CECTIAL			      (1 << 2)
+#define MASK_CECTIEND			      (1 << 1)
+
+#define CECRBUF1			      0x0634
+#define MASK_CECRACK			      (1 << 9)
+#define MASK_CECEOM			      (1 << 8)
+#define MASK_CECRBYTE			      (0xff << 0)
+
+#define CECTBUF1			      0x0674
+#define MASK_CECTEOM			      (1 << 8)
+#define MASK_CECTBYTE			      (0xff << 0)
+
+#define CECRCTR				      0x06b4
+#define MASK_CECRCTR			      (0x1f << 0)
+
+#define CECIMSK				      0x06c0
+#define MASK_CECTIM			      (1 << 1)
+#define MASK_CECRIM			      (1 << 0)
+
+#define CECICLR				      0x06cc
+#define MASK_CECTICLR			      (1 << 1)
+#define MASK_CECRICLR			      (1 << 0)
+
+
+#define HDMI_INT0                             0x8500
+#define MASK_I_KEY                            0x80
+#define MASK_I_MISC                           0x02
+#define MASK_I_PHYERR                         0x01
+
+#define HDMI_INT1                             0x8501
+#define MASK_I_GBD                            0x80
+#define MASK_I_HDCP                           0x40
+#define MASK_I_ERR                            0x20
+#define MASK_I_AUD                            0x10
+#define MASK_I_CBIT                           0x08
+#define MASK_I_PACKET                         0x04
+#define MASK_I_CLK                            0x02
+#define MASK_I_SYS                            0x01
+
+#define SYS_INT                               0x8502
+#define MASK_I_ACR_CTS                        0x80
+#define MASK_I_ACRN                           0x40
+#define MASK_I_DVI                            0x20
+#define MASK_I_HDMI                           0x10
+#define MASK_I_NOPMBDET                       0x08
+#define MASK_I_DPMBDET                        0x04
+#define MASK_I_TMDS                           0x02
+#define MASK_I_DDC                            0x01
+
+#define CLK_INT                               0x8503
+#define MASK_I_OUT_H_CHG                      0x40
+#define MASK_I_IN_DE_CHG                      0x20
+#define MASK_I_IN_HV_CHG                      0x10
+#define MASK_I_DC_CHG                         0x08
+#define MASK_I_PXCLK_CHG                      0x04
+#define MASK_I_PHYCLK_CHG                     0x02
+#define MASK_I_TMDSCLK_CHG                    0x01
+
+#define CBIT_INT                              0x8505
+#define MASK_I_AF_LOCK                        0x80
+#define MASK_I_AF_UNLOCK                      0x40
+#define MASK_I_CBIT_FS                        0x02
+
+#define AUDIO_INT                             0x8506
+
+#define ERR_INT                               0x8507
+#define MASK_I_EESS_ERR                       0x80
+
+#define HDCP_INT                              0x8508
+#define MASK_I_AVM_SET                        0x80
+#define MASK_I_AVM_CLR                        0x40
+#define MASK_I_LINKERR                        0x20
+#define MASK_I_SHA_END                        0x10
+#define MASK_I_R0_END                         0x08
+#define MASK_I_KM_END                         0x04
+#define MASK_I_AKSV_END                       0x02
+#define MASK_I_AN_END                         0x01
+
+#define MISC_INT                              0x850B
+#define MASK_I_AS_LAYOUT                      0x10
+#define MASK_I_NO_SPD                         0x08
+#define MASK_I_NO_VS                          0x03
+#define MASK_I_SYNC_CHG                       0x02
+#define MASK_I_AUDIO_MUTE                     0x01
+
+#define KEY_INT                               0x850F
+
+#define SYS_INTM                              0x8512
+#define MASK_M_ACR_CTS                        0x80
+#define MASK_M_ACR_N                          0x40
+#define MASK_M_DVI_DET                        0x20
+#define MASK_M_HDMI_DET                       0x10
+#define MASK_M_NOPMBDET                       0x08
+#define MASK_M_BPMBDET                        0x04
+#define MASK_M_TMDS                           0x02
+#define MASK_M_DDC                            0x01
+
+#define CLK_INTM                              0x8513
+#define MASK_M_OUT_H_CHG                      0x40
+#define MASK_M_IN_DE_CHG                      0x20
+#define MASK_M_IN_HV_CHG                      0x10
+#define MASK_M_DC_CHG                         0x08
+#define MASK_M_PXCLK_CHG                      0x04
+#define MASK_M_PHYCLK_CHG                     0x02
+#define MASK_M_TMDS_CHG                       0x01
+
+#define PACKET_INTM                           0x8514
+
+#define CBIT_INTM                             0x8515
+#define MASK_M_AF_LOCK                        0x80
+#define MASK_M_AF_UNLOCK                      0x40
+#define MASK_M_CBIT_FS                        0x02
+
+#define AUDIO_INTM                            0x8516
+#define MASK_M_BUFINIT_END                    0x01
+
+#define ERR_INTM                              0x8517
+#define MASK_M_EESS_ERR                       0x80
+
+#define HDCP_INTM                             0x8518
+#define MASK_M_AVM_SET                        0x80
+#define MASK_M_AVM_CLR                        0x40
+#define MASK_M_LINKERR                        0x20
+#define MASK_M_SHA_END                        0x10
+#define MASK_M_R0_END                         0x08
+#define MASK_M_KM_END                         0x04
+#define MASK_M_AKSV_END                       0x02
+#define MASK_M_AN_END                         0x01
+
+#define MISC_INTM                             0x851B
+#define MASK_M_AS_LAYOUT                      0x10
+#define MASK_M_NO_SPD                         0x08
+#define MASK_M_NO_VS                          0x03
+#define MASK_M_SYNC_CHG                       0x02
+#define MASK_M_AUDIO_MUTE                     0x01
+
+#define KEY_INTM                              0x851F
+
+#define SYS_STATUS                            0x8520
+#define MASK_S_SYNC                           0x80
+#define MASK_S_AVMUTE                         0x40
+#define MASK_S_HDCP                           0x20
+#define MASK_S_HDMI                           0x10
+#define MASK_S_PHY_SCDT                       0x08
+#define MASK_S_PHY_PLL                        0x04
+#define MASK_S_TMDS                           0x02
+#define MASK_S_DDC5V                          0x01
+
+#define CSI_STATUS                            0x0410
+#define MASK_S_WSYNC                          0x0400
+#define MASK_S_TXACT                          0x0200
+#define MASK_S_RXACT                          0x0100
+#define MASK_S_HLT                            0x0001
+
+#define VI_STATUS1                            0x8522
+#define MASK_S_V_GBD                          0x08
+#define MASK_S_DEEPCOLOR                      0x0c
+#define MASK_S_V_422                          0x02
+#define MASK_S_V_INTERLACE                    0x01
+
+#define AU_STATUS0                            0x8523
+#define MASK_S_A_SAMPLE                       0x01
+
+#define VI_STATUS3                            0x8528
+#define MASK_S_V_COLOR                        0x1e
+#define MASK_LIMITED                          0x01
+
+#define PHY_CTL0                              0x8531
+#define MASK_PHY_SYSCLK_IND                   0x02
+#define MASK_PHY_CTL                          0x01
+
+
+#define PHY_CTL1                              0x8532 /* Not in REF_01 */
+#define MASK_PHY_AUTO_RST1                    0xf0
+#define MASK_PHY_AUTO_RST1_OFF                0x00
+#define SET_PHY_AUTO_RST1_US(us)             ((((us) / 200) << 4) & \
+						MASK_PHY_AUTO_RST1)
+#define MASK_FREQ_RANGE_MODE                  0x0f
+#define SET_FREQ_RANGE_MODE_CYCLES(cycles)   (((cycles) - 1) & \
+						MASK_FREQ_RANGE_MODE)
+
+#define PHY_CTL2                              0x8533 /* Not in REF_01 */
+#define MASK_PHY_AUTO_RST4                    0x04
+#define MASK_PHY_AUTO_RST3                    0x02
+#define MASK_PHY_AUTO_RST2                    0x01
+#define MASK_PHY_AUTO_RSTn                    (MASK_PHY_AUTO_RST4 | \
+						MASK_PHY_AUTO_RST3 | \
+						MASK_PHY_AUTO_RST2)
+
+#define PHY_EN                                0x8534
+#define MASK_ENABLE_PHY                       0x01
+
+#define PHY_RST                               0x8535
+#define MASK_RESET_CTRL                       0x01   /* Reset active low */
+
+#define PHY_BIAS                              0x8536 /* Not in REF_01 */
+
+#define PHY_CSQ                               0x853F /* Not in REF_01 */
+#define MASK_CSQ_CNT                          0x0f
+#define SET_CSQ_CNT_LEVEL(n)                 (n & MASK_CSQ_CNT)
+
+#define SYS_FREQ0                             0x8540
+#define SYS_FREQ1                             0x8541
+
+#define SYS_CLK                               0x8542 /* Not in REF_01 */
+#define MASK_CLK_DIFF                         0x0C
+#define MASK_CLK_DIV                          0x03
+
+#define DDC_CTL                               0x8543
+#define MASK_DDC_ACK_POL                      0x08
+#define MASK_DDC_ACTION                       0x04
+#define MASK_DDC5V_MODE                       0x03
+#define MASK_DDC5V_MODE_0MS                   0x00
+#define MASK_DDC5V_MODE_50MS                  0x01
+#define MASK_DDC5V_MODE_100MS                 0x02
+#define MASK_DDC5V_MODE_200MS                 0x03
+
+#define HPD_CTL                               0x8544
+#define MASK_HPD_CTL0                         0x10
+#define MASK_HPD_OUT0                         0x01
+
+#define ANA_CTL                               0x8545
+#define MASK_APPL_PCSX                        0x30
+#define MASK_APPL_PCSX_HIZ                    0x00
+#define MASK_APPL_PCSX_L_FIX                  0x10
+#define MASK_APPL_PCSX_H_FIX                  0x20
+#define MASK_APPL_PCSX_NORMAL                 0x30
+#define MASK_ANALOG_ON                        0x01
+
+#define AVM_CTL                               0x8546
+
+#define INIT_END                              0x854A
+#define MASK_INIT_END                         0x01
+
+#define HDMI_DET                              0x8552 /* Not in REF_01 */
+#define MASK_HDMI_DET_MOD1                    0x80
+#define MASK_HDMI_DET_MOD0                    0x40
+#define MASK_HDMI_DET_V                       0x30
+#define MASK_HDMI_DET_V_SYNC                  0x00
+#define MASK_HDMI_DET_V_ASYNC_25MS            0x10
+#define MASK_HDMI_DET_V_ASYNC_50MS            0x20
+#define MASK_HDMI_DET_V_ASYNC_100MS           0x30
+#define MASK_HDMI_DET_NUM                     0x0f
+
+#define HDCP_MODE                             0x8560
+#define MASK_MODE_RST_TN                      0x20
+#define MASK_LINE_REKEY                       0x10
+#define MASK_AUTO_CLR                         0x04
+#define MASK_MANUAL_AUTHENTICATION            0x02 /* Not in REF_01 */
+
+#define HDCP_REG1                             0x8563 /* Not in REF_01 */
+#define MASK_AUTH_UNAUTH_SEL                  0x70
+#define MASK_AUTH_UNAUTH_SEL_12_FRAMES        0x70
+#define MASK_AUTH_UNAUTH_SEL_8_FRAMES         0x60
+#define MASK_AUTH_UNAUTH_SEL_4_FRAMES         0x50
+#define MASK_AUTH_UNAUTH_SEL_2_FRAMES         0x40
+#define MASK_AUTH_UNAUTH_SEL_64_FRAMES        0x30
+#define MASK_AUTH_UNAUTH_SEL_32_FRAMES        0x20
+#define MASK_AUTH_UNAUTH_SEL_16_FRAMES        0x10
+#define MASK_AUTH_UNAUTH_SEL_ONCE             0x00
+#define MASK_AUTH_UNAUTH                      0x01
+#define MASK_AUTH_UNAUTH_AUTO                 0x01
+
+#define HDCP_REG2                             0x8564 /* Not in REF_01 */
+#define MASK_AUTO_P3_RESET                    0x0F
+#define SET_AUTO_P3_RESET_FRAMES(n)          (n & MASK_AUTO_P3_RESET)
+#define MASK_AUTO_P3_RESET_OFF                0x00
+
+#define VI_MODE                               0x8570
+#define MASK_RGB_DVI                          0x08 /* Not in REF_01 */
+
+#define VOUT_SET2                             0x8573
+#define MASK_SEL422                           0x80
+#define MASK_VOUT_422FIL_100                  0x40
+#define MASK_VOUTCOLORMODE                    0x03
+#define MASK_VOUTCOLORMODE_THROUGH            0x00
+#define MASK_VOUTCOLORMODE_AUTO               0x01
+#define MASK_VOUTCOLORMODE_MANUAL             0x03
+
+#define VOUT_SET3                             0x8574
+#define MASK_VOUT_EXTCNT                      0x08
+
+#define VI_REP                                0x8576
+#define MASK_VOUT_COLOR_SEL                   0xe0
+#define MASK_VOUT_COLOR_RGB_FULL              0x00
+#define MASK_VOUT_COLOR_RGB_LIMITED           0x20
+#define MASK_VOUT_COLOR_601_YCBCR_FULL        0x40
+#define MASK_VOUT_COLOR_601_YCBCR_LIMITED     0x60
+#define MASK_VOUT_COLOR_709_YCBCR_FULL        0x80
+#define MASK_VOUT_COLOR_709_YCBCR_LIMITED     0xa0
+#define MASK_VOUT_COLOR_FULL_TO_LIMITED       0xc0
+#define MASK_VOUT_COLOR_LIMITED_TO_FULL       0xe0
+#define MASK_IN_REP_HEN                       0x10
+#define MASK_IN_REP                           0x0f
+
+#define VI_MUTE                               0x857F
+#define MASK_AUTO_MUTE                        0xc0
+#define MASK_VI_MUTE                          0x10
+
+#define DE_WIDTH_H_LO                         0x8582 /* Not in REF_01 */
+#define DE_WIDTH_H_HI                         0x8583 /* Not in REF_01 */
+#define DE_WIDTH_V_LO                         0x8588 /* Not in REF_01 */
+#define DE_WIDTH_V_HI                         0x8589 /* Not in REF_01 */
+#define H_SIZE_LO                             0x858A /* Not in REF_01 */
+#define H_SIZE_HI                             0x858B /* Not in REF_01 */
+#define V_SIZE_LO                             0x858C /* Not in REF_01 */
+#define V_SIZE_HI                             0x858D /* Not in REF_01 */
+#define FV_CNT_LO                             0x85A1 /* Not in REF_01 */
+#define FV_CNT_HI                             0x85A2 /* Not in REF_01 */
+
+#define FH_MIN0                               0x85AA /* Not in REF_01 */
+#define FH_MIN1                               0x85AB /* Not in REF_01 */
+#define FH_MAX0                               0x85AC /* Not in REF_01 */
+#define FH_MAX1                               0x85AD /* Not in REF_01 */
+
+#define HV_RST                                0x85AF /* Not in REF_01 */
+#define MASK_H_PI_RST                         0x20
+#define MASK_V_PI_RST                         0x10
+
+#define EDID_MODE                             0x85C7
+#define MASK_EDID_SPEED                       0x40
+#define MASK_EDID_MODE                        0x03
+#define MASK_EDID_MODE_DISABLE                0x00
+#define MASK_EDID_MODE_DDC2B                  0x01
+#define MASK_EDID_MODE_E_DDC                  0x02
+
+#define EDID_LEN1                             0x85CA
+#define EDID_LEN2                             0x85CB
+
+#define HDCP_REG3                             0x85D1 /* Not in REF_01 */
+#define KEY_RD_CMD                            0x01
+
+#define FORCE_MUTE                            0x8600
+#define MASK_FORCE_AMUTE                      0x10
+#define MASK_FORCE_DMUTE                      0x01
+
+#define CMD_AUD                               0x8601
+#define MASK_CMD_BUFINIT                      0x04
+#define MASK_CMD_LOCKDET                      0x02
+#define MASK_CMD_MUTE                         0x01
+
+#define AUTO_CMD0                             0x8602
+#define MASK_AUTO_MUTE7                       0x80
+#define MASK_AUTO_MUTE6                       0x40
+#define MASK_AUTO_MUTE5                       0x20
+#define MASK_AUTO_MUTE4                       0x10
+#define MASK_AUTO_MUTE3                       0x08
+#define MASK_AUTO_MUTE2                       0x04
+#define MASK_AUTO_MUTE1                       0x02
+#define MASK_AUTO_MUTE0                       0x01
+
+#define AUTO_CMD1                             0x8603
+#define MASK_AUTO_MUTE10                      0x04
+#define MASK_AUTO_MUTE9                       0x02
+#define MASK_AUTO_MUTE8                       0x01
+
+#define AUTO_CMD2                             0x8604
+#define MASK_AUTO_PLAY3                       0x08
+#define MASK_AUTO_PLAY2                       0x04
+
+#define BUFINIT_START                         0x8606
+#define SET_BUFINIT_START_MS(milliseconds)   ((milliseconds) / 100)
+
+#define FS_MUTE                               0x8607
+#define MASK_FS_ELSE_MUTE                     0x80
+#define MASK_FS22_MUTE                        0x40
+#define MASK_FS24_MUTE                        0x20
+#define MASK_FS88_MUTE                        0x10
+#define MASK_FS96_MUTE                        0x08
+#define MASK_FS176_MUTE                       0x04
+#define MASK_FS192_MUTE                       0x02
+#define MASK_FS_NO_MUTE                       0x01
+
+#define FS_IMODE                              0x8620
+#define MASK_NLPCM_HMODE                      0x40
+#define MASK_NLPCM_SMODE                      0x20
+#define MASK_NLPCM_IMODE                      0x10
+#define MASK_FS_HMODE                         0x08
+#define MASK_FS_AMODE                         0x04
+#define MASK_FS_SMODE                         0x02
+#define MASK_FS_IMODE                         0x01
+
+#define FS_SET                                0x8621
+#define MASK_FS                               0x0f
+
+#define LOCKDET_REF0                          0x8630
+#define LOCKDET_REF1                          0x8631
+#define LOCKDET_REF2                          0x8632
+
+#define ACR_MODE                              0x8640
+#define MASK_ACR_LOAD                         0x10
+#define MASK_N_MODE                           0x04
+#define MASK_CTS_MODE                         0x01
+
+#define ACR_MDF0                              0x8641
+#define MASK_ACR_L2MDF                        0x70
+#define MASK_ACR_L2MDF_0_PPM                  0x00
+#define MASK_ACR_L2MDF_61_PPM                 0x10
+#define MASK_ACR_L2MDF_122_PPM                0x20
+#define MASK_ACR_L2MDF_244_PPM                0x30
+#define MASK_ACR_L2MDF_488_PPM                0x40
+#define MASK_ACR_L2MDF_976_PPM                0x50
+#define MASK_ACR_L2MDF_1976_PPM               0x60
+#define MASK_ACR_L2MDF_3906_PPM               0x70
+#define MASK_ACR_L1MDF                        0x07
+#define MASK_ACR_L1MDF_0_PPM                  0x00
+#define MASK_ACR_L1MDF_61_PPM                 0x01
+#define MASK_ACR_L1MDF_122_PPM                0x02
+#define MASK_ACR_L1MDF_244_PPM                0x03
+#define MASK_ACR_L1MDF_488_PPM                0x04
+#define MASK_ACR_L1MDF_976_PPM                0x05
+#define MASK_ACR_L1MDF_1976_PPM               0x06
+#define MASK_ACR_L1MDF_3906_PPM               0x07
+
+#define ACR_MDF1                              0x8642
+#define MASK_ACR_L3MDF                        0x07
+#define MASK_ACR_L3MDF_0_PPM                  0x00
+#define MASK_ACR_L3MDF_61_PPM                 0x01
+#define MASK_ACR_L3MDF_122_PPM                0x02
+#define MASK_ACR_L3MDF_244_PPM                0x03
+#define MASK_ACR_L3MDF_488_PPM                0x04
+#define MASK_ACR_L3MDF_976_PPM                0x05
+#define MASK_ACR_L3MDF_1976_PPM               0x06
+#define MASK_ACR_L3MDF_3906_PPM               0x07
+
+#define SDO_MODE1                             0x8652
+#define MASK_SDO_BIT_LENG                     0x70
+#define MASK_SDO_FMT                          0x03
+#define MASK_SDO_FMT_RIGHT                    0x00
+#define MASK_SDO_FMT_LEFT                     0x01
+#define MASK_SDO_FMT_I2S                      0x02
+
+#define DIV_MODE                              0x8665 /* Not in REF_01 */
+#define MASK_DIV_DLY                          0xf0
+#define SET_DIV_DLY_MS(milliseconds)         ((((milliseconds) / 100) << 4) & \
+						MASK_DIV_DLY)
+#define MASK_DIV_MODE                         0x01
+
+#define NCO_F0_MOD                            0x8670
+#define MASK_NCO_F0_MOD                       0x03
+#define MASK_NCO_F0_MOD_42MHZ                 0x00
+#define MASK_NCO_F0_MOD_27MHZ                 0x01
+
+#define PK_INT_MODE                           0x8709
+#define MASK_ISRC2_INT_MODE                   0x80
+#define MASK_ISRC_INT_MODE                    0x40
+#define MASK_ACP_INT_MODE                     0x20
+#define MASK_VS_INT_MODE                      0x10
+#define MASK_SPD_INT_MODE                     0x08
+#define MASK_MS_INT_MODE                      0x04
+#define MASK_AUD_INT_MODE                     0x02
+#define MASK_AVI_INT_MODE                     0x01
+
+#define NO_PKT_LIMIT                          0x870B
+#define MASK_NO_ACP_LIMIT                     0xf0
+#define SET_NO_ACP_LIMIT_MS(milliseconds)    ((((milliseconds) / 80) << 4) & \
+						MASK_NO_ACP_LIMIT)
+#define MASK_NO_AVI_LIMIT                     0x0f
+#define SET_NO_AVI_LIMIT_MS(milliseconds)    (((milliseconds) / 80) & \
+						MASK_NO_AVI_LIMIT)
+
+#define NO_PKT_CLR                            0x870C
+#define MASK_NO_VS_CLR                        0x40
+#define MASK_NO_SPD_CLR                       0x20
+#define MASK_NO_ACP_CLR                       0x10
+#define MASK_NO_AVI_CLR1                      0x02
+#define MASK_NO_AVI_CLR0                      0x01
+
+#define ERR_PK_LIMIT                          0x870D
+#define NO_PKT_LIMIT2                         0x870E
+#define PK_AVI_0HEAD                          0x8710
+#define PK_AVI_1HEAD                          0x8711
+#define PK_AVI_2HEAD                          0x8712
+#define PK_AVI_0BYTE                          0x8713
+#define PK_AVI_1BYTE                          0x8714
+#define PK_AVI_2BYTE                          0x8715
+#define PK_AVI_3BYTE                          0x8716
+#define PK_AVI_4BYTE                          0x8717
+#define PK_AVI_5BYTE                          0x8718
+#define PK_AVI_6BYTE                          0x8719
+#define PK_AVI_7BYTE                          0x871A
+#define PK_AVI_8BYTE                          0x871B
+#define PK_AVI_9BYTE                          0x871C
+#define PK_AVI_10BYTE                         0x871D
+#define PK_AVI_11BYTE                         0x871E
+#define PK_AVI_12BYTE                         0x871F
+#define PK_AVI_13BYTE                         0x8720
+#define PK_AVI_14BYTE                         0x8721
+#define PK_AVI_15BYTE                         0x8722
+#define PK_AVI_16BYTE                         0x8723
+
+#define BKSV                                  0x8800
+
+#define BCAPS                                 0x8840
+#define MASK_HDMI_RSVD                        0x80
+#define MASK_REPEATER                         0x40
+#define MASK_READY                            0x20
+#define MASK_FASTI2C                          0x10
+#define MASK_1_1_FEA                          0x02
+#define MASK_FAST_REAU                        0x01
+
+#define BSTATUS1                              0x8842
+#define MASK_MAX_EXCED                        0x08
+
+#define EDID_RAM                              0x8C00
+#define NO_GDB_LIMIT                          0x9007
+
+#endif
diff --git a/drivers/media/i2c/tc358746.c b/drivers/media/i2c/tc358746.c
new file mode 100644
index 000000000..566f5eadd
--- /dev/null
+++ b/drivers/media/i2c/tc358746.c
@@ -0,0 +1,1692 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * TC358746 - Parallel <-> CSI-2 Bridge
+ *
+ * Copyright 2022 Marco Felsch <kernel@pengutronix.de>
+ *
+ * Notes:
+ *  - Currently only 'Parallel-in -> CSI-out' mode is supported!
+ */
+
+#include <linux/bitfield.h>
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/phy/phy-mipi-dphy.h>
+#include <linux/property.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/units.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-mc.h>
+
+/* 16-bit registers */
+#define CHIPID_REG			0x0000
+#define		CHIPID			GENMASK(15, 8)
+
+#define SYSCTL_REG			0x0002
+#define		SRESET			BIT(0)
+
+#define CONFCTL_REG			0x0004
+#define		PDATAF_MASK		GENMASK(9, 8)
+#define		PDATAF_MODE0		0
+#define		PDATAF_MODE1		1
+#define		PDATAF_MODE2		2
+#define		PDATAF(val)		FIELD_PREP(PDATAF_MASK, (val))
+#define		PPEN			BIT(6)
+#define		DATALANE_MASK		GENMASK(1, 0)
+
+#define FIFOCTL_REG			0x0006
+#define DATAFMT_REG			0x0008
+#define		PDFMT(val)		FIELD_PREP(GENMASK(7, 4), (val))
+
+#define MCLKCTL_REG			0x000c
+#define		MCLK_HIGH_MASK		GENMASK(15, 8)
+#define		MCLK_LOW_MASK		GENMASK(7, 0)
+#define		MCLK_HIGH(val)		FIELD_PREP(MCLK_HIGH_MASK, (val))
+#define		MCLK_LOW(val)		FIELD_PREP(MCLK_LOW_MASK, (val))
+
+#define PLLCTL0_REG			0x0016
+#define		PLL_PRD_MASK		GENMASK(15, 12)
+#define		PLL_PRD(val)		FIELD_PREP(PLL_PRD_MASK, (val))
+#define		PLL_FBD_MASK		GENMASK(8, 0)
+#define		PLL_FBD(val)		FIELD_PREP(PLL_FBD_MASK, (val))
+
+#define PLLCTL1_REG			0x0018
+#define		PLL_FRS_MASK		GENMASK(11, 10)
+#define		PLL_FRS(val)		FIELD_PREP(PLL_FRS_MASK, (val))
+#define		CKEN			BIT(4)
+#define		RESETB			BIT(1)
+#define		PLL_EN			BIT(0)
+
+#define CLKCTL_REG			0x0020
+#define		MCLKDIV_MASK		GENMASK(3, 2)
+#define		MCLKDIV(val)		FIELD_PREP(MCLKDIV_MASK, (val))
+#define		MCLKDIV_8		0
+#define		MCLKDIV_4		1
+#define		MCLKDIV_2		2
+
+#define WORDCNT_REG			0x0022
+#define PP_MISC_REG			0x0032
+#define		FRMSTOP			BIT(15)
+#define		RSTPTR			BIT(14)
+
+/* 32-bit registers */
+#define CLW_DPHYCONTTX_REG		0x0100
+#define CLW_CNTRL_REG			0x0140
+#define D0W_CNTRL_REG			0x0144
+#define		LANEDISABLE		BIT(0)
+
+#define STARTCNTRL_REG			0x0204
+#define		START			BIT(0)
+
+#define LINEINITCNT_REG			0x0210
+#define LPTXTIMECNT_REG			0x0214
+#define TCLK_HEADERCNT_REG		0x0218
+#define		TCLK_ZEROCNT(val)	FIELD_PREP(GENMASK(15, 8), (val))
+#define		TCLK_PREPARECNT(val)	FIELD_PREP(GENMASK(6, 0), (val))
+
+#define TCLK_TRAILCNT_REG		0x021C
+#define THS_HEADERCNT_REG		0x0220
+#define		THS_ZEROCNT(val)	FIELD_PREP(GENMASK(14, 8), (val))
+#define		THS_PREPARECNT(val)	FIELD_PREP(GENMASK(6, 0), (val))
+
+#define TWAKEUP_REG			0x0224
+#define TCLK_POSTCNT_REG		0x0228
+#define THS_TRAILCNT_REG		0x022C
+#define HSTXVREGEN_REG			0x0234
+#define TXOPTIONCNTRL_REG		0x0238
+#define CSI_CONTROL_REG			0x040C
+#define		CSI_MODE		BIT(15)
+#define		TXHSMD			BIT(7)
+#define		NOL(val)		FIELD_PREP(GENMASK(2, 1), (val))
+
+#define CSI_CONFW_REG			0x0500
+#define		MODE(val)		FIELD_PREP(GENMASK(31, 29), (val))
+#define		MODE_SET		0x5
+#define		ADDRESS(val)		FIELD_PREP(GENMASK(28, 24), (val))
+#define		CSI_CONTROL_ADDRESS	0x3
+#define		DATA(val)		FIELD_PREP(GENMASK(15, 0), (val))
+
+#define CSI_START_REG			0x0518
+#define		STRT			BIT(0)
+
+static const struct v4l2_mbus_framefmt tc358746_def_fmt = {
+	.width		= 640,
+	.height		= 480,
+	.code		= MEDIA_BUS_FMT_UYVY8_2X8,
+	.field		= V4L2_FIELD_NONE,
+	.colorspace	= V4L2_COLORSPACE_DEFAULT,
+	.ycbcr_enc	= V4L2_YCBCR_ENC_DEFAULT,
+	.quantization	= V4L2_QUANTIZATION_DEFAULT,
+	.xfer_func	= V4L2_XFER_FUNC_DEFAULT,
+};
+
+static const char * const tc358746_supplies[] = {
+	"vddc", "vddio", "vddmipi"
+};
+
+enum {
+	TC358746_SINK,
+	TC358746_SOURCE,
+	TC358746_NR_PADS
+};
+
+struct tc358746 {
+	struct v4l2_subdev		sd;
+	struct media_pad		pads[TC358746_NR_PADS];
+	struct v4l2_async_notifier	notifier;
+	struct v4l2_fwnode_endpoint	csi_vep;
+
+	struct v4l2_ctrl_handler	ctrl_hdl;
+
+	struct regmap			*regmap;
+	struct clk			*refclk;
+	struct gpio_desc		*reset_gpio;
+	struct regulator_bulk_data	supplies[ARRAY_SIZE(tc358746_supplies)];
+
+	struct clk_hw			mclk_hw;
+	unsigned long			mclk_rate;
+	u8				mclk_prediv;
+	u16				mclk_postdiv;
+
+	unsigned long			pll_rate;
+	u8				pll_post_div;
+	u16				pll_pre_div;
+	u16				pll_mul;
+
+#define TC358746_VB_MAX_SIZE		(511 * 32)
+#define TC358746_VB_DEFAULT_SIZE	  (1 * 32)
+	unsigned int			vb_size; /* Video buffer size in bits */
+
+	struct phy_configure_opts_mipi_dphy dphy_cfg;
+};
+
+static inline struct tc358746 *to_tc358746(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct tc358746, sd);
+}
+
+static inline struct tc358746 *clk_hw_to_tc358746(struct clk_hw *hw)
+{
+	return container_of(hw, struct tc358746, mclk_hw);
+}
+
+struct tc358746_format {
+	u32		code;
+	bool		csi_format;
+	unsigned char	bus_width;
+	unsigned char	bpp;
+	/* Register values */
+	u8		pdformat; /* Peripheral Data Format */
+	u8		pdataf;   /* Parallel Data Format Option */
+};
+
+enum {
+	PDFORMAT_RAW8 = 0,
+	PDFORMAT_RAW10,
+	PDFORMAT_RAW12,
+	PDFORMAT_RGB888,
+	PDFORMAT_RGB666,
+	PDFORMAT_RGB565,
+	PDFORMAT_YUV422_8BIT,
+	/* RESERVED = 7 */
+	PDFORMAT_RAW14 = 8,
+	PDFORMAT_YUV422_10BIT,
+	PDFORMAT_YUV444,
+};
+
+/* Check tc358746_src_mbus_code() if you add new formats */
+static const struct tc358746_format tc358746_formats[] = {
+	{
+		.code = MEDIA_BUS_FMT_UYVY8_2X8,
+		.bus_width = 8,
+		.bpp = 16,
+		.pdformat = PDFORMAT_YUV422_8BIT,
+		.pdataf = PDATAF_MODE0,
+	}, {
+		.code = MEDIA_BUS_FMT_UYVY8_1X16,
+		.csi_format = true,
+		.bus_width = 16,
+		.bpp = 16,
+		.pdformat = PDFORMAT_YUV422_8BIT,
+		.pdataf = PDATAF_MODE1,
+	}, {
+		.code = MEDIA_BUS_FMT_YUYV8_1X16,
+		.csi_format = true,
+		.bus_width = 16,
+		.bpp = 16,
+		.pdformat = PDFORMAT_YUV422_8BIT,
+		.pdataf = PDATAF_MODE2,
+	}, {
+		.code = MEDIA_BUS_FMT_UYVY10_2X10,
+		.bus_width = 10,
+		.bpp = 20,
+		.pdformat = PDFORMAT_YUV422_10BIT,
+		.pdataf = PDATAF_MODE0, /* don't care */
+	}
+};
+
+/* Get n-th format for pad */
+static const struct tc358746_format *
+tc358746_get_format_by_idx(unsigned int pad, unsigned int index)
+{
+	unsigned int idx = 0;
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(tc358746_formats); i++) {
+		const struct tc358746_format *fmt = &tc358746_formats[i];
+
+		if ((pad == TC358746_SOURCE && fmt->csi_format) ||
+		    (pad == TC358746_SINK)) {
+			if (idx == index)
+				return fmt;
+			idx++;
+		}
+	}
+
+	return ERR_PTR(-EINVAL);
+}
+
+static const struct tc358746_format *
+tc358746_get_format_by_code(unsigned int pad, u32 code)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(tc358746_formats); i++) {
+		const struct tc358746_format *fmt = &tc358746_formats[i];
+
+		if (pad == TC358746_SINK && fmt->code == code)
+			return fmt;
+
+		if (pad == TC358746_SOURCE && !fmt->csi_format)
+			continue;
+
+		if (fmt->code == code)
+			return fmt;
+	}
+
+	return ERR_PTR(-EINVAL);
+}
+
+static u32 tc358746_src_mbus_code(u32 code)
+{
+	switch (code) {
+	case MEDIA_BUS_FMT_UYVY8_2X8:
+		return MEDIA_BUS_FMT_UYVY8_1X16;
+	case MEDIA_BUS_FMT_UYVY10_2X10:
+		return MEDIA_BUS_FMT_UYVY10_1X20;
+	default:
+		return code;
+	}
+}
+
+static bool tc358746_valid_reg(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case CHIPID_REG ... CSI_START_REG:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static const struct regmap_config tc358746_regmap_config = {
+	.name = "tc358746",
+	.reg_bits = 16,
+	.val_bits = 16,
+	.max_register = CSI_START_REG,
+	.writeable_reg = tc358746_valid_reg,
+	.readable_reg = tc358746_valid_reg,
+	.reg_format_endian = REGMAP_ENDIAN_BIG,
+	.val_format_endian = REGMAP_ENDIAN_BIG,
+};
+
+static int tc358746_write(struct tc358746 *tc358746, u32 reg, u32 val)
+{
+	size_t count;
+	int err;
+
+	/* 32-bit registers starting from CLW_DPHYCONTTX */
+	count = reg < CLW_DPHYCONTTX_REG ? 1 : 2;
+
+	err = regmap_bulk_write(tc358746->regmap, reg, &val, count);
+	if (err)
+		dev_err(tc358746->sd.dev,
+			"Failed to write reg:0x%04x err:%d\n", reg, err);
+
+	return err;
+}
+
+static int tc358746_read(struct tc358746 *tc358746, u32 reg, u32 *val)
+{
+	size_t count;
+	int err;
+
+	/* 32-bit registers starting from CLW_DPHYCONTTX */
+	count = reg < CLW_DPHYCONTTX_REG ? 1 : 2;
+	*val = 0;
+
+	err = regmap_bulk_read(tc358746->regmap, reg, val, count);
+	if (err)
+		dev_err(tc358746->sd.dev,
+			"Failed to read reg:0x%04x err:%d\n", reg, err);
+
+	return err;
+}
+
+static int
+tc358746_update_bits(struct tc358746 *tc358746, u32 reg, u32 mask, u32 val)
+{
+	u32 tmp, orig;
+	int err;
+
+	err = tc358746_read(tc358746, reg, &orig);
+	if (err)
+		return err;
+
+	tmp = orig & ~mask;
+	tmp |= val & mask;
+
+	return tc358746_write(tc358746, reg, tmp);
+}
+
+static int tc358746_set_bits(struct tc358746 *tc358746, u32 reg, u32 bits)
+{
+	return tc358746_update_bits(tc358746, reg, bits, bits);
+}
+
+static int tc358746_clear_bits(struct tc358746 *tc358746, u32 reg, u32 bits)
+{
+	return tc358746_update_bits(tc358746, reg, bits, 0);
+}
+
+static int tc358746_sw_reset(struct tc358746 *tc358746)
+{
+	int err;
+
+	err = tc358746_set_bits(tc358746, SYSCTL_REG, SRESET);
+	if (err)
+		return err;
+
+	fsleep(10);
+
+	return tc358746_clear_bits(tc358746, SYSCTL_REG, SRESET);
+}
+
+static int
+tc358746_apply_pll_config(struct tc358746 *tc358746)
+{
+	u8 post = tc358746->pll_post_div;
+	u16 pre = tc358746->pll_pre_div;
+	u16 mul = tc358746->pll_mul;
+	u32 val, mask;
+	int err;
+
+	err = tc358746_read(tc358746, PLLCTL1_REG, &val);
+	if (err)
+		return err;
+
+	/* Don't touch the PLL if running */
+	if (FIELD_GET(PLL_EN, val) == 1)
+		return 0;
+
+	/* Pre-div and Multiplicator have a internal +1 logic */
+	val = PLL_PRD(pre - 1) | PLL_FBD(mul - 1);
+	mask = PLL_PRD_MASK | PLL_FBD_MASK;
+	err = tc358746_update_bits(tc358746, PLLCTL0_REG, mask, val);
+	if (err)
+		return err;
+
+	val = PLL_FRS(ilog2(post)) | RESETB | PLL_EN;
+	mask = PLL_FRS_MASK | RESETB | PLL_EN;
+	err = tc358746_update_bits(tc358746, PLLCTL1_REG, mask, val);
+	if (err)
+		return err;
+
+	fsleep(1000);
+
+	return tc358746_set_bits(tc358746, PLLCTL1_REG, CKEN);
+}
+
+static int tc358746_apply_misc_config(struct tc358746 *tc358746)
+{
+	const struct v4l2_mbus_framefmt *mbusfmt;
+	struct v4l2_subdev *sd = &tc358746->sd;
+	struct v4l2_subdev_state *sink_state;
+	const struct tc358746_format *fmt;
+	struct device *dev = sd->dev;
+	u32 val;
+	int err;
+
+	sink_state = v4l2_subdev_lock_and_get_active_state(sd);
+
+	mbusfmt = v4l2_subdev_get_pad_format(sd, sink_state, TC358746_SINK);
+	fmt = tc358746_get_format_by_code(TC358746_SINK, mbusfmt->code);
+
+	/* Self defined CSI user data type id's are not supported yet */
+	val = PDFMT(fmt->pdformat);
+	dev_dbg(dev, "DATAFMT: 0x%x\n", val);
+	err = tc358746_write(tc358746, DATAFMT_REG, val);
+	if (err)
+		goto out;
+
+	val = PDATAF(fmt->pdataf);
+	dev_dbg(dev, "CONFCTL[PDATAF]: 0x%x\n", fmt->pdataf);
+	err = tc358746_update_bits(tc358746, CONFCTL_REG, PDATAF_MASK, val);
+	if (err)
+		goto out;
+
+	val = tc358746->vb_size / 32;
+	dev_dbg(dev, "FIFOCTL: %u (0x%x)\n", val, val);
+	err = tc358746_write(tc358746, FIFOCTL_REG, val);
+	if (err)
+		goto out;
+
+	/* Total number of bytes for each line/width */
+	val = mbusfmt->width * fmt->bpp / 8;
+	dev_dbg(dev, "WORDCNT: %u (0x%x)\n", val, val);
+	err = tc358746_write(tc358746, WORDCNT_REG, val);
+
+out:
+	v4l2_subdev_unlock_state(sink_state);
+
+	return err;
+}
+
+/* Use MHz as base so the div needs no u64 */
+static u32 tc358746_cfg_to_cnt(unsigned int cfg_val,
+			       unsigned int clk_mhz,
+			       unsigned int time_base)
+{
+	return DIV_ROUND_UP(cfg_val * clk_mhz, time_base);
+}
+
+static u32 tc358746_ps_to_cnt(unsigned int cfg_val,
+			      unsigned int clk_mhz)
+{
+	return tc358746_cfg_to_cnt(cfg_val, clk_mhz, USEC_PER_SEC);
+}
+
+static u32 tc358746_us_to_cnt(unsigned int cfg_val,
+			      unsigned int clk_mhz)
+{
+	return tc358746_cfg_to_cnt(cfg_val, clk_mhz, 1);
+}
+
+static int tc358746_apply_dphy_config(struct tc358746 *tc358746)
+{
+	struct phy_configure_opts_mipi_dphy *cfg = &tc358746->dphy_cfg;
+	bool non_cont_clk = !!(tc358746->csi_vep.bus.mipi_csi2.flags &
+			       V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK);
+	struct device *dev = tc358746->sd.dev;
+	unsigned long hs_byte_clk, hf_clk;
+	u32 val, val2, lptxcnt;
+	int err;
+
+	/* The hs_byte_clk is also called SYSCLK in the excel sheet */
+	hs_byte_clk = cfg->hs_clk_rate / 8;
+	hs_byte_clk /= HZ_PER_MHZ;
+	hf_clk = hs_byte_clk / 2;
+
+	val = tc358746_us_to_cnt(cfg->init, hf_clk) - 1;
+	dev_dbg(dev, "LINEINITCNT: %u (0x%x)\n", val, val);
+	err = tc358746_write(tc358746, LINEINITCNT_REG, val);
+	if (err)
+		return err;
+
+	val = tc358746_ps_to_cnt(cfg->lpx, hs_byte_clk) - 1;
+	lptxcnt = val;
+	dev_dbg(dev, "LPTXTIMECNT: %u (0x%x)\n", val, val);
+	err = tc358746_write(tc358746, LPTXTIMECNT_REG, val);
+	if (err)
+		return err;
+
+	val = tc358746_ps_to_cnt(cfg->clk_prepare, hs_byte_clk) - 1;
+	val2 = tc358746_ps_to_cnt(cfg->clk_zero, hs_byte_clk) - 1;
+	dev_dbg(dev, "TCLK_PREPARECNT: %u (0x%x)\n", val, val);
+	dev_dbg(dev, "TCLK_ZEROCNT: %u (0x%x)\n", val2, val2);
+	dev_dbg(dev, "TCLK_HEADERCNT: 0x%x\n",
+		(u32)(TCLK_PREPARECNT(val) | TCLK_ZEROCNT(val2)));
+	err = tc358746_write(tc358746, TCLK_HEADERCNT_REG,
+			     TCLK_PREPARECNT(val) | TCLK_ZEROCNT(val2));
+	if (err)
+		return err;
+
+	val = tc358746_ps_to_cnt(cfg->clk_trail, hs_byte_clk);
+	dev_dbg(dev, "TCLK_TRAILCNT: %u (0x%x)\n", val, val);
+	err = tc358746_write(tc358746, TCLK_TRAILCNT_REG, val);
+	if (err)
+		return err;
+
+	val = tc358746_ps_to_cnt(cfg->hs_prepare, hs_byte_clk) - 1;
+	val2 = tc358746_ps_to_cnt(cfg->hs_zero, hs_byte_clk) - 1;
+	dev_dbg(dev, "THS_PREPARECNT: %u (0x%x)\n", val, val);
+	dev_dbg(dev, "THS_ZEROCNT: %u (0x%x)\n", val2, val2);
+	dev_dbg(dev, "THS_HEADERCNT: 0x%x\n",
+		(u32)(THS_PREPARECNT(val) | THS_ZEROCNT(val2)));
+	err = tc358746_write(tc358746, THS_HEADERCNT_REG,
+			     THS_PREPARECNT(val) | THS_ZEROCNT(val2));
+	if (err)
+		return err;
+
+	/* TWAKEUP > 1ms in lptxcnt steps */
+	val = tc358746_us_to_cnt(cfg->wakeup, hs_byte_clk);
+	val = val / (lptxcnt + 1) - 1;
+	dev_dbg(dev, "TWAKEUP: %u (0x%x)\n", val, val);
+	err = tc358746_write(tc358746, TWAKEUP_REG, val);
+	if (err)
+		return err;
+
+	val = tc358746_ps_to_cnt(cfg->clk_post, hs_byte_clk);
+	dev_dbg(dev, "TCLK_POSTCNT: %u (0x%x)\n", val, val);
+	err = tc358746_write(tc358746, TCLK_POSTCNT_REG, val);
+	if (err)
+		return err;
+
+	val = tc358746_ps_to_cnt(cfg->hs_trail, hs_byte_clk);
+	dev_dbg(dev, "THS_TRAILCNT: %u (0x%x)\n", val, val);
+	err = tc358746_write(tc358746, THS_TRAILCNT_REG, val);
+	if (err)
+		return err;
+
+	dev_dbg(dev, "CONTCLKMODE: %u", non_cont_clk ? 0 : 1);
+
+	return  tc358746_write(tc358746, TXOPTIONCNTRL_REG, non_cont_clk ? 0 : 1);
+}
+
+#define MAX_DATA_LANES 4
+
+static int tc358746_enable_csi_lanes(struct tc358746 *tc358746, int enable)
+{
+	unsigned int lanes = tc358746->dphy_cfg.lanes;
+	unsigned int lane;
+	u32 reg, val;
+	int err;
+
+	err = tc358746_update_bits(tc358746, CONFCTL_REG, DATALANE_MASK,
+				   lanes - 1);
+	if (err)
+		return err;
+
+	/* Clock lane */
+	val = enable ? 0 : LANEDISABLE;
+	dev_dbg(tc358746->sd.dev, "CLW_CNTRL: 0x%x\n", val);
+	err = tc358746_write(tc358746, CLW_CNTRL_REG, val);
+	if (err)
+		return err;
+
+	for (lane = 0; lane < MAX_DATA_LANES; lane++) {
+		/* Data lanes */
+		reg = D0W_CNTRL_REG + lane * 0x4;
+		val = (enable && lane < lanes) ? 0 : LANEDISABLE;
+
+		dev_dbg(tc358746->sd.dev, "D%uW_CNTRL: 0x%x\n", lane, val);
+		err = tc358746_write(tc358746, reg, val);
+		if (err)
+			return err;
+	}
+
+	val = 0;
+	if (enable) {
+		/* Clock lane */
+		val |= BIT(0);
+
+		/* Data lanes */
+		for (lane = 1; lane <= lanes; lane++)
+			val |= BIT(lane);
+	}
+
+	dev_dbg(tc358746->sd.dev, "HSTXVREGEN: 0x%x\n", val);
+
+	return tc358746_write(tc358746, HSTXVREGEN_REG, val);
+}
+
+static int tc358746_enable_csi_module(struct tc358746 *tc358746, int enable)
+{
+	unsigned int lanes = tc358746->dphy_cfg.lanes;
+	int err;
+
+	/*
+	 * START and STRT are only reseted/disabled by sw reset. This is
+	 * required to put the lane state back into LP-11 state. The sw reset
+	 * don't reset register values.
+	 */
+	if (!enable)
+		return tc358746_sw_reset(tc358746);
+
+	err = tc358746_write(tc358746, STARTCNTRL_REG, START);
+	if (err)
+		return err;
+
+	err = tc358746_write(tc358746, CSI_START_REG, STRT);
+	if (err)
+		return err;
+
+	/* CSI_CONTROL_REG is only indirect accessible */
+	return tc358746_write(tc358746, CSI_CONFW_REG,
+			      MODE(MODE_SET) |
+			      ADDRESS(CSI_CONTROL_ADDRESS) |
+			      DATA(CSI_MODE | TXHSMD | NOL(lanes - 1)));
+}
+
+static int tc358746_enable_parallel_port(struct tc358746 *tc358746, int enable)
+{
+	int err;
+
+	if (enable) {
+		err = tc358746_write(tc358746, PP_MISC_REG, 0);
+		if (err)
+			return err;
+
+		return tc358746_set_bits(tc358746, CONFCTL_REG, PPEN);
+	}
+
+	err = tc358746_set_bits(tc358746, PP_MISC_REG, FRMSTOP);
+	if (err)
+		return err;
+
+	err = tc358746_clear_bits(tc358746, CONFCTL_REG, PPEN);
+	if (err)
+		return err;
+
+	return tc358746_set_bits(tc358746, PP_MISC_REG, RSTPTR);
+}
+
+static inline struct v4l2_subdev *tc358746_get_remote_sd(struct media_pad *pad)
+{
+	pad = media_pad_remote_pad_first(pad);
+	if (!pad)
+		return NULL;
+
+	return media_entity_to_v4l2_subdev(pad->entity);
+}
+
+static int tc358746_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct tc358746 *tc358746 = to_tc358746(sd);
+	struct v4l2_subdev *src;
+	int err;
+
+	dev_dbg(sd->dev, "%sable\n", enable ? "en" : "dis");
+
+	src = tc358746_get_remote_sd(&tc358746->pads[TC358746_SINK]);
+	if (!src)
+		return -EPIPE;
+
+	if (enable) {
+		err = pm_runtime_resume_and_get(sd->dev);
+		if (err)
+			return err;
+
+		err = tc358746_apply_dphy_config(tc358746);
+		if (err)
+			goto err_out;
+
+		err = tc358746_apply_misc_config(tc358746);
+		if (err)
+			goto err_out;
+
+		err = tc358746_enable_csi_lanes(tc358746, 1);
+		if (err)
+			goto err_out;
+
+		err = tc358746_enable_csi_module(tc358746, 1);
+		if (err)
+			goto err_out;
+
+		err = tc358746_enable_parallel_port(tc358746, 1);
+		if (err)
+			goto err_out;
+
+		err = v4l2_subdev_call(src, video, s_stream, 1);
+		if (err)
+			goto err_out;
+
+		return 0;
+
+err_out:
+		pm_runtime_mark_last_busy(sd->dev);
+		pm_runtime_put_sync_autosuspend(sd->dev);
+
+		return err;
+	}
+
+	/*
+	 * The lanes must be disabled first (before the csi module) so the
+	 * LP-11 state is entered correctly.
+	 */
+	err = tc358746_enable_csi_lanes(tc358746, 0);
+	if (err)
+		return err;
+
+	err = tc358746_enable_csi_module(tc358746, 0);
+	if (err)
+		return err;
+
+	err = tc358746_enable_parallel_port(tc358746, 0);
+	if (err)
+		return err;
+
+	pm_runtime_mark_last_busy(sd->dev);
+	pm_runtime_put_sync_autosuspend(sd->dev);
+
+	return v4l2_subdev_call(src, video, s_stream, 0);
+}
+
+static int tc358746_init_cfg(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *state)
+{
+	struct v4l2_mbus_framefmt *fmt;
+
+	fmt = v4l2_subdev_get_pad_format(sd, state, TC358746_SINK);
+	*fmt = tc358746_def_fmt;
+
+	fmt = v4l2_subdev_get_pad_format(sd, state, TC358746_SOURCE);
+	*fmt = tc358746_def_fmt;
+	fmt->code = tc358746_src_mbus_code(tc358746_def_fmt.code);
+
+	return 0;
+}
+
+static int tc358746_enum_mbus_code(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_mbus_code_enum *code)
+{
+	const struct tc358746_format *fmt;
+
+	fmt = tc358746_get_format_by_idx(code->pad, code->index);
+	if (IS_ERR(fmt))
+		return PTR_ERR(fmt);
+
+	code->code = fmt->code;
+
+	return 0;
+}
+
+static int tc358746_set_fmt(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_state *sd_state,
+			    struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *src_fmt, *sink_fmt;
+	const struct tc358746_format *fmt;
+
+	/* Source follows the sink */
+	if (format->pad == TC358746_SOURCE)
+		return v4l2_subdev_get_fmt(sd, sd_state, format);
+
+	sink_fmt = v4l2_subdev_get_pad_format(sd, sd_state, TC358746_SINK);
+
+	fmt = tc358746_get_format_by_code(format->pad, format->format.code);
+	if (IS_ERR(fmt))
+		fmt = tc358746_get_format_by_code(format->pad, tc358746_def_fmt.code);
+
+	format->format.code = fmt->code;
+	format->format.field = V4L2_FIELD_NONE;
+
+	dev_dbg(sd->dev, "Update format: %ux%u code:0x%x -> %ux%u code:0x%x",
+		sink_fmt->width, sink_fmt->height, sink_fmt->code,
+		format->format.width, format->format.height, format->format.code);
+
+	*sink_fmt = format->format;
+
+	src_fmt = v4l2_subdev_get_pad_format(sd, sd_state, TC358746_SOURCE);
+	*src_fmt = *sink_fmt;
+	src_fmt->code = tc358746_src_mbus_code(sink_fmt->code);
+
+	return 0;
+}
+
+static unsigned long tc358746_find_pll_settings(struct tc358746 *tc358746,
+						unsigned long refclk,
+						unsigned long fout)
+
+{
+	struct device *dev = tc358746->sd.dev;
+	unsigned long best_freq = 0;
+	u32 min_delta = 0xffffffff;
+	u16 prediv_max = 17;
+	u16 prediv_min = 1;
+	u16 m_best = 0, mul;
+	u16 p_best = 1, p;
+	u8 postdiv;
+
+	if (fout > 1000 * HZ_PER_MHZ) {
+		dev_err(dev, "HS-Clock above 1 Ghz are not supported\n");
+		return 0;
+	}
+
+	if (fout >= 500 * HZ_PER_MHZ)
+		postdiv = 1;
+	else if (fout >= 250 * HZ_PER_MHZ)
+		postdiv = 2;
+	else if (fout >= 125 * HZ_PER_MHZ)
+		postdiv = 4;
+	else
+		postdiv = 8;
+
+	for (p = prediv_min; p <= prediv_max; p++) {
+		unsigned long delta, fin;
+		u64 tmp;
+
+		fin = DIV_ROUND_CLOSEST(refclk, p);
+		if (fin < 4 * HZ_PER_MHZ || fin > 40 * HZ_PER_MHZ)
+			continue;
+
+		tmp = fout * p * postdiv;
+		do_div(tmp, fin);
+		mul = tmp;
+		if (mul > 511)
+			continue;
+
+		tmp = mul * fin;
+		do_div(tmp, p * postdiv);
+
+		delta = abs(fout - tmp);
+		if (delta < min_delta) {
+			p_best = p;
+			m_best = mul;
+			min_delta = delta;
+			best_freq = tmp;
+		}
+
+		if (delta == 0)
+			break;
+	}
+
+	if (!best_freq) {
+		dev_err(dev, "Failed find PLL frequency\n");
+		return 0;
+	}
+
+	tc358746->pll_post_div = postdiv;
+	tc358746->pll_pre_div = p_best;
+	tc358746->pll_mul = m_best;
+
+	if (best_freq != fout)
+		dev_warn(dev, "Request PLL freq:%lu, found PLL freq:%lu\n",
+			 fout, best_freq);
+
+	dev_dbg(dev, "Found PLL settings: freq:%lu prediv:%u multi:%u postdiv:%u\n",
+		best_freq, p_best, m_best, postdiv);
+
+	return best_freq;
+}
+
+#define TC358746_PRECISION 10
+
+static int
+tc358746_link_validate(struct v4l2_subdev *sd, struct media_link *link,
+		       struct v4l2_subdev_format *source_fmt,
+		       struct v4l2_subdev_format *sink_fmt)
+{
+	struct tc358746 *tc358746 = to_tc358746(sd);
+	unsigned long csi_bitrate, source_bitrate;
+	struct v4l2_subdev_state *sink_state;
+	struct v4l2_mbus_framefmt *mbusfmt;
+	const struct tc358746_format *fmt;
+	unsigned int fifo_sz, tmp, n;
+	struct v4l2_subdev *source;
+	s64 source_link_freq;
+	int err;
+
+	err = v4l2_subdev_link_validate_default(sd, link, source_fmt, sink_fmt);
+	if (err)
+		return err;
+
+	sink_state = v4l2_subdev_lock_and_get_active_state(sd);
+	mbusfmt = v4l2_subdev_get_pad_format(sd, sink_state, TC358746_SINK);
+
+	/* Check the FIFO settings */
+	fmt = tc358746_get_format_by_code(TC358746_SINK, mbusfmt->code);
+
+	source = media_entity_to_v4l2_subdev(link->source->entity);
+	source_link_freq = v4l2_get_link_freq(source->ctrl_handler, 0, 0);
+	if (source_link_freq <= 0) {
+		dev_err(tc358746->sd.dev,
+			"Failed to query or invalid source link frequency\n");
+		v4l2_subdev_unlock_state(sink_state);
+		/* Return -EINVAL in case of source_link_freq is 0 */
+		return source_link_freq ? : -EINVAL;
+	}
+	source_bitrate = source_link_freq * fmt->bus_width;
+
+	csi_bitrate = tc358746->dphy_cfg.lanes * tc358746->pll_rate;
+
+	dev_dbg(tc358746->sd.dev,
+		"Fifo settings params: source-bitrate:%lu csi-bitrate:%lu",
+		source_bitrate, csi_bitrate);
+
+	/* Avoid possible FIFO overflows */
+	if (csi_bitrate < source_bitrate) {
+		v4l2_subdev_unlock_state(sink_state);
+		return -EINVAL;
+	}
+
+	/* Best case */
+	if (csi_bitrate == source_bitrate) {
+		fifo_sz = TC358746_VB_DEFAULT_SIZE;
+		tc358746->vb_size = TC358746_VB_DEFAULT_SIZE;
+		goto out;
+	}
+
+	/*
+	 * Avoid possible FIFO underflow in case of
+	 * csi_bitrate > source_bitrate. For such case the chip has a internal
+	 * fifo which can be used to delay the line output.
+	 *
+	 * Fifo size calculation (excluding precision):
+	 *
+	 * fifo-sz, image-width - in bits
+	 * sbr                  - source_bitrate in bits/s
+	 * csir                 - csi_bitrate in bits/s
+	 *
+	 * image-width / csir >= (image-width - fifo-sz) / sbr
+	 * image-width * sbr / csir >= image-width - fifo-sz
+	 * fifo-sz >= image-width - image-width * sbr / csir; with n = csir/sbr
+	 * fifo-sz >= image-width - image-width / n
+	 */
+
+	source_bitrate /= TC358746_PRECISION;
+	n = csi_bitrate / source_bitrate;
+	tmp = (mbusfmt->width * TC358746_PRECISION) / n;
+	fifo_sz = mbusfmt->width - tmp;
+	fifo_sz *= fmt->bpp;
+	tc358746->vb_size = round_up(fifo_sz, 32);
+
+out:
+	dev_dbg(tc358746->sd.dev,
+		"Found FIFO size[bits]:%u -> aligned to size[bits]:%u\n",
+		fifo_sz, tc358746->vb_size);
+
+	v4l2_subdev_unlock_state(sink_state);
+
+	return tc358746->vb_size > TC358746_VB_MAX_SIZE ? -EINVAL : 0;
+}
+
+static int tc358746_get_mbus_config(struct v4l2_subdev *sd, unsigned int pad,
+				    struct v4l2_mbus_config *config)
+{
+	struct tc358746 *tc358746 = to_tc358746(sd);
+
+	if (pad != TC358746_SOURCE)
+		return -EINVAL;
+
+	config->type = V4L2_MBUS_CSI2_DPHY;
+	config->bus.mipi_csi2 = tc358746->csi_vep.bus.mipi_csi2;
+
+	return 0;
+}
+
+static int __maybe_unused
+tc358746_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+{
+	struct tc358746 *tc358746 = to_tc358746(sd);
+	u32 val;
+	int err;
+
+	/* 32-bit registers starting from CLW_DPHYCONTTX */
+	reg->size = reg->reg < CLW_DPHYCONTTX_REG ? 2 : 4;
+
+	if (!pm_runtime_get_if_in_use(sd->dev))
+		return 0;
+
+	err = tc358746_read(tc358746, reg->reg, &val);
+	reg->val = val;
+
+	pm_runtime_mark_last_busy(sd->dev);
+	pm_runtime_put_sync_autosuspend(sd->dev);
+
+	return err;
+}
+
+static int __maybe_unused
+tc358746_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
+{
+	struct tc358746 *tc358746 = to_tc358746(sd);
+
+	if (!pm_runtime_get_if_in_use(sd->dev))
+		return 0;
+
+	tc358746_write(tc358746, (u32)reg->reg, (u32)reg->val);
+
+	pm_runtime_mark_last_busy(sd->dev);
+	pm_runtime_put_sync_autosuspend(sd->dev);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops tc358746_core_ops = {
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = tc358746_g_register,
+	.s_register = tc358746_s_register,
+#endif
+};
+
+static const struct v4l2_subdev_video_ops tc358746_video_ops = {
+	.s_stream = tc358746_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops tc358746_pad_ops = {
+	.init_cfg = tc358746_init_cfg,
+	.enum_mbus_code = tc358746_enum_mbus_code,
+	.set_fmt = tc358746_set_fmt,
+	.get_fmt = v4l2_subdev_get_fmt,
+	.link_validate = tc358746_link_validate,
+	.get_mbus_config = tc358746_get_mbus_config,
+};
+
+static const struct v4l2_subdev_ops tc358746_ops = {
+	.core = &tc358746_core_ops,
+	.video = &tc358746_video_ops,
+	.pad = &tc358746_pad_ops,
+};
+
+static const struct media_entity_operations tc358746_entity_ops = {
+	.get_fwnode_pad = v4l2_subdev_get_fwnode_pad_1_to_1,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static int tc358746_mclk_enable(struct clk_hw *hw)
+{
+	struct tc358746 *tc358746 = clk_hw_to_tc358746(hw);
+	unsigned int div;
+	u32 val;
+	int err;
+
+	div = tc358746->mclk_postdiv / 2;
+	val = MCLK_HIGH(div - 1) | MCLK_LOW(div - 1);
+	dev_dbg(tc358746->sd.dev, "MCLKCTL: %u (0x%x)\n", val, val);
+	err = tc358746_write(tc358746, MCLKCTL_REG, val);
+	if (err)
+		return err;
+
+	if (tc358746->mclk_prediv == 8)
+		val = MCLKDIV(MCLKDIV_8);
+	else if (tc358746->mclk_prediv == 4)
+		val = MCLKDIV(MCLKDIV_4);
+	else
+		val = MCLKDIV(MCLKDIV_2);
+
+	dev_dbg(tc358746->sd.dev, "CLKCTL[MCLKDIV]: %u (0x%x)\n", val, val);
+
+	return tc358746_update_bits(tc358746, CLKCTL_REG, MCLKDIV_MASK, val);
+}
+
+static void tc358746_mclk_disable(struct clk_hw *hw)
+{
+	struct tc358746 *tc358746 = clk_hw_to_tc358746(hw);
+
+	tc358746_write(tc358746, MCLKCTL_REG, 0);
+}
+
+static long
+tc358746_find_mclk_settings(struct tc358746 *tc358746, unsigned long mclk_rate)
+{
+	unsigned long pll_rate = tc358746->pll_rate;
+	const unsigned char prediv[] = { 2, 4, 8 };
+	unsigned int mclk_prediv, mclk_postdiv;
+	struct device *dev = tc358746->sd.dev;
+	unsigned int postdiv, mclkdiv;
+	unsigned long best_mclk_rate;
+	unsigned int i;
+
+	/*
+	 *                          MCLK-Div
+	 *           -------------------´`---------------------
+	 *          ´                                          `
+	 *         +-------------+     +------------------------+
+	 *         | MCLK-PreDiv |     |       MCLK-PostDiv     |
+	 * PLL --> |   (2/4/8)   | --> | (mclk_low + mclk_high) | --> MCLK
+	 *         +-------------+     +------------------------+
+	 *
+	 * The register value of mclk_low/high is mclk_low/high+1, i.e.:
+	 *   mclk_low/high = 1   --> 2 MCLK-Ref Counts
+	 *   mclk_low/high = 255 --> 256 MCLK-Ref Counts == max.
+	 * If mclk_low and mclk_high are 0 then MCLK is disabled.
+	 *
+	 * Keep it simple and support 50/50 duty cycles only for now,
+	 * so the calc will be:
+	 *
+	 *   MCLK = PLL / (MCLK-PreDiv * 2 * MCLK-PostDiv)
+	 */
+
+	if (mclk_rate == tc358746->mclk_rate)
+		return mclk_rate;
+
+	/* Highest possible rate */
+	mclkdiv = pll_rate / mclk_rate;
+	if (mclkdiv <= 8) {
+		mclk_prediv = 2;
+		mclk_postdiv = 4;
+		best_mclk_rate = pll_rate / (2 * 4);
+		goto out;
+	}
+
+	/* First check the prediv */
+	for (i = 0; i < ARRAY_SIZE(prediv); i++) {
+		postdiv = mclkdiv / prediv[i];
+
+		if (postdiv % 2)
+			continue;
+
+		if (postdiv >= 4 && postdiv <= 512) {
+			mclk_prediv = prediv[i];
+			mclk_postdiv = postdiv;
+			best_mclk_rate = pll_rate / (prediv[i] * postdiv);
+			goto out;
+		}
+	}
+
+	/* No suitable prediv found, so try to adjust the postdiv */
+	for (postdiv = 4; postdiv <= 512; postdiv += 2) {
+		unsigned int pre;
+
+		pre = mclkdiv / postdiv;
+		if (pre == 2 || pre == 4 || pre == 8) {
+			mclk_prediv = pre;
+			mclk_postdiv = postdiv;
+			best_mclk_rate = pll_rate / (pre * postdiv);
+			goto out;
+		}
+	}
+
+	/* The MCLK <-> PLL gap is to high -> use largest possible div */
+	mclk_prediv = 8;
+	mclk_postdiv = 512;
+	best_mclk_rate = pll_rate / (8 * 512);
+
+out:
+	tc358746->mclk_prediv = mclk_prediv;
+	tc358746->mclk_postdiv = mclk_postdiv;
+	tc358746->mclk_rate = best_mclk_rate;
+
+	if (best_mclk_rate != mclk_rate)
+		dev_warn(dev, "Request MCLK freq:%lu, found MCLK freq:%lu\n",
+			 mclk_rate, best_mclk_rate);
+
+	dev_dbg(dev, "Found MCLK settings: freq:%lu prediv:%u postdiv:%u\n",
+		best_mclk_rate, mclk_prediv, mclk_postdiv);
+
+	return best_mclk_rate;
+}
+
+static unsigned long
+tc358746_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
+{
+	struct tc358746 *tc358746 = clk_hw_to_tc358746(hw);
+	unsigned int prediv, postdiv;
+	u32 val;
+	int err;
+
+	err = tc358746_read(tc358746, MCLKCTL_REG, &val);
+	if (err)
+		return 0;
+
+	postdiv = FIELD_GET(MCLK_LOW_MASK, val) + 1;
+	postdiv += FIELD_GET(MCLK_HIGH_MASK, val) + 1;
+
+	err = tc358746_read(tc358746, CLKCTL_REG, &val);
+	if (err)
+		return 0;
+
+	prediv = FIELD_GET(MCLKDIV_MASK, val);
+	if (prediv == MCLKDIV_8)
+		prediv = 8;
+	else if (prediv == MCLKDIV_4)
+		prediv = 4;
+	else
+		prediv = 2;
+
+	return tc358746->pll_rate / (prediv * postdiv);
+}
+
+static long tc358746_mclk_round_rate(struct clk_hw *hw, unsigned long rate,
+				     unsigned long *parent_rate)
+{
+	struct tc358746 *tc358746 = clk_hw_to_tc358746(hw);
+
+	*parent_rate = tc358746->pll_rate;
+
+	return tc358746_find_mclk_settings(tc358746, rate);
+}
+
+static int tc358746_mclk_set_rate(struct clk_hw *hw, unsigned long rate,
+				  unsigned long parent_rate)
+{
+	struct tc358746 *tc358746 = clk_hw_to_tc358746(hw);
+
+	tc358746_find_mclk_settings(tc358746, rate);
+
+	return tc358746_mclk_enable(hw);
+}
+
+static const struct clk_ops tc358746_mclk_ops = {
+	.enable = tc358746_mclk_enable,
+	.disable = tc358746_mclk_disable,
+	.recalc_rate = tc358746_recalc_rate,
+	.round_rate = tc358746_mclk_round_rate,
+	.set_rate = tc358746_mclk_set_rate,
+};
+
+static int tc358746_setup_mclk_provider(struct tc358746 *tc358746)
+{
+	struct clk_init_data mclk_initdata = { };
+	struct device *dev = tc358746->sd.dev;
+	const char *mclk_name;
+	int err;
+
+	/* MCLK clk provider support is optional */
+	if (!device_property_present(dev, "#clock-cells"))
+		return 0;
+
+	/* Init to highest possibel MCLK */
+	tc358746->mclk_postdiv = 512;
+	tc358746->mclk_prediv = 8;
+
+	mclk_name = "tc358746-mclk";
+	device_property_read_string(dev, "clock-output-names", &mclk_name);
+
+	mclk_initdata.name = mclk_name;
+	mclk_initdata.ops = &tc358746_mclk_ops;
+	tc358746->mclk_hw.init = &mclk_initdata;
+
+	err = devm_clk_hw_register(dev, &tc358746->mclk_hw);
+	if (err) {
+		dev_err(dev, "Failed to register mclk provider\n");
+		return err;
+	}
+
+	err = devm_of_clk_add_hw_provider(dev, of_clk_hw_simple_get,
+					  &tc358746->mclk_hw);
+	if (err)
+		dev_err(dev, "Failed to add mclk provider\n");
+
+	return err;
+}
+
+static int
+tc358746_init_subdev(struct tc358746 *tc358746, struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = &tc358746->sd;
+	int err;
+
+	v4l2_i2c_subdev_init(sd, client, &tc358746_ops);
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	sd->entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
+	sd->entity.ops = &tc358746_entity_ops;
+
+	tc358746->pads[TC358746_SINK].flags = MEDIA_PAD_FL_SINK;
+	tc358746->pads[TC358746_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
+	err = media_entity_pads_init(&sd->entity, TC358746_NR_PADS,
+				     tc358746->pads);
+	if (err)
+		return err;
+
+	err = v4l2_subdev_init_finalize(sd);
+	if (err)
+		media_entity_cleanup(&sd->entity);
+
+	return err;
+}
+
+static int
+tc358746_init_output_port(struct tc358746 *tc358746, unsigned long refclk)
+{
+	struct device *dev = tc358746->sd.dev;
+	struct v4l2_fwnode_endpoint *vep;
+	unsigned long csi_link_rate;
+	struct fwnode_handle *ep;
+	unsigned char csi_lanes;
+	int err;
+
+	ep = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev), TC358746_SOURCE,
+					     0, 0);
+	if (!ep) {
+		dev_err(dev, "Missing endpoint node\n");
+		return -EINVAL;
+	}
+
+	/* Currently we only support 'parallel in' -> 'csi out' */
+	vep = &tc358746->csi_vep;
+	vep->bus_type = V4L2_MBUS_CSI2_DPHY;
+	err = v4l2_fwnode_endpoint_alloc_parse(ep, vep);
+	fwnode_handle_put(ep);
+	if (err) {
+		dev_err(dev, "Failed to parse source endpoint\n");
+		return err;
+	}
+
+	csi_lanes = vep->bus.mipi_csi2.num_data_lanes;
+	if (csi_lanes == 0 || csi_lanes > 4 ||
+	    vep->nr_of_link_frequencies == 0) {
+		dev_err(dev, "error: Invalid CSI-2 settings\n");
+		err = -EINVAL;
+		goto err;
+	}
+
+	/* TODO: Add support to handle multiple link frequencies */
+	csi_link_rate = (unsigned long)vep->link_frequencies[0];
+	tc358746->pll_rate = tc358746_find_pll_settings(tc358746, refclk,
+							csi_link_rate * 2);
+	if (!tc358746->pll_rate) {
+		err = -EINVAL;
+		goto err;
+	}
+
+	err = phy_mipi_dphy_get_default_config_for_hsclk(tc358746->pll_rate,
+						csi_lanes, &tc358746->dphy_cfg);
+	if (err)
+		goto err;
+
+	tc358746->vb_size = TC358746_VB_DEFAULT_SIZE;
+
+	return 0;
+
+err:
+	v4l2_fwnode_endpoint_free(vep);
+
+	return err;
+}
+
+static int tc358746_init_hw(struct tc358746 *tc358746)
+{
+	struct device *dev = tc358746->sd.dev;
+	unsigned int chipid;
+	u32 val;
+	int err;
+
+	err = pm_runtime_resume_and_get(dev);
+	if (err < 0) {
+		dev_err(dev, "Failed to resume the device\n");
+		return err;
+	}
+
+	 /* Ensure that CSI interface is put into LP-11 state */
+	err = tc358746_sw_reset(tc358746);
+	if (err) {
+		pm_runtime_put_sync(dev);
+		dev_err(dev, "Failed to reset the device\n");
+		return err;
+	}
+
+	err = tc358746_read(tc358746, CHIPID_REG, &val);
+	pm_runtime_mark_last_busy(dev);
+	pm_runtime_put_sync_autosuspend(dev);
+	if (err)
+		return -ENODEV;
+
+	chipid = FIELD_GET(CHIPID, val);
+	if (chipid != 0x44) {
+		dev_err(dev, "Invalid chipid 0x%02x\n", chipid);
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static int tc358746_init_controls(struct tc358746 *tc358746)
+{
+	u64 *link_frequencies = tc358746->csi_vep.link_frequencies;
+	struct v4l2_ctrl *ctrl;
+	int err;
+
+	err = v4l2_ctrl_handler_init(&tc358746->ctrl_hdl, 1);
+	if (err)
+		return err;
+
+	/*
+	 * The driver currently supports only one link-frequency, regardless of
+	 * the input from the firmware, see: tc358746_init_output_port(). So
+	 * report only the first frequency from the array of possible given
+	 * frequencies.
+	 */
+	ctrl = v4l2_ctrl_new_int_menu(&tc358746->ctrl_hdl, NULL,
+				      V4L2_CID_LINK_FREQ, 0, 0,
+				      link_frequencies);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	err = tc358746->ctrl_hdl.error;
+	if (err) {
+		v4l2_ctrl_handler_free(&tc358746->ctrl_hdl);
+		return err;
+	}
+
+	tc358746->sd.ctrl_handler = &tc358746->ctrl_hdl;
+
+	return 0;
+}
+
+static int tc358746_notify_bound(struct v4l2_async_notifier *notifier,
+				 struct v4l2_subdev *sd,
+				 struct v4l2_async_connection *asd)
+{
+	struct tc358746 *tc358746 =
+		container_of(notifier, struct tc358746, notifier);
+	u32 flags = MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE;
+	struct media_pad *sink = &tc358746->pads[TC358746_SINK];
+
+	return v4l2_create_fwnode_links_to_pad(sd, sink, flags);
+}
+
+static const struct v4l2_async_notifier_operations tc358746_notify_ops = {
+	.bound = tc358746_notify_bound,
+};
+
+static int tc358746_async_register(struct tc358746 *tc358746)
+{
+	struct v4l2_fwnode_endpoint vep = {
+		.bus_type = V4L2_MBUS_PARALLEL,
+	};
+	struct v4l2_async_connection *asd;
+	struct fwnode_handle *ep;
+	int err;
+
+	ep = fwnode_graph_get_endpoint_by_id(dev_fwnode(tc358746->sd.dev),
+					     TC358746_SINK, 0, 0);
+	if (!ep)
+		return -ENOTCONN;
+
+	err = v4l2_fwnode_endpoint_parse(ep, &vep);
+	if (err) {
+		fwnode_handle_put(ep);
+		return err;
+	}
+
+	v4l2_async_subdev_nf_init(&tc358746->notifier, &tc358746->sd);
+	asd = v4l2_async_nf_add_fwnode_remote(&tc358746->notifier, ep,
+					      struct v4l2_async_connection);
+	fwnode_handle_put(ep);
+
+	if (IS_ERR(asd)) {
+		err = PTR_ERR(asd);
+		goto err_cleanup;
+	}
+
+	tc358746->notifier.ops = &tc358746_notify_ops;
+
+	err = v4l2_async_nf_register(&tc358746->notifier);
+	if (err)
+		goto err_cleanup;
+
+	err = v4l2_async_register_subdev(&tc358746->sd);
+	if (err)
+		goto err_unregister;
+
+	return 0;
+
+err_unregister:
+	v4l2_async_nf_unregister(&tc358746->notifier);
+err_cleanup:
+	v4l2_async_nf_cleanup(&tc358746->notifier);
+
+	return err;
+}
+
+static int tc358746_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct tc358746 *tc358746;
+	unsigned long refclk;
+	unsigned int i;
+	int err;
+
+	tc358746 = devm_kzalloc(&client->dev, sizeof(*tc358746), GFP_KERNEL);
+	if (!tc358746)
+		return -ENOMEM;
+
+	tc358746->regmap = devm_regmap_init_i2c(client, &tc358746_regmap_config);
+	if (IS_ERR(tc358746->regmap))
+		return dev_err_probe(dev, PTR_ERR(tc358746->regmap),
+				     "Failed to init regmap\n");
+
+	tc358746->refclk = devm_clk_get(dev, "refclk");
+	if (IS_ERR(tc358746->refclk))
+		return dev_err_probe(dev, PTR_ERR(tc358746->refclk),
+				     "Failed to get refclk\n");
+
+	err = clk_prepare_enable(tc358746->refclk);
+	if (err)
+		return dev_err_probe(dev, err,
+				     "Failed to enable refclk\n");
+
+	refclk = clk_get_rate(tc358746->refclk);
+	clk_disable_unprepare(tc358746->refclk);
+
+	if (refclk < 6 * HZ_PER_MHZ || refclk > 40 * HZ_PER_MHZ)
+		return dev_err_probe(dev, -EINVAL, "Invalid refclk range\n");
+
+	for (i = 0; i < ARRAY_SIZE(tc358746_supplies); i++)
+		tc358746->supplies[i].supply = tc358746_supplies[i];
+
+	err = devm_regulator_bulk_get(dev, ARRAY_SIZE(tc358746_supplies),
+				      tc358746->supplies);
+	if (err)
+		return dev_err_probe(dev, err, "Failed to get supplies\n");
+
+	tc358746->reset_gpio = devm_gpiod_get_optional(dev, "reset",
+						       GPIOD_OUT_HIGH);
+	if (IS_ERR(tc358746->reset_gpio))
+		return dev_err_probe(dev, PTR_ERR(tc358746->reset_gpio),
+				     "Failed to get reset-gpios\n");
+
+	err = tc358746_init_subdev(tc358746, client);
+	if (err)
+		return dev_err_probe(dev, err, "Failed to init subdev\n");
+
+	err = tc358746_init_output_port(tc358746, refclk);
+	if (err)
+		goto err_subdev;
+
+	/*
+	 * Keep this order since we need the output port link-frequencies
+	 * information.
+	 */
+	err = tc358746_init_controls(tc358746);
+	if (err)
+		goto err_fwnode;
+
+	dev_set_drvdata(dev, tc358746);
+
+	/* Set to 1sec to give the stream reconfiguration enough time */
+	pm_runtime_set_autosuspend_delay(dev, 1000);
+	pm_runtime_use_autosuspend(dev);
+	pm_runtime_enable(dev);
+
+	err = tc358746_init_hw(tc358746);
+	if (err)
+		goto err_pm;
+
+	err = tc358746_setup_mclk_provider(tc358746);
+	if (err)
+		goto err_pm;
+
+	err = tc358746_async_register(tc358746);
+	if (err < 0)
+		goto err_pm;
+
+	dev_dbg(dev, "%s found @ 0x%x (%s)\n", client->name,
+		client->addr, client->adapter->name);
+
+	return 0;
+
+err_pm:
+	pm_runtime_disable(dev);
+	pm_runtime_set_suspended(dev);
+	pm_runtime_dont_use_autosuspend(dev);
+	v4l2_ctrl_handler_free(&tc358746->ctrl_hdl);
+err_fwnode:
+	v4l2_fwnode_endpoint_free(&tc358746->csi_vep);
+err_subdev:
+	v4l2_subdev_cleanup(&tc358746->sd);
+	media_entity_cleanup(&tc358746->sd.entity);
+
+	return err;
+}
+
+static void tc358746_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct tc358746 *tc358746 = to_tc358746(sd);
+
+	v4l2_subdev_cleanup(sd);
+	v4l2_ctrl_handler_free(&tc358746->ctrl_hdl);
+	v4l2_fwnode_endpoint_free(&tc358746->csi_vep);
+	v4l2_async_nf_unregister(&tc358746->notifier);
+	v4l2_async_nf_cleanup(&tc358746->notifier);
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+
+	pm_runtime_disable(sd->dev);
+	pm_runtime_set_suspended(sd->dev);
+	pm_runtime_dont_use_autosuspend(sd->dev);
+}
+
+static int tc358746_suspend(struct device *dev)
+{
+	struct tc358746 *tc358746 = dev_get_drvdata(dev);
+	int err;
+
+	clk_disable_unprepare(tc358746->refclk);
+
+	err = regulator_bulk_disable(ARRAY_SIZE(tc358746_supplies),
+				     tc358746->supplies);
+	if (err)
+		clk_prepare_enable(tc358746->refclk);
+
+	return err;
+}
+
+static int tc358746_resume(struct device *dev)
+{
+	struct tc358746 *tc358746 = dev_get_drvdata(dev);
+	int err;
+
+	gpiod_set_value(tc358746->reset_gpio, 1);
+
+	err = regulator_bulk_enable(ARRAY_SIZE(tc358746_supplies),
+				    tc358746->supplies);
+	if (err)
+		return err;
+
+	/* min. 200ns */
+	usleep_range(10, 20);
+
+	gpiod_set_value(tc358746->reset_gpio, 0);
+
+	err = clk_prepare_enable(tc358746->refclk);
+	if (err)
+		goto err;
+
+	/* min. 700us ... 1ms */
+	usleep_range(1000, 1500);
+
+	/*
+	 * Enable the PLL here since it can be called by the clk-framework or by
+	 * the .s_stream() callback. So this is the common place for both.
+	 */
+	err = tc358746_apply_pll_config(tc358746);
+	if (err)
+		goto err_clk;
+
+	return 0;
+
+err_clk:
+	clk_disable_unprepare(tc358746->refclk);
+err:
+	regulator_bulk_disable(ARRAY_SIZE(tc358746_supplies),
+			       tc358746->supplies);
+	return err;
+}
+
+static DEFINE_RUNTIME_DEV_PM_OPS(tc358746_pm_ops, tc358746_suspend,
+				 tc358746_resume, NULL);
+
+static const struct of_device_id __maybe_unused tc358746_of_match[] = {
+	{ .compatible = "toshiba,tc358746" },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, tc358746_of_match);
+
+static struct i2c_driver tc358746_driver = {
+	.driver = {
+		.name = "tc358746",
+		.pm = pm_ptr(&tc358746_pm_ops),
+		.of_match_table = tc358746_of_match,
+	},
+	.probe = tc358746_probe,
+	.remove = tc358746_remove,
+};
+
+module_i2c_driver(tc358746_driver);
+
+MODULE_DESCRIPTION("Toshiba TC358746 Parallel to CSI-2 bridge driver");
+MODULE_AUTHOR("Marco Felsch <kernel@pengutronix.de>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/tda1997x.c b/drivers/media/i2c/tda1997x.c
new file mode 100644
index 000000000..325e99125
--- /dev/null
+++ b/drivers/media/i2c/tda1997x.c
@@ -0,0 +1,2846 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2018 Gateworks Corporation
+ */
+#include <linux/delay.h>
+#include <linux/hdmi.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of_graph.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/types.h>
+#include <linux/v4l2-dv-timings.h>
+#include <linux/videodev2.h>
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-dv-timings.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+#include <media/i2c/tda1997x.h>
+
+#include <sound/core.h>
+#include <sound/pcm.h>
+#include <sound/pcm_params.h>
+#include <sound/soc.h>
+
+#include <dt-bindings/media/tda1997x.h>
+
+#include "tda1997x_regs.h"
+
+#define TDA1997X_MBUS_CODES	5
+
+/* debug level */
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "debug level (0-2)");
+
+/* Audio formats */
+static const char * const audtype_names[] = {
+	"PCM",			/* PCM Samples */
+	"HBR",			/* High Bit Rate Audio */
+	"OBA",			/* One-Bit Audio */
+	"DST"			/* Direct Stream Transfer */
+};
+
+/* Audio output port formats */
+enum audfmt_types {
+	AUDFMT_TYPE_DISABLED = 0,
+	AUDFMT_TYPE_I2S,
+	AUDFMT_TYPE_SPDIF,
+};
+static const char * const audfmt_names[] = {
+	"Disabled",
+	"I2S",
+	"SPDIF",
+};
+
+/* Video input formats */
+static const char * const hdmi_colorspace_names[] = {
+	"RGB", "YUV422", "YUV444", "YUV420", "", "", "", "",
+};
+static const char * const hdmi_colorimetry_names[] = {
+	"", "ITU601", "ITU709", "Extended",
+};
+static const char * const v4l2_quantization_names[] = {
+	"Default",
+	"Full Range (0-255)",
+	"Limited Range (16-235)",
+};
+
+/* Video output port formats */
+static const char * const vidfmt_names[] = {
+	"RGB444/YUV444",	/* RGB/YUV444 16bit data bus, 8bpp */
+	"YUV422 semi-planar",	/* YUV422 16bit data base, 8bpp */
+	"YUV422 CCIR656",	/* BT656 (YUV 8bpp 2 clock per pixel) */
+	"Invalid",
+};
+
+/*
+ * Colorspace conversion matrices
+ */
+struct color_matrix_coefs {
+	const char *name;
+	/* Input offsets */
+	s16 offint1;
+	s16 offint2;
+	s16 offint3;
+	/* Coeficients */
+	s16 p11coef;
+	s16 p12coef;
+	s16 p13coef;
+	s16 p21coef;
+	s16 p22coef;
+	s16 p23coef;
+	s16 p31coef;
+	s16 p32coef;
+	s16 p33coef;
+	/* Output offsets */
+	s16 offout1;
+	s16 offout2;
+	s16 offout3;
+};
+
+enum {
+	ITU709_RGBFULL,
+	ITU601_RGBFULL,
+	RGBLIMITED_RGBFULL,
+	RGBLIMITED_ITU601,
+	RGBLIMITED_ITU709,
+	RGBFULL_ITU601,
+	RGBFULL_ITU709,
+};
+
+/* NB: 4096 is 1.0 using fixed point numbers */
+static const struct color_matrix_coefs conv_matrix[] = {
+	{
+		"YUV709 -> RGB full",
+		 -256, -2048,  -2048,
+		 4769, -2183,   -873,
+		 4769,  7343,      0,
+		 4769,     0,   8652,
+		    0,     0,      0,
+	},
+	{
+		"YUV601 -> RGB full",
+		 -256, -2048,  -2048,
+		 4769, -3330,  -1602,
+		 4769,  6538,      0,
+		 4769,     0,   8264,
+		  256,   256,    256,
+	},
+	{
+		"RGB limited -> RGB full",
+		 -256,  -256,   -256,
+		    0,  4769,      0,
+		    0,     0,   4769,
+		 4769,     0,      0,
+		    0,     0,      0,
+	},
+	{
+		"RGB limited -> ITU601",
+		 -256,  -256,   -256,
+		 2404,  1225,    467,
+		-1754,  2095,   -341,
+		-1388,  -707,   2095,
+		  256,  2048,   2048,
+	},
+	{
+		"RGB limited -> ITU709",
+		 -256,  -256,   -256,
+		 2918,   867,    295,
+		-1894,  2087,   -190,
+		-1607,  -477,   2087,
+		  256,  2048,   2048,
+	},
+	{
+		"RGB full -> ITU601",
+		    0,     0,      0,
+		 2065,  1052,    401,
+		-1506,  1799,   -293,
+		-1192,  -607,   1799,
+		  256,  2048,   2048,
+	},
+	{
+		"RGB full -> ITU709",
+		    0,     0,      0,
+		 2506,   745,    253,
+		-1627,  1792,   -163,
+		-1380,  -410,   1792,
+		  256,  2048,   2048,
+	},
+};
+
+static const struct v4l2_dv_timings_cap tda1997x_dv_timings_cap = {
+	.type = V4L2_DV_BT_656_1120,
+	/* keep this initialization for compatibility with GCC < 4.4.6 */
+	.reserved = { 0 },
+
+	V4L2_INIT_BT_TIMINGS(
+		640, 1920,			/* min/max width */
+		350, 1200,			/* min/max height */
+		13000000, 165000000,		/* min/max pixelclock */
+		/* standards */
+		V4L2_DV_BT_STD_CEA861 | V4L2_DV_BT_STD_DMT |
+			V4L2_DV_BT_STD_GTF | V4L2_DV_BT_STD_CVT,
+		/* capabilities */
+		V4L2_DV_BT_CAP_INTERLACED | V4L2_DV_BT_CAP_PROGRESSIVE |
+			V4L2_DV_BT_CAP_REDUCED_BLANKING |
+			V4L2_DV_BT_CAP_CUSTOM
+	)
+};
+
+/* regulator supplies */
+static const char * const tda1997x_supply_name[] = {
+	"DOVDD", /* Digital I/O supply */
+	"DVDD",  /* Digital Core supply */
+	"AVDD",  /* Analog supply */
+};
+
+#define TDA1997X_NUM_SUPPLIES ARRAY_SIZE(tda1997x_supply_name)
+
+enum tda1997x_type {
+	TDA19971,
+	TDA19973,
+};
+
+enum tda1997x_hdmi_pads {
+	TDA1997X_PAD_SOURCE,
+	TDA1997X_NUM_PADS,
+};
+
+struct tda1997x_chip_info {
+	enum tda1997x_type type;
+	const char *name;
+};
+
+struct tda1997x_state {
+	const struct tda1997x_chip_info *info;
+	struct tda1997x_platform_data pdata;
+	struct i2c_client *client;
+	struct i2c_client *client_cec;
+	struct v4l2_subdev sd;
+	struct regulator_bulk_data supplies[TDA1997X_NUM_SUPPLIES];
+	struct media_pad pads[TDA1997X_NUM_PADS];
+	struct mutex lock;
+	struct mutex page_lock;
+	char page;
+
+	/* detected info from chip */
+	int chip_revision;
+	char port_30bit;
+	char output_2p5;
+	char tmdsb_clk;
+	char tmdsb_soc;
+
+	/* status info */
+	char hdmi_status;
+	char mptrw_in_progress;
+	char activity_status;
+	char input_detect[2];
+
+	/* video */
+	struct hdmi_avi_infoframe avi_infoframe;
+	struct v4l2_hdmi_colorimetry colorimetry;
+	u32 rgb_quantization_range;
+	struct v4l2_dv_timings timings;
+	int fps;
+	const struct color_matrix_coefs *conv;
+	u32 mbus_codes[TDA1997X_MBUS_CODES];	/* available modes */
+	u32 mbus_code;		/* current mode */
+	u8 vid_fmt;
+
+	/* controls */
+	struct v4l2_ctrl_handler hdl;
+	struct v4l2_ctrl *detect_tx_5v_ctrl;
+	struct v4l2_ctrl *rgb_quantization_range_ctrl;
+
+	/* audio */
+	u8  audio_ch_alloc;
+	int audio_samplerate;
+	int audio_channels;
+	int audio_samplesize;
+	int audio_type;
+	struct mutex audio_lock;
+	struct snd_pcm_substream *audio_stream;
+
+	/* EDID */
+	struct {
+		u8 edid[256];
+		u32 present;
+		unsigned int blocks;
+	} edid;
+	struct delayed_work delayed_work_enable_hpd;
+};
+
+static const struct v4l2_event tda1997x_ev_fmt = {
+	.type = V4L2_EVENT_SOURCE_CHANGE,
+	.u.src_change.changes = V4L2_EVENT_SRC_CH_RESOLUTION,
+};
+
+static const struct tda1997x_chip_info tda1997x_chip_info[] = {
+	[TDA19971] = {
+		.type = TDA19971,
+		.name = "tda19971",
+	},
+	[TDA19973] = {
+		.type = TDA19973,
+		.name = "tda19973",
+	},
+};
+
+static inline struct tda1997x_state *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct tda1997x_state, sd);
+}
+
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct tda1997x_state, hdl)->sd;
+}
+
+static int tda1997x_cec_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct tda1997x_state *state = to_state(sd);
+	int val;
+
+	val = i2c_smbus_read_byte_data(state->client_cec, reg);
+	if (val < 0) {
+		v4l_err(state->client, "read reg error: reg=%2x\n", reg);
+		val = -1;
+	}
+
+	return val;
+}
+
+static int tda1997x_cec_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct tda1997x_state *state = to_state(sd);
+	int ret = 0;
+
+	ret = i2c_smbus_write_byte_data(state->client_cec, reg, val);
+	if (ret < 0) {
+		v4l_err(state->client, "write reg error:reg=%2x,val=%2x\n",
+			reg, val);
+		ret = -1;
+	}
+
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * I2C transfer
+ */
+
+static int tda1997x_setpage(struct v4l2_subdev *sd, u8 page)
+{
+	struct tda1997x_state *state = to_state(sd);
+	int ret;
+
+	if (state->page != page) {
+		ret = i2c_smbus_write_byte_data(state->client,
+			REG_CURPAGE_00H, page);
+		if (ret < 0) {
+			v4l_err(state->client,
+				"write reg error:reg=%2x,val=%2x\n",
+				REG_CURPAGE_00H, page);
+			return ret;
+		}
+		state->page = page;
+	}
+	return 0;
+}
+
+static inline int io_read(struct v4l2_subdev *sd, u16 reg)
+{
+	struct tda1997x_state *state = to_state(sd);
+	int val;
+
+	mutex_lock(&state->page_lock);
+	if (tda1997x_setpage(sd, reg >> 8)) {
+		val = -1;
+		goto out;
+	}
+
+	val = i2c_smbus_read_byte_data(state->client, reg&0xff);
+	if (val < 0) {
+		v4l_err(state->client, "read reg error: reg=%2x\n", reg & 0xff);
+		val = -1;
+		goto out;
+	}
+
+out:
+	mutex_unlock(&state->page_lock);
+	return val;
+}
+
+static inline long io_read16(struct v4l2_subdev *sd, u16 reg)
+{
+	int val;
+	long lval = 0;
+
+	val = io_read(sd, reg);
+	if (val < 0)
+		return val;
+	lval |= (val << 8);
+	val = io_read(sd, reg + 1);
+	if (val < 0)
+		return val;
+	lval |= val;
+
+	return lval;
+}
+
+static inline long io_read24(struct v4l2_subdev *sd, u16 reg)
+{
+	int val;
+	long lval = 0;
+
+	val = io_read(sd, reg);
+	if (val < 0)
+		return val;
+	lval |= (val << 16);
+	val = io_read(sd, reg + 1);
+	if (val < 0)
+		return val;
+	lval |= (val << 8);
+	val = io_read(sd, reg + 2);
+	if (val < 0)
+		return val;
+	lval |= val;
+
+	return lval;
+}
+
+static unsigned int io_readn(struct v4l2_subdev *sd, u16 reg, u8 len, u8 *data)
+{
+	int i;
+	int sz = 0;
+	int val;
+
+	for (i = 0; i < len; i++) {
+		val = io_read(sd, reg + i);
+		if (val < 0)
+			break;
+		data[i] = val;
+		sz++;
+	}
+
+	return sz;
+}
+
+static int io_write(struct v4l2_subdev *sd, u16 reg, u8 val)
+{
+	struct tda1997x_state *state = to_state(sd);
+	s32 ret = 0;
+
+	mutex_lock(&state->page_lock);
+	if (tda1997x_setpage(sd, reg >> 8)) {
+		ret = -1;
+		goto out;
+	}
+
+	ret = i2c_smbus_write_byte_data(state->client, reg & 0xff, val);
+	if (ret < 0) {
+		v4l_err(state->client, "write reg error:reg=%2x,val=%2x\n",
+			reg&0xff, val);
+		ret = -1;
+		goto out;
+	}
+
+out:
+	mutex_unlock(&state->page_lock);
+	return ret;
+}
+
+static int io_write16(struct v4l2_subdev *sd, u16 reg, u16 val)
+{
+	int ret;
+
+	ret = io_write(sd, reg, (val >> 8) & 0xff);
+	if (ret < 0)
+		return ret;
+	ret = io_write(sd, reg + 1, val & 0xff);
+	if (ret < 0)
+		return ret;
+	return 0;
+}
+
+static int io_write24(struct v4l2_subdev *sd, u16 reg, u32 val)
+{
+	int ret;
+
+	ret = io_write(sd, reg, (val >> 16) & 0xff);
+	if (ret < 0)
+		return ret;
+	ret = io_write(sd, reg + 1, (val >> 8) & 0xff);
+	if (ret < 0)
+		return ret;
+	ret = io_write(sd, reg + 2, val & 0xff);
+	if (ret < 0)
+		return ret;
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * Hotplug
+ */
+
+enum hpd_mode {
+	HPD_LOW_BP,	/* HPD low and pulse of at least 100ms */
+	HPD_LOW_OTHER,	/* HPD low and pulse of at least 100ms */
+	HPD_HIGH_BP,	/* HIGH */
+	HPD_HIGH_OTHER,
+	HPD_PULSE,	/* HPD low pulse */
+};
+
+/* manual HPD (Hot Plug Detect) control */
+static int tda1997x_manual_hpd(struct v4l2_subdev *sd, enum hpd_mode mode)
+{
+	u8 hpd_auto, hpd_pwr, hpd_man;
+
+	hpd_auto = io_read(sd, REG_HPD_AUTO_CTRL);
+	hpd_pwr = io_read(sd, REG_HPD_POWER);
+	hpd_man = io_read(sd, REG_HPD_MAN_CTRL);
+
+	/* mask out unused bits */
+	hpd_man &= (HPD_MAN_CTRL_HPD_PULSE |
+		    HPD_MAN_CTRL_5VEN |
+		    HPD_MAN_CTRL_HPD_B |
+		    HPD_MAN_CTRL_HPD_A);
+
+	switch (mode) {
+	/* HPD low and pulse of at least 100ms */
+	case HPD_LOW_BP:
+		/* hpd_bp=0 */
+		hpd_pwr &= ~HPD_POWER_BP_MASK;
+		/* disable HPD_A and HPD_B */
+		hpd_man &= ~(HPD_MAN_CTRL_HPD_A | HPD_MAN_CTRL_HPD_B);
+		io_write(sd, REG_HPD_POWER, hpd_pwr);
+		io_write(sd, REG_HPD_MAN_CTRL, hpd_man);
+		break;
+	/* HPD high */
+	case HPD_HIGH_BP:
+		/* hpd_bp=1 */
+		hpd_pwr &= ~HPD_POWER_BP_MASK;
+		hpd_pwr |= 1 << HPD_POWER_BP_SHIFT;
+		io_write(sd, REG_HPD_POWER, hpd_pwr);
+		break;
+	/* HPD low and pulse of at least 100ms */
+	case HPD_LOW_OTHER:
+		/* disable HPD_A and HPD_B */
+		hpd_man &= ~(HPD_MAN_CTRL_HPD_A | HPD_MAN_CTRL_HPD_B);
+		/* hp_other=0 */
+		hpd_auto &= ~HPD_AUTO_HP_OTHER;
+		io_write(sd, REG_HPD_AUTO_CTRL, hpd_auto);
+		io_write(sd, REG_HPD_MAN_CTRL, hpd_man);
+		break;
+	/* HPD high */
+	case HPD_HIGH_OTHER:
+		hpd_auto |= HPD_AUTO_HP_OTHER;
+		io_write(sd, REG_HPD_AUTO_CTRL, hpd_auto);
+		break;
+	/* HPD low pulse */
+	case HPD_PULSE:
+		/* disable HPD_A and HPD_B */
+		hpd_man &= ~(HPD_MAN_CTRL_HPD_A | HPD_MAN_CTRL_HPD_B);
+		io_write(sd, REG_HPD_MAN_CTRL, hpd_man);
+		break;
+	}
+
+	return 0;
+}
+
+static void tda1997x_delayed_work_enable_hpd(struct work_struct *work)
+{
+	struct delayed_work *dwork = to_delayed_work(work);
+	struct tda1997x_state *state = container_of(dwork,
+						    struct tda1997x_state,
+						    delayed_work_enable_hpd);
+	struct v4l2_subdev *sd = &state->sd;
+
+	v4l2_dbg(2, debug, sd, "%s\n", __func__);
+
+	/* Set HPD high */
+	tda1997x_manual_hpd(sd, HPD_HIGH_OTHER);
+	tda1997x_manual_hpd(sd, HPD_HIGH_BP);
+
+	state->edid.present = 1;
+}
+
+static void tda1997x_disable_edid(struct v4l2_subdev *sd)
+{
+	struct tda1997x_state *state = to_state(sd);
+
+	v4l2_dbg(1, debug, sd, "%s\n", __func__);
+	cancel_delayed_work_sync(&state->delayed_work_enable_hpd);
+
+	/* Set HPD low */
+	tda1997x_manual_hpd(sd, HPD_LOW_BP);
+}
+
+static void tda1997x_enable_edid(struct v4l2_subdev *sd)
+{
+	struct tda1997x_state *state = to_state(sd);
+
+	v4l2_dbg(1, debug, sd, "%s\n", __func__);
+
+	/* Enable hotplug after 100ms */
+	schedule_delayed_work(&state->delayed_work_enable_hpd, HZ / 10);
+}
+
+/* -----------------------------------------------------------------------------
+ * Signal Control
+ */
+
+/*
+ * configure vid_fmt based on mbus_code
+ */
+static int
+tda1997x_setup_format(struct tda1997x_state *state, u32 code)
+{
+	v4l_dbg(1, debug, state->client, "%s code=0x%x\n", __func__, code);
+	switch (code) {
+	case MEDIA_BUS_FMT_RGB121212_1X36:
+	case MEDIA_BUS_FMT_RGB888_1X24:
+	case MEDIA_BUS_FMT_YUV12_1X36:
+	case MEDIA_BUS_FMT_YUV8_1X24:
+		state->vid_fmt = OF_FMT_444;
+		break;
+	case MEDIA_BUS_FMT_UYVY12_1X24:
+	case MEDIA_BUS_FMT_UYVY10_1X20:
+	case MEDIA_BUS_FMT_UYVY8_1X16:
+		state->vid_fmt = OF_FMT_422_SMPT;
+		break;
+	case MEDIA_BUS_FMT_UYVY12_2X12:
+	case MEDIA_BUS_FMT_UYVY10_2X10:
+	case MEDIA_BUS_FMT_UYVY8_2X8:
+		state->vid_fmt = OF_FMT_422_CCIR;
+		break;
+	default:
+		v4l_err(state->client, "incompatible format (0x%x)\n", code);
+		return -EINVAL;
+	}
+	v4l_dbg(1, debug, state->client, "%s code=0x%x fmt=%s\n", __func__,
+		code, vidfmt_names[state->vid_fmt]);
+	state->mbus_code = code;
+
+	return 0;
+}
+
+/*
+ * The color conversion matrix will convert between the colorimetry of the
+ * HDMI input to the desired output format RGB|YUV. RGB output is to be
+ * full-range and YUV is to be limited range.
+ *
+ * RGB full-range uses values from 0 to 255 which is recommended on a monitor
+ * and RGB Limited uses values from 16 to 236 (16=black, 235=white) which is
+ * typically recommended on a TV.
+ */
+static void
+tda1997x_configure_csc(struct v4l2_subdev *sd)
+{
+	struct tda1997x_state *state = to_state(sd);
+	struct hdmi_avi_infoframe *avi = &state->avi_infoframe;
+	struct v4l2_hdmi_colorimetry *c = &state->colorimetry;
+	/* Blanking code values depend on output colorspace (RGB or YUV) */
+	struct blanking_codes {
+		s16 code_gy;
+		s16 code_bu;
+		s16 code_rv;
+	};
+	static const struct blanking_codes rgb_blanking = { 64, 64, 64 };
+	static const struct blanking_codes yuv_blanking = { 64, 512, 512 };
+	const struct blanking_codes *blanking_codes = NULL;
+	u8 reg;
+
+	v4l_dbg(1, debug, state->client, "input:%s quant:%s output:%s\n",
+		hdmi_colorspace_names[avi->colorspace],
+		v4l2_quantization_names[c->quantization],
+		vidfmt_names[state->vid_fmt]);
+	state->conv = NULL;
+	switch (state->vid_fmt) {
+	/* RGB output */
+	case OF_FMT_444:
+		blanking_codes = &rgb_blanking;
+		if (c->colorspace == V4L2_COLORSPACE_SRGB) {
+			if (c->quantization == V4L2_QUANTIZATION_LIM_RANGE)
+				state->conv = &conv_matrix[RGBLIMITED_RGBFULL];
+		} else {
+			if (c->colorspace == V4L2_COLORSPACE_REC709)
+				state->conv = &conv_matrix[ITU709_RGBFULL];
+			else if (c->colorspace == V4L2_COLORSPACE_SMPTE170M)
+				state->conv = &conv_matrix[ITU601_RGBFULL];
+		}
+		break;
+
+	/* YUV output */
+	case OF_FMT_422_SMPT: /* semi-planar */
+	case OF_FMT_422_CCIR: /* CCIR656 */
+		blanking_codes = &yuv_blanking;
+		if ((c->colorspace == V4L2_COLORSPACE_SRGB) &&
+		    (c->quantization == V4L2_QUANTIZATION_FULL_RANGE)) {
+			if (state->timings.bt.height <= 576)
+				state->conv = &conv_matrix[RGBFULL_ITU601];
+			else
+				state->conv = &conv_matrix[RGBFULL_ITU709];
+		} else if ((c->colorspace == V4L2_COLORSPACE_SRGB) &&
+			   (c->quantization == V4L2_QUANTIZATION_LIM_RANGE)) {
+			if (state->timings.bt.height <= 576)
+				state->conv = &conv_matrix[RGBLIMITED_ITU601];
+			else
+				state->conv = &conv_matrix[RGBLIMITED_ITU709];
+		}
+		break;
+	}
+
+	if (state->conv) {
+		v4l_dbg(1, debug, state->client, "%s\n",
+			state->conv->name);
+		/* enable matrix conversion */
+		reg = io_read(sd, REG_VDP_CTRL);
+		reg &= ~VDP_CTRL_MATRIX_BP;
+		io_write(sd, REG_VDP_CTRL, reg);
+		/* offset inputs */
+		io_write16(sd, REG_VDP_MATRIX + 0, state->conv->offint1);
+		io_write16(sd, REG_VDP_MATRIX + 2, state->conv->offint2);
+		io_write16(sd, REG_VDP_MATRIX + 4, state->conv->offint3);
+		/* coefficients */
+		io_write16(sd, REG_VDP_MATRIX + 6, state->conv->p11coef);
+		io_write16(sd, REG_VDP_MATRIX + 8, state->conv->p12coef);
+		io_write16(sd, REG_VDP_MATRIX + 10, state->conv->p13coef);
+		io_write16(sd, REG_VDP_MATRIX + 12, state->conv->p21coef);
+		io_write16(sd, REG_VDP_MATRIX + 14, state->conv->p22coef);
+		io_write16(sd, REG_VDP_MATRIX + 16, state->conv->p23coef);
+		io_write16(sd, REG_VDP_MATRIX + 18, state->conv->p31coef);
+		io_write16(sd, REG_VDP_MATRIX + 20, state->conv->p32coef);
+		io_write16(sd, REG_VDP_MATRIX + 22, state->conv->p33coef);
+		/* offset outputs */
+		io_write16(sd, REG_VDP_MATRIX + 24, state->conv->offout1);
+		io_write16(sd, REG_VDP_MATRIX + 26, state->conv->offout2);
+		io_write16(sd, REG_VDP_MATRIX + 28, state->conv->offout3);
+	} else {
+		/* disable matrix conversion */
+		reg = io_read(sd, REG_VDP_CTRL);
+		reg |= VDP_CTRL_MATRIX_BP;
+		io_write(sd, REG_VDP_CTRL, reg);
+	}
+
+	/* SetBlankingCodes */
+	if (blanking_codes) {
+		io_write16(sd, REG_BLK_GY, blanking_codes->code_gy);
+		io_write16(sd, REG_BLK_BU, blanking_codes->code_bu);
+		io_write16(sd, REG_BLK_RV, blanking_codes->code_rv);
+	}
+}
+
+/* Configure frame detection window and VHREF timing generator */
+static void
+tda1997x_configure_vhref(struct v4l2_subdev *sd)
+{
+	struct tda1997x_state *state = to_state(sd);
+	const struct v4l2_bt_timings *bt = &state->timings.bt;
+	int width, lines;
+	u16 href_start, href_end;
+	u16 vref_f1_start, vref_f2_start;
+	u8 vref_f1_width, vref_f2_width;
+	u8 field_polarity;
+	u16 fieldref_f1_start, fieldref_f2_start;
+	u8 reg;
+
+	href_start = bt->hbackporch + bt->hsync + 1;
+	href_end = href_start + bt->width;
+	vref_f1_start = bt->height + bt->vbackporch + bt->vsync +
+			bt->il_vbackporch + bt->il_vsync +
+			bt->il_vfrontporch;
+	vref_f1_width = bt->vbackporch + bt->vsync + bt->vfrontporch;
+	vref_f2_start = 0;
+	vref_f2_width = 0;
+	fieldref_f1_start = 0;
+	fieldref_f2_start = 0;
+	if (bt->interlaced) {
+		vref_f2_start = (bt->height / 2) +
+				(bt->il_vbackporch + bt->il_vsync - 1);
+		vref_f2_width = bt->il_vbackporch + bt->il_vsync +
+				bt->il_vfrontporch;
+		fieldref_f2_start = vref_f2_start + bt->il_vfrontporch +
+				    fieldref_f1_start;
+	}
+	field_polarity = 0;
+
+	width = V4L2_DV_BT_FRAME_WIDTH(bt);
+	lines = V4L2_DV_BT_FRAME_HEIGHT(bt);
+
+	/*
+	 * Configure Frame Detection Window:
+	 *  horiz area where the VHREF module consider a VSYNC a new frame
+	 */
+	io_write16(sd, REG_FDW_S, 0x2ef); /* start position */
+	io_write16(sd, REG_FDW_E, 0x141); /* end position */
+
+	/* Set Pixel And Line Counters */
+	if (state->chip_revision == 0)
+		io_write16(sd, REG_PXCNT_PR, 4);
+	else
+		io_write16(sd, REG_PXCNT_PR, 1);
+	io_write16(sd, REG_PXCNT_NPIX, width & MASK_VHREF);
+	io_write16(sd, REG_LCNT_PR, 1);
+	io_write16(sd, REG_LCNT_NLIN, lines & MASK_VHREF);
+
+	/*
+	 * Configure the VHRef timing generator responsible for rebuilding all
+	 * horiz and vert synch and ref signals from its input allowing auto
+	 * detection algorithms and forcing predefined modes (480i & 576i)
+	 */
+	reg = VHREF_STD_DET_OFF << VHREF_STD_DET_SHIFT;
+	io_write(sd, REG_VHREF_CTRL, reg);
+
+	/*
+	 * Configure the VHRef timing values. In case the VHREF generator has
+	 * been configured in manual mode, this will allow to manually set all
+	 * horiz and vert ref values (non-active pixel areas) of the generator
+	 * and allows setting the frame reference params.
+	 */
+	/* horizontal reference start/end */
+	io_write16(sd, REG_HREF_S, href_start & MASK_VHREF);
+	io_write16(sd, REG_HREF_E, href_end & MASK_VHREF);
+	/* vertical reference f1 start/end */
+	io_write16(sd, REG_VREF_F1_S, vref_f1_start & MASK_VHREF);
+	io_write(sd, REG_VREF_F1_WIDTH, vref_f1_width);
+	/* vertical reference f2 start/end */
+	io_write16(sd, REG_VREF_F2_S, vref_f2_start & MASK_VHREF);
+	io_write(sd, REG_VREF_F2_WIDTH, vref_f2_width);
+
+	/* F1/F2 FREF, field polarity */
+	reg = fieldref_f1_start & MASK_VHREF;
+	reg |= field_polarity << 8;
+	io_write16(sd, REG_FREF_F1_S, reg);
+	reg = fieldref_f2_start & MASK_VHREF;
+	io_write16(sd, REG_FREF_F2_S, reg);
+}
+
+/* Configure Video Output port signals */
+static int
+tda1997x_configure_vidout(struct tda1997x_state *state)
+{
+	struct v4l2_subdev *sd = &state->sd;
+	struct tda1997x_platform_data *pdata = &state->pdata;
+	u8 prefilter;
+	u8 reg;
+
+	/* Configure pixel clock generator: delay, polarity, rate */
+	reg = (state->vid_fmt == OF_FMT_422_CCIR) ?
+	       PCLK_SEL_X2 : PCLK_SEL_X1;
+	reg |= pdata->vidout_delay_pclk << PCLK_DELAY_SHIFT;
+	reg |= pdata->vidout_inv_pclk << PCLK_INV_SHIFT;
+	io_write(sd, REG_PCLK, reg);
+
+	/* Configure pre-filter */
+	prefilter = 0; /* filters off */
+	/* YUV422 mode requires conversion */
+	if ((state->vid_fmt == OF_FMT_422_SMPT) ||
+	    (state->vid_fmt == OF_FMT_422_CCIR)) {
+		/* 2/7 taps for Rv and Bu */
+		prefilter = FILTERS_CTRL_2_7TAP << FILTERS_CTRL_BU_SHIFT |
+			    FILTERS_CTRL_2_7TAP << FILTERS_CTRL_RV_SHIFT;
+	}
+	io_write(sd, REG_FILTERS_CTRL, prefilter);
+
+	/* Configure video port */
+	reg = state->vid_fmt & OF_FMT_MASK;
+	if (state->vid_fmt == OF_FMT_422_CCIR)
+		reg |= (OF_BLK | OF_TRC);
+	reg |= OF_VP_ENABLE;
+	io_write(sd, REG_OF, reg);
+
+	/* Configure formatter and conversions */
+	reg = io_read(sd, REG_VDP_CTRL);
+	/* pre-filter is needed unless (REG_FILTERS_CTRL == 0) */
+	if (!prefilter)
+		reg |= VDP_CTRL_PREFILTER_BP;
+	else
+		reg &= ~VDP_CTRL_PREFILTER_BP;
+	/* formatter is needed for YUV422 and for trc/blc codes */
+	if (state->vid_fmt == OF_FMT_444)
+		reg |= VDP_CTRL_FORMATTER_BP;
+	/* formatter and compdel needed for timing/blanking codes */
+	else
+		reg &= ~(VDP_CTRL_FORMATTER_BP | VDP_CTRL_COMPDEL_BP);
+	/* activate compdel for small sync delays */
+	if ((pdata->vidout_delay_vs < 4) || (pdata->vidout_delay_hs < 4))
+		reg &= ~VDP_CTRL_COMPDEL_BP;
+	io_write(sd, REG_VDP_CTRL, reg);
+
+	/* Configure DE output signal: delay, polarity, and source */
+	reg = pdata->vidout_delay_de << DE_FREF_DELAY_SHIFT |
+	      pdata->vidout_inv_de << DE_FREF_INV_SHIFT |
+	      pdata->vidout_sel_de << DE_FREF_SEL_SHIFT;
+	io_write(sd, REG_DE_FREF, reg);
+
+	/* Configure HS/HREF output signal: delay, polarity, and source */
+	if (state->vid_fmt != OF_FMT_422_CCIR) {
+		reg = pdata->vidout_delay_hs << HS_HREF_DELAY_SHIFT |
+		      pdata->vidout_inv_hs << HS_HREF_INV_SHIFT |
+		      pdata->vidout_sel_hs << HS_HREF_SEL_SHIFT;
+	} else
+		reg = HS_HREF_SEL_NONE << HS_HREF_SEL_SHIFT;
+	io_write(sd, REG_HS_HREF, reg);
+
+	/* Configure VS/VREF output signal: delay, polarity, and source */
+	if (state->vid_fmt != OF_FMT_422_CCIR) {
+		reg = pdata->vidout_delay_vs << VS_VREF_DELAY_SHIFT |
+		      pdata->vidout_inv_vs << VS_VREF_INV_SHIFT |
+		      pdata->vidout_sel_vs << VS_VREF_SEL_SHIFT;
+	} else
+		reg = VS_VREF_SEL_NONE << VS_VREF_SEL_SHIFT;
+	io_write(sd, REG_VS_VREF, reg);
+
+	return 0;
+}
+
+/* Configure Audio output port signals */
+static int
+tda1997x_configure_audout(struct v4l2_subdev *sd, u8 channel_assignment)
+{
+	struct tda1997x_state *state = to_state(sd);
+	struct tda1997x_platform_data *pdata = &state->pdata;
+	bool sp_used_by_fifo = true;
+	u8 reg;
+
+	if (!pdata->audout_format)
+		return 0;
+
+	/* channel assignment (CEA-861-D Table 20) */
+	io_write(sd, REG_AUDIO_PATH, channel_assignment);
+
+	/* Audio output configuration */
+	reg = 0;
+	switch (pdata->audout_format) {
+	case AUDFMT_TYPE_I2S:
+		reg |= AUDCFG_BUS_I2S << AUDCFG_BUS_SHIFT;
+		break;
+	case AUDFMT_TYPE_SPDIF:
+		reg |= AUDCFG_BUS_SPDIF << AUDCFG_BUS_SHIFT;
+		break;
+	}
+	switch (state->audio_type) {
+	case AUDCFG_TYPE_PCM:
+		reg |= AUDCFG_TYPE_PCM << AUDCFG_TYPE_SHIFT;
+		break;
+	case AUDCFG_TYPE_OBA:
+		reg |= AUDCFG_TYPE_OBA << AUDCFG_TYPE_SHIFT;
+		break;
+	case AUDCFG_TYPE_DST:
+		reg |= AUDCFG_TYPE_DST << AUDCFG_TYPE_SHIFT;
+		sp_used_by_fifo = false;
+		break;
+	case AUDCFG_TYPE_HBR:
+		reg |= AUDCFG_TYPE_HBR << AUDCFG_TYPE_SHIFT;
+		if (pdata->audout_layout == 1) {
+			/* demuxed via AP0:AP3 */
+			reg |= AUDCFG_HBR_DEMUX << AUDCFG_HBR_SHIFT;
+			if (pdata->audout_format == AUDFMT_TYPE_SPDIF)
+				sp_used_by_fifo = false;
+		} else {
+			/* straight via AP0 */
+			reg |= AUDCFG_HBR_STRAIGHT << AUDCFG_HBR_SHIFT;
+		}
+		break;
+	}
+	if (pdata->audout_width == 32)
+		reg |= AUDCFG_I2SW_32 << AUDCFG_I2SW_SHIFT;
+	else
+		reg |= AUDCFG_I2SW_16 << AUDCFG_I2SW_SHIFT;
+
+	/* automatic hardware mute */
+	if (pdata->audio_auto_mute)
+		reg |= AUDCFG_AUTO_MUTE_EN;
+	/* clock polarity */
+	if (pdata->audout_invert_clk)
+		reg |= AUDCFG_CLK_INVERT;
+	io_write(sd, REG_AUDCFG, reg);
+
+	/* audio layout */
+	reg = (pdata->audout_layout) ? AUDIO_LAYOUT_LAYOUT1 : 0;
+	if (!pdata->audout_layoutauto)
+		reg |= AUDIO_LAYOUT_MANUAL;
+	if (sp_used_by_fifo)
+		reg |= AUDIO_LAYOUT_SP_FLAG;
+	io_write(sd, REG_AUDIO_LAYOUT, reg);
+
+	/* FIFO Latency value */
+	io_write(sd, REG_FIFO_LATENCY_VAL, 0x80);
+
+	/* Audio output port config */
+	if (sp_used_by_fifo) {
+		reg = AUDIO_OUT_ENABLE_AP0;
+		if (channel_assignment >= 0x01)
+			reg |= AUDIO_OUT_ENABLE_AP1;
+		if (channel_assignment >= 0x04)
+			reg |= AUDIO_OUT_ENABLE_AP2;
+		if (channel_assignment >= 0x0c)
+			reg |= AUDIO_OUT_ENABLE_AP3;
+		/* specific cases where AP1 is not used */
+		if ((channel_assignment == 0x04)
+		 || (channel_assignment == 0x08)
+		 || (channel_assignment == 0x0c)
+		 || (channel_assignment == 0x10)
+		 || (channel_assignment == 0x14)
+		 || (channel_assignment == 0x18)
+		 || (channel_assignment == 0x1c))
+			reg &= ~AUDIO_OUT_ENABLE_AP1;
+		/* specific cases where AP2 is not used */
+		if ((channel_assignment >= 0x14)
+		 && (channel_assignment <= 0x17))
+			reg &= ~AUDIO_OUT_ENABLE_AP2;
+	} else {
+		reg = AUDIO_OUT_ENABLE_AP3 |
+		      AUDIO_OUT_ENABLE_AP2 |
+		      AUDIO_OUT_ENABLE_AP1 |
+		      AUDIO_OUT_ENABLE_AP0;
+	}
+	if (pdata->audout_format == AUDFMT_TYPE_I2S)
+		reg |= (AUDIO_OUT_ENABLE_ACLK | AUDIO_OUT_ENABLE_WS);
+	io_write(sd, REG_AUDIO_OUT_ENABLE, reg);
+
+	/* reset test mode to normal audio freq auto selection */
+	io_write(sd, REG_TEST_MODE, 0x00);
+
+	return 0;
+}
+
+/* Soft Reset of specific hdmi info */
+static int
+tda1997x_hdmi_info_reset(struct v4l2_subdev *sd, u8 info_rst, bool reset_sus)
+{
+	u8 reg;
+
+	/* reset infoframe engine packets */
+	reg = io_read(sd, REG_HDMI_INFO_RST);
+	io_write(sd, REG_HDMI_INFO_RST, info_rst);
+
+	/* if infoframe engine has been reset clear INT_FLG_MODE */
+	if (reg & RESET_IF) {
+		reg = io_read(sd, REG_INT_FLG_CLR_MODE);
+		io_write(sd, REG_INT_FLG_CLR_MODE, reg);
+	}
+
+	/* Disable REFTIM to restart start-up-sequencer (SUS) */
+	reg = io_read(sd, REG_RATE_CTRL);
+	reg &= ~RATE_REFTIM_ENABLE;
+	if (!reset_sus)
+		reg |= RATE_REFTIM_ENABLE;
+	reg = io_write(sd, REG_RATE_CTRL, reg);
+
+	return 0;
+}
+
+static void
+tda1997x_power_mode(struct tda1997x_state *state, bool enable)
+{
+	struct v4l2_subdev *sd = &state->sd;
+	u8 reg;
+
+	if (enable) {
+		/* Automatic control of TMDS */
+		io_write(sd, REG_PON_OVR_EN, PON_DIS);
+		/* Enable current bias unit */
+		io_write(sd, REG_CFG1, PON_EN);
+		/* Enable deep color PLL */
+		io_write(sd, REG_DEEP_PLL7_BYP, PON_DIS);
+		/* Output buffers active */
+		reg = io_read(sd, REG_OF);
+		reg &= ~OF_VP_ENABLE;
+		io_write(sd, REG_OF, reg);
+	} else {
+		/* Power down EDID mode sequence */
+		/* Output buffers in HiZ */
+		reg = io_read(sd, REG_OF);
+		reg |= OF_VP_ENABLE;
+		io_write(sd, REG_OF, reg);
+		/* Disable deep color PLL */
+		io_write(sd, REG_DEEP_PLL7_BYP, PON_EN);
+		/* Disable current bias unit */
+		io_write(sd, REG_CFG1, PON_DIS);
+		/* Manual control of TMDS */
+		io_write(sd, REG_PON_OVR_EN, PON_EN);
+	}
+}
+
+static bool
+tda1997x_detect_tx_5v(struct v4l2_subdev *sd)
+{
+	u8 reg = io_read(sd, REG_DETECT_5V);
+
+	return ((reg & DETECT_5V_SEL) ? 1 : 0);
+}
+
+static bool
+tda1997x_detect_tx_hpd(struct v4l2_subdev *sd)
+{
+	u8 reg = io_read(sd, REG_DETECT_5V);
+
+	return ((reg & DETECT_HPD) ? 1 : 0);
+}
+
+static int
+tda1997x_detect_std(struct tda1997x_state *state,
+		    struct v4l2_dv_timings *timings)
+{
+	struct v4l2_subdev *sd = &state->sd;
+
+	/*
+	 * Read the FMT registers
+	 *   REG_V_PER: Period of a frame (or field) in MCLK (27MHz) cycles
+	 *   REG_H_PER: Period of a line in MCLK (27MHz) cycles
+	 *   REG_HS_WIDTH: Period of horiz sync pulse in MCLK (27MHz) cycles
+	 */
+	u32 vper, vsync_pos;
+	u16 hper, hsync_pos, hsper, interlaced;
+	u16 htot, hact, hfront, hsync, hback;
+	u16 vtot, vact, vfront1, vfront2, vsync, vback1, vback2;
+
+	if (!state->input_detect[0] && !state->input_detect[1])
+		return -ENOLINK;
+
+	vper = io_read24(sd, REG_V_PER);
+	hper = io_read16(sd, REG_H_PER);
+	hsper = io_read16(sd, REG_HS_WIDTH);
+	vsync_pos = vper & MASK_VPER_SYNC_POS;
+	hsync_pos = hper & MASK_HPER_SYNC_POS;
+	interlaced = hsper & MASK_HSWIDTH_INTERLACED;
+	vper &= MASK_VPER;
+	hper &= MASK_HPER;
+	hsper &= MASK_HSWIDTH;
+	v4l2_dbg(1, debug, sd, "Signal Timings: %u/%u/%u\n", vper, hper, hsper);
+
+	htot = io_read16(sd, REG_FMT_H_TOT);
+	hact = io_read16(sd, REG_FMT_H_ACT);
+	hfront = io_read16(sd, REG_FMT_H_FRONT);
+	hsync = io_read16(sd, REG_FMT_H_SYNC);
+	hback = io_read16(sd, REG_FMT_H_BACK);
+
+	vtot = io_read16(sd, REG_FMT_V_TOT);
+	vact = io_read16(sd, REG_FMT_V_ACT);
+	vfront1 = io_read(sd, REG_FMT_V_FRONT_F1);
+	vfront2 = io_read(sd, REG_FMT_V_FRONT_F2);
+	vsync = io_read(sd, REG_FMT_V_SYNC);
+	vback1 = io_read(sd, REG_FMT_V_BACK_F1);
+	vback2 = io_read(sd, REG_FMT_V_BACK_F2);
+
+	v4l2_dbg(1, debug, sd, "Geometry: H %u %u %u %u %u Sync%c  V %u %u %u %u %u %u %u Sync%c\n",
+		 htot, hact, hfront, hsync, hback, hsync_pos ? '+' : '-',
+		 vtot, vact, vfront1, vfront2, vsync, vback1, vback2, vsync_pos ? '+' : '-');
+
+	if (!timings)
+		return 0;
+
+	timings->type = V4L2_DV_BT_656_1120;
+	timings->bt.width = hact;
+	timings->bt.hfrontporch = hfront;
+	timings->bt.hsync = hsync;
+	timings->bt.hbackporch = hback;
+	timings->bt.height = vact;
+	timings->bt.vfrontporch = vfront1;
+	timings->bt.vsync = vsync;
+	timings->bt.vbackporch = vback1;
+	timings->bt.interlaced = interlaced ? V4L2_DV_INTERLACED : V4L2_DV_PROGRESSIVE;
+	timings->bt.polarities = vsync_pos ? V4L2_DV_VSYNC_POS_POL : 0;
+	timings->bt.polarities |= hsync_pos ? V4L2_DV_HSYNC_POS_POL : 0;
+
+	timings->bt.pixelclock = (u64)htot * vtot * 27000000;
+	if (interlaced) {
+		timings->bt.il_vfrontporch = vfront2;
+		timings->bt.il_vsync = timings->bt.vsync;
+		timings->bt.il_vbackporch = vback2;
+		do_div(timings->bt.pixelclock, vper * 2 /* full frame */);
+	} else {
+		timings->bt.il_vfrontporch = 0;
+		timings->bt.il_vsync = 0;
+		timings->bt.il_vbackporch = 0;
+		do_div(timings->bt.pixelclock, vper);
+	}
+	v4l2_find_dv_timings_cap(timings, &tda1997x_dv_timings_cap,
+				 (u32)timings->bt.pixelclock / 500, NULL, NULL);
+	v4l2_print_dv_timings(sd->name, "Detected format: ", timings, false);
+	return 0;
+}
+
+/* some sort of errata workaround for chip revision 0 (N1) */
+static void tda1997x_reset_n1(struct tda1997x_state *state)
+{
+	struct v4l2_subdev *sd = &state->sd;
+	u8 reg;
+
+	/* clear HDMI mode flag in BCAPS */
+	io_write(sd, REG_CLK_CFG, CLK_CFG_SEL_ACLK_EN | CLK_CFG_SEL_ACLK);
+	io_write(sd, REG_PON_OVR_EN, PON_EN);
+	io_write(sd, REG_PON_CBIAS, PON_EN);
+	io_write(sd, REG_PON_PLL, PON_EN);
+
+	reg = io_read(sd, REG_MODE_REC_CFG1);
+	reg &= ~0x06;
+	reg |= 0x02;
+	io_write(sd, REG_MODE_REC_CFG1, reg);
+	io_write(sd, REG_CLK_CFG, CLK_CFG_DIS);
+	io_write(sd, REG_PON_OVR_EN, PON_DIS);
+	reg = io_read(sd, REG_MODE_REC_CFG1);
+	reg &= ~0x06;
+	io_write(sd, REG_MODE_REC_CFG1, reg);
+}
+
+/*
+ * Activity detection must only be notified when stable_clk_x AND active_x
+ * bits are set to 1. If only stable_clk_x bit is set to 1 but not
+ * active_x, it means that the TMDS clock is not in the defined range
+ * and activity detection must not be notified.
+ */
+static u8
+tda1997x_read_activity_status_regs(struct v4l2_subdev *sd)
+{
+	u8 reg, status = 0;
+
+	/* Read CLK_A_STATUS register */
+	reg = io_read(sd, REG_CLK_A_STATUS);
+	/* ignore if not active */
+	if ((reg & MASK_CLK_STABLE) && !(reg & MASK_CLK_ACTIVE))
+		reg &= ~MASK_CLK_STABLE;
+	status |= ((reg & MASK_CLK_STABLE) >> 2);
+
+	/* Read CLK_B_STATUS register */
+	reg = io_read(sd, REG_CLK_B_STATUS);
+	/* ignore if not active */
+	if ((reg & MASK_CLK_STABLE) && !(reg & MASK_CLK_ACTIVE))
+		reg &= ~MASK_CLK_STABLE;
+	status |= ((reg & MASK_CLK_STABLE) >> 1);
+
+	/* Read the SUS_STATUS register */
+	reg = io_read(sd, REG_SUS_STATUS);
+
+	/* If state = 5 => TMDS is locked */
+	if ((reg & MASK_SUS_STATUS) == LAST_STATE_REACHED)
+		status |= MASK_SUS_STATE;
+	else
+		status &= ~MASK_SUS_STATE;
+
+	return status;
+}
+
+static void
+set_rgb_quantization_range(struct tda1997x_state *state)
+{
+	struct v4l2_hdmi_colorimetry *c = &state->colorimetry;
+
+	state->colorimetry = v4l2_hdmi_rx_colorimetry(&state->avi_infoframe,
+						      NULL,
+						      state->timings.bt.height);
+	/* If ycbcr_enc is V4L2_YCBCR_ENC_DEFAULT, we receive RGB */
+	if (c->ycbcr_enc == V4L2_YCBCR_ENC_DEFAULT) {
+		switch (state->rgb_quantization_range) {
+		case V4L2_DV_RGB_RANGE_LIMITED:
+			c->quantization = V4L2_QUANTIZATION_FULL_RANGE;
+			break;
+		case V4L2_DV_RGB_RANGE_FULL:
+			c->quantization = V4L2_QUANTIZATION_LIM_RANGE;
+			break;
+		}
+	}
+	v4l_dbg(1, debug, state->client,
+		"colorspace=%d/%d colorimetry=%d range=%s content=%d\n",
+		state->avi_infoframe.colorspace, c->colorspace,
+		state->avi_infoframe.colorimetry,
+		v4l2_quantization_names[c->quantization],
+		state->avi_infoframe.content_type);
+}
+
+/* parse an infoframe and do some sanity checks on it */
+static unsigned int
+tda1997x_parse_infoframe(struct tda1997x_state *state, u16 addr)
+{
+	struct v4l2_subdev *sd = &state->sd;
+	union hdmi_infoframe frame;
+	u8 buffer[40] = { 0 };
+	u8 reg;
+	int len, err;
+
+	/* read data */
+	len = io_readn(sd, addr, sizeof(buffer), buffer);
+	err = hdmi_infoframe_unpack(&frame, buffer, len);
+	if (err) {
+		v4l_err(state->client,
+			"failed parsing %d byte infoframe: 0x%04x/0x%02x\n",
+			len, addr, buffer[0]);
+		return err;
+	}
+	hdmi_infoframe_log(KERN_INFO, &state->client->dev, &frame);
+	switch (frame.any.type) {
+	/* Audio InfoFrame: see HDMI spec 8.2.2 */
+	case HDMI_INFOFRAME_TYPE_AUDIO:
+		/* sample rate */
+		switch (frame.audio.sample_frequency) {
+		case HDMI_AUDIO_SAMPLE_FREQUENCY_32000:
+			state->audio_samplerate = 32000;
+			break;
+		case HDMI_AUDIO_SAMPLE_FREQUENCY_44100:
+			state->audio_samplerate = 44100;
+			break;
+		case HDMI_AUDIO_SAMPLE_FREQUENCY_48000:
+			state->audio_samplerate = 48000;
+			break;
+		case HDMI_AUDIO_SAMPLE_FREQUENCY_88200:
+			state->audio_samplerate = 88200;
+			break;
+		case HDMI_AUDIO_SAMPLE_FREQUENCY_96000:
+			state->audio_samplerate = 96000;
+			break;
+		case HDMI_AUDIO_SAMPLE_FREQUENCY_176400:
+			state->audio_samplerate = 176400;
+			break;
+		case HDMI_AUDIO_SAMPLE_FREQUENCY_192000:
+			state->audio_samplerate = 192000;
+			break;
+		default:
+		case HDMI_AUDIO_SAMPLE_FREQUENCY_STREAM:
+			break;
+		}
+
+		/* sample size */
+		switch (frame.audio.sample_size) {
+		case HDMI_AUDIO_SAMPLE_SIZE_16:
+			state->audio_samplesize = 16;
+			break;
+		case HDMI_AUDIO_SAMPLE_SIZE_20:
+			state->audio_samplesize = 20;
+			break;
+		case HDMI_AUDIO_SAMPLE_SIZE_24:
+			state->audio_samplesize = 24;
+			break;
+		case HDMI_AUDIO_SAMPLE_SIZE_STREAM:
+		default:
+			break;
+		}
+
+		/* Channel Count */
+		state->audio_channels = frame.audio.channels;
+		if (frame.audio.channel_allocation &&
+		    frame.audio.channel_allocation != state->audio_ch_alloc) {
+			/* use the channel assignment from the infoframe */
+			state->audio_ch_alloc = frame.audio.channel_allocation;
+			tda1997x_configure_audout(sd, state->audio_ch_alloc);
+			/* reset the audio FIFO */
+			tda1997x_hdmi_info_reset(sd, RESET_AUDIO, false);
+		}
+		break;
+
+	/* Auxiliary Video information (AVI) InfoFrame: see HDMI spec 8.2.1 */
+	case HDMI_INFOFRAME_TYPE_AVI:
+		state->avi_infoframe = frame.avi;
+		set_rgb_quantization_range(state);
+
+		/* configure upsampler: 0=bypass 1=repeatchroma 2=interpolate */
+		reg = io_read(sd, REG_PIX_REPEAT);
+		reg &= ~PIX_REPEAT_MASK_UP_SEL;
+		if (frame.avi.colorspace == HDMI_COLORSPACE_YUV422)
+			reg |= (PIX_REPEAT_CHROMA << PIX_REPEAT_SHIFT);
+		io_write(sd, REG_PIX_REPEAT, reg);
+
+		/* ConfigurePixelRepeater: repeat n-times each pixel */
+		reg = io_read(sd, REG_PIX_REPEAT);
+		reg &= ~PIX_REPEAT_MASK_REP;
+		reg |= frame.avi.pixel_repeat;
+		io_write(sd, REG_PIX_REPEAT, reg);
+
+		/* configure the receiver with the new colorspace */
+		tda1997x_configure_csc(sd);
+		break;
+	default:
+		break;
+	}
+	return 0;
+}
+
+static void tda1997x_irq_sus(struct tda1997x_state *state, u8 *flags)
+{
+	struct v4l2_subdev *sd = &state->sd;
+	u8 reg, source;
+
+	source = io_read(sd, REG_INT_FLG_CLR_SUS);
+	io_write(sd, REG_INT_FLG_CLR_SUS, source);
+
+	if (source & MASK_MPT) {
+		/* reset MTP in use flag if set */
+		if (state->mptrw_in_progress)
+			state->mptrw_in_progress = 0;
+	}
+
+	if (source & MASK_SUS_END) {
+		/* reset audio FIFO */
+		reg = io_read(sd, REG_HDMI_INFO_RST);
+		reg |= MASK_SR_FIFO_FIFO_CTRL;
+		io_write(sd, REG_HDMI_INFO_RST, reg);
+		reg &= ~MASK_SR_FIFO_FIFO_CTRL;
+		io_write(sd, REG_HDMI_INFO_RST, reg);
+
+		/* reset HDMI flags */
+		state->hdmi_status = 0;
+	}
+
+	/* filter FMT interrupt based on SUS state */
+	reg = io_read(sd, REG_SUS_STATUS);
+	if (((reg & MASK_SUS_STATUS) != LAST_STATE_REACHED)
+	   || (source & MASK_MPT)) {
+		source &= ~MASK_FMT;
+	}
+
+	if (source & (MASK_FMT | MASK_SUS_END)) {
+		reg = io_read(sd, REG_SUS_STATUS);
+		if ((reg & MASK_SUS_STATUS) != LAST_STATE_REACHED) {
+			v4l_err(state->client, "BAD SUS STATUS\n");
+			return;
+		}
+		if (debug)
+			tda1997x_detect_std(state, NULL);
+		/* notify user of change in resolution */
+		v4l2_subdev_notify_event(&state->sd, &tda1997x_ev_fmt);
+	}
+}
+
+static void tda1997x_irq_ddc(struct tda1997x_state *state, u8 *flags)
+{
+	struct v4l2_subdev *sd = &state->sd;
+	u8 source;
+
+	source = io_read(sd, REG_INT_FLG_CLR_DDC);
+	io_write(sd, REG_INT_FLG_CLR_DDC, source);
+	if (source & MASK_EDID_MTP) {
+		/* reset MTP in use flag if set */
+		if (state->mptrw_in_progress)
+			state->mptrw_in_progress = 0;
+	}
+
+	/* Detection of +5V */
+	if (source & MASK_DET_5V) {
+		v4l2_ctrl_s_ctrl(state->detect_tx_5v_ctrl,
+				 tda1997x_detect_tx_5v(sd));
+	}
+}
+
+static void tda1997x_irq_rate(struct tda1997x_state *state, u8 *flags)
+{
+	struct v4l2_subdev *sd = &state->sd;
+	u8 reg, source;
+
+	u8 irq_status;
+
+	source = io_read(sd, REG_INT_FLG_CLR_RATE);
+	io_write(sd, REG_INT_FLG_CLR_RATE, source);
+
+	/* read status regs */
+	irq_status = tda1997x_read_activity_status_regs(sd);
+
+	/*
+	 * read clock status reg until INT_FLG_CLR_RATE is still 0
+	 * after the read to make sure its the last one
+	 */
+	reg = source;
+	while (reg != 0) {
+		irq_status = tda1997x_read_activity_status_regs(sd);
+		reg = io_read(sd, REG_INT_FLG_CLR_RATE);
+		io_write(sd, REG_INT_FLG_CLR_RATE, reg);
+		source |= reg;
+	}
+
+	/* we only pay attention to stability change events */
+	if (source & (MASK_RATE_A_ST | MASK_RATE_B_ST)) {
+		int input = (source & MASK_RATE_A_ST)?0:1;
+		u8 mask = 1<<input;
+
+		/* state change */
+		if ((irq_status & mask) != (state->activity_status & mask)) {
+			/* activity lost */
+			if ((irq_status & mask) == 0) {
+				v4l_info(state->client,
+					 "HDMI-%c: Digital Activity Lost\n",
+					 input+'A');
+
+				/* bypass up/down sampler and pixel repeater */
+				reg = io_read(sd, REG_PIX_REPEAT);
+				reg &= ~PIX_REPEAT_MASK_UP_SEL;
+				reg &= ~PIX_REPEAT_MASK_REP;
+				io_write(sd, REG_PIX_REPEAT, reg);
+
+				if (state->chip_revision == 0)
+					tda1997x_reset_n1(state);
+
+				state->input_detect[input] = 0;
+				v4l2_subdev_notify_event(sd, &tda1997x_ev_fmt);
+			}
+
+			/* activity detected */
+			else {
+				v4l_info(state->client,
+					 "HDMI-%c: Digital Activity Detected\n",
+					 input+'A');
+				state->input_detect[input] = 1;
+			}
+
+			/* hold onto current state */
+			state->activity_status = (irq_status & mask);
+		}
+	}
+}
+
+static void tda1997x_irq_info(struct tda1997x_state *state, u8 *flags)
+{
+	struct v4l2_subdev *sd = &state->sd;
+	u8 source;
+
+	source = io_read(sd, REG_INT_FLG_CLR_INFO);
+	io_write(sd, REG_INT_FLG_CLR_INFO, source);
+
+	/* Audio infoframe */
+	if (source & MASK_AUD_IF) {
+		tda1997x_parse_infoframe(state, AUD_IF);
+		source &= ~MASK_AUD_IF;
+	}
+
+	/* Source Product Descriptor infoframe change */
+	if (source & MASK_SPD_IF) {
+		tda1997x_parse_infoframe(state, SPD_IF);
+		source &= ~MASK_SPD_IF;
+	}
+
+	/* Auxiliary Video Information infoframe */
+	if (source & MASK_AVI_IF) {
+		tda1997x_parse_infoframe(state, AVI_IF);
+		source &= ~MASK_AVI_IF;
+	}
+}
+
+static void tda1997x_irq_audio(struct tda1997x_state *state, u8 *flags)
+{
+	struct v4l2_subdev *sd = &state->sd;
+	u8 reg, source;
+
+	source = io_read(sd, REG_INT_FLG_CLR_AUDIO);
+	io_write(sd, REG_INT_FLG_CLR_AUDIO, source);
+
+	/* reset audio FIFO on FIFO pointer error or audio mute */
+	if (source & MASK_ERROR_FIFO_PT ||
+	    source & MASK_MUTE_FLG) {
+		/* audio reset audio FIFO */
+		reg = io_read(sd, REG_SUS_STATUS);
+		if ((reg & MASK_SUS_STATUS) == LAST_STATE_REACHED) {
+			reg = io_read(sd, REG_HDMI_INFO_RST);
+			reg |= MASK_SR_FIFO_FIFO_CTRL;
+			io_write(sd, REG_HDMI_INFO_RST, reg);
+			reg &= ~MASK_SR_FIFO_FIFO_CTRL;
+			io_write(sd, REG_HDMI_INFO_RST, reg);
+			/* reset channel status IT if present */
+			source &= ~(MASK_CH_STATE);
+		}
+	}
+	if (source & MASK_AUDIO_FREQ_FLG) {
+		static const int freq[] = {
+			0, 32000, 44100, 48000, 88200, 96000, 176400, 192000
+		};
+
+		reg = io_read(sd, REG_AUDIO_FREQ);
+		state->audio_samplerate = freq[reg & 7];
+		v4l_info(state->client, "Audio Frequency Change: %dHz\n",
+			 state->audio_samplerate);
+	}
+	if (source & MASK_AUDIO_FLG) {
+		reg = io_read(sd, REG_AUDIO_FLAGS);
+		if (reg & BIT(AUDCFG_TYPE_DST))
+			state->audio_type = AUDCFG_TYPE_DST;
+		if (reg & BIT(AUDCFG_TYPE_OBA))
+			state->audio_type = AUDCFG_TYPE_OBA;
+		if (reg & BIT(AUDCFG_TYPE_HBR))
+			state->audio_type = AUDCFG_TYPE_HBR;
+		if (reg & BIT(AUDCFG_TYPE_PCM))
+			state->audio_type = AUDCFG_TYPE_PCM;
+		v4l_info(state->client, "Audio Type: %s\n",
+			 audtype_names[state->audio_type]);
+	}
+}
+
+static void tda1997x_irq_hdcp(struct tda1997x_state *state, u8 *flags)
+{
+	struct v4l2_subdev *sd = &state->sd;
+	u8 reg, source;
+
+	source = io_read(sd, REG_INT_FLG_CLR_HDCP);
+	io_write(sd, REG_INT_FLG_CLR_HDCP, source);
+
+	/* reset MTP in use flag if set */
+	if (source & MASK_HDCP_MTP)
+		state->mptrw_in_progress = 0;
+	if (source & MASK_STATE_C5) {
+		/* REPEATER: mask AUDIO and IF irqs to avoid IF during auth */
+		reg = io_read(sd, REG_INT_MASK_TOP);
+		reg &= ~(INTERRUPT_AUDIO | INTERRUPT_INFO);
+		io_write(sd, REG_INT_MASK_TOP, reg);
+		*flags &= (INTERRUPT_AUDIO | INTERRUPT_INFO);
+	}
+}
+
+static irqreturn_t tda1997x_isr_thread(int irq, void *d)
+{
+	struct tda1997x_state *state = d;
+	struct v4l2_subdev *sd = &state->sd;
+	u8 flags;
+
+	mutex_lock(&state->lock);
+	do {
+		/* read interrupt flags */
+		flags = io_read(sd, REG_INT_FLG_CLR_TOP);
+		if (flags == 0)
+			break;
+
+		/* SUS interrupt source (Input activity events) */
+		if (flags & INTERRUPT_SUS)
+			tda1997x_irq_sus(state, &flags);
+		/* DDC interrupt source (Display Data Channel) */
+		else if (flags & INTERRUPT_DDC)
+			tda1997x_irq_ddc(state, &flags);
+		/* RATE interrupt source (Digital Input activity) */
+		else if (flags & INTERRUPT_RATE)
+			tda1997x_irq_rate(state, &flags);
+		/* Infoframe change interrupt */
+		else if (flags & INTERRUPT_INFO)
+			tda1997x_irq_info(state, &flags);
+		/* Audio interrupt source:
+		 *   freq change, DST,OBA,HBR,ASP flags, mute, FIFO err
+		 */
+		else if (flags & INTERRUPT_AUDIO)
+			tda1997x_irq_audio(state, &flags);
+		/* HDCP interrupt source (content protection) */
+		if (flags & INTERRUPT_HDCP)
+			tda1997x_irq_hdcp(state, &flags);
+	} while (flags != 0);
+	mutex_unlock(&state->lock);
+
+	return IRQ_HANDLED;
+}
+
+/* -----------------------------------------------------------------------------
+ * v4l2_subdev_video_ops
+ */
+
+static int
+tda1997x_g_input_status(struct v4l2_subdev *sd, u32 *status)
+{
+	struct tda1997x_state *state = to_state(sd);
+	u32 vper;
+	u16 hper;
+	u16 hsper;
+
+	mutex_lock(&state->lock);
+	vper = io_read24(sd, REG_V_PER) & MASK_VPER;
+	hper = io_read16(sd, REG_H_PER) & MASK_HPER;
+	hsper = io_read16(sd, REG_HS_WIDTH) & MASK_HSWIDTH;
+	/*
+	 * The tda1997x supports A/B inputs but only a single output.
+	 * The irq handler monitors for timing changes on both inputs and
+	 * sets the input_detect array to 0|1 depending on signal presence.
+	 * I believe selection of A vs B is automatic.
+	 *
+	 * The vper/hper/hsper registers provide the frame period, line period
+	 * and horiz sync period (units of MCLK clock cycles (27MHz)) and
+	 * testing shows these values to be random if no signal is present
+	 * or locked.
+	 */
+	v4l2_dbg(1, debug, sd, "inputs:%d/%d timings:%d/%d/%d\n",
+		 state->input_detect[0], state->input_detect[1],
+		 vper, hper, hsper);
+	if (!state->input_detect[0] && !state->input_detect[1])
+		*status = V4L2_IN_ST_NO_SIGNAL;
+	else if (!vper || !hper || !hsper)
+		*status = V4L2_IN_ST_NO_SYNC;
+	else
+		*status = 0;
+	mutex_unlock(&state->lock);
+
+	return 0;
+};
+
+static int tda1997x_s_dv_timings(struct v4l2_subdev *sd,
+				struct v4l2_dv_timings *timings)
+{
+	struct tda1997x_state *state = to_state(sd);
+
+	v4l_dbg(1, debug, state->client, "%s\n", __func__);
+
+	if (v4l2_match_dv_timings(&state->timings, timings, 0, false))
+		return 0; /* no changes */
+
+	if (!v4l2_valid_dv_timings(timings, &tda1997x_dv_timings_cap,
+				   NULL, NULL))
+		return -ERANGE;
+
+	mutex_lock(&state->lock);
+	state->timings = *timings;
+	/* setup frame detection window and VHREF timing generator */
+	tda1997x_configure_vhref(sd);
+	/* configure colorspace conversion */
+	tda1997x_configure_csc(sd);
+	mutex_unlock(&state->lock);
+
+	return 0;
+}
+
+static int tda1997x_g_dv_timings(struct v4l2_subdev *sd,
+				 struct v4l2_dv_timings *timings)
+{
+	struct tda1997x_state *state = to_state(sd);
+
+	v4l_dbg(1, debug, state->client, "%s\n", __func__);
+	mutex_lock(&state->lock);
+	*timings = state->timings;
+	mutex_unlock(&state->lock);
+
+	return 0;
+}
+
+static int tda1997x_query_dv_timings(struct v4l2_subdev *sd,
+				     struct v4l2_dv_timings *timings)
+{
+	struct tda1997x_state *state = to_state(sd);
+	int ret;
+
+	v4l_dbg(1, debug, state->client, "%s\n", __func__);
+	memset(timings, 0, sizeof(struct v4l2_dv_timings));
+	mutex_lock(&state->lock);
+	ret = tda1997x_detect_std(state, timings);
+	mutex_unlock(&state->lock);
+
+	return ret;
+}
+
+static const struct v4l2_subdev_video_ops tda1997x_video_ops = {
+	.g_input_status = tda1997x_g_input_status,
+	.s_dv_timings = tda1997x_s_dv_timings,
+	.g_dv_timings = tda1997x_g_dv_timings,
+	.query_dv_timings = tda1997x_query_dv_timings,
+};
+
+
+/* -----------------------------------------------------------------------------
+ * v4l2_subdev_pad_ops
+ */
+
+static int tda1997x_init_cfg(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *sd_state)
+{
+	struct tda1997x_state *state = to_state(sd);
+	struct v4l2_mbus_framefmt *mf;
+
+	mf = v4l2_subdev_get_try_format(sd, sd_state, 0);
+	mf->code = state->mbus_codes[0];
+
+	return 0;
+}
+
+static int tda1997x_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct tda1997x_state *state = to_state(sd);
+
+	v4l_dbg(1, debug, state->client, "%s %d\n", __func__, code->index);
+	if (code->index >= ARRAY_SIZE(state->mbus_codes))
+		return -EINVAL;
+
+	if (!state->mbus_codes[code->index])
+		return -EINVAL;
+
+	code->code = state->mbus_codes[code->index];
+
+	return 0;
+}
+
+static void tda1997x_fill_format(struct tda1997x_state *state,
+				 struct v4l2_mbus_framefmt *format)
+{
+	const struct v4l2_bt_timings *bt;
+
+	memset(format, 0, sizeof(*format));
+	bt = &state->timings.bt;
+	format->width = bt->width;
+	format->height = bt->height;
+	format->colorspace = state->colorimetry.colorspace;
+	format->field = (bt->interlaced) ?
+		V4L2_FIELD_SEQ_TB : V4L2_FIELD_NONE;
+}
+
+static int tda1997x_get_format(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_state *sd_state,
+			       struct v4l2_subdev_format *format)
+{
+	struct tda1997x_state *state = to_state(sd);
+
+	v4l_dbg(1, debug, state->client, "%s pad=%d which=%d\n",
+		__func__, format->pad, format->which);
+
+	tda1997x_fill_format(state, &format->format);
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		struct v4l2_mbus_framefmt *fmt;
+
+		fmt = v4l2_subdev_get_try_format(sd, sd_state, format->pad);
+		format->format.code = fmt->code;
+	} else
+		format->format.code = state->mbus_code;
+
+	return 0;
+}
+
+static int tda1997x_set_format(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_state *sd_state,
+			       struct v4l2_subdev_format *format)
+{
+	struct tda1997x_state *state = to_state(sd);
+	u32 code = 0;
+	int i;
+
+	v4l_dbg(1, debug, state->client, "%s pad=%d which=%d fmt=0x%x\n",
+		__func__, format->pad, format->which, format->format.code);
+
+	for (i = 0; i < ARRAY_SIZE(state->mbus_codes); i++) {
+		if (format->format.code == state->mbus_codes[i]) {
+			code = state->mbus_codes[i];
+			break;
+		}
+	}
+	if (!code)
+		code = state->mbus_codes[0];
+
+	tda1997x_fill_format(state, &format->format);
+	format->format.code = code;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		struct v4l2_mbus_framefmt *fmt;
+
+		fmt = v4l2_subdev_get_try_format(sd, sd_state, format->pad);
+		*fmt = format->format;
+	} else {
+		int ret = tda1997x_setup_format(state, format->format.code);
+
+		if (ret)
+			return ret;
+		/* mbus_code has changed - re-configure csc/vidout */
+		tda1997x_configure_csc(sd);
+		tda1997x_configure_vidout(state);
+	}
+
+	return 0;
+}
+
+static int tda1997x_get_edid(struct v4l2_subdev *sd, struct v4l2_edid *edid)
+{
+	struct tda1997x_state *state = to_state(sd);
+
+	v4l_dbg(1, debug, state->client, "%s pad=%d\n", __func__, edid->pad);
+	memset(edid->reserved, 0, sizeof(edid->reserved));
+
+	if (edid->start_block == 0 && edid->blocks == 0) {
+		edid->blocks = state->edid.blocks;
+		return 0;
+	}
+
+	if (!state->edid.present)
+		return -ENODATA;
+
+	if (edid->start_block >= state->edid.blocks)
+		return -EINVAL;
+
+	if (edid->start_block + edid->blocks > state->edid.blocks)
+		edid->blocks = state->edid.blocks - edid->start_block;
+
+	memcpy(edid->edid, state->edid.edid + edid->start_block * 128,
+	       edid->blocks * 128);
+
+	return 0;
+}
+
+static int tda1997x_set_edid(struct v4l2_subdev *sd, struct v4l2_edid *edid)
+{
+	struct tda1997x_state *state = to_state(sd);
+	int i;
+
+	v4l_dbg(1, debug, state->client, "%s pad=%d\n", __func__, edid->pad);
+	memset(edid->reserved, 0, sizeof(edid->reserved));
+
+	if (edid->start_block != 0)
+		return -EINVAL;
+
+	if (edid->blocks == 0) {
+		state->edid.blocks = 0;
+		state->edid.present = 0;
+		tda1997x_disable_edid(sd);
+		return 0;
+	}
+
+	if (edid->blocks > 2) {
+		edid->blocks = 2;
+		return -E2BIG;
+	}
+
+	tda1997x_disable_edid(sd);
+
+	/* write base EDID */
+	for (i = 0; i < 128; i++)
+		io_write(sd, REG_EDID_IN_BYTE0 + i, edid->edid[i]);
+
+	/* write CEA Extension */
+	for (i = 0; i < 128; i++)
+		io_write(sd, REG_EDID_IN_BYTE128 + i, edid->edid[i+128]);
+
+	/* store state */
+	memcpy(state->edid.edid, edid->edid, 256);
+	state->edid.blocks = edid->blocks;
+
+	tda1997x_enable_edid(sd);
+
+	return 0;
+}
+
+static int tda1997x_get_dv_timings_cap(struct v4l2_subdev *sd,
+				       struct v4l2_dv_timings_cap *cap)
+{
+	*cap = tda1997x_dv_timings_cap;
+	return 0;
+}
+
+static int tda1997x_enum_dv_timings(struct v4l2_subdev *sd,
+				    struct v4l2_enum_dv_timings *timings)
+{
+	return v4l2_enum_dv_timings_cap(timings, &tda1997x_dv_timings_cap,
+					NULL, NULL);
+}
+
+static const struct v4l2_subdev_pad_ops tda1997x_pad_ops = {
+	.init_cfg = tda1997x_init_cfg,
+	.enum_mbus_code = tda1997x_enum_mbus_code,
+	.get_fmt = tda1997x_get_format,
+	.set_fmt = tda1997x_set_format,
+	.get_edid = tda1997x_get_edid,
+	.set_edid = tda1997x_set_edid,
+	.dv_timings_cap = tda1997x_get_dv_timings_cap,
+	.enum_dv_timings = tda1997x_enum_dv_timings,
+};
+
+/* -----------------------------------------------------------------------------
+ * v4l2_subdev_core_ops
+ */
+
+static int tda1997x_log_infoframe(struct v4l2_subdev *sd, int addr)
+{
+	struct tda1997x_state *state = to_state(sd);
+	union hdmi_infoframe frame;
+	u8 buffer[40] = { 0 };
+	int len, err;
+
+	/* read data */
+	len = io_readn(sd, addr, sizeof(buffer), buffer);
+	v4l2_dbg(1, debug, sd, "infoframe: addr=%d len=%d\n", addr, len);
+	err = hdmi_infoframe_unpack(&frame, buffer, len);
+	if (err) {
+		v4l_err(state->client,
+			"failed parsing %d byte infoframe: 0x%04x/0x%02x\n",
+			len, addr, buffer[0]);
+		return err;
+	}
+	hdmi_infoframe_log(KERN_INFO, &state->client->dev, &frame);
+
+	return 0;
+}
+
+static int tda1997x_log_status(struct v4l2_subdev *sd)
+{
+	struct tda1997x_state *state = to_state(sd);
+	struct v4l2_dv_timings timings;
+	struct hdmi_avi_infoframe *avi = &state->avi_infoframe;
+
+	v4l2_info(sd, "-----Chip status-----\n");
+	v4l2_info(sd, "Chip: %s N%d\n", state->info->name,
+		  state->chip_revision + 1);
+	v4l2_info(sd, "EDID Enabled: %s\n", state->edid.present ? "yes" : "no");
+
+	v4l2_info(sd, "-----Signal status-----\n");
+	v4l2_info(sd, "Cable detected (+5V power): %s\n",
+		  tda1997x_detect_tx_5v(sd) ? "yes" : "no");
+	v4l2_info(sd, "HPD detected: %s\n",
+		  tda1997x_detect_tx_hpd(sd) ? "yes" : "no");
+
+	v4l2_info(sd, "-----Video Timings-----\n");
+	switch (tda1997x_detect_std(state, &timings)) {
+	case -ENOLINK:
+		v4l2_info(sd, "No video detected\n");
+		break;
+	case -ERANGE:
+		v4l2_info(sd, "Invalid signal detected\n");
+		break;
+	}
+	v4l2_print_dv_timings(sd->name, "Configured format: ",
+			      &state->timings, true);
+
+	v4l2_info(sd, "-----Color space-----\n");
+	v4l2_info(sd, "Input color space: %s %s %s",
+		  hdmi_colorspace_names[avi->colorspace],
+		  (avi->colorspace == HDMI_COLORSPACE_RGB) ? "" :
+			hdmi_colorimetry_names[avi->colorimetry],
+		  v4l2_quantization_names[state->colorimetry.quantization]);
+	v4l2_info(sd, "Output color space: %s",
+		  vidfmt_names[state->vid_fmt]);
+	v4l2_info(sd, "Color space conversion: %s", state->conv ?
+		  state->conv->name : "None");
+
+	v4l2_info(sd, "-----Audio-----\n");
+	if (state->audio_channels) {
+		v4l2_info(sd, "audio: %dch %dHz\n", state->audio_channels,
+			  state->audio_samplerate);
+	} else {
+		v4l2_info(sd, "audio: none\n");
+	}
+
+	v4l2_info(sd, "-----Infoframes-----\n");
+	tda1997x_log_infoframe(sd, AUD_IF);
+	tda1997x_log_infoframe(sd, SPD_IF);
+	tda1997x_log_infoframe(sd, AVI_IF);
+
+	return 0;
+}
+
+static int tda1997x_subscribe_event(struct v4l2_subdev *sd,
+				    struct v4l2_fh *fh,
+				    struct v4l2_event_subscription *sub)
+{
+	switch (sub->type) {
+	case V4L2_EVENT_SOURCE_CHANGE:
+		return v4l2_src_change_event_subdev_subscribe(sd, fh, sub);
+	case V4L2_EVENT_CTRL:
+		return v4l2_ctrl_subdev_subscribe_event(sd, fh, sub);
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct v4l2_subdev_core_ops tda1997x_core_ops = {
+	.log_status = tda1997x_log_status,
+	.subscribe_event = tda1997x_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+/* -----------------------------------------------------------------------------
+ * v4l2_subdev_ops
+ */
+
+static const struct v4l2_subdev_ops tda1997x_subdev_ops = {
+	.core = &tda1997x_core_ops,
+	.video = &tda1997x_video_ops,
+	.pad = &tda1997x_pad_ops,
+};
+
+/* -----------------------------------------------------------------------------
+ * v4l2_controls
+ */
+
+static int tda1997x_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+	struct tda1997x_state *state = to_state(sd);
+
+	switch (ctrl->id) {
+	/* allow overriding the default RGB quantization range */
+	case V4L2_CID_DV_RX_RGB_RANGE:
+		state->rgb_quantization_range = ctrl->val;
+		set_rgb_quantization_range(state);
+		tda1997x_configure_csc(sd);
+		return 0;
+	}
+
+	return -EINVAL;
+};
+
+static int tda1997x_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+	struct tda1997x_state *state = to_state(sd);
+
+	if (ctrl->id == V4L2_CID_DV_RX_IT_CONTENT_TYPE) {
+		ctrl->val = state->avi_infoframe.content_type;
+		return 0;
+	}
+	return -EINVAL;
+};
+
+static const struct v4l2_ctrl_ops tda1997x_ctrl_ops = {
+	.s_ctrl = tda1997x_s_ctrl,
+	.g_volatile_ctrl = tda1997x_g_volatile_ctrl,
+};
+
+static int tda1997x_core_init(struct v4l2_subdev *sd)
+{
+	struct tda1997x_state *state = to_state(sd);
+	struct tda1997x_platform_data *pdata = &state->pdata;
+	u8 reg;
+	int i;
+
+	/* disable HPD */
+	io_write(sd, REG_HPD_AUTO_CTRL, HPD_AUTO_HPD_UNSEL);
+	if (state->chip_revision == 0) {
+		io_write(sd, REG_MAN_SUS_HDMI_SEL, MAN_DIS_HDCP | MAN_RST_HDCP);
+		io_write(sd, REG_CGU_DBG_SEL, 1 << CGU_DBG_CLK_SEL_SHIFT);
+	}
+
+	/* reset infoframe at end of start-up-sequencer */
+	io_write(sd, REG_SUS_SET_RGB2, 0x06);
+	io_write(sd, REG_SUS_SET_RGB3, 0x06);
+
+	/* Enable TMDS pull-ups */
+	io_write(sd, REG_RT_MAN_CTRL, RT_MAN_CTRL_RT |
+		 RT_MAN_CTRL_RT_B | RT_MAN_CTRL_RT_A);
+
+	/* enable sync measurement timing */
+	tda1997x_cec_write(sd, REG_PWR_CONTROL & 0xff, 0x04);
+	/* adjust CEC clock divider */
+	tda1997x_cec_write(sd, REG_OSC_DIVIDER & 0xff, 0x03);
+	tda1997x_cec_write(sd, REG_EN_OSC_PERIOD_LSB & 0xff, 0xa0);
+	io_write(sd, REG_TIMER_D, 0x54);
+	/* enable power switch */
+	reg = tda1997x_cec_read(sd, REG_CONTROL & 0xff);
+	reg |= 0x20;
+	tda1997x_cec_write(sd, REG_CONTROL & 0xff, reg);
+	mdelay(50);
+
+	/* read the chip version */
+	reg = io_read(sd, REG_VERSION);
+	/* get the chip configuration */
+	reg = io_read(sd, REG_CMTP_REG10);
+
+	/* enable interrupts we care about */
+	io_write(sd, REG_INT_MASK_TOP,
+		 INTERRUPT_HDCP | INTERRUPT_AUDIO | INTERRUPT_INFO |
+		 INTERRUPT_RATE | INTERRUPT_SUS);
+	/* config_mtp,fmt,sus_end,sus_st */
+	io_write(sd, REG_INT_MASK_SUS, MASK_MPT | MASK_FMT | MASK_SUS_END);
+	/* rate stability change for inputs A/B */
+	io_write(sd, REG_INT_MASK_RATE, MASK_RATE_B_ST | MASK_RATE_A_ST);
+	/* aud,spd,avi*/
+	io_write(sd, REG_INT_MASK_INFO,
+		 MASK_AUD_IF | MASK_SPD_IF | MASK_AVI_IF);
+	/* audio_freq,audio_flg,mute_flg,fifo_err */
+	io_write(sd, REG_INT_MASK_AUDIO,
+		 MASK_AUDIO_FREQ_FLG | MASK_AUDIO_FLG | MASK_MUTE_FLG |
+		 MASK_ERROR_FIFO_PT);
+	/* HDCP C5 state reached */
+	io_write(sd, REG_INT_MASK_HDCP, MASK_STATE_C5);
+	/* 5V detect and HDP pulse end */
+	io_write(sd, REG_INT_MASK_DDC, MASK_DET_5V);
+	/* don't care about AFE/MODE */
+	io_write(sd, REG_INT_MASK_AFE, 0);
+	io_write(sd, REG_INT_MASK_MODE, 0);
+
+	/* clear all interrupts */
+	io_write(sd, REG_INT_FLG_CLR_TOP, 0xff);
+	io_write(sd, REG_INT_FLG_CLR_SUS, 0xff);
+	io_write(sd, REG_INT_FLG_CLR_DDC, 0xff);
+	io_write(sd, REG_INT_FLG_CLR_RATE, 0xff);
+	io_write(sd, REG_INT_FLG_CLR_MODE, 0xff);
+	io_write(sd, REG_INT_FLG_CLR_INFO, 0xff);
+	io_write(sd, REG_INT_FLG_CLR_AUDIO, 0xff);
+	io_write(sd, REG_INT_FLG_CLR_HDCP, 0xff);
+	io_write(sd, REG_INT_FLG_CLR_AFE, 0xff);
+
+	/* init TMDS equalizer */
+	if (state->chip_revision == 0)
+		io_write(sd, REG_CGU_DBG_SEL, 1 << CGU_DBG_CLK_SEL_SHIFT);
+	io_write24(sd, REG_CLK_MIN_RATE, CLK_MIN_RATE);
+	io_write24(sd, REG_CLK_MAX_RATE, CLK_MAX_RATE);
+	if (state->chip_revision == 0)
+		io_write(sd, REG_WDL_CFG, WDL_CFG_VAL);
+	/* DC filter */
+	io_write(sd, REG_DEEP_COLOR_CTRL, DC_FILTER_VAL);
+	/* disable test pattern */
+	io_write(sd, REG_SVC_MODE, 0x00);
+	/* update HDMI INFO CTRL */
+	io_write(sd, REG_INFO_CTRL, 0xff);
+	/* write HDMI INFO EXCEED value */
+	io_write(sd, REG_INFO_EXCEED, 3);
+
+	if (state->chip_revision == 0)
+		tda1997x_reset_n1(state);
+
+	/*
+	 * No HDCP acknowledge when HDCP is disabled
+	 * and reset SUS to force format detection
+	 */
+	tda1997x_hdmi_info_reset(sd, NACK_HDCP, true);
+
+	/* Set HPD low */
+	tda1997x_manual_hpd(sd, HPD_LOW_BP);
+
+	/* Configure receiver capabilities */
+	io_write(sd, REG_HDCP_BCAPS, HDCP_HDMI | HDCP_FAST_REAUTH);
+
+	/* Configure HDMI: Auto HDCP mode, packet controlled mute */
+	reg = HDMI_CTRL_MUTE_AUTO << HDMI_CTRL_MUTE_SHIFT;
+	reg |= HDMI_CTRL_HDCP_AUTO << HDMI_CTRL_HDCP_SHIFT;
+	io_write(sd, REG_HDMI_CTRL, reg);
+
+	/* reset start-up-sequencer to force format detection */
+	tda1997x_hdmi_info_reset(sd, 0, true);
+
+	/* disable matrix conversion */
+	reg = io_read(sd, REG_VDP_CTRL);
+	reg |= VDP_CTRL_MATRIX_BP;
+	io_write(sd, REG_VDP_CTRL, reg);
+
+	/* set video output mode */
+	tda1997x_configure_vidout(state);
+
+	/* configure video output port */
+	for (i = 0; i < 9; i++) {
+		v4l_dbg(1, debug, state->client, "vidout_cfg[%d]=0x%02x\n", i,
+			pdata->vidout_port_cfg[i]);
+		io_write(sd, REG_VP35_32_CTRL + i, pdata->vidout_port_cfg[i]);
+	}
+
+	/* configure audio output port */
+	tda1997x_configure_audout(sd, 0);
+
+	/* configure audio clock freq */
+	switch (pdata->audout_mclk_fs) {
+	case 512:
+		reg = AUDIO_CLOCK_SEL_512FS;
+		break;
+	case 256:
+		reg = AUDIO_CLOCK_SEL_256FS;
+		break;
+	case 128:
+		reg = AUDIO_CLOCK_SEL_128FS;
+		break;
+	case 64:
+		reg = AUDIO_CLOCK_SEL_64FS;
+		break;
+	case 32:
+		reg = AUDIO_CLOCK_SEL_32FS;
+		break;
+	default:
+		reg = AUDIO_CLOCK_SEL_16FS;
+		break;
+	}
+	io_write(sd, REG_AUDIO_CLOCK, reg);
+
+	/* reset advanced infoframes (ISRC1/ISRC2/ACP) */
+	tda1997x_hdmi_info_reset(sd, RESET_AI, false);
+	/* reset infoframe */
+	tda1997x_hdmi_info_reset(sd, RESET_IF, false);
+	/* reset audio infoframes */
+	tda1997x_hdmi_info_reset(sd, RESET_AUDIO, false);
+	/* reset gamut */
+	tda1997x_hdmi_info_reset(sd, RESET_GAMUT, false);
+
+	/* get initial HDMI status */
+	state->hdmi_status = io_read(sd, REG_HDMI_FLAGS);
+
+	io_write(sd, REG_EDID_ENABLE, EDID_ENABLE_A_EN | EDID_ENABLE_B_EN);
+	return 0;
+}
+
+static int tda1997x_set_power(struct tda1997x_state *state, bool on)
+{
+	int ret = 0;
+
+	if (on) {
+		ret = regulator_bulk_enable(TDA1997X_NUM_SUPPLIES,
+					     state->supplies);
+		msleep(300);
+	} else {
+		ret = regulator_bulk_disable(TDA1997X_NUM_SUPPLIES,
+					     state->supplies);
+	}
+
+	return ret;
+}
+
+static const struct i2c_device_id tda1997x_i2c_id[] = {
+	{"tda19971", (kernel_ulong_t)&tda1997x_chip_info[TDA19971]},
+	{"tda19973", (kernel_ulong_t)&tda1997x_chip_info[TDA19973]},
+	{ },
+};
+MODULE_DEVICE_TABLE(i2c, tda1997x_i2c_id);
+
+static const struct of_device_id tda1997x_of_id[] __maybe_unused = {
+	{ .compatible = "nxp,tda19971", .data = &tda1997x_chip_info[TDA19971] },
+	{ .compatible = "nxp,tda19973", .data = &tda1997x_chip_info[TDA19973] },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, tda1997x_of_id);
+
+static int tda1997x_parse_dt(struct tda1997x_state *state)
+{
+	struct tda1997x_platform_data *pdata = &state->pdata;
+	struct v4l2_fwnode_endpoint bus_cfg = { .bus_type = 0 };
+	struct device_node *ep;
+	struct device_node *np;
+	unsigned int flags;
+	const char *str;
+	int ret;
+	u32 v;
+
+	/*
+	 * setup default values:
+	 * - HREF: active high from start to end of row
+	 * - VS: Vertical Sync active high at beginning of frame
+	 * - DE: Active high when data valid
+	 * - A_CLK: 128*Fs
+	 */
+	pdata->vidout_sel_hs = HS_HREF_SEL_HREF_VHREF;
+	pdata->vidout_sel_vs = VS_VREF_SEL_VREF_HDMI;
+	pdata->vidout_sel_de = DE_FREF_SEL_DE_VHREF;
+
+	np = state->client->dev.of_node;
+	ep = of_graph_get_next_endpoint(np, NULL);
+	if (!ep)
+		return -EINVAL;
+
+	ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(ep), &bus_cfg);
+	if (ret) {
+		of_node_put(ep);
+		return ret;
+	}
+	of_node_put(ep);
+	pdata->vidout_bus_type = bus_cfg.bus_type;
+
+	/* polarity of HS/VS/DE */
+	flags = bus_cfg.bus.parallel.flags;
+	if (flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
+		pdata->vidout_inv_hs = 1;
+	if (flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
+		pdata->vidout_inv_vs = 1;
+	if (flags & V4L2_MBUS_DATA_ACTIVE_LOW)
+		pdata->vidout_inv_de = 1;
+	pdata->vidout_bus_width = bus_cfg.bus.parallel.bus_width;
+
+	/* video output port config */
+	ret = of_property_count_u32_elems(np, "nxp,vidout-portcfg");
+	if (ret > 0) {
+		u32 reg, val, i;
+
+		for (i = 0; i < ret / 2 && i < 9; i++) {
+			of_property_read_u32_index(np, "nxp,vidout-portcfg",
+						   i * 2, &reg);
+			of_property_read_u32_index(np, "nxp,vidout-portcfg",
+						   i * 2 + 1, &val);
+			if (reg < 9)
+				pdata->vidout_port_cfg[reg] = val;
+		}
+	} else {
+		v4l_err(state->client, "nxp,vidout-portcfg missing\n");
+		return -EINVAL;
+	}
+
+	/* default to channel layout dictated by packet header */
+	pdata->audout_layoutauto = true;
+
+	pdata->audout_format = AUDFMT_TYPE_DISABLED;
+	if (!of_property_read_string(np, "nxp,audout-format", &str)) {
+		if (strcmp(str, "i2s") == 0)
+			pdata->audout_format = AUDFMT_TYPE_I2S;
+		else if (strcmp(str, "spdif") == 0)
+			pdata->audout_format = AUDFMT_TYPE_SPDIF;
+		else {
+			v4l_err(state->client, "nxp,audout-format invalid\n");
+			return -EINVAL;
+		}
+		if (!of_property_read_u32(np, "nxp,audout-layout", &v)) {
+			switch (v) {
+			case 0:
+			case 1:
+				break;
+			default:
+				v4l_err(state->client,
+					"nxp,audout-layout invalid\n");
+				return -EINVAL;
+			}
+			pdata->audout_layout = v;
+		}
+		if (!of_property_read_u32(np, "nxp,audout-width", &v)) {
+			switch (v) {
+			case 16:
+			case 32:
+				break;
+			default:
+				v4l_err(state->client,
+					"nxp,audout-width invalid\n");
+				return -EINVAL;
+			}
+			pdata->audout_width = v;
+		}
+		if (!of_property_read_u32(np, "nxp,audout-mclk-fs", &v)) {
+			switch (v) {
+			case 512:
+			case 256:
+			case 128:
+			case 64:
+			case 32:
+			case 16:
+				break;
+			default:
+				v4l_err(state->client,
+					"nxp,audout-mclk-fs invalid\n");
+				return -EINVAL;
+			}
+			pdata->audout_mclk_fs = v;
+		}
+	}
+
+	return 0;
+}
+
+static int tda1997x_get_regulators(struct tda1997x_state *state)
+{
+	int i;
+
+	for (i = 0; i < TDA1997X_NUM_SUPPLIES; i++)
+		state->supplies[i].supply = tda1997x_supply_name[i];
+
+	return devm_regulator_bulk_get(&state->client->dev,
+				       TDA1997X_NUM_SUPPLIES,
+				       state->supplies);
+}
+
+static int tda1997x_identify_module(struct tda1997x_state *state)
+{
+	struct v4l2_subdev *sd = &state->sd;
+	enum tda1997x_type type;
+	u8 reg;
+
+	/* Read chip configuration*/
+	reg = io_read(sd, REG_CMTP_REG10);
+	state->tmdsb_clk = (reg >> 6) & 0x01; /* use tmds clock B_inv for B */
+	state->tmdsb_soc = (reg >> 5) & 0x01; /* tmds of input B */
+	state->port_30bit = (reg >> 2) & 0x03; /* 30bit vs 24bit */
+	state->output_2p5 = (reg >> 1) & 0x01; /* output supply 2.5v */
+	switch ((reg >> 4) & 0x03) {
+	case 0x00:
+		type = TDA19971;
+		break;
+	case 0x02:
+	case 0x03:
+		type = TDA19973;
+		break;
+	default:
+		dev_err(&state->client->dev, "unsupported chip ID\n");
+		return -EIO;
+	}
+	if (state->info->type != type) {
+		dev_err(&state->client->dev, "chip id mismatch\n");
+		return -EIO;
+	}
+
+	/* read chip revision */
+	state->chip_revision = io_read(sd, REG_CMTP_REG11);
+
+	return 0;
+}
+
+static const struct media_entity_operations tda1997x_media_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+
+/* -----------------------------------------------------------------------------
+ * HDMI Audio Codec
+ */
+
+/* refine sample-rate based on HDMI source */
+static int tda1997x_pcm_startup(struct snd_pcm_substream *substream,
+				struct snd_soc_dai *dai)
+{
+	struct v4l2_subdev *sd = snd_soc_dai_get_drvdata(dai);
+	struct tda1997x_state *state = to_state(sd);
+	struct snd_soc_component *component = dai->component;
+	struct snd_pcm_runtime *rtd = substream->runtime;
+	int rate, err;
+
+	rate = state->audio_samplerate;
+	err = snd_pcm_hw_constraint_minmax(rtd, SNDRV_PCM_HW_PARAM_RATE,
+					   rate, rate);
+	if (err < 0) {
+		dev_err(component->dev, "failed to constrain samplerate to %dHz\n",
+			rate);
+		return err;
+	}
+	dev_info(component->dev, "set samplerate constraint to %dHz\n", rate);
+
+	return 0;
+}
+
+static const struct snd_soc_dai_ops tda1997x_dai_ops = {
+	.startup = tda1997x_pcm_startup,
+};
+
+static struct snd_soc_dai_driver tda1997x_audio_dai = {
+	.name = "tda1997x",
+	.capture = {
+		.stream_name = "Capture",
+		.channels_min = 2,
+		.channels_max = 8,
+		.rates = SNDRV_PCM_RATE_32000 | SNDRV_PCM_RATE_44100 |
+			 SNDRV_PCM_RATE_48000 | SNDRV_PCM_RATE_88200 |
+			 SNDRV_PCM_RATE_96000 | SNDRV_PCM_RATE_176400 |
+			 SNDRV_PCM_RATE_192000,
+	},
+	.ops = &tda1997x_dai_ops,
+};
+
+static int tda1997x_codec_probe(struct snd_soc_component *component)
+{
+	return 0;
+}
+
+static void tda1997x_codec_remove(struct snd_soc_component *component)
+{
+}
+
+static struct snd_soc_component_driver tda1997x_codec_driver = {
+	.probe			= tda1997x_codec_probe,
+	.remove			= tda1997x_codec_remove,
+	.idle_bias_on		= 1,
+	.use_pmdown_time	= 1,
+	.endianness		= 1,
+};
+
+static int tda1997x_probe(struct i2c_client *client)
+{
+	const struct i2c_device_id *id = i2c_client_get_device_id(client);
+	struct tda1997x_state *state;
+	struct tda1997x_platform_data *pdata;
+	struct v4l2_subdev *sd;
+	struct v4l2_ctrl_handler *hdl;
+	struct v4l2_ctrl *ctrl;
+	static const struct v4l2_dv_timings cea1920x1080 =
+		V4L2_DV_BT_CEA_1920X1080P60;
+	u32 *mbus_codes;
+	int i, ret;
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+
+	state = kzalloc(sizeof(struct tda1997x_state), GFP_KERNEL);
+	if (!state)
+		return -ENOMEM;
+
+	state->client = client;
+	pdata = &state->pdata;
+	if (IS_ENABLED(CONFIG_OF) && client->dev.of_node) {
+		const struct of_device_id *oid;
+
+		oid = of_match_node(tda1997x_of_id, client->dev.of_node);
+		state->info = oid->data;
+
+		ret = tda1997x_parse_dt(state);
+		if (ret < 0) {
+			v4l_err(client, "DT parsing error\n");
+			goto err_free_state;
+		}
+	} else if (client->dev.platform_data) {
+		struct tda1997x_platform_data *pdata =
+			client->dev.platform_data;
+		state->info =
+			(const struct tda1997x_chip_info *)id->driver_data;
+		state->pdata = *pdata;
+	} else {
+		v4l_err(client, "No platform data\n");
+		ret = -ENODEV;
+		goto err_free_state;
+	}
+
+	ret = tda1997x_get_regulators(state);
+	if (ret)
+		goto err_free_state;
+
+	ret = tda1997x_set_power(state, 1);
+	if (ret)
+		goto err_free_state;
+
+	mutex_init(&state->page_lock);
+	mutex_init(&state->lock);
+	state->page = 0xff;
+
+	INIT_DELAYED_WORK(&state->delayed_work_enable_hpd,
+			  tda1997x_delayed_work_enable_hpd);
+
+	/* set video format based on chip and bus width */
+	ret = tda1997x_identify_module(state);
+	if (ret)
+		goto err_free_mutex;
+
+	/* initialize subdev */
+	sd = &state->sd;
+	v4l2_i2c_subdev_init(sd, client, &tda1997x_subdev_ops);
+	snprintf(sd->name, sizeof(sd->name), "%s %d-%04x",
+		 id->name, i2c_adapter_id(client->adapter),
+		 client->addr);
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
+	sd->entity.function = MEDIA_ENT_F_DV_DECODER;
+	sd->entity.ops = &tda1997x_media_ops;
+
+	/* set allowed mbus modes based on chip, bus-type, and bus-width */
+	i = 0;
+	mbus_codes = state->mbus_codes;
+	switch (state->info->type) {
+	case TDA19973:
+		switch (pdata->vidout_bus_type) {
+		case V4L2_MBUS_PARALLEL:
+			switch (pdata->vidout_bus_width) {
+			case 36:
+				mbus_codes[i++] = MEDIA_BUS_FMT_RGB121212_1X36;
+				mbus_codes[i++] = MEDIA_BUS_FMT_YUV12_1X36;
+				fallthrough;
+			case 24:
+				mbus_codes[i++] = MEDIA_BUS_FMT_UYVY12_1X24;
+				break;
+			}
+			break;
+		case V4L2_MBUS_BT656:
+			switch (pdata->vidout_bus_width) {
+			case 36:
+			case 24:
+			case 12:
+				mbus_codes[i++] = MEDIA_BUS_FMT_UYVY12_2X12;
+				mbus_codes[i++] = MEDIA_BUS_FMT_UYVY10_2X10;
+				mbus_codes[i++] = MEDIA_BUS_FMT_UYVY8_2X8;
+				break;
+			}
+			break;
+		default:
+			break;
+		}
+		break;
+	case TDA19971:
+		switch (pdata->vidout_bus_type) {
+		case V4L2_MBUS_PARALLEL:
+			switch (pdata->vidout_bus_width) {
+			case 24:
+				mbus_codes[i++] = MEDIA_BUS_FMT_RGB888_1X24;
+				mbus_codes[i++] = MEDIA_BUS_FMT_YUV8_1X24;
+				mbus_codes[i++] = MEDIA_BUS_FMT_UYVY12_1X24;
+				fallthrough;
+			case 20:
+				mbus_codes[i++] = MEDIA_BUS_FMT_UYVY10_1X20;
+				fallthrough;
+			case 16:
+				mbus_codes[i++] = MEDIA_BUS_FMT_UYVY8_1X16;
+				break;
+			}
+			break;
+		case V4L2_MBUS_BT656:
+			switch (pdata->vidout_bus_width) {
+			case 24:
+			case 20:
+			case 16:
+			case 12:
+				mbus_codes[i++] = MEDIA_BUS_FMT_UYVY12_2X12;
+				fallthrough;
+			case 10:
+				mbus_codes[i++] = MEDIA_BUS_FMT_UYVY10_2X10;
+				fallthrough;
+			case 8:
+				mbus_codes[i++] = MEDIA_BUS_FMT_UYVY8_2X8;
+				break;
+			}
+			break;
+		default:
+			break;
+		}
+		break;
+	}
+	if (WARN_ON(i > ARRAY_SIZE(state->mbus_codes))) {
+		ret = -EINVAL;
+		goto err_free_mutex;
+	}
+
+	/* default format */
+	tda1997x_setup_format(state, state->mbus_codes[0]);
+	state->timings = cea1920x1080;
+
+	/*
+	 * default to SRGB full range quantization
+	 * (in case we don't get an infoframe such as DVI signal
+	 */
+	state->colorimetry.colorspace = V4L2_COLORSPACE_SRGB;
+	state->colorimetry.quantization = V4L2_QUANTIZATION_FULL_RANGE;
+
+	/* disable/reset HDCP to get correct I2C access to Rx HDMI */
+	io_write(sd, REG_MAN_SUS_HDMI_SEL, MAN_RST_HDCP | MAN_DIS_HDCP);
+
+	/*
+	 * if N2 version, reset compdel_bp as it may generate some small pixel
+	 * shifts in case of embedded sync/or delay lower than 4
+	 */
+	if (state->chip_revision != 0) {
+		io_write(sd, REG_MAN_SUS_HDMI_SEL, 0x00);
+		io_write(sd, REG_VDP_CTRL, 0x1f);
+	}
+
+	v4l_info(client, "NXP %s N%d detected\n", state->info->name,
+		 state->chip_revision + 1);
+	v4l_info(client, "video: %dbit %s %d formats available\n",
+		pdata->vidout_bus_width,
+		(pdata->vidout_bus_type == V4L2_MBUS_PARALLEL) ?
+			"parallel" : "BT656",
+		i);
+	if (pdata->audout_format) {
+		v4l_info(client, "audio: %dch %s layout%d sysclk=%d*fs\n",
+			 pdata->audout_layout ? 2 : 8,
+			 audfmt_names[pdata->audout_format],
+			 pdata->audout_layout,
+			 pdata->audout_mclk_fs);
+	}
+
+	ret = 0x34 + ((io_read(sd, REG_SLAVE_ADDR)>>4) & 0x03);
+	state->client_cec = devm_i2c_new_dummy_device(&client->dev,
+						      client->adapter, ret);
+	if (IS_ERR(state->client_cec)) {
+		ret = PTR_ERR(state->client_cec);
+		goto err_free_mutex;
+	}
+
+	v4l_info(client, "CEC slave address 0x%02x\n", ret);
+
+	ret = tda1997x_core_init(sd);
+	if (ret)
+		goto err_free_mutex;
+
+	/* control handlers */
+	hdl = &state->hdl;
+	v4l2_ctrl_handler_init(hdl, 3);
+	ctrl = v4l2_ctrl_new_std_menu(hdl, &tda1997x_ctrl_ops,
+			V4L2_CID_DV_RX_IT_CONTENT_TYPE,
+			V4L2_DV_IT_CONTENT_TYPE_NO_ITC, 0,
+			V4L2_DV_IT_CONTENT_TYPE_NO_ITC);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_VOLATILE;
+	/* custom controls */
+	state->detect_tx_5v_ctrl = v4l2_ctrl_new_std(hdl, NULL,
+			V4L2_CID_DV_RX_POWER_PRESENT, 0, 1, 0, 0);
+	state->rgb_quantization_range_ctrl = v4l2_ctrl_new_std_menu(hdl,
+			&tda1997x_ctrl_ops,
+			V4L2_CID_DV_RX_RGB_RANGE, V4L2_DV_RGB_RANGE_FULL, 0,
+			V4L2_DV_RGB_RANGE_AUTO);
+	state->sd.ctrl_handler = hdl;
+	if (hdl->error) {
+		ret = hdl->error;
+		goto err_free_handler;
+	}
+	v4l2_ctrl_handler_setup(hdl);
+
+	/* initialize source pads */
+	state->pads[TDA1997X_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&sd->entity, TDA1997X_NUM_PADS,
+		state->pads);
+	if (ret) {
+		v4l_err(client, "failed entity_init: %d", ret);
+		goto err_free_handler;
+	}
+
+	ret = v4l2_async_register_subdev(sd);
+	if (ret)
+		goto err_free_media;
+
+	/* register audio DAI */
+	if (pdata->audout_format) {
+		u64 formats;
+
+		if (pdata->audout_width == 32)
+			formats = SNDRV_PCM_FMTBIT_S32_LE;
+		else
+			formats = SNDRV_PCM_FMTBIT_S16_LE;
+		tda1997x_audio_dai.capture.formats = formats;
+		ret = devm_snd_soc_register_component(&state->client->dev,
+					     &tda1997x_codec_driver,
+					     &tda1997x_audio_dai, 1);
+		if (ret) {
+			dev_err(&client->dev, "register audio codec failed\n");
+			goto err_free_media;
+		}
+		v4l_info(state->client, "registered audio codec\n");
+	}
+
+	/* request irq */
+	ret = devm_request_threaded_irq(&client->dev, client->irq,
+					NULL, tda1997x_isr_thread,
+					IRQF_TRIGGER_LOW | IRQF_ONESHOT,
+					KBUILD_MODNAME, state);
+	if (ret) {
+		v4l_err(client, "irq%d reg failed: %d\n", client->irq, ret);
+		goto err_free_media;
+	}
+
+	return 0;
+
+err_free_media:
+	media_entity_cleanup(&sd->entity);
+err_free_handler:
+	v4l2_ctrl_handler_free(&state->hdl);
+err_free_mutex:
+	cancel_delayed_work(&state->delayed_work_enable_hpd);
+	mutex_destroy(&state->page_lock);
+	mutex_destroy(&state->lock);
+	tda1997x_set_power(state, 0);
+err_free_state:
+	kfree(state);
+	dev_err(&client->dev, "%s failed: %d\n", __func__, ret);
+
+	return ret;
+}
+
+static void tda1997x_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct tda1997x_state *state = to_state(sd);
+	struct tda1997x_platform_data *pdata = &state->pdata;
+
+	if (pdata->audout_format) {
+		mutex_destroy(&state->audio_lock);
+	}
+
+	disable_irq(state->client->irq);
+	tda1997x_power_mode(state, 0);
+
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(&state->hdl);
+	regulator_bulk_disable(TDA1997X_NUM_SUPPLIES, state->supplies);
+	cancel_delayed_work_sync(&state->delayed_work_enable_hpd);
+	mutex_destroy(&state->page_lock);
+	mutex_destroy(&state->lock);
+
+	kfree(state);
+}
+
+static struct i2c_driver tda1997x_i2c_driver = {
+	.driver = {
+		.name = "tda1997x",
+		.of_match_table = of_match_ptr(tda1997x_of_id),
+	},
+	.probe = tda1997x_probe,
+	.remove = tda1997x_remove,
+	.id_table = tda1997x_i2c_id,
+};
+
+module_i2c_driver(tda1997x_i2c_driver);
+
+MODULE_AUTHOR("Tim Harvey <tharvey@gateworks.com>");
+MODULE_DESCRIPTION("TDA1997X HDMI Receiver driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/tda1997x_regs.h b/drivers/media/i2c/tda1997x_regs.h
new file mode 100644
index 000000000..115371ba3
--- /dev/null
+++ b/drivers/media/i2c/tda1997x_regs.h
@@ -0,0 +1,644 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2018 Gateworks Corporation
+ */
+
+/* Page 0x00 - General Control */
+#define REG_VERSION		0x0000
+#define REG_INPUT_SEL		0x0001
+#define REG_SVC_MODE		0x0002
+#define REG_HPD_MAN_CTRL	0x0003
+#define REG_RT_MAN_CTRL		0x0004
+#define REG_STANDBY_SOFT_RST	0x000A
+#define REG_HDMI_SOFT_RST	0x000B
+#define REG_HDMI_INFO_RST	0x000C
+#define REG_INT_FLG_CLR_TOP	0x000E
+#define REG_INT_FLG_CLR_SUS	0x000F
+#define REG_INT_FLG_CLR_DDC	0x0010
+#define REG_INT_FLG_CLR_RATE	0x0011
+#define REG_INT_FLG_CLR_MODE	0x0012
+#define REG_INT_FLG_CLR_INFO	0x0013
+#define REG_INT_FLG_CLR_AUDIO	0x0014
+#define REG_INT_FLG_CLR_HDCP	0x0015
+#define REG_INT_FLG_CLR_AFE	0x0016
+#define REG_INT_MASK_TOP	0x0017
+#define REG_INT_MASK_SUS	0x0018
+#define REG_INT_MASK_DDC	0x0019
+#define REG_INT_MASK_RATE	0x001A
+#define REG_INT_MASK_MODE	0x001B
+#define REG_INT_MASK_INFO	0x001C
+#define REG_INT_MASK_AUDIO	0x001D
+#define REG_INT_MASK_HDCP	0x001E
+#define REG_INT_MASK_AFE	0x001F
+#define REG_DETECT_5V		0x0020
+#define REG_SUS_STATUS		0x0021
+#define REG_V_PER		0x0022
+#define REG_H_PER		0x0025
+#define REG_HS_WIDTH		0x0027
+#define REG_FMT_H_TOT		0x0029
+#define REG_FMT_H_ACT		0x002b
+#define REG_FMT_H_FRONT		0x002d
+#define REG_FMT_H_SYNC		0x002f
+#define REG_FMT_H_BACK		0x0031
+#define REG_FMT_V_TOT		0x0033
+#define REG_FMT_V_ACT		0x0035
+#define REG_FMT_V_FRONT_F1	0x0037
+#define REG_FMT_V_FRONT_F2	0x0038
+#define REG_FMT_V_SYNC		0x0039
+#define REG_FMT_V_BACK_F1	0x003a
+#define REG_FMT_V_BACK_F2	0x003b
+#define REG_FMT_DE_ACT		0x003c
+#define REG_RATE_CTRL		0x0040
+#define REG_CLK_MIN_RATE	0x0043
+#define REG_CLK_MAX_RATE	0x0046
+#define REG_CLK_A_STATUS	0x0049
+#define REG_CLK_A_RATE		0x004A
+#define REG_DRIFT_CLK_A_REG	0x004D
+#define REG_CLK_B_STATUS	0x004E
+#define REG_CLK_B_RATE		0x004F
+#define REG_DRIFT_CLK_B_REG	0x0052
+#define REG_HDCP_CTRL		0x0060
+#define REG_HDCP_KDS		0x0061
+#define REG_HDCP_BCAPS		0x0063
+#define REG_HDCP_KEY_CTRL	0x0064
+#define REG_INFO_CTRL		0x0076
+#define REG_INFO_EXCEED		0x0077
+#define REG_PIX_REPEAT		0x007B
+#define REG_AUDIO_PATH		0x007C
+#define REG_AUDCFG		0x007D
+#define REG_AUDIO_OUT_ENABLE	0x007E
+#define REG_AUDIO_OUT_HIZ	0x007F
+#define REG_VDP_CTRL		0x0080
+#define REG_VDP_MATRIX		0x0081
+#define REG_VHREF_CTRL		0x00A0
+#define REG_PXCNT_PR		0x00A2
+#define REG_PXCNT_NPIX		0x00A4
+#define REG_LCNT_PR		0x00A6
+#define REG_LCNT_NLIN		0x00A8
+#define REG_HREF_S		0x00AA
+#define REG_HREF_E		0x00AC
+#define REG_HS_S		0x00AE
+#define REG_HS_E		0x00B0
+#define REG_VREF_F1_S		0x00B2
+#define REG_VREF_F1_WIDTH	0x00B4
+#define REG_VREF_F2_S		0x00B5
+#define REG_VREF_F2_WIDTH	0x00B7
+#define REG_VS_F1_LINE_S	0x00B8
+#define REG_VS_F1_LINE_WIDTH	0x00BA
+#define REG_VS_F2_LINE_S	0x00BB
+#define REG_VS_F2_LINE_WIDTH	0x00BD
+#define REG_VS_F1_PIX_S		0x00BE
+#define REG_VS_F1_PIX_E		0x00C0
+#define REG_VS_F2_PIX_S		0x00C2
+#define REG_VS_F2_PIX_E		0x00C4
+#define REG_FREF_F1_S		0x00C6
+#define REG_FREF_F2_S		0x00C8
+#define REG_FDW_S		0x00ca
+#define REG_FDW_E		0x00cc
+#define REG_BLK_GY		0x00da
+#define REG_BLK_BU		0x00dc
+#define REG_BLK_RV		0x00de
+#define REG_FILTERS_CTRL	0x00e0
+#define REG_DITHERING_CTRL	0x00E9
+#define REG_OF			0x00EA
+#define REG_PCLK		0x00EB
+#define REG_HS_HREF		0x00EC
+#define REG_VS_VREF		0x00ED
+#define REG_DE_FREF		0x00EE
+#define REG_VP35_32_CTRL	0x00EF
+#define REG_VP31_28_CTRL	0x00F0
+#define REG_VP27_24_CTRL	0x00F1
+#define REG_VP23_20_CTRL	0x00F2
+#define REG_VP19_16_CTRL	0x00F3
+#define REG_VP15_12_CTRL	0x00F4
+#define REG_VP11_08_CTRL	0x00F5
+#define REG_VP07_04_CTRL	0x00F6
+#define REG_VP03_00_CTRL	0x00F7
+#define REG_CURPAGE_00H		0xFF
+
+#define MASK_VPER		0x3fffff
+#define MASK_VPER_SYNC_POS	0x800000
+#define MASK_VHREF		0x3fff
+#define MASK_HPER		0x0fff
+#define MASK_HPER_SYNC_POS	0x8000
+#define MASK_HSWIDTH		0x03ff
+#define MASK_HSWIDTH_INTERLACED	0x8000
+
+/* HPD Detection */
+#define DETECT_UTIL		BIT(7)	/* utility of HDMI level */
+#define DETECT_HPD		BIT(6)	/* HPD of HDMI level */
+#define DETECT_5V_SEL		BIT(2)	/* 5V present on selected input */
+#define DETECT_5V_B		BIT(1)	/* 5V present on input B */
+#define DETECT_5V_A		BIT(0)	/* 5V present on input A */
+
+/* Input Select */
+#define INPUT_SEL_RST_FMT	BIT(7)	/* 1=reset format measurement */
+#define INPUT_SEL_RST_VDP	BIT(2)	/* 1=reset video data path */
+#define INPUT_SEL_OUT_MODE	BIT(1)	/* 0=loop 1=bypass */
+#define INPUT_SEL_B		BIT(0)	/* 0=inputA 1=inputB */
+
+/* Service Mode */
+#define SVC_MODE_CLK2_MASK	0xc0
+#define SVC_MODE_CLK2_SHIFT	6
+#define SVC_MODE_CLK2_XTL	0L
+#define SVC_MODE_CLK2_XTLDIV2	1L
+#define SVC_MODE_CLK2_HDMIX2	3L
+#define SVC_MODE_CLK1_MASK	0x30
+#define SVC_MODE_CLK1_SHIFT	4
+#define SVC_MODE_CLK1_XTAL	0L
+#define SVC_MODE_CLK1_XTLDIV2	1L
+#define SVC_MODE_CLK1_HDMI	3L
+#define SVC_MODE_RAMP		BIT(3)	/* 0=colorbar 1=ramp */
+#define SVC_MODE_PAL		BIT(2)	/* 0=NTSC(480i/p) 1=PAL(576i/p) */
+#define SVC_MODE_INT_PROG	BIT(1)	/* 0=interlaced 1=progressive */
+#define SVC_MODE_SM_ON		BIT(0)	/* Enable color bars and tone gen */
+
+/* HDP Manual Control */
+#define HPD_MAN_CTRL_HPD_PULSE	BIT(7)	/* HPD Pulse low 110ms */
+#define HPD_MAN_CTRL_5VEN	BIT(2)	/* Output 5V */
+#define HPD_MAN_CTRL_HPD_B	BIT(1)	/* Assert HPD High for Input A */
+#define HPD_MAN_CTRL_HPD_A	BIT(0)	/* Assert HPD High for Input A */
+
+/* RT_MAN_CTRL */
+#define RT_MAN_CTRL_RT_AUTO	BIT(7)
+#define RT_MAN_CTRL_RT		BIT(6)
+#define RT_MAN_CTRL_RT_B	BIT(1)	/* enable TMDS pull-up on Input B */
+#define RT_MAN_CTRL_RT_A	BIT(0)	/* enable TMDS pull-up on Input A */
+
+/* VDP_CTRL */
+#define VDP_CTRL_COMPDEL_BP	BIT(5)	/* bypass compdel */
+#define VDP_CTRL_FORMATTER_BP	BIT(4)	/* bypass formatter */
+#define VDP_CTRL_PREFILTER_BP	BIT(1)	/* bypass prefilter */
+#define VDP_CTRL_MATRIX_BP	BIT(0)	/* bypass matrix conversion */
+
+/* REG_VHREF_CTRL */
+#define VHREF_INT_DET		BIT(7)	/* interlace detect: 1=alt 0=frame */
+#define VHREF_VSYNC_MASK	0x60
+#define VHREF_VSYNC_SHIFT	6
+#define VHREF_VSYNC_AUTO	0L
+#define VHREF_VSYNC_FDW		1L
+#define VHREF_VSYNC_EVEN	2L
+#define VHREF_VSYNC_ODD		3L
+#define VHREF_STD_DET_MASK	0x18
+#define VHREF_STD_DET_SHIFT	3
+#define VHREF_STD_DET_PAL	0L
+#define VHREF_STD_DET_NTSC	1L
+#define VHREF_STD_DET_AUTO	2L
+#define VHREF_STD_DET_OFF	3L
+#define VHREF_VREF_SRC_STD	BIT(2)	/* 1=from standard 0=manual */
+#define VHREF_HREF_SRC_STD	BIT(1)	/* 1=from standard 0=manual */
+#define VHREF_HSYNC_SEL_HS	BIT(0)	/* 1=HS 0=VS */
+
+/* AUDIO_OUT_ENABLE */
+#define AUDIO_OUT_ENABLE_ACLK	BIT(5)
+#define AUDIO_OUT_ENABLE_WS	BIT(4)
+#define AUDIO_OUT_ENABLE_AP3	BIT(3)
+#define AUDIO_OUT_ENABLE_AP2	BIT(2)
+#define AUDIO_OUT_ENABLE_AP1	BIT(1)
+#define AUDIO_OUT_ENABLE_AP0	BIT(0)
+
+/* Prefilter Control */
+#define FILTERS_CTRL_BU_MASK	0x0c
+#define FILTERS_CTRL_BU_SHIFT	2
+#define FILTERS_CTRL_RV_MASK	0x03
+#define FILTERS_CTRL_RV_SHIFT	0
+#define FILTERS_CTRL_OFF	0L	/* off */
+#define FILTERS_CTRL_2TAP	1L	/* 2 Taps */
+#define FILTERS_CTRL_7TAP	2L	/* 7 Taps */
+#define FILTERS_CTRL_2_7TAP	3L	/* 2/7 Taps */
+
+/* PCLK Configuration */
+#define PCLK_DELAY_MASK		0x70
+#define PCLK_DELAY_SHIFT	4	/* Pixel delay (-8..+7) */
+#define PCLK_INV_SHIFT		2
+#define PCLK_SEL_MASK		0x03	/* clock scaler */
+#define PCLK_SEL_SHIFT		0
+#define PCLK_SEL_X1		0L
+#define PCLK_SEL_X2		1L
+#define PCLK_SEL_DIV2		2L
+#define PCLK_SEL_DIV4		3L
+
+/* Pixel Repeater */
+#define PIX_REPEAT_MASK_UP_SEL	0x30
+#define PIX_REPEAT_MASK_REP	0x0f
+#define PIX_REPEAT_SHIFT	4
+#define PIX_REPEAT_CHROMA	1
+
+/* Page 0x01 - HDMI info and packets */
+#define REG_HDMI_FLAGS		0x0100
+#define REG_DEEP_COLOR_MODE	0x0101
+#define REG_AUDIO_FLAGS		0x0108
+#define REG_AUDIO_FREQ		0x0109
+#define REG_ACP_PACKET_TYPE	0x0141
+#define REG_ISRC1_PACKET_TYPE	0x0161
+#define REG_ISRC2_PACKET_TYPE	0x0181
+#define REG_GBD_PACKET_TYPE	0x01a1
+
+/* HDMI_FLAGS */
+#define HDMI_FLAGS_AUDIO	BIT(7)	/* Audio packet in last videoframe */
+#define HDMI_FLAGS_HDMI		BIT(6)	/* HDMI detected */
+#define HDMI_FLAGS_EESS		BIT(5)	/* EESS detected */
+#define HDMI_FLAGS_HDCP		BIT(4)	/* HDCP detected */
+#define HDMI_FLAGS_AVMUTE	BIT(3)	/* AVMUTE */
+#define HDMI_FLAGS_AUD_LAYOUT	BIT(2)	/* Layout status Audio sample packet */
+#define HDMI_FLAGS_AUD_FIFO_OF	BIT(1)	/* FIFO read/write pointers crossed */
+#define HDMI_FLAGS_AUD_FIFO_LOW	BIT(0)	/* FIFO read ptr within 2 of write */
+
+/* Page 0x12 - HDMI Extra control and debug */
+#define REG_CLK_CFG		0x1200
+#define REG_CLK_OUT_CFG		0x1201
+#define REG_CFG1		0x1202
+#define REG_CFG2		0x1203
+#define REG_WDL_CFG		0x1210
+#define REG_DELOCK_DELAY	0x1212
+#define REG_PON_OVR_EN		0x12A0
+#define REG_PON_CBIAS		0x12A1
+#define REG_PON_RESCAL		0x12A2
+#define REG_PON_RES		0x12A3
+#define REG_PON_CLK		0x12A4
+#define REG_PON_PLL		0x12A5
+#define REG_PON_EQ		0x12A6
+#define REG_PON_DES		0x12A7
+#define REG_PON_OUT		0x12A8
+#define REG_PON_MUX		0x12A9
+#define REG_MODE_REC_CFG1	0x12F8
+#define REG_MODE_REC_CFG2	0x12F9
+#define REG_MODE_REC_STS	0x12FA
+#define REG_AUDIO_LAYOUT	0x12D0
+
+#define PON_EN			1
+#define PON_DIS			0
+
+/* CLK CFG */
+#define CLK_CFG_INV_OUT_CLK	BIT(7)
+#define CLK_CFG_INV_BUS_CLK	BIT(6)
+#define CLK_CFG_SEL_ACLK_EN	BIT(1)
+#define CLK_CFG_SEL_ACLK	BIT(0)
+#define CLK_CFG_DIS		0
+
+/* Page 0x13 - HDMI Extra control and debug */
+#define REG_DEEP_COLOR_CTRL	0x1300
+#define REG_CGU_DBG_SEL		0x1305
+#define REG_HDCP_DDC_ADDR	0x1310
+#define REG_HDCP_KIDX		0x1316
+#define REG_DEEP_PLL7_BYP	0x1347
+#define REG_HDCP_DE_CTRL	0x1370
+#define REG_HDCP_EP_FILT_CTRL	0x1371
+#define REG_HDMI_CTRL		0x1377
+#define REG_HMTP_CTRL		0x137a
+#define REG_TIMER_D		0x13CF
+#define REG_SUS_SET_RGB0	0x13E1
+#define REG_SUS_SET_RGB1	0x13E2
+#define REG_SUS_SET_RGB2	0x13E3
+#define REG_SUS_SET_RGB3	0x13E4
+#define REG_SUS_SET_RGB4	0x13E5
+#define REG_MAN_SUS_HDMI_SEL	0x13E8
+#define REG_MAN_HDMI_SET	0x13E9
+#define REG_SUS_CLOCK_GOOD	0x13EF
+
+/* HDCP DE Control */
+#define HDCP_DE_MODE_MASK	0xc0	/* DE Measurement mode */
+#define HDCP_DE_MODE_SHIFT	6
+#define HDCP_DE_REGEN_EN	BIT(5)	/* enable regen mode */
+#define HDCP_DE_FILTER_MASK	0x18	/* DE filter sensitivity */
+#define HDCP_DE_FILTER_SHIFT	3
+#define HDCP_DE_COMP_MASK	0x07	/* DE Composition mode */
+#define HDCP_DE_COMP_MIXED	6L
+#define HDCP_DE_COMP_OR		5L
+#define HDCP_DE_COMP_AND	4L
+#define HDCP_DE_COMP_CH3	3L
+#define HDCP_DE_COMP_CH2	2L
+#define HDCP_DE_COMP_CH1	1L
+#define HDCP_DE_COMP_CH0	0L
+
+/* HDCP EP Filter Control */
+#define HDCP_EP_FIL_CTL_MASK	0x30
+#define HDCP_EP_FIL_CTL_SHIFT	4
+#define HDCP_EP_FIL_VS_MASK	0x0c
+#define HDCP_EP_FIL_VS_SHIFT	2
+#define HDCP_EP_FIL_HS_MASK	0x03
+#define HDCP_EP_FIL_HS_SHIFT	0
+
+/* HDMI_CTRL */
+#define HDMI_CTRL_MUTE_MASK	0x0c
+#define HDMI_CTRL_MUTE_SHIFT	2
+#define HDMI_CTRL_MUTE_AUTO	0L
+#define HDMI_CTRL_MUTE_OFF	1L
+#define HDMI_CTRL_MUTE_ON	2L
+#define HDMI_CTRL_HDCP_MASK	0x03
+#define HDMI_CTRL_HDCP_SHIFT	0
+#define HDMI_CTRL_HDCP_EESS	2L
+#define HDMI_CTRL_HDCP_OESS	1L
+#define HDMI_CTRL_HDCP_AUTO	0L
+
+/* CGU_DBG_SEL bits */
+#define CGU_DBG_CLK_SEL_MASK	0x18
+#define CGU_DBG_CLK_SEL_SHIFT	3
+#define CGU_DBG_XO_FRO_SEL	BIT(2)
+#define CGU_DBG_VDP_CLK_SEL	BIT(1)
+#define CGU_DBG_PIX_CLK_SEL	BIT(0)
+
+/* REG_MAN_SUS_HDMI_SEL / REG_MAN_HDMI_SET bits */
+#define MAN_DIS_OUT_BUF		BIT(7)
+#define MAN_DIS_ANA_PATH	BIT(6)
+#define MAN_DIS_HDCP		BIT(5)
+#define MAN_DIS_TMDS_ENC	BIT(4)
+#define MAN_DIS_TMDS_FLOW	BIT(3)
+#define MAN_RST_HDCP		BIT(2)
+#define MAN_RST_TMDS_ENC	BIT(1)
+#define MAN_RST_TMDS_FLOW	BIT(0)
+
+/* Page 0x14 - Audio Extra control and debug */
+#define REG_FIFO_LATENCY_VAL	0x1403
+#define REG_AUDIO_CLOCK		0x1411
+#define REG_TEST_NCTS_CTRL	0x1415
+#define REG_TEST_AUDIO_FREQ	0x1426
+#define REG_TEST_MODE		0x1437
+
+/* Audio Clock Configuration */
+#define AUDIO_CLOCK_PLL_PD	BIT(7)	/* powerdown PLL */
+#define AUDIO_CLOCK_SEL_MASK	0x7f
+#define AUDIO_CLOCK_SEL_16FS	0L	/* 16*fs */
+#define AUDIO_CLOCK_SEL_32FS	1L	/* 32*fs */
+#define AUDIO_CLOCK_SEL_64FS	2L	/* 64*fs */
+#define AUDIO_CLOCK_SEL_128FS	3L	/* 128*fs */
+#define AUDIO_CLOCK_SEL_256FS	4L	/* 256*fs */
+#define AUDIO_CLOCK_SEL_512FS	5L	/* 512*fs */
+
+/* Page 0x20: EDID and Hotplug Detect */
+#define REG_EDID_IN_BYTE0	0x2000 /* EDID base */
+#define REG_EDID_IN_VERSION	0x2080
+#define REG_EDID_ENABLE		0x2081
+#define REG_HPD_POWER		0x2084
+#define REG_HPD_AUTO_CTRL	0x2085
+#define REG_HPD_DURATION	0x2086
+#define REG_RX_HPD_HEAC		0x2087
+
+/* EDID_ENABLE */
+#define EDID_ENABLE_NACK_OFF	BIT(7)
+#define EDID_ENABLE_EDID_ONLY	BIT(6)
+#define EDID_ENABLE_B_EN	BIT(1)
+#define EDID_ENABLE_A_EN	BIT(0)
+
+/* HPD Power */
+#define HPD_POWER_BP_MASK	0x0c
+#define HPD_POWER_BP_SHIFT	2
+#define HPD_POWER_BP_LOW	0L
+#define HPD_POWER_BP_HIGH	1L
+#define HPD_POWER_EDID_ONLY	BIT(1)
+
+/* HPD Auto control */
+#define HPD_AUTO_READ_EDID	BIT(7)
+#define HPD_AUTO_HPD_F3TECH	BIT(5)
+#define HPD_AUTO_HP_OTHER	BIT(4)
+#define HPD_AUTO_HPD_UNSEL	BIT(3)
+#define HPD_AUTO_HPD_ALL_CH	BIT(2)
+#define HPD_AUTO_HPD_PRV_CH	BIT(1)
+#define HPD_AUTO_HPD_NEW_CH	BIT(0)
+
+/* Page 0x21 - EDID content */
+#define REG_EDID_IN_BYTE128	0x2100 /* CEA Extension block */
+#define REG_EDID_IN_SPA_SUB	0x2180
+#define REG_EDID_IN_SPA_AB_A	0x2181
+#define REG_EDID_IN_SPA_CD_A	0x2182
+#define REG_EDID_IN_CKSUM_A	0x2183
+#define REG_EDID_IN_SPA_AB_B	0x2184
+#define REG_EDID_IN_SPA_CD_B	0x2185
+#define REG_EDID_IN_CKSUM_B	0x2186
+
+/* Page 0x30 - NV Configuration */
+#define REG_RT_AUTO_CTRL	0x3000
+#define REG_EQ_MAN_CTRL0	0x3001
+#define REG_EQ_MAN_CTRL1	0x3002
+#define REG_OUTPUT_CFG		0x3003
+#define REG_MUTE_CTRL		0x3004
+#define REG_SLAVE_ADDR		0x3005
+#define REG_CMTP_REG6		0x3006
+#define REG_CMTP_REG7		0x3007
+#define REG_CMTP_REG8		0x3008
+#define REG_CMTP_REG9		0x3009
+#define REG_CMTP_REGA		0x300A
+#define REG_CMTP_REGB		0x300B
+#define REG_CMTP_REGC		0x300C
+#define REG_CMTP_REGD		0x300D
+#define REG_CMTP_REGE		0x300E
+#define REG_CMTP_REGF		0x300F
+#define REG_CMTP_REG10		0x3010
+#define REG_CMTP_REG11		0x3011
+
+/* Page 0x80 - CEC */
+#define REG_PWR_CONTROL		0x80F4
+#define REG_OSC_DIVIDER		0x80F5
+#define REG_EN_OSC_PERIOD_LSB	0x80F8
+#define REG_CONTROL		0x80FF
+
+/* global interrupt flags (INT_FLG_CRL_TOP) */
+#define INTERRUPT_AFE		BIT(7) /* AFE module */
+#define INTERRUPT_HDCP		BIT(6) /* HDCP module */
+#define INTERRUPT_AUDIO		BIT(5) /* Audio module */
+#define INTERRUPT_INFO		BIT(4) /* Infoframe module */
+#define INTERRUPT_MODE		BIT(3) /* HDMI mode module */
+#define INTERRUPT_RATE		BIT(2) /* rate module */
+#define INTERRUPT_DDC		BIT(1) /* DDC module */
+#define INTERRUPT_SUS		BIT(0) /* SUS module */
+
+/* INT_FLG_CLR_HDCP bits */
+#define MASK_HDCP_MTP		BIT(7) /* HDCP MTP busy */
+#define MASK_HDCP_DLMTP		BIT(4) /* HDCP end download MTP to SRAM */
+#define MASK_HDCP_DLRAM		BIT(3) /* HDCP end download keys from SRAM */
+#define MASK_HDCP_ENC		BIT(2) /* HDCP ENC */
+#define MASK_STATE_C5		BIT(1) /* HDCP State C5 reached */
+#define MASK_AKSV		BIT(0) /* AKSV received (start of auth) */
+
+/* INT_FLG_CLR_RATE bits */
+#define MASK_RATE_B_DRIFT	BIT(7) /* Rate measurement drifted */
+#define MASK_RATE_B_ST		BIT(6) /* Rate measurement stability change */
+#define MASK_RATE_B_ACT		BIT(5) /* Rate measurement activity change */
+#define MASK_RATE_B_PST		BIT(4) /* Rate measreument presence change */
+#define MASK_RATE_A_DRIFT	BIT(3) /* Rate measurement drifted */
+#define MASK_RATE_A_ST		BIT(2) /* Rate measurement stability change */
+#define MASK_RATE_A_ACT		BIT(1) /* Rate measurement presence change */
+#define MASK_RATE_A_PST		BIT(0) /* Rate measreument presence change */
+
+/* INT_FLG_CLR_SUS (Start Up Sequencer) bits */
+#define MASK_MPT		BIT(7) /* Config MTP end of process */
+#define MASK_FMT		BIT(5) /* Video format changed */
+#define MASK_RT_PULSE		BIT(4) /* End of termination resistance pulse */
+#define MASK_SUS_END		BIT(3) /* SUS last state reached */
+#define MASK_SUS_ACT		BIT(2) /* Activity of selected input changed */
+#define MASK_SUS_CH		BIT(1) /* Selected input changed */
+#define MASK_SUS_ST		BIT(0) /* SUS state changed */
+
+/* INT_FLG_CLR_DDC bits */
+#define MASK_EDID_MTP		BIT(7) /* EDID MTP end of process */
+#define MASK_DDC_ERR		BIT(6) /* master DDC error */
+#define MASK_DDC_CMD_DONE	BIT(5) /* master DDC cmd send correct */
+#define MASK_READ_DONE		BIT(4) /* End of down EDID read */
+#define MASK_RX_DDC_SW		BIT(3) /* Output DDC switching finished */
+#define MASK_HDCP_DDC_SW	BIT(2) /* HDCP DDC switching finished */
+#define MASK_HDP_PULSE_END	BIT(1) /* End of Hot Plug Detect pulse */
+#define MASK_DET_5V		BIT(0) /* Detection of +5V */
+
+/* INT_FLG_CLR_MODE bits */
+#define MASK_HDMI_FLG		BIT(7) /* HDMI mode/avmute/encrypt/FIFO fail */
+#define MASK_GAMUT		BIT(6) /* Gamut packet */
+#define MASK_ISRC2		BIT(5) /* ISRC2 packet */
+#define MASK_ISRC1		BIT(4) /* ISRC1 packet */
+#define MASK_ACP		BIT(3) /* Audio Content Protection packet */
+#define MASK_DC_NO_GCP		BIT(2) /* GCP not received in 5 frames */
+#define MASK_DC_PHASE		BIT(1) /* deepcolor pixel phase needs update */
+#define MASK_DC_MODE		BIT(0) /* deepcolor color depth changed */
+
+/* INT_FLG_CLR_INFO bits (Infoframe Change Status) */
+#define MASK_MPS_IF		BIT(6) /* MPEG Source Product */
+#define MASK_AUD_IF		BIT(5) /* Audio */
+#define MASK_SPD_IF		BIT(4) /* Source Product Descriptor */
+#define MASK_AVI_IF		BIT(3) /* Auxiliary Video IF */
+#define MASK_VS_IF_OTHER_BK2	BIT(2) /* Vendor Specific (bank2) */
+#define MASK_VS_IF_OTHER_BK1	BIT(1) /* Vendor Specific (bank1) */
+#define MASK_VS_IF_HDMI		BIT(0) /* Vendor Specific (w/ HDMI LLC code) */
+
+/* INT_FLG_CLR_AUDIO bits */
+#define MASK_AUDIO_FREQ_FLG	BIT(5) /* Audio freq change */
+#define MASK_AUDIO_FLG		BIT(4) /* DST, OBA, HBR, ASP change */
+#define MASK_MUTE_FLG		BIT(3) /* Audio Mute */
+#define MASK_CH_STATE		BIT(2) /* Channel status */
+#define MASK_UNMUTE_FIFO	BIT(1) /* Audio Unmute */
+#define MASK_ERROR_FIFO_PT	BIT(0) /* Audio FIFO pointer error */
+
+/* INT_FLG_CLR_AFE bits */
+#define MASK_AFE_WDL_UNLOCKED	BIT(7) /* Wordlocker was unlocked */
+#define MASK_AFE_GAIN_DONE	BIT(6) /* Gain calibration done */
+#define MASK_AFE_OFFSET_DONE	BIT(5) /* Offset calibration done */
+#define MASK_AFE_ACTIVITY_DET	BIT(4) /* Activity detected on data */
+#define MASK_AFE_PLL_LOCK	BIT(3) /* TMDS PLL is locked */
+#define MASK_AFE_TRMCAL_DONE	BIT(2) /* Termination calibration done */
+#define MASK_AFE_ASU_STATE	BIT(1) /* ASU state is reached */
+#define MASK_AFE_ASU_READY	BIT(0) /* AFE calibration done: TMDS ready */
+
+/* Audio Output */
+#define AUDCFG_CLK_INVERT	BIT(7)	/* invert A_CLK polarity */
+#define AUDCFG_TEST_TONE	BIT(6)	/* enable test tone generator */
+#define AUDCFG_BUS_SHIFT	5
+#define AUDCFG_BUS_I2S		0L
+#define AUDCFG_BUS_SPDIF	1L
+#define AUDCFG_I2SW_SHIFT	4
+#define AUDCFG_I2SW_16		0L
+#define AUDCFG_I2SW_32		1L
+#define AUDCFG_AUTO_MUTE_EN	BIT(3)	/* Enable Automatic audio mute */
+#define AUDCFG_HBR_SHIFT	2
+#define AUDCFG_HBR_STRAIGHT	0L	/* straight via AP0 */
+#define AUDCFG_HBR_DEMUX	1L	/* demuxed via AP0:AP3 */
+#define AUDCFG_TYPE_MASK	0x03
+#define AUDCFG_TYPE_SHIFT	0
+#define AUDCFG_TYPE_DST		3L	/* Direct Stream Transfer (DST) */
+#define AUDCFG_TYPE_OBA		2L	/* One Bit Audio (OBA) */
+#define AUDCFG_TYPE_HBR		1L	/* High Bit Rate (HBR) */
+#define AUDCFG_TYPE_PCM		0L	/* Audio samples */
+
+/* Video Formatter */
+#define OF_VP_ENABLE		BIT(7)	/* VP[35:0]/HS/VS/DE/CLK */
+#define OF_BLK			BIT(4)	/* blanking codes */
+#define OF_TRC			BIT(3)	/* timing codes (SAV/EAV) */
+#define OF_FMT_MASK		0x3
+#define OF_FMT_444		0L	/* RGB444/YUV444 */
+#define OF_FMT_422_SMPT		1L	/* YUV422 semi-planar */
+#define OF_FMT_422_CCIR		2L	/* YUV422 CCIR656 */
+
+/* HS/HREF output control */
+#define HS_HREF_DELAY_MASK	0xf0
+#define HS_HREF_DELAY_SHIFT	4	/* Pixel delay (-8..+7) */
+#define HS_HREF_PXQ_SHIFT	3	/* Timing codes from HREF */
+#define HS_HREF_INV_SHIFT	2	/* polarity (1=invert) */
+#define HS_HREF_SEL_MASK	0x03
+#define HS_HREF_SEL_SHIFT	0
+#define HS_HREF_SEL_HS_VHREF	0L	/* HS from VHREF */
+#define HS_HREF_SEL_HREF_VHREF	1L	/* HREF from VHREF */
+#define HS_HREF_SEL_HREF_HDMI	2L	/* HREF from HDMI */
+#define HS_HREF_SEL_NONE	3L	/* not generated */
+
+/* VS output control */
+#define VS_VREF_DELAY_MASK	0xf0
+#define VS_VREF_DELAY_SHIFT	4	/* Pixel delay (-8..+7) */
+#define VS_VREF_INV_SHIFT	2	/* polarity (1=invert) */
+#define VS_VREF_SEL_MASK	0x03
+#define VS_VREF_SEL_SHIFT	0
+#define VS_VREF_SEL_VS_VHREF	0L	/* VS from VHREF */
+#define VS_VREF_SEL_VREF_VHREF	1L	/* VREF from VHREF */
+#define VS_VREF_SEL_VREF_HDMI	2L	/* VREF from HDMI */
+#define VS_VREF_SEL_NONE	3L	/* not generated */
+
+/* DE/FREF output control */
+#define DE_FREF_DELAY_MASK	0xf0
+#define DE_FREF_DELAY_SHIFT	4	/* Pixel delay (-8..+7) */
+#define DE_FREF_DE_PXQ_SHIFT	3	/* Timing codes from DE */
+#define DE_FREF_INV_SHIFT	2	/* polarity (1=invert) */
+#define DE_FREF_SEL_MASK	0x03
+#define DE_FREF_SEL_SHIFT	0
+#define DE_FREF_SEL_DE_VHREF	0L	/* DE from VHREF (HREF and not(VREF) */
+#define DE_FREF_SEL_FREF_VHREF	1L	/* FREF from VHREF */
+#define DE_FREF_SEL_FREF_HDMI	2L	/* FREF from HDMI */
+#define DE_FREF_SEL_NONE	3L	/* not generated */
+
+/* HDMI_SOFT_RST bits */
+#define RESET_DC		BIT(7)	/* Reset deep color module */
+#define RESET_HDCP		BIT(6)	/* Reset HDCP module */
+#define RESET_KSV		BIT(5)	/* Reset KSV-FIFO */
+#define RESET_SCFG		BIT(4)	/* Reset HDCP and repeater function */
+#define RESET_HCFG		BIT(3)	/* Reset HDCP DDC part */
+#define RESET_PA		BIT(2)	/* Reset polarity adjust */
+#define RESET_EP		BIT(1)	/* Reset Error protection */
+#define RESET_TMDS		BIT(0)	/* Reset TMDS (calib, encoding, flow) */
+
+/* HDMI_INFO_RST bits */
+#define NACK_HDCP		BIT(7)	/* No ACK on HDCP request */
+#define RESET_FIFO		BIT(4)	/* Reset Audio FIFO control */
+#define RESET_GAMUT		BIT(3)	/* Clear Gamut packet */
+#define RESET_AI		BIT(2)	/* Clear ACP and ISRC packets */
+#define RESET_IF		BIT(1)	/* Clear all Audio infoframe packets */
+#define RESET_AUDIO		BIT(0)	/* Reset Audio FIFO control */
+
+/* HDCP_BCAPS bits */
+#define HDCP_HDMI		BIT(7)	/* HDCP supports HDMI (vs DVI only) */
+#define HDCP_REPEATER		BIT(6)	/* HDCP supports repeater function */
+#define HDCP_READY		BIT(5)	/* set by repeater function */
+#define HDCP_FAST		BIT(4)	/* Up to 400kHz */
+#define HDCP_11			BIT(1)	/* HDCP 1.1 supported */
+#define HDCP_FAST_REAUTH	BIT(0)	/* fast reauthentication supported */
+
+/* Audio output formatter */
+#define AUDIO_LAYOUT_SP_FLAG	BIT(2)	/* sp flag used by FIFO */
+#define AUDIO_LAYOUT_MANUAL	BIT(1)	/* manual layout (vs per pkt) */
+#define AUDIO_LAYOUT_LAYOUT1	BIT(0)  /* Layout1: AP0-3 vs Layout0:AP0 */
+
+/* masks for interrupt status registers */
+#define MASK_SUS_STATUS		0x1F
+#define LAST_STATE_REACHED	0x1B
+#define MASK_CLK_STABLE		0x04
+#define MASK_CLK_ACTIVE		0x02
+#define MASK_SUS_STATE		0x10
+#define MASK_SR_FIFO_FIFO_CTRL	0x30
+#define MASK_AUDIO_FLAG		0x10
+
+/* Rate measurement */
+#define RATE_REFTIM_ENABLE	0x01
+#define CLK_MIN_RATE		0x0057e4
+#define CLK_MAX_RATE		0x0395f8
+#define WDL_CFG_VAL		0x82
+#define DC_FILTER_VAL		0x31
+
+/* Infoframe */
+#define VS_HDMI_IF_UPDATE	0x0200
+#define VS_HDMI_IF		0x0201
+#define VS_BK1_IF_UPDATE	0x0220
+#define VS_BK1_IF		0x0221
+#define VS_BK2_IF_UPDATE	0x0240
+#define VS_BK2_IF		0x0241
+#define AVI_IF_UPDATE		0x0260
+#define AVI_IF			0x0261
+#define SPD_IF_UPDATE		0x0280
+#define SPD_IF			0x0281
+#define AUD_IF_UPDATE		0x02a0
+#define AUD_IF			0x02a1
+#define MPS_IF_UPDATE		0x02c0
+#define MPS_IF			0x02c1
diff --git a/drivers/media/i2c/tda7432.c b/drivers/media/i2c/tda7432.c
new file mode 100644
index 000000000..6ecdc8e2e
--- /dev/null
+++ b/drivers/media/i2c/tda7432.c
@@ -0,0 +1,417 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * For the STS-Thompson TDA7432 audio processor chip
+ *
+ * Handles audio functions: volume, balance, tone, loudness
+ * This driver will not complain if used with any
+ * other i2c device with the same address.
+ *
+ * Muting and tone control by Jonathan Isom <jisom@ematic.com>
+ *
+ * Copyright (c) 2000 Eric Sandeen <eric_sandeen@bigfoot.com>
+ * Copyright (c) 2006 Mauro Carvalho Chehab <mchehab@kernel.org>
+ *
+ * Based on tda9855.c by Steve VanDeBogart (vandebo@uclink.berkeley.edu)
+ * Which was based on tda8425.c by Greg Alexander (c) 1998
+ *
+ * OPTIONS:
+ * debug    - set to 1 if you'd like to see debug messages
+ *            set to 2 if you'd like to be inundated with debug messages
+ *
+ * loudness - set between 0 and 15 for varying degrees of loudness effect
+ *
+ * maxvol   - set maximum volume to +20db (1), default is 0db(0)
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/string.h>
+#include <linux/timer.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <linux/i2c.h>
+
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-ctrls.h>
+
+#ifndef VIDEO_AUDIO_BALANCE
+# define VIDEO_AUDIO_BALANCE 32
+#endif
+
+MODULE_AUTHOR("Eric Sandeen <eric_sandeen@bigfoot.com>");
+MODULE_DESCRIPTION("bttv driver for the tda7432 audio processor chip");
+MODULE_LICENSE("GPL");
+
+static int maxvol;
+static int loudness; /* disable loudness by default */
+static int debug;	 /* insmod parameter */
+module_param(debug, int, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(debug, "Set debugging level from 0 to 3. Default is off(0).");
+module_param(loudness, int, S_IRUGO);
+MODULE_PARM_DESC(loudness, "Turn loudness on(1) else off(0). Default is off(0).");
+module_param(maxvol, int, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(maxvol, "Set maximum volume to +20dB(0) else +0dB(1). Default is +20dB(0).");
+
+
+/* Structure of address and subaddresses for the tda7432 */
+
+struct tda7432 {
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
+	struct {
+		/* bass/treble cluster */
+		struct v4l2_ctrl *bass;
+		struct v4l2_ctrl *treble;
+	};
+	struct {
+		/* mute/balance cluster */
+		struct v4l2_ctrl *mute;
+		struct v4l2_ctrl *balance;
+	};
+};
+
+static inline struct tda7432 *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct tda7432, sd);
+}
+
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct tda7432, hdl)->sd;
+}
+
+/* The TDA7432 is made by STS-Thompson
+ * http://www.st.com
+ * http://us.st.com/stonline/books/pdf/docs/4056.pdf
+ *
+ * TDA7432: I2C-bus controlled basic audio processor
+ *
+ * The TDA7432 controls basic audio functions like volume, balance,
+ * and tone control (including loudness).  It also has four channel
+ * output (for front and rear).  Since most vidcap cards probably
+ * don't have 4 channel output, this driver will set front & rear
+ * together (no independent control).
+ */
+
+		/* Subaddresses for TDA7432 */
+
+#define TDA7432_IN	0x00 /* Input select                 */
+#define TDA7432_VL	0x01 /* Volume                       */
+#define TDA7432_TN	0x02 /* Bass, Treble (Tone)          */
+#define TDA7432_LF	0x03 /* Attenuation LF (Left Front)  */
+#define TDA7432_LR	0x04 /* Attenuation LR (Left Rear)   */
+#define TDA7432_RF	0x05 /* Attenuation RF (Right Front) */
+#define TDA7432_RR	0x06 /* Attenuation RR (Right Rear)  */
+#define TDA7432_LD	0x07 /* Loudness                     */
+
+
+		/* Masks for bits in TDA7432 subaddresses */
+
+/* Many of these not used - just for documentation */
+
+/* Subaddress 0x00 - Input selection and bass control */
+
+/* Bits 0,1,2 control input:
+ * 0x00 - Stereo input
+ * 0x02 - Mono input
+ * 0x03 - Mute  (Using Attenuators Plays better with modules)
+ * Mono probably isn't used - I'm guessing only the stereo
+ * input is connected on most cards, so we'll set it to stereo.
+ *
+ * Bit 3 controls bass cut: 0/1 is non-symmetric/symmetric bass cut
+ * Bit 4 controls bass range: 0/1 is extended/standard bass range
+ *
+ * Highest 3 bits not used
+ */
+
+#define TDA7432_STEREO_IN	0
+#define TDA7432_MONO_IN		2	/* Probably won't be used */
+#define TDA7432_BASS_SYM	1 << 3
+#define TDA7432_BASS_NORM	1 << 4
+
+/* Subaddress 0x01 - Volume */
+
+/* Lower 7 bits control volume from -79dB to +32dB in 1dB steps
+ * Recommended maximum is +20 dB
+ *
+ * +32dB: 0x00
+ * +20dB: 0x0c
+ *   0dB: 0x20
+ * -79dB: 0x6f
+ *
+ * MSB (bit 7) controls loudness: 1/0 is loudness on/off
+ */
+
+#define	TDA7432_VOL_0DB		0x20
+#define TDA7432_LD_ON		1 << 7
+
+
+/* Subaddress 0x02 - Tone control */
+
+/* Bits 0,1,2 control absolute treble gain from 0dB to 14dB
+ * 0x0 is 14dB, 0x7 is 0dB
+ *
+ * Bit 3 controls treble attenuation/gain (sign)
+ * 1 = gain (+)
+ * 0 = attenuation (-)
+ *
+ * Bits 4,5,6 control absolute bass gain from 0dB to 14dB
+ * (This is only true for normal base range, set in 0x00)
+ * 0x0 << 4 is 14dB, 0x7 is 0dB
+ *
+ * Bit 7 controls bass attenuation/gain (sign)
+ * 1 << 7 = gain (+)
+ * 0 << 7 = attenuation (-)
+ *
+ * Example:
+ * 1 1 0 1 0 1 0 1 is +4dB bass, -4dB treble
+ */
+
+#define TDA7432_TREBLE_0DB		0xf
+#define TDA7432_TREBLE			7
+#define TDA7432_TREBLE_GAIN		1 << 3
+#define TDA7432_BASS_0DB		0xf
+#define TDA7432_BASS			7 << 4
+#define TDA7432_BASS_GAIN		1 << 7
+
+
+/* Subaddress 0x03 - Left  Front attenuation */
+/* Subaddress 0x04 - Left  Rear  attenuation */
+/* Subaddress 0x05 - Right Front attenuation */
+/* Subaddress 0x06 - Right Rear  attenuation */
+
+/* Bits 0,1,2,3,4 control attenuation from 0dB to -37.5dB
+ * in 1.5dB steps.
+ *
+ * 0x00 is     0dB
+ * 0x1f is -37.5dB
+ *
+ * Bit 5 mutes that channel when set (1 = mute, 0 = unmute)
+ * We'll use the mute on the input, though (above)
+ * Bits 6,7 unused
+ */
+
+#define TDA7432_ATTEN_0DB	0x00
+#define TDA7432_MUTE        0x1 << 5
+
+
+/* Subaddress 0x07 - Loudness Control */
+
+/* Bits 0,1,2,3 control loudness from 0dB to -15dB in 1dB steps
+ * when bit 4 is NOT set
+ *
+ * 0x0 is   0dB
+ * 0xf is -15dB
+ *
+ * If bit 4 is set, then there is a flat attenuation according to
+ * the lower 4 bits, as above.
+ *
+ * Bits 5,6,7 unused
+ */
+
+
+
+/* Begin code */
+
+static int tda7432_write(struct v4l2_subdev *sd, int subaddr, int val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	unsigned char buffer[2];
+
+	v4l2_dbg(2, debug, sd, "In tda7432_write\n");
+	v4l2_dbg(1, debug, sd, "Writing %d 0x%x\n", subaddr, val);
+	buffer[0] = subaddr;
+	buffer[1] = val;
+	if (2 != i2c_master_send(client, buffer, 2)) {
+		v4l2_err(sd, "I/O error, trying (write %d 0x%x)\n",
+		       subaddr, val);
+		return -1;
+	}
+	return 0;
+}
+
+static int tda7432_set(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	unsigned char buf[16];
+
+	buf[0]  = TDA7432_IN;
+	buf[1]  = TDA7432_STEREO_IN |  /* Main (stereo) input   */
+		  TDA7432_BASS_SYM  |  /* Symmetric bass cut    */
+		  TDA7432_BASS_NORM;   /* Normal bass range     */
+	buf[2]  = 0x3b;
+	if (loudness)			 /* Turn loudness on?     */
+		buf[2] |= TDA7432_LD_ON;
+	buf[3]  = TDA7432_TREBLE_0DB | (TDA7432_BASS_0DB << 4);
+	buf[4]  = TDA7432_ATTEN_0DB;
+	buf[5]  = TDA7432_ATTEN_0DB;
+	buf[6]  = TDA7432_ATTEN_0DB;
+	buf[7]  = TDA7432_ATTEN_0DB;
+	buf[8]  = loudness;
+	if (9 != i2c_master_send(client, buf, 9)) {
+		v4l2_err(sd, "I/O error, trying tda7432_set\n");
+		return -1;
+	}
+
+	return 0;
+}
+
+static int tda7432_log_status(struct v4l2_subdev *sd)
+{
+	struct tda7432 *state = to_state(sd);
+
+	v4l2_ctrl_handler_log_status(&state->hdl, sd->name);
+	return 0;
+}
+
+static int tda7432_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+	struct tda7432 *t = to_state(sd);
+	u8 bass, treble, volume;
+	u8 lf, lr, rf, rr;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUDIO_MUTE:
+		if (t->balance->val < 0) {
+			/* shifted to left, attenuate right */
+			rr = rf = -t->balance->val;
+			lr = lf = TDA7432_ATTEN_0DB;
+		} else if (t->balance->val > 0) {
+			/* shifted to right, attenuate left */
+			rr = rf = TDA7432_ATTEN_0DB;
+			lr = lf = t->balance->val;
+		} else {
+			/* centered */
+			rr = rf = TDA7432_ATTEN_0DB;
+			lr = lf = TDA7432_ATTEN_0DB;
+		}
+		if (t->mute->val) {
+			lf |= TDA7432_MUTE;
+			lr |= TDA7432_MUTE;
+			rf |= TDA7432_MUTE;
+			rr |= TDA7432_MUTE;
+		}
+		/* Mute & update balance*/
+		tda7432_write(sd, TDA7432_LF, lf);
+		tda7432_write(sd, TDA7432_LR, lr);
+		tda7432_write(sd, TDA7432_RF, rf);
+		tda7432_write(sd, TDA7432_RR, rr);
+		return 0;
+	case V4L2_CID_AUDIO_VOLUME:
+		volume = 0x6f - ctrl->val;
+		if (loudness)		/* Turn on the loudness bit */
+			volume |= TDA7432_LD_ON;
+
+		tda7432_write(sd, TDA7432_VL, volume);
+		return 0;
+	case V4L2_CID_AUDIO_BASS:
+		bass = t->bass->val;
+		treble = t->treble->val;
+		if (bass >= 0x8)
+			bass = 14 - (bass - 8);
+		if (treble >= 0x8)
+			treble = 14 - (treble - 8);
+
+		tda7432_write(sd, TDA7432_TN, 0x10 | (bass << 4) | treble);
+		return 0;
+	}
+	return -EINVAL;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_ctrl_ops tda7432_ctrl_ops = {
+	.s_ctrl = tda7432_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops tda7432_core_ops = {
+	.log_status = tda7432_log_status,
+};
+
+static const struct v4l2_subdev_ops tda7432_ops = {
+	.core = &tda7432_core_ops,
+};
+
+/* ----------------------------------------------------------------------- */
+
+/* *********************** *
+ * i2c interface functions *
+ * *********************** */
+
+static int tda7432_probe(struct i2c_client *client)
+{
+	struct tda7432 *t;
+	struct v4l2_subdev *sd;
+
+	v4l_info(client, "chip found @ 0x%02x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	t = devm_kzalloc(&client->dev, sizeof(*t), GFP_KERNEL);
+	if (!t)
+		return -ENOMEM;
+	sd = &t->sd;
+	v4l2_i2c_subdev_init(sd, client, &tda7432_ops);
+	v4l2_ctrl_handler_init(&t->hdl, 5);
+	v4l2_ctrl_new_std(&t->hdl, &tda7432_ctrl_ops,
+		V4L2_CID_AUDIO_VOLUME, 0, maxvol ? 0x68 : 0x4f, 1, maxvol ? 0x5d : 0x47);
+	t->mute = v4l2_ctrl_new_std(&t->hdl, &tda7432_ctrl_ops,
+		V4L2_CID_AUDIO_MUTE, 0, 1, 1, 0);
+	t->balance = v4l2_ctrl_new_std(&t->hdl, &tda7432_ctrl_ops,
+		V4L2_CID_AUDIO_BALANCE, -31, 31, 1, 0);
+	t->bass = v4l2_ctrl_new_std(&t->hdl, &tda7432_ctrl_ops,
+		V4L2_CID_AUDIO_BASS, 0, 14, 1, 7);
+	t->treble = v4l2_ctrl_new_std(&t->hdl, &tda7432_ctrl_ops,
+		V4L2_CID_AUDIO_TREBLE, 0, 14, 1, 7);
+	sd->ctrl_handler = &t->hdl;
+	if (t->hdl.error) {
+		int err = t->hdl.error;
+
+		v4l2_ctrl_handler_free(&t->hdl);
+		return err;
+	}
+	v4l2_ctrl_cluster(2, &t->bass);
+	v4l2_ctrl_cluster(2, &t->mute);
+	v4l2_ctrl_handler_setup(&t->hdl);
+	if (loudness < 0 || loudness > 15) {
+		v4l2_warn(sd, "loudness parameter must be between 0 and 15\n");
+		if (loudness < 0)
+			loudness = 0;
+		if (loudness > 15)
+			loudness = 15;
+	}
+
+	tda7432_set(sd);
+	return 0;
+}
+
+static void tda7432_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct tda7432 *t = to_state(sd);
+
+	tda7432_set(sd);
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&t->hdl);
+}
+
+static const struct i2c_device_id tda7432_id[] = {
+	{ "tda7432", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, tda7432_id);
+
+static struct i2c_driver tda7432_driver = {
+	.driver = {
+		.name	= "tda7432",
+	},
+	.probe		= tda7432_probe,
+	.remove		= tda7432_remove,
+	.id_table	= tda7432_id,
+};
+
+module_i2c_driver(tda7432_driver);
diff --git a/drivers/media/i2c/tda9840.c b/drivers/media/i2c/tda9840.c
new file mode 100644
index 000000000..1911ef212
--- /dev/null
+++ b/drivers/media/i2c/tda9840.c
@@ -0,0 +1,199 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+ /*
+    tda9840 - i2c-driver for the tda9840 by SGS Thomson
+
+    Copyright (C) 1998-2003 Michael Hunold <michael@mihu.de>
+    Copyright (C) 2008 Hans Verkuil <hverkuil@xs4all.nl>
+
+    The tda9840 is a stereo/dual sound processor with digital
+    identification. It can be found at address 0x84 on the i2c-bus.
+
+    For detailed information download the specifications directly
+    from SGS Thomson at http://www.st.com
+
+  */
+
+
+#include <linux/module.h>
+#include <linux/ioctl.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <media/v4l2-device.h>
+
+MODULE_AUTHOR("Michael Hunold <michael@mihu.de>");
+MODULE_DESCRIPTION("tda9840 driver");
+MODULE_LICENSE("GPL");
+
+static int debug;
+module_param(debug, int, 0644);
+
+MODULE_PARM_DESC(debug, "Debug level (0-1)");
+
+#define	SWITCH		0x00
+#define	LEVEL_ADJUST	0x02
+#define	STEREO_ADJUST	0x03
+#define	TEST		0x04
+
+#define TDA9840_SET_MUTE                0x00
+#define TDA9840_SET_MONO                0x10
+#define TDA9840_SET_STEREO              0x2a
+#define TDA9840_SET_LANG1               0x12
+#define TDA9840_SET_LANG2               0x1e
+#define TDA9840_SET_BOTH                0x1a
+#define TDA9840_SET_BOTH_R              0x16
+#define TDA9840_SET_EXTERNAL            0x7a
+
+
+static void tda9840_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (i2c_smbus_write_byte_data(client, reg, val))
+		v4l2_dbg(1, debug, sd, "error writing %02x to %02x\n",
+				val, reg);
+}
+
+static int tda9840_status(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int rc;
+	u8 byte;
+
+	rc = i2c_master_recv(client, &byte, 1);
+	if (rc != 1) {
+		v4l2_dbg(1, debug, sd,
+			"i2c_master_recv() failed\n");
+		if (rc < 0)
+			return rc;
+		return -EIO;
+	}
+
+	if (byte & 0x80) {
+		v4l2_dbg(1, debug, sd,
+			"TDA9840_DETECT: register contents invalid\n");
+		return -EINVAL;
+	}
+
+	v4l2_dbg(1, debug, sd, "TDA9840_DETECT: byte: 0x%02x\n", byte);
+	return byte & 0x60;
+}
+
+static int tda9840_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *t)
+{
+	int stat = tda9840_status(sd);
+	int byte;
+
+	if (t->index)
+		return -EINVAL;
+
+	stat = stat < 0 ? 0 : stat;
+	if (stat == 0 || stat == 0x60) /* mono input */
+		byte = TDA9840_SET_MONO;
+	else if (stat == 0x40) /* stereo input */
+		byte = (t->audmode == V4L2_TUNER_MODE_MONO) ?
+			TDA9840_SET_MONO : TDA9840_SET_STEREO;
+	else { /* bilingual */
+		switch (t->audmode) {
+		case V4L2_TUNER_MODE_LANG1_LANG2:
+			byte = TDA9840_SET_BOTH;
+			break;
+		case V4L2_TUNER_MODE_LANG2:
+			byte = TDA9840_SET_LANG2;
+			break;
+		default:
+			byte = TDA9840_SET_LANG1;
+			break;
+		}
+	}
+	v4l2_dbg(1, debug, sd, "TDA9840_SWITCH: 0x%02x\n", byte);
+	tda9840_write(sd, SWITCH, byte);
+	return 0;
+}
+
+static int tda9840_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *t)
+{
+	int stat = tda9840_status(sd);
+
+	if (stat < 0)
+		return stat;
+
+	t->rxsubchans = V4L2_TUNER_SUB_MONO;
+
+	switch (stat & 0x60) {
+	case 0x00:
+		t->rxsubchans = V4L2_TUNER_SUB_MONO;
+		break;
+	case 0x20:
+		t->rxsubchans = V4L2_TUNER_SUB_LANG1 | V4L2_TUNER_SUB_LANG2;
+		break;
+	case 0x40:
+		t->rxsubchans = V4L2_TUNER_SUB_STEREO | V4L2_TUNER_SUB_MONO;
+		break;
+	default: /* Incorrect detect */
+		t->rxsubchans = V4L2_TUNER_MODE_MONO;
+		break;
+	}
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_tuner_ops tda9840_tuner_ops = {
+	.s_tuner = tda9840_s_tuner,
+	.g_tuner = tda9840_g_tuner,
+};
+
+static const struct v4l2_subdev_ops tda9840_ops = {
+	.tuner = &tda9840_tuner_ops,
+};
+
+/* ----------------------------------------------------------------------- */
+
+static int tda9840_probe(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd;
+
+	/* let's see whether this adapter can support what we need */
+	if (!i2c_check_functionality(client->adapter,
+			I2C_FUNC_SMBUS_READ_BYTE_DATA |
+			I2C_FUNC_SMBUS_WRITE_BYTE_DATA))
+		return -EIO;
+
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	sd = devm_kzalloc(&client->dev, sizeof(*sd), GFP_KERNEL);
+	if (sd == NULL)
+		return -ENOMEM;
+	v4l2_i2c_subdev_init(sd, client, &tda9840_ops);
+
+	/* set initial values for level & stereo - adjustment, mode */
+	tda9840_write(sd, LEVEL_ADJUST, 0);
+	tda9840_write(sd, STEREO_ADJUST, 0);
+	tda9840_write(sd, SWITCH, TDA9840_SET_STEREO);
+	return 0;
+}
+
+static void tda9840_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+}
+
+static const struct i2c_device_id tda9840_id[] = {
+	{ "tda9840", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, tda9840_id);
+
+static struct i2c_driver tda9840_driver = {
+	.driver = {
+		.name	= "tda9840",
+	},
+	.probe		= tda9840_probe,
+	.remove		= tda9840_remove,
+	.id_table	= tda9840_id,
+};
+
+module_i2c_driver(tda9840_driver);
diff --git a/drivers/media/i2c/tea6415c.c b/drivers/media/i2c/tea6415c.c
new file mode 100644
index 000000000..3ed6e441d
--- /dev/null
+++ b/drivers/media/i2c/tea6415c.c
@@ -0,0 +1,158 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+ /*
+    tea6415c - i2c-driver for the tea6415c by SGS Thomson
+
+    Copyright (C) 1998-2003 Michael Hunold <michael@mihu.de>
+    Copyright (C) 2008 Hans Verkuil <hverkuil@xs4all.nl>
+
+    The tea6415c is a bus controlled video-matrix-switch
+    with 8 inputs and 6 outputs.
+    It is cascadable, i.e. it can be found at the addresses
+    0x86 and 0x06 on the i2c-bus.
+
+    For detailed information download the specifications directly
+    from SGS Thomson at http://www.st.com
+
+  */
+
+
+#include <linux/module.h>
+#include <linux/ioctl.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <media/v4l2-device.h>
+#include "tea6415c.h"
+
+MODULE_AUTHOR("Michael Hunold <michael@mihu.de>");
+MODULE_DESCRIPTION("tea6415c driver");
+MODULE_LICENSE("GPL");
+
+static int debug;
+module_param(debug, int, 0644);
+
+MODULE_PARM_DESC(debug, "Debug level (0-1)");
+
+
+/* makes a connection between the input-pin 'i' and the output-pin 'o' */
+static int tea6415c_s_routing(struct v4l2_subdev *sd,
+			      u32 i, u32 o, u32 config)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u8 byte = 0;
+	int ret;
+
+	v4l2_dbg(1, debug, sd, "i=%d, o=%d\n", i, o);
+
+	/* check if the pins are valid */
+	if (0 == ((1 == i ||  3 == i ||  5 == i ||  6 == i ||  8 == i || 10 == i || 20 == i || 11 == i)
+	      && (18 == o || 17 == o || 16 == o || 15 == o || 14 == o || 13 == o)))
+		return -EINVAL;
+
+	/* to understand this, have a look at the tea6415c-specs (p.5) */
+	switch (o) {
+	case 18:
+		byte = 0x00;
+		break;
+	case 14:
+		byte = 0x20;
+		break;
+	case 16:
+		byte = 0x10;
+		break;
+	case 17:
+		byte = 0x08;
+		break;
+	case 15:
+		byte = 0x18;
+		break;
+	case 13:
+		byte = 0x28;
+		break;
+	}
+
+	switch (i) {
+	case 5:
+		byte |= 0x00;
+		break;
+	case 8:
+		byte |= 0x04;
+		break;
+	case 3:
+		byte |= 0x02;
+		break;
+	case 20:
+		byte |= 0x06;
+		break;
+	case 6:
+		byte |= 0x01;
+		break;
+	case 10:
+		byte |= 0x05;
+		break;
+	case 1:
+		byte |= 0x03;
+		break;
+	case 11:
+		byte |= 0x07;
+		break;
+	}
+
+	ret = i2c_smbus_write_byte(client, byte);
+	if (ret) {
+		v4l2_dbg(1, debug, sd,
+			"i2c_smbus_write_byte() failed, ret:%d\n", ret);
+		return -EIO;
+	}
+	return ret;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_video_ops tea6415c_video_ops = {
+	.s_routing = tea6415c_s_routing,
+};
+
+static const struct v4l2_subdev_ops tea6415c_ops = {
+	.video = &tea6415c_video_ops,
+};
+
+static int tea6415c_probe(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd;
+
+	/* let's see whether this adapter can support what we need */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_WRITE_BYTE))
+		return -EIO;
+
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+			client->addr << 1, client->adapter->name);
+	sd = devm_kzalloc(&client->dev, sizeof(*sd), GFP_KERNEL);
+	if (sd == NULL)
+		return -ENOMEM;
+	v4l2_i2c_subdev_init(sd, client, &tea6415c_ops);
+	return 0;
+}
+
+static void tea6415c_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+}
+
+static const struct i2c_device_id tea6415c_id[] = {
+	{ "tea6415c", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, tea6415c_id);
+
+static struct i2c_driver tea6415c_driver = {
+	.driver = {
+		.name	= "tea6415c",
+	},
+	.probe		= tea6415c_probe,
+	.remove		= tea6415c_remove,
+	.id_table	= tea6415c_id,
+};
+
+module_i2c_driver(tea6415c_driver);
diff --git a/drivers/media/i2c/tea6415c.h b/drivers/media/i2c/tea6415c.h
new file mode 100644
index 000000000..f43228207
--- /dev/null
+++ b/drivers/media/i2c/tea6415c.h
@@ -0,0 +1,28 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef __INCLUDED_TEA6415C__
+#define __INCLUDED_TEA6415C__
+
+/* the tea6415c's design is quite brain-dead. although there are
+   8 inputs and 6 outputs, these aren't enumerated in any way. because
+   I don't want to say "connect input pin 20 to output pin 17", I define
+   a "virtual" pin-order. */
+
+/* input pins */
+#define TEA6415C_OUTPUT1 18
+#define TEA6415C_OUTPUT2 14
+#define TEA6415C_OUTPUT3 16
+#define TEA6415C_OUTPUT4 17
+#define TEA6415C_OUTPUT5 13
+#define TEA6415C_OUTPUT6 15
+
+/* output pins */
+#define TEA6415C_INPUT1 5
+#define TEA6415C_INPUT2 8
+#define TEA6415C_INPUT3 3
+#define TEA6415C_INPUT4 20
+#define TEA6415C_INPUT5 6
+#define TEA6415C_INPUT6 10
+#define TEA6415C_INPUT7 1
+#define TEA6415C_INPUT8 11
+
+#endif
diff --git a/drivers/media/i2c/tea6420.c b/drivers/media/i2c/tea6420.c
new file mode 100644
index 000000000..63f23784b
--- /dev/null
+++ b/drivers/media/i2c/tea6420.c
@@ -0,0 +1,140 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+ /*
+    tea6420 - i2c-driver for the tea6420 by SGS Thomson
+
+    Copyright (C) 1998-2003 Michael Hunold <michael@mihu.de>
+    Copyright (C) 2008 Hans Verkuil <hverkuil@xs4all.nl>
+
+    The tea6420 is a bus controlled audio-matrix with 5 stereo inputs,
+    4 stereo outputs and gain control for each output.
+    It is cascadable, i.e. it can be found at the addresses 0x98
+    and 0x9a on the i2c-bus.
+
+    For detailed information download the specifications directly
+    from SGS Thomson at http://www.st.com
+
+  */
+
+
+#include <linux/module.h>
+#include <linux/ioctl.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <media/v4l2-device.h>
+#include "tea6420.h"
+
+MODULE_AUTHOR("Michael Hunold <michael@mihu.de>");
+MODULE_DESCRIPTION("tea6420 driver");
+MODULE_LICENSE("GPL");
+
+static int debug;
+module_param(debug, int, 0644);
+
+MODULE_PARM_DESC(debug, "Debug level (0-1)");
+
+
+/* make a connection between the input 'i' and the output 'o'
+   with gain 'g' (note: i = 6 means 'mute') */
+static int tea6420_s_routing(struct v4l2_subdev *sd,
+			     u32 i, u32 o, u32 config)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int g = (o >> 4) & 0xf;
+	u8 byte;
+	int ret;
+
+	o &= 0xf;
+	v4l2_dbg(1, debug, sd, "i=%d, o=%d, g=%d\n", i, o, g);
+
+	/* check if the parameters are valid */
+	if (i < 1 || i > 6 || o < 1 || o > 4 || g < 0 || g > 6 || g % 2 != 0)
+		return -EINVAL;
+
+	byte = ((o - 1) << 5);
+	byte |= (i - 1);
+
+	/* to understand this, have a look at the tea6420-specs (p.5) */
+	switch (g) {
+	case 0:
+		byte |= (3 << 3);
+		break;
+	case 2:
+		byte |= (2 << 3);
+		break;
+	case 4:
+		byte |= (1 << 3);
+		break;
+	case 6:
+		break;
+	}
+
+	ret = i2c_smbus_write_byte(client, byte);
+	if (ret) {
+		v4l2_dbg(1, debug, sd,
+			"i2c_smbus_write_byte() failed, ret:%d\n", ret);
+		return -EIO;
+	}
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_audio_ops tea6420_audio_ops = {
+	.s_routing = tea6420_s_routing,
+};
+
+static const struct v4l2_subdev_ops tea6420_ops = {
+	.audio = &tea6420_audio_ops,
+};
+
+static int tea6420_probe(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd;
+	int err, i;
+
+	/* let's see whether this adapter can support what we need */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_WRITE_BYTE))
+		return -EIO;
+
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	sd = devm_kzalloc(&client->dev, sizeof(*sd), GFP_KERNEL);
+	if (sd == NULL)
+		return -ENOMEM;
+	v4l2_i2c_subdev_init(sd, client, &tea6420_ops);
+
+	/* set initial values: set "mute"-input to all outputs at gain 0 */
+	err = 0;
+	for (i = 1; i < 5; i++)
+		err += tea6420_s_routing(sd, 6, i, 0);
+	if (err) {
+		v4l_dbg(1, debug, client, "could not initialize tea6420\n");
+		return -ENODEV;
+	}
+	return 0;
+}
+
+static void tea6420_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+}
+
+static const struct i2c_device_id tea6420_id[] = {
+	{ "tea6420", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, tea6420_id);
+
+static struct i2c_driver tea6420_driver = {
+	.driver = {
+		.name	= "tea6420",
+	},
+	.probe		= tea6420_probe,
+	.remove		= tea6420_remove,
+	.id_table	= tea6420_id,
+};
+
+module_i2c_driver(tea6420_driver);
diff --git a/drivers/media/i2c/tea6420.h b/drivers/media/i2c/tea6420.h
new file mode 100644
index 000000000..07f9d72a8
--- /dev/null
+++ b/drivers/media/i2c/tea6420.h
@@ -0,0 +1,25 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef __INCLUDED_TEA6420__
+#define __INCLUDED_TEA6420__
+
+/* input pins */
+#define TEA6420_OUTPUT1 1
+#define TEA6420_OUTPUT2 2
+#define TEA6420_OUTPUT3 3
+#define TEA6420_OUTPUT4 4
+
+/* output pins */
+#define TEA6420_INPUT1 1
+#define TEA6420_INPUT2 2
+#define TEA6420_INPUT3 3
+#define TEA6420_INPUT4 4
+#define TEA6420_INPUT5 5
+#define TEA6420_INPUT6 6
+
+/* gain on the output pins, ORed with the output pin */
+#define TEA6420_GAIN0 0x00
+#define TEA6420_GAIN2 0x20
+#define TEA6420_GAIN4 0x40
+#define TEA6420_GAIN6 0x60
+
+#endif
diff --git a/drivers/media/i2c/ths7303.c b/drivers/media/i2c/ths7303.c
new file mode 100644
index 000000000..ea70c1c13
--- /dev/null
+++ b/drivers/media/i2c/ths7303.c
@@ -0,0 +1,384 @@
+/*
+ * ths7303/53- THS7303/53 Video Amplifier driver
+ *
+ * Copyright (C) 2009 Texas Instruments Incorporated - http://www.ti.com/
+ * Copyright 2013 Cisco Systems, Inc. and/or its affiliates.
+ *
+ * Author: Chaithrika U S <chaithrika@ti.com>
+ *
+ * Contributors:
+ *     Hans Verkuil <hans.verkuil@cisco.com>
+ *     Lad, Prabhakar <prabhakar.lad@ti.com>
+ *     Martin Bugge <marbugge@cisco.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed .as is. WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+
+#include <media/i2c/ths7303.h>
+#include <media/v4l2-device.h>
+
+#define THS7303_CHANNEL_1	1
+#define THS7303_CHANNEL_2	2
+#define THS7303_CHANNEL_3	3
+
+struct ths7303_state {
+	struct v4l2_subdev		sd;
+	const struct ths7303_platform_data *pdata;
+	struct v4l2_bt_timings		bt;
+	int std_id;
+	int stream_on;
+};
+
+enum ths7303_filter_mode {
+	THS7303_FILTER_MODE_480I_576I,
+	THS7303_FILTER_MODE_480P_576P,
+	THS7303_FILTER_MODE_720P_1080I,
+	THS7303_FILTER_MODE_1080P,
+	THS7303_FILTER_MODE_DISABLE
+};
+
+MODULE_DESCRIPTION("TI THS7303 video amplifier driver");
+MODULE_AUTHOR("Chaithrika U S");
+MODULE_LICENSE("GPL");
+
+static inline struct ths7303_state *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct ths7303_state, sd);
+}
+
+static int ths7303_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return i2c_smbus_read_byte_data(client, reg);
+}
+
+static int ths7303_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+	int i;
+
+	for (i = 0; i < 3; i++) {
+		ret = i2c_smbus_write_byte_data(client, reg, val);
+		if (ret == 0)
+			return 0;
+	}
+	return ret;
+}
+
+/* following function is used to set ths7303 */
+static int ths7303_setval(struct v4l2_subdev *sd,
+			  enum ths7303_filter_mode mode)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ths7303_state *state = to_state(sd);
+	const struct ths7303_platform_data *pdata = state->pdata;
+	u8 val, sel = 0;
+	int err, disable = 0;
+
+	if (!client)
+		return -EINVAL;
+
+	switch (mode) {
+	case THS7303_FILTER_MODE_1080P:
+		sel = 0x3;	/*1080p and SXGA/UXGA */
+		break;
+	case THS7303_FILTER_MODE_720P_1080I:
+		sel = 0x2;	/*720p, 1080i and SVGA/XGA */
+		break;
+	case THS7303_FILTER_MODE_480P_576P:
+		sel = 0x1;	/* EDTV 480p/576p and VGA */
+		break;
+	case THS7303_FILTER_MODE_480I_576I:
+		sel = 0x0;	/* SDTV, S-Video, 480i/576i */
+		break;
+	default:
+		/* disable all channels */
+		disable = 1;
+	}
+
+	val = (sel << 6) | (sel << 3);
+	if (!disable)
+		val |= (pdata->ch_1 & 0x27);
+	err = ths7303_write(sd, THS7303_CHANNEL_1, val);
+	if (err)
+		goto out;
+
+	val = (sel << 6) | (sel << 3);
+	if (!disable)
+		val |= (pdata->ch_2 & 0x27);
+	err = ths7303_write(sd, THS7303_CHANNEL_2, val);
+	if (err)
+		goto out;
+
+	val = (sel << 6) | (sel << 3);
+	if (!disable)
+		val |= (pdata->ch_3 & 0x27);
+	err = ths7303_write(sd, THS7303_CHANNEL_3, val);
+	if (err)
+		goto out;
+
+	return 0;
+out:
+	pr_info("write byte data failed\n");
+	return err;
+}
+
+static int ths7303_s_std_output(struct v4l2_subdev *sd, v4l2_std_id norm)
+{
+	struct ths7303_state *state = to_state(sd);
+
+	if (norm & (V4L2_STD_ALL & ~V4L2_STD_SECAM)) {
+		state->std_id = 1;
+		state->bt.pixelclock = 0;
+		return ths7303_setval(sd, THS7303_FILTER_MODE_480I_576I);
+	}
+
+	return ths7303_setval(sd, THS7303_FILTER_MODE_DISABLE);
+}
+
+static int ths7303_config(struct v4l2_subdev *sd)
+{
+	struct ths7303_state *state = to_state(sd);
+	int res;
+
+	if (!state->stream_on) {
+		ths7303_write(sd, THS7303_CHANNEL_1,
+			      (ths7303_read(sd, THS7303_CHANNEL_1) & 0xf8) |
+			      0x00);
+		ths7303_write(sd, THS7303_CHANNEL_2,
+			      (ths7303_read(sd, THS7303_CHANNEL_2) & 0xf8) |
+			      0x00);
+		ths7303_write(sd, THS7303_CHANNEL_3,
+			      (ths7303_read(sd, THS7303_CHANNEL_3) & 0xf8) |
+			      0x00);
+		return 0;
+	}
+
+	if (state->bt.pixelclock > 120000000)
+		res = ths7303_setval(sd, THS7303_FILTER_MODE_1080P);
+	else if (state->bt.pixelclock > 70000000)
+		res = ths7303_setval(sd, THS7303_FILTER_MODE_720P_1080I);
+	else if (state->bt.pixelclock > 20000000)
+		res = ths7303_setval(sd, THS7303_FILTER_MODE_480P_576P);
+	else if (state->std_id)
+		res = ths7303_setval(sd, THS7303_FILTER_MODE_480I_576I);
+	else
+		/* disable all channels */
+		res = ths7303_setval(sd, THS7303_FILTER_MODE_DISABLE);
+
+	return res;
+
+}
+
+static int ths7303_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct ths7303_state *state = to_state(sd);
+
+	state->stream_on = enable;
+
+	return ths7303_config(sd);
+}
+
+/* for setting filter for HD output */
+static int ths7303_s_dv_timings(struct v4l2_subdev *sd,
+			       struct v4l2_dv_timings *dv_timings)
+{
+	struct ths7303_state *state = to_state(sd);
+
+	if (!dv_timings || dv_timings->type != V4L2_DV_BT_656_1120)
+		return -EINVAL;
+
+	state->bt = dv_timings->bt;
+	state->std_id = 0;
+
+	return ths7303_config(sd);
+}
+
+static const struct v4l2_subdev_video_ops ths7303_video_ops = {
+	.s_stream	= ths7303_s_stream,
+	.s_std_output	= ths7303_s_std_output,
+	.s_dv_timings   = ths7303_s_dv_timings,
+};
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+
+static int ths7303_g_register(struct v4l2_subdev *sd,
+			      struct v4l2_dbg_register *reg)
+{
+	reg->size = 1;
+	reg->val = ths7303_read(sd, reg->reg);
+	return 0;
+}
+
+static int ths7303_s_register(struct v4l2_subdev *sd,
+			      const struct v4l2_dbg_register *reg)
+{
+	ths7303_write(sd, reg->reg, reg->val);
+	return 0;
+}
+#endif
+
+static const char * const stc_lpf_sel_txt[4] = {
+	"500-kHz Filter",
+	"2.5-MHz Filter",
+	"5-MHz Filter",
+	"5-MHz Filter",
+};
+
+static const char * const in_mux_sel_txt[2] = {
+	"Input A Select",
+	"Input B Select",
+};
+
+static const char * const lpf_freq_sel_txt[4] = {
+	"9-MHz LPF",
+	"16-MHz LPF",
+	"35-MHz LPF",
+	"Bypass LPF",
+};
+
+static const char * const in_bias_sel_dis_cont_txt[8] = {
+	"Disable Channel",
+	"Mute Function - No Output",
+	"DC Bias Select",
+	"DC Bias + 250 mV Offset Select",
+	"AC Bias Select",
+	"Sync Tip Clamp with low bias",
+	"Sync Tip Clamp with mid bias",
+	"Sync Tip Clamp with high bias",
+};
+
+static void ths7303_log_channel_status(struct v4l2_subdev *sd, u8 reg)
+{
+	u8 val = ths7303_read(sd, reg);
+
+	if ((val & 0x7) == 0) {
+		v4l2_info(sd, "Channel %d Off\n", reg);
+		return;
+	}
+
+	v4l2_info(sd, "Channel %d On\n", reg);
+	v4l2_info(sd, "  value 0x%x\n", val);
+	v4l2_info(sd, "  %s\n", stc_lpf_sel_txt[(val >> 6) & 0x3]);
+	v4l2_info(sd, "  %s\n", in_mux_sel_txt[(val >> 5) & 0x1]);
+	v4l2_info(sd, "  %s\n", lpf_freq_sel_txt[(val >> 3) & 0x3]);
+	v4l2_info(sd, "  %s\n", in_bias_sel_dis_cont_txt[(val >> 0) & 0x7]);
+}
+
+static int ths7303_log_status(struct v4l2_subdev *sd)
+{
+	struct ths7303_state *state = to_state(sd);
+
+	v4l2_info(sd, "stream %s\n", state->stream_on ? "On" : "Off");
+
+	if (state->bt.pixelclock) {
+		struct v4l2_bt_timings *bt = &state->bt;
+		u32 frame_width, frame_height;
+
+		frame_width = V4L2_DV_BT_FRAME_WIDTH(bt);
+		frame_height = V4L2_DV_BT_FRAME_HEIGHT(bt);
+		v4l2_info(sd,
+			  "timings: %dx%d%s%d (%dx%d). Pix freq. = %d Hz. Polarities = 0x%x\n",
+			  bt->width, bt->height, bt->interlaced ? "i" : "p",
+			  (frame_height * frame_width) > 0 ?
+			  (int)bt->pixelclock /
+			  (frame_height * frame_width) : 0,
+			  frame_width, frame_height,
+			  (int)bt->pixelclock, bt->polarities);
+	} else {
+		v4l2_info(sd, "no timings set\n");
+	}
+
+	ths7303_log_channel_status(sd, THS7303_CHANNEL_1);
+	ths7303_log_channel_status(sd, THS7303_CHANNEL_2);
+	ths7303_log_channel_status(sd, THS7303_CHANNEL_3);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops ths7303_core_ops = {
+	.log_status = ths7303_log_status,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = ths7303_g_register,
+	.s_register = ths7303_s_register,
+#endif
+};
+
+static const struct v4l2_subdev_ops ths7303_ops = {
+	.core	= &ths7303_core_ops,
+	.video	= &ths7303_video_ops,
+};
+
+static int ths7303_probe(struct i2c_client *client)
+{
+	struct ths7303_platform_data *pdata = client->dev.platform_data;
+	struct ths7303_state *state;
+	struct v4l2_subdev *sd;
+
+	if (pdata == NULL) {
+		dev_err(&client->dev, "No platform data\n");
+		return -EINVAL;
+	}
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -ENODEV;
+
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	state = devm_kzalloc(&client->dev, sizeof(struct ths7303_state),
+			     GFP_KERNEL);
+	if (!state)
+		return -ENOMEM;
+
+	state->pdata = pdata;
+	sd = &state->sd;
+	v4l2_i2c_subdev_init(sd, client, &ths7303_ops);
+
+	/* set to default 480I_576I filter mode */
+	if (ths7303_setval(sd, THS7303_FILTER_MODE_480I_576I) < 0) {
+		v4l_err(client, "Setting to 480I_576I filter mode failed!\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void ths7303_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+}
+
+static const struct i2c_device_id ths7303_id[] = {
+	{"ths7303", 0},
+	{"ths7353", 0},
+	{},
+};
+
+MODULE_DEVICE_TABLE(i2c, ths7303_id);
+
+static struct i2c_driver ths7303_driver = {
+	.driver = {
+		.name	= "ths73x3",
+	},
+	.probe		= ths7303_probe,
+	.remove		= ths7303_remove,
+	.id_table	= ths7303_id,
+};
+
+module_i2c_driver(ths7303_driver);
diff --git a/drivers/media/i2c/ths8200.c b/drivers/media/i2c/ths8200.c
new file mode 100644
index 000000000..0e0f676cd
--- /dev/null
+++ b/drivers/media/i2c/ths8200.c
@@ -0,0 +1,507 @@
+/*
+ * ths8200 - Texas Instruments THS8200 video encoder driver
+ *
+ * Copyright 2013 Cisco Systems, Inc. and/or its affiliates.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed .as is. WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/v4l2-dv-timings.h>
+
+#include <media/v4l2-dv-timings.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-device.h>
+
+#include "ths8200_regs.h"
+
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "debug level (0-2)");
+
+MODULE_DESCRIPTION("Texas Instruments THS8200 video encoder driver");
+MODULE_AUTHOR("Mats Randgaard <mats.randgaard@cisco.com>");
+MODULE_AUTHOR("Martin Bugge <martin.bugge@cisco.com>");
+MODULE_LICENSE("GPL v2");
+
+struct ths8200_state {
+	struct v4l2_subdev sd;
+	uint8_t chip_version;
+	/* Is the ths8200 powered on? */
+	bool power_on;
+	struct v4l2_dv_timings dv_timings;
+};
+
+static const struct v4l2_dv_timings_cap ths8200_timings_cap = {
+	.type = V4L2_DV_BT_656_1120,
+	/* keep this initialization for compatibility with GCC < 4.4.6 */
+	.reserved = { 0 },
+	V4L2_INIT_BT_TIMINGS(640, 1920, 350, 1080, 25000000, 148500000,
+		V4L2_DV_BT_STD_CEA861, V4L2_DV_BT_CAP_PROGRESSIVE)
+};
+
+static inline struct ths8200_state *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct ths8200_state, sd);
+}
+
+static inline unsigned htotal(const struct v4l2_bt_timings *t)
+{
+	return V4L2_DV_BT_FRAME_WIDTH(t);
+}
+
+static inline unsigned vtotal(const struct v4l2_bt_timings *t)
+{
+	return V4L2_DV_BT_FRAME_HEIGHT(t);
+}
+
+static int ths8200_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return i2c_smbus_read_byte_data(client, reg);
+}
+
+static int ths8200_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+	int i;
+
+	for (i = 0; i < 3; i++) {
+		ret = i2c_smbus_write_byte_data(client, reg, val);
+		if (ret == 0)
+			return 0;
+	}
+	v4l2_err(sd, "I2C Write Problem\n");
+	return ret;
+}
+
+/* To set specific bits in the register, a clear-mask is given (to be AND-ed),
+ * and then the value-mask (to be OR-ed).
+ */
+static inline void
+ths8200_write_and_or(struct v4l2_subdev *sd, u8 reg,
+		     uint8_t clr_mask, uint8_t val_mask)
+{
+	ths8200_write(sd, reg, (ths8200_read(sd, reg) & clr_mask) | val_mask);
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+
+static int ths8200_g_register(struct v4l2_subdev *sd,
+			      struct v4l2_dbg_register *reg)
+{
+	reg->val = ths8200_read(sd, reg->reg & 0xff);
+	reg->size = 1;
+
+	return 0;
+}
+
+static int ths8200_s_register(struct v4l2_subdev *sd,
+			      const struct v4l2_dbg_register *reg)
+{
+	ths8200_write(sd, reg->reg & 0xff, reg->val & 0xff);
+
+	return 0;
+}
+#endif
+
+static int ths8200_log_status(struct v4l2_subdev *sd)
+{
+	struct ths8200_state *state = to_state(sd);
+	uint8_t reg_03 = ths8200_read(sd, THS8200_CHIP_CTL);
+
+	v4l2_info(sd, "----- Chip status -----\n");
+	v4l2_info(sd, "version: %u\n", state->chip_version);
+	v4l2_info(sd, "power: %s\n", (reg_03 & 0x0c) ? "off" : "on");
+	v4l2_info(sd, "reset: %s\n", (reg_03 & 0x01) ? "off" : "on");
+	v4l2_info(sd, "test pattern: %s\n",
+		  (reg_03 & 0x20) ? "enabled" : "disabled");
+	v4l2_info(sd, "format: %ux%u\n",
+		  ths8200_read(sd, THS8200_DTG2_PIXEL_CNT_MSB) * 256 +
+		  ths8200_read(sd, THS8200_DTG2_PIXEL_CNT_LSB),
+		  (ths8200_read(sd, THS8200_DTG2_LINE_CNT_MSB) & 0x07) * 256 +
+		  ths8200_read(sd, THS8200_DTG2_LINE_CNT_LSB));
+	v4l2_print_dv_timings(sd->name, "Configured format:",
+			      &state->dv_timings, true);
+	return 0;
+}
+
+/* Power up/down ths8200 */
+static int ths8200_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct ths8200_state *state = to_state(sd);
+
+	v4l2_dbg(1, debug, sd, "%s: power %s\n", __func__, on ? "on" : "off");
+
+	state->power_on = on;
+
+	/* Power up/down - leave in reset state until input video is present */
+	ths8200_write_and_or(sd, THS8200_CHIP_CTL, 0xf2, (on ? 0x00 : 0x0c));
+
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops ths8200_core_ops = {
+	.log_status = ths8200_log_status,
+	.s_power = ths8200_s_power,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = ths8200_g_register,
+	.s_register = ths8200_s_register,
+#endif
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev video operations
+ */
+
+static int ths8200_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct ths8200_state *state = to_state(sd);
+
+	if (enable && !state->power_on)
+		ths8200_s_power(sd, true);
+
+	ths8200_write_and_or(sd, THS8200_CHIP_CTL, 0xfe,
+			     (enable ? 0x01 : 0x00));
+
+	v4l2_dbg(1, debug, sd, "%s: %sable\n",
+		 __func__, (enable ? "en" : "dis"));
+
+	return 0;
+}
+
+static void ths8200_core_init(struct v4l2_subdev *sd)
+{
+	/* setup clocks */
+	ths8200_write_and_or(sd, THS8200_CHIP_CTL, 0x3f, 0xc0);
+
+	/**** Data path control (DATA) ****/
+	/* Set FSADJ 700 mV,
+	 * bypass 422-444 interpolation,
+	 * input format 30 bit RGB444
+	 */
+	ths8200_write(sd, THS8200_DATA_CNTL, 0x70);
+
+	/* DTG Mode (Video blocked during blanking
+	 * VESA slave
+	 */
+	ths8200_write(sd, THS8200_DTG1_MODE, 0x87);
+
+	/**** Display Timing Generator Control, Part 1 (DTG1). ****/
+
+	/* Disable embedded syncs on the output by setting
+	 * the amplitude to zero for all channels.
+	 */
+	ths8200_write(sd, THS8200_DTG1_Y_SYNC_MSB, 0x00);
+	ths8200_write(sd, THS8200_DTG1_CBCR_SYNC_MSB, 0x00);
+}
+
+static void ths8200_setup(struct v4l2_subdev *sd, struct v4l2_bt_timings *bt)
+{
+	uint8_t polarity = 0;
+	uint16_t line_start_active_video = (bt->vsync + bt->vbackporch);
+	uint16_t line_start_front_porch  = (vtotal(bt) - bt->vfrontporch);
+
+	/*** System ****/
+	/* Set chip in reset while it is configured */
+	ths8200_s_stream(sd, false);
+
+	/* configure video output timings */
+	ths8200_write(sd, THS8200_DTG1_SPEC_A, bt->hsync);
+	ths8200_write(sd, THS8200_DTG1_SPEC_B, bt->hfrontporch);
+
+	/* Zero for progressive scan formats.*/
+	if (!bt->interlaced)
+		ths8200_write(sd, THS8200_DTG1_SPEC_C, 0x00);
+
+	/* Distance from leading edge of h sync to start of active video.
+	 * MSB in 0x2b
+	 */
+	ths8200_write(sd, THS8200_DTG1_SPEC_D_LSB,
+		      (bt->hbackporch + bt->hsync) & 0xff);
+	/* Zero for SDTV-mode. MSB in 0x2b */
+	ths8200_write(sd, THS8200_DTG1_SPEC_E_LSB, 0x00);
+	/*
+	 * MSB for dtg1_spec(d/e/h). See comment for
+	 * corresponding LSB registers.
+	 */
+	ths8200_write(sd, THS8200_DTG1_SPEC_DEH_MSB,
+		      ((bt->hbackporch + bt->hsync) & 0x100) >> 1);
+
+	/* h front porch */
+	ths8200_write(sd, THS8200_DTG1_SPEC_K_LSB, (bt->hfrontporch) & 0xff);
+	ths8200_write(sd, THS8200_DTG1_SPEC_K_MSB,
+		      ((bt->hfrontporch) & 0x700) >> 8);
+
+	/* Half the line length. Used to calculate SDTV line types. */
+	ths8200_write(sd, THS8200_DTG1_SPEC_G_LSB, (htotal(bt)/2) & 0xff);
+	ths8200_write(sd, THS8200_DTG1_SPEC_G_MSB,
+		      ((htotal(bt)/2) >> 8) & 0x0f);
+
+	/* Total pixels per line (ex. 720p: 1650) */
+	ths8200_write(sd, THS8200_DTG1_TOT_PIXELS_MSB, htotal(bt) >> 8);
+	ths8200_write(sd, THS8200_DTG1_TOT_PIXELS_LSB, htotal(bt) & 0xff);
+
+	/* Frame height and field height */
+	/* Field height should be programmed higher than frame_size for
+	 * progressive scan formats
+	 */
+	ths8200_write(sd, THS8200_DTG1_FRAME_FIELD_SZ_MSB,
+		      ((vtotal(bt) >> 4) & 0xf0) + 0x7);
+	ths8200_write(sd, THS8200_DTG1_FRAME_SZ_LSB, vtotal(bt) & 0xff);
+
+	/* Should be programmed higher than frame_size
+	 * for progressive formats
+	 */
+	if (!bt->interlaced)
+		ths8200_write(sd, THS8200_DTG1_FIELD_SZ_LSB, 0xff);
+
+	/**** Display Timing Generator Control, Part 2 (DTG2). ****/
+	/* Set breakpoint line numbers and types
+	 * THS8200 generates line types with different properties. A line type
+	 * that sets all the RGB-outputs to zero is used in the blanking areas,
+	 * while a line type that enable the RGB-outputs is used in active video
+	 * area. The line numbers for start of active video, start of front
+	 * porch and after the last line in the frame must be set with the
+	 * corresponding line types.
+	 *
+	 * Line types:
+	 * 0x9 - Full normal sync pulse: Blocks data when dtg1_pass is off.
+	 *       Used in blanking area.
+	 * 0x0 - Active video: Video data is always passed. Used in active
+	 *       video area.
+	 */
+	ths8200_write_and_or(sd, THS8200_DTG2_BP1_2_MSB, 0x88,
+			     ((line_start_active_video >> 4) & 0x70) +
+			     ((line_start_front_porch >> 8) & 0x07));
+	ths8200_write(sd, THS8200_DTG2_BP3_4_MSB, ((vtotal(bt)) >> 4) & 0x70);
+	ths8200_write(sd, THS8200_DTG2_BP1_LSB, line_start_active_video & 0xff);
+	ths8200_write(sd, THS8200_DTG2_BP2_LSB, line_start_front_porch & 0xff);
+	ths8200_write(sd, THS8200_DTG2_BP3_LSB, (vtotal(bt)) & 0xff);
+
+	/* line types */
+	ths8200_write(sd, THS8200_DTG2_LINETYPE1, 0x90);
+	ths8200_write(sd, THS8200_DTG2_LINETYPE2, 0x90);
+
+	/* h sync width transmitted */
+	ths8200_write(sd, THS8200_DTG2_HLENGTH_LSB, bt->hsync & 0xff);
+	ths8200_write_and_or(sd, THS8200_DTG2_HLENGTH_LSB_HDLY_MSB, 0x3f,
+			     (bt->hsync >> 2) & 0xc0);
+
+	/* The pixel value h sync is asserted on */
+	ths8200_write_and_or(sd, THS8200_DTG2_HLENGTH_LSB_HDLY_MSB, 0xe0,
+			     (htotal(bt) >> 8) & 0x1f);
+	ths8200_write(sd, THS8200_DTG2_HLENGTH_HDLY_LSB, htotal(bt));
+
+	/* v sync width transmitted (must add 1 to get correct output) */
+	ths8200_write(sd, THS8200_DTG2_VLENGTH1_LSB, (bt->vsync + 1) & 0xff);
+	ths8200_write_and_or(sd, THS8200_DTG2_VLENGTH1_MSB_VDLY1_MSB, 0x3f,
+			     ((bt->vsync + 1) >> 2) & 0xc0);
+
+	/* The pixel value v sync is asserted on (must add 1 to get correct output) */
+	ths8200_write_and_or(sd, THS8200_DTG2_VLENGTH1_MSB_VDLY1_MSB, 0xf8,
+			     ((vtotal(bt) + 1) >> 8) & 0x7);
+	ths8200_write(sd, THS8200_DTG2_VDLY1_LSB, vtotal(bt) + 1);
+
+	/* For progressive video vlength2 must be set to all 0 and vdly2 must
+	 * be set to all 1.
+	 */
+	ths8200_write(sd, THS8200_DTG2_VLENGTH2_LSB, 0x00);
+	ths8200_write(sd, THS8200_DTG2_VLENGTH2_MSB_VDLY2_MSB, 0x07);
+	ths8200_write(sd, THS8200_DTG2_VDLY2_LSB, 0xff);
+
+	/* Internal delay factors to synchronize the sync pulses and the data */
+	/* Experimental values delays (hor 0, ver 0) */
+	ths8200_write(sd, THS8200_DTG2_HS_IN_DLY_MSB, 0);
+	ths8200_write(sd, THS8200_DTG2_HS_IN_DLY_LSB, 0);
+	ths8200_write(sd, THS8200_DTG2_VS_IN_DLY_MSB, 0);
+	ths8200_write(sd, THS8200_DTG2_VS_IN_DLY_LSB, 0);
+
+	/* Polarity of received and transmitted sync signals */
+	if (bt->polarities & V4L2_DV_HSYNC_POS_POL) {
+		polarity |= 0x01; /* HS_IN */
+		polarity |= 0x08; /* HS_OUT */
+	}
+	if (bt->polarities & V4L2_DV_VSYNC_POS_POL) {
+		polarity |= 0x02; /* VS_IN */
+		polarity |= 0x10; /* VS_OUT */
+	}
+
+	/* RGB mode, no embedded timings */
+	/* Timing of video input bus is derived from HS, VS, and FID dedicated
+	 * inputs
+	 */
+	ths8200_write(sd, THS8200_DTG2_CNTL, 0x44 | polarity);
+
+	/* leave reset */
+	ths8200_s_stream(sd, true);
+
+	v4l2_dbg(1, debug, sd, "%s: frame %dx%d, polarity %d\n"
+		 "horizontal: front porch %d, back porch %d, sync %d\n"
+		 "vertical: sync %d\n", __func__, htotal(bt), vtotal(bt),
+		 polarity, bt->hfrontporch, bt->hbackporch,
+		 bt->hsync, bt->vsync);
+}
+
+static int ths8200_s_dv_timings(struct v4l2_subdev *sd,
+				struct v4l2_dv_timings *timings)
+{
+	struct ths8200_state *state = to_state(sd);
+
+	v4l2_dbg(1, debug, sd, "%s:\n", __func__);
+
+	if (!v4l2_valid_dv_timings(timings, &ths8200_timings_cap,
+				NULL, NULL))
+		return -EINVAL;
+
+	if (!v4l2_find_dv_timings_cap(timings, &ths8200_timings_cap, 10,
+				NULL, NULL)) {
+		v4l2_dbg(1, debug, sd, "Unsupported format\n");
+		return -EINVAL;
+	}
+
+	timings->bt.flags &= ~V4L2_DV_FL_REDUCED_FPS;
+
+	/* save timings */
+	state->dv_timings = *timings;
+
+	ths8200_setup(sd, &timings->bt);
+
+	return 0;
+}
+
+static int ths8200_g_dv_timings(struct v4l2_subdev *sd,
+				struct v4l2_dv_timings *timings)
+{
+	struct ths8200_state *state = to_state(sd);
+
+	v4l2_dbg(1, debug, sd, "%s:\n", __func__);
+
+	*timings = state->dv_timings;
+
+	return 0;
+}
+
+static int ths8200_enum_dv_timings(struct v4l2_subdev *sd,
+				   struct v4l2_enum_dv_timings *timings)
+{
+	if (timings->pad != 0)
+		return -EINVAL;
+
+	return v4l2_enum_dv_timings_cap(timings, &ths8200_timings_cap,
+			NULL, NULL);
+}
+
+static int ths8200_dv_timings_cap(struct v4l2_subdev *sd,
+				  struct v4l2_dv_timings_cap *cap)
+{
+	if (cap->pad != 0)
+		return -EINVAL;
+
+	*cap = ths8200_timings_cap;
+	return 0;
+}
+
+/* Specific video subsystem operation handlers */
+static const struct v4l2_subdev_video_ops ths8200_video_ops = {
+	.s_stream = ths8200_s_stream,
+	.s_dv_timings = ths8200_s_dv_timings,
+	.g_dv_timings = ths8200_g_dv_timings,
+};
+
+static const struct v4l2_subdev_pad_ops ths8200_pad_ops = {
+	.enum_dv_timings = ths8200_enum_dv_timings,
+	.dv_timings_cap = ths8200_dv_timings_cap,
+};
+
+/* V4L2 top level operation handlers */
+static const struct v4l2_subdev_ops ths8200_ops = {
+	.core  = &ths8200_core_ops,
+	.video = &ths8200_video_ops,
+	.pad = &ths8200_pad_ops,
+};
+
+static int ths8200_probe(struct i2c_client *client)
+{
+	struct ths8200_state *state;
+	struct v4l2_subdev *sd;
+	int error;
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+
+	state = devm_kzalloc(&client->dev, sizeof(*state), GFP_KERNEL);
+	if (!state)
+		return -ENOMEM;
+
+	sd = &state->sd;
+	v4l2_i2c_subdev_init(sd, client, &ths8200_ops);
+
+	state->chip_version = ths8200_read(sd, THS8200_VERSION);
+	v4l2_dbg(1, debug, sd, "chip version 0x%x\n", state->chip_version);
+
+	ths8200_core_init(sd);
+
+	error = v4l2_async_register_subdev(&state->sd);
+	if (error)
+		return error;
+
+	v4l2_info(sd, "%s found @ 0x%x (%s)\n", client->name,
+		  client->addr << 1, client->adapter->name);
+
+	return 0;
+}
+
+static void ths8200_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ths8200_state *decoder = to_state(sd);
+
+	v4l2_dbg(1, debug, sd, "%s removed @ 0x%x (%s)\n", client->name,
+		 client->addr << 1, client->adapter->name);
+
+	ths8200_s_power(sd, false);
+	v4l2_async_unregister_subdev(&decoder->sd);
+}
+
+static const struct i2c_device_id ths8200_id[] = {
+	{ "ths8200", 0 },
+	{},
+};
+MODULE_DEVICE_TABLE(i2c, ths8200_id);
+
+#if IS_ENABLED(CONFIG_OF)
+static const struct of_device_id ths8200_of_match[] = {
+	{ .compatible = "ti,ths8200", },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, ths8200_of_match);
+#endif
+
+static struct i2c_driver ths8200_driver = {
+	.driver = {
+		.name = "ths8200",
+		.of_match_table = of_match_ptr(ths8200_of_match),
+	},
+	.probe = ths8200_probe,
+	.remove = ths8200_remove,
+	.id_table = ths8200_id,
+};
+
+module_i2c_driver(ths8200_driver);
diff --git a/drivers/media/i2c/ths8200_regs.h b/drivers/media/i2c/ths8200_regs.h
new file mode 100644
index 000000000..6bc9fd111
--- /dev/null
+++ b/drivers/media/i2c/ths8200_regs.h
@@ -0,0 +1,161 @@
+/*
+ * ths8200 - Texas Instruments THS8200 video encoder driver
+ *
+ * Copyright 2013 Cisco Systems, Inc. and/or its affiliates.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed .as is. WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef THS8200_REGS_H
+#define THS8200_REGS_H
+
+/* Register offset macros */
+#define THS8200_VERSION				0x02
+#define THS8200_CHIP_CTL			0x03
+#define THS8200_CSC_R11				0x04
+#define THS8200_CSC_R12				0x05
+#define THS8200_CSC_R21				0x06
+#define THS8200_CSC_R22				0x07
+#define THS8200_CSC_R31				0x08
+#define THS8200_CSC_R32				0x09
+#define THS8200_CSC_G11				0x0a
+#define THS8200_CSC_G12				0x0b
+#define THS8200_CSC_G21				0x0c
+#define THS8200_CSC_G22				0x0d
+#define THS8200_CSC_G31				0x0e
+#define THS8200_CSC_G32				0x0f
+#define THS8200_CSC_B11				0x10
+#define THS8200_CSC_B12				0x11
+#define THS8200_CSC_B21				0x12
+#define THS8200_CSC_B22				0x13
+#define THS8200_CSC_B31				0x14
+#define THS8200_CSC_B32				0x15
+#define THS8200_CSC_OFFS1			0x16
+#define THS8200_CSC_OFFS12			0x17
+#define THS8200_CSC_OFFS23			0x18
+#define THS8200_CSC_OFFS3			0x19
+#define THS8200_TST_CNTL1			0x1a
+#define THS8200_TST_CNTL2			0x1b
+#define THS8200_DATA_CNTL			0x1c
+#define THS8200_DTG1_Y_SYNC1_LSB		0x1d
+#define THS8200_DTG1_Y_SYNC2_LSB		0x1e
+#define THS8200_DTG1_Y_SYNC3_LSB		0x1f
+#define THS8200_DTG1_CBCR_SYNC1_LSB		0x20
+#define THS8200_DTG1_CBCR_SYNC2_LSB		0x21
+#define THS8200_DTG1_CBCR_SYNC3_LSB		0x22
+#define THS8200_DTG1_Y_SYNC_MSB			0x23
+#define THS8200_DTG1_CBCR_SYNC_MSB		0x24
+#define THS8200_DTG1_SPEC_A			0x25
+#define THS8200_DTG1_SPEC_B			0x26
+#define THS8200_DTG1_SPEC_C			0x27
+#define THS8200_DTG1_SPEC_D_LSB			0x28
+#define THS8200_DTG1_SPEC_D1			0x29
+#define THS8200_DTG1_SPEC_E_LSB			0x2a
+#define THS8200_DTG1_SPEC_DEH_MSB		0x2b
+#define THS8200_DTG1_SPEC_H_LSB			0x2c
+#define THS8200_DTG1_SPEC_I_MSB			0x2d
+#define THS8200_DTG1_SPEC_I_LSB			0x2e
+#define THS8200_DTG1_SPEC_K_LSB			0x2f
+#define THS8200_DTG1_SPEC_K_MSB			0x30
+#define THS8200_DTG1_SPEC_K1			0x31
+#define THS8200_DTG1_SPEC_G_LSB			0x32
+#define THS8200_DTG1_SPEC_G_MSB			0x33
+#define THS8200_DTG1_TOT_PIXELS_MSB		0x34
+#define THS8200_DTG1_TOT_PIXELS_LSB		0x35
+#define THS8200_DTG1_FLD_FLIP_LINECNT_MSB	0x36
+#define THS8200_DTG1_LINECNT_LSB		0x37
+#define THS8200_DTG1_MODE			0x38
+#define THS8200_DTG1_FRAME_FIELD_SZ_MSB		0x39
+#define THS8200_DTG1_FRAME_SZ_LSB		0x3a
+#define THS8200_DTG1_FIELD_SZ_LSB		0x3b
+#define THS8200_DTG1_VESA_CBAR_SIZE		0x3c
+#define THS8200_DAC_CNTL_MSB			0x3d
+#define THS8200_DAC1_CNTL_LSB			0x3e
+#define THS8200_DAC2_CNTL_LSB			0x3f
+#define THS8200_DAC3_CNTL_LSB			0x40
+#define THS8200_CSM_CLIP_GY_LOW			0x41
+#define THS8200_CSM_CLIP_BCB_LOW		0x42
+#define THS8200_CSM_CLIP_RCR_LOW		0x43
+#define THS8200_CSM_CLIP_GY_HIGH		0x44
+#define THS8200_CSM_CLIP_BCB_HIGH		0x45
+#define THS8200_CSM_CLIP_RCR_HIGH		0x46
+#define THS8200_CSM_SHIFT_GY			0x47
+#define THS8200_CSM_SHIFT_BCB			0x48
+#define THS8200_CSM_SHIFT_RCR			0x49
+#define THS8200_CSM_GY_CNTL_MULT_MSB		0x4a
+#define THS8200_CSM_MULT_BCB_RCR_MSB		0x4b
+#define THS8200_CSM_MULT_GY_LSB			0x4c
+#define THS8200_CSM_MULT_BCB_LSB		0x4d
+#define THS8200_CSM_MULT_RCR_LSB		0x4e
+#define THS8200_CSM_MULT_RCR_BCB_CNTL		0x4f
+#define THS8200_CSM_MULT_RCR_LSB		0x4e
+#define THS8200_DTG2_BP1_2_MSB			0x50
+#define THS8200_DTG2_BP3_4_MSB			0x51
+#define THS8200_DTG2_BP5_6_MSB			0x52
+#define THS8200_DTG2_BP7_8_MSB			0x53
+#define THS8200_DTG2_BP9_10_MSB			0x54
+#define THS8200_DTG2_BP11_12_MSB		0x55
+#define THS8200_DTG2_BP13_14_MSB		0x56
+#define THS8200_DTG2_BP15_16_MSB		0x57
+#define THS8200_DTG2_BP1_LSB			0x58
+#define THS8200_DTG2_BP2_LSB			0x59
+#define THS8200_DTG2_BP3_LSB			0x5a
+#define THS8200_DTG2_BP4_LSB			0x5b
+#define THS8200_DTG2_BP5_LSB			0x5c
+#define THS8200_DTG2_BP6_LSB			0x5d
+#define THS8200_DTG2_BP7_LSB			0x5e
+#define THS8200_DTG2_BP8_LSB			0x5f
+#define THS8200_DTG2_BP9_LSB			0x60
+#define THS8200_DTG2_BP10_LSB			0x61
+#define THS8200_DTG2_BP11_LSB			0x62
+#define THS8200_DTG2_BP12_LSB			0x63
+#define THS8200_DTG2_BP13_LSB			0x64
+#define THS8200_DTG2_BP14_LSB			0x65
+#define THS8200_DTG2_BP15_LSB			0x66
+#define THS8200_DTG2_BP16_LSB			0x67
+#define THS8200_DTG2_LINETYPE1			0x68
+#define THS8200_DTG2_LINETYPE2			0x69
+#define THS8200_DTG2_LINETYPE3			0x6a
+#define THS8200_DTG2_LINETYPE4			0x6b
+#define THS8200_DTG2_LINETYPE5			0x6c
+#define THS8200_DTG2_LINETYPE6			0x6d
+#define THS8200_DTG2_LINETYPE7			0x6e
+#define THS8200_DTG2_LINETYPE8			0x6f
+#define THS8200_DTG2_HLENGTH_LSB		0x70
+#define THS8200_DTG2_HLENGTH_LSB_HDLY_MSB	0x71
+#define THS8200_DTG2_HLENGTH_HDLY_LSB		0x72
+#define THS8200_DTG2_VLENGTH1_LSB		0x73
+#define THS8200_DTG2_VLENGTH1_MSB_VDLY1_MSB	0x74
+#define THS8200_DTG2_VDLY1_LSB			0x75
+#define THS8200_DTG2_VLENGTH2_LSB		0x76
+#define THS8200_DTG2_VLENGTH2_MSB_VDLY2_MSB	0x77
+#define THS8200_DTG2_VDLY2_LSB			0x78
+#define THS8200_DTG2_HS_IN_DLY_MSB		0x79
+#define THS8200_DTG2_HS_IN_DLY_LSB		0x7a
+#define THS8200_DTG2_VS_IN_DLY_MSB		0x7b
+#define THS8200_DTG2_VS_IN_DLY_LSB		0x7c
+#define THS8200_DTG2_PIXEL_CNT_MSB		0x7d
+#define THS8200_DTG2_PIXEL_CNT_LSB		0x7e
+#define THS8200_DTG2_LINE_CNT_MSB		0x7f
+#define THS8200_DTG2_LINE_CNT_LSB		0x80
+#define THS8200_DTG2_CNTL			0x82
+#define THS8200_CGMS_CNTL_HEADER		0x83
+#define THS8200_CGMS_PAYLOAD_MSB		0x84
+#define THS8200_CGMS_PAYLOAD_LSB		0x85
+#define THS8200_MISC_PPL_LSB			0x86
+#define THS8200_MISC_PPL_MSB			0x87
+#define THS8200_MISC_LPF_MSB			0x88
+#define THS8200_MISC_LPF_LSB			0x89
+
+#endif /* THS8200_REGS_H */
diff --git a/drivers/media/i2c/tlv320aic23b.c b/drivers/media/i2c/tlv320aic23b.c
new file mode 100644
index 000000000..d800ff8af
--- /dev/null
+++ b/drivers/media/i2c/tlv320aic23b.c
@@ -0,0 +1,205 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * tlv320aic23b - driver version 0.0.1
+ *
+ * Copyright (C) 2006 Scott Alfter <salfter@ssai.us>
+ *
+ * Based on wm8775 driver
+ *
+ * Copyright (C) 2004 Ulf Eklund <ivtv at eklund.to>
+ * Copyright (C) 2005 Hans Verkuil <hverkuil@xs4all.nl>
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <linux/ioctl.h>
+#include <linux/uaccess.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+
+MODULE_DESCRIPTION("tlv320aic23b driver");
+MODULE_AUTHOR("Scott Alfter, Ulf Eklund, Hans Verkuil");
+MODULE_LICENSE("GPL");
+
+
+/* ----------------------------------------------------------------------- */
+
+struct tlv320aic23b_state {
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
+};
+
+static inline struct tlv320aic23b_state *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct tlv320aic23b_state, sd);
+}
+
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct tlv320aic23b_state, hdl)->sd;
+}
+
+static int tlv320aic23b_write(struct v4l2_subdev *sd, int reg, u16 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int i;
+
+	if ((reg < 0 || reg > 9) && (reg != 15)) {
+		v4l2_err(sd, "Invalid register R%d\n", reg);
+		return -1;
+	}
+
+	for (i = 0; i < 3; i++)
+		if (i2c_smbus_write_byte_data(client,
+				(reg << 1) | (val >> 8), val & 0xff) == 0)
+			return 0;
+	v4l2_err(sd, "I2C: cannot write %03x to register R%d\n", val, reg);
+	return -1;
+}
+
+static int tlv320aic23b_s_clock_freq(struct v4l2_subdev *sd, u32 freq)
+{
+	switch (freq) {
+	case 32000: /* set sample rate to 32 kHz */
+		tlv320aic23b_write(sd, 8, 0x018);
+		break;
+	case 44100: /* set sample rate to 44.1 kHz */
+		tlv320aic23b_write(sd, 8, 0x022);
+		break;
+	case 48000: /* set sample rate to 48 kHz */
+		tlv320aic23b_write(sd, 8, 0x000);
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int tlv320aic23b_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUDIO_MUTE:
+		tlv320aic23b_write(sd, 0, 0x180); /* mute both channels */
+		/* set gain on both channels to +3.0 dB */
+		if (!ctrl->val)
+			tlv320aic23b_write(sd, 0, 0x119);
+		return 0;
+	}
+	return -EINVAL;
+}
+
+static int tlv320aic23b_log_status(struct v4l2_subdev *sd)
+{
+	struct tlv320aic23b_state *state = to_state(sd);
+
+	v4l2_ctrl_handler_log_status(&state->hdl, sd->name);
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_ctrl_ops tlv320aic23b_ctrl_ops = {
+	.s_ctrl = tlv320aic23b_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops tlv320aic23b_core_ops = {
+	.log_status = tlv320aic23b_log_status,
+};
+
+static const struct v4l2_subdev_audio_ops tlv320aic23b_audio_ops = {
+	.s_clock_freq = tlv320aic23b_s_clock_freq,
+};
+
+static const struct v4l2_subdev_ops tlv320aic23b_ops = {
+	.core = &tlv320aic23b_core_ops,
+	.audio = &tlv320aic23b_audio_ops,
+};
+
+/* ----------------------------------------------------------------------- */
+
+/* i2c implementation */
+
+/*
+ * Generic i2c probe
+ * concerning the addresses: i2c wants 7 bit (without the r/w bit), so '>>1'
+ */
+
+static int tlv320aic23b_probe(struct i2c_client *client)
+{
+	struct tlv320aic23b_state *state;
+	struct v4l2_subdev *sd;
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	state = devm_kzalloc(&client->dev, sizeof(*state), GFP_KERNEL);
+	if (state == NULL)
+		return -ENOMEM;
+	sd = &state->sd;
+	v4l2_i2c_subdev_init(sd, client, &tlv320aic23b_ops);
+
+	/* Initialize tlv320aic23b */
+
+	/* RESET */
+	tlv320aic23b_write(sd, 15, 0x000);
+	/* turn off DAC & mic input */
+	tlv320aic23b_write(sd, 6, 0x00A);
+	/* left-justified, 24-bit, master mode */
+	tlv320aic23b_write(sd, 7, 0x049);
+	/* set gain on both channels to +3.0 dB */
+	tlv320aic23b_write(sd, 0, 0x119);
+	/* set sample rate to 48 kHz */
+	tlv320aic23b_write(sd, 8, 0x000);
+	/* activate digital interface */
+	tlv320aic23b_write(sd, 9, 0x001);
+
+	v4l2_ctrl_handler_init(&state->hdl, 1);
+	v4l2_ctrl_new_std(&state->hdl, &tlv320aic23b_ctrl_ops,
+			V4L2_CID_AUDIO_MUTE, 0, 1, 1, 0);
+	sd->ctrl_handler = &state->hdl;
+	if (state->hdl.error) {
+		int err = state->hdl.error;
+
+		v4l2_ctrl_handler_free(&state->hdl);
+		return err;
+	}
+	v4l2_ctrl_handler_setup(&state->hdl);
+	return 0;
+}
+
+static void tlv320aic23b_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct tlv320aic23b_state *state = to_state(sd);
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&state->hdl);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id tlv320aic23b_id[] = {
+	{ "tlv320aic23b", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, tlv320aic23b_id);
+
+static struct i2c_driver tlv320aic23b_driver = {
+	.driver = {
+		.name	= "tlv320aic23b",
+	},
+	.probe		= tlv320aic23b_probe,
+	.remove		= tlv320aic23b_remove,
+	.id_table	= tlv320aic23b_id,
+};
+
+module_i2c_driver(tlv320aic23b_driver);
diff --git a/drivers/media/i2c/tvaudio.c b/drivers/media/i2c/tvaudio.c
new file mode 100644
index 000000000..ba20f35ca
--- /dev/null
+++ b/drivers/media/i2c/tvaudio.c
@@ -0,0 +1,2103 @@
+/*
+ * Driver for simple i2c audio chips.
+ *
+ * Copyright (c) 2000 Gerd Knorr
+ * based on code by:
+ *   Eric Sandeen (eric_sandeen@bigfoot.com)
+ *   Steve VanDeBogart (vandebo@uclink.berkeley.edu)
+ *   Greg Alexander (galexand@acm.org)
+ *
+ * For the TDA9875 part:
+ * Copyright (c) 2000 Guillaume Delvit based on Gerd Knorr source
+ * and Eric Sandeen
+ *
+ * Copyright(c) 2005-2008 Mauro Carvalho Chehab
+ *	- Some cleanups, code fixes, etc
+ *	- Convert it to V4L2 API
+ *
+ * This code is placed under the terms of the GNU General Public License
+ *
+ * OPTIONS:
+ *   debug - set to 1 if you'd like to see debug messages
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/string.h>
+#include <linux/timer.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/kthread.h>
+#include <linux/freezer.h>
+
+#include <media/i2c/tvaudio.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+
+/* ---------------------------------------------------------------------- */
+/* insmod args                                                            */
+
+static int debug;	/* insmod parameter */
+module_param(debug, int, 0644);
+
+MODULE_DESCRIPTION("device driver for various i2c TV sound decoder / audiomux chips");
+MODULE_AUTHOR("Eric Sandeen, Steve VanDeBogart, Greg Alexander, Gerd Knorr");
+MODULE_LICENSE("GPL");
+
+#define UNSET    (-1U)
+
+/* ---------------------------------------------------------------------- */
+/* our structs                                                            */
+
+#define MAXREGS 256
+
+struct CHIPSTATE;
+typedef int  (*getvalue)(int);
+typedef int  (*checkit)(struct CHIPSTATE*);
+typedef int  (*initialize)(struct CHIPSTATE*);
+typedef int  (*getrxsubchans)(struct CHIPSTATE *);
+typedef void (*setaudmode)(struct CHIPSTATE*, int mode);
+
+/* i2c command */
+typedef struct AUDIOCMD {
+	int             count;             /* # of bytes to send */
+	unsigned char   bytes[MAXREGS+1];  /* addr, data, data, ... */
+} audiocmd;
+
+/* chip description */
+struct CHIPDESC {
+	char       *name;             /* chip name         */
+	int        addr_lo, addr_hi;  /* i2c address range */
+	int        registers;         /* # of registers    */
+
+	int        *insmodopt;
+	checkit    checkit;
+	initialize initialize;
+	int        flags;
+#define CHIP_HAS_VOLUME      1
+#define CHIP_HAS_BASSTREBLE  2
+#define CHIP_HAS_INPUTSEL    4
+#define CHIP_NEED_CHECKMODE  8
+
+	/* various i2c command sequences */
+	audiocmd   init;
+
+	/* which register has which value */
+	int    leftreg, rightreg, treblereg, bassreg;
+
+	/* initialize with (defaults to 65535/32768/32768 */
+	int    volinit, trebleinit, bassinit;
+
+	/* functions to convert the values (v4l -> chip) */
+	getvalue volfunc, treblefunc, bassfunc;
+
+	/* get/set mode */
+	getrxsubchans	getrxsubchans;
+	setaudmode	setaudmode;
+
+	/* input switch register + values for v4l inputs */
+	int  inputreg;
+	int  inputmap[4];
+	int  inputmute;
+	int  inputmask;
+};
+
+/* current state of the chip */
+struct CHIPSTATE {
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
+	struct {
+		/* volume/balance cluster */
+		struct v4l2_ctrl *volume;
+		struct v4l2_ctrl *balance;
+	};
+
+	/* chip-specific description - should point to
+	   an entry at CHIPDESC table */
+	struct CHIPDESC *desc;
+
+	/* shadow register set */
+	audiocmd   shadow;
+
+	/* current settings */
+	u16 muted;
+	int prevmode;
+	int radio;
+	int input;
+
+	/* thread */
+	struct task_struct   *thread;
+	struct timer_list    wt;
+	int		     audmode;
+};
+
+static inline struct CHIPSTATE *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct CHIPSTATE, sd);
+}
+
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct CHIPSTATE, hdl)->sd;
+}
+
+
+/* ---------------------------------------------------------------------- */
+/* i2c I/O functions                                                      */
+
+static int chip_write(struct CHIPSTATE *chip, int subaddr, int val)
+{
+	struct v4l2_subdev *sd = &chip->sd;
+	struct i2c_client *c = v4l2_get_subdevdata(sd);
+	unsigned char buffer[2];
+	int rc;
+
+	if (subaddr < 0) {
+		v4l2_dbg(1, debug, sd, "chip_write: 0x%x\n", val);
+		chip->shadow.bytes[1] = val;
+		buffer[0] = val;
+		rc = i2c_master_send(c, buffer, 1);
+		if (rc != 1) {
+			v4l2_warn(sd, "I/O error (write 0x%x)\n", val);
+			if (rc < 0)
+				return rc;
+			return -EIO;
+		}
+	} else {
+		if (subaddr + 1 >= ARRAY_SIZE(chip->shadow.bytes)) {
+			v4l2_info(sd,
+				"Tried to access a non-existent register: %d\n",
+				subaddr);
+			return -EINVAL;
+		}
+
+		v4l2_dbg(1, debug, sd, "chip_write: reg%d=0x%x\n",
+			subaddr, val);
+		chip->shadow.bytes[subaddr+1] = val;
+		buffer[0] = subaddr;
+		buffer[1] = val;
+		rc = i2c_master_send(c, buffer, 2);
+		if (rc != 2) {
+			v4l2_warn(sd, "I/O error (write reg%d=0x%x)\n",
+				subaddr, val);
+			if (rc < 0)
+				return rc;
+			return -EIO;
+		}
+	}
+	return 0;
+}
+
+static int chip_write_masked(struct CHIPSTATE *chip,
+			     int subaddr, int val, int mask)
+{
+	struct v4l2_subdev *sd = &chip->sd;
+
+	if (mask != 0) {
+		if (subaddr < 0) {
+			val = (chip->shadow.bytes[1] & ~mask) | (val & mask);
+		} else {
+			if (subaddr + 1 >= ARRAY_SIZE(chip->shadow.bytes)) {
+				v4l2_info(sd,
+					"Tried to access a non-existent register: %d\n",
+					subaddr);
+				return -EINVAL;
+			}
+
+			val = (chip->shadow.bytes[subaddr+1] & ~mask) | (val & mask);
+		}
+	}
+	return chip_write(chip, subaddr, val);
+}
+
+static int chip_read(struct CHIPSTATE *chip)
+{
+	struct v4l2_subdev *sd = &chip->sd;
+	struct i2c_client *c = v4l2_get_subdevdata(sd);
+	unsigned char buffer;
+	int rc;
+
+	rc = i2c_master_recv(c, &buffer, 1);
+	if (rc != 1) {
+		v4l2_warn(sd, "I/O error (read)\n");
+		if (rc < 0)
+			return rc;
+		return -EIO;
+	}
+	v4l2_dbg(1, debug, sd, "chip_read: 0x%x\n", buffer);
+	return buffer;
+}
+
+static int chip_read2(struct CHIPSTATE *chip, int subaddr)
+{
+	struct v4l2_subdev *sd = &chip->sd;
+	struct i2c_client *c = v4l2_get_subdevdata(sd);
+	int rc;
+	unsigned char write[1];
+	unsigned char read[1];
+	struct i2c_msg msgs[2] = {
+		{
+			.addr = c->addr,
+			.len = 1,
+			.buf = write
+		},
+		{
+			.addr = c->addr,
+			.flags = I2C_M_RD,
+			.len = 1,
+			.buf = read
+		}
+	};
+
+	write[0] = subaddr;
+
+	rc = i2c_transfer(c->adapter, msgs, 2);
+	if (rc != 2) {
+		v4l2_warn(sd, "I/O error (read2)\n");
+		if (rc < 0)
+			return rc;
+		return -EIO;
+	}
+	v4l2_dbg(1, debug, sd, "chip_read2: reg%d=0x%x\n",
+		subaddr, read[0]);
+	return read[0];
+}
+
+static int chip_cmd(struct CHIPSTATE *chip, char *name, audiocmd *cmd)
+{
+	struct v4l2_subdev *sd = &chip->sd;
+	struct i2c_client *c = v4l2_get_subdevdata(sd);
+	int i, rc;
+
+	if (0 == cmd->count)
+		return 0;
+
+	if (cmd->count + cmd->bytes[0] - 1 >= ARRAY_SIZE(chip->shadow.bytes)) {
+		v4l2_info(sd,
+			 "Tried to access a non-existent register range: %d to %d\n",
+			 cmd->bytes[0] + 1, cmd->bytes[0] + cmd->count - 1);
+		return -EINVAL;
+	}
+
+	/* FIXME: it seems that the shadow bytes are wrong below !*/
+
+	/* update our shadow register set; print bytes if (debug > 0) */
+	v4l2_dbg(1, debug, sd, "chip_cmd(%s): reg=%d, data:",
+		name, cmd->bytes[0]);
+	for (i = 1; i < cmd->count; i++) {
+		if (debug)
+			printk(KERN_CONT " 0x%x", cmd->bytes[i]);
+		chip->shadow.bytes[i+cmd->bytes[0]] = cmd->bytes[i];
+	}
+	if (debug)
+		printk(KERN_CONT "\n");
+
+	/* send data to the chip */
+	rc = i2c_master_send(c, cmd->bytes, cmd->count);
+	if (rc != cmd->count) {
+		v4l2_warn(sd, "I/O error (%s)\n", name);
+		if (rc < 0)
+			return rc;
+		return -EIO;
+	}
+	return 0;
+}
+
+/* ---------------------------------------------------------------------- */
+/* kernel thread for doing i2c stuff asyncronly
+ *   right now it is used only to check the audio mode (mono/stereo/whatever)
+ *   some time after switching to another TV channel, then turn on stereo
+ *   if available, ...
+ */
+
+static void chip_thread_wake(struct timer_list *t)
+{
+	struct CHIPSTATE *chip = from_timer(chip, t, wt);
+	wake_up_process(chip->thread);
+}
+
+static int chip_thread(void *data)
+{
+	struct CHIPSTATE *chip = data;
+	struct CHIPDESC  *desc = chip->desc;
+	struct v4l2_subdev *sd = &chip->sd;
+	int mode, selected;
+
+	v4l2_dbg(1, debug, sd, "thread started\n");
+	set_freezable();
+	for (;;) {
+		set_current_state(TASK_INTERRUPTIBLE);
+		if (!kthread_should_stop())
+			schedule();
+		set_current_state(TASK_RUNNING);
+		try_to_freeze();
+		if (kthread_should_stop())
+			break;
+		v4l2_dbg(1, debug, sd, "thread wakeup\n");
+
+		/* don't do anything for radio */
+		if (chip->radio)
+			continue;
+
+		/* have a look what's going on */
+		mode = desc->getrxsubchans(chip);
+		if (mode == chip->prevmode)
+			continue;
+
+		/* chip detected a new audio mode - set it */
+		v4l2_dbg(1, debug, sd, "thread checkmode\n");
+
+		chip->prevmode = mode;
+
+		selected = V4L2_TUNER_MODE_MONO;
+		switch (chip->audmode) {
+		case V4L2_TUNER_MODE_MONO:
+			if (mode & V4L2_TUNER_SUB_LANG1)
+				selected = V4L2_TUNER_MODE_LANG1;
+			break;
+		case V4L2_TUNER_MODE_STEREO:
+		case V4L2_TUNER_MODE_LANG1:
+			if (mode & V4L2_TUNER_SUB_LANG1)
+				selected = V4L2_TUNER_MODE_LANG1;
+			else if (mode & V4L2_TUNER_SUB_STEREO)
+				selected = V4L2_TUNER_MODE_STEREO;
+			break;
+		case V4L2_TUNER_MODE_LANG2:
+			if (mode & V4L2_TUNER_SUB_LANG2)
+				selected = V4L2_TUNER_MODE_LANG2;
+			else if (mode & V4L2_TUNER_SUB_STEREO)
+				selected = V4L2_TUNER_MODE_STEREO;
+			break;
+		case V4L2_TUNER_MODE_LANG1_LANG2:
+			if (mode & V4L2_TUNER_SUB_LANG2)
+				selected = V4L2_TUNER_MODE_LANG1_LANG2;
+			else if (mode & V4L2_TUNER_SUB_STEREO)
+				selected = V4L2_TUNER_MODE_STEREO;
+		}
+		desc->setaudmode(chip, selected);
+
+		/* schedule next check */
+		mod_timer(&chip->wt, jiffies+msecs_to_jiffies(2000));
+	}
+
+	v4l2_dbg(1, debug, sd, "thread exiting\n");
+	return 0;
+}
+
+/* ---------------------------------------------------------------------- */
+/* audio chip descriptions - defines+functions for tda9840                */
+
+#define TDA9840_SW         0x00
+#define TDA9840_LVADJ      0x02
+#define TDA9840_STADJ      0x03
+#define TDA9840_TEST       0x04
+
+#define TDA9840_MONO       0x10
+#define TDA9840_STEREO     0x2a
+#define TDA9840_DUALA      0x12
+#define TDA9840_DUALB      0x1e
+#define TDA9840_DUALAB     0x1a
+#define TDA9840_DUALBA     0x16
+#define TDA9840_EXTERNAL   0x7a
+
+#define TDA9840_DS_DUAL    0x20 /* Dual sound identified          */
+#define TDA9840_ST_STEREO  0x40 /* Stereo sound identified        */
+#define TDA9840_PONRES     0x80 /* Power-on reset detected if = 1 */
+
+#define TDA9840_TEST_INT1SN 0x1 /* Integration time 0.5s when set */
+#define TDA9840_TEST_INTFU 0x02 /* Disables integrator function */
+
+static int tda9840_getrxsubchans(struct CHIPSTATE *chip)
+{
+	struct v4l2_subdev *sd = &chip->sd;
+	int val, mode;
+
+	mode = V4L2_TUNER_SUB_MONO;
+
+	val = chip_read(chip);
+	if (val < 0)
+		return mode;
+
+	if (val & TDA9840_DS_DUAL)
+		mode |= V4L2_TUNER_SUB_LANG1 | V4L2_TUNER_SUB_LANG2;
+	if (val & TDA9840_ST_STEREO)
+		mode = V4L2_TUNER_SUB_STEREO;
+
+	v4l2_dbg(1, debug, sd,
+		"tda9840_getrxsubchans(): raw chip read: %d, return: %d\n",
+		val, mode);
+	return mode;
+}
+
+static void tda9840_setaudmode(struct CHIPSTATE *chip, int mode)
+{
+	int update = 1;
+	int t = chip->shadow.bytes[TDA9840_SW + 1] & ~0x7e;
+
+	switch (mode) {
+	case V4L2_TUNER_MODE_MONO:
+		t |= TDA9840_MONO;
+		break;
+	case V4L2_TUNER_MODE_STEREO:
+		t |= TDA9840_STEREO;
+		break;
+	case V4L2_TUNER_MODE_LANG1:
+		t |= TDA9840_DUALA;
+		break;
+	case V4L2_TUNER_MODE_LANG2:
+		t |= TDA9840_DUALB;
+		break;
+	case V4L2_TUNER_MODE_LANG1_LANG2:
+		t |= TDA9840_DUALAB;
+		break;
+	default:
+		update = 0;
+	}
+
+	if (update)
+		chip_write(chip, TDA9840_SW, t);
+}
+
+static int tda9840_checkit(struct CHIPSTATE *chip)
+{
+	int rc;
+
+	rc = chip_read(chip);
+	if (rc < 0)
+		return 0;
+
+
+	/* lower 5 bits should be 0 */
+	return ((rc & 0x1f) == 0) ? 1 : 0;
+}
+
+/* ---------------------------------------------------------------------- */
+/* audio chip descriptions - defines+functions for tda985x                */
+
+/* subaddresses for TDA9855 */
+#define TDA9855_VR	0x00 /* Volume, right */
+#define TDA9855_VL	0x01 /* Volume, left */
+#define TDA9855_BA	0x02 /* Bass */
+#define TDA9855_TR	0x03 /* Treble */
+#define TDA9855_SW	0x04 /* Subwoofer - not connected on DTV2000 */
+
+/* subaddresses for TDA9850 */
+#define TDA9850_C4	0x04 /* Control 1 for TDA9850 */
+
+/* subaddesses for both chips */
+#define TDA985x_C5	0x05 /* Control 2 for TDA9850, Control 1 for TDA9855 */
+#define TDA985x_C6	0x06 /* Control 3 for TDA9850, Control 2 for TDA9855 */
+#define TDA985x_C7	0x07 /* Control 4 for TDA9850, Control 3 for TDA9855 */
+#define TDA985x_A1	0x08 /* Alignment 1 for both chips */
+#define TDA985x_A2	0x09 /* Alignment 2 for both chips */
+#define TDA985x_A3	0x0a /* Alignment 3 for both chips */
+
+/* Masks for bits in TDA9855 subaddresses */
+/* 0x00 - VR in TDA9855 */
+/* 0x01 - VL in TDA9855 */
+/* lower 7 bits control gain from -71dB (0x28) to 16dB (0x7f)
+ * in 1dB steps - mute is 0x27 */
+
+
+/* 0x02 - BA in TDA9855 */
+/* lower 5 bits control bass gain from -12dB (0x06) to 16.5dB (0x19)
+ * in .5dB steps - 0 is 0x0E */
+
+
+/* 0x03 - TR in TDA9855 */
+/* 4 bits << 1 control treble gain from -12dB (0x3) to 12dB (0xb)
+ * in 3dB steps - 0 is 0x7 */
+
+/* Masks for bits in both chips' subaddresses */
+/* 0x04 - SW in TDA9855, C4/Control 1 in TDA9850 */
+/* Unique to TDA9855: */
+/* 4 bits << 2 control subwoofer/surround gain from -14db (0x1) to 14db (0xf)
+ * in 3dB steps - mute is 0x0 */
+
+/* Unique to TDA9850: */
+/* lower 4 bits control stereo noise threshold, over which stereo turns off
+ * set to values of 0x00 through 0x0f for Ster1 through Ster16 */
+
+
+/* 0x05 - C5 - Control 1 in TDA9855 , Control 2 in TDA9850*/
+/* Unique to TDA9855: */
+#define TDA9855_MUTE	1<<7 /* GMU, Mute at outputs */
+#define TDA9855_AVL	1<<6 /* AVL, Automatic Volume Level */
+#define TDA9855_LOUD	1<<5 /* Loudness, 1==off */
+#define TDA9855_SUR	1<<3 /* Surround / Subwoofer 1==.5(L-R) 0==.5(L+R) */
+			     /* Bits 0 to 3 select various combinations
+			      * of line in and line out, only the
+			      * interesting ones are defined */
+#define TDA9855_EXT	1<<2 /* Selects inputs LIR and LIL.  Pins 41 & 12 */
+#define TDA9855_INT	0    /* Selects inputs LOR and LOL.  (internal) */
+
+/* Unique to TDA9850:  */
+/* lower 4 bits control SAP noise threshold, over which SAP turns off
+ * set to values of 0x00 through 0x0f for SAP1 through SAP16 */
+
+
+/* 0x06 - C6 - Control 2 in TDA9855, Control 3 in TDA9850 */
+/* Common to TDA9855 and TDA9850: */
+#define TDA985x_SAP	3<<6 /* Selects SAP output, mute if not received */
+#define TDA985x_MONOSAP	2<<6 /* Selects Mono on left, SAP on right */
+#define TDA985x_STEREO	1<<6 /* Selects Stereo output, mono if not received */
+#define TDA985x_MONO	0    /* Forces Mono output */
+#define TDA985x_LMU	1<<3 /* Mute (LOR/LOL for 9855, OUTL/OUTR for 9850) */
+
+/* Unique to TDA9855: */
+#define TDA9855_TZCM	1<<5 /* If set, don't mute till zero crossing */
+#define TDA9855_VZCM	1<<4 /* If set, don't change volume till zero crossing*/
+#define TDA9855_LINEAR	0    /* Linear Stereo */
+#define TDA9855_PSEUDO	1    /* Pseudo Stereo */
+#define TDA9855_SPAT_30	2    /* Spatial Stereo, 30% anti-phase crosstalk */
+#define TDA9855_SPAT_50	3    /* Spatial Stereo, 52% anti-phase crosstalk */
+#define TDA9855_E_MONO	7    /* Forced mono - mono select elseware, so useless*/
+
+/* 0x07 - C7 - Control 3 in TDA9855, Control 4 in TDA9850 */
+/* Common to both TDA9855 and TDA9850: */
+/* lower 4 bits control input gain from -3.5dB (0x0) to 4dB (0xF)
+ * in .5dB steps -  0dB is 0x7 */
+
+/* 0x08, 0x09 - A1 and A2 (read/write) */
+/* Common to both TDA9855 and TDA9850: */
+/* lower 5 bites are wideband and spectral expander alignment
+ * from 0x00 to 0x1f - nominal at 0x0f and 0x10 (read/write) */
+#define TDA985x_STP	1<<5 /* Stereo Pilot/detect (read-only) */
+#define TDA985x_SAPP	1<<6 /* SAP Pilot/detect (read-only) */
+#define TDA985x_STS	1<<7 /* Stereo trigger 1= <35mV 0= <30mV (write-only)*/
+
+/* 0x0a - A3 */
+/* Common to both TDA9855 and TDA9850: */
+/* lower 3 bits control timing current for alignment: -30% (0x0), -20% (0x1),
+ * -10% (0x2), nominal (0x3), +10% (0x6), +20% (0x5), +30% (0x4) */
+#define TDA985x_ADJ	1<<7 /* Stereo adjust on/off (wideband and spectral */
+
+static int tda9855_volume(int val) { return val/0x2e8+0x27; }
+static int tda9855_bass(int val)   { return val/0xccc+0x06; }
+static int tda9855_treble(int val) { return (val/0x1c71+0x3)<<1; }
+
+static int  tda985x_getrxsubchans(struct CHIPSTATE *chip)
+{
+	int mode, val;
+
+	/* Add mono mode regardless of SAP and stereo */
+	/* Allows forced mono */
+	mode = V4L2_TUNER_SUB_MONO;
+	val = chip_read(chip);
+	if (val < 0)
+		return mode;
+
+	if (val & TDA985x_STP)
+		mode = V4L2_TUNER_SUB_STEREO;
+	if (val & TDA985x_SAPP)
+		mode |= V4L2_TUNER_SUB_SAP;
+	return mode;
+}
+
+static void tda985x_setaudmode(struct CHIPSTATE *chip, int mode)
+{
+	int update = 1;
+	int c6 = chip->shadow.bytes[TDA985x_C6+1] & 0x3f;
+
+	switch (mode) {
+	case V4L2_TUNER_MODE_MONO:
+		c6 |= TDA985x_MONO;
+		break;
+	case V4L2_TUNER_MODE_STEREO:
+	case V4L2_TUNER_MODE_LANG1:
+		c6 |= TDA985x_STEREO;
+		break;
+	case V4L2_TUNER_MODE_SAP:
+		c6 |= TDA985x_SAP;
+		break;
+	case V4L2_TUNER_MODE_LANG1_LANG2:
+		c6 |= TDA985x_MONOSAP;
+		break;
+	default:
+		update = 0;
+	}
+	if (update)
+		chip_write(chip,TDA985x_C6,c6);
+}
+
+
+/* ---------------------------------------------------------------------- */
+/* audio chip descriptions - defines+functions for tda9873h               */
+
+/* Subaddresses for TDA9873H */
+
+#define TDA9873_SW	0x00 /* Switching                    */
+#define TDA9873_AD	0x01 /* Adjust                       */
+#define TDA9873_PT	0x02 /* Port                         */
+
+/* Subaddress 0x00: Switching Data
+ * B7..B0:
+ *
+ * B1, B0: Input source selection
+ *  0,  0  internal
+ *  1,  0  external stereo
+ *  0,  1  external mono
+ */
+#define TDA9873_INP_MASK    3
+#define TDA9873_INTERNAL    0
+#define TDA9873_EXT_STEREO  2
+#define TDA9873_EXT_MONO    1
+
+/*    B3, B2: output signal select
+ * B4    : transmission mode
+ *  0, 0, 1   Mono
+ *  1, 0, 0   Stereo
+ *  1, 1, 1   Stereo (reversed channel)
+ *  0, 0, 0   Dual AB
+ *  0, 0, 1   Dual AA
+ *  0, 1, 0   Dual BB
+ *  0, 1, 1   Dual BA
+ */
+
+#define TDA9873_TR_MASK     (7 << 2)
+#define TDA9873_TR_MONO     4
+#define TDA9873_TR_STEREO   1 << 4
+#define TDA9873_TR_REVERSE  ((1 << 3) | (1 << 2))
+#define TDA9873_TR_DUALA    1 << 2
+#define TDA9873_TR_DUALB    1 << 3
+#define TDA9873_TR_DUALAB   0
+
+/* output level controls
+ * B5:  output level switch (0 = reduced gain, 1 = normal gain)
+ * B6:  mute                (1 = muted)
+ * B7:  auto-mute           (1 = auto-mute enabled)
+ */
+
+#define TDA9873_GAIN_NORMAL 1 << 5
+#define TDA9873_MUTE        1 << 6
+#define TDA9873_AUTOMUTE    1 << 7
+
+/* Subaddress 0x01:  Adjust/standard */
+
+/* Lower 4 bits (C3..C0) control stereo adjustment on R channel (-0.6 - +0.7 dB)
+ * Recommended value is +0 dB
+ */
+
+#define	TDA9873_STEREO_ADJ	0x06 /* 0dB gain */
+
+/* Bits C6..C4 control FM stantard
+ * C6, C5, C4
+ *  0,  0,  0   B/G (PAL FM)
+ *  0,  0,  1   M
+ *  0,  1,  0   D/K(1)
+ *  0,  1,  1   D/K(2)
+ *  1,  0,  0   D/K(3)
+ *  1,  0,  1   I
+ */
+#define TDA9873_BG		0
+#define TDA9873_M       1
+#define TDA9873_DK1     2
+#define TDA9873_DK2     3
+#define TDA9873_DK3     4
+#define TDA9873_I       5
+
+/* C7 controls identification response time (1=fast/0=normal)
+ */
+#define TDA9873_IDR_NORM 0
+#define TDA9873_IDR_FAST 1 << 7
+
+
+/* Subaddress 0x02: Port data */
+
+/* E1, E0   free programmable ports P1/P2
+    0,  0   both ports low
+    0,  1   P1 high
+    1,  0   P2 high
+    1,  1   both ports high
+*/
+
+#define TDA9873_PORTS    3
+
+/* E2: test port */
+#define TDA9873_TST_PORT 1 << 2
+
+/* E5..E3 control mono output channel (together with transmission mode bit B4)
+ *
+ * E5 E4 E3 B4     OUTM
+ *  0  0  0  0     mono
+ *  0  0  1  0     DUAL B
+ *  0  1  0  1     mono (from stereo decoder)
+ */
+#define TDA9873_MOUT_MONO   0
+#define TDA9873_MOUT_FMONO  0
+#define TDA9873_MOUT_DUALA  0
+#define TDA9873_MOUT_DUALB  1 << 3
+#define TDA9873_MOUT_ST     1 << 4
+#define TDA9873_MOUT_EXTM   ((1 << 4) | (1 << 3))
+#define TDA9873_MOUT_EXTL   1 << 5
+#define TDA9873_MOUT_EXTR   ((1 << 5) | (1 << 3))
+#define TDA9873_MOUT_EXTLR  ((1 << 5) | (1 << 4))
+#define TDA9873_MOUT_MUTE   ((1 << 5) | (1 << 4) | (1 << 3))
+
+/* Status bits: (chip read) */
+#define TDA9873_PONR        0 /* Power-on reset detected if = 1 */
+#define TDA9873_STEREO      2 /* Stereo sound is identified     */
+#define TDA9873_DUAL        4 /* Dual sound is identified       */
+
+static int tda9873_getrxsubchans(struct CHIPSTATE *chip)
+{
+	struct v4l2_subdev *sd = &chip->sd;
+	int val,mode;
+
+	mode = V4L2_TUNER_SUB_MONO;
+
+	val = chip_read(chip);
+	if (val < 0)
+		return mode;
+
+	if (val & TDA9873_STEREO)
+		mode = V4L2_TUNER_SUB_STEREO;
+	if (val & TDA9873_DUAL)
+		mode |= V4L2_TUNER_SUB_LANG1 | V4L2_TUNER_SUB_LANG2;
+	v4l2_dbg(1, debug, sd,
+		"tda9873_getrxsubchans(): raw chip read: %d, return: %d\n",
+		val, mode);
+	return mode;
+}
+
+static void tda9873_setaudmode(struct CHIPSTATE *chip, int mode)
+{
+	struct v4l2_subdev *sd = &chip->sd;
+	int sw_data  = chip->shadow.bytes[TDA9873_SW+1] & ~ TDA9873_TR_MASK;
+	/*	int adj_data = chip->shadow.bytes[TDA9873_AD+1] ; */
+
+	if ((sw_data & TDA9873_INP_MASK) != TDA9873_INTERNAL) {
+		v4l2_dbg(1, debug, sd,
+			 "tda9873_setaudmode(): external input\n");
+		return;
+	}
+
+	v4l2_dbg(1, debug, sd,
+		 "tda9873_setaudmode(): chip->shadow.bytes[%d] = %d\n",
+		 TDA9873_SW+1, chip->shadow.bytes[TDA9873_SW+1]);
+	v4l2_dbg(1, debug, sd, "tda9873_setaudmode(): sw_data  = %d\n",
+		 sw_data);
+
+	switch (mode) {
+	case V4L2_TUNER_MODE_MONO:
+		sw_data |= TDA9873_TR_MONO;
+		break;
+	case V4L2_TUNER_MODE_STEREO:
+		sw_data |= TDA9873_TR_STEREO;
+		break;
+	case V4L2_TUNER_MODE_LANG1:
+		sw_data |= TDA9873_TR_DUALA;
+		break;
+	case V4L2_TUNER_MODE_LANG2:
+		sw_data |= TDA9873_TR_DUALB;
+		break;
+	case V4L2_TUNER_MODE_LANG1_LANG2:
+		sw_data |= TDA9873_TR_DUALAB;
+		break;
+	default:
+		return;
+	}
+
+	chip_write(chip, TDA9873_SW, sw_data);
+	v4l2_dbg(1, debug, sd,
+		"tda9873_setaudmode(): req. mode %d; chip_write: %d\n",
+		mode, sw_data);
+}
+
+static int tda9873_checkit(struct CHIPSTATE *chip)
+{
+	int rc;
+
+	rc = chip_read2(chip, 254);
+	if (rc < 0)
+		return 0;
+	return (rc & ~0x1f) == 0x80;
+}
+
+
+/* ---------------------------------------------------------------------- */
+/* audio chip description - defines+functions for tda9874h and tda9874a   */
+/* Dariusz Kowalewski <darekk@automex.pl>                                 */
+
+/* Subaddresses for TDA9874H and TDA9874A (slave rx) */
+#define TDA9874A_AGCGR		0x00	/* AGC gain */
+#define TDA9874A_GCONR		0x01	/* general config */
+#define TDA9874A_MSR		0x02	/* monitor select */
+#define TDA9874A_C1FRA		0x03	/* carrier 1 freq. */
+#define TDA9874A_C1FRB		0x04	/* carrier 1 freq. */
+#define TDA9874A_C1FRC		0x05	/* carrier 1 freq. */
+#define TDA9874A_C2FRA		0x06	/* carrier 2 freq. */
+#define TDA9874A_C2FRB		0x07	/* carrier 2 freq. */
+#define TDA9874A_C2FRC		0x08	/* carrier 2 freq. */
+#define TDA9874A_DCR		0x09	/* demodulator config */
+#define TDA9874A_FMER		0x0a	/* FM de-emphasis */
+#define TDA9874A_FMMR		0x0b	/* FM dematrix */
+#define TDA9874A_C1OLAR		0x0c	/* ch.1 output level adj. */
+#define TDA9874A_C2OLAR		0x0d	/* ch.2 output level adj. */
+#define TDA9874A_NCONR		0x0e	/* NICAM config */
+#define TDA9874A_NOLAR		0x0f	/* NICAM output level adj. */
+#define TDA9874A_NLELR		0x10	/* NICAM lower error limit */
+#define TDA9874A_NUELR		0x11	/* NICAM upper error limit */
+#define TDA9874A_AMCONR		0x12	/* audio mute control */
+#define TDA9874A_SDACOSR	0x13	/* stereo DAC output select */
+#define TDA9874A_AOSR		0x14	/* analog output select */
+#define TDA9874A_DAICONR	0x15	/* digital audio interface config */
+#define TDA9874A_I2SOSR		0x16	/* I2S-bus output select */
+#define TDA9874A_I2SOLAR	0x17	/* I2S-bus output level adj. */
+#define TDA9874A_MDACOSR	0x18	/* mono DAC output select (tda9874a) */
+#define TDA9874A_ESP		0xFF	/* easy standard progr. (tda9874a) */
+
+/* Subaddresses for TDA9874H and TDA9874A (slave tx) */
+#define TDA9874A_DSR		0x00	/* device status */
+#define TDA9874A_NSR		0x01	/* NICAM status */
+#define TDA9874A_NECR		0x02	/* NICAM error count */
+#define TDA9874A_DR1		0x03	/* add. data LSB */
+#define TDA9874A_DR2		0x04	/* add. data MSB */
+#define TDA9874A_LLRA		0x05	/* monitor level read-out LSB */
+#define TDA9874A_LLRB		0x06	/* monitor level read-out MSB */
+#define TDA9874A_SIFLR		0x07	/* SIF level */
+#define TDA9874A_TR2		252	/* test reg. 2 */
+#define TDA9874A_TR1		253	/* test reg. 1 */
+#define TDA9874A_DIC		254	/* device id. code */
+#define TDA9874A_SIC		255	/* software id. code */
+
+
+static int tda9874a_mode = 1;		/* 0: A2, 1: NICAM */
+static int tda9874a_GCONR = 0xc0;	/* default config. input pin: SIFSEL=0 */
+static int tda9874a_NCONR = 0x01;	/* default NICAM config.: AMSEL=0,AMUTE=1 */
+static int tda9874a_ESP = 0x07;		/* default standard: NICAM D/K */
+static int tda9874a_dic = -1;		/* device id. code */
+
+/* insmod options for tda9874a */
+static unsigned int tda9874a_SIF   = UNSET;
+static unsigned int tda9874a_AMSEL = UNSET;
+static unsigned int tda9874a_STD   = UNSET;
+module_param(tda9874a_SIF, int, 0444);
+module_param(tda9874a_AMSEL, int, 0444);
+module_param(tda9874a_STD, int, 0444);
+
+/*
+ * initialization table for tda9874 decoder:
+ *  - carrier 1 freq. registers (3 bytes)
+ *  - carrier 2 freq. registers (3 bytes)
+ *  - demudulator config register
+ *  - FM de-emphasis register (slow identification mode)
+ * Note: frequency registers must be written in single i2c transfer.
+ */
+static struct tda9874a_MODES {
+	char *name;
+	audiocmd cmd;
+} tda9874a_modelist[9] = {
+  {	"A2, B/G", /* default */
+	{ 9, { TDA9874A_C1FRA, 0x72,0x95,0x55, 0x77,0xA0,0x00, 0x00,0x00 }} },
+  {	"A2, M (Korea)",
+	{ 9, { TDA9874A_C1FRA, 0x5D,0xC0,0x00, 0x62,0x6A,0xAA, 0x20,0x22 }} },
+  {	"A2, D/K (1)",
+	{ 9, { TDA9874A_C1FRA, 0x87,0x6A,0xAA, 0x82,0x60,0x00, 0x00,0x00 }} },
+  {	"A2, D/K (2)",
+	{ 9, { TDA9874A_C1FRA, 0x87,0x6A,0xAA, 0x8C,0x75,0x55, 0x00,0x00 }} },
+  {	"A2, D/K (3)",
+	{ 9, { TDA9874A_C1FRA, 0x87,0x6A,0xAA, 0x77,0xA0,0x00, 0x00,0x00 }} },
+  {	"NICAM, I",
+	{ 9, { TDA9874A_C1FRA, 0x7D,0x00,0x00, 0x88,0x8A,0xAA, 0x08,0x33 }} },
+  {	"NICAM, B/G",
+	{ 9, { TDA9874A_C1FRA, 0x72,0x95,0x55, 0x79,0xEA,0xAA, 0x08,0x33 }} },
+  {	"NICAM, D/K",
+	{ 9, { TDA9874A_C1FRA, 0x87,0x6A,0xAA, 0x79,0xEA,0xAA, 0x08,0x33 }} },
+  {	"NICAM, L",
+	{ 9, { TDA9874A_C1FRA, 0x87,0x6A,0xAA, 0x79,0xEA,0xAA, 0x09,0x33 }} }
+};
+
+static int tda9874a_setup(struct CHIPSTATE *chip)
+{
+	struct v4l2_subdev *sd = &chip->sd;
+
+	chip_write(chip, TDA9874A_AGCGR, 0x00); /* 0 dB */
+	chip_write(chip, TDA9874A_GCONR, tda9874a_GCONR);
+	chip_write(chip, TDA9874A_MSR, (tda9874a_mode) ? 0x03:0x02);
+	if(tda9874a_dic == 0x11) {
+		chip_write(chip, TDA9874A_FMMR, 0x80);
+	} else { /* dic == 0x07 */
+		chip_cmd(chip,"tda9874_modelist",&tda9874a_modelist[tda9874a_STD].cmd);
+		chip_write(chip, TDA9874A_FMMR, 0x00);
+	}
+	chip_write(chip, TDA9874A_C1OLAR, 0x00); /* 0 dB */
+	chip_write(chip, TDA9874A_C2OLAR, 0x00); /* 0 dB */
+	chip_write(chip, TDA9874A_NCONR, tda9874a_NCONR);
+	chip_write(chip, TDA9874A_NOLAR, 0x00); /* 0 dB */
+	/* Note: If signal quality is poor you may want to change NICAM */
+	/* error limit registers (NLELR and NUELR) to some greater values. */
+	/* Then the sound would remain stereo, but won't be so clear. */
+	chip_write(chip, TDA9874A_NLELR, 0x14); /* default */
+	chip_write(chip, TDA9874A_NUELR, 0x50); /* default */
+
+	if(tda9874a_dic == 0x11) {
+		chip_write(chip, TDA9874A_AMCONR, 0xf9);
+		chip_write(chip, TDA9874A_SDACOSR, (tda9874a_mode) ? 0x81:0x80);
+		chip_write(chip, TDA9874A_AOSR, 0x80);
+		chip_write(chip, TDA9874A_MDACOSR, (tda9874a_mode) ? 0x82:0x80);
+		chip_write(chip, TDA9874A_ESP, tda9874a_ESP);
+	} else { /* dic == 0x07 */
+		chip_write(chip, TDA9874A_AMCONR, 0xfb);
+		chip_write(chip, TDA9874A_SDACOSR, (tda9874a_mode) ? 0x81:0x80);
+		chip_write(chip, TDA9874A_AOSR, 0x00); /* or 0x10 */
+	}
+	v4l2_dbg(1, debug, sd, "tda9874a_setup(): %s [0x%02X].\n",
+		tda9874a_modelist[tda9874a_STD].name,tda9874a_STD);
+	return 1;
+}
+
+static int tda9874a_getrxsubchans(struct CHIPSTATE *chip)
+{
+	struct v4l2_subdev *sd = &chip->sd;
+	int dsr,nsr,mode;
+	int necr; /* just for debugging */
+
+	mode = V4L2_TUNER_SUB_MONO;
+
+	dsr = chip_read2(chip, TDA9874A_DSR);
+	if (dsr < 0)
+		return mode;
+	nsr = chip_read2(chip, TDA9874A_NSR);
+	if (nsr < 0)
+		return mode;
+	necr = chip_read2(chip, TDA9874A_NECR);
+	if (necr < 0)
+		return mode;
+
+	/* need to store dsr/nsr somewhere */
+	chip->shadow.bytes[MAXREGS-2] = dsr;
+	chip->shadow.bytes[MAXREGS-1] = nsr;
+
+	if(tda9874a_mode) {
+		/* Note: DSR.RSSF and DSR.AMSTAT bits are also checked.
+		 * If NICAM auto-muting is enabled, DSR.AMSTAT=1 indicates
+		 * that sound has (temporarily) switched from NICAM to
+		 * mono FM (or AM) on 1st sound carrier due to high NICAM bit
+		 * error count. So in fact there is no stereo in this case :-(
+		 * But changing the mode to V4L2_TUNER_MODE_MONO would switch
+		 * external 4052 multiplexer in audio_hook().
+		 */
+		if(nsr & 0x02) /* NSR.S/MB=1 */
+			mode = V4L2_TUNER_SUB_STEREO;
+		if(nsr & 0x01) /* NSR.D/SB=1 */
+			mode |= V4L2_TUNER_SUB_LANG1 | V4L2_TUNER_SUB_LANG2;
+	} else {
+		if(dsr & 0x02) /* DSR.IDSTE=1 */
+			mode = V4L2_TUNER_SUB_STEREO;
+		if(dsr & 0x04) /* DSR.IDDUA=1 */
+			mode |= V4L2_TUNER_SUB_LANG1 | V4L2_TUNER_SUB_LANG2;
+	}
+
+	v4l2_dbg(1, debug, sd,
+		 "tda9874a_getrxsubchans(): DSR=0x%X, NSR=0x%X, NECR=0x%X, return: %d.\n",
+		 dsr, nsr, necr, mode);
+	return mode;
+}
+
+static void tda9874a_setaudmode(struct CHIPSTATE *chip, int mode)
+{
+	struct v4l2_subdev *sd = &chip->sd;
+
+	/* Disable/enable NICAM auto-muting (based on DSR.RSSF status bit). */
+	/* If auto-muting is disabled, we can hear a signal of degrading quality. */
+	if (tda9874a_mode) {
+		if(chip->shadow.bytes[MAXREGS-2] & 0x20) /* DSR.RSSF=1 */
+			tda9874a_NCONR &= 0xfe; /* enable */
+		else
+			tda9874a_NCONR |= 0x01; /* disable */
+		chip_write(chip, TDA9874A_NCONR, tda9874a_NCONR);
+	}
+
+	/* Note: TDA9874A supports automatic FM dematrixing (FMMR register)
+	 * and has auto-select function for audio output (AOSR register).
+	 * Old TDA9874H doesn't support these features.
+	 * TDA9874A also has additional mono output pin (OUTM), which
+	 * on same (all?) tv-cards is not used, anyway (as well as MONOIN).
+	 */
+	if(tda9874a_dic == 0x11) {
+		int aosr = 0x80;
+		int mdacosr = (tda9874a_mode) ? 0x82:0x80;
+
+		switch(mode) {
+		case V4L2_TUNER_MODE_MONO:
+		case V4L2_TUNER_MODE_STEREO:
+			break;
+		case V4L2_TUNER_MODE_LANG1:
+			aosr = 0x80; /* auto-select, dual A/A */
+			mdacosr = (tda9874a_mode) ? 0x82:0x80;
+			break;
+		case V4L2_TUNER_MODE_LANG2:
+			aosr = 0xa0; /* auto-select, dual B/B */
+			mdacosr = (tda9874a_mode) ? 0x83:0x81;
+			break;
+		case V4L2_TUNER_MODE_LANG1_LANG2:
+			aosr = 0x00; /* always route L to L and R to R */
+			mdacosr = (tda9874a_mode) ? 0x82:0x80;
+			break;
+		default:
+			return;
+		}
+		chip_write(chip, TDA9874A_AOSR, aosr);
+		chip_write(chip, TDA9874A_MDACOSR, mdacosr);
+
+		v4l2_dbg(1, debug, sd,
+			"tda9874a_setaudmode(): req. mode %d; AOSR=0x%X, MDACOSR=0x%X.\n",
+			mode, aosr, mdacosr);
+
+	} else { /* dic == 0x07 */
+		int fmmr,aosr;
+
+		switch(mode) {
+		case V4L2_TUNER_MODE_MONO:
+			fmmr = 0x00; /* mono */
+			aosr = 0x10; /* A/A */
+			break;
+		case V4L2_TUNER_MODE_STEREO:
+			if(tda9874a_mode) {
+				fmmr = 0x00;
+				aosr = 0x00; /* handled by NICAM auto-mute */
+			} else {
+				fmmr = (tda9874a_ESP == 1) ? 0x05 : 0x04; /* stereo */
+				aosr = 0x00;
+			}
+			break;
+		case V4L2_TUNER_MODE_LANG1:
+			fmmr = 0x02; /* dual */
+			aosr = 0x10; /* dual A/A */
+			break;
+		case V4L2_TUNER_MODE_LANG2:
+			fmmr = 0x02; /* dual */
+			aosr = 0x20; /* dual B/B */
+			break;
+		case V4L2_TUNER_MODE_LANG1_LANG2:
+			fmmr = 0x02; /* dual */
+			aosr = 0x00; /* dual A/B */
+			break;
+		default:
+			return;
+		}
+		chip_write(chip, TDA9874A_FMMR, fmmr);
+		chip_write(chip, TDA9874A_AOSR, aosr);
+
+		v4l2_dbg(1, debug, sd,
+			"tda9874a_setaudmode(): req. mode %d; FMMR=0x%X, AOSR=0x%X.\n",
+			mode, fmmr, aosr);
+	}
+}
+
+static int tda9874a_checkit(struct CHIPSTATE *chip)
+{
+	struct v4l2_subdev *sd = &chip->sd;
+	int dic,sic;	/* device id. and software id. codes */
+
+	dic = chip_read2(chip, TDA9874A_DIC);
+	if (dic < 0)
+		return 0;
+	sic = chip_read2(chip, TDA9874A_SIC);
+	if (sic < 0)
+		return 0;
+
+	v4l2_dbg(1, debug, sd, "tda9874a_checkit(): DIC=0x%X, SIC=0x%X.\n", dic, sic);
+
+	if((dic == 0x11)||(dic == 0x07)) {
+		v4l2_info(sd, "found tda9874%s.\n", (dic == 0x11) ? "a" : "h");
+		tda9874a_dic = dic;	/* remember device id. */
+		return 1;
+	}
+	return 0;	/* not found */
+}
+
+static int tda9874a_initialize(struct CHIPSTATE *chip)
+{
+	if (tda9874a_SIF > 2)
+		tda9874a_SIF = 1;
+	if (tda9874a_STD >= ARRAY_SIZE(tda9874a_modelist))
+		tda9874a_STD = 0;
+	if(tda9874a_AMSEL > 1)
+		tda9874a_AMSEL = 0;
+
+	if(tda9874a_SIF == 1)
+		tda9874a_GCONR = 0xc0;	/* sound IF input 1 */
+	else
+		tda9874a_GCONR = 0xc1;	/* sound IF input 2 */
+
+	tda9874a_ESP = tda9874a_STD;
+	tda9874a_mode = (tda9874a_STD < 5) ? 0 : 1;
+
+	if(tda9874a_AMSEL == 0)
+		tda9874a_NCONR = 0x01; /* auto-mute: analog mono input */
+	else
+		tda9874a_NCONR = 0x05; /* auto-mute: 1st carrier FM or AM */
+
+	tda9874a_setup(chip);
+	return 0;
+}
+
+/* ---------------------------------------------------------------------- */
+/* audio chip description - defines+functions for tda9875                 */
+/* The TDA9875 is made by Philips Semiconductor
+ * http://www.semiconductors.philips.com
+ * TDA9875: I2C-bus controlled DSP audio processor, FM demodulator
+ *
+ */
+
+/* subaddresses for TDA9875 */
+#define TDA9875_MUT         0x12  /*General mute  (value --> 0b11001100*/
+#define TDA9875_CFG         0x01  /* Config register (value --> 0b00000000 */
+#define TDA9875_DACOS       0x13  /*DAC i/o select (ADC) 0b0000100*/
+#define TDA9875_LOSR        0x16  /*Line output select regirter 0b0100 0001*/
+
+#define TDA9875_CH1V        0x0c  /*Channel 1 volume (mute)*/
+#define TDA9875_CH2V        0x0d  /*Channel 2 volume (mute)*/
+#define TDA9875_SC1         0x14  /*SCART 1 in (mono)*/
+#define TDA9875_SC2         0x15  /*SCART 2 in (mono)*/
+
+#define TDA9875_ADCIS       0x17  /*ADC input select (mono) 0b0110 000*/
+#define TDA9875_AER         0x19  /*Audio effect (AVL+Pseudo) 0b0000 0110*/
+#define TDA9875_MCS         0x18  /*Main channel select (DAC) 0b0000100*/
+#define TDA9875_MVL         0x1a  /* Main volume gauche */
+#define TDA9875_MVR         0x1b  /* Main volume droite */
+#define TDA9875_MBA         0x1d  /* Main Basse */
+#define TDA9875_MTR         0x1e  /* Main treble */
+#define TDA9875_ACS         0x1f  /* Auxiliary channel select (FM) 0b0000000*/
+#define TDA9875_AVL         0x20  /* Auxiliary volume gauche */
+#define TDA9875_AVR         0x21  /* Auxiliary volume droite */
+#define TDA9875_ABA         0x22  /* Auxiliary Basse */
+#define TDA9875_ATR         0x23  /* Auxiliary treble */
+
+#define TDA9875_MSR         0x02  /* Monitor select register */
+#define TDA9875_C1MSB       0x03  /* Carrier 1 (FM) frequency register MSB */
+#define TDA9875_C1MIB       0x04  /* Carrier 1 (FM) frequency register (16-8]b */
+#define TDA9875_C1LSB       0x05  /* Carrier 1 (FM) frequency register LSB */
+#define TDA9875_C2MSB       0x06  /* Carrier 2 (nicam) frequency register MSB */
+#define TDA9875_C2MIB       0x07  /* Carrier 2 (nicam) frequency register (16-8]b */
+#define TDA9875_C2LSB       0x08  /* Carrier 2 (nicam) frequency register LSB */
+#define TDA9875_DCR         0x09  /* Demodulateur configuration regirter*/
+#define TDA9875_DEEM        0x0a  /* FM de-emphasis regirter*/
+#define TDA9875_FMAT        0x0b  /* FM Matrix regirter*/
+
+/* values */
+#define TDA9875_MUTE_ON	    0xff /* general mute */
+#define TDA9875_MUTE_OFF    0xcc /* general no mute */
+
+static int tda9875_initialize(struct CHIPSTATE *chip)
+{
+	chip_write(chip, TDA9875_CFG, 0xd0); /*reg de config 0 (reset)*/
+	chip_write(chip, TDA9875_MSR, 0x03);    /* Monitor 0b00000XXX*/
+	chip_write(chip, TDA9875_C1MSB, 0x00);  /*Car1(FM) MSB XMHz*/
+	chip_write(chip, TDA9875_C1MIB, 0x00);  /*Car1(FM) MIB XMHz*/
+	chip_write(chip, TDA9875_C1LSB, 0x00);  /*Car1(FM) LSB XMHz*/
+	chip_write(chip, TDA9875_C2MSB, 0x00);  /*Car2(NICAM) MSB XMHz*/
+	chip_write(chip, TDA9875_C2MIB, 0x00);  /*Car2(NICAM) MIB XMHz*/
+	chip_write(chip, TDA9875_C2LSB, 0x00);  /*Car2(NICAM) LSB XMHz*/
+	chip_write(chip, TDA9875_DCR, 0x00);    /*Demod config 0x00*/
+	chip_write(chip, TDA9875_DEEM, 0x44);   /*DE-Emph 0b0100 0100*/
+	chip_write(chip, TDA9875_FMAT, 0x00);   /*FM Matrix reg 0x00*/
+	chip_write(chip, TDA9875_SC1, 0x00);    /* SCART 1 (SC1)*/
+	chip_write(chip, TDA9875_SC2, 0x01);    /* SCART 2 (sc2)*/
+
+	chip_write(chip, TDA9875_CH1V, 0x10);  /* Channel volume 1 mute*/
+	chip_write(chip, TDA9875_CH2V, 0x10);  /* Channel volume 2 mute */
+	chip_write(chip, TDA9875_DACOS, 0x02); /* sig DAC i/o(in:nicam)*/
+	chip_write(chip, TDA9875_ADCIS, 0x6f); /* sig ADC input(in:mono)*/
+	chip_write(chip, TDA9875_LOSR, 0x00);  /* line out (in:mono)*/
+	chip_write(chip, TDA9875_AER, 0x00);   /*06 Effect (AVL+PSEUDO) */
+	chip_write(chip, TDA9875_MCS, 0x44);   /* Main ch select (DAC) */
+	chip_write(chip, TDA9875_MVL, 0x03);   /* Vol Main left 10dB */
+	chip_write(chip, TDA9875_MVR, 0x03);   /* Vol Main right 10dB*/
+	chip_write(chip, TDA9875_MBA, 0x00);   /* Main Bass Main 0dB*/
+	chip_write(chip, TDA9875_MTR, 0x00);   /* Main Treble Main 0dB*/
+	chip_write(chip, TDA9875_ACS, 0x44);   /* Aux chan select (dac)*/
+	chip_write(chip, TDA9875_AVL, 0x00);   /* Vol Aux left 0dB*/
+	chip_write(chip, TDA9875_AVR, 0x00);   /* Vol Aux right 0dB*/
+	chip_write(chip, TDA9875_ABA, 0x00);   /* Aux Bass Main 0dB*/
+	chip_write(chip, TDA9875_ATR, 0x00);   /* Aux Aigus Main 0dB*/
+
+	chip_write(chip, TDA9875_MUT, 0xcc);   /* General mute  */
+	return 0;
+}
+
+static int tda9875_volume(int val) { return (unsigned char)(val / 602 - 84); }
+static int tda9875_bass(int val) { return (unsigned char)(max(-12, val / 2115 - 15)); }
+static int tda9875_treble(int val) { return (unsigned char)(val / 2622 - 12); }
+
+/* ----------------------------------------------------------------------- */
+
+
+/* *********************** *
+ * i2c interface functions *
+ * *********************** */
+
+static int tda9875_checkit(struct CHIPSTATE *chip)
+{
+	struct v4l2_subdev *sd = &chip->sd;
+	int dic, rev;
+
+	dic = chip_read2(chip, 254);
+	if (dic < 0)
+		return 0;
+	rev = chip_read2(chip, 255);
+	if (rev < 0)
+		return 0;
+
+	if (dic == 0 || dic == 2) { /* tda9875 and tda9875A */
+		v4l2_info(sd, "found tda9875%s rev. %d.\n",
+			dic == 0 ? "" : "A", rev);
+		return 1;
+	}
+	return 0;
+}
+
+/* ---------------------------------------------------------------------- */
+/* audio chip descriptions - defines+functions for tea6420                */
+
+#define TEA6300_VL         0x00  /* volume left */
+#define TEA6300_VR         0x01  /* volume right */
+#define TEA6300_BA         0x02  /* bass */
+#define TEA6300_TR         0x03  /* treble */
+#define TEA6300_FA         0x04  /* fader control */
+#define TEA6300_S          0x05  /* switch register */
+				 /* values for those registers: */
+#define TEA6300_S_SA       0x01  /* stereo A input */
+#define TEA6300_S_SB       0x02  /* stereo B */
+#define TEA6300_S_SC       0x04  /* stereo C */
+#define TEA6300_S_GMU      0x80  /* general mute */
+
+#define TEA6320_V          0x00  /* volume (0-5)/loudness off (6)/zero crossing mute(7) */
+#define TEA6320_FFR        0x01  /* fader front right (0-5) */
+#define TEA6320_FFL        0x02  /* fader front left (0-5) */
+#define TEA6320_FRR        0x03  /* fader rear right (0-5) */
+#define TEA6320_FRL        0x04  /* fader rear left (0-5) */
+#define TEA6320_BA         0x05  /* bass (0-4) */
+#define TEA6320_TR         0x06  /* treble (0-4) */
+#define TEA6320_S          0x07  /* switch register */
+				 /* values for those registers: */
+#define TEA6320_S_SA       0x07  /* stereo A input */
+#define TEA6320_S_SB       0x06  /* stereo B */
+#define TEA6320_S_SC       0x05  /* stereo C */
+#define TEA6320_S_SD       0x04  /* stereo D */
+#define TEA6320_S_GMU      0x80  /* general mute */
+
+#define TEA6420_S_SA       0x00  /* stereo A input */
+#define TEA6420_S_SB       0x01  /* stereo B */
+#define TEA6420_S_SC       0x02  /* stereo C */
+#define TEA6420_S_SD       0x03  /* stereo D */
+#define TEA6420_S_SE       0x04  /* stereo E */
+#define TEA6420_S_GMU      0x05  /* general mute */
+
+static int tea6300_shift10(int val) { return val >> 10; }
+static int tea6300_shift12(int val) { return val >> 12; }
+
+/* Assumes 16bit input (values 0x3f to 0x0c are unique, values less than */
+/* 0x0c mirror those immediately higher) */
+static int tea6320_volume(int val) { return (val / (65535/(63-12)) + 12) & 0x3f; }
+static int tea6320_shift11(int val) { return val >> 11; }
+static int tea6320_initialize(struct CHIPSTATE * chip)
+{
+	chip_write(chip, TEA6320_FFR, 0x3f);
+	chip_write(chip, TEA6320_FFL, 0x3f);
+	chip_write(chip, TEA6320_FRR, 0x3f);
+	chip_write(chip, TEA6320_FRL, 0x3f);
+
+	return 0;
+}
+
+
+/* ---------------------------------------------------------------------- */
+/* audio chip descriptions - defines+functions for tda8425                */
+
+#define TDA8425_VL         0x00  /* volume left */
+#define TDA8425_VR         0x01  /* volume right */
+#define TDA8425_BA         0x02  /* bass */
+#define TDA8425_TR         0x03  /* treble */
+#define TDA8425_S1         0x08  /* switch functions */
+				 /* values for those registers: */
+#define TDA8425_S1_OFF     0xEE  /* audio off (mute on) */
+#define TDA8425_S1_CH1     0xCE  /* audio channel 1 (mute off) - "linear stereo" mode */
+#define TDA8425_S1_CH2     0xCF  /* audio channel 2 (mute off) - "linear stereo" mode */
+#define TDA8425_S1_MU      0x20  /* mute bit */
+#define TDA8425_S1_STEREO  0x18  /* stereo bits */
+#define TDA8425_S1_STEREO_SPATIAL 0x18 /* spatial stereo */
+#define TDA8425_S1_STEREO_LINEAR  0x08 /* linear stereo */
+#define TDA8425_S1_STEREO_PSEUDO  0x10 /* pseudo stereo */
+#define TDA8425_S1_STEREO_MONO    0x00 /* forced mono */
+#define TDA8425_S1_ML      0x06        /* language selector */
+#define TDA8425_S1_ML_SOUND_A 0x02     /* sound a */
+#define TDA8425_S1_ML_SOUND_B 0x04     /* sound b */
+#define TDA8425_S1_ML_STEREO  0x06     /* stereo */
+#define TDA8425_S1_IS      0x01        /* channel selector */
+
+
+static int tda8425_shift10(int val) { return (val >> 10) | 0xc0; }
+static int tda8425_shift12(int val) { return (val >> 12) | 0xf0; }
+
+static void tda8425_setaudmode(struct CHIPSTATE *chip, int mode)
+{
+	int s1 = chip->shadow.bytes[TDA8425_S1+1] & 0xe1;
+
+	switch (mode) {
+	case V4L2_TUNER_MODE_LANG1:
+		s1 |= TDA8425_S1_ML_SOUND_A;
+		s1 |= TDA8425_S1_STEREO_PSEUDO;
+		break;
+	case V4L2_TUNER_MODE_LANG2:
+		s1 |= TDA8425_S1_ML_SOUND_B;
+		s1 |= TDA8425_S1_STEREO_PSEUDO;
+		break;
+	case V4L2_TUNER_MODE_LANG1_LANG2:
+		s1 |= TDA8425_S1_ML_STEREO;
+		s1 |= TDA8425_S1_STEREO_LINEAR;
+		break;
+	case V4L2_TUNER_MODE_MONO:
+		s1 |= TDA8425_S1_ML_STEREO;
+		s1 |= TDA8425_S1_STEREO_MONO;
+		break;
+	case V4L2_TUNER_MODE_STEREO:
+		s1 |= TDA8425_S1_ML_STEREO;
+		s1 |= TDA8425_S1_STEREO_SPATIAL;
+		break;
+	default:
+		return;
+	}
+	chip_write(chip,TDA8425_S1,s1);
+}
+
+
+/* ---------------------------------------------------------------------- */
+/* audio chip descriptions - defines+functions for pic16c54 (PV951)       */
+
+/* the registers of 16C54, I2C sub address. */
+#define PIC16C54_REG_KEY_CODE     0x01	       /* Not use. */
+#define PIC16C54_REG_MISC         0x02
+
+/* bit definition of the RESET register, I2C data. */
+#define PIC16C54_MISC_RESET_REMOTE_CTL 0x01 /* bit 0, Reset to receive the key */
+					    /*        code of remote controller */
+#define PIC16C54_MISC_MTS_MAIN         0x02 /* bit 1 */
+#define PIC16C54_MISC_MTS_SAP          0x04 /* bit 2 */
+#define PIC16C54_MISC_MTS_BOTH         0x08 /* bit 3 */
+#define PIC16C54_MISC_SND_MUTE         0x10 /* bit 4, Mute Audio(Line-in and Tuner) */
+#define PIC16C54_MISC_SND_NOTMUTE      0x20 /* bit 5 */
+#define PIC16C54_MISC_SWITCH_TUNER     0x40 /* bit 6	, Switch to Line-in */
+#define PIC16C54_MISC_SWITCH_LINE      0x80 /* bit 7	, Switch to Tuner */
+
+/* ---------------------------------------------------------------------- */
+/* audio chip descriptions - defines+functions for TA8874Z                */
+
+/* write 1st byte */
+#define TA8874Z_LED_STE	0x80
+#define TA8874Z_LED_BIL	0x40
+#define TA8874Z_LED_EXT	0x20
+#define TA8874Z_MONO_SET	0x10
+#define TA8874Z_MUTE	0x08
+#define TA8874Z_F_MONO	0x04
+#define TA8874Z_MODE_SUB	0x02
+#define TA8874Z_MODE_MAIN	0x01
+
+/* write 2nd byte */
+/*#define TA8874Z_TI	0x80  */ /* test mode */
+#define TA8874Z_SEPARATION	0x3f
+#define TA8874Z_SEPARATION_DEFAULT	0x10
+
+/* read */
+#define TA8874Z_B1	0x80
+#define TA8874Z_B0	0x40
+#define TA8874Z_CHAG_FLAG	0x20
+
+/*
+ *        B1 B0
+ * mono    L  H
+ * stereo  L  L
+ * BIL     H  L
+ */
+static int ta8874z_getrxsubchans(struct CHIPSTATE *chip)
+{
+	int val, mode;
+
+	mode = V4L2_TUNER_SUB_MONO;
+
+	val = chip_read(chip);
+	if (val < 0)
+		return mode;
+
+	if (val & TA8874Z_B1){
+		mode |= V4L2_TUNER_SUB_LANG1 | V4L2_TUNER_SUB_LANG2;
+	}else if (!(val & TA8874Z_B0)){
+		mode = V4L2_TUNER_SUB_STEREO;
+	}
+	/* v4l2_dbg(1, debug, &chip->sd,
+		 "ta8874z_getrxsubchans(): raw chip read: 0x%02x, return: 0x%02x\n",
+		 val, mode); */
+	return mode;
+}
+
+static audiocmd ta8874z_stereo = { 2, {0, TA8874Z_SEPARATION_DEFAULT}};
+static audiocmd ta8874z_mono = {2, { TA8874Z_MONO_SET, TA8874Z_SEPARATION_DEFAULT}};
+static audiocmd ta8874z_main = {2, { 0, TA8874Z_SEPARATION_DEFAULT}};
+static audiocmd ta8874z_sub = {2, { TA8874Z_MODE_SUB, TA8874Z_SEPARATION_DEFAULT}};
+static audiocmd ta8874z_both = {2, { TA8874Z_MODE_MAIN | TA8874Z_MODE_SUB, TA8874Z_SEPARATION_DEFAULT}};
+
+static void ta8874z_setaudmode(struct CHIPSTATE *chip, int mode)
+{
+	struct v4l2_subdev *sd = &chip->sd;
+	int update = 1;
+	audiocmd *t = NULL;
+
+	v4l2_dbg(1, debug, sd, "ta8874z_setaudmode(): mode: 0x%02x\n", mode);
+
+	switch(mode){
+	case V4L2_TUNER_MODE_MONO:
+		t = &ta8874z_mono;
+		break;
+	case V4L2_TUNER_MODE_STEREO:
+		t = &ta8874z_stereo;
+		break;
+	case V4L2_TUNER_MODE_LANG1:
+		t = &ta8874z_main;
+		break;
+	case V4L2_TUNER_MODE_LANG2:
+		t = &ta8874z_sub;
+		break;
+	case V4L2_TUNER_MODE_LANG1_LANG2:
+		t = &ta8874z_both;
+		break;
+	default:
+		update = 0;
+	}
+
+	if(update)
+		chip_cmd(chip, "TA8874Z", t);
+}
+
+static int ta8874z_checkit(struct CHIPSTATE *chip)
+{
+	int rc;
+
+	rc = chip_read(chip);
+	if (rc < 0)
+		return rc;
+
+	return ((rc & 0x1f) == 0x1f) ? 1 : 0;
+}
+
+/* ---------------------------------------------------------------------- */
+/* audio chip descriptions - struct CHIPDESC                              */
+
+/* insmod options to enable/disable individual audio chips */
+static int tda8425  = 1;
+static int tda9840  = 1;
+static int tda9850  = 1;
+static int tda9855  = 1;
+static int tda9873  = 1;
+static int tda9874a = 1;
+static int tda9875  = 1;
+static int tea6300;	/* default 0 - address clash with msp34xx */
+static int tea6320;	/* default 0 - address clash with msp34xx */
+static int tea6420  = 1;
+static int pic16c54 = 1;
+static int ta8874z;	/* default 0 - address clash with tda9840 */
+
+module_param(tda8425, int, 0444);
+module_param(tda9840, int, 0444);
+module_param(tda9850, int, 0444);
+module_param(tda9855, int, 0444);
+module_param(tda9873, int, 0444);
+module_param(tda9874a, int, 0444);
+module_param(tda9875, int, 0444);
+module_param(tea6300, int, 0444);
+module_param(tea6320, int, 0444);
+module_param(tea6420, int, 0444);
+module_param(pic16c54, int, 0444);
+module_param(ta8874z, int, 0444);
+
+static struct CHIPDESC chiplist[] = {
+	{
+		.name       = "tda9840",
+		.insmodopt  = &tda9840,
+		.addr_lo    = I2C_ADDR_TDA9840 >> 1,
+		.addr_hi    = I2C_ADDR_TDA9840 >> 1,
+		.registers  = 5,
+		.flags      = CHIP_NEED_CHECKMODE,
+
+		/* callbacks */
+		.checkit    = tda9840_checkit,
+		.getrxsubchans = tda9840_getrxsubchans,
+		.setaudmode = tda9840_setaudmode,
+
+		.init       = { 2, { TDA9840_TEST, TDA9840_TEST_INT1SN
+				/* ,TDA9840_SW, TDA9840_MONO */} }
+	},
+	{
+		.name       = "tda9873h",
+		.insmodopt  = &tda9873,
+		.addr_lo    = I2C_ADDR_TDA985x_L >> 1,
+		.addr_hi    = I2C_ADDR_TDA985x_H >> 1,
+		.registers  = 3,
+		.flags      = CHIP_HAS_INPUTSEL | CHIP_NEED_CHECKMODE,
+
+		/* callbacks */
+		.checkit    = tda9873_checkit,
+		.getrxsubchans = tda9873_getrxsubchans,
+		.setaudmode = tda9873_setaudmode,
+
+		.init       = { 4, { TDA9873_SW, 0xa4, 0x06, 0x03 } },
+		.inputreg   = TDA9873_SW,
+		.inputmute  = TDA9873_MUTE | TDA9873_AUTOMUTE,
+		.inputmap   = {0xa0, 0xa2, 0xa0, 0xa0},
+		.inputmask  = TDA9873_INP_MASK|TDA9873_MUTE|TDA9873_AUTOMUTE,
+
+	},
+	{
+		.name       = "tda9874h/a",
+		.insmodopt  = &tda9874a,
+		.addr_lo    = I2C_ADDR_TDA9874 >> 1,
+		.addr_hi    = I2C_ADDR_TDA9874 >> 1,
+		.flags      = CHIP_NEED_CHECKMODE,
+
+		/* callbacks */
+		.initialize = tda9874a_initialize,
+		.checkit    = tda9874a_checkit,
+		.getrxsubchans = tda9874a_getrxsubchans,
+		.setaudmode = tda9874a_setaudmode,
+	},
+	{
+		.name       = "tda9875",
+		.insmodopt  = &tda9875,
+		.addr_lo    = I2C_ADDR_TDA9875 >> 1,
+		.addr_hi    = I2C_ADDR_TDA9875 >> 1,
+		.flags      = CHIP_HAS_VOLUME | CHIP_HAS_BASSTREBLE,
+
+		/* callbacks */
+		.initialize = tda9875_initialize,
+		.checkit    = tda9875_checkit,
+		.volfunc    = tda9875_volume,
+		.bassfunc   = tda9875_bass,
+		.treblefunc = tda9875_treble,
+		.leftreg    = TDA9875_MVL,
+		.rightreg   = TDA9875_MVR,
+		.bassreg    = TDA9875_MBA,
+		.treblereg  = TDA9875_MTR,
+		.volinit    = 58880,
+	},
+	{
+		.name       = "tda9850",
+		.insmodopt  = &tda9850,
+		.addr_lo    = I2C_ADDR_TDA985x_L >> 1,
+		.addr_hi    = I2C_ADDR_TDA985x_H >> 1,
+		.registers  = 11,
+
+		.getrxsubchans = tda985x_getrxsubchans,
+		.setaudmode = tda985x_setaudmode,
+
+		.init       = { 8, { TDA9850_C4, 0x08, 0x08, TDA985x_STEREO, 0x07, 0x10, 0x10, 0x03 } }
+	},
+	{
+		.name       = "tda9855",
+		.insmodopt  = &tda9855,
+		.addr_lo    = I2C_ADDR_TDA985x_L >> 1,
+		.addr_hi    = I2C_ADDR_TDA985x_H >> 1,
+		.registers  = 11,
+		.flags      = CHIP_HAS_VOLUME | CHIP_HAS_BASSTREBLE,
+
+		.leftreg    = TDA9855_VL,
+		.rightreg   = TDA9855_VR,
+		.bassreg    = TDA9855_BA,
+		.treblereg  = TDA9855_TR,
+
+		/* callbacks */
+		.volfunc    = tda9855_volume,
+		.bassfunc   = tda9855_bass,
+		.treblefunc = tda9855_treble,
+		.getrxsubchans = tda985x_getrxsubchans,
+		.setaudmode = tda985x_setaudmode,
+
+		.init       = { 12, { 0, 0x6f, 0x6f, 0x0e, 0x07<<1, 0x8<<2,
+				    TDA9855_MUTE | TDA9855_AVL | TDA9855_LOUD | TDA9855_INT,
+				    TDA985x_STEREO | TDA9855_LINEAR | TDA9855_TZCM | TDA9855_VZCM,
+				    0x07, 0x10, 0x10, 0x03 }}
+	},
+	{
+		.name       = "tea6300",
+		.insmodopt  = &tea6300,
+		.addr_lo    = I2C_ADDR_TEA6300 >> 1,
+		.addr_hi    = I2C_ADDR_TEA6300 >> 1,
+		.registers  = 6,
+		.flags      = CHIP_HAS_VOLUME | CHIP_HAS_BASSTREBLE | CHIP_HAS_INPUTSEL,
+
+		.leftreg    = TEA6300_VR,
+		.rightreg   = TEA6300_VL,
+		.bassreg    = TEA6300_BA,
+		.treblereg  = TEA6300_TR,
+
+		/* callbacks */
+		.volfunc    = tea6300_shift10,
+		.bassfunc   = tea6300_shift12,
+		.treblefunc = tea6300_shift12,
+
+		.inputreg   = TEA6300_S,
+		.inputmap   = { TEA6300_S_SA, TEA6300_S_SB, TEA6300_S_SC },
+		.inputmute  = TEA6300_S_GMU,
+	},
+	{
+		.name       = "tea6320",
+		.insmodopt  = &tea6320,
+		.addr_lo    = I2C_ADDR_TEA6300 >> 1,
+		.addr_hi    = I2C_ADDR_TEA6300 >> 1,
+		.registers  = 8,
+		.flags      = CHIP_HAS_VOLUME | CHIP_HAS_BASSTREBLE | CHIP_HAS_INPUTSEL,
+
+		.leftreg    = TEA6320_V,
+		.rightreg   = TEA6320_V,
+		.bassreg    = TEA6320_BA,
+		.treblereg  = TEA6320_TR,
+
+		/* callbacks */
+		.initialize = tea6320_initialize,
+		.volfunc    = tea6320_volume,
+		.bassfunc   = tea6320_shift11,
+		.treblefunc = tea6320_shift11,
+
+		.inputreg   = TEA6320_S,
+		.inputmap   = { TEA6320_S_SA, TEA6420_S_SB, TEA6300_S_SC, TEA6320_S_SD },
+		.inputmute  = TEA6300_S_GMU,
+	},
+	{
+		.name       = "tea6420",
+		.insmodopt  = &tea6420,
+		.addr_lo    = I2C_ADDR_TEA6420 >> 1,
+		.addr_hi    = I2C_ADDR_TEA6420 >> 1,
+		.registers  = 1,
+		.flags      = CHIP_HAS_INPUTSEL,
+
+		.inputreg   = -1,
+		.inputmap   = { TEA6420_S_SA, TEA6420_S_SB, TEA6420_S_SC },
+		.inputmute  = TEA6420_S_GMU,
+		.inputmask  = 0x07,
+	},
+	{
+		.name       = "tda8425",
+		.insmodopt  = &tda8425,
+		.addr_lo    = I2C_ADDR_TDA8425 >> 1,
+		.addr_hi    = I2C_ADDR_TDA8425 >> 1,
+		.registers  = 9,
+		.flags      = CHIP_HAS_VOLUME | CHIP_HAS_BASSTREBLE | CHIP_HAS_INPUTSEL,
+
+		.leftreg    = TDA8425_VL,
+		.rightreg   = TDA8425_VR,
+		.bassreg    = TDA8425_BA,
+		.treblereg  = TDA8425_TR,
+
+		/* callbacks */
+		.volfunc    = tda8425_shift10,
+		.bassfunc   = tda8425_shift12,
+		.treblefunc = tda8425_shift12,
+		.setaudmode = tda8425_setaudmode,
+
+		.inputreg   = TDA8425_S1,
+		.inputmap   = { TDA8425_S1_CH1, TDA8425_S1_CH1, TDA8425_S1_CH1 },
+		.inputmute  = TDA8425_S1_OFF,
+
+	},
+	{
+		.name       = "pic16c54 (PV951)",
+		.insmodopt  = &pic16c54,
+		.addr_lo    = I2C_ADDR_PIC16C54 >> 1,
+		.addr_hi    = I2C_ADDR_PIC16C54>> 1,
+		.registers  = 2,
+		.flags      = CHIP_HAS_INPUTSEL,
+
+		.inputreg   = PIC16C54_REG_MISC,
+		.inputmap   = {PIC16C54_MISC_SND_NOTMUTE|PIC16C54_MISC_SWITCH_TUNER,
+			     PIC16C54_MISC_SND_NOTMUTE|PIC16C54_MISC_SWITCH_LINE,
+			     PIC16C54_MISC_SND_NOTMUTE|PIC16C54_MISC_SWITCH_LINE,
+			     PIC16C54_MISC_SND_MUTE},
+		.inputmute  = PIC16C54_MISC_SND_MUTE,
+	},
+	{
+		.name       = "ta8874z",
+		.checkit    = ta8874z_checkit,
+		.insmodopt  = &ta8874z,
+		.addr_lo    = I2C_ADDR_TDA9840 >> 1,
+		.addr_hi    = I2C_ADDR_TDA9840 >> 1,
+		.registers  = 2,
+
+		/* callbacks */
+		.getrxsubchans = ta8874z_getrxsubchans,
+		.setaudmode = ta8874z_setaudmode,
+
+		.init       = {2, { TA8874Z_MONO_SET, TA8874Z_SEPARATION_DEFAULT}},
+	},
+	{ .name = NULL } /* EOF */
+};
+
+
+/* ---------------------------------------------------------------------- */
+
+static int tvaudio_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+	struct CHIPSTATE *chip = to_state(sd);
+	struct CHIPDESC *desc = chip->desc;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUDIO_MUTE:
+		chip->muted = ctrl->val;
+		if (chip->muted)
+			chip_write_masked(chip,desc->inputreg,desc->inputmute,desc->inputmask);
+		else
+			chip_write_masked(chip,desc->inputreg,
+					desc->inputmap[chip->input],desc->inputmask);
+		return 0;
+	case V4L2_CID_AUDIO_VOLUME: {
+		u32 volume, balance;
+		u32 left, right;
+
+		volume = chip->volume->val;
+		balance = chip->balance->val;
+		left = (min(65536U - balance, 32768U) * volume) / 32768U;
+		right = (min(balance, 32768U) * volume) / 32768U;
+
+		chip_write(chip, desc->leftreg, desc->volfunc(left));
+		chip_write(chip, desc->rightreg, desc->volfunc(right));
+		return 0;
+	}
+	case V4L2_CID_AUDIO_BASS:
+		chip_write(chip, desc->bassreg, desc->bassfunc(ctrl->val));
+		return 0;
+	case V4L2_CID_AUDIO_TREBLE:
+		chip_write(chip, desc->treblereg, desc->treblefunc(ctrl->val));
+		return 0;
+	}
+	return -EINVAL;
+}
+
+
+/* ---------------------------------------------------------------------- */
+/* video4linux interface                                                  */
+
+static int tvaudio_s_radio(struct v4l2_subdev *sd)
+{
+	struct CHIPSTATE *chip = to_state(sd);
+
+	chip->radio = 1;
+	/* del_timer(&chip->wt); */
+	return 0;
+}
+
+static int tvaudio_s_routing(struct v4l2_subdev *sd,
+			     u32 input, u32 output, u32 config)
+{
+	struct CHIPSTATE *chip = to_state(sd);
+	struct CHIPDESC *desc = chip->desc;
+
+	if (!(desc->flags & CHIP_HAS_INPUTSEL))
+		return 0;
+	if (input >= 4)
+		return -EINVAL;
+	/* There are four inputs: tuner, radio, extern and intern. */
+	chip->input = input;
+	if (chip->muted)
+		return 0;
+	chip_write_masked(chip, desc->inputreg,
+			desc->inputmap[chip->input], desc->inputmask);
+	return 0;
+}
+
+static int tvaudio_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *vt)
+{
+	struct CHIPSTATE *chip = to_state(sd);
+	struct CHIPDESC *desc = chip->desc;
+
+	if (!desc->setaudmode)
+		return 0;
+	if (chip->radio)
+		return 0;
+
+	switch (vt->audmode) {
+	case V4L2_TUNER_MODE_MONO:
+	case V4L2_TUNER_MODE_STEREO:
+	case V4L2_TUNER_MODE_LANG1:
+	case V4L2_TUNER_MODE_LANG2:
+	case V4L2_TUNER_MODE_LANG1_LANG2:
+		break;
+	default:
+		return -EINVAL;
+	}
+	chip->audmode = vt->audmode;
+
+	if (chip->thread)
+		wake_up_process(chip->thread);
+	else
+		desc->setaudmode(chip, vt->audmode);
+
+	return 0;
+}
+
+static int tvaudio_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
+{
+	struct CHIPSTATE *chip = to_state(sd);
+	struct CHIPDESC *desc = chip->desc;
+
+	if (!desc->getrxsubchans)
+		return 0;
+	if (chip->radio)
+		return 0;
+
+	vt->audmode = chip->audmode;
+	vt->rxsubchans = desc->getrxsubchans(chip);
+	vt->capability |= V4L2_TUNER_CAP_STEREO |
+		V4L2_TUNER_CAP_LANG1 | V4L2_TUNER_CAP_LANG2;
+
+	return 0;
+}
+
+static int tvaudio_s_std(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct CHIPSTATE *chip = to_state(sd);
+
+	chip->radio = 0;
+	return 0;
+}
+
+static int tvaudio_s_frequency(struct v4l2_subdev *sd, const struct v4l2_frequency *freq)
+{
+	struct CHIPSTATE *chip = to_state(sd);
+	struct CHIPDESC *desc = chip->desc;
+
+	/* For chips that provide getrxsubchans and setaudmode, and doesn't
+	   automatically follows the stereo carrier, a kthread is
+	   created to set the audio standard. In this case, when then
+	   the video channel is changed, tvaudio starts on MONO mode.
+	   After waiting for 2 seconds, the kernel thread is called,
+	   to follow whatever audio standard is pointed by the
+	   audio carrier.
+	 */
+	if (chip->thread) {
+		desc->setaudmode(chip, V4L2_TUNER_MODE_MONO);
+		chip->prevmode = -1; /* reset previous mode */
+		mod_timer(&chip->wt, jiffies+msecs_to_jiffies(2000));
+	}
+	return 0;
+}
+
+static int tvaudio_log_status(struct v4l2_subdev *sd)
+{
+	struct CHIPSTATE *chip = to_state(sd);
+	struct CHIPDESC *desc = chip->desc;
+
+	v4l2_info(sd, "Chip: %s\n", desc->name);
+	v4l2_ctrl_handler_log_status(&chip->hdl, sd->name);
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_ctrl_ops tvaudio_ctrl_ops = {
+	.s_ctrl = tvaudio_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops tvaudio_core_ops = {
+	.log_status = tvaudio_log_status,
+};
+
+static const struct v4l2_subdev_tuner_ops tvaudio_tuner_ops = {
+	.s_radio = tvaudio_s_radio,
+	.s_frequency = tvaudio_s_frequency,
+	.s_tuner = tvaudio_s_tuner,
+	.g_tuner = tvaudio_g_tuner,
+};
+
+static const struct v4l2_subdev_audio_ops tvaudio_audio_ops = {
+	.s_routing = tvaudio_s_routing,
+};
+
+static const struct v4l2_subdev_video_ops tvaudio_video_ops = {
+	.s_std = tvaudio_s_std,
+};
+
+static const struct v4l2_subdev_ops tvaudio_ops = {
+	.core = &tvaudio_core_ops,
+	.tuner = &tvaudio_tuner_ops,
+	.audio = &tvaudio_audio_ops,
+	.video = &tvaudio_video_ops,
+};
+
+/* ----------------------------------------------------------------------- */
+
+
+/* i2c registration                                                       */
+
+static int tvaudio_probe(struct i2c_client *client)
+{
+	const struct i2c_device_id *id = i2c_client_get_device_id(client);
+	struct CHIPSTATE *chip;
+	struct CHIPDESC  *desc;
+	struct v4l2_subdev *sd;
+
+	if (debug) {
+		printk(KERN_INFO "tvaudio: TV audio decoder + audio/video mux driver\n");
+		printk(KERN_INFO "tvaudio: known chips: ");
+		for (desc = chiplist; desc->name != NULL; desc++)
+			printk(KERN_CONT "%s%s",
+			       (desc == chiplist) ? "" : ", ", desc->name);
+		printk(KERN_CONT "\n");
+	}
+
+	chip = devm_kzalloc(&client->dev, sizeof(*chip), GFP_KERNEL);
+	if (!chip)
+		return -ENOMEM;
+	sd = &chip->sd;
+	v4l2_i2c_subdev_init(sd, client, &tvaudio_ops);
+
+	/* find description for the chip */
+	v4l2_dbg(1, debug, sd, "chip found @ 0x%x\n", client->addr<<1);
+	for (desc = chiplist; desc->name != NULL; desc++) {
+		if (0 == *(desc->insmodopt))
+			continue;
+		if (client->addr < desc->addr_lo ||
+		    client->addr > desc->addr_hi)
+			continue;
+		if (desc->checkit && !desc->checkit(chip))
+			continue;
+		break;
+	}
+	if (desc->name == NULL) {
+		v4l2_dbg(1, debug, sd, "no matching chip description found\n");
+		return -EIO;
+	}
+	v4l2_info(sd, "%s found @ 0x%x (%s)\n", desc->name, client->addr<<1, client->adapter->name);
+	if (desc->flags) {
+		v4l2_dbg(1, debug, sd, "matches:%s%s%s.\n",
+			(desc->flags & CHIP_HAS_VOLUME)     ? " volume"      : "",
+			(desc->flags & CHIP_HAS_BASSTREBLE) ? " bass/treble" : "",
+			(desc->flags & CHIP_HAS_INPUTSEL)   ? " audiomux"    : "");
+	}
+
+	/* fill required data structures */
+	if (!id)
+		strscpy(client->name, desc->name, I2C_NAME_SIZE);
+	chip->desc = desc;
+	chip->shadow.count = desc->registers+1;
+	chip->prevmode = -1;
+	chip->audmode = V4L2_TUNER_MODE_LANG1;
+
+	/* initialization  */
+	if (desc->initialize != NULL)
+		desc->initialize(chip);
+	else
+		chip_cmd(chip, "init", &desc->init);
+
+	v4l2_ctrl_handler_init(&chip->hdl, 5);
+	if (desc->flags & CHIP_HAS_INPUTSEL)
+		v4l2_ctrl_new_std(&chip->hdl, &tvaudio_ctrl_ops,
+			V4L2_CID_AUDIO_MUTE, 0, 1, 1, 0);
+	if (desc->flags & CHIP_HAS_VOLUME) {
+		if (!desc->volfunc) {
+			/* This shouldn't be happen. Warn user, but keep working
+			   without volume controls
+			 */
+			v4l2_info(sd, "volume callback undefined!\n");
+			desc->flags &= ~CHIP_HAS_VOLUME;
+		} else {
+			chip->volume = v4l2_ctrl_new_std(&chip->hdl,
+				&tvaudio_ctrl_ops, V4L2_CID_AUDIO_VOLUME,
+				0, 65535, 65535 / 100,
+				desc->volinit ? desc->volinit : 65535);
+			chip->balance = v4l2_ctrl_new_std(&chip->hdl,
+				&tvaudio_ctrl_ops, V4L2_CID_AUDIO_BALANCE,
+				0, 65535, 65535 / 100, 32768);
+			v4l2_ctrl_cluster(2, &chip->volume);
+		}
+	}
+	if (desc->flags & CHIP_HAS_BASSTREBLE) {
+		if (!desc->bassfunc || !desc->treblefunc) {
+			/* This shouldn't be happen. Warn user, but keep working
+			   without bass/treble controls
+			 */
+			v4l2_info(sd, "bass/treble callbacks undefined!\n");
+			desc->flags &= ~CHIP_HAS_BASSTREBLE;
+		} else {
+			v4l2_ctrl_new_std(&chip->hdl,
+				&tvaudio_ctrl_ops, V4L2_CID_AUDIO_BASS,
+				0, 65535, 65535 / 100,
+				desc->bassinit ? desc->bassinit : 32768);
+			v4l2_ctrl_new_std(&chip->hdl,
+				&tvaudio_ctrl_ops, V4L2_CID_AUDIO_TREBLE,
+				0, 65535, 65535 / 100,
+				desc->trebleinit ? desc->trebleinit : 32768);
+		}
+	}
+
+	sd->ctrl_handler = &chip->hdl;
+	if (chip->hdl.error) {
+		int err = chip->hdl.error;
+
+		v4l2_ctrl_handler_free(&chip->hdl);
+		return err;
+	}
+	/* set controls to the default values */
+	v4l2_ctrl_handler_setup(&chip->hdl);
+
+	chip->thread = NULL;
+	timer_setup(&chip->wt, chip_thread_wake, 0);
+	if (desc->flags & CHIP_NEED_CHECKMODE) {
+		if (!desc->getrxsubchans || !desc->setaudmode) {
+			/* This shouldn't be happen. Warn user, but keep working
+			   without kthread
+			 */
+			v4l2_info(sd, "set/get mode callbacks undefined!\n");
+			return 0;
+		}
+		/* start async thread */
+		chip->thread = kthread_run(chip_thread, chip, "%s",
+					   client->name);
+		if (IS_ERR(chip->thread)) {
+			v4l2_warn(sd, "failed to create kthread\n");
+			chip->thread = NULL;
+		}
+	}
+	return 0;
+}
+
+static void tvaudio_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct CHIPSTATE *chip = to_state(sd);
+
+	del_timer_sync(&chip->wt);
+	if (chip->thread) {
+		/* shutdown async thread */
+		kthread_stop(chip->thread);
+		chip->thread = NULL;
+	}
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&chip->hdl);
+}
+
+/* This driver supports many devices and the idea is to let the driver
+   detect which device is present. So rather than listing all supported
+   devices here, we pretend to support a single, fake device type. */
+static const struct i2c_device_id tvaudio_id[] = {
+	{ "tvaudio", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, tvaudio_id);
+
+static struct i2c_driver tvaudio_driver = {
+	.driver = {
+		.name	= "tvaudio",
+	},
+	.probe		= tvaudio_probe,
+	.remove		= tvaudio_remove,
+	.id_table	= tvaudio_id,
+};
+
+module_i2c_driver(tvaudio_driver);
diff --git a/drivers/media/i2c/tvp514x.c b/drivers/media/i2c/tvp514x.c
new file mode 100644
index 000000000..aa6d4b67b
--- /dev/null
+++ b/drivers/media/i2c/tvp514x.c
@@ -0,0 +1,1216 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * drivers/media/i2c/tvp514x.c
+ *
+ * TI TVP5146/47 decoder driver
+ *
+ * Copyright (C) 2008 Texas Instruments Inc
+ * Author: Vaibhav Hiremath <hvaibhav@ti.com>
+ *
+ * Contributors:
+ *     Sivaraj R <sivaraj@ti.com>
+ *     Brijesh R Jadav <brijesh.j@ti.com>
+ *     Hardik Shah <hardik.shah@ti.com>
+ *     Manjunath Hadli <mrh@ti.com>
+ *     Karicheri Muralidharan <m-karicheri2@ti.com>
+ *     Prabhakar Lad <prabhakar.lad@ti.com>
+ */
+
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/videodev2.h>
+#include <linux/module.h>
+#include <linux/v4l2-mediabus.h>
+#include <linux/of.h>
+#include <linux/of_graph.h>
+
+#include <media/v4l2-async.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-mediabus.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-ctrls.h>
+#include <media/i2c/tvp514x.h>
+#include <media/media-entity.h>
+
+#include "tvp514x_regs.h"
+
+/* Private macros for TVP */
+#define I2C_RETRY_COUNT                 (5)
+#define LOCK_RETRY_COUNT                (5)
+#define LOCK_RETRY_DELAY                (200)
+
+/* Debug functions */
+static bool debug;
+module_param(debug, bool, 0644);
+MODULE_PARM_DESC(debug, "Debug level (0-1)");
+
+MODULE_AUTHOR("Texas Instruments");
+MODULE_DESCRIPTION("TVP514X linux decoder driver");
+MODULE_LICENSE("GPL");
+
+/* enum tvp514x_std - enum for supported standards */
+enum tvp514x_std {
+	STD_NTSC_MJ = 0,
+	STD_PAL_BDGHIN,
+	STD_INVALID
+};
+
+/**
+ * struct tvp514x_std_info - Structure to store standard information
+ * @width: Line width in pixels
+ * @height:Number of active lines
+ * @video_std: Value to write in REG_VIDEO_STD register
+ * @standard: v4l2 standard structure information
+ */
+struct tvp514x_std_info {
+	unsigned long width;
+	unsigned long height;
+	u8 video_std;
+	struct v4l2_standard standard;
+};
+
+static struct tvp514x_reg tvp514x_reg_list_default[0x40];
+
+static int tvp514x_s_stream(struct v4l2_subdev *sd, int enable);
+/**
+ * struct tvp514x_decoder - TVP5146/47 decoder object
+ * @sd: Subdevice Slave handle
+ * @hdl: embedded &struct v4l2_ctrl_handler
+ * @tvp514x_regs: copy of hw's regs with preset values.
+ * @pdata: Board specific
+ * @ver: Chip version
+ * @streaming: TVP5146/47 decoder streaming - enabled or disabled.
+ * @pix: Current pixel format
+ * @num_fmts: Number of formats
+ * @fmt_list: Format list
+ * @current_std: Current standard
+ * @num_stds: Number of standards
+ * @std_list: Standards list
+ * @input: Input routing at chip level
+ * @output: Output routing at chip level
+ * @pad: subdev media pad associated with the decoder
+ * @format: media bus frame format
+ * @int_seq: driver's register init sequence
+ */
+struct tvp514x_decoder {
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
+	struct tvp514x_reg tvp514x_regs[ARRAY_SIZE(tvp514x_reg_list_default)];
+	const struct tvp514x_platform_data *pdata;
+
+	int ver;
+	int streaming;
+
+	struct v4l2_pix_format pix;
+	int num_fmts;
+	const struct v4l2_fmtdesc *fmt_list;
+
+	enum tvp514x_std current_std;
+	int num_stds;
+	const struct tvp514x_std_info *std_list;
+	/* Input and Output Routing parameters */
+	u32 input;
+	u32 output;
+
+	/* mc related members */
+	struct media_pad pad;
+	struct v4l2_mbus_framefmt format;
+
+	struct tvp514x_reg *int_seq;
+};
+
+/* TVP514x default register values */
+static struct tvp514x_reg tvp514x_reg_list_default[] = {
+	/* Composite selected */
+	{TOK_WRITE, REG_INPUT_SEL, 0x05},
+	{TOK_WRITE, REG_AFE_GAIN_CTRL, 0x0F},
+	/* Auto mode */
+	{TOK_WRITE, REG_VIDEO_STD, 0x00},
+	{TOK_WRITE, REG_OPERATION_MODE, 0x00},
+	{TOK_SKIP, REG_AUTOSWITCH_MASK, 0x3F},
+	{TOK_WRITE, REG_COLOR_KILLER, 0x10},
+	{TOK_WRITE, REG_LUMA_CONTROL1, 0x00},
+	{TOK_WRITE, REG_LUMA_CONTROL2, 0x00},
+	{TOK_WRITE, REG_LUMA_CONTROL3, 0x02},
+	{TOK_WRITE, REG_BRIGHTNESS, 0x80},
+	{TOK_WRITE, REG_CONTRAST, 0x80},
+	{TOK_WRITE, REG_SATURATION, 0x80},
+	{TOK_WRITE, REG_HUE, 0x00},
+	{TOK_WRITE, REG_CHROMA_CONTROL1, 0x00},
+	{TOK_WRITE, REG_CHROMA_CONTROL2, 0x0E},
+	/* Reserved */
+	{TOK_SKIP, 0x0F, 0x00},
+	{TOK_WRITE, REG_COMP_PR_SATURATION, 0x80},
+	{TOK_WRITE, REG_COMP_Y_CONTRAST, 0x80},
+	{TOK_WRITE, REG_COMP_PB_SATURATION, 0x80},
+	/* Reserved */
+	{TOK_SKIP, 0x13, 0x00},
+	{TOK_WRITE, REG_COMP_Y_BRIGHTNESS, 0x80},
+	/* Reserved */
+	{TOK_SKIP, 0x15, 0x00},
+	/* NTSC timing */
+	{TOK_SKIP, REG_AVID_START_PIXEL_LSB, 0x55},
+	{TOK_SKIP, REG_AVID_START_PIXEL_MSB, 0x00},
+	{TOK_SKIP, REG_AVID_STOP_PIXEL_LSB, 0x25},
+	{TOK_SKIP, REG_AVID_STOP_PIXEL_MSB, 0x03},
+	/* NTSC timing */
+	{TOK_SKIP, REG_HSYNC_START_PIXEL_LSB, 0x00},
+	{TOK_SKIP, REG_HSYNC_START_PIXEL_MSB, 0x00},
+	{TOK_SKIP, REG_HSYNC_STOP_PIXEL_LSB, 0x40},
+	{TOK_SKIP, REG_HSYNC_STOP_PIXEL_MSB, 0x00},
+	/* NTSC timing */
+	{TOK_SKIP, REG_VSYNC_START_LINE_LSB, 0x04},
+	{TOK_SKIP, REG_VSYNC_START_LINE_MSB, 0x00},
+	{TOK_SKIP, REG_VSYNC_STOP_LINE_LSB, 0x07},
+	{TOK_SKIP, REG_VSYNC_STOP_LINE_MSB, 0x00},
+	/* NTSC timing */
+	{TOK_SKIP, REG_VBLK_START_LINE_LSB, 0x01},
+	{TOK_SKIP, REG_VBLK_START_LINE_MSB, 0x00},
+	{TOK_SKIP, REG_VBLK_STOP_LINE_LSB, 0x15},
+	{TOK_SKIP, REG_VBLK_STOP_LINE_MSB, 0x00},
+	/* Reserved */
+	{TOK_SKIP, 0x26, 0x00},
+	/* Reserved */
+	{TOK_SKIP, 0x27, 0x00},
+	{TOK_SKIP, REG_FAST_SWTICH_CONTROL, 0xCC},
+	/* Reserved */
+	{TOK_SKIP, 0x29, 0x00},
+	{TOK_SKIP, REG_FAST_SWTICH_SCART_DELAY, 0x00},
+	/* Reserved */
+	{TOK_SKIP, 0x2B, 0x00},
+	{TOK_SKIP, REG_SCART_DELAY, 0x00},
+	{TOK_SKIP, REG_CTI_DELAY, 0x00},
+	{TOK_SKIP, REG_CTI_CONTROL, 0x00},
+	/* Reserved */
+	{TOK_SKIP, 0x2F, 0x00},
+	/* Reserved */
+	{TOK_SKIP, 0x30, 0x00},
+	/* Reserved */
+	{TOK_SKIP, 0x31, 0x00},
+	/* HS, VS active high */
+	{TOK_WRITE, REG_SYNC_CONTROL, 0x00},
+	/* 10-bit BT.656 */
+	{TOK_WRITE, REG_OUTPUT_FORMATTER1, 0x00},
+	/* Enable clk & data */
+	{TOK_WRITE, REG_OUTPUT_FORMATTER2, 0x11},
+	/* Enable AVID & FLD */
+	{TOK_WRITE, REG_OUTPUT_FORMATTER3, 0xEE},
+	/* Enable VS & HS */
+	{TOK_WRITE, REG_OUTPUT_FORMATTER4, 0xAF},
+	{TOK_WRITE, REG_OUTPUT_FORMATTER5, 0xFF},
+	{TOK_WRITE, REG_OUTPUT_FORMATTER6, 0xFF},
+	/* Clear status */
+	{TOK_WRITE, REG_CLEAR_LOST_LOCK, 0x01},
+	{TOK_TERM, 0, 0},
+};
+
+/*
+ * List of image formats supported by TVP5146/47 decoder
+ * Currently we are using 8 bit mode only, but can be
+ * extended to 10/20 bit mode.
+ */
+static const struct v4l2_fmtdesc tvp514x_fmt_list[] = {
+	{
+	 .index		= 0,
+	 .type		= V4L2_BUF_TYPE_VIDEO_CAPTURE,
+	 .flags		= 0,
+	 .description	= "8-bit UYVY 4:2:2 Format",
+	 .pixelformat	= V4L2_PIX_FMT_UYVY,
+	},
+};
+
+/*
+ * Supported standards -
+ *
+ * Currently supports two standards only, need to add support for rest of the
+ * modes, like SECAM, etc...
+ */
+static const struct tvp514x_std_info tvp514x_std_list[] = {
+	/* Standard: STD_NTSC_MJ */
+	[STD_NTSC_MJ] = {
+	 .width = NTSC_NUM_ACTIVE_PIXELS,
+	 .height = NTSC_NUM_ACTIVE_LINES,
+	 .video_std = VIDEO_STD_NTSC_MJ_BIT,
+	 .standard = {
+		      .index = 0,
+		      .id = V4L2_STD_NTSC,
+		      .name = "NTSC",
+		      .frameperiod = {1001, 30000},
+		      .framelines = 525
+		     },
+	/* Standard: STD_PAL_BDGHIN */
+	},
+	[STD_PAL_BDGHIN] = {
+	 .width = PAL_NUM_ACTIVE_PIXELS,
+	 .height = PAL_NUM_ACTIVE_LINES,
+	 .video_std = VIDEO_STD_PAL_BDGHIN_BIT,
+	 .standard = {
+		      .index = 1,
+		      .id = V4L2_STD_PAL,
+		      .name = "PAL",
+		      .frameperiod = {1, 25},
+		      .framelines = 625
+		     },
+	},
+	/* Standard: need to add for additional standard */
+};
+
+
+static inline struct tvp514x_decoder *to_decoder(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct tvp514x_decoder, sd);
+}
+
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct tvp514x_decoder, hdl)->sd;
+}
+
+
+/**
+ * tvp514x_read_reg() - Read a value from a register in an TVP5146/47.
+ * @sd: ptr to v4l2_subdev struct
+ * @reg: TVP5146/47 register address
+ *
+ * Returns value read if successful, or non-zero (-1) otherwise.
+ */
+static int tvp514x_read_reg(struct v4l2_subdev *sd, u8 reg)
+{
+	int err, retry = 0;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+read_again:
+
+	err = i2c_smbus_read_byte_data(client, reg);
+	if (err < 0) {
+		if (retry <= I2C_RETRY_COUNT) {
+			v4l2_warn(sd, "Read: retry ... %d\n", retry);
+			retry++;
+			msleep_interruptible(10);
+			goto read_again;
+		}
+	}
+
+	return err;
+}
+
+/**
+ * dump_reg() - dump the register content of TVP5146/47.
+ * @sd: ptr to v4l2_subdev struct
+ * @reg: TVP5146/47 register address
+ */
+static void dump_reg(struct v4l2_subdev *sd, u8 reg)
+{
+	u32 val;
+
+	val = tvp514x_read_reg(sd, reg);
+	v4l2_info(sd, "Reg(0x%.2X): 0x%.2X\n", reg, val);
+}
+
+/**
+ * tvp514x_write_reg() - Write a value to a register in TVP5146/47
+ * @sd: ptr to v4l2_subdev struct
+ * @reg: TVP5146/47 register address
+ * @val: value to be written to the register
+ *
+ * Write a value to a register in an TVP5146/47 decoder device.
+ * Returns zero if successful, or non-zero otherwise.
+ */
+static int tvp514x_write_reg(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	int err, retry = 0;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+write_again:
+
+	err = i2c_smbus_write_byte_data(client, reg, val);
+	if (err) {
+		if (retry <= I2C_RETRY_COUNT) {
+			v4l2_warn(sd, "Write: retry ... %d\n", retry);
+			retry++;
+			msleep_interruptible(10);
+			goto write_again;
+		}
+	}
+
+	return err;
+}
+
+/**
+ * tvp514x_write_regs() : Initializes a list of TVP5146/47 registers
+ * @sd: ptr to v4l2_subdev struct
+ * @reglist: list of TVP5146/47 registers and values
+ *
+ * Initializes a list of TVP5146/47 registers:-
+ *		if token is TOK_TERM, then entire write operation terminates
+ *		if token is TOK_DELAY, then a delay of 'val' msec is introduced
+ *		if token is TOK_SKIP, then the register write is skipped
+ *		if token is TOK_WRITE, then the register write is performed
+ * Returns zero if successful, or non-zero otherwise.
+ */
+static int tvp514x_write_regs(struct v4l2_subdev *sd,
+			      const struct tvp514x_reg reglist[])
+{
+	int err;
+	const struct tvp514x_reg *next = reglist;
+
+	for (; next->token != TOK_TERM; next++) {
+		if (next->token == TOK_DELAY) {
+			msleep(next->val);
+			continue;
+		}
+
+		if (next->token == TOK_SKIP)
+			continue;
+
+		err = tvp514x_write_reg(sd, next->reg, (u8) next->val);
+		if (err) {
+			v4l2_err(sd, "Write failed. Err[%d]\n", err);
+			return err;
+		}
+	}
+	return 0;
+}
+
+/**
+ * tvp514x_query_current_std() : Query the current standard detected by TVP5146/47
+ * @sd: ptr to v4l2_subdev struct
+ *
+ * Returns the current standard detected by TVP5146/47, STD_INVALID if there is no
+ * standard detected.
+ */
+static enum tvp514x_std tvp514x_query_current_std(struct v4l2_subdev *sd)
+{
+	u8 std, std_status;
+
+	std = tvp514x_read_reg(sd, REG_VIDEO_STD);
+	if ((std & VIDEO_STD_MASK) == VIDEO_STD_AUTO_SWITCH_BIT)
+		/* use the standard status register */
+		std_status = tvp514x_read_reg(sd, REG_VIDEO_STD_STATUS);
+	else
+		/* use the standard register itself */
+		std_status = std;
+
+	switch (std_status & VIDEO_STD_MASK) {
+	case VIDEO_STD_NTSC_MJ_BIT:
+		return STD_NTSC_MJ;
+
+	case VIDEO_STD_PAL_BDGHIN_BIT:
+		return STD_PAL_BDGHIN;
+
+	default:
+		return STD_INVALID;
+	}
+
+	return STD_INVALID;
+}
+
+/* TVP5146/47 register dump function */
+static void tvp514x_reg_dump(struct v4l2_subdev *sd)
+{
+	dump_reg(sd, REG_INPUT_SEL);
+	dump_reg(sd, REG_AFE_GAIN_CTRL);
+	dump_reg(sd, REG_VIDEO_STD);
+	dump_reg(sd, REG_OPERATION_MODE);
+	dump_reg(sd, REG_COLOR_KILLER);
+	dump_reg(sd, REG_LUMA_CONTROL1);
+	dump_reg(sd, REG_LUMA_CONTROL2);
+	dump_reg(sd, REG_LUMA_CONTROL3);
+	dump_reg(sd, REG_BRIGHTNESS);
+	dump_reg(sd, REG_CONTRAST);
+	dump_reg(sd, REG_SATURATION);
+	dump_reg(sd, REG_HUE);
+	dump_reg(sd, REG_CHROMA_CONTROL1);
+	dump_reg(sd, REG_CHROMA_CONTROL2);
+	dump_reg(sd, REG_COMP_PR_SATURATION);
+	dump_reg(sd, REG_COMP_Y_CONTRAST);
+	dump_reg(sd, REG_COMP_PB_SATURATION);
+	dump_reg(sd, REG_COMP_Y_BRIGHTNESS);
+	dump_reg(sd, REG_AVID_START_PIXEL_LSB);
+	dump_reg(sd, REG_AVID_START_PIXEL_MSB);
+	dump_reg(sd, REG_AVID_STOP_PIXEL_LSB);
+	dump_reg(sd, REG_AVID_STOP_PIXEL_MSB);
+	dump_reg(sd, REG_HSYNC_START_PIXEL_LSB);
+	dump_reg(sd, REG_HSYNC_START_PIXEL_MSB);
+	dump_reg(sd, REG_HSYNC_STOP_PIXEL_LSB);
+	dump_reg(sd, REG_HSYNC_STOP_PIXEL_MSB);
+	dump_reg(sd, REG_VSYNC_START_LINE_LSB);
+	dump_reg(sd, REG_VSYNC_START_LINE_MSB);
+	dump_reg(sd, REG_VSYNC_STOP_LINE_LSB);
+	dump_reg(sd, REG_VSYNC_STOP_LINE_MSB);
+	dump_reg(sd, REG_VBLK_START_LINE_LSB);
+	dump_reg(sd, REG_VBLK_START_LINE_MSB);
+	dump_reg(sd, REG_VBLK_STOP_LINE_LSB);
+	dump_reg(sd, REG_VBLK_STOP_LINE_MSB);
+	dump_reg(sd, REG_SYNC_CONTROL);
+	dump_reg(sd, REG_OUTPUT_FORMATTER1);
+	dump_reg(sd, REG_OUTPUT_FORMATTER2);
+	dump_reg(sd, REG_OUTPUT_FORMATTER3);
+	dump_reg(sd, REG_OUTPUT_FORMATTER4);
+	dump_reg(sd, REG_OUTPUT_FORMATTER5);
+	dump_reg(sd, REG_OUTPUT_FORMATTER6);
+	dump_reg(sd, REG_CLEAR_LOST_LOCK);
+}
+
+/**
+ * tvp514x_configure() - Configure the TVP5146/47 registers
+ * @sd: ptr to v4l2_subdev struct
+ * @decoder: ptr to tvp514x_decoder structure
+ *
+ * Returns zero if successful, or non-zero otherwise.
+ */
+static int tvp514x_configure(struct v4l2_subdev *sd,
+		struct tvp514x_decoder *decoder)
+{
+	int err;
+
+	/* common register initialization */
+	err =
+	    tvp514x_write_regs(sd, decoder->tvp514x_regs);
+	if (err)
+		return err;
+
+	if (debug)
+		tvp514x_reg_dump(sd);
+
+	return 0;
+}
+
+/**
+ * tvp514x_detect() - Detect if an tvp514x is present, and if so which revision.
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @decoder: pointer to tvp514x_decoder structure
+ *
+ * A device is considered to be detected if the chip ID (LSB and MSB)
+ * registers match the expected values.
+ * Any value of the rom version register is accepted.
+ * Returns ENODEV error number if no device is detected, or zero
+ * if a device is detected.
+ */
+static int tvp514x_detect(struct v4l2_subdev *sd,
+		struct tvp514x_decoder *decoder)
+{
+	u8 chip_id_msb, chip_id_lsb, rom_ver;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	chip_id_msb = tvp514x_read_reg(sd, REG_CHIP_ID_MSB);
+	chip_id_lsb = tvp514x_read_reg(sd, REG_CHIP_ID_LSB);
+	rom_ver = tvp514x_read_reg(sd, REG_ROM_VERSION);
+
+	v4l2_dbg(1, debug, sd,
+		 "chip id detected msb:0x%x lsb:0x%x rom version:0x%x\n",
+		 chip_id_msb, chip_id_lsb, rom_ver);
+	if ((chip_id_msb != TVP514X_CHIP_ID_MSB)
+		|| ((chip_id_lsb != TVP5146_CHIP_ID_LSB)
+		&& (chip_id_lsb != TVP5147_CHIP_ID_LSB))) {
+		/* We didn't read the values we expected, so this must not be
+		 * an TVP5146/47.
+		 */
+		v4l2_err(sd, "chip id mismatch msb:0x%x lsb:0x%x\n",
+				chip_id_msb, chip_id_lsb);
+		return -ENODEV;
+	}
+
+	decoder->ver = rom_ver;
+
+	v4l2_info(sd, "%s (Version - 0x%.2x) found at 0x%x (%s)\n",
+			client->name, decoder->ver,
+			client->addr << 1, client->adapter->name);
+	return 0;
+}
+
+/**
+ * tvp514x_querystd() - V4L2 decoder interface handler for querystd
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @std_id: standard V4L2 std_id ioctl enum
+ *
+ * Returns the current standard detected by TVP5146/47. If no active input is
+ * detected then *std_id is set to 0 and the function returns 0.
+ */
+static int tvp514x_querystd(struct v4l2_subdev *sd, v4l2_std_id *std_id)
+{
+	struct tvp514x_decoder *decoder = to_decoder(sd);
+	enum tvp514x_std current_std;
+	enum tvp514x_input input_sel;
+	u8 sync_lock_status, lock_mask;
+
+	if (std_id == NULL)
+		return -EINVAL;
+
+	/* To query the standard the TVP514x must power on the ADCs. */
+	if (!decoder->streaming) {
+		tvp514x_s_stream(sd, 1);
+		msleep(LOCK_RETRY_DELAY);
+	}
+
+	/* query the current standard */
+	current_std = tvp514x_query_current_std(sd);
+	if (current_std == STD_INVALID) {
+		*std_id = V4L2_STD_UNKNOWN;
+		return 0;
+	}
+
+	input_sel = decoder->input;
+
+	switch (input_sel) {
+	case INPUT_CVBS_VI1A:
+	case INPUT_CVBS_VI1B:
+	case INPUT_CVBS_VI1C:
+	case INPUT_CVBS_VI2A:
+	case INPUT_CVBS_VI2B:
+	case INPUT_CVBS_VI2C:
+	case INPUT_CVBS_VI3A:
+	case INPUT_CVBS_VI3B:
+	case INPUT_CVBS_VI3C:
+	case INPUT_CVBS_VI4A:
+		lock_mask = STATUS_CLR_SUBCAR_LOCK_BIT |
+			STATUS_HORZ_SYNC_LOCK_BIT |
+			STATUS_VIRT_SYNC_LOCK_BIT;
+		break;
+
+	case INPUT_SVIDEO_VI2A_VI1A:
+	case INPUT_SVIDEO_VI2B_VI1B:
+	case INPUT_SVIDEO_VI2C_VI1C:
+	case INPUT_SVIDEO_VI2A_VI3A:
+	case INPUT_SVIDEO_VI2B_VI3B:
+	case INPUT_SVIDEO_VI2C_VI3C:
+	case INPUT_SVIDEO_VI4A_VI1A:
+	case INPUT_SVIDEO_VI4A_VI1B:
+	case INPUT_SVIDEO_VI4A_VI1C:
+	case INPUT_SVIDEO_VI4A_VI3A:
+	case INPUT_SVIDEO_VI4A_VI3B:
+	case INPUT_SVIDEO_VI4A_VI3C:
+		lock_mask = STATUS_HORZ_SYNC_LOCK_BIT |
+			STATUS_VIRT_SYNC_LOCK_BIT;
+		break;
+		/*Need to add other interfaces*/
+	default:
+		return -EINVAL;
+	}
+	/* check whether signal is locked */
+	sync_lock_status = tvp514x_read_reg(sd, REG_STATUS1);
+	if (lock_mask != (sync_lock_status & lock_mask)) {
+		*std_id = V4L2_STD_UNKNOWN;
+		return 0;	/* No input detected */
+	}
+
+	*std_id &= decoder->std_list[current_std].standard.id;
+
+	v4l2_dbg(1, debug, sd, "Current STD: %s\n",
+			decoder->std_list[current_std].standard.name);
+	return 0;
+}
+
+/**
+ * tvp514x_s_std() - V4L2 decoder interface handler for s_std
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @std_id: standard V4L2 v4l2_std_id ioctl enum
+ *
+ * If std_id is supported, sets the requested standard. Otherwise, returns
+ * -EINVAL
+ */
+static int tvp514x_s_std(struct v4l2_subdev *sd, v4l2_std_id std_id)
+{
+	struct tvp514x_decoder *decoder = to_decoder(sd);
+	int err, i;
+
+	for (i = 0; i < decoder->num_stds; i++)
+		if (std_id & decoder->std_list[i].standard.id)
+			break;
+
+	if ((i == decoder->num_stds) || (i == STD_INVALID))
+		return -EINVAL;
+
+	err = tvp514x_write_reg(sd, REG_VIDEO_STD,
+				decoder->std_list[i].video_std);
+	if (err)
+		return err;
+
+	decoder->current_std = i;
+	decoder->tvp514x_regs[REG_VIDEO_STD].val =
+		decoder->std_list[i].video_std;
+
+	v4l2_dbg(1, debug, sd, "Standard set to: %s\n",
+			decoder->std_list[i].standard.name);
+	return 0;
+}
+
+/**
+ * tvp514x_s_routing() - V4L2 decoder interface handler for s_routing
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @input: input selector for routing the signal
+ * @output: output selector for routing the signal
+ * @config: config value. Not used
+ *
+ * If index is valid, selects the requested input. Otherwise, returns -EINVAL if
+ * the input is not supported or there is no active signal present in the
+ * selected input.
+ */
+static int tvp514x_s_routing(struct v4l2_subdev *sd,
+				u32 input, u32 output, u32 config)
+{
+	struct tvp514x_decoder *decoder = to_decoder(sd);
+	int err;
+	enum tvp514x_input input_sel;
+	enum tvp514x_output output_sel;
+
+	if ((input >= INPUT_INVALID) ||
+			(output >= OUTPUT_INVALID))
+		/* Index out of bound */
+		return -EINVAL;
+
+	input_sel = input;
+	output_sel = output;
+
+	err = tvp514x_write_reg(sd, REG_INPUT_SEL, input_sel);
+	if (err)
+		return err;
+
+	output_sel |= tvp514x_read_reg(sd,
+			REG_OUTPUT_FORMATTER1) & 0x7;
+	err = tvp514x_write_reg(sd, REG_OUTPUT_FORMATTER1,
+			output_sel);
+	if (err)
+		return err;
+
+	decoder->tvp514x_regs[REG_INPUT_SEL].val = input_sel;
+	decoder->tvp514x_regs[REG_OUTPUT_FORMATTER1].val = output_sel;
+	decoder->input = input;
+	decoder->output = output;
+
+	v4l2_dbg(1, debug, sd, "Input set to: %d\n", input_sel);
+
+	return 0;
+}
+
+/**
+ * tvp514x_s_ctrl() - V4L2 decoder interface handler for s_ctrl
+ * @ctrl: pointer to v4l2_ctrl structure
+ *
+ * If the requested control is supported, sets the control's current
+ * value in HW. Otherwise, returns -EINVAL if the control is not supported.
+ */
+static int tvp514x_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+	struct tvp514x_decoder *decoder = to_decoder(sd);
+	int err = -EINVAL, value;
+
+	value = ctrl->val;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		err = tvp514x_write_reg(sd, REG_BRIGHTNESS, value);
+		if (!err)
+			decoder->tvp514x_regs[REG_BRIGHTNESS].val = value;
+		break;
+	case V4L2_CID_CONTRAST:
+		err = tvp514x_write_reg(sd, REG_CONTRAST, value);
+		if (!err)
+			decoder->tvp514x_regs[REG_CONTRAST].val = value;
+		break;
+	case V4L2_CID_SATURATION:
+		err = tvp514x_write_reg(sd, REG_SATURATION, value);
+		if (!err)
+			decoder->tvp514x_regs[REG_SATURATION].val = value;
+		break;
+	case V4L2_CID_HUE:
+		if (value == 180)
+			value = 0x7F;
+		else if (value == -180)
+			value = 0x80;
+		err = tvp514x_write_reg(sd, REG_HUE, value);
+		if (!err)
+			decoder->tvp514x_regs[REG_HUE].val = value;
+		break;
+	case V4L2_CID_AUTOGAIN:
+		err = tvp514x_write_reg(sd, REG_AFE_GAIN_CTRL, value ? 0x0f : 0x0c);
+		if (!err)
+			decoder->tvp514x_regs[REG_AFE_GAIN_CTRL].val = value;
+		break;
+	}
+
+	v4l2_dbg(1, debug, sd, "Set Control: ID - %d - %d\n",
+			ctrl->id, ctrl->val);
+	return err;
+}
+
+/**
+ * tvp514x_g_frame_interval() - V4L2 decoder interface handler
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @ival: pointer to a v4l2_subdev_frame_interval structure
+ *
+ * Returns the decoder's video CAPTURE parameters.
+ */
+static int
+tvp514x_g_frame_interval(struct v4l2_subdev *sd,
+			 struct v4l2_subdev_frame_interval *ival)
+{
+	struct tvp514x_decoder *decoder = to_decoder(sd);
+	enum tvp514x_std current_std;
+
+
+	/* get the current standard */
+	current_std = decoder->current_std;
+
+	ival->interval =
+		decoder->std_list[current_std].standard.frameperiod;
+
+	return 0;
+}
+
+/**
+ * tvp514x_s_frame_interval() - V4L2 decoder interface handler
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @ival: pointer to a v4l2_subdev_frame_interval structure
+ *
+ * Configures the decoder to use the input parameters, if possible. If
+ * not possible, returns the appropriate error code.
+ */
+static int
+tvp514x_s_frame_interval(struct v4l2_subdev *sd,
+			 struct v4l2_subdev_frame_interval *ival)
+{
+	struct tvp514x_decoder *decoder = to_decoder(sd);
+	struct v4l2_fract *timeperframe;
+	enum tvp514x_std current_std;
+
+
+	timeperframe = &ival->interval;
+
+	/* get the current standard */
+	current_std = decoder->current_std;
+
+	*timeperframe =
+	    decoder->std_list[current_std].standard.frameperiod;
+
+	return 0;
+}
+
+/**
+ * tvp514x_s_stream() - V4L2 decoder i/f handler for s_stream
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @enable: streaming enable or disable
+ *
+ * Sets streaming to enable or disable, if possible.
+ */
+static int tvp514x_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	int err = 0;
+	struct tvp514x_decoder *decoder = to_decoder(sd);
+
+	if (decoder->streaming == enable)
+		return 0;
+
+	switch (enable) {
+	case 0:
+	{
+		/* Power Down Sequence */
+		err = tvp514x_write_reg(sd, REG_OPERATION_MODE, 0x01);
+		if (err) {
+			v4l2_err(sd, "Unable to turn off decoder\n");
+			return err;
+		}
+		decoder->streaming = enable;
+		break;
+	}
+	case 1:
+	{
+		/* Power Up Sequence */
+		err = tvp514x_write_regs(sd, decoder->int_seq);
+		if (err) {
+			v4l2_err(sd, "Unable to turn on decoder\n");
+			return err;
+		}
+		/* Detect if not already detected */
+		err = tvp514x_detect(sd, decoder);
+		if (err) {
+			v4l2_err(sd, "Unable to detect decoder\n");
+			return err;
+		}
+		err = tvp514x_configure(sd, decoder);
+		if (err) {
+			v4l2_err(sd, "Unable to configure decoder\n");
+			return err;
+		}
+		decoder->streaming = enable;
+		break;
+	}
+	default:
+		err = -ENODEV;
+		break;
+	}
+
+	return err;
+}
+
+static const struct v4l2_ctrl_ops tvp514x_ctrl_ops = {
+	.s_ctrl = tvp514x_s_ctrl,
+};
+
+/**
+ * tvp514x_enum_mbus_code() - V4L2 decoder interface handler for enum_mbus_code
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @sd_state: subdev state
+ * @code: pointer to v4l2_subdev_mbus_code_enum structure
+ *
+ * Enumertaes mbus codes supported
+ */
+static int tvp514x_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	u32 pad = code->pad;
+	u32 index = code->index;
+
+	memset(code, 0, sizeof(*code));
+	code->index = index;
+	code->pad = pad;
+
+	if (index != 0)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_UYVY8_2X8;
+
+	return 0;
+}
+
+/**
+ * tvp514x_get_pad_format() - V4L2 decoder interface handler for get pad format
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @sd_state: subdev state
+ * @format: pointer to v4l2_subdev_format structure
+ *
+ * Retrieves pad format which is active or tried based on requirement
+ */
+static int tvp514x_get_pad_format(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_format *format)
+{
+	struct tvp514x_decoder *decoder = to_decoder(sd);
+	__u32 which = format->which;
+
+	if (format->pad)
+		return -EINVAL;
+
+	if (which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		format->format = decoder->format;
+		return 0;
+	}
+
+	format->format.code = MEDIA_BUS_FMT_UYVY8_2X8;
+	format->format.width = tvp514x_std_list[decoder->current_std].width;
+	format->format.height = tvp514x_std_list[decoder->current_std].height;
+	format->format.colorspace = V4L2_COLORSPACE_SMPTE170M;
+	format->format.field = V4L2_FIELD_INTERLACED;
+
+	return 0;
+}
+
+/**
+ * tvp514x_set_pad_format() - V4L2 decoder interface handler for set pad format
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @sd_state: subdev state
+ * @fmt: pointer to v4l2_subdev_format structure
+ *
+ * Set pad format for the output pad
+ */
+static int tvp514x_set_pad_format(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_format *fmt)
+{
+	struct tvp514x_decoder *decoder = to_decoder(sd);
+
+	if (fmt->format.field != V4L2_FIELD_INTERLACED ||
+	    fmt->format.code != MEDIA_BUS_FMT_UYVY8_2X8 ||
+	    fmt->format.colorspace != V4L2_COLORSPACE_SMPTE170M ||
+	    fmt->format.width != tvp514x_std_list[decoder->current_std].width ||
+	    fmt->format.height != tvp514x_std_list[decoder->current_std].height)
+		return -EINVAL;
+
+	decoder->format = fmt->format;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops tvp514x_video_ops = {
+	.s_std = tvp514x_s_std,
+	.s_routing = tvp514x_s_routing,
+	.querystd = tvp514x_querystd,
+	.g_frame_interval = tvp514x_g_frame_interval,
+	.s_frame_interval = tvp514x_s_frame_interval,
+	.s_stream = tvp514x_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops tvp514x_pad_ops = {
+	.enum_mbus_code = tvp514x_enum_mbus_code,
+	.get_fmt = tvp514x_get_pad_format,
+	.set_fmt = tvp514x_set_pad_format,
+};
+
+static const struct v4l2_subdev_ops tvp514x_ops = {
+	.video = &tvp514x_video_ops,
+	.pad = &tvp514x_pad_ops,
+};
+
+static const struct tvp514x_decoder tvp514x_dev = {
+	.streaming = 0,
+	.fmt_list = tvp514x_fmt_list,
+	.num_fmts = ARRAY_SIZE(tvp514x_fmt_list),
+	.pix = {
+		/* Default to NTSC 8-bit YUV 422 */
+		.width		= NTSC_NUM_ACTIVE_PIXELS,
+		.height		= NTSC_NUM_ACTIVE_LINES,
+		.pixelformat	= V4L2_PIX_FMT_UYVY,
+		.field		= V4L2_FIELD_INTERLACED,
+		.bytesperline	= NTSC_NUM_ACTIVE_PIXELS * 2,
+		.sizeimage	= NTSC_NUM_ACTIVE_PIXELS * 2 *
+					NTSC_NUM_ACTIVE_LINES,
+		.colorspace	= V4L2_COLORSPACE_SMPTE170M,
+		},
+	.current_std = STD_NTSC_MJ,
+	.std_list = tvp514x_std_list,
+	.num_stds = ARRAY_SIZE(tvp514x_std_list),
+
+};
+
+static struct tvp514x_platform_data *
+tvp514x_get_pdata(struct i2c_client *client)
+{
+	struct tvp514x_platform_data *pdata = NULL;
+	struct v4l2_fwnode_endpoint bus_cfg = { .bus_type = 0 };
+	struct device_node *endpoint;
+	unsigned int flags;
+
+	if (!IS_ENABLED(CONFIG_OF) || !client->dev.of_node)
+		return client->dev.platform_data;
+
+	endpoint = of_graph_get_next_endpoint(client->dev.of_node, NULL);
+	if (!endpoint)
+		return NULL;
+
+	if (v4l2_fwnode_endpoint_parse(of_fwnode_handle(endpoint), &bus_cfg))
+		goto done;
+
+	pdata = devm_kzalloc(&client->dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		goto done;
+
+	flags = bus_cfg.bus.parallel.flags;
+
+	if (flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
+		pdata->hs_polarity = 1;
+
+	if (flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH)
+		pdata->vs_polarity = 1;
+
+	if (flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
+		pdata->clk_polarity = 1;
+
+done:
+	of_node_put(endpoint);
+	return pdata;
+}
+
+/**
+ * tvp514x_probe() - decoder driver i2c probe handler
+ * @client: i2c driver client device structure
+ *
+ * Register decoder as an i2c client device and V4L2
+ * device.
+ */
+static int
+tvp514x_probe(struct i2c_client *client)
+{
+	const struct i2c_device_id *id = i2c_client_get_device_id(client);
+	struct tvp514x_platform_data *pdata = tvp514x_get_pdata(client);
+	struct tvp514x_decoder *decoder;
+	struct v4l2_subdev *sd;
+	int ret;
+
+	if (pdata == NULL) {
+		dev_err(&client->dev, "No platform data\n");
+		return -EINVAL;
+	}
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+
+	decoder = devm_kzalloc(&client->dev, sizeof(*decoder), GFP_KERNEL);
+	if (!decoder)
+		return -ENOMEM;
+
+	/* Initialize the tvp514x_decoder with default configuration */
+	*decoder = tvp514x_dev;
+	/* Copy default register configuration */
+	memcpy(decoder->tvp514x_regs, tvp514x_reg_list_default,
+			sizeof(tvp514x_reg_list_default));
+
+	decoder->int_seq = (struct tvp514x_reg *)id->driver_data;
+
+	/* Copy board specific information here */
+	decoder->pdata = pdata;
+
+	/**
+	 * Fetch platform specific data, and configure the
+	 * tvp514x_reg_list[] accordingly. Since this is one
+	 * time configuration, no need to preserve.
+	 */
+	decoder->tvp514x_regs[REG_OUTPUT_FORMATTER2].val |=
+		(decoder->pdata->clk_polarity << 1);
+	decoder->tvp514x_regs[REG_SYNC_CONTROL].val |=
+		((decoder->pdata->hs_polarity << 2) |
+		 (decoder->pdata->vs_polarity << 3));
+	/* Set default standard to auto */
+	decoder->tvp514x_regs[REG_VIDEO_STD].val =
+		VIDEO_STD_AUTO_SWITCH_BIT;
+
+	/* Register with V4L2 layer as slave device */
+	sd = &decoder->sd;
+	v4l2_i2c_subdev_init(sd, client, &tvp514x_ops);
+
+#if defined(CONFIG_MEDIA_CONTROLLER)
+	decoder->pad.flags = MEDIA_PAD_FL_SOURCE;
+	decoder->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	decoder->sd.entity.function = MEDIA_ENT_F_ATV_DECODER;
+
+	ret = media_entity_pads_init(&decoder->sd.entity, 1, &decoder->pad);
+	if (ret < 0) {
+		v4l2_err(sd, "%s decoder driver failed to register !!\n",
+			 sd->name);
+		return ret;
+	}
+#endif
+	v4l2_ctrl_handler_init(&decoder->hdl, 5);
+	v4l2_ctrl_new_std(&decoder->hdl, &tvp514x_ctrl_ops,
+		V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(&decoder->hdl, &tvp514x_ctrl_ops,
+		V4L2_CID_CONTRAST, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(&decoder->hdl, &tvp514x_ctrl_ops,
+		V4L2_CID_SATURATION, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(&decoder->hdl, &tvp514x_ctrl_ops,
+		V4L2_CID_HUE, -180, 180, 180, 0);
+	v4l2_ctrl_new_std(&decoder->hdl, &tvp514x_ctrl_ops,
+		V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+	sd->ctrl_handler = &decoder->hdl;
+	if (decoder->hdl.error) {
+		ret = decoder->hdl.error;
+		goto done;
+	}
+	v4l2_ctrl_handler_setup(&decoder->hdl);
+
+	ret = v4l2_async_register_subdev(&decoder->sd);
+	if (!ret)
+		v4l2_info(sd, "%s decoder driver registered !!\n", sd->name);
+
+done:
+	if (ret < 0) {
+		v4l2_ctrl_handler_free(&decoder->hdl);
+		media_entity_cleanup(&decoder->sd.entity);
+	}
+	return ret;
+}
+
+/**
+ * tvp514x_remove() - decoder driver i2c remove handler
+ * @client: i2c driver client device structure
+ *
+ * Unregister decoder as an i2c client device and V4L2
+ * device. Complement of tvp514x_probe().
+ */
+static void tvp514x_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct tvp514x_decoder *decoder = to_decoder(sd);
+
+	v4l2_async_unregister_subdev(&decoder->sd);
+	media_entity_cleanup(&decoder->sd.entity);
+	v4l2_ctrl_handler_free(&decoder->hdl);
+}
+/* TVP5146 Init/Power on Sequence */
+static const struct tvp514x_reg tvp5146_init_reg_seq[] = {
+	{TOK_WRITE, REG_VBUS_ADDRESS_ACCESS1, 0x02},
+	{TOK_WRITE, REG_VBUS_ADDRESS_ACCESS2, 0x00},
+	{TOK_WRITE, REG_VBUS_ADDRESS_ACCESS3, 0x80},
+	{TOK_WRITE, REG_VBUS_DATA_ACCESS_NO_VBUS_ADDR_INCR, 0x01},
+	{TOK_WRITE, REG_VBUS_ADDRESS_ACCESS1, 0x60},
+	{TOK_WRITE, REG_VBUS_ADDRESS_ACCESS2, 0x00},
+	{TOK_WRITE, REG_VBUS_ADDRESS_ACCESS3, 0xB0},
+	{TOK_WRITE, REG_VBUS_DATA_ACCESS_NO_VBUS_ADDR_INCR, 0x01},
+	{TOK_WRITE, REG_VBUS_DATA_ACCESS_NO_VBUS_ADDR_INCR, 0x00},
+	{TOK_WRITE, REG_OPERATION_MODE, 0x01},
+	{TOK_WRITE, REG_OPERATION_MODE, 0x00},
+	{TOK_TERM, 0, 0},
+};
+
+/* TVP5147 Init/Power on Sequence */
+static const struct tvp514x_reg tvp5147_init_reg_seq[] =	{
+	{TOK_WRITE, REG_VBUS_ADDRESS_ACCESS1, 0x02},
+	{TOK_WRITE, REG_VBUS_ADDRESS_ACCESS2, 0x00},
+	{TOK_WRITE, REG_VBUS_ADDRESS_ACCESS3, 0x80},
+	{TOK_WRITE, REG_VBUS_DATA_ACCESS_NO_VBUS_ADDR_INCR, 0x01},
+	{TOK_WRITE, REG_VBUS_ADDRESS_ACCESS1, 0x60},
+	{TOK_WRITE, REG_VBUS_ADDRESS_ACCESS2, 0x00},
+	{TOK_WRITE, REG_VBUS_ADDRESS_ACCESS3, 0xB0},
+	{TOK_WRITE, REG_VBUS_DATA_ACCESS_NO_VBUS_ADDR_INCR, 0x01},
+	{TOK_WRITE, REG_VBUS_ADDRESS_ACCESS1, 0x16},
+	{TOK_WRITE, REG_VBUS_ADDRESS_ACCESS2, 0x00},
+	{TOK_WRITE, REG_VBUS_ADDRESS_ACCESS3, 0xA0},
+	{TOK_WRITE, REG_VBUS_DATA_ACCESS_NO_VBUS_ADDR_INCR, 0x16},
+	{TOK_WRITE, REG_VBUS_ADDRESS_ACCESS1, 0x60},
+	{TOK_WRITE, REG_VBUS_ADDRESS_ACCESS2, 0x00},
+	{TOK_WRITE, REG_VBUS_ADDRESS_ACCESS3, 0xB0},
+	{TOK_WRITE, REG_VBUS_DATA_ACCESS_NO_VBUS_ADDR_INCR, 0x00},
+	{TOK_WRITE, REG_OPERATION_MODE, 0x01},
+	{TOK_WRITE, REG_OPERATION_MODE, 0x00},
+	{TOK_TERM, 0, 0},
+};
+
+/* TVP5146M2/TVP5147M1 Init/Power on Sequence */
+static const struct tvp514x_reg tvp514xm_init_reg_seq[] = {
+	{TOK_WRITE, REG_OPERATION_MODE, 0x01},
+	{TOK_WRITE, REG_OPERATION_MODE, 0x00},
+	{TOK_TERM, 0, 0},
+};
+
+/*
+ * I2C Device Table -
+ *
+ * name - Name of the actual device/chip.
+ * driver_data - Driver data
+ */
+static const struct i2c_device_id tvp514x_id[] = {
+	{"tvp5146", (unsigned long)tvp5146_init_reg_seq},
+	{"tvp5146m2", (unsigned long)tvp514xm_init_reg_seq},
+	{"tvp5147", (unsigned long)tvp5147_init_reg_seq},
+	{"tvp5147m1", (unsigned long)tvp514xm_init_reg_seq},
+	{},
+};
+
+MODULE_DEVICE_TABLE(i2c, tvp514x_id);
+
+#if IS_ENABLED(CONFIG_OF)
+static const struct of_device_id tvp514x_of_match[] = {
+	{ .compatible = "ti,tvp5146", },
+	{ .compatible = "ti,tvp5146m2", },
+	{ .compatible = "ti,tvp5147", },
+	{ .compatible = "ti,tvp5147m1", },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, tvp514x_of_match);
+#endif
+
+static struct i2c_driver tvp514x_driver = {
+	.driver = {
+		.of_match_table = of_match_ptr(tvp514x_of_match),
+		.name = TVP514X_MODULE_NAME,
+	},
+	.probe = tvp514x_probe,
+	.remove = tvp514x_remove,
+	.id_table = tvp514x_id,
+};
+
+module_i2c_driver(tvp514x_driver);
diff --git a/drivers/media/i2c/tvp514x_regs.h b/drivers/media/i2c/tvp514x_regs.h
new file mode 100644
index 000000000..b452725d5
--- /dev/null
+++ b/drivers/media/i2c/tvp514x_regs.h
@@ -0,0 +1,274 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * drivers/media/i2c/tvp514x_regs.h
+ *
+ * Copyright (C) 2008 Texas Instruments Inc
+ * Author: Vaibhav Hiremath <hvaibhav@ti.com>
+ *
+ * Contributors:
+ *     Sivaraj R <sivaraj@ti.com>
+ *     Brijesh R Jadav <brijesh.j@ti.com>
+ *     Hardik Shah <hardik.shah@ti.com>
+ *     Manjunath Hadli <mrh@ti.com>
+ *     Karicheri Muralidharan <m-karicheri2@ti.com>
+ */
+
+#ifndef _TVP514X_REGS_H
+#define _TVP514X_REGS_H
+
+/*
+ * TVP5146/47 registers
+ */
+#define REG_INPUT_SEL			(0x00)
+#define REG_AFE_GAIN_CTRL		(0x01)
+#define REG_VIDEO_STD			(0x02)
+#define REG_OPERATION_MODE		(0x03)
+#define REG_AUTOSWITCH_MASK		(0x04)
+
+#define REG_COLOR_KILLER		(0x05)
+#define REG_LUMA_CONTROL1		(0x06)
+#define REG_LUMA_CONTROL2		(0x07)
+#define REG_LUMA_CONTROL3		(0x08)
+
+#define REG_BRIGHTNESS			(0x09)
+#define REG_CONTRAST			(0x0A)
+#define REG_SATURATION			(0x0B)
+#define REG_HUE				(0x0C)
+
+#define REG_CHROMA_CONTROL1		(0x0D)
+#define REG_CHROMA_CONTROL2		(0x0E)
+
+/* 0x0F Reserved */
+
+#define REG_COMP_PR_SATURATION		(0x10)
+#define REG_COMP_Y_CONTRAST		(0x11)
+#define REG_COMP_PB_SATURATION		(0x12)
+
+/* 0x13 Reserved */
+
+#define REG_COMP_Y_BRIGHTNESS		(0x14)
+
+/* 0x15 Reserved */
+
+#define REG_AVID_START_PIXEL_LSB	(0x16)
+#define REG_AVID_START_PIXEL_MSB	(0x17)
+#define REG_AVID_STOP_PIXEL_LSB		(0x18)
+#define REG_AVID_STOP_PIXEL_MSB		(0x19)
+
+#define REG_HSYNC_START_PIXEL_LSB	(0x1A)
+#define REG_HSYNC_START_PIXEL_MSB	(0x1B)
+#define REG_HSYNC_STOP_PIXEL_LSB	(0x1C)
+#define REG_HSYNC_STOP_PIXEL_MSB	(0x1D)
+
+#define REG_VSYNC_START_LINE_LSB	(0x1E)
+#define REG_VSYNC_START_LINE_MSB	(0x1F)
+#define REG_VSYNC_STOP_LINE_LSB		(0x20)
+#define REG_VSYNC_STOP_LINE_MSB		(0x21)
+
+#define REG_VBLK_START_LINE_LSB		(0x22)
+#define REG_VBLK_START_LINE_MSB		(0x23)
+#define REG_VBLK_STOP_LINE_LSB		(0x24)
+#define REG_VBLK_STOP_LINE_MSB		(0x25)
+
+/* 0x26 - 0x27 Reserved */
+
+#define REG_FAST_SWTICH_CONTROL		(0x28)
+
+/* 0x29 Reserved */
+
+#define REG_FAST_SWTICH_SCART_DELAY	(0x2A)
+
+/* 0x2B Reserved */
+
+#define REG_SCART_DELAY			(0x2C)
+#define REG_CTI_DELAY			(0x2D)
+#define REG_CTI_CONTROL			(0x2E)
+
+/* 0x2F - 0x31 Reserved */
+
+#define REG_SYNC_CONTROL		(0x32)
+#define REG_OUTPUT_FORMATTER1		(0x33)
+#define REG_OUTPUT_FORMATTER2		(0x34)
+#define REG_OUTPUT_FORMATTER3		(0x35)
+#define REG_OUTPUT_FORMATTER4		(0x36)
+#define REG_OUTPUT_FORMATTER5		(0x37)
+#define REG_OUTPUT_FORMATTER6		(0x38)
+#define REG_CLEAR_LOST_LOCK		(0x39)
+
+#define REG_STATUS1			(0x3A)
+#define REG_STATUS2			(0x3B)
+
+#define REG_AGC_GAIN_STATUS_LSB		(0x3C)
+#define REG_AGC_GAIN_STATUS_MSB		(0x3D)
+
+/* 0x3E Reserved */
+
+#define REG_VIDEO_STD_STATUS		(0x3F)
+#define REG_GPIO_INPUT1			(0x40)
+#define REG_GPIO_INPUT2			(0x41)
+
+/* 0x42 - 0x45 Reserved */
+
+#define REG_AFE_COARSE_GAIN_CH1		(0x46)
+#define REG_AFE_COARSE_GAIN_CH2		(0x47)
+#define REG_AFE_COARSE_GAIN_CH3		(0x48)
+#define REG_AFE_COARSE_GAIN_CH4		(0x49)
+
+#define REG_AFE_FINE_GAIN_PB_B_LSB	(0x4A)
+#define REG_AFE_FINE_GAIN_PB_B_MSB	(0x4B)
+#define REG_AFE_FINE_GAIN_Y_G_CHROMA_LSB	(0x4C)
+#define REG_AFE_FINE_GAIN_Y_G_CHROMA_MSB	(0x4D)
+#define REG_AFE_FINE_GAIN_PR_R_LSB	(0x4E)
+#define REG_AFE_FINE_GAIN_PR_R_MSB	(0x4F)
+#define REG_AFE_FINE_GAIN_CVBS_LUMA_LSB	(0x50)
+#define REG_AFE_FINE_GAIN_CVBS_LUMA_MSB	(0x51)
+
+/* 0x52 - 0x68 Reserved */
+
+#define REG_FBIT_VBIT_CONTROL1		(0x69)
+
+/* 0x6A - 0x6B Reserved */
+
+#define REG_BACKEND_AGC_CONTROL		(0x6C)
+
+/* 0x6D - 0x6E Reserved */
+
+#define REG_AGC_DECREMENT_SPEED_CONTROL	(0x6F)
+#define REG_ROM_VERSION			(0x70)
+
+/* 0x71 - 0x73 Reserved */
+
+#define REG_AGC_WHITE_PEAK_PROCESSING	(0x74)
+#define REG_FBIT_VBIT_CONTROL2		(0x75)
+#define REG_VCR_TRICK_MODE_CONTROL	(0x76)
+#define REG_HORIZONTAL_SHAKE_INCREMENT	(0x77)
+#define REG_AGC_INCREMENT_SPEED		(0x78)
+#define REG_AGC_INCREMENT_DELAY		(0x79)
+
+/* 0x7A - 0x7F Reserved */
+
+#define REG_CHIP_ID_MSB			(0x80)
+#define REG_CHIP_ID_LSB			(0x81)
+
+/* 0x82 Reserved */
+
+#define REG_CPLL_SPEED_CONTROL		(0x83)
+
+/* 0x84 - 0x96 Reserved */
+
+#define REG_STATUS_REQUEST		(0x97)
+
+/* 0x98 - 0x99 Reserved */
+
+#define REG_VERTICAL_LINE_COUNT_LSB	(0x9A)
+#define REG_VERTICAL_LINE_COUNT_MSB	(0x9B)
+
+/* 0x9C - 0x9D Reserved */
+
+#define REG_AGC_DECREMENT_DELAY		(0x9E)
+
+/* 0x9F - 0xB0 Reserved */
+
+#define REG_VDP_TTX_FILTER_1_MASK1	(0xB1)
+#define REG_VDP_TTX_FILTER_1_MASK2	(0xB2)
+#define REG_VDP_TTX_FILTER_1_MASK3	(0xB3)
+#define REG_VDP_TTX_FILTER_1_MASK4	(0xB4)
+#define REG_VDP_TTX_FILTER_1_MASK5	(0xB5)
+#define REG_VDP_TTX_FILTER_2_MASK1	(0xB6)
+#define REG_VDP_TTX_FILTER_2_MASK2	(0xB7)
+#define REG_VDP_TTX_FILTER_2_MASK3	(0xB8)
+#define REG_VDP_TTX_FILTER_2_MASK4	(0xB9)
+#define REG_VDP_TTX_FILTER_2_MASK5	(0xBA)
+#define REG_VDP_TTX_FILTER_CONTROL	(0xBB)
+#define REG_VDP_FIFO_WORD_COUNT		(0xBC)
+#define REG_VDP_FIFO_INTERRUPT_THRLD	(0xBD)
+
+/* 0xBE Reserved */
+
+#define REG_VDP_FIFO_RESET		(0xBF)
+#define REG_VDP_FIFO_OUTPUT_CONTROL	(0xC0)
+#define REG_VDP_LINE_NUMBER_INTERRUPT	(0xC1)
+#define REG_VDP_PIXEL_ALIGNMENT_LSB	(0xC2)
+#define REG_VDP_PIXEL_ALIGNMENT_MSB	(0xC3)
+
+/* 0xC4 - 0xD5 Reserved */
+
+#define REG_VDP_LINE_START		(0xD6)
+#define REG_VDP_LINE_STOP		(0xD7)
+#define REG_VDP_GLOBAL_LINE_MODE	(0xD8)
+#define REG_VDP_FULL_FIELD_ENABLE	(0xD9)
+#define REG_VDP_FULL_FIELD_MODE		(0xDA)
+
+/* 0xDB - 0xDF Reserved */
+
+#define REG_VBUS_DATA_ACCESS_NO_VBUS_ADDR_INCR	(0xE0)
+#define REG_VBUS_DATA_ACCESS_VBUS_ADDR_INCR	(0xE1)
+#define REG_FIFO_READ_DATA			(0xE2)
+
+/* 0xE3 - 0xE7 Reserved */
+
+#define REG_VBUS_ADDRESS_ACCESS1	(0xE8)
+#define REG_VBUS_ADDRESS_ACCESS2	(0xE9)
+#define REG_VBUS_ADDRESS_ACCESS3	(0xEA)
+
+/* 0xEB - 0xEF Reserved */
+
+#define REG_INTERRUPT_RAW_STATUS0	(0xF0)
+#define REG_INTERRUPT_RAW_STATUS1	(0xF1)
+#define REG_INTERRUPT_STATUS0		(0xF2)
+#define REG_INTERRUPT_STATUS1		(0xF3)
+#define REG_INTERRUPT_MASK0		(0xF4)
+#define REG_INTERRUPT_MASK1		(0xF5)
+#define REG_INTERRUPT_CLEAR0		(0xF6)
+#define REG_INTERRUPT_CLEAR1		(0xF7)
+
+/* 0xF8 - 0xFF Reserved */
+
+/*
+ * Mask and bit definitions of TVP5146/47 registers
+ */
+/* The ID values we are looking for */
+#define TVP514X_CHIP_ID_MSB		(0x51)
+#define TVP5146_CHIP_ID_LSB		(0x46)
+#define TVP5147_CHIP_ID_LSB		(0x47)
+
+#define VIDEO_STD_MASK			(0x07)
+#define VIDEO_STD_AUTO_SWITCH_BIT	(0x00)
+#define VIDEO_STD_NTSC_MJ_BIT		(0x01)
+#define VIDEO_STD_PAL_BDGHIN_BIT	(0x02)
+#define VIDEO_STD_PAL_M_BIT		(0x03)
+#define VIDEO_STD_PAL_COMBINATION_N_BIT	(0x04)
+#define VIDEO_STD_NTSC_4_43_BIT		(0x05)
+#define VIDEO_STD_SECAM_BIT		(0x06)
+#define VIDEO_STD_PAL_60_BIT		(0x07)
+
+/*
+ * Status bit
+ */
+#define STATUS_TV_VCR_BIT		(1<<0)
+#define STATUS_HORZ_SYNC_LOCK_BIT	(1<<1)
+#define STATUS_VIRT_SYNC_LOCK_BIT	(1<<2)
+#define STATUS_CLR_SUBCAR_LOCK_BIT	(1<<3)
+#define STATUS_LOST_LOCK_DETECT_BIT	(1<<4)
+#define STATUS_FEILD_RATE_BIT		(1<<5)
+#define STATUS_LINE_ALTERNATING_BIT	(1<<6)
+#define STATUS_PEAK_WHITE_DETECT_BIT	(1<<7)
+
+/* Tokens for register write */
+#define TOK_WRITE                       (0)     /* token for write operation */
+#define TOK_TERM                        (1)     /* terminating token */
+#define TOK_DELAY                       (2)     /* delay token for reg list */
+#define TOK_SKIP                        (3)     /* token to skip a register */
+/**
+ * struct tvp514x_reg - Structure for TVP5146/47 register initialization values
+ * @token: Token: TOK_WRITE, TOK_TERM etc..
+ * @reg: Register offset
+ * @val: Register Value for TOK_WRITE or delay in ms for TOK_DELAY
+ */
+struct tvp514x_reg {
+	u8 token;
+	u8 reg;
+	u32 val;
+};
+
+#endif				/* ifndef _TVP514X_REGS_H */
diff --git a/drivers/media/i2c/tvp5150.c b/drivers/media/i2c/tvp5150.c
new file mode 100644
index 000000000..e543b3f7a
--- /dev/null
+++ b/drivers/media/i2c/tvp5150.c
@@ -0,0 +1,2292 @@
+// SPDX-License-Identifier: GPL-2.0
+//
+// tvp5150 - Texas Instruments TVP5150A/AM1 and TVP5151 video decoder driver
+//
+// Copyright (c) 2005,2006 Mauro Carvalho Chehab <mchehab@kernel.org>
+
+#include <dt-bindings/media/tvp5150.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/of_graph.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-mc.h>
+#include <media/v4l2-rect.h>
+
+#include "tvp5150_reg.h"
+
+#define TVP5150_H_MAX		720U
+#define TVP5150_V_MAX_525_60	480U
+#define TVP5150_V_MAX_OTHERS	576U
+#define TVP5150_MAX_CROP_LEFT	511
+#define TVP5150_MAX_CROP_TOP	127
+#define TVP5150_CROP_SHIFT	2
+#define TVP5150_MBUS_FMT	MEDIA_BUS_FMT_UYVY8_2X8
+#define TVP5150_FIELD		V4L2_FIELD_ALTERNATE
+#define TVP5150_COLORSPACE	V4L2_COLORSPACE_SMPTE170M
+#define TVP5150_STD_MASK	(V4L2_STD_NTSC     | \
+				 V4L2_STD_NTSC_443 | \
+				 V4L2_STD_PAL      | \
+				 V4L2_STD_PAL_M    | \
+				 V4L2_STD_PAL_N    | \
+				 V4L2_STD_PAL_Nc   | \
+				 V4L2_STD_SECAM)
+
+#define TVP5150_MAX_CONNECTORS	3 /* Check dt-bindings for more information */
+
+MODULE_DESCRIPTION("Texas Instruments TVP5150A/TVP5150AM1/TVP5151 video decoder driver");
+MODULE_AUTHOR("Mauro Carvalho Chehab");
+MODULE_LICENSE("GPL v2");
+
+
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "Debug level (0-2)");
+
+#define dprintk0(__dev, __arg...) dev_dbg_lvl(__dev, 0, 0, __arg)
+
+enum tvp5150_pads {
+	TVP5150_PAD_AIP1A,
+	TVP5150_PAD_AIP1B,
+	TVP5150_PAD_VID_OUT,
+	TVP5150_NUM_PADS
+};
+
+struct tvp5150_connector {
+	struct v4l2_fwnode_connector base;
+	struct media_entity ent;
+	struct media_pad pad;
+};
+
+struct tvp5150 {
+	struct v4l2_subdev sd;
+
+	struct media_pad pads[TVP5150_NUM_PADS];
+	struct tvp5150_connector connectors[TVP5150_MAX_CONNECTORS];
+	struct tvp5150_connector *cur_connector;
+	unsigned int connectors_num;
+
+	struct v4l2_ctrl_handler hdl;
+	struct v4l2_rect rect;
+	struct regmap *regmap;
+	int irq;
+
+	v4l2_std_id norm;	/* Current set standard */
+	v4l2_std_id detected_norm;
+	u32 input;
+	u32 output;
+	u32 oe;
+	int enable;
+	bool lock;
+
+	u16 dev_id;
+	u16 rom_ver;
+
+	enum v4l2_mbus_type mbus_type;
+};
+
+static inline struct tvp5150 *to_tvp5150(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct tvp5150, sd);
+}
+
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct tvp5150, hdl)->sd;
+}
+
+static int tvp5150_read(struct v4l2_subdev *sd, unsigned char addr)
+{
+	struct tvp5150 *decoder = to_tvp5150(sd);
+	int ret, val;
+
+	ret = regmap_read(decoder->regmap, addr, &val);
+	if (ret < 0)
+		return ret;
+
+	return val;
+}
+
+static void dump_reg_range(struct v4l2_subdev *sd, char *s, u8 init,
+				const u8 end, int max_line)
+{
+	u8 buf[16];
+	int i = 0, j, len;
+
+	if (max_line > 16) {
+		dprintk0(sd->dev, "too much data to dump\n");
+		return;
+	}
+
+	for (i = init; i < end; i += max_line) {
+		len = (end - i > max_line) ? max_line : end - i;
+
+		for (j = 0; j < len; j++)
+			buf[j] = tvp5150_read(sd, i + j);
+
+		dprintk0(sd->dev, "%s reg %02x = %*ph\n", s, i, len, buf);
+	}
+}
+
+static int tvp5150_log_status(struct v4l2_subdev *sd)
+{
+	dprintk0(sd->dev, "tvp5150: Video input source selection #1 = 0x%02x\n",
+		tvp5150_read(sd, TVP5150_VD_IN_SRC_SEL_1));
+	dprintk0(sd->dev, "tvp5150: Analog channel controls = 0x%02x\n",
+		tvp5150_read(sd, TVP5150_ANAL_CHL_CTL));
+	dprintk0(sd->dev, "tvp5150: Operation mode controls = 0x%02x\n",
+		tvp5150_read(sd, TVP5150_OP_MODE_CTL));
+	dprintk0(sd->dev, "tvp5150: Miscellaneous controls = 0x%02x\n",
+		tvp5150_read(sd, TVP5150_MISC_CTL));
+	dprintk0(sd->dev, "tvp5150: Autoswitch mask= 0x%02x\n",
+		tvp5150_read(sd, TVP5150_AUTOSW_MSK));
+	dprintk0(sd->dev, "tvp5150: Color killer threshold control = 0x%02x\n",
+		tvp5150_read(sd, TVP5150_COLOR_KIL_THSH_CTL));
+	dprintk0(sd->dev, "tvp5150: Luminance processing controls #1 #2 and #3 = %02x %02x %02x\n",
+		tvp5150_read(sd, TVP5150_LUMA_PROC_CTL_1),
+		tvp5150_read(sd, TVP5150_LUMA_PROC_CTL_2),
+		tvp5150_read(sd, TVP5150_LUMA_PROC_CTL_3));
+	dprintk0(sd->dev, "tvp5150: Brightness control = 0x%02x\n",
+		tvp5150_read(sd, TVP5150_BRIGHT_CTL));
+	dprintk0(sd->dev, "tvp5150: Color saturation control = 0x%02x\n",
+		tvp5150_read(sd, TVP5150_SATURATION_CTL));
+	dprintk0(sd->dev, "tvp5150: Hue control = 0x%02x\n",
+		tvp5150_read(sd, TVP5150_HUE_CTL));
+	dprintk0(sd->dev, "tvp5150: Contrast control = 0x%02x\n",
+		tvp5150_read(sd, TVP5150_CONTRAST_CTL));
+	dprintk0(sd->dev, "tvp5150: Outputs and data rates select = 0x%02x\n",
+		tvp5150_read(sd, TVP5150_DATA_RATE_SEL));
+	dprintk0(sd->dev, "tvp5150: Configuration shared pins = 0x%02x\n",
+		tvp5150_read(sd, TVP5150_CONF_SHARED_PIN));
+	dprintk0(sd->dev, "tvp5150: Active video cropping start = 0x%02x%02x\n",
+		tvp5150_read(sd, TVP5150_ACT_VD_CROP_ST_MSB),
+		tvp5150_read(sd, TVP5150_ACT_VD_CROP_ST_LSB));
+	dprintk0(sd->dev, "tvp5150: Active video cropping stop  = 0x%02x%02x\n",
+		tvp5150_read(sd, TVP5150_ACT_VD_CROP_STP_MSB),
+		tvp5150_read(sd, TVP5150_ACT_VD_CROP_STP_LSB));
+	dprintk0(sd->dev, "tvp5150: Genlock/RTC = 0x%02x\n",
+		tvp5150_read(sd, TVP5150_GENLOCK));
+	dprintk0(sd->dev, "tvp5150: Horizontal sync start = 0x%02x\n",
+		tvp5150_read(sd, TVP5150_HORIZ_SYNC_START));
+	dprintk0(sd->dev, "tvp5150: Vertical blanking start = 0x%02x\n",
+		tvp5150_read(sd, TVP5150_VERT_BLANKING_START));
+	dprintk0(sd->dev, "tvp5150: Vertical blanking stop = 0x%02x\n",
+		tvp5150_read(sd, TVP5150_VERT_BLANKING_STOP));
+	dprintk0(sd->dev, "tvp5150: Chrominance processing control #1 and #2 = %02x %02x\n",
+		tvp5150_read(sd, TVP5150_CHROMA_PROC_CTL_1),
+		tvp5150_read(sd, TVP5150_CHROMA_PROC_CTL_2));
+	dprintk0(sd->dev, "tvp5150: Interrupt reset register B = 0x%02x\n",
+		tvp5150_read(sd, TVP5150_INT_RESET_REG_B));
+	dprintk0(sd->dev, "tvp5150: Interrupt enable register B = 0x%02x\n",
+		tvp5150_read(sd, TVP5150_INT_ENABLE_REG_B));
+	dprintk0(sd->dev, "tvp5150: Interrupt configuration register B = 0x%02x\n",
+		tvp5150_read(sd, TVP5150_INTT_CONFIG_REG_B));
+	dprintk0(sd->dev, "tvp5150: Video standard = 0x%02x\n",
+		tvp5150_read(sd, TVP5150_VIDEO_STD));
+	dprintk0(sd->dev, "tvp5150: Chroma gain factor: Cb=0x%02x Cr=0x%02x\n",
+		tvp5150_read(sd, TVP5150_CB_GAIN_FACT),
+		tvp5150_read(sd, TVP5150_CR_GAIN_FACTOR));
+	dprintk0(sd->dev, "tvp5150: Macrovision on counter = 0x%02x\n",
+		tvp5150_read(sd, TVP5150_MACROVISION_ON_CTR));
+	dprintk0(sd->dev, "tvp5150: Macrovision off counter = 0x%02x\n",
+		tvp5150_read(sd, TVP5150_MACROVISION_OFF_CTR));
+	dprintk0(sd->dev, "tvp5150: ITU-R BT.656.%d timing(TVP5150AM1 only)\n",
+		(tvp5150_read(sd, TVP5150_REV_SELECT) & 1) ? 3 : 4);
+	dprintk0(sd->dev, "tvp5150: Device ID = %02x%02x\n",
+		tvp5150_read(sd, TVP5150_MSB_DEV_ID),
+		tvp5150_read(sd, TVP5150_LSB_DEV_ID));
+	dprintk0(sd->dev, "tvp5150: ROM version = (hex) %02x.%02x\n",
+		tvp5150_read(sd, TVP5150_ROM_MAJOR_VER),
+		tvp5150_read(sd, TVP5150_ROM_MINOR_VER));
+	dprintk0(sd->dev, "tvp5150: Vertical line count = 0x%02x%02x\n",
+		tvp5150_read(sd, TVP5150_VERT_LN_COUNT_MSB),
+		tvp5150_read(sd, TVP5150_VERT_LN_COUNT_LSB));
+	dprintk0(sd->dev, "tvp5150: Interrupt status register B = 0x%02x\n",
+		tvp5150_read(sd, TVP5150_INT_STATUS_REG_B));
+	dprintk0(sd->dev, "tvp5150: Interrupt active register B = 0x%02x\n",
+		tvp5150_read(sd, TVP5150_INT_ACTIVE_REG_B));
+	dprintk0(sd->dev, "tvp5150: Status regs #1 to #5 = %02x %02x %02x %02x %02x\n",
+		tvp5150_read(sd, TVP5150_STATUS_REG_1),
+		tvp5150_read(sd, TVP5150_STATUS_REG_2),
+		tvp5150_read(sd, TVP5150_STATUS_REG_3),
+		tvp5150_read(sd, TVP5150_STATUS_REG_4),
+		tvp5150_read(sd, TVP5150_STATUS_REG_5));
+
+	dump_reg_range(sd, "Teletext filter 1",   TVP5150_TELETEXT_FIL1_INI,
+			TVP5150_TELETEXT_FIL1_END, 8);
+	dump_reg_range(sd, "Teletext filter 2",   TVP5150_TELETEXT_FIL2_INI,
+			TVP5150_TELETEXT_FIL2_END, 8);
+
+	dprintk0(sd->dev, "tvp5150: Teletext filter enable = 0x%02x\n",
+		tvp5150_read(sd, TVP5150_TELETEXT_FIL_ENA));
+	dprintk0(sd->dev, "tvp5150: Interrupt status register A = 0x%02x\n",
+		tvp5150_read(sd, TVP5150_INT_STATUS_REG_A));
+	dprintk0(sd->dev, "tvp5150: Interrupt enable register A = 0x%02x\n",
+		tvp5150_read(sd, TVP5150_INT_ENABLE_REG_A));
+	dprintk0(sd->dev, "tvp5150: Interrupt configuration = 0x%02x\n",
+		tvp5150_read(sd, TVP5150_INT_CONF));
+	dprintk0(sd->dev, "tvp5150: VDP status register = 0x%02x\n",
+		tvp5150_read(sd, TVP5150_VDP_STATUS_REG));
+	dprintk0(sd->dev, "tvp5150: FIFO word count = 0x%02x\n",
+		tvp5150_read(sd, TVP5150_FIFO_WORD_COUNT));
+	dprintk0(sd->dev, "tvp5150: FIFO interrupt threshold = 0x%02x\n",
+		tvp5150_read(sd, TVP5150_FIFO_INT_THRESHOLD));
+	dprintk0(sd->dev, "tvp5150: FIFO reset = 0x%02x\n",
+		tvp5150_read(sd, TVP5150_FIFO_RESET));
+	dprintk0(sd->dev, "tvp5150: Line number interrupt = 0x%02x\n",
+		tvp5150_read(sd, TVP5150_LINE_NUMBER_INT));
+	dprintk0(sd->dev, "tvp5150: Pixel alignment register = 0x%02x%02x\n",
+		tvp5150_read(sd, TVP5150_PIX_ALIGN_REG_HIGH),
+		tvp5150_read(sd, TVP5150_PIX_ALIGN_REG_LOW));
+	dprintk0(sd->dev, "tvp5150: FIFO output control = 0x%02x\n",
+		tvp5150_read(sd, TVP5150_FIFO_OUT_CTRL));
+	dprintk0(sd->dev, "tvp5150: Full field enable = 0x%02x\n",
+		tvp5150_read(sd, TVP5150_FULL_FIELD_ENA));
+	dprintk0(sd->dev, "tvp5150: Full field mode register = 0x%02x\n",
+		tvp5150_read(sd, TVP5150_FULL_FIELD_MODE_REG));
+
+	dump_reg_range(sd, "CC   data",   TVP5150_CC_DATA_INI,
+			TVP5150_CC_DATA_END, 8);
+
+	dump_reg_range(sd, "WSS  data",   TVP5150_WSS_DATA_INI,
+			TVP5150_WSS_DATA_END, 8);
+
+	dump_reg_range(sd, "VPS  data",   TVP5150_VPS_DATA_INI,
+			TVP5150_VPS_DATA_END, 8);
+
+	dump_reg_range(sd, "VITC data",   TVP5150_VITC_DATA_INI,
+			TVP5150_VITC_DATA_END, 10);
+
+	dump_reg_range(sd, "Line mode",   TVP5150_LINE_MODE_INI,
+			TVP5150_LINE_MODE_END, 8);
+	return 0;
+}
+
+/****************************************************************************
+			Basic functions
+ ****************************************************************************/
+
+static void tvp5150_selmux(struct v4l2_subdev *sd)
+{
+	int opmode = 0;
+	struct tvp5150 *decoder = to_tvp5150(sd);
+	unsigned int mask, val;
+	int input = 0;
+
+	/* Only tvp5150am1 and tvp5151 have signal generator support */
+	if ((decoder->dev_id == 0x5150 && decoder->rom_ver == 0x0400) ||
+	    (decoder->dev_id == 0x5151 && decoder->rom_ver == 0x0100)) {
+		if (!decoder->enable)
+			input = 8;
+	}
+
+	switch (decoder->input) {
+	case TVP5150_COMPOSITE1:
+		input |= 2;
+		fallthrough;
+	case TVP5150_COMPOSITE0:
+		break;
+	case TVP5150_SVIDEO:
+	default:
+		input |= 1;
+		break;
+	}
+
+	dev_dbg_lvl(sd->dev, 1, debug,
+		    "Selecting video route: route input=%s, output=%s => tvp5150 input=0x%02x, opmode=0x%02x\n",
+		    decoder->input == 0 ? "aip1a" :
+		    decoder->input == 2 ? "aip1b" : "svideo",
+		    decoder->output == 0 ? "normal" : "black-frame-gen",
+		    input, opmode);
+
+	regmap_write(decoder->regmap, TVP5150_OP_MODE_CTL, opmode);
+	regmap_write(decoder->regmap, TVP5150_VD_IN_SRC_SEL_1, input);
+
+	/*
+	 * Setup the FID/GLCO/VLK/HVLK and INTREQ/GPCL/VBLK output signals. For
+	 * S-Video we output the vertical lock (VLK) signal on FID/GLCO/VLK/HVLK
+	 * and set INTREQ/GPCL/VBLK to logic 0. For composite we output the
+	 * field indicator (FID) signal on FID/GLCO/VLK/HVLK and set
+	 * INTREQ/GPCL/VBLK to logic 1.
+	 */
+	mask = TVP5150_MISC_CTL_GPCL | TVP5150_MISC_CTL_HVLK;
+	if (decoder->input == TVP5150_SVIDEO)
+		val = TVP5150_MISC_CTL_HVLK;
+	else
+		val = TVP5150_MISC_CTL_GPCL;
+	regmap_update_bits(decoder->regmap, TVP5150_MISC_CTL, mask, val);
+};
+
+struct i2c_reg_value {
+	unsigned char reg;
+	unsigned char value;
+};
+
+/* Default values as sugested at TVP5150AM1 datasheet */
+static const struct i2c_reg_value tvp5150_init_default[] = {
+	{ /* 0x00 */
+		TVP5150_VD_IN_SRC_SEL_1, 0x00
+	},
+	{ /* 0x01 */
+		TVP5150_ANAL_CHL_CTL, 0x15
+	},
+	{ /* 0x02 */
+		TVP5150_OP_MODE_CTL, 0x00
+	},
+	{ /* 0x03 */
+		TVP5150_MISC_CTL, 0x01
+	},
+	{ /* 0x06 */
+		TVP5150_COLOR_KIL_THSH_CTL, 0x10
+	},
+	{ /* 0x07 */
+		TVP5150_LUMA_PROC_CTL_1, 0x60
+	},
+	{ /* 0x08 */
+		TVP5150_LUMA_PROC_CTL_2, 0x00
+	},
+	{ /* 0x09 */
+		TVP5150_BRIGHT_CTL, 0x80
+	},
+	{ /* 0x0a */
+		TVP5150_SATURATION_CTL, 0x80
+	},
+	{ /* 0x0b */
+		TVP5150_HUE_CTL, 0x00
+	},
+	{ /* 0x0c */
+		TVP5150_CONTRAST_CTL, 0x80
+	},
+	{ /* 0x0d */
+		TVP5150_DATA_RATE_SEL, 0x47
+	},
+	{ /* 0x0e */
+		TVP5150_LUMA_PROC_CTL_3, 0x00
+	},
+	{ /* 0x0f */
+		TVP5150_CONF_SHARED_PIN, 0x08
+	},
+	{ /* 0x11 */
+		TVP5150_ACT_VD_CROP_ST_MSB, 0x00
+	},
+	{ /* 0x12 */
+		TVP5150_ACT_VD_CROP_ST_LSB, 0x00
+	},
+	{ /* 0x13 */
+		TVP5150_ACT_VD_CROP_STP_MSB, 0x00
+	},
+	{ /* 0x14 */
+		TVP5150_ACT_VD_CROP_STP_LSB, 0x00
+	},
+	{ /* 0x15 */
+		TVP5150_GENLOCK, 0x01
+	},
+	{ /* 0x16 */
+		TVP5150_HORIZ_SYNC_START, 0x80
+	},
+	{ /* 0x18 */
+		TVP5150_VERT_BLANKING_START, 0x00
+	},
+	{ /* 0x19 */
+		TVP5150_VERT_BLANKING_STOP, 0x00
+	},
+	{ /* 0x1a */
+		TVP5150_CHROMA_PROC_CTL_1, 0x0c
+	},
+	{ /* 0x1b */
+		TVP5150_CHROMA_PROC_CTL_2, 0x14
+	},
+	{ /* 0x1c */
+		TVP5150_INT_RESET_REG_B, 0x00
+	},
+	{ /* 0x1d */
+		TVP5150_INT_ENABLE_REG_B, 0x00
+	},
+	{ /* 0x1e */
+		TVP5150_INTT_CONFIG_REG_B, 0x00
+	},
+	{ /* 0x28 */
+		TVP5150_VIDEO_STD, 0x00
+	},
+	{ /* 0x2e */
+		TVP5150_MACROVISION_ON_CTR, 0x0f
+	},
+	{ /* 0x2f */
+		TVP5150_MACROVISION_OFF_CTR, 0x01
+	},
+	{ /* 0xbb */
+		TVP5150_TELETEXT_FIL_ENA, 0x00
+	},
+	{ /* 0xc0 */
+		TVP5150_INT_STATUS_REG_A, 0x00
+	},
+	{ /* 0xc1 */
+		TVP5150_INT_ENABLE_REG_A, 0x00
+	},
+	{ /* 0xc2 */
+		TVP5150_INT_CONF, 0x04
+	},
+	{ /* 0xc8 */
+		TVP5150_FIFO_INT_THRESHOLD, 0x80
+	},
+	{ /* 0xc9 */
+		TVP5150_FIFO_RESET, 0x00
+	},
+	{ /* 0xca */
+		TVP5150_LINE_NUMBER_INT, 0x00
+	},
+	{ /* 0xcb */
+		TVP5150_PIX_ALIGN_REG_LOW, 0x4e
+	},
+	{ /* 0xcc */
+		TVP5150_PIX_ALIGN_REG_HIGH, 0x00
+	},
+	{ /* 0xcd */
+		TVP5150_FIFO_OUT_CTRL, 0x01
+	},
+	{ /* 0xcf */
+		TVP5150_FULL_FIELD_ENA, 0x00
+	},
+	{ /* 0xd0 */
+		TVP5150_LINE_MODE_INI, 0x00
+	},
+	{ /* 0xfc */
+		TVP5150_FULL_FIELD_MODE_REG, 0x7f
+	},
+	{ /* end of data */
+		0xff, 0xff
+	}
+};
+
+/* Default values as sugested at TVP5150AM1 datasheet */
+static const struct i2c_reg_value tvp5150_init_enable[] = {
+	{	/* Automatic offset and AGC enabled */
+		TVP5150_ANAL_CHL_CTL, 0x15
+	}, {	/* Activate YCrCb output 0x9 or 0xd ? */
+		TVP5150_MISC_CTL, TVP5150_MISC_CTL_GPCL |
+				  TVP5150_MISC_CTL_INTREQ_OE |
+				  TVP5150_MISC_CTL_YCBCR_OE |
+				  TVP5150_MISC_CTL_SYNC_OE |
+				  TVP5150_MISC_CTL_VBLANK |
+				  TVP5150_MISC_CTL_CLOCK_OE,
+	}, {	/* Activates video std autodetection for all standards */
+		TVP5150_AUTOSW_MSK, 0x0
+	}, {	/* Default format: 0x47. For 4:2:2: 0x40 */
+		TVP5150_DATA_RATE_SEL, 0x47
+	}, {
+		TVP5150_CHROMA_PROC_CTL_1, 0x0c
+	}, {
+		TVP5150_CHROMA_PROC_CTL_2, 0x54
+	}, {	/* Non documented, but initialized on WinTV USB2 */
+		0x27, 0x20
+	}, {
+		0xff, 0xff
+	}
+};
+
+struct tvp5150_vbi_type {
+	unsigned int vbi_type;
+	unsigned int ini_line;
+	unsigned int end_line;
+	unsigned int by_field :1;
+};
+
+struct i2c_vbi_ram_value {
+	u16 reg;
+	struct tvp5150_vbi_type type;
+	unsigned char values[16];
+};
+
+/* This struct have the values for each supported VBI Standard
+ * by
+ tvp5150_vbi_types should follow the same order as vbi_ram_default
+ * value 0 means rom position 0x10, value 1 means rom position 0x30
+ * and so on. There are 16 possible locations from 0 to 15.
+ */
+
+static struct i2c_vbi_ram_value vbi_ram_default[] = {
+
+	/*
+	 * FIXME: Current api doesn't handle all VBI types, those not
+	 * yet supported are placed under #if 0
+	 */
+#if 0
+	[0] = {0x010, /* Teletext, SECAM, WST System A */
+		{V4L2_SLICED_TELETEXT_SECAM, 6, 23, 1},
+		{ 0xaa, 0xaa, 0xff, 0xff, 0xe7, 0x2e, 0x20, 0x26,
+		  0xe6, 0xb4, 0x0e, 0x00, 0x00, 0x00, 0x10, 0x00 }
+	},
+#endif
+	[1] = {0x030, /* Teletext, PAL, WST System B */
+		{V4L2_SLICED_TELETEXT_B, 6, 22, 1},
+		{ 0xaa, 0xaa, 0xff, 0xff, 0x27, 0x2e, 0x20, 0x2b,
+		  0xa6, 0x72, 0x10, 0x00, 0x00, 0x00, 0x10, 0x00 }
+	},
+#if 0
+	[2] = {0x050, /* Teletext, PAL, WST System C */
+		{V4L2_SLICED_TELETEXT_PAL_C, 6, 22, 1},
+		{ 0xaa, 0xaa, 0xff, 0xff, 0xe7, 0x2e, 0x20, 0x22,
+		  0xa6, 0x98, 0x0d, 0x00, 0x00, 0x00, 0x10, 0x00 }
+	},
+	[3] = {0x070, /* Teletext, NTSC, WST System B */
+		{V4L2_SLICED_TELETEXT_NTSC_B, 10, 21, 1},
+		{ 0xaa, 0xaa, 0xff, 0xff, 0x27, 0x2e, 0x20, 0x23,
+		  0x69, 0x93, 0x0d, 0x00, 0x00, 0x00, 0x10, 0x00 }
+	},
+	[4] = {0x090, /* Tetetext, NTSC NABTS System C */
+		{V4L2_SLICED_TELETEXT_NTSC_C, 10, 21, 1},
+		{ 0xaa, 0xaa, 0xff, 0xff, 0xe7, 0x2e, 0x20, 0x22,
+		  0x69, 0x93, 0x0d, 0x00, 0x00, 0x00, 0x15, 0x00 }
+	},
+	[5] = {0x0b0, /* Teletext, NTSC-J, NABTS System D */
+		{V4L2_SLICED_TELETEXT_NTSC_D, 10, 21, 1},
+		{ 0xaa, 0xaa, 0xff, 0xff, 0xa7, 0x2e, 0x20, 0x23,
+		  0x69, 0x93, 0x0d, 0x00, 0x00, 0x00, 0x10, 0x00 }
+	},
+	[6] = {0x0d0, /* Closed Caption, PAL/SECAM */
+		{V4L2_SLICED_CAPTION_625, 22, 22, 1},
+		{ 0xaa, 0x2a, 0xff, 0x3f, 0x04, 0x51, 0x6e, 0x02,
+		  0xa6, 0x7b, 0x09, 0x00, 0x00, 0x00, 0x27, 0x00 }
+	},
+#endif
+	[7] = {0x0f0, /* Closed Caption, NTSC */
+		{V4L2_SLICED_CAPTION_525, 21, 21, 1},
+		{ 0xaa, 0x2a, 0xff, 0x3f, 0x04, 0x51, 0x6e, 0x02,
+		  0x69, 0x8c, 0x09, 0x00, 0x00, 0x00, 0x27, 0x00 }
+	},
+	[8] = {0x110, /* Wide Screen Signal, PAL/SECAM */
+		{V4L2_SLICED_WSS_625, 23, 23, 1},
+		{ 0x5b, 0x55, 0xc5, 0xff, 0x00, 0x71, 0x6e, 0x42,
+		  0xa6, 0xcd, 0x0f, 0x00, 0x00, 0x00, 0x3a, 0x00 }
+	},
+#if 0
+	[9] = {0x130, /* Wide Screen Signal, NTSC C */
+		{V4L2_SLICED_WSS_525, 20, 20, 1},
+		{ 0x38, 0x00, 0x3f, 0x00, 0x00, 0x71, 0x6e, 0x43,
+		  0x69, 0x7c, 0x08, 0x00, 0x00, 0x00, 0x39, 0x00 }
+	},
+	[10] = {0x150, /* Vertical Interval Timecode (VITC), PAL/SECAM */
+		{V4l2_SLICED_VITC_625, 6, 22, 0},
+		{ 0x00, 0x00, 0x00, 0x00, 0x00, 0x8f, 0x6d, 0x49,
+		  0xa6, 0x85, 0x08, 0x00, 0x00, 0x00, 0x4c, 0x00 }
+	},
+	[11] = {0x170, /* Vertical Interval Timecode (VITC), NTSC */
+		{V4l2_SLICED_VITC_525, 10, 20, 0},
+		{ 0x00, 0x00, 0x00, 0x00, 0x00, 0x8f, 0x6d, 0x49,
+		  0x69, 0x94, 0x08, 0x00, 0x00, 0x00, 0x4c, 0x00 }
+	},
+#endif
+	[12] = {0x190, /* Video Program System (VPS), PAL */
+		{V4L2_SLICED_VPS, 16, 16, 0},
+		{ 0xaa, 0xaa, 0xff, 0xff, 0xba, 0xce, 0x2b, 0x0d,
+		  0xa6, 0xda, 0x0b, 0x00, 0x00, 0x00, 0x60, 0x00 }
+	},
+	/* 0x1d0 User programmable */
+};
+
+static int tvp5150_write_inittab(struct v4l2_subdev *sd,
+				const struct i2c_reg_value *regs)
+{
+	struct tvp5150 *decoder = to_tvp5150(sd);
+
+	while (regs->reg != 0xff) {
+		regmap_write(decoder->regmap, regs->reg, regs->value);
+		regs++;
+	}
+	return 0;
+}
+
+static int tvp5150_vdp_init(struct v4l2_subdev *sd)
+{
+	struct tvp5150 *decoder = to_tvp5150(sd);
+	struct regmap *map = decoder->regmap;
+	unsigned int i;
+	int j;
+
+	/* Disable Full Field */
+	regmap_write(map, TVP5150_FULL_FIELD_ENA, 0);
+
+	/* Before programming, Line mode should be at 0xff */
+	for (i = TVP5150_LINE_MODE_INI; i <= TVP5150_LINE_MODE_END; i++)
+		regmap_write(map, i, 0xff);
+
+	/* Load Ram Table */
+	for (j = 0; j < ARRAY_SIZE(vbi_ram_default); j++) {
+		const struct i2c_vbi_ram_value *regs = &vbi_ram_default[j];
+
+		if (!regs->type.vbi_type)
+			continue;
+
+		regmap_write(map, TVP5150_CONF_RAM_ADDR_HIGH, regs->reg >> 8);
+		regmap_write(map, TVP5150_CONF_RAM_ADDR_LOW, regs->reg);
+
+		for (i = 0; i < 16; i++)
+			regmap_write(map, TVP5150_VDP_CONF_RAM_DATA,
+				     regs->values[i]);
+	}
+	return 0;
+}
+
+/* Fills VBI capabilities based on i2c_vbi_ram_value struct */
+static int tvp5150_g_sliced_vbi_cap(struct v4l2_subdev *sd,
+				struct v4l2_sliced_vbi_cap *cap)
+{
+	int line, i;
+
+	dev_dbg_lvl(sd->dev, 1, debug, "g_sliced_vbi_cap\n");
+	memset(cap, 0, sizeof(*cap));
+
+	for (i = 0; i < ARRAY_SIZE(vbi_ram_default); i++) {
+		const struct i2c_vbi_ram_value *regs = &vbi_ram_default[i];
+
+		if (!regs->type.vbi_type)
+			continue;
+
+		for (line = regs->type.ini_line;
+		     line <= regs->type.end_line;
+		     line++) {
+			cap->service_lines[0][line] |= regs->type.vbi_type;
+		}
+		cap->service_set |= regs->type.vbi_type;
+	}
+	return 0;
+}
+
+/* Set vbi processing
+ * type - one of tvp5150_vbi_types
+ * line - line to gather data
+ * fields: bit 0 field1, bit 1, field2
+ * flags (default=0xf0) is a bitmask, were set means:
+ *	bit 7: enable filtering null bytes on CC
+ *	bit 6: send data also to FIFO
+ *	bit 5: don't allow data with errors on FIFO
+ *	bit 4: enable ECC when possible
+ * pix_align = pix alignment:
+ *	LSB = field1
+ *	MSB = field2
+ */
+static int tvp5150_set_vbi(struct v4l2_subdev *sd,
+			unsigned int type, u8 flags, int line,
+			const int fields)
+{
+	struct tvp5150 *decoder = to_tvp5150(sd);
+	v4l2_std_id std = decoder->norm;
+	u8 reg;
+	int i, pos = 0;
+
+	if (std == V4L2_STD_ALL) {
+		dev_err(sd->dev, "VBI can't be configured without knowing number of lines\n");
+		return 0;
+	} else if (std & V4L2_STD_625_50) {
+		/* Don't follow NTSC Line number convension */
+		line += 3;
+	}
+
+	if (line < 6 || line > 27)
+		return 0;
+
+	for (i = 0; i < ARRAY_SIZE(vbi_ram_default); i++) {
+		const struct i2c_vbi_ram_value *regs =  &vbi_ram_default[i];
+
+		if (!regs->type.vbi_type)
+			continue;
+
+		if ((type & regs->type.vbi_type) &&
+		    (line >= regs->type.ini_line) &&
+		    (line <= regs->type.end_line))
+			break;
+		pos++;
+	}
+
+	type = pos | (flags & 0xf0);
+	reg = ((line - 6) << 1) + TVP5150_LINE_MODE_INI;
+
+	if (fields & 1)
+		regmap_write(decoder->regmap, reg, type);
+
+	if (fields & 2)
+		regmap_write(decoder->regmap, reg + 1, type);
+
+	return type;
+}
+
+static int tvp5150_get_vbi(struct v4l2_subdev *sd, int line)
+{
+	struct tvp5150 *decoder = to_tvp5150(sd);
+	v4l2_std_id std = decoder->norm;
+	u8 reg;
+	int pos, type = 0;
+	int i, ret = 0;
+
+	if (std == V4L2_STD_ALL) {
+		dev_err(sd->dev, "VBI can't be configured without knowing number of lines\n");
+		return 0;
+	} else if (std & V4L2_STD_625_50) {
+		/* Don't follow NTSC Line number convension */
+		line += 3;
+	}
+
+	if (line < 6 || line > 27)
+		return 0;
+
+	reg = ((line - 6) << 1) + TVP5150_LINE_MODE_INI;
+
+	for (i = 0; i <= 1; i++) {
+		ret = tvp5150_read(sd, reg + i);
+		if (ret < 0) {
+			dev_err(sd->dev, "%s: failed with error = %d\n",
+				 __func__, ret);
+			return 0;
+		}
+		pos = ret & 0x0f;
+		if (pos < ARRAY_SIZE(vbi_ram_default))
+			type |= vbi_ram_default[pos].type.vbi_type;
+	}
+
+	return type;
+}
+
+static int tvp5150_set_std(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct tvp5150 *decoder = to_tvp5150(sd);
+	int fmt = 0;
+
+	/* First tests should be against specific std */
+
+	if (std == V4L2_STD_NTSC_443) {
+		fmt = VIDEO_STD_NTSC_4_43_BIT;
+	} else if (std == V4L2_STD_PAL_M) {
+		fmt = VIDEO_STD_PAL_M_BIT;
+	} else if (std == V4L2_STD_PAL_N || std == V4L2_STD_PAL_Nc) {
+		fmt = VIDEO_STD_PAL_COMBINATION_N_BIT;
+	} else {
+		/* Then, test against generic ones */
+		if (std & V4L2_STD_NTSC)
+			fmt = VIDEO_STD_NTSC_MJ_BIT;
+		else if (std & V4L2_STD_PAL)
+			fmt = VIDEO_STD_PAL_BDGHIN_BIT;
+		else if (std & V4L2_STD_SECAM)
+			fmt = VIDEO_STD_SECAM_BIT;
+	}
+
+	dev_dbg_lvl(sd->dev, 1, debug, "Set video std register to %d.\n", fmt);
+	regmap_write(decoder->regmap, TVP5150_VIDEO_STD, fmt);
+	return 0;
+}
+
+static int tvp5150_g_std(struct v4l2_subdev *sd, v4l2_std_id *std)
+{
+	struct tvp5150 *decoder = to_tvp5150(sd);
+
+	*std = decoder->norm;
+
+	return 0;
+}
+
+static int tvp5150_s_std(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct tvp5150 *decoder = to_tvp5150(sd);
+	struct tvp5150_connector *cur_con = decoder->cur_connector;
+	v4l2_std_id supported_stds;
+
+	if (decoder->norm == std)
+		return 0;
+
+	/* In case of no of-connectors are available no limitations are made */
+	if (!decoder->connectors_num)
+		supported_stds = V4L2_STD_ALL;
+	else
+		supported_stds = cur_con->base.connector.analog.sdtv_stds;
+
+	/*
+	 * Check if requested std or group of std's is/are supported by the
+	 * connector.
+	 */
+	if ((supported_stds & std) == 0)
+		return -EINVAL;
+
+	/* Change cropping height limits */
+	if (std & V4L2_STD_525_60)
+		decoder->rect.height = TVP5150_V_MAX_525_60;
+	else
+		decoder->rect.height = TVP5150_V_MAX_OTHERS;
+
+	/* Set only the specific supported std in case of group of std's. */
+	decoder->norm = supported_stds & std;
+
+	return tvp5150_set_std(sd, std);
+}
+
+static v4l2_std_id tvp5150_read_std(struct v4l2_subdev *sd)
+{
+	int val = tvp5150_read(sd, TVP5150_STATUS_REG_5);
+
+	switch (val & 0x0F) {
+	case 0x01:
+		return V4L2_STD_NTSC;
+	case 0x03:
+		return V4L2_STD_PAL;
+	case 0x05:
+		return V4L2_STD_PAL_M;
+	case 0x07:
+		return V4L2_STD_PAL_N | V4L2_STD_PAL_Nc;
+	case 0x09:
+		return V4L2_STD_NTSC_443;
+	case 0xb:
+		return V4L2_STD_SECAM;
+	default:
+		return V4L2_STD_UNKNOWN;
+	}
+}
+
+static int query_lock(struct v4l2_subdev *sd)
+{
+	struct tvp5150 *decoder = to_tvp5150(sd);
+	int status;
+
+	if (decoder->irq)
+		return decoder->lock;
+
+	regmap_read(decoder->regmap, TVP5150_STATUS_REG_1, &status);
+
+	/* For standard detection, we need the 3 locks */
+	return (status & 0x0e) == 0x0e;
+}
+
+static int tvp5150_querystd(struct v4l2_subdev *sd, v4l2_std_id *std_id)
+{
+	*std_id = query_lock(sd) ? tvp5150_read_std(sd) : V4L2_STD_UNKNOWN;
+
+	return 0;
+}
+
+static const struct v4l2_event tvp5150_ev_fmt = {
+	.type = V4L2_EVENT_SOURCE_CHANGE,
+	.u.src_change.changes = V4L2_EVENT_SRC_CH_RESOLUTION,
+};
+
+static irqreturn_t tvp5150_isr(int irq, void *dev_id)
+{
+	struct tvp5150 *decoder = dev_id;
+	struct regmap *map = decoder->regmap;
+	unsigned int mask, active = 0, status = 0;
+
+	mask = TVP5150_MISC_CTL_YCBCR_OE | TVP5150_MISC_CTL_SYNC_OE |
+	       TVP5150_MISC_CTL_CLOCK_OE;
+
+	regmap_read(map, TVP5150_INT_STATUS_REG_A, &status);
+	if (status) {
+		regmap_write(map, TVP5150_INT_STATUS_REG_A, status);
+
+		if (status & TVP5150_INT_A_LOCK) {
+			decoder->lock = !!(status & TVP5150_INT_A_LOCK_STATUS);
+			dev_dbg_lvl(decoder->sd.dev, 1, debug,
+				    "sync lo%s signal\n",
+				    decoder->lock ? "ck" : "ss");
+			v4l2_subdev_notify_event(&decoder->sd, &tvp5150_ev_fmt);
+			regmap_update_bits(map, TVP5150_MISC_CTL, mask,
+					   decoder->lock ? decoder->oe : 0);
+		}
+
+		return IRQ_HANDLED;
+	}
+
+	regmap_read(map, TVP5150_INT_ACTIVE_REG_B, &active);
+	if (active) {
+		status = 0;
+		regmap_read(map, TVP5150_INT_STATUS_REG_B, &status);
+		if (status)
+			regmap_write(map, TVP5150_INT_RESET_REG_B, status);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int tvp5150_reset(struct v4l2_subdev *sd, u32 val)
+{
+	struct tvp5150 *decoder = to_tvp5150(sd);
+	struct regmap *map = decoder->regmap;
+
+	/* Initializes TVP5150 to its default values */
+	tvp5150_write_inittab(sd, tvp5150_init_default);
+
+	if (decoder->irq) {
+		/* Configure pins: FID, VSYNC, INTREQ, SCLK */
+		regmap_write(map, TVP5150_CONF_SHARED_PIN, 0x0);
+		/* Set interrupt polarity to active high */
+		regmap_write(map, TVP5150_INT_CONF, TVP5150_VDPOE | 0x1);
+		regmap_write(map, TVP5150_INTT_CONFIG_REG_B, 0x1);
+	} else {
+		/* Configure pins: FID, VSYNC, GPCL/VBLK, SCLK */
+		regmap_write(map, TVP5150_CONF_SHARED_PIN, 0x2);
+		/* Keep interrupt polarity active low */
+		regmap_write(map, TVP5150_INT_CONF, TVP5150_VDPOE);
+		regmap_write(map, TVP5150_INTT_CONFIG_REG_B, 0x0);
+	}
+
+	/* Initializes VDP registers */
+	tvp5150_vdp_init(sd);
+
+	/* Selects decoder input */
+	tvp5150_selmux(sd);
+
+	/* Initialize image preferences */
+	v4l2_ctrl_handler_setup(&decoder->hdl);
+
+	return 0;
+}
+
+static int tvp5150_enable(struct v4l2_subdev *sd)
+{
+	struct tvp5150 *decoder = to_tvp5150(sd);
+	v4l2_std_id std;
+
+	/* Initializes TVP5150 to stream enabled values */
+	tvp5150_write_inittab(sd, tvp5150_init_enable);
+
+	if (decoder->norm == V4L2_STD_ALL)
+		std = tvp5150_read_std(sd);
+	else
+		std = decoder->norm;
+
+	/* Disable autoswitch mode */
+	tvp5150_set_std(sd, std);
+
+	/*
+	 * Enable the YCbCr and clock outputs. In discrete sync mode
+	 * (non-BT.656) additionally enable the sync outputs.
+	 */
+	switch (decoder->mbus_type) {
+	case V4L2_MBUS_PARALLEL:
+		/* 8-bit 4:2:2 YUV with discrete sync output */
+		regmap_update_bits(decoder->regmap, TVP5150_DATA_RATE_SEL,
+				   0x7, 0x0);
+		decoder->oe = TVP5150_MISC_CTL_YCBCR_OE |
+			      TVP5150_MISC_CTL_CLOCK_OE |
+			      TVP5150_MISC_CTL_SYNC_OE;
+		break;
+	case V4L2_MBUS_BT656:
+		decoder->oe = TVP5150_MISC_CTL_YCBCR_OE |
+			      TVP5150_MISC_CTL_CLOCK_OE;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+};
+
+static int tvp5150_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+	struct tvp5150 *decoder = to_tvp5150(sd);
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		regmap_write(decoder->regmap, TVP5150_BRIGHT_CTL, ctrl->val);
+		return 0;
+	case V4L2_CID_CONTRAST:
+		regmap_write(decoder->regmap, TVP5150_CONTRAST_CTL, ctrl->val);
+		return 0;
+	case V4L2_CID_SATURATION:
+		regmap_write(decoder->regmap, TVP5150_SATURATION_CTL,
+			     ctrl->val);
+		return 0;
+	case V4L2_CID_HUE:
+		regmap_write(decoder->regmap, TVP5150_HUE_CTL, ctrl->val);
+		return 0;
+	case V4L2_CID_TEST_PATTERN:
+		decoder->enable = ctrl->val ? false : true;
+		tvp5150_selmux(sd);
+		return 0;
+	}
+	return -EINVAL;
+}
+
+static void tvp5150_set_default(v4l2_std_id std, struct v4l2_rect *crop)
+{
+	/* Default is no cropping */
+	crop->top = 0;
+	crop->left = 0;
+	crop->width = TVP5150_H_MAX;
+	if (std & V4L2_STD_525_60)
+		crop->height = TVP5150_V_MAX_525_60;
+	else
+		crop->height = TVP5150_V_MAX_OTHERS;
+}
+
+static struct v4l2_rect *
+tvp5150_get_pad_crop(struct tvp5150 *decoder,
+		     struct v4l2_subdev_state *sd_state, unsigned int pad,
+		     enum v4l2_subdev_format_whence which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return &decoder->rect;
+	case V4L2_SUBDEV_FORMAT_TRY:
+#if defined(CONFIG_VIDEO_V4L2_SUBDEV_API)
+		return v4l2_subdev_get_try_crop(&decoder->sd, sd_state, pad);
+#else
+		return ERR_PTR(-EINVAL);
+#endif
+	default:
+		return ERR_PTR(-EINVAL);
+	}
+}
+
+static int tvp5150_fill_fmt(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_state *sd_state,
+			    struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *f;
+	struct tvp5150 *decoder = to_tvp5150(sd);
+
+	if (!format || (format->pad != TVP5150_PAD_VID_OUT))
+		return -EINVAL;
+
+	f = &format->format;
+
+	f->width = decoder->rect.width;
+	f->height = decoder->rect.height / 2;
+
+	f->code = TVP5150_MBUS_FMT;
+	f->field = TVP5150_FIELD;
+	f->colorspace = TVP5150_COLORSPACE;
+
+	dev_dbg_lvl(sd->dev, 1, debug, "width = %d, height = %d\n", f->width,
+		    f->height);
+	return 0;
+}
+
+static unsigned int tvp5150_get_hmax(struct v4l2_subdev *sd)
+{
+	struct tvp5150 *decoder = to_tvp5150(sd);
+	v4l2_std_id std;
+
+	/* Calculate height based on current standard */
+	if (decoder->norm == V4L2_STD_ALL)
+		std = tvp5150_read_std(sd);
+	else
+		std = decoder->norm;
+
+	return (std & V4L2_STD_525_60) ?
+		TVP5150_V_MAX_525_60 : TVP5150_V_MAX_OTHERS;
+}
+
+static void tvp5150_set_hw_selection(struct v4l2_subdev *sd,
+				     struct v4l2_rect *rect)
+{
+	struct tvp5150 *decoder = to_tvp5150(sd);
+	unsigned int hmax = tvp5150_get_hmax(sd);
+
+	regmap_write(decoder->regmap, TVP5150_VERT_BLANKING_START, rect->top);
+	regmap_write(decoder->regmap, TVP5150_VERT_BLANKING_STOP,
+		     rect->top + rect->height - hmax);
+	regmap_write(decoder->regmap, TVP5150_ACT_VD_CROP_ST_MSB,
+		     rect->left >> TVP5150_CROP_SHIFT);
+	regmap_write(decoder->regmap, TVP5150_ACT_VD_CROP_ST_LSB,
+		     rect->left | (1 << TVP5150_CROP_SHIFT));
+	regmap_write(decoder->regmap, TVP5150_ACT_VD_CROP_STP_MSB,
+		     (rect->left + rect->width - TVP5150_MAX_CROP_LEFT) >>
+		     TVP5150_CROP_SHIFT);
+	regmap_write(decoder->regmap, TVP5150_ACT_VD_CROP_STP_LSB,
+		     rect->left + rect->width - TVP5150_MAX_CROP_LEFT);
+}
+
+static int tvp5150_set_selection(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_selection *sel)
+{
+	struct tvp5150 *decoder = to_tvp5150(sd);
+	struct v4l2_rect *rect = &sel->r;
+	struct v4l2_rect *crop;
+	unsigned int hmax;
+
+	if (sel->target != V4L2_SEL_TGT_CROP)
+		return -EINVAL;
+
+	dev_dbg_lvl(sd->dev, 1, debug, "%s left=%d, top=%d, width=%d, height=%d\n",
+		__func__, rect->left, rect->top, rect->width, rect->height);
+
+	/* tvp5150 has some special limits */
+	rect->left = clamp(rect->left, 0, TVP5150_MAX_CROP_LEFT);
+	rect->top = clamp(rect->top, 0, TVP5150_MAX_CROP_TOP);
+	hmax = tvp5150_get_hmax(sd);
+
+	/*
+	 * alignments:
+	 *  - width = 2 due to UYVY colorspace
+	 *  - height, image = no special alignment
+	 */
+	v4l_bound_align_image(&rect->width,
+			      TVP5150_H_MAX - TVP5150_MAX_CROP_LEFT - rect->left,
+			      TVP5150_H_MAX - rect->left, 1, &rect->height,
+			      hmax - TVP5150_MAX_CROP_TOP - rect->top,
+			      hmax - rect->top, 0, 0);
+
+	if (!IS_ENABLED(CONFIG_VIDEO_V4L2_SUBDEV_API) &&
+	    sel->which == V4L2_SUBDEV_FORMAT_TRY)
+		return 0;
+
+	crop = tvp5150_get_pad_crop(decoder, sd_state, sel->pad, sel->which);
+	if (IS_ERR(crop))
+		return PTR_ERR(crop);
+
+	/*
+	 * Update output image size if the selection (crop) rectangle size or
+	 * position has been modified.
+	 */
+	if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE &&
+	    !v4l2_rect_equal(rect, crop))
+		tvp5150_set_hw_selection(sd, rect);
+
+	*crop = *rect;
+
+	return 0;
+}
+
+static int tvp5150_get_selection(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_selection *sel)
+{
+	struct tvp5150 *decoder = container_of(sd, struct tvp5150, sd);
+	struct v4l2_rect *crop;
+	v4l2_std_id std;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = TVP5150_H_MAX;
+
+		/* Calculate height based on current standard */
+		if (decoder->norm == V4L2_STD_ALL)
+			std = tvp5150_read_std(sd);
+		else
+			std = decoder->norm;
+		if (std & V4L2_STD_525_60)
+			sel->r.height = TVP5150_V_MAX_525_60;
+		else
+			sel->r.height = TVP5150_V_MAX_OTHERS;
+		return 0;
+	case V4L2_SEL_TGT_CROP:
+		crop = tvp5150_get_pad_crop(decoder, sd_state, sel->pad,
+					    sel->which);
+		if (IS_ERR(crop))
+			return PTR_ERR(crop);
+		sel->r = *crop;
+		return 0;
+	default:
+		return -EINVAL;
+	}
+}
+
+static int tvp5150_get_mbus_config(struct v4l2_subdev *sd,
+				   unsigned int pad,
+				   struct v4l2_mbus_config *cfg)
+{
+	struct tvp5150 *decoder = to_tvp5150(sd);
+
+	cfg->type = decoder->mbus_type;
+	cfg->bus.parallel.flags = V4L2_MBUS_MASTER
+				| V4L2_MBUS_PCLK_SAMPLE_RISING
+				| V4L2_MBUS_FIELD_EVEN_LOW
+				| V4L2_MBUS_DATA_ACTIVE_HIGH;
+
+	return 0;
+}
+
+/****************************************************************************
+			V4L2 subdev pad ops
+ ****************************************************************************/
+static int tvp5150_init_cfg(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_state *sd_state)
+{
+	struct tvp5150 *decoder = to_tvp5150(sd);
+	v4l2_std_id std;
+
+	/*
+	 * Reset selection to maximum on subdev_open() if autodetection is on
+	 * and a standard change is detected.
+	 */
+	if (decoder->norm == V4L2_STD_ALL) {
+		std = tvp5150_read_std(sd);
+		if (std != decoder->detected_norm) {
+			decoder->detected_norm = std;
+			tvp5150_set_default(std, &decoder->rect);
+		}
+	}
+
+	return 0;
+}
+
+static int tvp5150_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_state *sd_state,
+		struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->pad || code->index)
+		return -EINVAL;
+
+	code->code = TVP5150_MBUS_FMT;
+	return 0;
+}
+
+static int tvp5150_enum_frame_size(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct tvp5150 *decoder = to_tvp5150(sd);
+
+	if (fse->index >= 8 || fse->code != TVP5150_MBUS_FMT)
+		return -EINVAL;
+
+	fse->code = TVP5150_MBUS_FMT;
+	fse->min_width = decoder->rect.width;
+	fse->max_width = decoder->rect.width;
+	fse->min_height = decoder->rect.height / 2;
+	fse->max_height = decoder->rect.height / 2;
+
+	return 0;
+}
+
+/****************************************************************************
+ *			Media entity ops
+ ****************************************************************************/
+#if defined(CONFIG_MEDIA_CONTROLLER)
+static int tvp5150_set_link(struct media_pad *connector_pad,
+			    struct media_pad *tvp5150_pad, u32 flags)
+{
+	struct media_link *link;
+
+	link = media_entity_find_link(connector_pad, tvp5150_pad);
+	if (!link)
+		return -EINVAL;
+
+	link->flags = flags;
+	link->reverse->flags = link->flags;
+
+	return 0;
+}
+
+static int tvp5150_disable_all_input_links(struct tvp5150 *decoder)
+{
+	struct media_pad *connector_pad;
+	unsigned int i;
+	int err;
+
+	for (i = 0; i < TVP5150_NUM_PADS - 1; i++) {
+		connector_pad = media_pad_remote_pad_first(&decoder->pads[i]);
+		if (!connector_pad)
+			continue;
+
+		err = tvp5150_set_link(connector_pad, &decoder->pads[i], 0);
+		if (err)
+			return err;
+	}
+
+	return 0;
+}
+
+static int tvp5150_s_routing(struct v4l2_subdev *sd, u32 input, u32 output,
+			     u32 config);
+
+static int tvp5150_link_setup(struct media_entity *entity,
+			      const struct media_pad *tvp5150_pad,
+			      const struct media_pad *remote, u32 flags)
+{
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+	struct tvp5150 *decoder = to_tvp5150(sd);
+	struct media_pad *other_tvp5150_pad =
+		&decoder->pads[tvp5150_pad->index ^ 1];
+	struct v4l2_fwnode_connector *v4l2c;
+	bool is_svideo = false;
+	unsigned int i;
+	int err;
+
+	/*
+	 * The TVP5150 state is determined by the enabled sink pad link(s).
+	 * Enabling or disabling the source pad link has no effect.
+	 */
+	if (tvp5150_pad->flags & MEDIA_PAD_FL_SOURCE)
+		return 0;
+
+	/* Check if the svideo connector should be enabled */
+	for (i = 0; i < decoder->connectors_num; i++) {
+		if (remote->entity == &decoder->connectors[i].ent) {
+			v4l2c = &decoder->connectors[i].base;
+			is_svideo = v4l2c->type == V4L2_CONN_SVIDEO;
+			break;
+		}
+	}
+
+	dev_dbg_lvl(sd->dev, 1, debug, "link setup '%s':%d->'%s':%d[%d]",
+		    remote->entity->name, remote->index,
+		    tvp5150_pad->entity->name, tvp5150_pad->index,
+		    flags & MEDIA_LNK_FL_ENABLED);
+	if (is_svideo)
+		dev_dbg_lvl(sd->dev, 1, debug,
+			    "link setup '%s':%d->'%s':%d[%d]",
+			    remote->entity->name, remote->index,
+			    other_tvp5150_pad->entity->name,
+			    other_tvp5150_pad->index,
+			    flags & MEDIA_LNK_FL_ENABLED);
+
+	/*
+	 * The TVP5150 has an internal mux which allows the following setup:
+	 *
+	 * comp-connector1  --\
+	 *		       |---> AIP1A
+	 *		      /
+	 * svideo-connector -|
+	 *		      \
+	 *		       |---> AIP1B
+	 * comp-connector2  --/
+	 *
+	 * We can't rely on user space that the current connector gets disabled
+	 * first before enabling the new connector. Disable all active
+	 * connector links to be on the safe side.
+	 */
+	err = tvp5150_disable_all_input_links(decoder);
+	if (err)
+		return err;
+
+	tvp5150_s_routing(sd, is_svideo ? TVP5150_SVIDEO : tvp5150_pad->index,
+			  flags & MEDIA_LNK_FL_ENABLED ? TVP5150_NORMAL :
+			  TVP5150_BLACK_SCREEN, 0);
+
+	if (flags & MEDIA_LNK_FL_ENABLED) {
+		struct v4l2_fwnode_connector_analog *v4l2ca;
+		u32 new_norm;
+
+		/*
+		 * S-Video connector is conneted to both ports AIP1A and AIP1B.
+		 * Both links must be enabled in one-shot regardless which link
+		 * the user requests.
+		 */
+		if (is_svideo) {
+			err = tvp5150_set_link((struct media_pad *)remote,
+					       other_tvp5150_pad, flags);
+			if (err)
+				return err;
+		}
+
+		if (!decoder->connectors_num)
+			return 0;
+
+		/* Update the current connector */
+		decoder->cur_connector =
+			container_of(remote, struct tvp5150_connector, pad);
+
+		/*
+		 * Do nothing if the new connector supports the same tv-norms as
+		 * the old one.
+		 */
+		v4l2ca = &decoder->cur_connector->base.connector.analog;
+		new_norm = decoder->norm & v4l2ca->sdtv_stds;
+		if (decoder->norm == new_norm)
+			return 0;
+
+		/*
+		 * Fallback to the new connector tv-norms if we can't find any
+		 * common between the current tv-norm and the new one.
+		 */
+		tvp5150_s_std(sd, new_norm ? new_norm : v4l2ca->sdtv_stds);
+	}
+
+	return 0;
+}
+
+static const struct media_entity_operations tvp5150_sd_media_ops = {
+	.link_setup = tvp5150_link_setup,
+};
+#endif
+/****************************************************************************
+			I2C Command
+ ****************************************************************************/
+static int __maybe_unused tvp5150_runtime_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct tvp5150 *decoder = to_tvp5150(sd);
+
+	if (decoder->irq)
+		/* Disable lock interrupt */
+		return regmap_update_bits(decoder->regmap,
+					  TVP5150_INT_ENABLE_REG_A,
+					  TVP5150_INT_A_LOCK, 0);
+	return 0;
+}
+
+static int __maybe_unused tvp5150_runtime_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct tvp5150 *decoder = to_tvp5150(sd);
+
+	if (decoder->irq)
+		/* Enable lock interrupt */
+		return regmap_update_bits(decoder->regmap,
+					  TVP5150_INT_ENABLE_REG_A,
+					  TVP5150_INT_A_LOCK,
+					  TVP5150_INT_A_LOCK);
+	return 0;
+}
+
+static int tvp5150_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct tvp5150 *decoder = to_tvp5150(sd);
+	unsigned int mask, val = 0;
+	int ret;
+
+	mask = TVP5150_MISC_CTL_YCBCR_OE | TVP5150_MISC_CTL_SYNC_OE |
+	       TVP5150_MISC_CTL_CLOCK_OE;
+
+	if (enable) {
+		ret = pm_runtime_resume_and_get(sd->dev);
+		if (ret < 0)
+			return ret;
+
+		tvp5150_enable(sd);
+
+		/* Enable outputs if decoder is locked */
+		if (decoder->irq)
+			val = decoder->lock ? decoder->oe : 0;
+		else
+			val = decoder->oe;
+
+		v4l2_subdev_notify_event(&decoder->sd, &tvp5150_ev_fmt);
+	} else {
+		pm_runtime_put(sd->dev);
+	}
+
+	regmap_update_bits(decoder->regmap, TVP5150_MISC_CTL, mask, val);
+
+	return 0;
+}
+
+static int tvp5150_s_routing(struct v4l2_subdev *sd,
+			     u32 input, u32 output, u32 config)
+{
+	struct tvp5150 *decoder = to_tvp5150(sd);
+
+	decoder->input = input;
+	decoder->output = output;
+
+	if (output == TVP5150_BLACK_SCREEN)
+		decoder->enable = false;
+	else
+		decoder->enable = true;
+
+	tvp5150_selmux(sd);
+	return 0;
+}
+
+static int tvp5150_s_raw_fmt(struct v4l2_subdev *sd, struct v4l2_vbi_format *fmt)
+{
+	struct tvp5150 *decoder = to_tvp5150(sd);
+
+	/*
+	 * this is for capturing 36 raw vbi lines
+	 * if there's a way to cut off the beginning 2 vbi lines
+	 * with the tvp5150 then the vbi line count could be lowered
+	 * to 17 lines/field again, although I couldn't find a register
+	 * which could do that cropping
+	 */
+
+	if (fmt->sample_format == V4L2_PIX_FMT_GREY)
+		regmap_write(decoder->regmap, TVP5150_LUMA_PROC_CTL_1, 0x70);
+	if (fmt->count[0] == 18 && fmt->count[1] == 18) {
+		regmap_write(decoder->regmap, TVP5150_VERT_BLANKING_START,
+			     0x00);
+		regmap_write(decoder->regmap, TVP5150_VERT_BLANKING_STOP, 0x01);
+	}
+	return 0;
+}
+
+static int tvp5150_s_sliced_fmt(struct v4l2_subdev *sd, struct v4l2_sliced_vbi_format *svbi)
+{
+	struct tvp5150 *decoder = to_tvp5150(sd);
+	int i;
+
+	if (svbi->service_set != 0) {
+		for (i = 0; i <= 23; i++) {
+			svbi->service_lines[1][i] = 0;
+			svbi->service_lines[0][i] =
+				tvp5150_set_vbi(sd, svbi->service_lines[0][i],
+						0xf0, i, 3);
+		}
+		/* Enables FIFO */
+		regmap_write(decoder->regmap, TVP5150_FIFO_OUT_CTRL, 1);
+	} else {
+		/* Disables FIFO*/
+		regmap_write(decoder->regmap, TVP5150_FIFO_OUT_CTRL, 0);
+
+		/* Disable Full Field */
+		regmap_write(decoder->regmap, TVP5150_FULL_FIELD_ENA, 0);
+
+		/* Disable Line modes */
+		for (i = TVP5150_LINE_MODE_INI; i <= TVP5150_LINE_MODE_END; i++)
+			regmap_write(decoder->regmap, i, 0xff);
+	}
+	return 0;
+}
+
+static int tvp5150_g_sliced_fmt(struct v4l2_subdev *sd, struct v4l2_sliced_vbi_format *svbi)
+{
+	int i, mask = 0;
+
+	memset(svbi->service_lines, 0, sizeof(svbi->service_lines));
+
+	for (i = 0; i <= 23; i++) {
+		svbi->service_lines[0][i] =
+			tvp5150_get_vbi(sd, i);
+		mask |= svbi->service_lines[0][i];
+	}
+	svbi->service_set = mask;
+	return 0;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int tvp5150_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+{
+	int res;
+
+	res = tvp5150_read(sd, reg->reg & 0xff);
+	if (res < 0) {
+		dev_err(sd->dev, "%s: failed with error = %d\n", __func__, res);
+		return res;
+	}
+
+	reg->val = res;
+	reg->size = 1;
+	return 0;
+}
+
+static int tvp5150_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
+{
+	struct tvp5150 *decoder = to_tvp5150(sd);
+
+	return regmap_write(decoder->regmap, reg->reg & 0xff, reg->val & 0xff);
+}
+#endif
+
+static int tvp5150_subscribe_event(struct v4l2_subdev *sd, struct v4l2_fh *fh,
+				   struct v4l2_event_subscription *sub)
+{
+	switch (sub->type) {
+	case V4L2_EVENT_SOURCE_CHANGE:
+		return v4l2_src_change_event_subdev_subscribe(sd, fh, sub);
+	case V4L2_EVENT_CTRL:
+		return v4l2_ctrl_subdev_subscribe_event(sd, fh, sub);
+	default:
+		return -EINVAL;
+	}
+}
+
+static int tvp5150_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
+{
+	int status = tvp5150_read(sd, 0x88);
+
+	vt->signal = ((status & 0x04) && (status & 0x02)) ? 0xffff : 0x0;
+	return 0;
+}
+
+static int tvp5150_registered(struct v4l2_subdev *sd)
+{
+#if defined(CONFIG_MEDIA_CONTROLLER)
+	struct tvp5150 *decoder = to_tvp5150(sd);
+	unsigned int i;
+	int ret;
+
+	/*
+	 * Setup connector pads and links. Enable the link to the first
+	 * available connector per default.
+	 */
+	for (i = 0; i < decoder->connectors_num; i++) {
+		struct media_entity *con = &decoder->connectors[i].ent;
+		struct media_pad *pad = &decoder->connectors[i].pad;
+		struct v4l2_fwnode_connector *v4l2c =
+			&decoder->connectors[i].base;
+		struct v4l2_connector_link *link =
+			v4l2_connector_first_link(v4l2c);
+		unsigned int port = link->fwnode_link.remote_port;
+		unsigned int flags = i ? 0 : MEDIA_LNK_FL_ENABLED;
+		bool is_svideo = v4l2c->type == V4L2_CONN_SVIDEO;
+
+		pad->flags = MEDIA_PAD_FL_SOURCE;
+		ret = media_entity_pads_init(con, 1, pad);
+		if (ret < 0)
+			goto err;
+
+		ret = media_device_register_entity(sd->v4l2_dev->mdev, con);
+		if (ret < 0)
+			goto err;
+
+		ret = media_create_pad_link(con, 0, &sd->entity, port, flags);
+		if (ret < 0)
+			goto err;
+
+		if (is_svideo) {
+			/*
+			 * Check tvp5150_link_setup() comments for more
+			 * information.
+			 */
+			link = v4l2_connector_last_link(v4l2c);
+			port = link->fwnode_link.remote_port;
+			ret = media_create_pad_link(con, 0, &sd->entity, port,
+						    flags);
+			if (ret < 0)
+				goto err;
+		}
+
+		/* Enable default input. */
+		if (flags == MEDIA_LNK_FL_ENABLED) {
+			decoder->input =
+				is_svideo ? TVP5150_SVIDEO :
+				port == 0 ? TVP5150_COMPOSITE0 :
+				TVP5150_COMPOSITE1;
+
+			tvp5150_selmux(sd);
+			decoder->cur_connector = &decoder->connectors[i];
+			tvp5150_s_std(sd, v4l2c->connector.analog.sdtv_stds);
+		}
+	}
+
+	return 0;
+
+err:
+	for (i = 0; i < decoder->connectors_num; i++) {
+		media_device_unregister_entity(&decoder->connectors[i].ent);
+		media_entity_cleanup(&decoder->connectors[i].ent);
+	}
+	return ret;
+#endif
+
+	return 0;
+}
+
+static int tvp5150_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	return pm_runtime_resume_and_get(sd->dev);
+}
+
+static int tvp5150_close(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	pm_runtime_put(sd->dev);
+
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_ctrl_ops tvp5150_ctrl_ops = {
+	.s_ctrl = tvp5150_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops tvp5150_core_ops = {
+	.log_status = tvp5150_log_status,
+	.reset = tvp5150_reset,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = tvp5150_g_register,
+	.s_register = tvp5150_s_register,
+#endif
+	.subscribe_event = tvp5150_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_tuner_ops tvp5150_tuner_ops = {
+	.g_tuner = tvp5150_g_tuner,
+};
+
+static const struct v4l2_subdev_video_ops tvp5150_video_ops = {
+	.s_std = tvp5150_s_std,
+	.g_std = tvp5150_g_std,
+	.querystd = tvp5150_querystd,
+	.s_stream = tvp5150_s_stream,
+	.s_routing = tvp5150_s_routing,
+};
+
+static const struct v4l2_subdev_vbi_ops tvp5150_vbi_ops = {
+	.g_sliced_vbi_cap = tvp5150_g_sliced_vbi_cap,
+	.g_sliced_fmt = tvp5150_g_sliced_fmt,
+	.s_sliced_fmt = tvp5150_s_sliced_fmt,
+	.s_raw_fmt = tvp5150_s_raw_fmt,
+};
+
+static const struct v4l2_subdev_pad_ops tvp5150_pad_ops = {
+	.init_cfg = tvp5150_init_cfg,
+	.enum_mbus_code = tvp5150_enum_mbus_code,
+	.enum_frame_size = tvp5150_enum_frame_size,
+	.set_fmt = tvp5150_fill_fmt,
+	.get_fmt = tvp5150_fill_fmt,
+	.get_selection = tvp5150_get_selection,
+	.set_selection = tvp5150_set_selection,
+	.get_mbus_config = tvp5150_get_mbus_config,
+};
+
+static const struct v4l2_subdev_ops tvp5150_ops = {
+	.core = &tvp5150_core_ops,
+	.tuner = &tvp5150_tuner_ops,
+	.video = &tvp5150_video_ops,
+	.vbi = &tvp5150_vbi_ops,
+	.pad = &tvp5150_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops tvp5150_internal_ops = {
+	.registered = tvp5150_registered,
+	.open = tvp5150_open,
+	.close = tvp5150_close,
+};
+
+/****************************************************************************
+			I2C Client & Driver
+ ****************************************************************************/
+
+static const struct regmap_range tvp5150_readable_ranges[] = {
+	{
+		.range_min = TVP5150_VD_IN_SRC_SEL_1,
+		.range_max = TVP5150_AUTOSW_MSK,
+	}, {
+		.range_min = TVP5150_COLOR_KIL_THSH_CTL,
+		.range_max = TVP5150_CONF_SHARED_PIN,
+	}, {
+		.range_min = TVP5150_ACT_VD_CROP_ST_MSB,
+		.range_max = TVP5150_HORIZ_SYNC_START,
+	}, {
+		.range_min = TVP5150_VERT_BLANKING_START,
+		.range_max = TVP5150_INTT_CONFIG_REG_B,
+	}, {
+		.range_min = TVP5150_VIDEO_STD,
+		.range_max = TVP5150_VIDEO_STD,
+	}, {
+		.range_min = TVP5150_CB_GAIN_FACT,
+		.range_max = TVP5150_REV_SELECT,
+	}, {
+		.range_min = TVP5150_MSB_DEV_ID,
+		.range_max = TVP5150_STATUS_REG_5,
+	}, {
+		.range_min = TVP5150_CC_DATA_INI,
+		.range_max = TVP5150_TELETEXT_FIL_ENA,
+	}, {
+		.range_min = TVP5150_INT_STATUS_REG_A,
+		.range_max = TVP5150_FIFO_OUT_CTRL,
+	}, {
+		.range_min = TVP5150_FULL_FIELD_ENA,
+		.range_max = TVP5150_FULL_FIELD_MODE_REG,
+	},
+};
+
+static bool tvp5150_volatile_reg(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case TVP5150_VERT_LN_COUNT_MSB:
+	case TVP5150_VERT_LN_COUNT_LSB:
+	case TVP5150_INT_STATUS_REG_A:
+	case TVP5150_INT_STATUS_REG_B:
+	case TVP5150_INT_ACTIVE_REG_B:
+	case TVP5150_STATUS_REG_1:
+	case TVP5150_STATUS_REG_2:
+	case TVP5150_STATUS_REG_3:
+	case TVP5150_STATUS_REG_4:
+	case TVP5150_STATUS_REG_5:
+	/* CC, WSS, VPS, VITC data? */
+	case TVP5150_VBI_FIFO_READ_DATA:
+	case TVP5150_VDP_STATUS_REG:
+	case TVP5150_FIFO_WORD_COUNT:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static const struct regmap_access_table tvp5150_readable_table = {
+	.yes_ranges = tvp5150_readable_ranges,
+	.n_yes_ranges = ARRAY_SIZE(tvp5150_readable_ranges),
+};
+
+static struct regmap_config tvp5150_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+	.max_register = 0xff,
+
+	.cache_type = REGCACHE_RBTREE,
+
+	.rd_table = &tvp5150_readable_table,
+	.volatile_reg = tvp5150_volatile_reg,
+};
+
+static int tvp5150_detect_version(struct tvp5150 *core)
+{
+	struct v4l2_subdev *sd = &core->sd;
+	struct i2c_client *c = v4l2_get_subdevdata(sd);
+	u8 regs[4];
+	int res;
+
+	/*
+	 * Read consequent registers - TVP5150_MSB_DEV_ID, TVP5150_LSB_DEV_ID,
+	 * TVP5150_ROM_MAJOR_VER, TVP5150_ROM_MINOR_VER
+	 */
+	res = regmap_bulk_read(core->regmap, TVP5150_MSB_DEV_ID, regs, 4);
+	if (res < 0) {
+		dev_err(&c->dev, "reading ID registers failed: %d\n", res);
+		return res;
+	}
+
+	core->dev_id = (regs[0] << 8) | regs[1];
+	core->rom_ver = (regs[2] << 8) | regs[3];
+
+	dev_info(sd->dev, "tvp%04x (%u.%u) chip found @ 0x%02x (%s)\n",
+		  core->dev_id, regs[2], regs[3], c->addr << 1,
+		  c->adapter->name);
+
+	if (core->dev_id == 0x5150 && core->rom_ver == 0x0321) {
+		dev_info(sd->dev, "tvp5150a detected.\n");
+	} else if (core->dev_id == 0x5150 && core->rom_ver == 0x0400) {
+		dev_info(sd->dev, "tvp5150am1 detected.\n");
+
+		/* ITU-T BT.656.4 timing */
+		regmap_write(core->regmap, TVP5150_REV_SELECT, 0);
+	} else if (core->dev_id == 0x5151 && core->rom_ver == 0x0100) {
+		dev_info(sd->dev, "tvp5151 detected.\n");
+	} else {
+		dev_info(sd->dev, "*** unknown tvp%04x chip detected.\n",
+			  core->dev_id);
+	}
+
+	return 0;
+}
+
+static int tvp5150_init(struct i2c_client *c)
+{
+	struct gpio_desc *pdn_gpio;
+	struct gpio_desc *reset_gpio;
+
+	pdn_gpio = devm_gpiod_get_optional(&c->dev, "pdn", GPIOD_OUT_HIGH);
+	if (IS_ERR(pdn_gpio))
+		return PTR_ERR(pdn_gpio);
+
+	if (pdn_gpio) {
+		gpiod_set_value_cansleep(pdn_gpio, 0);
+		/* Delay time between power supplies active and reset */
+		msleep(20);
+	}
+
+	reset_gpio = devm_gpiod_get_optional(&c->dev, "reset", GPIOD_OUT_HIGH);
+	if (IS_ERR(reset_gpio))
+		return PTR_ERR(reset_gpio);
+
+	if (reset_gpio) {
+		/* RESETB pulse duration */
+		ndelay(500);
+		gpiod_set_value_cansleep(reset_gpio, 0);
+		/* Delay time between end of reset to I2C active */
+		usleep_range(200, 250);
+	}
+
+	return 0;
+}
+
+#if defined(CONFIG_MEDIA_CONTROLLER)
+static int tvp5150_mc_init(struct tvp5150 *decoder)
+{
+	struct v4l2_subdev *sd = &decoder->sd;
+	unsigned int i;
+
+	sd->entity.ops = &tvp5150_sd_media_ops;
+	sd->entity.function = MEDIA_ENT_F_ATV_DECODER;
+
+	for (i = 0; i < TVP5150_NUM_PADS - 1; i++) {
+		decoder->pads[i].flags = MEDIA_PAD_FL_SINK;
+		decoder->pads[i].sig_type = PAD_SIGNAL_ANALOG;
+	}
+
+	decoder->pads[i].flags = MEDIA_PAD_FL_SOURCE;
+	decoder->pads[i].sig_type = PAD_SIGNAL_DV;
+
+	return media_entity_pads_init(&sd->entity, TVP5150_NUM_PADS,
+				      decoder->pads);
+}
+
+#else /* !defined(CONFIG_MEDIA_CONTROLLER) */
+
+static inline int tvp5150_mc_init(struct tvp5150 *decoder)
+{
+	return 0;
+}
+#endif /* defined(CONFIG_MEDIA_CONTROLLER) */
+
+static int tvp5150_validate_connectors(struct tvp5150 *decoder)
+{
+	struct device *dev = decoder->sd.dev;
+	struct tvp5150_connector *tvpc;
+	struct v4l2_fwnode_connector *v4l2c;
+	unsigned int i;
+
+	if (!decoder->connectors_num) {
+		dev_err(dev, "No valid connector found\n");
+		return -ENODEV;
+	}
+
+	for (i = 0; i < decoder->connectors_num; i++) {
+		struct v4l2_connector_link *link0 = NULL;
+		struct v4l2_connector_link *link1;
+
+		tvpc = &decoder->connectors[i];
+		v4l2c = &tvpc->base;
+
+		if (v4l2c->type == V4L2_CONN_COMPOSITE) {
+			if (v4l2c->nr_of_links != 1) {
+				dev_err(dev, "Composite: connector needs 1 link\n");
+				return -EINVAL;
+			}
+			link0 = v4l2_connector_first_link(v4l2c);
+			if (!link0) {
+				dev_err(dev, "Composite: invalid first link\n");
+				return -EINVAL;
+			}
+			if (link0->fwnode_link.remote_id == 1) {
+				dev_err(dev, "Composite: invalid endpoint id\n");
+				return -EINVAL;
+			}
+		}
+
+		if (v4l2c->type == V4L2_CONN_SVIDEO) {
+			if (v4l2c->nr_of_links != 2) {
+				dev_err(dev, "SVideo: connector needs 2 links\n");
+				return -EINVAL;
+			}
+			link0 = v4l2_connector_first_link(v4l2c);
+			if (!link0) {
+				dev_err(dev, "SVideo: invalid first link\n");
+				return -EINVAL;
+			}
+			link1 = v4l2_connector_last_link(v4l2c);
+			if (link0->fwnode_link.remote_port ==
+			    link1->fwnode_link.remote_port) {
+				dev_err(dev, "SVideo: invalid link setup\n");
+				return -EINVAL;
+			}
+		}
+
+		if (!(v4l2c->connector.analog.sdtv_stds & TVP5150_STD_MASK)) {
+			dev_err(dev, "Unsupported tv-norm on connector %s\n",
+				v4l2c->name);
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+static int tvp5150_parse_dt(struct tvp5150 *decoder, struct device_node *np)
+{
+	struct device *dev = decoder->sd.dev;
+	struct v4l2_fwnode_endpoint bus_cfg = {
+		.bus_type = V4L2_MBUS_UNKNOWN
+	};
+	struct device_node *ep_np;
+	struct tvp5150_connector *tvpc;
+	struct v4l2_fwnode_connector *v4l2c;
+	unsigned int flags, ep_num;
+	unsigned int i;
+	int ret;
+
+	/* At least 1 output and 1 input */
+	ep_num = of_graph_get_endpoint_count(np);
+	if (ep_num < 2 || ep_num > 5) {
+		dev_err(dev, "At least 1 input and 1 output must be connected to the device.\n");
+		return -EINVAL;
+	}
+
+	/* Layout if all connectors are used:
+	 *
+	 * tvp-5150 port@0 (AIP1A)
+	 *	endpoint@0 -----------> Comp0-Con  port
+	 *	endpoint@1 --------+--> Svideo-Con port
+	 * tvp-5150 port@1 (AIP1B) |
+	 *	endpoint@1 --------+
+	 *	endpoint@0 -----------> Comp1-Con  port
+	 * tvp-5150 port@2
+	 *	endpoint (video bitstream output at YOUT[0-7] parallel bus)
+	 */
+	for_each_endpoint_of_node(np, ep_np) {
+		struct fwnode_handle *ep_fwnode = of_fwnode_handle(ep_np);
+		unsigned int next_connector = decoder->connectors_num;
+		struct of_endpoint ep;
+
+		of_graph_parse_endpoint(ep_np, &ep);
+		if (ep.port > 1 || ep.id > 1) {
+			dev_dbg(dev, "Ignore connector on port@%u/ep@%u\n",
+				ep.port, ep.id);
+			continue;
+		}
+
+		tvpc = &decoder->connectors[next_connector];
+		v4l2c = &tvpc->base;
+
+		if (ep.port == 0 || (ep.port == 1 && ep.id == 0)) {
+			ret = v4l2_fwnode_connector_parse(ep_fwnode, v4l2c);
+			if (ret)
+				goto err_put;
+			ret = v4l2_fwnode_connector_add_link(ep_fwnode, v4l2c);
+			if (ret)
+				goto err_put;
+			decoder->connectors_num++;
+		} else {
+			/* Adding the 2nd svideo link */
+			for (i = 0; i < TVP5150_MAX_CONNECTORS; i++) {
+				tvpc = &decoder->connectors[i];
+				v4l2c = &tvpc->base;
+				if (v4l2c->type == V4L2_CONN_SVIDEO)
+					break;
+			}
+
+			ret = v4l2_fwnode_connector_add_link(ep_fwnode, v4l2c);
+			if (ret)
+				goto err_put;
+		}
+	}
+
+	ret = tvp5150_validate_connectors(decoder);
+	if (ret)
+		goto err_free;
+
+	for (i = 0; i < decoder->connectors_num; i++) {
+		tvpc = &decoder->connectors[i];
+		v4l2c = &tvpc->base;
+		tvpc->ent.flags = MEDIA_ENT_FL_CONNECTOR;
+		tvpc->ent.function = v4l2c->type == V4L2_CONN_SVIDEO ?
+			MEDIA_ENT_F_CONN_SVIDEO : MEDIA_ENT_F_CONN_COMPOSITE;
+		tvpc->ent.name = devm_kasprintf(dev, GFP_KERNEL, "%s %s",
+						v4l2c->name, v4l2c->label ?
+						v4l2c->label : "");
+		if (!tvpc->ent.name) {
+			ret = -ENOMEM;
+			goto err_free;
+		}
+	}
+
+	ep_np = of_graph_get_endpoint_by_regs(np, TVP5150_PAD_VID_OUT, 0);
+	if (!ep_np) {
+		ret = -EINVAL;
+		dev_err(dev, "Error no output endpoint available\n");
+		goto err_free;
+	}
+	ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(ep_np), &bus_cfg);
+	of_node_put(ep_np);
+	if (ret)
+		goto err_free;
+
+	flags = bus_cfg.bus.parallel.flags;
+	if (bus_cfg.bus_type == V4L2_MBUS_PARALLEL &&
+	    !(flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH &&
+	      flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH &&
+	      flags & V4L2_MBUS_FIELD_EVEN_LOW)) {
+		ret = -EINVAL;
+		goto err_free;
+	}
+
+	decoder->mbus_type = bus_cfg.bus_type;
+
+	return 0;
+
+err_put:
+	of_node_put(ep_np);
+err_free:
+	for (i = 0; i < TVP5150_MAX_CONNECTORS; i++)
+		v4l2_fwnode_connector_free(&decoder->connectors[i].base);
+
+	return ret;
+}
+
+static const char * const tvp5150_test_patterns[2] = {
+	"Disabled",
+	"Black screen"
+};
+
+static int tvp5150_probe(struct i2c_client *c)
+{
+	struct tvp5150 *core;
+	struct v4l2_subdev *sd;
+	struct device_node *np = c->dev.of_node;
+	struct regmap *map;
+	unsigned int i;
+	int res;
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(c->adapter,
+	     I2C_FUNC_SMBUS_READ_BYTE | I2C_FUNC_SMBUS_WRITE_BYTE_DATA))
+		return -EIO;
+
+	res = tvp5150_init(c);
+	if (res)
+		return res;
+
+	core = devm_kzalloc(&c->dev, sizeof(*core), GFP_KERNEL);
+	if (!core)
+		return -ENOMEM;
+
+	map = devm_regmap_init_i2c(c, &tvp5150_config);
+	if (IS_ERR(map))
+		return PTR_ERR(map);
+
+	core->regmap = map;
+	sd = &core->sd;
+	v4l2_i2c_subdev_init(sd, c, &tvp5150_ops);
+	sd->internal_ops = &tvp5150_internal_ops;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
+
+	if (IS_ENABLED(CONFIG_OF) && np) {
+		res = tvp5150_parse_dt(core, np);
+		if (res) {
+			dev_err(sd->dev, "DT parsing error: %d\n", res);
+			return res;
+		}
+	} else {
+		/* Default to BT.656 embedded sync */
+		core->mbus_type = V4L2_MBUS_BT656;
+	}
+
+	res = tvp5150_mc_init(core);
+	if (res)
+		return res;
+
+	res = tvp5150_detect_version(core);
+	if (res < 0)
+		return res;
+
+	/*
+	 * Iterate over all available connectors in case they are supported and
+	 * successfully parsed. Fallback to default autodetect in case they
+	 * aren't supported.
+	 */
+	for (i = 0; i < core->connectors_num; i++) {
+		struct v4l2_fwnode_connector *v4l2c;
+
+		v4l2c = &core->connectors[i].base;
+		core->norm |= v4l2c->connector.analog.sdtv_stds;
+	}
+
+	if (!core->connectors_num)
+		core->norm = V4L2_STD_ALL;
+
+	core->detected_norm = V4L2_STD_UNKNOWN;
+	core->input = TVP5150_COMPOSITE1;
+	core->enable = true;
+
+	v4l2_ctrl_handler_init(&core->hdl, 5);
+	v4l2_ctrl_new_std(&core->hdl, &tvp5150_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(&core->hdl, &tvp5150_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(&core->hdl, &tvp5150_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(&core->hdl, &tvp5150_ctrl_ops,
+			V4L2_CID_HUE, -128, 127, 1, 0);
+	v4l2_ctrl_new_std(&core->hdl, &tvp5150_ctrl_ops,
+			V4L2_CID_PIXEL_RATE, 27000000,
+			27000000, 1, 27000000);
+	v4l2_ctrl_new_std_menu_items(&core->hdl, &tvp5150_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(tvp5150_test_patterns) - 1,
+				     0, 0, tvp5150_test_patterns);
+	sd->ctrl_handler = &core->hdl;
+	if (core->hdl.error) {
+		res = core->hdl.error;
+		goto err;
+	}
+
+	tvp5150_set_default(tvp5150_read_std(sd), &core->rect);
+
+	core->irq = c->irq;
+	tvp5150_reset(sd, 0);	/* Calls v4l2_ctrl_handler_setup() */
+	if (c->irq) {
+		res = devm_request_threaded_irq(&c->dev, c->irq, NULL,
+						tvp5150_isr, IRQF_TRIGGER_HIGH |
+						IRQF_ONESHOT, "tvp5150", core);
+		if (res)
+			goto err;
+	}
+
+	res = v4l2_async_register_subdev(sd);
+	if (res < 0)
+		goto err;
+
+	if (debug > 1)
+		tvp5150_log_status(sd);
+
+	pm_runtime_set_active(&c->dev);
+	pm_runtime_enable(&c->dev);
+	pm_runtime_idle(&c->dev);
+
+	return 0;
+
+err:
+	v4l2_ctrl_handler_free(&core->hdl);
+	return res;
+}
+
+static void tvp5150_remove(struct i2c_client *c)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(c);
+	struct tvp5150 *decoder = to_tvp5150(sd);
+	unsigned int i;
+
+	dev_dbg_lvl(sd->dev, 1, debug,
+		"tvp5150.c: removing tvp5150 adapter on address 0x%x\n",
+		c->addr << 1);
+
+	for (i = 0; i < decoder->connectors_num; i++)
+		v4l2_fwnode_connector_free(&decoder->connectors[i].base);
+	for (i = 0; i < decoder->connectors_num; i++) {
+		media_device_unregister_entity(&decoder->connectors[i].ent);
+		media_entity_cleanup(&decoder->connectors[i].ent);
+	}
+	v4l2_async_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&decoder->hdl);
+	pm_runtime_disable(&c->dev);
+	pm_runtime_set_suspended(&c->dev);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct dev_pm_ops tvp5150_pm_ops = {
+	SET_RUNTIME_PM_OPS(tvp5150_runtime_suspend,
+			   tvp5150_runtime_resume,
+			   NULL)
+};
+
+static const struct i2c_device_id tvp5150_id[] = {
+	{ "tvp5150", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, tvp5150_id);
+
+#if IS_ENABLED(CONFIG_OF)
+static const struct of_device_id tvp5150_of_match[] = {
+	{ .compatible = "ti,tvp5150", },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, tvp5150_of_match);
+#endif
+
+static struct i2c_driver tvp5150_driver = {
+	.driver = {
+		.of_match_table = of_match_ptr(tvp5150_of_match),
+		.name	= "tvp5150",
+		.pm	= &tvp5150_pm_ops,
+	},
+	.probe		= tvp5150_probe,
+	.remove		= tvp5150_remove,
+	.id_table	= tvp5150_id,
+};
+
+module_i2c_driver(tvp5150_driver);
diff --git a/drivers/media/i2c/tvp5150_reg.h b/drivers/media/i2c/tvp5150_reg.h
new file mode 100644
index 000000000..f716129ad
--- /dev/null
+++ b/drivers/media/i2c/tvp5150_reg.h
@@ -0,0 +1,152 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ *
+ * tvp5150 - Texas Instruments TVP5150A/AM1 video decoder registers
+ *
+ * Copyright (c) 2005,2006 Mauro Carvalho Chehab <mchehab@kernel.org>
+ */
+
+#define TVP5150_VD_IN_SRC_SEL_1      0x00 /* Video input source selection #1 */
+#define TVP5150_ANAL_CHL_CTL         0x01 /* Analog channel controls */
+#define TVP5150_OP_MODE_CTL          0x02 /* Operation mode controls */
+#define TVP5150_MISC_CTL             0x03 /* Miscellaneous controls */
+#define TVP5150_MISC_CTL_VBLK_GPCL	BIT(7)
+#define TVP5150_MISC_CTL_GPCL		BIT(6)
+#define TVP5150_MISC_CTL_INTREQ_OE	BIT(5)
+#define TVP5150_MISC_CTL_HVLK		BIT(4)
+#define TVP5150_MISC_CTL_YCBCR_OE	BIT(3)
+#define TVP5150_MISC_CTL_SYNC_OE	BIT(2)
+#define TVP5150_MISC_CTL_VBLANK		BIT(1)
+#define TVP5150_MISC_CTL_CLOCK_OE	BIT(0)
+
+#define TVP5150_AUTOSW_MSK           0x04 /* Autoswitch mask: TVP5150A / TVP5150AM */
+
+/* Reserved 05h */
+
+#define TVP5150_COLOR_KIL_THSH_CTL   0x06 /* Color killer threshold control */
+#define TVP5150_LUMA_PROC_CTL_1      0x07 /* Luminance processing control #1 */
+#define TVP5150_LUMA_PROC_CTL_2      0x08 /* Luminance processing control #2 */
+#define TVP5150_BRIGHT_CTL           0x09 /* Brightness control */
+#define TVP5150_SATURATION_CTL       0x0a /* Color saturation control */
+#define TVP5150_HUE_CTL              0x0b /* Hue control */
+#define TVP5150_CONTRAST_CTL         0x0c /* Contrast control */
+#define TVP5150_DATA_RATE_SEL        0x0d /* Outputs and data rates select */
+#define TVP5150_LUMA_PROC_CTL_3      0x0e /* Luminance processing control #3 */
+#define TVP5150_CONF_SHARED_PIN      0x0f /* Configuration shared pins */
+
+/* Reserved 10h */
+
+#define TVP5150_ACT_VD_CROP_ST_MSB   0x11 /* Active video cropping start MSB */
+#define TVP5150_ACT_VD_CROP_ST_LSB   0x12 /* Active video cropping start LSB */
+#define TVP5150_ACT_VD_CROP_STP_MSB  0x13 /* Active video cropping stop MSB */
+#define TVP5150_ACT_VD_CROP_STP_LSB  0x14 /* Active video cropping stop LSB */
+#define TVP5150_GENLOCK              0x15 /* Genlock/RTC */
+#define TVP5150_HORIZ_SYNC_START     0x16 /* Horizontal sync start */
+
+/* Reserved 17h */
+
+#define TVP5150_VERT_BLANKING_START 0x18 /* Vertical blanking start */
+#define TVP5150_VERT_BLANKING_STOP  0x19 /* Vertical blanking stop */
+#define TVP5150_CHROMA_PROC_CTL_1   0x1a /* Chrominance processing control #1 */
+#define TVP5150_CHROMA_PROC_CTL_2   0x1b /* Chrominance processing control #2 */
+#define TVP5150_INT_RESET_REG_B     0x1c /* Interrupt reset register B */
+#define TVP5150_INT_ENABLE_REG_B    0x1d /* Interrupt enable register B */
+#define TVP5150_INTT_CONFIG_REG_B   0x1e /* Interrupt configuration register B */
+
+/* Reserved 1Fh-27h */
+
+#define VIDEO_STD_MASK			 (0x07 >> 1)
+#define TVP5150_VIDEO_STD                0x28 /* Video standard */
+#define VIDEO_STD_AUTO_SWITCH_BIT	 0x00
+#define VIDEO_STD_NTSC_MJ_BIT		 0x02
+#define VIDEO_STD_PAL_BDGHIN_BIT	 0x04
+#define VIDEO_STD_PAL_M_BIT		 0x06
+#define VIDEO_STD_PAL_COMBINATION_N_BIT	 0x08
+#define VIDEO_STD_NTSC_4_43_BIT		 0x0a
+#define VIDEO_STD_SECAM_BIT		 0x0c
+
+#define VIDEO_STD_NTSC_MJ_BIT_AS                 0x01
+#define VIDEO_STD_PAL_BDGHIN_BIT_AS              0x03
+#define VIDEO_STD_PAL_M_BIT_AS			 0x05
+#define VIDEO_STD_PAL_COMBINATION_N_BIT_AS	 0x07
+#define VIDEO_STD_NTSC_4_43_BIT_AS		 0x09
+#define VIDEO_STD_SECAM_BIT_AS			 0x0b
+
+/* Reserved 29h-2bh */
+
+#define TVP5150_CB_GAIN_FACT        0x2c /* Cb gain factor */
+#define TVP5150_CR_GAIN_FACTOR      0x2d /* Cr gain factor */
+#define TVP5150_MACROVISION_ON_CTR  0x2e /* Macrovision on counter */
+#define TVP5150_MACROVISION_OFF_CTR 0x2f /* Macrovision off counter */
+#define TVP5150_REV_SELECT          0x30 /* revision select (TVP5150AM1 only) */
+
+/* Reserved	31h-7Fh */
+
+#define TVP5150_MSB_DEV_ID          0x80 /* MSB of device ID */
+#define TVP5150_LSB_DEV_ID          0x81 /* LSB of device ID */
+#define TVP5150_ROM_MAJOR_VER       0x82 /* ROM major version */
+#define TVP5150_ROM_MINOR_VER       0x83 /* ROM minor version */
+#define TVP5150_VERT_LN_COUNT_MSB   0x84 /* Vertical line count MSB */
+#define TVP5150_VERT_LN_COUNT_LSB   0x85 /* Vertical line count LSB */
+#define TVP5150_INT_STATUS_REG_B    0x86 /* Interrupt status register B */
+#define TVP5150_INT_ACTIVE_REG_B    0x87 /* Interrupt active register B */
+#define TVP5150_STATUS_REG_1        0x88 /* Status register #1 */
+#define TVP5150_STATUS_REG_2        0x89 /* Status register #2 */
+#define TVP5150_STATUS_REG_3        0x8a /* Status register #3 */
+#define TVP5150_STATUS_REG_4        0x8b /* Status register #4 */
+#define TVP5150_STATUS_REG_5        0x8c /* Status register #5 */
+/* Reserved	8Dh-8Fh */
+ /* Closed caption data registers */
+#define TVP5150_CC_DATA_INI         0x90
+#define TVP5150_CC_DATA_END         0x93
+
+ /* WSS data registers */
+#define TVP5150_WSS_DATA_INI        0x94
+#define TVP5150_WSS_DATA_END        0x99
+
+/* VPS data registers */
+#define TVP5150_VPS_DATA_INI        0x9a
+#define TVP5150_VPS_DATA_END        0xa6
+
+/* VITC data registers */
+#define TVP5150_VITC_DATA_INI       0xa7
+#define TVP5150_VITC_DATA_END       0xaf
+
+#define TVP5150_VBI_FIFO_READ_DATA  0xb0 /* VBI FIFO read data */
+
+/* Teletext filter 1 */
+#define TVP5150_TELETEXT_FIL1_INI  0xb1
+#define TVP5150_TELETEXT_FIL1_END  0xb5
+
+/* Teletext filter 2 */
+#define TVP5150_TELETEXT_FIL2_INI  0xb6
+#define TVP5150_TELETEXT_FIL2_END  0xba
+
+#define TVP5150_TELETEXT_FIL_ENA    0xbb /* Teletext filter enable */
+/* Reserved	BCh-BFh */
+#define TVP5150_INT_STATUS_REG_A    0xc0 /* Interrupt status register A */
+#define   TVP5150_INT_A_LOCK_STATUS BIT(7)
+#define   TVP5150_INT_A_LOCK        BIT(6)
+#define TVP5150_INT_ENABLE_REG_A    0xc1 /* Interrupt enable register A */
+#define TVP5150_INT_CONF            0xc2 /* Interrupt configuration */
+#define   TVP5150_VDPOE             BIT(2)
+#define TVP5150_VDP_CONF_RAM_DATA   0xc3 /* VDP configuration RAM data */
+#define TVP5150_CONF_RAM_ADDR_LOW   0xc4 /* Configuration RAM address low byte */
+#define TVP5150_CONF_RAM_ADDR_HIGH  0xc5 /* Configuration RAM address high byte */
+#define TVP5150_VDP_STATUS_REG      0xc6 /* VDP status register */
+#define TVP5150_FIFO_WORD_COUNT     0xc7 /* FIFO word count */
+#define TVP5150_FIFO_INT_THRESHOLD  0xc8 /* FIFO interrupt threshold */
+#define TVP5150_FIFO_RESET          0xc9 /* FIFO reset */
+#define TVP5150_LINE_NUMBER_INT     0xca /* Line number interrupt */
+#define TVP5150_PIX_ALIGN_REG_LOW   0xcb /* Pixel alignment register low byte */
+#define TVP5150_PIX_ALIGN_REG_HIGH  0xcc /* Pixel alignment register high byte */
+#define TVP5150_FIFO_OUT_CTRL       0xcd /* FIFO output control */
+/* Reserved	CEh */
+#define TVP5150_FULL_FIELD_ENA      0xcf /* Full field enable 1 */
+
+/* Line mode registers */
+#define TVP5150_LINE_MODE_INI       0xd0
+#define TVP5150_LINE_MODE_END       0xfb
+
+#define TVP5150_FULL_FIELD_MODE_REG 0xfc /* Full field mode register */
+/* Reserved	FDh-FFh */
diff --git a/drivers/media/i2c/tvp7002.c b/drivers/media/i2c/tvp7002.c
new file mode 100644
index 000000000..a2d7bc799
--- /dev/null
+++ b/drivers/media/i2c/tvp7002.c
@@ -0,0 +1,1087 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/* Texas Instruments Triple 8-/10-BIT 165-/110-MSPS Video and Graphics
+ * Digitizer with Horizontal PLL registers
+ *
+ * Copyright (C) 2009 Texas Instruments Inc
+ * Author: Santiago Nunez-Corrales <santiago.nunez@ridgerun.com>
+ *
+ * This code is partially based upon the TVP5150 driver
+ * written by Mauro Carvalho Chehab <mchehab@kernel.org>,
+ * the TVP514x driver written by Vaibhav Hiremath <hvaibhav@ti.com>
+ * and the TVP7002 driver in the TI LSP 2.10.00.14. Revisions by
+ * Muralidharan Karicheri and Snehaprabha Narnakaje (TI).
+ */
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_graph.h>
+#include <linux/v4l2-dv-timings.h>
+#include <media/i2c/tvp7002.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fwnode.h>
+
+#include "tvp7002_reg.h"
+
+MODULE_DESCRIPTION("TI TVP7002 Video and Graphics Digitizer driver");
+MODULE_AUTHOR("Santiago Nunez-Corrales <santiago.nunez@ridgerun.com>");
+MODULE_LICENSE("GPL");
+
+/* I2C retry attempts */
+#define I2C_RETRY_COUNT		(5)
+
+/* End of registers */
+#define TVP7002_EOR		0x5c
+
+/* Read write definition for registers */
+#define TVP7002_READ		0
+#define TVP7002_WRITE		1
+#define TVP7002_RESERVED	2
+
+/* Interlaced vs progressive mask and shift */
+#define TVP7002_IP_SHIFT	5
+#define TVP7002_INPR_MASK	(0x01 << TVP7002_IP_SHIFT)
+
+/* Shift for CPL and LPF registers */
+#define TVP7002_CL_SHIFT	8
+#define TVP7002_CL_MASK		0x0f
+
+/* Debug functions */
+static bool debug;
+module_param(debug, bool, 0644);
+MODULE_PARM_DESC(debug, "Debug level (0-2)");
+
+/* Structure for register values */
+struct i2c_reg_value {
+	u8 reg;
+	u8 value;
+	u8 type;
+};
+
+/*
+ * Register default values (according to tvp7002 datasheet)
+ * In the case of read-only registers, the value (0xff) is
+ * never written. R/W functionality is controlled by the
+ * writable bit in the register struct definition.
+ */
+static const struct i2c_reg_value tvp7002_init_default[] = {
+	{ TVP7002_CHIP_REV, 0xff, TVP7002_READ },
+	{ TVP7002_HPLL_FDBK_DIV_MSBS, 0x67, TVP7002_WRITE },
+	{ TVP7002_HPLL_FDBK_DIV_LSBS, 0x20, TVP7002_WRITE },
+	{ TVP7002_HPLL_CRTL, 0xa0, TVP7002_WRITE },
+	{ TVP7002_HPLL_PHASE_SEL, 0x80, TVP7002_WRITE },
+	{ TVP7002_CLAMP_START, 0x32, TVP7002_WRITE },
+	{ TVP7002_CLAMP_W, 0x20, TVP7002_WRITE },
+	{ TVP7002_HSYNC_OUT_W, 0x60, TVP7002_WRITE },
+	{ TVP7002_B_FINE_GAIN, 0x00, TVP7002_WRITE },
+	{ TVP7002_G_FINE_GAIN, 0x00, TVP7002_WRITE },
+	{ TVP7002_R_FINE_GAIN, 0x00, TVP7002_WRITE },
+	{ TVP7002_B_FINE_OFF_MSBS, 0x80, TVP7002_WRITE },
+	{ TVP7002_G_FINE_OFF_MSBS, 0x80, TVP7002_WRITE },
+	{ TVP7002_R_FINE_OFF_MSBS, 0x80, TVP7002_WRITE },
+	{ TVP7002_SYNC_CTL_1, 0x20, TVP7002_WRITE },
+	{ TVP7002_HPLL_AND_CLAMP_CTL, 0x2e, TVP7002_WRITE },
+	{ TVP7002_SYNC_ON_G_THRS, 0x5d, TVP7002_WRITE },
+	{ TVP7002_SYNC_SEPARATOR_THRS, 0x47, TVP7002_WRITE },
+	{ TVP7002_HPLL_PRE_COAST, 0x00, TVP7002_WRITE },
+	{ TVP7002_HPLL_POST_COAST, 0x00, TVP7002_WRITE },
+	{ TVP7002_SYNC_DETECT_STAT, 0xff, TVP7002_READ },
+	{ TVP7002_OUT_FORMATTER, 0x47, TVP7002_WRITE },
+	{ TVP7002_MISC_CTL_1, 0x01, TVP7002_WRITE },
+	{ TVP7002_MISC_CTL_2, 0x00, TVP7002_WRITE },
+	{ TVP7002_MISC_CTL_3, 0x01, TVP7002_WRITE },
+	{ TVP7002_IN_MUX_SEL_1, 0x00, TVP7002_WRITE },
+	{ TVP7002_IN_MUX_SEL_2, 0x67, TVP7002_WRITE },
+	{ TVP7002_B_AND_G_COARSE_GAIN, 0x77, TVP7002_WRITE },
+	{ TVP7002_R_COARSE_GAIN, 0x07, TVP7002_WRITE },
+	{ TVP7002_FINE_OFF_LSBS, 0x00, TVP7002_WRITE },
+	{ TVP7002_B_COARSE_OFF, 0x10, TVP7002_WRITE },
+	{ TVP7002_G_COARSE_OFF, 0x10, TVP7002_WRITE },
+	{ TVP7002_R_COARSE_OFF, 0x10, TVP7002_WRITE },
+	{ TVP7002_HSOUT_OUT_START, 0x08, TVP7002_WRITE },
+	{ TVP7002_MISC_CTL_4, 0x00, TVP7002_WRITE },
+	{ TVP7002_B_DGTL_ALC_OUT_LSBS, 0xff, TVP7002_READ },
+	{ TVP7002_G_DGTL_ALC_OUT_LSBS, 0xff, TVP7002_READ },
+	{ TVP7002_R_DGTL_ALC_OUT_LSBS, 0xff, TVP7002_READ },
+	{ TVP7002_AUTO_LVL_CTL_ENABLE, 0x80, TVP7002_WRITE },
+	{ TVP7002_DGTL_ALC_OUT_MSBS, 0xff, TVP7002_READ },
+	{ TVP7002_AUTO_LVL_CTL_FILTER, 0x53, TVP7002_WRITE },
+	{ 0x29, 0x08, TVP7002_RESERVED },
+	{ TVP7002_FINE_CLAMP_CTL, 0x07, TVP7002_WRITE },
+	/* PWR_CTL is controlled only by the probe and reset functions */
+	{ TVP7002_PWR_CTL, 0x00, TVP7002_RESERVED },
+	{ TVP7002_ADC_SETUP, 0x50, TVP7002_WRITE },
+	{ TVP7002_COARSE_CLAMP_CTL, 0x00, TVP7002_WRITE },
+	{ TVP7002_SOG_CLAMP, 0x80, TVP7002_WRITE },
+	{ TVP7002_RGB_COARSE_CLAMP_CTL, 0x8c, TVP7002_WRITE },
+	{ TVP7002_SOG_COARSE_CLAMP_CTL, 0x04, TVP7002_WRITE },
+	{ TVP7002_ALC_PLACEMENT, 0x5a, TVP7002_WRITE },
+	{ 0x32, 0x18, TVP7002_RESERVED },
+	{ 0x33, 0x60, TVP7002_RESERVED },
+	{ TVP7002_MVIS_STRIPPER_W, 0xff, TVP7002_RESERVED },
+	{ TVP7002_VSYNC_ALGN, 0x10, TVP7002_WRITE },
+	{ TVP7002_SYNC_BYPASS, 0x00, TVP7002_WRITE },
+	{ TVP7002_L_FRAME_STAT_LSBS, 0xff, TVP7002_READ },
+	{ TVP7002_L_FRAME_STAT_MSBS, 0xff, TVP7002_READ },
+	{ TVP7002_CLK_L_STAT_LSBS, 0xff, TVP7002_READ },
+	{ TVP7002_CLK_L_STAT_MSBS, 0xff, TVP7002_READ },
+	{ TVP7002_HSYNC_W, 0xff, TVP7002_READ },
+	{ TVP7002_VSYNC_W, 0xff, TVP7002_READ },
+	{ TVP7002_L_LENGTH_TOL, 0x03, TVP7002_WRITE },
+	{ 0x3e, 0x60, TVP7002_RESERVED },
+	{ TVP7002_VIDEO_BWTH_CTL, 0x01, TVP7002_WRITE },
+	{ TVP7002_AVID_START_PIXEL_LSBS, 0x01, TVP7002_WRITE },
+	{ TVP7002_AVID_START_PIXEL_MSBS, 0x2c, TVP7002_WRITE },
+	{ TVP7002_AVID_STOP_PIXEL_LSBS, 0x06, TVP7002_WRITE },
+	{ TVP7002_AVID_STOP_PIXEL_MSBS, 0x2c, TVP7002_WRITE },
+	{ TVP7002_VBLK_F_0_START_L_OFF, 0x05, TVP7002_WRITE },
+	{ TVP7002_VBLK_F_1_START_L_OFF, 0x00, TVP7002_WRITE },
+	{ TVP7002_VBLK_F_0_DURATION, 0x1e, TVP7002_WRITE },
+	{ TVP7002_VBLK_F_1_DURATION, 0x00, TVP7002_WRITE },
+	{ TVP7002_FBIT_F_0_START_L_OFF, 0x00, TVP7002_WRITE },
+	{ TVP7002_FBIT_F_1_START_L_OFF, 0x00, TVP7002_WRITE },
+	{ TVP7002_YUV_Y_G_COEF_LSBS, 0xe3, TVP7002_WRITE },
+	{ TVP7002_YUV_Y_G_COEF_MSBS, 0x16, TVP7002_WRITE },
+	{ TVP7002_YUV_Y_B_COEF_LSBS, 0x4f, TVP7002_WRITE },
+	{ TVP7002_YUV_Y_B_COEF_MSBS, 0x02, TVP7002_WRITE },
+	{ TVP7002_YUV_Y_R_COEF_LSBS, 0xce, TVP7002_WRITE },
+	{ TVP7002_YUV_Y_R_COEF_MSBS, 0x06, TVP7002_WRITE },
+	{ TVP7002_YUV_U_G_COEF_LSBS, 0xab, TVP7002_WRITE },
+	{ TVP7002_YUV_U_G_COEF_MSBS, 0xf3, TVP7002_WRITE },
+	{ TVP7002_YUV_U_B_COEF_LSBS, 0x00, TVP7002_WRITE },
+	{ TVP7002_YUV_U_B_COEF_MSBS, 0x10, TVP7002_WRITE },
+	{ TVP7002_YUV_U_R_COEF_LSBS, 0x55, TVP7002_WRITE },
+	{ TVP7002_YUV_U_R_COEF_MSBS, 0xfc, TVP7002_WRITE },
+	{ TVP7002_YUV_V_G_COEF_LSBS, 0x78, TVP7002_WRITE },
+	{ TVP7002_YUV_V_G_COEF_MSBS, 0xf1, TVP7002_WRITE },
+	{ TVP7002_YUV_V_B_COEF_LSBS, 0x88, TVP7002_WRITE },
+	{ TVP7002_YUV_V_B_COEF_MSBS, 0xfe, TVP7002_WRITE },
+	{ TVP7002_YUV_V_R_COEF_LSBS, 0x00, TVP7002_WRITE },
+	{ TVP7002_YUV_V_R_COEF_MSBS, 0x10, TVP7002_WRITE },
+	/* This signals end of register values */
+	{ TVP7002_EOR, 0xff, TVP7002_RESERVED }
+};
+
+/* Register parameters for 480P */
+static const struct i2c_reg_value tvp7002_parms_480P[] = {
+	{ TVP7002_HPLL_FDBK_DIV_MSBS, 0x35, TVP7002_WRITE },
+	{ TVP7002_HPLL_FDBK_DIV_LSBS, 0xa0, TVP7002_WRITE },
+	{ TVP7002_HPLL_CRTL, 0x02, TVP7002_WRITE },
+	{ TVP7002_AVID_START_PIXEL_LSBS, 0x91, TVP7002_WRITE },
+	{ TVP7002_AVID_START_PIXEL_MSBS, 0x00, TVP7002_WRITE },
+	{ TVP7002_AVID_STOP_PIXEL_LSBS, 0x0B, TVP7002_WRITE },
+	{ TVP7002_AVID_STOP_PIXEL_MSBS, 0x00, TVP7002_WRITE },
+	{ TVP7002_VBLK_F_0_START_L_OFF, 0x03, TVP7002_WRITE },
+	{ TVP7002_VBLK_F_1_START_L_OFF, 0x01, TVP7002_WRITE },
+	{ TVP7002_VBLK_F_0_DURATION, 0x13, TVP7002_WRITE },
+	{ TVP7002_VBLK_F_1_DURATION, 0x13, TVP7002_WRITE },
+	{ TVP7002_ALC_PLACEMENT, 0x18, TVP7002_WRITE },
+	{ TVP7002_CLAMP_START, 0x06, TVP7002_WRITE },
+	{ TVP7002_CLAMP_W, 0x10, TVP7002_WRITE },
+	{ TVP7002_HPLL_PRE_COAST, 0x03, TVP7002_WRITE },
+	{ TVP7002_HPLL_POST_COAST, 0x03, TVP7002_WRITE },
+	{ TVP7002_EOR, 0xff, TVP7002_RESERVED }
+};
+
+/* Register parameters for 576P */
+static const struct i2c_reg_value tvp7002_parms_576P[] = {
+	{ TVP7002_HPLL_FDBK_DIV_MSBS, 0x36, TVP7002_WRITE },
+	{ TVP7002_HPLL_FDBK_DIV_LSBS, 0x00, TVP7002_WRITE },
+	{ TVP7002_HPLL_CRTL, 0x18, TVP7002_WRITE },
+	{ TVP7002_AVID_START_PIXEL_LSBS, 0x9B, TVP7002_WRITE },
+	{ TVP7002_AVID_START_PIXEL_MSBS, 0x00, TVP7002_WRITE },
+	{ TVP7002_AVID_STOP_PIXEL_LSBS, 0x0F, TVP7002_WRITE },
+	{ TVP7002_AVID_STOP_PIXEL_MSBS, 0x00, TVP7002_WRITE },
+	{ TVP7002_VBLK_F_0_START_L_OFF, 0x00, TVP7002_WRITE },
+	{ TVP7002_VBLK_F_1_START_L_OFF, 0x00, TVP7002_WRITE },
+	{ TVP7002_VBLK_F_0_DURATION, 0x2D, TVP7002_WRITE },
+	{ TVP7002_VBLK_F_1_DURATION, 0x00, TVP7002_WRITE },
+	{ TVP7002_ALC_PLACEMENT, 0x18, TVP7002_WRITE },
+	{ TVP7002_CLAMP_START, 0x06, TVP7002_WRITE },
+	{ TVP7002_CLAMP_W, 0x10, TVP7002_WRITE },
+	{ TVP7002_HPLL_PRE_COAST, 0x03, TVP7002_WRITE },
+	{ TVP7002_HPLL_POST_COAST, 0x03, TVP7002_WRITE },
+	{ TVP7002_EOR, 0xff, TVP7002_RESERVED }
+};
+
+/* Register parameters for 1080I60 */
+static const struct i2c_reg_value tvp7002_parms_1080I60[] = {
+	{ TVP7002_HPLL_FDBK_DIV_MSBS, 0x89, TVP7002_WRITE },
+	{ TVP7002_HPLL_FDBK_DIV_LSBS, 0x80, TVP7002_WRITE },
+	{ TVP7002_HPLL_CRTL, 0x98, TVP7002_WRITE },
+	{ TVP7002_AVID_START_PIXEL_LSBS, 0x06, TVP7002_WRITE },
+	{ TVP7002_AVID_START_PIXEL_MSBS, 0x01, TVP7002_WRITE },
+	{ TVP7002_AVID_STOP_PIXEL_LSBS, 0x8a, TVP7002_WRITE },
+	{ TVP7002_AVID_STOP_PIXEL_MSBS, 0x08, TVP7002_WRITE },
+	{ TVP7002_VBLK_F_0_START_L_OFF, 0x02, TVP7002_WRITE },
+	{ TVP7002_VBLK_F_1_START_L_OFF, 0x02, TVP7002_WRITE },
+	{ TVP7002_VBLK_F_0_DURATION, 0x16, TVP7002_WRITE },
+	{ TVP7002_VBLK_F_1_DURATION, 0x17, TVP7002_WRITE },
+	{ TVP7002_ALC_PLACEMENT, 0x5a, TVP7002_WRITE },
+	{ TVP7002_CLAMP_START, 0x32, TVP7002_WRITE },
+	{ TVP7002_CLAMP_W, 0x20, TVP7002_WRITE },
+	{ TVP7002_HPLL_PRE_COAST, 0x01, TVP7002_WRITE },
+	{ TVP7002_HPLL_POST_COAST, 0x00, TVP7002_WRITE },
+	{ TVP7002_EOR, 0xff, TVP7002_RESERVED }
+};
+
+/* Register parameters for 1080P60 */
+static const struct i2c_reg_value tvp7002_parms_1080P60[] = {
+	{ TVP7002_HPLL_FDBK_DIV_MSBS, 0x89, TVP7002_WRITE },
+	{ TVP7002_HPLL_FDBK_DIV_LSBS, 0x80, TVP7002_WRITE },
+	{ TVP7002_HPLL_CRTL, 0xE0, TVP7002_WRITE },
+	{ TVP7002_AVID_START_PIXEL_LSBS, 0x06, TVP7002_WRITE },
+	{ TVP7002_AVID_START_PIXEL_MSBS, 0x01, TVP7002_WRITE },
+	{ TVP7002_AVID_STOP_PIXEL_LSBS, 0x8a, TVP7002_WRITE },
+	{ TVP7002_AVID_STOP_PIXEL_MSBS, 0x08, TVP7002_WRITE },
+	{ TVP7002_VBLK_F_0_START_L_OFF, 0x02, TVP7002_WRITE },
+	{ TVP7002_VBLK_F_1_START_L_OFF, 0x02, TVP7002_WRITE },
+	{ TVP7002_VBLK_F_0_DURATION, 0x16, TVP7002_WRITE },
+	{ TVP7002_VBLK_F_1_DURATION, 0x17, TVP7002_WRITE },
+	{ TVP7002_ALC_PLACEMENT, 0x5a, TVP7002_WRITE },
+	{ TVP7002_CLAMP_START, 0x32, TVP7002_WRITE },
+	{ TVP7002_CLAMP_W, 0x20, TVP7002_WRITE },
+	{ TVP7002_HPLL_PRE_COAST, 0x01, TVP7002_WRITE },
+	{ TVP7002_HPLL_POST_COAST, 0x00, TVP7002_WRITE },
+	{ TVP7002_EOR, 0xff, TVP7002_RESERVED }
+};
+
+/* Register parameters for 1080I50 */
+static const struct i2c_reg_value tvp7002_parms_1080I50[] = {
+	{ TVP7002_HPLL_FDBK_DIV_MSBS, 0xa5, TVP7002_WRITE },
+	{ TVP7002_HPLL_FDBK_DIV_LSBS, 0x00, TVP7002_WRITE },
+	{ TVP7002_HPLL_CRTL, 0x98, TVP7002_WRITE },
+	{ TVP7002_AVID_START_PIXEL_LSBS, 0x06, TVP7002_WRITE },
+	{ TVP7002_AVID_START_PIXEL_MSBS, 0x01, TVP7002_WRITE },
+	{ TVP7002_AVID_STOP_PIXEL_LSBS, 0x8a, TVP7002_WRITE },
+	{ TVP7002_AVID_STOP_PIXEL_MSBS, 0x08, TVP7002_WRITE },
+	{ TVP7002_VBLK_F_0_START_L_OFF, 0x02, TVP7002_WRITE },
+	{ TVP7002_VBLK_F_1_START_L_OFF, 0x02, TVP7002_WRITE },
+	{ TVP7002_VBLK_F_0_DURATION, 0x16, TVP7002_WRITE },
+	{ TVP7002_VBLK_F_1_DURATION, 0x17, TVP7002_WRITE },
+	{ TVP7002_ALC_PLACEMENT, 0x5a, TVP7002_WRITE },
+	{ TVP7002_CLAMP_START, 0x32, TVP7002_WRITE },
+	{ TVP7002_CLAMP_W, 0x20, TVP7002_WRITE },
+	{ TVP7002_HPLL_PRE_COAST, 0x01, TVP7002_WRITE },
+	{ TVP7002_HPLL_POST_COAST, 0x00, TVP7002_WRITE },
+	{ TVP7002_EOR, 0xff, TVP7002_RESERVED }
+};
+
+/* Register parameters for 720P60 */
+static const struct i2c_reg_value tvp7002_parms_720P60[] = {
+	{ TVP7002_HPLL_FDBK_DIV_MSBS, 0x67, TVP7002_WRITE },
+	{ TVP7002_HPLL_FDBK_DIV_LSBS, 0x20, TVP7002_WRITE },
+	{ TVP7002_HPLL_CRTL, 0xa0, TVP7002_WRITE },
+	{ TVP7002_AVID_START_PIXEL_LSBS, 0x47, TVP7002_WRITE },
+	{ TVP7002_AVID_START_PIXEL_MSBS, 0x01, TVP7002_WRITE },
+	{ TVP7002_AVID_STOP_PIXEL_LSBS, 0x4B, TVP7002_WRITE },
+	{ TVP7002_AVID_STOP_PIXEL_MSBS, 0x06, TVP7002_WRITE },
+	{ TVP7002_VBLK_F_0_START_L_OFF, 0x05, TVP7002_WRITE },
+	{ TVP7002_VBLK_F_1_START_L_OFF, 0x00, TVP7002_WRITE },
+	{ TVP7002_VBLK_F_0_DURATION, 0x2D, TVP7002_WRITE },
+	{ TVP7002_VBLK_F_1_DURATION, 0x00, TVP7002_WRITE },
+	{ TVP7002_ALC_PLACEMENT, 0x5a, TVP7002_WRITE },
+	{ TVP7002_CLAMP_START, 0x32, TVP7002_WRITE },
+	{ TVP7002_CLAMP_W, 0x20, TVP7002_WRITE },
+	{ TVP7002_HPLL_PRE_COAST, 0x00, TVP7002_WRITE },
+	{ TVP7002_HPLL_POST_COAST, 0x00, TVP7002_WRITE },
+	{ TVP7002_EOR, 0xff, TVP7002_RESERVED }
+};
+
+/* Register parameters for 720P50 */
+static const struct i2c_reg_value tvp7002_parms_720P50[] = {
+	{ TVP7002_HPLL_FDBK_DIV_MSBS, 0x7b, TVP7002_WRITE },
+	{ TVP7002_HPLL_FDBK_DIV_LSBS, 0xc0, TVP7002_WRITE },
+	{ TVP7002_HPLL_CRTL, 0x98, TVP7002_WRITE },
+	{ TVP7002_AVID_START_PIXEL_LSBS, 0x47, TVP7002_WRITE },
+	{ TVP7002_AVID_START_PIXEL_MSBS, 0x01, TVP7002_WRITE },
+	{ TVP7002_AVID_STOP_PIXEL_LSBS, 0x4B, TVP7002_WRITE },
+	{ TVP7002_AVID_STOP_PIXEL_MSBS, 0x06, TVP7002_WRITE },
+	{ TVP7002_VBLK_F_0_START_L_OFF, 0x05, TVP7002_WRITE },
+	{ TVP7002_VBLK_F_1_START_L_OFF, 0x00, TVP7002_WRITE },
+	{ TVP7002_VBLK_F_0_DURATION, 0x2D, TVP7002_WRITE },
+	{ TVP7002_VBLK_F_1_DURATION, 0x00, TVP7002_WRITE },
+	{ TVP7002_ALC_PLACEMENT, 0x5a, TVP7002_WRITE },
+	{ TVP7002_CLAMP_START, 0x32, TVP7002_WRITE },
+	{ TVP7002_CLAMP_W, 0x20, TVP7002_WRITE },
+	{ TVP7002_HPLL_PRE_COAST, 0x01, TVP7002_WRITE },
+	{ TVP7002_HPLL_POST_COAST, 0x00, TVP7002_WRITE },
+	{ TVP7002_EOR, 0xff, TVP7002_RESERVED }
+};
+
+/* Timings definition for handling device operation */
+struct tvp7002_timings_definition {
+	struct v4l2_dv_timings timings;
+	const struct i2c_reg_value *p_settings;
+	enum v4l2_colorspace color_space;
+	enum v4l2_field scanmode;
+	u16 progressive;
+	u16 lines_per_frame;
+	u16 cpl_min;
+	u16 cpl_max;
+};
+
+/* Struct list for digital video timings */
+static const struct tvp7002_timings_definition tvp7002_timings[] = {
+	{
+		V4L2_DV_BT_CEA_1280X720P60,
+		tvp7002_parms_720P60,
+		V4L2_COLORSPACE_REC709,
+		V4L2_FIELD_NONE,
+		1,
+		0x2EE,
+		135,
+		153
+	},
+	{
+		V4L2_DV_BT_CEA_1920X1080I60,
+		tvp7002_parms_1080I60,
+		V4L2_COLORSPACE_REC709,
+		V4L2_FIELD_INTERLACED,
+		0,
+		0x465,
+		181,
+		205
+	},
+	{
+		V4L2_DV_BT_CEA_1920X1080I50,
+		tvp7002_parms_1080I50,
+		V4L2_COLORSPACE_REC709,
+		V4L2_FIELD_INTERLACED,
+		0,
+		0x465,
+		217,
+		245
+	},
+	{
+		V4L2_DV_BT_CEA_1280X720P50,
+		tvp7002_parms_720P50,
+		V4L2_COLORSPACE_REC709,
+		V4L2_FIELD_NONE,
+		1,
+		0x2EE,
+		163,
+		183
+	},
+	{
+		V4L2_DV_BT_CEA_1920X1080P60,
+		tvp7002_parms_1080P60,
+		V4L2_COLORSPACE_REC709,
+		V4L2_FIELD_NONE,
+		1,
+		0x465,
+		90,
+		102
+	},
+	{
+		V4L2_DV_BT_CEA_720X480P59_94,
+		tvp7002_parms_480P,
+		V4L2_COLORSPACE_SMPTE170M,
+		V4L2_FIELD_NONE,
+		1,
+		0x20D,
+		0xffff,
+		0xffff
+	},
+	{
+		V4L2_DV_BT_CEA_720X576P50,
+		tvp7002_parms_576P,
+		V4L2_COLORSPACE_SMPTE170M,
+		V4L2_FIELD_NONE,
+		1,
+		0x271,
+		0xffff,
+		0xffff
+	}
+};
+
+#define NUM_TIMINGS ARRAY_SIZE(tvp7002_timings)
+
+/* Device definition */
+struct tvp7002 {
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
+	const struct tvp7002_config *pdata;
+
+	int ver;
+	int streaming;
+
+	const struct tvp7002_timings_definition *current_timings;
+	struct media_pad pad;
+};
+
+/*
+ * to_tvp7002 - Obtain device handler TVP7002
+ * @sd: ptr to v4l2_subdev struct
+ *
+ * Returns device handler tvp7002.
+ */
+static inline struct tvp7002 *to_tvp7002(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct tvp7002, sd);
+}
+
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct tvp7002, hdl)->sd;
+}
+
+/*
+ * tvp7002_read - Read a value from a register in an TVP7002
+ * @sd: ptr to v4l2_subdev struct
+ * @addr: TVP7002 register address
+ * @dst: pointer to 8-bit destination
+ *
+ * Returns value read if successful, or non-zero (-1) otherwise.
+ */
+static int tvp7002_read(struct v4l2_subdev *sd, u8 addr, u8 *dst)
+{
+	struct i2c_client *c = v4l2_get_subdevdata(sd);
+	int retry;
+	int error;
+
+	for (retry = 0; retry < I2C_RETRY_COUNT; retry++) {
+		error = i2c_smbus_read_byte_data(c, addr);
+
+		if (error >= 0) {
+			*dst = (u8)error;
+			return 0;
+		}
+
+		msleep_interruptible(10);
+	}
+	v4l2_err(sd, "TVP7002 read error %d\n", error);
+	return error;
+}
+
+/*
+ * tvp7002_read_err() - Read a register value with error code
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @reg: destination register
+ * @val: value to be read
+ * @err: pointer to error value
+ *
+ * Read a value in a register and save error value in pointer.
+ * Also update the register table if successful
+ */
+static inline void tvp7002_read_err(struct v4l2_subdev *sd, u8 reg,
+							u8 *dst, int *err)
+{
+	if (!*err)
+		*err = tvp7002_read(sd, reg, dst);
+}
+
+/*
+ * tvp7002_write() - Write a value to a register in TVP7002
+ * @sd: ptr to v4l2_subdev struct
+ * @addr: TVP7002 register address
+ * @value: value to be written to the register
+ *
+ * Write a value to a register in an TVP7002 decoder device.
+ * Returns zero if successful, or non-zero otherwise.
+ */
+static int tvp7002_write(struct v4l2_subdev *sd, u8 addr, u8 value)
+{
+	struct i2c_client *c;
+	int retry;
+	int error;
+
+	c = v4l2_get_subdevdata(sd);
+
+	for (retry = 0; retry < I2C_RETRY_COUNT; retry++) {
+		error = i2c_smbus_write_byte_data(c, addr, value);
+
+		if (error >= 0)
+			return 0;
+
+		v4l2_warn(sd, "Write: retry ... %d\n", retry);
+		msleep_interruptible(10);
+	}
+	v4l2_err(sd, "TVP7002 write error %d\n", error);
+	return error;
+}
+
+/*
+ * tvp7002_write_err() - Write a register value with error code
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @reg: destination register
+ * @val: value to be written
+ * @err: pointer to error value
+ *
+ * Write a value in a register and save error value in pointer.
+ * Also update the register table if successful
+ */
+static inline void tvp7002_write_err(struct v4l2_subdev *sd, u8 reg,
+							u8 val, int *err)
+{
+	if (!*err)
+		*err = tvp7002_write(sd, reg, val);
+}
+
+/*
+ * tvp7002_write_inittab() - Write initialization values
+ * @sd: ptr to v4l2_subdev struct
+ * @regs: ptr to i2c_reg_value struct
+ *
+ * Write initialization values.
+ * Returns zero or -EINVAL if read operation fails.
+ */
+static int tvp7002_write_inittab(struct v4l2_subdev *sd,
+					const struct i2c_reg_value *regs)
+{
+	int error = 0;
+
+	/* Initialize the first (defined) registers */
+	while (TVP7002_EOR != regs->reg) {
+		if (TVP7002_WRITE == regs->type)
+			tvp7002_write_err(sd, regs->reg, regs->value, &error);
+		regs++;
+	}
+
+	return error;
+}
+
+static int tvp7002_s_dv_timings(struct v4l2_subdev *sd,
+					struct v4l2_dv_timings *dv_timings)
+{
+	struct tvp7002 *device = to_tvp7002(sd);
+	const struct v4l2_bt_timings *bt = &dv_timings->bt;
+	int i;
+
+	if (dv_timings->type != V4L2_DV_BT_656_1120)
+		return -EINVAL;
+	for (i = 0; i < NUM_TIMINGS; i++) {
+		const struct v4l2_bt_timings *t = &tvp7002_timings[i].timings.bt;
+
+		if (!memcmp(bt, t, &bt->standards - &bt->width)) {
+			device->current_timings = &tvp7002_timings[i];
+			return tvp7002_write_inittab(sd, tvp7002_timings[i].p_settings);
+		}
+	}
+	return -EINVAL;
+}
+
+static int tvp7002_g_dv_timings(struct v4l2_subdev *sd,
+					struct v4l2_dv_timings *dv_timings)
+{
+	struct tvp7002 *device = to_tvp7002(sd);
+
+	*dv_timings = device->current_timings->timings;
+	return 0;
+}
+
+/*
+ * tvp7002_s_ctrl() - Set a control
+ * @ctrl: ptr to v4l2_ctrl struct
+ *
+ * Set a control in TVP7002 decoder device.
+ * Returns zero when successful or -EINVAL if register access fails.
+ */
+static int tvp7002_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+	int error = 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_GAIN:
+		tvp7002_write_err(sd, TVP7002_R_FINE_GAIN, ctrl->val, &error);
+		tvp7002_write_err(sd, TVP7002_G_FINE_GAIN, ctrl->val, &error);
+		tvp7002_write_err(sd, TVP7002_B_FINE_GAIN, ctrl->val, &error);
+		return error;
+	}
+	return -EINVAL;
+}
+
+/*
+ * tvp7002_query_dv() - query DV timings
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @index: index into the tvp7002_timings array
+ *
+ * Returns the current DV timings detected by TVP7002. If no active input is
+ * detected, returns -EINVAL
+ */
+static int tvp7002_query_dv(struct v4l2_subdev *sd, int *index)
+{
+	const struct tvp7002_timings_definition *timings = tvp7002_timings;
+	u8 progressive;
+	u32 lpfr;
+	u32 cpln;
+	int error = 0;
+	u8 lpf_lsb;
+	u8 lpf_msb;
+	u8 cpl_lsb;
+	u8 cpl_msb;
+
+	/* Return invalid index if no active input is detected */
+	*index = NUM_TIMINGS;
+
+	/* Read standards from device registers */
+	tvp7002_read_err(sd, TVP7002_L_FRAME_STAT_LSBS, &lpf_lsb, &error);
+	tvp7002_read_err(sd, TVP7002_L_FRAME_STAT_MSBS, &lpf_msb, &error);
+
+	if (error < 0)
+		return error;
+
+	tvp7002_read_err(sd, TVP7002_CLK_L_STAT_LSBS, &cpl_lsb, &error);
+	tvp7002_read_err(sd, TVP7002_CLK_L_STAT_MSBS, &cpl_msb, &error);
+
+	if (error < 0)
+		return error;
+
+	/* Get lines per frame, clocks per line and interlaced/progresive */
+	lpfr = lpf_lsb | ((TVP7002_CL_MASK & lpf_msb) << TVP7002_CL_SHIFT);
+	cpln = cpl_lsb | ((TVP7002_CL_MASK & cpl_msb) << TVP7002_CL_SHIFT);
+	progressive = (lpf_msb & TVP7002_INPR_MASK) >> TVP7002_IP_SHIFT;
+
+	/* Do checking of video modes */
+	for (*index = 0; *index < NUM_TIMINGS; (*index)++, timings++)
+		if (lpfr == timings->lines_per_frame &&
+			progressive == timings->progressive) {
+			if (timings->cpl_min == 0xffff)
+				break;
+			if (cpln >= timings->cpl_min && cpln <= timings->cpl_max)
+				break;
+		}
+
+	if (*index == NUM_TIMINGS) {
+		v4l2_dbg(1, debug, sd, "detection failed: lpf = %x, cpl = %x\n",
+								lpfr, cpln);
+		return -ENOLINK;
+	}
+
+	/* Update lines per frame and clocks per line info */
+	v4l2_dbg(1, debug, sd, "detected timings: %d\n", *index);
+	return 0;
+}
+
+static int tvp7002_query_dv_timings(struct v4l2_subdev *sd,
+					struct v4l2_dv_timings *timings)
+{
+	int index;
+	int err = tvp7002_query_dv(sd, &index);
+
+	if (err)
+		return err;
+	*timings = tvp7002_timings[index].timings;
+	return 0;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+/*
+ * tvp7002_g_register() - Get the value of a register
+ * @sd: ptr to v4l2_subdev struct
+ * @reg: ptr to v4l2_dbg_register struct
+ *
+ * Get the value of a TVP7002 decoder device register.
+ * Returns zero when successful, -EINVAL if register read fails or
+ * access to I2C client fails.
+ */
+static int tvp7002_g_register(struct v4l2_subdev *sd,
+						struct v4l2_dbg_register *reg)
+{
+	u8 val;
+	int ret;
+
+	ret = tvp7002_read(sd, reg->reg & 0xff, &val);
+	if (ret < 0)
+		return ret;
+	reg->val = val;
+	reg->size = 1;
+	return 0;
+}
+
+/*
+ * tvp7002_s_register() - set a control
+ * @sd: ptr to v4l2_subdev struct
+ * @reg: ptr to v4l2_dbg_register struct
+ *
+ * Get the value of a TVP7002 decoder device register.
+ * Returns zero when successful, -EINVAL if register read fails.
+ */
+static int tvp7002_s_register(struct v4l2_subdev *sd,
+						const struct v4l2_dbg_register *reg)
+{
+	return tvp7002_write(sd, reg->reg & 0xff, reg->val & 0xff);
+}
+#endif
+
+/*
+ * tvp7002_s_stream() - V4L2 decoder i/f handler for s_stream
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @enable: streaming enable or disable
+ *
+ * Sets streaming to enable or disable, if possible.
+ */
+static int tvp7002_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct tvp7002 *device = to_tvp7002(sd);
+	int error;
+
+	if (device->streaming == enable)
+		return 0;
+
+	/* low impedance: on, high impedance: off */
+	error = tvp7002_write(sd, TVP7002_MISC_CTL_2, enable ? 0x00 : 0x03);
+	if (error) {
+		v4l2_dbg(1, debug, sd, "Fail to set streaming\n");
+		return error;
+	}
+
+	device->streaming = enable;
+	return 0;
+}
+
+/*
+ * tvp7002_log_status() - Print information about register settings
+ * @sd: ptr to v4l2_subdev struct
+ *
+ * Log register values of a TVP7002 decoder device.
+ * Returns zero or -EINVAL if read operation fails.
+ */
+static int tvp7002_log_status(struct v4l2_subdev *sd)
+{
+	struct tvp7002 *device = to_tvp7002(sd);
+	const struct v4l2_bt_timings *bt;
+	int detected;
+
+	/* Find my current timings */
+	tvp7002_query_dv(sd, &detected);
+
+	bt = &device->current_timings->timings.bt;
+	v4l2_info(sd, "Selected DV Timings: %ux%u\n", bt->width, bt->height);
+	if (detected == NUM_TIMINGS) {
+		v4l2_info(sd, "Detected DV Timings: None\n");
+	} else {
+		bt = &tvp7002_timings[detected].timings.bt;
+		v4l2_info(sd, "Detected DV Timings: %ux%u\n",
+				bt->width, bt->height);
+	}
+	v4l2_info(sd, "Streaming enabled: %s\n",
+					device->streaming ? "yes" : "no");
+
+	/* Print the current value of the gain control */
+	v4l2_ctrl_handler_log_status(&device->hdl, sd->name);
+
+	return 0;
+}
+
+static int tvp7002_enum_dv_timings(struct v4l2_subdev *sd,
+		struct v4l2_enum_dv_timings *timings)
+{
+	if (timings->pad != 0)
+		return -EINVAL;
+
+	/* Check requested format index is within range */
+	if (timings->index >= NUM_TIMINGS)
+		return -EINVAL;
+
+	timings->timings = tvp7002_timings[timings->index].timings;
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops tvp7002_ctrl_ops = {
+	.s_ctrl = tvp7002_s_ctrl,
+};
+
+/*
+ * tvp7002_enum_mbus_code() - Enum supported digital video format on pad
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @cfg: pad configuration
+ * @code: pointer to subdev enum mbus code struct
+ *
+ * Enumerate supported digital video formats for pad.
+ */
+static int
+tvp7002_enum_mbus_code(struct v4l2_subdev *sd,
+		       struct v4l2_subdev_state *sd_state,
+		       struct v4l2_subdev_mbus_code_enum *code)
+{
+	/* Check requested format index is within range */
+	if (code->index != 0)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_YUYV10_1X20;
+
+	return 0;
+}
+
+/*
+ * tvp7002_get_pad_format() - get video format on pad
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @cfg: pad configuration
+ * @fmt: pointer to subdev format struct
+ *
+ * get video format for pad.
+ */
+static int
+tvp7002_get_pad_format(struct v4l2_subdev *sd,
+		       struct v4l2_subdev_state *sd_state,
+		       struct v4l2_subdev_format *fmt)
+{
+	struct tvp7002 *tvp7002 = to_tvp7002(sd);
+
+	fmt->format.code = MEDIA_BUS_FMT_YUYV10_1X20;
+	fmt->format.width = tvp7002->current_timings->timings.bt.width;
+	fmt->format.height = tvp7002->current_timings->timings.bt.height;
+	fmt->format.field = tvp7002->current_timings->scanmode;
+	fmt->format.colorspace = tvp7002->current_timings->color_space;
+
+	return 0;
+}
+
+/*
+ * tvp7002_set_pad_format() - set video format on pad
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @cfg: pad configuration
+ * @fmt: pointer to subdev format struct
+ *
+ * set video format for pad.
+ */
+static int
+tvp7002_set_pad_format(struct v4l2_subdev *sd,
+		       struct v4l2_subdev_state *sd_state,
+		       struct v4l2_subdev_format *fmt)
+{
+	return tvp7002_get_pad_format(sd, sd_state, fmt);
+}
+
+/* V4L2 core operation handlers */
+static const struct v4l2_subdev_core_ops tvp7002_core_ops = {
+	.log_status = tvp7002_log_status,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = tvp7002_g_register,
+	.s_register = tvp7002_s_register,
+#endif
+};
+
+/* Specific video subsystem operation handlers */
+static const struct v4l2_subdev_video_ops tvp7002_video_ops = {
+	.g_dv_timings = tvp7002_g_dv_timings,
+	.s_dv_timings = tvp7002_s_dv_timings,
+	.query_dv_timings = tvp7002_query_dv_timings,
+	.s_stream = tvp7002_s_stream,
+};
+
+/* media pad related operation handlers */
+static const struct v4l2_subdev_pad_ops tvp7002_pad_ops = {
+	.enum_mbus_code = tvp7002_enum_mbus_code,
+	.get_fmt = tvp7002_get_pad_format,
+	.set_fmt = tvp7002_set_pad_format,
+	.enum_dv_timings = tvp7002_enum_dv_timings,
+};
+
+/* V4L2 top level operation handlers */
+static const struct v4l2_subdev_ops tvp7002_ops = {
+	.core = &tvp7002_core_ops,
+	.video = &tvp7002_video_ops,
+	.pad = &tvp7002_pad_ops,
+};
+
+static struct tvp7002_config *
+tvp7002_get_pdata(struct i2c_client *client)
+{
+	struct v4l2_fwnode_endpoint bus_cfg = { .bus_type = 0 };
+	struct tvp7002_config *pdata = NULL;
+	struct device_node *endpoint;
+	unsigned int flags;
+
+	if (!IS_ENABLED(CONFIG_OF) || !client->dev.of_node)
+		return client->dev.platform_data;
+
+	endpoint = of_graph_get_next_endpoint(client->dev.of_node, NULL);
+	if (!endpoint)
+		return NULL;
+
+	if (v4l2_fwnode_endpoint_parse(of_fwnode_handle(endpoint), &bus_cfg))
+		goto done;
+
+	pdata = devm_kzalloc(&client->dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		goto done;
+
+	flags = bus_cfg.bus.parallel.flags;
+
+	if (flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
+		pdata->hs_polarity = 1;
+
+	if (flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH)
+		pdata->vs_polarity = 1;
+
+	if (flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
+		pdata->clk_polarity = 1;
+
+	if (flags & V4L2_MBUS_FIELD_EVEN_HIGH)
+		pdata->fid_polarity = 1;
+
+	if (flags & V4L2_MBUS_VIDEO_SOG_ACTIVE_HIGH)
+		pdata->sog_polarity = 1;
+
+done:
+	of_node_put(endpoint);
+	return pdata;
+}
+
+/*
+ * tvp7002_probe - Probe a TVP7002 device
+ * @c: ptr to i2c_client struct
+ * @id: ptr to i2c_device_id struct
+ *
+ * Initialize the TVP7002 device
+ * Returns zero when successful, -EINVAL if register read fails or
+ * -EIO if i2c access is not available.
+ */
+static int tvp7002_probe(struct i2c_client *c)
+{
+	struct tvp7002_config *pdata = tvp7002_get_pdata(c);
+	struct v4l2_subdev *sd;
+	struct tvp7002 *device;
+	struct v4l2_dv_timings timings;
+	int polarity_a;
+	int polarity_b;
+	u8 revision;
+	int error;
+
+	if (pdata == NULL) {
+		dev_err(&c->dev, "No platform data\n");
+		return -EINVAL;
+	}
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(c->adapter,
+		I2C_FUNC_SMBUS_READ_BYTE | I2C_FUNC_SMBUS_WRITE_BYTE_DATA))
+		return -EIO;
+
+	device = devm_kzalloc(&c->dev, sizeof(struct tvp7002), GFP_KERNEL);
+
+	if (!device)
+		return -ENOMEM;
+
+	sd = &device->sd;
+	device->pdata = pdata;
+	device->current_timings = tvp7002_timings;
+
+	/* Tell v4l2 the device is ready */
+	v4l2_i2c_subdev_init(sd, c, &tvp7002_ops);
+	v4l_info(c, "tvp7002 found @ 0x%02x (%s)\n",
+					c->addr, c->adapter->name);
+
+	error = tvp7002_read(sd, TVP7002_CHIP_REV, &revision);
+	if (error < 0)
+		return error;
+
+	/* Get revision number */
+	v4l2_info(sd, "Rev. %02x detected.\n", revision);
+	if (revision != 0x02)
+		v4l2_info(sd, "Unknown revision detected.\n");
+
+	/* Initializes TVP7002 to its default values */
+	error = tvp7002_write_inittab(sd, tvp7002_init_default);
+
+	if (error < 0)
+		return error;
+
+	/* Set polarity information after registers have been set */
+	polarity_a = 0x20 | device->pdata->hs_polarity << 5
+			| device->pdata->vs_polarity << 2;
+	error = tvp7002_write(sd, TVP7002_SYNC_CTL_1, polarity_a);
+	if (error < 0)
+		return error;
+
+	polarity_b = 0x01  | device->pdata->fid_polarity << 2
+			| device->pdata->sog_polarity << 1
+			| device->pdata->clk_polarity;
+	error = tvp7002_write(sd, TVP7002_MISC_CTL_3, polarity_b);
+	if (error < 0)
+		return error;
+
+	/* Set registers according to default video mode */
+	timings = device->current_timings->timings;
+	error = tvp7002_s_dv_timings(sd, &timings);
+
+#if defined(CONFIG_MEDIA_CONTROLLER)
+	device->pad.flags = MEDIA_PAD_FL_SOURCE;
+	device->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	device->sd.entity.function = MEDIA_ENT_F_ATV_DECODER;
+
+	error = media_entity_pads_init(&device->sd.entity, 1, &device->pad);
+	if (error < 0)
+		return error;
+#endif
+
+	v4l2_ctrl_handler_init(&device->hdl, 1);
+	v4l2_ctrl_new_std(&device->hdl, &tvp7002_ctrl_ops,
+			V4L2_CID_GAIN, 0, 255, 1, 0);
+	sd->ctrl_handler = &device->hdl;
+	if (device->hdl.error) {
+		error = device->hdl.error;
+		goto error;
+	}
+	v4l2_ctrl_handler_setup(&device->hdl);
+
+	error = v4l2_async_register_subdev(&device->sd);
+	if (error)
+		goto error;
+
+	return 0;
+
+error:
+	v4l2_ctrl_handler_free(&device->hdl);
+#if defined(CONFIG_MEDIA_CONTROLLER)
+	media_entity_cleanup(&device->sd.entity);
+#endif
+	return error;
+}
+
+/*
+ * tvp7002_remove - Remove TVP7002 device support
+ * @c: ptr to i2c_client struct
+ *
+ * Reset the TVP7002 device
+ * Returns zero.
+ */
+static void tvp7002_remove(struct i2c_client *c)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(c);
+	struct tvp7002 *device = to_tvp7002(sd);
+
+	v4l2_dbg(1, debug, sd, "Removing tvp7002 adapter"
+				"on address 0x%x\n", c->addr);
+	v4l2_async_unregister_subdev(&device->sd);
+#if defined(CONFIG_MEDIA_CONTROLLER)
+	media_entity_cleanup(&device->sd.entity);
+#endif
+	v4l2_ctrl_handler_free(&device->hdl);
+}
+
+/* I2C Device ID table */
+static const struct i2c_device_id tvp7002_id[] = {
+	{ "tvp7002", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, tvp7002_id);
+
+#if IS_ENABLED(CONFIG_OF)
+static const struct of_device_id tvp7002_of_match[] = {
+	{ .compatible = "ti,tvp7002", },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, tvp7002_of_match);
+#endif
+
+/* I2C driver data */
+static struct i2c_driver tvp7002_driver = {
+	.driver = {
+		.of_match_table = of_match_ptr(tvp7002_of_match),
+		.name = TVP7002_MODULE_NAME,
+	},
+	.probe = tvp7002_probe,
+	.remove = tvp7002_remove,
+	.id_table = tvp7002_id,
+};
+
+module_i2c_driver(tvp7002_driver);
diff --git a/drivers/media/i2c/tvp7002_reg.h b/drivers/media/i2c/tvp7002_reg.h
new file mode 100644
index 000000000..ef3cc9965
--- /dev/null
+++ b/drivers/media/i2c/tvp7002_reg.h
@@ -0,0 +1,137 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/* Texas Instruments Triple 8-/10-BIT 165-/110-MSPS Video and Graphics
+ * Digitizer with Horizontal PLL registers
+ *
+ * Copyright (C) 2009 Texas Instruments Inc
+ * Author: Santiago Nunez-Corrales <santiago.nunez@ridgerun.com>
+ *
+ * This code is partially based upon the TVP5150 driver
+ * written by Mauro Carvalho Chehab <mchehab@kernel.org>,
+ * the TVP514x driver written by Vaibhav Hiremath <hvaibhav@ti.com>
+ * and the TVP7002 driver in the TI LSP 2.10.00.14
+ */
+
+/* Naming conventions
+ * ------------------
+ *
+ * FDBK:  Feedback
+ * DIV:   Divider
+ * CTL:   Control
+ * SEL:   Select
+ * IN:    Input
+ * OUT:   Output
+ * R:     Red
+ * G:     Green
+ * B:     Blue
+ * OFF:   Offset
+ * THRS:  Threshold
+ * DGTL:  Digital
+ * LVL:   Level
+ * PWR:   Power
+ * MVIS:  Macrovision
+ * W:     Width
+ * H:     Height
+ * ALGN:  Alignment
+ * CLK:   Clocks
+ * TOL:   Tolerance
+ * BWTH:  Bandwidth
+ * COEF:  Coefficient
+ * STAT:  Status
+ * AUTO:  Automatic
+ * FLD:   Field
+ * L:	  Line
+ */
+
+#define TVP7002_CHIP_REV		0x00
+#define TVP7002_HPLL_FDBK_DIV_MSBS	0x01
+#define TVP7002_HPLL_FDBK_DIV_LSBS	0x02
+#define TVP7002_HPLL_CRTL		0x03
+#define TVP7002_HPLL_PHASE_SEL		0x04
+#define TVP7002_CLAMP_START		0x05
+#define TVP7002_CLAMP_W			0x06
+#define TVP7002_HSYNC_OUT_W		0x07
+#define TVP7002_B_FINE_GAIN		0x08
+#define TVP7002_G_FINE_GAIN		0x09
+#define TVP7002_R_FINE_GAIN		0x0a
+#define TVP7002_B_FINE_OFF_MSBS		0x0b
+#define TVP7002_G_FINE_OFF_MSBS         0x0c
+#define TVP7002_R_FINE_OFF_MSBS         0x0d
+#define TVP7002_SYNC_CTL_1		0x0e
+#define TVP7002_HPLL_AND_CLAMP_CTL	0x0f
+#define TVP7002_SYNC_ON_G_THRS		0x10
+#define TVP7002_SYNC_SEPARATOR_THRS	0x11
+#define TVP7002_HPLL_PRE_COAST		0x12
+#define TVP7002_HPLL_POST_COAST		0x13
+#define TVP7002_SYNC_DETECT_STAT	0x14
+#define TVP7002_OUT_FORMATTER		0x15
+#define TVP7002_MISC_CTL_1		0x16
+#define TVP7002_MISC_CTL_2              0x17
+#define TVP7002_MISC_CTL_3              0x18
+#define TVP7002_IN_MUX_SEL_1		0x19
+#define TVP7002_IN_MUX_SEL_2            0x1a
+#define TVP7002_B_AND_G_COARSE_GAIN	0x1b
+#define TVP7002_R_COARSE_GAIN		0x1c
+#define TVP7002_FINE_OFF_LSBS		0x1d
+#define TVP7002_B_COARSE_OFF		0x1e
+#define TVP7002_G_COARSE_OFF            0x1f
+#define TVP7002_R_COARSE_OFF            0x20
+#define TVP7002_HSOUT_OUT_START		0x21
+#define TVP7002_MISC_CTL_4		0x22
+#define TVP7002_B_DGTL_ALC_OUT_LSBS	0x23
+#define TVP7002_G_DGTL_ALC_OUT_LSBS     0x24
+#define TVP7002_R_DGTL_ALC_OUT_LSBS     0x25
+#define TVP7002_AUTO_LVL_CTL_ENABLE	0x26
+#define TVP7002_DGTL_ALC_OUT_MSBS	0x27
+#define TVP7002_AUTO_LVL_CTL_FILTER	0x28
+/* Reserved 0x29*/
+#define TVP7002_FINE_CLAMP_CTL		0x2a
+#define TVP7002_PWR_CTL			0x2b
+#define TVP7002_ADC_SETUP		0x2c
+#define TVP7002_COARSE_CLAMP_CTL	0x2d
+#define TVP7002_SOG_CLAMP		0x2e
+#define TVP7002_RGB_COARSE_CLAMP_CTL	0x2f
+#define TVP7002_SOG_COARSE_CLAMP_CTL	0x30
+#define TVP7002_ALC_PLACEMENT		0x31
+/* Reserved 0x32 */
+/* Reserved 0x33 */
+#define TVP7002_MVIS_STRIPPER_W		0x34
+#define TVP7002_VSYNC_ALGN		0x35
+#define TVP7002_SYNC_BYPASS		0x36
+#define TVP7002_L_FRAME_STAT_LSBS	0x37
+#define TVP7002_L_FRAME_STAT_MSBS	0x38
+#define TVP7002_CLK_L_STAT_LSBS		0x39
+#define TVP7002_CLK_L_STAT_MSBS		0x3a
+#define TVP7002_HSYNC_W			0x3b
+#define TVP7002_VSYNC_W                 0x3c
+#define TVP7002_L_LENGTH_TOL		0x3d
+/* Reserved 0x3e */
+#define TVP7002_VIDEO_BWTH_CTL		0x3f
+#define TVP7002_AVID_START_PIXEL_LSBS	0x40
+#define TVP7002_AVID_START_PIXEL_MSBS   0x41
+#define TVP7002_AVID_STOP_PIXEL_LSBS	0x42
+#define TVP7002_AVID_STOP_PIXEL_MSBS    0x43
+#define TVP7002_VBLK_F_0_START_L_OFF	0x44
+#define TVP7002_VBLK_F_1_START_L_OFF    0x45
+#define TVP7002_VBLK_F_0_DURATION	0x46
+#define TVP7002_VBLK_F_1_DURATION       0x47
+#define TVP7002_FBIT_F_0_START_L_OFF	0x48
+#define TVP7002_FBIT_F_1_START_L_OFF    0x49
+#define TVP7002_YUV_Y_G_COEF_LSBS	0x4a
+#define TVP7002_YUV_Y_G_COEF_MSBS       0x4b
+#define TVP7002_YUV_Y_B_COEF_LSBS       0x4c
+#define TVP7002_YUV_Y_B_COEF_MSBS       0x4d
+#define TVP7002_YUV_Y_R_COEF_LSBS       0x4e
+#define TVP7002_YUV_Y_R_COEF_MSBS       0x4f
+#define TVP7002_YUV_U_G_COEF_LSBS       0x50
+#define TVP7002_YUV_U_G_COEF_MSBS       0x51
+#define TVP7002_YUV_U_B_COEF_LSBS       0x52
+#define TVP7002_YUV_U_B_COEF_MSBS       0x53
+#define TVP7002_YUV_U_R_COEF_LSBS       0x54
+#define TVP7002_YUV_U_R_COEF_MSBS       0x55
+#define TVP7002_YUV_V_G_COEF_LSBS       0x56
+#define TVP7002_YUV_V_G_COEF_MSBS       0x57
+#define TVP7002_YUV_V_B_COEF_LSBS       0x58
+#define TVP7002_YUV_V_B_COEF_MSBS       0x59
+#define TVP7002_YUV_V_R_COEF_LSBS       0x5a
+#define TVP7002_YUV_V_R_COEF_MSBS       0x5b
+
diff --git a/drivers/media/i2c/tw2804.c b/drivers/media/i2c/tw2804.c
new file mode 100644
index 000000000..6a2521e3a
--- /dev/null
+++ b/drivers/media/i2c/tw2804.c
@@ -0,0 +1,435 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2005-2006 Micronas USA Inc.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <linux/ioctl.h>
+#include <linux/slab.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+
+#define TW2804_REG_AUTOGAIN		0x02
+#define TW2804_REG_HUE			0x0f
+#define TW2804_REG_SATURATION		0x10
+#define TW2804_REG_CONTRAST		0x11
+#define TW2804_REG_BRIGHTNESS		0x12
+#define TW2804_REG_COLOR_KILLER		0x14
+#define TW2804_REG_GAIN			0x3c
+#define TW2804_REG_CHROMA_GAIN		0x3d
+#define TW2804_REG_BLUE_BALANCE		0x3e
+#define TW2804_REG_RED_BALANCE		0x3f
+
+struct tw2804 {
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
+	u8 channel:2;
+	u8 input:1;
+	int norm;
+};
+
+static const u8 global_registers[] = {
+	0x39, 0x00,
+	0x3a, 0xff,
+	0x3b, 0x84,
+	0x3c, 0x80,
+	0x3d, 0x80,
+	0x3e, 0x82,
+	0x3f, 0x82,
+	0x78, 0x00,
+	0xff, 0xff, /* Terminator (reg 0xff does not exist) */
+};
+
+static const u8 channel_registers[] = {
+	0x01, 0xc4,
+	0x02, 0xa5,
+	0x03, 0x20,
+	0x04, 0xd0,
+	0x05, 0x20,
+	0x06, 0xd0,
+	0x07, 0x88,
+	0x08, 0x20,
+	0x09, 0x07,
+	0x0a, 0xf0,
+	0x0b, 0x07,
+	0x0c, 0xf0,
+	0x0d, 0x40,
+	0x0e, 0xd2,
+	0x0f, 0x80,
+	0x10, 0x80,
+	0x11, 0x80,
+	0x12, 0x80,
+	0x13, 0x1f,
+	0x14, 0x00,
+	0x15, 0x00,
+	0x16, 0x00,
+	0x17, 0x00,
+	0x18, 0xff,
+	0x19, 0xff,
+	0x1a, 0xff,
+	0x1b, 0xff,
+	0x1c, 0xff,
+	0x1d, 0xff,
+	0x1e, 0xff,
+	0x1f, 0xff,
+	0x20, 0x07,
+	0x21, 0x07,
+	0x22, 0x00,
+	0x23, 0x91,
+	0x24, 0x51,
+	0x25, 0x03,
+	0x26, 0x00,
+	0x27, 0x00,
+	0x28, 0x00,
+	0x29, 0x00,
+	0x2a, 0x00,
+	0x2b, 0x00,
+	0x2c, 0x00,
+	0x2d, 0x00,
+	0x2e, 0x00,
+	0x2f, 0x00,
+	0x30, 0x00,
+	0x31, 0x00,
+	0x32, 0x00,
+	0x33, 0x00,
+	0x34, 0x00,
+	0x35, 0x00,
+	0x36, 0x00,
+	0x37, 0x00,
+	0xff, 0xff, /* Terminator (reg 0xff does not exist) */
+};
+
+static int write_reg(struct i2c_client *client, u8 reg, u8 value, u8 channel)
+{
+	return i2c_smbus_write_byte_data(client, reg | (channel << 6), value);
+}
+
+static int write_regs(struct i2c_client *client, const u8 *regs, u8 channel)
+{
+	int ret;
+	int i;
+
+	for (i = 0; regs[i] != 0xff; i += 2) {
+		ret = i2c_smbus_write_byte_data(client,
+				regs[i] | (channel << 6), regs[i + 1]);
+		if (ret < 0)
+			return ret;
+	}
+	return 0;
+}
+
+static int read_reg(struct i2c_client *client, u8 reg, u8 channel)
+{
+	return i2c_smbus_read_byte_data(client, (reg) | (channel << 6));
+}
+
+static inline struct tw2804 *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct tw2804, sd);
+}
+
+static inline struct tw2804 *to_state_from_ctrl(struct v4l2_ctrl *ctrl)
+{
+	return container_of(ctrl->handler, struct tw2804, hdl);
+}
+
+static int tw2804_log_status(struct v4l2_subdev *sd)
+{
+	struct tw2804 *state = to_state(sd);
+
+	v4l2_info(sd, "Standard: %s\n",
+			state->norm & V4L2_STD_525_60 ? "60 Hz" : "50 Hz");
+	v4l2_info(sd, "Channel: %d\n", state->channel);
+	v4l2_info(sd, "Input: %d\n", state->input);
+	return v4l2_ctrl_subdev_log_status(sd);
+}
+
+/*
+ * These volatile controls are needed because all four channels share
+ * these controls. So a change made to them through one channel would
+ * require another channel to be updated.
+ *
+ * Normally this would have been done in a different way, but since the one
+ * board that uses this driver sees this single chip as if it was on four
+ * different i2c adapters (each adapter belonging to a separate instance of
+ * the same USB driver) there is no reliable method that I have found to let
+ * the instances know about each other.
+ *
+ * So implementing these global registers as volatile is the best we can do.
+ */
+static int tw2804_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct tw2804 *state = to_state_from_ctrl(ctrl);
+	struct i2c_client *client = v4l2_get_subdevdata(&state->sd);
+
+	switch (ctrl->id) {
+	case V4L2_CID_GAIN:
+		ctrl->val = read_reg(client, TW2804_REG_GAIN, 0);
+		return 0;
+
+	case V4L2_CID_CHROMA_GAIN:
+		ctrl->val = read_reg(client, TW2804_REG_CHROMA_GAIN, 0);
+		return 0;
+
+	case V4L2_CID_BLUE_BALANCE:
+		ctrl->val = read_reg(client, TW2804_REG_BLUE_BALANCE, 0);
+		return 0;
+
+	case V4L2_CID_RED_BALANCE:
+		ctrl->val = read_reg(client, TW2804_REG_RED_BALANCE, 0);
+		return 0;
+	}
+	return 0;
+}
+
+static int tw2804_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct tw2804 *state = to_state_from_ctrl(ctrl);
+	struct i2c_client *client = v4l2_get_subdevdata(&state->sd);
+	int addr;
+	int reg;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTOGAIN:
+		addr = TW2804_REG_AUTOGAIN;
+		reg = read_reg(client, addr, state->channel);
+		if (reg < 0)
+			return reg;
+		if (ctrl->val == 0)
+			reg &= ~(1 << 7);
+		else
+			reg |= 1 << 7;
+		return write_reg(client, addr, reg, state->channel);
+
+	case V4L2_CID_COLOR_KILLER:
+		addr = TW2804_REG_COLOR_KILLER;
+		reg = read_reg(client, addr, state->channel);
+		if (reg < 0)
+			return reg;
+		reg = (reg & ~(0x03)) | (ctrl->val == 0 ? 0x02 : 0x03);
+		return write_reg(client, addr, reg, state->channel);
+
+	case V4L2_CID_GAIN:
+		return write_reg(client, TW2804_REG_GAIN, ctrl->val, 0);
+
+	case V4L2_CID_CHROMA_GAIN:
+		return write_reg(client, TW2804_REG_CHROMA_GAIN, ctrl->val, 0);
+
+	case V4L2_CID_BLUE_BALANCE:
+		return write_reg(client, TW2804_REG_BLUE_BALANCE, ctrl->val, 0);
+
+	case V4L2_CID_RED_BALANCE:
+		return write_reg(client, TW2804_REG_RED_BALANCE, ctrl->val, 0);
+
+	case V4L2_CID_BRIGHTNESS:
+		return write_reg(client, TW2804_REG_BRIGHTNESS,
+				ctrl->val, state->channel);
+
+	case V4L2_CID_CONTRAST:
+		return write_reg(client, TW2804_REG_CONTRAST,
+				ctrl->val, state->channel);
+
+	case V4L2_CID_SATURATION:
+		return write_reg(client, TW2804_REG_SATURATION,
+				ctrl->val, state->channel);
+
+	case V4L2_CID_HUE:
+		return write_reg(client, TW2804_REG_HUE,
+				ctrl->val, state->channel);
+
+	default:
+		break;
+	}
+	return -EINVAL;
+}
+
+static int tw2804_s_std(struct v4l2_subdev *sd, v4l2_std_id norm)
+{
+	struct tw2804 *dec = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	bool is_60hz = norm & V4L2_STD_525_60;
+	u8 regs[] = {
+		0x01, is_60hz ? 0xc4 : 0x84,
+		0x09, is_60hz ? 0x07 : 0x04,
+		0x0a, is_60hz ? 0xf0 : 0x20,
+		0x0b, is_60hz ? 0x07 : 0x04,
+		0x0c, is_60hz ? 0xf0 : 0x20,
+		0x0d, is_60hz ? 0x40 : 0x4a,
+		0x16, is_60hz ? 0x00 : 0x40,
+		0x17, is_60hz ? 0x00 : 0x40,
+		0x20, is_60hz ? 0x07 : 0x0f,
+		0x21, is_60hz ? 0x07 : 0x0f,
+		0xff, 0xff,
+	};
+
+	write_regs(client, regs, dec->channel);
+	dec->norm = norm;
+	return 0;
+}
+
+static int tw2804_s_video_routing(struct v4l2_subdev *sd, u32 input, u32 output,
+	u32 config)
+{
+	struct tw2804 *dec = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int reg;
+
+	if (config && config - 1 != dec->channel) {
+		if (config > 4) {
+			dev_err(&client->dev,
+				"channel %d is not between 1 and 4!\n", config);
+			return -EINVAL;
+		}
+		dec->channel = config - 1;
+		dev_dbg(&client->dev, "initializing TW2804 channel %d\n",
+			dec->channel);
+		if (dec->channel == 0 &&
+				write_regs(client, global_registers, 0) < 0) {
+			dev_err(&client->dev,
+				"error initializing TW2804 global registers\n");
+			return -EIO;
+		}
+		if (write_regs(client, channel_registers, dec->channel) < 0) {
+			dev_err(&client->dev,
+				"error initializing TW2804 channel %d\n",
+				dec->channel);
+			return -EIO;
+		}
+	}
+
+	if (input > 1)
+		return -EINVAL;
+
+	if (input == dec->input)
+		return 0;
+
+	reg = read_reg(client, 0x22, dec->channel);
+
+	if (reg >= 0) {
+		if (input == 0)
+			reg &= ~(1 << 2);
+		else
+			reg |= 1 << 2;
+		reg = write_reg(client, 0x22, reg, dec->channel);
+	}
+
+	if (reg >= 0)
+		dec->input = input;
+	else
+		return reg;
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops tw2804_ctrl_ops = {
+	.g_volatile_ctrl = tw2804_g_volatile_ctrl,
+	.s_ctrl = tw2804_s_ctrl,
+};
+
+static const struct v4l2_subdev_video_ops tw2804_video_ops = {
+	.s_std = tw2804_s_std,
+	.s_routing = tw2804_s_video_routing,
+};
+
+static const struct v4l2_subdev_core_ops tw2804_core_ops = {
+	.log_status = tw2804_log_status,
+};
+
+static const struct v4l2_subdev_ops tw2804_ops = {
+	.core = &tw2804_core_ops,
+	.video = &tw2804_video_ops,
+};
+
+static int tw2804_probe(struct i2c_client *client)
+{
+	struct i2c_adapter *adapter = client->adapter;
+	struct tw2804 *state;
+	struct v4l2_subdev *sd;
+	struct v4l2_ctrl *ctrl;
+	int err;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -ENODEV;
+
+	state = devm_kzalloc(&client->dev, sizeof(*state), GFP_KERNEL);
+	if (state == NULL)
+		return -ENOMEM;
+	sd = &state->sd;
+	v4l2_i2c_subdev_init(sd, client, &tw2804_ops);
+	state->channel = -1;
+	state->norm = V4L2_STD_NTSC;
+
+	v4l2_ctrl_handler_init(&state->hdl, 10);
+	v4l2_ctrl_new_std(&state->hdl, &tw2804_ctrl_ops,
+				V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(&state->hdl, &tw2804_ctrl_ops,
+				V4L2_CID_CONTRAST, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(&state->hdl, &tw2804_ctrl_ops,
+				V4L2_CID_SATURATION, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(&state->hdl, &tw2804_ctrl_ops,
+				V4L2_CID_HUE, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(&state->hdl, &tw2804_ctrl_ops,
+				V4L2_CID_COLOR_KILLER, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&state->hdl, &tw2804_ctrl_ops,
+				V4L2_CID_AUTOGAIN, 0, 1, 1, 0);
+	ctrl = v4l2_ctrl_new_std(&state->hdl, &tw2804_ctrl_ops,
+				V4L2_CID_GAIN, 0, 255, 1, 128);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_VOLATILE;
+	ctrl = v4l2_ctrl_new_std(&state->hdl, &tw2804_ctrl_ops,
+				V4L2_CID_CHROMA_GAIN, 0, 255, 1, 128);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_VOLATILE;
+	ctrl = v4l2_ctrl_new_std(&state->hdl, &tw2804_ctrl_ops,
+				V4L2_CID_BLUE_BALANCE, 0, 255, 1, 122);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_VOLATILE;
+	ctrl = v4l2_ctrl_new_std(&state->hdl, &tw2804_ctrl_ops,
+				V4L2_CID_RED_BALANCE, 0, 255, 1, 122);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_VOLATILE;
+	sd->ctrl_handler = &state->hdl;
+	err = state->hdl.error;
+	if (err) {
+		v4l2_ctrl_handler_free(&state->hdl);
+		return err;
+	}
+
+	v4l_info(client, "chip found @ 0x%02x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	return 0;
+}
+
+static void tw2804_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct tw2804 *state = to_state(sd);
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&state->hdl);
+}
+
+static const struct i2c_device_id tw2804_id[] = {
+	{ "tw2804", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, tw2804_id);
+
+static struct i2c_driver tw2804_driver = {
+	.driver = {
+		.name	= "tw2804",
+	},
+	.probe		= tw2804_probe,
+	.remove		= tw2804_remove,
+	.id_table	= tw2804_id,
+};
+
+module_i2c_driver(tw2804_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("TW2804/TW2802 V4L2 i2c driver");
+MODULE_AUTHOR("Micronas USA Inc");
diff --git a/drivers/media/i2c/tw9903.c b/drivers/media/i2c/tw9903.c
new file mode 100644
index 000000000..996be3960
--- /dev/null
+++ b/drivers/media/i2c/tw9903.c
@@ -0,0 +1,261 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2005-2006 Micronas USA Inc.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <linux/ioctl.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <linux/slab.h>
+
+MODULE_DESCRIPTION("TW9903 I2C subdev driver");
+MODULE_LICENSE("GPL v2");
+
+/*
+ * This driver is based on the wis-tw9903.c source that was in
+ * drivers/staging/media/go7007. That source had commented out code for
+ * saturation and scaling (neither seemed to work). If anyone ever gets
+ * hardware to test this driver, then that code might be useful to look at.
+ * You need to get the kernel sources of, say, kernel 3.8 where that
+ * wis-tw9903 driver is still present.
+ */
+
+struct tw9903 {
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
+	v4l2_std_id norm;
+};
+
+static inline struct tw9903 *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct tw9903, sd);
+}
+
+static const u8 initial_registers[] = {
+	0x02, 0x44, /* input 1, composite */
+	0x03, 0x92, /* correct digital format */
+	0x04, 0x00,
+	0x05, 0x80, /* or 0x00 for PAL */
+	0x06, 0x40, /* second internal current reference */
+	0x07, 0x02, /* window */
+	0x08, 0x14, /* window */
+	0x09, 0xf0, /* window */
+	0x0a, 0x81, /* window */
+	0x0b, 0xd0, /* window */
+	0x0c, 0x8c,
+	0x0d, 0x00, /* scaling */
+	0x0e, 0x11, /* scaling */
+	0x0f, 0x00, /* scaling */
+	0x10, 0x00, /* brightness */
+	0x11, 0x60, /* contrast */
+	0x12, 0x01, /* sharpness */
+	0x13, 0x7f, /* U gain */
+	0x14, 0x5a, /* V gain */
+	0x15, 0x00, /* hue */
+	0x16, 0xc3, /* sharpness */
+	0x18, 0x00,
+	0x19, 0x58, /* vbi */
+	0x1a, 0x80,
+	0x1c, 0x0f, /* video norm */
+	0x1d, 0x7f, /* video norm */
+	0x20, 0xa0, /* clamping gain (working 0x50) */
+	0x21, 0x22,
+	0x22, 0xf0,
+	0x23, 0xfe,
+	0x24, 0x3c,
+	0x25, 0x38,
+	0x26, 0x44,
+	0x27, 0x20,
+	0x28, 0x00,
+	0x29, 0x15,
+	0x2a, 0xa0,
+	0x2b, 0x44,
+	0x2c, 0x37,
+	0x2d, 0x00,
+	0x2e, 0xa5, /* burst PLL control (working: a9) */
+	0x2f, 0xe0, /* 0xea is blue test frame -- 0xe0 for normal */
+	0x31, 0x00,
+	0x33, 0x22,
+	0x34, 0x11,
+	0x35, 0x35,
+	0x3b, 0x05,
+	0x06, 0xc0, /* reset device */
+	0x00, 0x00, /* Terminator (reg 0x00 is read-only) */
+};
+
+static int write_reg(struct v4l2_subdev *sd, u8 reg, u8 value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return i2c_smbus_write_byte_data(client, reg, value);
+}
+
+static int write_regs(struct v4l2_subdev *sd, const u8 *regs)
+{
+	int i;
+
+	for (i = 0; regs[i] != 0x00; i += 2)
+		if (write_reg(sd, regs[i], regs[i + 1]) < 0)
+			return -1;
+	return 0;
+}
+
+static int tw9903_s_video_routing(struct v4l2_subdev *sd, u32 input,
+				      u32 output, u32 config)
+{
+	write_reg(sd, 0x02, 0x40 | (input << 1));
+	return 0;
+}
+
+static int tw9903_s_std(struct v4l2_subdev *sd, v4l2_std_id norm)
+{
+	struct tw9903 *dec = to_state(sd);
+	bool is_60hz = norm & V4L2_STD_525_60;
+	static const u8 config_60hz[] = {
+		0x05, 0x80,
+		0x07, 0x02,
+		0x08, 0x14,
+		0x09, 0xf0,
+		0,    0,
+	};
+	static const u8 config_50hz[] = {
+		0x05, 0x00,
+		0x07, 0x12,
+		0x08, 0x18,
+		0x09, 0x20,
+		0,    0,
+	};
+
+	write_regs(sd, is_60hz ? config_60hz : config_50hz);
+	dec->norm = norm;
+	return 0;
+}
+
+
+static int tw9903_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct tw9903 *dec = container_of(ctrl->handler, struct tw9903, hdl);
+	struct v4l2_subdev *sd = &dec->sd;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		write_reg(sd, 0x10, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		write_reg(sd, 0x11, ctrl->val);
+		break;
+	case V4L2_CID_HUE:
+		write_reg(sd, 0x15, ctrl->val);
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int tw9903_log_status(struct v4l2_subdev *sd)
+{
+	struct tw9903 *dec = to_state(sd);
+	bool is_60hz = dec->norm & V4L2_STD_525_60;
+
+	v4l2_info(sd, "Standard: %d Hz\n", is_60hz ? 60 : 50);
+	v4l2_ctrl_subdev_log_status(sd);
+	return 0;
+}
+
+/* --------------------------------------------------------------------------*/
+
+static const struct v4l2_ctrl_ops tw9903_ctrl_ops = {
+	.s_ctrl = tw9903_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops tw9903_core_ops = {
+	.log_status = tw9903_log_status,
+};
+
+static const struct v4l2_subdev_video_ops tw9903_video_ops = {
+	.s_std = tw9903_s_std,
+	.s_routing = tw9903_s_video_routing,
+};
+
+static const struct v4l2_subdev_ops tw9903_ops = {
+	.core = &tw9903_core_ops,
+	.video = &tw9903_video_ops,
+};
+
+/* --------------------------------------------------------------------------*/
+
+static int tw9903_probe(struct i2c_client *client)
+{
+	struct tw9903 *dec;
+	struct v4l2_subdev *sd;
+	struct v4l2_ctrl_handler *hdl;
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+
+	v4l_info(client, "chip found @ 0x%02x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	dec = devm_kzalloc(&client->dev, sizeof(*dec), GFP_KERNEL);
+	if (dec == NULL)
+		return -ENOMEM;
+	sd = &dec->sd;
+	v4l2_i2c_subdev_init(sd, client, &tw9903_ops);
+	hdl = &dec->hdl;
+	v4l2_ctrl_handler_init(hdl, 4);
+	v4l2_ctrl_new_std(hdl, &tw9903_ctrl_ops,
+		V4L2_CID_BRIGHTNESS, -128, 127, 1, 0);
+	v4l2_ctrl_new_std(hdl, &tw9903_ctrl_ops,
+		V4L2_CID_CONTRAST, 0, 255, 1, 0x60);
+	v4l2_ctrl_new_std(hdl, &tw9903_ctrl_ops,
+		V4L2_CID_HUE, -128, 127, 1, 0);
+	sd->ctrl_handler = hdl;
+	if (hdl->error) {
+		int err = hdl->error;
+
+		v4l2_ctrl_handler_free(hdl);
+		return err;
+	}
+
+	/* Initialize tw9903 */
+	dec->norm = V4L2_STD_NTSC;
+
+	if (write_regs(sd, initial_registers) < 0) {
+		v4l2_err(client, "error initializing TW9903\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void tw9903_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&to_state(sd)->hdl);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id tw9903_id[] = {
+	{ "tw9903", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, tw9903_id);
+
+static struct i2c_driver tw9903_driver = {
+	.driver = {
+		.name	= "tw9903",
+	},
+	.probe = tw9903_probe,
+	.remove = tw9903_remove,
+	.id_table = tw9903_id,
+};
+module_i2c_driver(tw9903_driver);
diff --git a/drivers/media/i2c/tw9906.c b/drivers/media/i2c/tw9906.c
new file mode 100644
index 000000000..25c625f6d
--- /dev/null
+++ b/drivers/media/i2c/tw9906.c
@@ -0,0 +1,229 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2005-2006 Micronas USA Inc.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <linux/ioctl.h>
+#include <linux/slab.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+
+MODULE_DESCRIPTION("TW9906 I2C subdev driver");
+MODULE_LICENSE("GPL v2");
+
+struct tw9906 {
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
+	v4l2_std_id norm;
+};
+
+static inline struct tw9906 *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct tw9906, sd);
+}
+
+static const u8 initial_registers[] = {
+	0x02, 0x40, /* input 0, composite */
+	0x03, 0xa2, /* correct digital format */
+	0x05, 0x81, /* or 0x01 for PAL */
+	0x07, 0x02, /* window */
+	0x08, 0x14, /* window */
+	0x09, 0xf0, /* window */
+	0x0a, 0x10, /* window */
+	0x0b, 0xd0, /* window */
+	0x0d, 0x00, /* scaling */
+	0x0e, 0x11, /* scaling */
+	0x0f, 0x00, /* scaling */
+	0x10, 0x00, /* brightness */
+	0x11, 0x60, /* contrast */
+	0x12, 0x11, /* sharpness */
+	0x13, 0x7e, /* U gain */
+	0x14, 0x7e, /* V gain */
+	0x15, 0x00, /* hue */
+	0x19, 0x57, /* vbi */
+	0x1a, 0x0f,
+	0x1b, 0x40,
+	0x29, 0x03,
+	0x55, 0x00,
+	0x6b, 0x26,
+	0x6c, 0x36,
+	0x6d, 0xf0,
+	0x6e, 0x41,
+	0x6f, 0x13,
+	0xad, 0x70,
+	0x00, 0x00, /* Terminator (reg 0x00 is read-only) */
+};
+
+static int write_reg(struct v4l2_subdev *sd, u8 reg, u8 value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return i2c_smbus_write_byte_data(client, reg, value);
+}
+
+static int write_regs(struct v4l2_subdev *sd, const u8 *regs)
+{
+	int i;
+
+	for (i = 0; regs[i] != 0x00; i += 2)
+		if (write_reg(sd, regs[i], regs[i + 1]) < 0)
+			return -1;
+	return 0;
+}
+
+static int tw9906_s_video_routing(struct v4l2_subdev *sd, u32 input,
+				      u32 output, u32 config)
+{
+	write_reg(sd, 0x02, 0x40 | (input << 1));
+	return 0;
+}
+
+static int tw9906_s_std(struct v4l2_subdev *sd, v4l2_std_id norm)
+{
+	struct tw9906 *dec = to_state(sd);
+	bool is_60hz = norm & V4L2_STD_525_60;
+	static const u8 config_60hz[] = {
+		0x05, 0x81,
+		0x07, 0x02,
+		0x08, 0x14,
+		0x09, 0xf0,
+		0,    0,
+	};
+	static const u8 config_50hz[] = {
+		0x05, 0x01,
+		0x07, 0x12,
+		0x08, 0x18,
+		0x09, 0x20,
+		0,    0,
+	};
+
+	write_regs(sd, is_60hz ? config_60hz : config_50hz);
+	dec->norm = norm;
+	return 0;
+}
+
+static int tw9906_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct tw9906 *dec = container_of(ctrl->handler, struct tw9906, hdl);
+	struct v4l2_subdev *sd = &dec->sd;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		write_reg(sd, 0x10, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		write_reg(sd, 0x11, ctrl->val);
+		break;
+	case V4L2_CID_HUE:
+		write_reg(sd, 0x15, ctrl->val);
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int tw9906_log_status(struct v4l2_subdev *sd)
+{
+	struct tw9906 *dec = to_state(sd);
+	bool is_60hz = dec->norm & V4L2_STD_525_60;
+
+	v4l2_info(sd, "Standard: %d Hz\n", is_60hz ? 60 : 50);
+	v4l2_ctrl_subdev_log_status(sd);
+	return 0;
+}
+
+/* --------------------------------------------------------------------------*/
+
+static const struct v4l2_ctrl_ops tw9906_ctrl_ops = {
+	.s_ctrl = tw9906_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops tw9906_core_ops = {
+	.log_status = tw9906_log_status,
+};
+
+static const struct v4l2_subdev_video_ops tw9906_video_ops = {
+	.s_std = tw9906_s_std,
+	.s_routing = tw9906_s_video_routing,
+};
+
+static const struct v4l2_subdev_ops tw9906_ops = {
+	.core = &tw9906_core_ops,
+	.video = &tw9906_video_ops,
+};
+
+static int tw9906_probe(struct i2c_client *client)
+{
+	struct tw9906 *dec;
+	struct v4l2_subdev *sd;
+	struct v4l2_ctrl_handler *hdl;
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+
+	v4l_info(client, "chip found @ 0x%02x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	dec = devm_kzalloc(&client->dev, sizeof(*dec), GFP_KERNEL);
+	if (dec == NULL)
+		return -ENOMEM;
+	sd = &dec->sd;
+	v4l2_i2c_subdev_init(sd, client, &tw9906_ops);
+	hdl = &dec->hdl;
+	v4l2_ctrl_handler_init(hdl, 4);
+	v4l2_ctrl_new_std(hdl, &tw9906_ctrl_ops,
+		V4L2_CID_BRIGHTNESS, -128, 127, 1, 0);
+	v4l2_ctrl_new_std(hdl, &tw9906_ctrl_ops,
+		V4L2_CID_CONTRAST, 0, 255, 1, 0x60);
+	v4l2_ctrl_new_std(hdl, &tw9906_ctrl_ops,
+		V4L2_CID_HUE, -128, 127, 1, 0);
+	sd->ctrl_handler = hdl;
+	if (hdl->error) {
+		int err = hdl->error;
+
+		v4l2_ctrl_handler_free(hdl);
+		return err;
+	}
+
+	/* Initialize tw9906 */
+	dec->norm = V4L2_STD_NTSC;
+
+	if (write_regs(sd, initial_registers) < 0) {
+		v4l2_err(client, "error initializing TW9906\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void tw9906_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&to_state(sd)->hdl);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id tw9906_id[] = {
+	{ "tw9906", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, tw9906_id);
+
+static struct i2c_driver tw9906_driver = {
+	.driver = {
+		.name	= "tw9906",
+	},
+	.probe = tw9906_probe,
+	.remove = tw9906_remove,
+	.id_table = tw9906_id,
+};
+module_i2c_driver(tw9906_driver);
diff --git a/drivers/media/i2c/tw9910.c b/drivers/media/i2c/tw9910.c
new file mode 100644
index 000000000..477a64d8f
--- /dev/null
+++ b/drivers/media/i2c/tw9910.c
@@ -0,0 +1,1024 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * tw9910 Video Driver
+ *
+ * Copyright (C) 2017 Jacopo Mondi <jacopo+renesas@jmondi.org>
+ *
+ * Copyright (C) 2008 Renesas Solutions Corp.
+ * Kuninori Morimoto <morimoto.kuninori@renesas.com>
+ *
+ * Based on ov772x driver,
+ *
+ * Copyright (C) 2008 Kuninori Morimoto <morimoto.kuninori@renesas.com>
+ * Copyright 2006-7 Jonathan Corbet <corbet@lwn.net>
+ * Copyright (C) 2008 Magnus Damm
+ * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/v4l2-mediabus.h>
+#include <linux/videodev2.h>
+
+#include <media/i2c/tw9910.h>
+#include <media/v4l2-subdev.h>
+
+#define GET_ID(val)  ((val & 0xF8) >> 3)
+#define GET_REV(val) (val & 0x07)
+
+/*
+ * register offset
+ */
+#define ID		0x00 /* Product ID Code Register */
+#define STATUS1		0x01 /* Chip Status Register I */
+#define INFORM		0x02 /* Input Format */
+#define OPFORM		0x03 /* Output Format Control Register */
+#define DLYCTR		0x04 /* Hysteresis and HSYNC Delay Control */
+#define OUTCTR1		0x05 /* Output Control I */
+#define ACNTL1		0x06 /* Analog Control Register 1 */
+#define CROP_HI		0x07 /* Cropping Register, High */
+#define VDELAY_LO	0x08 /* Vertical Delay Register, Low */
+#define VACTIVE_LO	0x09 /* Vertical Active Register, Low */
+#define HDELAY_LO	0x0A /* Horizontal Delay Register, Low */
+#define HACTIVE_LO	0x0B /* Horizontal Active Register, Low */
+#define CNTRL1		0x0C /* Control Register I */
+#define VSCALE_LO	0x0D /* Vertical Scaling Register, Low */
+#define SCALE_HI	0x0E /* Scaling Register, High */
+#define HSCALE_LO	0x0F /* Horizontal Scaling Register, Low */
+#define BRIGHT		0x10 /* BRIGHTNESS Control Register */
+#define CONTRAST	0x11 /* CONTRAST Control Register */
+#define SHARPNESS	0x12 /* SHARPNESS Control Register I */
+#define SAT_U		0x13 /* Chroma (U) Gain Register */
+#define SAT_V		0x14 /* Chroma (V) Gain Register */
+#define HUE		0x15 /* Hue Control Register */
+#define CORING1		0x17
+#define CORING2		0x18 /* Coring and IF compensation */
+#define VBICNTL		0x19 /* VBI Control Register */
+#define ACNTL2		0x1A /* Analog Control 2 */
+#define OUTCTR2		0x1B /* Output Control 2 */
+#define SDT		0x1C /* Standard Selection */
+#define SDTR		0x1D /* Standard Recognition */
+#define TEST		0x1F /* Test Control Register */
+#define CLMPG		0x20 /* Clamping Gain */
+#define IAGC		0x21 /* Individual AGC Gain */
+#define AGCGAIN		0x22 /* AGC Gain */
+#define PEAKWT		0x23 /* White Peak Threshold */
+#define CLMPL		0x24 /* Clamp level */
+#define SYNCT		0x25 /* Sync Amplitude */
+#define MISSCNT		0x26 /* Sync Miss Count Register */
+#define PCLAMP		0x27 /* Clamp Position Register */
+#define VCNTL1		0x28 /* Vertical Control I */
+#define VCNTL2		0x29 /* Vertical Control II */
+#define CKILL		0x2A /* Color Killer Level Control */
+#define COMB		0x2B /* Comb Filter Control */
+#define LDLY		0x2C /* Luma Delay and H Filter Control */
+#define MISC1		0x2D /* Miscellaneous Control I */
+#define LOOP		0x2E /* LOOP Control Register */
+#define MISC2		0x2F /* Miscellaneous Control II */
+#define MVSN		0x30 /* Macrovision Detection */
+#define STATUS2		0x31 /* Chip STATUS II */
+#define HFREF		0x32 /* H monitor */
+#define CLMD		0x33 /* CLAMP MODE */
+#define IDCNTL		0x34 /* ID Detection Control */
+#define CLCNTL1		0x35 /* Clamp Control I */
+#define ANAPLLCTL	0x4C
+#define VBIMIN		0x4D
+#define HSLOWCTL	0x4E
+#define WSS3		0x4F
+#define FILLDATA	0x50
+#define SDID		0x51
+#define DID		0x52
+#define WSS1		0x53
+#define WSS2		0x54
+#define VVBI		0x55
+#define LCTL6		0x56
+#define LCTL7		0x57
+#define LCTL8		0x58
+#define LCTL9		0x59
+#define LCTL10		0x5A
+#define LCTL11		0x5B
+#define LCTL12		0x5C
+#define LCTL13		0x5D
+#define LCTL14		0x5E
+#define LCTL15		0x5F
+#define LCTL16		0x60
+#define LCTL17		0x61
+#define LCTL18		0x62
+#define LCTL19		0x63
+#define LCTL20		0x64
+#define LCTL21		0x65
+#define LCTL22		0x66
+#define LCTL23		0x67
+#define LCTL24		0x68
+#define LCTL25		0x69
+#define LCTL26		0x6A
+#define HSBEGIN		0x6B
+#define HSEND		0x6C
+#define OVSDLY		0x6D
+#define OVSEND		0x6E
+#define VBIDELAY	0x6F
+
+/*
+ * register detail
+ */
+
+/* INFORM */
+#define FC27_ON     0x40 /* 1 : Input crystal clock frequency is 27MHz */
+#define FC27_FF     0x00 /* 0 : Square pixel mode. */
+			 /*     Must use 24.54MHz for 60Hz field rate */
+			 /*     source or 29.5MHz for 50Hz field rate */
+#define IFSEL_S     0x10 /* 01 : S-video decoding */
+#define IFSEL_C     0x00 /* 00 : Composite video decoding */
+			 /* Y input video selection */
+#define YSEL_M0     0x00 /*  00 : Mux0 selected */
+#define YSEL_M1     0x04 /*  01 : Mux1 selected */
+#define YSEL_M2     0x08 /*  10 : Mux2 selected */
+#define YSEL_M3     0x10 /*  11 : Mux3 selected */
+
+/* OPFORM */
+#define MODE        0x80 /* 0 : CCIR601 compatible YCrCb 4:2:2 format */
+			 /* 1 : ITU-R-656 compatible data sequence format */
+#define LEN         0x40 /* 0 : 8-bit YCrCb 4:2:2 output format */
+			 /* 1 : 16-bit YCrCb 4:2:2 output format.*/
+#define LLCMODE     0x20 /* 1 : LLC output mode. */
+			 /* 0 : free-run output mode */
+#define AINC        0x10 /* Serial interface auto-indexing control */
+			 /* 0 : auto-increment */
+			 /* 1 : non-auto */
+#define VSCTL       0x08 /* 1 : Vertical out ctrl by DVALID */
+			 /* 0 : Vertical out ctrl by HACTIVE and DVALID */
+#define OEN_TRI_SEL_MASK	0x07
+#define OEN_TRI_SEL_ALL_ON	0x00 /* Enable output for Rev0/Rev1 */
+#define OEN_TRI_SEL_ALL_OFF_r0	0x06 /* All tri-stated for Rev0 */
+#define OEN_TRI_SEL_ALL_OFF_r1	0x07 /* All tri-stated for Rev1 */
+
+/* OUTCTR1 */
+#define VSP_LO      0x00 /* 0 : VS pin output polarity is active low */
+#define VSP_HI      0x80 /* 1 : VS pin output polarity is active high. */
+			 /* VS pin output control */
+#define VSSL_VSYNC  0x00 /*   0 : VSYNC  */
+#define VSSL_VACT   0x10 /*   1 : VACT   */
+#define VSSL_FIELD  0x20 /*   2 : FIELD  */
+#define VSSL_VVALID 0x30 /*   3 : VVALID */
+#define VSSL_ZERO   0x70 /*   7 : 0      */
+#define HSP_LOW     0x00 /* 0 : HS pin output polarity is active low */
+#define HSP_HI      0x08 /* 1 : HS pin output polarity is active high.*/
+			 /* HS pin output control */
+#define HSSL_HACT   0x00 /*   0 : HACT   */
+#define HSSL_HSYNC  0x01 /*   1 : HSYNC  */
+#define HSSL_DVALID 0x02 /*   2 : DVALID */
+#define HSSL_HLOCK  0x03 /*   3 : HLOCK  */
+#define HSSL_ASYNCW 0x04 /*   4 : ASYNCW */
+#define HSSL_ZERO   0x07 /*   7 : 0      */
+
+/* ACNTL1 */
+#define SRESET      0x80 /* resets the device to its default state
+			  * but all register content remain unchanged.
+			  * This bit is self-resetting.
+			  */
+#define ACNTL1_PDN_MASK	0x0e
+#define CLK_PDN		0x08 /* system clock power down */
+#define Y_PDN		0x04 /* Luma ADC power down */
+#define C_PDN		0x02 /* Chroma ADC power down */
+
+/* ACNTL2 */
+#define ACNTL2_PDN_MASK	0x40
+#define PLL_PDN		0x40 /* PLL power down */
+
+/* VBICNTL */
+
+/* RTSEL : control the real time signal output from the MPOUT pin */
+#define RTSEL_MASK  0x07
+#define RTSEL_VLOSS 0x00 /* 0000 = Video loss */
+#define RTSEL_HLOCK 0x01 /* 0001 = H-lock */
+#define RTSEL_SLOCK 0x02 /* 0010 = S-lock */
+#define RTSEL_VLOCK 0x03 /* 0011 = V-lock */
+#define RTSEL_MONO  0x04 /* 0100 = MONO */
+#define RTSEL_DET50 0x05 /* 0101 = DET50 */
+#define RTSEL_FIELD 0x06 /* 0110 = FIELD */
+#define RTSEL_RTCO  0x07 /* 0111 = RTCO ( Real Time Control ) */
+
+/* HSYNC start and end are constant for now */
+#define HSYNC_START	0x0260
+#define HSYNC_END	0x0300
+
+/*
+ * structure
+ */
+
+struct regval_list {
+	unsigned char reg_num;
+	unsigned char value;
+};
+
+struct tw9910_scale_ctrl {
+	char           *name;
+	unsigned short  width;
+	unsigned short  height;
+	u16             hscale;
+	u16             vscale;
+};
+
+struct tw9910_priv {
+	struct v4l2_subdev		subdev;
+	struct clk			*clk;
+	struct tw9910_video_info	*info;
+	struct gpio_desc		*pdn_gpio;
+	struct gpio_desc		*rstb_gpio;
+	const struct tw9910_scale_ctrl	*scale;
+	v4l2_std_id			norm;
+	u32				revision;
+};
+
+static const struct tw9910_scale_ctrl tw9910_ntsc_scales[] = {
+	{
+		.name   = "NTSC SQ",
+		.width  = 640,
+		.height = 480,
+		.hscale = 0x0100,
+		.vscale = 0x0100,
+	},
+	{
+		.name   = "NTSC CCIR601",
+		.width  = 720,
+		.height = 480,
+		.hscale = 0x0100,
+		.vscale = 0x0100,
+	},
+	{
+		.name   = "NTSC SQ (CIF)",
+		.width  = 320,
+		.height = 240,
+		.hscale = 0x0200,
+		.vscale = 0x0200,
+	},
+	{
+		.name   = "NTSC CCIR601 (CIF)",
+		.width  = 360,
+		.height = 240,
+		.hscale = 0x0200,
+		.vscale = 0x0200,
+	},
+	{
+		.name   = "NTSC SQ (QCIF)",
+		.width  = 160,
+		.height = 120,
+		.hscale = 0x0400,
+		.vscale = 0x0400,
+	},
+	{
+		.name   = "NTSC CCIR601 (QCIF)",
+		.width  = 180,
+		.height = 120,
+		.hscale = 0x0400,
+		.vscale = 0x0400,
+	},
+};
+
+static const struct tw9910_scale_ctrl tw9910_pal_scales[] = {
+	{
+		.name   = "PAL SQ",
+		.width  = 768,
+		.height = 576,
+		.hscale = 0x0100,
+		.vscale = 0x0100,
+	},
+	{
+		.name   = "PAL CCIR601",
+		.width  = 720,
+		.height = 576,
+		.hscale = 0x0100,
+		.vscale = 0x0100,
+	},
+	{
+		.name   = "PAL SQ (CIF)",
+		.width  = 384,
+		.height = 288,
+		.hscale = 0x0200,
+		.vscale = 0x0200,
+	},
+	{
+		.name   = "PAL CCIR601 (CIF)",
+		.width  = 360,
+		.height = 288,
+		.hscale = 0x0200,
+		.vscale = 0x0200,
+	},
+	{
+		.name   = "PAL SQ (QCIF)",
+		.width  = 192,
+		.height = 144,
+		.hscale = 0x0400,
+		.vscale = 0x0400,
+	},
+	{
+		.name   = "PAL CCIR601 (QCIF)",
+		.width  = 180,
+		.height = 144,
+		.hscale = 0x0400,
+		.vscale = 0x0400,
+	},
+};
+
+/*
+ * general function
+ */
+static struct tw9910_priv *to_tw9910(const struct i2c_client *client)
+{
+	return container_of(i2c_get_clientdata(client), struct tw9910_priv,
+			    subdev);
+}
+
+static int tw9910_mask_set(struct i2c_client *client, u8 command,
+			   u8 mask, u8 set)
+{
+	s32 val = i2c_smbus_read_byte_data(client, command);
+
+	if (val < 0)
+		return val;
+
+	val &= ~mask;
+	val |= set & mask;
+
+	return i2c_smbus_write_byte_data(client, command, val);
+}
+
+static int tw9910_set_scale(struct i2c_client *client,
+			    const struct tw9910_scale_ctrl *scale)
+{
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(client, SCALE_HI,
+					(scale->vscale & 0x0F00) >> 4 |
+					(scale->hscale & 0x0F00) >> 8);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_write_byte_data(client, HSCALE_LO,
+					scale->hscale & 0x00FF);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_write_byte_data(client, VSCALE_LO,
+					scale->vscale & 0x00FF);
+
+	return ret;
+}
+
+static int tw9910_set_hsync(struct i2c_client *client)
+{
+	struct tw9910_priv *priv = to_tw9910(client);
+	int ret;
+
+	/* bit 10 - 3 */
+	ret = i2c_smbus_write_byte_data(client, HSBEGIN,
+					(HSYNC_START & 0x07F8) >> 3);
+	if (ret < 0)
+		return ret;
+
+	/* bit 10 - 3 */
+	ret = i2c_smbus_write_byte_data(client, HSEND,
+					(HSYNC_END & 0x07F8) >> 3);
+	if (ret < 0)
+		return ret;
+
+	/* So far only revisions 0 and 1 have been seen. */
+	/* bit 2 - 0 */
+	if (priv->revision == 1)
+		ret = tw9910_mask_set(client, HSLOWCTL, 0x77,
+				      (HSYNC_START & 0x0007) << 4 |
+				      (HSYNC_END   & 0x0007));
+
+	return ret;
+}
+
+static void tw9910_reset(struct i2c_client *client)
+{
+	tw9910_mask_set(client, ACNTL1, SRESET, SRESET);
+	usleep_range(1000, 5000);
+}
+
+static int tw9910_power(struct i2c_client *client, int enable)
+{
+	int ret;
+	u8 acntl1;
+	u8 acntl2;
+
+	if (enable) {
+		acntl1 = 0;
+		acntl2 = 0;
+	} else {
+		acntl1 = CLK_PDN | Y_PDN | C_PDN;
+		acntl2 = PLL_PDN;
+	}
+
+	ret = tw9910_mask_set(client, ACNTL1, ACNTL1_PDN_MASK, acntl1);
+	if (ret < 0)
+		return ret;
+
+	return tw9910_mask_set(client, ACNTL2, ACNTL2_PDN_MASK, acntl2);
+}
+
+static const struct tw9910_scale_ctrl *tw9910_select_norm(v4l2_std_id norm,
+							  u32 width, u32 height)
+{
+	const struct tw9910_scale_ctrl *scale;
+	const struct tw9910_scale_ctrl *ret = NULL;
+	__u32 diff = 0xffffffff, tmp;
+	int size, i;
+
+	if (norm & V4L2_STD_NTSC) {
+		scale = tw9910_ntsc_scales;
+		size = ARRAY_SIZE(tw9910_ntsc_scales);
+	} else if (norm & V4L2_STD_PAL) {
+		scale = tw9910_pal_scales;
+		size = ARRAY_SIZE(tw9910_pal_scales);
+	} else {
+		return NULL;
+	}
+
+	for (i = 0; i < size; i++) {
+		tmp = abs(width - scale[i].width) +
+		      abs(height - scale[i].height);
+		if (tmp < diff) {
+			diff = tmp;
+			ret = scale + i;
+		}
+	}
+
+	return ret;
+}
+
+/*
+ * subdevice operations
+ */
+static int tw9910_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct tw9910_priv *priv = to_tw9910(client);
+	u8 val;
+	int ret;
+
+	if (!enable) {
+		switch (priv->revision) {
+		case 0:
+			val = OEN_TRI_SEL_ALL_OFF_r0;
+			break;
+		case 1:
+			val = OEN_TRI_SEL_ALL_OFF_r1;
+			break;
+		default:
+			dev_err(&client->dev, "un-supported revision\n");
+			return -EINVAL;
+		}
+	} else {
+		val = OEN_TRI_SEL_ALL_ON;
+
+		if (!priv->scale) {
+			dev_err(&client->dev, "norm select error\n");
+			return -EPERM;
+		}
+
+		dev_dbg(&client->dev, "%s %dx%d\n",
+			priv->scale->name,
+			priv->scale->width,
+			priv->scale->height);
+	}
+
+	ret = tw9910_mask_set(client, OPFORM, OEN_TRI_SEL_MASK, val);
+	if (ret < 0)
+		return ret;
+
+	return tw9910_power(client, enable);
+}
+
+static int tw9910_g_std(struct v4l2_subdev *sd, v4l2_std_id *norm)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct tw9910_priv *priv = to_tw9910(client);
+
+	*norm = priv->norm;
+
+	return 0;
+}
+
+static int tw9910_s_std(struct v4l2_subdev *sd, v4l2_std_id norm)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct tw9910_priv *priv = to_tw9910(client);
+	const unsigned int hact = 720;
+	const unsigned int hdelay = 15;
+	unsigned int vact;
+	unsigned int vdelay;
+	int ret;
+
+	if (!(norm & (V4L2_STD_NTSC | V4L2_STD_PAL)))
+		return -EINVAL;
+
+	priv->norm = norm;
+	if (norm & V4L2_STD_525_60) {
+		vact = 240;
+		vdelay = 18;
+		ret = tw9910_mask_set(client, VVBI, 0x10, 0x10);
+	} else {
+		vact = 288;
+		vdelay = 24;
+		ret = tw9910_mask_set(client, VVBI, 0x10, 0x00);
+	}
+	if (!ret)
+		ret = i2c_smbus_write_byte_data(client, CROP_HI,
+						((vdelay >> 2) & 0xc0)	|
+						((vact >> 4) & 0x30)	|
+						((hdelay >> 6) & 0x0c)	|
+						((hact >> 8) & 0x03));
+	if (!ret)
+		ret = i2c_smbus_write_byte_data(client, VDELAY_LO,
+						vdelay & 0xff);
+	if (!ret)
+		ret = i2c_smbus_write_byte_data(client, VACTIVE_LO,
+						vact & 0xff);
+
+	return ret;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int tw9910_g_register(struct v4l2_subdev *sd,
+			     struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+	if (reg->reg > 0xff)
+		return -EINVAL;
+
+	reg->size = 1;
+	ret = i2c_smbus_read_byte_data(client, reg->reg);
+	if (ret < 0)
+		return ret;
+
+	/*
+	 * ret      = int
+	 * reg->val = __u64
+	 */
+	reg->val = (__u64)ret;
+
+	return 0;
+}
+
+static int tw9910_s_register(struct v4l2_subdev *sd,
+			     const struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (reg->reg > 0xff ||
+	    reg->val > 0xff)
+		return -EINVAL;
+
+	return i2c_smbus_write_byte_data(client, reg->reg, reg->val);
+}
+#endif
+
+static void tw9910_set_gpio_value(struct gpio_desc *desc, int value)
+{
+	if (desc) {
+		gpiod_set_value(desc, value);
+		usleep_range(500, 1000);
+	}
+}
+
+static int tw9910_power_on(struct tw9910_priv *priv)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&priv->subdev);
+	int ret;
+
+	if (priv->clk) {
+		ret = clk_prepare_enable(priv->clk);
+		if (ret)
+			return ret;
+	}
+
+	tw9910_set_gpio_value(priv->pdn_gpio, 0);
+
+	/*
+	 * FIXME: The reset signal is connected to a shared GPIO on some
+	 * platforms (namely the SuperH Migo-R). Until a framework becomes
+	 * available to handle this cleanly, request the GPIO temporarily
+	 * to avoid conflicts.
+	 */
+	priv->rstb_gpio = gpiod_get_optional(&client->dev, "rstb",
+					     GPIOD_OUT_LOW);
+	if (IS_ERR(priv->rstb_gpio)) {
+		dev_info(&client->dev, "Unable to get GPIO \"rstb\"");
+		clk_disable_unprepare(priv->clk);
+		tw9910_set_gpio_value(priv->pdn_gpio, 1);
+		return PTR_ERR(priv->rstb_gpio);
+	}
+
+	if (priv->rstb_gpio) {
+		tw9910_set_gpio_value(priv->rstb_gpio, 1);
+		tw9910_set_gpio_value(priv->rstb_gpio, 0);
+
+		gpiod_put(priv->rstb_gpio);
+	}
+
+	return 0;
+}
+
+static int tw9910_power_off(struct tw9910_priv *priv)
+{
+	clk_disable_unprepare(priv->clk);
+	tw9910_set_gpio_value(priv->pdn_gpio, 1);
+
+	return 0;
+}
+
+static int tw9910_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct tw9910_priv *priv = to_tw9910(client);
+
+	return on ? tw9910_power_on(priv) : tw9910_power_off(priv);
+}
+
+static int tw9910_set_frame(struct v4l2_subdev *sd, u32 *width, u32 *height)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct tw9910_priv *priv = to_tw9910(client);
+	int ret = -EINVAL;
+	u8 val;
+
+	/* Select suitable norm. */
+	priv->scale = tw9910_select_norm(priv->norm, *width, *height);
+	if (!priv->scale)
+		goto tw9910_set_fmt_error;
+
+	/* Reset hardware. */
+	tw9910_reset(client);
+
+	/* Set bus width. */
+	val = 0x00;
+	if (priv->info->buswidth == 16)
+		val = LEN;
+
+	ret = tw9910_mask_set(client, OPFORM, LEN, val);
+	if (ret < 0)
+		goto tw9910_set_fmt_error;
+
+	/* Select MPOUT behavior. */
+	switch (priv->info->mpout) {
+	case TW9910_MPO_VLOSS:
+		val = RTSEL_VLOSS; break;
+	case TW9910_MPO_HLOCK:
+		val = RTSEL_HLOCK; break;
+	case TW9910_MPO_SLOCK:
+		val = RTSEL_SLOCK; break;
+	case TW9910_MPO_VLOCK:
+		val = RTSEL_VLOCK; break;
+	case TW9910_MPO_MONO:
+		val = RTSEL_MONO;  break;
+	case TW9910_MPO_DET50:
+		val = RTSEL_DET50; break;
+	case TW9910_MPO_FIELD:
+		val = RTSEL_FIELD; break;
+	case TW9910_MPO_RTCO:
+		val = RTSEL_RTCO;  break;
+	default:
+		val = 0;
+	}
+
+	ret = tw9910_mask_set(client, VBICNTL, RTSEL_MASK, val);
+	if (ret < 0)
+		goto tw9910_set_fmt_error;
+
+	/* Set scale. */
+	ret = tw9910_set_scale(client, priv->scale);
+	if (ret < 0)
+		goto tw9910_set_fmt_error;
+
+	/* Set hsync. */
+	ret = tw9910_set_hsync(client);
+	if (ret < 0)
+		goto tw9910_set_fmt_error;
+
+	*width = priv->scale->width;
+	*height = priv->scale->height;
+
+	return ret;
+
+tw9910_set_fmt_error:
+
+	tw9910_reset(client);
+	priv->scale = NULL;
+
+	return ret;
+}
+
+static int tw9910_get_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *sd_state,
+				struct v4l2_subdev_selection *sel)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct tw9910_priv *priv = to_tw9910(client);
+
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
+		return -EINVAL;
+	/* Only CROP, CROP_DEFAULT and CROP_BOUNDS are supported. */
+	if (sel->target > V4L2_SEL_TGT_CROP_BOUNDS)
+		return -EINVAL;
+
+	sel->r.left	= 0;
+	sel->r.top	= 0;
+	if (priv->norm & V4L2_STD_NTSC) {
+		sel->r.width	= 640;
+		sel->r.height	= 480;
+	} else {
+		sel->r.width	= 768;
+		sel->r.height	= 576;
+	}
+
+	return 0;
+}
+
+static int tw9910_get_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *mf = &format->format;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct tw9910_priv *priv = to_tw9910(client);
+
+	if (format->pad)
+		return -EINVAL;
+
+	if (!priv->scale) {
+		priv->scale = tw9910_select_norm(priv->norm, 640, 480);
+		if (!priv->scale)
+			return -EINVAL;
+	}
+
+	mf->width	= priv->scale->width;
+	mf->height	= priv->scale->height;
+	mf->code	= MEDIA_BUS_FMT_UYVY8_2X8;
+	mf->colorspace	= V4L2_COLORSPACE_SMPTE170M;
+	mf->field	= V4L2_FIELD_INTERLACED_BT;
+
+	return 0;
+}
+
+static int tw9910_s_fmt(struct v4l2_subdev *sd,
+			struct v4l2_mbus_framefmt *mf)
+{
+	u32 width = mf->width, height = mf->height;
+	int ret;
+
+	WARN_ON(mf->field != V4L2_FIELD_ANY &&
+		mf->field != V4L2_FIELD_INTERLACED_BT);
+
+	/* Check color format. */
+	if (mf->code != MEDIA_BUS_FMT_UYVY8_2X8)
+		return -EINVAL;
+
+	mf->colorspace = V4L2_COLORSPACE_SMPTE170M;
+
+	ret = tw9910_set_frame(sd, &width, &height);
+	if (ret)
+		return ret;
+
+	mf->width	= width;
+	mf->height	= height;
+
+	return 0;
+}
+
+static int tw9910_set_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *mf = &format->format;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct tw9910_priv *priv = to_tw9910(client);
+	const struct tw9910_scale_ctrl *scale;
+
+	if (format->pad)
+		return -EINVAL;
+
+	if (mf->field == V4L2_FIELD_ANY) {
+		mf->field = V4L2_FIELD_INTERLACED_BT;
+	} else if (mf->field != V4L2_FIELD_INTERLACED_BT) {
+		dev_err(&client->dev, "Field type %d invalid\n", mf->field);
+		return -EINVAL;
+	}
+
+	mf->code = MEDIA_BUS_FMT_UYVY8_2X8;
+	mf->colorspace = V4L2_COLORSPACE_SMPTE170M;
+
+	/* Select suitable norm. */
+	scale = tw9910_select_norm(priv->norm, mf->width, mf->height);
+	if (!scale)
+		return -EINVAL;
+
+	mf->width	= scale->width;
+	mf->height	= scale->height;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		return tw9910_s_fmt(sd, mf);
+
+	sd_state->pads->try_fmt = *mf;
+
+	return 0;
+}
+
+static int tw9910_video_probe(struct i2c_client *client)
+{
+	struct tw9910_priv *priv = to_tw9910(client);
+	s32 id;
+	int ret;
+
+	/* TW9910 only use 8 or 16 bit bus width. */
+	if (priv->info->buswidth != 16 && priv->info->buswidth != 8) {
+		dev_err(&client->dev, "bus width error\n");
+		return -ENODEV;
+	}
+
+	ret = tw9910_s_power(&priv->subdev, 1);
+	if (ret < 0)
+		return ret;
+
+	/*
+	 * Check and show Product ID.
+	 * So far only revisions 0 and 1 have been seen.
+	 */
+	id = i2c_smbus_read_byte_data(client, ID);
+	priv->revision = GET_REV(id);
+	id = GET_ID(id);
+
+	if (id != 0x0b || priv->revision > 0x01) {
+		dev_err(&client->dev, "Product ID error %x:%x\n",
+			id, priv->revision);
+		ret = -ENODEV;
+		goto done;
+	}
+
+	dev_info(&client->dev, "tw9910 Product ID %0x:%0x\n",
+		 id, priv->revision);
+
+	priv->norm = V4L2_STD_NTSC;
+	priv->scale = &tw9910_ntsc_scales[0];
+
+done:
+	tw9910_s_power(&priv->subdev, 0);
+
+	return ret;
+}
+
+static const struct v4l2_subdev_core_ops tw9910_subdev_core_ops = {
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register	= tw9910_g_register,
+	.s_register	= tw9910_s_register,
+#endif
+	.s_power	= tw9910_s_power,
+};
+
+static int tw9910_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->pad || code->index)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_UYVY8_2X8;
+
+	return 0;
+}
+
+static int tw9910_g_tvnorms(struct v4l2_subdev *sd, v4l2_std_id *norm)
+{
+	*norm = V4L2_STD_NTSC | V4L2_STD_PAL;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops tw9910_subdev_video_ops = {
+	.s_std		= tw9910_s_std,
+	.g_std		= tw9910_g_std,
+	.s_stream	= tw9910_s_stream,
+	.g_tvnorms	= tw9910_g_tvnorms,
+};
+
+static const struct v4l2_subdev_pad_ops tw9910_subdev_pad_ops = {
+	.enum_mbus_code = tw9910_enum_mbus_code,
+	.get_selection	= tw9910_get_selection,
+	.get_fmt	= tw9910_get_fmt,
+	.set_fmt	= tw9910_set_fmt,
+};
+
+static const struct v4l2_subdev_ops tw9910_subdev_ops = {
+	.core	= &tw9910_subdev_core_ops,
+	.video	= &tw9910_subdev_video_ops,
+	.pad	= &tw9910_subdev_pad_ops,
+};
+
+/*
+ * i2c_driver function
+ */
+
+static int tw9910_probe(struct i2c_client *client)
+
+{
+	struct tw9910_priv		*priv;
+	struct tw9910_video_info	*info;
+	struct i2c_adapter		*adapter = client->adapter;
+	int ret;
+
+	if (!client->dev.platform_data) {
+		dev_err(&client->dev, "TW9910: missing platform data!\n");
+		return -EINVAL;
+	}
+
+	info = client->dev.platform_data;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA)) {
+		dev_err(&client->dev,
+			"I2C-Adapter doesn't support I2C_FUNC_SMBUS_BYTE_DATA\n");
+		return -EIO;
+	}
+
+	priv = devm_kzalloc(&client->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->info = info;
+
+	v4l2_i2c_subdev_init(&priv->subdev, client, &tw9910_subdev_ops);
+
+	priv->clk = clk_get(&client->dev, "xti");
+	if (PTR_ERR(priv->clk) == -ENOENT) {
+		priv->clk = NULL;
+	} else if (IS_ERR(priv->clk)) {
+		dev_err(&client->dev, "Unable to get xti clock\n");
+		return PTR_ERR(priv->clk);
+	}
+
+	priv->pdn_gpio = gpiod_get_optional(&client->dev, "pdn",
+					    GPIOD_OUT_HIGH);
+	if (IS_ERR(priv->pdn_gpio)) {
+		dev_info(&client->dev, "Unable to get GPIO \"pdn\"");
+		ret = PTR_ERR(priv->pdn_gpio);
+		goto error_clk_put;
+	}
+
+	ret = tw9910_video_probe(client);
+	if (ret < 0)
+		goto error_gpio_put;
+
+	ret = v4l2_async_register_subdev(&priv->subdev);
+	if (ret)
+		goto error_gpio_put;
+
+	return ret;
+
+error_gpio_put:
+	if (priv->pdn_gpio)
+		gpiod_put(priv->pdn_gpio);
+error_clk_put:
+	clk_put(priv->clk);
+
+	return ret;
+}
+
+static void tw9910_remove(struct i2c_client *client)
+{
+	struct tw9910_priv *priv = to_tw9910(client);
+
+	if (priv->pdn_gpio)
+		gpiod_put(priv->pdn_gpio);
+	clk_put(priv->clk);
+	v4l2_async_unregister_subdev(&priv->subdev);
+}
+
+static const struct i2c_device_id tw9910_id[] = {
+	{ "tw9910", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, tw9910_id);
+
+static struct i2c_driver tw9910_i2c_driver = {
+	.driver = {
+		.name = "tw9910",
+	},
+	.probe    = tw9910_probe,
+	.remove   = tw9910_remove,
+	.id_table = tw9910_id,
+};
+
+module_i2c_driver(tw9910_i2c_driver);
+
+MODULE_DESCRIPTION("V4L2 driver for TW9910 video decoder");
+MODULE_AUTHOR("Kuninori Morimoto");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/uda1342.c b/drivers/media/i2c/uda1342.c
new file mode 100644
index 000000000..da7bc4700
--- /dev/null
+++ b/drivers/media/i2c/uda1342.c
@@ -0,0 +1,98 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2005-2006 Micronas USA Inc.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/i2c/uda1342.h>
+#include <linux/slab.h>
+
+static int write_reg(struct i2c_client *client, int reg, int value)
+{
+	/* UDA1342 wants MSB first, but SMBus sends LSB first */
+	i2c_smbus_write_word_data(client, reg, swab16(value));
+	return 0;
+}
+
+static int uda1342_s_routing(struct v4l2_subdev *sd,
+		u32 input, u32 output, u32 config)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	switch (input) {
+	case UDA1342_IN1:
+		write_reg(client, 0x00, 0x1241); /* select input 1 */
+		break;
+	case UDA1342_IN2:
+		write_reg(client, 0x00, 0x1441); /* select input 2 */
+		break;
+	default:
+		v4l2_err(sd, "input %d not supported\n", input);
+		break;
+	}
+	return 0;
+}
+
+static const struct v4l2_subdev_audio_ops uda1342_audio_ops = {
+	.s_routing = uda1342_s_routing,
+};
+
+static const struct v4l2_subdev_ops uda1342_ops = {
+	.audio = &uda1342_audio_ops,
+};
+
+static int uda1342_probe(struct i2c_client *client)
+{
+	struct i2c_adapter *adapter = client->adapter;
+	struct v4l2_subdev *sd;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA))
+		return -ENODEV;
+
+	dev_dbg(&client->dev, "initializing UDA1342 at address %d on %s\n",
+		client->addr, adapter->name);
+
+	sd = devm_kzalloc(&client->dev, sizeof(*sd), GFP_KERNEL);
+	if (sd == NULL)
+		return -ENOMEM;
+
+	v4l2_i2c_subdev_init(sd, client, &uda1342_ops);
+
+	write_reg(client, 0x00, 0x8000); /* reset registers */
+	write_reg(client, 0x00, 0x1241); /* select input 1 */
+
+	v4l_info(client, "chip found @ 0x%02x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	return 0;
+}
+
+static void uda1342_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+}
+
+static const struct i2c_device_id uda1342_id[] = {
+	{ "uda1342", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, uda1342_id);
+
+static struct i2c_driver uda1342_driver = {
+	.driver = {
+		.name	= "uda1342",
+	},
+	.probe		= uda1342_probe,
+	.remove		= uda1342_remove,
+	.id_table	= uda1342_id,
+};
+
+module_i2c_driver(uda1342_driver);
+
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/upd64031a.c b/drivers/media/i2c/upd64031a.c
new file mode 100644
index 000000000..54c2ba0ba
--- /dev/null
+++ b/drivers/media/i2c/upd64031a.c
@@ -0,0 +1,236 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * upd64031A - NEC Electronics Ghost Reduction for NTSC in Japan
+ *
+ * 2003 by T.Adachi <tadachi@tadachi-net.com>
+ * 2003 by Takeru KOMORIYA <komoriya@paken.org>
+ * 2006 by Hans Verkuil <hverkuil@xs4all.nl>
+ */
+
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <linux/slab.h>
+#include <media/v4l2-device.h>
+#include <media/i2c/upd64031a.h>
+
+/* --------------------- read registers functions define -------------------- */
+
+/* bit masks */
+#define GR_MODE_MASK              0xc0
+#define DIRECT_3DYCS_CONNECT_MASK 0xc0
+#define SYNC_CIRCUIT_MASK         0xa0
+
+/* -------------------------------------------------------------------------- */
+
+MODULE_DESCRIPTION("uPD64031A driver");
+MODULE_AUTHOR("T. Adachi, Takeru KOMORIYA, Hans Verkuil");
+MODULE_LICENSE("GPL");
+
+static int debug;
+module_param(debug, int, 0644);
+
+MODULE_PARM_DESC(debug, "Debug level (0-1)");
+
+
+enum {
+	R00 = 0, R01, R02, R03, R04,
+	R05, R06, R07, R08, R09,
+	R0A, R0B, R0C, R0D, R0E, R0F,
+	/* unused registers
+	 R10, R11, R12, R13, R14,
+	 R15, R16, R17,
+	 */
+	TOT_REGS
+};
+
+struct upd64031a_state {
+	struct v4l2_subdev sd;
+	u8 regs[TOT_REGS];
+	u8 gr_mode;
+	u8 direct_3dycs_connect;
+	u8 ext_comp_sync;
+	u8 ext_vert_sync;
+};
+
+static inline struct upd64031a_state *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct upd64031a_state, sd);
+}
+
+static u8 upd64031a_init[] = {
+	0x00, 0xb8, 0x48, 0xd2, 0xe6,
+	0x03, 0x10, 0x0b, 0xaf, 0x7f,
+	0x00, 0x00, 0x1d, 0x5e, 0x00,
+	0xd0
+};
+
+/* ------------------------------------------------------------------------ */
+
+static u8 upd64031a_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u8 buf[2];
+
+	if (reg >= sizeof(buf))
+		return 0xff;
+	i2c_master_recv(client, buf, 2);
+	return buf[reg];
+}
+
+/* ------------------------------------------------------------------------ */
+
+static void upd64031a_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u8 buf[2];
+
+	buf[0] = reg;
+	buf[1] = val;
+	v4l2_dbg(1, debug, sd, "write reg: %02X val: %02X\n", reg, val);
+	if (i2c_master_send(client, buf, 2) != 2)
+		v4l2_err(sd, "I/O error write 0x%02x/0x%02x\n", reg, val);
+}
+
+/* ------------------------------------------------------------------------ */
+
+/* The input changed due to new input or channel changed */
+static int upd64031a_s_frequency(struct v4l2_subdev *sd, const struct v4l2_frequency *freq)
+{
+	struct upd64031a_state *state = to_state(sd);
+	u8 reg = state->regs[R00];
+
+	v4l2_dbg(1, debug, sd, "changed input or channel\n");
+	upd64031a_write(sd, R00, reg | 0x10);
+	upd64031a_write(sd, R00, reg & ~0x10);
+	return 0;
+}
+
+/* ------------------------------------------------------------------------ */
+
+static int upd64031a_s_routing(struct v4l2_subdev *sd,
+			       u32 input, u32 output, u32 config)
+{
+	struct upd64031a_state *state = to_state(sd);
+	u8 r00, r05, r08;
+
+	state->gr_mode = (input & 3) << 6;
+	state->direct_3dycs_connect = (input & 0xc) << 4;
+	state->ext_comp_sync =
+		(input & UPD64031A_COMPOSITE_EXTERNAL) << 1;
+	state->ext_vert_sync =
+		(input & UPD64031A_VERTICAL_EXTERNAL) << 2;
+	r00 = (state->regs[R00] & ~GR_MODE_MASK) | state->gr_mode;
+	r05 = (state->regs[R00] & ~SYNC_CIRCUIT_MASK) |
+		state->ext_comp_sync | state->ext_vert_sync;
+	r08 = (state->regs[R08] & ~DIRECT_3DYCS_CONNECT_MASK) |
+		state->direct_3dycs_connect;
+	upd64031a_write(sd, R00, r00);
+	upd64031a_write(sd, R05, r05);
+	upd64031a_write(sd, R08, r08);
+	return upd64031a_s_frequency(sd, NULL);
+}
+
+static int upd64031a_log_status(struct v4l2_subdev *sd)
+{
+	v4l2_info(sd, "Status: SA00=0x%02x SA01=0x%02x\n",
+			upd64031a_read(sd, 0), upd64031a_read(sd, 1));
+	return 0;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int upd64031a_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+{
+	reg->val = upd64031a_read(sd, reg->reg & 0xff);
+	reg->size = 1;
+	return 0;
+}
+
+static int upd64031a_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
+{
+	upd64031a_write(sd, reg->reg & 0xff, reg->val & 0xff);
+	return 0;
+}
+#endif
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_core_ops upd64031a_core_ops = {
+	.log_status = upd64031a_log_status,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = upd64031a_g_register,
+	.s_register = upd64031a_s_register,
+#endif
+};
+
+static const struct v4l2_subdev_tuner_ops upd64031a_tuner_ops = {
+	.s_frequency = upd64031a_s_frequency,
+};
+
+static const struct v4l2_subdev_video_ops upd64031a_video_ops = {
+	.s_routing = upd64031a_s_routing,
+};
+
+static const struct v4l2_subdev_ops upd64031a_ops = {
+	.core = &upd64031a_core_ops,
+	.tuner = &upd64031a_tuner_ops,
+	.video = &upd64031a_video_ops,
+};
+
+/* ------------------------------------------------------------------------ */
+
+/* i2c implementation */
+
+static int upd64031a_probe(struct i2c_client *client)
+{
+	struct upd64031a_state *state;
+	struct v4l2_subdev *sd;
+	int i;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	state = devm_kzalloc(&client->dev, sizeof(*state), GFP_KERNEL);
+	if (state == NULL)
+		return -ENOMEM;
+	sd = &state->sd;
+	v4l2_i2c_subdev_init(sd, client, &upd64031a_ops);
+	memcpy(state->regs, upd64031a_init, sizeof(state->regs));
+	state->gr_mode = UPD64031A_GR_ON << 6;
+	state->direct_3dycs_connect = UPD64031A_3DYCS_COMPOSITE << 4;
+	state->ext_comp_sync = state->ext_vert_sync = 0;
+	for (i = 0; i < TOT_REGS; i++)
+		upd64031a_write(sd, i, state->regs[i]);
+	return 0;
+}
+
+static void upd64031a_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id upd64031a_id[] = {
+	{ "upd64031a", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, upd64031a_id);
+
+static struct i2c_driver upd64031a_driver = {
+	.driver = {
+		.name	= "upd64031a",
+	},
+	.probe		= upd64031a_probe,
+	.remove		= upd64031a_remove,
+	.id_table	= upd64031a_id,
+};
+
+module_i2c_driver(upd64031a_driver);
diff --git a/drivers/media/i2c/upd64083.c b/drivers/media/i2c/upd64083.c
new file mode 100644
index 000000000..2a820589a
--- /dev/null
+++ b/drivers/media/i2c/upd64083.c
@@ -0,0 +1,207 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * upd6408x - NEC Electronics 3-Dimensional Y/C separation driver
+ *
+ * 2003 by T.Adachi (tadachi@tadachi-net.com)
+ * 2003 by Takeru KOMORIYA <komoriya@paken.org>
+ * 2006 by Hans Verkuil <hverkuil@xs4all.nl>
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <linux/slab.h>
+#include <media/v4l2-device.h>
+#include <media/i2c/upd64083.h>
+
+MODULE_DESCRIPTION("uPD64083 driver");
+MODULE_AUTHOR("T. Adachi, Takeru KOMORIYA, Hans Verkuil");
+MODULE_LICENSE("GPL");
+
+static bool debug;
+module_param(debug, bool, 0644);
+
+MODULE_PARM_DESC(debug, "Debug level (0-1)");
+
+
+enum {
+	R00 = 0, R01, R02, R03, R04,
+	R05, R06, R07, R08, R09,
+	R0A, R0B, R0C, R0D, R0E, R0F,
+	R10, R11, R12, R13, R14,
+	R15, R16,
+	TOT_REGS
+};
+
+struct upd64083_state {
+	struct v4l2_subdev sd;
+	u8 mode;
+	u8 ext_y_adc;
+	u8 regs[TOT_REGS];
+};
+
+static inline struct upd64083_state *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct upd64083_state, sd);
+}
+
+/* Initial values when used in combination with the
+   NEC upd64031a ghost reduction chip. */
+static u8 upd64083_init[] = {
+	0x1f, 0x01, 0xa0, 0x2d, 0x29,  /* we use EXCSS=0 */
+	0x36, 0xdd, 0x05, 0x56, 0x48,
+	0x00, 0x3a, 0xa0, 0x05, 0x08,
+	0x44, 0x60, 0x08, 0x52, 0xf8,
+	0x53, 0x60, 0x10
+};
+
+/* ------------------------------------------------------------------------ */
+
+static void upd64083_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u8 buf[2];
+
+	buf[0] = reg;
+	buf[1] = val;
+	v4l2_dbg(1, debug, sd, "write reg: %02x val: %02x\n", reg, val);
+	if (i2c_master_send(client, buf, 2) != 2)
+		v4l2_err(sd, "I/O error write 0x%02x/0x%02x\n", reg, val);
+}
+
+/* ------------------------------------------------------------------------ */
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static u8 upd64083_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u8 buf[7];
+
+	if (reg >= sizeof(buf))
+		return 0xff;
+	i2c_master_recv(client, buf, sizeof(buf));
+	return buf[reg];
+}
+#endif
+
+/* ------------------------------------------------------------------------ */
+
+static int upd64083_s_routing(struct v4l2_subdev *sd,
+			      u32 input, u32 output, u32 config)
+{
+	struct upd64083_state *state = to_state(sd);
+	u8 r00, r02;
+
+	if (input > 7 || (input & 6) == 6)
+		return -EINVAL;
+	state->mode = (input & 3) << 6;
+	state->ext_y_adc = (input & UPD64083_EXT_Y_ADC) << 3;
+	r00 = (state->regs[R00] & ~(3 << 6)) | state->mode;
+	r02 = (state->regs[R02] & ~(1 << 5)) | state->ext_y_adc;
+	upd64083_write(sd, R00, r00);
+	upd64083_write(sd, R02, r02);
+	return 0;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int upd64083_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+{
+	reg->val = upd64083_read(sd, reg->reg & 0xff);
+	reg->size = 1;
+	return 0;
+}
+
+static int upd64083_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
+{
+	upd64083_write(sd, reg->reg & 0xff, reg->val & 0xff);
+	return 0;
+}
+#endif
+
+static int upd64083_log_status(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u8 buf[7];
+
+	i2c_master_recv(client, buf, 7);
+	v4l2_info(sd, "Status: SA00=%02x SA01=%02x SA02=%02x SA03=%02x "
+		      "SA04=%02x SA05=%02x SA06=%02x\n",
+		buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6]);
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_core_ops upd64083_core_ops = {
+	.log_status = upd64083_log_status,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = upd64083_g_register,
+	.s_register = upd64083_s_register,
+#endif
+};
+
+static const struct v4l2_subdev_video_ops upd64083_video_ops = {
+	.s_routing = upd64083_s_routing,
+};
+
+static const struct v4l2_subdev_ops upd64083_ops = {
+	.core = &upd64083_core_ops,
+	.video = &upd64083_video_ops,
+};
+
+/* ------------------------------------------------------------------------ */
+
+/* i2c implementation */
+
+static int upd64083_probe(struct i2c_client *client)
+{
+	struct upd64083_state *state;
+	struct v4l2_subdev *sd;
+	int i;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	state = devm_kzalloc(&client->dev, sizeof(*state), GFP_KERNEL);
+	if (state == NULL)
+		return -ENOMEM;
+	sd = &state->sd;
+	v4l2_i2c_subdev_init(sd, client, &upd64083_ops);
+	/* Initially assume that a ghost reduction chip is present */
+	state->mode = 0;  /* YCS mode */
+	state->ext_y_adc = (1 << 5);
+	memcpy(state->regs, upd64083_init, TOT_REGS);
+	for (i = 0; i < TOT_REGS; i++)
+		upd64083_write(sd, i, state->regs[i]);
+	return 0;
+}
+
+static void upd64083_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id upd64083_id[] = {
+	{ "upd64083", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, upd64083_id);
+
+static struct i2c_driver upd64083_driver = {
+	.driver = {
+		.name	= "upd64083",
+	},
+	.probe		= upd64083_probe,
+	.remove		= upd64083_remove,
+	.id_table	= upd64083_id,
+};
+
+module_i2c_driver(upd64083_driver);
diff --git a/drivers/media/i2c/video-i2c.c b/drivers/media/i2c/video-i2c.c
new file mode 100644
index 000000000..537ebd9fa
--- /dev/null
+++ b/drivers/media/i2c/video-i2c.c
@@ -0,0 +1,971 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * video-i2c.c - Support for I2C transport video devices
+ *
+ * Copyright (C) 2018 Matt Ranostay <matt.ranostay@konsulko.com>
+ *
+ * Supported:
+ * - Panasonic AMG88xx Grid-Eye Sensors
+ * - Melexis MLX90640 Thermal Cameras
+ */
+
+#include <linux/bits.h>
+#include <linux/delay.h>
+#include <linux/freezer.h>
+#include <linux/hwmon.h>
+#include <linux/kthread.h>
+#include <linux/i2c.h>
+#include <linux/list.h>
+#include <linux/mod_devicetable.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/pm_runtime.h>
+#include <linux/nvmem-provider.h>
+#include <linux/regmap.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/videobuf2-vmalloc.h>
+
+#define VIDEO_I2C_DRIVER	"video-i2c"
+
+/* Power control register */
+#define AMG88XX_REG_PCTL	0x00
+#define AMG88XX_PCTL_NORMAL		0x00
+#define AMG88XX_PCTL_SLEEP		0x10
+
+/* Reset register */
+#define AMG88XX_REG_RST		0x01
+#define AMG88XX_RST_FLAG		0x30
+#define AMG88XX_RST_INIT		0x3f
+
+/* Frame rate register */
+#define AMG88XX_REG_FPSC	0x02
+#define AMG88XX_FPSC_1FPS		BIT(0)
+
+/* Thermistor register */
+#define AMG88XX_REG_TTHL	0x0e
+
+/* Temperature register */
+#define AMG88XX_REG_T01L	0x80
+
+/* RAM */
+#define MLX90640_RAM_START_ADDR		0x0400
+
+/* EEPROM */
+#define MLX90640_EEPROM_START_ADDR	0x2400
+
+/* Control register */
+#define MLX90640_REG_CTL1		0x800d
+#define MLX90640_REG_CTL1_MASK		GENMASK(9, 7)
+#define MLX90640_REG_CTL1_MASK_SHIFT	7
+
+struct video_i2c_chip;
+
+struct video_i2c_buffer {
+	struct vb2_v4l2_buffer vb;
+	struct list_head list;
+};
+
+struct video_i2c_data {
+	struct regmap *regmap;
+	const struct video_i2c_chip *chip;
+	struct mutex lock;
+	spinlock_t slock;
+	unsigned int sequence;
+	struct mutex queue_lock;
+
+	struct v4l2_device v4l2_dev;
+	struct video_device vdev;
+	struct vb2_queue vb_vidq;
+
+	struct task_struct *kthread_vid_cap;
+	struct list_head vid_cap_active;
+
+	struct v4l2_fract frame_interval;
+};
+
+static const struct v4l2_fmtdesc amg88xx_format = {
+	.pixelformat = V4L2_PIX_FMT_Y12,
+};
+
+static const struct v4l2_frmsize_discrete amg88xx_size = {
+	.width = 8,
+	.height = 8,
+};
+
+static const struct v4l2_fmtdesc mlx90640_format = {
+	.pixelformat = V4L2_PIX_FMT_Y16_BE,
+};
+
+static const struct v4l2_frmsize_discrete mlx90640_size = {
+	.width = 32,
+	.height = 26, /* 24 lines of pixel data + 2 lines of processing data */
+};
+
+static const struct regmap_config amg88xx_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+	.max_register = 0xff
+};
+
+static const struct regmap_config mlx90640_regmap_config = {
+	.reg_bits = 16,
+	.val_bits = 16,
+};
+
+struct video_i2c_chip {
+	/* video dimensions */
+	const struct v4l2_fmtdesc *format;
+	const struct v4l2_frmsize_discrete *size;
+
+	/* available frame intervals */
+	const struct v4l2_fract *frame_intervals;
+	unsigned int num_frame_intervals;
+
+	/* pixel buffer size */
+	unsigned int buffer_size;
+
+	/* pixel size in bits */
+	unsigned int bpp;
+
+	const struct regmap_config *regmap_config;
+	struct nvmem_config *nvmem_config;
+
+	/* setup function */
+	int (*setup)(struct video_i2c_data *data);
+
+	/* xfer function */
+	int (*xfer)(struct video_i2c_data *data, char *buf);
+
+	/* power control function */
+	int (*set_power)(struct video_i2c_data *data, bool on);
+
+	/* hwmon init function */
+	int (*hwmon_init)(struct video_i2c_data *data);
+};
+
+static int mlx90640_nvram_read(void *priv, unsigned int offset, void *val,
+			     size_t bytes)
+{
+	struct video_i2c_data *data = priv;
+
+	return regmap_bulk_read(data->regmap, MLX90640_EEPROM_START_ADDR + offset, val, bytes);
+}
+
+static struct nvmem_config mlx90640_nvram_config = {
+	.name = "mlx90640_nvram",
+	.word_size = 2,
+	.stride = 1,
+	.size = 1664,
+	.reg_read = mlx90640_nvram_read,
+};
+
+static int amg88xx_xfer(struct video_i2c_data *data, char *buf)
+{
+	return regmap_bulk_read(data->regmap, AMG88XX_REG_T01L, buf,
+				data->chip->buffer_size);
+}
+
+static int mlx90640_xfer(struct video_i2c_data *data, char *buf)
+{
+	return regmap_bulk_read(data->regmap, MLX90640_RAM_START_ADDR, buf,
+				data->chip->buffer_size);
+}
+
+static int amg88xx_setup(struct video_i2c_data *data)
+{
+	unsigned int mask = AMG88XX_FPSC_1FPS;
+	unsigned int val;
+
+	if (data->frame_interval.numerator == data->frame_interval.denominator)
+		val = mask;
+	else
+		val = 0;
+
+	return regmap_update_bits(data->regmap, AMG88XX_REG_FPSC, mask, val);
+}
+
+static int mlx90640_setup(struct video_i2c_data *data)
+{
+	unsigned int n, idx;
+
+	for (n = 0; n < data->chip->num_frame_intervals - 1; n++) {
+		if (V4L2_FRACT_COMPARE(data->frame_interval, ==,
+				       data->chip->frame_intervals[n]))
+			break;
+	}
+
+	idx = data->chip->num_frame_intervals - n - 1;
+
+	return regmap_update_bits(data->regmap, MLX90640_REG_CTL1,
+				  MLX90640_REG_CTL1_MASK,
+				  idx << MLX90640_REG_CTL1_MASK_SHIFT);
+}
+
+static int amg88xx_set_power_on(struct video_i2c_data *data)
+{
+	int ret;
+
+	ret = regmap_write(data->regmap, AMG88XX_REG_PCTL, AMG88XX_PCTL_NORMAL);
+	if (ret)
+		return ret;
+
+	msleep(50);
+
+	ret = regmap_write(data->regmap, AMG88XX_REG_RST, AMG88XX_RST_INIT);
+	if (ret)
+		return ret;
+
+	usleep_range(2000, 3000);
+
+	ret = regmap_write(data->regmap, AMG88XX_REG_RST, AMG88XX_RST_FLAG);
+	if (ret)
+		return ret;
+
+	/*
+	 * Wait two frames before reading thermistor and temperature registers
+	 */
+	msleep(200);
+
+	return 0;
+}
+
+static int amg88xx_set_power_off(struct video_i2c_data *data)
+{
+	int ret;
+
+	ret = regmap_write(data->regmap, AMG88XX_REG_PCTL, AMG88XX_PCTL_SLEEP);
+	if (ret)
+		return ret;
+	/*
+	 * Wait for a while to avoid resuming normal mode immediately after
+	 * entering sleep mode, otherwise the device occasionally goes wrong
+	 * (thermistor and temperature registers are not updated at all)
+	 */
+	msleep(100);
+
+	return 0;
+}
+
+static int amg88xx_set_power(struct video_i2c_data *data, bool on)
+{
+	if (on)
+		return amg88xx_set_power_on(data);
+
+	return amg88xx_set_power_off(data);
+}
+
+#if IS_REACHABLE(CONFIG_HWMON)
+
+static const u32 amg88xx_temp_config[] = {
+	HWMON_T_INPUT,
+	0
+};
+
+static const struct hwmon_channel_info amg88xx_temp = {
+	.type = hwmon_temp,
+	.config = amg88xx_temp_config,
+};
+
+static const struct hwmon_channel_info * const amg88xx_info[] = {
+	&amg88xx_temp,
+	NULL
+};
+
+static umode_t amg88xx_is_visible(const void *drvdata,
+				  enum hwmon_sensor_types type,
+				  u32 attr, int channel)
+{
+	return 0444;
+}
+
+static int amg88xx_read(struct device *dev, enum hwmon_sensor_types type,
+			u32 attr, int channel, long *val)
+{
+	struct video_i2c_data *data = dev_get_drvdata(dev);
+	__le16 buf;
+	int tmp;
+
+	tmp = pm_runtime_resume_and_get(regmap_get_device(data->regmap));
+	if (tmp < 0)
+		return tmp;
+
+	tmp = regmap_bulk_read(data->regmap, AMG88XX_REG_TTHL, &buf, 2);
+	pm_runtime_mark_last_busy(regmap_get_device(data->regmap));
+	pm_runtime_put_autosuspend(regmap_get_device(data->regmap));
+	if (tmp)
+		return tmp;
+
+	tmp = le16_to_cpu(buf);
+
+	/*
+	 * Check for sign bit, this isn't a two's complement value but an
+	 * absolute temperature that needs to be inverted in the case of being
+	 * negative.
+	 */
+	if (tmp & BIT(11))
+		tmp = -(tmp & 0x7ff);
+
+	*val = (tmp * 625) / 10;
+
+	return 0;
+}
+
+static const struct hwmon_ops amg88xx_hwmon_ops = {
+	.is_visible = amg88xx_is_visible,
+	.read = amg88xx_read,
+};
+
+static const struct hwmon_chip_info amg88xx_chip_info = {
+	.ops = &amg88xx_hwmon_ops,
+	.info = amg88xx_info,
+};
+
+static int amg88xx_hwmon_init(struct video_i2c_data *data)
+{
+	struct device *dev = regmap_get_device(data->regmap);
+	void *hwmon = devm_hwmon_device_register_with_info(dev, "amg88xx", data,
+						&amg88xx_chip_info, NULL);
+
+	return PTR_ERR_OR_ZERO(hwmon);
+}
+#else
+#define	amg88xx_hwmon_init	NULL
+#endif
+
+enum {
+	AMG88XX,
+	MLX90640,
+};
+
+static const struct v4l2_fract amg88xx_frame_intervals[] = {
+	{ 1, 10 },
+	{ 1, 1 },
+};
+
+static const struct v4l2_fract mlx90640_frame_intervals[] = {
+	{ 1, 64 },
+	{ 1, 32 },
+	{ 1, 16 },
+	{ 1, 8 },
+	{ 1, 4 },
+	{ 1, 2 },
+	{ 1, 1 },
+	{ 2, 1 },
+};
+
+static const struct video_i2c_chip video_i2c_chip[] = {
+	[AMG88XX] = {
+		.size		= &amg88xx_size,
+		.format		= &amg88xx_format,
+		.frame_intervals	= amg88xx_frame_intervals,
+		.num_frame_intervals	= ARRAY_SIZE(amg88xx_frame_intervals),
+		.buffer_size	= 128,
+		.bpp		= 16,
+		.regmap_config	= &amg88xx_regmap_config,
+		.setup		= &amg88xx_setup,
+		.xfer		= &amg88xx_xfer,
+		.set_power	= amg88xx_set_power,
+		.hwmon_init	= amg88xx_hwmon_init,
+	},
+	[MLX90640] = {
+		.size		= &mlx90640_size,
+		.format		= &mlx90640_format,
+		.frame_intervals	= mlx90640_frame_intervals,
+		.num_frame_intervals	= ARRAY_SIZE(mlx90640_frame_intervals),
+		.buffer_size	= 1664,
+		.bpp		= 16,
+		.regmap_config	= &mlx90640_regmap_config,
+		.nvmem_config	= &mlx90640_nvram_config,
+		.setup		= mlx90640_setup,
+		.xfer		= mlx90640_xfer,
+	},
+};
+
+static const struct v4l2_file_operations video_i2c_fops = {
+	.owner		= THIS_MODULE,
+	.open		= v4l2_fh_open,
+	.release	= vb2_fop_release,
+	.poll		= vb2_fop_poll,
+	.read		= vb2_fop_read,
+	.mmap		= vb2_fop_mmap,
+	.unlocked_ioctl = video_ioctl2,
+};
+
+static int queue_setup(struct vb2_queue *vq,
+		       unsigned int *nbuffers, unsigned int *nplanes,
+		       unsigned int sizes[], struct device *alloc_devs[])
+{
+	struct video_i2c_data *data = vb2_get_drv_priv(vq);
+	unsigned int size = data->chip->buffer_size;
+
+	if (vq->num_buffers + *nbuffers < 2)
+		*nbuffers = 2;
+
+	if (*nplanes)
+		return sizes[0] < size ? -EINVAL : 0;
+
+	*nplanes = 1;
+	sizes[0] = size;
+
+	return 0;
+}
+
+static int buffer_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct video_i2c_data *data = vb2_get_drv_priv(vb->vb2_queue);
+	unsigned int size = data->chip->buffer_size;
+
+	if (vb2_plane_size(vb, 0) < size)
+		return -EINVAL;
+
+	vbuf->field = V4L2_FIELD_NONE;
+	vb2_set_plane_payload(vb, 0, size);
+
+	return 0;
+}
+
+static void buffer_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct video_i2c_data *data = vb2_get_drv_priv(vb->vb2_queue);
+	struct video_i2c_buffer *buf =
+			container_of(vbuf, struct video_i2c_buffer, vb);
+
+	spin_lock(&data->slock);
+	list_add_tail(&buf->list, &data->vid_cap_active);
+	spin_unlock(&data->slock);
+}
+
+static int video_i2c_thread_vid_cap(void *priv)
+{
+	struct video_i2c_data *data = priv;
+	u32 delay = mult_frac(1000000UL, data->frame_interval.numerator,
+			       data->frame_interval.denominator);
+	s64 end_us = ktime_to_us(ktime_get());
+
+	set_freezable();
+
+	do {
+		struct video_i2c_buffer *vid_cap_buf = NULL;
+		s64 current_us;
+		int schedule_delay;
+
+		try_to_freeze();
+
+		spin_lock(&data->slock);
+
+		if (!list_empty(&data->vid_cap_active)) {
+			vid_cap_buf = list_last_entry(&data->vid_cap_active,
+						 struct video_i2c_buffer, list);
+			list_del(&vid_cap_buf->list);
+		}
+
+		spin_unlock(&data->slock);
+
+		if (vid_cap_buf) {
+			struct vb2_buffer *vb2_buf = &vid_cap_buf->vb.vb2_buf;
+			void *vbuf = vb2_plane_vaddr(vb2_buf, 0);
+			int ret;
+
+			ret = data->chip->xfer(data, vbuf);
+			vb2_buf->timestamp = ktime_get_ns();
+			vid_cap_buf->vb.sequence = data->sequence++;
+			vb2_buffer_done(vb2_buf, ret ?
+				VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+		}
+
+		end_us += delay;
+		current_us = ktime_to_us(ktime_get());
+		if (current_us < end_us) {
+			schedule_delay = end_us - current_us;
+			usleep_range(schedule_delay * 3 / 4, schedule_delay);
+		} else {
+			end_us = current_us;
+		}
+	} while (!kthread_should_stop());
+
+	return 0;
+}
+
+static void video_i2c_del_list(struct vb2_queue *vq, enum vb2_buffer_state state)
+{
+	struct video_i2c_data *data = vb2_get_drv_priv(vq);
+	struct video_i2c_buffer *buf, *tmp;
+
+	spin_lock(&data->slock);
+
+	list_for_each_entry_safe(buf, tmp, &data->vid_cap_active, list) {
+		list_del(&buf->list);
+		vb2_buffer_done(&buf->vb.vb2_buf, state);
+	}
+
+	spin_unlock(&data->slock);
+}
+
+static int start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct video_i2c_data *data = vb2_get_drv_priv(vq);
+	struct device *dev = regmap_get_device(data->regmap);
+	int ret;
+
+	if (data->kthread_vid_cap)
+		return 0;
+
+	ret = pm_runtime_resume_and_get(dev);
+	if (ret < 0)
+		goto error_del_list;
+
+	ret = data->chip->setup(data);
+	if (ret)
+		goto error_rpm_put;
+
+	data->sequence = 0;
+	data->kthread_vid_cap = kthread_run(video_i2c_thread_vid_cap, data,
+					    "%s-vid-cap", data->v4l2_dev.name);
+	ret = PTR_ERR_OR_ZERO(data->kthread_vid_cap);
+	if (!ret)
+		return 0;
+
+error_rpm_put:
+	pm_runtime_mark_last_busy(dev);
+	pm_runtime_put_autosuspend(dev);
+error_del_list:
+	video_i2c_del_list(vq, VB2_BUF_STATE_QUEUED);
+
+	return ret;
+}
+
+static void stop_streaming(struct vb2_queue *vq)
+{
+	struct video_i2c_data *data = vb2_get_drv_priv(vq);
+
+	if (data->kthread_vid_cap == NULL)
+		return;
+
+	kthread_stop(data->kthread_vid_cap);
+	data->kthread_vid_cap = NULL;
+	pm_runtime_mark_last_busy(regmap_get_device(data->regmap));
+	pm_runtime_put_autosuspend(regmap_get_device(data->regmap));
+
+	video_i2c_del_list(vq, VB2_BUF_STATE_ERROR);
+}
+
+static const struct vb2_ops video_i2c_video_qops = {
+	.queue_setup		= queue_setup,
+	.buf_prepare		= buffer_prepare,
+	.buf_queue		= buffer_queue,
+	.start_streaming	= start_streaming,
+	.stop_streaming		= stop_streaming,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
+};
+
+static int video_i2c_querycap(struct file *file, void  *priv,
+				struct v4l2_capability *vcap)
+{
+	struct video_i2c_data *data = video_drvdata(file);
+	struct device *dev = regmap_get_device(data->regmap);
+	struct i2c_client *client = to_i2c_client(dev);
+
+	strscpy(vcap->driver, data->v4l2_dev.name, sizeof(vcap->driver));
+	strscpy(vcap->card, data->vdev.name, sizeof(vcap->card));
+
+	sprintf(vcap->bus_info, "I2C:%d-%d", client->adapter->nr, client->addr);
+
+	return 0;
+}
+
+static int video_i2c_g_input(struct file *file, void *fh, unsigned int *inp)
+{
+	*inp = 0;
+
+	return 0;
+}
+
+static int video_i2c_s_input(struct file *file, void *fh, unsigned int inp)
+{
+	return (inp > 0) ? -EINVAL : 0;
+}
+
+static int video_i2c_enum_input(struct file *file, void *fh,
+				  struct v4l2_input *vin)
+{
+	if (vin->index > 0)
+		return -EINVAL;
+
+	strscpy(vin->name, "Camera", sizeof(vin->name));
+
+	vin->type = V4L2_INPUT_TYPE_CAMERA;
+
+	return 0;
+}
+
+static int video_i2c_enum_fmt_vid_cap(struct file *file, void *fh,
+					struct v4l2_fmtdesc *fmt)
+{
+	struct video_i2c_data *data = video_drvdata(file);
+	enum v4l2_buf_type type = fmt->type;
+
+	if (fmt->index > 0)
+		return -EINVAL;
+
+	*fmt = *data->chip->format;
+	fmt->type = type;
+
+	return 0;
+}
+
+static int video_i2c_enum_framesizes(struct file *file, void *fh,
+				       struct v4l2_frmsizeenum *fsize)
+{
+	const struct video_i2c_data *data = video_drvdata(file);
+	const struct v4l2_frmsize_discrete *size = data->chip->size;
+
+	/* currently only one frame size is allowed */
+	if (fsize->index > 0)
+		return -EINVAL;
+
+	if (fsize->pixel_format != data->chip->format->pixelformat)
+		return -EINVAL;
+
+	fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+	fsize->discrete.width = size->width;
+	fsize->discrete.height = size->height;
+
+	return 0;
+}
+
+static int video_i2c_enum_frameintervals(struct file *file, void *priv,
+					   struct v4l2_frmivalenum *fe)
+{
+	const struct video_i2c_data *data = video_drvdata(file);
+	const struct v4l2_frmsize_discrete *size = data->chip->size;
+
+	if (fe->index >= data->chip->num_frame_intervals)
+		return -EINVAL;
+
+	if (fe->width != size->width || fe->height != size->height)
+		return -EINVAL;
+
+	fe->type = V4L2_FRMIVAL_TYPE_DISCRETE;
+	fe->discrete = data->chip->frame_intervals[fe->index];
+
+	return 0;
+}
+
+static int video_i2c_try_fmt_vid_cap(struct file *file, void *fh,
+				       struct v4l2_format *fmt)
+{
+	const struct video_i2c_data *data = video_drvdata(file);
+	const struct v4l2_frmsize_discrete *size = data->chip->size;
+	struct v4l2_pix_format *pix = &fmt->fmt.pix;
+	unsigned int bpp = data->chip->bpp / 8;
+
+	pix->width = size->width;
+	pix->height = size->height;
+	pix->pixelformat = data->chip->format->pixelformat;
+	pix->field = V4L2_FIELD_NONE;
+	pix->bytesperline = pix->width * bpp;
+	pix->sizeimage = pix->bytesperline * pix->height;
+	pix->colorspace = V4L2_COLORSPACE_RAW;
+
+	return 0;
+}
+
+static int video_i2c_s_fmt_vid_cap(struct file *file, void *fh,
+				     struct v4l2_format *fmt)
+{
+	struct video_i2c_data *data = video_drvdata(file);
+
+	if (vb2_is_busy(&data->vb_vidq))
+		return -EBUSY;
+
+	return video_i2c_try_fmt_vid_cap(file, fh, fmt);
+}
+
+static int video_i2c_g_parm(struct file *filp, void *priv,
+			      struct v4l2_streamparm *parm)
+{
+	struct video_i2c_data *data = video_drvdata(filp);
+
+	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	parm->parm.capture.readbuffers = 1;
+	parm->parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
+	parm->parm.capture.timeperframe = data->frame_interval;
+
+	return 0;
+}
+
+static int video_i2c_s_parm(struct file *filp, void *priv,
+			      struct v4l2_streamparm *parm)
+{
+	struct video_i2c_data *data = video_drvdata(filp);
+	int i;
+
+	for (i = 0; i < data->chip->num_frame_intervals - 1; i++) {
+		if (V4L2_FRACT_COMPARE(parm->parm.capture.timeperframe, <=,
+				       data->chip->frame_intervals[i]))
+			break;
+	}
+	data->frame_interval = data->chip->frame_intervals[i];
+
+	return video_i2c_g_parm(filp, priv, parm);
+}
+
+static const struct v4l2_ioctl_ops video_i2c_ioctl_ops = {
+	.vidioc_querycap		= video_i2c_querycap,
+	.vidioc_g_input			= video_i2c_g_input,
+	.vidioc_s_input			= video_i2c_s_input,
+	.vidioc_enum_input		= video_i2c_enum_input,
+	.vidioc_enum_fmt_vid_cap	= video_i2c_enum_fmt_vid_cap,
+	.vidioc_enum_framesizes		= video_i2c_enum_framesizes,
+	.vidioc_enum_frameintervals	= video_i2c_enum_frameintervals,
+	.vidioc_g_fmt_vid_cap		= video_i2c_try_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap		= video_i2c_s_fmt_vid_cap,
+	.vidioc_g_parm			= video_i2c_g_parm,
+	.vidioc_s_parm			= video_i2c_s_parm,
+	.vidioc_try_fmt_vid_cap		= video_i2c_try_fmt_vid_cap,
+	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
+	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
+	.vidioc_prepare_buf		= vb2_ioctl_prepare_buf,
+	.vidioc_querybuf		= vb2_ioctl_querybuf,
+	.vidioc_qbuf			= vb2_ioctl_qbuf,
+	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
+	.vidioc_streamon		= vb2_ioctl_streamon,
+	.vidioc_streamoff		= vb2_ioctl_streamoff,
+};
+
+static void video_i2c_release(struct video_device *vdev)
+{
+	struct video_i2c_data *data = video_get_drvdata(vdev);
+
+	v4l2_device_unregister(&data->v4l2_dev);
+	mutex_destroy(&data->lock);
+	mutex_destroy(&data->queue_lock);
+	regmap_exit(data->regmap);
+	kfree(data);
+}
+
+static int video_i2c_probe(struct i2c_client *client)
+{
+	const struct i2c_device_id *id = i2c_client_get_device_id(client);
+	struct video_i2c_data *data;
+	struct v4l2_device *v4l2_dev;
+	struct vb2_queue *queue;
+	int ret = -ENODEV;
+
+	data = kzalloc(sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	if (dev_fwnode(&client->dev))
+		data->chip = device_get_match_data(&client->dev);
+	else if (id)
+		data->chip = &video_i2c_chip[id->driver_data];
+	else
+		goto error_free_device;
+
+	data->regmap = regmap_init_i2c(client, data->chip->regmap_config);
+	if (IS_ERR(data->regmap)) {
+		ret = PTR_ERR(data->regmap);
+		goto error_free_device;
+	}
+
+	v4l2_dev = &data->v4l2_dev;
+	strscpy(v4l2_dev->name, VIDEO_I2C_DRIVER, sizeof(v4l2_dev->name));
+
+	ret = v4l2_device_register(&client->dev, v4l2_dev);
+	if (ret < 0)
+		goto error_regmap_exit;
+
+	mutex_init(&data->lock);
+	mutex_init(&data->queue_lock);
+
+	queue = &data->vb_vidq;
+	queue->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	queue->io_modes = VB2_DMABUF | VB2_MMAP | VB2_USERPTR | VB2_READ;
+	queue->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	queue->drv_priv = data;
+	queue->buf_struct_size = sizeof(struct video_i2c_buffer);
+	queue->min_buffers_needed = 1;
+	queue->ops = &video_i2c_video_qops;
+	queue->mem_ops = &vb2_vmalloc_memops;
+
+	ret = vb2_queue_init(queue);
+	if (ret < 0)
+		goto error_unregister_device;
+
+	data->vdev.queue = queue;
+	data->vdev.queue->lock = &data->queue_lock;
+
+	snprintf(data->vdev.name, sizeof(data->vdev.name),
+				 "I2C %d-%d Transport Video",
+				 client->adapter->nr, client->addr);
+
+	data->vdev.v4l2_dev = v4l2_dev;
+	data->vdev.fops = &video_i2c_fops;
+	data->vdev.lock = &data->lock;
+	data->vdev.ioctl_ops = &video_i2c_ioctl_ops;
+	data->vdev.release = video_i2c_release;
+	data->vdev.device_caps = V4L2_CAP_VIDEO_CAPTURE |
+				 V4L2_CAP_READWRITE | V4L2_CAP_STREAMING;
+
+	spin_lock_init(&data->slock);
+	INIT_LIST_HEAD(&data->vid_cap_active);
+
+	data->frame_interval = data->chip->frame_intervals[0];
+
+	video_set_drvdata(&data->vdev, data);
+	i2c_set_clientdata(client, data);
+
+	if (data->chip->set_power) {
+		ret = data->chip->set_power(data, true);
+		if (ret)
+			goto error_unregister_device;
+	}
+
+	pm_runtime_get_noresume(&client->dev);
+	pm_runtime_set_active(&client->dev);
+	pm_runtime_enable(&client->dev);
+	pm_runtime_set_autosuspend_delay(&client->dev, 2000);
+	pm_runtime_use_autosuspend(&client->dev);
+
+	if (data->chip->hwmon_init) {
+		ret = data->chip->hwmon_init(data);
+		if (ret < 0) {
+			dev_warn(&client->dev,
+				 "failed to register hwmon device\n");
+		}
+	}
+
+	if (data->chip->nvmem_config) {
+		struct nvmem_config *config = data->chip->nvmem_config;
+		struct nvmem_device *device;
+
+		config->priv = data;
+		config->dev = &client->dev;
+
+		device = devm_nvmem_register(&client->dev, config);
+
+		if (IS_ERR(device)) {
+			dev_warn(&client->dev,
+				 "failed to register nvmem device\n");
+		}
+	}
+
+	ret = video_register_device(&data->vdev, VFL_TYPE_VIDEO, -1);
+	if (ret < 0)
+		goto error_pm_disable;
+
+	pm_runtime_mark_last_busy(&client->dev);
+	pm_runtime_put_autosuspend(&client->dev);
+
+	return 0;
+
+error_pm_disable:
+	pm_runtime_disable(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+	pm_runtime_put_noidle(&client->dev);
+
+	if (data->chip->set_power)
+		data->chip->set_power(data, false);
+
+error_unregister_device:
+	v4l2_device_unregister(v4l2_dev);
+	mutex_destroy(&data->lock);
+	mutex_destroy(&data->queue_lock);
+
+error_regmap_exit:
+	regmap_exit(data->regmap);
+
+error_free_device:
+	kfree(data);
+
+	return ret;
+}
+
+static void video_i2c_remove(struct i2c_client *client)
+{
+	struct video_i2c_data *data = i2c_get_clientdata(client);
+
+	pm_runtime_get_sync(&client->dev);
+	pm_runtime_disable(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+	pm_runtime_put_noidle(&client->dev);
+
+	if (data->chip->set_power)
+		data->chip->set_power(data, false);
+
+	video_unregister_device(&data->vdev);
+}
+
+#ifdef CONFIG_PM
+
+static int video_i2c_pm_runtime_suspend(struct device *dev)
+{
+	struct video_i2c_data *data = i2c_get_clientdata(to_i2c_client(dev));
+
+	if (!data->chip->set_power)
+		return 0;
+
+	return data->chip->set_power(data, false);
+}
+
+static int video_i2c_pm_runtime_resume(struct device *dev)
+{
+	struct video_i2c_data *data = i2c_get_clientdata(to_i2c_client(dev));
+
+	if (!data->chip->set_power)
+		return 0;
+
+	return data->chip->set_power(data, true);
+}
+
+#endif
+
+static const struct dev_pm_ops video_i2c_pm_ops = {
+	SET_RUNTIME_PM_OPS(video_i2c_pm_runtime_suspend,
+			   video_i2c_pm_runtime_resume, NULL)
+};
+
+static const struct i2c_device_id video_i2c_id_table[] = {
+	{ "amg88xx", AMG88XX },
+	{ "mlx90640", MLX90640 },
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, video_i2c_id_table);
+
+static const struct of_device_id video_i2c_of_match[] = {
+	{ .compatible = "panasonic,amg88xx", .data = &video_i2c_chip[AMG88XX] },
+	{ .compatible = "melexis,mlx90640", .data = &video_i2c_chip[MLX90640] },
+	{}
+};
+MODULE_DEVICE_TABLE(of, video_i2c_of_match);
+
+static struct i2c_driver video_i2c_driver = {
+	.driver = {
+		.name	= VIDEO_I2C_DRIVER,
+		.of_match_table = video_i2c_of_match,
+		.pm	= &video_i2c_pm_ops,
+	},
+	.probe		= video_i2c_probe,
+	.remove		= video_i2c_remove,
+	.id_table	= video_i2c_id_table,
+};
+
+module_i2c_driver(video_i2c_driver);
+
+MODULE_AUTHOR("Matt Ranostay <matt.ranostay@konsulko.com>");
+MODULE_DESCRIPTION("I2C transport video support");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/vp27smpx.c b/drivers/media/i2c/vp27smpx.c
new file mode 100644
index 000000000..0ba3c2b68
--- /dev/null
+++ b/drivers/media/i2c/vp27smpx.c
@@ -0,0 +1,189 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * vp27smpx - driver version 0.0.1
+ *
+ * Copyright (C) 2007 Hans Verkuil <hverkuil@xs4all.nl>
+ *
+ * Based on a tvaudio patch from Takahiro Adachi <tadachi@tadachi-net.com>
+ * and Kazuhiko Kawakami <kazz-0@mail.goo.ne.jp>
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <linux/ioctl.h>
+#include <linux/uaccess.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+
+MODULE_DESCRIPTION("vp27smpx driver");
+MODULE_AUTHOR("Hans Verkuil");
+MODULE_LICENSE("GPL");
+
+
+/* ----------------------------------------------------------------------- */
+
+struct vp27smpx_state {
+	struct v4l2_subdev sd;
+	int radio;
+	u32 audmode;
+};
+
+static inline struct vp27smpx_state *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct vp27smpx_state, sd);
+}
+
+static void vp27smpx_set_audmode(struct v4l2_subdev *sd, u32 audmode)
+{
+	struct vp27smpx_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u8 data[3] = { 0x00, 0x00, 0x04 };
+
+	switch (audmode) {
+	case V4L2_TUNER_MODE_MONO:
+	case V4L2_TUNER_MODE_LANG1:
+		break;
+	case V4L2_TUNER_MODE_STEREO:
+	case V4L2_TUNER_MODE_LANG1_LANG2:
+		data[1] = 0x01;
+		break;
+	case V4L2_TUNER_MODE_LANG2:
+		data[1] = 0x02;
+		break;
+	}
+
+	if (i2c_master_send(client, data, sizeof(data)) != sizeof(data))
+		v4l2_err(sd, "I/O error setting audmode\n");
+	else
+		state->audmode = audmode;
+}
+
+static int vp27smpx_s_radio(struct v4l2_subdev *sd)
+{
+	struct vp27smpx_state *state = to_state(sd);
+
+	state->radio = 1;
+	return 0;
+}
+
+static int vp27smpx_s_std(struct v4l2_subdev *sd, v4l2_std_id norm)
+{
+	struct vp27smpx_state *state = to_state(sd);
+
+	state->radio = 0;
+	return 0;
+}
+
+static int vp27smpx_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *vt)
+{
+	struct vp27smpx_state *state = to_state(sd);
+
+	if (!state->radio)
+		vp27smpx_set_audmode(sd, vt->audmode);
+	return 0;
+}
+
+static int vp27smpx_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
+{
+	struct vp27smpx_state *state = to_state(sd);
+
+	if (state->radio)
+		return 0;
+	vt->audmode = state->audmode;
+	vt->capability = V4L2_TUNER_CAP_STEREO |
+		V4L2_TUNER_CAP_LANG1 | V4L2_TUNER_CAP_LANG2;
+	vt->rxsubchans = V4L2_TUNER_SUB_MONO;
+	return 0;
+}
+
+static int vp27smpx_log_status(struct v4l2_subdev *sd)
+{
+	struct vp27smpx_state *state = to_state(sd);
+
+	v4l2_info(sd, "Audio Mode: %u%s\n", state->audmode,
+			state->radio ? " (Radio)" : "");
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_core_ops vp27smpx_core_ops = {
+	.log_status = vp27smpx_log_status,
+};
+
+static const struct v4l2_subdev_tuner_ops vp27smpx_tuner_ops = {
+	.s_radio = vp27smpx_s_radio,
+	.s_tuner = vp27smpx_s_tuner,
+	.g_tuner = vp27smpx_g_tuner,
+};
+
+static const struct v4l2_subdev_video_ops vp27smpx_video_ops = {
+	.s_std = vp27smpx_s_std,
+};
+
+static const struct v4l2_subdev_ops vp27smpx_ops = {
+	.core = &vp27smpx_core_ops,
+	.tuner = &vp27smpx_tuner_ops,
+	.video = &vp27smpx_video_ops,
+};
+
+/* ----------------------------------------------------------------------- */
+
+/* i2c implementation */
+
+/*
+ * Generic i2c probe
+ * concerning the addresses: i2c wants 7 bit (without the r/w bit), so '>>1'
+ */
+
+static int vp27smpx_probe(struct i2c_client *client)
+{
+	struct vp27smpx_state *state;
+	struct v4l2_subdev *sd;
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	state = devm_kzalloc(&client->dev, sizeof(*state), GFP_KERNEL);
+	if (state == NULL)
+		return -ENOMEM;
+	sd = &state->sd;
+	v4l2_i2c_subdev_init(sd, client, &vp27smpx_ops);
+	state->audmode = V4L2_TUNER_MODE_STEREO;
+
+	/* initialize vp27smpx */
+	vp27smpx_set_audmode(sd, state->audmode);
+	return 0;
+}
+
+static void vp27smpx_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id vp27smpx_id[] = {
+	{ "vp27smpx", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, vp27smpx_id);
+
+static struct i2c_driver vp27smpx_driver = {
+	.driver = {
+		.name	= "vp27smpx",
+	},
+	.probe		= vp27smpx_probe,
+	.remove		= vp27smpx_remove,
+	.id_table	= vp27smpx_id,
+};
+
+module_i2c_driver(vp27smpx_driver);
diff --git a/drivers/media/i2c/vpx3220.c b/drivers/media/i2c/vpx3220.c
new file mode 100644
index 000000000..1eaae886f
--- /dev/null
+++ b/drivers/media/i2c/vpx3220.c
@@ -0,0 +1,554 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * vpx3220a, vpx3216b & vpx3214c video decoder driver version 0.0.1
+ *
+ * Copyright (C) 2001 Laurent Pinchart <lpinchart@freegates.be>
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+
+MODULE_DESCRIPTION("vpx3220a/vpx3216b/vpx3214c video decoder driver");
+MODULE_AUTHOR("Laurent Pinchart");
+MODULE_LICENSE("GPL");
+
+static int debug;
+module_param(debug, int, 0);
+MODULE_PARM_DESC(debug, "Debug level (0-1)");
+
+
+#define VPX_TIMEOUT_COUNT  10
+
+/* ----------------------------------------------------------------------- */
+
+struct vpx3220 {
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
+	unsigned char reg[255];
+
+	v4l2_std_id norm;
+	int input;
+	int enable;
+};
+
+static inline struct vpx3220 *to_vpx3220(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct vpx3220, sd);
+}
+
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct vpx3220, hdl)->sd;
+}
+
+static char *inputs[] = { "internal", "composite", "svideo" };
+
+/* ----------------------------------------------------------------------- */
+
+static inline int vpx3220_write(struct v4l2_subdev *sd, u8 reg, u8 value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct vpx3220 *decoder = i2c_get_clientdata(client);
+
+	decoder->reg[reg] = value;
+	return i2c_smbus_write_byte_data(client, reg, value);
+}
+
+static inline int vpx3220_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return i2c_smbus_read_byte_data(client, reg);
+}
+
+static int vpx3220_fp_status(struct v4l2_subdev *sd)
+{
+	unsigned char status;
+	unsigned int i;
+
+	for (i = 0; i < VPX_TIMEOUT_COUNT; i++) {
+		status = vpx3220_read(sd, 0x29);
+
+		if (!(status & 4))
+			return 0;
+
+		udelay(10);
+
+		if (need_resched())
+			cond_resched();
+	}
+
+	return -1;
+}
+
+static int vpx3220_fp_write(struct v4l2_subdev *sd, u8 fpaddr, u16 data)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	/* Write the 16-bit address to the FPWR register */
+	if (i2c_smbus_write_word_data(client, 0x27, swab16(fpaddr)) == -1) {
+		v4l2_dbg(1, debug, sd, "%s: failed\n", __func__);
+		return -1;
+	}
+
+	if (vpx3220_fp_status(sd) < 0)
+		return -1;
+
+	/* Write the 16-bit data to the FPDAT register */
+	if (i2c_smbus_write_word_data(client, 0x28, swab16(data)) == -1) {
+		v4l2_dbg(1, debug, sd, "%s: failed\n", __func__);
+		return -1;
+	}
+
+	return 0;
+}
+
+static int vpx3220_fp_read(struct v4l2_subdev *sd, u16 fpaddr)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	s16 data;
+
+	/* Write the 16-bit address to the FPRD register */
+	if (i2c_smbus_write_word_data(client, 0x26, swab16(fpaddr)) == -1) {
+		v4l2_dbg(1, debug, sd, "%s: failed\n", __func__);
+		return -1;
+	}
+
+	if (vpx3220_fp_status(sd) < 0)
+		return -1;
+
+	/* Read the 16-bit data from the FPDAT register */
+	data = i2c_smbus_read_word_data(client, 0x28);
+	if (data == -1) {
+		v4l2_dbg(1, debug, sd, "%s: failed\n", __func__);
+		return -1;
+	}
+
+	return swab16(data);
+}
+
+static int vpx3220_write_block(struct v4l2_subdev *sd, const u8 *data, unsigned int len)
+{
+	u8 reg;
+	int ret = -1;
+
+	while (len >= 2) {
+		reg = *data++;
+		ret = vpx3220_write(sd, reg, *data++);
+		if (ret < 0)
+			break;
+		len -= 2;
+	}
+
+	return ret;
+}
+
+static int vpx3220_write_fp_block(struct v4l2_subdev *sd,
+		const u16 *data, unsigned int len)
+{
+	u8 reg;
+	int ret = 0;
+
+	while (len > 1) {
+		reg = *data++;
+		ret |= vpx3220_fp_write(sd, reg, *data++);
+		len -= 2;
+	}
+
+	return ret;
+}
+
+/* ---------------------------------------------------------------------- */
+
+static const unsigned short init_ntsc[] = {
+	0x1c, 0x00,		/* NTSC tint angle */
+	0x88, 17,		/* Window 1 vertical */
+	0x89, 240,		/* Vertical lines in */
+	0x8a, 240,		/* Vertical lines out */
+	0x8b, 000,		/* Horizontal begin */
+	0x8c, 640,		/* Horizontal length */
+	0x8d, 640,		/* Number of pixels */
+	0x8f, 0xc00,		/* Disable window 2 */
+	0xf0, 0x73,		/* 13.5 MHz transport, Forced
+				 * mode, latch windows */
+	0xf2, 0x13,		/* NTSC M, composite input */
+	0xe7, 0x1e1,		/* Enable vertical standard
+				 * locking @ 240 lines */
+};
+
+static const unsigned short init_pal[] = {
+	0x88, 23,		/* Window 1 vertical begin */
+	0x89, 288,		/* Vertical lines in (16 lines
+				 * skipped by the VFE) */
+	0x8a, 288,		/* Vertical lines out (16 lines
+				 * skipped by the VFE) */
+	0x8b, 16,		/* Horizontal begin */
+	0x8c, 768,		/* Horizontal length */
+	0x8d, 784,		/* Number of pixels
+				 * Must be >= Horizontal begin + Horizontal length */
+	0x8f, 0xc00,		/* Disable window 2 */
+	0xf0, 0x77,		/* 13.5 MHz transport, Forced
+				 * mode, latch windows */
+	0xf2, 0x3d1,		/* PAL B,G,H,I, composite input */
+	0xe7, 0x241,		/* PAL/SECAM set to 288 lines */
+};
+
+static const unsigned short init_secam[] = {
+	0x88, 23,		/* Window 1 vertical begin */
+	0x89, 288,		/* Vertical lines in (16 lines
+				 * skipped by the VFE) */
+	0x8a, 288,		/* Vertical lines out (16 lines
+				 * skipped by the VFE) */
+	0x8b, 16,		/* Horizontal begin */
+	0x8c, 768,		/* Horizontal length */
+	0x8d, 784,		/* Number of pixels
+				 * Must be >= Horizontal begin + Horizontal length */
+	0x8f, 0xc00,		/* Disable window 2 */
+	0xf0, 0x77,		/* 13.5 MHz transport, Forced
+				 * mode, latch windows */
+	0xf2, 0x3d5,		/* SECAM, composite input */
+	0xe7, 0x241,		/* PAL/SECAM set to 288 lines */
+};
+
+static const unsigned char init_common[] = {
+	0xf2, 0x00,		/* Disable all outputs */
+	0x33, 0x0d,		/* Luma : VIN2, Chroma : CIN
+				 * (clamp off) */
+	0xd8, 0xa8,		/* HREF/VREF active high, VREF
+				 * pulse = 2, Odd/Even flag */
+	0x20, 0x03,		/* IF compensation 0dB/oct */
+	0xe0, 0xff,		/* Open up all comparators */
+	0xe1, 0x00,
+	0xe2, 0x7f,
+	0xe3, 0x80,
+	0xe4, 0x7f,
+	0xe5, 0x80,
+	0xe6, 0x00,		/* Brightness set to 0 */
+	0xe7, 0xe0,		/* Contrast to 1.0, noise shaping
+				 * 10 to 8 2-bit error diffusion */
+	0xe8, 0xf8,		/* YUV422, CbCr binary offset,
+				 * ... (p.32) */
+	0xea, 0x18,		/* LLC2 connected, output FIFO
+				 * reset with VACTintern */
+	0xf0, 0x8a,		/* Half full level to 10, bus
+				 * shuffler [7:0, 23:16, 15:8] */
+	0xf1, 0x18,		/* Single clock, sync mode, no
+				 * FE delay, no HLEN counter */
+	0xf8, 0x12,		/* Port A, PIXCLK, HF# & FE#
+				 * strength to 2 */
+	0xf9, 0x24,		/* Port B, HREF, VREF, PREF &
+				 * ALPHA strength to 4 */
+};
+
+static const unsigned short init_fp[] = {
+	0x59, 0,
+	0xa0, 2070,		/* ACC reference */
+	0xa3, 0,
+	0xa4, 0,
+	0xa8, 30,
+	0xb2, 768,
+	0xbe, 27,
+	0x58, 0,
+	0x26, 0,
+	0x4b, 0x298,		/* PLL gain */
+};
+
+
+static int vpx3220_init(struct v4l2_subdev *sd, u32 val)
+{
+	struct vpx3220 *decoder = to_vpx3220(sd);
+
+	vpx3220_write_block(sd, init_common, sizeof(init_common));
+	vpx3220_write_fp_block(sd, init_fp, sizeof(init_fp) >> 1);
+	if (decoder->norm & V4L2_STD_NTSC)
+		vpx3220_write_fp_block(sd, init_ntsc, sizeof(init_ntsc) >> 1);
+	else if (decoder->norm & V4L2_STD_PAL)
+		vpx3220_write_fp_block(sd, init_pal, sizeof(init_pal) >> 1);
+	else if (decoder->norm & V4L2_STD_SECAM)
+		vpx3220_write_fp_block(sd, init_secam, sizeof(init_secam) >> 1);
+	else
+		vpx3220_write_fp_block(sd, init_pal, sizeof(init_pal) >> 1);
+	return 0;
+}
+
+static int vpx3220_status(struct v4l2_subdev *sd, u32 *pstatus, v4l2_std_id *pstd)
+{
+	int res = V4L2_IN_ST_NO_SIGNAL, status;
+	v4l2_std_id std = pstd ? *pstd : V4L2_STD_ALL;
+
+	status = vpx3220_fp_read(sd, 0x0f3);
+
+	v4l2_dbg(1, debug, sd, "status: 0x%04x\n", status);
+
+	if (status < 0)
+		return status;
+
+	if ((status & 0x20) == 0) {
+		res = 0;
+
+		switch (status & 0x18) {
+		case 0x00:
+		case 0x10:
+		case 0x14:
+		case 0x18:
+			std &= V4L2_STD_PAL;
+			break;
+
+		case 0x08:
+			std &= V4L2_STD_SECAM;
+			break;
+
+		case 0x04:
+		case 0x0c:
+		case 0x1c:
+			std &= V4L2_STD_NTSC;
+			break;
+		}
+	} else {
+		std = V4L2_STD_UNKNOWN;
+	}
+	if (pstd)
+		*pstd = std;
+	if (pstatus)
+		*pstatus = res;
+	return 0;
+}
+
+static int vpx3220_querystd(struct v4l2_subdev *sd, v4l2_std_id *std)
+{
+	v4l2_dbg(1, debug, sd, "querystd\n");
+	return vpx3220_status(sd, NULL, std);
+}
+
+static int vpx3220_g_input_status(struct v4l2_subdev *sd, u32 *status)
+{
+	v4l2_dbg(1, debug, sd, "g_input_status\n");
+	return vpx3220_status(sd, status, NULL);
+}
+
+static int vpx3220_s_std(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct vpx3220 *decoder = to_vpx3220(sd);
+	int temp_input;
+
+	/* Here we back up the input selection because it gets
+	   overwritten when we fill the registers with the
+	   chosen video norm */
+	temp_input = vpx3220_fp_read(sd, 0xf2);
+
+	v4l2_dbg(1, debug, sd, "s_std %llx\n", (unsigned long long)std);
+	if (std & V4L2_STD_NTSC) {
+		vpx3220_write_fp_block(sd, init_ntsc, sizeof(init_ntsc) >> 1);
+		v4l2_dbg(1, debug, sd, "norm switched to NTSC\n");
+	} else if (std & V4L2_STD_PAL) {
+		vpx3220_write_fp_block(sd, init_pal, sizeof(init_pal) >> 1);
+		v4l2_dbg(1, debug, sd, "norm switched to PAL\n");
+	} else if (std & V4L2_STD_SECAM) {
+		vpx3220_write_fp_block(sd, init_secam, sizeof(init_secam) >> 1);
+		v4l2_dbg(1, debug, sd, "norm switched to SECAM\n");
+	} else {
+		return -EINVAL;
+	}
+
+	decoder->norm = std;
+
+	/* And here we set the backed up video input again */
+	vpx3220_fp_write(sd, 0xf2, temp_input | 0x0010);
+	udelay(10);
+	return 0;
+}
+
+static int vpx3220_s_routing(struct v4l2_subdev *sd,
+			     u32 input, u32 output, u32 config)
+{
+	int data;
+
+	/* RJ:   input = 0: ST8 (PCTV) input
+		 input = 1: COMPOSITE  input
+		 input = 2: SVHS       input  */
+
+	static const int input_vals[3][2] = {
+		{0x0c, 0},
+		{0x0d, 0},
+		{0x0e, 1}
+	};
+
+	if (input > 2)
+		return -EINVAL;
+
+	v4l2_dbg(1, debug, sd, "input switched to %s\n", inputs[input]);
+
+	vpx3220_write(sd, 0x33, input_vals[input][0]);
+
+	data = vpx3220_fp_read(sd, 0xf2) & ~(0x0020);
+	if (data < 0)
+		return data;
+	/* 0x0010 is required to latch the setting */
+	vpx3220_fp_write(sd, 0xf2,
+			data | (input_vals[input][1] << 5) | 0x0010);
+
+	udelay(10);
+	return 0;
+}
+
+static int vpx3220_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	v4l2_dbg(1, debug, sd, "s_stream %s\n", enable ? "on" : "off");
+
+	vpx3220_write(sd, 0xf2, (enable ? 0x1b : 0x00));
+	return 0;
+}
+
+static int vpx3220_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		vpx3220_write(sd, 0xe6, ctrl->val);
+		return 0;
+	case V4L2_CID_CONTRAST:
+		/* Bit 7 and 8 is for noise shaping */
+		vpx3220_write(sd, 0xe7, ctrl->val + 192);
+		return 0;
+	case V4L2_CID_SATURATION:
+		vpx3220_fp_write(sd, 0xa0, ctrl->val);
+		return 0;
+	case V4L2_CID_HUE:
+		vpx3220_fp_write(sd, 0x1c, ctrl->val);
+		return 0;
+	}
+	return -EINVAL;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_ctrl_ops vpx3220_ctrl_ops = {
+	.s_ctrl = vpx3220_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops vpx3220_core_ops = {
+	.init = vpx3220_init,
+};
+
+static const struct v4l2_subdev_video_ops vpx3220_video_ops = {
+	.s_std = vpx3220_s_std,
+	.s_routing = vpx3220_s_routing,
+	.s_stream = vpx3220_s_stream,
+	.querystd = vpx3220_querystd,
+	.g_input_status = vpx3220_g_input_status,
+};
+
+static const struct v4l2_subdev_ops vpx3220_ops = {
+	.core = &vpx3220_core_ops,
+	.video = &vpx3220_video_ops,
+};
+
+/* -----------------------------------------------------------------------
+ * Client management code
+ */
+
+static int vpx3220_probe(struct i2c_client *client)
+{
+	struct vpx3220 *decoder;
+	struct v4l2_subdev *sd;
+	const char *name = NULL;
+	u8 ver;
+	u16 pn;
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter,
+		I2C_FUNC_SMBUS_BYTE_DATA | I2C_FUNC_SMBUS_WORD_DATA))
+		return -ENODEV;
+
+	decoder = devm_kzalloc(&client->dev, sizeof(*decoder), GFP_KERNEL);
+	if (decoder == NULL)
+		return -ENOMEM;
+	sd = &decoder->sd;
+	v4l2_i2c_subdev_init(sd, client, &vpx3220_ops);
+	decoder->norm = V4L2_STD_PAL;
+	decoder->input = 0;
+	decoder->enable = 1;
+	v4l2_ctrl_handler_init(&decoder->hdl, 4);
+	v4l2_ctrl_new_std(&decoder->hdl, &vpx3220_ctrl_ops,
+		V4L2_CID_BRIGHTNESS, -128, 127, 1, 0);
+	v4l2_ctrl_new_std(&decoder->hdl, &vpx3220_ctrl_ops,
+		V4L2_CID_CONTRAST, 0, 63, 1, 32);
+	v4l2_ctrl_new_std(&decoder->hdl, &vpx3220_ctrl_ops,
+		V4L2_CID_SATURATION, 0, 4095, 1, 2048);
+	v4l2_ctrl_new_std(&decoder->hdl, &vpx3220_ctrl_ops,
+		V4L2_CID_HUE, -512, 511, 1, 0);
+	sd->ctrl_handler = &decoder->hdl;
+	if (decoder->hdl.error) {
+		int err = decoder->hdl.error;
+
+		v4l2_ctrl_handler_free(&decoder->hdl);
+		return err;
+	}
+	v4l2_ctrl_handler_setup(&decoder->hdl);
+
+	ver = i2c_smbus_read_byte_data(client, 0x00);
+	pn = (i2c_smbus_read_byte_data(client, 0x02) << 8) +
+		i2c_smbus_read_byte_data(client, 0x01);
+	if (ver == 0xec) {
+		switch (pn) {
+		case 0x4680:
+			name = "vpx3220a";
+			break;
+		case 0x4260:
+			name = "vpx3216b";
+			break;
+		case 0x4280:
+			name = "vpx3214c";
+			break;
+		}
+	}
+	if (name)
+		v4l2_info(sd, "%s found @ 0x%x (%s)\n", name,
+			client->addr << 1, client->adapter->name);
+	else
+		v4l2_info(sd, "chip (%02x:%04x) found @ 0x%x (%s)\n",
+			ver, pn, client->addr << 1, client->adapter->name);
+
+	vpx3220_write_block(sd, init_common, sizeof(init_common));
+	vpx3220_write_fp_block(sd, init_fp, sizeof(init_fp) >> 1);
+	/* Default to PAL */
+	vpx3220_write_fp_block(sd, init_pal, sizeof(init_pal) >> 1);
+	return 0;
+}
+
+static void vpx3220_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct vpx3220 *decoder = to_vpx3220(sd);
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&decoder->hdl);
+}
+
+static const struct i2c_device_id vpx3220_id[] = {
+	{ "vpx3220a", 0 },
+	{ "vpx3216b", 0 },
+	{ "vpx3214c", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, vpx3220_id);
+
+static struct i2c_driver vpx3220_driver = {
+	.driver = {
+		.name	= "vpx3220",
+	},
+	.probe		= vpx3220_probe,
+	.remove		= vpx3220_remove,
+	.id_table	= vpx3220_id,
+};
+
+module_i2c_driver(vpx3220_driver);
diff --git a/drivers/media/i2c/wm8739.c b/drivers/media/i2c/wm8739.c
new file mode 100644
index 000000000..19bf7a00d
--- /dev/null
+++ b/drivers/media/i2c/wm8739.c
@@ -0,0 +1,260 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * wm8739
+ *
+ * Copyright (C) 2005 T. Adachi <tadachi@tadachi-net.com>
+ *
+ * Copyright (C) 2005 Hans Verkuil <hverkuil@xs4all.nl>
+ * - Cleanup
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <linux/ioctl.h>
+#include <linux/uaccess.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+
+MODULE_DESCRIPTION("wm8739 driver");
+MODULE_AUTHOR("T. Adachi, Hans Verkuil");
+MODULE_LICENSE("GPL");
+
+static int debug;
+
+module_param(debug, int, 0644);
+
+MODULE_PARM_DESC(debug, "Debug level (0-1)");
+
+
+/* ------------------------------------------------------------------------ */
+
+enum {
+	R0 = 0, R1,
+	R5 = 5, R6, R7, R8, R9, R15 = 15,
+	TOT_REGS
+};
+
+struct wm8739_state {
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
+	struct {
+		/* audio cluster */
+		struct v4l2_ctrl *volume;
+		struct v4l2_ctrl *mute;
+		struct v4l2_ctrl *balance;
+	};
+	u32 clock_freq;
+};
+
+static inline struct wm8739_state *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct wm8739_state, sd);
+}
+
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct wm8739_state, hdl)->sd;
+}
+
+/* ------------------------------------------------------------------------ */
+
+static int wm8739_write(struct v4l2_subdev *sd, int reg, u16 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int i;
+
+	if (reg < 0 || reg >= TOT_REGS) {
+		v4l2_err(sd, "Invalid register R%d\n", reg);
+		return -1;
+	}
+
+	v4l2_dbg(1, debug, sd, "write: %02x %02x\n", reg, val);
+
+	for (i = 0; i < 3; i++)
+		if (i2c_smbus_write_byte_data(client,
+				(reg << 1) | (val >> 8), val & 0xff) == 0)
+			return 0;
+	v4l2_err(sd, "I2C: cannot write %03x to register R%d\n", val, reg);
+	return -1;
+}
+
+static int wm8739_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+	struct wm8739_state *state = to_state(sd);
+	unsigned int work_l, work_r;
+	u8 vol_l;	/* +12dB to -34.5dB 1.5dB step (5bit) def:0dB */
+	u8 vol_r;	/* +12dB to -34.5dB 1.5dB step (5bit) def:0dB */
+	u16 mute;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUDIO_VOLUME:
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	/* normalize ( 65535 to 0 -> 31 to 0 (12dB to -34.5dB) ) */
+	work_l = (min(65536 - state->balance->val, 32768) * state->volume->val) / 32768;
+	work_r = (min(state->balance->val, 32768) * state->volume->val) / 32768;
+
+	vol_l = (long)work_l * 31 / 65535;
+	vol_r = (long)work_r * 31 / 65535;
+
+	/* set audio volume etc. */
+	mute = state->mute->val ? 0x80 : 0;
+
+	/* Volume setting: bits 0-4, 0x1f = 12 dB, 0x00 = -34.5 dB
+	 * Default setting: 0x17 = 0 dB
+	 */
+	wm8739_write(sd, R0, (vol_l & 0x1f) | mute);
+	wm8739_write(sd, R1, (vol_r & 0x1f) | mute);
+	return 0;
+}
+
+/* ------------------------------------------------------------------------ */
+
+static int wm8739_s_clock_freq(struct v4l2_subdev *sd, u32 audiofreq)
+{
+	struct wm8739_state *state = to_state(sd);
+
+	state->clock_freq = audiofreq;
+	/* de-activate */
+	wm8739_write(sd, R9, 0x000);
+	switch (audiofreq) {
+	case 44100:
+		/* 256fps, fs=44.1k */
+		wm8739_write(sd, R8, 0x020);
+		break;
+	case 48000:
+		/* 256fps, fs=48k */
+		wm8739_write(sd, R8, 0x000);
+		break;
+	case 32000:
+		/* 256fps, fs=32k */
+		wm8739_write(sd, R8, 0x018);
+		break;
+	default:
+		break;
+	}
+	/* activate */
+	wm8739_write(sd, R9, 0x001);
+	return 0;
+}
+
+static int wm8739_log_status(struct v4l2_subdev *sd)
+{
+	struct wm8739_state *state = to_state(sd);
+
+	v4l2_info(sd, "Frequency: %u Hz\n", state->clock_freq);
+	v4l2_ctrl_handler_log_status(&state->hdl, sd->name);
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_ctrl_ops wm8739_ctrl_ops = {
+	.s_ctrl = wm8739_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops wm8739_core_ops = {
+	.log_status = wm8739_log_status,
+};
+
+static const struct v4l2_subdev_audio_ops wm8739_audio_ops = {
+	.s_clock_freq = wm8739_s_clock_freq,
+};
+
+static const struct v4l2_subdev_ops wm8739_ops = {
+	.core = &wm8739_core_ops,
+	.audio = &wm8739_audio_ops,
+};
+
+/* ------------------------------------------------------------------------ */
+
+/* i2c implementation */
+
+static int wm8739_probe(struct i2c_client *client)
+{
+	struct wm8739_state *state;
+	struct v4l2_subdev *sd;
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	state = devm_kzalloc(&client->dev, sizeof(*state), GFP_KERNEL);
+	if (state == NULL)
+		return -ENOMEM;
+	sd = &state->sd;
+	v4l2_i2c_subdev_init(sd, client, &wm8739_ops);
+	v4l2_ctrl_handler_init(&state->hdl, 2);
+	state->volume = v4l2_ctrl_new_std(&state->hdl, &wm8739_ctrl_ops,
+			V4L2_CID_AUDIO_VOLUME, 0, 65535, 65535 / 100, 50736);
+	state->mute = v4l2_ctrl_new_std(&state->hdl, &wm8739_ctrl_ops,
+			V4L2_CID_AUDIO_MUTE, 0, 1, 1, 0);
+	state->balance = v4l2_ctrl_new_std(&state->hdl, &wm8739_ctrl_ops,
+			V4L2_CID_AUDIO_BALANCE, 0, 65535, 65535 / 100, 32768);
+	sd->ctrl_handler = &state->hdl;
+	if (state->hdl.error) {
+		int err = state->hdl.error;
+
+		v4l2_ctrl_handler_free(&state->hdl);
+		return err;
+	}
+	v4l2_ctrl_cluster(3, &state->volume);
+
+	state->clock_freq = 48000;
+
+	/* Initialize wm8739 */
+
+	/* reset */
+	wm8739_write(sd, R15, 0x00);
+	/* filter setting, high path, offet clear */
+	wm8739_write(sd, R5, 0x000);
+	/* ADC, OSC, Power Off mode Disable */
+	wm8739_write(sd, R6, 0x000);
+	/* Digital Audio interface format:
+	   Enable Master mode, 24 bit, MSB first/left justified */
+	wm8739_write(sd, R7, 0x049);
+	/* sampling control: normal, 256fs, 48KHz sampling rate */
+	wm8739_write(sd, R8, 0x000);
+	/* activate */
+	wm8739_write(sd, R9, 0x001);
+	/* set volume/mute */
+	v4l2_ctrl_handler_setup(&state->hdl);
+	return 0;
+}
+
+static void wm8739_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct wm8739_state *state = to_state(sd);
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&state->hdl);
+}
+
+static const struct i2c_device_id wm8739_id[] = {
+	{ "wm8739", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, wm8739_id);
+
+static struct i2c_driver wm8739_driver = {
+	.driver = {
+		.name	= "wm8739",
+	},
+	.probe		= wm8739_probe,
+	.remove		= wm8739_remove,
+	.id_table	= wm8739_id,
+};
+
+module_i2c_driver(wm8739_driver);
diff --git a/drivers/media/i2c/wm8775.c b/drivers/media/i2c/wm8775.c
new file mode 100644
index 000000000..d1b716fd6
--- /dev/null
+++ b/drivers/media/i2c/wm8775.c
@@ -0,0 +1,306 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * wm8775 - driver version 0.0.1
+ *
+ * Copyright (C) 2004 Ulf Eklund <ivtv at eklund.to>
+ *
+ * Based on saa7115 driver
+ *
+ * Copyright (C) 2005 Hans Verkuil <hverkuil@xs4all.nl>
+ * - Cleanup
+ * - V4L2 API update
+ * - sound fixes
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <linux/ioctl.h>
+#include <linux/uaccess.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <media/i2c/wm8775.h>
+
+MODULE_DESCRIPTION("wm8775 driver");
+MODULE_AUTHOR("Ulf Eklund, Hans Verkuil");
+MODULE_LICENSE("GPL");
+
+
+
+/* ----------------------------------------------------------------------- */
+
+enum {
+	R7 = 7, R11 = 11,
+	R12, R13, R14, R15, R16, R17, R18, R19, R20, R21, R23 = 23,
+	TOT_REGS
+};
+
+#define ALC_HOLD 0x85 /* R17: use zero cross detection, ALC hold time 42.6 ms */
+#define ALC_EN 0x100  /* R17: ALC enable */
+
+struct wm8775_state {
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
+	struct v4l2_ctrl *mute;
+	struct v4l2_ctrl *vol;
+	struct v4l2_ctrl *bal;
+	struct v4l2_ctrl *loud;
+	u8 input;		/* Last selected input (0-0xf) */
+};
+
+static inline struct wm8775_state *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct wm8775_state, sd);
+}
+
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct wm8775_state, hdl)->sd;
+}
+
+static int wm8775_write(struct v4l2_subdev *sd, int reg, u16 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int i;
+
+	if (reg < 0 || reg >= TOT_REGS) {
+		v4l2_err(sd, "Invalid register R%d\n", reg);
+		return -1;
+	}
+
+	for (i = 0; i < 3; i++)
+		if (i2c_smbus_write_byte_data(client,
+				(reg << 1) | (val >> 8), val & 0xff) == 0)
+			return 0;
+	v4l2_err(sd, "I2C: cannot write %03x to register R%d\n", val, reg);
+	return -1;
+}
+
+static void wm8775_set_audio(struct v4l2_subdev *sd, int quietly)
+{
+	struct wm8775_state *state = to_state(sd);
+	u8 vol_l, vol_r;
+	int muted = 0 != state->mute->val;
+	u16 volume = (u16)state->vol->val;
+	u16 balance = (u16)state->bal->val;
+
+	/* normalize ( 65535 to 0 -> 255 to 0 (+24dB to -103dB) ) */
+	vol_l = (min(65536 - balance, 32768) * volume) >> 23;
+	vol_r = (min(balance, (u16)32768) * volume) >> 23;
+
+	/* Mute */
+	if (muted || quietly)
+		wm8775_write(sd, R21, 0x0c0 | state->input);
+
+	wm8775_write(sd, R14, vol_l | 0x100); /* 0x100= Left channel ADC zero cross enable */
+	wm8775_write(sd, R15, vol_r | 0x100); /* 0x100= Right channel ADC zero cross enable */
+
+	/* Un-mute */
+	if (!muted)
+		wm8775_write(sd, R21, state->input);
+}
+
+static int wm8775_s_routing(struct v4l2_subdev *sd,
+			    u32 input, u32 output, u32 config)
+{
+	struct wm8775_state *state = to_state(sd);
+
+	/* There are 4 inputs and one output. Zero or more inputs
+	   are multiplexed together to the output. Hence there are
+	   16 combinations.
+	   If only one input is active (the normal case) then the
+	   input values 1, 2, 4 or 8 should be used. */
+	if (input > 15) {
+		v4l2_err(sd, "Invalid input %d.\n", input);
+		return -EINVAL;
+	}
+	state->input = input;
+	if (v4l2_ctrl_g_ctrl(state->mute))
+		return 0;
+	if (!v4l2_ctrl_g_ctrl(state->vol))
+		return 0;
+	wm8775_set_audio(sd, 1);
+	return 0;
+}
+
+static int wm8775_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUDIO_MUTE:
+	case V4L2_CID_AUDIO_VOLUME:
+	case V4L2_CID_AUDIO_BALANCE:
+		wm8775_set_audio(sd, 0);
+		return 0;
+	case V4L2_CID_AUDIO_LOUDNESS:
+		wm8775_write(sd, R17, (ctrl->val ? ALC_EN : 0) | ALC_HOLD);
+		return 0;
+	}
+	return -EINVAL;
+}
+
+static int wm8775_log_status(struct v4l2_subdev *sd)
+{
+	struct wm8775_state *state = to_state(sd);
+
+	v4l2_info(sd, "Input: %d\n", state->input);
+	v4l2_ctrl_handler_log_status(&state->hdl, sd->name);
+	return 0;
+}
+
+static int wm8775_s_frequency(struct v4l2_subdev *sd, const struct v4l2_frequency *freq)
+{
+	wm8775_set_audio(sd, 0);
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_ctrl_ops wm8775_ctrl_ops = {
+	.s_ctrl = wm8775_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops wm8775_core_ops = {
+	.log_status = wm8775_log_status,
+};
+
+static const struct v4l2_subdev_tuner_ops wm8775_tuner_ops = {
+	.s_frequency = wm8775_s_frequency,
+};
+
+static const struct v4l2_subdev_audio_ops wm8775_audio_ops = {
+	.s_routing = wm8775_s_routing,
+};
+
+static const struct v4l2_subdev_ops wm8775_ops = {
+	.core = &wm8775_core_ops,
+	.tuner = &wm8775_tuner_ops,
+	.audio = &wm8775_audio_ops,
+};
+
+/* ----------------------------------------------------------------------- */
+
+/* i2c implementation */
+
+/*
+ * Generic i2c probe
+ * concerning the addresses: i2c wants 7 bit (without the r/w bit), so '>>1'
+ */
+
+static int wm8775_probe(struct i2c_client *client)
+{
+	struct wm8775_state *state;
+	struct v4l2_subdev *sd;
+	int err;
+	bool is_nova_s = false;
+
+	if (client->dev.platform_data) {
+		struct wm8775_platform_data *data = client->dev.platform_data;
+		is_nova_s = data->is_nova_s;
+	}
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+
+	v4l_info(client, "chip found @ 0x%02x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	state = devm_kzalloc(&client->dev, sizeof(*state), GFP_KERNEL);
+	if (state == NULL)
+		return -ENOMEM;
+	sd = &state->sd;
+	v4l2_i2c_subdev_init(sd, client, &wm8775_ops);
+	state->input = 2;
+
+	v4l2_ctrl_handler_init(&state->hdl, 4);
+	state->mute = v4l2_ctrl_new_std(&state->hdl, &wm8775_ctrl_ops,
+			V4L2_CID_AUDIO_MUTE, 0, 1, 1, 0);
+	state->vol = v4l2_ctrl_new_std(&state->hdl, &wm8775_ctrl_ops,
+			V4L2_CID_AUDIO_VOLUME, 0, 65535, (65535+99)/100, 0xCF00); /* 0dB*/
+	state->bal = v4l2_ctrl_new_std(&state->hdl, &wm8775_ctrl_ops,
+			V4L2_CID_AUDIO_BALANCE, 0, 65535, (65535+99)/100, 32768);
+	state->loud = v4l2_ctrl_new_std(&state->hdl, &wm8775_ctrl_ops,
+			V4L2_CID_AUDIO_LOUDNESS, 0, 1, 1, 1);
+	sd->ctrl_handler = &state->hdl;
+	err = state->hdl.error;
+	if (err) {
+		v4l2_ctrl_handler_free(&state->hdl);
+		return err;
+	}
+
+	/* Initialize wm8775 */
+
+	/* RESET */
+	wm8775_write(sd, R23, 0x000);
+	/* Disable zero cross detect timeout */
+	wm8775_write(sd, R7, 0x000);
+	/* HPF enable, left justified, 24-bit (Philips) mode */
+	wm8775_write(sd, R11, 0x021);
+	/* Master mode, clock ratio 256fs */
+	wm8775_write(sd, R12, 0x102);
+	/* Powered up */
+	wm8775_write(sd, R13, 0x000);
+
+	if (!is_nova_s) {
+		/* ADC gain +2.5dB, enable zero cross */
+		wm8775_write(sd, R14, 0x1d4);
+		/* ADC gain +2.5dB, enable zero cross */
+		wm8775_write(sd, R15, 0x1d4);
+		/* ALC Stereo, ALC target level -1dB FS max gain +8dB */
+		wm8775_write(sd, R16, 0x1bf);
+		/* Enable gain control, use zero cross detection,
+		   ALC hold time 42.6 ms */
+		wm8775_write(sd, R17, 0x185);
+	} else {
+		/* ALC stereo, ALC target level -5dB FS, ALC max gain +8dB */
+		wm8775_write(sd, R16, 0x1bb);
+		/* Set ALC mode and hold time */
+		wm8775_write(sd, R17, (state->loud->val ? ALC_EN : 0) | ALC_HOLD);
+	}
+	/* ALC gain ramp up delay 34 s, ALC gain ramp down delay 33 ms */
+	wm8775_write(sd, R18, 0x0a2);
+	/* Enable noise gate, threshold -72dBfs */
+	wm8775_write(sd, R19, 0x005);
+	if (!is_nova_s) {
+		/* Transient window 4ms, lower PGA gain limit -1dB */
+		wm8775_write(sd, R20, 0x07a);
+		/* LRBOTH = 1, use input 2. */
+		wm8775_write(sd, R21, 0x102);
+	} else {
+		/* Transient window 4ms, ALC min gain -5dB  */
+		wm8775_write(sd, R20, 0x0fb);
+
+		wm8775_set_audio(sd, 1);      /* set volume/mute/mux */
+	}
+	return 0;
+}
+
+static void wm8775_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct wm8775_state *state = to_state(sd);
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&state->hdl);
+}
+
+static const struct i2c_device_id wm8775_id[] = {
+	{ "wm8775", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, wm8775_id);
+
+static struct i2c_driver wm8775_driver = {
+	.driver = {
+		.name	= "wm8775",
+	},
+	.probe		= wm8775_probe,
+	.remove		= wm8775_remove,
+	.id_table	= wm8775_id,
+};
+
+module_i2c_driver(wm8775_driver);