Linux 2.6.31 Release for Digi ConnectCore Wi-i.MX boards2.6.31-digi-201102101717

1 files changed, 769 insertions, 316 deletions

diff --git a/drivers/media/video/mxc/capture/mt9v111.c b/drivers/media/video/mxc/capture/mt9v111.c
index c95a20683924..dfdd455db3dd 100644
--- a/drivers/media/video/mxc/capture/mt9v111.c
+++ b/drivers/media/video/mxc/capture/mt9v111.c
@@ -18,6 +18,9 @@
  *
  * @ingroup Camera
  */
+
+//#define MT9V111_DEBUG
+
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/slab.h>
@@ -40,7 +43,6 @@ static mt9v111_conf mt9v111_device;
 /*!
  * Holds the current frame rate.
  */
-static int reset_frame_rate = MT9V111_FRAME_RATE;
 
 struct sensor {
 	const struct mt9v111_platform_data *platform_data;
@@ -49,33 +51,45 @@ struct sensor {
 	struct v4l2_pix_format pix;
 	struct v4l2_captureparm streamcap;
 	bool on;
+	bool used;
 
 	/* control settings */
 	int brightness;
-	int hue;
-	int contrast;
 	int saturation;
-	int red;
-	int green;
-	int blue;
+	int sharpness;
+	int gain;
 	int ae_mode;
 
-} mt9v111_data;
-
-extern void gpio_sensor_active(void);
-extern void gpio_sensor_inactive(void);
+};
 
 static int mt9v111_probe(struct i2c_client *client,
 			 const struct i2c_device_id *id);
 static int mt9v111_remove(struct i2c_client *client);
 
 static const struct i2c_device_id mt9v111_id[] = {
-	{"mt9v111", 0},
+	{"mt9v111_1", 2},
+	{"mt9v111_2", 3},
 	{},
 };
 
+struct sensor mt9v111_data[ARRAY_SIZE(mt9v111_id)-1];
+
 MODULE_DEVICE_TABLE(i2c, mt9v111_id);
 
