// SPDX-License-Identifier: GPL-2.0 /* * motor driver * * Copyright (C) 2020 Rockchip Electronics Co., Ltd. * */ //#define DEBUG #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #define DRIVER_VERSION KERNEL_VERSION(0, 0x01, 0x00) #define DRIVER_NAME "fp5501" #define MAX_START_UP_HZ (500) #define FOCUS_MAX_STEP_DEF 7500 #define ZOOM_MAX_STEP_DEF 7500 #define FOCUS_MAX_BACK_DELAY 4 #define ZOOM_MAX_BACK_DELAY 4 #define MOTOR_PHASE_PINS (4) #define PHASE_TYPES (4) static u8 phases_cw[PHASE_TYPES] = {0x09, 0x0c, 0x06, 0x03}; enum { MOTOR_STATUS_STOPPED = 0, MOTOR_STATUS_CW = 1, MOTOR_STATUS_CCW = 2, }; enum ext_dev_type { TYPE_FOCUS = 1, TYPE_ZOOM = 2, }; struct ext_dev { u8 type; u32 step_max; u32 last_pos; u32 step_cnt; u32 cur_step_cnt; u32 move_status; u32 last_status; u32 reback; u32 reback_status; u32 phase_index; u32 cur_back_delay; u32 max_back_delay; bool is_need_reback; bool is_need_update_tim; bool is_running; bool is_dir_opp; bool reback_ctrl; struct rk_cam_vcm_tim mv_tim; struct completion comp_in; struct completion comp_out; struct gpio_desc *phase_gpios[MOTOR_PHASE_PINS]; }; struct motor_dev { struct v4l2_subdev sd; struct v4l2_ctrl_handler ctrl_handler; struct v4l2_ctrl *focus_ctrl; struct v4l2_ctrl *zoom_ctrl; struct device *dev; struct hrtimer timer; u64 phase_interval_ns; u32 start_up_speed; u32 module_index; bool is_timer_restart; const char *module_facing; spinlock_t spin_lock; struct ext_dev focus; struct ext_dev zoom; }; static int set_motor_running_status(struct motor_dev *motor, struct ext_dev *ext_dev, u32 pos, bool is_need_reback) { int ret = 0; u32 mv_cnt = 0; int status = 0; unsigned long lock_flags = 0; if (ext_dev->move_status != MOTOR_STATUS_STOPPED) wait_for_completion(&ext_dev->comp_in); mv_cnt = abs(pos - ext_dev->last_pos); if (is_need_reback) mv_cnt += ext_dev->reback; if (mv_cnt == 0) return 0; reinit_completion(&ext_dev->comp_in); if (ext_dev->is_dir_opp) { if (pos > ext_dev->last_pos) status = MOTOR_STATUS_CCW; else status = MOTOR_STATUS_CW; } else { if (pos > ext_dev->last_pos) status = MOTOR_STATUS_CW; else status = MOTOR_STATUS_CCW; } if (ext_dev->last_status != MOTOR_STATUS_STOPPED && ext_dev->last_status != status) msleep(130); ext_dev->last_status = status; ext_dev->last_pos = pos; spin_lock_irqsave(&motor->spin_lock, lock_flags); ext_dev->move_status = status; ext_dev->is_need_reback = is_need_reback; ext_dev->step_cnt = mv_cnt + 1; ext_dev->cur_step_cnt = ext_dev->step_cnt; ext_dev->is_need_update_tim = true; if (motor->is_timer_restart == false) { motor->is_timer_restart = true; spin_unlock_irqrestore(&motor->spin_lock, lock_flags); hrtimer_start(&motor->timer, ktime_set(0, 0), HRTIMER_MODE_REL_PINNED); } else { spin_unlock_irqrestore(&motor->spin_lock, lock_flags); } ext_dev->is_running = true; dev_dbg(motor->dev, "ext tpye %d, mv_cnt %d, status %d, is_need_reback %d, reback %d, reback status %d\n", ext_dev->type, ext_dev->cur_step_cnt, status, is_need_reback, ext_dev->reback, ext_dev->reback_status); return ret; } static void motor_gpio_set_phase(struct ext_dev *dev, int phase_index) { int i; int ctrl = phases_cw[phase_index]; for (i = 0; i < MOTOR_PHASE_PINS; i++) gpiod_set_value(dev->phase_gpios[i], (ctrl >> i) & 0x1); } static void motor_gpio_set_stop(struct ext_dev *dev) { int i = 0; if (dev->is_running) { for (i = 0; i < MOTOR_PHASE_PINS; i++) gpiod_set_value(dev->phase_gpios[i], 0); } } static void motor_set_config(struct ext_dev *dev) { int phase_types = PHASE_TYPES; if (dev->cur_step_cnt != 0) { if (dev->step_cnt == dev->cur_step_cnt) { if (dev->is_need_update_tim) { dev->mv_tim.vcm_start_t = ns_to_timeval(ktime_get_ns()); dev->is_need_update_tim = false; } } else { if (dev->move_status == MOTOR_STATUS_CW) dev->phase_index = (dev->phase_index + 1) % phase_types; else dev->phase_index = (dev->phase_index - 1) % phase_types; } motor_gpio_set_phase(dev, dev->phase_index); dev->cur_step_cnt--; } else { if (dev->is_need_reback) { if (dev->cur_back_delay < dev->max_back_delay) { if (dev->cur_back_delay == 0) motor_gpio_set_stop(dev); dev->cur_back_delay++; } else { dev->cur_step_cnt = dev->reback + 1; dev->step_cnt = dev->reback + 1; dev->move_status = dev->reback_status; dev->last_status = dev->reback_status; dev->cur_back_delay = 0; dev->is_need_reback = false; } } else { motor_gpio_set_stop(dev); dev->move_status = MOTOR_STATUS_STOPPED; dev->mv_tim.vcm_end_t = ns_to_timeval(ktime_get_ns()); dev->is_running = false; complete(&dev->comp_out); complete(&dev->comp_in); } } } static enum hrtimer_restart motor_timer_func(struct hrtimer *timer) { struct motor_dev *motor; unsigned long lock_flags = 0; static struct timeval tv_last = {0}; struct timeval tv = {0}; u64 time_dist = 0; motor = container_of(timer, struct motor_dev, timer); do_gettimeofday(&tv); time_dist = tv.tv_sec * 1000000 + tv.tv_usec - (tv_last.tv_sec * 1000000 + tv_last.tv_usec); tv_last = tv; if (abs(time_dist * 1000 - motor->phase_interval_ns) > 250000) dev_dbg(motor->dev, "focus cnt %d, zoom cnt %d, Current interrupt interval %llu\n", motor->focus.cur_step_cnt, motor->zoom.cur_step_cnt, time_dist); spin_lock_irqsave(&motor->spin_lock, lock_flags); if (motor->focus.move_status != MOTOR_STATUS_STOPPED) motor_set_config(&motor->focus); if (motor->zoom.move_status != MOTOR_STATUS_STOPPED) motor_set_config(&motor->zoom); if (motor->focus.move_status == MOTOR_STATUS_STOPPED && motor->zoom.move_status == MOTOR_STATUS_STOPPED) motor->is_timer_restart = false; if (motor->is_timer_restart) { spin_unlock_irqrestore(&motor->spin_lock, lock_flags); do_gettimeofday(&tv); time_dist = tv.tv_sec * 1000000 + tv.tv_usec - (tv_last.tv_sec * 1000000 + tv_last.tv_usec); hrtimer_forward_now(timer, ns_to_ktime(motor->phase_interval_ns + (time_dist * 1000))); return HRTIMER_RESTART; } else { spin_unlock_irqrestore(&motor->spin_lock, lock_flags); } return HRTIMER_NORESTART; } static void wait_for_motor_stop(struct motor_dev *motor, struct ext_dev *dev) { unsigned long ret = 0; if (dev->is_running) { reinit_completion(&dev->comp_out); ret = wait_for_completion_timeout(&dev->comp_out, 16 * HZ); if (ret == 0) dev_info(motor->dev, "wait for complete timeout\n"); } } static int motor_s_ctrl(struct v4l2_ctrl *ctrl) { int ret = 0; struct motor_dev *motor = container_of(ctrl->handler, struct motor_dev, ctrl_handler); bool is_need_reback = false; switch (ctrl->id) { case V4L2_CID_FOCUS_ABSOLUTE: if (motor->focus.reback_ctrl) { if (ctrl->val >= motor->focus.last_pos) is_need_reback = false; else is_need_reback = true; } ret = set_motor_running_status(motor, &motor->focus, ctrl->val, is_need_reback); wait_for_motor_stop(motor, &motor->focus); dev_dbg(motor->dev, "set focus pos %d\n", ctrl->val); break; case V4L2_CID_ZOOM_ABSOLUTE: if (motor->zoom.reback_ctrl) { if (ctrl->val >= motor->zoom.last_pos) is_need_reback = false; else is_need_reback = true; } ret = set_motor_running_status(motor, &motor->zoom, ctrl->val, is_need_reback); wait_for_motor_stop(motor, &motor->zoom); dev_dbg(motor->dev, "set zoom pos %d\n", ctrl->val); break; default: dev_err(motor->dev, "not support cmd %d\n", ctrl->id); break; } return ret; } static int motor_set_zoom_follow(struct motor_dev *motor, struct rk_cam_set_zoom *mv_param) { int i = 0; bool is_need_zoom_reback = mv_param->is_need_zoom_reback; bool is_need_focus_reback = mv_param->is_need_focus_reback; for (i = 0; i < mv_param->setzoom_cnt; i++) { if (i == (mv_param->setzoom_cnt - 1)) { set_motor_running_status(motor, &motor->focus, mv_param->zoom_pos[i].focus_pos, is_need_focus_reback); set_motor_running_status(motor, &motor->zoom, mv_param->zoom_pos[i].zoom_pos, is_need_zoom_reback); } else { set_motor_running_status(motor, &motor->focus, mv_param->zoom_pos[i].focus_pos, false); set_motor_running_status(motor, &motor->zoom, mv_param->zoom_pos[i].zoom_pos, false); } dev_dbg(motor->dev, "%s zoom %d, focus %d, i %d\n", __func__, mv_param->zoom_pos[i].zoom_pos, mv_param->zoom_pos[i].focus_pos, i); } wait_for_motor_stop(motor, &motor->focus); wait_for_motor_stop(motor, &motor->zoom); return 0; } static int motor_set_focus(struct motor_dev *motor, struct rk_cam_set_focus *mv_param) { int ret = 0; bool is_need_reback = mv_param->is_need_reback; #ifdef REBACK_CTRL_BY_DRV if (mv_param->focus_pos >= motor->focus.last_pos) is_need_reback = false; else is_need_reback = true; #endif ret = set_motor_running_status(motor, &motor->focus, mv_param->focus_pos, is_need_reback); wait_for_motor_stop(motor, &motor->focus); return ret; } static int motor_reinit_focus(struct motor_dev *motor) { int ret = 0; motor->focus.last_pos = motor->focus.step_max; ret = set_motor_running_status(motor, &motor->focus, 0, true); wait_for_motor_stop(motor, &motor->focus); return ret; } static void motor_reinit_focus_pos(struct motor_dev *motor) { motor_reinit_focus(motor); motor->focus.last_pos = 0; motor->focus.phase_index = 0; __v4l2_ctrl_modify_range(motor->focus_ctrl, 0, motor->focus.step_max - motor->focus.reback, 1, 0); } static int motor_reinit_zoom(struct motor_dev *motor) { int ret = 0; motor->zoom.last_pos = motor->zoom.step_max; ret = set_motor_running_status(motor, &motor->zoom, 0, true); wait_for_motor_stop(motor, &motor->zoom); return ret; } static void motor_reinit_zoom_pos(struct motor_dev *motor) { motor_reinit_zoom(motor); motor->zoom.last_pos = 0; motor->zoom.phase_index = 0; __v4l2_ctrl_modify_range(motor->zoom_ctrl, 0, motor->zoom.step_max - motor->zoom.reback, 1, 0); } static long motor_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg) { struct rk_cam_vcm_tim *mv_tim; struct motor_dev *motor = container_of(sd, struct motor_dev, sd); struct rk_cam_set_zoom *mv_param; struct rk_cam_set_focus *focus_param; struct rk_cam_modify_pos *pos; switch (cmd) { case RK_VIDIOC_VCM_TIMEINFO: mv_tim = (struct rk_cam_vcm_tim *)arg; memcpy(mv_tim, &motor->focus.mv_tim, sizeof(*mv_tim)); dev_dbg(motor->dev, "get_focus_move_tim 0x%lx, 0x%lx, 0x%lx, 0x%lx\n", mv_tim->vcm_start_t.tv_sec, mv_tim->vcm_start_t.tv_usec, mv_tim->vcm_end_t.tv_sec, mv_tim->vcm_end_t.tv_usec); break; case RK_VIDIOC_ZOOM_TIMEINFO: mv_tim = (struct rk_cam_vcm_tim *)arg; memcpy(mv_tim, &motor->zoom.mv_tim, sizeof(*mv_tim)); dev_dbg(motor->dev, "get_zoom_move_tim 0x%lx, 0x%lx, 0x%lx, 0x%lx\n", mv_tim->vcm_start_t.tv_sec, mv_tim->vcm_start_t.tv_usec, mv_tim->vcm_end_t.tv_sec, mv_tim->vcm_end_t.tv_usec); break; case RK_VIDIOC_FOCUS_CORRECTION: motor_reinit_focus_pos(motor); break; case RK_VIDIOC_ZOOM_CORRECTION: motor_reinit_zoom_pos(motor); break; case RK_VIDIOC_ZOOM_SET_POSITION: mv_param = (struct rk_cam_set_zoom *)arg; motor_set_zoom_follow(motor, mv_param); break; case RK_VIDIOC_FOCUS_SET_POSITION: focus_param = (struct rk_cam_set_focus *)arg; motor_set_focus(motor, focus_param); break; case RK_VIDIOC_MODIFY_POSITION: pos = (struct rk_cam_modify_pos *)arg; motor->focus.last_pos = pos->focus_pos; motor->zoom.last_pos = pos->zoom_pos; break; default: break; } return 0; } #ifdef CONFIG_COMPAT static long motor_compat_ioctl32(struct v4l2_subdev *sd, unsigned int cmd, unsigned long arg) { void __user *up = compat_ptr(arg); struct rk_cam_vcm_tim *mv_tim; int ret = 0; u32 val = 0; struct rk_cam_set_zoom *mv_param; struct rk_cam_set_focus *focus_param; struct rk_cam_modify_pos *pos; switch (cmd) { case RK_VIDIOC_VCM_TIMEINFO: case RK_VIDIOC_ZOOM_TIMEINFO: mv_tim = kzalloc(sizeof(*mv_tim), GFP_KERNEL); if (!mv_tim) { ret = -ENOMEM; return ret; } ret = motor_ioctl(sd, cmd, mv_tim); if (!ret) { if (copy_to_user(up, mv_tim, sizeof(*mv_tim))) { kfree(mv_tim); return -EFAULT; } } kfree(mv_tim); break; case RK_VIDIOC_FOCUS_CORRECTION: case RK_VIDIOC_ZOOM_CORRECTION: if (copy_from_user(&val, up, sizeof(val))) return -EFAULT; ret = motor_ioctl(sd, cmd, &val); break; case RK_VIDIOC_ZOOM_SET_POSITION: mv_param = kzalloc(sizeof(*mv_param), GFP_KERNEL); if (!mv_param) { ret = -ENOMEM; return ret; } if (copy_from_user(mv_param, up, sizeof(*mv_param))) { kfree(mv_param); return -EFAULT; } ret = motor_ioctl(sd, cmd, mv_param); kfree(mv_param); break; case RK_VIDIOC_FOCUS_SET_POSITION: focus_param = kzalloc(sizeof(*focus_param), GFP_KERNEL); if (!focus_param) { ret = -ENOMEM; return ret; } if (copy_from_user(focus_param, up, sizeof(*focus_param))) { kfree(focus_param); return -EFAULT; } ret = motor_ioctl(sd, cmd, focus_param); kfree(focus_param); break; case RK_VIDIOC_MODIFY_POSITION: pos = kzalloc(sizeof(*pos), GFP_KERNEL); if (!pos) { ret = -ENOMEM; return ret; } if (copy_from_user(pos, up, sizeof(*pos))) { kfree(pos); return -EFAULT; } ret = motor_ioctl(sd, cmd, pos); kfree(pos); break; default: break; } return ret; } #endif static int motor_g_volatile_ctrl(struct v4l2_ctrl *ctrl) { struct motor_dev *motor = container_of(ctrl->handler, struct motor_dev, ctrl_handler); switch (ctrl->id) { case V4L2_CID_FOCUS_ABSOLUTE: ctrl->val = motor->focus.last_pos; return 0; case V4L2_CID_ZOOM_ABSOLUTE: ctrl->val = motor->zoom.last_pos; return 0; } return 0; } static const struct v4l2_subdev_core_ops motor_core_ops = { .ioctl = motor_ioctl, #ifdef CONFIG_COMPAT .compat_ioctl32 = motor_compat_ioctl32 #endif }; static const struct v4l2_subdev_ops motor_subdev_ops = { .core = &motor_core_ops, }; static const struct v4l2_ctrl_ops motor_ctrl_ops = { .g_volatile_ctrl = motor_g_volatile_ctrl, .s_ctrl = motor_s_ctrl, }; static int motor_initialize_controls(struct motor_dev *motor) { struct v4l2_ctrl_handler *handler; int ret = 0; unsigned long flags = V4L2_CTRL_FLAG_EXECUTE_ON_WRITE | V4L2_CTRL_FLAG_VOLATILE; handler = &motor->ctrl_handler; ret = v4l2_ctrl_handler_init(handler, 2); if (ret) return ret; #ifdef REINIT_BOOT ret = motor_reinit_focus(motor); if (ret < 0) return -EINVAL; #endif motor->focus.last_pos = 0; motor->focus_ctrl = v4l2_ctrl_new_std(handler, &motor_ctrl_ops, V4L2_CID_FOCUS_ABSOLUTE, 0, motor->focus.step_max - motor->focus.reback, 1, 0); if (motor->focus_ctrl) motor->focus_ctrl->flags |= flags; #ifdef REINIT_BOOT ret = motor_reinit_zoom(motor); if (ret < 0) return -EINVAL; #endif motor->zoom.last_pos = 0; motor->zoom_ctrl = v4l2_ctrl_new_std(handler, &motor_ctrl_ops, V4L2_CID_ZOOM_ABSOLUTE, 0, motor->zoom.step_max - motor->zoom.reback, 1, 0); if (motor->zoom_ctrl) motor->zoom_ctrl->flags |= flags; if (handler->error) { ret = handler->error; dev_err(motor->dev, "Failed to init controls(%d)\n", ret); goto err_free_handler; } motor->sd.ctrl_handler = handler; return ret; err_free_handler: v4l2_ctrl_handler_free(handler); return ret; } #define USED_SYS_DEBUG #ifdef USED_SYS_DEBUG static ssize_t reinit_focus_pos(struct device *dev, struct device_attribute *attr, const char *buf, size_t count) { struct v4l2_subdev *sd = dev_get_drvdata(dev); struct motor_dev *motor = container_of(sd, struct motor_dev, sd); int val = 0; int ret = 0; ret = kstrtoint(buf, 0, &val); if (!ret) { if (val == 1) motor_reinit_focus_pos(motor); } return count; } static ssize_t reinit_zoom_pos(struct device *dev, struct device_attribute *attr, const char *buf, size_t count) { struct v4l2_subdev *sd = dev_get_drvdata(dev); struct motor_dev *motor = container_of(sd, struct motor_dev, sd); int val = 0; int ret = 0; ret = kstrtoint(buf, 0, &val); if (!ret) { if (val == 1) motor_reinit_zoom_pos(motor); } return count; } static ssize_t set_focus_reback_ctrl(struct device *dev, struct device_attribute *attr, const char *buf, size_t count) { struct v4l2_subdev *sd = dev_get_drvdata(dev); struct motor_dev *motor = container_of(sd, struct motor_dev, sd); int val = 0; int ret = 0; ret = kstrtoint(buf, 0, &val); if (!ret) { if (val == 1) motor->focus.reback_ctrl = true; else motor->focus.reback_ctrl = false; } return count; } static ssize_t set_zoom_reback_ctrl(struct device *dev, struct device_attribute *attr, const char *buf, size_t count) { struct v4l2_subdev *sd = dev_get_drvdata(dev); struct motor_dev *motor = container_of(sd, struct motor_dev, sd); int val = 0; int ret = 0; ret = kstrtoint(buf, 0, &val); if (!ret) { if (val == 1) motor->zoom.reback_ctrl = true; else motor->zoom.reback_ctrl = false; } return count; } static ssize_t set_motor_speed(struct device *dev, struct device_attribute *attr, const char *buf, size_t count) { struct v4l2_subdev *sd = dev_get_drvdata(dev); struct motor_dev *motor = container_of(sd, struct motor_dev, sd); int val = 0; int ret = 0; ret = kstrtoint(buf, 0, &val); if (!ret) { if (val < 100 || val > 600) dev_info(motor->dev, "min speed 100pps, max speed 600pps, cur speed %d pps\n", val); else motor->phase_interval_ns = div_u64(NSEC_PER_SEC, val); } return count; } static struct device_attribute attributes[] = { __ATTR(reinit_focus, S_IWUSR, NULL, reinit_focus_pos), __ATTR(reinit_zoom, S_IWUSR, NULL, reinit_zoom_pos), __ATTR(focus_reback_ctrl, S_IWUSR, NULL, set_focus_reback_ctrl), __ATTR(zoom_reback_ctrl, S_IWUSR, NULL, set_zoom_reback_ctrl), __ATTR(speed, S_IWUSR, NULL, set_motor_speed), }; static int add_sysfs_interfaces(struct device *dev) { int i; for (i = 0; i < ARRAY_SIZE(attributes); i++) if (device_create_file(dev, attributes + i)) goto undo; return 0; undo: for (i--; i >= 0 ; i--) device_remove_file(dev, attributes + i); dev_err(dev, "%s: failed to create sysfs interface\n", __func__); return -ENODEV; } static int remove_sysfs_interfaces(struct device *dev) { int i; for (i = 0; i < ARRAY_SIZE(attributes); i++) device_remove_file(dev, attributes + i); return 0; } #endif static void dev_param_init(struct motor_dev *motor) { motor->focus.type = TYPE_FOCUS; motor->zoom.type = TYPE_ZOOM; motor->phase_interval_ns = div_u64(NSEC_PER_SEC, motor->start_up_speed); motor->focus.mv_tim.vcm_start_t = ns_to_timeval(ktime_get_ns()); motor->focus.mv_tim.vcm_end_t = ns_to_timeval(ktime_get_ns()); motor->focus.move_status = MOTOR_STATUS_STOPPED; motor->focus.last_status = MOTOR_STATUS_STOPPED; init_completion(&motor->focus.comp_in); init_completion(&motor->focus.comp_out); motor->focus.is_running = false; motor->focus.reback_ctrl = false; motor->focus.phase_index = 0; if (motor->focus.reback != 0) { motor->focus.cur_back_delay = 0; if (motor->focus.is_dir_opp) motor->focus.reback_status = MOTOR_STATUS_CCW; else motor->focus.reback_status = MOTOR_STATUS_CW; } motor->zoom.mv_tim.vcm_start_t = ns_to_timeval(ktime_get_ns()); motor->zoom.mv_tim.vcm_end_t = ns_to_timeval(ktime_get_ns()); motor->zoom.move_status = MOTOR_STATUS_STOPPED; motor->zoom.last_status = MOTOR_STATUS_STOPPED; init_completion(&motor->zoom.comp_in); init_completion(&motor->zoom.comp_out); motor->zoom.is_running = false; motor->zoom.reback_ctrl = false; motor->zoom.phase_index = 0; if (motor->zoom.reback != 0) { motor->zoom.cur_back_delay = 0; if (motor->zoom.is_dir_opp) motor->zoom.reback_status = MOTOR_STATUS_CCW; else motor->zoom.reback_status = MOTOR_STATUS_CW; } spin_lock_init(&motor->spin_lock); motor->is_timer_restart = false; } static int motor_dev_parse_dt(struct motor_dev *motor) { struct device_node *node = motor->dev->of_node; int ret = 0; int i = 0; motor->focus.phase_gpios[i] = devm_gpiod_get(motor->dev, "a1", GPIOD_OUT_LOW); if (IS_ERR(motor->focus.phase_gpios[i])) { dev_err(motor->dev, "Failed to get focus control gpios\n"); return -EINVAL; } i++; motor->focus.phase_gpios[i] = devm_gpiod_get(motor->dev, "b1", GPIOD_OUT_LOW); if (IS_ERR(motor->focus.phase_gpios[i])) { dev_err(motor->dev, "Failed to get focus control gpios\n"); return -EINVAL; } i++; motor->focus.phase_gpios[i] = devm_gpiod_get(motor->dev, "a2", GPIOD_OUT_LOW); if (IS_ERR(motor->focus.phase_gpios[i])) { dev_err(motor->dev, "Failed to get focus control gpios\n"); return -EINVAL; } i++; motor->focus.phase_gpios[i] = devm_gpiod_get(motor->dev, "b2", GPIOD_OUT_LOW); if (IS_ERR(motor->focus.phase_gpios[i])) { dev_err(motor->dev, "Failed to get focus control gpios\n"); return -EINVAL; } i = 0; motor->zoom.phase_gpios[i] = devm_gpiod_get(motor->dev, "c1", GPIOD_OUT_LOW); if (IS_ERR(motor->focus.phase_gpios[i])) { dev_err(motor->dev, "Failed to get focus control gpios\n"); return -EINVAL; } i++; motor->zoom.phase_gpios[i] = devm_gpiod_get(motor->dev, "d1", GPIOD_OUT_LOW); if (IS_ERR(motor->focus.phase_gpios[i])) { dev_err(motor->dev, "Failed to get focus control gpios\n"); return -EINVAL; } i++; motor->zoom.phase_gpios[i] = devm_gpiod_get(motor->dev, "c2", GPIOD_OUT_LOW); if (IS_ERR(motor->focus.phase_gpios[i])) { dev_err(motor->dev, "Failed to get focus control gpios\n"); return -EINVAL; } i++; motor->zoom.phase_gpios[i] = devm_gpiod_get(motor->dev, "d2", GPIOD_OUT_LOW); if (IS_ERR(motor->focus.phase_gpios[i])) { dev_err(motor->dev, "Failed to get focus control gpios\n"); return -EINVAL; } ret = of_property_read_u32(node, "focus-step-max", &motor->focus.step_max); if (ret != 0) { motor->focus.step_max = FOCUS_MAX_STEP_DEF; dev_err(motor->dev, "failed get iris focus_pos_max,use dafult value 7500\n"); } ret = of_property_read_u32(node, "zoom-step-max", &motor->zoom.step_max); if (ret != 0) { motor->zoom.step_max = ZOOM_MAX_STEP_DEF; dev_err(motor->dev, "failed get iris zoom_pos_max,use dafult value 7500\n"); } ret = of_property_read_u32(node, "motor-start-up-speed", &motor->start_up_speed); if (ret != 0) { motor->start_up_speed = MAX_START_UP_HZ; dev_err(motor->dev, "failed get motor start up speed,use dafult value\n"); } ret = of_property_read_u32(node, "focus-reback-distance", &motor->focus.reback); if (ret != 0) { dev_err(motor->dev, "failed get focus reback distance, return\n"); return -EINVAL; } ret = of_property_read_u32(node, "focus-reback-delay", &motor->focus.max_back_delay); if (ret != 0) { motor->focus.max_back_delay = FOCUS_MAX_BACK_DELAY; dev_err(motor->dev, "failed get focus reback delay, used default\n"); } ret = of_property_read_u32(node, "zoom-reback-distance", &motor->zoom.reback); if (ret != 0) { dev_err(motor->dev, "failed get zoom reback distance, return\n"); return -EINVAL; } ret = of_property_read_u32(node, "zoom-reback-delay", &motor->zoom.max_back_delay); if (ret != 0) { motor->zoom.max_back_delay = ZOOM_MAX_BACK_DELAY; dev_err(motor->dev, "failed get zoom reback delay, used default\n"); } motor->focus.is_dir_opp = device_property_read_bool(motor->dev, "focus-dir-opposite"); motor->zoom.is_dir_opp = device_property_read_bool(motor->dev, "zoom-dir-opposite"); ret = of_property_read_u32(node, RKMODULE_CAMERA_MODULE_INDEX, &motor->module_index); ret |= of_property_read_string(node, RKMODULE_CAMERA_MODULE_FACING, &motor->module_facing); if (ret) { dev_err(motor->dev, "could not get module information!\n"); return -EINVAL; } return 0; } static int motor_dev_probe(struct platform_device *pdev) { int ret = 0; struct motor_dev *motor; struct v4l2_subdev *sd; char facing[2]; dev_info(&pdev->dev, "driver version: %02x.%02x.%02x", DRIVER_VERSION >> 16, (DRIVER_VERSION & 0xff00) >> 8, DRIVER_VERSION & 0x00ff); motor = devm_kzalloc(&pdev->dev, sizeof(*motor), GFP_KERNEL); if (!motor) return -ENOMEM; motor->dev = &pdev->dev; dev_set_name(motor->dev, "motor"); dev_set_drvdata(motor->dev, motor); if (motor_dev_parse_dt(motor)) { dev_err(motor->dev, "parse dt error\n"); return -EINVAL; } dev_param_init(motor); hrtimer_init(&motor->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL_PINNED); motor->timer.function = motor_timer_func; v4l2_subdev_init(&motor->sd, &motor_subdev_ops); motor->sd.owner = pdev->dev.driver->owner; motor->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE; motor->sd.dev = &pdev->dev; v4l2_set_subdevdata(&motor->sd, pdev); platform_set_drvdata(pdev, &motor->sd); motor_initialize_controls(motor); if (ret) goto err_free; ret = media_entity_pads_init(&motor->sd.entity, 0, NULL); if (ret < 0) goto err_free; sd = &motor->sd; sd->entity.function = MEDIA_ENT_F_LENS; sd->entity.flags = 0; memset(facing, 0, sizeof(facing)); if (strcmp(motor->module_facing, "back") == 0) facing[0] = 'b'; else facing[0] = 'f'; snprintf(sd->name, sizeof(sd->name), "m%02d_%s_%s", motor->module_index, facing, DRIVER_NAME); ret = v4l2_async_register_subdev(sd); if (ret) dev_err(&pdev->dev, "v4l2 async register subdev failed\n"); #ifdef USED_SYS_DEBUG add_sysfs_interfaces(&pdev->dev); #endif dev_info(motor->dev, "gpio motor driver probe success\n"); return 0; err_free: v4l2_ctrl_handler_free(&motor->ctrl_handler); v4l2_device_unregister_subdev(&motor->sd); media_entity_cleanup(&motor->sd.entity); return ret; } static int motor_dev_remove(struct platform_device *pdev) { struct v4l2_subdev *sd = platform_get_drvdata(pdev); struct motor_dev *motor; if (sd) motor = v4l2_get_subdevdata(sd); else return -ENODEV; hrtimer_cancel(&motor->timer); if (sd) v4l2_device_unregister_subdev(sd); v4l2_ctrl_handler_free(&motor->ctrl_handler); media_entity_cleanup(&motor->sd.entity); #ifdef USED_SYS_DEBUG remove_sysfs_interfaces(motor->dev); #endif return 0; } #if defined(CONFIG_OF) static const struct of_device_id motor_dev_of_match[] = { { .compatible = "fitipower,fp5501", }, {}, }; #endif static struct platform_driver motor_dev_driver = { .driver = { .name = DRIVER_NAME, .owner = THIS_MODULE, .of_match_table = of_match_ptr(motor_dev_of_match), }, .probe = motor_dev_probe, .remove = motor_dev_remove, }; module_platform_driver(motor_dev_driver); MODULE_LICENSE("GPL"); MODULE_ALIAS("platform:motor"); MODULE_AUTHOR("ROCKCHIP");