// 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 #include #define DRIVER_VERSION KERNEL_VERSION(0, 0x01, 0x00) #define DRIVER_NAME "mp6507" #define MAX_START_UP_HZ (1200) #define MOTOR_MAX_HZ (2500) #define SPEED_QUEUE_MAX (71) #define THRESHOLD_TO_SPEEDED_UP_DEF (500) #define STEP_PER_SPEED_DEF (8) #define SPEED_QUEUE_NUM_DEF (71) #define IRIS_MAX_STEP_DEF 80 #define FOCUS_MAX_STEP_DEF 7500 #define ZOOM_MAX_STEP_DEF 7500 #define IRIS_MAX_LOG 80 #define FOCUS_MAX_LOG 7500 #define ZOOM_MAX_LOG 7500 #define IRIS_LOG_STEP 4 #define FOCUS_LOG_STEP 4 #define ZOOM_LOG_STEP 4 enum { MOTOR_STATUS_STOPPED = 0, MOTOR_STATUS_CW = 1, MOTOR_STATUS_CCW = 2, }; enum ext_dev_type { TYPE_IRIS = 0, TYPE_FOCUS = 1, TYPE_ZOOM = 2, }; struct speed_s { u32 count; u64 phase_interval_ns; }; struct speed_queue_s { int count; struct speed_s *speed_p; }; struct ext_dev { u8 type; u32 step_max; u32 cur_pos; u32 step_per_pos; u32 start_up_speed; u32 max_speed; u32 speed_queue_num; u32 first_speed_step; u32 ths_speeded_up; u32 speed_up_step_cnt; u32 *speed_up_table; u32 *speed_down_table; u32 length_up;//speed_up_table length u32 length_down; struct gpio_desc *en_gpio; struct rk_cam_vcm_tim mv_tim; struct speed_queue_s speed_que; struct speed_queue_s one_speed_que; }; struct motor_dev { struct v4l2_subdev sd; struct v4l2_ctrl_handler ctrl_handler; struct pwm_device *pwm_a1; struct pwm_device *pwm_a2; struct pwm_device *pwm_b1; struct pwm_device *pwm_b2; struct v4l2_ctrl *iris_ctrl; struct v4l2_ctrl *focus_ctrl; struct v4l2_ctrl *zoom_ctrl; struct device *dev; struct hrtimer timer; struct mutex mutex; u32 move_status; u32 move_cnt; u32 module_index; const char *module_facing; bool resched; struct completion complete; struct ext_dev iris; struct ext_dev focus; struct ext_dev zoom; struct ext_dev *cur_ext_dev; struct speed_queue_s *run_queue; struct pwm_state pwm_state; }; static int set_motor_running_status(struct motor_dev *motor, struct ext_dev *cur_ext_dev, int status, u32 pos) { int ret = 0; u64 mv_us = 0; u64 mv_s = 0; u64 move_time = 0; u32 step_cnt = 0; int i = 0; if (motor->move_status != MOTOR_STATUS_STOPPED) wait_for_completion(&motor->complete); motor->cur_ext_dev = cur_ext_dev; if (!IS_ERR(cur_ext_dev->en_gpio)) gpiod_set_value_cansleep(cur_ext_dev->en_gpio, 1); motor->move_status = status; step_cnt = pos * cur_ext_dev->step_per_pos; if (cur_ext_dev->speed_queue_num > 1 && step_cnt >= cur_ext_dev->ths_speeded_up) { motor->run_queue = &cur_ext_dev->speed_que; motor->run_queue->speed_p[cur_ext_dev->length_up - 1].count = step_cnt - cur_ext_dev->speed_up_step_cnt; } else { motor->run_queue = &cur_ext_dev->one_speed_que; motor->run_queue->speed_p[0].count = step_cnt; } motor->move_cnt = motor->run_queue->count; reinit_completion(&motor->complete); cur_ext_dev->mv_tim.vcm_start_t = ns_to_timeval(ktime_get_ns()); for (i = 0; i < motor->run_queue->count; i++) { move_time += (u64)motor->run_queue->speed_p[i].count * (u64)motor->run_queue->speed_p[i].phase_interval_ns; dev_dbg(motor->dev, "speed_que.speed[%d], count %d, phase_interval_ns %llu\n", i, motor->run_queue->speed_p[i].count, motor->run_queue->speed_p[i].phase_interval_ns); } mv_us = div_u64(move_time, 1000); dev_dbg(motor->dev, "motor move needs %lld us\n", mv_us); mv_us += cur_ext_dev->mv_tim.vcm_start_t.tv_usec; if (mv_us >= 1000000) { mv_s = div_u64(mv_us, 1000000); cur_ext_dev->mv_tim.vcm_end_t.tv_sec = cur_ext_dev->mv_tim.vcm_start_t.tv_sec + mv_s; cur_ext_dev->mv_tim.vcm_end_t.tv_usec = mv_us - (mv_s * 1000000); } else { cur_ext_dev->mv_tim.vcm_end_t.tv_sec = cur_ext_dev->mv_tim.vcm_start_t.tv_sec; cur_ext_dev->mv_tim.vcm_end_t.tv_usec = mv_us; } hrtimer_start(&motor->timer, ktime_set(0, 0), HRTIMER_MODE_REL); return ret; } static int fill_speed_squeue(struct device *dev, struct ext_dev *ext_dev) { struct device_node *node = dev->of_node; struct property *prop = NULL; u32 length_up = 0; u32 length_down = 0; u32 *speed_up_table = NULL; u32 *speed_down_table = NULL; int i = 0; size_t size; u32 step_cnt = 0; u32 step_total = 0; int ret = 0; ext_dev->ths_speeded_up = 0; size = sizeof(*ext_dev->one_speed_que.speed_p); ext_dev->one_speed_que.speed_p = devm_kzalloc(dev, size, GFP_KERNEL); if (!ext_dev->one_speed_que.speed_p) return -ENOMEM; ext_dev->one_speed_que.count = 1; ext_dev->one_speed_que.speed_p[0].count = ext_dev->first_speed_step; ext_dev->one_speed_que.speed_p[0].phase_interval_ns = div_u64(NSEC_PER_SEC, ext_dev->start_up_speed); switch (ext_dev->type) { case TYPE_IRIS: //max step is 80, needn't speed-up return 0; case TYPE_FOCUS: prop = of_find_property(node, "focus-speed-up-table", &length_up); if (prop) length_up /= sizeof(u32); if (length_up > 0) { size = sizeof(*speed_up_table) * length_up; speed_up_table = devm_kzalloc(dev, size, GFP_KERNEL); if (!speed_up_table) return -ENOMEM; ret = of_property_read_u32_array(node, "focus-speed-up-table", speed_up_table, length_up); if (ret < 0) { dev_info(dev, "fail to get speed table, used default speed!\n"); devm_kfree(dev, speed_up_table); speed_up_table = NULL; ext_dev->speed_queue_num = 1; return 0; } dev_dbg(dev, "dev tpype %d, speed-up table length %d, buf size %u\n", ext_dev->type, length_up, size); } prop = of_find_property(node, "focus-speed-down-table", &length_down); if (prop) length_down /= sizeof(u32); if (length_down > 0) { size = sizeof(*speed_down_table) * length_down; speed_down_table = devm_kzalloc(dev, size, GFP_KERNEL); if (!speed_down_table) return -ENOMEM; ret = of_property_read_u32_array(node, "focus-speed-down-table", speed_down_table, length_down); if (ret < 0) { dev_info(dev, "fail to get speed table, used default speed!\n"); devm_kfree(dev, speed_down_table); speed_down_table = NULL; } else { dev_dbg(dev, "dev tpype %d, speed-down table length %d, buf size %u\n", ext_dev->type, length_up, size); } } break; case TYPE_ZOOM: prop = of_find_property(node, "zoom-speed-up-table", &length_up); if (prop) length_up /= sizeof(u32); if (length_up > 0) { size = sizeof(*speed_up_table) * length_up; speed_up_table = devm_kzalloc(dev, size, GFP_KERNEL); if (!speed_up_table) return -ENOMEM; ret = of_property_read_u32_array(node, "zoom-speed-up-table", speed_up_table, length_up); if (ret < 0) { dev_info(dev, "fail to get speed table, used default speed!\n"); ext_dev->speed_queue_num = 1; devm_kfree(dev, speed_up_table); speed_up_table = NULL; return 0; } dev_dbg(dev, "dev tpype %d, speed-up table length %d, buf size %u\n", ext_dev->type, length_up, size); } prop = of_find_property(node, "zoom-speed-down-table", &length_down); if (prop) length_down /= sizeof(u32); if (length_down > 0) { size = sizeof(*speed_down_table) * length_down; speed_down_table = devm_kzalloc(dev, size, GFP_KERNEL); if (!speed_down_table) return -ENOMEM; ret = of_property_read_u32_array(node, "zoom-speed-down-table", speed_down_table, length_down); if (ret < 0) { dev_info(dev, "fail to get speed table, used default speed!\n"); devm_kfree(dev, speed_down_table); speed_down_table = NULL; } else { dev_dbg(dev, "dev tpype %d, speed-down table length %d, buf size %u\n", ext_dev->type, length_up, size); } } break; default: return -EINVAL; } if (speed_up_table == NULL || speed_up_table[0] > ext_dev->start_up_speed || speed_up_table[length_up - 1] > ext_dev->max_speed) { dev_info(dev, "speed_up_table data error, not to used it!\n"); ext_dev->speed_queue_num = 1; } else { ext_dev->length_up = length_up; if (speed_down_table != NULL) ext_dev->speed_queue_num = length_up + length_down; else ext_dev->speed_queue_num = length_up * 2 - 1; size = sizeof(*ext_dev->speed_que.speed_p) * ext_dev->speed_queue_num; ext_dev->speed_que.speed_p = devm_kzalloc(dev, size, GFP_KERNEL); if (!ext_dev->speed_que.speed_p) return -ENOMEM; for (i = 0; i < length_up - 1; i++) { if (i == 0) step_cnt = ext_dev->first_speed_step; else step_cnt = ext_dev->first_speed_step * speed_up_table[i] / speed_up_table[0]; step_cnt = (step_cnt + 3) / 4 * 4; ext_dev->speed_que.speed_p[i].count = step_cnt; ext_dev->speed_que.speed_p[i].phase_interval_ns = div_u64(NSEC_PER_SEC, speed_up_table[i]); step_total += step_cnt; if (speed_down_table == NULL || speed_down_table[0] > speed_up_table[length_up - 1]) { dev_info(dev, "speed_down_table data error, used speed_up_table\n"); ext_dev->speed_que.speed_p[ext_dev->speed_queue_num - i - 1].count = step_cnt; ext_dev->speed_que.speed_p[ext_dev->speed_queue_num - i - 1].phase_interval_ns = div_u64(NSEC_PER_SEC, speed_up_table[i]); step_total += step_cnt; } dev_info(dev, "index %d, speed %d, count %d\n", i, speed_up_table[i], ext_dev->speed_que.speed_p[i].count); } ext_dev->speed_up_table = speed_up_table; if (speed_down_table != NULL && speed_down_table[0] <= speed_up_table[length_up - 1]) { for (i = 0; i < length_down; i++) { step_cnt = ext_dev->first_speed_step * speed_down_table[i] / speed_up_table[0]; step_cnt = (step_cnt + 3) / 4 * 4; ext_dev->speed_que.speed_p[length_up + i].count = step_cnt; ext_dev->speed_que.speed_p[length_up + i].phase_interval_ns = div_u64(NSEC_PER_SEC, speed_down_table[i]); step_total += step_cnt; } ext_dev->speed_down_table = speed_down_table; ext_dev->length_down = length_down; } ext_dev->speed_up_step_cnt = step_total; step_cnt = ext_dev->first_speed_step * speed_up_table[length_up - 1] / speed_up_table[0]; step_cnt = (step_cnt + 3) / 4 * 4; ext_dev->speed_que.speed_p[length_up - 1].count = step_cnt; ext_dev->speed_que.speed_p[length_up - 1].phase_interval_ns = div_u64(NSEC_PER_SEC, speed_up_table[length_up - 1]); step_total += step_cnt; ext_dev->ths_speeded_up = step_total; ext_dev->speed_que.count = ext_dev->speed_queue_num; } return 0; } static int motor_dev_parse_dt(struct motor_dev *motor) { struct device_node *node = motor->dev->of_node; int ret = 0; int error = 0; motor->pwm_a1 = devm_pwm_get(motor->dev, "ain1"); motor->pwm_a2 = devm_pwm_get(motor->dev, "ain2"); motor->pwm_b1 = devm_pwm_get(motor->dev, "bin1"); motor->pwm_b2 = devm_pwm_get(motor->dev, "bin2"); if (IS_ERR(motor->pwm_a1)) { error = PTR_ERR(motor->pwm_a1); if (error != -EPROBE_DEFER) dev_err(motor->dev, "Failed to request PWM a1 device: %d\n", error); return error; } if (IS_ERR(motor->pwm_a2)) { error = PTR_ERR(motor->pwm_a2); if (error != -EPROBE_DEFER) dev_err(motor->dev, "Failed to request PWM a2 device: %d\n", error); return error; } if (IS_ERR(motor->pwm_b1)) { error = PTR_ERR(motor->pwm_b1); if (error != -EPROBE_DEFER) dev_err(motor->dev, "Failed to request PWM b1 device: %d\n", error); return error; } if (IS_ERR(motor->pwm_b2)) { error = PTR_ERR(motor->pwm_b2); if (error != -EPROBE_DEFER) dev_err(motor->dev, "Failed to request PWM b2 device: %d\n", error); return error; } /* get iris_en gpio */ motor->iris.en_gpio = devm_gpiod_get(motor->dev, "iris_en", GPIOD_OUT_LOW); if (IS_ERR(motor->iris.en_gpio)) dev_err(motor->dev, "Failed to get iris_en-gpios\n"); /* get focus_en gpio */ motor->focus.en_gpio = devm_gpiod_get(motor->dev, "focus_en", GPIOD_OUT_LOW); if (IS_ERR(motor->focus.en_gpio)) dev_err(motor->dev, "Failed to get focus_en-gpios\n"); /* get zoom_en gpio */ motor->zoom.en_gpio = devm_gpiod_get(motor->dev, "zoom_en", GPIOD_OUT_LOW); if (IS_ERR(motor->zoom.en_gpio)) dev_err(motor->dev, "Failed to get zoom_en-gpios\n"); ret = of_property_read_u32(node, "iris-step-max", &motor->iris.step_max); if (ret != 0) { motor->iris.step_max = IRIS_MAX_STEP_DEF; dev_err(motor->dev, "failed get iris iris_pos_max,use dafult value 80\n"); } 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, "iris-start-up-speed", &motor->iris.start_up_speed); if (ret != 0) { motor->iris.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, "iris-max-speed", &motor->iris.max_speed); if (ret != 0) { motor->iris.max_speed = MOTOR_MAX_HZ; dev_err(motor->dev, "failed get motor max speed,use dafult value\n"); } ret = of_property_read_u32(node, "focus-start-up-speed", &motor->focus.start_up_speed); if (ret != 0) { motor->focus.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-max-speed", &motor->focus.max_speed); if (ret != 0) { motor->focus.max_speed = MOTOR_MAX_HZ; dev_err(motor->dev, "failed get motor max speed,use dafult value\n"); } ret = of_property_read_u32(node, "zoom-start-up-speed", &motor->zoom.start_up_speed); if (ret != 0) { motor->zoom.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, "zoom-max-speed", &motor->zoom.max_speed); if (ret != 0) { motor->zoom.max_speed = MOTOR_MAX_HZ; dev_err(motor->dev, "failed get motor max speed,use dafult value\n"); } ret = of_property_read_u32(node, "focus-first-speed-step", &motor->focus.first_speed_step); if (ret != 0) { motor->focus.first_speed_step = STEP_PER_SPEED_DEF; dev_err(motor->dev, "failed get motor step of first speed,use dafult value\n"); } ret = of_property_read_u32(node, "zoom-first-speed-step", &motor->zoom.first_speed_step); if (ret != 0) { motor->zoom.first_speed_step = STEP_PER_SPEED_DEF; dev_err(motor->dev, "failed get motor step of first speed,use dafult value\n"); } motor->iris.type = TYPE_IRIS; ret = fill_speed_squeue(motor->dev, &motor->iris); motor->focus.type = TYPE_FOCUS; ret |= fill_speed_squeue(motor->dev, &motor->focus); motor->zoom.type = TYPE_ZOOM; ret |= fill_speed_squeue(motor->dev, &motor->zoom); 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 enum hrtimer_restart motor_timer_func(struct hrtimer *timer) { struct motor_dev *motor; struct pwm_state *pwm_state; int idx = 0; u64 time_cnt = 0; motor = container_of(timer, struct motor_dev, timer); pwm_state = &motor->pwm_state; if (motor->move_cnt < 1 || motor->move_status == MOTOR_STATUS_STOPPED) { pwm_state->enabled = false; pwm_apply_state(motor->pwm_b1, pwm_state); pwm_apply_state(motor->pwm_b2, pwm_state); pwm_apply_state(motor->pwm_a1, pwm_state); pwm_apply_state(motor->pwm_a2, pwm_state); if (!IS_ERR(motor->cur_ext_dev->en_gpio)) gpiod_set_value(motor->cur_ext_dev->en_gpio, 0); motor->move_status = MOTOR_STATUS_STOPPED; motor->resched = false; complete(&motor->complete); dev_dbg(motor->dev, "motor stop\n"); } else { /* do phase change */ switch (motor->move_status) { case MOTOR_STATUS_CW: if (motor->resched == true) { pwm_state->enabled = false; pwm_apply_state(motor->pwm_b1, pwm_state); pwm_apply_state(motor->pwm_b2, pwm_state); pwm_apply_state(motor->pwm_a1, pwm_state); pwm_apply_state(motor->pwm_a2, pwm_state); } idx = motor->run_queue->count - motor->move_cnt; pwm_state->polarity = PWM_POLARITY_INVERSED; pwm_state->enabled = true; pwm_state->period = motor->run_queue->speed_p[idx].phase_interval_ns * 4; pwm_state->duty_cycle = motor->run_queue->speed_p[idx].phase_interval_ns * 2; pwm_apply_state(motor->pwm_b1, pwm_state); pwm_state->enabled = true; pwm_state->polarity = PWM_POLARITY_NORMAL; pwm_apply_state(motor->pwm_b2, pwm_state); pwm_state->polarity = PWM_POLARITY_NORMAL; pwm_state->enabled = true; pwm_apply_state(motor->pwm_a1, pwm_state); pwm_state->polarity = PWM_POLARITY_INVERSED; pwm_state->enabled = true; pwm_apply_state(motor->pwm_a2, pwm_state); break; case MOTOR_STATUS_CCW: if (motor->resched == true) { pwm_state->enabled = false; pwm_apply_state(motor->pwm_b1, pwm_state); pwm_apply_state(motor->pwm_b2, pwm_state); pwm_apply_state(motor->pwm_a1, pwm_state); pwm_apply_state(motor->pwm_a2, pwm_state); } idx = motor->run_queue->count - motor->move_cnt; pwm_state->polarity = PWM_POLARITY_INVERSED; pwm_state->enabled = true; pwm_state->period = motor->run_queue->speed_p[idx].phase_interval_ns * 4; pwm_state->duty_cycle = motor->run_queue->speed_p[idx].phase_interval_ns * 2; pwm_apply_state(motor->pwm_b1, pwm_state); pwm_state->polarity = PWM_POLARITY_NORMAL; pwm_state->enabled = true; pwm_apply_state(motor->pwm_b2, pwm_state); pwm_state->polarity = PWM_POLARITY_INVERSED; pwm_state->enabled = true; pwm_apply_state(motor->pwm_a1, pwm_state); pwm_state->polarity = PWM_POLARITY_NORMAL; pwm_state->enabled = true; pwm_apply_state(motor->pwm_a2, pwm_state); break; default: break; } if (motor->resched == false) motor->resched = true; motor->move_cnt--; } if (motor->resched) { time_cnt = ((u64)motor->run_queue->speed_p[idx].phase_interval_ns * motor->run_queue->speed_p[idx].count); hrtimer_forward_now(timer, ns_to_ktime(time_cnt - 80000)); return HRTIMER_RESTART; } return HRTIMER_NORESTART; } 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); switch (ctrl->id) { case V4L2_CID_IRIS_ABSOLUTE: if (ctrl->val > motor->iris.cur_pos) ret = set_motor_running_status(motor, &motor->iris, MOTOR_STATUS_CCW, abs(ctrl->val - motor->iris.cur_pos)); else ret = set_motor_running_status(motor, &motor->iris, MOTOR_STATUS_CW, abs(ctrl->val - motor->iris.cur_pos)); motor->iris.cur_pos = ctrl->val; dev_dbg(motor->dev, "set iris pos %d\n", ctrl->val); break; case V4L2_CID_FOCUS_ABSOLUTE: if (ctrl->val > motor->focus.cur_pos) ret = set_motor_running_status(motor, &motor->focus, MOTOR_STATUS_CCW, abs(ctrl->val - motor->focus.cur_pos)); else ret = set_motor_running_status(motor, &motor->focus, MOTOR_STATUS_CW, abs(ctrl->val - motor->focus.cur_pos)); motor->focus.cur_pos = ctrl->val; dev_dbg(motor->dev, "set focus pos %d\n", ctrl->val); break; case V4L2_CID_ZOOM_ABSOLUTE: if (ctrl->val > motor->zoom.cur_pos) ret = set_motor_running_status(motor, &motor->zoom, MOTOR_STATUS_CCW, abs(ctrl->val - motor->zoom.cur_pos)); else ret = set_motor_running_status(motor, &motor->zoom, MOTOR_STATUS_CW, abs(ctrl->val - motor->zoom.cur_pos)); motor->zoom.cur_pos = ctrl->val; 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_init_iris_status(struct motor_dev *motor) { int ret = 0; ret = set_motor_running_status(motor, &motor->iris, MOTOR_STATUS_CCW, IRIS_MAX_LOG); motor->iris.cur_pos = IRIS_MAX_LOG; __v4l2_ctrl_modify_range(motor->iris_ctrl, 0, IRIS_MAX_LOG, IRIS_LOG_STEP, motor->iris.cur_pos); return ret; } static int motor_init_focus_status(struct motor_dev *motor) { int ret = 0; ret = set_motor_running_status(motor, &motor->focus, MOTOR_STATUS_CW, FOCUS_MAX_LOG); motor->focus.cur_pos = 0; __v4l2_ctrl_modify_range(motor->focus_ctrl, 0, FOCUS_MAX_LOG, FOCUS_LOG_STEP, motor->focus.cur_pos); return ret; } static int motor_init_zoom_status(struct motor_dev *motor) { int ret = 0; ret = set_motor_running_status(motor, &motor->zoom, MOTOR_STATUS_CW, ZOOM_MAX_LOG); motor->zoom.cur_pos = 0; __v4l2_ctrl_modify_range(motor->zoom_ctrl, 0, ZOOM_MAX_LOG, ZOOM_LOG_STEP, motor->zoom.cur_pos); return ret; } 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); 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_IRIS_TIMEINFO: mv_tim = (struct rk_cam_vcm_tim *)arg; memcpy(mv_tim, &motor->iris.mv_tim, sizeof(*mv_tim)); dev_dbg(motor->dev, "get_iris_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_IRIS_CORRECTION: motor_init_iris_status(motor); break; case RK_VIDIOC_FOCUS_CORRECTION: motor_init_focus_status(motor); break; case RK_VIDIOC_ZOOM_CORRECTION: motor_init_zoom_status(motor); break; default: break; } return 0; } static const struct v4l2_subdev_core_ops motor_core_ops = { .ioctl = motor_ioctl, }; static const struct v4l2_subdev_ops motor_subdev_ops = { .core = &motor_core_ops, }; static const struct v4l2_ctrl_ops motor_ctrl_ops = { .s_ctrl = motor_s_ctrl, }; static int motor_initialize_controls(struct motor_dev *motor) { struct v4l2_ctrl_handler *handler; int ret = 0; handler = &motor->ctrl_handler; ret = v4l2_ctrl_handler_init(handler, 3); if (ret) return ret; handler->lock = &motor->mutex; if (!IS_ERR(motor->iris.en_gpio)) { motor->iris_ctrl = v4l2_ctrl_new_std(handler, &motor_ctrl_ops, V4L2_CID_IRIS_ABSOLUTE, 0, IRIS_MAX_LOG, IRIS_LOG_STEP, 0); ret = motor_init_iris_status(motor); } if (!IS_ERR(motor->focus.en_gpio)) { motor->focus_ctrl = v4l2_ctrl_new_std(handler, &motor_ctrl_ops, V4L2_CID_FOCUS_ABSOLUTE, 0, FOCUS_MAX_LOG, FOCUS_LOG_STEP, 0); } if (!IS_ERR(motor->zoom.en_gpio)) { motor->zoom_ctrl = v4l2_ctrl_new_std(handler, &motor_ctrl_ops, V4L2_CID_ZOOM_ABSOLUTE, 0, ZOOM_MAX_LOG, ZOOM_LOG_STEP, 0); } 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 set_iris_correction(struct device *dev, struct device_attribute *attr, const char *buf, size_t count) { struct motor_dev *motor = dev_get_drvdata(dev); int status = 0; int ret = 0; ret = kstrtoint(buf, 0, &status); if (!ret && status >= 0 && status < 2) { if (status) motor_init_iris_status(motor); dev_info(dev, "camera iris position correction\n"); } return count; } static ssize_t set_focus_correction(struct device *dev, struct device_attribute *attr, const char *buf, size_t count) { struct motor_dev *motor = dev_get_drvdata(dev); int status = 0; int ret = 0; ret = kstrtoint(buf, 0, &status); if (!ret && status >= 0 && status < 2) { if (status) motor_init_focus_status(motor); dev_info(dev, "camera focus position correction\n"); } return count; } static ssize_t set_zoom_correction(struct device *dev, struct device_attribute *attr, const char *buf, size_t count) { struct motor_dev *motor = dev_get_drvdata(dev); int status = 0; int ret = 0; ret = kstrtoint(buf, 0, &status); if (!ret && status >= 0 && status < 2) { if (status) motor_init_zoom_status(motor); dev_info(dev, "camera zoom position correction\n"); } return count; } static struct device_attribute attributes[] = { __ATTR(is_iris_correction, S_IWUSR, NULL, set_iris_correction), __ATTR(is_focus_correction, S_IWUSR, NULL, set_focus_correction), __ATTR(is_zoom_correction, S_IWUSR, NULL, set_zoom_correction), }; 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; } #endif static void dev_param_init(struct motor_dev *motor) { motor->iris.step_per_pos = motor->iris.step_max / IRIS_MAX_LOG; motor->iris.mv_tim.vcm_start_t = ns_to_timeval(ktime_get_ns()); motor->iris.mv_tim.vcm_end_t = ns_to_timeval(ktime_get_ns()); motor->focus.step_per_pos = motor->focus.step_max / FOCUS_MAX_LOG; 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->zoom.step_per_pos = motor->zoom.step_max / ZOOM_MAX_LOG; 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->move_status = MOTOR_STATUS_STOPPED; motor->resched = false; } 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); mutex_init(&motor->mutex); hrtimer_init(&motor->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL); motor->timer.function = motor_timer_func; init_completion(&motor->complete); 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); return 0; } #if defined(CONFIG_OF) static const struct of_device_id motor_dev_of_match[] = { { .compatible = "monolithicpower,mp6507", }, {}, }; #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");