+static int mt9v111_suspend(struct i2c_client *client, pm_message_t mesg)
+{
+	pr_debug("In mt9v111_suspend\n");
+
+	return 0;
+}
+
+static int mt9v111_resume(struct i2c_client *client)
+{
+	pr_debug("In mt9v111_resume\n");
+
+	return 0;
+}
+
 static struct i2c_driver mt9v111_i2c_driver = {
 	.driver = {
 		   .owner = THIS_MODULE,
@@ -84,37 +98,52 @@ static struct i2c_driver mt9v111_i2c_driver = {
 	.probe = mt9v111_probe,
 	.remove = mt9v111_remove,
 	.id_table = mt9v111_id,
-/* To add power management add .suspend and .resume functions */
+	.suspend = mt9v111_suspend,
+	.resume = mt9v111_resume,
 };
 
 /*
  * Function definitions
  */
 
-#ifdef MT9V111_DEBUG
-static inline int mt9v111_read_reg(u8 reg)
+static int mt9v111_id_from_name ( const char * name )
+{
+	int id = -1;
+
+	if( name == NULL || ( strlen(name) < strlen("mt9v111_n") ) )
+		return -1;
+
+	id = (int)simple_strtol(name+strlen("mt9v111_"),NULL,0) - 1;
+	if( id >= ARRAY_SIZE(mt9v111_id) ) {
+		printk("Invalid sensor index %d for %s\n",id,name);
+		return -1;
+	}
+
+	return id;
+}
+
+static inline int mt9v111_read_reg(int sensorid , u8 reg)
 {
-	int val = i2c_smbus_read_word_data(mt9v111_data.i2c_client, reg);
+	int val = i2c_smbus_read_word_data(mt9v111_data[sensorid].i2c_client, reg);
 	if (val != -1)
 		val = cpu_to_be16(val);
 	return val;
 }
-#endif
 
 /*!
  * Writes to the register via I2C.
  */
-static inline int mt9v111_write_reg(u8 reg, u16 val)
+static inline int mt9v111_write_reg(int sensorid , u8 reg, u16 val)
 {
-	pr_debug("In mt9v111_write_reg (0x%x, 0x%x)\n", reg, val);
+	pr_debug("[%d] In mt9v111_write_reg (0x%x, 0x%x)\n", sensorid , reg, val);
 	pr_debug("   write reg %x val %x.\n", reg, val);
 
-	return i2c_smbus_write_word_data(mt9v111_data.i2c_client,
+	return i2c_smbus_write_word_data(mt9v111_data[sensorid].i2c_client,
 					 reg, cpu_to_be16(val));
 }
 
 /*!
- * Initialize mt9v111_sensor_lib
+ * Initialize mt9v111_sensor_lib_datasheet
  * Libarary for Sensor configuration through I2C
  *
  * @param       coreReg       Core Registers
@@ -122,7 +151,7 @@ static inline int mt9v111_write_reg(u8 reg, u16 val)
  *
  * @return status
  */
-static u8 mt9v111_sensor_lib(mt9v111_coreReg * coreReg, mt9v111_IFPReg * ifpReg)
+static u8 mt9v111_sensor_lib_datasheet(int sensorid , mt9v111_coreReg * coreReg, mt9v111_IFPReg * ifpReg)
 {
 	u8 reg;
 	u16 data;
@@ -130,200 +159,103 @@ static u8 mt9v111_sensor_lib(mt9v111_coreReg * coreReg, mt9v111_IFPReg * ifpReg)
 
 	pr_debug("In mt9v111_sensor_lib\n");
 
+	/* IFP R51(0x33)=5137,R57(0x39)=290,R59(0x3B)=1068,R62(0x3E)=4095,R89(0x59)=504,R90(0x5A)=605,R92(0x5C)=8222,R93(0x5D)=10021,R100(0x64)=4477 */
+
 	/*
 	 * setup to IFP registers
 	 */
 	reg = MT9V111I_ADDR_SPACE_SEL;
 	data = ifpReg->addrSpaceSel;
-	mt9v111_write_reg(reg, data);
-
-	/* Operation Mode Control */
-	reg = MT9V111I_MODE_CONTROL;
-	data = ifpReg->modeControl;
-	mt9v111_write_reg(reg, data);
-
-	/* Output format */
-	reg = MT9V111I_FORMAT_CONTROL;
-	data = ifpReg->formatControl;	/* Set bit 12 */
-	mt9v111_write_reg(reg, data);
-
-	/* AE limit 4 */
-	reg = MT9V111I_SHUTTER_WIDTH_LIMIT_AE;
-	data = ifpReg->gainLimitAE;
-	mt9v111_write_reg(reg, data);
-
-	reg = MT9V111I_OUTPUT_FORMAT_CTRL2;
-	data = ifpReg->outputFormatCtrl2;
-	mt9v111_write_reg(reg, data);
-
-	reg = MT9V111I_AE_SPEED;
-	data = ifpReg->AESpeed;
-	mt9v111_write_reg(reg, data);
-
-	/* output image size */
-	reg = MT9V111i_H_PAN;
-	data = 0x8000 | ifpReg->HPan;
-	mt9v111_write_reg(reg, data);
-
-	reg = MT9V111i_H_ZOOM;
-	data = 0x8000 | ifpReg->HZoom;
-	mt9v111_write_reg(reg, data);
-
-	reg = MT9V111i_H_SIZE;
-	data = 0x8000 | ifpReg->HSize;
-	mt9v111_write_reg(reg, data);
-
-	reg = MT9V111i_V_PAN;
-	data = 0x8000 | ifpReg->VPan;
-	mt9v111_write_reg(reg, data);
+	mt9v111_write_reg(sensorid,reg, data);
 
-	reg = MT9V111i_V_ZOOM;
-	data = 0x8000 | ifpReg->VZoom;
-	mt9v111_write_reg(reg, data);
+	reg = MT9V111I_LIMIT_SHARP_SATU_CTRL;
+	data = ifpReg->limitSharpSatuCtrl;
+	mt9v111_write_reg(sensorid,reg, data);
 
-	reg = MT9V111i_V_SIZE;
-	data = 0x8000 | ifpReg->VSize;
-	mt9v111_write_reg(reg, data);
-
-	reg = MT9V111i_H_PAN;
-	data = ~0x8000 & ifpReg->HPan;
-	mt9v111_write_reg(reg, data);
-#if 0
 	reg = MT9V111I_UPPER_SHUTTER_DELAY_LIM;
 	data = ifpReg->upperShutterDelayLi;
-	mt9v111_write_reg(reg, data);
+	mt9v111_write_reg(sensorid,reg, data);
+
+	reg = MT9V111I_IPF_BLACK_LEVEL_SUB;
+	data = ifpReg->ipfBlackLevelSub;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	reg = MT9V111I_GAIN_THRE_CCAM_ADJ;
+	data = ifpReg->agimnThreCamAdj;
+	mt9v111_write_reg(sensorid,reg, data);
 
 	reg = MT9V111I_SHUTTER_60;
 	data = ifpReg->shutter_width_60;
-	mt9v111_write_reg(reg, data);
+	mt9v111_write_reg(sensorid,reg, data);
+
+	reg = MT9V111I_AUTO_EXPOSURE_17;
+	data = ifpReg->auto_exposure_17;
+	mt9v111_write_reg(sensorid,reg, data);
 
 	reg = MT9V111I_SEARCH_FLICK_60;
 	data = ifpReg->search_flicker_60;
-	mt9v111_write_reg(reg, data);
-#endif
+	mt9v111_write_reg(sensorid,reg, data);
+
+	reg = MT9V111I_RESERVED93;
+	data = ifpReg->reserved93;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	reg = MT9V111I_RESERVED100;
+	data = ifpReg->reserved100;
+	mt9v111_write_reg(sensorid,reg, data);
 
 	/*
 	 * setup to sensor core registers
 	 */
 	reg = MT9V111I_ADDR_SPACE_SEL;
 	data = coreReg->addressSelect;
-	mt9v111_write_reg(reg, data);
-
-	/* enable changes and put the Sync bit on */
-	reg = MT9V111S_OUTPUT_CTRL;
-	data = MT9V111S_OUTCTRL_SYNC | MT9V111S_OUTCTRL_CHIP_ENABLE | 0x3000;
-	mt9v111_write_reg(reg, data);
+	mt9v111_write_reg(sensorid,reg, data);
 
-	/* min PIXCLK - Default */
-	reg = MT9V111S_PIXEL_CLOCK_SPEED;
-	data = coreReg->pixelClockSpeed;
-	mt9v111_write_reg(reg, data);
+	/* Core R5=46, R7[4]=0 (DEFAULT) ,R33=58369*/
 
-	/* Setup image flipping / Dark rows / row/column skip */
-	reg = MT9V111S_READ_MODE;
-	data = coreReg->readMode;
-	mt9v111_write_reg(reg, data);
-
-	/* zoom 0 */
-	reg = MT9V111S_DIGITAL_ZOOM;
-	data = coreReg->digitalZoom;
-	mt9v111_write_reg(reg, data);
-
-	/* min H-blank */
 	reg = MT9V111S_HOR_BLANKING;
 	data = coreReg->horizontalBlanking;
-	mt9v111_write_reg(reg, data);
-
-	/* min V-blank */
-	reg = MT9V111S_VER_BLANKING;
-	data = coreReg->verticalBlanking;
-	mt9v111_write_reg(reg, data);
-
-	reg = MT9V111S_SHUTTER_WIDTH;
-	data = coreReg->shutterWidth;
-	mt9v111_write_reg(reg, data);
+	mt9v111_write_reg(sensorid,reg, data);
 
-	reg = MT9V111S_SHUTTER_DELAY;
-	data = ifpReg->upperShutterDelayLi;
-	mt9v111_write_reg(reg, data);
-
-	/* changes become effective */
-	reg = MT9V111S_OUTPUT_CTRL;
-	data = MT9V111S_OUTCTRL_CHIP_ENABLE | 0x3000;
-	mt9v111_write_reg(reg, data);
+	reg = MT9V111S_RESERVED33;
+	data = coreReg->reserved33;
+	mt9v111_write_reg(sensorid,reg, data);
 
 	return error;
 }
 
-/*!
- * MT9V111 frame rate calculate
- *
- * @param frame_rate       int *
- * @param mclk             int
- * @return  None
- */
-static void mt9v111_rate_cal(int *frame_rate, int mclk)
+void mt9v111_config_datasheet(void)
 {
-	int num_clock_per_row;
-	int max_rate = 0;
+	pr_debug("In mt9v111_config_datasheet\n");
 
-	pr_debug("In mt9v111_rate_cal\n");
+	mt9v111_device.coreReg->addressSelect = MT9V111I_SEL_SCA;
 
-	num_clock_per_row = (MT9V111_MAX_WIDTH + 114 + MT9V111_HORZBLANK_MIN)
-			* 2;
-	max_rate = mclk / (num_clock_per_row *
-			   (MT9V111_MAX_HEIGHT + MT9V111_VERTBLANK_DEFAULT));
+	/* MT9V111I_ADDR_SPACE_SEL */
+	mt9v111_device.ifpReg->addrSpaceSel = MT9V111I_SEL_IFP;
 
-	if ((*frame_rate > max_rate) || (*frame_rate == 0)) {
-		*frame_rate = max_rate;
-	}
+	/* Recommended values for 30fps @ 27MHz from datasheet*/
 
-	mt9v111_device.coreReg->verticalBlanking
-	    = mclk / (*frame_rate * num_clock_per_row) - MT9V111_MAX_HEIGHT;
+	/* Core R5=132, R6=10 , R7[4]=0 (DEFAULT) ,R33=58369*/
 
-	reset_frame_rate = *frame_rate;
-}
+	mt9v111_device.coreReg->horizontalBlanking = 132;
+	mt9v111_device.coreReg->verticalBlanking = 10;
+	mt9v111_device.coreReg->reserved33 = 58369;
 
-/*!
- * MT9V111 sensor configuration
- */
-void mt9v111_config(void)
-{
-	pr_debug("In mt9v111_config\n");
+	/* IFP R51(0x33)=5137,R57(0x39)=290,R59(0x3B)=1068,R62(0x3E)=4095,R89(0x59)=504,R90(0x5A)=605,R92(0x5C)=8222,R93(0x5D)=10021,R100(0x64)=4477 */
 
-	mt9v111_device.coreReg->addressSelect = MT9V111I_SEL_SCA;
-	mt9v111_device.ifpReg->addrSpaceSel = MT9V111I_SEL_IFP;
+	mt9v111_device.ifpReg->limitSharpSatuCtrl = 5137;
+	mt9v111_device.ifpReg->upperShutterDelayLi = 290;
+	mt9v111_device.ifpReg->ipfBlackLevelSub = 1068;
+	mt9v111_device.ifpReg->agimnThreCamAdj = 4095;
 
-	mt9v111_device.coreReg->windowHeight = MT9V111_WINHEIGHT;
-	mt9v111_device.coreReg->windowWidth = MT9V111_WINWIDTH;
-	mt9v111_device.coreReg->zoomColStart = 0;
-	mt9v111_device.coreReg->zomRowStart = 0;
-	mt9v111_device.coreReg->digitalZoom = 0x0;
-
-	mt9v111_device.coreReg->verticalBlanking = MT9V111_VERTBLANK_DEFAULT;
-	mt9v111_device.coreReg->horizontalBlanking = MT9V111_HORZBLANK_MIN;
-	mt9v111_device.coreReg->pixelClockSpeed = 0;
-	mt9v111_device.coreReg->readMode = 0xd0a1;
-
-	mt9v111_device.ifpReg->outputFormatCtrl2 = 0;
-	mt9v111_device.ifpReg->gainLimitAE = 0x300;
-	mt9v111_device.ifpReg->AESpeed = 0x80;
-
-	/* here is the default value */
-	mt9v111_device.ifpReg->formatControl = 0xc800;
-	mt9v111_device.ifpReg->modeControl = 0x708e;
-	mt9v111_device.ifpReg->awbSpeed = 0x4514;
-	mt9v111_device.coreReg->shutterWidth = 0xf8;
-
-	/* output size */
-	mt9v111_device.ifpReg->HPan = 0;
-	mt9v111_device.ifpReg->HZoom = MT9V111_MAX_WIDTH;
-	mt9v111_device.ifpReg->HSize = MT9V111_MAX_WIDTH;
-	mt9v111_device.ifpReg->VPan = 0;
-	mt9v111_device.ifpReg->VZoom = MT9V111_MAX_HEIGHT;
-	mt9v111_device.ifpReg->VSize = MT9V111_MAX_HEIGHT;
+	mt9v111_device.ifpReg->shutter_width_60 = 504;
+	mt9v111_device.ifpReg->auto_exposure_17 = 605;
+	mt9v111_device.ifpReg->search_flicker_60 = 8222;
+	mt9v111_device.ifpReg->reserved93 = 10021;
+	mt9v111_device.ifpReg->reserved100 = 4477;
 }
 
+
 /*!
  * mt9v111 sensor set saturtionn
  *
@@ -331,7 +263,7 @@ void mt9v111_config(void)
 
  * @return  Error code of 0.
  */
-static int mt9v111_set_saturation(int saturation)
+static int mt9v111_set_saturation(int sensorid , int saturation)
 {
 	u8 reg;
 	u16 data;
@@ -357,6 +289,9 @@ static int mt9v111_set_saturation(int saturation)
 	case 25:
 		mt9v111_device.ifpReg->awbSpeed = 0x6514;
 		break;
+	case 0:
+		mt9v111_device.ifpReg->awbSpeed = 0x7514;
+		break;
 	default:
 		mt9v111_device.ifpReg->awbSpeed = 0x4514;
 		break;
@@ -364,12 +299,348 @@ static int mt9v111_set_saturation(int saturation)
 
 	reg = MT9V111I_ADDR_SPACE_SEL;
 	data = mt9v111_device.ifpReg->addrSpaceSel;
-	mt9v111_write_reg(reg, data);
+	mt9v111_write_reg(sensorid,reg, data);
 
 	/* Operation Mode Control */
 	reg = MT9V111I_AWB_SPEED;
 	data = mt9v111_device.ifpReg->awbSpeed;
-	mt9v111_write_reg(reg, data);
+	mt9v111_write_reg(sensorid,reg, data);
+
+	return 0;
+}
+
+#if 0
+/*!
+ * mt9v111 sensor set digital zoom
+ *
+ * @param on/off   int
+
+ * @return  0 on success, -1 on error.
+ */
+static int mt9v111_set_digitalzoom(int sensorid , unsigned int on)
+{
+	u8 reg;
+	u16 data;
+	pr_debug("In mt9v111_set_digitalzoom(%d)\n",on);
+
+	if( on > 1 )
+	    return -1;
+
+	mt9v111_device.coreReg->digitalZoom = on;
+
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = mt9v111_device.coreReg->addressSelect;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	/* Operation Mode Control */
+	reg = MT9V111S_DIGITAL_ZOOM;
+	data = mt9v111_device.coreReg->digitalZoom;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	return 0;
+}
+
+/*!
+ * mt9v111 sensor set digital pan
+ *
+ * @param pan_level   int
+
+ * @return  0 on success, -1 on error.
+ */
+static int mt9v111_set_digitalpan (int sensorid , int pan_level)
+{
+	u8 reg;
+	u16 data;
+	pr_debug("In mt9v111_set_digitalpan(%d)\n",
+		pan_level);
+
+	mt9v111_device.ifpReg->HPan = 8;
+	if (pan_level & 0xFFFF0000) {
+		pan_level = (0xFFFFFFFF - pan_level);
+		pan_level = pan_level / 0x14;
+		mt9v111_device.ifpReg->HPan =
+			mt9v111_device.ifpReg->HPan - (pan_level & 0x3FF);
+	} else {
+		pan_level = pan_level / 0x14;
+		mt9v111_device.ifpReg->HPan =
+			mt9v111_device.ifpReg->HPan + (pan_level - 1);
+	}
+
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = mt9v111_device.ifpReg->addrSpaceSel;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	/* Operation Mode Control */
+	reg = MT9V111i_H_PAN;
+	data = mt9v111_device.ifpReg->HPan;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	return 0;
+}
+
+/*!
+ * mt9v111 sensor set digital tilt
+ *
+ * @param tilt_level   int
+
+ * @return  0 on success, -1 on error.
+ */
+static int mt9v111_set_digitaltilt (int sensorid , int tilt_level)
+{
+	u8 reg;
+	u16 data;
+	pr_debug("In mt9v111_set_digitaltilt(%d)\n",
+		tilt_level);
+
+	mt9v111_device.ifpReg->VPan = 8;
+        if( tilt_level & 0xFFFF0000 ) {
+				tilt_level = (0xFFFFFFFF - tilt_level);
+				tilt_level = tilt_level / 0x14;
+                mt9v111_device.ifpReg->VPan = mt9v111_device.ifpReg->VPan - (tilt_level & 0x3FF);
+        }
+        else {
+				tilt_level = tilt_level / 0x14;
+                mt9v111_device.ifpReg->VPan = mt9v111_device.ifpReg->VPan + (tilt_level - 1);
+        }
+
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = mt9v111_device.ifpReg->addrSpaceSel;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	/* Operation Mode Control */
+	reg = MT9V111i_V_PAN;
+	data = mt9v111_device.ifpReg->VPan;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	return 0;
+}
+
+/*!
+ * mt9v111 sensor set output resolution
+ *
+ * @param resolution   res
+
+ * @return  0 on success, -1 on error.
+ */
+static int mt9v111_set_outputresolution(int sensorid , MT9V111_OutputResolution res)
+{
+	u8 reg;
+	u16 data;
+	int zoom = 0;
+
+	pr_debug("In mt9v111_set_outputresolution(%d)\n",res);
+
+	switch (res) {
+	    case MT9V111_OutputResolution_VGA:
+			/* 640x480 */
+			mt9v111_device.ifpReg->HSize = 0x0280;
+			mt9v111_device.ifpReg->VSize = 0x01E0;
+			break;
+
+	    case MT9V111_OutputResolution_QVGA:
+			/* 320x240 */
+			mt9v111_device.ifpReg->HSize = 0x0140;
+			mt9v111_device.ifpReg->VSize = 0x00F0;
+			break;
+
+	    case MT9V111_OutputResolution_CIF:
+			/* 352x288 */
+			mt9v111_device.ifpReg->HSize = 0x0160;
+			mt9v111_device.ifpReg->VSize = 0x0120;
+			mt9v111_device.ifpReg->HZoom = 0x0160;
+			mt9v111_device.ifpReg->VZoom = 0x0120;
+			zoom = 1;
+			break;
+
+	    case MT9V111_OutputResolution_QCIF:
+			/* 176X220 */
+			mt9v111_device.ifpReg->HSize = 0x00B0;
+			mt9v111_device.ifpReg->VSize = 0x0090;
+			mt9v111_device.ifpReg->HZoom = 0x00B0;
+			mt9v111_device.ifpReg->VZoom = 0x0090;
+			zoom = 1;
+			break;
+
+	    case MT9V111_OutputResolution_QQVGA:
+			/* 2048*1536 */
+			mt9v111_device.ifpReg->HSize = 0x00A0;
+			mt9v111_device.ifpReg->VSize = 0x0078;
+			mt9v111_device.ifpReg->HZoom = 0x00A0;
+			mt9v111_device.ifpReg->VZoom = 0x0078;
+			zoom = 1;
+			break;
+
+	    case MT9V111_OutputResolution_SXGA:
+		/* 1280x1024 */
+			break;
+
+		default:
+			break;
+	}
+
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = mt9v111_device.ifpReg->addrSpaceSel;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	reg = MT9V111i_V_SIZE;
+	data = mt9v111_device.ifpReg->VSize;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	reg = MT9V111i_H_SIZE;
+	data = mt9v111_device.ifpReg->HSize;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	if ( zoom ) {
+	    reg = MT9V111i_V_ZOOM;
+	    data = mt9v111_device.ifpReg->VZoom;
+	    mt9v111_write_reg(sensorid,reg, data);
+
+	    reg = MT9V111i_H_ZOOM;
+	    data = mt9v111_device.ifpReg->HZoom;
+	    mt9v111_write_reg(sensorid,reg, data);
+	}
+
+	return 0;
+}
+
+/*!
+ * mt9v111 sensor set digital flash
+ *
+ * @param flash_level   int
+
+ * @return  0 on success, -1 on error.
+ */
+static int mt9v111_set_digitalflash (int sensorid , int flash_level)
+{
+	u8 reg;
+	u16 data = mt9v111_read_reg(sensorid,MT9V111i_FLASH_CTRL);
+	pr_debug("In mt9v111_set_digitalflash(%d)\n",
+			flash_level);
+
+	if(flash_level) {
+	    data &= (0xFF00);
+	    data |= ((flash_level & 0x00FF) | (1<<13));
+	}
+	else {
+	    data &= ~(1<<13);
+	}
+
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = mt9v111_device.ifpReg->addrSpaceSel;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	/* Operation Mode Control */
+	reg = MT9V111i_FLASH_CTRL;
+	mt9v111_device.ifpReg->flashCtrl = data;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	return 0;
+}
+
+/*!
+ * mt9v111 sensor set digital monochrome
+ *
+ * @param on   int
+
+ * @return  0 on success, -1 on error.
+ */
+static int mt9v111_set_digitalmonochrome (int sensorid , int on)
+{
+	u8 reg;
+	u16 data = mt9v111_read_reg(sensorid,MT9V111I_FORMAT_CONTROL);
+	pr_debug("In mt9v111_set_digitalmonochrome(%d)\n",
+			on);
+
+        /* clear the monochrome bit field */
+        data &= ~(1<<5);
+
+         /* enable or disable monochrome mode */
+         if( on )
+                 data |= (0<<5);
+         else
+                 data |= (1<<5);
+
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = mt9v111_device.ifpReg->addrSpaceSel;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	/* Operation Mode Control */
+	reg = MT9V111I_FORMAT_CONTROL;
+	mt9v111_device.ifpReg->formatControl = data;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	return 0;
+}
+#endif
+
+/*!
+ * mt9v111 sensor set digital sharpness
+ *
+ * @param value   int
+
+ * @return  0 on success, -1 on error.
+ */
+static int mt9v111_set_digitalsharpness (int sensorid , int value)
+{
+	u8 reg;
+	u16 data ;
+
+	pr_debug("In mt9v111_set_digitalsharpness(%d)\n",value);
+
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = mt9v111_device.ifpReg->addrSpaceSel;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	data = mt9v111_read_reg(sensorid,MT9V111I_APERTURE_GAIN);
+
+	/* erase current and remove auto reduce sharpness in low light */
+	 data &= ~(0x000F);
+	 data |= (value & (0x000F));
+	 if( data > (0x000F) )
+			 return -1;
+
+	/* Operation Mode Control */
+	reg = MT9V111I_APERTURE_GAIN;
+	mt9v111_device.ifpReg->apertureGain = data;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	return 0;
+}
+
+/*!
+ * mt9v111 sensor set digital brightness
+ *
+ * @param value   int
+
+ * @return  0 on success, -1 on error.
+ */
+static int mt9v111_set_digitalbrightness (int sensorid , int value)
+{
+	u8 reg;
+	u16 data;
+	u32 max_brightness, min_brightness;
+
+	data = mt9v111_read_reg(sensorid,MT9V111I_CLIP_LIMIT_OUTPUT_LUMI);
+	max_brightness = data >> 8;
+	min_brightness = (u8)data;
+
+	if( value > max_brightness )
+		value = max_brightness;
+	else if( value < min_brightness )
+		value = min_brightness;
+
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = mt9v111_device.ifpReg->addrSpaceSel;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	data = mt9v111_read_reg(sensorid,MT9V111I_AE_PRECISION_TARGET);
+	data &= 0xFF00; /* Clear target luminance */
+	data |= ((u8)value );
+
+	/* Operation Mode Control */
+	reg = MT9V111I_AE_PRECISION_TARGET;
+	mt9v111_device.ifpReg->AEPrecisionTarget = data;
+	mt9v111_write_reg(sensorid,reg, data);
 
 	return 0;
 }
@@ -380,7 +651,7 @@ static int mt9v111_set_saturation(int saturation)
  * @param ae_mode      int
  * @return  Error code of 0 (no Error)
  */
-static int mt9v111_set_ae_mode(int ae_mode)
+static int mt9v111_set_ae_mode(int sensorid , int ae_mode)
 {
 	u8 reg;
 	u16 data;
@@ -408,19 +679,20 @@ static int mt9v111_set_ae_mode(int ae_mode)
 	/* V4L2_EXPOSURE_MANUAL = 1 needs register setting of 0x308E */
 	mt9v111_device.ifpReg->modeControl &= 0x3fff;
 	mt9v111_device.ifpReg->modeControl |= (ae_mode & 0x03) << 14;
-	mt9v111_data.ae_mode = ae_mode;
+	mt9v111_data[sensorid].ae_mode = ae_mode;
 
 	reg = MT9V111I_ADDR_SPACE_SEL;
 	data = mt9v111_device.ifpReg->addrSpaceSel;
-	mt9v111_write_reg(reg, data);
+	mt9v111_write_reg(sensorid,reg, data);
 
 	reg = MT9V111I_MODE_CONTROL;
 	data = mt9v111_device.ifpReg->modeControl;
-	mt9v111_write_reg(reg, data);
+	mt9v111_write_reg(sensorid,reg, data);
 
 	return 0;
 }
 
+#if 0
 /*!
  * mt9v111 sensor get AE measurement window mode configuration
  *
@@ -435,6 +707,7 @@ static void mt9v111_get_ae_mode(int *ae_mode)
 		*ae_mode = (mt9v111_device.ifpReg->modeControl & 0xc) >> 2;
 	}
 }
+#endif
 
 #ifdef MT9V111_DEBUG
 /*!
@@ -442,33 +715,33 @@ static void mt9v111_get_ae_mode(int *ae_mode)
  *
  * @return none
  */
-static void mt9v111_test_pattern(bool flag)
+static void mt9v111_test_pattern(int sensorid , bool flag)
 {
 	u16 data;
 
 	/* switch to sensor registers */
-	mt9v111_write_reg(MT9V111I_ADDR_SPACE_SEL, MT9V111I_SEL_SCA);
+	mt9v111_write_reg(sensorid,MT9V111I_ADDR_SPACE_SEL, MT9V111I_SEL_SCA);
 
 	if (flag == true) {
 		testpattern = MT9V111S_OUTCTRL_TEST_MODE;
 
-		data = mt9v111_read_reg(MT9V111S_ROW_NOISE_CTRL) & 0xBF;
-		mt9v111_write_reg(MT9V111S_ROW_NOISE_CTRL, data);
+		data = mt9v111_read_reg(sensorid,MT9V111S_ROW_NOISE_CTRL) & 0xBF;
+		mt9v111_write_reg(sensorid,MT9V111S_ROW_NOISE_CTRL, data);
 
-		mt9v111_write_reg(MT9V111S_TEST_DATA, 0);
+		mt9v111_write_reg(sensorid,MT9V111S_TEST_DATA, 0);
 
 		/* changes take effect */
 		data = MT9V111S_OUTCTRL_CHIP_ENABLE | testpattern | 0x3000;
-		mt9v111_write_reg(MT9V111S_OUTPUT_CTRL, data);
+		mt9v111_write_reg(sensorid,MT9V111S_OUTPUT_CTRL, data);
 	} else {
 		testpattern = 0;
 
-		data = mt9v111_read_reg(MT9V111S_ROW_NOISE_CTRL) | 0x40;
+		data = mt9v111_read_reg(sensorid,MT9V111S_ROW_NOISE_CTRL) | 0x40;
 		mt9v111_write_reg(MT9V111S_ROW_NOISE_CTRL, data);
 
 		/* changes take effect */
 		data = MT9V111S_OUTCTRL_CHIP_ENABLE | testpattern | 0x3000;
-		mt9v111_write_reg(MT9V111S_OUTPUT_CTRL, data);
+		mt9v111_write_reg(sensorid,MT9V111S_OUTPUT_CTRL, data);
 	}
 }
 #endif
@@ -507,6 +780,7 @@ static int ioctl_g_ifparm(struct v4l2_int_device *s, struct v4l2_ifparm *p)
 	p->u.bt656.clock_curr = MT9V111_MCLK;
 	p->if_type = V4L2_IF_TYPE_BT656;
 	p->u.bt656.mode = V4L2_IF_TYPE_BT656_MODE_NOBT_8BIT;
+	p->u.bt656.bt_sync_correct = 1;	// translates to CSI ext vsync
 	p->u.bt656.clock_min = MT9V111_CLK_MIN;
 	p->u.bt656.clock_max = MT9V111_CLK_MAX;
 
@@ -534,10 +808,12 @@ static int ioctl_s_power(struct v4l2_int_device *s, int on)
 
 	sensor->on = on;
 
-	if (on)
-		gpio_sensor_active();
-	else
-		gpio_sensor_inactive();
+	if(on) {
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, 0 /* cam->csi */, true, true);
+	}
+	else {
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, 0 /* cam->csi */, false, false);
+	}
 
 	return 0;
 }
