/*
|
* Copyright (c) 2019 Rockchip Corporation
|
*
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
* you may not use this file except in compliance with the License.
|
* You may obtain a copy of the License at
|
*
|
* http://www.apache.org/licenses/LICENSE-2.0
|
*
|
* Unless required by applicable law or agreed to in writing, software
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
* See the License for the specific language governing permissions and
|
* limitations under the License.
|
*
|
*/
|
|
#include <linux/v4l2-subdev.h>
|
#include "LensHw.h"
|
|
namespace RkCam {
|
|
uint16_t LensHw::DEFAULT_POOL_SIZE = 20;
|
|
LensHw::LensHw(const char* name)
|
: V4l2SubDevice (name)
|
{
|
ENTER_CAMHW_FUNCTION();
|
_rec_sof_idx = 0;
|
memset(_frame_time, 0, sizeof(_frame_time));
|
memset(_frame_sequence, 0, sizeof(_frame_sequence));
|
_afInfoPool = new RkAiqAfInfoPool("LensLocalAfInfoParams", LensHw::DEFAULT_POOL_SIZE);
|
_irisInfoPool = new RkAiqIrisParamsPool("LensLocalIrisInfoParams", LensHw::DEFAULT_POOL_SIZE);
|
_piris_step = -1;
|
EXIT_CAMHW_FUNCTION();
|
}
|
|
LensHw::~LensHw()
|
{
|
ENTER_CAMHW_FUNCTION();
|
EXIT_CAMHW_FUNCTION();
|
}
|
|
XCamReturn
|
LensHw::queryLensSupport()
|
{
|
ENTER_CAMHW_FUNCTION();
|
|
_iris_enable = true;
|
_focus_enable = true;
|
_zoom_enable = true;
|
|
memset(&_iris_query, 0, sizeof(_iris_query));
|
_iris_query.id = V4L2_CID_IRIS_ABSOLUTE;
|
if (io_control(VIDIOC_QUERYCTRL, &_iris_query) < 0) {
|
LOGI_CAMHW_SUBM(LENS_SUBM, "query iris ctrl failed");
|
_iris_enable = false;
|
} else {
|
_iris_enable = true;
|
}
|
|
memset(&_focus_query, 0, sizeof(_focus_query));
|
_focus_query.id = V4L2_CID_FOCUS_ABSOLUTE;
|
if (io_control(VIDIOC_QUERYCTRL, &_focus_query) < 0) {
|
LOGI_CAMHW_SUBM(LENS_SUBM, "query focus ctrl failed");
|
_focus_enable = false;
|
} else {
|
_focus_enable = true;
|
}
|
|
memset(&_zoom_query, 0, sizeof(_zoom_query));
|
_zoom_query.id = V4L2_CID_ZOOM_ABSOLUTE;
|
if (io_control(VIDIOC_QUERYCTRL, &_zoom_query) < 0) {
|
LOGI_CAMHW_SUBM(LENS_SUBM, "query zoom ctrl failed");
|
_zoom_enable = false;
|
} else {
|
_zoom_enable = true;
|
}
|
|
EXIT_CAMHW_FUNCTION();
|
return XCAM_RETURN_NO_ERROR;
|
}
|
|
XCamReturn
|
LensHw::start_internal()
|
{
|
ENTER_CAMHW_FUNCTION();
|
|
if (_active)
|
return XCAM_RETURN_NO_ERROR;
|
|
_rec_sof_idx = 0;
|
_piris_step = 0;
|
_last_piris_step = 0;
|
_dciris_pwmduty = 0;
|
_last_dciris_pwmduty = 0;
|
_focus_pos = -1;
|
_zoom_pos = -1;
|
memset(&_focus_tim, 0, sizeof(_focus_tim));
|
memset(&_zoom_tim, 0, sizeof(_zoom_tim));
|
memset(_frame_time, 0, sizeof(_frame_time));
|
memset(_frame_sequence, 0, sizeof(_frame_sequence));
|
queryLensSupport();
|
|
_active = true;
|
EXIT_CAMHW_FUNCTION();
|
return XCAM_RETURN_NO_ERROR;
|
}
|
|
XCamReturn
|
LensHw::start()
|
{
|
ENTER_CAMHW_FUNCTION();
|
SmartLock locker (_mutex);
|
|
start_internal();
|
|
EXIT_CAMHW_FUNCTION();
|
return XCAM_RETURN_NO_ERROR;
|
}
|
|
XCamReturn
|
LensHw::stop()
|
{
|
ENTER_CAMHW_FUNCTION();
|
SmartLock locker (_mutex);
|
|
_active = false;
|
EXIT_CAMHW_FUNCTION();
|
return XCAM_RETURN_NO_ERROR;
|
}
|
|
XCamReturn
|
LensHw::getLensModeData(rk_aiq_lens_descriptor& lens_des)
|
{
|
ENTER_CAMHW_FUNCTION();
|
SmartLock locker (_mutex);
|
|
queryLensSupport();
|
lens_des.focus_support = _focus_enable;
|
lens_des.iris_support = _iris_enable;
|
lens_des.zoom_support = _zoom_enable;
|
EXIT_CAMHW_FUNCTION();
|
|
return XCAM_RETURN_NO_ERROR;
|
}
|
|
XCamReturn
|
LensHw::getLensVcmCfg(rk_aiq_lens_vcmcfg& lens_cfg)
|
{
|
ENTER_CAMHW_FUNCTION();
|
struct rk_cam_vcm_cfg cfg;
|
|
if (io_control (RK_VIDIOC_GET_VCM_CFG, &cfg) < 0) {
|
LOGE_CAMHW_SUBM(LENS_SUBM, "get vcm cfg failed");
|
return XCAM_RETURN_ERROR_IOCTL;
|
}
|
lens_cfg.start_ma = cfg.start_ma;
|
lens_cfg.rated_ma = cfg.rated_ma;
|
lens_cfg.step_mode = cfg.step_mode;
|
|
EXIT_CAMHW_FUNCTION();
|
return XCAM_RETURN_NO_ERROR;
|
}
|
|
XCamReturn
|
LensHw::setLensVcmCfg(rk_aiq_lens_vcmcfg& lens_cfg)
|
{
|
ENTER_CAMHW_FUNCTION();
|
struct rk_cam_vcm_cfg cfg;
|
|
cfg.start_ma = lens_cfg.start_ma;
|
cfg.rated_ma = lens_cfg.rated_ma;
|
cfg.step_mode = lens_cfg.step_mode;
|
if (io_control (RK_VIDIOC_SET_VCM_CFG, &cfg) < 0) {
|
LOGE_CAMHW_SUBM(LENS_SUBM, "set vcm cfg failed");
|
return XCAM_RETURN_ERROR_IOCTL;
|
}
|
|
EXIT_CAMHW_FUNCTION();
|
return XCAM_RETURN_NO_ERROR;
|
}
|
|
XCamReturn
|
LensHw::setFocusParams(int position)
|
{
|
ENTER_CAMHW_FUNCTION();
|
SmartLock locker (_mutex);
|
struct v4l2_control control;
|
|
if (!_focus_enable) {
|
LOGE_CAMHW_SUBM(LENS_SUBM, "focus is not supported");
|
return XCAM_RETURN_NO_ERROR;
|
}
|
|
if (!_active)
|
start_internal();
|
|
if (position < _focus_query.minimum)
|
position = _focus_query.minimum;
|
if (position > _focus_query.maximum)
|
position = _focus_query.maximum;
|
|
xcam_mem_clear (control);
|
control.id = V4L2_CID_FOCUS_ABSOLUTE;
|
control.value = position;
|
|
LOGD_CAMHW_SUBM(LENS_SUBM, "|||set focus result: %d, control.value %d", position, control.value);
|
if (io_control (VIDIOC_S_CTRL, &control) < 0) {
|
LOGE_CAMHW_SUBM(LENS_SUBM, "set focus result failed to device");
|
return XCAM_RETURN_ERROR_IOCTL;
|
}
|
_focus_pos = position;
|
|
struct rk_cam_vcm_tim tim;
|
if (io_control (RK_VIDIOC_VCM_TIMEINFO, &tim) < 0) {
|
LOGE_CAMHW_SUBM(LENS_SUBM, "get focus timeinfo failed");
|
return XCAM_RETURN_ERROR_IOCTL;
|
}
|
_focus_tim = tim;
|
|
EXIT_CAMHW_FUNCTION();
|
return XCAM_RETURN_NO_ERROR;
|
}
|
|
XCamReturn
|
LensHw::setPIrisParams(int step)
|
{
|
ENTER_CAMHW_FUNCTION();
|
SmartLock locker (_mutex);
|
struct v4l2_control control;
|
|
if (!_iris_enable) {
|
LOGE_CAMHW_SUBM(LENS_SUBM, "iris is not supported");
|
return XCAM_RETURN_NO_ERROR;
|
}
|
|
if (!_active)
|
start_internal();
|
|
if (_piris_step == step)
|
return XCAM_RETURN_NO_ERROR;
|
|
//get old Piris-step
|
_last_piris_step = _piris_step;
|
|
xcam_mem_clear (control);
|
control.id = V4L2_CID_IRIS_ABSOLUTE;
|
control.value = step;
|
|
LOGD_CAMHW_SUBM(LENS_SUBM, "|||set iris result: %d, control.value %d", step, control.value);
|
if (io_control (VIDIOC_S_CTRL, &control) < 0) {
|
LOGE_CAMHW_SUBM(LENS_SUBM, "set iris result failed to device");
|
return XCAM_RETURN_ERROR_IOCTL;
|
}
|
_piris_step = step;
|
|
struct rk_cam_motor_tim tim;
|
if (io_control (RK_VIDIOC_IRIS_TIMEINFO, &tim) < 0) {
|
LOGE_CAMHW_SUBM(LENS_SUBM, "get iris timeinfo failed");
|
return XCAM_RETURN_ERROR_IOCTL;
|
}
|
_piris_tim = tim;
|
|
EXIT_CAMHW_FUNCTION();
|
return XCAM_RETURN_NO_ERROR;
|
}
|
|
XCamReturn
|
LensHw::setDCIrisParams(int pwmDuty)
|
{
|
ENTER_CAMHW_FUNCTION();
|
SmartLock locker (_mutex);
|
struct v4l2_control control;
|
|
if (!_iris_enable) {
|
LOGE_CAMHW_SUBM(LENS_SUBM, "iris is not supported");
|
return XCAM_RETURN_NO_ERROR;
|
}
|
|
if (!_active)
|
start_internal();
|
|
if (_dciris_pwmduty == pwmDuty)
|
return XCAM_RETURN_NO_ERROR;
|
|
//get old Piris-step
|
_last_dciris_pwmduty = pwmDuty;
|
|
xcam_mem_clear (control);
|
control.id = V4L2_CID_IRIS_ABSOLUTE;
|
control.value = pwmDuty;
|
|
LOGD_CAMHW_SUBM(LENS_SUBM, "|||set dc-iris result: %d, control.value %d", pwmDuty, control.value);
|
if (io_control (VIDIOC_S_CTRL, &control) < 0) {
|
LOGE_CAMHW_SUBM(LENS_SUBM, "set dc-iris result failed to device");
|
return XCAM_RETURN_ERROR_IOCTL;
|
}
|
_dciris_pwmduty = pwmDuty;
|
|
EXIT_CAMHW_FUNCTION();
|
return XCAM_RETURN_NO_ERROR;
|
}
|
|
XCamReturn
|
LensHw::getPIrisParams(int* step)
|
{
|
ENTER_CAMHW_FUNCTION();
|
SmartLock locker (_mutex);
|
struct v4l2_control control;
|
|
if (!_iris_enable) {
|
LOGE_CAMHW_SUBM(LENS_SUBM, "iris is not supported");
|
return XCAM_RETURN_ERROR_FAILED;
|
}
|
|
xcam_mem_clear (control);
|
control.id = V4L2_CID_IRIS_ABSOLUTE;
|
|
if (io_control (VIDIOC_G_CTRL, &control) < 0) {
|
LOGE_CAMHW_SUBM(LENS_SUBM, "get iris result failed");
|
return XCAM_RETURN_ERROR_IOCTL;
|
}
|
*step = control.value;
|
LOGD_CAMHW_SUBM(LENS_SUBM, "|||get iris result: %d, control.value %d", *step, control.value);
|
|
EXIT_CAMHW_FUNCTION();
|
return XCAM_RETURN_NO_ERROR;
|
}
|
|
XCamReturn
|
LensHw::setZoomParams(int position)
|
{
|
ENTER_CAMHW_FUNCTION();
|
SmartLock locker (_mutex);
|
struct v4l2_control control;
|
|
if (!_zoom_enable) {
|
LOGE_CAMHW_SUBM(LENS_SUBM, "zoom is not supported");
|
return XCAM_RETURN_NO_ERROR;
|
}
|
|
if (!_active)
|
start_internal();
|
|
if (position < _zoom_query.minimum)
|
position = _zoom_query.minimum;
|
if (position > _zoom_query.maximum)
|
position = _zoom_query.maximum;
|
xcam_mem_clear (control);
|
control.id = V4L2_CID_ZOOM_ABSOLUTE;
|
control.value = position;
|
|
LOGD_CAMHW_SUBM(LENS_SUBM, "||| set zoom result: %d, control.value %d", position, control.value);
|
if (io_control (VIDIOC_S_CTRL, &control) < 0) {
|
LOGE_CAMHW_SUBM(LENS_SUBM, "set zoom result failed to device");
|
return XCAM_RETURN_ERROR_IOCTL;
|
}
|
_zoom_pos = position;
|
|
struct rk_cam_vcm_tim tim;
|
if (io_control (RK_VIDIOC_ZOOM_TIMEINFO, &tim) < 0) {
|
LOGE_CAMHW_SUBM(LENS_SUBM, "get zoom timeinfo failed");
|
return XCAM_RETURN_ERROR_IOCTL;
|
}
|
_zoom_tim = tim;
|
|
EXIT_CAMHW_FUNCTION();
|
return XCAM_RETURN_NO_ERROR;
|
}
|
|
XCamReturn
|
LensHw::getFocusParams(int* position)
|
{
|
ENTER_CAMHW_FUNCTION();
|
SmartLock locker (_mutex);
|
struct v4l2_control control;
|
|
if (!_focus_enable) {
|
LOGE_CAMHW_SUBM(LENS_SUBM, "focus is not supported");
|
return XCAM_RETURN_ERROR_FAILED;
|
}
|
|
xcam_mem_clear (control);
|
control.id = V4L2_CID_FOCUS_ABSOLUTE;
|
|
if (io_control (VIDIOC_G_CTRL, &control) < 0) {
|
LOGE_CAMHW_SUBM(LENS_SUBM, "get focus result failed");
|
return XCAM_RETURN_ERROR_IOCTL;
|
}
|
*position = control.value;
|
LOGD_CAMHW_SUBM(LENS_SUBM, "|||get focus result: %d, control.value %d", *position, control.value);
|
|
_focus_pos = *position;
|
|
EXIT_CAMHW_FUNCTION();
|
return XCAM_RETURN_NO_ERROR;
|
}
|
|
XCamReturn
|
LensHw::getZoomParams(int* position)
|
{
|
ENTER_CAMHW_FUNCTION();
|
SmartLock locker (_mutex);
|
struct v4l2_control control;
|
|
if (!_zoom_enable) {
|
LOGE_CAMHW_SUBM(LENS_SUBM, "zoom is not supported");
|
return XCAM_RETURN_ERROR_FAILED;
|
}
|
|
xcam_mem_clear (control);
|
control.id = V4L2_CID_ZOOM_ABSOLUTE;
|
|
if (io_control (VIDIOC_G_CTRL, &control) < 0) {
|
LOGE_CAMHW_SUBM(LENS_SUBM, "get zoom result failed");
|
return XCAM_RETURN_ERROR_IOCTL;
|
}
|
*position = control.value;
|
LOGD_CAMHW_SUBM(LENS_SUBM, "|||get zoom result: %d, control.value %d", *position, control.value);
|
|
_zoom_pos = *position;
|
|
EXIT_CAMHW_FUNCTION();
|
return XCAM_RETURN_NO_ERROR;
|
}
|
|
XCamReturn
|
LensHw::FocusCorrection()
|
{
|
ENTER_CAMHW_FUNCTION();
|
SmartLock locker (_mutex);
|
struct v4l2_control control;
|
int correction = 0;
|
|
if (!_focus_enable) {
|
LOGE_CAMHW_SUBM(LENS_SUBM, "focus is not supported");
|
return XCAM_RETURN_ERROR_FAILED;
|
}
|
|
if (io_control (RK_VIDIOC_FOCUS_CORRECTION, &correction) < 0) {
|
LOGE_CAMHW_SUBM(LENS_SUBM, "focus correction failed");
|
return XCAM_RETURN_ERROR_IOCTL;
|
}
|
|
EXIT_CAMHW_FUNCTION();
|
return XCAM_RETURN_NO_ERROR;
|
}
|
|
XCamReturn
|
LensHw::ZoomCorrection()
|
{
|
ENTER_CAMHW_FUNCTION();
|
SmartLock locker (_mutex);
|
struct v4l2_control control;
|
int correction = 0;
|
|
if (!_zoom_enable) {
|
LOGE_CAMHW_SUBM(LENS_SUBM, "zoom is not supported");
|
return XCAM_RETURN_ERROR_FAILED;
|
}
|
|
if (io_control (RK_VIDIOC_ZOOM_CORRECTION, &correction) < 0) {
|
LOGE_CAMHW_SUBM(LENS_SUBM, "zoom correction failed");
|
return XCAM_RETURN_ERROR_IOCTL;
|
}
|
|
EXIT_CAMHW_FUNCTION();
|
return XCAM_RETURN_NO_ERROR;
|
}
|
|
XCamReturn
|
LensHw::handle_sof(int64_t time, int frameid)
|
{
|
ENTER_CAMHW_FUNCTION();
|
//SmartLock locker (_mutex);
|
|
XCamReturn ret = XCAM_RETURN_NO_ERROR;
|
int idx;
|
|
idx = (_rec_sof_idx + 1) % LENSHW_RECORD_SOF_NUM;
|
_frame_sequence[idx] = frameid;
|
_frame_time[idx] = time;
|
_rec_sof_idx = idx;
|
|
LOGD_CAMHW_SUBM(LENS_SUBM, "%s: frm_id %d, time %lld\n", __func__, frameid, time);
|
|
EXIT_CAMHW_FUNCTION();
|
return ret;
|
}
|
|
XCamReturn
|
LensHw::getIrisInfoParams(SmartPtr<RkAiqIrisParamsProxy>& irisParams, int frame_id)
|
{
|
ENTER_CAMHW_FUNCTION();
|
//SmartLock locker (_mutex);
|
|
int i;
|
|
irisParams = NULL;
|
if (_irisInfoPool->has_free_items()) {
|
irisParams = (SmartPtr<RkAiqIrisParamsProxy>)_irisInfoPool->get_item();
|
} else {
|
LOGE_CAMHW_SUBM(LENS_SUBM, "%s: no free params buffer!\n", __FUNCTION__);
|
return XCAM_RETURN_ERROR_MEM;
|
}
|
|
for (i = 0; i < LENSHW_RECORD_SOF_NUM; i++) {
|
if (frame_id == _frame_sequence[i])
|
break;
|
}
|
|
//Piris
|
irisParams->data()->PIris.StartTim = _piris_tim.motor_start_t;
|
irisParams->data()->PIris.EndTim = _piris_tim.motor_end_t;
|
irisParams->data()->PIris.laststep = _last_piris_step;
|
irisParams->data()->PIris.step = _piris_step;
|
|
//DCiris
|
|
if (i < LENSHW_RECORD_SOF_NUM) {
|
irisParams->data()->sofTime = _frame_time[i];
|
} else {
|
LOGE_CAMHW_SUBM(LENS_SUBM, "%s: frame_id %d, can not find sof time!\n", __FUNCTION__, frame_id);
|
return XCAM_RETURN_ERROR_PARAM;
|
}
|
|
LOGD_CAMHW_SUBM(LENS_SUBM, "%s: frm_id %d, time %lld\n", __func__, frame_id, irisParams->data()->sofTime);
|
|
EXIT_CAMHW_FUNCTION();
|
|
return XCAM_RETURN_NO_ERROR;
|
}
|
|
XCamReturn
|
LensHw::getAfInfoParams(SmartPtr<RkAiqAfInfoProxy>& afInfo, int frame_id)
|
{
|
ENTER_CAMHW_FUNCTION();
|
SmartLock locker (_mutex);
|
|
int i;
|
|
afInfo = NULL;
|
if (_afInfoPool->has_free_items()) {
|
afInfo = (SmartPtr<RkAiqAfInfoProxy>)_afInfoPool->get_item();
|
} else {
|
LOGE_CAMHW_SUBM(LENS_SUBM, "%s: no free params buffer!\n", __FUNCTION__);
|
return XCAM_RETURN_ERROR_MEM;
|
}
|
|
for (i = 0; i < LENSHW_RECORD_SOF_NUM; i++) {
|
if (frame_id == _frame_sequence[i])
|
break;
|
}
|
|
afInfo->data()->focusStartTim = _focus_tim.vcm_start_t;
|
afInfo->data()->focusEndTim = _focus_tim.vcm_end_t;
|
afInfo->data()->zoomStartTim = _zoom_tim.vcm_start_t;
|
afInfo->data()->zoomEndTim = _zoom_tim.vcm_end_t;
|
afInfo->data()->focusCode = _focus_pos;
|
afInfo->data()->zoomCode = _zoom_pos;
|
if (i < LENSHW_RECORD_SOF_NUM) {
|
afInfo->data()->sofTime = _frame_time[i];
|
} else {
|
LOGE_CAMHW_SUBM(LENS_SUBM, "%s: frame_id %d, can not find sof time!\n", __FUNCTION__, frame_id);
|
return XCAM_RETURN_ERROR_PARAM;
|
}
|
|
LOGD_CAMHW_SUBM(LENS_SUBM, "%s: frm_id %d, time %lld\n", __func__, frame_id, afInfo->data()->sofTime);
|
|
EXIT_CAMHW_FUNCTION();
|
|
return XCAM_RETURN_NO_ERROR;
|
}
|
|
}; //namespace RkCam
|
