From dbac31a536f63b45c7f87c4b253a4aa8dab1f299 Mon Sep 17 00:00:00 2001 From: Wang Panzhenzhuan Date: Wed, 23 Dec 2020 16:46:25 +0800 Subject: [PATCH] media: i2c: nvp6158: add v4l2 interface Signed-off-by: Wang Panzhenzhuan Change-Id: I7932353657f5a75f2787647c7b4f31c6c9dbde44 --- drivers/media/i2c/nvp6158_drv/Makefile | 3 +- drivers/media/i2c/nvp6158_drv/nvp6158_v4l2.c | 1251 ++++++++++++++++++ 2 files changed, 1253 insertions(+), 1 deletion(-) create mode 100644 drivers/media/i2c/nvp6158_drv/nvp6158_v4l2.c diff --git a/drivers/media/i2c/nvp6158_drv/Makefile b/drivers/media/i2c/nvp6158_drv/Makefile index d0ef4e490436..e4cc0be8860a 100644 --- a/drivers/media/i2c/nvp6158_drv/Makefile +++ b/drivers/media/i2c/nvp6158_drv/Makefile @@ -4,5 +4,6 @@ nvp6158-objs += nvp6158_drv.o nvp6158_i2c.o \ nvp6158_video.o nvp6158_coax_protocol.o \ nvp6158_motion.o nvp6158_video_eq.o \ nvp6158_audio.o nvp6158_video_auto_detect.o \ - nvp6158_dev.o + nvp6158_v4l2.o + obj-$(CONFIG_VIDEO_NVP6158) += nvp6158.o diff --git a/drivers/media/i2c/nvp6158_drv/nvp6158_v4l2.c b/drivers/media/i2c/nvp6158_drv/nvp6158_v4l2.c new file mode 100644 index 000000000000..70558b4f9332 --- /dev/null +++ b/drivers/media/i2c/nvp6158_drv/nvp6158_v4l2.c @@ -0,0 +1,1251 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * nvp6158_v4l2 interface driver + * + * Copyright (c) 2021 Rockchip Electronics Co. Ltd. + * + * V0.0X01.0X00 first version. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "nvp6158_common.h" +#include "nvp6158_video.h" +#include "nvp6158_coax_protocol.h" +#include "nvp6158_motion.h" +#include "nvp6158_video_eq.h" +#include "nvp6158_drv.h" +#include "nvp6158_audio.h" +#include "nvp6158_video_auto_detect.h" +#include "nvp6158_drv.h" + +#define DRIVER_VERSION KERNEL_VERSION(0, 0x01, 0x0) + +#ifndef V4L2_CID_DIGITAL_GAIN +#define V4L2_CID_DIGITAL_GAIN V4L2_CID_GAIN +#endif + +#define NVP6158_XVCLK_FREQ 24000000 +#define NVP6158_BITS_PER_SAMPLE 8 + +/* pixel rate = link frequency * 2 * lanes / BITS_PER_SAMPLE */ +#define NVP6158_PIXEL_RATE 297000000LL + +#define OF_CAMERA_PINCTRL_STATE_DEFAULT "rockchip,camera_default" +#define OF_CAMERA_PINCTRL_STATE_SLEEP "rockchip,camera_sleep" + +#define OF_CAMERA_MODULE_REGULATORS "rockchip,regulator-names" +#define OF_CAMERA_MODULE_REGULATOR_VOLTAGES "rockchip,regulator-voltages" + +/* DVP MODE, BT1120 or BT656 */ +#define RK_CAMERA_MODULE_DVP_MODE "rockchip,dvp_mode" +#define RK_CAMERA_MODULE_CHANNEL_NUMS "rockchip,channel_nums" +#define RK_CAMERA_MODULE_DUAL_EDGE "rockchip,dual_edge" +#define RK_CAMERA_MODULE_DEFAULT_RECT "rockchip,default_rect" + +#define NVP6158_DEFAULT_DVP_MODE "BT1120" +#define NVP6158_DEFAULT_CHANNEL_NUMS 4U +#define NVP6158_DEFAULT_DUAL_EDGE 0U +#define NVP6158_NAME "nvp6158" +#define NVP6158_DEFAULT_WIDTH 1920 +#define NVP6158_DEFAULT_HEIGHT 1080 + +enum nvp6158_max_pad { + PAD0, + PAD1, + PAD2, + PAD3, + PAD_MAX, +}; + +struct nvp6158_gpio { + int pltfrm_gpio; + const char *label; + enum of_gpio_flags active_low; +}; + +struct nvp6158_regulator { + struct regulator *regulator; + u32 min_uV; + u32 max_uV; +}; + +struct nvp6158_regulators { + u32 cnt; + struct nvp6158_regulator *regulator; +}; + +struct nvp6158_pixfmt { + u32 code; +}; + +struct nvp6158_framesize { + u16 width; + u16 height; + NC_VIVO_CH_FORMATDEF fmt_idx; + struct v4l2_fract max_fps; +}; + +struct nvp6158_default_rect { + unsigned int width; + unsigned int height; +}; + +struct nvp6158 { + struct i2c_client *client; + struct clk *xvclk; + struct gpio_desc *pwr_gpio; + struct gpio_desc *pwr2_gpio; + struct gpio_desc *rst_gpio; + struct gpio_desc *rst2_gpio; + struct gpio_desc *pwdn_gpio; + struct gpio_desc *pwdn2_gpio; + + struct pinctrl *pinctrl; + struct pinctrl_state *pins_default; + struct pinctrl_state *pins_sleep; + + struct v4l2_subdev subdev; + struct media_pad pad[PAD_MAX]; + struct v4l2_ctrl_handler ctrl_handler; + struct mutex mutex; + bool power_on; + struct nvp6158_regulators regulators; + + u32 module_index; + const char *module_facing; + const char *module_name; + const char *len_name; + const char *dvp_mode; + NVP6158_DVP_MODE mode; + u32 ch_nums; + u32 dual_edge; + + struct v4l2_mbus_framefmt format; + const struct nvp6158_framesize *frame_size; + int streaming; + struct nvp6158_default_rect defrect; +}; + +#define to_nvp6158(sd) container_of(sd, struct nvp6158, subdev) + +static const struct nvp6158_framesize nvp6158_framesizes[] = { + { + .width = 1280, + .height = 720, + .fmt_idx = AHD20_720P_30P, + .max_fps = { + .numerator = 10000, + .denominator = 250000, + }, + }, + { + .width = 1920, + .height = 1080, + .fmt_idx = AHD20_1080P_25P, + .max_fps = { + .numerator = 10000, + .denominator = 250000, + }, + }, + /* test modes, Interlace mode*/ + { + .width = 720, + .height = 480, + .fmt_idx = AHD20_SD_SH720_NT, + .max_fps = { + .numerator = 10000, + .denominator = 250000, + }, + }, + { + .width = 720, + .height = 576, + .fmt_idx = AHD20_SD_SH720_PAL, + .max_fps = { + .numerator = 10000, + .denominator = 250000, + }, + }, + { + .width = 960, + .height = 576, + .fmt_idx = AHD20_SD_H960_PAL, + .max_fps = { + .numerator = 10000, + .denominator = 250000, + }, + }, + { + .width = 1920, + .height = 576, + .fmt_idx = AHD20_SD_H960_EX_PAL, + .max_fps = { + .numerator = 10000, + .denominator = 250000, + }, + } +}; + +static char *nvp6158_dvp_mode_lists[] = { + [BT601] = "BT601", + [BT656_1MUX] = "BT656_1MUX", + [BT656_2MUX] = "BT656_2MUX", + [BT656_4MUX] = "BT656_4MUX", + [BT1120_1MUX] = "BT1120_1MUX", + [BT1120_2MUX] = "BT1120_2MUX", + [BT1120_4MUX] = "BT1120_4MUX", + [BT656I_TEST_MODES] = "BT656I_TEST_MODES" +}; + +static const struct nvp6158_pixfmt nvp6158_formats[] = { + { + .code = MEDIA_BUS_FMT_UYVY8_2X8 + }, +}; + +static int nvp6158_querystd(struct v4l2_subdev *sd, v4l2_std_id *std) +{ + struct nvp6158 *nvp6158 = to_nvp6158(sd); + + if ((nvp6158->mode > BT656I_TEST_MODES) && + (nvp6158->mode < NVP6158_DVP_MODES_END)) { + /* for vicap detect bt1120 */ + *std = V4L2_STD_ATSC; + } else { + *std = V4L2_STD_PAL; + } + return 0; +} + +static int __nvp6158_power_on(struct nvp6158 *nvp6158) +{ + u32 i; + int ret; + struct nvp6158_regulator *regulator; + struct device *dev = &nvp6158->client->dev; + + dev_info(dev, "%s(%d)\n", __func__, __LINE__); + + if (!IS_ERR_OR_NULL(nvp6158->pins_default)) { + ret = pinctrl_select_state(nvp6158->pinctrl, + nvp6158->pins_default); + if (ret < 0) + dev_err(dev, "could not set pins. ret=%d\n", ret); + } + + ret = clk_prepare_enable(nvp6158->xvclk); + if (ret < 0) { + dev_err(dev, "Failed to enable xvclk\n"); + return ret; + } + + if (nvp6158->regulators.regulator) { + for (i = 0; i < nvp6158->regulators.cnt; i++) { + regulator = nvp6158->regulators.regulator + i; + if (IS_ERR(regulator->regulator)) + continue; + regulator_set_voltage( + regulator->regulator, + regulator->min_uV, + regulator->max_uV); + if (regulator_enable(regulator->regulator)) { + dev_err(dev, + "regulator_enable failed!\n"); + goto disable_clk; + } + } + } + usleep_range(3000, 5000); + + if (!IS_ERR(nvp6158->pwr_gpio)) { + gpiod_direction_output(nvp6158->pwr_gpio, 1); + usleep_range(3000, 5000); + } + + if (!IS_ERR(nvp6158->pwr2_gpio)) { + gpiod_direction_output(nvp6158->pwr2_gpio, 1); + usleep_range(3000, 5000); + } + + if (!IS_ERR(nvp6158->pwdn_gpio)) { + gpiod_direction_output(nvp6158->pwdn_gpio, 1); + usleep_range(1500, 2000); + } + + if (!IS_ERR(nvp6158->pwdn2_gpio)) { + gpiod_direction_output(nvp6158->pwdn2_gpio, 1); + usleep_range(1500, 2000); + } + + if (!IS_ERR(nvp6158->rst_gpio)) { + gpiod_direction_output(nvp6158->rst_gpio, 0); + usleep_range(50000, 100000); + gpiod_direction_output(nvp6158->rst_gpio, 1); + usleep_range(3000, 5000); + } + + if (!IS_ERR(nvp6158->rst2_gpio)) { + gpiod_direction_output(nvp6158->rst2_gpio, 0); + usleep_range(1500, 2000); + gpiod_direction_output(nvp6158->rst2_gpio, 1); + usleep_range(3000, 5000); + } + + return 0; + +disable_clk: + clk_disable_unprepare(nvp6158->xvclk); + + return ret; +} + +static void __nvp6158_power_off(struct nvp6158 *nvp6158) +{ + u32 i; + int ret; + struct nvp6158_regulator *regulator; + struct device *dev = &nvp6158->client->dev; + + dev_info(dev, "%s(%d)\n", __func__, __LINE__); + clk_disable_unprepare(nvp6158->xvclk); + + if (!IS_ERR(nvp6158->rst_gpio)) + gpiod_direction_output(nvp6158->rst_gpio, 0); + + if (!IS_ERR(nvp6158->rst2_gpio)) + gpiod_direction_output(nvp6158->rst2_gpio, 0); + + if (!IS_ERR(nvp6158->pwdn_gpio)) + gpiod_direction_output(nvp6158->pwdn_gpio, 0); + + if (!IS_ERR(nvp6158->pwdn_gpio)) + gpiod_direction_output(nvp6158->pwdn2_gpio, 0); + + if (!IS_ERR(nvp6158->pwr_gpio)) + gpiod_direction_output(nvp6158->pwr_gpio, 0); + + if (!IS_ERR(nvp6158->pwr2_gpio)) + gpiod_direction_output(nvp6158->pwr2_gpio, 0); + + if (!IS_ERR_OR_NULL(nvp6158->pins_sleep)) { + ret = pinctrl_select_state(nvp6158->pinctrl, + nvp6158->pins_sleep); + if (ret < 0) + dev_err(dev, "could not set pins\n"); + } + + if (nvp6158->regulators.regulator) { + for (i = 0; i < nvp6158->regulators.cnt; i++) { + regulator = nvp6158->regulators.regulator + i; + if (IS_ERR(regulator->regulator)) + continue; + regulator_disable(regulator->regulator); + } + } +} + +static int nvp6158_power(struct v4l2_subdev *sd, int on) +{ + struct i2c_client *client = v4l2_get_subdevdata(sd); + struct nvp6158 *nvp6158 = to_nvp6158(sd); + int ret = 0; + + dev_dbg(&client->dev, "%s: on %d\n", __func__, on); + mutex_lock(&nvp6158->mutex); + + /* If the power state is not modified - no work to do. */ + if (nvp6158->power_on == !!on) + goto exit; + + if (on) { + ret = __nvp6158_power_on(nvp6158); + if (ret < 0) + goto exit; + + nvp6158->power_on = true; + } else { + __nvp6158_power_off(nvp6158); + nvp6158->power_on = false; + } + +exit: + mutex_unlock(&nvp6158->mutex); + + return ret; +} + +#define CROP_START(SRC, DST) (((SRC) - (DST)) / 2 / 4 * 4) +/* + * The resolution of the driver configuration needs to be exactly + * the same as the current output resolution of the sensor, + * the input width of the isp needs to be 16 aligned, + * the input height of the isp needs to be 8 aligned. + * Can be cropped to standard resolution by this function, + * otherwise it will crop out strange resolution according + * to the alignment rules. + */ +static int nvp6158_get_selection(struct v4l2_subdev *sd, + struct v4l2_subdev_pad_config *cfg, + struct v4l2_subdev_selection *sel) +{ + struct nvp6158 *nvp6158 = to_nvp6158(sd); + + if (sel->target == V4L2_SEL_TGT_CROP_BOUNDS) { + sel->r.left = CROP_START(0, 0); + sel->r.width = nvp6158->frame_size->width; + sel->r.top = CROP_START(0, 0); + sel->r.height = nvp6158->frame_size->height; + return 0; + } + return -EINVAL; +} + +static int nvp6158_initialize_controls(struct nvp6158 *nvp6158) +{ + struct v4l2_ctrl_handler *handler; + int ret; + + handler = &nvp6158->ctrl_handler; + ret = v4l2_ctrl_handler_init(handler, 2); + if (ret) + return ret; + handler->lock = &nvp6158->mutex; + + v4l2_ctrl_new_std(handler, NULL, V4L2_CID_PIXEL_RATE, + 0, NVP6158_PIXEL_RATE, 1, NVP6158_PIXEL_RATE); + + if (handler->error) { + ret = handler->error; + dev_err(&nvp6158->client->dev, + "Failed to init controls(%d)\n", ret); + goto err_free_handler; + } + + nvp6158->subdev.ctrl_handler = handler; + + return 0; + +err_free_handler: + v4l2_ctrl_handler_free(handler); + + return ret; +} +static void nvp6158_get_default_format(struct nvp6158 *nvp6158) +{ + + const struct nvp6158_framesize *fsize = &nvp6158_framesizes[0]; + const struct nvp6158_framesize *match = NULL; + int i = ARRAY_SIZE(nvp6158_framesizes); + unsigned int min_err = UINT_MAX; + struct v4l2_mbus_framefmt *format = &nvp6158->format; + struct nvp6158_default_rect *rect = &nvp6158->defrect; + + while (i--) { + unsigned int err = abs(fsize->width - rect->width) + + abs(fsize->height - rect->height); + if (err < min_err) { + min_err = err; + match = fsize; + } + fsize++; + } + + if (!match) + match = &nvp6158_framesizes[0]; + + format->width = match->width; + format->height = match->height; + format->colorspace = V4L2_COLORSPACE_SRGB; + format->code = nvp6158_formats[0].code; + if (BT656I_TEST_MODES == nvp6158->mode) + format->field = V4L2_FIELD_INTERLACED; + else + format->field = V4L2_FIELD_NONE; + nvp6158->frame_size = match; +} + +static int nvp6158_stream(struct v4l2_subdev *sd, int on) +{ + struct i2c_client *client = v4l2_get_subdevdata(sd); + struct nvp6158 *nvp6158 = to_nvp6158(sd); + video_init_all video_init; + NC_VIVO_CH_FORMATDEF fmt_idx; + int ch; + + dev_info(&client->dev, "%s: on: %d, %dx%d\n", __func__, on, + nvp6158->frame_size->width, + nvp6158->frame_size->height); + + mutex_lock(&nvp6158->mutex); + on = !!on; + + if (nvp6158->streaming == on) + goto unlock; + + if (on) { + for (ch = 0; ch < 4; ch++) { + fmt_idx = nvp6158->frame_size->fmt_idx; + video_init.ch_param[ch].ch = ch; + video_init.ch_param[ch].format = fmt_idx; + } + video_init.mode = nvp6158->mode; + nvp6158_start(&video_init); + } else { + nvp6158_stop(); + } + + nvp6158->streaming = on; + +unlock: + mutex_unlock(&nvp6158->mutex); + + return 0; +} + +static int nvp6158_enum_frame_interval(struct v4l2_subdev *sd, + struct v4l2_subdev_pad_config *cfg, + struct v4l2_subdev_frame_interval_enum *fie) +{ + struct nvp6158 *nvp6158 = to_nvp6158(sd); + struct i2c_client *client = nvp6158->client; + + dev_dbg(&client->dev, "%s enter.\n", __func__); + + if (fie->index >= ARRAY_SIZE(nvp6158_framesizes)) + return -EINVAL; + + fie->width = nvp6158_framesizes[fie->index].width; + fie->height = nvp6158_framesizes[fie->index].height; + fie->interval = nvp6158_framesizes[fie->index].max_fps; + return 0; +} + +static int nvp6158_enum_mbus_code(struct v4l2_subdev *sd, + struct v4l2_subdev_pad_config *cfg, + struct v4l2_subdev_mbus_code_enum *code) +{ + if (code->index >= ARRAY_SIZE(nvp6158_formats)) + return -EINVAL; + + code->code = nvp6158_formats[code->index].code; + + return 0; +} + +static int nvp6158_enum_frame_sizes(struct v4l2_subdev *sd, + struct v4l2_subdev_pad_config *cfg, + struct v4l2_subdev_frame_size_enum *fse) +{ + struct i2c_client *client = v4l2_get_subdevdata(sd); + int i = ARRAY_SIZE(nvp6158_formats); + + dev_dbg(&client->dev, "%s: enter!\n", __func__); + + if (fse->index >= ARRAY_SIZE(nvp6158_framesizes)) + return -EINVAL; + + while (--i) + if (fse->code == nvp6158_formats[i].code) + break; + + fse->code = nvp6158_formats[i].code; + + fse->min_width = nvp6158_framesizes[fse->index].width; + fse->max_width = fse->min_width; + fse->max_height = nvp6158_framesizes[fse->index].height; + fse->min_height = fse->max_height; + + return 0; +} + +static int nvp6158_g_mbus_config(struct v4l2_subdev *sd, + struct v4l2_mbus_config *cfg) +{ + struct nvp6158 *nvp6158 = to_nvp6158(sd); + + cfg->type = V4L2_MBUS_BT656; + if (nvp6158->dual_edge == 1) { + cfg->flags = RKMODULE_CAMERA_BT656_CHANNELS | + V4L2_MBUS_PCLK_SAMPLE_RISING | + V4L2_MBUS_PCLK_SAMPLE_FALLING; + } else { + cfg->flags = RKMODULE_CAMERA_BT656_CHANNELS | + V4L2_MBUS_PCLK_SAMPLE_RISING; + } + return 0; +} + +static int nvp6158_get_fmt(struct v4l2_subdev *sd, + struct v4l2_subdev_pad_config *cfg, + struct v4l2_subdev_format *fmt) +{ + struct i2c_client *client = v4l2_get_subdevdata(sd); + struct nvp6158 *nvp6158 = to_nvp6158(sd); + + if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) { +#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API + struct v4l2_mbus_framefmt *mf; + + mf = v4l2_subdev_get_try_format(sd, cfg, 0); + mutex_lock(&nvp6158->mutex); + fmt->format = *mf; + mutex_unlock(&nvp6158->mutex); + return 0; +#else + return -ENOTTY; +#endif + } + + mutex_lock(&nvp6158->mutex); + fmt->format = nvp6158->format; + mutex_unlock(&nvp6158->mutex); + + dev_dbg(&client->dev, "%s: %x %dx%d\n", __func__, + nvp6158->format.code, nvp6158->format.width, + nvp6158->format.height); + + return 0; +} + +static void __nvp6158_try_frame_size(struct v4l2_mbus_framefmt *mf, + const struct nvp6158_framesize **size) +{ + const struct nvp6158_framesize *fsize = &nvp6158_framesizes[0]; + const struct nvp6158_framesize *match = NULL; + int i = ARRAY_SIZE(nvp6158_framesizes); + unsigned int min_err = UINT_MAX; + + while (i--) { + unsigned int err = abs(fsize->width - mf->width) + + abs(fsize->height - mf->height); + if (err < min_err) { + min_err = err; + match = fsize; + } + fsize++; + } + + if (!match) + match = &nvp6158_framesizes[0]; + + mf->width = match->width; + mf->height = match->height; + + if (size) + *size = match; +} + +static int nvp6158_set_fmt(struct v4l2_subdev *sd, + struct v4l2_subdev_pad_config *cfg, + struct v4l2_subdev_format *fmt) +{ + int index = ARRAY_SIZE(nvp6158_formats); + struct v4l2_mbus_framefmt *mf = &fmt->format; + const struct nvp6158_framesize *size = NULL; + struct nvp6158 *nvp6158 = to_nvp6158(sd); + int ret = 0; + + __nvp6158_try_frame_size(mf, &size); + + while (--index >= 0) + if (nvp6158_formats[index].code == mf->code) + break; + + if (index < 0) + return -EINVAL; + + mf->colorspace = V4L2_COLORSPACE_SRGB; + mf->code = nvp6158_formats[index].code; + mf->field = nvp6158->format.field; + + mutex_lock(&nvp6158->mutex); + + if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) { +#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API + mf = v4l2_subdev_get_try_format(sd, cfg, fmt->pad); + *mf = fmt->format; +#else + return -ENOTTY; +#endif + } else { + if (nvp6158->streaming) { + mutex_unlock(&nvp6158->mutex); + return -EBUSY; + } + + nvp6158->frame_size = size; + nvp6158->format = fmt->format; + } + + mutex_unlock(&nvp6158->mutex); + return ret; +} + +static void nvp6158_get_module_inf(struct nvp6158 *nvp6158, + struct rkmodule_inf *inf) +{ + memset(inf, 0, sizeof(*inf)); + strlcpy(inf->base.sensor, NVP6158_NAME, sizeof(inf->base.sensor)); + strlcpy(inf->base.module, nvp6158->module_name, + sizeof(inf->base.module)); + strlcpy(inf->base.lens, nvp6158->len_name, sizeof(inf->base.lens)); +} + +static __maybe_unused void +nvp6158_get_bt656_module_inf(struct nvp6158 *nvp6158, + struct rkmodule_bt656_mbus_info *inf) +{ + memset(inf, 0, sizeof(*inf)); + inf->flags = RKMODULE_CAMERA_BT656_PARSE_ID_LSB; + switch (nvp6158->ch_nums) { + case 1: + inf->flags |= RKMODULE_CAMERA_BT656_CHANNEL_0; + break; + case 2: + inf->flags |= RKMODULE_CAMERA_BT656_CHANNEL_0 | + RKMODULE_CAMERA_BT656_CHANNEL_1; + break; + case 4: + inf->flags |= RKMODULE_CAMERA_BT656_CHANNELS; + break; + default: + inf->flags |= RKMODULE_CAMERA_BT656_CHANNELS; + } +} + +static long nvp6158_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg) +{ + struct nvp6158 *nvp6158 = to_nvp6158(sd); + long ret = 0; + + switch (cmd) { + case RKMODULE_GET_MODULE_INFO: + nvp6158_get_module_inf(nvp6158, (struct rkmodule_inf *)arg); + break; + case RKMODULE_GET_BT656_MBUS_INFO: + nvp6158_get_bt656_module_inf(nvp6158, + (struct rkmodule_bt656_mbus_info + *)arg); + break; + case RKMODULE_GET_START_STREAM_SEQ: + if ((nvp6158->mode > BT656_4MUX) && + (nvp6158->mode < NVP6158_DVP_MODES_END)) + *(int *)arg = RKMODULE_START_STREAM_FRONT; + break; + default: + ret = -ENOTTY; + break; + } + + return ret; +} + +#ifdef CONFIG_COMPAT +static long nvp6158_compat_ioctl32(struct v4l2_subdev *sd, + unsigned int cmd, unsigned long arg) +{ + void __user *up = compat_ptr(arg); + struct rkmodule_inf *inf; + struct rkmodule_awb_cfg *cfg; + long ret; + struct rkmodule_bt656_mbus_info *bt565_inf; + int *seq; + + switch (cmd) { + case RKMODULE_GET_MODULE_INFO: + inf = kzalloc(sizeof(*inf), GFP_KERNEL); + if (!inf) { + ret = -ENOMEM; + return ret; + } + + ret = nvp6158_ioctl(sd, cmd, inf); + if (!ret) + ret = copy_to_user(up, inf, sizeof(*inf)); + kfree(inf); + break; + case RKMODULE_AWB_CFG: + cfg = kzalloc(sizeof(*cfg), GFP_KERNEL); + if (!cfg) { + ret = -ENOMEM; + return ret; + } + + ret = copy_from_user(cfg, up, sizeof(*cfg)); + if (!ret) + ret = nvp6158_ioctl(sd, cmd, cfg); + kfree(cfg); + break; + case RKMODULE_GET_BT656_MBUS_INFO: + bt565_inf = kzalloc(sizeof(*bt565_inf), GFP_KERNEL); + if (!bt565_inf) { + ret = -ENOMEM; + return ret; + } + + ret = nvp6158_ioctl(sd, cmd, bt565_inf); + if (!ret) + ret = copy_to_user(up, bt565_inf, sizeof(*bt565_inf)); + kfree(bt565_inf); + break; + case RKMODULE_GET_START_STREAM_SEQ: + seq = kzalloc(sizeof(*seq), GFP_KERNEL); + if (!seq) { + ret = -ENOMEM; + return ret; + } + ret = nvp6158_ioctl(sd, cmd, seq); + if (!ret) + ret = copy_to_user(up, seq, sizeof(*seq)); + kfree(seq); + break; + + default: + ret = -ENOIOCTLCMD; + break; + } + + return ret; +} +#endif + +static int nvp6158_runtime_resume(struct device *dev) +{ + struct i2c_client *client = to_i2c_client(dev); + struct v4l2_subdev *sd = i2c_get_clientdata(client); + struct nvp6158 *nvp6158 = to_nvp6158(sd); + + return __nvp6158_power_on(nvp6158); +} + +static int nvp6158_runtime_suspend(struct device *dev) +{ + struct i2c_client *client = to_i2c_client(dev); + struct v4l2_subdev *sd = i2c_get_clientdata(client); + struct nvp6158 *nvp6158 = to_nvp6158(sd); + + __nvp6158_power_off(nvp6158); + + return 0; +} + +static const struct dev_pm_ops nvp6158_pm_ops = { + SET_RUNTIME_PM_OPS(nvp6158_runtime_suspend, + nvp6158_runtime_resume, NULL) +}; + +static const struct v4l2_subdev_video_ops nvp6158_video_ops = { + .s_stream = nvp6158_stream, + .g_mbus_config = nvp6158_g_mbus_config, + .querystd = nvp6158_querystd, +}; + +static const struct v4l2_subdev_pad_ops nvp6158_subdev_pad_ops = { + .enum_mbus_code = nvp6158_enum_mbus_code, + .enum_frame_size = nvp6158_enum_frame_sizes, + .get_fmt = nvp6158_get_fmt, + .set_fmt = nvp6158_set_fmt, + .get_selection = nvp6158_get_selection, + .enum_frame_interval = nvp6158_enum_frame_interval, +}; + +static const struct v4l2_subdev_core_ops nvp6158_core_ops = { + .s_power = nvp6158_power, + .ioctl = nvp6158_ioctl, +#ifdef CONFIG_COMPAT + .compat_ioctl32 = nvp6158_compat_ioctl32, +#endif +}; + +static const struct v4l2_subdev_ops nvp6158_subdev_ops = { + .core = &nvp6158_core_ops, + .video = &nvp6158_video_ops, + .pad = &nvp6158_subdev_pad_ops, +}; + +static void get_dvp_mode(struct nvp6158 *nvp6158) +{ + struct device *dev = &nvp6158->client->dev; + char mode[128]; + u32 i; + + sprintf(mode, "%s_%dMUX", nvp6158->dvp_mode, nvp6158->ch_nums); + dev_info(dev, "combined dvp mode is(%s)\n", mode); + for (i = 0; i < NVP6158_DVP_MODES_END; i++) { + if (!strcmp(mode, nvp6158_dvp_mode_lists[i])) + break; + } + + if (i < NVP6158_DVP_MODES_END) + nvp6158->mode = i; + else + nvp6158->mode = BT656I_TEST_MODES; + dev_info(dev, "get dvp mode (%s)\n", nvp6158_dvp_mode_lists[nvp6158->mode]); +} + +static int nvp6158_parse_dts(struct nvp6158 *nvp6158) +{ + int ret; + int elem_size, elem_index; + const char *str = ""; + struct property *prop; + struct nvp6158_regulator *regulator; + struct device *dev = &nvp6158->client->dev; + struct device_node *np = of_node_get(dev->of_node); + + nvp6158->xvclk = devm_clk_get(dev, "xvclk"); + if (IS_ERR(nvp6158->xvclk)) { + dev_err(dev, "Failed to get xvclk\n"); + return -EINVAL; + } + ret = clk_set_rate(nvp6158->xvclk, NVP6158_XVCLK_FREQ); + if (ret < 0) { + dev_err(dev, "Failed to set xvclk rate (24MHz)\n"); + return ret; + } + if (clk_get_rate(nvp6158->xvclk) != NVP6158_XVCLK_FREQ) + dev_warn(dev, "xvclk mismatched, modes are based on 24MHz\n"); + + nvp6158->pinctrl = devm_pinctrl_get(dev); + if (!IS_ERR(nvp6158->pinctrl)) { + nvp6158->pins_default = + pinctrl_lookup_state(nvp6158->pinctrl, + OF_CAMERA_PINCTRL_STATE_DEFAULT); + if (IS_ERR(nvp6158->pins_default)) + dev_err(dev, "could not get default pinstate\n"); + + nvp6158->pins_sleep = + pinctrl_lookup_state(nvp6158->pinctrl, + OF_CAMERA_PINCTRL_STATE_SLEEP); + if (IS_ERR(nvp6158->pins_sleep)) + dev_err(dev, "could not get sleep pinstate\n"); + } else { + dev_err(dev, "no pinctrl\n"); + } + + elem_size = of_property_count_elems_of_size( + np, + OF_CAMERA_MODULE_REGULATOR_VOLTAGES, + sizeof(u32)); + prop = of_find_property( + np, + OF_CAMERA_MODULE_REGULATORS, + NULL); + if (elem_size > 0 && !IS_ERR_OR_NULL(prop)) { + nvp6158->regulators.regulator = + devm_kzalloc(&nvp6158->client->dev, + elem_size * sizeof(struct nvp6158_regulator), + GFP_KERNEL); + if (!nvp6158->regulators.regulator) + dev_err(dev, "could not malloc nvp6158_regulator\n"); + + nvp6158->regulators.cnt = elem_size; + + str = NULL; + elem_index = 0; + regulator = nvp6158->regulators.regulator; + if (regulator) { + do { + str = of_prop_next_string(prop, str); + if (!str) { + dev_err(dev, "%s is not match %s in dts\n", + OF_CAMERA_MODULE_REGULATORS, + OF_CAMERA_MODULE_REGULATOR_VOLTAGES); + break; + } + regulator->regulator = + devm_regulator_get_optional(dev, str); + if (IS_ERR(regulator->regulator)) + dev_err(dev, "devm_regulator_get %s failed\n", + str); + of_property_read_u32_index( + np, + OF_CAMERA_MODULE_REGULATOR_VOLTAGES, + elem_index++, + ®ulator->min_uV); + regulator->max_uV = regulator->min_uV; + regulator++; + } while (--elem_size); + } + } + + if (of_property_read_string(np, + RK_CAMERA_MODULE_DVP_MODE, + &nvp6158->dvp_mode)) { + nvp6158->dvp_mode = NVP6158_DEFAULT_DVP_MODE; + dev_warn(dev, + "can not get module %s from dts, use default(%s)!\n", + RK_CAMERA_MODULE_DVP_MODE, + NVP6158_DEFAULT_DVP_MODE); + } else { + dev_info(dev, + "get module %s from dts, dvp mode(%s)!\n", + RK_CAMERA_MODULE_DVP_MODE, nvp6158->dvp_mode); + } + + if (of_property_read_u32(np, + RK_CAMERA_MODULE_CHANNEL_NUMS, + &nvp6158->ch_nums)) { + nvp6158->ch_nums = NVP6158_DEFAULT_CHANNEL_NUMS; + dev_warn(dev, + "can not get module %s from dts, use default(%d)!\n", + RK_CAMERA_MODULE_CHANNEL_NUMS, + NVP6158_DEFAULT_CHANNEL_NUMS); + } else { + dev_info(dev, + "get module %s from dts, channel_nums(%d)!\n", + RK_CAMERA_MODULE_DVP_MODE, nvp6158->ch_nums); + } + + if (of_property_read_u32(np, + RK_CAMERA_MODULE_DUAL_EDGE, + &nvp6158->dual_edge)) { + nvp6158->dual_edge = NVP6158_DEFAULT_DUAL_EDGE; + dev_warn(dev, + "can not get module %s from dts, use default(%d)!\n", + RK_CAMERA_MODULE_DUAL_EDGE, + NVP6158_DEFAULT_DUAL_EDGE); + } else { + dev_info(dev, + "get module %s from dts, dual_edge(%d)!\n", + RK_CAMERA_MODULE_DUAL_EDGE, nvp6158->dual_edge); + } + + + + if (of_property_read_u32_array(np, + RK_CAMERA_MODULE_DEFAULT_RECT, + (unsigned int *)&nvp6158->defrect, 2)) { + nvp6158->defrect.width = NVP6158_DEFAULT_WIDTH; + nvp6158->defrect.height = NVP6158_DEFAULT_HEIGHT; + dev_warn(dev, + "can not get module %s from dts, use default wxh(%dx%d)!\n", + RK_CAMERA_MODULE_DEFAULT_RECT, + NVP6158_DEFAULT_WIDTH, NVP6158_DEFAULT_HEIGHT); + } else { + dev_info(dev, + "get module %s from dts, wxh(%dx%d)!\n", + RK_CAMERA_MODULE_DEFAULT_RECT, + nvp6158->defrect.width, + nvp6158->defrect.height); + } + + /* AHD_PWR_EN */ + nvp6158->pwr_gpio = devm_gpiod_get(dev, "pwr", GPIOD_OUT_LOW); + if (IS_ERR(nvp6158->pwr_gpio)) + dev_warn(dev, "can not find pd-gpios, error %ld\n", + PTR_ERR(nvp6158->pwr_gpio)); + /* AHD CAM_PWR_EN*/ + nvp6158->pwr2_gpio = devm_gpiod_get(dev, "pwr2", GPIOD_OUT_LOW); + if (IS_ERR(nvp6158->pwr2_gpio)) + dev_warn(dev, "can not find pd2-gpios, error %ld\n", + PTR_ERR(nvp6158->pwr2_gpio)); + + nvp6158->rst_gpio = devm_gpiod_get(dev, "rst", GPIOD_OUT_LOW); + if (IS_ERR(nvp6158->rst_gpio)) + dev_warn(dev, "can not find rst-gpios, error %ld\n", + PTR_ERR(nvp6158->rst_gpio)); + + nvp6158->rst2_gpio = devm_gpiod_get(dev, "rst2", GPIOD_OUT_LOW); + if (IS_ERR(nvp6158->rst2_gpio)) + dev_warn(dev, "can not find rst2-gpios, error %ld\n", + PTR_ERR(nvp6158->rst2_gpio)); + + nvp6158->pwdn_gpio = devm_gpiod_get(dev, "pwdn", GPIOD_OUT_LOW); + if (IS_ERR(nvp6158->pwdn_gpio)) + dev_warn(dev, "can not find pwd-gpios, error %ld\n", + PTR_ERR(nvp6158->pwdn_gpio)); + + nvp6158->pwdn2_gpio = devm_gpiod_get(dev, "pwdn2", GPIOD_OUT_LOW); + if (IS_ERR(nvp6158->pwdn2_gpio)) + dev_warn(dev, "can not find pwd2-gpios, error %ld\n", + PTR_ERR(nvp6158->pwdn2_gpio)); + + return 0; +} + + +static int nvp6158_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + struct device *dev = &client->dev; + struct device_node *node = dev->of_node; + struct nvp6158 *nvp6158; + struct v4l2_subdev *sd; + __maybe_unused char facing[2]; + int ret, index; + + dev_info(dev, "driver version: %02x.%02x.%02x", + DRIVER_VERSION >> 16, + (DRIVER_VERSION & 0xff00) >> 8, + DRIVER_VERSION & 0x00ff); + + nvp6158 = devm_kzalloc(dev, sizeof(*nvp6158), GFP_KERNEL); + if (!nvp6158) + return -ENOMEM; + + ret = of_property_read_u32(node, RKMODULE_CAMERA_MODULE_INDEX, + &nvp6158->module_index); + ret |= of_property_read_string(node, RKMODULE_CAMERA_MODULE_FACING, + &nvp6158->module_facing); + ret |= of_property_read_string(node, RKMODULE_CAMERA_MODULE_NAME, + &nvp6158->module_name); + ret |= of_property_read_string(node, RKMODULE_CAMERA_LENS_NAME, + &nvp6158->len_name); + if (ret) { + dev_err(dev, "could not get %s!\n", RKMODULE_CAMERA_LENS_NAME); + return -EINVAL; + } + + nvp6158->client = client; + + ret = nvp6158_parse_dts(nvp6158); + if (ret) { + dev_err(dev, "Failed to analyze dts\n"); + return ret; + } + get_dvp_mode(nvp6158); + + mutex_init(&nvp6158->mutex); + nvp6158_get_default_format(nvp6158); + + sd = &nvp6158->subdev; + v4l2_i2c_subdev_init(sd, client, &nvp6158_subdev_ops); + ret = nvp6158_initialize_controls(nvp6158); + if (ret) + goto err_destroy_mutex; + + __nvp6158_power_on(nvp6158); + ret = nvp6158_init(i2c_adapter_id(client->adapter)); + if (ret) { + dev_err(dev, "Failed to init nvp6158\n"); + goto err_power_off; + } + +#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API + sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE; +#endif + +#if defined(CONFIG_MEDIA_CONTROLLER) + for (index = 0; index < nvp6158->ch_nums; index++) + nvp6158->pad[index].flags = MEDIA_PAD_FL_SOURCE; + sd->entity.function = MEDIA_ENT_F_CAM_SENSOR; + ret = media_entity_pads_init(&sd->entity, nvp6158->ch_nums, nvp6158->pad); + if (ret < 0) + goto err_power_off; +#endif + + memset(facing, 0, sizeof(facing)); + if (strcmp(nvp6158->module_facing, "back") == 0) + facing[0] = 'b'; + else + facing[0] = 'f'; + + snprintf(sd->name, sizeof(sd->name), "m%02d_%s_%s %s", + nvp6158->module_index, facing, + NVP6158_NAME, dev_name(sd->dev)); + + ret = v4l2_async_register_subdev_sensor_common(sd); + if (ret) { + dev_err(dev, "v4l2 async register subdev failed\n"); + goto err_clean_entity; + } + pm_runtime_set_active(dev); + pm_runtime_enable(dev); + pm_runtime_idle(dev); + + return 0; + +err_clean_entity: +#if defined(CONFIG_MEDIA_CONTROLLER) + media_entity_cleanup(&sd->entity); +#endif +err_power_off: + __nvp6158_power_off(nvp6158); +err_destroy_mutex: + mutex_destroy(&nvp6158->mutex); + + return ret; +} + +static int nvp6158_remove(struct i2c_client *client) +{ + struct v4l2_subdev *sd = i2c_get_clientdata(client); + struct nvp6158 *nvp6158 = to_nvp6158(sd); + + nvp6158_exit(); + v4l2_ctrl_handler_free(&nvp6158->ctrl_handler); + mutex_destroy(&nvp6158->mutex); + + pm_runtime_disable(&client->dev); + if (!pm_runtime_status_suspended(&client->dev)) + __nvp6158_power_off(nvp6158); + pm_runtime_set_suspended(&client->dev); + + return 0; +} + +#if IS_ENABLED(CONFIG_OF) +static const struct of_device_id nvp6158_of_match[] = { + { .compatible = "nvp6158-v4l2" }, + {}, +}; +MODULE_DEVICE_TABLE(of, nvp6158_of_match); +#endif + +static const struct i2c_device_id nvp6158_match_id[] = { + { "nvp6158-v4l2", 0 }, + { }, +}; + +static struct i2c_driver nvp6158_i2c_driver = { + .driver = { + .name = NVP6158_NAME, + .pm = &nvp6158_pm_ops, + .of_match_table = of_match_ptr(nvp6158_of_match), + }, + .probe = &nvp6158_probe, + .remove = &nvp6158_remove, + .id_table = nvp6158_match_id, +}; + +static int __init sensor_mod_init(void) +{ + return i2c_add_driver(&nvp6158_i2c_driver); +} + +static void __exit sensor_mod_exit(void) +{ + i2c_del_driver(&nvp6158_i2c_driver); +} + +device_initcall_sync(sensor_mod_init); +module_exit(sensor_mod_exit); + +MODULE_DESCRIPTION("nvp6158 sensor driver"); +MODULE_LICENSE("GPL v2");