@@ -554,19 +830,23 @@ static int ioctl_g_parm(struct v4l2_int_device *s, struct v4l2_streamparm *a)
 	int ret = 0;
 	struct v4l2_captureparm *cparm = &a->parm.capture;
 	/* s->priv points to mt9v111_data */
+	int sensorid = mt9v111_id_from_name(((struct sensor *)s->priv)->v4l2_int_device->name);
 
 	pr_debug("In mt9v111:ioctl_g_parm\n");
 
+	if( sensorid < 0 )
+		return ret;
+
 	switch (a->type) {
 	/* This is the only case currently handled. */
 	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 		pr_debug("   type is V4L2_BUF_TYPE_VIDEO_CAPTURE\n");
 		memset(a, 0, sizeof(*a));
 		a->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-		cparm->capability = mt9v111_data.streamcap.capability;
+		cparm->capability = mt9v111_data[sensorid].streamcap.capability;
 		cparm->timeperframe =
-				mt9v111_data.streamcap.timeperframe;
-		cparm->capturemode = mt9v111_data.streamcap.capturemode;
+				mt9v111_data[sensorid].streamcap.timeperframe;
+		cparm->capturemode = mt9v111_data[sensorid].streamcap.capturemode;
 		ret = 0;
 		break;
 
@@ -605,9 +885,13 @@ static int ioctl_s_parm(struct v4l2_int_device *s, struct v4l2_streamparm *a)
 	int ret = 0;
 	struct v4l2_captureparm *cparm = &a->parm.capture;
 	/* s->priv points to mt9v111_data */
+	int sensorid = mt9v111_id_from_name(((struct sensor *)s->priv)->v4l2_int_device->name);
 
 	pr_debug("In mt9v111:ioctl_s_parm\n");
 
+	if( sensorid < 0 )
+		return ret;
+
 	switch (a->type) {
 	/* This is the only case currently handled. */
 	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
@@ -617,13 +901,13 @@ static int ioctl_s_parm(struct v4l2_int_device *s, struct v4l2_streamparm *a)
 		 * Changing the frame rate is not allowed on this
 		 *camera. */
 		if (cparm->timeperframe.denominator !=
-		    mt9v111_data.streamcap.timeperframe.denominator) {
+		    mt9v111_data[sensorid].streamcap.timeperframe.denominator) {
 			pr_err("ERROR: mt9v111: ioctl_s_parm: " \
 			       "This camera does not allow frame rate "
 			       "changes.\n");
 			ret = -EINVAL;
 		} else {
-			mt9v111_data.streamcap.timeperframe =
+			mt9v111_data[sensorid].streamcap.timeperframe =
 						cparm->timeperframe;
 		      /* Call any camera functions to match settings. */
 		}
@@ -635,7 +919,7 @@ static int ioctl_s_parm(struct v4l2_int_device *s, struct v4l2_streamparm *a)
 				"unsupported capture mode\n");
 			ret  = -EINVAL;
 		} else {
-			mt9v111_data.streamcap.capturemode =
+			mt9v111_data[sensorid].streamcap.capturemode =
 						cparm->capturemode;
 		      /* Call any camera functions to match settings. */
 		      /* Right now this camera only supports 1 mode. */
@@ -685,6 +969,7 @@ static int ioctl_g_fmt_cap(struct v4l2_int_device *s, struct v4l2_format *f)
 	return 0;
 }
 
