Merge branch 'staging/for_v3.7' into v4l_for_linus

* staging/for_v3.7: (2891 commits)
  em28xx: regression fix: use DRX-K sync firmware requests on em28xx
  drxk: allow loading firmware synchrousnously
  em28xx: Make all em28xx extensions to be initialized asynchronously
  [media] tda18271: properly report read errors in tda18271_get_id
  [media] tda18271: delay IR & RF calibration until init() if delay_cal is set
  [media] MAINTAINERS: add Michael Krufky as tda827x maintainer
  [media] MAINTAINERS: add Michael Krufky as tda8290 maintainer
  [media] MAINTAINERS: add Michael Krufky as cxusb maintainer
  [media] MAINTAINERS: add Michael Krufky as lg2160 maintainer
  [media] MAINTAINERS: add Michael Krufky as lgdt3305 maintainer
  [media] MAINTAINERS: add Michael Krufky as mxl111sf maintainer
  [media] MAINTAINERS: add Michael Krufky as mxl5007t maintainer
  [media] MAINTAINERS: add Michael Krufky as tda18271 maintainer
  [media] s5p-tv: Report only multi-plane capabilities in vidioc_querycap
  [media] s5p-mfc: Fix misplaced return statement in s5p_mfc_suspend()
  [media] exynos-gsc: Add missing static storage class specifiers
  [media] exynos-gsc: Remove <linux/version.h> header file inclusion
  [media] s5p-fimc: Fix incorrect condition in fimc_lite_reqbufs()
  [media] s5p-tv: Fix potential NULL pointer dereference error
  [media] s5k6aa: Fix possible NULL pointer dereference
  ...

Conflicts:
	drivers/media/platform/s5p-fimc/fimc-capture.c
	drivers/media/platform/s5p-fimc/fimc-lite.c

124 files changed, 76631 insertions, 0 deletions

diff --git a/drivers/media/i2c/Kconfig b/drivers/media/i2c/Kconfig
new file mode 100644
index 000000000000..24d78e28e493
--- /dev/null
+++ b/drivers/media/i2c/Kconfig
@@ -0,0 +1,591 @@
+#
+# Generic video config states
+#
+
+config VIDEO_BTCX
+	depends on PCI
+	tristate
+
+config VIDEO_TVEEPROM
+	tristate
+	depends on I2C
+
+#
+# Multimedia Video device configuration
+#
+
+if VIDEO_V4L2
+
+config VIDEO_IR_I2C
+	tristate "I2C module for IR" if !MEDIA_SUBDRV_AUTOSELECT
+	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.
+
+#
+# Encoder / Decoder module configuration
+#
+
+menu "Encoders, decoders, sensors and other helper chips"
+	visible if !MEDIA_SUBDRV_AUTOSELECT
+
+comment "Audio decoders, processors and mixers"
+
+config VIDEO_TVAUDIO
+	tristate "Simple audio decoder chips"
+	depends on VIDEO_V4L2 && 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_TDA7432
+	tristate "Philips TDA7432 audio processor"
+	depends on VIDEO_V4L2 && 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_MSP3400
+	tristate "Micronas MSP34xx audio decoders"
+	depends on VIDEO_V4L2 && 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_CS5345
+	tristate "Cirrus Logic CS5345 audio ADC"
+	depends on VIDEO_V4L2 && 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_V4L2 && 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_TLV320AIC23B
+	tristate "Texas Instruments TLV320AIC23B audio codec"
+	depends on VIDEO_V4L2 && 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_WM8775
+	tristate "Wolfson Microelectronics WM8775 audio ADC with input mixer"
+	depends on VIDEO_V4L2 && 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.
+
+config VIDEO_WM8739
+	tristate "Wolfson Microelectronics WM8739 stereo audio ADC"
+	depends on VIDEO_V4L2 && 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_VP27SMPX
+	tristate "Panasonic VP27s internal MPX"
+	depends on VIDEO_V4L2 && 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.
+
+comment "RDS decoders"
+
+config VIDEO_SAA6588
+	tristate "SAA6588 Radio Chip RDS decoder support"
+	depends on VIDEO_V4L2 && 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.
+
+comment "Video decoders"
+
+config VIDEO_ADV7180
+	tristate "Analog Devices ADV7180 decoder"
+	depends on VIDEO_V4L2 && I2C
+	---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_V4L2 && 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_ADV7604
+	tristate "Analog Devices ADV7604 decoder"
+	depends on VIDEO_V4L2 && I2C && VIDEO_V4L2_SUBDEV_API
+	---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_BT819
+	tristate "BT819A VideoStream decoder"
+	depends on VIDEO_V4L2 && 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_V4L2 && 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_V4L2 && 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_KS0127
+	tristate "KS0127 video decoder"
+	depends on VIDEO_V4L2 && 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_SAA7110
+	tristate "Philips SAA7110 video decoder"
+	depends on VIDEO_V4L2 && 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_V4L2 && 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_SAA7191
+	tristate "Philips SAA7191 video decoder"
+	depends on VIDEO_V4L2 && I2C
+	---help---
+	  Support for the Philips SAA7191 video decoder.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called saa7191.
+
+config VIDEO_TVP514X
+	tristate "Texas Instruments TVP514x video decoder"
+	depends on VIDEO_V4L2 && I2C
+	---help---
+	  This is a Video4Linux2 sensor-level 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_V4L2 && 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_V4L2 && I2C
+	---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_VPX3220
+	tristate "vpx3220a, vpx3216b & vpx3214c video decoders"
+	depends on VIDEO_V4L2 && 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_V4L2 && 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"
+
+comment "MPEG video encoders"
+
+config VIDEO_CX2341X
+	tristate "Conexant CX2341x MPEG encoders"
+	depends on VIDEO_V4L2
+	---help---
+	  Support for the Conexant CX23416 MPEG encoders
+	  and CX23415 MPEG encoder/decoders.
+
+	  This module currently supports the encoding functions only.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called cx2341x.
+
+comment "Video encoders"
+
+config VIDEO_SAA7127
+	tristate "Philips SAA7127/9 digital video encoders"
+	depends on VIDEO_V4L2 && 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_V4L2 && 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_ADV7170
+	tristate "Analog Devices ADV7170 video encoder"
+	depends on VIDEO_V4L2 && 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_V4L2 && 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
+	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_AD9389B
+	tristate "Analog Devices AD9389B encoder"
+	depends on VIDEO_V4L2 && I2C && VIDEO_V4L2_SUBDEV_API
+	---help---
+	  Support for the Analog Devices AD9389B video encoder.
+
+	  This is a Analog Devices HDMI transmitter.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ad9389b.
+
+config VIDEO_AK881X
+	tristate "AK8813/AK8814 video encoders"
+	depends on I2C
+	help
+	  Video output driver for AKM AK8813 and AK8814 TV encoders
+
+comment "Camera sensor devices"
+
+config VIDEO_APTINA_PLL
+	tristate
+
+config VIDEO_SMIAPP_PLL
+	tristate
+
+config VIDEO_OV7670
+	tristate "OmniVision OV7670 sensor support"
+	depends on I2C && VIDEO_V4L2
+	depends on MEDIA_CAMERA_SUPPORT
+	---help---
+	  This is a Video4Linux2 sensor-level driver for the OmniVision
+	  OV7670 VGA camera.  It currently only works with the M88ALP01
+	  controller.
+
+config VIDEO_VS6624
+	tristate "ST VS6624 sensor support"
+	depends on VIDEO_V4L2 && I2C
+	depends on MEDIA_CAMERA_SUPPORT
+	---help---
+	  This is a Video4Linux2 sensor-level driver for the ST VS6624
+	  camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called vs6624.
+
+config VIDEO_MT9M032
+	tristate "MT9M032 camera sensor support"
+	depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
+	depends on MEDIA_CAMERA_SUPPORT
+	select VIDEO_APTINA_PLL
+	---help---
+	  This driver supports MT9M032 camera sensors from Aptina, monochrome
+	  models only.
+
+config VIDEO_MT9P031
+	tristate "Aptina MT9P031 support"
+	depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
+	depends on MEDIA_CAMERA_SUPPORT
+	select VIDEO_APTINA_PLL
+	---help---
+	  This is a Video4Linux2 sensor-level driver for the Aptina
+	  (Micron) mt9p031 5 Mpixel camera.
+
+config VIDEO_MT9T001
+	tristate "Aptina MT9T001 support"
+	depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
+	depends on MEDIA_CAMERA_SUPPORT
+	---help---
+	  This is a Video4Linux2 sensor-level driver for the Aptina
+	  (Micron) mt0t001 3 Mpixel camera.
+
+config VIDEO_MT9V011
+	tristate "Micron mt9v011 sensor support"
+	depends on I2C && VIDEO_V4L2
+	depends on MEDIA_CAMERA_SUPPORT
+	---help---
+	  This is a Video4Linux2 sensor-level 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"
+	depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
+	depends on MEDIA_CAMERA_SUPPORT
+	---help---
+	  This is a Video4Linux2 sensor-level driver for the Micron
+	  MT9V032 752x480 CMOS sensor.
+
+config VIDEO_TCM825X
+	tristate "TCM825x camera sensor support"
+	depends on I2C && VIDEO_V4L2
+	depends on MEDIA_CAMERA_SUPPORT
+	---help---
+	  This is a driver for the Toshiba TCM825x VGA camera sensor.
+	  It is used for example in Nokia N800.
+
+config VIDEO_SR030PC30
+	tristate "Siliconfile SR030PC30 sensor support"
+	depends on I2C && VIDEO_V4L2
+	depends on MEDIA_CAMERA_SUPPORT
+	---help---
+	  This driver supports SR030PC30 VGA camera from Siliconfile
+
+config VIDEO_NOON010PC30
+	tristate "Siliconfile NOON010PC30 sensor support"
+	depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
+	depends on MEDIA_CAMERA_SUPPORT
+	---help---
+	  This driver supports NOON010PC30 CIF camera from Siliconfile
+
+source "drivers/media/i2c/m5mols/Kconfig"
+
+config VIDEO_S5K6AA
+	tristate "Samsung S5K6AAFX sensor support"
+	depends on MEDIA_CAMERA_SUPPORT
+	depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
+	---help---
+	  This is a V4L2 sensor-level driver for Samsung S5K6AA(FX) 1.3M
+	  camera sensor with an embedded SoC image signal processor.
+
+config VIDEO_S5K4ECGX
+        tristate "Samsung S5K4ECGX sensor support"
+        depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
+        ---help---
+          This is a V4L2 sensor-level driver for Samsung S5K4ECGX 5M
+          camera sensor with an embedded SoC image signal processor.
+
+source "drivers/media/i2c/smiapp/Kconfig"
+
+comment "Flash devices"
+
+config VIDEO_ADP1653
+	tristate "ADP1653 flash support"
+	depends on I2C && VIDEO_V4L2 && MEDIA_CONTROLLER
+	depends on MEDIA_CAMERA_SUPPORT
+	---help---
+	  This is a driver for the ADP1653 flash controller. It is used for
+	  example in Nokia N900.
+
+config VIDEO_AS3645A
+	tristate "AS3645A flash driver support"
+	depends on I2C && VIDEO_V4L2 && MEDIA_CONTROLLER
+	depends on MEDIA_CAMERA_SUPPORT
+	---help---
+	  This is a driver for the AS3645A and LM3555 flash controllers. It has
+	  build in control for flash, torch and indicator LEDs.
+
+comment "Video improvement chips"
+
+config VIDEO_UPD64031A
+	tristate "NEC Electronics uPD64031A Ghost Reduction"
+	depends on VIDEO_V4L2 && I2C
+	---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_V4L2 && 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.
+
+comment "Miscelaneous helper chips"
+
+config VIDEO_THS7303
+	tristate "THS7303 Video Amplifier"
+	depends on I2C
+	help
+	  Support for TI THS7303 video amplifier
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ths7303.
+
+config VIDEO_M52790
+	tristate "Mitsubishi M52790 A/V switch"
+	depends on VIDEO_V4L2 && 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.
+endmenu
+
+menu "Sensors used on soc_camera driver"
+
+if SOC_CAMERA
+	source "drivers/media/i2c/soc_camera/Kconfig"
+endif
+
+endmenu
+
+endif
diff --git a/drivers/media/i2c/Makefile b/drivers/media/i2c/Makefile
new file mode 100644
index 000000000000..b1d62dfd49b8
--- /dev/null
+++ b/drivers/media/i2c/Makefile
@@ -0,0 +1,67 @@
+msp3400-objs	:=	msp3400-driver.o msp3400-kthreads.o
+obj-$(CONFIG_VIDEO_MSP3400) += msp3400.o
+
+obj-$(CONFIG_VIDEO_SMIAPP)	+= smiapp/
+obj-$(CONFIG_VIDEO_CX25840) += cx25840/
+obj-$(CONFIG_VIDEO_M5MOLS)	+= m5mols/
+obj-y				+= soc_camera/
+
+obj-$(CONFIG_VIDEO_APTINA_PLL) += aptina-pll.o
+obj-$(CONFIG_VIDEO_TVAUDIO) += tvaudio.o
+obj-$(CONFIG_VIDEO_TDA7432) += tda7432.o
+obj-$(CONFIG_VIDEO_SAA6588) += saa6588.o
+obj-$(CONFIG_VIDEO_TDA9840) += tda9840.o
+obj-$(CONFIG_VIDEO_TEA6415C) += tea6415c.o
+obj-$(CONFIG_VIDEO_TEA6420) += tea6420.o
+obj-$(CONFIG_VIDEO_SAA7110) += saa7110.o
+obj-$(CONFIG_VIDEO_SAA711X) += saa7115.o
+obj-$(CONFIG_VIDEO_SAA717X) += saa717x.o
+obj-$(CONFIG_VIDEO_SAA7127) += saa7127.o
+obj-$(CONFIG_VIDEO_SAA7185) += saa7185.o
+obj-$(CONFIG_VIDEO_SAA7191) += saa7191.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_ADV7604) += adv7604.o
+obj-$(CONFIG_VIDEO_AD9389B) += ad9389b.o
+obj-$(CONFIG_VIDEO_VPX3220) += vpx3220.o
+obj-$(CONFIG_VIDEO_VS6624)  += vs6624.o
+obj-$(CONFIG_VIDEO_BT819) += bt819.o
+obj-$(CONFIG_VIDEO_BT856) += bt856.o
+obj-$(CONFIG_VIDEO_BT866) += bt866.o
+obj-$(CONFIG_VIDEO_KS0127) += ks0127.o
+obj-$(CONFIG_VIDEO_THS7303) += ths7303.o
+obj-$(CONFIG_VIDEO_TVP5150) += tvp5150.o
+obj-$(CONFIG_VIDEO_TVP514X) += tvp514x.o
+obj-$(CONFIG_VIDEO_TVP7002) += tvp7002.o
+obj-$(CONFIG_VIDEO_CS5345) += cs5345.o
+obj-$(CONFIG_VIDEO_CS53L32A) += cs53l32a.o
+obj-$(CONFIG_VIDEO_M52790) += m52790.o
+obj-$(CONFIG_VIDEO_TLV320AIC23B) += tlv320aic23b.o
+obj-$(CONFIG_VIDEO_WM8775) += wm8775.o
+obj-$(CONFIG_VIDEO_WM8739) += wm8739.o
+obj-$(CONFIG_VIDEO_VP27SMPX) += vp27smpx.o
+obj-$(CONFIG_VIDEO_UPD64031A) += upd64031a.o
+obj-$(CONFIG_VIDEO_UPD64083) += upd64083.o
+obj-$(CONFIG_VIDEO_OV7670) 	+= ov7670.o
+obj-$(CONFIG_VIDEO_TCM825X) += tcm825x.o
+obj-$(CONFIG_VIDEO_TVEEPROM) += tveeprom.o
+obj-$(CONFIG_VIDEO_MT9M032) += mt9m032.o
+obj-$(CONFIG_VIDEO_MT9P031) += mt9p031.o
+obj-$(CONFIG_VIDEO_MT9T001) += mt9t001.o
+obj-$(CONFIG_VIDEO_MT9V011) += mt9v011.o
+obj-$(CONFIG_VIDEO_MT9V032) += mt9v032.o
+obj-$(CONFIG_VIDEO_SR030PC30)	+= sr030pc30.o
+obj-$(CONFIG_VIDEO_NOON010PC30)	+= noon010pc30.o
+obj-$(CONFIG_VIDEO_S5K6AA)	+= s5k6aa.o
+obj-$(CONFIG_VIDEO_S5K4ECGX)	+= s5k4ecgx.o
+obj-$(CONFIG_VIDEO_ADP1653)	+= adp1653.o
+obj-$(CONFIG_VIDEO_AS3645A)	+= as3645a.o
+obj-$(CONFIG_VIDEO_SMIAPP_PLL)	+= smiapp-pll.o
+obj-$(CONFIG_VIDEO_BTCX)  += btcx-risc.o
+obj-$(CONFIG_VIDEO_CX2341X) += cx2341x.o
+obj-$(CONFIG_VIDEO_AK881X)		+= ak881x.o
+obj-$(CONFIG_VIDEO_IR_I2C)  += ir-kbd-i2c.o
diff --git a/drivers/media/i2c/ad9389b.c b/drivers/media/i2c/ad9389b.c
new file mode 100644
index 000000000000..c2886b6a727d
--- /dev/null
+++ b/drivers/media/i2c/ad9389b.c
@@ -0,0 +1,1328 @@
+/*
+ * Analog Devices AD9389B/AD9889B video encoder driver
+ *
+ * Copyright 2012 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * 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.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+/*
+ * References (c = chapter, p = page):
+ * REF_01 - Analog Devices, Programming Guide, AD9889B/AD9389B,
+ * HDMI Transitter, Rev. A, October 2010
+ */
+
+#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 <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ctrls.h>
+#include <media/ad9389b.h>
+
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "debug level (0-2)");
+
+MODULE_DESCRIPTION("Analog Devices AD9389B/AD9889B video encoder driver");
+MODULE_AUTHOR("Hans Verkuil <hans.verkuil@cisco.com>");
+MODULE_AUTHOR("Martin Bugge <marbugge@cisco.com>");
+MODULE_LICENSE("GPL");
+
+#define MASK_AD9389B_EDID_RDY_INT   0x04
+#define MASK_AD9389B_MSEN_INT       0x40
+#define MASK_AD9389B_HPD_INT        0x80
+
+#define MASK_AD9389B_HPD_DETECT     0x40
+#define MASK_AD9389B_MSEN_DETECT    0x20
+#define MASK_AD9389B_EDID_RDY       0x10
+
+#define EDID_MAX_RETRIES (8)
+#define EDID_DELAY 250
+#define EDID_MAX_SEGM 8
+
+/*
+**********************************************************************
+*
+*  Arrays with configuration parameters for the AD9389B
+*
+**********************************************************************
+*/
+
+struct i2c_reg_value {
+	u8 reg;
+	u8 value;
+};
+
+struct ad9389b_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;
+};
+
+struct ad9389b_state {
+	struct ad9389b_platform_data pdata;
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct v4l2_ctrl_handler hdl;
+	int chip_revision;
+	/* Is the ad9389b powered on? */
+	bool power_on;
+	/* Did we receive hotplug and rx-sense signals? */
+	bool have_monitor;
+	/* timings from s_dv_timings */
+	struct v4l2_dv_timings dv_timings;
+	/* 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 i2c_client *edid_i2c_client;
+	struct ad9389b_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 ad9389b_check_monitor_present_status(struct v4l2_subdev *sd);
+static bool ad9389b_check_edid_status(struct v4l2_subdev *sd);
+static void ad9389b_setup(struct v4l2_subdev *sd);
+static int ad9389b_s_i2s_clock_freq(struct v4l2_subdev *sd, u32 freq);
+static int ad9389b_s_clock_freq(struct v4l2_subdev *sd, u32 freq);
+
+static inline struct ad9389b_state *get_ad9389b_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct ad9389b_state, sd);
+}
+
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct ad9389b_state, hdl)->sd;
+}
+
+/* ------------------------ I2C ----------------------------------------------- */
+
+static int ad9389b_rd(struct v4l2_subdev *sd, u8 reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return i2c_smbus_read_byte_data(client, reg);
+}
+
+static int ad9389b_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, "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 ad9389b_wr_and_or(struct v4l2_subdev *sd, u8 reg,
+						u8 clr_mask, u8 val_mask)
+{
+	ad9389b_wr(sd, reg, (ad9389b_rd(sd, reg) & clr_mask) | val_mask);
+}
+
+static void ad9389b_edid_rd(struct v4l2_subdev *sd, u16 len, u8 *buf)
+{
+	struct ad9389b_state *state = get_ad9389b_state(sd);
+	int i;
+
+	v4l2_dbg(1, debug, sd, "%s:\n", __func__);
+
+	for (i = 0; i < len; i++)
+		buf[i] = i2c_smbus_read_byte_data(state->edid_i2c_client, i);
+}
+
+static inline bool ad9389b_have_hotplug(struct v4l2_subdev *sd)
+{
+	return ad9389b_rd(sd, 0x42) & MASK_AD9389B_HPD_DETECT;
+}
+
+static inline bool ad9389b_have_rx_sense(struct v4l2_subdev *sd)
+{
+	return ad9389b_rd(sd, 0x42) & MASK_AD9389B_MSEN_DETECT;
+}
+
+static void ad9389b_csc_conversion_mode(struct v4l2_subdev *sd, u8 mode)
+{
+	ad9389b_wr_and_or(sd, 0x17, 0xe7, (mode & 0x3)<<3);
+	ad9389b_wr_and_or(sd, 0x18, 0x9f, (mode & 0x3)<<5);
+}
+
+static void ad9389b_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 */
+	ad9389b_wr_and_or(sd, 0x18, 0xe0, A1>>8);
+	ad9389b_wr(sd, 0x19, A1);
+	ad9389b_wr_and_or(sd, 0x1A, 0xe0, A2>>8);
+	ad9389b_wr(sd, 0x1B, A2);
+	ad9389b_wr_and_or(sd, 0x1c, 0xe0, A3>>8);
+	ad9389b_wr(sd, 0x1d, A3);
+	ad9389b_wr_and_or(sd, 0x1e, 0xe0, A4>>8);
+	ad9389b_wr(sd, 0x1f, A4);
+
+	/* B */
+	ad9389b_wr_and_or(sd, 0x20, 0xe0, B1>>8);
+	ad9389b_wr(sd, 0x21, B1);
+	ad9389b_wr_and_or(sd, 0x22, 0xe0, B2>>8);
+	ad9389b_wr(sd, 0x23, B2);
+	ad9389b_wr_and_or(sd, 0x24, 0xe0, B3>>8);
+	ad9389b_wr(sd, 0x25, B3);
+	ad9389b_wr_and_or(sd, 0x26, 0xe0, B4>>8);
+	ad9389b_wr(sd, 0x27, B4);
+
+	/* C */
+	ad9389b_wr_and_or(sd, 0x28, 0xe0, C1>>8);
+	ad9389b_wr(sd, 0x29, C1);
+	ad9389b_wr_and_or(sd, 0x2A, 0xe0, C2>>8);
+	ad9389b_wr(sd, 0x2B, C2);
+	ad9389b_wr_and_or(sd, 0x2C, 0xe0, C3>>8);
+	ad9389b_wr(sd, 0x2D, C3);
+	ad9389b_wr_and_or(sd, 0x2E, 0xe0, C4>>8);
+	ad9389b_wr(sd, 0x2F, C4);
+}
+
+static void ad9389b_csc_rgb_full2limit(struct v4l2_subdev *sd, bool enable)
+{
+	if (enable) {
+		u8 csc_mode = 0;
+
+		ad9389b_csc_conversion_mode(sd, csc_mode);
+		ad9389b_csc_coeff(sd,
+				  4096-564, 0, 0, 256,
+				  0, 4096-564, 0, 256,
+				  0, 0, 4096-564, 256);
+		/* enable CSC */
+		ad9389b_wr_and_or(sd, 0x3b, 0xfe, 0x1);
+		/* AVI infoframe: Limited range RGB (16-235) */
+		ad9389b_wr_and_or(sd, 0xcd, 0xf9, 0x02);
+	} else {
+		/* disable CSC */
+		ad9389b_wr_and_or(sd, 0x3b, 0xfe, 0x0);
+		/* AVI infoframe: Full range RGB (0-255) */
+		ad9389b_wr_and_or(sd, 0xcd, 0xf9, 0x04);
+	}
+}
+
+static void ad9389b_set_IT_content_AVI_InfoFrame(struct v4l2_subdev *sd)
+{
+	struct ad9389b_state *state = get_ad9389b_state(sd);
+
+	if (state->dv_timings.bt.standards & V4L2_DV_BT_STD_CEA861) {
+		/* CEA format, not IT  */
+		ad9389b_wr_and_or(sd, 0xcd, 0xbf, 0x00);
+	} else {
+		/* IT format */
+		ad9389b_wr_and_or(sd, 0xcd, 0xbf, 0x40);
+	}
+}
+
+static int ad9389b_set_rgb_quantization_mode(struct v4l2_subdev *sd, struct v4l2_ctrl *ctrl)
+{
+	struct ad9389b_state *state = get_ad9389b_state(sd);
+
+	switch (ctrl->val) {
+	case V4L2_DV_RGB_RANGE_AUTO:
+		/* automatic */
+		if (state->dv_timings.bt.standards & V4L2_DV_BT_STD_CEA861) {
+			/* cea format, RGB limited range (16-235) */
+			ad9389b_csc_rgb_full2limit(sd, true);
+		} else {
+			/* not cea format, RGB full range (0-255) */
+			ad9389b_csc_rgb_full2limit(sd, false);
+		}
+		break;
+	case V4L2_DV_RGB_RANGE_LIMITED:
+		/* RGB limited range (16-235) */
+		ad9389b_csc_rgb_full2limit(sd, true);
+		break;
+	case V4L2_DV_RGB_RANGE_FULL:
+		/* RGB full range (0-255) */
+		ad9389b_csc_rgb_full2limit(sd, false);
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static void ad9389b_set_manual_pll_gear(struct v4l2_subdev *sd, u32 pixelclock)
+{
+	u8 gear;
+
+	/* Workaround for TMDS PLL problem
+	 * The TMDS PLL in AD9389b change gear when the chip is heated above a
+	 * certain temperature. The output is disabled when the PLL change gear
+	 * so the monitor has to lock on the signal again. A workaround for
+	 * this is to use the manual PLL gears. This is a solution from Analog
+	 * Devices that is not documented in the datasheets.
+	 * 0x98 [7] = enable manual gearing. 0x98 [6:4] = gear
+	 *
+	 * The pixel frequency ranges are based on readout of the gear the
+	 * automatic gearing selects for different pixel clocks
+	 * (read from 0x9e [3:1]).
+	 */
+
+	if (pixelclock > 140000000)
+		gear = 0xc0; /* 4th gear */
+	else if (pixelclock > 117000000)
+		gear = 0xb0; /* 3rd gear */
+	else if (pixelclock > 87000000)
+		gear = 0xa0; /* 2nd gear */
+	else if (pixelclock > 60000000)
+		gear = 0x90; /* 1st gear */
+	else
+		gear = 0x80; /* 0th gear */
+
+	ad9389b_wr_and_or(sd, 0x98, 0x0f, gear);
+}
+
+/* ------------------------------ CTRL OPS ------------------------------ */
+
+static int ad9389b_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+	struct ad9389b_state *state = get_ad9389b_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 */
+		ad9389b_wr_and_or(sd, 0xaf, 0xfd,
+				ctrl->val == V4L2_DV_TX_MODE_HDMI ? 0x02 : 0x00);
+		return 0;
+	}
+	if (state->rgb_quantization_range_ctrl == ctrl)
+		return ad9389b_set_rgb_quantization_mode(sd, ctrl);
+	return -EINVAL;
+}
+
+static const struct v4l2_ctrl_ops ad9389b_ctrl_ops = {
+	.s_ctrl = ad9389b_s_ctrl,
+};
+
+/* ---------------------------- CORE OPS ------------------------------------------- */
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int ad9389b_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	reg->val = ad9389b_rd(sd, reg->reg & 0xff);
+	reg->size = 1;
+	return 0;
+}
+
+static int ad9389b_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	ad9389b_wr(sd, reg->reg & 0xff, reg->val & 0xff);
+	return 0;
+}
+#endif
+
+static int ad9389b_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ident *chip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_AD9389B, 0);
+}
+
+static int ad9389b_log_status(struct v4l2_subdev *sd)
+{
+	struct ad9389b_state *state = get_ad9389b_state(sd);
+	struct ad9389b_state_edid *edid = &state->edid;
+
+	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"
+	};
+
+	u8 manual_gear;
+
+	v4l2_info(sd, "chip revision %d\n", state->chip_revision);
+	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",
+			(ad9389b_rd(sd, 0x42) & MASK_AD9389B_HPD_DETECT) ?
+							"detected" : "no",
+			(ad9389b_rd(sd, 0x42) & MASK_AD9389B_MSEN_DETECT) ?
+							"detected" : "no",
+			edid->segments ? "found" : "no", edid->blocks);
+	if (state->have_monitor) {
+		v4l2_info(sd, "%s output %s\n",
+				  (ad9389b_rd(sd, 0xaf) & 0x02) ?
+				  "HDMI" : "DVI-D",
+				  (ad9389b_rd(sd, 0xa1) & 0x3c) ?
+				  "disabled" : "enabled");
+	}
+	v4l2_info(sd, "ad9389b: %s\n", (ad9389b_rd(sd, 0xb8) & 0x40) ?
+					"encrypted" : "no encryption");
+	v4l2_info(sd, "state: %s, error: %s, detect count: %u, msk/irq: %02x/%02x\n",
+			states[ad9389b_rd(sd, 0xc8) & 0xf],
+			errors[ad9389b_rd(sd, 0xc8) >> 4],
+			state->edid_detect_counter,
+			ad9389b_rd(sd, 0x94), ad9389b_rd(sd, 0x96));
+	manual_gear = ad9389b_rd(sd, 0x98) & 0x80;
+	v4l2_info(sd, "ad9389b: RGB quantization: %s range\n",
+			ad9389b_rd(sd, 0x3b) & 0x01 ? "limited" : "full");
+	v4l2_info(sd, "ad9389b: %s gear %d\n",
+		  manual_gear ? "manual" : "automatic",
+		  manual_gear ? ((ad9389b_rd(sd, 0x98) & 0x70) >> 4) :
+				((ad9389b_rd(sd, 0x9e) & 0x0e) >> 1));
+	if (state->have_monitor) {
+		if (ad9389b_rd(sd, 0xaf) & 0x02) {
+			/* HDMI only */
+			u8 manual_cts = ad9389b_rd(sd, 0x0a) & 0x80;
+			u32 N = (ad9389b_rd(sd, 0x01) & 0xf) << 16 |
+				 ad9389b_rd(sd, 0x02) << 8 |
+				 ad9389b_rd(sd, 0x03);
+			u8 vic_detect = ad9389b_rd(sd, 0x3e) >> 2;
+			u8 vic_sent = ad9389b_rd(sd, 0x3d) & 0x3f;
+			u32 CTS;
+
+			if (manual_cts)
+				CTS = (ad9389b_rd(sd, 0x07) & 0xf) << 16 |
+				       ad9389b_rd(sd, 0x08) << 8 |
+				       ad9389b_rd(sd, 0x09);
+			else
+				CTS = (ad9389b_rd(sd, 0x04) & 0xf) << 16 |
+				       ad9389b_rd(sd, 0x05) << 8 |
+				       ad9389b_rd(sd, 0x06);
+			N = (ad9389b_rd(sd, 0x01) & 0xf) << 16 |
+			     ad9389b_rd(sd, 0x02) << 8 |
+			     ad9389b_rd(sd, 0x03);
+
+			v4l2_info(sd, "ad9389b: CTS %s mode: N %d, CTS %d\n",
+				manual_cts ? "manual" : "automatic", N, CTS);
+
+			v4l2_info(sd, "ad9389b: VIC: detected %d, sent %d\n",
+				vic_detect, vic_sent);
+		}
+	}
+	if (state->dv_timings.type == V4L2_DV_BT_656_1120) {
+		struct v4l2_bt_timings *bt = bt = &state->dv_timings.bt;
+		u32 frame_width = bt->width + bt->hfrontporch +
+			bt->hsync + bt->hbackporch;
+		u32 frame_height = bt->height + bt->vfrontporch +
+			bt->vsync + bt->vbackporch;
+		u32 frame_size = frame_width * frame_height;
+
+		v4l2_info(sd, "timings: %ux%u%s%u (%ux%u). Pix freq. = %u Hz. Polarities = 0x%x\n",
+			bt->width, bt->height, bt->interlaced ? "i" : "p",
+			frame_size > 0 ?  (unsigned)bt->pixelclock / frame_size : 0,
+			frame_width, frame_height,
+			(unsigned)bt->pixelclock, bt->polarities);
+	} else {
+		v4l2_info(sd, "no timings set\n");
+	}
+	return 0;
+}
+
+/* Power up/down ad9389b */
+static int ad9389b_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct ad9389b_state *state = get_ad9389b_state(sd);
+	struct ad9389b_platform_data *pdata = &state->pdata;
+	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 */
+		ad9389b_wr_and_or(sd, 0x41, 0xbf, 0x40);
+		return true;
+	}
+
+	/* Power up */
+	/* The ad9389b does not always come up immediately.
+	   Retry multiple times. */
+	for (i = 0; i < retries; i++) {
+		ad9389b_wr_and_or(sd, 0x41, 0xbf, 0x0);
+		if ((ad9389b_rd(sd, 0x41) & 0x40) == 0)
+			break;
+		ad9389b_wr_and_or(sd, 0x41, 0xbf, 0x40);
+		msleep(10);
+	}
+	if (i == retries) {
+		v4l2_dbg(1, debug, sd, "failed to powerup the ad9389b\n");
+		ad9389b_s_power(sd, 0);
+		return false;
+	}
+	if (i > 1)
+		v4l2_dbg(1, debug, sd,
+			"needed %d retries to powerup the ad9389b\n", i);
+
+	/* Select chip: AD9389B */
+	ad9389b_wr_and_or(sd, 0xba, 0xef, 0x10);
+
+	/* Reserved registers that must be set according to REF_01 p. 11*/
+	ad9389b_wr_and_or(sd, 0x98, 0xf0, 0x07);
+	ad9389b_wr(sd, 0x9c, 0x38);
+	ad9389b_wr_and_or(sd, 0x9d, 0xfc, 0x01);
+
+	/* Differential output drive strength */
+	if (pdata->diff_data_drive_strength > 0)
+		ad9389b_wr(sd, 0xa2, pdata->diff_data_drive_strength);
+	else
+		ad9389b_wr(sd, 0xa2, 0x87);
+
+	if (pdata->diff_clk_drive_strength > 0)
+		ad9389b_wr(sd, 0xa3, pdata->diff_clk_drive_strength);
+	else
+		ad9389b_wr(sd, 0xa3, 0x87);
+
+	ad9389b_wr(sd, 0x0a, 0x01);
+	ad9389b_wr(sd, 0xbb, 0xff);
+
+	/* Set number of attempts to read the EDID */
+	ad9389b_wr(sd, 0xc9, 0xf);
+	return true;
+}
+
+/* Enable interrupts */
+static void ad9389b_set_isr(struct v4l2_subdev *sd, bool enable)
+{
+	u8 irqs = MASK_AD9389B_HPD_INT | MASK_AD9389B_MSEN_INT;
+	u8 irqs_rd;
+	int retries = 100;
+
+	/* The datasheet says that the EDID ready interrupt should be
+	   disabled if there is no hotplug. */
+	if (!enable)
+		irqs = 0;
+	else if (ad9389b_have_hotplug(sd))
+		irqs |= MASK_AD9389B_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 {
+		ad9389b_wr(sd, 0x94, irqs);
+		irqs_rd = ad9389b_rd(sd, 0x94);
+	} while (retries-- && irqs_rd != irqs);
+
+	if (irqs_rd != irqs)
+		v4l2_err(sd, "Could not set interrupts: hw failure?\n");
+}
+
+/* Interrupt handler */
+static int ad9389b_isr(struct v4l2_subdev *sd, u32 status, bool *handled)
+{
+	u8 irq_status;
+
+	/* disable interrupts to prevent a race condition */
+	ad9389b_set_isr(sd, false);
+	irq_status = ad9389b_rd(sd, 0x96);
+	/* clear detected interrupts */
+	ad9389b_wr(sd, 0x96, irq_status);
+
+	if (irq_status & (MASK_AD9389B_HPD_INT | MASK_AD9389B_MSEN_INT))
+		ad9389b_check_monitor_present_status(sd);
+	if (irq_status & MASK_AD9389B_EDID_RDY_INT)
+		ad9389b_check_edid_status(sd);
+
+	/* enable interrupts */
+	ad9389b_set_isr(sd, true);
+	*handled = true;
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops ad9389b_core_ops = {
+	.log_status = ad9389b_log_status,
+	.g_chip_ident = ad9389b_g_chip_ident,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = ad9389b_g_register,
+	.s_register = ad9389b_s_register,
+#endif
+	.s_power = ad9389b_s_power,
+	.interrupt_service_routine = ad9389b_isr,
+};
+
+/* ------------------------------ PAD OPS ------------------------------ */
+
+static int ad9389b_get_edid(struct v4l2_subdev *sd, struct v4l2_subdev_edid *edid)
+{
+	struct ad9389b_state *state = get_ad9389b_state(sd);
+
+	if (edid->pad != 0)
+		return -EINVAL;
+	if (edid->blocks == 0 || edid->blocks > 256)
+		return -EINVAL;
+	if (!edid->edid)
+		return -EINVAL;
+	if (!state->edid.segments) {
+		v4l2_dbg(1, debug, sd, "EDID segment 0 not found\n");
+		return -ENODATA;
+	}
+	if (edid->start_block >= state->edid.segments * 2)
+		return -E2BIG;
+	if (edid->blocks + edid->start_block >= state->edid.segments * 2)
+		edid->blocks = state->edid.segments * 2 - edid->start_block;
+	memcpy(edid->edid, &state->edid.data[edid->start_block * 128],
+				128 * edid->blocks);
+	return 0;
+}
+
+static const struct v4l2_subdev_pad_ops ad9389b_pad_ops = {
+	.get_edid = ad9389b_get_edid,
+};
+
+/* ------------------------------ VIDEO OPS ------------------------------ */
+
+/* Enable/disable ad9389b output */
+static int ad9389b_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct ad9389b_state *state = get_ad9389b_state(sd);
+
+	v4l2_dbg(1, debug, sd, "%s: %sable\n", __func__, (enable ? "en" : "dis"));
+
+	ad9389b_wr_and_or(sd, 0xa1, ~0x3c, (enable ? 0 : 0x3c));
+	if (enable) {
+		ad9389b_check_monitor_present_status(sd);
+	} else {
+		ad9389b_s_power(sd, 0);
+		state->have_monitor = false;
+	}
+	return 0;
+}
+
+static const struct v4l2_dv_timings ad9389b_timings[] = {
+	V4L2_DV_BT_CEA_720X480P59_94,
+	V4L2_DV_BT_CEA_720X576P50,
+	V4L2_DV_BT_CEA_1280X720P24,
+	V4L2_DV_BT_CEA_1280X720P25,
+	V4L2_DV_BT_CEA_1280X720P30,
+	V4L2_DV_BT_CEA_1280X720P50,
+	V4L2_DV_BT_CEA_1280X720P60,
+	V4L2_DV_BT_CEA_1920X1080P24,
+	V4L2_DV_BT_CEA_1920X1080P25,
+	V4L2_DV_BT_CEA_1920X1080P30,
+	V4L2_DV_BT_CEA_1920X1080P50,
+	V4L2_DV_BT_CEA_1920X1080P60,
+
+	V4L2_DV_BT_DMT_640X350P85,
+	V4L2_DV_BT_DMT_640X400P85,
+	V4L2_DV_BT_DMT_720X400P85,
+	V4L2_DV_BT_DMT_640X480P60,
+	V4L2_DV_BT_DMT_640X480P72,
+	V4L2_DV_BT_DMT_640X480P75,
+	V4L2_DV_BT_DMT_640X480P85,
+	V4L2_DV_BT_DMT_800X600P56,
+	V4L2_DV_BT_DMT_800X600P60,
+	V4L2_DV_BT_DMT_800X600P72,
+	V4L2_DV_BT_DMT_800X600P75,
+	V4L2_DV_BT_DMT_800X600P85,
+	V4L2_DV_BT_DMT_848X480P60,
+	V4L2_DV_BT_DMT_1024X768P60,
+	V4L2_DV_BT_DMT_1024X768P70,
+	V4L2_DV_BT_DMT_1024X768P75,
+	V4L2_DV_BT_DMT_1024X768P85,
+	V4L2_DV_BT_DMT_1152X864P75,
+	V4L2_DV_BT_DMT_1280X768P60_RB,
+	V4L2_DV_BT_DMT_1280X768P60,
+	V4L2_DV_BT_DMT_1280X768P75,
+	V4L2_DV_BT_DMT_1280X768P85,
+	V4L2_DV_BT_DMT_1280X800P60_RB,
+	V4L2_DV_BT_DMT_1280X800P60,
+	V4L2_DV_BT_DMT_1280X800P75,
+	V4L2_DV_BT_DMT_1280X800P85,
+	V4L2_DV_BT_DMT_1280X960P60,
+	V4L2_DV_BT_DMT_1280X960P85,
+	V4L2_DV_BT_DMT_1280X1024P60,
+	V4L2_DV_BT_DMT_1280X1024P75,
+	V4L2_DV_BT_DMT_1280X1024P85,
+	V4L2_DV_BT_DMT_1360X768P60,
+	V4L2_DV_BT_DMT_1400X1050P60_RB,
+	V4L2_DV_BT_DMT_1400X1050P60,
+	V4L2_DV_BT_DMT_1400X1050P75,
+	V4L2_DV_BT_DMT_1400X1050P85,
+	V4L2_DV_BT_DMT_1440X900P60_RB,
+	V4L2_DV_BT_DMT_1440X900P60,
+	V4L2_DV_BT_DMT_1600X1200P60,
+	V4L2_DV_BT_DMT_1680X1050P60_RB,
+	V4L2_DV_BT_DMT_1680X1050P60,
+	V4L2_DV_BT_DMT_1792X1344P60,
+	V4L2_DV_BT_DMT_1856X1392P60,
+	V4L2_DV_BT_DMT_1920X1200P60_RB,
+	V4L2_DV_BT_DMT_1366X768P60,
+	V4L2_DV_BT_DMT_1920X1080P60,
+	{},
+};
+
+static int ad9389b_s_dv_timings(struct v4l2_subdev *sd,
+				struct v4l2_dv_timings *timings)
+{
+	struct ad9389b_state *state = get_ad9389b_state(sd);
+	int i;
+
+	v4l2_dbg(1, debug, sd, "%s:\n", __func__);
+
+	/* quick sanity check */
+	if (timings->type != V4L2_DV_BT_656_1120)
+		return -EINVAL;
+
+	if (timings->bt.interlaced)
+		return -EINVAL;
+	if (timings->bt.pixelclock < 27000000 ||
+	    timings->bt.pixelclock > 170000000)
+		return -EINVAL;
+
+	/* Fill the optional fields .standards and .flags in struct v4l2_dv_timings
+	   if the format is listed in ad9389b_timings[] */
+	for (i = 0; ad9389b_timings[i].bt.width; i++) {
+		if (v4l_match_dv_timings(timings, &ad9389b_timings[i], 0)) {
+			*timings = ad9389b_timings[i];
+			break;
+		}
+	}
+
+	timings->bt.flags &= ~V4L2_DV_FL_REDUCED_FPS;
+
+	/* save timings */
+	state->dv_timings = *timings;
+
+	/* update quantization range based on new dv_timings */
+	ad9389b_set_rgb_quantization_mode(sd, state->rgb_quantization_range_ctrl);
+
+	/* update PLL gear based on new dv_timings */
+	if (state->pdata.tmds_pll_gear == AD9389B_TMDS_PLL_GEAR_SEMI_AUTOMATIC)
+		ad9389b_set_manual_pll_gear(sd, (u32)timings->bt.pixelclock);
+
+	/* update AVI infoframe */
+	ad9389b_set_IT_content_AVI_InfoFrame(sd);
+
+	return 0;
+}
+
+static int ad9389b_g_dv_timings(struct v4l2_subdev *sd,
+				struct v4l2_dv_timings *timings)
+{
+	struct ad9389b_state *state = get_ad9389b_state(sd);
+
+	v4l2_dbg(1, debug, sd, "%s:\n", __func__);
+
+	if (!timings)
+		return -EINVAL;
+
+	*timings = state->dv_timings;
+
+	return 0;
+}
+
+static int ad9389b_enum_dv_timings(struct v4l2_subdev *sd,
+			struct v4l2_enum_dv_timings *timings)
+{
+	if (timings->index >= ARRAY_SIZE(ad9389b_timings))
+		return -EINVAL;
+
+	memset(timings->reserved, 0, sizeof(timings->reserved));
+	timings->timings = ad9389b_timings[timings->index];
+	return 0;
+}
+
+static int ad9389b_dv_timings_cap(struct v4l2_subdev *sd,
+			struct v4l2_dv_timings_cap *cap)
+{
+	cap->type = V4L2_DV_BT_656_1120;
+	cap->bt.max_width = 1920;
+	cap->bt.max_height = 1200;
+	cap->bt.min_pixelclock = 27000000;
+	cap->bt.max_pixelclock = 170000000;
+	cap->bt.standards = V4L2_DV_BT_STD_CEA861 | V4L2_DV_BT_STD_DMT |
+			 V4L2_DV_BT_STD_GTF | V4L2_DV_BT_STD_CVT;
+	cap->bt.capabilities = V4L2_DV_BT_CAP_PROGRESSIVE |
+		V4L2_DV_BT_CAP_REDUCED_BLANKING | V4L2_DV_BT_CAP_CUSTOM;
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops ad9389b_video_ops = {
+	.s_stream = ad9389b_s_stream,
+	.s_dv_timings = ad9389b_s_dv_timings,
+	.g_dv_timings = ad9389b_g_dv_timings,
+	.enum_dv_timings = ad9389b_enum_dv_timings,
+	.dv_timings_cap = ad9389b_dv_timings_cap,
+};
+
+static int ad9389b_s_audio_stream(struct v4l2_subdev *sd, int enable)
+{
+	v4l2_dbg(1, debug, sd, "%s: %sable\n", __func__, (enable ? "en" : "dis"));
+
+	if (enable)
+		ad9389b_wr_and_or(sd, 0x45, 0x3f, 0x80);
+	else
+		ad9389b_wr_and_or(sd, 0x45, 0x3f, 0x40);
+
+	return 0;
+}
+
+static int ad9389b_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) */
+	ad9389b_wr(sd, 0x01, (N >> 16) & 0xf);
+	ad9389b_wr(sd, 0x02, (N >> 8) & 0xff);
+	ad9389b_wr(sd, 0x03, N & 0xff);
+
+	return 0;
+}
+
+static int ad9389b_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 */
+	ad9389b_wr_and_or(sd, 0x15, 0xf, i2s_sf);
+
+	return 0;
+}
+
+static int ad9389b_s_routing(struct v4l2_subdev *sd, u32 input, u32 output, u32 config)
+{
+	/* TODO based on input/output/config */
+	/* TODO See datasheet "Programmers guide" p. 39-40 */
+
+	/* Only 2 channels in use for application */
+	ad9389b_wr_and_or(sd, 0x50, 0x1f, 0x20);
+	/* Speaker mapping */
+	ad9389b_wr(sd, 0x51, 0x00);
+
+	/* TODO Where should this be placed? */
+	/* 16 bit audio word length */
+	ad9389b_wr_and_or(sd, 0x14, 0xf0, 0x02);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_audio_ops ad9389b_audio_ops = {
+	.s_stream = ad9389b_s_audio_stream,
+	.s_clock_freq = ad9389b_s_clock_freq,
+	.s_i2s_clock_freq = ad9389b_s_i2s_clock_freq,
+	.s_routing = ad9389b_s_routing,
+};
+
+/* --------------------- SUBDEV OPS --------------------------------------- */
+
+static const struct v4l2_subdev_ops ad9389b_ops = {
+	.core  = &ad9389b_core_ops,
+	.video = &ad9389b_video_ops,
+	.audio = &ad9389b_audio_ops,
+	.pad = &ad9389b_pad_ops,
+};
+
+/* ----------------------------------------------------------------------- */
+static void ad9389b_dbg_dump_edid(int lvl, int debug, struct v4l2_subdev *sd,
+							int segment, u8 *buf)
+{
+	int i, j;
+
+	if (debug < lvl)
+		return;
+
+	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 ad9389b_edid_handler(struct work_struct *work)
+{
+	struct delayed_work *dwork = to_delayed_work(work);
+	struct ad9389b_state *state = container_of(dwork,
+			struct ad9389b_state, edid_handler);
+	struct v4l2_subdev *sd = &state->sd;
+	struct ad9389b_edid_detect ed;
+
+	v4l2_dbg(1, debug, sd, "%s:\n", __func__);
+
+	if (ad9389b_check_edid_status(sd)) {
+		/* Return if we received the EDID. */
+		return;
+	}
+
+	if (ad9389b_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--;
+			/* EDID read failed, trigger a retry */
+			ad9389b_wr(sd, 0xc9, 0xf);
+			queue_delayed_work(state->work_queue,
+					&state->edid_handler, EDID_DELAY);
+			return;
+		}
+	}
+
+	/* We failed to read the EDID, so send an event for this. */
+	ed.present = false;
+	ed.segment = ad9389b_rd(sd, 0xc4);
+	v4l2_subdev_notify(sd, AD9389B_EDID_DETECT, (void *)&ed);
+	v4l2_dbg(1, debug, sd, "%s: no edid found\n", __func__);
+}
+
+static void ad9389b_audio_setup(struct v4l2_subdev *sd)
+{
+	v4l2_dbg(1, debug, sd, "%s\n", __func__);
+
+	ad9389b_s_i2s_clock_freq(sd, 48000);
+	ad9389b_s_clock_freq(sd, 48000);
+	ad9389b_s_routing(sd, 0, 0, 0);
+}
+
+/* Initial setup of AD9389b */
+
+/* Configure hdmi transmitter. */
+static void ad9389b_setup(struct v4l2_subdev *sd)
+{
+	struct ad9389b_state *state = get_ad9389b_state(sd);
+
+	v4l2_dbg(1, debug, sd, "%s\n", __func__);
+
+	/* Input format: RGB 4:4:4 */
+	ad9389b_wr_and_or(sd, 0x15, 0xf1, 0x0);
+	/* Output format: RGB 4:4:4 */
+	ad9389b_wr_and_or(sd, 0x16, 0x3f, 0x0);
+	/* CSC fixed point: +/-2, 1st order interpolation 4:2:2 -> 4:4:4 up
+	   conversion, Aspect ratio: 16:9 */
+	ad9389b_wr_and_or(sd, 0x17, 0xe1, 0x0e);
+	/* Disable pixel repetition and CSC */
+	ad9389b_wr_and_or(sd, 0x3b, 0x9e, 0x0);
+	/* Output format: RGB 4:4:4, Active Format Information is valid. */
+	ad9389b_wr_and_or(sd, 0x45, 0xc7, 0x08);
+	/* Underscanned */
+	ad9389b_wr_and_or(sd, 0x46, 0x3f, 0x80);
+	/* Setup video format */
+	ad9389b_wr(sd, 0x3c, 0x0);
+	/* Active format aspect ratio: same as picure. */
+	ad9389b_wr(sd, 0x47, 0x80);
+	/* No encryption */
+	ad9389b_wr_and_or(sd, 0xaf, 0xef, 0x0);
+	/* Positive clk edge capture for input video clock */
+	ad9389b_wr_and_or(sd, 0xba, 0x1f, 0x60);
+
+	ad9389b_audio_setup(sd);
+
+	v4l2_ctrl_handler_setup(&state->hdl);
+
+	ad9389b_set_IT_content_AVI_InfoFrame(sd);
+}
+
+static void ad9389b_notify_monitor_detect(struct v4l2_subdev *sd)
+{
+	struct ad9389b_monitor_detect mdt;
+	struct ad9389b_state *state = get_ad9389b_state(sd);
+
+	mdt.present = state->have_monitor;
+	v4l2_subdev_notify(sd, AD9389B_MONITOR_DETECT, (void *)&mdt);
+}
+
+static void ad9389b_check_monitor_present_status(struct v4l2_subdev *sd)
+{
+	struct ad9389b_state *state = get_ad9389b_state(sd);
+	/* read hotplug and rx-sense state */
+	u8 status = ad9389b_rd(sd, 0x42);
+
+	v4l2_dbg(1, debug, sd, "%s: status: 0x%x%s%s\n",
+			 __func__,
+			 status,
+			 status & MASK_AD9389B_HPD_DETECT ? ", hotplug" : "",
+			 status & MASK_AD9389B_MSEN_DETECT ? ", rx-sense" : "");
+
+	if ((status & MASK_AD9389B_HPD_DETECT) &&
+	    ((status & MASK_AD9389B_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;
+			ad9389b_set_isr(sd, true);
+			if (!ad9389b_s_power(sd, true)) {
+				v4l2_dbg(1, debug, sd,
+					"%s: monitor detected, powerup failed\n", __func__);
+				return;
+			}
+			ad9389b_setup(sd);
+			ad9389b_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_AD9389B_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_AD9389B_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;
+			ad9389b_notify_monitor_detect(sd);
+		}
+		ad9389b_s_power(sd, false);
+		memset(&state->edid, 0, sizeof(struct ad9389b_state_edid));
+	}
+
+	/* update read only ctrls */
+	v4l2_ctrl_s_ctrl(state->hotplug_ctrl, ad9389b_have_hotplug(sd) ? 0x1 : 0x0);
+	v4l2_ctrl_s_ctrl(state->rx_sense_ctrl, ad9389b_have_rx_sense(sd) ? 0x1 : 0x0);
+	v4l2_ctrl_s_ctrl(state->have_edid0_ctrl, state->edid.segments ? 0x1 : 0x0);
+}
+
+static bool edid_block_verify_crc(u8 *edid_block)
+{
+	int i;
+	u8 sum = 0;
+
+	for (i = 0; i < 127; i++)
+		sum += *(edid_block + i);
+	return ((255 - sum + 1) == edid_block[127]);
+}
+
+static bool edid_segment_verify_crc(struct v4l2_subdev *sd, u32 segment)
+{
+	struct ad9389b_state *state = get_ad9389b_state(sd);
+	u32 blocks = state->edid.blocks;
+	u8 *data = state->edid.data;
+
+	if (edid_block_verify_crc(&data[segment * 256])) {
+		if ((segment + 1) * 2 <= blocks)
+			return edid_block_verify_crc(&data[segment * 256 + 128]);
+		return true;
+	}
+	return false;
+}
+
+static bool ad9389b_check_edid_status(struct v4l2_subdev *sd)
+{
+	struct ad9389b_state *state = get_ad9389b_state(sd);
+	struct ad9389b_edid_detect ed;
+	int segment;
+	u8 edidRdy = ad9389b_rd(sd, 0xc5);
+
+	v4l2_dbg(1, debug, sd, "%s: edid ready (retries: %d)\n",
+			 __func__, EDID_MAX_RETRIES - state->edid.read_retries);
+
+	if (!(edidRdy & MASK_AD9389B_EDID_RDY))
+		return false;
+
+	segment = ad9389b_rd(sd, 0xc4);
+	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);
+	ad9389b_edid_rd(sd, 256, &state->edid.data[segment * 256]);
+	ad9389b_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 (!edid_segment_verify_crc(sd, segment)) {
+		/* edid crc error, force reread of edid segment */
+		ad9389b_s_power(sd, false);
+		ad9389b_s_power(sd, true);
+		return false;
+	}
+	/* one more segment read ok */
+	state->edid.segments = segment + 1;
+	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);
+		ad9389b_wr(sd, 0xc9, 0xf);
+		ad9389b_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;
+	}
+
+	/* report when we have all segments but report only for segment 0 */
+	ed.present = true;
+	ed.segment = 0;
+	v4l2_subdev_notify(sd, AD9389B_EDID_DETECT, (void *)&ed);
+	state->edid_detect_counter++;
+	v4l2_ctrl_s_ctrl(state->have_edid0_ctrl, state->edid.segments ? 0x1 : 0x0);
+	return ed.present;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static void ad9389b_init_setup(struct v4l2_subdev *sd)
+{
+	struct ad9389b_state *state = get_ad9389b_state(sd);
+	struct ad9389b_state_edid *edid = &state->edid;
+
+	v4l2_dbg(1, debug, sd, "%s\n", __func__);
+
+	/* clear all interrupts */
+	ad9389b_wr(sd, 0x96, 0xff);
+
+	memset(edid, 0, sizeof(struct ad9389b_state_edid));
+	state->have_monitor = false;
+	ad9389b_set_isr(sd, false);
+}
+
+static int ad9389b_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	const struct v4l2_dv_timings dv1080p60 = V4L2_DV_BT_CEA_1920X1080P60;
+	struct ad9389b_state *state;
+	struct ad9389b_platform_data *pdata = client->dev.platform_data;
+	struct v4l2_ctrl_handler *hdl;
+	struct v4l2_subdev *sd;
+	int err = -EIO;
+
+	/* 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 ad9389b client on address 0x%x\n",
+			client->addr << 1);
+
+	state = kzalloc(sizeof(struct ad9389b_state), GFP_KERNEL);
+	if (!state)
+		return -ENOMEM;
+
+	/* Platform data */
+	if (pdata == NULL) {
+		v4l_err(client, "No platform data!\n");
+		err = -ENODEV;
+		goto err_free;
+	}
+	memcpy(&state->pdata, pdata, sizeof(state->pdata));
+
+	sd = &state->sd;
+	v4l2_i2c_subdev_init(sd, client, &ad9389b_ops);
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	hdl = &state->hdl;
+	v4l2_ctrl_handler_init(hdl, 5);
+
+	/* private controls */
+
+	state->hdmi_mode_ctrl = v4l2_ctrl_new_std_menu(hdl, &ad9389b_ctrl_ops,
+			V4L2_CID_DV_TX_MODE, V4L2_DV_TX_MODE_HDMI,
+			0, V4L2_DV_TX_MODE_DVI_D);
+	state->hdmi_mode_ctrl->is_private = true;
+	state->hotplug_ctrl = v4l2_ctrl_new_std(hdl, NULL,
+			V4L2_CID_DV_TX_HOTPLUG, 0, 1, 0, 0);
+	state->hotplug_ctrl->is_private = true;
+	state->rx_sense_ctrl = v4l2_ctrl_new_std(hdl, NULL,
+			V4L2_CID_DV_TX_RXSENSE, 0, 1, 0, 0);
+	state->rx_sense_ctrl->is_private = true;
+	state->have_edid0_ctrl = v4l2_ctrl_new_std(hdl, NULL,
+			V4L2_CID_DV_TX_EDID_PRESENT, 0, 1, 0, 0);
+	state->have_edid0_ctrl->is_private = true;
+	state->rgb_quantization_range_ctrl =
+		v4l2_ctrl_new_std_menu(hdl, &ad9389b_ctrl_ops,
+			V4L2_CID_DV_TX_RGB_RANGE, V4L2_DV_RGB_RANGE_FULL,
+			0, V4L2_DV_RGB_RANGE_AUTO);
+	state->rgb_quantization_range_ctrl->is_private = true;
+	sd->ctrl_handler = hdl;
+	if (hdl->error) {
+		err = hdl->error;
+
+		goto err_hdl;
+	}
+
+	state->pad.flags = MEDIA_PAD_FL_SINK;
+	err = media_entity_init(&sd->entity, 1, &state->pad, 0);
+	if (err)
+		goto err_hdl;
+
+	state->chip_revision = ad9389b_rd(sd, 0x0);
+	if (state->chip_revision != 2) {
+		v4l2_err(sd, "chip_revision %d != 2\n", state->chip_revision);
+		err = -EIO;
+		goto err_entity;
+	}
+	v4l2_dbg(1, debug, sd, "reg 0x41 0x%x, chip version (reg 0x00) 0x%x\n",
+			ad9389b_rd(sd, 0x41), state->chip_revision);
+
+	state->edid_i2c_client = i2c_new_dummy(client->adapter, (0x7e>>1));
+	if (state->edid_i2c_client == NULL) {
+		v4l2_err(sd, "failed to register edid i2c client\n");
+		goto err_entity;
+	}
+
+	state->work_queue = create_singlethread_workqueue(sd->name);
+	if (state->work_queue == NULL) {
+		v4l2_err(sd, "could not create workqueue\n");
+		goto err_unreg;
+	}
+
+	INIT_DELAYED_WORK(&state->edid_handler, ad9389b_edid_handler);
+	state->dv_timings = dv1080p60;
+
+	ad9389b_init_setup(sd);
+	ad9389b_set_isr(sd, true);
+
+	v4l2_info(sd, "%s found @ 0x%x (%s)\n", client->name,
+			  client->addr << 1, client->adapter->name);
+	return 0;
+
+err_unreg:
+	i2c_unregister_device(state->edid_i2c_client);
+err_entity:
+	media_entity_cleanup(&sd->entity);
+err_hdl:
+	v4l2_ctrl_handler_free(&state->hdl);
+err_free:
+	kfree(state);
+	return err;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static int ad9389b_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ad9389b_state *state = get_ad9389b_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);
+
+	ad9389b_s_stream(sd, false);
+	ad9389b_s_audio_stream(sd, false);
+	ad9389b_init_setup(sd);
+	cancel_delayed_work(&state->edid_handler);
+	i2c_unregister_device(state->edid_i2c_client);
+	destroy_workqueue(state->work_queue);
+	v4l2_device_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+	kfree(get_ad9389b_state(sd));
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static struct i2c_device_id ad9389b_id[] = {
+	{ "ad9389b", V4L2_IDENT_AD9389B },
+	{ "ad9889b", V4L2_IDENT_AD9389B },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ad9389b_id);
+
+static struct i2c_driver ad9389b_driver = {
+	.driver = {
+		.owner = THIS_MODULE,
+		.name = "ad9389b",
+	},
+	.probe = ad9389b_probe,
+	.remove = ad9389b_remove,
+	.id_table = ad9389b_id,
+};
+
+module_i2c_driver(ad9389b_driver);
diff --git a/drivers/media/i2c/adp1653.c b/drivers/media/i2c/adp1653.c
new file mode 100644
index 000000000000..18a38b38fcb8
--- /dev/null
+++ b/drivers/media/i2c/adp1653.c
@@ -0,0 +1,487 @@
+/*
+ * drivers/media/i2c/adp1653.c
+ *
+ * Copyright (C) 2008--2011 Nokia Corporation
+ *
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>
+ *
+ * Contributors:
+ *	Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>
+ *	Tuukka Toivonen <tuukkat76@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ * 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 <media/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 (IS_ERR_VALUE(fault))
+		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 (IS_ERR_VALUE(rval))
+		return rval;
+
+	flash->led_mode->val = V4L2_FLASH_LED_MODE_NONE;
+
+	rval = adp1653_update_hw(flash);
+	if (IS_ERR_VALUE(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 (IS_ERR_VALUE(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 (IS_ERR_VALUE(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;
+
+	ret = flash->platform_data->power(&flash->subdev, on);
+	if (ret < 0)
+		return ret;
+
+	if (!on)
+		return 0;
+
+	ret = adp1653_init_device(flash);
+	if (ret < 0)
+		flash->platform_data->power(&flash->subdev, 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 i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	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 i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	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_probe(struct i2c_client *client,
+			 const struct i2c_device_id *devid)
+{
+	struct adp1653_flash *flash;
+	int ret;
+
+	/* we couldn't work without platform data */
+	if (client->dev.platform_data == NULL)
+		return -ENODEV;
+
+	flash = kzalloc(sizeof(*flash), GFP_KERNEL);
+	if (flash == NULL)
+		return -ENOMEM;
+
+	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_init(&flash->subdev.entity, 0, NULL, 0);
+	if (ret < 0)
+		goto free_and_quit;
+
+	flash->subdev.entity.type = MEDIA_ENT_T_V4L2_SUBDEV_FLASH;
+
+	return 0;
+
+free_and_quit:
+	v4l2_ctrl_handler_free(&flash->ctrls);
+	kfree(flash);
+	return ret;
+}
+
+static int __exit 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);
+	kfree(flash);
+	return 0;
+}
+
+static const struct i2c_device_id adp1653_id_table[] = {
+	{ ADP1653_NAME, 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, adp1653_id_table);
+
+static 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		= __exit_p(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 000000000000..6bc01fb98ff8
--- /dev/null
+++ b/drivers/media/i2c/adv7170.c
@@ -0,0 +1,410 @@
+/*
+ * 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)
+ *
+ * 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; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <linux/ioctl.h>
+#include <asm/uaccess.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.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 enum v4l2_mbus_pixelcode adv7170_codes[] = {
+	V4L2_MBUS_FMT_UYVY8_2X8,
+	V4L2_MBUS_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_fmt(struct v4l2_subdev *sd, unsigned int index,
+				enum v4l2_mbus_pixelcode *code)
+{
+	if (index >= ARRAY_SIZE(adv7170_codes))
+		return -EINVAL;
+
+	*code = adv7170_codes[index];
+	return 0;
+}
+
+static int adv7170_g_fmt(struct v4l2_subdev *sd,
+				struct v4l2_mbus_framefmt *mf)
+{
+	u8 val = adv7170_read(sd, 0x7);
+
+	if ((val & 0x40) == (1 << 6))
+		mf->code = V4L2_MBUS_FMT_UYVY8_1X16;
+	else
+		mf->code = V4L2_MBUS_FMT_UYVY8_2X8;
+
+	mf->colorspace  = V4L2_COLORSPACE_SMPTE170M;
+	mf->width       = 0;
+	mf->height      = 0;
+	mf->field       = V4L2_FIELD_ANY;
+
+	return 0;
+}
+
+static int adv7170_s_fmt(struct v4l2_subdev *sd,
+				struct v4l2_mbus_framefmt *mf)
+{
+	u8 val = adv7170_read(sd, 0x7);
+	int ret;
+
+	switch (mf->code) {
+	case V4L2_MBUS_FMT_UYVY8_2X8:
+		val &= ~0x40;
+		break;
+
+	case V4L2_MBUS_FMT_UYVY8_1X16:
+		val |= 0x40;
+		break;
+
+	default:
+		v4l2_dbg(1, debug, sd,
+			"illegal v4l2_mbus_framefmt code: %d\n", mf->code);
+		return -EINVAL;
+	}
+
+	ret = adv7170_write(sd, 0x7, val);
+
+	return ret;
+}
+
+static int adv7170_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ident *chip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_ADV7170, 0);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_core_ops adv7170_core_ops = {
+	.g_chip_ident = adv7170_g_chip_ident,
+};
+
+static const struct v4l2_subdev_video_ops adv7170_video_ops = {
+	.s_std_output = adv7170_s_std_output,
+	.s_routing = adv7170_s_routing,
+	.s_mbus_fmt = adv7170_s_fmt,
+	.g_mbus_fmt = adv7170_g_fmt,
+	.enum_mbus_fmt  = adv7170_enum_fmt,
+};
+
+static const struct v4l2_subdev_ops adv7170_ops = {
+	.core = &adv7170_core_ops,
+	.video = &adv7170_video_ops,
+};
+
+/* ----------------------------------------------------------------------- */
+
+static int adv7170_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	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 = kzalloc(sizeof(struct adv7170), 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 int adv7170_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	kfree(to_adv7170(sd));
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+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 = {
+		.owner	= THIS_MODULE,
+		.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 000000000000..c7640fab5730
--- /dev/null
+++ b/drivers/media/i2c/adv7175.c
@@ -0,0 +1,460 @@
+/*
+ *  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)
+ *
+ * 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; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <linux/ioctl.h>
+#include <asm/uaccess.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.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 enum v4l2_mbus_pixelcode adv7175_codes[] = {
+	V4L2_MBUS_FMT_UYVY8_2X8,
+	V4L2_MBUS_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 syncronized 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_fmt(struct v4l2_subdev *sd, unsigned int index,
+				enum v4l2_mbus_pixelcode *code)
+{
+	if (index >= ARRAY_SIZE(adv7175_codes))
+		return -EINVAL;
+
+	*code = adv7175_codes[index];
+	return 0;
+}
+
+static int adv7175_g_fmt(struct v4l2_subdev *sd,
+				struct v4l2_mbus_framefmt *mf)
+{
+	u8 val = adv7175_read(sd, 0x7);
+
+	if ((val & 0x40) == (1 << 6))
+		mf->code = V4L2_MBUS_FMT_UYVY8_1X16;
+	else
+		mf->code = V4L2_MBUS_FMT_UYVY8_2X8;
+
+	mf->colorspace  = V4L2_COLORSPACE_SMPTE170M;
+	mf->width       = 0;
+	mf->height      = 0;
+	mf->field       = V4L2_FIELD_ANY;
+
+	return 0;
+}
+
+static int adv7175_s_fmt(struct v4l2_subdev *sd,
+				struct v4l2_mbus_framefmt *mf)
+{
+	u8 val = adv7175_read(sd, 0x7);
+	int ret;
+
+	switch (mf->code) {
+	case V4L2_MBUS_FMT_UYVY8_2X8:
+		val &= ~0x40;
+		break;
+
+	case V4L2_MBUS_FMT_UYVY8_1X16:
+		val |= 0x40;
+		break;
+
+	default:
+		v4l2_dbg(1, debug, sd,
+			"illegal v4l2_mbus_framefmt code: %d\n", mf->code);
+		return -EINVAL;
+	}
+
+	ret = adv7175_write(sd, 0x7, val);
+
+	return ret;
+}
+
+static int adv7175_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ident *chip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_ADV7175, 0);
+}
+
+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 = {
+	.g_chip_ident = adv7175_g_chip_ident,
+	.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,
+	.s_mbus_fmt = adv7175_s_fmt,
+	.g_mbus_fmt = adv7175_g_fmt,
+	.enum_mbus_fmt  = adv7175_enum_fmt,
+};
+
+static const struct v4l2_subdev_ops adv7175_ops = {
+	.core = &adv7175_core_ops,
+	.video = &adv7175_video_ops,
+};
+
+/* ----------------------------------------------------------------------- */
+
+static int adv7175_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	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 = kzalloc(sizeof(struct adv7175), 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 int adv7175_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	kfree(to_adv7175(sd));
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+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 = {
+		.owner	= THIS_MODULE,
+		.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 000000000000..45ecf8db1eae
--- /dev/null
+++ b/drivers/media/i2c/adv7180.c
@@ -0,0 +1,667 @@
+/*
+ * adv7180.c Analog Devices ADV7180 video decoder driver
+ * Copyright (c) 2009 Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#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 <media/v4l2-ioctl.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-chip-ident.h>
+#include <linux/mutex.h>
+
+#define ADV7180_INPUT_CONTROL_REG			0x00
+#define ADV7180_INPUT_CONTROL_AD_PAL_BG_NTSC_J_SECAM	0x00
+#define ADV7180_INPUT_CONTROL_AD_PAL_BG_NTSC_J_SECAM_PED 0x10
+#define ADV7180_INPUT_CONTROL_AD_PAL_N_NTSC_J_SECAM	0x20
+#define ADV7180_INPUT_CONTROL_AD_PAL_N_NTSC_M_SECAM	0x30
+#define ADV7180_INPUT_CONTROL_NTSC_J			0x40
+#define ADV7180_INPUT_CONTROL_NTSC_M			0x50
+#define ADV7180_INPUT_CONTROL_PAL60			0x60
+#define ADV7180_INPUT_CONTROL_NTSC_443			0x70
+#define ADV7180_INPUT_CONTROL_PAL_BG			0x80
+#define ADV7180_INPUT_CONTROL_PAL_N			0x90
+#define ADV7180_INPUT_CONTROL_PAL_M			0xa0
+#define ADV7180_INPUT_CONTROL_PAL_M_PED			0xb0
+#define ADV7180_INPUT_CONTROL_PAL_COMB_N		0xc0
+#define ADV7180_INPUT_CONTROL_PAL_COMB_N_PED		0xd0
+#define ADV7180_INPUT_CONTROL_PAL_SECAM			0xe0
+#define ADV7180_INPUT_CONTROL_PAL_SECAM_PED		0xf0
+#define ADV7180_INPUT_CONTROL_INSEL_MASK		0x0f
+
+#define ADV7180_EXTENDED_OUTPUT_CONTROL_REG		0x04
+#define ADV7180_EXTENDED_OUTPUT_CONTROL_NTSCDIS		0xC5
+
+#define ADV7180_AUTODETECT_ENABLE_REG			0x07
+#define ADV7180_AUTODETECT_DEFAULT			0x7f
+/* Contrast */
+#define ADV7180_CON_REG		0x08	/*Unsigned */
+#define ADV7180_CON_MIN		0
+#define ADV7180_CON_DEF		128
+#define ADV7180_CON_MAX		255
+/* Brightness*/
+#define ADV7180_BRI_REG		0x0a	/*Signed */
+#define ADV7180_BRI_MIN		-128
+#define ADV7180_BRI_DEF		0
+#define ADV7180_BRI_MAX		127
+/* Hue */
+#define ADV7180_HUE_REG		0x0b	/*Signed, inverted */
+#define ADV7180_HUE_MIN		-127
+#define ADV7180_HUE_DEF		0
+#define ADV7180_HUE_MAX		128
+
+#define ADV7180_ADI_CTRL_REG				0x0e
+#define ADV7180_ADI_CTRL_IRQ_SPACE			0x20
+
+#define ADV7180_PWR_MAN_REG		0x0f
+#define ADV7180_PWR_MAN_ON		0x04
+#define ADV7180_PWR_MAN_OFF		0x24
+#define ADV7180_PWR_MAN_RES		0x80
+
+#define ADV7180_STATUS1_REG				0x10
+#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_IDENT_REG 0x11
+#define ADV7180_ID_7180 0x18
+
+#define ADV7180_ICONF1_ADI		0x40
+#define ADV7180_ICONF1_ACTIVE_LOW	0x01
+#define ADV7180_ICONF1_PSYNC_ONLY	0x10
+#define ADV7180_ICONF1_ACTIVE_TO_CLR	0xC0
+/* Saturation */
+#define ADV7180_SD_SAT_CB_REG	0xe3	/*Unsigned */
+#define ADV7180_SD_SAT_CR_REG	0xe4	/*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_ISR1_ADI	0x42
+#define ADV7180_ICR1_ADI	0x43
+#define ADV7180_IMR1_ADI	0x44
+#define ADV7180_IMR2_ADI	0x48
+#define ADV7180_IRQ3_AD_CHANGE	0x08
+#define ADV7180_ISR3_ADI	0x4A
+#define ADV7180_ICR3_ADI	0x4B
+#define ADV7180_IMR3_ADI	0x4C
+#define ADV7180_IMR4_ADI	0x50
+
+#define ADV7180_NTSC_V_BIT_END_REG	0xE6
+#define ADV7180_NTSC_V_BIT_END_MANUAL_NVEND	0x4F
+
+struct adv7180_state {
+	struct v4l2_ctrl_handler ctrl_hdl;
+	struct v4l2_subdev	sd;
+	struct work_struct	work;
+	struct mutex		mutex; /* mutual excl. when accessing chip */
+	int			irq;
+	v4l2_std_id		curr_norm;
+	bool			autodetect;
+	u8			input;
+};
+#define to_adv7180_sd(_ctrl) (&container_of(_ctrl->handler,		\
+					    struct adv7180_state,	\
+					    ctrl_hdl)->sd)
+
+static v4l2_std_id adv7180_std_to_v4l2(u8 status1)
+{
+	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_INPUT_CONTROL_PAL60;
+	if (std == V4L2_STD_NTSC_443)
+		return ADV7180_INPUT_CONTROL_NTSC_443;
+	if (std == V4L2_STD_PAL_N)
+		return ADV7180_INPUT_CONTROL_PAL_N;
+	if (std == V4L2_STD_PAL_M)
+		return ADV7180_INPUT_CONTROL_PAL_M;
+	if (std == V4L2_STD_PAL_Nc)
+		return ADV7180_INPUT_CONTROL_PAL_COMB_N;
+
+	if (std & V4L2_STD_PAL)
+		return ADV7180_INPUT_CONTROL_PAL_BG;
+	if (std & V4L2_STD_NTSC)
+		return ADV7180_INPUT_CONTROL_NTSC_M;
+	if (std & V4L2_STD_SECAM)
+		return ADV7180_INPUT_CONTROL_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 i2c_client *client, u32 *status,
+			    v4l2_std_id *std)
+{
+	int status1 = i2c_smbus_read_byte_data(client, ADV7180_STATUS1_REG);
+
+	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;
+
+	/* when we are interrupt driven we know the state */
+	if (!state->autodetect || state->irq > 0)
+		*std = state->curr_norm;
+	else
+		err = __adv7180_status(v4l2_get_subdevdata(sd), NULL, std);
+
+	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);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (ret)
+		return ret;
+
+	/* We cannot discriminate between LQFP and 40-pin LFCSP, so accept
+	 * all inputs and let the card driver take care of validation
+	 */
+	if ((input & ADV7180_INPUT_CONTROL_INSEL_MASK) != input)
+		goto out;
+
+	ret = i2c_smbus_read_byte_data(client, ADV7180_INPUT_CONTROL_REG);
+
+	if (ret < 0)
+		goto out;
+
+	ret &= ~ADV7180_INPUT_CONTROL_INSEL_MASK;
+	ret = i2c_smbus_write_byte_data(client,
+					ADV7180_INPUT_CONTROL_REG, ret | input);
+	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(v4l2_get_subdevdata(sd), status, NULL);
+	mutex_unlock(&state->mutex);
+	return ret;
+}
+
+static int adv7180_g_chip_ident(struct v4l2_subdev *sd,
+				struct v4l2_dbg_chip_ident *chip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_ADV7180, 0);
+}
+
+static int adv7180_s_std(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct adv7180_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = mutex_lock_interruptible(&state->mutex);
+	if (ret)
+		return ret;
+
+	/* all standards -> autodetect */
+	if (std == V4L2_STD_ALL) {
+		ret =
+		    i2c_smbus_write_byte_data(client, ADV7180_INPUT_CONTROL_REG,
+				ADV7180_INPUT_CONTROL_AD_PAL_BG_NTSC_J_SECAM
+					      | state->input);
+		if (ret < 0)
+			goto out;
+
+		__adv7180_status(client, NULL, &state->curr_norm);
+		state->autodetect = true;
+	} else {
+		ret = v4l2_std_to_adv7180(std);
+		if (ret < 0)
+			goto out;
+
+		ret = i2c_smbus_write_byte_data(client,
+						ADV7180_INPUT_CONTROL_REG,
+						ret | state->input);
+		if (ret < 0)
+			goto out;
+
+		state->curr_norm = std;
+		state->autodetect = false;
+	}
+	ret = 0;
+out:
+	mutex_unlock(&state->mutex);
+	return ret;
+}
+
+static int adv7180_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_adv7180_sd(ctrl);
+	struct adv7180_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(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 = i2c_smbus_write_byte_data(client, ADV7180_BRI_REG, val);
+		break;
+	case V4L2_CID_HUE:
+		/*Hue is inverted according to HSL chart */
+		ret = i2c_smbus_write_byte_data(client, ADV7180_HUE_REG, -val);
+		break;
+	case V4L2_CID_CONTRAST:
+		ret = i2c_smbus_write_byte_data(client, ADV7180_CON_REG, 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 = i2c_smbus_write_byte_data(client, ADV7180_SD_SAT_CB_REG,
+						val);
+		if (ret < 0)
+			break;
+		ret = i2c_smbus_write_byte_data(client, ADV7180_SD_SAT_CR_REG,
+						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 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);
+	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 const struct v4l2_subdev_video_ops adv7180_video_ops = {
+	.querystd = adv7180_querystd,
+	.g_input_status = adv7180_g_input_status,
+	.s_routing = adv7180_s_routing,
+};
+
+static const struct v4l2_subdev_core_ops adv7180_core_ops = {
+	.g_chip_ident = adv7180_g_chip_ident,
+	.s_std = adv7180_s_std,
+	.queryctrl = v4l2_subdev_queryctrl,
+	.g_ctrl = v4l2_subdev_g_ctrl,
+	.s_ctrl = v4l2_subdev_s_ctrl,
+};
+
+static const struct v4l2_subdev_ops adv7180_ops = {
+	.core = &adv7180_core_ops,
+	.video = &adv7180_video_ops,
+};
+
+static void adv7180_work(struct work_struct *work)
+{
+	struct adv7180_state *state = container_of(work, struct adv7180_state,
+						   work);
+	struct i2c_client *client = v4l2_get_subdevdata(&state->sd);
+	u8 isr3;
+
+	mutex_lock(&state->mutex);
+	i2c_smbus_write_byte_data(client, ADV7180_ADI_CTRL_REG,
+				  ADV7180_ADI_CTRL_IRQ_SPACE);
+	isr3 = i2c_smbus_read_byte_data(client, ADV7180_ISR3_ADI);
+	/* clear */
+	i2c_smbus_write_byte_data(client, ADV7180_ICR3_ADI, isr3);
+	i2c_smbus_write_byte_data(client, ADV7180_ADI_CTRL_REG, 0);
+
+	if (isr3 & ADV7180_IRQ3_AD_CHANGE && state->autodetect)
+		__adv7180_status(client, NULL, &state->curr_norm);
+	mutex_unlock(&state->mutex);
+
+	enable_irq(state->irq);
+}
+
+static irqreturn_t adv7180_irq(int irq, void *devid)
+{
+	struct adv7180_state *state = devid;
+
+	schedule_work(&state->work);
+
+	disable_irq_nosync(state->irq);
+
+	return IRQ_HANDLED;
+}
+
+static int init_device(struct i2c_client *client, struct adv7180_state *state)
+{
+	int ret;
+
+	/* Initialize adv7180 */
+	/* Enable autodetection */
+	if (state->autodetect) {
+		ret =
+		    i2c_smbus_write_byte_data(client, ADV7180_INPUT_CONTROL_REG,
+				ADV7180_INPUT_CONTROL_AD_PAL_BG_NTSC_J_SECAM
+					      | state->input);
+		if (ret < 0)
+			return ret;
+
+		ret =
+		    i2c_smbus_write_byte_data(client,
+					      ADV7180_AUTODETECT_ENABLE_REG,
+					      ADV7180_AUTODETECT_DEFAULT);
+		if (ret < 0)
+			return ret;
+	} else {
+		ret = v4l2_std_to_adv7180(state->curr_norm);
+		if (ret < 0)
+			return ret;
+
+		ret =
+		    i2c_smbus_write_byte_data(client, ADV7180_INPUT_CONTROL_REG,
+					      ret | state->input);
+		if (ret < 0)
+			return ret;
+
+	}
+	/* ITU-R BT.656-4 compatible */
+	ret = i2c_smbus_write_byte_data(client,
+			ADV7180_EXTENDED_OUTPUT_CONTROL_REG,
+			ADV7180_EXTENDED_OUTPUT_CONTROL_NTSCDIS);
+	if (ret < 0)
+		return ret;
+
+	/* Manually set V bit end position in NTSC mode */
+	ret = i2c_smbus_write_byte_data(client,
+					ADV7180_NTSC_V_BIT_END_REG,
+					ADV7180_NTSC_V_BIT_END_MANUAL_NVEND);
+	if (ret < 0)
+		return ret;
+
+	/* read current norm */
+	__adv7180_status(client, NULL, &state->curr_norm);
+
+	/* register for interrupts */
+	if (state->irq > 0) {
+		ret = request_irq(state->irq, adv7180_irq, 0, KBUILD_MODNAME,
+				  state);
+		if (ret)
+			return ret;
+
+		ret = i2c_smbus_write_byte_data(client, ADV7180_ADI_CTRL_REG,
+						ADV7180_ADI_CTRL_IRQ_SPACE);
+		if (ret < 0)
+			return ret;
+
+		/* config the Interrupt pin to be active low */
+		ret = i2c_smbus_write_byte_data(client, ADV7180_ICONF1_ADI,
+						ADV7180_ICONF1_ACTIVE_LOW |
+						ADV7180_ICONF1_PSYNC_ONLY);
+		if (ret < 0)
+			return ret;
+
+		ret = i2c_smbus_write_byte_data(client, ADV7180_IMR1_ADI, 0);
+		if (ret < 0)
+			return ret;
+
+		ret = i2c_smbus_write_byte_data(client, ADV7180_IMR2_ADI, 0);
+		if (ret < 0)
+			return ret;
+
+		/* enable AD change interrupts interrupts */
+		ret = i2c_smbus_write_byte_data(client, ADV7180_IMR3_ADI,
+						ADV7180_IRQ3_AD_CHANGE);
+		if (ret < 0)
+			return ret;
+
+		ret = i2c_smbus_write_byte_data(client, ADV7180_IMR4_ADI, 0);
+		if (ret < 0)
+			return ret;
+
+		ret = i2c_smbus_write_byte_data(client, ADV7180_ADI_CTRL_REG,
+						0);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+static __devinit int adv7180_probe(struct i2c_client *client,
+				   const struct i2c_device_id *id)
+{
+	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;
+
+	v4l_info(client, "chip found @ 0x%02x (%s)\n",
+		 client->addr, client->adapter->name);
+
+	state = kzalloc(sizeof(struct adv7180_state), GFP_KERNEL);
+	if (state == NULL) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	state->irq = client->irq;
+	INIT_WORK(&state->work, adv7180_work);
+	mutex_init(&state->mutex);
+	state->autodetect = true;
+	state->input = 0;
+	sd = &state->sd;
+	v4l2_i2c_subdev_init(sd, client, &adv7180_ops);
+
+	ret = adv7180_init_controls(state);
+	if (ret)
+		goto err_unreg_subdev;
+	ret = init_device(client, state);
+	if (ret)
+		goto err_free_ctrl;
+	return 0;
+
+err_free_ctrl:
+	adv7180_exit_controls(state);
+err_unreg_subdev:
+	mutex_destroy(&state->mutex);
+	v4l2_device_unregister_subdev(sd);
+	kfree(state);
+err:
+	printk(KERN_ERR KBUILD_MODNAME ": Failed to probe: %d\n", ret);
+	return ret;
+}
+
+static __devexit int adv7180_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct adv7180_state *state = to_state(sd);
+
+	if (state->irq > 0) {
+		free_irq(client->irq, state);
+		if (cancel_work_sync(&state->work)) {
+			/*
+			 * Work was pending, therefore we need to enable
+			 * IRQ here to balance the disable_irq() done in the
+			 * interrupt handler.
+			 */
+			enable_irq(state->irq);
+		}
+	}
+
+	mutex_destroy(&state->mutex);
+	v4l2_device_unregister_subdev(sd);
+	kfree(to_state(sd));
+	return 0;
+}
+
+static const struct i2c_device_id adv7180_id[] = {
+	{KBUILD_MODNAME, 0},
+	{},
+};
+
+#ifdef CONFIG_PM
+static int adv7180_suspend(struct i2c_client *client, pm_message_t state)
+{
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(client, ADV7180_PWR_MAN_REG,
+					ADV7180_PWR_MAN_OFF);
+	if (ret < 0)
+		return ret;
+	return 0;
+}
+
+static int adv7180_resume(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct adv7180_state *state = to_state(sd);
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(client, ADV7180_PWR_MAN_REG,
+					ADV7180_PWR_MAN_ON);
+	if (ret < 0)
+		return ret;
+	ret = init_device(client, state);
+	if (ret < 0)
+		return ret;
+	return 0;
+}
+#endif
+
+MODULE_DEVICE_TABLE(i2c, adv7180_id);
+
+static struct i2c_driver adv7180_driver = {
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = KBUILD_MODNAME,
+		   },
+	.probe = adv7180_probe,
+	.remove = __devexit_p(adv7180_remove),
+#ifdef CONFIG_PM
+	.suspend = adv7180_suspend,
+	.resume = adv7180_resume,
+#endif
+	.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 000000000000..e1d4c89d7140
--- /dev/null
+++ b/drivers/media/i2c/adv7183.c
@@ -0,0 +1,699 @@
+/*
+ * adv7183.c Analog Devices ADV7183 video decoder driver
+ *
+ * Copyright (c) 2011 Analog Devices Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/gpio.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/adv7183.h>
+#include <media/v4l2-chip-ident.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;
+	unsigned reset_pin;
+	unsigned 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 positon 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_fmt(struct v4l2_subdev *sd, unsigned index,
+				enum v4l2_mbus_pixelcode *code)
+{
+	if (index > 0)
+		return -EINVAL;
+
+	*code = V4L2_MBUS_FMT_UYVY8_2X8;
+	return 0;
+}
+
+static int adv7183_try_mbus_fmt(struct v4l2_subdev *sd,
+				struct v4l2_mbus_framefmt *fmt)
+{
+	struct adv7183 *decoder = to_adv7183(sd);
+
+	fmt->code = V4L2_MBUS_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;
+	}
+	return 0;
+}
+
+static int adv7183_s_mbus_fmt(struct v4l2_subdev *sd,
+				struct v4l2_mbus_framefmt *fmt)
+{
+	struct adv7183 *decoder = to_adv7183(sd);
+
+	adv7183_try_mbus_fmt(sd, fmt);
+	decoder->fmt = *fmt;
+	return 0;
+}
+
+static int adv7183_g_mbus_fmt(struct v4l2_subdev *sd,
+				struct v4l2_mbus_framefmt *fmt)
+{
+	struct adv7183 *decoder = to_adv7183(sd);
+
+	*fmt = decoder->fmt;
+	return 0;
+}
+
+static int adv7183_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct adv7183 *decoder = to_adv7183(sd);
+
+	if (enable)
+		gpio_direction_output(decoder->oe_pin, 0);
+	else
+		gpio_direction_output(decoder->oe_pin, 1);
+	udelay(1);
+	return 0;
+}
+
+static int adv7183_g_chip_ident(struct v4l2_subdev *sd,
+		struct v4l2_dbg_chip_ident *chip)
+{
+	int rev;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	/* 0x11 for adv7183, 0x13 for adv7183b */
+	rev = adv7183_read(sd, ADV7183_IDENT);
+
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_ADV7183, rev);
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int adv7183_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	reg->val = adv7183_read(sd, reg->reg & 0xff);
+	reg->size = 1;
+	return 0;
+}
+
+static int adv7183_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	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,
+	.g_std = adv7183_g_std,
+	.s_std = adv7183_s_std,
+	.reset = adv7183_reset,
+	.g_chip_ident = adv7183_g_chip_ident,
+#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 = {
+	.s_routing = adv7183_s_routing,
+	.querystd = adv7183_querystd,
+	.g_input_status = adv7183_g_input_status,
+	.enum_mbus_fmt = adv7183_enum_mbus_fmt,
+	.try_mbus_fmt = adv7183_try_mbus_fmt,
+	.s_mbus_fmt = adv7183_s_mbus_fmt,
+	.g_mbus_fmt = adv7183_g_mbus_fmt,
+	.s_stream = adv7183_s_stream,
+};
+
+static const struct v4l2_subdev_ops adv7183_ops = {
+	.core = &adv7183_core_ops,
+	.video = &adv7183_video_ops,
+};
+
+static int adv7183_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct adv7183 *decoder;
+	struct v4l2_subdev *sd;
+	struct v4l2_ctrl_handler *hdl;
+	int ret;
+	struct v4l2_mbus_framefmt fmt;
+	const unsigned *pin_array;
+
+	/* 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);
+
+	pin_array = client->dev.platform_data;
+	if (pin_array == NULL)
+		return -EINVAL;
+
+	decoder = kzalloc(sizeof(struct adv7183), GFP_KERNEL);
+	if (decoder == NULL)
+		return -ENOMEM;
+
+	decoder->reset_pin = pin_array[0];
+	decoder->oe_pin = pin_array[1];
+
+	if (gpio_request(decoder->reset_pin, "ADV7183 Reset")) {
+		v4l_err(client, "failed to request GPIO %d\n", decoder->reset_pin);
+		ret = -EBUSY;
+		goto err_free_decoder;
+	}
+
+	if (gpio_request(decoder->oe_pin, "ADV7183 Output Enable")) {
+		v4l_err(client, "failed to request GPIO %d\n", decoder->oe_pin);
+		ret = -EBUSY;
+		goto err_free_reset;
+	}
+
+	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);
+		goto err_free_oe;
+	}
+
+	/* 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;
+
+	gpio_direction_output(decoder->oe_pin, 1);
+	/* reset chip */
+	gpio_direction_output(decoder->reset_pin, 0);
+	/* reset pulse width at least 5ms */
+	mdelay(10);
+	gpio_direction_output(decoder->reset_pin, 1);
+	/* 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.width = 720;
+	fmt.height = 576;
+	adv7183_s_mbus_fmt(sd, &fmt);
+
+	/* initialize the hardware to the default control values */
+	ret = v4l2_ctrl_handler_setup(hdl);
+	if (ret) {
+		v4l2_ctrl_handler_free(hdl);
+		goto err_free_oe;
+	}
+
+	return 0;
+err_free_oe:
+	gpio_free(decoder->oe_pin);
+err_free_reset:
+	gpio_free(decoder->reset_pin);
+err_free_decoder:
+	kfree(decoder);
+	return ret;
+}
+
+static int adv7183_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct adv7183 *decoder = to_adv7183(sd);
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+	gpio_free(decoder->oe_pin);
+	gpio_free(decoder->reset_pin);
+	kfree(decoder);
+	return 0;
+}
+
+static const struct i2c_device_id adv7183_id[] = {
+	{"adv7183", 0},
+	{},
+};
+
+MODULE_DEVICE_TABLE(i2c, adv7183_id);
+
+static struct i2c_driver adv7183_driver = {
+	.driver = {
+		.owner  = THIS_MODULE,
+		.name   = "adv7183",
+	},
+	.probe          = adv7183_probe,
+	.remove         = __devexit_p(adv7183_remove),
+	.id_table       = adv7183_id,
+};
+
+static __init int adv7183_init(void)
+{
+	return i2c_add_driver(&adv7183_driver);
+}
+
+static __exit void adv7183_exit(void)
+{
+	i2c_del_driver(&adv7183_driver);
+}
+
+module_init(adv7183_init);
+module_exit(adv7183_exit);
+
+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 000000000000..4a5b7d211d2f
--- /dev/null
+++ b/drivers/media/i2c/adv7183_regs.h
@@ -0,0 +1,107 @@
+/*
+ * adv7183 - Analog Devices ADV7183 video decoder registers
+ *
+ * Copyright (c) 2011 Analog Devices Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#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 positon control 1 */
+#define ADV7183_HS_POS_CTRL_2      0x35 /* Hsync positon control 2 */
+#define ADV7183_HS_POS_CTRL_3      0x36 /* Hsync positon 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 swithc 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 000000000000..2b5aa676a84e
--- /dev/null
+++ b/drivers/media/i2c/adv7343.c
@@ -0,0 +1,476 @@
+/*
+ * 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 <media/adv7343.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.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;
+	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;
+
+	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;
+
+	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);
+	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 int adv7343_g_chip_ident(struct v4l2_subdev *sd,
+				struct v4l2_dbg_chip_ident *chip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_ADV7343, 0);
+}
+
+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,
+	.g_chip_ident = adv7343_g_chip_ident,
+	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
+	.try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
+	.s_ext_ctrls = v4l2_subdev_s_ext_ctrls,
+	.g_ctrl = v4l2_subdev_g_ctrl,
+	.s_ctrl = v4l2_subdev_s_ctrl,
+	.queryctrl = v4l2_subdev_queryctrl,
+	.querymenu = v4l2_subdev_querymenu,
+};
+
+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 int adv7343_probe(struct i2c_client *client,
+				const struct i2c_device_id *id)
+{
+	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 = kzalloc(sizeof(struct adv7343_state), GFP_KERNEL);
+	if (state == NULL)
+		return -ENOMEM;
+
+	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) {
+		int err = state->hdl.error;
+
+		v4l2_ctrl_handler_free(&state->hdl);
+		kfree(state);
+		return err;
+	}
+	v4l2_ctrl_handler_setup(&state->hdl);
+
+	err = adv7343_initialize(&state->sd);
+	if (err) {
+		v4l2_ctrl_handler_free(&state->hdl);
+		kfree(state);
+	}
+	return err;
+}
+
+static int adv7343_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct adv7343_state *state = to_state(sd);
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&state->hdl);
+	kfree(state);
+
+	return 0;
+}
+
+static const struct i2c_device_id adv7343_id[] = {
+	{"adv7343", 0},
+	{},
+};
+
+MODULE_DEVICE_TABLE(i2c, adv7343_id);
+
+static struct i2c_driver adv7343_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.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 000000000000..446606764346
--- /dev/null
+++ b/drivers/media/i2c/adv7343_regs.h
@@ -0,0 +1,181 @@
+/*
+ * ADV7343 encoder related structure and register definitions
+ *
+ * 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.
+ */
+
+#ifndef ADV7343_REG_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 000000000000..3dc6098c7267
--- /dev/null
+++ b/drivers/media/i2c/adv7393.c
@@ -0,0 +1,487 @@
+/*
+ * 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/adv7393.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.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 int adv7393_g_chip_ident(struct v4l2_subdev *sd,
+				struct v4l2_dbg_chip_ident *chip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_ADV7393, 0);
+}
+
+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,
+	.g_chip_ident = adv7393_g_chip_ident,
+	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
+	.try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
+	.s_ext_ctrls = v4l2_subdev_s_ext_ctrls,
+	.g_ctrl = v4l2_subdev_g_ctrl,
+	.s_ctrl = v4l2_subdev_s_ctrl,
+	.queryctrl = v4l2_subdev_queryctrl,
+	.querymenu = v4l2_subdev_querymenu,
+};
+
+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,
+				const struct i2c_device_id *id)
+{
+	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 = kzalloc(sizeof(struct adv7393_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);
+		kfree(state);
+		return err;
+	}
+	v4l2_ctrl_handler_setup(&state->hdl);
+
+	err = adv7393_initialize(&state->sd);
+	if (err) {
+		v4l2_ctrl_handler_free(&state->hdl);
+		kfree(state);
+	}
+	return err;
+}
+
+static int 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);
+	kfree(state);
+
+	return 0;
+}
+
+static const struct i2c_device_id adv7393_id[] = {
+	{"adv7393", 0},
+	{},
+};
+MODULE_DEVICE_TABLE(i2c, adv7393_id);
+
+static struct i2c_driver adv7393_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.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 000000000000..78968330f0be
--- /dev/null
+++ b/drivers/media/i2c/adv7393_regs.h
@@ -0,0 +1,188 @@
+/*
+ * 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/
+ *
+ * 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 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/adv7604.c b/drivers/media/i2c/adv7604.c
new file mode 100644
index 000000000000..109bc9b12e74
--- /dev/null
+++ b/drivers/media/i2c/adv7604.c
@@ -0,0 +1,1959 @@
+/*
+ * adv7604 - Analog Devices ADV7604 video decoder driver
+ *
+ * Copyright 2012 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * 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.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ */
+
+/*
+ * 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/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 <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/adv7604.h>
+
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "debug level (0-2)");
+
+MODULE_DESCRIPTION("Analog Devices ADV7604 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 ADV7604_fsc (28636360)
+
+#define DIGITAL_INPUT ((state->prim_mode == ADV7604_PRIM_MODE_HDMI_COMP) || \
+			(state->prim_mode == ADV7604_PRIM_MODE_HDMI_GR))
+
+/*
+ **********************************************************************
+ *
+ *  Arrays with configuration parameters for the ADV7604
+ *
+ **********************************************************************
+ */
+struct adv7604_state {
+	struct adv7604_platform_data pdata;
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct v4l2_ctrl_handler hdl;
+	enum adv7604_prim_mode prim_mode;
+	struct v4l2_dv_timings timings;
+	u8 edid[256];
+	unsigned edid_blocks;
+	struct v4l2_fract aspect_ratio;
+	u32 rgb_quantization_range;
+	struct workqueue_struct *work_queues;
+	struct delayed_work delayed_work_enable_hotplug;
+	bool connector_hdmi;
+
+	/* i2c clients */
+	struct i2c_client *i2c_avlink;
+	struct i2c_client *i2c_cec;
+	struct i2c_client *i2c_infoframe;
+	struct i2c_client *i2c_esdp;
+	struct i2c_client *i2c_dpp;
+	struct i2c_client *i2c_afe;
+	struct i2c_client *i2c_repeater;
+	struct i2c_client *i2c_edid;
+	struct i2c_client *i2c_hdmi;
+	struct i2c_client *i2c_test;
+	struct i2c_client *i2c_cp;
+	struct i2c_client *i2c_vdp;
+
+	/* 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;
+};
+
+/* Supported CEA and DMT timings */
+static const struct v4l2_dv_timings adv7604_timings[] = {
+	V4L2_DV_BT_CEA_720X480P59_94,
+	V4L2_DV_BT_CEA_720X576P50,
+	V4L2_DV_BT_CEA_1280X720P24,
+	V4L2_DV_BT_CEA_1280X720P25,
+	V4L2_DV_BT_CEA_1280X720P30,
+	V4L2_DV_BT_CEA_1280X720P50,
+	V4L2_DV_BT_CEA_1280X720P60,
+	V4L2_DV_BT_CEA_1920X1080P24,
+	V4L2_DV_BT_CEA_1920X1080P25,
+	V4L2_DV_BT_CEA_1920X1080P30,
+	V4L2_DV_BT_CEA_1920X1080P50,
+	V4L2_DV_BT_CEA_1920X1080P60,
+
+	V4L2_DV_BT_DMT_640X350P85,
+	V4L2_DV_BT_DMT_640X400P85,
+	V4L2_DV_BT_DMT_720X400P85,
+	V4L2_DV_BT_DMT_640X480P60,
+	V4L2_DV_BT_DMT_640X480P72,
+	V4L2_DV_BT_DMT_640X480P75,
+	V4L2_DV_BT_DMT_640X480P85,
+	V4L2_DV_BT_DMT_800X600P56,
+	V4L2_DV_BT_DMT_800X600P60,
+	V4L2_DV_BT_DMT_800X600P72,
+	V4L2_DV_BT_DMT_800X600P75,
+	V4L2_DV_BT_DMT_800X600P85,
+	V4L2_DV_BT_DMT_848X480P60,
+	V4L2_DV_BT_DMT_1024X768P60,
+	V4L2_DV_BT_DMT_1024X768P70,
+	V4L2_DV_BT_DMT_1024X768P75,
+	V4L2_DV_BT_DMT_1024X768P85,
+	V4L2_DV_BT_DMT_1152X864P75,
+	V4L2_DV_BT_DMT_1280X768P60_RB,
+	V4L2_DV_BT_DMT_1280X768P60,
+	V4L2_DV_BT_DMT_1280X768P75,
+	V4L2_DV_BT_DMT_1280X768P85,
+	V4L2_DV_BT_DMT_1280X800P60_RB,
+	V4L2_DV_BT_DMT_1280X800P60,
+	V4L2_DV_BT_DMT_1280X800P75,
+	V4L2_DV_BT_DMT_1280X800P85,
+	V4L2_DV_BT_DMT_1280X960P60,
+	V4L2_DV_BT_DMT_1280X960P85,
+	V4L2_DV_BT_DMT_1280X1024P60,
+	V4L2_DV_BT_DMT_1280X1024P75,
+	V4L2_DV_BT_DMT_1280X1024P85,
+	V4L2_DV_BT_DMT_1360X768P60,
+	V4L2_DV_BT_DMT_1400X1050P60_RB,
+	V4L2_DV_BT_DMT_1400X1050P60,
+	V4L2_DV_BT_DMT_1400X1050P75,
+	V4L2_DV_BT_DMT_1400X1050P85,
+	V4L2_DV_BT_DMT_1440X900P60_RB,
+	V4L2_DV_BT_DMT_1440X900P60,
+	V4L2_DV_BT_DMT_1600X1200P60,
+	V4L2_DV_BT_DMT_1680X1050P60_RB,
+	V4L2_DV_BT_DMT_1680X1050P60,
+	V4L2_DV_BT_DMT_1792X1344P60,
+	V4L2_DV_BT_DMT_1856X1392P60,
+	V4L2_DV_BT_DMT_1920X1200P60_RB,
+	V4L2_DV_BT_DMT_1366X768P60,
+	V4L2_DV_BT_DMT_1920X1080P60,
+	{ },
+};
+
+/* ----------------------------------------------------------------------- */
+
+static inline struct adv7604_state *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct adv7604_state, sd);
+}
+
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct adv7604_state, hdl)->sd;
+}
+
+static inline unsigned hblanking(const struct v4l2_bt_timings *t)
+{
+	return t->hfrontporch + t->hsync + t->hbackporch;
+}
+
+static inline unsigned htotal(const struct v4l2_bt_timings *t)
+{
+	return t->width + t->hfrontporch + t->hsync + t->hbackporch;
+}
+
+static inline unsigned vblanking(const struct v4l2_bt_timings *t)
+{
+	return t->vfrontporch + t->vsync + t->vbackporch;
+}
+
+static inline unsigned vtotal(const struct v4l2_bt_timings *t)
+{
+	return t->height + t->vfrontporch + t->vsync + t->vbackporch;
+}
+
+/* ----------------------------------------------------------------------- */
+
+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)
+{
+	return adv_smbus_read_byte_data_check(client, command, true);
+}
+
+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 s32 adv_smbus_write_i2c_block_data(struct i2c_client *client,
+	       u8 command, unsigned length, const u8 *values)
+{
+	union i2c_smbus_data data;
+
+	if (length > I2C_SMBUS_BLOCK_MAX)
+		length = I2C_SMBUS_BLOCK_MAX;
+	data.block[0] = length;
+	memcpy(data.block + 1, values, length);
+	return i2c_smbus_xfer(client->adapter, client->addr, client->flags,
+			      I2C_SMBUS_WRITE, command,
+			      I2C_SMBUS_I2C_BLOCK_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 avlink_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct adv7604_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 adv7604_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 adv7604_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 adv7604_state *state = to_state(sd);
+
+	return adv_smbus_write_byte_data(state->i2c_cec, reg, val);
+}
+
+static inline int cec_write_and_or(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 adv7604_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 adv7604_state *state = to_state(sd);
+
+	return adv_smbus_write_byte_data(state->i2c_infoframe, reg, val);
+}
+
+static inline int esdp_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct adv7604_state *state = to_state(sd);
+
+	return adv_smbus_read_byte_data(state->i2c_esdp, reg);
+}
+
+static inline int esdp_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct adv7604_state *state = to_state(sd);
+
+	return adv_smbus_write_byte_data(state->i2c_esdp, reg, val);
+}
+
+static inline int dpp_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct adv7604_state *state = to_state(sd);
+
+	return adv_smbus_read_byte_data(state->i2c_dpp, reg);
+}
+
+static inline int dpp_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct adv7604_state *state = to_state(sd);
+
+	return adv_smbus_write_byte_data(state->i2c_dpp, reg, val);
+}
+
+static inline int afe_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct adv7604_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 adv7604_state *state = to_state(sd);
+
+	return adv_smbus_write_byte_data(state->i2c_afe, reg, val);
+}
+
+static inline int rep_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct adv7604_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 adv7604_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 adv7604_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 adv7604_state *state = to_state(sd);
+
+	return adv_smbus_write_byte_data(state->i2c_edid, reg, val);
+}
+
+static inline int edid_read_block(struct v4l2_subdev *sd, unsigned len, u8 *val)
+{
+	struct adv7604_state *state = to_state(sd);
+	struct i2c_client *client = state->i2c_edid;
+	u8 msgbuf0[1] = { 0 };
+	u8 msgbuf1[256];
+	struct i2c_msg msg[2] = { { client->addr, 0, 1, msgbuf0 },
+				  { client->addr, 0 | I2C_M_RD, len, msgbuf1 }
+				};
+
+	if (i2c_transfer(client->adapter, msg, 2) < 0)
+		return -EIO;
+	memcpy(val, msgbuf1, len);
+	return 0;
+}
+
+static void adv7604_delayed_work_enable_hotplug(struct work_struct *work)
+{
+	struct delayed_work *dwork = to_delayed_work(work);
+	struct adv7604_state *state = container_of(dwork, struct adv7604_state,
+						delayed_work_enable_hotplug);
+	struct v4l2_subdev *sd = &state->sd;
+
+	v4l2_dbg(2, debug, sd, "%s: enable hotplug\n", __func__);
+
+	v4l2_subdev_notify(sd, ADV7604_HOTPLUG, (void *)1);
+}
+
+static inline int edid_write_block(struct v4l2_subdev *sd,
+					unsigned len, const u8 *val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct adv7604_state *state = to_state(sd);
+	int err = 0;
+	int i;
+
+	v4l2_dbg(2, debug, sd, "%s: write EDID block (%d byte)\n", __func__, len);
+
+	v4l2_subdev_notify(sd, ADV7604_HOTPLUG, (void *)0);
+
+	/* Disables I2C access to internal EDID ram from DDC port */
+	rep_write_and_or(sd, 0x77, 0xf0, 0x0);
+
+	for (i = 0; !err && i < len; i += I2C_SMBUS_BLOCK_MAX)
+		err = adv_smbus_write_i2c_block_data(state->i2c_edid, i,
+				I2C_SMBUS_BLOCK_MAX, val + i);
+	if (err)
+		return err;
+
+	/* adv7604 calculates the checksums and enables I2C access to internal
+	   EDID ram from DDC port. */
+	rep_write_and_or(sd, 0x77, 0xf0, 0x1);
+
+	for (i = 0; i < 1000; i++) {
+		if (rep_read(sd, 0x7d) & 1)
+			break;
+		mdelay(1);
+	}
+	if (i == 1000) {
+		v4l_err(client, "error enabling edid\n");
+		return -EIO;
+	}
+
+	/* enable hotplug after 100 ms */
+	queue_delayed_work(state->work_queues,
+			&state->delayed_work_enable_hotplug, HZ / 10);
+	return 0;
+}
+
+static inline int hdmi_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct adv7604_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 adv7604_state *state = to_state(sd);
+
+	return adv_smbus_write_byte_data(state->i2c_hdmi, reg, val);
+}
+
+static inline int test_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct adv7604_state *state = to_state(sd);
+
+	return adv_smbus_read_byte_data(state->i2c_test, reg);
+}
+
+static inline int test_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct adv7604_state *state = to_state(sd);
+
+	return adv_smbus_write_byte_data(state->i2c_test, reg, val);
+}
+
+static inline int cp_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct adv7604_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 adv7604_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 adv7604_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 adv7604_state *state = to_state(sd);
+
+	return adv_smbus_write_byte_data(state->i2c_vdp, reg, val);
+}
+
+/* ----------------------------------------------------------------------- */
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static void adv7604_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 adv7604_g_register(struct v4l2_subdev *sd,
+					struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	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 = esdp_read(sd, reg->reg & 0xff);
+		break;
+	case 5:
+		reg->val = dpp_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 = test_read(sd, reg->reg & 0xff);
+		break;
+	case 0xb:
+		reg->val = cp_read(sd, reg->reg & 0xff);
+		break;
+	case 0xc:
+		reg->val = vdp_read(sd, reg->reg & 0xff);
+		break;
+	default:
+		v4l2_info(sd, "Register %03llx not supported\n", reg->reg);
+		adv7604_inv_register(sd);
+		break;
+	}
+	return 0;
+}
+
+static int adv7604_s_register(struct v4l2_subdev *sd,
+					struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	switch (reg->reg >> 8) {
+	case 0:
+		io_write(sd, reg->reg & 0xff, reg->val & 0xff);
+		break;
+	case 1:
+		avlink_write(sd, reg->reg & 0xff, reg->val & 0xff);
+		break;
+	case 2:
+		cec_write(sd, reg->reg & 0xff, reg->val & 0xff);
+		break;
+	case 3:
+		infoframe_write(sd, reg->reg & 0xff, reg->val & 0xff);
+		break;
+	case 4:
+		esdp_write(sd, reg->reg & 0xff, reg->val & 0xff);
+		break;
+	case 5:
+		dpp_write(sd, reg->reg & 0xff, reg->val & 0xff);
+		break;
+	case 6:
+		afe_write(sd, reg->reg & 0xff, reg->val & 0xff);
+		break;
+	case 7:
+		rep_write(sd, reg->reg & 0xff, reg->val & 0xff);
+		break;
+	case 8:
+		edid_write(sd, reg->reg & 0xff, reg->val & 0xff);
+		break;
+	case 9:
+		hdmi_write(sd, reg->reg & 0xff, reg->val & 0xff);
+		break;
+	case 0xa:
+		test_write(sd, reg->reg & 0xff, reg->val & 0xff);
+		break;
+	case 0xb:
+		cp_write(sd, reg->reg & 0xff, reg->val & 0xff);
+		break;
+	case 0xc:
+		vdp_write(sd, reg->reg & 0xff, reg->val & 0xff);
+		break;
+	default:
+		v4l2_info(sd, "Register %03llx not supported\n", reg->reg);
+		adv7604_inv_register(sd);
+		break;
+	}
+	return 0;
+}
+#endif
+
+static int adv7604_s_detect_tx_5v_ctrl(struct v4l2_subdev *sd)
+{
+	struct adv7604_state *state = to_state(sd);
+
+	/* port A only */
+	return v4l2_ctrl_s_ctrl(state->detect_tx_5v_ctrl,
+				((io_read(sd, 0x6f) & 0x10) >> 4));
+}
+
+static void configure_free_run(struct v4l2_subdev *sd, const struct v4l2_bt_timings *timings)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u32 width = htotal(timings);
+	u32 height = vtotal(timings);
+	u16 ch1_fr_ll = (((u32)timings->pixelclock / 100) > 0) ?
+		((width * (ADV7604_fsc / 100)) / ((u32)timings->pixelclock / 100)) : 0;
+
+	v4l2_dbg(2, debug, sd, "%s\n", __func__);
+
+	cp_write(sd, 0x8f, (ch1_fr_ll >> 8) & 0x7);	/* CH1_FR_LL */
+	cp_write(sd, 0x90, ch1_fr_ll & 0xff);		/* CH1_FR_LL */
+	cp_write(sd, 0xab, (height >> 4) & 0xff); /* CP_LCOUNT_MAX */
+	cp_write(sd, 0xac, (height & 0x0f) << 4); /* CP_LCOUNT_MAX */
+	/* TODO support interlaced */
+	cp_write(sd, 0x91, 0x10);	/* INTERLACED */
+
+	/* Should only be set in auto-graphics mode [REF_02 p. 91-92] */
+	if ((io_read(sd, 0x00) == 0x07) && (io_read(sd, 0x01) == 0x02)) {
+		u16 cp_start_sav, cp_start_eav, cp_start_vbi, cp_end_vbi;
+		const u8 pll[2] = {
+			(0xc0 | ((width >> 8) & 0x1f)),
+			(width & 0xff)
+		};
+
+		/* setup PLL_DIV_MAN_EN and PLL_DIV_RATIO */
+		/* IO-map reg. 0x16 and 0x17 should be written in sequence */
+		if (adv_smbus_write_i2c_block_data(client, 0x16, 2, pll)) {
+			v4l2_err(sd, "writing to reg 0x16 and 0x17 failed\n");
+			return;
+		}
+
+		/* active video - horizontal timing */
+		cp_start_sav = timings->hsync + timings->hbackporch - 4;
+		cp_start_eav = width - timings->hfrontporch;
+		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_start_vbi = height - timings->vfrontporch;
+		cp_end_vbi = timings->vsync + timings->vbackporch;
+		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 {
+		/* reset to default values */
+		io_write(sd, 0x16, 0x43);
+		io_write(sd, 0x17, 0x5a);
+		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);
+	}
+}
+
+
+static void set_rgb_quantization_range(struct v4l2_subdev *sd)
+{
+	struct adv7604_state *state = to_state(sd);
+
+	switch (state->rgb_quantization_range) {
+	case V4L2_DV_RGB_RANGE_AUTO:
+		/* automatic */
+		if ((hdmi_read(sd, 0x05) & 0x80) ||
+				(state->prim_mode == ADV7604_PRIM_MODE_COMP) ||
+				(state->prim_mode == ADV7604_PRIM_MODE_RGB)) {
+			/* receiving HDMI or analog signal */
+			io_write_and_or(sd, 0x02, 0x0f, 0xf0);
+		} else {
+			/* receiving DVI-D signal */
+
+			/* ADV7604 selects RGB limited range regardless of
+			   input format (CE/IT) in automatic mode */
+			if (state->timings.bt.standards & V4L2_DV_BT_STD_CEA861) {
+				/* 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);
+			}
+		}
+		break;
+	case V4L2_DV_RGB_RANGE_LIMITED:
+		/* RGB limited range (16-235) */
+		io_write_and_or(sd, 0x02, 0x0f, 0x00);
+		break;
+	case V4L2_DV_RGB_RANGE_FULL:
+		/* RGB full range (0-255) */
+		io_write_and_or(sd, 0x02, 0x0f, 0x10);
+		break;
+	}
+}
+
+
+static int adv7604_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+	struct adv7604_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:
+		/* 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_and_or(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 adv7604_g_chip_ident(struct v4l2_subdev *sd,
+					struct v4l2_dbg_chip_ident *chip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_ADV7604, 0);
+}
+
+/* ----------------------------------------------------------------------- */
+
+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)
+{
+	/* TODO port B, C and D */
+	return !(io_read(sd, 0x6a) & 0x10);
+}
+
+static inline bool no_lock_tmds(struct v4l2_subdev *sd)
+{
+	return (io_read(sd, 0x6a) & 0xe0) != 0xe0;
+}
+
+static inline bool no_lock_sspd(struct v4l2_subdev *sd)
+{
+	/* 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)
+{
+	struct adv7604_state *state = to_state(sd);
+	bool ret;
+
+	ret = no_power(sd);
+
+	ret |= no_lock_stdi(sd);
+	ret |= no_lock_sspd(sd);
+
+	if (DIGITAL_INPUT) {
+		ret |= no_lock_tmds(sd);
+		ret |= no_signal_tmds(sd);
+	}
+
+	return ret;
+}
+
+static inline bool no_lock_cp(struct v4l2_subdev *sd)
+{
+	/* 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 int adv7604_g_input_status(struct v4l2_subdev *sd, u32 *status)
+{
+	struct adv7604_state *state = to_state(sd);
+
+	*status = 0;
+	*status |= no_power(sd) ? V4L2_IN_ST_NO_POWER : 0;
+	*status |= no_signal(sd) ? V4L2_IN_ST_NO_SIGNAL : 0;
+	if (no_lock_cp(sd))
+		*status |= DIGITAL_INPUT ? 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;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static void adv7604_print_timings(struct v4l2_subdev *sd,
+	struct v4l2_dv_timings *timings, const char *txt, bool detailed)
+{
+	struct v4l2_bt_timings *bt = &timings->bt;
+	u32 htot, vtot;
+
+	if (timings->type != V4L2_DV_BT_656_1120)
+		return;
+
+	htot = htotal(bt);
+	vtot = vtotal(bt);
+
+	v4l2_info(sd, "%s %dx%d%s%d (%dx%d)",
+			txt, bt->width, bt->height, bt->interlaced ? "i" : "p",
+			(htot * vtot) > 0 ? ((u32)bt->pixelclock /
+				(htot * vtot)) : 0,
+			htot, vtot);
+
+	if (detailed) {
+		v4l2_info(sd, "    horizontal: fp = %d, %ssync = %d, bp = %d\n",
+				bt->hfrontporch,
+				(bt->polarities & V4L2_DV_HSYNC_POS_POL) ? "+" : "-",
+				bt->hsync, bt->hbackporch);
+		v4l2_info(sd, "    vertical: fp = %d, %ssync = %d, bp = %d\n",
+				bt->vfrontporch,
+				(bt->polarities & V4L2_DV_VSYNC_POS_POL) ? "+" : "-",
+				bt->vsync, bt->vbackporch);
+		v4l2_info(sd, "    pixelclock: %lld, flags: 0x%x, standards: 0x%x\n",
+				bt->pixelclock, bt->flags, bt->standards);
+	}
+}
+
+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 adv7604_state *state = to_state(sd);
+	u32 hfreq = (ADV7604_fsc * 8) / stdi->bl;
+	u32 pix_clk;
+	int i;
+
+	for (i = 0; adv7604_timings[i].bt.height; i++) {
+		if (vtotal(&adv7604_timings[i].bt) != stdi->lcf + 1)
+			continue;
+		if (adv7604_timings[i].bt.vsync != stdi->lcvs)
+			continue;
+
+		pix_clk = hfreq * htotal(&adv7604_timings[i].bt);
+
+		if ((pix_clk < adv7604_timings[i].bt.pixelclock + 1000000) &&
+		    (pix_clk > adv7604_timings[i].bt.pixelclock - 1000000)) {
+			*timings = adv7604_timings[i];
+			return 0;
+		}
+	}
+
+	if (v4l2_detect_cvt(stdi->lcf + 1, hfreq, stdi->lcvs,
+			(stdi->hs_pol == '+' ? V4L2_DV_HSYNC_POS_POL : 0) |
+			(stdi->vs_pol == '+' ? V4L2_DV_VSYNC_POS_POL : 0),
+			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),
+			state->aspect_ratio, timings))
+		return 0;
+
+	v4l2_dbg(2, debug, sd, "%s: No format candidate found for lcf=%d, bl = %d\n",
+			__func__, stdi->lcf, stdi->bl);
+	return -1;
+}
+
+static int read_stdi(struct v4l2_subdev *sd, struct stdi_readback *stdi)
+{
+	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_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;
+	stdi->interlaced = io_read(sd, 0x12) & 0x10;
+
+	/* read SSPD */
+	if ((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';
+	}
+
+	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 adv7604_enum_dv_timings(struct v4l2_subdev *sd,
+			struct v4l2_enum_dv_timings *timings)
+{
+	if (timings->index >= ARRAY_SIZE(adv7604_timings) - 1)
+		return -EINVAL;
+	memset(timings->reserved, 0, sizeof(timings->reserved));
+	timings->timings = adv7604_timings[timings->index];
+	return 0;
+}
+
+static int adv7604_dv_timings_cap(struct v4l2_subdev *sd,
+			struct v4l2_dv_timings_cap *cap)
+{
+	struct adv7604_state *state = to_state(sd);
+
+	cap->type = V4L2_DV_BT_656_1120;
+	cap->bt.max_width = 1920;
+	cap->bt.max_height = 1200;
+	cap->bt.min_pixelclock = 27000000;
+	if (DIGITAL_INPUT)
+		cap->bt.max_pixelclock = 225000000;
+	else
+		cap->bt.max_pixelclock = 170000000;
+	cap->bt.standards = V4L2_DV_BT_STD_CEA861 | V4L2_DV_BT_STD_DMT |
+			 V4L2_DV_BT_STD_GTF | V4L2_DV_BT_STD_CVT;
+	cap->bt.capabilities = V4L2_DV_BT_CAP_PROGRESSIVE |
+		V4L2_DV_BT_CAP_REDUCED_BLANKING | V4L2_DV_BT_CAP_CUSTOM;
+	return 0;
+}
+
+/* Fill the optional fields .standards and .flags in struct v4l2_dv_timings
+   if the format is listed in adv7604_timings[] */
+static void adv7604_fill_optional_dv_timings_fields(struct v4l2_subdev *sd,
+		struct v4l2_dv_timings *timings)
+{
+	struct adv7604_state *state = to_state(sd);
+	int i;
+
+	for (i = 0; adv7604_timings[i].bt.width; i++) {
+		if (v4l_match_dv_timings(timings, &adv7604_timings[i],
+					DIGITAL_INPUT ? 250000 : 1000000)) {
+			*timings = adv7604_timings[i];
+			break;
+		}
+	}
+}
+
+static int adv7604_query_dv_timings(struct v4l2_subdev *sd,
+			struct v4l2_dv_timings *timings)
+{
+	struct adv7604_state *state = to_state(sd);
+	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)) {
+		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 (DIGITAL_INPUT) {
+		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);
+		bt->pixelclock = (hdmi_read(sd, 0x06) * 1000000) +
+			((hdmi_read(sd, 0x3b) & 0x30) >> 4) * 250000;
+		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->vbackporch = ((hdmi_read(sd, 0x34) & 0x1f) * 256 +
+					hdmi_read(sd, 0x35)) / 2;
+		}
+		adv7604_fill_optional_dv_timings_fields(sd, timings);
+	} else {
+		/* find format
+		 * Since LCVS values are inaccurate (REF_03, page 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)) {
+			v4l2_dbg(1, debug, sd, "%s: format not supported\n", __func__);
+			return -ERANGE;
+		}
+	}
+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 ((!DIGITAL_INPUT && bt->pixelclock > 170000000) ||
+			(DIGITAL_INPUT && bt->pixelclock > 225000000)) {
+		v4l2_dbg(1, debug, sd, "%s: pixelclock out of range %d\n",
+				__func__, (u32)bt->pixelclock);
+		return -ERANGE;
+	}
+
+	if (debug > 1)
+		adv7604_print_timings(sd, timings,
+				"adv7604_query_dv_timings:", true);
+
+	return 0;
+}
+
+static int adv7604_s_dv_timings(struct v4l2_subdev *sd,
+		struct v4l2_dv_timings *timings)
+{
+	struct adv7604_state *state = to_state(sd);
+	struct v4l2_bt_timings *bt;
+
+	if (!timings)
+		return -EINVAL;
+
+	bt = &timings->bt;
+
+	if ((!DIGITAL_INPUT && bt->pixelclock > 170000000) ||
+			(DIGITAL_INPUT && bt->pixelclock > 225000000)) {
+		v4l2_dbg(1, debug, sd, "%s: pixelclock out of range %d\n",
+				__func__, (u32)bt->pixelclock);
+		return -ERANGE;
+	}
+	adv7604_fill_optional_dv_timings_fields(sd, timings);
+
+	state->timings = *timings;
+
+	/* freerun */
+	configure_free_run(sd, bt);
+
+	set_rgb_quantization_range(sd);
+
+
+	if (debug > 1)
+		adv7604_print_timings(sd, timings,
+				"adv7604_s_dv_timings:", true);
+	return 0;
+}
+
+static int adv7604_g_dv_timings(struct v4l2_subdev *sd,
+		struct v4l2_dv_timings *timings)
+{
+	struct adv7604_state *state = to_state(sd);
+
+	*timings = state->timings;
+	return 0;
+}
+
+static void enable_input(struct v4l2_subdev *sd, enum adv7604_prim_mode prim_mode)
+{
+	switch (prim_mode) {
+	case ADV7604_PRIM_MODE_COMP:
+	case ADV7604_PRIM_MODE_RGB:
+		/* enable */
+		io_write(sd, 0x15, 0xb0);   /* Disable Tristate of Pins (no audio) */
+		break;
+	case ADV7604_PRIM_MODE_HDMI_COMP:
+	case ADV7604_PRIM_MODE_HDMI_GR:
+		/* enable */
+		hdmi_write(sd, 0x1a, 0x0a); /* Unmute audio */
+		hdmi_write(sd, 0x01, 0x00); /* Enable HDMI clock terminators */
+		io_write(sd, 0x15, 0xa0);   /* Disable Tristate of Pins */
+		break;
+	default:
+		v4l2_err(sd, "%s: reserved primary mode 0x%0x\n",
+				__func__, prim_mode);
+		break;
+	}
+}
+
+static void disable_input(struct v4l2_subdev *sd)
+{
+	/* disable */
+	io_write(sd, 0x15, 0xbe);   /* Tristate all outputs from video core */
+	hdmi_write(sd, 0x1a, 0x1a); /* Mute audio */
+	hdmi_write(sd, 0x01, 0x78); /* Disable HDMI clock terminators */
+}
+
+static void select_input(struct v4l2_subdev *sd, enum adv7604_prim_mode prim_mode)
+{
+	switch (prim_mode) {
+	case ADV7604_PRIM_MODE_COMP:
+	case ADV7604_PRIM_MODE_RGB:
+		/* set mode and select free run resolution */
+		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);
+
+		/* reset ADI recommended settings for HDMI: */
+		/* "ADV7604 Register Settings Recommendations (rev. 2.5, June 2010)" p. 4. */
+		hdmi_write(sd, 0x0d, 0x04); /* HDMI filter optimization */
+		hdmi_write(sd, 0x3d, 0x00); /* DDC bus active pull-up control */
+		hdmi_write(sd, 0x3e, 0x74); /* TMDS PLL optimization */
+		hdmi_write(sd, 0x4e, 0x3b); /* TMDS PLL optimization */
+		hdmi_write(sd, 0x57, 0x74); /* TMDS PLL optimization */
+		hdmi_write(sd, 0x58, 0x63); /* TMDS PLL optimization */
+		hdmi_write(sd, 0x8d, 0x18); /* equaliser */
+		hdmi_write(sd, 0x8e, 0x34); /* equaliser */
+		hdmi_write(sd, 0x93, 0x88); /* equaliser */
+		hdmi_write(sd, 0x94, 0x2e); /* equaliser */
+		hdmi_write(sd, 0x96, 0x00); /* enable automatic EQ changing */
+
+		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 */
+
+		/* set ADI recommended settings for digitizer */
+		/* "ADV7604 Register Settings Recommendations (rev. 2.5, June 2010)" p. 17. */
+		afe_write(sd, 0x12, 0x7b); /* ADC noise shaping filter controls */
+		afe_write(sd, 0x0c, 0x1f); /* CP core gain controls */
+		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 ADV7604_PRIM_MODE_HDMI_COMP:
+	case ADV7604_PRIM_MODE_HDMI_GR:
+		/* set mode and select free run resolution */
+		/* video std */
+		io_write(sd, 0x00,
+			(prim_mode == ADV7604_PRIM_MODE_HDMI_GR) ? 0x02 : 0x1e);
+		io_write(sd, 0x01, prim_mode); /* prim mode */
+		/* disable embedded syncs for auto graphics mode */
+		cp_write_and_or(sd, 0x81, 0xef, 0x00);
+
+		/* set ADI recommended settings for HDMI: */
+		/* "ADV7604 Register Settings Recommendations (rev. 2.5, June 2010)" p. 4. */
+		hdmi_write(sd, 0x0d, 0x84); /* HDMI filter optimization */
+		hdmi_write(sd, 0x3d, 0x10); /* DDC bus active pull-up control */
+		hdmi_write(sd, 0x3e, 0x39); /* TMDS PLL optimization */
+		hdmi_write(sd, 0x4e, 0x3b); /* TMDS PLL optimization */
+		hdmi_write(sd, 0x57, 0xb6); /* TMDS PLL optimization */
+		hdmi_write(sd, 0x58, 0x03); /* TMDS PLL optimization */
+		hdmi_write(sd, 0x8d, 0x18); /* equaliser */
+		hdmi_write(sd, 0x8e, 0x34); /* equaliser */
+		hdmi_write(sd, 0x93, 0x8b); /* equaliser */
+		hdmi_write(sd, 0x94, 0x2d); /* equaliser */
+		hdmi_write(sd, 0x96, 0x01); /* enable automatic EQ changing */
+
+		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 */
+
+		/* reset ADI recommended settings for digitizer */
+		/* "ADV7604 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_write(sd, 0xc3, 0x39); /* CP coast control. Graphics mode */
+		cp_write(sd, 0x40, 0x80); /* CP core pre-gain control. Graphics mode */
+
+		break;
+	default:
+		v4l2_err(sd, "%s: reserved primary mode 0x%0x\n", __func__, prim_mode);
+		break;
+	}
+}
+
+static int adv7604_s_routing(struct v4l2_subdev *sd,
+		u32 input, u32 output, u32 config)
+{
+	struct adv7604_state *state = to_state(sd);
+
+	v4l2_dbg(2, debug, sd, "%s: input %d", __func__, input);
+
+	switch (input) {
+	case 0:
+		/* TODO select HDMI_COMP or HDMI_GR */
+		state->prim_mode = ADV7604_PRIM_MODE_HDMI_COMP;
+		break;
+	case 1:
+		state->prim_mode = ADV7604_PRIM_MODE_RGB;
+		break;
+	case 2:
+		state->prim_mode = ADV7604_PRIM_MODE_COMP;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	disable_input(sd);
+
+	select_input(sd, state->prim_mode);
+
+	enable_input(sd, state->prim_mode);
+
+	return 0;
+}
+
+static int adv7604_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned int index,
+			     enum v4l2_mbus_pixelcode *code)
+{
+	if (index)
+		return -EINVAL;
+	/* Good enough for now */
+	*code = V4L2_MBUS_FMT_FIXED;
+	return 0;
+}
+
+static int adv7604_g_mbus_fmt(struct v4l2_subdev *sd,
+		struct v4l2_mbus_framefmt *fmt)
+{
+	struct adv7604_state *state = to_state(sd);
+
+	fmt->width = state->timings.bt.width;
+	fmt->height = state->timings.bt.height;
+	fmt->code = V4L2_MBUS_FMT_FIXED;
+	fmt->field = V4L2_FIELD_NONE;
+	if (state->timings.bt.standards & V4L2_DV_BT_STD_CEA861) {
+		fmt->colorspace = (state->timings.bt.height <= 576) ?
+			V4L2_COLORSPACE_SMPTE170M : V4L2_COLORSPACE_REC709;
+	}
+	return 0;
+}
+
+static int adv7604_isr(struct v4l2_subdev *sd, u32 status, bool *handled)
+{
+	struct adv7604_state *state = to_state(sd);
+	u8 fmt_change, fmt_change_digital, tx_5v;
+
+	/* format change */
+	fmt_change = io_read(sd, 0x43) & 0x98;
+	if (fmt_change)
+		io_write(sd, 0x44, fmt_change);
+	fmt_change_digital = DIGITAL_INPUT ? (io_read(sd, 0x6b) & 0xc0) : 0;
+	if (fmt_change_digital)
+		io_write(sd, 0x6c, fmt_change_digital);
+	if (fmt_change || fmt_change_digital) {
+		v4l2_dbg(1, debug, sd,
+			"%s: ADV7604_FMT_CHANGE, fmt_change = 0x%x, fmt_change_digital = 0x%x\n",
+			__func__, fmt_change, fmt_change_digital);
+		v4l2_subdev_notify(sd, ADV7604_FMT_CHANGE, NULL);
+		if (handled)
+			*handled = true;
+	}
+	/* tx 5v detect */
+	tx_5v = io_read(sd, 0x70) & 0x10;
+	if (tx_5v) {
+		v4l2_dbg(1, debug, sd, "%s: tx_5v: 0x%x\n", __func__, tx_5v);
+		io_write(sd, 0x71, tx_5v);
+		adv7604_s_detect_tx_5v_ctrl(sd);
+		if (handled)
+			*handled = true;
+	}
+	return 0;
+}
+
+static int adv7604_get_edid(struct v4l2_subdev *sd, struct v4l2_subdev_edid *edid)
+{
+	struct adv7604_state *state = to_state(sd);
+
+	if (edid->pad != 0)
+		return -EINVAL;
+	if (edid->blocks == 0)
+		return -EINVAL;
+	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;
+	if (!edid->edid)
+		return -EINVAL;
+	memcpy(edid->edid + edid->start_block * 128,
+	       state->edid + edid->start_block * 128,
+	       edid->blocks * 128);
+	return 0;
+}
+
+static int adv7604_set_edid(struct v4l2_subdev *sd, struct v4l2_subdev_edid *edid)
+{
+	struct adv7604_state *state = to_state(sd);
+	int err;
+
+	if (edid->pad != 0)
+		return -EINVAL;
+	if (edid->start_block != 0)
+		return -EINVAL;
+	if (edid->blocks == 0) {
+		/* Pull down the hotplug pin */
+		v4l2_subdev_notify(sd, ADV7604_HOTPLUG, (void *)0);
+		/* Disables I2C access to internal EDID ram from DDC port */
+		rep_write_and_or(sd, 0x77, 0xf0, 0x0);
+		state->edid_blocks = 0;
+		/* Fall back to a 16:9 aspect ratio */
+		state->aspect_ratio.numerator = 16;
+		state->aspect_ratio.denominator = 9;
+		return 0;
+	}
+	if (edid->blocks > 2)
+		return -E2BIG;
+	if (!edid->edid)
+		return -EINVAL;
+	memcpy(state->edid, edid->edid, 128 * edid->blocks);
+	state->edid_blocks = edid->blocks;
+	state->aspect_ratio = v4l2_calc_aspect_ratio(edid->edid[0x15],
+			edid->edid[0x16]);
+	err = edid_write_block(sd, 128 * edid->blocks, state->edid);
+	if (err < 0)
+		v4l2_err(sd, "error %d writing edid\n", err);
+	return err;
+}
+
+/*********** avi info frame CEA-861-E **************/
+
+static void print_avi_infoframe(struct v4l2_subdev *sd)
+{
+	int i;
+	u8 buf[14];
+	u8 avi_len;
+	u8 avi_ver;
+
+	if (!(hdmi_read(sd, 0x05) & 0x80)) {
+		v4l2_info(sd, "receive DVI-D signal (AVI infoframe not supported)\n");
+		return;
+	}
+	if (!(io_read(sd, 0x60) & 0x01)) {
+		v4l2_info(sd, "AVI infoframe not received\n");
+		return;
+	}
+
+	if (io_read(sd, 0x83) & 0x01) {
+		v4l2_info(sd, "AVI infoframe checksum error has occurred earlier\n");
+		io_write(sd, 0x85, 0x01); /* clear AVI_INF_CKS_ERR_RAW */
+		if (io_read(sd, 0x83) & 0x01) {
+			v4l2_info(sd, "AVI infoframe checksum error still present\n");
+			io_write(sd, 0x85, 0x01); /* clear AVI_INF_CKS_ERR_RAW */
+		}
+	}
+
+	avi_len = infoframe_read(sd, 0xe2);
+	avi_ver = infoframe_read(sd, 0xe1);
+	v4l2_info(sd, "AVI infoframe version %d (%d byte)\n",
+			avi_ver, avi_len);
+
+	if (avi_ver != 0x02)
+		return;
+
+	for (i = 0; i < 14; i++)
+		buf[i] = infoframe_read(sd, i);
+
+	v4l2_info(sd,
+		"\t%02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x\n",
+		buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6], buf[7],
+		buf[8], buf[9], buf[10], buf[11], buf[12], buf[13]);
+}
+
+static int adv7604_log_status(struct v4l2_subdev *sd)
+{
+	struct adv7604_state *state = to_state(sd);
+	struct v4l2_dv_timings timings;
+	struct stdi_readback stdi;
+	u8 reg_io_0x02 = io_read(sd, 0x02);
+
+	char *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"
+	};
+	char *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"
+	};
+	char *rgb_quantization_range_txt[] = {
+		"Automatic",
+		"RGB limited range (16-235)",
+		"RGB full range (0-255)",
+	};
+
+	v4l2_info(sd, "-----Chip status-----\n");
+	v4l2_info(sd, "Chip power: %s\n", no_power(sd) ? "off" : "on");
+	v4l2_info(sd, "Connector type: %s\n", state->connector_hdmi ?
+			"HDMI" : (DIGITAL_INPUT ? "DVI-D" : "DVI-A"));
+	v4l2_info(sd, "EDID: %s\n", ((rep_read(sd, 0x7d) & 0x01) &&
+			(rep_read(sd, 0x77) & 0x01)) ? "enabled" : "disabled ");
+	v4l2_info(sd, "CEC: %s\n", !!(cec_read(sd, 0x2a) & 0x01) ?
+			"enabled" : "disabled");
+
+	v4l2_info(sd, "-----Signal status-----\n");
+	v4l2_info(sd, "Cable detected (+5V power): %s\n",
+			(io_read(sd, 0x6f) & 0x10) ? "true" : "false");
+	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",
+			(!!(cp_read(sd, 0xff) & 0x10) ? "on" : "off"));
+	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, "-----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 (adv7604_query_dv_timings(sd, &timings))
+		v4l2_info(sd, "No video detected\n");
+	else
+		adv7604_print_timings(sd, &timings, "Detected format:", true);
+	adv7604_print_timings(sd, &state->timings, "Configured format:", true);
+
+	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, saturator %s\n",
+			(reg_io_0x02 & 0x02) ? "RGB" : "YCbCr",
+			(reg_io_0x02 & 0x04) ? "(16-235)" : "(0-255)",
+			((reg_io_0x02 & 0x04) ^ (reg_io_0x02 & 0x01)) ?
+					"enabled" : "disabled");
+	v4l2_info(sd, "Color space conversion: %s\n",
+			csc_coeff_sel_rb[cp_read(sd, 0xfc) >> 4]);
+
+	/* Digital video */
+	if (DIGITAL_INPUT) {
+		v4l2_info(sd, "-----HDMI status-----\n");
+		v4l2_info(sd, "HDCP encrypted content: %s\n",
+				hdmi_read(sd, 0x05) & 0x40 ? "true" : "false");
+
+		print_avi_infoframe(sd);
+	}
+
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_ctrl_ops adv7604_ctrl_ops = {
+	.s_ctrl = adv7604_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops adv7604_core_ops = {
+	.log_status = adv7604_log_status,
+	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
+	.try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
+	.s_ext_ctrls = v4l2_subdev_s_ext_ctrls,
+	.g_ctrl = v4l2_subdev_g_ctrl,
+	.s_ctrl = v4l2_subdev_s_ctrl,
+	.queryctrl = v4l2_subdev_queryctrl,
+	.querymenu = v4l2_subdev_querymenu,
+	.g_chip_ident = adv7604_g_chip_ident,
+	.interrupt_service_routine = adv7604_isr,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = adv7604_g_register,
+	.s_register = adv7604_s_register,
+#endif
+};
+
+static const struct v4l2_subdev_video_ops adv7604_video_ops = {
+	.s_routing = adv7604_s_routing,
+	.g_input_status = adv7604_g_input_status,
+	.s_dv_timings = adv7604_s_dv_timings,
+	.g_dv_timings = adv7604_g_dv_timings,
+	.query_dv_timings = adv7604_query_dv_timings,
+	.enum_dv_timings = adv7604_enum_dv_timings,
+	.dv_timings_cap = adv7604_dv_timings_cap,
+	.enum_mbus_fmt = adv7604_enum_mbus_fmt,
+	.g_mbus_fmt = adv7604_g_mbus_fmt,
+	.try_mbus_fmt = adv7604_g_mbus_fmt,
+	.s_mbus_fmt = adv7604_g_mbus_fmt,
+};
+
+static const struct v4l2_subdev_pad_ops adv7604_pad_ops = {
+	.get_edid = adv7604_get_edid,
+	.set_edid = adv7604_set_edid,
+};
+
+static const struct v4l2_subdev_ops adv7604_ops = {
+	.core = &adv7604_core_ops,
+	.video = &adv7604_video_ops,
+	.pad = &adv7604_pad_ops,
+};
+
+/* -------------------------- custom ctrls ---------------------------------- */
+
+static const struct v4l2_ctrl_config adv7604_ctrl_analog_sampling_phase = {
+	.ops = &adv7604_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 adv7604_ctrl_free_run_color_manual = {
+	.ops = &adv7604_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 adv7604_ctrl_free_run_color = {
+	.ops = &adv7604_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,
+};
+
+/* ----------------------------------------------------------------------- */
+
+static int adv7604_core_init(struct v4l2_subdev *sd)
+{
+	struct adv7604_state *state = to_state(sd);
+	struct adv7604_platform_data *pdata = &state->pdata;
+
+	hdmi_write(sd, 0x48,
+		(pdata->disable_pwrdnb ? 0x80 : 0) |
+		(pdata->disable_cable_det_rst ? 0x40 : 0));
+
+	disable_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 */
+
+	/* video format */
+	io_write_and_or(sd, 0x02, 0xf0,
+			pdata->alt_gamma << 3 |
+			pdata->op_656_range << 2 |
+			pdata->rgb_out << 1 |
+			pdata->alt_data_sat << 0);
+	io_write(sd, 0x03, pdata->op_format_sel);
+	io_write_and_or(sd, 0x04, 0x1f, pdata->op_ch_sel << 5);
+	io_write_and_or(sd, 0x05, 0xf0, pdata->blank_data << 3 |
+					pdata->insert_av_codes << 2 |
+					pdata->replicate_av_codes << 1 |
+					pdata->invert_cbcr << 0);
+
+	/* TODO from platform data */
+	cp_write(sd, 0x69, 0x30);   /* Enable CP CSC */
+	io_write(sd, 0x06, 0xa6);   /* positive VS and HS */
+	io_write(sd, 0x14, 0x7f);   /* Drive strength adjusted to max */
+	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 */
+
+	/* TODO from platform data */
+	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);
+
+	state->prim_mode = pdata->prim_mode;
+	select_input(sd, pdata->prim_mode);
+
+	enable_input(sd, pdata->prim_mode);
+
+	/* interrupts */
+	io_write(sd, 0x40, 0xc2); /* Configure INT1 */
+	io_write(sd, 0x41, 0xd7); /* STDI irq for any change, disable INT2 */
+	io_write(sd, 0x46, 0x98); /* Enable SSPD, STDI and CP unlocked interrupts */
+	io_write(sd, 0x6e, 0xc0); /* Enable V_LOCKED and DE_REGEN_LCK interrupts */
+	io_write(sd, 0x73, 0x10); /* Enable CABLE_DET_A_ST (+5v) interrupt */
+
+	return v4l2_ctrl_handler_setup(sd->ctrl_handler);
+}
+
+static void adv7604_unregister_clients(struct adv7604_state *state)
+{
+	if (state->i2c_avlink)
+		i2c_unregister_device(state->i2c_avlink);
+	if (state->i2c_cec)
+		i2c_unregister_device(state->i2c_cec);
+	if (state->i2c_infoframe)
+		i2c_unregister_device(state->i2c_infoframe);
+	if (state->i2c_esdp)
+		i2c_unregister_device(state->i2c_esdp);
+	if (state->i2c_dpp)
+		i2c_unregister_device(state->i2c_dpp);
+	if (state->i2c_afe)
+		i2c_unregister_device(state->i2c_afe);
+	if (state->i2c_repeater)
+		i2c_unregister_device(state->i2c_repeater);
+	if (state->i2c_edid)
+		i2c_unregister_device(state->i2c_edid);
+	if (state->i2c_hdmi)
+		i2c_unregister_device(state->i2c_hdmi);
+	if (state->i2c_test)
+		i2c_unregister_device(state->i2c_test);
+	if (state->i2c_cp)
+		i2c_unregister_device(state->i2c_cp);
+	if (state->i2c_vdp)
+		i2c_unregister_device(state->i2c_vdp);
+}
+
+static struct i2c_client *adv7604_dummy_client(struct v4l2_subdev *sd,
+							u8 addr, u8 io_reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (addr)
+		io_write(sd, io_reg, addr << 1);
+	return i2c_new_dummy(client->adapter, io_read(sd, io_reg) >> 1);
+}
+
+static int adv7604_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	struct adv7604_state *state;
+	struct adv7604_platform_data *pdata = client->dev.platform_data;
+	struct v4l2_ctrl_handler *hdl;
+	struct v4l2_subdev *sd;
+	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 adv7604 client on address 0x%x\n",
+			client->addr << 1);
+
+	state = kzalloc(sizeof(struct adv7604_state), GFP_KERNEL);
+	if (!state) {
+		v4l_err(client, "Could not allocate adv7604_state memory!\n");
+		return -ENOMEM;
+	}
+
+	/* platform data */
+	if (!pdata) {
+		v4l_err(client, "No platform data!\n");
+		err = -ENODEV;
+		goto err_state;
+	}
+	memcpy(&state->pdata, pdata, sizeof(state->pdata));
+
+	sd = &state->sd;
+	v4l2_i2c_subdev_init(sd, client, &adv7604_ops);
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	state->connector_hdmi = pdata->connector_hdmi;
+
+	/* i2c access to adv7604? */
+	if (adv_smbus_read_byte_data_check(client, 0xfb, false) != 0x68) {
+		v4l2_info(sd, "not an adv7604 on address 0x%x\n",
+				client->addr << 1);
+		err = -ENODEV;
+		goto err_state;
+	}
+
+	/* control handlers */
+	hdl = &state->hdl;
+	v4l2_ctrl_handler_init(hdl, 9);
+
+	v4l2_ctrl_new_std(hdl, &adv7604_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, -128, 127, 1, 0);
+	v4l2_ctrl_new_std(hdl, &adv7604_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(hdl, &adv7604_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(hdl, &adv7604_ctrl_ops,
+			V4L2_CID_HUE, 0, 128, 1, 0);
+
+	/* private controls */
+	state->detect_tx_5v_ctrl = v4l2_ctrl_new_std(hdl, NULL,
+			V4L2_CID_DV_RX_POWER_PRESENT, 0, 1, 0, 0);
+	state->detect_tx_5v_ctrl->is_private = true;
+	state->rgb_quantization_range_ctrl =
+		v4l2_ctrl_new_std_menu(hdl, &adv7604_ctrl_ops,
+			V4L2_CID_DV_RX_RGB_RANGE, V4L2_DV_RGB_RANGE_FULL,
+			0, V4L2_DV_RGB_RANGE_AUTO);
+	state->rgb_quantization_range_ctrl->is_private = true;
+
+	/* custom controls */
+	state->analog_sampling_phase_ctrl =
+		v4l2_ctrl_new_custom(hdl, &adv7604_ctrl_analog_sampling_phase, NULL);
+	state->analog_sampling_phase_ctrl->is_private = true;
+	state->free_run_color_manual_ctrl =
+		v4l2_ctrl_new_custom(hdl, &adv7604_ctrl_free_run_color_manual, NULL);
+	state->free_run_color_manual_ctrl->is_private = true;
+	state->free_run_color_ctrl =
+		v4l2_ctrl_new_custom(hdl, &adv7604_ctrl_free_run_color, NULL);
+	state->free_run_color_ctrl->is_private = true;
+
+	sd->ctrl_handler = hdl;
+	if (hdl->error) {
+		err = hdl->error;
+		goto err_hdl;
+	}
+	if (adv7604_s_detect_tx_5v_ctrl(sd)) {
+		err = -ENODEV;
+		goto err_hdl;
+	}
+
+	state->i2c_avlink = adv7604_dummy_client(sd, pdata->i2c_avlink, 0xf3);
+	state->i2c_cec = adv7604_dummy_client(sd, pdata->i2c_cec, 0xf4);
+	state->i2c_infoframe = adv7604_dummy_client(sd, pdata->i2c_infoframe, 0xf5);
+	state->i2c_esdp = adv7604_dummy_client(sd, pdata->i2c_esdp, 0xf6);
+	state->i2c_dpp = adv7604_dummy_client(sd, pdata->i2c_dpp, 0xf7);
+	state->i2c_afe = adv7604_dummy_client(sd, pdata->i2c_afe, 0xf8);
+	state->i2c_repeater = adv7604_dummy_client(sd, pdata->i2c_repeater, 0xf9);
+	state->i2c_edid = adv7604_dummy_client(sd, pdata->i2c_edid, 0xfa);
+	state->i2c_hdmi = adv7604_dummy_client(sd, pdata->i2c_hdmi, 0xfb);
+	state->i2c_test = adv7604_dummy_client(sd, pdata->i2c_test, 0xfc);
+	state->i2c_cp = adv7604_dummy_client(sd, pdata->i2c_cp, 0xfd);
+	state->i2c_vdp = adv7604_dummy_client(sd, pdata->i2c_vdp, 0xfe);
+	if (!state->i2c_avlink || !state->i2c_cec || !state->i2c_infoframe ||
+	    !state->i2c_esdp || !state->i2c_dpp || !state->i2c_afe ||
+	    !state->i2c_repeater || !state->i2c_edid || !state->i2c_hdmi ||
+	    !state->i2c_test || !state->i2c_cp || !state->i2c_vdp) {
+		err = -ENOMEM;
+		v4l2_err(sd, "failed to create all i2c clients\n");
+		goto err_i2c;
+	}
+
+	/* work queues */
+	state->work_queues = create_singlethread_workqueue(client->name);
+	if (!state->work_queues) {
+		v4l2_err(sd, "Could not create work queue\n");
+		err = -ENOMEM;
+		goto err_i2c;
+	}
+
+	INIT_DELAYED_WORK(&state->delayed_work_enable_hotplug,
+			adv7604_delayed_work_enable_hotplug);
+
+	state->pad.flags = MEDIA_PAD_FL_SOURCE;
+	err = media_entity_init(&sd->entity, 1, &state->pad, 0);
+	if (err)
+		goto err_work_queues;
+
+	err = adv7604_core_init(sd);
+	if (err)
+		goto err_entity;
+	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);
+	destroy_workqueue(state->work_queues);
+err_i2c:
+	adv7604_unregister_clients(state);
+err_hdl:
+	v4l2_ctrl_handler_free(hdl);
+err_state:
+	kfree(state);
+	return err;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static int adv7604_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct adv7604_state *state = to_state(sd);
+
+	cancel_delayed_work(&state->delayed_work_enable_hotplug);
+	destroy_workqueue(state->work_queues);
+	v4l2_device_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	adv7604_unregister_clients(to_state(sd));
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+	kfree(to_state(sd));
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static struct i2c_device_id adv7604_id[] = {
+	{ "adv7604", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, adv7604_id);
+
+static struct i2c_driver adv7604_driver = {
+	.driver = {
+		.owner = THIS_MODULE,
+		.name = "adv7604",
+	},
+	.probe = adv7604_probe,
+	.remove = adv7604_remove,
+	.id_table = adv7604_id,
+};
+
+module_i2c_driver(adv7604_driver);
diff --git a/drivers/media/i2c/ak881x.c b/drivers/media/i2c/ak881x.c
new file mode 100644
index 000000000000..ba674656b10d
--- /dev/null
+++ b/drivers/media/i2c/ak881x.c
@@ -0,0 +1,359 @@
+/*
+ * Driver for AK8813 / AK8814 TV-ecoders from Asahi Kasei Microsystems Co., Ltd. (AKM)
+ *
+ * Copyright (C) 2010, Guennadi Liakhovetski <g.liakhovetski@gmx.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#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/ak881x.h>
+#include <media/v4l2-chip-ident.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;
+	int id;	/* DEVICE_ID code V4L2_IDENT_AK881X code from v4l2-chip-ident.h */
+	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);
+}
+
+static int ak881x_g_chip_ident(struct v4l2_subdev *sd,
+			       struct v4l2_dbg_chip_ident *id)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ak881x *ak881x = to_ak881x(client);
+
+	if (id->match.type != V4L2_CHIP_MATCH_I2C_ADDR)
+		return -EINVAL;
+
+	if (id->match.addr != client->addr)
+		return -ENODEV;
+
+	id->ident	= ak881x->id;
+	id->revision	= ak881x->revision;
+
+	return 0;
+}
+
+#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->match.type != V4L2_CHIP_MATCH_I2C_ADDR || reg->reg > 0x26)
+		return -EINVAL;
+
+	if (reg->match.addr != client->addr)
+		return -ENODEV;
+
+	reg->val = reg_read(client, reg->reg);
+
+	if (reg->val > 0xffff)
+		return -EIO;
+
+	return 0;
+}
+
+static int ak881x_s_register(struct v4l2_subdev *sd,
+			     struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (reg->match.type != V4L2_CHIP_MATCH_I2C_ADDR || reg->reg > 0x26)
+		return -EINVAL;
+
+	if (reg->match.addr != client->addr)
+		return -ENODEV;
+
+	if (reg_write(client, reg->reg, reg->val) < 0)
+		return -EIO;
+
+	return 0;
+}
+#endif
+
+static int ak881x_try_g_mbus_fmt(struct v4l2_subdev *sd,
+				 struct v4l2_mbus_framefmt *mf)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ak881x *ak881x = to_ak881x(client);
+
+	v4l_bound_align_image(&mf->width, 0, 720, 2,
+			      &mf->height, 0, ak881x->lines, 1, 0);
+	mf->field	= V4L2_FIELD_INTERLACED;
+	mf->code	= V4L2_MBUS_FMT_YUYV8_2X8;
+	mf->colorspace	= V4L2_COLORSPACE_SMPTE170M;
+
+	return 0;
+}
+
+static int ak881x_s_mbus_fmt(struct v4l2_subdev *sd,
+			     struct v4l2_mbus_framefmt *mf)
+{
+	if (mf->field != V4L2_FIELD_INTERLACED ||
+	    mf->code != V4L2_MBUS_FMT_YUYV8_2X8)
+		return -EINVAL;
+
+	return ak881x_try_g_mbus_fmt(sd, mf);
+}
+
+static int ak881x_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned int index,
+				enum v4l2_mbus_pixelcode *code)
+{
+	if (index)
+		return -EINVAL;
+
+	*code = V4L2_MBUS_FMT_YUYV8_2X8;
+	return 0;
+}
+
+static int ak881x_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ak881x *ak881x = to_ak881x(client);
+
+	a->bounds.left			= 0;
+	a->bounds.top			= 0;
+	a->bounds.width			= 720;
+	a->bounds.height		= ak881x->lines;
+	a->defrect			= a->bounds;
+	a->type				= V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	a->pixelaspect.numerator	= 1;
+	a->pixelaspect.denominator	= 1;
+
+	return 0;
+}
+
+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 && !(std & ~V4L2_STD_PAL)) {
+		vp1 = 0xf;
+		ak881x->lines = 576;
+	} else if (std && !(std & ~V4L2_STD_NTSC)) {
+		vp1 = 0;
+		ak881x->lines = 480;
+	} 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 struct v4l2_subdev_core_ops ak881x_subdev_core_ops = {
+	.g_chip_ident	= ak881x_g_chip_ident,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register	= ak881x_g_register,
+	.s_register	= ak881x_s_register,
+#endif
+};
+
+static struct v4l2_subdev_video_ops ak881x_subdev_video_ops = {
+	.s_mbus_fmt	= ak881x_s_mbus_fmt,
+	.g_mbus_fmt	= ak881x_try_g_mbus_fmt,
+	.try_mbus_fmt	= ak881x_try_g_mbus_fmt,
+	.cropcap	= ak881x_cropcap,
+	.enum_mbus_fmt	= ak881x_enum_mbus_fmt,
+	.s_std_output	= ak881x_s_std_output,
+	.s_stream	= ak881x_s_stream,
+};
+
+static struct v4l2_subdev_ops ak881x_subdev_ops = {
+	.core	= &ak881x_subdev_core_ops,
+	.video	= &ak881x_subdev_video_ops,
+};
+
+static int ak881x_probe(struct i2c_client *client,
+			const struct i2c_device_id *did)
+{
+	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
+	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 = kzalloc(sizeof(struct 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:
+		ak881x->id = V4L2_IDENT_AK8813;
+		break;
+	case 0x14:
+		ak881x->id = V4L2_IDENT_AK8814;
+		break;
+	default:
+		dev_err(&client->dev,
+			"No ak881x chip detected, register read %x\n", data);
+		kfree(ak881x);
+		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 int ak881x_remove(struct i2c_client *client)
+{
+	struct ak881x *ak881x = to_ak881x(client);
+
+	v4l2_device_unregister_subdev(&ak881x->subdev);
+	kfree(ak881x);
+
+	return 0;
+}
+
+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 000000000000..8153a449846b
--- /dev/null
+++ b/drivers/media/i2c/aptina-pll.c
@@ -0,0 +1,173 @@
+/*
+ * Aptina Sensor PLL Configuration
+ *
+ * Copyright (C) 2012 Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ */
+
+#include <linux/device.h>
+#include <linux/gcd.h>
+#include <linux/kernel.h>
+#include <linux/lcm.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 000000000000..b370e341e75d
--- /dev/null
+++ b/drivers/media/i2c/aptina-pll.h
@@ -0,0 +1,56 @@
+/*
+ * Aptina Sensor PLL Configuration
+ *
+ * Copyright (C) 2012 Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ */
+
+#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/as3645a.c b/drivers/media/i2c/as3645a.c
new file mode 100644
index 000000000000..3bfdbf9d9bf1
--- /dev/null
+++ b/drivers/media/i2c/as3645a.c
@@ -0,0 +1,888 @@
+/*
+ * drivers/media/i2c/as3645a.c - AS3645A and LM3555 flash controllers driver
+ *
+ * Copyright (C) 2008-2011 Nokia Corporation
+ * Copyright (c) 2011, Intel Corporation.
+ *
+ * Contact: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ * TODO:
+ * - Check hardware FSTROBE control when sensor driver add support for this
+ *
+ */
+
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/slab.h>
+
+#include <media/as3645a.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+
+#define AS_TIMER_MS_TO_CODE(t)			(((t) - 100) / 50)
+#define AS_TIMER_CODE_TO_MS(c)			(50 * (c) + 100)
+
+/* Register definitions */
+
+/* Read-only Design info register: Reset state: xxxx 0001 */
+#define AS_DESIGN_INFO_REG			0x00
+#define AS_DESIGN_INFO_FACTORY(x)		(((x) >> 4))
+#define AS_DESIGN_INFO_MODEL(x)			((x) & 0x0f)
+
+/* Read-only Version control register: Reset state: 0000 0000
+ * for first engineering samples
+ */
+#define AS_VERSION_CONTROL_REG			0x01
+#define AS_VERSION_CONTROL_RFU(x)		(((x) >> 4))
+#define AS_VERSION_CONTROL_VERSION(x)		((x) & 0x0f)
+
+/* Read / Write	(Indicator and timer register): Reset state: 0000 1111 */
+#define AS_INDICATOR_AND_TIMER_REG		0x02
+#define AS_INDICATOR_AND_TIMER_TIMEOUT_SHIFT	0
+#define AS_INDICATOR_AND_TIMER_VREF_SHIFT	4
+#define AS_INDICATOR_AND_TIMER_INDICATOR_SHIFT	6
+
+/* Read / Write	(Current set register): Reset state: 0110 1001 */
+#define AS_CURRENT_SET_REG			0x03
+#define AS_CURRENT_ASSIST_LIGHT_SHIFT		0
+#define AS_CURRENT_LED_DET_ON			(1 << 3)
+#define AS_CURRENT_FLASH_CURRENT_SHIFT		4
+
+/* Read / Write	(Control register): Reset state: 1011 0100 */
+#define AS_CONTROL_REG				0x04
+#define AS_CONTROL_MODE_SETTING_SHIFT		0
+#define AS_CONTROL_STROBE_ON			(1 << 2)
+#define AS_CONTROL_OUT_ON			(1 << 3)
+#define AS_CONTROL_EXT_TORCH_ON			(1 << 4)
+#define AS_CONTROL_STROBE_TYPE_EDGE		(0 << 5)
+#define AS_CONTROL_STROBE_TYPE_LEVEL		(1 << 5)
+#define AS_CONTROL_COIL_PEAK_SHIFT		6
+
+/* Read only (D3 is read / write) (Fault and info): Reset state: 0000 x000 */
+#define AS_FAULT_INFO_REG			0x05
+#define AS_FAULT_INFO_INDUCTOR_PEAK_LIMIT	(1 << 1)
+#define AS_FAULT_INFO_INDICATOR_LED		(1 << 2)
+#define AS_FAULT_INFO_LED_AMOUNT		(1 << 3)
+#define AS_FAULT_INFO_TIMEOUT			(1 << 4)
+#define AS_FAULT_INFO_OVER_TEMPERATURE		(1 << 5)
+#define AS_FAULT_INFO_SHORT_CIRCUIT		(1 << 6)
+#define AS_FAULT_INFO_OVER_VOLTAGE		(1 << 7)
+
+/* Boost register */
+#define AS_BOOST_REG				0x0d
+#define AS_BOOST_CURRENT_DISABLE		(0 << 0)
+#define AS_BOOST_CURRENT_ENABLE			(1 << 0)
+
+/* Password register is used to unlock boost register writing */
+#define AS_PASSWORD_REG				0x0f
+#define AS_PASSWORD_UNLOCK_VALUE		0x55
+
+enum as_mode {
+	AS_MODE_EXT_TORCH = 0 << AS_CONTROL_MODE_SETTING_SHIFT,
+	AS_MODE_INDICATOR = 1 << AS_CONTROL_MODE_SETTING_SHIFT,
+	AS_MODE_ASSIST = 2 << AS_CONTROL_MODE_SETTING_SHIFT,
+	AS_MODE_FLASH = 3 << AS_CONTROL_MODE_SETTING_SHIFT,
+};
+
+/*
+ * struct as3645a
+ *
+ * @subdev:		V4L2 subdev
+ * @pdata:		Flash platform data
+ * @power_lock:		Protects power_count
+ * @power_count:	Power reference count
+ * @led_mode:		V4L2 flash LED mode
+ * @timeout:		Flash timeout in microseconds
+ * @flash_current:	Flash current (0=200mA ... 15=500mA). Maximum
+ *			values are 400mA for two LEDs and 500mA for one LED.
+ * @assist_current:	Torch/Assist light current (0=20mA, 1=40mA ... 7=160mA)
+ * @indicator_current:	Indicator LED current (0=0mA, 1=2.5mA ... 4=10mA)
+ * @strobe_source:	Flash strobe source (software or external)
+ */
+struct as3645a {
+	struct v4l2_subdev subdev;
+	const struct as3645a_platform_data *pdata;
+
+	struct mutex power_lock;
+	int power_count;
+
+	/* Controls */
+	struct v4l2_ctrl_handler ctrls;
+
+	enum v4l2_flash_led_mode led_mode;
+	unsigned int timeout;
+	u8 flash_current;
+	u8 assist_current;
+	u8 indicator_current;
+	enum v4l2_flash_strobe_source strobe_source;
+};
+
+#define to_as3645a(sd) container_of(sd, struct as3645a, subdev)
+
+/* Return negative errno else zero on success */
+static int as3645a_write(struct as3645a *flash, u8 addr, u8 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&flash->subdev);
+	int rval;
+
+	rval = i2c_smbus_write_byte_data(client, addr, val);
+
+	dev_dbg(&client->dev, "Write Addr:%02X Val:%02X %s\n", addr, val,
+		rval < 0 ? "fail" : "ok");
+
+	return rval;
+}
+
+/* Return negative errno else a data byte received from the device. */
+static int as3645a_read(struct as3645a *flash, u8 addr)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&flash->subdev);
+	int rval;
+
+	rval = i2c_smbus_read_byte_data(client, addr);
+
+	dev_dbg(&client->dev, "Read Addr:%02X Val:%02X %s\n", addr, rval,
+		rval < 0 ? "fail" : "ok");
+
+	return rval;
+}
+
+/* -----------------------------------------------------------------------------
+ * Hardware configuration and trigger
+ */
+
+/*
+ * as3645a_set_config - Set flash configuration registers
+ * @flash: The flash
+ *
+ * Configure the hardware with flash, assist and indicator currents, as well as
+ * flash timeout.
+ *
+ * Return 0 on success, or a negative error code if an I2C communication error
+ * occurred.
+ */
+static int as3645a_set_config(struct as3645a *flash)
+{
+	int ret;
+	u8 val;
+
+	val = (flash->flash_current << AS_CURRENT_FLASH_CURRENT_SHIFT)
+	    | (flash->assist_current << AS_CURRENT_ASSIST_LIGHT_SHIFT)
+	    | AS_CURRENT_LED_DET_ON;
+
+	ret = as3645a_write(flash, AS_CURRENT_SET_REG, val);
+	if (ret < 0)
+		return ret;
+
+	val = AS_TIMER_MS_TO_CODE(flash->timeout / 1000)
+		    << AS_INDICATOR_AND_TIMER_TIMEOUT_SHIFT;
+
+	val |= (flash->pdata->vref << AS_INDICATOR_AND_TIMER_VREF_SHIFT)
+	    |  ((flash->indicator_current ? flash->indicator_current - 1 : 0)
+		 << AS_INDICATOR_AND_TIMER_INDICATOR_SHIFT);
+
+	return as3645a_write(flash, AS_INDICATOR_AND_TIMER_REG, val);
+}
+
+/*
+ * as3645a_set_control - Set flash control register
+ * @flash: The flash
+ * @mode: Desired output mode
+ * @on: Desired output state
+ *
+ * Configure the hardware with output mode and state.
+ *
+ * Return 0 on success, or a negative error code if an I2C communication error
+ * occurred.
+ */
+static int
+as3645a_set_control(struct as3645a *flash, enum as_mode mode, bool on)
+{
+	u8 reg;
+
+	/* Configure output parameters and operation mode. */
+	reg = (flash->pdata->peak << AS_CONTROL_COIL_PEAK_SHIFT)
+	    | (on ? AS_CONTROL_OUT_ON : 0)
+	    | mode;
+
+	if (flash->led_mode == V4L2_FLASH_LED_MODE_FLASH &&
+	    flash->strobe_source == V4L2_FLASH_STROBE_SOURCE_EXTERNAL) {
+		reg |= AS_CONTROL_STROBE_TYPE_LEVEL
+		    |  AS_CONTROL_STROBE_ON;
+	}
+
+	return as3645a_write(flash, AS_CONTROL_REG, reg);
+}
+
+/*
+ * as3645a_set_output - Configure output and operation mode
+ * @flash: Flash controller
+ * @strobe: Strobe the flash (only valid in flash mode)
+ *
+ * Turn the LEDs output on/off and set the operation mode based on the current
+ * parameters.
+ *
+ * The AS3645A can't control the indicator LED independently of the flash/torch
+ * LED. If the flash controller is in V4L2_FLASH_LED_MODE_NONE mode, set the
+ * chip to indicator mode. Otherwise set it to assist light (torch) or flash
+ * mode.
+ *
+ * In indicator and assist modes, turn the output on/off based on the indicator
+ * and torch currents. In software strobe flash mode, turn the output on/off
+ * based on the strobe parameter.
+ */
+static int as3645a_set_output(struct as3645a *flash, bool strobe)
+{
+	enum as_mode mode;
+	bool on;
+
+	switch (flash->led_mode) {
+	case V4L2_FLASH_LED_MODE_NONE:
+		on = flash->indicator_current != 0;
+		mode = AS_MODE_INDICATOR;
+		break;
+	case V4L2_FLASH_LED_MODE_TORCH:
+		on = true;
+		mode = AS_MODE_ASSIST;
+		break;
+	case V4L2_FLASH_LED_MODE_FLASH:
+		on = strobe;
+		mode = AS_MODE_FLASH;
+		break;
+	default:
+		BUG();
+	}
+
+	/* Configure output parameters and operation mode. */
+	return as3645a_set_control(flash, mode, on);
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 controls
+ */
+
+static int as3645a_is_active(struct as3645a *flash)
+{
+	int ret;
+
+	ret = as3645a_read(flash, AS_CONTROL_REG);
+	return ret < 0 ? ret : !!(ret & AS_CONTROL_OUT_ON);
+}
+
+static int as3645a_read_fault(struct as3645a *flash)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&flash->subdev);
+	int rval;
+
+	/* NOTE: reading register clear fault status */
+	rval = as3645a_read(flash, AS_FAULT_INFO_REG);
+	if (rval < 0)
+		return rval;
+
+	if (rval & AS_FAULT_INFO_INDUCTOR_PEAK_LIMIT)
+		dev_dbg(&client->dev, "Inductor Peak limit fault\n");
+
+	if (rval & AS_FAULT_INFO_INDICATOR_LED)
+		dev_dbg(&client->dev, "Indicator LED fault: "
+			"Short circuit or open loop\n");
+
+	dev_dbg(&client->dev, "%u connected LEDs\n",
+		rval & AS_FAULT_INFO_LED_AMOUNT ? 2 : 1);
+
+	if (rval & AS_FAULT_INFO_TIMEOUT)
+		dev_dbg(&client->dev, "Timeout fault\n");
+
+	if (rval & AS_FAULT_INFO_OVER_TEMPERATURE)
+		dev_dbg(&client->dev, "Over temperature fault\n");
+
+	if (rval & AS_FAULT_INFO_SHORT_CIRCUIT)
+		dev_dbg(&client->dev, "Short circuit fault\n");
+
+	if (rval & AS_FAULT_INFO_OVER_VOLTAGE)
+		dev_dbg(&client->dev, "Over voltage fault: "
+			"Indicates missing capacitor or open connection\n");
+
+	return rval;
+}
+
+static int as3645a_get_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct as3645a *flash =
+		container_of(ctrl->handler, struct as3645a, ctrls);
+	struct i2c_client *client = v4l2_get_subdevdata(&flash->subdev);
+	int value;
+
+	switch (ctrl->id) {
+	case V4L2_CID_FLASH_FAULT:
+		value = as3645a_read_fault(flash);
+		if (value < 0)
+			return value;
+
+		ctrl->cur.val = 0;
+		if (value & AS_FAULT_INFO_SHORT_CIRCUIT)
+			ctrl->cur.val |= V4L2_FLASH_FAULT_SHORT_CIRCUIT;
+		if (value & AS_FAULT_INFO_OVER_TEMPERATURE)
+			ctrl->cur.val |= V4L2_FLASH_FAULT_OVER_TEMPERATURE;
+		if (value & AS_FAULT_INFO_TIMEOUT)
+			ctrl->cur.val |= V4L2_FLASH_FAULT_TIMEOUT;
+		if (value & AS_FAULT_INFO_OVER_VOLTAGE)
+			ctrl->cur.val |= V4L2_FLASH_FAULT_OVER_VOLTAGE;
+		if (value & AS_FAULT_INFO_INDUCTOR_PEAK_LIMIT)
+			ctrl->cur.val |= V4L2_FLASH_FAULT_OVER_CURRENT;
+		if (value & AS_FAULT_INFO_INDICATOR_LED)
+			ctrl->cur.val |= V4L2_FLASH_FAULT_INDICATOR;
+		break;
+
+	case V4L2_CID_FLASH_STROBE_STATUS:
+		if (flash->led_mode != V4L2_FLASH_LED_MODE_FLASH) {
+			ctrl->cur.val = 0;
+			break;
+		}
+
+		value = as3645a_is_active(flash);
+		if (value < 0)
+			return value;
+
+		ctrl->cur.val = value;
+		break;
+	}
+
+	dev_dbg(&client->dev, "G_CTRL %08x:%d\n", ctrl->id, ctrl->cur.val);
+
+	return 0;
+}
+
+static int as3645a_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct as3645a *flash =
+		container_of(ctrl->handler, struct as3645a, ctrls);
+	struct i2c_client *client = v4l2_get_subdevdata(&flash->subdev);
+	int ret;
+
+	dev_dbg(&client->dev, "S_CTRL %08x:%d\n", ctrl->id, ctrl->val);
+
+	/* If a control that doesn't apply to the current mode is modified,
+	 * we store the value and return immediately. The setting will be
+	 * applied when the LED mode is changed. Otherwise we apply the setting
+	 * immediately.
+	 */
+
+	switch (ctrl->id) {
+	case V4L2_CID_FLASH_LED_MODE:
+		if (flash->indicator_current)
+			return -EBUSY;
+
+		ret = as3645a_set_config(flash);
+		if (ret < 0)
+			return ret;
+
+		flash->led_mode = ctrl->val;
+		return as3645a_set_output(flash, false);
+
+	case V4L2_CID_FLASH_STROBE_SOURCE:
+		flash->strobe_source = ctrl->val;
+
+		/* Applies to flash mode only. */
+		if (flash->led_mode != V4L2_FLASH_LED_MODE_FLASH)
+			break;
+
+		return as3645a_set_output(flash, false);
+
+	case V4L2_CID_FLASH_STROBE:
+		if (flash->led_mode != V4L2_FLASH_LED_MODE_FLASH)
+			return -EBUSY;
+
+		return as3645a_set_output(flash, true);
+
+	case V4L2_CID_FLASH_STROBE_STOP:
+		if (flash->led_mode != V4L2_FLASH_LED_MODE_FLASH)
+			return -EBUSY;
+
+		return as3645a_set_output(flash, false);
+
+	case V4L2_CID_FLASH_TIMEOUT:
+		flash->timeout = ctrl->val;
+
+		/* Applies to flash mode only. */
+		if (flash->led_mode != V4L2_FLASH_LED_MODE_FLASH)
+			break;
+
+		return as3645a_set_config(flash);
+
+	case V4L2_CID_FLASH_INTENSITY:
+		flash->flash_current = (ctrl->val - AS3645A_FLASH_INTENSITY_MIN)
+				     / AS3645A_FLASH_INTENSITY_STEP;
+
+		/* Applies to flash mode only. */
+		if (flash->led_mode != V4L2_FLASH_LED_MODE_FLASH)
+			break;
+
+		return as3645a_set_config(flash);
+
+	case V4L2_CID_FLASH_TORCH_INTENSITY:
+		flash->assist_current =
+			(ctrl->val - AS3645A_TORCH_INTENSITY_MIN)
+			/ AS3645A_TORCH_INTENSITY_STEP;
+
+		/* Applies to torch mode only. */
+		if (flash->led_mode != V4L2_FLASH_LED_MODE_TORCH)
+			break;
+
+		return as3645a_set_config(flash);
+
+	case V4L2_CID_FLASH_INDICATOR_INTENSITY:
+		if (flash->led_mode != V4L2_FLASH_LED_MODE_NONE)
+			return -EBUSY;
+
+		flash->indicator_current =
+			(ctrl->val - AS3645A_INDICATOR_INTENSITY_MIN)
+			/ AS3645A_INDICATOR_INTENSITY_STEP;
+
+		ret = as3645a_set_config(flash);
+		if (ret < 0)
+			return ret;
+
+		if ((ctrl->val == 0) == (ctrl->cur.val == 0))
+			break;
+
+		return as3645a_set_output(flash, false);
+	}
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops as3645a_ctrl_ops = {
+	.g_volatile_ctrl = as3645a_get_ctrl,
+	.s_ctrl = as3645a_set_ctrl,
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev core operations
+ */
+
+/* Put device into know state. */
+static int as3645a_setup(struct as3645a *flash)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&flash->subdev);
+	int ret;
+
+	/* clear errors */
+	ret = as3645a_read(flash, AS_FAULT_INFO_REG);
+	if (ret < 0)
+		return ret;
+
+	dev_dbg(&client->dev, "Fault info: %02x\n", ret);
+
+	ret = as3645a_set_config(flash);
+	if (ret < 0)
+		return ret;
+
+	ret = as3645a_set_output(flash, false);
+	if (ret < 0)
+		return ret;
+
+	/* read status */
+	ret = as3645a_read_fault(flash);
+	if (ret < 0)
+		return ret;
+
+	dev_dbg(&client->dev, "AS_INDICATOR_AND_TIMER_REG: %02x\n",
+		as3645a_read(flash, AS_INDICATOR_AND_TIMER_REG));
+	dev_dbg(&client->dev, "AS_CURRENT_SET_REG: %02x\n",
+		as3645a_read(flash, AS_CURRENT_SET_REG));
+	dev_dbg(&client->dev, "AS_CONTROL_REG: %02x\n",
+		as3645a_read(flash, AS_CONTROL_REG));
+
+	return ret & ~AS_FAULT_INFO_LED_AMOUNT ? -EIO : 0;
+}
+
+static int __as3645a_set_power(struct as3645a *flash, int on)
+{
+	int ret;
+
+	if (!on)
+		as3645a_set_control(flash, AS_MODE_EXT_TORCH, false);
+
+	if (flash->pdata->set_power) {
+		ret = flash->pdata->set_power(&flash->subdev, on);
+		if (ret < 0)
+			return ret;
+	}
+
+	if (!on)
+		return 0;
+
+	ret = as3645a_setup(flash);
+	if (ret < 0) {
+		if (flash->pdata->set_power)
+			flash->pdata->set_power(&flash->subdev, 0);
+	}
+
+	return ret;
+}
+
+static int as3645a_set_power(struct v4l2_subdev *sd, int on)
+{
+	struct as3645a *flash = to_as3645a(sd);
+	int ret = 0;
+
+	mutex_lock(&flash->power_lock);
+
+	if (flash->power_count == !on) {
+		ret = __as3645a_set_power(flash, !!on);
+		if (ret < 0)
+			goto done;
+	}
+
+	flash->power_count += on ? 1 : -1;
+	WARN_ON(flash->power_count < 0);
+
+done:
+	mutex_unlock(&flash->power_lock);
+	return ret;
+}
+
+static int as3645a_registered(struct v4l2_subdev *sd)
+{
+	struct as3645a *flash = to_as3645a(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int rval, man, model, rfu, version;
+	const char *vendor;
+
+	/* Power up the flash driver and read manufacturer ID, model ID, RFU
+	 * and version.
+	 */
+	rval = as3645a_set_power(&flash->subdev, 1);
+	if (rval < 0)
+		return rval;
+
+	rval = as3645a_read(flash, AS_DESIGN_INFO_REG);
+	if (rval < 0)
+		goto power_off;
+
+	man = AS_DESIGN_INFO_FACTORY(rval);
+	model = AS_DESIGN_INFO_MODEL(rval);
+
+	rval = as3645a_read(flash, AS_VERSION_CONTROL_REG);
+	if (rval < 0)
+		goto power_off;
+
+	rfu = AS_VERSION_CONTROL_RFU(rval);
+	version = AS_VERSION_CONTROL_VERSION(rval);
+
+	/* Verify the chip model and version. */
+	if (model != 0x01 || rfu != 0x00) {
+		dev_err(&client->dev, "AS3645A not detected "
+			"(model %d rfu %d)\n", model, rfu);
+		rval = -ENODEV;
+		goto power_off;
+	}
+
+	switch (man) {
+	case 1:
+		vendor = "AMS, Austria Micro Systems";
+		break;
+	case 2:
+		vendor = "ADI, Analog Devices Inc.";
+		break;
+	case 3:
+		vendor = "NSC, National Semiconductor";
+		break;
+	case 4:
+		vendor = "NXP";
+		break;
+	case 5:
+		vendor = "TI, Texas Instrument";
+		break;
+	default:
+		vendor = "Unknown";
+	}
+
+	dev_info(&client->dev, "Chip vendor: %s (%d) Version: %d\n", vendor,
+		 man, version);
+
+	rval = as3645a_write(flash, AS_PASSWORD_REG, AS_PASSWORD_UNLOCK_VALUE);
+	if (rval < 0)
+		goto power_off;
+
+	rval = as3645a_write(flash, AS_BOOST_REG, AS_BOOST_CURRENT_DISABLE);
+	if (rval < 0)
+		goto power_off;
+
+	/* Setup default values. This makes sure that the chip is in a known
+	 * state, in case the power rail can't be controlled.
+	 */
+	rval = as3645a_setup(flash);
+
+power_off:
+	as3645a_set_power(&flash->subdev, 0);
+
+	return rval;
+}
+
+static int as3645a_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	return as3645a_set_power(sd, 1);
+}
+
+static int as3645a_close(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	return as3645a_set_power(sd, 0);
+}
+
+static const struct v4l2_subdev_core_ops as3645a_core_ops = {
+	.s_power		= as3645a_set_power,
+};
+
+static const struct v4l2_subdev_ops as3645a_ops = {
+	.core = &as3645a_core_ops,
+};
+
+static const struct v4l2_subdev_internal_ops as3645a_internal_ops = {
+	.registered = as3645a_registered,
+	.open = as3645a_open,
+	.close = as3645a_close,
+};
+
+/* -----------------------------------------------------------------------------
+ *  I2C driver
+ */
+#ifdef CONFIG_PM
+
+static int as3645a_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct as3645a *flash = to_as3645a(subdev);
+	int rval;
+
+	if (flash->power_count == 0)
+		return 0;
+
+	rval = __as3645a_set_power(flash, 0);
+
+	dev_dbg(&client->dev, "Suspend %s\n", rval < 0 ? "failed" : "ok");
+
+	return rval;
+}
+
+static int as3645a_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct as3645a *flash = to_as3645a(subdev);
+	int rval;
+
+	if (flash->power_count == 0)
+		return 0;
+
+	rval = __as3645a_set_power(flash, 1);
+
+	dev_dbg(&client->dev, "Resume %s\n", rval < 0 ? "fail" : "ok");
+
+	return rval;
+}
+
+#else
+
+#define as3645a_suspend	NULL
+#define as3645a_resume	NULL
+
+#endif /* CONFIG_PM */
+
+/*
+ * as3645a_init_controls - Create controls
+ * @flash: The flash
+ *
+ * The number of LEDs reported in platform data is used to compute default
+ * limits. Parameters passed through platform data can override those limits.
+ */
+static int __devinit as3645a_init_controls(struct as3645a *flash)
+{
+	const struct as3645a_platform_data *pdata = flash->pdata;
+	struct v4l2_ctrl *ctrl;
+	int maximum;
+
+	v4l2_ctrl_handler_init(&flash->ctrls, 10);
+
+	/* V4L2_CID_FLASH_LED_MODE */
+	v4l2_ctrl_new_std_menu(&flash->ctrls, &as3645a_ctrl_ops,
+			       V4L2_CID_FLASH_LED_MODE, 2, ~7,
+			       V4L2_FLASH_LED_MODE_NONE);
+
+	/* V4L2_CID_FLASH_STROBE_SOURCE */
+	v4l2_ctrl_new_std_menu(&flash->ctrls, &as3645a_ctrl_ops,
+			       V4L2_CID_FLASH_STROBE_SOURCE,
+			       pdata->ext_strobe ? 1 : 0,
+			       pdata->ext_strobe ? ~3 : ~1,
+			       V4L2_FLASH_STROBE_SOURCE_SOFTWARE);
+
+	flash->strobe_source = V4L2_FLASH_STROBE_SOURCE_SOFTWARE;
+
+	/* V4L2_CID_FLASH_STROBE */
+	v4l2_ctrl_new_std(&flash->ctrls, &as3645a_ctrl_ops,
+			  V4L2_CID_FLASH_STROBE, 0, 0, 0, 0);
+
+	/* V4L2_CID_FLASH_STROBE_STOP */
+	v4l2_ctrl_new_std(&flash->ctrls, &as3645a_ctrl_ops,
+			  V4L2_CID_FLASH_STROBE_STOP, 0, 0, 0, 0);
+
+	/* V4L2_CID_FLASH_STROBE_STATUS */
+	ctrl = v4l2_ctrl_new_std(&flash->ctrls, &as3645a_ctrl_ops,
+				 V4L2_CID_FLASH_STROBE_STATUS, 0, 1, 1, 1);
+	if (ctrl != NULL)
+		ctrl->flags |= V4L2_CTRL_FLAG_VOLATILE;
+
+	/* V4L2_CID_FLASH_TIMEOUT */
+	maximum = pdata->timeout_max;
+
+	v4l2_ctrl_new_std(&flash->ctrls, &as3645a_ctrl_ops,
+			  V4L2_CID_FLASH_TIMEOUT, AS3645A_FLASH_TIMEOUT_MIN,
+			  maximum, AS3645A_FLASH_TIMEOUT_STEP, maximum);
+
+	flash->timeout = maximum;
+
+	/* V4L2_CID_FLASH_INTENSITY */
+	maximum = pdata->flash_max_current;
+
+	v4l2_ctrl_new_std(&flash->ctrls, &as3645a_ctrl_ops,
+			  V4L2_CID_FLASH_INTENSITY, AS3645A_FLASH_INTENSITY_MIN,
+			  maximum, AS3645A_FLASH_INTENSITY_STEP, maximum);
+
+	flash->flash_current = (maximum - AS3645A_FLASH_INTENSITY_MIN)
+			     / AS3645A_FLASH_INTENSITY_STEP;
+
+	/* V4L2_CID_FLASH_TORCH_INTENSITY */
+	maximum = pdata->torch_max_current;
+
+	v4l2_ctrl_new_std(&flash->ctrls, &as3645a_ctrl_ops,
+			  V4L2_CID_FLASH_TORCH_INTENSITY,
+			  AS3645A_TORCH_INTENSITY_MIN, maximum,
+			  AS3645A_TORCH_INTENSITY_STEP,
+			  AS3645A_TORCH_INTENSITY_MIN);
+
+	flash->assist_current = 0;
+
+	/* V4L2_CID_FLASH_INDICATOR_INTENSITY */
+	v4l2_ctrl_new_std(&flash->ctrls, &as3645a_ctrl_ops,
+			  V4L2_CID_FLASH_INDICATOR_INTENSITY,
+			  AS3645A_INDICATOR_INTENSITY_MIN,
+			  AS3645A_INDICATOR_INTENSITY_MAX,
+			  AS3645A_INDICATOR_INTENSITY_STEP,
+			  AS3645A_INDICATOR_INTENSITY_MIN);
+
+	flash->indicator_current = 0;
+
+	/* V4L2_CID_FLASH_FAULT */
+	ctrl = v4l2_ctrl_new_std(&flash->ctrls, &as3645a_ctrl_ops,
+				 V4L2_CID_FLASH_FAULT, 0,
+				 V4L2_FLASH_FAULT_OVER_VOLTAGE |
+				 V4L2_FLASH_FAULT_TIMEOUT |
+				 V4L2_FLASH_FAULT_OVER_TEMPERATURE |
+				 V4L2_FLASH_FAULT_SHORT_CIRCUIT, 0, 0);
+	if (ctrl != NULL)
+		ctrl->flags |= V4L2_CTRL_FLAG_VOLATILE;
+
+	flash->subdev.ctrl_handler = &flash->ctrls;
+
+	return flash->ctrls.error;
+}
+
+static int __devinit as3645a_probe(struct i2c_client *client,
+				   const struct i2c_device_id *devid)
+{
+	struct as3645a *flash;
+	int ret;
+
+	if (client->dev.platform_data == NULL)
+		return -ENODEV;
+
+	flash = kzalloc(sizeof(*flash), GFP_KERNEL);
+	if (flash == NULL)
+		return -ENOMEM;
+
+	flash->pdata = client->dev.platform_data;
+
+	v4l2_i2c_subdev_init(&flash->subdev, client, &as3645a_ops);
+	flash->subdev.internal_ops = &as3645a_internal_ops;
+	flash->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	ret = as3645a_init_controls(flash);
+	if (ret < 0)
+		goto done;
+
+	ret = media_entity_init(&flash->subdev.entity, 0, NULL, 0);
+	if (ret < 0)
+		goto done;
+
+	flash->subdev.entity.type = MEDIA_ENT_T_V4L2_SUBDEV_FLASH;
+
+	mutex_init(&flash->power_lock);
+
+	flash->led_mode = V4L2_FLASH_LED_MODE_NONE;
+
+done:
+	if (ret < 0) {
+		v4l2_ctrl_handler_free(&flash->ctrls);
+		kfree(flash);
+	}
+
+	return ret;
+}
+
+static int __devexit as3645a_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct as3645a *flash = to_as3645a(subdev);
+
+	v4l2_device_unregister_subdev(subdev);
+	v4l2_ctrl_handler_free(&flash->ctrls);
+	media_entity_cleanup(&flash->subdev.entity);
+	mutex_destroy(&flash->power_lock);
+	kfree(flash);
+
+	return 0;
+}
+
+static const struct i2c_device_id as3645a_id_table[] = {
+	{ AS3645A_NAME, 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(i2c, as3645a_id_table);
+
+static const struct dev_pm_ops as3645a_pm_ops = {
+	.suspend = as3645a_suspend,
+	.resume = as3645a_resume,
+};
+
+static struct i2c_driver as3645a_i2c_driver = {
+	.driver	= {
+		.name = AS3645A_NAME,
+		.pm   = &as3645a_pm_ops,
+	},
+	.probe	= as3645a_probe,
+	.remove	= __devexit_p(as3645a_remove),
+	.id_table = as3645a_id_table,
+};
+
+module_i2c_driver(as3645a_i2c_driver);
+
+MODULE_AUTHOR("Laurent Pinchart <laurent.pinchart@ideasonboard.com>");
+MODULE_DESCRIPTION("LED flash driver for AS3645A, LM3555 and their clones");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/bt819.c b/drivers/media/i2c/bt819.c
new file mode 100644
index 000000000000..377bf05b1efd
--- /dev/null
+++ b/drivers/media/i2c/bt819.c
@@ -0,0 +1,517 @@
+/*
+ *  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.
+ *
+ * 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; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#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-chip-ident.h>
+#include <media/v4l2-ctrls.h>
+#include <media/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 ident;
+	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 Vertial Scaling msb 0x00
+					   chroma comb OFF, line drop scaling, interlace scaling
+					   BUG? Why does turning the chroma comb on fuck up color?
+					   Bug in the bt819 stepping on my board?
+					*/
+		0x14, 0x00,	/* 0x14 Vertial 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;
+
+	if ((status & 0x80))
+		res = 0;
+
+	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 int bt819_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ident *chip)
+{
+	struct bt819 *decoder = to_bt819(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return v4l2_chip_ident_i2c_client(client, chip, decoder->ident, 0);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_ctrl_ops bt819_ctrl_ops = {
+	.s_ctrl = bt819_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops bt819_core_ops = {
+	.g_chip_ident = bt819_g_chip_ident,
+	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
+	.try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
+	.s_ext_ctrls = v4l2_subdev_s_ext_ctrls,
+	.g_ctrl = v4l2_subdev_g_ctrl,
+	.s_ctrl = v4l2_subdev_s_ctrl,
+	.queryctrl = v4l2_subdev_queryctrl,
+	.querymenu = v4l2_subdev_querymenu,
+	.s_std = bt819_s_std,
+};
+
+static const struct v4l2_subdev_video_ops bt819_video_ops = {
+	.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 = {
+	.core = &bt819_core_ops,
+	.video = &bt819_video_ops,
+};
+
+/* ----------------------------------------------------------------------- */
+
+static int bt819_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	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 = kzalloc(sizeof(struct bt819), 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";
+		decoder->ident = V4L2_IDENT_BT819A;
+		break;
+	case 0x60:
+		name = "bt817a";
+		decoder->ident = V4L2_IDENT_BT817A;
+		break;
+	case 0x20:
+		name = "bt815a";
+		decoder->ident = V4L2_IDENT_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);
+		kfree(decoder);
+		return err;
+	}
+	v4l2_ctrl_handler_setup(&decoder->hdl);
+	return 0;
+}
+
+static int 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);
+	kfree(decoder);
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+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 = {
+		.owner	= THIS_MODULE,
+		.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 000000000000..7e5bd365c239
--- /dev/null
+++ b/drivers/media/i2c/bt856.c
@@ -0,0 +1,273 @@
+/*
+ * 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)
+ *
+ * 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; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <linux/ioctl.h>
+#include <asm/uaccess.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.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 int bt856_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ident *chip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_BT856, 0);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_core_ops bt856_core_ops = {
+	.g_chip_ident = bt856_g_chip_ident,
+	.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,
+			const struct i2c_device_id *id)
+{
+	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 = kzalloc(sizeof(struct bt856), 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 int bt856_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	kfree(to_bt856(sd));
+	return 0;
+}
+
+static const struct i2c_device_id bt856_id[] = {
+	{ "bt856", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, bt856_id);
+
+static struct i2c_driver bt856_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.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 000000000000..905320b67a1c
--- /dev/null
+++ b/drivers/media/i2c/bt866.c
@@ -0,0 +1,243 @@
+/*
+    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.
+
+    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; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+*/
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <linux/ioctl.h>
+#include <asm/uaccess.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.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 int bt866_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ident *chip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_BT866, 0);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_core_ops bt866_core_ops = {
+	.g_chip_ident = bt866_g_chip_ident,
+};
+
+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 = {
+	.core = &bt866_core_ops,
+	.video = &bt866_video_ops,
+};
+
+static int bt866_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct bt866 *encoder;
+	struct v4l2_subdev *sd;
+
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	encoder = kzalloc(sizeof(*encoder), GFP_KERNEL);
+	if (encoder == NULL)
+		return -ENOMEM;
+	sd = &encoder->sd;
+	v4l2_i2c_subdev_init(sd, client, &bt866_ops);
+	return 0;
+}
+
+static int bt866_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	kfree(to_bt866(sd));
+	return 0;
+}
+
+static const struct i2c_device_id bt866_id[] = {
+	{ "bt866", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, bt866_id);
+
+static struct i2c_driver bt866_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "bt866",
+	},
+	.probe		= bt866_probe,
+	.remove		= bt866_remove,
+	.id_table	= bt866_id,
+};
+
+module_i2c_driver(bt866_driver);
diff --git a/drivers/media/i2c/btcx-risc.c b/drivers/media/i2c/btcx-risc.c
new file mode 100644
index 000000000000..ac1b2687a20d
--- /dev/null
+++ b/drivers/media/i2c/btcx-risc.c
@@ -0,0 +1,260 @@
+/*
+
+    btcx-risc.c
+
+    bt848/bt878/cx2388x risc code generator.
+
+    (c) 2000-03 Gerd Knorr <kraxel@bytesex.org> [SuSE Labs]
+
+    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; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/pci.h>
+#include <linux/interrupt.h>
+#include <linux/videodev2.h>
+#include <asm/page.h>
+#include <asm/pgtable.h>
+
+#include "btcx-risc.h"
+
+MODULE_DESCRIPTION("some code shared by bttv and cx88xx drivers");
+MODULE_AUTHOR("Gerd Knorr");
+MODULE_LICENSE("GPL");
+
+static unsigned int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug,"debug messages, default is 0 (no)");
+
+/* ---------------------------------------------------------- */
+/* allocate/free risc memory                                  */
+
+static int memcnt;
+
+void btcx_riscmem_free(struct pci_dev *pci,
+		       struct btcx_riscmem *risc)
+{
+	if (NULL == risc->cpu)
+		return;
+	if (debug) {
+		memcnt--;
+		printk("btcx: riscmem free [%d] dma=%lx\n",
+		       memcnt, (unsigned long)risc->dma);
+	}
+	pci_free_consistent(pci, risc->size, risc->cpu, risc->dma);
+	memset(risc,0,sizeof(*risc));
+}
+
+int btcx_riscmem_alloc(struct pci_dev *pci,
+		       struct btcx_riscmem *risc,
+		       unsigned int size)
+{
+	__le32 *cpu;
+	dma_addr_t dma = 0;
+
+	if (NULL != risc->cpu && risc->size < size)
+		btcx_riscmem_free(pci,risc);
+	if (NULL == risc->cpu) {
+		cpu = pci_alloc_consistent(pci, size, &dma);
+		if (NULL == cpu)
+			return -ENOMEM;
+		risc->cpu  = cpu;
+		risc->dma  = dma;
+		risc->size = size;
+		if (debug) {
+			memcnt++;
+			printk("btcx: riscmem alloc [%d] dma=%lx cpu=%p size=%d\n",
+			       memcnt, (unsigned long)dma, cpu, size);
+		}
+	}
+	memset(risc->cpu,0,risc->size);
+	return 0;
+}
+
+/* ---------------------------------------------------------- */
+/* screen overlay helpers                                     */
+
+int
+btcx_screen_clips(int swidth, int sheight, struct v4l2_rect *win,
+		  struct v4l2_clip *clips, unsigned int n)
+{
+	if (win->left < 0) {
+		/* left */
+		clips[n].c.left = 0;
+		clips[n].c.top = 0;
+		clips[n].c.width  = -win->left;
+		clips[n].c.height = win->height;
+		n++;
+	}
+	if (win->left + win->width > swidth) {
+		/* right */
+		clips[n].c.left   = swidth - win->left;
+		clips[n].c.top    = 0;
+		clips[n].c.width  = win->width - clips[n].c.left;
+		clips[n].c.height = win->height;
+		n++;
+	}
+	if (win->top < 0) {
+		/* top */
+		clips[n].c.left = 0;
+		clips[n].c.top = 0;
+		clips[n].c.width  = win->width;
+		clips[n].c.height = -win->top;
+		n++;
+	}
+	if (win->top + win->height > sheight) {
+		/* bottom */
+		clips[n].c.left = 0;
+		clips[n].c.top = sheight - win->top;
+		clips[n].c.width  = win->width;
+		clips[n].c.height = win->height - clips[n].c.top;
+		n++;
+	}
+	return n;
+}
+
+int
+btcx_align(struct v4l2_rect *win, struct v4l2_clip *clips, unsigned int n, int mask)
+{
+	s32 nx,nw,dx;
+	unsigned int i;
+
+	/* fixup window */
+	nx = (win->left + mask) & ~mask;
+	nw = (win->width) & ~mask;
+	if (nx + nw > win->left + win->width)
+		nw -= mask+1;
+	dx = nx - win->left;
+	win->left  = nx;
+	win->width = nw;
+	if (debug)
+		printk(KERN_DEBUG "btcx: window align %dx%d+%d+%d [dx=%d]\n",
+		       win->width, win->height, win->left, win->top, dx);
+
+	/* fixup clips */
+	for (i = 0; i < n; i++) {
+		nx = (clips[i].c.left-dx) & ~mask;
+		nw = (clips[i].c.width) & ~mask;
+		if (nx + nw < clips[i].c.left-dx + clips[i].c.width)
+			nw += mask+1;
+		clips[i].c.left  = nx;
+		clips[i].c.width = nw;
+		if (debug)
+			printk(KERN_DEBUG "btcx:   clip align %dx%d+%d+%d\n",
+			       clips[i].c.width, clips[i].c.height,
+			       clips[i].c.left, clips[i].c.top);
+	}
+	return 0;
+}
+
+void
+btcx_sort_clips(struct v4l2_clip *clips, unsigned int nclips)
+{
+	struct v4l2_clip swap;
+	int i,j,n;
+
+	if (nclips < 2)
+		return;
+	for (i = nclips-2; i >= 0; i--) {
+		for (n = 0, j = 0; j <= i; j++) {
+			if (clips[j].c.left > clips[j+1].c.left) {
+				swap = clips[j];
+				clips[j] = clips[j+1];
+				clips[j+1] = swap;
+				n++;
+			}
+		}
+		if (0 == n)
+			break;
+	}
+}
+
+void
+btcx_calc_skips(int line, int width, int *maxy,
+		struct btcx_skiplist *skips, unsigned int *nskips,
+		const struct v4l2_clip *clips, unsigned int nclips)
+{
+	unsigned int clip,skip;
+	int end, maxline;
+
+	skip=0;
+	maxline = 9999;
+	for (clip = 0; clip < nclips; clip++) {
+
+		/* sanity checks */
+		if (clips[clip].c.left + clips[clip].c.width <= 0)
+			continue;
+		if (clips[clip].c.left > (signed)width)
+			break;
+
+		/* vertical range */
+		if (line > clips[clip].c.top+clips[clip].c.height-1)
+			continue;
+		if (line < clips[clip].c.top) {
+			if (maxline > clips[clip].c.top-1)
+				maxline = clips[clip].c.top-1;
+			continue;
+		}
+		if (maxline > clips[clip].c.top+clips[clip].c.height-1)
+			maxline = clips[clip].c.top+clips[clip].c.height-1;
+
+		/* horizontal range */
+		if (0 == skip || clips[clip].c.left > skips[skip-1].end) {
+			/* new one */
+			skips[skip].start = clips[clip].c.left;
+			if (skips[skip].start < 0)
+				skips[skip].start = 0;
+			skips[skip].end = clips[clip].c.left + clips[clip].c.width;
+			if (skips[skip].end > width)
+				skips[skip].end = width;
+			skip++;
+		} else {
+			/* overlaps -- expand last one */
+			end = clips[clip].c.left + clips[clip].c.width;
+			if (skips[skip-1].end < end)
+				skips[skip-1].end = end;
+			if (skips[skip-1].end > width)
+				skips[skip-1].end = width;
+		}
+	}
+	*nskips = skip;
+	*maxy = maxline;
+
+	if (debug) {
+		printk(KERN_DEBUG "btcx: skips line %d-%d:",line,maxline);
+		for (skip = 0; skip < *nskips; skip++) {
+			printk(" %d-%d",skips[skip].start,skips[skip].end);
+		}
+		printk("\n");
+	}
+}
+
+/* ---------------------------------------------------------- */
+
+EXPORT_SYMBOL(btcx_riscmem_alloc);
+EXPORT_SYMBOL(btcx_riscmem_free);
+
+EXPORT_SYMBOL(btcx_screen_clips);
+EXPORT_SYMBOL(btcx_align);
+EXPORT_SYMBOL(btcx_sort_clips);
+EXPORT_SYMBOL(btcx_calc_skips);
+
+/*
+ * Local variables:
+ * c-basic-offset: 8
+ * End:
+ */
diff --git a/drivers/media/i2c/btcx-risc.h b/drivers/media/i2c/btcx-risc.h
new file mode 100644
index 000000000000..f8bc6e8e7b51
--- /dev/null
+++ b/drivers/media/i2c/btcx-risc.h
@@ -0,0 +1,34 @@
+/*
+ */
+struct btcx_riscmem {
+	unsigned int   size;
+	__le32         *cpu;
+	__le32         *jmp;
+	dma_addr_t     dma;
+};
+
+struct btcx_skiplist {
+	int start;
+	int end;
+};
+
+int  btcx_riscmem_alloc(struct pci_dev *pci,
+			struct btcx_riscmem *risc,
+			unsigned int size);
+void btcx_riscmem_free(struct pci_dev *pci,
+		       struct btcx_riscmem *risc);
+
+int btcx_screen_clips(int swidth, int sheight, struct v4l2_rect *win,
+		      struct v4l2_clip *clips, unsigned int n);
+int btcx_align(struct v4l2_rect *win, struct v4l2_clip *clips,
+	       unsigned int n, int mask);
+void btcx_sort_clips(struct v4l2_clip *clips, unsigned int nclips);
+void btcx_calc_skips(int line, int width, int *maxy,
+		     struct btcx_skiplist *skips, unsigned int *nskips,
+		     const struct v4l2_clip *clips, unsigned int nclips);
+
+/*
+ * Local variables:
+ * c-basic-offset: 8
+ * End:
+ */
diff --git a/drivers/media/i2c/cs5345.c b/drivers/media/i2c/cs5345.c
new file mode 100644
index 000000000000..c8581e26fa9c
--- /dev/null
+++ b/drivers/media/i2c/cs5345.c
@@ -0,0 +1,252 @@
+/*
+ * cs5345 Cirrus Logic 24-bit, 192 kHz Stereo Audio ADC
+ * Copyright (C) 2007 Hans Verkuil
+ *
+ * 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; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+
+#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-chip-ident.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)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	reg->size = 1;
+	reg->val = cs5345_read(sd, reg->reg & 0x1f);
+	return 0;
+}
+
+static int cs5345_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	cs5345_write(sd, reg->reg & 0x1f, reg->val & 0xff);
+	return 0;
+}
+#endif
+
+static int cs5345_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ident *chip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_CS5345, 0);
+}
+
+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,
+	.g_chip_ident = cs5345_g_chip_ident,
+	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
+	.try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
+	.s_ext_ctrls = v4l2_subdev_s_ext_ctrls,
+	.g_ctrl = v4l2_subdev_g_ctrl,
+	.s_ctrl = v4l2_subdev_s_ctrl,
+	.queryctrl = v4l2_subdev_queryctrl,
+	.querymenu = v4l2_subdev_querymenu,
+#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,
+			const struct i2c_device_id *id)
+{
+	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 = kzalloc(sizeof(struct cs5345_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);
+		kfree(state);
+		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 int 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);
+	kfree(state);
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id cs5345_id[] = {
+	{ "cs5345", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, cs5345_id);
+
+static struct i2c_driver cs5345_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.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 000000000000..b293912206eb
--- /dev/null
+++ b/drivers/media/i2c/cs53l32a.c
@@ -0,0 +1,251 @@
+/*
+ * 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
+ *
+ * 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; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <linux/ioctl.h>
+#include <asm/uaccess.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.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_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ident *chip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return v4l2_chip_ident_i2c_client(client,
+			chip, V4L2_IDENT_CS53l32A, 0);
+}
+
+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,
+	.g_chip_ident = cs53l32a_g_chip_ident,
+	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
+	.try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
+	.s_ext_ctrls = v4l2_subdev_s_ext_ctrls,
+	.g_ctrl = v4l2_subdev_g_ctrl,
+	.s_ctrl = v4l2_subdev_s_ctrl,
+	.queryctrl = v4l2_subdev_queryctrl,
+	.querymenu = v4l2_subdev_querymenu,
+};
+
+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)
+{
+	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)
+		strlcpy(client->name, "cs53l32a", sizeof(client->name));
+
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	state = kzalloc(sizeof(struct cs53l32a_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);
+		kfree(state);
+		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 int 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);
+	kfree(state);
+	return 0;
+}
+
+static const struct i2c_device_id cs53l32a_id[] = {
+	{ "cs53l32a", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, cs53l32a_id);
+
+static struct i2c_driver cs53l32a_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "cs53l32a",
+	},
+	.probe		= cs53l32a_probe,
+	.remove		= cs53l32a_remove,
+	.id_table	= cs53l32a_id,
+};
+
+module_i2c_driver(cs53l32a_driver);
diff --git a/drivers/media/i2c/cx2341x.c b/drivers/media/i2c/cx2341x.c
new file mode 100644
index 000000000000..103ef6bad2e2
--- /dev/null
+++ b/drivers/media/i2c/cx2341x.c
@@ -0,0 +1,1726 @@
+/*
+ * cx2341x - generic code for cx23415/6/8 based devices
+ *
+ * Copyright (C) 2006 Hans Verkuil <hverkuil@xs4all.nl>
+ *
+ * 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; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/videodev2.h>
+
+#include <media/tuner.h>
+#include <media/cx2341x.h>
+#include <media/v4l2-common.h>
+
+MODULE_DESCRIPTION("cx23415/6/8 driver");
+MODULE_AUTHOR("Hans Verkuil");
+MODULE_LICENSE("GPL");
+
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "Debug level (0-1)");
+
+/********************** COMMON CODE *********************/
+
+/* definitions for audio properties bits 29-28 */
+#define CX2341X_AUDIO_ENCODING_METHOD_MPEG	0
+#define CX2341X_AUDIO_ENCODING_METHOD_AC3	1
+#define CX2341X_AUDIO_ENCODING_METHOD_LPCM	2
+
+static const char *cx2341x_get_name(u32 id)
+{
+	switch (id) {
+	case V4L2_CID_MPEG_CX2341X_VIDEO_SPATIAL_FILTER_MODE:
+		return "Spatial Filter Mode";
+	case V4L2_CID_MPEG_CX2341X_VIDEO_SPATIAL_FILTER:
+		return "Spatial Filter";
+	case V4L2_CID_MPEG_CX2341X_VIDEO_LUMA_SPATIAL_FILTER_TYPE:
+		return "Spatial Luma Filter Type";
+	case V4L2_CID_MPEG_CX2341X_VIDEO_CHROMA_SPATIAL_FILTER_TYPE:
+		return "Spatial Chroma Filter Type";
+	case V4L2_CID_MPEG_CX2341X_VIDEO_TEMPORAL_FILTER_MODE:
+		return "Temporal Filter Mode";
+	case V4L2_CID_MPEG_CX2341X_VIDEO_TEMPORAL_FILTER:
+		return "Temporal Filter";
+	case V4L2_CID_MPEG_CX2341X_VIDEO_MEDIAN_FILTER_TYPE:
+		return "Median Filter Type";
+	case V4L2_CID_MPEG_CX2341X_VIDEO_LUMA_MEDIAN_FILTER_TOP:
+		return "Median Luma Filter Maximum";
+	case V4L2_CID_MPEG_CX2341X_VIDEO_LUMA_MEDIAN_FILTER_BOTTOM:
+		return "Median Luma Filter Minimum";
+	case V4L2_CID_MPEG_CX2341X_VIDEO_CHROMA_MEDIAN_FILTER_TOP:
+		return "Median Chroma Filter Maximum";
+	case V4L2_CID_MPEG_CX2341X_VIDEO_CHROMA_MEDIAN_FILTER_BOTTOM:
+		return "Median Chroma Filter Minimum";
+	case V4L2_CID_MPEG_CX2341X_STREAM_INSERT_NAV_PACKETS:
+		return "Insert Navigation Packets";
+	}
+	return NULL;
+}
+
+static const char **cx2341x_get_menu(u32 id)
+{
+	static const char *cx2341x_video_spatial_filter_mode_menu[] = {
+		"Manual",
+		"Auto",
+		NULL
+	};
+
+	static const char *cx2341x_video_luma_spatial_filter_type_menu[] = {
+		"Off",
+		"1D Horizontal",
+		"1D Vertical",
+		"2D H/V Separable",
+		"2D Symmetric non-separable",
+		NULL
+	};
+
+	static const char *cx2341x_video_chroma_spatial_filter_type_menu[] = {
+		"Off",
+		"1D Horizontal",
+		NULL
+	};
+
+	static const char *cx2341x_video_temporal_filter_mode_menu[] = {
+		"Manual",
+		"Auto",
+		NULL
+	};
+
+	static const char *cx2341x_video_median_filter_type_menu[] = {
+		"Off",
+		"Horizontal",
+		"Vertical",
+		"Horizontal/Vertical",
+		"Diagonal",
+		NULL
+	};
+
+	switch (id) {
+	case V4L2_CID_MPEG_CX2341X_VIDEO_SPATIAL_FILTER_MODE:
+		return cx2341x_video_spatial_filter_mode_menu;
+	case V4L2_CID_MPEG_CX2341X_VIDEO_LUMA_SPATIAL_FILTER_TYPE:
+		return cx2341x_video_luma_spatial_filter_type_menu;
+	case V4L2_CID_MPEG_CX2341X_VIDEO_CHROMA_SPATIAL_FILTER_TYPE:
+		return cx2341x_video_chroma_spatial_filter_type_menu;
+	case V4L2_CID_MPEG_CX2341X_VIDEO_TEMPORAL_FILTER_MODE:
+		return cx2341x_video_temporal_filter_mode_menu;
+	case V4L2_CID_MPEG_CX2341X_VIDEO_MEDIAN_FILTER_TYPE:
+		return cx2341x_video_median_filter_type_menu;
+	}
+	return NULL;
+}
+
+static void cx2341x_ctrl_fill(u32 id, const char **name, enum v4l2_ctrl_type *type,
+		    s32 *min, s32 *max, s32 *step, s32 *def, u32 *flags)
+{
+	*name = cx2341x_get_name(id);
+	*flags = 0;
+
+	switch (id) {
+	case V4L2_CID_MPEG_CX2341X_VIDEO_SPATIAL_FILTER_MODE:
+	case V4L2_CID_MPEG_CX2341X_VIDEO_LUMA_SPATIAL_FILTER_TYPE:
+	case V4L2_CID_MPEG_CX2341X_VIDEO_CHROMA_SPATIAL_FILTER_TYPE:
+	case V4L2_CID_MPEG_CX2341X_VIDEO_TEMPORAL_FILTER_MODE:
+	case V4L2_CID_MPEG_CX2341X_VIDEO_MEDIAN_FILTER_TYPE:
+		*type = V4L2_CTRL_TYPE_MENU;
+		*min = 0;
+		*step = 0;
+		break;
+	case V4L2_CID_MPEG_CX2341X_STREAM_INSERT_NAV_PACKETS:
+		*type = V4L2_CTRL_TYPE_BOOLEAN;
+		*min = 0;
+		*max = *step = 1;
+		break;
+	default:
+		*type = V4L2_CTRL_TYPE_INTEGER;
+		break;
+	}
+	switch (id) {
+	case V4L2_CID_MPEG_CX2341X_VIDEO_SPATIAL_FILTER_MODE:
+	case V4L2_CID_MPEG_CX2341X_VIDEO_TEMPORAL_FILTER_MODE:
+	case V4L2_CID_MPEG_CX2341X_VIDEO_MEDIAN_FILTER_TYPE:
+		*flags |= V4L2_CTRL_FLAG_UPDATE;
+		break;
+	case V4L2_CID_MPEG_CX2341X_VIDEO_SPATIAL_FILTER:
+	case V4L2_CID_MPEG_CX2341X_VIDEO_TEMPORAL_FILTER:
+	case V4L2_CID_MPEG_CX2341X_VIDEO_LUMA_MEDIAN_FILTER_TOP:
+	case V4L2_CID_MPEG_CX2341X_VIDEO_LUMA_MEDIAN_FILTER_BOTTOM:
+	case V4L2_CID_MPEG_CX2341X_VIDEO_CHROMA_MEDIAN_FILTER_TOP:
+	case V4L2_CID_MPEG_CX2341X_VIDEO_CHROMA_MEDIAN_FILTER_BOTTOM:
+		*flags |= V4L2_CTRL_FLAG_SLIDER;
+		break;
+	case V4L2_CID_MPEG_VIDEO_ENCODING:
+		*flags |= V4L2_CTRL_FLAG_READ_ONLY;
+		break;
+	}
+}
+
+
+/********************** OLD CODE *********************/
+
+/* Must be sorted from low to high control ID! */
+const u32 cx2341x_mpeg_ctrls[] = {
+	V4L2_CID_MPEG_CLASS,
+	V4L2_CID_MPEG_STREAM_TYPE,
+	V4L2_CID_MPEG_STREAM_VBI_FMT,
+	V4L2_CID_MPEG_AUDIO_SAMPLING_FREQ,
+	V4L2_CID_MPEG_AUDIO_ENCODING,
+	V4L2_CID_MPEG_AUDIO_L2_BITRATE,
+	V4L2_CID_MPEG_AUDIO_MODE,
+	V4L2_CID_MPEG_AUDIO_MODE_EXTENSION,
+	V4L2_CID_MPEG_AUDIO_EMPHASIS,
+	V4L2_CID_MPEG_AUDIO_CRC,
+	V4L2_CID_MPEG_AUDIO_MUTE,
+	V4L2_CID_MPEG_AUDIO_AC3_BITRATE,
+	V4L2_CID_MPEG_VIDEO_ENCODING,
+	V4L2_CID_MPEG_VIDEO_ASPECT,
+	V4L2_CID_MPEG_VIDEO_B_FRAMES,
+	V4L2_CID_MPEG_VIDEO_GOP_SIZE,
+	V4L2_CID_MPEG_VIDEO_GOP_CLOSURE,
+	V4L2_CID_MPEG_VIDEO_BITRATE_MODE,
+	V4L2_CID_MPEG_VIDEO_BITRATE,
+	V4L2_CID_MPEG_VIDEO_BITRATE_PEAK,
+	V4L2_CID_MPEG_VIDEO_TEMPORAL_DECIMATION,
+	V4L2_CID_MPEG_VIDEO_MUTE,
+	V4L2_CID_MPEG_VIDEO_MUTE_YUV,
+	V4L2_CID_MPEG_CX2341X_VIDEO_SPATIAL_FILTER_MODE,
+	V4L2_CID_MPEG_CX2341X_VIDEO_SPATIAL_FILTER,
+	V4L2_CID_MPEG_CX2341X_VIDEO_LUMA_SPATIAL_FILTER_TYPE,
+	V4L2_CID_MPEG_CX2341X_VIDEO_CHROMA_SPATIAL_FILTER_TYPE,
+	V4L2_CID_MPEG_CX2341X_VIDEO_TEMPORAL_FILTER_MODE,
+	V4L2_CID_MPEG_CX2341X_VIDEO_TEMPORAL_FILTER,
+	V4L2_CID_MPEG_CX2341X_VIDEO_MEDIAN_FILTER_TYPE,
+	V4L2_CID_MPEG_CX2341X_VIDEO_LUMA_MEDIAN_FILTER_BOTTOM,
+	V4L2_CID_MPEG_CX2341X_VIDEO_LUMA_MEDIAN_FILTER_TOP,
+	V4L2_CID_MPEG_CX2341X_VIDEO_CHROMA_MEDIAN_FILTER_BOTTOM,
+	V4L2_CID_MPEG_CX2341X_VIDEO_CHROMA_MEDIAN_FILTER_TOP,
+	V4L2_CID_MPEG_CX2341X_STREAM_INSERT_NAV_PACKETS,
+	0
+};
+EXPORT_SYMBOL(cx2341x_mpeg_ctrls);
+
+static const struct cx2341x_mpeg_params default_params = {
+	/* misc */
+	.capabilities = 0,
+	.port = CX2341X_PORT_MEMORY,
+	.width = 720,
+	.height = 480,
+	.is_50hz = 0,
+
+	/* stream */
+	.stream_type = V4L2_MPEG_STREAM_TYPE_MPEG2_PS,
+	.stream_vbi_fmt = V4L2_MPEG_STREAM_VBI_FMT_NONE,
+	.stream_insert_nav_packets = 0,
+
+	/* audio */
+	.audio_sampling_freq = V4L2_MPEG_AUDIO_SAMPLING_FREQ_48000,
+	.audio_encoding = V4L2_MPEG_AUDIO_ENCODING_LAYER_2,
+	.audio_l2_bitrate = V4L2_MPEG_AUDIO_L2_BITRATE_224K,
+	.audio_ac3_bitrate = V4L2_MPEG_AUDIO_AC3_BITRATE_224K,
+	.audio_mode = V4L2_MPEG_AUDIO_MODE_STEREO,
+	.audio_mode_extension = V4L2_MPEG_AUDIO_MODE_EXTENSION_BOUND_4,
+	.audio_emphasis = V4L2_MPEG_AUDIO_EMPHASIS_NONE,
+	.audio_crc = V4L2_MPEG_AUDIO_CRC_NONE,
+	.audio_mute = 0,
+
+	/* video */
+	.video_encoding = V4L2_MPEG_VIDEO_ENCODING_MPEG_2,
+	.video_aspect = V4L2_MPEG_VIDEO_ASPECT_4x3,
+	.video_b_frames = 2,
+	.video_gop_size = 12,
+	.video_gop_closure = 1,
+	.video_bitrate_mode = V4L2_MPEG_VIDEO_BITRATE_MODE_VBR,
+	.video_bitrate = 6000000,
+	.video_bitrate_peak = 8000000,
+	.video_temporal_decimation = 0,
+	.video_mute = 0,
+	.video_mute_yuv = 0x008080,  /* YCbCr value for black */
+
+	/* encoding filters */
+	.video_spatial_filter_mode =
+		V4L2_MPEG_CX2341X_VIDEO_SPATIAL_FILTER_MODE_MANUAL,
+	.video_spatial_filter = 0,
+	.video_luma_spatial_filter_type =
+		V4L2_MPEG_CX2341X_VIDEO_LUMA_SPATIAL_FILTER_TYPE_1D_HOR,
+	.video_chroma_spatial_filter_type =
+		V4L2_MPEG_CX2341X_VIDEO_CHROMA_SPATIAL_FILTER_TYPE_1D_HOR,
+	.video_temporal_filter_mode =
+		V4L2_MPEG_CX2341X_VIDEO_TEMPORAL_FILTER_MODE_MANUAL,
+	.video_temporal_filter = 8,
+	.video_median_filter_type =
+		V4L2_MPEG_CX2341X_VIDEO_MEDIAN_FILTER_TYPE_OFF,
+	.video_luma_median_filter_top = 255,
+	.video_luma_median_filter_bottom = 0,
+	.video_chroma_median_filter_top = 255,
+	.video_chroma_median_filter_bottom = 0,
+};
+/* Map the control ID to the correct field in the cx2341x_mpeg_params
+   struct. Return -EINVAL if the ID is unknown, else return 0. */
+static int cx2341x_get_ctrl(const struct cx2341x_mpeg_params *params,
+		struct v4l2_ext_control *ctrl)
+{
+	switch (ctrl->id) {
+	case V4L2_CID_MPEG_AUDIO_SAMPLING_FREQ:
+		ctrl->value = params->audio_sampling_freq;
+		break;
+	case V4L2_CID_MPEG_AUDIO_ENCODING:
+		ctrl->value = params->audio_encoding;
+		break;
+	case V4L2_CID_MPEG_AUDIO_L2_BITRATE:
+		ctrl->value = params->audio_l2_bitrate;
+		break;
+	case V4L2_CID_MPEG_AUDIO_AC3_BITRATE:
+		ctrl->value = params->audio_ac3_bitrate;
+		break;
+	case V4L2_CID_MPEG_AUDIO_MODE:
+		ctrl->value = params->audio_mode;
+		break;
+	case V4L2_CID_MPEG_AUDIO_MODE_EXTENSION:
+		ctrl->value = params->audio_mode_extension;
+		break;
+	case V4L2_CID_MPEG_AUDIO_EMPHASIS:
+		ctrl->value = params->audio_emphasis;
+		break;
+	case V4L2_CID_MPEG_AUDIO_CRC:
+		ctrl->value = params->audio_crc;
+		break;
+	case V4L2_CID_MPEG_AUDIO_MUTE:
+		ctrl->value = params->audio_mute;
+		break;
+	case V4L2_CID_MPEG_VIDEO_ENCODING:
+		ctrl->value = params->video_encoding;
+		break;
+	case V4L2_CID_MPEG_VIDEO_ASPECT:
+		ctrl->value = params->video_aspect;
+		break;
+	case V4L2_CID_MPEG_VIDEO_B_FRAMES:
+		ctrl->value = params->video_b_frames;
+		break;
+	case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
+		ctrl->value = params->video_gop_size;
+		break;
+	case V4L2_CID_MPEG_VIDEO_GOP_CLOSURE:
+		ctrl->value = params->video_gop_closure;
+		break;
+	case V4L2_CID_MPEG_VIDEO_BITRATE_MODE:
+		ctrl->value = params->video_bitrate_mode;
+		break;
+	case V4L2_CID_MPEG_VIDEO_BITRATE:
+		ctrl->value = params->video_bitrate;
+		break;
+	case V4L2_CID_MPEG_VIDEO_BITRATE_PEAK:
+		ctrl->value = params->video_bitrate_peak;
+		break;
+	case V4L2_CID_MPEG_VIDEO_TEMPORAL_DECIMATION:
+		ctrl->value = params->video_temporal_decimation;
+		break;
+	case V4L2_CID_MPEG_VIDEO_MUTE:
+		ctrl->value = params->video_mute;
+		break;
+	case V4L2_CID_MPEG_VIDEO_MUTE_YUV:
+		ctrl->value = params->video_mute_yuv;
+		break;
+	case V4L2_CID_MPEG_STREAM_TYPE:
+		ctrl->value = params->stream_type;
+		break;
+	case V4L2_CID_MPEG_STREAM_VBI_FMT:
+		ctrl->value = params->stream_vbi_fmt;
+		break;
+	case V4L2_CID_MPEG_CX2341X_VIDEO_SPATIAL_FILTER_MODE:
+		ctrl->value = params->video_spatial_filter_mode;
+		break;
+	case V4L2_CID_MPEG_CX2341X_VIDEO_SPATIAL_FILTER:
+		ctrl->value = params->video_spatial_filter;
+		break;
+	case V4L2_CID_MPEG_CX2341X_VIDEO_LUMA_SPATIAL_FILTER_TYPE:
+		ctrl->value = params->video_luma_spatial_filter_type;
+		break;
+	case V4L2_CID_MPEG_CX2341X_VIDEO_CHROMA_SPATIAL_FILTER_TYPE:
+		ctrl->value = params->video_chroma_spatial_filter_type;
+		break;
+	case V4L2_CID_MPEG_CX2341X_VIDEO_TEMPORAL_FILTER_MODE:
+		ctrl->value = params->video_temporal_filter_mode;
+		break;
+	case V4L2_CID_MPEG_CX2341X_VIDEO_TEMPORAL_FILTER:
+		ctrl->value = params->video_temporal_filter;
+		break;
+	case V4L2_CID_MPEG_CX2341X_VIDEO_MEDIAN_FILTER_TYPE:
+		ctrl->value = params->video_median_filter_type;
+		break;
+	case V4L2_CID_MPEG_CX2341X_VIDEO_LUMA_MEDIAN_FILTER_TOP:
+		ctrl->value = params->video_luma_median_filter_top;
+		break;
+	case V4L2_CID_MPEG_CX2341X_VIDEO_LUMA_MEDIAN_FILTER_BOTTOM:
+		ctrl->value = params->video_luma_median_filter_bottom;
+		break;
+	case V4L2_CID_MPEG_CX2341X_VIDEO_CHROMA_MEDIAN_FILTER_TOP:
+		ctrl->value = params->video_chroma_median_filter_top;
+		break;
+	case V4L2_CID_MPEG_CX2341X_VIDEO_CHROMA_MEDIAN_FILTER_BOTTOM:
+		ctrl->value = params->video_chroma_median_filter_bottom;
+		break;
+	case V4L2_CID_MPEG_CX2341X_STREAM_INSERT_NAV_PACKETS:
+		ctrl->value = params->stream_insert_nav_packets;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/* Map the control ID to the correct field in the cx2341x_mpeg_params
+   struct. Return -EINVAL if the ID is unknown, else return 0. */
+static int cx2341x_set_ctrl(struct cx2341x_mpeg_params *params, int busy,
+		struct v4l2_ext_control *ctrl)
+{
+	switch (ctrl->id) {
+	case V4L2_CID_MPEG_AUDIO_SAMPLING_FREQ:
+		if (busy)
+			return -EBUSY;
+		params->audio_sampling_freq = ctrl->value;
+		break;
+	case V4L2_CID_MPEG_AUDIO_ENCODING:
+		if (busy)
+			return -EBUSY;
+		if (params->capabilities & CX2341X_CAP_HAS_AC3)
+			if (ctrl->value != V4L2_MPEG_AUDIO_ENCODING_LAYER_2 &&
+			    ctrl->value != V4L2_MPEG_AUDIO_ENCODING_AC3)
+				return -ERANGE;
+		params->audio_encoding = ctrl->value;
+		break;
+	case V4L2_CID_MPEG_AUDIO_L2_BITRATE:
+		if (busy)
+			return -EBUSY;
+		params->audio_l2_bitrate = ctrl->value;
+		break;
+	case V4L2_CID_MPEG_AUDIO_AC3_BITRATE:
+		if (busy)
+			return -EBUSY;
+		if (!(params->capabilities & CX2341X_CAP_HAS_AC3))
+			return -EINVAL;
+		params->audio_ac3_bitrate = ctrl->value;
+		break;
+	case V4L2_CID_MPEG_AUDIO_MODE:
+		params->audio_mode = ctrl->value;
+		break;
+	case V4L2_CID_MPEG_AUDIO_MODE_EXTENSION:
+		params->audio_mode_extension = ctrl->value;
+		break;
+	case V4L2_CID_MPEG_AUDIO_EMPHASIS:
+		params->audio_emphasis = ctrl->value;
+		break;
+	case V4L2_CID_MPEG_AUDIO_CRC:
+		params->audio_crc = ctrl->value;
+		break;
+	case V4L2_CID_MPEG_AUDIO_MUTE:
+		params->audio_mute = ctrl->value;
+		break;
+	case V4L2_CID_MPEG_VIDEO_ASPECT:
+		params->video_aspect = ctrl->value;
+		break;
+	case V4L2_CID_MPEG_VIDEO_B_FRAMES: {
+		int b = ctrl->value + 1;
+		int gop = params->video_gop_size;
+		params->video_b_frames = ctrl->value;
+		params->video_gop_size = b * ((gop + b - 1) / b);
+		/* Max GOP size = 34 */
+		while (params->video_gop_size > 34)
+			params->video_gop_size -= b;
+		break;
+	}
+	case V4L2_CID_MPEG_VIDEO_GOP_SIZE: {
+		int b = params->video_b_frames + 1;
+		int gop = ctrl->value;
+		params->video_gop_size = b * ((gop + b - 1) / b);
+		/* Max GOP size = 34 */
+		while (params->video_gop_size > 34)
+			params->video_gop_size -= b;
+		ctrl->value = params->video_gop_size;
+		break;
+	}
+	case V4L2_CID_MPEG_VIDEO_GOP_CLOSURE:
+		params->video_gop_closure = ctrl->value;
+		break;
+	case V4L2_CID_MPEG_VIDEO_BITRATE_MODE:
+		if (busy)
+			return -EBUSY;
+		/* MPEG-1 only allows CBR */
+		if (params->video_encoding == V4L2_MPEG_VIDEO_ENCODING_MPEG_1 &&
+		    ctrl->value != V4L2_MPEG_VIDEO_BITRATE_MODE_CBR)
+			return -EINVAL;
+		params->video_bitrate_mode = ctrl->value;
+		break;
+	case V4L2_CID_MPEG_VIDEO_BITRATE:
+		if (busy)
+			return -EBUSY;
+		params->video_bitrate = ctrl->value;
+		break;
+	case V4L2_CID_MPEG_VIDEO_BITRATE_PEAK:
+		if (busy)
+			return -EBUSY;
+		params->video_bitrate_peak = ctrl->value;
+		break;
+	case V4L2_CID_MPEG_VIDEO_TEMPORAL_DECIMATION:
+		params->video_temporal_decimation = ctrl->value;
+		break;
+	case V4L2_CID_MPEG_VIDEO_MUTE:
+		params->video_mute = (ctrl->value != 0);
+		break;
+	case V4L2_CID_MPEG_VIDEO_MUTE_YUV:
+		params->video_mute_yuv = ctrl->value;
+		break;
+	case V4L2_CID_MPEG_STREAM_TYPE:
+		if (busy)
+			return -EBUSY;
+		params->stream_type = ctrl->value;
+		params->video_encoding =
+		    (params->stream_type == V4L2_MPEG_STREAM_TYPE_MPEG1_SS ||
+		     params->stream_type == V4L2_MPEG_STREAM_TYPE_MPEG1_VCD) ?
+			V4L2_MPEG_VIDEO_ENCODING_MPEG_1 :
+			V4L2_MPEG_VIDEO_ENCODING_MPEG_2;
+		if (params->video_encoding == V4L2_MPEG_VIDEO_ENCODING_MPEG_1)
+			/* MPEG-1 implies CBR */
+			params->video_bitrate_mode =
+				V4L2_MPEG_VIDEO_BITRATE_MODE_CBR;
+		break;
+	case V4L2_CID_MPEG_STREAM_VBI_FMT:
+		params->stream_vbi_fmt = ctrl->value;
+		break;
+	case V4L2_CID_MPEG_CX2341X_VIDEO_SPATIAL_FILTER_MODE:
+		params->video_spatial_filter_mode = ctrl->value;
+		break;
+	case V4L2_CID_MPEG_CX2341X_VIDEO_SPATIAL_FILTER:
+		params->video_spatial_filter = ctrl->value;
+		break;
+	case V4L2_CID_MPEG_CX2341X_VIDEO_LUMA_SPATIAL_FILTER_TYPE:
+		params->video_luma_spatial_filter_type = ctrl->value;
+		break;
+	case V4L2_CID_MPEG_CX2341X_VIDEO_CHROMA_SPATIAL_FILTER_TYPE:
+		params->video_chroma_spatial_filter_type = ctrl->value;
+		break;
+	case V4L2_CID_MPEG_CX2341X_VIDEO_TEMPORAL_FILTER_MODE:
+		params->video_temporal_filter_mode = ctrl->value;
+		break;
+	case V4L2_CID_MPEG_CX2341X_VIDEO_TEMPORAL_FILTER:
+		params->video_temporal_filter = ctrl->value;
+		break;
+	case V4L2_CID_MPEG_CX2341X_VIDEO_MEDIAN_FILTER_TYPE:
+		params->video_median_filter_type = ctrl->value;
+		break;
+	case V4L2_CID_MPEG_CX2341X_VIDEO_LUMA_MEDIAN_FILTER_TOP:
+		params->video_luma_median_filter_top = ctrl->value;
+		break;
+	case V4L2_CID_MPEG_CX2341X_VIDEO_LUMA_MEDIAN_FILTER_BOTTOM:
+		params->video_luma_median_filter_bottom = ctrl->value;
+		break;
+	case V4L2_CID_MPEG_CX2341X_VIDEO_CHROMA_MEDIAN_FILTER_TOP:
+		params->video_chroma_median_filter_top = ctrl->value;
+		break;
+	case V4L2_CID_MPEG_CX2341X_VIDEO_CHROMA_MEDIAN_FILTER_BOTTOM:
+		params->video_chroma_median_filter_bottom = ctrl->value;
+		break;
+	case V4L2_CID_MPEG_CX2341X_STREAM_INSERT_NAV_PACKETS:
+		params->stream_insert_nav_packets = ctrl->value;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int cx2341x_ctrl_query_fill(struct v4l2_queryctrl *qctrl,
+				   s32 min, s32 max, s32 step, s32 def)
+{
+	const char *name;
+
+	switch (qctrl->id) {
+	/* MPEG controls */
+	case V4L2_CID_MPEG_CX2341X_VIDEO_SPATIAL_FILTER_MODE:
+	case V4L2_CID_MPEG_CX2341X_VIDEO_SPATIAL_FILTER:
+	case V4L2_CID_MPEG_CX2341X_VIDEO_LUMA_SPATIAL_FILTER_TYPE:
+	case V4L2_CID_MPEG_CX2341X_VIDEO_CHROMA_SPATIAL_FILTER_TYPE:
+	case V4L2_CID_MPEG_CX2341X_VIDEO_TEMPORAL_FILTER_MODE:
+	case V4L2_CID_MPEG_CX2341X_VIDEO_TEMPORAL_FILTER:
+	case V4L2_CID_MPEG_CX2341X_VIDEO_MEDIAN_FILTER_TYPE:
+	case V4L2_CID_MPEG_CX2341X_VIDEO_LUMA_MEDIAN_FILTER_TOP:
+	case V4L2_CID_MPEG_CX2341X_VIDEO_LUMA_MEDIAN_FILTER_BOTTOM:
+	case V4L2_CID_MPEG_CX2341X_VIDEO_CHROMA_MEDIAN_FILTER_TOP:
+	case V4L2_CID_MPEG_CX2341X_VIDEO_CHROMA_MEDIAN_FILTER_BOTTOM:
+	case V4L2_CID_MPEG_CX2341X_STREAM_INSERT_NAV_PACKETS:
+		cx2341x_ctrl_fill(qctrl->id, &name, &qctrl->type,
+				&min, &max, &step, &def, &qctrl->flags);
+		qctrl->minimum = min;
+		qctrl->maximum = max;
+		qctrl->step = step;
+		qctrl->default_value = def;
+		qctrl->reserved[0] = qctrl->reserved[1] = 0;
+		strlcpy(qctrl->name, name, sizeof(qctrl->name));
+		return 0;
+
+	default:
+		return v4l2_ctrl_query_fill(qctrl, min, max, step, def);
+	}
+}
+
+int cx2341x_ctrl_query(const struct cx2341x_mpeg_params *params,
+		       struct v4l2_queryctrl *qctrl)
+{
+	int err;
+
+	switch (qctrl->id) {
+	case V4L2_CID_MPEG_CLASS:
+		return v4l2_ctrl_query_fill(qctrl, 0, 0, 0, 0);
+	case V4L2_CID_MPEG_STREAM_TYPE:
+		return v4l2_ctrl_query_fill(qctrl,
+				V4L2_MPEG_STREAM_TYPE_MPEG2_PS,
+				V4L2_MPEG_STREAM_TYPE_MPEG2_SVCD, 1,
+				V4L2_MPEG_STREAM_TYPE_MPEG2_PS);
+
+	case V4L2_CID_MPEG_STREAM_VBI_FMT:
+		if (params->capabilities & CX2341X_CAP_HAS_SLICED_VBI)
+			return v4l2_ctrl_query_fill(qctrl,
+					V4L2_MPEG_STREAM_VBI_FMT_NONE,
+					V4L2_MPEG_STREAM_VBI_FMT_IVTV, 1,
+					V4L2_MPEG_STREAM_VBI_FMT_NONE);
+		return cx2341x_ctrl_query_fill(qctrl,
+				V4L2_MPEG_STREAM_VBI_FMT_NONE,
+				V4L2_MPEG_STREAM_VBI_FMT_NONE, 1,
+				default_params.stream_vbi_fmt);
+
+	case V4L2_CID_MPEG_AUDIO_SAMPLING_FREQ:
+		return v4l2_ctrl_query_fill(qctrl,
+				V4L2_MPEG_AUDIO_SAMPLING_FREQ_44100,
+				V4L2_MPEG_AUDIO_SAMPLING_FREQ_32000, 1,
+				V4L2_MPEG_AUDIO_SAMPLING_FREQ_48000);
+
+	case V4L2_CID_MPEG_AUDIO_ENCODING:
+		if (params->capabilities & CX2341X_CAP_HAS_AC3) {
+			/*
+			 * The state of L2 & AC3 bitrate controls can change
+			 * when this control changes, but v4l2_ctrl_query_fill()
+			 * already sets V4L2_CTRL_FLAG_UPDATE for
+			 * V4L2_CID_MPEG_AUDIO_ENCODING, so we don't here.
+			 */
+			return v4l2_ctrl_query_fill(qctrl,
+					V4L2_MPEG_AUDIO_ENCODING_LAYER_2,
+					V4L2_MPEG_AUDIO_ENCODING_AC3, 1,
+					default_params.audio_encoding);
+		}
+
+		return v4l2_ctrl_query_fill(qctrl,
+				V4L2_MPEG_AUDIO_ENCODING_LAYER_2,
+				V4L2_MPEG_AUDIO_ENCODING_LAYER_2, 1,
+				default_params.audio_encoding);
+
+	case V4L2_CID_MPEG_AUDIO_L2_BITRATE:
+		err = v4l2_ctrl_query_fill(qctrl,
+				V4L2_MPEG_AUDIO_L2_BITRATE_192K,
+				V4L2_MPEG_AUDIO_L2_BITRATE_384K, 1,
+				default_params.audio_l2_bitrate);
+		if (err)
+			return err;
+		if (params->capabilities & CX2341X_CAP_HAS_AC3 &&
+		    params->audio_encoding != V4L2_MPEG_AUDIO_ENCODING_LAYER_2)
+			qctrl->flags |= V4L2_CTRL_FLAG_INACTIVE;
+		return 0;
+
+	case V4L2_CID_MPEG_AUDIO_MODE:
+		return v4l2_ctrl_query_fill(qctrl,
+				V4L2_MPEG_AUDIO_MODE_STEREO,
+				V4L2_MPEG_AUDIO_MODE_MONO, 1,
+				V4L2_MPEG_AUDIO_MODE_STEREO);
+
+	case V4L2_CID_MPEG_AUDIO_MODE_EXTENSION:
+		err = v4l2_ctrl_query_fill(qctrl,
+				V4L2_MPEG_AUDIO_MODE_EXTENSION_BOUND_4,
+				V4L2_MPEG_AUDIO_MODE_EXTENSION_BOUND_16, 1,
+				V4L2_MPEG_AUDIO_MODE_EXTENSION_BOUND_4);
+		if (err == 0 &&
+		    params->audio_mode != V4L2_MPEG_AUDIO_MODE_JOINT_STEREO)
+			qctrl->flags |= V4L2_CTRL_FLAG_INACTIVE;
+		return err;
+
+	case V4L2_CID_MPEG_AUDIO_EMPHASIS:
+		return v4l2_ctrl_query_fill(qctrl,
+				V4L2_MPEG_AUDIO_EMPHASIS_NONE,
+				V4L2_MPEG_AUDIO_EMPHASIS_CCITT_J17, 1,
+				V4L2_MPEG_AUDIO_EMPHASIS_NONE);
+
+	case V4L2_CID_MPEG_AUDIO_CRC:
+		return v4l2_ctrl_query_fill(qctrl,
+				V4L2_MPEG_AUDIO_CRC_NONE,
+				V4L2_MPEG_AUDIO_CRC_CRC16, 1,
+				V4L2_MPEG_AUDIO_CRC_NONE);
+
+	case V4L2_CID_MPEG_AUDIO_MUTE:
+		return v4l2_ctrl_query_fill(qctrl, 0, 1, 1, 0);
+
+	case V4L2_CID_MPEG_AUDIO_AC3_BITRATE:
+		err = v4l2_ctrl_query_fill(qctrl,
+				V4L2_MPEG_AUDIO_AC3_BITRATE_48K,
+				V4L2_MPEG_AUDIO_AC3_BITRATE_448K, 1,
+				default_params.audio_ac3_bitrate);
+		if (err)
+			return err;
+		if (params->capabilities & CX2341X_CAP_HAS_AC3) {
+			if (params->audio_encoding !=
+						   V4L2_MPEG_AUDIO_ENCODING_AC3)
+				qctrl->flags |= V4L2_CTRL_FLAG_INACTIVE;
+		} else
+			qctrl->flags |= V4L2_CTRL_FLAG_DISABLED;
+		return 0;
+
+	case V4L2_CID_MPEG_VIDEO_ENCODING:
+		/* this setting is read-only for the cx2341x since the
+		   V4L2_CID_MPEG_STREAM_TYPE really determines the
+		   MPEG-1/2 setting */
+		err = v4l2_ctrl_query_fill(qctrl,
+					   V4L2_MPEG_VIDEO_ENCODING_MPEG_1,
+					   V4L2_MPEG_VIDEO_ENCODING_MPEG_2, 1,
+					   V4L2_MPEG_VIDEO_ENCODING_MPEG_2);
+		if (err == 0)
+			qctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+		return err;
+
+	case V4L2_CID_MPEG_VIDEO_ASPECT:
+		return v4l2_ctrl_query_fill(qctrl,
+				V4L2_MPEG_VIDEO_ASPECT_1x1,
+				V4L2_MPEG_VIDEO_ASPECT_221x100, 1,
+				V4L2_MPEG_VIDEO_ASPECT_4x3);
+
+	case V4L2_CID_MPEG_VIDEO_B_FRAMES:
+		return v4l2_ctrl_query_fill(qctrl, 0, 33, 1, 2);
+
+	case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
+		return v4l2_ctrl_query_fill(qctrl, 1, 34, 1,
+				params->is_50hz ? 12 : 15);
+
+	case V4L2_CID_MPEG_VIDEO_GOP_CLOSURE:
+		return v4l2_ctrl_query_fill(qctrl, 0, 1, 1, 1);
+
+	case V4L2_CID_MPEG_VIDEO_BITRATE_MODE:
+		err = v4l2_ctrl_query_fill(qctrl,
+				V4L2_MPEG_VIDEO_BITRATE_MODE_VBR,
+				V4L2_MPEG_VIDEO_BITRATE_MODE_CBR, 1,
+				V4L2_MPEG_VIDEO_BITRATE_MODE_VBR);
+		if (err == 0 &&
+		    params->video_encoding == V4L2_MPEG_VIDEO_ENCODING_MPEG_1)
+			qctrl->flags |= V4L2_CTRL_FLAG_INACTIVE;
+		return err;
+
+	case V4L2_CID_MPEG_VIDEO_BITRATE:
+		return v4l2_ctrl_query_fill(qctrl, 0, 27000000, 1, 6000000);
+
+	case V4L2_CID_MPEG_VIDEO_BITRATE_PEAK:
+		err = v4l2_ctrl_query_fill(qctrl, 0, 27000000, 1, 8000000);
+		if (err == 0 &&
+		    params->video_bitrate_mode ==
+				V4L2_MPEG_VIDEO_BITRATE_MODE_CBR)
+			qctrl->flags |= V4L2_CTRL_FLAG_INACTIVE;
+		return err;
+
+	case V4L2_CID_MPEG_VIDEO_TEMPORAL_DECIMATION:
+		return v4l2_ctrl_query_fill(qctrl, 0, 255, 1, 0);
+
+	case V4L2_CID_MPEG_VIDEO_MUTE:
+		return v4l2_ctrl_query_fill(qctrl, 0, 1, 1, 0);
+
+	case V4L2_CID_MPEG_VIDEO_MUTE_YUV:  /* Init YUV (really YCbCr) to black */
+		return v4l2_ctrl_query_fill(qctrl, 0, 0xffffff, 1, 0x008080);
+
+	/* CX23415/6 specific */
+	case V4L2_CID_MPEG_CX2341X_VIDEO_SPATIAL_FILTER_MODE:
+		return cx2341x_ctrl_query_fill(qctrl,
+			V4L2_MPEG_CX2341X_VIDEO_SPATIAL_FILTER_MODE_MANUAL,
+			V4L2_MPEG_CX2341X_VIDEO_SPATIAL_FILTER_MODE_AUTO, 1,
+			default_params.video_spatial_filter_mode);
+
+	case V4L2_CID_MPEG_CX2341X_VIDEO_SPATIAL_FILTER:
+		cx2341x_ctrl_query_fill(qctrl, 0, 15, 1,
+				default_params.video_spatial_filter);
+		qctrl->flags |= V4L2_CTRL_FLAG_SLIDER;
+		if (params->video_spatial_filter_mode ==
+			    V4L2_MPEG_CX2341X_VIDEO_SPATIAL_FILTER_MODE_AUTO)
+			qctrl->flags |= V4L2_CTRL_FLAG_INACTIVE;
+		return 0;
+
+	case V4L2_CID_MPEG_CX2341X_VIDEO_LUMA_SPATIAL_FILTER_TYPE:
+		cx2341x_ctrl_query_fill(qctrl,
+			V4L2_MPEG_CX2341X_VIDEO_LUMA_SPATIAL_FILTER_TYPE_OFF,
+			V4L2_MPEG_CX2341X_VIDEO_LUMA_SPATIAL_FILTER_TYPE_2D_SYM_NON_SEPARABLE,
+			1,
+			default_params.video_luma_spatial_filter_type);
+		if (params->video_spatial_filter_mode ==
+			    V4L2_MPEG_CX2341X_VIDEO_SPATIAL_FILTER_MODE_AUTO)
+			qctrl->flags |= V4L2_CTRL_FLAG_INACTIVE;
+		return 0;
+
+	case V4L2_CID_MPEG_CX2341X_VIDEO_CHROMA_SPATIAL_FILTER_TYPE:
+		cx2341x_ctrl_query_fill(qctrl,
+		    V4L2_MPEG_CX2341X_VIDEO_CHROMA_SPATIAL_FILTER_TYPE_OFF,
+		    V4L2_MPEG_CX2341X_VIDEO_CHROMA_SPATIAL_FILTER_TYPE_1D_HOR,
+		    1,
+		    default_params.video_chroma_spatial_filter_type);
+		if (params->video_spatial_filter_mode ==
+			V4L2_MPEG_CX2341X_VIDEO_SPATIAL_FILTER_MODE_AUTO)
+			qctrl->flags |= V4L2_CTRL_FLAG_INACTIVE;
+		return 0;
+
+	case V4L2_CID_MPEG_CX2341X_VIDEO_TEMPORAL_FILTER_MODE:
+		return cx2341x_ctrl_query_fill(qctrl,
+			V4L2_MPEG_CX2341X_VIDEO_TEMPORAL_FILTER_MODE_MANUAL,
+			V4L2_MPEG_CX2341X_VIDEO_TEMPORAL_FILTER_MODE_AUTO, 1,
+			default_params.video_temporal_filter_mode);
+
+	case V4L2_CID_MPEG_CX2341X_VIDEO_TEMPORAL_FILTER:
+		cx2341x_ctrl_query_fill(qctrl, 0, 31, 1,
+				default_params.video_temporal_filter);
+		qctrl->flags |= V4L2_CTRL_FLAG_SLIDER;
+		if (params->video_temporal_filter_mode ==
+			V4L2_MPEG_CX2341X_VIDEO_TEMPORAL_FILTER_MODE_AUTO)
+			qctrl->flags |= V4L2_CTRL_FLAG_INACTIVE;
+		return 0;
+
+	case V4L2_CID_MPEG_CX2341X_VIDEO_MEDIAN_FILTER_TYPE:
+		return cx2341x_ctrl_query_fill(qctrl,
+			V4L2_MPEG_CX2341X_VIDEO_MEDIAN_FILTER_TYPE_OFF,
+			V4L2_MPEG_CX2341X_VIDEO_MEDIAN_FILTER_TYPE_DIAG, 1,
+			default_params.video_median_filter_type);
+
+	case V4L2_CID_MPEG_CX2341X_VIDEO_LUMA_MEDIAN_FILTER_TOP:
+		cx2341x_ctrl_query_fill(qctrl, 0, 255, 1,
+				default_params.video_luma_median_filter_top);
+		qctrl->flags |= V4L2_CTRL_FLAG_SLIDER;
+		if (params->video_median_filter_type ==
+				V4L2_MPEG_CX2341X_VIDEO_MEDIAN_FILTER_TYPE_OFF)
+			qctrl->flags |= V4L2_CTRL_FLAG_INACTIVE;
+		return 0;
+
+	case V4L2_CID_MPEG_CX2341X_VIDEO_LUMA_MEDIAN_FILTER_BOTTOM:
+		cx2341x_ctrl_query_fill(qctrl, 0, 255, 1,
+				default_params.video_luma_median_filter_bottom);
+		qctrl->flags |= V4L2_CTRL_FLAG_SLIDER;
+		if (params->video_median_filter_type ==
+				V4L2_MPEG_CX2341X_VIDEO_MEDIAN_FILTER_TYPE_OFF)
+			qctrl->flags |= V4L2_CTRL_FLAG_INACTIVE;
+		return 0;
+
+	case V4L2_CID_MPEG_CX2341X_VIDEO_CHROMA_MEDIAN_FILTER_TOP:
+		cx2341x_ctrl_query_fill(qctrl, 0, 255, 1,
+				default_params.video_chroma_median_filter_top);
+		qctrl->flags |= V4L2_CTRL_FLAG_SLIDER;
+		if (params->video_median_filter_type ==
+				V4L2_MPEG_CX2341X_VIDEO_MEDIAN_FILTER_TYPE_OFF)
+			qctrl->flags |= V4L2_CTRL_FLAG_INACTIVE;
+		return 0;
+
+	case V4L2_CID_MPEG_CX2341X_VIDEO_CHROMA_MEDIAN_FILTER_BOTTOM:
+		cx2341x_ctrl_query_fill(qctrl, 0, 255, 1,
+			default_params.video_chroma_median_filter_bottom);
+		qctrl->flags |= V4L2_CTRL_FLAG_SLIDER;
+		if (params->video_median_filter_type ==
+				V4L2_MPEG_CX2341X_VIDEO_MEDIAN_FILTER_TYPE_OFF)
+			qctrl->flags |= V4L2_CTRL_FLAG_INACTIVE;
+		return 0;
+
+	case V4L2_CID_MPEG_CX2341X_STREAM_INSERT_NAV_PACKETS:
+		return cx2341x_ctrl_query_fill(qctrl, 0, 1, 1,
+				default_params.stream_insert_nav_packets);
+
+	default:
+		return -EINVAL;
+
+	}
+}
+EXPORT_SYMBOL(cx2341x_ctrl_query);
+
+const char * const *cx2341x_ctrl_get_menu(const struct cx2341x_mpeg_params *p, u32 id)
+{
+	static const char * const mpeg_stream_type_without_ts[] = {
+		"MPEG-2 Program Stream",
+		"",
+		"MPEG-1 System Stream",
+		"MPEG-2 DVD-compatible Stream",
+		"MPEG-1 VCD-compatible Stream",
+		"MPEG-2 SVCD-compatible Stream",
+		NULL
+	};
+
+	static const char *mpeg_stream_type_with_ts[] = {
+		"MPEG-2 Program Stream",
+		"MPEG-2 Transport Stream",
+		"MPEG-1 System Stream",
+		"MPEG-2 DVD-compatible Stream",
+		"MPEG-1 VCD-compatible Stream",
+		"MPEG-2 SVCD-compatible Stream",
+		NULL
+	};
+
+	static const char *mpeg_audio_encoding_l2_ac3[] = {
+		"",
+		"MPEG-1/2 Layer II",
+		"",
+		"",
+		"AC-3",
+		NULL
+	};
+
+	switch (id) {
+	case V4L2_CID_MPEG_STREAM_TYPE:
+		return (p->capabilities & CX2341X_CAP_HAS_TS) ?
+			mpeg_stream_type_with_ts : mpeg_stream_type_without_ts;
+	case V4L2_CID_MPEG_AUDIO_ENCODING:
+		return (p->capabilities & CX2341X_CAP_HAS_AC3) ?
+			mpeg_audio_encoding_l2_ac3 : v4l2_ctrl_get_menu(id);
+	case V4L2_CID_MPEG_AUDIO_L1_BITRATE:
+	case V4L2_CID_MPEG_AUDIO_L3_BITRATE:
+		return NULL;
+	case V4L2_CID_MPEG_CX2341X_VIDEO_SPATIAL_FILTER_MODE:
+	case V4L2_CID_MPEG_CX2341X_VIDEO_LUMA_SPATIAL_FILTER_TYPE:
+	case V4L2_CID_MPEG_CX2341X_VIDEO_CHROMA_SPATIAL_FILTER_TYPE:
+	case V4L2_CID_MPEG_CX2341X_VIDEO_TEMPORAL_FILTER_MODE:
+	case V4L2_CID_MPEG_CX2341X_VIDEO_MEDIAN_FILTER_TYPE:
+		return cx2341x_get_menu(id);
+	default:
+		return v4l2_ctrl_get_menu(id);
+	}
+}
+EXPORT_SYMBOL(cx2341x_ctrl_get_menu);
+
+static void cx2341x_calc_audio_properties(struct cx2341x_mpeg_params *params)
+{
+	params->audio_properties =
+		(params->audio_sampling_freq << 0) |
+		(params->audio_mode << 8) |
+		(params->audio_mode_extension << 10) |
+		(((params->audio_emphasis == V4L2_MPEG_AUDIO_EMPHASIS_CCITT_J17)
+		  ? 3 : params->audio_emphasis) << 12) |
+		(params->audio_crc << 14);
+
+	if ((params->capabilities & CX2341X_CAP_HAS_AC3) &&
+	    params->audio_encoding == V4L2_MPEG_AUDIO_ENCODING_AC3) {
+		params->audio_properties |=
+			/* Not sure if this MPEG Layer II setting is required */
+			((3 - V4L2_MPEG_AUDIO_ENCODING_LAYER_2) << 2) |
+			(params->audio_ac3_bitrate << 4) |
+			(CX2341X_AUDIO_ENCODING_METHOD_AC3 << 28);
+	} else {
+		/* Assuming MPEG Layer II */
+		params->audio_properties |=
+			((3 - params->audio_encoding) << 2) |
+			((1 + params->audio_l2_bitrate) << 4);
+	}
+}
+
+int cx2341x_ext_ctrls(struct cx2341x_mpeg_params *params, int busy,
+		  struct v4l2_ext_controls *ctrls, unsigned int cmd)
+{
+	int err = 0;
+	int i;
+
+	if (cmd == VIDIOC_G_EXT_CTRLS) {
+		for (i = 0; i < ctrls->count; i++) {
+			struct v4l2_ext_control *ctrl = ctrls->controls + i;
+
+			err = cx2341x_get_ctrl(params, ctrl);
+			if (err) {
+				ctrls->error_idx = i;
+				break;
+			}
+		}
+		return err;
+	}
+	for (i = 0; i < ctrls->count; i++) {
+		struct v4l2_ext_control *ctrl = ctrls->controls + i;
+		struct v4l2_queryctrl qctrl;
+		const char * const *menu_items = NULL;
+
+		qctrl.id = ctrl->id;
+		err = cx2341x_ctrl_query(params, &qctrl);
+		if (err)
+			break;
+		if (qctrl.type == V4L2_CTRL_TYPE_MENU)
+			menu_items = cx2341x_ctrl_get_menu(params, qctrl.id);
+		err = v4l2_ctrl_check(ctrl, &qctrl, menu_items);
+		if (err)
+			break;
+		err = cx2341x_set_ctrl(params, busy, ctrl);
+		if (err)
+			break;
+	}
+	if (err == 0 &&
+	    params->video_bitrate_mode == V4L2_MPEG_VIDEO_BITRATE_MODE_VBR &&
+	    params->video_bitrate_peak < params->video_bitrate) {
+		err = -ERANGE;
+		ctrls->error_idx = ctrls->count;
+	}
+	if (err)
+		ctrls->error_idx = i;
+	else
+		cx2341x_calc_audio_properties(params);
+	return err;
+}
+EXPORT_SYMBOL(cx2341x_ext_ctrls);
+
+void cx2341x_fill_defaults(struct cx2341x_mpeg_params *p)
+{
+	*p = default_params;
+	cx2341x_calc_audio_properties(p);
+}
+EXPORT_SYMBOL(cx2341x_fill_defaults);
+
+static int cx2341x_api(void *priv, cx2341x_mbox_func func,
+		       u32 cmd, int args, ...)
+{
+	u32 data[CX2341X_MBOX_MAX_DATA];
+	va_list vargs;
+	int i;
+
+	va_start(vargs, args);
+
+	for (i = 0; i < args; i++)
+		data[i] = va_arg(vargs, int);
+	va_end(vargs);
+	return func(priv, cmd, args, 0, data);
+}
+
+#define NEQ(field) (old->field != new->field)
+
+int cx2341x_update(void *priv, cx2341x_mbox_func func,
+		   const struct cx2341x_mpeg_params *old,
+		   const struct cx2341x_mpeg_params *new)
+{
+	static int mpeg_stream_type[] = {
+		0,	/* MPEG-2 PS */
+		1,	/* MPEG-2 TS */
+		2,	/* MPEG-1 SS */
+		14,	/* DVD */
+		11,	/* VCD */
+		12,	/* SVCD */
+	};
+
+	int err = 0;
+	int force = (old == NULL);
+	u16 temporal = new->video_temporal_filter;
+
+	cx2341x_api(priv, func, CX2341X_ENC_SET_OUTPUT_PORT, 2, new->port, 0);
+
+	if (force || NEQ(is_50hz)) {
+		err = cx2341x_api(priv, func, CX2341X_ENC_SET_FRAME_RATE, 1,
+				  new->is_50hz);
+		if (err) return err;
+	}
+
+	if (force || NEQ(width) || NEQ(height) || NEQ(video_encoding)) {
+		u16 w = new->width;
+		u16 h = new->height;
+
+		if (new->video_encoding == V4L2_MPEG_VIDEO_ENCODING_MPEG_1) {
+			w /= 2;
+			h /= 2;
+		}
+		err = cx2341x_api(priv, func, CX2341X_ENC_SET_FRAME_SIZE, 2,
+				  h, w);
+		if (err) return err;
+	}
+	if (force || NEQ(stream_type)) {
+		err = cx2341x_api(priv, func, CX2341X_ENC_SET_STREAM_TYPE, 1,
+				  mpeg_stream_type[new->stream_type]);
+		if (err) return err;
+	}
+	if (force || NEQ(video_aspect)) {
+		err = cx2341x_api(priv, func, CX2341X_ENC_SET_ASPECT_RATIO, 1,
+				  1 + new->video_aspect);
+		if (err) return err;
+	}
+	if (force || NEQ(video_b_frames) || NEQ(video_gop_size)) {
+		err = cx2341x_api(priv, func, CX2341X_ENC_SET_GOP_PROPERTIES, 2,
+				new->video_gop_size, new->video_b_frames + 1);
+		if (err) return err;
+	}
+	if (force || NEQ(video_gop_closure)) {
+		err = cx2341x_api(priv, func, CX2341X_ENC_SET_GOP_CLOSURE, 1,
+				  new->video_gop_closure);
+		if (err) return err;
+	}
+	if (force || NEQ(audio_properties)) {
+		err = cx2341x_api(priv, func, CX2341X_ENC_SET_AUDIO_PROPERTIES,
+				  1, new->audio_properties);
+		if (err) return err;
+	}
+	if (force || NEQ(audio_mute)) {
+		err = cx2341x_api(priv, func, CX2341X_ENC_MUTE_AUDIO, 1,
+				  new->audio_mute);
+		if (err) return err;
+	}
+	if (force || NEQ(video_bitrate_mode) || NEQ(video_bitrate) ||
+						NEQ(video_bitrate_peak)) {
+		err = cx2341x_api(priv, func, CX2341X_ENC_SET_BIT_RATE, 5,
+				new->video_bitrate_mode, new->video_bitrate,
+				new->video_bitrate_peak / 400, 0, 0);
+		if (err) return err;
+	}
+	if (force || NEQ(video_spatial_filter_mode) ||
+		     NEQ(video_temporal_filter_mode) ||
+		     NEQ(video_median_filter_type)) {
+		err = cx2341x_api(priv, func, CX2341X_ENC_SET_DNR_FILTER_MODE,
+				  2, new->video_spatial_filter_mode |
+					(new->video_temporal_filter_mode << 1),
+				new->video_median_filter_type);
+		if (err) return err;
+	}
+	if (force || NEQ(video_luma_median_filter_bottom) ||
+		     NEQ(video_luma_median_filter_top) ||
+		     NEQ(video_chroma_median_filter_bottom) ||
+		     NEQ(video_chroma_median_filter_top)) {
+		err = cx2341x_api(priv, func, CX2341X_ENC_SET_CORING_LEVELS, 4,
+				new->video_luma_median_filter_bottom,
+				new->video_luma_median_filter_top,
+				new->video_chroma_median_filter_bottom,
+				new->video_chroma_median_filter_top);
+		if (err) return err;
+	}
+	if (force || NEQ(video_luma_spatial_filter_type) ||
+		     NEQ(video_chroma_spatial_filter_type)) {
+		err = cx2341x_api(priv, func,
+				  CX2341X_ENC_SET_SPATIAL_FILTER_TYPE,
+				  2, new->video_luma_spatial_filter_type,
+				  new->video_chroma_spatial_filter_type);
+		if (err) return err;
+	}
+	if (force || NEQ(video_spatial_filter) ||
+		     old->video_temporal_filter != temporal) {
+		err = cx2341x_api(priv, func, CX2341X_ENC_SET_DNR_FILTER_PROPS,
+				  2, new->video_spatial_filter, temporal);
+		if (err) return err;
+	}
+	if (force || NEQ(video_temporal_decimation)) {
+		err = cx2341x_api(priv, func, CX2341X_ENC_SET_FRAME_DROP_RATE,
+				  1, new->video_temporal_decimation);
+		if (err) return err;
+	}
+	if (force || NEQ(video_mute) ||
+		(new->video_mute && NEQ(video_mute_yuv))) {
+		err = cx2341x_api(priv, func, CX2341X_ENC_MUTE_VIDEO, 1,
+				new->video_mute | (new->video_mute_yuv << 8));
+		if (err) return err;
+	}
+	if (force || NEQ(stream_insert_nav_packets)) {
+		err = cx2341x_api(priv, func, CX2341X_ENC_MISC, 2,
+				7, new->stream_insert_nav_packets);
+		if (err) return err;
+	}
+	return 0;
+}
+EXPORT_SYMBOL(cx2341x_update);
+
+static const char *cx2341x_menu_item(const struct cx2341x_mpeg_params *p, u32 id)
+{
+	const char * const *menu = cx2341x_ctrl_get_menu(p, id);
+	struct v4l2_ext_control ctrl;
+
+	if (menu == NULL)
+		goto invalid;
+	ctrl.id = id;
+	if (cx2341x_get_ctrl(p, &ctrl))
+		goto invalid;
+	while (ctrl.value-- && *menu) menu++;
+	if (*menu == NULL)
+		goto invalid;
+	return *menu;
+
+invalid:
+	return "<invalid>";
+}
+
+void cx2341x_log_status(const struct cx2341x_mpeg_params *p, const char *prefix)
+{
+	int is_mpeg1 = p->video_encoding == V4L2_MPEG_VIDEO_ENCODING_MPEG_1;
+
+	/* Stream */
+	printk(KERN_INFO "%s: Stream: %s",
+		prefix,
+		cx2341x_menu_item(p, V4L2_CID_MPEG_STREAM_TYPE));
+	if (p->stream_insert_nav_packets)
+		printk(" (with navigation packets)");
+	printk("\n");
+	printk(KERN_INFO "%s: VBI Format: %s\n",
+		prefix,
+		cx2341x_menu_item(p, V4L2_CID_MPEG_STREAM_VBI_FMT));
+
+	/* Video */
+	printk(KERN_INFO "%s: Video:  %dx%d, %d fps%s\n",
+		prefix,
+		p->width / (is_mpeg1 ? 2 : 1), p->height / (is_mpeg1 ? 2 : 1),
+		p->is_50hz ? 25 : 30,
+		(p->video_mute) ? " (muted)" : "");
+	printk(KERN_INFO "%s: Video:  %s, %s, %s, %d",
+		prefix,
+		cx2341x_menu_item(p, V4L2_CID_MPEG_VIDEO_ENCODING),
+		cx2341x_menu_item(p, V4L2_CID_MPEG_VIDEO_ASPECT),
+		cx2341x_menu_item(p, V4L2_CID_MPEG_VIDEO_BITRATE_MODE),
+		p->video_bitrate);
+	if (p->video_bitrate_mode == V4L2_MPEG_VIDEO_BITRATE_MODE_VBR)
+		printk(", Peak %d", p->video_bitrate_peak);
+	printk("\n");
+	printk(KERN_INFO
+		"%s: Video:  GOP Size %d, %d B-Frames, %sGOP Closure\n",
+		prefix,
+		p->video_gop_size, p->video_b_frames,
+		p->video_gop_closure ? "" : "No ");
+	if (p->video_temporal_decimation)
+		printk(KERN_INFO "%s: Video: Temporal Decimation %d\n",
+			prefix, p->video_temporal_decimation);
+
+	/* Audio */
+	printk(KERN_INFO "%s: Audio:  %s, %s, %s, %s%s",
+		prefix,
+		cx2341x_menu_item(p, V4L2_CID_MPEG_AUDIO_SAMPLING_FREQ),
+		cx2341x_menu_item(p, V4L2_CID_MPEG_AUDIO_ENCODING),
+		cx2341x_menu_item(p,
+			   p->audio_encoding == V4L2_MPEG_AUDIO_ENCODING_AC3
+					      ? V4L2_CID_MPEG_AUDIO_AC3_BITRATE
+					      : V4L2_CID_MPEG_AUDIO_L2_BITRATE),
+		cx2341x_menu_item(p, V4L2_CID_MPEG_AUDIO_MODE),
+		p->audio_mute ? " (muted)" : "");
+	if (p->audio_mode == V4L2_MPEG_AUDIO_MODE_JOINT_STEREO)
+		printk(", %s", cx2341x_menu_item(p,
+				V4L2_CID_MPEG_AUDIO_MODE_EXTENSION));
+	printk(", %s, %s\n",
+		cx2341x_menu_item(p, V4L2_CID_MPEG_AUDIO_EMPHASIS),
+		cx2341x_menu_item(p, V4L2_CID_MPEG_AUDIO_CRC));
+
+	/* Encoding filters */
+	printk(KERN_INFO "%s: Spatial Filter:  %s, Luma %s, Chroma %s, %d\n",
+		prefix,
+		cx2341x_menu_item(p,
+		    V4L2_CID_MPEG_CX2341X_VIDEO_SPATIAL_FILTER_MODE),
+		cx2341x_menu_item(p,
+		    V4L2_CID_MPEG_CX2341X_VIDEO_LUMA_SPATIAL_FILTER_TYPE),
+		cx2341x_menu_item(p,
+		    V4L2_CID_MPEG_CX2341X_VIDEO_CHROMA_SPATIAL_FILTER_TYPE),
+		p->video_spatial_filter);
+
+	printk(KERN_INFO "%s: Temporal Filter: %s, %d\n",
+		prefix,
+		cx2341x_menu_item(p,
+			V4L2_CID_MPEG_CX2341X_VIDEO_TEMPORAL_FILTER_MODE),
+		p->video_temporal_filter);
+	printk(KERN_INFO
+		"%s: Median Filter:   %s, Luma [%d, %d], Chroma [%d, %d]\n",
+		prefix,
+		cx2341x_menu_item(p,
+			V4L2_CID_MPEG_CX2341X_VIDEO_MEDIAN_FILTER_TYPE),
+		p->video_luma_median_filter_bottom,
+		p->video_luma_median_filter_top,
+		p->video_chroma_median_filter_bottom,
+		p->video_chroma_median_filter_top);
+}
+EXPORT_SYMBOL(cx2341x_log_status);
+
+
+
+/********************** NEW CODE *********************/
+
+static inline struct cx2341x_handler *to_cxhdl(struct v4l2_ctrl *ctrl)
+{
+	return container_of(ctrl->handler, struct cx2341x_handler, hdl);
+}
+
+static int cx2341x_hdl_api(struct cx2341x_handler *hdl,
+		       u32 cmd, int args, ...)
+{
+	u32 data[CX2341X_MBOX_MAX_DATA];
+	va_list vargs;
+	int i;
+
+	va_start(vargs, args);
+
+	for (i = 0; i < args; i++)
+		data[i] = va_arg(vargs, int);
+	va_end(vargs);
+	return hdl->func(hdl->priv, cmd, args, 0, data);
+}
+
+/* ctrl->handler->lock is held, so it is safe to access cur.val */
+static inline int cx2341x_neq(struct v4l2_ctrl *ctrl)
+{
+	return ctrl && ctrl->val != ctrl->cur.val;
+}
+
+static int cx2341x_try_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct cx2341x_handler *hdl = to_cxhdl(ctrl);
+	s32 val = ctrl->val;
+
+	switch (ctrl->id) {
+	case V4L2_CID_MPEG_VIDEO_B_FRAMES: {
+		/* video gop cluster */
+		int b = val + 1;
+		int gop = hdl->video_gop_size->val;
+
+		gop = b * ((gop + b - 1) / b);
+
+		/* Max GOP size = 34 */
+		while (gop > 34)
+			gop -= b;
+		hdl->video_gop_size->val = gop;
+		break;
+	}
+
+	case V4L2_CID_MPEG_STREAM_TYPE:
+		/* stream type cluster */
+		hdl->video_encoding->val =
+		    (hdl->stream_type->val == V4L2_MPEG_STREAM_TYPE_MPEG1_SS ||
+		     hdl->stream_type->val == V4L2_MPEG_STREAM_TYPE_MPEG1_VCD) ?
+			V4L2_MPEG_VIDEO_ENCODING_MPEG_1 :
+			V4L2_MPEG_VIDEO_ENCODING_MPEG_2;
+		if (hdl->video_encoding->val == V4L2_MPEG_VIDEO_ENCODING_MPEG_1)
+			/* MPEG-1 implies CBR */
+			hdl->video_bitrate_mode->val =
+				V4L2_MPEG_VIDEO_BITRATE_MODE_CBR;
+		/* peak bitrate shall be >= normal bitrate */
+		if (hdl->video_bitrate_mode->val == V4L2_MPEG_VIDEO_BITRATE_MODE_VBR &&
+		    hdl->video_bitrate_peak->val < hdl->video_bitrate->val)
+			hdl->video_bitrate_peak->val = hdl->video_bitrate->val;
+		break;
+	}
+	return 0;
+}
+
+static int cx2341x_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	static const int mpeg_stream_type[] = {
+		0,	/* MPEG-2 PS */
+		1,	/* MPEG-2 TS */
+		2,	/* MPEG-1 SS */
+		14,	/* DVD */
+		11,	/* VCD */
+		12,	/* SVCD */
+	};
+	struct cx2341x_handler *hdl = to_cxhdl(ctrl);
+	s32 val = ctrl->val;
+	u32 props;
+	int err;
+
+	switch (ctrl->id) {
+	case V4L2_CID_MPEG_STREAM_VBI_FMT:
+		if (hdl->ops && hdl->ops->s_stream_vbi_fmt)
+			return hdl->ops->s_stream_vbi_fmt(hdl, val);
+		return 0;
+
+	case V4L2_CID_MPEG_VIDEO_ASPECT:
+		return cx2341x_hdl_api(hdl,
+			CX2341X_ENC_SET_ASPECT_RATIO, 1, val + 1);
+
+	case V4L2_CID_MPEG_VIDEO_GOP_CLOSURE:
+		return cx2341x_hdl_api(hdl, CX2341X_ENC_SET_GOP_CLOSURE, 1, val);
+
+	case V4L2_CID_MPEG_AUDIO_MUTE:
+		return cx2341x_hdl_api(hdl, CX2341X_ENC_MUTE_AUDIO, 1, val);
+
+	case V4L2_CID_MPEG_VIDEO_TEMPORAL_DECIMATION:
+		return cx2341x_hdl_api(hdl,
+			CX2341X_ENC_SET_FRAME_DROP_RATE, 1, val);
+
+	case V4L2_CID_MPEG_CX2341X_STREAM_INSERT_NAV_PACKETS:
+		return cx2341x_hdl_api(hdl, CX2341X_ENC_MISC, 2, 7, val);
+
+	case V4L2_CID_MPEG_AUDIO_SAMPLING_FREQ:
+		/* audio properties cluster */
+		props = (hdl->audio_sampling_freq->val << 0) |
+			(hdl->audio_mode->val << 8) |
+			(hdl->audio_mode_extension->val << 10) |
+			(hdl->audio_crc->val << 14);
+		if (hdl->audio_emphasis->val == V4L2_MPEG_AUDIO_EMPHASIS_CCITT_J17)
+			props |= 3 << 12;
+		else
+			props |= hdl->audio_emphasis->val << 12;
+
+		if (hdl->audio_encoding->val == V4L2_MPEG_AUDIO_ENCODING_AC3) {
+			props |=
+#if 1
+				/* Not sure if this MPEG Layer II setting is required */
+				((3 - V4L2_MPEG_AUDIO_ENCODING_LAYER_2) << 2) |
+#endif
+				(hdl->audio_ac3_bitrate->val << 4) |
+				(CX2341X_AUDIO_ENCODING_METHOD_AC3 << 28);
+		} else {
+			/* Assuming MPEG Layer II */
+			props |=
+				((3 - hdl->audio_encoding->val) << 2) |
+				((1 + hdl->audio_l2_bitrate->val) << 4);
+		}
+		err = cx2341x_hdl_api(hdl,
+				CX2341X_ENC_SET_AUDIO_PROPERTIES, 1, props);
+		if (err)
+			return err;
+
+		hdl->audio_properties = props;
+		if (hdl->audio_ac3_bitrate) {
+			int is_ac3 = hdl->audio_encoding->val ==
+						V4L2_MPEG_AUDIO_ENCODING_AC3;
+
+			v4l2_ctrl_activate(hdl->audio_ac3_bitrate, is_ac3);
+			v4l2_ctrl_activate(hdl->audio_l2_bitrate, !is_ac3);
+		}
+		v4l2_ctrl_activate(hdl->audio_mode_extension,
+			hdl->audio_mode->val == V4L2_MPEG_AUDIO_MODE_JOINT_STEREO);
+		if (cx2341x_neq(hdl->audio_sampling_freq) &&
+		    hdl->ops && hdl->ops->s_audio_sampling_freq)
+			return hdl->ops->s_audio_sampling_freq(hdl, hdl->audio_sampling_freq->val);
+		if (cx2341x_neq(hdl->audio_mode) &&
+		    hdl->ops && hdl->ops->s_audio_mode)
+			return hdl->ops->s_audio_mode(hdl, hdl->audio_mode->val);
+		return 0;
+
+	case V4L2_CID_MPEG_VIDEO_B_FRAMES:
+		/* video gop cluster */
+		return cx2341x_hdl_api(hdl, CX2341X_ENC_SET_GOP_PROPERTIES, 2,
+				hdl->video_gop_size->val,
+				hdl->video_b_frames->val + 1);
+
+	case V4L2_CID_MPEG_STREAM_TYPE:
+		/* stream type cluster */
+		err = cx2341x_hdl_api(hdl,
+			CX2341X_ENC_SET_STREAM_TYPE, 1, mpeg_stream_type[val]);
+		if (err)
+			return err;
+
+		err = cx2341x_hdl_api(hdl, CX2341X_ENC_SET_BIT_RATE, 5,
+				hdl->video_bitrate_mode->val,
+				hdl->video_bitrate->val,
+				hdl->video_bitrate_peak->val / 400, 0, 0);
+		if (err)
+			return err;
+
+		v4l2_ctrl_activate(hdl->video_bitrate_mode,
+			hdl->video_encoding->val != V4L2_MPEG_VIDEO_ENCODING_MPEG_1);
+		v4l2_ctrl_activate(hdl->video_bitrate_peak,
+			hdl->video_bitrate_mode->val != V4L2_MPEG_VIDEO_BITRATE_MODE_CBR);
+		if (cx2341x_neq(hdl->video_encoding) &&
+		    hdl->ops && hdl->ops->s_video_encoding)
+			return hdl->ops->s_video_encoding(hdl, hdl->video_encoding->val);
+		return 0;
+
+	case V4L2_CID_MPEG_VIDEO_MUTE:
+		/* video mute cluster */
+		return cx2341x_hdl_api(hdl, CX2341X_ENC_MUTE_VIDEO, 1,
+				hdl->video_mute->val |
+					(hdl->video_mute_yuv->val << 8));
+
+	case V4L2_CID_MPEG_CX2341X_VIDEO_SPATIAL_FILTER_MODE: {
+		int active_filter;
+
+		/* video filter mode */
+		err = cx2341x_hdl_api(hdl, CX2341X_ENC_SET_DNR_FILTER_MODE, 2,
+				hdl->video_spatial_filter_mode->val |
+					(hdl->video_temporal_filter_mode->val << 1),
+				hdl->video_median_filter_type->val);
+		if (err)
+			return err;
+
+		active_filter = hdl->video_spatial_filter_mode->val !=
+				V4L2_MPEG_CX2341X_VIDEO_SPATIAL_FILTER_MODE_AUTO;
+		v4l2_ctrl_activate(hdl->video_spatial_filter, active_filter);
+		v4l2_ctrl_activate(hdl->video_luma_spatial_filter_type, active_filter);
+		v4l2_ctrl_activate(hdl->video_chroma_spatial_filter_type, active_filter);
+		active_filter = hdl->video_temporal_filter_mode->val !=
+				V4L2_MPEG_CX2341X_VIDEO_TEMPORAL_FILTER_MODE_AUTO;
+		v4l2_ctrl_activate(hdl->video_temporal_filter, active_filter);
+		active_filter = hdl->video_median_filter_type->val !=
+				V4L2_MPEG_CX2341X_VIDEO_MEDIAN_FILTER_TYPE_OFF;
+		v4l2_ctrl_activate(hdl->video_luma_median_filter_bottom, active_filter);
+		v4l2_ctrl_activate(hdl->video_luma_median_filter_top, active_filter);
+		v4l2_ctrl_activate(hdl->video_chroma_median_filter_bottom, active_filter);
+		v4l2_ctrl_activate(hdl->video_chroma_median_filter_top, active_filter);
+		return 0;
+	}
+
+	case V4L2_CID_MPEG_CX2341X_VIDEO_LUMA_SPATIAL_FILTER_TYPE:
+		/* video filter type cluster */
+		return cx2341x_hdl_api(hdl,
+				CX2341X_ENC_SET_SPATIAL_FILTER_TYPE, 2,
+				hdl->video_luma_spatial_filter_type->val,
+				hdl->video_chroma_spatial_filter_type->val);
+
+	case V4L2_CID_MPEG_CX2341X_VIDEO_SPATIAL_FILTER:
+		/* video filter cluster */
+		return cx2341x_hdl_api(hdl, CX2341X_ENC_SET_DNR_FILTER_PROPS, 2,
+				hdl->video_spatial_filter->val,
+				hdl->video_temporal_filter->val);
+
+	case V4L2_CID_MPEG_CX2341X_VIDEO_LUMA_MEDIAN_FILTER_TOP:
+		/* video median cluster */
+		return cx2341x_hdl_api(hdl, CX2341X_ENC_SET_CORING_LEVELS, 4,
+				hdl->video_luma_median_filter_bottom->val,
+				hdl->video_luma_median_filter_top->val,
+				hdl->video_chroma_median_filter_bottom->val,
+				hdl->video_chroma_median_filter_top->val);
+	}
+	return -EINVAL;
+}
+
+static const struct v4l2_ctrl_ops cx2341x_ops = {
+	.try_ctrl = cx2341x_try_ctrl,
+	.s_ctrl = cx2341x_s_ctrl,
+};
+
+static struct v4l2_ctrl *cx2341x_ctrl_new_custom(struct v4l2_ctrl_handler *hdl,
+			u32 id, s32 min, s32 max, s32 step, s32 def)
+{
+	struct v4l2_ctrl_config cfg;
+
+	cx2341x_ctrl_fill(id, &cfg.name, &cfg.type, &min, &max, &step, &def, &cfg.flags);
+	cfg.ops = &cx2341x_ops;
+	cfg.id = id;
+	cfg.min = min;
+	cfg.max = max;
+	cfg.def = def;
+	if (cfg.type == V4L2_CTRL_TYPE_MENU) {
+		cfg.step = 0;
+		cfg.menu_skip_mask = step;
+		cfg.qmenu = cx2341x_get_menu(id);
+	} else {
+		cfg.step = step;
+		cfg.menu_skip_mask = 0;
+	}
+	return v4l2_ctrl_new_custom(hdl, &cfg, NULL);
+}
+
+static struct v4l2_ctrl *cx2341x_ctrl_new_std(struct v4l2_ctrl_handler *hdl,
+			u32 id, s32 min, s32 max, s32 step, s32 def)
+{
+	return v4l2_ctrl_new_std(hdl, &cx2341x_ops, id, min, max, step, def);
+}
+
+static struct v4l2_ctrl *cx2341x_ctrl_new_menu(struct v4l2_ctrl_handler *hdl,
+			u32 id, s32 max, s32 mask, s32 def)
+{
+	return v4l2_ctrl_new_std_menu(hdl, &cx2341x_ops, id, max, mask, def);
+}
+
+int cx2341x_handler_init(struct cx2341x_handler *cxhdl,
+			 unsigned nr_of_controls_hint)
+{
+	struct v4l2_ctrl_handler *hdl = &cxhdl->hdl;
+	u32 caps = cxhdl->capabilities;
+	int has_sliced_vbi = caps & CX2341X_CAP_HAS_SLICED_VBI;
+	int has_ac3 = caps & CX2341X_CAP_HAS_AC3;
+	int has_ts = caps & CX2341X_CAP_HAS_TS;
+
+	cxhdl->width = 720;
+	cxhdl->height = 480;
+
+	v4l2_ctrl_handler_init(hdl, nr_of_controls_hint);
+
+	/* Add controls in ascending control ID order for fastest
+	   insertion time. */
+	cxhdl->stream_type = cx2341x_ctrl_new_menu(hdl,
+			V4L2_CID_MPEG_STREAM_TYPE,
+			V4L2_MPEG_STREAM_TYPE_MPEG2_SVCD, has_ts ? 0 : 2,
+			V4L2_MPEG_STREAM_TYPE_MPEG2_PS);
+	cxhdl->stream_vbi_fmt = cx2341x_ctrl_new_menu(hdl,
+			V4L2_CID_MPEG_STREAM_VBI_FMT,
+			V4L2_MPEG_STREAM_VBI_FMT_IVTV, has_sliced_vbi ? 0 : 2,
+			V4L2_MPEG_STREAM_VBI_FMT_NONE);
+	cxhdl->audio_sampling_freq = cx2341x_ctrl_new_menu(hdl,
+			V4L2_CID_MPEG_AUDIO_SAMPLING_FREQ,
+			V4L2_MPEG_AUDIO_SAMPLING_FREQ_32000, 0,
+			V4L2_MPEG_AUDIO_SAMPLING_FREQ_48000);
+	cxhdl->audio_encoding = cx2341x_ctrl_new_menu(hdl,
+			V4L2_CID_MPEG_AUDIO_ENCODING,
+			V4L2_MPEG_AUDIO_ENCODING_AC3, has_ac3 ? ~0x12 : ~0x2,
+			V4L2_MPEG_AUDIO_ENCODING_LAYER_2);
+	cxhdl->audio_l2_bitrate = cx2341x_ctrl_new_menu(hdl,
+			V4L2_CID_MPEG_AUDIO_L2_BITRATE,
+			V4L2_MPEG_AUDIO_L2_BITRATE_384K, 0x1ff,
+			V4L2_MPEG_AUDIO_L2_BITRATE_224K);
+	cxhdl->audio_mode = cx2341x_ctrl_new_menu(hdl,
+			V4L2_CID_MPEG_AUDIO_MODE,
+			V4L2_MPEG_AUDIO_MODE_MONO, 0,
+			V4L2_MPEG_AUDIO_MODE_STEREO);
+	cxhdl->audio_mode_extension = cx2341x_ctrl_new_menu(hdl,
+			V4L2_CID_MPEG_AUDIO_MODE_EXTENSION,
+			V4L2_MPEG_AUDIO_MODE_EXTENSION_BOUND_16, 0,
+			V4L2_MPEG_AUDIO_MODE_EXTENSION_BOUND_4);
+	cxhdl->audio_emphasis = cx2341x_ctrl_new_menu(hdl,
+			V4L2_CID_MPEG_AUDIO_EMPHASIS,
+			V4L2_MPEG_AUDIO_EMPHASIS_CCITT_J17, 0,
+			V4L2_MPEG_AUDIO_EMPHASIS_NONE);
+	cxhdl->audio_crc = cx2341x_ctrl_new_menu(hdl,
+			V4L2_CID_MPEG_AUDIO_CRC,
+			V4L2_MPEG_AUDIO_CRC_CRC16, 0,
+			V4L2_MPEG_AUDIO_CRC_NONE);
+
+	cx2341x_ctrl_new_std(hdl, V4L2_CID_MPEG_AUDIO_MUTE, 0, 1, 1, 0);
+	if (has_ac3)
+		cxhdl->audio_ac3_bitrate = cx2341x_ctrl_new_menu(hdl,
+				V4L2_CID_MPEG_AUDIO_AC3_BITRATE,
+				V4L2_MPEG_AUDIO_AC3_BITRATE_448K, 0x03,
+				V4L2_MPEG_AUDIO_AC3_BITRATE_224K);
+	cxhdl->video_encoding = cx2341x_ctrl_new_menu(hdl,
+			V4L2_CID_MPEG_VIDEO_ENCODING,
+			V4L2_MPEG_VIDEO_ENCODING_MPEG_2, 0,
+			V4L2_MPEG_VIDEO_ENCODING_MPEG_2);
+	cx2341x_ctrl_new_menu(hdl,
+			V4L2_CID_MPEG_VIDEO_ASPECT,
+			V4L2_MPEG_VIDEO_ASPECT_221x100, 0,
+			V4L2_MPEG_VIDEO_ASPECT_4x3);
+	cxhdl->video_b_frames = cx2341x_ctrl_new_std(hdl,
+			V4L2_CID_MPEG_VIDEO_B_FRAMES, 0, 33, 1, 2);
+	cxhdl->video_gop_size = cx2341x_ctrl_new_std(hdl,
+			V4L2_CID_MPEG_VIDEO_GOP_SIZE,
+			1, 34, 1, cxhdl->is_50hz ? 12 : 15);
+	cx2341x_ctrl_new_std(hdl, V4L2_CID_MPEG_VIDEO_GOP_CLOSURE, 0, 1, 1, 1);
+	cxhdl->video_bitrate_mode = cx2341x_ctrl_new_menu(hdl,
+			V4L2_CID_MPEG_VIDEO_BITRATE_MODE,
+			V4L2_MPEG_VIDEO_BITRATE_MODE_CBR, 0,
+			V4L2_MPEG_VIDEO_BITRATE_MODE_VBR);
+	cxhdl->video_bitrate = cx2341x_ctrl_new_std(hdl,
+			V4L2_CID_MPEG_VIDEO_BITRATE,
+			0, 27000000, 1, 6000000);
+	cxhdl->video_bitrate_peak = cx2341x_ctrl_new_std(hdl,
+			V4L2_CID_MPEG_VIDEO_BITRATE_PEAK,
+			0, 27000000, 1, 8000000);
+	cx2341x_ctrl_new_std(hdl,
+			V4L2_CID_MPEG_VIDEO_TEMPORAL_DECIMATION, 0, 255, 1, 0);
+	cxhdl->video_mute = cx2341x_ctrl_new_std(hdl,
+			V4L2_CID_MPEG_VIDEO_MUTE, 0, 1, 1, 0);
+	cxhdl->video_mute_yuv = cx2341x_ctrl_new_std(hdl,
+			V4L2_CID_MPEG_VIDEO_MUTE_YUV, 0, 0xffffff, 1, 0x008080);
+
+	/* CX23415/6 specific */
+	cxhdl->video_spatial_filter_mode = cx2341x_ctrl_new_custom(hdl,
+			V4L2_CID_MPEG_CX2341X_VIDEO_SPATIAL_FILTER_MODE,
+			V4L2_MPEG_CX2341X_VIDEO_SPATIAL_FILTER_MODE_MANUAL,
+			V4L2_MPEG_CX2341X_VIDEO_SPATIAL_FILTER_MODE_AUTO, 0,
+			V4L2_MPEG_CX2341X_VIDEO_SPATIAL_FILTER_MODE_MANUAL);
+	cxhdl->video_spatial_filter = cx2341x_ctrl_new_custom(hdl,
+			V4L2_CID_MPEG_CX2341X_VIDEO_SPATIAL_FILTER,
+			0, 15, 1, 0);
+	cxhdl->video_luma_spatial_filter_type = cx2341x_ctrl_new_custom(hdl,
+			V4L2_CID_MPEG_CX2341X_VIDEO_LUMA_SPATIAL_FILTER_TYPE,
+			V4L2_MPEG_CX2341X_VIDEO_LUMA_SPATIAL_FILTER_TYPE_OFF,
+			V4L2_MPEG_CX2341X_VIDEO_LUMA_SPATIAL_FILTER_TYPE_2D_SYM_NON_SEPARABLE,
+			0,
+			V4L2_MPEG_CX2341X_VIDEO_LUMA_SPATIAL_FILTER_TYPE_1D_HOR);
+	cxhdl->video_chroma_spatial_filter_type = cx2341x_ctrl_new_custom(hdl,
+			V4L2_CID_MPEG_CX2341X_VIDEO_CHROMA_SPATIAL_FILTER_TYPE,
+			V4L2_MPEG_CX2341X_VIDEO_CHROMA_SPATIAL_FILTER_TYPE_OFF,
+			V4L2_MPEG_CX2341X_VIDEO_CHROMA_SPATIAL_FILTER_TYPE_1D_HOR,
+			0,
+			V4L2_MPEG_CX2341X_VIDEO_CHROMA_SPATIAL_FILTER_TYPE_1D_HOR);
+	cxhdl->video_temporal_filter_mode = cx2341x_ctrl_new_custom(hdl,
+			V4L2_CID_MPEG_CX2341X_VIDEO_TEMPORAL_FILTER_MODE,
+			V4L2_MPEG_CX2341X_VIDEO_TEMPORAL_FILTER_MODE_MANUAL,
+			V4L2_MPEG_CX2341X_VIDEO_TEMPORAL_FILTER_MODE_AUTO,
+			0,
+			V4L2_MPEG_CX2341X_VIDEO_TEMPORAL_FILTER_MODE_MANUAL);
+	cxhdl->video_temporal_filter = cx2341x_ctrl_new_custom(hdl,
+			V4L2_CID_MPEG_CX2341X_VIDEO_TEMPORAL_FILTER,
+			0, 31, 1, 8);
+	cxhdl->video_median_filter_type = cx2341x_ctrl_new_custom(hdl,
+			V4L2_CID_MPEG_CX2341X_VIDEO_MEDIAN_FILTER_TYPE,
+			V4L2_MPEG_CX2341X_VIDEO_MEDIAN_FILTER_TYPE_OFF,
+			V4L2_MPEG_CX2341X_VIDEO_MEDIAN_FILTER_TYPE_DIAG,
+			0,
+			V4L2_MPEG_CX2341X_VIDEO_MEDIAN_FILTER_TYPE_OFF);
+	cxhdl->video_luma_median_filter_bottom = cx2341x_ctrl_new_custom(hdl,
+			V4L2_CID_MPEG_CX2341X_VIDEO_LUMA_MEDIAN_FILTER_BOTTOM,
+			0, 255, 1, 0);
+	cxhdl->video_luma_median_filter_top = cx2341x_ctrl_new_custom(hdl,
+			V4L2_CID_MPEG_CX2341X_VIDEO_LUMA_MEDIAN_FILTER_TOP,
+			0, 255, 1, 255);
+	cxhdl->video_chroma_median_filter_bottom = cx2341x_ctrl_new_custom(hdl,
+			V4L2_CID_MPEG_CX2341X_VIDEO_CHROMA_MEDIAN_FILTER_BOTTOM,
+			0, 255, 1, 0);
+	cxhdl->video_chroma_median_filter_top = cx2341x_ctrl_new_custom(hdl,
+			V4L2_CID_MPEG_CX2341X_VIDEO_CHROMA_MEDIAN_FILTER_TOP,
+			0, 255, 1, 255);
+	cx2341x_ctrl_new_custom(hdl, V4L2_CID_MPEG_CX2341X_STREAM_INSERT_NAV_PACKETS,
+			0, 1, 1, 0);
+
+	if (hdl->error) {
+		int err = hdl->error;
+
+		v4l2_ctrl_handler_free(hdl);
+		return err;
+	}
+
+	v4l2_ctrl_cluster(8, &cxhdl->audio_sampling_freq);
+	v4l2_ctrl_cluster(2, &cxhdl->video_b_frames);
+	v4l2_ctrl_cluster(5, &cxhdl->stream_type);
+	v4l2_ctrl_cluster(2, &cxhdl->video_mute);
+	v4l2_ctrl_cluster(3, &cxhdl->video_spatial_filter_mode);
+	v4l2_ctrl_cluster(2, &cxhdl->video_luma_spatial_filter_type);
+	v4l2_ctrl_cluster(2, &cxhdl->video_spatial_filter);
+	v4l2_ctrl_cluster(4, &cxhdl->video_luma_median_filter_top);
+
+	return 0;
+}
+EXPORT_SYMBOL(cx2341x_handler_init);
+
+void cx2341x_handler_set_50hz(struct cx2341x_handler *cxhdl, int is_50hz)
+{
+	cxhdl->is_50hz = is_50hz;
+	cxhdl->video_gop_size->default_value = cxhdl->is_50hz ? 12 : 15;
+}
+EXPORT_SYMBOL(cx2341x_handler_set_50hz);
+
+int cx2341x_handler_setup(struct cx2341x_handler *cxhdl)
+{
+	int h = cxhdl->height;
+	int w = cxhdl->width;
+	int err;
+
+	err = cx2341x_hdl_api(cxhdl, CX2341X_ENC_SET_OUTPUT_PORT, 2, cxhdl->port, 0);
+	if (err)
+		return err;
+	err = cx2341x_hdl_api(cxhdl, CX2341X_ENC_SET_FRAME_RATE, 1, cxhdl->is_50hz);
+	if (err)
+		return err;
+
+	if (v4l2_ctrl_g_ctrl(cxhdl->video_encoding) == V4L2_MPEG_VIDEO_ENCODING_MPEG_1) {
+		w /= 2;
+		h /= 2;
+	}
+	err = cx2341x_hdl_api(cxhdl, CX2341X_ENC_SET_FRAME_SIZE, 2, h, w);
+	if (err)
+		return err;
+	return v4l2_ctrl_handler_setup(&cxhdl->hdl);
+}
+EXPORT_SYMBOL(cx2341x_handler_setup);
+
+void cx2341x_handler_set_busy(struct cx2341x_handler *cxhdl, int busy)
+{
+	v4l2_ctrl_grab(cxhdl->audio_sampling_freq, busy);
+	v4l2_ctrl_grab(cxhdl->audio_encoding, busy);
+	v4l2_ctrl_grab(cxhdl->audio_l2_bitrate, busy);
+	v4l2_ctrl_grab(cxhdl->audio_ac3_bitrate, busy);
+	v4l2_ctrl_grab(cxhdl->stream_vbi_fmt, busy);
+	v4l2_ctrl_grab(cxhdl->stream_type, busy);
+	v4l2_ctrl_grab(cxhdl->video_bitrate_mode, busy);
+	v4l2_ctrl_grab(cxhdl->video_bitrate, busy);
+	v4l2_ctrl_grab(cxhdl->video_bitrate_peak, busy);
+}
+EXPORT_SYMBOL(cx2341x_handler_set_busy);
diff --git a/drivers/media/i2c/cx25840/Kconfig b/drivers/media/i2c/cx25840/Kconfig
new file mode 100644
index 000000000000..451133ad41ff
--- /dev/null
+++ b/drivers/media/i2c/cx25840/Kconfig
@@ -0,0 +1,8 @@
+config VIDEO_CX25840
+	tristate "Conexant CX2584x audio/video decoders"
+	depends on VIDEO_V4L2 && 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 000000000000..898eb13340ae
--- /dev/null
+++ b/drivers/media/i2c/cx25840/Makefile
@@ -0,0 +1,6 @@
+cx25840-objs    := cx25840-core.o cx25840-audio.o cx25840-firmware.o \
+		   cx25840-vbi.o cx25840-ir.o
+
+obj-$(CONFIG_VIDEO_CX25840) += cx25840.o
+
+ccflags-y += -Idrivers/media/i2c
diff --git a/drivers/media/i2c/cx25840/cx25840-audio.c b/drivers/media/i2c/cx25840/cx25840-audio.c
new file mode 100644
index 000000000000..34b96c7cfd62
--- /dev/null
+++ b/drivers/media/i2c/cx25840/cx25840-audio.c
@@ -0,0 +1,571 @@
+/* cx25840 audio functions
+ *
+ * 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; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+
+#include <linux/videodev2.h>
+#include <linux/i2c.h>
+#include <media/v4l2-common.h>
+#include <media/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 000000000000..d8eac3e30a7e
--- /dev/null
+++ b/drivers/media/i2c/cx25840/cx25840-core.c
@@ -0,0 +1,5340 @@
+/* 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>
+ *
+ * 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; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+
+#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/v4l2-chip-ident.h>
+#include <media/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 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 &
+					(V4L2_SUBDEV_IO_PIN_DISABLE |
+					 V4L2_SUBDEV_IO_PIN_INPUT)) {
+					pin_ctrl &= ~(0x1 << 25);
+				} else {
+					pin_ctrl |= (0x1 << 25);
+				}
+				if (p[i].flags &
+					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 & 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 & 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 & 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 & 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 & 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 & 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 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);
+	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);
+}
+
+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");
+	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);
+
+	/* 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));
+	struct workqueue_struct *q;
+
+	/*
+	 * 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 V4L2_IDENT_CX23888_AV:
+		/*
+		 * 50.0 MHz * (0xb + 0xe8ba26/0x2000000)/4 = 5 * 28.636363 MHz
+		 * 572.73 MHz before post divide
+		 */
+		/* 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 V4L2_IDENT_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 V4L2_IDENT_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 V4L2_IDENT_CX23888_AV:
+		/* 888/HVR1250 specific */
+		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 */
+	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 V4L2_IDENT_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 */
+		cx25840_write4(client, 0x114, 0x017dbf48);
+		cx25840_write4(client, 0x110, 0x000a030e);
+		break;
+	case V4L2_IDENT_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 V4L2_IDENT_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");
+	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  - Undocumented Register */
+	cx25840_write(client, 0x42f, 0x66);
+
+	/* HVR-1250 / HVR1850 DIF related */
+	/* Power everything up */
+	cx25840_write4(client, 0x130, 0x0);
+
+	/* Undocumented */
+	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");
+	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);
+
+	if (std & V4L2_STD_625_50) {
+		hblank = 132;
+		hactive = 720;
+		burst = 93;
+		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) {
+			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;
+
+		src_decimation = 0x21f;
+		if (std == V4L2_STD_PAL_60) {
+			vblank = 26;
+			vblank656 = 26;
+			burst = 0x5b;
+			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 {
+			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,
+			0xff & (((hblank >> 8) & 0x3) | (hactive << 4)));
+	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,
+			0xff & (((vblank >> 8) & 0x3) | (vactive << 4)));
+	cx25840_write(client, 0x476, vactive >> 4);
+	cx25840_write(client, 0x477, vblank656);
+
+	/* Sets src decimation rate */
+	cx25840_write(client, 0x478, 0xff & src_decimation);
+	cx25840_write(client, 0x479, 0xff & (src_decimation >> 8));
+
+	/* 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, 0xff & sc >> 8);
+	cx25840_write(client, 0x47e, 0xff & sc >> 16);
+
+	/* 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);
+
+			/* on the 887, 0x418 is HSCALE_CTRL, on the 888 it is 
+			   CHROMA_CTRL */
+			if (is_cx23888(state))
+				cx25840_write4(client, 0x418, 0x01008080);
+			else
+				cx25840_write4(client, 0x418, 0x01000000);
+
+			cx25840_write4(client, 0x41c, 0x00000000);
+
+			/* on the 887, 0x420 is CHROMA_CTRL, on the 888 it is 
+			   CRUSH_CTRL */
+			if (is_cx23888(state))
+				cx25840_write4(client, 0x420, 0x001c3e0f);
+			else
+				cx25840_write4(client, 0x420, 0x001c8282);
+
+			cx25840_write4(client, 0x42c, 0x42600000);
+			cx25840_write4(client, 0x430, 0x0000039b);
+			cx25840_write4(client, 0x438, 0x00000000);
+
+			cx25840_write4(client, 0x440, 0xF8E3E824);
+			cx25840_write4(client, 0x444, 0x401040dc);
+			cx25840_write4(client, 0x448, 0xcd3f02a0);
+			cx25840_write4(client, 0x44c, 0x161f1000);
+			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_s_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *fmt)
+{
+	struct cx25840_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int HSC, VSC, Vsrc, Hsrc, filter, Vlines;
+	int is_50Hz = !(state->std & V4L2_STD_525_60);
+
+	if (fmt->code != V4L2_MBUS_FMT_FIXED)
+		return -EINVAL;
+
+	fmt->field = V4L2_FIELD_INTERLACED;
+	fmt->colorspace = V4L2_COLORSPACE_SMPTE170M;
+
+	if (is_cx23888(state)) {
+		Vsrc = (cx25840_read(client, 0x42a) & 0x3f) << 4;
+		Vsrc |= (cx25840_read(client, 0x429) & 0xf0) >> 4;
+	} else {
+		Vsrc = (cx25840_read(client, 0x476) & 0x3f) << 4;
+		Vsrc |= (cx25840_read(client, 0x475) & 0xf0) >> 4;
+	}
+
+	if (is_cx23888(state)) {
+		Hsrc = (cx25840_read(client, 0x426) & 0x3f) << 4;
+		Hsrc |= (cx25840_read(client, 0x425) & 0xf0) >> 4;
+	} else {
+		Hsrc = (cx25840_read(client, 0x472) & 0x3f) << 4;
+		Hsrc |= (cx25840_read(client, 0x471) & 0xf0) >> 4;
+	}
+
+	Vlines = fmt->height + (is_50Hz ? 4 : 7);
+
+	if ((fmt->width * 16 < Hsrc) || (Hsrc < fmt->width) ||
+			(Vlines * 8 < Vsrc) || (Vsrc < Vlines)) {
+		v4l_err(client, "%dx%d is not a valid size!\n",
+				fmt->width, fmt->height);
+		return -ERANGE;
+	}
+
+	HSC = (Hsrc * (1 << 20)) / fmt->width - (1 << 20);
+	VSC = (1 << 16) - (Vsrc * (1 << 9) / Vlines - (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 %dx%d -> scale  %ux%u\n",
+			fmt->width, fmt->height, HSC, VSC);
+
+	/* HSCALE=HSC */
+	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);
+	}
+}
+
+/* ----------------------------------------------------------------------- */
+
+/* This load_fw operation must be called to load the driver's firmware.
+   Without this the audio standard detection will fail and you will
+   only get mono.
+
+   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);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!state->is_initialized) {
+		/* initialize and load firmware */
+		state->is_initialized = 1;
+		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);
+	}
+	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);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	reg->size = 1;
+	reg->val = cx25840_read(client, reg->reg & 0x0fff);
+	return 0;
+}
+
+static int cx25840_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	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");
+	if (enable) {
+		if (is_cx2388x(state) || is_cx231xx(state)) {
+			v = cx25840_read(client, 0x421) | 0x0b;
+			cx25840_write(client, 0x421, v);
+		} else {
+			v = cx25840_read(client, 0x115) | 0x0c;
+			cx25840_write(client, 0x115, v);
+			v = cx25840_read(client, 0x116) | 0x04;
+			cx25840_write(client, 0x116, v);
+		}
+	} else {
+		if (is_cx2388x(state) || is_cx231xx(state)) {
+			v = cx25840_read(client, 0x421) & ~(0x0b);
+			cx25840_write(client, 0x421, 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_g_std(struct v4l2_subdev *sd, v4l2_std_id *std)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	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,
+		/* 1001 */ V4L2_STD_UNKNOWN,
+		/* 1010 */ V4L2_STD_UNKNOWN,
+		/* 1011 */ 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, "g_std 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_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);
+
+	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, 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, 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_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);
+	return 0;
+}
+
+static int cx25840_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ident *chip)
+{
+	struct cx25840_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return v4l2_chip_ident_i2c_client(client, chip, state->id, state->rev);
+}
+
+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)
+
+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_NTSC) {
+		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,
+	.g_chip_ident = cx25840_g_chip_ident,
+	.g_ctrl = v4l2_subdev_g_ctrl,
+	.s_ctrl = v4l2_subdev_s_ctrl,
+	.s_ext_ctrls = v4l2_subdev_s_ext_ctrls,
+	.try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
+	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
+	.queryctrl = v4l2_subdev_queryctrl,
+	.querymenu = v4l2_subdev_querymenu,
+	.s_std = cx25840_s_std,
+	.g_std = cx25840_g_std,
+	.reset = cx25840_reset,
+	.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 = {
+	.s_routing = cx25840_s_video_routing,
+	.s_mbus_fmt = cx25840_s_mbus_fmt,
+	.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_ops cx25840_ops = {
+	.core = &cx25840_core_ops,
+	.tuner = &cx25840_tuner_ops,
+	.audio = &cx25840_audio_ops,
+	.video = &cx25840_video_ops,
+	.vbi = &cx25840_vbi_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 = V4L2_IDENT_CX23885_AV;
+		} else {
+			/* CX23887 has a broken DIF, but the registers
+			 * appear valid (but unused), good enough to detect. */
+			ret = V4L2_IDENT_CX23887_AV;
+		}
+	} else if (cx25840_read4(client, 0x300) & 0x0fffffff) {
+		/* DIF PLL Freq Word reg exists; chip must be a CX23888 */
+		ret = V4L2_IDENT_CX23888_AV;
+	} else {
+		v4l_err(client, "Unable to detect h/w, assuming cx23887\n");
+		ret = V4L2_IDENT_CX23887_AV;
+	}
+
+	/* Back into digital power down */
+	cx25840_write(client, 0x000, 2);
+	return ret;
+}
+
+static int cx25840_probe(struct i2c_client *client,
+			 const struct i2c_device_id *did)
+{
+	struct cx25840_state *state;
+	struct v4l2_subdev *sd;
+	int default_volume;
+	u32 id = V4L2_IDENT_NONE;
+	u16 device_id;
+
+	/* 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 = V4L2_IDENT_CX25836 + ((device_id >> 4) & 0xf) - 6;
+	} else if ((device_id & 0xff00) == 0x8400) {
+		id = V4L2_IDENT_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 = V4L2_IDENT_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 = kzalloc(sizeof(struct cx25840_state), GFP_KERNEL);
+	if (state == NULL)
+		return -ENOMEM;
+
+	sd = &state->sd;
+	v4l2_i2c_subdev_init(sd, client, &cx25840_ops);
+
+	switch (id) {
+	case V4L2_IDENT_CX23885_AV:
+		v4l_info(client, "cx23885 A/V decoder found @ 0x%x (%s)\n",
+			 client->addr << 1, client->adapter->name);
+		break;
+	case V4L2_IDENT_CX23887_AV:
+		v4l_info(client, "cx23887 A/V decoder found @ 0x%x (%s)\n",
+			 client->addr << 1, client->adapter->name);
+		break;
+	case V4L2_IDENT_CX23888_AV:
+		v4l_info(client, "cx23888 A/V decoder found @ 0x%x (%s)\n",
+			 client->addr << 1, client->adapter->name);
+		break;
+	case V4L2_IDENT_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 V4L2_IDENT_CX25840:
+	case V4L2_IDENT_CX25841:
+	case V4L2_IDENT_CX25842:
+	case V4L2_IDENT_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 V4L2_IDENT_CX25836:
+	case V4L2_IDENT_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;
+	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);
+		kfree(state);
+		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 int 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);
+	kfree(state);
+	return 0;
+}
+
+static const struct i2c_device_id cx25840_id[] = {
+	{ "cx25840", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, cx25840_id);
+
+static struct i2c_driver cx25840_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.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 000000000000..bd4ada28b490
--- /dev/null
+++ b/drivers/media/i2c/cx25840/cx25840-core.h
@@ -0,0 +1,137 @@
+/* cx25840 internal API header
+ *
+ * Copyright (C) 2003-2004 Chris Kennedy
+ *
+ * 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; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef _CX25840_CORE_H_
+#define _CX25840_CORE_H_
+
+
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-ctrls.h>
+#include <linux/i2c.h>
+
+struct cx25840_ir_state;
+
+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;
+	int radio;
+	v4l2_std_id std;
+	enum cx25840_video_input vid_input;
+	enum cx25840_audio_input aud_input;
+	u32 audclk_freq;
+	int audmode;
+	int vbi_line_offset;
+	u32 id;
+	u32 rev;
+	int is_initialized;
+	wait_queue_head_t fw_wait;    /* wake up when the fw load is finished */
+	struct work_struct fw_work;   /* work entry for fw load */
+	struct cx25840_ir_state *ir_state;
+};
+
+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 == V4L2_IDENT_CX25836 ||
+	       state->id == V4L2_IDENT_CX25837;
+}
+
+static inline bool is_cx231xx(struct cx25840_state *state)
+{
+	return state->id == V4L2_IDENT_CX2310X_AV;
+}
+
+static inline bool is_cx2388x(struct cx25840_state *state)
+{
+	return state->id == V4L2_IDENT_CX23885_AV ||
+	       state->id == V4L2_IDENT_CX23887_AV ||
+	       state->id == V4L2_IDENT_CX23888_AV;
+}
+
+static inline bool is_cx23885(struct cx25840_state *state)
+{
+	return state->id == V4L2_IDENT_CX23885_AV;
+}
+
+static inline bool is_cx23887(struct cx25840_state *state)
+{
+	return state->id == V4L2_IDENT_CX23887_AV;
+}
+
+static inline bool is_cx23888(struct cx25840_state *state)
+{
+	return state->id == V4L2_IDENT_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 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 000000000000..b3169f94ece8
--- /dev/null
+++ b/drivers/media/i2c/cx25840/cx25840-firmware.c
@@ -0,0 +1,175 @@
+/* cx25840 firmware functions
+ *
+ * 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; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/firmware.h>
+#include <media/v4l2-common.h>
+#include <media/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);
+	}
+
+	if (is_cx231xx(state) && MAX_BUF_SIZE > 16) {
+		v4l_err(client, " Firmware download size changed to 16 bytes max length\n");
+		MAX_BUF_SIZE = 16;  /* cx231xx cannot accept more than 16 bytes at a time */
+	}
+
+	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 000000000000..38ce76ed1924
--- /dev/null
+++ b/drivers/media/i2c/cx25840/cx25840-ir.c
@@ -0,0 +1,1281 @@
+/*
+ *  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>
+ *
+ *  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; either version 2
+ *  of the License, or (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ *  02110-1301, USA.
+ */
+
+#include <linux/slab.h>
+#include <linux/kfifo.h>
+#include <linux/module.h>
+#include <media/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 ns_to_clock_divider(unsigned int ns)
+{
+	return count_to_clock_divider(
+		DIV_ROUND_CLOSEST(CX25840_IR_REFCLK_FREQ / 1000000 * ns, 1000));
+}
+
+static inline unsigned int clock_divider_to_ns(unsigned int divider)
+{
+	/* Period of the Rx or Tx clock in ns */
+	return DIV_ROUND_CLOSEST((divider + 1) * 1000,
+				 CX25840_IR_REFCLK_FREQ / 1000000);
+}
+
+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 u16 freq_to_clock_divider(unsigned int freq,
+					unsigned int rollovers)
+{
+	return count_to_clock_divider(
+		   DIV_ROUND_CLOSEST(CX25840_IR_REFCLK_FREQ, freq * rollovers));
+}
+
+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 compuations
+ */
+static u32 clock_divider_to_resolution(u16 divider)
+{
+	/*
+	 * Resolution is the duration of 1 tick of the readable portion of
+	 * 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 Reuqest 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 reseting 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);
+		if (v > IR_MAX_DURATION)
+			v = IR_MAX_DURATION;
+
+		init_ir_raw_event(&p->ir_core_data);
+		p->ir_core_data.pulse = u;
+		p->ir_core_data.duration = v;
+		p->ir_core_data.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 = kzalloc(sizeof(struct cx25840_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)) {
+		kfree(ir_state);
+		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);
+	memcpy(&default_params, &default_rx_params,
+		       sizeof(struct v4l2_subdev_ir_parameters));
+	v4l2_subdev_call(sd, ir, rx_s_parameters, &default_params);
+
+	mutex_init(&ir_state->tx_params_lock);
+	memcpy(&default_params, &default_tx_params,
+		       sizeof(struct v4l2_subdev_ir_parameters));
+	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);
+	kfree(ir_state);
+	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 000000000000..c39e91dc1137
--- /dev/null
+++ b/drivers/media/i2c/cx25840/cx25840-vbi.c
@@ -0,0 +1,257 @@
+/* cx25840 VBI functions
+ *
+ * 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; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+
+#include <linux/videodev2.h>
+#include <linux/i2c.h>
+#include <media/v4l2-common.h>
+#include <media/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 */
+	if ((cx25840_read(client, 0x404) & 0x10) == 0)
+		return 0;
+
+	if (is_pal) {
+		for (i = 7; i <= 23; i++) {
+			u8 v = cx25840_read(client, 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, 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 */
+	cx25840_write(client, 0x47f, vbi_offset);
+	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 */
+	cx25840_write(client, 0x404, 0x32);	/* Ancillary data */
+	cx25840_write(client, 0x406, 0x13);
+	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 = 0x424; i <= 0x434; i++, x++)
+			cx25840_write(client, i, lcr[6 + x]);
+	} else {
+		for (x = 1, i = 0x424; i <= 0x430; i++, x++)
+			cx25840_write(client, i, lcr[9 + x]);
+		for (i = 0x431; i <= 0x434; i++)
+			cx25840_write(client, i, 0);
+	}
+
+	cx25840_write(client, 0x43c, 0x16);
+	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/ir-kbd-i2c.c b/drivers/media/i2c/ir-kbd-i2c.c
new file mode 100644
index 000000000000..04f192a0398a
--- /dev/null
+++ b/drivers/media/i2c/ir-kbd-i2c.c
@@ -0,0 +1,489 @@
+/*
+ *
+ * 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>
+ *
+ *  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; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#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/ir-kbd-i2c.h>
+
+/* ----------------------------------------------------------------------- */
+/* insmod parameters                                                       */
+
+static int debug;
+module_param(debug, int, 0644);    /* debug level (0,1,2) */
+
+
+#define MODULE_NAME "ir-kbd-i2c"
+#define dprintk(level, fmt, arg...)	if (debug >= level) \
+	printk(KERN_DEBUG MODULE_NAME ": " fmt , ## arg)
+
+/* ----------------------------------------------------------------------- */
+
+static int get_key_haup_common(struct IR_i2c *ir, u32 *ir_key, u32 *ir_raw,
+			       int size, int offset)
+{
+	unsigned char buf[6];
+	int start, range, toggle, dev, code, ircode;
+
+	/* poll IR chip */
+	if (size != i2c_master_recv(ir->c, buf, size))
+		return -EIO;
+
+	/* 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;
+	/*
+	 * Hauppauge remotes (black/silver) always use
+	 * specific device ids. If we do not filter the
+	 * device ids then messages destined for devices
+	 * such as TVs (id=0) will get through causing
+	 * mis-fired events.
+	 *
+	 * We also filter out invalid key presses which
+	 * produce annoying debug log entries.
+	 */
+	ircode= (start << 12) | (toggle << 11) | (dev << 6) | code;
+	if ((ircode & 0x1fff)==0x1fff)
+		/* invalid key press */
+		return 0;
+
+	if (!range)
+		code += 64;
+
+	dprintk(1,"ir hauppauge (rc5): s%d r%d t%d dev=%d code=%d\n",
+		start, range, toggle, dev, code);
+
+	/* return key */
+	*ir_key = (dev << 8) | code;
+	*ir_raw = ircode;
+	return 1;
+}
+
+static int get_key_haup(struct IR_i2c *ir, u32 *ir_key, u32 *ir_raw)
+{
+	return get_key_haup_common (ir, ir_key, ir_raw, 3, 0);
+}
+
+static int get_key_haup_xvr(struct IR_i2c *ir, u32 *ir_key, u32 *ir_raw)
+{
+	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, ir_key, ir_raw, 6, 3);
+}
+
+static int get_key_pixelview(struct IR_i2c *ir, u32 *ir_key, u32 *ir_raw)
+{
+	unsigned char b;
+
+	/* poll IR chip */
+	if (1 != i2c_master_recv(ir->c, &b, 1)) {
+		dprintk(1,"read error\n");
+		return -EIO;
+	}
+	*ir_key = b;
+	*ir_raw = b;
+	return 1;
+}
+
+static int get_key_fusionhdtv(struct IR_i2c *ir, u32 *ir_key, u32 *ir_raw)
+{
+	unsigned char buf[4];
+
+	/* poll IR chip */
+	if (4 != i2c_master_recv(ir->c, buf, 4)) {
+		dprintk(1,"read error\n");
+		return -EIO;
+	}
+
+	if(buf[0] !=0 || buf[1] !=0 || buf[2] !=0 || buf[3] != 0)
+		dprintk(2, "%s: 0x%2x 0x%2x 0x%2x 0x%2x\n", __func__,
+			buf[0], buf[1], buf[2], buf[3]);
+
+	/* no key pressed or signal from other ir remote */
+	if(buf[0] != 0x1 ||  buf[1] != 0xfe)
+		return 0;
+
+	*ir_key = buf[2];
+	*ir_raw = (buf[2] << 8) | buf[3];
+
+	return 1;
+}
+
+static int get_key_knc1(struct IR_i2c *ir, u32 *ir_key, u32 *ir_raw)
+{
+	unsigned char b;
+
+	/* poll IR chip */
+	if (1 != i2c_master_recv(ir->c, &b, 1)) {
+		dprintk(1,"read error\n");
+		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 */
+
+	dprintk(2,"key %02x\n", b);
+
+	if (b == 0xff)
+		return 0;
+
+	if (b == 0xfe)
+		/* keep old data */
+		return 1;
+
+	*ir_key = b;
+	*ir_raw = b;
+	return 1;
+}
+
+static int get_key_avermedia_cardbus(struct IR_i2c *ir,
+				     u32 *ir_key, u32 *ir_raw)
+{
+	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)) {
+		dprintk(1, "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)) {
+		dprintk(1, "read error\n");
+		return -EIO;
+	}
+
+	if (keygroup == 0xff)
+		return 0;
+
+	dprintk(1, "read key 0x%02x/0x%02x\n", key, keygroup);
+	if (keygroup < 2 || keygroup > 3) {
+		/* Only a warning */
+		dprintk(1, "warning: invalid key group 0x%02x for key 0x%02x\n",
+								keygroup, key);
+	}
+	key |= (keygroup & 1) << 6;
+
+	*ir_key = key;
+	*ir_raw = key;
+	return 1;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static int ir_key_poll(struct IR_i2c *ir)
+{
+	static u32 ir_key, ir_raw;
+	int rc;
+
+	dprintk(3, "%s\n", __func__);
+	rc = ir->get_key(ir, &ir_key, &ir_raw);
+	if (rc < 0) {
+		dprintk(2,"error\n");
+		return rc;
+	}
+
+	if (rc) {
+		dprintk(1, "%s: keycode = 0x%04x\n", __func__, ir_key);
+		rc_keydown(ir->rc, ir_key, 0);
+	}
+	return 0;
+}
+
+static void ir_work(struct work_struct *work)
+{
+	int rc;
+	struct IR_i2c *ir = container_of(work, struct IR_i2c, work.work);
+
+	rc = ir_key_poll(ir);
+	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_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	char *ir_codes = NULL;
+	const char *name = NULL;
+	u64 rc_type = RC_TYPE_UNKNOWN;
+	struct IR_i2c *ir;
+	struct rc_dev *rc = NULL;
+	struct i2c_adapter *adap = client->adapter;
+	unsigned short addr = client->addr;
+	int err;
+
+	ir = kzalloc(sizeof(struct IR_i2c), 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_type     = RC_TYPE_OTHER;
+		ir_codes    = RC_MAP_EMPTY;
+		break;
+	case 0x18:
+	case 0x1f:
+	case 0x1a:
+		name        = "Hauppauge";
+		ir->get_key = get_key_haup;
+		rc_type     = RC_TYPE_RC5;
+		ir_codes    = RC_MAP_HAUPPAUGE;
+		break;
+	case 0x30:
+		name        = "KNC One";
+		ir->get_key = get_key_knc1;
+		rc_type     = RC_TYPE_OTHER;
+		ir_codes    = RC_MAP_EMPTY;
+		break;
+	case 0x6b:
+		name        = "FusionHDTV";
+		ir->get_key = get_key_fusionhdtv;
+		rc_type     = RC_TYPE_RC5;
+		ir_codes    = RC_MAP_FUSIONHDTV_MCE;
+		break;
+	case 0x40:
+		name        = "AVerMedia Cardbus remote";
+		ir->get_key = get_key_avermedia_cardbus;
+		rc_type     = RC_TYPE_OTHER;
+		ir_codes    = RC_MAP_AVERMEDIA_CARDBUS;
+		break;
+	case 0x71:
+		name        = "Hauppauge/Zilog Z8";
+		ir->get_key = get_key_haup_xvr;
+		rc_type     = RC_TYPE_RC5;
+		ir_codes    = RC_MAP_HAUPPAUGE;
+		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_type = 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_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, initilize it
+		 * internally
+		 */
+		rc = rc_allocate_device();
+		if (!rc) {
+			err = -ENOMEM;
+			goto err_out_free;
+		}
+	}
+	ir->rc = rc;
+
+	/* Make sure we are all setup before going on */
+	if (!name || !ir->get_key || !rc_type || !ir_codes) {
+		dprintk(1, ": Unsupported device at address 0x%02x\n",
+			addr);
+		err = -ENODEV;
+		goto err_out_free;
+	}
+
+	/* Sets name */
+	snprintf(ir->name, sizeof(ir->name), "i2c IR (%s)", name);
+	ir->ir_codes = ir_codes;
+
+	snprintf(ir->phys, sizeof(ir->phys), "%s/%s/ir0",
+		 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->input_name	     = ir->name;
+
+	/*
+	 * Initialize the other fields of rc_dev
+	 */
+	rc->map_name       = ir->ir_codes;
+	rc->allowed_protos = rc_type;
+	if (!rc->driver_name)
+		rc->driver_name = MODULE_NAME;
+
+	err = rc_register_device(rc);
+	if (err)
+		goto err_out_free;
+
+	printk(MODULE_NAME ": %s detected at %s [%s]\n",
+	       ir->name, ir->phys, adap->name);
+
+	/* start polling via eventd */
+	INIT_DELAYED_WORK(&ir->work, ir_work);
+	schedule_delayed_work(&ir->work, 0);
+
+	return 0;
+
+ err_out_free:
+	/* Only frees rc if it were allocated internally */
+	rc_free_device(rc);
+	kfree(ir);
+	return err;
+}
+
+static int ir_remove(struct i2c_client *client)
+{
+	struct IR_i2c *ir = i2c_get_clientdata(client);
+
+	/* kill outstanding polls */
+	cancel_delayed_work_sync(&ir->work);
+
+	/* unregister device */
+	if (ir->rc)
+		rc_unregister_device(ir->rc);
+
+	/* free memory */
+	kfree(ir);
+	return 0;
+}
+
+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_rx_z8f0811_haup", 0 },
+	{ "ir_rx_z8f0811_hdpvr", 0 },
+	{ }
+};
+
+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/ks0127.c b/drivers/media/i2c/ks0127.c
new file mode 100644
index 000000000000..04a6efa37cc3
--- /dev/null
+++ b/drivers/media/i2c/ks0127.c
@@ -0,0 +1,733 @@
+/*
+ * 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>
+ *
+ * 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; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+ *
+ *****************************************************************************
+ *
+ * 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 <media/v4l2-chip-ident.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;
+	int		ident;
+	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)
+{
+	struct ks0127 *ks = to_ks0127(sd);
+	u8 *table = reg_defaults;
+	int i;
+
+	ks->ident = V4L2_IDENT_KS0127;
+
+	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) {
+		ks->ident = V4L2_IDENT_KS0122S;
+		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:
+		ks->ident = V4L2_IDENT_KS0127B;
+		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 = 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;
+	if ((status & 0x08))		   /* PALDET set */
+		std = V4L2_STD_PAL;
+	else
+		std = V4L2_STD_NTSC;
+	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 int ks0127_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ident *chip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ks0127 *ks = to_ks0127(sd);
+
+	return v4l2_chip_ident_i2c_client(client, chip, ks->ident, 0);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_core_ops ks0127_core_ops = {
+	.g_chip_ident = ks0127_g_chip_ident,
+	.s_std = ks0127_s_std,
+};
+
+static const struct v4l2_subdev_video_ops ks0127_video_ops = {
+	.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 = {
+	.core = &ks0127_core_ops,
+	.video = &ks0127_video_ops,
+};
+
+/* ----------------------------------------------------------------------- */
+
+
+static int ks0127_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	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 = kzalloc(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 int 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 */
+	kfree(to_ks0127(sd));
+	return 0;
+}
+
+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 = {
+		.owner	= THIS_MODULE,
+		.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 000000000000..cb8abd5403b3
--- /dev/null
+++ b/drivers/media/i2c/ks0127.h
@@ -0,0 +1,51 @@
+/*
+ * 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>
+ *
+ * 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; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+ */
+
+#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/m52790.c b/drivers/media/i2c/m52790.c
new file mode 100644
index 000000000000..0991576f4c82
--- /dev/null
+++ b/drivers/media/i2c/m52790.c
@@ -0,0 +1,216 @@
+/*
+ * m52790 i2c ivtv driver.
+ * Copyright (C) 2007  Hans Verkuil
+ *
+ * A/V source switching Mitsubishi M52790SP/FP
+ *
+ * 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; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <linux/ioctl.h>
+#include <asm/uaccess.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <media/m52790.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.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);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	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, struct v4l2_dbg_register *reg)
+{
+	struct m52790_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	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_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ident *chip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_M52790, 0);
+}
+
+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,
+	.g_chip_ident = m52790_g_chip_ident,
+#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,
+			const struct i2c_device_id *id)
+{
+	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 = kzalloc(sizeof(struct m52790_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 int m52790_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	kfree(to_state(sd));
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id m52790_id[] = {
+	{ "m52790", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, m52790_id);
+
+static struct i2c_driver m52790_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "m52790",
+	},
+	.probe		= m52790_probe,
+	.remove		= m52790_remove,
+	.id_table	= m52790_id,
+};
+
+module_i2c_driver(m52790_driver);
diff --git a/drivers/media/i2c/m5mols/Kconfig b/drivers/media/i2c/m5mols/Kconfig
new file mode 100644
index 000000000000..dc8c2505907e
--- /dev/null
+++ b/drivers/media/i2c/m5mols/Kconfig
@@ -0,0 +1,6 @@
+config VIDEO_M5MOLS
+	tristate "Fujitsu M-5MOLS 8MP sensor support"
+	depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
+	depends on MEDIA_CAMERA_SUPPORT
+	---help---
+	  This driver supports Fujitsu M-5MOLS camera sensor with ISP
diff --git a/drivers/media/i2c/m5mols/Makefile b/drivers/media/i2c/m5mols/Makefile
new file mode 100644
index 000000000000..0a44e028edc7
--- /dev/null
+++ b/drivers/media/i2c/m5mols/Makefile
@@ -0,0 +1,3 @@
+m5mols-objs	:= m5mols_core.o m5mols_controls.o m5mols_capture.o
+
+obj-$(CONFIG_VIDEO_M5MOLS)		+= m5mols.o
diff --git a/drivers/media/i2c/m5mols/m5mols.h b/drivers/media/i2c/m5mols/m5mols.h
new file mode 100644
index 000000000000..86c815be348c
--- /dev/null
+++ b/drivers/media/i2c/m5mols/m5mols.h
@@ -0,0 +1,338 @@
+/*
+ * Header for M-5MOLS 8M Pixel camera sensor with ISP
+ *
+ * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ * Author: HeungJun Kim <riverful.kim@samsung.com>
+ *
+ * Copyright (C) 2009 Samsung Electronics Co., Ltd.
+ * Author: Dongsoo Nathaniel Kim <dongsoo45.kim@samsung.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; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#ifndef M5MOLS_H
+#define M5MOLS_H
+
+#include <media/v4l2-subdev.h>
+#include "m5mols_reg.h"
+
+extern int m5mols_debug;
+
+enum m5mols_restype {
+	M5MOLS_RESTYPE_MONITOR,
+	M5MOLS_RESTYPE_CAPTURE,
+	M5MOLS_RESTYPE_MAX,
+};
+
+/**
+ * struct m5mols_resolution - structure for the resolution
+ * @type: resolution type according to the pixel code
+ * @width: width of the resolution
+ * @height: height of the resolution
+ * @reg: resolution preset register value
+ */
+struct m5mols_resolution {
+	u8 reg;
+	enum m5mols_restype type;
+	u16 width;
+	u16 height;
+};
+
+/**
+ * struct m5mols_exif - structure for the EXIF information of M-5MOLS
+ * @exposure_time: exposure time register value
+ * @shutter_speed: speed of the shutter register value
+ * @aperture: aperture register value
+ * @exposure_bias: it calls also EV bias
+ * @iso_speed: ISO register value
+ * @flash: status register value of the flash
+ * @sdr: status register value of the Subject Distance Range
+ * @qval: not written exact meaning in document
+ */
+struct m5mols_exif {
+	u32 exposure_time;
+	u32 shutter_speed;
+	u32 aperture;
+	u32 brightness;
+	u32 exposure_bias;
+	u16 iso_speed;
+	u16 flash;
+	u16 sdr;
+	u16 qval;
+};
+
+/**
+ * struct m5mols_capture - Structure for the capture capability
+ * @exif: EXIF information
+ * @main: size in bytes of the main image
+ * @thumb: size in bytes of the thumb image, if it was accompanied
+ * @total: total size in bytes of the produced image
+ */
+struct m5mols_capture {
+	struct m5mols_exif exif;
+	u32 main;
+	u32 thumb;
+	u32 total;
+};
+
+/**
+ * struct m5mols_scenemode - structure for the scenemode capability
+ * @metering: metering light register value
+ * @ev_bias: EV bias register value
+ * @wb_mode: mode which means the WhiteBalance is Auto or Manual
+ * @wb_preset: whitebalance preset register value in the Manual mode
+ * @chroma_en: register value whether the Chroma capability is enabled or not
+ * @chroma_lvl: chroma's level register value
+ * @edge_en: register value Whether the Edge capability is enabled or not
+ * @edge_lvl: edge's level register value
+ * @af_range: Auto Focus's range
+ * @fd_mode: Face Detection mode
+ * @mcc: Multi-axis Color Conversion which means emotion color
+ * @light: status of the Light
+ * @flash: status of the Flash
+ * @tone: Tone color which means Contrast
+ * @iso: ISO register value
+ * @capt_mode: Mode of the Image Stabilization while the camera capturing
+ * @wdr: Wide Dynamic Range register value
+ *
+ * The each value according to each scenemode is recommended in the documents.
+ */
+struct m5mols_scenemode {
+	u8 metering;
+	u8 ev_bias;
+	u8 wb_mode;
+	u8 wb_preset;
+	u8 chroma_en;
+	u8 chroma_lvl;
+	u8 edge_en;
+	u8 edge_lvl;
+	u8 af_range;
+	u8 fd_mode;
+	u8 mcc;
+	u8 light;
+	u8 flash;
+	u8 tone;
+	u8 iso;
+	u8 capt_mode;
+	u8 wdr;
+};
+
+/**
+ * struct m5mols_version - firmware version information
+ * @customer:	customer information
+ * @project:	version of project information according to customer
+ * @fw:		firmware revision
+ * @hw:		hardware revision
+ * @param:	version of the parameter
+ * @awb:	Auto WhiteBalance algorithm version
+ * @str:	information about manufacturer and packaging vendor
+ * @af:		Auto Focus version
+ *
+ * The register offset starts the customer version at 0x0, and it ends
+ * the awb version at 0x09. The customer, project information occupies 1 bytes
+ * each. And also the fw, hw, param, awb each requires 2 bytes. The str is
+ * unique string associated with firmware's version. It includes information
+ * about manufacturer and the vendor of the sensor's packaging. The least
+ * significant 2 bytes of the string indicate packaging manufacturer.
+ */
+#define VERSION_STRING_SIZE	22
+struct m5mols_version {
+	u8	customer;
+	u8	project;
+	u16	fw;
+	u16	hw;
+	u16	param;
+	u16	awb;
+	u8	str[VERSION_STRING_SIZE];
+	u8	af;
+};
+
+/**
+ * struct m5mols_info - M-5MOLS driver data structure
+ * @pdata: platform data
+ * @sd: v4l-subdev instance
+ * @pad: media pad
+ * @irq_waitq: waitqueue for the capture
+ * @irq_done: set to 1 in the interrupt handler
+ * @handle: control handler
+ * @auto_exposure: auto/manual exposure control
+ * @exposure_bias: exposure compensation control
+ * @exposure: manual exposure control
+ * @metering: exposure metering control
+ * @auto_iso: auto/manual ISO sensitivity control
+ * @iso: manual ISO sensitivity control
+ * @auto_wb: auto white balance control
+ * @lock_3a: 3A lock control
+ * @colorfx: color effect control
+ * @saturation: saturation control
+ * @zoom: zoom control
+ * @wdr: wide dynamic range control
+ * @stabilization: image stabilization control
+ * @jpeg_quality: JPEG compression quality control
+ * @set_power: optional power callback to the board code
+ * @lock: mutex protecting the structure fields below
+ * @ffmt: current fmt according to resolution type
+ * @res_type: current resolution type
+ * @ver: information of the version
+ * @cap: the capture mode attributes
+ * @isp_ready: 1 when the ISP controller has completed booting
+ * @power: current sensor's power status
+ * @ctrl_sync: 1 when the control handler state is restored in H/W
+ * @resolution:	register value for current resolution
+ * @mode: register value for current operation mode
+ */
+struct m5mols_info {
+	const struct m5mols_platform_data *pdata;
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+
+	wait_queue_head_t irq_waitq;
+	atomic_t irq_done;
+
+	struct v4l2_ctrl_handler handle;
+	struct {
+		/* exposure/exposure bias/auto exposure cluster */
+		struct v4l2_ctrl *auto_exposure;
+		struct v4l2_ctrl *exposure_bias;
+		struct v4l2_ctrl *exposure;
+		struct v4l2_ctrl *metering;
+	};
+	struct {
+		/* iso/auto iso cluster */
+		struct v4l2_ctrl *auto_iso;
+		struct v4l2_ctrl *iso;
+	};
+	struct v4l2_ctrl *auto_wb;
+
+	struct v4l2_ctrl *lock_3a;
+	struct v4l2_ctrl *colorfx;
+	struct v4l2_ctrl *saturation;
+	struct v4l2_ctrl *zoom;
+	struct v4l2_ctrl *wdr;
+	struct v4l2_ctrl *stabilization;
+	struct v4l2_ctrl *jpeg_quality;
+
+	int (*set_power)(struct device *dev, int on);
+
+	struct mutex lock;
+
+	struct v4l2_mbus_framefmt ffmt[M5MOLS_RESTYPE_MAX];
+	int res_type;
+
+	struct m5mols_version ver;
+	struct m5mols_capture cap;
+
+	unsigned int isp_ready:1;
+	unsigned int power:1;
+	unsigned int ctrl_sync:1;
+
+	u8 resolution;
+	u8 mode;
+};
+
+#define is_available_af(__info)	(__info->ver.af)
+#define is_code(__code, __type) (__code == m5mols_default_ffmt[__type].code)
+#define is_manufacturer(__info, __manufacturer)	\
+				(__info->ver.str[0] == __manufacturer[0] && \
+				 __info->ver.str[1] == __manufacturer[1])
+/*
+ * I2C operation of the M-5MOLS
+ *
+ * The I2C read operation of the M-5MOLS requires 2 messages. The first
+ * message sends the information about the command, command category, and total
+ * message size. The second message is used to retrieve the data specifed in
+ * the first message
+ *
+ *   1st message                                2nd message
+ *   +-------+---+----------+-----+-------+     +------+------+------+------+
+ *   | size1 | R | category | cmd | size2 |     | d[0] | d[1] | d[2] | d[3] |
+ *   +-------+---+----------+-----+-------+     +------+------+------+------+
+ *   - size1: message data size(5 in this case)
+ *   - size2: desired buffer size of the 2nd message
+ *   - d[0..3]: according to size2
+ *
+ * The I2C write operation needs just one message. The message includes
+ * category, command, total size, and desired data.
+ *
+ *   1st message
+ *   +-------+---+----------+-----+------+------+------+------+
+ *   | size1 | W | category | cmd | d[0] | d[1] | d[2] | d[3] |
+ *   +-------+---+----------+-----+------+------+------+------+
+ *   - d[0..3]: according to size1
+ */
+int m5mols_read_u8(struct v4l2_subdev *sd, u32 reg_comb, u8 *val);
+int m5mols_read_u16(struct v4l2_subdev *sd, u32 reg_comb, u16 *val);
+int m5mols_read_u32(struct v4l2_subdev *sd, u32 reg_comb, u32 *val);
+int m5mols_write(struct v4l2_subdev *sd, u32 reg_comb, u32 val);
+
+int m5mols_busy_wait(struct v4l2_subdev *sd, u32 reg, u32 value, u32 mask,
+		     int timeout);
+
+/* Mask value for busy waiting until M-5MOLS I2C interface is initialized */
+#define M5MOLS_I2C_RDY_WAIT_FL		(1 << 16)
+/* ISP state transition timeout, in ms */
+#define M5MOLS_MODE_CHANGE_TIMEOUT	200
+#define M5MOLS_BUSY_WAIT_DEF_TIMEOUT	250
+
+/*
+ * Mode operation of the M-5MOLS
+ *
+ * Changing the mode of the M-5MOLS is needed right executing order.
+ * There are three modes(PARAMETER, MONITOR, CAPTURE) which can be changed
+ * by user. There are various categories associated with each mode.
+ *
+ * +============================================================+
+ * | mode	| category					|
+ * +============================================================+
+ * | FLASH	| FLASH(only after Stand-by or Power-on)	|
+ * | SYSTEM	| SYSTEM(only after sensor arm-booting)		|
+ * | PARAMETER	| PARAMETER					|
+ * | MONITOR	| MONITOR(preview), Auto Focus, Face Detection	|
+ * | CAPTURE	| Single CAPTURE, Preview(recording)		|
+ * +============================================================+
+ *
+ * The available executing order between each modes are as follows:
+ *   PARAMETER <---> MONITOR <---> CAPTURE
+ */
+int m5mols_set_mode(struct m5mols_info *info, u8 mode);
+
+int m5mols_enable_interrupt(struct v4l2_subdev *sd, u8 reg);
+int m5mols_wait_interrupt(struct v4l2_subdev *sd, u8 condition, u32 timeout);
+int m5mols_restore_controls(struct m5mols_info *info);
+int m5mols_start_capture(struct m5mols_info *info);
+int m5mols_do_scenemode(struct m5mols_info *info, u8 mode);
+int m5mols_lock_3a(struct m5mols_info *info, bool lock);
+int m5mols_set_ctrl(struct v4l2_ctrl *ctrl);
+int m5mols_init_controls(struct v4l2_subdev *sd);
+
+/* The firmware function */
+int m5mols_update_fw(struct v4l2_subdev *sd,
+		     int (*set_power)(struct m5mols_info *, bool));
+
+static inline struct m5mols_info *to_m5mols(struct v4l2_subdev *subdev)
+{
+	return container_of(subdev, struct m5mols_info, sd);
+}
+
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	struct m5mols_info *info = container_of(ctrl->handler,
+						struct m5mols_info, handle);
+	return &info->sd;
+}
+
+static inline void m5mols_set_ctrl_mode(struct v4l2_ctrl *ctrl,
+					unsigned int mode)
+{
+	ctrl->priv = (void *)(uintptr_t)mode;
+}
+
+static inline unsigned int m5mols_get_ctrl_mode(struct v4l2_ctrl *ctrl)
+{
+	return (unsigned int)(uintptr_t)ctrl->priv;
+}
+
+#endif	/* M5MOLS_H */
diff --git a/drivers/media/i2c/m5mols/m5mols_capture.c b/drivers/media/i2c/m5mols/m5mols_capture.c
new file mode 100644
index 000000000000..cb243bd278ce
--- /dev/null
+++ b/drivers/media/i2c/m5mols/m5mols_capture.c
@@ -0,0 +1,155 @@
+
+/*
+ * The Capture code for Fujitsu M-5MOLS ISP
+ *
+ * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ * Author: HeungJun Kim <riverful.kim@samsung.com>
+ *
+ * Copyright (C) 2009 Samsung Electronics Co., Ltd.
+ * Author: Dongsoo Nathaniel Kim <dongsoo45.kim@samsung.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; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/regulator/consumer.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+#include <media/m5mols.h>
+#include <media/s5p_fimc.h>
+
+#include "m5mols.h"
+#include "m5mols_reg.h"
+
+/**
+ * m5mols_read_rational - I2C read of a rational number
+ *
+ * Read numerator and denominator from registers @addr_num and @addr_den
+ * respectively and return the division result in @val.
+ */
+static int m5mols_read_rational(struct v4l2_subdev *sd, u32 addr_num,
+				u32 addr_den, u32 *val)
+{
+	u32 num, den;
+
+	int ret = m5mols_read_u32(sd, addr_num, &num);
+	if (!ret)
+		ret = m5mols_read_u32(sd, addr_den, &den);
+	if (ret)
+		return ret;
+	*val = den == 0 ? 0 : num / den;
+	return ret;
+}
+
+/**
+ * m5mols_capture_info - Gather captured image information
+ *
+ * For now it gathers only EXIF information and file size.
+ */
+static int m5mols_capture_info(struct m5mols_info *info)
+{
+	struct m5mols_exif *exif = &info->cap.exif;
+	struct v4l2_subdev *sd = &info->sd;
+	int ret;
+
+	ret = m5mols_read_rational(sd, EXIF_INFO_EXPTIME_NU,
+				   EXIF_INFO_EXPTIME_DE, &exif->exposure_time);
+	if (ret)
+		return ret;
+	ret = m5mols_read_rational(sd, EXIF_INFO_TV_NU, EXIF_INFO_TV_DE,
+				   &exif->shutter_speed);
+	if (ret)
+		return ret;
+	ret = m5mols_read_rational(sd, EXIF_INFO_AV_NU, EXIF_INFO_AV_DE,
+				   &exif->aperture);
+	if (ret)
+		return ret;
+	ret = m5mols_read_rational(sd, EXIF_INFO_BV_NU, EXIF_INFO_BV_DE,
+				   &exif->brightness);
+	if (ret)
+		return ret;
+	ret = m5mols_read_rational(sd, EXIF_INFO_EBV_NU, EXIF_INFO_EBV_DE,
+				   &exif->exposure_bias);
+	if (ret)
+		return ret;
+
+	ret = m5mols_read_u16(sd, EXIF_INFO_ISO, &exif->iso_speed);
+	if (!ret)
+		ret = m5mols_read_u16(sd, EXIF_INFO_FLASH, &exif->flash);
+	if (!ret)
+		ret = m5mols_read_u16(sd, EXIF_INFO_SDR, &exif->sdr);
+	if (!ret)
+		ret = m5mols_read_u16(sd, EXIF_INFO_QVAL, &exif->qval);
+	if (ret)
+		return ret;
+
+	if (!ret)
+		ret = m5mols_read_u32(sd, CAPC_IMAGE_SIZE, &info->cap.main);
+	if (!ret)
+		ret = m5mols_read_u32(sd, CAPC_THUMB_SIZE, &info->cap.thumb);
+	if (!ret)
+		info->cap.total = info->cap.main + info->cap.thumb;
+
+	return ret;
+}
+
+int m5mols_start_capture(struct m5mols_info *info)
+{
+	struct v4l2_subdev *sd = &info->sd;
+	int ret;
+
+	/*
+	 * Synchronize the controls, set the capture frame resolution and color
+	 * format. The frame capture is initiated during switching from Monitor
+	 * to Capture mode.
+	 */
+	ret = m5mols_set_mode(info, REG_MONITOR);
+	if (!ret)
+		ret = m5mols_restore_controls(info);
+	if (!ret)
+		ret = m5mols_write(sd, CAPP_YUVOUT_MAIN, REG_JPEG);
+	if (!ret)
+		ret = m5mols_write(sd, CAPP_MAIN_IMAGE_SIZE, info->resolution);
+	if (!ret)
+		ret = m5mols_set_mode(info, REG_CAPTURE);
+	if (!ret)
+		/* Wait until a frame is captured to ISP internal memory */
+		ret = m5mols_wait_interrupt(sd, REG_INT_CAPTURE, 2000);
+	if (ret)
+		return ret;
+
+	/*
+	 * Initiate the captured data transfer to a MIPI-CSI receiver.
+	 */
+	ret = m5mols_write(sd, CAPC_SEL_FRAME, 1);
+	if (!ret)
+		ret = m5mols_write(sd, CAPC_START, REG_CAP_START_MAIN);
+	if (!ret) {
+		bool captured = false;
+		unsigned int size;
+
+		/* Wait for the capture completion interrupt */
+		ret = m5mols_wait_interrupt(sd, REG_INT_CAPTURE, 2000);
+		if (!ret) {
+			captured = true;
+			ret = m5mols_capture_info(info);
+		}
+		size = captured ? info->cap.main : 0;
+		v4l2_dbg(1, m5mols_debug, sd, "%s: size: %d, thumb.: %d B\n",
+			 __func__, size, info->cap.thumb);
+
+		v4l2_subdev_notify(sd, S5P_FIMC_TX_END_NOTIFY, &size);
+	}
+
+	return ret;
+}
diff --git a/drivers/media/i2c/m5mols/m5mols_controls.c b/drivers/media/i2c/m5mols/m5mols_controls.c
new file mode 100644
index 000000000000..f34429e452ab
--- /dev/null
+++ b/drivers/media/i2c/m5mols/m5mols_controls.c
@@ -0,0 +1,628 @@
+/*
+ * Controls for M-5MOLS 8M Pixel camera sensor with ISP
+ *
+ * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ * Author: HeungJun Kim <riverful.kim@samsung.com>
+ *
+ * Copyright (C) 2009 Samsung Electronics Co., Ltd.
+ * Author: Dongsoo Nathaniel Kim <dongsoo45.kim@samsung.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; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-ctrls.h>
+
+#include "m5mols.h"
+#include "m5mols_reg.h"
+
+static struct m5mols_scenemode m5mols_default_scenemode[] = {
+	[REG_SCENE_NORMAL] = {
+		REG_AE_CENTER, REG_AE_INDEX_00, REG_AWB_AUTO, 0,
+		REG_CHROMA_ON, 3, REG_EDGE_ON, 5,
+		REG_AF_NORMAL, REG_FD_OFF,
+		REG_MCC_NORMAL, REG_LIGHT_OFF, REG_FLASH_OFF,
+		5, REG_ISO_AUTO, REG_CAP_NONE, REG_WDR_OFF,
+	},
+	[REG_SCENE_PORTRAIT] = {
+		REG_AE_CENTER, REG_AE_INDEX_00, REG_AWB_AUTO, 0,
+		REG_CHROMA_ON, 3, REG_EDGE_ON, 4,
+		REG_AF_NORMAL, BIT_FD_EN | BIT_FD_DRAW_FACE_FRAME,
+		REG_MCC_OFF, REG_LIGHT_OFF, REG_FLASH_OFF,
+		6, REG_ISO_AUTO, REG_CAP_NONE, REG_WDR_OFF,
+	},
+	[REG_SCENE_LANDSCAPE] = {
+		REG_AE_ALL, REG_AE_INDEX_00, REG_AWB_AUTO, 0,
+		REG_CHROMA_ON, 4, REG_EDGE_ON, 6,
+		REG_AF_NORMAL, REG_FD_OFF,
+		REG_MCC_OFF, REG_LIGHT_OFF, REG_FLASH_OFF,
+		6, REG_ISO_AUTO, REG_CAP_NONE, REG_WDR_OFF,
+	},
+	[REG_SCENE_SPORTS] = {
+		REG_AE_CENTER, REG_AE_INDEX_00, REG_AWB_AUTO, 0,
+		REG_CHROMA_ON, 3, REG_EDGE_ON, 5,
+		REG_AF_NORMAL, REG_FD_OFF,
+		REG_MCC_OFF, REG_LIGHT_OFF, REG_FLASH_OFF,
+		6, REG_ISO_AUTO, REG_CAP_NONE, REG_WDR_OFF,
+	},
+	[REG_SCENE_PARTY_INDOOR] = {
+		REG_AE_CENTER, REG_AE_INDEX_00, REG_AWB_AUTO, 0,
+		REG_CHROMA_ON, 4, REG_EDGE_ON, 5,
+		REG_AF_NORMAL, REG_FD_OFF,
+		REG_MCC_OFF, REG_LIGHT_OFF, REG_FLASH_OFF,
+		6, REG_ISO_200, REG_CAP_NONE, REG_WDR_OFF,
+	},
+	[REG_SCENE_BEACH_SNOW] = {
+		REG_AE_CENTER, REG_AE_INDEX_10_POS, REG_AWB_AUTO, 0,
+		REG_CHROMA_ON, 4, REG_EDGE_ON, 5,
+		REG_AF_NORMAL, REG_FD_OFF,
+		REG_MCC_OFF, REG_LIGHT_OFF, REG_FLASH_OFF,
+		6, REG_ISO_50, REG_CAP_NONE, REG_WDR_OFF,
+	},
+	[REG_SCENE_SUNSET] = {
+		REG_AE_CENTER, REG_AE_INDEX_00, REG_AWB_PRESET,
+		REG_AWB_DAYLIGHT,
+		REG_CHROMA_ON, 3, REG_EDGE_ON, 5,
+		REG_AF_NORMAL, REG_FD_OFF,
+		REG_MCC_OFF, REG_LIGHT_OFF, REG_FLASH_OFF,
+		6, REG_ISO_AUTO, REG_CAP_NONE, REG_WDR_OFF,
+	},
+	[REG_SCENE_DAWN_DUSK] = {
+		REG_AE_CENTER, REG_AE_INDEX_00, REG_AWB_PRESET,
+		REG_AWB_FLUORESCENT_1,
+		REG_CHROMA_ON, 3, REG_EDGE_ON, 5,
+		REG_AF_NORMAL, REG_FD_OFF,
+		REG_MCC_OFF, REG_LIGHT_OFF, REG_FLASH_OFF,
+		6, REG_ISO_AUTO, REG_CAP_NONE, REG_WDR_OFF,
+	},
+	[REG_SCENE_FALL] = {
+		REG_AE_CENTER, REG_AE_INDEX_00, REG_AWB_AUTO, 0,
+		REG_CHROMA_ON, 5, REG_EDGE_ON, 5,
+		REG_AF_NORMAL, REG_FD_OFF,
+		REG_MCC_OFF, REG_LIGHT_OFF, REG_FLASH_OFF,
+		6, REG_ISO_AUTO, REG_CAP_NONE, REG_WDR_OFF,
+	},
+	[REG_SCENE_NIGHT] = {
+		REG_AE_CENTER, REG_AE_INDEX_00, REG_AWB_AUTO, 0,
+		REG_CHROMA_ON, 3, REG_EDGE_ON, 5,
+		REG_AF_NORMAL, REG_FD_OFF,
+		REG_MCC_OFF, REG_LIGHT_OFF, REG_FLASH_OFF,
+		6, REG_ISO_AUTO, REG_CAP_NONE, REG_WDR_OFF,
+	},
+	[REG_SCENE_AGAINST_LIGHT] = {
+		REG_AE_CENTER, REG_AE_INDEX_00, REG_AWB_AUTO, 0,
+		REG_CHROMA_ON, 3, REG_EDGE_ON, 5,
+		REG_AF_NORMAL, REG_FD_OFF,
+		REG_MCC_OFF, REG_LIGHT_OFF, REG_FLASH_OFF,
+		6, REG_ISO_AUTO, REG_CAP_NONE, REG_WDR_OFF,
+	},
+	[REG_SCENE_FIRE] = {
+		REG_AE_CENTER, REG_AE_INDEX_00, REG_AWB_AUTO, 0,
+		REG_CHROMA_ON, 3, REG_EDGE_ON, 5,
+		REG_AF_NORMAL, REG_FD_OFF,
+		REG_MCC_OFF, REG_LIGHT_OFF, REG_FLASH_OFF,
+		6, REG_ISO_50, REG_CAP_NONE, REG_WDR_OFF,
+	},
+	[REG_SCENE_TEXT] = {
+		REG_AE_CENTER, REG_AE_INDEX_00, REG_AWB_AUTO, 0,
+		REG_CHROMA_ON, 3, REG_EDGE_ON, 7,
+		REG_AF_MACRO, REG_FD_OFF,
+		REG_MCC_OFF, REG_LIGHT_OFF, REG_FLASH_OFF,
+		6, REG_ISO_AUTO, REG_CAP_ANTI_SHAKE, REG_WDR_ON,
+	},
+	[REG_SCENE_CANDLE] = {
+		REG_AE_CENTER, REG_AE_INDEX_00, REG_AWB_AUTO, 0,
+		REG_CHROMA_ON, 3, REG_EDGE_ON, 5,
+		REG_AF_NORMAL, REG_FD_OFF,
+		REG_MCC_OFF, REG_LIGHT_OFF, REG_FLASH_OFF,
+		6, REG_ISO_AUTO, REG_CAP_NONE, REG_WDR_OFF,
+	},
+};
+
+/**
+ * m5mols_do_scenemode() - Change current scenemode
+ * @mode:	Desired mode of the scenemode
+ *
+ * WARNING: The execution order is important. Do not change the order.
+ */
+int m5mols_do_scenemode(struct m5mols_info *info, u8 mode)
+{
+	struct v4l2_subdev *sd = &info->sd;
+	struct m5mols_scenemode scenemode = m5mols_default_scenemode[mode];
+	int ret;
+
+	if (mode > REG_SCENE_CANDLE)
+		return -EINVAL;
+
+	ret = v4l2_ctrl_s_ctrl(info->lock_3a, 0);
+	if (!ret)
+		ret = m5mols_write(sd, AE_EV_PRESET_MONITOR, mode);
+	if (!ret)
+		ret = m5mols_write(sd, AE_EV_PRESET_CAPTURE, mode);
+	if (!ret)
+		ret = m5mols_write(sd, AE_MODE, scenemode.metering);
+	if (!ret)
+		ret = m5mols_write(sd, AE_INDEX, scenemode.ev_bias);
+	if (!ret)
+		ret = m5mols_write(sd, AWB_MODE, scenemode.wb_mode);
+	if (!ret)
+		ret = m5mols_write(sd, AWB_MANUAL, scenemode.wb_preset);
+	if (!ret)
+		ret = m5mols_write(sd, MON_CHROMA_EN, scenemode.chroma_en);
+	if (!ret)
+		ret = m5mols_write(sd, MON_CHROMA_LVL, scenemode.chroma_lvl);
+	if (!ret)
+		ret = m5mols_write(sd, MON_EDGE_EN, scenemode.edge_en);
+	if (!ret)
+		ret = m5mols_write(sd, MON_EDGE_LVL, scenemode.edge_lvl);
+	if (!ret && is_available_af(info))
+		ret = m5mols_write(sd, AF_MODE, scenemode.af_range);
+	if (!ret && is_available_af(info))
+		ret = m5mols_write(sd, FD_CTL, scenemode.fd_mode);
+	if (!ret)
+		ret = m5mols_write(sd, MON_TONE_CTL, scenemode.tone);
+	if (!ret)
+		ret = m5mols_write(sd, AE_ISO, scenemode.iso);
+	if (!ret)
+		ret = m5mols_set_mode(info, REG_CAPTURE);
+	if (!ret)
+		ret = m5mols_write(sd, CAPP_WDR_EN, scenemode.wdr);
+	if (!ret)
+		ret = m5mols_write(sd, CAPP_MCC_MODE, scenemode.mcc);
+	if (!ret)
+		ret = m5mols_write(sd, CAPP_LIGHT_CTRL, scenemode.light);
+	if (!ret)
+		ret = m5mols_write(sd, CAPP_FLASH_CTRL, scenemode.flash);
+	if (!ret)
+		ret = m5mols_write(sd, CAPC_MODE, scenemode.capt_mode);
+	if (!ret)
+		ret = m5mols_set_mode(info, REG_MONITOR);
+
+	return ret;
+}
+
+static int m5mols_3a_lock(struct m5mols_info *info, struct v4l2_ctrl *ctrl)
+{
+	bool af_lock = ctrl->val & V4L2_LOCK_FOCUS;
+	int ret = 0;
+
+	if ((ctrl->val ^ ctrl->cur.val) & V4L2_LOCK_EXPOSURE) {
+		bool ae_lock = ctrl->val & V4L2_LOCK_EXPOSURE;
+
+		ret = m5mols_write(&info->sd, AE_LOCK, ae_lock ?
+				   REG_AE_LOCK : REG_AE_UNLOCK);
+		if (ret)
+			return ret;
+	}
+
+	if (((ctrl->val ^ ctrl->cur.val) & V4L2_LOCK_WHITE_BALANCE)
+	    && info->auto_wb->val) {
+		bool awb_lock = ctrl->val & V4L2_LOCK_WHITE_BALANCE;
+
+		ret = m5mols_write(&info->sd, AWB_LOCK, awb_lock ?
+				   REG_AWB_LOCK : REG_AWB_UNLOCK);
+		if (ret)
+			return ret;
+	}
+
+	if (!info->ver.af || !af_lock)
+		return ret;
+
+	if ((ctrl->val ^ ctrl->cur.val) & V4L2_LOCK_FOCUS)
+		ret = m5mols_write(&info->sd, AF_EXECUTE, REG_AF_STOP);
+
+	return ret;
+}
+
+static int m5mols_set_metering_mode(struct m5mols_info *info, int mode)
+{
+	unsigned int metering;
+
+	switch (mode) {
+	case V4L2_EXPOSURE_METERING_CENTER_WEIGHTED:
+		metering = REG_AE_CENTER;
+		break;
+	case V4L2_EXPOSURE_METERING_SPOT:
+		metering = REG_AE_SPOT;
+		break;
+	default:
+		metering = REG_AE_ALL;
+		break;
+	}
+
+	return m5mols_write(&info->sd, AE_MODE, metering);
+}
+
+static int m5mols_set_exposure(struct m5mols_info *info, int exposure)
+{
+	struct v4l2_subdev *sd = &info->sd;
+	int ret = 0;
+
+	if (exposure == V4L2_EXPOSURE_AUTO) {
+		/* Unlock auto exposure */
+		info->lock_3a->val &= ~V4L2_LOCK_EXPOSURE;
+		m5mols_3a_lock(info, info->lock_3a);
+
+		ret = m5mols_set_metering_mode(info, info->metering->val);
+		if (ret < 0)
+			return ret;
+
+		v4l2_dbg(1, m5mols_debug, sd,
+			 "%s: exposure bias: %#x, metering: %#x\n",
+			 __func__, info->exposure_bias->val,
+			 info->metering->val);
+
+		return m5mols_write(sd, AE_INDEX, info->exposure_bias->val);
+	}
+
+	if (exposure == V4L2_EXPOSURE_MANUAL) {
+		ret = m5mols_write(sd, AE_MODE, REG_AE_OFF);
+		if (ret == 0)
+			ret = m5mols_write(sd, AE_MAN_GAIN_MON,
+					   info->exposure->val);
+		if (ret == 0)
+			ret = m5mols_write(sd, AE_MAN_GAIN_CAP,
+					   info->exposure->val);
+
+		v4l2_dbg(1, m5mols_debug, sd, "%s: exposure: %#x\n",
+			 __func__, info->exposure->val);
+	}
+
+	return ret;
+}
+
+static int m5mols_set_white_balance(struct m5mols_info *info, int val)
+{
+	static const unsigned short wb[][2] = {
+		{ V4L2_WHITE_BALANCE_INCANDESCENT,  REG_AWB_INCANDESCENT },
+		{ V4L2_WHITE_BALANCE_FLUORESCENT,   REG_AWB_FLUORESCENT_1 },
+		{ V4L2_WHITE_BALANCE_FLUORESCENT_H, REG_AWB_FLUORESCENT_2 },
+		{ V4L2_WHITE_BALANCE_HORIZON,       REG_AWB_HORIZON },
+		{ V4L2_WHITE_BALANCE_DAYLIGHT,      REG_AWB_DAYLIGHT },
+		{ V4L2_WHITE_BALANCE_FLASH,         REG_AWB_LEDLIGHT },
+		{ V4L2_WHITE_BALANCE_CLOUDY,        REG_AWB_CLOUDY },
+		{ V4L2_WHITE_BALANCE_SHADE,         REG_AWB_SHADE },
+		{ V4L2_WHITE_BALANCE_AUTO,          REG_AWB_AUTO },
+	};
+	int i;
+	struct v4l2_subdev *sd = &info->sd;
+	int ret = -EINVAL;
+
+	for (i = 0; i < ARRAY_SIZE(wb); i++) {
+		int awb;
+		if (wb[i][0] != val)
+			continue;
+
+		v4l2_dbg(1, m5mols_debug, sd,
+			 "Setting white balance to: %#x\n", wb[i][0]);
+
+		awb = wb[i][0] == V4L2_WHITE_BALANCE_AUTO;
+		ret = m5mols_write(sd, AWB_MODE, awb ? REG_AWB_AUTO :
+						 REG_AWB_PRESET);
+		if (ret < 0)
+			return ret;
+
+		if (!awb)
+			ret = m5mols_write(sd, AWB_MANUAL, wb[i][1]);
+	}
+
+	return ret;
+}
+
+static int m5mols_set_saturation(struct m5mols_info *info, int val)
+{
+	int ret = m5mols_write(&info->sd, MON_CHROMA_LVL, val);
+	if (ret < 0)
+		return ret;
+
+	return m5mols_write(&info->sd, MON_CHROMA_EN, REG_CHROMA_ON);
+}
+
+static int m5mols_set_color_effect(struct m5mols_info *info, int val)
+{
+	unsigned int m_effect = REG_COLOR_EFFECT_OFF;
+	unsigned int p_effect = REG_EFFECT_OFF;
+	unsigned int cfix_r = 0, cfix_b = 0;
+	struct v4l2_subdev *sd = &info->sd;
+	int ret = 0;
+
+	switch (val) {
+	case V4L2_COLORFX_BW:
+		m_effect = REG_COLOR_EFFECT_ON;
+		break;
+	case V4L2_COLORFX_NEGATIVE:
+		p_effect = REG_EFFECT_NEGA;
+		break;
+	case V4L2_COLORFX_EMBOSS:
+		p_effect = REG_EFFECT_EMBOSS;
+		break;
+	case V4L2_COLORFX_SEPIA:
+		m_effect = REG_COLOR_EFFECT_ON;
+		cfix_r = REG_CFIXR_SEPIA;
+		cfix_b = REG_CFIXB_SEPIA;
+		break;
+	}
+
+	ret = m5mols_write(sd, PARM_EFFECT, p_effect);
+	if (!ret)
+		ret = m5mols_write(sd, MON_EFFECT, m_effect);
+
+	if (ret == 0 && m_effect == REG_COLOR_EFFECT_ON) {
+		ret = m5mols_write(sd, MON_CFIXR, cfix_r);
+		if (!ret)
+			ret = m5mols_write(sd, MON_CFIXB, cfix_b);
+	}
+
+	v4l2_dbg(1, m5mols_debug, sd,
+		 "p_effect: %#x, m_effect: %#x, r: %#x, b: %#x (%d)\n",
+		 p_effect, m_effect, cfix_r, cfix_b, ret);
+
+	return ret;
+}
+
+static int m5mols_set_iso(struct m5mols_info *info, int auto_iso)
+{
+	u32 iso = auto_iso ? 0 : info->iso->val + 1;
+
+	return m5mols_write(&info->sd, AE_ISO, iso);
+}
+
+static int m5mols_set_wdr(struct m5mols_info *info, int wdr)
+{
+	int ret;
+
+	ret = m5mols_write(&info->sd, MON_TONE_CTL, wdr ? 9 : 5);
+	if (ret < 0)
+		return ret;
+
+	ret = m5mols_set_mode(info, REG_CAPTURE);
+	if (ret < 0)
+		return ret;
+
+	return m5mols_write(&info->sd, CAPP_WDR_EN, wdr);
+}
+
+static int m5mols_set_stabilization(struct m5mols_info *info, int val)
+{
+	struct v4l2_subdev *sd = &info->sd;
+	unsigned int evp = val ? 0xe : 0x0;
+	int ret;
+
+	ret = m5mols_write(sd, AE_EV_PRESET_MONITOR, evp);
+	if (ret < 0)
+		return ret;
+
+	return m5mols_write(sd, AE_EV_PRESET_CAPTURE, evp);
+}
+
+static int m5mols_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+	struct m5mols_info *info = to_m5mols(sd);
+	int ret = 0;
+	u8 status;
+
+	v4l2_dbg(1, m5mols_debug, sd, "%s: ctrl: %s (%d)\n",
+		 __func__, ctrl->name, info->isp_ready);
+
+	if (!info->isp_ready)
+		return -EBUSY;
+
+	switch (ctrl->id) {
+	case V4L2_CID_ISO_SENSITIVITY_AUTO:
+		ret = m5mols_read_u8(sd, AE_ISO, &status);
+		if (ret == 0)
+			ctrl->val = !status;
+		if (status != REG_ISO_AUTO)
+			info->iso->val = status - 1;
+		break;
+
+	case V4L2_CID_3A_LOCK:
+		ctrl->val &= ~0x7;
+
+		ret = m5mols_read_u8(sd, AE_LOCK, &status);
+		if (ret)
+			return ret;
+		if (status)
+			info->lock_3a->val |= V4L2_LOCK_EXPOSURE;
+
+		ret = m5mols_read_u8(sd, AWB_LOCK, &status);
+		if (ret)
+			return ret;
+		if (status)
+			info->lock_3a->val |= V4L2_LOCK_EXPOSURE;
+
+		ret = m5mols_read_u8(sd, AF_EXECUTE, &status);
+		if (!status)
+			info->lock_3a->val |= V4L2_LOCK_EXPOSURE;
+		break;
+	}
+
+	return ret;
+}
+
+static int m5mols_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	unsigned int ctrl_mode = m5mols_get_ctrl_mode(ctrl);
+	struct v4l2_subdev *sd = to_sd(ctrl);
+	struct m5mols_info *info = to_m5mols(sd);
+	int last_mode = info->mode;
+	int ret = 0;
+
+	/*
+	 * If needed, defer restoring the controls until
+	 * the device is fully initialized.
+	 */
+	if (!info->isp_ready) {
+		info->ctrl_sync = 0;
+		return 0;
+	}
+
+	v4l2_dbg(1, m5mols_debug, sd, "%s: %s, val: %d, priv: %p\n",
+		 __func__, ctrl->name, ctrl->val, ctrl->priv);
+
+	if (ctrl_mode && ctrl_mode != info->mode) {
+		ret = m5mols_set_mode(info, ctrl_mode);
+		if (ret < 0)
+			return ret;
+	}
+
+	switch (ctrl->id) {
+	case V4L2_CID_3A_LOCK:
+		ret = m5mols_3a_lock(info, ctrl);
+		break;
+
+	case V4L2_CID_ZOOM_ABSOLUTE:
+		ret = m5mols_write(sd, MON_ZOOM, ctrl->val);
+		break;
+
+	case V4L2_CID_EXPOSURE_AUTO:
+		ret = m5mols_set_exposure(info, ctrl->val);
+		break;
+
+	case V4L2_CID_ISO_SENSITIVITY:
+		ret = m5mols_set_iso(info, ctrl->val);
+		break;
+
+	case V4L2_CID_AUTO_N_PRESET_WHITE_BALANCE:
+		ret = m5mols_set_white_balance(info, ctrl->val);
+		break;
+
+	case V4L2_CID_SATURATION:
+		ret = m5mols_set_saturation(info, ctrl->val);
+		break;
+
+	case V4L2_CID_COLORFX:
+		ret = m5mols_set_color_effect(info, ctrl->val);
+		break;
+
+	case V4L2_CID_WIDE_DYNAMIC_RANGE:
+		ret = m5mols_set_wdr(info, ctrl->val);
+		break;
+
+	case V4L2_CID_IMAGE_STABILIZATION:
+		ret = m5mols_set_stabilization(info, ctrl->val);
+		break;
+
+	case V4L2_CID_JPEG_COMPRESSION_QUALITY:
+		ret = m5mols_write(sd, CAPP_JPEG_RATIO, ctrl->val);
+		break;
+	}
+
+	if (ret == 0 && info->mode != last_mode)
+		ret = m5mols_set_mode(info, last_mode);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops m5mols_ctrl_ops = {
+	.g_volatile_ctrl	= m5mols_g_volatile_ctrl,
+	.s_ctrl			= m5mols_s_ctrl,
+};
+
+/* Supported manual ISO values */
+static const s64 iso_qmenu[] = {
+	/* AE_ISO: 0x01...0x07 (ISO: 50...3200) */
+	50000, 100000, 200000, 400000, 800000, 1600000, 3200000
+};
+
+/* Supported Exposure Bias values, -2.0EV...+2.0EV */
+static const s64 ev_bias_qmenu[] = {
+	/* AE_INDEX: 0x00...0x08 */
+	-2000, -1500, -1000, -500, 0, 500, 1000, 1500, 2000
+};
+
+int m5mols_init_controls(struct v4l2_subdev *sd)
+{
+	struct m5mols_info *info = to_m5mols(sd);
+	u16 exposure_max;
+	u16 zoom_step;
+	int ret;
+
+	/* Determine the firmware dependant control range and step values */
+	ret = m5mols_read_u16(sd, AE_MAX_GAIN_MON, &exposure_max);
+	if (ret < 0)
+		return ret;
+
+	zoom_step = is_manufacturer(info, REG_SAMSUNG_OPTICS) ? 31 : 1;
+	v4l2_ctrl_handler_init(&info->handle, 20);
+
+	info->auto_wb = v4l2_ctrl_new_std_menu(&info->handle,
+			&m5mols_ctrl_ops, V4L2_CID_AUTO_N_PRESET_WHITE_BALANCE,
+			9, ~0x3fe, V4L2_WHITE_BALANCE_AUTO);
+
+	/* Exposure control cluster */
+	info->auto_exposure = v4l2_ctrl_new_std_menu(&info->handle,
+			&m5mols_ctrl_ops, V4L2_CID_EXPOSURE_AUTO,
+			1, ~0x03, V4L2_EXPOSURE_AUTO);
+
+	info->exposure = v4l2_ctrl_new_std(&info->handle,
+			&m5mols_ctrl_ops, V4L2_CID_EXPOSURE,
+			0, exposure_max, 1, exposure_max / 2);
+
+	info->exposure_bias = v4l2_ctrl_new_int_menu(&info->handle,
+			&m5mols_ctrl_ops, V4L2_CID_AUTO_EXPOSURE_BIAS,
+			ARRAY_SIZE(ev_bias_qmenu) - 1,
+			ARRAY_SIZE(ev_bias_qmenu)/2 - 1,
+			ev_bias_qmenu);
+
+	info->metering = v4l2_ctrl_new_std_menu(&info->handle,
+			&m5mols_ctrl_ops, V4L2_CID_EXPOSURE_METERING,
+			2, ~0x7, V4L2_EXPOSURE_METERING_AVERAGE);
+
+	/* ISO control cluster */
+	info->auto_iso = v4l2_ctrl_new_std_menu(&info->handle, &m5mols_ctrl_ops,
+			V4L2_CID_ISO_SENSITIVITY_AUTO, 1, ~0x03, 1);
+
+	info->iso = v4l2_ctrl_new_int_menu(&info->handle, &m5mols_ctrl_ops,
+			V4L2_CID_ISO_SENSITIVITY, ARRAY_SIZE(iso_qmenu) - 1,
+			ARRAY_SIZE(iso_qmenu)/2 - 1, iso_qmenu);
+
+	info->saturation = v4l2_ctrl_new_std(&info->handle, &m5mols_ctrl_ops,
+			V4L2_CID_SATURATION, 1, 5, 1, 3);
+
+	info->zoom = v4l2_ctrl_new_std(&info->handle, &m5mols_ctrl_ops,
+			V4L2_CID_ZOOM_ABSOLUTE, 1, 70, zoom_step, 1);
+
+	info->colorfx = v4l2_ctrl_new_std_menu(&info->handle, &m5mols_ctrl_ops,
+			V4L2_CID_COLORFX, 4, 0, V4L2_COLORFX_NONE);
+
+	info->wdr = v4l2_ctrl_new_std(&info->handle, &m5mols_ctrl_ops,
+			V4L2_CID_WIDE_DYNAMIC_RANGE, 0, 1, 1, 0);
+
+	info->stabilization = v4l2_ctrl_new_std(&info->handle, &m5mols_ctrl_ops,
+			V4L2_CID_IMAGE_STABILIZATION, 0, 1, 1, 0);
+
+	info->jpeg_quality = v4l2_ctrl_new_std(&info->handle, &m5mols_ctrl_ops,
+			V4L2_CID_JPEG_COMPRESSION_QUALITY, 1, 100, 1, 80);
+
+	info->lock_3a = v4l2_ctrl_new_std(&info->handle, &m5mols_ctrl_ops,
+			V4L2_CID_3A_LOCK, 0, 0x7, 0, 0);
+
+	if (info->handle.error) {
+		int ret = info->handle.error;
+		v4l2_err(sd, "Failed to initialize controls: %d\n", ret);
+		v4l2_ctrl_handler_free(&info->handle);
+		return ret;
+	}
+
+	v4l2_ctrl_auto_cluster(4, &info->auto_exposure, 1, false);
+	info->auto_iso->flags |= V4L2_CTRL_FLAG_VOLATILE |
+				V4L2_CTRL_FLAG_UPDATE;
+	v4l2_ctrl_auto_cluster(2, &info->auto_iso, 0, false);
+
+	info->lock_3a->flags |= V4L2_CTRL_FLAG_VOLATILE;
+
+	m5mols_set_ctrl_mode(info->auto_exposure, REG_PARAMETER);
+	m5mols_set_ctrl_mode(info->auto_wb, REG_PARAMETER);
+	m5mols_set_ctrl_mode(info->colorfx, REG_MONITOR);
+
+	sd->ctrl_handler = &info->handle;
+
+	return 0;
+}
diff --git a/drivers/media/i2c/m5mols/m5mols_core.c b/drivers/media/i2c/m5mols/m5mols_core.c
new file mode 100644
index 000000000000..2f490ef26c38
--- /dev/null
+++ b/drivers/media/i2c/m5mols/m5mols_core.c
@@ -0,0 +1,1010 @@
+/*
+ * Driver for M-5MOLS 8M Pixel camera sensor with ISP
+ *
+ * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ * Author: HeungJun Kim <riverful.kim@samsung.com>
+ *
+ * Copyright (C) 2009 Samsung Electronics Co., Ltd.
+ * Author: Dongsoo Nathaniel Kim <dongsoo45.kim@samsung.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; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/regulator/consumer.h>
+#include <linux/videodev2.h>
+#include <linux/module.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+#include <media/m5mols.h>
+
+#include "m5mols.h"
+#include "m5mols_reg.h"
+
+int m5mols_debug;
+module_param(m5mols_debug, int, 0644);
+
+#define MODULE_NAME		"M5MOLS"
+#define M5MOLS_I2C_CHECK_RETRY	500
+
+/* The regulator consumer names for external voltage regulators */
+static struct regulator_bulk_data supplies[] = {
+	{
+		.supply = "core",	/* ARM core power, 1.2V */
+	}, {
+		.supply	= "dig_18",	/* digital power 1, 1.8V */
+	}, {
+		.supply	= "d_sensor",	/* sensor power 1, 1.8V */
+	}, {
+		.supply	= "dig_28",	/* digital power 2, 2.8V */
+	}, {
+		.supply	= "a_sensor",	/* analog power */
+	}, {
+		.supply	= "dig_12",	/* digital power 3, 1.2V */
+	},
+};
+
+static struct v4l2_mbus_framefmt m5mols_default_ffmt[M5MOLS_RESTYPE_MAX] = {
+	[M5MOLS_RESTYPE_MONITOR] = {
+		.width		= 1920,
+		.height		= 1080,
+		.code		= V4L2_MBUS_FMT_VYUY8_2X8,
+		.field		= V4L2_FIELD_NONE,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+	},
+	[M5MOLS_RESTYPE_CAPTURE] = {
+		.width		= 1920,
+		.height		= 1080,
+		.code		= V4L2_MBUS_FMT_JPEG_1X8,
+		.field		= V4L2_FIELD_NONE,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+	},
+};
+#define SIZE_DEFAULT_FFMT	ARRAY_SIZE(m5mols_default_ffmt)
+
+static const struct m5mols_resolution m5mols_reg_res[] = {
+	{ 0x01, M5MOLS_RESTYPE_MONITOR, 128, 96 },	/* SUB-QCIF */
+	{ 0x03, M5MOLS_RESTYPE_MONITOR, 160, 120 },	/* QQVGA */
+	{ 0x05, M5MOLS_RESTYPE_MONITOR, 176, 144 },	/* QCIF */
+	{ 0x06, M5MOLS_RESTYPE_MONITOR, 176, 176 },
+	{ 0x08, M5MOLS_RESTYPE_MONITOR, 240, 320 },	/* QVGA */
+	{ 0x09, M5MOLS_RESTYPE_MONITOR, 320, 240 },	/* QVGA */
+	{ 0x0c, M5MOLS_RESTYPE_MONITOR, 240, 400 },	/* WQVGA */
+	{ 0x0d, M5MOLS_RESTYPE_MONITOR, 400, 240 },	/* WQVGA */
+	{ 0x0e, M5MOLS_RESTYPE_MONITOR, 352, 288 },	/* CIF */
+	{ 0x13, M5MOLS_RESTYPE_MONITOR, 480, 360 },
+	{ 0x15, M5MOLS_RESTYPE_MONITOR, 640, 360 },	/* qHD */
+	{ 0x17, M5MOLS_RESTYPE_MONITOR, 640, 480 },	/* VGA */
+	{ 0x18, M5MOLS_RESTYPE_MONITOR, 720, 480 },
+	{ 0x1a, M5MOLS_RESTYPE_MONITOR, 800, 480 },	/* WVGA */
+	{ 0x1f, M5MOLS_RESTYPE_MONITOR, 800, 600 },	/* SVGA */
+	{ 0x21, M5MOLS_RESTYPE_MONITOR, 1280, 720 },	/* HD */
+	{ 0x25, M5MOLS_RESTYPE_MONITOR, 1920, 1080 },	/* 1080p */
+	{ 0x29, M5MOLS_RESTYPE_MONITOR, 3264, 2448 },	/* 2.63fps 8M */
+	{ 0x39, M5MOLS_RESTYPE_MONITOR, 800, 602 },	/* AHS_MON debug */
+
+	{ 0x02, M5MOLS_RESTYPE_CAPTURE, 320, 240 },	/* QVGA */
+	{ 0x04, M5MOLS_RESTYPE_CAPTURE, 400, 240 },	/* WQVGA */
+	{ 0x07, M5MOLS_RESTYPE_CAPTURE, 480, 360 },
+	{ 0x08, M5MOLS_RESTYPE_CAPTURE, 640, 360 },	/* qHD */
+	{ 0x09, M5MOLS_RESTYPE_CAPTURE, 640, 480 },	/* VGA */
+	{ 0x0a, M5MOLS_RESTYPE_CAPTURE, 800, 480 },	/* WVGA */
+	{ 0x10, M5MOLS_RESTYPE_CAPTURE, 1280, 720 },	/* HD */
+	{ 0x14, M5MOLS_RESTYPE_CAPTURE, 1280, 960 },	/* 1M */
+	{ 0x17, M5MOLS_RESTYPE_CAPTURE, 1600, 1200 },	/* 2M */
+	{ 0x19, M5MOLS_RESTYPE_CAPTURE, 1920, 1080 },	/* Full-HD */
+	{ 0x1a, M5MOLS_RESTYPE_CAPTURE, 2048, 1152 },	/* 3Mega */
+	{ 0x1b, M5MOLS_RESTYPE_CAPTURE, 2048, 1536 },
+	{ 0x1c, M5MOLS_RESTYPE_CAPTURE, 2560, 1440 },	/* 4Mega */
+	{ 0x1d, M5MOLS_RESTYPE_CAPTURE, 2560, 1536 },
+	{ 0x1f, M5MOLS_RESTYPE_CAPTURE, 2560, 1920 },	/* 5Mega */
+	{ 0x21, M5MOLS_RESTYPE_CAPTURE, 3264, 1836 },	/* 6Mega */
+	{ 0x22, M5MOLS_RESTYPE_CAPTURE, 3264, 1960 },
+	{ 0x25, M5MOLS_RESTYPE_CAPTURE, 3264, 2448 },	/* 8Mega */
+};
+
+/**
+ * m5mols_swap_byte - an byte array to integer conversion function
+ * @size: size in bytes of I2C packet defined in the M-5MOLS datasheet
+ *
+ * Convert I2C data byte array with performing any required byte
+ * reordering to assure proper values for each data type, regardless
+ * of the architecture endianness.
+ */
+static u32 m5mols_swap_byte(u8 *data, u8 length)
+{
+	if (length == 1)
+		return *data;
+	else if (length == 2)
+		return be16_to_cpu(*((u16 *)data));
+	else
+		return be32_to_cpu(*((u32 *)data));
+}
+
+/**
+ * m5mols_read -  I2C read function
+ * @reg: combination of size, category and command for the I2C packet
+ * @size: desired size of I2C packet
+ * @val: read value
+ *
+ * Returns 0 on success, or else negative errno.
+ */
+static int m5mols_read(struct v4l2_subdev *sd, u32 size, u32 reg, u32 *val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct m5mols_info *info = to_m5mols(sd);
+	u8 rbuf[M5MOLS_I2C_MAX_SIZE + 1];
+	u8 category = I2C_CATEGORY(reg);
+	u8 cmd = I2C_COMMAND(reg);
+	struct i2c_msg msg[2];
+	u8 wbuf[5];
+	int ret;
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	msg[0].addr = client->addr;
+	msg[0].flags = 0;
+	msg[0].len = 5;
+	msg[0].buf = wbuf;
+	wbuf[0] = 5;
+	wbuf[1] = M5MOLS_BYTE_READ;
+	wbuf[2] = category;
+	wbuf[3] = cmd;
+	wbuf[4] = size;
+
+	msg[1].addr = client->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = size + 1;
+	msg[1].buf = rbuf;
+
+	/* minimum stabilization time */
+	usleep_range(200, 200);
+
+	ret = i2c_transfer(client->adapter, msg, 2);
+
+	if (ret == 2) {
+		*val = m5mols_swap_byte(&rbuf[1], size);
+		return 0;
+	}
+
+	if (info->isp_ready)
+		v4l2_err(sd, "read failed: size:%d cat:%02x cmd:%02x. %d\n",
+			 size, category, cmd, ret);
+
+	return ret < 0 ? ret : -EIO;
+}
+
+int m5mols_read_u8(struct v4l2_subdev *sd, u32 reg, u8 *val)
+{
+	u32 val_32;
+	int ret;
+
+	if (I2C_SIZE(reg) != 1) {
+		v4l2_err(sd, "Wrong data size\n");
+		return -EINVAL;
+	}
+
+	ret = m5mols_read(sd, I2C_SIZE(reg), reg, &val_32);
+	if (ret)
+		return ret;
+
+	*val = (u8)val_32;
+	return ret;
+}
+
+int m5mols_read_u16(struct v4l2_subdev *sd, u32 reg, u16 *val)
+{
+	u32 val_32;
+	int ret;
+
+	if (I2C_SIZE(reg) != 2) {
+		v4l2_err(sd, "Wrong data size\n");
+		return -EINVAL;
+	}
+
+	ret = m5mols_read(sd, I2C_SIZE(reg), reg, &val_32);
+	if (ret)
+		return ret;
+
+	*val = (u16)val_32;
+	return ret;
+}
+
+int m5mols_read_u32(struct v4l2_subdev *sd, u32 reg, u32 *val)
+{
+	if (I2C_SIZE(reg) != 4) {
+		v4l2_err(sd, "Wrong data size\n");
+		return -EINVAL;
+	}
+
+	return m5mols_read(sd, I2C_SIZE(reg), reg, val);
+}
+
+/**
+ * m5mols_write - I2C command write function
+ * @reg: combination of size, category and command for the I2C packet
+ * @val: value to write
+ *
+ * Returns 0 on success, or else negative errno.
+ */
+int m5mols_write(struct v4l2_subdev *sd, u32 reg, u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct m5mols_info *info = to_m5mols(sd);
+	u8 wbuf[M5MOLS_I2C_MAX_SIZE + 4];
+	u8 category = I2C_CATEGORY(reg);
+	u8 cmd = I2C_COMMAND(reg);
+	u8 size	= I2C_SIZE(reg);
+	u32 *buf = (u32 *)&wbuf[4];
+	struct i2c_msg msg[1];
+	int ret;
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	if (size != 1 && size != 2 && size != 4) {
+		v4l2_err(sd, "Wrong data size\n");
+		return -EINVAL;
+	}
+
+	msg->addr = client->addr;
+	msg->flags = 0;
+	msg->len = (u16)size + 4;
+	msg->buf = wbuf;
+	wbuf[0] = size + 4;
+	wbuf[1] = M5MOLS_BYTE_WRITE;
+	wbuf[2] = category;
+	wbuf[3] = cmd;
+
+	*buf = m5mols_swap_byte((u8 *)&val, size);
+
+	usleep_range(200, 200);
+
+	ret = i2c_transfer(client->adapter, msg, 1);
+	if (ret == 1)
+		return 0;
+
+	if (info->isp_ready)
+		v4l2_err(sd, "write failed: cat:%02x cmd:%02x ret:%d\n",
+			 category, cmd, ret);
+
+	return ret < 0 ? ret : -EIO;
+}
+
+/**
+ * m5mols_busy_wait - Busy waiting with I2C register polling
+ * @reg: the I2C_REG() address of an 8-bit status register to check
+ * @value: expected status register value
+ * @mask: bit mask for the read status register value
+ * @timeout: timeout in miliseconds, or -1 for default timeout
+ *
+ * The @reg register value is ORed with @mask before comparing with @value.
+ *
+ * Return: 0 if the requested condition became true within less than
+ *         @timeout ms, or else negative errno.
+ */
+int m5mols_busy_wait(struct v4l2_subdev *sd, u32 reg, u32 value, u32 mask,
+		     int timeout)
+{
+	int ms = timeout < 0 ? M5MOLS_BUSY_WAIT_DEF_TIMEOUT : timeout;
+	unsigned long end = jiffies + msecs_to_jiffies(ms);
+	u8 status;
+
+	do {
+		int ret = m5mols_read_u8(sd, reg, &status);
+
+		if (ret < 0 && !(mask & M5MOLS_I2C_RDY_WAIT_FL))
+			return ret;
+		if (!ret && (status & mask & 0xff) == (value & 0xff))
+			return 0;
+		usleep_range(100, 250);
+	} while (ms > 0 && time_is_after_jiffies(end));
+
+	return -EBUSY;
+}
+
+/**
+ * m5mols_enable_interrupt - Clear interrupt pending bits and unmask interrupts
+ *
+ * Before writing desired interrupt value the INT_FACTOR register should
+ * be read to clear pending interrupts.
+ */
+int m5mols_enable_interrupt(struct v4l2_subdev *sd, u8 reg)
+{
+	struct m5mols_info *info = to_m5mols(sd);
+	u8 mask = is_available_af(info) ? REG_INT_AF : 0;
+	u8 dummy;
+	int ret;
+
+	ret = m5mols_read_u8(sd, SYSTEM_INT_FACTOR, &dummy);
+	if (!ret)
+		ret = m5mols_write(sd, SYSTEM_INT_ENABLE, reg & ~mask);
+	return ret;
+}
+
+int m5mols_wait_interrupt(struct v4l2_subdev *sd, u8 irq_mask, u32 timeout)
+{
+	struct m5mols_info *info = to_m5mols(sd);
+
+	int ret = wait_event_interruptible_timeout(info->irq_waitq,
+				atomic_add_unless(&info->irq_done, -1, 0),
+				msecs_to_jiffies(timeout));
+	if (ret <= 0)
+		return ret ? ret : -ETIMEDOUT;
+
+	return m5mols_busy_wait(sd, SYSTEM_INT_FACTOR, irq_mask,
+				M5MOLS_I2C_RDY_WAIT_FL | irq_mask, -1);
+}
+
+/**
+ * m5mols_reg_mode - Write the mode and check busy status
+ *
+ * It always accompanies a little delay changing the M-5MOLS mode, so it is
+ * needed checking current busy status to guarantee right mode.
+ */
+static int m5mols_reg_mode(struct v4l2_subdev *sd, u8 mode)
+{
+	int ret = m5mols_write(sd, SYSTEM_SYSMODE, mode);
+	if (ret < 0)
+		return ret;
+	return m5mols_busy_wait(sd, SYSTEM_SYSMODE, mode, 0xff,
+				M5MOLS_MODE_CHANGE_TIMEOUT);
+}
+
+/**
+ * m5mols_set_mode - set the M-5MOLS controller mode
+ * @mode: the required operation mode
+ *
+ * The commands of M-5MOLS are grouped into specific modes. Each functionality
+ * can be guaranteed only when the sensor is operating in mode which a command
+ * belongs to.
+ */
+int m5mols_set_mode(struct m5mols_info *info, u8 mode)
+{
+	struct v4l2_subdev *sd = &info->sd;
+	int ret = -EINVAL;
+	u8 reg;
+
+	if (mode < REG_PARAMETER || mode > REG_CAPTURE)
+		return ret;
+
+	ret = m5mols_read_u8(sd, SYSTEM_SYSMODE, &reg);
+	if (ret || reg == mode)
+		return ret;
+
+	switch (reg) {
+	case REG_PARAMETER:
+		ret = m5mols_reg_mode(sd, REG_MONITOR);
+		if (mode == REG_MONITOR)
+			break;
+		if (!ret)
+			ret = m5mols_reg_mode(sd, REG_CAPTURE);
+		break;
+
+	case REG_MONITOR:
+		if (mode == REG_PARAMETER) {
+			ret = m5mols_reg_mode(sd, REG_PARAMETER);
+			break;
+		}
+
+		ret = m5mols_reg_mode(sd, REG_CAPTURE);
+		break;
+
+	case REG_CAPTURE:
+		ret = m5mols_reg_mode(sd, REG_MONITOR);
+		if (mode == REG_MONITOR)
+			break;
+		if (!ret)
+			ret = m5mols_reg_mode(sd, REG_PARAMETER);
+		break;
+
+	default:
+		v4l2_warn(sd, "Wrong mode: %d\n", mode);
+	}
+
+	if (!ret)
+		info->mode = mode;
+
+	return ret;
+}
+
+/**
+ * m5mols_get_version - retrieve full revisions information of M-5MOLS
+ *
+ * The version information includes revisions of hardware and firmware,
+ * AutoFocus alghorithm version and the version string.
+ */
+static int m5mols_get_version(struct v4l2_subdev *sd)
+{
+	struct m5mols_info *info = to_m5mols(sd);
+	struct m5mols_version *ver = &info->ver;
+	u8 *str = ver->str;
+	int i;
+	int ret;
+
+	ret = m5mols_read_u8(sd, SYSTEM_VER_CUSTOMER, &ver->customer);
+	if (!ret)
+		ret = m5mols_read_u8(sd, SYSTEM_VER_PROJECT, &ver->project);
+	if (!ret)
+		ret = m5mols_read_u16(sd, SYSTEM_VER_FIRMWARE, &ver->fw);
+	if (!ret)
+		ret = m5mols_read_u16(sd, SYSTEM_VER_HARDWARE, &ver->hw);
+	if (!ret)
+		ret = m5mols_read_u16(sd, SYSTEM_VER_PARAMETER, &ver->param);
+	if (!ret)
+		ret = m5mols_read_u16(sd, SYSTEM_VER_AWB, &ver->awb);
+	if (!ret)
+		ret = m5mols_read_u8(sd, AF_VERSION, &ver->af);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < VERSION_STRING_SIZE; i++) {
+		ret = m5mols_read_u8(sd, SYSTEM_VER_STRING, &str[i]);
+		if (ret)
+			return ret;
+	}
+
+	ver->fw = be16_to_cpu(ver->fw);
+	ver->hw = be16_to_cpu(ver->hw);
+	ver->param = be16_to_cpu(ver->param);
+	ver->awb = be16_to_cpu(ver->awb);
+
+	v4l2_info(sd, "Manufacturer\t[%s]\n",
+			is_manufacturer(info, REG_SAMSUNG_ELECTRO) ?
+			"Samsung Electro-Machanics" :
+			is_manufacturer(info, REG_SAMSUNG_OPTICS) ?
+			"Samsung Fiber-Optics" :
+			is_manufacturer(info, REG_SAMSUNG_TECHWIN) ?
+			"Samsung Techwin" : "None");
+	v4l2_info(sd, "Customer/Project\t[0x%02x/0x%02x]\n",
+			info->ver.customer, info->ver.project);
+
+	if (!is_available_af(info))
+		v4l2_info(sd, "No support Auto Focus on this firmware\n");
+
+	return ret;
+}
+
+/**
+ * __find_restype - Lookup M-5MOLS resolution type according to pixel code
+ * @code: pixel code
+ */
+static enum m5mols_restype __find_restype(enum v4l2_mbus_pixelcode code)
+{
+	enum m5mols_restype type = M5MOLS_RESTYPE_MONITOR;
+
+	do {
+		if (code == m5mols_default_ffmt[type].code)
+			return type;
+	} while (type++ != SIZE_DEFAULT_FFMT);
+
+	return 0;
+}
+
+/**
+ * __find_resolution - Lookup preset and type of M-5MOLS's resolution
+ * @mf: pixel format to find/negotiate the resolution preset for
+ * @type: M-5MOLS resolution type
+ * @resolution:	M-5MOLS resolution preset register value
+ *
+ * Find nearest resolution matching resolution preset and adjust mf
+ * to supported values.
+ */
+static int __find_resolution(struct v4l2_subdev *sd,
+			     struct v4l2_mbus_framefmt *mf,
+			     enum m5mols_restype *type,
+			     u32 *resolution)
+{
+	const struct m5mols_resolution *fsize = &m5mols_reg_res[0];
+	const struct m5mols_resolution *match = NULL;
+	enum m5mols_restype stype = __find_restype(mf->code);
+	int i = ARRAY_SIZE(m5mols_reg_res);
+	unsigned int min_err = ~0;
+
+	while (i--) {
+		int err;
+		if (stype == fsize->type) {
+			err = abs(fsize->width - mf->width)
+				+ abs(fsize->height - mf->height);
+
+			if (err < min_err) {
+				min_err = err;
+				match = fsize;
+			}
+		}
+		fsize++;
+	}
+	if (match) {
+		mf->width  = match->width;
+		mf->height = match->height;
+		*resolution = match->reg;
+		*type = stype;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static struct v4l2_mbus_framefmt *__find_format(struct m5mols_info *info,
+				struct v4l2_subdev_fh *fh,
+				enum v4l2_subdev_format_whence which,
+				enum m5mols_restype type)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return fh ? v4l2_subdev_get_try_format(fh, 0) : NULL;
+
+	return &info->ffmt[type];
+}
+
+static int m5mols_get_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct m5mols_info *info = to_m5mols(sd);
+	struct v4l2_mbus_framefmt *format;
+	int ret = 0;
+
+	mutex_lock(&info->lock);
+
+	format = __find_format(info, fh, fmt->which, info->res_type);
+	if (!format)
+		fmt->format = *format;
+	else
+		ret = -EINVAL;
+
+	mutex_unlock(&info->lock);
+	return ret;
+}
+
+static int m5mols_set_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct m5mols_info *info = to_m5mols(sd);
+	struct v4l2_mbus_framefmt *format = &fmt->format;
+	struct v4l2_mbus_framefmt *sfmt;
+	enum m5mols_restype type;
+	u32 resolution = 0;
+	int ret;
+
+	ret = __find_resolution(sd, format, &type, &resolution);
+	if (ret < 0)
+		return ret;
+
+	sfmt = __find_format(info, fh, fmt->which, type);
+	if (!sfmt)
+		return 0;
+
+	mutex_lock(&info->lock);
+
+	format->code = m5mols_default_ffmt[type].code;
+	format->colorspace = V4L2_COLORSPACE_JPEG;
+	format->field = V4L2_FIELD_NONE;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		*sfmt = *format;
+		info->resolution = resolution;
+		info->res_type = type;
+	}
+
+	mutex_unlock(&info->lock);
+	return ret;
+}
+
+static int m5mols_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_fh *fh,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (!code || code->index >= SIZE_DEFAULT_FFMT)
+		return -EINVAL;
+
+	code->code = m5mols_default_ffmt[code->index].code;
+
+	return 0;
+}
+
+static struct v4l2_subdev_pad_ops m5mols_pad_ops = {
+	.enum_mbus_code	= m5mols_enum_mbus_code,
+	.get_fmt	= m5mols_get_fmt,
+	.set_fmt	= m5mols_set_fmt,
+};
+
+/**
+ * m5mols_restore_controls - Apply current control values to the registers
+ *
+ * m5mols_do_scenemode() handles all parameters for which there is yet no
+ * individual control. It should be replaced at some point by setting each
+ * control individually, in required register set up order.
+ */
+int m5mols_restore_controls(struct m5mols_info *info)
+{
+	int ret;
+
+	if (info->ctrl_sync)
+		return 0;
+
+	ret = m5mols_do_scenemode(info, REG_SCENE_NORMAL);
+	if (ret)
+		return ret;
+
+	ret = v4l2_ctrl_handler_setup(&info->handle);
+	info->ctrl_sync = !ret;
+
+	return ret;
+}
+
+/**
+ * m5mols_start_monitor - Start the monitor mode
+ *
+ * Before applying the controls setup the resolution and frame rate
+ * in PARAMETER mode, and then switch over to MONITOR mode.
+ */
+static int m5mols_start_monitor(struct m5mols_info *info)
+{
+	struct v4l2_subdev *sd = &info->sd;
+	int ret;
+
+	ret = m5mols_set_mode(info, REG_PARAMETER);
+	if (!ret)
+		ret = m5mols_write(sd, PARM_MON_SIZE, info->resolution);
+	if (!ret)
+		ret = m5mols_write(sd, PARM_MON_FPS, REG_FPS_30);
+	if (!ret)
+		ret = m5mols_set_mode(info, REG_MONITOR);
+	if (!ret)
+		ret = m5mols_restore_controls(info);
+
+	return ret;
+}
+
+static int m5mols_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct m5mols_info *info = to_m5mols(sd);
+	u32 code;
+	int ret;
+
+	mutex_lock(&info->lock);
+	code = info->ffmt[info->res_type].code;
+
+	if (enable) {
+		if (is_code(code, M5MOLS_RESTYPE_MONITOR))
+			ret = m5mols_start_monitor(info);
+		if (is_code(code, M5MOLS_RESTYPE_CAPTURE))
+			ret = m5mols_start_capture(info);
+		else
+			ret = -EINVAL;
+	} else {
+		ret = m5mols_set_mode(info, REG_PARAMETER);
+	}
+
+	mutex_unlock(&info->lock);
+	return ret;
+}
+
+static const struct v4l2_subdev_video_ops m5mols_video_ops = {
+	.s_stream	= m5mols_s_stream,
+};
+
+static int m5mols_sensor_power(struct m5mols_info *info, bool enable)
+{
+	struct v4l2_subdev *sd = &info->sd;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	const struct m5mols_platform_data *pdata = info->pdata;
+	int ret;
+
+	if (info->power == enable)
+		return 0;
+
+	if (enable) {
+		if (info->set_power) {
+			ret = info->set_power(&client->dev, 1);
+			if (ret)
+				return ret;
+		}
+
+		ret = regulator_bulk_enable(ARRAY_SIZE(supplies), supplies);
+		if (ret) {
+			info->set_power(&client->dev, 0);
+			return ret;
+		}
+
+		gpio_set_value(pdata->gpio_reset, !pdata->reset_polarity);
+		info->power = 1;
+
+		return ret;
+	}
+
+	ret = regulator_bulk_disable(ARRAY_SIZE(supplies), supplies);
+	if (ret)
+		return ret;
+
+	if (info->set_power)
+		info->set_power(&client->dev, 0);
+
+	gpio_set_value(pdata->gpio_reset, pdata->reset_polarity);
+
+	info->isp_ready = 0;
+	info->power = 0;
+
+	return ret;
+}
+
+/* m5mols_update_fw - optional firmware update routine */
+int __attribute__ ((weak)) m5mols_update_fw(struct v4l2_subdev *sd,
+		int (*set_power)(struct m5mols_info *, bool))
+{
+	return 0;
+}
+
+/**
+ * m5mols_fw_start - M-5MOLS internal ARM controller initialization
+ *
+ * Execute the M-5MOLS internal ARM controller initialization sequence.
+ * This function should be called after the supply voltage has been
+ * applied and before any requests to the device are made.
+ */
+static int m5mols_fw_start(struct v4l2_subdev *sd)
+{
+	struct m5mols_info *info = to_m5mols(sd);
+	int ret;
+
+	atomic_set(&info->irq_done, 0);
+	/* Wait until I2C slave is initialized in Flash Writer mode */
+	ret = m5mols_busy_wait(sd, FLASH_CAM_START, REG_IN_FLASH_MODE,
+			       M5MOLS_I2C_RDY_WAIT_FL | 0xff, -1);
+	if (!ret)
+		ret = m5mols_write(sd, FLASH_CAM_START, REG_START_ARM_BOOT);
+	if (!ret)
+		ret = m5mols_wait_interrupt(sd, REG_INT_MODE, 2000);
+	if (ret < 0)
+		return ret;
+
+	info->isp_ready = 1;
+
+	ret = m5mols_get_version(sd);
+	if (!ret)
+		ret = m5mols_update_fw(sd, m5mols_sensor_power);
+	if (ret)
+		return ret;
+
+	v4l2_dbg(1, m5mols_debug, sd, "Success ARM Booting\n");
+
+	ret = m5mols_write(sd, PARM_INTERFACE, REG_INTERFACE_MIPI);
+	if (!ret)
+		ret = m5mols_enable_interrupt(sd,
+				REG_INT_AF | REG_INT_CAPTURE);
+
+	return ret;
+}
+
+/* Execute the lens soft-landing algorithm */
+static int m5mols_auto_focus_stop(struct m5mols_info *info)
+{
+	int ret;
+
+	ret = m5mols_write(&info->sd, AF_EXECUTE, REG_AF_STOP);
+	if (!ret)
+		ret = m5mols_write(&info->sd, AF_MODE, REG_AF_POWEROFF);
+	if (!ret)
+		ret = m5mols_busy_wait(&info->sd, SYSTEM_STATUS, REG_AF_IDLE,
+				       0xff, -1);
+	return ret;
+}
+
+/**
+ * m5mols_s_power - Main sensor power control function
+ *
+ * To prevent breaking the lens when the sensor is powered off the Soft-Landing
+ * algorithm is called where available. The Soft-Landing algorithm availability
+ * dependends on the firmware provider.
+ */
+static int m5mols_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct m5mols_info *info = to_m5mols(sd);
+	int ret;
+
+	mutex_lock(&info->lock);
+
+	if (on) {
+		ret = m5mols_sensor_power(info, true);
+		if (!ret)
+			ret = m5mols_fw_start(sd);
+	} else {
+		if (is_manufacturer(info, REG_SAMSUNG_TECHWIN)) {
+			ret = m5mols_set_mode(info, REG_MONITOR);
+			if (!ret)
+				ret = m5mols_auto_focus_stop(info);
+			if (ret < 0)
+				v4l2_warn(sd, "Soft landing lens failed\n");
+		}
+		ret = m5mols_sensor_power(info, false);
+
+		info->ctrl_sync = 0;
+	}
+
+	mutex_unlock(&info->lock);
+	return ret;
+}
+
+static int m5mols_log_status(struct v4l2_subdev *sd)
+{
+	struct m5mols_info *info = to_m5mols(sd);
+
+	v4l2_ctrl_handler_log_status(&info->handle, sd->name);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops m5mols_core_ops = {
+	.s_power	= m5mols_s_power,
+	.log_status	= m5mols_log_status,
+};
+
+/*
+ * V4L2 subdev internal operations
+ */
+static int m5mols_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_mbus_framefmt *format = v4l2_subdev_get_try_format(fh, 0);
+
+	*format = m5mols_default_ffmt[0];
+	return 0;
+}
+
+static const struct v4l2_subdev_internal_ops m5mols_subdev_internal_ops = {
+	.open		= m5mols_open,
+};
+
+static const struct v4l2_subdev_ops m5mols_ops = {
+	.core		= &m5mols_core_ops,
+	.pad		= &m5mols_pad_ops,
+	.video		= &m5mols_video_ops,
+};
+
+static irqreturn_t m5mols_irq_handler(int irq, void *data)
+{
+	struct m5mols_info *info = to_m5mols(data);
+
+	atomic_set(&info->irq_done, 1);
+	wake_up_interruptible(&info->irq_waitq);
+
+	return IRQ_HANDLED;
+}
+
+static int __devinit m5mols_probe(struct i2c_client *client,
+				  const struct i2c_device_id *id)
+{
+	const struct m5mols_platform_data *pdata = client->dev.platform_data;
+	struct m5mols_info *info;
+	struct v4l2_subdev *sd;
+	int ret;
+
+	if (pdata == NULL) {
+		dev_err(&client->dev, "No platform data\n");
+		return -EINVAL;
+	}
+
+	if (!gpio_is_valid(pdata->gpio_reset)) {
+		dev_err(&client->dev, "No valid RESET GPIO specified\n");
+		return -EINVAL;
+	}
+
+	if (!client->irq) {
+		dev_err(&client->dev, "Interrupt not assigned\n");
+		return -EINVAL;
+	}
+
+	info = kzalloc(sizeof(struct m5mols_info), GFP_KERNEL);
+	if (!info)
+		return -ENOMEM;
+
+	info->pdata = pdata;
+	info->set_power	= pdata->set_power;
+
+	ret = gpio_request(pdata->gpio_reset, "M5MOLS_NRST");
+	if (ret) {
+		dev_err(&client->dev, "Failed to request gpio: %d\n", ret);
+		goto out_free;
+	}
+	gpio_direction_output(pdata->gpio_reset, pdata->reset_polarity);
+
+	ret = regulator_bulk_get(&client->dev, ARRAY_SIZE(supplies), supplies);
+	if (ret) {
+		dev_err(&client->dev, "Failed to get regulators: %d\n", ret);
+		goto out_gpio;
+	}
+
+	sd = &info->sd;
+	v4l2_i2c_subdev_init(sd, client, &m5mols_ops);
+	strlcpy(sd->name, MODULE_NAME, sizeof(sd->name));
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	sd->internal_ops = &m5mols_subdev_internal_ops;
+	info->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_init(&sd->entity, 1, &info->pad, 0);
+	if (ret < 0)
+		goto out_reg;
+	sd->entity.type = MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
+
+	init_waitqueue_head(&info->irq_waitq);
+	mutex_init(&info->lock);
+
+	ret = request_irq(client->irq, m5mols_irq_handler,
+			  IRQF_TRIGGER_RISING, MODULE_NAME, sd);
+	if (ret) {
+		dev_err(&client->dev, "Interrupt request failed: %d\n", ret);
+		goto out_me;
+	}
+	info->res_type = M5MOLS_RESTYPE_MONITOR;
+	info->ffmt[0] = m5mols_default_ffmt[0];
+	info->ffmt[1] =	m5mols_default_ffmt[1];
+
+	ret = m5mols_sensor_power(info, true);
+	if (ret)
+		goto out_irq;
+
+	ret = m5mols_fw_start(sd);
+	if (!ret)
+		ret = m5mols_init_controls(sd);
+
+	ret = m5mols_sensor_power(info, false);
+	if (!ret)
+		return 0;
+out_irq:
+	free_irq(client->irq, sd);
+out_me:
+	media_entity_cleanup(&sd->entity);
+out_reg:
+	regulator_bulk_free(ARRAY_SIZE(supplies), supplies);
+out_gpio:
+	gpio_free(pdata->gpio_reset);
+out_free:
+	kfree(info);
+	return ret;
+}
+
+static int __devexit m5mols_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct m5mols_info *info = to_m5mols(sd);
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+	free_irq(client->irq, sd);
+
+	regulator_bulk_free(ARRAY_SIZE(supplies), supplies);
+	gpio_free(info->pdata->gpio_reset);
+	media_entity_cleanup(&sd->entity);
+	kfree(info);
+	return 0;
+}
+
+static const struct i2c_device_id m5mols_id[] = {
+	{ MODULE_NAME, 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(i2c, m5mols_id);
+
+static struct i2c_driver m5mols_i2c_driver = {
+	.driver = {
+		.name	= MODULE_NAME,
+	},
+	.probe		= m5mols_probe,
+	.remove		= __devexit_p(m5mols_remove),
+	.id_table	= m5mols_id,
+};
+
+module_i2c_driver(m5mols_i2c_driver);
+
+MODULE_AUTHOR("HeungJun Kim <riverful.kim@samsung.com>");
+MODULE_AUTHOR("Dongsoo Kim <dongsoo45.kim@samsung.com>");
+MODULE_DESCRIPTION("Fujitsu M-5MOLS 8M Pixel camera driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/m5mols/m5mols_reg.h b/drivers/media/i2c/m5mols/m5mols_reg.h
new file mode 100644
index 000000000000..14d4be72aeff
--- /dev/null
+++ b/drivers/media/i2c/m5mols/m5mols_reg.h
@@ -0,0 +1,362 @@
+/*
+ * Register map for M-5MOLS 8M Pixel camera sensor with ISP
+ *
+ * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ * Author: HeungJun Kim <riverful.kim@samsung.com>
+ *
+ * Copyright (C) 2009 Samsung Electronics Co., Ltd.
+ * Author: Dongsoo Nathaniel Kim <dongsoo45.kim@samsung.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; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#ifndef M5MOLS_REG_H
+#define M5MOLS_REG_H
+
+#define M5MOLS_I2C_MAX_SIZE	4
+#define M5MOLS_BYTE_READ	0x01
+#define M5MOLS_BYTE_WRITE	0x02
+
+#define I2C_CATEGORY(__cat)		((__cat >> 16) & 0xff)
+#define I2C_COMMAND(__comm)		((__comm >> 8) & 0xff)
+#define I2C_SIZE(__reg_s)		((__reg_s) & 0xff)
+#define I2C_REG(__cat, __cmd, __reg_s)	((__cat << 16) | (__cmd << 8) | __reg_s)
+
+/*
+ * Category section register
+ *
+ * The category means set including relevant command of M-5MOLS.
+ */
+#define CAT_SYSTEM		0x00
+#define CAT_PARAM		0x01
+#define CAT_MONITOR		0x02
+#define CAT_AE			0x03
+#define CAT_WB			0x06
+#define CAT_EXIF		0x07
+#define CAT_FD			0x09
+#define CAT_LENS		0x0a
+#define CAT_CAPT_PARM		0x0b
+#define CAT_CAPT_CTRL		0x0c
+#define CAT_FLASH		0x0f	/* related to FW, revisions, booting */
+
+/*
+ * Category 0 - SYSTEM mode
+ *
+ * The SYSTEM mode in the M-5MOLS means area available to handle with the whole
+ * & all-round system of sensor. It deals with version/interrupt/setting mode &
+ * even sensor's status. Especially, the M-5MOLS sensor with ISP varies by
+ * packaging & manufacturer, even the customer and project code. And the
+ * function details may vary among them. The version information helps to
+ * determine what methods shall be used in the driver.
+ *
+ * There is many registers between customer version address and awb one. For
+ * more specific contents, see definition if file m5mols.h.
+ */
+#define SYSTEM_VER_CUSTOMER	I2C_REG(CAT_SYSTEM, 0x00, 1)
+#define SYSTEM_VER_PROJECT	I2C_REG(CAT_SYSTEM, 0x01, 1)
+#define SYSTEM_VER_FIRMWARE	I2C_REG(CAT_SYSTEM, 0x02, 2)
+#define SYSTEM_VER_HARDWARE	I2C_REG(CAT_SYSTEM, 0x04, 2)
+#define SYSTEM_VER_PARAMETER	I2C_REG(CAT_SYSTEM, 0x06, 2)
+#define SYSTEM_VER_AWB		I2C_REG(CAT_SYSTEM, 0x08, 2)
+
+#define SYSTEM_SYSMODE		I2C_REG(CAT_SYSTEM, 0x0b, 1)
+#define REG_SYSINIT		0x00	/* SYSTEM mode */
+#define REG_PARAMETER		0x01	/* PARAMETER mode */
+#define REG_MONITOR		0x02	/* MONITOR mode */
+#define REG_CAPTURE		0x03	/* CAPTURE mode */
+
+#define SYSTEM_CMD(__cmd)	I2C_REG(CAT_SYSTEM, cmd, 1)
+#define SYSTEM_VER_STRING	I2C_REG(CAT_SYSTEM, 0x0a, 1)
+#define REG_SAMSUNG_ELECTRO	"SE"	/* Samsung Electro-Mechanics */
+#define REG_SAMSUNG_OPTICS	"OP"	/* Samsung Fiber-Optics */
+#define REG_SAMSUNG_TECHWIN	"TB"	/* Samsung Techwin */
+/* SYSTEM mode status */
+#define SYSTEM_STATUS	I2C_REG(CAT_SYSTEM, 0x0c, 1)
+
+/* Interrupt pending register */
+#define SYSTEM_INT_FACTOR	I2C_REG(CAT_SYSTEM, 0x10, 1)
+/* interrupt enable register */
+#define SYSTEM_INT_ENABLE	I2C_REG(CAT_SYSTEM, 0x11, 1)
+#define REG_INT_MODE		(1 << 0)
+#define REG_INT_AF		(1 << 1)
+#define REG_INT_ZOOM		(1 << 2)
+#define REG_INT_CAPTURE		(1 << 3)
+#define REG_INT_FRAMESYNC	(1 << 4)
+#define REG_INT_FD		(1 << 5)
+#define REG_INT_LENS_INIT	(1 << 6)
+#define REG_INT_SOUND		(1 << 7)
+#define REG_INT_MASK		0x0f
+
+/*
+ * category 1 - PARAMETER mode
+ *
+ * This category supports function of camera features of M-5MOLS. It means we
+ * can handle with preview(MONITOR) resolution size/frame per second/interface
+ * between the sensor and the Application Processor/even the image effect.
+ */
+
+/* Resolution in the MONITOR mode */
+#define PARM_MON_SIZE		I2C_REG(CAT_PARAM, 0x01, 1)
+
+/* Frame rate */
+#define PARM_MON_FPS		I2C_REG(CAT_PARAM, 0x02, 1)
+#define REG_FPS_30		0x02
+
+/* Video bus between the sensor and a host processor */
+#define PARM_INTERFACE		I2C_REG(CAT_PARAM, 0x00, 1)
+#define REG_INTERFACE_MIPI	0x02
+
+/* Image effects */
+#define PARM_EFFECT		I2C_REG(CAT_PARAM, 0x0b, 1)
+#define REG_EFFECT_OFF		0x00
+#define REG_EFFECT_NEGA		0x01
+#define REG_EFFECT_EMBOSS	0x06
+#define REG_EFFECT_OUTLINE	0x07
+#define REG_EFFECT_WATERCOLOR	0x08
+
+/*
+ * Category 2 - MONITOR mode
+ *
+ * The MONITOR mode is same as preview mode as we said. The M-5MOLS has another
+ * mode named "Preview", but this preview mode is used at the case specific
+ * vider-recording mode. This mmode supports only YUYV format. On the other
+ * hand, the JPEG & RAW formats is supports by CAPTURE mode. And, there are
+ * another options like zoom/color effect(different with effect in PARAMETER
+ * mode)/anti hand shaking algorithm.
+ */
+
+/* Target digital zoom position */
+#define MON_ZOOM		I2C_REG(CAT_MONITOR, 0x01, 1)
+
+/* CR value for color effect */
+#define MON_CFIXR		I2C_REG(CAT_MONITOR, 0x0a, 1)
+/* CB value for color effect */
+#define MON_CFIXB		I2C_REG(CAT_MONITOR, 0x09, 1)
+#define REG_CFIXB_SEPIA		0xd8
+#define REG_CFIXR_SEPIA		0x18
+
+#define MON_EFFECT		I2C_REG(CAT_MONITOR, 0x0b, 1)
+#define REG_COLOR_EFFECT_OFF	0x00
+#define REG_COLOR_EFFECT_ON	0x01
+
+/* Chroma enable */
+#define MON_CHROMA_EN		I2C_REG(CAT_MONITOR, 0x10, 1)
+/* Chroma level */
+#define MON_CHROMA_LVL		I2C_REG(CAT_MONITOR, 0x0f, 1)
+#define REG_CHROMA_OFF		0x00
+#define REG_CHROMA_ON		0x01
+
+/* Sharpness on/off */
+#define MON_EDGE_EN		I2C_REG(CAT_MONITOR, 0x12, 1)
+/* Sharpness level */
+#define MON_EDGE_LVL		I2C_REG(CAT_MONITOR, 0x11, 1)
+#define REG_EDGE_OFF		0x00
+#define REG_EDGE_ON		0x01
+
+/* Set color tone (contrast) */
+#define MON_TONE_CTL		I2C_REG(CAT_MONITOR, 0x25, 1)
+
+/*
+ * Category 3 - Auto Exposure
+ *
+ * The M-5MOLS exposure capbility is detailed as which is similar to digital
+ * camera. This category supports AE locking/various AE mode(range of exposure)
+ * /ISO/flickering/EV bias/shutter/meteoring, and anything else. And the
+ * maximum/minimum exposure gain value depending on M-5MOLS firmware, may be
+ * different. So, this category also provide getting the max/min values. And,
+ * each MONITOR and CAPTURE mode has each gain/shutter/max exposure values.
+ */
+
+/* Auto Exposure locking */
+#define AE_LOCK			I2C_REG(CAT_AE, 0x00, 1)
+#define REG_AE_UNLOCK		0x00
+#define REG_AE_LOCK		0x01
+
+/* Auto Exposure algorithm mode */
+#define AE_MODE			I2C_REG(CAT_AE, 0x01, 1)
+#define REG_AE_OFF		0x00	/* AE off */
+#define REG_AE_ALL		0x01	/* calc AE in all block integral */
+#define REG_AE_CENTER		0x03	/* calc AE in center weighted */
+#define REG_AE_SPOT		0x06	/* calc AE in specific spot */
+
+#define AE_ISO			I2C_REG(CAT_AE, 0x05, 1)
+#define REG_ISO_AUTO		0x00
+#define REG_ISO_50		0x01
+#define REG_ISO_100		0x02
+#define REG_ISO_200		0x03
+#define REG_ISO_400		0x04
+#define REG_ISO_800		0x05
+
+/* EV (scenemode) preset for MONITOR */
+#define AE_EV_PRESET_MONITOR	I2C_REG(CAT_AE, 0x0a, 1)
+/* EV (scenemode) preset for CAPTURE */
+#define AE_EV_PRESET_CAPTURE	I2C_REG(CAT_AE, 0x0b, 1)
+#define REG_SCENE_NORMAL	0x00
+#define REG_SCENE_PORTRAIT	0x01
+#define REG_SCENE_LANDSCAPE	0x02
+#define REG_SCENE_SPORTS	0x03
+#define REG_SCENE_PARTY_INDOOR	0x04
+#define REG_SCENE_BEACH_SNOW	0x05
+#define REG_SCENE_SUNSET	0x06
+#define REG_SCENE_DAWN_DUSK	0x07
+#define REG_SCENE_FALL		0x08
+#define REG_SCENE_NIGHT		0x09
+#define REG_SCENE_AGAINST_LIGHT	0x0a
+#define REG_SCENE_FIRE		0x0b
+#define REG_SCENE_TEXT		0x0c
+#define REG_SCENE_CANDLE	0x0d
+
+/* Manual gain in MONITOR mode */
+#define AE_MAN_GAIN_MON		I2C_REG(CAT_AE, 0x12, 2)
+/* Maximum gain in MONITOR mode */
+#define AE_MAX_GAIN_MON		I2C_REG(CAT_AE, 0x1a, 2)
+/* Manual gain in CAPTURE mode */
+#define AE_MAN_GAIN_CAP		I2C_REG(CAT_AE, 0x26, 2)
+
+#define AE_INDEX		I2C_REG(CAT_AE, 0x38, 1)
+#define REG_AE_INDEX_20_NEG	0x00
+#define REG_AE_INDEX_15_NEG	0x01
+#define REG_AE_INDEX_10_NEG	0x02
+#define REG_AE_INDEX_05_NEG	0x03
+#define REG_AE_INDEX_00		0x04
+#define REG_AE_INDEX_05_POS	0x05
+#define REG_AE_INDEX_10_POS	0x06
+#define REG_AE_INDEX_15_POS	0x07
+#define REG_AE_INDEX_20_POS	0x08
+
+/*
+ * Category 6 - White Balance
+ */
+
+/* Auto Whitebalance locking */
+#define AWB_LOCK		I2C_REG(CAT_WB, 0x00, 1)
+#define REG_AWB_UNLOCK		0x00
+#define REG_AWB_LOCK		0x01
+
+#define AWB_MODE		I2C_REG(CAT_WB, 0x02, 1)
+#define REG_AWB_AUTO		0x01	/* AWB off */
+#define REG_AWB_PRESET		0x02	/* AWB preset */
+
+/* Manual WB (preset) */
+#define AWB_MANUAL		I2C_REG(CAT_WB, 0x03, 1)
+#define REG_AWB_INCANDESCENT	0x01
+#define REG_AWB_FLUORESCENT_1	0x02
+#define REG_AWB_FLUORESCENT_2	0x03
+#define REG_AWB_DAYLIGHT	0x04
+#define REG_AWB_CLOUDY		0x05
+#define REG_AWB_SHADE		0x06
+#define REG_AWB_HORIZON		0x07
+#define REG_AWB_LEDLIGHT	0x09
+
+/*
+ * Category 7 - EXIF information
+ */
+#define EXIF_INFO_EXPTIME_NU	I2C_REG(CAT_EXIF, 0x00, 4)
+#define EXIF_INFO_EXPTIME_DE	I2C_REG(CAT_EXIF, 0x04, 4)
+#define EXIF_INFO_TV_NU		I2C_REG(CAT_EXIF, 0x08, 4)
+#define EXIF_INFO_TV_DE		I2C_REG(CAT_EXIF, 0x0c, 4)
+#define EXIF_INFO_AV_NU		I2C_REG(CAT_EXIF, 0x10, 4)
+#define EXIF_INFO_AV_DE		I2C_REG(CAT_EXIF, 0x14, 4)
+#define EXIF_INFO_BV_NU		I2C_REG(CAT_EXIF, 0x18, 4)
+#define EXIF_INFO_BV_DE		I2C_REG(CAT_EXIF, 0x1c, 4)
+#define EXIF_INFO_EBV_NU	I2C_REG(CAT_EXIF, 0x20, 4)
+#define EXIF_INFO_EBV_DE	I2C_REG(CAT_EXIF, 0x24, 4)
+#define EXIF_INFO_ISO		I2C_REG(CAT_EXIF, 0x28, 2)
+#define EXIF_INFO_FLASH		I2C_REG(CAT_EXIF, 0x2a, 2)
+#define EXIF_INFO_SDR		I2C_REG(CAT_EXIF, 0x2c, 2)
+#define EXIF_INFO_QVAL		I2C_REG(CAT_EXIF, 0x2e, 2)
+
+/*
+ * Category 9 - Face Detection
+ */
+#define FD_CTL			I2C_REG(CAT_FD, 0x00, 1)
+#define BIT_FD_EN		0
+#define BIT_FD_DRAW_FACE_FRAME	4
+#define BIT_FD_DRAW_SMILE_LVL	6
+#define REG_FD(shift)		(1 << shift)
+#define REG_FD_OFF		0x0
+
+/*
+ * Category A - Lens Parameter
+ */
+#define AF_MODE			I2C_REG(CAT_LENS, 0x01, 1)
+#define REG_AF_NORMAL		0x00	/* Normal AF, one time */
+#define REG_AF_MACRO		0x01	/* Macro AF, one time */
+#define REG_AF_POWEROFF		0x07
+
+#define AF_EXECUTE		I2C_REG(CAT_LENS, 0x02, 1)
+#define REG_AF_STOP		0x00
+#define REG_AF_EXE_AUTO		0x01
+#define REG_AF_EXE_CAF		0x02
+
+#define AF_STATUS		I2C_REG(CAT_LENS, 0x03, 1)
+#define REG_AF_FAIL		0x00
+#define REG_AF_SUCCESS		0x02
+#define REG_AF_IDLE		0x04
+#define REG_AF_BUSY		0x05
+
+#define AF_VERSION		I2C_REG(CAT_LENS, 0x0a, 1)
+
+/*
+ * Category B - CAPTURE Parameter
+ */
+#define CAPP_YUVOUT_MAIN	I2C_REG(CAT_CAPT_PARM, 0x00, 1)
+#define REG_YUV422		0x00
+#define REG_BAYER10		0x05
+#define REG_BAYER8		0x06
+#define REG_JPEG		0x10
+
+#define CAPP_MAIN_IMAGE_SIZE	I2C_REG(CAT_CAPT_PARM, 0x01, 1)
+#define CAPP_JPEG_RATIO		I2C_REG(CAT_CAPT_PARM, 0x17, 1)
+
+#define CAPP_MCC_MODE		I2C_REG(CAT_CAPT_PARM, 0x1d, 1)
+#define REG_MCC_OFF		0x00
+#define REG_MCC_NORMAL		0x01
+
+#define CAPP_WDR_EN		I2C_REG(CAT_CAPT_PARM, 0x2c, 1)
+#define REG_WDR_OFF		0x00
+#define REG_WDR_ON		0x01
+#define REG_WDR_AUTO		0x02
+
+#define CAPP_LIGHT_CTRL		I2C_REG(CAT_CAPT_PARM, 0x40, 1)
+#define REG_LIGHT_OFF		0x00
+#define REG_LIGHT_ON		0x01
+#define REG_LIGHT_AUTO		0x02
+
+#define CAPP_FLASH_CTRL		I2C_REG(CAT_CAPT_PARM, 0x41, 1)
+#define REG_FLASH_OFF		0x00
+#define REG_FLASH_ON		0x01
+#define REG_FLASH_AUTO		0x02
+
+/*
+ * Category C - CAPTURE Control
+ */
+#define CAPC_MODE		I2C_REG(CAT_CAPT_CTRL, 0x00, 1)
+#define REG_CAP_NONE		0x00
+#define REG_CAP_ANTI_SHAKE	0x02
+
+/* Select single- or multi-shot capture */
+#define CAPC_SEL_FRAME		I2C_REG(CAT_CAPT_CTRL, 0x06, 1)
+
+#define CAPC_START		I2C_REG(CAT_CAPT_CTRL, 0x09, 1)
+#define REG_CAP_START_MAIN	0x01
+#define REG_CAP_START_THUMB	0x03
+
+#define CAPC_IMAGE_SIZE		I2C_REG(CAT_CAPT_CTRL, 0x0d, 4)
+#define CAPC_THUMB_SIZE		I2C_REG(CAT_CAPT_CTRL, 0x11, 4)
+
+/*
+ * Category F - Flash
+ *
+ * This mode provides functions about internal flash stuff and system startup.
+ */
+
+/* Starts internal ARM core booting after power-up */
+#define FLASH_CAM_START		I2C_REG(CAT_FLASH, 0x12, 1)
+#define REG_START_ARM_BOOT	0x01	/* write value */
+#define REG_IN_FLASH_MODE	0x00	/* read value */
+
+#endif	/* M5MOLS_REG_H */
diff --git a/drivers/media/i2c/msp3400-driver.c b/drivers/media/i2c/msp3400-driver.c
new file mode 100644
index 000000000000..766305f69a28
--- /dev/null
+++ b/drivers/media/i2c/msp3400-driver.c
@@ -0,0 +1,927 @@
+/*
+ * 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 allways 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
+ *
+ * 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; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+
+#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/msp3400.h>
+#include <media/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
+		},
+	};
+
+	v4l_dbg(3, msp_debug, client, "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) {
+		v4l_err(client, "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;
+		v4l_warn(client, "I/O error #%d (read 0x%02x/0x%02x)\n", err,
+		       dev, addr);
+		schedule_timeout_interruptible(msecs_to_jiffies(10));
+	}
+	if (err == 3) {
+		v4l_warn(client, "resetting chip, sound will go off.\n");
+		msp_reset(client);
+		return -1;
+	}
+	retval = read[0] << 8 | read[1];
+	v4l_dbg(3, msp_debug, client, "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;
+
+	v4l_dbg(3, msp_debug, client, "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;
+		v4l_warn(client, "I/O error #%d (write 0x%02x/0x%02x)\n", err,
+		       dev, addr);
+		schedule_timeout_interruptible(msecs_to_jiffies(10));
+	}
+	if (err == 3) {
+		v4l_warn(client, "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 */
+
+	v4l_dbg(1, msp_debug, client, "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;
+
+		v4l_dbg(1, msp_debug, client, "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;
+	v4l_dbg(1, msp_debug, client, "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, 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;
+
+	v4l_dbg(2, msp_debug, client,
+		"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, 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);
+
+	v4l_dbg(1, msp_debug, client, "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_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ident *chip)
+{
+	struct msp_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return v4l2_chip_ident_i2c_client(client, chip, state->ident,
+			(state->rev1 << 16) | state->rev2);
+}
+
+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);
+	v4l_info(client, "%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) {
+		v4l_info(client, "Mode:     %s\n", p);
+	} else if (state->opmode == OPMODE_MANUAL) {
+		v4l_info(client, "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)
+			v4l_info(client, "Mode:     %s\n", p);
+		v4l_info(client, "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" : "");
+	}
+	v4l_info(client, "Audmode:  0x%04x\n", state->audmode);
+	v4l_info(client, "Routing:  0x%08x (input) 0x%08x (output)\n",
+			state->route_in, state->route_out);
+	v4l_info(client, "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);
+	v4l_dbg(1, msp_debug, client, "suspend\n");
+	msp_reset(client);
+	return 0;
+}
+
+static int msp_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	v4l_dbg(1, msp_debug, client, "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,
+	.g_chip_ident = msp_g_chip_ident,
+	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
+	.try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
+	.s_ext_ctrls = v4l2_subdev_s_ext_ctrls,
+	.g_ctrl = v4l2_subdev_g_ctrl,
+	.s_ctrl = v4l2_subdev_s_ctrl,
+	.queryctrl = v4l2_subdev_queryctrl,
+	.querymenu = v4l2_subdev_querymenu,
+	.s_std = msp_s_std,
+};
+
+static const struct v4l2_subdev_video_ops msp_video_ops = {
+	.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 int msp_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	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 (!id)
+		strlcpy(client->name, "msp3400", sizeof(client->name));
+
+	if (msp_reset(client) == -1) {
+		v4l_dbg(1, msp_debug, client, "msp3400 not found\n");
+		return -ENODEV;
+	}
+
+	state = kzalloc(sizeof(*state), GFP_KERNEL);
+	if (!state)
+		return -ENOMEM;
+
+	sd = &state->sd;
+	v4l2_i2c_subdev_init(sd, client, &msp_ops);
+
+	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);
+	v4l_dbg(1, msp_debug, client, "rev1=0x%04x, rev2=0x%04x\n",
+			state->rev1, state->rev2);
+	if (state->rev1 == -1 || (state->rev1 == 0 && state->rev2 == 0)) {
+		v4l_dbg(1, msp_debug, client,
+				"not an msp3400 (cannot read chip version)\n");
+		kfree(state);
+		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_AUTO) {
+		/* 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);
+		kfree(state);
+		return err;
+	}
+
+	v4l2_ctrl_cluster(2, &state->volume);
+	v4l2_ctrl_handler_setup(hdl);
+
+	/* hello world :-) */
+	v4l_info(client, "MSP%d4%02d%c-%c%d found @ 0x%x (%s)\n",
+			msp_family, msp_product,
+			msp_revision, msp_hard, msp_rom,
+			client->addr << 1, client->adapter->name);
+	v4l_info(client, "%s ", client->name);
+	if (state->has_nicam && state->has_radio)
+		printk(KERN_CONT "supports nicam and radio, ");
+	else if (state->has_nicam)
+		printk(KERN_CONT "supports nicam, ");
+	else if (state->has_radio)
+		printk(KERN_CONT "supports radio, ");
+	printk(KERN_CONT "mode is ");
+
+	/* version-specific initialization */
+	switch (state->opmode) {
+	case OPMODE_MANUAL:
+		printk(KERN_CONT "manual");
+		thread_func = msp3400c_thread;
+		break;
+	case OPMODE_AUTODETECT:
+		printk(KERN_CONT "autodetect");
+		thread_func = msp3410d_thread;
+		break;
+	case OPMODE_AUTOSELECT:
+		printk(KERN_CONT "autodetect and autoselect");
+		thread_func = msp34xxg_thread;
+		break;
+	}
+	printk(KERN_CONT "\n");
+
+	/* startup control thread if needed */
+	if (thread_func) {
+		state->kthread = kthread_run(thread_func, client, "msp34xx");
+
+		if (IS_ERR(state->kthread))
+			v4l_warn(client, "kernel_thread() failed\n");
+		msp_wake_thread(client);
+	}
+	return 0;
+}
+
+static int 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);
+	kfree(state);
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+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 = {
+		.owner	= THIS_MODULE,
+		.name	= "msp3400",
+		.pm	= &msp3400_pm_ops,
+	},
+	.probe		= msp_probe,
+	.remove		= msp_remove,
+	.id_table	= msp_id,
+};
+
+module_i2c_driver(msp_driver);
+
+/*
+ * Overrides for Emacs so that we follow Linus's tabbing style.
+ * ---------------------------------------------------------------------------
+ * Local variables:
+ * c-basic-offset: 8
+ * End:
+ */
diff --git a/drivers/media/i2c/msp3400-driver.h b/drivers/media/i2c/msp3400-driver.h
new file mode 100644
index 000000000000..fbe5e0715f93
--- /dev/null
+++ b/drivers/media/i2c/msp3400-driver.h
@@ -0,0 +1,137 @@
+/*
+ */
+
+#ifndef MSP3400_DRIVER_H
+#define MSP3400_DRIVER_H
+
+#include <media/msp3400.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.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;
+
+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;
+};
+
+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 000000000000..f8b51714f2f9
--- /dev/null
+++ b/drivers/media/i2c/msp3400-kthreads.c
@@ -0,0 +1,1165 @@
+/*
+ * Programming the mspx4xx sound processor family
+ *
+ * (c) 1997-2001 Gerd Knorr <kraxel@bytesex.org>
+ *
+ * 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; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+
+#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/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;
+
+	v4l_dbg(1, msp_debug, client, "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
+		 */
+		v4l_dbg(1, msp_debug, client,
+			"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:
+		v4l_dbg(1, msp_debug, client, "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:
+		v4l_dbg(1, msp_debug, client, "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:
+		v4l_dbg(1, msp_debug, client,
+			"NICAM set_audmode: %s\n", modestr);
+		if (state->nicam_on)
+			src = 0x0100;  /* NICAM */
+		break;
+	case MSP_MODE_BTSC:
+		v4l_dbg(1, msp_debug, client,
+			"BTSC set_audmode: %s\n", modestr);
+		break;
+	case MSP_MODE_EXTERN:
+		v4l_dbg(1, msp_debug, client,
+			"extern set_audmode: %s\n", modestr);
+		src = 0x0200;  /* SCART */
+		break;
+	case MSP_MODE_FM_RADIO:
+		v4l_dbg(1, msp_debug, client,
+			"FM-Radio set_audmode: %s\n", modestr);
+		break;
+	default:
+		v4l_dbg(1, msp_debug, client, "mono set_audmode\n");
+		return;
+	}
+
+	/* switch audio */
+	v4l_dbg(1, msp_debug, client, "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) {
+			v4l_dbg(1, msp_debug, client, "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;
+	}
+	v4l_dbg(1, msp_debug, client,
+		"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)
+		v4l_dbg(1, msp_debug, client,
+			"mono sound carrier: %d.%03d MHz\n",
+			state->main / 910000, (state->main / 910) % 1000);
+	else
+		v4l_dbg(1, msp_debug, client,
+			"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)
+		v4l_dbg(1, msp_debug, client,
+			"NICAM/FM carrier  : %d.%03d MHz\n",
+			state->second / 910000, (state->second/910) % 1000);
+	if (state->mode == MSP_MODE_AM_NICAM)
+		v4l_dbg(1, msp_debug, client,
+			"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) {
+		v4l_dbg(1, msp_debug, client,
+			"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;
+		v4l_dbg(2, msp_debug, client,
+			"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);
+		v4l_dbg(2, msp_debug, client, "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;
+		v4l_dbg(1, msp_debug, client,
+			"watch: rxsubchans %02x => %02x\n",
+			state->rxsubchans, rxsubchans);
+		state->rxsubchans = rxsubchans;
+	}
+	if (newnicam != state->nicam_on) {
+		update = 1;
+		v4l_dbg(1, msp_debug, client, "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;
+
+	v4l_dbg(1, msp_debug, client, "msp3400 daemon started\n");
+	state->detected_std = V4L2_STD_ALL;
+	set_freezable();
+	for (;;) {
+		v4l_dbg(2, msp_debug, client, "msp3400 thread: sleep\n");
+		msp_sleep(state, -1);
+		v4l_dbg(2, msp_debug, client, "msp3400 thread: wakeup\n");
+
+restart:
+		v4l_dbg(2, msp_debug, client, "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 */
+			v4l_dbg(1, msp_debug, client,
+				"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;
+			v4l_dbg(1, msp_debug, client, "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;
+			v4l_dbg(1, msp_debug, client,
+				"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;
+			v4l_dbg(1, msp_debug, client,
+				"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;
+		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);
+		}
+	}
+	v4l_dbg(1, msp_debug, client, "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;
+
+	v4l_dbg(1, msp_debug, client, "msp3410 daemon started\n");
+	state->detected_std = V4L2_STD_ALL;
+	set_freezable();
+	for (;;) {
+		v4l_dbg(2, msp_debug, client, "msp3410 thread: sleep\n");
+		msp_sleep(state, -1);
+		v4l_dbg(2, msp_debug, client, "msp3410 thread: wakeup\n");
+
+restart:
+		v4l_dbg(2, msp_debug, client, "thread: restart scan\n");
+		state->restart = 0;
+		if (kthread_should_stop())
+			break;
+
+		if (state->mode == MSP_MODE_EXTERN) {
+			/* no carrier scan needed, just unmute */
+			v4l_dbg(1, msp_debug, client,
+				"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)
+			v4l_dbg(2, msp_debug, client,
+				"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;
+				v4l_dbg(2, msp_debug, client,
+					"detection still in progress\n");
+			}
+		}
+		for (i = 0; msp_stdlist[i].name != NULL; i++)
+			if (msp_stdlist[i].retval == val)
+				break;
+		v4l_dbg(1, msp_debug, client, "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 */
+			v4l_dbg(1, msp_debug, client, "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);
+		}
+	}
+	v4l_dbg(1, msp_debug, client, "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) {
+		v4l_dbg(1, msp_debug, client, "selected radio modus\n");
+		return 0x0001;
+	}
+	if (state->v4l2_std == V4L2_STD_NTSC_M_JP) {
+		v4l_dbg(1, msp_debug, client, "selected M (EIA-J) modus\n");
+		return 0x4001;
+	}
+	if (state->v4l2_std == V4L2_STD_NTSC_M_KR) {
+		v4l_dbg(1, msp_debug, client, "selected M (A2) modus\n");
+		return 0x0001;
+	}
+	if (state->v4l2_std == V4L2_STD_SECAM_L) {
+		v4l_dbg(1, msp_debug, client, "selected SECAM-L modus\n");
+		return 0x6001;
+	}
+	if (state->v4l2_std & V4L2_STD_MN) {
+		v4l_dbg(1, msp_debug, client, "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;
+
+	v4l_dbg(1, msp_debug, client,
+		"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;
+
+	v4l_dbg(1, msp_debug, client, "msp34xxg daemon started\n");
+	state->detected_std = V4L2_STD_ALL;
+	set_freezable();
+	for (;;) {
+		v4l_dbg(2, msp_debug, client, "msp34xxg thread: sleep\n");
+		msp_sleep(state, -1);
+		v4l_dbg(2, msp_debug, client, "msp34xxg thread: wakeup\n");
+
+restart:
+		v4l_dbg(1, msp_debug, client, "thread: restart scan\n");
+		state->restart = 0;
+		if (kthread_should_stop())
+			break;
+
+		if (state->mode == MSP_MODE_EXTERN) {
+			/* no carrier scan needed, just unmute */
+			v4l_dbg(1, msp_debug, client,
+				"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 */
+		v4l_dbg(1, msp_debug, client,
+			"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;
+			}
+			v4l_dbg(2, msp_debug, client,
+				"detection still in progress\n");
+		}
+		if (state->std == 1) {
+			v4l_dbg(1, msp_debug, client,
+				"detection still in progress after 10 tries. giving up.\n");
+			continue;
+		}
+
+unmute:
+		v4l_dbg(1, msp_debug, client,
+			"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;
+		}
+	}
+	v4l_dbg(1, msp_debug, client, "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;
+	}
+	v4l_dbg(1, msp_debug, client,
+		"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/mt9m032.c b/drivers/media/i2c/mt9m032.c
new file mode 100644
index 000000000000..f80c1d7ec884
--- /dev/null
+++ b/drivers/media/i2c/mt9m032.c
@@ -0,0 +1,878 @@
+/*
+ * Driver for MT9M032 CMOS Image Sensor from Micron
+ *
+ * Copyright (C) 2010-2011 Lund Engineering
+ * Contact: Gil Lund <gwlund@lundeng.com>
+ * Author: Martin Hostettler <martin@neutronstar.dyndns.org>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ */
+
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/math64.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/slab.h>
+#include <linux/v4l2-mediabus.h>
+
+#include <media/media-entity.h>
+#include <media/mt9m032.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+
+#include "aptina-pll.h"
+
+/*
+ * width and height include active boundary and black parts
+ *
+ * column    0-  15 active boundary
+ * column   16-1455 image
+ * column 1456-1471 active boundary
+ * column 1472-1599 black
+ *
+ * row       0-  51 black
+ * row      53-  59 active boundary
+ * row      60-1139 image
+ * row    1140-1147 active boundary
+ * row    1148-1151 black
+ */
+
+#define MT9M032_PIXEL_ARRAY_WIDTH			1600
+#define MT9M032_PIXEL_ARRAY_HEIGHT			1152
+
+#define MT9M032_CHIP_VERSION				0x00
+#define		MT9M032_CHIP_VERSION_VALUE		0x1402
+#define MT9M032_ROW_START				0x01
+#define		MT9M032_ROW_START_MIN			0
+#define		MT9M032_ROW_START_MAX			1152
+#define		MT9M032_ROW_START_DEF			60
+#define MT9M032_COLUMN_START				0x02
+#define		MT9M032_COLUMN_START_MIN		0
+#define		MT9M032_COLUMN_START_MAX		1600
+#define		MT9M032_COLUMN_START_DEF		16
+#define MT9M032_ROW_SIZE				0x03
+#define		MT9M032_ROW_SIZE_MIN			32
+#define		MT9M032_ROW_SIZE_MAX			1152
+#define		MT9M032_ROW_SIZE_DEF			1080
+#define MT9M032_COLUMN_SIZE				0x04
+#define		MT9M032_COLUMN_SIZE_MIN			32
+#define		MT9M032_COLUMN_SIZE_MAX			1600
+#define		MT9M032_COLUMN_SIZE_DEF			1440
+#define MT9M032_HBLANK					0x05
+#define MT9M032_VBLANK					0x06
+#define		MT9M032_VBLANK_MAX			0x7ff
+#define MT9M032_SHUTTER_WIDTH_HIGH			0x08
+#define MT9M032_SHUTTER_WIDTH_LOW			0x09
+#define		MT9M032_SHUTTER_WIDTH_MIN		1
+#define		MT9M032_SHUTTER_WIDTH_MAX		1048575
+#define		MT9M032_SHUTTER_WIDTH_DEF		1943
+#define MT9M032_PIX_CLK_CTRL				0x0a
+#define		MT9M032_PIX_CLK_CTRL_INV_PIXCLK		0x8000
+#define MT9M032_RESTART					0x0b
+#define MT9M032_RESET					0x0d
+#define MT9M032_PLL_CONFIG1				0x11
+#define		MT9M032_PLL_CONFIG1_OUTDIV_MASK		0x3f
+#define		MT9M032_PLL_CONFIG1_MUL_SHIFT		8
+#define MT9M032_READ_MODE1				0x1e
+#define MT9M032_READ_MODE2				0x20
+#define		MT9M032_READ_MODE2_VFLIP_SHIFT		15
+#define		MT9M032_READ_MODE2_HFLIP_SHIFT		14
+#define		MT9M032_READ_MODE2_ROW_BLC		0x40
+#define MT9M032_GAIN_GREEN1				0x2b
+#define MT9M032_GAIN_BLUE				0x2c
+#define MT9M032_GAIN_RED				0x2d
+#define MT9M032_GAIN_GREEN2				0x2e
+
+/* write only */
+#define MT9M032_GAIN_ALL				0x35
+#define		MT9M032_GAIN_DIGITAL_MASK		0x7f
+#define		MT9M032_GAIN_DIGITAL_SHIFT		8
+#define		MT9M032_GAIN_AMUL_SHIFT			6
+#define		MT9M032_GAIN_ANALOG_MASK		0x3f
+#define MT9M032_FORMATTER1				0x9e
+#define MT9M032_FORMATTER2				0x9f
+#define		MT9M032_FORMATTER2_DOUT_EN		0x1000
+#define		MT9M032_FORMATTER2_PIXCLK_EN		0x2000
+
+/*
+ * The available MT9M032 datasheet is missing documentation for register 0x10
+ * MT9P031 seems to be close enough, so use constants from that datasheet for
+ * now.
+ * But keep the name MT9P031 to remind us, that this isn't really confirmed
+ * for this sensor.
+ */
+#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_CONFIG2				0x11
+#define		MT9P031_PLL_CONFIG2_P1_DIV_MASK		0x1f
+
+struct mt9m032 {
+	struct v4l2_subdev subdev;
+	struct media_pad pad;
+	struct mt9m032_platform_data *pdata;
+
+	unsigned int pix_clock;
+
+	struct v4l2_ctrl_handler ctrls;
+	struct {
+		struct v4l2_ctrl *hflip;
+		struct v4l2_ctrl *vflip;
+	};
+
+	struct mutex lock; /* Protects streaming, format, interval and crop */
+
+	bool streaming;
+
+	struct v4l2_mbus_framefmt format;
+	struct v4l2_rect crop;
+	struct v4l2_fract frame_interval;
+};
+
+#define to_mt9m032(sd)	container_of(sd, struct mt9m032, subdev)
+#define to_dev(sensor) \
+	(&((struct i2c_client *)v4l2_get_subdevdata(&(sensor)->subdev))->dev)
+
+static int mt9m032_read(struct i2c_client *client, u8 reg)
+{
+	return i2c_smbus_read_word_swapped(client, reg);
+}
+
+static int mt9m032_write(struct i2c_client *client, u8 reg, const u16 data)
+{
+	return i2c_smbus_write_word_swapped(client, reg, data);
+}
+
+static u32 mt9m032_row_time(struct mt9m032 *sensor, unsigned int width)
+{
+	unsigned int effective_width;
+	u32 ns;
+
+	effective_width = width + 716; /* empirical value */
+	ns = div_u64(1000000000ULL * effective_width, sensor->pix_clock);
+	dev_dbg(to_dev(sensor),	"MT9M032 line time: %u ns\n", ns);
+	return ns;
+}
+
+static int mt9m032_update_timing(struct mt9m032 *sensor,
+				 struct v4l2_fract *interval)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->subdev);
+	struct v4l2_rect *crop = &sensor->crop;
+	unsigned int min_vblank;
+	unsigned int vblank;
+	u32 row_time;
+
+	if (!interval)
+		interval = &sensor->frame_interval;
+
+	row_time = mt9m032_row_time(sensor, crop->width);
+
+	vblank = div_u64(1000000000ULL * interval->numerator,
+			 (u64)row_time * interval->denominator)
+	       - crop->height;
+
+	if (vblank > MT9M032_VBLANK_MAX) {
+		/* hardware limits to 11 bit values */
+		interval->denominator = 1000;
+		interval->numerator =
+			div_u64((crop->height + MT9M032_VBLANK_MAX) *
+				(u64)row_time * interval->denominator,
+				1000000000ULL);
+		vblank = div_u64(1000000000ULL * interval->numerator,
+				 (u64)row_time * interval->denominator)
+		       - crop->height;
+	}
+	/* enforce minimal 1.6ms blanking time. */
+	min_vblank = 1600000 / row_time;
+	vblank = clamp_t(unsigned int, vblank, min_vblank, MT9M032_VBLANK_MAX);
+
+	return mt9m032_write(client, MT9M032_VBLANK, vblank);
+}
+
+static int mt9m032_update_geom_timing(struct mt9m032 *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->subdev);
+	int ret;
+
+	ret = mt9m032_write(client, MT9M032_COLUMN_SIZE,
+			    sensor->crop.width - 1);
+	if (!ret)
+		ret = mt9m032_write(client, MT9M032_ROW_SIZE,
+				    sensor->crop.height - 1);
+	if (!ret)
+		ret = mt9m032_write(client, MT9M032_COLUMN_START,
+				    sensor->crop.left);
+	if (!ret)
+		ret = mt9m032_write(client, MT9M032_ROW_START,
+				    sensor->crop.top);
+	if (!ret)
+		ret = mt9m032_update_timing(sensor, NULL);
+	return ret;
+}
+
+static int update_formatter2(struct mt9m032 *sensor, bool streaming)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->subdev);
+	u16 reg_val =   MT9M032_FORMATTER2_DOUT_EN
+		      | 0x0070;  /* parts reserved! */
+				 /* possibly for changing to 14-bit mode */
+
+	if (streaming)
+		reg_val |= MT9M032_FORMATTER2_PIXCLK_EN;   /* pixclock enable */
+
+	return mt9m032_write(client, MT9M032_FORMATTER2, reg_val);
+}
+
+static int mt9m032_setup_pll(struct mt9m032 *sensor)
+{
+	static const struct aptina_pll_limits limits = {
+		.ext_clock_min = 8000000,
+		.ext_clock_max = 16500000,
+		.int_clock_min = 2000000,
+		.int_clock_max = 24000000,
+		.out_clock_min = 322000000,
+		.out_clock_max = 693000000,
+		.pix_clock_max = 99000000,
+		.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(&sensor->subdev);
+	struct mt9m032_platform_data *pdata = sensor->pdata;
+	struct aptina_pll pll;
+	int ret;
+
+	pll.ext_clock = pdata->ext_clock;
+	pll.pix_clock = pdata->pix_clock;
+
+	ret = aptina_pll_calculate(&client->dev, &limits, &pll);
+	if (ret < 0)
+		return ret;
+
+	sensor->pix_clock = pdata->pix_clock;
+
+	ret = mt9m032_write(client, MT9M032_PLL_CONFIG1,
+			    (pll.m << MT9M032_PLL_CONFIG1_MUL_SHIFT)
+			    | (pll.p1 - 1));
+	if (!ret)
+		ret = mt9m032_write(client, MT9P031_PLL_CONFIG2, pll.n - 1);
+	if (!ret)
+		ret = mt9m032_write(client, MT9P031_PLL_CONTROL,
+				    MT9P031_PLL_CONTROL_PWRON |
+				    MT9P031_PLL_CONTROL_USEPLL);
+	if (!ret)		/* more reserved, Continuous, Master Mode */
+		ret = mt9m032_write(client, MT9M032_READ_MODE1, 0x8006);
+	if (!ret)		/* Set 14-bit mode, select 7 divider */
+		ret = mt9m032_write(client, MT9M032_FORMATTER1, 0x111e);
+
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * Subdev pad operations
+ */
+
+static int mt9m032_enum_mbus_code(struct v4l2_subdev *subdev,
+				  struct v4l2_subdev_fh *fh,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index != 0)
+		return -EINVAL;
+
+	code->code = V4L2_MBUS_FMT_Y8_1X8;
+	return 0;
+}
+
+static int mt9m032_enum_frame_size(struct v4l2_subdev *subdev,
+				   struct v4l2_subdev_fh *fh,
+				   struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->index != 0 || fse->code != V4L2_MBUS_FMT_Y8_1X8)
+		return -EINVAL;
+
+	fse->min_width = MT9M032_COLUMN_SIZE_DEF;
+	fse->max_width = MT9M032_COLUMN_SIZE_DEF;
+	fse->min_height = MT9M032_ROW_SIZE_DEF;
+	fse->max_height = MT9M032_ROW_SIZE_DEF;
+
+	return 0;
+}
+
+/**
+ * __mt9m032_get_pad_crop() - get crop rect
+ * @sensor: pointer to the sensor struct
+ * @fh: file handle for getting the try crop rect from
+ * @which: select try or active crop rect
+ *
+ * Returns a pointer the current active or fh relative try crop rect
+ */
+static struct v4l2_rect *
+__mt9m032_get_pad_crop(struct mt9m032 *sensor, struct v4l2_subdev_fh *fh,
+		       enum v4l2_subdev_format_whence which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_crop(fh, 0);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return &sensor->crop;
+	default:
+		return NULL;
+	}
+}
+
+/**
+ * __mt9m032_get_pad_format() - get format
+ * @sensor: pointer to the sensor struct
+ * @fh: file handle for getting the try format from
+ * @which: select try or active format
+ *
+ * Returns a pointer the current active or fh relative try format
+ */
+static struct v4l2_mbus_framefmt *
+__mt9m032_get_pad_format(struct mt9m032 *sensor, struct v4l2_subdev_fh *fh,
+			 enum v4l2_subdev_format_whence which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_format(fh, 0);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return &sensor->format;
+	default:
+		return NULL;
+	}
+}
+
+static int mt9m032_get_pad_format(struct v4l2_subdev *subdev,
+				  struct v4l2_subdev_fh *fh,
+				  struct v4l2_subdev_format *fmt)
+{
+	struct mt9m032 *sensor = to_mt9m032(subdev);
+
+	mutex_lock(&sensor->lock);
+	fmt->format = *__mt9m032_get_pad_format(sensor, fh, fmt->which);
+	mutex_unlock(&sensor->lock);
+
+	return 0;
+}
+
+static int mt9m032_set_pad_format(struct v4l2_subdev *subdev,
+				  struct v4l2_subdev_fh *fh,
+				  struct v4l2_subdev_format *fmt)
+{
+	struct mt9m032 *sensor = to_mt9m032(subdev);
+	int ret;
+
+	mutex_lock(&sensor->lock);
+
+	if (sensor->streaming && fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		ret = -EBUSY;
+		goto done;
+	}
+
+	/* Scaling is not supported, the format is thus fixed. */
+	fmt->format = *__mt9m032_get_pad_format(sensor, fh, fmt->which);
+	ret = 0;
+
+done:
+	mutex_unlock(&sensor->lock);
+	return ret;
+}
+
+static int mt9m032_get_pad_crop(struct v4l2_subdev *subdev,
+				struct v4l2_subdev_fh *fh,
+				struct v4l2_subdev_crop *crop)
+{
+	struct mt9m032 *sensor = to_mt9m032(subdev);
+
+	mutex_lock(&sensor->lock);
+	crop->rect = *__mt9m032_get_pad_crop(sensor, fh, crop->which);
+	mutex_unlock(&sensor->lock);
+
+	return 0;
+}
+
+static int mt9m032_set_pad_crop(struct v4l2_subdev *subdev,
+				struct v4l2_subdev_fh *fh,
+				struct v4l2_subdev_crop *crop)
+{
+	struct mt9m032 *sensor = to_mt9m032(subdev);
+	struct v4l2_mbus_framefmt *format;
+	struct v4l2_rect *__crop;
+	struct v4l2_rect rect;
+	int ret = 0;
+
+	mutex_lock(&sensor->lock);
+
+	if (sensor->streaming && crop->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		ret = -EBUSY;
+		goto done;
+	}
+
+	/* Clamp the crop rectangle boundaries and align them to a multiple of 2
+	 * pixels to ensure a GRBG Bayer pattern.
+	 */
+	rect.left = clamp(ALIGN(crop->rect.left, 2), MT9M032_COLUMN_START_MIN,
+			  MT9M032_COLUMN_START_MAX);
+	rect.top = clamp(ALIGN(crop->rect.top, 2), MT9M032_ROW_START_MIN,
+			 MT9M032_ROW_START_MAX);
+	rect.width = clamp(ALIGN(crop->rect.width, 2), MT9M032_COLUMN_SIZE_MIN,
+			   MT9M032_COLUMN_SIZE_MAX);
+	rect.height = clamp(ALIGN(crop->rect.height, 2), MT9M032_ROW_SIZE_MIN,
+			    MT9M032_ROW_SIZE_MAX);
+
+	rect.width = min(rect.width, MT9M032_PIXEL_ARRAY_WIDTH - rect.left);
+	rect.height = min(rect.height, MT9M032_PIXEL_ARRAY_HEIGHT - rect.top);
+
+	__crop = __mt9m032_get_pad_crop(sensor, fh, crop->which);
+
+	if (rect.width != __crop->width || rect.height != __crop->height) {
+		/* Reset the output image size if the crop rectangle size has
+		 * been modified.
+		 */
+		format = __mt9m032_get_pad_format(sensor, fh, crop->which);
+		format->width = rect.width;
+		format->height = rect.height;
+	}
+
+	*__crop = rect;
+	crop->rect = rect;
+
+	if (crop->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		ret = mt9m032_update_geom_timing(sensor);
+
+done:
+	mutex_unlock(&sensor->lock);
+	return ret;
+}
+
+static int mt9m032_get_frame_interval(struct v4l2_subdev *subdev,
+				      struct v4l2_subdev_frame_interval *fi)
+{
+	struct mt9m032 *sensor = to_mt9m032(subdev);
+
+	mutex_lock(&sensor->lock);
+	memset(fi, 0, sizeof(*fi));
+	fi->interval = sensor->frame_interval;
+	mutex_unlock(&sensor->lock);
+
+	return 0;
+}
+
+static int mt9m032_set_frame_interval(struct v4l2_subdev *subdev,
+				      struct v4l2_subdev_frame_interval *fi)
+{
+	struct mt9m032 *sensor = to_mt9m032(subdev);
+	int ret;
+
+	mutex_lock(&sensor->lock);
+
+	if (sensor->streaming) {
+		ret = -EBUSY;
+		goto done;
+	}
+
+	/* Avoid divisions by 0. */
+	if (fi->interval.denominator == 0)
+		fi->interval.denominator = 1;
+
+	ret = mt9m032_update_timing(sensor, &fi->interval);
+	if (!ret)
+		sensor->frame_interval = fi->interval;
+
+done:
+	mutex_unlock(&sensor->lock);
+	return ret;
+}
+
+static int mt9m032_s_stream(struct v4l2_subdev *subdev, int streaming)
+{
+	struct mt9m032 *sensor = to_mt9m032(subdev);
+	int ret;
+
+	mutex_lock(&sensor->lock);
+	ret = update_formatter2(sensor, streaming);
+	if (!ret)
+		sensor->streaming = streaming;
+	mutex_unlock(&sensor->lock);
+
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev core operations
+ */
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int mt9m032_g_register(struct v4l2_subdev *sd,
+			      struct v4l2_dbg_register *reg)
+{
+	struct mt9m032 *sensor = to_mt9m032(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->subdev);
+	int val;
+
+	if (reg->match.type != V4L2_CHIP_MATCH_I2C_ADDR || reg->reg > 0xff)
+		return -EINVAL;
+	if (reg->match.addr != client->addr)
+		return -ENODEV;
+
+	val = mt9m032_read(client, reg->reg);
+	if (val < 0)
+		return -EIO;
+
+	reg->size = 2;
+	reg->val = val;
+
+	return 0;
+}
+
+static int mt9m032_s_register(struct v4l2_subdev *sd,
+			      struct v4l2_dbg_register *reg)
+{
+	struct mt9m032 *sensor = to_mt9m032(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->subdev);
+
+	if (reg->match.type != V4L2_CHIP_MATCH_I2C_ADDR || reg->reg > 0xff)
+		return -EINVAL;
+
+	if (reg->match.addr != client->addr)
+		return -ENODEV;
+
+	return mt9m032_write(client, reg->reg, reg->val);
+}
+#endif
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev control operations
+ */
+
+static int update_read_mode2(struct mt9m032 *sensor, bool vflip, bool hflip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->subdev);
+	int reg_val = (vflip << MT9M032_READ_MODE2_VFLIP_SHIFT)
+		    | (hflip << MT9M032_READ_MODE2_HFLIP_SHIFT)
+		    | MT9M032_READ_MODE2_ROW_BLC
+		    | 0x0007;
+
+	return mt9m032_write(client, MT9M032_READ_MODE2, reg_val);
+}
+
+static int mt9m032_set_gain(struct mt9m032 *sensor, s32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->subdev);
+	int digital_gain_val;	/* in 1/8th (0..127) */
+	int analog_mul;		/* 0 or 1 */
+	int analog_gain_val;	/* in 1/16th. (0..63) */
+	u16 reg_val;
+
+	digital_gain_val = 51; /* from setup example */
+
+	if (val < 63) {
+		analog_mul = 0;
+		analog_gain_val = val;
+	} else {
+		analog_mul = 1;
+		analog_gain_val = val / 2;
+	}
+
+	/* a_gain = (1 + analog_mul) + (analog_gain_val + 1) / 16 */
+	/* overall_gain = a_gain * (1 + digital_gain_val / 8) */
+
+	reg_val = ((digital_gain_val & MT9M032_GAIN_DIGITAL_MASK)
+		   << MT9M032_GAIN_DIGITAL_SHIFT)
+		| ((analog_mul & 1) << MT9M032_GAIN_AMUL_SHIFT)
+		| (analog_gain_val & MT9M032_GAIN_ANALOG_MASK);
+
+	return mt9m032_write(client, MT9M032_GAIN_ALL, reg_val);
+}
+
+static int mt9m032_try_ctrl(struct v4l2_ctrl *ctrl)
+{
+	if (ctrl->id == V4L2_CID_GAIN && ctrl->val >= 63) {
+		/* round because of multiplier used for values >= 63 */
+		ctrl->val &= ~1;
+	}
+
+	return 0;
+}
+
+static int mt9m032_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct mt9m032 *sensor =
+		container_of(ctrl->handler, struct mt9m032, ctrls);
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->subdev);
+	int ret;
+
+	switch (ctrl->id) {
+	case V4L2_CID_GAIN:
+		return mt9m032_set_gain(sensor, ctrl->val);
+
+	case V4L2_CID_HFLIP:
+	/* case V4L2_CID_VFLIP: -- In the same cluster */
+		return update_read_mode2(sensor, sensor->vflip->val,
+					 sensor->hflip->val);
+
+	case V4L2_CID_EXPOSURE:
+		ret = mt9m032_write(client, MT9M032_SHUTTER_WIDTH_HIGH,
+				    (ctrl->val >> 16) & 0xffff);
+		if (ret < 0)
+			return ret;
+
+		return mt9m032_write(client, MT9M032_SHUTTER_WIDTH_LOW,
+				     ctrl->val & 0xffff);
+	}
+
+	return 0;
+}
+
+static struct v4l2_ctrl_ops mt9m032_ctrl_ops = {
+	.s_ctrl = mt9m032_set_ctrl,
+	.try_ctrl = mt9m032_try_ctrl,
+};
+
+/* -------------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_core_ops mt9m032_core_ops = {
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = mt9m032_g_register,
+	.s_register = mt9m032_s_register,
+#endif
+};
+
+static const struct v4l2_subdev_video_ops mt9m032_video_ops = {
+	.s_stream = mt9m032_s_stream,
+	.g_frame_interval = mt9m032_get_frame_interval,
+	.s_frame_interval = mt9m032_set_frame_interval,
+};
+
+static const struct v4l2_subdev_pad_ops mt9m032_pad_ops = {
+	.enum_mbus_code = mt9m032_enum_mbus_code,
+	.enum_frame_size = mt9m032_enum_frame_size,
+	.get_fmt = mt9m032_get_pad_format,
+	.set_fmt = mt9m032_set_pad_format,
+	.set_crop = mt9m032_set_pad_crop,
+	.get_crop = mt9m032_get_pad_crop,
+};
+
+static const struct v4l2_subdev_ops mt9m032_ops = {
+	.core = &mt9m032_core_ops,
+	.video = &mt9m032_video_ops,
+	.pad = &mt9m032_pad_ops,
+};
+
+/* -----------------------------------------------------------------------------
+ * Driver initialization and probing
+ */
+
+static int mt9m032_probe(struct i2c_client *client,
+			 const struct i2c_device_id *devid)
+{
+	struct mt9m032_platform_data *pdata = client->dev.platform_data;
+	struct i2c_adapter *adapter = client->adapter;
+	struct mt9m032 *sensor;
+	int chip_version;
+	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;
+	}
+
+	if (!client->dev.platform_data)
+		return -ENODEV;
+
+	sensor = kzalloc(sizeof(*sensor), GFP_KERNEL);
+	if (sensor == NULL)
+		return -ENOMEM;
+
+	mutex_init(&sensor->lock);
+
+	sensor->pdata = pdata;
+
+	v4l2_i2c_subdev_init(&sensor->subdev, client, &mt9m032_ops);
+	sensor->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	chip_version = mt9m032_read(client, MT9M032_CHIP_VERSION);
+	if (chip_version != MT9M032_CHIP_VERSION_VALUE) {
+		dev_err(&client->dev, "MT9M032 not detected, wrong version "
+			"0x%04x\n", chip_version);
+		ret = -ENODEV;
+		goto error_sensor;
+	}
+
+	dev_info(&client->dev, "MT9M032 detected at address 0x%02x\n",
+		 client->addr);
+
+	sensor->frame_interval.numerator = 1;
+	sensor->frame_interval.denominator = 30;
+
+	sensor->crop.left = MT9M032_COLUMN_START_DEF;
+	sensor->crop.top = MT9M032_ROW_START_DEF;
+	sensor->crop.width = MT9M032_COLUMN_SIZE_DEF;
+	sensor->crop.height = MT9M032_ROW_SIZE_DEF;
+
+	sensor->format.width = sensor->crop.width;
+	sensor->format.height = sensor->crop.height;
+	sensor->format.code = V4L2_MBUS_FMT_Y8_1X8;
+	sensor->format.field = V4L2_FIELD_NONE;
+	sensor->format.colorspace = V4L2_COLORSPACE_SRGB;
+
+	v4l2_ctrl_handler_init(&sensor->ctrls, 5);
+
+	v4l2_ctrl_new_std(&sensor->ctrls, &mt9m032_ctrl_ops,
+			  V4L2_CID_GAIN, 0, 127, 1, 64);
+
+	sensor->hflip = v4l2_ctrl_new_std(&sensor->ctrls,
+					  &mt9m032_ctrl_ops,
+					  V4L2_CID_HFLIP, 0, 1, 1, 0);
+	sensor->vflip = v4l2_ctrl_new_std(&sensor->ctrls,
+					  &mt9m032_ctrl_ops,
+					  V4L2_CID_VFLIP, 0, 1, 1, 0);
+
+	v4l2_ctrl_new_std(&sensor->ctrls, &mt9m032_ctrl_ops,
+			  V4L2_CID_EXPOSURE, MT9M032_SHUTTER_WIDTH_MIN,
+			  MT9M032_SHUTTER_WIDTH_MAX, 1,
+			  MT9M032_SHUTTER_WIDTH_DEF);
+	v4l2_ctrl_new_std(&sensor->ctrls, &mt9m032_ctrl_ops,
+			  V4L2_CID_PIXEL_RATE, pdata->pix_clock,
+			  pdata->pix_clock, 1, pdata->pix_clock);
+
+	if (sensor->ctrls.error) {
+		ret = sensor->ctrls.error;
+		dev_err(&client->dev, "control initialization error %d\n", ret);
+		goto error_ctrl;
+	}
+
+	v4l2_ctrl_cluster(2, &sensor->hflip);
+
+	sensor->subdev.ctrl_handler = &sensor->ctrls;
+	sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_init(&sensor->subdev.entity, 1, &sensor->pad, 0);
+	if (ret < 0)
+		goto error_ctrl;
+
+	ret = mt9m032_write(client, MT9M032_RESET, 1);	/* reset on */
+	if (ret < 0)
+		goto error_entity;
+	ret = mt9m032_write(client, MT9M032_RESET, 0);	/* reset off */
+	if (ret < 0)
+		goto error_entity;
+
+	ret = mt9m032_setup_pll(sensor);
+	if (ret < 0)
+		goto error_entity;
+	usleep_range(10000, 11000);
+
+	ret = v4l2_ctrl_handler_setup(&sensor->ctrls);
+	if (ret < 0)
+		goto error_entity;
+
+	/* SIZE */
+	ret = mt9m032_update_geom_timing(sensor);
+	if (ret < 0)
+		goto error_entity;
+
+	ret = mt9m032_write(client, 0x41, 0x0000);	/* reserved !!! */
+	if (ret < 0)
+		goto error_entity;
+	ret = mt9m032_write(client, 0x42, 0x0003);	/* reserved !!! */
+	if (ret < 0)
+		goto error_entity;
+	ret = mt9m032_write(client, 0x43, 0x0003);	/* reserved !!! */
+	if (ret < 0)
+		goto error_entity;
+	ret = mt9m032_write(client, 0x7f, 0x0000);	/* reserved !!! */
+	if (ret < 0)
+		goto error_entity;
+	if (sensor->pdata->invert_pixclock) {
+		ret = mt9m032_write(client, MT9M032_PIX_CLK_CTRL,
+				    MT9M032_PIX_CLK_CTRL_INV_PIXCLK);
+		if (ret < 0)
+			goto error_entity;
+	}
+
+	ret = mt9m032_write(client, MT9M032_RESTART, 1); /* Restart on */
+	if (ret < 0)
+		goto error_entity;
+	msleep(100);
+	ret = mt9m032_write(client, MT9M032_RESTART, 0); /* Restart off */
+	if (ret < 0)
+		goto error_entity;
+	msleep(100);
+	ret = update_formatter2(sensor, false);
+	if (ret < 0)
+		goto error_entity;
+
+	return ret;
+
+error_entity:
+	media_entity_cleanup(&sensor->subdev.entity);
+error_ctrl:
+	v4l2_ctrl_handler_free(&sensor->ctrls);
+error_sensor:
+	mutex_destroy(&sensor->lock);
+	kfree(sensor);
+	return ret;
+}
+
+static int mt9m032_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct mt9m032 *sensor = to_mt9m032(subdev);
+
+	v4l2_device_unregister_subdev(subdev);
+	v4l2_ctrl_handler_free(&sensor->ctrls);
+	media_entity_cleanup(&subdev->entity);
+	mutex_destroy(&sensor->lock);
+	kfree(sensor);
+	return 0;
+}
+
+static const struct i2c_device_id mt9m032_id_table[] = {
+	{ MT9M032_NAME, 0 },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(i2c, mt9m032_id_table);
+
+static struct i2c_driver mt9m032_i2c_driver = {
+	.driver = {
+		.name = MT9M032_NAME,
+	},
+	.probe = mt9m032_probe,
+	.remove = mt9m032_remove,
+	.id_table = mt9m032_id_table,
+};
+
+module_i2c_driver(mt9m032_i2c_driver);
+
+MODULE_AUTHOR("Martin Hostettler <martin@neutronstar.dyndns.org>");
+MODULE_DESCRIPTION("MT9M032 camera sensor driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/mt9p031.c b/drivers/media/i2c/mt9p031.c
new file mode 100644
index 000000000000..2c0f4077c491
--- /dev/null
+++ b/drivers/media/i2c/mt9p031.c
@@ -0,0 +1,1071 @@
+/*
+ * 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.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/gpio.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/log2.h>
+#include <linux/pm.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+
+#include <media/mt9p031.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.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_FRAME_RESTART				0x0b
+#define MT9P031_SHUTTER_DELAY				0x0c
+#define MT9P031_RST					0x0d
+#define		MT9P031_RST_ENABLE			1
+#define		MT9P031_RST_DISABLE			0
+#define MT9P031_READ_MODE_1				0x1e
+#define MT9P031_READ_MODE_2				0x20
+#define		MT9P031_READ_MODE_2_ROW_MIR		(1 << 15)
+#define		MT9P031_READ_MODE_2_COL_MIR		(1 << 14)
+#define		MT9P031_READ_MODE_2_ROW_BLC		(1 << 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		(1 << 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			(1 << 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		(1 << 0)
+#define		MT9P031_TEST_PATTERN_DISABLE		(0 << 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;
+
+	enum mt9p031_model model;
+	struct aptina_pll pll;
+	int 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, MT9P031_RST_DISABLE);
+	if (ret < 0)
+		return ret;
+
+	return mt9p031_set_output_control(mt9p031, MT9P031_OUTPUT_CONTROL_CEN,
+					  0);
+}
+
+static int mt9p031_pll_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;
+
+	mt9p031->pll.ext_clock = pdata->ext_freq;
+	mt9p031->pll.pix_clock = pdata->target_freq;
+
+	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;
+
+	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);
+
+	return mt9p031_write(client, MT9P031_PLL_CONTROL,
+			     MT9P031_PLL_CONTROL_PWROFF);
+}
+
+static int mt9p031_power_on(struct mt9p031 *mt9p031)
+{
+	/* Ensure RESET_BAR is low */
+	if (mt9p031->reset != -1) {
+		gpio_set_value(mt9p031->reset, 0);
+		usleep_range(1000, 2000);
+	}
+
+	/* Emable clock */
+	if (mt9p031->pdata->set_xclk)
+		mt9p031->pdata->set_xclk(&mt9p031->subdev,
+					 mt9p031->pdata->ext_freq);
+
+	/* Now RESET_BAR must be high */
+	if (mt9p031->reset != -1) {
+		gpio_set_value(mt9p031->reset, 1);
+		usleep_range(1000, 2000);
+	}
+
+	return 0;
+}
+
+static void mt9p031_power_off(struct mt9p031 *mt9p031)
+{
+	if (mt9p031->reset != -1) {
+		gpio_set_value(mt9p031->reset, 0);
+		usleep_range(1000, 2000);
+	}
+
+	if (mt9p031->pdata->set_xclk)
+		mt9p031->pdata->set_xclk(&mt9p031->subdev, 0);
+}
+
+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;
+	}
+
+	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);
+	int ret;
+
+	if (!enable) {
+		/* 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;
+
+	return mt9p031_pll_enable(mt9p031);
+}
+
+static int mt9p031_enum_mbus_code(struct v4l2_subdev *subdev,
+				  struct v4l2_subdev_fh *fh,
+				  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_fh *fh,
+				   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_fh *fh,
+			 unsigned int pad, u32 which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_format(fh, 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_fh *fh,
+		     unsigned int pad, u32 which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_crop(fh, pad);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return &mt9p031->crop;
+	default:
+		return NULL;
+	}
+}
+
+static int mt9p031_get_format(struct v4l2_subdev *subdev,
+			      struct v4l2_subdev_fh *fh,
+			      struct v4l2_subdev_format *fmt)
+{
+	struct mt9p031 *mt9p031 = to_mt9p031(subdev);
+
+	fmt->format = *__mt9p031_get_pad_format(mt9p031, fh, fmt->pad,
+						fmt->which);
+	return 0;
+}
+
+static int mt9p031_set_format(struct v4l2_subdev *subdev,
+			      struct v4l2_subdev_fh *fh,
+			      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, fh, 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(__crop->width / 7, MT9P031_WINDOW_WIDTH_MIN),
+			__crop->width);
+	height = clamp_t(unsigned int, ALIGN(format->format.height, 2),
+			max(__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, fh, format->pad,
+					    format->which);
+	__format->width = __crop->width / hratio;
+	__format->height = __crop->height / vratio;
+
+	format->format = *__format;
+
+	return 0;
+}
+
+static int mt9p031_get_crop(struct v4l2_subdev *subdev,
+			    struct v4l2_subdev_fh *fh,
+			    struct v4l2_subdev_crop *crop)
+{
+	struct mt9p031 *mt9p031 = to_mt9p031(subdev);
+
+	crop->rect = *__mt9p031_get_pad_crop(mt9p031, fh, crop->pad,
+					     crop->which);
+	return 0;
+}
+
+static int mt9p031_set_crop(struct v4l2_subdev *subdev,
+			    struct v4l2_subdev_fh *fh,
+			    struct v4l2_subdev_crop *crop)
+{
+	struct mt9p031 *mt9p031 = to_mt9p031(subdev);
+	struct v4l2_mbus_framefmt *__format;
+	struct v4l2_rect *__crop;
+	struct v4l2_rect rect;
+
+	/* Clamp the crop rectangle boundaries and align them to a multiple of 2
+	 * pixels to ensure a GRBG Bayer pattern.
+	 */
+	rect.left = clamp(ALIGN(crop->rect.left, 2), MT9P031_COLUMN_START_MIN,
+			  MT9P031_COLUMN_START_MAX);
+	rect.top = clamp(ALIGN(crop->rect.top, 2), MT9P031_ROW_START_MIN,
+			 MT9P031_ROW_START_MAX);
+	rect.width = clamp(ALIGN(crop->rect.width, 2),
+			   MT9P031_WINDOW_WIDTH_MIN,
+			   MT9P031_WINDOW_WIDTH_MAX);
+	rect.height = clamp(ALIGN(crop->rect.height, 2),
+			    MT9P031_WINDOW_HEIGHT_MIN,
+			    MT9P031_WINDOW_HEIGHT_MAX);
+
+	rect.width = min(rect.width, MT9P031_PIXEL_ARRAY_WIDTH - rect.left);
+	rect.height = min(rect.height, MT9P031_PIXEL_ARRAY_HEIGHT - rect.top);
+
+	__crop = __mt9p031_get_pad_crop(mt9p031, fh, crop->pad, crop->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, fh, crop->pad,
+						    crop->which);
+		__format->width = rect.width;
+		__format->height = rect.height;
+	}
+
+	*__crop = rect;
+	crop->rect = rect;
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev control operations
+ */
+
+#define V4L2_CID_TEST_PATTERN		(V4L2_CID_USER_BASE | 0x1001)
+#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_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;
+
+	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:
+		if (!ctrl->val) {
+			/* Restore the black level compensation settings. */
+			if (mt9p031->blc_auto->cur.val != 0) {
+				ret = mt9p031_s_ctrl(mt9p031->blc_auto);
+				if (ret < 0)
+					return ret;
+			}
+			if (mt9p031->blc_offset->cur.val != 0) {
+				ret = mt9p031_s_ctrl(mt9p031->blc_offset);
+				if (ret < 0)
+					return ret;
+			}
+			return mt9p031_write(client, MT9P031_TEST_PATTERN,
+					     MT9P031_TEST_PATTERN_DISABLE);
+		}
+
+		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 black level compensation when using 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 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_TEST_PATTERN,
+		.type		= V4L2_CTRL_TYPE_MENU,
+		.name		= "Test Pattern",
+		.min		= 0,
+		.max		= ARRAY_SIZE(mt9p031_test_pattern_menu) - 1,
+		.step		= 0,
+		.def		= 0,
+		.flags		= 0,
+		.menu_skip_mask	= 0,
+		.qmenu		= mt9p031_test_pattern_menu,
+	}, {
+		.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);
+	if (data != MT9P031_CHIP_VERSION_VALUE) {
+		dev_err(&client->dev, "MT9P031 not detected, wrong version "
+			"0x%04x\n", data);
+		return -ENODEV;
+	}
+
+	mt9p031_power_off(mt9p031);
+
+	dev_info(&client->dev, "MT9P031 detected at address 0x%02x\n",
+		 client->addr);
+
+	return ret;
+}
+
+static int mt9p031_open(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
+{
+	struct mt9p031 *mt9p031 = to_mt9p031(subdev);
+	struct v4l2_mbus_framefmt *format;
+	struct v4l2_rect *crop;
+
+	crop = v4l2_subdev_get_try_crop(fh, 0);
+	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 = v4l2_subdev_get_try_format(fh, 0);
+
+	if (mt9p031->model == MT9P031_MODEL_MONOCHROME)
+		format->code = V4L2_MBUS_FMT_Y12_1X12;
+	else
+		format->code = V4L2_MBUS_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 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 struct v4l2_subdev_core_ops mt9p031_subdev_core_ops = {
+	.s_power        = mt9p031_set_power,
+};
+
+static struct v4l2_subdev_video_ops mt9p031_subdev_video_ops = {
+	.s_stream       = mt9p031_s_stream,
+};
+
+static struct v4l2_subdev_pad_ops mt9p031_subdev_pad_ops = {
+	.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_crop = mt9p031_get_crop,
+	.set_crop = mt9p031_set_crop,
+};
+
+static 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 int mt9p031_probe(struct i2c_client *client,
+			 const struct i2c_device_id *did)
+{
+	struct mt9p031_platform_data *pdata = client->dev.platform_data;
+	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
+	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 = kzalloc(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->reset = -1;
+
+	v4l2_ctrl_handler_init(&mt9p031->ctrls, ARRAY_SIZE(mt9p031_ctrls) + 5);
+
+	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);
+
+	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);
+
+	mutex_init(&mt9p031->power_lock);
+	v4l2_i2c_subdev_init(&mt9p031->subdev, client, &mt9p031_subdev_ops);
+	mt9p031->subdev.internal_ops = &mt9p031_subdev_internal_ops;
+
+	mt9p031->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_init(&mt9p031->subdev.entity, 1, &mt9p031->pad, 0);
+	if (ret < 0)
+		goto done;
+
+	mt9p031->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	mt9p031->crop.width = MT9P031_WINDOW_WIDTH_DEF;
+	mt9p031->crop.height = MT9P031_WINDOW_HEIGHT_DEF;
+	mt9p031->crop.left = MT9P031_COLUMN_START_DEF;
+	mt9p031->crop.top = MT9P031_ROW_START_DEF;
+
+	if (mt9p031->model == MT9P031_MODEL_MONOCHROME)
+		mt9p031->format.code = V4L2_MBUS_FMT_Y12_1X12;
+	else
+		mt9p031->format.code = V4L2_MBUS_FMT_SGRBG12_1X12;
+
+	mt9p031->format.width = MT9P031_WINDOW_WIDTH_DEF;
+	mt9p031->format.height = MT9P031_WINDOW_HEIGHT_DEF;
+	mt9p031->format.field = V4L2_FIELD_NONE;
+	mt9p031->format.colorspace = V4L2_COLORSPACE_SRGB;
+
+	if (pdata->reset != -1) {
+		ret = gpio_request_one(pdata->reset, GPIOF_OUT_INIT_LOW,
+				       "mt9p031_rst");
+		if (ret < 0)
+			goto done;
+
+		mt9p031->reset = pdata->reset;
+	}
+
+	ret = mt9p031_pll_setup(mt9p031);
+
+done:
+	if (ret < 0) {
+		if (mt9p031->reset != -1)
+			gpio_free(mt9p031->reset);
+
+		v4l2_ctrl_handler_free(&mt9p031->ctrls);
+		media_entity_cleanup(&mt9p031->subdev.entity);
+		kfree(mt9p031);
+	}
+
+	return ret;
+}
+
+static int 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_device_unregister_subdev(subdev);
+	media_entity_cleanup(&subdev->entity);
+	if (mt9p031->reset != -1)
+		gpio_free(mt9p031->reset);
+	kfree(mt9p031);
+
+	return 0;
+}
+
+static const struct i2c_device_id mt9p031_id[] = {
+	{ "mt9p031", MT9P031_MODEL_COLOR },
+	{ "mt9p031m", MT9P031_MODEL_MONOCHROME },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, mt9p031_id);
+
+static struct i2c_driver mt9p031_i2c_driver = {
+	.driver = {
+		.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/mt9t001.c b/drivers/media/i2c/mt9t001.c
new file mode 100644
index 000000000000..6d343adf891d
--- /dev/null
+++ b/drivers/media/i2c/mt9t001.c
@@ -0,0 +1,833 @@
+/*
+ * Driver for MT9T001 CMOS Image Sensor from Aptina (Micron)
+ *
+ * Copyright (C) 2010-2011, Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *
+ * Based on the MT9M001 driver,
+ *
+ * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/log2.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <linux/v4l2-mediabus.h>
+
+#include <media/mt9t001.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+
+#define MT9T001_PIXEL_ARRAY_HEIGHT			1568
+#define MT9T001_PIXEL_ARRAY_WIDTH			2112
+
+#define MT9T001_CHIP_VERSION				0x00
+#define		MT9T001_CHIP_ID				0x1621
+#define MT9T001_ROW_START				0x01
+#define		MT9T001_ROW_START_MIN			0
+#define		MT9T001_ROW_START_DEF			20
+#define		MT9T001_ROW_START_MAX			1534
+#define MT9T001_COLUMN_START				0x02
+#define		MT9T001_COLUMN_START_MIN		0
+#define		MT9T001_COLUMN_START_DEF		32
+#define		MT9T001_COLUMN_START_MAX		2046
+#define MT9T001_WINDOW_HEIGHT				0x03
+#define		MT9T001_WINDOW_HEIGHT_MIN		1
+#define		MT9T001_WINDOW_HEIGHT_DEF		1535
+#define		MT9T001_WINDOW_HEIGHT_MAX		1567
+#define MT9T001_WINDOW_WIDTH				0x04
+#define		MT9T001_WINDOW_WIDTH_MIN		1
+#define		MT9T001_WINDOW_WIDTH_DEF		2047
+#define		MT9T001_WINDOW_WIDTH_MAX		2111
+#define MT9T001_HORIZONTAL_BLANKING			0x05
+#define		MT9T001_HORIZONTAL_BLANKING_MIN		21
+#define		MT9T001_HORIZONTAL_BLANKING_MAX		1023
+#define MT9T001_VERTICAL_BLANKING			0x06
+#define		MT9T001_VERTICAL_BLANKING_MIN		3
+#define		MT9T001_VERTICAL_BLANKING_MAX		1023
+#define MT9T001_OUTPUT_CONTROL				0x07
+#define		MT9T001_OUTPUT_CONTROL_SYNC		(1 << 0)
+#define		MT9T001_OUTPUT_CONTROL_CHIP_ENABLE	(1 << 1)
+#define		MT9T001_OUTPUT_CONTROL_TEST_DATA	(1 << 6)
+#define MT9T001_SHUTTER_WIDTH_HIGH			0x08
+#define MT9T001_SHUTTER_WIDTH_LOW			0x09
+#define		MT9T001_SHUTTER_WIDTH_MIN		1
+#define		MT9T001_SHUTTER_WIDTH_DEF		1561
+#define		MT9T001_SHUTTER_WIDTH_MAX		(1024 * 1024)
+#define MT9T001_PIXEL_CLOCK				0x0a
+#define		MT9T001_PIXEL_CLOCK_INVERT		(1 << 15)
+#define		MT9T001_PIXEL_CLOCK_SHIFT_MASK		(7 << 8)
+#define		MT9T001_PIXEL_CLOCK_SHIFT_SHIFT		8
+#define		MT9T001_PIXEL_CLOCK_DIVIDE_MASK		(0x7f << 0)
+#define MT9T001_FRAME_RESTART				0x0b
+#define MT9T001_SHUTTER_DELAY				0x0c
+#define		MT9T001_SHUTTER_DELAY_MAX		2047
+#define MT9T001_RESET					0x0d
+#define MT9T001_READ_MODE1				0x1e
+#define		MT9T001_READ_MODE_SNAPSHOT		(1 << 8)
+#define		MT9T001_READ_MODE_STROBE_ENABLE		(1 << 9)
+#define		MT9T001_READ_MODE_STROBE_WIDTH		(1 << 10)
+#define		MT9T001_READ_MODE_STROBE_OVERRIDE	(1 << 11)
+#define MT9T001_READ_MODE2				0x20
+#define		MT9T001_READ_MODE_BAD_FRAMES		(1 << 0)
+#define		MT9T001_READ_MODE_LINE_VALID_CONTINUOUS	(1 << 9)
+#define		MT9T001_READ_MODE_LINE_VALID_FRAME	(1 << 10)
+#define MT9T001_READ_MODE3				0x21
+#define		MT9T001_READ_MODE_GLOBAL_RESET		(1 << 0)
+#define		MT9T001_READ_MODE_GHST_CTL		(1 << 1)
+#define MT9T001_ROW_ADDRESS_MODE			0x22
+#define		MT9T001_ROW_SKIP_MASK			(7 << 0)
+#define		MT9T001_ROW_BIN_MASK			(3 << 3)
+#define		MT9T001_ROW_BIN_SHIFT			3
+#define MT9T001_COLUMN_ADDRESS_MODE			0x23
+#define		MT9T001_COLUMN_SKIP_MASK		(7 << 0)
+#define		MT9T001_COLUMN_BIN_MASK			(3 << 3)
+#define		MT9T001_COLUMN_BIN_SHIFT		3
+#define MT9T001_GREEN1_GAIN				0x2b
+#define MT9T001_BLUE_GAIN				0x2c
+#define MT9T001_RED_GAIN				0x2d
+#define MT9T001_GREEN2_GAIN				0x2e
+#define MT9T001_TEST_DATA				0x32
+#define MT9T001_GLOBAL_GAIN				0x35
+#define		MT9T001_GLOBAL_GAIN_MIN			8
+#define		MT9T001_GLOBAL_GAIN_MAX			1024
+#define MT9T001_BLACK_LEVEL				0x49
+#define MT9T001_ROW_BLACK_DEFAULT_OFFSET		0x4b
+#define MT9T001_BLC_DELTA_THRESHOLDS			0x5d
+#define MT9T001_CAL_THRESHOLDS				0x5f
+#define MT9T001_GREEN1_OFFSET				0x60
+#define MT9T001_GREEN2_OFFSET				0x61
+#define MT9T001_BLACK_LEVEL_CALIBRATION			0x62
+#define		MT9T001_BLACK_LEVEL_OVERRIDE		(1 << 0)
+#define		MT9T001_BLACK_LEVEL_DISABLE_OFFSET	(1 << 1)
+#define		MT9T001_BLACK_LEVEL_RECALCULATE		(1 << 12)
+#define		MT9T001_BLACK_LEVEL_LOCK_RED_BLUE	(1 << 13)
+#define		MT9T001_BLACK_LEVEL_LOCK_GREEN		(1 << 14)
+#define MT9T001_RED_OFFSET				0x63
+#define MT9T001_BLUE_OFFSET				0x64
+
+struct mt9t001 {
+	struct v4l2_subdev subdev;
+	struct media_pad pad;
+
+	struct v4l2_mbus_framefmt format;
+	struct v4l2_rect crop;
+
+	struct v4l2_ctrl_handler ctrls;
+	struct v4l2_ctrl *gains[4];
+
+	u16 output_control;
+	u16 black_level;
+};
+
+static inline struct mt9t001 *to_mt9t001(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct mt9t001, subdev);
+}
+
+static int mt9t001_read(struct i2c_client *client, u8 reg)
+{
+	return i2c_smbus_read_word_swapped(client, reg);
+}
+
+static int mt9t001_write(struct i2c_client *client, u8 reg, u16 data)
+{
+	return i2c_smbus_write_word_swapped(client, reg, data);
+}
+
+static int mt9t001_set_output_control(struct mt9t001 *mt9t001, u16 clear,
+				      u16 set)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9t001->subdev);
+	u16 value = (mt9t001->output_control & ~clear) | set;
+	int ret;
+
+	if (value == mt9t001->output_control)
+		return 0;
+
+	ret = mt9t001_write(client, MT9T001_OUTPUT_CONTROL, value);
+	if (ret < 0)
+		return ret;
+
+	mt9t001->output_control = value;
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev video operations
+ */
+
+static struct v4l2_mbus_framefmt *
+__mt9t001_get_pad_format(struct mt9t001 *mt9t001, struct v4l2_subdev_fh *fh,
+			 unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_format(fh, pad);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return &mt9t001->format;
+	default:
+		return NULL;
+	}
+}
+
+static struct v4l2_rect *
+__mt9t001_get_pad_crop(struct mt9t001 *mt9t001, struct v4l2_subdev_fh *fh,
+		       unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_crop(fh, pad);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return &mt9t001->crop;
+	default:
+		return NULL;
+	}
+}
+
+static int mt9t001_s_stream(struct v4l2_subdev *subdev, int enable)
+{
+	const u16 mode = MT9T001_OUTPUT_CONTROL_CHIP_ENABLE;
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
+	struct mt9t001 *mt9t001 = to_mt9t001(subdev);
+	struct v4l2_mbus_framefmt *format = &mt9t001->format;
+	struct v4l2_rect *crop = &mt9t001->crop;
+	unsigned int hratio;
+	unsigned int vratio;
+	int ret;
+
+	if (!enable)
+		return mt9t001_set_output_control(mt9t001, mode, 0);
+
+	/* Configure the window size and row/column bin */
+	hratio = DIV_ROUND_CLOSEST(crop->width, format->width);
+	vratio = DIV_ROUND_CLOSEST(crop->height, format->height);
+
+	ret = mt9t001_write(client, MT9T001_ROW_ADDRESS_MODE, hratio - 1);
+	if (ret < 0)
+		return ret;
+
+	ret = mt9t001_write(client, MT9T001_COLUMN_ADDRESS_MODE, vratio - 1);
+	if (ret < 0)
+		return ret;
+
+	ret = mt9t001_write(client, MT9T001_COLUMN_START, crop->left);
+	if (ret < 0)
+		return ret;
+
+	ret = mt9t001_write(client, MT9T001_ROW_START, crop->top);
+	if (ret < 0)
+		return ret;
+
+	ret = mt9t001_write(client, MT9T001_WINDOW_WIDTH, crop->width - 1);
+	if (ret < 0)
+		return ret;
+
+	ret = mt9t001_write(client, MT9T001_WINDOW_HEIGHT, crop->height - 1);
+	if (ret < 0)
+		return ret;
+
+	/* Switch to master "normal" mode */
+	return mt9t001_set_output_control(mt9t001, 0, mode);
+}
+
+static int mt9t001_enum_mbus_code(struct v4l2_subdev *subdev,
+				  struct v4l2_subdev_fh *fh,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index > 0)
+		return -EINVAL;
+
+	code->code = V4L2_MBUS_FMT_SGRBG10_1X10;
+	return 0;
+}
+
+static int mt9t001_enum_frame_size(struct v4l2_subdev *subdev,
+				   struct v4l2_subdev_fh *fh,
+				   struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->index >= 8 || fse->code != V4L2_MBUS_FMT_SGRBG10_1X10)
+		return -EINVAL;
+
+	fse->min_width = (MT9T001_WINDOW_WIDTH_DEF + 1) / fse->index;
+	fse->max_width = fse->min_width;
+	fse->min_height = (MT9T001_WINDOW_HEIGHT_DEF + 1) / fse->index;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static int mt9t001_get_format(struct v4l2_subdev *subdev,
+			      struct v4l2_subdev_fh *fh,
+			      struct v4l2_subdev_format *format)
+{
+	struct mt9t001 *mt9t001 = to_mt9t001(subdev);
+
+	format->format = *__mt9t001_get_pad_format(mt9t001, fh, format->pad,
+						   format->which);
+	return 0;
+}
+
+static int mt9t001_set_format(struct v4l2_subdev *subdev,
+			      struct v4l2_subdev_fh *fh,
+			      struct v4l2_subdev_format *format)
+{
+	struct mt9t001 *mt9t001 = to_mt9t001(subdev);
+	struct v4l2_mbus_framefmt *__format;
+	struct v4l2_rect *__crop;
+	unsigned int width;
+	unsigned int height;
+	unsigned int hratio;
+	unsigned int vratio;
+
+	__crop = __mt9t001_get_pad_crop(mt9t001, fh, 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(__crop->width / 8, MT9T001_WINDOW_HEIGHT_MIN + 1),
+			__crop->width);
+	height = clamp_t(unsigned int, ALIGN(format->format.height, 2),
+			 max(__crop->height / 8, MT9T001_WINDOW_HEIGHT_MIN + 1),
+			 __crop->height);
+
+	hratio = DIV_ROUND_CLOSEST(__crop->width, width);
+	vratio = DIV_ROUND_CLOSEST(__crop->height, height);
+
+	__format = __mt9t001_get_pad_format(mt9t001, fh, format->pad,
+					    format->which);
+	__format->width = __crop->width / hratio;
+	__format->height = __crop->height / vratio;
+
+	format->format = *__format;
+
+	return 0;
+}
+
+static int mt9t001_get_crop(struct v4l2_subdev *subdev,
+			    struct v4l2_subdev_fh *fh,
+			    struct v4l2_subdev_crop *crop)
+{
+	struct mt9t001 *mt9t001 = to_mt9t001(subdev);
+
+	crop->rect = *__mt9t001_get_pad_crop(mt9t001, fh, crop->pad,
+					     crop->which);
+	return 0;
+}
+
+static int mt9t001_set_crop(struct v4l2_subdev *subdev,
+			    struct v4l2_subdev_fh *fh,
+			    struct v4l2_subdev_crop *crop)
+{
+	struct mt9t001 *mt9t001 = to_mt9t001(subdev);
+	struct v4l2_mbus_framefmt *__format;
+	struct v4l2_rect *__crop;
+	struct v4l2_rect rect;
+
+	/* Clamp the crop rectangle boundaries and align them to a multiple of 2
+	 * pixels.
+	 */
+	rect.left = clamp(ALIGN(crop->rect.left, 2),
+			  MT9T001_COLUMN_START_MIN,
+			  MT9T001_COLUMN_START_MAX);
+	rect.top = clamp(ALIGN(crop->rect.top, 2),
+			 MT9T001_ROW_START_MIN,
+			 MT9T001_ROW_START_MAX);
+	rect.width = clamp(ALIGN(crop->rect.width, 2),
+			   MT9T001_WINDOW_WIDTH_MIN + 1,
+			   MT9T001_WINDOW_WIDTH_MAX + 1);
+	rect.height = clamp(ALIGN(crop->rect.height, 2),
+			    MT9T001_WINDOW_HEIGHT_MIN + 1,
+			    MT9T001_WINDOW_HEIGHT_MAX + 1);
+
+	rect.width = min(rect.width, MT9T001_PIXEL_ARRAY_WIDTH - rect.left);
+	rect.height = min(rect.height, MT9T001_PIXEL_ARRAY_HEIGHT - rect.top);
+
+	__crop = __mt9t001_get_pad_crop(mt9t001, fh, crop->pad, crop->which);
+
+	if (rect.width != __crop->width || rect.height != __crop->height) {
+		/* Reset the output image size if the crop rectangle size has
+		 * been modified.
+		 */
+		__format = __mt9t001_get_pad_format(mt9t001, fh, crop->pad,
+						    crop->which);
+		__format->width = rect.width;
+		__format->height = rect.height;
+	}
+
+	*__crop = rect;
+	crop->rect = rect;
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev control operations
+ */
+
+#define V4L2_CID_TEST_PATTERN		(V4L2_CID_USER_BASE | 0x1001)
+#define V4L2_CID_BLACK_LEVEL_AUTO	(V4L2_CID_USER_BASE | 0x1002)
+#define V4L2_CID_BLACK_LEVEL_OFFSET	(V4L2_CID_USER_BASE | 0x1003)
+#define V4L2_CID_BLACK_LEVEL_CALIBRATE	(V4L2_CID_USER_BASE | 0x1004)
+
+#define V4L2_CID_GAIN_RED		(V4L2_CTRL_CLASS_CAMERA | 0x1001)
+#define V4L2_CID_GAIN_GREEN_RED		(V4L2_CTRL_CLASS_CAMERA | 0x1002)
+#define V4L2_CID_GAIN_GREEN_BLUE	(V4L2_CTRL_CLASS_CAMERA | 0x1003)
+#define V4L2_CID_GAIN_BLUE		(V4L2_CTRL_CLASS_CAMERA | 0x1004)
+
+static u16 mt9t001_gain_value(s32 *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 (*gain <= 32)
+		return *gain;
+
+	if (*gain <= 64) {
+		*gain &= ~1;
+		return (1 << 6) | (*gain >> 1);
+	}
+
+	*gain &= ~7;
+	return ((*gain - 64) << 5) | (1 << 6) | 32;
+}
+
+static int mt9t001_ctrl_freeze(struct mt9t001 *mt9t001, bool freeze)
+{
+	return mt9t001_set_output_control(mt9t001,
+		freeze ? 0 : MT9T001_OUTPUT_CONTROL_SYNC,
+		freeze ? MT9T001_OUTPUT_CONTROL_SYNC : 0);
+}
+
+static int mt9t001_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	static const u8 gains[4] = {
+		MT9T001_RED_GAIN, MT9T001_GREEN1_GAIN,
+		MT9T001_GREEN2_GAIN, MT9T001_BLUE_GAIN
+	};
+
+	struct mt9t001 *mt9t001 =
+			container_of(ctrl->handler, struct mt9t001, ctrls);
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9t001->subdev);
+	unsigned int count;
+	unsigned int i;
+	u16 value;
+	int ret;
+
+	switch (ctrl->id) {
+	case V4L2_CID_GAIN_RED:
+	case V4L2_CID_GAIN_GREEN_RED:
+	case V4L2_CID_GAIN_GREEN_BLUE:
+	case V4L2_CID_GAIN_BLUE:
+
+		/* Disable control updates if more than one control has changed
+		 * in the cluster.
+		 */
+		for (i = 0, count = 0; i < 4; ++i) {
+			struct v4l2_ctrl *gain = mt9t001->gains[i];
+
+			if (gain->val != gain->cur.val)
+				count++;
+		}
+
+		if (count > 1) {
+			ret = mt9t001_ctrl_freeze(mt9t001, true);
+			if (ret < 0)
+				return ret;
+		}
+
+		/* Update the gain controls. */
+		for (i = 0; i < 4; ++i) {
+			struct v4l2_ctrl *gain = mt9t001->gains[i];
+
+			if (gain->val == gain->cur.val)
+				continue;
+
+			value = mt9t001_gain_value(&gain->val);
+			ret = mt9t001_write(client, gains[i], value);
+			if (ret < 0) {
+				mt9t001_ctrl_freeze(mt9t001, false);
+				return ret;
+			}
+		}
+
+		/* Enable control updates. */
+		if (count > 1) {
+			ret = mt9t001_ctrl_freeze(mt9t001, false);
+			if (ret < 0)
+				return ret;
+		}
+
+		break;
+
+	case V4L2_CID_EXPOSURE:
+		ret = mt9t001_write(client, MT9T001_SHUTTER_WIDTH_LOW,
+				    ctrl->val & 0xffff);
+		if (ret < 0)
+			return ret;
+
+		return mt9t001_write(client, MT9T001_SHUTTER_WIDTH_HIGH,
+				     ctrl->val >> 16);
+
+	case V4L2_CID_TEST_PATTERN:
+		ret = mt9t001_set_output_control(mt9t001,
+			ctrl->val ? 0 : MT9T001_OUTPUT_CONTROL_TEST_DATA,
+			ctrl->val ? MT9T001_OUTPUT_CONTROL_TEST_DATA : 0);
+		if (ret < 0)
+			return ret;
+
+		return mt9t001_write(client, MT9T001_TEST_DATA, ctrl->val << 2);
+
+	case V4L2_CID_BLACK_LEVEL_AUTO:
+		value = ctrl->val ? 0 : MT9T001_BLACK_LEVEL_OVERRIDE;
+		ret = mt9t001_write(client, MT9T001_BLACK_LEVEL_CALIBRATION,
+				    value);
+		if (ret < 0)
+			return ret;
+
+		mt9t001->black_level = value;
+		break;
+
+	case V4L2_CID_BLACK_LEVEL_OFFSET:
+		ret = mt9t001_write(client, MT9T001_GREEN1_OFFSET, ctrl->val);
+		if (ret < 0)
+			return ret;
+
+		ret = mt9t001_write(client, MT9T001_GREEN2_OFFSET, ctrl->val);
+		if (ret < 0)
+			return ret;
+
+		ret = mt9t001_write(client, MT9T001_RED_OFFSET, ctrl->val);
+		if (ret < 0)
+			return ret;
+
+		return mt9t001_write(client, MT9T001_BLUE_OFFSET, ctrl->val);
+
+	case V4L2_CID_BLACK_LEVEL_CALIBRATE:
+		return mt9t001_write(client, MT9T001_BLACK_LEVEL_CALIBRATION,
+				     MT9T001_BLACK_LEVEL_RECALCULATE |
+				     mt9t001->black_level);
+	}
+
+	return 0;
+}
+
+static struct v4l2_ctrl_ops mt9t001_ctrl_ops = {
+	.s_ctrl = mt9t001_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config mt9t001_ctrls[] = {
+	{
+		.ops		= &mt9t001_ctrl_ops,
+		.id		= V4L2_CID_TEST_PATTERN,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "Test pattern",
+		.min		= 0,
+		.max		= 1023,
+		.step		= 1,
+		.def		= 0,
+		.flags		= 0,
+	}, {
+		.ops		= &mt9t001_ctrl_ops,
+		.id		= V4L2_CID_BLACK_LEVEL_AUTO,
+		.type		= V4L2_CTRL_TYPE_BOOLEAN,
+		.name		= "Black Level, Auto",
+		.min		= 0,
+		.max		= 1,
+		.step		= 1,
+		.def		= 1,
+		.flags		= 0,
+	}, {
+		.ops		= &mt9t001_ctrl_ops,
+		.id		= V4L2_CID_BLACK_LEVEL_OFFSET,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "Black Level, Offset",
+		.min		= -256,
+		.max		= 255,
+		.step		= 1,
+		.def		= 32,
+		.flags		= 0,
+	}, {
+		.ops		= &mt9t001_ctrl_ops,
+		.id		= V4L2_CID_BLACK_LEVEL_CALIBRATE,
+		.type		= V4L2_CTRL_TYPE_BUTTON,
+		.name		= "Black Level, Calibrate",
+		.min		= 0,
+		.max		= 0,
+		.step		= 0,
+		.def		= 0,
+		.flags		= V4L2_CTRL_FLAG_WRITE_ONLY,
+	},
+};
+
+static const struct v4l2_ctrl_config mt9t001_gains[] = {
+	{
+		.ops		= &mt9t001_ctrl_ops,
+		.id		= V4L2_CID_GAIN_RED,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "Gain, Red",
+		.min		= MT9T001_GLOBAL_GAIN_MIN,
+		.max		= MT9T001_GLOBAL_GAIN_MAX,
+		.step		= 1,
+		.def		= MT9T001_GLOBAL_GAIN_MIN,
+		.flags		= 0,
+	}, {
+		.ops		= &mt9t001_ctrl_ops,
+		.id		= V4L2_CID_GAIN_GREEN_RED,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "Gain, Green (R)",
+		.min		= MT9T001_GLOBAL_GAIN_MIN,
+		.max		= MT9T001_GLOBAL_GAIN_MAX,
+		.step		= 1,
+		.def		= MT9T001_GLOBAL_GAIN_MIN,
+		.flags		= 0,
+	}, {
+		.ops		= &mt9t001_ctrl_ops,
+		.id		= V4L2_CID_GAIN_GREEN_BLUE,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "Gain, Green (B)",
+		.min		= MT9T001_GLOBAL_GAIN_MIN,
+		.max		= MT9T001_GLOBAL_GAIN_MAX,
+		.step		= 1,
+		.def		= MT9T001_GLOBAL_GAIN_MIN,
+		.flags		= 0,
+	}, {
+		.ops		= &mt9t001_ctrl_ops,
+		.id		= V4L2_CID_GAIN_BLUE,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "Gain, Blue",
+		.min		= MT9T001_GLOBAL_GAIN_MIN,
+		.max		= MT9T001_GLOBAL_GAIN_MAX,
+		.step		= 1,
+		.def		= MT9T001_GLOBAL_GAIN_MIN,
+		.flags		= 0,
+	},
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev internal operations
+ */
+
+static int mt9t001_open(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_mbus_framefmt *format;
+	struct v4l2_rect *crop;
+
+	crop = v4l2_subdev_get_try_crop(fh, 0);
+	crop->left = MT9T001_COLUMN_START_DEF;
+	crop->top = MT9T001_ROW_START_DEF;
+	crop->width = MT9T001_WINDOW_WIDTH_DEF + 1;
+	crop->height = MT9T001_WINDOW_HEIGHT_DEF + 1;
+
+	format = v4l2_subdev_get_try_format(fh, 0);
+	format->code = V4L2_MBUS_FMT_SGRBG10_1X10;
+	format->width = MT9T001_WINDOW_WIDTH_DEF + 1;
+	format->height = MT9T001_WINDOW_HEIGHT_DEF + 1;
+	format->field = V4L2_FIELD_NONE;
+	format->colorspace = V4L2_COLORSPACE_SRGB;
+
+	return 0;
+}
+
+static struct v4l2_subdev_video_ops mt9t001_subdev_video_ops = {
+	.s_stream = mt9t001_s_stream,
+};
+
+static struct v4l2_subdev_pad_ops mt9t001_subdev_pad_ops = {
+	.enum_mbus_code = mt9t001_enum_mbus_code,
+	.enum_frame_size = mt9t001_enum_frame_size,
+	.get_fmt = mt9t001_get_format,
+	.set_fmt = mt9t001_set_format,
+	.get_crop = mt9t001_get_crop,
+	.set_crop = mt9t001_set_crop,
+};
+
+static struct v4l2_subdev_ops mt9t001_subdev_ops = {
+	.video = &mt9t001_subdev_video_ops,
+	.pad = &mt9t001_subdev_pad_ops,
+};
+
+static struct v4l2_subdev_internal_ops mt9t001_subdev_internal_ops = {
+	.open = mt9t001_open,
+};
+
+static int mt9t001_video_probe(struct i2c_client *client)
+{
+	struct mt9t001_platform_data *pdata = client->dev.platform_data;
+	s32 data;
+	int ret;
+
+	dev_info(&client->dev, "Probing MT9T001 at address 0x%02x\n",
+		 client->addr);
+
+	/* Reset the chip and stop data read out */
+	ret = mt9t001_write(client, MT9T001_RESET, 1);
+	if (ret < 0)
+		return ret;
+
+	ret = mt9t001_write(client, MT9T001_RESET, 0);
+	if (ret < 0)
+		return ret;
+
+	ret  = mt9t001_write(client, MT9T001_OUTPUT_CONTROL, 0);
+	if (ret < 0)
+		return ret;
+
+	/* Configure the pixel clock polarity */
+	if (pdata->clk_pol) {
+		ret  = mt9t001_write(client, MT9T001_PIXEL_CLOCK,
+				     MT9T001_PIXEL_CLOCK_INVERT);
+		if (ret < 0)
+			return ret;
+	}
+
+	/* Read and check the sensor version */
+	data = mt9t001_read(client, MT9T001_CHIP_VERSION);
+	if (data != MT9T001_CHIP_ID) {
+		dev_err(&client->dev, "MT9T001 not detected, wrong version "
+			"0x%04x\n", data);
+		return -ENODEV;
+	}
+
+	dev_info(&client->dev, "MT9T001 detected at address 0x%02x\n",
+		 client->addr);
+
+	return ret;
+}
+
+static int mt9t001_probe(struct i2c_client *client,
+			 const struct i2c_device_id *did)
+{
+	struct mt9t001_platform_data *pdata = client->dev.platform_data;
+	struct mt9t001 *mt9t001;
+	unsigned int i;
+	int ret;
+
+	if (pdata == NULL) {
+		dev_err(&client->dev, "No platform data\n");
+		return -EINVAL;
+	}
+
+	if (!i2c_check_functionality(client->adapter,
+				     I2C_FUNC_SMBUS_WORD_DATA)) {
+		dev_warn(&client->adapter->dev,
+			 "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
+		return -EIO;
+	}
+
+	ret = mt9t001_video_probe(client);
+	if (ret < 0)
+		return ret;
+
+	mt9t001 = kzalloc(sizeof(*mt9t001), GFP_KERNEL);
+	if (!mt9t001)
+		return -ENOMEM;
+
+	v4l2_ctrl_handler_init(&mt9t001->ctrls, ARRAY_SIZE(mt9t001_ctrls) +
+						ARRAY_SIZE(mt9t001_gains) + 3);
+
+	v4l2_ctrl_new_std(&mt9t001->ctrls, &mt9t001_ctrl_ops,
+			  V4L2_CID_EXPOSURE, MT9T001_SHUTTER_WIDTH_MIN,
+			  MT9T001_SHUTTER_WIDTH_MAX, 1,
+			  MT9T001_SHUTTER_WIDTH_DEF);
+	v4l2_ctrl_new_std(&mt9t001->ctrls, &mt9t001_ctrl_ops,
+			  V4L2_CID_BLACK_LEVEL, 1, 1, 1, 1);
+	v4l2_ctrl_new_std(&mt9t001->ctrls, &mt9t001_ctrl_ops,
+			  V4L2_CID_PIXEL_RATE, pdata->ext_clk, pdata->ext_clk,
+			  1, pdata->ext_clk);
+
+	for (i = 0; i < ARRAY_SIZE(mt9t001_ctrls); ++i)
+		v4l2_ctrl_new_custom(&mt9t001->ctrls, &mt9t001_ctrls[i], NULL);
+
+	for (i = 0; i < ARRAY_SIZE(mt9t001_gains); ++i)
+		mt9t001->gains[i] = v4l2_ctrl_new_custom(&mt9t001->ctrls,
+			&mt9t001_gains[i], NULL);
+
+	v4l2_ctrl_cluster(ARRAY_SIZE(mt9t001_gains), mt9t001->gains);
+
+	mt9t001->subdev.ctrl_handler = &mt9t001->ctrls;
+
+	if (mt9t001->ctrls.error) {
+		printk(KERN_INFO "%s: control initialization error %d\n",
+		       __func__, mt9t001->ctrls.error);
+		ret = -EINVAL;
+		goto done;
+	}
+
+	mt9t001->crop.left = MT9T001_COLUMN_START_DEF;
+	mt9t001->crop.top = MT9T001_ROW_START_DEF;
+	mt9t001->crop.width = MT9T001_WINDOW_WIDTH_DEF + 1;
+	mt9t001->crop.height = MT9T001_WINDOW_HEIGHT_DEF + 1;
+
+	mt9t001->format.code = V4L2_MBUS_FMT_SGRBG10_1X10;
+	mt9t001->format.width = MT9T001_WINDOW_WIDTH_DEF + 1;
+	mt9t001->format.height = MT9T001_WINDOW_HEIGHT_DEF + 1;
+	mt9t001->format.field = V4L2_FIELD_NONE;
+	mt9t001->format.colorspace = V4L2_COLORSPACE_SRGB;
+
+	v4l2_i2c_subdev_init(&mt9t001->subdev, client, &mt9t001_subdev_ops);
+	mt9t001->subdev.internal_ops = &mt9t001_subdev_internal_ops;
+	mt9t001->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	mt9t001->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_init(&mt9t001->subdev.entity, 1, &mt9t001->pad, 0);
+
+done:
+	if (ret < 0) {
+		v4l2_ctrl_handler_free(&mt9t001->ctrls);
+		media_entity_cleanup(&mt9t001->subdev.entity);
+		kfree(mt9t001);
+	}
+
+	return ret;
+}
+
+static int mt9t001_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct mt9t001 *mt9t001 = to_mt9t001(subdev);
+
+	v4l2_ctrl_handler_free(&mt9t001->ctrls);
+	v4l2_device_unregister_subdev(subdev);
+	media_entity_cleanup(&subdev->entity);
+	kfree(mt9t001);
+	return 0;
+}
+
+static const struct i2c_device_id mt9t001_id[] = {
+	{ "mt9t001", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, mt9t001_id);
+
+static struct i2c_driver mt9t001_driver = {
+	.driver = {
+		.name = "mt9t001",
+	},
+	.probe		= mt9t001_probe,
+	.remove		= mt9t001_remove,
+	.id_table	= mt9t001_id,
+};
+
+module_i2c_driver(mt9t001_driver);
+
+MODULE_DESCRIPTION("Aptina (Micron) MT9T001 Camera driver");
+MODULE_AUTHOR("Laurent Pinchart <laurent.pinchart@ideasonboard.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/mt9v011.c b/drivers/media/i2c/mt9v011.c
new file mode 100644
index 000000000000..6bf01ad62765
--- /dev/null
+++ b/drivers/media/i2c/mt9v011.c
@@ -0,0 +1,712 @@
+/*
+ * mt9v011 -Micron 1/4-Inch VGA Digital Image Sensor
+ *
+ * Copyright (c) 2009 Mauro Carvalho Chehab (mchehab@redhat.com)
+ * This code is placed under the terms of the GNU General Public License v2
+ */
+
+#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-chip-ident.h>
+#include <media/mt9v011.h>
+
+MODULE_DESCRIPTION("Micron mt9v011 sensor driver");
+MODULE_AUTHOR("Mauro Carvalho Chehab <mchehab@redhat.com>");
+MODULE_LICENSE("GPL");
+
+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
+
+/* supported controls */
+static struct v4l2_queryctrl mt9v011_qctrl[] = {
+	{
+		.id = V4L2_CID_GAIN,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Gain",
+		.minimum = 0,
+		.maximum = (1 << 12) - 1 - 0x0020,
+		.step = 1,
+		.default_value = 0x0020,
+		.flags = 0,
+	}, {
+		.id = V4L2_CID_EXPOSURE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Exposure",
+		.minimum = 0,
+		.maximum = 2047,
+		.step = 1,
+		.default_value = 0x01fc,
+		.flags = 0,
+	}, {
+		.id = V4L2_CID_RED_BALANCE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Red Balance",
+		.minimum = -1 << 9,
+		.maximum = (1 << 9) - 1,
+		.step = 1,
+		.default_value = 0,
+		.flags = 0,
+	}, {
+		.id = V4L2_CID_BLUE_BALANCE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Blue Balance",
+		.minimum = -1 << 9,
+		.maximum = (1 << 9) - 1,
+		.step = 1,
+		.default_value = 0,
+		.flags = 0,
+	}, {
+		.id      = V4L2_CID_HFLIP,
+		.type    = V4L2_CTRL_TYPE_BOOLEAN,
+		.name    = "Mirror",
+		.minimum = 0,
+		.maximum = 1,
+		.step    = 1,
+		.default_value = 0,
+		.flags = 0,
+	}, {
+		.id      = V4L2_CID_VFLIP,
+		.type    = V4L2_CTRL_TYPE_BOOLEAN,
+		.name    = "Vflip",
+		.minimum = 0,
+		.maximum = 1,
+		.step    = 1,
+		.default_value = 0,
+		.flags = 0,
+	}, {
+	}
+};
+
+struct mt9v011 {
+	struct v4l2_subdev sd;
+	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_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+	struct mt9v011 *core = to_mt9v011(sd);
+
+	v4l2_dbg(1, debug, sd, "g_ctrl called\n");
+
+	switch (ctrl->id) {
+	case V4L2_CID_GAIN:
+		ctrl->value = core->global_gain;
+		return 0;
+	case V4L2_CID_EXPOSURE:
+		ctrl->value = core->exposure;
+		return 0;
+	case V4L2_CID_RED_BALANCE:
+		ctrl->value = core->red_bal;
+		return 0;
+	case V4L2_CID_BLUE_BALANCE:
+		ctrl->value = core->blue_bal;
+		return 0;
+	case V4L2_CID_HFLIP:
+		ctrl->value = core->hflip ? 1 : 0;
+		return 0;
+	case V4L2_CID_VFLIP:
+		ctrl->value = core->vflip ? 1 : 0;
+		return 0;
+	}
+	return -EINVAL;
+}
+
+static int mt9v011_queryctrl(struct v4l2_subdev *sd, struct v4l2_queryctrl *qc)
+{
+	int i;
+
+	v4l2_dbg(1, debug, sd, "queryctrl called\n");
+
+	for (i = 0; i < ARRAY_SIZE(mt9v011_qctrl); i++)
+		if (qc->id && qc->id == mt9v011_qctrl[i].id) {
+			memcpy(qc, &(mt9v011_qctrl[i]),
+			       sizeof(*qc));
+			return 0;
+		}
+
+	return -EINVAL;
+}
+
+
+static int mt9v011_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+	struct mt9v011 *core = to_mt9v011(sd);
+	u8 i, n;
+	n = ARRAY_SIZE(mt9v011_qctrl);
+
+	for (i = 0; i < n; i++) {
+		if (ctrl->id != mt9v011_qctrl[i].id)
+			continue;
+		if (ctrl->value < mt9v011_qctrl[i].minimum ||
+		    ctrl->value > mt9v011_qctrl[i].maximum)
+			return -ERANGE;
+		v4l2_dbg(1, debug, sd, "s_ctrl: id=%d, value=%d\n",
+					ctrl->id, ctrl->value);
+		break;
+	}
+
+	switch (ctrl->id) {
+	case V4L2_CID_GAIN:
+		core->global_gain = ctrl->value;
+		break;
+	case V4L2_CID_EXPOSURE:
+		core->exposure = ctrl->value;
+		break;
+	case V4L2_CID_RED_BALANCE:
+		core->red_bal = ctrl->value;
+		break;
+	case V4L2_CID_BLUE_BALANCE:
+		core->blue_bal = ctrl->value;
+		break;
+	case V4L2_CID_HFLIP:
+		core->hflip = ctrl->value;
+		set_read_mode(sd);
+		return 0;
+	case V4L2_CID_VFLIP:
+		core->vflip = ctrl->value;
+		set_read_mode(sd);
+		return 0;
+	default:
+		return -EINVAL;
+	}
+
+	set_balance(sd);
+
+	return 0;
+}
+
+static int mt9v011_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned index,
+					enum v4l2_mbus_pixelcode *code)
+{
+	if (index > 0)
+		return -EINVAL;
+
+	*code = V4L2_MBUS_FMT_SGRBG8_1X8;
+	return 0;
+}
+
+static int mt9v011_try_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *fmt)
+{
+	if (fmt->code != V4L2_MBUS_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;
+
+	return 0;
+}
+
+static int mt9v011_g_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *parms)
+{
+	struct v4l2_captureparm *cp = &parms->parm.capture;
+
+	if (parms->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	memset(cp, 0, sizeof(struct v4l2_captureparm));
+	cp->capability = V4L2_CAP_TIMEPERFRAME;
+	calc_fps(sd,
+		 &cp->timeperframe.numerator,
+		 &cp->timeperframe.denominator);
+
+	return 0;
+}
+
+static int mt9v011_s_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *parms)
+{
+	struct v4l2_captureparm *cp = &parms->parm.capture;
+	struct v4l2_fract *tpf = &cp->timeperframe;
+	u16 speed;
+
+	if (parms->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	if (cp->extendedmode != 0)
+		return -EINVAL;
+
+	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;
+}
+
+static int mt9v011_s_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *fmt)
+{
+	struct mt9v011 *core = to_mt9v011(sd);
+	int rc;
+
+	rc = mt9v011_try_mbus_fmt(sd, fmt);
+	if (rc < 0)
+		return -EINVAL;
+
+	core->width = fmt->width;
+	core->height = fmt->height;
+
+	set_res(sd);
+
+	return 0;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int mt9v011_g_register(struct v4l2_subdev *sd,
+			      struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+
+	reg->val = mt9v011_read(sd, reg->reg & 0xff);
+	reg->size = 2;
+
+	return 0;
+}
+
+static int mt9v011_s_register(struct v4l2_subdev *sd,
+			      struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+
+	mt9v011_write(sd, reg->reg & 0xff, reg->val & 0xffff);
+
+	return 0;
+}
+#endif
+
+static int mt9v011_g_chip_ident(struct v4l2_subdev *sd,
+				struct v4l2_dbg_chip_ident *chip)
+{
+	u16 version;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	version = mt9v011_read(sd, R00_MT9V011_CHIP_VERSION);
+
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_MT9V011,
+					  version);
+}
+
+static const struct v4l2_subdev_core_ops mt9v011_core_ops = {
+	.queryctrl = mt9v011_queryctrl,
+	.g_ctrl = mt9v011_g_ctrl,
+	.s_ctrl = mt9v011_s_ctrl,
+	.reset = mt9v011_reset,
+	.g_chip_ident = mt9v011_g_chip_ident,
+#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 = {
+	.enum_mbus_fmt = mt9v011_enum_mbus_fmt,
+	.try_mbus_fmt = mt9v011_try_mbus_fmt,
+	.s_mbus_fmt = mt9v011_s_mbus_fmt,
+	.g_parm = mt9v011_g_parm,
+	.s_parm = mt9v011_s_parm,
+};
+
+static const struct v4l2_subdev_ops mt9v011_ops = {
+	.core  = &mt9v011_core_ops,
+	.video = &mt9v011_video_ops,
+};
+
+
+/****************************************************************************
+			I2C Client & Driver
+ ****************************************************************************/
+
+static int mt9v011_probe(struct i2c_client *c,
+			 const struct i2c_device_id *id)
+{
+	u16 version;
+	struct mt9v011 *core;
+	struct v4l2_subdev *sd;
+
+	/* 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 = kzalloc(sizeof(struct mt9v011), GFP_KERNEL);
+	if (!core)
+		return -ENOMEM;
+
+	sd = &core->sd;
+	v4l2_i2c_subdev_init(sd, c, &mt9v011_ops);
+
+	/* 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);
+		kfree(core);
+		return -EINVAL;
+	}
+
+	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 int mt9v011_remove(struct i2c_client *c)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(c);
+
+	v4l2_dbg(1, debug, sd,
+		"mt9v011.c: removing mt9v011 adapter on address 0x%x\n",
+		c->addr << 1);
+
+	v4l2_device_unregister_subdev(sd);
+	kfree(to_mt9v011(sd));
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id mt9v011_id[] = {
+	{ "mt9v011", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, mt9v011_id);
+
+static struct i2c_driver mt9v011_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.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 000000000000..e2177405dad2
--- /dev/null
+++ b/drivers/media/i2c/mt9v032.c
@@ -0,0 +1,853 @@
+/*
+ * Driver for MT9V032 CMOS Image Sensor from Micron
+ *
+ * Copyright (C) 2010, Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *
+ * Based on the MT9M001 driver,
+ *
+ * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/log2.h>
+#include <linux/mutex.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <linux/v4l2-mediabus.h>
+#include <linux/module.h>
+
+#include <media/mt9v032.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+
+#define MT9V032_PIXEL_ARRAY_HEIGHT			492
+#define MT9V032_PIXEL_ARRAY_WIDTH			782
+
+#define MT9V032_SYSCLK_FREQ_DEF				26600000
+
+#define MT9V032_CHIP_VERSION				0x00
+#define		MT9V032_CHIP_ID_REV1			0x1311
+#define		MT9V032_CHIP_ID_REV3			0x1313
+#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		MT9V032_HORIZONTAL_BLANKING_DEF		94
+#define		MT9V032_HORIZONTAL_BLANKING_MAX		1023
+#define MT9V032_VERTICAL_BLANKING			0x06
+#define		MT9V032_VERTICAL_BLANKING_MIN		4
+#define		MT9V032_VERTICAL_BLANKING_DEF		45
+#define		MT9V032_VERTICAL_BLANKING_MAX		3000
+#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		MT9V032_TOTAL_SHUTTER_WIDTH_DEF		480
+#define		MT9V032_TOTAL_SHUTTER_WIDTH_MAX		32767
+#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_PIXEL_OPERATION_MODE			0x0f
+#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		MT9V032_ROW_NOISE_CORR_ENABLE		(1 << 5)
+#define		MT9V032_ROW_NOISE_CORR_USE_BLK_AVG	(1 << 7)
+#define MT9V032_PIXEL_CLOCK				0x74
+#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_AEC_AGC_ENABLE				0xaf
+#define		MT9V032_AEC_ENABLE			(1 << 0)
+#define		MT9V032_AGC_ENABLE			(1 << 1)
+#define MT9V032_THERMAL_INFO				0xc1
+
+struct mt9v032 {
+	struct v4l2_subdev subdev;
+	struct media_pad pad;
+
+	struct v4l2_mbus_framefmt format;
+	struct v4l2_rect crop;
+
+	struct v4l2_ctrl_handler ctrls;
+	struct {
+		struct v4l2_ctrl *link_freq;
+		struct v4l2_ctrl *pixel_rate;
+	};
+
+	struct mutex power_lock;
+	int power_count;
+
+	struct mt9v032_platform_data *pdata;
+
+	u32 sysclk;
+	u16 chip_control;
+	u16 aec_agc;
+	u16 hblank;
+};
+
+static struct mt9v032 *to_mt9v032(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct mt9v032, subdev);
+}
+
+static int mt9v032_read(struct i2c_client *client, const u8 reg)
+{
+	s32 data = i2c_smbus_read_word_swapped(client, reg);
+	dev_dbg(&client->dev, "%s: read 0x%04x from 0x%02x\n", __func__,
+		data, reg);
+	return data;
+}
+
+static int mt9v032_write(struct i2c_client *client, const u8 reg,
+			 const u16 data)
+{
+	dev_dbg(&client->dev, "%s: writing 0x%04x to 0x%02x\n", __func__,
+		data, reg);
+	return i2c_smbus_write_word_swapped(client, reg, data);
+}
+
+static int mt9v032_set_chip_control(struct mt9v032 *mt9v032, u16 clear, u16 set)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9v032->subdev);
+	u16 value = (mt9v032->chip_control & ~clear) | set;
+	int ret;
+
+	ret = mt9v032_write(client, MT9V032_CHIP_CONTROL, value);
+	if (ret < 0)
+		return ret;
+
+	mt9v032->chip_control = value;
+	return 0;
+}
+
+static int
+mt9v032_update_aec_agc(struct mt9v032 *mt9v032, u16 which, int enable)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9v032->subdev);
+	u16 value = mt9v032->aec_agc;
+	int ret;
+
+	if (enable)
+		value |= which;
+	else
+		value &= ~which;
+
+	ret = mt9v032_write(client, 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 i2c_client *client = v4l2_get_subdevdata(&mt9v032->subdev);
+	struct v4l2_rect *crop = &mt9v032->crop;
+
+	return mt9v032_write(client, MT9V032_HORIZONTAL_BLANKING,
+			     max_t(s32, mt9v032->hblank, 660 - crop->width));
+}
+
+#define EXT_CLK		25000000
+
+static int mt9v032_power_on(struct mt9v032 *mt9v032)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9v032->subdev);
+	int ret;
+
+	if (mt9v032->pdata->set_clock) {
+		mt9v032->pdata->set_clock(&mt9v032->subdev, mt9v032->sysclk);
+		udelay(1);
+	}
+
+	/* Reset the chip and stop data read out */
+	ret = mt9v032_write(client, MT9V032_RESET, 1);
+	if (ret < 0)
+		return ret;
+
+	ret = mt9v032_write(client, MT9V032_RESET, 0);
+	if (ret < 0)
+		return ret;
+
+	return mt9v032_write(client, MT9V032_CHIP_CONTROL, 0);
+}
+
+static void mt9v032_power_off(struct mt9v032 *mt9v032)
+{
+	if (mt9v032->pdata->set_clock)
+		mt9v032->pdata->set_clock(&mt9v032->subdev, 0);
+}
+
+static int __mt9v032_set_power(struct mt9v032 *mt9v032, bool on)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9v032->subdev);
+	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 = mt9v032_write(client, MT9V032_PIXEL_CLOCK,
+				MT9V032_PIXEL_CLOCK_INV_PXL_CLK);
+		if (ret < 0)
+			return ret;
+	}
+
+	/* Disable the noise correction algorithm and restore the controls. */
+	ret = mt9v032_write(client, 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_fh *fh,
+			 unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_format(fh, 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_fh *fh,
+		       unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_crop(fh, 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_MASTER_MODE
+		       | MT9V032_CHIP_CONTROL_DOUT_ENABLE
+		       | MT9V032_CHIP_CONTROL_SEQUENTIAL;
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
+	struct mt9v032 *mt9v032 = to_mt9v032(subdev);
+	struct v4l2_mbus_framefmt *format = &mt9v032->format;
+	struct v4l2_rect *crop = &mt9v032->crop;
+	unsigned int hratio;
+	unsigned int vratio;
+	int ret;
+
+	if (!enable)
+		return mt9v032_set_chip_control(mt9v032, mode, 0);
+
+	/* Configure the window size and row/column bin */
+	hratio = DIV_ROUND_CLOSEST(crop->width, format->width);
+	vratio = DIV_ROUND_CLOSEST(crop->height, format->height);
+
+	ret = mt9v032_write(client, MT9V032_READ_MODE,
+		    (hratio - 1) << MT9V032_READ_MODE_ROW_BIN_SHIFT |
+		    (vratio - 1) << MT9V032_READ_MODE_COLUMN_BIN_SHIFT);
+	if (ret < 0)
+		return ret;
+
+	ret = mt9v032_write(client, MT9V032_COLUMN_START, crop->left);
+	if (ret < 0)
+		return ret;
+
+	ret = mt9v032_write(client, MT9V032_ROW_START, crop->top);
+	if (ret < 0)
+		return ret;
+
+	ret = mt9v032_write(client, MT9V032_WINDOW_WIDTH, crop->width);
+	if (ret < 0)
+		return ret;
+
+	ret = mt9v032_write(client, 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 mt9v032_set_chip_control(mt9v032, 0, mode);
+}
+
+static int mt9v032_enum_mbus_code(struct v4l2_subdev *subdev,
+				  struct v4l2_subdev_fh *fh,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index > 0)
+		return -EINVAL;
+
+	code->code = V4L2_MBUS_FMT_SGRBG10_1X10;
+	return 0;
+}
+
+static int mt9v032_enum_frame_size(struct v4l2_subdev *subdev,
+				   struct v4l2_subdev_fh *fh,
+				   struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->index >= 8 || fse->code != V4L2_MBUS_FMT_SGRBG10_1X10)
+		return -EINVAL;
+
+	fse->min_width = MT9V032_WINDOW_WIDTH_DEF / fse->index;
+	fse->max_width = fse->min_width;
+	fse->min_height = MT9V032_WINDOW_HEIGHT_DEF / fse->index;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static int mt9v032_get_format(struct v4l2_subdev *subdev,
+			      struct v4l2_subdev_fh *fh,
+			      struct v4l2_subdev_format *format)
+{
+	struct mt9v032 *mt9v032 = to_mt9v032(subdev);
+
+	format->format = *__mt9v032_get_pad_format(mt9v032, fh, format->pad,
+						   format->which);
+	return 0;
+}
+
+static void mt9v032_configure_pixel_rate(struct mt9v032 *mt9v032,
+					 unsigned int hratio)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9v032->subdev);
+	int ret;
+
+	ret = v4l2_ctrl_s_ctrl_int64(mt9v032->pixel_rate,
+				     mt9v032->sysclk / hratio);
+	if (ret < 0)
+		dev_warn(&client->dev, "failed to set pixel rate (%d)\n", ret);
+}
+
+static int mt9v032_set_format(struct v4l2_subdev *subdev,
+			      struct v4l2_subdev_fh *fh,
+			      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, fh, 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(__crop->width / 8, MT9V032_WINDOW_WIDTH_MIN),
+			__crop->width);
+	height = clamp_t(unsigned int, ALIGN(format->format.height, 2),
+			 max(__crop->height / 8, MT9V032_WINDOW_HEIGHT_MIN),
+			 __crop->height);
+
+	hratio = DIV_ROUND_CLOSEST(__crop->width, width);
+	vratio = DIV_ROUND_CLOSEST(__crop->height, height);
+
+	__format = __mt9v032_get_pad_format(mt9v032, fh, format->pad,
+					    format->which);
+	__format->width = __crop->width / hratio;
+	__format->height = __crop->height / vratio;
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		mt9v032_configure_pixel_rate(mt9v032, hratio);
+
+	format->format = *__format;
+
+	return 0;
+}
+
+static int mt9v032_get_crop(struct v4l2_subdev *subdev,
+			    struct v4l2_subdev_fh *fh,
+			    struct v4l2_subdev_crop *crop)
+{
+	struct mt9v032 *mt9v032 = to_mt9v032(subdev);
+
+	crop->rect = *__mt9v032_get_pad_crop(mt9v032, fh, crop->pad,
+					     crop->which);
+	return 0;
+}
+
+static int mt9v032_set_crop(struct v4l2_subdev *subdev,
+			    struct v4l2_subdev_fh *fh,
+			    struct v4l2_subdev_crop *crop)
+{
+	struct mt9v032 *mt9v032 = to_mt9v032(subdev);
+	struct v4l2_mbus_framefmt *__format;
+	struct v4l2_rect *__crop;
+	struct v4l2_rect rect;
+
+	/* 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(crop->rect.left + 1, 2) - 1,
+			  MT9V032_COLUMN_START_MIN,
+			  MT9V032_COLUMN_START_MAX);
+	rect.top = clamp(ALIGN(crop->rect.top + 1, 2) - 1,
+			 MT9V032_ROW_START_MIN,
+			 MT9V032_ROW_START_MAX);
+	rect.width = clamp(ALIGN(crop->rect.width, 2),
+			   MT9V032_WINDOW_WIDTH_MIN,
+			   MT9V032_WINDOW_WIDTH_MAX);
+	rect.height = clamp(ALIGN(crop->rect.height, 2),
+			    MT9V032_WINDOW_HEIGHT_MIN,
+			    MT9V032_WINDOW_HEIGHT_MAX);
+
+	rect.width = min(rect.width, MT9V032_PIXEL_ARRAY_WIDTH - rect.left);
+	rect.height = min(rect.height, MT9V032_PIXEL_ARRAY_HEIGHT - rect.top);
+
+	__crop = __mt9v032_get_pad_crop(mt9v032, fh, crop->pad, crop->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, fh, crop->pad,
+						    crop->which);
+		__format->width = rect.width;
+		__format->height = rect.height;
+		if (crop->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+			mt9v032_configure_pixel_rate(mt9v032, 1);
+	}
+
+	*__crop = rect;
+	crop->rect = rect;
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev control operations
+ */
+
+#define V4L2_CID_TEST_PATTERN		(V4L2_CID_USER_BASE | 0x1001)
+
+static int mt9v032_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct mt9v032 *mt9v032 =
+			container_of(ctrl->handler, struct mt9v032, ctrls);
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9v032->subdev);
+	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 mt9v032_write(client, 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 mt9v032_write(client, MT9V032_TOTAL_SHUTTER_WIDTH,
+				     ctrl->val);
+
+	case V4L2_CID_HBLANK:
+		mt9v032->hblank = ctrl->val;
+		return mt9v032_update_hblank(mt9v032);
+
+	case V4L2_CID_VBLANK:
+		return mt9v032_write(client, 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->val64 = freq;
+		mt9v032->sysclk = freq;
+		break;
+
+	case V4L2_CID_TEST_PATTERN:
+		switch (ctrl->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 = (ctrl->val << MT9V032_TEST_PATTERN_DATA_SHIFT)
+			     | MT9V032_TEST_PATTERN_USE_DATA
+			     | MT9V032_TEST_PATTERN_ENABLE
+			     | MT9V032_TEST_PATTERN_FLIP;
+			break;
+		}
+
+		return mt9v032_write(client, MT9V032_TEST_PATTERN, data);
+	}
+
+	return 0;
+}
+
+static struct v4l2_ctrl_ops mt9v032_ctrl_ops = {
+	.s_ctrl = mt9v032_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config mt9v032_ctrls[] = {
+	{
+		.ops		= &mt9v032_ctrl_ops,
+		.id		= V4L2_CID_TEST_PATTERN,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "Test pattern",
+		.min		= 0,
+		.max		= 1023,
+		.step		= 1,
+		.def		= 0,
+		.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);
+	s32 data;
+	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 */
+	data = mt9v032_read(client, MT9V032_CHIP_VERSION);
+	if (data != MT9V032_CHIP_ID_REV1 && data != MT9V032_CHIP_ID_REV3) {
+		dev_err(&client->dev, "MT9V032 not detected, wrong version "
+				"0x%04x\n", data);
+		return -ENODEV;
+	}
+
+	mt9v032_power_off(mt9v032);
+
+	dev_info(&client->dev, "MT9V032 detected at address 0x%02x\n",
+			client->addr);
+
+	mt9v032_configure_pixel_rate(mt9v032, 1);
+
+	return ret;
+}
+
+static int mt9v032_open(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_mbus_framefmt *format;
+	struct v4l2_rect *crop;
+
+	crop = v4l2_subdev_get_try_crop(fh, 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(fh, 0);
+	format->code = V4L2_MBUS_FMT_SGRBG10_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 struct v4l2_subdev_core_ops mt9v032_subdev_core_ops = {
+	.s_power	= mt9v032_set_power,
+};
+
+static struct v4l2_subdev_video_ops mt9v032_subdev_video_ops = {
+	.s_stream	= mt9v032_s_stream,
+};
+
+static 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_crop = mt9v032_get_crop,
+	.set_crop = mt9v032_set_crop,
+};
+
+static 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,
+};
+
+/* -----------------------------------------------------------------------------
+ * Driver initialization and probing
+ */
+
+static int mt9v032_probe(struct i2c_client *client,
+		const struct i2c_device_id *did)
+{
+	struct mt9v032_platform_data *pdata = client->dev.platform_data;
+	struct mt9v032 *mt9v032;
+	unsigned int i;
+	int ret;
+
+	if (!i2c_check_functionality(client->adapter,
+				     I2C_FUNC_SMBUS_WORD_DATA)) {
+		dev_warn(&client->adapter->dev,
+			 "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
+		return -EIO;
+	}
+
+	mt9v032 = kzalloc(sizeof(*mt9v032), GFP_KERNEL);
+	if (!mt9v032)
+		return -ENOMEM;
+
+	mutex_init(&mt9v032->power_lock);
+	mt9v032->pdata = pdata;
+
+	v4l2_ctrl_handler_init(&mt9v032->ctrls, ARRAY_SIZE(mt9v032_ctrls) + 8);
+
+	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_TOTAL_SHUTTER_WIDTH_MIN,
+			  MT9V032_TOTAL_SHUTTER_WIDTH_MAX, 1,
+			  MT9V032_TOTAL_SHUTTER_WIDTH_DEF);
+	v4l2_ctrl_new_std(&mt9v032->ctrls, &mt9v032_ctrl_ops,
+			  V4L2_CID_HBLANK, MT9V032_HORIZONTAL_BLANKING_MIN,
+			  MT9V032_HORIZONTAL_BLANKING_MAX, 1,
+			  MT9V032_HORIZONTAL_BLANKING_DEF);
+	v4l2_ctrl_new_std(&mt9v032->ctrls, &mt9v032_ctrl_ops,
+			  V4L2_CID_VBLANK, MT9V032_VERTICAL_BLANKING_MIN,
+			  MT9V032_VERTICAL_BLANKING_MAX, 1,
+			  MT9V032_VERTICAL_BLANKING_DEF);
+
+	mt9v032->pixel_rate =
+		v4l2_ctrl_new_std(&mt9v032->ctrls, &mt9v032_ctrl_ops,
+				  V4L2_CID_PIXEL_RATE, 0, 0, 1, 0);
+
+	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);
+	}
+
+	for (i = 0; i < ARRAY_SIZE(mt9v032_ctrls); ++i)
+		v4l2_ctrl_new_custom(&mt9v032->ctrls, &mt9v032_ctrls[i], NULL);
+
+	mt9v032->subdev.ctrl_handler = &mt9v032->ctrls;
+
+	if (mt9v032->ctrls.error)
+		printk(KERN_INFO "%s: control initialization error %d\n",
+		       __func__, mt9v032->ctrls.error);
+
+	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;
+
+	mt9v032->format.code = V4L2_MBUS_FMT_SGRBG10_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->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->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_init(&mt9v032->subdev.entity, 1, &mt9v032->pad, 0);
+	if (ret < 0)
+		kfree(mt9v032);
+
+	return ret;
+}
+
+static int mt9v032_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct mt9v032 *mt9v032 = to_mt9v032(subdev);
+
+	v4l2_device_unregister_subdev(subdev);
+	media_entity_cleanup(&subdev->entity);
+	kfree(mt9v032);
+	return 0;
+}
+
+static const struct i2c_device_id mt9v032_id[] = {
+	{ "mt9v032", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, mt9v032_id);
+
+static struct i2c_driver mt9v032_driver = {
+	.driver = {
+		.name = "mt9v032",
+	},
+	.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/noon010pc30.c b/drivers/media/i2c/noon010pc30.c
new file mode 100644
index 000000000000..440c12962bae
--- /dev/null
+++ b/drivers/media/i2c/noon010pc30.c
@@ -0,0 +1,851 @@
+/*
+ * Driver for SiliconFile NOON010PC30 CIF (1/11") Image Sensor with ISP
+ *
+ * Copyright (C) 2010 - 2011 Samsung Electronics Co., Ltd.
+ * Contact: Sylwester Nawrocki, <s.nawrocki@samsung.com>
+ *
+ * Initial register configuration based on a driver authored by
+ * HeungJun Kim <riverful.kim@samsung.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; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/regulator/consumer.h>
+#include <media/noon010pc30.h>
+#include <media/v4l2-chip-ident.h>
+#include <linux/videodev2.h>
+#include <linux/module.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-mediabus.h>
+#include <media/v4l2-subdev.h>
+
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "Enable module debug trace. Set to 1 to enable.");
+
+#define MODULE_NAME		"NOON010PC30"
+
+/*
+ * Register offsets within a page
+ * b15..b8 - page id, b7..b0 - register address
+ */
+#define POWER_CTRL_REG		0x0001
+#define PAGEMODE_REG		0x03
+#define DEVICE_ID_REG		0x0004
+#define NOON010PC30_ID		0x86
+#define VDO_CTL_REG(n)		(0x0010 + (n))
+#define SYNC_CTL_REG		0x0012
+/* Window size and position */
+#define WIN_ROWH_REG		0x0013
+#define WIN_ROWL_REG		0x0014
+#define WIN_COLH_REG		0x0015
+#define WIN_COLL_REG		0x0016
+#define WIN_HEIGHTH_REG		0x0017
+#define WIN_HEIGHTL_REG		0x0018
+#define WIN_WIDTHH_REG		0x0019
+#define WIN_WIDTHL_REG		0x001A
+#define HBLANKH_REG		0x001B
+#define HBLANKL_REG		0x001C
+#define VSYNCH_REG		0x001D
+#define VSYNCL_REG		0x001E
+/* VSYNC control */
+#define VS_CTL_REG(n)		(0x00A1 + (n))
+/* page 1 */
+#define ISP_CTL_REG(n)		(0x0110 + (n))
+#define YOFS_REG		0x0119
+#define DARK_YOFS_REG		0x011A
+#define SAT_CTL_REG		0x0120
+#define BSAT_REG		0x0121
+#define RSAT_REG		0x0122
+/* Color correction */
+#define CMC_CTL_REG		0x0130
+#define CMC_OFSGH_REG		0x0133
+#define CMC_OFSGL_REG		0x0135
+#define CMC_SIGN_REG		0x0136
+#define CMC_GOFS_REG		0x0137
+#define CMC_COEF_REG(n)		(0x0138 + (n))
+#define CMC_OFS_REG(n)		(0x0141 + (n))
+/* Gamma correction */
+#define GMA_CTL_REG		0x0160
+#define GMA_COEF_REG(n)		(0x0161 + (n))
+/* Lens Shading */
+#define LENS_CTRL_REG		0x01D0
+#define LENS_XCEN_REG		0x01D1
+#define LENS_YCEN_REG		0x01D2
+#define LENS_RC_REG		0x01D3
+#define LENS_GC_REG		0x01D4
+#define LENS_BC_REG		0x01D5
+#define L_AGON_REG		0x01D6
+#define L_AGOFF_REG		0x01D7
+/* Page 3 - Auto Exposure */
+#define AE_CTL_REG(n)		(0x0310 + (n))
+#define AE_CTL9_REG		0x032C
+#define AE_CTL10_REG		0x032D
+#define AE_YLVL_REG		0x031C
+#define AE_YTH_REG(n)		(0x031D + (n))
+#define AE_WGT_REG		0x0326
+#define EXP_TIMEH_REG		0x0333
+#define EXP_TIMEM_REG		0x0334
+#define EXP_TIMEL_REG		0x0335
+#define EXP_MMINH_REG		0x0336
+#define EXP_MMINL_REG		0x0337
+#define EXP_MMAXH_REG		0x0338
+#define EXP_MMAXM_REG		0x0339
+#define EXP_MMAXL_REG		0x033A
+/* Page 4 - Auto White Balance */
+#define AWB_CTL_REG(n)		(0x0410 + (n))
+#define AWB_ENABE		0x80
+#define AWB_WGHT_REG		0x0419
+#define BGAIN_PAR_REG(n)	(0x044F + (n))
+/* Manual white balance, when AWB_CTL2[0]=1 */
+#define MWB_RGAIN_REG		0x0466
+#define MWB_BGAIN_REG		0x0467
+
+/* The token to mark an array end */
+#define REG_TERM		0xFFFF
+
+struct noon010_format {
+	enum v4l2_mbus_pixelcode code;
+	enum v4l2_colorspace colorspace;
+	u16 ispctl1_reg;
+};
+
+struct noon010_frmsize {
+	u16 width;
+	u16 height;
+	int vid_ctl1;
+};
+
+static const char * const noon010_supply_name[] = {
+	"vdd_core", "vddio", "vdda"
+};
+
+#define NOON010_NUM_SUPPLIES ARRAY_SIZE(noon010_supply_name)
+
+struct noon010_info {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct v4l2_ctrl_handler hdl;
+	struct regulator_bulk_data supply[NOON010_NUM_SUPPLIES];
+	u32 gpio_nreset;
+	u32 gpio_nstby;
+
+	/* Protects the struct members below */
+	struct mutex lock;
+
+	const struct noon010_format *curr_fmt;
+	const struct noon010_frmsize *curr_win;
+	unsigned int apply_new_cfg:1;
+	unsigned int streaming:1;
+	unsigned int hflip:1;
+	unsigned int vflip:1;
+	unsigned int power:1;
+	u8 i2c_reg_page;
+};
+
+struct i2c_regval {
+	u16 addr;
+	u16 val;
+};
+
+/* Supported resolutions. */
+static const struct noon010_frmsize noon010_sizes[] = {
+	{
+		.width		= 352,
+		.height		= 288,
+		.vid_ctl1	= 0,
+	}, {
+		.width		= 176,
+		.height		= 144,
+		.vid_ctl1	= 0x10,
+	}, {
+		.width		= 88,
+		.height		= 72,
+		.vid_ctl1	= 0x20,
+	},
+};
+
+/* Supported pixel formats. */
+static const struct noon010_format noon010_formats[] = {
+	{
+		.code		= V4L2_MBUS_FMT_YUYV8_2X8,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+		.ispctl1_reg	= 0x03,
+	}, {
+		.code		= V4L2_MBUS_FMT_YVYU8_2X8,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+		.ispctl1_reg	= 0x02,
+	}, {
+		.code		= V4L2_MBUS_FMT_VYUY8_2X8,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+		.ispctl1_reg	= 0,
+	}, {
+		.code		= V4L2_MBUS_FMT_UYVY8_2X8,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+		.ispctl1_reg	= 0x01,
+	}, {
+		.code		= V4L2_MBUS_FMT_RGB565_2X8_BE,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+		.ispctl1_reg	= 0x40,
+	},
+};
+
+static const struct i2c_regval noon010_base_regs[] = {
+	{ WIN_COLL_REG,		0x06 },	{ HBLANKL_REG,		0x7C },
+	/* Color corection and saturation */
+	{ ISP_CTL_REG(0),	0x30 }, { ISP_CTL_REG(2),	0x30 },
+	{ YOFS_REG,		0x80 }, { DARK_YOFS_REG,	0x04 },
+	{ SAT_CTL_REG,		0x1F }, { BSAT_REG,		0x90 },
+	{ CMC_CTL_REG,		0x0F }, { CMC_OFSGH_REG,	0x3C },
+	{ CMC_OFSGL_REG,	0x2C }, { CMC_SIGN_REG,		0x3F },
+	{ CMC_COEF_REG(0),	0x79 }, { CMC_OFS_REG(0),	0x00 },
+	{ CMC_COEF_REG(1),	0x39 }, { CMC_OFS_REG(1),	0x00 },
+	{ CMC_COEF_REG(2),	0x00 }, { CMC_OFS_REG(2),	0x00 },
+	{ CMC_COEF_REG(3),	0x11 }, { CMC_OFS_REG(3),	0x8B },
+	{ CMC_COEF_REG(4),	0x65 }, { CMC_OFS_REG(4),	0x07 },
+	{ CMC_COEF_REG(5),	0x14 }, { CMC_OFS_REG(5),	0x04 },
+	{ CMC_COEF_REG(6),	0x01 }, { CMC_OFS_REG(6),	0x9C },
+	{ CMC_COEF_REG(7),	0x33 }, { CMC_OFS_REG(7),	0x89 },
+	{ CMC_COEF_REG(8),	0x74 }, { CMC_OFS_REG(8),	0x25 },
+	/* Automatic white balance */
+	{ AWB_CTL_REG(0),	0x78 }, { AWB_CTL_REG(1),	0x2E },
+	{ AWB_CTL_REG(2),	0x20 }, { AWB_CTL_REG(3),	0x85 },
+	/* Auto exposure */
+	{ AE_CTL_REG(0),	0xDC }, { AE_CTL_REG(1),	0x81 },
+	{ AE_CTL_REG(2),	0x30 }, { AE_CTL_REG(3),	0xA5 },
+	{ AE_CTL_REG(4),	0x40 }, { AE_CTL_REG(5),	0x51 },
+	{ AE_CTL_REG(6),	0x33 }, { AE_CTL_REG(7),	0x7E },
+	{ AE_CTL9_REG,		0x00 }, { AE_CTL10_REG,		0x02 },
+	{ AE_YLVL_REG,		0x44 },	{ AE_YTH_REG(0),	0x34 },
+	{ AE_YTH_REG(1),	0x30 },	{ AE_WGT_REG,		0xD5 },
+	/* Lens shading compensation */
+	{ LENS_CTRL_REG,	0x01 }, { LENS_XCEN_REG,	0x80 },
+	{ LENS_YCEN_REG,	0x70 }, { LENS_RC_REG,		0x53 },
+	{ LENS_GC_REG,		0x40 }, { LENS_BC_REG,		0x3E },
+	{ REG_TERM,		0 },
+};
+
+static inline struct noon010_info *to_noon010(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct noon010_info, sd);
+}
+
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct noon010_info, hdl)->sd;
+}
+
+static inline int set_i2c_page(struct noon010_info *info,
+			       struct i2c_client *client, unsigned int reg)
+{
+	u32 page = reg >> 8 & 0xFF;
+	int ret = 0;
+
+	if (info->i2c_reg_page != page && (reg & 0xFF) != 0x03) {
+		ret = i2c_smbus_write_byte_data(client, PAGEMODE_REG, page);
+		if (!ret)
+			info->i2c_reg_page = page;
+	}
+	return ret;
+}
+
+static int cam_i2c_read(struct v4l2_subdev *sd, u32 reg_addr)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct noon010_info *info = to_noon010(sd);
+	int ret = set_i2c_page(info, client, reg_addr);
+
+	if (ret)
+		return ret;
+	return i2c_smbus_read_byte_data(client, reg_addr & 0xFF);
+}
+
+static int cam_i2c_write(struct v4l2_subdev *sd, u32 reg_addr, u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct noon010_info *info = to_noon010(sd);
+	int ret = set_i2c_page(info, client, reg_addr);
+
+	if (ret)
+		return ret;
+	return i2c_smbus_write_byte_data(client, reg_addr & 0xFF, val);
+}
+
+static inline int noon010_bulk_write_reg(struct v4l2_subdev *sd,
+					 const struct i2c_regval *msg)
+{
+	while (msg->addr != REG_TERM) {
+		int ret = cam_i2c_write(sd, msg->addr, msg->val);
+
+		if (ret)
+			return ret;
+		msg++;
+	}
+	return 0;
+}
+
+/* Device reset and sleep mode control */
+static int noon010_power_ctrl(struct v4l2_subdev *sd, bool reset, bool sleep)
+{
+	struct noon010_info *info = to_noon010(sd);
+	u8 reg = sleep ? 0xF1 : 0xF0;
+	int ret = 0;
+
+	if (reset) {
+		ret = cam_i2c_write(sd, POWER_CTRL_REG, reg | 0x02);
+		udelay(20);
+	}
+	if (!ret) {
+		ret = cam_i2c_write(sd, POWER_CTRL_REG, reg);
+		if (reset && !ret)
+			info->i2c_reg_page = -1;
+	}
+	return ret;
+}
+
+/* Automatic white balance control */
+static int noon010_enable_autowhitebalance(struct v4l2_subdev *sd, int on)
+{
+	int ret;
+
+	ret = cam_i2c_write(sd, AWB_CTL_REG(1), on ? 0x2E : 0x2F);
+	if (!ret)
+		ret = cam_i2c_write(sd, AWB_CTL_REG(0), on ? 0xFB : 0x7B);
+	return ret;
+}
+
+/* Called with struct noon010_info.lock mutex held */
+static int noon010_set_flip(struct v4l2_subdev *sd, int hflip, int vflip)
+{
+	struct noon010_info *info = to_noon010(sd);
+	int reg, ret;
+
+	reg = cam_i2c_read(sd, VDO_CTL_REG(1));
+	if (reg < 0)
+		return reg;
+
+	reg &= 0x7C;
+	if (hflip)
+		reg |= 0x01;
+	if (vflip)
+		reg |= 0x02;
+
+	ret = cam_i2c_write(sd, VDO_CTL_REG(1), reg | 0x80);
+	if (!ret) {
+		info->hflip = hflip;
+		info->vflip = vflip;
+	}
+	return ret;
+}
+
+/* Configure resolution and color format */
+static int noon010_set_params(struct v4l2_subdev *sd)
+{
+	struct noon010_info *info = to_noon010(sd);
+
+	int ret = cam_i2c_write(sd, VDO_CTL_REG(0),
+				info->curr_win->vid_ctl1);
+	if (ret)
+		return ret;
+	return cam_i2c_write(sd, ISP_CTL_REG(0),
+			     info->curr_fmt->ispctl1_reg);
+}
+
+/* Find nearest matching image pixel size. */
+static int noon010_try_frame_size(struct v4l2_mbus_framefmt *mf,
+				  const struct noon010_frmsize **size)
+{
+	unsigned int min_err = ~0;
+	int i = ARRAY_SIZE(noon010_sizes);
+	const struct noon010_frmsize *fsize = &noon010_sizes[0],
+		*match = NULL;
+
+	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) {
+		mf->width  = match->width;
+		mf->height = match->height;
+		if (size)
+			*size = match;
+		return 0;
+	}
+	return -EINVAL;
+}
+
+/* Called with info.lock mutex held */
+static int power_enable(struct noon010_info *info)
+{
+	int ret;
+
+	if (info->power) {
+		v4l2_info(&info->sd, "%s: sensor is already on\n", __func__);
+		return 0;
+	}
+
+	if (gpio_is_valid(info->gpio_nstby))
+		gpio_set_value(info->gpio_nstby, 0);
+
+	if (gpio_is_valid(info->gpio_nreset))
+		gpio_set_value(info->gpio_nreset, 0);
+
+	ret = regulator_bulk_enable(NOON010_NUM_SUPPLIES, info->supply);
+	if (ret)
+		return ret;
+
+	if (gpio_is_valid(info->gpio_nreset)) {
+		msleep(50);
+		gpio_set_value(info->gpio_nreset, 1);
+	}
+	if (gpio_is_valid(info->gpio_nstby)) {
+		udelay(1000);
+		gpio_set_value(info->gpio_nstby, 1);
+	}
+	if (gpio_is_valid(info->gpio_nreset)) {
+		udelay(1000);
+		gpio_set_value(info->gpio_nreset, 0);
+		msleep(100);
+		gpio_set_value(info->gpio_nreset, 1);
+		msleep(20);
+	}
+	info->power = 1;
+
+	v4l2_dbg(1, debug, &info->sd,  "%s: sensor is on\n", __func__);
+	return 0;
+}
+
+/* Called with info.lock mutex held */
+static int power_disable(struct noon010_info *info)
+{
+	int ret;
+
+	if (!info->power) {
+		v4l2_info(&info->sd, "%s: sensor is already off\n", __func__);
+		return 0;
+	}
+
+	ret = regulator_bulk_disable(NOON010_NUM_SUPPLIES, info->supply);
+	if (ret)
+		return ret;
+
+	if (gpio_is_valid(info->gpio_nstby))
+		gpio_set_value(info->gpio_nstby, 0);
+
+	if (gpio_is_valid(info->gpio_nreset))
+		gpio_set_value(info->gpio_nreset, 0);
+
+	info->power = 0;
+
+	v4l2_dbg(1, debug, &info->sd,  "%s: sensor is off\n", __func__);
+
+	return 0;
+}
+
+static int noon010_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+	struct noon010_info *info = to_noon010(sd);
+	int ret = 0;
+
+	v4l2_dbg(1, debug, sd, "%s: ctrl_id: %d, value: %d\n",
+		 __func__, ctrl->id, ctrl->val);
+
+	mutex_lock(&info->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 (!info->power)
+		goto unlock;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		ret = noon010_enable_autowhitebalance(sd, ctrl->val);
+		break;
+	case V4L2_CID_BLUE_BALANCE:
+		ret = cam_i2c_write(sd, MWB_BGAIN_REG, ctrl->val);
+		break;
+	case V4L2_CID_RED_BALANCE:
+		ret =  cam_i2c_write(sd, MWB_RGAIN_REG, ctrl->val);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+unlock:
+	mutex_unlock(&info->lock);
+	return ret;
+}
+
+static int noon010_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_fh *fh,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index >= ARRAY_SIZE(noon010_formats))
+		return -EINVAL;
+
+	code->code = noon010_formats[code->index].code;
+	return 0;
+}
+
+static int noon010_get_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+			   struct v4l2_subdev_format *fmt)
+{
+	struct noon010_info *info = to_noon010(sd);
+	struct v4l2_mbus_framefmt *mf;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		if (fh) {
+			mf = v4l2_subdev_get_try_format(fh, 0);
+			fmt->format = *mf;
+		}
+		return 0;
+	}
+	mf = &fmt->format;
+
+	mutex_lock(&info->lock);
+	mf->width = info->curr_win->width;
+	mf->height = info->curr_win->height;
+	mf->code = info->curr_fmt->code;
+	mf->colorspace = info->curr_fmt->colorspace;
+	mf->field = V4L2_FIELD_NONE;
+
+	mutex_unlock(&info->lock);
+	return 0;
+}
+
+/* Return nearest media bus frame format. */
+static const struct noon010_format *noon010_try_fmt(struct v4l2_subdev *sd,
+					    struct v4l2_mbus_framefmt *mf)
+{
+	int i = ARRAY_SIZE(noon010_formats);
+
+	while (--i)
+		if (mf->code == noon010_formats[i].code)
+			break;
+	mf->code = noon010_formats[i].code;
+
+	return &noon010_formats[i];
+}
+
+static int noon010_set_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+			   struct v4l2_subdev_format *fmt)
+{
+	struct noon010_info *info = to_noon010(sd);
+	const struct noon010_frmsize *size = NULL;
+	const struct noon010_format *nf;
+	struct v4l2_mbus_framefmt *mf;
+	int ret = 0;
+
+	nf = noon010_try_fmt(sd, &fmt->format);
+	noon010_try_frame_size(&fmt->format, &size);
+	fmt->format.colorspace = V4L2_COLORSPACE_JPEG;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		if (fh) {
+			mf = v4l2_subdev_get_try_format(fh, 0);
+			*mf = fmt->format;
+		}
+		return 0;
+	}
+	mutex_lock(&info->lock);
+	if (!info->streaming) {
+		info->apply_new_cfg = 1;
+		info->curr_fmt = nf;
+		info->curr_win = size;
+	} else {
+		ret = -EBUSY;
+	}
+	mutex_unlock(&info->lock);
+	return ret;
+}
+
+/* Called with struct noon010_info.lock mutex held */
+static int noon010_base_config(struct v4l2_subdev *sd)
+{
+	int ret = noon010_bulk_write_reg(sd, noon010_base_regs);
+	if (!ret)
+		ret = noon010_set_params(sd);
+	if (!ret)
+		ret = noon010_set_flip(sd, 1, 0);
+
+	return ret;
+}
+
+static int noon010_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct noon010_info *info = to_noon010(sd);
+	int ret;
+
+	mutex_lock(&info->lock);
+	if (on) {
+		ret = power_enable(info);
+		if (!ret)
+			ret = noon010_base_config(sd);
+	} else {
+		noon010_power_ctrl(sd, false, true);
+		ret = power_disable(info);
+	}
+	mutex_unlock(&info->lock);
+
+	/* Restore the controls state */
+	if (!ret && on)
+		ret = v4l2_ctrl_handler_setup(&info->hdl);
+
+	return ret;
+}
+
+static int noon010_s_stream(struct v4l2_subdev *sd, int on)
+{
+	struct noon010_info *info = to_noon010(sd);
+	int ret = 0;
+
+	mutex_lock(&info->lock);
+	if (!info->streaming != !on) {
+		ret = noon010_power_ctrl(sd, false, !on);
+		if (!ret)
+			info->streaming = on;
+	}
+	if (!ret && on && info->apply_new_cfg) {
+		ret = noon010_set_params(sd);
+		if (!ret)
+			info->apply_new_cfg = 0;
+	}
+	mutex_unlock(&info->lock);
+	return ret;
+}
+
+static int noon010_log_status(struct v4l2_subdev *sd)
+{
+	struct noon010_info *info = to_noon010(sd);
+
+	v4l2_ctrl_handler_log_status(&info->hdl, sd->name);
+	return 0;
+}
+
+static int noon010_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_mbus_framefmt *mf = v4l2_subdev_get_try_format(fh, 0);
+
+	mf->width = noon010_sizes[0].width;
+	mf->height = noon010_sizes[0].height;
+	mf->code = noon010_formats[0].code;
+	mf->colorspace = V4L2_COLORSPACE_JPEG;
+	mf->field = V4L2_FIELD_NONE;
+	return 0;
+}
+
+static const struct v4l2_subdev_internal_ops noon010_subdev_internal_ops = {
+	.open = noon010_open,
+};
+
+static const struct v4l2_ctrl_ops noon010_ctrl_ops = {
+	.s_ctrl = noon010_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops noon010_core_ops = {
+	.s_power	= noon010_s_power,
+	.g_ctrl		= v4l2_subdev_g_ctrl,
+	.s_ctrl		= v4l2_subdev_s_ctrl,
+	.queryctrl	= v4l2_subdev_queryctrl,
+	.querymenu	= v4l2_subdev_querymenu,
+	.g_ext_ctrls	= v4l2_subdev_g_ext_ctrls,
+	.try_ext_ctrls	= v4l2_subdev_try_ext_ctrls,
+	.s_ext_ctrls	= v4l2_subdev_s_ext_ctrls,
+	.log_status	= noon010_log_status,
+};
+
+static struct v4l2_subdev_pad_ops noon010_pad_ops = {
+	.enum_mbus_code	= noon010_enum_mbus_code,
+	.get_fmt	= noon010_get_fmt,
+	.set_fmt	= noon010_set_fmt,
+};
+
+static struct v4l2_subdev_video_ops noon010_video_ops = {
+	.s_stream	= noon010_s_stream,
+};
+
+static const struct v4l2_subdev_ops noon010_ops = {
+	.core	= &noon010_core_ops,
+	.pad	= &noon010_pad_ops,
+	.video	= &noon010_video_ops,
+};
+
+/* Return 0 if NOON010PC30L sensor type was detected or -ENODEV otherwise. */
+static int noon010_detect(struct i2c_client *client, struct noon010_info *info)
+{
+	int ret;
+
+	ret = power_enable(info);
+	if (ret)
+		return ret;
+
+	ret = i2c_smbus_read_byte_data(client, DEVICE_ID_REG);
+	if (ret < 0)
+		dev_err(&client->dev, "I2C read failed: 0x%X\n", ret);
+
+	power_disable(info);
+
+	return ret == NOON010PC30_ID ? 0 : -ENODEV;
+}
+
+static int noon010_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	struct noon010_info *info;
+	struct v4l2_subdev *sd;
+	const struct noon010pc30_platform_data *pdata
+		= client->dev.platform_data;
+	int ret;
+	int i;
+
+	if (!pdata) {
+		dev_err(&client->dev, "No platform data!\n");
+		return -EIO;
+	}
+
+	info = kzalloc(sizeof(*info), GFP_KERNEL);
+	if (!info)
+		return -ENOMEM;
+
+	mutex_init(&info->lock);
+	sd = &info->sd;
+	v4l2_i2c_subdev_init(sd, client, &noon010_ops);
+	strlcpy(sd->name, MODULE_NAME, sizeof(sd->name));
+
+	sd->internal_ops = &noon010_subdev_internal_ops;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	v4l2_ctrl_handler_init(&info->hdl, 3);
+
+	v4l2_ctrl_new_std(&info->hdl, &noon010_ctrl_ops,
+			  V4L2_CID_AUTO_WHITE_BALANCE, 0, 1, 1, 1);
+	v4l2_ctrl_new_std(&info->hdl, &noon010_ctrl_ops,
+			  V4L2_CID_RED_BALANCE, 0, 127, 1, 64);
+	v4l2_ctrl_new_std(&info->hdl, &noon010_ctrl_ops,
+			  V4L2_CID_BLUE_BALANCE, 0, 127, 1, 64);
+
+	sd->ctrl_handler = &info->hdl;
+
+	ret = info->hdl.error;
+	if (ret)
+		goto np_err;
+
+	info->i2c_reg_page	= -1;
+	info->gpio_nreset	= -EINVAL;
+	info->gpio_nstby	= -EINVAL;
+	info->curr_fmt		= &noon010_formats[0];
+	info->curr_win		= &noon010_sizes[0];
+
+	if (gpio_is_valid(pdata->gpio_nreset)) {
+		ret = gpio_request(pdata->gpio_nreset, "NOON010PC30 NRST");
+		if (ret) {
+			dev_err(&client->dev, "GPIO request error: %d\n", ret);
+			goto np_err;
+		}
+		info->gpio_nreset = pdata->gpio_nreset;
+		gpio_direction_output(info->gpio_nreset, 0);
+		gpio_export(info->gpio_nreset, 0);
+	}
+
+	if (gpio_is_valid(pdata->gpio_nstby)) {
+		ret = gpio_request(pdata->gpio_nstby, "NOON010PC30 NSTBY");
+		if (ret) {
+			dev_err(&client->dev, "GPIO request error: %d\n", ret);
+			goto np_gpio_err;
+		}
+		info->gpio_nstby = pdata->gpio_nstby;
+		gpio_direction_output(info->gpio_nstby, 0);
+		gpio_export(info->gpio_nstby, 0);
+	}
+
+	for (i = 0; i < NOON010_NUM_SUPPLIES; i++)
+		info->supply[i].supply = noon010_supply_name[i];
+
+	ret = regulator_bulk_get(&client->dev, NOON010_NUM_SUPPLIES,
+				 info->supply);
+	if (ret)
+		goto np_reg_err;
+
+	info->pad.flags = MEDIA_PAD_FL_SOURCE;
+	sd->entity.type = MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
+	ret = media_entity_init(&sd->entity, 1, &info->pad, 0);
+	if (ret < 0)
+		goto np_me_err;
+
+	ret = noon010_detect(client, info);
+	if (!ret)
+		return 0;
+
+np_me_err:
+	regulator_bulk_free(NOON010_NUM_SUPPLIES, info->supply);
+np_reg_err:
+	if (gpio_is_valid(info->gpio_nstby))
+		gpio_free(info->gpio_nstby);
+np_gpio_err:
+	if (gpio_is_valid(info->gpio_nreset))
+		gpio_free(info->gpio_nreset);
+np_err:
+	v4l2_ctrl_handler_free(&info->hdl);
+	v4l2_device_unregister_subdev(sd);
+	kfree(info);
+	return ret;
+}
+
+static int noon010_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct noon010_info *info = to_noon010(sd);
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&info->hdl);
+
+	regulator_bulk_free(NOON010_NUM_SUPPLIES, info->supply);
+
+	if (gpio_is_valid(info->gpio_nreset))
+		gpio_free(info->gpio_nreset);
+
+	if (gpio_is_valid(info->gpio_nstby))
+		gpio_free(info->gpio_nstby);
+
+	media_entity_cleanup(&sd->entity);
+	kfree(info);
+	return 0;
+}
+
+static const struct i2c_device_id noon010_id[] = {
+	{ MODULE_NAME, 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(i2c, noon010_id);
+
+
+static struct i2c_driver noon010_i2c_driver = {
+	.driver = {
+		.name = MODULE_NAME
+	},
+	.probe		= noon010_probe,
+	.remove		= noon010_remove,
+	.id_table	= noon010_id,
+};
+
+module_i2c_driver(noon010_i2c_driver);
+
+MODULE_DESCRIPTION("Siliconfile NOON010PC30 camera driver");
+MODULE_AUTHOR("Sylwester Nawrocki <s.nawrocki@samsung.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/ov7670.c b/drivers/media/i2c/ov7670.c
new file mode 100644
index 000000000000..e7c82b297514
--- /dev/null
+++ b/drivers/media/i2c/ov7670.c
@@ -0,0 +1,1586 @@
+/*
+ * 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>
+ *
+ * This file may be distributed under the terms of the GNU General
+ * Public License, version 2.
+ */
+#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 <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-mediabus.h>
+#include <media/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)");
+
+/*
+ * Basic window sizes.  These probably belong somewhere more globally
+ * useful.
+ */
+#define VGA_WIDTH	640
+#define VGA_HEIGHT	480
+#define QVGA_WIDTH	320
+#define QVGA_HEIGHT	240
+#define CIF_WIDTH	352
+#define CIF_HEIGHT	288
+#define QCIF_WIDTH	176
+#define	QCIF_HEIGHT	144
+
+/*
+ * The 7670 sits on i2c with ID 0x42
+ */
+#define OV7670_I2C_ADDR 0x42
+
+/* 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_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 */
+
+
+/*
+ * Information we maintain about a known sensor.
+ */
+struct ov7670_format_struct;  /* coming later */
+struct ov7670_info {
+	struct v4l2_subdev sd;
+	struct ov7670_format_struct *fmt;  /* Current format */
+	unsigned char sat;		/* Saturation value */
+	int hue;			/* Hue value */
+	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 */
+};
+
+static inline struct ov7670_info *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct ov7670_info, 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 */
+	{ 0x70, 0x3a },		{ 0x71, 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, 0x18 }, /* 4x 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);
+}
+
+/*
+ * 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 {
+	enum v4l2_mbus_pixelcode mbus_code;
+	enum v4l2_colorspace colorspace;
+	struct regval_list *regs;
+	int cmatrix[CMATRIX_LEN];
+} ov7670_formats[] = {
+	{
+		.mbus_code	= V4L2_MBUS_FMT_YUYV8_2X8,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+		.regs 		= ov7670_fmt_yuv422,
+		.cmatrix	= { 128, -128, 0, -34, -94, 128 },
+	},
+	{
+		.mbus_code	= V4L2_MBUS_FMT_RGB444_2X8_PADHI_LE,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.regs		= ov7670_fmt_rgb444,
+		.cmatrix	= { 179, -179, 0, -61, -176, 228 },
+	},
+	{
+		.mbus_code	= V4L2_MBUS_FMT_RGB565_2X8_LE,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.regs		= ov7670_fmt_rgb565,
+		.cmatrix	= { 179, -179, 0, -61, -176, 228 },
+	},
+	{
+		.mbus_code	= V4L2_MBUS_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 {
+	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 */
+/* h/vref stuff */
+} 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,
+	},
+};
+
+#define N_WIN_SIZES (ARRAY_SIZE(ov7670_win_sizes))
+
+
+/*
+ * 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);
+	ret += ov7670_write(sd, REG_HSTOP, (hstop >> 3) & 0xff);
+	ret += ov7670_read(sd, REG_HREF, &v);
+	v = (v & 0xc0) | ((hstop & 0x7) << 3) | (hstart & 0x7);
+	msleep(10);
+	ret += ov7670_write(sd, REG_HREF, v);
+/*
+ * Vertical: similar arrangement, but only 10 bits.
+ */
+	ret += ov7670_write(sd, REG_VSTART, (vstart >> 2) & 0xff);
+	ret += ov7670_write(sd, REG_VSTOP, (vstop >> 2) & 0xff);
+	ret += ov7670_read(sd, REG_VREF, &v);
+	v = (v & 0xf0) | ((vstop & 0x3) << 2) | (vstart & 0x3);
+	msleep(10);
+	ret += ov7670_write(sd, REG_VREF, v);
+	return ret;
+}
+
+
+static int ov7670_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned index,
+					enum v4l2_mbus_pixelcode *code)
+{
+	if (index >= N_OV7670_FMTS)
+		return -EINVAL;
+
+	*code = ov7670_formats[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;
+	struct ov7670_win_size *wsize;
+
+	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;
+	/*
+	 * Round requested image size down to the nearest
+	 * we support, but not below the smallest.
+	 */
+	for (wsize = ov7670_win_sizes; wsize < ov7670_win_sizes + N_WIN_SIZES;
+	     wsize++)
+		if (fmt->width >= wsize->width && fmt->height >= wsize->height)
+			break;
+	if (wsize >= ov7670_win_sizes + N_WIN_SIZES)
+		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;
+	return 0;
+}
+
+static int ov7670_try_mbus_fmt(struct v4l2_subdev *sd,
+			    struct v4l2_mbus_framefmt *fmt)
+{
+	return ov7670_try_fmt_internal(sd, fmt, NULL, NULL);
+}
+
+/*
+ * Set a format.
+ */
+static int ov7670_s_mbus_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_mbus_framefmt *fmt)
+{
+	struct ov7670_format_struct *ovfmt;
+	struct ov7670_win_size *wsize;
+	struct ov7670_info *info = to_state(sd);
+	unsigned char com7;
+	int ret;
+
+	ret = ov7670_try_fmt_internal(sd, fmt, &ovfmt, &wsize);
+
+	if (ret)
+		return 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 = ovfmt->regs[0].value;
+	com7 |= wsize->com7_bit;
+	ov7670_write(sd, REG_COM7, com7);
+	/*
+	 * Now write the rest of the array.  Also store start/stops
+	 */
+	ov7670_write_array(sd, ovfmt->regs + 1);
+	ov7670_set_hw(sd, wsize->hstart, wsize->hstop, wsize->vstart,
+			wsize->vstop);
+	ret = 0;
+	if (wsize->regs)
+		ret = ov7670_write_array(sd, wsize->regs);
+	info->fmt = ovfmt;
+
+	/*
+	 * 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.
+	 */
+	if (ret == 0)
+		ret = ov7670_write(sd, REG_CLKRC, info->clkrc);
+	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_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *parms)
+{
+	struct v4l2_captureparm *cp = &parms->parm.capture;
+	struct ov7670_info *info = to_state(sd);
+
+	if (parms->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	memset(cp, 0, sizeof(struct v4l2_captureparm));
+	cp->capability = V4L2_CAP_TIMEPERFRAME;
+	cp->timeperframe.numerator = 1;
+	cp->timeperframe.denominator = info->clock_speed;
+	if ((info->clkrc & CLK_EXT) == 0 && (info->clkrc & CLK_SCALE) > 1)
+		cp->timeperframe.denominator /= (info->clkrc & CLK_SCALE);
+	return 0;
+}
+
+static int ov7670_s_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *parms)
+{
+	struct v4l2_captureparm *cp = &parms->parm.capture;
+	struct v4l2_fract *tpf = &cp->timeperframe;
+	struct ov7670_info *info = to_state(sd);
+	int div;
+
+	if (parms->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	if (cp->extendedmode != 0)
+		return -EINVAL;
+
+	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;
+	return ov7670_write(sd, REG_CLKRC, info->clkrc);
+}
+
+
+/*
+ * 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_frameintervals(struct v4l2_subdev *sd,
+		struct v4l2_frmivalenum *interval)
+{
+	if (interval->index >= ARRAY_SIZE(ov7670_frame_rates))
+		return -EINVAL;
+	interval->type = V4L2_FRMIVAL_TYPE_DISCRETE;
+	interval->discrete.numerator = 1;
+	interval->discrete.denominator = ov7670_frame_rates[interval->index];
+	return 0;
+}
+
+/*
+ * Frame size enumeration
+ */
+static int ov7670_enum_framesizes(struct v4l2_subdev *sd,
+		struct v4l2_frmsizeenum *fsize)
+{
+	struct ov7670_info *info = to_state(sd);
+	int i;
+	int num_valid = -1;
+	__u32 index = fsize->index;
+
+	/*
+	 * 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 = &ov7670_win_sizes[index];
+		if (info->min_width && win->width < info->min_width)
+			continue;
+		if (info->min_height && win->height < info->min_height)
+			continue;
+		if (index == ++num_valid) {
+			fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+			fsize->discrete.width = win->width;
+			fsize->discrete.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);
+	}
+	ret += ov7670_write(sd, REG_CMATRIX_SIGN, signbits);
+	return ret;
+}
+
+
+/*
+ * 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 i;
+	/*
+	 * Apply the current saturation setting first.
+	 */
+	for (i = 0; i < CMATRIX_LEN; i++)
+		matrix[i] = (info->fmt->cmatrix[i]*info->sat) >> 7;
+	/*
+	 * Then, if need be, rotate the hue value.
+	 */
+	if (info->hue != 0) {
+		int sinth, costh, tmpmatrix[CMATRIX_LEN];
+
+		memcpy(tmpmatrix, matrix, CMATRIX_LEN*sizeof(int));
+		sinth = ov7670_sine(info->hue);
+		costh = ov7670_cosine(info->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(struct v4l2_subdev *sd, int value)
+{
+	struct ov7670_info *info = to_state(sd);
+	int matrix[CMATRIX_LEN];
+	int ret;
+
+	info->sat = value;
+	ov7670_calc_cmatrix(info, matrix);
+	ret = ov7670_store_cmatrix(sd, matrix);
+	return ret;
+}
+
+static int ov7670_g_sat(struct v4l2_subdev *sd, __s32 *value)
+{
+	struct ov7670_info *info = to_state(sd);
+
+	*value = info->sat;
+	return 0;
+}
+
+static int ov7670_s_hue(struct v4l2_subdev *sd, int value)
+{
+	struct ov7670_info *info = to_state(sd);
+	int matrix[CMATRIX_LEN];
+	int ret;
+
+	if (value < -180 || value > 180)
+		return -EINVAL;
+	info->hue = value;
+	ov7670_calc_cmatrix(info, matrix);
+	ret = ov7670_store_cmatrix(sd, matrix);
+	return ret;
+}
+
+
+static int ov7670_g_hue(struct v4l2_subdev *sd, __s32 *value)
+{
+	struct ov7670_info *info = to_state(sd);
+
+	*value = info->hue;
+	return 0;
+}
+
+
+/*
+ * Some weird registers seem to store values in a sign/magnitude format!
+ */
+static unsigned char ov7670_sm_to_abs(unsigned char v)
+{
+	if ((v & 0x80) == 0)
+		return v + 128;
+	return 128 - (v & 0x7f);
+}
+
+
+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;
+	int ret;
+
+	ov7670_read(sd, REG_COM8, &com8);
+	com8 &= ~COM8_AEC;
+	ov7670_write(sd, REG_COM8, com8);
+	v = ov7670_abs_to_sm(value);
+	ret = ov7670_write(sd, REG_BRIGHT, v);
+	return ret;
+}
+
+static int ov7670_g_brightness(struct v4l2_subdev *sd, __s32 *value)
+{
+	unsigned char v = 0;
+	int ret = ov7670_read(sd, REG_BRIGHT, &v);
+
+	*value = ov7670_sm_to_abs(v);
+	return ret;
+}
+
+static int ov7670_s_contrast(struct v4l2_subdev *sd, int value)
+{
+	return ov7670_write(sd, REG_CONTRAS, (unsigned char) value);
+}
+
+static int ov7670_g_contrast(struct v4l2_subdev *sd, __s32 *value)
+{
+	unsigned char v = 0;
+	int ret = ov7670_read(sd, REG_CONTRAS, &v);
+
+	*value = v;
+	return ret;
+}
+
+static int ov7670_g_hflip(struct v4l2_subdev *sd, __s32 *value)
+{
+	int ret;
+	unsigned char v = 0;
+
+	ret = ov7670_read(sd, REG_MVFP, &v);
+	*value = (v & MVFP_MIRROR) == MVFP_MIRROR;
+	return ret;
+}
+
+
+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 (value)
+		v |= MVFP_MIRROR;
+	else
+		v &= ~MVFP_MIRROR;
+	msleep(10);  /* FIXME */
+	ret += ov7670_write(sd, REG_MVFP, v);
+	return ret;
+}
+
+
+
+static int ov7670_g_vflip(struct v4l2_subdev *sd, __s32 *value)
+{
+	int ret;
+	unsigned char v = 0;
+
+	ret = ov7670_read(sd, REG_MVFP, &v);
+	*value = (v & MVFP_FLIP) == MVFP_FLIP;
+	return ret;
+}
+
+
+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 (value)
+		v |= MVFP_FLIP;
+	else
+		v &= ~MVFP_FLIP;
+	msleep(10);  /* FIXME */
+	ret += ov7670_write(sd, REG_MVFP, v);
+	return ret;
+}
+
+/*
+ * 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);
+	*value = gain;
+	return ret;
+}
+
+static int ov7670_s_gain(struct v4l2_subdev *sd, int value)
+{
+	int ret;
+	unsigned char com8;
+
+	ret = ov7670_write(sd, REG_GAIN, value & 0xff);
+	/* Have to turn off AGC as well */
+	if (ret == 0) {
+		ret = ov7670_read(sd, REG_COM8, &com8);
+		ret = ov7670_write(sd, REG_COM8, com8 & ~COM8_AGC);
+	}
+	return ret;
+}
+
+/*
+ * Tweak autogain.
+ */
+static int ov7670_g_autogain(struct v4l2_subdev *sd, __s32 *value)
+{
+	int ret;
+	unsigned char com8;
+
+	ret = ov7670_read(sd, REG_COM8, &com8);
+	*value = (com8 & COM8_AGC) != 0;
+	return ret;
+}
+
+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;
+}
+
+/*
+ * Exposure is spread all over the place: top 6 bits in AECHH, middle
+ * 8 in AECH, and two stashed in COM1 just for the hell of it.
+ */
+static int ov7670_g_exp(struct v4l2_subdev *sd, __s32 *value)
+{
+	int ret;
+	unsigned char com1, aech, aechh;
+
+	ret = ov7670_read(sd, REG_COM1, &com1) +
+		ov7670_read(sd, REG_AECH, &aech) +
+		ov7670_read(sd, REG_AECHH, &aechh);
+	*value = ((aechh & 0x3f) << 10) | (aech << 2) | (com1 & 0x03);
+	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_g_autoexp(struct v4l2_subdev *sd, __s32 *value)
+{
+	int ret;
+	unsigned char com8;
+	enum v4l2_exposure_auto_type *atype = (enum v4l2_exposure_auto_type *) value;
+
+	ret = ov7670_read(sd, REG_COM8, &com8);
+	if (com8 & COM8_AEC)
+		*atype = V4L2_EXPOSURE_AUTO;
+	else
+		*atype = V4L2_EXPOSURE_MANUAL;
+	return ret;
+}
+
+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 int ov7670_queryctrl(struct v4l2_subdev *sd,
+		struct v4l2_queryctrl *qc)
+{
+	/* Fill in min, max, step and default value for these controls. */
+	switch (qc->id) {
+	case V4L2_CID_BRIGHTNESS:
+		return v4l2_ctrl_query_fill(qc, 0, 255, 1, 128);
+	case V4L2_CID_CONTRAST:
+		return v4l2_ctrl_query_fill(qc, 0, 127, 1, 64);
+	case V4L2_CID_VFLIP:
+	case V4L2_CID_HFLIP:
+		return v4l2_ctrl_query_fill(qc, 0, 1, 1, 0);
+	case V4L2_CID_SATURATION:
+		return v4l2_ctrl_query_fill(qc, 0, 256, 1, 128);
+	case V4L2_CID_HUE:
+		return v4l2_ctrl_query_fill(qc, -180, 180, 5, 0);
+	case V4L2_CID_GAIN:
+		return v4l2_ctrl_query_fill(qc, 0, 255, 1, 128);
+	case V4L2_CID_AUTOGAIN:
+		return v4l2_ctrl_query_fill(qc, 0, 1, 1, 1);
+	case V4L2_CID_EXPOSURE:
+		return v4l2_ctrl_query_fill(qc, 0, 65535, 1, 500);
+	case V4L2_CID_EXPOSURE_AUTO:
+		return v4l2_ctrl_query_fill(qc, 0, 1, 1, 0);
+	}
+	return -EINVAL;
+}
+
+static int ov7670_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		return ov7670_g_brightness(sd, &ctrl->value);
+	case V4L2_CID_CONTRAST:
+		return ov7670_g_contrast(sd, &ctrl->value);
+	case V4L2_CID_SATURATION:
+		return ov7670_g_sat(sd, &ctrl->value);
+	case V4L2_CID_HUE:
+		return ov7670_g_hue(sd, &ctrl->value);
+	case V4L2_CID_VFLIP:
+		return ov7670_g_vflip(sd, &ctrl->value);
+	case V4L2_CID_HFLIP:
+		return ov7670_g_hflip(sd, &ctrl->value);
+	case V4L2_CID_GAIN:
+		return ov7670_g_gain(sd, &ctrl->value);
+	case V4L2_CID_AUTOGAIN:
+		return ov7670_g_autogain(sd, &ctrl->value);
+	case V4L2_CID_EXPOSURE:
+		return ov7670_g_exp(sd, &ctrl->value);
+	case V4L2_CID_EXPOSURE_AUTO:
+		return ov7670_g_autoexp(sd, &ctrl->value);
+	}
+	return -EINVAL;
+}
+
+static int ov7670_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		return ov7670_s_brightness(sd, ctrl->value);
+	case V4L2_CID_CONTRAST:
+		return ov7670_s_contrast(sd, ctrl->value);
+	case V4L2_CID_SATURATION:
+		return ov7670_s_sat(sd, ctrl->value);
+	case V4L2_CID_HUE:
+		return ov7670_s_hue(sd, ctrl->value);
+	case V4L2_CID_VFLIP:
+		return ov7670_s_vflip(sd, ctrl->value);
+	case V4L2_CID_HFLIP:
+		return ov7670_s_hflip(sd, ctrl->value);
+	case V4L2_CID_GAIN:
+		return ov7670_s_gain(sd, ctrl->value);
+	case V4L2_CID_AUTOGAIN:
+		return ov7670_s_autogain(sd, ctrl->value);
+	case V4L2_CID_EXPOSURE:
+		return ov7670_s_exp(sd, ctrl->value);
+	case V4L2_CID_EXPOSURE_AUTO:
+		return ov7670_s_autoexp(sd,
+				(enum v4l2_exposure_auto_type) ctrl->value);
+	}
+	return -EINVAL;
+}
+
+static int ov7670_g_chip_ident(struct v4l2_subdev *sd,
+		struct v4l2_dbg_chip_ident *chip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_OV7670, 0);
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int ov7670_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	unsigned char val = 0;
+	int ret;
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	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, struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	ov7670_write(sd, reg->reg & 0xff, reg->val & 0xff);
+	return 0;
+}
+#endif
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_core_ops ov7670_core_ops = {
+	.g_chip_ident = ov7670_g_chip_ident,
+	.g_ctrl = ov7670_g_ctrl,
+	.s_ctrl = ov7670_s_ctrl,
+	.queryctrl = ov7670_queryctrl,
+	.reset = ov7670_reset,
+	.init = ov7670_init,
+#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 = {
+	.enum_mbus_fmt = ov7670_enum_mbus_fmt,
+	.try_mbus_fmt = ov7670_try_mbus_fmt,
+	.s_mbus_fmt = ov7670_s_mbus_fmt,
+	.s_parm = ov7670_s_parm,
+	.g_parm = ov7670_g_parm,
+	.enum_frameintervals = ov7670_enum_frameintervals,
+	.enum_framesizes = ov7670_enum_framesizes,
+};
+
+static const struct v4l2_subdev_ops ov7670_ops = {
+	.core = &ov7670_core_ops,
+	.video = &ov7670_video_ops,
+};
+
+/* ----------------------------------------------------------------------- */
+
+static int ov7670_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct v4l2_subdev *sd;
+	struct ov7670_info *info;
+	int ret;
+
+	info = kzalloc(sizeof(struct ov7670_info), GFP_KERNEL);
+	if (info == NULL)
+		return -ENOMEM;
+	sd = &info->sd;
+	v4l2_i2c_subdev_init(sd, client, &ov7670_ops);
+
+	info->clock_speed = 30; /* default: a guess */
+	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;
+	}
+
+	/* 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);
+		kfree(info);
+		return ret;
+	}
+	v4l_info(client, "chip found @ 0x%02x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	info->fmt = &ov7670_formats[0];
+	info->sat = 128;	/* Review this */
+	info->clkrc = info->clock_speed / 30;
+	return 0;
+}
+
+
+static int ov7670_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	kfree(to_state(sd));
+	return 0;
+}
+
+static const struct i2c_device_id ov7670_id[] = {
+	{ "ov7670", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ov7670_id);
+
+static struct i2c_driver ov7670_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "ov7670",
+	},
+	.probe		= ov7670_probe,
+	.remove		= ov7670_remove,
+	.id_table	= ov7670_id,
+};
+
+module_i2c_driver(ov7670_driver);
diff --git a/drivers/media/i2c/s5k4ecgx.c b/drivers/media/i2c/s5k4ecgx.c
new file mode 100644
index 000000000000..49c1b3abb425
--- /dev/null
+++ b/drivers/media/i2c/s5k4ecgx.c
@@ -0,0 +1,1036 @@
+/*
+ * Driver for Samsung S5K4ECGX 1/4" 5Mp CMOS Image Sensor SoC
+ * with an Embedded Image Signal Processor.
+ *
+ * Copyright (C) 2012, Linaro, Sangwook Lee <sangwook.lee@linaro.org>
+ * Copyright (C) 2012, Insignal Co,. Ltd, Homin Lee <suapapa@insignal.co.kr>
+ *
+ * Based on s5k6aa and noon010pc30 driver
+ * Copyright (C) 2011, Samsung Electronics Co., Ltd.
+ *
+ * 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; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/clk.h>
+#include <linux/crc32.h>
+#include <linux/ctype.h>
+#include <linux/delay.h>
+#include <linux/firmware.h>
+#include <linux/gpio.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <asm/unaligned.h>
+
+#include <media/media-entity.h>
+#include <media/s5k4ecgx.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-mediabus.h>
+#include <media/v4l2-subdev.h>
+
+static int debug;
+module_param(debug, int, 0644);
+
+#define S5K4ECGX_DRIVER_NAME		"s5k4ecgx"
+#define S5K4ECGX_FIRMWARE		"s5k4ecgx.bin"
+
+/* Firmware revision information */
+#define REG_FW_REVISION			0x700001a6
+#define REG_FW_VERSION			0x700001a4
+#define S5K4ECGX_REVISION_1_1		0x11
+#define S5K4ECGX_FW_VERSION		0x4ec0
+
+/* General purpose parameters */
+#define REG_USER_BRIGHTNESS		0x7000022c
+#define REG_USER_CONTRAST		0x7000022e
+#define REG_USER_SATURATION		0x70000230
+
+#define REG_G_ENABLE_PREV		0x7000023e
+#define REG_G_ENABLE_PREV_CHG		0x70000240
+#define REG_G_NEW_CFG_SYNC		0x7000024a
+#define REG_G_PREV_IN_WIDTH		0x70000250
+#define REG_G_PREV_IN_HEIGHT		0x70000252
+#define REG_G_PREV_IN_XOFFS		0x70000254
+#define REG_G_PREV_IN_YOFFS		0x70000256
+#define REG_G_CAP_IN_WIDTH		0x70000258
+#define REG_G_CAP_IN_HEIGHT		0x7000025a
+#define REG_G_CAP_IN_XOFFS		0x7000025c
+#define REG_G_CAP_IN_YOFFS		0x7000025e
+#define REG_G_INPUTS_CHANGE_REQ		0x70000262
+#define REG_G_ACTIVE_PREV_CFG		0x70000266
+#define REG_G_PREV_CFG_CHG		0x70000268
+#define REG_G_PREV_OPEN_AFTER_CH	0x7000026a
+
+/* Preview context register sets. n = 0...4. */
+#define PREG(n, x)			((n) * 0x30 + (x))
+#define REG_P_OUT_WIDTH(n)		PREG(n, 0x700002a6)
+#define REG_P_OUT_HEIGHT(n)		PREG(n, 0x700002a8)
+#define REG_P_FMT(n)			PREG(n, 0x700002aa)
+#define REG_P_PVI_MASK(n)		PREG(n, 0x700002b4)
+#define REG_P_FR_TIME_TYPE(n)		PREG(n, 0x700002be)
+#define  FR_TIME_DYNAMIC		0
+#define  FR_TIME_FIXED			1
+#define  FR_TIME_FIXED_ACCURATE		2
+#define REG_P_FR_TIME_Q_TYPE(n)		PREG(n, 0x700002c0)
+#define  FR_TIME_Q_DYNAMIC		0
+#define  FR_TIME_Q_BEST_FRRATE		1
+#define  FR_TIME_Q_BEST_QUALITY		2
+
+/* Frame period in 0.1 ms units */
+#define REG_P_MAX_FR_TIME(n)		PREG(n, 0x700002c2)
+#define REG_P_MIN_FR_TIME(n)		PREG(n, 0x700002c4)
+#define  US_TO_FR_TIME(__t)		((__t) / 100)
+#define REG_P_PREV_MIRROR(n)		PREG(n, 0x700002d0)
+#define REG_P_CAP_MIRROR(n)		PREG(n, 0x700002d2)
+
+#define REG_G_PREVZOOM_IN_WIDTH		0x70000494
+#define REG_G_PREVZOOM_IN_HEIGHT	0x70000496
+#define REG_G_PREVZOOM_IN_XOFFS		0x70000498
+#define REG_G_PREVZOOM_IN_YOFFS		0x7000049a
+#define REG_G_CAPZOOM_IN_WIDTH		0x7000049c
+#define REG_G_CAPZOOM_IN_HEIGHT		0x7000049e
+#define REG_G_CAPZOOM_IN_XOFFS		0x700004a0
+#define REG_G_CAPZOOM_IN_YOFFS		0x700004a2
+
+/* n = 0...4 */
+#define REG_USER_SHARPNESS(n)		(0x70000a28 + (n) * 0xb6)
+
+/* Reduce sharpness range for user space API */
+#define SHARPNESS_DIV			8208
+#define TOK_TERM			0xffffffff
+
+/*
+ * FIXME: This is copied from s5k6aa, because of no information
+ * in the S5K4ECGX datasheet.
+ * H/W register Interface (0xd0000000 - 0xd0000fff)
+ */
+#define AHB_MSB_ADDR_PTR		0xfcfc
+#define GEN_REG_OFFSH			0xd000
+#define REG_CMDWR_ADDRH			0x0028
+#define REG_CMDWR_ADDRL			0x002a
+#define REG_CMDRD_ADDRH			0x002c
+#define REG_CMDRD_ADDRL			0x002e
+#define REG_CMDBUF0_ADDR		0x0f12
+
+struct s5k4ecgx_frmsize {
+	struct v4l2_frmsize_discrete size;
+	/* Fixed sensor matrix crop rectangle */
+	struct v4l2_rect input_window;
+};
+
+struct regval_list {
+	u32 addr;
+	u16 val;
+};
+
+/*
+ * TODO: currently only preview is supported and snapshot (capture)
+ * is not implemented yet
+ */
+static const struct s5k4ecgx_frmsize s5k4ecgx_prev_sizes[] = {
+	{
+		.size = { 176, 144 },
+		.input_window = { 0x00, 0x00, 0x928, 0x780 },
+	}, {
+		.size = { 352, 288 },
+		.input_window = { 0x00, 0x00, 0x928, 0x780 },
+	}, {
+		.size = { 640, 480 },
+		.input_window = { 0x00, 0x00, 0xa00, 0x780 },
+	}, {
+		.size = { 720, 480 },
+		.input_window = { 0x00, 0x00, 0xa00, 0x6a8 },
+	}
+};
+
+#define S5K4ECGX_NUM_PREV ARRAY_SIZE(s5k4ecgx_prev_sizes)
+
+struct s5k4ecgx_pixfmt {
+	enum v4l2_mbus_pixelcode code;
+	u32 colorspace;
+	/* REG_TC_PCFG_Format register value */
+	u16 reg_p_format;
+};
+
+/* By default value, output from sensor will be YUV422 0-255 */
+static const struct s5k4ecgx_pixfmt s5k4ecgx_formats[] = {
+	{ V4L2_MBUS_FMT_YUYV8_2X8, V4L2_COLORSPACE_JPEG, 5 },
+};
+
+static const char * const s5k4ecgx_supply_names[] = {
+	/*
+	 * Usually 2.8V is used for analog power (vdda)
+	 * and digital IO (vddio, vdddcore)
+	 */
+	"vdda",
+	"vddio",
+	"vddcore",
+	"vddreg", /* The internal s5k4ecgx regulator's supply (1.8V) */
+};
+
+#define S5K4ECGX_NUM_SUPPLIES ARRAY_SIZE(s5k4ecgx_supply_names)
+
+enum s5k4ecgx_gpio_id {
+	STBY,
+	RST,
+	GPIO_NUM,
+};
+
+struct s5k4ecgx {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct v4l2_ctrl_handler handler;
+
+	struct s5k4ecgx_platform_data *pdata;
+	const struct s5k4ecgx_pixfmt *curr_pixfmt;
+	const struct s5k4ecgx_frmsize *curr_frmsize;
+	struct mutex lock;
+	u8 streaming;
+	u8 set_params;
+
+	struct regulator_bulk_data supplies[S5K4ECGX_NUM_SUPPLIES];
+	struct s5k4ecgx_gpio gpio[GPIO_NUM];
+};
+
+static inline struct s5k4ecgx *to_s5k4ecgx(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct s5k4ecgx, sd);
+}
+
+static int s5k4ecgx_i2c_read(struct i2c_client *client, u16 addr, u16 *val)
+{
+	u8 wbuf[2] = { addr >> 8, addr & 0xff };
+	struct i2c_msg msg[2];
+	u8 rbuf[2];
+	int ret;
+
+	msg[0].addr = client->addr;
+	msg[0].flags = 0;
+	msg[0].len = 2;
+	msg[0].buf = wbuf;
+
+	msg[1].addr = client->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = 2;
+	msg[1].buf = rbuf;
+
+	ret = i2c_transfer(client->adapter, msg, 2);
+	*val = be16_to_cpu(*((u16 *)rbuf));
+
+	v4l2_dbg(4, debug, client, "i2c_read: 0x%04X : 0x%04x\n", addr, *val);
+
+	return ret == 2 ? 0 : ret;
+}
+
+static int s5k4ecgx_i2c_write(struct i2c_client *client, u16 addr, u16 val)
+{
+	u8 buf[4] = { addr >> 8, addr & 0xff, val >> 8, val & 0xff };
+
+	int ret = i2c_master_send(client, buf, 4);
+	v4l2_dbg(4, debug, client, "i2c_write: 0x%04x : 0x%04x\n", addr, val);
+
+	return ret == 4 ? 0 : ret;
+}
+
+static int s5k4ecgx_write(struct i2c_client *client, u32 addr, u16 val)
+{
+	u16 high = addr >> 16, low = addr & 0xffff;
+	int ret;
+
+	v4l2_dbg(3, debug, client, "write: 0x%08x : 0x%04x\n", addr, val);
+
+	ret = s5k4ecgx_i2c_write(client, REG_CMDWR_ADDRH, high);
+	if (!ret)
+		ret = s5k4ecgx_i2c_write(client, REG_CMDWR_ADDRL, low);
+	if (!ret)
+		ret = s5k4ecgx_i2c_write(client, REG_CMDBUF0_ADDR, val);
+
+	return ret;
+}
+
+static int s5k4ecgx_read(struct i2c_client *client, u32 addr, u16 *val)
+{
+	u16 high = addr >> 16, low =  addr & 0xffff;
+	int ret;
+
+	ret = s5k4ecgx_i2c_write(client, REG_CMDRD_ADDRH, high);
+	if (!ret)
+		ret = s5k4ecgx_i2c_write(client, REG_CMDRD_ADDRL, low);
+	if (!ret)
+		ret = s5k4ecgx_i2c_read(client, REG_CMDBUF0_ADDR, val);
+	if (!ret)
+		dev_err(&client->dev, "Failed to execute read command\n");
+
+	return ret;
+}
+
+static int s5k4ecgx_read_fw_ver(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u16 hw_rev, fw_ver = 0;
+	int ret;
+
+	ret = s5k4ecgx_read(client, REG_FW_VERSION, &fw_ver);
+	if (ret < 0 || fw_ver != S5K4ECGX_FW_VERSION) {
+		v4l2_err(sd, "FW version check failed!\n");
+		return -ENODEV;
+	}
+
+	ret = s5k4ecgx_read(client, REG_FW_REVISION, &hw_rev);
+	if (ret < 0)
+		return ret;
+
+	v4l2_info(sd, "chip found FW ver: 0x%x, HW rev: 0x%x\n",
+						fw_ver, hw_rev);
+	return 0;
+}
+
+static int s5k4ecgx_set_ahb_address(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+	/* Set APB peripherals start address */
+	ret = s5k4ecgx_i2c_write(client, AHB_MSB_ADDR_PTR, GEN_REG_OFFSH);
+	if (ret < 0)
+		return ret;
+	/*
+	 * FIXME: This is copied from s5k6aa, because of no information
+	 * in s5k4ecgx's datasheet.
+	 * sw_reset is activated to put device into idle status
+	 */
+	ret = s5k4ecgx_i2c_write(client, 0x0010, 0x0001);
+	if (ret < 0)
+		return ret;
+
+	ret = s5k4ecgx_i2c_write(client, 0x1030, 0x0000);
+	if (ret < 0)
+		return ret;
+	/* Halt ARM CPU */
+	return s5k4ecgx_i2c_write(client, 0x0014, 0x0001);
+}
+
+#define FW_CRC_SIZE	4
+/* Register address, value are 4, 2 bytes */
+#define FW_RECORD_SIZE	6
+/*
+ * The firmware has following format:
+ * < total number of records (4 bytes + 2 bytes padding) N >,
+ * < record 0 >, ..., < record N - 1 >, < CRC32-CCITT (4-bytes) >,
+ * where "record" is a 4-byte register address followed by 2-byte
+ * register value (little endian).
+ * The firmware generator can be found in following git repository:
+ * git://git.linaro.org/people/sangwook/fimc-v4l2-app.git
+ */
+static int s5k4ecgx_load_firmware(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	const struct firmware *fw;
+	const u8 *ptr;
+	int err, i, regs_num;
+	u32 addr, crc, crc_file, addr_inc = 0;
+	u16 val;
+
+	err = request_firmware(&fw, S5K4ECGX_FIRMWARE, sd->v4l2_dev->dev);
+	if (err) {
+		v4l2_err(sd, "Failed to read firmware %s\n", S5K4ECGX_FIRMWARE);
+		return err;
+	}
+	regs_num = le32_to_cpu(get_unaligned_le32(fw->data));
+
+	v4l2_dbg(3, debug, sd, "FW: %s size %d register sets %d\n",
+		 S5K4ECGX_FIRMWARE, fw->size, regs_num);
+
+	regs_num++; /* Add header */
+	if (fw->size != regs_num * FW_RECORD_SIZE + FW_CRC_SIZE) {
+		err = -EINVAL;
+		goto fw_out;
+	}
+	crc_file = le32_to_cpu(get_unaligned_le32(fw->data +
+						  regs_num * FW_RECORD_SIZE));
+	crc = crc32_le(~0, fw->data, regs_num * FW_RECORD_SIZE);
+	if (crc != crc_file) {
+		v4l2_err(sd, "FW: invalid crc (%#x:%#x)\n", crc, crc_file);
+		err = -EINVAL;
+		goto fw_out;
+	}
+	ptr = fw->data + FW_RECORD_SIZE;
+	for (i = 1; i < regs_num; i++) {
+		addr = le32_to_cpu(get_unaligned_le32(ptr));
+		ptr += sizeof(u32);
+		val = le16_to_cpu(get_unaligned_le16(ptr));
+		ptr += sizeof(u16);
+		if (addr - addr_inc != 2)
+			err = s5k4ecgx_write(client, addr, val);
+		else
+			err = s5k4ecgx_i2c_write(client, REG_CMDBUF0_ADDR, val);
+		if (err)
+			break;
+		addr_inc = addr;
+	}
+fw_out:
+	release_firmware(fw);
+	return err;
+}
+
+/* Set preview and capture input window */
+static int s5k4ecgx_set_input_window(struct i2c_client *c,
+				     const struct v4l2_rect *r)
+{
+	int ret;
+
+	ret = s5k4ecgx_write(c, REG_G_PREV_IN_WIDTH, r->width);
+	if (!ret)
+		ret = s5k4ecgx_write(c, REG_G_PREV_IN_HEIGHT, r->height);
+	if (!ret)
+		ret = s5k4ecgx_write(c, REG_G_PREV_IN_XOFFS, r->left);
+	if (!ret)
+		ret = s5k4ecgx_write(c, REG_G_PREV_IN_YOFFS, r->top);
+	if (!ret)
+		ret = s5k4ecgx_write(c, REG_G_CAP_IN_WIDTH, r->width);
+	if (!ret)
+		ret = s5k4ecgx_write(c, REG_G_CAP_IN_HEIGHT, r->height);
+	if (!ret)
+		ret = s5k4ecgx_write(c, REG_G_CAP_IN_XOFFS, r->left);
+	if (!ret)
+		ret = s5k4ecgx_write(c, REG_G_CAP_IN_YOFFS, r->top);
+
+	return ret;
+}
+
+/* Set preview and capture zoom input window */
+static int s5k4ecgx_set_zoom_window(struct i2c_client *c,
+				    const struct v4l2_rect *r)
+{
+	int ret;
+
+	ret = s5k4ecgx_write(c, REG_G_PREVZOOM_IN_WIDTH, r->width);
+	if (!ret)
+		ret = s5k4ecgx_write(c, REG_G_PREVZOOM_IN_HEIGHT, r->height);
+	if (!ret)
+		ret = s5k4ecgx_write(c, REG_G_PREVZOOM_IN_XOFFS, r->left);
+	if (!ret)
+		ret = s5k4ecgx_write(c, REG_G_PREVZOOM_IN_YOFFS, r->top);
+	if (!ret)
+		ret = s5k4ecgx_write(c, REG_G_CAPZOOM_IN_WIDTH, r->width);
+	if (!ret)
+		ret = s5k4ecgx_write(c, REG_G_CAPZOOM_IN_HEIGHT, r->height);
+	if (!ret)
+		ret = s5k4ecgx_write(c, REG_G_CAPZOOM_IN_XOFFS, r->left);
+	if (!ret)
+		ret = s5k4ecgx_write(c, REG_G_CAPZOOM_IN_YOFFS, r->top);
+
+	return ret;
+}
+
+static int s5k4ecgx_set_output_framefmt(struct s5k4ecgx *priv)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&priv->sd);
+	int ret;
+
+	ret = s5k4ecgx_write(client, REG_P_OUT_WIDTH(0),
+			     priv->curr_frmsize->size.width);
+	if (!ret)
+		ret = s5k4ecgx_write(client, REG_P_OUT_HEIGHT(0),
+				     priv->curr_frmsize->size.height);
+	if (!ret)
+		ret = s5k4ecgx_write(client, REG_P_FMT(0),
+				     priv->curr_pixfmt->reg_p_format);
+	return ret;
+}
+
+static int s5k4ecgx_init_sensor(struct v4l2_subdev *sd)
+{
+	int ret;
+
+	ret = s5k4ecgx_set_ahb_address(sd);
+
+	/* The delay is from manufacturer's settings */
+	msleep(100);
+
+	if (!ret)
+		ret = s5k4ecgx_load_firmware(sd);
+	if (ret)
+		v4l2_err(sd, "Failed to write initial settings\n");
+
+	return ret;
+}
+
+static int s5k4ecgx_gpio_set_value(struct s5k4ecgx *priv, int id, u32 val)
+{
+	if (!gpio_is_valid(priv->gpio[id].gpio))
+		return 0;
+	gpio_set_value(priv->gpio[id].gpio, val);
+
+	return 1;
+}
+
+static int __s5k4ecgx_power_on(struct s5k4ecgx *priv)
+{
+	int ret;
+
+	ret = regulator_bulk_enable(S5K4ECGX_NUM_SUPPLIES, priv->supplies);
+	if (ret)
+		return ret;
+	usleep_range(30, 50);
+
+	/* The polarity of STBY is controlled by TSP */
+	if (s5k4ecgx_gpio_set_value(priv, STBY, priv->gpio[STBY].level))
+		usleep_range(30, 50);
+
+	if (s5k4ecgx_gpio_set_value(priv, RST, priv->gpio[RST].level))
+		usleep_range(30, 50);
+
+	return 0;
+}
+
+static int __s5k4ecgx_power_off(struct s5k4ecgx *priv)
+{
+	if (s5k4ecgx_gpio_set_value(priv, RST, !priv->gpio[RST].level))
+		usleep_range(30, 50);
+
+	if (s5k4ecgx_gpio_set_value(priv, STBY, !priv->gpio[STBY].level))
+		usleep_range(30, 50);
+
+	priv->streaming = 0;
+
+	return regulator_bulk_disable(S5K4ECGX_NUM_SUPPLIES, priv->supplies);
+}
+
+/* Find nearest matching image pixel size. */
+static int s5k4ecgx_try_frame_size(struct v4l2_mbus_framefmt *mf,
+				  const struct s5k4ecgx_frmsize **size)
+{
+	unsigned int min_err = ~0;
+	int i = ARRAY_SIZE(s5k4ecgx_prev_sizes);
+	const struct s5k4ecgx_frmsize *fsize = &s5k4ecgx_prev_sizes[0],
+		*match = NULL;
+
+	while (i--) {
+		int err = abs(fsize->size.width - mf->width)
+				+ abs(fsize->size.height - mf->height);
+		if (err < min_err) {
+			min_err = err;
+			match = fsize;
+		}
+		fsize++;
+	}
+	if (match) {
+		mf->width  = match->size.width;
+		mf->height = match->size.height;
+		if (size)
+			*size = match;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int s5k4ecgx_enum_mbus_code(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_fh *fh,
+				   struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index >= ARRAY_SIZE(s5k4ecgx_formats))
+		return -EINVAL;
+	code->code = s5k4ecgx_formats[code->index].code;
+
+	return 0;
+}
+
+static int s5k4ecgx_get_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+			   struct v4l2_subdev_format *fmt)
+{
+	struct s5k4ecgx *priv = to_s5k4ecgx(sd);
+	struct v4l2_mbus_framefmt *mf;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		if (fh) {
+			mf = v4l2_subdev_get_try_format(fh, 0);
+			fmt->format = *mf;
+		}
+		return 0;
+	}
+
+	mf = &fmt->format;
+
+	mutex_lock(&priv->lock);
+	mf->width = priv->curr_frmsize->size.width;
+	mf->height = priv->curr_frmsize->size.height;
+	mf->code = priv->curr_pixfmt->code;
+	mf->colorspace = priv->curr_pixfmt->colorspace;
+	mf->field = V4L2_FIELD_NONE;
+	mutex_unlock(&priv->lock);
+
+	return 0;
+}
+
+static const struct s5k4ecgx_pixfmt *s5k4ecgx_try_fmt(struct v4l2_subdev *sd,
+					    struct v4l2_mbus_framefmt *mf)
+{
+	int i = ARRAY_SIZE(s5k4ecgx_formats);
+
+	while (--i)
+		if (mf->code == s5k4ecgx_formats[i].code)
+			break;
+	mf->code = s5k4ecgx_formats[i].code;
+
+	return &s5k4ecgx_formats[i];
+}
+
+static int s5k4ecgx_set_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+			    struct v4l2_subdev_format *fmt)
+{
+	struct s5k4ecgx *priv = to_s5k4ecgx(sd);
+	const struct s5k4ecgx_frmsize *fsize = NULL;
+	const struct s5k4ecgx_pixfmt *pf;
+	struct v4l2_mbus_framefmt *mf;
+	int ret = 0;
+
+	pf = s5k4ecgx_try_fmt(sd, &fmt->format);
+	s5k4ecgx_try_frame_size(&fmt->format, &fsize);
+	fmt->format.colorspace = V4L2_COLORSPACE_JPEG;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		if (fh) {
+			mf = v4l2_subdev_get_try_format(fh, 0);
+			*mf = fmt->format;
+		}
+		return 0;
+	}
+
+	mutex_lock(&priv->lock);
+	if (!priv->streaming) {
+		priv->curr_frmsize = fsize;
+		priv->curr_pixfmt = pf;
+		priv->set_params = 1;
+	} else {
+		ret = -EBUSY;
+	}
+	mutex_unlock(&priv->lock);
+
+	return ret;
+}
+
+static const struct v4l2_subdev_pad_ops s5k4ecgx_pad_ops = {
+	.enum_mbus_code	= s5k4ecgx_enum_mbus_code,
+	.get_fmt	= s5k4ecgx_get_fmt,
+	.set_fmt	= s5k4ecgx_set_fmt,
+};
+
+/*
+ * V4L2 subdev controls
+ */
+static int s5k4ecgx_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = &container_of(ctrl->handler, struct s5k4ecgx,
+						handler)->sd;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct s5k4ecgx *priv = to_s5k4ecgx(sd);
+	unsigned int i;
+	int err = 0;
+
+	v4l2_dbg(1, debug, sd, "ctrl: 0x%x, value: %d\n", ctrl->id, ctrl->val);
+
+	mutex_lock(&priv->lock);
+	switch (ctrl->id) {
+	case V4L2_CID_CONTRAST:
+		err = s5k4ecgx_write(client, REG_USER_CONTRAST, ctrl->val);
+		break;
+
+	case V4L2_CID_SATURATION:
+		err = s5k4ecgx_write(client, REG_USER_SATURATION, ctrl->val);
+		break;
+
+	case V4L2_CID_SHARPNESS:
+		/* TODO: Revisit, is this setting for all presets ? */
+		for (i = 0; i < 4 && !err; i++)
+			err = s5k4ecgx_write(client, REG_USER_SHARPNESS(i),
+					     ctrl->val * SHARPNESS_DIV);
+		break;
+
+	case V4L2_CID_BRIGHTNESS:
+		err = s5k4ecgx_write(client, REG_USER_BRIGHTNESS, ctrl->val);
+		break;
+	}
+	mutex_unlock(&priv->lock);
+	if (err < 0)
+		v4l2_err(sd, "Failed to write s_ctrl err %d\n", err);
+
+	return err;
+}
+
+static const struct v4l2_ctrl_ops s5k4ecgx_ctrl_ops = {
+	.s_ctrl = s5k4ecgx_s_ctrl,
+};
+
+/*
+ * Reading s5k4ecgx version information
+ */
+static int s5k4ecgx_registered(struct v4l2_subdev *sd)
+{
+	int ret;
+	struct s5k4ecgx *priv = to_s5k4ecgx(sd);
+
+	mutex_lock(&priv->lock);
+	ret = __s5k4ecgx_power_on(priv);
+	if (!ret) {
+		ret = s5k4ecgx_read_fw_ver(sd);
+		__s5k4ecgx_power_off(priv);
+	}
+	mutex_unlock(&priv->lock);
+
+	return ret;
+}
+
+/*
+ * V4L2 subdev internal operations
+ */
+static int s5k4ecgx_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_mbus_framefmt *mf = v4l2_subdev_get_try_format(fh, 0);
+
+	mf->width = s5k4ecgx_prev_sizes[0].size.width;
+	mf->height = s5k4ecgx_prev_sizes[0].size.height;
+	mf->code = s5k4ecgx_formats[0].code;
+	mf->colorspace = V4L2_COLORSPACE_JPEG;
+	mf->field = V4L2_FIELD_NONE;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_internal_ops s5k4ecgx_subdev_internal_ops = {
+	.registered = s5k4ecgx_registered,
+	.open = s5k4ecgx_open,
+};
+
+static int s5k4ecgx_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct s5k4ecgx *priv = to_s5k4ecgx(sd);
+	int ret;
+
+	v4l2_dbg(1, debug, sd, "Switching %s\n", on ? "on" : "off");
+
+	if (on) {
+		ret = __s5k4ecgx_power_on(priv);
+		if (ret < 0)
+			return ret;
+		/* Time to stabilize sensor */
+		msleep(100);
+		ret = s5k4ecgx_init_sensor(sd);
+		if (ret < 0)
+			__s5k4ecgx_power_off(priv);
+		else
+			priv->set_params = 1;
+	} else {
+		ret = __s5k4ecgx_power_off(priv);
+	}
+
+	return ret;
+}
+
+static int s5k4ecgx_log_status(struct v4l2_subdev *sd)
+{
+	v4l2_ctrl_handler_log_status(sd->ctrl_handler, sd->name);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops s5k4ecgx_core_ops = {
+	.s_power	= s5k4ecgx_s_power,
+	.log_status	= s5k4ecgx_log_status,
+};
+
+static int __s5k4ecgx_s_params(struct s5k4ecgx *priv)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&priv->sd);
+	const struct v4l2_rect *crop_rect = &priv->curr_frmsize->input_window;
+	int ret;
+
+	ret = s5k4ecgx_set_input_window(client, crop_rect);
+	if (!ret)
+		ret = s5k4ecgx_set_zoom_window(client, crop_rect);
+	if (!ret)
+		ret = s5k4ecgx_write(client, REG_G_INPUTS_CHANGE_REQ, 1);
+	if (!ret)
+		ret = s5k4ecgx_write(client, 0x70000a1e, 0x28);
+	if (!ret)
+		ret = s5k4ecgx_write(client, 0x70000ad4, 0x3c);
+	if (!ret)
+		ret = s5k4ecgx_set_output_framefmt(priv);
+	if (!ret)
+		ret = s5k4ecgx_write(client, REG_P_PVI_MASK(0), 0x52);
+	if (!ret)
+		ret = s5k4ecgx_write(client, REG_P_FR_TIME_TYPE(0),
+				     FR_TIME_DYNAMIC);
+	if (!ret)
+		ret = s5k4ecgx_write(client, REG_P_FR_TIME_Q_TYPE(0),
+				     FR_TIME_Q_BEST_FRRATE);
+	if (!ret)
+		ret = s5k4ecgx_write(client,  REG_P_MIN_FR_TIME(0),
+				     US_TO_FR_TIME(33300));
+	if (!ret)
+		ret = s5k4ecgx_write(client, REG_P_MAX_FR_TIME(0),
+				     US_TO_FR_TIME(66600));
+	if (!ret)
+		ret = s5k4ecgx_write(client, REG_P_PREV_MIRROR(0), 0);
+	if (!ret)
+		ret = s5k4ecgx_write(client, REG_P_CAP_MIRROR(0), 0);
+	if (!ret)
+		ret = s5k4ecgx_write(client, REG_G_ACTIVE_PREV_CFG, 0);
+	if (!ret)
+		ret = s5k4ecgx_write(client, REG_G_PREV_OPEN_AFTER_CH, 1);
+	if (!ret)
+		ret = s5k4ecgx_write(client, REG_G_NEW_CFG_SYNC, 1);
+	if (!ret)
+		ret = s5k4ecgx_write(client, REG_G_PREV_CFG_CHG, 1);
+
+	return ret;
+}
+
+static int __s5k4ecgx_s_stream(struct s5k4ecgx *priv, int on)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&priv->sd);
+	int ret;
+
+	if (on && priv->set_params) {
+		ret = __s5k4ecgx_s_params(priv);
+		if (ret < 0)
+			return ret;
+		priv->set_params = 0;
+	}
+	/*
+	 * This enables/disables preview stream only. Capture requests
+	 * are not supported yet.
+	 */
+	ret = s5k4ecgx_write(client, REG_G_ENABLE_PREV, on);
+	if (ret < 0)
+		return ret;
+	return s5k4ecgx_write(client, REG_G_ENABLE_PREV_CHG, 1);
+}
+
+static int s5k4ecgx_s_stream(struct v4l2_subdev *sd, int on)
+{
+	struct s5k4ecgx *priv = to_s5k4ecgx(sd);
+	int ret = 0;
+
+	v4l2_dbg(1, debug, sd, "Turn streaming %s\n", on ? "on" : "off");
+
+	mutex_lock(&priv->lock);
+
+	if (priv->streaming == !on) {
+		ret = __s5k4ecgx_s_stream(priv, on);
+		if (!ret)
+			priv->streaming = on & 1;
+	}
+
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static const struct v4l2_subdev_video_ops s5k4ecgx_video_ops = {
+	.s_stream = s5k4ecgx_s_stream,
+};
+
+static const struct v4l2_subdev_ops s5k4ecgx_ops = {
+	.core = &s5k4ecgx_core_ops,
+	.pad = &s5k4ecgx_pad_ops,
+	.video = &s5k4ecgx_video_ops,
+};
+
+/*
+ * GPIO setup
+ */
+static int s5k4ecgx_config_gpio(int nr, int val, const char *name)
+{
+	unsigned long flags = val ? GPIOF_OUT_INIT_HIGH : GPIOF_OUT_INIT_LOW;
+	int ret;
+
+	if (!gpio_is_valid(nr))
+		return 0;
+	ret = gpio_request_one(nr, flags, name);
+	if (!ret)
+		gpio_export(nr, 0);
+
+	return ret;
+}
+
+static void s5k4ecgx_free_gpios(struct s5k4ecgx *priv)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(priv->gpio); i++) {
+		if (!gpio_is_valid(priv->gpio[i].gpio))
+			continue;
+		gpio_free(priv->gpio[i].gpio);
+		priv->gpio[i].gpio = -EINVAL;
+	}
+}
+
+static int s5k4ecgx_config_gpios(struct s5k4ecgx *priv,
+				  const struct s5k4ecgx_platform_data *pdata)
+{
+	const struct s5k4ecgx_gpio *gpio = &pdata->gpio_stby;
+	int ret;
+
+	priv->gpio[STBY].gpio = -EINVAL;
+	priv->gpio[RST].gpio  = -EINVAL;
+
+	ret = s5k4ecgx_config_gpio(gpio->gpio, gpio->level, "S5K4ECGX_STBY");
+
+	if (ret) {
+		s5k4ecgx_free_gpios(priv);
+		return ret;
+	}
+	priv->gpio[STBY] = *gpio;
+	if (gpio_is_valid(gpio->gpio))
+		gpio_set_value(gpio->gpio, 0);
+
+	gpio = &pdata->gpio_reset;
+
+	ret = s5k4ecgx_config_gpio(gpio->gpio, gpio->level, "S5K4ECGX_RST");
+	if (ret) {
+		s5k4ecgx_free_gpios(priv);
+		return ret;
+	}
+	priv->gpio[RST] = *gpio;
+	if (gpio_is_valid(gpio->gpio))
+		gpio_set_value(gpio->gpio, 0);
+
+	return 0;
+}
+
+static int s5k4ecgx_init_v4l2_ctrls(struct s5k4ecgx *priv)
+{
+	const struct v4l2_ctrl_ops *ops = &s5k4ecgx_ctrl_ops;
+	struct v4l2_ctrl_handler *hdl = &priv->handler;
+	int ret;
+
+	ret = v4l2_ctrl_handler_init(hdl, 4);
+	if (ret)
+		return ret;
+
+	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BRIGHTNESS, -208, 127, 1, 0);
+	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_CONTRAST, -127, 127, 1, 0);
+	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SATURATION, -127, 127, 1, 0);
+
+	/* Sharpness default is 24612, and then (24612/SHARPNESS_DIV) = 2 */
+	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SHARPNESS, -32704/SHARPNESS_DIV,
+			  24612/SHARPNESS_DIV, 1, 2);
+	if (hdl->error) {
+		ret = hdl->error;
+		v4l2_ctrl_handler_free(hdl);
+		return ret;
+	}
+	priv->sd.ctrl_handler = hdl;
+
+	return 0;
+};
+
+static int s5k4ecgx_probe(struct i2c_client *client,
+			  const struct i2c_device_id *id)
+{
+	struct s5k4ecgx_platform_data *pdata = client->dev.platform_data;
+	struct v4l2_subdev *sd;
+	struct s5k4ecgx *priv;
+	int ret, i;
+
+	if (pdata == NULL) {
+		dev_err(&client->dev, "platform data is missing!\n");
+		return -EINVAL;
+	}
+
+	priv = devm_kzalloc(&client->dev, sizeof(struct s5k4ecgx), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	mutex_init(&priv->lock);
+	priv->streaming = 0;
+
+	sd = &priv->sd;
+	/* Registering subdev */
+	v4l2_i2c_subdev_init(sd, client, &s5k4ecgx_ops);
+	strlcpy(sd->name, S5K4ECGX_DRIVER_NAME, sizeof(sd->name));
+
+	sd->internal_ops = &s5k4ecgx_subdev_internal_ops;
+	/* Support v4l2 sub-device user space API */
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	priv->pad.flags = MEDIA_PAD_FL_SOURCE;
+	sd->entity.type = MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
+	ret = media_entity_init(&sd->entity, 1, &priv->pad, 0);
+	if (ret)
+		return ret;
+
+	ret = s5k4ecgx_config_gpios(priv, pdata);
+	if (ret) {
+		dev_err(&client->dev, "Failed to set gpios\n");
+		goto out_err1;
+	}
+	for (i = 0; i < S5K4ECGX_NUM_SUPPLIES; i++)
+		priv->supplies[i].supply = s5k4ecgx_supply_names[i];
+
+	ret = devm_regulator_bulk_get(&client->dev, S5K4ECGX_NUM_SUPPLIES,
+				 priv->supplies);
+	if (ret) {
+		dev_err(&client->dev, "Failed to get regulators\n");
+		goto out_err2;
+	}
+	ret = s5k4ecgx_init_v4l2_ctrls(priv);
+	if (ret)
+		goto out_err2;
+
+	priv->curr_pixfmt = &s5k4ecgx_formats[0];
+	priv->curr_frmsize = &s5k4ecgx_prev_sizes[0];
+
+	return 0;
+
+out_err2:
+	s5k4ecgx_free_gpios(priv);
+out_err1:
+	media_entity_cleanup(&priv->sd.entity);
+
+	return ret;
+}
+
+static int s5k4ecgx_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct s5k4ecgx *priv = to_s5k4ecgx(sd);
+
+	mutex_destroy(&priv->lock);
+	s5k4ecgx_free_gpios(priv);
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&priv->handler);
+	media_entity_cleanup(&sd->entity);
+
+	return 0;
+}
+
+static const struct i2c_device_id s5k4ecgx_id[] = {
+	{ S5K4ECGX_DRIVER_NAME, 0 },
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, s5k4ecgx_id);
+
+static struct i2c_driver v4l2_i2c_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name = S5K4ECGX_DRIVER_NAME,
+	},
+	.probe = s5k4ecgx_probe,
+	.remove = s5k4ecgx_remove,
+	.id_table = s5k4ecgx_id,
+};
+
+module_i2c_driver(v4l2_i2c_driver);
+
+MODULE_DESCRIPTION("Samsung S5K4ECGX 5MP SOC camera");
+MODULE_AUTHOR("Sangwook Lee <sangwook.lee@linaro.org>");
+MODULE_AUTHOR("Seok-Young Jang <quartz.jang@samsung.com>");
+MODULE_LICENSE("GPL");
+MODULE_FIRMWARE(S5K4ECGX_FIRMWARE);
diff --git a/drivers/media/i2c/s5k6aa.c b/drivers/media/i2c/s5k6aa.c
new file mode 100644
index 000000000000..57cd4fa0193d
--- /dev/null
+++ b/drivers/media/i2c/s5k6aa.c
@@ -0,0 +1,1664 @@
+/*
+ * Driver for Samsung S5K6AAFX SXGA 1/6" 1.3M CMOS Image Sensor
+ * with embedded SoC ISP.
+ *
+ * Copyright (C) 2011, Samsung Electronics Co., Ltd.
+ * Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * Based on a driver authored by Dongsoo Nathaniel Kim.
+ * Copyright (C) 2009, Dongsoo Nathaniel Kim <dongsoo45.kim@samsung.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; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/i2c.h>
+#include <linux/media.h>
+#include <linux/module.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/s5k6aa.h>
+
+static int debug;
+module_param(debug, int, 0644);
+
+#define DRIVER_NAME			"S5K6AA"
+
+/* The token to indicate array termination */
+#define S5K6AA_TERM			0xffff
+#define S5K6AA_OUT_WIDTH_DEF		640
+#define S5K6AA_OUT_HEIGHT_DEF		480
+#define S5K6AA_WIN_WIDTH_MAX		1280
+#define S5K6AA_WIN_HEIGHT_MAX		1024
+#define S5K6AA_WIN_WIDTH_MIN		8
+#define S5K6AA_WIN_HEIGHT_MIN		8
+
+/*
+ * H/W register Interface (0xD0000000 - 0xD0000FFF)
+ */
+#define AHB_MSB_ADDR_PTR		0xfcfc
+#define GEN_REG_OFFSH			0xd000
+#define REG_CMDWR_ADDRH			0x0028
+#define REG_CMDWR_ADDRL			0x002a
+#define REG_CMDRD_ADDRH			0x002c
+#define REG_CMDRD_ADDRL			0x002e
+#define REG_CMDBUF0_ADDR		0x0f12
+#define REG_CMDBUF1_ADDR		0x0f10
+
+/*
+ * Host S/W Register interface (0x70000000 - 0x70002000)
+ * The value of the two most significant address bytes is 0x7000,
+ * (HOST_SWIF_OFFS_H). The register addresses below specify 2 LSBs.
+ */
+#define HOST_SWIF_OFFSH			0x7000
+
+/* 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		27000U
+#define REG_I_USE_NPVI_CLOCKS		0x01c6
+#define REG_I_USE_NMIPI_CLOCKS		0x01c8
+
+/* 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  SYS_PLL_OUT_FREQ		(48000000 / 4000)
+#define  PCLK_FREQ_MIN			(24000000 / 4000)
+#define  PCLK_FREQ_MAX			(48000000 / 4000)
+#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_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
+
+/* 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 REG_P_CLK_INDEX(n)		PREG(n, 0x024e)
+#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_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)
+/* Conversion to REG_P_[MAX/MIN]_FR_TIME value; __t: time in us */
+#define  US_TO_FR_TIME(__t)		((__t) / 100)
+#define  S5K6AA_MIN_FR_TIME		33300  /* us */
+#define  S5K6AA_MAX_FR_TIME		650000 /* us */
+#define  S5K6AA_MAX_HIGHRES_FR_TIME	666    /* x100 us */
+/* The below 5 registers are for "device correction" values */
+#define REG_P_COLORTEMP(n)		PREG(n, 0x025e)
+#define REG_P_PREV_MIRROR(n)		PREG(n, 0x0262)
+
+/* Extended image property controls */
+/* Exposure time in 10 us units */
+#define REG_SF_USR_EXPOSURE_L		0x03c6
+#define REG_SF_USR_EXPOSURE_H		0x03c8
+#define REG_SF_USR_EXPOSURE_CHG		0x03ca
+#define REG_SF_USR_TOT_GAIN		0x03cc
+#define REG_SF_USR_TOT_GAIN_CHG		0x03ce
+#define REG_SF_RGAIN			0x03d0
+#define REG_SF_RGAIN_CHG		0x03d2
+#define REG_SF_GGAIN			0x03d4
+#define REG_SF_GGAIN_CHG		0x03d6
+#define REG_SF_BGAIN			0x03d8
+#define REG_SF_BGAIN_CHG		0x03da
+#define REG_SF_FLICKER_QUANT		0x03dc
+#define REG_SF_FLICKER_QUANT_CHG	0x03de
+
+/* Output interface (parallel/MIPI) setup */
+#define REG_OIF_EN_MIPI_LANES		0x03fa
+#define REG_OIF_EN_PACKETS		0x03fc
+#define REG_OIF_CFG_CHG			0x03fe
+
+/* Auto-algorithms enable mask */
+#define REG_DBG_AUTOALG_EN		0x0400
+#define  AALG_ALL_EN_MASK		(1 << 0)
+#define  AALG_AE_EN_MASK		(1 << 1)
+#define  AALG_DIVLEI_EN_MASK		(1 << 2)
+#define  AALG_WB_EN_MASK		(1 << 3)
+#define  AALG_FLICKER_EN_MASK		(1 << 5)
+#define  AALG_FIT_EN_MASK		(1 << 6)
+#define  AALG_WRHW_EN_MASK		(1 << 7)
+
+/* Firmware revision information */
+#define REG_FW_APIVER			0x012e
+#define  S5K6AAFX_FW_APIVER		0x0001
+#define REG_FW_REVISION			0x0130
+
+/* For now we use only one user configuration register set */
+#define S5K6AA_MAX_PRESETS		1
+
+static const char * const s5k6aa_supply_names[] = {
+	"vdd_core",	/* Digital core supply 1.5V (1.4V to 1.6V) */
+	"vdda",		/* Analog power supply 2.8V (2.6V to 3.0V) */
+	"vdd_reg",	/* Regulator input power 1.8V (1.7V to 1.9V)
+			   or 2.8V (2.6V to 3.0) */
+	"vddio",	/* I/O supply 1.8V (1.65V to 1.95V)
+			   or 2.8V (2.5V to 3.1V) */
+};
+#define S5K6AA_NUM_SUPPLIES ARRAY_SIZE(s5k6aa_supply_names)
+
+enum s5k6aa_gpio_id {
+	STBY,
+	RST,
+	GPIO_NUM,
+};
+
+struct s5k6aa_regval {
+	u16 addr;
+	u16 val;
+};
+
+struct s5k6aa_pixfmt {
+	enum v4l2_mbus_pixelcode code;
+	u32 colorspace;
+	/* REG_P_FMT(x) register value */
+	u16 reg_p_fmt;
+};
+
+struct s5k6aa_preset {
+	/* output pixel format and resolution */
+	struct v4l2_mbus_framefmt mbus_fmt;
+	u8 clk_id;
+	u8 index;
+};
+
+struct s5k6aa_ctrls {
+	struct v4l2_ctrl_handler handler;
+	/* Auto / manual white balance cluster */
+	struct v4l2_ctrl *awb;
+	struct v4l2_ctrl *gain_red;
+	struct v4l2_ctrl *gain_blue;
+	struct v4l2_ctrl *gain_green;
+	/* Mirror cluster */
+	struct v4l2_ctrl *hflip;
+	struct v4l2_ctrl *vflip;
+	/* Auto exposure / manual exposure and gain cluster */
+	struct v4l2_ctrl *auto_exp;
+	struct v4l2_ctrl *exposure;
+	struct v4l2_ctrl *gain;
+};
+
+struct s5k6aa_interval {
+	u16 reg_fr_time;
+	struct v4l2_fract interval;
+	/* Maximum rectangle for the interval */
+	struct v4l2_frmsize_discrete size;
+};
+
+struct s5k6aa {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+
+	enum v4l2_mbus_type bus_type;
+	u8 mipi_lanes;
+
+	int (*s_power)(int enable);
+	struct regulator_bulk_data supplies[S5K6AA_NUM_SUPPLIES];
+	struct s5k6aa_gpio gpio[GPIO_NUM];
+
+	/* external master clock frequency */
+	unsigned long mclk_frequency;
+	/* ISP internal master clock frequency */
+	u16 clk_fop;
+	/* output pixel clock frequency range */
+	u16 pclk_fmin;
+	u16 pclk_fmax;
+
+	unsigned int inv_hflip:1;
+	unsigned int inv_vflip:1;
+
+	/* protects the struct members below */
+	struct mutex lock;
+
+	/* sensor matrix scan window */
+	struct v4l2_rect ccd_rect;
+
+	struct s5k6aa_ctrls ctrls;
+	struct s5k6aa_preset presets[S5K6AA_MAX_PRESETS];
+	struct s5k6aa_preset *preset;
+	const struct s5k6aa_interval *fiv;
+
+	unsigned int streaming:1;
+	unsigned int apply_cfg:1;
+	unsigned int apply_crop:1;
+	unsigned int power;
+};
+
+static struct s5k6aa_regval s5k6aa_analog_config[] = {
+	/* Analog settings */
+	{ 0x112a, 0x0000 }, { 0x1132, 0x0000 },
+	{ 0x113e, 0x0000 }, { 0x115c, 0x0000 },
+	{ 0x1164, 0x0000 }, { 0x1174, 0x0000 },
+	{ 0x1178, 0x0000 }, { 0x077a, 0x0000 },
+	{ 0x077c, 0x0000 }, { 0x077e, 0x0000 },
+	{ 0x0780, 0x0000 }, { 0x0782, 0x0000 },
+	{ 0x0784, 0x0000 }, { 0x0786, 0x0000 },
+	{ 0x0788, 0x0000 }, { 0x07a2, 0x0000 },
+	{ 0x07a4, 0x0000 }, { 0x07a6, 0x0000 },
+	{ 0x07a8, 0x0000 }, { 0x07b6, 0x0000 },
+	{ 0x07b8, 0x0002 }, { 0x07ba, 0x0004 },
+	{ 0x07bc, 0x0004 }, { 0x07be, 0x0005 },
+	{ 0x07c0, 0x0005 }, { S5K6AA_TERM, 0 },
+};
+
+/* TODO: Add RGB888 and Bayer format */
+static const struct s5k6aa_pixfmt s5k6aa_formats[] = {
+	{ V4L2_MBUS_FMT_YUYV8_2X8,	V4L2_COLORSPACE_JPEG,	5 },
+	/* range 16-240 */
+	{ V4L2_MBUS_FMT_YUYV8_2X8,	V4L2_COLORSPACE_REC709,	6 },
+	{ V4L2_MBUS_FMT_RGB565_2X8_BE,	V4L2_COLORSPACE_JPEG,	0 },
+};
+
+static const struct s5k6aa_interval s5k6aa_intervals[] = {
+	{ 1000, {10000, 1000000}, {1280, 1024} }, /* 10 fps */
+	{ 666,  {15000, 1000000}, {1280, 1024} }, /* 15 fps */
+	{ 500,  {20000, 1000000}, {1280, 720} },  /* 20 fps */
+	{ 400,  {25000, 1000000}, {640, 480} },   /* 25 fps */
+	{ 333,  {33300, 1000000}, {640, 480} },   /* 30 fps */
+};
+
+#define S5K6AA_INTERVAL_DEF_INDEX 1 /* 15 fps */
+
+static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct s5k6aa, ctrls.handler)->sd;
+}
+
+static inline struct s5k6aa *to_s5k6aa(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct s5k6aa, sd);
+}
+
+/* Set initial values for all preview presets */
+static void s5k6aa_presets_data_init(struct s5k6aa *s5k6aa)
+{
+	struct s5k6aa_preset *preset = &s5k6aa->presets[0];
+	int i;
+
+	for (i = 0; i < S5K6AA_MAX_PRESETS; i++) {
+		preset->mbus_fmt.width	= S5K6AA_OUT_WIDTH_DEF;
+		preset->mbus_fmt.height	= S5K6AA_OUT_HEIGHT_DEF;
+		preset->mbus_fmt.code	= s5k6aa_formats[0].code;
+		preset->index		= i;
+		preset->clk_id		= 0;
+		preset++;
+	}
+
+	s5k6aa->fiv = &s5k6aa_intervals[S5K6AA_INTERVAL_DEF_INDEX];
+	s5k6aa->preset = &s5k6aa->presets[0];
+}
+
+static int s5k6aa_i2c_read(struct i2c_client *client, u16 addr, u16 *val)
+{
+	u8 wbuf[2] = {addr >> 8, addr & 0xFF};
+	struct i2c_msg msg[2];
+	u8 rbuf[2];
+	int ret;
+
+	msg[0].addr = client->addr;
+	msg[0].flags = 0;
+	msg[0].len = 2;
+	msg[0].buf = wbuf;
+
+	msg[1].addr = client->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = 2;
+	msg[1].buf = rbuf;
+
+	ret = i2c_transfer(client->adapter, msg, 2);
+	*val = be16_to_cpu(*((u16 *)rbuf));
+
+	v4l2_dbg(3, debug, client, "i2c_read: 0x%04X : 0x%04x\n", addr, *val);
+
+	return ret == 2 ? 0 : ret;
+}
+
+static int s5k6aa_i2c_write(struct i2c_client *client, u16 addr, u16 val)
+{
+	u8 buf[4] = {addr >> 8, addr & 0xFF, val >> 8, val & 0xFF};
+
+	int ret = i2c_master_send(client, buf, 4);
+	v4l2_dbg(3, debug, client, "i2c_write: 0x%04X : 0x%04x\n", addr, val);
+
+	return ret == 4 ? 0 : ret;
+}
+
+/* The command register write, assumes Command_Wr_addH = 0x7000. */
+static int s5k6aa_write(struct i2c_client *c, u16 addr, u16 val)
+{
+	int ret = s5k6aa_i2c_write(c, REG_CMDWR_ADDRL, addr);
+	if (ret)
+		return ret;
+	return s5k6aa_i2c_write(c, REG_CMDBUF0_ADDR, val);
+}
+
+/* The command register read, assumes Command_Rd_addH = 0x7000. */
+static int s5k6aa_read(struct i2c_client *client, u16 addr, u16 *val)
+{
+	int ret = s5k6aa_i2c_write(client, REG_CMDRD_ADDRL, addr);
+	if (ret)
+		return ret;
+	return s5k6aa_i2c_read(client, REG_CMDBUF0_ADDR, val);
+}
+
+static int s5k6aa_write_array(struct v4l2_subdev *sd,
+			      const struct s5k6aa_regval *msg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u16 addr_incr = 0;
+	int ret = 0;
+
+	while (msg->addr != S5K6AA_TERM) {
+		if (addr_incr != 2)
+			ret = s5k6aa_i2c_write(client, REG_CMDWR_ADDRL,
+					       msg->addr);
+		if (ret)
+			break;
+		ret = s5k6aa_i2c_write(client, REG_CMDBUF0_ADDR, msg->val);
+		if (ret)
+			break;
+		/* Assume that msg->addr is always less than 0xfffc */
+		addr_incr = (msg + 1)->addr - msg->addr;
+		msg++;
+	}
+
+	return ret;
+}
+
+/* Configure the AHB high address bytes for GTG registers access */
+static int s5k6aa_set_ahb_address(struct i2c_client *client)
+{
+	int ret = s5k6aa_i2c_write(client, AHB_MSB_ADDR_PTR, GEN_REG_OFFSH);
+	if (ret)
+		return ret;
+	ret = s5k6aa_i2c_write(client, REG_CMDRD_ADDRH, HOST_SWIF_OFFSH);
+	if (ret)
+		return ret;
+	return s5k6aa_i2c_write(client, REG_CMDWR_ADDRH, HOST_SWIF_OFFSH);
+}
+
+/**
+ * s5k6aa_configure_pixel_clock - apply ISP main clock/PLL configuration
+ *
+ * Configure the internal ISP PLL for the required output frequency.
+ * Locking: called with s5k6aa.lock mutex held.
+ */
+static int s5k6aa_configure_pixel_clocks(struct s5k6aa *s5k6aa)
+{
+	struct i2c_client *c = v4l2_get_subdevdata(&s5k6aa->sd);
+	unsigned long fmclk = s5k6aa->mclk_frequency / 1000;
+	u16 status;
+	int ret;
+
+	if (WARN(fmclk < MIN_MCLK_FREQ_KHZ || fmclk > MAX_MCLK_FREQ_KHZ,
+		 "Invalid clock frequency: %ld\n", fmclk))
+		return -EINVAL;
+
+	s5k6aa->pclk_fmin = PCLK_FREQ_MIN;
+	s5k6aa->pclk_fmax = PCLK_FREQ_MAX;
+	s5k6aa->clk_fop = SYS_PLL_OUT_FREQ;
+
+	/* External input clock frequency in kHz */
+	ret = s5k6aa_write(c, REG_I_INCLK_FREQ_H, fmclk >> 16);
+	if (!ret)
+		ret = s5k6aa_write(c, REG_I_INCLK_FREQ_L, fmclk & 0xFFFF);
+	if (!ret)
+		ret = s5k6aa_write(c, REG_I_USE_NPVI_CLOCKS, 1);
+	/* Internal PLL frequency */
+	if (!ret)
+		ret = s5k6aa_write(c, REG_I_OPCLK_4KHZ(0), s5k6aa->clk_fop);
+	if (!ret)
+		ret = s5k6aa_write(c, REG_I_MIN_OUTRATE_4KHZ(0),
+				   s5k6aa->pclk_fmin);
+	if (!ret)
+		ret = s5k6aa_write(c, REG_I_MAX_OUTRATE_4KHZ(0),
+				   s5k6aa->pclk_fmax);
+	if (!ret)
+		ret = s5k6aa_write(c, REG_I_INIT_PARAMS_UPDATED, 1);
+	if (!ret)
+		ret = s5k6aa_read(c, REG_I_ERROR_INFO, &status);
+
+	return ret ? ret : (status ? -EINVAL : 0);
+}
+
+/* Set horizontal and vertical image flipping */
+static int s5k6aa_set_mirror(struct s5k6aa *s5k6aa, int horiz_flip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&s5k6aa->sd);
+	int index = s5k6aa->preset->index;
+
+	unsigned int vflip = s5k6aa->ctrls.vflip->val ^ s5k6aa->inv_vflip;
+	unsigned int flip = (horiz_flip ^ s5k6aa->inv_hflip) | (vflip << 1);
+
+	return s5k6aa_write(client, REG_P_PREV_MIRROR(index), flip);
+}
+
+/* Configure auto/manual white balance and R/G/B gains */
+static int s5k6aa_set_awb(struct s5k6aa *s5k6aa, int awb)
+{
+	struct i2c_client *c = v4l2_get_subdevdata(&s5k6aa->sd);
+	struct s5k6aa_ctrls *ctrls = &s5k6aa->ctrls;
+	u16 reg;
+
+	int ret = s5k6aa_read(c, REG_DBG_AUTOALG_EN, &reg);
+
+	if (!ret && !awb) {
+		ret = s5k6aa_write(c, REG_SF_RGAIN, ctrls->gain_red->val);
+		if (!ret)
+			ret = s5k6aa_write(c, REG_SF_RGAIN_CHG, 1);
+		if (ret)
+			return ret;
+
+		ret = s5k6aa_write(c, REG_SF_GGAIN, ctrls->gain_green->val);
+		if (!ret)
+			ret = s5k6aa_write(c, REG_SF_GGAIN_CHG, 1);
+		if (ret)
+			return ret;
+
+		ret = s5k6aa_write(c, REG_SF_BGAIN, ctrls->gain_blue->val);
+		if (!ret)
+			ret = s5k6aa_write(c, REG_SF_BGAIN_CHG, 1);
+	}
+	if (!ret) {
+		reg = awb ? reg | AALG_WB_EN_MASK : reg & ~AALG_WB_EN_MASK;
+		ret = s5k6aa_write(c, REG_DBG_AUTOALG_EN, reg);
+	}
+
+	return ret;
+}
+
+/* Program FW with exposure time, 'exposure' in us units */
+static int s5k6aa_set_user_exposure(struct i2c_client *client, int exposure)
+{
+	unsigned int time = exposure / 10;
+
+	int ret = s5k6aa_write(client, REG_SF_USR_EXPOSURE_L, time & 0xffff);
+	if (!ret)
+		ret = s5k6aa_write(client, REG_SF_USR_EXPOSURE_H, time >> 16);
+	if (ret)
+		return ret;
+	return s5k6aa_write(client, REG_SF_USR_EXPOSURE_CHG, 1);
+}
+
+static int s5k6aa_set_user_gain(struct i2c_client *client, int gain)
+{
+	int ret = s5k6aa_write(client, REG_SF_USR_TOT_GAIN, gain);
+	if (ret)
+		return ret;
+	return s5k6aa_write(client, REG_SF_USR_TOT_GAIN_CHG, 1);
+}
+
+/* Set auto/manual exposure and total gain */
+static int s5k6aa_set_auto_exposure(struct s5k6aa *s5k6aa, int value)
+{
+	struct i2c_client *c = v4l2_get_subdevdata(&s5k6aa->sd);
+	unsigned int exp_time = s5k6aa->ctrls.exposure->val;
+	u16 auto_alg;
+
+	int ret = s5k6aa_read(c, REG_DBG_AUTOALG_EN, &auto_alg);
+	if (ret)
+		return ret;
+
+	v4l2_dbg(1, debug, c, "man_exp: %d, auto_exp: %d, a_alg: 0x%x\n",
+		 exp_time, value, auto_alg);
+
+	if (value == V4L2_EXPOSURE_AUTO) {
+		auto_alg |= AALG_AE_EN_MASK | AALG_DIVLEI_EN_MASK;
+	} else {
+		ret = s5k6aa_set_user_exposure(c, exp_time);
+		if (ret)
+			return ret;
+		ret = s5k6aa_set_user_gain(c, s5k6aa->ctrls.gain->val);
+		if (ret)
+			return ret;
+		auto_alg &= ~(AALG_AE_EN_MASK | AALG_DIVLEI_EN_MASK);
+	}
+
+	return s5k6aa_write(c, REG_DBG_AUTOALG_EN, auto_alg);
+}
+
+static int s5k6aa_set_anti_flicker(struct s5k6aa *s5k6aa, int value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&s5k6aa->sd);
+	u16 auto_alg;
+	int ret;
+
+	ret = s5k6aa_read(client, REG_DBG_AUTOALG_EN, &auto_alg);
+	if (ret)
+		return ret;
+
+	if (value == V4L2_CID_POWER_LINE_FREQUENCY_AUTO) {
+		auto_alg |= AALG_FLICKER_EN_MASK;
+	} else {
+		auto_alg &= ~AALG_FLICKER_EN_MASK;
+		/* The V4L2_CID_LINE_FREQUENCY control values match
+		 * the register values */
+		ret = s5k6aa_write(client, REG_SF_FLICKER_QUANT, value);
+		if (ret)
+			return ret;
+		ret = s5k6aa_write(client, REG_SF_FLICKER_QUANT_CHG, 1);
+		if (ret)
+			return ret;
+	}
+
+	return s5k6aa_write(client, REG_DBG_AUTOALG_EN, auto_alg);
+}
+
+static int s5k6aa_set_colorfx(struct s5k6aa *s5k6aa, int val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&s5k6aa->sd);
+	static const struct v4l2_control 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 },
+	};
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(colorfx); i++) {
+		if (colorfx[i].id == val)
+			return s5k6aa_write(client, REG_G_SPEC_EFFECTS,
+					    colorfx[i].value);
+	}
+	return -EINVAL;
+}
+
+static int s5k6aa_preview_config_status(struct i2c_client *client)
+{
+	u16 error = 0;
+	int ret = s5k6aa_read(client, REG_G_PREV_CFG_ERROR, &error);
+
+	v4l2_dbg(1, debug, client, "error: 0x%x (%d)\n", error, ret);
+	return ret ? ret : (error ? -EINVAL : 0);
+}
+
+static int s5k6aa_get_pixfmt_index(struct s5k6aa *s5k6aa,
+				   struct v4l2_mbus_framefmt *mf)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(s5k6aa_formats); i++)
+		if (mf->colorspace == s5k6aa_formats[i].colorspace &&
+		    mf->code == s5k6aa_formats[i].code)
+			return i;
+	return 0;
+}
+
+static int s5k6aa_set_output_framefmt(struct s5k6aa *s5k6aa,
+				      struct s5k6aa_preset *preset)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&s5k6aa->sd);
+	int fmt_index = s5k6aa_get_pixfmt_index(s5k6aa, &preset->mbus_fmt);
+	int ret;
+
+	ret = s5k6aa_write(client, REG_P_OUT_WIDTH(preset->index),
+			   preset->mbus_fmt.width);
+	if (!ret)
+		ret = s5k6aa_write(client, REG_P_OUT_HEIGHT(preset->index),
+				   preset->mbus_fmt.height);
+	if (!ret)
+		ret = s5k6aa_write(client, REG_P_FMT(preset->index),
+				   s5k6aa_formats[fmt_index].reg_p_fmt);
+	return ret;
+}
+
+static int s5k6aa_set_input_params(struct s5k6aa *s5k6aa)
+{
+	struct i2c_client *c = v4l2_get_subdevdata(&s5k6aa->sd);
+	struct v4l2_rect *r = &s5k6aa->ccd_rect;
+	int ret;
+
+	ret = s5k6aa_write(c, REG_G_PREVZOOM_IN_WIDTH, r->width);
+	if (!ret)
+		ret = s5k6aa_write(c, REG_G_PREVZOOM_IN_HEIGHT, r->height);
+	if (!ret)
+		ret = s5k6aa_write(c, REG_G_PREVZOOM_IN_XOFFS, r->left);
+	if (!ret)
+		ret = s5k6aa_write(c, REG_G_PREVZOOM_IN_YOFFS, r->top);
+	if (!ret)
+		ret = s5k6aa_write(c, REG_G_INPUTS_CHANGE_REQ, 1);
+	if (!ret)
+		s5k6aa->apply_crop = 0;
+
+	return ret;
+}
+
+/**
+ * s5k6aa_configure_video_bus - configure the video output interface
+ * @bus_type: video bus type: parallel or MIPI-CSI
+ * @nlanes: number of MIPI lanes to be used (MIPI-CSI only)
+ *
+ * Note: Only parallel bus operation has been tested.
+ */
+static int s5k6aa_configure_video_bus(struct s5k6aa *s5k6aa,
+				      enum v4l2_mbus_type bus_type, int nlanes)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&s5k6aa->sd);
+	u16 cfg = 0;
+	int ret;
+
+	/*
+	 * TODO: The sensor is supposed to support BT.601 and BT.656
+	 * but there is nothing indicating how to switch between both
+	 * in the datasheet. For now default BT.601 interface is assumed.
+	 */
+	if (bus_type == V4L2_MBUS_CSI2)
+		cfg = nlanes;
+	else if (bus_type != V4L2_MBUS_PARALLEL)
+		return -EINVAL;
+
+	ret = s5k6aa_write(client, REG_OIF_EN_MIPI_LANES, cfg);
+	if (ret)
+		return ret;
+	return s5k6aa_write(client, REG_OIF_CFG_CHG, 1);
+}
+
+/* This function should be called when switching to new user configuration set*/
+static int s5k6aa_new_config_sync(struct i2c_client *client, int timeout,
+				  int cid)
+{
+	unsigned long end = jiffies + msecs_to_jiffies(timeout);
+	u16 reg = 1;
+	int ret;
+
+	ret = s5k6aa_write(client, REG_G_ACTIVE_PREV_CFG, cid);
+	if (!ret)
+		ret = s5k6aa_write(client, REG_G_PREV_CFG_CHG, 1);
+	if (!ret)
+		ret = s5k6aa_write(client, REG_G_NEW_CFG_SYNC, 1);
+	if (timeout == 0)
+		return ret;
+
+	while (ret >= 0 && time_is_after_jiffies(end)) {
+		ret = s5k6aa_read(client, REG_G_NEW_CFG_SYNC, &reg);
+		if (!reg)
+			return 0;
+		usleep_range(1000, 5000);
+	}
+	return ret ? ret : -ETIMEDOUT;
+}
+
+/**
+ * s5k6aa_set_prev_config - write user preview register set
+ *
+ * Configure output resolution and color fromat, pixel clock
+ * frequency range, device frame rate type and frame period range.
+ */
+static int s5k6aa_set_prev_config(struct s5k6aa *s5k6aa,
+				  struct s5k6aa_preset *preset)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&s5k6aa->sd);
+	int idx = preset->index;
+	u16 frame_rate_q;
+	int ret;
+
+	if (s5k6aa->fiv->reg_fr_time >= S5K6AA_MAX_HIGHRES_FR_TIME)
+		frame_rate_q = FR_RATE_Q_BEST_FRRATE;
+	else
+		frame_rate_q = FR_RATE_Q_BEST_QUALITY;
+
+	ret = s5k6aa_set_output_framefmt(s5k6aa, preset);
+	if (!ret)
+		ret = s5k6aa_write(client, REG_P_MAX_OUT_RATE(idx),
+				   s5k6aa->pclk_fmax);
+	if (!ret)
+		ret = s5k6aa_write(client, REG_P_MIN_OUT_RATE(idx),
+				   s5k6aa->pclk_fmin);
+	if (!ret)
+		ret = s5k6aa_write(client, REG_P_CLK_INDEX(idx),
+				   preset->clk_id);
+	if (!ret)
+		ret = s5k6aa_write(client, REG_P_FR_RATE_TYPE(idx),
+				   FR_RATE_DYNAMIC);
+	if (!ret)
+		ret = s5k6aa_write(client, REG_P_FR_RATE_Q_TYPE(idx),
+				   frame_rate_q);
+	if (!ret)
+		ret = s5k6aa_write(client, REG_P_MAX_FR_TIME(idx),
+				   s5k6aa->fiv->reg_fr_time + 33);
+	if (!ret)
+		ret = s5k6aa_write(client, REG_P_MIN_FR_TIME(idx),
+				   s5k6aa->fiv->reg_fr_time - 33);
+	if (!ret)
+		ret = s5k6aa_new_config_sync(client, 250, idx);
+	if (!ret)
+		ret = s5k6aa_preview_config_status(client);
+	if (!ret)
+		s5k6aa->apply_cfg = 0;
+
+	v4l2_dbg(1, debug, client, "Frame interval: %d +/- 3.3ms. (%d)\n",
+		 s5k6aa->fiv->reg_fr_time, ret);
+	return ret;
+}
+
+/**
+ * s5k6aa_initialize_isp - basic ISP MCU initialization
+ *
+ * Configure AHB addresses for registers read/write; configure PLLs for
+ * required output pixel clock. The ISP power supply needs to be already
+ * enabled, with an optional H/W reset.
+ * Locking: called with s5k6aa.lock mutex held.
+ */
+static int s5k6aa_initialize_isp(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct s5k6aa *s5k6aa = to_s5k6aa(sd);
+	int ret;
+
+	s5k6aa->apply_crop = 1;
+	s5k6aa->apply_cfg = 1;
+	msleep(100);
+
+	ret = s5k6aa_set_ahb_address(client);
+	if (ret)
+		return ret;
+	ret = s5k6aa_configure_video_bus(s5k6aa, s5k6aa->bus_type,
+					 s5k6aa->mipi_lanes);
+	if (ret)
+		return ret;
+	ret = s5k6aa_write_array(sd, s5k6aa_analog_config);
+	if (ret)
+		return ret;
+	msleep(20);
+
+	return s5k6aa_configure_pixel_clocks(s5k6aa);
+}
+
+static int s5k6aa_gpio_set_value(struct s5k6aa *priv, int id, u32 val)
+{
+	if (!gpio_is_valid(priv->gpio[id].gpio))
+		return 0;
+	gpio_set_value(priv->gpio[id].gpio, !!val);
+	return 1;
+}
+
+static int s5k6aa_gpio_assert(struct s5k6aa *priv, int id)
+{
+	return s5k6aa_gpio_set_value(priv, id, priv->gpio[id].level);
+}
+
+static int s5k6aa_gpio_deassert(struct s5k6aa *priv, int id)
+{
+	return s5k6aa_gpio_set_value(priv, id, !priv->gpio[id].level);
+}
+
+static int __s5k6aa_power_on(struct s5k6aa *s5k6aa)
+{
+	int ret;
+
+	ret = regulator_bulk_enable(S5K6AA_NUM_SUPPLIES, s5k6aa->supplies);
+	if (ret)
+		return ret;
+	if (s5k6aa_gpio_deassert(s5k6aa, STBY))
+		usleep_range(150, 200);
+
+	if (s5k6aa->s_power)
+		ret = s5k6aa->s_power(1);
+	usleep_range(4000, 4000);
+
+	if (s5k6aa_gpio_deassert(s5k6aa, RST))
+		msleep(20);
+
+	return ret;
+}
+
+static int __s5k6aa_power_off(struct s5k6aa *s5k6aa)
+{
+	int ret;
+
+	if (s5k6aa_gpio_assert(s5k6aa, RST))
+		usleep_range(100, 150);
+
+	if (s5k6aa->s_power) {
+		ret = s5k6aa->s_power(0);
+		if (ret)
+			return ret;
+	}
+	if (s5k6aa_gpio_assert(s5k6aa, STBY))
+		usleep_range(50, 100);
+	s5k6aa->streaming = 0;
+
+	return regulator_bulk_disable(S5K6AA_NUM_SUPPLIES, s5k6aa->supplies);
+}
+
+/*
+ * V4L2 subdev core and video operations
+ */
+static int s5k6aa_set_power(struct v4l2_subdev *sd, int on)
+{
+	struct s5k6aa *s5k6aa = to_s5k6aa(sd);
+	int ret = 0;
+
+	mutex_lock(&s5k6aa->lock);
+
+	if (!on == s5k6aa->power) {
+		if (on) {
+			ret = __s5k6aa_power_on(s5k6aa);
+			if (!ret)
+				ret = s5k6aa_initialize_isp(sd);
+		} else {
+			ret = __s5k6aa_power_off(s5k6aa);
+		}
+
+		if (!ret)
+			s5k6aa->power += on ? 1 : -1;
+	}
+
+	mutex_unlock(&s5k6aa->lock);
+
+	if (!on || ret || s5k6aa->power != 1)
+		return ret;
+
+	return v4l2_ctrl_handler_setup(sd->ctrl_handler);
+}
+
+static int __s5k6aa_stream(struct s5k6aa *s5k6aa, int enable)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&s5k6aa->sd);
+	int ret = 0;
+
+	ret = s5k6aa_write(client, REG_G_ENABLE_PREV, enable);
+	if (!ret)
+		ret = s5k6aa_write(client, REG_G_ENABLE_PREV_CHG, 1);
+	if (!ret)
+		s5k6aa->streaming = enable;
+
+	return ret;
+}
+
+static int s5k6aa_s_stream(struct v4l2_subdev *sd, int on)
+{
+	struct s5k6aa *s5k6aa = to_s5k6aa(sd);
+	int ret = 0;
+
+	mutex_lock(&s5k6aa->lock);
+
+	if (s5k6aa->streaming == !on) {
+		if (!ret && s5k6aa->apply_cfg)
+			ret = s5k6aa_set_prev_config(s5k6aa, s5k6aa->preset);
+		if (s5k6aa->apply_crop)
+			ret = s5k6aa_set_input_params(s5k6aa);
+		if (!ret)
+			ret = __s5k6aa_stream(s5k6aa, !!on);
+	}
+	mutex_unlock(&s5k6aa->lock);
+
+	return ret;
+}
+
+static int s5k6aa_g_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *fi)
+{
+	struct s5k6aa *s5k6aa = to_s5k6aa(sd);
+
+	mutex_lock(&s5k6aa->lock);
+	fi->interval = s5k6aa->fiv->interval;
+	mutex_unlock(&s5k6aa->lock);
+
+	return 0;
+}
+
+static int __s5k6aa_set_frame_interval(struct s5k6aa *s5k6aa,
+				       struct v4l2_subdev_frame_interval *fi)
+{
+	struct v4l2_mbus_framefmt *mbus_fmt = &s5k6aa->preset->mbus_fmt;
+	const struct s5k6aa_interval *fiv = &s5k6aa_intervals[0];
+	unsigned int err, min_err = UINT_MAX;
+	unsigned int i, fr_time;
+
+	if (fi->interval.denominator == 0)
+		return -EINVAL;
+
+	fr_time = fi->interval.numerator * 10000 / fi->interval.denominator;
+
+	for (i = 0; i < ARRAY_SIZE(s5k6aa_intervals); i++) {
+		const struct s5k6aa_interval *iv = &s5k6aa_intervals[i];
+
+		if (mbus_fmt->width > iv->size.width ||
+		    mbus_fmt->height > iv->size.height)
+			continue;
+
+		err = abs(iv->reg_fr_time - fr_time);
+		if (err < min_err) {
+			fiv = iv;
+			min_err = err;
+		}
+	}
+	s5k6aa->fiv = fiv;
+
+	v4l2_dbg(1, debug, &s5k6aa->sd, "Changed frame interval to %d us\n",
+		 fiv->reg_fr_time * 100);
+	return 0;
+}
+
+static int s5k6aa_s_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *fi)
+{
+	struct s5k6aa *s5k6aa = to_s5k6aa(sd);
+	int ret;
+
+	v4l2_dbg(1, debug, sd, "Setting %d/%d frame interval\n",
+		 fi->interval.numerator, fi->interval.denominator);
+
+	mutex_lock(&s5k6aa->lock);
+	ret = __s5k6aa_set_frame_interval(s5k6aa, fi);
+	s5k6aa->apply_cfg = 1;
+
+	mutex_unlock(&s5k6aa->lock);
+	return ret;
+}
+
+/*
+ * V4L2 subdev pad level and video operations
+ */
+static int s5k6aa_enum_frame_interval(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_fh *fh,
+			      struct v4l2_subdev_frame_interval_enum *fie)
+{
+	struct s5k6aa *s5k6aa = to_s5k6aa(sd);
+	const struct s5k6aa_interval *fi;
+	int ret = 0;
+
+	if (fie->index > ARRAY_SIZE(s5k6aa_intervals))
+		return -EINVAL;
+
+	v4l_bound_align_image(&fie->width, S5K6AA_WIN_WIDTH_MIN,
+			      S5K6AA_WIN_WIDTH_MAX, 1,
+			      &fie->height, S5K6AA_WIN_HEIGHT_MIN,
+			      S5K6AA_WIN_HEIGHT_MAX, 1, 0);
+
+	mutex_lock(&s5k6aa->lock);
+	fi = &s5k6aa_intervals[fie->index];
+	if (fie->width > fi->size.width || fie->height > fi->size.height)
+		ret = -EINVAL;
+	else
+		fie->interval = fi->interval;
+	mutex_unlock(&s5k6aa->lock);
+
+	return ret;
+}
+
+static int s5k6aa_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_fh *fh,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index >= ARRAY_SIZE(s5k6aa_formats))
+		return -EINVAL;
+
+	code->code = s5k6aa_formats[code->index].code;
+	return 0;
+}
+
+static int s5k6aa_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_fh *fh,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	int i = ARRAY_SIZE(s5k6aa_formats);
+
+	if (fse->index > 0)
+		return -EINVAL;
+
+	while (--i)
+		if (fse->code == s5k6aa_formats[i].code)
+			break;
+
+	fse->code = s5k6aa_formats[i].code;
+	fse->min_width  = S5K6AA_WIN_WIDTH_MIN;
+	fse->max_width  = S5K6AA_WIN_WIDTH_MAX;
+	fse->max_height = S5K6AA_WIN_HEIGHT_MIN;
+	fse->min_height = S5K6AA_WIN_HEIGHT_MAX;
+
+	return 0;
+}
+
+static struct v4l2_rect *
+__s5k6aa_get_crop_rect(struct s5k6aa *s5k6aa, struct v4l2_subdev_fh *fh,
+		       enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		return &s5k6aa->ccd_rect;
+
+	WARN_ON(which != V4L2_SUBDEV_FORMAT_TRY);
+	return v4l2_subdev_get_try_crop(fh, 0);
+}
+
+static void s5k6aa_try_format(struct s5k6aa *s5k6aa,
+			      struct v4l2_mbus_framefmt *mf)
+{
+	unsigned int index;
+
+	v4l_bound_align_image(&mf->width, S5K6AA_WIN_WIDTH_MIN,
+			      S5K6AA_WIN_WIDTH_MAX, 1,
+			      &mf->height, S5K6AA_WIN_HEIGHT_MIN,
+			      S5K6AA_WIN_HEIGHT_MAX, 1, 0);
+
+	if (mf->colorspace != V4L2_COLORSPACE_JPEG &&
+	    mf->colorspace != V4L2_COLORSPACE_REC709)
+		mf->colorspace = V4L2_COLORSPACE_JPEG;
+
+	index = s5k6aa_get_pixfmt_index(s5k6aa, mf);
+
+	mf->colorspace	= s5k6aa_formats[index].colorspace;
+	mf->code	= s5k6aa_formats[index].code;
+	mf->field	= V4L2_FIELD_NONE;
+}
+
+static int s5k6aa_get_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct s5k6aa *s5k6aa = to_s5k6aa(sd);
+	struct v4l2_mbus_framefmt *mf;
+
+	memset(fmt->reserved, 0, sizeof(fmt->reserved));
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		mf = v4l2_subdev_get_try_format(fh, 0);
+		fmt->format = *mf;
+		return 0;
+	}
+
+	mutex_lock(&s5k6aa->lock);
+	fmt->format = s5k6aa->preset->mbus_fmt;
+	mutex_unlock(&s5k6aa->lock);
+
+	return 0;
+}
+
+static int s5k6aa_set_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct s5k6aa *s5k6aa = to_s5k6aa(sd);
+	struct s5k6aa_preset *preset = s5k6aa->preset;
+	struct v4l2_mbus_framefmt *mf;
+	struct v4l2_rect *crop;
+	int ret = 0;
+
+	mutex_lock(&s5k6aa->lock);
+	s5k6aa_try_format(s5k6aa, &fmt->format);
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		mf = v4l2_subdev_get_try_format(fh, fmt->pad);
+		crop = v4l2_subdev_get_try_crop(fh, 0);
+	} else {
+		if (s5k6aa->streaming) {
+			ret = -EBUSY;
+		} else {
+			mf = &preset->mbus_fmt;
+			crop = &s5k6aa->ccd_rect;
+			s5k6aa->apply_cfg = 1;
+		}
+	}
+
+	if (ret == 0) {
+		struct v4l2_subdev_frame_interval fiv = {
+			.interval = {0, 1}
+		};
+
+		*mf = fmt->format;
+		/*
+		 * Make sure the crop window is valid, i.e. its size is
+		 * greater than the output window, as the ISP supports
+		 * only down-scaling.
+		 */
+		crop->width = clamp_t(unsigned int, crop->width, mf->width,
+				      S5K6AA_WIN_WIDTH_MAX);
+		crop->height = clamp_t(unsigned int, crop->height, mf->height,
+				       S5K6AA_WIN_HEIGHT_MAX);
+		crop->left = clamp_t(unsigned int, crop->left, 0,
+				     S5K6AA_WIN_WIDTH_MAX - crop->width);
+		crop->top  = clamp_t(unsigned int, crop->top, 0,
+				     S5K6AA_WIN_HEIGHT_MAX - crop->height);
+
+		/* Reset to minimum possible frame interval */
+		ret = __s5k6aa_set_frame_interval(s5k6aa, &fiv);
+	}
+	mutex_unlock(&s5k6aa->lock);
+
+	return ret;
+}
+
+static int s5k6aa_get_crop(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+			   struct v4l2_subdev_crop *crop)
+{
+	struct s5k6aa *s5k6aa = to_s5k6aa(sd);
+	struct v4l2_rect *rect;
+
+	memset(crop->reserved, 0, sizeof(crop->reserved));
+
+	mutex_lock(&s5k6aa->lock);
+	rect = __s5k6aa_get_crop_rect(s5k6aa, fh, crop->which);
+	crop->rect = *rect;
+	mutex_unlock(&s5k6aa->lock);
+
+	v4l2_dbg(1, debug, sd, "Current crop rectangle: (%d,%d)/%dx%d\n",
+		 rect->left, rect->top, rect->width, rect->height);
+
+	return 0;
+}
+
+static int s5k6aa_set_crop(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+			   struct v4l2_subdev_crop *crop)
+{
+	struct s5k6aa *s5k6aa = to_s5k6aa(sd);
+	struct v4l2_mbus_framefmt *mf;
+	unsigned int max_x, max_y;
+	struct v4l2_rect *crop_r;
+
+	mutex_lock(&s5k6aa->lock);
+	crop_r = __s5k6aa_get_crop_rect(s5k6aa, fh, crop->which);
+
+	if (crop->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		mf = &s5k6aa->preset->mbus_fmt;
+		s5k6aa->apply_crop = 1;
+	} else {
+		mf = v4l2_subdev_get_try_format(fh, 0);
+	}
+	v4l_bound_align_image(&crop->rect.width, mf->width,
+			      S5K6AA_WIN_WIDTH_MAX, 1,
+			      &crop->rect.height, mf->height,
+			      S5K6AA_WIN_HEIGHT_MAX, 1, 0);
+
+	max_x = (S5K6AA_WIN_WIDTH_MAX - crop->rect.width) & ~1;
+	max_y = (S5K6AA_WIN_HEIGHT_MAX - crop->rect.height) & ~1;
+
+	crop->rect.left = clamp_t(unsigned int, crop->rect.left, 0, max_x);
+	crop->rect.top  = clamp_t(unsigned int, crop->rect.top, 0, max_y);
+
+	*crop_r = crop->rect;
+
+	mutex_unlock(&s5k6aa->lock);
+
+	v4l2_dbg(1, debug, sd, "Set crop rectangle: (%d,%d)/%dx%d\n",
+		 crop_r->left, crop_r->top, crop_r->width, crop_r->height);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_pad_ops s5k6aa_pad_ops = {
+	.enum_mbus_code		= s5k6aa_enum_mbus_code,
+	.enum_frame_size	= s5k6aa_enum_frame_size,
+	.enum_frame_interval	= s5k6aa_enum_frame_interval,
+	.get_fmt		= s5k6aa_get_fmt,
+	.set_fmt		= s5k6aa_set_fmt,
+	.get_crop		= s5k6aa_get_crop,
+	.set_crop		= s5k6aa_set_crop,
+};
+
+static const struct v4l2_subdev_video_ops s5k6aa_video_ops = {
+	.g_frame_interval	= s5k6aa_g_frame_interval,
+	.s_frame_interval	= s5k6aa_s_frame_interval,
+	.s_stream		= s5k6aa_s_stream,
+};
+
+/*
+ * V4L2 subdev controls
+ */
+
+static int s5k6aa_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct s5k6aa *s5k6aa = to_s5k6aa(sd);
+	int idx, err = 0;
+
+	v4l2_dbg(1, debug, sd, "ctrl: 0x%x, value: %d\n", ctrl->id, ctrl->val);
+
+	mutex_lock(&s5k6aa->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 (s5k6aa->power == 0)
+		goto unlock;
+	idx = s5k6aa->preset->index;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		err = s5k6aa_set_awb(s5k6aa, ctrl->val);
+		break;
+
+	case V4L2_CID_BRIGHTNESS:
+		err = s5k6aa_write(client, REG_USER_BRIGHTNESS, ctrl->val);
+		break;
+
+	case V4L2_CID_COLORFX:
+		err = s5k6aa_set_colorfx(s5k6aa, ctrl->val);
+		break;
+
+	case V4L2_CID_CONTRAST:
+		err = s5k6aa_write(client, REG_USER_CONTRAST, ctrl->val);
+		break;
+
+	case V4L2_CID_EXPOSURE_AUTO:
+		err = s5k6aa_set_auto_exposure(s5k6aa, ctrl->val);
+		break;
+
+	case V4L2_CID_HFLIP:
+		err = s5k6aa_set_mirror(s5k6aa, ctrl->val);
+		if (err)
+			break;
+		err = s5k6aa_write(client, REG_G_PREV_CFG_CHG, 1);
+		break;
+
+	case V4L2_CID_POWER_LINE_FREQUENCY:
+		err = s5k6aa_set_anti_flicker(s5k6aa, ctrl->val);
+		break;
+
+	case V4L2_CID_SATURATION:
+		err = s5k6aa_write(client, REG_USER_SATURATION, ctrl->val);
+		break;
+
+	case V4L2_CID_SHARPNESS:
+		err = s5k6aa_write(client, REG_USER_SHARPBLUR, ctrl->val);
+		break;
+
+	case V4L2_CID_WHITE_BALANCE_TEMPERATURE:
+		err = s5k6aa_write(client, REG_P_COLORTEMP(idx), ctrl->val);
+		if (err)
+			break;
+		err = s5k6aa_write(client, REG_G_PREV_CFG_CHG, 1);
+		break;
+	}
+unlock:
+	mutex_unlock(&s5k6aa->lock);
+	return err;
+}
+
+static const struct v4l2_ctrl_ops s5k6aa_ctrl_ops = {
+	.s_ctrl	= s5k6aa_s_ctrl,
+};
+
+static int s5k6aa_log_status(struct v4l2_subdev *sd)
+{
+	v4l2_ctrl_handler_log_status(sd->ctrl_handler, sd->name);
+	return 0;
+}
+
+#define V4L2_CID_RED_GAIN	(V4L2_CTRL_CLASS_CAMERA | 0x1001)
+#define V4L2_CID_GREEN_GAIN	(V4L2_CTRL_CLASS_CAMERA | 0x1002)
+#define V4L2_CID_BLUE_GAIN	(V4L2_CTRL_CLASS_CAMERA | 0x1003)
+
+static const struct v4l2_ctrl_config s5k6aa_ctrls[] = {
+	{
+		.ops	= &s5k6aa_ctrl_ops,
+		.id	= V4L2_CID_RED_GAIN,
+		.type	= V4L2_CTRL_TYPE_INTEGER,
+		.name	= "Gain, Red",
+		.min	= 0,
+		.max	= 256,
+		.def	= 127,
+		.step	= 1,
+	}, {
+		.ops	= &s5k6aa_ctrl_ops,
+		.id	= V4L2_CID_GREEN_GAIN,
+		.type	= V4L2_CTRL_TYPE_INTEGER,
+		.name	= "Gain, Green",
+		.min	= 0,
+		.max	= 256,
+		.def	= 127,
+		.step	= 1,
+	}, {
+		.ops	= &s5k6aa_ctrl_ops,
+		.id	= V4L2_CID_BLUE_GAIN,
+		.type	= V4L2_CTRL_TYPE_INTEGER,
+		.name	= "Gain, Blue",
+		.min	= 0,
+		.max	= 256,
+		.def	= 127,
+		.step	= 1,
+	},
+};
+
+static int s5k6aa_initialize_ctrls(struct s5k6aa *s5k6aa)
+{
+	const struct v4l2_ctrl_ops *ops = &s5k6aa_ctrl_ops;
+	struct s5k6aa_ctrls *ctrls = &s5k6aa->ctrls;
+	struct v4l2_ctrl_handler *hdl = &ctrls->handler;
+
+	int ret = v4l2_ctrl_handler_init(hdl, 16);
+	if (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_custom(hdl, &s5k6aa_ctrls[0], NULL);
+	ctrls->gain_green = v4l2_ctrl_new_custom(hdl, &s5k6aa_ctrls[1], NULL);
+	ctrls->gain_blue = v4l2_ctrl_new_custom(hdl, &s5k6aa_ctrls[2], NULL);
+	v4l2_ctrl_auto_cluster(4, &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);
+
+	if (hdl->error) {
+		ret = hdl->error;
+		v4l2_ctrl_handler_free(hdl);
+		return ret;
+	}
+
+	s5k6aa->sd.ctrl_handler = hdl;
+	return 0;
+}
+
+/*
+ * V4L2 subdev internal operations
+ */
+static int s5k6aa_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_mbus_framefmt *format = v4l2_subdev_get_try_format(fh, 0);
+	struct v4l2_rect *crop = v4l2_subdev_get_try_crop(fh, 0);
+
+	format->colorspace = s5k6aa_formats[0].colorspace;
+	format->code = s5k6aa_formats[0].code;
+	format->width = S5K6AA_OUT_WIDTH_DEF;
+	format->height = S5K6AA_OUT_HEIGHT_DEF;
+	format->field = V4L2_FIELD_NONE;
+
+	crop->width = S5K6AA_WIN_WIDTH_MAX;
+	crop->height = S5K6AA_WIN_HEIGHT_MAX;
+	crop->left = 0;
+	crop->top = 0;
+
+	return 0;
+}
+
+static int s5k6aa_check_fw_revision(struct s5k6aa *s5k6aa)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&s5k6aa->sd);
+	u16 api_ver = 0, fw_rev = 0;
+
+	int ret = s5k6aa_set_ahb_address(client);
+
+	if (!ret)
+		ret = s5k6aa_read(client, REG_FW_APIVER, &api_ver);
+	if (!ret)
+		ret = s5k6aa_read(client, REG_FW_REVISION, &fw_rev);
+	if (ret) {
+		v4l2_err(&s5k6aa->sd, "FW revision check failed!\n");
+		return ret;
+	}
+
+	v4l2_info(&s5k6aa->sd, "FW API ver.: 0x%X, FW rev.: 0x%X\n",
+		  api_ver, fw_rev);
+
+	return api_ver == S5K6AAFX_FW_APIVER ? 0 : -ENODEV;
+}
+
+static int s5k6aa_registered(struct v4l2_subdev *sd)
+{
+	struct s5k6aa *s5k6aa = to_s5k6aa(sd);
+	int ret;
+
+	mutex_lock(&s5k6aa->lock);
+	ret = __s5k6aa_power_on(s5k6aa);
+	if (!ret) {
+		msleep(100);
+		ret = s5k6aa_check_fw_revision(s5k6aa);
+		__s5k6aa_power_off(s5k6aa);
+	}
+	mutex_unlock(&s5k6aa->lock);
+
+	return ret;
+}
+
+static const struct v4l2_subdev_internal_ops s5k6aa_subdev_internal_ops = {
+	.registered = s5k6aa_registered,
+	.open = s5k6aa_open,
+};
+
+static const struct v4l2_subdev_core_ops s5k6aa_core_ops = {
+	.s_power = s5k6aa_set_power,
+	.log_status = s5k6aa_log_status,
+};
+
+static const struct v4l2_subdev_ops s5k6aa_subdev_ops = {
+	.core = &s5k6aa_core_ops,
+	.pad = &s5k6aa_pad_ops,
+	.video = &s5k6aa_video_ops,
+};
+
+/*
+ * GPIO setup
+ */
+static int s5k6aa_configure_gpio(int nr, int val, const char *name)
+{
+	unsigned long flags = val ? GPIOF_OUT_INIT_HIGH : GPIOF_OUT_INIT_LOW;
+	int ret;
+
+	if (!gpio_is_valid(nr))
+		return 0;
+	ret = gpio_request_one(nr, flags, name);
+	if (!ret)
+		gpio_export(nr, 0);
+	return ret;
+}
+
+static void s5k6aa_free_gpios(struct s5k6aa *s5k6aa)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(s5k6aa->gpio); i++) {
+		if (!gpio_is_valid(s5k6aa->gpio[i].gpio))
+			continue;
+		gpio_free(s5k6aa->gpio[i].gpio);
+		s5k6aa->gpio[i].gpio = -EINVAL;
+	}
+}
+
+static int s5k6aa_configure_gpios(struct s5k6aa *s5k6aa,
+				  const struct s5k6aa_platform_data *pdata)
+{
+	const struct s5k6aa_gpio *gpio = &pdata->gpio_stby;
+	int ret;
+
+	s5k6aa->gpio[STBY].gpio = -EINVAL;
+	s5k6aa->gpio[RST].gpio  = -EINVAL;
+
+	ret = s5k6aa_configure_gpio(gpio->gpio, gpio->level, "S5K6AA_STBY");
+	if (ret) {
+		s5k6aa_free_gpios(s5k6aa);
+		return ret;
+	}
+	s5k6aa->gpio[STBY] = *gpio;
+	if (gpio_is_valid(gpio->gpio))
+		gpio_set_value(gpio->gpio, 0);
+
+	gpio = &pdata->gpio_reset;
+	ret = s5k6aa_configure_gpio(gpio->gpio, gpio->level, "S5K6AA_RST");
+	if (ret) {
+		s5k6aa_free_gpios(s5k6aa);
+		return ret;
+	}
+	s5k6aa->gpio[RST] = *gpio;
+	if (gpio_is_valid(gpio->gpio))
+		gpio_set_value(gpio->gpio, 0);
+
+	return 0;
+}
+
+static int s5k6aa_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	const struct s5k6aa_platform_data *pdata = client->dev.platform_data;
+	struct v4l2_subdev *sd;
+	struct s5k6aa *s5k6aa;
+	int i, ret;
+
+	if (pdata == NULL) {
+		dev_err(&client->dev, "Platform data not specified\n");
+		return -EINVAL;
+	}
+
+	if (pdata->mclk_frequency == 0) {
+		dev_err(&client->dev, "MCLK frequency not specified\n");
+		return -EINVAL;
+	}
+
+	s5k6aa = devm_kzalloc(&client->dev, sizeof(*s5k6aa), GFP_KERNEL);
+	if (!s5k6aa)
+		return -ENOMEM;
+
+	mutex_init(&s5k6aa->lock);
+
+	s5k6aa->mclk_frequency = pdata->mclk_frequency;
+	s5k6aa->bus_type = pdata->bus_type;
+	s5k6aa->mipi_lanes = pdata->nlanes;
+	s5k6aa->s_power	= pdata->set_power;
+	s5k6aa->inv_hflip = pdata->horiz_flip;
+	s5k6aa->inv_vflip = pdata->vert_flip;
+
+	sd = &s5k6aa->sd;
+	v4l2_i2c_subdev_init(sd, client, &s5k6aa_subdev_ops);
+	strlcpy(sd->name, DRIVER_NAME, sizeof(sd->name));
+
+	sd->internal_ops = &s5k6aa_subdev_internal_ops;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	s5k6aa->pad.flags = MEDIA_PAD_FL_SOURCE;
+	sd->entity.type = MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
+	ret = media_entity_init(&sd->entity, 1, &s5k6aa->pad, 0);
+	if (ret)
+		return ret;
+
+	ret = s5k6aa_configure_gpios(s5k6aa, pdata);
+	if (ret)
+		goto out_err2;
+
+	for (i = 0; i < S5K6AA_NUM_SUPPLIES; i++)
+		s5k6aa->supplies[i].supply = s5k6aa_supply_names[i];
+
+	ret = regulator_bulk_get(&client->dev, S5K6AA_NUM_SUPPLIES,
+				 s5k6aa->supplies);
+	if (ret) {
+		dev_err(&client->dev, "Failed to get regulators\n");
+		goto out_err3;
+	}
+
+	ret = s5k6aa_initialize_ctrls(s5k6aa);
+	if (ret)
+		goto out_err4;
+
+	s5k6aa_presets_data_init(s5k6aa);
+
+	s5k6aa->ccd_rect.width = S5K6AA_WIN_WIDTH_MAX;
+	s5k6aa->ccd_rect.height	= S5K6AA_WIN_HEIGHT_MAX;
+	s5k6aa->ccd_rect.left = 0;
+	s5k6aa->ccd_rect.top = 0;
+
+	return 0;
+
+out_err4:
+	regulator_bulk_free(S5K6AA_NUM_SUPPLIES, s5k6aa->supplies);
+out_err3:
+	s5k6aa_free_gpios(s5k6aa);
+out_err2:
+	media_entity_cleanup(&s5k6aa->sd.entity);
+	return ret;
+}
+
+static int s5k6aa_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct s5k6aa *s5k6aa = to_s5k6aa(sd);
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+	media_entity_cleanup(&sd->entity);
+	regulator_bulk_free(S5K6AA_NUM_SUPPLIES, s5k6aa->supplies);
+	s5k6aa_free_gpios(s5k6aa);
+
+	return 0;
+}
+
+static const struct i2c_device_id s5k6aa_id[] = {
+	{ DRIVER_NAME, 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(i2c, s5k6aa_id);
+
+
+static struct i2c_driver s5k6aa_i2c_driver = {
+	.driver = {
+		.name = DRIVER_NAME
+	},
+	.probe		= s5k6aa_probe,
+	.remove		= s5k6aa_remove,
+	.id_table	= s5k6aa_id,
+};
+
+module_i2c_driver(s5k6aa_i2c_driver);
+
+MODULE_DESCRIPTION("Samsung S5K6AA(FX) SXGA camera driver");
+MODULE_AUTHOR("Sylwester Nawrocki <s.nawrocki@samsung.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/saa6588.c b/drivers/media/i2c/saa6588.c
new file mode 100644
index 000000000000..0caac50d7cf4
--- /dev/null
+++ b/drivers/media/i2c/saa6588.c
@@ -0,0 +1,542 @@
+/*
+    Driver for SAA6588 RDS decoder
+
+    (c) 2005 Hans J. Koch
+
+    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; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+*/
+
+
+#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 <asm/uaccess.h>
+
+#include <media/saa6588.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.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 int block_to_user_buf(struct saa6588 *s, unsigned char __user *user_buf)
+{
+	int i;
+
+	if (s->rd_index == s->wr_index) {
+		if (debug > 2)
+			dprintk(PREFIX "Read: buffer empty.\n");
+		return 0;
+	}
+
+	if (debug > 2) {
+		dprintk(PREFIX "Read: ");
+		for (i = s->rd_index; i < s->rd_index + 3; i++)
+			dprintk("0x%02x ", s->buffer[i]);
+	}
+
+	if (copy_to_user(user_buf, &s->buffer[s->rd_index], 3))
+		return -EFAULT;
+
+	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 1;
+}
+
+static void read_from_buf(struct saa6588 *s, struct saa6588_command *a)
+{
+	unsigned long flags;
+
+	unsigned char __user *buf_ptr = a->buffer;
+	unsigned int i;
+	unsigned int rd_blocks;
+
+	a->result = 0;
+	if (!a->buffer)
+		return;
+
+	while (!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;
+		}
+	}
+
+	spin_lock_irqsave(&s->lock, flags);
+	rd_blocks = a->block_count;
+	if (rd_blocks > s->block_count)
+		rd_blocks = s->block_count;
+
+	if (!rd_blocks) {
+		spin_unlock_irqrestore(&s->lock, flags);
+		return;
+	}
+
+	for (i = 0; i < rd_blocks; i++) {
+		if (block_to_user_buf(s, buf_ptr)) {
+			buf_ptr += 3;
+			a->result++;
+		} else
+			break;
+	}
+	a->result *= 3;
+	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.
+	 */
+
+	tmp = tmpbuf[2];
+	tmpbuf[2] = tmpbuf[0];
+	tmpbuf[0] = tmp;
+
+	/* 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_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
+{
+	struct saa6588 *s = to_saa6588(sd);
+	struct saa6588_command *a = arg;
+
+	switch (cmd) {
+		/* --- open() for /dev/radio --- */
+	case SAA6588_CMD_OPEN:
+		a->result = 0;	/* return error if chip doesn't work ??? */
+		break;
+		/* --- close() for /dev/radio --- */
+	case SAA6588_CMD_CLOSE:
+		s->data_available_for_read = 1;
+		wake_up_interruptible(&s->read_queue);
+		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->result = 0;
+		if (s->data_available_for_read) {
+			a->result |= POLLIN | POLLRDNORM;
+		}
+		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, struct v4l2_tuner *vt)
+{
+	struct saa6588 *s = to_saa6588(sd);
+
+	saa6588_configure(s);
+	return 0;
+}
+
+static int saa6588_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ident *chip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_SAA6588, 0);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_core_ops saa6588_core_ops = {
+	.g_chip_ident = saa6588_g_chip_ident,
+	.ioctl = saa6588_ioctl,
+};
+
+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,
+			 const struct i2c_device_id *id)
+{
+	struct saa6588 *s;
+	struct v4l2_subdev *sd;
+
+	v4l_info(client, "saa6588 found @ 0x%x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	s = kzalloc(sizeof(*s), GFP_KERNEL);
+	if (s == NULL)
+		return -ENOMEM;
+
+	s->buf_size = bufblocks * 3;
+
+	s->buffer = kmalloc(s->buf_size, GFP_KERNEL);
+	if (s->buffer == NULL) {
+		kfree(s);
+		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 int 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);
+
+	kfree(s->buffer);
+	kfree(s);
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id saa6588_id[] = {
+	{ "saa6588", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, saa6588_id);
+
+static struct i2c_driver saa6588_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "saa6588",
+	},
+	.probe		= saa6588_probe,
+	.remove		= saa6588_remove,
+	.id_table	= saa6588_id,
+};
+
+module_i2c_driver(saa6588_driver);
diff --git a/drivers/media/i2c/saa7110.c b/drivers/media/i2c/saa7110.c
new file mode 100644
index 000000000000..51cd4c8f0520
--- /dev/null
+++ b/drivers/media/i2c/saa7110.c
@@ -0,0 +1,494 @@
+/*
+ * 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)
+ *
+ * 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; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#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 <asm/uaccess.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.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 decoder->norm;	/* no change*/
+	}
+	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)
+{
+	*(v4l2_std_id *)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 int saa7110_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ident *chip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_SAA7110, 0);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_ctrl_ops saa7110_ctrl_ops = {
+	.s_ctrl = saa7110_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops saa7110_core_ops = {
+	.g_chip_ident = saa7110_g_chip_ident,
+	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
+	.try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
+	.s_ext_ctrls = v4l2_subdev_s_ext_ctrls,
+	.g_ctrl = v4l2_subdev_g_ctrl,
+	.s_ctrl = v4l2_subdev_s_ctrl,
+	.queryctrl = v4l2_subdev_queryctrl,
+	.querymenu = v4l2_subdev_querymenu,
+	.s_std = saa7110_s_std,
+};
+
+static const struct v4l2_subdev_video_ops saa7110_video_ops = {
+	.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 = {
+	.core = &saa7110_core_ops,
+	.video = &saa7110_video_ops,
+};
+
+/* ----------------------------------------------------------------------- */
+
+static int saa7110_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	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 = kzalloc(sizeof(struct saa7110), 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);
+		kfree(decoder);
+		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 int 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);
+	kfree(decoder);
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id saa7110_id[] = {
+	{ "saa7110", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, saa7110_id);
+
+static struct i2c_driver saa7110_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.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 000000000000..6b6788cd08f6
--- /dev/null
+++ b/drivers/media/i2c/saa7115.c
@@ -0,0 +1,1728 @@
+/* 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@infradead.org>
+ *	SAA7111, SAA7113 and SAA7118 support
+ *
+ * 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; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#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-chip-ident.h>
+#include <media/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)");
+
+
+struct saa711x_state {
+	struct v4l2_subdev sd;
+	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;
+	u32 ident;
+	u32 audclk_freq;
+	u32 crystal_freq;
+	u8 ucgc;
+	u8 cgcdiv;
+	u8 apll;
+};
+
+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 == V4L2_IDENT_SAA7111)
+		return reg < 0x20 && reg != 0x01 && reg != 0x0f &&
+		       (reg < 0x13 || reg > 0x19) && reg != 0x1d && reg != 0x1e;
+	if (id == V4L2_IDENT_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 V4L2_IDENT_SAA7113:
+		return reg != 0x14 && (reg < 0x18 || reg > 0x1e) && (reg < 0x20 || reg > 0x3f) &&
+		       reg != 0x5d && reg < 0x63;
+	case V4L2_IDENT_SAA7114:
+		return (reg < 0x1a || reg > 0x1e) && (reg < 0x20 || reg > 0x2f) &&
+		       (reg < 0x63 || reg > 0x7f) && reg != 0x33 && reg != 0x37 &&
+		       reg != 0x81 && reg < 0xf0;
+	case V4L2_IDENT_SAA7115:
+		return (reg < 0x20 || reg > 0x2f) && reg != 0x65 && (reg < 0xfc || reg > 0xfe);
+	case V4L2_IDENT_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
+};
+
+/* 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,
+	R_07_H_SYNC_STOP, 0x0d,
+	R_08_SYNC_CNTL, 0x88,
+	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,
+	R_11_MODE_DELAY_CNTL, 0x0c,
+	R_12_RT_SIGNAL_CNTL, 0x07,
+	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;
+
+	saa711x_write(sd, R_38_CLK_RATIO_AMXCLK_TO_ASCLK, 0x03);
+	saa711x_write(sd, R_39_CLK_RATIO_ASCLK_TO_ALRCLK, 0x10);
+	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");
+		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");
+		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 <= V4L2_IDENT_SAA7113) {
+		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 == V4L2_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 != V4L2_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 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_s_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *fmt)
+{
+	if (fmt->code != V4L2_MBUS_FMT_FIXED)
+		return -EINVAL;
+	fmt->field = V4L2_FIELD_INTERLACED;
+	fmt->colorspace = V4L2_COLORSPACE_SMPTE170M;
+	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 <= V4L2_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 <= V4L2_IDENT_SAA7113 &&
+	    (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 <= V4L2_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 == V4L2_IDENT_SAA7114 ||
+			state->ident == V4L2_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));
+	}
+	return 0;
+}
+
+static int saa711x_s_gpio(struct v4l2_subdev *sd, u32 val)
+{
+	struct saa711x_state *state = to_state(sd);
+
+	if (state->ident > V4L2_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->cgcdiv = (flags & SAA7115_FREQ_FL_CGCDIV) ? 3 : 4;
+	state->ucgc = (flags & SAA7115_FREQ_FL_UCGC) ? 1 : 0;
+	state->apll = (flags & SAA7115_FREQ_FL_APLL) ? 1 : 0;
+	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);
+	v4l2_dbg(1, debug, sd, "Status byte 2 (0x1f)=0x%02x\n", reg1f);
+
+	/* horizontal/vertical not locked */
+	if (reg1f & 0x40)
+		goto ret;
+
+	if (reg1f & 0x20)
+		*std &= V4L2_STD_525_60;
+	else
+		*std &= V4L2_STD_625_50;
+
+	if (state->ident != V4L2_IDENT_SAA7115)
+		goto ret;
+
+	reg1e = saa711x_read(sd, R_1E_STATUS_BYTE_1_VD_DEC);
+
+	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:
+		/* Can't detect anything */
+		break;
+	}
+
+	v4l2_dbg(1, debug, sd, "Status byte 1 (0x1e)=0x%02x\n", reg1e);
+
+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 == V4L2_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)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	reg->val = saa711x_read(sd, reg->reg & 0xff);
+	reg->size = 1;
+	return 0;
+}
+
+static int saa711x_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	saa711x_write(sd, reg->reg & 0xff, reg->val & 0xff);
+	return 0;
+}
+#endif
+
+static int saa711x_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ident *chip)
+{
+	struct saa711x_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return v4l2_chip_ident_i2c_client(client, chip, state->ident, 0);
+}
+
+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 != V4L2_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,
+	.g_chip_ident = saa711x_g_chip_ident,
+	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
+	.try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
+	.s_ext_ctrls = v4l2_subdev_s_ext_ctrls,
+	.g_ctrl = v4l2_subdev_g_ctrl,
+	.s_ctrl = v4l2_subdev_s_ctrl,
+	.queryctrl = v4l2_subdev_queryctrl,
+	.querymenu = v4l2_subdev_querymenu,
+	.s_std = saa711x_s_std,
+	.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_routing = saa711x_s_routing,
+	.s_crystal_freq = saa711x_s_crystal_freq,
+	.s_mbus_fmt = saa711x_s_mbus_fmt,
+	.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_ops saa711x_ops = {
+	.core = &saa711x_core_ops,
+	.tuner = &saa711x_tuner_ops,
+	.audio = &saa711x_audio_ops,
+	.video = &saa711x_video_ops,
+	.vbi = &saa711x_vbi_ops,
+};
+
+/* ----------------------------------------------------------------------- */
+
+static int saa711x_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	struct saa711x_state *state;
+	struct v4l2_subdev *sd;
+	struct v4l2_ctrl_handler *hdl;
+	int i;
+	char name[17];
+	char chip_id;
+	int autodetect = !id || id->driver_data == 1;
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+
+	for (i = 0; i < 0x0f; i++) {
+		i2c_smbus_write_byte_data(client, 0, i);
+		name[i] = (i2c_smbus_read_byte_data(client, 0) & 0x0f) + '0';
+		if (name[i] > '9')
+			name[i] += 'a' - '9' - 1;
+	}
+	name[i] = '\0';
+
+	chip_id = name[5];
+
+	/* Check whether this chip is part of the saa711x series */
+	if (memcmp(name + 1, "f711", 4)) {
+		v4l_dbg(1, debug, client, "chip found @ 0x%x (ID %s) does not match a known saa711x chip.\n",
+			client->addr << 1, name);
+		return -ENODEV;
+	}
+
+	/* Safety check */
+	if (!autodetect && id->name[6] != chip_id) {
+		v4l_warn(client, "found saa711%c while %s was expected\n",
+			 chip_id, id->name);
+	}
+	snprintf(client->name, sizeof(client->name), "saa711%c", chip_id);
+	v4l_info(client, "saa711%c found (%s) @ 0x%x (%s)\n", chip_id, name,
+		 client->addr << 1, client->adapter->name);
+
+	state = kzalloc(sizeof(struct saa711x_state), GFP_KERNEL);
+	if (state == NULL)
+		return -ENOMEM;
+	sd = &state->sd;
+	v4l2_i2c_subdev_init(sd, client, &saa711x_ops);
+
+	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);
+		kfree(state);
+		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;
+	switch (chip_id) {
+	case '1':
+		state->ident = V4L2_IDENT_SAA7111;
+		if (saa711x_read(sd, R_00_CHIP_VERSION) & 0xf0) {
+			v4l_info(client, "saa7111a variant found\n");
+			state->ident = V4L2_IDENT_SAA7111A;
+		}
+		break;
+	case '3':
+		state->ident = V4L2_IDENT_SAA7113;
+		break;
+	case '4':
+		state->ident = V4L2_IDENT_SAA7114;
+		break;
+	case '5':
+		state->ident = V4L2_IDENT_SAA7115;
+		break;
+	case '8':
+		state->ident = V4L2_IDENT_SAA7118;
+		break;
+	default:
+		state->ident = V4L2_IDENT_SAA7111;
+		v4l2_info(sd, "WARNING: Chip is not known - Falling back to saa7111\n");
+		break;
+	}
+
+	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;
+	switch (state->ident) {
+	case V4L2_IDENT_SAA7111:
+	case V4L2_IDENT_SAA7111A:
+		saa711x_writeregs(sd, saa7111_init);
+		break;
+	case V4L2_IDENT_SAA7113:
+		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 > V4L2_IDENT_SAA7111A)
+		saa711x_writeregs(sd, saa7115_init_misc);
+	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 int 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);
+	kfree(to_state(sd));
+	return 0;
+}
+
+static const struct i2c_device_id saa711x_id[] = {
+	{ "saa7115_auto", 1 }, /* autodetect */
+	{ "saa7111", 0 },
+	{ "saa7113", 0 },
+	{ "saa7114", 0 },
+	{ "saa7115", 0 },
+	{ "saa7118", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, saa711x_id);
+
+static struct i2c_driver saa711x_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.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 000000000000..4e5f2eb0a2c1
--- /dev/null
+++ b/drivers/media/i2c/saa711x_regs.h
@@ -0,0 +1,549 @@
+/* saa711x - Philips SAA711x video decoder register specifications
+ *
+ * Copyright (c) 2006 Mauro Carvalho Chehab <mchehab@infradead.org>
+ *
+ * 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; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#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
+
+#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 000000000000..b745f68fbc92
--- /dev/null
+++ b/drivers/media/i2c/saa7127.c
@@ -0,0 +1,852 @@
+/*
+ * 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.
+ *
+ * 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; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+
+#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-chip-ident.h>
+#include <media/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
+ *
+ **********************************************************************
+ */
+
+struct saa7127_state {
+	struct v4l2_subdev sd;
+	v4l2_std_id std;
+	u32 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 == V4L2_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 == V4L2_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 == V4L2_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 == V4L2_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)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	reg->val = saa7127_read(sd, reg->reg & 0xff);
+	reg->size = 1;
+	return 0;
+}
+
+static int saa7127_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	saa7127_write(sd, reg->reg & 0xff, reg->val & 0xff);
+	return 0;
+}
+#endif
+
+static int saa7127_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ident *chip)
+{
+	struct saa7127_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return v4l2_chip_ident_i2c_client(client, chip, state->ident, 0);
+}
+
+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,
+	.g_chip_ident = saa7127_g_chip_ident,
+#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)
+{
+	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 = kzalloc(sizeof(struct saa7127_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");
+		kfree(state);
+		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 = V4L2_IDENT_SAA7129;
+			strlcpy(client->name, "saa7129", I2C_NAME_SIZE);
+		} else {
+			state->ident = V4L2_IDENT_SAA7127;
+			strlcpy(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 == V4L2_IDENT_SAA7129)
+		saa7127_write_inittab(sd, saa7129_init_config_extra);
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static int 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);
+	kfree(to_state(sd));
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static struct i2c_device_id saa7127_id[] = {
+	{ "saa7127_auto", 0 },	/* auto-detection */
+	{ "saa7126", V4L2_IDENT_SAA7127 },
+	{ "saa7127", V4L2_IDENT_SAA7127 },
+	{ "saa7128", V4L2_IDENT_SAA7129 },
+	{ "saa7129", V4L2_IDENT_SAA7129 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, saa7127_id);
+
+static struct i2c_driver saa7127_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.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 000000000000..1e84466515aa
--- /dev/null
+++ b/drivers/media/i2c/saa717x.c
@@ -0,0 +1,1378 @@
+/*
+ * 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.
+ *
+ * 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; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#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] & 0xff)  | ((mm2[1] & 0xff) >> 8) | ((mm2[0] & 0xff) >> 16);
+	else
+		value = mm2[0] & 0xff;
+
+	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;
+
+	/* horizonal 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)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	reg->val = saa717x_read(sd, reg->reg);
+	reg->size = 1;
+	return 0;
+}
+
+static int saa717x_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u16 addr = reg->reg & 0xffff;
+	u8 val = reg->val & 0xff;
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	saa717x_write(sd, addr, val);
+	return 0;
+}
+#endif
+
+static int saa717x_s_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *fmt)
+{
+	int prescale, h_scale, v_scale;
+
+	v4l2_dbg(1, debug, sd, "decoder set size\n");
+
+	if (fmt->code != V4L2_MBUS_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;
+
+	/* 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 implementd)\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, 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
+	.s_std = saa717x_s_std,
+	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
+	.try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
+	.s_ext_ctrls = v4l2_subdev_s_ext_ctrls,
+	.g_ctrl = v4l2_subdev_g_ctrl,
+	.s_ctrl = v4l2_subdev_s_ctrl,
+	.queryctrl = v4l2_subdev_queryctrl,
+	.querymenu = v4l2_subdev_querymenu,
+	.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_routing = saa717x_s_video_routing,
+	.s_mbus_fmt = saa717x_s_mbus_fmt,
+	.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_ops saa717x_ops = {
+	.core = &saa717x_core_ops,
+	.tuner = &saa717x_tuner_ops,
+	.audio = &saa717x_audio_ops,
+	.video = &saa717x_video_ops,
+};
+
+/* ----------------------------------------------------------------------- */
+
+
+/* i2c implementation */
+
+/* ----------------------------------------------------------------------- */
+static int saa717x_probe(struct i2c_client *client,
+			 const struct i2c_device_id *did)
+{
+	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 = kzalloc(sizeof(struct saa717x_state), 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);
+		kfree(decoder);
+		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);
+		kfree(decoder);
+		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 int 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);
+	kfree(to_state(sd));
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id saa717x_id[] = {
+	{ "saa717x", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, saa717x_id);
+
+static struct i2c_driver saa717x_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.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 000000000000..2c6b65c76e2b
--- /dev/null
+++ b/drivers/media/i2c/saa7185.c
@@ -0,0 +1,377 @@
+/*
+ * 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)
+ *
+ * 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; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <linux/ioctl.h>
+#include <asm/uaccess.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.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 int saa7185_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ident *chip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_SAA7185, 0);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_core_ops saa7185_core_ops = {
+	.g_chip_ident = saa7185_g_chip_ident,
+	.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,
+			const struct i2c_device_id *id)
+{
+	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 = kzalloc(sizeof(struct saa7185), 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 int 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);
+	kfree(encoder);
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id saa7185_id[] = {
+	{ "saa7185", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, saa7185_id);
+
+static struct i2c_driver saa7185_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "saa7185",
+	},
+	.probe		= saa7185_probe,
+	.remove		= saa7185_remove,
+	.id_table	= saa7185_id,
+};
+
+module_i2c_driver(saa7185_driver);
diff --git a/drivers/media/i2c/saa7191.c b/drivers/media/i2c/saa7191.c
new file mode 100644
index 000000000000..d7d1670e0ca3
--- /dev/null
+++ b/drivers/media/i2c/saa7191.c
@@ -0,0 +1,659 @@
+/*
+ *  saa7191.c - Philips SAA7191 video decoder driver
+ *
+ *  Copyright (C) 2003 Ladislav Michl <ladis@linux-mips.org>
+ *  Copyright (C) 2004,2005 Mikael Nousiainen <tmnousia@cc.hut.fi>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2 as
+ *  published by the Free Software Foundation.
+ */
+
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/major.h>
+#include <linux/module.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+
+#include <linux/videodev2.h>
+#include <linux/i2c.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.h>
+
+#include "saa7191.h"
+
+#define SAA7191_MODULE_VERSION	"0.0.5"
+
+MODULE_DESCRIPTION("Philips SAA7191 video decoder driver");
+MODULE_VERSION(SAA7191_MODULE_VERSION);
+MODULE_AUTHOR("Mikael Nousiainen <tmnousia@cc.hut.fi>");
+MODULE_LICENSE("GPL");
+
+
+// #define SAA7191_DEBUG
+
+#ifdef SAA7191_DEBUG
+#define dprintk(x...) printk("SAA7191: " x);
+#else
+#define dprintk(x...)
+#endif
+
+#define SAA7191_SYNC_COUNT	30
+#define SAA7191_SYNC_DELAY	100	/* milliseconds */
+
+struct saa7191 {
+	struct v4l2_subdev sd;
+
+	/* the register values are stored here as the actual
+	 * I2C-registers are write-only */
+	u8 reg[25];
+
+	int input;
+	v4l2_std_id norm;
+};
+
+static inline struct saa7191 *to_saa7191(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct saa7191, sd);
+}
+
+static const u8 initseq[] = {
+	0,	/* Subaddress */
+
+	0x50,	/* (0x50) SAA7191_REG_IDEL */
+
+	/* 50 Hz signal timing */
+	0x30,	/* (0x30) SAA7191_REG_HSYB */
+	0x00,	/* (0x00) SAA7191_REG_HSYS */
+	0xe8,	/* (0xe8) SAA7191_REG_HCLB */
+	0xb6,	/* (0xb6) SAA7191_REG_HCLS */
+	0xf4,	/* (0xf4) SAA7191_REG_HPHI */
+
+	/* control */
+	SAA7191_LUMA_APER_1,	/* (0x01) SAA7191_REG_LUMA - CVBS mode */
+	0x00,	/* (0x00) SAA7191_REG_HUEC */
+	0xf8,	/* (0xf8) SAA7191_REG_CKTQ */
+	0xf8,	/* (0xf8) SAA7191_REG_CKTS */
+	0x90,	/* (0x90) SAA7191_REG_PLSE */
+	0x90,	/* (0x90) SAA7191_REG_SESE */
+	0x00,	/* (0x00) SAA7191_REG_GAIN */
+	SAA7191_STDC_NFEN | SAA7191_STDC_HRMV,	/* (0x0c) SAA7191_REG_STDC
+						 * - not SECAM,
+						 * slow time constant */
+	SAA7191_IOCK_OEDC | SAA7191_IOCK_OEHS | SAA7191_IOCK_OEVS
+	| SAA7191_IOCK_OEDY,	/* (0x78) SAA7191_REG_IOCK
+				 * - chroma from CVBS, GPSW1 & 2 off */
+	SAA7191_CTL3_AUFD | SAA7191_CTL3_SCEN | SAA7191_CTL3_OFTS
+	| SAA7191_CTL3_YDEL0,	/* (0x99) SAA7191_REG_CTL3
+				 * - automatic field detection */
+	0x00,	/* (0x00) SAA7191_REG_CTL4 */
+	0x2c,	/* (0x2c) SAA7191_REG_CHCV - PAL nominal value */
+	0x00,	/* unused */
+	0x00,	/* unused */
+
+	/* 60 Hz signal timing */
+	0x34,	/* (0x34) SAA7191_REG_HS6B */
+	0x0a,	/* (0x0a) SAA7191_REG_HS6S */
+	0xf4,	/* (0xf4) SAA7191_REG_HC6B */
+	0xce,	/* (0xce) SAA7191_REG_HC6S */
+	0xf4,	/* (0xf4) SAA7191_REG_HP6I */
+};
+
+/* SAA7191 register handling */
+
+static u8 saa7191_read_reg(struct v4l2_subdev *sd, u8 reg)
+{
+	return to_saa7191(sd)->reg[reg];
+}
+
+static int saa7191_read_status(struct v4l2_subdev *sd, u8 *value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+	ret = i2c_master_recv(client, value, 1);
+	if (ret < 0) {
+		printk(KERN_ERR "SAA7191: saa7191_read_status(): read failed\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+
+static int saa7191_write_reg(struct v4l2_subdev *sd, u8 reg, u8 value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	to_saa7191(sd)->reg[reg] = value;
+	return i2c_smbus_write_byte_data(client, reg, value);
+}
+
+/* the first byte of data must be the first subaddress number (register) */
+static int saa7191_write_block(struct v4l2_subdev *sd,
+			       u8 length, const u8 *data)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct saa7191 *decoder = to_saa7191(sd);
+	int i;
+	int ret;
+
+	for (i = 0; i < (length - 1); i++) {
+		decoder->reg[data[0] + i] = data[i + 1];
+	}
+
+	ret = i2c_master_send(client, data, length);
+	if (ret < 0) {
+		printk(KERN_ERR "SAA7191: saa7191_write_block(): "
+		       "write failed\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+/* Helper functions */
+
+static int saa7191_s_routing(struct v4l2_subdev *sd,
+			     u32 input, u32 output, u32 config)
+{
+	struct saa7191 *decoder = to_saa7191(sd);
+	u8 luma = saa7191_read_reg(sd, SAA7191_REG_LUMA);
+	u8 iock = saa7191_read_reg(sd, SAA7191_REG_IOCK);
+	int err;
+
+	switch (input) {
+	case SAA7191_INPUT_COMPOSITE: /* Set Composite input */
+		iock &= ~(SAA7191_IOCK_CHRS | SAA7191_IOCK_GPSW1
+			  | SAA7191_IOCK_GPSW2);
+		/* Chrominance trap active */
+		luma &= ~SAA7191_LUMA_BYPS;
+		break;
+	case SAA7191_INPUT_SVIDEO: /* Set S-Video input */
+		iock |= SAA7191_IOCK_CHRS | SAA7191_IOCK_GPSW2;
+		/* Chrominance trap bypassed */
+		luma |= SAA7191_LUMA_BYPS;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	err = saa7191_write_reg(sd, SAA7191_REG_LUMA, luma);
+	if (err)
+		return -EIO;
+	err = saa7191_write_reg(sd, SAA7191_REG_IOCK, iock);
+	if (err)
+		return -EIO;
+
+	decoder->input = input;
+
+	return 0;
+}
+
+static int saa7191_s_std(struct v4l2_subdev *sd, v4l2_std_id norm)
+{
+	struct saa7191 *decoder = to_saa7191(sd);
+	u8 stdc = saa7191_read_reg(sd, SAA7191_REG_STDC);
+	u8 ctl3 = saa7191_read_reg(sd, SAA7191_REG_CTL3);
+	u8 chcv = saa7191_read_reg(sd, SAA7191_REG_CHCV);
+	int err;
+
+	if (norm & V4L2_STD_PAL) {
+		stdc &= ~SAA7191_STDC_SECS;
+		ctl3 &= ~(SAA7191_CTL3_AUFD | SAA7191_CTL3_FSEL);
+		chcv = SAA7191_CHCV_PAL;
+	} else if (norm & V4L2_STD_NTSC) {
+		stdc &= ~SAA7191_STDC_SECS;
+		ctl3 &= ~SAA7191_CTL3_AUFD;
+		ctl3 |= SAA7191_CTL3_FSEL;
+		chcv = SAA7191_CHCV_NTSC;
+	} else if (norm & V4L2_STD_SECAM) {
+		stdc |= SAA7191_STDC_SECS;
+		ctl3 &= ~(SAA7191_CTL3_AUFD | SAA7191_CTL3_FSEL);
+		chcv = SAA7191_CHCV_PAL;
+	} else {
+		return -EINVAL;
+	}
+
+	err = saa7191_write_reg(sd, SAA7191_REG_CTL3, ctl3);
+	if (err)
+		return -EIO;
+	err = saa7191_write_reg(sd, SAA7191_REG_STDC, stdc);
+	if (err)
+		return -EIO;
+	err = saa7191_write_reg(sd, SAA7191_REG_CHCV, chcv);
+	if (err)
+		return -EIO;
+
+	decoder->norm = norm;
+
+	dprintk("ctl3: %02x stdc: %02x chcv: %02x\n", ctl3,
+		stdc, chcv);
+	dprintk("norm: %llx\n", norm);
+
+	return 0;
+}
+
+static int saa7191_wait_for_signal(struct v4l2_subdev *sd, u8 *status)
+{
+	int i = 0;
+
+	dprintk("Checking for signal...\n");
+
+	for (i = 0; i < SAA7191_SYNC_COUNT; i++) {
+		if (saa7191_read_status(sd, status))
+			return -EIO;
+
+		if (((*status) & SAA7191_STATUS_HLCK) == 0) {
+			dprintk("Signal found\n");
+			return 0;
+		}
+
+		msleep(SAA7191_SYNC_DELAY);
+	}
+
+	dprintk("No signal\n");
+
+	return -EBUSY;
+}
+
+static int saa7191_querystd(struct v4l2_subdev *sd, v4l2_std_id *norm)
+{
+	struct saa7191 *decoder = to_saa7191(sd);
+	u8 stdc = saa7191_read_reg(sd, SAA7191_REG_STDC);
+	u8 ctl3 = saa7191_read_reg(sd, SAA7191_REG_CTL3);
+	u8 status;
+	v4l2_std_id old_norm = decoder->norm;
+	int err = 0;
+
+	dprintk("SAA7191 extended signal auto-detection...\n");
+
+	*norm = V4L2_STD_NTSC | V4L2_STD_PAL | V4L2_STD_SECAM;
+	stdc &= ~SAA7191_STDC_SECS;
+	ctl3 &= ~(SAA7191_CTL3_FSEL);
+
+	err = saa7191_write_reg(sd, SAA7191_REG_STDC, stdc);
+	if (err) {
+		err = -EIO;
+		goto out;
+	}
+	err = saa7191_write_reg(sd, SAA7191_REG_CTL3, ctl3);
+	if (err) {
+		err = -EIO;
+		goto out;
+	}
+
+	ctl3 |= SAA7191_CTL3_AUFD;
+	err = saa7191_write_reg(sd, SAA7191_REG_CTL3, ctl3);
+	if (err) {
+		err = -EIO;
+		goto out;
+	}
+
+	msleep(SAA7191_SYNC_DELAY);
+
+	err = saa7191_wait_for_signal(sd, &status);
+	if (err)
+		goto out;
+
+	if (status & SAA7191_STATUS_FIDT) {
+		/* 60Hz signal -> NTSC */
+		dprintk("60Hz signal: NTSC\n");
+		*norm = V4L2_STD_NTSC;
+		return 0;
+	}
+
+	/* 50Hz signal */
+	dprintk("50Hz signal: Trying PAL...\n");
+
+	/* try PAL first */
+	err = saa7191_s_std(sd, V4L2_STD_PAL);
+	if (err)
+		goto out;
+
+	msleep(SAA7191_SYNC_DELAY);
+
+	err = saa7191_wait_for_signal(sd, &status);
+	if (err)
+		goto out;
+
+	/* not 50Hz ? */
+	if (status & SAA7191_STATUS_FIDT) {
+		dprintk("No 50Hz signal\n");
+		saa7191_s_std(sd, old_norm);
+		return -EAGAIN;
+	}
+
+	if (status & SAA7191_STATUS_CODE) {
+		dprintk("PAL\n");
+		*norm = V4L2_STD_PAL;
+		return saa7191_s_std(sd, old_norm);
+	}
+
+	dprintk("No color detected with PAL - Trying SECAM...\n");
+
+	/* no color detected ? -> try SECAM */
+	err = saa7191_s_std(sd, V4L2_STD_SECAM);
+	if (err)
+		goto out;
+
+	msleep(SAA7191_SYNC_DELAY);
+
+	err = saa7191_wait_for_signal(sd, &status);
+	if (err)
+		goto out;
+
+	/* not 50Hz ? */
+	if (status & SAA7191_STATUS_FIDT) {
+		dprintk("No 50Hz signal\n");
+		err = -EAGAIN;
+		goto out;
+	}
+
+	if (status & SAA7191_STATUS_CODE) {
+		/* Color detected -> SECAM */
+		dprintk("SECAM\n");
+		*norm = V4L2_STD_SECAM;
+		return saa7191_s_std(sd, old_norm);
+	}
+
+	dprintk("No color detected with SECAM - Going back to PAL.\n");
+
+out:
+	return saa7191_s_std(sd, old_norm);
+}
+
+static int saa7191_autodetect_norm(struct v4l2_subdev *sd)
+{
+	u8 status;
+
+	dprintk("SAA7191 signal auto-detection...\n");
+
+	dprintk("Reading status...\n");
+
+	if (saa7191_read_status(sd, &status))
+		return -EIO;
+
+	dprintk("Checking for signal...\n");
+
+	/* no signal ? */
+	if (status & SAA7191_STATUS_HLCK) {
+		dprintk("No signal\n");
+		return -EBUSY;
+	}
+
+	dprintk("Signal found\n");
+
+	if (status & SAA7191_STATUS_FIDT) {
+		/* 60hz signal -> NTSC */
+		dprintk("NTSC\n");
+		return saa7191_s_std(sd, V4L2_STD_NTSC);
+	} else {
+		/* 50hz signal -> PAL */
+		dprintk("PAL\n");
+		return saa7191_s_std(sd, V4L2_STD_PAL);
+	}
+}
+
+static int saa7191_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+	u8 reg;
+	int ret = 0;
+
+	switch (ctrl->id) {
+	case SAA7191_CONTROL_BANDPASS:
+	case SAA7191_CONTROL_BANDPASS_WEIGHT:
+	case SAA7191_CONTROL_CORING:
+		reg = saa7191_read_reg(sd, SAA7191_REG_LUMA);
+		switch (ctrl->id) {
+		case SAA7191_CONTROL_BANDPASS:
+			ctrl->value = ((s32)reg & SAA7191_LUMA_BPSS_MASK)
+				>> SAA7191_LUMA_BPSS_SHIFT;
+			break;
+		case SAA7191_CONTROL_BANDPASS_WEIGHT:
+			ctrl->value = ((s32)reg & SAA7191_LUMA_APER_MASK)
+				>> SAA7191_LUMA_APER_SHIFT;
+			break;
+		case SAA7191_CONTROL_CORING:
+			ctrl->value = ((s32)reg & SAA7191_LUMA_CORI_MASK)
+				>> SAA7191_LUMA_CORI_SHIFT;
+			break;
+		}
+		break;
+	case SAA7191_CONTROL_FORCE_COLOUR:
+	case SAA7191_CONTROL_CHROMA_GAIN:
+		reg = saa7191_read_reg(sd, SAA7191_REG_GAIN);
+		if (ctrl->id == SAA7191_CONTROL_FORCE_COLOUR)
+			ctrl->value = ((s32)reg & SAA7191_GAIN_COLO) ? 1 : 0;
+		else
+			ctrl->value = ((s32)reg & SAA7191_GAIN_LFIS_MASK)
+				>> SAA7191_GAIN_LFIS_SHIFT;
+		break;
+	case V4L2_CID_HUE:
+		reg = saa7191_read_reg(sd, SAA7191_REG_HUEC);
+		if (reg < 0x80)
+			reg += 0x80;
+		else
+			reg -= 0x80;
+		ctrl->value = (s32)reg;
+		break;
+	case SAA7191_CONTROL_VTRC:
+		reg = saa7191_read_reg(sd, SAA7191_REG_STDC);
+		ctrl->value = ((s32)reg & SAA7191_STDC_VTRC) ? 1 : 0;
+		break;
+	case SAA7191_CONTROL_LUMA_DELAY:
+		reg = saa7191_read_reg(sd, SAA7191_REG_CTL3);
+		ctrl->value = ((s32)reg & SAA7191_CTL3_YDEL_MASK)
+			>> SAA7191_CTL3_YDEL_SHIFT;
+		if (ctrl->value >= 4)
+			ctrl->value -= 8;
+		break;
+	case SAA7191_CONTROL_VNR:
+		reg = saa7191_read_reg(sd, SAA7191_REG_CTL4);
+		ctrl->value = ((s32)reg & SAA7191_CTL4_VNOI_MASK)
+			>> SAA7191_CTL4_VNOI_SHIFT;
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static int saa7191_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+	u8 reg;
+	int ret = 0;
+
+	switch (ctrl->id) {
+	case SAA7191_CONTROL_BANDPASS:
+	case SAA7191_CONTROL_BANDPASS_WEIGHT:
+	case SAA7191_CONTROL_CORING:
+		reg = saa7191_read_reg(sd, SAA7191_REG_LUMA);
+		switch (ctrl->id) {
+		case SAA7191_CONTROL_BANDPASS:
+			reg &= ~SAA7191_LUMA_BPSS_MASK;
+			reg |= (ctrl->value << SAA7191_LUMA_BPSS_SHIFT)
+				& SAA7191_LUMA_BPSS_MASK;
+			break;
+		case SAA7191_CONTROL_BANDPASS_WEIGHT:
+			reg &= ~SAA7191_LUMA_APER_MASK;
+			reg |= (ctrl->value << SAA7191_LUMA_APER_SHIFT)
+				& SAA7191_LUMA_APER_MASK;
+			break;
+		case SAA7191_CONTROL_CORING:
+			reg &= ~SAA7191_LUMA_CORI_MASK;
+			reg |= (ctrl->value << SAA7191_LUMA_CORI_SHIFT)
+				& SAA7191_LUMA_CORI_MASK;
+			break;
+		}
+		ret = saa7191_write_reg(sd, SAA7191_REG_LUMA, reg);
+		break;
+	case SAA7191_CONTROL_FORCE_COLOUR:
+	case SAA7191_CONTROL_CHROMA_GAIN:
+		reg = saa7191_read_reg(sd, SAA7191_REG_GAIN);
+		if (ctrl->id == SAA7191_CONTROL_FORCE_COLOUR) {
+			if (ctrl->value)
+				reg |= SAA7191_GAIN_COLO;
+			else
+				reg &= ~SAA7191_GAIN_COLO;
+		} else {
+			reg &= ~SAA7191_GAIN_LFIS_MASK;
+			reg |= (ctrl->value << SAA7191_GAIN_LFIS_SHIFT)
+				& SAA7191_GAIN_LFIS_MASK;
+		}
+		ret = saa7191_write_reg(sd, SAA7191_REG_GAIN, reg);
+		break;
+	case V4L2_CID_HUE:
+		reg = ctrl->value & 0xff;
+		if (reg < 0x80)
+			reg += 0x80;
+		else
+			reg -= 0x80;
+		ret = saa7191_write_reg(sd, SAA7191_REG_HUEC, reg);
+		break;
+	case SAA7191_CONTROL_VTRC:
+		reg = saa7191_read_reg(sd, SAA7191_REG_STDC);
+		if (ctrl->value)
+			reg |= SAA7191_STDC_VTRC;
+		else
+			reg &= ~SAA7191_STDC_VTRC;
+		ret = saa7191_write_reg(sd, SAA7191_REG_STDC, reg);
+		break;
+	case SAA7191_CONTROL_LUMA_DELAY: {
+		s32 value = ctrl->value;
+		if (value < 0)
+			value += 8;
+		reg = saa7191_read_reg(sd, SAA7191_REG_CTL3);
+		reg &= ~SAA7191_CTL3_YDEL_MASK;
+		reg |= (value << SAA7191_CTL3_YDEL_SHIFT)
+			& SAA7191_CTL3_YDEL_MASK;
+		ret = saa7191_write_reg(sd, SAA7191_REG_CTL3, reg);
+		break;
+	}
+	case SAA7191_CONTROL_VNR:
+		reg = saa7191_read_reg(sd, SAA7191_REG_CTL4);
+		reg &= ~SAA7191_CTL4_VNOI_MASK;
+		reg |= (ctrl->value << SAA7191_CTL4_VNOI_SHIFT)
+			& SAA7191_CTL4_VNOI_MASK;
+		ret = saa7191_write_reg(sd, SAA7191_REG_CTL4, reg);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+/* I2C-interface */
+
+static int saa7191_g_input_status(struct v4l2_subdev *sd, u32 *status)
+{
+	u8 status_reg;
+	int res = V4L2_IN_ST_NO_SIGNAL;
+
+	if (saa7191_read_status(sd, &status_reg))
+		return -EIO;
+	if ((status_reg & SAA7191_STATUS_HLCK) == 0)
+		res = 0;
+	if (!(status_reg & SAA7191_STATUS_CODE))
+		res |= V4L2_IN_ST_NO_COLOR;
+	*status = res;
+	return 0;
+}
+
+
+static int saa7191_g_chip_ident(struct v4l2_subdev *sd,
+		struct v4l2_dbg_chip_ident *chip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_SAA7191, 0);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_core_ops saa7191_core_ops = {
+	.g_chip_ident = saa7191_g_chip_ident,
+	.g_ctrl = saa7191_g_ctrl,
+	.s_ctrl = saa7191_s_ctrl,
+	.s_std = saa7191_s_std,
+};
+
+static const struct v4l2_subdev_video_ops saa7191_video_ops = {
+	.s_routing = saa7191_s_routing,
+	.querystd = saa7191_querystd,
+	.g_input_status = saa7191_g_input_status,
+};
+
+static const struct v4l2_subdev_ops saa7191_ops = {
+	.core = &saa7191_core_ops,
+	.video = &saa7191_video_ops,
+};
+
+static int saa7191_probe(struct i2c_client *client,
+			  const struct i2c_device_id *id)
+{
+	int err = 0;
+	struct saa7191 *decoder;
+	struct v4l2_subdev *sd;
+
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	decoder = kzalloc(sizeof(*decoder), GFP_KERNEL);
+	if (!decoder)
+		return -ENOMEM;
+
+	sd = &decoder->sd;
+	v4l2_i2c_subdev_init(sd, client, &saa7191_ops);
+
+	err = saa7191_write_block(sd, sizeof(initseq), initseq);
+	if (err) {
+		printk(KERN_ERR "SAA7191 initialization failed\n");
+		kfree(decoder);
+		return err;
+	}
+
+	printk(KERN_INFO "SAA7191 initialized\n");
+
+	decoder->input = SAA7191_INPUT_COMPOSITE;
+	decoder->norm = V4L2_STD_PAL;
+
+	err = saa7191_autodetect_norm(sd);
+	if (err && (err != -EBUSY))
+		printk(KERN_ERR "SAA7191: Signal auto-detection failed\n");
+
+	return 0;
+}
+
+static int saa7191_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	kfree(to_saa7191(sd));
+	return 0;
+}
+
+static const struct i2c_device_id saa7191_id[] = {
+	{ "saa7191", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, saa7191_id);
+
+static struct i2c_driver saa7191_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "saa7191",
+	},
+	.probe		= saa7191_probe,
+	.remove		= saa7191_remove,
+	.id_table	= saa7191_id,
+};
+
+module_i2c_driver(saa7191_driver);
diff --git a/drivers/media/i2c/saa7191.h b/drivers/media/i2c/saa7191.h
new file mode 100644
index 000000000000..803c74d6066f
--- /dev/null
+++ b/drivers/media/i2c/saa7191.h
@@ -0,0 +1,245 @@
+/*
+ *  saa7191.h - Philips SAA7191 video decoder driver
+ *
+ *  Copyright (C) 2003 Ladislav Michl <ladis@linux-mips.org>
+ *  Copyright (C) 2004,2005 Mikael Nousiainen <tmnousia@cc.hut.fi>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2 as
+ *  published by the Free Software Foundation.
+ */
+
+#ifndef _SAA7191_H_
+#define _SAA7191_H_
+
+/* Philips SAA7191 DMSD I2C bus address */
+#define SAA7191_ADDR		0x8a
+
+/* Register subaddresses. */
+#define SAA7191_REG_IDEL	0x00
+#define SAA7191_REG_HSYB	0x01
+#define SAA7191_REG_HSYS	0x02
+#define SAA7191_REG_HCLB	0x03
+#define SAA7191_REG_HCLS	0x04
+#define SAA7191_REG_HPHI	0x05
+#define SAA7191_REG_LUMA	0x06
+#define SAA7191_REG_HUEC	0x07
+#define SAA7191_REG_CKTQ	0x08 /* bits 3-7 */
+#define SAA7191_REG_CKTS	0x09 /* bits 3-7 */
+#define SAA7191_REG_PLSE	0x0a
+#define SAA7191_REG_SESE	0x0b
+#define SAA7191_REG_GAIN	0x0c
+#define SAA7191_REG_STDC	0x0d
+#define SAA7191_REG_IOCK	0x0e
+#define SAA7191_REG_CTL3	0x0f
+#define SAA7191_REG_CTL4	0x10
+#define SAA7191_REG_CHCV	0x11
+#define SAA7191_REG_HS6B	0x14
+#define SAA7191_REG_HS6S	0x15
+#define SAA7191_REG_HC6B	0x16
+#define SAA7191_REG_HC6S	0x17
+#define SAA7191_REG_HP6I	0x18
+#define SAA7191_REG_STATUS	0xff	/* not really a subaddress */
+
+/* Status Register definitions */
+#define SAA7191_STATUS_CODE	0x01	/* color detected flag */
+#define SAA7191_STATUS_FIDT	0x20	/* signal type 50/60 Hz */
+#define SAA7191_STATUS_HLCK	0x40	/* PLL unlocked(1)/locked(0) */
+#define SAA7191_STATUS_STTC	0x80	/* tv/vtr time constant */
+
+/* Luminance Control Register definitions */
+/* input mode select bit:
+ * 0=CVBS (chrominance trap active), 1=S-Video (trap bypassed) */
+#define SAA7191_LUMA_BYPS	0x80
+/* pre-filter (only when chrominance trap is active) */
+#define SAA7191_LUMA_PREF	0x40
+/* aperture bandpass to select different characteristics with maximums
+ * (bits 4-5) */
+#define SAA7191_LUMA_BPSS_MASK	0x30
+#define SAA7191_LUMA_BPSS_SHIFT	4
+#define SAA7191_LUMA_BPSS_3	0x30
+#define SAA7191_LUMA_BPSS_2	0x20
+#define SAA7191_LUMA_BPSS_1	0x10
+#define SAA7191_LUMA_BPSS_0	0x00
+/* coring range for high frequency components according to 8-bit luminance
+ * (bits 2-3)
+ * 0=coring off, n= (+-)n LSB */
+#define SAA7191_LUMA_CORI_MASK	0x0c
+#define SAA7191_LUMA_CORI_SHIFT	2
+#define SAA7191_LUMA_CORI_3	0x0c
+#define SAA7191_LUMA_CORI_2	0x08
+#define SAA7191_LUMA_CORI_1	0x04
+#define SAA7191_LUMA_CORI_0	0x00
+/* aperture bandpass filter weights high frequency components of luminance
+ * signal (bits 0-1)
+ * 0=factor 0, 1=0.25, 2=0.5, 3=1 */
+#define SAA7191_LUMA_APER_MASK	0x03
+#define SAA7191_LUMA_APER_SHIFT	0
+#define SAA7191_LUMA_APER_3	0x03
+#define SAA7191_LUMA_APER_2	0x02
+#define SAA7191_LUMA_APER_1	0x01
+#define SAA7191_LUMA_APER_0	0x00
+
+/* Chrominance Gain Control Settings Register definitions */
+/* colour on: 0=automatic colour-killer enabled, 1=forced colour on */
+#define SAA7191_GAIN_COLO	0x80
+/* chrominance gain control (AGC filter)
+ * 0=loop filter time constant slow, 1=medium, 2=fast, 3=actual gain */
+#define SAA7191_GAIN_LFIS_MASK	0x60
+#define SAA7191_GAIN_LFIS_SHIFT	5
+#define SAA7191_GAIN_LFIS_3	0x60
+#define SAA7191_GAIN_LFIS_2	0x40
+#define SAA7191_GAIN_LFIS_1	0x20
+#define SAA7191_GAIN_LFIS_0	0x00
+
+/* Standard/Mode Control Register definitions */
+/* tv/vtr mode bit: 0=TV mode (slow time constant),
+ * 1=VTR mode (fast time constant) */
+#define SAA7191_STDC_VTRC	0x80
+/* SAA7191B-specific functions enable (RTCO, ODD and GPSW0 outputs)
+ * 0=outputs set to high-impedance (circuit equals SAA7191), 1=enabled */
+#define SAA7191_STDC_NFEN	0x08
+/* HREF generation: 0=like SAA7191, 1=HREF is 8xLLC2 clocks earlier */
+#define SAA7191_STDC_HRMV	0x04
+/* general purpose switch 0
+ * (not used with VINO afaik) */
+#define SAA7191_STDC_GPSW0	0x02
+/* SECAM mode bit: 0=other standards, 1=SECAM */
+#define SAA7191_STDC_SECS	0x01
+
+/* I/O and Clock Control Register definitions */
+/* horizontal clock PLL: 0=PLL closed,
+ * 1=PLL circuit open and horizontal freq fixed */
+#define SAA7191_IOCK_HPLL	0x80
+/* colour-difference output enable (outputs UV0-UV7) */
+#define SAA7191_IOCK_OEDC	0x40
+/* H-sync output enable */
+#define SAA7191_IOCK_OEHS	0x20
+/* V-sync output enable */
+#define SAA7191_IOCK_OEVS	0x10
+/* luminance output enable (outputs Y0-Y7) */
+#define SAA7191_IOCK_OEDY	0x08
+/* S-VHS bit (chrominance from CVBS or from chrominance input):
+ * 0=controlled by BYPS-bit, 1=from chrominance input */
+#define SAA7191_IOCK_CHRS	0x04
+/* general purpose switch 2
+ * VINO-specific: 0=used with CVBS, 1=used with S-Video */
+#define SAA7191_IOCK_GPSW2	0x02
+/* general purpose switch 1 */
+/* VINO-specific: 0=always, 1=not used!*/
+#define SAA7191_IOCK_GPSW1	0x01
+
+/* Miscellaneous Control #1 Register definitions */
+/* automatic field detection (50/60Hz standard) */
+#define SAA7191_CTL3_AUFD	0x80
+/* field select: (if AUFD=0)
+ * 0=50Hz (625 lines), 1=60Hz (525 lines) */
+#define SAA7191_CTL3_FSEL	0x40
+/* SECAM cross-colour reduction enable */
+#define SAA7191_CTL3_SXCR	0x20
+/* sync and clamping pulse enable (HCL and HSY outputs) */
+#define SAA7191_CTL3_SCEN	0x10
+/* output format: 0=4:1:1, 1=4:2:2 (4:2:2 for VINO) */
+#define SAA7191_CTL3_OFTS	0x08
+/* luminance delay compensation
+ * 0=0*2/LLC,  1=+1*2/LLC, 2=+2*2/LLC, 3=+3*2/LLC,
+ * 4=-4*2/LLC, 5=-3*2/LLC, 6=-2*2/LLC, 7=-1*2/LLC
+ * step size = 2/LLC = 67.8ns for 50Hz, 81.5ns for 60Hz */
+#define SAA7191_CTL3_YDEL_MASK	0x07
+#define SAA7191_CTL3_YDEL_SHIFT	0
+#define SAA7191_CTL3_YDEL2	0x04
+#define SAA7191_CTL3_YDEL1	0x02
+#define SAA7191_CTL3_YDEL0	0x01
+
+/* Miscellaneous Control #2 Register definitions */
+/* select HREF position
+ * 0=normal, HREF is matched to YUV output port,
+ * 1=HREF is matched to CVBS input port */
+#define SAA7191_CTL4_HRFS	0x04
+/* vertical noise reduction
+ * 0=normal, 1=searching window, 2=auto-deflection, 3=reduction bypassed */
+#define SAA7191_CTL4_VNOI_MASK	0x03
+#define SAA7191_CTL4_VNOI_SHIFT	0
+#define SAA7191_CTL4_VNOI_3	0x03
+#define SAA7191_CTL4_VNOI_2	0x02
+#define SAA7191_CTL4_VNOI_1	0x01
+#define SAA7191_CTL4_VNOI_0	0x00
+
+/* Chrominance Gain Control Register definitions
+ * - for QAM-modulated input signals, effects output amplitude
+ * (SECAM gain fixed)
+ * (nominal values for UV CCIR level) */
+#define SAA7191_CHCV_NTSC	0x2c
+#define SAA7191_CHCV_PAL	0x59
+
+/* Driver interface definitions */
+#define SAA7191_INPUT_COMPOSITE	0
+#define SAA7191_INPUT_SVIDEO	1
+
+#define SAA7191_NORM_PAL	1
+#define SAA7191_NORM_NTSC	2
+#define SAA7191_NORM_SECAM	3
+
+struct saa7191_status {
+	/* 0=no signal, 1=signal detected */
+	int signal;
+	/* 0=50hz (pal) signal, 1=60hz (ntsc) signal */
+	int signal_60hz;
+	/* 0=no color detected, 1=color detected */
+	int color;
+
+	/* current SAA7191_INPUT_ */
+	int input;
+	/* current SAA7191_NORM_ */
+	int norm;
+};
+
+#define SAA7191_BANDPASS_MIN		0x00
+#define SAA7191_BANDPASS_MAX		0x03
+#define SAA7191_BANDPASS_DEFAULT	0x00
+
+#define SAA7191_BANDPASS_WEIGHT_MIN	0x00
+#define SAA7191_BANDPASS_WEIGHT_MAX	0x03
+#define SAA7191_BANDPASS_WEIGHT_DEFAULT	0x01
+
+#define SAA7191_CORING_MIN		0x00
+#define SAA7191_CORING_MAX		0x03
+#define SAA7191_CORING_DEFAULT		0x00
+
+#define SAA7191_HUE_MIN			0x00
+#define SAA7191_HUE_MAX			0xff
+#define SAA7191_HUE_DEFAULT		0x80
+
+#define SAA7191_VTRC_MIN		0x00
+#define SAA7191_VTRC_MAX		0x01
+#define SAA7191_VTRC_DEFAULT		0x00
+
+#define SAA7191_FORCE_COLOUR_MIN	0x00
+#define SAA7191_FORCE_COLOUR_MAX	0x01
+#define SAA7191_FORCE_COLOUR_DEFAULT	0x00
+
+#define SAA7191_CHROMA_GAIN_MIN		0x00
+#define SAA7191_CHROMA_GAIN_MAX		0x03
+#define SAA7191_CHROMA_GAIN_DEFAULT	0x00
+
+#define SAA7191_LUMA_DELAY_MIN		-0x04
+#define SAA7191_LUMA_DELAY_MAX		0x03
+#define SAA7191_LUMA_DELAY_DEFAULT	0x01
+
+#define SAA7191_VNR_MIN			0x00
+#define SAA7191_VNR_MAX			0x03
+#define SAA7191_VNR_DEFAULT		0x00
+
+#define SAA7191_CONTROL_BANDPASS	(V4L2_CID_PRIVATE_BASE + 0)
+#define SAA7191_CONTROL_BANDPASS_WEIGHT	(V4L2_CID_PRIVATE_BASE + 1)
+#define SAA7191_CONTROL_CORING		(V4L2_CID_PRIVATE_BASE + 2)
+#define SAA7191_CONTROL_FORCE_COLOUR	(V4L2_CID_PRIVATE_BASE + 3)
+#define SAA7191_CONTROL_CHROMA_GAIN	(V4L2_CID_PRIVATE_BASE + 4)
+#define SAA7191_CONTROL_VTRC		(V4L2_CID_PRIVATE_BASE + 5)
+#define SAA7191_CONTROL_LUMA_DELAY	(V4L2_CID_PRIVATE_BASE + 6)
+#define SAA7191_CONTROL_VNR		(V4L2_CID_PRIVATE_BASE + 7)
+
+#define	DECODER_SAA7191_GET_STATUS	_IOR('d', 195, struct saa7191_status)
+#define	DECODER_SAA7191_SET_NORM	_IOW('d', 196, int)
+
+#endif
diff --git a/drivers/media/i2c/smiapp-pll.c b/drivers/media/i2c/smiapp-pll.c
new file mode 100644
index 000000000000..a577614bd84f
--- /dev/null
+++ b/drivers/media/i2c/smiapp-pll.c
@@ -0,0 +1,418 @@
+/*
+ * drivers/media/i2c/smiapp-pll.c
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2011--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#include <linux/gcd.h>
+#include <linux/lcm.h>
+#include <linux/module.h>
+
+#include "smiapp-pll.h"
+
+/* Return an even number or one. */
+static inline uint32_t clk_div_even(uint32_t a)
+{
+	return max_t(uint32_t, 1, a & ~1);
+}
+
+/* Return an even number or one. */
+static inline uint32_t clk_div_even_up(uint32_t a)
+{
+	if (a == 1)
+		return 1;
+	return (a + 1) & ~1;
+}
+
+static inline uint32_t is_one_or_even(uint32_t a)
+{
+	if (a == 1)
+		return 1;
+	if (a & 1)
+		return 0;
+
+	return 1;
+}
+
+static int bounds_check(struct device *dev, uint32_t val,
+			uint32_t min, uint32_t max, char *str)
+{
+	if (val >= min && val <= max)
+		return 0;
+
+	dev_warn(dev, "%s out of bounds: %d (%d--%d)\n", str, val, min, max);
+
+	return -EINVAL;
+}
+
+static void print_pll(struct device *dev, struct smiapp_pll *pll)
+{
+	dev_dbg(dev, "pre_pll_clk_div\t%d\n",  pll->pre_pll_clk_div);
+	dev_dbg(dev, "pll_multiplier \t%d\n",  pll->pll_multiplier);
+	if (pll->flags != SMIAPP_PLL_FLAG_NO_OP_CLOCKS) {
+		dev_dbg(dev, "op_sys_clk_div \t%d\n", pll->op_sys_clk_div);
+		dev_dbg(dev, "op_pix_clk_div \t%d\n", pll->op_pix_clk_div);
+	}
+	dev_dbg(dev, "vt_sys_clk_div \t%d\n",  pll->vt_sys_clk_div);
+	dev_dbg(dev, "vt_pix_clk_div \t%d\n",  pll->vt_pix_clk_div);
+
+	dev_dbg(dev, "ext_clk_freq_hz \t%d\n", pll->ext_clk_freq_hz);
+	dev_dbg(dev, "pll_ip_clk_freq_hz \t%d\n", pll->pll_ip_clk_freq_hz);
+	dev_dbg(dev, "pll_op_clk_freq_hz \t%d\n", pll->pll_op_clk_freq_hz);
+	if (pll->flags & SMIAPP_PLL_FLAG_NO_OP_CLOCKS) {
+		dev_dbg(dev, "op_sys_clk_freq_hz \t%d\n",
+			pll->op_sys_clk_freq_hz);
+		dev_dbg(dev, "op_pix_clk_freq_hz \t%d\n",
+			pll->op_pix_clk_freq_hz);
+	}
+	dev_dbg(dev, "vt_sys_clk_freq_hz \t%d\n", pll->vt_sys_clk_freq_hz);
+	dev_dbg(dev, "vt_pix_clk_freq_hz \t%d\n", pll->vt_pix_clk_freq_hz);
+}
+
+int smiapp_pll_calculate(struct device *dev, struct smiapp_pll_limits *limits,
+			 struct smiapp_pll *pll)
+{
+	uint32_t sys_div;
+	uint32_t best_pix_div = INT_MAX >> 1;
+	uint32_t vt_op_binning_div;
+	uint32_t lane_op_clock_ratio;
+	uint32_t mul, div;
+	uint32_t more_mul_min, more_mul_max;
+	uint32_t more_mul_factor;
+	uint32_t min_vt_div, max_vt_div, vt_div;
+	uint32_t min_sys_div, max_sys_div;
+	unsigned int i;
+	int rval;
+
+	if (pll->flags & SMIAPP_PLL_FLAG_OP_PIX_CLOCK_PER_LANE)
+		lane_op_clock_ratio = pll->lanes;
+	else
+		lane_op_clock_ratio = 1;
+	dev_dbg(dev, "lane_op_clock_ratio: %d\n", lane_op_clock_ratio);
+
+	dev_dbg(dev, "binning: %dx%d\n", pll->binning_horizontal,
+		pll->binning_vertical);
+
+	/* CSI transfers 2 bits per clock per lane; thus times 2 */
+	pll->pll_op_clk_freq_hz = pll->link_freq * 2
+		* (pll->lanes / lane_op_clock_ratio);
+
+	/* Figure out limits for pre-pll divider based on extclk */
+	dev_dbg(dev, "min / max pre_pll_clk_div: %d / %d\n",
+		limits->min_pre_pll_clk_div, limits->max_pre_pll_clk_div);
+	limits->max_pre_pll_clk_div =
+		min_t(uint16_t, limits->max_pre_pll_clk_div,
+		      clk_div_even(pll->ext_clk_freq_hz /
+				   limits->min_pll_ip_freq_hz));
+	limits->min_pre_pll_clk_div =
+		max_t(uint16_t, limits->min_pre_pll_clk_div,
+		      clk_div_even_up(
+			      DIV_ROUND_UP(pll->ext_clk_freq_hz,
+					   limits->max_pll_ip_freq_hz)));
+	dev_dbg(dev, "pre-pll check: min / max pre_pll_clk_div: %d / %d\n",
+		limits->min_pre_pll_clk_div, limits->max_pre_pll_clk_div);
+
+	i = gcd(pll->pll_op_clk_freq_hz, pll->ext_clk_freq_hz);
+	mul = div_u64(pll->pll_op_clk_freq_hz, i);
+	div = pll->ext_clk_freq_hz / i;
+	dev_dbg(dev, "mul %d / div %d\n", mul, div);
+
+	limits->min_pre_pll_clk_div =
+		max_t(uint16_t, limits->min_pre_pll_clk_div,
+		      clk_div_even_up(
+			      DIV_ROUND_UP(mul * pll->ext_clk_freq_hz,
+					   limits->max_pll_op_freq_hz)));
+	dev_dbg(dev, "pll_op check: min / max pre_pll_clk_div: %d / %d\n",
+		limits->min_pre_pll_clk_div, limits->max_pre_pll_clk_div);
+
+	if (limits->min_pre_pll_clk_div > limits->max_pre_pll_clk_div) {
+		dev_err(dev, "unable to compute pre_pll divisor\n");
+		return -EINVAL;
+	}
+
+	pll->pre_pll_clk_div = limits->min_pre_pll_clk_div;
+
+	/*
+	 * Get pre_pll_clk_div so that our pll_op_clk_freq_hz won't be
+	 * too high.
+	 */
+	dev_dbg(dev, "pre_pll_clk_div %d\n", pll->pre_pll_clk_div);
+
+	/* Don't go above max pll multiplier. */
+	more_mul_max = limits->max_pll_multiplier / mul;
+	dev_dbg(dev, "more_mul_max: max_pll_multiplier check: %d\n",
+		more_mul_max);
+	/* Don't go above max pll op frequency. */
+	more_mul_max =
+		min_t(int,
+		      more_mul_max,
+		      limits->max_pll_op_freq_hz
+		      / (pll->ext_clk_freq_hz / pll->pre_pll_clk_div * mul));
+	dev_dbg(dev, "more_mul_max: max_pll_op_freq_hz check: %d\n",
+		more_mul_max);
+	/* Don't go above the division capability of op sys clock divider. */
+	more_mul_max = min(more_mul_max,
+			   limits->max_op_sys_clk_div * pll->pre_pll_clk_div
+			   / div);
+	dev_dbg(dev, "more_mul_max: max_op_sys_clk_div check: %d\n",
+		more_mul_max);
+	/* Ensure we won't go above min_pll_multiplier. */
+	more_mul_max = min(more_mul_max,
+			   DIV_ROUND_UP(limits->max_pll_multiplier, mul));
+	dev_dbg(dev, "more_mul_max: min_pll_multiplier check: %d\n",
+		more_mul_max);
+
+	/* Ensure we won't go below min_pll_op_freq_hz. */
+	more_mul_min = DIV_ROUND_UP(limits->min_pll_op_freq_hz,
+				    pll->ext_clk_freq_hz / pll->pre_pll_clk_div
+				    * mul);
+	dev_dbg(dev, "more_mul_min: min_pll_op_freq_hz check: %d\n",
+		more_mul_min);
+	/* Ensure we won't go below min_pll_multiplier. */
+	more_mul_min = max(more_mul_min,
+			   DIV_ROUND_UP(limits->min_pll_multiplier, mul));
+	dev_dbg(dev, "more_mul_min: min_pll_multiplier check: %d\n",
+		more_mul_min);
+
+	if (more_mul_min > more_mul_max) {
+		dev_warn(dev,
+			 "unable to compute more_mul_min and more_mul_max");
+		return -EINVAL;
+	}
+
+	more_mul_factor = lcm(div, pll->pre_pll_clk_div) / div;
+	dev_dbg(dev, "more_mul_factor: %d\n", more_mul_factor);
+	more_mul_factor = lcm(more_mul_factor, limits->min_op_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: %d\n", i);
+	if (i > more_mul_max) {
+		dev_warn(dev, "final more_mul is bad, max %d", more_mul_max);
+		return -EINVAL;
+	}
+
+	pll->pll_multiplier = mul * i;
+	pll->op_sys_clk_div = div * i / pll->pre_pll_clk_div;
+	dev_dbg(dev, "op_sys_clk_div: %d\n", pll->op_sys_clk_div);
+
+	pll->pll_ip_clk_freq_hz = pll->ext_clk_freq_hz
+		/ pll->pre_pll_clk_div;
+
+	pll->pll_op_clk_freq_hz = pll->pll_ip_clk_freq_hz
+		* pll->pll_multiplier;
+
+	/* Derive pll_op_clk_freq_hz. */
+	pll->op_sys_clk_freq_hz =
+		pll->pll_op_clk_freq_hz / pll->op_sys_clk_div;
+
+	pll->op_pix_clk_div = pll->bits_per_pixel;
+	dev_dbg(dev, "op_pix_clk_div: %d\n", pll->op_pix_clk_div);
+
+	pll->op_pix_clk_freq_hz =
+		pll->op_sys_clk_freq_hz / pll->op_pix_clk_div;
+
+	/*
+	 * 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 (limits->min_line_length_pck_bin > limits->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: %d\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 into account as well.
+	 *
+	 * Find absolute limits for the factor of vt divider.
+	 */
+	dev_dbg(dev, "scale_m: %d\n", pll->scale_m);
+	min_vt_div = DIV_ROUND_UP(pll->op_pix_clk_div * pll->op_sys_clk_div
+				  * pll->scale_n,
+				  lane_op_clock_ratio * vt_op_binning_div
+				  * pll->scale_m);
+
+	/* Find smallest and biggest allowed vt divisor. */
+	dev_dbg(dev, "min_vt_div: %d\n", min_vt_div);
+	min_vt_div = max(min_vt_div,
+			 DIV_ROUND_UP(pll->pll_op_clk_freq_hz,
+				      limits->max_vt_pix_clk_freq_hz));
+	dev_dbg(dev, "min_vt_div: max_vt_pix_clk_freq_hz: %d\n",
+		min_vt_div);
+	min_vt_div = max_t(uint32_t, min_vt_div,
+			   limits->min_vt_pix_clk_div
+			   * limits->min_vt_sys_clk_div);
+	dev_dbg(dev, "min_vt_div: min_vt_clk_div: %d\n", min_vt_div);
+
+	max_vt_div = limits->max_vt_sys_clk_div * limits->max_vt_pix_clk_div;
+	dev_dbg(dev, "max_vt_div: %d\n", max_vt_div);
+	max_vt_div = min(max_vt_div,
+			 DIV_ROUND_UP(pll->pll_op_clk_freq_hz,
+				      limits->min_vt_pix_clk_freq_hz));
+	dev_dbg(dev, "max_vt_div: min_vt_pix_clk_freq_hz: %d\n",
+		max_vt_div);
+
+	/*
+	 * Find limitsits for sys_clk_div. Not all values are possible
+	 * with all values of pix_clk_div.
+	 */
+	min_sys_div = limits->min_vt_sys_clk_div;
+	dev_dbg(dev, "min_sys_div: %d\n", min_sys_div);
+	min_sys_div = max(min_sys_div,
+			  DIV_ROUND_UP(min_vt_div,
+				       limits->max_vt_pix_clk_div));
+	dev_dbg(dev, "min_sys_div: max_vt_pix_clk_div: %d\n", min_sys_div);
+	min_sys_div = max(min_sys_div,
+			  pll->pll_op_clk_freq_hz
+			  / limits->max_vt_sys_clk_freq_hz);
+	dev_dbg(dev, "min_sys_div: max_pll_op_clk_freq_hz: %d\n", min_sys_div);
+	min_sys_div = clk_div_even_up(min_sys_div);
+	dev_dbg(dev, "min_sys_div: one or even: %d\n", min_sys_div);
+
+	max_sys_div = limits->max_vt_sys_clk_div;
+	dev_dbg(dev, "max_sys_div: %d\n", max_sys_div);
+	max_sys_div = min(max_sys_div,
+			  DIV_ROUND_UP(max_vt_div,
+				       limits->min_vt_pix_clk_div));
+	dev_dbg(dev, "max_sys_div: min_vt_pix_clk_div: %d\n", max_sys_div);
+	max_sys_div = min(max_sys_div,
+			  DIV_ROUND_UP(pll->pll_op_clk_freq_hz,
+				       limits->min_vt_pix_clk_freq_hz));
+	dev_dbg(dev, "max_sys_div: min_vt_pix_clk_freq_hz: %d\n", 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 += 2 - (vt_div & 1)) {
+		for (sys_div = min_sys_div;
+		     sys_div <= max_sys_div;
+		     sys_div += 2 - (sys_div & 1)) {
+			int pix_div = DIV_ROUND_UP(vt_div, sys_div);
+
+			if (pix_div < limits->min_vt_pix_clk_div
+			    || pix_div > limits->max_vt_pix_clk_div) {
+				dev_dbg(dev,
+					"pix_div %d too small or too big (%d--%d)\n",
+					pix_div,
+					limits->min_vt_pix_clk_div,
+					limits->max_vt_pix_clk_div);
+				continue;
+			}
+
+			/* Check if this one is better. */
+			if (pix_div * sys_div
+			    <= roundup(min_vt_div, best_pix_div))
+				best_pix_div = pix_div;
+		}
+		if (best_pix_div < INT_MAX >> 1)
+			break;
+	}
+
+	pll->vt_sys_clk_div = DIV_ROUND_UP(min_vt_div, best_pix_div);
+	pll->vt_pix_clk_div = best_pix_div;
+
+	pll->vt_sys_clk_freq_hz =
+		pll->pll_op_clk_freq_hz / pll->vt_sys_clk_div;
+	pll->vt_pix_clk_freq_hz =
+		pll->vt_sys_clk_freq_hz / pll->vt_pix_clk_div;
+
+	pll->pixel_rate_csi =
+		pll->op_pix_clk_freq_hz * lane_op_clock_ratio;
+
+	print_pll(dev, pll);
+
+	rval = bounds_check(dev, pll->pre_pll_clk_div,
+			    limits->min_pre_pll_clk_div,
+			    limits->max_pre_pll_clk_div, "pre_pll_clk_div");
+	if (!rval)
+		rval = bounds_check(
+			dev, pll->pll_ip_clk_freq_hz,
+			limits->min_pll_ip_freq_hz, limits->max_pll_ip_freq_hz,
+			"pll_ip_clk_freq_hz");
+	if (!rval)
+		rval = bounds_check(
+			dev, pll->pll_multiplier,
+			limits->min_pll_multiplier, limits->max_pll_multiplier,
+			"pll_multiplier");
+	if (!rval)
+		rval = bounds_check(
+			dev, pll->pll_op_clk_freq_hz,
+			limits->min_pll_op_freq_hz, limits->max_pll_op_freq_hz,
+			"pll_op_clk_freq_hz");
+	if (!rval)
+		rval = bounds_check(
+			dev, pll->op_sys_clk_div,
+			limits->min_op_sys_clk_div, limits->max_op_sys_clk_div,
+			"op_sys_clk_div");
+	if (!rval)
+		rval = bounds_check(
+			dev, pll->op_pix_clk_div,
+			limits->min_op_pix_clk_div, limits->max_op_pix_clk_div,
+			"op_pix_clk_div");
+	if (!rval)
+		rval = bounds_check(
+			dev, pll->op_sys_clk_freq_hz,
+			limits->min_op_sys_clk_freq_hz,
+			limits->max_op_sys_clk_freq_hz,
+			"op_sys_clk_freq_hz");
+	if (!rval)
+		rval = bounds_check(
+			dev, pll->op_pix_clk_freq_hz,
+			limits->min_op_pix_clk_freq_hz,
+			limits->max_op_pix_clk_freq_hz,
+			"op_pix_clk_freq_hz");
+	if (!rval)
+		rval = bounds_check(
+			dev, pll->vt_sys_clk_freq_hz,
+			limits->min_vt_sys_clk_freq_hz,
+			limits->max_vt_sys_clk_freq_hz,
+			"vt_sys_clk_freq_hz");
+	if (!rval)
+		rval = bounds_check(
+			dev, pll->vt_pix_clk_freq_hz,
+			limits->min_vt_pix_clk_freq_hz,
+			limits->max_vt_pix_clk_freq_hz,
+			"vt_pix_clk_freq_hz");
+
+	return rval;
+}
+EXPORT_SYMBOL_GPL(smiapp_pll_calculate);
+
+MODULE_AUTHOR("Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>");
+MODULE_DESCRIPTION("Generic SMIA/SMIA++ PLL calculator");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/smiapp-pll.h b/drivers/media/i2c/smiapp-pll.h
new file mode 100644
index 000000000000..cb2d2db5d02d
--- /dev/null
+++ b/drivers/media/i2c/smiapp-pll.h
@@ -0,0 +1,103 @@
+/*
+ * drivers/media/i2c/smiapp-pll.h
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#ifndef SMIAPP_PLL_H
+#define SMIAPP_PLL_H
+
+#include <linux/device.h>
+
+struct smiapp_pll {
+	uint8_t lanes;
+	uint8_t binning_horizontal;
+	uint8_t binning_vertical;
+	uint8_t scale_m;
+	uint8_t scale_n;
+	uint8_t bits_per_pixel;
+	uint16_t flags;
+	uint32_t link_freq;
+
+	uint16_t pre_pll_clk_div;
+	uint16_t pll_multiplier;
+	uint16_t op_sys_clk_div;
+	uint16_t op_pix_clk_div;
+	uint16_t vt_sys_clk_div;
+	uint16_t vt_pix_clk_div;
+
+	uint32_t ext_clk_freq_hz;
+	uint32_t pll_ip_clk_freq_hz;
+	uint32_t pll_op_clk_freq_hz;
+	uint32_t op_sys_clk_freq_hz;
+	uint32_t op_pix_clk_freq_hz;
+	uint32_t vt_sys_clk_freq_hz;
+	uint32_t vt_pix_clk_freq_hz;
+
+	uint32_t pixel_rate_csi;
+};
+
+struct smiapp_pll_limits {
+	/* Strict PLL limits */
+	uint32_t min_ext_clk_freq_hz;
+	uint32_t max_ext_clk_freq_hz;
+	uint16_t min_pre_pll_clk_div;
+	uint16_t max_pre_pll_clk_div;
+	uint32_t min_pll_ip_freq_hz;
+	uint32_t max_pll_ip_freq_hz;
+	uint16_t min_pll_multiplier;
+	uint16_t max_pll_multiplier;
+	uint32_t min_pll_op_freq_hz;
+	uint32_t max_pll_op_freq_hz;
+
+	uint16_t min_vt_sys_clk_div;
+	uint16_t max_vt_sys_clk_div;
+	uint32_t min_vt_sys_clk_freq_hz;
+	uint32_t max_vt_sys_clk_freq_hz;
+	uint16_t min_vt_pix_clk_div;
+	uint16_t max_vt_pix_clk_div;
+	uint32_t min_vt_pix_clk_freq_hz;
+	uint32_t max_vt_pix_clk_freq_hz;
+
+	uint16_t min_op_sys_clk_div;
+	uint16_t max_op_sys_clk_div;
+	uint32_t min_op_sys_clk_freq_hz;
+	uint32_t max_op_sys_clk_freq_hz;
+	uint16_t min_op_pix_clk_div;
+	uint16_t max_op_pix_clk_div;
+	uint32_t min_op_pix_clk_freq_hz;
+	uint32_t max_op_pix_clk_freq_hz;
+
+	/* Other relevant limits */
+	uint32_t min_line_length_pck_bin;
+	uint32_t min_line_length_pck;
+};
+
+/* op pix clock is for all lanes in total normally */
+#define SMIAPP_PLL_FLAG_OP_PIX_CLOCK_PER_LANE			(1 << 0)
+#define SMIAPP_PLL_FLAG_NO_OP_CLOCKS				(1 << 1)
+
+struct device;
+
+int smiapp_pll_calculate(struct device *dev, struct smiapp_pll_limits *limits,
+			 struct smiapp_pll *pll);
+
+#endif /* SMIAPP_PLL_H */
diff --git a/drivers/media/i2c/smiapp/Kconfig b/drivers/media/i2c/smiapp/Kconfig
new file mode 100644
index 000000000000..3149cda1d0db
--- /dev/null
+++ b/drivers/media/i2c/smiapp/Kconfig
@@ -0,0 +1,7 @@
+config VIDEO_SMIAPP
+	tristate "SMIA++/SMIA sensor support"
+	depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API && HAVE_CLK
+	depends on MEDIA_CAMERA_SUPPORT
+	select VIDEO_SMIAPP_PLL
+	---help---
+	  This is a generic driver for SMIA++/SMIA camera modules.
diff --git a/drivers/media/i2c/smiapp/Makefile b/drivers/media/i2c/smiapp/Makefile
new file mode 100644
index 000000000000..f45a003cbe7e
--- /dev/null
+++ b/drivers/media/i2c/smiapp/Makefile
@@ -0,0 +1,5 @@
+smiapp-objs			+= smiapp-core.o smiapp-regs.o \
+				   smiapp-quirk.o smiapp-limits.o
+obj-$(CONFIG_VIDEO_SMIAPP)	+= smiapp.o
+
+ccflags-y += -Idrivers/media/i2c
diff --git a/drivers/media/i2c/smiapp/smiapp-core.c b/drivers/media/i2c/smiapp/smiapp-core.c
new file mode 100644
index 000000000000..e08e588ad24b
--- /dev/null
+++ b/drivers/media/i2c/smiapp/smiapp-core.c
@@ -0,0 +1,2898 @@
+/*
+ * drivers/media/i2c/smiapp/smiapp-core.c
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2010--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.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>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/gpio.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/regulator/consumer.h>
+#include <linux/v4l2-mediabus.h>
+#include <media/v4l2-device.h>
+
+#include "smiapp.h"
+
+#define SMIAPP_ALIGN_DIM(dim, flags)	\
+	((flags) & V4L2_SEL_FLAG_GE	\
+	 ? ALIGN((dim), 2)		\
+	 : (dim) & ~1)
+
+/*
+ * smiapp_module_idents - supported camera modules
+ */
+static const struct smiapp_module_ident smiapp_module_idents[] = {
+	SMIAPP_IDENT_L(0x01, 0x022b, -1, "vs6555"),
+	SMIAPP_IDENT_L(0x01, 0x022e, -1, "vw6558"),
+	SMIAPP_IDENT_L(0x07, 0x7698, -1, "ovm7698"),
+	SMIAPP_IDENT_L(0x0b, 0x4242, -1, "smiapp-003"),
+	SMIAPP_IDENT_L(0x0c, 0x208a, -1, "tcm8330md"),
+	SMIAPP_IDENT_LQ(0x0c, 0x2134, -1, "tcm8500md", &smiapp_tcm8500md_quirk),
+	SMIAPP_IDENT_L(0x0c, 0x213e, -1, "et8en2"),
+	SMIAPP_IDENT_L(0x0c, 0x2184, -1, "tcm8580md"),
+	SMIAPP_IDENT_LQ(0x0c, 0x560f, -1, "jt8ew9", &smiapp_jt8ew9_quirk),
+	SMIAPP_IDENT_LQ(0x10, 0x4141, -1, "jt8ev1", &smiapp_jt8ev1_quirk),
+	SMIAPP_IDENT_LQ(0x10, 0x4241, -1, "imx125es", &smiapp_imx125es_quirk),
+};
+
+/*
+ *
+ * Dynamic Capability Identification
+ *
+ */
+
+static int smiapp_read_frame_fmt(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	u32 fmt_model_type, fmt_model_subtype, ncol_desc, nrow_desc;
+	unsigned int i;
+	int rval;
+	int line_count = 0;
+	int embedded_start = -1, embedded_end = -1;
+	int image_start = 0;
+
+	rval = smiapp_read(sensor, SMIAPP_REG_U8_FRAME_FORMAT_MODEL_TYPE,
+			   &fmt_model_type);
+	if (rval)
+		return rval;
+
+	rval = smiapp_read(sensor, SMIAPP_REG_U8_FRAME_FORMAT_MODEL_SUBTYPE,
+			   &fmt_model_subtype);
+	if (rval)
+		return rval;
+
+	ncol_desc = (fmt_model_subtype
+		     & SMIAPP_FRAME_FORMAT_MODEL_SUBTYPE_NCOLS_MASK)
+		>> SMIAPP_FRAME_FORMAT_MODEL_SUBTYPE_NCOLS_SHIFT;
+	nrow_desc = fmt_model_subtype
+		& SMIAPP_FRAME_FORMAT_MODEL_SUBTYPE_NROWS_MASK;
+
+	dev_dbg(&client->dev, "format_model_type %s\n",
+		fmt_model_type == SMIAPP_FRAME_FORMAT_MODEL_TYPE_2BYTE
+		? "2 byte" :
+		fmt_model_type == SMIAPP_FRAME_FORMAT_MODEL_TYPE_4BYTE
+		? "4 byte" : "is simply bad");
+
+	for (i = 0; i < ncol_desc + nrow_desc; i++) {
+		u32 desc;
+		u32 pixelcode;
+		u32 pixels;
+		char *which;
+		char *what;
+
+		if (fmt_model_type == SMIAPP_FRAME_FORMAT_MODEL_TYPE_2BYTE) {
+			rval = smiapp_read(
+				sensor,
+				SMIAPP_REG_U16_FRAME_FORMAT_DESCRIPTOR_2(i),
+				&desc);
+			if (rval)
+				return rval;
+
+			pixelcode =
+				(desc
+				 & SMIAPP_FRAME_FORMAT_DESC_2_PIXELCODE_MASK)
+				>> SMIAPP_FRAME_FORMAT_DESC_2_PIXELCODE_SHIFT;
+			pixels = desc & SMIAPP_FRAME_FORMAT_DESC_2_PIXELS_MASK;
+		} else if (fmt_model_type
+			   == SMIAPP_FRAME_FORMAT_MODEL_TYPE_4BYTE) {
+			rval = smiapp_read(
+				sensor,
+				SMIAPP_REG_U32_FRAME_FORMAT_DESCRIPTOR_4(i),
+				&desc);
+			if (rval)
+				return rval;
+
+			pixelcode =
+				(desc
+				 & SMIAPP_FRAME_FORMAT_DESC_4_PIXELCODE_MASK)
+				>> SMIAPP_FRAME_FORMAT_DESC_4_PIXELCODE_SHIFT;
+			pixels = desc & SMIAPP_FRAME_FORMAT_DESC_4_PIXELS_MASK;
+		} else {
+			dev_dbg(&client->dev,
+				"invalid frame format model type %d\n",
+				fmt_model_type);
+			return -EINVAL;
+		}
+
+		if (i < ncol_desc)
+			which = "columns";
+		else
+			which = "rows";
+
+		switch (pixelcode) {
+		case SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_EMBEDDED:
+			what = "embedded";
+			break;
+		case SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_DUMMY:
+			what = "dummy";
+			break;
+		case SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_BLACK:
+			what = "black";
+			break;
+		case SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_DARK:
+			what = "dark";
+			break;
+		case SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_VISIBLE:
+			what = "visible";
+			break;
+		default:
+			what = "invalid";
+			dev_dbg(&client->dev, "pixelcode %d\n", pixelcode);
+			break;
+		}
+
+		dev_dbg(&client->dev, "%s pixels: %d %s\n",
+			what, pixels, which);
+
+		if (i < ncol_desc)
+			continue;
+
+		/* Handle row descriptors */
+		if (pixelcode
+		    == SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_EMBEDDED) {
+			embedded_start = line_count;
+		} else {
+			if (pixelcode == SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_VISIBLE
+			    || pixels >= sensor->limits[SMIAPP_LIMIT_MIN_FRAME_LENGTH_LINES] / 2)
+				image_start = line_count;
+			if (embedded_start != -1 && embedded_end == -1)
+				embedded_end = line_count;
+		}
+		line_count += pixels;
+	}
+
+	if (embedded_start == -1 || embedded_end == -1) {
+		embedded_start = 0;
+		embedded_end = 0;
+	}
+
+	dev_dbg(&client->dev, "embedded data from lines %d to %d\n",
+		embedded_start, embedded_end);
+	dev_dbg(&client->dev, "image data starts at line %d\n", image_start);
+
+	return 0;
+}
+
+static int smiapp_pll_configure(struct smiapp_sensor *sensor)
+{
+	struct smiapp_pll *pll = &sensor->pll;
+	int rval;
+
+	rval = smiapp_write(
+		sensor, SMIAPP_REG_U16_VT_PIX_CLK_DIV, pll->vt_pix_clk_div);
+	if (rval < 0)
+		return rval;
+
+	rval = smiapp_write(
+		sensor, SMIAPP_REG_U16_VT_SYS_CLK_DIV, pll->vt_sys_clk_div);
+	if (rval < 0)
+		return rval;
+
+	rval = smiapp_write(
+		sensor, SMIAPP_REG_U16_PRE_PLL_CLK_DIV, pll->pre_pll_clk_div);
+	if (rval < 0)
+		return rval;
+
+	rval = smiapp_write(
+		sensor, SMIAPP_REG_U16_PLL_MULTIPLIER, pll->pll_multiplier);
+	if (rval < 0)
+		return rval;
+
+	/* Lane op clock ratio does not apply here. */
+	rval = smiapp_write(
+		sensor, SMIAPP_REG_U32_REQUESTED_LINK_BIT_RATE_MBPS,
+		DIV_ROUND_UP(pll->op_sys_clk_freq_hz, 1000000 / 256 / 256));
+	if (rval < 0 || sensor->minfo.smiapp_profile == SMIAPP_PROFILE_0)
+		return rval;
+
+	rval = smiapp_write(
+		sensor, SMIAPP_REG_U16_OP_PIX_CLK_DIV, pll->op_pix_clk_div);
+	if (rval < 0)
+		return rval;
+
+	return smiapp_write(
+		sensor, SMIAPP_REG_U16_OP_SYS_CLK_DIV, pll->op_sys_clk_div);
+}
+
+static int smiapp_pll_update(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	struct smiapp_pll_limits lim = {
+		.min_pre_pll_clk_div = sensor->limits[SMIAPP_LIMIT_MIN_PRE_PLL_CLK_DIV],
+		.max_pre_pll_clk_div = sensor->limits[SMIAPP_LIMIT_MAX_PRE_PLL_CLK_DIV],
+		.min_pll_ip_freq_hz = sensor->limits[SMIAPP_LIMIT_MIN_PLL_IP_FREQ_HZ],
+		.max_pll_ip_freq_hz = sensor->limits[SMIAPP_LIMIT_MAX_PLL_IP_FREQ_HZ],
+		.min_pll_multiplier = sensor->limits[SMIAPP_LIMIT_MIN_PLL_MULTIPLIER],
+		.max_pll_multiplier = sensor->limits[SMIAPP_LIMIT_MAX_PLL_MULTIPLIER],
+		.min_pll_op_freq_hz = sensor->limits[SMIAPP_LIMIT_MIN_PLL_OP_FREQ_HZ],
+		.max_pll_op_freq_hz = sensor->limits[SMIAPP_LIMIT_MAX_PLL_OP_FREQ_HZ],
+
+		.min_op_sys_clk_div = sensor->limits[SMIAPP_LIMIT_MIN_OP_SYS_CLK_DIV],
+		.max_op_sys_clk_div = sensor->limits[SMIAPP_LIMIT_MAX_OP_SYS_CLK_DIV],
+		.min_op_pix_clk_div = sensor->limits[SMIAPP_LIMIT_MIN_OP_PIX_CLK_DIV],
+		.max_op_pix_clk_div = sensor->limits[SMIAPP_LIMIT_MAX_OP_PIX_CLK_DIV],
+		.min_op_sys_clk_freq_hz = sensor->limits[SMIAPP_LIMIT_MIN_OP_SYS_CLK_FREQ_HZ],
+		.max_op_sys_clk_freq_hz = sensor->limits[SMIAPP_LIMIT_MAX_OP_SYS_CLK_FREQ_HZ],
+		.min_op_pix_clk_freq_hz = sensor->limits[SMIAPP_LIMIT_MIN_OP_PIX_CLK_FREQ_HZ],
+		.max_op_pix_clk_freq_hz = sensor->limits[SMIAPP_LIMIT_MAX_OP_PIX_CLK_FREQ_HZ],
+
+		.min_vt_sys_clk_div = sensor->limits[SMIAPP_LIMIT_MIN_VT_SYS_CLK_DIV],
+		.max_vt_sys_clk_div = sensor->limits[SMIAPP_LIMIT_MAX_VT_SYS_CLK_DIV],
+		.min_vt_pix_clk_div = sensor->limits[SMIAPP_LIMIT_MIN_VT_PIX_CLK_DIV],
+		.max_vt_pix_clk_div = sensor->limits[SMIAPP_LIMIT_MAX_VT_PIX_CLK_DIV],
+		.min_vt_sys_clk_freq_hz = sensor->limits[SMIAPP_LIMIT_MIN_VT_SYS_CLK_FREQ_HZ],
+		.max_vt_sys_clk_freq_hz = sensor->limits[SMIAPP_LIMIT_MAX_VT_SYS_CLK_FREQ_HZ],
+		.min_vt_pix_clk_freq_hz = sensor->limits[SMIAPP_LIMIT_MIN_VT_PIX_CLK_FREQ_HZ],
+		.max_vt_pix_clk_freq_hz = sensor->limits[SMIAPP_LIMIT_MAX_VT_PIX_CLK_FREQ_HZ],
+
+		.min_line_length_pck_bin = sensor->limits[SMIAPP_LIMIT_MIN_LINE_LENGTH_PCK_BIN],
+		.min_line_length_pck = sensor->limits[SMIAPP_LIMIT_MIN_LINE_LENGTH_PCK],
+	};
+	struct smiapp_pll *pll = &sensor->pll;
+	int rval;
+
+	memset(&sensor->pll, 0, sizeof(sensor->pll));
+
+	pll->lanes = sensor->platform_data->lanes;
+	pll->ext_clk_freq_hz = sensor->platform_data->ext_clk;
+
+	if (sensor->minfo.smiapp_profile == SMIAPP_PROFILE_0) {
+		/*
+		 * Fill in operational clock divisors limits from the
+		 * video timing ones. On profile 0 sensors the
+		 * requirements regarding them are essentially the
+		 * same as on VT ones.
+		 */
+		lim.min_op_sys_clk_div = lim.min_vt_sys_clk_div;
+		lim.max_op_sys_clk_div = lim.max_vt_sys_clk_div;
+		lim.min_op_pix_clk_div = lim.min_vt_pix_clk_div;
+		lim.max_op_pix_clk_div = lim.max_vt_pix_clk_div;
+		lim.min_op_sys_clk_freq_hz = lim.min_vt_sys_clk_freq_hz;
+		lim.max_op_sys_clk_freq_hz = lim.max_vt_sys_clk_freq_hz;
+		lim.min_op_pix_clk_freq_hz = lim.min_vt_pix_clk_freq_hz;
+		lim.max_op_pix_clk_freq_hz = lim.max_vt_pix_clk_freq_hz;
+		/* Profile 0 sensors have no separate OP clock branch. */
+		pll->flags |= SMIAPP_PLL_FLAG_NO_OP_CLOCKS;
+	}
+
+	if (smiapp_needs_quirk(sensor,
+			       SMIAPP_QUIRK_FLAG_OP_PIX_CLOCK_PER_LANE))
+		pll->flags |= SMIAPP_PLL_FLAG_OP_PIX_CLOCK_PER_LANE;
+
+	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->scale_n = sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN];
+	pll->bits_per_pixel = sensor->csi_format->compressed;
+
+	rval = smiapp_pll_calculate(&client->dev, &lim, pll);
+	if (rval < 0)
+		return rval;
+
+	sensor->pixel_rate_parray->cur.val64 = pll->vt_pix_clk_freq_hz;
+	sensor->pixel_rate_csi->cur.val64 = pll->pixel_rate_csi;
+
+	return 0;
+}
+
+
+/*
+ *
+ * V4L2 Controls handling
+ *
+ */
+
+static void __smiapp_update_exposure_limits(struct smiapp_sensor *sensor)
+{
+	struct v4l2_ctrl *ctrl = sensor->exposure;
+	int max;
+
+	max = sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].height
+		+ sensor->vblank->val
+		- sensor->limits[SMIAPP_LIMIT_COARSE_INTEGRATION_TIME_MAX_MARGIN];
+
+	ctrl->maximum = max;
+	if (ctrl->default_value > max)
+		ctrl->default_value = max;
+	if (ctrl->val > max)
+		ctrl->val = max;
+	if (ctrl->cur.val > max)
+		ctrl->cur.val = 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 smiapp_csi_data_format smiapp_csi_data_formats[] = {
+	{ V4L2_MBUS_FMT_SGRBG12_1X12, 12, 12, SMIAPP_PIXEL_ORDER_GRBG, },
+	{ V4L2_MBUS_FMT_SRGGB12_1X12, 12, 12, SMIAPP_PIXEL_ORDER_RGGB, },
+	{ V4L2_MBUS_FMT_SBGGR12_1X12, 12, 12, SMIAPP_PIXEL_ORDER_BGGR, },
+	{ V4L2_MBUS_FMT_SGBRG12_1X12, 12, 12, SMIAPP_PIXEL_ORDER_GBRG, },
+	{ V4L2_MBUS_FMT_SGRBG10_1X10, 10, 10, SMIAPP_PIXEL_ORDER_GRBG, },
+	{ V4L2_MBUS_FMT_SRGGB10_1X10, 10, 10, SMIAPP_PIXEL_ORDER_RGGB, },
+	{ V4L2_MBUS_FMT_SBGGR10_1X10, 10, 10, SMIAPP_PIXEL_ORDER_BGGR, },
+	{ V4L2_MBUS_FMT_SGBRG10_1X10, 10, 10, SMIAPP_PIXEL_ORDER_GBRG, },
+	{ V4L2_MBUS_FMT_SGRBG10_DPCM8_1X8, 10, 8, SMIAPP_PIXEL_ORDER_GRBG, },
+	{ V4L2_MBUS_FMT_SRGGB10_DPCM8_1X8, 10, 8, SMIAPP_PIXEL_ORDER_RGGB, },
+	{ V4L2_MBUS_FMT_SBGGR10_DPCM8_1X8, 10, 8, SMIAPP_PIXEL_ORDER_BGGR, },
+	{ V4L2_MBUS_FMT_SGBRG10_DPCM8_1X8, 10, 8, SMIAPP_PIXEL_ORDER_GBRG, },
+	{ V4L2_MBUS_FMT_SGRBG8_1X8, 8, 8, SMIAPP_PIXEL_ORDER_GRBG, },
+	{ V4L2_MBUS_FMT_SRGGB8_1X8, 8, 8, SMIAPP_PIXEL_ORDER_RGGB, },
+	{ V4L2_MBUS_FMT_SBGGR8_1X8, 8, 8, SMIAPP_PIXEL_ORDER_BGGR, },
+	{ V4L2_MBUS_FMT_SGBRG8_1X8, 8, 8, SMIAPP_PIXEL_ORDER_GBRG, },
+};
+
+const char *pixel_order_str[] = { "GRBG", "RGGB", "BGGR", "GBRG" };
+
+#define to_csi_format_idx(fmt) (((unsigned long)(fmt)			\
+				 - (unsigned long)smiapp_csi_data_formats) \
+				/ sizeof(*smiapp_csi_data_formats))
+
+static u32 smiapp_pixel_order(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	int flip = 0;
+
+	if (sensor->hflip) {
+		if (sensor->hflip->val)
+			flip |= SMIAPP_IMAGE_ORIENTATION_HFLIP;
+
+		if (sensor->vflip->val)
+			flip |= SMIAPP_IMAGE_ORIENTATION_VFLIP;
+	}
+
+	flip ^= sensor->hvflip_inv_mask;
+
+	dev_dbg(&client->dev, "flip %d\n", flip);
+	return sensor->default_pixel_order ^ flip;
+}
+
+static void smiapp_update_mbus_formats(struct smiapp_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 = smiapp_pixel_order(sensor);
+
+	sensor->mbus_frame_fmts =
+		sensor->default_mbus_frame_fmts << pixel_order;
+	sensor->csi_format =
+		&smiapp_csi_data_formats[csi_format_idx + pixel_order];
+	sensor->internal_csi_format =
+		&smiapp_csi_data_formats[internal_csi_format_idx
+					 + pixel_order];
+
+	BUG_ON(max(internal_csi_format_idx, csi_format_idx) + pixel_order
+	       >= ARRAY_SIZE(smiapp_csi_data_formats));
+	BUG_ON(min(internal_csi_format_idx, csi_format_idx) < 0);
+
+	dev_dbg(&client->dev, "new pixel order %s\n",
+		pixel_order_str[pixel_order]);
+}
+
+static int smiapp_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct smiapp_sensor *sensor =
+		container_of(ctrl->handler, struct smiapp_subdev, ctrl_handler)
+			->sensor;
+	u32 orient = 0;
+	int exposure;
+	int rval;
+
+	switch (ctrl->id) {
+	case V4L2_CID_ANALOGUE_GAIN:
+		return smiapp_write(
+			sensor,
+			SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_GLOBAL, ctrl->val);
+
+	case V4L2_CID_EXPOSURE:
+		return smiapp_write(
+			sensor,
+			SMIAPP_REG_U16_COARSE_INTEGRATION_TIME, ctrl->val);
+
+	case V4L2_CID_HFLIP:
+	case V4L2_CID_VFLIP:
+		if (sensor->streaming)
+			return -EBUSY;
+
+		if (sensor->hflip->val)
+			orient |= SMIAPP_IMAGE_ORIENTATION_HFLIP;
+
+		if (sensor->vflip->val)
+			orient |= SMIAPP_IMAGE_ORIENTATION_VFLIP;
+
+		orient ^= sensor->hvflip_inv_mask;
+		rval = smiapp_write(sensor,
+				    SMIAPP_REG_U8_IMAGE_ORIENTATION,
+				    orient);
+		if (rval < 0)
+			return rval;
+
+		smiapp_update_mbus_formats(sensor);
+
+		return 0;
+
+	case V4L2_CID_VBLANK:
+		exposure = sensor->exposure->val;
+
+		__smiapp_update_exposure_limits(sensor);
+
+		if (exposure > sensor->exposure->maximum) {
+			sensor->exposure->val =
+				sensor->exposure->maximum;
+			rval = smiapp_set_ctrl(
+				sensor->exposure);
+			if (rval < 0)
+				return rval;
+		}
+
+		return smiapp_write(
+			sensor, SMIAPP_REG_U16_FRAME_LENGTH_LINES,
+			sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].height
+			+ ctrl->val);
+
+	case V4L2_CID_HBLANK:
+		return smiapp_write(
+			sensor, SMIAPP_REG_U16_LINE_LENGTH_PCK,
+			sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].width
+			+ ctrl->val);
+
+	case V4L2_CID_LINK_FREQ:
+		if (sensor->streaming)
+			return -EBUSY;
+
+		return smiapp_pll_update(sensor);
+
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct v4l2_ctrl_ops smiapp_ctrl_ops = {
+	.s_ctrl = smiapp_set_ctrl,
+};
+
+static int smiapp_init_controls(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	unsigned int max;
+	int rval;
+
+	rval = v4l2_ctrl_handler_init(&sensor->pixel_array->ctrl_handler, 7);
+	if (rval)
+		return rval;
+	sensor->pixel_array->ctrl_handler.lock = &sensor->mutex;
+
+	sensor->analog_gain = v4l2_ctrl_new_std(
+		&sensor->pixel_array->ctrl_handler, &smiapp_ctrl_ops,
+		V4L2_CID_ANALOGUE_GAIN,
+		sensor->limits[SMIAPP_LIMIT_ANALOGUE_GAIN_CODE_MIN],
+		sensor->limits[SMIAPP_LIMIT_ANALOGUE_GAIN_CODE_MAX],
+		max(sensor->limits[SMIAPP_LIMIT_ANALOGUE_GAIN_CODE_STEP], 1U),
+		sensor->limits[SMIAPP_LIMIT_ANALOGUE_GAIN_CODE_MIN]);
+
+	/* Exposure limits will be updated soon, use just something here. */
+	sensor->exposure = v4l2_ctrl_new_std(
+		&sensor->pixel_array->ctrl_handler, &smiapp_ctrl_ops,
+		V4L2_CID_EXPOSURE, 0, 0, 1, 0);
+
+	sensor->hflip = v4l2_ctrl_new_std(
+		&sensor->pixel_array->ctrl_handler, &smiapp_ctrl_ops,
+		V4L2_CID_HFLIP, 0, 1, 1, 0);
+	sensor->vflip = v4l2_ctrl_new_std(
+		&sensor->pixel_array->ctrl_handler, &smiapp_ctrl_ops,
+		V4L2_CID_VFLIP, 0, 1, 1, 0);
+
+	sensor->vblank = v4l2_ctrl_new_std(
+		&sensor->pixel_array->ctrl_handler, &smiapp_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, &smiapp_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, &smiapp_ctrl_ops,
+		V4L2_CID_PIXEL_RATE, 0, 0, 1, 0);
+
+	if (sensor->pixel_array->ctrl_handler.error) {
+		dev_err(&client->dev,
+			"pixel array controls initialization failed (%d)\n",
+			sensor->pixel_array->ctrl_handler.error);
+		rval = sensor->pixel_array->ctrl_handler.error;
+		goto 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)
+		goto error;
+	sensor->src->ctrl_handler.lock = &sensor->mutex;
+
+	for (max = 0; sensor->platform_data->op_sys_clock[max + 1]; max++);
+
+	sensor->link_freq = v4l2_ctrl_new_int_menu(
+		&sensor->src->ctrl_handler, &smiapp_ctrl_ops,
+		V4L2_CID_LINK_FREQ, max, 0,
+		sensor->platform_data->op_sys_clock);
+
+	sensor->pixel_rate_csi = v4l2_ctrl_new_std(
+		&sensor->src->ctrl_handler, &smiapp_ctrl_ops,
+		V4L2_CID_PIXEL_RATE, 0, 0, 1, 0);
+
+	if (sensor->src->ctrl_handler.error) {
+		dev_err(&client->dev,
+			"src controls initialization failed (%d)\n",
+			sensor->src->ctrl_handler.error);
+		rval = sensor->src->ctrl_handler.error;
+		goto error;
+	}
+
+	sensor->src->sd.ctrl_handler =
+		&sensor->src->ctrl_handler;
+
+	return 0;
+
+error:
+	v4l2_ctrl_handler_free(&sensor->pixel_array->ctrl_handler);
+	v4l2_ctrl_handler_free(&sensor->src->ctrl_handler);
+
+	return rval;
+}
+
+static void smiapp_free_controls(struct smiapp_sensor *sensor)
+{
+	unsigned int i;
+
+	for (i = 0; i < sensor->ssds_used; i++)
+		v4l2_ctrl_handler_free(&sensor->ssds[i].ctrl_handler);
+}
+
+static int smiapp_get_limits(struct smiapp_sensor *sensor, int const *limit,
+			     unsigned int n)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	unsigned int i;
+	u32 val;
+	int rval;
+
+	for (i = 0; i < n; i++) {
+		rval = smiapp_read(
+			sensor, smiapp_reg_limits[limit[i]].addr, &val);
+		if (rval)
+			return rval;
+		sensor->limits[limit[i]] = val;
+		dev_dbg(&client->dev, "0x%8.8x \"%s\" = %d, 0x%x\n",
+			smiapp_reg_limits[limit[i]].addr,
+			smiapp_reg_limits[limit[i]].what, val, val);
+	}
+
+	return 0;
+}
+
+static int smiapp_get_all_limits(struct smiapp_sensor *sensor)
+{
+	unsigned int i;
+	int rval;
+
+	for (i = 0; i < SMIAPP_LIMIT_LAST; i++) {
+		rval = smiapp_get_limits(sensor, &i, 1);
+		if (rval < 0)
+			return rval;
+	}
+
+	if (sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN] == 0)
+		smiapp_replace_limit(sensor, SMIAPP_LIMIT_SCALER_N_MIN, 16);
+
+	return 0;
+}
+
+static int smiapp_get_limits_binning(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	static u32 const limits[] = {
+		SMIAPP_LIMIT_MIN_FRAME_LENGTH_LINES_BIN,
+		SMIAPP_LIMIT_MAX_FRAME_LENGTH_LINES_BIN,
+		SMIAPP_LIMIT_MIN_LINE_LENGTH_PCK_BIN,
+		SMIAPP_LIMIT_MAX_LINE_LENGTH_PCK_BIN,
+		SMIAPP_LIMIT_MIN_LINE_BLANKING_PCK_BIN,
+		SMIAPP_LIMIT_FINE_INTEGRATION_TIME_MIN_BIN,
+		SMIAPP_LIMIT_FINE_INTEGRATION_TIME_MAX_MARGIN_BIN,
+	};
+	static u32 const limits_replace[] = {
+		SMIAPP_LIMIT_MIN_FRAME_LENGTH_LINES,
+		SMIAPP_LIMIT_MAX_FRAME_LENGTH_LINES,
+		SMIAPP_LIMIT_MIN_LINE_LENGTH_PCK,
+		SMIAPP_LIMIT_MAX_LINE_LENGTH_PCK,
+		SMIAPP_LIMIT_MIN_LINE_BLANKING_PCK,
+		SMIAPP_LIMIT_FINE_INTEGRATION_TIME_MIN,
+		SMIAPP_LIMIT_FINE_INTEGRATION_TIME_MAX_MARGIN,
+	};
+	unsigned int i;
+	int rval;
+
+	if (sensor->limits[SMIAPP_LIMIT_BINNING_CAPABILITY] ==
+	    SMIAPP_BINNING_CAPABILITY_NO) {
+		for (i = 0; i < ARRAY_SIZE(limits); i++)
+			sensor->limits[limits[i]] =
+				sensor->limits[limits_replace[i]];
+
+		return 0;
+	}
+
+	rval = smiapp_get_limits(sensor, limits, ARRAY_SIZE(limits));
+	if (rval < 0)
+		return rval;
+
+	/*
+	 * Sanity check whether the binning limits are valid. If not,
+	 * use the non-binning ones.
+	 */
+	if (sensor->limits[SMIAPP_LIMIT_MIN_FRAME_LENGTH_LINES_BIN]
+	    && sensor->limits[SMIAPP_LIMIT_MIN_LINE_LENGTH_PCK_BIN]
+	    && sensor->limits[SMIAPP_LIMIT_MIN_LINE_BLANKING_PCK_BIN])
+		return 0;
+
+	for (i = 0; i < ARRAY_SIZE(limits); i++) {
+		dev_dbg(&client->dev,
+			"replace limit 0x%8.8x \"%s\" = %d, 0x%x\n",
+			smiapp_reg_limits[limits[i]].addr,
+			smiapp_reg_limits[limits[i]].what,
+			sensor->limits[limits_replace[i]],
+			sensor->limits[limits_replace[i]]);
+		sensor->limits[limits[i]] =
+			sensor->limits[limits_replace[i]];
+	}
+
+	return 0;
+}
+
+static int smiapp_get_mbus_formats(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	unsigned int type, n;
+	unsigned int i, pixel_order;
+	int rval;
+
+	rval = smiapp_read(
+		sensor, SMIAPP_REG_U8_DATA_FORMAT_MODEL_TYPE, &type);
+	if (rval)
+		return rval;
+
+	dev_dbg(&client->dev, "data_format_model_type %d\n", type);
+
+	rval = smiapp_read(sensor, SMIAPP_REG_U8_PIXEL_ORDER,
+			   &pixel_order);
+	if (rval)
+		return rval;
+
+	if (pixel_order >= ARRAY_SIZE(pixel_order_str)) {
+		dev_dbg(&client->dev, "bad pixel order %d\n", pixel_order);
+		return -EINVAL;
+	}
+
+	dev_dbg(&client->dev, "pixel order %d (%s)\n", pixel_order,
+		pixel_order_str[pixel_order]);
+
+	switch (type) {
+	case SMIAPP_DATA_FORMAT_MODEL_TYPE_NORMAL:
+		n = SMIAPP_DATA_FORMAT_MODEL_TYPE_NORMAL_N;
+		break;
+	case SMIAPP_DATA_FORMAT_MODEL_TYPE_EXTENDED:
+		n = SMIAPP_DATA_FORMAT_MODEL_TYPE_EXTENDED_N;
+		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;
+
+		rval = smiapp_read(
+			sensor,
+			SMIAPP_REG_U16_DATA_FORMAT_DESCRIPTOR(i), &fmt);
+		if (rval)
+			return rval;
+
+		dev_dbg(&client->dev, "bpp %d, compressed %d\n",
+			fmt >> 8, (u8)fmt);
+
+		for (j = 0; j < ARRAY_SIZE(smiapp_csi_data_formats); j++) {
+			const struct smiapp_csi_data_format *f =
+				&smiapp_csi_data_formats[j];
+
+			if (f->pixel_order != SMIAPP_PIXEL_ORDER_GRBG)
+				continue;
+
+			if (f->width != fmt >> 8 || f->compressed != (u8)fmt)
+				continue;
+
+			dev_dbg(&client->dev, "jolly good! %d\n", j);
+
+			sensor->default_mbus_frame_fmts |= 1 << j;
+			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;
+	}
+
+	smiapp_update_mbus_formats(sensor);
+
+	return 0;
+}
+
+static void smiapp_update_blanking(struct smiapp_sensor *sensor)
+{
+	struct v4l2_ctrl *vblank = sensor->vblank;
+	struct v4l2_ctrl *hblank = sensor->hblank;
+
+	vblank->minimum =
+		max_t(int,
+		      sensor->limits[SMIAPP_LIMIT_MIN_FRAME_BLANKING_LINES],
+		      sensor->limits[SMIAPP_LIMIT_MIN_FRAME_LENGTH_LINES_BIN] -
+		      sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].height);
+	vblank->maximum =
+		sensor->limits[SMIAPP_LIMIT_MAX_FRAME_LENGTH_LINES_BIN] -
+		sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].height;
+
+	vblank->val = clamp_t(int, vblank->val,
+			      vblank->minimum, vblank->maximum);
+	vblank->default_value = vblank->minimum;
+	vblank->val = vblank->val;
+	vblank->cur.val = vblank->val;
+
+	hblank->minimum =
+		max_t(int,
+		      sensor->limits[SMIAPP_LIMIT_MIN_LINE_LENGTH_PCK_BIN] -
+		      sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].width,
+		      sensor->limits[SMIAPP_LIMIT_MIN_LINE_BLANKING_PCK_BIN]);
+	hblank->maximum =
+		sensor->limits[SMIAPP_LIMIT_MAX_LINE_LENGTH_PCK_BIN] -
+		sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].width;
+
+	hblank->val = clamp_t(int, hblank->val,
+			      hblank->minimum, hblank->maximum);
+	hblank->default_value = hblank->minimum;
+	hblank->val = hblank->val;
+	hblank->cur.val = hblank->val;
+
+	__smiapp_update_exposure_limits(sensor);
+}
+
+static int smiapp_update_mode(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	unsigned int binning_mode;
+	int rval;
+
+	dev_dbg(&client->dev, "frame size: %dx%d\n",
+		sensor->src->crop[SMIAPP_PAD_SRC].width,
+		sensor->src->crop[SMIAPP_PAD_SRC].height);
+	dev_dbg(&client->dev, "csi format width: %d\n",
+		sensor->csi_format->width);
+
+	/* Binning has to be set up here; it affects limits */
+	if (sensor->binning_horizontal == 1 &&
+	    sensor->binning_vertical == 1) {
+		binning_mode = 0;
+	} else {
+		u8 binning_type =
+			(sensor->binning_horizontal << 4)
+			| sensor->binning_vertical;
+
+		rval = smiapp_write(
+			sensor, SMIAPP_REG_U8_BINNING_TYPE, binning_type);
+		if (rval < 0)
+			return rval;
+
+		binning_mode = 1;
+	}
+	rval = smiapp_write(sensor, SMIAPP_REG_U8_BINNING_MODE, binning_mode);
+	if (rval < 0)
+		return rval;
+
+	/* Get updated limits due to binning */
+	rval = smiapp_get_limits_binning(sensor);
+	if (rval < 0)
+		return rval;
+
+	rval = smiapp_pll_update(sensor);
+	if (rval < 0)
+		return rval;
+
+	/* Output from pixel array, including blanking */
+	smiapp_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.vt_pix_clk_freq_hz /
+		((sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].width
+		  + sensor->hblank->val) *
+		 (sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].height
+		  + sensor->vblank->val) / 100));
+
+	return 0;
+}
+
+/*
+ *
+ * SMIA++ NVM handling
+ *
+ */
+static int smiapp_read_nvm(struct smiapp_sensor *sensor,
+			   unsigned char *nvm)
+{
+	u32 i, s, p, np, v;
+	int rval = 0, rval2;
+
+	np = sensor->nvm_size / SMIAPP_NVM_PAGE_SIZE;
+	for (p = 0; p < np; p++) {
+		rval = smiapp_write(
+			sensor,
+			SMIAPP_REG_U8_DATA_TRANSFER_IF_1_PAGE_SELECT, p);
+		if (rval)
+			goto out;
+
+		rval = smiapp_write(sensor,
+				    SMIAPP_REG_U8_DATA_TRANSFER_IF_1_CTRL,
+				    SMIAPP_DATA_TRANSFER_IF_1_CTRL_EN |
+				    SMIAPP_DATA_TRANSFER_IF_1_CTRL_RD_EN);
+		if (rval)
+			goto out;
+
+		for (i = 0; i < 1000; i++) {
+			rval = smiapp_read(
+				sensor,
+				SMIAPP_REG_U8_DATA_TRANSFER_IF_1_STATUS, &s);
+
+			if (rval)
+				goto out;
+
+			if (s & SMIAPP_DATA_TRANSFER_IF_1_STATUS_RD_READY)
+				break;
+
+			if (--i == 0) {
+				rval = -ETIMEDOUT;
+				goto out;
+			}
+
+		}
+
+		for (i = 0; i < SMIAPP_NVM_PAGE_SIZE; i++) {
+			rval = smiapp_read(
+				sensor,
+				SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_0 + i,
+				&v);
+			if (rval)
+				goto out;
+
+			*nvm++ = v;
+		}
+	}
+
+out:
+	rval2 = smiapp_write(sensor, SMIAPP_REG_U8_DATA_TRANSFER_IF_1_CTRL, 0);
+	if (rval < 0)
+		return rval;
+	else
+		return rval2;
+}
+
+/*
+ *
+ * SMIA++ CCI address control
+ *
+ */
+static int smiapp_change_cci_addr(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	int rval;
+	u32 val;
+
+	client->addr = sensor->platform_data->i2c_addr_dfl;
+
+	rval = smiapp_write(sensor,
+			    SMIAPP_REG_U8_CCI_ADDRESS_CONTROL,
+			    sensor->platform_data->i2c_addr_alt << 1);
+	if (rval)
+		return rval;
+
+	client->addr = sensor->platform_data->i2c_addr_alt;
+
+	/* verify addr change went ok */
+	rval = smiapp_read(sensor, SMIAPP_REG_U8_CCI_ADDRESS_CONTROL, &val);
+	if (rval)
+		return rval;
+
+	if (val != sensor->platform_data->i2c_addr_alt << 1)
+		return -ENODEV;
+
+	return 0;
+}
+
+/*
+ *
+ * SMIA++ Mode Control
+ *
+ */
+static int smiapp_setup_flash_strobe(struct smiapp_sensor *sensor)
+{
+	struct smiapp_flash_strobe_parms *strobe_setup;
+	unsigned int ext_freq = sensor->platform_data->ext_clk;
+	u32 tmp;
+	u32 strobe_adjustment;
+	u32 strobe_width_high_rs;
+	int rval;
+
+	strobe_setup = sensor->platform_data->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@maxwell.research.nokia.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 = smiapp_write(sensor, SMIAPP_REG_U8_FLASH_MODE_RS,
+			    strobe_setup->mode);
+	if (rval < 0)
+		goto out;
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U8_FLASH_STROBE_ADJUSTMENT,
+			    strobe_adjustment);
+	if (rval < 0)
+		goto out;
+
+	rval = smiapp_write(
+		sensor, SMIAPP_REG_U16_TFLASH_STROBE_WIDTH_HIGH_RS_CTRL,
+		strobe_width_high_rs);
+	if (rval < 0)
+		goto out;
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U16_TFLASH_STROBE_DELAY_RS_CTRL,
+			    strobe_setup->strobe_delay);
+	if (rval < 0)
+		goto out;
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U16_FLASH_STROBE_START_POINT,
+			    strobe_setup->stobe_start_point);
+	if (rval < 0)
+		goto out;
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U8_FLASH_TRIGGER_RS,
+			    strobe_setup->trigger);
+
+out:
+	sensor->platform_data->strobe_setup->trigger = 0;
+
+	return rval;
+}
+
+/* -----------------------------------------------------------------------------
+ * Power management
+ */
+
+static int smiapp_power_on(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	unsigned int sleep;
+	int rval;
+
+	rval = regulator_enable(sensor->vana);
+	if (rval) {
+		dev_err(&client->dev, "failed to enable vana regulator\n");
+		return rval;
+	}
+	usleep_range(1000, 1000);
+
+	if (sensor->platform_data->set_xclk)
+		rval = sensor->platform_data->set_xclk(
+			&sensor->src->sd, sensor->platform_data->ext_clk);
+	else
+		rval = clk_enable(sensor->ext_clk);
+	if (rval < 0) {
+		dev_dbg(&client->dev, "failed to set xclk\n");
+		goto out_xclk_fail;
+	}
+	usleep_range(1000, 1000);
+
+	if (sensor->platform_data->xshutdown != SMIAPP_NO_XSHUTDOWN)
+		gpio_set_value(sensor->platform_data->xshutdown, 1);
+
+	sleep = SMIAPP_RESET_DELAY(sensor->platform_data->ext_clk);
+	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->platform_data->i2c_addr_alt) {
+		rval = smiapp_change_cci_addr(sensor);
+		if (rval) {
+			dev_err(&client->dev, "cci address change error\n");
+			goto out_cci_addr_fail;
+		}
+	}
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U8_SOFTWARE_RESET,
+			    SMIAPP_SOFTWARE_RESET);
+	if (rval < 0) {
+		dev_err(&client->dev, "software reset failed\n");
+		goto out_cci_addr_fail;
+	}
+
+	if (sensor->platform_data->i2c_addr_alt) {
+		rval = smiapp_change_cci_addr(sensor);
+		if (rval) {
+			dev_err(&client->dev, "cci address change error\n");
+			goto out_cci_addr_fail;
+		}
+	}
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U16_COMPRESSION_MODE,
+			    SMIAPP_COMPRESSION_MODE_SIMPLE_PREDICTOR);
+	if (rval) {
+		dev_err(&client->dev, "compression mode set failed\n");
+		goto out_cci_addr_fail;
+	}
+
+	rval = smiapp_write(
+		sensor, SMIAPP_REG_U16_EXTCLK_FREQUENCY_MHZ,
+		sensor->platform_data->ext_clk / (1000000 / (1 << 8)));
+	if (rval) {
+		dev_err(&client->dev, "extclk frequency set failed\n");
+		goto out_cci_addr_fail;
+	}
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U8_CSI_LANE_MODE,
+			    sensor->platform_data->lanes - 1);
+	if (rval) {
+		dev_err(&client->dev, "csi lane mode set failed\n");
+		goto out_cci_addr_fail;
+	}
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U8_FAST_STANDBY_CTRL,
+			    SMIAPP_FAST_STANDBY_CTRL_IMMEDIATE);
+	if (rval) {
+		dev_err(&client->dev, "fast standby set failed\n");
+		goto out_cci_addr_fail;
+	}
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U8_CSI_SIGNALLING_MODE,
+			    sensor->platform_data->csi_signalling_mode);
+	if (rval) {
+		dev_err(&client->dev, "csi signalling mode set failed\n");
+		goto out_cci_addr_fail;
+	}
+
+	/* DPHY control done by sensor based on requested link rate */
+	rval = smiapp_write(sensor, SMIAPP_REG_U8_DPHY_CTRL,
+			    SMIAPP_DPHY_CTRL_UI);
+	if (rval < 0)
+		return rval;
+
+	rval = smiapp_call_quirk(sensor, post_poweron);
+	if (rval) {
+		dev_err(&client->dev, "post_poweron quirks failed\n");
+		goto out_cci_addr_fail;
+	}
+
+	/* Are we still initialising...? If yes, return here. */
+	if (!sensor->pixel_array)
+		return 0;
+
+	rval = v4l2_ctrl_handler_setup(
+		&sensor->pixel_array->ctrl_handler);
+	if (rval)
+		goto out_cci_addr_fail;
+
+	rval = v4l2_ctrl_handler_setup(&sensor->src->ctrl_handler);
+	if (rval)
+		goto out_cci_addr_fail;
+
+	mutex_lock(&sensor->mutex);
+	rval = smiapp_update_mode(sensor);
+	mutex_unlock(&sensor->mutex);
+	if (rval < 0)
+		goto out_cci_addr_fail;
+
+	return 0;
+
+out_cci_addr_fail:
+	if (sensor->platform_data->xshutdown != SMIAPP_NO_XSHUTDOWN)
+		gpio_set_value(sensor->platform_data->xshutdown, 0);
+	if (sensor->platform_data->set_xclk)
+		sensor->platform_data->set_xclk(&sensor->src->sd, 0);
+	else
+		clk_disable(sensor->ext_clk);
+
+out_xclk_fail:
+	regulator_disable(sensor->vana);
+	return rval;
+}
+
+static void smiapp_power_off(struct smiapp_sensor *sensor)
+{
+	/*
+	 * 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->platform_data->i2c_addr_alt)
+		smiapp_write(sensor,
+			     SMIAPP_REG_U8_SOFTWARE_RESET,
+			     SMIAPP_SOFTWARE_RESET);
+
+	if (sensor->platform_data->xshutdown != SMIAPP_NO_XSHUTDOWN)
+		gpio_set_value(sensor->platform_data->xshutdown, 0);
+	if (sensor->platform_data->set_xclk)
+		sensor->platform_data->set_xclk(&sensor->src->sd, 0);
+	else
+		clk_disable(sensor->ext_clk);
+	usleep_range(5000, 5000);
+	regulator_disable(sensor->vana);
+	sensor->streaming = 0;
+}
+
+static int smiapp_set_power(struct v4l2_subdev *subdev, int on)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	int ret = 0;
+
+	mutex_lock(&sensor->power_mutex);
+
+	/*
+	 * If the power count is modified from 0 to != 0 or from != 0
+	 * to 0, update the power state.
+	 */
+	if (!sensor->power_count == !on)
+		goto out;
+
+	if (on) {
+		/* Power on and perform initialisation. */
+		ret = smiapp_power_on(sensor);
+		if (ret < 0)
+			goto out;
+	} else {
+		smiapp_power_off(sensor);
+	}
+
+	/* Update the power count. */
+	sensor->power_count += on ? 1 : -1;
+	WARN_ON(sensor->power_count < 0);
+
+out:
+	mutex_unlock(&sensor->power_mutex);
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * Video stream management
+ */
+
+static int smiapp_start_streaming(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	int rval;
+
+	mutex_lock(&sensor->mutex);
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U16_CSI_DATA_FORMAT,
+			    (sensor->csi_format->width << 8) |
+			    sensor->csi_format->compressed);
+	if (rval)
+		goto out;
+
+	rval = smiapp_pll_configure(sensor);
+	if (rval)
+		goto out;
+
+	/* Analog crop start coordinates */
+	rval = smiapp_write(sensor, SMIAPP_REG_U16_X_ADDR_START,
+			    sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].left);
+	if (rval < 0)
+		goto out;
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U16_Y_ADDR_START,
+			    sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].top);
+	if (rval < 0)
+		goto out;
+
+	/* Analog crop end coordinates */
+	rval = smiapp_write(
+		sensor, SMIAPP_REG_U16_X_ADDR_END,
+		sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].left
+		+ sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].width - 1);
+	if (rval < 0)
+		goto out;
+
+	rval = smiapp_write(
+		sensor, SMIAPP_REG_U16_Y_ADDR_END,
+		sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].top
+		+ sensor->pixel_array->crop[SMIAPP_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 (sensor->limits[SMIAPP_LIMIT_DIGITAL_CROP_CAPABILITY]
+	    == SMIAPP_DIGITAL_CROP_CAPABILITY_INPUT_CROP) {
+		rval = smiapp_write(
+			sensor, SMIAPP_REG_U16_DIGITAL_CROP_X_OFFSET,
+			sensor->scaler->crop[SMIAPP_PAD_SINK].left);
+		if (rval < 0)
+			goto out;
+
+		rval = smiapp_write(
+			sensor, SMIAPP_REG_U16_DIGITAL_CROP_Y_OFFSET,
+			sensor->scaler->crop[SMIAPP_PAD_SINK].top);
+		if (rval < 0)
+			goto out;
+
+		rval = smiapp_write(
+			sensor, SMIAPP_REG_U16_DIGITAL_CROP_IMAGE_WIDTH,
+			sensor->scaler->crop[SMIAPP_PAD_SINK].width);
+		if (rval < 0)
+			goto out;
+
+		rval = smiapp_write(
+			sensor, SMIAPP_REG_U16_DIGITAL_CROP_IMAGE_HEIGHT,
+			sensor->scaler->crop[SMIAPP_PAD_SINK].height);
+		if (rval < 0)
+			goto out;
+	}
+
+	/* Scaling */
+	if (sensor->limits[SMIAPP_LIMIT_SCALING_CAPABILITY]
+	    != SMIAPP_SCALING_CAPABILITY_NONE) {
+		rval = smiapp_write(sensor, SMIAPP_REG_U16_SCALING_MODE,
+				    sensor->scaling_mode);
+		if (rval < 0)
+			goto out;
+
+		rval = smiapp_write(sensor, SMIAPP_REG_U16_SCALE_M,
+				    sensor->scale_m);
+		if (rval < 0)
+			goto out;
+	}
+
+	/* Output size from sensor */
+	rval = smiapp_write(sensor, SMIAPP_REG_U16_X_OUTPUT_SIZE,
+			    sensor->src->crop[SMIAPP_PAD_SRC].width);
+	if (rval < 0)
+		goto out;
+	rval = smiapp_write(sensor, SMIAPP_REG_U16_Y_OUTPUT_SIZE,
+			    sensor->src->crop[SMIAPP_PAD_SRC].height);
+	if (rval < 0)
+		goto out;
+
+	if ((sensor->flash_capability &
+	     (SMIAPP_FLASH_MODE_CAPABILITY_SINGLE_STROBE |
+	      SMIAPP_FLASH_MODE_CAPABILITY_MULTIPLE_STROBE)) &&
+	    sensor->platform_data->strobe_setup != NULL &&
+	    sensor->platform_data->strobe_setup->trigger != 0) {
+		rval = smiapp_setup_flash_strobe(sensor);
+		if (rval)
+			goto out;
+	}
+
+	rval = smiapp_call_quirk(sensor, pre_streamon);
+	if (rval) {
+		dev_err(&client->dev, "pre_streamon quirks failed\n");
+		goto out;
+	}
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U8_MODE_SELECT,
+			    SMIAPP_MODE_SELECT_STREAMING);
+
+out:
+	mutex_unlock(&sensor->mutex);
+
+	return rval;
+}
+
+static int smiapp_stop_streaming(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	int rval;
+
+	mutex_lock(&sensor->mutex);
+	rval = smiapp_write(sensor, SMIAPP_REG_U8_MODE_SELECT,
+			    SMIAPP_MODE_SELECT_SOFTWARE_STANDBY);
+	if (rval)
+		goto out;
+
+	rval = smiapp_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 smiapp_set_stream(struct v4l2_subdev *subdev, int enable)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	int rval;
+
+	if (sensor->streaming == enable)
+		return 0;
+
+	if (enable) {
+		sensor->streaming = 1;
+		rval = smiapp_start_streaming(sensor);
+		if (rval < 0)
+			sensor->streaming = 0;
+	} else {
+		rval = smiapp_stop_streaming(sensor);
+		sensor->streaming = 0;
+	}
+
+	return rval;
+}
+
+static int smiapp_enum_mbus_code(struct v4l2_subdev *subdev,
+				 struct v4l2_subdev_fh *fh,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	unsigned int i;
+	int idx = -1;
+	int rval = -EINVAL;
+
+	mutex_lock(&sensor->mutex);
+
+	dev_err(&client->dev, "subdev %s, pad %d, index %d\n",
+		subdev->name, code->pad, code->index);
+
+	if (subdev != &sensor->src->sd || code->pad != SMIAPP_PAD_SRC) {
+		if (code->index)
+			goto out;
+
+		code->code = sensor->internal_csi_format->code;
+		rval = 0;
+		goto out;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(smiapp_csi_data_formats); i++) {
+		if (sensor->mbus_frame_fmts & (1 << i))
+			idx++;
+
+		if (idx == code->index) {
+			code->code = smiapp_csi_data_formats[i].code;
+			dev_err(&client->dev, "found index %d, i %d, code %x\n",
+				code->index, i, code->code);
+			rval = 0;
+			break;
+		}
+	}
+
+out:
+	mutex_unlock(&sensor->mutex);
+
+	return rval;
+}
+
+static u32 __smiapp_get_mbus_code(struct v4l2_subdev *subdev,
+				  unsigned int pad)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+
+	if (subdev == &sensor->src->sd && pad == SMIAPP_PAD_SRC)
+		return sensor->csi_format->code;
+	else
+		return sensor->internal_csi_format->code;
+}
+
+static int __smiapp_get_format(struct v4l2_subdev *subdev,
+			       struct v4l2_subdev_fh *fh,
+			       struct v4l2_subdev_format *fmt)
+{
+	struct smiapp_subdev *ssd = to_smiapp_subdev(subdev);
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		fmt->format = *v4l2_subdev_get_try_format(fh, 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 = __smiapp_get_mbus_code(subdev, fmt->pad);
+		fmt->format.width = r->width;
+		fmt->format.height = r->height;
+	}
+
+	return 0;
+}
+
+static int smiapp_get_format(struct v4l2_subdev *subdev,
+			     struct v4l2_subdev_fh *fh,
+			     struct v4l2_subdev_format *fmt)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	int rval;
+
+	mutex_lock(&sensor->mutex);
+	rval = __smiapp_get_format(subdev, fh, fmt);
+	mutex_unlock(&sensor->mutex);
+
+	return rval;
+}
+
+static void smiapp_get_crop_compose(struct v4l2_subdev *subdev,
+				    struct v4l2_subdev_fh *fh,
+				    struct v4l2_rect **crops,
+				    struct v4l2_rect **comps, int which)
+{
+	struct smiapp_subdev *ssd = to_smiapp_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(fh, i);
+				BUG_ON(!crops[i]);
+			}
+		}
+		if (comps) {
+			*comps = v4l2_subdev_get_try_compose(fh,
+							     SMIAPP_PAD_SINK);
+			BUG_ON(!*comps);
+		}
+	}
+}
+
+/* Changes require propagation only on sink pad. */
+static void smiapp_propagate(struct v4l2_subdev *subdev,
+			     struct v4l2_subdev_fh *fh, int which,
+			     int target)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	struct smiapp_subdev *ssd = to_smiapp_subdev(subdev);
+	struct v4l2_rect *comp, *crops[SMIAPP_PADS];
+
+	smiapp_get_crop_compose(subdev, fh, crops, &comp, which);
+
+	switch (target) {
+	case V4L2_SEL_TGT_CROP:
+		comp->width = crops[SMIAPP_PAD_SINK]->width;
+		comp->height = crops[SMIAPP_PAD_SINK]->height;
+		if (which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+			if (ssd == sensor->scaler) {
+				sensor->scale_m =
+					sensor->limits[
+						SMIAPP_LIMIT_SCALER_N_MIN];
+				sensor->scaling_mode =
+					SMIAPP_SCALING_MODE_NONE;
+			} else if (ssd == sensor->binner) {
+				sensor->binning_horizontal = 1;
+				sensor->binning_vertical = 1;
+			}
+		}
+		/* Fall through */
+	case V4L2_SEL_TGT_COMPOSE:
+		*crops[SMIAPP_PAD_SRC] = *comp;
+		break;
+	default:
+		BUG();
+	}
+}
+
+static const struct smiapp_csi_data_format
+*smiapp_validate_csi_data_format(struct smiapp_sensor *sensor, u32 code)
+{
+	const struct smiapp_csi_data_format *csi_format = sensor->csi_format;
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(smiapp_csi_data_formats); i++) {
+		if (sensor->mbus_frame_fmts & (1 << i)
+		    && smiapp_csi_data_formats[i].code == code)
+			return &smiapp_csi_data_formats[i];
+	}
+
+	return csi_format;
+}
+
+static int smiapp_set_format(struct v4l2_subdev *subdev,
+			     struct v4l2_subdev_fh *fh,
+			     struct v4l2_subdev_format *fmt)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	struct smiapp_subdev *ssd = to_smiapp_subdev(subdev);
+	struct v4l2_rect *crops[SMIAPP_PADS];
+
+	mutex_lock(&sensor->mutex);
+
+	/*
+	 * Media bus code is changeable on src subdev's source pad. On
+	 * other source pads we just get format here.
+	 */
+	if (fmt->pad == ssd->source_pad) {
+		u32 code = fmt->format.code;
+		int rval = __smiapp_get_format(subdev, fh, fmt);
+
+		if (!rval && subdev == &sensor->src->sd) {
+			const struct smiapp_csi_data_format *csi_format =
+				smiapp_validate_csi_data_format(sensor, code);
+			if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+				sensor->csi_format = csi_format;
+			fmt->format.code = csi_format->code;
+		}
+
+		mutex_unlock(&sensor->mutex);
+		return rval;
+	}
+
+	/* Sink pad. Width and height are changeable here. */
+	fmt->format.code = __smiapp_get_mbus_code(subdev, fmt->pad);
+	fmt->format.width &= ~1;
+	fmt->format.height &= ~1;
+
+	fmt->format.width =
+		clamp(fmt->format.width,
+		      sensor->limits[SMIAPP_LIMIT_MIN_X_OUTPUT_SIZE],
+		      sensor->limits[SMIAPP_LIMIT_MAX_X_OUTPUT_SIZE]);
+	fmt->format.height =
+		clamp(fmt->format.height,
+		      sensor->limits[SMIAPP_LIMIT_MIN_Y_OUTPUT_SIZE],
+		      sensor->limits[SMIAPP_LIMIT_MAX_Y_OUTPUT_SIZE]);
+
+	smiapp_get_crop_compose(subdev, fh, 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];
+	smiapp_propagate(subdev, fh, 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 smiapp_sensor *sensor = to_smiapp_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 < sensor->limits[SMIAPP_LIMIT_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_h, h, ask_h, val);
+
+	return val;
+}
+
+static void smiapp_set_compose_binner(struct v4l2_subdev *subdev,
+				      struct v4l2_subdev_fh *fh,
+				      struct v4l2_subdev_selection *sel,
+				      struct v4l2_rect **crops,
+				      struct v4l2_rect *comp)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	unsigned int i;
+	unsigned int binh = 1, binv = 1;
+	unsigned int best = scaling_goodness(
+		subdev,
+		crops[SMIAPP_PAD_SINK]->width, sel->r.width,
+		crops[SMIAPP_PAD_SINK]->height, sel->r.height, sel->flags);
+
+	for (i = 0; i < sensor->nbinning_subtypes; i++) {
+		int this = scaling_goodness(
+			subdev,
+			crops[SMIAPP_PAD_SINK]->width
+			/ sensor->binning_subtypes[i].horizontal,
+			sel->r.width,
+			crops[SMIAPP_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[SMIAPP_PAD_SINK]->width / binh) & ~1;
+	sel->r.height = (crops[SMIAPP_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 smiapp_set_compose_scaler(struct v4l2_subdev *subdev,
+				      struct v4l2_subdev_fh *fh,
+				      struct v4l2_subdev_selection *sel,
+				      struct v4l2_rect **crops,
+				      struct v4l2_rect *comp)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	u32 min, max, a, b, max_m;
+	u32 scale_m = sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN];
+	int mode = SMIAPP_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[SMIAPP_PAD_SINK]->width);
+	sel->r.height = min_t(unsigned int, sel->r.height,
+			      crops[SMIAPP_PAD_SINK]->height);
+
+	a = crops[SMIAPP_PAD_SINK]->width
+		* sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN] / sel->r.width;
+	b = crops[SMIAPP_PAD_SINK]->height
+		* sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN] / sel->r.height;
+	max_m = crops[SMIAPP_PAD_SINK]->width
+		* sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN]
+		/ sensor->limits[SMIAPP_LIMIT_MIN_X_OUTPUT_SIZE];
+
+	a = min(sensor->limits[SMIAPP_LIMIT_SCALER_M_MAX],
+		max(a, sensor->limits[SMIAPP_LIMIT_SCALER_M_MIN]));
+	b = min(sensor->limits[SMIAPP_LIMIT_SCALER_M_MAX],
+		max(b, sensor->limits[SMIAPP_LIMIT_SCALER_M_MIN]));
+	max_m = min(sensor->limits[SMIAPP_LIMIT_SCALER_M_MAX],
+		    max(max_m, sensor->limits[SMIAPP_LIMIT_SCALER_M_MIN]));
+
+	dev_dbg(&client->dev, "scaling: a %d b %d max_m %d\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[SMIAPP_PAD_SINK]->width
+			/ try[i]
+			* sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN],
+			sel->r.width,
+			crops[SMIAPP_PAD_SINK]->height,
+			sel->r.height,
+			sel->flags);
+
+		dev_dbg(&client->dev, "trying factor %d (%d)\n", try[i], i);
+
+		if (this > best) {
+			scale_m = try[i];
+			mode = SMIAPP_SCALING_MODE_HORIZONTAL;
+			best = this;
+		}
+
+		if (sensor->limits[SMIAPP_LIMIT_SCALING_CAPABILITY]
+		    == SMIAPP_SCALING_CAPABILITY_HORIZONTAL)
+			continue;
+
+		this = scaling_goodness(
+			subdev, crops[SMIAPP_PAD_SINK]->width
+			/ try[i]
+			* sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN],
+			sel->r.width,
+			crops[SMIAPP_PAD_SINK]->height
+			/ try[i]
+			* sensor->limits[SMIAPP_LIMIT_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[SMIAPP_PAD_SINK]->width
+		 / scale_m
+		 * sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN]) & ~1;
+	if (mode == SMIAPP_SCALING_MODE_BOTH)
+		sel->r.height =
+			(crops[SMIAPP_PAD_SINK]->height
+			 / scale_m
+			 * sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN])
+			& ~1;
+	else
+		sel->r.height = crops[SMIAPP_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 smiapp_set_compose(struct v4l2_subdev *subdev,
+			      struct v4l2_subdev_fh *fh,
+			      struct v4l2_subdev_selection *sel)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	struct smiapp_subdev *ssd = to_smiapp_subdev(subdev);
+	struct v4l2_rect *comp, *crops[SMIAPP_PADS];
+
+	smiapp_get_crop_compose(subdev, fh, crops, &comp, sel->which);
+
+	sel->r.top = 0;
+	sel->r.left = 0;
+
+	if (ssd == sensor->binner)
+		smiapp_set_compose_binner(subdev, fh, sel, crops, comp);
+	else
+		smiapp_set_compose_scaler(subdev, fh, sel, crops, comp);
+
+	*comp = sel->r;
+	smiapp_propagate(subdev, fh, sel->which,
+			 V4L2_SEL_TGT_COMPOSE);
+
+	if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		return smiapp_update_mode(sensor);
+
+	return 0;
+}
+
+static int __smiapp_sel_supported(struct v4l2_subdev *subdev,
+				  struct v4l2_subdev_selection *sel)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	struct smiapp_subdev *ssd = to_smiapp_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 == SMIAPP_PA_PAD_SRC)
+			return 0;
+		if (ssd == sensor->src
+		    && sel->pad == SMIAPP_PAD_SRC)
+			return 0;
+		if (ssd == sensor->scaler
+		    && sel->pad == SMIAPP_PAD_SINK
+		    && sensor->limits[SMIAPP_LIMIT_DIGITAL_CROP_CAPABILITY]
+		    == SMIAPP_DIGITAL_CROP_CAPABILITY_INPUT_CROP)
+			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
+		    && sensor->limits[SMIAPP_LIMIT_SCALING_CAPABILITY]
+		    != SMIAPP_SCALING_CAPABILITY_NONE)
+			return 0;
+		/* Fall through */
+	default:
+		return -EINVAL;
+	}
+}
+
+static int smiapp_set_crop(struct v4l2_subdev *subdev,
+			   struct v4l2_subdev_fh *fh,
+			   struct v4l2_subdev_selection *sel)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	struct smiapp_subdev *ssd = to_smiapp_subdev(subdev);
+	struct v4l2_rect *src_size, *crops[SMIAPP_PADS];
+	struct v4l2_rect _r;
+
+	smiapp_get_crop_compose(subdev, fh, 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(fh, sel->pad)
+				->width;
+			_r.height = v4l2_subdev_get_try_format(fh, sel->pad)
+				->height;
+			src_size = &_r;
+		} else {
+			src_size =
+				v4l2_subdev_get_try_compose(
+					fh, ssd->sink_pad);
+		}
+	}
+
+	if (ssd == sensor->src && sel->pad == SMIAPP_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(sel->r.left, src_size->width - sel->r.width);
+	sel->r.top = min(sel->r.top, src_size->height - sel->r.height);
+
+	*crops[sel->pad] = sel->r;
+
+	if (ssd != sensor->pixel_array && sel->pad == SMIAPP_PAD_SINK)
+		smiapp_propagate(subdev, fh, sel->which,
+				 V4L2_SEL_TGT_CROP);
+
+	return 0;
+}
+
+static int __smiapp_get_selection(struct v4l2_subdev *subdev,
+				  struct v4l2_subdev_fh *fh,
+				  struct v4l2_subdev_selection *sel)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	struct smiapp_subdev *ssd = to_smiapp_subdev(subdev);
+	struct v4l2_rect *comp, *crops[SMIAPP_PADS];
+	struct v4l2_rect sink_fmt;
+	int ret;
+
+	ret = __smiapp_sel_supported(subdev, sel);
+	if (ret)
+		return ret;
+
+	smiapp_get_crop_compose(subdev, fh, 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(fh, 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:
+		if (ssd == sensor->pixel_array) {
+			sel->r.width =
+				sensor->limits[SMIAPP_LIMIT_X_ADDR_MAX] + 1;
+			sel->r.height =
+				sensor->limits[SMIAPP_LIMIT_Y_ADDR_MAX] + 1;
+		} 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 smiapp_get_selection(struct v4l2_subdev *subdev,
+				struct v4l2_subdev_fh *fh,
+				struct v4l2_subdev_selection *sel)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	int rval;
+
+	mutex_lock(&sensor->mutex);
+	rval = __smiapp_get_selection(subdev, fh, sel);
+	mutex_unlock(&sensor->mutex);
+
+	return rval;
+}
+static int smiapp_set_selection(struct v4l2_subdev *subdev,
+				struct v4l2_subdev_fh *fh,
+				struct v4l2_subdev_selection *sel)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	int ret;
+
+	ret = __smiapp_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 = max(0, SMIAPP_ALIGN_DIM(sel->r.width, sel->flags));
+	sel->r.height = max(0, SMIAPP_ALIGN_DIM(sel->r.height, sel->flags));
+
+	sel->r.width = max_t(unsigned int,
+			     sensor->limits[SMIAPP_LIMIT_MIN_X_OUTPUT_SIZE],
+			     sel->r.width);
+	sel->r.height = max_t(unsigned int,
+			      sensor->limits[SMIAPP_LIMIT_MIN_Y_OUTPUT_SIZE],
+			      sel->r.height);
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		ret = smiapp_set_crop(subdev, fh, sel);
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+		ret = smiapp_set_compose(subdev, fh, sel);
+		break;
+	default:
+		BUG();
+	}
+
+	mutex_unlock(&sensor->mutex);
+	return ret;
+}
+
+static int smiapp_get_skip_frames(struct v4l2_subdev *subdev, u32 *frames)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+
+	*frames = sensor->frame_skip;
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * sysfs attributes
+ */
+
+static ssize_t
+smiapp_sysfs_nvm_read(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 smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	unsigned int nbytes;
+
+	if (!sensor->dev_init_done)
+		return -EBUSY;
+
+	if (!sensor->nvm_size) {
+		/* NVM not read yet - read it now */
+		sensor->nvm_size = sensor->platform_data->nvm_size;
+		if (smiapp_set_power(subdev, 1) < 0)
+			return -ENODEV;
+		if (smiapp_read_nvm(sensor, sensor->nvm)) {
+			dev_err(&client->dev, "nvm read failed\n");
+			return -ENODEV;
+		}
+		smiapp_set_power(subdev, 0);
+	}
+	/*
+	 * NVM is still way below a PAGE_SIZE, so we can safely
+	 * assume this for now.
+	 */
+	nbytes = min_t(unsigned int, sensor->nvm_size, PAGE_SIZE);
+	memcpy(buf, sensor->nvm, nbytes);
+
+	return nbytes;
+}
+static DEVICE_ATTR(nvm, S_IRUGO, smiapp_sysfs_nvm_read, NULL);
+
+static ssize_t
+smiapp_sysfs_ident_read(struct device *dev, struct device_attribute *attr,
+			char *buf)
+{
+	struct v4l2_subdev *subdev = i2c_get_clientdata(to_i2c_client(dev));
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	struct smiapp_module_info *minfo = &sensor->minfo;
+
+	return snprintf(buf, PAGE_SIZE, "%2.2x%4.4x%2.2x\n",
+			minfo->manufacturer_id, minfo->model_id,
+			minfo->revision_number_major) + 1;
+}
+
+static DEVICE_ATTR(ident, S_IRUGO, smiapp_sysfs_ident_read, NULL);
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev core operations
+ */
+
+static int smiapp_identify_module(struct v4l2_subdev *subdev)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
+	struct smiapp_module_info *minfo = &sensor->minfo;
+	unsigned int i;
+	int rval = 0;
+
+	minfo->name = SMIAPP_NAME;
+
+	/* Module info */
+	rval = smiapp_read_8only(sensor, SMIAPP_REG_U8_MANUFACTURER_ID,
+				 &minfo->manufacturer_id);
+	if (!rval)
+		rval = smiapp_read_8only(sensor, SMIAPP_REG_U16_MODEL_ID,
+					 &minfo->model_id);
+	if (!rval)
+		rval = smiapp_read_8only(sensor,
+					 SMIAPP_REG_U8_REVISION_NUMBER_MAJOR,
+					 &minfo->revision_number_major);
+	if (!rval)
+		rval = smiapp_read_8only(sensor,
+					 SMIAPP_REG_U8_REVISION_NUMBER_MINOR,
+					 &minfo->revision_number_minor);
+	if (!rval)
+		rval = smiapp_read_8only(sensor,
+					 SMIAPP_REG_U8_MODULE_DATE_YEAR,
+					 &minfo->module_year);
+	if (!rval)
+		rval = smiapp_read_8only(sensor,
+					 SMIAPP_REG_U8_MODULE_DATE_MONTH,
+					 &minfo->module_month);
+	if (!rval)
+		rval = smiapp_read_8only(sensor, SMIAPP_REG_U8_MODULE_DATE_DAY,
+					 &minfo->module_day);
+
+	/* Sensor info */
+	if (!rval)
+		rval = smiapp_read_8only(sensor,
+					 SMIAPP_REG_U8_SENSOR_MANUFACTURER_ID,
+					 &minfo->sensor_manufacturer_id);
+	if (!rval)
+		rval = smiapp_read_8only(sensor,
+					 SMIAPP_REG_U16_SENSOR_MODEL_ID,
+					 &minfo->sensor_model_id);
+	if (!rval)
+		rval = smiapp_read_8only(sensor,
+					 SMIAPP_REG_U8_SENSOR_REVISION_NUMBER,
+					 &minfo->sensor_revision_number);
+	if (!rval)
+		rval = smiapp_read_8only(sensor,
+					 SMIAPP_REG_U8_SENSOR_FIRMWARE_VERSION,
+					 &minfo->sensor_firmware_version);
+
+	/* SMIA */
+	if (!rval)
+		rval = smiapp_read_8only(sensor, SMIAPP_REG_U8_SMIA_VERSION,
+					 &minfo->smia_version);
+	if (!rval)
+		rval = smiapp_read_8only(sensor, SMIAPP_REG_U8_SMIAPP_VERSION,
+					 &minfo->smiapp_version);
+
+	if (rval) {
+		dev_err(&client->dev, "sensor detection failed\n");
+		return -ENODEV;
+	}
+
+	dev_dbg(&client->dev, "module 0x%2.2x-0x%4.4x\n",
+		minfo->manufacturer_id, minfo->model_id);
+
+	dev_dbg(&client->dev,
+		"module revision 0x%2.2x-0x%2.2x date %2.2d-%2.2d-%2.2d\n",
+		minfo->revision_number_major, minfo->revision_number_minor,
+		minfo->module_year, minfo->module_month, minfo->module_day);
+
+	dev_dbg(&client->dev, "sensor 0x%2.2x-0x%4.4x\n",
+		minfo->sensor_manufacturer_id, minfo->sensor_model_id);
+
+	dev_dbg(&client->dev,
+		"sensor revision 0x%2.2x firmware version 0x%2.2x\n",
+		minfo->sensor_revision_number, minfo->sensor_firmware_version);
+
+	dev_dbg(&client->dev, "smia version %2.2d smiapp version %2.2d\n",
+		minfo->smia_version, minfo->smiapp_version);
+
+	/*
+	 * 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->manufacturer_id && !minfo->model_id) {
+		minfo->manufacturer_id = minfo->sensor_manufacturer_id;
+		minfo->model_id = minfo->sensor_model_id;
+		minfo->revision_number_major = minfo->sensor_revision_number;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(smiapp_module_idents); i++) {
+		if (smiapp_module_idents[i].manufacturer_id
+		    != minfo->manufacturer_id)
+			continue;
+		if (smiapp_module_idents[i].model_id != minfo->model_id)
+			continue;
+		if (smiapp_module_idents[i].flags
+		    & SMIAPP_MODULE_IDENT_FLAG_REV_LE) {
+			if (smiapp_module_idents[i].revision_number_major
+			    < minfo->revision_number_major)
+				continue;
+		} else {
+			if (smiapp_module_idents[i].revision_number_major
+			    != minfo->revision_number_major)
+				continue;
+		}
+
+		minfo->name = smiapp_module_idents[i].name;
+		minfo->quirk = smiapp_module_idents[i].quirk;
+		break;
+	}
+
+	if (i >= ARRAY_SIZE(smiapp_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, ident %2.2x%4.4x%2.2x\n",
+		minfo->name, minfo->manufacturer_id, minfo->model_id,
+		minfo->revision_number_major);
+
+	strlcpy(subdev->name, sensor->minfo.name, sizeof(subdev->name));
+
+	return 0;
+}
+
+static const struct v4l2_subdev_ops smiapp_ops;
+static const struct v4l2_subdev_internal_ops smiapp_internal_ops;
+static const struct media_entity_operations smiapp_entity_ops;
+
+static int smiapp_registered(struct v4l2_subdev *subdev)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
+	struct smiapp_subdev *last = NULL;
+	u32 tmp;
+	unsigned int i;
+	int rval;
+
+	sensor->vana = devm_regulator_get(&client->dev, "VANA");
+	if (IS_ERR(sensor->vana)) {
+		dev_err(&client->dev, "could not get regulator for vana\n");
+		return -ENODEV;
+	}
+
+	if (!sensor->platform_data->set_xclk) {
+		sensor->ext_clk = devm_clk_get(&client->dev,
+					sensor->platform_data->ext_clk_name);
+		if (IS_ERR(sensor->ext_clk)) {
+			dev_err(&client->dev, "could not get clock %s\n",
+				sensor->platform_data->ext_clk_name);
+			return -ENODEV;
+		}
+
+		rval = clk_set_rate(sensor->ext_clk,
+				    sensor->platform_data->ext_clk);
+		if (rval < 0) {
+			dev_err(&client->dev,
+				"unable to set clock %s freq to %u\n",
+				sensor->platform_data->ext_clk_name,
+				sensor->platform_data->ext_clk);
+			return -ENODEV;
+		}
+	}
+
+	if (sensor->platform_data->xshutdown != SMIAPP_NO_XSHUTDOWN) {
+		if (gpio_request_one(sensor->platform_data->xshutdown, 0,
+				     "SMIA++ xshutdown") != 0) {
+			dev_err(&client->dev,
+				"unable to acquire reset gpio %d\n",
+				sensor->platform_data->xshutdown);
+			return -ENODEV;
+		}
+	}
+
+	rval = smiapp_power_on(sensor);
+	if (rval) {
+		rval = -ENODEV;
+		goto out_smiapp_power_on;
+	}
+
+	rval = smiapp_identify_module(subdev);
+	if (rval) {
+		rval = -ENODEV;
+		goto out_power_off;
+	}
+
+	rval = smiapp_get_all_limits(sensor);
+	if (rval) {
+		rval = -ENODEV;
+		goto out_power_off;
+	}
+
+	/*
+	 * Handle Sensor Module orientation on the board.
+	 *
+	 * The application of H-FLIP and V-FLIP on the sensor is modified by
+	 * the sensor orientation on the board.
+	 *
+	 * For SMIAPP_BOARD_SENSOR_ORIENT_180 the default behaviour is to set
+	 * both H-FLIP and V-FLIP for normal operation which also implies
+	 * that a set/unset operation for user space HFLIP and VFLIP v4l2
+	 * controls will need to be internally inverted.
+	 *
+	 * Rotation also changes the bayer pattern.
+	 */
+	if (sensor->platform_data->module_board_orient ==
+	    SMIAPP_MODULE_BOARD_ORIENT_180)
+		sensor->hvflip_inv_mask = SMIAPP_IMAGE_ORIENTATION_HFLIP |
+					  SMIAPP_IMAGE_ORIENTATION_VFLIP;
+
+	rval = smiapp_get_mbus_formats(sensor);
+	if (rval) {
+		rval = -ENODEV;
+		goto out_power_off;
+	}
+
+	if (sensor->limits[SMIAPP_LIMIT_BINNING_CAPABILITY]) {
+		u32 val;
+
+		rval = smiapp_read(sensor,
+				   SMIAPP_REG_U8_BINNING_SUBTYPES, &val);
+		if (rval < 0) {
+			rval = -ENODEV;
+			goto out_power_off;
+		}
+		sensor->nbinning_subtypes = min_t(u8, val,
+						  SMIAPP_BINNING_SUBTYPES);
+
+		for (i = 0; i < sensor->nbinning_subtypes; i++) {
+			rval = smiapp_read(
+				sensor, SMIAPP_REG_U8_BINNING_TYPE_n(i), &val);
+			if (rval < 0) {
+				rval = -ENODEV;
+				goto out_power_off;
+			}
+			sensor->binning_subtypes[i] =
+				*(struct smiapp_binning_subtype *)&val;
+
+			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_power_off;
+	}
+	/* SMIA++ NVM initialization - it will be read from the sensor
+	 * when it is first requested by userspace.
+	 */
+	if (sensor->minfo.smiapp_version && sensor->platform_data->nvm_size) {
+		sensor->nvm = devm_kzalloc(&client->dev,
+				sensor->platform_data->nvm_size, GFP_KERNEL);
+		if (sensor->nvm == NULL) {
+			dev_err(&client->dev, "nvm buf allocation failed\n");
+			rval = -ENOMEM;
+			goto out_ident_release;
+		}
+
+		if (device_create_file(&client->dev, &dev_attr_nvm) != 0) {
+			dev_err(&client->dev, "sysfs nvm entry failed\n");
+			rval = -EBUSY;
+			goto out_ident_release;
+		}
+	}
+
+	rval = smiapp_call_quirk(sensor, limits);
+	if (rval) {
+		dev_err(&client->dev, "limits quirks failed\n");
+		goto out_nvm_release;
+	}
+
+	/* We consider this as profile 0 sensor if any of these are zero. */
+	if (!sensor->limits[SMIAPP_LIMIT_MIN_OP_SYS_CLK_DIV] ||
+	    !sensor->limits[SMIAPP_LIMIT_MAX_OP_SYS_CLK_DIV] ||
+	    !sensor->limits[SMIAPP_LIMIT_MIN_OP_PIX_CLK_DIV] ||
+	    !sensor->limits[SMIAPP_LIMIT_MAX_OP_PIX_CLK_DIV]) {
+		sensor->minfo.smiapp_profile = SMIAPP_PROFILE_0;
+	} else if (sensor->limits[SMIAPP_LIMIT_SCALING_CAPABILITY]
+		   != SMIAPP_SCALING_CAPABILITY_NONE) {
+		if (sensor->limits[SMIAPP_LIMIT_SCALING_CAPABILITY]
+		    == SMIAPP_SCALING_CAPABILITY_HORIZONTAL)
+			sensor->minfo.smiapp_profile = SMIAPP_PROFILE_1;
+		else
+			sensor->minfo.smiapp_profile = SMIAPP_PROFILE_2;
+		sensor->scaler = &sensor->ssds[sensor->ssds_used];
+		sensor->ssds_used++;
+	} else if (sensor->limits[SMIAPP_LIMIT_DIGITAL_CROP_CAPABILITY]
+		   == SMIAPP_DIGITAL_CROP_CAPABILITY_INPUT_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 = sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN];
+
+	for (i = 0; i < SMIAPP_SUBDEVS; i++) {
+		struct {
+			struct smiapp_subdev *ssd;
+			char *name;
+		} const __this[] = {
+			{ sensor->scaler, "scaler", },
+			{ sensor->binner, "binner", },
+			{ sensor->pixel_array, "pixel array", },
+		}, *_this = &__this[i];
+		struct smiapp_subdev *this = _this->ssd;
+
+		if (!this)
+			continue;
+
+		if (this != sensor->src)
+			v4l2_subdev_init(&this->sd, &smiapp_ops);
+
+		this->sensor = sensor;
+
+		if (this == sensor->pixel_array) {
+			this->npads = 1;
+		} else {
+			this->npads = 2;
+			this->source_pad = 1;
+		}
+
+		snprintf(this->sd.name,
+			 sizeof(this->sd.name), "%s %s",
+			 sensor->minfo.name, _this->name);
+
+		this->sink_fmt.width =
+			sensor->limits[SMIAPP_LIMIT_X_ADDR_MAX] + 1;
+		this->sink_fmt.height =
+			sensor->limits[SMIAPP_LIMIT_Y_ADDR_MAX] + 1;
+		this->compose.width = this->sink_fmt.width;
+		this->compose.height = this->sink_fmt.height;
+		this->crop[this->source_pad] = this->compose;
+		this->pads[this->source_pad].flags = MEDIA_PAD_FL_SOURCE;
+		if (this != sensor->pixel_array) {
+			this->crop[this->sink_pad] = this->compose;
+			this->pads[this->sink_pad].flags = MEDIA_PAD_FL_SINK;
+		}
+
+		this->sd.entity.ops = &smiapp_entity_ops;
+
+		if (last == NULL) {
+			last = this;
+			continue;
+		}
+
+		this->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+		this->sd.internal_ops = &smiapp_internal_ops;
+		this->sd.owner = NULL;
+		v4l2_set_subdevdata(&this->sd, client);
+
+		rval = media_entity_init(&this->sd.entity,
+					 this->npads, this->pads, 0);
+		if (rval) {
+			dev_err(&client->dev,
+				"media_entity_init failed\n");
+			goto out_nvm_release;
+		}
+
+		rval = media_entity_create_link(&this->sd.entity,
+						this->source_pad,
+						&last->sd.entity,
+						last->sink_pad,
+						MEDIA_LNK_FL_ENABLED |
+						MEDIA_LNK_FL_IMMUTABLE);
+		if (rval) {
+			dev_err(&client->dev,
+				"media_entity_create_link failed\n");
+			goto out_nvm_release;
+		}
+
+		rval = v4l2_device_register_subdev(sensor->src->sd.v4l2_dev,
+						   &this->sd);
+		if (rval) {
+			dev_err(&client->dev,
+				"v4l2_device_register_subdev failed\n");
+			goto out_nvm_release;
+		}
+
+		last = this;
+	}
+
+	dev_dbg(&client->dev, "profile %d\n", sensor->minfo.smiapp_profile);
+
+	sensor->pixel_array->sd.entity.type = MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
+
+	/* final steps */
+	smiapp_read_frame_fmt(sensor);
+	rval = smiapp_init_controls(sensor);
+	if (rval < 0)
+		goto out_nvm_release;
+
+	rval = smiapp_update_mode(sensor);
+	if (rval) {
+		dev_err(&client->dev, "update mode failed\n");
+		goto out_nvm_release;
+	}
+
+	sensor->streaming = false;
+	sensor->dev_init_done = true;
+
+	/* check flash capability */
+	rval = smiapp_read(sensor, SMIAPP_REG_U8_FLASH_MODE_CAPABILITY, &tmp);
+	sensor->flash_capability = tmp;
+	if (rval)
+		goto out_nvm_release;
+
+	smiapp_power_off(sensor);
+
+	return 0;
+
+out_nvm_release:
+	device_remove_file(&client->dev, &dev_attr_nvm);
+
+out_ident_release:
+	device_remove_file(&client->dev, &dev_attr_ident);
+
+out_power_off:
+	smiapp_power_off(sensor);
+
+out_smiapp_power_on:
+	if (sensor->platform_data->xshutdown != SMIAPP_NO_XSHUTDOWN)
+		gpio_free(sensor->platform_data->xshutdown);
+
+	return rval;
+}
+
+static int smiapp_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct smiapp_subdev *ssd = to_smiapp_subdev(sd);
+	struct smiapp_sensor *sensor = ssd->sensor;
+	u32 mbus_code =
+		smiapp_csi_data_formats[smiapp_pixel_order(sensor)].code;
+	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(fh, i);
+		struct v4l2_rect *try_crop = v4l2_subdev_get_try_crop(fh, i);
+		struct v4l2_rect *try_comp;
+
+		try_fmt->width = sensor->limits[SMIAPP_LIMIT_X_ADDR_MAX] + 1;
+		try_fmt->height = sensor->limits[SMIAPP_LIMIT_Y_ADDR_MAX] + 1;
+		try_fmt->code = mbus_code;
+
+		try_crop->top = 0;
+		try_crop->left = 0;
+		try_crop->width = try_fmt->width;
+		try_crop->height = try_fmt->height;
+
+		if (ssd != sensor->pixel_array)
+			continue;
+
+		try_comp = v4l2_subdev_get_try_compose(fh, i);
+		*try_comp = *try_crop;
+	}
+
+	mutex_unlock(&sensor->mutex);
+
+	return smiapp_set_power(sd, 1);
+}
+
+static int smiapp_close(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	return smiapp_set_power(sd, 0);
+}
+
+static const struct v4l2_subdev_video_ops smiapp_video_ops = {
+	.s_stream = smiapp_set_stream,
+};
+
+static const struct v4l2_subdev_core_ops smiapp_core_ops = {
+	.s_power = smiapp_set_power,
+};
+
+static const struct v4l2_subdev_pad_ops smiapp_pad_ops = {
+	.enum_mbus_code = smiapp_enum_mbus_code,
+	.get_fmt = smiapp_get_format,
+	.set_fmt = smiapp_set_format,
+	.get_selection = smiapp_get_selection,
+	.set_selection = smiapp_set_selection,
+};
+
+static const struct v4l2_subdev_sensor_ops smiapp_sensor_ops = {
+	.g_skip_frames = smiapp_get_skip_frames,
+};
+
+static const struct v4l2_subdev_ops smiapp_ops = {
+	.core = &smiapp_core_ops,
+	.video = &smiapp_video_ops,
+	.pad = &smiapp_pad_ops,
+	.sensor = &smiapp_sensor_ops,
+};
+
+static const struct media_entity_operations smiapp_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static const struct v4l2_subdev_internal_ops smiapp_internal_src_ops = {
+	.registered = smiapp_registered,
+	.open = smiapp_open,
+	.close = smiapp_close,
+};
+
+static const struct v4l2_subdev_internal_ops smiapp_internal_ops = {
+	.open = smiapp_open,
+	.close = smiapp_close,
+};
+
+/* -----------------------------------------------------------------------------
+ * I2C Driver
+ */
+
+#ifdef CONFIG_PM
+
+static int smiapp_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	bool streaming;
+
+	BUG_ON(mutex_is_locked(&sensor->mutex));
+
+	if (sensor->power_count == 0)
+		return 0;
+
+	if (sensor->streaming)
+		smiapp_stop_streaming(sensor);
+
+	streaming = sensor->streaming;
+
+	smiapp_power_off(sensor);
+
+	/* save state for resume */
+	sensor->streaming = streaming;
+
+	return 0;
+}
+
+static int smiapp_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	int rval;
+
+	if (sensor->power_count == 0)
+		return 0;
+
+	rval = smiapp_power_on(sensor);
+	if (rval)
+		return rval;
+
+	if (sensor->streaming)
+		rval = smiapp_start_streaming(sensor);
+
+	return rval;
+}
+
+#else
+
+#define smiapp_suspend	NULL
+#define smiapp_resume	NULL
+
+#endif /* CONFIG_PM */
+
+static int smiapp_probe(struct i2c_client *client,
+			const struct i2c_device_id *devid)
+{
+	struct smiapp_sensor *sensor;
+
+	if (client->dev.platform_data == NULL)
+		return -ENODEV;
+
+	sensor = devm_kzalloc(&client->dev, sizeof(*sensor), GFP_KERNEL);
+	if (sensor == NULL)
+		return -ENOMEM;
+
+	sensor->platform_data = client->dev.platform_data;
+	mutex_init(&sensor->mutex);
+	mutex_init(&sensor->power_mutex);
+	sensor->src = &sensor->ssds[sensor->ssds_used];
+
+	v4l2_i2c_subdev_init(&sensor->src->sd, client, &smiapp_ops);
+	sensor->src->sd.internal_ops = &smiapp_internal_src_ops;
+	sensor->src->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	sensor->src->sensor = sensor;
+
+	sensor->src->pads[0].flags = MEDIA_PAD_FL_SOURCE;
+	return media_entity_init(&sensor->src->sd.entity, 2,
+				 sensor->src->pads, 0);
+}
+
+static int __exit smiapp_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	unsigned int i;
+
+	if (sensor->power_count) {
+		if (sensor->platform_data->xshutdown != SMIAPP_NO_XSHUTDOWN)
+			gpio_set_value(sensor->platform_data->xshutdown, 0);
+		if (sensor->platform_data->set_xclk)
+			sensor->platform_data->set_xclk(&sensor->src->sd, 0);
+		else
+			clk_disable(sensor->ext_clk);
+		sensor->power_count = 0;
+	}
+
+	device_remove_file(&client->dev, &dev_attr_ident);
+	if (sensor->nvm)
+		device_remove_file(&client->dev, &dev_attr_nvm);
+
+	for (i = 0; i < sensor->ssds_used; i++) {
+		media_entity_cleanup(&sensor->ssds[i].sd.entity);
+		v4l2_device_unregister_subdev(&sensor->ssds[i].sd);
+	}
+	smiapp_free_controls(sensor);
+	if (sensor->platform_data->xshutdown != SMIAPP_NO_XSHUTDOWN)
+		gpio_free(sensor->platform_data->xshutdown);
+
+	return 0;
+}
+
+static const struct i2c_device_id smiapp_id_table[] = {
+	{ SMIAPP_NAME, 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(i2c, smiapp_id_table);
+
+static const struct dev_pm_ops smiapp_pm_ops = {
+	.suspend	= smiapp_suspend,
+	.resume		= smiapp_resume,
+};
+
+static struct i2c_driver smiapp_i2c_driver = {
+	.driver	= {
+		.name = SMIAPP_NAME,
+		.pm = &smiapp_pm_ops,
+	},
+	.probe	= smiapp_probe,
+	.remove	= __exit_p(smiapp_remove),
+	.id_table = smiapp_id_table,
+};
+
+module_i2c_driver(smiapp_i2c_driver);
+
+MODULE_AUTHOR("Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>");
+MODULE_DESCRIPTION("Generic SMIA/SMIA++ camera module driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/smiapp/smiapp-limits.c b/drivers/media/i2c/smiapp/smiapp-limits.c
new file mode 100644
index 000000000000..fb2f81ad8c3b
--- /dev/null
+++ b/drivers/media/i2c/smiapp/smiapp-limits.c
@@ -0,0 +1,132 @@
+/*
+ * drivers/media/i2c/smiapp/smiapp-limits.c
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2011--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#include "smiapp.h"
+
+struct smiapp_reg_limits smiapp_reg_limits[] = {
+	{ SMIAPP_REG_U16_ANALOGUE_GAIN_CAPABILITY, "analogue_gain_capability" }, /* 0 */
+	{ SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_MIN, "analogue_gain_code_min" },
+	{ SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_MAX, "analogue_gain_code_max" },
+	{ SMIAPP_REG_U8_THS_ZERO_MIN, "ths_zero_min" },
+	{ SMIAPP_REG_U8_TCLK_TRAIL_MIN, "tclk_trail_min" },
+	{ SMIAPP_REG_U16_INTEGRATION_TIME_CAPABILITY, "integration_time_capability" }, /* 5 */
+	{ SMIAPP_REG_U16_COARSE_INTEGRATION_TIME_MIN, "coarse_integration_time_min" },
+	{ SMIAPP_REG_U16_COARSE_INTEGRATION_TIME_MAX_MARGIN, "coarse_integration_time_max_margin" },
+	{ SMIAPP_REG_U16_FINE_INTEGRATION_TIME_MIN, "fine_integration_time_min" },
+	{ SMIAPP_REG_U16_FINE_INTEGRATION_TIME_MAX_MARGIN, "fine_integration_time_max_margin" },
+	{ SMIAPP_REG_U16_DIGITAL_GAIN_CAPABILITY, "digital_gain_capability" }, /* 10 */
+	{ SMIAPP_REG_U16_DIGITAL_GAIN_MIN, "digital_gain_min" },
+	{ SMIAPP_REG_U16_DIGITAL_GAIN_MAX, "digital_gain_max" },
+	{ SMIAPP_REG_F32_MIN_EXT_CLK_FREQ_HZ, "min_ext_clk_freq_hz" },
+	{ SMIAPP_REG_F32_MAX_EXT_CLK_FREQ_HZ, "max_ext_clk_freq_hz" },
+	{ SMIAPP_REG_U16_MIN_PRE_PLL_CLK_DIV, "min_pre_pll_clk_div" }, /* 15 */
+	{ SMIAPP_REG_U16_MAX_PRE_PLL_CLK_DIV, "max_pre_pll_clk_div" },
+	{ SMIAPP_REG_F32_MIN_PLL_IP_FREQ_HZ, "min_pll_ip_freq_hz" },
+	{ SMIAPP_REG_F32_MAX_PLL_IP_FREQ_HZ, "max_pll_ip_freq_hz" },
+	{ SMIAPP_REG_U16_MIN_PLL_MULTIPLIER, "min_pll_multiplier" },
+	{ SMIAPP_REG_U16_MAX_PLL_MULTIPLIER, "max_pll_multiplier" }, /* 20 */
+	{ SMIAPP_REG_F32_MIN_PLL_OP_FREQ_HZ, "min_pll_op_freq_hz" },
+	{ SMIAPP_REG_F32_MAX_PLL_OP_FREQ_HZ, "max_pll_op_freq_hz" },
+	{ SMIAPP_REG_U16_MIN_VT_SYS_CLK_DIV, "min_vt_sys_clk_div" },
+	{ SMIAPP_REG_U16_MAX_VT_SYS_CLK_DIV, "max_vt_sys_clk_div" },
+	{ SMIAPP_REG_F32_MIN_VT_SYS_CLK_FREQ_HZ, "min_vt_sys_clk_freq_hz" }, /* 25 */
+	{ SMIAPP_REG_F32_MAX_VT_SYS_CLK_FREQ_HZ, "max_vt_sys_clk_freq_hz" },
+	{ SMIAPP_REG_F32_MIN_VT_PIX_CLK_FREQ_HZ, "min_vt_pix_clk_freq_hz" },
+	{ SMIAPP_REG_F32_MAX_VT_PIX_CLK_FREQ_HZ, "max_vt_pix_clk_freq_hz" },
+	{ SMIAPP_REG_U16_MIN_VT_PIX_CLK_DIV, "min_vt_pix_clk_div" },
+	{ SMIAPP_REG_U16_MAX_VT_PIX_CLK_DIV, "max_vt_pix_clk_div" }, /* 30 */
+	{ SMIAPP_REG_U16_MIN_FRAME_LENGTH_LINES, "min_frame_length_lines" },
+	{ SMIAPP_REG_U16_MAX_FRAME_LENGTH_LINES, "max_frame_length_lines" },
+	{ SMIAPP_REG_U16_MIN_LINE_LENGTH_PCK, "min_line_length_pck" },
+	{ SMIAPP_REG_U16_MAX_LINE_LENGTH_PCK, "max_line_length_pck" },
+	{ SMIAPP_REG_U16_MIN_LINE_BLANKING_PCK, "min_line_blanking_pck" }, /* 35 */
+	{ SMIAPP_REG_U16_MIN_FRAME_BLANKING_LINES, "min_frame_blanking_lines" },
+	{ SMIAPP_REG_U8_MIN_LINE_LENGTH_PCK_STEP_SIZE, "min_line_length_pck_step_size" },
+	{ SMIAPP_REG_U16_MIN_OP_SYS_CLK_DIV, "min_op_sys_clk_div" },
+	{ SMIAPP_REG_U16_MAX_OP_SYS_CLK_DIV, "max_op_sys_clk_div" },
+	{ SMIAPP_REG_F32_MIN_OP_SYS_CLK_FREQ_HZ, "min_op_sys_clk_freq_hz" }, /* 40 */
+	{ SMIAPP_REG_F32_MAX_OP_SYS_CLK_FREQ_HZ, "max_op_sys_clk_freq_hz" },
+	{ SMIAPP_REG_U16_MIN_OP_PIX_CLK_DIV, "min_op_pix_clk_div" },
+	{ SMIAPP_REG_U16_MAX_OP_PIX_CLK_DIV, "max_op_pix_clk_div" },
+	{ SMIAPP_REG_F32_MIN_OP_PIX_CLK_FREQ_HZ, "min_op_pix_clk_freq_hz" },
+	{ SMIAPP_REG_F32_MAX_OP_PIX_CLK_FREQ_HZ, "max_op_pix_clk_freq_hz" }, /* 45 */
+	{ SMIAPP_REG_U16_X_ADDR_MIN, "x_addr_min" },
+	{ SMIAPP_REG_U16_Y_ADDR_MIN, "y_addr_min" },
+	{ SMIAPP_REG_U16_X_ADDR_MAX, "x_addr_max" },
+	{ SMIAPP_REG_U16_Y_ADDR_MAX, "y_addr_max" },
+	{ SMIAPP_REG_U16_MIN_X_OUTPUT_SIZE, "min_x_output_size" }, /* 50 */
+	{ SMIAPP_REG_U16_MIN_Y_OUTPUT_SIZE, "min_y_output_size" },
+	{ SMIAPP_REG_U16_MAX_X_OUTPUT_SIZE, "max_x_output_size" },
+	{ SMIAPP_REG_U16_MAX_Y_OUTPUT_SIZE, "max_y_output_size" },
+	{ SMIAPP_REG_U16_MIN_EVEN_INC, "min_even_inc" },
+	{ SMIAPP_REG_U16_MAX_EVEN_INC, "max_even_inc" }, /* 55 */
+	{ SMIAPP_REG_U16_MIN_ODD_INC, "min_odd_inc" },
+	{ SMIAPP_REG_U16_MAX_ODD_INC, "max_odd_inc" },
+	{ SMIAPP_REG_U16_SCALING_CAPABILITY, "scaling_capability" },
+	{ SMIAPP_REG_U16_SCALER_M_MIN, "scaler_m_min" },
+	{ SMIAPP_REG_U16_SCALER_M_MAX, "scaler_m_max" }, /* 60 */
+	{ SMIAPP_REG_U16_SCALER_N_MIN, "scaler_n_min" },
+	{ SMIAPP_REG_U16_SCALER_N_MAX, "scaler_n_max" },
+	{ SMIAPP_REG_U16_SPATIAL_SAMPLING_CAPABILITY, "spatial_sampling_capability" },
+	{ SMIAPP_REG_U8_DIGITAL_CROP_CAPABILITY, "digital_crop_capability" },
+	{ SMIAPP_REG_U16_COMPRESSION_CAPABILITY, "compression_capability" }, /* 65 */
+	{ SMIAPP_REG_U8_FIFO_SUPPORT_CAPABILITY, "fifo_support_capability" },
+	{ SMIAPP_REG_U8_DPHY_CTRL_CAPABILITY, "dphy_ctrl_capability" },
+	{ SMIAPP_REG_U8_CSI_LANE_MODE_CAPABILITY, "csi_lane_mode_capability" },
+	{ SMIAPP_REG_U8_CSI_SIGNALLING_MODE_CAPABILITY, "csi_signalling_mode_capability" },
+	{ SMIAPP_REG_U8_FAST_STANDBY_CAPABILITY, "fast_standby_capability" }, /* 70 */
+	{ SMIAPP_REG_U8_CCI_ADDRESS_CONTROL_CAPABILITY, "cci_address_control_capability" },
+	{ SMIAPP_REG_U32_MAX_PER_LANE_BITRATE_1_LANE_MODE_MBPS, "max_per_lane_bitrate_1_lane_mode_mbps" },
+	{ SMIAPP_REG_U32_MAX_PER_LANE_BITRATE_2_LANE_MODE_MBPS, "max_per_lane_bitrate_2_lane_mode_mbps" },
+	{ SMIAPP_REG_U32_MAX_PER_LANE_BITRATE_3_LANE_MODE_MBPS, "max_per_lane_bitrate_3_lane_mode_mbps" },
+	{ SMIAPP_REG_U32_MAX_PER_LANE_BITRATE_4_LANE_MODE_MBPS, "max_per_lane_bitrate_4_lane_mode_mbps" }, /* 75 */
+	{ SMIAPP_REG_U8_TEMP_SENSOR_CAPABILITY, "temp_sensor_capability" },
+	{ SMIAPP_REG_U16_MIN_FRAME_LENGTH_LINES_BIN, "min_frame_length_lines_bin" },
+	{ SMIAPP_REG_U16_MAX_FRAME_LENGTH_LINES_BIN, "max_frame_length_lines_bin" },
+	{ SMIAPP_REG_U16_MIN_LINE_LENGTH_PCK_BIN, "min_line_length_pck_bin" },
+	{ SMIAPP_REG_U16_MAX_LINE_LENGTH_PCK_BIN, "max_line_length_pck_bin" }, /* 80 */
+	{ SMIAPP_REG_U16_MIN_LINE_BLANKING_PCK_BIN, "min_line_blanking_pck_bin" },
+	{ SMIAPP_REG_U16_FINE_INTEGRATION_TIME_MIN_BIN, "fine_integration_time_min_bin" },
+	{ SMIAPP_REG_U16_FINE_INTEGRATION_TIME_MAX_MARGIN_BIN, "fine_integration_time_max_margin_bin" },
+	{ SMIAPP_REG_U8_BINNING_CAPABILITY, "binning_capability" },
+	{ SMIAPP_REG_U8_BINNING_WEIGHTING_CAPABILITY, "binning_weighting_capability" }, /* 85 */
+	{ SMIAPP_REG_U8_DATA_TRANSFER_IF_CAPABILITY, "data_transfer_if_capability" },
+	{ SMIAPP_REG_U8_SHADING_CORRECTION_CAPABILITY, "shading_correction_capability" },
+	{ SMIAPP_REG_U8_GREEN_IMBALANCE_CAPABILITY, "green_imbalance_capability" },
+	{ SMIAPP_REG_U8_BLACK_LEVEL_CAPABILITY, "black_level_capability" },
+	{ SMIAPP_REG_U8_MODULE_SPECIFIC_CORRECTION_CAPABILITY, "module_specific_correction_capability" }, /* 90 */
+	{ SMIAPP_REG_U16_DEFECT_CORRECTION_CAPABILITY, "defect_correction_capability" },
+	{ SMIAPP_REG_U16_DEFECT_CORRECTION_CAPABILITY_2, "defect_correction_capability_2" },
+	{ SMIAPP_REG_U8_EDOF_CAPABILITY, "edof_capability" },
+	{ SMIAPP_REG_U8_COLOUR_FEEDBACK_CAPABILITY, "colour_feedback_capability" },
+	{ SMIAPP_REG_U8_ESTIMATION_MODE_CAPABILITY, "estimation_mode_capability" }, /* 95 */
+	{ SMIAPP_REG_U8_ESTIMATION_ZONE_CAPABILITY, "estimation_zone_capability" },
+	{ SMIAPP_REG_U16_CAPABILITY_TRDY_MIN, "capability_trdy_min" },
+	{ SMIAPP_REG_U8_FLASH_MODE_CAPABILITY, "flash_mode_capability" },
+	{ SMIAPP_REG_U8_ACTUATOR_CAPABILITY, "actuator_capability" },
+	{ SMIAPP_REG_U8_BRACKETING_LUT_CAPABILITY_1, "bracketing_lut_capability_1" }, /* 100 */
+	{ SMIAPP_REG_U8_BRACKETING_LUT_CAPABILITY_2, "bracketing_lut_capability_2" },
+	{ SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_STEP, "analogue_gain_code_step" },
+	{ 0, NULL },
+};
diff --git a/drivers/media/i2c/smiapp/smiapp-limits.h b/drivers/media/i2c/smiapp/smiapp-limits.h
new file mode 100644
index 000000000000..9ae765e23ea5
--- /dev/null
+++ b/drivers/media/i2c/smiapp/smiapp-limits.h
@@ -0,0 +1,128 @@
+/*
+ * drivers/media/i2c/smiapp/smiapp-limits.h
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2011--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#define SMIAPP_LIMIT_ANALOGUE_GAIN_CAPABILITY			0
+#define SMIAPP_LIMIT_ANALOGUE_GAIN_CODE_MIN			1
+#define SMIAPP_LIMIT_ANALOGUE_GAIN_CODE_MAX			2
+#define SMIAPP_LIMIT_THS_ZERO_MIN				3
+#define SMIAPP_LIMIT_TCLK_TRAIL_MIN				4
+#define SMIAPP_LIMIT_INTEGRATION_TIME_CAPABILITY		5
+#define SMIAPP_LIMIT_COARSE_INTEGRATION_TIME_MIN		6
+#define SMIAPP_LIMIT_COARSE_INTEGRATION_TIME_MAX_MARGIN		7
+#define SMIAPP_LIMIT_FINE_INTEGRATION_TIME_MIN			8
+#define SMIAPP_LIMIT_FINE_INTEGRATION_TIME_MAX_MARGIN		9
+#define SMIAPP_LIMIT_DIGITAL_GAIN_CAPABILITY			10
+#define SMIAPP_LIMIT_DIGITAL_GAIN_MIN				11
+#define SMIAPP_LIMIT_DIGITAL_GAIN_MAX				12
+#define SMIAPP_LIMIT_MIN_EXT_CLK_FREQ_HZ			13
+#define SMIAPP_LIMIT_MAX_EXT_CLK_FREQ_HZ			14
+#define SMIAPP_LIMIT_MIN_PRE_PLL_CLK_DIV			15
+#define SMIAPP_LIMIT_MAX_PRE_PLL_CLK_DIV			16
+#define SMIAPP_LIMIT_MIN_PLL_IP_FREQ_HZ				17
+#define SMIAPP_LIMIT_MAX_PLL_IP_FREQ_HZ				18
+#define SMIAPP_LIMIT_MIN_PLL_MULTIPLIER				19
+#define SMIAPP_LIMIT_MAX_PLL_MULTIPLIER				20
+#define SMIAPP_LIMIT_MIN_PLL_OP_FREQ_HZ				21
+#define SMIAPP_LIMIT_MAX_PLL_OP_FREQ_HZ				22
+#define SMIAPP_LIMIT_MIN_VT_SYS_CLK_DIV				23
+#define SMIAPP_LIMIT_MAX_VT_SYS_CLK_DIV				24
+#define SMIAPP_LIMIT_MIN_VT_SYS_CLK_FREQ_HZ			25
+#define SMIAPP_LIMIT_MAX_VT_SYS_CLK_FREQ_HZ			26
+#define SMIAPP_LIMIT_MIN_VT_PIX_CLK_FREQ_HZ			27
+#define SMIAPP_LIMIT_MAX_VT_PIX_CLK_FREQ_HZ			28
+#define SMIAPP_LIMIT_MIN_VT_PIX_CLK_DIV				29
+#define SMIAPP_LIMIT_MAX_VT_PIX_CLK_DIV				30
+#define SMIAPP_LIMIT_MIN_FRAME_LENGTH_LINES			31
+#define SMIAPP_LIMIT_MAX_FRAME_LENGTH_LINES			32
+#define SMIAPP_LIMIT_MIN_LINE_LENGTH_PCK			33
+#define SMIAPP_LIMIT_MAX_LINE_LENGTH_PCK			34
+#define SMIAPP_LIMIT_MIN_LINE_BLANKING_PCK			35
+#define SMIAPP_LIMIT_MIN_FRAME_BLANKING_LINES			36
+#define SMIAPP_LIMIT_MIN_LINE_LENGTH_PCK_STEP_SIZE		37
+#define SMIAPP_LIMIT_MIN_OP_SYS_CLK_DIV				38
+#define SMIAPP_LIMIT_MAX_OP_SYS_CLK_DIV				39
+#define SMIAPP_LIMIT_MIN_OP_SYS_CLK_FREQ_HZ			40
+#define SMIAPP_LIMIT_MAX_OP_SYS_CLK_FREQ_HZ			41
+#define SMIAPP_LIMIT_MIN_OP_PIX_CLK_DIV				42
+#define SMIAPP_LIMIT_MAX_OP_PIX_CLK_DIV				43
+#define SMIAPP_LIMIT_MIN_OP_PIX_CLK_FREQ_HZ			44
+#define SMIAPP_LIMIT_MAX_OP_PIX_CLK_FREQ_HZ			45
+#define SMIAPP_LIMIT_X_ADDR_MIN					46
+#define SMIAPP_LIMIT_Y_ADDR_MIN					47
+#define SMIAPP_LIMIT_X_ADDR_MAX					48
+#define SMIAPP_LIMIT_Y_ADDR_MAX					49
+#define SMIAPP_LIMIT_MIN_X_OUTPUT_SIZE				50
+#define SMIAPP_LIMIT_MIN_Y_OUTPUT_SIZE				51
+#define SMIAPP_LIMIT_MAX_X_OUTPUT_SIZE				52
+#define SMIAPP_LIMIT_MAX_Y_OUTPUT_SIZE				53
+#define SMIAPP_LIMIT_MIN_EVEN_INC				54
+#define SMIAPP_LIMIT_MAX_EVEN_INC				55
+#define SMIAPP_LIMIT_MIN_ODD_INC				56
+#define SMIAPP_LIMIT_MAX_ODD_INC				57
+#define SMIAPP_LIMIT_SCALING_CAPABILITY				58
+#define SMIAPP_LIMIT_SCALER_M_MIN				59
+#define SMIAPP_LIMIT_SCALER_M_MAX				60
+#define SMIAPP_LIMIT_SCALER_N_MIN				61
+#define SMIAPP_LIMIT_SCALER_N_MAX				62
+#define SMIAPP_LIMIT_SPATIAL_SAMPLING_CAPABILITY		63
+#define SMIAPP_LIMIT_DIGITAL_CROP_CAPABILITY			64
+#define SMIAPP_LIMIT_COMPRESSION_CAPABILITY			65
+#define SMIAPP_LIMIT_FIFO_SUPPORT_CAPABILITY			66
+#define SMIAPP_LIMIT_DPHY_CTRL_CAPABILITY			67
+#define SMIAPP_LIMIT_CSI_LANE_MODE_CAPABILITY			68
+#define SMIAPP_LIMIT_CSI_SIGNALLING_MODE_CAPABILITY		69
+#define SMIAPP_LIMIT_FAST_STANDBY_CAPABILITY			70
+#define SMIAPP_LIMIT_CCI_ADDRESS_CONTROL_CAPABILITY		71
+#define SMIAPP_LIMIT_MAX_PER_LANE_BITRATE_1_LANE_MODE_MBPS	72
+#define SMIAPP_LIMIT_MAX_PER_LANE_BITRATE_2_LANE_MODE_MBPS	73
+#define SMIAPP_LIMIT_MAX_PER_LANE_BITRATE_3_LANE_MODE_MBPS	74
+#define SMIAPP_LIMIT_MAX_PER_LANE_BITRATE_4_LANE_MODE_MBPS	75
+#define SMIAPP_LIMIT_TEMP_SENSOR_CAPABILITY			76
+#define SMIAPP_LIMIT_MIN_FRAME_LENGTH_LINES_BIN			77
+#define SMIAPP_LIMIT_MAX_FRAME_LENGTH_LINES_BIN			78
+#define SMIAPP_LIMIT_MIN_LINE_LENGTH_PCK_BIN			79
+#define SMIAPP_LIMIT_MAX_LINE_LENGTH_PCK_BIN			80
+#define SMIAPP_LIMIT_MIN_LINE_BLANKING_PCK_BIN			81
+#define SMIAPP_LIMIT_FINE_INTEGRATION_TIME_MIN_BIN		82
+#define SMIAPP_LIMIT_FINE_INTEGRATION_TIME_MAX_MARGIN_BIN	83
+#define SMIAPP_LIMIT_BINNING_CAPABILITY				84
+#define SMIAPP_LIMIT_BINNING_WEIGHTING_CAPABILITY		85
+#define SMIAPP_LIMIT_DATA_TRANSFER_IF_CAPABILITY		86
+#define SMIAPP_LIMIT_SHADING_CORRECTION_CAPABILITY		87
+#define SMIAPP_LIMIT_GREEN_IMBALANCE_CAPABILITY			88
+#define SMIAPP_LIMIT_BLACK_LEVEL_CAPABILITY			89
+#define SMIAPP_LIMIT_MODULE_SPECIFIC_CORRECTION_CAPABILITY	90
+#define SMIAPP_LIMIT_DEFECT_CORRECTION_CAPABILITY		91
+#define SMIAPP_LIMIT_DEFECT_CORRECTION_CAPABILITY_2		92
+#define SMIAPP_LIMIT_EDOF_CAPABILITY				93
+#define SMIAPP_LIMIT_COLOUR_FEEDBACK_CAPABILITY			94
+#define SMIAPP_LIMIT_ESTIMATION_MODE_CAPABILITY			95
+#define SMIAPP_LIMIT_ESTIMATION_ZONE_CAPABILITY			96
+#define SMIAPP_LIMIT_CAPABILITY_TRDY_MIN			97
+#define SMIAPP_LIMIT_FLASH_MODE_CAPABILITY			98
+#define SMIAPP_LIMIT_ACTUATOR_CAPABILITY			99
+#define SMIAPP_LIMIT_BRACKETING_LUT_CAPABILITY_1		100
+#define SMIAPP_LIMIT_BRACKETING_LUT_CAPABILITY_2		101
+#define SMIAPP_LIMIT_ANALOGUE_GAIN_CODE_STEP			102
+#define SMIAPP_LIMIT_LAST					103
diff --git a/drivers/media/i2c/smiapp/smiapp-quirk.c b/drivers/media/i2c/smiapp/smiapp-quirk.c
new file mode 100644
index 000000000000..725cf62836c6
--- /dev/null
+++ b/drivers/media/i2c/smiapp/smiapp-quirk.c
@@ -0,0 +1,286 @@
+/*
+ * drivers/media/i2c/smiapp/smiapp-quirk.c
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2011--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#include <linux/delay.h>
+
+#include "smiapp.h"
+
+static int smiapp_write_8(struct smiapp_sensor *sensor, u16 reg, u8 val)
+{
+	return smiapp_write(sensor, (SMIA_REG_8BIT << 16) | reg, val);
+}
+
+static int smiapp_write_8s(struct smiapp_sensor *sensor,
+			   struct smiapp_reg_8 *regs, int len)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	int rval;
+
+	for (; len > 0; len--, regs++) {
+		rval = smiapp_write_8(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;
+}
+
+void smiapp_replace_limit(struct smiapp_sensor *sensor,
+			  u32 limit, u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+
+	dev_dbg(&client->dev, "quirk: 0x%8.8x \"%s\" = %d, 0x%x\n",
+		smiapp_reg_limits[limit].addr,
+		smiapp_reg_limits[limit].what, val, val);
+	sensor->limits[limit] = val;
+}
+
+bool smiapp_quirk_reg(struct smiapp_sensor *sensor,
+		      u32 reg, u32 *val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	const struct smia_reg *sreg;
+
+	if (!sensor->minfo.quirk)
+		return false;
+
+	sreg = sensor->minfo.quirk->regs;
+
+	if (!sreg)
+		return false;
+
+	while (sreg->type) {
+		u16 type = reg >> 16;
+		u16 reg16 = reg;
+
+		if (sreg->type != type || sreg->reg != reg16) {
+			sreg++;
+			continue;
+		}
+
+		switch ((u8)type) {
+		case SMIA_REG_8BIT:
+			dev_dbg(&client->dev, "quirk: 0x%8.8x: 0x%2.2x\n",
+				reg, sreg->val);
+			break;
+		case SMIA_REG_16BIT:
+			dev_dbg(&client->dev, "quirk: 0x%8.8x: 0x%4.4x\n",
+				reg, sreg->val);
+			break;
+		case SMIA_REG_32BIT:
+			dev_dbg(&client->dev, "quirk: 0x%8.8x: 0x%8.8x\n",
+				reg, sreg->val);
+			break;
+		}
+
+		*val = sreg->val;
+
+		return true;
+	}
+
+	return false;
+}
+
+static int jt8ew9_limits(struct smiapp_sensor *sensor)
+{
+	if (sensor->minfo.revision_number_major < 0x03)
+		sensor->frame_skip = 1;
+
+	/* Below 24 gain doesn't have effect at all, */
+	/* but ~59 is needed for full dynamic range */
+	smiapp_replace_limit(sensor, SMIAPP_LIMIT_ANALOGUE_GAIN_CODE_MIN, 59);
+	smiapp_replace_limit(
+		sensor, SMIAPP_LIMIT_ANALOGUE_GAIN_CODE_MAX, 6000);
+
+	return 0;
+}
+
+static int jt8ew9_post_poweron(struct smiapp_sensor *sensor)
+{
+	struct smiapp_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 smiapp_write_8s(sensor, regs, ARRAY_SIZE(regs));
+}
+
+const struct smiapp_quirk smiapp_jt8ew9_quirk = {
+	.limits = jt8ew9_limits,
+	.post_poweron = jt8ew9_post_poweron,
+};
+
+static int imx125es_post_poweron(struct smiapp_sensor *sensor)
+{
+	/* Taken from v02. No idea what the other two are. */
+	struct smiapp_reg_8 regs[] = {
+		/*
+		 * 0x3302: clk during frame blanking:
+		 * 0x00 - HS mode, 0x01 - LP11
+		 */
+		{ 0x3302, 0x01 },
+		{ 0x302d, 0x00 },
+		{ 0x3b08, 0x8c },
+	};
+
+	return smiapp_write_8s(sensor, regs, ARRAY_SIZE(regs));
+}
+
+const struct smiapp_quirk smiapp_imx125es_quirk = {
+	.post_poweron = imx125es_post_poweron,
+};
+
+static int jt8ev1_limits(struct smiapp_sensor *sensor)
+{
+	smiapp_replace_limit(sensor, SMIAPP_LIMIT_X_ADDR_MAX, 4271);
+	smiapp_replace_limit(sensor,
+			     SMIAPP_LIMIT_MIN_LINE_BLANKING_PCK_BIN, 184);
+
+	return 0;
+}
+
+static int jt8ev1_post_poweron(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	int rval;
+
+	struct smiapp_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. */
+	};
+
+	struct smiapp_reg_8 regs_96[] = {
+		{ 0x30ae, 0x00 }, /* For control of ADC clock */
+		{ 0x30af, 0xd0 },
+		{ 0x30b0, 0x01 },
+	};
+
+	rval = smiapp_write_8s(sensor, regs, ARRAY_SIZE(regs));
+	if (rval < 0)
+		return rval;
+
+	switch (sensor->platform_data->ext_clk) {
+	case 9600000:
+		return smiapp_write_8s(sensor, regs_96,
+				       ARRAY_SIZE(regs_96));
+	default:
+		dev_warn(&client->dev, "no MSRs for %d Hz ext_clk\n",
+			 sensor->platform_data->ext_clk);
+		return 0;
+	}
+}
+
+static int jt8ev1_pre_streamon(struct smiapp_sensor *sensor)
+{
+	return smiapp_write_8(sensor, 0x3328, 0x00);
+}
+
+static int jt8ev1_post_streamoff(struct smiapp_sensor *sensor)
+{
+	int rval;
+
+	/* Workaround: allows fast standby to work properly */
+	rval = smiapp_write_8(sensor, 0x3205, 0x04);
+	if (rval < 0)
+		return rval;
+
+	/* Wait for 1 ms + one line => 2 ms is likely enough */
+	usleep_range(2000, 2000);
+
+	/* Restore it */
+	rval = smiapp_write_8(sensor, 0x3205, 0x00);
+	if (rval < 0)
+		return rval;
+
+	return smiapp_write_8(sensor, 0x3328, 0x80);
+}
+
+const struct smiapp_quirk smiapp_jt8ev1_quirk = {
+	.limits = jt8ev1_limits,
+	.post_poweron = jt8ev1_post_poweron,
+	.pre_streamon = jt8ev1_pre_streamon,
+	.post_streamoff = jt8ev1_post_streamoff,
+	.flags = SMIAPP_QUIRK_FLAG_OP_PIX_CLOCK_PER_LANE,
+};
+
+static int tcm8500md_limits(struct smiapp_sensor *sensor)
+{
+	smiapp_replace_limit(sensor, SMIAPP_LIMIT_MIN_PLL_IP_FREQ_HZ, 2700000);
+
+	return 0;
+}
+
+const struct smiapp_quirk smiapp_tcm8500md_quirk = {
+	.limits = tcm8500md_limits,
+};
diff --git a/drivers/media/i2c/smiapp/smiapp-quirk.h b/drivers/media/i2c/smiapp/smiapp-quirk.h
new file mode 100644
index 000000000000..86fd3e8bfb0f
--- /dev/null
+++ b/drivers/media/i2c/smiapp/smiapp-quirk.h
@@ -0,0 +1,83 @@
+/*
+ * drivers/media/i2c/smiapp/smiapp-quirk.h
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2011--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#ifndef __SMIAPP_QUIRK__
+#define __SMIAPP_QUIRK__
+
+struct smiapp_sensor;
+
+/**
+ * struct smiapp_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_streamon: Called right after stopping streaming.
+ */
+struct smiapp_quirk {
+	int (*limits)(struct smiapp_sensor *sensor);
+	int (*post_poweron)(struct smiapp_sensor *sensor);
+	int (*pre_streamon)(struct smiapp_sensor *sensor);
+	int (*post_streamoff)(struct smiapp_sensor *sensor);
+	const struct smia_reg *regs;
+	unsigned long flags;
+};
+
+/* op pix clock is for all lanes in total normally */
+#define SMIAPP_QUIRK_FLAG_OP_PIX_CLOCK_PER_LANE			(1 << 0)
+#define SMIAPP_QUIRK_FLAG_8BIT_READ_ONLY			(1 << 1)
+
+struct smiapp_reg_8 {
+	u16 reg;
+	u8 val;
+};
+
+void smiapp_replace_limit(struct smiapp_sensor *sensor,
+			  u32 limit, u32 val);
+bool smiapp_quirk_reg(struct smiapp_sensor *sensor,
+		      u32 reg, u32 *val);
+
+#define SMIAPP_MK_QUIRK_REG(_reg, _val) \
+	{				\
+		.type = (_reg >> 16),	\
+		.reg = (u16)_reg,	\
+		.val = _val,		\
+	}
+
+#define smiapp_call_quirk(_sensor, _quirk, ...)				\
+	(_sensor->minfo.quirk &&					\
+	 _sensor->minfo.quirk->_quirk ?					\
+	 _sensor->minfo.quirk->_quirk(_sensor, ##__VA_ARGS__) : 0)
+
+#define smiapp_needs_quirk(_sensor, _quirk)		\
+	(_sensor->minfo.quirk ?				\
+	 _sensor->minfo.quirk->flags & _quirk : 0)
+
+extern const struct smiapp_quirk smiapp_jt8ev1_quirk;
+extern const struct smiapp_quirk smiapp_imx125es_quirk;
+extern const struct smiapp_quirk smiapp_jt8ew9_quirk;
+extern const struct smiapp_quirk smiapp_tcm8500md_quirk;
+
+#endif /* __SMIAPP_QUIRK__ */
diff --git a/drivers/media/i2c/smiapp/smiapp-reg-defs.h b/drivers/media/i2c/smiapp/smiapp-reg-defs.h
new file mode 100644
index 000000000000..defa7c5adebf
--- /dev/null
+++ b/drivers/media/i2c/smiapp/smiapp-reg-defs.h
@@ -0,0 +1,503 @@
+/*
+ * drivers/media/i2c/smiapp/smiapp-reg-defs.h
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2011--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+#define SMIAPP_REG_MK_U8(r) ((SMIA_REG_8BIT << 16) | (r))
+#define SMIAPP_REG_MK_U16(r) ((SMIA_REG_16BIT << 16) | (r))
+#define SMIAPP_REG_MK_U32(r) ((SMIA_REG_32BIT << 16) | (r))
+
+#define SMIAPP_REG_MK_F32(r) (SMIA_REG_FLAG_FLOAT | (SMIA_REG_32BIT << 16) | (r))
+
+#define SMIAPP_REG_U16_MODEL_ID					SMIAPP_REG_MK_U16(0x0000)
+#define SMIAPP_REG_U8_REVISION_NUMBER_MAJOR			SMIAPP_REG_MK_U8(0x0002)
+#define SMIAPP_REG_U8_MANUFACTURER_ID				SMIAPP_REG_MK_U8(0x0003)
+#define SMIAPP_REG_U8_SMIA_VERSION				SMIAPP_REG_MK_U8(0x0004)
+#define SMIAPP_REG_U8_FRAME_COUNT				SMIAPP_REG_MK_U8(0x0005)
+#define SMIAPP_REG_U8_PIXEL_ORDER				SMIAPP_REG_MK_U8(0x0006)
+#define SMIAPP_REG_U16_DATA_PEDESTAL				SMIAPP_REG_MK_U16(0x0008)
+#define SMIAPP_REG_U8_PIXEL_DEPTH				SMIAPP_REG_MK_U8(0x000c)
+#define SMIAPP_REG_U8_REVISION_NUMBER_MINOR			SMIAPP_REG_MK_U8(0x0010)
+#define SMIAPP_REG_U8_SMIAPP_VERSION				SMIAPP_REG_MK_U8(0x0011)
+#define SMIAPP_REG_U8_MODULE_DATE_YEAR				SMIAPP_REG_MK_U8(0x0012)
+#define SMIAPP_REG_U8_MODULE_DATE_MONTH				SMIAPP_REG_MK_U8(0x0013)
+#define SMIAPP_REG_U8_MODULE_DATE_DAY				SMIAPP_REG_MK_U8(0x0014)
+#define SMIAPP_REG_U8_MODULE_DATE_PHASE				SMIAPP_REG_MK_U8(0x0015)
+#define SMIAPP_REG_U16_SENSOR_MODEL_ID				SMIAPP_REG_MK_U16(0x0016)
+#define SMIAPP_REG_U8_SENSOR_REVISION_NUMBER			SMIAPP_REG_MK_U8(0x0018)
+#define SMIAPP_REG_U8_SENSOR_MANUFACTURER_ID			SMIAPP_REG_MK_U8(0x0019)
+#define SMIAPP_REG_U8_SENSOR_FIRMWARE_VERSION			SMIAPP_REG_MK_U8(0x001a)
+#define SMIAPP_REG_U32_SERIAL_NUMBER				SMIAPP_REG_MK_U32(0x001c)
+#define SMIAPP_REG_U8_FRAME_FORMAT_MODEL_TYPE			SMIAPP_REG_MK_U8(0x0040)
+#define SMIAPP_REG_U8_FRAME_FORMAT_MODEL_SUBTYPE		SMIAPP_REG_MK_U8(0x0041)
+#define SMIAPP_REG_U16_FRAME_FORMAT_DESCRIPTOR_2(n)		SMIAPP_REG_MK_U16(0x0042 + ((n) << 1)) /* 0 <= n <= 14 */
+#define SMIAPP_REG_U32_FRAME_FORMAT_DESCRIPTOR_4(n)		SMIAPP_REG_MK_U32(0x0060 + ((n) << 2)) /* 0 <= n <= 7 */
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CAPABILITY			SMIAPP_REG_MK_U16(0x0080)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_MIN			SMIAPP_REG_MK_U16(0x0084)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_MAX			SMIAPP_REG_MK_U16(0x0086)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_STEP			SMIAPP_REG_MK_U16(0x0088)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_TYPE			SMIAPP_REG_MK_U16(0x008a)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_M0				SMIAPP_REG_MK_U16(0x008c)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_C0				SMIAPP_REG_MK_U16(0x008e)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_M1				SMIAPP_REG_MK_U16(0x0090)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_C1				SMIAPP_REG_MK_U16(0x0092)
+#define SMIAPP_REG_U8_DATA_FORMAT_MODEL_TYPE			SMIAPP_REG_MK_U8(0x00c0)
+#define SMIAPP_REG_U8_DATA_FORMAT_MODEL_SUBTYPE			SMIAPP_REG_MK_U8(0x00c1)
+#define SMIAPP_REG_U16_DATA_FORMAT_DESCRIPTOR(n)		SMIAPP_REG_MK_U16(0x00c2 + ((n) << 1))
+#define SMIAPP_REG_U8_MODE_SELECT				SMIAPP_REG_MK_U8(0x0100)
+#define SMIAPP_REG_U8_IMAGE_ORIENTATION				SMIAPP_REG_MK_U8(0x0101)
+#define SMIAPP_REG_U8_SOFTWARE_RESET				SMIAPP_REG_MK_U8(0x0103)
+#define SMIAPP_REG_U8_GROUPED_PARAMETER_HOLD			SMIAPP_REG_MK_U8(0x0104)
+#define SMIAPP_REG_U8_MASK_CORRUPTED_FRAMES			SMIAPP_REG_MK_U8(0x0105)
+#define SMIAPP_REG_U8_FAST_STANDBY_CTRL				SMIAPP_REG_MK_U8(0x0106)
+#define SMIAPP_REG_U8_CCI_ADDRESS_CONTROL			SMIAPP_REG_MK_U8(0x0107)
+#define SMIAPP_REG_U8_2ND_CCI_IF_CONTROL			SMIAPP_REG_MK_U8(0x0108)
+#define SMIAPP_REG_U8_2ND_CCI_ADDRESS_CONTROL			SMIAPP_REG_MK_U8(0x0109)
+#define SMIAPP_REG_U8_CSI_CHANNEL_IDENTIFIER			SMIAPP_REG_MK_U8(0x0110)
+#define SMIAPP_REG_U8_CSI_SIGNALLING_MODE			SMIAPP_REG_MK_U8(0x0111)
+#define SMIAPP_REG_U16_CSI_DATA_FORMAT				SMIAPP_REG_MK_U16(0x0112)
+#define SMIAPP_REG_U8_CSI_LANE_MODE				SMIAPP_REG_MK_U8(0x0114)
+#define SMIAPP_REG_U8_CSI2_10_TO_8_DT				SMIAPP_REG_MK_U8(0x0115)
+#define SMIAPP_REG_U8_CSI2_10_TO_7_DT				SMIAPP_REG_MK_U8(0x0116)
+#define SMIAPP_REG_U8_CSI2_10_TO_6_DT				SMIAPP_REG_MK_U8(0x0117)
+#define SMIAPP_REG_U8_CSI2_12_TO_8_DT				SMIAPP_REG_MK_U8(0x0118)
+#define SMIAPP_REG_U8_CSI2_12_TO_7_DT				SMIAPP_REG_MK_U8(0x0119)
+#define SMIAPP_REG_U8_CSI2_12_TO_6_DT				SMIAPP_REG_MK_U8(0x011a)
+#define SMIAPP_REG_U8_CSI2_14_TO_10_DT				SMIAPP_REG_MK_U8(0x011b)
+#define SMIAPP_REG_U8_CSI2_14_TO_8_DT				SMIAPP_REG_MK_U8(0x011c)
+#define SMIAPP_REG_U8_CSI2_16_TO_10_DT				SMIAPP_REG_MK_U8(0x011d)
+#define SMIAPP_REG_U8_CSI2_16_TO_8_DT				SMIAPP_REG_MK_U8(0x011e)
+#define SMIAPP_REG_U8_GAIN_MODE					SMIAPP_REG_MK_U8(0x0120)
+#define SMIAPP_REG_U16_VANA_VOLTAGE				SMIAPP_REG_MK_U16(0x0130)
+#define SMIAPP_REG_U16_VDIG_VOLTAGE				SMIAPP_REG_MK_U16(0x0132)
+#define SMIAPP_REG_U16_VIO_VOLTAGE				SMIAPP_REG_MK_U16(0x0134)
+#define SMIAPP_REG_U16_EXTCLK_FREQUENCY_MHZ			SMIAPP_REG_MK_U16(0x0136)
+#define SMIAPP_REG_U8_TEMP_SENSOR_CONTROL			SMIAPP_REG_MK_U8(0x0138)
+#define SMIAPP_REG_U8_TEMP_SENSOR_MODE				SMIAPP_REG_MK_U8(0x0139)
+#define SMIAPP_REG_U8_TEMP_SENSOR_OUTPUT			SMIAPP_REG_MK_U8(0x013a)
+#define SMIAPP_REG_U16_FINE_INTEGRATION_TIME			SMIAPP_REG_MK_U16(0x0200)
+#define SMIAPP_REG_U16_COARSE_INTEGRATION_TIME			SMIAPP_REG_MK_U16(0x0202)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_GLOBAL		SMIAPP_REG_MK_U16(0x0204)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_GREENR		SMIAPP_REG_MK_U16(0x0206)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_RED			SMIAPP_REG_MK_U16(0x0208)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_BLUE			SMIAPP_REG_MK_U16(0x020a)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_GREENB		SMIAPP_REG_MK_U16(0x020c)
+#define SMIAPP_REG_U16_DIGITAL_GAIN_GREENR			SMIAPP_REG_MK_U16(0x020e)
+#define SMIAPP_REG_U16_DIGITAL_GAIN_RED				SMIAPP_REG_MK_U16(0x0210)
+#define SMIAPP_REG_U16_DIGITAL_GAIN_BLUE			SMIAPP_REG_MK_U16(0x0212)
+#define SMIAPP_REG_U16_DIGITAL_GAIN_GREENB			SMIAPP_REG_MK_U16(0x0214)
+#define SMIAPP_REG_U16_VT_PIX_CLK_DIV				SMIAPP_REG_MK_U16(0x0300)
+#define SMIAPP_REG_U16_VT_SYS_CLK_DIV				SMIAPP_REG_MK_U16(0x0302)
+#define SMIAPP_REG_U16_PRE_PLL_CLK_DIV				SMIAPP_REG_MK_U16(0x0304)
+#define SMIAPP_REG_U16_PLL_MULTIPLIER				SMIAPP_REG_MK_U16(0x0306)
+#define SMIAPP_REG_U16_OP_PIX_CLK_DIV				SMIAPP_REG_MK_U16(0x0308)
+#define SMIAPP_REG_U16_OP_SYS_CLK_DIV				SMIAPP_REG_MK_U16(0x030a)
+#define SMIAPP_REG_U16_FRAME_LENGTH_LINES			SMIAPP_REG_MK_U16(0x0340)
+#define SMIAPP_REG_U16_LINE_LENGTH_PCK				SMIAPP_REG_MK_U16(0x0342)
+#define SMIAPP_REG_U16_X_ADDR_START				SMIAPP_REG_MK_U16(0x0344)
+#define SMIAPP_REG_U16_Y_ADDR_START				SMIAPP_REG_MK_U16(0x0346)
+#define SMIAPP_REG_U16_X_ADDR_END				SMIAPP_REG_MK_U16(0x0348)
+#define SMIAPP_REG_U16_Y_ADDR_END				SMIAPP_REG_MK_U16(0x034a)
+#define SMIAPP_REG_U16_X_OUTPUT_SIZE				SMIAPP_REG_MK_U16(0x034c)
+#define SMIAPP_REG_U16_Y_OUTPUT_SIZE				SMIAPP_REG_MK_U16(0x034e)
+#define SMIAPP_REG_U16_X_EVEN_INC				SMIAPP_REG_MK_U16(0x0380)
+#define SMIAPP_REG_U16_X_ODD_INC				SMIAPP_REG_MK_U16(0x0382)
+#define SMIAPP_REG_U16_Y_EVEN_INC				SMIAPP_REG_MK_U16(0x0384)
+#define SMIAPP_REG_U16_Y_ODD_INC				SMIAPP_REG_MK_U16(0x0386)
+#define SMIAPP_REG_U16_SCALING_MODE				SMIAPP_REG_MK_U16(0x0400)
+#define SMIAPP_REG_U16_SPATIAL_SAMPLING				SMIAPP_REG_MK_U16(0x0402)
+#define SMIAPP_REG_U16_SCALE_M					SMIAPP_REG_MK_U16(0x0404)
+#define SMIAPP_REG_U16_SCALE_N					SMIAPP_REG_MK_U16(0x0406)
+#define SMIAPP_REG_U16_DIGITAL_CROP_X_OFFSET			SMIAPP_REG_MK_U16(0x0408)
+#define SMIAPP_REG_U16_DIGITAL_CROP_Y_OFFSET			SMIAPP_REG_MK_U16(0x040a)
+#define SMIAPP_REG_U16_DIGITAL_CROP_IMAGE_WIDTH			SMIAPP_REG_MK_U16(0x040c)
+#define SMIAPP_REG_U16_DIGITAL_CROP_IMAGE_HEIGHT		SMIAPP_REG_MK_U16(0x040e)
+#define SMIAPP_REG_U16_COMPRESSION_MODE				SMIAPP_REG_MK_U16(0x0500)
+#define SMIAPP_REG_U16_TEST_PATTERN_MODE			SMIAPP_REG_MK_U16(0x0600)
+#define SMIAPP_REG_U16_TEST_DATA_RED				SMIAPP_REG_MK_U16(0x0602)
+#define SMIAPP_REG_U16_TEST_DATA_GREENR				SMIAPP_REG_MK_U16(0x0604)
+#define SMIAPP_REG_U16_TEST_DATA_BLUE				SMIAPP_REG_MK_U16(0x0606)
+#define SMIAPP_REG_U16_TEST_DATA_GREENB				SMIAPP_REG_MK_U16(0x0608)
+#define SMIAPP_REG_U16_HORIZONTAL_CURSOR_WIDTH			SMIAPP_REG_MK_U16(0x060a)
+#define SMIAPP_REG_U16_HORIZONTAL_CURSOR_POSITION		SMIAPP_REG_MK_U16(0x060c)
+#define SMIAPP_REG_U16_VERTICAL_CURSOR_WIDTH			SMIAPP_REG_MK_U16(0x060e)
+#define SMIAPP_REG_U16_VERTICAL_CURSOR_POSITION			SMIAPP_REG_MK_U16(0x0610)
+#define SMIAPP_REG_U16_FIFO_WATER_MARK_PIXELS			SMIAPP_REG_MK_U16(0x0700)
+#define SMIAPP_REG_U8_TCLK_POST					SMIAPP_REG_MK_U8(0x0800)
+#define SMIAPP_REG_U8_THS_PREPARE				SMIAPP_REG_MK_U8(0x0801)
+#define SMIAPP_REG_U8_THS_ZERO_MIN				SMIAPP_REG_MK_U8(0x0802)
+#define SMIAPP_REG_U8_THS_TRAIL					SMIAPP_REG_MK_U8(0x0803)
+#define SMIAPP_REG_U8_TCLK_TRAIL_MIN				SMIAPP_REG_MK_U8(0x0804)
+#define SMIAPP_REG_U8_TCLK_PREPARE				SMIAPP_REG_MK_U8(0x0805)
+#define SMIAPP_REG_U8_TCLK_ZERO					SMIAPP_REG_MK_U8(0x0806)
+#define SMIAPP_REG_U8_TLPX					SMIAPP_REG_MK_U8(0x0807)
+#define SMIAPP_REG_U8_DPHY_CTRL					SMIAPP_REG_MK_U8(0x0808)
+#define SMIAPP_REG_U32_REQUESTED_LINK_BIT_RATE_MBPS		SMIAPP_REG_MK_U32(0x0820)
+#define SMIAPP_REG_U8_BINNING_MODE				SMIAPP_REG_MK_U8(0x0900)
+#define SMIAPP_REG_U8_BINNING_TYPE				SMIAPP_REG_MK_U8(0x0901)
+#define SMIAPP_REG_U8_BINNING_WEIGHTING				SMIAPP_REG_MK_U8(0x0902)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_CTRL			SMIAPP_REG_MK_U8(0x0a00)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_STATUS			SMIAPP_REG_MK_U8(0x0a01)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_PAGE_SELECT		SMIAPP_REG_MK_U8(0x0a02)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_0			SMIAPP_REG_MK_U8(0x0a04)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_1			SMIAPP_REG_MK_U8(0x0a05)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_2			SMIAPP_REG_MK_U8(0x0a06)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_3			SMIAPP_REG_MK_U8(0x0a07)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_4			SMIAPP_REG_MK_U8(0x0a08)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_5			SMIAPP_REG_MK_U8(0x0a09)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_12		SMIAPP_REG_MK_U8(0x0a10)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_13		SMIAPP_REG_MK_U8(0x0a11)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_14		SMIAPP_REG_MK_U8(0x0a12)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_15		SMIAPP_REG_MK_U8(0x0a13)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_16		SMIAPP_REG_MK_U8(0x0a14)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_17		SMIAPP_REG_MK_U8(0x0a15)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_18		SMIAPP_REG_MK_U8(0x0a16)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_19		SMIAPP_REG_MK_U8(0x0a17)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_20		SMIAPP_REG_MK_U8(0x0a18)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_21		SMIAPP_REG_MK_U8(0x0a19)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_22		SMIAPP_REG_MK_U8(0x0a1a)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_23		SMIAPP_REG_MK_U8(0x0a1b)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_24		SMIAPP_REG_MK_U8(0x0a1c)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_25		SMIAPP_REG_MK_U8(0x0a1d)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_26		SMIAPP_REG_MK_U8(0x0a1e)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_27		SMIAPP_REG_MK_U8(0x0a1f)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_28		SMIAPP_REG_MK_U8(0x0a20)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_29		SMIAPP_REG_MK_U8(0x0a21)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_30		SMIAPP_REG_MK_U8(0x0a22)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_31		SMIAPP_REG_MK_U8(0x0a23)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_32		SMIAPP_REG_MK_U8(0x0a24)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_33		SMIAPP_REG_MK_U8(0x0a25)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_34		SMIAPP_REG_MK_U8(0x0a26)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_35		SMIAPP_REG_MK_U8(0x0a27)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_36		SMIAPP_REG_MK_U8(0x0a28)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_37		SMIAPP_REG_MK_U8(0x0a29)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_38		SMIAPP_REG_MK_U8(0x0a2a)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_39		SMIAPP_REG_MK_U8(0x0a2b)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_40		SMIAPP_REG_MK_U8(0x0a2c)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_41		SMIAPP_REG_MK_U8(0x0a2d)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_42		SMIAPP_REG_MK_U8(0x0a2e)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_43		SMIAPP_REG_MK_U8(0x0a2f)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_44		SMIAPP_REG_MK_U8(0x0a30)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_45		SMIAPP_REG_MK_U8(0x0a31)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_46		SMIAPP_REG_MK_U8(0x0a32)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_47		SMIAPP_REG_MK_U8(0x0a33)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_48		SMIAPP_REG_MK_U8(0x0a34)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_49		SMIAPP_REG_MK_U8(0x0a35)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_50		SMIAPP_REG_MK_U8(0x0a36)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_51		SMIAPP_REG_MK_U8(0x0a37)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_52		SMIAPP_REG_MK_U8(0x0a38)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_53		SMIAPP_REG_MK_U8(0x0a39)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_54		SMIAPP_REG_MK_U8(0x0a3a)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_55		SMIAPP_REG_MK_U8(0x0a3b)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_56		SMIAPP_REG_MK_U8(0x0a3c)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_57		SMIAPP_REG_MK_U8(0x0a3d)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_58		SMIAPP_REG_MK_U8(0x0a3e)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_59		SMIAPP_REG_MK_U8(0x0a3f)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_60		SMIAPP_REG_MK_U8(0x0a40)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_61		SMIAPP_REG_MK_U8(0x0a41)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_62		SMIAPP_REG_MK_U8(0x0a42)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_63		SMIAPP_REG_MK_U8(0x0a43)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_CTRL			SMIAPP_REG_MK_U8(0x0a44)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_STATUS			SMIAPP_REG_MK_U8(0x0a45)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_PAGE_SELECT		SMIAPP_REG_MK_U8(0x0a46)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_0			SMIAPP_REG_MK_U8(0x0a48)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_1			SMIAPP_REG_MK_U8(0x0a49)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_2			SMIAPP_REG_MK_U8(0x0a4a)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_3			SMIAPP_REG_MK_U8(0x0a4b)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_4			SMIAPP_REG_MK_U8(0x0a4c)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_5			SMIAPP_REG_MK_U8(0x0a4d)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_6			SMIAPP_REG_MK_U8(0x0a4e)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_7			SMIAPP_REG_MK_U8(0x0a4f)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_8			SMIAPP_REG_MK_U8(0x0a50)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_9			SMIAPP_REG_MK_U8(0x0a51)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_10		SMIAPP_REG_MK_U8(0x0a52)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_11		SMIAPP_REG_MK_U8(0x0a53)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_12		SMIAPP_REG_MK_U8(0x0a54)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_13		SMIAPP_REG_MK_U8(0x0a55)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_14		SMIAPP_REG_MK_U8(0x0a56)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_15		SMIAPP_REG_MK_U8(0x0a57)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_16		SMIAPP_REG_MK_U8(0x0a58)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_17		SMIAPP_REG_MK_U8(0x0a59)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_18		SMIAPP_REG_MK_U8(0x0a5a)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_19		SMIAPP_REG_MK_U8(0x0a5b)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_20		SMIAPP_REG_MK_U8(0x0a5c)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_21		SMIAPP_REG_MK_U8(0x0a5d)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_22		SMIAPP_REG_MK_U8(0x0a5e)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_23		SMIAPP_REG_MK_U8(0x0a5f)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_24		SMIAPP_REG_MK_U8(0x0a60)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_25		SMIAPP_REG_MK_U8(0x0a61)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_26		SMIAPP_REG_MK_U8(0x0a62)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_27		SMIAPP_REG_MK_U8(0x0a63)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_28		SMIAPP_REG_MK_U8(0x0a64)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_29		SMIAPP_REG_MK_U8(0x0a65)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_30		SMIAPP_REG_MK_U8(0x0a66)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_31		SMIAPP_REG_MK_U8(0x0a67)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_32		SMIAPP_REG_MK_U8(0x0a68)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_33		SMIAPP_REG_MK_U8(0x0a69)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_34		SMIAPP_REG_MK_U8(0x0a6a)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_35		SMIAPP_REG_MK_U8(0x0a6b)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_36		SMIAPP_REG_MK_U8(0x0a6c)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_37		SMIAPP_REG_MK_U8(0x0a6d)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_38		SMIAPP_REG_MK_U8(0x0a6e)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_39		SMIAPP_REG_MK_U8(0x0a6f)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_40		SMIAPP_REG_MK_U8(0x0a70)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_41		SMIAPP_REG_MK_U8(0x0a71)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_42		SMIAPP_REG_MK_U8(0x0a72)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_43		SMIAPP_REG_MK_U8(0x0a73)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_44		SMIAPP_REG_MK_U8(0x0a74)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_45		SMIAPP_REG_MK_U8(0x0a75)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_46		SMIAPP_REG_MK_U8(0x0a76)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_47		SMIAPP_REG_MK_U8(0x0a77)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_48		SMIAPP_REG_MK_U8(0x0a78)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_49		SMIAPP_REG_MK_U8(0x0a79)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_50		SMIAPP_REG_MK_U8(0x0a7a)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_51		SMIAPP_REG_MK_U8(0x0a7b)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_52		SMIAPP_REG_MK_U8(0x0a7c)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_53		SMIAPP_REG_MK_U8(0x0a7d)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_54		SMIAPP_REG_MK_U8(0x0a7e)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_55		SMIAPP_REG_MK_U8(0x0a7f)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_56		SMIAPP_REG_MK_U8(0x0a80)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_57		SMIAPP_REG_MK_U8(0x0a81)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_58		SMIAPP_REG_MK_U8(0x0a82)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_59		SMIAPP_REG_MK_U8(0x0a83)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_60		SMIAPP_REG_MK_U8(0x0a84)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_61		SMIAPP_REG_MK_U8(0x0a85)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_62		SMIAPP_REG_MK_U8(0x0a86)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_63		SMIAPP_REG_MK_U8(0x0a87)
+#define SMIAPP_REG_U8_SHADING_CORRECTION_ENABLE			SMIAPP_REG_MK_U8(0x0b00)
+#define SMIAPP_REG_U8_LUMINANCE_CORRECTION_LEVEL		SMIAPP_REG_MK_U8(0x0b01)
+#define SMIAPP_REG_U8_GREEN_IMBALANCE_FILTER_ENABLE		SMIAPP_REG_MK_U8(0x0b02)
+#define SMIAPP_REG_U8_GREEN_IMBALANCE_FILTER_WEIGHT		SMIAPP_REG_MK_U8(0x0b03)
+#define SMIAPP_REG_U8_BLACK_LEVEL_CORRECTION_ENABLE		SMIAPP_REG_MK_U8(0x0b04)
+#define SMIAPP_REG_U8_MAPPED_COUPLET_CORRECT_ENABLE		SMIAPP_REG_MK_U8(0x0b05)
+#define SMIAPP_REG_U8_SINGLE_DEFECT_CORRECT_ENABLE		SMIAPP_REG_MK_U8(0x0b06)
+#define SMIAPP_REG_U8_SINGLE_DEFECT_CORRECT_WEIGHT		SMIAPP_REG_MK_U8(0x0b07)
+#define SMIAPP_REG_U8_DYNAMIC_COUPLET_CORRECT_ENABLE		SMIAPP_REG_MK_U8(0x0b08)
+#define SMIAPP_REG_U8_DYNAMIC_COUPLET_CORRECT_WEIGHT		SMIAPP_REG_MK_U8(0x0b09)
+#define SMIAPP_REG_U8_COMBINED_DEFECT_CORRECT_ENABLE		SMIAPP_REG_MK_U8(0x0b0a)
+#define SMIAPP_REG_U8_COMBINED_DEFECT_CORRECT_WEIGHT		SMIAPP_REG_MK_U8(0x0b0b)
+#define SMIAPP_REG_U8_MODULE_SPECIFIC_CORRECTION_ENABLE		SMIAPP_REG_MK_U8(0x0b0c)
+#define SMIAPP_REG_U8_MODULE_SPECIFIC_CORRECTION_WEIGHT		SMIAPP_REG_MK_U8(0x0b0d)
+#define SMIAPP_REG_U8_MAPPED_LINE_DEFECT_CORRECT_ENABLE		SMIAPP_REG_MK_U8(0x0b0e)
+#define SMIAPP_REG_U8_MAPPED_LINE_DEFECT_CORRECT_ADJUST		SMIAPP_REG_MK_U8(0x0b0f)
+#define SMIAPP_REG_U8_MAPPED_COUPLET_CORRECT_ADJUST		SMIAPP_REG_MK_U8(0x0b10)
+#define SMIAPP_REG_U8_MAPPED_TRIPLET_DEFECT_CORRECT_ENABLE	SMIAPP_REG_MK_U8(0x0b11)
+#define SMIAPP_REG_U8_MAPPED_TRIPLET_DEFECT_CORRECT_ADJUST	SMIAPP_REG_MK_U8(0x0b12)
+#define SMIAPP_REG_U8_DYNAMIC_TRIPLET_DEFECT_CORRECT_ENABLE	SMIAPP_REG_MK_U8(0x0b13)
+#define SMIAPP_REG_U8_DYNAMIC_TRIPLET_DEFECT_CORRECT_ADJUST	SMIAPP_REG_MK_U8(0x0b14)
+#define SMIAPP_REG_U8_DYNAMIC_LINE_DEFECT_CORRECT_ENABLE	SMIAPP_REG_MK_U8(0x0b15)
+#define SMIAPP_REG_U8_DYNAMIC_LINE_DEFECT_CORRECT_ADJUST	SMIAPP_REG_MK_U8(0x0b16)
+#define SMIAPP_REG_U8_EDOF_MODE					SMIAPP_REG_MK_U8(0x0b80)
+#define SMIAPP_REG_U8_SHARPNESS					SMIAPP_REG_MK_U8(0x0b83)
+#define SMIAPP_REG_U8_DENOISING					SMIAPP_REG_MK_U8(0x0b84)
+#define SMIAPP_REG_U8_MODULE_SPECIFIC				SMIAPP_REG_MK_U8(0x0b85)
+#define SMIAPP_REG_U16_DEPTH_OF_FIELD				SMIAPP_REG_MK_U16(0x0b86)
+#define SMIAPP_REG_U16_FOCUS_DISTANCE				SMIAPP_REG_MK_U16(0x0b88)
+#define SMIAPP_REG_U8_ESTIMATION_MODE_CTRL			SMIAPP_REG_MK_U8(0x0b8a)
+#define SMIAPP_REG_U16_COLOUR_TEMPERATURE			SMIAPP_REG_MK_U16(0x0b8c)
+#define SMIAPP_REG_U16_ABSOLUTE_GAIN_GREENR			SMIAPP_REG_MK_U16(0x0b8e)
+#define SMIAPP_REG_U16_ABSOLUTE_GAIN_RED			SMIAPP_REG_MK_U16(0x0b90)
+#define SMIAPP_REG_U16_ABSOLUTE_GAIN_BLUE			SMIAPP_REG_MK_U16(0x0b92)
+#define SMIAPP_REG_U16_ABSOLUTE_GAIN_GREENB			SMIAPP_REG_MK_U16(0x0b94)
+#define SMIAPP_REG_U8_ESTIMATION_ZONE_MODE			SMIAPP_REG_MK_U8(0x0bc0)
+#define SMIAPP_REG_U16_FIXED_ZONE_WEIGHTING			SMIAPP_REG_MK_U16(0x0bc2)
+#define SMIAPP_REG_U16_CUSTOM_ZONE_X_START			SMIAPP_REG_MK_U16(0x0bc4)
+#define SMIAPP_REG_U16_CUSTOM_ZONE_Y_START			SMIAPP_REG_MK_U16(0x0bc6)
+#define SMIAPP_REG_U16_CUSTOM_ZONE_WIDTH			SMIAPP_REG_MK_U16(0x0bc8)
+#define SMIAPP_REG_U16_CUSTOM_ZONE_HEIGHT			SMIAPP_REG_MK_U16(0x0bca)
+#define SMIAPP_REG_U8_GLOBAL_RESET_CTRL1			SMIAPP_REG_MK_U8(0x0c00)
+#define SMIAPP_REG_U8_GLOBAL_RESET_CTRL2			SMIAPP_REG_MK_U8(0x0c01)
+#define SMIAPP_REG_U8_GLOBAL_RESET_MODE_CONFIG_1		SMIAPP_REG_MK_U8(0x0c02)
+#define SMIAPP_REG_U8_GLOBAL_RESET_MODE_CONFIG_2		SMIAPP_REG_MK_U8(0x0c03)
+#define SMIAPP_REG_U16_TRDY_CTRL				SMIAPP_REG_MK_U16(0x0c04)
+#define SMIAPP_REG_U16_TRDOUT_CTRL				SMIAPP_REG_MK_U16(0x0c06)
+#define SMIAPP_REG_U16_TSHUTTER_STROBE_DELAY_CTRL		SMIAPP_REG_MK_U16(0x0c08)
+#define SMIAPP_REG_U16_TSHUTTER_STROBE_WIDTH_CTRL		SMIAPP_REG_MK_U16(0x0c0a)
+#define SMIAPP_REG_U16_TFLASH_STROBE_DELAY_CTRL			SMIAPP_REG_MK_U16(0x0c0c)
+#define SMIAPP_REG_U16_TFLASH_STROBE_WIDTH_HIGH_CTRL		SMIAPP_REG_MK_U16(0x0c0e)
+#define SMIAPP_REG_U16_TGRST_INTERVAL_CTRL			SMIAPP_REG_MK_U16(0x0c10)
+#define SMIAPP_REG_U8_FLASH_STROBE_ADJUSTMENT			SMIAPP_REG_MK_U8(0x0c12)
+#define SMIAPP_REG_U16_FLASH_STROBE_START_POINT			SMIAPP_REG_MK_U16(0x0c14)
+#define SMIAPP_REG_U16_TFLASH_STROBE_DELAY_RS_CTRL		SMIAPP_REG_MK_U16(0x0c16)
+#define SMIAPP_REG_U16_TFLASH_STROBE_WIDTH_HIGH_RS_CTRL		SMIAPP_REG_MK_U16(0x0c18)
+#define SMIAPP_REG_U8_FLASH_MODE_RS				SMIAPP_REG_MK_U8(0x0c1a)
+#define SMIAPP_REG_U8_FLASH_TRIGGER_RS				SMIAPP_REG_MK_U8(0x0c1b)
+#define SMIAPP_REG_U8_FLASH_STATUS				SMIAPP_REG_MK_U8(0x0c1c)
+#define SMIAPP_REG_U8_SA_STROBE_MODE				SMIAPP_REG_MK_U8(0x0c1d)
+#define SMIAPP_REG_U16_SA_STROBE_START_POINT			SMIAPP_REG_MK_U16(0x0c1e)
+#define SMIAPP_REG_U16_TSA_STROBE_DELAY_CTRL			SMIAPP_REG_MK_U16(0x0c20)
+#define SMIAPP_REG_U16_TSA_STROBE_WIDTH_CTRL			SMIAPP_REG_MK_U16(0x0c22)
+#define SMIAPP_REG_U8_SA_STROBE_TRIGGER				SMIAPP_REG_MK_U8(0x0c24)
+#define SMIAPP_REG_U8_SPECIAL_ACTUATOR_STATUS			SMIAPP_REG_MK_U8(0x0c25)
+#define SMIAPP_REG_U16_TFLASH_STROBE_WIDTH2_HIGH_RS_CTRL	SMIAPP_REG_MK_U16(0x0c26)
+#define SMIAPP_REG_U16_TFLASH_STROBE_WIDTH_LOW_RS_CTRL		SMIAPP_REG_MK_U16(0x0c28)
+#define SMIAPP_REG_U8_TFLASH_STROBE_COUNT_RS_CTRL		SMIAPP_REG_MK_U8(0x0c2a)
+#define SMIAPP_REG_U8_TFLASH_STROBE_COUNT_CTRL			SMIAPP_REG_MK_U8(0x0c2b)
+#define SMIAPP_REG_U16_TFLASH_STROBE_WIDTH2_HIGH_CTRL		SMIAPP_REG_MK_U16(0x0c2c)
+#define SMIAPP_REG_U16_TFLASH_STROBE_WIDTH_LOW_CTRL		SMIAPP_REG_MK_U16(0x0c2e)
+#define SMIAPP_REG_U8_LOW_LEVEL_CTRL				SMIAPP_REG_MK_U8(0x0c80)
+#define SMIAPP_REG_U16_MAIN_TRIGGER_REF_POINT			SMIAPP_REG_MK_U16(0x0c82)
+#define SMIAPP_REG_U16_MAIN_TRIGGER_T3				SMIAPP_REG_MK_U16(0x0c84)
+#define SMIAPP_REG_U8_MAIN_TRIGGER_COUNT			SMIAPP_REG_MK_U8(0x0c86)
+#define SMIAPP_REG_U16_PHASE1_TRIGGER_T3			SMIAPP_REG_MK_U16(0x0c88)
+#define SMIAPP_REG_U8_PHASE1_TRIGGER_COUNT			SMIAPP_REG_MK_U8(0x0c8a)
+#define SMIAPP_REG_U16_PHASE2_TRIGGER_T3			SMIAPP_REG_MK_U16(0x0c8c)
+#define SMIAPP_REG_U8_PHASE2_TRIGGER_COUNT			SMIAPP_REG_MK_U8(0x0c8e)
+#define SMIAPP_REG_U8_MECH_SHUTTER_CTRL				SMIAPP_REG_MK_U8(0x0d00)
+#define SMIAPP_REG_U8_OPERATION_MODE				SMIAPP_REG_MK_U8(0x0d01)
+#define SMIAPP_REG_U8_ACT_STATE1				SMIAPP_REG_MK_U8(0x0d02)
+#define SMIAPP_REG_U8_ACT_STATE2				SMIAPP_REG_MK_U8(0x0d03)
+#define SMIAPP_REG_U16_FOCUS_CHANGE				SMIAPP_REG_MK_U16(0x0d80)
+#define SMIAPP_REG_U16_FOCUS_CHANGE_CONTROL			SMIAPP_REG_MK_U16(0x0d82)
+#define SMIAPP_REG_U16_FOCUS_CHANGE_NUMBER_PHASE1		SMIAPP_REG_MK_U16(0x0d84)
+#define SMIAPP_REG_U16_FOCUS_CHANGE_NUMBER_PHASE2		SMIAPP_REG_MK_U16(0x0d86)
+#define SMIAPP_REG_U8_STROBE_COUNT_PHASE1			SMIAPP_REG_MK_U8(0x0d88)
+#define SMIAPP_REG_U8_STROBE_COUNT_PHASE2			SMIAPP_REG_MK_U8(0x0d89)
+#define SMIAPP_REG_U8_POSITION					SMIAPP_REG_MK_U8(0x0d8a)
+#define SMIAPP_REG_U8_BRACKETING_LUT_CONTROL			SMIAPP_REG_MK_U8(0x0e00)
+#define SMIAPP_REG_U8_BRACKETING_LUT_MODE			SMIAPP_REG_MK_U8(0x0e01)
+#define SMIAPP_REG_U8_BRACKETING_LUT_ENTRY_CONTROL		SMIAPP_REG_MK_U8(0x0e02)
+#define SMIAPP_REG_U8_LUT_PARAMETERS_START			SMIAPP_REG_MK_U8(0x0e10)
+#define SMIAPP_REG_U8_LUT_PARAMETERS_END			SMIAPP_REG_MK_U8(0x0eff)
+#define SMIAPP_REG_U16_INTEGRATION_TIME_CAPABILITY		SMIAPP_REG_MK_U16(0x1000)
+#define SMIAPP_REG_U16_COARSE_INTEGRATION_TIME_MIN		SMIAPP_REG_MK_U16(0x1004)
+#define SMIAPP_REG_U16_COARSE_INTEGRATION_TIME_MAX_MARGIN	SMIAPP_REG_MK_U16(0x1006)
+#define SMIAPP_REG_U16_FINE_INTEGRATION_TIME_MIN		SMIAPP_REG_MK_U16(0x1008)
+#define SMIAPP_REG_U16_FINE_INTEGRATION_TIME_MAX_MARGIN		SMIAPP_REG_MK_U16(0x100a)
+#define SMIAPP_REG_U16_DIGITAL_GAIN_CAPABILITY			SMIAPP_REG_MK_U16(0x1080)
+#define SMIAPP_REG_U16_DIGITAL_GAIN_MIN				SMIAPP_REG_MK_U16(0x1084)
+#define SMIAPP_REG_U16_DIGITAL_GAIN_MAX				SMIAPP_REG_MK_U16(0x1086)
+#define SMIAPP_REG_U16_DIGITAL_GAIN_STEP_SIZE			SMIAPP_REG_MK_U16(0x1088)
+#define SMIAPP_REG_F32_MIN_EXT_CLK_FREQ_HZ			SMIAPP_REG_MK_F32(0x1100)
+#define SMIAPP_REG_F32_MAX_EXT_CLK_FREQ_HZ			SMIAPP_REG_MK_F32(0x1104)
+#define SMIAPP_REG_U16_MIN_PRE_PLL_CLK_DIV			SMIAPP_REG_MK_U16(0x1108)
+#define SMIAPP_REG_U16_MAX_PRE_PLL_CLK_DIV			SMIAPP_REG_MK_U16(0x110a)
+#define SMIAPP_REG_F32_MIN_PLL_IP_FREQ_HZ			SMIAPP_REG_MK_F32(0x110c)
+#define SMIAPP_REG_F32_MAX_PLL_IP_FREQ_HZ			SMIAPP_REG_MK_F32(0x1110)
+#define SMIAPP_REG_U16_MIN_PLL_MULTIPLIER			SMIAPP_REG_MK_U16(0x1114)
+#define SMIAPP_REG_U16_MAX_PLL_MULTIPLIER			SMIAPP_REG_MK_U16(0x1116)
+#define SMIAPP_REG_F32_MIN_PLL_OP_FREQ_HZ			SMIAPP_REG_MK_F32(0x1118)
+#define SMIAPP_REG_F32_MAX_PLL_OP_FREQ_HZ			SMIAPP_REG_MK_F32(0x111c)
+#define SMIAPP_REG_U16_MIN_VT_SYS_CLK_DIV			SMIAPP_REG_MK_U16(0x1120)
+#define SMIAPP_REG_U16_MAX_VT_SYS_CLK_DIV			SMIAPP_REG_MK_U16(0x1122)
+#define SMIAPP_REG_F32_MIN_VT_SYS_CLK_FREQ_HZ			SMIAPP_REG_MK_F32(0x1124)
+#define SMIAPP_REG_F32_MAX_VT_SYS_CLK_FREQ_HZ			SMIAPP_REG_MK_F32(0x1128)
+#define SMIAPP_REG_F32_MIN_VT_PIX_CLK_FREQ_HZ			SMIAPP_REG_MK_F32(0x112c)
+#define SMIAPP_REG_F32_MAX_VT_PIX_CLK_FREQ_HZ			SMIAPP_REG_MK_F32(0x1130)
+#define SMIAPP_REG_U16_MIN_VT_PIX_CLK_DIV			SMIAPP_REG_MK_U16(0x1134)
+#define SMIAPP_REG_U16_MAX_VT_PIX_CLK_DIV			SMIAPP_REG_MK_U16(0x1136)
+#define SMIAPP_REG_U16_MIN_FRAME_LENGTH_LINES			SMIAPP_REG_MK_U16(0x1140)
+#define SMIAPP_REG_U16_MAX_FRAME_LENGTH_LINES			SMIAPP_REG_MK_U16(0x1142)
+#define SMIAPP_REG_U16_MIN_LINE_LENGTH_PCK			SMIAPP_REG_MK_U16(0x1144)
+#define SMIAPP_REG_U16_MAX_LINE_LENGTH_PCK			SMIAPP_REG_MK_U16(0x1146)
+#define SMIAPP_REG_U16_MIN_LINE_BLANKING_PCK			SMIAPP_REG_MK_U16(0x1148)
+#define SMIAPP_REG_U16_MIN_FRAME_BLANKING_LINES			SMIAPP_REG_MK_U16(0x114a)
+#define SMIAPP_REG_U8_MIN_LINE_LENGTH_PCK_STEP_SIZE		SMIAPP_REG_MK_U8(0x114c)
+#define SMIAPP_REG_U16_MIN_OP_SYS_CLK_DIV			SMIAPP_REG_MK_U16(0x1160)
+#define SMIAPP_REG_U16_MAX_OP_SYS_CLK_DIV			SMIAPP_REG_MK_U16(0x1162)
+#define SMIAPP_REG_F32_MIN_OP_SYS_CLK_FREQ_HZ			SMIAPP_REG_MK_F32(0x1164)
+#define SMIAPP_REG_F32_MAX_OP_SYS_CLK_FREQ_HZ			SMIAPP_REG_MK_F32(0x1168)
+#define SMIAPP_REG_U16_MIN_OP_PIX_CLK_DIV			SMIAPP_REG_MK_U16(0x116c)
+#define SMIAPP_REG_U16_MAX_OP_PIX_CLK_DIV			SMIAPP_REG_MK_U16(0x116e)
+#define SMIAPP_REG_F32_MIN_OP_PIX_CLK_FREQ_HZ			SMIAPP_REG_MK_F32(0x1170)
+#define SMIAPP_REG_F32_MAX_OP_PIX_CLK_FREQ_HZ			SMIAPP_REG_MK_F32(0x1174)
+#define SMIAPP_REG_U16_X_ADDR_MIN				SMIAPP_REG_MK_U16(0x1180)
+#define SMIAPP_REG_U16_Y_ADDR_MIN				SMIAPP_REG_MK_U16(0x1182)
+#define SMIAPP_REG_U16_X_ADDR_MAX				SMIAPP_REG_MK_U16(0x1184)
+#define SMIAPP_REG_U16_Y_ADDR_MAX				SMIAPP_REG_MK_U16(0x1186)
+#define SMIAPP_REG_U16_MIN_X_OUTPUT_SIZE			SMIAPP_REG_MK_U16(0x1188)
+#define SMIAPP_REG_U16_MIN_Y_OUTPUT_SIZE			SMIAPP_REG_MK_U16(0x118a)
+#define SMIAPP_REG_U16_MAX_X_OUTPUT_SIZE			SMIAPP_REG_MK_U16(0x118c)
+#define SMIAPP_REG_U16_MAX_Y_OUTPUT_SIZE			SMIAPP_REG_MK_U16(0x118e)
+#define SMIAPP_REG_U16_MIN_EVEN_INC				SMIAPP_REG_MK_U16(0x11c0)
+#define SMIAPP_REG_U16_MAX_EVEN_INC				SMIAPP_REG_MK_U16(0x11c2)
+#define SMIAPP_REG_U16_MIN_ODD_INC				SMIAPP_REG_MK_U16(0x11c4)
+#define SMIAPP_REG_U16_MAX_ODD_INC				SMIAPP_REG_MK_U16(0x11c6)
+#define SMIAPP_REG_U16_SCALING_CAPABILITY			SMIAPP_REG_MK_U16(0x1200)
+#define SMIAPP_REG_U16_SCALER_M_MIN				SMIAPP_REG_MK_U16(0x1204)
+#define SMIAPP_REG_U16_SCALER_M_MAX				SMIAPP_REG_MK_U16(0x1206)
+#define SMIAPP_REG_U16_SCALER_N_MIN				SMIAPP_REG_MK_U16(0x1208)
+#define SMIAPP_REG_U16_SCALER_N_MAX				SMIAPP_REG_MK_U16(0x120a)
+#define SMIAPP_REG_U16_SPATIAL_SAMPLING_CAPABILITY		SMIAPP_REG_MK_U16(0x120c)
+#define SMIAPP_REG_U8_DIGITAL_CROP_CAPABILITY			SMIAPP_REG_MK_U8(0x120e)
+#define SMIAPP_REG_U16_COMPRESSION_CAPABILITY			SMIAPP_REG_MK_U16(0x1300)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_REDINRED			SMIAPP_REG_MK_U16(0x1400)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_GREENINRED		SMIAPP_REG_MK_U16(0x1402)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_BLUEINRED			SMIAPP_REG_MK_U16(0x1404)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_REDINGREEN		SMIAPP_REG_MK_U16(0x1406)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_GREENINGREEN		SMIAPP_REG_MK_U16(0x1408)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_BLUEINGREEN		SMIAPP_REG_MK_U16(0x140a)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_REDINBLUE			SMIAPP_REG_MK_U16(0x140c)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_GREENINBLUE		SMIAPP_REG_MK_U16(0x140e)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_BLUEINBLUE		SMIAPP_REG_MK_U16(0x1410)
+#define SMIAPP_REG_U16_FIFO_SIZE_PIXELS				SMIAPP_REG_MK_U16(0x1500)
+#define SMIAPP_REG_U8_FIFO_SUPPORT_CAPABILITY			SMIAPP_REG_MK_U8(0x1502)
+#define SMIAPP_REG_U8_DPHY_CTRL_CAPABILITY			SMIAPP_REG_MK_U8(0x1600)
+#define SMIAPP_REG_U8_CSI_LANE_MODE_CAPABILITY			SMIAPP_REG_MK_U8(0x1601)
+#define SMIAPP_REG_U8_CSI_SIGNALLING_MODE_CAPABILITY		SMIAPP_REG_MK_U8(0x1602)
+#define SMIAPP_REG_U8_FAST_STANDBY_CAPABILITY			SMIAPP_REG_MK_U8(0x1603)
+#define SMIAPP_REG_U8_CCI_ADDRESS_CONTROL_CAPABILITY		SMIAPP_REG_MK_U8(0x1604)
+#define SMIAPP_REG_U32_MAX_PER_LANE_BITRATE_1_LANE_MODE_MBPS	SMIAPP_REG_MK_U32(0x1608)
+#define SMIAPP_REG_U32_MAX_PER_LANE_BITRATE_2_LANE_MODE_MBPS	SMIAPP_REG_MK_U32(0x160c)
+#define SMIAPP_REG_U32_MAX_PER_LANE_BITRATE_3_LANE_MODE_MBPS	SMIAPP_REG_MK_U32(0x1610)
+#define SMIAPP_REG_U32_MAX_PER_LANE_BITRATE_4_LANE_MODE_MBPS	SMIAPP_REG_MK_U32(0x1614)
+#define SMIAPP_REG_U8_TEMP_SENSOR_CAPABILITY			SMIAPP_REG_MK_U8(0x1618)
+#define SMIAPP_REG_U16_MIN_FRAME_LENGTH_LINES_BIN		SMIAPP_REG_MK_U16(0x1700)
+#define SMIAPP_REG_U16_MAX_FRAME_LENGTH_LINES_BIN		SMIAPP_REG_MK_U16(0x1702)
+#define SMIAPP_REG_U16_MIN_LINE_LENGTH_PCK_BIN			SMIAPP_REG_MK_U16(0x1704)
+#define SMIAPP_REG_U16_MAX_LINE_LENGTH_PCK_BIN			SMIAPP_REG_MK_U16(0x1706)
+#define SMIAPP_REG_U16_MIN_LINE_BLANKING_PCK_BIN		SMIAPP_REG_MK_U16(0x1708)
+#define SMIAPP_REG_U16_FINE_INTEGRATION_TIME_MIN_BIN		SMIAPP_REG_MK_U16(0x170a)
+#define SMIAPP_REG_U16_FINE_INTEGRATION_TIME_MAX_MARGIN_BIN	SMIAPP_REG_MK_U16(0x170c)
+#define SMIAPP_REG_U8_BINNING_CAPABILITY			SMIAPP_REG_MK_U8(0x1710)
+#define SMIAPP_REG_U8_BINNING_WEIGHTING_CAPABILITY		SMIAPP_REG_MK_U8(0x1711)
+#define SMIAPP_REG_U8_BINNING_SUBTYPES				SMIAPP_REG_MK_U8(0x1712)
+#define SMIAPP_REG_U8_BINNING_TYPE_n(n)				SMIAPP_REG_MK_U8(0x1713 + (n)) /* 1 <= n <= 237 */
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_CAPABILITY		SMIAPP_REG_MK_U8(0x1800)
+#define SMIAPP_REG_U8_SHADING_CORRECTION_CAPABILITY		SMIAPP_REG_MK_U8(0x1900)
+#define SMIAPP_REG_U8_GREEN_IMBALANCE_CAPABILITY		SMIAPP_REG_MK_U8(0x1901)
+#define SMIAPP_REG_U8_BLACK_LEVEL_CAPABILITY			SMIAPP_REG_MK_U8(0x1902)
+#define SMIAPP_REG_U8_MODULE_SPECIFIC_CORRECTION_CAPABILITY	SMIAPP_REG_MK_U8(0x1903)
+#define SMIAPP_REG_U16_DEFECT_CORRECTION_CAPABILITY		SMIAPP_REG_MK_U16(0x1904)
+#define SMIAPP_REG_U16_DEFECT_CORRECTION_CAPABILITY_2		SMIAPP_REG_MK_U16(0x1906)
+#define SMIAPP_REG_U8_EDOF_CAPABILITY				SMIAPP_REG_MK_U8(0x1980)
+#define SMIAPP_REG_U8_ESTIMATION_FRAMES				SMIAPP_REG_MK_U8(0x1981)
+#define SMIAPP_REG_U8_SUPPORTS_SHARPNESS_ADJ			SMIAPP_REG_MK_U8(0x1982)
+#define SMIAPP_REG_U8_SUPPORTS_DENOISING_ADJ			SMIAPP_REG_MK_U8(0x1983)
+#define SMIAPP_REG_U8_SUPPORTS_MODULE_SPECIFIC_ADJ		SMIAPP_REG_MK_U8(0x1984)
+#define SMIAPP_REG_U8_SUPPORTS_DEPTH_OF_FIELD_ADJ		SMIAPP_REG_MK_U8(0x1985)
+#define SMIAPP_REG_U8_SUPPORTS_FOCUS_DISTANCE_ADJ		SMIAPP_REG_MK_U8(0x1986)
+#define SMIAPP_REG_U8_COLOUR_FEEDBACK_CAPABILITY		SMIAPP_REG_MK_U8(0x1987)
+#define SMIAPP_REG_U8_EDOF_SUPPORT_AB_NXM			SMIAPP_REG_MK_U8(0x1988)
+#define SMIAPP_REG_U8_ESTIMATION_MODE_CAPABILITY		SMIAPP_REG_MK_U8(0x19c0)
+#define SMIAPP_REG_U8_ESTIMATION_ZONE_CAPABILITY		SMIAPP_REG_MK_U8(0x19c1)
+#define SMIAPP_REG_U16_EST_DEPTH_OF_FIELD			SMIAPP_REG_MK_U16(0x19c2)
+#define SMIAPP_REG_U16_EST_FOCUS_DISTANCE			SMIAPP_REG_MK_U16(0x19c4)
+#define SMIAPP_REG_U16_CAPABILITY_TRDY_MIN			SMIAPP_REG_MK_U16(0x1a00)
+#define SMIAPP_REG_U8_FLASH_MODE_CAPABILITY			SMIAPP_REG_MK_U8(0x1a02)
+#define SMIAPP_REG_U16_MECH_SHUT_AND_ACT_START_ADDR		SMIAPP_REG_MK_U16(0x1b02)
+#define SMIAPP_REG_U8_ACTUATOR_CAPABILITY			SMIAPP_REG_MK_U8(0x1b04)
+#define SMIAPP_REG_U16_ACTUATOR_TYPE				SMIAPP_REG_MK_U16(0x1b40)
+#define SMIAPP_REG_U8_AF_DEVICE_ADDRESS				SMIAPP_REG_MK_U8(0x1b42)
+#define SMIAPP_REG_U16_FOCUS_CHANGE_ADDRESS			SMIAPP_REG_MK_U16(0x1b44)
+#define SMIAPP_REG_U8_BRACKETING_LUT_CAPABILITY_1		SMIAPP_REG_MK_U8(0x1c00)
+#define SMIAPP_REG_U8_BRACKETING_LUT_CAPABILITY_2		SMIAPP_REG_MK_U8(0x1c01)
+#define SMIAPP_REG_U8_BRACKETING_LUT_SIZE			SMIAPP_REG_MK_U8(0x1c02)
diff --git a/drivers/media/i2c/smiapp/smiapp-reg.h b/drivers/media/i2c/smiapp/smiapp-reg.h
new file mode 100644
index 000000000000..54568ca2fe6d
--- /dev/null
+++ b/drivers/media/i2c/smiapp/smiapp-reg.h
@@ -0,0 +1,122 @@
+/*
+ * drivers/media/i2c/smiapp/smiapp-reg.h
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2011--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#ifndef __SMIAPP_REG_H_
+#define __SMIAPP_REG_H_
+
+#include "smiapp-reg-defs.h"
+
+/* Bits for above register */
+#define SMIAPP_IMAGE_ORIENTATION_HFLIP		(1 << 0)
+#define SMIAPP_IMAGE_ORIENTATION_VFLIP		(1 << 1)
+
+#define SMIAPP_DATA_TRANSFER_IF_1_CTRL_EN		(1 << 0)
+#define SMIAPP_DATA_TRANSFER_IF_1_CTRL_RD_EN		(0 << 1)
+#define SMIAPP_DATA_TRANSFER_IF_1_CTRL_WR_EN		(1 << 1)
+#define SMIAPP_DATA_TRANSFER_IF_1_CTRL_ERR_CLEAR	(1 << 2)
+#define SMIAPP_DATA_TRANSFER_IF_1_STATUS_RD_READY	(1 << 0)
+#define SMIAPP_DATA_TRANSFER_IF_1_STATUS_WR_READY	(1 << 1)
+#define SMIAPP_DATA_TRANSFER_IF_1_STATUS_EDATA		(1 << 2)
+#define SMIAPP_DATA_TRANSFER_IF_1_STATUS_EUSAGE		(1 << 3)
+
+#define SMIAPP_SOFTWARE_RESET				(1 << 0)
+
+#define SMIAPP_FLASH_MODE_CAPABILITY_SINGLE_STROBE	(1 << 0)
+#define SMIAPP_FLASH_MODE_CAPABILITY_MULTIPLE_STROBE	(1 << 1)
+
+#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_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
+
+/* Image statistics registers */
+/* Registers 0x2000 to 0x2fff are reserved for future
+ * use for statistics features.
+ */
+
+/* Manufacturer Specific Registers: 0x3000 to 0x3fff
+ * The manufacturer specifies these as a black box.
+ */
+
+#endif /* __SMIAPP_REG_H_ */
diff --git a/drivers/media/i2c/smiapp/smiapp-regs.c b/drivers/media/i2c/smiapp/smiapp-regs.c
new file mode 100644
index 000000000000..70e0d8db0130
--- /dev/null
+++ b/drivers/media/i2c/smiapp/smiapp-regs.c
@@ -0,0 +1,273 @@
+/*
+ * drivers/media/i2c/smiapp/smiapp-regs.c
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2011--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#include <linux/delay.h>
+#include <linux/i2c.h>
+
+#include "smiapp.h"
+#include "smiapp-regs.h"
+
+static uint32_t float_to_u32_mul_1000000(struct i2c_client *client,
+					 uint32_t phloat)
+{
+	int32_t exp;
+	uint64_t 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 ____smiapp_read(struct smiapp_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[4];
+	u16 offset = reg;
+	int r;
+
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = 2;
+	msg.buf = data;
+
+	/* high byte goes out first */
+	data[0] = (u8) (offset >> 8);
+	data[1] = (u8) offset;
+	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;
+	r = i2c_transfer(client->adapter, &msg, 1);
+	if (r != 1) {
+		if (r >= 0)
+			r = -EBUSY;
+		goto err;
+	}
+
+	*val = 0;
+	/* high byte comes first */
+	switch (len) {
+	case SMIA_REG_32BIT:
+		*val = (data[0] << 24) + (data[1] << 16) + (data[2] << 8) +
+			data[3];
+		break;
+	case SMIA_REG_16BIT:
+		*val = (data[0] << 8) + data[1];
+		break;
+	case SMIA_REG_8BIT:
+		*val = data[0];
+		break;
+	default:
+		BUG();
+	}
+
+	return 0;
+
+err:
+	dev_err(&client->dev, "read from offset 0x%x error %d\n", offset, r);
+
+	return r;
+}
+
+/* Read a register using 8-bit access only. */
+static int ____smiapp_read_8only(struct smiapp_sensor *sensor, u16 reg,
+				 u16 len, u32 *val)
+{
+	unsigned int i;
+	int rval;
+
+	*val = 0;
+
+	for (i = 0; i < len; i++) {
+		u32 val8;
+
+		rval = ____smiapp_read(sensor, reg + i, 1, &val8);
+		if (rval < 0)
+			return rval;
+		*val |= val8 << ((len - i - 1) << 3);
+	}
+
+	return 0;
+}
+
+/*
+ * Read a 8/16/32-bit i2c register.  The value is returned in 'val'.
+ * Returns zero if successful, or non-zero otherwise.
+ */
+static int __smiapp_read(struct smiapp_sensor *sensor, u32 reg, u32 *val,
+			 bool only8)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	unsigned int len = (u8)(reg >> 16);
+	int rval;
+
+	if (len != SMIA_REG_8BIT && len != SMIA_REG_16BIT
+	    && len != SMIA_REG_32BIT)
+		return -EINVAL;
+
+	if (smiapp_quirk_reg(sensor, reg, val))
+		goto found_quirk;
+
+	if (len == SMIA_REG_8BIT && !only8)
+		rval = ____smiapp_read(sensor, (u16)reg, len, val);
+	else
+		rval = ____smiapp_read_8only(sensor, (u16)reg, len, val);
+	if (rval < 0)
+		return rval;
+
+found_quirk:
+	if (reg & SMIA_REG_FLAG_FLOAT)
+		*val = float_to_u32_mul_1000000(client, *val);
+
+	return 0;
+}
+
+int smiapp_read(struct smiapp_sensor *sensor, u32 reg, u32 *val)
+{
+	return __smiapp_read(
+		sensor, reg, val,
+		smiapp_needs_quirk(sensor,
+				   SMIAPP_QUIRK_FLAG_8BIT_READ_ONLY));
+}
+
+int smiapp_read_8only(struct smiapp_sensor *sensor, u32 reg, u32 *val)
+{
+	return __smiapp_read(sensor, reg, val, true);
+}
+
+/*
+ * Write to a 8/16-bit register.
+ * Returns zero if successful, or non-zero otherwise.
+ */
+int smiapp_write(struct smiapp_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 retries;
+	unsigned int flags = reg >> 24;
+	unsigned int len = (u8)(reg >> 16);
+	u16 offset = reg;
+	int r;
+
+	if ((len != SMIA_REG_8BIT && len != SMIA_REG_16BIT &&
+	     len != SMIA_REG_32BIT) || flags)
+		return -EINVAL;
+
+	msg.addr = client->addr;
+	msg.flags = 0; /* Write */
+	msg.len = 2 + len;
+	msg.buf = data;
+
+	/* high byte goes out first */
+	data[0] = (u8) (reg >> 8);
+	data[1] = (u8) (reg & 0xff);
+
+	switch (len) {
+	case SMIA_REG_8BIT:
+		data[2] = val;
+		break;
+	case SMIA_REG_16BIT:
+		data[2] = val >> 8;
+		data[3] = val;
+		break;
+	case SMIA_REG_32BIT:
+		data[2] = val >> 24;
+		data[3] = val >> 16;
+		data[4] = val >> 8;
+		data[5] = val;
+		break;
+	default:
+		BUG();
+	}
+
+	for (retries = 0; retries < 5; 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) {
+			if (retries)
+				dev_err(&client->dev,
+					"sensor i2c stall encountered. "
+					"retries: %d\n", retries);
+			return 0;
+		}
+
+		usleep_range(2000, 2000);
+	}
+
+	dev_err(&client->dev,
+		"wrote 0x%x to offset 0x%x error %d\n", val, offset, r);
+
+	return r;
+}
diff --git a/drivers/media/i2c/smiapp/smiapp-regs.h b/drivers/media/i2c/smiapp/smiapp-regs.h
new file mode 100644
index 000000000000..7f9013b47971
--- /dev/null
+++ b/drivers/media/i2c/smiapp/smiapp-regs.h
@@ -0,0 +1,49 @@
+/*
+ * include/media/smiapp/smiapp-regs.h
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2011--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#ifndef SMIAPP_REGS_H
+#define SMIAPP_REGS_H
+
+#include <linux/i2c.h>
+#include <linux/types.h>
+
+/* Use upper 8 bits of the type field for flags */
+#define SMIA_REG_FLAG_FLOAT		(1 << 24)
+
+#define SMIA_REG_8BIT			1
+#define SMIA_REG_16BIT			2
+#define SMIA_REG_32BIT			4
+struct smia_reg {
+	u16 type;
+	u16 reg;			/* 16-bit offset */
+	u32 val;			/* 8/16/32-bit value */
+};
+
+struct smiapp_sensor;
+
+int smiapp_read(struct smiapp_sensor *sensor, u32 reg, u32 *val);
+int smiapp_read_8only(struct smiapp_sensor *sensor, u32 reg, u32 *val);
+int smiapp_write(struct smiapp_sensor *sensor, u32 reg, u32 val);
+
+#endif
diff --git a/drivers/media/i2c/smiapp/smiapp.h b/drivers/media/i2c/smiapp/smiapp.h
new file mode 100644
index 000000000000..4182a695ab53
--- /dev/null
+++ b/drivers/media/i2c/smiapp/smiapp.h
@@ -0,0 +1,252 @@
+/*
+ * drivers/media/i2c/smiapp/smiapp.h
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2010--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#ifndef __SMIAPP_PRIV_H_
+#define __SMIAPP_PRIV_H_
+
+#include <linux/mutex.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-subdev.h>
+#include <media/smiapp.h>
+
+#include "smiapp-pll.h"
+#include "smiapp-reg.h"
+#include "smiapp-regs.h"
+#include "smiapp-quirk.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))
+
+#include "smiapp-limits.h"
+
+struct smiapp_quirk;
+
+#define SMIAPP_MODULE_IDENT_FLAG_REV_LE		(1 << 0)
+
+struct smiapp_module_ident {
+	u8 manufacturer_id;
+	u16 model_id;
+	u8 revision_number_major;
+
+	u8 flags;
+
+	char *name;
+	const struct smiapp_quirk *quirk;
+};
+
+struct smiapp_module_info {
+	u32 manufacturer_id;
+	u32 model_id;
+	u32 revision_number_major;
+	u32 revision_number_minor;
+
+	u32 module_year;
+	u32 module_month;
+	u32 module_day;
+
+	u32 sensor_manufacturer_id;
+	u32 sensor_model_id;
+	u32 sensor_revision_number;
+	u32 sensor_firmware_version;
+
+	u32 smia_version;
+	u32 smiapp_version;
+
+	u32 smiapp_profile;
+
+	char *name;
+	const struct smiapp_quirk *quirk;
+};
+
+#define SMIAPP_IDENT_FQ(manufacturer, model, rev, fl, _name, _quirk)	\
+	{ .manufacturer_id = manufacturer,				\
+	  .model_id = model,						\
+	  .revision_number_major = rev,					\
+	  .flags = fl,							\
+	  .name = _name,						\
+	  .quirk = _quirk, }
+
+#define SMIAPP_IDENT_LQ(manufacturer, model, rev, _name, _quirk)	\
+	{ .manufacturer_id = manufacturer,				\
+	  .model_id = model,						\
+	  .revision_number_major = rev,					\
+	  .flags = SMIAPP_MODULE_IDENT_FLAG_REV_LE,			\
+	  .name = _name,						\
+	  .quirk = _quirk, }
+
+#define SMIAPP_IDENT_L(manufacturer, model, rev, _name)			\
+	{ .manufacturer_id = manufacturer,				\
+	  .model_id = model,						\
+	  .revision_number_major = rev,					\
+	  .flags = SMIAPP_MODULE_IDENT_FLAG_REV_LE,			\
+	  .name = _name, }
+
+#define SMIAPP_IDENT_Q(manufacturer, model, rev, _name, _quirk)		\
+	{ .manufacturer_id = manufacturer,				\
+	  .model_id = model,						\
+	  .revision_number_major = rev,					\
+	  .flags = 0,							\
+	  .name = _name,						\
+	  .quirk = _quirk, }
+
+#define SMIAPP_IDENT(manufacturer, model, rev, _name)			\
+	{ .manufacturer_id = manufacturer,				\
+	  .model_id = model,						\
+	  .revision_number_major = rev,					\
+	  .flags = 0,							\
+	  .name = _name, }
+
+struct smiapp_reg_limits {
+	u32 addr;
+	char *what;
+};
+
+extern struct smiapp_reg_limits smiapp_reg_limits[];
+
+struct smiapp_csi_data_format {
+	u32 code;
+	u8 width;
+	u8 compressed;
+	u8 pixel_order;
+};
+
+#define SMIAPP_SUBDEVS			3
+
+#define SMIAPP_PA_PAD_SRC		0
+#define SMIAPP_PAD_SINK			0
+#define SMIAPP_PAD_SRC			1
+#define SMIAPP_PADS			2
+
+struct smiapp_binning_subtype {
+	u8 horizontal:4;
+	u8 vertical:4;
+} __packed;
+
+struct smiapp_subdev {
+	struct v4l2_subdev sd;
+	struct media_pad pads[2];
+	struct v4l2_rect sink_fmt;
+	struct v4l2_rect crop[2];
+	struct v4l2_rect compose; /* compose on sink */
+	unsigned short sink_pad;
+	unsigned short source_pad;
+	int npads;
+	struct smiapp_sensor *sensor;
+	struct v4l2_ctrl_handler ctrl_handler;
+};
+
+/*
+ * struct smiapp_sensor - Main device structure
+ */
+struct smiapp_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. The exception to this rule is the power_mutex
+	 * below.
+	 */
+	struct mutex mutex;
+	/*
+	 * power_mutex is used to serialise power management related
+	 * activities. Acquiring "mutex" at that time isn't necessary
+	 * since there are no other users anyway.
+	 */
+	struct mutex power_mutex;
+	struct smiapp_subdev ssds[SMIAPP_SUBDEVS];
+	u32 ssds_used;
+	struct smiapp_subdev *src;
+	struct smiapp_subdev *binner;
+	struct smiapp_subdev *scaler;
+	struct smiapp_subdev *pixel_array;
+	struct smiapp_platform_data *platform_data;
+	struct regulator *vana;
+	struct clk *ext_clk;
+	u32 limits[SMIAPP_LIMIT_LAST];
+	u8 nbinning_subtypes;
+	struct smiapp_binning_subtype binning_subtypes[SMIAPP_BINNING_SUBTYPES];
+	u32 mbus_frame_fmts;
+	const struct smiapp_csi_data_format *csi_format;
+	const struct smiapp_csi_data_format *internal_csi_format;
+	u32 default_mbus_frame_fmts;
+	int default_pixel_order;
+
+	u8 binning_horizontal;
+	u8 binning_vertical;
+
+	u8 scale_m;
+	u8 scaling_mode;
+
+	u8 hvflip_inv_mask; /* H/VFLIP inversion due to sensor orientation */
+	u8 flash_capability;
+	u8 frame_skip;
+
+	int power_count;
+
+	bool streaming;
+	bool dev_init_done;
+
+	u8 *nvm;		/* nvm memory buffer */
+	unsigned int nvm_size;	/* bytes */
+
+	struct smiapp_module_info minfo;
+
+	struct smiapp_pll pll;
+
+	/* Pixel array controls */
+	struct v4l2_ctrl *analog_gain;
+	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;
+	/* src controls */
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *pixel_rate_csi;
+};
+
+#define to_smiapp_subdev(_sd)				\
+	container_of(_sd, struct smiapp_subdev, sd)
+
+#define to_smiapp_sensor(_sd)	\
+	(to_smiapp_subdev(_sd)->sensor)
+
+#endif /* __SMIAPP_PRIV_H_ */
diff --git a/drivers/media/i2c/soc_camera/Kconfig b/drivers/media/i2c/soc_camera/Kconfig
new file mode 100644
index 000000000000..6dff2b7ad520
--- /dev/null
+++ b/drivers/media/i2c/soc_camera/Kconfig
@@ -0,0 +1,89 @@
+comment "soc_camera sensor drivers"
+
+config SOC_CAMERA_IMX074
+	tristate "imx074 support"
+	depends on SOC_CAMERA && I2C
+	help
+	  This driver supports IMX074 cameras from Sony
+
+config SOC_CAMERA_MT9M001
+	tristate "mt9m001 support"
+	depends on SOC_CAMERA && I2C
+	select GPIO_PCA953X if MT9M001_PCA9536_SWITCH
+	help
+	  This driver supports MT9M001 cameras from Micron, monochrome
+	  and colour models.
+
+config SOC_CAMERA_MT9M111
+	tristate "mt9m111, mt9m112 and mt9m131 support"
+	depends on SOC_CAMERA && I2C
+	help
+	  This driver supports MT9M111, MT9M112 and MT9M131 cameras from
+	  Micron/Aptina
+
+config SOC_CAMERA_MT9T031
+	tristate "mt9t031 support"
+	depends on SOC_CAMERA && I2C
+	help
+	  This driver supports MT9T031 cameras from Micron.
+
+config SOC_CAMERA_MT9T112
+	tristate "mt9t112 support"
+	depends on SOC_CAMERA && I2C
+	help
+	  This driver supports MT9T112 cameras from Aptina.
+
+config SOC_CAMERA_MT9V022
+	tristate "mt9v022 and mt9v024 support"
+	depends on SOC_CAMERA && I2C
+	select GPIO_PCA953X if MT9V022_PCA9536_SWITCH
+	help
+	  This driver supports MT9V022 cameras from Micron
+
+config SOC_CAMERA_OV2640
+	tristate "ov2640 camera support"
+	depends on SOC_CAMERA && I2C
+	help
+	  This is a ov2640 camera driver
+
+config SOC_CAMERA_OV5642
+	tristate "ov5642 camera support"
+	depends on SOC_CAMERA && I2C
+	help
+	  This is a V4L2 camera driver for the OmniVision OV5642 sensor
+
+config SOC_CAMERA_OV6650
+	tristate "ov6650 sensor support"
+	depends on SOC_CAMERA && I2C
+	---help---
+	  This is a V4L2 SoC camera driver for the OmniVision OV6650 sensor
+
+config SOC_CAMERA_OV772X
+	tristate "ov772x camera support"
+	depends on SOC_CAMERA && I2C
+	help
+	  This is a ov772x camera driver
+
+config SOC_CAMERA_OV9640
+	tristate "ov9640 camera support"
+	depends on SOC_CAMERA && I2C
+	help
+	  This is a ov9640 camera driver
+
+config SOC_CAMERA_OV9740
+	tristate "ov9740 camera support"
+	depends on SOC_CAMERA && I2C
+	help
+	  This is a ov9740 camera driver
+
+config SOC_CAMERA_RJ54N1
+	tristate "rj54n1cb0c support"
+	depends on SOC_CAMERA && I2C
+	help
+	  This is a rj54n1cb0c video driver
+
+config SOC_CAMERA_TW9910
+	tristate "tw9910 support"
+	depends on SOC_CAMERA && I2C
+	help
+	  This is a tw9910 video driver
diff --git a/drivers/media/i2c/soc_camera/Makefile b/drivers/media/i2c/soc_camera/Makefile
new file mode 100644
index 000000000000..d0421feaa796
--- /dev/null
+++ b/drivers/media/i2c/soc_camera/Makefile
@@ -0,0 +1,14 @@
+obj-$(CONFIG_SOC_CAMERA_IMX074)		+= imx074.o
+obj-$(CONFIG_SOC_CAMERA_MT9M001)	+= mt9m001.o
+obj-$(CONFIG_SOC_CAMERA_MT9M111)	+= mt9m111.o
+obj-$(CONFIG_SOC_CAMERA_MT9T031)	+= mt9t031.o
+obj-$(CONFIG_SOC_CAMERA_MT9T112)	+= mt9t112.o
+obj-$(CONFIG_SOC_CAMERA_MT9V022)	+= mt9v022.o
+obj-$(CONFIG_SOC_CAMERA_OV2640)		+= ov2640.o
+obj-$(CONFIG_SOC_CAMERA_OV5642)		+= ov5642.o
+obj-$(CONFIG_SOC_CAMERA_OV6650)		+= ov6650.o
+obj-$(CONFIG_SOC_CAMERA_OV772X)		+= ov772x.o
+obj-$(CONFIG_SOC_CAMERA_OV9640)		+= ov9640.o
+obj-$(CONFIG_SOC_CAMERA_OV9740)		+= ov9740.o
+obj-$(CONFIG_SOC_CAMERA_RJ54N1)		+= rj54n1cb0c.o
+obj-$(CONFIG_SOC_CAMERA_TW9910)		+= tw9910.o
diff --git a/drivers/media/i2c/soc_camera/imx074.c b/drivers/media/i2c/soc_camera/imx074.c
new file mode 100644
index 000000000000..f8534eec9de9
--- /dev/null
+++ b/drivers/media/i2c/soc_camera/imx074.c
@@ -0,0 +1,495 @@
+/*
+ * Driver for IMX074 CMOS Image Sensor from Sony
+ *
+ * Copyright (C) 2010, Guennadi Liakhovetski <g.liakhovetski@gmx.de>
+ *
+ * Partially inspired by the IMX074 driver from the Android / MSM tree
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/v4l2-mediabus.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <linux/module.h>
+
+#include <media/soc_camera.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-chip-ident.h>
+
+/* IMX074 registers */
+
+#define MODE_SELECT			0x0100
+#define IMAGE_ORIENTATION		0x0101
+#define GROUPED_PARAMETER_HOLD		0x0104
+
+/* Integration Time */
+#define COARSE_INTEGRATION_TIME_HI	0x0202
+#define COARSE_INTEGRATION_TIME_LO	0x0203
+/* Gain */
+#define ANALOGUE_GAIN_CODE_GLOBAL_HI	0x0204
+#define ANALOGUE_GAIN_CODE_GLOBAL_LO	0x0205
+
+/* PLL registers */
+#define PRE_PLL_CLK_DIV			0x0305
+#define PLL_MULTIPLIER			0x0307
+#define PLSTATIM			0x302b
+#define VNDMY_ABLMGSHLMT		0x300a
+#define Y_OPBADDR_START_DI		0x3014
+/* mode setting */
+#define FRAME_LENGTH_LINES_HI		0x0340
+#define FRAME_LENGTH_LINES_LO		0x0341
+#define LINE_LENGTH_PCK_HI		0x0342
+#define LINE_LENGTH_PCK_LO		0x0343
+#define YADDR_START			0x0347
+#define YADDR_END			0x034b
+#define X_OUTPUT_SIZE_MSB		0x034c
+#define X_OUTPUT_SIZE_LSB		0x034d
+#define Y_OUTPUT_SIZE_MSB		0x034e
+#define Y_OUTPUT_SIZE_LSB		0x034f
+#define X_EVEN_INC			0x0381
+#define X_ODD_INC			0x0383
+#define Y_EVEN_INC			0x0385
+#define Y_ODD_INC			0x0387
+
+#define HMODEADD			0x3001
+#define VMODEADD			0x3016
+#define VAPPLINE_START			0x3069
+#define VAPPLINE_END			0x306b
+#define SHUTTER				0x3086
+#define HADDAVE				0x30e8
+#define LANESEL				0x3301
+
+/* IMX074 supported geometry */
+#define IMX074_WIDTH			1052
+#define IMX074_HEIGHT			780
+
+/* IMX074 has only one fixed colorspace per pixelcode */
+struct imx074_datafmt {
+	enum v4l2_mbus_pixelcode	code;
+	enum v4l2_colorspace		colorspace;
+};
+
+struct imx074 {
+	struct v4l2_subdev		subdev;
+	const struct imx074_datafmt	*fmt;
+};
+
+static const struct imx074_datafmt imx074_colour_fmts[] = {
+	{V4L2_MBUS_FMT_SBGGR8_1X8, V4L2_COLORSPACE_SRGB},
+};
+
+static struct imx074 *to_imx074(const struct i2c_client *client)
+{
+	return container_of(i2c_get_clientdata(client), struct imx074, subdev);
+}
+
+/* Find a data format by a pixel code in an array */
+static const struct imx074_datafmt *imx074_find_datafmt(enum v4l2_mbus_pixelcode code)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(imx074_colour_fmts); i++)
+		if (imx074_colour_fmts[i].code == code)
+			return imx074_colour_fmts + i;
+
+	return NULL;
+}
+
+static int reg_write(struct i2c_client *client, const u16 addr, const u8 data)
+{
+	struct i2c_adapter *adap = client->adapter;
+	struct i2c_msg msg;
+	unsigned char tx[3];
+	int ret;
+
+	msg.addr = client->addr;
+	msg.buf = tx;
+	msg.len = 3;
+	msg.flags = 0;
+
+	tx[0] = addr >> 8;
+	tx[1] = addr & 0xff;
+	tx[2] = data;
+
+	ret = i2c_transfer(adap, &msg, 1);
+
+	mdelay(2);
+
+	return ret == 1 ? 0 : -EIO;
+}
+
+static int reg_read(struct i2c_client *client, const u16 addr)
+{
+	u8 buf[2] = {addr >> 8, addr & 0xff};
+	int ret;
+	struct i2c_msg msgs[] = {
+		{
+			.addr  = client->addr,
+			.flags = 0,
+			.len   = 2,
+			.buf   = buf,
+		}, {
+			.addr  = client->addr,
+			.flags = I2C_M_RD,
+			.len   = 2,
+			.buf   = buf,
+		},
+	};
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret < 0) {
+		dev_warn(&client->dev, "Reading register %x from %x failed\n",
+			 addr, client->addr);
+		return ret;
+	}
+
+	return buf[0] & 0xff; /* no sign-extension */
+}
+
+static int imx074_try_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_mbus_framefmt *mf)
+{
+	const struct imx074_datafmt *fmt = imx074_find_datafmt(mf->code);
+
+	dev_dbg(sd->v4l2_dev->dev, "%s(%u)\n", __func__, mf->code);
+
+	if (!fmt) {
+		mf->code	= imx074_colour_fmts[0].code;
+		mf->colorspace	= imx074_colour_fmts[0].colorspace;
+	}
+
+	mf->width	= IMX074_WIDTH;
+	mf->height	= IMX074_HEIGHT;
+	mf->field	= V4L2_FIELD_NONE;
+
+	return 0;
+}
+
+static int imx074_s_fmt(struct v4l2_subdev *sd,
+			struct v4l2_mbus_framefmt *mf)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct imx074 *priv = to_imx074(client);
+
+	dev_dbg(sd->v4l2_dev->dev, "%s(%u)\n", __func__, mf->code);
+
+	/* MIPI CSI could have changed the format, double-check */
+	if (!imx074_find_datafmt(mf->code))
+		return -EINVAL;
+
+	imx074_try_fmt(sd, mf);
+
+	priv->fmt = imx074_find_datafmt(mf->code);
+
+	return 0;
+}
+
+static int imx074_g_fmt(struct v4l2_subdev *sd,
+			struct v4l2_mbus_framefmt *mf)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct imx074 *priv = to_imx074(client);
+
+	const struct imx074_datafmt *fmt = priv->fmt;
+
+	mf->code	= fmt->code;
+	mf->colorspace	= fmt->colorspace;
+	mf->width	= IMX074_WIDTH;
+	mf->height	= IMX074_HEIGHT;
+	mf->field	= V4L2_FIELD_NONE;
+
+	return 0;
+}
+
+static int imx074_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+{
+	struct v4l2_rect *rect = &a->c;
+
+	a->type		= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	rect->top	= 0;
+	rect->left	= 0;
+	rect->width	= IMX074_WIDTH;
+	rect->height	= IMX074_HEIGHT;
+
+	return 0;
+}
+
+static int imx074_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
+{
+	a->bounds.left			= 0;
+	a->bounds.top			= 0;
+	a->bounds.width			= IMX074_WIDTH;
+	a->bounds.height		= IMX074_HEIGHT;
+	a->defrect			= a->bounds;
+	a->type				= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	a->pixelaspect.numerator	= 1;
+	a->pixelaspect.denominator	= 1;
+
+	return 0;
+}
+
+static int imx074_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
+			   enum v4l2_mbus_pixelcode *code)
+{
+	if ((unsigned int)index >= ARRAY_SIZE(imx074_colour_fmts))
+		return -EINVAL;
+
+	*code = imx074_colour_fmts[index].code;
+	return 0;
+}
+
+static int imx074_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	/* MODE_SELECT: stream or standby */
+	return reg_write(client, MODE_SELECT, !!enable);
+}
+
+static int imx074_g_chip_ident(struct v4l2_subdev *sd,
+			       struct v4l2_dbg_chip_ident *id)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (id->match.type != V4L2_CHIP_MATCH_I2C_ADDR)
+		return -EINVAL;
+
+	if (id->match.addr != client->addr)
+		return -ENODEV;
+
+	id->ident	= V4L2_IDENT_IMX074;
+	id->revision	= 0;
+
+	return 0;
+}
+
+static int imx074_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+	return soc_camera_set_power(&client->dev, icl, on);
+}
+
+static int imx074_g_mbus_config(struct v4l2_subdev *sd,
+				struct v4l2_mbus_config *cfg)
+{
+	cfg->type = V4L2_MBUS_CSI2;
+	cfg->flags = V4L2_MBUS_CSI2_2_LANE |
+		V4L2_MBUS_CSI2_CHANNEL_0 |
+		V4L2_MBUS_CSI2_CONTINUOUS_CLOCK;
+
+	return 0;
+}
+
+static struct v4l2_subdev_video_ops imx074_subdev_video_ops = {
+	.s_stream	= imx074_s_stream,
+	.s_mbus_fmt	= imx074_s_fmt,
+	.g_mbus_fmt	= imx074_g_fmt,
+	.try_mbus_fmt	= imx074_try_fmt,
+	.enum_mbus_fmt	= imx074_enum_fmt,
+	.g_crop		= imx074_g_crop,
+	.cropcap	= imx074_cropcap,
+	.g_mbus_config	= imx074_g_mbus_config,
+};
+
+static struct v4l2_subdev_core_ops imx074_subdev_core_ops = {
+	.g_chip_ident	= imx074_g_chip_ident,
+	.s_power	= imx074_s_power,
+};
+
+static struct v4l2_subdev_ops imx074_subdev_ops = {
+	.core	= &imx074_subdev_core_ops,
+	.video	= &imx074_subdev_video_ops,
+};
+
+static int imx074_video_probe(struct i2c_client *client)
+{
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	int ret;
+	u16 id;
+
+	ret = imx074_s_power(subdev, 1);
+	if (ret < 0)
+		return ret;
+
+	/* Read sensor Model ID */
+	ret = reg_read(client, 0);
+	if (ret < 0)
+		goto done;
+
+	id = ret << 8;
+
+	ret = reg_read(client, 1);
+	if (ret < 0)
+		goto done;
+
+	id |= ret;
+
+	dev_info(&client->dev, "Chip ID 0x%04x detected\n", id);
+
+	if (id != 0x74) {
+		ret = -ENODEV;
+		goto done;
+	}
+
+	/* PLL Setting EXTCLK=24MHz, 22.5times */
+	reg_write(client, PLL_MULTIPLIER, 0x2D);
+	reg_write(client, PRE_PLL_CLK_DIV, 0x02);
+	reg_write(client, PLSTATIM, 0x4B);
+
+	/* 2-lane mode */
+	reg_write(client, 0x3024, 0x00);
+
+	reg_write(client, IMAGE_ORIENTATION, 0x00);
+
+	/* select RAW mode:
+	 * 0x08+0x08 = top 8 bits
+	 * 0x0a+0x08 = compressed 8-bits
+	 * 0x0a+0x0a = 10 bits
+	 */
+	reg_write(client, 0x0112, 0x08);
+	reg_write(client, 0x0113, 0x08);
+
+	/* Base setting for High frame mode */
+	reg_write(client, VNDMY_ABLMGSHLMT, 0x80);
+	reg_write(client, Y_OPBADDR_START_DI, 0x08);
+	reg_write(client, 0x3015, 0x37);
+	reg_write(client, 0x301C, 0x01);
+	reg_write(client, 0x302C, 0x05);
+	reg_write(client, 0x3031, 0x26);
+	reg_write(client, 0x3041, 0x60);
+	reg_write(client, 0x3051, 0x24);
+	reg_write(client, 0x3053, 0x34);
+	reg_write(client, 0x3057, 0xC0);
+	reg_write(client, 0x305C, 0x09);
+	reg_write(client, 0x305D, 0x07);
+	reg_write(client, 0x3060, 0x30);
+	reg_write(client, 0x3065, 0x00);
+	reg_write(client, 0x30AA, 0x08);
+	reg_write(client, 0x30AB, 0x1C);
+	reg_write(client, 0x30B0, 0x32);
+	reg_write(client, 0x30B2, 0x83);
+	reg_write(client, 0x30D3, 0x04);
+	reg_write(client, 0x3106, 0x78);
+	reg_write(client, 0x310C, 0x82);
+	reg_write(client, 0x3304, 0x05);
+	reg_write(client, 0x3305, 0x04);
+	reg_write(client, 0x3306, 0x11);
+	reg_write(client, 0x3307, 0x02);
+	reg_write(client, 0x3308, 0x0C);
+	reg_write(client, 0x3309, 0x06);
+	reg_write(client, 0x330A, 0x08);
+	reg_write(client, 0x330B, 0x04);
+	reg_write(client, 0x330C, 0x08);
+	reg_write(client, 0x330D, 0x06);
+	reg_write(client, 0x330E, 0x01);
+	reg_write(client, 0x3381, 0x00);
+
+	/* V : 1/2V-addition (1,3), H : 1/2H-averaging (1,3) -> Full HD */
+	/* 1608 = 1560 + 48 (black lines) */
+	reg_write(client, FRAME_LENGTH_LINES_HI, 0x06);
+	reg_write(client, FRAME_LENGTH_LINES_LO, 0x48);
+	reg_write(client, YADDR_START, 0x00);
+	reg_write(client, YADDR_END, 0x2F);
+	/* 0x838 == 2104 */
+	reg_write(client, X_OUTPUT_SIZE_MSB, 0x08);
+	reg_write(client, X_OUTPUT_SIZE_LSB, 0x38);
+	/* 0x618 == 1560 */
+	reg_write(client, Y_OUTPUT_SIZE_MSB, 0x06);
+	reg_write(client, Y_OUTPUT_SIZE_LSB, 0x18);
+	reg_write(client, X_EVEN_INC, 0x01);
+	reg_write(client, X_ODD_INC, 0x03);
+	reg_write(client, Y_EVEN_INC, 0x01);
+	reg_write(client, Y_ODD_INC, 0x03);
+	reg_write(client, HMODEADD, 0x00);
+	reg_write(client, VMODEADD, 0x16);
+	reg_write(client, VAPPLINE_START, 0x24);
+	reg_write(client, VAPPLINE_END, 0x53);
+	reg_write(client, SHUTTER, 0x00);
+	reg_write(client, HADDAVE, 0x80);
+
+	reg_write(client, LANESEL, 0x00);
+
+	reg_write(client, GROUPED_PARAMETER_HOLD, 0x00);	/* off */
+
+	ret = 0;
+
+done:
+	imx074_s_power(subdev, 0);
+	return ret;
+}
+
+static int imx074_probe(struct i2c_client *client,
+			const struct i2c_device_id *did)
+{
+	struct imx074 *priv;
+	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+	int ret;
+
+	if (!icl) {
+		dev_err(&client->dev, "IMX074: missing platform data!\n");
+		return -EINVAL;
+	}
+
+	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;
+	}
+
+	priv = kzalloc(sizeof(struct imx074), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	v4l2_i2c_subdev_init(&priv->subdev, client, &imx074_subdev_ops);
+
+	priv->fmt	= &imx074_colour_fmts[0];
+
+	ret = imx074_video_probe(client);
+	if (ret < 0) {
+		kfree(priv);
+		return ret;
+	}
+
+	return ret;
+}
+
+static int imx074_remove(struct i2c_client *client)
+{
+	struct imx074 *priv = to_imx074(client);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+	if (icl->free_bus)
+		icl->free_bus(icl);
+	kfree(priv);
+
+	return 0;
+}
+
+static const struct i2c_device_id imx074_id[] = {
+	{ "imx074", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, imx074_id);
+
+static struct i2c_driver imx074_i2c_driver = {
+	.driver = {
+		.name = "imx074",
+	},
+	.probe		= imx074_probe,
+	.remove		= imx074_remove,
+	.id_table	= imx074_id,
+};
+
+module_i2c_driver(imx074_i2c_driver);
+
+MODULE_DESCRIPTION("Sony IMX074 Camera driver");
+MODULE_AUTHOR("Guennadi Liakhovetski <g.liakhovetski@gmx.de>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/soc_camera/mt9m001.c b/drivers/media/i2c/soc_camera/mt9m001.c
new file mode 100644
index 000000000000..19f8a07764f9
--- /dev/null
+++ b/drivers/media/i2c/soc_camera/mt9m001.c
@@ -0,0 +1,757 @@
+/*
+ * Driver for MT9M001 CMOS Image Sensor from Micron
+ *
+ * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/videodev2.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/log2.h>
+#include <linux/module.h>
+
+#include <media/soc_camera.h>
+#include <media/soc_mediabus.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-ctrls.h>
+
+/*
+ * mt9m001 i2c address 0x5d
+ * The platform has to define struct i2c_board_info objects and link to them
+ * from struct soc_camera_link
+ */
+
+/* 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
+
+/* MT9M001 has only one fixed colorspace per pixelcode */
+struct mt9m001_datafmt {
+	enum v4l2_mbus_pixelcode	code;
+	enum v4l2_colorspace		colorspace;
+};
+
+/* Find a data format by a pixel code in an array */
+static const struct mt9m001_datafmt *mt9m001_find_datafmt(
+	enum v4l2_mbus_pixelcode 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
+	 */
+	{V4L2_MBUS_FMT_SBGGR10_1X10, V4L2_COLORSPACE_SRGB},
+	{V4L2_MBUS_FMT_SBGGR8_1X8, V4L2_COLORSPACE_SRGB},
+};
+
+static const struct mt9m001_datafmt mt9m001_monochrome_fmts[] = {
+	/* Order important - see above */
+	{V4L2_MBUS_FMT_Y10_1X10, V4L2_COLORSPACE_JPEG},
+	{V4L2_MBUS_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;
+	};
+	struct v4l2_rect rect;	/* Sensor window */
+	const struct mt9m001_datafmt *fmt;
+	const struct mt9m001_datafmt *fmts;
+	int num_fmts;
+	int model;	/* V4L2_IDENT_MT9M001* codes from v4l2-chip-ident.h */
+	unsigned int total_h;
+	unsigned short y_skip_top;	/* Lines to skip at the top */
+};
+
+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);
+}
+
+static int mt9m001_init(struct i2c_client *client)
+{
+	int ret;
+
+	dev_dbg(&client->dev, "%s\n", __func__);
+
+	/*
+	 * We don't know, whether platform provides reset, issue a soft reset
+	 * too. This returns all registers to their default values.
+	 */
+	ret = reg_write(client, MT9M001_RESET, 1);
+	if (!ret)
+		ret = reg_write(client, MT9M001_RESET, 0);
+
+	/* Disable chip, synchronous option update */
+	if (!ret)
+		ret = reg_write(client, MT9M001_OUTPUT_CONTROL, 0);
+
+	return ret;
+}
+
+static int mt9m001_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	/* Switch to master "normal" mode or stop sensor readout */
+	if (reg_write(client, MT9M001_OUTPUT_CONTROL, enable ? 2 : 0) < 0)
+		return -EIO;
+	return 0;
+}
+
+static int mt9m001_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9m001 *mt9m001 = to_mt9m001(client);
+	struct v4l2_rect rect = a->c;
+	int ret;
+	const u16 hblank = 9, vblank = 25;
+
+	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);
+
+	soc_camera_limit_side(&rect.left, &rect.width,
+		     MT9M001_COLUMN_SKIP, MT9M001_MIN_WIDTH, MT9M001_MAX_WIDTH);
+
+	soc_camera_limit_side(&rect.top, &rect.height,
+		     MT9M001_ROW_SKIP, MT9M001_MIN_HEIGHT, MT9M001_MAX_HEIGHT);
+
+	mt9m001->total_h = rect.height + mt9m001->y_skip_top + vblank;
+
+	/* Blanking and start values - default... */
+	ret = reg_write(client, MT9M001_HORIZONTAL_BLANKING, hblank);
+	if (!ret)
+		ret = reg_write(client, MT9M001_VERTICAL_BLANKING, vblank);
+
+	/*
+	 * The caller provides a supported format, as verified per
+	 * call to .try_mbus_fmt()
+	 */
+	if (!ret)
+		ret = reg_write(client, MT9M001_COLUMN_START, rect.left);
+	if (!ret)
+		ret = reg_write(client, MT9M001_ROW_START, rect.top);
+	if (!ret)
+		ret = reg_write(client, MT9M001_WINDOW_WIDTH, rect.width - 1);
+	if (!ret)
+		ret = reg_write(client, MT9M001_WINDOW_HEIGHT,
+				rect.height + mt9m001->y_skip_top - 1);
+	if (!ret && v4l2_ctrl_g_ctrl(mt9m001->autoexposure) == V4L2_EXPOSURE_AUTO)
+		ret = reg_write(client, MT9M001_SHUTTER_WIDTH, mt9m001->total_h);
+
+	if (!ret)
+		mt9m001->rect = rect;
+
+	return ret;
+}
+
+static int mt9m001_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9m001 *mt9m001 = to_mt9m001(client);
+
+	a->c	= mt9m001->rect;
+	a->type	= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	return 0;
+}
+
+static int mt9m001_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
+{
+	a->bounds.left			= MT9M001_COLUMN_SKIP;
+	a->bounds.top			= MT9M001_ROW_SKIP;
+	a->bounds.width			= MT9M001_MAX_WIDTH;
+	a->bounds.height		= MT9M001_MAX_HEIGHT;
+	a->defrect			= a->bounds;
+	a->type				= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	a->pixelaspect.numerator	= 1;
+	a->pixelaspect.denominator	= 1;
+
+	return 0;
+}
+
+static int mt9m001_g_fmt(struct v4l2_subdev *sd,
+			 struct v4l2_mbus_framefmt *mf)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9m001 *mt9m001 = to_mt9m001(client);
+
+	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;
+
+	return 0;
+}
+
+static int mt9m001_s_fmt(struct v4l2_subdev *sd,
+			 struct v4l2_mbus_framefmt *mf)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9m001 *mt9m001 = to_mt9m001(client);
+	struct v4l2_crop a = {
+		.c = {
+			.left	= mt9m001->rect.left,
+			.top	= mt9m001->rect.top,
+			.width	= mf->width,
+			.height	= mf->height,
+		},
+	};
+	int ret;
+
+	/* No support for scaling so far, just crop. TODO: use skipping */
+	ret = mt9m001_s_crop(sd, &a);
+	if (!ret) {
+		mf->width	= mt9m001->rect.width;
+		mf->height	= mt9m001->rect.height;
+		mt9m001->fmt	= mt9m001_find_datafmt(mf->code,
+					mt9m001->fmts, mt9m001->num_fmts);
+		mf->colorspace	= mt9m001->fmt->colorspace;
+	}
+
+	return ret;
+}
+
+static int mt9m001_try_fmt(struct v4l2_subdev *sd,
+			   struct v4l2_mbus_framefmt *mf)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9m001 *mt9m001 = to_mt9m001(client);
+	const struct mt9m001_datafmt *fmt;
+
+	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;
+
+	return 0;
+}
+
+static int mt9m001_g_chip_ident(struct v4l2_subdev *sd,
+				struct v4l2_dbg_chip_ident *id)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9m001 *mt9m001 = to_mt9m001(client);
+
+	if (id->match.type != V4L2_CHIP_MATCH_I2C_ADDR)
+		return -EINVAL;
+
+	if (id->match.addr != client->addr)
+		return -ENODEV;
+
+	id->ident	= mt9m001->model;
+	id->revision	= 0;
+
+	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->match.type != V4L2_CHIP_MATCH_I2C_ADDR || reg->reg > 0xff)
+		return -EINVAL;
+
+	if (reg->match.addr != client->addr)
+		return -ENODEV;
+
+	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,
+			      struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (reg->match.type != V4L2_CHIP_MATCH_I2C_ADDR || reg->reg > 0xff)
+		return -EINVAL;
+
+	if (reg->match.addr != client->addr)
+		return -ENODEV;
+
+	if (reg_write(client, reg->reg, reg->val) < 0)
+		return -EIO;
+
+	return 0;
+}
+#endif
+
+static int mt9m001_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+	return soc_camera_set_power(&client->dev, icl, on);
+}
+
+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;
+
+	switch (ctrl->id) {
+	case V4L2_CID_VFLIP:
+		if (ctrl->val)
+			data = reg_set(client, MT9M001_READ_OPTIONS2, 0x8000);
+		else
+			data = reg_clear(client, MT9M001_READ_OPTIONS2, 0x8000);
+		if (data < 0)
+			return -EIO;
+		return 0;
+
+	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 - ctrl->minimum) * 8 + range / 2) / range;
+
+			dev_dbg(&client->dev, "Setting gain %d\n", data);
+			data = reg_write(client, MT9M001_GLOBAL_GAIN, data);
+			if (data < 0)
+				return -EIO;
+		} 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 - 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);
+			data = reg_write(client, MT9M001_GLOBAL_GAIN, data);
+			if (data < 0)
+				return -EIO;
+		}
+		return 0;
+
+	case V4L2_CID_EXPOSURE_AUTO:
+		if (ctrl->val == V4L2_EXPOSURE_MANUAL) {
+			unsigned long range = exp->maximum - exp->minimum;
+			unsigned long shutter = ((exp->val - 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);
+			if (reg_write(client, MT9M001_SHUTTER_WIDTH, shutter) < 0)
+				return -EIO;
+		} else {
+			const u16 vblank = 25;
+
+			mt9m001->total_h = mt9m001->rect.height +
+				mt9m001->y_skip_top + vblank;
+			if (reg_write(client, MT9M001_SHUTTER_WIDTH, mt9m001->total_h) < 0)
+				return -EIO;
+		}
+		return 0;
+	}
+	return -EINVAL;
+}
+
+/*
+ * 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 soc_camera_link *icl,
+			       struct i2c_client *client)
+{
+	struct mt9m001 *mt9m001 = to_mt9m001(client);
+	s32 data;
+	unsigned long flags;
+	int ret;
+
+	ret = mt9m001_s_power(&mt9m001->subdev, 1);
+	if (ret < 0)
+		return 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->model = V4L2_IDENT_MT9M001C12ST;
+		mt9m001->fmts = mt9m001_colour_fmts;
+		break;
+	case 0x8431:
+		mt9m001->model = V4L2_IDENT_MT9M001C12STM;
+		mt9m001->fmts = mt9m001_monochrome_fmts;
+		break;
+	default:
+		dev_err(&client->dev,
+			"No MT9M001 chip detected, register read %x\n", data);
+		ret = -ENODEV;
+		goto done;
+	}
+
+	mt9m001->num_fmts = 0;
+
+	/*
+	 * This is a 10bit sensor, so by default we only allow 10bit.
+	 * The platform may support different bus widths due to
+	 * different routing of the data lines.
+	 */
+	if (icl->query_bus_param)
+		flags = icl->query_bus_param(icl);
+	else
+		flags = SOCAM_DATAWIDTH_10;
+
+	if (flags & SOCAM_DATAWIDTH_10)
+		mt9m001->num_fmts++;
+	else
+		mt9m001->fmts++;
+
+	if (flags & SOCAM_DATAWIDTH_8)
+		mt9m001->num_fmts++;
+
+	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:
+	mt9m001_s_power(&mt9m001->subdev, 0);
+	return ret;
+}
+
+static void mt9m001_video_remove(struct soc_camera_link *icl)
+{
+	if (icl->free_bus)
+		icl->free_bus(icl);
+}
+
+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 struct v4l2_subdev_core_ops mt9m001_subdev_core_ops = {
+	.g_chip_ident	= mt9m001_g_chip_ident,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register	= mt9m001_g_register,
+	.s_register	= mt9m001_s_register,
+#endif
+	.s_power	= mt9m001_s_power,
+};
+
+static int mt9m001_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
+			    enum v4l2_mbus_pixelcode *code)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9m001 *mt9m001 = to_mt9m001(client);
+
+	if (index >= mt9m001->num_fmts)
+		return -EINVAL;
+
+	*code = mt9m001->fmts[index].code;
+	return 0;
+}
+
+static int mt9m001_g_mbus_config(struct v4l2_subdev *sd,
+				struct v4l2_mbus_config *cfg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+	/* MT9M001 has all capture_format parameters fixed */
+	cfg->flags = V4L2_MBUS_PCLK_SAMPLE_FALLING |
+		V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_HIGH |
+		V4L2_MBUS_DATA_ACTIVE_HIGH | V4L2_MBUS_MASTER;
+	cfg->type = V4L2_MBUS_PARALLEL;
+	cfg->flags = soc_camera_apply_board_flags(icl, cfg);
+
+	return 0;
+}
+
+static int mt9m001_s_mbus_config(struct v4l2_subdev *sd,
+				const struct v4l2_mbus_config *cfg)
+{
+	const struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+	struct mt9m001 *mt9m001 = to_mt9m001(client);
+	unsigned int bps = soc_mbus_get_fmtdesc(mt9m001->fmt->code)->bits_per_sample;
+
+	if (icl->set_bus_param)
+		return icl->set_bus_param(icl, 1 << (bps - 1));
+
+	/*
+	 * Without board specific bus width settings we only support the
+	 * sensors native bus width
+	 */
+	return bps == 10 ? 0 : -EINVAL;
+}
+
+static struct v4l2_subdev_video_ops mt9m001_subdev_video_ops = {
+	.s_stream	= mt9m001_s_stream,
+	.s_mbus_fmt	= mt9m001_s_fmt,
+	.g_mbus_fmt	= mt9m001_g_fmt,
+	.try_mbus_fmt	= mt9m001_try_fmt,
+	.s_crop		= mt9m001_s_crop,
+	.g_crop		= mt9m001_g_crop,
+	.cropcap	= mt9m001_cropcap,
+	.enum_mbus_fmt	= mt9m001_enum_fmt,
+	.g_mbus_config	= mt9m001_g_mbus_config,
+	.s_mbus_config	= mt9m001_s_mbus_config,
+};
+
+static struct v4l2_subdev_sensor_ops mt9m001_subdev_sensor_ops = {
+	.g_skip_top_lines	= mt9m001_g_skip_top_lines,
+};
+
+static struct v4l2_subdev_ops mt9m001_subdev_ops = {
+	.core	= &mt9m001_subdev_core_ops,
+	.video	= &mt9m001_subdev_video_ops,
+	.sensor	= &mt9m001_subdev_sensor_ops,
+};
+
+static int mt9m001_probe(struct i2c_client *client,
+			 const struct i2c_device_id *did)
+{
+	struct mt9m001 *mt9m001;
+	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+	int ret;
+
+	if (!icl) {
+		dev_err(&client->dev, "MT9M001 driver needs platform data\n");
+		return -EINVAL;
+	}
+
+	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 = kzalloc(sizeof(struct mt9m001), GFP_KERNEL);
+	if (!mt9m001)
+		return -ENOMEM;
+
+	v4l2_i2c_subdev_init(&mt9m001->subdev, client, &mt9m001_subdev_ops);
+	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) {
+		int err = mt9m001->hdl.error;
+
+		kfree(mt9m001);
+		return err;
+	}
+	v4l2_ctrl_auto_cluster(2, &mt9m001->autoexposure,
+					V4L2_EXPOSURE_MANUAL, true);
+
+	/* 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_video_probe(icl, client);
+	if (ret) {
+		v4l2_ctrl_handler_free(&mt9m001->hdl);
+		kfree(mt9m001);
+	}
+
+	return ret;
+}
+
+static int mt9m001_remove(struct i2c_client *client)
+{
+	struct mt9m001 *mt9m001 = to_mt9m001(client);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+	v4l2_device_unregister_subdev(&mt9m001->subdev);
+	v4l2_ctrl_handler_free(&mt9m001->hdl);
+	mt9m001_video_remove(icl);
+	kfree(mt9m001);
+
+	return 0;
+}
+
+static const struct i2c_device_id mt9m001_id[] = {
+	{ "mt9m001", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, mt9m001_id);
+
+static struct i2c_driver mt9m001_i2c_driver = {
+	.driver = {
+		.name = "mt9m001",
+	},
+	.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");
diff --git a/drivers/media/i2c/soc_camera/mt9m111.c b/drivers/media/i2c/soc_camera/mt9m111.c
new file mode 100644
index 000000000000..62fd94af599b
--- /dev/null
+++ b/drivers/media/i2c/soc_camera/mt9m111.c
@@ -0,0 +1,1046 @@
+/*
+ * Driver for MT9M111/MT9M112/MT9M131 CMOS Image Sensor from Micron/Aptina
+ *
+ * Copyright (C) 2008, Robert Jarzmik <robert.jarzmik@free.fr>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#include <linux/videodev2.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/log2.h>
+#include <linux/gpio.h>
+#include <linux/delay.h>
+#include <linux/v4l2-mediabus.h>
+#include <linux/module.h>
+
+#include <media/soc_camera.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-chip-ident.h>
+
+/*
+ * MT9M111, MT9M112 and MT9M131:
+ * i2c address is 0x48 or 0x5d (depending on SADDR pin)
+ * The platform has to define i2c_board_info and call i2c_register_board_info()
+ */
+
+/*
+ * 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_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_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)
+
+/*
+ * 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 {
+	enum v4l2_mbus_pixelcode	code;
+	enum v4l2_colorspace		colorspace;
+};
+
+static const struct mt9m111_datafmt mt9m111_colour_fmts[] = {
+	{V4L2_MBUS_FMT_YUYV8_2X8, V4L2_COLORSPACE_JPEG},
+	{V4L2_MBUS_FMT_YVYU8_2X8, V4L2_COLORSPACE_JPEG},
+	{V4L2_MBUS_FMT_UYVY8_2X8, V4L2_COLORSPACE_JPEG},
+	{V4L2_MBUS_FMT_VYUY8_2X8, V4L2_COLORSPACE_JPEG},
+	{V4L2_MBUS_FMT_RGB555_2X8_PADHI_LE, V4L2_COLORSPACE_SRGB},
+	{V4L2_MBUS_FMT_RGB555_2X8_PADHI_BE, V4L2_COLORSPACE_SRGB},
+	{V4L2_MBUS_FMT_RGB565_2X8_LE, V4L2_COLORSPACE_SRGB},
+	{V4L2_MBUS_FMT_RGB565_2X8_BE, V4L2_COLORSPACE_SRGB},
+	{V4L2_MBUS_FMT_BGR565_2X8_LE, V4L2_COLORSPACE_SRGB},
+	{V4L2_MBUS_FMT_BGR565_2X8_BE, V4L2_COLORSPACE_SRGB},
+	{V4L2_MBUS_FMT_SBGGR8_1X8, V4L2_COLORSPACE_SRGB},
+	{V4L2_MBUS_FMT_SBGGR10_2X8_PADHI_LE, V4L2_COLORSPACE_SRGB},
+};
+
+struct mt9m111 {
+	struct v4l2_subdev subdev;
+	struct v4l2_ctrl_handler hdl;
+	struct v4l2_ctrl *gain;
+	int model;	/* V4L2_IDENT_MT9M111 or V4L2_IDENT_MT9M112 code
+			 * from v4l2-chip-ident.h */
+	struct mt9m111_context *ctx;
+	struct v4l2_rect rect;	/* cropping rectangle */
+	int width;		/* output */
+	int height;		/* sizes */
+	struct mutex power_lock; /* lock to protect power_count */
+	int power_count;
+	const struct mt9m111_datafmt *fmt;
+	int lastpage;	/* PageMap cache value */
+};
+
+/* Find a data format by a pixel code */
+static const struct mt9m111_datafmt *mt9m111_find_datafmt(struct mt9m111 *mt9m111,
+						enum v4l2_mbus_pixelcode 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, enum v4l2_mbus_pixelcode 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 != V4L2_MBUS_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_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
+{
+	struct v4l2_rect rect = a->c;
+	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
+	int width, height;
+	int ret;
+
+	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	if (mt9m111->fmt->code == V4L2_MBUS_FMT_SBGGR8_1X8 ||
+	    mt9m111->fmt->code == V4L2_MBUS_FMT_SBGGR10_2X8_PADHI_LE) {
+		/* Bayer format - even size lengths */
+		rect.width	= ALIGN(rect.width, 2);
+		rect.height	= ALIGN(rect.height, 2);
+		/* Let the user play with the starting pixel */
+	}
+
+	/* FIXME: the datasheet doesn't specify minimum sizes */
+	soc_camera_limit_side(&rect.left, &rect.width,
+		     MT9M111_MIN_DARK_COLS, 2, MT9M111_MAX_WIDTH);
+
+	soc_camera_limit_side(&rect.top, &rect.height,
+		     MT9M111_MIN_DARK_ROWS, 2, MT9M111_MAX_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_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+{
+	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
+
+	a->c	= mt9m111->rect;
+	a->type	= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	return 0;
+}
+
+static int mt9m111_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
+{
+	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	a->bounds.left			= MT9M111_MIN_DARK_COLS;
+	a->bounds.top			= MT9M111_MIN_DARK_ROWS;
+	a->bounds.width			= MT9M111_MAX_WIDTH;
+	a->bounds.height		= MT9M111_MAX_HEIGHT;
+	a->defrect			= a->bounds;
+	a->pixelaspect.numerator	= 1;
+	a->pixelaspect.denominator	= 1;
+
+	return 0;
+}
+
+static int mt9m111_g_fmt(struct v4l2_subdev *sd,
+			 struct v4l2_mbus_framefmt *mf)
+{
+	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
+
+	mf->width	= mt9m111->width;
+	mf->height	= mt9m111->height;
+	mf->code	= mt9m111->fmt->code;
+	mf->colorspace	= mt9m111->fmt->colorspace;
+	mf->field	= V4L2_FIELD_NONE;
+
+	return 0;
+}
+
+static int mt9m111_set_pixfmt(struct mt9m111 *mt9m111,
+			      enum v4l2_mbus_pixelcode 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 V4L2_MBUS_FMT_SBGGR8_1X8:
+		data_outfmt2 = MT9M111_OUTFMT_PROCESSED_BAYER |
+			MT9M111_OUTFMT_RGB;
+		break;
+	case V4L2_MBUS_FMT_SBGGR10_2X8_PADHI_LE:
+		data_outfmt2 = MT9M111_OUTFMT_BYPASS_IFP | MT9M111_OUTFMT_RGB;
+		break;
+	case V4L2_MBUS_FMT_RGB555_2X8_PADHI_LE:
+		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB555 |
+			MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
+		break;
+	case V4L2_MBUS_FMT_RGB555_2X8_PADHI_BE:
+		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB555;
+		break;
+	case V4L2_MBUS_FMT_RGB565_2X8_LE:
+		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
+			MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
+		break;
+	case V4L2_MBUS_FMT_RGB565_2X8_BE:
+		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565;
+		break;
+	case V4L2_MBUS_FMT_BGR565_2X8_BE:
+		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
+			MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
+		break;
+	case V4L2_MBUS_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 V4L2_MBUS_FMT_UYVY8_2X8:
+		data_outfmt2 = 0;
+		break;
+	case V4L2_MBUS_FMT_VYUY8_2X8:
+		data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
+		break;
+	case V4L2_MBUS_FMT_YUYV8_2X8:
+		data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
+		break;
+	case V4L2_MBUS_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;
+	}
+
+	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_try_fmt(struct v4l2_subdev *sd,
+			   struct v4l2_mbus_framefmt *mf)
+{
+	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;
+
+	fmt = mt9m111_find_datafmt(mt9m111, mf->code);
+
+	bayer = fmt->code == V4L2_MBUS_FMT_SBGGR8_1X8 ||
+		fmt->code == V4L2_MBUS_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 == V4L2_MBUS_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;
+
+	return 0;
+}
+
+static int mt9m111_s_fmt(struct v4l2_subdev *sd,
+			 struct v4l2_mbus_framefmt *mf)
+{
+	const struct mt9m111_datafmt *fmt;
+	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
+	struct v4l2_rect *rect = &mt9m111->rect;
+	int ret;
+
+	mt9m111_try_fmt(sd, mf);
+	fmt = mt9m111_find_datafmt(mt9m111, mf->code);
+	/* try_fmt() guarantees fmt != NULL && fmt->code == mf->code */
+
+	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 int mt9m111_g_chip_ident(struct v4l2_subdev *sd,
+				struct v4l2_dbg_chip_ident *id)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
+
+	if (id->match.type != V4L2_CHIP_MATCH_I2C_ADDR)
+		return -EINVAL;
+
+	if (id->match.addr != client->addr)
+		return -ENODEV;
+
+	id->ident	= mt9m111->model;
+	id->revision	= 0;
+
+	return 0;
+}
+
+#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->match.type != V4L2_CHIP_MATCH_I2C_ADDR || reg->reg > 0x2ff)
+		return -EINVAL;
+	if (reg->match.addr != client->addr)
+		return -ENODEV;
+
+	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,
+			      struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (reg->match.type != V4L2_CHIP_MATCH_I2C_ADDR || reg->reg > 0x2ff)
+		return -EINVAL;
+
+	if (reg->match.addr != client->addr)
+		return -ENODEV;
+
+	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 on)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
+
+	if (on)
+		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 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);
+	}
+
+	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)
+{
+	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);
+}
+
+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;
+
+	/* Default HIGHPOWER context */
+	mt9m111->ctx = &context_b;
+	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);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+	int ret;
+
+	ret = soc_camera_power_on(&client->dev, icl);
+	if (ret < 0)
+		return ret;
+
+	ret = mt9m111_resume(mt9m111);
+	if (ret < 0) {
+		dev_err(&client->dev, "Failed to resume the sensor: %d\n", ret);
+		soc_camera_power_off(&client->dev, icl);
+	}
+
+	return ret;
+}
+
+static void mt9m111_power_off(struct mt9m111 *mt9m111)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+	mt9m111_suspend(mt9m111);
+	soc_camera_power_off(&client->dev, icl);
+}
+
+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 struct v4l2_subdev_core_ops mt9m111_subdev_core_ops = {
+	.g_chip_ident	= mt9m111_g_chip_ident,
+	.s_power	= mt9m111_s_power,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register	= mt9m111_g_register,
+	.s_register	= mt9m111_s_register,
+#endif
+};
+
+static int mt9m111_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
+			    enum v4l2_mbus_pixelcode *code)
+{
+	if (index >= ARRAY_SIZE(mt9m111_colour_fmts))
+		return -EINVAL;
+
+	*code = mt9m111_colour_fmts[index].code;
+	return 0;
+}
+
+static int mt9m111_g_mbus_config(struct v4l2_subdev *sd,
+				struct v4l2_mbus_config *cfg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+	cfg->flags = V4L2_MBUS_MASTER | V4L2_MBUS_PCLK_SAMPLE_RISING |
+		V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_HIGH |
+		V4L2_MBUS_DATA_ACTIVE_HIGH;
+	cfg->type = V4L2_MBUS_PARALLEL;
+	cfg->flags = soc_camera_apply_board_flags(icl, cfg);
+
+	return 0;
+}
+
+static struct v4l2_subdev_video_ops mt9m111_subdev_video_ops = {
+	.s_mbus_fmt	= mt9m111_s_fmt,
+	.g_mbus_fmt	= mt9m111_g_fmt,
+	.try_mbus_fmt	= mt9m111_try_fmt,
+	.s_crop		= mt9m111_s_crop,
+	.g_crop		= mt9m111_g_crop,
+	.cropcap	= mt9m111_cropcap,
+	.enum_mbus_fmt	= mt9m111_enum_fmt,
+	.g_mbus_config	= mt9m111_g_mbus_config,
+};
+
+static struct v4l2_subdev_ops mt9m111_subdev_ops = {
+	.core	= &mt9m111_subdev_core_ops,
+	.video	= &mt9m111_subdev_video_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 */
+		mt9m111->model = V4L2_IDENT_MT9M111;
+		dev_info(&client->dev,
+			"Detected a MT9M111/MT9M131 chip ID %x\n", data);
+		break;
+	case 0x148c: /* MT9M112 */
+		mt9m111->model = V4L2_IDENT_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(struct i2c_client *client,
+			 const struct i2c_device_id *did)
+{
+	struct mt9m111 *mt9m111;
+	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+	int ret;
+
+	if (!icl) {
+		dev_err(&client->dev, "mt9m111: driver needs platform data\n");
+		return -EINVAL;
+	}
+
+	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 = kzalloc(sizeof(struct mt9m111), GFP_KERNEL);
+	if (!mt9m111)
+		return -ENOMEM;
+
+	v4l2_i2c_subdev_init(&mt9m111->subdev, client, &mt9m111_subdev_ops);
+	v4l2_ctrl_handler_init(&mt9m111->hdl, 5);
+	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);
+	mt9m111->subdev.ctrl_handler = &mt9m111->hdl;
+	if (mt9m111->hdl.error) {
+		int err = mt9m111->hdl.error;
+
+		kfree(mt9m111);
+		return err;
+	}
+
+	/* 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->fmt		= &mt9m111_colour_fmts[0];
+	mt9m111->lastpage	= -1;
+	mutex_init(&mt9m111->power_lock);
+
+	ret = mt9m111_video_probe(client);
+	if (ret) {
+		v4l2_ctrl_handler_free(&mt9m111->hdl);
+		kfree(mt9m111);
+	}
+
+	return ret;
+}
+
+static int mt9m111_remove(struct i2c_client *client)
+{
+	struct mt9m111 *mt9m111 = to_mt9m111(client);
+
+	v4l2_device_unregister_subdev(&mt9m111->subdev);
+	v4l2_ctrl_handler_free(&mt9m111->hdl);
+	kfree(mt9m111);
+
+	return 0;
+}
+
+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",
+	},
+	.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/soc_camera/mt9t031.c b/drivers/media/i2c/soc_camera/mt9t031.c
new file mode 100644
index 000000000000..40800b10a080
--- /dev/null
+++ b/drivers/media/i2c/soc_camera/mt9t031.c
@@ -0,0 +1,857 @@
+/*
+ * Driver for MT9T031 CMOS Image Sensor from Micron
+ *
+ * Copyright (C) 2008, Guennadi Liakhovetski, DENX Software Engineering <lg@denx.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/device.h>
+#include <linux/i2c.h>
+#include <linux/log2.h>
+#include <linux/pm.h>
+#include <linux/slab.h>
+#include <linux/v4l2-mediabus.h>
+#include <linux/videodev2.h>
+#include <linux/module.h>
+
+#include <media/soc_camera.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-ctrls.h>
+
+/*
+ * ATTENTION: this driver still cannot be used outside of the soc-camera
+ * framework because of its PM implementation, using the video_device node.
+ * If hardware becomes available for testing, alternative PM approaches shall
+ * be considered and tested.
+ */
+
+/*
+ * mt9t031 i2c address 0x5d
+ * The platform has to define i2c_board_info and link to it from
+ * struct soc_camera_link
+ */
+
+/* mt9t031 selected register addresses */
+#define MT9T031_CHIP_VERSION		0x00
+#define MT9T031_ROW_START		0x01
+#define MT9T031_COLUMN_START		0x02
+#define MT9T031_WINDOW_HEIGHT		0x03
+#define MT9T031_WINDOW_WIDTH		0x04
+#define MT9T031_HORIZONTAL_BLANKING	0x05
+#define MT9T031_VERTICAL_BLANKING	0x06
+#define MT9T031_OUTPUT_CONTROL		0x07
+#define MT9T031_SHUTTER_WIDTH_UPPER	0x08
+#define MT9T031_SHUTTER_WIDTH		0x09
+#define MT9T031_PIXEL_CLOCK_CONTROL	0x0a
+#define MT9T031_FRAME_RESTART		0x0b
+#define MT9T031_SHUTTER_DELAY		0x0c
+#define MT9T031_RESET			0x0d
+#define MT9T031_READ_MODE_1		0x1e
+#define MT9T031_READ_MODE_2		0x20
+#define MT9T031_READ_MODE_3		0x21
+#define MT9T031_ROW_ADDRESS_MODE	0x22
+#define MT9T031_COLUMN_ADDRESS_MODE	0x23
+#define MT9T031_GLOBAL_GAIN		0x35
+#define MT9T031_CHIP_ENABLE		0xF8
+
+#define MT9T031_MAX_HEIGHT		1536
+#define MT9T031_MAX_WIDTH		2048
+#define MT9T031_MIN_HEIGHT		2
+#define MT9T031_MIN_WIDTH		18
+#define MT9T031_HORIZONTAL_BLANK	142
+#define MT9T031_VERTICAL_BLANK		25
+#define MT9T031_COLUMN_SKIP		32
+#define MT9T031_ROW_SKIP		20
+
+struct mt9t031 {
+	struct v4l2_subdev subdev;
+	struct v4l2_ctrl_handler hdl;
+	struct {
+		/* exposure/auto-exposure cluster */
+		struct v4l2_ctrl *autoexposure;
+		struct v4l2_ctrl *exposure;
+	};
+	struct v4l2_rect rect;	/* Sensor window */
+	int model;	/* V4L2_IDENT_MT9T031* codes from v4l2-chip-ident.h */
+	u16 xskip;
+	u16 yskip;
+	unsigned int total_h;
+	unsigned short y_skip_top;	/* Lines to skip at the top */
+};
+
+static struct mt9t031 *to_mt9t031(const struct i2c_client *client)
+{
+	return container_of(i2c_get_clientdata(client), struct mt9t031, 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);
+}
+
+static int set_shutter(struct i2c_client *client, const u32 data)
+{
+	int ret;
+
+	ret = reg_write(client, MT9T031_SHUTTER_WIDTH_UPPER, data >> 16);
+
+	if (ret >= 0)
+		ret = reg_write(client, MT9T031_SHUTTER_WIDTH, data & 0xffff);
+
+	return ret;
+}
+
+static int get_shutter(struct i2c_client *client, u32 *data)
+{
+	int ret;
+
+	ret = reg_read(client, MT9T031_SHUTTER_WIDTH_UPPER);
+	*data = ret << 16;
+
+	if (ret >= 0)
+		ret = reg_read(client, MT9T031_SHUTTER_WIDTH);
+	*data |= ret & 0xffff;
+
+	return ret < 0 ? ret : 0;
+}
+
+static int mt9t031_idle(struct i2c_client *client)
+{
+	int ret;
+
+	/* Disable chip output, synchronous option update */
+	ret = reg_write(client, MT9T031_RESET, 1);
+	if (ret >= 0)
+		ret = reg_write(client, MT9T031_RESET, 0);
+	if (ret >= 0)
+		ret = reg_clear(client, MT9T031_OUTPUT_CONTROL, 2);
+
+	return ret >= 0 ? 0 : -EIO;
+}
+
+static int mt9t031_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+	if (enable)
+		/* Switch to master "normal" mode */
+		ret = reg_set(client, MT9T031_OUTPUT_CONTROL, 2);
+	else
+		/* Stop sensor readout */
+		ret = reg_clear(client, MT9T031_OUTPUT_CONTROL, 2);
+
+	if (ret < 0)
+		return -EIO;
+
+	return 0;
+}
+
+/* target must be _even_ */
+static u16 mt9t031_skip(s32 *source, s32 target, s32 max)
+{
+	unsigned int skip;
+
+	if (*source < target + target / 2) {
+		*source = target;
+		return 1;
+	}
+
+	skip = min(max, *source + target / 2) / target;
+	if (skip > 8)
+		skip = 8;
+	*source = target * skip;
+
+	return skip;
+}
+
+/* rect is the sensor rectangle, the caller guarantees parameter validity */
+static int mt9t031_set_params(struct i2c_client *client,
+			      struct v4l2_rect *rect, u16 xskip, u16 yskip)
+{
+	struct mt9t031 *mt9t031 = to_mt9t031(client);
+	int ret;
+	u16 xbin, ybin;
+	const u16 hblank = MT9T031_HORIZONTAL_BLANK,
+		vblank = MT9T031_VERTICAL_BLANK;
+
+	xbin = min(xskip, (u16)3);
+	ybin = min(yskip, (u16)3);
+
+	/*
+	 * Could just do roundup(rect->left, [xy]bin * 2); but this is cheaper.
+	 * There is always a valid suitably aligned value. The worst case is
+	 * xbin = 3, width = 2048. Then we will start at 36, the last read out
+	 * pixel will be 2083, which is < 2085 - first black pixel.
+	 *
+	 * MT9T031 datasheet imposes window left border alignment, depending on
+	 * the selected xskip. Failing to conform to this requirement produces
+	 * dark horizontal stripes in the image. However, even obeying to this
+	 * requirement doesn't eliminate the stripes in all configurations. They
+	 * appear "locally reproducibly," but can differ between tests under
+	 * different lighting conditions.
+	 */
+	switch (xbin) {
+	case 1:
+		rect->left &= ~1;
+		break;
+	case 2:
+		rect->left &= ~3;
+		break;
+	case 3:
+		rect->left = rect->left > roundup(MT9T031_COLUMN_SKIP, 6) ?
+			(rect->left / 6) * 6 : roundup(MT9T031_COLUMN_SKIP, 6);
+	}
+
+	rect->top &= ~1;
+
+	dev_dbg(&client->dev, "skip %u:%u, rect %ux%u@%u:%u\n",
+		xskip, yskip, rect->width, rect->height, rect->left, rect->top);
+
+	/* Disable register update, reconfigure atomically */
+	ret = reg_set(client, MT9T031_OUTPUT_CONTROL, 1);
+	if (ret < 0)
+		return ret;
+
+	/* Blanking and start values - default... */
+	ret = reg_write(client, MT9T031_HORIZONTAL_BLANKING, hblank);
+	if (ret >= 0)
+		ret = reg_write(client, MT9T031_VERTICAL_BLANKING, vblank);
+
+	if (yskip != mt9t031->yskip || xskip != mt9t031->xskip) {
+		/* Binning, skipping */
+		if (ret >= 0)
+			ret = reg_write(client, MT9T031_COLUMN_ADDRESS_MODE,
+					((xbin - 1) << 4) | (xskip - 1));
+		if (ret >= 0)
+			ret = reg_write(client, MT9T031_ROW_ADDRESS_MODE,
+					((ybin - 1) << 4) | (yskip - 1));
+	}
+	dev_dbg(&client->dev, "new physical left %u, top %u\n",
+		rect->left, rect->top);
+
+	/*
+	 * The caller provides a supported format, as guaranteed by
+	 * .try_mbus_fmt(), soc_camera_s_crop() and soc_camera_cropcap()
+	 */
+	if (ret >= 0)
+		ret = reg_write(client, MT9T031_COLUMN_START, rect->left);
+	if (ret >= 0)
+		ret = reg_write(client, MT9T031_ROW_START, rect->top);
+	if (ret >= 0)
+		ret = reg_write(client, MT9T031_WINDOW_WIDTH, rect->width - 1);
+	if (ret >= 0)
+		ret = reg_write(client, MT9T031_WINDOW_HEIGHT,
+				rect->height + mt9t031->y_skip_top - 1);
+	if (ret >= 0 && v4l2_ctrl_g_ctrl(mt9t031->autoexposure) == V4L2_EXPOSURE_AUTO) {
+		mt9t031->total_h = rect->height + mt9t031->y_skip_top + vblank;
+
+		ret = set_shutter(client, mt9t031->total_h);
+	}
+
+	/* Re-enable register update, commit all changes */
+	if (ret >= 0)
+		ret = reg_clear(client, MT9T031_OUTPUT_CONTROL, 1);
+
+	if (ret >= 0) {
+		mt9t031->rect = *rect;
+		mt9t031->xskip = xskip;
+		mt9t031->yskip = yskip;
+	}
+
+	return ret < 0 ? ret : 0;
+}
+
+static int mt9t031_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
+{
+	struct v4l2_rect rect = a->c;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9t031 *mt9t031 = to_mt9t031(client);
+
+	rect.width = ALIGN(rect.width, 2);
+	rect.height = ALIGN(rect.height, 2);
+
+	soc_camera_limit_side(&rect.left, &rect.width,
+		     MT9T031_COLUMN_SKIP, MT9T031_MIN_WIDTH, MT9T031_MAX_WIDTH);
+
+	soc_camera_limit_side(&rect.top, &rect.height,
+		     MT9T031_ROW_SKIP, MT9T031_MIN_HEIGHT, MT9T031_MAX_HEIGHT);
+
+	return mt9t031_set_params(client, &rect, mt9t031->xskip, mt9t031->yskip);
+}
+
+static int mt9t031_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9t031 *mt9t031 = to_mt9t031(client);
+
+	a->c	= mt9t031->rect;
+	a->type	= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	return 0;
+}
+
+static int mt9t031_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
+{
+	a->bounds.left			= MT9T031_COLUMN_SKIP;
+	a->bounds.top			= MT9T031_ROW_SKIP;
+	a->bounds.width			= MT9T031_MAX_WIDTH;
+	a->bounds.height		= MT9T031_MAX_HEIGHT;
+	a->defrect			= a->bounds;
+	a->type				= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	a->pixelaspect.numerator	= 1;
+	a->pixelaspect.denominator	= 1;
+
+	return 0;
+}
+
+static int mt9t031_g_fmt(struct v4l2_subdev *sd,
+			 struct v4l2_mbus_framefmt *mf)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9t031 *mt9t031 = to_mt9t031(client);
+
+	mf->width	= mt9t031->rect.width / mt9t031->xskip;
+	mf->height	= mt9t031->rect.height / mt9t031->yskip;
+	mf->code	= V4L2_MBUS_FMT_SBGGR10_1X10;
+	mf->colorspace	= V4L2_COLORSPACE_SRGB;
+	mf->field	= V4L2_FIELD_NONE;
+
+	return 0;
+}
+
+static int mt9t031_s_fmt(struct v4l2_subdev *sd,
+			 struct v4l2_mbus_framefmt *mf)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9t031 *mt9t031 = to_mt9t031(client);
+	u16 xskip, yskip;
+	struct v4l2_rect rect = mt9t031->rect;
+
+	/*
+	 * try_fmt has put width and height within limits.
+	 * S_FMT: use binning and skipping for scaling
+	 */
+	xskip = mt9t031_skip(&rect.width, mf->width, MT9T031_MAX_WIDTH);
+	yskip = mt9t031_skip(&rect.height, mf->height, MT9T031_MAX_HEIGHT);
+
+	mf->code	= V4L2_MBUS_FMT_SBGGR10_1X10;
+	mf->colorspace	= V4L2_COLORSPACE_SRGB;
+
+	/* mt9t031_set_params() doesn't change width and height */
+	return mt9t031_set_params(client, &rect, xskip, yskip);
+}
+
+/*
+ * If a user window larger than sensor window is requested, we'll increase the
+ * sensor window.
+ */
+static int mt9t031_try_fmt(struct v4l2_subdev *sd,
+			   struct v4l2_mbus_framefmt *mf)
+{
+	v4l_bound_align_image(
+		&mf->width, MT9T031_MIN_WIDTH, MT9T031_MAX_WIDTH, 1,
+		&mf->height, MT9T031_MIN_HEIGHT, MT9T031_MAX_HEIGHT, 1, 0);
+
+	mf->code	= V4L2_MBUS_FMT_SBGGR10_1X10;
+	mf->colorspace	= V4L2_COLORSPACE_SRGB;
+
+	return 0;
+}
+
+static int mt9t031_g_chip_ident(struct v4l2_subdev *sd,
+				struct v4l2_dbg_chip_ident *id)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9t031 *mt9t031 = to_mt9t031(client);
+
+	if (id->match.type != V4L2_CHIP_MATCH_I2C_ADDR)
+		return -EINVAL;
+
+	if (id->match.addr != client->addr)
+		return -ENODEV;
+
+	id->ident	= mt9t031->model;
+	id->revision	= 0;
+
+	return 0;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int mt9t031_g_register(struct v4l2_subdev *sd,
+			      struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (reg->match.type != V4L2_CHIP_MATCH_I2C_ADDR || reg->reg > 0xff)
+		return -EINVAL;
+
+	if (reg->match.addr != client->addr)
+		return -ENODEV;
+
+	reg->val = reg_read(client, reg->reg);
+
+	if (reg->val > 0xffff)
+		return -EIO;
+
+	return 0;
+}
+
+static int mt9t031_s_register(struct v4l2_subdev *sd,
+			      struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (reg->match.type != V4L2_CHIP_MATCH_I2C_ADDR || reg->reg > 0xff)
+		return -EINVAL;
+
+	if (reg->match.addr != client->addr)
+		return -ENODEV;
+
+	if (reg_write(client, reg->reg, reg->val) < 0)
+		return -EIO;
+
+	return 0;
+}
+#endif
+
+static int mt9t031_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct mt9t031 *mt9t031 = container_of(ctrl->handler,
+					       struct mt9t031, hdl);
+	const u32 shutter_max = MT9T031_MAX_HEIGHT + MT9T031_VERTICAL_BLANK;
+	s32 min, max;
+
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE_AUTO:
+		min = mt9t031->exposure->minimum;
+		max = mt9t031->exposure->maximum;
+		mt9t031->exposure->val =
+			(shutter_max / 2 + (mt9t031->total_h - 1) * (max - min))
+				/ shutter_max + min;
+		break;
+	}
+	return 0;
+}
+
+static int mt9t031_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct mt9t031 *mt9t031 = container_of(ctrl->handler,
+					       struct mt9t031, hdl);
+	struct v4l2_subdev *sd = &mt9t031->subdev;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct v4l2_ctrl *exp = mt9t031->exposure;
+	int data;
+
+	switch (ctrl->id) {
+	case V4L2_CID_VFLIP:
+		if (ctrl->val)
+			data = reg_set(client, MT9T031_READ_MODE_2, 0x8000);
+		else
+			data = reg_clear(client, MT9T031_READ_MODE_2, 0x8000);
+		if (data < 0)
+			return -EIO;
+		return 0;
+	case V4L2_CID_HFLIP:
+		if (ctrl->val)
+			data = reg_set(client, MT9T031_READ_MODE_2, 0x4000);
+		else
+			data = reg_clear(client, MT9T031_READ_MODE_2, 0x4000);
+		if (data < 0)
+			return -EIO;
+		return 0;
+	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 - ctrl->minimum) * 8 + range / 2) / range;
+
+			dev_dbg(&client->dev, "Setting gain %d\n", data);
+			data = reg_write(client, MT9T031_GLOBAL_GAIN, data);
+			if (data < 0)
+				return -EIO;
+		} else {
+			/* Pack it into 1.125..128 variable step, register values 9..0x7860 */
+			/* We assume qctrl->maximum - qctrl->default_value - 1 > 0 */
+			unsigned long range = ctrl->maximum - ctrl->default_value - 1;
+			/* calculated gain: map 65..127 to 9..1024 step 0.125 */
+			unsigned long gain = ((ctrl->val - ctrl->default_value - 1) *
+					       1015 + range / 2) / range + 9;
+
+			if (gain <= 32)		/* calculated gain 9..32 -> 9..32 */
+				data = gain;
+			else if (gain <= 64)	/* calculated gain 33..64 -> 0x51..0x60 */
+				data = ((gain - 32) * 16 + 16) / 32 + 80;
+			else
+				/* calculated gain 65..1024 -> (1..120) << 8 + 0x60 */
+				data = (((gain - 64 + 7) * 32) & 0xff00) | 0x60;
+
+			dev_dbg(&client->dev, "Set gain from 0x%x to 0x%x\n",
+				reg_read(client, MT9T031_GLOBAL_GAIN), data);
+			data = reg_write(client, MT9T031_GLOBAL_GAIN, data);
+			if (data < 0)
+				return -EIO;
+		}
+		return 0;
+
+	case V4L2_CID_EXPOSURE_AUTO:
+		if (ctrl->val == V4L2_EXPOSURE_MANUAL) {
+			unsigned int range = exp->maximum - exp->minimum;
+			unsigned int shutter = ((exp->val - exp->minimum) * 1048 +
+						 range / 2) / range + 1;
+			u32 old;
+
+			get_shutter(client, &old);
+			dev_dbg(&client->dev, "Set shutter from %u to %u\n",
+				old, shutter);
+			if (set_shutter(client, shutter) < 0)
+				return -EIO;
+		} else {
+			const u16 vblank = MT9T031_VERTICAL_BLANK;
+			mt9t031->total_h = mt9t031->rect.height +
+				mt9t031->y_skip_top + vblank;
+
+			if (set_shutter(client, mt9t031->total_h) < 0)
+				return -EIO;
+		}
+		return 0;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/*
+ * Power Management:
+ * This function does nothing for now but must be present for pm to work
+ */
+static int mt9t031_runtime_suspend(struct device *dev)
+{
+	return 0;
+}
+
+/*
+ * Power Management:
+ * COLUMN_ADDRESS_MODE and ROW_ADDRESS_MODE are not rewritten if unchanged
+ * they are however changed at reset if the platform hook is present
+ * thus we rewrite them with the values stored by the driver
+ */
+static int mt9t031_runtime_resume(struct device *dev)
+{
+	struct video_device *vdev = to_video_device(dev);
+	struct v4l2_subdev *sd = soc_camera_vdev_to_subdev(vdev);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9t031 *mt9t031 = to_mt9t031(client);
+
+	int ret;
+	u16 xbin, ybin;
+
+	xbin = min(mt9t031->xskip, (u16)3);
+	ybin = min(mt9t031->yskip, (u16)3);
+
+	ret = reg_write(client, MT9T031_COLUMN_ADDRESS_MODE,
+		((xbin - 1) << 4) | (mt9t031->xskip - 1));
+	if (ret < 0)
+		return ret;
+
+	ret = reg_write(client, MT9T031_ROW_ADDRESS_MODE,
+		((ybin - 1) << 4) | (mt9t031->yskip - 1));
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+static struct dev_pm_ops mt9t031_dev_pm_ops = {
+	.runtime_suspend	= mt9t031_runtime_suspend,
+	.runtime_resume		= mt9t031_runtime_resume,
+};
+
+static struct device_type mt9t031_dev_type = {
+	.name	= "MT9T031",
+	.pm	= &mt9t031_dev_pm_ops,
+};
+
+static int mt9t031_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+	struct video_device *vdev = soc_camera_i2c_to_vdev(client);
+	int ret;
+
+	if (on) {
+		ret = soc_camera_power_on(&client->dev, icl);
+		if (ret < 0)
+			return ret;
+		vdev->dev.type = &mt9t031_dev_type;
+	} else {
+		vdev->dev.type = NULL;
+		soc_camera_power_off(&client->dev, icl);
+	}
+
+	return 0;
+}
+
+/*
+ * 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 mt9t031_video_probe(struct i2c_client *client)
+{
+	struct mt9t031 *mt9t031 = to_mt9t031(client);
+	s32 data;
+	int ret;
+
+	ret = mt9t031_s_power(&mt9t031->subdev, 1);
+	if (ret < 0)
+		return ret;
+
+	ret = mt9t031_idle(client);
+	if (ret < 0) {
+		dev_err(&client->dev, "Failed to initialise the camera\n");
+		goto done;
+	}
+
+	/* Read out the chip version register */
+	data = reg_read(client, MT9T031_CHIP_VERSION);
+
+	switch (data) {
+	case 0x1621:
+		mt9t031->model = V4L2_IDENT_MT9T031;
+		break;
+	default:
+		dev_err(&client->dev,
+			"No MT9T031 chip detected, register read %x\n", data);
+		ret = -ENODEV;
+		goto done;
+	}
+
+	dev_info(&client->dev, "Detected a MT9T031 chip ID %x\n", data);
+
+	ret = v4l2_ctrl_handler_setup(&mt9t031->hdl);
+
+done:
+	mt9t031_s_power(&mt9t031->subdev, 0);
+
+	return ret;
+}
+
+static int mt9t031_g_skip_top_lines(struct v4l2_subdev *sd, u32 *lines)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9t031 *mt9t031 = to_mt9t031(client);
+
+	*lines = mt9t031->y_skip_top;
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops mt9t031_ctrl_ops = {
+	.g_volatile_ctrl = mt9t031_g_volatile_ctrl,
+	.s_ctrl = mt9t031_s_ctrl,
+};
+
+static struct v4l2_subdev_core_ops mt9t031_subdev_core_ops = {
+	.g_chip_ident	= mt9t031_g_chip_ident,
+	.s_power	= mt9t031_s_power,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register	= mt9t031_g_register,
+	.s_register	= mt9t031_s_register,
+#endif
+};
+
+static int mt9t031_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
+			    enum v4l2_mbus_pixelcode *code)
+{
+	if (index)
+		return -EINVAL;
+
+	*code = V4L2_MBUS_FMT_SBGGR10_1X10;
+	return 0;
+}
+
+static int mt9t031_g_mbus_config(struct v4l2_subdev *sd,
+				struct v4l2_mbus_config *cfg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+	cfg->flags = V4L2_MBUS_MASTER | V4L2_MBUS_PCLK_SAMPLE_RISING |
+		V4L2_MBUS_PCLK_SAMPLE_FALLING | V4L2_MBUS_HSYNC_ACTIVE_HIGH |
+		V4L2_MBUS_VSYNC_ACTIVE_HIGH | V4L2_MBUS_DATA_ACTIVE_HIGH;
+	cfg->type = V4L2_MBUS_PARALLEL;
+	cfg->flags = soc_camera_apply_board_flags(icl, cfg);
+
+	return 0;
+}
+
+static int mt9t031_s_mbus_config(struct v4l2_subdev *sd,
+				const struct v4l2_mbus_config *cfg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+	if (soc_camera_apply_board_flags(icl, cfg) &
+	    V4L2_MBUS_PCLK_SAMPLE_FALLING)
+		return reg_clear(client, MT9T031_PIXEL_CLOCK_CONTROL, 0x8000);
+	else
+		return reg_set(client, MT9T031_PIXEL_CLOCK_CONTROL, 0x8000);
+}
+
+static struct v4l2_subdev_video_ops mt9t031_subdev_video_ops = {
+	.s_stream	= mt9t031_s_stream,
+	.s_mbus_fmt	= mt9t031_s_fmt,
+	.g_mbus_fmt	= mt9t031_g_fmt,
+	.try_mbus_fmt	= mt9t031_try_fmt,
+	.s_crop		= mt9t031_s_crop,
+	.g_crop		= mt9t031_g_crop,
+	.cropcap	= mt9t031_cropcap,
+	.enum_mbus_fmt	= mt9t031_enum_fmt,
+	.g_mbus_config	= mt9t031_g_mbus_config,
+	.s_mbus_config	= mt9t031_s_mbus_config,
+};
+
+static struct v4l2_subdev_sensor_ops mt9t031_subdev_sensor_ops = {
+	.g_skip_top_lines	= mt9t031_g_skip_top_lines,
+};
+
+static struct v4l2_subdev_ops mt9t031_subdev_ops = {
+	.core	= &mt9t031_subdev_core_ops,
+	.video	= &mt9t031_subdev_video_ops,
+	.sensor	= &mt9t031_subdev_sensor_ops,
+};
+
+static int mt9t031_probe(struct i2c_client *client,
+			 const struct i2c_device_id *did)
+{
+	struct mt9t031 *mt9t031;
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
+	int ret;
+
+	if (!icl) {
+		dev_err(&client->dev, "MT9T031 driver needs platform data\n");
+		return -EINVAL;
+	}
+
+	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;
+	}
+
+	mt9t031 = kzalloc(sizeof(struct mt9t031), GFP_KERNEL);
+	if (!mt9t031)
+		return -ENOMEM;
+
+	v4l2_i2c_subdev_init(&mt9t031->subdev, client, &mt9t031_subdev_ops);
+	v4l2_ctrl_handler_init(&mt9t031->hdl, 5);
+	v4l2_ctrl_new_std(&mt9t031->hdl, &mt9t031_ctrl_ops,
+			V4L2_CID_VFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&mt9t031->hdl, &mt9t031_ctrl_ops,
+			V4L2_CID_HFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&mt9t031->hdl, &mt9t031_ctrl_ops,
+			V4L2_CID_GAIN, 0, 127, 1, 64);
+
+	/*
+	 * Simulated autoexposure. If enabled, we calculate shutter width
+	 * ourselves in the driver based on vertical blanking and frame width
+	 */
+	mt9t031->autoexposure = v4l2_ctrl_new_std_menu(&mt9t031->hdl,
+			&mt9t031_ctrl_ops, V4L2_CID_EXPOSURE_AUTO, 1, 0,
+			V4L2_EXPOSURE_AUTO);
+	mt9t031->exposure = v4l2_ctrl_new_std(&mt9t031->hdl, &mt9t031_ctrl_ops,
+			V4L2_CID_EXPOSURE, 1, 255, 1, 255);
+
+	mt9t031->subdev.ctrl_handler = &mt9t031->hdl;
+	if (mt9t031->hdl.error) {
+		int err = mt9t031->hdl.error;
+
+		kfree(mt9t031);
+		return err;
+	}
+	v4l2_ctrl_auto_cluster(2, &mt9t031->autoexposure,
+				V4L2_EXPOSURE_MANUAL, true);
+
+	mt9t031->y_skip_top	= 0;
+	mt9t031->rect.left	= MT9T031_COLUMN_SKIP;
+	mt9t031->rect.top	= MT9T031_ROW_SKIP;
+	mt9t031->rect.width	= MT9T031_MAX_WIDTH;
+	mt9t031->rect.height	= MT9T031_MAX_HEIGHT;
+
+	mt9t031->xskip = 1;
+	mt9t031->yskip = 1;
+
+	ret = mt9t031_video_probe(client);
+	if (ret) {
+		v4l2_ctrl_handler_free(&mt9t031->hdl);
+		kfree(mt9t031);
+	}
+
+	return ret;
+}
+
+static int mt9t031_remove(struct i2c_client *client)
+{
+	struct mt9t031 *mt9t031 = to_mt9t031(client);
+
+	v4l2_device_unregister_subdev(&mt9t031->subdev);
+	v4l2_ctrl_handler_free(&mt9t031->hdl);
+	kfree(mt9t031);
+
+	return 0;
+}
+
+static const struct i2c_device_id mt9t031_id[] = {
+	{ "mt9t031", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, mt9t031_id);
+
+static struct i2c_driver mt9t031_i2c_driver = {
+	.driver = {
+		.name = "mt9t031",
+	},
+	.probe		= mt9t031_probe,
+	.remove		= mt9t031_remove,
+	.id_table	= mt9t031_id,
+};
+
+module_i2c_driver(mt9t031_i2c_driver);
+
+MODULE_DESCRIPTION("Micron MT9T031 Camera driver");
+MODULE_AUTHOR("Guennadi Liakhovetski <lg@denx.de>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/soc_camera/mt9t112.c b/drivers/media/i2c/soc_camera/mt9t112.c
new file mode 100644
index 000000000000..de7cd836b0a2
--- /dev/null
+++ b/drivers/media/i2c/soc_camera/mt9t112.c
@@ -0,0 +1,1142 @@
+/*
+ * mt9t112 Camera Driver
+ *
+ * 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>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/delay.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/mt9t112.h>
+#include <media/soc_camera.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-common.h>
+
+/* you can check PLL/clock info */
+/* #define EXT_CLOCK 24000000 */
+
+/************************************************************************
+			macro
+************************************************************************/
+/*
+ * frame size
+ */
+#define MAX_WIDTH   2048
+#define MAX_HEIGHT  1536
+
+#define VGA_WIDTH   640
+#define VGA_HEIGHT  480
+
+/*
+ * 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 {
+	enum v4l2_mbus_pixelcode code;
+	enum v4l2_colorspace colorspace;
+	u16 fmt;
+	u16 order;
+};
+
+struct mt9t112_priv {
+	struct v4l2_subdev		 subdev;
+	struct mt9t112_camera_info	*info;
+	struct i2c_client		*client;
+	struct v4l2_rect		 frame;
+	const struct mt9t112_format	*format;
+	int				 model;
+	u32				 flags;
+/* for flags */
+#define INIT_DONE	(1 << 0)
+#define PCLK_RISING	(1 << 1)
+};
+
+/************************************************************************
+			supported format
+************************************************************************/
+
+static const struct mt9t112_format mt9t112_cfmts[] = {
+	{
+		.code		= V4L2_MBUS_FMT_UYVY8_2X8,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+		.fmt		= 1,
+		.order		= 0,
+	}, {
+		.code		= V4L2_MBUS_FMT_VYUY8_2X8,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+		.fmt		= 1,
+		.order		= 1,
+	}, {
+		.code		= V4L2_MBUS_FMT_YUYV8_2X8,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+		.fmt		= 1,
+		.order		= 2,
+	}, {
+		.code		= V4L2_MBUS_FMT_YVYU8_2X8,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+		.fmt		= 1,
+		.order		= 3,
+	}, {
+		.code		= V4L2_MBUS_FMT_RGB555_2X8_PADHI_LE,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.fmt		= 8,
+		.order		= 2,
+	}, {
+		.code		= V4L2_MBUS_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 mean error.
+	 * else, under 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);
+	if (ret >= 0)
+		ret = 0;
+
+	return 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);
+	msleep(1);
+	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 = ((6000 > ext) || (54000 < ext)) ? "X" : "";
+	dev_dbg(&client->dev, "EXTCLK          : %10u K %s\n", ext, enable);
+
+	vco = 2 * m * ext / (n+1);
+	enable = ((384000 > vco) || (768000 < vco)) ? "X" : "";
+	dev_dbg(&client->dev, "VCO             : %10u K %s\n", vco, enable);
+
+	clk = vco / (p1+1) / (p2+1);
+	enable = (96000 < clk) ? "X" : "";
+	dev_dbg(&client->dev, "PIXCLK          : %10u K %s\n", clk, enable);
+
+	clk = vco / (p3+1);
+	enable = (768000 < clk) ? "X" : "";
+	dev_dbg(&client->dev, "MIPICLK         : %10u K %s\n", clk, enable);
+
+	clk = vco / (p6+1);
+	enable = (96000 < clk) ? "X" : "";
+	dev_dbg(&client->dev, "MCU CLK         : %10u K %s\n", clk, enable);
+
+	clk = vco / (p5+1);
+	enable = (54000 < clk) ? "X" : "";
+	dev_dbg(&client->dev, "SOC CLK         : %10u K %s\n", clk, enable);
+
+	clk = vco / (p4+1);
+	enable = (70000 < clk) ? "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 = ((2000 > clk) || (24000 < clk)) ? "X" : "";
+	dev_dbg(&client->dev, "PFD             : %10u K %s\n", clk, enable);
+
+	return 0;
+}
+#endif
+
+static void mt9t112_frame_check(u32 *width, u32 *height, u32 *left, u32 *top)
+{
+	soc_camera_limit_side(left, width, 0, 0, MAX_WIDTH);
+	soc_camera_limit_side(top, height, 0, 0, MAX_HEIGHT);
+}
+
+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);
+	mt9t112_reg_write(ret, client, 0x0016, 0x0400); /* JPEG initialization workaround */
+	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 (!(0x4000 & data))
+			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 Dectection 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
+************************************************************************/
+static int mt9t112_g_chip_ident(struct v4l2_subdev *sd,
+				struct v4l2_dbg_chip_ident *id)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9t112_priv *priv = to_mt9t112(client);
+
+	id->ident    = priv->model;
+	id->revision = 0;
+
+	return 0;
+}
+
+#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,
+			      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_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+	return soc_camera_set_power(&client->dev, icl, on);
+}
+
+static struct v4l2_subdev_core_ops mt9t112_subdev_core_ops = {
+	.g_chip_ident	= mt9t112_g_chip_ident,
+#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->flags & INIT_DONE)) {
+		u16 param = PCLK_RISING & priv->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->flags |= INIT_DONE;
+	}
+
+	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,
+			      enum v4l2_mbus_pixelcode code)
+{
+	int i;
+
+	/*
+	 * get color format
+	 */
+	for (i = 0; i < ARRAY_SIZE(mt9t112_cfmts); i++)
+		if (mt9t112_cfmts[i].code == code)
+			break;
+
+	if (i == ARRAY_SIZE(mt9t112_cfmts))
+		return -EINVAL;
+
+	priv->frame  = *rect;
+
+	/*
+	 * frame size check
+	 */
+	mt9t112_frame_check(&priv->frame.width, &priv->frame.height,
+			    &priv->frame.left, &priv->frame.top);
+
+	priv->format = mt9t112_cfmts + i;
+
+	return 0;
+}
+
+static int mt9t112_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
+{
+	a->bounds.left			= 0;
+	a->bounds.top			= 0;
+	a->bounds.width			= MAX_WIDTH;
+	a->bounds.height		= MAX_HEIGHT;
+	a->defrect.left			= 0;
+	a->defrect.top			= 0;
+	a->defrect.width		= VGA_WIDTH;
+	a->defrect.height		= VGA_HEIGHT;
+	a->type				= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	a->pixelaspect.numerator	= 1;
+	a->pixelaspect.denominator	= 1;
+
+	return 0;
+}
+
+static int mt9t112_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9t112_priv *priv = to_mt9t112(client);
+
+	a->c	= priv->frame;
+	a->type	= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	return 0;
+}
+
+static int mt9t112_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9t112_priv *priv = to_mt9t112(client);
+	const struct v4l2_rect *rect = &a->c;
+
+	return mt9t112_set_params(priv, rect, priv->format->code);
+}
+
+static int mt9t112_g_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);
+
+	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_try_fmt(struct v4l2_subdev *sd,
+			   struct v4l2_mbus_framefmt *mf)
+{
+	unsigned int top, left;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(mt9t112_cfmts); i++)
+		if (mt9t112_cfmts[i].code == mf->code)
+			break;
+
+	if (i == ARRAY_SIZE(mt9t112_cfmts)) {
+		mf->code = V4L2_MBUS_FMT_UYVY8_2X8;
+		mf->colorspace = V4L2_COLORSPACE_JPEG;
+	} else {
+		mf->colorspace	= mt9t112_cfmts[i].colorspace;
+	}
+
+	mt9t112_frame_check(&mf->width, &mf->height, &left, &top);
+
+	mf->field = V4L2_FIELD_NONE;
+
+	return 0;
+}
+
+static int mt9t112_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
+			   enum v4l2_mbus_pixelcode *code)
+{
+	if (index >= ARRAY_SIZE(mt9t112_cfmts))
+		return -EINVAL;
+
+	*code = mt9t112_cfmts[index].code;
+
+	return 0;
+}
+
+static int mt9t112_g_mbus_config(struct v4l2_subdev *sd,
+				 struct v4l2_mbus_config *cfg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+	cfg->flags = V4L2_MBUS_MASTER | V4L2_MBUS_VSYNC_ACTIVE_HIGH |
+		V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_DATA_ACTIVE_HIGH |
+		V4L2_MBUS_PCLK_SAMPLE_RISING | V4L2_MBUS_PCLK_SAMPLE_FALLING;
+	cfg->type = V4L2_MBUS_PARALLEL;
+	cfg->flags = soc_camera_apply_board_flags(icl, cfg);
+
+	return 0;
+}
+
+static int mt9t112_s_mbus_config(struct v4l2_subdev *sd,
+				 const struct v4l2_mbus_config *cfg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+	struct mt9t112_priv *priv = to_mt9t112(client);
+
+	if (soc_camera_apply_board_flags(icl, cfg) & V4L2_MBUS_PCLK_SAMPLE_RISING)
+		priv->flags |= PCLK_RISING;
+
+	return 0;
+}
+
+static struct v4l2_subdev_video_ops mt9t112_subdev_video_ops = {
+	.s_stream	= mt9t112_s_stream,
+	.g_mbus_fmt	= mt9t112_g_fmt,
+	.s_mbus_fmt	= mt9t112_s_fmt,
+	.try_mbus_fmt	= mt9t112_try_fmt,
+	.cropcap	= mt9t112_cropcap,
+	.g_crop		= mt9t112_g_crop,
+	.s_crop		= mt9t112_s_crop,
+	.enum_mbus_fmt	= mt9t112_enum_fmt,
+	.g_mbus_config	= mt9t112_g_mbus_config,
+	.s_mbus_config	= mt9t112_s_mbus_config,
+};
+
+/************************************************************************
+			i2c driver
+************************************************************************/
+static struct v4l2_subdev_ops mt9t112_subdev_ops = {
+	.core	= &mt9t112_subdev_core_ops,
+	.video	= &mt9t112_subdev_video_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->model = V4L2_IDENT_MT9T111;
+		break;
+	case 0x2682:
+		devname = "mt9t112";
+		priv->model = V4L2_IDENT_MT9T112;
+		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,
+			 const struct i2c_device_id *did)
+{
+	struct mt9t112_priv *priv;
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+	struct v4l2_rect rect = {
+		.width = VGA_WIDTH,
+		.height = VGA_HEIGHT,
+		.left = (MAX_WIDTH - VGA_WIDTH) / 2,
+		.top = (MAX_HEIGHT - VGA_HEIGHT) / 2,
+	};
+	int ret;
+
+	if (!icl || !icl->priv) {
+		dev_err(&client->dev, "mt9t112: missing platform data!\n");
+		return -EINVAL;
+	}
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->info = icl->priv;
+
+	v4l2_i2c_subdev_init(&priv->subdev, client, &mt9t112_subdev_ops);
+
+	ret = mt9t112_camera_probe(client);
+	if (ret) {
+		kfree(priv);
+		return ret;
+	}
+
+	/* Cannot fail: using the default supported pixel code */
+	mt9t112_set_params(priv, &rect, V4L2_MBUS_FMT_UYVY8_2X8);
+
+	return ret;
+}
+
+static int mt9t112_remove(struct i2c_client *client)
+{
+	struct mt9t112_priv *priv = to_mt9t112(client);
+
+	kfree(priv);
+	return 0;
+}
+
+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("SoC Camera driver for mt9t112");
+MODULE_AUTHOR("Kuninori Morimoto");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/soc_camera/mt9v022.c b/drivers/media/i2c/soc_camera/mt9v022.c
new file mode 100644
index 000000000000..13057b966ee9
--- /dev/null
+++ b/drivers/media/i2c/soc_camera/mt9v022.c
@@ -0,0 +1,919 @@
+/*
+ * Driver for MT9V022 CMOS Image Sensor from Micron
+ *
+ * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/videodev2.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <linux/log2.h>
+#include <linux/module.h>
+
+#include <media/soc_camera.h>
+#include <media/soc_mediabus.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-ctrls.h>
+
+/*
+ * mt9v022 i2c address 0x48, 0x4c, 0x58, 0x5c
+ * The platform has to define struct i2c_board_info objects and link to them
+ * from struct soc_camera_link
+ */
+
+static char *sensor_type;
+module_param(sensor_type, charp, S_IRUGO);
+MODULE_PARM_DESC(sensor_type, "Sensor type: \"colour\" or \"monochrome\"");
+
+/* mt9v022 selected register addresses */
+#define MT9V022_CHIP_VERSION		0x00
+#define MT9V022_COLUMN_START		0x01
+#define MT9V022_ROW_START		0x02
+#define MT9V022_WINDOW_HEIGHT		0x03
+#define MT9V022_WINDOW_WIDTH		0x04
+#define MT9V022_HORIZONTAL_BLANKING	0x05
+#define MT9V022_VERTICAL_BLANKING	0x06
+#define MT9V022_CHIP_CONTROL		0x07
+#define MT9V022_SHUTTER_WIDTH1		0x08
+#define MT9V022_SHUTTER_WIDTH2		0x09
+#define MT9V022_SHUTTER_WIDTH_CTRL	0x0a
+#define MT9V022_TOTAL_SHUTTER_WIDTH	0x0b
+#define MT9V022_RESET			0x0c
+#define MT9V022_READ_MODE		0x0d
+#define MT9V022_MONITOR_MODE		0x0e
+#define MT9V022_PIXEL_OPERATION_MODE	0x0f
+#define MT9V022_LED_OUT_CONTROL		0x1b
+#define MT9V022_ADC_MODE_CONTROL	0x1c
+#define MT9V022_ANALOG_GAIN		0x35
+#define MT9V022_BLACK_LEVEL_CALIB_CTRL	0x47
+#define MT9V022_PIXCLK_FV_LV		0x74
+#define MT9V022_DIGITAL_TEST_PATTERN	0x7f
+#define MT9V022_AEC_AGC_ENABLE		0xAF
+#define MT9V022_MAX_TOTAL_SHUTTER_WIDTH	0xBD
+
+/* mt9v024 partial list register addresses changes with respect to mt9v022 */
+#define MT9V024_PIXCLK_FV_LV		0x72
+#define MT9V024_MAX_TOTAL_SHUTTER_WIDTH	0xAD
+
+/* Progressive scan, master, defaults */
+#define MT9V022_CHIP_CONTROL_DEFAULT	0x188
+
+#define MT9V022_MAX_WIDTH		752
+#define MT9V022_MAX_HEIGHT		480
+#define MT9V022_MIN_WIDTH		48
+#define MT9V022_MIN_HEIGHT		32
+#define MT9V022_COLUMN_SKIP		1
+#define MT9V022_ROW_SKIP		4
+
+#define is_mt9v024(id) (id == 0x1324)
+
+/* MT9V022 has only one fixed colorspace per pixelcode */
+struct mt9v022_datafmt {
+	enum v4l2_mbus_pixelcode	code;
+	enum v4l2_colorspace		colorspace;
+};
+
+/* Find a data format by a pixel code in an array */
+static const struct mt9v022_datafmt *mt9v022_find_datafmt(
+	enum v4l2_mbus_pixelcode code, const struct mt9v022_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 mt9v022_datafmt mt9v022_colour_fmts[] = {
+	/*
+	 * Order important: first natively supported,
+	 * second supported with a GPIO extender
+	 */
+	{V4L2_MBUS_FMT_SBGGR10_1X10, V4L2_COLORSPACE_SRGB},
+	{V4L2_MBUS_FMT_SBGGR8_1X8, V4L2_COLORSPACE_SRGB},
+};
+
+static const struct mt9v022_datafmt mt9v022_monochrome_fmts[] = {
+	/* Order important - see above */
+	{V4L2_MBUS_FMT_Y10_1X10, V4L2_COLORSPACE_JPEG},
+	{V4L2_MBUS_FMT_Y8_1X8, V4L2_COLORSPACE_JPEG},
+};
+
+/* only registers with different addresses on different mt9v02x sensors */
+struct mt9v02x_register {
+	u8	max_total_shutter_width;
+	u8	pixclk_fv_lv;
+};
+
+static const struct mt9v02x_register mt9v022_register = {
+	.max_total_shutter_width	= MT9V022_MAX_TOTAL_SHUTTER_WIDTH,
+	.pixclk_fv_lv			= MT9V022_PIXCLK_FV_LV,
+};
+
+static const struct mt9v02x_register mt9v024_register = {
+	.max_total_shutter_width	= MT9V024_MAX_TOTAL_SHUTTER_WIDTH,
+	.pixclk_fv_lv			= MT9V024_PIXCLK_FV_LV,
+};
+
+struct mt9v022 {
+	struct v4l2_subdev subdev;
+	struct v4l2_ctrl_handler hdl;
+	struct {
+		/* exposure/auto-exposure cluster */
+		struct v4l2_ctrl *autoexposure;
+		struct v4l2_ctrl *exposure;
+	};
+	struct {
+		/* gain/auto-gain cluster */
+		struct v4l2_ctrl *autogain;
+		struct v4l2_ctrl *gain;
+	};
+	struct v4l2_rect rect;	/* Sensor window */
+	const struct mt9v022_datafmt *fmt;
+	const struct mt9v022_datafmt *fmts;
+	const struct mt9v02x_register *reg;
+	int num_fmts;
+	int model;	/* V4L2_IDENT_MT9V022* codes from v4l2-chip-ident.h */
+	u16 chip_control;
+	unsigned short y_skip_top;	/* Lines to skip at the top */
+};
+
+static struct mt9v022 *to_mt9v022(const struct i2c_client *client)
+{
+	return container_of(i2c_get_clientdata(client), struct mt9v022, 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);
+}
+
+static int mt9v022_init(struct i2c_client *client)
+{
+	struct mt9v022 *mt9v022 = to_mt9v022(client);
+	int ret;
+
+	/*
+	 * Almost the default mode: master, parallel, simultaneous, and an
+	 * undocumented bit 0x200, which is present in table 7, but not in 8,
+	 * plus snapshot mode to disable scan for now
+	 */
+	mt9v022->chip_control |= 0x10;
+	ret = reg_write(client, MT9V022_CHIP_CONTROL, mt9v022->chip_control);
+	if (!ret)
+		ret = reg_write(client, MT9V022_READ_MODE, 0x300);
+
+	/* All defaults */
+	if (!ret)
+		/* AEC, AGC on */
+		ret = reg_set(client, MT9V022_AEC_AGC_ENABLE, 0x3);
+	if (!ret)
+		ret = reg_write(client, MT9V022_ANALOG_GAIN, 16);
+	if (!ret)
+		ret = reg_write(client, MT9V022_TOTAL_SHUTTER_WIDTH, 480);
+	if (!ret)
+		ret = reg_write(client, mt9v022->reg->max_total_shutter_width, 480);
+	if (!ret)
+		/* default - auto */
+		ret = reg_clear(client, MT9V022_BLACK_LEVEL_CALIB_CTRL, 1);
+	if (!ret)
+		ret = reg_write(client, MT9V022_DIGITAL_TEST_PATTERN, 0);
+	if (!ret)
+		return v4l2_ctrl_handler_setup(&mt9v022->hdl);
+
+	return ret;
+}
+
+static int mt9v022_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9v022 *mt9v022 = to_mt9v022(client);
+
+	if (enable)
+		/* Switch to master "normal" mode */
+		mt9v022->chip_control &= ~0x10;
+	else
+		/* Switch to snapshot mode */
+		mt9v022->chip_control |= 0x10;
+
+	if (reg_write(client, MT9V022_CHIP_CONTROL, mt9v022->chip_control) < 0)
+		return -EIO;
+	return 0;
+}
+
+static int mt9v022_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9v022 *mt9v022 = to_mt9v022(client);
+	struct v4l2_rect rect = a->c;
+	int ret;
+
+	/* Bayer format - even size lengths */
+	if (mt9v022->fmts == mt9v022_colour_fmts) {
+		rect.width	= ALIGN(rect.width, 2);
+		rect.height	= ALIGN(rect.height, 2);
+		/* Let the user play with the starting pixel */
+	}
+
+	soc_camera_limit_side(&rect.left, &rect.width,
+		     MT9V022_COLUMN_SKIP, MT9V022_MIN_WIDTH, MT9V022_MAX_WIDTH);
+
+	soc_camera_limit_side(&rect.top, &rect.height,
+		     MT9V022_ROW_SKIP, MT9V022_MIN_HEIGHT, MT9V022_MAX_HEIGHT);
+
+	/* Like in example app. Contradicts the datasheet though */
+	ret = reg_read(client, MT9V022_AEC_AGC_ENABLE);
+	if (ret >= 0) {
+		if (ret & 1) /* Autoexposure */
+			ret = reg_write(client, mt9v022->reg->max_total_shutter_width,
+					rect.height + mt9v022->y_skip_top + 43);
+		else
+			ret = reg_write(client, MT9V022_TOTAL_SHUTTER_WIDTH,
+					rect.height + mt9v022->y_skip_top + 43);
+	}
+	/* Setup frame format: defaults apart from width and height */
+	if (!ret)
+		ret = reg_write(client, MT9V022_COLUMN_START, rect.left);
+	if (!ret)
+		ret = reg_write(client, MT9V022_ROW_START, rect.top);
+	if (!ret)
+		/*
+		 * Default 94, Phytec driver says:
+		 * "width + horizontal blank >= 660"
+		 */
+		ret = reg_write(client, MT9V022_HORIZONTAL_BLANKING,
+				rect.width > 660 - 43 ? 43 :
+				660 - rect.width);
+	if (!ret)
+		ret = reg_write(client, MT9V022_VERTICAL_BLANKING, 45);
+	if (!ret)
+		ret = reg_write(client, MT9V022_WINDOW_WIDTH, rect.width);
+	if (!ret)
+		ret = reg_write(client, MT9V022_WINDOW_HEIGHT,
+				rect.height + mt9v022->y_skip_top);
+
+	if (ret < 0)
+		return ret;
+
+	dev_dbg(&client->dev, "Frame %dx%d pixel\n", rect.width, rect.height);
+
+	mt9v022->rect = rect;
+
+	return 0;
+}
+
+static int mt9v022_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9v022 *mt9v022 = to_mt9v022(client);
+
+	a->c	= mt9v022->rect;
+	a->type	= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	return 0;
+}
+
+static int mt9v022_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
+{
+	a->bounds.left			= MT9V022_COLUMN_SKIP;
+	a->bounds.top			= MT9V022_ROW_SKIP;
+	a->bounds.width			= MT9V022_MAX_WIDTH;
+	a->bounds.height		= MT9V022_MAX_HEIGHT;
+	a->defrect			= a->bounds;
+	a->type				= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	a->pixelaspect.numerator	= 1;
+	a->pixelaspect.denominator	= 1;
+
+	return 0;
+}
+
+static int mt9v022_g_fmt(struct v4l2_subdev *sd,
+			 struct v4l2_mbus_framefmt *mf)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9v022 *mt9v022 = to_mt9v022(client);
+
+	mf->width	= mt9v022->rect.width;
+	mf->height	= mt9v022->rect.height;
+	mf->code	= mt9v022->fmt->code;
+	mf->colorspace	= mt9v022->fmt->colorspace;
+	mf->field	= V4L2_FIELD_NONE;
+
+	return 0;
+}
+
+static int mt9v022_s_fmt(struct v4l2_subdev *sd,
+			 struct v4l2_mbus_framefmt *mf)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9v022 *mt9v022 = to_mt9v022(client);
+	struct v4l2_crop a = {
+		.c = {
+			.left	= mt9v022->rect.left,
+			.top	= mt9v022->rect.top,
+			.width	= mf->width,
+			.height	= mf->height,
+		},
+	};
+	int ret;
+
+	/*
+	 * The caller provides a supported format, as verified per call to
+	 * .try_mbus_fmt(), datawidth is from our supported format list
+	 */
+	switch (mf->code) {
+	case V4L2_MBUS_FMT_Y8_1X8:
+	case V4L2_MBUS_FMT_Y10_1X10:
+		if (mt9v022->model != V4L2_IDENT_MT9V022IX7ATM)
+			return -EINVAL;
+		break;
+	case V4L2_MBUS_FMT_SBGGR8_1X8:
+	case V4L2_MBUS_FMT_SBGGR10_1X10:
+		if (mt9v022->model != V4L2_IDENT_MT9V022IX7ATC)
+			return -EINVAL;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/* No support for scaling on this camera, just crop. */
+	ret = mt9v022_s_crop(sd, &a);
+	if (!ret) {
+		mf->width	= mt9v022->rect.width;
+		mf->height	= mt9v022->rect.height;
+		mt9v022->fmt	= mt9v022_find_datafmt(mf->code,
+					mt9v022->fmts, mt9v022->num_fmts);
+		mf->colorspace	= mt9v022->fmt->colorspace;
+	}
+
+	return ret;
+}
+
+static int mt9v022_try_fmt(struct v4l2_subdev *sd,
+			   struct v4l2_mbus_framefmt *mf)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9v022 *mt9v022 = to_mt9v022(client);
+	const struct mt9v022_datafmt *fmt;
+	int align = mf->code == V4L2_MBUS_FMT_SBGGR8_1X8 ||
+		mf->code == V4L2_MBUS_FMT_SBGGR10_1X10;
+
+	v4l_bound_align_image(&mf->width, MT9V022_MIN_WIDTH,
+		MT9V022_MAX_WIDTH, align,
+		&mf->height, MT9V022_MIN_HEIGHT + mt9v022->y_skip_top,
+		MT9V022_MAX_HEIGHT + mt9v022->y_skip_top, align, 0);
+
+	fmt = mt9v022_find_datafmt(mf->code, mt9v022->fmts,
+				   mt9v022->num_fmts);
+	if (!fmt) {
+		fmt = mt9v022->fmt;
+		mf->code = fmt->code;
+	}
+
+	mf->colorspace	= fmt->colorspace;
+
+	return 0;
+}
+
+static int mt9v022_g_chip_ident(struct v4l2_subdev *sd,
+				struct v4l2_dbg_chip_ident *id)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9v022 *mt9v022 = to_mt9v022(client);
+
+	if (id->match.type != V4L2_CHIP_MATCH_I2C_ADDR)
+		return -EINVAL;
+
+	if (id->match.addr != client->addr)
+		return -ENODEV;
+
+	id->ident	= mt9v022->model;
+	id->revision	= 0;
+
+	return 0;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int mt9v022_g_register(struct v4l2_subdev *sd,
+			      struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (reg->match.type != V4L2_CHIP_MATCH_I2C_ADDR || reg->reg > 0xff)
+		return -EINVAL;
+
+	if (reg->match.addr != client->addr)
+		return -ENODEV;
+
+	reg->size = 2;
+	reg->val = reg_read(client, reg->reg);
+
+	if (reg->val > 0xffff)
+		return -EIO;
+
+	return 0;
+}
+
+static int mt9v022_s_register(struct v4l2_subdev *sd,
+			      struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (reg->match.type != V4L2_CHIP_MATCH_I2C_ADDR || reg->reg > 0xff)
+		return -EINVAL;
+
+	if (reg->match.addr != client->addr)
+		return -ENODEV;
+
+	if (reg_write(client, reg->reg, reg->val) < 0)
+		return -EIO;
+
+	return 0;
+}
+#endif
+
+static int mt9v022_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+	return soc_camera_set_power(&client->dev, icl, on);
+}
+
+static int mt9v022_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct mt9v022 *mt9v022 = container_of(ctrl->handler,
+					       struct mt9v022, hdl);
+	struct v4l2_subdev *sd = &mt9v022->subdev;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct v4l2_ctrl *gain = mt9v022->gain;
+	struct v4l2_ctrl *exp = mt9v022->exposure;
+	unsigned long range;
+	int data;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTOGAIN:
+		data = reg_read(client, MT9V022_ANALOG_GAIN);
+		if (data < 0)
+			return -EIO;
+
+		range = gain->maximum - gain->minimum;
+		gain->val = ((data - 16) * range + 24) / 48 + gain->minimum;
+		return 0;
+	case V4L2_CID_EXPOSURE_AUTO:
+		data = reg_read(client, MT9V022_TOTAL_SHUTTER_WIDTH);
+		if (data < 0)
+			return -EIO;
+
+		range = exp->maximum - exp->minimum;
+		exp->val = ((data - 1) * range + 239) / 479 + exp->minimum;
+		return 0;
+	}
+	return -EINVAL;
+}
+
+static int mt9v022_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct mt9v022 *mt9v022 = container_of(ctrl->handler,
+					       struct mt9v022, hdl);
+	struct v4l2_subdev *sd = &mt9v022->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, MT9V022_READ_MODE, 0x10);
+		else
+			data = reg_clear(client, MT9V022_READ_MODE, 0x10);
+		if (data < 0)
+			return -EIO;
+		return 0;
+	case V4L2_CID_HFLIP:
+		if (ctrl->val)
+			data = reg_set(client, MT9V022_READ_MODE, 0x20);
+		else
+			data = reg_clear(client, MT9V022_READ_MODE, 0x20);
+		if (data < 0)
+			return -EIO;
+		return 0;
+	case V4L2_CID_AUTOGAIN:
+		if (ctrl->val) {
+			if (reg_set(client, MT9V022_AEC_AGC_ENABLE, 0x2) < 0)
+				return -EIO;
+		} else {
+			struct v4l2_ctrl *gain = mt9v022->gain;
+			/* mt9v022 has minimum == default */
+			unsigned long range = gain->maximum - gain->minimum;
+			/* Valid values 16 to 64, 32 to 64 must be even. */
+			unsigned long gain_val = ((gain->val - gain->minimum) *
+					      48 + range / 2) / range + 16;
+
+			if (gain_val >= 32)
+				gain_val &= ~1;
+
+			/*
+			 * The user wants to set gain manually, hope, she
+			 * knows, what she's doing... Switch AGC off.
+			 */
+			if (reg_clear(client, MT9V022_AEC_AGC_ENABLE, 0x2) < 0)
+				return -EIO;
+
+			dev_dbg(&client->dev, "Setting gain from %d to %lu\n",
+				reg_read(client, MT9V022_ANALOG_GAIN), gain_val);
+			if (reg_write(client, MT9V022_ANALOG_GAIN, gain_val) < 0)
+				return -EIO;
+		}
+		return 0;
+	case V4L2_CID_EXPOSURE_AUTO:
+		if (ctrl->val == V4L2_EXPOSURE_AUTO) {
+			data = reg_set(client, MT9V022_AEC_AGC_ENABLE, 0x1);
+		} else {
+			struct v4l2_ctrl *exp = mt9v022->exposure;
+			unsigned long range = exp->maximum - exp->minimum;
+			unsigned long shutter = ((exp->val - exp->minimum) *
+					479 + range / 2) / range + 1;
+
+			/*
+			 * The user wants to set shutter width manually, hope,
+			 * she knows, what she's doing... Switch AEC off.
+			 */
+			data = reg_clear(client, MT9V022_AEC_AGC_ENABLE, 0x1);
+			if (data < 0)
+				return -EIO;
+			dev_dbg(&client->dev, "Shutter width from %d to %lu\n",
+					reg_read(client, MT9V022_TOTAL_SHUTTER_WIDTH),
+					shutter);
+			if (reg_write(client, MT9V022_TOTAL_SHUTTER_WIDTH,
+						shutter) < 0)
+				return -EIO;
+		}
+		return 0;
+	}
+	return -EINVAL;
+}
+
+/*
+ * 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 mt9v022_video_probe(struct i2c_client *client)
+{
+	struct mt9v022 *mt9v022 = to_mt9v022(client);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+	s32 data;
+	int ret;
+	unsigned long flags;
+
+	ret = mt9v022_s_power(&mt9v022->subdev, 1);
+	if (ret < 0)
+		return ret;
+
+	/* Read out the chip version register */
+	data = reg_read(client, MT9V022_CHIP_VERSION);
+
+	/* must be 0x1311, 0x1313 or 0x1324 */
+	if (data != 0x1311 && data != 0x1313 && data != 0x1324) {
+		ret = -ENODEV;
+		dev_info(&client->dev, "No MT9V022 found, ID register 0x%x\n",
+			 data);
+		goto ei2c;
+	}
+
+	mt9v022->reg = is_mt9v024(data) ? &mt9v024_register :
+			&mt9v022_register;
+
+	/* Soft reset */
+	ret = reg_write(client, MT9V022_RESET, 1);
+	if (ret < 0)
+		goto ei2c;
+	/* 15 clock cycles */
+	udelay(200);
+	if (reg_read(client, MT9V022_RESET)) {
+		dev_err(&client->dev, "Resetting MT9V022 failed!\n");
+		if (ret > 0)
+			ret = -EIO;
+		goto ei2c;
+	}
+
+	/* Set monochrome or colour sensor type */
+	if (sensor_type && (!strcmp("colour", sensor_type) ||
+			    !strcmp("color", sensor_type))) {
+		ret = reg_write(client, MT9V022_PIXEL_OPERATION_MODE, 4 | 0x11);
+		mt9v022->model = V4L2_IDENT_MT9V022IX7ATC;
+		mt9v022->fmts = mt9v022_colour_fmts;
+	} else {
+		ret = reg_write(client, MT9V022_PIXEL_OPERATION_MODE, 0x11);
+		mt9v022->model = V4L2_IDENT_MT9V022IX7ATM;
+		mt9v022->fmts = mt9v022_monochrome_fmts;
+	}
+
+	if (ret < 0)
+		goto ei2c;
+
+	mt9v022->num_fmts = 0;
+
+	/*
+	 * This is a 10bit sensor, so by default we only allow 10bit.
+	 * The platform may support different bus widths due to
+	 * different routing of the data lines.
+	 */
+	if (icl->query_bus_param)
+		flags = icl->query_bus_param(icl);
+	else
+		flags = SOCAM_DATAWIDTH_10;
+
+	if (flags & SOCAM_DATAWIDTH_10)
+		mt9v022->num_fmts++;
+	else
+		mt9v022->fmts++;
+
+	if (flags & SOCAM_DATAWIDTH_8)
+		mt9v022->num_fmts++;
+
+	mt9v022->fmt = &mt9v022->fmts[0];
+
+	dev_info(&client->dev, "Detected a MT9V022 chip ID %x, %s sensor\n",
+		 data, mt9v022->model == V4L2_IDENT_MT9V022IX7ATM ?
+		 "monochrome" : "colour");
+
+	ret = mt9v022_init(client);
+	if (ret < 0)
+		dev_err(&client->dev, "Failed to initialise the camera\n");
+
+ei2c:
+	mt9v022_s_power(&mt9v022->subdev, 0);
+	return ret;
+}
+
+static int mt9v022_g_skip_top_lines(struct v4l2_subdev *sd, u32 *lines)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9v022 *mt9v022 = to_mt9v022(client);
+
+	*lines = mt9v022->y_skip_top;
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops mt9v022_ctrl_ops = {
+	.g_volatile_ctrl = mt9v022_g_volatile_ctrl,
+	.s_ctrl = mt9v022_s_ctrl,
+};
+
+static struct v4l2_subdev_core_ops mt9v022_subdev_core_ops = {
+	.g_chip_ident	= mt9v022_g_chip_ident,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register	= mt9v022_g_register,
+	.s_register	= mt9v022_s_register,
+#endif
+	.s_power	= mt9v022_s_power,
+};
+
+static int mt9v022_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
+			    enum v4l2_mbus_pixelcode *code)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9v022 *mt9v022 = to_mt9v022(client);
+
+	if (index >= mt9v022->num_fmts)
+		return -EINVAL;
+
+	*code = mt9v022->fmts[index].code;
+	return 0;
+}
+
+static int mt9v022_g_mbus_config(struct v4l2_subdev *sd,
+				struct v4l2_mbus_config *cfg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+	cfg->flags = V4L2_MBUS_MASTER | V4L2_MBUS_SLAVE |
+		V4L2_MBUS_PCLK_SAMPLE_RISING | V4L2_MBUS_PCLK_SAMPLE_FALLING |
+		V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_HSYNC_ACTIVE_LOW |
+		V4L2_MBUS_VSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_LOW |
+		V4L2_MBUS_DATA_ACTIVE_HIGH;
+	cfg->type = V4L2_MBUS_PARALLEL;
+	cfg->flags = soc_camera_apply_board_flags(icl, cfg);
+
+	return 0;
+}
+
+static int mt9v022_s_mbus_config(struct v4l2_subdev *sd,
+				 const struct v4l2_mbus_config *cfg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+	struct mt9v022 *mt9v022 = to_mt9v022(client);
+	unsigned long flags = soc_camera_apply_board_flags(icl, cfg);
+	unsigned int bps = soc_mbus_get_fmtdesc(mt9v022->fmt->code)->bits_per_sample;
+	int ret;
+	u16 pixclk = 0;
+
+	if (icl->set_bus_param) {
+		ret = icl->set_bus_param(icl, 1 << (bps - 1));
+		if (ret)
+			return ret;
+	} else if (bps != 10) {
+		/*
+		 * Without board specific bus width settings we only support the
+		 * sensors native bus width
+		 */
+		return -EINVAL;
+	}
+
+	if (flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
+		pixclk |= 0x10;
+
+	if (!(flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH))
+		pixclk |= 0x1;
+
+	if (!(flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH))
+		pixclk |= 0x2;
+
+	ret = reg_write(client, mt9v022->reg->pixclk_fv_lv, pixclk);
+	if (ret < 0)
+		return ret;
+
+	if (!(flags & V4L2_MBUS_MASTER))
+		mt9v022->chip_control &= ~0x8;
+
+	ret = reg_write(client, MT9V022_CHIP_CONTROL, mt9v022->chip_control);
+	if (ret < 0)
+		return ret;
+
+	dev_dbg(&client->dev, "Calculated pixclk 0x%x, chip control 0x%x\n",
+		pixclk, mt9v022->chip_control);
+
+	return 0;
+}
+
+static struct v4l2_subdev_video_ops mt9v022_subdev_video_ops = {
+	.s_stream	= mt9v022_s_stream,
+	.s_mbus_fmt	= mt9v022_s_fmt,
+	.g_mbus_fmt	= mt9v022_g_fmt,
+	.try_mbus_fmt	= mt9v022_try_fmt,
+	.s_crop		= mt9v022_s_crop,
+	.g_crop		= mt9v022_g_crop,
+	.cropcap	= mt9v022_cropcap,
+	.enum_mbus_fmt	= mt9v022_enum_fmt,
+	.g_mbus_config	= mt9v022_g_mbus_config,
+	.s_mbus_config	= mt9v022_s_mbus_config,
+};
+
+static struct v4l2_subdev_sensor_ops mt9v022_subdev_sensor_ops = {
+	.g_skip_top_lines	= mt9v022_g_skip_top_lines,
+};
+
+static struct v4l2_subdev_ops mt9v022_subdev_ops = {
+	.core	= &mt9v022_subdev_core_ops,
+	.video	= &mt9v022_subdev_video_ops,
+	.sensor	= &mt9v022_subdev_sensor_ops,
+};
+
+static int mt9v022_probe(struct i2c_client *client,
+			 const struct i2c_device_id *did)
+{
+	struct mt9v022 *mt9v022;
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
+	int ret;
+
+	if (!icl) {
+		dev_err(&client->dev, "MT9V022 driver needs platform data\n");
+		return -EINVAL;
+	}
+
+	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;
+	}
+
+	mt9v022 = kzalloc(sizeof(struct mt9v022), GFP_KERNEL);
+	if (!mt9v022)
+		return -ENOMEM;
+
+	v4l2_i2c_subdev_init(&mt9v022->subdev, client, &mt9v022_subdev_ops);
+	v4l2_ctrl_handler_init(&mt9v022->hdl, 6);
+	v4l2_ctrl_new_std(&mt9v022->hdl, &mt9v022_ctrl_ops,
+			V4L2_CID_VFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&mt9v022->hdl, &mt9v022_ctrl_ops,
+			V4L2_CID_HFLIP, 0, 1, 1, 0);
+	mt9v022->autogain = v4l2_ctrl_new_std(&mt9v022->hdl, &mt9v022_ctrl_ops,
+			V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+	mt9v022->gain = v4l2_ctrl_new_std(&mt9v022->hdl, &mt9v022_ctrl_ops,
+			V4L2_CID_GAIN, 0, 127, 1, 64);
+
+	/*
+	 * Simulated autoexposure. If enabled, we calculate shutter width
+	 * ourselves in the driver based on vertical blanking and frame width
+	 */
+	mt9v022->autoexposure = v4l2_ctrl_new_std_menu(&mt9v022->hdl,
+			&mt9v022_ctrl_ops, V4L2_CID_EXPOSURE_AUTO, 1, 0,
+			V4L2_EXPOSURE_AUTO);
+	mt9v022->exposure = v4l2_ctrl_new_std(&mt9v022->hdl, &mt9v022_ctrl_ops,
+			V4L2_CID_EXPOSURE, 1, 255, 1, 255);
+
+	mt9v022->subdev.ctrl_handler = &mt9v022->hdl;
+	if (mt9v022->hdl.error) {
+		int err = mt9v022->hdl.error;
+
+		kfree(mt9v022);
+		return err;
+	}
+	v4l2_ctrl_auto_cluster(2, &mt9v022->autoexposure,
+				V4L2_EXPOSURE_MANUAL, true);
+	v4l2_ctrl_auto_cluster(2, &mt9v022->autogain, 0, true);
+
+	mt9v022->chip_control = MT9V022_CHIP_CONTROL_DEFAULT;
+
+	/*
+	 * MT9V022 _really_ corrupts the first read out line.
+	 * TODO: verify on i.MX31
+	 */
+	mt9v022->y_skip_top	= 1;
+	mt9v022->rect.left	= MT9V022_COLUMN_SKIP;
+	mt9v022->rect.top	= MT9V022_ROW_SKIP;
+	mt9v022->rect.width	= MT9V022_MAX_WIDTH;
+	mt9v022->rect.height	= MT9V022_MAX_HEIGHT;
+
+	ret = mt9v022_video_probe(client);
+	if (ret) {
+		v4l2_ctrl_handler_free(&mt9v022->hdl);
+		kfree(mt9v022);
+	}
+
+	return ret;
+}
+
+static int mt9v022_remove(struct i2c_client *client)
+{
+	struct mt9v022 *mt9v022 = to_mt9v022(client);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+	v4l2_device_unregister_subdev(&mt9v022->subdev);
+	if (icl->free_bus)
+		icl->free_bus(icl);
+	v4l2_ctrl_handler_free(&mt9v022->hdl);
+	kfree(mt9v022);
+
+	return 0;
+}
+static const struct i2c_device_id mt9v022_id[] = {
+	{ "mt9v022", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, mt9v022_id);
+
+static struct i2c_driver mt9v022_i2c_driver = {
+	.driver = {
+		.name = "mt9v022",
+	},
+	.probe		= mt9v022_probe,
+	.remove		= mt9v022_remove,
+	.id_table	= mt9v022_id,
+};
+
+module_i2c_driver(mt9v022_i2c_driver);
+
+MODULE_DESCRIPTION("Micron MT9V022 Camera driver");
+MODULE_AUTHOR("Guennadi Liakhovetski <kernel@pengutronix.de>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/soc_camera/ov2640.c b/drivers/media/i2c/soc_camera/ov2640.c
new file mode 100644
index 000000000000..78ac5744cb5d
--- /dev/null
+++ b/drivers/media/i2c/soc_camera/ov2640.c
@@ -0,0 +1,1122 @@
+/*
+ * 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
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#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/soc_camera.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-ctrls.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
+#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 /* Banding filter ON/OFF */
+#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 BD50        0x4F /* 50Hz Banding AEC 8 LSBs */
+#define BD60        0x50 /* 60Hz Banding AEC 8 LSBs */
+#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;
+};
+
+/* Supported resolutions */
+enum ov2640_width {
+	W_QCIF	= 176,
+	W_QVGA	= 320,
+	W_CIF	= 352,
+	W_VGA	= 640,
+	W_SVGA	= 800,
+	W_XGA	= 1024,
+	W_SXGA	= 1280,
+	W_UXGA	= 1600,
+};
+
+enum ov2640_height {
+	H_QCIF	= 144,
+	H_QVGA	= 240,
+	H_CIF	= 288,
+	H_VGA	= 480,
+	H_SVGA	= 600,
+	H_XGA	= 768,
+	H_SXGA	= 1024,
+	H_UXGA	= 1200,
+};
+
+struct ov2640_win_size {
+	char				*name;
+	enum ov2640_width		width;
+	enum ov2640_height		height;
+	const struct regval_list	*regs;
+};
+
+
+struct ov2640_priv {
+	struct v4l2_subdev		subdev;
+	struct v4l2_ctrl_handler	hdl;
+	enum v4l2_mbus_pixelcode	cfmt_code;
+	const struct ov2640_win_size	*win;
+	int				model;
+};
+
+/*
+ * 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 },
+	{ 0x5a,   0x57 },
+	{ BD50,   0xbb },
+	{ BD60,   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 },
+	{ 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 },
+	{ HSIZE8, HSIZE8_SET(W_UXGA) },
+	{ VSIZE8, VSIZE8_SET(H_UXGA) },
+	{ CTRL2, CTRL2_DCW_EN | CTRL2_SDE_EN |
+		 CTRL2_UV_AVG_EN | CTRL2_CMX_EN | CTRL2_UV_ADJ_EN },
+	{ HSIZE, HSIZE_SET(W_UXGA) },
+	{ VSIZE, VSIZE_SET(H_UXGA) },
+	{ XOFFL, XOFFL_SET(0) },
+	{ YOFFL, YOFFL_SET(0) },
+	{ VHYX, VHYX_HSIZE_SET(W_UXGA) | VHYX_VSIZE_SET(H_UXGA) |
+		VHYX_XOFF_SET(0) | VHYX_YOFF_SET(0)},
+	{ TEST, TEST_HSIZE_SET(W_UXGA) },
+	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(W_QCIF, H_QCIF, 3, 3, 4),
+	ENDMARKER,
+};
+
+static const struct regval_list ov2640_qvga_regs[] = {
+	PER_SIZE_REG_SEQ(W_QVGA, H_QVGA, 2, 2, 4),
+	ENDMARKER,
+};
+
+static const struct regval_list ov2640_cif_regs[] = {
+	PER_SIZE_REG_SEQ(W_CIF, H_CIF, 2, 2, 8),
+	ENDMARKER,
+};
+
+static const struct regval_list ov2640_vga_regs[] = {
+	PER_SIZE_REG_SEQ(W_VGA, H_VGA, 0, 0, 2),
+	ENDMARKER,
+};
+
+static const struct regval_list ov2640_svga_regs[] = {
+	PER_SIZE_REG_SEQ(W_SVGA, H_SVGA, 1, 1, 2),
+	ENDMARKER,
+};
+
+static const struct regval_list ov2640_xga_regs[] = {
+	PER_SIZE_REG_SEQ(W_XGA, H_XGA, 0, 0, 2),
+	{ CTRLI,    0x00},
+	ENDMARKER,
+};
+
+static const struct regval_list ov2640_sxga_regs[] = {
+	PER_SIZE_REG_SEQ(W_SXGA, H_SXGA, 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(W_UXGA, H_UXGA, 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", W_QCIF, H_QCIF, ov2640_qcif_regs),
+	OV2640_SIZE("QVGA", W_QVGA, H_QVGA, ov2640_qvga_regs),
+	OV2640_SIZE("CIF", W_CIF, H_CIF, ov2640_cif_regs),
+	OV2640_SIZE("VGA", W_VGA, H_VGA, ov2640_vga_regs),
+	OV2640_SIZE("SVGA", W_SVGA, H_SVGA, ov2640_svga_regs),
+	OV2640_SIZE("XGA", W_XGA, H_XGA, ov2640_xga_regs),
+	OV2640_SIZE("SXGA", W_SXGA, H_SXGA, ov2640_sxga_regs),
+	OV2640_SIZE("UXGA", W_UXGA, H_UXGA, 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_yuv422_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_regs[] = {
+	{ IMAGE_MODE, IMAGE_MODE_LBYTE_FIRST | IMAGE_MODE_RGB565 },
+	{ 0xd7, 0x03 },
+	{ RESET,  0x00 },
+	{ R_BYPASS, R_BYPASS_USE_DSP },
+	ENDMARKER,
+};
+
+static enum v4l2_mbus_pixelcode ov2640_codes[] = {
+	V4L2_MBUS_FMT_UYVY8_2X8,
+	V4L2_MBUS_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;
+	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;
+}
+
+/*
+ * soc_camera_ops functions
+ */
+static int ov2640_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	return 0;
+}
+
+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);
+	u8 val;
+
+	switch (ctrl->id) {
+	case V4L2_CID_VFLIP:
+		val = ctrl->val ? REG04_VFLIP_IMG : 0x00;
+		return ov2640_mask_set(client, REG04, REG04_VFLIP_IMG, val);
+	case V4L2_CID_HFLIP:
+		val = ctrl->val ? REG04_HFLIP_IMG : 0x00;
+		return ov2640_mask_set(client, REG04, REG04_HFLIP_IMG, val);
+	}
+
+	return -EINVAL;
+}
+
+static int ov2640_g_chip_ident(struct v4l2_subdev *sd,
+			       struct v4l2_dbg_chip_ident *id)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov2640_priv *priv = to_ov2640(client);
+
+	id->ident    = priv->model;
+	id->revision = 0;
+
+	return 0;
+}
+
+#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,
+			     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 int ov2640_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+	return soc_camera_set_power(&client->dev, icl, on);
+}
+
+/* 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) {
+			*width = ov2640_supported_win_sizes[i].width;
+			*height = ov2640_supported_win_sizes[i].height;
+			return &ov2640_supported_win_sizes[i];
+		}
+	}
+
+	*width = ov2640_supported_win_sizes[default_size].width;
+	*height = ov2640_supported_win_sizes[default_size].height;
+	return &ov2640_supported_win_sizes[default_size];
+}
+
+static int ov2640_set_params(struct i2c_client *client, u32 *width, u32 *height,
+			     enum v4l2_mbus_pixelcode code)
+{
+	struct ov2640_priv       *priv = to_ov2640(client);
+	const struct regval_list *selected_cfmt_regs;
+	int ret;
+
+	/* select win */
+	priv->win = ov2640_select_win(width, height);
+
+	/* select format */
+	priv->cfmt_code = 0;
+	switch (code) {
+	case V4L2_MBUS_FMT_RGB565_2X8_LE:
+		dev_dbg(&client->dev, "%s: Selected cfmt RGB565", __func__);
+		selected_cfmt_regs = ov2640_rgb565_regs;
+		break;
+	default:
+	case V4L2_MBUS_FMT_UYVY8_2X8:
+		dev_dbg(&client->dev, "%s: Selected cfmt YUV422", __func__);
+		selected_cfmt_regs = ov2640_yuv422_regs;
+	}
+
+	/* 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__, priv->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, priv->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;
+
+	priv->cfmt_code = code;
+	*width = priv->win->width;
+	*height = priv->win->height;
+
+	return 0;
+
+err:
+	dev_err(&client->dev, "%s: Error %d", __func__, ret);
+	ov2640_reset(client);
+	priv->win = NULL;
+
+	return ret;
+}
+
+static int ov2640_g_fmt(struct v4l2_subdev *sd,
+			struct v4l2_mbus_framefmt *mf)
+{
+	struct i2c_client  *client = v4l2_get_subdevdata(sd);
+	struct ov2640_priv *priv = to_ov2640(client);
+
+	if (!priv->win) {
+		u32 width = W_SVGA, height = H_SVGA;
+		priv->win = ov2640_select_win(&width, &height);
+		priv->cfmt_code = V4L2_MBUS_FMT_UYVY8_2X8;
+	}
+
+	mf->width	= priv->win->width;
+	mf->height	= priv->win->height;
+	mf->code	= priv->cfmt_code;
+
+	switch (mf->code) {
+	case V4L2_MBUS_FMT_RGB565_2X8_LE:
+		mf->colorspace = V4L2_COLORSPACE_SRGB;
+		break;
+	default:
+	case V4L2_MBUS_FMT_UYVY8_2X8:
+		mf->colorspace = V4L2_COLORSPACE_JPEG;
+	}
+	mf->field	= V4L2_FIELD_NONE;
+
+	return 0;
+}
+
+static int ov2640_s_fmt(struct v4l2_subdev *sd,
+			struct v4l2_mbus_framefmt *mf)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+
+	switch (mf->code) {
+	case V4L2_MBUS_FMT_RGB565_2X8_LE:
+		mf->colorspace = V4L2_COLORSPACE_SRGB;
+		break;
+	default:
+		mf->code = V4L2_MBUS_FMT_UYVY8_2X8;
+	case V4L2_MBUS_FMT_UYVY8_2X8:
+		mf->colorspace = V4L2_COLORSPACE_JPEG;
+	}
+
+	ret = ov2640_set_params(client, &mf->width, &mf->height, mf->code);
+
+	return ret;
+}
+
+static int ov2640_try_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_mbus_framefmt *mf)
+{
+	const struct ov2640_win_size *win;
+
+	/*
+	 * select suitable win
+	 */
+	win = ov2640_select_win(&mf->width, &mf->height);
+
+	mf->field	= V4L2_FIELD_NONE;
+
+	switch (mf->code) {
+	case V4L2_MBUS_FMT_RGB565_2X8_LE:
+		mf->colorspace = V4L2_COLORSPACE_SRGB;
+		break;
+	default:
+		mf->code = V4L2_MBUS_FMT_UYVY8_2X8;
+	case V4L2_MBUS_FMT_UYVY8_2X8:
+		mf->colorspace = V4L2_COLORSPACE_JPEG;
+	}
+
+	return 0;
+}
+
+static int ov2640_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
+			   enum v4l2_mbus_pixelcode *code)
+{
+	if (index >= ARRAY_SIZE(ov2640_codes))
+		return -EINVAL;
+
+	*code = ov2640_codes[index];
+	return 0;
+}
+
+static int ov2640_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+{
+	a->c.left	= 0;
+	a->c.top	= 0;
+	a->c.width	= W_UXGA;
+	a->c.height	= H_UXGA;
+	a->type		= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	return 0;
+}
+
+static int ov2640_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
+{
+	a->bounds.left			= 0;
+	a->bounds.top			= 0;
+	a->bounds.width			= W_UXGA;
+	a->bounds.height		= H_UXGA;
+	a->defrect			= a->bounds;
+	a->type				= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	a->pixelaspect.numerator	= 1;
+	a->pixelaspect.denominator	= 1;
+
+	return 0;
+}
+
+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";
+		priv->model = V4L2_IDENT_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);
+
+	ret = v4l2_ctrl_handler_setup(&priv->hdl);
+
+done:
+	ov2640_s_power(&priv->subdev, 0);
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ov2640_ctrl_ops = {
+	.s_ctrl = ov2640_s_ctrl,
+};
+
+static struct v4l2_subdev_core_ops ov2640_subdev_core_ops = {
+	.g_chip_ident	= ov2640_g_chip_ident,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register	= ov2640_g_register,
+	.s_register	= ov2640_s_register,
+#endif
+	.s_power	= ov2640_s_power,
+};
+
+static int ov2640_g_mbus_config(struct v4l2_subdev *sd,
+				struct v4l2_mbus_config *cfg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+	cfg->flags = V4L2_MBUS_PCLK_SAMPLE_RISING | V4L2_MBUS_MASTER |
+		V4L2_MBUS_VSYNC_ACTIVE_HIGH | V4L2_MBUS_HSYNC_ACTIVE_HIGH |
+		V4L2_MBUS_DATA_ACTIVE_HIGH;
+	cfg->type = V4L2_MBUS_PARALLEL;
+	cfg->flags = soc_camera_apply_board_flags(icl, cfg);
+
+	return 0;
+}
+
+static struct v4l2_subdev_video_ops ov2640_subdev_video_ops = {
+	.s_stream	= ov2640_s_stream,
+	.g_mbus_fmt	= ov2640_g_fmt,
+	.s_mbus_fmt	= ov2640_s_fmt,
+	.try_mbus_fmt	= ov2640_try_fmt,
+	.cropcap	= ov2640_cropcap,
+	.g_crop		= ov2640_g_crop,
+	.enum_mbus_fmt	= ov2640_enum_fmt,
+	.g_mbus_config	= ov2640_g_mbus_config,
+};
+
+static struct v4l2_subdev_ops ov2640_subdev_ops = {
+	.core	= &ov2640_subdev_core_ops,
+	.video	= &ov2640_subdev_video_ops,
+};
+
+/*
+ * i2c_driver functions
+ */
+static int ov2640_probe(struct i2c_client *client,
+			const struct i2c_device_id *did)
+{
+	struct ov2640_priv	*priv;
+	struct soc_camera_link	*icl = soc_camera_i2c_to_link(client);
+	struct i2c_adapter	*adapter = to_i2c_adapter(client->dev.parent);
+	int			ret;
+
+	if (!icl) {
+		dev_err(&adapter->dev,
+			"OV2640: Missing platform_data for driver\n");
+		return -EINVAL;
+	}
+
+	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 = kzalloc(sizeof(struct ov2640_priv), GFP_KERNEL);
+	if (!priv) {
+		dev_err(&adapter->dev,
+			"Failed to allocate memory for private data!\n");
+		return -ENOMEM;
+	}
+
+	v4l2_i2c_subdev_init(&priv->subdev, client, &ov2640_subdev_ops);
+	v4l2_ctrl_handler_init(&priv->hdl, 2);
+	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);
+	priv->subdev.ctrl_handler = &priv->hdl;
+	if (priv->hdl.error) {
+		int err = priv->hdl.error;
+
+		kfree(priv);
+		return err;
+	}
+
+	ret = ov2640_video_probe(client);
+	if (ret) {
+		v4l2_ctrl_handler_free(&priv->hdl);
+		kfree(priv);
+	} else {
+		dev_info(&adapter->dev, "OV2640 Probed\n");
+	}
+
+	return ret;
+}
+
+static int ov2640_remove(struct i2c_client *client)
+{
+	struct ov2640_priv       *priv = to_ov2640(client);
+
+	v4l2_device_unregister_subdev(&priv->subdev);
+	v4l2_ctrl_handler_free(&priv->hdl);
+	kfree(priv);
+	return 0;
+}
+
+static const struct i2c_device_id ov2640_id[] = {
+	{ "ov2640", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ov2640_id);
+
+static struct i2c_driver ov2640_i2c_driver = {
+	.driver = {
+		.name = "ov2640",
+	},
+	.probe    = ov2640_probe,
+	.remove   = ov2640_remove,
+	.id_table = ov2640_id,
+};
+
+module_i2c_driver(ov2640_i2c_driver);
+
+MODULE_DESCRIPTION("SoC Camera driver for Omni Vision 2640 sensor");
+MODULE_AUTHOR("Alberto Panizzo");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/soc_camera/ov5642.c b/drivers/media/i2c/soc_camera/ov5642.c
new file mode 100644
index 000000000000..8577e0cfb7fe
--- /dev/null
+++ b/drivers/media/i2c/soc_camera/ov5642.c
@@ -0,0 +1,1088 @@
+/*
+ * Driver for OV5642 CMOS Image Sensor from Omnivision
+ *
+ * Copyright (C) 2011, Bastian Hecht <hechtb@gmail.com>
+ *
+ * Based on Sony IMX074 Camera Driver
+ * Copyright (C) 2010, Guennadi Liakhovetski <g.liakhovetski@gmx.de>
+ *
+ * Based on Omnivision OV7670 Camera Driver
+ * Copyright (C) 2006-7 Jonathan Corbet <corbet@lwn.net>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/bitops.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <linux/module.h>
+#include <linux/v4l2-mediabus.h>
+
+#include <media/soc_camera.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-subdev.h>
+
+/* OV5642 registers */
+#define REG_CHIP_ID_HIGH		0x300a
+#define REG_CHIP_ID_LOW			0x300b
+
+#define REG_WINDOW_START_X_HIGH		0x3800
+#define REG_WINDOW_START_X_LOW		0x3801
+#define REG_WINDOW_START_Y_HIGH		0x3802
+#define REG_WINDOW_START_Y_LOW		0x3803
+#define REG_WINDOW_WIDTH_HIGH		0x3804
+#define REG_WINDOW_WIDTH_LOW		0x3805
+#define REG_WINDOW_HEIGHT_HIGH		0x3806
+#define REG_WINDOW_HEIGHT_LOW		0x3807
+#define REG_OUT_WIDTH_HIGH		0x3808
+#define REG_OUT_WIDTH_LOW		0x3809
+#define REG_OUT_HEIGHT_HIGH		0x380a
+#define REG_OUT_HEIGHT_LOW		0x380b
+#define REG_OUT_TOTAL_WIDTH_HIGH	0x380c
+#define REG_OUT_TOTAL_WIDTH_LOW		0x380d
+#define REG_OUT_TOTAL_HEIGHT_HIGH	0x380e
+#define REG_OUT_TOTAL_HEIGHT_LOW	0x380f
+#define REG_OUTPUT_FORMAT		0x4300
+#define REG_ISP_CTRL_01			0x5001
+#define REG_AVG_WINDOW_END_X_HIGH	0x5682
+#define REG_AVG_WINDOW_END_X_LOW	0x5683
+#define REG_AVG_WINDOW_END_Y_HIGH	0x5686
+#define REG_AVG_WINDOW_END_Y_LOW	0x5687
+
+/* active pixel array size */
+#define OV5642_SENSOR_SIZE_X	2592
+#define OV5642_SENSOR_SIZE_Y	1944
+
+/*
+ * About OV5642 resolution, cropping and binning:
+ * This sensor supports it all, at least in the feature description.
+ * Unfortunately, no combination of appropriate registers settings could make
+ * the chip work the intended way. As it works with predefined register lists,
+ * some undocumented registers are presumably changed there to achieve their
+ * goals.
+ * This driver currently only works for resolutions up to 720 lines with a
+ * 1:1 scale. Hopefully these restrictions will be removed in the future.
+ */
+#define OV5642_MAX_WIDTH	OV5642_SENSOR_SIZE_X
+#define OV5642_MAX_HEIGHT	720
+
+/* default sizes */
+#define OV5642_DEFAULT_WIDTH	1280
+#define OV5642_DEFAULT_HEIGHT	OV5642_MAX_HEIGHT
+
+/* minimum extra blanking */
+#define BLANKING_EXTRA_WIDTH		500
+#define BLANKING_EXTRA_HEIGHT		20
+
+/*
+ * the sensor's autoexposure is buggy when setting total_height low.
+ * It tries to expose longer than 1 frame period without taking care of it
+ * and this leads to weird output. So we set 1000 lines as minimum.
+ */
+#define BLANKING_MIN_HEIGHT		1000
+
+struct regval_list {
+	u16 reg_num;
+	u8 value;
+};
+
+static struct regval_list ov5642_default_regs_init[] = {
+	{ 0x3103, 0x93 },
+	{ 0x3008, 0x82 },
+	{ 0x3017, 0x7f },
+	{ 0x3018, 0xfc },
+	{ 0x3810, 0xc2 },
+	{ 0x3615, 0xf0 },
+	{ 0x3000, 0x0  },
+	{ 0x3001, 0x0  },
+	{ 0x3002, 0x0  },
+	{ 0x3003, 0x0  },
+	{ 0x3004, 0xff },
+	{ 0x3030, 0x2b },
+	{ 0x3011, 0x8  },
+	{ 0x3010, 0x10 },
+	{ 0x3604, 0x60 },
+	{ 0x3622, 0x60 },
+	{ 0x3621, 0x9  },
+	{ 0x3709, 0x0  },
+	{ 0x4000, 0x21 },
+	{ 0x401d, 0x22 },
+	{ 0x3600, 0x54 },
+	{ 0x3605, 0x4  },
+	{ 0x3606, 0x3f },
+	{ 0x3c01, 0x80 },
+	{ 0x300d, 0x22 },
+	{ 0x3623, 0x22 },
+	{ 0x5000, 0x4f },
+	{ 0x5020, 0x4  },
+	{ 0x5181, 0x79 },
+	{ 0x5182, 0x0  },
+	{ 0x5185, 0x22 },
+	{ 0x5197, 0x1  },
+	{ 0x5500, 0xa  },
+	{ 0x5504, 0x0  },
+	{ 0x5505, 0x7f },
+	{ 0x5080, 0x8  },
+	{ 0x300e, 0x18 },
+	{ 0x4610, 0x0  },
+	{ 0x471d, 0x5  },
+	{ 0x4708, 0x6  },
+	{ 0x370c, 0xa0 },
+	{ 0x5687, 0x94 },
+	{ 0x501f, 0x0  },
+	{ 0x5000, 0x4f },
+	{ 0x5001, 0xcf },
+	{ 0x4300, 0x30 },
+	{ 0x4300, 0x30 },
+	{ 0x460b, 0x35 },
+	{ 0x471d, 0x0  },
+	{ 0x3002, 0xc  },
+	{ 0x3002, 0x0  },
+	{ 0x4713, 0x3  },
+	{ 0x471c, 0x50 },
+	{ 0x4721, 0x2  },
+	{ 0x4402, 0x90 },
+	{ 0x460c, 0x22 },
+	{ 0x3815, 0x44 },
+	{ 0x3503, 0x7  },
+	{ 0x3501, 0x73 },
+	{ 0x3502, 0x80 },
+	{ 0x350b, 0x0  },
+	{ 0x3818, 0xc8 },
+	{ 0x3824, 0x11 },
+	{ 0x3a00, 0x78 },
+	{ 0x3a1a, 0x4  },
+	{ 0x3a13, 0x30 },
+	{ 0x3a18, 0x0  },
+	{ 0x3a19, 0x7c },
+	{ 0x3a08, 0x12 },
+	{ 0x3a09, 0xc0 },
+	{ 0x3a0a, 0xf  },
+	{ 0x3a0b, 0xa0 },
+	{ 0x350c, 0x7  },
+	{ 0x350d, 0xd0 },
+	{ 0x3a0d, 0x8  },
+	{ 0x3a0e, 0x6  },
+	{ 0x3500, 0x0  },
+	{ 0x3501, 0x0  },
+	{ 0x3502, 0x0  },
+	{ 0x350a, 0x0  },
+	{ 0x350b, 0x0  },
+	{ 0x3503, 0x0  },
+	{ 0x3a0f, 0x3c },
+	{ 0x3a10, 0x32 },
+	{ 0x3a1b, 0x3c },
+	{ 0x3a1e, 0x32 },
+	{ 0x3a11, 0x80 },
+	{ 0x3a1f, 0x20 },
+	{ 0x3030, 0x2b },
+	{ 0x3a02, 0x0  },
+	{ 0x3a03, 0x7d },
+	{ 0x3a04, 0x0  },
+	{ 0x3a14, 0x0  },
+	{ 0x3a15, 0x7d },
+	{ 0x3a16, 0x0  },
+	{ 0x3a00, 0x78 },
+	{ 0x3a08, 0x9  },
+	{ 0x3a09, 0x60 },
+	{ 0x3a0a, 0x7  },
+	{ 0x3a0b, 0xd0 },
+	{ 0x3a0d, 0x10 },
+	{ 0x3a0e, 0xd  },
+	{ 0x4407, 0x4  },
+	{ 0x5193, 0x70 },
+	{ 0x589b, 0x0  },
+	{ 0x589a, 0xc0 },
+	{ 0x401e, 0x20 },
+	{ 0x4001, 0x42 },
+	{ 0x401c, 0x6  },
+	{ 0x3825, 0xac },
+	{ 0x3827, 0xc  },
+	{ 0x528a, 0x1  },
+	{ 0x528b, 0x4  },
+	{ 0x528c, 0x8  },
+	{ 0x528d, 0x10 },
+	{ 0x528e, 0x20 },
+	{ 0x528f, 0x28 },
+	{ 0x5290, 0x30 },
+	{ 0x5292, 0x0  },
+	{ 0x5293, 0x1  },
+	{ 0x5294, 0x0  },
+	{ 0x5295, 0x4  },
+	{ 0x5296, 0x0  },
+	{ 0x5297, 0x8  },
+	{ 0x5298, 0x0  },
+	{ 0x5299, 0x10 },
+	{ 0x529a, 0x0  },
+	{ 0x529b, 0x20 },
+	{ 0x529c, 0x0  },
+	{ 0x529d, 0x28 },
+	{ 0x529e, 0x0  },
+	{ 0x529f, 0x30 },
+	{ 0x5282, 0x0  },
+	{ 0x5300, 0x0  },
+	{ 0x5301, 0x20 },
+	{ 0x5302, 0x0  },
+	{ 0x5303, 0x7c },
+	{ 0x530c, 0x0  },
+	{ 0x530d, 0xc  },
+	{ 0x530e, 0x20 },
+	{ 0x530f, 0x80 },
+	{ 0x5310, 0x20 },
+	{ 0x5311, 0x80 },
+	{ 0x5308, 0x20 },
+	{ 0x5309, 0x40 },
+	{ 0x5304, 0x0  },
+	{ 0x5305, 0x30 },
+	{ 0x5306, 0x0  },
+	{ 0x5307, 0x80 },
+	{ 0x5314, 0x8  },
+	{ 0x5315, 0x20 },
+	{ 0x5319, 0x30 },
+	{ 0x5316, 0x10 },
+	{ 0x5317, 0x0  },
+	{ 0x5318, 0x2  },
+	{ 0x5380, 0x1  },
+	{ 0x5381, 0x0  },
+	{ 0x5382, 0x0  },
+	{ 0x5383, 0x4e },
+	{ 0x5384, 0x0  },
+	{ 0x5385, 0xf  },
+	{ 0x5386, 0x0  },
+	{ 0x5387, 0x0  },
+	{ 0x5388, 0x1  },
+	{ 0x5389, 0x15 },
+	{ 0x538a, 0x0  },
+	{ 0x538b, 0x31 },
+	{ 0x538c, 0x0  },
+	{ 0x538d, 0x0  },
+	{ 0x538e, 0x0  },
+	{ 0x538f, 0xf  },
+	{ 0x5390, 0x0  },
+	{ 0x5391, 0xab },
+	{ 0x5392, 0x0  },
+	{ 0x5393, 0xa2 },
+	{ 0x5394, 0x8  },
+	{ 0x5480, 0x14 },
+	{ 0x5481, 0x21 },
+	{ 0x5482, 0x36 },
+	{ 0x5483, 0x57 },
+	{ 0x5484, 0x65 },
+	{ 0x5485, 0x71 },
+	{ 0x5486, 0x7d },
+	{ 0x5487, 0x87 },
+	{ 0x5488, 0x91 },
+	{ 0x5489, 0x9a },
+	{ 0x548a, 0xaa },
+	{ 0x548b, 0xb8 },
+	{ 0x548c, 0xcd },
+	{ 0x548d, 0xdd },
+	{ 0x548e, 0xea },
+	{ 0x548f, 0x1d },
+	{ 0x5490, 0x5  },
+	{ 0x5491, 0x0  },
+	{ 0x5492, 0x4  },
+	{ 0x5493, 0x20 },
+	{ 0x5494, 0x3  },
+	{ 0x5495, 0x60 },
+	{ 0x5496, 0x2  },
+	{ 0x5497, 0xb8 },
+	{ 0x5498, 0x2  },
+	{ 0x5499, 0x86 },
+	{ 0x549a, 0x2  },
+	{ 0x549b, 0x5b },
+	{ 0x549c, 0x2  },
+	{ 0x549d, 0x3b },
+	{ 0x549e, 0x2  },
+	{ 0x549f, 0x1c },
+	{ 0x54a0, 0x2  },
+	{ 0x54a1, 0x4  },
+	{ 0x54a2, 0x1  },
+	{ 0x54a3, 0xed },
+	{ 0x54a4, 0x1  },
+	{ 0x54a5, 0xc5 },
+	{ 0x54a6, 0x1  },
+	{ 0x54a7, 0xa5 },
+	{ 0x54a8, 0x1  },
+	{ 0x54a9, 0x6c },
+	{ 0x54aa, 0x1  },
+	{ 0x54ab, 0x41 },
+	{ 0x54ac, 0x1  },
+	{ 0x54ad, 0x20 },
+	{ 0x54ae, 0x0  },
+	{ 0x54af, 0x16 },
+	{ 0x54b0, 0x1  },
+	{ 0x54b1, 0x20 },
+	{ 0x54b2, 0x0  },
+	{ 0x54b3, 0x10 },
+	{ 0x54b4, 0x0  },
+	{ 0x54b5, 0xf0 },
+	{ 0x54b6, 0x0  },
+	{ 0x54b7, 0xdf },
+	{ 0x5402, 0x3f },
+	{ 0x5403, 0x0  },
+	{ 0x3406, 0x0  },
+	{ 0x5180, 0xff },
+	{ 0x5181, 0x52 },
+	{ 0x5182, 0x11 },
+	{ 0x5183, 0x14 },
+	{ 0x5184, 0x25 },
+	{ 0x5185, 0x24 },
+	{ 0x5186, 0x6  },
+	{ 0x5187, 0x8  },
+	{ 0x5188, 0x8  },
+	{ 0x5189, 0x7c },
+	{ 0x518a, 0x60 },
+	{ 0x518b, 0xb2 },
+	{ 0x518c, 0xb2 },
+	{ 0x518d, 0x44 },
+	{ 0x518e, 0x3d },
+	{ 0x518f, 0x58 },
+	{ 0x5190, 0x46 },
+	{ 0x5191, 0xf8 },
+	{ 0x5192, 0x4  },
+	{ 0x5193, 0x70 },
+	{ 0x5194, 0xf0 },
+	{ 0x5195, 0xf0 },
+	{ 0x5196, 0x3  },
+	{ 0x5197, 0x1  },
+	{ 0x5198, 0x4  },
+	{ 0x5199, 0x12 },
+	{ 0x519a, 0x4  },
+	{ 0x519b, 0x0  },
+	{ 0x519c, 0x6  },
+	{ 0x519d, 0x82 },
+	{ 0x519e, 0x0  },
+	{ 0x5025, 0x80 },
+	{ 0x3a0f, 0x38 },
+	{ 0x3a10, 0x30 },
+	{ 0x3a1b, 0x3a },
+	{ 0x3a1e, 0x2e },
+	{ 0x3a11, 0x60 },
+	{ 0x3a1f, 0x10 },
+	{ 0x5688, 0xa6 },
+	{ 0x5689, 0x6a },
+	{ 0x568a, 0xea },
+	{ 0x568b, 0xae },
+	{ 0x568c, 0xa6 },
+	{ 0x568d, 0x6a },
+	{ 0x568e, 0x62 },
+	{ 0x568f, 0x26 },
+	{ 0x5583, 0x40 },
+	{ 0x5584, 0x40 },
+	{ 0x5580, 0x2  },
+	{ 0x5000, 0xcf },
+	{ 0x5800, 0x27 },
+	{ 0x5801, 0x19 },
+	{ 0x5802, 0x12 },
+	{ 0x5803, 0xf  },
+	{ 0x5804, 0x10 },
+	{ 0x5805, 0x15 },
+	{ 0x5806, 0x1e },
+	{ 0x5807, 0x2f },
+	{ 0x5808, 0x15 },
+	{ 0x5809, 0xd  },
+	{ 0x580a, 0xa  },
+	{ 0x580b, 0x9  },
+	{ 0x580c, 0xa  },
+	{ 0x580d, 0xc  },
+	{ 0x580e, 0x12 },
+	{ 0x580f, 0x19 },
+	{ 0x5810, 0xb  },
+	{ 0x5811, 0x7  },
+	{ 0x5812, 0x4  },
+	{ 0x5813, 0x3  },
+	{ 0x5814, 0x3  },
+	{ 0x5815, 0x6  },
+	{ 0x5816, 0xa  },
+	{ 0x5817, 0xf  },
+	{ 0x5818, 0xa  },
+	{ 0x5819, 0x5  },
+	{ 0x581a, 0x1  },
+	{ 0x581b, 0x0  },
+	{ 0x581c, 0x0  },
+	{ 0x581d, 0x3  },
+	{ 0x581e, 0x8  },
+	{ 0x581f, 0xc  },
+	{ 0x5820, 0xa  },
+	{ 0x5821, 0x5  },
+	{ 0x5822, 0x1  },
+	{ 0x5823, 0x0  },
+	{ 0x5824, 0x0  },
+	{ 0x5825, 0x3  },
+	{ 0x5826, 0x8  },
+	{ 0x5827, 0xc  },
+	{ 0x5828, 0xe  },
+	{ 0x5829, 0x8  },
+	{ 0x582a, 0x6  },
+	{ 0x582b, 0x4  },
+	{ 0x582c, 0x5  },
+	{ 0x582d, 0x7  },
+	{ 0x582e, 0xb  },
+	{ 0x582f, 0x12 },
+	{ 0x5830, 0x18 },
+	{ 0x5831, 0x10 },
+	{ 0x5832, 0xc  },
+	{ 0x5833, 0xa  },
+	{ 0x5834, 0xb  },
+	{ 0x5835, 0xe  },
+	{ 0x5836, 0x15 },
+	{ 0x5837, 0x19 },
+	{ 0x5838, 0x32 },
+	{ 0x5839, 0x1f },
+	{ 0x583a, 0x18 },
+	{ 0x583b, 0x16 },
+	{ 0x583c, 0x17 },
+	{ 0x583d, 0x1e },
+	{ 0x583e, 0x26 },
+	{ 0x583f, 0x53 },
+	{ 0x5840, 0x10 },
+	{ 0x5841, 0xf  },
+	{ 0x5842, 0xd  },
+	{ 0x5843, 0xc  },
+	{ 0x5844, 0xe  },
+	{ 0x5845, 0x9  },
+	{ 0x5846, 0x11 },
+	{ 0x5847, 0x10 },
+	{ 0x5848, 0x10 },
+	{ 0x5849, 0x10 },
+	{ 0x584a, 0x10 },
+	{ 0x584b, 0xe  },
+	{ 0x584c, 0x10 },
+	{ 0x584d, 0x10 },
+	{ 0x584e, 0x11 },
+	{ 0x584f, 0x10 },
+	{ 0x5850, 0xf  },
+	{ 0x5851, 0xc  },
+	{ 0x5852, 0xf  },
+	{ 0x5853, 0x10 },
+	{ 0x5854, 0x10 },
+	{ 0x5855, 0xf  },
+	{ 0x5856, 0xe  },
+	{ 0x5857, 0xb  },
+	{ 0x5858, 0x10 },
+	{ 0x5859, 0xd  },
+	{ 0x585a, 0xd  },
+	{ 0x585b, 0xc  },
+	{ 0x585c, 0xc  },
+	{ 0x585d, 0xc  },
+	{ 0x585e, 0xb  },
+	{ 0x585f, 0xc  },
+	{ 0x5860, 0xc  },
+	{ 0x5861, 0xc  },
+	{ 0x5862, 0xd  },
+	{ 0x5863, 0x8  },
+	{ 0x5864, 0x11 },
+	{ 0x5865, 0x18 },
+	{ 0x5866, 0x18 },
+	{ 0x5867, 0x19 },
+	{ 0x5868, 0x17 },
+	{ 0x5869, 0x19 },
+	{ 0x586a, 0x16 },
+	{ 0x586b, 0x13 },
+	{ 0x586c, 0x13 },
+	{ 0x586d, 0x12 },
+	{ 0x586e, 0x13 },
+	{ 0x586f, 0x16 },
+	{ 0x5870, 0x14 },
+	{ 0x5871, 0x12 },
+	{ 0x5872, 0x10 },
+	{ 0x5873, 0x11 },
+	{ 0x5874, 0x11 },
+	{ 0x5875, 0x16 },
+	{ 0x5876, 0x14 },
+	{ 0x5877, 0x11 },
+	{ 0x5878, 0x10 },
+	{ 0x5879, 0xf  },
+	{ 0x587a, 0x10 },
+	{ 0x587b, 0x14 },
+	{ 0x587c, 0x13 },
+	{ 0x587d, 0x12 },
+	{ 0x587e, 0x11 },
+	{ 0x587f, 0x11 },
+	{ 0x5880, 0x12 },
+	{ 0x5881, 0x15 },
+	{ 0x5882, 0x14 },
+	{ 0x5883, 0x15 },
+	{ 0x5884, 0x15 },
+	{ 0x5885, 0x15 },
+	{ 0x5886, 0x13 },
+	{ 0x5887, 0x17 },
+	{ 0x3710, 0x10 },
+	{ 0x3632, 0x51 },
+	{ 0x3702, 0x10 },
+	{ 0x3703, 0xb2 },
+	{ 0x3704, 0x18 },
+	{ 0x370b, 0x40 },
+	{ 0x370d, 0x3  },
+	{ 0x3631, 0x1  },
+	{ 0x3632, 0x52 },
+	{ 0x3606, 0x24 },
+	{ 0x3620, 0x96 },
+	{ 0x5785, 0x7  },
+	{ 0x3a13, 0x30 },
+	{ 0x3600, 0x52 },
+	{ 0x3604, 0x48 },
+	{ 0x3606, 0x1b },
+	{ 0x370d, 0xb  },
+	{ 0x370f, 0xc0 },
+	{ 0x3709, 0x1  },
+	{ 0x3823, 0x0  },
+	{ 0x5007, 0x0  },
+	{ 0x5009, 0x0  },
+	{ 0x5011, 0x0  },
+	{ 0x5013, 0x0  },
+	{ 0x519e, 0x0  },
+	{ 0x5086, 0x0  },
+	{ 0x5087, 0x0  },
+	{ 0x5088, 0x0  },
+	{ 0x5089, 0x0  },
+	{ 0x302b, 0x0  },
+	{ 0x3503, 0x7  },
+	{ 0x3011, 0x8  },
+	{ 0x350c, 0x2  },
+	{ 0x350d, 0xe4 },
+	{ 0x3621, 0xc9 },
+	{ 0x370a, 0x81 },
+	{ 0xffff, 0xff },
+};
+
+static struct regval_list ov5642_default_regs_finalise[] = {
+	{ 0x3810, 0xc2 },
+	{ 0x3818, 0xc9 },
+	{ 0x381c, 0x10 },
+	{ 0x381d, 0xa0 },
+	{ 0x381e, 0x5  },
+	{ 0x381f, 0xb0 },
+	{ 0x3820, 0x0  },
+	{ 0x3821, 0x0  },
+	{ 0x3824, 0x11 },
+	{ 0x3a08, 0x1b },
+	{ 0x3a09, 0xc0 },
+	{ 0x3a0a, 0x17 },
+	{ 0x3a0b, 0x20 },
+	{ 0x3a0d, 0x2  },
+	{ 0x3a0e, 0x1  },
+	{ 0x401c, 0x4  },
+	{ 0x5682, 0x5  },
+	{ 0x5683, 0x0  },
+	{ 0x5686, 0x2  },
+	{ 0x5687, 0xcc },
+	{ 0x5001, 0x4f },
+	{ 0x589b, 0x6  },
+	{ 0x589a, 0xc5 },
+	{ 0x3503, 0x0  },
+	{ 0x460c, 0x20 },
+	{ 0x460b, 0x37 },
+	{ 0x471c, 0xd0 },
+	{ 0x471d, 0x5  },
+	{ 0x3815, 0x1  },
+	{ 0x3818, 0xc1 },
+	{ 0x501f, 0x0  },
+	{ 0x5002, 0xe0 },
+	{ 0x4300, 0x32 }, /* UYVY */
+	{ 0x3002, 0x1c },
+	{ 0x4800, 0x14 },
+	{ 0x4801, 0xf  },
+	{ 0x3007, 0x3b },
+	{ 0x300e, 0x4  },
+	{ 0x4803, 0x50 },
+	{ 0x3815, 0x1  },
+	{ 0x4713, 0x2  },
+	{ 0x4842, 0x1  },
+	{ 0x300f, 0xe  },
+	{ 0x3003, 0x3  },
+	{ 0x3003, 0x1  },
+	{ 0xffff, 0xff },
+};
+
+struct ov5642_datafmt {
+	enum v4l2_mbus_pixelcode	code;
+	enum v4l2_colorspace		colorspace;
+};
+
+struct ov5642 {
+	struct v4l2_subdev		subdev;
+	const struct ov5642_datafmt	*fmt;
+	struct v4l2_rect                crop_rect;
+
+	/* blanking information */
+	int total_width;
+	int total_height;
+};
+
+static const struct ov5642_datafmt ov5642_colour_fmts[] = {
+	{V4L2_MBUS_FMT_UYVY8_2X8, V4L2_COLORSPACE_JPEG},
+};
+
+static struct ov5642 *to_ov5642(const struct i2c_client *client)
+{
+	return container_of(i2c_get_clientdata(client), struct ov5642, subdev);
+}
+
+/* Find a data format by a pixel code in an array */
+static const struct ov5642_datafmt
+			*ov5642_find_datafmt(enum v4l2_mbus_pixelcode code)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(ov5642_colour_fmts); i++)
+		if (ov5642_colour_fmts[i].code == code)
+			return ov5642_colour_fmts + i;
+
+	return NULL;
+}
+
+static int reg_read(struct i2c_client *client, u16 reg, u8 *val)
+{
+	int ret;
+	/* We have 16-bit i2c addresses - care for endianess */
+	unsigned char data[2] = { reg >> 8, reg & 0xff };
+
+	ret = i2c_master_send(client, data, 2);
+	if (ret < 2) {
+		dev_err(&client->dev, "%s: i2c read error, reg: %x\n",
+			__func__, reg);
+		return ret < 0 ? ret : -EIO;
+	}
+
+	ret = i2c_master_recv(client, val, 1);
+	if (ret < 1) {
+		dev_err(&client->dev, "%s: i2c read error, reg: %x\n",
+				__func__, reg);
+		return ret < 0 ? ret : -EIO;
+	}
+	return 0;
+}
+
+static int reg_write(struct i2c_client *client, u16 reg, u8 val)
+{
+	int ret;
+	unsigned char data[3] = { reg >> 8, reg & 0xff, val };
+
+	ret = i2c_master_send(client, data, 3);
+	if (ret < 3) {
+		dev_err(&client->dev, "%s: i2c write error, reg: %x\n",
+			__func__, reg);
+		return ret < 0 ? ret : -EIO;
+	}
+
+	return 0;
+}
+
+/*
+ * convenience function to write 16 bit register values that are split up
+ * into two consecutive high and low parts
+ */
+static int reg_write16(struct i2c_client *client, u16 reg, u16 val16)
+{
+	int ret;
+
+	ret = reg_write(client, reg, val16 >> 8);
+	if (ret)
+		return ret;
+	return reg_write(client, reg + 1, val16 & 0x00ff);
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int ov5642_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 & ~0xffff)
+		return -EINVAL;
+
+	reg->size = 1;
+
+	ret = reg_read(client, reg->reg, &val);
+	if (!ret)
+		reg->val = (__u64)val;
+
+	return ret;
+}
+
+static int ov5642_set_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (reg->reg & ~0xffff || reg->val & ~0xff)
+		return -EINVAL;
+
+	return reg_write(client, reg->reg, reg->val);
+}
+#endif
+
+static int ov5642_write_array(struct i2c_client *client,
+				struct regval_list *vals)
+{
+	while (vals->reg_num != 0xffff || vals->value != 0xff) {
+		int ret = reg_write(client, vals->reg_num, vals->value);
+		if (ret < 0)
+			return ret;
+		vals++;
+	}
+	dev_dbg(&client->dev, "Register list loaded\n");
+	return 0;
+}
+
+static int ov5642_set_resolution(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov5642 *priv = to_ov5642(client);
+	int width = priv->crop_rect.width;
+	int height = priv->crop_rect.height;
+	int total_width = priv->total_width;
+	int total_height = priv->total_height;
+	int start_x = (OV5642_SENSOR_SIZE_X - width) / 2;
+	int start_y = (OV5642_SENSOR_SIZE_Y - height) / 2;
+	int ret;
+
+	/*
+	 * This should set the starting point for cropping.
+	 * Doesn't work so far.
+	 */
+	ret = reg_write16(client, REG_WINDOW_START_X_HIGH, start_x);
+	if (!ret)
+		ret = reg_write16(client, REG_WINDOW_START_Y_HIGH, start_y);
+	if (!ret) {
+		priv->crop_rect.left = start_x;
+		priv->crop_rect.top = start_y;
+	}
+
+	if (!ret)
+		ret = reg_write16(client, REG_WINDOW_WIDTH_HIGH, width);
+	if (!ret)
+		ret = reg_write16(client, REG_WINDOW_HEIGHT_HIGH, height);
+	if (ret)
+		return ret;
+	priv->crop_rect.width = width;
+	priv->crop_rect.height = height;
+
+	/* Set the output window size. Only 1:1 scale is supported so far. */
+	ret = reg_write16(client, REG_OUT_WIDTH_HIGH, width);
+	if (!ret)
+		ret = reg_write16(client, REG_OUT_HEIGHT_HIGH, height);
+
+	/* Total width = output size + blanking */
+	if (!ret)
+		ret = reg_write16(client, REG_OUT_TOTAL_WIDTH_HIGH, total_width);
+	if (!ret)
+		ret = reg_write16(client, REG_OUT_TOTAL_HEIGHT_HIGH, total_height);
+
+	/* Sets the window for AWB calculations */
+	if (!ret)
+		ret = reg_write16(client, REG_AVG_WINDOW_END_X_HIGH, width);
+	if (!ret)
+		ret = reg_write16(client, REG_AVG_WINDOW_END_Y_HIGH, height);
+
+	return ret;
+}
+
+static int ov5642_try_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_mbus_framefmt *mf)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov5642 *priv = to_ov5642(client);
+	const struct ov5642_datafmt *fmt = ov5642_find_datafmt(mf->code);
+
+	mf->width = priv->crop_rect.width;
+	mf->height = priv->crop_rect.height;
+
+	if (!fmt) {
+		mf->code	= ov5642_colour_fmts[0].code;
+		mf->colorspace	= ov5642_colour_fmts[0].colorspace;
+	}
+
+	mf->field	= V4L2_FIELD_NONE;
+
+	return 0;
+}
+
+static int ov5642_s_fmt(struct v4l2_subdev *sd,
+			struct v4l2_mbus_framefmt *mf)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov5642 *priv = to_ov5642(client);
+
+	/* MIPI CSI could have changed the format, double-check */
+	if (!ov5642_find_datafmt(mf->code))
+		return -EINVAL;
+
+	ov5642_try_fmt(sd, mf);
+	priv->fmt = ov5642_find_datafmt(mf->code);
+
+	return 0;
+}
+
+static int ov5642_g_fmt(struct v4l2_subdev *sd,
+			struct v4l2_mbus_framefmt *mf)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov5642 *priv = to_ov5642(client);
+
+	const struct ov5642_datafmt *fmt = priv->fmt;
+
+	mf->code	= fmt->code;
+	mf->colorspace	= fmt->colorspace;
+	mf->width	= priv->crop_rect.width;
+	mf->height	= priv->crop_rect.height;
+	mf->field	= V4L2_FIELD_NONE;
+
+	return 0;
+}
+
+static int ov5642_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
+			   enum v4l2_mbus_pixelcode *code)
+{
+	if (index >= ARRAY_SIZE(ov5642_colour_fmts))
+		return -EINVAL;
+
+	*code = ov5642_colour_fmts[index].code;
+	return 0;
+}
+
+static int ov5642_g_chip_ident(struct v4l2_subdev *sd,
+			       struct v4l2_dbg_chip_ident *id)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (id->match.type != V4L2_CHIP_MATCH_I2C_ADDR)
+		return -EINVAL;
+
+	if (id->match.addr != client->addr)
+		return -ENODEV;
+
+	id->ident	= V4L2_IDENT_OV5642;
+	id->revision	= 0;
+
+	return 0;
+}
+
+static int ov5642_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov5642 *priv = to_ov5642(client);
+	struct v4l2_rect rect = a->c;
+	int ret;
+
+	v4l_bound_align_image(&rect.width, 48, OV5642_MAX_WIDTH, 1,
+			      &rect.height, 32, OV5642_MAX_HEIGHT, 1, 0);
+
+	priv->crop_rect.width	= rect.width;
+	priv->crop_rect.height	= rect.height;
+	priv->total_width	= rect.width + BLANKING_EXTRA_WIDTH;
+	priv->total_height	= max_t(int, rect.height +
+							BLANKING_EXTRA_HEIGHT,
+							BLANKING_MIN_HEIGHT);
+	priv->crop_rect.width		= rect.width;
+	priv->crop_rect.height		= rect.height;
+
+	ret = ov5642_write_array(client, ov5642_default_regs_init);
+	if (!ret)
+		ret = ov5642_set_resolution(sd);
+	if (!ret)
+		ret = ov5642_write_array(client, ov5642_default_regs_finalise);
+
+	return ret;
+}
+
+static int ov5642_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov5642 *priv = to_ov5642(client);
+	struct v4l2_rect *rect = &a->c;
+
+	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	*rect = priv->crop_rect;
+
+	return 0;
+}
+
+static int ov5642_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
+{
+	a->bounds.left			= 0;
+	a->bounds.top			= 0;
+	a->bounds.width			= OV5642_MAX_WIDTH;
+	a->bounds.height		= OV5642_MAX_HEIGHT;
+	a->defrect			= a->bounds;
+	a->type				= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	a->pixelaspect.numerator	= 1;
+	a->pixelaspect.denominator	= 1;
+
+	return 0;
+}
+
+static int ov5642_g_mbus_config(struct v4l2_subdev *sd,
+				struct v4l2_mbus_config *cfg)
+{
+	cfg->type = V4L2_MBUS_CSI2;
+	cfg->flags = V4L2_MBUS_CSI2_2_LANE | V4L2_MBUS_CSI2_CHANNEL_0 |
+					V4L2_MBUS_CSI2_CONTINUOUS_CLOCK;
+
+	return 0;
+}
+
+static int ov5642_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+	int ret;
+
+	if (!on)
+		return soc_camera_power_off(&client->dev, icl);
+
+	ret = soc_camera_power_on(&client->dev, icl);
+	if (ret < 0)
+		return ret;
+
+	ret = ov5642_write_array(client, ov5642_default_regs_init);
+	if (!ret)
+		ret = ov5642_set_resolution(sd);
+	if (!ret)
+		ret = ov5642_write_array(client, ov5642_default_regs_finalise);
+
+	return ret;
+}
+
+static struct v4l2_subdev_video_ops ov5642_subdev_video_ops = {
+	.s_mbus_fmt	= ov5642_s_fmt,
+	.g_mbus_fmt	= ov5642_g_fmt,
+	.try_mbus_fmt	= ov5642_try_fmt,
+	.enum_mbus_fmt	= ov5642_enum_fmt,
+	.s_crop		= ov5642_s_crop,
+	.g_crop		= ov5642_g_crop,
+	.cropcap	= ov5642_cropcap,
+	.g_mbus_config	= ov5642_g_mbus_config,
+};
+
+static struct v4l2_subdev_core_ops ov5642_subdev_core_ops = {
+	.s_power	= ov5642_s_power,
+	.g_chip_ident	= ov5642_g_chip_ident,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register	= ov5642_get_register,
+	.s_register	= ov5642_set_register,
+#endif
+};
+
+static struct v4l2_subdev_ops ov5642_subdev_ops = {
+	.core	= &ov5642_subdev_core_ops,
+	.video	= &ov5642_subdev_video_ops,
+};
+
+static int ov5642_video_probe(struct i2c_client *client)
+{
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	int ret;
+	u8 id_high, id_low;
+	u16 id;
+
+	ret = ov5642_s_power(subdev, 1);
+	if (ret < 0)
+		return ret;
+
+	/* Read sensor Model ID */
+	ret = reg_read(client, REG_CHIP_ID_HIGH, &id_high);
+	if (ret < 0)
+		goto done;
+
+	id = id_high << 8;
+
+	ret = reg_read(client, REG_CHIP_ID_LOW, &id_low);
+	if (ret < 0)
+		goto done;
+
+	id |= id_low;
+
+	dev_info(&client->dev, "Chip ID 0x%04x detected\n", id);
+
+	if (id != 0x5642) {
+		ret = -ENODEV;
+		goto done;
+	}
+
+	ret = 0;
+
+done:
+	ov5642_s_power(subdev, 0);
+	return ret;
+}
+
+static int ov5642_probe(struct i2c_client *client,
+			const struct i2c_device_id *did)
+{
+	struct ov5642 *priv;
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+	int ret;
+
+	if (!icl) {
+		dev_err(&client->dev, "OV5642: missing platform data!\n");
+		return -EINVAL;
+	}
+
+	priv = kzalloc(sizeof(struct ov5642), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	v4l2_i2c_subdev_init(&priv->subdev, client, &ov5642_subdev_ops);
+
+	priv->fmt		= &ov5642_colour_fmts[0];
+
+	priv->crop_rect.width	= OV5642_DEFAULT_WIDTH;
+	priv->crop_rect.height	= OV5642_DEFAULT_HEIGHT;
+	priv->crop_rect.left	= (OV5642_MAX_WIDTH - OV5642_DEFAULT_WIDTH) / 2;
+	priv->crop_rect.top	= (OV5642_MAX_HEIGHT - OV5642_DEFAULT_HEIGHT) / 2;
+	priv->total_width = OV5642_DEFAULT_WIDTH + BLANKING_EXTRA_WIDTH;
+	priv->total_height = BLANKING_MIN_HEIGHT;
+
+	ret = ov5642_video_probe(client);
+	if (ret < 0)
+		goto error;
+
+	return 0;
+
+error:
+	kfree(priv);
+	return ret;
+}
+
+static int ov5642_remove(struct i2c_client *client)
+{
+	struct ov5642 *priv = to_ov5642(client);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+	if (icl->free_bus)
+		icl->free_bus(icl);
+	kfree(priv);
+
+	return 0;
+}
+
+static const struct i2c_device_id ov5642_id[] = {
+	{ "ov5642", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ov5642_id);
+
+static struct i2c_driver ov5642_i2c_driver = {
+	.driver = {
+		.name = "ov5642",
+	},
+	.probe		= ov5642_probe,
+	.remove		= ov5642_remove,
+	.id_table	= ov5642_id,
+};
+
+module_i2c_driver(ov5642_i2c_driver);
+
+MODULE_DESCRIPTION("Omnivision OV5642 Camera driver");
+MODULE_AUTHOR("Bastian Hecht <hechtb@gmail.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/soc_camera/ov6650.c b/drivers/media/i2c/soc_camera/ov6650.c
new file mode 100644
index 000000000000..e87feb0881e3
--- /dev/null
+++ b/drivers/media/i2c/soc_camera/ov6650.c
@@ -0,0 +1,1069 @@
+/*
+ * V4L2 SoC Camera 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 initialy based on former work by Matt Callow
+ * drivers/media/video/omap/sensor_ov6650.c
+ * Copyright (C) 2006 Matt Callow
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/bitops.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/v4l2-mediabus.h>
+#include <linux/module.h>
+
+#include <media/soc_camera.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-ctrls.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_6MHz		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 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;
+	};
+	bool			half_scale;	/* scale down output by 2 */
+	struct v4l2_rect	rect;		/* sensor cropping window */
+	unsigned long		pclk_limit;	/* from host */
+	unsigned long		pclk_max;	/* from resolution and format */
+	struct v4l2_fract	tpf;		/* as requested with s_parm */
+	enum v4l2_mbus_pixelcode code;
+	enum v4l2_colorspace	colorspace;
+};
+
+
+static enum v4l2_mbus_pixelcode ov6650_codes[] = {
+	V4L2_MBUS_FMT_YUYV8_2X8,
+	V4L2_MBUS_FMT_UYVY8_2X8,
+	V4L2_MBUS_FMT_YVYU8_2X8,
+	V4L2_MBUS_FMT_VYUY8_2X8,
+	V4L2_MBUS_FMT_SBGGR8_1X8,
+	V4L2_MBUS_FMT_Y8_1X8,
+};
+
+/* 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;
+}
+
+/* Get chip identification */
+static int ov6650_g_chip_ident(struct v4l2_subdev *sd,
+				struct v4l2_dbg_chip_ident *id)
+{
+	id->ident	= V4L2_IDENT_OV6650;
+	id->revision	= 0;
+
+	return 0;
+}
+
+#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,
+				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 soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+	return soc_camera_set_power(&client->dev, icl, on);
+}
+
+static int ov6650_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov6650 *priv = to_ov6650(client);
+
+	a->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	a->c = priv->rect;
+
+	return 0;
+}
+
+static int ov6650_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov6650 *priv = to_ov6650(client);
+	struct v4l2_rect rect = a->c;
+	int ret;
+
+	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	rect.left   = ALIGN(rect.left,   2);
+	rect.width  = ALIGN(rect.width,  2);
+	rect.top    = ALIGN(rect.top,    2);
+	rect.height = ALIGN(rect.height, 2);
+	soc_camera_limit_side(&rect.left, &rect.width,
+			DEF_HSTRT << 1, 2, W_CIF);
+	soc_camera_limit_side(&rect.top, &rect.height,
+			DEF_VSTRT << 1, 2, H_CIF);
+
+	ret = ov6650_reg_write(client, REG_HSTRT, rect.left >> 1);
+	if (!ret) {
+		priv->rect.left = rect.left;
+		ret = ov6650_reg_write(client, REG_HSTOP,
+				(rect.left + rect.width) >> 1);
+	}
+	if (!ret) {
+		priv->rect.width = rect.width;
+		ret = ov6650_reg_write(client, REG_VSTRT, rect.top >> 1);
+	}
+	if (!ret) {
+		priv->rect.top = rect.top;
+		ret = ov6650_reg_write(client, REG_VSTOP,
+				(rect.top + rect.height) >> 1);
+	}
+	if (!ret)
+		priv->rect.height = rect.height;
+
+	return ret;
+}
+
+static int ov6650_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
+{
+	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	a->bounds.left			= DEF_HSTRT << 1;
+	a->bounds.top			= DEF_VSTRT << 1;
+	a->bounds.width			= W_CIF;
+	a->bounds.height		= H_CIF;
+	a->defrect			= a->bounds;
+	a->pixelaspect.numerator	= 1;
+	a->pixelaspect.denominator	= 1;
+
+	return 0;
+}
+
+static int ov6650_g_fmt(struct v4l2_subdev *sd,
+			 struct v4l2_mbus_framefmt *mf)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov6650 *priv = to_ov6650(client);
+
+	mf->width	= priv->rect.width >> priv->half_scale;
+	mf->height	= priv->rect.height >> priv->half_scale;
+	mf->code	= priv->code;
+	mf->colorspace	= priv->colorspace;
+	mf->field	= V4L2_FIELD_NONE;
+
+	return 0;
+}
+
+static bool is_unscaled_ok(int width, int height, struct v4l2_rect *rect)
+{
+	return width > rect->width >> 1 || height > rect->height >> 1;
+}
+
+static u8 to_clkrc(struct v4l2_fract *timeperframe,
+		unsigned long pclk_limit, unsigned long pclk_max)
+{
+	unsigned long pclk;
+
+	if (timeperframe->numerator && timeperframe->denominator)
+		pclk = pclk_max * timeperframe->denominator /
+				(FRAME_RATE_MAX * timeperframe->numerator);
+	else
+		pclk = pclk_max;
+
+	if (pclk_limit && pclk_limit < pclk)
+		pclk = pclk_limit;
+
+	return (pclk_max - 1) / pclk;
+}
+
+/* set the format we will capture in */
+static int ov6650_s_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *mf)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct soc_camera_device *icd = v4l2_get_subdev_hostdata(sd);
+	struct soc_camera_sense *sense = icd->sense;
+	struct ov6650 *priv = to_ov6650(client);
+	bool half_scale = !is_unscaled_ok(mf->width, mf->height, &priv->rect);
+	struct v4l2_crop a = {
+		.type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
+		.c = {
+			.left	= priv->rect.left + (priv->rect.width >> 1) -
+					(mf->width >> (1 - half_scale)),
+			.top	= priv->rect.top + (priv->rect.height >> 1) -
+					(mf->height >> (1 - half_scale)),
+			.width	= mf->width << half_scale,
+			.height	= mf->height << half_scale,
+		},
+	};
+	enum v4l2_mbus_pixelcode code = mf->code;
+	unsigned long mclk, pclk;
+	u8 coma_set = 0, coma_mask = 0, coml_set, coml_mask, clkrc;
+	int ret;
+
+	/* select color matrix configuration for given color encoding */
+	switch (code) {
+	case V4L2_MBUS_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 V4L2_MBUS_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 V4L2_MBUS_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 V4L2_MBUS_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 V4L2_MBUS_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 V4L2_MBUS_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;
+	}
+	priv->code = code;
+
+	if (code == V4L2_MBUS_FMT_Y8_1X8 ||
+			code == V4L2_MBUS_FMT_SBGGR8_1X8) {
+		coml_mask = COML_ONE_CHANNEL;
+		coml_set = 0;
+		priv->pclk_max = 4000000;
+	} else {
+		coml_mask = 0;
+		coml_set = COML_ONE_CHANNEL;
+		priv->pclk_max = 8000000;
+	}
+
+	if (code == V4L2_MBUS_FMT_SBGGR8_1X8)
+		priv->colorspace = V4L2_COLORSPACE_SRGB;
+	else if (code != 0)
+		priv->colorspace = V4L2_COLORSPACE_JPEG;
+
+	if (half_scale) {
+		dev_dbg(&client->dev, "max resolution: QCIF\n");
+		coma_set |= COMA_QCIF;
+		priv->pclk_max /= 2;
+	} else {
+		dev_dbg(&client->dev, "max resolution: CIF\n");
+		coma_mask |= COMA_QCIF;
+	}
+	priv->half_scale = half_scale;
+
+	if (sense) {
+		if (sense->master_clock == 8000000) {
+			dev_dbg(&client->dev, "8MHz input clock\n");
+			clkrc = CLKRC_6MHz;
+		} else if (sense->master_clock == 12000000) {
+			dev_dbg(&client->dev, "12MHz input clock\n");
+			clkrc = CLKRC_12MHz;
+		} else if (sense->master_clock == 16000000) {
+			dev_dbg(&client->dev, "16MHz input clock\n");
+			clkrc = CLKRC_16MHz;
+		} else if (sense->master_clock == 24000000) {
+			dev_dbg(&client->dev, "24MHz input clock\n");
+			clkrc = CLKRC_24MHz;
+		} else {
+			dev_err(&client->dev,
+				"unsupported input clock, check platform data\n");
+			return -EINVAL;
+		}
+		mclk = sense->master_clock;
+		priv->pclk_limit = sense->pixel_clock_max;
+	} else {
+		clkrc = CLKRC_24MHz;
+		mclk = 24000000;
+		priv->pclk_limit = 0;
+		dev_dbg(&client->dev, "using default 24MHz input clock\n");
+	}
+
+	clkrc |= to_clkrc(&priv->tpf, priv->pclk_limit, priv->pclk_max);
+
+	pclk = priv->pclk_max / GET_CLKRC_DIV(clkrc);
+	dev_dbg(&client->dev, "pixel clock divider: %ld.%ld\n",
+			mclk / pclk, 10 * mclk % pclk / pclk);
+
+	ret = ov6650_s_crop(sd, &a);
+	if (!ret)
+		ret = ov6650_reg_rmw(client, REG_COMA, coma_set, coma_mask);
+	if (!ret)
+		ret = ov6650_reg_write(client, REG_CLKRC, clkrc);
+	if (!ret)
+		ret = ov6650_reg_rmw(client, REG_COML, coml_set, coml_mask);
+
+	if (!ret) {
+		mf->colorspace	= priv->colorspace;
+		mf->width = priv->rect.width >> half_scale;
+		mf->height = priv->rect.height >> half_scale;
+	}
+
+	return ret;
+}
+
+static int ov6650_try_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_mbus_framefmt *mf)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov6650 *priv = to_ov6650(client);
+
+	if (is_unscaled_ok(mf->width, mf->height, &priv->rect))
+		v4l_bound_align_image(&mf->width, 2, W_CIF, 1,
+				&mf->height, 2, H_CIF, 1, 0);
+
+	mf->field = V4L2_FIELD_NONE;
+
+	switch (mf->code) {
+	case V4L2_MBUS_FMT_Y10_1X10:
+		mf->code = V4L2_MBUS_FMT_Y8_1X8;
+	case V4L2_MBUS_FMT_Y8_1X8:
+	case V4L2_MBUS_FMT_YVYU8_2X8:
+	case V4L2_MBUS_FMT_YUYV8_2X8:
+	case V4L2_MBUS_FMT_VYUY8_2X8:
+	case V4L2_MBUS_FMT_UYVY8_2X8:
+		mf->colorspace = V4L2_COLORSPACE_JPEG;
+		break;
+	default:
+		mf->code = V4L2_MBUS_FMT_SBGGR8_1X8;
+	case V4L2_MBUS_FMT_SBGGR8_1X8:
+		mf->colorspace = V4L2_COLORSPACE_SRGB;
+		break;
+	}
+
+	return 0;
+}
+
+static int ov6650_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
+			   enum v4l2_mbus_pixelcode *code)
+{
+	if (index >= ARRAY_SIZE(ov6650_codes))
+		return -EINVAL;
+
+	*code = ov6650_codes[index];
+	return 0;
+}
+
+static int ov6650_g_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *parms)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov6650 *priv = to_ov6650(client);
+	struct v4l2_captureparm *cp = &parms->parm.capture;
+
+	if (parms->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	memset(cp, 0, sizeof(*cp));
+	cp->capability = V4L2_CAP_TIMEPERFRAME;
+	cp->timeperframe.numerator = GET_CLKRC_DIV(to_clkrc(&priv->tpf,
+			priv->pclk_limit, priv->pclk_max));
+	cp->timeperframe.denominator = FRAME_RATE_MAX;
+
+	dev_dbg(&client->dev, "Frame interval: %u/%u s\n",
+		cp->timeperframe.numerator, cp->timeperframe.denominator);
+
+	return 0;
+}
+
+static int ov6650_s_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *parms)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov6650 *priv = to_ov6650(client);
+	struct v4l2_captureparm *cp = &parms->parm.capture;
+	struct v4l2_fract *tpf = &cp->timeperframe;
+	int div, ret;
+	u8 clkrc;
+
+	if (parms->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	if (cp->extendedmode != 0)
+		return -EINVAL;
+
+	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);
+
+	/*
+	 * Keep result to be used as tpf limit
+	 * for subseqent clock divider calculations
+	 */
+	priv->tpf.numerator = div;
+	priv->tpf.denominator = FRAME_RATE_MAX;
+
+	clkrc = to_clkrc(&priv->tpf, priv->pclk_limit, priv->pclk_max);
+
+	ret = ov6650_reg_rmw(client, REG_CLKRC, clkrc, CLKRC_DIV_MASK);
+	if (!ret) {
+		tpf->numerator = GET_CLKRC_DIV(clkrc);
+		tpf->denominator = FRAME_RATE_MAX;
+	}
+
+	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)
+{
+	int ret;
+
+	dev_dbg(&client->dev, "initializing\n");
+
+	ret = ov6650_reg_write(client, REG_COMA, 0);	/* ~COMA_RESET */
+	if (!ret)
+		ret = ov6650_reg_rmw(client, REG_COMB, 0, COMB_BAND_FILTER);
+
+	return ret;
+}
+
+static int ov6650_video_probe(struct i2c_client *client)
+{
+	struct ov6650 *priv = to_ov6650(client);
+	u8		pidh, pidl, midh, midl;
+	int		ret;
+
+	ret = ov6650_s_power(&priv->subdev, 1);
+	if (ret < 0)
+		return ret;
+
+	/*
+	 * 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);
+	if (!ret)
+		ret = v4l2_ctrl_handler_setup(&priv->hdl);
+
+done:
+	ov6650_s_power(&priv->subdev, 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 struct v4l2_subdev_core_ops ov6650_core_ops = {
+	.g_chip_ident		= ov6650_g_chip_ident,
+#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_g_mbus_config(struct v4l2_subdev *sd,
+				struct v4l2_mbus_config *cfg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+	cfg->flags = V4L2_MBUS_MASTER |
+		V4L2_MBUS_PCLK_SAMPLE_RISING | V4L2_MBUS_PCLK_SAMPLE_FALLING |
+		V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_HSYNC_ACTIVE_LOW |
+		V4L2_MBUS_VSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_LOW |
+		V4L2_MBUS_DATA_ACTIVE_HIGH;
+	cfg->type = V4L2_MBUS_PARALLEL;
+	cfg->flags = soc_camera_apply_board_flags(icl, cfg);
+
+	return 0;
+}
+
+/* Alter bus settings on camera side */
+static int ov6650_s_mbus_config(struct v4l2_subdev *sd,
+				const struct v4l2_mbus_config *cfg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+	unsigned long flags = soc_camera_apply_board_flags(icl, cfg);
+	int ret;
+
+	if (flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
+		ret = ov6650_reg_rmw(client, REG_COMJ, COMJ_PCLK_RISING, 0);
+	else
+		ret = ov6650_reg_rmw(client, REG_COMJ, 0, COMJ_PCLK_RISING);
+	if (ret)
+		return ret;
+
+	if (flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
+		ret = ov6650_reg_rmw(client, REG_COMF, COMF_HREF_LOW, 0);
+	else
+		ret = ov6650_reg_rmw(client, REG_COMF, 0, COMF_HREF_LOW);
+	if (ret)
+		return ret;
+
+	if (flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH)
+		ret = ov6650_reg_rmw(client, REG_COMJ, COMJ_VSYNC_HIGH, 0);
+	else
+		ret = ov6650_reg_rmw(client, REG_COMJ, 0, COMJ_VSYNC_HIGH);
+
+	return ret;
+}
+
+static struct v4l2_subdev_video_ops ov6650_video_ops = {
+	.s_stream	= ov6650_s_stream,
+	.g_mbus_fmt	= ov6650_g_fmt,
+	.s_mbus_fmt	= ov6650_s_fmt,
+	.try_mbus_fmt	= ov6650_try_fmt,
+	.enum_mbus_fmt	= ov6650_enum_fmt,
+	.cropcap	= ov6650_cropcap,
+	.g_crop		= ov6650_g_crop,
+	.s_crop		= ov6650_s_crop,
+	.g_parm		= ov6650_g_parm,
+	.s_parm		= ov6650_s_parm,
+	.g_mbus_config	= ov6650_g_mbus_config,
+	.s_mbus_config	= ov6650_s_mbus_config,
+};
+
+static struct v4l2_subdev_ops ov6650_subdev_ops = {
+	.core	= &ov6650_core_ops,
+	.video	= &ov6650_video_ops,
+};
+
+/*
+ * i2c_driver function
+ */
+static int ov6650_probe(struct i2c_client *client,
+			const struct i2c_device_id *did)
+{
+	struct ov6650 *priv;
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+	int ret;
+
+	if (!icl) {
+		dev_err(&client->dev, "Missing platform_data for driver\n");
+		return -EINVAL;
+	}
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv) {
+		dev_err(&client->dev,
+			"Failed to allocate memory for private data!\n");
+		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) {
+		int err = priv->hdl.error;
+
+		kfree(priv);
+		return err;
+	}
+	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;
+	priv->half_scale  = false;
+	priv->code	  = V4L2_MBUS_FMT_YUYV8_2X8;
+	priv->colorspace  = V4L2_COLORSPACE_JPEG;
+
+	ret = ov6650_video_probe(client);
+	if (ret) {
+		v4l2_ctrl_handler_free(&priv->hdl);
+		kfree(priv);
+	}
+
+	return ret;
+}
+
+static int ov6650_remove(struct i2c_client *client)
+{
+	struct ov6650 *priv = to_ov6650(client);
+
+	v4l2_device_unregister_subdev(&priv->subdev);
+	v4l2_ctrl_handler_free(&priv->hdl);
+	kfree(priv);
+	return 0;
+}
+
+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("SoC Camera driver for OmniVision OV6650");
+MODULE_AUTHOR("Janusz Krzysztofik <jkrzyszt@tis.icnet.pl>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/soc_camera/ov772x.c b/drivers/media/i2c/soc_camera/ov772x.c
new file mode 100644
index 000000000000..e4a10751894d
--- /dev/null
+++ b/drivers/media/i2c/soc_camera/ov772x.c
@@ -0,0 +1,1137 @@
+/*
+ * 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>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/init.h>
+#include <linux/kernel.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/ov772x.h>
+#include <media/soc_camera.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-chip-ident.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)
+
+#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 */
+
+/* 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 */
+
+/* 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 VGA_WIDTH		640
+#define VGA_HEIGHT		480
+#define QVGA_WIDTH		320
+#define QVGA_HEIGHT		240
+#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))
+
+/*
+ * struct
+ */
+
+struct ov772x_color_format {
+	enum v4l2_mbus_pixelcode code;
+	enum v4l2_colorspace colorspace;
+	u8 dsp3;
+	u8 dsp4;
+	u8 com3;
+	u8 com7;
+};
+
+struct ov772x_win_size {
+	char                     *name;
+	unsigned char             com7_bit;
+	struct v4l2_rect	  rect;
+};
+
+struct ov772x_priv {
+	struct v4l2_subdev                subdev;
+	struct v4l2_ctrl_handler	  hdl;
+	struct ov772x_camera_info        *info;
+	const struct ov772x_color_format *cfmt;
+	const struct ov772x_win_size     *win;
+	int                               model;
+	unsigned short                    flag_vflip:1;
+	unsigned short                    flag_hflip:1;
+	/* band_filter = COM8[5] ? 256 - BDBASE : 0 */
+	unsigned short                    band_filter;
+};
+
+/*
+ * supported color format list
+ */
+static const struct ov772x_color_format ov772x_cfmts[] = {
+	{
+		.code		= V4L2_MBUS_FMT_YUYV8_2X8,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+		.dsp3		= 0x0,
+		.dsp4		= DSP_OFMT_YUV,
+		.com3		= SWAP_YUV,
+		.com7		= OFMT_YUV,
+	},
+	{
+		.code		= V4L2_MBUS_FMT_YVYU8_2X8,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+		.dsp3		= UV_ON,
+		.dsp4		= DSP_OFMT_YUV,
+		.com3		= SWAP_YUV,
+		.com7		= OFMT_YUV,
+	},
+	{
+		.code		= V4L2_MBUS_FMT_UYVY8_2X8,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+		.dsp3		= 0x0,
+		.dsp4		= DSP_OFMT_YUV,
+		.com3		= 0x0,
+		.com7		= OFMT_YUV,
+	},
+	{
+		.code		= V4L2_MBUS_FMT_RGB555_2X8_PADHI_LE,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.dsp3		= 0x0,
+		.dsp4		= DSP_OFMT_YUV,
+		.com3		= SWAP_RGB,
+		.com7		= FMT_RGB555 | OFMT_RGB,
+	},
+	{
+		.code		= V4L2_MBUS_FMT_RGB555_2X8_PADHI_BE,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.dsp3		= 0x0,
+		.dsp4		= DSP_OFMT_YUV,
+		.com3		= 0x0,
+		.com7		= FMT_RGB555 | OFMT_RGB,
+	},
+	{
+		.code		= V4L2_MBUS_FMT_RGB565_2X8_LE,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.dsp3		= 0x0,
+		.dsp4		= DSP_OFMT_YUV,
+		.com3		= SWAP_RGB,
+		.com7		= FMT_RGB565 | OFMT_RGB,
+	},
+	{
+		.code		= V4L2_MBUS_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		= V4L2_MBUS_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,
+		.rect = {
+			.left = 140,
+			.top = 14,
+			.width = VGA_WIDTH,
+			.height = VGA_HEIGHT,
+		},
+	}, {
+		.name     = "QVGA",
+		.com7_bit = SLCT_QVGA,
+		.rect = {
+			.left = 252,
+			.top = 6,
+			.width = QVGA_WIDTH,
+			.height = QVGA_HEIGHT,
+		},
+	},
+};
+
+/*
+ * general function
+ */
+
+static struct ov772x_priv *to_ov772x(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct ov772x_priv, subdev);
+}
+
+static inline int ov772x_read(struct i2c_client *client, u8 addr)
+{
+	return i2c_smbus_read_byte_data(client, addr);
+}
+
+static inline int ov772x_write(struct i2c_client *client, u8 addr, u8 value)
+{
+	return i2c_smbus_write_byte_data(client, addr, value);
+}
+
+static int ov772x_mask_set(struct i2c_client *client, u8  command, u8  mask,
+			   u8  set)
+{
+	s32 val = ov772x_read(client, command);
+	if (val < 0)
+		return val;
+
+	val &= ~mask;
+	val |= set & mask;
+
+	return ov772x_write(client, command, val);
+}
+
+static int ov772x_reset(struct i2c_client *client)
+{
+	int ret;
+
+	ret = ov772x_write(client, COM7, SCCB_RESET);
+	if (ret < 0)
+		return ret;
+
+	msleep(1);
+
+	return ov772x_mask_set(client, COM2, SOFT_SLEEP_MODE, SOFT_SLEEP_MODE);
+}
+
+/*
+ * soc_camera_ops function
+ */
+
+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);
+
+	if (!enable) {
+		ov772x_mask_set(client, COM2, SOFT_SLEEP_MODE, SOFT_SLEEP_MODE);
+		return 0;
+	}
+
+	ov772x_mask_set(client, COM2, SOFT_SLEEP_MODE, 0);
+
+	dev_dbg(&client->dev, "format %d, win %s\n",
+		priv->cfmt->code, priv->win->name);
+
+	return 0;
+}
+
+static int ov772x_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov772x_priv *priv = container_of(ctrl->handler,
+						struct ov772x_priv, hdl);
+	struct v4l2_subdev *sd = &priv->subdev;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+	u8 val;
+
+	switch (ctrl->id) {
+	case V4L2_CID_VFLIP:
+		val = ctrl->val ? VFLIP_IMG : 0x00;
+		priv->flag_vflip = ctrl->val;
+		if (priv->info->flags & OV772X_FLAG_VFLIP)
+			val ^= VFLIP_IMG;
+		return ov772x_mask_set(client, COM3, VFLIP_IMG, val);
+	case V4L2_CID_HFLIP:
+		val = ctrl->val ? HFLIP_IMG : 0x00;
+		priv->flag_hflip = ctrl->val;
+		if (priv->info->flags & OV772X_FLAG_HFLIP)
+			val ^= HFLIP_IMG;
+		return ov772x_mask_set(client, COM3, HFLIP_IMG, val);
+	case V4L2_CID_BAND_STOP_FILTER:
+		if (!ctrl->val) {
+			/* Switch the filter off, it is on now */
+			ret = ov772x_mask_set(client, BDBASE, 0xff, 0xff);
+			if (!ret)
+				ret = ov772x_mask_set(client, COM8,
+						      BNDF_ON_OFF, 0);
+		} else {
+			/* Switch the filter on, set AEC low limit */
+			val = 256 - ctrl->val;
+			ret = ov772x_mask_set(client, COM8,
+					      BNDF_ON_OFF, BNDF_ON_OFF);
+			if (!ret)
+				ret = ov772x_mask_set(client, BDBASE,
+						      0xff, val);
+		}
+		if (!ret)
+			priv->band_filter = ctrl->val;
+		return ret;
+	}
+
+	return -EINVAL;
+}
+
+static int ov772x_g_chip_ident(struct v4l2_subdev *sd,
+			       struct v4l2_dbg_chip_ident *id)
+{
+	struct ov772x_priv *priv = to_ov772x(sd);
+
+	id->ident    = priv->model;
+	id->revision = 0;
+
+	return 0;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int ov772x_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 = ov772x_read(client, reg->reg);
+	if (ret < 0)
+		return ret;
+
+	reg->val = (__u64)ret;
+
+	return 0;
+}
+
+static int ov772x_s_register(struct v4l2_subdev *sd,
+			     struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (reg->reg > 0xff ||
+	    reg->val > 0xff)
+		return -EINVAL;
+
+	return ov772x_write(client, reg->reg, reg->val);
+}
+#endif
+
+static int ov772x_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+	return soc_camera_set_power(&client->dev, icl, on);
+}
+
+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_set_params(struct ov772x_priv *priv,
+			     const struct ov772x_color_format *cfmt,
+			     const struct ov772x_win_size *win)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&priv->subdev);
+	int ret;
+	u8  val;
+
+	/*
+	 * reset hardware
+	 */
+	ov772x_reset(client);
+
+	/*
+	 * Edge Ctrl
+	 */
+	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 = ov772x_mask_set(client, DSPAUTO, EDGE_ACTRL, 0x00);
+		if (ret < 0)
+			goto ov772x_set_fmt_error;
+
+		ret = ov772x_mask_set(client,
+				      EDGE_TRSHLD, OV772X_EDGE_THRESHOLD_MASK,
+				      priv->info->edgectrl.threshold);
+		if (ret < 0)
+			goto ov772x_set_fmt_error;
+
+		ret = ov772x_mask_set(client,
+				      EDGE_STRNGT, OV772X_EDGE_STRENGTH_MASK,
+				      priv->info->edgectrl.strength);
+		if (ret < 0)
+			goto ov772x_set_fmt_error;
+
+	} else if (priv->info->edgectrl.upper > priv->info->edgectrl.lower) {
+		/*
+		 * Auto Edge Control Mode
+		 *
+		 * set upper and lower limit
+		 */
+		ret = ov772x_mask_set(client,
+				      EDGE_UPPER, OV772X_EDGE_UPPER_MASK,
+				      priv->info->edgectrl.upper);
+		if (ret < 0)
+			goto ov772x_set_fmt_error;
+
+		ret = ov772x_mask_set(client,
+				      EDGE_LOWER, OV772X_EDGE_LOWER_MASK,
+				      priv->info->edgectrl.lower);
+		if (ret < 0)
+			goto ov772x_set_fmt_error;
+	}
+
+	/* Format and window size */
+	ret = ov772x_write(client, HSTART, win->rect.left >> 2);
+	if (ret < 0)
+		goto ov772x_set_fmt_error;
+	ret = ov772x_write(client, HSIZE, win->rect.width >> 2);
+	if (ret < 0)
+		goto ov772x_set_fmt_error;
+	ret = ov772x_write(client, VSTART, win->rect.top >> 1);
+	if (ret < 0)
+		goto ov772x_set_fmt_error;
+	ret = ov772x_write(client, VSIZE, win->rect.height >> 1);
+	if (ret < 0)
+		goto ov772x_set_fmt_error;
+	ret = ov772x_write(client, HOUTSIZE, win->rect.width >> 2);
+	if (ret < 0)
+		goto ov772x_set_fmt_error;
+	ret = ov772x_write(client, VOUTSIZE, win->rect.height >> 1);
+	if (ret < 0)
+		goto ov772x_set_fmt_error;
+	ret = ov772x_write(client, 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 = ov772x_write(client, 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 = ov772x_mask_set(client,
+				      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 = ov772x_write(client, DSP_CTRL4, cfmt->dsp4);
+		if (ret < 0)
+			goto ov772x_set_fmt_error;
+	}
+
+	/*
+	 * set COM3
+	 */
+	val = cfmt->com3;
+	if (priv->info->flags & OV772X_FLAG_VFLIP)
+		val |= VFLIP_IMG;
+	if (priv->info->flags & OV772X_FLAG_HFLIP)
+		val |= HFLIP_IMG;
+	if (priv->flag_vflip)
+		val ^= VFLIP_IMG;
+	if (priv->flag_hflip)
+		val ^= HFLIP_IMG;
+
+	ret = ov772x_mask_set(client,
+			      COM3, SWAP_MASK | IMG_MASK, val);
+	if (ret < 0)
+		goto ov772x_set_fmt_error;
+
+	/* COM7: Sensor resolution and output format control. */
+	ret = ov772x_write(client, COM7, win->com7_bit | cfmt->com7);
+	if (ret < 0)
+		goto ov772x_set_fmt_error;
+
+	/*
+	 * set COM8
+	 */
+	if (priv->band_filter) {
+		ret = ov772x_mask_set(client, COM8, BNDF_ON_OFF, 1);
+		if (!ret)
+			ret = ov772x_mask_set(client, BDBASE,
+					      0xff, 256 - priv->band_filter);
+		if (ret < 0)
+			goto ov772x_set_fmt_error;
+	}
+
+	return ret;
+
+ov772x_set_fmt_error:
+
+	ov772x_reset(client);
+
+	return ret;
+}
+
+static int ov772x_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+{
+	a->c.left	= 0;
+	a->c.top	= 0;
+	a->c.width	= VGA_WIDTH;
+	a->c.height	= VGA_HEIGHT;
+	a->type		= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	return 0;
+}
+
+static int ov772x_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
+{
+	a->bounds.left			= 0;
+	a->bounds.top			= 0;
+	a->bounds.width			= OV772X_MAX_WIDTH;
+	a->bounds.height		= OV772X_MAX_HEIGHT;
+	a->defrect			= a->bounds;
+	a->type				= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	a->pixelaspect.numerator	= 1;
+	a->pixelaspect.denominator	= 1;
+
+	return 0;
+}
+
+static int ov772x_g_fmt(struct v4l2_subdev *sd,
+			struct v4l2_mbus_framefmt *mf)
+{
+	struct ov772x_priv *priv = to_ov772x(sd);
+
+	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_s_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *mf)
+{
+	struct ov772x_priv *priv = to_ov772x(sd);
+	const struct ov772x_color_format *cfmt;
+	const struct ov772x_win_size *win;
+	int ret;
+
+	ov772x_select_params(mf, &cfmt, &win);
+
+	ret = ov772x_set_params(priv, cfmt, win);
+	if (ret < 0)
+		return ret;
+
+	priv->win = win;
+	priv->cfmt = cfmt;
+
+	mf->code = cfmt->code;
+	mf->width = win->rect.width;
+	mf->height = win->rect.height;
+	mf->field = V4L2_FIELD_NONE;
+	mf->colorspace = cfmt->colorspace;
+
+	return 0;
+}
+
+static int ov772x_try_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_mbus_framefmt *mf)
+{
+	const struct ov772x_color_format *cfmt;
+	const struct ov772x_win_size *win;
+
+	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;
+
+	return 0;
+}
+
+static int ov772x_video_probe(struct ov772x_priv *priv)
+{
+	struct i2c_client  *client = v4l2_get_subdevdata(&priv->subdev);
+	u8                  pid, ver;
+	const char         *devname;
+	int		    ret;
+
+	ret = ov772x_s_power(&priv->subdev, 1);
+	if (ret < 0)
+		return ret;
+
+	/*
+	 * check and show product ID and manufacturer ID
+	 */
+	pid = ov772x_read(client, PID);
+	ver = ov772x_read(client, VER);
+
+	switch (VERSION(pid, ver)) {
+	case OV7720:
+		devname     = "ov7720";
+		priv->model = V4L2_IDENT_OV7720;
+		break;
+	case OV7725:
+		devname     = "ov7725";
+		priv->model = V4L2_IDENT_OV7725;
+		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,
+		 ov772x_read(client, MIDH),
+		 ov772x_read(client, MIDL));
+	ret = v4l2_ctrl_handler_setup(&priv->hdl);
+
+done:
+	ov772x_s_power(&priv->subdev, 0);
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ov772x_ctrl_ops = {
+	.s_ctrl = ov772x_s_ctrl,
+};
+
+static struct v4l2_subdev_core_ops ov772x_subdev_core_ops = {
+	.g_chip_ident	= ov772x_g_chip_ident,
+#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_fmt(struct v4l2_subdev *sd, unsigned int index,
+			   enum v4l2_mbus_pixelcode *code)
+{
+	if (index >= ARRAY_SIZE(ov772x_cfmts))
+		return -EINVAL;
+
+	*code = ov772x_cfmts[index].code;
+	return 0;
+}
+
+static int ov772x_g_mbus_config(struct v4l2_subdev *sd,
+				struct v4l2_mbus_config *cfg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+	cfg->flags = V4L2_MBUS_PCLK_SAMPLE_RISING | V4L2_MBUS_MASTER |
+		V4L2_MBUS_VSYNC_ACTIVE_HIGH | V4L2_MBUS_HSYNC_ACTIVE_HIGH |
+		V4L2_MBUS_DATA_ACTIVE_HIGH;
+	cfg->type = V4L2_MBUS_PARALLEL;
+	cfg->flags = soc_camera_apply_board_flags(icl, cfg);
+
+	return 0;
+}
+
+static struct v4l2_subdev_video_ops ov772x_subdev_video_ops = {
+	.s_stream	= ov772x_s_stream,
+	.g_mbus_fmt	= ov772x_g_fmt,
+	.s_mbus_fmt	= ov772x_s_fmt,
+	.try_mbus_fmt	= ov772x_try_fmt,
+	.cropcap	= ov772x_cropcap,
+	.g_crop		= ov772x_g_crop,
+	.enum_mbus_fmt	= ov772x_enum_fmt,
+	.g_mbus_config	= ov772x_g_mbus_config,
+};
+
+static struct v4l2_subdev_ops ov772x_subdev_ops = {
+	.core	= &ov772x_subdev_core_ops,
+	.video	= &ov772x_subdev_video_ops,
+};
+
+/*
+ * i2c_driver function
+ */
+
+static int ov772x_probe(struct i2c_client *client,
+			const struct i2c_device_id *did)
+{
+	struct ov772x_priv	*priv;
+	struct soc_camera_link	*icl = soc_camera_i2c_to_link(client);
+	struct i2c_adapter	*adapter = to_i2c_adapter(client->dev.parent);
+	int			ret;
+
+	if (!icl || !icl->priv) {
+		dev_err(&client->dev, "OV772X: missing platform data!\n");
+		return -EINVAL;
+	}
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA)) {
+		dev_err(&adapter->dev,
+			"I2C-Adapter doesn't support "
+			"I2C_FUNC_SMBUS_BYTE_DATA\n");
+		return -EIO;
+	}
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->info = icl->priv;
+
+	v4l2_i2c_subdev_init(&priv->subdev, client, &ov772x_subdev_ops);
+	v4l2_ctrl_handler_init(&priv->hdl, 3);
+	v4l2_ctrl_new_std(&priv->hdl, &ov772x_ctrl_ops,
+			V4L2_CID_VFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&priv->hdl, &ov772x_ctrl_ops,
+			V4L2_CID_HFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&priv->hdl, &ov772x_ctrl_ops,
+			V4L2_CID_BAND_STOP_FILTER, 0, 256, 1, 0);
+	priv->subdev.ctrl_handler = &priv->hdl;
+	if (priv->hdl.error) {
+		ret = priv->hdl.error;
+		goto done;
+	}
+
+	ret = ov772x_video_probe(priv);
+	if (ret < 0)
+		goto done;
+
+	priv->cfmt = &ov772x_cfmts[0];
+	priv->win = &ov772x_win_sizes[0];
+
+done:
+	if (ret) {
+		v4l2_ctrl_handler_free(&priv->hdl);
+		kfree(priv);
+	}
+	return ret;
+}
+
+static int ov772x_remove(struct i2c_client *client)
+{
+	struct ov772x_priv *priv = to_ov772x(i2c_get_clientdata(client));
+
+	v4l2_device_unregister_subdev(&priv->subdev);
+	v4l2_ctrl_handler_free(&priv->hdl);
+	kfree(priv);
+	return 0;
+}
+
+static const struct i2c_device_id ov772x_id[] = {
+	{ "ov772x", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ov772x_id);
+
+static struct i2c_driver ov772x_i2c_driver = {
+	.driver = {
+		.name = "ov772x",
+	},
+	.probe    = ov772x_probe,
+	.remove   = ov772x_remove,
+	.id_table = ov772x_id,
+};
+
+module_i2c_driver(ov772x_i2c_driver);
+
+MODULE_DESCRIPTION("SoC Camera driver for ov772x");
+MODULE_AUTHOR("Kuninori Morimoto");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/soc_camera/ov9640.c b/drivers/media/i2c/soc_camera/ov9640.c
new file mode 100644
index 000000000000..b323684eaf77
--- /dev/null
+++ b/drivers/media/i2c/soc_camera/ov9640.c
@@ -0,0 +1,763 @@
+/*
+ * 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>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#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/soc_camera.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ctrls.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 enum v4l2_mbus_pixelcode ov9640_codes[] = {
+	V4L2_MBUS_FMT_UYVY8_2X8,
+	V4L2_MBUS_FMT_RGB555_2X8_PADHI_LE,
+	V4L2_MBUS_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 val;
+	}
+
+	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;
+}
+
+/* Get chip identification */
+static int ov9640_g_chip_ident(struct v4l2_subdev *sd,
+				struct v4l2_dbg_chip_ident *id)
+{
+	struct ov9640_priv *priv = to_ov9640_sensor(sd);
+
+	id->ident	= priv->model;
+	id->revision	= priv->revision;
+
+	return 0;
+}
+
+#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,
+				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 i2c_client *client = v4l2_get_subdevdata(sd);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+	return soc_camera_set_power(&client->dev, icl, on);
+}
+
+/* select nearest higher resolution for capture */
+static void ov9640_res_roundup(u32 *width, u32 *height)
+{
+	int i;
+	enum { QQCIF, QQVGA, QCIF, QVGA, CIF, VGA, SXGA };
+	int res_x[] = { 88, 160, 176, 320, 352, 640, 1280 };
+	int 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(enum v4l2_mbus_pixelcode code,
+			      struct ov9640_reg_alt *alt)
+{
+	switch (code) {
+	default:
+	case V4L2_MBUS_FMT_UYVY8_2X8:
+		alt->com12	= OV9640_COM12_YUV_AVG;
+		alt->com13	= OV9640_COM13_Y_DELAY_EN |
+					OV9640_COM13_YUV_DLY(0x01);
+		break;
+	case V4L2_MBUS_FMT_RGB555_2X8_PADHI_LE:
+		alt->com7	= OV9640_COM7_RGB;
+		alt->com13	= OV9640_COM13_RGB_AVG;
+		alt->com15	= OV9640_COM15_RGB_555;
+		break;
+	case V4L2_MBUS_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,
+		enum v4l2_mbus_pixelcode code, struct ov9640_reg_alt *alts)
+{
+	const struct ov9640_reg	*ov9640_regs, *matrix_regs;
+	int			ov9640_regs_len, matrix_regs_len;
+	int			i, 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 == V4L2_MBUS_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)
+{
+	int i, 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 */
+	mdelay(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};
+	enum v4l2_colorspace cspace;
+	enum v4l2_mbus_pixelcode code = mf->code;
+	int ret;
+
+	ov9640_res_roundup(&mf->width, &mf->height);
+	ov9640_alter_regs(mf->code, &alts);
+
+	ov9640_reset(client);
+
+	ret = ov9640_prog_dflt(client);
+	if (ret)
+		return ret;
+
+	switch (code) {
+	case V4L2_MBUS_FMT_RGB555_2X8_PADHI_LE:
+	case V4L2_MBUS_FMT_RGB565_2X8_LE:
+		cspace = V4L2_COLORSPACE_SRGB;
+		break;
+	default:
+		code = V4L2_MBUS_FMT_UYVY8_2X8;
+	case V4L2_MBUS_FMT_UYVY8_2X8:
+		cspace = V4L2_COLORSPACE_JPEG;
+	}
+
+	ret = ov9640_write_regs(client, mf->width, code, &alts);
+	if (!ret) {
+		mf->code	= code;
+		mf->colorspace	= cspace;
+	}
+
+	return ret;
+}
+
+static int ov9640_try_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_mbus_framefmt *mf)
+{
+	ov9640_res_roundup(&mf->width, &mf->height);
+
+	mf->field = V4L2_FIELD_NONE;
+
+	switch (mf->code) {
+	case V4L2_MBUS_FMT_RGB555_2X8_PADHI_LE:
+	case V4L2_MBUS_FMT_RGB565_2X8_LE:
+		mf->colorspace = V4L2_COLORSPACE_SRGB;
+		break;
+	default:
+		mf->code = V4L2_MBUS_FMT_UYVY8_2X8;
+	case V4L2_MBUS_FMT_UYVY8_2X8:
+		mf->colorspace = V4L2_COLORSPACE_JPEG;
+	}
+
+	return 0;
+}
+
+static int ov9640_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
+			   enum v4l2_mbus_pixelcode *code)
+{
+	if (index >= ARRAY_SIZE(ov9640_codes))
+		return -EINVAL;
+
+	*code = ov9640_codes[index];
+	return 0;
+}
+
+static int ov9640_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+{
+	a->c.left	= 0;
+	a->c.top	= 0;
+	a->c.width	= W_SXGA;
+	a->c.height	= H_SXGA;
+	a->type		= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	return 0;
+}
+
+static int ov9640_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
+{
+	a->bounds.left			= 0;
+	a->bounds.top			= 0;
+	a->bounds.width			= W_SXGA;
+	a->bounds.height		= H_SXGA;
+	a->defrect			= a->bounds;
+	a->type				= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	a->pixelaspect.numerator	= 1;
+	a->pixelaspect.denominator	= 1;
+
+	return 0;
+}
+
+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->model	= V4L2_IDENT_OV9640;
+		priv->revision	= 2;
+		break;
+	case OV9640_V3:
+		devname		= "ov9640";
+		priv->model	= V4L2_IDENT_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 struct v4l2_subdev_core_ops ov9640_core_ops = {
+	.g_chip_ident		= ov9640_g_chip_ident,
+#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_g_mbus_config(struct v4l2_subdev *sd,
+				struct v4l2_mbus_config *cfg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+	cfg->flags = V4L2_MBUS_PCLK_SAMPLE_RISING | V4L2_MBUS_MASTER |
+		V4L2_MBUS_VSYNC_ACTIVE_HIGH | V4L2_MBUS_HSYNC_ACTIVE_HIGH |
+		V4L2_MBUS_DATA_ACTIVE_HIGH;
+	cfg->type = V4L2_MBUS_PARALLEL;
+	cfg->flags = soc_camera_apply_board_flags(icl, cfg);
+
+	return 0;
+}
+
+static struct v4l2_subdev_video_ops ov9640_video_ops = {
+	.s_stream	= ov9640_s_stream,
+	.s_mbus_fmt	= ov9640_s_fmt,
+	.try_mbus_fmt	= ov9640_try_fmt,
+	.enum_mbus_fmt	= ov9640_enum_fmt,
+	.cropcap	= ov9640_cropcap,
+	.g_crop		= ov9640_g_crop,
+	.g_mbus_config	= ov9640_g_mbus_config,
+};
+
+static struct v4l2_subdev_ops ov9640_subdev_ops = {
+	.core	= &ov9640_core_ops,
+	.video	= &ov9640_video_ops,
+};
+
+/*
+ * i2c_driver function
+ */
+static int ov9640_probe(struct i2c_client *client,
+			const struct i2c_device_id *did)
+{
+	struct ov9640_priv *priv;
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+	int ret;
+
+	if (!icl) {
+		dev_err(&client->dev, "Missing platform_data for driver\n");
+		return -EINVAL;
+	}
+
+	priv = kzalloc(sizeof(struct ov9640_priv), GFP_KERNEL);
+	if (!priv) {
+		dev_err(&client->dev,
+			"Failed to allocate memory for private data!\n");
+		return -ENOMEM;
+	}
+
+	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);
+	priv->subdev.ctrl_handler = &priv->hdl;
+	if (priv->hdl.error) {
+		int err = priv->hdl.error;
+
+		kfree(priv);
+		return err;
+	}
+
+	ret = ov9640_video_probe(client);
+
+	if (ret) {
+		v4l2_ctrl_handler_free(&priv->hdl);
+		kfree(priv);
+	}
+
+	return ret;
+}
+
+static int ov9640_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov9640_priv *priv = to_ov9640_sensor(sd);
+
+	v4l2_device_unregister_subdev(&priv->subdev);
+	v4l2_ctrl_handler_free(&priv->hdl);
+	kfree(priv);
+	return 0;
+}
+
+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("SoC Camera driver for OmniVision OV96xx");
+MODULE_AUTHOR("Marek Vasut <marek.vasut@gmail.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/soc_camera/ov9640.h b/drivers/media/i2c/soc_camera/ov9640.h
new file mode 100644
index 000000000000..6b33a972c83c
--- /dev/null
+++ b/drivers/media/i2c/soc_camera/ov9640.h
@@ -0,0 +1,207 @@
+/*
+ * OmniVision OV96xx Camera Header File
+ *
+ * Copyright (C) 2009 Marek Vasut <marek.vasut@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#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;
+
+	int				model;
+	int				revision;
+};
+
+#endif	/* __DRIVERS_MEDIA_VIDEO_OV9640_H__ */
diff --git a/drivers/media/i2c/soc_camera/ov9740.c b/drivers/media/i2c/soc_camera/ov9740.c
new file mode 100644
index 000000000000..7a55889e397b
--- /dev/null
+++ b/drivers/media/i2c/soc_camera/ov9740.c
@@ -0,0 +1,1020 @@
+/*
+ * OmniVision OV9740 Camera Driver
+ *
+ * Copyright (C) 2011 NVIDIA Corporation
+ *
+ * Based on ov9640 camera driver.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/v4l2-mediabus.h>
+
+#include <media/soc_camera.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-ctrls.h>
+
+#define to_ov9740(sd)		container_of(sd, struct ov9740_priv, subdev)
+
+/* General Status Registers */
+#define OV9740_MODEL_ID_HI		0x0000
+#define OV9740_MODEL_ID_LO		0x0001
+#define OV9740_REVISION_NUMBER		0x0002
+#define OV9740_MANUFACTURER_ID		0x0003
+#define OV9740_SMIA_VERSION		0x0004
+
+/* General Setup Registers */
+#define OV9740_MODE_SELECT		0x0100
+#define OV9740_IMAGE_ORT		0x0101
+#define OV9740_SOFTWARE_RESET		0x0103
+#define OV9740_GRP_PARAM_HOLD		0x0104
+#define OV9740_MSK_CORRUP_FM		0x0105
+
+/* Timing Setting */
+#define OV9740_FRM_LENGTH_LN_HI		0x0340 /* VTS */
+#define OV9740_FRM_LENGTH_LN_LO		0x0341 /* VTS */
+#define OV9740_LN_LENGTH_PCK_HI		0x0342 /* HTS */
+#define OV9740_LN_LENGTH_PCK_LO		0x0343 /* HTS */
+#define OV9740_X_ADDR_START_HI		0x0344
+#define OV9740_X_ADDR_START_LO		0x0345
+#define OV9740_Y_ADDR_START_HI		0x0346
+#define OV9740_Y_ADDR_START_LO		0x0347
+#define OV9740_X_ADDR_END_HI		0x0348
+#define OV9740_X_ADDR_END_LO		0x0349
+#define OV9740_Y_ADDR_END_HI		0x034a
+#define OV9740_Y_ADDR_END_LO		0x034b
+#define OV9740_X_OUTPUT_SIZE_HI		0x034c
+#define OV9740_X_OUTPUT_SIZE_LO		0x034d
+#define OV9740_Y_OUTPUT_SIZE_HI		0x034e
+#define OV9740_Y_OUTPUT_SIZE_LO		0x034f
+
+/* IO Control Registers */
+#define OV9740_IO_CREL00		0x3002
+#define OV9740_IO_CREL01		0x3004
+#define OV9740_IO_CREL02		0x3005
+#define OV9740_IO_OUTPUT_SEL01		0x3026
+#define OV9740_IO_OUTPUT_SEL02		0x3027
+
+/* AWB Registers */
+#define OV9740_AWB_MANUAL_CTRL		0x3406
+
+/* Analog Control Registers */
+#define OV9740_ANALOG_CTRL01		0x3601
+#define OV9740_ANALOG_CTRL02		0x3602
+#define OV9740_ANALOG_CTRL03		0x3603
+#define OV9740_ANALOG_CTRL04		0x3604
+#define OV9740_ANALOG_CTRL10		0x3610
+#define OV9740_ANALOG_CTRL12		0x3612
+#define OV9740_ANALOG_CTRL15		0x3615
+#define OV9740_ANALOG_CTRL20		0x3620
+#define OV9740_ANALOG_CTRL21		0x3621
+#define OV9740_ANALOG_CTRL22		0x3622
+#define OV9740_ANALOG_CTRL30		0x3630
+#define OV9740_ANALOG_CTRL31		0x3631
+#define OV9740_ANALOG_CTRL32		0x3632
+#define OV9740_ANALOG_CTRL33		0x3633
+
+/* Sensor Control */
+#define OV9740_SENSOR_CTRL03		0x3703
+#define OV9740_SENSOR_CTRL04		0x3704
+#define OV9740_SENSOR_CTRL05		0x3705
+#define OV9740_SENSOR_CTRL07		0x3707
+
+/* Timing Control */
+#define OV9740_TIMING_CTRL17		0x3817
+#define OV9740_TIMING_CTRL19		0x3819
+#define OV9740_TIMING_CTRL33		0x3833
+#define OV9740_TIMING_CTRL35		0x3835
+
+/* Banding Filter */
+#define OV9740_AEC_MAXEXPO_60_H		0x3a02
+#define OV9740_AEC_MAXEXPO_60_L		0x3a03
+#define OV9740_AEC_B50_STEP_HI		0x3a08
+#define OV9740_AEC_B50_STEP_LO		0x3a09
+#define OV9740_AEC_B60_STEP_HI		0x3a0a
+#define OV9740_AEC_B60_STEP_LO		0x3a0b
+#define OV9740_AEC_CTRL0D		0x3a0d
+#define OV9740_AEC_CTRL0E		0x3a0e
+#define OV9740_AEC_MAXEXPO_50_H		0x3a14
+#define OV9740_AEC_MAXEXPO_50_L		0x3a15
+
+/* AEC/AGC Control */
+#define OV9740_AEC_ENABLE		0x3503
+#define OV9740_GAIN_CEILING_01		0x3a18
+#define OV9740_GAIN_CEILING_02		0x3a19
+#define OV9740_AEC_HI_THRESHOLD		0x3a11
+#define OV9740_AEC_3A1A			0x3a1a
+#define OV9740_AEC_CTRL1B_WPT2		0x3a1b
+#define OV9740_AEC_CTRL0F_WPT		0x3a0f
+#define OV9740_AEC_CTRL10_BPT		0x3a10
+#define OV9740_AEC_CTRL1E_BPT2		0x3a1e
+#define OV9740_AEC_LO_THRESHOLD		0x3a1f
+
+/* BLC Control */
+#define OV9740_BLC_AUTO_ENABLE		0x4002
+#define OV9740_BLC_MODE			0x4005
+
+/* VFIFO */
+#define OV9740_VFIFO_READ_START_HI	0x4608
+#define OV9740_VFIFO_READ_START_LO	0x4609
+
+/* DVP Control */
+#define OV9740_DVP_VSYNC_CTRL02		0x4702
+#define OV9740_DVP_VSYNC_MODE		0x4704
+#define OV9740_DVP_VSYNC_CTRL06		0x4706
+
+/* PLL Setting */
+#define OV9740_PLL_MODE_CTRL01		0x3104
+#define OV9740_PRE_PLL_CLK_DIV		0x0305
+#define OV9740_PLL_MULTIPLIER		0x0307
+#define OV9740_VT_SYS_CLK_DIV		0x0303
+#define OV9740_VT_PIX_CLK_DIV		0x0301
+#define OV9740_PLL_CTRL3010		0x3010
+#define OV9740_VFIFO_CTRL00		0x460e
+
+/* ISP Control */
+#define OV9740_ISP_CTRL00		0x5000
+#define OV9740_ISP_CTRL01		0x5001
+#define OV9740_ISP_CTRL03		0x5003
+#define OV9740_ISP_CTRL05		0x5005
+#define OV9740_ISP_CTRL12		0x5012
+#define OV9740_ISP_CTRL19		0x5019
+#define OV9740_ISP_CTRL1A		0x501a
+#define OV9740_ISP_CTRL1E		0x501e
+#define OV9740_ISP_CTRL1F		0x501f
+#define OV9740_ISP_CTRL20		0x5020
+#define OV9740_ISP_CTRL21		0x5021
+
+/* AWB */
+#define OV9740_AWB_CTRL00		0x5180
+#define OV9740_AWB_CTRL01		0x5181
+#define OV9740_AWB_CTRL02		0x5182
+#define OV9740_AWB_CTRL03		0x5183
+#define OV9740_AWB_ADV_CTRL01		0x5184
+#define OV9740_AWB_ADV_CTRL02		0x5185
+#define OV9740_AWB_ADV_CTRL03		0x5186
+#define OV9740_AWB_ADV_CTRL04		0x5187
+#define OV9740_AWB_ADV_CTRL05		0x5188
+#define OV9740_AWB_ADV_CTRL06		0x5189
+#define OV9740_AWB_ADV_CTRL07		0x518a
+#define OV9740_AWB_ADV_CTRL08		0x518b
+#define OV9740_AWB_ADV_CTRL09		0x518c
+#define OV9740_AWB_ADV_CTRL10		0x518d
+#define OV9740_AWB_ADV_CTRL11		0x518e
+#define OV9740_AWB_CTRL0F		0x518f
+#define OV9740_AWB_CTRL10		0x5190
+#define OV9740_AWB_CTRL11		0x5191
+#define OV9740_AWB_CTRL12		0x5192
+#define OV9740_AWB_CTRL13		0x5193
+#define OV9740_AWB_CTRL14		0x5194
+
+/* MIPI Control */
+#define OV9740_MIPI_CTRL00		0x4800
+#define OV9740_MIPI_3837		0x3837
+#define OV9740_MIPI_CTRL01		0x4801
+#define OV9740_MIPI_CTRL03		0x4803
+#define OV9740_MIPI_CTRL05		0x4805
+#define OV9740_VFIFO_RD_CTRL		0x4601
+#define OV9740_MIPI_CTRL_3012		0x3012
+#define OV9740_SC_CMMM_MIPI_CTR		0x3014
+
+#define OV9740_MAX_WIDTH		1280
+#define OV9740_MAX_HEIGHT		720
+
+/* Misc. structures */
+struct ov9740_reg {
+	u16				reg;
+	u8				val;
+};
+
+struct ov9740_priv {
+	struct v4l2_subdev		subdev;
+	struct v4l2_ctrl_handler	hdl;
+
+	int				ident;
+	u16				model;
+	u8				revision;
+	u8				manid;
+	u8				smiaver;
+
+	bool				flag_vflip;
+	bool				flag_hflip;
+
+	/* For suspend/resume. */
+	struct v4l2_mbus_framefmt	current_mf;
+	bool				current_enable;
+};
+
+static const struct ov9740_reg ov9740_defaults[] = {
+	/* Software Reset */
+	{ OV9740_SOFTWARE_RESET,	0x01 },
+
+	/* Banding Filter */
+	{ OV9740_AEC_B50_STEP_HI,	0x00 },
+	{ OV9740_AEC_B50_STEP_LO,	0xe8 },
+	{ OV9740_AEC_CTRL0E,		0x03 },
+	{ OV9740_AEC_MAXEXPO_50_H,	0x15 },
+	{ OV9740_AEC_MAXEXPO_50_L,	0xc6 },
+	{ OV9740_AEC_B60_STEP_HI,	0x00 },
+	{ OV9740_AEC_B60_STEP_LO,	0xc0 },
+	{ OV9740_AEC_CTRL0D,		0x04 },
+	{ OV9740_AEC_MAXEXPO_60_H,	0x18 },
+	{ OV9740_AEC_MAXEXPO_60_L,	0x20 },
+
+	/* LC */
+	{ 0x5842, 0x02 }, { 0x5843, 0x5e }, { 0x5844, 0x04 }, { 0x5845, 0x32 },
+	{ 0x5846, 0x03 }, { 0x5847, 0x29 }, { 0x5848, 0x02 }, { 0x5849, 0xcc },
+
+	/* Un-documented OV9740 registers */
+	{ 0x5800, 0x29 }, { 0x5801, 0x25 }, { 0x5802, 0x20 }, { 0x5803, 0x21 },
+	{ 0x5804, 0x26 }, { 0x5805, 0x2e }, { 0x5806, 0x11 }, { 0x5807, 0x0c },
+	{ 0x5808, 0x09 }, { 0x5809, 0x0a }, { 0x580a, 0x0e }, { 0x580b, 0x16 },
+	{ 0x580c, 0x06 }, { 0x580d, 0x02 }, { 0x580e, 0x00 }, { 0x580f, 0x00 },
+	{ 0x5810, 0x04 }, { 0x5811, 0x0a }, { 0x5812, 0x05 }, { 0x5813, 0x02 },
+	{ 0x5814, 0x00 }, { 0x5815, 0x00 }, { 0x5816, 0x03 }, { 0x5817, 0x09 },
+	{ 0x5818, 0x0f }, { 0x5819, 0x0a }, { 0x581a, 0x07 }, { 0x581b, 0x08 },
+	{ 0x581c, 0x0b }, { 0x581d, 0x14 }, { 0x581e, 0x28 }, { 0x581f, 0x23 },
+	{ 0x5820, 0x1d }, { 0x5821, 0x1e }, { 0x5822, 0x24 }, { 0x5823, 0x2a },
+	{ 0x5824, 0x4f }, { 0x5825, 0x6f }, { 0x5826, 0x5f }, { 0x5827, 0x7f },
+	{ 0x5828, 0x9f }, { 0x5829, 0x5f }, { 0x582a, 0x8f }, { 0x582b, 0x9e },
+	{ 0x582c, 0x8f }, { 0x582d, 0x9f }, { 0x582e, 0x4f }, { 0x582f, 0x87 },
+	{ 0x5830, 0x86 }, { 0x5831, 0x97 }, { 0x5832, 0xae }, { 0x5833, 0x3f },
+	{ 0x5834, 0x8e }, { 0x5835, 0x7c }, { 0x5836, 0x7e }, { 0x5837, 0xaf },
+	{ 0x5838, 0x8f }, { 0x5839, 0x8f }, { 0x583a, 0x9f }, { 0x583b, 0x7f },
+	{ 0x583c, 0x5f },
+
+	/* Y Gamma */
+	{ 0x5480, 0x07 }, { 0x5481, 0x18 }, { 0x5482, 0x2c }, { 0x5483, 0x4e },
+	{ 0x5484, 0x5e }, { 0x5485, 0x6b }, { 0x5486, 0x77 }, { 0x5487, 0x82 },
+	{ 0x5488, 0x8c }, { 0x5489, 0x95 }, { 0x548a, 0xa4 }, { 0x548b, 0xb1 },
+	{ 0x548c, 0xc6 }, { 0x548d, 0xd8 }, { 0x548e, 0xe9 },
+
+	/* UV Gamma */
+	{ 0x5490, 0x0f }, { 0x5491, 0xff }, { 0x5492, 0x0d }, { 0x5493, 0x05 },
+	{ 0x5494, 0x07 }, { 0x5495, 0x1a }, { 0x5496, 0x04 }, { 0x5497, 0x01 },
+	{ 0x5498, 0x03 }, { 0x5499, 0x53 }, { 0x549a, 0x02 }, { 0x549b, 0xeb },
+	{ 0x549c, 0x02 }, { 0x549d, 0xa0 }, { 0x549e, 0x02 }, { 0x549f, 0x67 },
+	{ 0x54a0, 0x02 }, { 0x54a1, 0x3b }, { 0x54a2, 0x02 }, { 0x54a3, 0x18 },
+	{ 0x54a4, 0x01 }, { 0x54a5, 0xe7 }, { 0x54a6, 0x01 }, { 0x54a7, 0xc3 },
+	{ 0x54a8, 0x01 }, { 0x54a9, 0x94 }, { 0x54aa, 0x01 }, { 0x54ab, 0x72 },
+	{ 0x54ac, 0x01 }, { 0x54ad, 0x57 },
+
+	/* AWB */
+	{ OV9740_AWB_CTRL00,		0xf0 },
+	{ OV9740_AWB_CTRL01,		0x00 },
+	{ OV9740_AWB_CTRL02,		0x41 },
+	{ OV9740_AWB_CTRL03,		0x42 },
+	{ OV9740_AWB_ADV_CTRL01,	0x8a },
+	{ OV9740_AWB_ADV_CTRL02,	0x61 },
+	{ OV9740_AWB_ADV_CTRL03,	0xce },
+	{ OV9740_AWB_ADV_CTRL04,	0xa8 },
+	{ OV9740_AWB_ADV_CTRL05,	0x17 },
+	{ OV9740_AWB_ADV_CTRL06,	0x1f },
+	{ OV9740_AWB_ADV_CTRL07,	0x27 },
+	{ OV9740_AWB_ADV_CTRL08,	0x41 },
+	{ OV9740_AWB_ADV_CTRL09,	0x34 },
+	{ OV9740_AWB_ADV_CTRL10,	0xf0 },
+	{ OV9740_AWB_ADV_CTRL11,	0x10 },
+	{ OV9740_AWB_CTRL0F,		0xff },
+	{ OV9740_AWB_CTRL10,		0x00 },
+	{ OV9740_AWB_CTRL11,		0xff },
+	{ OV9740_AWB_CTRL12,		0x00 },
+	{ OV9740_AWB_CTRL13,		0xff },
+	{ OV9740_AWB_CTRL14,		0x00 },
+
+	/* CIP */
+	{ 0x530d, 0x12 },
+
+	/* CMX */
+	{ 0x5380, 0x01 }, { 0x5381, 0x00 }, { 0x5382, 0x00 }, { 0x5383, 0x17 },
+	{ 0x5384, 0x00 }, { 0x5385, 0x01 }, { 0x5386, 0x00 }, { 0x5387, 0x00 },
+	{ 0x5388, 0x00 }, { 0x5389, 0xe0 }, { 0x538a, 0x00 }, { 0x538b, 0x20 },
+	{ 0x538c, 0x00 }, { 0x538d, 0x00 }, { 0x538e, 0x00 }, { 0x538f, 0x16 },
+	{ 0x5390, 0x00 }, { 0x5391, 0x9c }, { 0x5392, 0x00 }, { 0x5393, 0xa0 },
+	{ 0x5394, 0x18 },
+
+	/* 50/60 Detection */
+	{ 0x3c0a, 0x9c }, { 0x3c0b, 0x3f },
+
+	/* Output Select */
+	{ OV9740_IO_OUTPUT_SEL01,	0x00 },
+	{ OV9740_IO_OUTPUT_SEL02,	0x00 },
+	{ OV9740_IO_CREL00,		0x00 },
+	{ OV9740_IO_CREL01,		0x00 },
+	{ OV9740_IO_CREL02,		0x00 },
+
+	/* AWB Control */
+	{ OV9740_AWB_MANUAL_CTRL,	0x00 },
+
+	/* Analog Control */
+	{ OV9740_ANALOG_CTRL03,		0xaa },
+	{ OV9740_ANALOG_CTRL32,		0x2f },
+	{ OV9740_ANALOG_CTRL20,		0x66 },
+	{ OV9740_ANALOG_CTRL21,		0xc0 },
+	{ OV9740_ANALOG_CTRL31,		0x52 },
+	{ OV9740_ANALOG_CTRL33,		0x50 },
+	{ OV9740_ANALOG_CTRL30,		0xca },
+	{ OV9740_ANALOG_CTRL04,		0x0c },
+	{ OV9740_ANALOG_CTRL01,		0x40 },
+	{ OV9740_ANALOG_CTRL02,		0x16 },
+	{ OV9740_ANALOG_CTRL10,		0xa1 },
+	{ OV9740_ANALOG_CTRL12,		0x24 },
+	{ OV9740_ANALOG_CTRL22,		0x9f },
+	{ OV9740_ANALOG_CTRL15,		0xf0 },
+
+	/* Sensor Control */
+	{ OV9740_SENSOR_CTRL03,		0x42 },
+	{ OV9740_SENSOR_CTRL04,		0x10 },
+	{ OV9740_SENSOR_CTRL05,		0x45 },
+	{ OV9740_SENSOR_CTRL07,		0x14 },
+
+	/* Timing Control */
+	{ OV9740_TIMING_CTRL33,		0x04 },
+	{ OV9740_TIMING_CTRL35,		0x02 },
+	{ OV9740_TIMING_CTRL19,		0x6e },
+	{ OV9740_TIMING_CTRL17,		0x94 },
+
+	/* AEC/AGC Control */
+	{ OV9740_AEC_ENABLE,		0x10 },
+	{ OV9740_GAIN_CEILING_01,	0x00 },
+	{ OV9740_GAIN_CEILING_02,	0x7f },
+	{ OV9740_AEC_HI_THRESHOLD,	0xa0 },
+	{ OV9740_AEC_3A1A,		0x05 },
+	{ OV9740_AEC_CTRL1B_WPT2,	0x50 },
+	{ OV9740_AEC_CTRL0F_WPT,	0x50 },
+	{ OV9740_AEC_CTRL10_BPT,	0x4c },
+	{ OV9740_AEC_CTRL1E_BPT2,	0x4c },
+	{ OV9740_AEC_LO_THRESHOLD,	0x26 },
+
+	/* BLC Control */
+	{ OV9740_BLC_AUTO_ENABLE,	0x45 },
+	{ OV9740_BLC_MODE,		0x18 },
+
+	/* DVP Control */
+	{ OV9740_DVP_VSYNC_CTRL02,	0x04 },
+	{ OV9740_DVP_VSYNC_MODE,	0x00 },
+	{ OV9740_DVP_VSYNC_CTRL06,	0x08 },
+
+	/* PLL Setting */
+	{ OV9740_PLL_MODE_CTRL01,	0x20 },
+	{ OV9740_PRE_PLL_CLK_DIV,	0x03 },
+	{ OV9740_PLL_MULTIPLIER,	0x4c },
+	{ OV9740_VT_SYS_CLK_DIV,	0x01 },
+	{ OV9740_VT_PIX_CLK_DIV,	0x08 },
+	{ OV9740_PLL_CTRL3010,		0x01 },
+	{ OV9740_VFIFO_CTRL00,		0x82 },
+
+	/* Timing Setting */
+	/* VTS */
+	{ OV9740_FRM_LENGTH_LN_HI,	0x03 },
+	{ OV9740_FRM_LENGTH_LN_LO,	0x07 },
+	/* HTS */
+	{ OV9740_LN_LENGTH_PCK_HI,	0x06 },
+	{ OV9740_LN_LENGTH_PCK_LO,	0x62 },
+
+	/* MIPI Control */
+	{ OV9740_MIPI_CTRL00,		0x44 }, /* 0x64 for discontinuous clk */
+	{ OV9740_MIPI_3837,		0x01 },
+	{ OV9740_MIPI_CTRL01,		0x0f },
+	{ OV9740_MIPI_CTRL03,		0x05 },
+	{ OV9740_MIPI_CTRL05,		0x10 },
+	{ OV9740_VFIFO_RD_CTRL,		0x16 },
+	{ OV9740_MIPI_CTRL_3012,	0x70 },
+	{ OV9740_SC_CMMM_MIPI_CTR,	0x01 },
+
+	/* YUYV order */
+	{ OV9740_ISP_CTRL19,		0x02 },
+};
+
+static enum v4l2_mbus_pixelcode ov9740_codes[] = {
+	V4L2_MBUS_FMT_YUYV8_2X8,
+};
+
+/* read a register */
+static int ov9740_reg_read(struct i2c_client *client, u16 reg, u8 *val)
+{
+	int ret;
+	struct i2c_msg msg[] = {
+		{
+			.addr	= client->addr,
+			.flags	= 0,
+			.len	= 2,
+			.buf	= (u8 *)&reg,
+		},
+		{
+			.addr	= client->addr,
+			.flags	= I2C_M_RD,
+			.len	= 1,
+			.buf	= val,
+		},
+	};
+
+	reg = swab16(reg);
+
+	ret = i2c_transfer(client->adapter, msg, 2);
+	if (ret < 0) {
+		dev_err(&client->dev, "Failed reading register 0x%04x!\n", reg);
+		return ret;
+	}
+
+	return 0;
+}
+
+/* write a register */
+static int ov9740_reg_write(struct i2c_client *client, u16 reg, u8 val)
+{
+	struct i2c_msg msg;
+	struct {
+		u16 reg;
+		u8 val;
+	} __packed buf;
+	int ret;
+
+	reg = swab16(reg);
+
+	buf.reg = reg;
+	buf.val = val;
+
+	msg.addr	= client->addr;
+	msg.flags	= 0;
+	msg.len		= 3;
+	msg.buf		= (u8 *)&buf;
+
+	ret = i2c_transfer(client->adapter, &msg, 1);
+	if (ret < 0) {
+		dev_err(&client->dev, "Failed writing register 0x%04x!\n", reg);
+		return ret;
+	}
+
+	return 0;
+}
+
+
+/* Read a register, alter its bits, write it back */
+static int ov9740_reg_rmw(struct i2c_client *client, u16 reg, u8 set, u8 unset)
+{
+	u8 val;
+	int ret;
+
+	ret = ov9740_reg_read(client, reg, &val);
+	if (ret < 0) {
+		dev_err(&client->dev,
+			"[Read]-Modify-Write of register 0x%04x failed!\n",
+			reg);
+		return ret;
+	}
+
+	val |= set;
+	val &= ~unset;
+
+	ret = ov9740_reg_write(client, reg, val);
+	if (ret < 0) {
+		dev_err(&client->dev,
+			"Read-Modify-[Write] of register 0x%04x failed!\n",
+			reg);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ov9740_reg_write_array(struct i2c_client *client,
+				  const struct ov9740_reg *regarray,
+				  int regarraylen)
+{
+	int i;
+	int ret;
+
+	for (i = 0; i < regarraylen; i++) {
+		ret = ov9740_reg_write(client,
+				       regarray[i].reg, regarray[i].val);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+/* Start/Stop streaming from the device */
+static int ov9740_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov9740_priv *priv = to_ov9740(sd);
+	int ret;
+
+	/* Program orientation register. */
+	if (priv->flag_vflip)
+		ret = ov9740_reg_rmw(client, OV9740_IMAGE_ORT, 0x2, 0);
+	else
+		ret = ov9740_reg_rmw(client, OV9740_IMAGE_ORT, 0, 0x2);
+	if (ret < 0)
+		return ret;
+
+	if (priv->flag_hflip)
+		ret = ov9740_reg_rmw(client, OV9740_IMAGE_ORT, 0x1, 0);
+	else
+		ret = ov9740_reg_rmw(client, OV9740_IMAGE_ORT, 0, 0x1);
+	if (ret < 0)
+		return ret;
+
+	if (enable) {
+		dev_dbg(&client->dev, "Enabling Streaming\n");
+		/* Start Streaming */
+		ret = ov9740_reg_write(client, OV9740_MODE_SELECT, 0x01);
+
+	} else {
+		dev_dbg(&client->dev, "Disabling Streaming\n");
+		/* Software Reset */
+		ret = ov9740_reg_write(client, OV9740_SOFTWARE_RESET, 0x01);
+		if (!ret)
+			/* Setting Streaming to Standby */
+			ret = ov9740_reg_write(client, OV9740_MODE_SELECT,
+					       0x00);
+	}
+
+	priv->current_enable = enable;
+
+	return ret;
+}
+
+/* select nearest higher resolution for capture */
+static void ov9740_res_roundup(u32 *width, u32 *height)
+{
+	/* Width must be a multiple of 4 pixels. */
+	*width = ALIGN(*width, 4);
+
+	/* Max resolution is 1280x720 (720p). */
+	if (*width > OV9740_MAX_WIDTH)
+		*width = OV9740_MAX_WIDTH;
+
+	if (*height > OV9740_MAX_HEIGHT)
+		*height = OV9740_MAX_HEIGHT;
+}
+
+/* Setup registers according to resolution and color encoding */
+static int ov9740_set_res(struct i2c_client *client, u32 width, u32 height)
+{
+	u32 x_start;
+	u32 y_start;
+	u32 x_end;
+	u32 y_end;
+	bool scaling = 0;
+	u32 scale_input_x;
+	u32 scale_input_y;
+	int ret;
+
+	if ((width != OV9740_MAX_WIDTH) || (height != OV9740_MAX_HEIGHT))
+		scaling = 1;
+
+	/*
+	 * Try to use as much of the sensor area as possible when supporting
+	 * smaller resolutions.  Depending on the aspect ratio of the
+	 * chosen resolution, we can either use the full width of the sensor,
+	 * or the full height of the sensor (or both if the aspect ratio is
+	 * the same as 1280x720.
+	 */
+	if ((OV9740_MAX_WIDTH * height) > (OV9740_MAX_HEIGHT * width)) {
+		scale_input_x = (OV9740_MAX_HEIGHT * width) / height;
+		scale_input_y = OV9740_MAX_HEIGHT;
+	} else {
+		scale_input_x = OV9740_MAX_WIDTH;
+		scale_input_y = (OV9740_MAX_WIDTH * height) / width;
+	}
+
+	/* These describe the area of the sensor to use. */
+	x_start = (OV9740_MAX_WIDTH - scale_input_x) / 2;
+	y_start = (OV9740_MAX_HEIGHT - scale_input_y) / 2;
+	x_end = x_start + scale_input_x - 1;
+	y_end = y_start + scale_input_y - 1;
+
+	ret = ov9740_reg_write(client, OV9740_X_ADDR_START_HI, x_start >> 8);
+	if (ret)
+		goto done;
+	ret = ov9740_reg_write(client, OV9740_X_ADDR_START_LO, x_start & 0xff);
+	if (ret)
+		goto done;
+	ret = ov9740_reg_write(client, OV9740_Y_ADDR_START_HI, y_start >> 8);
+	if (ret)
+		goto done;
+	ret = ov9740_reg_write(client, OV9740_Y_ADDR_START_LO, y_start & 0xff);
+	if (ret)
+		goto done;
+
+	ret = ov9740_reg_write(client, OV9740_X_ADDR_END_HI, x_end >> 8);
+	if (ret)
+		goto done;
+	ret = ov9740_reg_write(client, OV9740_X_ADDR_END_LO, x_end & 0xff);
+	if (ret)
+		goto done;
+	ret = ov9740_reg_write(client, OV9740_Y_ADDR_END_HI, y_end >> 8);
+	if (ret)
+		goto done;
+	ret = ov9740_reg_write(client, OV9740_Y_ADDR_END_LO, y_end & 0xff);
+	if (ret)
+		goto done;
+
+	ret = ov9740_reg_write(client, OV9740_X_OUTPUT_SIZE_HI, width >> 8);
+	if (ret)
+		goto done;
+	ret = ov9740_reg_write(client, OV9740_X_OUTPUT_SIZE_LO, width & 0xff);
+	if (ret)
+		goto done;
+	ret = ov9740_reg_write(client, OV9740_Y_OUTPUT_SIZE_HI, height >> 8);
+	if (ret)
+		goto done;
+	ret = ov9740_reg_write(client, OV9740_Y_OUTPUT_SIZE_LO, height & 0xff);
+	if (ret)
+		goto done;
+
+	ret = ov9740_reg_write(client, OV9740_ISP_CTRL1E, scale_input_x >> 8);
+	if (ret)
+		goto done;
+	ret = ov9740_reg_write(client, OV9740_ISP_CTRL1F, scale_input_x & 0xff);
+	if (ret)
+		goto done;
+	ret = ov9740_reg_write(client, OV9740_ISP_CTRL20, scale_input_y >> 8);
+	if (ret)
+		goto done;
+	ret = ov9740_reg_write(client, OV9740_ISP_CTRL21, scale_input_y & 0xff);
+	if (ret)
+		goto done;
+
+	ret = ov9740_reg_write(client, OV9740_VFIFO_READ_START_HI,
+			       (scale_input_x - width) >> 8);
+	if (ret)
+		goto done;
+	ret = ov9740_reg_write(client, OV9740_VFIFO_READ_START_LO,
+			       (scale_input_x - width) & 0xff);
+	if (ret)
+		goto done;
+
+	ret = ov9740_reg_write(client, OV9740_ISP_CTRL00, 0xff);
+	if (ret)
+		goto done;
+	ret = ov9740_reg_write(client, OV9740_ISP_CTRL01, 0xef |
+							  (scaling << 4));
+	if (ret)
+		goto done;
+	ret = ov9740_reg_write(client, OV9740_ISP_CTRL03, 0xff);
+
+done:
+	return ret;
+}
+
+/* set the format we will capture in */
+static int ov9740_s_fmt(struct v4l2_subdev *sd,
+			struct v4l2_mbus_framefmt *mf)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov9740_priv *priv = to_ov9740(sd);
+	enum v4l2_colorspace cspace;
+	enum v4l2_mbus_pixelcode code = mf->code;
+	int ret;
+
+	ov9740_res_roundup(&mf->width, &mf->height);
+
+	switch (code) {
+	case V4L2_MBUS_FMT_YUYV8_2X8:
+		cspace = V4L2_COLORSPACE_SRGB;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	ret = ov9740_reg_write_array(client, ov9740_defaults,
+				     ARRAY_SIZE(ov9740_defaults));
+	if (ret < 0)
+		return ret;
+
+	ret = ov9740_set_res(client, mf->width, mf->height);
+	if (ret < 0)
+		return ret;
+
+	mf->code	= code;
+	mf->colorspace	= cspace;
+
+	memcpy(&priv->current_mf, mf, sizeof(struct v4l2_mbus_framefmt));
+
+	return ret;
+}
+
+static int ov9740_try_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_mbus_framefmt *mf)
+{
+	ov9740_res_roundup(&mf->width, &mf->height);
+
+	mf->field = V4L2_FIELD_NONE;
+	mf->code = V4L2_MBUS_FMT_YUYV8_2X8;
+	mf->colorspace = V4L2_COLORSPACE_SRGB;
+
+	return 0;
+}
+
+static int ov9740_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
+			   enum v4l2_mbus_pixelcode *code)
+{
+	if (index >= ARRAY_SIZE(ov9740_codes))
+		return -EINVAL;
+
+	*code = ov9740_codes[index];
+
+	return 0;
+}
+
+static int ov9740_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
+{
+	a->bounds.left		= 0;
+	a->bounds.top		= 0;
+	a->bounds.width		= OV9740_MAX_WIDTH;
+	a->bounds.height	= OV9740_MAX_HEIGHT;
+	a->defrect		= a->bounds;
+	a->type			= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	a->pixelaspect.numerator	= 1;
+	a->pixelaspect.denominator	= 1;
+
+	return 0;
+}
+
+static int ov9740_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+{
+	a->c.left		= 0;
+	a->c.top		= 0;
+	a->c.width		= OV9740_MAX_WIDTH;
+	a->c.height		= OV9740_MAX_HEIGHT;
+	a->type			= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	return 0;
+}
+
+/* Set status of additional camera capabilities */
+static int ov9740_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov9740_priv *priv =
+		container_of(ctrl->handler, struct ov9740_priv, hdl);
+
+	switch (ctrl->id) {
+	case V4L2_CID_VFLIP:
+		priv->flag_vflip = ctrl->val;
+		break;
+	case V4L2_CID_HFLIP:
+		priv->flag_hflip = ctrl->val;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* Get chip identification */
+static int ov9740_g_chip_ident(struct v4l2_subdev *sd,
+			       struct v4l2_dbg_chip_ident *id)
+{
+	struct ov9740_priv *priv = to_ov9740(sd);
+
+	id->ident = priv->ident;
+	id->revision = priv->revision;
+
+	return 0;
+}
+
+static int ov9740_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+	struct ov9740_priv *priv = to_ov9740(sd);
+	int ret;
+
+	if (on) {
+		ret = soc_camera_power_on(&client->dev, icl);
+		if (ret < 0)
+			return ret;
+
+		if (priv->current_enable) {
+			ov9740_s_fmt(sd, &priv->current_mf);
+			ov9740_s_stream(sd, 1);
+		}
+	} else {
+		if (priv->current_enable) {
+			ov9740_s_stream(sd, 0);
+			priv->current_enable = true;
+		}
+
+		soc_camera_power_off(&client->dev, icl);
+	}
+
+	return 0;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int ov9740_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 & ~0xffff)
+		return -EINVAL;
+
+	reg->size = 2;
+
+	ret = ov9740_reg_read(client, reg->reg, &val);
+	if (ret)
+		return ret;
+
+	reg->val = (__u64)val;
+
+	return ret;
+}
+
+static int ov9740_set_register(struct v4l2_subdev *sd,
+			       struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (reg->reg & ~0xffff || reg->val & ~0xff)
+		return -EINVAL;
+
+	return ov9740_reg_write(client, reg->reg, reg->val);
+}
+#endif
+
+static int ov9740_video_probe(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov9740_priv *priv = to_ov9740(sd);
+	u8 modelhi, modello;
+	int ret;
+
+	ret = ov9740_s_power(&priv->subdev, 1);
+	if (ret < 0)
+		return ret;
+
+	/*
+	 * check and show product ID and manufacturer ID
+	 */
+	ret = ov9740_reg_read(client, OV9740_MODEL_ID_HI, &modelhi);
+	if (ret < 0)
+		goto done;
+
+	ret = ov9740_reg_read(client, OV9740_MODEL_ID_LO, &modello);
+	if (ret < 0)
+		goto done;
+
+	priv->model = (modelhi << 8) | modello;
+
+	ret = ov9740_reg_read(client, OV9740_REVISION_NUMBER, &priv->revision);
+	if (ret < 0)
+		goto done;
+
+	ret = ov9740_reg_read(client, OV9740_MANUFACTURER_ID, &priv->manid);
+	if (ret < 0)
+		goto done;
+
+	ret = ov9740_reg_read(client, OV9740_SMIA_VERSION, &priv->smiaver);
+	if (ret < 0)
+		goto done;
+
+	if (priv->model != 0x9740) {
+		ret = -ENODEV;
+		goto done;
+	}
+
+	priv->ident = V4L2_IDENT_OV9740;
+
+	dev_info(&client->dev, "ov9740 Model ID 0x%04x, Revision 0x%02x, "
+		 "Manufacturer 0x%02x, SMIA Version 0x%02x\n",
+		 priv->model, priv->revision, priv->manid, priv->smiaver);
+
+	ret = v4l2_ctrl_handler_setup(&priv->hdl);
+
+done:
+	ov9740_s_power(&priv->subdev, 0);
+	return ret;
+}
+
+/* Request bus settings on camera side */
+static int ov9740_g_mbus_config(struct v4l2_subdev *sd,
+				struct v4l2_mbus_config *cfg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+	cfg->flags = V4L2_MBUS_PCLK_SAMPLE_RISING | V4L2_MBUS_MASTER |
+		V4L2_MBUS_VSYNC_ACTIVE_HIGH | V4L2_MBUS_HSYNC_ACTIVE_HIGH |
+		V4L2_MBUS_DATA_ACTIVE_HIGH;
+	cfg->type = V4L2_MBUS_PARALLEL;
+	cfg->flags = soc_camera_apply_board_flags(icl, cfg);
+
+	return 0;
+}
+
+static struct v4l2_subdev_video_ops ov9740_video_ops = {
+	.s_stream	= ov9740_s_stream,
+	.s_mbus_fmt	= ov9740_s_fmt,
+	.try_mbus_fmt	= ov9740_try_fmt,
+	.enum_mbus_fmt	= ov9740_enum_fmt,
+	.cropcap	= ov9740_cropcap,
+	.g_crop		= ov9740_g_crop,
+	.g_mbus_config	= ov9740_g_mbus_config,
+};
+
+static struct v4l2_subdev_core_ops ov9740_core_ops = {
+	.g_chip_ident		= ov9740_g_chip_ident,
+	.s_power		= ov9740_s_power,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register		= ov9740_get_register,
+	.s_register		= ov9740_set_register,
+#endif
+};
+
+static struct v4l2_subdev_ops ov9740_subdev_ops = {
+	.core			= &ov9740_core_ops,
+	.video			= &ov9740_video_ops,
+};
+
+static const struct v4l2_ctrl_ops ov9740_ctrl_ops = {
+	.s_ctrl = ov9740_s_ctrl,
+};
+
+/*
+ * i2c_driver function
+ */
+static int ov9740_probe(struct i2c_client *client,
+			const struct i2c_device_id *did)
+{
+	struct ov9740_priv *priv;
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+	int ret;
+
+	if (!icl) {
+		dev_err(&client->dev, "Missing platform_data for driver\n");
+		return -EINVAL;
+	}
+
+	priv = kzalloc(sizeof(struct ov9740_priv), GFP_KERNEL);
+	if (!priv) {
+		dev_err(&client->dev, "Failed to allocate private data!\n");
+		return -ENOMEM;
+	}
+
+	v4l2_i2c_subdev_init(&priv->subdev, client, &ov9740_subdev_ops);
+	v4l2_ctrl_handler_init(&priv->hdl, 13);
+	v4l2_ctrl_new_std(&priv->hdl, &ov9740_ctrl_ops,
+			V4L2_CID_VFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&priv->hdl, &ov9740_ctrl_ops,
+			V4L2_CID_HFLIP, 0, 1, 1, 0);
+	priv->subdev.ctrl_handler = &priv->hdl;
+	if (priv->hdl.error) {
+		int err = priv->hdl.error;
+
+		kfree(priv);
+		return err;
+	}
+
+	ret = ov9740_video_probe(client);
+	if (ret < 0) {
+		v4l2_ctrl_handler_free(&priv->hdl);
+		kfree(priv);
+	}
+
+	return ret;
+}
+
+static int ov9740_remove(struct i2c_client *client)
+{
+	struct ov9740_priv *priv = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(&priv->subdev);
+	v4l2_ctrl_handler_free(&priv->hdl);
+	kfree(priv);
+	return 0;
+}
+
+static const struct i2c_device_id ov9740_id[] = {
+	{ "ov9740", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ov9740_id);
+
+static struct i2c_driver ov9740_i2c_driver = {
+	.driver = {
+		.name = "ov9740",
+	},
+	.probe    = ov9740_probe,
+	.remove   = ov9740_remove,
+	.id_table = ov9740_id,
+};
+
+module_i2c_driver(ov9740_i2c_driver);
+
+MODULE_DESCRIPTION("SoC Camera driver for OmniVision OV9740");
+MODULE_AUTHOR("Andrew Chew <achew@nvidia.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/soc_camera/rj54n1cb0c.c b/drivers/media/i2c/soc_camera/rj54n1cb0c.c
new file mode 100644
index 000000000000..02f0400051d9
--- /dev/null
+++ b/drivers/media/i2c/soc_camera/rj54n1cb0c.c
@@ -0,0 +1,1431 @@
+/*
+ * Driver for RJ54N1CB0C CMOS Image Sensor from Sharp
+ *
+ * Copyright (C) 2009, Guennadi Liakhovetski <g.liakhovetski@gmx.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/v4l2-mediabus.h>
+#include <linux/videodev2.h>
+#include <linux/module.h>
+
+#include <media/rj54n1cb0c.h>
+#include <media/soc_camera.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-ctrls.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 {
+	enum v4l2_mbus_pixelcode	code;
+	enum v4l2_colorspace		colorspace;
+};
+
+/* Find a data format by a pixel code in an array */
+static const struct rj54n1_datafmt *rj54n1_find_datafmt(
+	enum v4l2_mbus_pixelcode 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[] = {
+	{V4L2_MBUS_FMT_YUYV8_2X8, V4L2_COLORSPACE_JPEG},
+	{V4L2_MBUS_FMT_YVYU8_2X8, V4L2_COLORSPACE_JPEG},
+	{V4L2_MBUS_FMT_RGB565_2X8_LE, V4L2_COLORSPACE_SRGB},
+	{V4L2_MBUS_FMT_RGB565_2X8_BE, V4L2_COLORSPACE_SRGB},
+	{V4L2_MBUS_FMT_SBGGR10_2X8_PADHI_LE, V4L2_COLORSPACE_SRGB},
+	{V4L2_MBUS_FMT_SBGGR10_2X8_PADLO_LE, V4L2_COLORSPACE_SRGB},
+	{V4L2_MBUS_FMT_SBGGR10_2X8_PADHI_BE, V4L2_COLORSPACE_SRGB},
+	{V4L2_MBUS_FMT_SBGGR10_2X8_PADLO_BE, V4L2_COLORSPACE_SRGB},
+	{V4L2_MBUS_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 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_fmt(struct v4l2_subdev *sd, unsigned int index,
+			   enum v4l2_mbus_pixelcode *code)
+{
+	if (index >= ARRAY_SIZE(rj54n1_colour_fmts))
+		return -EINVAL;
+
+	*code = rj54n1_colour_fmts[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_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct rj54n1 *rj54n1 = to_rj54n1(client);
+	const struct v4l2_rect *rect = &a->c;
+	int dummy = 0, output_w, output_h,
+		input_w = rect->width, input_h = rect->height;
+	int ret;
+
+	/* arbitrary minimum width and height, edges unimportant */
+	soc_camera_limit_side(&dummy, &input_w,
+		     RJ54N1_COLUMN_SKIP, 8, RJ54N1_MAX_WIDTH);
+
+	soc_camera_limit_side(&dummy, &input_h,
+		     RJ54N1_ROW_SKIP, 8, RJ54N1_MAX_HEIGHT);
+
+	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_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct rj54n1 *rj54n1 = to_rj54n1(client);
+
+	a->c	= rj54n1->rect;
+	a->type	= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	return 0;
+}
+
+static int rj54n1_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
+{
+	a->bounds.left			= RJ54N1_COLUMN_SKIP;
+	a->bounds.top			= RJ54N1_ROW_SKIP;
+	a->bounds.width			= RJ54N1_MAX_WIDTH;
+	a->bounds.height		= RJ54N1_MAX_HEIGHT;
+	a->defrect			= a->bounds;
+	a->type				= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	a->pixelaspect.numerator	= 1;
+	a->pixelaspect.denominator	= 1;
+
+	return 0;
+}
+
+static int rj54n1_g_fmt(struct v4l2_subdev *sd,
+			struct v4l2_mbus_framefmt *mf)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct rj54n1 *rj54n1 = to_rj54n1(client);
+
+	mf->code	= rj54n1->fmt->code;
+	mf->colorspace	= rj54n1->fmt->colorspace;
+	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_try_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_mbus_framefmt *mf)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct rj54n1 *rj54n1 = to_rj54n1(client);
+	const struct rj54n1_datafmt *fmt;
+	int align = mf->code == V4L2_MBUS_FMT_SBGGR10_1X10 ||
+		mf->code == V4L2_MBUS_FMT_SBGGR10_2X8_PADHI_BE ||
+		mf->code == V4L2_MBUS_FMT_SBGGR10_2X8_PADLO_BE ||
+		mf->code == V4L2_MBUS_FMT_SBGGR10_2X8_PADHI_LE ||
+		mf->code == V4L2_MBUS_FMT_SBGGR10_2X8_PADLO_LE;
+
+	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);
+
+	return 0;
+}
+
+static int rj54n1_s_fmt(struct v4l2_subdev *sd,
+			struct v4l2_mbus_framefmt *mf)
+{
+	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 ret;
+
+	/*
+	 * The host driver can call us without .try_fmt(), so, we have to take
+	 * care ourseleves
+	 */
+	rj54n1_try_fmt(sd, mf);
+
+	/*
+	 * 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;
+	}
+
+	dev_dbg(&client->dev, "%s: code = %d, width = %u, height = %u\n",
+		__func__, mf->code, mf->width, mf->height);
+
+	/* RA_SEL_UL is only relevant for raw modes, ignored otherwise. */
+	switch (mf->code) {
+	case V4L2_MBUS_FMT_YUYV8_2X8:
+		ret = reg_write(client, RJ54N1_OUT_SEL, 0);
+		if (!ret)
+			ret = reg_set(client, RJ54N1_BYTE_SWAP, 8, 8);
+		break;
+	case V4L2_MBUS_FMT_YVYU8_2X8:
+		ret = reg_write(client, RJ54N1_OUT_SEL, 0);
+		if (!ret)
+			ret = reg_set(client, RJ54N1_BYTE_SWAP, 0, 8);
+		break;
+	case V4L2_MBUS_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 V4L2_MBUS_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 V4L2_MBUS_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 V4L2_MBUS_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 V4L2_MBUS_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 V4L2_MBUS_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 V4L2_MBUS_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 == V4L2_MBUS_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;
+}
+
+static int rj54n1_g_chip_ident(struct v4l2_subdev *sd,
+			       struct v4l2_dbg_chip_ident *id)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (id->match.type != V4L2_CHIP_MATCH_I2C_ADDR)
+		return -EINVAL;
+
+	if (id->match.addr != client->addr)
+		return -ENODEV;
+
+	id->ident	= V4L2_IDENT_RJ54N1CB0C;
+	id->revision	= 0;
+
+	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->match.type != V4L2_CHIP_MATCH_I2C_ADDR ||
+	    reg->reg < 0x400 || reg->reg > 0x1fff)
+		/* Registers > 0x0800 are only available from Sharp support */
+		return -EINVAL;
+
+	if (reg->match.addr != client->addr)
+		return -ENODEV;
+
+	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,
+			     struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (reg->match.type != V4L2_CHIP_MATCH_I2C_ADDR ||
+	    reg->reg < 0x400 || reg->reg > 0x1fff)
+		/* Registers >= 0x0800 are only available from Sharp support */
+		return -EINVAL;
+
+	if (reg->match.addr != client->addr)
+		return -ENODEV;
+
+	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 soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+	return soc_camera_set_power(&client->dev, icl, on);
+}
+
+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 struct v4l2_subdev_core_ops rj54n1_subdev_core_ops = {
+	.g_chip_ident	= rj54n1_g_chip_ident,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register	= rj54n1_g_register,
+	.s_register	= rj54n1_s_register,
+#endif
+	.s_power	= rj54n1_s_power,
+};
+
+static int rj54n1_g_mbus_config(struct v4l2_subdev *sd,
+				struct v4l2_mbus_config *cfg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+	cfg->flags =
+		V4L2_MBUS_PCLK_SAMPLE_RISING | V4L2_MBUS_PCLK_SAMPLE_FALLING |
+		V4L2_MBUS_MASTER | V4L2_MBUS_DATA_ACTIVE_HIGH |
+		V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_HIGH;
+	cfg->type = V4L2_MBUS_PARALLEL;
+	cfg->flags = soc_camera_apply_board_flags(icl, cfg);
+
+	return 0;
+}
+
+static int rj54n1_s_mbus_config(struct v4l2_subdev *sd,
+				const struct v4l2_mbus_config *cfg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+	/* Figures 2.5-1 to 2.5-3 - default falling pixclk edge */
+	if (soc_camera_apply_board_flags(icl, cfg) &
+	    V4L2_MBUS_PCLK_SAMPLE_RISING)
+		return reg_write(client, RJ54N1_OUT_SIGPO, 1 << 4);
+	else
+		return reg_write(client, RJ54N1_OUT_SIGPO, 0);
+}
+
+static struct v4l2_subdev_video_ops rj54n1_subdev_video_ops = {
+	.s_stream	= rj54n1_s_stream,
+	.s_mbus_fmt	= rj54n1_s_fmt,
+	.g_mbus_fmt	= rj54n1_g_fmt,
+	.try_mbus_fmt	= rj54n1_try_fmt,
+	.enum_mbus_fmt	= rj54n1_enum_fmt,
+	.g_crop		= rj54n1_g_crop,
+	.s_crop		= rj54n1_s_crop,
+	.cropcap	= rj54n1_cropcap,
+	.g_mbus_config	= rj54n1_g_mbus_config,
+	.s_mbus_config	= rj54n1_s_mbus_config,
+};
+
+static struct v4l2_subdev_ops rj54n1_subdev_ops = {
+	.core	= &rj54n1_subdev_core_ops,
+	.video	= &rj54n1_subdev_video_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,
+			const struct i2c_device_id *did)
+{
+	struct rj54n1 *rj54n1;
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
+	struct rj54n1_pdata *rj54n1_priv;
+	int ret;
+
+	if (!icl || !icl->priv) {
+		dev_err(&client->dev, "RJ54N1CB0C: missing platform data!\n");
+		return -EINVAL;
+	}
+
+	rj54n1_priv = icl->priv;
+
+	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 = kzalloc(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) {
+		int err = rj54n1->hdl.error;
+
+		kfree(rj54n1);
+		return err;
+	}
+
+	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);
+
+	ret = rj54n1_video_probe(client, rj54n1_priv);
+	if (ret < 0) {
+		v4l2_ctrl_handler_free(&rj54n1->hdl);
+		kfree(rj54n1);
+	}
+
+	return ret;
+}
+
+static int rj54n1_remove(struct i2c_client *client)
+{
+	struct rj54n1 *rj54n1 = to_rj54n1(client);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+	v4l2_device_unregister_subdev(&rj54n1->subdev);
+	if (icl->free_bus)
+		icl->free_bus(icl);
+	v4l2_ctrl_handler_free(&rj54n1->hdl);
+	kfree(rj54n1);
+
+	return 0;
+}
+
+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/soc_camera/tw9910.c b/drivers/media/i2c/soc_camera/tw9910.c
new file mode 100644
index 000000000000..140716e71a15
--- /dev/null
+++ b/drivers/media/i2c/soc_camera/tw9910.c
@@ -0,0 +1,973 @@
+/*
+ * tw9910 Video Driver
+ *
+ * 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>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/v4l2-mediabus.h>
+#include <linux/videodev2.h>
+
+#include <media/soc_camera.h>
+#include <media/tw9910.h>
+#include <media/v4l2-chip-ident.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 tw9910_video_info	*info;
+	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 (1 == priv->revision)
+		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);
+	msleep(1);
+}
+
+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);
+
+	if (!(norm & (V4L2_STD_NTSC | V4L2_STD_PAL)))
+		return -EINVAL;
+
+	priv->norm = norm;
+
+	return 0;
+}
+
+static int tw9910_g_chip_ident(struct v4l2_subdev *sd,
+			       struct v4l2_dbg_chip_ident *id)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct tw9910_priv *priv = to_tw9910(client);
+
+	id->ident = V4L2_IDENT_TW9910;
+	id->revision = priv->revision;
+
+	return 0;
+}
+
+#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;
+
+	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,
+			     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 int tw9910_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+	return soc_camera_set_power(&client->dev, icl, on);
+}
+
+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 (SOCAM_DATAWIDTH_16 == priv->info->buswidth)
+		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_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct tw9910_priv *priv = to_tw9910(client);
+
+	a->c.left	= 0;
+	a->c.top	= 0;
+	if (priv->norm & V4L2_STD_NTSC) {
+		a->c.width	= 640;
+		a->c.height	= 480;
+	} else {
+		a->c.width	= 768;
+		a->c.height	= 576;
+	}
+	a->type		= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	return 0;
+}
+
+static int tw9910_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct tw9910_priv *priv = to_tw9910(client);
+
+	a->bounds.left			= 0;
+	a->bounds.top			= 0;
+	if (priv->norm & V4L2_STD_NTSC) {
+		a->bounds.width		= 640;
+		a->bounds.height	= 480;
+	} else {
+		a->bounds.width		= 768;
+		a->bounds.height	= 576;
+	}
+	a->defrect			= a->bounds;
+	a->type				= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	a->pixelaspect.numerator	= 1;
+	a->pixelaspect.denominator	= 1;
+
+	return 0;
+}
+
+static int tw9910_g_fmt(struct v4l2_subdev *sd,
+			struct v4l2_mbus_framefmt *mf)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct tw9910_priv *priv = to_tw9910(client);
+
+	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	= V4L2_MBUS_FMT_UYVY8_2X8;
+	mf->colorspace	= V4L2_COLORSPACE_JPEG;
+	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 != V4L2_MBUS_FMT_UYVY8_2X8)
+		return -EINVAL;
+
+	mf->colorspace = V4L2_COLORSPACE_JPEG;
+
+	ret = tw9910_set_frame(sd, &width, &height);
+	if (!ret) {
+		mf->width	= width;
+		mf->height	= height;
+	}
+	return ret;
+}
+
+static int tw9910_try_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_mbus_framefmt *mf)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct tw9910_priv *priv = to_tw9910(client);
+	const struct tw9910_scale_ctrl *scale;
+
+	if (V4L2_FIELD_ANY == mf->field) {
+		mf->field = V4L2_FIELD_INTERLACED_BT;
+	} else if (V4L2_FIELD_INTERLACED_BT != mf->field) {
+		dev_err(&client->dev, "Field type %d invalid.\n", mf->field);
+		return -EINVAL;
+	}
+
+	mf->code = V4L2_MBUS_FMT_UYVY8_2X8;
+	mf->colorspace = V4L2_COLORSPACE_JPEG;
+
+	/*
+	 * 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;
+
+	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 (SOCAM_DATAWIDTH_16 != priv->info->buswidth &&
+	    SOCAM_DATAWIDTH_8  != priv->info->buswidth) {
+		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 (0x0B != id ||
+	    0x01 < priv->revision) {
+		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;
+
+done:
+	tw9910_s_power(&priv->subdev, 0);
+	return ret;
+}
+
+static struct v4l2_subdev_core_ops tw9910_subdev_core_ops = {
+	.g_chip_ident	= tw9910_g_chip_ident,
+	.s_std		= tw9910_s_std,
+	.g_std		= tw9910_g_std,
+#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_fmt(struct v4l2_subdev *sd, unsigned int index,
+			   enum v4l2_mbus_pixelcode *code)
+{
+	if (index)
+		return -EINVAL;
+
+	*code = V4L2_MBUS_FMT_UYVY8_2X8;
+	return 0;
+}
+
+static int tw9910_g_mbus_config(struct v4l2_subdev *sd,
+				struct v4l2_mbus_config *cfg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+
+	cfg->flags = V4L2_MBUS_PCLK_SAMPLE_RISING | V4L2_MBUS_MASTER |
+		V4L2_MBUS_VSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_LOW |
+		V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_HSYNC_ACTIVE_LOW |
+		V4L2_MBUS_DATA_ACTIVE_HIGH;
+	cfg->type = V4L2_MBUS_PARALLEL;
+	cfg->flags = soc_camera_apply_board_flags(icl, cfg);
+
+	return 0;
+}
+
+static int tw9910_s_mbus_config(struct v4l2_subdev *sd,
+				const struct v4l2_mbus_config *cfg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
+	u8 val = VSSL_VVALID | HSSL_DVALID;
+	unsigned long flags = soc_camera_apply_board_flags(icl, cfg);
+
+	/*
+	 * set OUTCTR1
+	 *
+	 * We use VVALID and DVALID signals to control VSYNC and HSYNC
+	 * outputs, in this mode their polarity is inverted.
+	 */
+	if (flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
+		val |= HSP_HI;
+
+	if (flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
+		val |= VSP_HI;
+
+	return i2c_smbus_write_byte_data(client, OUTCTR1, val);
+}
+
+static struct v4l2_subdev_video_ops tw9910_subdev_video_ops = {
+	.s_stream	= tw9910_s_stream,
+	.g_mbus_fmt	= tw9910_g_fmt,
+	.s_mbus_fmt	= tw9910_s_fmt,
+	.try_mbus_fmt	= tw9910_try_fmt,
+	.cropcap	= tw9910_cropcap,
+	.g_crop		= tw9910_g_crop,
+	.enum_mbus_fmt	= tw9910_enum_fmt,
+	.g_mbus_config	= tw9910_g_mbus_config,
+	.s_mbus_config	= tw9910_s_mbus_config,
+};
+
+static struct v4l2_subdev_ops tw9910_subdev_ops = {
+	.core	= &tw9910_subdev_core_ops,
+	.video	= &tw9910_subdev_video_ops,
+};
+
+/*
+ * i2c_driver function
+ */
+
+static int tw9910_probe(struct i2c_client *client,
+			const struct i2c_device_id *did)
+
+{
+	struct tw9910_priv		*priv;
+	struct tw9910_video_info	*info;
+	struct i2c_adapter		*adapter =
+		to_i2c_adapter(client->dev.parent);
+	struct soc_camera_link		*icl = soc_camera_i2c_to_link(client);
+	int				ret;
+
+	if (!icl || !icl->priv) {
+		dev_err(&client->dev, "TW9910: missing platform data!\n");
+		return -EINVAL;
+	}
+
+	info = icl->priv;
+
+	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 = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->info   = info;
+
+	v4l2_i2c_subdev_init(&priv->subdev, client, &tw9910_subdev_ops);
+
+	ret = tw9910_video_probe(client);
+	if (ret)
+		kfree(priv);
+
+	return ret;
+}
+
+static int tw9910_remove(struct i2c_client *client)
+{
+	struct tw9910_priv *priv = to_tw9910(client);
+
+	kfree(priv);
+	return 0;
+}
+
+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("SoC Camera driver for tw9910");
+MODULE_AUTHOR("Kuninori Morimoto");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/sr030pc30.c b/drivers/media/i2c/sr030pc30.c
new file mode 100644
index 000000000000..e9d95bda2ab1
--- /dev/null
+++ b/drivers/media/i2c/sr030pc30.c
@@ -0,0 +1,871 @@
+/*
+ * Driver for SiliconFile SR030PC30 VGA (1/10-Inch) Image Sensor with ISP
+ *
+ * Copyright (C) 2010 Samsung Electronics Co., Ltd
+ * Author: Sylwester Nawrocki, s.nawrocki@samsung.com
+ *
+ * Based on original driver authored by Dongsoo Nathaniel Kim
+ * and HeungJun Kim <riverful.kim@samsung.com>.
+ *
+ * Based on mt9v011 Micron Digital Image Sensor driver
+ * Copyright (c) 2009 Mauro Carvalho Chehab (mchehab@redhat.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; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-mediabus.h>
+#include <media/sr030pc30.h>
+
+static int debug;
+module_param(debug, int, 0644);
+
+#define MODULE_NAME	"SR030PC30"
+
+/*
+ * Register offsets within a page
+ * b15..b8 - page id, b7..b0 - register address
+ */
+#define POWER_CTRL_REG		0x0001
+#define PAGEMODE_REG		0x03
+#define DEVICE_ID_REG		0x0004
+#define NOON010PC30_ID		0x86
+#define SR030PC30_ID		0x8C
+#define VDO_CTL1_REG		0x0010
+#define SUBSAMPL_NONE_VGA	0
+#define SUBSAMPL_QVGA		0x10
+#define SUBSAMPL_QQVGA		0x20
+#define VDO_CTL2_REG		0x0011
+#define SYNC_CTL_REG		0x0012
+#define WIN_ROWH_REG		0x0020
+#define WIN_ROWL_REG		0x0021
+#define WIN_COLH_REG		0x0022
+#define WIN_COLL_REG		0x0023
+#define WIN_HEIGHTH_REG		0x0024
+#define WIN_HEIGHTL_REG		0x0025
+#define WIN_WIDTHH_REG		0x0026
+#define WIN_WIDTHL_REG		0x0027
+#define HBLANKH_REG		0x0040
+#define HBLANKL_REG		0x0041
+#define VSYNCH_REG		0x0042
+#define VSYNCL_REG		0x0043
+/* page 10 */
+#define ISP_CTL_REG(n)		(0x1010 + (n))
+#define YOFS_REG		0x1040
+#define DARK_YOFS_REG		0x1041
+#define AG_ABRTH_REG		0x1050
+#define SAT_CTL_REG		0x1060
+#define BSAT_REG		0x1061
+#define RSAT_REG		0x1062
+#define AG_SAT_TH_REG		0x1063
+/* page 11 */
+#define ZLPF_CTRL_REG		0x1110
+#define ZLPF_CTRL2_REG		0x1112
+#define ZLPF_AGH_THR_REG	0x1121
+#define ZLPF_THR_REG		0x1160
+#define ZLPF_DYN_THR_REG	0x1160
+/* page 12 */
+#define YCLPF_CTL1_REG		0x1240
+#define YCLPF_CTL2_REG		0x1241
+#define YCLPF_THR_REG		0x1250
+#define BLPF_CTL_REG		0x1270
+#define BLPF_THR1_REG		0x1274
+#define BLPF_THR2_REG		0x1275
+/* page 14 - Lens Shading Compensation */
+#define LENS_CTRL_REG		0x1410
+#define LENS_XCEN_REG		0x1420
+#define LENS_YCEN_REG		0x1421
+#define LENS_R_COMP_REG		0x1422
+#define LENS_G_COMP_REG		0x1423
+#define LENS_B_COMP_REG		0x1424
+/* page 15 - Color correction */
+#define CMC_CTL_REG		0x1510
+#define CMC_OFSGH_REG		0x1514
+#define CMC_OFSGL_REG		0x1516
+#define CMC_SIGN_REG		0x1517
+/* Color correction coefficients */
+#define CMC_COEF_REG(n)		(0x1530 + (n))
+/* Color correction offset coefficients */
+#define CMC_OFS_REG(n)		(0x1540 + (n))
+/* page 16 - Gamma correction */
+#define GMA_CTL_REG		0x1610
+/* Gamma correction coefficients 0.14 */
+#define GMA_COEF_REG(n)		(0x1630 + (n))
+/* page 20 - Auto Exposure */
+#define AE_CTL1_REG		0x2010
+#define AE_CTL2_REG		0x2011
+#define AE_FRM_CTL_REG		0x2020
+#define AE_FINE_CTL_REG(n)	(0x2028 + (n))
+#define EXP_TIMEH_REG		0x2083
+#define EXP_TIMEM_REG		0x2084
+#define EXP_TIMEL_REG		0x2085
+#define EXP_MMINH_REG		0x2086
+#define EXP_MMINL_REG		0x2087
+#define EXP_MMAXH_REG		0x2088
+#define EXP_MMAXM_REG		0x2089
+#define EXP_MMAXL_REG		0x208A
+/* page 22 - Auto White Balance */
+#define AWB_CTL1_REG		0x2210
+#define AWB_ENABLE		0x80
+#define AWB_CTL2_REG		0x2211
+#define MWB_ENABLE		0x01
+/* RGB gain control (manual WB) when AWB_CTL1[7]=0 */
+#define AWB_RGAIN_REG		0x2280
+#define AWB_GGAIN_REG		0x2281
+#define AWB_BGAIN_REG		0x2282
+#define AWB_RMAX_REG		0x2283
+#define AWB_RMIN_REG		0x2284
+#define AWB_BMAX_REG		0x2285
+#define AWB_BMIN_REG		0x2286
+/* R, B gain range in bright light conditions */
+#define AWB_RMAXB_REG		0x2287
+#define AWB_RMINB_REG		0x2288
+#define AWB_BMAXB_REG		0x2289
+#define AWB_BMINB_REG		0x228A
+/* manual white balance, when AWB_CTL2[0]=1 */
+#define MWB_RGAIN_REG		0x22B2
+#define MWB_BGAIN_REG		0x22B3
+/* the token to mark an array end */
+#define REG_TERM		0xFFFF
+
+/* Minimum and maximum exposure time in ms */
+#define EXPOS_MIN_MS		1
+#define EXPOS_MAX_MS		125
+
+struct sr030pc30_info {
+	struct v4l2_subdev sd;
+	const struct sr030pc30_platform_data *pdata;
+	const struct sr030pc30_format *curr_fmt;
+	const struct sr030pc30_frmsize *curr_win;
+	unsigned int auto_wb:1;
+	unsigned int auto_exp:1;
+	unsigned int hflip:1;
+	unsigned int vflip:1;
+	unsigned int sleep:1;
+	unsigned int exposure;
+	u8 blue_balance;
+	u8 red_balance;
+	u8 i2c_reg_page;
+};
+
+struct sr030pc30_format {
+	enum v4l2_mbus_pixelcode code;
+	enum v4l2_colorspace colorspace;
+	u16 ispctl1_reg;
+};
+
+struct sr030pc30_frmsize {
+	u16 width;
+	u16 height;
+	int vid_ctl1;
+};
+
+struct i2c_regval {
+	u16 addr;
+	u16 val;
+};
+
+static const struct v4l2_queryctrl sr030pc30_ctrl[] = {
+	{
+		.id		= V4L2_CID_AUTO_WHITE_BALANCE,
+		.type		= V4L2_CTRL_TYPE_BOOLEAN,
+		.name		= "Auto White Balance",
+		.minimum	= 0,
+		.maximum	= 1,
+		.step		= 1,
+		.default_value	= 1,
+	}, {
+		.id		= V4L2_CID_RED_BALANCE,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "Red Balance",
+		.minimum	= 0,
+		.maximum	= 127,
+		.step		= 1,
+		.default_value	= 64,
+		.flags		= 0,
+	}, {
+		.id		= V4L2_CID_BLUE_BALANCE,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "Blue Balance",
+		.minimum	= 0,
+		.maximum	= 127,
+		.step		= 1,
+		.default_value	= 64,
+	}, {
+		.id		= V4L2_CID_EXPOSURE_AUTO,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "Auto Exposure",
+		.minimum	= 0,
+		.maximum	= 1,
+		.step		= 1,
+		.default_value	= 1,
+	}, {
+		.id		= V4L2_CID_EXPOSURE,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "Exposure",
+		.minimum	= EXPOS_MIN_MS,
+		.maximum	= EXPOS_MAX_MS,
+		.step		= 1,
+		.default_value	= 1,
+	}, {
+	}
+};
+
+/* supported resolutions */
+static const struct sr030pc30_frmsize sr030pc30_sizes[] = {
+	{
+		.width		= 640,
+		.height		= 480,
+		.vid_ctl1	= SUBSAMPL_NONE_VGA,
+	}, {
+		.width		= 320,
+		.height		= 240,
+		.vid_ctl1	= SUBSAMPL_QVGA,
+	}, {
+		.width		= 160,
+		.height		= 120,
+		.vid_ctl1	= SUBSAMPL_QQVGA,
+	},
+};
+
+/* supported pixel formats */
+static const struct sr030pc30_format sr030pc30_formats[] = {
+	{
+		.code		= V4L2_MBUS_FMT_YUYV8_2X8,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+		.ispctl1_reg	= 0x03,
+	}, {
+		.code		= V4L2_MBUS_FMT_YVYU8_2X8,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+		.ispctl1_reg	= 0x02,
+	}, {
+		.code		= V4L2_MBUS_FMT_VYUY8_2X8,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+		.ispctl1_reg	= 0,
+	}, {
+		.code		= V4L2_MBUS_FMT_UYVY8_2X8,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+		.ispctl1_reg	= 0x01,
+	}, {
+		.code		= V4L2_MBUS_FMT_RGB565_2X8_BE,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+		.ispctl1_reg	= 0x40,
+	},
+};
+
+static const struct i2c_regval sr030pc30_base_regs[] = {
+	/* Window size and position within pixel matrix */
+	{ WIN_ROWH_REG,		0x00 }, { WIN_ROWL_REG,		0x06 },
+	{ WIN_COLH_REG,		0x00 },	{ WIN_COLL_REG,		0x06 },
+	{ WIN_HEIGHTH_REG,	0x01 }, { WIN_HEIGHTL_REG,	0xE0 },
+	{ WIN_WIDTHH_REG,	0x02 }, { WIN_WIDTHL_REG,	0x80 },
+	{ HBLANKH_REG,		0x01 }, { HBLANKL_REG,		0x50 },
+	{ VSYNCH_REG,		0x00 }, { VSYNCL_REG,		0x14 },
+	{ SYNC_CTL_REG,		0 },
+	/* Color corection and saturation */
+	{ ISP_CTL_REG(0),	0x30 }, { YOFS_REG,		0x80 },
+	{ DARK_YOFS_REG,	0x04 }, { AG_ABRTH_REG,		0x78 },
+	{ SAT_CTL_REG,		0x1F }, { BSAT_REG,		0x90 },
+	{ AG_SAT_TH_REG,	0xF0 }, { 0x1064,		0x80 },
+	{ CMC_CTL_REG,		0x03 }, { CMC_OFSGH_REG,	0x3C },
+	{ CMC_OFSGL_REG,	0x2C }, { CMC_SIGN_REG,		0x2F },
+	{ CMC_COEF_REG(0),	0xCB }, { CMC_OFS_REG(0),	0x87 },
+	{ CMC_COEF_REG(1),	0x61 }, { CMC_OFS_REG(1),	0x18 },
+	{ CMC_COEF_REG(2),	0x16 }, { CMC_OFS_REG(2),	0x91 },
+	{ CMC_COEF_REG(3),	0x23 }, { CMC_OFS_REG(3),	0x94 },
+	{ CMC_COEF_REG(4),	0xCE }, { CMC_OFS_REG(4),	0x9f },
+	{ CMC_COEF_REG(5),	0x2B }, { CMC_OFS_REG(5),	0x33 },
+	{ CMC_COEF_REG(6),	0x01 }, { CMC_OFS_REG(6),	0x00 },
+	{ CMC_COEF_REG(7),	0x34 }, { CMC_OFS_REG(7),	0x94 },
+	{ CMC_COEF_REG(8),	0x75 }, { CMC_OFS_REG(8),	0x14 },
+	/* Color corection coefficients */
+	{ GMA_CTL_REG,		0x03 },	{ GMA_COEF_REG(0),	0x00 },
+	{ GMA_COEF_REG(1),	0x19 },	{ GMA_COEF_REG(2),	0x26 },
+	{ GMA_COEF_REG(3),	0x3B },	{ GMA_COEF_REG(4),	0x5D },
+	{ GMA_COEF_REG(5),	0x79 }, { GMA_COEF_REG(6),	0x8E },
+	{ GMA_COEF_REG(7),	0x9F },	{ GMA_COEF_REG(8),	0xAF },
+	{ GMA_COEF_REG(9),	0xBD },	{ GMA_COEF_REG(10),	0xCA },
+	{ GMA_COEF_REG(11),	0xDD }, { GMA_COEF_REG(12),	0xEC },
+	{ GMA_COEF_REG(13),	0xF7 },	{ GMA_COEF_REG(14),	0xFF },
+	/* Noise reduction, Z-LPF, YC-LPF and BLPF filters setup */
+	{ ZLPF_CTRL_REG,	0x99 }, { ZLPF_CTRL2_REG,	0x0E },
+	{ ZLPF_AGH_THR_REG,	0x29 }, { ZLPF_THR_REG,		0x0F },
+	{ ZLPF_DYN_THR_REG,	0x63 }, { YCLPF_CTL1_REG,	0x23 },
+	{ YCLPF_CTL2_REG,	0x3B }, { YCLPF_THR_REG,	0x05 },
+	{ BLPF_CTL_REG,		0x1D }, { BLPF_THR1_REG,	0x05 },
+	{ BLPF_THR2_REG,	0x04 },
+	/* Automatic white balance */
+	{ AWB_CTL1_REG,		0xFB }, { AWB_CTL2_REG,		0x26 },
+	{ AWB_RMAX_REG,		0x54 }, { AWB_RMIN_REG,		0x2B },
+	{ AWB_BMAX_REG,		0x57 }, { AWB_BMIN_REG,		0x29 },
+	{ AWB_RMAXB_REG,	0x50 }, { AWB_RMINB_REG,	0x43 },
+	{ AWB_BMAXB_REG,	0x30 }, { AWB_BMINB_REG,	0x22 },
+	/* Auto exposure */
+	{ AE_CTL1_REG,		0x8C }, { AE_CTL2_REG,		0x04 },
+	{ AE_FRM_CTL_REG,	0x01 }, { AE_FINE_CTL_REG(0),	0x3F },
+	{ AE_FINE_CTL_REG(1),	0xA3 }, { AE_FINE_CTL_REG(3),	0x34 },
+	/* Lens shading compensation */
+	{ LENS_CTRL_REG,	0x01 }, { LENS_XCEN_REG,	0x80 },
+	{ LENS_YCEN_REG,	0x70 }, { LENS_R_COMP_REG,	0x53 },
+	{ LENS_G_COMP_REG,	0x40 }, { LENS_B_COMP_REG,	0x3e },
+	{ REG_TERM,		0 },
+};
+
+static inline struct sr030pc30_info *to_sr030pc30(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct sr030pc30_info, sd);
+}
+
+static inline int set_i2c_page(struct sr030pc30_info *info,
+			       struct i2c_client *client, unsigned int reg)
+{
+	int ret = 0;
+	u32 page = reg >> 8 & 0xFF;
+
+	if (info->i2c_reg_page != page && (reg & 0xFF) != 0x03) {
+		ret = i2c_smbus_write_byte_data(client, PAGEMODE_REG, page);
+		if (!ret)
+			info->i2c_reg_page = page;
+	}
+	return ret;
+}
+
+static int cam_i2c_read(struct v4l2_subdev *sd, u32 reg_addr)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct sr030pc30_info *info = to_sr030pc30(sd);
+
+	int ret = set_i2c_page(info, client, reg_addr);
+	if (!ret)
+		ret = i2c_smbus_read_byte_data(client, reg_addr & 0xFF);
+	return ret;
+}
+
+static int cam_i2c_write(struct v4l2_subdev *sd, u32 reg_addr, u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct sr030pc30_info *info = to_sr030pc30(sd);
+
+	int ret = set_i2c_page(info, client, reg_addr);
+	if (!ret)
+		ret = i2c_smbus_write_byte_data(
+			client, reg_addr & 0xFF, val);
+	return ret;
+}
+
+static inline int sr030pc30_bulk_write_reg(struct v4l2_subdev *sd,
+				const struct i2c_regval *msg)
+{
+	while (msg->addr != REG_TERM) {
+		int ret = cam_i2c_write(sd, msg->addr, msg->val);
+		if (ret)
+			return ret;
+		msg++;
+	}
+	return 0;
+}
+
+/* Device reset and sleep mode control */
+static int sr030pc30_pwr_ctrl(struct v4l2_subdev *sd,
+				     bool reset, bool sleep)
+{
+	struct sr030pc30_info *info = to_sr030pc30(sd);
+	u8 reg = sleep ? 0xF1 : 0xF0;
+	int ret = 0;
+
+	if (reset)
+		ret = cam_i2c_write(sd, POWER_CTRL_REG, reg | 0x02);
+	if (!ret) {
+		ret = cam_i2c_write(sd, POWER_CTRL_REG, reg);
+		if (!ret) {
+			info->sleep = sleep;
+			if (reset)
+				info->i2c_reg_page = -1;
+		}
+	}
+	return ret;
+}
+
+static inline int sr030pc30_enable_autoexposure(struct v4l2_subdev *sd, int on)
+{
+	struct sr030pc30_info *info = to_sr030pc30(sd);
+	/* auto anti-flicker is also enabled here */
+	int ret = cam_i2c_write(sd, AE_CTL1_REG, on ? 0xDC : 0x0C);
+	if (!ret)
+		info->auto_exp = on;
+	return ret;
+}
+
+static int sr030pc30_set_exposure(struct v4l2_subdev *sd, int value)
+{
+	struct sr030pc30_info *info = to_sr030pc30(sd);
+
+	unsigned long expos = value * info->pdata->clk_rate / (8 * 1000);
+
+	int ret = cam_i2c_write(sd, EXP_TIMEH_REG, expos >> 16 & 0xFF);
+	if (!ret)
+		ret = cam_i2c_write(sd, EXP_TIMEM_REG, expos >> 8 & 0xFF);
+	if (!ret)
+		ret = cam_i2c_write(sd, EXP_TIMEL_REG, expos & 0xFF);
+	if (!ret) { /* Turn off AE */
+		info->exposure = value;
+		ret = sr030pc30_enable_autoexposure(sd, 0);
+	}
+	return ret;
+}
+
+/* Automatic white balance control */
+static int sr030pc30_enable_autowhitebalance(struct v4l2_subdev *sd, int on)
+{
+	struct sr030pc30_info *info = to_sr030pc30(sd);
+
+	int ret = cam_i2c_write(sd, AWB_CTL2_REG, on ? 0x2E : 0x2F);
+	if (!ret)
+		ret = cam_i2c_write(sd, AWB_CTL1_REG, on ? 0xFB : 0x7B);
+	if (!ret)
+		info->auto_wb = on;
+
+	return ret;
+}
+
+static int sr030pc30_set_flip(struct v4l2_subdev *sd)
+{
+	struct sr030pc30_info *info = to_sr030pc30(sd);
+
+	s32 reg = cam_i2c_read(sd, VDO_CTL2_REG);
+	if (reg < 0)
+		return reg;
+
+	reg &= 0x7C;
+	if (info->hflip)
+		reg |= 0x01;
+	if (info->vflip)
+		reg |= 0x02;
+	return cam_i2c_write(sd, VDO_CTL2_REG, reg | 0x80);
+}
+
+/* Configure resolution, color format and image flip */
+static int sr030pc30_set_params(struct v4l2_subdev *sd)
+{
+	struct sr030pc30_info *info = to_sr030pc30(sd);
+	int ret;
+
+	if (!info->curr_win)
+		return -EINVAL;
+
+	/* Configure the resolution through subsampling */
+	ret = cam_i2c_write(sd, VDO_CTL1_REG,
+			    info->curr_win->vid_ctl1);
+
+	if (!ret && info->curr_fmt)
+		ret = cam_i2c_write(sd, ISP_CTL_REG(0),
+				info->curr_fmt->ispctl1_reg);
+	if (!ret)
+		ret = sr030pc30_set_flip(sd);
+
+	return ret;
+}
+
+/* Find nearest matching image pixel size. */
+static int sr030pc30_try_frame_size(struct v4l2_mbus_framefmt *mf)
+{
+	unsigned int min_err = ~0;
+	int i = ARRAY_SIZE(sr030pc30_sizes);
+	const struct sr030pc30_frmsize *fsize = &sr030pc30_sizes[0],
+					*match = NULL;
+	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) {
+		mf->width  = match->width;
+		mf->height = match->height;
+		return 0;
+	}
+	return -EINVAL;
+}
+
+static int sr030pc30_queryctrl(struct v4l2_subdev *sd,
+			       struct v4l2_queryctrl *qc)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(sr030pc30_ctrl); i++)
+		if (qc->id == sr030pc30_ctrl[i].id) {
+			*qc = sr030pc30_ctrl[i];
+			v4l2_dbg(1, debug, sd, "%s id: %d\n",
+				 __func__, qc->id);
+			return 0;
+		}
+
+	return -EINVAL;
+}
+
+static inline int sr030pc30_set_bluebalance(struct v4l2_subdev *sd, int value)
+{
+	int ret = cam_i2c_write(sd, MWB_BGAIN_REG, value);
+	if (!ret)
+		to_sr030pc30(sd)->blue_balance = value;
+	return ret;
+}
+
+static inline int sr030pc30_set_redbalance(struct v4l2_subdev *sd, int value)
+{
+	int ret = cam_i2c_write(sd, MWB_RGAIN_REG, value);
+	if (!ret)
+		to_sr030pc30(sd)->red_balance = value;
+	return ret;
+}
+
+static int sr030pc30_s_ctrl(struct v4l2_subdev *sd,
+			    struct v4l2_control *ctrl)
+{
+	int i, ret = 0;
+
+	for (i = 0; i < ARRAY_SIZE(sr030pc30_ctrl); i++)
+		if (ctrl->id == sr030pc30_ctrl[i].id)
+			break;
+
+	if (i == ARRAY_SIZE(sr030pc30_ctrl))
+		return -EINVAL;
+
+	if (ctrl->value < sr030pc30_ctrl[i].minimum ||
+		ctrl->value > sr030pc30_ctrl[i].maximum)
+			return -ERANGE;
+
+	v4l2_dbg(1, debug, sd, "%s: ctrl_id: %d, value: %d\n",
+			 __func__, ctrl->id, ctrl->value);
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		sr030pc30_enable_autowhitebalance(sd, ctrl->value);
+		break;
+	case V4L2_CID_BLUE_BALANCE:
+		ret = sr030pc30_set_bluebalance(sd, ctrl->value);
+		break;
+	case V4L2_CID_RED_BALANCE:
+		ret = sr030pc30_set_redbalance(sd, ctrl->value);
+		break;
+	case V4L2_CID_EXPOSURE_AUTO:
+		sr030pc30_enable_autoexposure(sd,
+			ctrl->value == V4L2_EXPOSURE_AUTO);
+		break;
+	case V4L2_CID_EXPOSURE:
+		ret = sr030pc30_set_exposure(sd, ctrl->value);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return ret;
+}
+
+static int sr030pc30_g_ctrl(struct v4l2_subdev *sd,
+			    struct v4l2_control *ctrl)
+{
+	struct sr030pc30_info *info = to_sr030pc30(sd);
+
+	v4l2_dbg(1, debug, sd, "%s: id: %d\n", __func__, ctrl->id);
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		ctrl->value = info->auto_wb;
+		break;
+	case V4L2_CID_BLUE_BALANCE:
+		ctrl->value = info->blue_balance;
+		break;
+	case V4L2_CID_RED_BALANCE:
+		ctrl->value = info->red_balance;
+		break;
+	case V4L2_CID_EXPOSURE_AUTO:
+		ctrl->value = info->auto_exp;
+		break;
+	case V4L2_CID_EXPOSURE:
+		ctrl->value = info->exposure;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int sr030pc30_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
+			      enum v4l2_mbus_pixelcode *code)
+{
+	if (!code || index >= ARRAY_SIZE(sr030pc30_formats))
+		return -EINVAL;
+
+	*code = sr030pc30_formats[index].code;
+	return 0;
+}
+
+static int sr030pc30_g_fmt(struct v4l2_subdev *sd,
+			   struct v4l2_mbus_framefmt *mf)
+{
+	struct sr030pc30_info *info = to_sr030pc30(sd);
+	int ret;
+
+	if (!mf)
+		return -EINVAL;
+
+	if (!info->curr_win || !info->curr_fmt) {
+		ret = sr030pc30_set_params(sd);
+		if (ret)
+			return ret;
+	}
+
+	mf->width	= info->curr_win->width;
+	mf->height	= info->curr_win->height;
+	mf->code	= info->curr_fmt->code;
+	mf->colorspace	= info->curr_fmt->colorspace;
+	mf->field	= V4L2_FIELD_NONE;
+
+	return 0;
+}
+
+/* Return nearest media bus frame format. */
+static const struct sr030pc30_format *try_fmt(struct v4l2_subdev *sd,
+					      struct v4l2_mbus_framefmt *mf)
+{
+	int i = ARRAY_SIZE(sr030pc30_formats);
+
+	sr030pc30_try_frame_size(mf);
+
+	while (i--)
+		if (mf->code == sr030pc30_formats[i].code)
+			break;
+
+	mf->code = sr030pc30_formats[i].code;
+
+	return &sr030pc30_formats[i];
+}
+
+/* Return nearest media bus frame format. */
+static int sr030pc30_try_fmt(struct v4l2_subdev *sd,
+			     struct v4l2_mbus_framefmt *mf)
+{
+	if (!sd || !mf)
+		return -EINVAL;
+
+	try_fmt(sd, mf);
+	return 0;
+}
+
+static int sr030pc30_s_fmt(struct v4l2_subdev *sd,
+			   struct v4l2_mbus_framefmt *mf)
+{
+	struct sr030pc30_info *info = to_sr030pc30(sd);
+
+	if (!sd || !mf)
+		return -EINVAL;
+
+	info->curr_fmt = try_fmt(sd, mf);
+
+	return sr030pc30_set_params(sd);
+}
+
+static int sr030pc30_base_config(struct v4l2_subdev *sd)
+{
+	struct sr030pc30_info *info = to_sr030pc30(sd);
+	int ret;
+	unsigned long expmin, expmax;
+
+	ret = sr030pc30_bulk_write_reg(sd, sr030pc30_base_regs);
+	if (!ret) {
+		info->curr_fmt = &sr030pc30_formats[0];
+		info->curr_win = &sr030pc30_sizes[0];
+		ret = sr030pc30_set_params(sd);
+	}
+	if (!ret)
+		ret = sr030pc30_pwr_ctrl(sd, false, false);
+
+	if (!ret && !info->pdata)
+		return ret;
+
+	expmin = EXPOS_MIN_MS * info->pdata->clk_rate / (8 * 1000);
+	expmax = EXPOS_MAX_MS * info->pdata->clk_rate / (8 * 1000);
+
+	v4l2_dbg(1, debug, sd, "%s: expmin= %lx, expmax= %lx", __func__,
+		 expmin, expmax);
+
+	/* Setting up manual exposure time range */
+	ret = cam_i2c_write(sd, EXP_MMINH_REG, expmin >> 8 & 0xFF);
+	if (!ret)
+		ret = cam_i2c_write(sd, EXP_MMINL_REG, expmin & 0xFF);
+	if (!ret)
+		ret = cam_i2c_write(sd, EXP_MMAXH_REG, expmax >> 16 & 0xFF);
+	if (!ret)
+		ret = cam_i2c_write(sd, EXP_MMAXM_REG, expmax >> 8 & 0xFF);
+	if (!ret)
+		ret = cam_i2c_write(sd, EXP_MMAXL_REG, expmax & 0xFF);
+
+	return ret;
+}
+
+static int sr030pc30_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct sr030pc30_info *info = to_sr030pc30(sd);
+	const struct sr030pc30_platform_data *pdata = info->pdata;
+	int ret;
+
+	if (pdata == NULL) {
+		WARN(1, "No platform data!\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * Put sensor into power sleep mode before switching off
+	 * power and disabling MCLK.
+	 */
+	if (!on)
+		sr030pc30_pwr_ctrl(sd, false, true);
+
+	/* set_power controls sensor's power and clock */
+	if (pdata->set_power) {
+		ret = pdata->set_power(&client->dev, on);
+		if (ret)
+			return ret;
+	}
+
+	if (on) {
+		ret = sr030pc30_base_config(sd);
+	} else {
+		ret = 0;
+		info->curr_win = NULL;
+		info->curr_fmt = NULL;
+	}
+
+	return ret;
+}
+
+static const struct v4l2_subdev_core_ops sr030pc30_core_ops = {
+	.s_power	= sr030pc30_s_power,
+	.queryctrl	= sr030pc30_queryctrl,
+	.s_ctrl		= sr030pc30_s_ctrl,
+	.g_ctrl		= sr030pc30_g_ctrl,
+};
+
+static const struct v4l2_subdev_video_ops sr030pc30_video_ops = {
+	.g_mbus_fmt	= sr030pc30_g_fmt,
+	.s_mbus_fmt	= sr030pc30_s_fmt,
+	.try_mbus_fmt	= sr030pc30_try_fmt,
+	.enum_mbus_fmt	= sr030pc30_enum_fmt,
+};
+
+static const struct v4l2_subdev_ops sr030pc30_ops = {
+	.core	= &sr030pc30_core_ops,
+	.video	= &sr030pc30_video_ops,
+};
+
+/*
+ * Detect sensor type. Return 0 if SR030PC30 was detected
+ * or -ENODEV otherwise.
+ */
+static int sr030pc30_detect(struct i2c_client *client)
+{
+	const struct sr030pc30_platform_data *pdata
+		= client->dev.platform_data;
+	int ret;
+
+	/* Enable sensor's power and clock */
+	if (pdata->set_power) {
+		ret = pdata->set_power(&client->dev, 1);
+		if (ret)
+			return ret;
+	}
+
+	ret = i2c_smbus_read_byte_data(client, DEVICE_ID_REG);
+
+	if (pdata->set_power)
+		pdata->set_power(&client->dev, 0);
+
+	if (ret < 0) {
+		dev_err(&client->dev, "%s: I2C read failed\n", __func__);
+		return ret;
+	}
+
+	return ret == SR030PC30_ID ? 0 : -ENODEV;
+}
+
+
+static int sr030pc30_probe(struct i2c_client *client,
+			   const struct i2c_device_id *id)
+{
+	struct sr030pc30_info *info;
+	struct v4l2_subdev *sd;
+	const struct sr030pc30_platform_data *pdata
+		= client->dev.platform_data;
+	int ret;
+
+	if (!pdata) {
+		dev_err(&client->dev, "No platform data!");
+		return -EIO;
+	}
+
+	ret = sr030pc30_detect(client);
+	if (ret)
+		return ret;
+
+	info = kzalloc(sizeof(*info), GFP_KERNEL);
+	if (!info)
+		return -ENOMEM;
+
+	sd = &info->sd;
+	strcpy(sd->name, MODULE_NAME);
+	info->pdata = client->dev.platform_data;
+
+	v4l2_i2c_subdev_init(sd, client, &sr030pc30_ops);
+
+	info->i2c_reg_page	= -1;
+	info->hflip		= 1;
+	info->auto_exp		= 1;
+	info->exposure		= 30;
+
+	return 0;
+}
+
+static int sr030pc30_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct sr030pc30_info *info = to_sr030pc30(sd);
+
+	v4l2_device_unregister_subdev(sd);
+	kfree(info);
+	return 0;
+}
+
+static const struct i2c_device_id sr030pc30_id[] = {
+	{ MODULE_NAME, 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(i2c, sr030pc30_id);
+
+
+static struct i2c_driver sr030pc30_i2c_driver = {
+	.driver = {
+		.name = MODULE_NAME
+	},
+	.probe		= sr030pc30_probe,
+	.remove		= sr030pc30_remove,
+	.id_table	= sr030pc30_id,
+};
+
+module_i2c_driver(sr030pc30_i2c_driver);
+
+MODULE_DESCRIPTION("Siliconfile SR030PC30 camera driver");
+MODULE_AUTHOR("Sylwester Nawrocki <s.nawrocki@samsung.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/tcm825x.c b/drivers/media/i2c/tcm825x.c
new file mode 100644
index 000000000000..9252529fc5dd
--- /dev/null
+++ b/drivers/media/i2c/tcm825x.c
@@ -0,0 +1,937 @@
+/*
+ * drivers/media/i2c/tcm825x.c
+ *
+ * TCM825X camera sensor driver.
+ *
+ * Copyright (C) 2007 Nokia Corporation.
+ *
+ * Contact: Sakari Ailus <sakari.ailus@nokia.com>
+ *
+ * Based on code from David Cohen <david.cohen@indt.org.br>
+ *
+ * This driver was based on ov9640 sensor driver from MontaVista
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <media/v4l2-int-device.h>
+
+#include "tcm825x.h"
+
+/*
+ * The sensor has two fps modes: the lower one just gives half the fps
+ * at the same xclk than the high one.
+ */
+#define MAX_FPS 30
+#define MIN_FPS 8
+#define MAX_HALF_FPS (MAX_FPS / 2)
+#define HIGH_FPS_MODE_LOWER_LIMIT 14
+#define DEFAULT_FPS MAX_HALF_FPS
+
+struct tcm825x_sensor {
+	const struct tcm825x_platform_data *platform_data;
+	struct v4l2_int_device *v4l2_int_device;
+	struct i2c_client *i2c_client;
+	struct v4l2_pix_format pix;
+	struct v4l2_fract timeperframe;
+};
+
+/* list of image formats supported by TCM825X sensor */
+static const struct v4l2_fmtdesc tcm825x_formats[] = {
+	{
+		.description = "YUYV (YUV 4:2:2), packed",
+		.pixelformat = V4L2_PIX_FMT_UYVY,
+	}, {
+		/* Note:  V4L2 defines RGB565 as:
+		 *
+		 *      Byte 0                    Byte 1
+		 *      g2 g1 g0 r4 r3 r2 r1 r0   b4 b3 b2 b1 b0 g5 g4 g3
+		 *
+		 * We interpret RGB565 as:
+		 *
+		 *      Byte 0                    Byte 1
+		 *      g2 g1 g0 b4 b3 b2 b1 b0   r4 r3 r2 r1 r0 g5 g4 g3
+		 */
+		.description = "RGB565, le",
+		.pixelformat = V4L2_PIX_FMT_RGB565,
+	},
+};
+
+#define TCM825X_NUM_CAPTURE_FORMATS	ARRAY_SIZE(tcm825x_formats)
+
+/*
+ * TCM825X register configuration for all combinations of pixel format and
+ * image size
+ */
+static const struct tcm825x_reg subqcif	=	{ 0x20, TCM825X_PICSIZ };
+static const struct tcm825x_reg qcif	=	{ 0x18, TCM825X_PICSIZ };
+static const struct tcm825x_reg cif	=	{ 0x14, TCM825X_PICSIZ };
+static const struct tcm825x_reg qqvga	=	{ 0x0c, TCM825X_PICSIZ };
+static const struct tcm825x_reg qvga	=	{ 0x04, TCM825X_PICSIZ };
+static const struct tcm825x_reg vga	=	{ 0x00, TCM825X_PICSIZ };
+
+static const struct tcm825x_reg yuv422	=	{ 0x00, TCM825X_PICFMT };
+static const struct tcm825x_reg rgb565	=	{ 0x02, TCM825X_PICFMT };
+
+/* Our own specific controls */
+#define V4L2_CID_ALC				V4L2_CID_PRIVATE_BASE
+#define V4L2_CID_H_EDGE_EN			V4L2_CID_PRIVATE_BASE + 1
+#define V4L2_CID_V_EDGE_EN			V4L2_CID_PRIVATE_BASE + 2
+#define V4L2_CID_LENS				V4L2_CID_PRIVATE_BASE + 3
+#define V4L2_CID_MAX_EXPOSURE_TIME		V4L2_CID_PRIVATE_BASE + 4
+#define V4L2_CID_LAST_PRIV			V4L2_CID_MAX_EXPOSURE_TIME
+
+/*  Video controls  */
+static struct vcontrol {
+	struct v4l2_queryctrl qc;
+	u16 reg;
+	u16 start_bit;
+} video_control[] = {
+	{
+		{
+			.id = V4L2_CID_GAIN,
+			.type = V4L2_CTRL_TYPE_INTEGER,
+			.name = "Gain",
+			.minimum = 0,
+			.maximum = 63,
+			.step = 1,
+		},
+		.reg = TCM825X_AG,
+		.start_bit = 0,
+	},
+	{
+		{
+			.id = V4L2_CID_RED_BALANCE,
+			.type = V4L2_CTRL_TYPE_INTEGER,
+			.name = "Red Balance",
+			.minimum = 0,
+			.maximum = 255,
+			.step = 1,
+		},
+		.reg = TCM825X_MRG,
+		.start_bit = 0,
+	},
+	{
+		{
+			.id = V4L2_CID_BLUE_BALANCE,
+			.type = V4L2_CTRL_TYPE_INTEGER,
+			.name = "Blue Balance",
+			.minimum = 0,
+			.maximum = 255,
+			.step = 1,
+		},
+		.reg = TCM825X_MBG,
+		.start_bit = 0,
+	},
+	{
+		{
+			.id = V4L2_CID_AUTO_WHITE_BALANCE,
+			.type = V4L2_CTRL_TYPE_BOOLEAN,
+			.name = "Auto White Balance",
+			.minimum = 0,
+			.maximum = 1,
+			.step = 0,
+		},
+		.reg = TCM825X_AWBSW,
+		.start_bit = 7,
+	},
+	{
+		{
+			.id = V4L2_CID_EXPOSURE,
+			.type = V4L2_CTRL_TYPE_INTEGER,
+			.name = "Exposure Time",
+			.minimum = 0,
+			.maximum = 0x1fff,
+			.step = 1,
+		},
+		.reg = TCM825X_ESRSPD_U,
+		.start_bit = 0,
+	},
+	{
+		{
+			.id = V4L2_CID_HFLIP,
+			.type = V4L2_CTRL_TYPE_BOOLEAN,
+			.name = "Mirror Image",
+			.minimum = 0,
+			.maximum = 1,
+			.step = 0,
+		},
+		.reg = TCM825X_H_INV,
+		.start_bit = 6,
+	},
+	{
+		{
+			.id = V4L2_CID_VFLIP,
+			.type = V4L2_CTRL_TYPE_BOOLEAN,
+			.name = "Vertical Flip",
+			.minimum = 0,
+			.maximum = 1,
+			.step = 0,
+		},
+		.reg = TCM825X_V_INV,
+		.start_bit = 7,
+	},
+	/* Private controls */
+	{
+		{
+			.id = V4L2_CID_ALC,
+			.type = V4L2_CTRL_TYPE_BOOLEAN,
+			.name = "Auto Luminance Control",
+			.minimum = 0,
+			.maximum = 1,
+			.step = 0,
+		},
+		.reg = TCM825X_ALCSW,
+		.start_bit = 7,
+	},
+	{
+		{
+			.id = V4L2_CID_H_EDGE_EN,
+			.type = V4L2_CTRL_TYPE_INTEGER,
+			.name = "Horizontal Edge Enhancement",
+			.minimum = 0,
+			.maximum = 0xff,
+			.step = 1,
+		},
+		.reg = TCM825X_HDTG,
+		.start_bit = 0,
+	},
+	{
+		{
+			.id = V4L2_CID_V_EDGE_EN,
+			.type = V4L2_CTRL_TYPE_INTEGER,
+			.name = "Vertical Edge Enhancement",
+			.minimum = 0,
+			.maximum = 0xff,
+			.step = 1,
+		},
+		.reg = TCM825X_VDTG,
+		.start_bit = 0,
+	},
+	{
+		{
+			.id = V4L2_CID_LENS,
+			.type = V4L2_CTRL_TYPE_INTEGER,
+			.name = "Lens Shading Compensation",
+			.minimum = 0,
+			.maximum = 0x3f,
+			.step = 1,
+		},
+		.reg = TCM825X_LENS,
+		.start_bit = 0,
+	},
+	{
+		{
+			.id = V4L2_CID_MAX_EXPOSURE_TIME,
+			.type = V4L2_CTRL_TYPE_INTEGER,
+			.name = "Maximum Exposure Time",
+			.minimum = 0,
+			.maximum = 0x3,
+			.step = 1,
+		},
+		.reg = TCM825X_ESRLIM,
+		.start_bit = 5,
+	},
+};
+
+
+static const struct tcm825x_reg *tcm825x_siz_reg[NUM_IMAGE_SIZES] =
+{ &subqcif, &qqvga, &qcif, &qvga, &cif, &vga };
+
+static const struct tcm825x_reg *tcm825x_fmt_reg[NUM_PIXEL_FORMATS] =
+{ &yuv422, &rgb565 };
+
+/*
+ * Read a value from a register in an TCM825X sensor device.  The value is
+ * returned in 'val'.
+ * Returns zero if successful, or non-zero otherwise.
+ */
+static int tcm825x_read_reg(struct i2c_client *client, int reg)
+{
+	int err;
+	struct i2c_msg msg[2];
+	u8 reg_buf, data_buf = 0;
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	msg[0].addr = client->addr;
+	msg[0].flags = 0;
+	msg[0].len = 1;
+	msg[0].buf = &reg_buf;
+	msg[1].addr = client->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = 1;
+	msg[1].buf = &data_buf;
+
+	reg_buf = reg;
+
+	err = i2c_transfer(client->adapter, msg, 2);
+	if (err < 0)
+		return err;
+	return data_buf;
+}
+
+/*
+ * Write a value to a register in an TCM825X sensor device.
+ * Returns zero if successful, or non-zero otherwise.
+ */
+static int tcm825x_write_reg(struct i2c_client *client, u8 reg, u8 val)
+{
+	int err;
+	struct i2c_msg msg[1];
+	unsigned char data[2];
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	msg->addr = client->addr;
+	msg->flags = 0;
+	msg->len = 2;
+	msg->buf = data;
+	data[0] = reg;
+	data[1] = val;
+	err = i2c_transfer(client->adapter, msg, 1);
+	if (err >= 0)
+		return 0;
+	return err;
+}
+
+static int __tcm825x_write_reg_mask(struct i2c_client *client,
+				    u8 reg, u8 val, u8 mask)
+{
+	int rc;
+
+	/* need to do read - modify - write */
+	rc = tcm825x_read_reg(client, reg);
+	if (rc < 0)
+		return rc;
+
+	rc &= (~mask);	/* Clear the masked bits */
+	val &= mask;	/* Enforce mask on value */
+	val |= rc;
+
+	/* write the new value to the register */
+	rc = tcm825x_write_reg(client, reg, val);
+	if (rc)
+		return rc;
+
+	return 0;
+}
+
+#define tcm825x_write_reg_mask(client, regmask, val)			\
+	__tcm825x_write_reg_mask(client, TCM825X_ADDR((regmask)), val,	\
+				 TCM825X_MASK((regmask)))
+
+
+/*
+ * Initialize a list of TCM825X registers.
+ * The list of registers is terminated by the pair of values
+ * { TCM825X_REG_TERM, TCM825X_VAL_TERM }.
+ * Returns zero if successful, or non-zero otherwise.
+ */
+static int tcm825x_write_default_regs(struct i2c_client *client,
+				      const struct tcm825x_reg *reglist)
+{
+	int err;
+	const struct tcm825x_reg *next = reglist;
+
+	while (!((next->reg == TCM825X_REG_TERM)
+		 && (next->val == TCM825X_VAL_TERM))) {
+		err = tcm825x_write_reg(client, next->reg, next->val);
+		if (err) {
+			dev_err(&client->dev, "register writing failed\n");
+			return err;
+		}
+		next++;
+	}
+
+	return 0;
+}
+
+static struct vcontrol *find_vctrl(int id)
+{
+	int i;
+
+	if (id < V4L2_CID_BASE)
+		return NULL;
+
+	for (i = 0; i < ARRAY_SIZE(video_control); i++)
+		if (video_control[i].qc.id == id)
+			return &video_control[i];
+
+	return NULL;
+}
+
+/*
+ * Find the best match for a requested image capture size.  The best match
+ * is chosen as the nearest match that has the same number or fewer pixels
+ * as the requested size, or the smallest image size if the requested size
+ * has fewer pixels than the smallest image.
+ */
+static enum image_size tcm825x_find_size(struct v4l2_int_device *s,
+					 unsigned int width,
+					 unsigned int height)
+{
+	enum image_size isize;
+	unsigned long pixels = width * height;
+	struct tcm825x_sensor *sensor = s->priv;
+
+	for (isize = subQCIF; isize < VGA; isize++) {
+		if (tcm825x_sizes[isize + 1].height
+		    * tcm825x_sizes[isize + 1].width > pixels) {
+			dev_dbg(&sensor->i2c_client->dev, "size %d\n", isize);
+
+			return isize;
+		}
+	}
+
+	dev_dbg(&sensor->i2c_client->dev, "format default VGA\n");
+
+	return VGA;
+}
+
+/*
+ * Configure the TCM825X for current image size, pixel format, and
+ * frame period. fper is the frame period (in seconds) expressed as a
+ * fraction. Returns zero if successful, or non-zero otherwise. The
+ * actual frame period is returned in fper.
+ */
+static int tcm825x_configure(struct v4l2_int_device *s)
+{
+	struct tcm825x_sensor *sensor = s->priv;
+	struct v4l2_pix_format *pix = &sensor->pix;
+	enum image_size isize = tcm825x_find_size(s, pix->width, pix->height);
+	struct v4l2_fract *fper = &sensor->timeperframe;
+	enum pixel_format pfmt;
+	int err;
+	u32 tgt_fps;
+	u8 val;
+
+	/* common register initialization */
+	err = tcm825x_write_default_regs(
+		sensor->i2c_client, sensor->platform_data->default_regs());
+	if (err)
+		return err;
+
+	/* configure image size */
+	val = tcm825x_siz_reg[isize]->val;
+	dev_dbg(&sensor->i2c_client->dev,
+		"configuring image size %d\n", isize);
+	err = tcm825x_write_reg_mask(sensor->i2c_client,
+				     tcm825x_siz_reg[isize]->reg, val);
+	if (err)
+		return err;
+
+	/* configure pixel format */
+	switch (pix->pixelformat) {
+	default:
+	case V4L2_PIX_FMT_RGB565:
+		pfmt = RGB565;
+		break;
+	case V4L2_PIX_FMT_UYVY:
+		pfmt = YUV422;
+		break;
+	}
+
+	dev_dbg(&sensor->i2c_client->dev,
+		"configuring pixel format %d\n", pfmt);
+	val = tcm825x_fmt_reg[pfmt]->val;
+
+	err = tcm825x_write_reg_mask(sensor->i2c_client,
+				     tcm825x_fmt_reg[pfmt]->reg, val);
+	if (err)
+		return err;
+
+	/*
+	 * For frame rate < 15, the FPS reg (addr 0x02, bit 7) must be
+	 * set. Frame rate will be halved from the normal.
+	 */
+	tgt_fps = fper->denominator / fper->numerator;
+	if (tgt_fps <= HIGH_FPS_MODE_LOWER_LIMIT) {
+		val = tcm825x_read_reg(sensor->i2c_client, 0x02);
+		val |= 0x80;
+		tcm825x_write_reg(sensor->i2c_client, 0x02, val);
+	}
+
+	return 0;
+}
+
+static int ioctl_queryctrl(struct v4l2_int_device *s,
+				struct v4l2_queryctrl *qc)
+{
+	struct vcontrol *control;
+
+	control = find_vctrl(qc->id);
+
+	if (control == NULL)
+		return -EINVAL;
+
+	*qc = control->qc;
+
+	return 0;
+}
+
+static int ioctl_g_ctrl(struct v4l2_int_device *s,
+			     struct v4l2_control *vc)
+{
+	struct tcm825x_sensor *sensor = s->priv;
+	struct i2c_client *client = sensor->i2c_client;
+	int val, r;
+	struct vcontrol *lvc;
+
+	/* exposure time is special, spread across 2 registers */
+	if (vc->id == V4L2_CID_EXPOSURE) {
+		int val_lower, val_upper;
+
+		val_upper = tcm825x_read_reg(client,
+					     TCM825X_ADDR(TCM825X_ESRSPD_U));
+		if (val_upper < 0)
+			return val_upper;
+		val_lower = tcm825x_read_reg(client,
+					     TCM825X_ADDR(TCM825X_ESRSPD_L));
+		if (val_lower < 0)
+			return val_lower;
+
+		vc->value = ((val_upper & 0x1f) << 8) | (val_lower);
+		return 0;
+	}
+
+	lvc = find_vctrl(vc->id);
+	if (lvc == NULL)
+		return -EINVAL;
+
+	r = tcm825x_read_reg(client, TCM825X_ADDR(lvc->reg));
+	if (r < 0)
+		return r;
+	val = r & TCM825X_MASK(lvc->reg);
+	val >>= lvc->start_bit;
+
+	if (val < 0)
+		return val;
+
+	if (vc->id == V4L2_CID_HFLIP || vc->id == V4L2_CID_VFLIP)
+		val ^= sensor->platform_data->is_upside_down();
+
+	vc->value = val;
+	return 0;
+}
+
+static int ioctl_s_ctrl(struct v4l2_int_device *s,
+			     struct v4l2_control *vc)
+{
+	struct tcm825x_sensor *sensor = s->priv;
+	struct i2c_client *client = sensor->i2c_client;
+	struct vcontrol *lvc;
+	int val = vc->value;
+
+	/* exposure time is special, spread across 2 registers */
+	if (vc->id == V4L2_CID_EXPOSURE) {
+		int val_lower, val_upper;
+		val_lower = val & TCM825X_MASK(TCM825X_ESRSPD_L);
+		val_upper = (val >> 8) & TCM825X_MASK(TCM825X_ESRSPD_U);
+
+		if (tcm825x_write_reg_mask(client,
+					   TCM825X_ESRSPD_U, val_upper))
+			return -EIO;
+
+		if (tcm825x_write_reg_mask(client,
+					   TCM825X_ESRSPD_L, val_lower))
+			return -EIO;
+
+		return 0;
+	}
+
+	lvc = find_vctrl(vc->id);
+	if (lvc == NULL)
+		return -EINVAL;
+
+	if (vc->id == V4L2_CID_HFLIP || vc->id == V4L2_CID_VFLIP)
+		val ^= sensor->platform_data->is_upside_down();
+
+	val = val << lvc->start_bit;
+	if (tcm825x_write_reg_mask(client, lvc->reg, val))
+		return -EIO;
+
+	return 0;
+}
+
+static int ioctl_enum_fmt_cap(struct v4l2_int_device *s,
+				   struct v4l2_fmtdesc *fmt)
+{
+	int index = fmt->index;
+
+	switch (fmt->type) {
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		if (index >= TCM825X_NUM_CAPTURE_FORMATS)
+			return -EINVAL;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	fmt->flags = tcm825x_formats[index].flags;
+	strlcpy(fmt->description, tcm825x_formats[index].description,
+		sizeof(fmt->description));
+	fmt->pixelformat = tcm825x_formats[index].pixelformat;
+
+	return 0;
+}
+
+static int ioctl_try_fmt_cap(struct v4l2_int_device *s,
+			     struct v4l2_format *f)
+{
+	struct tcm825x_sensor *sensor = s->priv;
+	enum image_size isize;
+	int ifmt;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+
+	isize = tcm825x_find_size(s, pix->width, pix->height);
+	dev_dbg(&sensor->i2c_client->dev, "isize = %d num_capture = %lu\n",
+		isize, (unsigned long)TCM825X_NUM_CAPTURE_FORMATS);
+
+	pix->width = tcm825x_sizes[isize].width;
+	pix->height = tcm825x_sizes[isize].height;
+
+	for (ifmt = 0; ifmt < TCM825X_NUM_CAPTURE_FORMATS; ifmt++)
+		if (pix->pixelformat == tcm825x_formats[ifmt].pixelformat)
+			break;
+
+	if (ifmt == TCM825X_NUM_CAPTURE_FORMATS)
+		ifmt = 0;	/* Default = YUV 4:2:2 */
+
+	pix->pixelformat = tcm825x_formats[ifmt].pixelformat;
+	pix->field = V4L2_FIELD_NONE;
+	pix->bytesperline = pix->width * TCM825X_BYTES_PER_PIXEL;
+	pix->sizeimage = pix->bytesperline * pix->height;
+	pix->priv = 0;
+	dev_dbg(&sensor->i2c_client->dev, "format = 0x%08x\n",
+		pix->pixelformat);
+
+	switch (pix->pixelformat) {
+	case V4L2_PIX_FMT_UYVY:
+	default:
+		pix->colorspace = V4L2_COLORSPACE_JPEG;
+		break;
+	case V4L2_PIX_FMT_RGB565:
+		pix->colorspace = V4L2_COLORSPACE_SRGB;
+		break;
+	}
+
+	return 0;
+}
+
+static int ioctl_s_fmt_cap(struct v4l2_int_device *s,
+				struct v4l2_format *f)
+{
+	struct tcm825x_sensor *sensor = s->priv;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	int rval;
+
+	rval = ioctl_try_fmt_cap(s, f);
+	if (rval)
+		return rval;
+
+	rval = tcm825x_configure(s);
+
+	sensor->pix = *pix;
+
+	return rval;
+}
+
+static int ioctl_g_fmt_cap(struct v4l2_int_device *s,
+				struct v4l2_format *f)
+{
+	struct tcm825x_sensor *sensor = s->priv;
+
+	f->fmt.pix = sensor->pix;
+
+	return 0;
+}
+
+static int ioctl_g_parm(struct v4l2_int_device *s,
+			     struct v4l2_streamparm *a)
+{
+	struct tcm825x_sensor *sensor = s->priv;
+	struct v4l2_captureparm *cparm = &a->parm.capture;
+
+	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	memset(a, 0, sizeof(*a));
+	a->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	cparm->capability = V4L2_CAP_TIMEPERFRAME;
+	cparm->timeperframe = sensor->timeperframe;
+
+	return 0;
+}
+
+static int ioctl_s_parm(struct v4l2_int_device *s,
+			     struct v4l2_streamparm *a)
+{
+	struct tcm825x_sensor *sensor = s->priv;
+	struct v4l2_fract *timeperframe = &a->parm.capture.timeperframe;
+	u32 tgt_fps;	/* target frames per secound */
+	int rval;
+
+	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	if ((timeperframe->numerator == 0)
+	    || (timeperframe->denominator == 0)) {
+		timeperframe->denominator = DEFAULT_FPS;
+		timeperframe->numerator = 1;
+	}
+
+	tgt_fps = timeperframe->denominator / timeperframe->numerator;
+
+	if (tgt_fps > MAX_FPS) {
+		timeperframe->denominator = MAX_FPS;
+		timeperframe->numerator = 1;
+	} else if (tgt_fps < MIN_FPS) {
+		timeperframe->denominator = MIN_FPS;
+		timeperframe->numerator = 1;
+	}
+
+	sensor->timeperframe = *timeperframe;
+
+	rval = tcm825x_configure(s);
+
+	return rval;
+}
+
+static int ioctl_s_power(struct v4l2_int_device *s, int on)
+{
+	struct tcm825x_sensor *sensor = s->priv;
+
+	return sensor->platform_data->power_set(on);
+}
+
+/*
+ * Given the image capture format in pix, the nominal frame period in
+ * timeperframe, calculate the required xclk frequency.
+ *
+ * TCM825X input frequency characteristics are:
+ *     Minimum 11.9 MHz, Typical 24.57 MHz and maximum 25/27 MHz
+ */
+
+static int ioctl_g_ifparm(struct v4l2_int_device *s, struct v4l2_ifparm *p)
+{
+	struct tcm825x_sensor *sensor = s->priv;
+	struct v4l2_fract *timeperframe = &sensor->timeperframe;
+	u32 tgt_xclk;	/* target xclk */
+	u32 tgt_fps;	/* target frames per secound */
+	int rval;
+
+	rval = sensor->platform_data->ifparm(p);
+	if (rval)
+		return rval;
+
+	tgt_fps = timeperframe->denominator / timeperframe->numerator;
+
+	tgt_xclk = (tgt_fps <= HIGH_FPS_MODE_LOWER_LIMIT) ?
+		(2457 * tgt_fps) / MAX_HALF_FPS :
+		(2457 * tgt_fps) / MAX_FPS;
+	tgt_xclk *= 10000;
+
+	tgt_xclk = min(tgt_xclk, (u32)TCM825X_XCLK_MAX);
+	tgt_xclk = max(tgt_xclk, (u32)TCM825X_XCLK_MIN);
+
+	p->u.bt656.clock_curr = tgt_xclk;
+
+	return 0;
+}
+
+static int ioctl_g_needs_reset(struct v4l2_int_device *s, void *buf)
+{
+	struct tcm825x_sensor *sensor = s->priv;
+
+	return sensor->platform_data->needs_reset(s, buf, &sensor->pix);
+}
+
+static int ioctl_reset(struct v4l2_int_device *s)
+{
+	return -EBUSY;
+}
+
+static int ioctl_init(struct v4l2_int_device *s)
+{
+	return tcm825x_configure(s);
+}
+
+static int ioctl_dev_exit(struct v4l2_int_device *s)
+{
+	return 0;
+}
+
+static int ioctl_dev_init(struct v4l2_int_device *s)
+{
+	struct tcm825x_sensor *sensor = s->priv;
+	int r;
+
+	r = tcm825x_read_reg(sensor->i2c_client, 0x01);
+	if (r < 0)
+		return r;
+	if (r == 0) {
+		dev_err(&sensor->i2c_client->dev, "device not detected\n");
+		return -EIO;
+	}
+	return 0;
+}
+
+static struct v4l2_int_ioctl_desc tcm825x_ioctl_desc[] = {
+	{ vidioc_int_dev_init_num,
+	  (v4l2_int_ioctl_func *)ioctl_dev_init },
+	{ vidioc_int_dev_exit_num,
+	  (v4l2_int_ioctl_func *)ioctl_dev_exit },
+	{ vidioc_int_s_power_num,
+	  (v4l2_int_ioctl_func *)ioctl_s_power },
+	{ vidioc_int_g_ifparm_num,
+	  (v4l2_int_ioctl_func *)ioctl_g_ifparm },
+	{ vidioc_int_g_needs_reset_num,
+	  (v4l2_int_ioctl_func *)ioctl_g_needs_reset },
+	{ vidioc_int_reset_num,
+	  (v4l2_int_ioctl_func *)ioctl_reset },
+	{ vidioc_int_init_num,
+	  (v4l2_int_ioctl_func *)ioctl_init },
+	{ vidioc_int_enum_fmt_cap_num,
+	  (v4l2_int_ioctl_func *)ioctl_enum_fmt_cap },
+	{ vidioc_int_try_fmt_cap_num,
+	  (v4l2_int_ioctl_func *)ioctl_try_fmt_cap },
+	{ vidioc_int_g_fmt_cap_num,
+	  (v4l2_int_ioctl_func *)ioctl_g_fmt_cap },
+	{ vidioc_int_s_fmt_cap_num,
+	  (v4l2_int_ioctl_func *)ioctl_s_fmt_cap },
+	{ vidioc_int_g_parm_num,
+	  (v4l2_int_ioctl_func *)ioctl_g_parm },
+	{ vidioc_int_s_parm_num,
+	  (v4l2_int_ioctl_func *)ioctl_s_parm },
+	{ vidioc_int_queryctrl_num,
+	  (v4l2_int_ioctl_func *)ioctl_queryctrl },
+	{ vidioc_int_g_ctrl_num,
+	  (v4l2_int_ioctl_func *)ioctl_g_ctrl },
+	{ vidioc_int_s_ctrl_num,
+	  (v4l2_int_ioctl_func *)ioctl_s_ctrl },
+};
+
+static struct v4l2_int_slave tcm825x_slave = {
+	.ioctls = tcm825x_ioctl_desc,
+	.num_ioctls = ARRAY_SIZE(tcm825x_ioctl_desc),
+};
+
+static struct tcm825x_sensor tcm825x;
+
+static struct v4l2_int_device tcm825x_int_device = {
+	.module = THIS_MODULE,
+	.name = TCM825X_NAME,
+	.priv = &tcm825x,
+	.type = v4l2_int_type_slave,
+	.u = {
+		.slave = &tcm825x_slave,
+	},
+};
+
+static int tcm825x_probe(struct i2c_client *client,
+			 const struct i2c_device_id *did)
+{
+	struct tcm825x_sensor *sensor = &tcm825x;
+
+	if (i2c_get_clientdata(client))
+		return -EBUSY;
+
+	sensor->platform_data = client->dev.platform_data;
+
+	if (sensor->platform_data == NULL
+	    || !sensor->platform_data->is_okay())
+		return -ENODEV;
+
+	sensor->v4l2_int_device = &tcm825x_int_device;
+
+	sensor->i2c_client = client;
+	i2c_set_clientdata(client, sensor);
+
+	/* Make the default capture format QVGA RGB565 */
+	sensor->pix.width = tcm825x_sizes[QVGA].width;
+	sensor->pix.height = tcm825x_sizes[QVGA].height;
+	sensor->pix.pixelformat = V4L2_PIX_FMT_RGB565;
+
+	return v4l2_int_device_register(sensor->v4l2_int_device);
+}
+
+static int tcm825x_remove(struct i2c_client *client)
+{
+	struct tcm825x_sensor *sensor = i2c_get_clientdata(client);
+
+	if (!client->adapter)
+		return -ENODEV;	/* our client isn't attached */
+
+	v4l2_int_device_unregister(sensor->v4l2_int_device);
+
+	return 0;
+}
+
+static const struct i2c_device_id tcm825x_id[] = {
+	{ "tcm825x", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, tcm825x_id);
+
+static struct i2c_driver tcm825x_i2c_driver = {
+	.driver	= {
+		.name = TCM825X_NAME,
+	},
+	.probe	= tcm825x_probe,
+	.remove	= tcm825x_remove,
+	.id_table = tcm825x_id,
+};
+
+static struct tcm825x_sensor tcm825x = {
+	.timeperframe = {
+		.numerator   = 1,
+		.denominator = DEFAULT_FPS,
+	},
+};
+
+static int __init tcm825x_init(void)
+{
+	int rval;
+
+	rval = i2c_add_driver(&tcm825x_i2c_driver);
+	if (rval)
+		printk(KERN_INFO "%s: failed registering " TCM825X_NAME "\n",
+		       __func__);
+
+	return rval;
+}
+
+static void __exit tcm825x_exit(void)
+{
+	i2c_del_driver(&tcm825x_i2c_driver);
+}
+
+/*
+ * FIXME: Menelaus isn't ready (?) at module_init stage, so use
+ * late_initcall for now.
+ */
+late_initcall(tcm825x_init);
+module_exit(tcm825x_exit);
+
+MODULE_AUTHOR("Sakari Ailus <sakari.ailus@nokia.com>");
+MODULE_DESCRIPTION("TCM825x camera sensor driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/tcm825x.h b/drivers/media/i2c/tcm825x.h
new file mode 100644
index 000000000000..8ebab953963f
--- /dev/null
+++ b/drivers/media/i2c/tcm825x.h
@@ -0,0 +1,200 @@
+/*
+ * drivers/media/i2c/tcm825x.h
+ *
+ * Register definitions for the TCM825X CameraChip.
+ *
+ * Author: David Cohen (david.cohen@indt.org.br)
+ *
+ * This file is licensed under the terms of the GNU General Public License
+ * version 2. This program is licensed "as is" without any warranty of any
+ * kind, whether express or implied.
+ *
+ * This file was based on ov9640.h from MontaVista
+ */
+
+#ifndef TCM825X_H
+#define TCM825X_H
+
+#include <linux/videodev2.h>
+
+#include <media/v4l2-int-device.h>
+
+#define TCM825X_NAME "tcm825x"
+
+#define TCM825X_MASK(x)  x & 0x00ff
+#define TCM825X_ADDR(x) (x & 0xff00) >> 8
+
+/* The TCM825X I2C sensor chip has a fixed slave address of 0x3d. */
+#define TCM825X_I2C_ADDR	0x3d
+
+/*
+ * define register offsets for the TCM825X sensor chip
+ * OFFSET(8 bits) + MASK(8 bits)
+ * MASK bit 4 and 3 are used when the register uses more than one address
+ */
+#define TCM825X_FPS		0x0280
+#define TCM825X_ACF		0x0240
+#define TCM825X_DOUTBUF		0x020C
+#define TCM825X_DCLKP		0x0202
+#define TCM825X_ACFDET		0x0201
+#define TCM825X_DOUTSW		0x0380
+#define TCM825X_DATAHZ		0x0340
+#define TCM825X_PICSIZ		0x033c
+#define TCM825X_PICFMT		0x0302
+#define TCM825X_V_INV		0x0480
+#define TCM825X_H_INV		0x0440
+#define TCM825X_ESRLSW		0x0430
+#define TCM825X_V_LENGTH	0x040F
+#define TCM825X_ALCSW		0x0580
+#define TCM825X_ESRLIM		0x0560
+#define TCM825X_ESRSPD_U        0x051F
+#define TCM825X_ESRSPD_L        0x06FF
+#define TCM825X_AG		0x07FF
+#define TCM825X_ESRSPD2         0x06FF
+#define TCM825X_ALCMODE         0x0830
+#define TCM825X_ALCH            0x080F
+#define TCM825X_ALCL            0x09FF
+#define TCM825X_AWBSW           0x0A80
+#define TCM825X_MRG             0x0BFF
+#define TCM825X_MBG             0x0CFF
+#define TCM825X_GAMSW           0x0D80
+#define TCM825X_HDTG            0x0EFF
+#define TCM825X_VDTG            0x0FFF
+#define TCM825X_HDTCORE         0x10F0
+#define TCM825X_VDTCORE         0x100F
+#define TCM825X_CONT            0x11FF
+#define TCM825X_BRIGHT          0x12FF
+#define TCM825X_VHUE            0x137F
+#define TCM825X_UHUE            0x147F
+#define TCM825X_VGAIN           0x153F
+#define TCM825X_UGAIN           0x163F
+#define TCM825X_UVCORE          0x170F
+#define TCM825X_SATU            0x187F
+#define TCM825X_MHMODE          0x1980
+#define TCM825X_MHLPFSEL        0x1940
+#define TCM825X_YMODE           0x1930
+#define TCM825X_MIXHG           0x1907
+#define TCM825X_LENS            0x1A3F
+#define TCM825X_AGLIM           0x1BE0
+#define TCM825X_LENSRPOL        0x1B10
+#define TCM825X_LENSRGAIN       0x1B0F
+#define TCM825X_ES100S          0x1CFF
+#define TCM825X_ES120S          0x1DFF
+#define TCM825X_DMASK           0x1EC0
+#define TCM825X_CODESW          0x1E20
+#define TCM825X_CODESEL         0x1E10
+#define TCM825X_TESPIC          0x1E04
+#define TCM825X_PICSEL          0x1E03
+#define TCM825X_HNUM            0x20FF
+#define TCM825X_VOUTPH          0x287F
+#define TCM825X_ESROUT          0x327F
+#define TCM825X_ESROUT2         0x33FF
+#define TCM825X_AGOUT           0x34FF
+#define TCM825X_DGOUT           0x353F
+#define TCM825X_AGSLOW1         0x39C0
+#define TCM825X_FLLSMODE        0x3930
+#define TCM825X_FLLSLIM         0x390F
+#define TCM825X_DETSEL          0x3AF0
+#define TCM825X_ACDETNC         0x3A0F
+#define TCM825X_AGSLOW2         0x3BC0
+#define TCM825X_DG              0x3B3F
+#define TCM825X_REJHLEV         0x3CFF
+#define TCM825X_ALCLOCK         0x3D80
+#define TCM825X_FPSLNKSW        0x3D40
+#define TCM825X_ALCSPD          0x3D30
+#define TCM825X_REJH            0x3D03
+#define TCM825X_SHESRSW         0x3E80
+#define TCM825X_ESLIMSEL        0x3E40
+#define TCM825X_SHESRSPD        0x3E30
+#define TCM825X_ELSTEP          0x3E0C
+#define TCM825X_ELSTART         0x3E03
+#define TCM825X_AGMIN           0x3FFF
+#define TCM825X_PREGRG          0x423F
+#define TCM825X_PREGBG          0x433F
+#define TCM825X_PRERG           0x443F
+#define TCM825X_PREBG           0x453F
+#define TCM825X_MSKBR           0x477F
+#define TCM825X_MSKGR           0x487F
+#define TCM825X_MSKRB           0x497F
+#define TCM825X_MSKGB           0x4A7F
+#define TCM825X_MSKRG           0x4B7F
+#define TCM825X_MSKBG           0x4C7F
+#define TCM825X_HDTCSW          0x4D80
+#define TCM825X_VDTCSW          0x4D40
+#define TCM825X_DTCYL           0x4D3F
+#define TCM825X_HDTPSW          0x4E80
+#define TCM825X_VDTPSW          0x4E40
+#define TCM825X_DTCGAIN         0x4E3F
+#define TCM825X_DTLLIMSW        0x4F10
+#define TCM825X_DTLYLIM         0x4F0F
+#define TCM825X_YLCUTLMSK       0x5080
+#define TCM825X_YLCUTL          0x503F
+#define TCM825X_YLCUTHMSK       0x5180
+#define TCM825X_YLCUTH          0x513F
+#define TCM825X_UVSKNC          0x527F
+#define TCM825X_UVLJ            0x537F
+#define TCM825X_WBGMIN          0x54FF
+#define TCM825X_WBGMAX          0x55FF
+#define TCM825X_WBSPDUP         0x5603
+#define TCM825X_ALLAREA         0x5820
+#define TCM825X_WBLOCK          0x5810
+#define TCM825X_WB2SP           0x580F
+#define TCM825X_KIZUSW          0x5920
+#define TCM825X_PBRSW           0x5910
+#define TCM825X_ABCSW           0x5903
+#define TCM825X_PBDLV           0x5AFF
+#define TCM825X_PBC1LV          0x5BFF
+
+#define TCM825X_NUM_REGS	(TCM825X_ADDR(TCM825X_PBC1LV) + 1)
+
+#define TCM825X_BYTES_PER_PIXEL 2
+
+#define TCM825X_REG_TERM 0xff		/* terminating list entry for reg */
+#define TCM825X_VAL_TERM 0xff		/* terminating list entry for val */
+
+/* define a structure for tcm825x register initialization values */
+struct tcm825x_reg {
+	u8 val;
+	u16 reg;
+};
+
+enum image_size { subQCIF = 0, QQVGA, QCIF, QVGA, CIF, VGA };
+enum pixel_format { YUV422 = 0, RGB565 };
+#define NUM_IMAGE_SIZES 6
+#define NUM_PIXEL_FORMATS 2
+
+#define TCM825X_XCLK_MIN	11900000
+#define TCM825X_XCLK_MAX	25000000
+
+struct capture_size {
+	unsigned long width;
+	unsigned long height;
+};
+
+struct tcm825x_platform_data {
+	/* Is the sensor usable? Doesn't yet mean it's there, but you
+	 * can try! */
+	int (*is_okay)(void);
+	/* Set power state, zero is off, non-zero is on. */
+	int (*power_set)(int power);
+	/* Default registers written after power-on or reset. */
+	const struct tcm825x_reg *(*default_regs)(void);
+	int (*needs_reset)(struct v4l2_int_device *s, void *buf,
+			   struct v4l2_pix_format *fmt);
+	int (*ifparm)(struct v4l2_ifparm *p);
+	int (*is_upside_down)(void);
+};
+
+/* Array of image sizes supported by TCM825X.  These must be ordered from
+ * smallest image size to largest.
+ */
+static const struct capture_size tcm825x_sizes[] = {
+	{ 128,  96 }, /* subQCIF */
+	{ 160, 120 }, /* QQVGA */
+	{ 176, 144 }, /* QCIF */
+	{ 320, 240 }, /* QVGA */
+	{ 352, 288 }, /* CIF */
+	{ 640, 480 }, /* VGA */
+};
+
+#endif /* ifndef TCM825X_H */
diff --git a/drivers/media/i2c/tda7432.c b/drivers/media/i2c/tda7432.c
new file mode 100644
index 000000000000..f7707e65761e
--- /dev/null
+++ b/drivers/media/i2c/tda7432.c
@@ -0,0 +1,485 @@
+/*
+ * 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@infradead.org>
+ * This code is placed under the terms of the GNU General Public License
+ * 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 maximium 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/i2c-addr.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 maximium 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;
+	int addr;
+	int input;
+	int volume;
+	int muted;
+	int bass, treble;
+	int lf, lr, rf, rr;
+	int loud;
+};
+
+static inline struct tda7432 *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct tda7432, 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);
+	struct tda7432 *t = to_state(sd);
+	unsigned char buf[16];
+
+	v4l2_dbg(1, debug, sd,
+		"tda7432: 7432_set(0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x)\n",
+		t->input, t->volume, t->bass, t->treble, t->lf, t->lr,
+		t->rf, t->rr, t->loud);
+	buf[0]  = TDA7432_IN;
+	buf[1]  = t->input;
+	buf[2]  = t->volume;
+	buf[3]  = t->bass;
+	buf[4]	= t->treble;
+	buf[5]  = t->lf;
+	buf[6]  = t->lr;
+	buf[7]  = t->rf;
+	buf[8]  = t->rr;
+	buf[9]  = t->loud;
+	if (10 != i2c_master_send(client, buf, 10)) {
+		v4l2_err(sd, "I/O error, trying tda7432_set\n");
+		return -1;
+	}
+
+	return 0;
+}
+
+static void do_tda7432_init(struct v4l2_subdev *sd)
+{
+	struct tda7432 *t = to_state(sd);
+
+	v4l2_dbg(2, debug, sd, "In tda7432_init\n");
+
+	t->input  = TDA7432_STEREO_IN |  /* Main (stereo) input   */
+		    TDA7432_BASS_SYM  |  /* Symmetric bass cut    */
+		    TDA7432_BASS_NORM;   /* Normal bass range     */
+	t->volume =  0x3b ;				 /* -27dB Volume            */
+	if (loudness)			 /* Turn loudness on?     */
+		t->volume |= TDA7432_LD_ON;
+	t->muted    = 1;
+	t->treble   = TDA7432_TREBLE_0DB; /* 0dB Treble            */
+	t->bass		= TDA7432_BASS_0DB; 	 /* 0dB Bass              */
+	t->lf     = TDA7432_ATTEN_0DB;	 /* 0dB attenuation       */
+	t->lr     = TDA7432_ATTEN_0DB;	 /* 0dB attenuation       */
+	t->rf     = TDA7432_ATTEN_0DB;	 /* 0dB attenuation       */
+	t->rr     = TDA7432_ATTEN_0DB;	 /* 0dB attenuation       */
+	t->loud   = loudness;		 /* insmod parameter      */
+
+	tda7432_set(sd);
+}
+
+static int tda7432_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+	struct tda7432 *t = to_state(sd);
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUDIO_MUTE:
+		ctrl->value=t->muted;
+		return 0;
+	case V4L2_CID_AUDIO_VOLUME:
+		if (!maxvol){  /* max +20db */
+			ctrl->value = ( 0x6f - (t->volume & 0x7F) ) * 630;
+		} else {       /* max 0db   */
+			ctrl->value = ( 0x6f - (t->volume & 0x7F) ) * 829;
+		}
+		return 0;
+	case V4L2_CID_AUDIO_BALANCE:
+	{
+		if ( (t->lf) < (t->rf) )
+			/* right is attenuated, balance shifted left */
+			ctrl->value = (32768 - 1057*(t->rf));
+		else
+			/* left is attenuated, balance shifted right */
+			ctrl->value = (32768 + 1057*(t->lf));
+		return 0;
+	}
+	case V4L2_CID_AUDIO_BASS:
+	{
+		/* Bass/treble 4 bits each */
+		int bass=t->bass;
+		if(bass >= 0x8)
+			bass = ~(bass - 0x8) & 0xf;
+		ctrl->value = (bass << 12)+(bass << 8)+(bass << 4)+(bass);
+		return 0;
+	}
+	case V4L2_CID_AUDIO_TREBLE:
+	{
+		int treble=t->treble;
+		if(treble >= 0x8)
+			treble = ~(treble - 0x8) & 0xf;
+		ctrl->value = (treble << 12)+(treble << 8)+(treble << 4)+(treble);
+		return 0;
+	}
+	}
+	return -EINVAL;
+}
+
+static int tda7432_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+	struct tda7432 *t = to_state(sd);
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUDIO_MUTE:
+		t->muted=ctrl->value;
+		break;
+	case V4L2_CID_AUDIO_VOLUME:
+		if(!maxvol){ /* max +20db */
+			t->volume = 0x6f - ((ctrl->value)/630);
+		} else {    /* max 0db   */
+			t->volume = 0x6f - ((ctrl->value)/829);
+		}
+		if (loudness)		/* Turn on the loudness bit */
+			t->volume |= TDA7432_LD_ON;
+
+		tda7432_write(sd, TDA7432_VL, t->volume);
+		return 0;
+	case V4L2_CID_AUDIO_BALANCE:
+		if (ctrl->value < 32768) {
+			/* shifted to left, attenuate right */
+			t->rr = (32768 - ctrl->value)/1057;
+			t->rf = t->rr;
+			t->lr = TDA7432_ATTEN_0DB;
+			t->lf = TDA7432_ATTEN_0DB;
+		} else if(ctrl->value > 32769) {
+			/* shifted to right, attenuate left */
+			t->lf = (ctrl->value - 32768)/1057;
+			t->lr = t->lf;
+			t->rr = TDA7432_ATTEN_0DB;
+			t->rf = TDA7432_ATTEN_0DB;
+		} else {
+			/* centered */
+			t->rr = TDA7432_ATTEN_0DB;
+			t->rf = TDA7432_ATTEN_0DB;
+			t->lf = TDA7432_ATTEN_0DB;
+			t->lr = TDA7432_ATTEN_0DB;
+		}
+		break;
+	case V4L2_CID_AUDIO_BASS:
+		t->bass = ctrl->value >> 12;
+		if(t->bass>= 0x8)
+				t->bass = (~t->bass & 0xf) + 0x8 ;
+
+		tda7432_write(sd, TDA7432_TN, 0x10 | (t->bass << 4) | t->treble);
+		return 0;
+	case V4L2_CID_AUDIO_TREBLE:
+		t->treble= ctrl->value >> 12;
+		if(t->treble>= 0x8)
+				t->treble = (~t->treble & 0xf) + 0x8 ;
+
+		tda7432_write(sd, TDA7432_TN, 0x10 | (t->bass << 4) | t->treble);
+		return 0;
+	default:
+		return -EINVAL;
+	}
+
+	/* Used for both mute and balance changes */
+	if (t->muted)
+	{
+		/* Mute & update balance*/
+		tda7432_write(sd, TDA7432_LF, t->lf | TDA7432_MUTE);
+		tda7432_write(sd, TDA7432_LR, t->lr | TDA7432_MUTE);
+		tda7432_write(sd, TDA7432_RF, t->rf | TDA7432_MUTE);
+		tda7432_write(sd, TDA7432_RR, t->rr | TDA7432_MUTE);
+	} else {
+		tda7432_write(sd, TDA7432_LF, t->lf);
+		tda7432_write(sd, TDA7432_LR, t->lr);
+		tda7432_write(sd, TDA7432_RF, t->rf);
+		tda7432_write(sd, TDA7432_RR, t->rr);
+	}
+	return 0;
+}
+
+static int tda7432_queryctrl(struct v4l2_subdev *sd, struct v4l2_queryctrl *qc)
+{
+	switch (qc->id) {
+	case V4L2_CID_AUDIO_VOLUME:
+		return v4l2_ctrl_query_fill(qc, 0, 65535, 65535 / 100, 58880);
+	case V4L2_CID_AUDIO_MUTE:
+		return v4l2_ctrl_query_fill(qc, 0, 1, 1, 0);
+	case V4L2_CID_AUDIO_BALANCE:
+	case V4L2_CID_AUDIO_BASS:
+	case V4L2_CID_AUDIO_TREBLE:
+		return v4l2_ctrl_query_fill(qc, 0, 65535, 65535 / 100, 32768);
+	}
+	return -EINVAL;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_core_ops tda7432_core_ops = {
+	.queryctrl = tda7432_queryctrl,
+	.g_ctrl = tda7432_g_ctrl,
+	.s_ctrl = tda7432_s_ctrl,
+};
+
+static const struct v4l2_subdev_ops tda7432_ops = {
+	.core = &tda7432_core_ops,
+};
+
+/* ----------------------------------------------------------------------- */
+
+/* *********************** *
+ * i2c interface functions *
+ * *********************** */
+
+static int tda7432_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct tda7432 *t;
+	struct v4l2_subdev *sd;
+
+	v4l_info(client, "chip found @ 0x%02x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	t = kzalloc(sizeof(*t), GFP_KERNEL);
+	if (!t)
+		return -ENOMEM;
+	sd = &t->sd;
+	v4l2_i2c_subdev_init(sd, client, &tda7432_ops);
+	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;
+	}
+
+	do_tda7432_init(sd);
+	return 0;
+}
+
+static int tda7432_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	do_tda7432_init(sd);
+	v4l2_device_unregister_subdev(sd);
+	kfree(to_state(sd));
+	return 0;
+}
+
+static const struct i2c_device_id tda7432_id[] = {
+	{ "tda7432", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, tda7432_id);
+
+static struct i2c_driver tda7432_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.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 000000000000..3d7ddd93282d
--- /dev/null
+++ b/drivers/media/i2c/tda9840.c
@@ -0,0 +1,224 @@
+ /*
+    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 informations download the specifications directly
+    from SGS Thomson at http://www.st.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; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+  */
+
+
+#include <linux/module.h>
+#include <linux/ioctl.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.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);
+	u8 byte;
+
+	if (1 != i2c_master_recv(client, &byte, 1)) {
+		v4l2_dbg(1, debug, sd,
+			"i2c_master_recv() failed\n");
+		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, 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 int tda9840_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ident *chip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_TDA9840, 0);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_core_ops tda9840_core_ops = {
+	.g_chip_ident = tda9840_g_chip_ident,
+};
+
+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 = {
+	.core = &tda9840_core_ops,
+	.tuner = &tda9840_tuner_ops,
+};
+
+/* ----------------------------------------------------------------------- */
+
+static int tda9840_probe(struct i2c_client *client,
+			  const struct i2c_device_id *id)
+{
+	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 = kzalloc(sizeof(struct v4l2_subdev), 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 int tda9840_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	kfree(sd);
+	return 0;
+}
+
+static const struct i2c_device_id tda9840_id[] = {
+	{ "tda9840", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, tda9840_id);
+
+static struct i2c_driver tda9840_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.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 000000000000..3d5b06a5c308
--- /dev/null
+++ b/drivers/media/i2c/tea6415c.c
@@ -0,0 +1,187 @@
+ /*
+    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 informations download the specifications directly
+    from SGS Thomson at http://www.st.com
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License vs published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mvss Ave, Cambridge, MA 02139, USA.
+  */
+
+
+#include <linux/module.h>
+#include <linux/ioctl.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.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 int tea6415c_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ident *chip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_TEA6415C, 0);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_core_ops tea6415c_core_ops = {
+	.g_chip_ident = tea6415c_g_chip_ident,
+};
+
+static const struct v4l2_subdev_video_ops tea6415c_video_ops = {
+	.s_routing = tea6415c_s_routing,
+};
+
+static const struct v4l2_subdev_ops tea6415c_ops = {
+	.core = &tea6415c_core_ops,
+	.video = &tea6415c_video_ops,
+};
+
+static int tea6415c_probe(struct i2c_client *client,
+			  const struct i2c_device_id *id)
+{
+	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 = kzalloc(sizeof(struct v4l2_subdev), GFP_KERNEL);
+	if (sd == NULL)
+		return -ENOMEM;
+	v4l2_i2c_subdev_init(sd, client, &tea6415c_ops);
+	return 0;
+}
+
+static int tea6415c_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	kfree(sd);
+	return 0;
+}
+
+static const struct i2c_device_id tea6415c_id[] = {
+	{ "tea6415c", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, tea6415c_id);
+
+static struct i2c_driver tea6415c_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.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 000000000000..3a47d697536e
--- /dev/null
+++ b/drivers/media/i2c/tea6415c.h
@@ -0,0 +1,27 @@
+#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 000000000000..38757217a074
--- /dev/null
+++ b/drivers/media/i2c/tea6420.c
@@ -0,0 +1,169 @@
+ /*
+    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 informations download the specifications directly
+    from SGS Thomson at http://www.st.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; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+  */
+
+
+#include <linux/module.h>
+#include <linux/ioctl.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.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 int tea6420_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ident *chip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_TEA6420, 0);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_core_ops tea6420_core_ops = {
+	.g_chip_ident = tea6420_g_chip_ident,
+};
+
+static const struct v4l2_subdev_audio_ops tea6420_audio_ops = {
+	.s_routing = tea6420_s_routing,
+};
+
+static const struct v4l2_subdev_ops tea6420_ops = {
+	.core = &tea6420_core_ops,
+	.audio = &tea6420_audio_ops,
+};
+
+static int tea6420_probe(struct i2c_client *client,
+			  const struct i2c_device_id *id)
+{
+	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 = kzalloc(sizeof(struct v4l2_subdev), 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 int tea6420_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	kfree(sd);
+	return 0;
+}
+
+static const struct i2c_device_id tea6420_id[] = {
+	{ "tea6420", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, tea6420_id);
+
+static struct i2c_driver tea6420_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.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 000000000000..4aa3edb3e193
--- /dev/null
+++ b/drivers/media/i2c/tea6420.h
@@ -0,0 +1,24 @@
+#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 000000000000..e5c0eedebc58
--- /dev/null
+++ b/drivers/media/i2c/ths7303.c
@@ -0,0 +1,140 @@
+/*
+ * ths7303- THS7303 Video Amplifier 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/uaccess.h>
+#include <linux/videodev2.h>
+
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-chip-ident.h>
+
+MODULE_DESCRIPTION("TI THS7303 video amplifier driver");
+MODULE_AUTHOR("Chaithrika U S");
+MODULE_LICENSE("GPL");
+
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "Debug level 0-1");
+
+/* following function is used to set ths7303 */
+static int ths7303_setvalue(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	int err = 0;
+	u8 val;
+	struct i2c_client *client;
+
+	client = v4l2_get_subdevdata(sd);
+
+	if (std & (V4L2_STD_ALL & ~V4L2_STD_SECAM)) {
+		val = 0x02;
+		v4l2_dbg(1, debug, sd, "setting value for SDTV format\n");
+	} else {
+		val = 0x00;
+		v4l2_dbg(1, debug, sd, "disabling all channels\n");
+	}
+
+	err |= i2c_smbus_write_byte_data(client, 0x01, val);
+	err |= i2c_smbus_write_byte_data(client, 0x02, val);
+	err |= i2c_smbus_write_byte_data(client, 0x03, val);
+
+	if (err)
+		v4l2_err(sd, "write failed\n");
+
+	return err;
+}
+
+static int ths7303_s_std_output(struct v4l2_subdev *sd, v4l2_std_id norm)
+{
+	return ths7303_setvalue(sd, norm);
+}
+
+static int ths7303_g_chip_ident(struct v4l2_subdev *sd,
+				struct v4l2_dbg_chip_ident *chip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_THS7303, 0);
+}
+
+static const struct v4l2_subdev_video_ops ths7303_video_ops = {
+	.s_std_output	= ths7303_s_std_output,
+};
+
+static const struct v4l2_subdev_core_ops ths7303_core_ops = {
+	.g_chip_ident = ths7303_g_chip_ident,
+};
+
+static const struct v4l2_subdev_ops ths7303_ops = {
+	.core	= &ths7303_core_ops,
+	.video 	= &ths7303_video_ops,
+};
+
+static int ths7303_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct v4l2_subdev *sd;
+	v4l2_std_id std_id = V4L2_STD_NTSC;
+
+	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);
+
+	sd = kzalloc(sizeof(struct v4l2_subdev), GFP_KERNEL);
+	if (sd == NULL)
+		return -ENOMEM;
+
+	v4l2_i2c_subdev_init(sd, client, &ths7303_ops);
+
+	return ths7303_setvalue(sd, std_id);
+}
+
+static int ths7303_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	kfree(sd);
+
+	return 0;
+}
+
+static const struct i2c_device_id ths7303_id[] = {
+	{"ths7303", 0},
+	{},
+};
+
+MODULE_DEVICE_TABLE(i2c, ths7303_id);
+
+static struct i2c_driver ths7303_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "ths7303",
+	},
+	.probe		= ths7303_probe,
+	.remove		= ths7303_remove,
+	.id_table	= ths7303_id,
+};
+
+module_i2c_driver(ths7303_driver);
diff --git a/drivers/media/i2c/tlv320aic23b.c b/drivers/media/i2c/tlv320aic23b.c
new file mode 100644
index 000000000000..809a75a558ee
--- /dev/null
+++ b/drivers/media/i2c/tlv320aic23b.c
@@ -0,0 +1,230 @@
+/*
+ * 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>
+ *
+ * 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; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <linux/ioctl.h>
+#include <asm/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,
+	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
+	.try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
+	.s_ext_ctrls = v4l2_subdev_s_ext_ctrls,
+	.g_ctrl = v4l2_subdev_g_ctrl,
+	.s_ctrl = v4l2_subdev_s_ctrl,
+	.queryctrl = v4l2_subdev_queryctrl,
+	.querymenu = v4l2_subdev_querymenu,
+};
+
+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,
+			      const struct i2c_device_id *id)
+{
+	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 = kzalloc(sizeof(struct tlv320aic23b_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);
+		kfree(state);
+		return err;
+	}
+	v4l2_ctrl_handler_setup(&state->hdl);
+	return 0;
+}
+
+static int 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);
+	kfree(state);
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id tlv320aic23b_id[] = {
+	{ "tlv320aic23b", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, tlv320aic23b_id);
+
+static struct i2c_driver tlv320aic23b_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.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 000000000000..3b24d3fc1866
--- /dev/null
+++ b/drivers/media/i2c/tvaudio.c
@@ -0,0 +1,2131 @@
+/*
+ * 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/tvaudio.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.h>
+
+#include <media/i2c-addr.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/65535/32768/32768 */
+	int    leftinit,rightinit,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;
+
+	/* chip-specific description - should point to
+	   an entry at CHIPDESC table */
+	struct CHIPDESC *desc;
+
+	/* shadow register set */
+	audiocmd   shadow;
+
+	/* current settings */
+	__u16 left, right, treble, bass, 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);
+}
+
+
+/* ---------------------------------------------------------------------- */
+/* 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];
+
+	if (subaddr < 0) {
+		v4l2_dbg(1, debug, sd, "chip_write: 0x%x\n", val);
+		chip->shadow.bytes[1] = val;
+		buffer[0] = val;
+		if (1 != i2c_master_send(c, buffer, 1)) {
+			v4l2_warn(sd, "I/O error (write 0x%x)\n", val);
+			return -1;
+		}
+	} 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;
+		if (2 != i2c_master_send(c, buffer, 2)) {
+			v4l2_warn(sd, "I/O error (write reg%d=0x%x)\n",
+				subaddr, val);
+			return -1;
+		}
+	}
+	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;
+
+	if (1 != i2c_master_recv(c, &buffer, 1)) {
+		v4l2_warn(sd, "I/O error (read)\n");
+		return -1;
+	}
+	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);
+	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;
+
+	if (2 != i2c_transfer(c->adapter, msgs, 2)) {
+		v4l2_warn(sd, "I/O error (read2)\n");
+		return -1;
+	}
+	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;
+
+	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 bellow !*/
+
+	/* 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 */
+	if (cmd->count != i2c_master_send(c, cmd->bytes, cmd->count)) {
+		v4l2_warn(sd, "I/O error (%s)\n", name);
+		return -1;
+	}
+	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(unsigned long data)
+{
+	struct CHIPSTATE *chip = (struct CHIPSTATE*)data;
+	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;
+
+	val = chip_read(chip);
+	mode = V4L2_TUNER_SUB_MONO;
+	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);
+	/* 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 contol 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 ouput, 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 & 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;
+
+	val = chip_read(chip);
+	mode = V4L2_TUNER_SUB_MONO;
+	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;
+
+	if (-1 == (rc = chip_read2(chip,254)))
+		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;
+
+	if(-1 == (dsr = chip_read2(chip,TDA9874A_DSR)))
+		return mode;
+	if(-1 == (nsr = chip_read2(chip,TDA9874A_NSR)))
+		return mode;
+	if(-1 == (necr = chip_read2(chip,TDA9874A_NECR)))
+		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 */
+
+	if(-1 == (dic = chip_read2(chip,TDA9874A_DIC)))
+		return 0;
+	if(-1 == (sic = chip_read2(chip,TDA9874A_SIC)))
+		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);
+	rev = chip_read2(chip, 255);
+
+	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;
+
+	val = chip_read(chip);
+	mode = V4L2_TUNER_SUB_MONO;
+	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);
+	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,
+		.leftinit   = 58880,
+		.rightinit  = 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  = TEA6300_S_GMU,
+	},
+	{
+		.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_g_ctrl(struct v4l2_subdev *sd,
+			    struct v4l2_control *ctrl)
+{
+	struct CHIPSTATE *chip = to_state(sd);
+	struct CHIPDESC *desc = chip->desc;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUDIO_MUTE:
+		if (!(desc->flags & CHIP_HAS_INPUTSEL))
+			break;
+		ctrl->value=chip->muted;
+		return 0;
+	case V4L2_CID_AUDIO_VOLUME:
+		if (!(desc->flags & CHIP_HAS_VOLUME))
+			break;
+		ctrl->value = max(chip->left,chip->right);
+		return 0;
+	case V4L2_CID_AUDIO_BALANCE:
+	{
+		int volume;
+		if (!(desc->flags & CHIP_HAS_VOLUME))
+			break;
+		volume = max(chip->left,chip->right);
+		if (volume)
+			ctrl->value=(32768*min(chip->left,chip->right))/volume;
+		else
+			ctrl->value=32768;
+		return 0;
+	}
+	case V4L2_CID_AUDIO_BASS:
+		if (!(desc->flags & CHIP_HAS_BASSTREBLE))
+			break;
+		ctrl->value = chip->bass;
+		return 0;
+	case V4L2_CID_AUDIO_TREBLE:
+		if (!(desc->flags & CHIP_HAS_BASSTREBLE))
+			break;
+		ctrl->value = chip->treble;
+		return 0;
+	}
+	return -EINVAL;
+}
+
+static int tvaudio_s_ctrl(struct v4l2_subdev *sd,
+			    struct v4l2_control *ctrl)
+{
+	struct CHIPSTATE *chip = to_state(sd);
+	struct CHIPDESC *desc = chip->desc;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUDIO_MUTE:
+		if (!(desc->flags & CHIP_HAS_INPUTSEL))
+			break;
+
+		if (ctrl->value < 0 || ctrl->value >= 2)
+			return -ERANGE;
+		chip->muted = ctrl->value;
+		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:
+	{
+		int volume,balance;
+
+		if (!(desc->flags & CHIP_HAS_VOLUME))
+			break;
+
+		volume = max(chip->left,chip->right);
+		if (volume)
+			balance=(32768*min(chip->left,chip->right))/volume;
+		else
+			balance=32768;
+
+		volume=ctrl->value;
+		chip->left = (min(65536 - balance,32768) * volume) / 32768;
+		chip->right = (min(balance,volume *(__u16)32768)) / 32768;
+
+		chip_write(chip,desc->leftreg,desc->volfunc(chip->left));
+		chip_write(chip,desc->rightreg,desc->volfunc(chip->right));
+
+		return 0;
+	}
+	case V4L2_CID_AUDIO_BALANCE:
+	{
+		int volume, balance;
+
+		if (!(desc->flags & CHIP_HAS_VOLUME))
+			break;
+
+		volume = max(chip->left, chip->right);
+		balance = ctrl->value;
+		chip->left = (min(65536 - balance, 32768) * volume) / 32768;
+		chip->right = (min(balance, volume * (__u16)32768)) / 32768;
+
+		chip_write(chip, desc->leftreg, desc->volfunc(chip->left));
+		chip_write(chip, desc->rightreg, desc->volfunc(chip->right));
+
+		return 0;
+	}
+	case V4L2_CID_AUDIO_BASS:
+		if (!(desc->flags & CHIP_HAS_BASSTREBLE))
+			break;
+		chip->bass = ctrl->value;
+		chip_write(chip,desc->bassreg,desc->bassfunc(chip->bass));
+
+		return 0;
+	case V4L2_CID_AUDIO_TREBLE:
+		if (!(desc->flags & CHIP_HAS_BASSTREBLE))
+			break;
+		chip->treble = ctrl->value;
+		chip_write(chip,desc->treblereg,desc->treblefunc(chip->treble));
+
+		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_queryctrl(struct v4l2_subdev *sd, struct v4l2_queryctrl *qc)
+{
+	struct CHIPSTATE *chip = to_state(sd);
+	struct CHIPDESC *desc = chip->desc;
+
+	switch (qc->id) {
+	case V4L2_CID_AUDIO_MUTE:
+		if (desc->flags & CHIP_HAS_INPUTSEL)
+			return v4l2_ctrl_query_fill(qc, 0, 1, 1, 0);
+		break;
+	case V4L2_CID_AUDIO_VOLUME:
+		if (desc->flags & CHIP_HAS_VOLUME)
+			return v4l2_ctrl_query_fill(qc, 0, 65535, 65535 / 100, 58880);
+		break;
+	case V4L2_CID_AUDIO_BALANCE:
+		if (desc->flags & CHIP_HAS_VOLUME)
+			return v4l2_ctrl_query_fill(qc, 0, 65535, 65535 / 100, 32768);
+		break;
+	case V4L2_CID_AUDIO_BASS:
+	case V4L2_CID_AUDIO_TREBLE:
+		if (desc->flags & CHIP_HAS_BASSTREBLE)
+			return v4l2_ctrl_query_fill(qc, 0, 65535, 65535 / 100, 32768);
+		break;
+	default:
+		break;
+	}
+	return -EINVAL;
+}
+
+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, 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, 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_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ident *chip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_TVAUDIO, 0);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_core_ops tvaudio_core_ops = {
+	.g_chip_ident = tvaudio_g_chip_ident,
+	.queryctrl = tvaudio_queryctrl,
+	.g_ctrl = tvaudio_g_ctrl,
+	.s_ctrl = tvaudio_s_ctrl,
+	.s_std = tvaudio_s_std,
+};
+
+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_ops tvaudio_ops = {
+	.core = &tvaudio_core_ops,
+	.tuner = &tvaudio_tuner_ops,
+	.audio = &tvaudio_audio_ops,
+};
+
+/* ----------------------------------------------------------------------- */
+
+
+/* i2c registration                                                       */
+
+static int tvaudio_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	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("%s%s", (desc == chiplist) ? "" : ", ", desc->name);
+		printk("\n");
+	}
+
+	chip = kzalloc(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");
+		kfree(chip);
+		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)
+		strlcpy(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);
+
+	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->left  = desc->leftinit  ? desc->leftinit  : 65535;
+			chip->right = desc->rightinit ? desc->rightinit : 65535;
+			chip_write(chip, desc->leftreg,
+				   desc->volfunc(chip->left));
+			chip_write(chip, desc->rightreg,
+				   desc->volfunc(chip->right));
+		}
+	}
+	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 {
+			chip->treble = desc->trebleinit ?
+						desc->trebleinit : 32768;
+			chip->bass   = desc->bassinit   ?
+						desc->bassinit   : 32768;
+			chip_write(chip, desc->bassreg,
+				   desc->bassfunc(chip->bass));
+			chip_write(chip, desc->treblereg,
+				   desc->treblefunc(chip->treble));
+		}
+	}
+
+	chip->thread = NULL;
+	init_timer(&chip->wt);
+	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->wt.function = chip_thread_wake;
+		chip->wt.data     = (unsigned long)chip;
+		chip->thread = kthread_run(chip_thread, chip, client->name);
+		if (IS_ERR(chip->thread)) {
+			v4l2_warn(sd, "failed to create kthread\n");
+			chip->thread = NULL;
+		}
+	}
+	return 0;
+}
+
+static int 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);
+	kfree(chip);
+	return 0;
+}
+
+/* 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 = {
+		.owner	= THIS_MODULE,
+		.name	= "tvaudio",
+	},
+	.probe		= tvaudio_probe,
+	.remove		= tvaudio_remove,
+	.id_table	= tvaudio_id,
+};
+
+module_i2c_driver(tvaudio_driver);
diff --git a/drivers/media/i2c/tveeprom.c b/drivers/media/i2c/tveeprom.c
new file mode 100644
index 000000000000..3b6cf034976a
--- /dev/null
+++ b/drivers/media/i2c/tveeprom.c
@@ -0,0 +1,792 @@
+/*
+ * tveeprom - eeprom decoder for tvcard configuration eeproms
+ *
+ * Data and decoding routines shamelessly borrowed from bttv-cards.c
+ * eeprom access routine shamelessly borrowed from bttv-if.c
+ * which are:
+
+    Copyright (C) 1996,97,98 Ralph  Metzler (rjkm@thp.uni-koeln.de)
+			   & Marcus Metzler (mocm@thp.uni-koeln.de)
+    (c) 1999-2001 Gerd Knorr <kraxel@goldbach.in-berlin.de>
+
+ * Adjustments to fit a more general model and all bugs:
+
+	Copyright (C) 2003 John Klar <linpvr at projectplasma.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; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/videodev2.h>
+#include <linux/i2c.h>
+
+#include <media/tuner.h>
+#include <media/tveeprom.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-chip-ident.h>
+
+MODULE_DESCRIPTION("i2c Hauppauge eeprom decoder driver");
+MODULE_AUTHOR("John Klar");
+MODULE_LICENSE("GPL");
+
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "Debug level (0-1)");
+
+#define STRM(array, i) \
+	(i < sizeof(array) / sizeof(char *) ? array[i] : "unknown")
+
+#define tveeprom_info(fmt, arg...) \
+	v4l_printk(KERN_INFO, "tveeprom", c->adapter, c->addr, fmt , ## arg)
+#define tveeprom_warn(fmt, arg...) \
+	v4l_printk(KERN_WARNING, "tveeprom", c->adapter, c->addr, fmt , ## arg)
+#define tveeprom_dbg(fmt, arg...) do { \
+	if (debug) \
+		v4l_printk(KERN_DEBUG, "tveeprom", \
+				c->adapter, c->addr, fmt , ## arg); \
+	} while (0)
+
+/*
+ * The Hauppauge eeprom uses an 8bit field to determine which
+ * tuner formats the tuner supports.
+ */
+static struct HAUPPAUGE_TUNER_FMT
+{
+	int	id;
+	char *name;
+}
+hauppauge_tuner_fmt[] =
+{
+	{ V4L2_STD_UNKNOWN,                   " UNKNOWN" },
+	{ V4L2_STD_UNKNOWN,                   " FM" },
+	{ V4L2_STD_B|V4L2_STD_GH,             " PAL(B/G)" },
+	{ V4L2_STD_MN,                        " NTSC(M)" },
+	{ V4L2_STD_PAL_I,                     " PAL(I)" },
+	{ V4L2_STD_SECAM_L|V4L2_STD_SECAM_LC, " SECAM(L/L')" },
+	{ V4L2_STD_DK,                        " PAL(D/D1/K)" },
+	{ V4L2_STD_ATSC,                      " ATSC/DVB Digital" },
+};
+
+/* This is the full list of possible tuners. Many thanks to Hauppauge for
+   supplying this information. Note that many tuners where only used for
+   testing and never made it to the outside world. So you will only see
+   a subset in actual produced cards. */
+static struct HAUPPAUGE_TUNER
+{
+	int  id;
+	char *name;
+}
+hauppauge_tuner[] =
+{
+	/* 0-9 */
+	{ TUNER_ABSENT,        		"None" },
+	{ TUNER_ABSENT,        		"External" },
+	{ TUNER_ABSENT,        		"Unspecified" },
+	{ TUNER_PHILIPS_PAL,   		"Philips FI1216" },
+	{ TUNER_PHILIPS_SECAM, 		"Philips FI1216MF" },
+	{ TUNER_PHILIPS_NTSC,  		"Philips FI1236" },
+	{ TUNER_PHILIPS_PAL_I, 		"Philips FI1246" },
+	{ TUNER_PHILIPS_PAL_DK,		"Philips FI1256" },
+	{ TUNER_PHILIPS_PAL,   		"Philips FI1216 MK2" },
+	{ TUNER_PHILIPS_SECAM, 		"Philips FI1216MF MK2" },
+	/* 10-19 */
+	{ TUNER_PHILIPS_NTSC,  		"Philips FI1236 MK2" },
+	{ TUNER_PHILIPS_PAL_I, 		"Philips FI1246 MK2" },
+	{ TUNER_PHILIPS_PAL_DK,		"Philips FI1256 MK2" },
+	{ TUNER_TEMIC_NTSC,    		"Temic 4032FY5" },
+	{ TUNER_TEMIC_PAL,     		"Temic 4002FH5" },
+	{ TUNER_TEMIC_PAL_I,   		"Temic 4062FY5" },
+	{ TUNER_PHILIPS_PAL,   		"Philips FR1216 MK2" },
+	{ TUNER_PHILIPS_SECAM, 		"Philips FR1216MF MK2" },
+	{ TUNER_PHILIPS_NTSC,  		"Philips FR1236 MK2" },
+	{ TUNER_PHILIPS_PAL_I, 		"Philips FR1246 MK2" },
+	/* 20-29 */
+	{ TUNER_PHILIPS_PAL_DK,		"Philips FR1256 MK2" },
+	{ TUNER_PHILIPS_PAL,   		"Philips FM1216" },
+	{ TUNER_PHILIPS_SECAM, 		"Philips FM1216MF" },
+	{ TUNER_PHILIPS_NTSC,  		"Philips FM1236" },
+	{ TUNER_PHILIPS_PAL_I, 		"Philips FM1246" },
+	{ TUNER_PHILIPS_PAL_DK,		"Philips FM1256" },
+	{ TUNER_TEMIC_4036FY5_NTSC, 	"Temic 4036FY5" },
+	{ TUNER_ABSENT,        		"Samsung TCPN9082D" },
+	{ TUNER_ABSENT,        		"Samsung TCPM9092P" },
+	{ TUNER_TEMIC_4006FH5_PAL, 	"Temic 4006FH5" },
+	/* 30-39 */
+	{ TUNER_ABSENT,        		"Samsung TCPN9085D" },
+	{ TUNER_ABSENT,        		"Samsung TCPB9085P" },
+	{ TUNER_ABSENT,        		"Samsung TCPL9091P" },
+	{ TUNER_TEMIC_4039FR5_NTSC, 	"Temic 4039FR5" },
+	{ TUNER_PHILIPS_FQ1216ME,   	"Philips FQ1216 ME" },
+	{ TUNER_TEMIC_4066FY5_PAL_I, 	"Temic 4066FY5" },
+	{ TUNER_PHILIPS_NTSC,        	"Philips TD1536" },
+	{ TUNER_PHILIPS_NTSC,        	"Philips TD1536D" },
+	{ TUNER_PHILIPS_NTSC,  		"Philips FMR1236" }, /* mono radio */
+	{ TUNER_ABSENT,        		"Philips FI1256MP" },
+	/* 40-49 */
+	{ TUNER_ABSENT,        		"Samsung TCPQ9091P" },
+	{ TUNER_TEMIC_4006FN5_MULTI_PAL, "Temic 4006FN5" },
+	{ TUNER_TEMIC_4009FR5_PAL, 	"Temic 4009FR5" },
+	{ TUNER_TEMIC_4046FM5,     	"Temic 4046FM5" },
+	{ TUNER_TEMIC_4009FN5_MULTI_PAL_FM, "Temic 4009FN5" },
+	{ TUNER_ABSENT,        		"Philips TD1536D FH 44"},
+	{ TUNER_LG_NTSC_FM,    		"LG TP18NSR01F"},
+	{ TUNER_LG_PAL_FM,     		"LG TP18PSB01D"},
+	{ TUNER_LG_PAL,        		"LG TP18PSB11D"},
+	{ TUNER_LG_PAL_I_FM,   		"LG TAPC-I001D"},
+	/* 50-59 */
+	{ TUNER_LG_PAL_I,      		"LG TAPC-I701D"},
+	{ TUNER_ABSENT,       		"Temic 4042FI5"},
+	{ TUNER_MICROTUNE_4049FM5, 	"Microtune 4049 FM5"},
+	{ TUNER_ABSENT,        		"LG TPI8NSR11F"},
+	{ TUNER_ABSENT,        		"Microtune 4049 FM5 Alt I2C"},
+	{ TUNER_PHILIPS_FM1216ME_MK3, 	"Philips FQ1216ME MK3"},
+	{ TUNER_ABSENT,        		"Philips FI1236 MK3"},
+	{ TUNER_PHILIPS_FM1216ME_MK3, 	"Philips FM1216 ME MK3"},
+	{ TUNER_PHILIPS_FM1236_MK3, 	"Philips FM1236 MK3"},
+	{ TUNER_ABSENT,        		"Philips FM1216MP MK3"},
+	/* 60-69 */
+	{ TUNER_PHILIPS_FM1216ME_MK3, 	"LG S001D MK3"},
+	{ TUNER_ABSENT,        		"LG M001D MK3"},
+	{ TUNER_PHILIPS_FM1216ME_MK3, 	"LG S701D MK3"},
+	{ TUNER_ABSENT,        		"LG M701D MK3"},
+	{ TUNER_ABSENT,        		"Temic 4146FM5"},
+	{ TUNER_ABSENT,        		"Temic 4136FY5"},
+	{ TUNER_ABSENT,        		"Temic 4106FH5"},
+	{ TUNER_ABSENT,        		"Philips FQ1216LMP MK3"},
+	{ TUNER_LG_NTSC_TAPE,  		"LG TAPE H001F MK3"},
+	{ TUNER_LG_NTSC_TAPE,  		"LG TAPE H701F MK3"},
+	/* 70-79 */
+	{ TUNER_ABSENT,        		"LG TALN H200T"},
+	{ TUNER_ABSENT,        		"LG TALN H250T"},
+	{ TUNER_ABSENT,        		"LG TALN M200T"},
+	{ TUNER_ABSENT,        		"LG TALN Z200T"},
+	{ TUNER_ABSENT,        		"LG TALN S200T"},
+	{ TUNER_ABSENT,        		"Thompson DTT7595"},
+	{ TUNER_ABSENT,        		"Thompson DTT7592"},
+	{ TUNER_ABSENT,        		"Silicon TDA8275C1 8290"},
+	{ TUNER_ABSENT,        		"Silicon TDA8275C1 8290 FM"},
+	{ TUNER_ABSENT,        		"Thompson DTT757"},
+	/* 80-89 */
+	{ TUNER_PHILIPS_FQ1216LME_MK3, 	"Philips FQ1216LME MK3"},
+	{ TUNER_LG_PAL_NEW_TAPC, 	"LG TAPC G701D"},
+	{ TUNER_LG_NTSC_NEW_TAPC, 	"LG TAPC H791F"},
+	{ TUNER_LG_PAL_NEW_TAPC, 	"TCL 2002MB 3"},
+	{ TUNER_LG_PAL_NEW_TAPC, 	"TCL 2002MI 3"},
+	{ TUNER_TCL_2002N,     		"TCL 2002N 6A"},
+	{ TUNER_PHILIPS_FM1236_MK3, 	"Philips FQ1236 MK3"},
+	{ TUNER_SAMSUNG_TCPN_2121P30A, 	"Samsung TCPN 2121P30A"},
+	{ TUNER_ABSENT,        		"Samsung TCPE 4121P30A"},
+	{ TUNER_PHILIPS_FM1216ME_MK3, 	"TCL MFPE05 2"},
+	/* 90-99 */
+	{ TUNER_ABSENT,        		"LG TALN H202T"},
+	{ TUNER_PHILIPS_FQ1216AME_MK4, 	"Philips FQ1216AME MK4"},
+	{ TUNER_PHILIPS_FQ1236A_MK4, 	"Philips FQ1236A MK4"},
+	{ TUNER_ABSENT,       		"Philips FQ1286A MK4"},
+	{ TUNER_ABSENT,        		"Philips FQ1216ME MK5"},
+	{ TUNER_ABSENT,        		"Philips FQ1236 MK5"},
+	{ TUNER_SAMSUNG_TCPG_6121P30A, 	"Samsung TCPG 6121P30A"},
+	{ TUNER_TCL_2002MB,    		"TCL 2002MB_3H"},
+	{ TUNER_ABSENT,        		"TCL 2002MI_3H"},
+	{ TUNER_TCL_2002N,     		"TCL 2002N 5H"},
+	/* 100-109 */
+	{ TUNER_PHILIPS_FMD1216ME_MK3, 	"Philips FMD1216ME"},
+	{ TUNER_TEA5767,       		"Philips TEA5768HL FM Radio"},
+	{ TUNER_ABSENT,        		"Panasonic ENV57H12D5"},
+	{ TUNER_PHILIPS_FM1236_MK3, 	"TCL MFNM05-4"},
+	{ TUNER_PHILIPS_FM1236_MK3,	"TCL MNM05-4"},
+	{ TUNER_PHILIPS_FM1216ME_MK3, 	"TCL MPE05-2"},
+	{ TUNER_ABSENT,        		"TCL MQNM05-4"},
+	{ TUNER_ABSENT,        		"LG TAPC-W701D"},
+	{ TUNER_ABSENT,        		"TCL 9886P-WM"},
+	{ TUNER_ABSENT,        		"TCL 1676NM-WM"},
+	/* 110-119 */
+	{ TUNER_ABSENT,        		"Thompson DTT75105"},
+	{ TUNER_ABSENT,        		"Conexant_CX24109"},
+	{ TUNER_TCL_2002N,     		"TCL M2523_5N_E"},
+	{ TUNER_TCL_2002MB,    		"TCL M2523_3DB_E"},
+	{ TUNER_ABSENT,        		"Philips 8275A"},
+	{ TUNER_ABSENT,        		"Microtune MT2060"},
+	{ TUNER_PHILIPS_FM1236_MK3, 	"Philips FM1236 MK5"},
+	{ TUNER_PHILIPS_FM1216ME_MK3, 	"Philips FM1216ME MK5"},
+	{ TUNER_ABSENT,        		"TCL M2523_3DI_E"},
+	{ TUNER_ABSENT,        		"Samsung THPD5222FG30A"},
+	/* 120-129 */
+	{ TUNER_XC2028,        		"Xceive XC3028"},
+	{ TUNER_PHILIPS_FQ1216LME_MK3,	"Philips FQ1216LME MK5"},
+	{ TUNER_ABSENT,        		"Philips FQD1216LME"},
+	{ TUNER_ABSENT,        		"Conexant CX24118A"},
+	{ TUNER_ABSENT,        		"TCL DMF11WIP"},
+	{ TUNER_ABSENT,        		"TCL MFNM05_4H_E"},
+	{ TUNER_ABSENT,        		"TCL MNM05_4H_E"},
+	{ TUNER_ABSENT,        		"TCL MPE05_2H_E"},
+	{ TUNER_ABSENT,        		"TCL MQNM05_4_U"},
+	{ TUNER_ABSENT,        		"TCL M2523_5NH_E"},
+	/* 130-139 */
+	{ TUNER_ABSENT,        		"TCL M2523_3DBH_E"},
+	{ TUNER_ABSENT,        		"TCL M2523_3DIH_E"},
+	{ TUNER_ABSENT,        		"TCL MFPE05_2_U"},
+	{ TUNER_PHILIPS_FMD1216MEX_MK3,	"Philips FMD1216MEX"},
+	{ TUNER_ABSENT,        		"Philips FRH2036B"},
+	{ TUNER_ABSENT,        		"Panasonic ENGF75_01GF"},
+	{ TUNER_ABSENT,        		"MaxLinear MXL5005"},
+	{ TUNER_ABSENT,        		"MaxLinear MXL5003"},
+	{ TUNER_ABSENT,        		"Xceive XC2028"},
+	{ TUNER_ABSENT,        		"Microtune MT2131"},
+	/* 140-149 */
+	{ TUNER_ABSENT,        		"Philips 8275A_8295"},
+	{ TUNER_ABSENT,        		"TCL MF02GIP_5N_E"},
+	{ TUNER_ABSENT,        		"TCL MF02GIP_3DB_E"},
+	{ TUNER_ABSENT,        		"TCL MF02GIP_3DI_E"},
+	{ TUNER_ABSENT,        		"Microtune MT2266"},
+	{ TUNER_ABSENT,        		"TCL MF10WPP_4N_E"},
+	{ TUNER_ABSENT,        		"LG TAPQ_H702F"},
+	{ TUNER_ABSENT,        		"TCL M09WPP_4N_E"},
+	{ TUNER_ABSENT,        		"MaxLinear MXL5005_v2"},
+	{ TUNER_PHILIPS_TDA8290, 	"Philips 18271_8295"},
+	/* 150-159 */
+	{ TUNER_XC5000,                 "Xceive XC5000"},
+	{ TUNER_ABSENT,                 "Xceive XC3028L"},
+	{ TUNER_ABSENT,                 "NXP 18271C2_716x"},
+	{ TUNER_ABSENT,                 "Xceive XC4000"},
+	{ TUNER_ABSENT,                 "Dibcom 7070"},
+	{ TUNER_PHILIPS_TDA8290,        "NXP 18271C2"},
+	{ TUNER_ABSENT,                 "Siano SMS1010"},
+	{ TUNER_ABSENT,                 "Siano SMS1150"},
+	{ TUNER_ABSENT,                 "MaxLinear 5007"},
+	{ TUNER_ABSENT,                 "TCL M09WPP_2P_E"},
+	/* 160-169 */
+	{ TUNER_ABSENT,                 "Siano SMS1180"},
+	{ TUNER_ABSENT,                 "Maxim_MAX2165"},
+	{ TUNER_ABSENT,                 "Siano SMS1140"},
+	{ TUNER_ABSENT,                 "Siano SMS1150 B1"},
+	{ TUNER_ABSENT,                 "MaxLinear 111"},
+	{ TUNER_ABSENT,                 "Dibcom 7770"},
+	{ TUNER_ABSENT,                 "Siano SMS1180VNS"},
+	{ TUNER_ABSENT,                 "Siano SMS1184"},
+	{ TUNER_PHILIPS_FQ1236_MK5,	"TCL M30WTP-4N-E"},
+	{ TUNER_ABSENT,                 "TCL_M11WPP_2PN_E"},
+	/* 170-179 */
+	{ TUNER_ABSENT,                 "MaxLinear 301"},
+	{ TUNER_ABSENT,                 "Mirics MSi001"},
+	{ TUNER_ABSENT,                 "MaxLinear MxL241SF"},
+	{ TUNER_XC5000C,                "Xceive XC5000C"},
+	{ TUNER_ABSENT,                 "Montage M68TS2020"},
+	{ TUNER_ABSENT,                 "Siano SMS1530"},
+	{ TUNER_ABSENT,                 "Dibcom 7090"},
+	{ TUNER_ABSENT,                 "Xceive XC5200C"},
+	{ TUNER_ABSENT,                 "NXP 18273"},
+	{ TUNER_ABSENT,                 "Montage M88TS2022"},
+	/* 180-189 */
+	{ TUNER_ABSENT,                 "NXP 18272M"},
+	{ TUNER_ABSENT,                 "NXP 18272S"},
+};
+
+/* Use V4L2_IDENT_AMBIGUOUS for those audio 'chips' that are
+ * internal to a video chip, i.e. not a separate audio chip. */
+static struct HAUPPAUGE_AUDIOIC
+{
+	u32   id;
+	char *name;
+}
+audioIC[] =
+{
+	/* 0-4 */
+	{ V4L2_IDENT_NONE,      "None"      },
+	{ V4L2_IDENT_UNKNOWN,   "TEA6300"   },
+	{ V4L2_IDENT_UNKNOWN,   "TEA6320"   },
+	{ V4L2_IDENT_UNKNOWN,   "TDA9850"   },
+	{ V4L2_IDENT_MSPX4XX,   "MSP3400C"  },
+	/* 5-9 */
+	{ V4L2_IDENT_MSPX4XX,   "MSP3410D"  },
+	{ V4L2_IDENT_MSPX4XX,   "MSP3415"   },
+	{ V4L2_IDENT_MSPX4XX,   "MSP3430"   },
+	{ V4L2_IDENT_MSPX4XX,   "MSP3438"   },
+	{ V4L2_IDENT_UNKNOWN,   "CS5331"    },
+	/* 10-14 */
+	{ V4L2_IDENT_MSPX4XX,   "MSP3435"   },
+	{ V4L2_IDENT_MSPX4XX,   "MSP3440"   },
+	{ V4L2_IDENT_MSPX4XX,   "MSP3445"   },
+	{ V4L2_IDENT_MSPX4XX,   "MSP3411"   },
+	{ V4L2_IDENT_MSPX4XX,   "MSP3416"   },
+	/* 15-19 */
+	{ V4L2_IDENT_MSPX4XX,   "MSP3425"   },
+	{ V4L2_IDENT_MSPX4XX,   "MSP3451"   },
+	{ V4L2_IDENT_MSPX4XX,   "MSP3418"   },
+	{ V4L2_IDENT_UNKNOWN,   "Type 0x12" },
+	{ V4L2_IDENT_UNKNOWN,   "OKI7716"   },
+	/* 20-24 */
+	{ V4L2_IDENT_MSPX4XX,   "MSP4410"   },
+	{ V4L2_IDENT_MSPX4XX,   "MSP4420"   },
+	{ V4L2_IDENT_MSPX4XX,   "MSP4440"   },
+	{ V4L2_IDENT_MSPX4XX,   "MSP4450"   },
+	{ V4L2_IDENT_MSPX4XX,   "MSP4408"   },
+	/* 25-29 */
+	{ V4L2_IDENT_MSPX4XX,   "MSP4418"   },
+	{ V4L2_IDENT_MSPX4XX,   "MSP4428"   },
+	{ V4L2_IDENT_MSPX4XX,   "MSP4448"   },
+	{ V4L2_IDENT_MSPX4XX,   "MSP4458"   },
+	{ V4L2_IDENT_MSPX4XX,   "Type 0x1d" },
+	/* 30-34 */
+	{ V4L2_IDENT_AMBIGUOUS, "CX880"     },
+	{ V4L2_IDENT_AMBIGUOUS, "CX881"     },
+	{ V4L2_IDENT_AMBIGUOUS, "CX883"     },
+	{ V4L2_IDENT_AMBIGUOUS, "CX882"     },
+	{ V4L2_IDENT_AMBIGUOUS, "CX25840"   },
+	/* 35-39 */
+	{ V4L2_IDENT_AMBIGUOUS, "CX25841"   },
+	{ V4L2_IDENT_AMBIGUOUS, "CX25842"   },
+	{ V4L2_IDENT_AMBIGUOUS, "CX25843"   },
+	{ V4L2_IDENT_AMBIGUOUS, "CX23418"   },
+	{ V4L2_IDENT_AMBIGUOUS, "CX23885"   },
+	/* 40-44 */
+	{ V4L2_IDENT_AMBIGUOUS, "CX23888"   },
+	{ V4L2_IDENT_AMBIGUOUS, "SAA7131"   },
+	{ V4L2_IDENT_AMBIGUOUS, "CX23887"   },
+	{ V4L2_IDENT_AMBIGUOUS, "SAA7164"   },
+	{ V4L2_IDENT_AMBIGUOUS, "AU8522"    },
+};
+
+/* This list is supplied by Hauppauge. Thanks! */
+static const char *decoderIC[] = {
+	/* 0-4 */
+	"None", "BT815", "BT817", "BT819", "BT815A",
+	/* 5-9 */
+	"BT817A", "BT819A", "BT827", "BT829", "BT848",
+	/* 10-14 */
+	"BT848A", "BT849A", "BT829A", "BT827A", "BT878",
+	/* 15-19 */
+	"BT879", "BT880", "VPX3226E", "SAA7114", "SAA7115",
+	/* 20-24 */
+	"CX880", "CX881", "CX883", "SAA7111", "SAA7113",
+	/* 25-29 */
+	"CX882", "TVP5150A", "CX25840", "CX25841", "CX25842",
+	/* 30-34 */
+	"CX25843", "CX23418", "NEC61153", "CX23885", "CX23888",
+	/* 35-39 */
+	"SAA7131", "CX25837", "CX23887", "CX23885A", "CX23887A",
+	/* 40-42 */
+	"SAA7164", "CX23885B", "AU8522"
+};
+
+static int hasRadioTuner(int tunerType)
+{
+	switch (tunerType) {
+	case 18: /* PNPEnv_TUNER_FR1236_MK2 */
+	case 23: /* PNPEnv_TUNER_FM1236 */
+	case 38: /* PNPEnv_TUNER_FMR1236 */
+	case 16: /* PNPEnv_TUNER_FR1216_MK2 */
+	case 19: /* PNPEnv_TUNER_FR1246_MK2 */
+	case 21: /* PNPEnv_TUNER_FM1216 */
+	case 24: /* PNPEnv_TUNER_FM1246 */
+	case 17: /* PNPEnv_TUNER_FR1216MF_MK2 */
+	case 22: /* PNPEnv_TUNER_FM1216MF */
+	case 20: /* PNPEnv_TUNER_FR1256_MK2 */
+	case 25: /* PNPEnv_TUNER_FM1256 */
+	case 33: /* PNPEnv_TUNER_4039FR5 */
+	case 42: /* PNPEnv_TUNER_4009FR5 */
+	case 52: /* PNPEnv_TUNER_4049FM5 */
+	case 54: /* PNPEnv_TUNER_4049FM5_AltI2C */
+	case 44: /* PNPEnv_TUNER_4009FN5 */
+	case 31: /* PNPEnv_TUNER_TCPB9085P */
+	case 30: /* PNPEnv_TUNER_TCPN9085D */
+	case 46: /* PNPEnv_TUNER_TP18NSR01F */
+	case 47: /* PNPEnv_TUNER_TP18PSB01D */
+	case 49: /* PNPEnv_TUNER_TAPC_I001D */
+	case 60: /* PNPEnv_TUNER_TAPE_S001D_MK3 */
+	case 57: /* PNPEnv_TUNER_FM1216ME_MK3 */
+	case 59: /* PNPEnv_TUNER_FM1216MP_MK3 */
+	case 58: /* PNPEnv_TUNER_FM1236_MK3 */
+	case 68: /* PNPEnv_TUNER_TAPE_H001F_MK3 */
+	case 61: /* PNPEnv_TUNER_TAPE_M001D_MK3 */
+	case 78: /* PNPEnv_TUNER_TDA8275C1_8290_FM */
+	case 89: /* PNPEnv_TUNER_TCL_MFPE05_2 */
+	case 92: /* PNPEnv_TUNER_PHILIPS_FQ1236A_MK4 */
+	case 105:
+		return 1;
+	}
+	return 0;
+}
+
+void tveeprom_hauppauge_analog(struct i2c_client *c, struct tveeprom *tvee,
+				unsigned char *eeprom_data)
+{
+	/* ----------------------------------------------
+	** The hauppauge eeprom format is tagged
+	**
+	** if packet[0] == 0x84, then packet[0..1] == length
+	** else length = packet[0] & 3f;
+	** if packet[0] & f8 == f8, then EOD and packet[1] == checksum
+	**
+	** In our (ivtv) case we're interested in the following:
+	** tuner type:   tag [00].05 or [0a].01 (index into hauppauge_tuner)
+	** tuner fmts:   tag [00].04 or [0a].00 (bitmask index into
+	**		 hauppauge_tuner_fmt)
+	** radio:        tag [00].{last} or [0e].00  (bitmask.  bit2=FM)
+	** audio proc:   tag [02].01 or [05].00 (mask with 0x7f)
+	** decoder proc: tag [09].01)
+
+	** Fun info:
+	** model:      tag [00].07-08 or [06].00-01
+	** revision:   tag [00].09-0b or [06].04-06
+	** serial#:    tag [01].05-07 or [04].04-06
+
+	** # of inputs/outputs ???
+	*/
+
+	int i, j, len, done, beenhere, tag, start;
+
+	int tuner1 = 0, t_format1 = 0, audioic = -1;
+	char *t_name1 = NULL;
+	const char *t_fmt_name1[8] = { " none", "", "", "", "", "", "", "" };
+
+	int tuner2 = 0, t_format2 = 0;
+	char *t_name2 = NULL;
+	const char *t_fmt_name2[8] = { " none", "", "", "", "", "", "", "" };
+
+	memset(tvee, 0, sizeof(*tvee));
+	tvee->tuner_type = TUNER_ABSENT;
+	tvee->tuner2_type = TUNER_ABSENT;
+
+	done = len = beenhere = 0;
+
+	/* Different eeprom start offsets for em28xx, cx2388x and cx23418 */
+	if (eeprom_data[0] == 0x1a &&
+	    eeprom_data[1] == 0xeb &&
+	    eeprom_data[2] == 0x67 &&
+	    eeprom_data[3] == 0x95)
+		start = 0xa0; /* Generic em28xx offset */
+	else if ((eeprom_data[0] & 0xe1) == 0x01 &&
+		 eeprom_data[1] == 0x00 &&
+		 eeprom_data[2] == 0x00 &&
+		 eeprom_data[8] == 0x84)
+		start = 8; /* Generic cx2388x offset */
+	else if (eeprom_data[1] == 0x70 &&
+		 eeprom_data[2] == 0x00 &&
+		 eeprom_data[4] == 0x74 &&
+		 eeprom_data[8] == 0x84)
+		start = 8; /* Generic cx23418 offset (models 74xxx) */
+	else
+		start = 0;
+
+	for (i = start; !done && i < 256; i += len) {
+		if (eeprom_data[i] == 0x84) {
+			len = eeprom_data[i + 1] + (eeprom_data[i + 2] << 8);
+			i += 3;
+		} else if ((eeprom_data[i] & 0xf0) == 0x70) {
+			if (eeprom_data[i] & 0x08) {
+				/* verify checksum! */
+				done = 1;
+				break;
+			}
+			len = eeprom_data[i] & 0x07;
+			++i;
+		} else {
+			tveeprom_warn("Encountered bad packet header [%02x]. "
+				"Corrupt or not a Hauppauge eeprom.\n",
+				eeprom_data[i]);
+			return;
+		}
+
+		if (debug) {
+			tveeprom_info("Tag [%02x] + %d bytes:",
+					eeprom_data[i], len - 1);
+			for (j = 1; j < len; j++)
+				printk(KERN_CONT " %02x", eeprom_data[i + j]);
+			printk(KERN_CONT "\n");
+		}
+
+		/* process by tag */
+		tag = eeprom_data[i];
+		switch (tag) {
+		case 0x00:
+			/* tag: 'Comprehensive' */
+			tuner1 = eeprom_data[i+6];
+			t_format1 = eeprom_data[i+5];
+			tvee->has_radio = eeprom_data[i+len-1];
+			/* old style tag, don't know how to detect
+			IR presence, mark as unknown. */
+			tvee->has_ir = 0;
+			tvee->model =
+				eeprom_data[i+8] +
+				(eeprom_data[i+9] << 8);
+			tvee->revision = eeprom_data[i+10] +
+				(eeprom_data[i+11] << 8) +
+				(eeprom_data[i+12] << 16);
+			break;
+
+		case 0x01:
+			/* tag: 'SerialID' */
+			tvee->serial_number =
+				eeprom_data[i+6] +
+				(eeprom_data[i+7] << 8) +
+				(eeprom_data[i+8] << 16);
+			break;
+
+		case 0x02:
+			/* tag 'AudioInfo'
+			Note mask with 0x7F, high bit used on some older models
+			to indicate 4052 mux was removed in favor of using MSP
+			inputs directly. */
+			audioic = eeprom_data[i+2] & 0x7f;
+			if (audioic < ARRAY_SIZE(audioIC))
+				tvee->audio_processor = audioIC[audioic].id;
+			else
+				tvee->audio_processor = V4L2_IDENT_UNKNOWN;
+			break;
+
+		/* case 0x03: tag 'EEInfo' */
+
+		case 0x04:
+			/* tag 'SerialID2' */
+			tvee->serial_number =
+				eeprom_data[i+5] +
+				(eeprom_data[i+6] << 8) +
+				(eeprom_data[i+7] << 16);
+
+			if ((eeprom_data[i + 8] & 0xf0) &&
+					(tvee->serial_number < 0xffffff)) {
+				tvee->MAC_address[0] = 0x00;
+				tvee->MAC_address[1] = 0x0D;
+				tvee->MAC_address[2] = 0xFE;
+				tvee->MAC_address[3] = eeprom_data[i + 7];
+				tvee->MAC_address[4] = eeprom_data[i + 6];
+				tvee->MAC_address[5] = eeprom_data[i + 5];
+				tvee->has_MAC_address = 1;
+			}
+			break;
+
+		case 0x05:
+			/* tag 'Audio2'
+			Note mask with 0x7F, high bit used on some older models
+			to indicate 4052 mux was removed in favor of using MSP
+			inputs directly. */
+			audioic = eeprom_data[i+1] & 0x7f;
+			if (audioic < ARRAY_SIZE(audioIC))
+				tvee->audio_processor = audioIC[audioic].id;
+			else
+				tvee->audio_processor = V4L2_IDENT_UNKNOWN;
+
+			break;
+
+		case 0x06:
+			/* tag 'ModelRev' */
+			tvee->model =
+				eeprom_data[i + 1] +
+				(eeprom_data[i + 2] << 8) +
+				(eeprom_data[i + 3] << 16) +
+				(eeprom_data[i + 4] << 24);
+			tvee->revision =
+				eeprom_data[i + 5] +
+				(eeprom_data[i + 6] << 8) +
+				(eeprom_data[i + 7] << 16);
+			break;
+
+		case 0x07:
+			/* tag 'Details': according to Hauppauge not interesting
+			on any PCI-era or later boards. */
+			break;
+
+		/* there is no tag 0x08 defined */
+
+		case 0x09:
+			/* tag 'Video' */
+			tvee->decoder_processor = eeprom_data[i + 1];
+			break;
+
+		case 0x0a:
+			/* tag 'Tuner' */
+			if (beenhere == 0) {
+				tuner1 = eeprom_data[i + 2];
+				t_format1 = eeprom_data[i + 1];
+				beenhere = 1;
+			} else {
+				/* a second (radio) tuner may be present */
+				tuner2 = eeprom_data[i + 2];
+				t_format2 = eeprom_data[i + 1];
+				/* not a TV tuner? */
+				if (t_format2 == 0)
+					tvee->has_radio = 1; /* must be radio */
+			}
+			break;
+
+		case 0x0b:
+			/* tag 'Inputs': according to Hauppauge this is specific
+			to each driver family, so no good assumptions can be
+			made. */
+			break;
+
+		/* case 0x0c: tag 'Balun' */
+		/* case 0x0d: tag 'Teletext' */
+
+		case 0x0e:
+			/* tag: 'Radio' */
+			tvee->has_radio = eeprom_data[i+1];
+			break;
+
+		case 0x0f:
+			/* tag 'IRInfo' */
+			tvee->has_ir = 1 | (eeprom_data[i+1] << 1);
+			break;
+
+		/* case 0x10: tag 'VBIInfo' */
+		/* case 0x11: tag 'QCInfo' */
+		/* case 0x12: tag 'InfoBits' */
+
+		default:
+			tveeprom_dbg("Not sure what to do with tag [%02x]\n",
+					tag);
+			/* dump the rest of the packet? */
+		}
+	}
+
+	if (!done) {
+		tveeprom_warn("Ran out of data!\n");
+		return;
+	}
+
+	if (tvee->revision != 0) {
+		tvee->rev_str[0] = 32 + ((tvee->revision >> 18) & 0x3f);
+		tvee->rev_str[1] = 32 + ((tvee->revision >> 12) & 0x3f);
+		tvee->rev_str[2] = 32 + ((tvee->revision >>  6) & 0x3f);
+		tvee->rev_str[3] = 32 + (tvee->revision & 0x3f);
+		tvee->rev_str[4] = 0;
+	}
+
+	if (hasRadioTuner(tuner1) && !tvee->has_radio) {
+		tveeprom_info("The eeprom says no radio is present, but the tuner type\n");
+		tveeprom_info("indicates otherwise. I will assume that radio is present.\n");
+		tvee->has_radio = 1;
+	}
+
+	if (tuner1 < ARRAY_SIZE(hauppauge_tuner)) {
+		tvee->tuner_type = hauppauge_tuner[tuner1].id;
+		t_name1 = hauppauge_tuner[tuner1].name;
+	} else {
+		t_name1 = "unknown";
+	}
+
+	if (tuner2 < ARRAY_SIZE(hauppauge_tuner)) {
+		tvee->tuner2_type = hauppauge_tuner[tuner2].id;
+		t_name2 = hauppauge_tuner[tuner2].name;
+	} else {
+		t_name2 = "unknown";
+	}
+
+	tvee->tuner_hauppauge_model = tuner1;
+	tvee->tuner2_hauppauge_model = tuner2;
+	tvee->tuner_formats = 0;
+	tvee->tuner2_formats = 0;
+	for (i = j = 0; i < 8; i++) {
+		if (t_format1 & (1 << i)) {
+			tvee->tuner_formats |= hauppauge_tuner_fmt[i].id;
+			t_fmt_name1[j++] = hauppauge_tuner_fmt[i].name;
+		}
+	}
+	for (i = j = 0; i < 8; i++) {
+		if (t_format2 & (1 << i)) {
+			tvee->tuner2_formats |= hauppauge_tuner_fmt[i].id;
+			t_fmt_name2[j++] = hauppauge_tuner_fmt[i].name;
+		}
+	}
+
+	tveeprom_info("Hauppauge model %d, rev %s, serial# %d\n",
+		tvee->model, tvee->rev_str, tvee->serial_number);
+	if (tvee->has_MAC_address == 1)
+		tveeprom_info("MAC address is %pM\n", tvee->MAC_address);
+	tveeprom_info("tuner model is %s (idx %d, type %d)\n",
+		t_name1, tuner1, tvee->tuner_type);
+	tveeprom_info("TV standards%s%s%s%s%s%s%s%s (eeprom 0x%02x)\n",
+		t_fmt_name1[0], t_fmt_name1[1], t_fmt_name1[2],
+		t_fmt_name1[3],	t_fmt_name1[4], t_fmt_name1[5],
+		t_fmt_name1[6], t_fmt_name1[7],	t_format1);
+	if (tuner2)
+		tveeprom_info("second tuner model is %s (idx %d, type %d)\n",
+					t_name2, tuner2, tvee->tuner2_type);
+	if (t_format2)
+		tveeprom_info("TV standards%s%s%s%s%s%s%s%s (eeprom 0x%02x)\n",
+			t_fmt_name2[0], t_fmt_name2[1], t_fmt_name2[2],
+			t_fmt_name2[3],	t_fmt_name2[4], t_fmt_name2[5],
+			t_fmt_name2[6], t_fmt_name2[7], t_format2);
+	if (audioic < 0) {
+		tveeprom_info("audio processor is unknown (no idx)\n");
+		tvee->audio_processor = V4L2_IDENT_UNKNOWN;
+	} else {
+		if (audioic < ARRAY_SIZE(audioIC))
+			tveeprom_info("audio processor is %s (idx %d)\n",
+					audioIC[audioic].name, audioic);
+		else
+			tveeprom_info("audio processor is unknown (idx %d)\n",
+								audioic);
+	}
+	if (tvee->decoder_processor)
+		tveeprom_info("decoder processor is %s (idx %d)\n",
+			STRM(decoderIC, tvee->decoder_processor),
+			tvee->decoder_processor);
+	if (tvee->has_ir)
+		tveeprom_info("has %sradio, has %sIR receiver, has %sIR transmitter\n",
+				tvee->has_radio ? "" : "no ",
+				(tvee->has_ir & 2) ? "" : "no ",
+				(tvee->has_ir & 4) ? "" : "no ");
+	else
+		tveeprom_info("has %sradio\n",
+				tvee->has_radio ? "" : "no ");
+}
+EXPORT_SYMBOL(tveeprom_hauppauge_analog);
+
+/* ----------------------------------------------------------------------- */
+/* generic helper functions                                                */
+
+int tveeprom_read(struct i2c_client *c, unsigned char *eedata, int len)
+{
+	unsigned char buf;
+	int err;
+
+	buf = 0;
+	err = i2c_master_send(c, &buf, 1);
+	if (err != 1) {
+		tveeprom_info("Huh, no eeprom present (err=%d)?\n", err);
+		return -1;
+	}
+	err = i2c_master_recv(c, eedata, len);
+	if (err != len) {
+		tveeprom_warn("i2c eeprom read error (err=%d)\n", err);
+		return -1;
+	}
+	if (debug) {
+		int i;
+
+		tveeprom_info("full 256-byte eeprom dump:\n");
+		for (i = 0; i < len; i++) {
+			if (0 == (i % 16))
+				tveeprom_info("%02x:", i);
+			printk(KERN_CONT " %02x", eedata[i]);
+			if (15 == (i % 16))
+				printk(KERN_CONT "\n");
+		}
+	}
+	return 0;
+}
+EXPORT_SYMBOL(tveeprom_read);
+
+/*
+ * Local variables:
+ * c-basic-offset: 8
+ * End:
+ */
diff --git a/drivers/media/i2c/tvp514x.c b/drivers/media/i2c/tvp514x.c
new file mode 100644
index 000000000000..1f3943bb87d5
--- /dev/null
+++ b/drivers/media/i2c/tvp514x.c
@@ -0,0 +1,1166 @@
+/*
+ * 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>
+ *
+ * This package is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ */
+
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/videodev2.h>
+#include <linux/module.h>
+
+#include <media/v4l2-device.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-mediabus.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-ctrls.h>
+#include <media/tvp514x.h>
+
+#include "tvp514x_regs.h"
+
+/* Module Name */
+#define TVP514X_MODULE_NAME		"tvp514x"
+
+/* 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 informations
+ * @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
+ * @tvp514x_regs: copy of hw's regs with preset values.
+ * @pdata: Board specific
+ * @ver: Chip version
+ * @streaming: TVP5146/47 decoder streaming - enabled or disabled.
+ * @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
+ */
+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;
+
+	enum tvp514x_std current_std;
+	int num_stds;
+	const struct tvp514x_std_info *std_list;
+	/* Input and Output Routing parameters */
+	u32 input;
+	u32 output;
+};
+
+/* 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},
+};
+
+/**
+ * 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;
+
+	*std_id = V4L2_STD_UNKNOWN;
+
+	/* query the current standard */
+	current_std = tvp514x_query_current_std(sd);
+	if (current_std == STD_INVALID)
+		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))
+		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;
+	u8 sync_lock_status, lock_mask;
+	int try_count = LOCK_RETRY_COUNT;
+
+	if ((input >= INPUT_INVALID) ||
+			(output >= OUTPUT_INVALID))
+		/* Index out of bound */
+		return -EINVAL;
+
+	/*
+	 * For the sequence streamon -> streamoff and again s_input
+	 * it fails to lock the signal, since streamoff puts TVP514x
+	 * into power off state which leads to failure in sub-sequent s_input.
+	 *
+	 * So power up the TVP514x device here, since it is important to lock
+	 * the signal at this stage.
+	 */
+	if (!decoder->streaming)
+		tvp514x_s_stream(sd, 1);
+
+	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;
+
+	/* Clear status */
+	msleep(LOCK_RETRY_DELAY);
+	err =
+	    tvp514x_write_reg(sd, REG_CLEAR_LOST_LOCK, 0x01);
+	if (err)
+		return err;
+
+	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;
+	}
+
+	while (try_count-- > 0) {
+		/* Allow decoder to sync up with new input */
+		msleep(LOCK_RETRY_DELAY);
+
+		sync_lock_status = tvp514x_read_reg(sd,
+				REG_STATUS1);
+		if (lock_mask == (sync_lock_status & lock_mask))
+			/* Input detected */
+			break;
+	}
+
+	if (try_count < 0)
+		return -EINVAL;
+
+	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_enum_mbus_fmt() - V4L2 decoder interface handler for enum_mbus_fmt
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @index: index of pixelcode to retrieve
+ * @code: receives the pixelcode
+ *
+ * Enumerates supported mediabus formats
+ */
+static int
+tvp514x_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned index,
+					enum v4l2_mbus_pixelcode *code)
+{
+	if (index)
+		return -EINVAL;
+
+	*code = V4L2_MBUS_FMT_YUYV10_2X10;
+	return 0;
+}
+
+/**
+ * tvp514x_mbus_fmt_cap() - V4L2 decoder interface handler for try/s/g_mbus_fmt
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @f: pointer to the mediabus format structure
+ *
+ * Negotiates the image capture size and mediabus format.
+ */
+static int
+tvp514x_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *f)
+{
+	struct tvp514x_decoder *decoder = to_decoder(sd);
+	enum tvp514x_std current_std;
+
+	if (f == NULL)
+		return -EINVAL;
+
+	/* Calculate height and width based on current standard */
+	current_std = decoder->current_std;
+
+	f->code = V4L2_MBUS_FMT_YUYV10_2X10;
+	f->width = decoder->std_list[current_std].width;
+	f->height = decoder->std_list[current_std].height;
+	f->field = V4L2_FIELD_INTERLACED;
+	f->colorspace = V4L2_COLORSPACE_SMPTE170M;
+
+	v4l2_dbg(1, debug, sd, "MBUS_FMT: Width - %d, Height - %d\n",
+			f->width, f->height);
+	return 0;
+}
+
+/**
+ * tvp514x_g_parm() - V4L2 decoder interface handler for g_parm
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @a: pointer to standard V4L2 VIDIOC_G_PARM ioctl structure
+ *
+ * Returns the decoder's video CAPTURE parameters.
+ */
+static int
+tvp514x_g_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *a)
+{
+	struct tvp514x_decoder *decoder = to_decoder(sd);
+	struct v4l2_captureparm *cparm;
+	enum tvp514x_std current_std;
+
+	if (a == NULL)
+		return -EINVAL;
+
+	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		/* only capture is supported */
+		return -EINVAL;
+
+	/* get the current standard */
+	current_std = decoder->current_std;
+
+	cparm = &a->parm.capture;
+	cparm->capability = V4L2_CAP_TIMEPERFRAME;
+	cparm->timeperframe =
+		decoder->std_list[current_std].standard.frameperiod;
+
+	return 0;
+}
+
+/**
+ * tvp514x_s_parm() - V4L2 decoder interface handler for s_parm
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @a: pointer to standard V4L2 VIDIOC_S_PARM ioctl structure
+ *
+ * Configures the decoder to use the input parameters, if possible. If
+ * not possible, returns the appropriate error code.
+ */
+static int
+tvp514x_s_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *a)
+{
+	struct tvp514x_decoder *decoder = to_decoder(sd);
+	struct v4l2_fract *timeperframe;
+	enum tvp514x_std current_std;
+
+	if (a == NULL)
+		return -EINVAL;
+
+	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		/* only capture is supported */
+		return -EINVAL;
+
+	timeperframe = &a->parm.capture.timeperframe;
+
+	/* 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 i2c_client *client = v4l2_get_subdevdata(sd);
+	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:
+	{
+		struct tvp514x_reg *int_seq = (struct tvp514x_reg *)
+				client->driver->id_table->driver_data;
+
+		/* Power Up Sequence */
+		err = tvp514x_write_regs(sd, 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,
+};
+
+static const struct v4l2_subdev_core_ops tvp514x_core_ops = {
+	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
+	.try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
+	.s_ext_ctrls = v4l2_subdev_s_ext_ctrls,
+	.g_ctrl = v4l2_subdev_g_ctrl,
+	.s_ctrl = v4l2_subdev_s_ctrl,
+	.queryctrl = v4l2_subdev_queryctrl,
+	.querymenu = v4l2_subdev_querymenu,
+	.s_std = tvp514x_s_std,
+};
+
+static const struct v4l2_subdev_video_ops tvp514x_video_ops = {
+	.s_routing = tvp514x_s_routing,
+	.querystd = tvp514x_querystd,
+	.enum_mbus_fmt = tvp514x_enum_mbus_fmt,
+	.g_mbus_fmt = tvp514x_mbus_fmt,
+	.try_mbus_fmt = tvp514x_mbus_fmt,
+	.s_mbus_fmt = tvp514x_mbus_fmt,
+	.g_parm = tvp514x_g_parm,
+	.s_parm = tvp514x_s_parm,
+	.s_stream = tvp514x_s_stream,
+};
+
+static const struct v4l2_subdev_ops tvp514x_ops = {
+	.core = &tvp514x_core_ops,
+	.video = &tvp514x_video_ops,
+};
+
+static struct tvp514x_decoder tvp514x_dev = {
+	.streaming = 0,
+	.current_std = STD_NTSC_MJ,
+	.std_list = tvp514x_std_list,
+	.num_stds = ARRAY_SIZE(tvp514x_std_list),
+
+};
+
+/**
+ * tvp514x_probe() - decoder driver i2c probe handler
+ * @client: i2c driver client device structure
+ * @id: i2c driver id table
+ *
+ * Register decoder as an i2c client device and V4L2
+ * device.
+ */
+static int
+tvp514x_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	struct tvp514x_decoder *decoder;
+	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;
+
+	if (!client->dev.platform_data) {
+		v4l2_err(client, "No platform data!!\n");
+		return -ENODEV;
+	}
+
+	decoder = kzalloc(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));
+
+	/* Copy board specific information here */
+	decoder->pdata = client->dev.platform_data;
+
+	/**
+	 * 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);
+
+	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) {
+		int err = decoder->hdl.error;
+
+		v4l2_ctrl_handler_free(&decoder->hdl);
+		kfree(decoder);
+		return err;
+	}
+	v4l2_ctrl_handler_setup(&decoder->hdl);
+
+	v4l2_info(sd, "%s decoder driver registered !!\n", sd->name);
+
+	return 0;
+
+}
+
+/**
+ * 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 int tvp514x_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct tvp514x_decoder *decoder = to_decoder(sd);
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&decoder->hdl);
+	kfree(decoder);
+	return 0;
+}
+/* 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);
+
+static struct i2c_driver tvp514x_driver = {
+	.driver = {
+		.owner = THIS_MODULE,
+		.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 000000000000..d23aa2fbb9b1
--- /dev/null
+++ b/drivers/media/i2c/tvp514x_regs.h
@@ -0,0 +1,287 @@
+/*
+ * 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>
+ *
+ * This package is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ */
+
+#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 000000000000..31104a960652
--- /dev/null
+++ b/drivers/media/i2c/tvp5150.c
@@ -0,0 +1,1274 @@
+/*
+ * tvp5150 - Texas Instruments TVP5150A/AM1 video decoder driver
+ *
+ * Copyright (c) 2005,2006 Mauro Carvalho Chehab (mchehab@infradead.org)
+ * This code is placed under the terms of the GNU General Public License v2
+ */
+
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <linux/delay.h>
+#include <linux/module.h>
+#include <media/v4l2-device.h>
+#include <media/tvp5150.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-ctrls.h>
+
+#include "tvp5150_reg.h"
+
+#define TVP5150_H_MAX		720
+#define TVP5150_V_MAX_525_60	480
+#define TVP5150_V_MAX_OTHERS	576
+#define TVP5150_MAX_CROP_LEFT	511
+#define TVP5150_MAX_CROP_TOP	127
+#define TVP5150_CROP_SHIFT	2
+
+MODULE_DESCRIPTION("Texas Instruments TVP5150A video decoder driver");
+MODULE_AUTHOR("Mauro Carvalho Chehab");
+MODULE_LICENSE("GPL");
+
+
+static int debug;
+module_param(debug, int, 0);
+MODULE_PARM_DESC(debug, "Debug level (0-2)");
+
+struct tvp5150 {
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
+	struct v4l2_rect rect;
+
+	v4l2_std_id norm;	/* Current set standard */
+	u32 input;
+	u32 output;
+	int enable;
+};
+
+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 i2c_client *c = v4l2_get_subdevdata(sd);
+	unsigned char buffer[1];
+	int rc;
+
+	buffer[0] = addr;
+
+	rc = i2c_master_send(c, buffer, 1);
+	if (rc < 0) {
+		v4l2_err(sd, "i2c i/o error: rc == %d (should be 1)\n", rc);
+		return rc;
+	}
+
+	msleep(10);
+
+	rc = i2c_master_recv(c, buffer, 1);
+	if (rc < 0) {
+		v4l2_err(sd, "i2c i/o error: rc == %d (should be 1)\n", rc);
+		return rc;
+	}
+
+	v4l2_dbg(2, debug, sd, "tvp5150: read 0x%02x = 0x%02x\n", addr, buffer[0]);
+
+	return (buffer[0]);
+}
+
+static inline void tvp5150_write(struct v4l2_subdev *sd, unsigned char addr,
+				 unsigned char value)
+{
+	struct i2c_client *c = v4l2_get_subdevdata(sd);
+	unsigned char buffer[2];
+	int rc;
+
+	buffer[0] = addr;
+	buffer[1] = value;
+	v4l2_dbg(2, debug, sd, "tvp5150: writing 0x%02x 0x%02x\n", buffer[0], buffer[1]);
+	if (2 != (rc = i2c_master_send(c, buffer, 2)))
+		v4l2_dbg(0, debug, sd, "i2c i/o error: rc == %d (should be 2)\n", rc);
+}
+
+static void dump_reg_range(struct v4l2_subdev *sd, char *s, u8 init,
+				const u8 end, int max_line)
+{
+	int i = 0;
+
+	while (init != (u8)(end + 1)) {
+		if ((i % max_line) == 0) {
+			if (i > 0)
+				printk("\n");
+			printk("tvp5150: %s reg 0x%02x = ", s, init);
+		}
+		printk("%02x ", tvp5150_read(sd, init));
+
+		init++;
+		i++;
+	}
+	printk("\n");
+}
+
+static int tvp5150_log_status(struct v4l2_subdev *sd)
+{
+	printk("tvp5150: Video input source selection #1 = 0x%02x\n",
+			tvp5150_read(sd, TVP5150_VD_IN_SRC_SEL_1));
+	printk("tvp5150: Analog channel controls = 0x%02x\n",
+			tvp5150_read(sd, TVP5150_ANAL_CHL_CTL));
+	printk("tvp5150: Operation mode controls = 0x%02x\n",
+			tvp5150_read(sd, TVP5150_OP_MODE_CTL));
+	printk("tvp5150: Miscellaneous controls = 0x%02x\n",
+			tvp5150_read(sd, TVP5150_MISC_CTL));
+	printk("tvp5150: Autoswitch mask= 0x%02x\n",
+			tvp5150_read(sd, TVP5150_AUTOSW_MSK));
+	printk("tvp5150: Color killer threshold control = 0x%02x\n",
+			tvp5150_read(sd, TVP5150_COLOR_KIL_THSH_CTL));
+	printk("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));
+	printk("tvp5150: Brightness control = 0x%02x\n",
+			tvp5150_read(sd, TVP5150_BRIGHT_CTL));
+	printk("tvp5150: Color saturation control = 0x%02x\n",
+			tvp5150_read(sd, TVP5150_SATURATION_CTL));
+	printk("tvp5150: Hue control = 0x%02x\n",
+			tvp5150_read(sd, TVP5150_HUE_CTL));
+	printk("tvp5150: Contrast control = 0x%02x\n",
+			tvp5150_read(sd, TVP5150_CONTRAST_CTL));
+	printk("tvp5150: Outputs and data rates select = 0x%02x\n",
+			tvp5150_read(sd, TVP5150_DATA_RATE_SEL));
+	printk("tvp5150: Configuration shared pins = 0x%02x\n",
+			tvp5150_read(sd, TVP5150_CONF_SHARED_PIN));
+	printk("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));
+	printk("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));
+	printk("tvp5150: Genlock/RTC = 0x%02x\n",
+			tvp5150_read(sd, TVP5150_GENLOCK));
+	printk("tvp5150: Horizontal sync start = 0x%02x\n",
+			tvp5150_read(sd, TVP5150_HORIZ_SYNC_START));
+	printk("tvp5150: Vertical blanking start = 0x%02x\n",
+			tvp5150_read(sd, TVP5150_VERT_BLANKING_START));
+	printk("tvp5150: Vertical blanking stop = 0x%02x\n",
+			tvp5150_read(sd, TVP5150_VERT_BLANKING_STOP));
+	printk("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));
+	printk("tvp5150: Interrupt reset register B = 0x%02x\n",
+			tvp5150_read(sd, TVP5150_INT_RESET_REG_B));
+	printk("tvp5150: Interrupt enable register B = 0x%02x\n",
+			tvp5150_read(sd, TVP5150_INT_ENABLE_REG_B));
+	printk("tvp5150: Interrupt configuration register B = 0x%02x\n",
+			tvp5150_read(sd, TVP5150_INTT_CONFIG_REG_B));
+	printk("tvp5150: Video standard = 0x%02x\n",
+			tvp5150_read(sd, TVP5150_VIDEO_STD));
+	printk("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));
+	printk("tvp5150: Macrovision on counter = 0x%02x\n",
+			tvp5150_read(sd, TVP5150_MACROVISION_ON_CTR));
+	printk("tvp5150: Macrovision off counter = 0x%02x\n",
+			tvp5150_read(sd, TVP5150_MACROVISION_OFF_CTR));
+	printk("tvp5150: ITU-R BT.656.%d timing(TVP5150AM1 only)\n",
+			(tvp5150_read(sd, TVP5150_REV_SELECT) & 1) ? 3 : 4);
+	printk("tvp5150: Device ID = %02x%02x\n",
+			tvp5150_read(sd, TVP5150_MSB_DEV_ID),
+			tvp5150_read(sd, TVP5150_LSB_DEV_ID));
+	printk("tvp5150: ROM version = (hex) %02x.%02x\n",
+			tvp5150_read(sd, TVP5150_ROM_MAJOR_VER),
+			tvp5150_read(sd, TVP5150_ROM_MINOR_VER));
+	printk("tvp5150: Vertical line count = 0x%02x%02x\n",
+			tvp5150_read(sd, TVP5150_VERT_LN_COUNT_MSB),
+			tvp5150_read(sd, TVP5150_VERT_LN_COUNT_LSB));
+	printk("tvp5150: Interrupt status register B = 0x%02x\n",
+			tvp5150_read(sd, TVP5150_INT_STATUS_REG_B));
+	printk("tvp5150: Interrupt active register B = 0x%02x\n",
+			tvp5150_read(sd, TVP5150_INT_ACTIVE_REG_B));
+	printk("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);
+
+	printk("tvp5150: Teletext filter enable = 0x%02x\n",
+			tvp5150_read(sd, TVP5150_TELETEXT_FIL_ENA));
+	printk("tvp5150: Interrupt status register A = 0x%02x\n",
+			tvp5150_read(sd, TVP5150_INT_STATUS_REG_A));
+	printk("tvp5150: Interrupt enable register A = 0x%02x\n",
+			tvp5150_read(sd, TVP5150_INT_ENABLE_REG_A));
+	printk("tvp5150: Interrupt configuration = 0x%02x\n",
+			tvp5150_read(sd, TVP5150_INT_CONF));
+	printk("tvp5150: VDP status register = 0x%02x\n",
+			tvp5150_read(sd, TVP5150_VDP_STATUS_REG));
+	printk("tvp5150: FIFO word count = 0x%02x\n",
+			tvp5150_read(sd, TVP5150_FIFO_WORD_COUNT));
+	printk("tvp5150: FIFO interrupt threshold = 0x%02x\n",
+			tvp5150_read(sd, TVP5150_FIFO_INT_THRESHOLD));
+	printk("tvp5150: FIFO reset = 0x%02x\n",
+			tvp5150_read(sd, TVP5150_FIFO_RESET));
+	printk("tvp5150: Line number interrupt = 0x%02x\n",
+			tvp5150_read(sd, TVP5150_LINE_NUMBER_INT));
+	printk("tvp5150: Pixel alignment register = 0x%02x%02x\n",
+			tvp5150_read(sd, TVP5150_PIX_ALIGN_REG_HIGH),
+			tvp5150_read(sd, TVP5150_PIX_ALIGN_REG_LOW));
+	printk("tvp5150: FIFO output control = 0x%02x\n",
+			tvp5150_read(sd, TVP5150_FIFO_OUT_CTRL));
+	printk("tvp5150: Full field enable = 0x%02x\n",
+			tvp5150_read(sd, TVP5150_FULL_FIELD_ENA));
+	printk("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 inline void tvp5150_selmux(struct v4l2_subdev *sd)
+{
+	int opmode = 0;
+	struct tvp5150 *decoder = to_tvp5150(sd);
+	int input = 0;
+	int val;
+
+	if ((decoder->output & TVP5150_BLACK_SCREEN) || !decoder->enable)
+		input = 8;
+
+	switch (decoder->input) {
+	case TVP5150_COMPOSITE1:
+		input |= 2;
+		/* fall through */
+	case TVP5150_COMPOSITE0:
+		break;
+	case TVP5150_SVIDEO:
+	default:
+		input |= 1;
+		break;
+	}
+
+	v4l2_dbg(1, debug, sd, "Selecting video route: route input=%i, output=%i "
+			"=> tvp5150 input=%i, opmode=%i\n",
+			decoder->input, decoder->output,
+			input, opmode);
+
+	tvp5150_write(sd, TVP5150_OP_MODE_CTL, opmode);
+	tvp5150_write(sd, TVP5150_VD_IN_SRC_SEL_1, input);
+
+	/* Svideo should enable YCrCb output and disable GPCL output
+	 * For Composite and TV, it should be the reverse
+	 */
+	val = tvp5150_read(sd, TVP5150_MISC_CTL);
+	if (val < 0) {
+		v4l2_err(sd, "%s: failed with error = %d\n", __func__, val);
+		return;
+	}
+
+	if (decoder->input == TVP5150_SVIDEO)
+		val = (val & ~0x40) | 0x10;
+	else
+		val = (val & ~0x10) | 0x40;
+	tvp5150_write(sd, TVP5150_MISC_CTL, 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[] = {
+	{
+		TVP5150_CONF_SHARED_PIN, 2
+	},{	/* Automatic offset and AGC enabled */
+		TVP5150_ANAL_CHL_CTL, 0x15
+	},{	/* Activate YCrCb output 0x9 or 0xd ? */
+		TVP5150_MISC_CTL, 0x6f
+	},{	/* 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
+	{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
+	{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
+	{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 }
+	},
+	{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 }
+	},
+	{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 }
+	},
+	{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 }
+	},
+	{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
+	{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 }
+	},
+	{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
+	{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 }
+	},
+	{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 }
+	},
+	{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
+	{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 */
+
+	/* End of struct */
+	{ (u16)-1 }
+};
+
+static int tvp5150_write_inittab(struct v4l2_subdev *sd,
+				const struct i2c_reg_value *regs)
+{
+	while (regs->reg != 0xff) {
+		tvp5150_write(sd, regs->reg, regs->value);
+		regs++;
+	}
+	return 0;
+}
+
+static int tvp5150_vdp_init(struct v4l2_subdev *sd,
+				const struct i2c_vbi_ram_value *regs)
+{
+	unsigned int i;
+
+	/* Disable Full Field */
+	tvp5150_write(sd, 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++)
+		tvp5150_write(sd, i, 0xff);
+
+	/* Load Ram Table */
+	while (regs->reg != (u16)-1) {
+		tvp5150_write(sd, TVP5150_CONF_RAM_ADDR_HIGH, regs->reg >> 8);
+		tvp5150_write(sd, TVP5150_CONF_RAM_ADDR_LOW, regs->reg);
+
+		for (i = 0; i < 16; i++)
+			tvp5150_write(sd, TVP5150_VDP_CONF_RAM_DATA, regs->values[i]);
+
+		regs++;
+	}
+	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)
+{
+	const struct i2c_vbi_ram_value *regs = vbi_ram_default;
+	int line;
+
+	v4l2_dbg(1, debug, sd, "g_sliced_vbi_cap\n");
+	memset(cap, 0, sizeof *cap);
+
+	while (regs->reg != (u16)-1 ) {
+		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;
+
+		regs++;
+	}
+	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,
+			const struct i2c_vbi_ram_value *regs,
+			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 pos=0;
+
+	if (std == V4L2_STD_ALL) {
+		v4l2_err(sd, "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;
+
+	while (regs->reg != (u16)-1 ) {
+		if ((type & regs->type.vbi_type) &&
+		    (line>=regs->type.ini_line) &&
+		    (line<=regs->type.end_line)) {
+			type=regs->type.vbi_type;
+			break;
+		}
+
+		regs++;
+		pos++;
+	}
+	if (regs->reg == (u16)-1)
+		return 0;
+
+	type=pos | (flags & 0xf0);
+	reg=((line-6)<<1)+TVP5150_LINE_MODE_INI;
+
+	if (fields&1) {
+		tvp5150_write(sd, reg, type);
+	}
+
+	if (fields&2) {
+		tvp5150_write(sd, reg+1, type);
+	}
+
+	return type;
+}
+
+static int tvp5150_get_vbi(struct v4l2_subdev *sd,
+			const struct i2c_vbi_ram_value *regs, 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) {
+		v4l2_err(sd, "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) {
+			v4l2_err(sd, "%s: failed with error = %d\n",
+				 __func__, ret);
+			return 0;
+		}
+		pos = ret & 0x0f;
+		if (pos < 0x0f)
+			type |= regs[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;
+
+	decoder->norm = std;
+
+	/* First tests should be against specific std */
+
+	if (std == V4L2_STD_ALL) {
+		fmt = VIDEO_STD_AUTO_SWITCH_BIT;	/* Autodetect mode */
+	} else 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 | 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;
+	}
+
+	v4l2_dbg(1, debug, sd, "Set video std register to %d.\n", fmt);
+	tvp5150_write(sd, TVP5150_VIDEO_STD, fmt);
+	return 0;
+}
+
+static int tvp5150_s_std(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct tvp5150 *decoder = to_tvp5150(sd);
+
+	if (decoder->norm == std)
+		return 0;
+
+	/* 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;
+
+
+	return tvp5150_set_std(sd, std);
+}
+
+static int tvp5150_reset(struct v4l2_subdev *sd, u32 val)
+{
+	struct tvp5150 *decoder = to_tvp5150(sd);
+
+	/* Initializes TVP5150 to its default values */
+	tvp5150_write_inittab(sd, tvp5150_init_default);
+
+	/* Initializes VDP registers */
+	tvp5150_vdp_init(sd, vbi_ram_default);
+
+	/* Selects decoder input */
+	tvp5150_selmux(sd);
+
+	/* Initializes TVP5150 to stream enabled values */
+	tvp5150_write_inittab(sd, tvp5150_init_enable);
+
+	/* Initialize image preferences */
+	v4l2_ctrl_handler_setup(&decoder->hdl);
+
+	tvp5150_set_std(sd, decoder->norm);
+	return 0;
+};
+
+static int tvp5150_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		tvp5150_write(sd, TVP5150_BRIGHT_CTL, ctrl->val);
+		return 0;
+	case V4L2_CID_CONTRAST:
+		tvp5150_write(sd, TVP5150_CONTRAST_CTL, ctrl->val);
+		return 0;
+	case V4L2_CID_SATURATION:
+		tvp5150_write(sd, TVP5150_SATURATION_CTL, ctrl->val);
+		return 0;
+	case V4L2_CID_HUE:
+		tvp5150_write(sd, TVP5150_HUE_CTL, ctrl->val);
+		return 0;
+	}
+	return -EINVAL;
+}
+
+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 tvp5150_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned index,
+						enum v4l2_mbus_pixelcode *code)
+{
+	if (index)
+		return -EINVAL;
+
+	*code = V4L2_MBUS_FMT_UYVY8_2X8;
+	return 0;
+}
+
+static int tvp5150_mbus_fmt(struct v4l2_subdev *sd,
+			    struct v4l2_mbus_framefmt *f)
+{
+	struct tvp5150 *decoder = to_tvp5150(sd);
+
+	if (f == NULL)
+		return -EINVAL;
+
+	tvp5150_reset(sd, 0);
+
+	f->width = decoder->rect.width;
+	f->height = decoder->rect.height;
+
+	f->code = V4L2_MBUS_FMT_UYVY8_2X8;
+	f->field = V4L2_FIELD_SEQ_TB;
+	f->colorspace = V4L2_COLORSPACE_SMPTE170M;
+
+	v4l2_dbg(1, debug, sd, "width = %d, height = %d\n", f->width,
+			f->height);
+	return 0;
+}
+
+static int tvp5150_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
+{
+	struct v4l2_rect rect = a->c;
+	struct tvp5150 *decoder = to_tvp5150(sd);
+	v4l2_std_id std;
+	int hmax;
+
+	v4l2_dbg(1, debug, sd, "%s left=%d, top=%d, width=%d, height=%d\n",
+		__func__, rect.left, rect.top, rect.width, rect.height);
+
+	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	/* tvp5150 has some special limits */
+	rect.left = clamp(rect.left, 0, TVP5150_MAX_CROP_LEFT);
+	rect.width = clamp(rect.width,
+			   TVP5150_H_MAX - TVP5150_MAX_CROP_LEFT - rect.left,
+			   TVP5150_H_MAX - rect.left);
+	rect.top = clamp(rect.top, 0, TVP5150_MAX_CROP_TOP);
+
+	/* 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)
+		hmax = TVP5150_V_MAX_525_60;
+	else
+		hmax = TVP5150_V_MAX_OTHERS;
+
+	rect.height = clamp(rect.height,
+			    hmax - TVP5150_MAX_CROP_TOP - rect.top,
+			    hmax - rect.top);
+
+	tvp5150_write(sd, TVP5150_VERT_BLANKING_START, rect.top);
+	tvp5150_write(sd, TVP5150_VERT_BLANKING_STOP,
+		      rect.top + rect.height - hmax);
+	tvp5150_write(sd, TVP5150_ACT_VD_CROP_ST_MSB,
+		      rect.left >> TVP5150_CROP_SHIFT);
+	tvp5150_write(sd, TVP5150_ACT_VD_CROP_ST_LSB,
+		      rect.left | (1 << TVP5150_CROP_SHIFT));
+	tvp5150_write(sd, TVP5150_ACT_VD_CROP_STP_MSB,
+		      (rect.left + rect.width - TVP5150_MAX_CROP_LEFT) >>
+		      TVP5150_CROP_SHIFT);
+	tvp5150_write(sd, TVP5150_ACT_VD_CROP_STP_LSB,
+		      rect.left + rect.width - TVP5150_MAX_CROP_LEFT);
+
+	decoder->rect = rect;
+
+	return 0;
+}
+
+static int tvp5150_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+{
+	struct tvp5150 *decoder = container_of(sd, struct tvp5150, sd);
+
+	a->c	= decoder->rect;
+	a->type	= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	return 0;
+}
+
+static int tvp5150_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
+{
+	struct tvp5150 *decoder = container_of(sd, struct tvp5150, sd);
+	v4l2_std_id std;
+
+	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	a->bounds.left			= 0;
+	a->bounds.top			= 0;
+	a->bounds.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)
+		a->bounds.height = TVP5150_V_MAX_525_60;
+	else
+		a->bounds.height = TVP5150_V_MAX_OTHERS;
+
+	a->defrect			= a->bounds;
+	a->pixelaspect.numerator	= 1;
+	a->pixelaspect.denominator	= 1;
+
+	return 0;
+}
+
+/****************************************************************************
+			I2C Command
+ ****************************************************************************/
+
+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;
+	tvp5150_selmux(sd);
+	return 0;
+}
+
+static int tvp5150_s_raw_fmt(struct v4l2_subdev *sd, struct v4l2_vbi_format *fmt)
+{
+	/* 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)
+		tvp5150_write(sd, TVP5150_LUMA_PROC_CTL_1, 0x70);
+	if (fmt->count[0] == 18 && fmt->count[1] == 18) {
+		tvp5150_write(sd, TVP5150_VERT_BLANKING_START, 0x00);
+		tvp5150_write(sd, TVP5150_VERT_BLANKING_STOP, 0x01);
+	}
+	return 0;
+}
+
+static int tvp5150_s_sliced_fmt(struct v4l2_subdev *sd, struct v4l2_sliced_vbi_format *svbi)
+{
+	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, vbi_ram_default,
+				       svbi->service_lines[0][i], 0xf0, i, 3);
+		}
+		/* Enables FIFO */
+		tvp5150_write(sd, TVP5150_FIFO_OUT_CTRL, 1);
+	} else {
+		/* Disables FIFO*/
+		tvp5150_write(sd, TVP5150_FIFO_OUT_CTRL, 0);
+
+		/* Disable Full Field */
+		tvp5150_write(sd, TVP5150_FULL_FIELD_ENA, 0);
+
+		/* Disable Line modes */
+		for (i = TVP5150_LINE_MODE_INI; i <= TVP5150_LINE_MODE_END; i++)
+			tvp5150_write(sd, 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, vbi_ram_default, i);
+		mask |= svbi->service_lines[0][i];
+	}
+	svbi->service_set = mask;
+	return 0;
+}
+
+static int tvp5150_g_chip_ident(struct v4l2_subdev *sd,
+				struct v4l2_dbg_chip_ident *chip)
+{
+	int rev;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	rev = tvp5150_read(sd, TVP5150_ROM_MAJOR_VER) << 8 |
+	      tvp5150_read(sd, TVP5150_ROM_MINOR_VER);
+
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_TVP5150,
+					  rev);
+}
+
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int tvp5150_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+{
+	int res;
+
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	res = tvp5150_read(sd, reg->reg & 0xff);
+	if (res < 0) {
+		v4l2_err(sd, "%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, struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	tvp5150_write(sd, reg->reg & 0xff, reg->val & 0xff);
+	return 0;
+}
+#endif
+
+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 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,
+	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
+	.try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
+	.s_ext_ctrls = v4l2_subdev_s_ext_ctrls,
+	.g_ctrl = v4l2_subdev_g_ctrl,
+	.s_ctrl = v4l2_subdev_s_ctrl,
+	.queryctrl = v4l2_subdev_queryctrl,
+	.querymenu = v4l2_subdev_querymenu,
+	.s_std = tvp5150_s_std,
+	.reset = tvp5150_reset,
+	.g_chip_ident = tvp5150_g_chip_ident,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = tvp5150_g_register,
+	.s_register = tvp5150_s_register,
+#endif
+};
+
+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_routing = tvp5150_s_routing,
+	.enum_mbus_fmt = tvp5150_enum_mbus_fmt,
+	.s_mbus_fmt = tvp5150_mbus_fmt,
+	.try_mbus_fmt = tvp5150_mbus_fmt,
+	.g_mbus_fmt = tvp5150_mbus_fmt,
+	.s_crop = tvp5150_s_crop,
+	.g_crop = tvp5150_g_crop,
+	.cropcap = tvp5150_cropcap,
+};
+
+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_ops tvp5150_ops = {
+	.core = &tvp5150_core_ops,
+	.tuner = &tvp5150_tuner_ops,
+	.video = &tvp5150_video_ops,
+	.vbi = &tvp5150_vbi_ops,
+};
+
+
+/****************************************************************************
+			I2C Client & Driver
+ ****************************************************************************/
+
+static int tvp5150_probe(struct i2c_client *c,
+			 const struct i2c_device_id *id)
+{
+	struct tvp5150 *core;
+	struct v4l2_subdev *sd;
+	int tvp5150_id[4];
+	int i, 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;
+
+	core = kzalloc(sizeof(struct tvp5150), GFP_KERNEL);
+	if (!core) {
+		return -ENOMEM;
+	}
+	sd = &core->sd;
+	v4l2_i2c_subdev_init(sd, c, &tvp5150_ops);
+
+	/* 
+	 * Read consequent registers - TVP5150_MSB_DEV_ID, TVP5150_LSB_DEV_ID,
+	 * TVP5150_ROM_MAJOR_VER, TVP5150_ROM_MINOR_VER 
+	 */
+	for (i = 0; i < 4; i++) {
+		res = tvp5150_read(sd, TVP5150_MSB_DEV_ID + i);
+		if (res < 0)
+			goto free_core;
+		tvp5150_id[i] = res;
+	}
+
+	v4l_info(c, "chip found @ 0x%02x (%s)\n",
+		 c->addr << 1, c->adapter->name);
+
+	if (tvp5150_id[2] == 4 && tvp5150_id[3] == 0) { /* Is TVP5150AM1 */
+		v4l2_info(sd, "tvp%02x%02xam1 detected.\n",
+			  tvp5150_id[0], tvp5150_id[1]);
+
+		/* ITU-T BT.656.4 timing */
+		tvp5150_write(sd, TVP5150_REV_SELECT, 0);
+	} else {
+		/* Is TVP5150A */
+		if (tvp5150_id[2] == 3 || tvp5150_id[3] == 0x21) {
+			v4l2_info(sd, "tvp%02x%02xa detected.\n",
+				  tvp5150_id[2], tvp5150_id[3]);
+		} else {
+			v4l2_info(sd, "*** unknown tvp%02x%02x chip detected.\n",
+				  tvp5150_id[2], tvp5150_id[3]);
+			v4l2_info(sd, "*** Rom ver is %d.%d\n",
+				  tvp5150_id[2], tvp5150_id[3]);
+		}
+	}
+
+	core->norm = V4L2_STD_ALL;	/* Default is autodetect */
+	core->input = TVP5150_COMPOSITE1;
+	core->enable = 1;
+
+	v4l2_ctrl_handler_init(&core->hdl, 4);
+	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);
+	sd->ctrl_handler = &core->hdl;
+	if (core->hdl.error) {
+		res = core->hdl.error;
+		v4l2_ctrl_handler_free(&core->hdl);
+		goto free_core;
+	}
+	v4l2_ctrl_handler_setup(&core->hdl);
+
+	/* Default is no cropping */
+	core->rect.top = 0;
+	if (tvp5150_read_std(sd) & V4L2_STD_525_60)
+		core->rect.height = TVP5150_V_MAX_525_60;
+	else
+		core->rect.height = TVP5150_V_MAX_OTHERS;
+	core->rect.left = 0;
+	core->rect.width = TVP5150_H_MAX;
+
+	if (debug > 1)
+		tvp5150_log_status(sd);
+	return 0;
+
+free_core:
+	kfree(core);
+	return res;
+}
+
+static int tvp5150_remove(struct i2c_client *c)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(c);
+	struct tvp5150 *decoder = to_tvp5150(sd);
+
+	v4l2_dbg(1, debug, sd,
+		"tvp5150.c: removing tvp5150 adapter on address 0x%x\n",
+		c->addr << 1);
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&decoder->hdl);
+	kfree(to_tvp5150(sd));
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id tvp5150_id[] = {
+	{ "tvp5150", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, tvp5150_id);
+
+static struct i2c_driver tvp5150_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "tvp5150",
+	},
+	.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 000000000000..25a994944918
--- /dev/null
+++ b/drivers/media/i2c/tvp5150_reg.h
@@ -0,0 +1,139 @@
+/*
+ * tvp5150 - Texas Instruments TVP5150A/AM1 video decoder registers
+ *
+ * Copyright (c) 2005,2006 Mauro Carvalho Chehab (mchehab@infradead.org)
+ * This code is placed under the terms of the GNU General Public License v2
+ */
+
+#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_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_ENABLE_REG_A    0xc1 /* Interrupt enable register A */
+#define TVP5150_INT_CONF            0xc2 /* Interrupt configuration */
+#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 000000000000..fb6a5b57eb83
--- /dev/null
+++ b/drivers/media/i2c/tvp7002.c
@@ -0,0 +1,1145 @@
+/* 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@infradead.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).
+ *
+ * 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; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <linux/module.h>
+#include <linux/v4l2-dv-timings.h>
+#include <media/tvp7002.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ctrls.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");
+
+/* Module Name */
+#define TVP7002_MODULE_NAME	"tvp7002"
+
+/* 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 }
+};
+
+/* Preset definition for handling device operation */
+struct tvp7002_preset_definition {
+	u32 preset;
+	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 presets */
+static const struct tvp7002_preset_definition tvp7002_presets[] = {
+	{
+		V4L2_DV_720P60,
+		V4L2_DV_BT_CEA_1280X720P60,
+		tvp7002_parms_720P60,
+		V4L2_COLORSPACE_REC709,
+		V4L2_FIELD_NONE,
+		1,
+		0x2EE,
+		135,
+		153
+	},
+	{
+		V4L2_DV_1080I60,
+		V4L2_DV_BT_CEA_1920X1080I60,
+		tvp7002_parms_1080I60,
+		V4L2_COLORSPACE_REC709,
+		V4L2_FIELD_INTERLACED,
+		0,
+		0x465,
+		181,
+		205
+	},
+	{
+		V4L2_DV_1080I50,
+		V4L2_DV_BT_CEA_1920X1080I50,
+		tvp7002_parms_1080I50,
+		V4L2_COLORSPACE_REC709,
+		V4L2_FIELD_INTERLACED,
+		0,
+		0x465,
+		217,
+		245
+	},
+	{
+		V4L2_DV_720P50,
+		V4L2_DV_BT_CEA_1280X720P50,
+		tvp7002_parms_720P50,
+		V4L2_COLORSPACE_REC709,
+		V4L2_FIELD_NONE,
+		1,
+		0x2EE,
+		163,
+		183
+	},
+	{
+		V4L2_DV_1080P60,
+		V4L2_DV_BT_CEA_1920X1080P60,
+		tvp7002_parms_1080P60,
+		V4L2_COLORSPACE_REC709,
+		V4L2_FIELD_NONE,
+		1,
+		0x465,
+		90,
+		102
+	},
+	{
+		V4L2_DV_480P59_94,
+		V4L2_DV_BT_CEA_720X480P59_94,
+		tvp7002_parms_480P,
+		V4L2_COLORSPACE_SMPTE170M,
+		V4L2_FIELD_NONE,
+		1,
+		0x20D,
+		0xffff,
+		0xffff
+	},
+	{
+		V4L2_DV_576P50,
+		V4L2_DV_BT_CEA_720X576P50,
+		tvp7002_parms_576P,
+		V4L2_COLORSPACE_SMPTE170M,
+		V4L2_FIELD_NONE,
+		1,
+		0x271,
+		0xffff,
+		0xffff
+	}
+};
+
+#define NUM_PRESETS	ARRAY_SIZE(tvp7002_presets)
+
+/* 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_preset_definition *current_preset;
+};
+
+/*
+ * 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_g_chip_ident() - Get chip identification number
+ * @sd: ptr to v4l2_subdev struct
+ * @chip: ptr to v4l2_dbg_chip_ident struct
+ *
+ * Obtains the chip's identification number.
+ * Returns zero or -EINVAL if read operation fails.
+ */
+static int tvp7002_g_chip_ident(struct v4l2_subdev *sd,
+					struct v4l2_dbg_chip_ident *chip)
+{
+	u8 rev;
+	int error;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	error = tvp7002_read(sd, TVP7002_CHIP_REV, &rev);
+
+	if (error < 0)
+		return error;
+
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_TVP7002, rev);
+}
+
+/*
+ * 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;
+}
+
+/*
+ * tvp7002_s_dv_preset() - Set digital video preset
+ * @sd: ptr to v4l2_subdev struct
+ * @dv_preset: ptr to v4l2_dv_preset struct
+ *
+ * Set the digital video preset for a TVP7002 decoder device.
+ * Returns zero when successful or -EINVAL if register access fails.
+ */
+static int tvp7002_s_dv_preset(struct v4l2_subdev *sd,
+					struct v4l2_dv_preset *dv_preset)
+{
+	struct tvp7002 *device = to_tvp7002(sd);
+	u32 preset;
+	int i;
+
+	for (i = 0; i < NUM_PRESETS; i++) {
+		preset = tvp7002_presets[i].preset;
+		if (preset == dv_preset->preset) {
+			device->current_preset = &tvp7002_presets[i];
+			return tvp7002_write_inittab(sd, tvp7002_presets[i].p_settings);
+		}
+	}
+
+	return -EINVAL;
+}
+
+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_PRESETS; i++) {
+		const struct v4l2_bt_timings *t = &tvp7002_presets[i].timings.bt;
+
+		if (!memcmp(bt, t, &bt->standards - &bt->width)) {
+			device->current_preset = &tvp7002_presets[i];
+			return tvp7002_write_inittab(sd, tvp7002_presets[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_preset->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_mbus_fmt() - V4L2 decoder interface handler for try/s/g_mbus_fmt
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @f: pointer to mediabus format structure
+ *
+ * Negotiate the image capture size and mediabus format.
+ * There is only one possible format, so this single function works for
+ * get, set and try.
+ */
+static int tvp7002_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *f)
+{
+	struct tvp7002 *device = to_tvp7002(sd);
+	struct v4l2_dv_enum_preset e_preset;
+	int error;
+
+	/* Calculate height and width based on current standard */
+	error = v4l_fill_dv_preset_info(device->current_preset->preset, &e_preset);
+	if (error)
+		return error;
+
+	f->width = e_preset.width;
+	f->height = e_preset.height;
+	f->code = V4L2_MBUS_FMT_YUYV10_1X20;
+	f->field = device->current_preset->scanmode;
+	f->colorspace = device->current_preset->color_space;
+
+	v4l2_dbg(1, debug, sd, "MBUS_FMT: Width - %d, Height - %d",
+			f->width, f->height);
+	return 0;
+}
+
+/*
+ * tvp7002_query_dv_preset() - query DV preset
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @qpreset: standard V4L2 v4l2_dv_preset structure
+ *
+ * Returns the current DV preset by TVP7002. If no active input is
+ * detected, returns -EINVAL
+ */
+static int tvp7002_query_dv(struct v4l2_subdev *sd, int *index)
+{
+	const struct tvp7002_preset_definition *presets = tvp7002_presets;
+	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_PRESETS;
+
+	/* 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_PRESETS; (*index)++, presets++)
+		if (lpfr == presets->lines_per_frame &&
+			progressive == presets->progressive) {
+			if (presets->cpl_min == 0xffff)
+				break;
+			if (cpln >= presets->cpl_min && cpln <= presets->cpl_max)
+				break;
+		}
+
+	if (*index == NUM_PRESETS) {
+		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 preset: %d\n", *index);
+	return 0;
+}
+
+static int tvp7002_query_dv_preset(struct v4l2_subdev *sd,
+					struct v4l2_dv_preset *qpreset)
+{
+	int index;
+	int err = tvp7002_query_dv(sd, &index);
+
+	if (err || index == NUM_PRESETS) {
+		qpreset->preset = V4L2_DV_INVALID;
+		if (err == -ENOLINK)
+			err = 0;
+		return err;
+	}
+	qpreset->preset = tvp7002_presets[index].preset;
+	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_presets[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, -EPERM if the call is not allowed
+ * by disabled CAP_SYS_ADMIN.
+ */
+static int tvp7002_g_register(struct v4l2_subdev *sd,
+						struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u8 val;
+	int ret;
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+
+	ret = tvp7002_read(sd, reg->reg & 0xff, &val);
+	reg->val = val;
+	return ret;
+}
+
+/*
+ * 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 or
+ * -EPERM if call not allowed.
+ */
+static int tvp7002_s_register(struct v4l2_subdev *sd,
+						struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+
+	return tvp7002_write(sd, reg->reg & 0xff, reg->val & 0xff);
+}
+#endif
+
+/*
+ * tvp7002_enum_mbus_fmt() - Enum supported mediabus formats
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @index: format index
+ * @code: pointer to mediabus format
+ *
+ * Enumerate supported mediabus formats.
+ */
+
+static int tvp7002_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned index,
+					enum v4l2_mbus_pixelcode *code)
+{
+	/* Check requested format index is within range */
+	if (index)
+		return -EINVAL;
+	*code = V4L2_MBUS_FMT_YUYV10_1X20;
+	return 0;
+}
+
+/*
+ * 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 = 0;
+
+	if (device->streaming == enable)
+		return 0;
+
+	if (enable) {
+		/* Set output state on (low impedance means stream on) */
+		error = tvp7002_write(sd, TVP7002_MISC_CTL_2, 0x00);
+		device->streaming = enable;
+	} else {
+		/* Set output state off (high impedance means stream off) */
+		error = tvp7002_write(sd, TVP7002_MISC_CTL_2, 0x03);
+		if (error)
+			v4l2_dbg(1, debug, sd, "Unable to stop streaming\n");
+
+		device->streaming = enable;
+	}
+
+	return error;
+}
+
+/*
+ * 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)
+{
+	const struct tvp7002_preset_definition *presets = tvp7002_presets;
+	struct tvp7002 *device = to_tvp7002(sd);
+	struct v4l2_dv_enum_preset e_preset;
+	struct v4l2_dv_preset detected;
+	int i;
+
+	detected.preset = V4L2_DV_INVALID;
+	/* Find my current standard*/
+	tvp7002_query_dv_preset(sd, &detected);
+
+	/* Print standard related code values */
+	for (i = 0; i < NUM_PRESETS; i++, presets++)
+		if (presets->preset == detected.preset)
+			break;
+
+	if (v4l_fill_dv_preset_info(device->current_preset->preset, &e_preset))
+		return -EINVAL;
+
+	v4l2_info(sd, "Selected DV Preset: %s\n", e_preset.name);
+	v4l2_info(sd, "   Pixels per line: %u\n", e_preset.width);
+	v4l2_info(sd, "   Lines per frame: %u\n\n", e_preset.height);
+	if (i == NUM_PRESETS) {
+		v4l2_info(sd, "Detected DV Preset: None\n");
+	} else {
+		if (v4l_fill_dv_preset_info(presets->preset, &e_preset))
+			return -EINVAL;
+		v4l2_info(sd, "Detected DV Preset: %s\n", e_preset.name);
+		v4l2_info(sd, "  Pixels per line: %u\n", e_preset.width);
+		v4l2_info(sd, "  Lines per frame: %u\n\n", e_preset.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;
+}
+
+/*
+ * tvp7002_enum_dv_presets() - Enum supported digital video formats
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @preset: pointer to format struct
+ *
+ * Enumerate supported digital video formats.
+ */
+static int tvp7002_enum_dv_presets(struct v4l2_subdev *sd,
+		struct v4l2_dv_enum_preset *preset)
+{
+	/* Check requested format index is within range */
+	if (preset->index >= NUM_PRESETS)
+		return -EINVAL;
+
+	return v4l_fill_dv_preset_info(tvp7002_presets[preset->index].preset, preset);
+}
+
+static int tvp7002_enum_dv_timings(struct v4l2_subdev *sd,
+		struct v4l2_enum_dv_timings *timings)
+{
+	/* Check requested format index is within range */
+	if (timings->index >= NUM_PRESETS)
+		return -EINVAL;
+
+	timings->timings = tvp7002_presets[timings->index].timings;
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops tvp7002_ctrl_ops = {
+	.s_ctrl = tvp7002_s_ctrl,
+};
+
+/* V4L2 core operation handlers */
+static const struct v4l2_subdev_core_ops tvp7002_core_ops = {
+	.g_chip_ident = tvp7002_g_chip_ident,
+	.log_status = tvp7002_log_status,
+	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
+	.try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
+	.s_ext_ctrls = v4l2_subdev_s_ext_ctrls,
+	.g_ctrl = v4l2_subdev_g_ctrl,
+	.s_ctrl = v4l2_subdev_s_ctrl,
+	.queryctrl = v4l2_subdev_queryctrl,
+	.querymenu = v4l2_subdev_querymenu,
+#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 = {
+	.enum_dv_presets = tvp7002_enum_dv_presets,
+	.s_dv_preset = tvp7002_s_dv_preset,
+	.query_dv_preset = tvp7002_query_dv_preset,
+	.g_dv_timings = tvp7002_g_dv_timings,
+	.s_dv_timings = tvp7002_s_dv_timings,
+	.enum_dv_timings = tvp7002_enum_dv_timings,
+	.query_dv_timings = tvp7002_query_dv_timings,
+	.s_stream = tvp7002_s_stream,
+	.g_mbus_fmt = tvp7002_mbus_fmt,
+	.try_mbus_fmt = tvp7002_mbus_fmt,
+	.s_mbus_fmt = tvp7002_mbus_fmt,
+	.enum_mbus_fmt = tvp7002_enum_mbus_fmt,
+};
+
+/* V4L2 top level operation handlers */
+static const struct v4l2_subdev_ops tvp7002_ops = {
+	.core = &tvp7002_core_ops,
+	.video = &tvp7002_video_ops,
+};
+
+/*
+ * 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, const struct i2c_device_id *id)
+{
+	struct v4l2_subdev *sd;
+	struct tvp7002 *device;
+	struct v4l2_dv_preset preset;
+	int polarity_a;
+	int polarity_b;
+	u8 revision;
+
+	int error;
+
+	/* 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;
+
+	if (!c->dev.platform_data) {
+		v4l_err(c, "No platform data!!\n");
+		return -ENODEV;
+	}
+
+	device = kzalloc(sizeof(struct tvp7002), GFP_KERNEL);
+
+	if (!device)
+		return -ENOMEM;
+
+	sd = &device->sd;
+	device->pdata = c->dev.platform_data;
+	device->current_preset = tvp7002_presets;
+
+	/* 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)
+		goto found_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)
+		goto found_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)
+		goto found_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)
+		goto found_error;
+
+	/* Set registers according to default video mode */
+	preset.preset = device->current_preset->preset;
+	error = tvp7002_s_dv_preset(sd, &preset);
+
+	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) {
+		int err = device->hdl.error;
+
+		v4l2_ctrl_handler_free(&device->hdl);
+		kfree(device);
+		return err;
+	}
+	v4l2_ctrl_handler_setup(&device->hdl);
+
+found_error:
+	if (error < 0)
+		kfree(device);
+
+	return error;
+}
+
+/*
+ * tvp7002_remove - Remove TVP7002 device support
+ * @c: ptr to i2c_client struct
+ *
+ * Reset the TVP7002 device
+ * Returns zero.
+ */
+static int 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_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&device->hdl);
+	kfree(device);
+	return 0;
+}
+
+/* I2C Device ID table */
+static const struct i2c_device_id tvp7002_id[] = {
+	{ "tvp7002", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, tvp7002_id);
+
+/* I2C driver data */
+static struct i2c_driver tvp7002_driver = {
+	.driver = {
+		.owner = THIS_MODULE,
+		.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 000000000000..0e34ca9bccf3
--- /dev/null
+++ b/drivers/media/i2c/tvp7002_reg.h
@@ -0,0 +1,150 @@
+/* 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@infradead.org),
+ * the TVP514x driver written by Vaibhav Hiremath <hvaibhav@ti.com>
+ * and the TVP7002 driver in the TI LSP 2.10.00.14
+ *
+ * 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; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+/* 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/upd64031a.c b/drivers/media/i2c/upd64031a.c
new file mode 100644
index 000000000000..1e7446542091
--- /dev/null
+++ b/drivers/media/i2c/upd64031a.c
@@ -0,0 +1,274 @@
+/*
+ * 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>
+ *
+ * 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; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+
+#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-chip-ident.h>
+#include <media/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, 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_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ident *chip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_UPD64031A, 0);
+}
+
+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)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	reg->val = upd64031a_read(sd, reg->reg & 0xff);
+	reg->size = 1;
+	return 0;
+}
+
+static int upd64031a_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	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,
+	.g_chip_ident = upd64031a_g_chip_ident,
+#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,
+			   const struct i2c_device_id *id)
+{
+	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 = kzalloc(sizeof(struct upd64031a_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 int upd64031a_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	kfree(to_state(sd));
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id upd64031a_id[] = {
+	{ "upd64031a", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, upd64031a_id);
+
+static struct i2c_driver upd64031a_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.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 000000000000..75d6acc62018
--- /dev/null
+++ b/drivers/media/i2c/upd64083.c
@@ -0,0 +1,246 @@
+/*
+ * 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>
+ *
+ * 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; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#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-chip-ident.h>
+#include <media/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)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	reg->val = upd64083_read(sd, reg->reg & 0xff);
+	reg->size = 1;
+	return 0;
+}
+
+static int upd64083_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	upd64083_write(sd, reg->reg & 0xff, reg->val & 0xff);
+	return 0;
+}
+#endif
+
+static int upd64083_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ident *chip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_UPD64083, 0);
+}
+
+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,
+	.g_chip_ident = upd64083_g_chip_ident,
+#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,
+			  const struct i2c_device_id *id)
+{
+	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 = kzalloc(sizeof(struct upd64083_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 int upd64083_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	kfree(to_state(sd));
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id upd64083_id[] = {
+	{ "upd64083", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, upd64083_id);
+
+static struct i2c_driver upd64083_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "upd64083",
+	},
+	.probe		= upd64083_probe,
+	.remove		= upd64083_remove,
+	.id_table	= upd64083_id,
+};
+
+module_i2c_driver(upd64083_driver);
diff --git a/drivers/media/i2c/vp27smpx.c b/drivers/media/i2c/vp27smpx.c
new file mode 100644
index 000000000000..7cfbc9d94a48
--- /dev/null
+++ b/drivers/media/i2c/vp27smpx.c
@@ -0,0 +1,211 @@
+/*
+ * 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>
+ *
+ * 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; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <linux/ioctl.h>
+#include <asm/uaccess.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.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, 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_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ident *chip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_VP27SMPX, 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,
+	.g_chip_ident = vp27smpx_g_chip_ident,
+	.s_std = vp27smpx_s_std,
+};
+
+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_ops vp27smpx_ops = {
+	.core = &vp27smpx_core_ops,
+	.tuner = &vp27smpx_tuner_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,
+			  const struct i2c_device_id *id)
+{
+	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 = kzalloc(sizeof(struct vp27smpx_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 int vp27smpx_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	kfree(to_state(sd));
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id vp27smpx_id[] = {
+	{ "vp27smpx", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, vp27smpx_id);
+
+static struct i2c_driver vp27smpx_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.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 000000000000..2f67b4c5c823
--- /dev/null
+++ b/drivers/media/i2c/vpx3220.c
@@ -0,0 +1,591 @@
+/*
+ * vpx3220a, vpx3216b & vpx3214c video decoder driver version 0.0.1
+ *
+ * Copyright (C) 2001 Laurent Pinchart <lpinchart@freegates.be>
+ *
+ * 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; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <asm/uaccess.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.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 ident;
+	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 u16 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 = 0;
+
+	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;
+		}
+	}
+	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  */
+
+	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 int vpx3220_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ident *chip)
+{
+	struct vpx3220 *decoder = to_vpx3220(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return v4l2_chip_ident_i2c_client(client, chip, decoder->ident, 0);
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_ctrl_ops vpx3220_ctrl_ops = {
+	.s_ctrl = vpx3220_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops vpx3220_core_ops = {
+	.g_chip_ident = vpx3220_g_chip_ident,
+	.init = vpx3220_init,
+	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
+	.try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
+	.s_ext_ctrls = v4l2_subdev_s_ext_ctrls,
+	.g_ctrl = v4l2_subdev_g_ctrl,
+	.s_ctrl = v4l2_subdev_s_ctrl,
+	.queryctrl = v4l2_subdev_queryctrl,
+	.querymenu = v4l2_subdev_querymenu,
+	.s_std = vpx3220_s_std,
+};
+
+static const struct v4l2_subdev_video_ops vpx3220_video_ops = {
+	.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,
+			const struct i2c_device_id *id)
+{
+	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 = kzalloc(sizeof(struct vpx3220), 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);
+		kfree(decoder);
+		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);
+	decoder->ident = V4L2_IDENT_VPX3220A;
+	if (ver == 0xec) {
+		switch (pn) {
+		case 0x4680:
+			name = "vpx3220a";
+			break;
+		case 0x4260:
+			name = "vpx3216b";
+			decoder->ident = V4L2_IDENT_VPX3216B;
+			break;
+		case 0x4280:
+			name = "vpx3214c";
+			decoder->ident = V4L2_IDENT_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 int 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);
+	kfree(decoder);
+	return 0;
+}
+
+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 = {
+		.owner	= THIS_MODULE,
+		.name	= "vpx3220",
+	},
+	.probe		= vpx3220_probe,
+	.remove		= vpx3220_remove,
+	.id_table	= vpx3220_id,
+};
+
+module_i2c_driver(vpx3220_driver);
diff --git a/drivers/media/i2c/vs6624.c b/drivers/media/i2c/vs6624.c
new file mode 100644
index 000000000000..42ae9dc9c574
--- /dev/null
+++ b/drivers/media/i2c/vs6624.c
@@ -0,0 +1,928 @@
+/*
+ * vs6624.c ST VS6624 CMOS image sensor driver
+ *
+ * Copyright (c) 2011 Analog Devices Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/gpio.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/v4l2-chip-ident.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-mediabus.h>
+
+#include "vs6624_regs.h"
+
+#define VGA_WIDTH       640
+#define VGA_HEIGHT      480
+#define QVGA_WIDTH      320
+#define QVGA_HEIGHT     240
+#define QQVGA_WIDTH     160
+#define QQVGA_HEIGHT    120
+#define CIF_WIDTH       352
+#define CIF_HEIGHT      288
+#define QCIF_WIDTH      176
+#define QCIF_HEIGHT     144
+#define QQCIF_WIDTH     88
+#define QQCIF_HEIGHT    72
+
+#define MAX_FRAME_RATE  30
+
+struct vs6624 {
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
+	struct v4l2_fract frame_rate;
+	struct v4l2_mbus_framefmt fmt;
+	unsigned ce_pin;
+};
+
+static const struct vs6624_format {
+	enum v4l2_mbus_pixelcode mbus_code;
+	enum v4l2_colorspace colorspace;
+} vs6624_formats[] = {
+	{
+		.mbus_code      = V4L2_MBUS_FMT_UYVY8_2X8,
+		.colorspace     = V4L2_COLORSPACE_JPEG,
+	},
+	{
+		.mbus_code      = V4L2_MBUS_FMT_YUYV8_2X8,
+		.colorspace     = V4L2_COLORSPACE_JPEG,
+	},
+	{
+		.mbus_code      = V4L2_MBUS_FMT_RGB565_2X8_LE,
+		.colorspace     = V4L2_COLORSPACE_SRGB,
+	},
+};
+
+static struct v4l2_mbus_framefmt vs6624_default_fmt = {
+	.width = VGA_WIDTH,
+	.height = VGA_HEIGHT,
+	.code = V4L2_MBUS_FMT_UYVY8_2X8,
+	.field = V4L2_FIELD_NONE,
+	.colorspace = V4L2_COLORSPACE_JPEG,
+};
+
+static const u16 vs6624_p1[] = {
+	0x8104, 0x03,
+	0x8105, 0x01,
+	0xc900, 0x03,
+	0xc904, 0x47,
+	0xc905, 0x10,
+	0xc906, 0x80,
+	0xc907, 0x3a,
+	0x903a, 0x02,
+	0x903b, 0x47,
+	0x903c, 0x15,
+	0xc908, 0x31,
+	0xc909, 0xdc,
+	0xc90a, 0x80,
+	0xc90b, 0x44,
+	0x9044, 0x02,
+	0x9045, 0x31,
+	0x9046, 0xe2,
+	0xc90c, 0x07,
+	0xc90d, 0xe0,
+	0xc90e, 0x80,
+	0xc90f, 0x47,
+	0x9047, 0x90,
+	0x9048, 0x83,
+	0x9049, 0x81,
+	0x904a, 0xe0,
+	0x904b, 0x60,
+	0x904c, 0x08,
+	0x904d, 0x90,
+	0x904e, 0xc0,
+	0x904f, 0x43,
+	0x9050, 0x74,
+	0x9051, 0x01,
+	0x9052, 0xf0,
+	0x9053, 0x80,
+	0x9054, 0x05,
+	0x9055, 0xE4,
+	0x9056, 0x90,
+	0x9057, 0xc0,
+	0x9058, 0x43,
+	0x9059, 0xf0,
+	0x905a, 0x02,
+	0x905b, 0x07,
+	0x905c, 0xec,
+	0xc910, 0x5d,
+	0xc911, 0xca,
+	0xc912, 0x80,
+	0xc913, 0x5d,
+	0x905d, 0xa3,
+	0x905e, 0x04,
+	0x905f, 0xf0,
+	0x9060, 0xa3,
+	0x9061, 0x04,
+	0x9062, 0xf0,
+	0x9063, 0x22,
+	0xc914, 0x72,
+	0xc915, 0x92,
+	0xc916, 0x80,
+	0xc917, 0x64,
+	0x9064, 0x74,
+	0x9065, 0x01,
+	0x9066, 0x02,
+	0x9067, 0x72,
+	0x9068, 0x95,
+	0xc918, 0x47,
+	0xc919, 0xf2,
+	0xc91a, 0x81,
+	0xc91b, 0x69,
+	0x9169, 0x74,
+	0x916a, 0x02,
+	0x916b, 0xf0,
+	0x916c, 0xec,
+	0x916d, 0xb4,
+	0x916e, 0x10,
+	0x916f, 0x0a,
+	0x9170, 0x90,
+	0x9171, 0x80,
+	0x9172, 0x16,
+	0x9173, 0xe0,
+	0x9174, 0x70,
+	0x9175, 0x04,
+	0x9176, 0x90,
+	0x9177, 0xd3,
+	0x9178, 0xc4,
+	0x9179, 0xf0,
+	0x917a, 0x22,
+	0xc91c, 0x0a,
+	0xc91d, 0xbe,
+	0xc91e, 0x80,
+	0xc91f, 0x73,
+	0x9073, 0xfc,
+	0x9074, 0xa3,
+	0x9075, 0xe0,
+	0x9076, 0xf5,
+	0x9077, 0x82,
+	0x9078, 0x8c,
+	0x9079, 0x83,
+	0x907a, 0xa3,
+	0x907b, 0xa3,
+	0x907c, 0xe0,
+	0x907d, 0xfc,
+	0x907e, 0xa3,
+	0x907f, 0xe0,
+	0x9080, 0xc3,
+	0x9081, 0x9f,
+	0x9082, 0xff,
+	0x9083, 0xec,
+	0x9084, 0x9e,
+	0x9085, 0xfe,
+	0x9086, 0x02,
+	0x9087, 0x0a,
+	0x9088, 0xea,
+	0xc920, 0x47,
+	0xc921, 0x38,
+	0xc922, 0x80,
+	0xc923, 0x89,
+	0x9089, 0xec,
+	0x908a, 0xd3,
+	0x908b, 0x94,
+	0x908c, 0x20,
+	0x908d, 0x40,
+	0x908e, 0x01,
+	0x908f, 0x1c,
+	0x9090, 0x90,
+	0x9091, 0xd3,
+	0x9092, 0xd4,
+	0x9093, 0xec,
+	0x9094, 0xf0,
+	0x9095, 0x02,
+	0x9096, 0x47,
+	0x9097, 0x3d,
+	0xc924, 0x45,
+	0xc925, 0xca,
+	0xc926, 0x80,
+	0xc927, 0x98,
+	0x9098, 0x12,
+	0x9099, 0x77,
+	0x909a, 0xd6,
+	0x909b, 0x02,
+	0x909c, 0x45,
+	0x909d, 0xcd,
+	0xc928, 0x20,
+	0xc929, 0xd5,
+	0xc92a, 0x80,
+	0xc92b, 0x9e,
+	0x909e, 0x90,
+	0x909f, 0x82,
+	0x90a0, 0x18,
+	0x90a1, 0xe0,
+	0x90a2, 0xb4,
+	0x90a3, 0x03,
+	0x90a4, 0x0e,
+	0x90a5, 0x90,
+	0x90a6, 0x83,
+	0x90a7, 0xbf,
+	0x90a8, 0xe0,
+	0x90a9, 0x60,
+	0x90aa, 0x08,
+	0x90ab, 0x90,
+	0x90ac, 0x81,
+	0x90ad, 0xfc,
+	0x90ae, 0xe0,
+	0x90af, 0xff,
+	0x90b0, 0xc3,
+	0x90b1, 0x13,
+	0x90b2, 0xf0,
+	0x90b3, 0x90,
+	0x90b4, 0x81,
+	0x90b5, 0xfc,
+	0x90b6, 0xe0,
+	0x90b7, 0xff,
+	0x90b8, 0x02,
+	0x90b9, 0x20,
+	0x90ba, 0xda,
+	0xc92c, 0x70,
+	0xc92d, 0xbc,
+	0xc92e, 0x80,
+	0xc92f, 0xbb,
+	0x90bb, 0x90,
+	0x90bc, 0x82,
+	0x90bd, 0x18,
+	0x90be, 0xe0,
+	0x90bf, 0xb4,
+	0x90c0, 0x03,
+	0x90c1, 0x06,
+	0x90c2, 0x90,
+	0x90c3, 0xc1,
+	0x90c4, 0x06,
+	0x90c5, 0x74,
+	0x90c6, 0x05,
+	0x90c7, 0xf0,
+	0x90c8, 0x90,
+	0x90c9, 0xd3,
+	0x90ca, 0xa0,
+	0x90cb, 0x02,
+	0x90cc, 0x70,
+	0x90cd, 0xbf,
+	0xc930, 0x72,
+	0xc931, 0x21,
+	0xc932, 0x81,
+	0xc933, 0x3b,
+	0x913b, 0x7d,
+	0x913c, 0x02,
+	0x913d, 0x7f,
+	0x913e, 0x7b,
+	0x913f, 0x02,
+	0x9140, 0x72,
+	0x9141, 0x25,
+	0xc934, 0x28,
+	0xc935, 0xae,
+	0xc936, 0x80,
+	0xc937, 0xd2,
+	0x90d2, 0xf0,
+	0x90d3, 0x90,
+	0x90d4, 0xd2,
+	0x90d5, 0x0a,
+	0x90d6, 0x02,
+	0x90d7, 0x28,
+	0x90d8, 0xb4,
+	0xc938, 0x28,
+	0xc939, 0xb1,
+	0xc93a, 0x80,
+	0xc93b, 0xd9,
+	0x90d9, 0x90,
+	0x90da, 0x83,
+	0x90db, 0xba,
+	0x90dc, 0xe0,
+	0x90dd, 0xff,
+	0x90de, 0x90,
+	0x90df, 0xd2,
+	0x90e0, 0x08,
+	0x90e1, 0xe0,
+	0x90e2, 0xe4,
+	0x90e3, 0xef,
+	0x90e4, 0xf0,
+	0x90e5, 0xa3,
+	0x90e6, 0xe0,
+	0x90e7, 0x74,
+	0x90e8, 0xff,
+	0x90e9, 0xf0,
+	0x90ea, 0x90,
+	0x90eb, 0xd2,
+	0x90ec, 0x0a,
+	0x90ed, 0x02,
+	0x90ee, 0x28,
+	0x90ef, 0xb4,
+	0xc93c, 0x29,
+	0xc93d, 0x79,
+	0xc93e, 0x80,
+	0xc93f, 0xf0,
+	0x90f0, 0xf0,
+	0x90f1, 0x90,
+	0x90f2, 0xd2,
+	0x90f3, 0x0e,
+	0x90f4, 0x02,
+	0x90f5, 0x29,
+	0x90f6, 0x7f,
+	0xc940, 0x29,
+	0xc941, 0x7c,
+	0xc942, 0x80,
+	0xc943, 0xf7,
+	0x90f7, 0x90,
+	0x90f8, 0x83,
+	0x90f9, 0xba,
+	0x90fa, 0xe0,
+	0x90fb, 0xff,
+	0x90fc, 0x90,
+	0x90fd, 0xd2,
+	0x90fe, 0x0c,
+	0x90ff, 0xe0,
+	0x9100, 0xe4,
+	0x9101, 0xef,
+	0x9102, 0xf0,
+	0x9103, 0xa3,
+	0x9104, 0xe0,
+	0x9105, 0x74,
+	0x9106, 0xff,
+	0x9107, 0xf0,
+	0x9108, 0x90,
+	0x9109, 0xd2,
+	0x910a, 0x0e,
+	0x910b, 0x02,
+	0x910c, 0x29,
+	0x910d, 0x7f,
+	0xc944, 0x2a,
+	0xc945, 0x42,
+	0xc946, 0x81,
+	0xc947, 0x0e,
+	0x910e, 0xf0,
+	0x910f, 0x90,
+	0x9110, 0xd2,
+	0x9111, 0x12,
+	0x9112, 0x02,
+	0x9113, 0x2a,
+	0x9114, 0x48,
+	0xc948, 0x2a,
+	0xc949, 0x45,
+	0xc94a, 0x81,
+	0xc94b, 0x15,
+	0x9115, 0x90,
+	0x9116, 0x83,
+	0x9117, 0xba,
+	0x9118, 0xe0,
+	0x9119, 0xff,
+	0x911a, 0x90,
+	0x911b, 0xd2,
+	0x911c, 0x10,
+	0x911d, 0xe0,
+	0x911e, 0xe4,
+	0x911f, 0xef,
+	0x9120, 0xf0,
+	0x9121, 0xa3,
+	0x9122, 0xe0,
+	0x9123, 0x74,
+	0x9124, 0xff,
+	0x9125, 0xf0,
+	0x9126, 0x90,
+	0x9127, 0xd2,
+	0x9128, 0x12,
+	0x9129, 0x02,
+	0x912a, 0x2a,
+	0x912b, 0x48,
+	0xc900, 0x01,
+	0x0000, 0x00,
+};
+
+static const u16 vs6624_p2[] = {
+	0x806f, 0x01,
+	0x058c, 0x01,
+	0x0000, 0x00,
+};
+
+static const u16 vs6624_run_setup[] = {
+	0x1d18, 0x00,				/* Enableconstrainedwhitebalance */
+	VS6624_PEAK_MIN_OUT_G_MSB, 0x3c,	/* Damper PeakGain Output MSB */
+	VS6624_PEAK_MIN_OUT_G_LSB, 0x66,	/* Damper PeakGain Output LSB */
+	VS6624_CM_LOW_THR_MSB, 0x65,		/* Damper Low MSB */
+	VS6624_CM_LOW_THR_LSB, 0xd1,		/* Damper Low LSB */
+	VS6624_CM_HIGH_THR_MSB, 0x66,		/* Damper High MSB */
+	VS6624_CM_HIGH_THR_LSB, 0x62,		/* Damper High LSB */
+	VS6624_CM_MIN_OUT_MSB, 0x00,		/* Damper Min output MSB */
+	VS6624_CM_MIN_OUT_LSB, 0x00,		/* Damper Min output LSB */
+	VS6624_NORA_DISABLE, 0x00,		/* Nora fDisable */
+	VS6624_NORA_USAGE, 0x04,		/* Nora usage */
+	VS6624_NORA_LOW_THR_MSB, 0x63,		/* Damper Low MSB Changed 0x63 to 0x65 */
+	VS6624_NORA_LOW_THR_LSB, 0xd1,		/* Damper Low LSB */
+	VS6624_NORA_HIGH_THR_MSB, 0x68,		/* Damper High MSB */
+	VS6624_NORA_HIGH_THR_LSB, 0xdd,		/* Damper High LSB */
+	VS6624_NORA_MIN_OUT_MSB, 0x3a,		/* Damper Min output MSB */
+	VS6624_NORA_MIN_OUT_LSB, 0x00,		/* Damper Min output LSB */
+	VS6624_F2B_DISABLE, 0x00,		/* Disable */
+	0x1d8a, 0x30,				/* MAXWeightHigh */
+	0x1d91, 0x62,				/* fpDamperLowThresholdHigh MSB */
+	0x1d92, 0x4a,				/* fpDamperLowThresholdHigh LSB */
+	0x1d95, 0x65,				/* fpDamperHighThresholdHigh MSB */
+	0x1d96, 0x0e,				/* fpDamperHighThresholdHigh LSB */
+	0x1da1, 0x3a,				/* fpMinimumDamperOutputLow MSB */
+	0x1da2, 0xb8,				/* fpMinimumDamperOutputLow LSB */
+	0x1e08, 0x06,				/* MAXWeightLow */
+	0x1e0a, 0x0a,				/* MAXWeightHigh */
+	0x1601, 0x3a,				/* Red A MSB */
+	0x1602, 0x14,				/* Red A LSB */
+	0x1605, 0x3b,				/* Blue A MSB */
+	0x1606, 0x85,				/* BLue A LSB */
+	0x1609, 0x3b,				/* RED B MSB */
+	0x160a, 0x85,				/* RED B LSB */
+	0x160d, 0x3a,				/* Blue B MSB */
+	0x160e, 0x14,				/* Blue B LSB */
+	0x1611, 0x30,				/* Max Distance from Locus MSB */
+	0x1612, 0x8f,				/* Max Distance from Locus MSB */
+	0x1614, 0x01,				/* Enable constrainer */
+	0x0000, 0x00,
+};
+
+static const u16 vs6624_default[] = {
+	VS6624_CONTRAST0, 0x84,
+	VS6624_SATURATION0, 0x75,
+	VS6624_GAMMA0, 0x11,
+	VS6624_CONTRAST1, 0x84,
+	VS6624_SATURATION1, 0x75,
+	VS6624_GAMMA1, 0x11,
+	VS6624_MAN_RG, 0x80,
+	VS6624_MAN_GG, 0x80,
+	VS6624_MAN_BG, 0x80,
+	VS6624_WB_MODE, 0x1,
+	VS6624_EXPO_COMPENSATION, 0xfe,
+	VS6624_EXPO_METER, 0x0,
+	VS6624_LIGHT_FREQ, 0x64,
+	VS6624_PEAK_GAIN, 0xe,
+	VS6624_PEAK_LOW_THR, 0x28,
+	VS6624_HMIRROR0, 0x0,
+	VS6624_VFLIP0, 0x0,
+	VS6624_ZOOM_HSTEP0_MSB, 0x0,
+	VS6624_ZOOM_HSTEP0_LSB, 0x1,
+	VS6624_ZOOM_VSTEP0_MSB, 0x0,
+	VS6624_ZOOM_VSTEP0_LSB, 0x1,
+	VS6624_PAN_HSTEP0_MSB, 0x0,
+	VS6624_PAN_HSTEP0_LSB, 0xf,
+	VS6624_PAN_VSTEP0_MSB, 0x0,
+	VS6624_PAN_VSTEP0_LSB, 0xf,
+	VS6624_SENSOR_MODE, 0x1,
+	VS6624_SYNC_CODE_SETUP, 0x21,
+	VS6624_DISABLE_FR_DAMPER, 0x0,
+	VS6624_FR_DEN, 0x1,
+	VS6624_FR_NUM_LSB, 0xf,
+	VS6624_INIT_PIPE_SETUP, 0x0,
+	VS6624_IMG_FMT0, 0x0,
+	VS6624_YUV_SETUP, 0x1,
+	VS6624_IMAGE_SIZE0, 0x2,
+	0x0000, 0x00,
+};
+
+static inline struct vs6624 *to_vs6624(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct vs6624, sd);
+}
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct vs6624, hdl)->sd;
+}
+
+static int vs6624_read(struct v4l2_subdev *sd, u16 index)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u8 buf[2];
+
+	buf[0] = index >> 8;
+	buf[1] = index;
+	i2c_master_send(client, buf, 2);
+	i2c_master_recv(client, buf, 1);
+
+	return buf[0];
+}
+
+static int vs6624_write(struct v4l2_subdev *sd, u16 index,
+				u8 value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u8 buf[3];
+
+	buf[0] = index >> 8;
+	buf[1] = index;
+	buf[2] = value;
+
+	return i2c_master_send(client, buf, 3);
+}
+
+static int vs6624_writeregs(struct v4l2_subdev *sd, const u16 *regs)
+{
+	u16 reg;
+	u8 data;
+
+	while (*regs != 0x00) {
+		reg = *regs++;
+		data = *regs++;
+
+		vs6624_write(sd, reg, data);
+	}
+	return 0;
+}
+
+static int vs6624_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+
+	switch (ctrl->id) {
+	case V4L2_CID_CONTRAST:
+		vs6624_write(sd, VS6624_CONTRAST0, ctrl->val);
+		break;
+	case V4L2_CID_SATURATION:
+		vs6624_write(sd, VS6624_SATURATION0, ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+		vs6624_write(sd, VS6624_HMIRROR0, ctrl->val);
+		break;
+	case V4L2_CID_VFLIP:
+		vs6624_write(sd, VS6624_VFLIP0, ctrl->val);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int vs6624_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned index,
+				enum v4l2_mbus_pixelcode *code)
+{
+	if (index >= ARRAY_SIZE(vs6624_formats))
+		return -EINVAL;
+
+	*code = vs6624_formats[index].mbus_code;
+	return 0;
+}
+
+static int vs6624_try_mbus_fmt(struct v4l2_subdev *sd,
+				struct v4l2_mbus_framefmt *fmt)
+{
+	int index;
+
+	for (index = 0; index < ARRAY_SIZE(vs6624_formats); index++)
+		if (vs6624_formats[index].mbus_code == fmt->code)
+			break;
+	if (index >= ARRAY_SIZE(vs6624_formats)) {
+		/* default to first format */
+		index = 0;
+		fmt->code = vs6624_formats[0].mbus_code;
+	}
+
+	/* sensor mode is VGA */
+	if (fmt->width > VGA_WIDTH)
+		fmt->width = VGA_WIDTH;
+	if (fmt->height > VGA_HEIGHT)
+		fmt->height = VGA_HEIGHT;
+	fmt->width = fmt->width & (~3);
+	fmt->height = fmt->height & (~3);
+	fmt->field = V4L2_FIELD_NONE;
+	fmt->colorspace = vs6624_formats[index].colorspace;
+	return 0;
+}
+
+static int vs6624_s_mbus_fmt(struct v4l2_subdev *sd,
+				struct v4l2_mbus_framefmt *fmt)
+{
+	struct vs6624 *sensor = to_vs6624(sd);
+	int ret;
+
+	ret = vs6624_try_mbus_fmt(sd, fmt);
+	if (ret)
+		return ret;
+
+	/* set image format */
+	switch (fmt->code) {
+	case V4L2_MBUS_FMT_UYVY8_2X8:
+		vs6624_write(sd, VS6624_IMG_FMT0, 0x0);
+		vs6624_write(sd, VS6624_YUV_SETUP, 0x1);
+		break;
+	case V4L2_MBUS_FMT_YUYV8_2X8:
+		vs6624_write(sd, VS6624_IMG_FMT0, 0x0);
+		vs6624_write(sd, VS6624_YUV_SETUP, 0x3);
+		break;
+	case V4L2_MBUS_FMT_RGB565_2X8_LE:
+		vs6624_write(sd, VS6624_IMG_FMT0, 0x4);
+		vs6624_write(sd, VS6624_RGB_SETUP, 0x0);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/* set image size */
+	if ((fmt->width == VGA_WIDTH) && (fmt->height == VGA_HEIGHT))
+		vs6624_write(sd, VS6624_IMAGE_SIZE0, 0x2);
+	else if ((fmt->width == QVGA_WIDTH) && (fmt->height == QVGA_HEIGHT))
+		vs6624_write(sd, VS6624_IMAGE_SIZE0, 0x4);
+	else if ((fmt->width == QQVGA_WIDTH) && (fmt->height == QQVGA_HEIGHT))
+		vs6624_write(sd, VS6624_IMAGE_SIZE0, 0x6);
+	else if ((fmt->width == CIF_WIDTH) && (fmt->height == CIF_HEIGHT))
+		vs6624_write(sd, VS6624_IMAGE_SIZE0, 0x3);
+	else if ((fmt->width == QCIF_WIDTH) && (fmt->height == QCIF_HEIGHT))
+		vs6624_write(sd, VS6624_IMAGE_SIZE0, 0x5);
+	else if ((fmt->width == QQCIF_WIDTH) && (fmt->height == QQCIF_HEIGHT))
+		vs6624_write(sd, VS6624_IMAGE_SIZE0, 0x7);
+	else {
+		vs6624_write(sd, VS6624_IMAGE_SIZE0, 0x8);
+		vs6624_write(sd, VS6624_MAN_HSIZE0_MSB, fmt->width >> 8);
+		vs6624_write(sd, VS6624_MAN_HSIZE0_LSB, fmt->width & 0xFF);
+		vs6624_write(sd, VS6624_MAN_VSIZE0_MSB, fmt->height >> 8);
+		vs6624_write(sd, VS6624_MAN_VSIZE0_LSB, fmt->height & 0xFF);
+		vs6624_write(sd, VS6624_CROP_CTRL0, 0x1);
+	}
+
+	sensor->fmt = *fmt;
+
+	return 0;
+}
+
+static int vs6624_g_mbus_fmt(struct v4l2_subdev *sd,
+				struct v4l2_mbus_framefmt *fmt)
+{
+	struct vs6624 *sensor = to_vs6624(sd);
+
+	*fmt = sensor->fmt;
+	return 0;
+}
+
+static int vs6624_g_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *parms)
+{
+	struct vs6624 *sensor = to_vs6624(sd);
+	struct v4l2_captureparm *cp = &parms->parm.capture;
+
+	if (parms->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	memset(cp, 0, sizeof(*cp));
+	cp->capability = V4L2_CAP_TIMEPERFRAME;
+	cp->timeperframe.numerator = sensor->frame_rate.denominator;
+	cp->timeperframe.denominator = sensor->frame_rate.numerator;
+	return 0;
+}
+
+static int vs6624_s_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *parms)
+{
+	struct vs6624 *sensor = to_vs6624(sd);
+	struct v4l2_captureparm *cp = &parms->parm.capture;
+	struct v4l2_fract *tpf = &cp->timeperframe;
+
+	if (parms->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	if (cp->extendedmode != 0)
+		return -EINVAL;
+
+	if (tpf->numerator == 0 || tpf->denominator == 0
+		|| (tpf->denominator > tpf->numerator * MAX_FRAME_RATE)) {
+		/* reset to max frame rate */
+		tpf->numerator = 1;
+		tpf->denominator = MAX_FRAME_RATE;
+	}
+	sensor->frame_rate.numerator = tpf->denominator;
+	sensor->frame_rate.denominator = tpf->numerator;
+	vs6624_write(sd, VS6624_DISABLE_FR_DAMPER, 0x0);
+	vs6624_write(sd, VS6624_FR_NUM_MSB,
+			sensor->frame_rate.numerator >> 8);
+	vs6624_write(sd, VS6624_FR_NUM_LSB,
+			sensor->frame_rate.numerator & 0xFF);
+	vs6624_write(sd, VS6624_FR_DEN,
+			sensor->frame_rate.denominator & 0xFF);
+	return 0;
+}
+
+static int vs6624_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	if (enable)
+		vs6624_write(sd, VS6624_USER_CMD, 0x2);
+	else
+		vs6624_write(sd, VS6624_USER_CMD, 0x4);
+	udelay(100);
+	return 0;
+}
+
+static int vs6624_g_chip_ident(struct v4l2_subdev *sd,
+		struct v4l2_dbg_chip_ident *chip)
+{
+	int rev;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	rev = (vs6624_read(sd, VS6624_FW_VSN_MAJOR) << 8)
+		| vs6624_read(sd, VS6624_FW_VSN_MINOR);
+
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_VS6624, rev);
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int vs6624_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	reg->val = vs6624_read(sd, reg->reg & 0xffff);
+	reg->size = 1;
+	return 0;
+}
+
+static int vs6624_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	vs6624_write(sd, reg->reg & 0xffff, reg->val & 0xff);
+	return 0;
+}
+#endif
+
+static const struct v4l2_ctrl_ops vs6624_ctrl_ops = {
+	.s_ctrl = vs6624_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops vs6624_core_ops = {
+	.g_chip_ident = vs6624_g_chip_ident,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = vs6624_g_register,
+	.s_register = vs6624_s_register,
+#endif
+};
+
+static const struct v4l2_subdev_video_ops vs6624_video_ops = {
+	.enum_mbus_fmt = vs6624_enum_mbus_fmt,
+	.try_mbus_fmt = vs6624_try_mbus_fmt,
+	.s_mbus_fmt = vs6624_s_mbus_fmt,
+	.g_mbus_fmt = vs6624_g_mbus_fmt,
+	.s_parm = vs6624_s_parm,
+	.g_parm = vs6624_g_parm,
+	.s_stream = vs6624_s_stream,
+};
+
+static const struct v4l2_subdev_ops vs6624_ops = {
+	.core = &vs6624_core_ops,
+	.video = &vs6624_video_ops,
+};
+
+static int __devinit vs6624_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct vs6624 *sensor;
+	struct v4l2_subdev *sd;
+	struct v4l2_ctrl_handler *hdl;
+	const unsigned *ce;
+	int ret;
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C))
+		return -EIO;
+
+	ce = client->dev.platform_data;
+	if (ce == NULL)
+		return -EINVAL;
+
+	ret = gpio_request(*ce, "VS6624 Chip Enable");
+	if (ret) {
+		v4l_err(client, "failed to request GPIO %d\n", *ce);
+		return ret;
+	}
+	gpio_direction_output(*ce, 1);
+	/* wait 100ms before any further i2c writes are performed */
+	mdelay(100);
+
+	sensor = kzalloc(sizeof(*sensor), GFP_KERNEL);
+	if (sensor == NULL) {
+		gpio_free(*ce);
+		return -ENOMEM;
+	}
+
+	sd = &sensor->sd;
+	v4l2_i2c_subdev_init(sd, client, &vs6624_ops);
+
+	vs6624_writeregs(sd, vs6624_p1);
+	vs6624_write(sd, VS6624_MICRO_EN, 0x2);
+	vs6624_write(sd, VS6624_DIO_EN, 0x1);
+	mdelay(10);
+	vs6624_writeregs(sd, vs6624_p2);
+
+	vs6624_writeregs(sd, vs6624_default);
+	vs6624_write(sd, VS6624_HSYNC_SETUP, 0xF);
+	vs6624_writeregs(sd, vs6624_run_setup);
+
+	/* set frame rate */
+	sensor->frame_rate.numerator = MAX_FRAME_RATE;
+	sensor->frame_rate.denominator = 1;
+	vs6624_write(sd, VS6624_DISABLE_FR_DAMPER, 0x0);
+	vs6624_write(sd, VS6624_FR_NUM_MSB,
+			sensor->frame_rate.numerator >> 8);
+	vs6624_write(sd, VS6624_FR_NUM_LSB,
+			sensor->frame_rate.numerator & 0xFF);
+	vs6624_write(sd, VS6624_FR_DEN,
+			sensor->frame_rate.denominator & 0xFF);
+
+	sensor->fmt = vs6624_default_fmt;
+	sensor->ce_pin = *ce;
+
+	v4l_info(client, "chip found @ 0x%02x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	hdl = &sensor->hdl;
+	v4l2_ctrl_handler_init(hdl, 4);
+	v4l2_ctrl_new_std(hdl, &vs6624_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 0xFF, 1, 0x87);
+	v4l2_ctrl_new_std(hdl, &vs6624_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 0xFF, 1, 0x78);
+	v4l2_ctrl_new_std(hdl, &vs6624_ctrl_ops,
+			V4L2_CID_HFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(hdl, &vs6624_ctrl_ops,
+			V4L2_CID_VFLIP, 0, 1, 1, 0);
+	/* hook the control handler into the driver */
+	sd->ctrl_handler = hdl;
+	if (hdl->error) {
+		int err = hdl->error;
+
+		v4l2_ctrl_handler_free(hdl);
+		kfree(sensor);
+		gpio_free(*ce);
+		return err;
+	}
+
+	/* initialize the hardware to the default control values */
+	ret = v4l2_ctrl_handler_setup(hdl);
+	if (ret) {
+		v4l2_ctrl_handler_free(hdl);
+		kfree(sensor);
+		gpio_free(*ce);
+	}
+	return ret;
+}
+
+static int __devexit vs6624_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct vs6624 *sensor = to_vs6624(sd);
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+	gpio_free(sensor->ce_pin);
+	kfree(sensor);
+	return 0;
+}
+
+static const struct i2c_device_id vs6624_id[] = {
+	{"vs6624", 0},
+	{},
+};
+
+MODULE_DEVICE_TABLE(i2c, vs6624_id);
+
+static struct i2c_driver vs6624_driver = {
+	.driver = {
+		.owner  = THIS_MODULE,
+		.name   = "vs6624",
+	},
+	.probe          = vs6624_probe,
+	.remove         = __devexit_p(vs6624_remove),
+	.id_table       = vs6624_id,
+};
+
+static __init int vs6624_init(void)
+{
+	return i2c_add_driver(&vs6624_driver);
+}
+
+static __exit void vs6624_exit(void)
+{
+	i2c_del_driver(&vs6624_driver);
+}
+
+module_init(vs6624_init);
+module_exit(vs6624_exit);
+
+MODULE_DESCRIPTION("VS6624 sensor driver");
+MODULE_AUTHOR("Scott Jiang <Scott.Jiang.Linux@gmail.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/vs6624_regs.h b/drivers/media/i2c/vs6624_regs.h
new file mode 100644
index 000000000000..6ba2ee25827e
--- /dev/null
+++ b/drivers/media/i2c/vs6624_regs.h
@@ -0,0 +1,337 @@
+/*
+ * vs6624 - ST VS6624 CMOS image sensor registers
+ *
+ * Copyright (c) 2011 Analog Devices Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _VS6624_REGS_H_
+#define _VS6624_REGS_H_
+
+/* low level control registers */
+#define VS6624_MICRO_EN               0xC003 /* power enable for all MCU clock */
+#define VS6624_DIO_EN                 0xC044 /* enable digital I/O */
+/* device parameters */
+#define VS6624_DEV_ID_MSB             0x0001 /* device id MSB */
+#define VS6624_DEV_ID_LSB             0x0002 /* device id LSB */
+#define VS6624_FW_VSN_MAJOR           0x0004 /* firmware version major */
+#define VS6624_FW_VSN_MINOR           0x0006 /* firmware version minor */
+#define VS6624_PATCH_VSN_MAJOR        0x0008 /* patch version major */
+#define VS6624_PATCH_VSN_MINOR        0x000A /* patch version minor */
+/* host interface manager control */
+#define VS6624_USER_CMD               0x0180 /* user level control of operating states */
+/* host interface manager status */
+#define VS6624_STATE                  0x0202 /* current state of the mode manager */
+/* run mode control */
+#define VS6624_METER_ON               0x0280 /* if false AE and AWB are disabled */
+/* mode setup */
+#define VS6624_ACTIVE_PIPE_SETUP      0x0302 /* select the active bank for non view live mode */
+#define VS6624_SENSOR_MODE            0x0308 /* select the different sensor mode */
+/* pipe setup bank0 */
+#define VS6624_IMAGE_SIZE0            0x0380 /* required output dimension */
+#define VS6624_MAN_HSIZE0_MSB         0x0383 /* input required manual H size MSB */
+#define VS6624_MAN_HSIZE0_LSB         0x0384 /* input required manual H size LSB */
+#define VS6624_MAN_VSIZE0_MSB         0x0387 /* input required manual V size MSB */
+#define VS6624_MAN_VSIZE0_LSB         0x0388 /* input required manual V size LSB */
+#define VS6624_ZOOM_HSTEP0_MSB        0x038B /* set the zoom H step MSB */
+#define VS6624_ZOOM_HSTEP0_LSB        0x038C /* set the zoom H step LSB */
+#define VS6624_ZOOM_VSTEP0_MSB        0x038F /* set the zoom V step MSB */
+#define VS6624_ZOOM_VSTEP0_LSB        0x0390 /* set the zoom V step LSB */
+#define VS6624_ZOOM_CTRL0             0x0392 /* control zoon in, out and stop */
+#define VS6624_PAN_HSTEP0_MSB         0x0395 /* set the pan H step MSB */
+#define VS6624_PAN_HSTEP0_LSB         0x0396 /* set the pan H step LSB */
+#define VS6624_PAN_VSTEP0_MSB         0x0399 /* set the pan V step MSB */
+#define VS6624_PAN_VSTEP0_LSB         0x039A /* set the pan V step LSB */
+#define VS6624_PAN_CTRL0              0x039C /* control pan operation */
+#define VS6624_CROP_CTRL0             0x039E /* select cropping mode */
+#define VS6624_CROP_HSTART0_MSB       0x03A1 /* set the cropping H start address MSB */
+#define VS6624_CROP_HSTART0_LSB       0x03A2 /* set the cropping H start address LSB */
+#define VS6624_CROP_HSIZE0_MSB        0x03A5 /* set the cropping H size MSB */
+#define VS6624_CROP_HSIZE0_LSB        0x03A6 /* set the cropping H size LSB */
+#define VS6624_CROP_VSTART0_MSB       0x03A9 /* set the cropping V start address MSB */
+#define VS6624_CROP_VSTART0_LSB       0x03AA /* set the cropping V start address LSB */
+#define VS6624_CROP_VSIZE0_MSB        0x03AD /* set the cropping V size MSB */
+#define VS6624_CROP_VSIZE0_LSB        0x03AE /* set the cropping V size LSB */
+#define VS6624_IMG_FMT0               0x03B0 /* select required output image format */
+#define VS6624_BAYER_OUT_ALIGN0       0x03B2 /* set bayer output alignment */
+#define VS6624_CONTRAST0              0x03B4 /* contrast control for output */
+#define VS6624_SATURATION0            0x03B6 /* saturation control for output */
+#define VS6624_GAMMA0                 0x03B8 /* gamma settings */
+#define VS6624_HMIRROR0               0x03BA /* horizontal image orientation flip */
+#define VS6624_VFLIP0                 0x03BC /* vertical image orientation flip */
+#define VS6624_CHANNEL_ID0            0x03BE /* logical DMA channel number */
+/* pipe setup bank1 */
+#define VS6624_IMAGE_SIZE1            0x0400 /* required output dimension */
+#define VS6624_MAN_HSIZE1_MSB         0x0403 /* input required manual H size MSB */
+#define VS6624_MAN_HSIZE1_LSB         0x0404 /* input required manual H size LSB */
+#define VS6624_MAN_VSIZE1_MSB         0x0407 /* input required manual V size MSB */
+#define VS6624_MAN_VSIZE1_LSB         0x0408 /* input required manual V size LSB */
+#define VS6624_ZOOM_HSTEP1_MSB        0x040B /* set the zoom H step MSB */
+#define VS6624_ZOOM_HSTEP1_LSB        0x040C /* set the zoom H step LSB */
+#define VS6624_ZOOM_VSTEP1_MSB        0x040F /* set the zoom V step MSB */
+#define VS6624_ZOOM_VSTEP1_LSB        0x0410 /* set the zoom V step LSB */
+#define VS6624_ZOOM_CTRL1             0x0412 /* control zoon in, out and stop */
+#define VS6624_PAN_HSTEP1_MSB         0x0415 /* set the pan H step MSB */
+#define VS6624_PAN_HSTEP1_LSB         0x0416 /* set the pan H step LSB */
+#define VS6624_PAN_VSTEP1_MSB         0x0419 /* set the pan V step MSB */
+#define VS6624_PAN_VSTEP1_LSB         0x041A /* set the pan V step LSB */
+#define VS6624_PAN_CTRL1              0x041C /* control pan operation */
+#define VS6624_CROP_CTRL1             0x041E /* select cropping mode */
+#define VS6624_CROP_HSTART1_MSB       0x0421 /* set the cropping H start address MSB */
+#define VS6624_CROP_HSTART1_LSB       0x0422 /* set the cropping H start address LSB */
+#define VS6624_CROP_HSIZE1_MSB        0x0425 /* set the cropping H size MSB */
+#define VS6624_CROP_HSIZE1_LSB        0x0426 /* set the cropping H size LSB */
+#define VS6624_CROP_VSTART1_MSB       0x0429 /* set the cropping V start address MSB */
+#define VS6624_CROP_VSTART1_LSB       0x042A /* set the cropping V start address LSB */
+#define VS6624_CROP_VSIZE1_MSB        0x042D /* set the cropping V size MSB */
+#define VS6624_CROP_VSIZE1_LSB        0x042E /* set the cropping V size LSB */
+#define VS6624_IMG_FMT1               0x0430 /* select required output image format */
+#define VS6624_BAYER_OUT_ALIGN1       0x0432 /* set bayer output alignment */
+#define VS6624_CONTRAST1              0x0434 /* contrast control for output */
+#define VS6624_SATURATION1            0x0436 /* saturation control for output */
+#define VS6624_GAMMA1                 0x0438 /* gamma settings */
+#define VS6624_HMIRROR1               0x043A /* horizontal image orientation flip */
+#define VS6624_VFLIP1                 0x043C /* vertical image orientation flip */
+#define VS6624_CHANNEL_ID1            0x043E /* logical DMA channel number */
+/* view live control */
+#define VS6624_VIEW_LIVE_EN           0x0480 /* enable view live mode */
+#define VS6624_INIT_PIPE_SETUP        0x0482 /* select initial pipe setup bank */
+/* view live status */
+#define VS6624_CUR_PIPE_SETUP         0x0500 /* indicates most recently applied setup bank */
+/* power management */
+#define VS6624_TIME_TO_POWER_DOWN     0x0580 /* automatically transition time to stop mode */
+/* video timing parameter host inputs */
+#define VS6624_EXT_CLK_FREQ_NUM_MSB   0x0605 /* external clock frequency numerator MSB */
+#define VS6624_EXT_CLK_FREQ_NUM_LSB   0x0606 /* external clock frequency numerator LSB */
+#define VS6624_EXT_CLK_FREQ_DEN       0x0608 /* external clock frequency denominator */
+/* video timing control */
+#define VS6624_SYS_CLK_MODE           0x0880 /* decides system clock frequency */
+/* frame dimension parameter host inputs */
+#define VS6624_LIGHT_FREQ             0x0C80 /* AC frequency used for flicker free time */
+#define VS6624_FLICKER_COMPAT         0x0C82 /* flicker compatible frame length */
+/* static frame rate control */
+#define VS6624_FR_NUM_MSB             0x0D81 /* desired frame rate numerator MSB */
+#define VS6624_FR_NUM_LSB             0x0D82 /* desired frame rate numerator LSB */
+#define VS6624_FR_DEN                 0x0D84 /* desired frame rate denominator */
+/* automatic frame rate control */
+#define VS6624_DISABLE_FR_DAMPER      0x0E80 /* defines frame rate mode */
+#define VS6624_MIN_DAMPER_OUT_MSB     0x0E8C /* minimum frame rate MSB */
+#define VS6624_MIN_DAMPER_OUT_LSB     0x0E8A /* minimum frame rate LSB */
+/* exposure controls */
+#define VS6624_EXPO_MODE              0x1180 /* exposure mode */
+#define VS6624_EXPO_METER             0x1182 /* weights to be associated with the zones */
+#define VS6624_EXPO_TIME_NUM          0x1184 /* exposure time numerator */
+#define VS6624_EXPO_TIME_DEN          0x1186 /* exposure time denominator */
+#define VS6624_EXPO_TIME_MSB          0x1189 /* exposure time for the Manual Mode MSB */
+#define VS6624_EXPO_TIME_LSB          0x118A /* exposure time for the Manual Mode LSB */
+#define VS6624_EXPO_COMPENSATION      0x1190 /* exposure compensation */
+#define VS6624_DIRECT_COARSE_MSB      0x1195 /* coarse integration lines for Direct Mode MSB */
+#define VS6624_DIRECT_COARSE_LSB      0x1196 /* coarse integration lines for Direct Mode LSB */
+#define VS6624_DIRECT_FINE_MSB        0x1199 /* fine integration pixels for Direct Mode MSB */
+#define VS6624_DIRECT_FINE_LSB        0x119A /* fine integration pixels for Direct Mode LSB */
+#define VS6624_DIRECT_ANAL_GAIN_MSB   0x119D /* analog gain for Direct Mode MSB */
+#define VS6624_DIRECT_ANAL_GAIN_LSB   0x119E /* analog gain for Direct Mode LSB */
+#define VS6624_DIRECT_DIGI_GAIN_MSB   0x11A1 /* digital gain for Direct Mode MSB */
+#define VS6624_DIRECT_DIGI_GAIN_LSB   0x11A2 /* digital gain for Direct Mode LSB */
+#define VS6624_FLASH_COARSE_MSB       0x11A5 /* coarse integration lines for Flash Gun Mode MSB */
+#define VS6624_FLASH_COARSE_LSB       0x11A6 /* coarse integration lines for Flash Gun Mode LSB */
+#define VS6624_FLASH_FINE_MSB         0x11A9 /* fine integration pixels for Flash Gun Mode MSB */
+#define VS6624_FLASH_FINE_LSB         0x11AA /* fine integration pixels for Flash Gun Mode LSB */
+#define VS6624_FLASH_ANAL_GAIN_MSB    0x11AD /* analog gain for Flash Gun Mode MSB */
+#define VS6624_FLASH_ANAL_GAIN_LSB    0x11AE /* analog gain for Flash Gun Mode LSB */
+#define VS6624_FLASH_DIGI_GAIN_MSB    0x11B1 /* digital gain for Flash Gun Mode MSB */
+#define VS6624_FLASH_DIGI_GAIN_LSB    0x11B2 /* digital gain for Flash Gun Mode LSB */
+#define VS6624_FREEZE_AE              0x11B4 /* freeze auto exposure */
+#define VS6624_MAX_INT_TIME_MSB       0x11B7 /* user maximum integration time MSB */
+#define VS6624_MAX_INT_TIME_LSB       0x11B8 /* user maximum integration time LSB */
+#define VS6624_FLASH_AG_THR_MSB       0x11BB /* recommend flash gun analog gain threshold MSB */
+#define VS6624_FLASH_AG_THR_LSB       0x11BC /* recommend flash gun analog gain threshold LSB */
+#define VS6624_ANTI_FLICKER_MODE      0x11C0 /* anti flicker mode */
+/* white balance control */
+#define VS6624_WB_MODE                0x1480 /* set white balance mode */
+#define VS6624_MAN_RG                 0x1482 /* user setting for red channel gain */
+#define VS6624_MAN_GG                 0x1484 /* user setting for green channel gain */
+#define VS6624_MAN_BG                 0x1486 /* user setting for blue channel gain */
+#define VS6624_FLASH_RG_MSB           0x148B /* red gain for Flash Gun MSB */
+#define VS6624_FLASH_RG_LSB           0x148C /* red gain for Flash Gun LSB */
+#define VS6624_FLASH_GG_MSB           0x148F /* green gain for Flash Gun MSB */
+#define VS6624_FLASH_GG_LSB           0x1490 /* green gain for Flash Gun LSB */
+#define VS6624_FLASH_BG_MSB           0x1493 /* blue gain for Flash Gun MSB */
+#define VS6624_FLASH_BG_LSB           0x1494 /* blue gain for Flash Gun LSB */
+/* sensor setup */
+#define VS6624_BC_OFFSET              0x1990 /* Black Correction Offset */
+/* image stability */
+#define VS6624_STABLE_WB              0x1900 /* white balance stable */
+#define VS6624_STABLE_EXPO            0x1902 /* exposure stable */
+#define VS6624_STABLE                 0x1906 /* system stable */
+/* flash control */
+#define VS6624_FLASH_MODE             0x1A80 /* flash mode */
+#define VS6624_FLASH_OFF_LINE_MSB     0x1A83 /* off line at flash pulse mode MSB */
+#define VS6624_FLASH_OFF_LINE_LSB     0x1A84 /* off line at flash pulse mode LSB */
+/* flash status */
+#define VS6624_FLASH_RECOM            0x1B00 /* flash gun is recommended */
+#define VS6624_FLASH_GRAB_COMPLETE    0x1B02 /* flash gun image has been grabbed */
+/* scythe filter controls */
+#define VS6624_SCYTHE_FILTER          0x1D80 /* disable scythe defect correction */
+/* jack filter controls */
+#define VS6624_JACK_FILTER            0x1E00 /* disable jack defect correction */
+/* demosaic control */
+#define VS6624_ANTI_ALIAS_FILTER      0x1E80 /* anti alias filter suppress */
+/* color matrix dampers */
+#define VS6624_CM_DISABLE             0x1F00 /* disable color matrix damper */
+#define VS6624_CM_LOW_THR_MSB         0x1F03 /* low threshold for exposure MSB */
+#define VS6624_CM_LOW_THR_LSB         0x1F04 /* low threshold for exposure LSB */
+#define VS6624_CM_HIGH_THR_MSB        0x1F07 /* high threshold for exposure MSB */
+#define VS6624_CM_HIGH_THR_LSB        0x1F08 /* high threshold for exposure LSB */
+#define VS6624_CM_MIN_OUT_MSB         0x1F0B /* minimum possible damper output MSB */
+#define VS6624_CM_MIN_OUT_LSB         0x1F0C /* minimum possible damper output LSB */
+/* peaking control */
+#define VS6624_PEAK_GAIN              0x2000 /* controls peaking gain */
+#define VS6624_PEAK_G_DISABLE         0x2002 /* disable peak gain damping */
+#define VS6624_PEAK_LOW_THR_G_MSB     0x2005 /* low threshold for exposure for gain MSB */
+#define VS6624_PEAK_LOW_THR_G_LSB     0x2006 /* low threshold for exposure for gain LSB */
+#define VS6624_PEAK_HIGH_THR_G_MSB    0x2009 /* high threshold for exposure for gain MSB */
+#define VS6624_PEAK_HIGH_THR_G_LSB    0x200A /* high threshold for exposure for gain LSB */
+#define VS6624_PEAK_MIN_OUT_G_MSB     0x200D /* minimum damper output for gain MSB */
+#define VS6624_PEAK_MIN_OUT_G_LSB     0x200E /* minimum damper output for gain LSB */
+#define VS6624_PEAK_LOW_THR           0x2010 /* adjust degree of coring */
+#define VS6624_PEAK_C_DISABLE         0x2012 /* disable coring damping */
+#define VS6624_PEAK_HIGH_THR          0x2014 /* adjust maximum gain */
+#define VS6624_PEAK_LOW_THR_C_MSB     0x2017 /* low threshold for exposure for coring MSB */
+#define VS6624_PEAK_LOW_THR_C_LSB     0x2018 /* low threshold for exposure for coring LSB */
+#define VS6624_PEAK_HIGH_THR_C_MSB    0x201B /* high threshold for exposure for coring MSB */
+#define VS6624_PEAK_HIGH_THR_C_LSB    0x201C /* high threshold for exposure for coring LSB */
+#define VS6624_PEAK_MIN_OUT_C_MSB     0x201F /* minimum damper output for coring MSB */
+#define VS6624_PEAK_MIN_OUT_C_LSB     0x2020 /* minimum damper output for coring LSB */
+/* pipe 0 RGB to YUV matrix manual control */
+#define VS6624_RYM0_MAN_CTRL          0x2180 /* enable manual RGB to YUV matrix */
+#define VS6624_RYM0_W00_MSB           0x2183 /* row 0 column 0 of YUV matrix MSB */
+#define VS6624_RYM0_W00_LSB           0x2184 /* row 0 column 0 of YUV matrix LSB */
+#define VS6624_RYM0_W01_MSB           0x2187 /* row 0 column 1 of YUV matrix MSB */
+#define VS6624_RYM0_W01_LSB           0x2188 /* row 0 column 1 of YUV matrix LSB */
+#define VS6624_RYM0_W02_MSB           0x218C /* row 0 column 2 of YUV matrix MSB */
+#define VS6624_RYM0_W02_LSB           0x218D /* row 0 column 2 of YUV matrix LSB */
+#define VS6624_RYM0_W10_MSB           0x2190 /* row 1 column 0 of YUV matrix MSB */
+#define VS6624_RYM0_W10_LSB           0x218F /* row 1 column 0 of YUV matrix LSB */
+#define VS6624_RYM0_W11_MSB           0x2193 /* row 1 column 1 of YUV matrix MSB */
+#define VS6624_RYM0_W11_LSB           0x2194 /* row 1 column 1 of YUV matrix LSB */
+#define VS6624_RYM0_W12_MSB           0x2197 /* row 1 column 2 of YUV matrix MSB */
+#define VS6624_RYM0_W12_LSB           0x2198 /* row 1 column 2 of YUV matrix LSB */
+#define VS6624_RYM0_W20_MSB           0x219B /* row 2 column 0 of YUV matrix MSB */
+#define VS6624_RYM0_W20_LSB           0x219C /* row 2 column 0 of YUV matrix LSB */
+#define VS6624_RYM0_W21_MSB           0x21A0 /* row 2 column 1 of YUV matrix MSB */
+#define VS6624_RYM0_W21_LSB           0x219F /* row 2 column 1 of YUV matrix LSB */
+#define VS6624_RYM0_W22_MSB           0x21A3 /* row 2 column 2 of YUV matrix MSB */
+#define VS6624_RYM0_W22_LSB           0x21A4 /* row 2 column 2 of YUV matrix LSB */
+#define VS6624_RYM0_YINY_MSB          0x21A7 /* Y in Y MSB */
+#define VS6624_RYM0_YINY_LSB          0x21A8 /* Y in Y LSB */
+#define VS6624_RYM0_YINCB_MSB         0x21AB /* Y in Cb MSB */
+#define VS6624_RYM0_YINCB_LSB         0x21AC /* Y in Cb LSB */
+#define VS6624_RYM0_YINCR_MSB         0x21B0 /* Y in Cr MSB */
+#define VS6624_RYM0_YINCR_LSB         0x21AF /* Y in Cr LSB */
+/* pipe 1 RGB to YUV matrix manual control */
+#define VS6624_RYM1_MAN_CTRL          0x2200 /* enable manual RGB to YUV matrix */
+#define VS6624_RYM1_W00_MSB           0x2203 /* row 0 column 0 of YUV matrix MSB */
+#define VS6624_RYM1_W00_LSB           0x2204 /* row 0 column 0 of YUV matrix LSB */
+#define VS6624_RYM1_W01_MSB           0x2207 /* row 0 column 1 of YUV matrix MSB */
+#define VS6624_RYM1_W01_LSB           0x2208 /* row 0 column 1 of YUV matrix LSB */
+#define VS6624_RYM1_W02_MSB           0x220C /* row 0 column 2 of YUV matrix MSB */
+#define VS6624_RYM1_W02_LSB           0x220D /* row 0 column 2 of YUV matrix LSB */
+#define VS6624_RYM1_W10_MSB           0x2210 /* row 1 column 0 of YUV matrix MSB */
+#define VS6624_RYM1_W10_LSB           0x220F /* row 1 column 0 of YUV matrix LSB */
+#define VS6624_RYM1_W11_MSB           0x2213 /* row 1 column 1 of YUV matrix MSB */
+#define VS6624_RYM1_W11_LSB           0x2214 /* row 1 column 1 of YUV matrix LSB */
+#define VS6624_RYM1_W12_MSB           0x2217 /* row 1 column 2 of YUV matrix MSB */
+#define VS6624_RYM1_W12_LSB           0x2218 /* row 1 column 2 of YUV matrix LSB */
+#define VS6624_RYM1_W20_MSB           0x221B /* row 2 column 0 of YUV matrix MSB */
+#define VS6624_RYM1_W20_LSB           0x221C /* row 2 column 0 of YUV matrix LSB */
+#define VS6624_RYM1_W21_MSB           0x2220 /* row 2 column 1 of YUV matrix MSB */
+#define VS6624_RYM1_W21_LSB           0x221F /* row 2 column 1 of YUV matrix LSB */
+#define VS6624_RYM1_W22_MSB           0x2223 /* row 2 column 2 of YUV matrix MSB */
+#define VS6624_RYM1_W22_LSB           0x2224 /* row 2 column 2 of YUV matrix LSB */
+#define VS6624_RYM1_YINY_MSB          0x2227 /* Y in Y MSB */
+#define VS6624_RYM1_YINY_LSB          0x2228 /* Y in Y LSB */
+#define VS6624_RYM1_YINCB_MSB         0x222B /* Y in Cb MSB */
+#define VS6624_RYM1_YINCB_LSB         0x222C /* Y in Cb LSB */
+#define VS6624_RYM1_YINCR_MSB         0x2220 /* Y in Cr MSB */
+#define VS6624_RYM1_YINCR_LSB         0x222F /* Y in Cr LSB */
+/* pipe 0 gamma manual control */
+#define VS6624_GAMMA_MAN_CTRL0        0x2280 /* enable manual gamma setup */
+#define VS6624_GAMMA_PEAK_R0          0x2282 /* peaked red channel gamma value */
+#define VS6624_GAMMA_PEAK_G0          0x2284 /* peaked green channel gamma value */
+#define VS6624_GAMMA_PEAK_B0          0x2286 /* peaked blue channel gamma value */
+#define VS6624_GAMMA_UNPEAK_R0        0x2288 /* unpeaked red channel gamma value */
+#define VS6624_GAMMA_UNPEAK_G0        0x228A /* unpeaked green channel gamma value */
+#define VS6624_GAMMA_UNPEAK_B0        0x228C /* unpeaked blue channel gamma value */
+/* pipe 1 gamma manual control */
+#define VS6624_GAMMA_MAN_CTRL1        0x2300 /* enable manual gamma setup */
+#define VS6624_GAMMA_PEAK_R1          0x2302 /* peaked red channel gamma value */
+#define VS6624_GAMMA_PEAK_G1          0x2304 /* peaked green channel gamma value */
+#define VS6624_GAMMA_PEAK_B1          0x2306 /* peaked blue channel gamma value */
+#define VS6624_GAMMA_UNPEAK_R1        0x2308 /* unpeaked red channel gamma value */
+#define VS6624_GAMMA_UNPEAK_G1        0x230A /* unpeaked green channel gamma value */
+#define VS6624_GAMMA_UNPEAK_B1        0x230C /* unpeaked blue channel gamma value */
+/* fade to black */
+#define VS6624_F2B_DISABLE            0x2480 /* disable fade to black */
+#define VS6624_F2B_BLACK_VAL_MSB      0x2483 /* black value MSB */
+#define VS6624_F2B_BLACK_VAL_LSB      0x2484 /* black value LSB */
+#define VS6624_F2B_LOW_THR_MSB        0x2487 /* low threshold for exposure MSB */
+#define VS6624_F2B_LOW_THR_LSB        0x2488 /* low threshold for exposure LSB */
+#define VS6624_F2B_HIGH_THR_MSB       0x248B /* high threshold for exposure MSB */
+#define VS6624_F2B_HIGH_THR_LSB       0x248C /* high threshold for exposure LSB */
+#define VS6624_F2B_MIN_OUT_MSB        0x248F /* minimum damper output MSB */
+#define VS6624_F2B_MIN_OUT_LSB        0x2490 /* minimum damper output LSB */
+/* output formatter control */
+#define VS6624_CODE_CK_EN             0x2580 /* code check enable */
+#define VS6624_BLANK_FMT              0x2582 /* blank format */
+#define VS6624_SYNC_CODE_SETUP        0x2584 /* sync code setup */
+#define VS6624_HSYNC_SETUP            0x2586 /* H sync setup */
+#define VS6624_VSYNC_SETUP            0x2588 /* V sync setup */
+#define VS6624_PCLK_SETUP             0x258A /* PCLK setup */
+#define VS6624_PCLK_EN                0x258C /* PCLK enable */
+#define VS6624_OPF_SP_SETUP           0x258E /* output formatter sp setup */
+#define VS6624_BLANK_DATA_MSB         0x2590 /* blank data MSB */
+#define VS6624_BLANK_DATA_LSB         0x2592 /* blank data LSB */
+#define VS6624_RGB_SETUP              0x2594 /* RGB setup */
+#define VS6624_YUV_SETUP              0x2596 /* YUV setup */
+#define VS6624_VSYNC_RIS_COARSE_H     0x2598 /* V sync rising coarse high */
+#define VS6624_VSYNC_RIS_COARSE_L     0x259A /* V sync rising coarse low */
+#define VS6624_VSYNC_RIS_FINE_H       0x259C /* V sync rising fine high */
+#define VS6624_VSYNC_RIS_FINE_L       0x259E /* V sync rising fine low */
+#define VS6624_VSYNC_FALL_COARSE_H    0x25A0 /* V sync falling coarse high */
+#define VS6624_VSYNC_FALL_COARSE_L    0x25A2 /* V sync falling coarse low */
+#define VS6624_VSYNC_FALL_FINE_H      0x25A4 /* V sync falling fine high */
+#define VS6624_VSYNC_FALL_FINE_L      0x25A6 /* V sync falling fine low */
+#define VS6624_HSYNC_RIS_H            0x25A8 /* H sync rising high */
+#define VS6624_HSYNC_RIS_L            0x25AA /* H sync rising low */
+#define VS6624_HSYNC_FALL_H           0x25AC /* H sync falling high */
+#define VS6624_HSYNC_FALL_L           0x25AE /* H sync falling low */
+#define VS6624_OUT_IF                 0x25B0 /* output interface */
+#define VS6624_CCP_EXT_DATA           0x25B2 /* CCP extra data */
+/* NoRA controls */
+#define VS6624_NORA_DISABLE           0x2600 /* NoRA control mode */
+#define VS6624_NORA_USAGE             0x2602 /* usage */
+#define VS6624_NORA_SPLIT_KN          0x2604 /* split kn */
+#define VS6624_NORA_SPLIT_NI          0x2606 /* split ni */
+#define VS6624_NORA_TIGHT_G           0x2608 /* tight green */
+#define VS6624_NORA_DISABLE_NP        0x260A /* disable noro promoting */
+#define VS6624_NORA_LOW_THR_MSB       0x260D /* low threshold for exposure MSB */
+#define VS6624_NORA_LOW_THR_LSB       0x260E /* low threshold for exposure LSB */
+#define VS6624_NORA_HIGH_THR_MSB      0x2611 /* high threshold for exposure MSB */
+#define VS6624_NORA_HIGH_THR_LSB      0x2612 /* high threshold for exposure LSB */
+#define VS6624_NORA_MIN_OUT_MSB       0x2615 /* minimum damper output MSB */
+#define VS6624_NORA_MIN_OUT_LSB       0x2616 /* minimum damper output LSB */
+
+#endif
diff --git a/drivers/media/i2c/wm8739.c b/drivers/media/i2c/wm8739.c
new file mode 100644
index 000000000000..3bb99e93febe
--- /dev/null
+++ b/drivers/media/i2c/wm8739.c
@@ -0,0 +1,294 @@
+/*
+ * wm8739
+ *
+ * Copyright (C) 2005 T. Adachi <tadachi@tadachi-net.com>
+ *
+ * Copyright (C) 2005 Hans Verkuil <hverkuil@xs4all.nl>
+ * - Cleanup
+ *
+ * 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; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <linux/ioctl.h>
+#include <asm/uaccess.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.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_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ident *chip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_WM8739, 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,
+	.g_chip_ident = wm8739_g_chip_ident,
+	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
+	.try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
+	.s_ext_ctrls = v4l2_subdev_s_ext_ctrls,
+	.g_ctrl = v4l2_subdev_g_ctrl,
+	.s_ctrl = v4l2_subdev_s_ctrl,
+	.queryctrl = v4l2_subdev_queryctrl,
+	.querymenu = v4l2_subdev_querymenu,
+};
+
+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,
+			const struct i2c_device_id *id)
+{
+	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 = kzalloc(sizeof(struct wm8739_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);
+		kfree(state);
+		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 int 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);
+	kfree(to_state(sd));
+	return 0;
+}
+
+static const struct i2c_device_id wm8739_id[] = {
+	{ "wm8739", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, wm8739_id);
+
+static struct i2c_driver wm8739_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.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 000000000000..bee77ea9f49e
--- /dev/null
+++ b/drivers/media/i2c/wm8775.c
@@ -0,0 +1,342 @@
+/*
+ * 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
+ *
+ * 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; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <linux/ioctl.h>
+#include <asm/uaccess.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-ctrls.h>
+#include <media/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;
+	if (!v4l2_ctrl_g_ctrl(state->bal))
+		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_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ident *chip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_WM8775, 0);
+}
+
+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, 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,
+	.g_chip_ident = wm8775_g_chip_ident,
+	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
+	.try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
+	.s_ext_ctrls = v4l2_subdev_s_ext_ctrls,
+	.g_ctrl = v4l2_subdev_g_ctrl,
+	.s_ctrl = v4l2_subdev_s_ctrl,
+	.queryctrl = v4l2_subdev_queryctrl,
+	.querymenu = v4l2_subdev_querymenu,
+};
+
+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,
+			const struct i2c_device_id *id)
+{
+	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 = kzalloc(sizeof(struct wm8775_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);
+		kfree(state);
+		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 int 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);
+	kfree(state);
+	return 0;
+}
+
+static const struct i2c_device_id wm8775_id[] = {
+	{ "wm8775", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, wm8775_id);
+
+static struct i2c_driver wm8775_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "wm8775",
+	},
+	.probe		= wm8775_probe,
+	.remove		= wm8775_remove,
+	.id_table	= wm8775_id,
+};
+
+module_i2c_driver(wm8775_driver);