1 // SPDX-License-Identifier: GPL-2.0
2 /*
3 * sc031gs driver
4 *
5 * Copyright (C) 2017 Fuzhou Rockchip Electronics Co., Ltd.
6 *
7 * V0.0X01.0X01 add poweron function.
8 * V0.0X01.0X02 fix mclk issue when probe multiple camera.
9 * V0.0X01.0X03 add enum_frame_interval function.
10 * V0.0X01.0X04 add quick stream on/off
11 */
12
13 #include <linux/clk.h>
14 #include <linux/device.h>
15 #include <linux/delay.h>
16 #include <linux/gpio/consumer.h>
17 #include <linux/i2c.h>
18 #include <linux/module.h>
19 #include <linux/pm_runtime.h>
20 #include <linux/regulator/consumer.h>
21 #include <linux/sysfs.h>
22 #include <linux/rk-camera-module.h>
23 #include <linux/slab.h>
24 #include <linux/version.h>
25 #include <media/media-entity.h>
26 #include <media/v4l2-async.h>
27 #include <media/v4l2-ctrls.h>
28 #include <media/v4l2-subdev.h>
29
30 #define DRIVER_VERSION KERNEL_VERSION(0, 0x01, 0x04)
31
32 #ifndef V4L2_CID_DIGITAL_GAIN
33 #define V4L2_CID_DIGITAL_GAIN V4L2_CID_GAIN
34 #endif
35
36 #define SC031GS_PIXEL_RATE (72 * 1000 * 1000)
37 #define SC031GS_XVCLK_FREQ 24000000
38
39 #define CHIP_ID 0x0031
40 #define SC031GS_REG_CHIP_ID 0x3107
41
42 #define SC031GS_REG_CTRL_MODE 0x0100
43 #define SC031GS_MODE_SW_STANDBY 0x0
44 #define SC031GS_MODE_STREAMING BIT(0)
45
46 #define SC031GS_REG_EXPOSURE 0x3e01
47 #define SC031GS_EXPOSURE_MIN 6
48 #define SC031GS_EXPOSURE_STEP 1
49 #define SC031GS_VTS_MAX 0xffff
50
51 #define SC031GS_REG_COARSE_AGAIN 0x3e08
52 #define SC031GS_REG_FINE_AGAIN 0x3e09
53 #define ANALOG_GAIN_MIN 0x10
54 #define ANALOG_GAIN_MAX 0x7c0 // 124x
55 #define ANALOG_GAIN_STEP 1
56 #define ANALOG_GAIN_DEFAULT 0x1f
57
58 #define SC031GS_REG_TEST_PATTERN 0x4501
59 #define SC031GS_TEST_PATTERN_ENABLE 0xcc
60 #define SC031GS_TEST_PATTERN_DISABLE 0xc4
61
62 #define SC031GS_REG_VTS 0x320e
63
64 #define REG_NULL 0xFFFF
65
66 #define SC031GS_REG_VALUE_08BIT 1
67 #define SC031GS_REG_VALUE_16BIT 2
68 #define SC031GS_REG_VALUE_24BIT 3
69 //#define DVP_INTERFACE
70
71 #ifdef DVP_INTERFACE
72 #define PIX_FORMAT MEDIA_BUS_FMT_Y8_1X8
73 #else
74 #define PIX_FORMAT MEDIA_BUS_FMT_Y10_1X10
75 #define SC031GS_LANES 1
76 #define SC031GS_BITS_PER_SAMPLE 10
77 #endif
78
79 #define SC031GS_NAME "sc031gs"
80
81 static const char * const sc031gs_supply_names[] = {
82 "avdd", /* Analog power */
83 "dovdd", /* Digital I/O power */
84 "dvdd", /* Digital core power */
85 };
86
87 #define SC031GS_NUM_SUPPLIES ARRAY_SIZE(sc031gs_supply_names)
88
89 struct regval {
90 u16 addr;
91 u8 val;
92 };
93
94 struct sc031gs_mode {
95 u32 width;
96 u32 height;
97 struct v4l2_fract max_fps;
98 u32 hts_def;
99 u32 vts_def;
100 u32 exp_def;
101 const struct regval *reg_list;
102 };
103
104 struct sc031gs {
105 struct i2c_client *client;
106 struct clk *xvclk;
107 struct gpio_desc *reset_gpio;
108 struct gpio_desc *pwdn_gpio;
109 struct regulator_bulk_data supplies[SC031GS_NUM_SUPPLIES];
110 struct v4l2_subdev subdev;
111 struct media_pad pad;
112 struct v4l2_ctrl_handler ctrl_handler;
113 struct v4l2_ctrl *exposure;
114 struct v4l2_ctrl *anal_gain;
115 struct v4l2_ctrl *digi_gain;
116 struct v4l2_ctrl *hblank;
117 struct v4l2_ctrl *vblank;
118 struct v4l2_ctrl *test_pattern;
119 struct mutex mutex;
120 struct v4l2_fract cur_fps;
121 u32 cur_vts;
122 bool streaming;
123 bool power_on;
124 const struct sc031gs_mode *cur_mode;
125 u32 module_index;
126 const char *module_facing;
127 const char *module_name;
128 const char *len_name;
129 };
130
131 #define to_sc031gs(sd) container_of(sd, struct sc031gs, subdev)
132
133 /*
134 * Xclk 24Mhz
135 * Pclk 45Mhz
136 * linelength 683(0x2ab)
137 * framelength 878(0x36e)
138 * grabwindow_width 640
139 * grabwindow_height 480
140 * max_framerate 120fps
141 * mipi_datarate per lane 720Mbps
142 */
143 static const struct regval sc031gs_global_regs[] = {
144 #ifdef DVP_INTERFACE
145 {0x0100, 0x00},
146 {0x300f, 0x0f},
147 {0x3018, 0x1f},
148 {0x3019, 0xff},
149 {0x301c, 0xb4},
150 {0x3028, 0x82},
151 {0x320c, 0x03},
152 {0x320d, 0x6e},
153 // {0x320e, 0x02}, //120fps
154 // {0x320f, 0xab},
155 {0x320e, 0x0a}, //30fps
156 {0x320f, 0xac},
157 {0x3250, 0xf0},
158 {0x3251, 0x02},
159 {0x3252, 0x02},
160 {0x3253, 0xa6},
161 {0x3254, 0x02},
162 {0x3255, 0x07},
163 {0x3304, 0x48},
164 {0x3306, 0x38},
165 {0x3309, 0x68},
166 {0x330b, 0xe0},
167 {0x330c, 0x18},
168 {0x330f, 0x20},
169 {0x3310, 0x10},
170 {0x3314, 0x3a},
171 {0x3315, 0x38},
172 {0x3316, 0x48},
173 {0x3317, 0x20},
174 {0x3329, 0x3c},
175 {0x332d, 0x3c},
176 {0x332f, 0x40},
177 {0x3335, 0x44},
178 {0x3344, 0x44},
179 {0x335b, 0x80},
180 {0x335f, 0x80},
181 {0x3366, 0x06},
182 {0x3385, 0x31},
183 {0x3387, 0x51},
184 {0x3389, 0x01},
185 {0x33b1, 0x03},
186 {0x33b2, 0x06},
187 {0x3621, 0xa4},
188 {0x3622, 0x05},
189 {0x3624, 0x47},
190 {0x3630, 0x46},
191 {0x3631, 0x48},
192 {0x3633, 0x52},
193 {0x3636, 0x25},
194 {0x3637, 0x89},
195 {0x3638, 0x0f},
196 {0x3639, 0x08},
197 {0x363a, 0x00},
198 {0x363b, 0x48},
199 {0x363c, 0x06},
200 {0x363d, 0x00},
201 {0x363e, 0xf8},
202 {0x3640, 0x02},
203 {0x3641, 0x01},
204 {0x36e9, 0x00},
205 {0x36ea, 0x3b},
206 {0x36eb, 0x1a},
207 {0x36ec, 0x0a},
208 {0x36ed, 0x33},
209 {0x36f9, 0x00},
210 {0x36fa, 0x3a},
211 {0x36fc, 0x01},
212 {0x3908, 0x91},
213 {0x3d08, 0x00},//0x01
214 {0x3e01, 0xd0},
215 {0x3e02, 0xff},
216 {0x3e06, 0x0c},
217 {0x4500, 0x59},
218 {0x4501, 0xc4},
219 {0x5011, 0x00},
220 {0x0100, 0x01},
221 {0x4418, 0x08},
222 {0x4419, 0x8e},
223 {0x0100, 0x00},
224 // test pattern
225 // {0x4501, 0xac},
226 // {0x5011, 0x01},
227 {REG_NULL, 0x00},
228 #else
229 {0x0100, 0x00},
230 {0x3000, 0x00},
231 {0x3001, 0x00},
232 {0x300f, 0x0f},
233 {0x3018, 0x13},
234 {0x3019, 0xfe},
235 {0x301c, 0x78},
236 {0x3031, 0x0a},
237 {0x3037, 0x20},
238 {0x303f, 0x01},
239 {0x320c, 0x03},
240 {0x320d, 0x6e},
241 {0x320e, 0x06},
242 {0x320f, 0x67},
243 {0x3250, 0xc0},
244 {0x3251, 0x02},
245 {0x3252, 0x02},
246 {0x3253, 0xa6},
247 {0x3254, 0x02},
248 {0x3255, 0x07},
249 {0x3304, 0x48},
250 {0x3306, 0x38},
251 {0x3309, 0x68},
252 {0x330b, 0xe0},
253 {0x330c, 0x18},
254 {0x330f, 0x20},
255 {0x3310, 0x10},
256 {0x3314, 0x3a},
257 {0x3315, 0x38},
258 {0x3316, 0x48},
259 {0x3317, 0x20},
260 {0x3329, 0x3c},
261 {0x332d, 0x3c},
262 {0x332f, 0x40},
263 {0x3335, 0x44},
264 {0x3344, 0x44},
265 {0x335b, 0x80},
266 {0x335f, 0x80},
267 {0x3366, 0x06},
268 {0x3385, 0x31},
269 {0x3387, 0x51},
270 {0x3389, 0x01},
271 {0x33b1, 0x03},
272 {0x33b2, 0x06},
273 {0x3621, 0xa4},
274 {0x3622, 0x05},
275 {0x3630, 0x46},
276 {0x3631, 0x48},
277 {0x3633, 0x52},
278 {0x3636, 0x25},
279 {0x3637, 0x89},
280 {0x3638, 0x0f},
281 {0x3639, 0x08},
282 {0x363a, 0x00},
283 {0x363b, 0x48},
284 {0x363c, 0x06},
285 {0x363d, 0x00},
286 {0x363e, 0xf8},
287 {0x3640, 0x00},
288 {0x3641, 0x01},
289 {0x36e9, 0x00},
290 {0x36ea, 0x3b},
291 {0x36eb, 0x0e},
292 {0x36ec, 0x0e},
293 {0x36ed, 0x33},
294 {0x36f9, 0x00},
295 {0x36fa, 0x3a},
296 {0x36fc, 0x01},
297 {0x3908, 0x91},
298 {0x3d08, 0x01},
299 {0x3e01, 0x14},
300 {0x3e02, 0x80},
301 {0x3e06, 0x00},
302 {0x4500, 0x59},
303 {0x4501, 0xc4},
304 {0x4603, 0x00},
305 {0x5011, 0x00},
306 {0x0100, 0x01},
307 {0x4418, 0x08},
308 {0x4419, 0x8e},
309 {0x0100, 0x00},
310 {REG_NULL, 0x00},
311 #endif
312 };
313
314 static const struct sc031gs_mode supported_modes[] = {
315 {
316 .width = 640,
317 .height = 480,
318 .max_fps = {
319 .numerator = 10000,
320 .denominator = 300000,
321 },
322 .exp_def = 0x0148,
323 .hts_def = 0x036e,
324 .vts_def = 0x0aac,
325 .reg_list = sc031gs_global_regs,
326 },
327 };
328
329 static const char * const sc031gs_test_pattern_menu[] = {
330 "Disabled",
331 "Vertical Color Bar Type 1",
332 "Vertical Color Bar Type 2",
333 "Vertical Color Bar Type 3",
334 "Vertical Color Bar Type 4"
335 };
336
337 #define SC031GS_LINK_FREQ_360MHZ (360 * 1000 * 1000)
338 static const s64 link_freq_menu_items[] = {
339 SC031GS_LINK_FREQ_360MHZ
340 };
341
342 /* Write registers up to 4 at a time */
sc031gs_write_reg(struct i2c_client * client,u16 reg,u32 len,u32 val)343 static int sc031gs_write_reg(struct i2c_client *client,
344 u16 reg, u32 len, u32 val)
345 {
346 u32 buf_i, val_i;
347 u8 buf[6];
348 u8 *val_p;
349 __be32 val_be;
350 u32 ret;
351
352 if (len > 4)
353 return -EINVAL;
354
355 buf[0] = reg >> 8;
356 buf[1] = reg & 0xff;
357
358 val_be = cpu_to_be32(val);
359 val_p = (u8 *)&val_be;
360 buf_i = 2;
361 val_i = 4 - len;
362
363 while (val_i < 4)
364 buf[buf_i++] = val_p[val_i++];
365
366 ret = i2c_master_send(client, buf, len + 2);
367 if (ret != len + 2)
368 return -EIO;
369
370 return 0;
371 }
372
sc031gs_write_array(struct i2c_client * client,const struct regval * regs)373 static int sc031gs_write_array(struct i2c_client *client,
374 const struct regval *regs)
375 {
376 u32 i;
377 int ret = 0;
378
379 for (i = 0; ret == 0 && regs[i].addr != REG_NULL; i++) {
380 ret = sc031gs_write_reg(client, regs[i].addr,
381 SC031GS_REG_VALUE_08BIT, regs[i].val);
382 if (regs[i].addr == 0x0100 && regs[i].val == 0x01)
383 msleep(10);
384 }
385
386 return ret;
387 }
388
389 /* Read registers up to 4 at a time */
sc031gs_read_reg(struct i2c_client * client,u16 reg,unsigned int len,u32 * val)390 static int sc031gs_read_reg(struct i2c_client *client,
391 u16 reg, unsigned int len, u32 *val)
392 {
393 struct i2c_msg msgs[2];
394 u8 *data_be_p;
395 __be32 data_be = 0;
396 __be16 reg_addr_be = cpu_to_be16(reg);
397 int ret;
398
399 if (len > 4 || !len)
400 return -EINVAL;
401
402 data_be_p = (u8 *)&data_be;
403 /* Write register address */
404 msgs[0].addr = client->addr;
405 msgs[0].flags = 0;
406 msgs[0].len = 2;
407 msgs[0].buf = (u8 *)®_addr_be;
408
409 /* Read data from register */
410 msgs[1].addr = client->addr;
411 msgs[1].flags = I2C_M_RD;
412 msgs[1].len = len;
413 msgs[1].buf = &data_be_p[4 - len];
414
415 ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
416 if (ret != ARRAY_SIZE(msgs))
417 return -EIO;
418
419 *val = be32_to_cpu(data_be);
420
421 return 0;
422 }
423
sc031gs_get_reso_dist(const struct sc031gs_mode * mode,struct v4l2_mbus_framefmt * framefmt)424 static int sc031gs_get_reso_dist(const struct sc031gs_mode *mode,
425 struct v4l2_mbus_framefmt *framefmt)
426 {
427 return abs(mode->width - framefmt->width) +
428 abs(mode->height - framefmt->height);
429 }
430
431 static const struct sc031gs_mode *
sc031gs_find_best_fit(struct v4l2_subdev_format * fmt)432 sc031gs_find_best_fit(struct v4l2_subdev_format *fmt)
433 {
434 struct v4l2_mbus_framefmt *framefmt = &fmt->format;
435 int dist;
436 int cur_best_fit = 0;
437 int cur_best_fit_dist = -1;
438 unsigned int i;
439
440 for (i = 0; i < ARRAY_SIZE(supported_modes); i++) {
441 dist = sc031gs_get_reso_dist(&supported_modes[i], framefmt);
442 if (cur_best_fit_dist == -1 || dist < cur_best_fit_dist) {
443 cur_best_fit_dist = dist;
444 cur_best_fit = i;
445 }
446 }
447
448 return &supported_modes[cur_best_fit];
449 }
450
sc031gs_set_fmt(struct v4l2_subdev * sd,struct v4l2_subdev_pad_config * cfg,struct v4l2_subdev_format * fmt)451 static int sc031gs_set_fmt(struct v4l2_subdev *sd,
452 struct v4l2_subdev_pad_config *cfg,
453 struct v4l2_subdev_format *fmt)
454 {
455 struct sc031gs *sc031gs = to_sc031gs(sd);
456 const struct sc031gs_mode *mode;
457 s64 h_blank, vblank_def;
458
459 mutex_lock(&sc031gs->mutex);
460
461 mode = sc031gs_find_best_fit(fmt);
462 fmt->format.code = PIX_FORMAT;
463 fmt->format.width = mode->width;
464 fmt->format.height = mode->height;
465 fmt->format.field = V4L2_FIELD_NONE;
466 if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
467 #ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
468 *v4l2_subdev_get_try_format(sd, cfg, fmt->pad) = fmt->format;
469 #else
470 mutex_unlock(&sc031gs->mutex);
471 return -ENOTTY;
472 #endif
473 } else {
474 sc031gs->cur_mode = mode;
475 h_blank = mode->hts_def - mode->width;
476 __v4l2_ctrl_modify_range(sc031gs->hblank, h_blank,
477 h_blank, 1, h_blank);
478 vblank_def = mode->vts_def - mode->height;
479 __v4l2_ctrl_modify_range(sc031gs->vblank, vblank_def,
480 SC031GS_VTS_MAX - mode->height,
481 1, vblank_def);
482 sc031gs->cur_fps = mode->max_fps;
483 sc031gs->cur_vts = mode->vts_def;
484 }
485
486 mutex_unlock(&sc031gs->mutex);
487
488 return 0;
489 }
490
sc031gs_get_fmt(struct v4l2_subdev * sd,struct v4l2_subdev_pad_config * cfg,struct v4l2_subdev_format * fmt)491 static int sc031gs_get_fmt(struct v4l2_subdev *sd,
492 struct v4l2_subdev_pad_config *cfg,
493 struct v4l2_subdev_format *fmt)
494 {
495 struct sc031gs *sc031gs = to_sc031gs(sd);
496 const struct sc031gs_mode *mode = sc031gs->cur_mode;
497
498 mutex_lock(&sc031gs->mutex);
499 if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
500 #ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
501 fmt->format = *v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
502 #else
503 mutex_unlock(&sc031gs->mutex);
504 return -ENOTTY;
505 #endif
506 } else {
507 fmt->format.width = mode->width;
508 fmt->format.height = mode->height;
509 fmt->format.code = PIX_FORMAT;
510 fmt->format.field = V4L2_FIELD_NONE;
511 }
512 mutex_unlock(&sc031gs->mutex);
513
514 return 0;
515 }
516
sc031gs_enum_mbus_code(struct v4l2_subdev * sd,struct v4l2_subdev_pad_config * cfg,struct v4l2_subdev_mbus_code_enum * code)517 static int sc031gs_enum_mbus_code(struct v4l2_subdev *sd,
518 struct v4l2_subdev_pad_config *cfg,
519 struct v4l2_subdev_mbus_code_enum *code)
520 {
521 if (code->index != 0)
522 return -EINVAL;
523 code->code = PIX_FORMAT;
524
525 return 0;
526 }
527
sc031gs_enum_frame_sizes(struct v4l2_subdev * sd,struct v4l2_subdev_pad_config * cfg,struct v4l2_subdev_frame_size_enum * fse)528 static int sc031gs_enum_frame_sizes(struct v4l2_subdev *sd,
529 struct v4l2_subdev_pad_config *cfg,
530 struct v4l2_subdev_frame_size_enum *fse)
531 {
532 if (fse->index >= ARRAY_SIZE(supported_modes))
533 return -EINVAL;
534
535 if (fse->code != PIX_FORMAT)
536 return -EINVAL;
537
538 fse->min_width = supported_modes[fse->index].width;
539 fse->max_width = supported_modes[fse->index].width;
540 fse->max_height = supported_modes[fse->index].height;
541 fse->min_height = supported_modes[fse->index].height;
542
543 return 0;
544 }
545
sc031gs_enable_test_pattern(struct sc031gs * sc031gs,u32 pattern)546 static int sc031gs_enable_test_pattern(struct sc031gs *sc031gs, u32 pattern)
547 {
548 u32 val;
549
550 if (pattern)
551 val = (pattern - 1) | SC031GS_TEST_PATTERN_ENABLE;
552 else
553 val = SC031GS_TEST_PATTERN_DISABLE;
554
555 return sc031gs_write_reg(sc031gs->client, SC031GS_REG_TEST_PATTERN,
556 SC031GS_REG_VALUE_08BIT, val);
557 }
558
sc031gs_get_module_inf(struct sc031gs * sc031gs,struct rkmodule_inf * inf)559 static void sc031gs_get_module_inf(struct sc031gs *sc031gs,
560 struct rkmodule_inf *inf)
561 {
562 memset(inf, 0, sizeof(*inf));
563 strlcpy(inf->base.sensor, SC031GS_NAME, sizeof(inf->base.sensor));
564 strlcpy(inf->base.module, sc031gs->module_name,
565 sizeof(inf->base.module));
566 strlcpy(inf->base.lens, sc031gs->len_name, sizeof(inf->base.lens));
567 }
568
sc031gs_ioctl(struct v4l2_subdev * sd,unsigned int cmd,void * arg)569 static long sc031gs_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
570 {
571 struct sc031gs *sc031gs = to_sc031gs(sd);
572 long ret = 0;
573 u32 stream = 0;
574
575 switch (cmd) {
576 case RKMODULE_GET_MODULE_INFO:
577 sc031gs_get_module_inf(sc031gs, (struct rkmodule_inf *)arg);
578 break;
579 case RKMODULE_SET_QUICK_STREAM:
580
581 stream = *((u32 *)arg);
582
583 if (stream)
584 ret = sc031gs_write_reg(sc031gs->client, SC031GS_REG_CTRL_MODE,
585 SC031GS_REG_VALUE_08BIT, SC031GS_MODE_STREAMING);
586 else
587 ret = sc031gs_write_reg(sc031gs->client, SC031GS_REG_CTRL_MODE,
588 SC031GS_REG_VALUE_08BIT, SC031GS_MODE_SW_STANDBY);
589 break;
590 default:
591 ret = -ENOIOCTLCMD;
592 break;
593 }
594
595 return ret;
596 }
597
598 #ifdef CONFIG_COMPAT
sc031gs_compat_ioctl32(struct v4l2_subdev * sd,unsigned int cmd,unsigned long arg)599 static long sc031gs_compat_ioctl32(struct v4l2_subdev *sd,
600 unsigned int cmd, unsigned long arg)
601 {
602 void __user *up = compat_ptr(arg);
603 struct rkmodule_inf *inf;
604 struct rkmodule_awb_cfg *cfg;
605 long ret;
606 u32 stream = 0;
607
608 switch (cmd) {
609 case RKMODULE_GET_MODULE_INFO:
610 inf = kzalloc(sizeof(*inf), GFP_KERNEL);
611 if (!inf) {
612 ret = -ENOMEM;
613 return ret;
614 }
615
616 ret = sc031gs_ioctl(sd, cmd, inf);
617 if (!ret)
618 if (copy_to_user(up, inf, sizeof(*inf))) {
619 kfree(inf);
620 return -EFAULT;
621 }
622 kfree(inf);
623 break;
624 case RKMODULE_AWB_CFG:
625 cfg = kzalloc(sizeof(*cfg), GFP_KERNEL);
626 if (!cfg) {
627 ret = -ENOMEM;
628 return ret;
629 }
630 if (copy_from_user(cfg, up, sizeof(*cfg))) {
631 kfree(cfg);
632 return -EFAULT;
633 }
634 ret = sc031gs_ioctl(sd, cmd, cfg);
635 kfree(cfg);
636 break;
637 case RKMODULE_SET_QUICK_STREAM:
638 if (copy_from_user(&stream, up, sizeof(u32)))
639 return -EFAULT;
640 ret = sc031gs_ioctl(sd, cmd, &stream);
641 break;
642 default:
643 ret = -ENOIOCTLCMD;
644 break;
645 }
646
647 return ret;
648 }
649 #endif
650
sc031gs_set_ctrl_gain(struct sc031gs * sc031gs,u32 a_gain)651 static int sc031gs_set_ctrl_gain(struct sc031gs *sc031gs, u32 a_gain)
652 {
653 int ret = 0;
654 u32 coarse_again, fine_again, fine_again_reg, coarse_again_reg, digital_gain_reg;
655
656 if (a_gain < 0x20) { /*1x ~ 2x*/
657 fine_again = a_gain - 16;
658 coarse_again = 0x03;
659 fine_again_reg = ((0x01 << 4) & 0x10) |
660 (fine_again & 0x0f);
661 coarse_again_reg = coarse_again & 0x1F;
662 digital_gain_reg = 0x80;
663 } else if (a_gain < 0x40) { /*2x ~ 4x*/
664 fine_again = (a_gain >> 1) - 16;
665 coarse_again = 0x7;
666 fine_again_reg = ((0x01 << 4) & 0x10) |
667 (fine_again & 0x0f);
668 coarse_again_reg = coarse_again & 0x1F;
669 digital_gain_reg = 0x80;
670 } else if (a_gain < 0x80) { /*4x ~ 8x*/
671 fine_again = (a_gain >> 2) - 16;
672 coarse_again = 0xf;
673 fine_again_reg = ((0x01 << 4) & 0x10) |
674 (fine_again & 0x0f);
675 coarse_again_reg = coarse_again & 0x1F;
676 digital_gain_reg = 0x80;
677 } else if (a_gain < 0x100) { /*8x ~ 16x*/
678 fine_again = (a_gain >> 3) - 16;
679 coarse_again = 0x1f;
680 fine_again_reg = ((0x01 << 4) & 0x10) |
681 (fine_again & 0x0f);
682 coarse_again_reg = coarse_again & 0x1F;
683 digital_gain_reg = 0x80;
684 } else if (a_gain < 0x200) { /*16x ~ 32x*/
685 fine_again_reg = 0x1f;
686 coarse_again_reg = 0x1f;
687 digital_gain_reg = (a_gain * 0x80 / 0x100) & 0xf8;
688 } else if (a_gain < 0x400) { /*32x ~ 64x*/
689 fine_again_reg = 0x1f;
690 coarse_again_reg = 0x1f;
691 digital_gain_reg = (a_gain * 0x80 / 0x200) & 0x1f8;
692 } else { /*64x ~ 124*/
693 fine_again_reg = 0x1f;
694 coarse_again_reg = 0x1f;
695 digital_gain_reg = (a_gain * 0x80 / 0x400) & 0x3f8;
696 }
697
698 if (a_gain < 0x20) {
699 ret |= sc031gs_write_reg(sc031gs->client, 0x3314,
700 SC031GS_REG_VALUE_08BIT, 0x42);
701 ret |= sc031gs_write_reg(sc031gs->client, 0x3317,
702 SC031GS_REG_VALUE_08BIT, 0x20);
703 } else {
704 ret |= sc031gs_write_reg(sc031gs->client, 0x3314,
705 SC031GS_REG_VALUE_08BIT, 0x4f);
706 ret |= sc031gs_write_reg(sc031gs->client, 0x3317,
707 SC031GS_REG_VALUE_08BIT, 0x0f);
708 }
709 ret |= sc031gs_write_reg(sc031gs->client,
710 SC031GS_REG_COARSE_AGAIN,
711 SC031GS_REG_VALUE_08BIT,
712 coarse_again_reg);
713 ret |= sc031gs_write_reg(sc031gs->client,
714 SC031GS_REG_FINE_AGAIN,
715 SC031GS_REG_VALUE_08BIT,
716 fine_again_reg);
717
718 ret |= sc031gs_write_reg(sc031gs->client, 0x3e06,
719 SC031GS_REG_VALUE_16BIT, digital_gain_reg);
720
721 return ret;
722 }
723
__sc031gs_start_stream(struct sc031gs * sc031gs)724 static int __sc031gs_start_stream(struct sc031gs *sc031gs)
725 {
726 int ret;
727
728 // ret = sc031gs_write_array(sc031gs->client, sc031gs_global_regs);
729 // if (ret)
730 // return ret;
731 ret = sc031gs_write_array(sc031gs->client, sc031gs->cur_mode->reg_list);
732 if (ret)
733 return ret;
734
735 /* In case these controls are set before streaming */
736 mutex_unlock(&sc031gs->mutex);
737 ret = v4l2_ctrl_handler_setup(&sc031gs->ctrl_handler);
738 mutex_lock(&sc031gs->mutex);
739 if (ret)
740 return ret;
741
742 return sc031gs_write_reg(sc031gs->client, SC031GS_REG_CTRL_MODE,
743 SC031GS_REG_VALUE_08BIT, SC031GS_MODE_STREAMING);
744 }
745
__sc031gs_stop_stream(struct sc031gs * sc031gs)746 static int __sc031gs_stop_stream(struct sc031gs *sc031gs)
747 {
748 return sc031gs_write_reg(sc031gs->client, SC031GS_REG_CTRL_MODE,
749 SC031GS_REG_VALUE_08BIT, SC031GS_MODE_SW_STANDBY);
750 }
751
sc031gs_s_stream(struct v4l2_subdev * sd,int on)752 static int sc031gs_s_stream(struct v4l2_subdev *sd, int on)
753 {
754 struct sc031gs *sc031gs = to_sc031gs(sd);
755 struct i2c_client *client = sc031gs->client;
756 int ret = 0;
757
758 mutex_lock(&sc031gs->mutex);
759 on = !!on;
760 if (on == sc031gs->streaming)
761 goto unlock_and_return;
762
763 if (on) {
764 ret = pm_runtime_get_sync(&client->dev);
765 if (ret < 0) {
766 pm_runtime_put_noidle(&client->dev);
767 goto unlock_and_return;
768 }
769
770 ret = __sc031gs_start_stream(sc031gs);
771 if (ret) {
772 v4l2_err(sd, "start stream failed while write regs\n");
773 pm_runtime_put(&client->dev);
774 goto unlock_and_return;
775 }
776 } else {
777 __sc031gs_stop_stream(sc031gs);
778 pm_runtime_put(&client->dev);
779 }
780
781 sc031gs->streaming = on;
782
783 unlock_and_return:
784 mutex_unlock(&sc031gs->mutex);
785
786 return ret;
787 }
788
sc031gs_g_frame_interval(struct v4l2_subdev * sd,struct v4l2_subdev_frame_interval * fi)789 static int sc031gs_g_frame_interval(struct v4l2_subdev *sd,
790 struct v4l2_subdev_frame_interval *fi)
791 {
792 struct sc031gs *sc031gs = to_sc031gs(sd);
793 const struct sc031gs_mode *mode = sc031gs->cur_mode;
794
795 if (sc031gs->streaming)
796 fi->interval = sc031gs->cur_fps;
797 else
798 fi->interval = mode->max_fps;
799
800 return 0;
801 }
802
sc031gs_s_power(struct v4l2_subdev * sd,int on)803 static int sc031gs_s_power(struct v4l2_subdev *sd, int on)
804 {
805 struct sc031gs *sc031gs = to_sc031gs(sd);
806 struct i2c_client *client = sc031gs->client;
807 int ret = 0;
808
809 mutex_lock(&sc031gs->mutex);
810
811 /* If the power state is not modified - no work to do. */
812 if (sc031gs->power_on == !!on)
813 goto unlock_and_return;
814
815 if (on) {
816 ret = pm_runtime_get_sync(&client->dev);
817 if (ret < 0) {
818 pm_runtime_put_noidle(&client->dev);
819 goto unlock_and_return;
820 }
821
822 sc031gs->power_on = true;
823 } else {
824 pm_runtime_put(&client->dev);
825 sc031gs->power_on = false;
826 }
827
828 unlock_and_return:
829 mutex_unlock(&sc031gs->mutex);
830
831 return ret;
832 }
833
834 /* Calculate the delay in us by clock rate and clock cycles */
sc031gs_cal_delay(u32 cycles)835 static inline u32 sc031gs_cal_delay(u32 cycles)
836 {
837 return DIV_ROUND_UP(cycles, SC031GS_XVCLK_FREQ / 1000 / 1000);
838 }
839
__sc031gs_power_on(struct sc031gs * sc031gs)840 static int __sc031gs_power_on(struct sc031gs *sc031gs)
841 {
842 int ret;
843 u32 delay_us;
844 struct device *dev = &sc031gs->client->dev;
845
846 ret = clk_set_rate(sc031gs->xvclk, SC031GS_XVCLK_FREQ);
847 if (ret < 0)
848 dev_warn(dev, "Failed to set xvclk rate (24MHz)\n");
849 if (clk_get_rate(sc031gs->xvclk) != SC031GS_XVCLK_FREQ)
850 dev_warn(dev, "xvclk mismatched, modes are based on 24MHz\n");
851 ret = clk_prepare_enable(sc031gs->xvclk);
852 if (ret < 0) {
853 dev_err(dev, "Failed to enable xvclk\n");
854 return ret;
855 }
856
857 ret = regulator_bulk_enable(SC031GS_NUM_SUPPLIES, sc031gs->supplies);
858 if (ret < 0) {
859 dev_err(dev, "Failed to enable regulators\n");
860 goto disable_clk;
861 }
862
863 if (!IS_ERR(sc031gs->pwdn_gpio))
864 gpiod_set_value_cansleep(sc031gs->pwdn_gpio, 1);
865
866 /* 8192 cycles prior to first SCCB transaction */
867 delay_us = sc031gs_cal_delay(8192);
868 usleep_range(delay_us, delay_us * 2);
869
870 return 0;
871
872 disable_clk:
873 clk_disable_unprepare(sc031gs->xvclk);
874
875 return ret;
876 }
877
__sc031gs_power_off(struct sc031gs * sc031gs)878 static void __sc031gs_power_off(struct sc031gs *sc031gs)
879 {
880 if (!IS_ERR(sc031gs->pwdn_gpio))
881 gpiod_set_value_cansleep(sc031gs->pwdn_gpio, 0);
882 clk_disable_unprepare(sc031gs->xvclk);
883
884 regulator_bulk_disable(SC031GS_NUM_SUPPLIES, sc031gs->supplies);
885 }
886
sc031gs_runtime_resume(struct device * dev)887 static int sc031gs_runtime_resume(struct device *dev)
888 {
889 struct i2c_client *client = to_i2c_client(dev);
890 struct v4l2_subdev *sd = i2c_get_clientdata(client);
891 struct sc031gs *sc031gs = to_sc031gs(sd);
892
893 return __sc031gs_power_on(sc031gs);
894 }
895
sc031gs_runtime_suspend(struct device * dev)896 static int sc031gs_runtime_suspend(struct device *dev)
897 {
898 struct i2c_client *client = to_i2c_client(dev);
899 struct v4l2_subdev *sd = i2c_get_clientdata(client);
900 struct sc031gs *sc031gs = to_sc031gs(sd);
901
902 __sc031gs_power_off(sc031gs);
903
904 return 0;
905 }
906
907 #ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
sc031gs_open(struct v4l2_subdev * sd,struct v4l2_subdev_fh * fh)908 static int sc031gs_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
909 {
910 struct sc031gs *sc031gs = to_sc031gs(sd);
911 struct v4l2_mbus_framefmt *try_fmt =
912 v4l2_subdev_get_try_format(sd, fh->pad, 0);
913 const struct sc031gs_mode *def_mode = &supported_modes[0];
914
915 mutex_lock(&sc031gs->mutex);
916 /* Initialize try_fmt */
917 try_fmt->width = def_mode->width;
918 try_fmt->height = def_mode->height;
919 try_fmt->code = PIX_FORMAT;
920 try_fmt->field = V4L2_FIELD_NONE;
921
922 mutex_unlock(&sc031gs->mutex);
923 /* No crop or compose */
924
925 return 0;
926 }
927 #endif
928
929 #ifdef DVP_INTERFACE
sc031gs_g_mbus_config(struct v4l2_subdev * sd,struct v4l2_mbus_config * config)930 static int sc031gs_g_mbus_config(struct v4l2_subdev *sd,
931 struct v4l2_mbus_config *config)
932 {
933 config->type = V4L2_MBUS_PARALLEL;
934 config->flags = V4L2_MBUS_HSYNC_ACTIVE_HIGH |
935 V4L2_MBUS_VSYNC_ACTIVE_LOW |
936 V4L2_MBUS_PCLK_SAMPLE_FALLING;
937 return 0;
938 }
939 #endif
940
sc031gs_enum_frame_interval(struct v4l2_subdev * sd,struct v4l2_subdev_pad_config * cfg,struct v4l2_subdev_frame_interval_enum * fie)941 static int sc031gs_enum_frame_interval(struct v4l2_subdev *sd,
942 struct v4l2_subdev_pad_config *cfg,
943 struct v4l2_subdev_frame_interval_enum *fie)
944 {
945 if (fie->index >= ARRAY_SIZE(supported_modes))
946 return -EINVAL;
947
948 fie->code = PIX_FORMAT;
949
950 fie->width = supported_modes[fie->index].width;
951 fie->height = supported_modes[fie->index].height;
952 fie->interval = supported_modes[fie->index].max_fps;
953 return 0;
954 }
955
956 static const struct dev_pm_ops sc031gs_pm_ops = {
957 SET_RUNTIME_PM_OPS(sc031gs_runtime_suspend,
958 sc031gs_runtime_resume, NULL)
959 };
960
961 #ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
962 static const struct v4l2_subdev_internal_ops sc031gs_internal_ops = {
963 .open = sc031gs_open,
964 };
965 #endif
966
967 static const struct v4l2_subdev_core_ops sc031gs_core_ops = {
968 .s_power = sc031gs_s_power,
969 .ioctl = sc031gs_ioctl,
970 #ifdef CONFIG_COMPAT
971 .compat_ioctl32 = sc031gs_compat_ioctl32,
972 #endif
973 };
974
975 static const struct v4l2_subdev_video_ops sc031gs_video_ops = {
976 .s_stream = sc031gs_s_stream,
977 .g_frame_interval = sc031gs_g_frame_interval,
978 #ifdef DVP_INTERFACE
979 .g_mbus_config = sc031gs_g_mbus_config,
980 #endif
981 };
982
983 static const struct v4l2_subdev_pad_ops sc031gs_pad_ops = {
984 .enum_mbus_code = sc031gs_enum_mbus_code,
985 .enum_frame_size = sc031gs_enum_frame_sizes,
986 .enum_frame_interval = sc031gs_enum_frame_interval,
987 .get_fmt = sc031gs_get_fmt,
988 .set_fmt = sc031gs_set_fmt,
989 };
990
991 static const struct v4l2_subdev_ops sc031gs_subdev_ops = {
992 .core = &sc031gs_core_ops,
993 .video = &sc031gs_video_ops,
994 .pad = &sc031gs_pad_ops,
995 };
996
sc031gs_modify_fps_info(struct sc031gs * sc031gs)997 static void sc031gs_modify_fps_info(struct sc031gs *sc031gs)
998 {
999 const struct sc031gs_mode *mode = sc031gs->cur_mode;
1000
1001 sc031gs->cur_fps.denominator = mode->max_fps.denominator * mode->vts_def /
1002 sc031gs->cur_vts;
1003 }
1004
sc031gs_set_ctrl(struct v4l2_ctrl * ctrl)1005 static int sc031gs_set_ctrl(struct v4l2_ctrl *ctrl)
1006 {
1007 struct sc031gs *sc031gs = container_of(ctrl->handler,
1008 struct sc031gs, ctrl_handler);
1009 struct i2c_client *client = sc031gs->client;
1010 s64 max;
1011 int ret = 0;
1012
1013 /* Propagate change of current control to all related controls */
1014 switch (ctrl->id) {
1015 case V4L2_CID_VBLANK:
1016 /* Update max exposure while meeting expected vblanking */
1017 max = sc031gs->cur_mode->height + ctrl->val - 4;
1018 __v4l2_ctrl_modify_range(sc031gs->exposure,
1019 sc031gs->exposure->minimum, max,
1020 sc031gs->exposure->step,
1021 sc031gs->exposure->default_value);
1022 break;
1023 }
1024
1025 if (!pm_runtime_get_if_in_use(&client->dev))
1026 return 0;
1027
1028 switch (ctrl->id) {
1029 case V4L2_CID_EXPOSURE:
1030 /* 4 least significant bits of expsoure are fractional part */
1031 ret = sc031gs_write_reg(sc031gs->client, SC031GS_REG_EXPOSURE,
1032 SC031GS_REG_VALUE_16BIT, ctrl->val << 4);
1033 break;
1034 case V4L2_CID_ANALOGUE_GAIN:
1035 ret = sc031gs_set_ctrl_gain(sc031gs, ctrl->val);
1036 break;
1037 case V4L2_CID_VBLANK:
1038 ret = sc031gs_write_reg(sc031gs->client, SC031GS_REG_VTS,
1039 SC031GS_REG_VALUE_16BIT,
1040 ctrl->val + sc031gs->cur_mode->height);
1041 if (!ret)
1042 sc031gs->cur_vts = ctrl->val + sc031gs->cur_mode->height;
1043 sc031gs_modify_fps_info(sc031gs);
1044 break;
1045 case V4L2_CID_TEST_PATTERN:
1046 ret = sc031gs_enable_test_pattern(sc031gs, ctrl->val);
1047 break;
1048 default:
1049 dev_warn(&client->dev, "%s Unhandled id:0x%x, val:0x%x\n",
1050 __func__, ctrl->id, ctrl->val);
1051 break;
1052 }
1053
1054 pm_runtime_put(&client->dev);
1055
1056 return ret;
1057 }
1058
1059 static const struct v4l2_ctrl_ops sc031gs_ctrl_ops = {
1060 .s_ctrl = sc031gs_set_ctrl,
1061 };
1062
sc031gs_initialize_controls(struct sc031gs * sc031gs)1063 static int sc031gs_initialize_controls(struct sc031gs *sc031gs)
1064 {
1065 const struct sc031gs_mode *mode;
1066 struct v4l2_ctrl_handler *handler;
1067 struct v4l2_ctrl *ctrl;
1068 s64 exposure_max, vblank_def;
1069 u32 h_blank;
1070 int ret;
1071
1072 handler = &sc031gs->ctrl_handler;
1073 mode = sc031gs->cur_mode;
1074 ret = v4l2_ctrl_handler_init(handler, 8);
1075 if (ret)
1076 return ret;
1077 handler->lock = &sc031gs->mutex;
1078
1079 ctrl = v4l2_ctrl_new_int_menu(handler, NULL, V4L2_CID_LINK_FREQ,
1080 0, 0, link_freq_menu_items);
1081 if (ctrl)
1082 ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
1083
1084 v4l2_ctrl_new_std(handler, NULL, V4L2_CID_PIXEL_RATE,
1085 0, SC031GS_PIXEL_RATE, 1, SC031GS_PIXEL_RATE);
1086
1087 h_blank = mode->hts_def - mode->width;
1088 sc031gs->hblank = v4l2_ctrl_new_std(handler, NULL, V4L2_CID_HBLANK,
1089 h_blank, h_blank, 1, h_blank);
1090 if (sc031gs->hblank)
1091 sc031gs->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
1092
1093 vblank_def = mode->vts_def - mode->height;
1094 sc031gs->cur_vts = mode->vts_def;
1095 sc031gs->vblank = v4l2_ctrl_new_std(handler, &sc031gs_ctrl_ops,
1096 V4L2_CID_VBLANK, vblank_def,
1097 SC031GS_VTS_MAX - mode->height,
1098 1, vblank_def);
1099
1100 exposure_max = mode->vts_def - 6;
1101 sc031gs->exposure = v4l2_ctrl_new_std(handler, &sc031gs_ctrl_ops,
1102 V4L2_CID_EXPOSURE, SC031GS_EXPOSURE_MIN,
1103 exposure_max, SC031GS_EXPOSURE_STEP,
1104 mode->exp_def);
1105
1106 sc031gs->anal_gain = v4l2_ctrl_new_std(handler, &sc031gs_ctrl_ops,
1107 V4L2_CID_ANALOGUE_GAIN, ANALOG_GAIN_MIN,
1108 ANALOG_GAIN_MAX, ANALOG_GAIN_STEP,
1109 ANALOG_GAIN_DEFAULT);
1110
1111 sc031gs->test_pattern = v4l2_ctrl_new_std_menu_items(handler,
1112 &sc031gs_ctrl_ops, V4L2_CID_TEST_PATTERN,
1113 ARRAY_SIZE(sc031gs_test_pattern_menu) - 1,
1114 0, 0, sc031gs_test_pattern_menu);
1115
1116 if (handler->error) {
1117 ret = handler->error;
1118 dev_err(&sc031gs->client->dev,
1119 "Failed to init controls(%d)\n", ret);
1120 goto err_free_handler;
1121 }
1122
1123 sc031gs->subdev.ctrl_handler = handler;
1124 sc031gs->cur_fps = mode->max_fps;
1125
1126 return 0;
1127
1128 err_free_handler:
1129 v4l2_ctrl_handler_free(handler);
1130
1131 return ret;
1132 }
1133
sc031gs_check_sensor_id(struct sc031gs * sc031gs,struct i2c_client * client)1134 static int sc031gs_check_sensor_id(struct sc031gs *sc031gs,
1135 struct i2c_client *client)
1136 {
1137 struct device *dev = &sc031gs->client->dev;
1138 u32 id = 0;
1139 int ret;
1140
1141 ret = sc031gs_read_reg(client, SC031GS_REG_CHIP_ID,
1142 SC031GS_REG_VALUE_16BIT, &id);
1143 if (id != CHIP_ID) {
1144 dev_err(dev, "Unexpected sensor id(%04x), ret(%d)\n", id, ret);
1145 return -ENODEV;
1146 }
1147
1148 dev_info(dev, "Detected SC031GS CHIP ID = 0x%04x sensor\n", CHIP_ID);
1149
1150 return 0;
1151 }
1152
sc031gs_configure_regulators(struct sc031gs * sc031gs)1153 static int sc031gs_configure_regulators(struct sc031gs *sc031gs)
1154 {
1155 unsigned int i;
1156
1157 for (i = 0; i < SC031GS_NUM_SUPPLIES; i++)
1158 sc031gs->supplies[i].supply = sc031gs_supply_names[i];
1159
1160 return devm_regulator_bulk_get(&sc031gs->client->dev,
1161 SC031GS_NUM_SUPPLIES,
1162 sc031gs->supplies);
1163 }
1164
sc031gs_probe(struct i2c_client * client,const struct i2c_device_id * id)1165 static int sc031gs_probe(struct i2c_client *client,
1166 const struct i2c_device_id *id)
1167 {
1168 struct device *dev = &client->dev;
1169 struct device_node *node = dev->of_node;
1170 struct sc031gs *sc031gs;
1171 struct v4l2_subdev *sd;
1172 char facing[2];
1173 int ret;
1174
1175 dev_info(dev, "driver version: %02x.%02x.%02x",
1176 DRIVER_VERSION >> 16,
1177 (DRIVER_VERSION & 0xff00) >> 8,
1178 DRIVER_VERSION & 0x00ff);
1179
1180 sc031gs = devm_kzalloc(dev, sizeof(*sc031gs), GFP_KERNEL);
1181 if (!sc031gs)
1182 return -ENOMEM;
1183
1184 ret = of_property_read_u32(node, RKMODULE_CAMERA_MODULE_INDEX,
1185 &sc031gs->module_index);
1186 ret |= of_property_read_string(node, RKMODULE_CAMERA_MODULE_FACING,
1187 &sc031gs->module_facing);
1188 ret |= of_property_read_string(node, RKMODULE_CAMERA_MODULE_NAME,
1189 &sc031gs->module_name);
1190 ret |= of_property_read_string(node, RKMODULE_CAMERA_LENS_NAME,
1191 &sc031gs->len_name);
1192 if (ret) {
1193 dev_err(dev, "could not get module information!\n");
1194 return -EINVAL;
1195 }
1196
1197 sc031gs->client = client;
1198 sc031gs->cur_mode = &supported_modes[0];
1199
1200 sc031gs->xvclk = devm_clk_get(dev, "xvclk");
1201 if (IS_ERR(sc031gs->xvclk)) {
1202 dev_err(dev, "Failed to get xvclk\n");
1203 return -EINVAL;
1204 }
1205
1206 sc031gs->reset_gpio = devm_gpiod_get(dev, "reset", GPIOD_OUT_LOW);
1207 if (IS_ERR(sc031gs->reset_gpio))
1208 dev_warn(dev, "Failed to get reset-gpios\n");
1209
1210 sc031gs->pwdn_gpio = devm_gpiod_get(dev, "pwdn", GPIOD_OUT_LOW);
1211 if (IS_ERR(sc031gs->pwdn_gpio))
1212 dev_warn(dev, "Failed to get pwdn-gpios\n");
1213 ret = sc031gs_configure_regulators(sc031gs);
1214 if (ret) {
1215 dev_err(dev, "Failed to get power regulators\n");
1216 return ret;
1217 }
1218
1219 mutex_init(&sc031gs->mutex);
1220
1221 sd = &sc031gs->subdev;
1222 v4l2_i2c_subdev_init(sd, client, &sc031gs_subdev_ops);
1223 ret = sc031gs_initialize_controls(sc031gs);
1224 if (ret)
1225 goto err_destroy_mutex;
1226
1227 ret = __sc031gs_power_on(sc031gs);
1228 if (ret)
1229 goto err_free_handler;
1230
1231 ret = sc031gs_check_sensor_id(sc031gs, client);
1232 if (ret)
1233 goto err_power_off;
1234
1235 #ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
1236 sd->internal_ops = &sc031gs_internal_ops;
1237 sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
1238 V4L2_SUBDEV_FL_HAS_EVENTS;
1239 #endif
1240 #if defined(CONFIG_MEDIA_CONTROLLER)
1241 sc031gs->pad.flags = MEDIA_PAD_FL_SOURCE;
1242 sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
1243 ret = media_entity_pads_init(&sd->entity, 1, &sc031gs->pad);
1244 if (ret < 0)
1245 goto err_power_off;
1246 #endif
1247
1248 memset(facing, 0, sizeof(facing));
1249 if (strcmp(sc031gs->module_facing, "back") == 0)
1250 facing[0] = 'b';
1251 else
1252 facing[0] = 'f';
1253
1254 snprintf(sd->name, sizeof(sd->name), "m%02d_%s_%s %s",
1255 sc031gs->module_index, facing,
1256 SC031GS_NAME, dev_name(sd->dev));
1257 ret = v4l2_async_register_subdev_sensor_common(sd);
1258 if (ret) {
1259 dev_err(dev, "v4l2 async register subdev failed\n");
1260 goto err_clean_entity;
1261 }
1262
1263 pm_runtime_set_active(dev);
1264 pm_runtime_enable(dev);
1265 pm_runtime_idle(dev);
1266
1267 return 0;
1268
1269 err_clean_entity:
1270 #if defined(CONFIG_MEDIA_CONTROLLER)
1271 media_entity_cleanup(&sd->entity);
1272 #endif
1273 err_power_off:
1274 __sc031gs_power_off(sc031gs);
1275 err_free_handler:
1276 v4l2_ctrl_handler_free(&sc031gs->ctrl_handler);
1277 err_destroy_mutex:
1278 mutex_destroy(&sc031gs->mutex);
1279
1280 return ret;
1281 }
1282
sc031gs_remove(struct i2c_client * client)1283 static int sc031gs_remove(struct i2c_client *client)
1284 {
1285 struct v4l2_subdev *sd = i2c_get_clientdata(client);
1286 struct sc031gs *sc031gs = to_sc031gs(sd);
1287
1288 v4l2_async_unregister_subdev(sd);
1289 #if defined(CONFIG_MEDIA_CONTROLLER)
1290 media_entity_cleanup(&sd->entity);
1291 #endif
1292 v4l2_ctrl_handler_free(&sc031gs->ctrl_handler);
1293 mutex_destroy(&sc031gs->mutex);
1294
1295 pm_runtime_disable(&client->dev);
1296 if (!pm_runtime_status_suspended(&client->dev))
1297 __sc031gs_power_off(sc031gs);
1298 pm_runtime_set_suspended(&client->dev);
1299
1300 return 0;
1301 }
1302
1303 #if IS_ENABLED(CONFIG_OF)
1304 static const struct of_device_id sc031gs_of_match[] = {
1305 { .compatible = "smartsens,sc031gs" },
1306 {},
1307 };
1308 MODULE_DEVICE_TABLE(of, sc031gs_of_match);
1309 #endif
1310
1311 static const struct i2c_device_id sc031gs_match_id[] = {
1312 { "smartsens,sc031gs", 0 },
1313 { },
1314 };
1315
1316 static struct i2c_driver sc031gs_i2c_driver = {
1317 .driver = {
1318 .name = SC031GS_NAME,
1319 .pm = &sc031gs_pm_ops,
1320 .of_match_table = of_match_ptr(sc031gs_of_match),
1321 },
1322 .probe = &sc031gs_probe,
1323 .remove = &sc031gs_remove,
1324 .id_table = sc031gs_match_id,
1325 };
1326
sensor_mod_init(void)1327 static int __init sensor_mod_init(void)
1328 {
1329 return i2c_add_driver(&sc031gs_i2c_driver);
1330 }
1331
sensor_mod_exit(void)1332 static void __exit sensor_mod_exit(void)
1333 {
1334 i2c_del_driver(&sc031gs_i2c_driver);
1335 }
1336
1337 device_initcall_sync(sensor_mod_init);
1338 module_exit(sensor_mod_exit);
1339
1340 MODULE_DESCRIPTION("Smartsens sc031gs sensor driver");
1341 MODULE_AUTHOR("zack.zeng");
1342 MODULE_LICENSE("GPL v2");
1343