1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17#include <linux/module.h>
18#include <linux/types.h>
19#include <linux/kernel.h>
20#include <linux/mm.h>
21#include <linux/string.h>
22#include <linux/errno.h>
23#include <linux/init.h>
24#include <linux/kmod.h>
25#include <linux/device.h>
26#include <linux/delay.h>
27#include <linux/slab.h>
28#include <linux/i2c.h>
29#include <linux/moduleparam.h>
30#include <media/v4l2-device.h>
31#include "../include/linux/atomisp_gmin_platform.h"
32#include <linux/acpi.h>
33#include <linux/io.h>
34
35#include "ov2722.h"
36
37
38static int ov2722_read_reg(struct i2c_client *client,
39 u16 data_length, u16 reg, u16 *val)
40{
41 int err;
42 struct i2c_msg msg[2];
43 unsigned char data[6];
44
45 if (!client->adapter) {
46 dev_err(&client->dev, "%s error, no client->adapter\n",
47 __func__);
48 return -ENODEV;
49 }
50
51 if (data_length != OV2722_8BIT && data_length != OV2722_16BIT
52 && data_length != OV2722_32BIT) {
53 dev_err(&client->dev, "%s error, invalid data length\n",
54 __func__);
55 return -EINVAL;
56 }
57
58 memset(msg, 0 , sizeof(msg));
59
60 msg[0].addr = client->addr;
61 msg[0].flags = 0;
62 msg[0].len = I2C_MSG_LENGTH;
63 msg[0].buf = data;
64
65
66 data[0] = (u8)(reg >> 8);
67 data[1] = (u8)(reg & 0xff);
68
69 msg[1].addr = client->addr;
70 msg[1].len = data_length;
71 msg[1].flags = I2C_M_RD;
72 msg[1].buf = data;
73
74 err = i2c_transfer(client->adapter, msg, 2);
75 if (err != 2) {
76 if (err >= 0)
77 err = -EIO;
78 dev_err(&client->dev,
79 "read from offset 0x%x error %d", reg, err);
80 return err;
81 }
82
83 *val = 0;
84
85 if (data_length == OV2722_8BIT)
86 *val = (u8)data[0];
87 else if (data_length == OV2722_16BIT)
88 *val = be16_to_cpu(*(__be16 *)&data[0]);
89 else
90 *val = be32_to_cpu(*(__be32 *)&data[0]);
91
92 return 0;
93}
94
95static int ov2722_i2c_write(struct i2c_client *client, u16 len, u8 *data)
96{
97 struct i2c_msg msg;
98 const int num_msg = 1;
99 int ret;
100
101 msg.addr = client->addr;
102 msg.flags = 0;
103 msg.len = len;
104 msg.buf = data;
105 ret = i2c_transfer(client->adapter, &msg, 1);
106
107 return ret == num_msg ? 0 : -EIO;
108}
109
110static int ov2722_write_reg(struct i2c_client *client, u16 data_length,
111 u16 reg, u16 val)
112{
113 int ret;
114 unsigned char data[4] = {0};
115 __be16 *wreg = (__be16 *)data;
116 const u16 len = data_length + sizeof(u16);
117
118 if (data_length != OV2722_8BIT && data_length != OV2722_16BIT) {
119 dev_err(&client->dev,
120 "%s error, invalid data_length\n", __func__);
121 return -EINVAL;
122 }
123
124
125 *wreg = cpu_to_be16(reg);
126
127 if (data_length == OV2722_8BIT) {
128 data[2] = (u8)(val);
129 } else {
130
131 __be16 *wdata = (__be16 *)&data[2];
132
133 *wdata = cpu_to_be16(val);
134 }
135
136 ret = ov2722_i2c_write(client, len, data);
137 if (ret)
138 dev_err(&client->dev,
139 "write error: wrote 0x%x to offset 0x%x error %d",
140 val, reg, ret);
141
142 return ret;
143}
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160static int __ov2722_flush_reg_array(struct i2c_client *client,
161 struct ov2722_write_ctrl *ctrl)
162{
163 u16 size;
164 __be16 *data16 = (void *)&ctrl->buffer.addr;
165
166 if (ctrl->index == 0)
167 return 0;
168
169 size = sizeof(u16) + ctrl->index;
170 *data16 = cpu_to_be16(ctrl->buffer.addr);
171 ctrl->index = 0;
172
173 return ov2722_i2c_write(client, size, (u8 *)&ctrl->buffer);
174}
175
176static int __ov2722_buf_reg_array(struct i2c_client *client,
177 struct ov2722_write_ctrl *ctrl,
178 const struct ov2722_reg *next)
179{
180 int size;
181 __be16 *data16;
182
183 switch (next->type) {
184 case OV2722_8BIT:
185 size = 1;
186 ctrl->buffer.data[ctrl->index] = (u8)next->val;
187 break;
188 case OV2722_16BIT:
189 size = 2;
190 data16 = (void *)&ctrl->buffer.data[ctrl->index];
191 *data16 = cpu_to_be16((u16)next->val);
192 break;
193 default:
194 return -EINVAL;
195 }
196
197
198 if (ctrl->index == 0)
199 ctrl->buffer.addr = next->reg;
200
201 ctrl->index += size;
202
203
204
205
206
207 if (ctrl->index + sizeof(u16) >= OV2722_MAX_WRITE_BUF_SIZE)
208 return __ov2722_flush_reg_array(client, ctrl);
209
210 return 0;
211}
212
213static int __ov2722_write_reg_is_consecutive(struct i2c_client *client,
214 struct ov2722_write_ctrl *ctrl,
215 const struct ov2722_reg *next)
216{
217 if (ctrl->index == 0)
218 return 1;
219
220 return ctrl->buffer.addr + ctrl->index == next->reg;
221}
222
223static int ov2722_write_reg_array(struct i2c_client *client,
224 const struct ov2722_reg *reglist)
225{
226 const struct ov2722_reg *next = reglist;
227 struct ov2722_write_ctrl ctrl;
228 int err;
229
230 ctrl.index = 0;
231 for (; next->type != OV2722_TOK_TERM; next++) {
232 switch (next->type & OV2722_TOK_MASK) {
233 case OV2722_TOK_DELAY:
234 err = __ov2722_flush_reg_array(client, &ctrl);
235 if (err)
236 return err;
237 msleep(next->val);
238 break;
239 default:
240
241
242
243
244 if (!__ov2722_write_reg_is_consecutive(client, &ctrl,
245 next)) {
246 err = __ov2722_flush_reg_array(client, &ctrl);
247 if (err)
248 return err;
249 }
250 err = __ov2722_buf_reg_array(client, &ctrl, next);
251 if (err) {
252 dev_err(&client->dev, "%s: write error, aborted\n",
253 __func__);
254 return err;
255 }
256 break;
257 }
258 }
259
260 return __ov2722_flush_reg_array(client, &ctrl);
261}
262static int ov2722_g_focal(struct v4l2_subdev *sd, s32 *val)
263{
264 *val = (OV2722_FOCAL_LENGTH_NUM << 16) | OV2722_FOCAL_LENGTH_DEM;
265 return 0;
266}
267
268static int ov2722_g_fnumber(struct v4l2_subdev *sd, s32 *val)
269{
270
271 *val = (OV2722_F_NUMBER_DEFAULT_NUM << 16) | OV2722_F_NUMBER_DEM;
272 return 0;
273}
274
275static int ov2722_g_fnumber_range(struct v4l2_subdev *sd, s32 *val)
276{
277 *val = (OV2722_F_NUMBER_DEFAULT_NUM << 24) |
278 (OV2722_F_NUMBER_DEM << 16) |
279 (OV2722_F_NUMBER_DEFAULT_NUM << 8) | OV2722_F_NUMBER_DEM;
280 return 0;
281}
282
283static int ov2722_get_intg_factor(struct i2c_client *client,
284 struct camera_mipi_info *info,
285 const struct ov2722_resolution *res)
286{
287 struct v4l2_subdev *sd = i2c_get_clientdata(client);
288 struct ov2722_device *dev = NULL;
289 struct atomisp_sensor_mode_data *buf = &info->data;
290 const unsigned int ext_clk_freq_hz = 19200000;
291 const unsigned int pll_invariant_div = 10;
292 unsigned int pix_clk_freq_hz;
293 u16 pre_pll_clk_div;
294 u16 pll_multiplier;
295 u16 op_pix_clk_div;
296 u16 reg_val;
297 int ret;
298
299 if (!info)
300 return -EINVAL;
301
302 dev = to_ov2722_sensor(sd);
303
304
305 ret = ov2722_read_reg(client, OV2722_8BIT,
306 OV2722_SC_CMMN_PLL_CTRL3, &pre_pll_clk_div);
307 if (ret)
308 return ret;
309
310 ret = ov2722_read_reg(client, OV2722_8BIT,
311 OV2722_SC_CMMN_PLL_MULTIPLIER, &pll_multiplier);
312 if (ret)
313 return ret;
314
315 ret = ov2722_read_reg(client, OV2722_8BIT,
316 OV2722_SC_CMMN_PLL_DEBUG_OPT, &op_pix_clk_div);
317 if (ret)
318 return ret;
319
320 pre_pll_clk_div = (pre_pll_clk_div & 0x70) >> 4;
321 if (0 == pre_pll_clk_div)
322 return -EINVAL;
323
324 pll_multiplier = pll_multiplier & 0x7f;
325 op_pix_clk_div = op_pix_clk_div & 0x03;
326 pix_clk_freq_hz = ext_clk_freq_hz / pre_pll_clk_div * pll_multiplier
327 * op_pix_clk_div / pll_invariant_div;
328
329 dev->vt_pix_clk_freq_mhz = pix_clk_freq_hz;
330 buf->vt_pix_clk_freq_mhz = pix_clk_freq_hz;
331
332
333 buf->coarse_integration_time_min = OV2722_COARSE_INTG_TIME_MIN;
334 buf->coarse_integration_time_max_margin =
335 OV2722_COARSE_INTG_TIME_MAX_MARGIN;
336
337 buf->fine_integration_time_min = OV2722_FINE_INTG_TIME_MIN;
338 buf->fine_integration_time_max_margin =
339 OV2722_FINE_INTG_TIME_MAX_MARGIN;
340
341 buf->fine_integration_time_def = OV2722_FINE_INTG_TIME_MIN;
342 buf->frame_length_lines = res->lines_per_frame;
343 buf->line_length_pck = res->pixels_per_line;
344 buf->read_mode = res->bin_mode;
345
346
347 ret = ov2722_read_reg(client, OV2722_16BIT,
348 OV2722_H_CROP_START_H, ®_val);
349 if (ret)
350 return ret;
351 buf->crop_horizontal_start = reg_val;
352
353 ret = ov2722_read_reg(client, OV2722_16BIT,
354 OV2722_V_CROP_START_H, ®_val);
355 if (ret)
356 return ret;
357 buf->crop_vertical_start = reg_val;
358
359 ret = ov2722_read_reg(client, OV2722_16BIT,
360 OV2722_H_CROP_END_H, ®_val);
361 if (ret)
362 return ret;
363 buf->crop_horizontal_end = reg_val;
364
365 ret = ov2722_read_reg(client, OV2722_16BIT,
366 OV2722_V_CROP_END_H, ®_val);
367 if (ret)
368 return ret;
369 buf->crop_vertical_end = reg_val;
370
371 ret = ov2722_read_reg(client, OV2722_16BIT,
372 OV2722_H_OUTSIZE_H, ®_val);
373 if (ret)
374 return ret;
375 buf->output_width = reg_val;
376
377 ret = ov2722_read_reg(client, OV2722_16BIT,
378 OV2722_V_OUTSIZE_H, ®_val);
379 if (ret)
380 return ret;
381 buf->output_height = reg_val;
382
383 buf->binning_factor_x = res->bin_factor_x ?
384 res->bin_factor_x : 1;
385 buf->binning_factor_y = res->bin_factor_y ?
386 res->bin_factor_y : 1;
387 return 0;
388}
389
390static long __ov2722_set_exposure(struct v4l2_subdev *sd, int coarse_itg,
391 int gain, int digitgain)
392
393{
394 struct i2c_client *client = v4l2_get_subdevdata(sd);
395 struct ov2722_device *dev = to_ov2722_sensor(sd);
396 u16 hts, vts;
397 int ret;
398
399 dev_dbg(&client->dev, "set_exposure without group hold\n");
400
401
402 ret = ov2722_write_reg(client, OV2722_16BIT, OV2722_VTS_DIFF_H, 0);
403 if (ret)
404 return ret;
405
406 hts = dev->pixels_per_line;
407 vts = dev->lines_per_frame;
408
409 if ((coarse_itg + OV2722_COARSE_INTG_TIME_MAX_MARGIN) > vts)
410 vts = coarse_itg + OV2722_COARSE_INTG_TIME_MAX_MARGIN;
411
412 coarse_itg <<= 4;
413 digitgain <<= 2;
414
415 ret = ov2722_write_reg(client, OV2722_16BIT,
416 OV2722_VTS_H, vts);
417 if (ret)
418 return ret;
419
420 ret = ov2722_write_reg(client, OV2722_16BIT,
421 OV2722_HTS_H, hts);
422 if (ret)
423 return ret;
424
425
426 ret = ov2722_write_reg(client, OV2722_8BIT,
427 OV2722_AEC_PK_EXPO_L,
428 coarse_itg & 0xff);
429 if (ret)
430 return ret;
431
432 ret = ov2722_write_reg(client, OV2722_16BIT,
433 OV2722_AEC_PK_EXPO_H,
434 (coarse_itg >> 8) & 0xfff);
435 if (ret)
436 return ret;
437
438
439 ret = ov2722_write_reg(client, OV2722_16BIT,
440 OV2722_AGC_ADJ_H, gain);
441 if (ret)
442 return ret;
443
444
445 ret = ov2722_write_reg(client, OV2722_16BIT,
446 OV2722_MWB_GAIN_R_H, digitgain);
447 if (ret)
448 return ret;
449
450 ret = ov2722_write_reg(client, OV2722_16BIT,
451 OV2722_MWB_GAIN_G_H, digitgain);
452 if (ret)
453 return ret;
454
455 ret = ov2722_write_reg(client, OV2722_16BIT,
456 OV2722_MWB_GAIN_B_H, digitgain);
457
458 return ret;
459}
460
461static int ov2722_set_exposure(struct v4l2_subdev *sd, int exposure,
462 int gain, int digitgain)
463{
464 struct ov2722_device *dev = to_ov2722_sensor(sd);
465 int ret;
466
467 mutex_lock(&dev->input_lock);
468 ret = __ov2722_set_exposure(sd, exposure, gain, digitgain);
469 mutex_unlock(&dev->input_lock);
470
471 return ret;
472}
473
474static long ov2722_s_exposure(struct v4l2_subdev *sd,
475 struct atomisp_exposure *exposure)
476{
477 int exp = exposure->integration_time[0];
478 int gain = exposure->gain[0];
479 int digitgain = exposure->gain[1];
480
481
482 if (gain == 0) {
483 struct i2c_client *client = v4l2_get_subdevdata(sd);
484 v4l2_err(client, "%s: invalid value\n", __func__);
485 return -EINVAL;
486 }
487
488 return ov2722_set_exposure(sd, exp, gain, digitgain);
489}
490
491static long ov2722_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
492{
493
494 switch (cmd) {
495 case ATOMISP_IOC_S_EXPOSURE:
496 return ov2722_s_exposure(sd, arg);
497 default:
498 return -EINVAL;
499 }
500 return 0;
501}
502
503
504
505
506static int ov2722_q_exposure(struct v4l2_subdev *sd, s32 *value)
507{
508 struct i2c_client *client = v4l2_get_subdevdata(sd);
509 u16 reg_v, reg_v2;
510 int ret;
511
512
513 ret = ov2722_read_reg(client, OV2722_8BIT,
514 OV2722_AEC_PK_EXPO_L,
515 ®_v);
516 if (ret)
517 goto err;
518
519 ret = ov2722_read_reg(client, OV2722_8BIT,
520 OV2722_AEC_PK_EXPO_M,
521 ®_v2);
522 if (ret)
523 goto err;
524
525 reg_v += reg_v2 << 8;
526 ret = ov2722_read_reg(client, OV2722_8BIT,
527 OV2722_AEC_PK_EXPO_H,
528 ®_v2);
529 if (ret)
530 goto err;
531
532 *value = reg_v + (((u32)reg_v2 << 16));
533err:
534 return ret;
535}
536
537static int ov2722_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
538{
539 struct ov2722_device *dev =
540 container_of(ctrl->handler, struct ov2722_device, ctrl_handler);
541 int ret = 0;
542 unsigned int val;
543 switch (ctrl->id) {
544 case V4L2_CID_EXPOSURE_ABSOLUTE:
545 ret = ov2722_q_exposure(&dev->sd, &ctrl->val);
546 break;
547 case V4L2_CID_FOCAL_ABSOLUTE:
548 ret = ov2722_g_focal(&dev->sd, &ctrl->val);
549 break;
550 case V4L2_CID_FNUMBER_ABSOLUTE:
551 ret = ov2722_g_fnumber(&dev->sd, &ctrl->val);
552 break;
553 case V4L2_CID_FNUMBER_RANGE:
554 ret = ov2722_g_fnumber_range(&dev->sd, &ctrl->val);
555 break;
556 case V4L2_CID_LINK_FREQ:
557 val = ov2722_res[dev->fmt_idx].mipi_freq;
558 if (val == 0)
559 return -EINVAL;
560
561 ctrl->val = val * 1000;
562 break;
563 default:
564 ret = -EINVAL;
565 }
566
567 return ret;
568}
569
570static const struct v4l2_ctrl_ops ctrl_ops = {
571 .g_volatile_ctrl = ov2722_g_volatile_ctrl
572};
573
574static const struct v4l2_ctrl_config ov2722_controls[] = {
575 {
576 .ops = &ctrl_ops,
577 .id = V4L2_CID_EXPOSURE_ABSOLUTE,
578 .type = V4L2_CTRL_TYPE_INTEGER,
579 .name = "exposure",
580 .min = 0x0,
581 .max = 0xffff,
582 .step = 0x01,
583 .def = 0x00,
584 .flags = 0,
585 },
586 {
587 .ops = &ctrl_ops,
588 .id = V4L2_CID_FOCAL_ABSOLUTE,
589 .type = V4L2_CTRL_TYPE_INTEGER,
590 .name = "focal length",
591 .min = OV2722_FOCAL_LENGTH_DEFAULT,
592 .max = OV2722_FOCAL_LENGTH_DEFAULT,
593 .step = 0x01,
594 .def = OV2722_FOCAL_LENGTH_DEFAULT,
595 .flags = 0,
596 },
597 {
598 .ops = &ctrl_ops,
599 .id = V4L2_CID_FNUMBER_ABSOLUTE,
600 .type = V4L2_CTRL_TYPE_INTEGER,
601 .name = "f-number",
602 .min = OV2722_F_NUMBER_DEFAULT,
603 .max = OV2722_F_NUMBER_DEFAULT,
604 .step = 0x01,
605 .def = OV2722_F_NUMBER_DEFAULT,
606 .flags = 0,
607 },
608 {
609 .ops = &ctrl_ops,
610 .id = V4L2_CID_FNUMBER_RANGE,
611 .type = V4L2_CTRL_TYPE_INTEGER,
612 .name = "f-number range",
613 .min = OV2722_F_NUMBER_RANGE,
614 .max = OV2722_F_NUMBER_RANGE,
615 .step = 0x01,
616 .def = OV2722_F_NUMBER_RANGE,
617 .flags = 0,
618 },
619 {
620 .ops = &ctrl_ops,
621 .id = V4L2_CID_LINK_FREQ,
622 .name = "Link Frequency",
623 .type = V4L2_CTRL_TYPE_INTEGER,
624 .min = 1,
625 .max = 1500000 * 1000,
626 .step = 1,
627 .def = 1,
628 .flags = V4L2_CTRL_FLAG_VOLATILE | V4L2_CTRL_FLAG_READ_ONLY,
629 },
630};
631
632static int ov2722_init(struct v4l2_subdev *sd)
633{
634 struct ov2722_device *dev = to_ov2722_sensor(sd);
635
636 mutex_lock(&dev->input_lock);
637
638
639 ov2722_res = ov2722_res_preview;
640 N_RES = N_RES_PREVIEW;
641
642 mutex_unlock(&dev->input_lock);
643
644 return 0;
645}
646
647static int power_ctrl(struct v4l2_subdev *sd, bool flag)
648{
649 int ret = -1;
650 struct ov2722_device *dev = to_ov2722_sensor(sd);
651
652 if (!dev || !dev->platform_data)
653 return -ENODEV;
654
655 if (flag) {
656 ret = dev->platform_data->v1p8_ctrl(sd, 1);
657 if (ret == 0) {
658 ret = dev->platform_data->v2p8_ctrl(sd, 1);
659 if (ret)
660 dev->platform_data->v1p8_ctrl(sd, 0);
661 }
662 } else {
663 ret = dev->platform_data->v1p8_ctrl(sd, 0);
664 ret |= dev->platform_data->v2p8_ctrl(sd, 0);
665 }
666
667 return ret;
668}
669
670static int gpio_ctrl(struct v4l2_subdev *sd, bool flag)
671{
672 struct ov2722_device *dev = to_ov2722_sensor(sd);
673 int ret = -1;
674
675 if (!dev || !dev->platform_data)
676 return -ENODEV;
677
678
679
680
681 ret = dev->platform_data->gpio0_ctrl(sd, flag);
682
683
684
685 ret |= dev->platform_data->gpio1_ctrl(sd, !flag);
686 return ret;
687}
688
689static int power_up(struct v4l2_subdev *sd)
690{
691 struct ov2722_device *dev = to_ov2722_sensor(sd);
692 struct i2c_client *client = v4l2_get_subdevdata(sd);
693 int ret;
694
695 if (!dev->platform_data) {
696 dev_err(&client->dev,
697 "no camera_sensor_platform_data");
698 return -ENODEV;
699 }
700
701
702 ret = power_ctrl(sd, 1);
703 if (ret)
704 goto fail_power;
705
706
707 usleep_range(5000, 6000);
708
709
710 ret = gpio_ctrl(sd, 1);
711 if (ret) {
712 ret = gpio_ctrl(sd, 0);
713 if (ret)
714 goto fail_power;
715 }
716
717
718 ret = dev->platform_data->flisclk_ctrl(sd, 1);
719 if (ret)
720 goto fail_clk;
721
722
723 msleep(20);
724
725 return 0;
726
727fail_clk:
728 gpio_ctrl(sd, 0);
729fail_power:
730 power_ctrl(sd, 0);
731 dev_err(&client->dev, "sensor power-up failed\n");
732
733 return ret;
734}
735
736static int power_down(struct v4l2_subdev *sd)
737{
738 struct ov2722_device *dev = to_ov2722_sensor(sd);
739 struct i2c_client *client = v4l2_get_subdevdata(sd);
740 int ret = 0;
741
742 if (!dev->platform_data) {
743 dev_err(&client->dev,
744 "no camera_sensor_platform_data");
745 return -ENODEV;
746 }
747
748 ret = dev->platform_data->flisclk_ctrl(sd, 0);
749 if (ret)
750 dev_err(&client->dev, "flisclk failed\n");
751
752
753 ret = gpio_ctrl(sd, 0);
754 if (ret) {
755 ret = gpio_ctrl(sd, 0);
756 if (ret)
757 dev_err(&client->dev, "gpio failed 2\n");
758 }
759
760
761 ret = power_ctrl(sd, 0);
762 if (ret)
763 dev_err(&client->dev, "vprog failed.\n");
764
765 return ret;
766}
767
768static int ov2722_s_power(struct v4l2_subdev *sd, int on)
769{
770 int ret;
771 if (on == 0)
772 return power_down(sd);
773 else {
774 ret = power_up(sd);
775 if (!ret)
776 return ov2722_init(sd);
777 }
778 return ret;
779}
780
781
782
783
784
785
786
787
788
789
790
791#define LARGEST_ALLOWED_RATIO_MISMATCH 800
792static int distance(struct ov2722_resolution *res, u32 w, u32 h)
793{
794 unsigned int w_ratio = (res->width << 13) / w;
795 unsigned int h_ratio;
796 int match;
797
798 if (h == 0)
799 return -1;
800 h_ratio = (res->height << 13) / h;
801 if (h_ratio == 0)
802 return -1;
803 match = abs(((w_ratio << 13) / h_ratio) - 8192);
804
805 if ((w_ratio < 8192) || (h_ratio < 8192) ||
806 (match > LARGEST_ALLOWED_RATIO_MISMATCH))
807 return -1;
808
809 return w_ratio + h_ratio;
810}
811
812
813static int nearest_resolution_index(int w, int h)
814{
815 int i;
816 int idx = -1;
817 int dist;
818 int min_dist = INT_MAX;
819 struct ov2722_resolution *tmp_res = NULL;
820
821 for (i = 0; i < N_RES; i++) {
822 tmp_res = &ov2722_res[i];
823 dist = distance(tmp_res, w, h);
824 if (dist == -1)
825 continue;
826 if (dist < min_dist) {
827 min_dist = dist;
828 idx = i;
829 }
830 }
831
832 return idx;
833}
834
835static int get_resolution_index(int w, int h)
836{
837 int i;
838
839 for (i = 0; i < N_RES; i++) {
840 if (w != ov2722_res[i].width)
841 continue;
842 if (h != ov2722_res[i].height)
843 continue;
844
845 return i;
846 }
847
848 return -1;
849}
850
851
852static int startup(struct v4l2_subdev *sd)
853{
854 struct ov2722_device *dev = to_ov2722_sensor(sd);
855 struct i2c_client *client = v4l2_get_subdevdata(sd);
856 int ret = 0;
857
858 ret = ov2722_write_reg(client, OV2722_8BIT,
859 OV2722_SW_RESET, 0x01);
860 if (ret) {
861 dev_err(&client->dev, "ov2722 reset err.\n");
862 return ret;
863 }
864
865 ret = ov2722_write_reg_array(client, ov2722_res[dev->fmt_idx].regs);
866 if (ret) {
867 dev_err(&client->dev, "ov2722 write register err.\n");
868 return ret;
869 }
870
871 return ret;
872}
873
874static int ov2722_set_fmt(struct v4l2_subdev *sd,
875 struct v4l2_subdev_pad_config *cfg,
876 struct v4l2_subdev_format *format)
877{
878 struct v4l2_mbus_framefmt *fmt = &format->format;
879 struct ov2722_device *dev = to_ov2722_sensor(sd);
880 struct i2c_client *client = v4l2_get_subdevdata(sd);
881 struct camera_mipi_info *ov2722_info = NULL;
882 int ret = 0;
883 int idx;
884 if (format->pad)
885 return -EINVAL;
886 if (!fmt)
887 return -EINVAL;
888 ov2722_info = v4l2_get_subdev_hostdata(sd);
889 if (!ov2722_info)
890 return -EINVAL;
891
892 mutex_lock(&dev->input_lock);
893 idx = nearest_resolution_index(fmt->width, fmt->height);
894 if (idx == -1) {
895
896 fmt->width = ov2722_res[N_RES - 1].width;
897 fmt->height = ov2722_res[N_RES - 1].height;
898 } else {
899 fmt->width = ov2722_res[idx].width;
900 fmt->height = ov2722_res[idx].height;
901 }
902 fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
903 if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
904 cfg->try_fmt = *fmt;
905 mutex_unlock(&dev->input_lock);
906 return 0;
907 }
908
909 dev->fmt_idx = get_resolution_index(fmt->width, fmt->height);
910 if (dev->fmt_idx == -1) {
911 dev_err(&client->dev, "get resolution fail\n");
912 mutex_unlock(&dev->input_lock);
913 return -EINVAL;
914 }
915
916 dev->pixels_per_line = ov2722_res[dev->fmt_idx].pixels_per_line;
917 dev->lines_per_frame = ov2722_res[dev->fmt_idx].lines_per_frame;
918
919 ret = startup(sd);
920 if (ret) {
921 int i = 0;
922 dev_err(&client->dev, "ov2722 startup err, retry to power up\n");
923 for (i = 0; i < OV2722_POWER_UP_RETRY_NUM; i++) {
924 dev_err(&client->dev,
925 "ov2722 retry to power up %d/%d times, result: ",
926 i + 1, OV2722_POWER_UP_RETRY_NUM);
927 power_down(sd);
928 ret = power_up(sd);
929 if (ret) {
930 dev_err(&client->dev, "power up failed, continue\n");
931 continue;
932 }
933 ret = startup(sd);
934 if (ret) {
935 dev_err(&client->dev, " startup FAILED!\n");
936 } else {
937 dev_err(&client->dev, " startup SUCCESS!\n");
938 break;
939 }
940 }
941 if (ret) {
942 dev_err(&client->dev, "ov2722 startup err\n");
943 goto err;
944 }
945 }
946
947 ret = ov2722_get_intg_factor(client, ov2722_info,
948 &ov2722_res[dev->fmt_idx]);
949 if (ret)
950 dev_err(&client->dev, "failed to get integration_factor\n");
951
952err:
953 mutex_unlock(&dev->input_lock);
954 return ret;
955}
956static int ov2722_get_fmt(struct v4l2_subdev *sd,
957 struct v4l2_subdev_pad_config *cfg,
958 struct v4l2_subdev_format *format)
959{
960 struct v4l2_mbus_framefmt *fmt = &format->format;
961 struct ov2722_device *dev = to_ov2722_sensor(sd);
962
963 if (format->pad)
964 return -EINVAL;
965 if (!fmt)
966 return -EINVAL;
967
968 fmt->width = ov2722_res[dev->fmt_idx].width;
969 fmt->height = ov2722_res[dev->fmt_idx].height;
970 fmt->code = MEDIA_BUS_FMT_SBGGR10_1X10;
971
972 return 0;
973}
974
975static int ov2722_detect(struct i2c_client *client)
976{
977 struct i2c_adapter *adapter = client->adapter;
978 u16 high, low;
979 int ret;
980 u16 id;
981 u8 revision;
982
983 if (!i2c_check_functionality(adapter, I2C_FUNC_I2C))
984 return -ENODEV;
985
986 ret = ov2722_read_reg(client, OV2722_8BIT,
987 OV2722_SC_CMMN_CHIP_ID_H, &high);
988 if (ret) {
989 dev_err(&client->dev, "sensor_id_high = 0x%x\n", high);
990 return -ENODEV;
991 }
992 ret = ov2722_read_reg(client, OV2722_8BIT,
993 OV2722_SC_CMMN_CHIP_ID_L, &low);
994 id = (high << 8) | low;
995
996 if ((id != OV2722_ID) && (id != OV2720_ID)) {
997 dev_err(&client->dev, "sensor ID error\n");
998 return -ENODEV;
999 }
1000
1001 ret = ov2722_read_reg(client, OV2722_8BIT,
1002 OV2722_SC_CMMN_SUB_ID, &high);
1003 revision = (u8) high & 0x0f;
1004
1005 dev_dbg(&client->dev, "sensor_revision = 0x%x\n", revision);
1006 dev_dbg(&client->dev, "detect ov2722 success\n");
1007 return 0;
1008}
1009
1010static int ov2722_s_stream(struct v4l2_subdev *sd, int enable)
1011{
1012 struct ov2722_device *dev = to_ov2722_sensor(sd);
1013 struct i2c_client *client = v4l2_get_subdevdata(sd);
1014 int ret;
1015
1016 mutex_lock(&dev->input_lock);
1017
1018 ret = ov2722_write_reg(client, OV2722_8BIT, OV2722_SW_STREAM,
1019 enable ? OV2722_START_STREAMING :
1020 OV2722_STOP_STREAMING);
1021
1022 mutex_unlock(&dev->input_lock);
1023 return ret;
1024}
1025
1026static int ov2722_s_config(struct v4l2_subdev *sd,
1027 int irq, void *platform_data)
1028{
1029 struct ov2722_device *dev = to_ov2722_sensor(sd);
1030 struct i2c_client *client = v4l2_get_subdevdata(sd);
1031 int ret = 0;
1032
1033 if (!platform_data)
1034 return -ENODEV;
1035
1036 dev->platform_data =
1037 (struct camera_sensor_platform_data *)platform_data;
1038
1039 mutex_lock(&dev->input_lock);
1040
1041
1042
1043
1044
1045 ret = power_down(sd);
1046 if (ret) {
1047 dev_err(&client->dev, "ov2722 power-off err.\n");
1048 goto fail_power_off;
1049 }
1050
1051 ret = power_up(sd);
1052 if (ret) {
1053 dev_err(&client->dev, "ov2722 power-up err.\n");
1054 goto fail_power_on;
1055 }
1056
1057 ret = dev->platform_data->csi_cfg(sd, 1);
1058 if (ret)
1059 goto fail_csi_cfg;
1060
1061
1062 ret = ov2722_detect(client);
1063 if (ret) {
1064 dev_err(&client->dev, "ov2722_detect err s_config.\n");
1065 goto fail_csi_cfg;
1066 }
1067
1068
1069 ret = power_down(sd);
1070 if (ret) {
1071 dev_err(&client->dev, "ov2722 power-off err.\n");
1072 goto fail_csi_cfg;
1073 }
1074 mutex_unlock(&dev->input_lock);
1075
1076 return 0;
1077
1078fail_csi_cfg:
1079 dev->platform_data->csi_cfg(sd, 0);
1080fail_power_on:
1081 power_down(sd);
1082 dev_err(&client->dev, "sensor power-gating failed\n");
1083fail_power_off:
1084 mutex_unlock(&dev->input_lock);
1085 return ret;
1086}
1087
1088static int ov2722_g_frame_interval(struct v4l2_subdev *sd,
1089 struct v4l2_subdev_frame_interval *interval)
1090{
1091 struct ov2722_device *dev = to_ov2722_sensor(sd);
1092
1093 interval->interval.numerator = 1;
1094 interval->interval.denominator = ov2722_res[dev->fmt_idx].fps;
1095
1096 return 0;
1097}
1098
1099static int ov2722_enum_mbus_code(struct v4l2_subdev *sd,
1100 struct v4l2_subdev_pad_config *cfg,
1101 struct v4l2_subdev_mbus_code_enum *code)
1102{
1103 if (code->index >= MAX_FMTS)
1104 return -EINVAL;
1105
1106 code->code = MEDIA_BUS_FMT_SBGGR10_1X10;
1107 return 0;
1108}
1109
1110static int ov2722_enum_frame_size(struct v4l2_subdev *sd,
1111 struct v4l2_subdev_pad_config *cfg,
1112 struct v4l2_subdev_frame_size_enum *fse)
1113{
1114 int index = fse->index;
1115
1116 if (index >= N_RES)
1117 return -EINVAL;
1118
1119 fse->min_width = ov2722_res[index].width;
1120 fse->min_height = ov2722_res[index].height;
1121 fse->max_width = ov2722_res[index].width;
1122 fse->max_height = ov2722_res[index].height;
1123
1124 return 0;
1125
1126}
1127
1128
1129static int ov2722_g_skip_frames(struct v4l2_subdev *sd, u32 *frames)
1130{
1131 struct ov2722_device *dev = to_ov2722_sensor(sd);
1132
1133 mutex_lock(&dev->input_lock);
1134 *frames = ov2722_res[dev->fmt_idx].skip_frames;
1135 mutex_unlock(&dev->input_lock);
1136
1137 return 0;
1138}
1139
1140static const struct v4l2_subdev_sensor_ops ov2722_sensor_ops = {
1141 .g_skip_frames = ov2722_g_skip_frames,
1142};
1143
1144static const struct v4l2_subdev_video_ops ov2722_video_ops = {
1145 .s_stream = ov2722_s_stream,
1146 .g_frame_interval = ov2722_g_frame_interval,
1147};
1148
1149static const struct v4l2_subdev_core_ops ov2722_core_ops = {
1150 .s_power = ov2722_s_power,
1151 .ioctl = ov2722_ioctl,
1152};
1153
1154static const struct v4l2_subdev_pad_ops ov2722_pad_ops = {
1155 .enum_mbus_code = ov2722_enum_mbus_code,
1156 .enum_frame_size = ov2722_enum_frame_size,
1157 .get_fmt = ov2722_get_fmt,
1158 .set_fmt = ov2722_set_fmt,
1159};
1160
1161static const struct v4l2_subdev_ops ov2722_ops = {
1162 .core = &ov2722_core_ops,
1163 .video = &ov2722_video_ops,
1164 .pad = &ov2722_pad_ops,
1165 .sensor = &ov2722_sensor_ops,
1166};
1167
1168static int ov2722_remove(struct i2c_client *client)
1169{
1170 struct v4l2_subdev *sd = i2c_get_clientdata(client);
1171 struct ov2722_device *dev = to_ov2722_sensor(sd);
1172 dev_dbg(&client->dev, "ov2722_remove...\n");
1173
1174 dev->platform_data->csi_cfg(sd, 0);
1175 v4l2_ctrl_handler_free(&dev->ctrl_handler);
1176 v4l2_device_unregister_subdev(sd);
1177
1178 atomisp_gmin_remove_subdev(sd);
1179
1180 media_entity_cleanup(&dev->sd.entity);
1181 kfree(dev);
1182
1183 return 0;
1184}
1185
1186static int __ov2722_init_ctrl_handler(struct ov2722_device *dev)
1187{
1188 struct v4l2_ctrl_handler *hdl;
1189 unsigned int i;
1190 hdl = &dev->ctrl_handler;
1191 v4l2_ctrl_handler_init(&dev->ctrl_handler, ARRAY_SIZE(ov2722_controls));
1192 for (i = 0; i < ARRAY_SIZE(ov2722_controls); i++)
1193 v4l2_ctrl_new_custom(&dev->ctrl_handler, &ov2722_controls[i],
1194 NULL);
1195
1196 dev->link_freq = v4l2_ctrl_find(&dev->ctrl_handler, V4L2_CID_LINK_FREQ);
1197
1198 if (dev->ctrl_handler.error || !dev->link_freq)
1199 return dev->ctrl_handler.error;
1200
1201 dev->sd.ctrl_handler = hdl;
1202
1203 return 0;
1204}
1205
1206static int ov2722_probe(struct i2c_client *client)
1207{
1208 struct ov2722_device *dev;
1209 void *ovpdev;
1210 int ret;
1211
1212 dev = kzalloc(sizeof(*dev), GFP_KERNEL);
1213 if (!dev)
1214 return -ENOMEM;
1215
1216 mutex_init(&dev->input_lock);
1217
1218 dev->fmt_idx = 0;
1219 v4l2_i2c_subdev_init(&(dev->sd), client, &ov2722_ops);
1220
1221 ovpdev = gmin_camera_platform_data(&dev->sd,
1222 ATOMISP_INPUT_FORMAT_RAW_10,
1223 atomisp_bayer_order_grbg);
1224
1225 ret = ov2722_s_config(&dev->sd, client->irq, ovpdev);
1226 if (ret)
1227 goto out_free;
1228
1229 ret = __ov2722_init_ctrl_handler(dev);
1230 if (ret)
1231 goto out_ctrl_handler_free;
1232
1233 dev->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1234 dev->pad.flags = MEDIA_PAD_FL_SOURCE;
1235 dev->format.code = MEDIA_BUS_FMT_SBGGR10_1X10;
1236 dev->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
1237
1238 ret = media_entity_pads_init(&dev->sd.entity, 1, &dev->pad);
1239 if (ret)
1240 ov2722_remove(client);
1241
1242 return atomisp_register_i2c_module(&dev->sd, ovpdev, RAW_CAMERA);
1243
1244out_ctrl_handler_free:
1245 v4l2_ctrl_handler_free(&dev->ctrl_handler);
1246
1247out_free:
1248 v4l2_device_unregister_subdev(&dev->sd);
1249 kfree(dev);
1250 return ret;
1251}
1252
1253static const struct acpi_device_id ov2722_acpi_match[] = {
1254 { "INT33FB" },
1255 {},
1256};
1257MODULE_DEVICE_TABLE(acpi, ov2722_acpi_match);
1258
1259static struct i2c_driver ov2722_driver = {
1260 .driver = {
1261 .name = "ov2722",
1262 .acpi_match_table = ov2722_acpi_match,
1263 },
1264 .probe_new = ov2722_probe,
1265 .remove = ov2722_remove,
1266};
1267module_i2c_driver(ov2722_driver);
1268
1269MODULE_AUTHOR("Wei Liu <wei.liu@intel.com>");
1270MODULE_DESCRIPTION("A low-level driver for OmniVision 2722 sensors");
1271MODULE_LICENSE("GPL");
1272