// 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 "linux/rk_vcm_head.h" #include #include #define DRIVER_VERSION KERNEL_VERSION(0, 0x01, 0x00) #define DRIVER_NAME "ms41908" #define PSUMAB 0X24 #define INTCTAB 0X25 #define PSUMCD 0X29 #define INTCTCD 0X2A #define START_UP_HZ_DEF (800) #define PIRIS_MAX_STEP_DEF (80) #define FOCUS_MAX_STEP_DEF (3060) #define ZOOM_MAX_STEP_DEF (1520) #define DCIRIS_MAX_LOG 1023 #define VD_FZ_US 10000 #define PPW_DEF 0xff #define MICRO_DEF 64 #define PHMODE_DEF 0 #define PPW_STOP 0x00 #define FOCUS_MAX_BACK_DELAY 4 #define ZOOM_MAX_BACK_DELAY 4 #define ZOOM1_MAX_BACK_DELAY 4 #define to_motor_dev(sd) container_of(sd, struct motor_dev, subdev) enum { MOTOR_STATUS_STOPPED = 0, MOTOR_STATUS_CCW = 1, MOTOR_STATUS_CW = 2, }; enum ext_dev_type { TYPE_IRIS = 0, TYPE_FOCUS = 1, TYPE_ZOOM = 2, TYPE_ZOOM1 = 3, }; struct motor_reg_s { u16 dt2_phmod; u16 ppw; u16 psum; u16 intct; }; struct reg_op_s { struct motor_reg_s reg; u16 tmp_psum; bool is_used; }; struct run_data_s { u32 count; u32 cur_count; u32 psum; u32 psum_last; u32 intct; u32 ppw; u32 ppw_stop; u32 phmode; u32 micro; }; struct ext_dev { u8 type; u32 step_max; int last_pos; u32 start_up_speed; u32 move_status; u32 reback_status; u32 move_time_us; u32 reback_move_time_us; u32 backlash; int reback; u32 last_dir; int min_pos; int max_pos; bool is_half_step_mode; bool is_mv_tim_update; bool is_need_update_tim; bool is_dir_opp; bool is_need_reback; bool reback_ctrl; struct rk_cam_vcm_tim mv_tim; struct run_data_s run_data; struct run_data_s reback_data; struct completion complete; struct reg_op_s *reg_op; struct gpio_desc *pic_gpio; struct gpio_desc *pia_gpio; struct gpio_desc *pie_gpio; int cur_back_delay; int max_back_delay; struct completion complete_out; bool is_running; }; struct dciris_dev { u32 last_log; u32 max_log; bool is_reversed_polarity; struct gpio_desc *vd_iris_gpio; }; struct motor_dev { struct spi_device *spi; struct v4l2_subdev subdev; struct v4l2_ctrl_handler ctrl_handler; struct v4l2_ctrl *iris_ctrl; struct v4l2_ctrl *focus_ctrl; struct v4l2_ctrl *zoom_ctrl; struct v4l2_ctrl *zoom1_ctrl; struct hrtimer timer; struct mutex mutex; u32 module_index; const char *module_facing; struct ext_dev *piris; struct ext_dev *focus; struct ext_dev *zoom; struct ext_dev *zoom1; struct ext_dev *dev0; struct ext_dev *dev1; bool is_use_dc_iris; bool is_use_p_iris; bool is_use_focus; bool is_use_zoom; bool is_use_zoom1; struct gpio_desc *reset_gpio; struct gpio_desc *vd_fz_gpio; bool is_timer_restart; bool is_timer_restart_bywq; bool is_should_wait; struct motor_work_s *wk; u32 vd_fz_period_us; struct reg_op_s motor_op[2]; struct dciris_dev *dciris; int id; int wait_cnt; int pi_gpio_usecnt; }; struct motor_work_s { struct work_struct work; struct motor_dev *dev; }; static const struct reg_op_s g_motor_op[2] = {{{0x22, 0x23, 0x24, 0x25}, 0, 0}, {{0x27, 0x28, 0x29, 0x2a}, 0, 0}}; static int spi_write_reg(struct spi_device *spi, u8 reg, u16 val) { int ret = 0; u8 buf_reg = reg; u16 buf_val = val; struct spi_message msg; struct spi_transfer tx[] = { { .tx_buf = &buf_reg, .len = 1, .delay_usecs = 1, }, { .tx_buf = &buf_val, .len = 2, .delay_usecs = 1, }, }; spi_message_init(&msg); spi_message_add_tail(&tx[0], &msg); spi_message_add_tail(&tx[1], &msg); ret = spi_sync(spi, &msg); return ret; } static __maybe_unused int spi_read_reg(struct spi_device *spi, u8 reg, u16 *val) { int ret = 0; u8 buf_reg = reg | 0x40; u16 buf_val = 0; struct spi_message msg; struct spi_transfer tx[] = { { .tx_buf = &buf_reg, .len = 1, .delay_usecs = 1, }, { .rx_buf = &buf_val, .len = 2, .delay_usecs = 1, }, }; spi_message_init(&msg); spi_message_add_tail(&tx[0], &msg); spi_message_add_tail(&tx[1], &msg); ret = spi_sync(spi, &msg); *val = buf_val; return ret; } static int set_motor_running_status(struct motor_dev *motor, struct ext_dev *ext_dev, s32 pos, bool is_need_update_tim, bool is_should_wait, bool is_need_reback) { int ret = 0; u32 step = 0; u16 psum = 0; struct run_data_s run_data = ext_dev->run_data; u32 micro = 0; u32 mv_cnt = 0; int status = 0; if (ext_dev->move_status != MOTOR_STATUS_STOPPED) wait_for_completion(&ext_dev->complete); ext_dev->is_mv_tim_update = false; ext_dev->move_time_us = 0; mv_cnt = abs(pos - ext_dev->last_pos); if (is_need_reback) mv_cnt += ext_dev->reback; dev_dbg(&motor->spi->dev, "dev type %d pos %d, last_pos %d, mv_cnt %d, status %d is_need_reback %d\n", ext_dev->type, pos, ext_dev->last_pos, mv_cnt, status, is_need_reback); if (mv_cnt == 0) { mutex_lock(&motor->mutex); if (is_need_update_tim) { ext_dev->mv_tim.vcm_start_t = ns_to_timeval(ktime_get_ns()); ext_dev->mv_tim.vcm_end_t = ext_dev->mv_tim.vcm_start_t; ext_dev->is_mv_tim_update = true; if (motor->wait_cnt == 0) { mutex_unlock(&motor->mutex); return 0; } } if (is_should_wait) { motor->wait_cnt++; } else if (motor->is_timer_restart == false && motor->wait_cnt) { motor->is_timer_restart = true; motor->is_timer_restart_bywq = false; hrtimer_start(&motor->timer, ktime_set(0, 0), HRTIMER_MODE_REL); motor->wait_cnt = 0; } else { motor->wait_cnt = 0; } mutex_unlock(&motor->mutex); return 0; } ext_dev->is_running = true; reinit_completion(&ext_dev->complete); reinit_completion(&ext_dev->complete_out); if (ext_dev->is_dir_opp) { if (pos > ext_dev->last_pos) { if (ext_dev->last_dir == MOTOR_STATUS_CCW) mv_cnt += ext_dev->backlash; status = MOTOR_STATUS_CW; } else { if (ext_dev->last_dir == MOTOR_STATUS_CW) mv_cnt += ext_dev->backlash; status = MOTOR_STATUS_CCW; } } else { if (pos > ext_dev->last_pos) { if (ext_dev->last_dir == MOTOR_STATUS_CW) mv_cnt += ext_dev->backlash; status = MOTOR_STATUS_CCW; } else { if (ext_dev->last_dir == MOTOR_STATUS_CCW) mv_cnt += ext_dev->backlash; status = MOTOR_STATUS_CW; } } if (ext_dev->is_half_step_mode) step = mv_cnt * 4; else step = mv_cnt * 8; run_data.count = (step + run_data.psum - 1) / run_data.psum; run_data.cur_count = run_data.count; run_data.psum_last = step % run_data.psum; if (run_data.psum_last == 0) run_data.psum_last = run_data.psum; switch (run_data.micro) { case 64: micro = 0x03; break; case 128: micro = 0x02; break; case 256: micro = 0x00; break; default: micro = 0x00; break; }; if (run_data.count == 1) psum = ((status - 1) << 8) | (1 << 10) | (micro << 12) | (run_data.psum_last); else psum = ((status - 1) << 8) | (1 << 10) | (micro << 12) | (run_data.psum); mutex_lock(&motor->mutex); ext_dev->is_need_update_tim = is_need_update_tim; ext_dev->is_need_reback = is_need_reback; ext_dev->move_time_us = (run_data.count + 1) * (motor->vd_fz_period_us + 500); if (is_need_reback) ext_dev->move_time_us += ext_dev->reback_move_time_us; ext_dev->last_pos = pos; ext_dev->run_data = run_data; ext_dev->move_status = status; spi_write_reg(motor->spi, 0x20, 0x1a01); spi_write_reg(motor->spi, ext_dev->reg_op->reg.ppw, run_data.ppw | (run_data.ppw << 8)); spi_write_reg(motor->spi, ext_dev->reg_op->reg.psum, psum); spi_write_reg(motor->spi, ext_dev->reg_op->reg.intct, ext_dev->run_data.intct); ext_dev->reg_op->tmp_psum = psum; ext_dev->last_dir = status; if (is_should_wait) { motor->wait_cnt++; } else if (motor->is_timer_restart == false) { motor->is_timer_restart = true; motor->is_timer_restart_bywq = false; hrtimer_start(&motor->timer, ktime_set(0, 0), HRTIMER_MODE_REL); motor->wait_cnt = 0; } else { motor->wait_cnt = 0; } mutex_unlock(&motor->mutex); dev_dbg(&motor->spi->dev, "ext_dev type %d move count %d, psum %d, psum_last %d, move_time_us %u!\n", ext_dev->type, ext_dev->run_data.count, ext_dev->run_data.psum, ext_dev->run_data.psum_last, ext_dev->move_time_us); return ret; } static int motor_dev_parse_dt(struct motor_dev *motor) { struct device_node *node = motor->spi->dev.of_node; int ret = 0; const char *str; int step_motor_cnt = 0; motor->is_use_dc_iris = device_property_read_bool(&motor->spi->dev, "use-dc-iris"); motor->is_use_p_iris = device_property_read_bool(&motor->spi->dev, "use-p-iris"); motor->is_use_focus = device_property_read_bool(&motor->spi->dev, "use-focus"); motor->is_use_zoom = device_property_read_bool(&motor->spi->dev, "use-zoom"); motor->is_use_zoom1 = device_property_read_bool(&motor->spi->dev, "use-zoom1"); /* get reset gpio */ motor->reset_gpio = devm_gpiod_get(&motor->spi->dev, "reset", GPIOD_OUT_LOW); if (IS_ERR(motor->reset_gpio)) dev_err(&motor->spi->dev, "Failed to get reset-gpios\n"); /* get vd_fz gpio */ motor->vd_fz_gpio = devm_gpiod_get(&motor->spi->dev, "vd_fz", GPIOD_OUT_LOW); if (IS_ERR(motor->vd_fz_gpio)) dev_info(&motor->spi->dev, "Failed to get vd_fz-gpios\n"); ret = of_property_read_u32(node, "vd_fz-period-us", &motor->vd_fz_period_us); if (ret != 0) { motor->vd_fz_period_us = VD_FZ_US; dev_err(&motor->spi->dev, "failed get vd_fz-period-us,use dafult value\n"); } ret = of_property_read_u32(node, "id", &motor->id); if (ret != 0) { motor->id = 0; dev_err(&motor->spi->dev, "failed get driver id,use dafult value\n"); } if (motor->is_use_dc_iris) { motor->dciris = devm_kzalloc(&motor->spi->dev, sizeof(*motor->dciris), GFP_KERNEL); if (!motor->dciris) { dev_err(&motor->spi->dev, "__line__ %d, devm_kzalloc return fail!\n", __LINE__); return -ENOMEM; } /* get vd_iris gpio */ motor->dciris->vd_iris_gpio = devm_gpiod_get(&motor->spi->dev, "vd_iris", GPIOD_OUT_LOW); if (IS_ERR(motor->dciris->vd_iris_gpio)) dev_info(&motor->spi->dev, "Failed to get vd_iris-gpios\n"); motor->dciris->is_reversed_polarity = device_property_read_bool(&motor->spi->dev, "dc-iris-reserved-polarity"); ret = of_property_read_u32(node, "dc-iris-max-log", &motor->dciris->max_log); if (ret != 0) { motor->dciris->max_log = DCIRIS_MAX_LOG; dev_err(&motor->spi->dev, "failed get dc-iris max log,use dafult value\n"); } } if (motor->is_use_p_iris) { if (motor->is_use_dc_iris) { dev_err(&motor->spi->dev, "Does not support p-iris and dc-iris on the same module\n"); return -EINVAL; } step_motor_cnt++; motor->piris = devm_kzalloc(&motor->spi->dev, sizeof(*motor->piris), GFP_KERNEL); if (!motor->piris) { dev_err(&motor->spi->dev, "__line__ %d, devm_kzalloc return fail!\n", __LINE__); return -ENOMEM; } ret = of_property_read_string(node, "piris-used-pin", &str); if (ret != 0) { dev_err(&motor->spi->dev, "get piris-used-pin fail, please check it!\n"); return -EINVAL; } if (strcmp(str, "ab") == 0) { motor->piris->reg_op = &motor->motor_op[0]; if (motor->piris->reg_op->is_used) { dev_err(&motor->spi->dev, "__line__ %d, pin already been used\n", __LINE__); return -EINVAL; } motor->piris->reg_op->is_used = true; } else if (strcmp(str, "cd") == 0) { motor->piris->reg_op = &motor->motor_op[1]; if (motor->piris->reg_op->is_used) { dev_err(&motor->spi->dev, "__line__ %d, pin already been used\n", __LINE__); return -EINVAL; } motor->piris->reg_op->is_used = true; } else { dev_err(&motor->spi->dev, "__line__ %d, pin require error\n", __LINE__); return -EINVAL; } ret = of_property_read_u32(node, "piris-backlash", &motor->piris->backlash); if (ret != 0) { motor->piris->backlash = 0; dev_err(&motor->spi->dev, "failed get motor backlash,use dafult value\n"); } ret = of_property_read_u32(node, "piris-start-up-speed", &motor->piris->start_up_speed); if (ret != 0) { motor->piris->start_up_speed = START_UP_HZ_DEF; dev_err(&motor->spi->dev, "failed get motor start up speed,use dafult value\n"); } ret = of_property_read_u32(node, "piris-step-max", &motor->piris->step_max); if (ret != 0) { motor->piris->step_max = PIRIS_MAX_STEP_DEF; dev_err(&motor->spi->dev, "failed get piris pos_max,use dafult value\n"); } ret = of_property_read_u32(node, "piris-ppw", &motor->piris->run_data.ppw); if (ret != 0 || (motor->piris->run_data.ppw > 0xff)) { motor->piris->run_data.ppw = PPW_DEF; dev_err(&motor->spi->dev, "failed get piris ppw,use dafult value\n"); } ret = of_property_read_u32(node, "piris-ppw-stop", &motor->piris->run_data.ppw_stop); if (ret != 0 || (motor->piris->run_data.ppw_stop > 0xff)) { motor->piris->run_data.ppw_stop = PPW_STOP; dev_err(&motor->spi->dev, "failed get piris ppw_stop,use dafult value\n"); } ret = of_property_read_u32(node, "piris-phmode", &motor->piris->run_data.phmode); if (ret != 0 || (motor->piris->run_data.phmode > 0x3f)) { motor->piris->run_data.phmode = PHMODE_DEF; dev_err(&motor->spi->dev, "failed get piris phmode,use dafult value\n"); } ret = of_property_read_u32(node, "piris-micro", &motor->piris->run_data.micro); if (ret != 0) { motor->piris->run_data.micro = MICRO_DEF; dev_err(&motor->spi->dev, "failed get piris micro,use dafult value\n"); } /* get piris pi gpio */ motor->piris->pic_gpio = devm_gpiod_get(&motor->spi->dev, "piris_pic", GPIOD_IN); if (IS_ERR(motor->piris->pic_gpio)) dev_err(&motor->spi->dev, "Failed to get piris-pi-c-gpios\n"); motor->piris->pia_gpio = devm_gpiod_get(&motor->spi->dev, "piris_pia", GPIOD_OUT_LOW); if (IS_ERR(motor->piris->pia_gpio)) dev_err(&motor->spi->dev, "Failed to get piris-pi-a-gpios\n"); motor->piris->pie_gpio = devm_gpiod_get(&motor->spi->dev, "piris_pie", GPIOD_OUT_LOW); if (IS_ERR(motor->piris->pie_gpio)) dev_err(&motor->spi->dev, "Failed to get piris-pi-e-gpios\n"); motor->piris->is_half_step_mode = device_property_read_bool(&motor->spi->dev, "piris-1-2phase-excitation"); motor->piris->is_dir_opp = device_property_read_bool(&motor->spi->dev, "piris-dir-opposite"); ret = of_property_read_s32(node, "piris-min-pos", &motor->piris->min_pos); if (ret != 0) { motor->piris->min_pos = 0; dev_err(&motor->spi->dev, "failed get piris min pos,use dafult value\n"); } ret = of_property_read_s32(node, "piris-max-pos", &motor->piris->max_pos); if (ret != 0) { motor->piris->max_pos = motor->piris->step_max; dev_err(&motor->spi->dev, "failed get piris max_pos pos,use dafult value\n"); } if (step_motor_cnt == 1) motor->dev0 = motor->piris; else if (step_motor_cnt == 2) motor->dev1 = motor->piris; ret = of_property_read_u32(node, "piris-reback-distance", &motor->piris->reback); if (ret != 0) { dev_err(&motor->spi->dev, "failed get piris reback distance, return\n"); return -EINVAL; } } if (motor->is_use_focus) { step_motor_cnt++; motor->focus = devm_kzalloc(&motor->spi->dev, sizeof(*motor->focus), GFP_KERNEL); if (!motor->focus) { dev_err(&motor->spi->dev, "__line__ %d, devm_kzalloc return fail!\n", __LINE__); return -ENOMEM; } ret = of_property_read_string(node, "focus-used-pin", &str); if (ret != 0) { dev_err(&motor->spi->dev, "get focus-used-pin fail, please check it!\n"); return -EINVAL; } if (strcmp(str, "ab") == 0) { motor->focus->reg_op = &motor->motor_op[0]; if (motor->focus->reg_op->is_used) { dev_err(&motor->spi->dev, "__line__ %d, pin already been used\n", __LINE__); return -EINVAL; } motor->focus->reg_op->is_used = true; } else if (strcmp(str, "cd") == 0) { motor->focus->reg_op = &motor->motor_op[1]; if (motor->focus->reg_op->is_used) { dev_err(&motor->spi->dev, "__line__ %d, pin already been used\n", __LINE__); return -EINVAL; } motor->focus->reg_op->is_used = true; } else { dev_err(&motor->spi->dev, "__line__ %d, pin require error\n", __LINE__); return -EINVAL; } ret = of_property_read_u32(node, "focus-backlash", &motor->focus->backlash); if (ret != 0) { motor->focus->backlash = 0; dev_err(&motor->spi->dev, "failed get motor backlash,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 = START_UP_HZ_DEF; dev_err(&motor->spi->dev, "failed get motor start up speed,use dafult value\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->spi->dev, "failed get focus_pos_max,use dafult value\n"); } ret = of_property_read_u32(node, "focus-ppw", &motor->focus->run_data.ppw); if (ret != 0 || (motor->focus->run_data.ppw > 0xff)) { motor->focus->run_data.ppw = PPW_DEF; dev_err(&motor->spi->dev, "failed get focus ppw,use dafult value\n"); } ret = of_property_read_u32(node, "focus-ppw-stop", &motor->focus->run_data.ppw_stop); if (ret != 0 || (motor->focus->run_data.ppw_stop > 0xff)) { motor->focus->run_data.ppw_stop = PPW_STOP; dev_err(&motor->spi->dev, "failed get focus ppw_stop,use dafult value\n"); } ret = of_property_read_u32(node, "focus-phmode", &motor->focus->run_data.phmode); if (ret != 0 || (motor->focus->run_data.phmode > 0x3f)) { motor->focus->run_data.phmode = PHMODE_DEF; dev_err(&motor->spi->dev, "failed get focus phmode,use dafult value\n"); } ret = of_property_read_u32(node, "focus-micro", &motor->focus->run_data.micro); if (ret != 0) { motor->focus->run_data.micro = MICRO_DEF; dev_err(&motor->spi->dev, "failed get focus micro,use dafult value\n"); } if (step_motor_cnt == 1) motor->dev0 = motor->focus; else if (step_motor_cnt == 2) motor->dev1 = motor->focus; /* get focus pi gpio */ motor->focus->pic_gpio = devm_gpiod_get(&motor->spi->dev, "focus_pic", GPIOD_IN); if (IS_ERR(motor->focus->pic_gpio)) dev_err(&motor->spi->dev, "Failed to get focus-pi-c-gpios\n"); motor->focus->pia_gpio = devm_gpiod_get(&motor->spi->dev, "focus_pia", GPIOD_OUT_LOW); if (IS_ERR(motor->focus->pia_gpio)) dev_err(&motor->spi->dev, "Failed to get focus-pi-a-gpios\n"); motor->focus->pie_gpio = devm_gpiod_get(&motor->spi->dev, "focus_pie", GPIOD_OUT_LOW); if (IS_ERR(motor->focus->pie_gpio)) dev_err(&motor->spi->dev, "Failed to get focus-pi-e-gpios\n"); ret = of_property_read_u32(node, "focus-reback-distance", &motor->focus->reback); if (ret != 0) { dev_err(&motor->spi->dev, "failed get focus reback distance, return\n"); return -EINVAL; } motor->focus->is_half_step_mode = device_property_read_bool(&motor->spi->dev, "focus-1-2phase-excitation"); motor->focus->is_dir_opp = device_property_read_bool(&motor->spi->dev, "focus-dir-opposite"); ret = of_property_read_s32(node, "focus-min-pos", &motor->focus->min_pos); if (ret != 0) { motor->focus->min_pos = 0; dev_err(&motor->spi->dev, "failed get focus min pos,use dafult value\n"); } ret = of_property_read_s32(node, "focus-max-pos", &motor->focus->max_pos); if (ret != 0) { motor->focus->max_pos = motor->focus->step_max; dev_err(&motor->spi->dev, "failed get focus max_pos pos,use dafult value\n"); } } if (motor->is_use_zoom) { if (step_motor_cnt >= 2) { dev_err(&motor->spi->dev, "The driver support step-motor max num is 2\n"); return -EINVAL; } step_motor_cnt++; motor->zoom = devm_kzalloc(&motor->spi->dev, sizeof(*motor->zoom), GFP_KERNEL); if (!motor->zoom) { dev_err(&motor->spi->dev, "__line__ %d, devm_kzalloc return fail!\n", __LINE__); return -ENOMEM; } ret = of_property_read_string(node, "zoom-used-pin", &str); if (ret != 0) { dev_err(&motor->spi->dev, "get zoom-used-pin fail, please check it!\n"); return -EINVAL; } if (strcmp(str, "ab") == 0) { motor->zoom->reg_op = &motor->motor_op[0]; if (motor->zoom->reg_op->is_used) { dev_err(&motor->spi->dev, "__line__ %d, pin already been used\n", __LINE__); return -EINVAL; } motor->zoom->reg_op->is_used = true; } else if (strcmp(str, "cd") == 0) { motor->zoom->reg_op = &motor->motor_op[1]; if (motor->zoom->reg_op->is_used) { dev_err(&motor->spi->dev, "__line__ %d, pin already been used\n", __LINE__); return -EINVAL; } motor->zoom->reg_op->is_used = true; } else { dev_err(&motor->spi->dev, "__line__ %d, pin require error\n", __LINE__); return -EINVAL; } ret = of_property_read_u32(node, "zoom-backlash", &motor->zoom->backlash); if (ret != 0) { motor->zoom->backlash = 0; dev_err(&motor->spi->dev, "failed get motor backlash,use dafult value\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->spi->dev, "failed get iris zoom_pos_max,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 = START_UP_HZ_DEF; dev_err(&motor->spi->dev, "failed get motor start up speed,use dafult value\n"); } ret = of_property_read_u32(node, "zoom-ppw", &motor->zoom->run_data.ppw); if (ret != 0 || (motor->zoom->run_data.ppw > 0xff)) { motor->zoom->run_data.ppw = PPW_DEF; dev_err(&motor->spi->dev, "failed get zoom ppw,use dafult value\n"); } ret = of_property_read_u32(node, "zoom-ppw-stop", &motor->zoom->run_data.ppw_stop); if (ret != 0 || (motor->zoom->run_data.ppw_stop > 0xff)) { motor->zoom->run_data.ppw_stop = PPW_STOP; dev_err(&motor->spi->dev, "failed get zoom ppw_stop,use dafult value\n"); } ret = of_property_read_u32(node, "zoom-phmode", &motor->zoom->run_data.phmode); if (ret != 0 || (motor->zoom->run_data.phmode > 0x3ff)) { motor->zoom->run_data.phmode = PHMODE_DEF; dev_err(&motor->spi->dev, "failed get zoom phmode,use dafult value\n"); } ret = of_property_read_u32(node, "zoom-micro", &motor->zoom->run_data.micro); if (ret != 0) { motor->zoom->run_data.micro = MICRO_DEF; dev_err(&motor->spi->dev, "failed get zoom micro,use dafult value\n"); } motor->zoom->pic_gpio = devm_gpiod_get(&motor->spi->dev, "zoom_pic", GPIOD_IN); if (IS_ERR(motor->zoom->pic_gpio)) dev_err(&motor->spi->dev, "Failed to get zoom-pi-c-gpios\n"); motor->zoom->is_half_step_mode = device_property_read_bool(&motor->spi->dev, "zoom-1-2phase-excitation"); motor->zoom->is_dir_opp = device_property_read_bool(&motor->spi->dev, "zoom-dir-opposite"); if (step_motor_cnt == 1) motor->dev0 = motor->zoom; else if (step_motor_cnt == 2) motor->dev1 = motor->zoom; ret = of_property_read_s32(node, "zoom-min-pos", &motor->zoom->min_pos); if (ret != 0) { motor->zoom->min_pos = 0; dev_err(&motor->spi->dev, "failed get zoom min pos,use dafult value\n"); } ret = of_property_read_s32(node, "zoom-max-pos", &motor->zoom->max_pos); if (ret != 0) { motor->zoom->max_pos = motor->zoom->step_max; dev_err(&motor->spi->dev, "failed get zoom max_pos pos,use dafult value\n"); } ret = of_property_read_u32(node, "zoom-reback-distance", &motor->zoom->reback); if (ret != 0) { dev_err(&motor->spi->dev, "failed get zoom reback distance, return\n"); return -EINVAL; } } if (motor->is_use_zoom1) { if (step_motor_cnt >= 2) { dev_err(&motor->spi->dev, "The driver support step-motor max num is 2\n"); return -EINVAL; } step_motor_cnt++; motor->zoom1 = devm_kzalloc(&motor->spi->dev, sizeof(*motor->zoom1), GFP_KERNEL); if (!motor->zoom1) { dev_err(&motor->spi->dev, "__line__ %d, devm_kzalloc return fail!\n", __LINE__); return -ENOMEM; } ret = of_property_read_string(node, "zoom1-used-pin", &str); if (ret != 0) { dev_err(&motor->spi->dev, "get zoom1-used-pin fail, please check it!\n"); return -EINVAL; } if (strcmp(str, "ab") == 0) { motor->zoom1->reg_op = &motor->motor_op[0]; if (motor->zoom1->reg_op->is_used) { dev_err(&motor->spi->dev, "__line__ %d, pin already been used\n", __LINE__); return -EINVAL; } motor->zoom1->reg_op->is_used = true; } else if (strcmp(str, "cd") == 0) { motor->zoom1->reg_op = &motor->motor_op[1]; if (motor->zoom1->reg_op->is_used) { dev_err(&motor->spi->dev, "__line__ %d, pin already been used\n", __LINE__); return -EINVAL; } motor->zoom1->reg_op->is_used = true; } else { dev_err(&motor->spi->dev, "__line__ %d, pin require error\n", __LINE__); return -EINVAL; } ret = of_property_read_u32(node, "zoom1-backlash", &motor->zoom1->backlash); if (ret != 0) { motor->zoom1->backlash = 0; dev_err(&motor->spi->dev, "failed get motor backlash,use dafult value\n"); } ret = of_property_read_u32(node, "zoom1-step-max", &motor->zoom1->step_max); if (ret != 0) { motor->zoom1->step_max = ZOOM_MAX_STEP_DEF; dev_err(&motor->spi->dev, "failed get zoom_pos_max,use dafult value\n"); } ret = of_property_read_u32(node, "zoom1-start-up-speed", &motor->zoom1->start_up_speed); if (ret != 0) { motor->zoom1->start_up_speed = START_UP_HZ_DEF; dev_err(&motor->spi->dev, "failed get motor start up speed,use dafult value\n"); } ret = of_property_read_u32(node, "zoom1-ppw", &motor->zoom1->run_data.ppw); if (ret != 0 || (motor->zoom1->run_data.ppw > 0xff)) { motor->zoom1->run_data.ppw = PPW_DEF; dev_err(&motor->spi->dev, "failed get zoom1 ppw,use dafult value\n"); } ret = of_property_read_u32(node, "zoom1-ppw-stop", &motor->zoom1->run_data.ppw_stop); if (ret != 0 || (motor->zoom1->run_data.ppw_stop > 0xff)) { motor->zoom1->run_data.ppw_stop = PPW_STOP; dev_err(&motor->spi->dev, "failed get zoom1 ppw_stop,use dafult value\n"); } ret = of_property_read_u32(node, "zoom1-phmode", &motor->zoom1->run_data.phmode); if (ret != 0 || (motor->zoom1->run_data.phmode > 0x3f)) { motor->zoom1->run_data.phmode = PHMODE_DEF; dev_err(&motor->spi->dev, "failed get zoom1 phmode,use dafult value\n"); } ret = of_property_read_u32(node, "zoom1-micro", &motor->zoom1->run_data.micro); if (ret != 0) { motor->zoom1->run_data.micro = MICRO_DEF; dev_err(&motor->spi->dev, "failed get zoom1 micro,use dafult value\n"); } /* get zoom1 pi gpio */ motor->zoom1->pic_gpio = devm_gpiod_get(&motor->spi->dev, "zoom1_pic", GPIOD_IN); if (IS_ERR(motor->zoom1->pic_gpio)) dev_err(&motor->spi->dev, "Failed to get zoom1-pi-c-gpios\n"); motor->zoom1->pia_gpio = devm_gpiod_get(&motor->spi->dev, "zoom1_pia", GPIOD_OUT_LOW); if (IS_ERR(motor->zoom1->pia_gpio)) dev_err(&motor->spi->dev, "Failed to get zoom1-pi-a-gpios\n"); motor->zoom1->pie_gpio = devm_gpiod_get(&motor->spi->dev, "zoom1_pie", GPIOD_OUT_LOW); if (IS_ERR(motor->zoom1->pie_gpio)) dev_err(&motor->spi->dev, "Failed to get zoom1-pi-e-gpios\n"); motor->zoom1->is_half_step_mode = device_property_read_bool(&motor->spi->dev, "zoom1-1-2phase-excitation"); motor->zoom1->is_dir_opp = device_property_read_bool(&motor->spi->dev, "zoom1-dir-opposite"); if (step_motor_cnt == 1) motor->dev0 = motor->zoom1; else if (step_motor_cnt == 2) motor->dev1 = motor->zoom1; ret = of_property_read_s32(node, "zoom1-min-pos", &motor->zoom1->min_pos); if (ret != 0) { motor->zoom1->min_pos = 0; dev_err(&motor->spi->dev, "failed get zoom1 min pos,use dafult value\n"); } ret = of_property_read_s32(node, "zoom1-max-pos", &motor->zoom1->max_pos); if (ret != 0) { motor->zoom1->max_pos = motor->zoom1->step_max; dev_err(&motor->spi->dev, "failed get zoom1 max_pos pos,use dafult value\n"); } ret = of_property_read_u32(node, "zoom1-reback-distance", &motor->zoom1->reback); if (ret != 0) { dev_err(&motor->spi->dev, "failed get zoom1 reback distance, return\n"); return -EINVAL; } } 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->spi->dev, "could not get module information!\n"); return -EINVAL; } return 0; } static void motor_config_dev_next_status(struct motor_dev *motor, struct ext_dev *dev) { u16 ppw = 0; u16 psum = 0; u16 micro = 0; struct spi_device *spi = motor->spi; #ifdef DEBUG u16 intct = 0; int i = 0; u16 val = 0; if (dev->move_status != MOTOR_STATUS_STOPPED) { dev_dbg(&spi->dev, "__line__ %d dev type %d, cur_count %d !\n", __LINE__, dev->type, dev->run_data.cur_count); dev_dbg(&spi->dev, "__line__ %d, motor reg table: 0x%02x 0x%02x 0x%02x!\n", __LINE__, dev->reg_op->reg.ppw, dev->reg_op->reg.psum, dev->reg_op->reg.intct); for (i = 0; i < 11; i++) { spi_read_reg(spi, 0x20 + i, &val); dev_dbg(&spi->dev, "========reg,val= 0x%02x, 0x%04x========\n", 0x20 + i, val); } } #endif #ifdef DEBUG spi_read_reg(spi, dev->reg_op->reg.ppw, &ppw); spi_read_reg(spi, dev->reg_op->reg.psum, &psum); spi_read_reg(spi, dev->reg_op->reg.intct, &intct); dev_info(&spi->dev, "__line__ %d dev type %d, cur_count %d , status %d! ppw 0x%x, psum 0x%x intct 0x%x\n", __LINE__, dev->type, dev->run_data.cur_count, dev->move_status, ppw, psum, intct); #endif if (dev->run_data.cur_count != 0) { if (dev->run_data.cur_count == dev->run_data.count && dev->is_need_update_tim) { dev->mv_tim.vcm_start_t = ns_to_timeval(ktime_get_ns()); dev->is_need_update_tim = false; } dev->run_data.cur_count--; ppw = (dev->run_data.ppw << 8) | dev->run_data.ppw; switch (dev->run_data.micro) { case 64: micro = 0x03; break; case 128: micro = 0x02; break; case 256: micro = 0x00; break; default: micro = 0x00; break; }; switch (dev->run_data.cur_count) { case 0: psum = ((dev->move_status - 1) << 8) | (micro << 12) | (1 << 10); ppw = (dev->run_data.ppw_stop << 8) | dev->run_data.ppw_stop; break; case 1: psum = ((dev->move_status - 1) << 8) | (1 << 10) | (micro << 12) | (dev->run_data.psum_last); break; default: psum = ((dev->move_status - 1) << 8) | (1 << 10) | (micro << 12) | (dev->run_data.psum); break; }; spi_write_reg(motor->spi, 0x20, 0x1a01); spi_write_reg(spi, dev->reg_op->reg.ppw, ppw); spi_write_reg(spi, dev->reg_op->reg.psum, psum); spi_write_reg(spi, dev->reg_op->reg.intct, dev->run_data.intct); dev->reg_op->tmp_psum = psum; } else if (dev->move_status != MOTOR_STATUS_STOPPED) { dev->mv_tim.vcm_end_t = ns_to_timeval(ktime_get_ns()); dev->is_mv_tim_update = true; dev->move_status = MOTOR_STATUS_STOPPED; dev->reg_op->tmp_psum = 0; dev->is_running = false; complete(&dev->complete_out); complete(&dev->complete); } } static void motor_op_work(struct work_struct *work) { struct motor_work_s *wk = container_of(work, struct motor_work_s, work); struct motor_dev *motor = wk->dev; static struct timeval tv_last = {0}; struct timeval tv = {0}; u64 time_dist = 0; 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 (time_dist < motor->vd_fz_period_us && motor->is_timer_restart_bywq) dev_info(&motor->spi->dev, "Timer error, Current interrupt interval %llu\n", time_dist); mutex_lock(&motor->mutex); gpiod_set_value(motor->vd_fz_gpio, 1); usleep_range(30, 60); gpiod_set_value(motor->vd_fz_gpio, 0); if (motor->dev0 && motor->dev0->run_data.cur_count == 0 && motor->dev0->is_need_reback) { if (motor->dev0->cur_back_delay < motor->dev0->max_back_delay) { motor->dev0->cur_back_delay++; motor->dev0->run_data.cur_count = 1; } else { motor->dev0->run_data = motor->dev0->reback_data; motor->dev0->is_need_reback = false; motor->dev0->move_status = motor->dev0->reback_status; motor->dev0->last_dir = motor->dev1->reback_status; motor->dev0->cur_back_delay = 0; } } if (motor->dev1 && motor->dev1->run_data.cur_count == 0 && motor->dev1->is_need_reback) { if (motor->dev1->cur_back_delay < motor->dev1->max_back_delay) { motor->dev1->cur_back_delay++; motor->dev1->run_data.cur_count = 1; } else { motor->dev1->run_data = motor->dev1->reback_data; motor->dev1->is_need_reback = false; motor->dev1->move_status = motor->dev1->reback_status; motor->dev1->last_dir = motor->dev1->reback_status; motor->dev1->cur_back_delay = 0; } } if ((motor->dev0 && motor->dev0->run_data.cur_count > 0) || (motor->dev1 && motor->dev1->run_data.cur_count > 0)) { motor->is_timer_restart = true; motor->is_timer_restart_bywq = true; hrtimer_start(&motor->timer, motor->vd_fz_period_us * 1000, HRTIMER_MODE_REL); } else { motor->is_timer_restart = false; motor->is_timer_restart_bywq = false; } usleep_range(660, 700);//delay more than DT1 if (motor->dev0 && motor->dev0->move_status != MOTOR_STATUS_STOPPED) motor_config_dev_next_status(motor, motor->dev0); if (motor->dev1 && motor->dev1->move_status != MOTOR_STATUS_STOPPED) motor_config_dev_next_status(motor, motor->dev1); mutex_unlock(&motor->mutex); motor->is_should_wait = false; } static enum hrtimer_restart motor_timer_func(struct hrtimer *timer) { struct motor_dev *motor = container_of(timer, struct motor_dev, timer); motor->is_should_wait = true; schedule_work_on(smp_processor_id(), &motor->wk->work); return HRTIMER_NORESTART; } 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_IRIS_ABSOLUTE: if (motor->is_use_dc_iris) ctrl->val = motor->dciris->last_log; else if (motor->is_use_p_iris) ctrl->val = motor->piris->last_pos; return 0; 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; case V4L2_CID_ZOOM_CONTINUOUS: ctrl->val = motor->zoom1->last_pos; return 0; } return 0; } static void wait_for_motor_stop(struct motor_dev *motor, struct ext_dev *dev) { unsigned long ret = 0; if (dev->is_running) { ret = wait_for_completion_timeout(&dev->complete_out, 10 * HZ); if (ret == 0) dev_info(&motor->spi->dev, "dev->type %d, wait for complete timeout\n", dev->type); } } static int motor_s_ctrl(struct v4l2_ctrl *ctrl) { #ifdef DEBUG int i = 0; u16 val = 0; #endif 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_IRIS_ABSOLUTE: if (motor->is_use_dc_iris) { if (motor->dciris->is_reversed_polarity) spi_write_reg(motor->spi, 0x00, motor->dciris->max_log - ctrl->val); else spi_write_reg(motor->spi, 0x00, ctrl->val); gpiod_set_value(motor->dciris->vd_iris_gpio, 1); usleep_range(200, 400); gpiod_set_value(motor->dciris->vd_iris_gpio, 0); motor->dciris->last_log = ctrl->val; dev_dbg(&motor->spi->dev, "set iris pos %d\n", ctrl->val); #ifdef DEBUG for (i = 0; i < 16; i++) { spi_read_reg(motor->spi, i, &val); dev_dbg(&motor->spi->dev, "reg,val=0x%02x,0x%04x\n", i, val); } #endif } else if (motor->is_use_p_iris) { ret = set_motor_running_status(motor, motor->piris, ctrl->val, true, false, false); wait_for_motor_stop(motor, motor->piris); dev_dbg(&motor->spi->dev, "set piris pos %d\n", ctrl->val); } break; 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, true, false, is_need_reback); wait_for_motor_stop(motor, motor->focus); dev_dbg(&motor->spi->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, true, false, is_need_reback); wait_for_motor_stop(motor, motor->zoom); dev_dbg(&motor->spi->dev, "set zoom pos %d\n", ctrl->val); break; case V4L2_CID_ZOOM_CONTINUOUS: if (motor->zoom1->reback_ctrl) { if (ctrl->val >= motor->zoom1->last_pos) is_need_reback = false; else is_need_reback = true; } ret = set_motor_running_status(motor, motor->zoom1, ctrl->val, true, false, is_need_reback); wait_for_motor_stop(motor, motor->zoom1); dev_dbg(&motor->spi->dev, "set zoom1 pos %d\n", ctrl->val); break; default: dev_err(&motor->spi->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; int ret = 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++) { dev_dbg(&motor->spi->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); if (i == (mv_param->setzoom_cnt - 1)) { ret = set_motor_running_status(motor, motor->focus, mv_param->zoom_pos[i].focus_pos, true, true, is_need_focus_reback); ret = set_motor_running_status(motor, motor->zoom, mv_param->zoom_pos[i].zoom_pos, true, false, is_need_zoom_reback); } else { set_motor_running_status(motor, motor->focus, mv_param->zoom_pos[i].focus_pos, false, true, false); set_motor_running_status(motor, motor->zoom, mv_param->zoom_pos[i].zoom_pos, false, false, false); } wait_for_motor_stop(motor, motor->focus); wait_for_motor_stop(motor, motor->zoom); } return ret; } static int motor_find_pi_binarysearch(struct motor_dev *motor, struct ext_dev *ext_dev, int min, int max) { int gpio_val = 0; int tmp_val = 0; int mid = 0; int last_pos = 0; int new_min = 0; int new_max = 0; if (min > max) return -EINVAL; tmp_val = gpiod_get_value(ext_dev->pic_gpio); mid = (min + max) / 2; if (mid == min) { dev_dbg(&motor->spi->dev, "ext dev %d find pi %d\n", ext_dev->type, mid); if (ext_dev->last_pos < mid) set_motor_running_status(motor, ext_dev, mid, false, false, false); else set_motor_running_status(motor, ext_dev, mid, false, false, true); wait_for_motor_stop(motor, ext_dev); return mid; } last_pos = ext_dev->last_pos; if (last_pos < mid) set_motor_running_status(motor, ext_dev, mid, false, false, false); else set_motor_running_status(motor, ext_dev, mid, false, false, true); wait_for_motor_stop(motor, ext_dev); gpio_val = gpiod_get_value(ext_dev->pic_gpio); if (tmp_val != gpio_val) { usleep_range(10, 20); gpio_val = gpiod_get_value(ext_dev->pic_gpio); } dev_dbg(&motor->spi->dev, "__line__ %d ext_dev type %d, get pi value %d, tmp_val %d, min %d, max %d\n", __LINE__, ext_dev->type, gpio_val, tmp_val, min, max); if (tmp_val != gpio_val) { if (last_pos == min) { new_min = min; new_max = mid; } else { new_min = mid; new_max = max; } } else { if (last_pos == min) { new_min = mid; new_max = max; } else { new_min = min; new_max = mid; } } return motor_find_pi_binarysearch(motor, ext_dev, new_min, new_max); } static int motor_find_pi(struct motor_dev *motor, struct ext_dev *ext_dev, int step) { int i = 0; int idx_max = ext_dev->step_max + step - 1; int tmp_val = 0; int gpio_val = 0; int min = 0; int max = 0; bool is_find_pi = false; tmp_val = gpiod_get_value(ext_dev->pic_gpio); for (i = ext_dev->last_pos + step; i < idx_max; i += step) { set_motor_running_status(motor, ext_dev, i, false, false, false); wait_for_motor_stop(motor, ext_dev); gpio_val = gpiod_get_value(ext_dev->pic_gpio); if (tmp_val != gpio_val) { usleep_range(10, 20); gpio_val = gpiod_get_value(ext_dev->pic_gpio); } dev_dbg(&motor->spi->dev, "__line__ %d ext_dev type %d, get pi value %d, i %d, tmp_val %d\n", __LINE__, ext_dev->type, gpio_val, i, tmp_val); if (tmp_val != gpio_val) { min = i - step; max = i; is_find_pi = true; break; } } if (i > idx_max) { for (i = ext_dev->last_pos - step; i > 0; i -= step) { set_motor_running_status(motor, ext_dev, i, false, false, true); wait_for_motor_stop(motor, ext_dev); gpio_val = gpiod_get_value(ext_dev->pic_gpio); if (tmp_val != gpio_val) { usleep_range(10, 20); gpio_val = gpiod_get_value(ext_dev->pic_gpio); } dev_dbg(&motor->spi->dev, "__line__ %d ext_dev type %d, get pi value %d, i %d, tmp_val %d\n", __LINE__, ext_dev->type, gpio_val, i, tmp_val); if (tmp_val != gpio_val) { min = i; max = i + step; is_find_pi = true; break; } } } if (is_find_pi) { if (abs(step) == 1) return ext_dev->last_pos; else return motor_find_pi_binarysearch(motor, ext_dev, min, max); } else { return -EINVAL; } } static int motor_reinit_piris(struct motor_dev *motor) { int ret = 0; if (!IS_ERR(motor->piris->pic_gpio)) { if (!IS_ERR(motor->piris->pia_gpio)) gpiod_set_value(motor->piris->pia_gpio, 1); if (!IS_ERR(motor->piris->pie_gpio)) gpiod_set_value(motor->piris->pie_gpio, 0); msleep(250); #ifdef PI_TEST motor->piris->last_pos = motor->piris->step_max; ret = set_motor_running_status(motor, motor->piris, 0, false, false, false); wait_for_motor_stop(motor, motor->piris); #else motor->piris->last_pos = 0; #endif ret = motor_find_pi(motor, motor->piris, 10); if (ret < 0) { dev_err(&motor->spi->dev, "get piris pi fail, pls check it\n"); return -EINVAL; } #ifdef PI_TEST min = -ret; max = motor->piris->step_max + min; motor->piris->min_pos = min; motor->piris->max_pos = max; #endif if (!IS_ERR(motor->piris->pia_gpio)) gpiod_set_value(motor->piris->pia_gpio, 0); if (!IS_ERR(motor->piris->pie_gpio)) gpiod_set_value(motor->piris->pie_gpio, 0); motor->piris->last_pos = 0; } else { motor->piris->last_pos = motor->piris->step_max; ret = set_motor_running_status(motor, motor->piris, 0, false, false, false); wait_for_motor_stop(motor, motor->piris); } return 0; } static void motor_reinit_piris_pos(struct motor_dev *motor) { if (!motor->piris) { dev_err(&motor->spi->dev, "not support piris\n"); return; } motor_reinit_piris(motor); motor->piris->last_pos = 0; __v4l2_ctrl_modify_range(motor->iris_ctrl, motor->piris->min_pos, motor->piris->max_pos - motor->piris->reback, 1, 0); } static int motor_reinit_focus(struct motor_dev *motor) { int ret = 0; if (!IS_ERR(motor->focus->pic_gpio)) { mutex_lock(&motor->mutex); if (motor->pi_gpio_usecnt == 0) { if (!IS_ERR(motor->focus->pia_gpio)) gpiod_set_value(motor->focus->pia_gpio, 1); if (!IS_ERR(motor->focus->pie_gpio)) gpiod_set_value(motor->focus->pie_gpio, 0); msleep(250); } motor->pi_gpio_usecnt++; mutex_unlock(&motor->mutex); #ifdef PI_TEST motor->focus->last_pos = motor->focus->step_max; ret = set_motor_running_status(motor, motor->focus, 0, false, false, false); wait_for_motor_stop(motor, motor->focus); #else motor->focus->last_pos = 0; #endif ret = motor_find_pi(motor, motor->focus, 200); if (ret < 0) { dev_info(&motor->spi->dev, "get focus pi fail, pls check it\n"); return -EINVAL; } #ifdef PI_TEST min = -ret; max = motor->focus->step_max + min; motor->focus->min_pos = min; motor->focus->max_pos = max; #endif mutex_lock(&motor->mutex); if (motor->pi_gpio_usecnt == 1) { if (!IS_ERR(motor->focus->pia_gpio)) gpiod_set_value(motor->focus->pia_gpio, 0); if (!IS_ERR(motor->focus->pie_gpio)) gpiod_set_value(motor->focus->pie_gpio, 0); } motor->pi_gpio_usecnt--; mutex_unlock(&motor->mutex); } else { motor->focus->last_pos = motor->focus->step_max; ret = set_motor_running_status(motor, motor->focus, 0, false, false, true); wait_for_motor_stop(motor, motor->focus); } return 0; } static void motor_reinit_focus_pos(struct motor_dev *motor) { if (!motor->focus) { dev_err(&motor->spi->dev, "not support focus\n"); return; } motor_reinit_focus(motor); motor->focus->last_pos = 0; __v4l2_ctrl_modify_range(motor->focus_ctrl, motor->focus->min_pos, motor->focus->max_pos - motor->focus->reback, 1, 0); } static int motor_reinit_zoom(struct motor_dev *motor) { int ret = 0; if (!IS_ERR(motor->zoom->pic_gpio)) { mutex_lock(&motor->mutex); if (motor->pi_gpio_usecnt == 0) { if (!IS_ERR(motor->focus->pia_gpio)) gpiod_set_value(motor->focus->pia_gpio, 1); if (!IS_ERR(motor->focus->pie_gpio)) gpiod_set_value(motor->focus->pie_gpio, 0); msleep(250); } motor->pi_gpio_usecnt++; mutex_unlock(&motor->mutex); #ifdef PI_TEST motor->zoom->last_pos = motor->zoom->step_max; ret = set_motor_running_status(motor, motor->zoom, 0, false, false, false); wait_for_motor_stop(motor, motor->zoom); #else motor->zoom->last_pos = 0; #endif ret = motor_find_pi(motor, motor->zoom, 200); if (ret < 0) { dev_err(&motor->spi->dev, "get zoom pi fail, pls check it\n"); return -EINVAL; } #ifdef PI_TEST min = -ret; max = motor->zoom->step_max + min; motor->zoom->min_pos = min; motor->zoom->max_pos = max; #endif mutex_lock(&motor->mutex); if (motor->pi_gpio_usecnt == 1) { if (!IS_ERR(motor->focus->pia_gpio)) gpiod_set_value(motor->focus->pia_gpio, 0); if (!IS_ERR(motor->focus->pie_gpio)) gpiod_set_value(motor->focus->pie_gpio, 0); } motor->pi_gpio_usecnt--; mutex_unlock(&motor->mutex); } else { motor->zoom->last_pos = motor->zoom->step_max; ret = set_motor_running_status(motor, motor->zoom, 0, false, false, true); wait_for_motor_stop(motor, motor->zoom); } return 0; } static void motor_reinit_zoom_pos(struct motor_dev *motor) { if (!motor->zoom) { dev_err(&motor->spi->dev, "not support zoom\n"); return; } motor_reinit_zoom(motor); motor->zoom->last_pos = 0; __v4l2_ctrl_modify_range(motor->zoom_ctrl, motor->zoom->min_pos, motor->zoom->max_pos - motor->zoom->reback, 1, 0); } static int motor_reinit_zoom1(struct motor_dev *motor) { int ret = 0; if (!IS_ERR(motor->zoom1->pic_gpio)) { if (!IS_ERR(motor->zoom1->pia_gpio)) gpiod_set_value(motor->zoom1->pia_gpio, 1); if (!IS_ERR(motor->zoom1->pie_gpio)) gpiod_set_value(motor->zoom1->pie_gpio, 0); msleep(250); #ifdef PI_TEST motor->zoom1->last_pos = motor->zoom1->step_max; ret = set_motor_running_status(motor, motor->zoom1, 0, false, false, false); wait_for_motor_stop(motor, motor->zoom1); #else motor->zoom1->last_pos = 0; #endif ret = motor_find_pi(motor, motor->zoom1, 200); if (ret < 0) { dev_err(&motor->spi->dev, "get zoom1 pi fail, pls check it\n"); return -EINVAL; } #ifdef PI_TEST min = -ret; max = motor->zoom1->step_max + min; motor->zoom1->min_pos = min; motor->zoom1->max_pos = max; #endif if (!IS_ERR(motor->zoom1->pia_gpio)) gpiod_set_value(motor->zoom1->pia_gpio, 0); if (!IS_ERR(motor->zoom1->pie_gpio)) gpiod_set_value(motor->zoom1->pie_gpio, 0); } else { motor->zoom1->last_pos = motor->zoom1->step_max; ret = set_motor_running_status(motor, motor->zoom1, 0, false, false, true); wait_for_motor_stop(motor, motor->zoom1); } return 0; } static void motor_reinit_zoom1_pos(struct motor_dev *motor) { if (!motor->zoom1) { dev_err(&motor->spi->dev, "not support zoom1\n"); return; } motor_reinit_zoom1(motor); motor->zoom1->last_pos = 0; __v4l2_ctrl_modify_range(motor->zoom1_ctrl, motor->zoom1->min_pos, motor->zoom1->max_pos - motor->zoom1->reback, 1, 0); } //#define REBACK_CTRL_BY_DRV 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 dev_dbg(&motor->spi->dev, "%s focus %d\n", __func__, mv_param->focus_pos); ret = set_motor_running_status(motor, motor->focus, mv_param->focus_pos, true, false, is_need_reback); wait_for_motor_stop(motor, motor->focus); 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 = to_motor_dev(sd); u32 *pbacklash = 0; struct rk_cam_set_zoom *mv_param; struct rk_cam_set_focus *focus_param; int ret = 0; struct rk_cam_modify_pos *pos; switch (cmd) { case RK_VIDIOC_IRIS_TIMEINFO: mv_tim = (struct rk_cam_vcm_tim *)arg; if (!motor->piris->is_mv_tim_update) usleep_range(motor->piris->move_time_us, motor->piris->move_time_us + 1000); if (motor->piris->is_mv_tim_update) { memcpy(mv_tim, &motor->piris->mv_tim, sizeof(*mv_tim)); dev_dbg(&motor->spi->dev, "get_piris_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); } else { dev_err(&motor->spi->dev, "get_piris_move_tim failed\n"); return -EINVAL; } break; case RK_VIDIOC_VCM_TIMEINFO: mv_tim = (struct rk_cam_vcm_tim *)arg; if (!motor->focus->is_mv_tim_update) usleep_range(motor->focus->move_time_us, motor->focus->move_time_us + 1000); if (motor->focus->is_mv_tim_update) { memcpy(mv_tim, &motor->focus->mv_tim, sizeof(*mv_tim)); dev_dbg(&motor->spi->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); } else { dev_err(&motor->spi->dev, "get_focus_move_tim failed\n"); return -EINVAL; } break; case RK_VIDIOC_ZOOM_TIMEINFO: mv_tim = (struct rk_cam_vcm_tim *)arg; if (!motor->zoom->is_mv_tim_update) usleep_range(motor->zoom->move_time_us, motor->zoom->move_time_us + 1000); if (motor->zoom->is_mv_tim_update) { memcpy(mv_tim, &motor->zoom->mv_tim, sizeof(*mv_tim)); dev_dbg(&motor->spi->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); } else { dev_err(&motor->spi->dev, "get_zoom_move_tim failed\n"); return -EINVAL; } break; case RK_VIDIOC_ZOOM1_TIMEINFO: mv_tim = (struct rk_cam_vcm_tim *)arg; if (!motor->zoom1->is_mv_tim_update) usleep_range(motor->zoom1->move_time_us, motor->zoom1->move_time_us + 1000); if (motor->zoom1->is_mv_tim_update) { memcpy(mv_tim, &motor->zoom1->mv_tim, sizeof(*mv_tim)); dev_dbg(&motor->spi->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); } else { dev_err(&motor->spi->dev, "get_zoom_move_tim failed\n"); return -EINVAL; } break; case RK_VIDIOC_IRIS_SET_BACKLASH: pbacklash = (u32 *)arg; motor->piris->backlash = *pbacklash; break; case RK_VIDIOC_FOCUS_SET_BACKLASH: pbacklash = (u32 *)arg; motor->focus->backlash = *pbacklash; break; case RK_VIDIOC_ZOOM_SET_BACKLASH: pbacklash = (u32 *)arg; motor->zoom->backlash = *pbacklash; break; case RK_VIDIOC_ZOOM1_SET_BACKLASH: pbacklash = (u32 *)arg; motor->zoom1->backlash = *pbacklash; break; case RK_VIDIOC_IRIS_CORRECTION: motor_reinit_piris_pos(motor); 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_ZOOM1_CORRECTION: motor_reinit_zoom1_pos(motor); break; case RK_VIDIOC_ZOOM_SET_POSITION: mv_param = (struct rk_cam_set_zoom *)arg; ret = motor_set_zoom_follow(motor, mv_param); break; case RK_VIDIOC_FOCUS_SET_POSITION: focus_param = (struct rk_cam_set_focus *)arg; ret = motor_set_focus(motor, focus_param); break; case RK_VIDIOC_MODIFY_POSITION: pos = (struct rk_cam_modify_pos *)arg; if (motor->focus) motor->focus->last_pos = pos->focus_pos; if (motor->zoom) motor->zoom->last_pos = pos->zoom_pos; if (motor->zoom1) motor->zoom1->last_pos = pos->zoom1_pos; break; default: break; } return ret; } #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_compat_vcm_tim *compat_mv_tim; struct rk_cam_set_zoom *mv_param; struct rk_cam_set_focus *focus_param; struct rk_cam_vcm_tim ioctl_mv_tim; unsigned int ioctl_cmd; int ret = 0; u32 val = 0; switch (cmd) { case RK_VIDIOC_COMPAT_VCM_TIMEINFO: ioctl_cmd = RK_VIDIOC_VCM_TIMEINFO; goto handle_mvtime; case RK_VIDIOC_COMPAT_IRIS_TIMEINFO: ioctl_cmd = RK_VIDIOC_IRIS_TIMEINFO; goto handle_mvtime; case RK_VIDIOC_COMPAT_ZOOM_TIMEINFO: ioctl_cmd = RK_VIDIOC_ZOOM_TIMEINFO; goto handle_mvtime; case RK_VIDIOC_COMPAT_ZOOM1_TIMEINFO: ioctl_cmd = RK_VIDIOC_ZOOM1_TIMEINFO; handle_mvtime: compat_mv_tim = kzalloc(sizeof(*compat_mv_tim), GFP_KERNEL); if (!compat_mv_tim) { ret = -ENOMEM; return ret; } ret = motor_ioctl(sd, ioctl_cmd, &ioctl_mv_tim); if (!ret) { compat_mv_tim->vcm_start_t.tv_sec = ioctl_mv_tim.vcm_start_t.tv_sec; compat_mv_tim->vcm_start_t.tv_usec = ioctl_mv_tim.vcm_start_t.tv_usec; compat_mv_tim->vcm_end_t.tv_sec = ioctl_mv_tim.vcm_end_t.tv_sec; compat_mv_tim->vcm_end_t.tv_usec = ioctl_mv_tim.vcm_end_t.tv_usec; if (copy_to_user(up, compat_mv_tim, sizeof(*compat_mv_tim))) { kfree(compat_mv_tim); return -EFAULT; } } kfree(compat_mv_tim); break; case RK_VIDIOC_IRIS_SET_BACKLASH: case RK_VIDIOC_FOCUS_SET_BACKLASH: case RK_VIDIOC_ZOOM_SET_BACKLASH: case RK_VIDIOC_ZOOM1_SET_BACKLASH: if (copy_from_user(&val, up, sizeof(val))) return -EFAULT; ret = motor_ioctl(sd, cmd, &val); break; case RK_VIDIOC_IRIS_CORRECTION: case RK_VIDIOC_FOCUS_CORRECTION: case RK_VIDIOC_ZOOM_CORRECTION: case RK_VIDIOC_ZOOM1_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; default: break; } return ret; } #endif #define USED_SYS_DEBUG #ifdef USED_SYS_DEBUG static ssize_t set_pid_dgain(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 = to_motor_dev(sd); int val = 0; int ret = 0; u16 reg_val = 0; ret = kstrtoint(buf, 0, &val); if (!ret) { if (motor->is_use_dc_iris) { spi_read_reg(motor->spi, 0x01, ®_val); reg_val &= 0x01ff; reg_val |= (val & 0x7f) << 9; spi_write_reg(motor->spi, 0x01, reg_val); gpiod_set_value(motor->dciris->vd_iris_gpio, 1); usleep_range(200, 400); gpiod_set_value(motor->dciris->vd_iris_gpio, 0); dev_info(dev, "set pid dgain %d, reg val 0x%x\n", val, reg_val); spi_read_reg(motor->spi, 0x01, ®_val); dev_info(dev, "pid dgain reg val 0x%x, read from register\n", reg_val); } else { dev_err(dev, "not support dc-iris, do nothing\n"); } } return count; } static ssize_t set_pid_zero(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 = to_motor_dev(sd); int val = 0; int ret = 0; u16 reg_val = 0; ret = kstrtoint(buf, 0, &val); if (!ret) { if (motor->is_use_dc_iris) { spi_read_reg(motor->spi, 0x02, ®_val); reg_val &= 0xf0ff; reg_val |= (val & 0xf) << 8; spi_write_reg(motor->spi, 0x02, reg_val); gpiod_set_value(motor->dciris->vd_iris_gpio, 1); usleep_range(200, 400); gpiod_set_value(motor->dciris->vd_iris_gpio, 0); dev_info(dev, "set pid zero %d, reg val 0x%x\n", val, reg_val); spi_read_reg(motor->spi, 0x02, ®_val); dev_info(dev, "pid zero reg val 0x%x, read from register\n", reg_val); } else { dev_err(dev, "not support dc-iris, do nothing\n"); } } return count; } static ssize_t set_pid_pole(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 = to_motor_dev(sd); int val = 0; int ret = 0; u16 reg_val = 0; ret = kstrtoint(buf, 0, &val); if (!ret) { if (motor->is_use_dc_iris) { spi_read_reg(motor->spi, 0x02, ®_val); reg_val &= 0x0fff; reg_val |= (val & 0xf) << 12; spi_write_reg(motor->spi, 0x02, reg_val); gpiod_set_value(motor->dciris->vd_iris_gpio, 1); usleep_range(200, 400); gpiod_set_value(motor->dciris->vd_iris_gpio, 0); dev_info(dev, "set pid pole %d, reg val 0x%x\n", val, reg_val); spi_read_reg(motor->spi, 0x02, ®_val); dev_info(dev, "pid pole reg val 0x%x, read from register\n", reg_val); } else { dev_err(dev, "not support dc-iris, do nothing\n"); } } return count; } static ssize_t set_hall_bias(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 = to_motor_dev(sd); int val = 0; int ret = 0; u16 reg_val = 0; ret = kstrtoint(buf, 0, &val); if (!ret) { if (motor->is_use_dc_iris) { spi_read_reg(motor->spi, 0x04, ®_val); reg_val &= 0xff00; reg_val |= val & 0xff; spi_write_reg(motor->spi, 0x04, reg_val); gpiod_set_value(motor->dciris->vd_iris_gpio, 1); usleep_range(200, 400); gpiod_set_value(motor->dciris->vd_iris_gpio, 0); dev_info(dev, "set hall_bias %d, reg val 0x%x\n", val, reg_val); spi_read_reg(motor->spi, 0x04, ®_val); dev_info(dev, "hall bias reg val 0x%x, read from register\n", reg_val); } else { dev_err(dev, "not support dc-iris, do nothing\n"); } } return count; } static ssize_t set_hall_offset(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 = to_motor_dev(sd); int val = 0; int ret = 0; u16 reg_val = 0; ret = kstrtoint(buf, 0, &val); if (!ret) { if (motor->is_use_dc_iris) { spi_read_reg(motor->spi, 0x04, ®_val); reg_val &= 0x00ff; reg_val |= (val & 0xff) << 8; spi_write_reg(motor->spi, 0x04, reg_val); gpiod_set_value(motor->dciris->vd_iris_gpio, 1); usleep_range(200, 400); gpiod_set_value(motor->dciris->vd_iris_gpio, 0); dev_info(dev, "set hall_offset %d, reg val 0x%x\n", val, reg_val); spi_read_reg(motor->spi, 0x04, ®_val); dev_info(dev, "hall offset reg val 0x%x, read from register\n", reg_val); } else { dev_err(dev, "not support dc-iris, do nothing\n"); } } return count; } static ssize_t set_hall_gain(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 = to_motor_dev(sd); int val = 0; int ret = 0; u16 reg_val = 0; ret = kstrtoint(buf, 0, &val); if (!ret) { if (motor->is_use_dc_iris) { spi_read_reg(motor->spi, 0x05, ®_val); reg_val &= 0xf0ff; reg_val |= (val & 0xf) << 8; spi_write_reg(motor->spi, 0x05, reg_val); gpiod_set_value(motor->dciris->vd_iris_gpio, 1); usleep_range(200, 400); gpiod_set_value(motor->dciris->vd_iris_gpio, 0); dev_info(dev, "set hall_offset %d, reg val 0x%04x\n", val, reg_val); spi_read_reg(motor->spi, 0x05, ®_val); dev_info(dev, "hall gain reg val 0x%04x, read from register\n", reg_val); } else { dev_err(dev, "not support dc-iris, do nothing\n"); } } return count; } static ssize_t reinit_piris_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 = to_motor_dev(sd); int val = 0; int ret = 0; ret = kstrtoint(buf, 0, &val); if (!ret) { if (val == 1) motor_reinit_piris_pos(motor); } return count; } 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 = to_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 = to_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 reinit_zoom1_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 = to_motor_dev(sd); int val = 0; int ret = 0; ret = kstrtoint(buf, 0, &val); if (!ret) { if (val == 1) motor_reinit_zoom1_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 = to_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 = to_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 struct device_attribute attributes[] = { __ATTR(pid_dgain, S_IWUSR, NULL, set_pid_dgain), __ATTR(pid_zero, S_IWUSR, NULL, set_pid_zero), __ATTR(pid_pole, S_IWUSR, NULL, set_pid_pole), __ATTR(hall_bias, S_IWUSR, NULL, set_hall_bias), __ATTR(hall_offset, S_IWUSR, NULL, set_hall_offset), __ATTR(hall_gain, S_IWUSR, NULL, set_hall_gain), __ATTR(reinit_piris, S_IWUSR, NULL, reinit_piris_pos), __ATTR(reinit_focus, S_IWUSR, NULL, reinit_focus_pos), __ATTR(reinit_zoom, S_IWUSR, NULL, reinit_zoom_pos), __ATTR(reinit_zoom1, S_IWUSR, NULL, reinit_zoom1_pos), __ATTR(focus_reback_ctrl, S_IWUSR, NULL, set_focus_reback_ctrl), __ATTR(zoom_reback_ctrl, S_IWUSR, NULL, set_zoom_reback_ctrl), }; 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 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; #ifdef PI_TEST int min = 0; int max = 0; #endif unsigned long flags = V4L2_CTRL_FLAG_EXECUTE_ON_WRITE | V4L2_CTRL_FLAG_VOLATILE; handler = &motor->ctrl_handler; ret = v4l2_ctrl_handler_init(handler, 3); if (ret) return ret; if (motor->is_use_dc_iris) { motor->iris_ctrl = v4l2_ctrl_new_std(handler, &motor_ctrl_ops, V4L2_CID_IRIS_ABSOLUTE, 0, motor->dciris->max_log, 1, 0); if (motor->iris_ctrl) motor->iris_ctrl->flags |= flags; } else if (motor->is_use_p_iris) { #ifdef REINIT_BOOT ret = motor_reinit_piris(motor); if (ret < 0) return -EINVAL; #endif motor->piris->last_pos = motor->piris->min_pos; motor->iris_ctrl = v4l2_ctrl_new_std(handler, &motor_ctrl_ops, V4L2_CID_IRIS_ABSOLUTE, motor->piris->min_pos, motor->piris->max_pos, 1, motor->piris->min_pos); if (motor->iris_ctrl) motor->iris_ctrl->flags |= flags; } if (motor->is_use_focus) { #ifdef REINIT_BOOT ret = motor_reinit_focus(motor); if (ret < 0) return -EINVAL; #endif motor->focus->last_pos = motor->focus->min_pos; motor->focus_ctrl = v4l2_ctrl_new_std(handler, &motor_ctrl_ops, V4L2_CID_FOCUS_ABSOLUTE, motor->focus->min_pos, motor->focus->max_pos - motor->focus->reback, 1, motor->focus->min_pos); if (motor->focus_ctrl) motor->focus_ctrl->flags |= flags; } if (motor->is_use_zoom) { #ifdef REINIT_BOOT ret = motor_reinit_zoom(motor); if (ret < 0) return -EINVAL; #endif motor->zoom->last_pos = motor->zoom->min_pos; motor->zoom_ctrl = v4l2_ctrl_new_std(handler, &motor_ctrl_ops, V4L2_CID_ZOOM_ABSOLUTE, motor->zoom->min_pos, motor->zoom->max_pos - motor->zoom->reback, 1, motor->zoom->min_pos); if (motor->zoom_ctrl) motor->zoom_ctrl->flags |= flags; } if (motor->is_use_zoom1) { #ifdef REINIT_BOOT ret = motor_reinit_zoom1(motor); if (ret < 0) return -EINVAL; #endif motor->zoom1->last_pos = motor->zoom1->min_pos; motor->zoom1_ctrl = v4l2_ctrl_new_std(handler, &motor_ctrl_ops, V4L2_CID_ZOOM_CONTINUOUS, motor->zoom1->min_pos, motor->zoom1->max_pos, 1, motor->zoom1->min_pos); if (motor->zoom1_ctrl) motor->zoom1_ctrl->flags |= flags; } if (handler->error) { ret = handler->error; dev_err(&motor->spi->dev, "Failed to init controls(%d)\n", ret); goto err_free_handler; } motor->subdev.ctrl_handler = handler; return ret; err_free_handler: v4l2_ctrl_handler_free(handler); return ret; } static void dev_param_init(struct motor_dev *motor) { int step = 0; u32 mv_cnt = 0; u32 status = 0; u32 reback_vd_cnt = 0; if (motor->is_use_dc_iris) motor->dciris->last_log = 0; if (motor->is_use_p_iris) { motor->piris->is_mv_tim_update = false; motor->piris->is_need_update_tim = false; motor->piris->move_status = MOTOR_STATUS_STOPPED; motor->piris->type = TYPE_IRIS; motor->piris->mv_tim.vcm_start_t = ns_to_timeval(ktime_get_ns()); motor->piris->mv_tim.vcm_end_t = ns_to_timeval(ktime_get_ns()); init_completion(&motor->piris->complete); init_completion(&motor->piris->complete_out); motor->piris->run_data.psum = motor->vd_fz_period_us * motor->piris->start_up_speed * 8 / 1000000; motor->piris->run_data.intct = 27 * motor->vd_fz_period_us / (motor->piris->run_data.psum * 24); motor->piris->is_running = false; dev_info(&motor->spi->dev, "piris vd_fz_period_us %u, psum %d, inict %d\n", motor->vd_fz_period_us, motor->piris->run_data.psum, motor->piris->run_data.intct); } if (motor->is_use_focus) { motor->focus->is_mv_tim_update = false; motor->focus->is_need_update_tim = false; motor->focus->move_status = MOTOR_STATUS_STOPPED; motor->focus->type = TYPE_FOCUS; 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()); init_completion(&motor->focus->complete); init_completion(&motor->focus->complete_out); motor->focus->run_data.psum = motor->vd_fz_period_us * motor->focus->start_up_speed * 8 / 1000000; motor->focus->run_data.intct = 27 * motor->vd_fz_period_us / (motor->focus->run_data.psum * 24); motor->focus->is_running = false; motor->focus->reback_ctrl = false; dev_info(&motor->spi->dev, "focus vd_fz_period_us %u, psum %d, inict %d\n", motor->vd_fz_period_us, motor->focus->run_data.psum, motor->focus->run_data.intct); if (motor->focus->reback != 0) { motor->focus->cur_back_delay = 0; motor->focus->max_back_delay = FOCUS_MAX_BACK_DELAY; motor->focus->reback_data = motor->focus->run_data; mv_cnt = motor->focus->reback; if (motor->focus->is_dir_opp) { mv_cnt += motor->focus->backlash; status = MOTOR_STATUS_CW; } else { mv_cnt += motor->focus->backlash; status = MOTOR_STATUS_CCW; } motor->focus->reback_status = status; if (motor->focus->is_half_step_mode) step = mv_cnt * 4; else step = mv_cnt * 8; motor->focus->reback_data.count = (step + motor->focus->reback_data.psum - 1) / motor->focus->reback_data.psum + 1; motor->focus->reback_data.cur_count = motor->focus->reback_data.count; motor->focus->reback_data.psum_last = step % motor->focus->reback_data.psum; if (motor->focus->reback_data.psum_last == 0) motor->focus->reback_data.psum_last = motor->focus->reback_data.psum; reback_vd_cnt = motor->focus->reback_data.count + motor->focus->max_back_delay; motor->focus->reback_move_time_us = reback_vd_cnt * (motor->vd_fz_period_us + 500); } } if (motor->is_use_zoom) { motor->zoom->is_mv_tim_update = false; motor->zoom->is_need_update_tim = false; motor->zoom->move_status = MOTOR_STATUS_STOPPED; motor->zoom->type = TYPE_ZOOM; 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()); init_completion(&motor->zoom->complete); init_completion(&motor->zoom->complete_out); motor->zoom->run_data.psum = motor->vd_fz_period_us * motor->zoom->start_up_speed * 8 / 1000000; motor->zoom->run_data.intct = 27 * motor->vd_fz_period_us / (motor->zoom->run_data.psum * 24); motor->zoom->is_running = false; motor->zoom->reback_ctrl = false; if (motor->zoom->reback != 0) { motor->zoom->cur_back_delay = 0; motor->zoom->max_back_delay = ZOOM_MAX_BACK_DELAY; motor->zoom->reback_data = motor->zoom->run_data; mv_cnt = motor->zoom->reback; if (motor->zoom->is_dir_opp) { mv_cnt += motor->zoom->backlash; status = MOTOR_STATUS_CW; } else { mv_cnt += motor->zoom->backlash; status = MOTOR_STATUS_CCW; } motor->zoom->reback_status = status; if (motor->zoom->is_half_step_mode) step = mv_cnt * 4; else step = mv_cnt * 8; motor->zoom->reback_data.count = (step + motor->zoom->reback_data.psum - 1) / motor->zoom->reback_data.psum + 1; motor->zoom->reback_data.cur_count = motor->zoom->reback_data.count; motor->zoom->reback_data.psum_last = step % motor->zoom->reback_data.psum; if (motor->zoom->reback_data.psum_last == 0) motor->zoom->reback_data.psum_last = motor->zoom->reback_data.psum; reback_vd_cnt = motor->zoom->reback_data.count + motor->zoom->max_back_delay; motor->zoom->reback_move_time_us = reback_vd_cnt * (motor->vd_fz_period_us + 500); } dev_info(&motor->spi->dev, "zoom vd_fz_period_us %u, psum %d, inict %d\n", motor->vd_fz_period_us, motor->zoom->run_data.psum, motor->zoom->run_data.intct); } if (motor->is_use_zoom1) { motor->zoom1->is_mv_tim_update = false; motor->zoom1->is_need_update_tim = false; motor->zoom1->move_status = MOTOR_STATUS_STOPPED; motor->zoom1->type = TYPE_ZOOM1; motor->zoom1->mv_tim.vcm_start_t = ns_to_timeval(ktime_get_ns()); motor->zoom1->mv_tim.vcm_end_t = ns_to_timeval(ktime_get_ns()); init_completion(&motor->zoom1->complete); init_completion(&motor->zoom1->complete_out); motor->zoom1->run_data.psum = motor->vd_fz_period_us * motor->zoom1->start_up_speed * 8 / 1000000; motor->zoom1->run_data.intct = 27 * motor->vd_fz_period_us / (motor->zoom1->run_data.psum * 24); motor->zoom1->is_running = false; motor->zoom1->reback_ctrl = false; dev_info(&motor->spi->dev, "zoom1 vd_fz_period_us %u, psum %d, inict %d\n", motor->vd_fz_period_us, motor->zoom1->run_data.psum, motor->zoom1->run_data.intct); } motor->is_should_wait = false; motor->is_timer_restart = false; motor->is_timer_restart_bywq = false; motor->wait_cnt = 0; motor->pi_gpio_usecnt = 0; } static void dev_reg_init(struct motor_dev *motor) { spi_write_reg(motor->spi, 0x20, 0x1a01);//27M/(30*2^3*2^0) spi_write_reg(motor->spi, 0x21, 0x0085); spi_write_reg(motor->spi, 0x23, PPW_STOP); spi_write_reg(motor->spi, 0x28, PPW_STOP); if (motor->dev0) spi_write_reg(motor->spi, motor->dev0->reg_op->reg.dt2_phmod, (motor->dev0->run_data.phmode << 8) | 0x0001); if (motor->dev1) spi_write_reg(motor->spi, motor->dev1->reg_op->reg.dt2_phmod, (motor->dev1->run_data.phmode << 8) | 0x0001); spi_write_reg(motor->spi, 0x0b, 0x0480); if (motor->is_use_dc_iris) { //DC-IRIS reg init if (motor->dciris->is_reversed_polarity) spi_write_reg(motor->spi, 0x00, motor->dciris->max_log - motor->dciris->last_log); else spi_write_reg(motor->spi, 0x00, motor->dciris->last_log); spi_write_reg(motor->spi, 0x01, 0x6000); spi_write_reg(motor->spi, 0x02, 0x66f0); spi_write_reg(motor->spi, 0x03, 0x0e10); spi_write_reg(motor->spi, 0x04, 0xd640); spi_write_reg(motor->spi, 0x05, 0x0004); spi_write_reg(motor->spi, 0x0b, 0x0480); spi_write_reg(motor->spi, 0x0a, 0x0000); spi_write_reg(motor->spi, 0x0e, 0x0300); } if (!IS_ERR(motor->vd_fz_gpio)) gpiod_set_value(motor->vd_fz_gpio, 1); if (motor->is_use_dc_iris && (!IS_ERR(motor->dciris->vd_iris_gpio))) gpiod_set_value(motor->dciris->vd_iris_gpio, 1); usleep_range(100, 200); if (!IS_ERR(motor->vd_fz_gpio)) gpiod_set_value(motor->vd_fz_gpio, 0); if (motor->is_use_dc_iris && (!IS_ERR(motor->dciris->vd_iris_gpio))) gpiod_set_value(motor->dciris->vd_iris_gpio, 0); } static int motor_check_id(struct motor_dev *motor) { u16 val = 0xffff; int i = 0; for (i = 0; i < 0x20; i++) spi_read_reg(motor->spi, i, &val); spi_read_reg(motor->spi, 0x20, &val); if (val == 0xffff) { dev_err(&motor->spi->dev, "check id fail, spi transfer err or driver not connect, val 0x%x\n", val); return -EINVAL; } return 0; } static int dev_init(struct motor_dev *motor) { int ret = 0; if (!IS_ERR(motor->reset_gpio)) { gpiod_set_value_cansleep(motor->reset_gpio, 0); usleep_range(100, 200); gpiod_set_value_cansleep(motor->reset_gpio, 1); } ret = motor_check_id(motor); if (ret < 0) return -EINVAL; dev_param_init(motor); dev_reg_init(motor); motor->wk = devm_kzalloc(&motor->spi->dev, sizeof(*motor->wk), GFP_KERNEL); if (!motor->wk) { dev_err(&motor->spi->dev, "failed to alloc work struct\n"); return -ENOMEM; } motor->wk->dev = motor; INIT_WORK(&motor->wk->work, motor_op_work); return 0; } static int motor_dev_probe(struct spi_device *spi) { int ret = 0; struct device *dev = &spi->dev; struct motor_dev *motor; struct v4l2_subdev *sd; char facing[2]; dev_info(dev, "driver version: %02x.%02x.%02x", DRIVER_VERSION >> 16, (DRIVER_VERSION & 0xff00) >> 8, DRIVER_VERSION & 0x00ff); motor = devm_kzalloc(dev, sizeof(*motor), GFP_KERNEL); if (!motor) return -ENOMEM; spi->mode = SPI_MODE_3 | SPI_LSB_FIRST | SPI_CS_HIGH; spi->irq = -1; spi->max_speed_hz = 5000000; spi->bits_per_word = 8; ret = spi_setup(spi); if (ret < 0) { dev_err(dev, "could not setup spi!\n"); return -EINVAL; } motor->spi = spi; motor->motor_op[0] = g_motor_op[0]; motor->motor_op[1] = g_motor_op[1]; if (motor_dev_parse_dt(motor)) { dev_err(&motor->spi->dev, "parse dt error\n"); return -EINVAL; } ret = dev_init(motor); if (ret) goto err_free; mutex_init(&motor->mutex); hrtimer_init(&motor->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL); motor->timer.function = motor_timer_func; sd = &motor->subdev; v4l2_spi_subdev_init(sd, spi, &motor_subdev_ops); sd->entity.function = MEDIA_ENT_F_LENS; sd->entity.flags = 0; sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE; motor_initialize_controls(motor); ret = media_entity_pads_init(&motor->subdev.entity, 0, NULL); if (ret < 0) goto err_free; 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_%d", motor->module_index, facing, DRIVER_NAME, motor->id); ret = v4l2_async_register_subdev(sd); if (ret) dev_err(&spi->dev, "v4l2 async register subdev failed\n"); #ifdef USED_SYS_DEBUG add_sysfs_interfaces(dev); #endif dev_info(&motor->spi->dev, "gpio motor driver probe success\n"); return 0; err_free: v4l2_ctrl_handler_free(&motor->ctrl_handler); v4l2_device_unregister_subdev(&motor->subdev); media_entity_cleanup(&motor->subdev.entity); return ret; } static int motor_dev_remove(struct spi_device *spi) { struct v4l2_subdev *sd = spi_get_drvdata(spi); struct motor_dev *motor = to_motor_dev(sd); hrtimer_cancel(&motor->timer); if (sd) v4l2_device_unregister_subdev(sd); v4l2_ctrl_handler_free(&motor->ctrl_handler); media_entity_cleanup(&motor->subdev.entity); #ifdef USED_SYS_DEBUG remove_sysfs_interfaces(&spi->dev); #endif return 0; } static const struct spi_device_id motor_match_id[] = { {"relmon,ms41908", 0 }, { } }; MODULE_DEVICE_TABLE(spi, motor_match_id); #if defined(CONFIG_OF) static const struct of_device_id motor_dev_of_match[] = { {.compatible = "relmon,ms41908", }, {}, }; #endif static struct spi_driver motor_dev_driver = { .driver = { .name = DRIVER_NAME, .of_match_table = of_match_ptr(motor_dev_of_match), }, .probe = &motor_dev_probe, .remove = &motor_dev_remove, .id_table = motor_match_id, }; static int __init motor_mod_init(void) { return spi_register_driver(&motor_dev_driver); } static void __exit motor_mod_exit(void) { spi_unregister_driver(&motor_dev_driver); } device_initcall_sync(motor_mod_init); module_exit(motor_mod_exit); MODULE_LICENSE("GPL"); MODULE_ALIAS("platform:motor"); MODULE_AUTHOR("zefa.chen@rock-chips.com");