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