media: video: tegra: sh532u: add get_rom_info()
[linux-3.10.git] / drivers / media / video / tegra / sh532u.c
1 /*
2  * SH532U focuser driver.
3  *
4  * Copyright (C) 2011 NVIDIA Corporation.
5  *
6  * This program is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * This program is distributed in the hope that it will be useful,
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program; if not, write to the Free Software
17  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA
18  * 02111-1307, USA
19  */
20
21 #include <linux/delay.h>
22 #include <linux/fs.h>
23 #include <linux/i2c.h>
24 #include <linux/miscdevice.h>
25 #include <linux/slab.h>
26 #include <linux/uaccess.h>
27 #include <linux/module.h>
28
29 #include <media/ov5650.h>
30 #include <media/sh532u.h>
31 #include <media/tegra_camera.h>
32
33 #include <asm/traps.h>
34
35 #define POS_LOW (0xA000)
36 #define POS_HIGH (0x6000)
37 #define SETTLETIME_MS (7)
38 #define FOCAL_LENGTH 0x408d70a4 /* (4.42f) */
39 #define FNUMBER 0x40333333 /* (2.8f) */
40
41 struct sh532u_sensor {
42         struct i2c_client *i2c_client;
43         struct sh532u_config config;
44         struct sh532u_platform_data pdata;
45 };
46
47 struct sh532u_info {
48         enum StereoCameraMode camera_mode;
49         struct sh532u_sensor *left;
50         struct sh532u_sensor *right;
51 };
52
53 static struct sh532u_info *stereo_sh532u_info; /* set to NULL by compiler */
54
55 static int sh532u_read_u8(struct i2c_client *client, u8 dev, u8 addr, u8 *val)
56 {
57         struct i2c_msg msg[2];
58         unsigned char data[3];
59
60         if (dev)
61                 msg[0].addr = dev;
62         else
63                 msg[0].addr = client->addr;
64         msg[0].flags = 0;
65         msg[0].len = 1;
66         msg[0].buf = data;
67
68         data[0] = (u8)addr;
69
70         if (dev)
71                 msg[1].addr = dev;
72         else
73                 msg[1].addr = client->addr;
74         msg[1].flags = I2C_M_RD;
75         msg[1].len = 1;
76         msg[1].buf = data + 2;
77
78         if (i2c_transfer(client->adapter, msg, 2) != 2)
79                 return -1;
80         *val = data[2];
81         return 0;
82 }
83
84 static int sh532u_read_u16(struct i2c_client *client, u8 addr, u16 *val)
85 {
86         struct i2c_msg msg[2];
87         u8 buf[4];
88
89         msg[0].addr = client->addr;
90         msg[0].flags = 0;
91         msg[0].len = 1;
92         msg[0].buf = &buf[0];
93
94         /* high byte goes out first */
95         buf[0] = (u8) (addr);
96
97         msg[1].addr = client->addr;
98         msg[1].flags = I2C_M_RD;
99         msg[1].len = 2;
100         msg[1].buf = &buf[1];
101
102         if (i2c_transfer(client->adapter, msg, 2) != 2)
103                 return -1;
104         *val = (((u16)buf[1] << 8) | (u16)buf[2]);
105         return 0;
106 }
107
108 static int eeprom_read_u32(struct i2c_client *client, u8 addr, u32 *val)
109 {
110         struct i2c_msg msg[2];
111         union {
112                 u8   dataU8[8];
113                 u32  dataU32[2];
114         } buffer;
115
116         msg[0].addr = 0x50;
117         msg[0].flags = 0;
118         msg[0].len = 1;
119         msg[0].buf = &(buffer.dataU8[0]);
120
121         /* high byte goes out first */
122         buffer.dataU8[0] = (u8) (addr);
123         buffer.dataU8[1] = (u8) (0);
124
125         msg[1].addr = 0x50;
126         msg[1].flags = I2C_M_RD;
127         msg[1].len = 4;
128         msg[1].buf = (u8 *)&(buffer.dataU32[1]);
129
130         if (i2c_transfer(client->adapter, msg, 2) != 2)
131                 return -1;
132         *val = buffer.dataU32[1];
133         return 0;
134 }
135
136 static int sh532u_write_u8(struct i2c_client *client, u16 addr, u8 val)
137 {
138         struct i2c_msg msg;
139         unsigned char data[2];
140
141         data[0] = (u8) (addr & 0xff);
142         data[1] = (u8) (val & 0xff);
143
144         msg.addr = client->addr;
145         msg.flags = 0;
146         msg.len = 2;
147         msg.buf = data;
148
149         if (i2c_transfer(client->adapter, &msg, 1) != 1)
150                 return -1;
151         else
152                 return 0;
153 }
154
155 static int sh532u_write_u16(struct i2c_client *client, u16 addr, u16 val)
156 {
157         struct i2c_msg msg;
158         unsigned char data[3];
159
160         data[0] = (u8) (addr & 0xff);
161         data[1] = (u8) (val >> 8);
162         data[2] = (u8) (val & 0xff);
163
164         msg.addr = client->addr;
165         msg.flags = 0;
166         msg.len = 3;
167         msg.buf = data;
168
169         if (i2c_transfer(client->adapter, &msg, 1) != 1)
170                 return -1;
171         return 0;
172 }
173
174 static void move_driver(struct i2c_client *client, s16 tarPos)
175 {
176         s16 curPos, moveStep;
177         u16 moveDistance;
178         int err;
179
180         /* Read Current Position */
181         err = sh532u_read_u16(client, RZ_211H, &curPos);
182         if (err)
183                 goto move_driver_error;
184         /* Check move distance to Target Position */
185         moveDistance = abs((int)curPos - (int)tarPos);
186
187         /* if move distance is shorter than MS1Z12(=Step width) */
188         if (moveDistance <= STMV_SIZE) {
189                 err = sh532u_write_u8(client, MSSET_211,
190                                                         (INI_MSSET_211 | 0x01));
191                 err = err | sh532u_write_u16(client, MS1Z22_211H, tarPos);
192                 if (err)
193                         goto move_driver_error;
194         } else {
195                 if (curPos < tarPos)
196                         moveStep = STMV_SIZE;
197                 else
198                         moveStep = -STMV_SIZE;
199
200                 /* Set StepMove Target Positon */
201                 err = sh532u_write_u16(client, MS1Z12_211H, moveStep);
202                 err = err | sh532u_write_u16(client, STMVENDH_211, tarPos);
203                 /* Start StepMove */
204                 err = err |
205                       sh532u_write_u8(client, STMVEN_211,
206                         (STMCHTG_ON | STMSV_ON | STMLFF_OFF | STMVEN_ON));
207                 if (err)
208                         goto move_driver_error;
209         }
210
211         return;
212 move_driver_error:
213         pr_err("sh532u: %s failed!\n", __func__);
214 }
215
216 static void wait_for_move(struct i2c_client *client)
217 {
218         u16 usSmvFin;
219         u8 moveTime, ucParMod, tmp;
220         int err;
221
222         moveTime = 0;
223         do {
224                 mdelay(1);
225                 err = sh532u_read_u8(client, 0, STMVEN_211, &ucParMod);
226                 err = err | sh532u_read_u16(client, RZ_211H, &usSmvFin);
227                 if (err)
228                         goto wait_for_move_error;
229                 /* StepMove Error Handling, Unexpected Position */
230                 if ((usSmvFin == 0x7FFF) || (usSmvFin == 0x8001)) {
231                         /* Stop StepMove Operation */
232                         err = sh532u_write_u8(client, STMVEN_211,
233                                                         ucParMod & 0xFE);
234                         if (err)
235                                 goto wait_for_move_error;
236                 }
237                 moveTime++;
238                 /* Wait StepMove operation end */
239         } while ((ucParMod & STMVEN_ON) && (moveTime < 50));
240
241         moveTime = 0;
242         if ((ucParMod & 0x08) == STMCHTG_ON) {
243                 mdelay(5);
244                 do {
245                         mdelay(1);
246                         moveTime++;
247                         err = sh532u_read_u8(client, 0, MSSET_211, &tmp);
248                         if (err)
249                                 goto wait_for_move_error;
250                 } while ((tmp & CHTGST_ON) && (moveTime < 15));
251         }
252
253         return;
254 wait_for_move_error:
255         pr_err("sh532u: %s failed!\n", __func__);
256 }
257
258 static void lens_move_pulse(struct i2c_client *client, s16 position)
259 {
260         move_driver(client, position);
261         wait_for_move(client);
262 }
263
264 static void get_rom_info(struct sh532u_sensor *info)
265 {
266         struct i2c_client *client = info->i2c_client;
267         u8 tmp;
268         int err;
269
270         /* Get Inf1, Mac1
271         Inf1 and Mac1 are the mechanical limit position.
272         Inf1     : Bottom limit.
273         Mac1 : Top limit. */
274         err = sh532u_read_u8(client, 0x50, addrMac1, &tmp);
275         if (err)
276                 goto get_rom_info_error;
277         info->config.limit_low = (tmp<<8) & 0xff00;
278         err = sh532u_read_u8(client, 0x50, addrInf1, &tmp);
279         if (err)
280                 goto get_rom_info_error;
281         info->config.limit_high = (tmp<<8) & 0xff00;
282
283         /* Get Inf2, Mac2
284         Inf2 and Mac2 are the calibration data for SEMCO AF lens.
285         Inf2: Best focus (lens position) when object distance is 1.2M.
286         Mac2: Best focus (lens position) when object distance is 10cm. */
287         err = sh532u_read_u8(client, 0x50, addrMac2, &tmp);
288         if (err)
289                 goto get_rom_info_error;
290         info->config.pos_low = (tmp << 8) & 0xff00;
291         err = sh532u_read_u8(client, 0x50, addrInf2, &tmp);
292         if (err)
293                 goto get_rom_info_error;
294         info->config.pos_high = (tmp << 8) & 0xff00;
295
296         return;
297 get_rom_info_error:
298         pr_err("sh532u: %s failed!\n", __func__);
299         info->config.limit_high = POS_HIGH;
300         info->config.limit_low = POS_LOW;
301         info->config.pos_high = POS_HIGH;
302         info->config.pos_low = POS_LOW;
303 }
304
305 static unsigned int a2buf[] = {
306         0x0018019c,
307         0x0018019d,
308         0x0000019e,
309         0x007f0192,
310         0x00000194,
311         0x00f00184,
312         0x00850187,
313         0x0000018a,
314         0x00fd7187,
315         0x007f7183,
316         0x0008025a,
317         0x05042218,
318         0x80010216,
319         0x000601a0,
320         0x00808183,
321         0xffffffff
322 };
323
324 /* Write 1 byte data to the HVCA Drive IC by data type */
325 static void sh532u_hvca_wr1(struct sh532u_sensor *info,
326                                         u8 ep_type, u8 ep_data1, u8 ep_addr)
327 {
328         struct i2c_client *client = info->i2c_client;
329         int err = 0;
330         u8 us_data;
331
332         switch (ep_type & 0xF0) {
333         case DIRECT_MODE:
334                 us_data = ep_data1;
335                 break;
336
337         case INDIRECT_EEPROM:
338                 err = sh532u_read_u8(client, 0x50, ep_data1, &us_data);
339                 break;
340
341         case INDIRECT_HVCA:
342                 err = sh532u_read_u8(client, 0, (u16)ep_data1, &us_data);
343                 break;
344
345         case MASK_AND:
346                 err = sh532u_read_u8(client, 0, (u16)ep_addr, &us_data);
347                 us_data = us_data & ep_data1;
348                 break;
349
350         case MASK_OR:
351                 err = sh532u_read_u8(client, 0, (u16)ep_addr, &us_data);
352                 us_data = us_data | ep_data1;
353                 break;
354
355         default:
356                 err = 1;
357         }
358         if (!err)
359                 err = sh532u_write_u8(client, (u16)ep_addr, us_data);
360
361         /* we output error message when there is I2C error, but we can't do
362          * anything about it nor recover from it. */
363         if (err)
364                 pr_err("sh532u: %s: Failed to init!: client=0x%x, ep_addr=0x%2x, us_data=0x%x, ret=%d\n",
365                         __func__, (u32)client, (u32)ep_addr, (u32)us_data, err);
366 }
367
368 /* Write 2 byte data to the HVCA Drive IC by data type */
369 static void sh532u_hvca_wr2(struct sh532u_sensor *info, u8 ep_type,
370                                 u8 ep_data1, u8 ep_data2, u8 ep_addr)
371 {
372         struct i2c_client *client = info->i2c_client;
373         int err = 0;
374         u8 uc_data1;
375         u8 uc_data2;
376         u16 us_data;
377
378         switch (ep_type & 0xF0) {
379         case DIRECT_MODE:
380                 us_data = (((u16)ep_data1 << 8) & 0xFF00) |
381                         ((u16)ep_data2 & 0x00FF);
382                 break;
383
384         case INDIRECT_EEPROM:
385                 err = sh532u_read_u8(client, 0x50, (u16)ep_data1,
386                                                         &uc_data1);
387                 err = err | sh532u_read_u8(client, 0x50, (u16)ep_data2,
388                         &uc_data2);
389                 us_data = (((u16)uc_data1 << 8) & 0xFF00) |
390                         ((u16)uc_data2 & 0x00FF);
391                 break;
392
393         case INDIRECT_HVCA:
394                 err = sh532u_read_u8(client, 0, (u16)ep_data1, &uc_data1);
395                 err = err | sh532u_read_u8(client, 0, (u16)ep_data2, &uc_data2);
396                 us_data = (((u16)uc_data1 << 8) & 0xFF00) |
397                         ((u16)uc_data2 & 0x00FF);
398                 break;
399
400         case MASK_AND:
401                 err = sh532u_read_u16(client, (u16)ep_addr, &us_data);
402                 us_data = us_data & ((((u16)ep_data1 << 8) & 0xFF00) |
403                         ((u16)ep_data2 & 0x00FF));
404                 break;
405
406         case MASK_OR:
407                 err = sh532u_read_u16(client, (u16)ep_addr, &us_data);
408                 us_data = us_data | ((((u16)ep_data1 << 8) & 0xFF00) |
409                         ((u16)ep_data2 & 0x00FF));
410                 break;
411
412         default:
413                 err = 1;
414         }
415         if (!err)
416                 err = sh532u_write_u16(client, (u16)ep_addr, us_data);
417
418         /* we output error message when there is I2C error, but we can't do
419          * anything about it nor recover from it. */
420         if (err)
421                 pr_err("sh532u: %s: Failed to init!: client=0x%x, ep_addr=0x%2x, us_data=0x%x, ret=%d\n",
422                         __func__, (u32)client, (u32)ep_addr, (u32)us_data, err);
423 }
424
425 static void init_driver(struct sh532u_sensor *info)
426 {
427         int eeprom_addr;
428         unsigned int eeprom_data = 0;
429         u8 ep_addr, ep_type, ep_data1, ep_data2;
430
431         pr_info("sh532u: init_driver: i2c_client = 0x%x\n",
432                         (u32)info->i2c_client);
433
434         for (eeprom_addr = 0x30; eeprom_addr <= 0x013C; eeprom_addr += 4) {
435                 if (eeprom_addr > 0xff) {
436                         /* use hardcoded data instead */
437                         eeprom_data = a2buf[(eeprom_addr & 0xFF) / 4];
438                 } else {
439                         if (eeprom_read_u32(info->i2c_client,
440                                 eeprom_addr & 0xFF, &eeprom_data))
441                                 pr_info("sh532u: cannot read eeprom\n");
442                 }
443
444                 /* HVCA Address to write eeprom Data1,Data2 by the Data type */
445                 ep_addr = (u8)(eeprom_data & 0x000000ff);
446                 ep_type = (u8)((eeprom_data & 0x0000ff00) >> 8);
447                 ep_data1 = (u8)((eeprom_data & 0x00ff0000) >> 16);
448                 ep_data2 = (u8)((eeprom_data & 0xff000000) >> 24);
449
450                 if (ep_addr == 0xFF)
451                         break;
452
453                 if (ep_addr == 0xDD) {
454                         mdelay((unsigned int)((ep_data1 << 8) | ep_data2));
455                 } else {
456                         if ((ep_type & 0x0F) == DATA_1BYTE) {
457                                 sh532u_hvca_wr1(info, ep_type, ep_data1,
458                                                                 ep_addr);
459                         } else {
460                                 sh532u_hvca_wr2(info,
461                                                 ep_type,
462                                                 ep_data1,
463                                                 ep_data2,
464                                                 ep_addr);
465                         }
466                 }
467         }
468
469         get_rom_info(info);
470 }
471
472
473 static int sh532u_set_position(struct sh532u_sensor *info, s16 position)
474 {
475         if (position > info->config.limit_high)
476                 return -1;
477         /* Caller's responsibility to check motor status. */
478         move_driver(info->i2c_client, position);
479         return 0;
480 }
481
482 static int sh532u_get_move_status(struct sh532u_sensor *info, unsigned long arg)
483 {
484         struct i2c_client *client = info->i2c_client;
485         enum sh532u_move_status status = SH532U_Forced32;
486         u8 ucTmp;
487         u16 usSmvFin;
488         int err = sh532u_read_u8(client, 0, STMVEN_211, &ucTmp) |
489                 sh532u_read_u16(client, RZ_211H, &usSmvFin);
490         if (err)
491                 return err;
492
493         /* StepMove Error Handling, Unexpected Position */
494         if ((usSmvFin == 0x7FFF) || (usSmvFin == 0x8001)) {
495                 /* Stop StepMove Operation */
496                 err = sh532u_write_u8(client, STMVEN_211, ucTmp & 0xFE);
497                 if (err)
498                         return err;
499         }
500
501         if (ucTmp & STMVEN_ON) {
502                 err = sh532u_read_u8(client, 0, MSSET_211, &ucTmp);
503                 if (err)
504                         return err;
505                 if  (ucTmp & CHTGST_ON)
506                         status = SH532U_WAIT_FOR_SETTLE;
507                 else
508                         status = SH532U_LENS_SETTLED;
509         } else
510                 status = SH532U_WAIT_FOR_MOVE_END;
511
512         if (copy_to_user((void __user *) arg, &status,
513                          sizeof(enum sh532u_move_status))) {
514                 pr_info("Error in copying move status: %s: %d\n",
515                                 __func__, __LINE__);
516                 return -EFAULT;
517         }
518         return 0;
519 }
520
521 static long sh532u_ioctl_helper(
522         struct sh532u_sensor *info,
523         unsigned int cmd,
524         unsigned long arg)
525 {
526         switch (cmd) {
527         case SH532U_IOCTL_GET_CONFIG:
528                 if (copy_to_user((void __user *) arg, &info->config,
529                                  sizeof(info->config))) {
530                         pr_err("Error in copying config: %s: %d\n",
531                                         __func__, __LINE__);
532                         return -EFAULT;
533                 }
534                 return 0;
535
536         case SH532U_IOCTL_SET_POSITION:
537                 return sh532u_set_position(info, (s16)(arg & 0xffff));
538
539         case SH532U_IOCTL_GET_MOVE_STATUS:
540                 return sh532u_get_move_status(info, arg);
541
542         default:
543                 return -EINVAL;
544         }
545 }
546
547 static long sh532u_ioctl(
548         struct file *file,
549         unsigned int cmd,
550         unsigned long arg)
551 {
552         struct sh532u_info *stereo_info = file->private_data;
553         int ret;
554
555         /* select a camera */
556         if (cmd == SH532U_IOCTL_SET_CAMERA_MODE) {
557                 stereo_info->camera_mode = arg;
558                 return 0;
559         }
560
561         if (StereoCameraMode_Left & stereo_info->camera_mode) {
562                 ret = sh532u_ioctl_helper(stereo_info->left, cmd, arg);
563                 if (ret)
564                         return ret;
565
566                 if (stereo_info->camera_mode == StereoCameraMode_Stereo) {
567                         /* To be finalized for stereo */
568                         if (cmd != SH532U_IOCTL_GET_CONFIG)
569                                 ret = sh532u_ioctl_helper(stereo_info->right,
570                                                                 cmd, arg);
571                 }
572                 return ret;
573         }
574
575         if (StereoCameraMode_Right & stereo_info->camera_mode)
576                 return sh532u_ioctl_helper(stereo_info->right, cmd, arg);
577
578         return 0;
579 }
580
581 static void sh532u_open_helper(struct sh532u_sensor *info)
582 {
583         if (info->pdata.board_init)
584                 info->pdata.board_init(info->pdata.context_data);
585         init_driver(info);
586 }
587
588 static void sh532u_release_helper(struct sh532u_sensor *info)
589 {
590         if (info->pdata.board_deinit)
591                 info->pdata.board_deinit(info->pdata.context_data);
592 }
593
594 static int sh532u_open(struct inode *inode, struct file *file)
595 {
596         pr_info("sh532u open: camera_mode: %2d\n",
597                         stereo_sh532u_info->camera_mode);
598
599         file->private_data = stereo_sh532u_info;
600
601         if (StereoCameraMode_Left & stereo_sh532u_info->camera_mode)
602                 sh532u_open_helper(stereo_sh532u_info->left);
603
604         if (StereoCameraMode_Right & stereo_sh532u_info->camera_mode)
605                 sh532u_open_helper(stereo_sh532u_info->right);
606
607         return 0;
608 }
609
610 int sh532u_release(struct inode *inode, struct file *file)
611 {
612         struct sh532u_info *info = file->private_data;
613
614         pr_info("sh532u release: camera_mode: %2d\n",
615                         info->camera_mode);
616
617         if (StereoCameraMode_Left & info->camera_mode)
618                 sh532u_release_helper(info->left);
619
620         if (StereoCameraMode_Right & info->camera_mode)
621                 sh532u_release_helper(info->right);
622
623         file->private_data = NULL;
624         return 0;
625 }
626
627
628 static const struct file_operations sh532u_fileops = {
629         .owner = THIS_MODULE,
630         .open = sh532u_open,
631         .unlocked_ioctl = sh532u_ioctl,
632         .release = sh532u_release,
633 };
634
635 static struct miscdevice sh532u_device = {
636         .minor = MISC_DYNAMIC_MINOR,
637         .name = "sh532u",
638         .fops = &sh532u_fileops,
639 };
640
641 static int sh532u_probe_init(struct i2c_client *client,
642                                 struct sh532u_sensor **info)
643 {
644         struct sh532u_platform_data *pdata = client->dev.platform_data;
645         struct sh532u_sensor *p_info =
646                 kzalloc(sizeof(struct sh532u_sensor), GFP_KERNEL);
647         if (!p_info) {
648                 pr_err("%s\n", "sh532u_sensor: Unable to allocate memory!\n");
649                 return -ENOMEM;
650         }
651
652         p_info->i2c_client = client;
653         p_info->config.settle_time = SETTLETIME_MS;
654         p_info->config.focal_length = FOCAL_LENGTH;
655         p_info->config.fnumber = FNUMBER;
656         p_info->config.pos_low = POS_LOW;
657         p_info->config.pos_high = POS_HIGH;
658         i2c_set_clientdata(client, p_info);
659
660         if (pdata) {
661                 p_info->pdata.context_data = pdata->context_data;
662                 p_info->pdata.board_init = pdata->board_init;
663                 p_info->pdata.board_deinit = pdata->board_deinit;
664         }
665
666         *info = p_info;
667         return 0;
668 }
669
670 static int sh532u_probe_helper(struct i2c_client *client)
671 {
672         int err;
673
674         if (!stereo_sh532u_info) {
675                 stereo_sh532u_info = kzalloc(sizeof(struct sh532u_info),
676                                                                 GFP_KERNEL);
677                 if (!stereo_sh532u_info) {
678                         pr_err("%s\n",
679                                 "sh532u_info: Unable to allocate memory!\n");
680                         return -ENOMEM;
681                 }
682
683                 err = misc_register(&sh532u_device);
684                 if (err) {
685                         pr_err("sh532u: Unable to register sh532u device!\n");
686                         kfree(stereo_sh532u_info);
687                         stereo_sh532u_info = NULL;
688                         return err;
689                 }
690
691                 err = sh532u_probe_init(client, &stereo_sh532u_info->left);
692                 if (err) {
693                         kfree(stereo_sh532u_info);
694                         stereo_sh532u_info = NULL;
695                         return -ENOMEM;
696                 }
697
698                 err = sh532u_probe_init(client, &stereo_sh532u_info->right);
699                 if (err) {
700                         kfree(stereo_sh532u_info);
701                         stereo_sh532u_info = NULL;
702                         kfree(stereo_sh532u_info->left);
703                         return -ENOMEM;
704                 }
705                 stereo_sh532u_info->camera_mode = StereoCameraMode_Left;
706         }
707
708         return 0;
709 }
710
711 static int left_sh532u_probe(
712         struct i2c_client *client,
713         const struct i2c_device_id *id)
714 {
715         int err ;
716
717         pr_info("left_sh532u: probing sensor: i2c_client=0x%x\n", (u32)client);
718
719         err = sh532u_probe_helper(client);
720
721         return err;
722 }
723
724 static int left_sh532u_remove(struct i2c_client *client)
725 {
726         pr_info("left_sh532u to be removed\n");
727         if (!stereo_sh532u_info || !client) {
728                 pr_info("left_sh532u_remove(): NULL pointers\n");
729                 return 0;
730         }
731
732         kfree(stereo_sh532u_info->left);
733         stereo_sh532u_info->left = NULL;
734
735         if (!stereo_sh532u_info->right) {
736                 misc_deregister(&sh532u_device);
737                 kfree(stereo_sh532u_info);
738                 stereo_sh532u_info = NULL;
739         }
740
741         return 0;
742 }
743
744 static const struct i2c_device_id left_sh532u_id[] = {
745         { "sh532u", 0 },
746         { "sh532uL", 0 },
747         { },
748 };
749
750 MODULE_DEVICE_TABLE(i2c, left_sh532u_id);
751
752 static struct i2c_driver left_sh532u_i2c_driver = {
753         .driver = {
754                 .name = "sh532uL",
755                 .owner = THIS_MODULE,
756         },
757         .probe = left_sh532u_probe,
758         .remove = left_sh532u_remove,
759         .id_table = left_sh532u_id,
760 };
761
762 static int right_sh532u_probe(
763         struct i2c_client *client,
764         const struct i2c_device_id *id)
765 {
766         int err ;
767
768         pr_info("right_sh532u: probing sensor: i2c_client=0x%x\n", (u32)client);
769
770         err = sh532u_probe_helper(client);
771
772         return err;
773 }
774
775 static int right_sh532u_remove(struct i2c_client *client)
776 {
777         if (!stereo_sh532u_info || !client) {
778                 pr_info("right_sh532u_remove(): NULL pointers\n");
779                 return 0;
780         }
781
782         kfree(stereo_sh532u_info->right);
783         stereo_sh532u_info->right = NULL;
784
785         if (!stereo_sh532u_info->left) {
786                 misc_deregister(&sh532u_device);
787                 kfree(stereo_sh532u_info);
788                 stereo_sh532u_info = NULL;
789         }
790
791         return 0;
792 }
793
794 static const struct i2c_device_id right_sh532u_id[] = {
795         { "sh532uR", 0 },
796         { },
797 };
798
799 MODULE_DEVICE_TABLE(i2c, right_sh532u_id);
800
801 static struct i2c_driver right_sh532u_i2c_driver = {
802         .driver = {
803                 .name = "sh532uR",
804                 .owner = THIS_MODULE,
805         },
806         .probe = right_sh532u_probe,
807         .remove = right_sh532u_remove,
808         .id_table = right_sh532u_id,
809 };
810
811 static int __init sh532u_init(void)
812 {
813         int ret;
814         pr_info("sh532u focuser driver loading\n");
815         ret = i2c_add_driver(&left_sh532u_i2c_driver);
816         if (ret)
817                 return ret;
818
819         ret = i2c_add_driver(&right_sh532u_i2c_driver);
820
821         return ret;
822 }
823
824 static void __exit sh532u_exit(void)
825 {
826         i2c_del_driver(&left_sh532u_i2c_driver);
827         i2c_del_driver(&right_sh532u_i2c_driver);
828 }
829
830 module_init(sh532u_init);
831 module_exit(sh532u_exit);
832