diff options
| -rw-r--r-- | MAINTAINERS | 1 | ||||
| -rw-r--r-- | drivers/media/i2c/Kconfig | 16 | ||||
| -rw-r--r-- | drivers/media/i2c/Makefile | 1 | ||||
| -rw-r--r-- | drivers/media/i2c/thp7312.c | 2240 |
4 files changed, 2258 insertions, 0 deletions
diff --git a/MAINTAINERS b/MAINTAINERS index c4468f6a4cb2..b8f0bb78700d 100644 --- a/MAINTAINERS +++ b/MAINTAINERS @@ -21671,6 +21671,7 @@ S: Maintained T: git git://linuxtv.org/media_tree.git F: Documentation/devicetree/bindings/media/i2c/thine,thp7312.yaml F: Documentation/userspace-api/media/drivers/thp7312.rst +F: drivers/media/i2c/thp7312.c F: include/uapi/linux/thp7312.h THUNDERBOLT DMA TRAFFIC TEST DRIVER diff --git a/drivers/media/i2c/Kconfig b/drivers/media/i2c/Kconfig index fc053c834782..e77444f64257 100644 --- a/drivers/media/i2c/Kconfig +++ b/drivers/media/i2c/Kconfig @@ -638,6 +638,22 @@ source "drivers/media/i2c/et8ek8/Kconfig" endif +menu "Camera ISPs" + visible if MEDIA_CAMERA_SUPPORT + +config VIDEO_THP7312 + tristate "THine THP7312 support" + depends on I2C + select MEDIA_CONTROLLER + select V4L2_CCI_I2C + select V4L2_FWNODE + select VIDEO_V4L2_SUBDEV_API + help + This is a Video4Linux2 sensor-level driver for the THine + THP7312 ISP. + +endmenu + menu "Lens drivers" visible if MEDIA_CAMERA_SUPPORT diff --git a/drivers/media/i2c/Makefile b/drivers/media/i2c/Makefile index 08e7e638dedc..a7ee96174397 100644 --- a/drivers/media/i2c/Makefile +++ b/drivers/media/i2c/Makefile @@ -129,6 +129,7 @@ obj-$(CONFIG_VIDEO_TDA7432) += tda7432.o obj-$(CONFIG_VIDEO_TDA9840) += tda9840.o obj-$(CONFIG_VIDEO_TEA6415C) += tea6415c.o obj-$(CONFIG_VIDEO_TEA6420) += tea6420.o +obj-$(CONFIG_VIDEO_THP7312) += thp7312.o obj-$(CONFIG_VIDEO_THS7303) += ths7303.o obj-$(CONFIG_VIDEO_THS8200) += ths8200.o obj-$(CONFIG_VIDEO_TLV320AIC23B) += tlv320aic23b.o diff --git a/drivers/media/i2c/thp7312.c b/drivers/media/i2c/thp7312.c new file mode 100644 index 000000000000..9b387e568603 --- /dev/null +++ b/drivers/media/i2c/thp7312.c @@ -0,0 +1,2240 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * Copyright (C) 2021 THine Electronics, Inc. + * Copyright (C) 2023 Ideas on Board Oy + */ + +#include <asm/unaligned.h> + +#include <linux/clk.h> +#include <linux/delay.h> +#include <linux/device.h> +#include <linux/firmware.h> +#include <linux/gpio/consumer.h> +#include <linux/i2c.h> +#include <linux/init.h> +#include <linux/iopoll.h> +#include <linux/kernel.h> +#include <linux/module.h> +#include <linux/mtd/spi-nor.h> +#include <linux/pm_runtime.h> +#include <linux/property.h> +#include <linux/regulator/consumer.h> +#include <linux/slab.h> +#include <linux/types.h> + +#include <media/v4l2-async.h> +#include <media/v4l2-cci.h> +#include <media/v4l2-ctrls.h> +#include <media/v4l2-device.h> +#include <media/v4l2-event.h> +#include <media/v4l2-fwnode.h> +#include <media/v4l2-subdev.h> + +#include <uapi/linux/thp7312.h> + +/* ISP registers */ + +#define THP7312_REG_FIRMWARE_VERSION_1 CCI_REG8(0xf000) +#define THP7312_REG_CAMERA_STATUS CCI_REG8(0xf001) +#define THP7312_REG_FIRMWARE_VERSION_2 CCI_REG8(0xf005) +#define THP7312_REG_SET_OUTPUT_ENABLE CCI_REG8(0xf008) +#define THP7312_OUTPUT_ENABLE 0x01 +#define THP7312_OUTPUT_DISABLE 0x00 +#define THP7312_REG_SET_OUTPUT_COLOR_COMPRESSION CCI_REG8(0xf009) +#define THP7312_REG_SET_OUTPUT_COLOR_UYVY 0x00 +#define THP7312_REG_SET_OUTPUT_COLOR_YUY2 0x04 +#define THP7312_REG_FLIP_MIRROR CCI_REG8(0xf00c) +#define THP7312_REG_FLIP_MIRROR_FLIP BIT(0) +#define THP7312_REG_FLIP_MIRROR_MIRROR BIT(1) +#define THP7312_REG_VIDEO_IMAGE_SIZE CCI_REG8(0xf00d) +#define THP7312_VIDEO_IMAGE_SIZE_640x360 0x52 +#define THP7312_VIDEO_IMAGE_SIZE_640x460 0x03 +#define THP7312_VIDEO_IMAGE_SIZE_1280x720 0x0a +#define THP7312_VIDEO_IMAGE_SIZE_1920x1080 0x0b +#define THP7312_VIDEO_IMAGE_SIZE_3840x2160 0x0d +#define THP7312_VIDEO_IMAGE_SIZE_4160x3120 0x14 +#define THP7312_VIDEO_IMAGE_SIZE_2016x1512 0x20 +#define THP7312_VIDEO_IMAGE_SIZE_2048x1536 0x21 +#define THP7312_REG_VIDEO_FRAME_RATE_MODE CCI_REG8(0xf00f) +#define THP7312_VIDEO_FRAME_RATE_MODE1 0x80 +#define THP7312_VIDEO_FRAME_RATE_MODE2 0x81 +#define THP7312_VIDEO_FRAME_RATE_MODE3 0x82 +#define THP7312_REG_SET_DRIVING_MODE CCI_REG8(0xf010) +#define THP7312_REG_DRIVING_MODE_STATUS CCI_REG8(0xf011) +#define THP7312_REG_JPEG_COMPRESSION_FACTOR CCI_REG8(0xf01b) +#define THP7312_REG_AE_EXPOSURE_COMPENSATION CCI_REG8(0xf022) +#define THP7312_REG_AE_FLICKER_MODE CCI_REG8(0xf023) +#define THP7312_AE_FLICKER_MODE_50 0x00 +#define THP7312_AE_FLICKER_MODE_60 0x01 +#define THP7312_AE_FLICKER_MODE_DISABLE 0x80 +#define THP7312_REG_AE_FIX_FRAME_RATE CCI_REG8(0xf02e) +#define THP7312_REG_MANUAL_WB_RED_GAIN CCI_REG8(0xf036) +#define THP7312_REG_MANUAL_WB_BLUE_GAIN CCI_REG8(0xf037) +#define THP7312_REG_WB_MODE CCI_REG8(0xf039) +#define THP7312_WB_MODE_AUTO 0x00 +#define THP7312_WB_MODE_MANUAL 0x11 +#define THP7312_REG_MANUAL_FOCUS_POSITION CCI_REG16(0xf03c) +#define THP7312_REG_AF_CONTROL CCI_REG8(0xf040) +#define THP7312_REG_AF_CONTROL_AF 0x01 +#define THP7312_REG_AF_CONTROL_MANUAL 0x10 +#define THP7312_REG_AF_CONTROL_LOCK 0x80 +#define THP7312_REG_AF_SETTING CCI_REG8(0xf041) +#define THP7312_REG_AF_SETTING_ONESHOT_CONTRAST 0x00 +#define THP7312_REG_AF_SETTING_ONESHOT_PDAF 0x40 +#define THP7312_REG_AF_SETTING_ONESHOT_HYBRID 0x80 +#define THP7312_REG_AF_SETTING_CONTINUOUS_CONTRAST 0x30 +#define THP7312_REG_AF_SETTING_CONTINUOUS_PDAF 0x70 +#define THP7312_REG_AF_SETTING_CONTINUOUS_HYBRID 0xf0 +#define THP7312_REG_AF_SUPPORT CCI_REG8(0xf043) +#define THP7312_AF_SUPPORT_PDAF BIT(1) +#define THP7312_AF_SUPPORT_CONTRAST BIT(0) +#define THP7312_REG_SATURATION CCI_REG8(0xf052) +#define THP7312_REG_SHARPNESS CCI_REG8(0xf053) +#define THP7312_REG_BRIGHTNESS CCI_REG8(0xf056) +#define THP7312_REG_CONTRAST CCI_REG8(0xf057) +#define THP7312_REG_NOISE_REDUCTION CCI_REG8(0xf059) +#define THP7312_REG_NOISE_REDUCTION_FIXED BIT(7) + +#define TH7312_REG_CUSTOM_MIPI_SET CCI_REG8(0xf0f6) +#define TH7312_REG_CUSTOM_MIPI_STATUS CCI_REG8(0xf0f7) +#define TH7312_REG_CUSTOM_MIPI_RD CCI_REG8(0xf0f8) +#define TH7312_REG_CUSTOM_MIPI_TD CCI_REG8(0xf0f9) + +/* + * Firmware update registers. Those use a different address space than the + * normal operation ISP registers. + */ + +#define THP7312_REG_FW_DRIVABILITY CCI_REG32(0xd65c) +#define THP7312_REG_FW_DEST_BANK_ADDR CCI_REG32(0xff08) +#define THP7312_REG_FW_VERIFY_RESULT CCI_REG8(0xff60) +#define THP7312_REG_FW_RESET_FLASH CCI_REG8(0xff61) +#define THP7312_REG_FW_MEMORY_IO_SETTING CCI_REG8(0xff62) +#define THP7312_FW_MEMORY_IO_GPIO0 1 +#define THP7312_FW_MEMORY_IO_GPIO1 0 +#define THP7312_REG_FW_CRC_RESULT CCI_REG32(0xff64) +#define THP7312_REG_FW_STATUS CCI_REG8(0xfffc) + +#define THP7312_FW_VERSION(major, minor) (((major) << 8) | (minor)) +#define THP7312_FW_VERSION_MAJOR(v) ((v) >> 8) +#define THP7312_FW_VERSION_MINOR(v) ((v) & 0xff) + +enum thp7312_focus_method { + THP7312_FOCUS_METHOD_CONTRAST, + THP7312_FOCUS_METHOD_PDAF, + THP7312_FOCUS_METHOD_HYBRID, +}; + +/* + * enum thp7312_focus_state - State of the focus handler + * + * @THP7312_FOCUS_STATE_MANUAL: Manual focus, controlled through the + * V4L2_CID_FOCUS_ABSOLUTE control + * @THP7312_FOCUS_STATE_AUTO: Continuous auto-focus + * @THP7312_FOCUS_STATE_LOCKED: Lock the focus to a fixed position. This state + * is entered when switching from auto to manual mode. + * @THP7312_FOCUS_STATE_ONESHOT: One-shot auto-focus + * + * Valid transitions are as follow: + * + * digraph fsm { + * node [shape=circle]; + * + * manual [label="MANUAL"]; + * auto [label="AUTO"]; + * locked [label="LOCKED"]; + * oneshot [label="ONESHOT"]; + * + * manual -> auto [label="FOCUS_AUTO <- true"] + * locked -> auto [label="FOCUS_AUTO <- true"] + * oneshot -> auto [label="FOCUS_AUTO <- true"] + * auto -> locked [label="FOCUS_AUTO <- false"] + * + * locked -> manual [label="FOCUS_ABSOLUTE <- *"] + * oneshot -> manual [label="FOCUS_ABSOLUTE <- *"] + * + * manual -> oneshot [label="FOCUS_START <- *"] + * locked -> oneshot [label="FOCUS_START <- *"] + * } + */ +enum thp7312_focus_state { + THP7312_FOCUS_STATE_MANUAL, + THP7312_FOCUS_STATE_AUTO, + THP7312_FOCUS_STATE_LOCKED, + THP7312_FOCUS_STATE_ONESHOT, +}; + +enum thp7312_boot_mode { + THP7312_BOOT_MODE_2WIRE_SLAVE = 0, + THP7312_BOOT_MODE_SPI_MASTER = 1, +}; + +struct thp7312_frame_rate { + u32 fps; + u32 link_freq; + u8 reg_frame_rate_mode; +}; + +struct thp7312_mode_info { + u32 width; + u32 height; + u8 reg_image_size; + const struct thp7312_frame_rate *rates; +}; + +static const u32 thp7312_colour_fmts[] = { + MEDIA_BUS_FMT_YUYV8_1X16, +}; + +/* regulator supplies */ +static const char * const thp7312_supply_name[] = { + "vddcore", + "vhtermrx", + "vddtx", + "vddhost", + "vddcmos", + "vddgpio-0", + "vddgpio-1", +}; + +static const struct thp7312_mode_info thp7312_mode_info_data[] = { + { + .width = 1920, + .height = 1080, + .reg_image_size = THP7312_VIDEO_IMAGE_SIZE_1920x1080, + .rates = (const struct thp7312_frame_rate[]) { + { 30, 300000000, 0x81 }, + { 60, 387500000, 0x82 }, + { 0 } + }, + }, { + .width = 2048, + .height = 1536, + .reg_image_size = THP7312_VIDEO_IMAGE_SIZE_2048x1536, + .rates = (const struct thp7312_frame_rate[]) { + { 30, 300000000, 0x81 }, + { 0 } + } + }, { + .width = 3840, + .height = 2160, + .reg_image_size = THP7312_VIDEO_IMAGE_SIZE_3840x2160, + .rates = (const struct thp7312_frame_rate[]) { + { 30, 600000000, 0x81 }, + { 0 } + }, + }, { + .width = 4160, + .height = 3120, + .reg_image_size = THP7312_VIDEO_IMAGE_SIZE_4160x3120, + .rates = (const struct thp7312_frame_rate[]) { + { 20, 600000000, 0x81 }, + { 0 } + }, + }, +}; + +struct thp7312_device; + +struct thp7312_sensor_info { + const char *model; +}; + +struct thp7312_sensor { + const struct thp7312_sensor_info *info; + u8 lane_remap; +}; + +struct thp7312_device { + struct device *dev; + struct regmap *regmap; + + struct v4l2_subdev sd; + struct media_pad pad; + + struct gpio_desc *reset_gpio; + struct regulator_bulk_data supplies[ARRAY_SIZE(thp7312_supply_name)]; + struct clk *iclk; + + u8 lane_remap; + + struct thp7312_sensor sensors[1]; + + enum thp7312_boot_mode boot_mode; + + struct v4l2_ctrl_handler ctrl_handler; + bool ctrls_applied; + + /* These are protected by v4l2 active state */ + const struct thp7312_mode_info *current_mode; + const struct thp7312_frame_rate *current_rate; + s64 link_freq; + + struct { + struct v4l2_ctrl *hflip; + struct v4l2_ctrl *vflip; + }; + + struct { + struct v4l2_ctrl *focus_auto; + struct v4l2_ctrl *focus_absolute; + struct v4l2_ctrl *focus_start; + struct v4l2_ctrl *focus_method; + }; + + enum thp7312_focus_state focus_state; + + struct { + struct v4l2_ctrl *noise_reduction_auto; + struct v4l2_ctrl *noise_reduction_absolute; + }; + + /* Lock to protect fw_cancel */ + struct mutex fw_lock; + struct fw_upload *fwl; + u8 *fw_write_buf; + bool fw_cancel; + + u16 fw_version; +}; + +static const struct thp7312_sensor_info thp7312_sensor_info[] = { + { + .model = "sony,imx258", + }, +}; + +static inline struct thp7312_device *to_thp7312_dev(struct v4l2_subdev *sd) +{ + return container_of(sd, struct thp7312_device, sd); +} + +/* ----------------------------------------------------------------------------- + * Device Access & Configuration + */ + +#define thp7312_read_poll_timeout(dev, addr, val, cond, sleep_us, timeout_us) \ +({ \ + int __ret, __err; \ + __ret = read_poll_timeout(cci_read, __err, __err || (cond), sleep_us, \ + timeout_us, false, (dev)->regmap, addr, \ + &(val), NULL); \ + __ret ? : __err; \ +}) + +static int thp7312_map_data_lanes(u8 *lane_remap, const u8 *lanes, u8 num_lanes) +{ + u8 used_lanes = 0; + u8 val = 0; + unsigned int i; + + /* + * The value that we write to the register is the index in the + * data-lanes array, so we need to do a conversion. Do this in the same + * pass as validating data-lanes. + */ + for (i = 0; i < num_lanes; i++) { + if (lanes[i] < 1 || lanes[i] > 4) + return -EINVAL; + + if (used_lanes & (BIT(lanes[i]))) + return -EINVAL; + + used_lanes |= BIT(lanes[i]); + + /* + * data-lanes is 1-indexed while the field position in the + * register is 0-indexed. + */ + val |= i << ((lanes[i] - 1) * 2); + } + + *lane_remap = val; + + return 0; +} + +static int thp7312_set_mipi_lanes(struct thp7312_device *thp7312) +{ + struct device *dev = thp7312->dev; + int ret = 0; + u64 val; + + cci_write(thp7312->regmap, TH7312_REG_CUSTOM_MIPI_RD, + thp7312->sensors[0].lane_remap, &ret); + cci_write(thp7312->regmap, TH7312_REG_CUSTOM_MIPI_TD, + thp7312->lane_remap, &ret); + cci_write(thp7312->regmap, TH7312_REG_CUSTOM_MIPI_SET, 1, &ret); + + if (ret) + return ret; + + ret = thp7312_read_poll_timeout(thp7312, TH7312_REG_CUSTOM_MIPI_STATUS, + val, val == 0x00, 100000, 2000000); + if (ret) { + dev_err(dev, "Failed to poll MIPI lane status: %d\n", ret); + return ret; + } + + return 0; +} + +static int thp7312_change_mode(struct thp7312_device *thp7312, + const struct thp7312_mode_info *mode, + const struct thp7312_frame_rate *rate) +{ + struct device *dev = thp7312->dev; + u64 val = 0; + int ret; + + ret = thp7312_read_poll_timeout(thp7312, THP7312_REG_CAMERA_STATUS, val, + val == 0x80, 20000, 200000); + if (ret < 0) { + dev_err(dev, "%s(): failed to poll ISP: %d\n", __func__, ret); + return ret; + } + + cci_write(thp7312->regmap, THP7312_REG_VIDEO_IMAGE_SIZE, + mode->reg_image_size, &ret); + cci_write(thp7312->regmap, THP7312_REG_VIDEO_FRAME_RATE_MODE, + rate->reg_frame_rate_mode, &ret); + cci_write(thp7312->regmap, THP7312_REG_JPEG_COMPRESSION_FACTOR, 0x5e, + &ret); + cci_write(thp7312->regmap, THP7312_REG_SET_DRIVING_MODE, 0x01, &ret); + + if (ret) + return ret; + + ret = thp7312_read_poll_timeout(thp7312, THP7312_REG_DRIVING_MODE_STATUS, + val, val == 0x01, 20000, 100000); + if (ret < 0) { + dev_err(dev, "%s(): failed\n", __func__); + return ret; + } + + return 0; +} + +static int thp7312_set_framefmt(struct thp7312_device *thp7312, + struct v4l2_mbus_framefmt *format) +{ + u8 val; + + switch (format->code) { + case MEDIA_BUS_FMT_UYVY8_1X16: + /* YUV422, UYVY */ + val = THP7312_REG_SET_OUTPUT_COLOR_UYVY; + break; + case MEDIA_BUS_FMT_YUYV8_1X16: + /* YUV422, YUYV */ + val = THP7312_REG_SET_OUTPUT_COLOR_YUY2; + break; + default: + /* Should never happen */ + return -EINVAL; + } + + return cci_write(thp7312->regmap, + THP7312_REG_SET_OUTPUT_COLOR_COMPRESSION, val, NULL); +} + +static int thp7312_init_mode(struct thp7312_device *thp7312, + struct v4l2_subdev_state *sd_state) +{ + struct v4l2_mbus_framefmt *fmt; + int ret; + + fmt = v4l2_subdev_state_get_format(sd_state, 0); + + ret = thp7312_set_framefmt(thp7312, fmt); + if (ret) + return ret; + + return thp7312_change_mode(thp7312, thp7312->current_mode, + thp7312->current_rate); +} + +static int thp7312_stream_enable(struct thp7312_device *thp7312, bool enable) +{ + return cci_write(thp7312->regmap, THP7312_REG_SET_OUTPUT_ENABLE, + enable ? THP7312_OUTPUT_ENABLE : THP7312_OUTPUT_DISABLE, + NULL); +} + +static int thp7312_check_status_stream_mode(struct thp7312_device *thp7312) +{ + struct device *dev = thp7312->dev; + u64 status = 0; + int ret; + + while (status != 0x80) { + ret = cci_read(thp7312->regmap, THP7312_REG_CAMERA_STATUS, + &status, NULL); + if (ret) + return ret; + + if (status == 0x80) { + dev_dbg(dev, "Camera initialization done\n"); + return 0; + } + + if (status != 0x00) { + dev_err(dev, "Invalid camera status %llx\n", status); + return -EINVAL; + } + + dev_dbg(dev, "Camera initializing...\n"); + usleep_range(70000, 80000); + } + + return 0; +} + +static void thp7312_reset(struct thp7312_device *thp7312) +{ + unsigned long rate; + + gpiod_set_value_cansleep(thp7312->reset_gpio, 1); + + /* + * The minimum reset duration is 8 clock cycles, make it 10 to provide + * a safety margin. + */ + rate = clk_get_rate(thp7312->iclk); + fsleep(DIV_ROUND_UP(10 * USEC_PER_SEC, rate)); + + gpiod_set_value_cansleep(thp7312->reset_gpio, 0); + + /* + * TODO: The documentation states that the device needs 2ms to + * initialize after reset is deasserted. It then proceeds to load the + * firmware from the flash memory, which takes an unspecified amount of + * time. Check if this delay could be reduced. + */ + fsleep(300000); +} + +/* ----------------------------------------------------------------------------- + * Power Management + */ + +static void __thp7312_power_off(struct thp7312_device *thp7312) +{ + regulator_bulk_disable(ARRAY_SIZE(thp7312->supplies), thp7312->supplies); + clk_disable_unprepare(thp7312->iclk); +} + +static void thp7312_power_off(struct thp7312_device *thp7312) +{ + __thp7312_power_off(thp7312); +} + +static int __thp7312_power_on(struct thp7312_device *thp7312) +{ + struct device *dev = thp7312->dev; + int ret; + + ret = regulator_bulk_enable(ARRAY_SIZE(thp7312->supplies), + thp7312->supplies); + if (ret < 0) + return ret; + + ret = clk_prepare_enable(thp7312->iclk); + if (ret < 0) { + dev_err(dev, "clk prepare enable failed\n"); + regulator_bulk_disable(ARRAY_SIZE(thp7312->supplies), + thp7312->supplies); + return ret; + } + + /* + * We cannot assume that turning off and on again will reset, so do a + * software reset on power up. + */ + thp7312_reset(thp7312); + + return 0; +} + +static int thp7312_power_on(struct thp7312_device *thp7312) +{ + int ret; + + ret = __thp7312_power_on(thp7312); + if (ret < 0) + return ret; + + ret = thp7312_check_status_stream_mode(thp7312); + if (ret < 0) + goto error; + + ret = thp7312_set_mipi_lanes(thp7312); + if (ret) + goto error; + + return 0; + +error: + thp7312_power_off(thp7312); + return ret; +} + +static int __maybe_unused thp7312_pm_runtime_suspend(struct device *dev) +{ + struct v4l2_subdev *sd = dev_get_drvdata(dev); + struct thp7312_device *thp7312 = to_thp7312_dev(sd); + + thp7312_power_off(thp7312); + + thp7312->ctrls_applied = false; + + return 0; +} + +static int __maybe_unused thp7312_pm_runtime_resume(struct device *dev) +{ + struct v4l2_subdev *sd = dev_get_drvdata(dev); + struct thp7312_device *thp7312 = to_thp7312_dev(sd); + + return thp7312_power_on(thp7312); +} + +static const struct dev_pm_ops thp7312_pm_ops = { + SET_RUNTIME_PM_OPS(thp7312_pm_runtime_suspend, + thp7312_pm_runtime_resume, NULL) +}; + +/* ----------------------------------------------------------------------------- + * V4L2 Subdev Operations + */ + +static bool thp7312_find_bus_code(u32 code) +{ + unsigned int i; + + for (i = 0; i < ARRAY_SIZE(thp7312_colour_fmts); ++i) { + if (thp7312_colour_fmts[i] == code) + return true; + } + + return false; +} + +static const struct thp7312_mode_info * +thp7312_find_mode(unsigned int width, unsigned int height, bool nearest) +{ + const struct thp7312_mode_info *mode; + + mode = v4l2_find_nearest_size(thp7312_mode_info_data, + ARRAY_SIZE(thp7312_mode_info_data), + width, height, width, height); + + if (!nearest && (mode->width != width || mode->height != height)) + return NULL; + + return mode; +} + +static void thp7312_set_frame_rate(struct thp7312_device *thp7312, + const struct thp7312_frame_rate *rate) +{ + thp7312->link_freq = rate->link_freq; + thp7312->current_rate = rate; +} + +static int thp7312_enum_mbus_code(struct v4l2_subdev *sd, + struct v4l2_subdev_state *sd_state, + struct v4l2_subdev_mbus_code_enum *code) +{ + if (code->index >= ARRAY_SIZE(thp7312_colour_fmts)) + return -EINVAL; + + code->code = thp7312_colour_fmts[code->index]; + + return 0; +} + +static int thp7312_enum_frame_size(struct v4l2_subdev *sd, + struct v4l2_subdev_state *sd_state, + struct v4l2_subdev_frame_size_enum *fse) +{ + if (!thp7312_find_bus_code(fse->code)) + return -EINVAL; + + if (fse->index >= ARRAY_SIZE(thp7312_mode_info_data)) + return -EINVAL; + + fse->min_width = thp7312_mode_info_data[fse->index].width; + fse->max_width = fse->min_width; + fse->min_height = thp7312_mode_info_data[fse->index].height; + fse->max_height = fse->min_height; + + return 0; +} + +static int thp7312_enum_frame_interval(struct v4l2_subdev *sd, + struct v4l2_subdev_state *sd_state, + struct v4l2_subdev_frame_interval_enum *fie) +{ + const struct thp7312_frame_rate *rate; + const struct thp7312_mode_info *mode; + unsigned int index = fie->index; + + if (!thp7312_find_bus_code(fie->code)) + return -EINVAL; + + mode = thp7312_find_mode(fie->width, fie->height, false); + if (!mode) + return -EINVAL; + + for (rate = mode->rates; rate->fps; ++rate, --index) { + if (!index) { + fie->interval.numerator = 1; + fie->interval.denominator = rate->fps; + + return 0; + } + } + + return -EINVAL; +} + +static int thp7312_set_fmt(struct v4l2_subdev *sd, + struct v4l2_subdev_state *sd_state, + struct v4l2_subdev_format *format) +{ + struct thp7312_device *thp7312 = to_thp7312_dev(sd); + struct v4l2_mbus_framefmt *mbus_fmt = &format->format; + struct v4l2_mbus_framefmt *fmt; + const struct thp7312_mode_info *mode; + + if (!thp7312_find_bus_code(mbus_fmt->code)) + mbus_fmt->code = thp7312_colour_fmts[0]; + + mode = thp7312_find_mode(mbus_fmt->width, mbus_fmt->height, true); + + fmt = v4l2_subdev_state_get_format(sd_state, 0); + + fmt->code = mbus_fmt->code; + fmt->width = mode->width; + fmt->height = mode->height; + fmt->colorspace = V4L2_COLORSPACE_SRGB; + fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(fmt->colorspace); + fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE; + fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(fmt->colorspace); + + *mbus_fmt = *fmt; + + if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE) { + thp7312->current_mode = mode; + thp7312_set_frame_rate(thp7312, &mode->rates[0]); + } + + return 0; +} + +static int thp7312_g_frame_interval(struct v4l2_subdev *sd, + struct v4l2_subdev_frame_interval *fi) +{ + struct thp7312_device *thp7312 = to_thp7312_dev(sd); + struct v4l2_subdev_state *sd_state; + + sd_state = v4l2_subdev_lock_and_get_active_state(sd); + fi->interval.numerator = 1; + fi->interval.denominator = thp7312->current_rate->fps; + v4l2_subdev_unlock_state(sd_state); + + return 0; +} + +static int thp7312_s_frame_interval(struct v4l2_subdev *sd, + struct v4l2_subdev_frame_interval *fi) +{ + struct thp7312_device *thp7312 = to_thp7312_dev(sd); + const struct thp7312_mode_info *mode; + const struct thp7312_frame_rate *best_rate = NULL; + const struct thp7312_frame_rate *rate; + struct v4l2_subdev_state *sd_state; + unsigned int best_delta = UINT_MAX; + unsigned int fps; + + /* Avoid divisions by 0, pick the highest frame if the interval is 0. */ + fps = fi->interval.numerator + ? DIV_ROUND_CLOSEST(fi->interval.denominator, fi->interval.numerator) + : UINT_MAX; + + sd_state = v4l2_subdev_lock_and_get_active_state(sd); + + mode = thp7312->current_mode; + + for (rate = mode->rates; rate->fps && best_delta; ++rate) { + unsigned int delta = abs(rate->fps - fps); + + if (delta <= best_delta) { + best_delta = delta; + best_rate = rate; + } + } + + thp7312_set_frame_rate(thp7312, best_rate); + + v4l2_subdev_unlock_state(sd_state); + + fi->interval.numerator = 1; + fi->interval.denominator = best_rate->fps; + + return 0; +} + +static int thp7312_s_stream(struct v4l2_subdev *sd, int enable) +{ + struct thp7312_device *thp7312 = to_thp7312_dev(sd); + struct v4l2_subdev_state *sd_state; + int ret; + + sd_state = v4l2_subdev_lock_and_get_active_state(sd); + + if (!enable) { + thp7312_stream_enable(thp7312, false); + + pm_runtime_mark_last_busy(thp7312->dev); + pm_runtime_put_autosuspend(thp7312->dev); + + v4l2_subdev_unlock_state(sd_state); + + return 0; + } + + ret = pm_runtime_resume_and_get(thp7312->dev); + if (ret) + goto finish_unlock; + + ret = thp7312_init_mode(thp7312, sd_state); + if (ret) + goto finish_pm; + + if (!thp7312->ctrls_applied) { + ret = __v4l2_ctrl_handler_setup(&thp7312->ctrl_handler); + if (ret) + goto finish_pm; + + thp7312->ctrls_applied = true; + } + + ret = thp7312_stream_enable(thp7312, true); + if (ret) + goto finish_pm; + + goto finish_unlock; + +finish_pm: + pm_runtime_mark_last_busy(thp7312->dev); + pm_runtime_put_autosuspend(thp7312->dev); +finish_unlock: + v4l2_subdev_unlock_state(sd_state); + + return ret; +} + +static int thp7312_init_cfg(struct v4l2_subdev *sd, + struct v4l2_subdev_state *sd_state) +{ + const struct thp7312_mode_info *default_mode = &thp7312_mode_info_data[0]; + struct v4l2_mbus_framefmt *fmt; + + fmt = v4l2_subdev_state_get_format(sd_state, 0); + + /* + * default init sequence initialize thp7312 to + * YUV422 YUYV VGA@30fps + */ + fmt->code = MEDIA_BUS_FMT_YUYV8_1X16; + fmt->colorspace = V4L2_COLORSPACE_SRGB; + fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(fmt->colorspace); + fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE; + fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(fmt->colorspace); + fmt->width = default_mode->width; + fmt->height = default_mode->height; + fmt->field = V4L2_FIELD_NONE; + + return 0; +} + +static const struct v4l2_subdev_core_ops thp7312_core_ops = { + .log_status = v4l2_ctrl_subdev_log_status, + .subscribe_event = v4l2_ctrl_subdev_subscribe_event, + .unsubscribe_event = v4l2_event_subdev_unsubscribe, +}; + +static const struct v4l2_subdev_video_ops thp7312_video_ops = { + .g_frame_interval = thp7312_g_frame_interval, + .s_frame_interval = thp7312_s_frame_interval, + .s_stream = thp7312_s_stream, +}; + +static const struct v4l2_subdev_pad_ops thp7312_pad_ops = { + .enum_mbus_code = thp7312_enum_mbus_code, + .init_cfg = thp7312_init_cfg, + .get_fmt = v4l2_subdev_get_fmt, + .set_fmt = thp7312_set_fmt, + .enum_frame_size = thp7312_enum_frame_size, + .enum_frame_interval = thp7312_enum_frame_interval, +}; + +static const struct v4l2_subdev_ops thp7312_subdev_ops = { + .core = &thp7312_core_ops, + .video = &thp7312_video_ops, + .pad = &thp7312_pad_ops, +}; + +/* ----------------------------------------------------------------------------- + * V4L2 Control Operations + */ + +static inline struct thp7312_device *to_thp7312_from_ctrl(struct v4l2_ctrl *ctrl) +{ + return container_of(ctrl->handler, struct thp7312_device, ctrl_handler); +} + +/* 0: 3000cm, 18: 8cm */ +static const u16 thp7312_focus_values[] = { + 3000, 1000, 600, 450, 350, + 290, 240, 200, 170, 150, + 140, 130, 120, 110, 100, + 93, 87, 83, 80, +}; + +static int thp7312_set_focus(struct thp7312_device *thp7312) +{ + enum thp7312_focus_state new_state = thp7312->focus_state; + bool continuous; + u8 af_control; + u8 af_setting; + int ret = 0; + + /* Start by programming the manual focus position if it has changed. */ + if (thp7312->focus_absolute->is_new) { + unsigned int value; + + value = thp7312_focus_values[thp7312->focus_absolute->val]; + + ret = cci_write(thp7312->regmap, + THP7312_REG_MANUAL_FOCUS_POSITION, value, NULL); + if (ret) + return ret; + } + + /* Calculate the new focus state. */ + switch (thp7312->focus_state) { + case THP7312_FOCUS_STATE_MANUAL: + default: + if (thp7312->focus_auto->val) + new_state = THP7312_FOCUS_STATE_AUTO; + else if (thp7312->focus_start->is_new) + new_state = THP7312_FOCUS_STATE_ONESHOT; + break; + + case THP7312_FOCUS_STATE_AUTO: + if (!thp7312->focus_auto->val) + new_state = THP7312_FOCUS_STATE_LOCKED; + break; + + case THP7312_FOCUS_STATE_LOCKED: + if (thp7312->focus_auto->val) + new_state = THP7312_FOCUS_STATE_AUTO; + else if (thp7312->focus_start->is_new) + new_state = THP7312_FOCUS_STATE_ONESHOT; + else if (thp7312->focus_absolute->is_new) + new_state = THP7312_FOCUS_STATE_MANUAL; + break; + + case THP7312_FOCUS_STATE_ONESHOT: + if (thp7312->focus_auto->val) + new_state = THP7312_FOCUS_STATE_AUTO; + else if (thp7312->focus_start->is_new) + new_state = THP7312_FOCUS_STATE_ONESHOT; + else if (thp7312->focus_absolute->is_new) + new_state = THP7312_FOCUS_STATE_MANUAL; + break; + } + + /* + * If neither the state nor the focus method has changed, and no new + * one-shot focus is requested, there's nothing new to program to the + * hardware. + */ + if (thp7312->focus_state == new_state && + !thp7312->focus_method->is_new && !thp7312->focus_start->is_new) + return 0; + + continuous = new_state == THP7312_FOCUS_STATE_MANUAL || + new_state == THP7312_FOCUS_STATE_ONESHOT; + + switch (thp7312->focus_method->val) { + case THP7312_FOCUS_METHOD_CONTRAST: + default: + af_setting = continuous + ? THP7312_REG_AF_SETTING_CONTINUOUS_CONTRAST + : THP7312_REG_AF_SETTING_ONESHOT_CONTRAST; + break; + case THP7312_FOCUS_METHOD_PDAF: + af_setting = continuous + ? THP7312_REG_AF_SETTING_CONTINUOUS_PDAF + : THP7312_REG_AF_SETTING_ONESHOT_PDAF; + break; + case THP7312_FOCUS_METHOD_HYBRID: + af_setting = continuous + ? THP7312_REG_AF_SETTING_CONTINUOUS_HYBRID + : THP7312_REG_AF_SETTING_ONESHOT_HYBRID; + break; + } + + switch (new_state) { + case THP7312_FOCUS_STATE_MANUAL: + default: + af_control = THP7312_REG_AF_CONTROL_MANUAL; + break; + case THP7312_FOCUS_STATE_AUTO: + case THP7312_FOCUS_STATE_ONESHOT: + af_control = THP7312_REG_AF_CONTROL_AF; + break; + case THP7312_FOCUS_STATE_LOCKED: + af_control = THP7312_REG_AF_CONTROL_LOCK; + break; + } + + cci_write(thp7312->regmap, THP7312_REG_AF_SETTING, af_setting, &ret); + + if (new_state == THP7312_FOCUS_STATE_MANUAL && + (thp7312->focus_state == THP7312_FOCUS_STATE_AUTO || + thp7312->focus_state == THP7312_FOCUS_STATE_ONESHOT)) { + /* When switching to manual state, lock AF first. */ + cci_write(thp7312->regmap, THP7312_REG_AF_CONTROL, + THP7312_REG_AF_CONTROL_LOCK, &ret); + } + + cci_write(thp7312->regmap, THP7312_REG_AF_CONTROL, af_control, &ret); + + if (ret) + return ret; + + thp7312->focus_state = new_state; + + return 0; +} + +static int thp7312_s_ctrl(struct v4l2_ctrl *ctrl) +{ + struct thp7312_device *thp7312 = to_thp7312_from_ctrl(ctrl); + int ret = 0; + u8 value; + + if (ctrl->flags & V4L2_CTRL_FLAG_INACTIVE) + return -EINVAL; + + if (!pm_runtime_get_if_active(thp7312->dev, true)) + return 0; + + switch (ctrl->id) { + case V4L2_CID_BRIGHTNESS: + cci_write(thp7312->regmap, THP7312_REG_BRIGHTNESS, + ctrl->val + 10, &ret); + break; + + case V4L2_CID_THP7312_LOW_LIGHT_COMPENSATION: + /* 0 = Auto adjust frame rate, 1 = Fix frame rate */ + cci_write(thp7312->regmap, THP7312_REG_AE_FIX_FRAME_RATE, + ctrl->val ? 0 : 1, &ret); + break; + + case V4L2_CID_FOCUS_AUTO: + case V4L2_CID_FOCUS_ABSOLUTE: + case V4L2_CID_AUTO_FOCUS_START: + case V4L2_CID_THP7312_AUTO_FOCUS_METHOD: + ret = thp7312_set_focus(thp7312); + break; + + case V4L2_CID_HFLIP: + case V4L2_CID_VFLIP: + value = (thp7312->hflip->val ? THP7312_REG_FLIP_MIRROR_MIRROR : 0) + | (thp7312->vflip->val ? THP7312_REG_FLIP_MIRROR_FLIP : 0); + + cci_write(thp7312->regmap, THP7312_REG_FLIP_MIRROR, value, &ret); + break; + + case V4L2_CID_THP7312_NOISE_REDUCTION_AUTO: + case V4L2_CID_THP7312_NOISE_REDUCTION_ABSOLUTE: + value = thp7312->noise_reduction_auto->val ? 0 + : THP7312_REG_NOISE_REDUCTION_FIXED | + thp7312->noise_reduction_absolute->val; + + cci_write(thp7312->regmap, THP7312_REG_NOISE_REDUCTION, value, + &ret); + break; + + case V4L2_CID_AUTO_WHITE_BALANCE: + value = ctrl->val ? THP7312_WB_MODE_AUTO : THP7312_WB_MODE_MANUAL; + + cci_write(thp7312->regmap, THP7312_REG_WB_MODE, value, &ret); + break; + + case V4L2_CID_RED_BALANCE: + cci_write(thp7312->regmap, THP7312_REG_MANUAL_WB_RED_GAIN, + ctrl->val, &ret); + break; + + case V4L2_CID_BLUE_BALANCE: + cci_write(thp7312->regmap, THP7312_REG_MANUAL_WB_BLUE_GAIN, + ctrl->val, &ret); + break; + + case V4L2_CID_AUTO_EXPOSURE_BIAS: + cci_write(thp |