+#if 0
 /*!
  * ioctl_queryctrl - V4L2 sensor interface handler for VIDIOC_QUERYCTRL ioctl
  * @s: pointer to standard V4L2 device structure
@@ -700,6 +985,7 @@ static int ioctl_queryctrl(struct v4l2_int_device *s, struct v4l2_queryctrl *qc)
 
 	return 0;
 }
+#endif
 
 /*!
  * ioctl_g_ctrl - V4L2 sensor interface handler for VIDIOC_G_CTRL ioctl
@@ -712,66 +998,29 @@ static int ioctl_queryctrl(struct v4l2_int_device *s, struct v4l2_queryctrl *qc)
  */
 static int ioctl_g_ctrl(struct v4l2_int_device *s, struct v4l2_control *vc)
 {
+	int sensorid = mt9v111_id_from_name(((struct sensor *)s->priv)->v4l2_int_device->name);
+
 	pr_debug("In mt9v111:ioctl_g_ctrl\n");
 
+	if( sensorid < 0 )
+		return 0;
+
 	switch (vc->id) {
 	case V4L2_CID_BRIGHTNESS:
 		pr_debug("   V4L2_CID_BRIGHTNESS\n");
-		vc->value = mt9v111_data.brightness;
-		break;
-	case V4L2_CID_CONTRAST:
-		pr_debug("   V4L2_CID_CONTRAST\n");
-		vc->value = mt9v111_data.contrast;
+		vc->value = mt9v111_data[sensorid].brightness;
 		break;
 	case V4L2_CID_SATURATION:
 		pr_debug("   V4L2_CID_SATURATION\n");
-		vc->value = mt9v111_data.saturation;
-		break;
-	case V4L2_CID_HUE:
-		pr_debug("   V4L2_CID_HUE\n");
-		vc->value = mt9v111_data.hue;
-		break;
-	case V4L2_CID_AUTO_WHITE_BALANCE:
-		pr_debug(
-			"   V4L2_CID_AUTO_WHITE_BALANCE\n");
-		vc->value = 0;
-		break;
-	case V4L2_CID_DO_WHITE_BALANCE:
-		pr_debug(
-			"   V4L2_CID_DO_WHITE_BALANCE\n");
-		vc->value = 0;
-		break;
-	case V4L2_CID_RED_BALANCE:
-		pr_debug("   V4L2_CID_RED_BALANCE\n");
-		vc->value = mt9v111_data.red;
-		break;
-	case V4L2_CID_BLUE_BALANCE:
-		pr_debug("   V4L2_CID_BLUE_BALANCE\n");
-		vc->value = mt9v111_data.blue;
-		break;
-	case V4L2_CID_GAMMA:
-		pr_debug("   V4L2_CID_GAMMA\n");
-		vc->value = 0;
+		vc->value = mt9v111_data[sensorid].saturation;
 		break;
 	case V4L2_CID_EXPOSURE:
 		pr_debug("   V4L2_CID_EXPOSURE\n");
-		vc->value = mt9v111_data.ae_mode;
-		break;
-	case V4L2_CID_AUTOGAIN:
-		pr_debug("   V4L2_CID_AUTOGAIN\n");
-		vc->value = 0;
+		vc->value = mt9v111_data[sensorid].ae_mode;
 		break;
 	case V4L2_CID_GAIN:
 		pr_debug("   V4L2_CID_GAIN\n");
-		vc->value = 0;
-		break;
-	case V4L2_CID_HFLIP:
-		pr_debug("   V4L2_CID_HFLIP\n");
-		vc->value = 0;
-		break;
-	case V4L2_CID_VFLIP:
-		pr_debug("   V4L2_CID_VFLIP\n");
-		vc->value = 0;
+		vc->value = mt9v111_data[sensorid].gain;
 		break;
 	default:
 		pr_debug("   Default case\n");
@@ -794,56 +1043,34 @@ static int ioctl_g_ctrl(struct v4l2_int_device *s, struct v4l2_control *vc)
 static int ioctl_s_ctrl(struct v4l2_int_device *s, struct v4l2_control *vc)
 {
 	int retval = 0;
+	int sensorid = mt9v111_id_from_name(((struct sensor *)s->priv)->v4l2_int_device->name);
 
 	pr_debug("In mt9v111:ioctl_s_ctrl %d\n",
 		vc->id);
 
+	if( sensorid < 0 )
+		return retval;
+
 	switch (vc->id) {
 	case V4L2_CID_BRIGHTNESS:
 		pr_debug("   V4L2_CID_BRIGHTNESS\n");
-		break;
-	case V4L2_CID_CONTRAST:
-		pr_debug("   V4L2_CID_CONTRAST\n");
+		mt9v111_set_digitalbrightness(sensorid,vc->value);
+		mt9v111_data[sensorid].brightness = vc->value;
 		break;
 	case V4L2_CID_SATURATION:
 		pr_debug("   V4L2_CID_SATURATION\n");
-		retval = mt9v111_set_saturation(vc->value);
-		break;
-	case V4L2_CID_HUE:
-		pr_debug("   V4L2_CID_HUE\n");
-		break;
-	case V4L2_CID_AUTO_WHITE_BALANCE:
-		pr_debug(
-			"   V4L2_CID_AUTO_WHITE_BALANCE\n");
-		break;
-	case V4L2_CID_DO_WHITE_BALANCE:
-		pr_debug(
-			"   V4L2_CID_DO_WHITE_BALANCE\n");
-		break;
-	case V4L2_CID_RED_BALANCE:
-		pr_debug("   V4L2_CID_RED_BALANCE\n");
-		break;
-	case V4L2_CID_BLUE_BALANCE:
-		pr_debug("   V4L2_CID_BLUE_BALANCE\n");
-		break;
-	case V4L2_CID_GAMMA:
-		pr_debug("   V4L2_CID_GAMMA\n");
+		retval = mt9v111_set_saturation(sensorid,vc->value);
+		mt9v111_data[sensorid].saturation = vc->value;
 		break;
 	case V4L2_CID_EXPOSURE:
 		pr_debug("   V4L2_CID_EXPOSURE\n");
-		retval = mt9v111_set_ae_mode(vc->value);
-		break;
-	case V4L2_CID_AUTOGAIN:
-		pr_debug("   V4L2_CID_AUTOGAIN\n");
+		retval = mt9v111_set_ae_mode(sensorid,vc->value);
+		mt9v111_data[sensorid].ae_mode = vc->value;
 		break;
 	case V4L2_CID_GAIN:
 		pr_debug("   V4L2_CID_GAIN\n");
-		break;
-	case V4L2_CID_HFLIP:
-		pr_debug("   V4L2_CID_HFLIP\n");
-		break;
-	case V4L2_CID_VFLIP:
-		pr_debug("   V4L2_CID_VFLIP\n");
+		mt9v111_set_digitalsharpness(sensorid,vc->value);
+		mt9v111_data[sensorid].gain = vc->value;
 		break;
 	default:
 		pr_debug("   Default case\n");
@@ -854,13 +1081,41 @@ static int ioctl_s_ctrl(struct v4l2_int_device *s, struct v4l2_control *vc)
 	return retval;
 }
 
+static void mt9v111_ifp_reset ( int sensorid )
+{
+	mt9v111_write_reg(sensorid,MT9V111S_ADDR_SPACE_SEL, 0x0001);
+	mt9v111_write_reg(sensorid,MT9V111I_SOFT_RESET, 0x0001);
+	msleep(100);
+	mt9v111_write_reg(sensorid,MT9V111I_SOFT_RESET, 0x0000);
+	msleep(100);
+}
+
+static void mt9v111_sensor_reset ( int sensorid )
+{
+	mt9v111_write_reg(sensorid,MT9V111S_ADDR_SPACE_SEL, 0x0004);
+	mt9v111_write_reg(sensorid,MT9V111S_RESET, 0x0001);
+	msleep(100);
+	mt9v111_write_reg(sensorid,MT9V111S_RESET, 0x0000);
+	msleep(100);
+}
+
 /*!
  * ioctl_init - V4L2 sensor interface handler for VIDIOC_INT_INIT
  * @s: pointer to standard V4L2 device structure
  */
 static int ioctl_init(struct v4l2_int_device *s)
 {
-	pr_debug("In mt9v111:ioctl_init\n");
+	int sensorid = 0;
+
+	sensorid = mt9v111_id_from_name(((struct sensor *)s->priv)->v4l2_int_device->name);
+	if( sensorid < 0 )
+		return 0;
+
+	pr_debug("In mt9v111:ioctl_init for sensor %d\n",sensorid);
+
+	mt9v111_sensor_reset(sensorid);
+	mt9v111_ifp_reset(sensorid);
+	mt9v111_sensor_lib_datasheet(sensorid,mt9v111_device.coreReg, mt9v111_device.ifpReg);
 
 	return 0;
 }
@@ -873,19 +1128,146 @@ static int ioctl_init(struct v4l2_int_device *s)
  */
 static int ioctl_dev_init(struct v4l2_int_device *s)
 {
+	int sensorid = 0;
 	uint32_t clock_rate = MT9V111_MCLK;
 
+	sensorid = mt9v111_id_from_name(((struct sensor *)s->priv)->v4l2_int_device->name);
+	if( sensorid < 0 )
+		return 0;
+
 	pr_debug("In mt9v111:ioctl_dev_init\n");
 
-	gpio_sensor_active();
+	set_mclk_rate(&clock_rate, 0);	// Both sensors use mclk0 on Digi ccwmx51
+
+	mt9v111_sensor_reset(sensorid);
+	mt9v111_ifp_reset(sensorid);
+	mt9v111_sensor_lib_datasheet(sensorid,mt9v111_device.coreReg, mt9v111_device.ifpReg);
 
-	set_mclk_rate(&clock_rate);
-	mt9v111_rate_cal(&reset_frame_rate, clock_rate);
-	mt9v111_sensor_lib(mt9v111_device.coreReg, mt9v111_device.ifpReg);
+	return 0;
+}
+
+/* list of image formats supported by sensor */
+static const struct v4l2_fmtdesc mt9v111_formats[] = {
+	{
+		.description = "RGB565",
+		.pixelformat = V4L2_PIX_FMT_RGB565,
+	},
+	{
+		.description = "YUV422 UYVY",
+		.pixelformat = V4L2_PIX_FMT_UYVY,
+	},
+};
+
+#define MT9V111_NUM_CAPTURE_FORMATS	ARRAY_SIZE(mt9v111_formats)
+
+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 >= MT9V111_NUM_CAPTURE_FORMATS)
+			return -EINVAL;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	fmt->flags = mt9v111_formats[index].flags;
+	strlcpy(fmt->description, mt9v111_formats[index].description,
+		sizeof(fmt->description));
+	fmt->pixelformat = mt9v111_formats[index].pixelformat;
 
 	return 0;
 }
 
+static int ioctl_s_fmt_cap(struct v4l2_int_device *s,
+			struct v4l2_format *f)
+{
+	unsigned short reg;
+	int sensorid = mt9v111_id_from_name(((struct sensor *)s->priv)->v4l2_int_device->name);
+	struct sensor *sensor = s->priv;
+	/* s->priv points to mt9v111_data */
+
+	if( sensorid < 0 )
+		return -ENODEV;
+
+	/* Select IFP registers */
+	mt9v111_write_reg (sensorid,MT9V111S_ADDR_SPACE_SEL, 0x0001);
+
+	switch (f->fmt.pix.pixelformat) {
+		case V4L2_PIX_FMT_RGB565:
+			/*MT9V111I_OUTPUT_FORMAT_CTRL2*/
+			reg = mt9v111_read_reg (sensorid,MT9V111I_OUTPUT_FORMAT_CTRL2);
+			reg &= ~(0x3 << 6);
+			mt9v111_write_reg (sensorid,MT9V111I_OUTPUT_FORMAT_CTRL2, reg);
+
+			/* MT9V111I_FORMAT_CONTROL */
+			reg = mt9v111_read_reg(sensorid,MT9V111I_FORMAT_CONTROL);
+			reg |= 1 << 12;
+			mt9v111_write_reg(sensorid,MT9V111I_FORMAT_CONTROL, reg);
+			break;
+
+		case V4L2_PIX_FMT_YUV444:
+		case V4L2_PIX_FMT_UYVY:
+		case V4L2_PIX_FMT_YVU420:
+		case V4L2_PIX_FMT_YUYV:
+			/* MT9V111I_FORMAT_CONTROL */
+			reg = mt9v111_read_reg(sensorid,MT9V111I_FORMAT_CONTROL);
+			reg &= ~(1 << 12);
+			mt9v111_write_reg(sensorid,MT9V111I_FORMAT_CONTROL, reg);
+			break;
+
+		default:
+			return -EINVAL;
+	}
+
+	sensor->pix.width = f->fmt.pix.width;
+	sensor->pix.height = f->fmt.pix.height;
+	sensor->pix.sizeimage = f->fmt.pix.sizeimage;
+	sensor->pix.pixelformat = f->fmt.pix.pixelformat;
+	return 0;
+}
+
+static int ioctl_try_fmt_cap(struct v4l2_int_device *s, struct v4l2_format *f)
+{
+	int i;
+
+	if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	for( i=0 ; i < MT9V111_NUM_CAPTURE_FORMATS ; i++) {
+		if( f->fmt.pix.pixelformat == mt9v111_formats[i].pixelformat )
+			return 0;
+	}
+
+	return -EINVAL;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int ioctl_get_register(struct v4l2_int_device *s,struct v4l2_dbg_register * dreg)
+{
+	int sensorid = mt9v111_id_from_name(((struct sensor *)s->priv)->v4l2_int_device->name);
+
+	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, 0 /* cam->csi */ , true, true);
+	dreg->val = mt9v111_read_reg (sensorid,dreg->reg);
+	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, 0 /* cam->csi */ , false, false);
+	return 0;
+}
+
+static int ioctl_set_register(struct v4l2_int_device *s,struct v4l2_dbg_register * dreg)
+{
+	int sensorid = mt9v111_id_from_name(((struct sensor *)s->priv)->v4l2_int_device->name);
+
+	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, 0 /* cam->csi */ , true, true);
+	mt9v111_write_reg (sensorid,dreg->reg, dreg->val);
+	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, 0 /* cam->csi */ , false, false);
+	return 0;
+}
+#endif
+
 /*!
  * This structure defines all the ioctls for this module and links them to the
  * enumeration.
@@ -910,16 +1292,23 @@ static struct v4l2_int_ioctl_desc mt9v111_ioctl_desc[] = {
 	/*!
 	 * VIDIOC_ENUM_FMT ioctl for the CAPTURE buffer type.
 	 */
