1 // SPDX-License-Identifier: GPL-2.0
2 /*
3 * motor driver
4 *
5 * Copyright (C) 2020 Rockchip Electronics Co., Ltd.
6 *
7 */
8 //#define DEBUG
9 #include <linux/io.h>
10 #include <linux/of_gpio.h>
11 #include <linux/module.h>
12 #include <linux/init.h>
13 #include <linux/device.h>
14 #include <linux/fb.h>
15 #include <linux/interrupt.h>
16 #include <linux/kernel.h>
17 #include <linux/gpio/consumer.h>
18 #include <linux/of_irq.h>
19 #include <linux/platform_device.h>
20 #include <linux/wakelock.h>
21 #include <linux/hrtimer.h>
22 #include <linux/pwm.h>
23 #include <linux/delay.h>
24 #include <media/v4l2-subdev.h>
25 #include <media/v4l2-ctrls.h>
26 #include <media/v4l2-device.h>
27 #include <linux/mutex.h>
28 #include <linux/version.h>
29 #include <linux/rk-camera-module.h>
30 #include <linux/completion.h>
31 #include <linux/rk_vcm_head.h>
32
33 #define DRIVER_VERSION KERNEL_VERSION(0, 0x01, 0x00)
34
35 #define DRIVER_NAME "mp6507"
36
37 #define MAX_START_UP_HZ (1200)
38 #define MOTOR_MAX_HZ (2500)
39 #define SPEED_QUEUE_MAX (71)
40 #define THRESHOLD_TO_SPEEDED_UP_DEF (500)
41 #define STEP_PER_SPEED_DEF (8)
42 #define SPEED_QUEUE_NUM_DEF (71)
43
44 #define IRIS_MAX_STEP_DEF 80
45 #define FOCUS_MAX_STEP_DEF 7500
46 #define ZOOM_MAX_STEP_DEF 7500
47
48 #define IRIS_MAX_LOG 80
49 #define FOCUS_MAX_LOG 7500
50 #define ZOOM_MAX_LOG 7500
51 #define IRIS_LOG_STEP 4
52 #define FOCUS_LOG_STEP 4
53 #define ZOOM_LOG_STEP 4
54
55 enum {
56 MOTOR_STATUS_STOPPED = 0,
57 MOTOR_STATUS_CW = 1,
58 MOTOR_STATUS_CCW = 2,
59 };
60
61 enum ext_dev_type {
62 TYPE_IRIS = 0,
63 TYPE_FOCUS = 1,
64 TYPE_ZOOM = 2,
65 };
66
67 struct speed_s {
68 u32 count;
69 u64 phase_interval_ns;
70 };
71
72 struct speed_queue_s {
73 int count;
74 struct speed_s *speed_p;
75 };
76
77 struct ext_dev {
78 u8 type;
79 u32 step_max;
80 u32 cur_pos;
81 u32 step_per_pos;
82 u32 start_up_speed;
83 u32 max_speed;
84 u32 speed_queue_num;
85 u32 first_speed_step;
86 u32 ths_speeded_up;
87 u32 speed_up_step_cnt;
88 u32 *speed_up_table;
89 u32 *speed_down_table;
90 u32 length_up;//speed_up_table length
91 u32 length_down;
92 struct gpio_desc *en_gpio;
93 struct rk_cam_vcm_tim mv_tim;
94 struct speed_queue_s speed_que;
95 struct speed_queue_s one_speed_que;
96 };
97
98 struct motor_dev {
99 struct v4l2_subdev sd;
100 struct v4l2_ctrl_handler ctrl_handler;
101 struct pwm_device *pwm_a1;
102 struct pwm_device *pwm_a2;
103 struct pwm_device *pwm_b1;
104 struct pwm_device *pwm_b2;
105 struct v4l2_ctrl *iris_ctrl;
106 struct v4l2_ctrl *focus_ctrl;
107 struct v4l2_ctrl *zoom_ctrl;
108 struct device *dev;
109 struct hrtimer timer;
110 struct mutex mutex;
111 u32 move_status;
112 u32 move_cnt;
113 u32 module_index;
114 const char *module_facing;
115 bool resched;
116 struct completion complete;
117 struct ext_dev iris;
118 struct ext_dev focus;
119 struct ext_dev zoom;
120 struct ext_dev *cur_ext_dev;
121 struct speed_queue_s *run_queue;
122 struct pwm_state pwm_state;
123 };
124
set_motor_running_status(struct motor_dev * motor,struct ext_dev * cur_ext_dev,int status,u32 pos)125 static int set_motor_running_status(struct motor_dev *motor,
126 struct ext_dev *cur_ext_dev,
127 int status, u32 pos)
128 {
129 int ret = 0;
130 u64 mv_us = 0;
131 u64 mv_s = 0;
132 u64 move_time = 0;
133 u32 step_cnt = 0;
134 int i = 0;
135
136 if (motor->move_status != MOTOR_STATUS_STOPPED)
137 wait_for_completion(&motor->complete);
138
139 motor->cur_ext_dev = cur_ext_dev;
140 if (!IS_ERR(cur_ext_dev->en_gpio))
141 gpiod_set_value_cansleep(cur_ext_dev->en_gpio, 1);
142
143 motor->move_status = status;
144 step_cnt = pos * cur_ext_dev->step_per_pos;
145 if (cur_ext_dev->speed_queue_num > 1 &&
146 step_cnt >= cur_ext_dev->ths_speeded_up) {
147 motor->run_queue = &cur_ext_dev->speed_que;
148 motor->run_queue->speed_p[cur_ext_dev->length_up - 1].count =
149 step_cnt - cur_ext_dev->speed_up_step_cnt;
150 } else {
151 motor->run_queue = &cur_ext_dev->one_speed_que;
152 motor->run_queue->speed_p[0].count = step_cnt;
153 }
154 motor->move_cnt = motor->run_queue->count;
155 reinit_completion(&motor->complete);
156
157 cur_ext_dev->mv_tim.vcm_start_t = ns_to_timeval(ktime_get_ns());
158 for (i = 0; i < motor->run_queue->count; i++) {
159 move_time += (u64)motor->run_queue->speed_p[i].count *
160 (u64)motor->run_queue->speed_p[i].phase_interval_ns;
161 dev_dbg(motor->dev, "speed_que.speed[%d], count %d, phase_interval_ns %llu\n",
162 i,
163 motor->run_queue->speed_p[i].count,
164 motor->run_queue->speed_p[i].phase_interval_ns);
165 }
166
167 mv_us = div_u64(move_time, 1000);
168 dev_dbg(motor->dev, "motor move needs %lld us\n", mv_us);
169 mv_us += cur_ext_dev->mv_tim.vcm_start_t.tv_usec;
170 if (mv_us >= 1000000) {
171 mv_s = div_u64(mv_us, 1000000);
172 cur_ext_dev->mv_tim.vcm_end_t.tv_sec =
173 cur_ext_dev->mv_tim.vcm_start_t.tv_sec + mv_s;
174 cur_ext_dev->mv_tim.vcm_end_t.tv_usec = mv_us - (mv_s * 1000000);
175 } else {
176 cur_ext_dev->mv_tim.vcm_end_t.tv_sec =
177 cur_ext_dev->mv_tim.vcm_start_t.tv_sec;
178 cur_ext_dev->mv_tim.vcm_end_t.tv_usec = mv_us;
179 }
180 hrtimer_start(&motor->timer, ktime_set(0, 0), HRTIMER_MODE_REL);
181 return ret;
182 }
183
fill_speed_squeue(struct device * dev,struct ext_dev * ext_dev)184 static int fill_speed_squeue(struct device *dev, struct ext_dev *ext_dev)
185 {
186 struct device_node *node = dev->of_node;
187 struct property *prop = NULL;
188 u32 length_up = 0;
189 u32 length_down = 0;
190 u32 *speed_up_table = NULL;
191 u32 *speed_down_table = NULL;
192 int i = 0;
193 size_t size;
194 u32 step_cnt = 0;
195 u32 step_total = 0;
196 int ret = 0;
197
198 ext_dev->ths_speeded_up = 0;
199 size = sizeof(*ext_dev->one_speed_que.speed_p);
200 ext_dev->one_speed_que.speed_p = devm_kzalloc(dev, size, GFP_KERNEL);
201 if (!ext_dev->one_speed_que.speed_p)
202 return -ENOMEM;
203 ext_dev->one_speed_que.count = 1;
204 ext_dev->one_speed_que.speed_p[0].count = ext_dev->first_speed_step;
205 ext_dev->one_speed_que.speed_p[0].phase_interval_ns =
206 div_u64(NSEC_PER_SEC, ext_dev->start_up_speed);
207 switch (ext_dev->type) {
208 case TYPE_IRIS:
209 //max step is 80, needn't speed-up
210 return 0;
211 case TYPE_FOCUS:
212 prop = of_find_property(node, "focus-speed-up-table", &length_up);
213 if (prop)
214 length_up /= sizeof(u32);
215 if (length_up > 0) {
216 size = sizeof(*speed_up_table) * length_up;
217 speed_up_table = devm_kzalloc(dev, size, GFP_KERNEL);
218 if (!speed_up_table)
219 return -ENOMEM;
220 ret = of_property_read_u32_array(node, "focus-speed-up-table",
221 speed_up_table,
222 length_up);
223 if (ret < 0) {
224 dev_info(dev,
225 "fail to get speed table, used default speed!\n");
226 devm_kfree(dev, speed_up_table);
227 speed_up_table = NULL;
228 ext_dev->speed_queue_num = 1;
229 return 0;
230 }
231 dev_dbg(dev,
232 "dev tpype %d, speed-up table length %d, buf size %u\n",
233 ext_dev->type,
234 length_up,
235 size);
236 }
237 prop = of_find_property(node, "focus-speed-down-table", &length_down);
238 if (prop)
239 length_down /= sizeof(u32);
240 if (length_down > 0) {
241 size = sizeof(*speed_down_table) * length_down;
242 speed_down_table = devm_kzalloc(dev, size, GFP_KERNEL);
243 if (!speed_down_table)
244 return -ENOMEM;
245 ret = of_property_read_u32_array(node, "focus-speed-down-table",
246 speed_down_table,
247 length_down);
248 if (ret < 0) {
249 dev_info(dev,
250 "fail to get speed table, used default speed!\n");
251 devm_kfree(dev, speed_down_table);
252 speed_down_table = NULL;
253 } else {
254 dev_dbg(dev,
255 "dev tpype %d, speed-down table length %d, buf size %u\n",
256 ext_dev->type,
257 length_up,
258 size);
259 }
260 }
261 break;
262 case TYPE_ZOOM:
263 prop = of_find_property(node, "zoom-speed-up-table", &length_up);
264 if (prop)
265 length_up /= sizeof(u32);
266 if (length_up > 0) {
267 size = sizeof(*speed_up_table) * length_up;
268 speed_up_table = devm_kzalloc(dev, size, GFP_KERNEL);
269 if (!speed_up_table)
270 return -ENOMEM;
271 ret = of_property_read_u32_array(node, "zoom-speed-up-table",
272 speed_up_table,
273 length_up);
274 if (ret < 0) {
275 dev_info(dev,
276 "fail to get speed table, used default speed!\n");
277 ext_dev->speed_queue_num = 1;
278 devm_kfree(dev, speed_up_table);
279 speed_up_table = NULL;
280 return 0;
281 }
282 dev_dbg(dev,
283 "dev tpype %d, speed-up table length %d, buf size %u\n",
284 ext_dev->type,
285 length_up,
286 size);
287 }
288 prop = of_find_property(node, "zoom-speed-down-table", &length_down);
289 if (prop)
290 length_down /= sizeof(u32);
291 if (length_down > 0) {
292 size = sizeof(*speed_down_table) * length_down;
293 speed_down_table = devm_kzalloc(dev, size, GFP_KERNEL);
294 if (!speed_down_table)
295 return -ENOMEM;
296 ret = of_property_read_u32_array(node, "zoom-speed-down-table",
297 speed_down_table,
298 length_down);
299 if (ret < 0) {
300 dev_info(dev,
301 "fail to get speed table, used default speed!\n");
302 devm_kfree(dev, speed_down_table);
303 speed_down_table = NULL;
304 } else {
305 dev_dbg(dev,
306 "dev tpype %d, speed-down table length %d, buf size %u\n",
307 ext_dev->type,
308 length_up,
309 size);
310 }
311 }
312 break;
313 default:
314 return -EINVAL;
315 }
316 if (speed_up_table == NULL || speed_up_table[0] > ext_dev->start_up_speed ||
317 speed_up_table[length_up - 1] > ext_dev->max_speed) {
318 dev_info(dev,
319 "speed_up_table data error, not to used it!\n");
320 ext_dev->speed_queue_num = 1;
321 } else {
322 ext_dev->length_up = length_up;
323 if (speed_down_table != NULL)
324 ext_dev->speed_queue_num = length_up + length_down;
325 else
326 ext_dev->speed_queue_num = length_up * 2 - 1;
327 size = sizeof(*ext_dev->speed_que.speed_p) * ext_dev->speed_queue_num;
328 ext_dev->speed_que.speed_p = devm_kzalloc(dev, size, GFP_KERNEL);
329 if (!ext_dev->speed_que.speed_p)
330 return -ENOMEM;
331 for (i = 0; i < length_up - 1; i++) {
332 if (i == 0)
333 step_cnt = ext_dev->first_speed_step;
334 else
335 step_cnt =
336 ext_dev->first_speed_step *
337 speed_up_table[i] / speed_up_table[0];
338 step_cnt = (step_cnt + 3) / 4 * 4;
339 ext_dev->speed_que.speed_p[i].count = step_cnt;
340 ext_dev->speed_que.speed_p[i].phase_interval_ns =
341 div_u64(NSEC_PER_SEC, speed_up_table[i]);
342 step_total += step_cnt;
343 if (speed_down_table == NULL ||
344 speed_down_table[0] > speed_up_table[length_up - 1]) {
345 dev_info(dev,
346 "speed_down_table data error, used speed_up_table\n");
347 ext_dev->speed_que.speed_p[ext_dev->speed_queue_num - i - 1].count =
348 step_cnt;
349 ext_dev->speed_que.speed_p[ext_dev->speed_queue_num - i - 1].phase_interval_ns =
350 div_u64(NSEC_PER_SEC, speed_up_table[i]);
351 step_total += step_cnt;
352 }
353 dev_info(dev,
354 "index %d, speed %d, count %d\n",
355 i, speed_up_table[i], ext_dev->speed_que.speed_p[i].count);
356 }
357 ext_dev->speed_up_table = speed_up_table;
358
359 if (speed_down_table != NULL &&
360 speed_down_table[0] <= speed_up_table[length_up - 1]) {
361 for (i = 0; i < length_down; i++) {
362 step_cnt =
363 ext_dev->first_speed_step *
364 speed_down_table[i] / speed_up_table[0];
365 step_cnt = (step_cnt + 3) / 4 * 4;
366 ext_dev->speed_que.speed_p[length_up + i].count =
367 step_cnt;
368 ext_dev->speed_que.speed_p[length_up + i].phase_interval_ns =
369 div_u64(NSEC_PER_SEC, speed_down_table[i]);
370 step_total += step_cnt;
371 }
372 ext_dev->speed_down_table = speed_down_table;
373 ext_dev->length_down = length_down;
374 }
375 ext_dev->speed_up_step_cnt = step_total;
376
377 step_cnt =
378 ext_dev->first_speed_step *
379 speed_up_table[length_up - 1] / speed_up_table[0];
380 step_cnt = (step_cnt + 3) / 4 * 4;
381 ext_dev->speed_que.speed_p[length_up - 1].count = step_cnt;
382 ext_dev->speed_que.speed_p[length_up - 1].phase_interval_ns =
383 div_u64(NSEC_PER_SEC, speed_up_table[length_up - 1]);
384 step_total += step_cnt;
385
386 ext_dev->ths_speeded_up = step_total;
387 ext_dev->speed_que.count = ext_dev->speed_queue_num;
388 }
389 return 0;
390 }
391
motor_dev_parse_dt(struct motor_dev * motor)392 static int motor_dev_parse_dt(struct motor_dev *motor)
393 {
394 struct device_node *node = motor->dev->of_node;
395 int ret = 0;
396 int error = 0;
397
398 motor->pwm_a1 = devm_pwm_get(motor->dev, "ain1");
399 motor->pwm_a2 = devm_pwm_get(motor->dev, "ain2");
400 motor->pwm_b1 = devm_pwm_get(motor->dev, "bin1");
401 motor->pwm_b2 = devm_pwm_get(motor->dev, "bin2");
402
403 if (IS_ERR(motor->pwm_a1)) {
404 error = PTR_ERR(motor->pwm_a1);
405 if (error != -EPROBE_DEFER)
406 dev_err(motor->dev, "Failed to request PWM a1 device: %d\n", error);
407 return error;
408 }
409 if (IS_ERR(motor->pwm_a2)) {
410 error = PTR_ERR(motor->pwm_a2);
411 if (error != -EPROBE_DEFER)
412 dev_err(motor->dev, "Failed to request PWM a2 device: %d\n", error);
413 return error;
414 }
415 if (IS_ERR(motor->pwm_b1)) {
416 error = PTR_ERR(motor->pwm_b1);
417 if (error != -EPROBE_DEFER)
418 dev_err(motor->dev, "Failed to request PWM b1 device: %d\n", error);
419 return error;
420 }
421 if (IS_ERR(motor->pwm_b2)) {
422 error = PTR_ERR(motor->pwm_b2);
423 if (error != -EPROBE_DEFER)
424 dev_err(motor->dev, "Failed to request PWM b2 device: %d\n", error);
425 return error;
426 }
427
428 /* get iris_en gpio */
429 motor->iris.en_gpio = devm_gpiod_get(motor->dev,
430 "iris_en", GPIOD_OUT_LOW);
431 if (IS_ERR(motor->iris.en_gpio))
432 dev_err(motor->dev, "Failed to get iris_en-gpios\n");
433
434 /* get focus_en gpio */
435 motor->focus.en_gpio = devm_gpiod_get(motor->dev,
436 "focus_en", GPIOD_OUT_LOW);
437 if (IS_ERR(motor->focus.en_gpio))
438 dev_err(motor->dev, "Failed to get focus_en-gpios\n");
439
440 /* get zoom_en gpio */
441 motor->zoom.en_gpio = devm_gpiod_get(motor->dev,
442 "zoom_en", GPIOD_OUT_LOW);
443 if (IS_ERR(motor->zoom.en_gpio))
444 dev_err(motor->dev, "Failed to get zoom_en-gpios\n");
445
446 ret = of_property_read_u32(node,
447 "iris-step-max",
448 &motor->iris.step_max);
449 if (ret != 0) {
450 motor->iris.step_max = IRIS_MAX_STEP_DEF;
451 dev_err(motor->dev,
452 "failed get iris iris_pos_max,use dafult value 80\n");
453 }
454
455 ret = of_property_read_u32(node,
456 "focus-step-max",
457 &motor->focus.step_max);
458 if (ret != 0) {
459 motor->focus.step_max = FOCUS_MAX_STEP_DEF;
460 dev_err(motor->dev,
461 "failed get iris focus_pos_max,use dafult value 7500\n");
462 }
463
464 ret = of_property_read_u32(node,
465 "zoom-step-max",
466 &motor->zoom.step_max);
467 if (ret != 0) {
468 motor->zoom.step_max = ZOOM_MAX_STEP_DEF;
469 dev_err(motor->dev,
470 "failed get iris zoom_pos_max,use dafult value 7500\n");
471 }
472
473 ret = of_property_read_u32(node,
474 "iris-start-up-speed",
475 &motor->iris.start_up_speed);
476 if (ret != 0) {
477 motor->iris.start_up_speed = MAX_START_UP_HZ;
478 dev_err(motor->dev,
479 "failed get motor start up speed,use dafult value\n");
480 }
481 ret = of_property_read_u32(node,
482 "iris-max-speed",
483 &motor->iris.max_speed);
484 if (ret != 0) {
485 motor->iris.max_speed = MOTOR_MAX_HZ;
486 dev_err(motor->dev,
487 "failed get motor max speed,use dafult value\n");
488 }
489
490 ret = of_property_read_u32(node,
491 "focus-start-up-speed",
492 &motor->focus.start_up_speed);
493 if (ret != 0) {
494 motor->focus.start_up_speed = MAX_START_UP_HZ;
495 dev_err(motor->dev,
496 "failed get motor start up speed,use dafult value\n");
497 }
498 ret = of_property_read_u32(node,
499 "focus-max-speed",
500 &motor->focus.max_speed);
501 if (ret != 0) {
502 motor->focus.max_speed = MOTOR_MAX_HZ;
503 dev_err(motor->dev,
504 "failed get motor max speed,use dafult value\n");
505 }
506
507 ret = of_property_read_u32(node,
508 "zoom-start-up-speed",
509 &motor->zoom.start_up_speed);
510 if (ret != 0) {
511 motor->zoom.start_up_speed = MAX_START_UP_HZ;
512 dev_err(motor->dev,
513 "failed get motor start up speed,use dafult value\n");
514 }
515 ret = of_property_read_u32(node,
516 "zoom-max-speed",
517 &motor->zoom.max_speed);
518 if (ret != 0) {
519 motor->zoom.max_speed = MOTOR_MAX_HZ;
520 dev_err(motor->dev,
521 "failed get motor max speed,use dafult value\n");
522 }
523
524 ret = of_property_read_u32(node,
525 "focus-first-speed-step",
526 &motor->focus.first_speed_step);
527 if (ret != 0) {
528 motor->focus.first_speed_step = STEP_PER_SPEED_DEF;
529 dev_err(motor->dev,
530 "failed get motor step of first speed,use dafult value\n");
531 }
532 ret = of_property_read_u32(node,
533 "zoom-first-speed-step",
534 &motor->zoom.first_speed_step);
535 if (ret != 0) {
536 motor->zoom.first_speed_step = STEP_PER_SPEED_DEF;
537 dev_err(motor->dev,
538 "failed get motor step of first speed,use dafult value\n");
539 }
540
541 motor->iris.type = TYPE_IRIS;
542 ret = fill_speed_squeue(motor->dev, &motor->iris);
543 motor->focus.type = TYPE_FOCUS;
544 ret |= fill_speed_squeue(motor->dev, &motor->focus);
545 motor->zoom.type = TYPE_ZOOM;
546 ret |= fill_speed_squeue(motor->dev, &motor->zoom);
547
548 ret = of_property_read_u32(node, RKMODULE_CAMERA_MODULE_INDEX,
549 &motor->module_index);
550 ret |= of_property_read_string(node, RKMODULE_CAMERA_MODULE_FACING,
551 &motor->module_facing);
552 if (ret) {
553 dev_err(motor->dev,
554 "could not get module information!\n");
555 return -EINVAL;
556 }
557 return 0;
558 }
559
motor_timer_func(struct hrtimer * timer)560 static enum hrtimer_restart motor_timer_func(struct hrtimer *timer)
561 {
562 struct motor_dev *motor;
563 struct pwm_state *pwm_state;
564 int idx = 0;
565 u64 time_cnt = 0;
566
567 motor = container_of(timer, struct motor_dev, timer);
568 pwm_state = &motor->pwm_state;
569 if (motor->move_cnt < 1 || motor->move_status == MOTOR_STATUS_STOPPED) {
570 pwm_state->enabled = false;
571 pwm_apply_state(motor->pwm_b1, pwm_state);
572 pwm_apply_state(motor->pwm_b2, pwm_state);
573 pwm_apply_state(motor->pwm_a1, pwm_state);
574 pwm_apply_state(motor->pwm_a2, pwm_state);
575 if (!IS_ERR(motor->cur_ext_dev->en_gpio))
576 gpiod_set_value(motor->cur_ext_dev->en_gpio, 0);
577 motor->move_status = MOTOR_STATUS_STOPPED;
578 motor->resched = false;
579 complete(&motor->complete);
580 dev_dbg(motor->dev, "motor stop\n");
581
582 } else {
583 /* do phase change */
584 switch (motor->move_status) {
585 case MOTOR_STATUS_CW:
586 if (motor->resched == true) {
587 pwm_state->enabled = false;
588 pwm_apply_state(motor->pwm_b1, pwm_state);
589 pwm_apply_state(motor->pwm_b2, pwm_state);
590 pwm_apply_state(motor->pwm_a1, pwm_state);
591 pwm_apply_state(motor->pwm_a2, pwm_state);
592 }
593 idx = motor->run_queue->count - motor->move_cnt;
594 pwm_state->polarity = PWM_POLARITY_INVERSED;
595 pwm_state->enabled = true;
596 pwm_state->period =
597 motor->run_queue->speed_p[idx].phase_interval_ns * 4;
598 pwm_state->duty_cycle =
599 motor->run_queue->speed_p[idx].phase_interval_ns * 2;
600 pwm_apply_state(motor->pwm_b1, pwm_state);
601 pwm_state->enabled = true;
602 pwm_state->polarity = PWM_POLARITY_NORMAL;
603 pwm_apply_state(motor->pwm_b2, pwm_state);
604 pwm_state->polarity = PWM_POLARITY_NORMAL;
605 pwm_state->enabled = true;
606 pwm_apply_state(motor->pwm_a1, pwm_state);
607 pwm_state->polarity = PWM_POLARITY_INVERSED;
608 pwm_state->enabled = true;
609 pwm_apply_state(motor->pwm_a2, pwm_state);
610 break;
611 case MOTOR_STATUS_CCW:
612 if (motor->resched == true) {
613 pwm_state->enabled = false;
614 pwm_apply_state(motor->pwm_b1, pwm_state);
615 pwm_apply_state(motor->pwm_b2, pwm_state);
616 pwm_apply_state(motor->pwm_a1, pwm_state);
617 pwm_apply_state(motor->pwm_a2, pwm_state);
618 }
619 idx = motor->run_queue->count - motor->move_cnt;
620 pwm_state->polarity = PWM_POLARITY_INVERSED;
621 pwm_state->enabled = true;
622 pwm_state->period =
623 motor->run_queue->speed_p[idx].phase_interval_ns * 4;
624 pwm_state->duty_cycle =
625 motor->run_queue->speed_p[idx].phase_interval_ns * 2;
626 pwm_apply_state(motor->pwm_b1, pwm_state);
627 pwm_state->polarity = PWM_POLARITY_NORMAL;
628 pwm_state->enabled = true;
629 pwm_apply_state(motor->pwm_b2, pwm_state);
630 pwm_state->polarity = PWM_POLARITY_INVERSED;
631 pwm_state->enabled = true;
632 pwm_apply_state(motor->pwm_a1, pwm_state);
633 pwm_state->polarity = PWM_POLARITY_NORMAL;
634 pwm_state->enabled = true;
635 pwm_apply_state(motor->pwm_a2, pwm_state);
636 break;
637 default:
638 break;
639 }
640 if (motor->resched == false)
641 motor->resched = true;
642 motor->move_cnt--;
643 }
644 if (motor->resched) {
645 time_cnt = ((u64)motor->run_queue->speed_p[idx].phase_interval_ns *
646 motor->run_queue->speed_p[idx].count);
647 hrtimer_forward_now(timer,
648 ns_to_ktime(time_cnt - 80000));
649 return HRTIMER_RESTART;
650 }
651 return HRTIMER_NORESTART;
652 }
653
motor_s_ctrl(struct v4l2_ctrl * ctrl)654 static int motor_s_ctrl(struct v4l2_ctrl *ctrl)
655 {
656 int ret = 0;
657 struct motor_dev *motor = container_of(ctrl->handler,
658 struct motor_dev, ctrl_handler);
659
660 switch (ctrl->id) {
661 case V4L2_CID_IRIS_ABSOLUTE:
662 if (ctrl->val > motor->iris.cur_pos)
663 ret = set_motor_running_status(motor,
664 &motor->iris,
665 MOTOR_STATUS_CCW,
666 abs(ctrl->val - motor->iris.cur_pos));
667 else
668 ret = set_motor_running_status(motor,
669 &motor->iris,
670 MOTOR_STATUS_CW,
671 abs(ctrl->val - motor->iris.cur_pos));
672 motor->iris.cur_pos = ctrl->val;
673 dev_dbg(motor->dev, "set iris pos %d\n", ctrl->val);
674 break;
675 case V4L2_CID_FOCUS_ABSOLUTE:
676 if (ctrl->val > motor->focus.cur_pos)
677 ret = set_motor_running_status(motor,
678 &motor->focus,
679 MOTOR_STATUS_CCW,
680 abs(ctrl->val - motor->focus.cur_pos));
681 else
682 ret = set_motor_running_status(motor,
683 &motor->focus,
684 MOTOR_STATUS_CW,
685 abs(ctrl->val - motor->focus.cur_pos));
686 motor->focus.cur_pos = ctrl->val;
687 dev_dbg(motor->dev, "set focus pos %d\n", ctrl->val);
688 break;
689 case V4L2_CID_ZOOM_ABSOLUTE:
690 if (ctrl->val > motor->zoom.cur_pos)
691 ret = set_motor_running_status(motor,
692 &motor->zoom,
693 MOTOR_STATUS_CCW,
694 abs(ctrl->val - motor->zoom.cur_pos));
695 else
696 ret = set_motor_running_status(motor,
697 &motor->zoom,
698 MOTOR_STATUS_CW,
699 abs(ctrl->val - motor->zoom.cur_pos));
700 motor->zoom.cur_pos = ctrl->val;
701 dev_dbg(motor->dev, "set zoom pos %d\n", ctrl->val);
702 break;
703 default:
704 dev_err(motor->dev, "not support cmd %d\n", ctrl->id);
705 break;
706 }
707 return ret;
708 }
709
motor_init_iris_status(struct motor_dev * motor)710 static int motor_init_iris_status(struct motor_dev *motor)
711 {
712 int ret = 0;
713
714 ret = set_motor_running_status(motor, &motor->iris,
715 MOTOR_STATUS_CCW, IRIS_MAX_LOG);
716 motor->iris.cur_pos = IRIS_MAX_LOG;
717 __v4l2_ctrl_modify_range(motor->iris_ctrl,
718 0,
719 IRIS_MAX_LOG,
720 IRIS_LOG_STEP,
721 motor->iris.cur_pos);
722 return ret;
723 }
724
motor_init_focus_status(struct motor_dev * motor)725 static int motor_init_focus_status(struct motor_dev *motor)
726 {
727 int ret = 0;
728
729 ret = set_motor_running_status(motor, &motor->focus,
730 MOTOR_STATUS_CW, FOCUS_MAX_LOG);
731 motor->focus.cur_pos = 0;
732 __v4l2_ctrl_modify_range(motor->focus_ctrl,
733 0,
734 FOCUS_MAX_LOG,
735 FOCUS_LOG_STEP,
736 motor->focus.cur_pos);
737 return ret;
738 }
739
motor_init_zoom_status(struct motor_dev * motor)740 static int motor_init_zoom_status(struct motor_dev *motor)
741 {
742 int ret = 0;
743
744 ret = set_motor_running_status(motor, &motor->zoom,
745 MOTOR_STATUS_CW, ZOOM_MAX_LOG);
746 motor->zoom.cur_pos = 0;
747 __v4l2_ctrl_modify_range(motor->zoom_ctrl,
748 0,
749 ZOOM_MAX_LOG,
750 ZOOM_LOG_STEP,
751 motor->zoom.cur_pos);
752 return ret;
753 }
754
motor_ioctl(struct v4l2_subdev * sd,unsigned int cmd,void * arg)755 static long motor_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
756 {
757 struct rk_cam_vcm_tim *mv_tim;
758 struct motor_dev *motor = container_of(sd, struct motor_dev, sd);
759
760 switch (cmd) {
761 case RK_VIDIOC_VCM_TIMEINFO:
762 mv_tim = (struct rk_cam_vcm_tim *)arg;
763 memcpy(mv_tim, &motor->focus.mv_tim, sizeof(*mv_tim));
764
765 dev_dbg(motor->dev, "get_focus_move_tim 0x%lx, 0x%lx, 0x%lx, 0x%lx\n",
766 mv_tim->vcm_start_t.tv_sec,
767 mv_tim->vcm_start_t.tv_usec,
768 mv_tim->vcm_end_t.tv_sec,
769 mv_tim->vcm_end_t.tv_usec);
770 break;
771 case RK_VIDIOC_IRIS_TIMEINFO:
772 mv_tim = (struct rk_cam_vcm_tim *)arg;
773 memcpy(mv_tim, &motor->iris.mv_tim, sizeof(*mv_tim));
774
775 dev_dbg(motor->dev, "get_iris_move_tim 0x%lx, 0x%lx, 0x%lx, 0x%lx\n",
776 mv_tim->vcm_start_t.tv_sec,
777 mv_tim->vcm_start_t.tv_usec,
778 mv_tim->vcm_end_t.tv_sec,
779 mv_tim->vcm_end_t.tv_usec);
780 break;
781 case RK_VIDIOC_ZOOM_TIMEINFO:
782 mv_tim = (struct rk_cam_vcm_tim *)arg;
783 memcpy(mv_tim, &motor->zoom.mv_tim, sizeof(*mv_tim));
784
785 dev_dbg(motor->dev, "get_zoom_move_tim 0x%lx, 0x%lx, 0x%lx, 0x%lx\n",
786 mv_tim->vcm_start_t.tv_sec,
787 mv_tim->vcm_start_t.tv_usec,
788 mv_tim->vcm_end_t.tv_sec,
789 mv_tim->vcm_end_t.tv_usec);
790 break;
791 case RK_VIDIOC_IRIS_CORRECTION:
792 motor_init_iris_status(motor);
793 break;
794 case RK_VIDIOC_FOCUS_CORRECTION:
795 motor_init_focus_status(motor);
796 break;
797 case RK_VIDIOC_ZOOM_CORRECTION:
798 motor_init_zoom_status(motor);
799 break;
800 default:
801 break;
802 }
803 return 0;
804 }
805
806 static const struct v4l2_subdev_core_ops motor_core_ops = {
807 .ioctl = motor_ioctl,
808 };
809
810 static const struct v4l2_subdev_ops motor_subdev_ops = {
811 .core = &motor_core_ops,
812 };
813
814 static const struct v4l2_ctrl_ops motor_ctrl_ops = {
815 .s_ctrl = motor_s_ctrl,
816 };
817
motor_initialize_controls(struct motor_dev * motor)818 static int motor_initialize_controls(struct motor_dev *motor)
819 {
820 struct v4l2_ctrl_handler *handler;
821 int ret = 0;
822
823 handler = &motor->ctrl_handler;
824 ret = v4l2_ctrl_handler_init(handler, 3);
825 if (ret)
826 return ret;
827 handler->lock = &motor->mutex;
828 if (!IS_ERR(motor->iris.en_gpio)) {
829 motor->iris_ctrl = v4l2_ctrl_new_std(handler, &motor_ctrl_ops,
830 V4L2_CID_IRIS_ABSOLUTE, 0, IRIS_MAX_LOG, IRIS_LOG_STEP, 0);
831
832 ret = motor_init_iris_status(motor);
833 }
834 if (!IS_ERR(motor->focus.en_gpio)) {
835 motor->focus_ctrl = v4l2_ctrl_new_std(handler, &motor_ctrl_ops,
836 V4L2_CID_FOCUS_ABSOLUTE, 0, FOCUS_MAX_LOG,
837 FOCUS_LOG_STEP, 0);
838 }
839 if (!IS_ERR(motor->zoom.en_gpio)) {
840 motor->zoom_ctrl = v4l2_ctrl_new_std(handler, &motor_ctrl_ops,
841 V4L2_CID_ZOOM_ABSOLUTE, 0, ZOOM_MAX_LOG,
842 ZOOM_LOG_STEP, 0);
843 }
844 if (handler->error) {
845 ret = handler->error;
846 dev_err(motor->dev,
847 "Failed to init controls(%d)\n", ret);
848 goto err_free_handler;
849 }
850
851 motor->sd.ctrl_handler = handler;
852 return ret;
853
854 err_free_handler:
855 v4l2_ctrl_handler_free(handler);
856
857 return ret;
858 }
859 #define USED_SYS_DEBUG
860 #ifdef USED_SYS_DEBUG
861
set_iris_correction(struct device * dev,struct device_attribute * attr,const char * buf,size_t count)862 static ssize_t set_iris_correction(struct device *dev,
863 struct device_attribute *attr,
864 const char *buf,
865 size_t count)
866 {
867 struct motor_dev *motor = dev_get_drvdata(dev);
868 int status = 0;
869 int ret = 0;
870
871 ret = kstrtoint(buf, 0, &status);
872 if (!ret && status >= 0 && status < 2) {
873 if (status)
874 motor_init_iris_status(motor);
875 dev_info(dev, "camera iris position correction\n");
876 }
877 return count;
878 }
879
set_focus_correction(struct device * dev,struct device_attribute * attr,const char * buf,size_t count)880 static ssize_t set_focus_correction(struct device *dev,
881 struct device_attribute *attr,
882 const char *buf,
883 size_t count)
884 {
885 struct motor_dev *motor = dev_get_drvdata(dev);
886 int status = 0;
887 int ret = 0;
888
889 ret = kstrtoint(buf, 0, &status);
890 if (!ret && status >= 0 && status < 2) {
891 if (status)
892 motor_init_focus_status(motor);
893 dev_info(dev, "camera focus position correction\n");
894 }
895 return count;
896 }
897
set_zoom_correction(struct device * dev,struct device_attribute * attr,const char * buf,size_t count)898 static ssize_t set_zoom_correction(struct device *dev,
899 struct device_attribute *attr,
900 const char *buf,
901 size_t count)
902 {
903 struct motor_dev *motor = dev_get_drvdata(dev);
904 int status = 0;
905 int ret = 0;
906
907 ret = kstrtoint(buf, 0, &status);
908 if (!ret && status >= 0 && status < 2) {
909 if (status)
910 motor_init_zoom_status(motor);
911 dev_info(dev, "camera zoom position correction\n");
912 }
913 return count;
914 }
915
916 static struct device_attribute attributes[] = {
917 __ATTR(is_iris_correction, S_IWUSR, NULL, set_iris_correction),
918 __ATTR(is_focus_correction, S_IWUSR, NULL, set_focus_correction),
919 __ATTR(is_zoom_correction, S_IWUSR, NULL, set_zoom_correction),
920 };
921
add_sysfs_interfaces(struct device * dev)922 static int add_sysfs_interfaces(struct device *dev)
923 {
924 int i;
925
926 for (i = 0; i < ARRAY_SIZE(attributes); i++)
927 if (device_create_file(dev, attributes + i))
928 goto undo;
929 return 0;
930 undo:
931 for (i--; i >= 0 ; i--)
932 device_remove_file(dev, attributes + i);
933 dev_err(dev, "%s: failed to create sysfs interface\n", __func__);
934 return -ENODEV;
935 }
936 #endif
937
dev_param_init(struct motor_dev * motor)938 static void dev_param_init(struct motor_dev *motor)
939 {
940
941 motor->iris.step_per_pos = motor->iris.step_max / IRIS_MAX_LOG;
942 motor->iris.mv_tim.vcm_start_t = ns_to_timeval(ktime_get_ns());
943 motor->iris.mv_tim.vcm_end_t = ns_to_timeval(ktime_get_ns());
944
945 motor->focus.step_per_pos = motor->focus.step_max / FOCUS_MAX_LOG;
946 motor->focus.mv_tim.vcm_start_t = ns_to_timeval(ktime_get_ns());
947 motor->focus.mv_tim.vcm_end_t = ns_to_timeval(ktime_get_ns());
948
949 motor->zoom.step_per_pos = motor->zoom.step_max / ZOOM_MAX_LOG;
950 motor->zoom.mv_tim.vcm_start_t = ns_to_timeval(ktime_get_ns());
951 motor->zoom.mv_tim.vcm_end_t = ns_to_timeval(ktime_get_ns());
952
953 motor->move_status = MOTOR_STATUS_STOPPED;
954 motor->resched = false;
955 }
956
motor_dev_probe(struct platform_device * pdev)957 static int motor_dev_probe(struct platform_device *pdev)
958 {
959 int ret = 0;
960 struct motor_dev *motor;
961 struct v4l2_subdev *sd;
962 char facing[2];
963
964 dev_info(&pdev->dev, "driver version: %02x.%02x.%02x",
965 DRIVER_VERSION >> 16,
966 (DRIVER_VERSION & 0xff00) >> 8,
967 DRIVER_VERSION & 0x00ff);
968 motor = devm_kzalloc(&pdev->dev, sizeof(*motor), GFP_KERNEL);
969 if (!motor)
970 return -ENOMEM;
971 motor->dev = &pdev->dev;
972 dev_set_name(motor->dev, "motor");
973 dev_set_drvdata(motor->dev, motor);
974 if (motor_dev_parse_dt(motor)) {
975 dev_err(motor->dev, "parse dt error\n");
976 return -EINVAL;
977 }
978 dev_param_init(motor);
979 mutex_init(&motor->mutex);
980 hrtimer_init(&motor->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
981 motor->timer.function = motor_timer_func;
982 init_completion(&motor->complete);
983 v4l2_subdev_init(&motor->sd, &motor_subdev_ops);
984 motor->sd.owner = pdev->dev.driver->owner;
985 motor->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
986 motor->sd.dev = &pdev->dev;
987 v4l2_set_subdevdata(&motor->sd, pdev);
988 platform_set_drvdata(pdev, &motor->sd);
989 motor_initialize_controls(motor);
990 if (ret)
991 goto err_free;
992 ret = media_entity_pads_init(&motor->sd.entity, 0, NULL);
993 if (ret < 0)
994 goto err_free;
995 sd = &motor->sd;
996 sd->entity.function = MEDIA_ENT_F_LENS;
997 sd->entity.flags = 0;
998
999 memset(facing, 0, sizeof(facing));
1000 if (strcmp(motor->module_facing, "back") == 0)
1001 facing[0] = 'b';
1002 else
1003 facing[0] = 'f';
1004 snprintf(sd->name, sizeof(sd->name), "m%02d_%s_%s",
1005 motor->module_index, facing,
1006 DRIVER_NAME);
1007 ret = v4l2_async_register_subdev(sd);
1008 if (ret)
1009 dev_err(&pdev->dev, "v4l2 async register subdev failed\n");
1010 #ifdef USED_SYS_DEBUG
1011 add_sysfs_interfaces(&pdev->dev);
1012 #endif
1013 dev_info(motor->dev, "gpio motor driver probe success\n");
1014 return 0;
1015 err_free:
1016 v4l2_ctrl_handler_free(&motor->ctrl_handler);
1017 v4l2_device_unregister_subdev(&motor->sd);
1018 media_entity_cleanup(&motor->sd.entity);
1019 return ret;
1020 }
1021
motor_dev_remove(struct platform_device * pdev)1022 static int motor_dev_remove(struct platform_device *pdev)
1023 {
1024 struct v4l2_subdev *sd = platform_get_drvdata(pdev);
1025 struct motor_dev *motor;
1026
1027 if (sd)
1028 motor = v4l2_get_subdevdata(sd);
1029 else
1030 return -ENODEV;
1031 hrtimer_cancel(&motor->timer);
1032 if (sd)
1033 v4l2_device_unregister_subdev(sd);
1034 v4l2_ctrl_handler_free(&motor->ctrl_handler);
1035 media_entity_cleanup(&motor->sd.entity);
1036 return 0;
1037 }
1038
1039 #if defined(CONFIG_OF)
1040 static const struct of_device_id motor_dev_of_match[] = {
1041 { .compatible = "monolithicpower,mp6507", },
1042 {},
1043 };
1044 #endif
1045
1046 static struct platform_driver motor_dev_driver = {
1047 .driver = {
1048 .name = DRIVER_NAME,
1049 .owner = THIS_MODULE,
1050 .of_match_table = of_match_ptr(motor_dev_of_match),
1051 },
1052 .probe = motor_dev_probe,
1053 .remove = motor_dev_remove,
1054 };
1055
1056 module_platform_driver(motor_dev_driver);
1057
1058 MODULE_LICENSE("GPL");
1059 MODULE_ALIAS("platform:motor");
1060 MODULE_AUTHOR("ROCKCHIP");
1061