forked from ~ljy/RK356X_SDK_RELEASE
blame | history | patch | commit | commitdiff
hc
2024-01-03 2f7c68cb55ecb7331f2381deb497c27155f32faf 
 kernel/drivers/media/i2c/dw9714.c
....@@ -2,7 +2,7 @@
22 /*
33  * dw9714 vcm driver
44  *
5
- * Copyright (C) 2019 Fuzhou Rockchip Electronics Co., Ltd.
5
+ * Copyright (C) 2022 Fuzhou Rockchip Electronics Co., Ltd.
66  */
77 
88 #include <linux/delay.h>
....@@ -14,12 +14,15 @@
1414 #include <media/v4l2-ctrls.h>
1515 #include <media/v4l2-device.h>
1616 #include <linux/rk_vcm_head.h>
17
+#include <linux/compat.h>
18
+#include <linux/regulator/consumer.h>
1719 
1820 #define DRIVER_VERSION			KERNEL_VERSION(0, 0x01, 0x0)
1921 #define DW9714_NAME			"dw9714"
2022 
21
-#define DW9714_MAX_CURRENT		100U
23
+#define DW9714_MAX_CURRENT		120U
2224 #define DW9714_MAX_REG			1023U
25
+#define DW9714_GRADUAL_MOVELENS_STEPS	32
2326 
2427 #define DW9714_DEFAULT_START_CURRENT	0
2528 #define DW9714_DEFAULT_RATED_CURRENT	100
....@@ -29,32 +32,73 @@
2932 #define DW9714_DEFAULT_T_SRC		0x0
3033 #define REG_NULL			0xFF
3134 
35
+/* dw9714p advanced mode */
36
+#define DW9714_ADVMODE_IC_INFO		0x00
37
+#define DW9714_ADVMODE_IC_VER		0x01
38
+#define DW9714_ADVMODE_CONTROL		0x02
39
+#define DW9714_ADVMODE_VCM_MSB		0x03
40
+#define DW9714_ADVMODE_VCM_LSB		0x04
41
+#define DW9714_ADVMODE_STATUS		0x05
42
+#define DW9714_ADVMODE_SAC_CFG		0x06
43
+#define DW9714_ADVMODE_PRESC		0x07
44
+#define DW9714_ADVMODE_SAC_TIME		0x08
45
+#define DW9714_ADVMODE_PRESET		0x09
46
+#define DW9714_ADVMODE_NRC		0x0A
47
+#define DW9714_ADVMODE_RING_EN		1
48
+
49
+#define DW9714_DEFAULT_ADVMODE		0x00
50
+#define DW9714_DEFAULT_SAC_MODE		0x04
51
+#define DW9714_DEFAULT_SAC_TIME		0x0A
52
+#define DW9714_DEFAULT_SAC_PRESCL	0x02
53
+#define DW9714_DEFAULT_NRC_EN		0x00
54
+#define DW9714_DEFAULT_NRC_MODE		0x00
55
+#define DW9714_DEFAULT_NRC_PRESET	0x00
56
+#define DW9714_DEFAULT_NRC_INFL		0x00
57
+#define DW9714_DEFAULT_NRC_TIME		0x00
58
+
3259 /* dw9714 device structure */
3360 struct dw9714_device {
3461 	struct v4l2_ctrl_handler ctrls_vcm;
62
+	struct v4l2_ctrl *focus;
3563 	struct v4l2_subdev sd;
3664 	struct v4l2_device vdev;
3765 	u16 current_val;
3866 
3967 	unsigned short current_related_pos;
4068 	unsigned short current_lens_pos;
69
+	unsigned int max_current;
4170 	unsigned int start_current;
4271 	unsigned int rated_current;
43
-	unsigned int step;
4472 	unsigned int step_mode;
4573 	unsigned int vcm_movefull_t;
4674 	unsigned int dlc_enable;
4775 	unsigned int t_src;
4876 	unsigned int mclk;
77
+	unsigned int max_logicalpos;
4978 
50
-	struct timeval start_move_tv;
51
-	struct timeval end_move_tv;
79
+	/* advanced mode*/
80
+	unsigned char adcanced_mode;
81
+	unsigned char sac_mode;
82
+	unsigned char sac_time;
83
+	unsigned char sac_prescl;
84
+	unsigned char nrc_en;
85
+	unsigned char nrc_mode;
86
+	unsigned char nrc_preset;
87
+	unsigned char nrc_infl;
88
+	unsigned char nrc_time;
89
+
90
+	struct __kernel_old_timeval start_move_tv;
91
+	struct __kernel_old_timeval end_move_tv;
5292 	unsigned long move_ms;
5393 
5494 	u32 module_index;
5595 	const char *module_facing;
5696 	struct rk_cam_vcm_cfg vcm_cfg;
57
-	int max_ma;
97
+
98
+	struct gpio_desc *xsd_gpio;
99
+	struct regulator *supply;
100
+	struct i2c_client *client;
101
+	bool power_on;
58102 };
59103 
60104 struct TimeTabel_s {
....@@ -183,7 +227,7 @@
183227 		msleep(20);
184228 	}
185229 	dev_err(&client->dev,
186
-		"%s: i2c write to failed with error %d\n", __func__, ret);
230
+		"%s: i2c read to failed with error %d\n", __func__, ret);
187231 	return ret;
188232 }
189233 
....@@ -221,12 +265,90 @@
221265 
222266 		dev_info(&client->dev,
223267 			"retrying I2C... %d\n", retries);
224
-		retries++;
225268 		msleep(20);
226269 	}
227270 	dev_err(&client->dev,
228271 		"i2c write to failed with error %d\n", ret);
229272 	return ret;
273
+}
274
+
275
+/* Write registers up to 4 at a time */
276
+static int dw9714_write_reg(struct i2c_client *client, u8 reg, u32 len, u32 val)
277
+{
278
+	u32 buf_i, val_i, retries;
279
+	u8 buf[5];
280
+	u8 *val_p;
281
+	__be32 val_be;
282
+
283
+	if (len > 4)
284
+		return -EINVAL;
285
+
286
+	buf[0] = reg;
287
+
288
+	val_be = cpu_to_be32(val);
289
+	val_p = (u8 *)&val_be;
290
+	buf_i = 1;
291
+	val_i = 4 - len;
292
+
293
+	while (val_i < 4)
294
+		buf[buf_i++] = val_p[val_i++];
295
+
296
+	for (retries = 0; retries < 5; retries++) {
297
+		if (i2c_master_send(client, buf, len + 1) == len + 1) {
298
+			dev_dbg(&client->dev,
299
+				"%s: vcm i2c ok, reg 0x%x, val 0x%x, len 0x%x\n",
300
+				__func__, reg, val, len);
301
+			return 0;
302
+		}
303
+
304
+		dev_info(&client->dev,
305
+			"retrying I2C... %d\n", retries);
306
+		msleep(20);
307
+	}
308
+
309
+	dev_err(&client->dev, "Failed to write 0x%04x,0x%x\n", reg, val);
310
+	return -EIO;
311
+}
312
+
313
+/* Read registers up to 4 at a time */
314
+static int dw9714_read_reg(struct i2c_client *client, u8 reg, u32 len, u32 *val)
315
+{
316
+	struct i2c_msg msgs[2];
317
+	__be32 data_be = 0;
318
+	u8 *data_be_p;
319
+	u32 retries;
320
+	int ret;
321
+
322
+	if (len > 4 || !len)
323
+		return -EINVAL;
324
+
325
+	data_be_p = (u8 *)&data_be;
326
+	/* Write register address */
327
+	msgs[0].addr = client->addr;
328
+	msgs[0].flags = 0;
329
+	msgs[0].len = 1;
330
+	msgs[0].buf = (u8 *)&reg;
331
+
332
+	/* Read data from register */
333
+	msgs[1].addr = client->addr;
334
+	msgs[1].flags = I2C_M_RD;
335
+	msgs[1].len = len;
336
+	msgs[1].buf = &data_be_p[4 - len];
337
+
338
+	for (retries = 0; retries < 5; retries++) {
339
+		ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
340
+		if (ret == ARRAY_SIZE(msgs)) {
341
+			*val = be32_to_cpu(data_be);
342
+			dev_dbg(&client->dev,
343
+				"%s: vcm i2c ok, reg 0x%x, val 0x%x\n",
344
+				__func__, reg, *val);
345
+			return 0;
346
+		}
347
+	}
348
+
349
+	dev_err(&client->dev,
350
+		"%s: i2c read to failed with error %d\n", __func__, ret);
351
+	return -EIO;
230352 }
231353 
232354 static unsigned int dw9714_move_time(struct dw9714_device *dev_vcm,
....@@ -238,10 +360,16 @@
238360 	unsigned int step_period_dlc = 0;
239361 	unsigned int codes_per_step = 1;
240362 	unsigned int step_case;
363
+	unsigned int sac_prescl;
241364 	int table_cnt = 0;
242365 	int i = 0;
243366 
244
-	if (dev_vcm->dlc_enable) {
367
+	if (dev_vcm->adcanced_mode) {
368
+		// sac setting time = tvib = (3.81ms+(SACT[6:0]*0.03ms)) * PRESC[1:0]))
369
+		sac_prescl = 1 << dev_vcm->sac_prescl;
370
+		move_time_ms = (((381 + 3 * dev_vcm->sac_time)) * sac_prescl + 99) / 100;
371
+		return move_time_ms;
372
+	} else if (dev_vcm->dlc_enable) {
245373 		step_case = dev_vcm->mclk & 0x3;
246374 		table_cnt = sizeof(dw9714_dlc_time_table) /
247375 					sizeof(struct TimeTabel_s);
....@@ -301,8 +429,7 @@
301429 	return move_time_ms;
302430 }
303431 
304
-static int dw9714_get_pos(struct dw9714_device *dev_vcm,
305
-	unsigned int *cur_pos)
432
+static int dw9714_get_dac(struct dw9714_device *dev_vcm, unsigned int *cur_dac)
306433 {
307434 	struct i2c_client *client = v4l2_get_subdevdata(&dev_vcm->sd);
308435 	int ret;
....@@ -310,22 +437,21 @@
310437 	unsigned char msb = 0;
311438 	unsigned int abs_step;
312439 
313
-	ret = dw9714_read_msg(client, &msb, &lsb);
314
-	if (ret != 0)
315
-		goto err;
440
+	if (dev_vcm->adcanced_mode) {
441
+		ret = dw9714_read_reg(client, DW9714_ADVMODE_VCM_MSB, 2, &abs_step);
442
+		if (ret != 0)
443
+			goto err;
444
+	} else {
445
+		ret = dw9714_read_msg(client, &msb, &lsb);
446
+		if (ret != 0)
447
+			goto err;
316448 
317
-	abs_step = (((unsigned int)(msb & 0x3FU)) << 4U) |
318
-		   (((unsigned int)lsb) >> 4U);
319
-	if (abs_step <= dev_vcm->start_current)
320
-		abs_step = VCMDRV_MAX_LOG;
321
-	else if ((abs_step > dev_vcm->start_current) &&
322
-		 (abs_step <= dev_vcm->rated_current))
323
-		abs_step = (dev_vcm->rated_current - abs_step) / dev_vcm->step;
324
-	else
325
-		abs_step = 0;
449
+		abs_step = (((unsigned int)(msb & 0x3FU)) << 4U) |
450
+			   (((unsigned int)lsb) >> 4U);
451
+	}
326452 
327
-	*cur_pos = abs_step;
328
-	dev_dbg(&client->dev, "%s: get position %d\n", __func__, *cur_pos);
453
+	*cur_dac = abs_step;
454
+	dev_dbg(&client->dev, "%s: get dac %d\n", __func__, *cur_dac);
329455 	return 0;
330456 
331457 err:
....@@ -334,37 +460,113 @@
334460 	return ret;
335461 }
336462 
463
+static int dw9714_set_dac(struct dw9714_device *dev_vcm,
464
+	unsigned int dest_dac)
465
+{
466
+	struct i2c_client *client = v4l2_get_subdevdata(&dev_vcm->sd);
467
+	int ret;
468
+
469
+	if (dev_vcm->adcanced_mode) {
470
+		unsigned int i;
471
+		bool vcm_idle = false;
472
+
473
+		/* wait for I2C bus idle */
474
+		vcm_idle = false;
475
+		for (i = 0; i < 10; i++) {
476
+			unsigned int status = 0;
477
+
478
+			dw9714_read_reg(client, DW9714_ADVMODE_STATUS, 1, &status);
479
+			status &= 0x01;
480
+			if (status == 0) {
481
+				vcm_idle = true;
482
+				break;
483
+			}
484
+			usleep_range(1000, 1200);
485
+		}
486
+
487
+		if (!vcm_idle) {
488
+			dev_err(&client->dev,
489
+				"%s: watting 0x05 flag timeout!\n", __func__);
490
+			return -ETIMEDOUT;
491
+		}
492
+
493
+		/* vcm move */
494
+		ret = dw9714_write_reg(client, DW9714_ADVMODE_VCM_MSB,
495
+					2, dest_dac);
496
+		if (ret != 0)
497
+			goto err;
498
+	} else {
499
+		unsigned char msb, lsb;
500
+
501
+		msb = (0x00U | ((dest_dac & 0x3F0U) >> 4U));
502
+		lsb = (((dest_dac & 0x0FU) << 4U) | dev_vcm->step_mode);
503
+		ret = dw9714_write_msg(client, msb, lsb);
504
+		if (ret != 0)
505
+			goto err;
506
+	}
507
+
508
+	return ret;
509
+err:
510
+	dev_err(&client->dev,
511
+		"%s: failed with error %d\n", __func__, ret);
512
+	return ret;
513
+}
514
+
515
+static int dw9714_get_pos(struct dw9714_device *dev_vcm,
516
+	unsigned int *cur_pos)
517
+{
518
+	struct i2c_client *client = v4l2_get_subdevdata(&dev_vcm->sd);
519
+	unsigned int dac, position, range;
520
+	int ret;
521
+
522
+	range = dev_vcm->rated_current - dev_vcm->start_current;
523
+	ret = dw9714_get_dac(dev_vcm, &dac);
524
+	if (!ret) {
525
+		if (dac <= dev_vcm->start_current) {
526
+			position = dev_vcm->max_logicalpos;
527
+		} else if ((dac > dev_vcm->start_current) &&
528
+			 (dac <= dev_vcm->rated_current)) {
529
+			position = (dac - dev_vcm->start_current) * dev_vcm->max_logicalpos / range;
530
+			position = dev_vcm->max_logicalpos - position;
531
+		} else {
532
+			position = 0;
533
+		}
534
+
535
+		*cur_pos = position;
536
+
537
+		dev_dbg(&client->dev, "%s: get position %d, dac %d\n", __func__, *cur_pos, dac);
538
+		return 0;
539
+	}
540
+
541
+	dev_err(&client->dev,
542
+		"%s: failed with error %d\n", __func__, ret);
543
+	return ret;
544
+}
545
+
337546 static int dw9714_set_pos(struct dw9714_device *dev_vcm,
338547 	unsigned int dest_pos)
339548 {
340
-	int ret;
341
-	unsigned char lsb = 0;
342
-	unsigned char msb = 0;
343
-	unsigned int position = 0;
344549 	struct i2c_client *client = v4l2_get_subdevdata(&dev_vcm->sd);
550
+	unsigned int position;
551
+	unsigned int range;
552
+	int ret;
345553 
346
-	if (dest_pos >= VCMDRV_MAX_LOG)
554
+	range = dev_vcm->rated_current - dev_vcm->start_current;
555
+	if (dest_pos >= dev_vcm->max_logicalpos)
347556 		position = dev_vcm->start_current;
348557 	else
349558 		position = dev_vcm->start_current +
350
-			   (dev_vcm->step * (VCMDRV_MAX_LOG - dest_pos));
559
+			   (range * (dev_vcm->max_logicalpos - dest_pos) / dev_vcm->max_logicalpos);
351560 
352561 	if (position > DW9714_MAX_REG)
353562 		position = DW9714_MAX_REG;
354563 
355564 	dev_vcm->current_lens_pos = position;
356565 	dev_vcm->current_related_pos = dest_pos;
357
-	msb = (0x00U | ((dev_vcm->current_lens_pos & 0x3F0U) >> 4U));
358
-	lsb = (((dev_vcm->current_lens_pos & 0x0FU) << 4U) |
359
-		dev_vcm->step_mode);
360
-	ret = dw9714_write_msg(client, msb, lsb);
361
-	if (ret != 0)
362
-		goto err;
363566 
364
-	return ret;
365
-err:
366
-	dev_err(&client->dev,
367
-		"%s: failed with error %d\n", __func__, ret);
567
+	ret = dw9714_set_dac(dev_vcm, position);
568
+	dev_dbg(&client->dev, "%s: set position %d, dac %d\n", __func__, dest_pos, position);
569
+
368570 	return ret;
369571 }
370572 
....@@ -388,10 +590,10 @@
388590 	int ret = 0;
389591 
390592 	if (ctrl->id == V4L2_CID_FOCUS_ABSOLUTE) {
391
-		if (dest_pos > VCMDRV_MAX_LOG) {
392
-			dev_info(&client->dev,
593
+		if (dest_pos > dev_vcm->max_logicalpos) {
594
+			dev_err(&client->dev,
393595 				"%s dest_pos is error. %d > %d\n",
394
-				__func__, dest_pos, VCMDRV_MAX_LOG);
596
+				__func__, dest_pos, dev_vcm->max_logicalpos);
395597 			return -EINVAL;
396598 		}
397599 		/* calculate move time */
....@@ -400,18 +602,18 @@
400602 			move_pos = -move_pos;
401603 
402604 		ret = dw9714_set_pos(dev_vcm, dest_pos);
403
-
404
-		if (dev_vcm->dlc_enable)
605
+		if (dev_vcm->dlc_enable || dev_vcm->adcanced_mode)
405606 			dev_vcm->move_ms = dev_vcm->vcm_movefull_t;
406607 		else
407608 			dev_vcm->move_ms =
408
-				((dev_vcm->vcm_movefull_t * (uint32_t)move_pos) / VCMDRV_MAX_LOG);
609
+				((dev_vcm->vcm_movefull_t * (uint32_t)move_pos) /
610
+				dev_vcm->max_logicalpos);
409611 
410612 		dev_dbg(&client->dev,
411
-			"dest_pos %d, move_ms %ld\n",
412
-			dest_pos, dev_vcm->move_ms);
613
+			"dest_pos %d, dac %d, move_ms %ld\n",
614
+			dest_pos, dev_vcm->current_lens_pos, dev_vcm->move_ms);
413615 
414
-		dev_vcm->start_move_tv = ns_to_timeval(ktime_get_ns());
616
+		dev_vcm->start_move_tv = ns_to_kernel_old_timeval(ktime_get_ns());
415617 		mv_us = dev_vcm->start_move_tv.tv_usec +
416618 				dev_vcm->move_ms * 1000;
417619 		if (mv_us >= 1000000) {
....@@ -433,8 +635,98 @@
433635 	.s_ctrl = dw9714_set_ctrl,
434636 };
435637 
638
+static int dw9714_init(struct i2c_client *client)
639
+{
640
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
641
+	struct dw9714_device *dw9714_dev = sd_to_dw9714_vcm(sd);
642
+	unsigned char data = 0x0;
643
+	int ret = 0;
644
+
645
+	if (dw9714_dev->adcanced_mode) {
646
+		// need to wait 1ms after poweron
647
+		usleep_range(1000, 1200);
648
+		// Advanced Mode
649
+		ret = dw9714_write_msg(client, 0xED, 0xAB);
650
+		if (ret)
651
+			goto err;
652
+		// Power down
653
+		ret = dw9714_write_msg(client, DW9714_ADVMODE_CONTROL, 0x01);
654
+		if (ret)
655
+			goto err;
656
+		// active
657
+		ret = dw9714_write_msg(client, DW9714_ADVMODE_CONTROL, 0x00);
658
+		if (ret)
659
+			goto err;
660
+		// delay 1ms
661
+		usleep_range(1000, 1200);
662
+		// SAC mode & nrc_time & nrc_infl
663
+		data = DW9714_ADVMODE_RING_EN << 7 |
664
+		       (dw9714_dev->nrc_infl & 0x3) << 5 |
665
+		       (dw9714_dev->nrc_time & 0x1) << 4 |
666
+		       (dw9714_dev->sac_mode & 0xF);
667
+		ret = dw9714_write_msg(client, DW9714_ADVMODE_SAC_CFG, data);
668
+		if (ret)
669
+			goto err;
670
+		// Set Tvib (PRESC[1:0] )
671
+		ret = dw9714_write_msg(client, DW9714_ADVMODE_PRESC, dw9714_dev->sac_prescl);
672
+		if (ret)
673
+			goto err;
674
+		// Set Tvib (SACT[6:0] )
675
+		ret = dw9714_write_msg(client, DW9714_ADVMODE_SAC_TIME, dw9714_dev->sac_time);
676
+		if (ret)
677
+			goto err;
678
+		// nrc preset
679
+		ret = dw9714_write_msg(client, DW9714_ADVMODE_PRESET, dw9714_dev->nrc_preset);
680
+		if (ret)
681
+			goto err;
682
+		// nrc en & nrc mode
683
+		data = (dw9714_dev->nrc_en & 0x1) << 1 |
684
+		       (dw9714_dev->nrc_mode & 0x1);
685
+		ret = dw9714_write_msg(client, DW9714_ADVMODE_NRC, data);
686
+		if (ret)
687
+			goto err;
688
+	} else {
689
+		// need to wait 12ms after poweron
690
+		usleep_range(12000, 12500);
691
+
692
+		ret = dw9714_write_msg(client, 0xEC, 0xA3);
693
+		if (ret)
694
+			goto err;
695
+
696
+		data = (dw9714_dev->mclk & 0x3) | 0x04 |
697
+				((dw9714_dev->dlc_enable << 0x3) & 0x08);
698
+		ret = dw9714_write_msg(client, 0xA1, data);
699
+		if (ret)
700
+			goto err;
701
+
702
+		data = (dw9714_dev->t_src << 0x3) & 0xf8;
703
+		ret = dw9714_write_msg(client, 0xF2, data);
704
+		if (ret)
705
+			goto err;
706
+
707
+		ret = dw9714_write_msg(client, 0xDC, 0x51);
708
+		if (ret)
709
+			goto err;
710
+
711
+		/* set normal mode */
712
+		ret = dw9714_write_msg(client, 0xDF, 0x5B);
713
+		if (ret != 0)
714
+			dev_err(&client->dev,
715
+				"%s: failed with error %d\n", __func__, ret);
716
+	}
717
+
718
+	return 0;
719
+err:
720
+	dev_err(&client->dev, "failed with error %d\n", ret);
721
+	return -1;
722
+}
723
+
436724 static int dw9714_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
437725 {
726
+	struct dw9714_device *dev_vcm = sd_to_dw9714_vcm(sd);
727
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
728
+	unsigned int move_time;
729
+	int dac = 0;
438730 	int rval;
439731 
440732 	rval = pm_runtime_get_sync(sd->dev);
....@@ -443,11 +735,50 @@
443735 		return rval;
444736 	}
445737 
738
+	dw9714_init(client);
739
+
740
+	dev_dbg(&client->dev, "%s: current_lens_pos %d, current_related_pos %d\n",
741
+		__func__, dev_vcm->current_lens_pos, dev_vcm->current_related_pos);
742
+	move_time = 1000 * dw9714_move_time(dev_vcm, DW9714_GRADUAL_MOVELENS_STEPS);
743
+	while (dac <= dev_vcm->current_lens_pos) {
744
+		dw9714_set_dac(dev_vcm, dac);
745
+		usleep_range(move_time, move_time + 1000);
746
+		dac += DW9714_GRADUAL_MOVELENS_STEPS;
747
+		if (dac >= dev_vcm->current_lens_pos)
748
+			break;
749
+	}
750
+
751
+	if (dac > dev_vcm->current_lens_pos) {
752
+		dac = dev_vcm->current_lens_pos;
753
+		dw9714_set_dac(dev_vcm, dac);
754
+	}
755
+
446756 	return 0;
447757 }
448758 
449759 static int dw9714_close(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
450760 {
761
+	struct dw9714_device *dev_vcm = sd_to_dw9714_vcm(sd);
762
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
763
+	int dac = dev_vcm->current_lens_pos;
764
+	unsigned int move_time;
765
+
766
+	dev_dbg(&client->dev, "%s: current_lens_pos %d, current_related_pos %d\n",
767
+		__func__, dev_vcm->current_lens_pos, dev_vcm->current_related_pos);
768
+	move_time = 1000 * dw9714_move_time(dev_vcm, DW9714_GRADUAL_MOVELENS_STEPS);
769
+	while (dac >= 0) {
770
+		dw9714_set_dac(dev_vcm, dac);
771
+		usleep_range(move_time, move_time + 1000);
772
+		dac -= DW9714_GRADUAL_MOVELENS_STEPS;
773
+		if (dac <= 0)
774
+			break;
775
+	}
776
+
777
+	if (dac < 0) {
778
+		dac = 0;
779
+		dw9714_set_dac(dev_vcm, dac);
780
+	}
781
+
451782 	pm_runtime_put(sd->dev);
452783 
453784 	return 0;
....@@ -461,28 +792,24 @@
461792 static void dw9714_update_vcm_cfg(struct dw9714_device *dev_vcm)
462793 {
463794 	struct i2c_client *client = v4l2_get_subdevdata(&dev_vcm->sd);
464
-	int cur_dist;
465795 
466
-	if (dev_vcm->max_ma == 0) {
796
+	if (dev_vcm->max_current == 0) {
467797 		dev_err(&client->dev, "max current is zero");
468798 		return;
469799 	}
470800 
471
-	cur_dist = dev_vcm->vcm_cfg.rated_ma - dev_vcm->vcm_cfg.start_ma;
472
-	cur_dist = cur_dist * DW9714_MAX_REG / dev_vcm->max_ma;
473
-	dev_vcm->step = (cur_dist + (VCMDRV_MAX_LOG - 1)) / VCMDRV_MAX_LOG;
474801 	dev_vcm->start_current = dev_vcm->vcm_cfg.start_ma *
475
-				 DW9714_MAX_REG / dev_vcm->max_ma;
476
-	dev_vcm->rated_current = dev_vcm->start_current +
477
-				 VCMDRV_MAX_LOG * dev_vcm->step;
802
+				 DW9714_MAX_REG / dev_vcm->max_current;
803
+	dev_vcm->rated_current = dev_vcm->vcm_cfg.rated_ma *
804
+				 DW9714_MAX_REG / dev_vcm->max_current;
478805 	dev_vcm->step_mode = dev_vcm->vcm_cfg.step_mode;
479806 
480807 	dev_dbg(&client->dev,
481
-		"vcm_cfg: %d, %d, %d, max_ma %d\n",
808
+		"vcm_cfg: %d, %d, %d, max_current %d\n",
482809 		dev_vcm->vcm_cfg.start_ma,
483810 		dev_vcm->vcm_cfg.rated_ma,
484811 		dev_vcm->vcm_cfg.step_mode,
485
-		dev_vcm->max_ma);
812
+		dev_vcm->max_current);
486813 }
487814 
488815 static long dw9714_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
....@@ -491,6 +818,7 @@
491818 	struct i2c_client *client = v4l2_get_subdevdata(sd);
492819 	struct rk_cam_vcm_tim *vcm_tim;
493820 	struct rk_cam_vcm_cfg *vcm_cfg;
821
+	unsigned int max_logicalpos;
494822 	int ret = 0;
495823 
496824 	if (cmd == RK_VIDIOC_VCM_TIMEINFO) {
....@@ -520,6 +848,16 @@
520848 		dev_vcm->vcm_cfg.rated_ma = vcm_cfg->rated_ma;
521849 		dev_vcm->vcm_cfg.step_mode = vcm_cfg->step_mode;
522850 		dw9714_update_vcm_cfg(dev_vcm);
851
+	} else if (cmd == RK_VIDIOC_SET_VCM_MAX_LOGICALPOS) {
852
+		max_logicalpos = *(unsigned int *)arg;
853
+
854
+		if (max_logicalpos > 0) {
855
+			dev_vcm->max_logicalpos = max_logicalpos;
856
+			__v4l2_ctrl_modify_range(dev_vcm->focus,
857
+				0, dev_vcm->max_logicalpos, 1, dev_vcm->max_logicalpos);
858
+		}
859
+		dev_dbg(&client->dev,
860
+			"max_logicalpos %d\n", max_logicalpos);
523861 	} else {
524862 		dev_err(&client->dev,
525863 			"cmd 0x%x not supported\n", cmd);
....@@ -538,6 +876,7 @@
538876 	struct rk_cam_compat_vcm_tim compat_vcm_tim;
539877 	struct rk_cam_vcm_tim vcm_tim;
540878 	struct rk_cam_vcm_cfg vcm_cfg;
879
+	unsigned int max_logicalpos;
541880 	long ret;
542881 
543882 	if (cmd == RK_VIDIOC_COMPAT_VCM_TIMEINFO) {
....@@ -559,12 +898,23 @@
559898 			&p32->vcm_end_t.tv_usec);
560899 	} else if (cmd == RK_VIDIOC_GET_VCM_CFG) {
561900 		ret = dw9714_ioctl(sd, RK_VIDIOC_GET_VCM_CFG, &vcm_cfg);
562
-		if (!ret)
901
+		if (!ret) {
563902 			ret = copy_to_user(up, &vcm_cfg, sizeof(vcm_cfg));
903
+			if (ret)
904
+				ret = -EFAULT;
905
+		}
564906 	} else if (cmd == RK_VIDIOC_SET_VCM_CFG) {
565907 		ret = copy_from_user(&vcm_cfg, up, sizeof(vcm_cfg));
566908 		if (!ret)
567909 			ret = dw9714_ioctl(sd, cmd, &vcm_cfg);
910
+		else
911
+			ret = -EFAULT;
912
+	} else if (cmd == RK_VIDIOC_SET_VCM_MAX_LOGICALPOS) {
913
+		ret = copy_from_user(&max_logicalpos, up, sizeof(max_logicalpos));
914
+		if (!ret)
915
+			ret = dw9714_ioctl(sd, cmd, &max_logicalpos);
916
+		else
917
+			ret = -EFAULT;
568918 	} else {
569919 		dev_err(&client->dev,
570920 			"cmd 0x%x not supported\n", cmd);
....@@ -601,8 +951,8 @@
601951 
602952 	v4l2_ctrl_handler_init(hdl, 1);
603953 
604
-	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_FOCUS_ABSOLUTE,
605
-			  0, VCMDRV_MAX_LOG, 1, VCMDRV_MAX_LOG);
954
+	dev_vcm->focus = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_FOCUS_ABSOLUTE,
955
+				0, dev_vcm->max_logicalpos, 1, dev_vcm->max_logicalpos);
606956 
607957 	if (hdl->error)
608958 		dev_err(dev_vcm->sd.dev, "%s fail error: 0x%x\n",
....@@ -611,97 +961,351 @@
611961 	return hdl->error;
612962 }
613963 
614
-static int dw9714_probe(struct i2c_client *client,
615
-			const struct i2c_device_id *id)
964
+#define USED_SYS_DEBUG
965
+#ifdef USED_SYS_DEBUG
966
+static ssize_t set_dacval(struct device *dev,
967
+	struct device_attribute *attr,
968
+	const char *buf,
969
+	size_t count)
616970 {
617
-	struct device_node *np = of_node_get(client->dev.of_node);
618
-	struct dw9714_device *dw9714_dev;
619
-	unsigned int max_ma, start_ma, rated_ma, step_mode;
620
-	unsigned int dlc_en, mclk, t_src;
621
-	struct v4l2_subdev *sd;
622
-	char facing[2];
971
+	struct i2c_client *client = to_i2c_client(dev);
972
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
973
+	struct dw9714_device *dev_vcm = sd_to_dw9714_vcm(sd);
974
+	int val = 0;
975
+	int ret = 0;
976
+
977
+	ret = kstrtoint(buf, 0, &val);
978
+	if (!ret)
979
+		dw9714_set_dac(dev_vcm, val);
980
+
981
+	return count;
982
+}
983
+
984
+static ssize_t get_dacval(struct device *dev,
985
+		struct device_attribute *attr, char *buf)
986
+{
987
+	struct i2c_client *client = to_i2c_client(dev);
988
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
989
+	struct dw9714_device *dev_vcm = sd_to_dw9714_vcm(sd);
990
+	unsigned int dac = 0;
991
+
992
+	dw9714_get_dac(dev_vcm, &dac);
993
+	return sprintf(buf, "%u\n", dac);
994
+}
995
+
996
+static struct device_attribute attributes[] = {
997
+	__ATTR(dacval, 0600, get_dacval, set_dacval),
998
+};
999
+
1000
+static int add_sysfs_interfaces(struct device *dev)
1001
+{
1002
+	int i;
1003
+
1004
+	for (i = 0; i < ARRAY_SIZE(attributes); i++)
1005
+		if (device_create_file(dev, attributes + i))
1006
+			goto undo;
1007
+	return 0;
1008
+undo:
1009
+	for (i--; i >= 0 ; i--)
1010
+		device_remove_file(dev, attributes + i);
1011
+	dev_err(dev, "%s: failed to create sysfs interface\n", __func__);
1012
+	return -ENODEV;
1013
+}
1014
+
1015
+static int remove_sysfs_interfaces(struct device *dev)
1016
+{
1017
+	int i;
1018
+
1019
+	for (i = 0; i < ARRAY_SIZE(attributes); i++)
1020
+		device_remove_file(dev, attributes + i);
1021
+	return 0;
1022
+}
1023
+#else
1024
+static inline int add_sysfs_interfaces(struct device *dev)
1025
+{
1026
+	return 0;
1027
+}
1028
+
1029
+static inline int remove_sysfs_interfaces(struct device *dev)
1030
+{
1031
+	return 0;
1032
+}
1033
+#endif
1034
+
1035
+static int dw9714_set_power(struct dw9714_device *dw9714, bool on)
1036
+{
1037
+	struct i2c_client *client = dw9714->client;
1038
+	int ret = 0;
1039
+
1040
+	dev_info(&client->dev, "%s(%d) on(%d)\n", __func__, __LINE__, on);
1041
+
1042
+	if (dw9714->power_on == !!on)
1043
+		goto unlock_and_return;
1044
+
1045
+	if (on) {
1046
+		ret = regulator_enable(dw9714->supply);
1047
+		if (ret < 0) {
1048
+			dev_err(&client->dev, "Failed to enable regulator\n");
1049
+			goto unlock_and_return;
1050
+		}
1051
+		dw9714->power_on = true;
1052
+	} else {
1053
+		ret = regulator_disable(dw9714->supply);
1054
+		if (ret < 0) {
1055
+			dev_err(&client->dev, "Failed to disable regulator\n");
1056
+			goto unlock_and_return;
1057
+		}
1058
+		dw9714->power_on = false;
1059
+	}
1060
+
1061
+unlock_and_return:
1062
+	return ret;
1063
+}
1064
+
1065
+static int dw9714_check_i2c(struct dw9714_device *dw9714,
1066
+			    struct i2c_client *client)
1067
+{
1068
+	struct device *dev = &client->dev;
6231069 	int ret;
6241070 
625
-	dev_info(&client->dev, "probing...\n");
1071
+	if (dw9714->adcanced_mode) {
1072
+		// need to wait 1ms after poweron
1073
+		usleep_range(1000, 1200);
1074
+		// Advanced Mode
1075
+		ret = dw9714_write_msg(client, 0xED, 0xAB);
1076
+	} else {
1077
+		// need to wait 12ms after poweron
1078
+		usleep_range(12000, 12500);
1079
+		ret = dw9714_write_msg(client, 0xEC, 0xA3);
1080
+	}
1081
+	if (!ret)
1082
+		dev_info(dev, "check dw9714 connection OK!\n");
1083
+	else
1084
+		dev_info(dev, "dw9714 not connect!\n");
1085
+
1086
+	return ret;
1087
+}
1088
+
1089
+static int dw9714_configure_regulator(struct dw9714_device *dw9714)
1090
+{
1091
+	struct i2c_client *client = dw9714->client;
1092
+	int ret = 0;
1093
+
1094
+	dw9714->supply = devm_regulator_get(&client->dev, "avdd");
1095
+	if (IS_ERR(dw9714->supply)) {
1096
+		ret = PTR_ERR(dw9714->supply);
1097
+		if (ret != -EPROBE_DEFER)
1098
+			dev_err(&client->dev, "could not get regulator avdd\n");
1099
+		return ret;
1100
+	}
1101
+	dw9714->power_on = false;
1102
+	return ret;
1103
+}
1104
+
1105
+static int dw9714_parse_dt_property(struct i2c_client *client,
1106
+				    struct dw9714_device *dev_vcm)
1107
+{
1108
+	struct device_node *np = of_node_get(client->dev.of_node);
1109
+	int ret;
1110
+
6261111 	if (of_property_read_u32(np,
6271112 		OF_CAMERA_VCMDRV_MAX_CURRENT,
628
-		(unsigned int *)&max_ma)) {
629
-		max_ma = DW9714_MAX_CURRENT;
1113
+		(unsigned int *)&dev_vcm->max_current)) {
1114
+		dev_vcm->max_current = DW9714_MAX_CURRENT;
6301115 		dev_info(&client->dev,
6311116 			"could not get module %s from dts!\n",
6321117 			OF_CAMERA_VCMDRV_MAX_CURRENT);
6331118 	}
634
-	if (max_ma == 0)
635
-		max_ma = DW9714_MAX_CURRENT;
1119
+	if (dev_vcm->max_current == 0)
1120
+		dev_vcm->max_current = DW9714_MAX_CURRENT;
6361121 
6371122 	if (of_property_read_u32(np,
6381123 		OF_CAMERA_VCMDRV_START_CURRENT,
639
-		(unsigned int *)&start_ma)) {
640
-		start_ma = DW9714_DEFAULT_START_CURRENT;
1124
+		(unsigned int *)&dev_vcm->vcm_cfg.start_ma)) {
1125
+		dev_vcm->vcm_cfg.start_ma = DW9714_DEFAULT_START_CURRENT;
6411126 		dev_info(&client->dev,
6421127 			"could not get module %s from dts!\n",
6431128 			OF_CAMERA_VCMDRV_START_CURRENT);
6441129 	}
6451130 	if (of_property_read_u32(np,
6461131 		OF_CAMERA_VCMDRV_RATED_CURRENT,
647
-		(unsigned int *)&rated_ma)) {
648
-		rated_ma = DW9714_DEFAULT_RATED_CURRENT;
1132
+		(unsigned int *)&dev_vcm->vcm_cfg.rated_ma)) {
1133
+		dev_vcm->vcm_cfg.rated_ma = DW9714_DEFAULT_RATED_CURRENT;
6491134 		dev_info(&client->dev,
6501135 			"could not get module %s from dts!\n",
6511136 			OF_CAMERA_VCMDRV_RATED_CURRENT);
6521137 	}
6531138 	if (of_property_read_u32(np,
6541139 		OF_CAMERA_VCMDRV_STEP_MODE,
655
-		(unsigned int *)&step_mode)) {
656
-		step_mode = DW9714_DEFAULT_STEP_MODE;
1140
+		(unsigned int *)&dev_vcm->vcm_cfg.step_mode)) {
1141
+		dev_vcm->vcm_cfg.step_mode = DW9714_DEFAULT_STEP_MODE;
6571142 		dev_info(&client->dev,
6581143 			"could not get module %s from dts!\n",
6591144 			OF_CAMERA_VCMDRV_STEP_MODE);
6601145 	}
6611146 	if (of_property_read_u32(np,
6621147 		OF_CAMERA_VCMDRV_DLC_ENABLE,
663
-		(unsigned int *)&dlc_en)) {
664
-		dlc_en = DW9714_DEFAULT_DLC_EN;
1148
+		(unsigned int *)&dev_vcm->dlc_enable)) {
1149
+		dev_vcm->dlc_enable = DW9714_DEFAULT_DLC_EN;
6651150 		dev_info(&client->dev,
6661151 			"could not get module %s from dts!\n",
6671152 			OF_CAMERA_VCMDRV_DLC_ENABLE);
6681153 	}
6691154 	if (of_property_read_u32(np,
6701155 		OF_CAMERA_VCMDRV_MCLK,
671
-		(unsigned int *)&mclk)) {
672
-		mclk = DW9714_DEFAULT_MCLK;
1156
+		(unsigned int *)&dev_vcm->mclk)) {
1157
+		dev_vcm->mclk = DW9714_DEFAULT_MCLK;
6731158 		dev_info(&client->dev,
6741159 			"could not get module %s from dts!\n",
6751160 			OF_CAMERA_VCMDRV_MCLK);
6761161 	}
6771162 	if (of_property_read_u32(np,
6781163 		OF_CAMERA_VCMDRV_T_SRC,
679
-		(unsigned int *)&t_src)) {
680
-		t_src = DW9714_DEFAULT_T_SRC;
1164
+		(unsigned int *)&dev_vcm->t_src)) {
1165
+		dev_vcm->t_src = DW9714_DEFAULT_T_SRC;
6811166 		dev_info(&client->dev,
6821167 			"could not get module %s from dts!\n",
6831168 			OF_CAMERA_VCMDRV_T_SRC);
6841169 	}
1170
+	if (of_property_read_u32(np,
1171
+		OF_CAMERA_VCMDRV_ADVANCED_MODE,
1172
+		(unsigned int *)&dev_vcm->adcanced_mode)) {
1173
+		dev_vcm->adcanced_mode = DW9714_DEFAULT_ADVMODE;
1174
+		dev_info(&client->dev,
1175
+			"could not get module %s from dts!\n",
1176
+			OF_CAMERA_VCMDRV_ADVANCED_MODE);
1177
+	}
1178
+	if (of_property_read_u32(np,
1179
+		OF_CAMERA_VCMDRV_SAC_MODE,
1180
+		(unsigned int *)&dev_vcm->sac_mode)) {
1181
+		dev_vcm->sac_mode = DW9714_DEFAULT_SAC_MODE;
1182
+		dev_info(&client->dev,
1183
+			"could not get module %s from dts!\n",
1184
+			OF_CAMERA_VCMDRV_SAC_MODE);
1185
+	}
1186
+	if (of_property_read_u32(np,
1187
+		OF_CAMERA_VCMDRV_SAC_TIME,
1188
+		(unsigned int *)&dev_vcm->sac_time)) {
1189
+		dev_vcm->sac_time = DW9714_DEFAULT_SAC_TIME;
1190
+		dev_info(&client->dev,
1191
+			"could not get module %s from dts!\n",
1192
+			OF_CAMERA_VCMDRV_SAC_TIME);
1193
+	}
1194
+	if (of_property_read_u32(np,
1195
+		OF_CAMERA_VCMDRV_PRESC,
1196
+		(unsigned int *)&dev_vcm->sac_prescl)) {
1197
+		dev_vcm->sac_prescl = DW9714_DEFAULT_SAC_PRESCL;
1198
+		dev_info(&client->dev,
1199
+			"could not get module %s from dts!\n",
1200
+			OF_CAMERA_VCMDRV_PRESC);
1201
+	}
1202
+	if (of_property_read_u32(np,
1203
+		OF_CAMERA_VCMDRV_NRC_EN,
1204
+		(unsigned int *)&dev_vcm->nrc_en)) {
1205
+		dev_vcm->nrc_en = DW9714_DEFAULT_NRC_EN;
1206
+		dev_info(&client->dev,
1207
+			"could not get module %s from dts!\n",
1208
+			OF_CAMERA_VCMDRV_NRC_EN);
1209
+	}
1210
+	if (of_property_read_u32(np,
1211
+		OF_CAMERA_VCMDRV_NRC_MODE,
1212
+		(unsigned int *)&dev_vcm->nrc_mode)) {
1213
+		dev_vcm->nrc_mode = DW9714_DEFAULT_NRC_MODE;
1214
+		dev_info(&client->dev,
1215
+			"could not get module %s from dts!\n",
1216
+			OF_CAMERA_VCMDRV_NRC_MODE);
1217
+	}
1218
+	if (of_property_read_u32(np,
1219
+		OF_CAMERA_VCMDRV_NRC_PRESET,
1220
+		(unsigned int *)&dev_vcm->nrc_preset)) {
1221
+		dev_vcm->nrc_preset = DW9714_DEFAULT_NRC_PRESET;
1222
+		dev_info(&client->dev,
1223
+			"could not get module %s from dts!\n",
1224
+			OF_CAMERA_VCMDRV_NRC_PRESET);
1225
+	}
1226
+	if (of_property_read_u32(np,
1227
+		OF_CAMERA_VCMDRV_NRC_INFL,
1228
+		(unsigned int *)&dev_vcm->nrc_infl)) {
1229
+		dev_vcm->nrc_infl = DW9714_DEFAULT_NRC_INFL;
1230
+		dev_info(&client->dev,
1231
+			"could not get module %s from dts!\n",
1232
+			OF_CAMERA_VCMDRV_NRC_INFL);
1233
+	}
1234
+	if (of_property_read_u32(np,
1235
+		OF_CAMERA_VCMDRV_NRC_TIME,
1236
+		(unsigned int *)&dev_vcm->nrc_time)) {
1237
+		dev_vcm->nrc_time = DW9714_DEFAULT_NRC_TIME;
1238
+		dev_info(&client->dev,
1239
+			"could not get module %s from dts!\n",
1240
+			OF_CAMERA_VCMDRV_NRC_TIME);
1241
+	}
6851242 
686
-	dw9714_dev = devm_kzalloc(&client->dev, sizeof(*dw9714_dev),
687
-				  GFP_KERNEL);
688
-	if (dw9714_dev == NULL)
689
-		return -ENOMEM;
1243
+	dev_vcm->xsd_gpio = devm_gpiod_get(&client->dev, "xsd", GPIOD_OUT_HIGH);
1244
+	if (IS_ERR(dev_vcm->xsd_gpio))
1245
+		dev_warn(&client->dev, "Failed to get xsd-gpios\n");
6901246 
6911247 	ret = of_property_read_u32(np, RKMODULE_CAMERA_MODULE_INDEX,
692
-				   &dw9714_dev->module_index);
1248
+				   &dev_vcm->module_index);
6931249 	ret |= of_property_read_string(np, RKMODULE_CAMERA_MODULE_FACING,
694
-				       &dw9714_dev->module_facing);
1250
+				       &dev_vcm->module_facing);
6951251 	if (ret) {
6961252 		dev_err(&client->dev,
6971253 			"could not get module information!\n");
6981254 		return -EINVAL;
6991255 	}
7001256 
1257
+	dev_vcm->client = client;
1258
+	ret = dw9714_configure_regulator(dev_vcm);
1259
+	if (ret) {
1260
+		dev_err(&client->dev, "Failed to get power regulator!\n");
1261
+		return ret;
1262
+	}
1263
+
1264
+	dev_dbg(&client->dev, "current: %d, %d, %d, dlc_en: %d, t_src: %d, mclk: %d",
1265
+		dev_vcm->max_current,
1266
+		dev_vcm->start_current,
1267
+		dev_vcm->rated_current,
1268
+		dev_vcm->dlc_enable,
1269
+		dev_vcm->t_src,
1270
+		dev_vcm->mclk);
1271
+
1272
+	/* advanced mode*/
1273
+	dev_dbg(&client->dev, "adcanced: %d, sac: %d, %d, %d, nrc: %d, %d, %d, %d, %d",
1274
+		dev_vcm->adcanced_mode,
1275
+		dev_vcm->sac_mode,
1276
+		dev_vcm->sac_time,
1277
+		dev_vcm->sac_prescl,
1278
+		dev_vcm->nrc_en,
1279
+		dev_vcm->nrc_mode,
1280
+		dev_vcm->nrc_preset,
1281
+		dev_vcm->nrc_infl,
1282
+		dev_vcm->nrc_time);
1283
+
1284
+	return 0;
1285
+}
1286
+
1287
+static int dw9714_probe(struct i2c_client *client,
1288
+			const struct i2c_device_id *id)
1289
+{
1290
+	struct dw9714_device *dw9714_dev;
1291
+	struct v4l2_subdev *sd;
1292
+	char facing[2];
1293
+	int ret;
1294
+
1295
+	dev_info(&client->dev, "probing...\n");
1296
+	dw9714_dev = devm_kzalloc(&client->dev, sizeof(*dw9714_dev),
1297
+				  GFP_KERNEL);
1298
+	if (dw9714_dev == NULL)
1299
+		return -ENOMEM;
1300
+
1301
+	ret = dw9714_parse_dt_property(client, dw9714_dev);
1302
+	if (ret)
1303
+		return ret;
7011304 	v4l2_i2c_subdev_init(&dw9714_dev->sd, client, &dw9714_ops);
7021305 	dw9714_dev->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
7031306 	dw9714_dev->sd.internal_ops = &dw9714_int_ops;
7041307 
1308
+	dw9714_dev->max_logicalpos = VCMDRV_MAX_LOG;
7051309 	ret = dw9714_init_controls(dw9714_dev);
7061310 	if (ret)
7071311 		goto err_cleanup;
....@@ -709,6 +1313,14 @@
7091313 	ret = media_entity_pads_init(&dw9714_dev->sd.entity, 0, NULL);
7101314 	if (ret < 0)
7111315 		goto err_cleanup;
1316
+
1317
+	ret = dw9714_set_power(dw9714_dev, true);
1318
+	if (ret)
1319
+		goto err_cleanup;
1320
+
1321
+	ret = dw9714_check_i2c(dw9714_dev, client);
1322
+	if (ret)
1323
+		goto err_power_off;
7121324 
7131325 	sd = &dw9714_dev->sd;
7141326 	sd->entity.function = MEDIA_ENT_F_LENS;
....@@ -726,30 +1338,23 @@
7261338 	if (ret)
7271339 		dev_err(&client->dev, "v4l2 async register subdev failed\n");
7281340 
729
-	dw9714_dev->max_ma = max_ma;
730
-	dw9714_dev->vcm_cfg.start_ma = start_ma;
731
-	dw9714_dev->vcm_cfg.rated_ma = rated_ma;
732
-	dw9714_dev->vcm_cfg.step_mode = step_mode;
7331341 	dw9714_update_vcm_cfg(dw9714_dev);
734
-	dw9714_dev->move_ms       = 0;
735
-	dw9714_dev->current_related_pos = VCMDRV_MAX_LOG;
736
-	dw9714_dev->start_move_tv = ns_to_timeval(ktime_get_ns());
737
-	dw9714_dev->end_move_tv = ns_to_timeval(ktime_get_ns());
738
-
739
-	dw9714_dev->dlc_enable = dlc_en;
740
-	dw9714_dev->mclk = mclk;
741
-	dw9714_dev->t_src = t_src;
742
-
1342
+	dw9714_dev->move_ms = 0;
1343
+	dw9714_dev->current_related_pos = dw9714_dev->max_logicalpos;
1344
+	dw9714_dev->current_lens_pos = dw9714_dev->start_current;
1345
+	dw9714_dev->start_move_tv = ns_to_kernel_old_timeval(ktime_get_ns());
1346
+	dw9714_dev->end_move_tv = ns_to_kernel_old_timeval(ktime_get_ns());
7431347 	dw9714_dev->vcm_movefull_t =
7441348 		dw9714_move_time(dw9714_dev, DW9714_MAX_REG);
745
-	pm_runtime_set_active(&client->dev);
7461349 	pm_runtime_enable(&client->dev);
747
-	pm_runtime_idle(&client->dev);
7481350 
1351
+	add_sysfs_interfaces(&client->dev);
7491352 	dev_info(&client->dev, "probing successful\n");
7501353 
7511354 	return 0;
7521355 
1356
+err_power_off:
1357
+	dw9714_set_power(dw9714_dev, false);
7531358 err_cleanup:
7541359 	dw9714_subdev_cleanup(dw9714_dev);
7551360 	dev_err(&client->dev, "Probe failed: %d\n", ret);
....@@ -761,55 +1366,20 @@
7611366 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
7621367 	struct dw9714_device *dw9714_dev = sd_to_dw9714_vcm(sd);
7631368 
1369
+	remove_sysfs_interfaces(&client->dev);
7641370 	pm_runtime_disable(&client->dev);
7651371 	dw9714_subdev_cleanup(dw9714_dev);
7661372 
7671373 	return 0;
7681374 }
7691375 
770
-static int dw9714_init(struct i2c_client *client)
771
-{
772
-	struct v4l2_subdev *sd = i2c_get_clientdata(client);
773
-	struct dw9714_device *dw9714_dev = sd_to_dw9714_vcm(sd);
774
-	unsigned char data = 0x0;
775
-	int ret = 0;
776
-
777
-	// need to wait 12ms after poweron
778
-	usleep_range(12000, 12500);
779
-
780
-	ret = dw9714_write_msg(client, 0xEC, 0xA3);
781
-	if (ret)
782
-		goto err;
783
-
784
-	data = (dw9714_dev->mclk & 0x3) | 0x04 |
785
-			((dw9714_dev->dlc_enable << 0x3) & 0x08);
786
-	ret = dw9714_write_msg(client, 0xA1, data);
787
-	if (ret)
788
-		goto err;
789
-
790
-	data = (dw9714_dev->t_src << 0x3) & 0xf8;
791
-	ret = dw9714_write_msg(client, 0xF2, data);
792
-	if (ret)
793
-		goto err;
794
-
795
-	ret = dw9714_write_msg(client, 0xDC, 0x51);
796
-	if (ret)
797
-		goto err;
798
-
799
-	/* set normal mode */
800
-	ret = dw9714_write_msg(client, 0xDF, 0x5B);
801
-	if (ret != 0)
802
-		dev_err(&client->dev,
803
-			"%s: failed with error %d\n", __func__, ret);
804
-
805
-	return 0;
806
-err:
807
-	dev_err(&client->dev, "failed with error %d\n", ret);
808
-	return -1;
809
-}
810
-
8111376 static int __maybe_unused dw9714_vcm_suspend(struct device *dev)
8121377 {
1378
+	struct i2c_client *client = to_i2c_client(dev);
1379
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
1380
+	struct dw9714_device *dev_vcm = sd_to_dw9714_vcm(sd);
1381
+
1382
+	dw9714_set_power(dev_vcm, false);
8131383 	return 0;
8141384 }
8151385 
....@@ -819,8 +1389,7 @@
8191389 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
8201390 	struct dw9714_device *dev_vcm = sd_to_dw9714_vcm(sd);
8211391 
822
-	dw9714_init(client);
823
-	dw9714_set_pos(dev_vcm, dev_vcm->current_related_pos);
1392
+	dw9714_set_power(dev_vcm, true);
8241393 	return 0;
8251394 }
8261395