-/*	{vidioc_int_enum_fmt_cap_num,
-				(v4l2_int_ioctl_func *) ioctl_enum_fmt_cap}, */
+	{vidioc_int_enum_fmt_cap_num,
+				(v4l2_int_ioctl_func *) ioctl_enum_fmt_cap},
+
 
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	{vidioc_int_g_register_num,
+				(v4l2_int_ioctl_func *) ioctl_get_register},
+	{vidioc_int_s_register_num,
+				(v4l2_int_ioctl_func *) ioctl_set_register},
+#endif
 	/*!
 	 * VIDIOC_TRY_FMT ioctl for the CAPTURE buffer type.
 	 * This ioctl is used to negotiate the image capture size and
 	 * pixel format without actually making it take effect.
 	 */
-/*	{vidioc_int_try_fmt_cap_num,
-				(v4l2_int_ioctl_func *) ioctl_try_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},
 
@@ -928,7 +1317,7 @@ static struct v4l2_int_ioctl_desc mt9v111_ioctl_desc[] = {
 	 * format, returns error code if format not supported or HW can't be
 	 * correctly configured.
 	 */
-/*	{vidioc_int_s_fmt_cap_num, (v4l2_int_ioctl_func *)ioctl_s_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},
@@ -937,20 +1326,56 @@ static struct v4l2_int_ioctl_desc mt9v111_ioctl_desc[] = {
 	{vidioc_int_s_ctrl_num, (v4l2_int_ioctl_func *) ioctl_s_ctrl},
 };
 
-static struct v4l2_int_slave mt9v111_slave = {
-	.ioctls = mt9v111_ioctl_desc,
-	.num_ioctls = ARRAY_SIZE(mt9v111_ioctl_desc),
+static struct v4l2_int_slave mt9v111_slave[] = {
+	{
+		.ioctls = mt9v111_ioctl_desc,
+		.num_ioctls = ARRAY_SIZE(mt9v111_ioctl_desc),
+		.attach_to = "mxc_v4l2_cap_1",
+	},
+	{
+		.ioctls = mt9v111_ioctl_desc,
+		.num_ioctls = ARRAY_SIZE(mt9v111_ioctl_desc),
+		.attach_to = "mxc_v4l2_cap_2",
+	},
 };
 
-static struct v4l2_int_device mt9v111_int_device = {
-	.module = THIS_MODULE,
-	.name = "mt9v111",
-	.type = v4l2_int_type_slave,
-	.u = {
-		.slave = &mt9v111_slave,
+static struct v4l2_int_device mt9v111_int_device [] = {
+		{
+			.module = THIS_MODULE,
+			.type = v4l2_int_type_slave,
+			.u = {
+				.slave = &mt9v111_slave[0],
+				},
+		},
+		{
+			.module = THIS_MODULE,
+			.type = v4l2_int_type_slave,
+			.u = {
+				.slave = &mt9v111_slave[1],
+				},
 		},
 };
 
+static int mt9v111_read_id( int sensoridx )
+{
+	int sensorid = 0;
+	int ret = 0;
+
+	mt9v111_write_reg (sensoridx,MT9V111S_ADDR_SPACE_SEL, 0x0004);
+
+	sensorid = mt9v111_read_reg (sensoridx,MT9V111S_CHIP_VERSION);
+	if( sensorid == 0x823a )
+	{
+		printk(KERN_INFO" MT9V111 ID %x\n",sensorid);
+	}
+	else
+	{
+		printk(KERN_ERR" MT9V111 Could not detect sensor (read %x)\n",sensorid);
+		ret = -ENODEV;
+	}
+	return ret;
+}
+
 /*!
  * mt9v111 I2C probe function
  * Function set in i2c_driver struct.
@@ -962,32 +1387,56 @@ static int mt9v111_probe(struct i2c_client *client,
 			 const struct i2c_device_id *id)
 {
 	int retval;
+	int sensorid;
 
 	pr_debug("In mt9v111_probe  device id is %s\n", id->name);
 
+	sensorid = mt9v111_id_from_name(id->name);
+
+	if( sensorid < 0 )
+		return -ENODEV;
+
 	/* Set initial values for the sensor struct. */
