mm: remove gup_flags FOLL_WRITE games from __get_user_pages()
[linux-3.10.git] / drivers / media / platform / tegra / camera / camera_common.c
1 /*
2  * camera_common.c - utilities for tegra camera driver
3  *
4  * Copyright (c) 2015-2016, NVIDIA CORPORATION.  All rights reserved.
5  *
6  * This program is free software; you can redistribute it and/or modify it
7  * under the terms and conditions of the GNU General Public License,
8  * version 2, as published by the Free Software Foundation.
9  *
10  * This program is distributed in the hope it will be useful, but WITHOUT
11  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
12  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
13  * more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
17  */
18
19 #include <media/camera_common.h>
20 #include <linux/of_graph.h>
21 #include <linux/string.h>
22 #include <mach/io_dpd.h>
23
24 #define has_s_op(master, op) \
25         (master->ops && master->ops->op)
26 #define call_s_op(master, op) \
27         (has_s_op(master, op) ? \
28          master->ops->op(master) : 0)
29 #define call_s_ops(master, op, ...) \
30         (has_s_op(master, op) ? \
31          master->ops->op(master, __VA_ARGS__) : 0)
32
33 static const struct camera_common_colorfmt camera_common_color_fmts[] = {
34         {
35                 V4L2_MBUS_FMT_SRGGB12_1X12,
36                 V4L2_COLORSPACE_SRGB,
37                 V4L2_PIX_FMT_SRGGB12,
38         },
39         {
40                 V4L2_MBUS_FMT_SRGGB10_1X10,
41                 V4L2_COLORSPACE_SRGB,
42                 V4L2_PIX_FMT_SRGGB10,
43         },
44         {
45                 V4L2_MBUS_FMT_SRGGB8_1X8,
46                 V4L2_COLORSPACE_SRGB,
47                 V4L2_PIX_FMT_SRGGB8,
48         },
49 };
50
51 static struct tegra_io_dpd camera_common_csi_io[] = {
52         {
53                 .name                   = "CSIA",
54                 .io_dpd_reg_index       = 0,
55                 .io_dpd_bit             = 0x0,
56         },
57         {
58                 .name                   = "CSIB",
59                 .io_dpd_reg_index       = 0,
60                 .io_dpd_bit             = 0x1,
61         },
62         {
63                 .name                   = "CSIC",
64                 .io_dpd_reg_index       = 1,
65                 .io_dpd_bit             = 0xa,
66         },
67         {
68                 .name                   = "CSID",
69                 .io_dpd_reg_index       = 1,
70                 .io_dpd_bit             = 0xb,
71         },
72         {
73                 .name                   = "CSIE",
74                 .io_dpd_reg_index       = 1,
75                 .io_dpd_bit             = 0xc,
76         },
77         {
78                 .name                   = "CSIF",
79                 .io_dpd_reg_index       = 1,
80                 .io_dpd_bit             = 0xd,
81         },
82 };
83
84 int camera_common_g_ctrl(struct camera_common_data *s_data,
85                          struct v4l2_control *control)
86 {
87         int i;
88
89         for (i = 0; i < s_data->numctrls; i++) {
90                 if (s_data->ctrls[i]->id == control->id) {
91                         control->value = s_data->ctrls[i]->val;
92                         dev_dbg(&s_data->i2c_client->dev,
93                                  "%s: found control %s\n", __func__,
94                                  s_data->ctrls[i]->name);
95                         return 0;
96                 }
97         }
98
99         return -EINVAL;
100 }
101 EXPORT_SYMBOL(camera_common_g_ctrl);
102
103 int camera_common_regulator_get(struct i2c_client *client,
104                        struct regulator **vreg, const char *vreg_name)
105 {
106         struct regulator *reg = NULL;
107         int err = 0;
108
109         reg = devm_regulator_get(&client->dev, vreg_name);
110         if (unlikely(IS_ERR(reg))) {
111                 dev_err(&client->dev, "%s %s ERR: %p\n",
112                         __func__, vreg_name, reg);
113                 err = PTR_ERR(reg);
114                 reg = NULL;
115         } else
116                 dev_dbg(&client->dev, "%s: %s\n",
117                         __func__, vreg_name);
118
119         *vreg = reg;
120         return err;
121 }
122 EXPORT_SYMBOL(camera_common_regulator_get);
123
124 int camera_common_parse_clocks(struct i2c_client *client,
125                         struct camera_common_pdata *pdata)
126 {
127         struct device_node *np = client->dev.of_node;
128         const char *prop;
129         int proplen = 0;
130         int i = 0;
131         int numclocks = 0;
132         int mclk_index = 0;
133         int parentclk_index = -1;
134         int err = 0;
135
136
137         pdata->mclk_name = NULL;
138         pdata->parentclk_name = NULL;
139         err = of_property_read_string(np, "mclk", &pdata->mclk_name);
140         if (!err) {
141                 dev_dbg(&client->dev, "mclk in DT %s\n", pdata->mclk_name);
142                 of_property_read_string(np, "parent-clk",
143                                               &pdata->parentclk_name);
144                 return 0;
145         }
146
147         prop = (const char *)of_get_property(np, "clock-names", &proplen);
148         if (prop == NULL)
149                 return -ENODATA;
150
151         /* find length of clock-names string array */
152         for (i = 0; i < proplen; i++) {
153                 if (prop[i] == '\0')
154                         numclocks++;
155         }
156
157         if (numclocks > 1) {
158                 err = of_property_read_u32(np, "mclk-index", &mclk_index);
159                 if (err) {
160                         dev_err(&client->dev, "Failed to find mclk index\n");
161                         return err;
162                 }
163                 err = of_property_read_u32(np, "parent-clk-index",
164                                            &parentclk_index);
165         }
166
167         for (i = 0; i < numclocks; i++) {
168                 if (i == mclk_index) {
169                         pdata->mclk_name = prop;
170                         dev_dbg(&client->dev, "%s: mclk_name is %s\n",
171                                  __func__, pdata->mclk_name);
172                 } else if (i == parentclk_index) {
173                         pdata->parentclk_name = prop;
174                         dev_dbg(&client->dev, "%s: parentclk_name is %s\n",
175                                  __func__, pdata->parentclk_name);
176                 } else
177                         dev_dbg(&client->dev, "%s: %s\n", __func__, prop);
178                 prop += strlen(prop) + 1;
179         }
180
181         return 0;
182 }
183
184 int camera_common_parse_ports(struct i2c_client *client,
185                               struct camera_common_data *s_data)
186 {
187         struct device_node *node = client->dev.of_node;
188         struct device_node *ep = NULL;
189         struct device_node *next;
190         int bus_width = 0;
191         int err = 0;
192         int port = 0;
193
194         /* Parse all the remote entities and put them into the list */
195         next = of_graph_get_next_endpoint(node, ep);
196         if (next == NULL)
197                 return -ENODATA;
198
199         of_node_put(ep);
200         ep = next;
201
202         err = of_property_read_u32(ep, "bus-width", &bus_width);
203         if (err) {
204                 dev_err(&client->dev,
205                         "Failed to find num of lanes\n");
206                 return err;
207         }
208         s_data->numlanes = bus_width;
209
210         err = of_property_read_u32(ep, "csi-port", &port);
211         if (err) {
212                 dev_err(&client->dev,
213                         "Failed to find CSI port\n");
214                 return err;
215         }
216         s_data->csi_port = port;
217
218         dev_dbg(&client->dev, "%s: csi port %d num of lanes %d\n",
219                 __func__, s_data->csi_port, s_data->numlanes);
220
221         return 0;
222 }
223
224 int camera_common_debugfs_show(struct seq_file *s, void *unused)
225 {
226         struct camera_common_data *s_data = s->private;
227
228         dev_dbg(&s_data->i2c_client->dev, "%s: ++\n", __func__);
229
230         return 0;
231 }
232
233 ssize_t camera_common_debugfs_write(
234         struct file *file,
235         char const __user *buf,
236         size_t count,
237         loff_t *offset)
238 {
239         struct camera_common_data *s_data =
240                 ((struct seq_file *)file->private_data)->private;
241         struct i2c_client *client = s_data->i2c_client;
242         int err = 0;
243         char buffer[MAX_BUFFER_SIZE];
244         u32 address;
245         u32 data;
246         u8 readback = 0;
247
248         dev_dbg(&client->dev, "%s: ++\n", __func__);
249
250         if (copy_from_user(&buffer, buf, sizeof(buffer)))
251                 goto debugfs_write_fail;
252
253         if (sscanf(buf, "0x%x 0x%x", &address, &data) == 2)
254                 goto set_attr;
255         if (sscanf(buf, "0X%x 0X%x", &address, &data) == 2)
256                 goto set_attr;
257         if (sscanf(buf, "%d %d", &address, &data) == 2)
258                 goto set_attr;
259
260         if (sscanf(buf, "0x%x 0x%x", &address, &data) == 1)
261                 goto read;
262         if (sscanf(buf, "0X%x 0X%x", &address, &data) == 1)
263                 goto read;
264         if (sscanf(buf, "%d %d", &address, &data) == 1)
265                 goto read;
266
267         dev_err(&client->dev, "SYNTAX ERROR: %s\n", buf);
268         return -EFAULT;
269
270 set_attr:
271         dev_dbg(&client->dev,
272                         "new address = %x, data = %x\n", address, data);
273         err |= call_s_ops(s_data, write_reg, address, data);
274 read:
275         err |= call_s_ops(s_data, read_reg, address, &readback);
276         dev_dbg(&client->dev,
277                         "wrote to address 0x%x with value 0x%x\n",
278                         address, readback);
279
280         if (err)
281                 goto debugfs_write_fail;
282
283         return count;
284
285 debugfs_write_fail:
286         dev_err(&client->dev,
287                         "%s: test pattern write failed\n", __func__);
288         return -EFAULT;
289 }
290
291 int camera_common_debugfs_open(struct inode *inode, struct file *file)
292 {
293         struct camera_common_data *s_data = inode->i_private;
294         struct i2c_client *client = s_data->i2c_client;
295
296         dev_dbg(&client->dev, "%s: ++\n", __func__);
297
298         return single_open(file, camera_common_debugfs_show, inode->i_private);
299 }
300
301 static const struct file_operations camera_common_debugfs_fops = {
302         .open           = camera_common_debugfs_open,
303         .read           = seq_read,
304         .write          = camera_common_debugfs_write,
305         .llseek         = seq_lseek,
306         .release        = single_release,
307 };
308
309 void camera_common_remove_debugfs(
310                 struct camera_common_data *s_data)
311 {
312         struct i2c_client *client = s_data->i2c_client;
313
314         dev_dbg(&client->dev, "%s: ++\n", __func__);
315
316         debugfs_remove_recursive(s_data->debugdir);
317         s_data->debugdir = NULL;
318 }
319 EXPORT_SYMBOL(camera_common_remove_debugfs);
320
321 void camera_common_create_debugfs(
322                 struct camera_common_data *s_data,
323                 const char *name)
324 {
325         struct dentry *err;
326         struct i2c_client *client = s_data->i2c_client;
327
328         dev_dbg(&client->dev, "%s %s\n", __func__, name);
329
330         s_data->debugdir =
331                 debugfs_create_dir(name, NULL);
332         if (!s_data->debugdir)
333                 goto remove_debugfs;
334
335         err = debugfs_create_file("d",
336                                 S_IWUSR | S_IRUGO,
337                                 s_data->debugdir, s_data,
338                                 &camera_common_debugfs_fops);
339         if (!err)
340                 goto remove_debugfs;
341
342         return;
343 remove_debugfs:
344         dev_err(&client->dev, "couldn't create debugfs\n");
345         camera_common_remove_debugfs(s_data);
346 }
347 EXPORT_SYMBOL(camera_common_create_debugfs);
348
349 /* Find a data format by a pixel code in an array */
350 const struct camera_common_colorfmt *camera_common_find_datafmt(
351                 enum v4l2_mbus_pixelcode code)
352 {
353         int i;
354
355         for (i = 0; i < ARRAY_SIZE(camera_common_color_fmts); i++)
356                 if (camera_common_color_fmts[i].code == code)
357                         return camera_common_color_fmts + i;
358
359         return NULL;
360 }
361 EXPORT_SYMBOL(camera_common_find_datafmt);
362
363 int camera_common_enum_mbus_code(struct v4l2_subdev *sd,
364                                 struct v4l2_subdev_fh *fh,
365                                 struct v4l2_subdev_mbus_code_enum *code)
366 {
367         struct i2c_client *client = v4l2_get_subdevdata(sd);
368         struct camera_common_data *s_data = to_camera_common_data(client);
369
370         if (s_data->num_color_fmts < 1 || !s_data->color_fmts) {
371                 s_data->color_fmts = camera_common_color_fmts;
372                 s_data->num_color_fmts = ARRAY_SIZE(camera_common_color_fmts);
373         }
374
375         if ((unsigned int)code->index >= s_data->num_color_fmts)
376                 return -EINVAL;
377
378         code->code = s_data->color_fmts[code->index].code;
379         return 0;
380 }
381 EXPORT_SYMBOL(camera_common_enum_mbus_code);
382
383 int camera_common_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
384                          enum v4l2_mbus_pixelcode *code)
385 {
386         struct i2c_client *client = v4l2_get_subdevdata(sd);
387         struct camera_common_data *s_data = to_camera_common_data(client);
388
389         if (s_data->num_color_fmts < 1 || !s_data->color_fmts) {
390                 s_data->color_fmts = camera_common_color_fmts;
391                 s_data->num_color_fmts = ARRAY_SIZE(camera_common_color_fmts);
392         }
393
394         if ((unsigned int)index >= s_data->num_color_fmts)
395                 return -EINVAL;
396
397         *code = s_data->color_fmts[index].code;
398         return 0;
399 }
400 EXPORT_SYMBOL(camera_common_enum_fmt);
401
402 int camera_common_try_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *mf)
403 {
404         struct i2c_client *client = v4l2_get_subdevdata(sd);
405         struct camera_common_data *s_data = to_camera_common_data(client);
406         struct v4l2_control hdr_control;
407         const struct camera_common_frmfmt *frmfmt = s_data->frmfmt;
408         int hdr_en;
409         int err = 0;
410         int i;
411
412         dev_dbg(&client->dev, "%s: size %i x %i\n", __func__,
413                  mf->width, mf->height);
414
415         /* check hdr enable ctrl */
416         hdr_control.id = V4L2_CID_HDR_EN;
417
418         err = v4l2_g_ctrl(s_data->ctrl_handler, &hdr_control);
419         if (err < 0) {
420                 dev_err(&client->dev, "could not find device ctrl.\n");
421                 return err;
422         }
423
424         hdr_en = switch_ctrl_qmenu[hdr_control.value];
425
426         s_data->mode = s_data->def_mode;
427         s_data->fmt_width = s_data->def_width;
428         s_data->fmt_height = s_data->def_height;
429         s_data->fmt_maxfps = s_data->def_maxfps;
430
431         if (s_data->use_sensor_mode_id &&
432                 s_data->sensor_mode_id >= 0 &&
433                 s_data->sensor_mode_id < s_data->numfmts) {
434                 dev_dbg(&client->dev, "%s: use_sensor_mode_id %d\n",
435                                 __func__, s_data->sensor_mode_id);
436                 s_data->mode = frmfmt[s_data->sensor_mode_id].mode;
437                 s_data->fmt_width = mf->width;
438                 s_data->fmt_height = mf->height;
439                 s_data->fmt_maxfps = mf->maxframerate;
440         } else {
441                 for (i = 0; i < s_data->numfmts; i++) {
442                         if (mf->width == frmfmt[i].size.width &&
443                                 mf->height == frmfmt[i].size.height &&
444                                 mf->maxframerate == frmfmt[i].framerates[0] &&
445                                 hdr_en == frmfmt[i].hdr_en) {
446                                 s_data->mode = frmfmt[i].mode;
447                                 s_data->fmt_width = mf->width;
448                                 s_data->fmt_height = mf->height;
449                                 s_data->fmt_maxfps = mf->maxframerate;
450                                 break;
451                         }
452                 }
453
454                 if (i == s_data->numfmts) {
455                         mf->width = s_data->fmt_width;
456                         mf->height = s_data->fmt_height;
457                         mf->maxframerate = s_data->fmt_maxfps;
458                         dev_dbg(&client->dev,
459                                 "%s: invalid resolution supplied(set mode) %d %d %d\n",
460                                 __func__, mf->width,
461                                 mf->height, mf->maxframerate);
462                 }
463         }
464         if (mf->code != V4L2_MBUS_FMT_SRGGB8_1X8 &&
465                 mf->code != V4L2_MBUS_FMT_SRGGB10_1X10) {
466                 mf->code = V4L2_MBUS_FMT_SRGGB10_1X10;
467                 err = -EINVAL;
468         }
469
470         mf->field = V4L2_FIELD_NONE;
471         mf->colorspace = V4L2_COLORSPACE_SRGB;
472
473         return err;
474 }
475 EXPORT_SYMBOL(camera_common_try_fmt);
476
477 int camera_common_s_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *mf)
478 {
479         struct i2c_client *client = v4l2_get_subdevdata(sd);
480         struct camera_common_data *s_data = to_camera_common_data(client);
481         int ret;
482
483         dev_dbg(&client->dev, "%s(%u) size %i x %i\n", __func__,
484                         mf->code, mf->width, mf->height);
485
486         /* MIPI CSI could have changed the format, double-check */
487         if (!camera_common_find_datafmt(mf->code))
488                 return -EINVAL;
489
490         ret = camera_common_try_fmt(sd, mf);
491
492         s_data->colorfmt = camera_common_find_datafmt(mf->code);
493
494         return ret;
495 }
496 EXPORT_SYMBOL(camera_common_s_fmt);
497
498 int camera_common_g_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *mf)
499 {
500         struct i2c_client *client = v4l2_get_subdevdata(sd);
501         struct camera_common_data *s_data = to_camera_common_data(client);
502         const struct camera_common_colorfmt *fmt = s_data->colorfmt;
503
504         dev_dbg(&client->dev, "%s++\n", __func__);
505
506         mf->code                = fmt->code;
507         mf->colorspace          = fmt->colorspace;
508         mf->width               = s_data->fmt_width;
509         mf->height              = s_data->fmt_height;
510         mf->field               = V4L2_FIELD_NONE;
511         mf->maxframerate        = s_data->fmt_maxfps;
512
513         return 0;
514 }
515 EXPORT_SYMBOL(camera_common_g_fmt);
516
517 static int camera_common_evaluate_color_format(struct v4l2_subdev *sd,
518                                                int pixelformat)
519 {
520         struct i2c_client *client = v4l2_get_subdevdata(sd);
521         struct camera_common_data *s_data = to_camera_common_data(client);
522         int i;
523
524         if (!s_data)
525                 return -EINVAL;
526
527         if (s_data->num_color_fmts < 1 || !s_data->color_fmts) {
528                 s_data->color_fmts = camera_common_color_fmts;
529                 s_data->num_color_fmts = ARRAY_SIZE(camera_common_color_fmts);
530         }
531
532         for (i = 0; i < s_data->num_color_fmts; i++) {
533                 if (s_data->color_fmts[i].pix_fmt == pixelformat)
534                         break;
535         }
536
537         if (i >= s_data->num_color_fmts)
538                 return -EINVAL;
539
540         return 0;
541 }
542
543 int camera_common_enum_framesizes(struct v4l2_subdev *sd,
544                                   struct v4l2_frmsizeenum *fsizes)
545 {
546         struct i2c_client *client = v4l2_get_subdevdata(sd);
547         struct camera_common_data *s_data = to_camera_common_data(client);
548         int ret;
549
550         if (!s_data || !s_data->frmfmt)
551                 return -EINVAL;
552
553         if (fsizes->index >= s_data->numfmts)
554                 return -EINVAL;
555
556         ret = camera_common_evaluate_color_format(sd, fsizes->pixel_format);
557         if (ret)
558                 return ret;
559
560         fsizes->type = V4L2_FRMSIZE_TYPE_DISCRETE;
561         fsizes->discrete = s_data->frmfmt[fsizes->index].size;
562
563         return 0;
564 }
565 EXPORT_SYMBOL(camera_common_enum_framesizes);
566
567 int camera_common_enum_frameintervals(struct v4l2_subdev *sd,
568                                       struct v4l2_frmivalenum *fintervals)
569 {
570         struct i2c_client *client = v4l2_get_subdevdata(sd);
571         struct camera_common_data *s_data = to_camera_common_data(client);
572         int i, ret;
573
574         if (!s_data || !s_data->frmfmt)
575                 return -EINVAL;
576
577         /* Check color format */
578         ret = camera_common_evaluate_color_format(sd, fintervals->pixel_format);
579         if (ret)
580                 return ret;
581
582         /* Check resolution sizes */
583         for (i = 0; i < s_data->numfmts; i++) {
584                 if (s_data->frmfmt[i].size.width == fintervals->width &&
585                     s_data->frmfmt[i].size.height == fintervals->height)
586                         break;
587         }
588         if (i >= s_data->numfmts)
589                 return -EINVAL;
590
591         /* Check index is in the rage of framerates array index */
592         if (fintervals->index >= s_data->frmfmt[i].num_framerates)
593                 return -EINVAL;
594
595         fintervals->type = V4L2_FRMSIZE_TYPE_DISCRETE;
596         fintervals->discrete.numerator = 1;
597         fintervals->discrete.denominator =
598                 s_data->frmfmt[i].framerates[fintervals->index];
599
600         return 0;
601 }
602 EXPORT_SYMBOL(camera_common_enum_frameintervals);
603
604 static void camera_common_mclk_disable(struct camera_common_data *s_data)
605 {
606         struct camera_common_power_rail *pw = s_data->power;
607
608         if (!pw) {
609                 dev_err(&s_data->i2c_client->dev, "%s: no device power rail\n",
610                         __func__);
611                 return;
612         }
613
614         dev_dbg(&s_data->i2c_client->dev, "%s: disable MCLK\n", __func__);
615         clk_disable_unprepare(pw->mclk);
616 }
617
618 static int camera_common_mclk_enable(struct camera_common_data *s_data)
619 {
620         int err;
621         struct camera_common_power_rail *pw = s_data->power;
622         unsigned long mclk_init_rate = s_data->def_clk_freq;
623
624         if (!pw) {
625                 dev_err(s_data->dev, "%s: no device power rail\n",
626                         __func__);
627                 return -ENODEV;
628         }
629
630         dev_dbg(s_data->dev, "%s: enable MCLK with %lu Hz\n",
631                 __func__, mclk_init_rate);
632
633         err = clk_set_rate(pw->mclk, mclk_init_rate);
634         if (!err)
635                 err = clk_prepare_enable(pw->mclk);
636         return err;
637 }
638
639 void camera_common_dpd_disable(struct camera_common_data *s_data)
640 {
641         int i;
642         int io_idx;
643         /* 2 lanes per port, divide by two to get numports */
644         int numports = (s_data->numlanes + 1) >> 1;
645
646         /* disable CSI IOs DPD mode to turn on camera */
647         for (i = 0; i < numports; i++) {
648                 io_idx = s_data->csi_port + i;
649                 tegra_io_dpd_disable(&camera_common_csi_io[io_idx]);
650                 dev_dbg(s_data->dev,
651                          "%s: csi %d\n", __func__, io_idx);
652         }
653 }
654
655 void camera_common_dpd_enable(struct camera_common_data *s_data)
656 {
657         int i;
658         int io_idx;
659         /* 2 lanes per port, divide by two to get numports */
660         int numports = (s_data->numlanes + 1) >> 1;
661
662         /* disable CSI IOs DPD mode to turn on camera */
663         for (i = 0; i < numports; i++) {
664                 io_idx = s_data->csi_port + i;
665                 tegra_io_dpd_enable(&camera_common_csi_io[io_idx]);
666                 dev_dbg(s_data->dev,
667                          "%s: csi %d\n", __func__, io_idx);
668         }
669 }
670
671 int camera_common_s_power(struct v4l2_subdev *sd, int on)
672 {
673         int err = 0;
674         struct i2c_client *client = v4l2_get_subdevdata(sd);
675         struct camera_common_data *s_data = to_camera_common_data(client);
676
677         if (on) {
678                 err = camera_common_mclk_enable(s_data);
679                 if (err)
680                         return err;
681
682                 err = call_s_op(s_data, power_on);
683                 if (err) {
684                         dev_err(s_data->dev,
685                                 "%s: error power on\n", __func__);
686                         camera_common_mclk_disable(s_data);
687                 }
688         } else {
689                 call_s_op(s_data, power_off);
690                 camera_common_mclk_disable(s_data);
691         }
692
693         return err;
694 }
695 EXPORT_SYMBOL(camera_common_s_power);
696
697 int camera_common_g_mbus_config(struct v4l2_subdev *sd,
698                                 struct v4l2_mbus_config *cfg)
699 {
700         cfg->type = V4L2_MBUS_CSI2;
701         cfg->flags = V4L2_MBUS_CSI2_4_LANE |
702                 V4L2_MBUS_CSI2_CHANNEL_0 |
703                 V4L2_MBUS_CSI2_CONTINUOUS_CLOCK;
704
705         return 0;
706 }
707 EXPORT_SYMBOL(camera_common_g_mbus_config);
708
709 int camera_common_focuser_s_power(struct v4l2_subdev *sd, int on)
710 {
711         int err = 0;
712         struct i2c_client *client = v4l2_get_subdevdata(sd);
713         struct camera_common_focuser_data *s_data =
714                         to_camera_common_focuser_data(client);
715
716         if (on) {
717                 err = call_s_op(s_data, power_on);
718                 if (err)
719                         dev_err(&s_data->i2c_client->dev,
720                                 "%s: error power on\n", __func__);
721         } else
722                 err = call_s_op(s_data, power_off);
723
724         return err;
725 }
726
727 int camera_common_focuser_init(struct camera_common_focuser_data *s_data)
728 {
729         int err = 0;
730
731         /* power on */
732         err = call_s_op(s_data, power_on);
733         if (err) {
734                 dev_err(&s_data->i2c_client->dev,
735                         "%s: error power on\n", __func__);
736                 return err;
737         }
738
739         /* load default configuration */
740         err = call_s_op(s_data, load_config);
741         if (err) {
742                 dev_err(&s_data->i2c_client->dev,
743                         "%s: error loading config\n", __func__);
744                 goto fail;
745         }
746
747         /* set controls */
748         err = call_s_op(s_data, ctrls_init);
749         if (err)
750                 dev_err(&s_data->i2c_client->dev,
751                         "%s: error initializing controls\n", __func__);
752
753 fail:
754         /* power off */
755         err |= call_s_op(s_data, power_off);
756
757         return err;
758 }
759
760 int camera_common_parse_sensor_mode(struct i2c_client *client,
761                         struct camera_common_pdata *pdata)
762 {
763         struct device_node *np = client->dev.of_node;
764         char temp_str[OF_MAX_STR_LEN];
765         const char *str;
766         struct device_node *node;
767         int num_modes = 0;
768         int err, i;
769
770         /* get number of modes */
771         for (i = 0; num_modes < MAX_NUM_SENSOR_MODES; i++) {
772                 snprintf(temp_str, sizeof(temp_str), "%s%d",
773                         OF_SENSORMODE_PREFIX, i);
774                 node = of_find_node_by_name(np, temp_str);
775                 if (node == NULL)
776                         break;
777                 num_modes++;
778         }
779
780         pdata->mode_info = devm_kzalloc(&client->dev,
781                 num_modes * sizeof(struct camera_common_mode_info),
782                 GFP_KERNEL);
783         if (!pdata->mode_info) {
784                 dev_err(&client->dev, "Failed to allocate memory for mode info\n");
785                 return -ENOMEM;
786         }
787         memset(pdata->mode_info, 0, num_modes *
788                sizeof(struct camera_common_mode_info));
789
790         /* parse mode info */
791         for (i = 0; i < num_modes; i++) {
792                 snprintf(temp_str, sizeof(temp_str), "%s%d",
793                         OF_SENSORMODE_PREFIX, i);
794                 node = of_find_node_by_name(np, temp_str);
795                 if (node == NULL) {
796                         dev_err(&client->dev, "Failed to find mode\n");
797                         return -ENODATA;
798                 };
799
800                 /* read mode width */
801                 of_property_read_string(node, "active_w", &str);
802                 if (str == NULL) {
803                         dev_err(&client->dev, "Failed to read mode width\n");
804                         return -ENODATA;
805                 };
806                 err = kstrtoint(str, 10, &pdata->mode_info[i].width);
807                 if (err) {
808                         dev_err(&client->dev, "Failed to convert mode width\n");
809                         return -EFAULT;
810                 }
811                 /* read mode height */
812                 of_property_read_string(node, "active_h", &str);
813                 if (str == NULL) {
814                         dev_err(&client->dev, "Failed to read mode height\n");
815                         return -ENODATA;
816                 };
817                 err = kstrtoint(str, 10, &pdata->mode_info[i].height);
818                 if (err) {
819                         dev_err(&client->dev, "Failed to convert mode height\n");
820                         return -EFAULT;
821                 }
822                 dev_info(&client->dev, "%s: mode %d x %d:\n", __func__,
823                         pdata->mode_info[i].width, pdata->mode_info[i].height);
824
825                 of_property_read_string(node, "line_length", &str);
826                 if (str == NULL) {
827                         dev_err(&client->dev, "Failed to read mode line_length\n");
828                         return -ENODATA;
829                 };
830                 err = kstrtoint(str, 10, &pdata->mode_info[i].line_length);
831                 if (err) {
832                         dev_err(&client->dev, "Failed to convert mode line_length\n");
833                         return -EFAULT;
834                 }
835
836                 of_property_read_string(node, "pix_clk_hz", &str);
837                 if (str == NULL) {
838                         dev_err(&client->dev, "Failed to read mode pix_clk_hz\n");
839                         return -ENODATA;
840                 };
841                 err = kstrtoll(str, 10, &pdata->mode_info[i].pixel_clock);
842                 if (err) {
843                         dev_err(&client->dev, "Failed to convert mode pix_clk_hz\n");
844                         return -EFAULT;
845                 }
846                 dev_info(&client->dev, "%s: line_length = %d, pixel_clock = %llu\n",
847                         __func__, pdata->mode_info[i].line_length,
848                         pdata->mode_info[i].pixel_clock);
849         }
850
851         return 0;
852 }
853 EXPORT_SYMBOL(camera_common_parse_sensor_mode);