ARM: Tegra: Sh532u: enable focuser attached to right camera

The sh532u focuser is enabled to pair with the stereo right camera
with the required power control configurations.

Bug 827897

Original-Change-Id: Icc21bcd2327671da193a8b8f24c338b0b2f6729c
Reviewed-on: http://git-master/r/36571
Reviewed-by: Varun Colbert <vcolbert@nvidia.com>
Tested-by: Varun Colbert <vcolbert@nvidia.com>

Rebase-Id: R5c6dcd9b9c2bc7ab89870ee13da408ee0a392276

5 files changed, 527 insertions, 302 deletions

diff --git a/drivers/media/video/tegra/ov5650.c b/drivers/media/video/tegra/ov5650.c
index 9f329d375f26..e8d00a5f830a 100644
--- a/drivers/media/video/tegra/ov5650.c
+++ b/drivers/media/video/tegra/ov5650.c
@@ -20,25 +20,13 @@
 #include <linux/slab.h>
 #include <linux/uaccess.h>
 #include <media/ov5650.h>
+#include <media/tegra_camera.h>
 
 struct ov5650_reg {
 	u16 addr;
 	u16 val;
 };
 
-enum StereoCameraMode{
-	/* Sets the default camera to Main */
-	Main,
-	/* Sets the stereo camera to stereo mode. */
-	Stereo,
-	/* Only the sensor on the left is on. */
-	LeftOnly,
-	/* Only the sensor on the right is on. */
-	RightOnly,
-	/* Ignore -- Forces compilers to make 32-bit enums. */
-	StereoCameraMode_Force32 = 0x7FFFFFFF
-};
-
 struct ov5650_sensor {
 	struct i2c_client *i2c_client;
 	struct ov5650_platform_data *pdata;
@@ -51,7 +39,7 @@ struct ov5650_info {
 	struct ov5650_sensor right;
 };
 
-static struct ov5650_info *info;
+static struct ov5650_info *stereo_ov5650_info;
 
 #define OV5650_TABLE_WAIT_MS 0
 #define OV5650_TABLE_END 1
@@ -253,92 +241,104 @@ static struct ov5650_reg mode_1296x972[] = {
 };
 
 static struct ov5650_reg mode_2080x1164[] = {
-	{0x3103, 0x93}, // power up system clock from PLL page 77
-	{0x3007, 0x3b}, // clock enable03 pg 98
-	{0x3017, 0xff}, // PAD output enable page 100
-	{0x3018, 0xfc}, // PAD output enable page 100
-
-	{0x3600, 0x54}, // analog pg 108
-	{0x3601, 0x05}, // analog pg 108
-	{0x3603, 0xa7}, // analog pg 108
-	{0x3604, 0x40}, // analog pg 108
-	{0x3605, 0x04}, // analog pg 108
-	{0x3606, 0x3f}, // analog pg 108
-	{0x3612, 0x1a}, // analog pg 108
-	{0x3613, 0x44}, // analog pg 108
-	{0x3615, 0x52}, // analog pg 108
-	{0x3620, 0x56}, // analog pg 108
-	{0x3623, 0x01}, // analog pg 108
-	{0x3630, 0x22}, // analog pg 108
-	{0x3631, 0x36}, // analog pg 108
-	{0x3632, 0x5f}, // analog pg 108
-	{0x3633, 0x24}, // analog pg 108
-
-	{0x3702, 0x3a}, // analog pg 108
-	{0x3704, 0x18}, // analog pg 108
-	{0x3706, 0x41}, // analog pg 108
-	{0x370b, 0x40}, // analog pg 108
-	{0x370e, 0x00}, // analog pg 108
-	{0x3710, 0x28}, // analog pg 108
+	{0x3103, 0x93}, /* power up system clock from PLL page 77 */
+	{0x3007, 0x3b}, /* clock enable03 pg 98 */
+	{0x3017, 0xff}, /* PAD output enable page 100 */
+	{0x3018, 0xfc}, /* PAD output enable page 100 */
+
+	{0x3600, 0x54}, /* analog pg 108 */
+	{0x3601, 0x05}, /* analog pg 108 */
+	{0x3603, 0xa7}, /* analog pg 108 */
+	{0x3604, 0x40}, /* analog pg 108 */
+	{0x3605, 0x04}, /* analog pg 108 */
+	{0x3606, 0x3f}, /* analog pg 108 */
+	{0x3612, 0x1a}, /* analog pg 108 */
+	{0x3613, 0x44}, /* analog pg 108 */
+	{0x3615, 0x52}, /* analog pg 108 */
+	{0x3620, 0x56}, /* analog pg 108 */
+	{0x3623, 0x01}, /* analog pg 108 */
+	{0x3630, 0x22}, /* analog pg 108 */
+	{0x3631, 0x36}, /* analog pg 108 */
+	{0x3632, 0x5f}, /* analog pg 108 */
+	{0x3633, 0x24}, /* analog pg 108 */
+
+	{0x3702, 0x3a}, /* analog pg 108 */
+	{0x3704, 0x18}, /* analog pg 108 */
+	{0x3706, 0x41}, /* analog pg 108 */
+	{0x370b, 0x40}, /* analog pg 108 */
+	{0x370e, 0x00}, /* analog pg 108 */
+	{0x3710, 0x28}, /* analog pg 108 */
 	{0x3711, 0x24},
-	{0x3712, 0x13}, // analog pg 108
-
-	{0x3810, 0x00}, // TIMING HVOFFS both are zero pg 80
-	{0x3815, 0x82}, // PCLK to SCLK ratio bit[4:0] is set to 2 pg 81
-	{0x3830, 0x50}, // manual exposure gain bit [0]
-	{0x3836, 0x00}, // TIMING HVPAD both are zero pg 82
-
-	{0x3a1a, 0x06}, // DIFF MAX an AEC register??? pg 114
-	{0x3a18, 0x00}, // AEC gain ceiling bit 8 pg 114
-	{0x3a19, 0xf8}, // AEC gain ceiling pg 114
-	{0x3a00, 0x38}, // AEC control 0 debug mode band low limit mode band func pg 112
-	{0x3a0d, 0x06}, // b60 max pg 113
-	{0x3c01, 0x34}, // 5060HZ_CTRL01 pg 116
-
-	{0x401f, 0x03}, // BLC enabled pg 120
-	{0x4000, 0x05}, // BLC enabled pg 120
-	{0x401d, 0x08}, // reserved pg 120
-	{0x4001, 0x02}, // BLC control pg 120
-
-	{0x5000, 0x00}, // ISP control00 features are disabled. pg 132
-	{0x5001, 0x00}, // ISP control01 awb disabled. pg 132
-	{0x5002, 0x00}, // ISP control02 debug mode disabled pg 132
-	{0x503d, 0x00}, // ISP control3D features disabled pg 133
-	{0x5046, 0x00}, // ISP control isp disable awbg disable pg 133
-
-	{0x300f, 0x8f}, // PLL control00 R_SELD5 [7:6] div by 4 R_DIVL [2] two lane div 1 SELD2P5 [1:0] div 2.5 pg 99
-	{0x3010, 0x10}, // PLL control01 DIVM [3:0] DIVS [7:4] div 1 pg 99
-	{0x3011, 0x14}, // PLL control02 R_DIVP [5:0] div 20 pg 99
-	{0x3012, 0x02}, // PLL CTR 03, default
-	{0x3503, 0x33}, // AEC auto AGC auto gain has delay of 2 frames. pg 38
-
-	{0x3621, 0x2f}, // analog horizontal binning/sampling not enabled. pg 108
-	{0x3703, 0xe6}, // analog pg 108
-	{0x370c, 0x00}, // analog pg 108
-	{0x370d, 0x04}, // analog pg 108
-	{0x3713, 0x22}, // analog pg 108
+	{0x3712, 0x13}, /* analog pg 108 */
+
+	{0x3810, 0x00}, /* TIMING HVOFFS both are zero pg 80 */
+	{0x3815, 0x82}, /* PCLK to SCLK ratio bit[4:0] is set to 2 pg 81 */
+	{0x3830, 0x50}, /* manual exposure gain bit [0] */
+	{0x3836, 0x00}, /* TIMING HVPAD both are zero pg 82 */
+
+	{0x3a1a, 0x06}, /* DIFF MAX an AEC register??? pg 114 */
+	{0x3a18, 0x00}, /* AEC gain ceiling bit 8 pg 114 */
+	{0x3a19, 0xf8}, /* AEC gain ceiling pg 114 */
+	{0x3a00, 0x38}, /* AEC control 0 debug mode band
+			low limit mode band func pg 112 */
+	{0x3a0d, 0x06}, /* b60 max pg 113 */
+	{0x3c01, 0x34}, /* 5060HZ_CTRL01 pg 116 */
+
+	{0x401f, 0x03}, /* BLC enabled pg 120 */
+	{0x4000, 0x05}, /* BLC enabled pg 120 */
+	{0x401d, 0x08}, /* reserved pg 120 */
+	{0x4001, 0x02}, /* BLC control pg 120 */
+
+	{0x5000, 0x00}, /* ISP control00 features are disabled. pg 132 */
+	{0x5001, 0x00}, /* ISP control01 awb disabled. pg 132 */
+	{0x5002, 0x00}, /* ISP control02 debug mode disabled pg 132 */
+	{0x503d, 0x00}, /* ISP control3D features disabled pg 133 */
+	{0x5046, 0x00}, /* ISP control isp disable awbg disable pg 133 */
+
+	{0x300f, 0x8f}, /* PLL control00 R_SELD5 [7:6] div by 4 R_DIVL [2]
+				two lane div 1 SELD2P5 [1:0] div 2.5 pg 99 */
+	{0x3010, 0x10}, /* PLL control01 DIVM [3:0] DIVS [7:4] div 1 pg 99 */
+	{0x3011, 0x14}, /* PLL control02 R_DIVP [5:0] div 20 pg 99 */
+	{0x3012, 0x02}, /* PLL CTR 03, default */
+	{0x3503, 0x33}, /* AEC auto AGC auto gain has delay of 2 frames.
+						pg 38 */
+
+	{0x3621, 0x2f}, /* analog horizontal binning/sampling not enabled.
+						pg 108 */
+	{0x3703, 0xe6}, /* analog pg 108 */
+	{0x370c, 0x00}, /* analog pg 108 */
+	{0x370d, 0x04}, /* analog pg 108 */
+	{0x3713, 0x22}, /* analog pg 108 */
 	{0x3714, 0x27},
 	{0x3705, 0xda},
 	{0x370a, 0x80},
 
-	{0x3800, 0x02}, // HREF start point higher 4 bits [3:0] pg 108
-	{0x3801, 0x12}, // HREF start point lower  8 bits [7:0] pg 108
-	{0x3802, 0x00}, // VREF start point higher 4 bits [3:0] pg 108
-	{0x3803, 0x0a}, // VREF start point [7:0] pg 108
-	{0x3804, 0x08}, // HREF width  higher 4 bits [3:0] pg 108
-	{0x3805, 0x20}, // HREF width  lower  8 bits [7:0] pg 108
-	{0x3806, 0x04}, // VREF height higher 4 bits [3:0] pg 109
-	{0x3807, 0x92}, // VREF height lower  8 bits [7:0] pg 109
-	{0x3808, 0x08}, // DVP horizontal output size higher 4 bits [3:0] pg 109
-	{0x3809, 0x20}, // DVP horizontal output size lower  8 bits [7:0] pg 109
-	{0x380a, 0x04}, // DVP vertical   output size higher 4 bits [3:0] pg 109
-	{0x380b, 0x92}, // DVP vertical   output size lower  8 bits [7:0] pg 109
-	{0x380c, 0x0a}, // total horizontal size higher 5 bits [4:0] pg 109, line length
-	{0x380d, 0x96}, // total horizontal size lower  8 bits [7:0] pg 109, line length
-	{0x380e, 0x04}, // total vertical   size higher 5 bits [4:0] pg 109, frame length
-	{0x380f, 0x9e}, // total vertical   size lower  8 bits [7:0] pg 109, frame length
-	{0x3818, 0xc0}, // timing control reg18 mirror & dkhf pg 110
-	{0x381a, 0x3c}, // HS mirror adjustment pg 110
+	{0x3800, 0x02}, /* HREF start point higher 4 bits [3:0] pg 108 */
+	{0x3801, 0x12}, /* HREF start point lower  8 bits [7:0] pg 108 */
+	{0x3802, 0x00}, /* VREF start point higher 4 bits [3:0] pg 108 */
+	{0x3803, 0x0a}, /* VREF start point [7:0] pg 108 */
+	{0x3804, 0x08}, /* HREF width  higher 4 bits [3:0] pg 108 */
+	{0x3805, 0x20}, /* HREF width  lower  8 bits [7:0] pg 108 */
+	{0x3806, 0x04}, /* VREF height higher 4 bits [3:0] pg 109 */
+	{0x3807, 0x92}, /* VREF height lower  8 bits [7:0] pg 109 */
+	{0x3808, 0x08}, /* DVP horizontal output size higher 4 bits [3:0]
+							pg 109 */
+	{0x3809, 0x20}, /* DVP horizontal output size lower  8 bits [7:0]
+							pg 109 */
+	{0x380a, 0x04}, /* DVP vertical   output size higher 4 bits [3:0]
+							pg 109 */
+	{0x380b, 0x92}, /* DVP vertical   output size lower  8 bits [7:0]
+							pg 109 */
+	{0x380c, 0x0a}, /* total horizontal size higher 5 bits [4:0] pg 109,
+							line length */
+	{0x380d, 0x96}, /* total horizontal size lower  8 bits [7:0] pg 109,
+							line length */
+	{0x380e, 0x04}, /* total vertical size higher 5 bits [4:0] pg 109,
+							frame length */
+	{0x380f, 0x9e}, /* total vertical   size lower  8 bits [7:0] pg 109,
+							frame length */
+	{0x3818, 0xc0}, /* timing control reg18 mirror & dkhf pg 110 */
+	{0x381a, 0x3c}, /* HS mirror adjustment pg 110 */
 	{0x381c, 0x31},
 	{0x381d, 0x8e},
 	{0x381e, 0x04},
@@ -349,16 +349,16 @@ static struct ov5650_reg mode_2080x1164[] = {
 	{0x3827, 0x0a},
 	{0x401c, 0x46},
 
-	{0x3003, 0x03}, // reset MIPI and DVP pg 97
-	{0x3500, 0x00}, // long exp 1/3 in unit of 1/16 line, pg 38
-	{0x3501, 0x49}, // long exp 2/3 in unit of 1/16 line, pg 38
-	{0x3502, 0xa0}, // long exp 3/3 in unit of 1/16 line, pg 38
-	{0x350a, 0x00}, // gain output to sensor, pg 38
-	{0x350b, 0x00}, // gain output to sensor, pg 38
-	{0x4801, 0x0f}, // MIPI control01 pg 125
-	{0x300e, 0x0c}, // SC_MIPI_SC_CTRL0 pg 73
-	{0x4803, 0x50}, // MIPI CTRL3 pg 91
-	{0x4800, 0x34}, // MIPI CTRl0 idle and short line pg 89
+	{0x3003, 0x03}, /* reset MIPI and DVP pg 97 */
+	{0x3500, 0x00}, /* long exp 1/3 in unit of 1/16 line, pg 38 */
+	{0x3501, 0x49}, /* long exp 2/3 in unit of 1/16 line, pg 38 */
+	{0x3502, 0xa0}, /* long exp 3/3 in unit of 1/16 line, pg 38 */
+	{0x350a, 0x00}, /* gain output to sensor, pg 38 */
+	{0x350b, 0x00}, /* gain output to sensor, pg 38 */
+	{0x4801, 0x0f}, /* MIPI control01 pg 125 */
+	{0x300e, 0x0c}, /* SC_MIPI_SC_CTRL0 pg 73 */
+	{0x4803, 0x50}, /* MIPI CTRL3 pg 91 */
+	{0x4800, 0x34}, /* MIPI CTRl0 idle and short line pg 89 */
 
 	{OV5650_TABLE_END, 0x0000}
 };
@@ -489,7 +489,7 @@ static inline void ov5650_get_frame_length_regs(struct ov5650_reg *regs,
 
 /* 3 regs to program coarse time */
 static inline void ov5650_get_coarse_time_regs(struct ov5650_reg *regs,
-                                               u32 coarse_time)
+						u32 coarse_time)
 {
 	regs->addr = 0x3500;
 	regs->val = (coarse_time >> 12) & 0xff;
@@ -544,16 +544,16 @@ static int ov5650_write_reg_helper(struct ov5650_info *info,
 	int ret;
 	switch (info->camera_mode) {
 	case Main:
-	case LeftOnly:
+	case StereoCameraMode_Left:
 		ret = ov5650_write_reg(info->left.i2c_client, addr, val);
 		break;
-	case Stereo:
+	case StereoCameraMode_Stereo:
 		ret = ov5650_write_reg(info->left.i2c_client, addr, val);
 		if (ret)
 			break;
 		ret = ov5650_write_reg(info->right.i2c_client, addr, val);
 		break;
-	case RightOnly:
+	case StereoCameraMode_Right:
 		ret = ov5650_write_reg(info->right.i2c_client, addr, val);
 		break;
 	default:
@@ -720,49 +720,29 @@ static int ov5650_test_pattern(struct ov5650_info *info,
 				  NULL, 0);
 }
 
+static int set_power_helper(struct ov5650_platform_data *pdata,
+				int powerLevel)
+{
+	if (pdata) {
+		if (powerLevel && pdata->power_on)
+			pdata->power_on();
+		else if (pdata->power_off)
+			pdata->power_off();
+	}
+	return 0;
+}
+
 static int ov5650_set_power(int powerLevel)
 {
 	pr_info("%s: powerLevel=%d camera mode=%d\n", __func__, powerLevel,
-			info->camera_mode);
+			stereo_ov5650_info->camera_mode);
 
-	switch (info->camera_mode) {
-	case Main:
-	case LeftOnly:
-		if (info->left.pdata) {
-			if (powerLevel && info->left.pdata->power_on)
-				info->left.pdata->power_on();
-			else if (info->left.pdata->power_off)
-				info->left.pdata->power_off();
-		}
-		break;
-
-	case Stereo:
-		if (info->left.pdata) {
-			if (powerLevel && info->left.pdata->power_on)
-				info->left.pdata->power_on();
-			else if (info->left.pdata->power_off)
-				info->left.pdata->power_off();
-		}
-		if (info->right.pdata) {
-			if (powerLevel && info->right.pdata->power_on)
-				info->right.pdata->power_on();
-			else if (info->right.pdata->power_off)
-				info->right.pdata->power_off();
-		}
-		break;
+	if (StereoCameraMode_Left & stereo_ov5650_info->camera_mode)
+		set_power_helper(stereo_ov5650_info->left.pdata, powerLevel);
 
-	case RightOnly:
-		if (info->right.pdata) {
-			if (powerLevel && info->right.pdata->power_on)
-				info->right.pdata->power_on();
-			else if (info->right.pdata->power_off)
-				info->right.pdata->power_off();
-		}
-		break;
+	if (StereoCameraMode_Right & stereo_ov5650_info->camera_mode)
+		set_power_helper(stereo_ov5650_info->right.pdata, powerLevel);
 
-	default:
-		return -1;
-	}
 	return 0;
 }
 
@@ -836,7 +816,7 @@ static long ov5650_ioctl(struct file *file,
 static int ov5650_open(struct inode *inode, struct file *file)
 {
 	pr_info("%s\n", __func__);
-	file->private_data = info;
+	file->private_data = stereo_ov5650_info;
 	ov5650_set_power(1);
 	return 0;
 }
@@ -862,37 +842,63 @@ static struct miscdevice ov5650_device = {
 	.fops = &ov5650_fileops,
 };
 
-static int left_ov5650_probe(struct i2c_client *client,
-			const struct i2c_device_id *id)
+static int ov5650_probe_common(void)
 {
 	int err;
-	pr_info("%s: probing sensor.\n", __func__);
 
-	if (!info) {
-		info = kzalloc(sizeof(struct ov5650_info), GFP_KERNEL);
-		if (!info) {
+	if (!stereo_ov5650_info) {
+		stereo_ov5650_info = kzalloc(sizeof(struct ov5650_info),
+					GFP_KERNEL);
+		if (!stereo_ov5650_info) {
 			pr_err("ov5650: Unable to allocate memory!\n");
 			return -ENOMEM;
 		}
+
+		err = misc_register(&ov5650_device);
+		if (err) {
+			pr_err("ov5650: Unable to register misc device!\n");
+			kfree(stereo_ov5650_info);
+			return err;
+		}
 	}
+	return 0;
+}
+
+static int ov5650_remove_common(struct i2c_client *client)
+{
+	if (stereo_ov5650_info->left.i2c_client ||
+		stereo_ov5650_info->right.i2c_client)
+		return 0;
+
+	misc_deregister(&ov5650_device);
+	kfree(stereo_ov5650_info);
+	stereo_ov5650_info = NULL;
+
+	return 0;
+}
+
+static int left_ov5650_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	int err;
+	pr_info("%s: probing sensor.\n", __func__);
 
-	err = misc_register(&ov5650_device);
-	if (err) {
-		pr_err("ov5650: Unable to register misc device!\n");
-		kfree(info);
+	err = ov5650_probe_common();
+	if (err)
 		return err;
-	}
 
-	info->left.pdata = client->dev.platform_data;
-	info->left.i2c_client = client;
+	stereo_ov5650_info->left.pdata = client->dev.platform_data;
+	stereo_ov5650_info->left.i2c_client = client;
 
 	return 0;
 }
 
 static int left_ov5650_remove(struct i2c_client *client)
 {
-	misc_deregister(&ov5650_device);
-	kfree(info);
+	if (stereo_ov5650_info) {
+		stereo_ov5650_info->left.i2c_client = NULL;
+		ov5650_remove_common(client);
+	}
 	return 0;
 }
 
@@ -917,23 +923,25 @@ static struct i2c_driver left_ov5650_i2c_driver = {
 static int right_ov5650_probe(struct i2c_client *client,
 			const struct i2c_device_id *id)
 {
+	int err;
 	pr_info("%s: probing sensor.\n", __func__);
-	if (!info) {
-		info = kzalloc(sizeof(struct ov5650_info), GFP_KERNEL);
-		if (!info) {
-			pr_err("ov5650_right: Unable to allocate memory!\n");
-			return -ENOMEM;
-		}
-	}
 
-	info->right.pdata = client->dev.platform_data;
-	info->right.i2c_client = client;
+	err = ov5650_probe_common();
+	if (err)
+		return err;
+
+	stereo_ov5650_info->right.pdata = client->dev.platform_data;
+	stereo_ov5650_info->right.i2c_client = client;
 
 	return 0;
 }
 
 static int right_ov5650_remove(struct i2c_client *client)
 {
+	if (stereo_ov5650_info) {
+		stereo_ov5650_info->right.i2c_client = NULL;
+		ov5650_remove_common(client);
+	}
 	return 0;
 }
 
diff --git a/drivers/media/video/tegra/sh532u.c b/drivers/media/video/tegra/sh532u.c
index 7a707dff21e8..1ee2832cc49e 100644
--- a/drivers/media/video/tegra/sh532u.c
+++ b/drivers/media/video/tegra/sh532u.c
@@ -24,7 +24,9 @@
 #include <linux/miscdevice.h>
 #include <linux/slab.h>
 #include <linux/uaccess.h>
+#include <media/ov5650.h>
 #include <media/sh532u.h>
+#include <media/tegra_camera.h>
 
 #include <asm/traps.h>
 
@@ -34,18 +36,22 @@
 #define FOCAL_LENGTH 0x408d70a4 /* (4.42f) */
 #define FNUMBER 0x40333333 /* (2.8f) */
 
-
-struct sh532u_info {
+struct sh532u_sensor {
 	struct i2c_client *i2c_client;
 	struct sh532u_config config;
-	struct sh532u_platform_data sh532u_pdata;
+	struct sh532u_platform_data pdata;
+};
+
+struct sh532u_info {
+	enum StereoCameraMode camera_mode;
+	struct sh532u_sensor *left;
+	struct sh532u_sensor *right;
 };
 
-static struct sh532u_info *info;
+static struct sh532u_info *stereo_sh532u_info; /* set to NULL by compiler */
 
-static int sh532u_read_u8(u8 dev, u8 addr, u8 *val)
+static int sh532u_read_u8(struct i2c_client *client, u8 dev, u8 addr, u8 *val)
 {
-	struct i2c_client *client = info->i2c_client;
 	struct i2c_msg msg[2];
 	unsigned char data[3];
 
@@ -73,9 +79,8 @@ static int sh532u_read_u8(u8 dev, u8 addr, u8 *val)
 	return 0;
 }
 
-static int sh532u_read_u16(u8 addr, u16 *val)
+static int sh532u_read_u16(struct i2c_client *client, u8 addr, u16 *val)
 {
-	struct i2c_client *client = info->i2c_client;
 	struct i2c_msg msg[2];
 	u8 buf[4];
 
@@ -98,9 +103,8 @@ static int sh532u_read_u16(u8 addr, u16 *val)
 	return 0;
 }
 
-static int eeprom_read_u32(u8 addr, u32 *val)
+static int eeprom_read_u32(struct i2c_client *client, u8 addr, u32 *val)
 {
-	struct i2c_client *client = info->i2c_client;
 	struct i2c_msg msg[2];
 	union {
 		u8   dataU8[8];
@@ -127,9 +131,8 @@ static int eeprom_read_u32(u8 addr, u32 *val)
 	return 0;
 }
 
-static int sh532u_write_u8(u16 addr, u8 val)
+static int sh532u_write_u8(struct i2c_client *client, u16 addr, u8 val)
 {
-	struct i2c_client *client = info->i2c_client;
 	struct i2c_msg msg;
 	unsigned char data[2];
 
@@ -143,13 +146,12 @@ static int sh532u_write_u8(u16 addr, u8 val)
 
 	if (i2c_transfer(client->adapter, &msg, 1) != 1)
 		return -1;
-
-	return 0;
+	else
+		return 0;
 }
 
-static int sh532u_write_u16(u16 addr, u16 val)
+static int sh532u_write_u16(struct i2c_client *client, u16 addr, u16 val)
 {
-	struct i2c_client *client = info->i2c_client;
 	struct i2c_msg msg;
 	unsigned char data[3];
 
@@ -167,14 +169,14 @@ static int sh532u_write_u16(u16 addr, u16 val)
 	return 0;
 }
 
-static void move_driver(s16 tarPos)
+static void move_driver(struct i2c_client *client, s16 tarPos)
 {
 	s16 curPos, moveStep;
 	u16 moveDistance;
 	int err;
 
 	/* Read Current Position */
-	err = sh532u_read_u16(RZ_211H, &curPos);
+	err = sh532u_read_u16(client, RZ_211H, &curPos);
 	if (err)
 		goto move_driver_error;
 	/* Check move distance to Target Position */
@@ -182,8 +184,9 @@ static void move_driver(s16 tarPos)
 
 	/* if move distance is shorter than MS1Z12(=Step width) */
 	if (moveDistance <= STMV_SIZE) {
-		err = sh532u_write_u8(MSSET_211, (INI_MSSET_211 | 0x01));
-		err = err | sh532u_write_u16(MS1Z22_211H, tarPos);
+		err = sh532u_write_u8(client, MSSET_211,
+							(INI_MSSET_211 | 0x01));
+		err = err | sh532u_write_u16(client, MS1Z22_211H, tarPos);
 		if (err)
 			goto move_driver_error;
 	} else {
@@ -193,12 +196,11 @@ static void move_driver(s16 tarPos)
 			moveStep = -STMV_SIZE;
 
 		/* Set StepMove Target Positon */
-		err = sh532u_write_u16(MS1Z12_211H, moveStep);
-		err = err | sh532u_write_u16(STMVENDH_211, tarPos);
+		err = sh532u_write_u16(client, MS1Z12_211H, moveStep);
+		err = err | sh532u_write_u16(client, STMVENDH_211, tarPos);
 		/* Start StepMove */
 		err = err |
-		      sh532u_write_u8(
-			STMVEN_211,
+		      sh532u_write_u8(client, STMVEN_211,
 			(STMCHTG_ON | STMSV_ON | STMLFF_OFF | STMVEN_ON));
 		if (err)
 			goto move_driver_error;
@@ -206,10 +208,10 @@ static void move_driver(s16 tarPos)
 
 	return;
 move_driver_error:
-	pr_err("Focuser: %s failed!\n", __func__);
+	pr_err("sh532u: %s failed!\n", __func__);
 }
 
-static void wait_for_move(void)
+static void wait_for_move(struct i2c_client *client)
 {
 	u16 usSmvFin;
 	u8 moveTime, ucParMod, tmp;
@@ -218,14 +220,15 @@ static void wait_for_move(void)
 	moveTime = 0;
 	do {
 		mdelay(1);
-		err = sh532u_read_u8(0, STMVEN_211, &ucParMod);
-		err = err | sh532u_read_u16(RZ_211H, &usSmvFin);
+		err = sh532u_read_u8(client, 0, STMVEN_211, &ucParMod);
+		err = err | sh532u_read_u16(client, RZ_211H, &usSmvFin);
 		if (err)
 			goto wait_for_move_error;
 		/* StepMove Error Handling, Unexpected Position */
 		if ((usSmvFin == 0x7FFF) || (usSmvFin == 0x8001)) {
 			/* Stop StepMove Operation */
-			err = sh532u_write_u8(STMVEN_211, ucParMod & 0xFE);
+			err = sh532u_write_u8(client, STMVEN_211,
+							ucParMod & 0xFE);
 			if (err)
 				goto wait_for_move_error;
 		}
@@ -239,7 +242,7 @@ static void wait_for_move(void)
 		do {
 			mdelay(1);
 			moveTime++;
-			err = sh532u_read_u8(0, MSSET_211, &tmp);
+			err = sh532u_read_u8(client, 0, MSSET_211, &tmp);
 			if (err)
 				goto wait_for_move_error;
 		} while ((tmp & CHTGST_ON) && (moveTime < 15));
@@ -247,17 +250,18 @@ static void wait_for_move(void)
 
 	return;
 wait_for_move_error:
-	pr_err("Focuser: %s failed!\n", __func__);
+	pr_err("sh532u: %s failed!\n", __func__);
 }
 
-static void lens_move_pulse(s16 position)
+static void lens_move_pulse(struct i2c_client *client, s16 position)
 {
-	move_driver(position);
-	wait_for_move();
+	move_driver(client, position);
+	wait_for_move(client);
 }
 
-static void get_rom_info(void)
+static void get_rom_info(struct sh532u_sensor *info)
 {
+	struct i2c_client *client = info->i2c_client;
 	u8 tmp;
 	int err;
 
@@ -265,11 +269,11 @@ static void get_rom_info(void)
 	Inf1 and Mac1 are the mechanical limit position.
 	Inf1     : Bottom limit.
 	Mac1 : Top limit. */
-	err = sh532u_read_u8(0x50, addrMac1, &tmp);
+	err = sh532u_read_u8(client, 0x50, addrMac1, &tmp);
 	if (err)
 		goto get_rom_info_error;
 	info->config.limit_low = (tmp<<8) & 0xff00;
-	err = sh532u_read_u8(0x50, addrInf1, &tmp);
+	err = sh532u_read_u8(client, 0x50, addrInf1, &tmp);
 	if (err)
 		goto get_rom_info_error;
 	info->config.limit_high = (tmp<<8) & 0xff00;
@@ -278,34 +282,35 @@ static void get_rom_info(void)
 	Inf2 and Mac2 are the calibration data for SEMCO AF lens.
 	Inf2: Best focus (lens position) when object distance is 1.2M.
 	Mac2: Best focus (lens position) when object distance is 10cm. */
-	err = sh532u_read_u8(0x50, addrMac2, &tmp);
+	err = sh532u_read_u8(client, 0x50, addrMac2, &tmp);
 	if (err)
 		goto get_rom_info_error;
 	info->config.pos_low = (tmp << 8) & 0xff00;
-	err = sh532u_read_u8(0x50, addrInf2, &tmp);
+	err = sh532u_read_u8(client, 0x50, addrInf2, &tmp);
 	if (err)
 		goto get_rom_info_error;
 	info->config.pos_high = (tmp << 8) & 0xff00;
 
 	return;
 get_rom_info_error:
-	pr_err("Focuser: %s failed!\n", __func__);
+	pr_err("sh532u: %s failed!\n", __func__);
 	info->config.limit_high = POS_HIGH;
 	info->config.limit_low = POS_LOW;
 	info->config.pos_high = POS_HIGH;
 	info->config.pos_low = POS_LOW;
 }
 
-static void init_hvca_pos(void)
+static void init_hvca_pos(struct sh532u_sensor *info)
 {
+	struct i2c_client *client = info->i2c_client;
 	short sBottomLimit, sTopLimit;
 
-	get_rom_info();
+	get_rom_info(info);
 	sBottomLimit = (((int)info->config.limit_low * 5) >> 3) & 0xFFC0;
-	lens_move_pulse(sBottomLimit);
+	lens_move_pulse(client, sBottomLimit);
 	sTopLimit = (((int)info->config.limit_high * 5) >> 3) & 0xFFC0;
-	lens_move_pulse(sTopLimit);
-	lens_move_pulse(info->config.pos_high);
+	lens_move_pulse(client, sTopLimit);
+	lens_move_pulse(client, info->config.pos_high);
 }
 
 static unsigned int a2buf[] = {
@@ -328,8 +333,10 @@ static unsigned int a2buf[] = {
 };
 
 /* Write 1 byte data to the HVCA Drive IC by data type */
-static void sh532u_hvca_wr1(u8 ep_type, u8 ep_data1, u8 ep_addr)
+static void sh532u_hvca_wr1(struct sh532u_sensor *info,
+					u8 ep_type, u8 ep_data1, u8 ep_addr)
 {
+	struct i2c_client *client = info->i2c_client;
 	int err = 0;
 	u8 us_data;
 
@@ -339,20 +346,20 @@ static void sh532u_hvca_wr1(u8 ep_type, u8 ep_data1, u8 ep_addr)
 		break;
 
 	case INDIRECT_EEPROM:
-		err = sh532u_read_u8(0x50, ep_data1, &us_data);
+		err = sh532u_read_u8(client, 0x50, ep_data1, &us_data);
 		break;
 
 	case INDIRECT_HVCA:
-		err = sh532u_read_u8(0, (u16)ep_data1, &us_data);
+		err = sh532u_read_u8(client, 0, (u16)ep_data1, &us_data);
 		break;
 
 	case MASK_AND:
-		err = sh532u_read_u8(0, (u16)ep_addr, &us_data);
+		err = sh532u_read_u8(client, 0, (u16)ep_addr, &us_data);
 		us_data = us_data & ep_data1;
 		break;
 
 	case MASK_OR:
-		err = sh532u_read_u8(0, (u16)ep_addr, &us_data);
+		err = sh532u_read_u8(client, 0, (u16)ep_addr, &us_data);
 		us_data = us_data | ep_data1;
 		break;
 
@@ -360,14 +367,20 @@ static void sh532u_hvca_wr1(u8 ep_type, u8 ep_data1, u8 ep_addr)
 		err = 1;
 	}
 	if (!err)
-		err = sh532u_write_u8((u16)ep_addr, us_data);
+		err = sh532u_write_u8(client, (u16)ep_addr, us_data);
+
+	/* we output error message when there is I2C error, but we can't do
+	 * anything about it nor recover from it. */
 	if (err)
-		pr_err("Focuser: Failed to init!\n");
+		pr_err("sh532u: %s: Failed to init!: client=0x%x, ep_addr=0x%2x, us_data=0x%x, ret=%d\n",
+			__func__, (u32)client, (u32)ep_addr, (u32)us_data, err);
 }
 
 /* Write 2 byte data to the HVCA Drive IC by data type */
-static void sh532u_hvca_wr2(u8 ep_type, u8 ep_data1, u8 ep_data2, u8 ep_addr)
+static void sh532u_hvca_wr2(struct sh532u_sensor *info, u8 ep_type,
+				u8 ep_data1, u8 ep_data2, u8 ep_addr)
 {
+	struct i2c_client *client = info->i2c_client;
 	int err = 0;
 	u8 uc_data1;
 	u8 uc_data2;
@@ -380,27 +393,29 @@ static void sh532u_hvca_wr2(u8 ep_type, u8 ep_data1, u8 ep_data2, u8 ep_addr)
 		break;
 
 	case INDIRECT_EEPROM:
-		err = sh532u_read_u8(0x50, (u16)ep_data1, &uc_data1);
-		err = err | sh532u_read_u8(0x50, (u16)ep_data2, &uc_data2);
+		err = sh532u_read_u8(client, 0x50, (u16)ep_data1,
+							&uc_data1);
+		err = err | sh532u_read_u8(client, 0x50, (u16)ep_data2,
+			&uc_data2);
 		us_data = (((u16)uc_data1 << 8) & 0xFF00) |
 			((u16)uc_data2 & 0x00FF);
 		break;
 
 	case INDIRECT_HVCA:
-		err = sh532u_read_u8(0, (u16)ep_data1, &uc_data1);
-		err = err | sh532u_read_u8(0, (u16)ep_data2, &uc_data2);
+		err = sh532u_read_u8(client, 0, (u16)ep_data1, &uc_data1);
+		err = err | sh532u_read_u8(client, 0, (u16)ep_data2, &uc_data2);
 		us_data = (((u16)uc_data1 << 8) & 0xFF00) |
 			((u16)uc_data2 & 0x00FF);
 		break;
 
 	case MASK_AND:
-		err = sh532u_read_u16((u16)ep_addr, &us_data);
+		err = sh532u_read_u16(client, (u16)ep_addr, &us_data);
 		us_data = us_data & ((((u16)ep_data1 << 8) & 0xFF00) |
 			((u16)ep_data2 & 0x00FF));
 		break;
 
 	case MASK_OR:
-		err = sh532u_read_u16((u16)ep_addr, &us_data);
+		err = sh532u_read_u16(client, (u16)ep_addr, &us_data);
 		us_data = us_data | ((((u16)ep_data1 << 8) & 0xFF00) |
 			((u16)ep_data2 & 0x00FF));
 		break;
@@ -409,23 +424,31 @@ static void sh532u_hvca_wr2(u8 ep_type, u8 ep_data1, u8 ep_data2, u8 ep_addr)
 		err = 1;
 	}
 	if (!err)
-		err = sh532u_write_u16((u16)ep_addr, us_data);
+		err = sh532u_write_u16(client, (u16)ep_addr, us_data);
+
+	/* we output error message when there is I2C error, but we can't do
+	 * anything about it nor recover from it. */
 	if (err)
-		pr_err("Focuser: Failed to init!\n");
+		pr_err("sh532u: %s: Failed to init!: client=0x%x, ep_addr=0x%2x, us_data=0x%x, ret=%d\n",
+			__func__, (u32)client, (u32)ep_addr, (u32)us_data, err);
 }
 
-static void init_driver(void)
+static void init_driver(struct sh532u_sensor *info)
 {
 	int eeprom_addr;
 	unsigned int eeprom_data = 0;
 	u8 ep_addr, ep_type, ep_data1, ep_data2;
 
+	pr_info("sh532u: init_driver: i2c_client = 0x%x\n",
+			(u32)info->i2c_client);
+
 	for (eeprom_addr = 0x30; eeprom_addr <= 0x013C; eeprom_addr += 4) {
 		if (eeprom_addr > 0xff) {
 			/* use hardcoded data instead */
 			eeprom_data = a2buf[(eeprom_addr & 0xFF) / 4];
 		} else {
-			if (eeprom_read_u32(eeprom_addr & 0xFF, &eeprom_data))
+			if (eeprom_read_u32(info->i2c_client,
+				eeprom_addr & 0xFF, &eeprom_data))
 				pr_info("sh532u: cannot read eeprom\n");
 		}
 
@@ -442,9 +465,11 @@ static void init_driver(void)
 			mdelay((unsigned int)((ep_data1 << 8) | ep_data2));
 		} else {
 			if ((ep_type & 0x0F) == DATA_1BYTE) {
-				sh532u_hvca_wr1(ep_type, ep_data1, ep_addr);
+				sh532u_hvca_wr1(info, ep_type, ep_data1,
+								ep_addr);
 			} else {
-				sh532u_hvca_wr2(ep_type,
+				sh532u_hvca_wr2(info,
+						ep_type,
 						ep_data1,
 						ep_data2,
 						ep_addr);
@@ -453,39 +478,40 @@ static void init_driver(void)
 	}
 	msleep(300);
 
-	init_hvca_pos();
+	init_hvca_pos(info);
 }
 
 
-static int sh532u_set_position(struct sh532u_info *info, s16 position)
+static int sh532u_set_position(struct sh532u_sensor *info, s16 position)
 {
 	if (position > info->config.limit_high)
 		return -1;
 	/* Caller's responsibility to check motor status. */
-	move_driver(position);
+	move_driver(info->i2c_client, position);
 	return 0;
 }
 
-static int sh532u_get_move_status(unsigned long arg)
+static int sh532u_get_move_status(struct sh532u_sensor *info, unsigned long arg)
 {
+	struct i2c_client *client = info->i2c_client;
 	enum sh532u_move_status status = SH532U_Forced32;
 	u8 ucTmp;
 	u16 usSmvFin;
-	int err = sh532u_read_u8(0, STMVEN_211, &ucTmp) |
-		sh532u_read_u16(RZ_211H, &usSmvFin);
+	int err = sh532u_read_u8(client, 0, STMVEN_211, &ucTmp) |
+		sh532u_read_u16(client, RZ_211H, &usSmvFin);
 	if (err)
 		return err;
 
 	/* StepMove Error Handling, Unexpected Position */
 	if ((usSmvFin == 0x7FFF) || (usSmvFin == 0x8001)) {
 		/* Stop StepMove Operation */
-		err = sh532u_write_u8(STMVEN_211, ucTmp & 0xFE);
+		err = sh532u_write_u8(client, STMVEN_211, ucTmp & 0xFE);
 		if (err)
 			return err;
 	}
 
 	if (ucTmp & STMVEN_ON) {
-		err = sh532u_read_u8(0, MSSET_211, &ucTmp);
+		err = sh532u_read_u8(client, 0, MSSET_211, &ucTmp);
 		if (err)
 			return err;
 		if  (ucTmp & CHTGST_ON)
@@ -497,24 +523,24 @@ static int sh532u_get_move_status(unsigned long arg)
 
 	if (copy_to_user((void __user *) arg, &status,
 			 sizeof(enum sh532u_move_status))) {
-		pr_info("Error in copying move status: %s: %d\n", __func__, __LINE__);
+		pr_info("Error in copying move status: %s: %d\n",
+				__func__, __LINE__);
 		return -EFAULT;
 	}
 	return 0;
 }
 
-static long sh532u_ioctl(
-	struct file *file,
+static long sh532u_ioctl_helper(
+	struct sh532u_sensor *info,
 	unsigned int cmd,
 	unsigned long arg)
 {
-	struct sh532u_info *info = file->private_data;
-
 	switch (cmd) {
 	case SH532U_IOCTL_GET_CONFIG:
 		if (copy_to_user((void __user *) arg, &info->config,
 				 sizeof(info->config))) {
-			pr_err("Error in copying config: %s: %d\n", __func__, __LINE__);
+			pr_err("Error in copying config: %s: %d\n",
+					__func__, __LINE__);
 			return -EFAULT;
 		}
 		return 0;
@@ -523,31 +549,89 @@ static long sh532u_ioctl(
 		return sh532u_set_position(info, (s16)(arg & 0xffff));
 
 	case SH532U_IOCTL_GET_MOVE_STATUS:
-		return sh532u_get_move_status(arg);
+		return sh532u_get_move_status(info, arg);
 
 	default:
 		return -EINVAL;
 	}
 }
 
+static long sh532u_ioctl(
+	struct file *file,
+	unsigned int cmd,
+	unsigned long arg)
+{
+	struct sh532u_info *stereo_info = file->private_data;
+	int ret;
+
+	/* select a camera */
+	if (cmd == SH532U_IOCTL_SET_CAMERA_MODE) {
+		stereo_info->camera_mode = arg;
+		return 0;
+	}
+
+	if (StereoCameraMode_Left & stereo_info->camera_mode) {
+		ret = sh532u_ioctl_helper(stereo_info->left, cmd, arg);
+		if (ret)
+			return ret;
+
+		if (StereoCameraMode_Stereo & stereo_info->camera_mode) {
+			/* To be finalized for stereo */
+			if (cmd != SH532U_IOCTL_GET_CONFIG)
+				ret = sh532u_ioctl_helper(stereo_info->right,
+								cmd, arg);
+		}
+		return ret;
+	}
+
+	if (StereoCameraMode_Right & stereo_info->camera_mode)
+		return sh532u_ioctl_helper(stereo_info->right, cmd, arg);
+
+	return 0;
+}
+
+static void sh532u_open_helper(struct sh532u_sensor *info)
+{
+	if (info->pdata.board_init)
+		info->pdata.board_init(info->pdata.context_data);
+	init_driver(info);
+}
+
+static void sh532u_release_helper(struct sh532u_sensor *info)
+{
+	if (info->pdata.board_deinit)
+		info->pdata.board_deinit(info->pdata.context_data);
+}
 
 static int sh532u_open(struct inode *inode, struct file *file)
 {
-	pr_info("sh532 open\n");
-	file->private_data = info;
-	if (info->sh532u_pdata.board_init)
-		info->sh532u_pdata.board_init(
-			info->sh532u_pdata.context_data);
-	init_driver();
+	pr_info("sh532u open: camera_mode: %2d\n",
+			stereo_sh532u_info->camera_mode);
+
+	file->private_data = stereo_sh532u_info;
+
+	if (StereoCameraMode_Left & stereo_sh532u_info->camera_mode)
+		sh532u_open_helper(stereo_sh532u_info->left);
+
+	if (StereoCameraMode_Right & stereo_sh532u_info->camera_mode)
+		sh532u_open_helper(stereo_sh532u_info->right);
+
 	return 0;
 }
 
 int sh532u_release(struct inode *inode, struct file *file)
 {
-	pr_info("sh532 release\n");
-	if (info->sh532u_pdata.board_deinit)
-		info->sh532u_pdata.board_deinit(
-			info->sh532u_pdata.context_data);
+	struct sh532u_info *info = file->private_data;
+
+	pr_info("sh532u release: camera_mode: %2d\n",
+			info->camera_mode);
+
+	if (StereoCameraMode_Left & info->camera_mode)
+		sh532u_release_helper(info->left);
+
+	if (StereoCameraMode_Right & info->camera_mode)
+		sh532u_release_helper(info->right);
+
 	file->private_data = NULL;
 	return 0;
 }
@@ -566,75 +650,194 @@ static struct miscdevice sh532u_device = {
 	.fops = &sh532u_fileops,
 };
 
-static int sh532u_probe(
-	struct i2c_client *client,
-	const struct i2c_device_id *id)
+static int sh532u_probe_init(struct i2c_client *client,
+				struct sh532u_sensor **info)
 {
-	int err;
-	struct sh532u_platform_data *sh532u_pdata = client->dev.platform_data;
-
-	pr_info("sh532u: probing sensor.\n");
-	info = kzalloc(sizeof(struct sh532u_info), GFP_KERNEL);
-	if (!info) {
-		pr_err("sh532u: Unable to allocate memory!\n");
+	struct sh532u_platform_data *pdata = client->dev.platform_data;
+	struct sh532u_sensor *p_info =
+		kzalloc(sizeof(struct sh532u_sensor), GFP_KERNEL);
+	if (!p_info) {
+		pr_err("%s\n", "sh532u_sensor: Unable to allocate memory!\n");
 		return -ENOMEM;
 	}
-	err = misc_register(&sh532u_device);
-	if (err) {
-		pr_err("sh532u: Unable to register misc device!\n");
-		kfree(info);
-		return err;
+
+	p_info->i2c_client = client;
+	p_info->config.settle_time = SETTLETIME_MS;
+	p_info->config.focal_length = FOCAL_LENGTH;
+	p_info->config.fnumber = FNUMBER;
+	p_info->config.pos_low = POS_LOW;
+	p_info->config.pos_high = POS_HIGH;
+	i2c_set_clientdata(client, p_info);
+
+	if (pdata) {
+		p_info->pdata.context_data = pdata->context_data;
+		p_info->pdata.board_init = pdata->board_init;
+		p_info->pdata.board_deinit = pdata->board_deinit;
 	}
-	info->i2c_client = client;
-	info->config.settle_time = SETTLETIME_MS;
-	info->config.focal_length = FOCAL_LENGTH;
-	info->config.fnumber = FNUMBER;
-	info->config.pos_low = POS_LOW;
-	info->config.pos_high = POS_HIGH;
-	i2c_set_clientdata(client, info);
 
-	if (sh532u_pdata) {
-		info->sh532u_pdata.context_data = sh532u_pdata->context_data;
-		info->sh532u_pdata.board_init = sh532u_pdata->board_init;
-		info->sh532u_pdata.board_deinit = sh532u_pdata->board_deinit;
+	*info = p_info;
+	return 0;
+}
+
+static int sh532u_probe_helper(struct i2c_client *client)
+{
+	struct sh532u_platform_data *pdata = client->dev.platform_data;
+	int err;
+
+	if (!stereo_sh532u_info) {
+		stereo_sh532u_info = kzalloc(sizeof(struct sh532u_info),
+								GFP_KERNEL);
+		if (!stereo_sh532u_info) {
+			pr_err("%s\n",
+				"sh532u_info: Unable to allocate memory!\n");
+			return -ENOMEM;
+		}
+
+		err = misc_register(&sh532u_device);
+		if (err) {
+			pr_err("sh532u: Unable to register sh532u device!\n");
+			kfree(stereo_sh532u_info);
+			stereo_sh532u_info = NULL;
+			return err;
+		}
+
+		err = sh532u_probe_init(client, &stereo_sh532u_info->left);
+		if (err) {
+			kfree(stereo_sh532u_info);
+			stereo_sh532u_info = NULL;
+			return -ENOMEM;
+		}
+
+		err = sh532u_probe_init(client, &stereo_sh532u_info->right);
+		if (err) {
+			kfree(stereo_sh532u_info);
+			stereo_sh532u_info = NULL;
+			kfree(stereo_sh532u_info->left);
+			return -ENOMEM;
+		}
+		stereo_sh532u_info->camera_mode = StereoCameraMode_Left;
 	}
+
 	return 0;
 }
 
-static int sh532u_remove(struct i2c_client *client)
+static int left_sh532u_probe(
+	struct i2c_client *client,
+	const struct i2c_device_id *id)
 {
-	struct sh532u_info *info;
-	info = i2c_get_clientdata(client);
-	misc_deregister(&sh532u_device);
-	kfree(info);
+	int err ;
+
+	pr_info("left_sh532u: probing sensor: i2c_client=0x%x\n", (u32)client);
+
+	err = sh532u_probe_helper(client);
+
+	return err;
+}
+
+static int left_sh532u_remove(struct i2c_client *client)
+{
+	pr_info("left_sh532u to be removed\n");
+	if (!stereo_sh532u_info || !client) {
+		pr_info("left_sh532u_remove(): NULL pointers\n");
+		return 0;
+	}
+
+	kfree(stereo_sh532u_info->left);
+	stereo_sh532u_info->left = NULL;
+
+	if (!stereo_sh532u_info->right) {
+		misc_deregister(&sh532u_device);
+		kfree(stereo_sh532u_info);
+		stereo_sh532u_info = NULL;
+	}
+
 	return 0;
 }
 
-static const struct i2c_device_id sh532u_id[] = {
+static const struct i2c_device_id left_sh532u_id[] = {
 	{ "sh532u", 0 },
+	{ "sh532uL", 0 },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(i2c, left_sh532u_id);
+
+static struct i2c_driver left_sh532u_i2c_driver = {
+	.driver = {
+		.name = "sh532uL",
+		.owner = THIS_MODULE,
+	},
+	.probe = left_sh532u_probe,
+	.remove = left_sh532u_remove,
+	.id_table = left_sh532u_id,
+};
+
+static int right_sh532u_probe(
+	struct i2c_client *client,
+	const struct i2c_device_id *id)
+{
+	int err ;
+
+	pr_info("right_sh532u: probing sensor: i2c_client=0x%x\n", (u32)client);
+
+	err = sh532u_probe_helper(client);
+
+	return err;
+}
+
+static int right_sh532u_remove(struct i2c_client *client)
+{
+	if (!stereo_sh532u_info || !client) {
+		pr_info("right_sh532u_remove(): NULL pointers\n");
+		return 0;
+	}
+
+	kfree(stereo_sh532u_info->right);
+	stereo_sh532u_info->right = NULL;
+
+	if (!stereo_sh532u_info->left) {
+		misc_deregister(&sh532u_device);
+		kfree(stereo_sh532u_info);
+		stereo_sh532u_info = NULL;
+	}
+
+	return 0;
+}
+
+static const struct i2c_device_id right_sh532u_id[] = {
+	{ "sh532uR", 0 },
 	{ },
 };
 
-MODULE_DEVICE_TABLE(i2c, sh532u_id);
+MODULE_DEVICE_TABLE(i2c, right_sh532u_id);
 
-static struct i2c_driver sh532u_i2c_driver = {
+static struct i2c_driver right_sh532u_i2c_driver = {
 	.driver = {
-		.name = "sh532u",
+		.name = "sh532uR",
 		.owner = THIS_MODULE,
 	},
-	.probe = sh532u_probe,
-	.remove = sh532u_remove,
-	.id_table = sh532u_id,
+	.probe = right_sh532u_probe,
+	.remove = right_sh532u_remove,
+	.id_table = right_sh532u_id,
 };
 
 static int __init sh532u_init(void)
 {
-	return i2c_add_driver(&sh532u_i2c_driver);
+	int ret;
+	pr_info("sh532u focuser driver loading\n");
+	ret = i2c_add_driver(&left_sh532u_i2c_driver);
+	if (ret)
+		return ret;
+
+	ret = i2c_add_driver(&right_sh532u_i2c_driver);
+
+	return ret;
 }
 
 static void __exit sh532u_exit(void)
 {
-	i2c_del_driver(&sh532u_i2c_driver);
+	i2c_del_driver(&left_sh532u_i2c_driver);
+	i2c_del_driver(&right_sh532u_i2c_driver);
 }
 
 module_init(sh532u_init);
diff --git a/include/media/ov5650.h b/include/media/ov5650.h
index 4a5f5baa5b43..d45a952eecf2 100644
--- a/include/media/ov5650.h
+++ b/include/media/ov5650.h
@@ -44,6 +44,7 @@ struct ov5650_mode {
 	__u32 coarse_time;
 	__u16 gain;
 };
+
 #ifdef __KERNEL__
 struct ov5650_platform_data {
 	int (*power_on)(void);
diff --git a/include/media/sh532u.h b/include/media/sh532u.h
index 5c4ffd9d42f8..166caad65f88 100644
--- a/include/media/sh532u.h
+++ b/include/media/sh532u.h
@@ -24,6 +24,7 @@
 #define SH532U_IOCTL_GET_CONFIG		_IOR('o', 1, struct sh532u_config)
 #define SH532U_IOCTL_SET_POSITION	_IOW('o', 2, u32)
 #define SH532U_IOCTL_GET_MOVE_STATUS	_IOW('o', 3, unsigned char)
+#define SH532U_IOCTL_SET_CAMERA_MODE	_IOW('o', 4, unsigned char)
 
 enum sh532u_move_status {
 	SH532U_STATE_UNKNOWN = 1,
diff --git a/include/media/tegra_camera.h b/include/media/tegra_camera.h
index 3c8ddcab26e3..f88d376e6769 100644
--- a/include/media/tegra_camera.h
+++ b/include/media/tegra_camera.h
@@ -13,7 +13,8 @@
  * GNU General Public License for more details.
  *
  */
-
+#ifndef TEGRA_CAMERA_H
+#define TEGRA_CAMERA_H
 enum {
 	TEGRA_CAMERA_MODULE_ISP = 0,
 	TEGRA_CAMERA_MODULE_VI,
@@ -31,8 +32,19 @@ struct tegra_camera_clk_info {
 	unsigned long rate;
 };
 
+enum StereoCameraMode {
+	Main = 0x0,		/* Sets the default camera to Main */
+	StereoCameraMode_Left = 0x01,	/* the left camera is on. */
+	StereoCameraMode_Right = 0x02,	/* the right camera is on. */
+	StereoCameraMode_Stereo = 0x03,	/* both cameras are on. */
+	StereoCameraMode_Force32 = 0x7FFFFFFF
+};
+
+
 #define TEGRA_CAMERA_IOCTL_ENABLE		_IOWR('i', 1, uint)
 #define TEGRA_CAMERA_IOCTL_DISABLE		_IOWR('i', 2, uint)
 #define TEGRA_CAMERA_IOCTL_CLK_SET_RATE		\
 	_IOWR('i', 3, struct tegra_camera_clk_info)
 #define TEGRA_CAMERA_IOCTL_RESET		_IOWR('i', 4, uint)
+
+#endif