-	memset(&mt9v111_data, 0, sizeof(mt9v111_data));
-	mt9v111_data.i2c_client = client;
+	memset(&mt9v111_data[sensorid], 0, sizeof(struct sensor));
+	mt9v111_data[sensorid].i2c_client = client;
 	pr_debug("   client name is %s\n", client->name);
-	mt9v111_data.pix.pixelformat = V4L2_PIX_FMT_UYVY;
-	mt9v111_data.pix.width = MT9V111_MAX_WIDTH;
-	mt9v111_data.pix.height = MT9V111_MAX_HEIGHT;
-	mt9v111_data.streamcap.capability = 0; /* No higher resolution or frame
-						* frame rate changes supported.
-						*/
-	mt9v111_data.streamcap.timeperframe.denominator = MT9V111_FRAME_RATE;
-	mt9v111_data.streamcap.timeperframe.numerator = 1;
+	mt9v111_data[sensorid].pix.pixelformat = V4L2_PIX_FMT_UYVY;
+	mt9v111_data[sensorid].pix.width = MT9V111_MAX_WIDTH;
+	mt9v111_data[sensorid].pix.height = MT9V111_MAX_HEIGHT;
+	mt9v111_data[sensorid].streamcap.capability = 0; /* No higher resolution or frame
+						* frame rate changes supported.*/
+	mt9v111_data[sensorid].streamcap.timeperframe.denominator = MT9V111_FRAME_RATE;
+	mt9v111_data[sensorid].streamcap.timeperframe.numerator = 1;
+
+	strcpy(mt9v111_int_device[sensorid].name,id->name);
+	pr_debug("   video device name is %s\n", mt9v111_data[sensorid].v4l2_int_device->name);
+	mt9v111_data[sensorid].v4l2_int_device = &mt9v111_int_device[sensorid];
+	mt9v111_int_device[sensorid].priv = &mt9v111_data[sensorid];
+
+	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, sensorid , true, true);
+
+	if( mt9v111_read_id(sensorid) != 0) {
+		printk(KERN_ERR"mt9v111_probe: No sensor found\n");
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, sensorid , false, false);
+		return -ENXIO;
+	}
 
-	mt9v111_int_device.priv = &mt9v111_data;
+#ifdef MT9V111_DEBUG
+	mt9v111_test_pattern(1);
+#endif
+
+	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, sensorid , false, false);
 
 	pr_debug("   type is %d (expect %d)\n",
-		mt9v111_int_device.type, v4l2_int_type_slave);
+		mt9v111_int_device[sensorid].type, v4l2_int_type_slave);
 	pr_debug("   num ioctls is %d\n",
-		mt9v111_int_device.u.slave->num_ioctls);
+		mt9v111_int_device[sensorid].u.slave->num_ioctls);
 
 	/* This function attaches this structure to the /dev/video0 device.
 	 * The pointer in priv points to the mt9v111_data structure here.*/
-	retval = v4l2_int_device_register(&mt9v111_int_device);
+	retval = v4l2_int_device_register(&mt9v111_int_device[sensorid]);
+	if( retval == 0 )
+		mt9v111_data[sensorid].used = 1;
 
 	return retval;
 }
@@ -998,9 +1447,14 @@ static int mt9v111_probe(struct i2c_client *client,
  */
 static int mt9v111_remove(struct i2c_client *client)
 {
+	int i;
+
 	pr_debug("In mt9v111_remove\n");
 
-	v4l2_int_device_unregister(&mt9v111_int_device);
+	for ( i=0 ; i < ARRAY_SIZE(mt9v111_int_device) ; i++ ) {
+		if( mt9v111_data[i].used )
+			v4l2_int_device_unregister(&mt9v111_int_device[i]);
+	}
 	return 0;
 }
 
@@ -1033,13 +1487,13 @@ static __init int mt9v111_init(void)
 	memset(mt9v111_device.ifpReg, 0, sizeof(mt9v111_IFPReg));
 
 	/* Set contents of the just created structures. */
-	mt9v111_config();
+	mt9v111_config_datasheet();
 
 	/* Tells the i2c driver what functions to call for this driver. */
 	err = i2c_add_driver(&mt9v111_i2c_driver);
 	if (err != 0)
 		pr_err("%s:driver registration failed, error=%d \n",
-		       __func__, err);
+			   __func__, err);
 
 	return err;
 }
@@ -1055,7 +1509,6 @@ static void __exit mt9v111_clean(void)
 	pr_debug("In mt9v111_clean()\n");
 
 	i2c_del_driver(&mt9v111_i2c_driver);
-	gpio_sensor_inactive();
 
 	if (mt9v111_device.coreReg) {
 		kfree(mt9v111_device.coreReg);